Урок 8. Управление движением робота

Урок 8. Управление движением робота

Введение

На протяжении предыдущих уроков вы последовательно освоили ключевые аспекты работы с ROS — от базовых понятий, таких как узлы, топики и launch-файлы, до более сложных тем, включая сервисы, действия, работу с симулятором Gazebo и описанием роботов с помощью формата URDF. Вы научились создавать собственные пакеты, управлять мобильным роботом TurtleBot3, использовать Docker для изоляции окружения, строить модели роботов и взаимодействовать с их сенсорами и приводами.

Теперь пришло время перейти к одному из самых важных этапов в разработке робототехнических систем — управлению движением. В реальных задачах недостаточно просто знать текущее положение шарниров или уметь читать данные с датчиков. Необходимо точно и эффективно управлять движением манипулятора, будь то отдельный шарнир или вся конечная точка (TCP) робота.

В этом уроке вы узнаете:

Как добавить в модель робота контроллеры с использованием пакета ros_control;
Как настраивать параметры ПИД-регуляторов и визуализировать переходные процессы с помощью инструментов ROS, таких как rqt_plot и rqt_reconfigure;
Как управлять движением отдельных шарниров и всей конечной точки робота в декартовом пространстве;
Как применять библиотеку KDL (Kinematics and Dynamics Library) для решения задач обратной кинематики;
Как реализовать простой узел на C++, управляющий перемещением TCP и использующий данные с симулятора Gazebo.

Этот урок станет логическим продолжением ваших знаний по управлению роботами и заложит основу для дальнейшего изучения таких тем, как планирование траекторий, навигация и взаимодействие с объектами в трёхмерном пространстве.

8.1 Управление движением шарниров робота с помощью пакетов ros_control

После того как мы научились описывать конструкцию манипулятора в формате URDF и моделировать его в Gazebo, настало время перейти к следующему важному этапу — управлению движением. Для этого в ROS существует удобный и мощный инструмент — библиотека ros_control. Она предоставляет универсальный интерфейс для создания контроллеров, подходящих как для симуляции (например, в Gazebo), так и для реальных роботов.

В этом разделе вы:

Добавите в модель UR5 контроллеры положения с использованием ros_control;
Настроите параметры ПИД-регуляторов, чтобы обеспечить точное и стабильное движение;
Научитесь управлять отдельными шарнирами, отправляя им команды через топики ROS;
Познакомитесь с основами взаимодействия между ROS и физическим движком симулятора, что позволит вам строить более реалистичные и функциональные модели управления.

Для работы мы будем использовать уже созданную ранее модель робота UR5, адаптированную под Gazebo. Это даст возможность сосредоточиться именно на реализации системы управления, не возвращаясь к этапу построения самой модели.

Цель этого раздела — показать, как превратить “статичный” механизм робота в систему, способную выполнять целенаправленные движения, реагировать на внешние команды и корректировать свою работу с помощью обратной связи.

8.1.1 Подготовка и настройка

В этом разделе мы подготовите окружение для работы с библиотекой ros_control.

Как уже повелось, для изоляции среды мы будем использовать docker-контейнер, чтобы не изменять систему на вашем компьютере. Мы создадим docker-образ, скорректировав docker-файл из предыдущего занятия, который мы использовали для симуляции UR5 в Gazebo. Мы установим необходимые пакеты, склонируем нужные репозитории, соберём Catkin workspace и проверим, что всё работает корректно.

Самостоятельно написать пакет с контроллерами для UR5 нам пока сложновато, поэтому давайте воспользуемся уже подготовленным ROS-пакетом из урока ROS Tutorial: Control the UR5 robot with ros_control – How to tune a PID Controller.

Склонируем ROS-пакет:

git clone https://github.com/dairal/ur5-joint-position-control.git

Также нам нужно установить ros_control и набор готовых контроллеров, поддерживающих наиболее распространенные варианты использования манипуляторов, мобильных и человекоподобных роботов.

apt-get install ros-$ROS_DISTRO-ros-control ros-$ROS_DISTRO-ros-controllers

В результате у нас получится такой docker-файл:

# Используем официальный образ ROS Noetic с полной установкой (включая Gazebo)
FROM osrf/ros:noetic-desktop-full

# Обновляем список пакетов и устанавливаем Git и необходимые ROS-пакеты
# ros-control — библиотека управления приводами
# ros-controllers — набор готовых контроллеров
RUN apt update -y && \
    apt upgrade -y && \
    apt install -y git ros-$ROS_DISTRO-ros-control ros-$ROS_DISTRO-ros-controllers

# Автоматически загружаем окружение ROS при запуске терминала
RUN echo "source /opt/ros/noetic/setup.bash" >> /home/.bashrc

# Переходим в рабочую директорию Catkin workspace
WORKDIR /home/catkin_ws/src

# Клонируем репозиторий с примерами контроллеров шарниров UR5
RUN git clone https://github.com/dairal/ur5-joint-position-control.git 

# Клонируем стандартные модели UR от ROS-Industrial
RUN git clone -b $ROS_DISTRO-devel https://github.com/ros-industrial/universal_robot.git 

# Возвращаемся в корень workspace
WORKDIR /home/catkin_ws

# Проверяем зависимости и устанавливаем их
RUN rosdep update && \
    rosdep install --rosdistro $ROS_DISTRO --ignore-src --from-paths src -y

# Собираем Catkin workspace
RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make'

# Добавляем автоматическую загрузку собранного workspace
RUN echo "source /home/catkin_ws/devel/setup.bash" >> /home/.bashrc

# Копируем скрипт запуска launch.sh в контейнер
COPY launch.sh /home/launch.sh

# Делаем его исполняемым
RUN chmod +x /home/launch.sh

# Точка входа: запускаем Gazebo и загружаем модель робота
ENTRYPOINT [ "/home/launch.sh" ]

TO DO: Попробовать, если получится, объяснить.

# Stage 1: Build stage
FROM osrf/ros:noetic-desktop-full AS builder

# Обновляем систему и устанавливаем Git и необходимые пакеты
RUN apt update -y && \
    apt install -y git ros-noetic-ros-control ros-noetic-ros-controllers

# Настройка окружения
RUN echo "source /opt/ros/noetic/setup.bash" >> /home/.bashrc

# Создаём рабочую директорию и клонируем репозитории
WORKDIR /home/catkin_ws/src
RUN git clone -b noetic-devel https://github.com/ros-industrial/universal_robot.git 
RUN git clone https://github.com/dairal/ur5-joint-position-control.git 
# Это ~на Новый год~ для п. 8.2
# RUN git clone https://github.com/dairal/ur5-tcp-position-control.git 

# Устанавливаем зависимости
WORKDIR /home/catkin_ws
RUN rosdep update && \
    rosdep install --rosdistro noetic --ignore-src --from-paths src -y

# Собираем Catkin workspace
RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make'

# Stage 2: Final runtime image
FROM osrf/ros:noetic-desktop-full

# Копируем собранное из builder-стадии
COPY --from=builder /home/catkin_ws /home/catkin_ws
COPY --from=builder /home/.bashrc /home/.bashrc

# Добавляем скрипт запуска
COPY launch.sh /home/launch.sh
RUN chmod +x /home/launch.sh

# Точка входа
ENTRYPOINT [ "/home/launch.sh" ]

Скрипт запуска будет запускать пустой мир в Gazebo, в который после мы заспавним модель робота:

#!/usr/bin/env bash

. /home/.bashrc
export LIBGL_ALWAYS_SOFTWARE=1

# Запускаем пустой мир в Gazebo и ставим симуляцию на паузу
roslaunch gazebo_ros empty_world.launch paused:=true

Построим образ:

docker build ~/dockerfiles/ur_gaz_control/ --tag ur5_gaz_control

Здесь вместо текущего каталога (“.”) я полностью указал каталог, в котором лежит docker-файл. В этом случае нам не обязательно переходить в каталог с docker-файлом.

Теперь я приведу последовательность команд с комментариями, Мы создадим пустой мир, заспавним в него робот и запустим наш ROS-пакет.

Запустим контейнер:

docker run --name ur5roscontrol -it --rm \
  --env="DISPLAY" \
  --env="QT_X11_NO_MITSHM=1" \
  --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
  ur5_gaz_control

Ну или в одну строчку, чтобы удобнее копипастить:

docker run --name ur5roscontrol -it --rm --env="DISPLAY" --env="QT_X11_NO_MITSHM=1" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" ur5_gaz_control

Напомню, что: - --env="DISPLAY" передаёт в контейнер переменную окружения DISPLAY, чтобы графические приложения из контейнера могли отображаться на X-сервере хоста. - --env="QT_X11_NO_MITSHM=1" отключает использование MIT-SHM (разделяемой памяти X11), что помогает избежать проблем при работе Qt-приложений в Docker-контейнере. - --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw монтирует сокет X11 из хоста в контейнер, чтобы графические приложения могли взаимодействовать с X-сервером хоста.

Откроем новый терминал (и не выходим из него до конкретного указания) и подключаемся к контейнеру с именем ur5roscontrol:

docker exec -it ur5roscontrol bash

В подключенном контейнере загружаем ROS-окружение:

source /home/.bashrc

Спавним модель робота в Gazebo:

rosrun gazebo_ros spawn_model \
  -file `rospack find ur5-joint-position-control`/urdf/ur5_jnt_pos_ctrl.urdf \
  -urdf -x 0 -y 0 -z 0.1 -model ur5 \
  -J shoulder_lift_joint -1.5 \
  -J elbow_joint 1.0

Ну или то же опять одной строкой для удобства копирования:

rosrun gazebo_ros spawn_model -file `rospack find ur5-joint-position-control`/urdf/ur5_jnt_pos_ctrl.urdf -urdf -x 0 -y 0 -z 0.1 -model ur5 -J shoulder_lift_joint -1.5 -J elbow_joint 1.0

Запускаем контроллеры

roslaunch ur5-joint-position-control ur5_joint_position_control.launch

Теперь переходим в интерфейсе Gazebo и нажимаем кнопку “Play”, чтобы начать симуляцию.

Если нас всё получилось, в Gazebo будет такая картина:

Теперь, когда Docker-образ собран и запуск симуляции протестирован, можно приступать к изменению модели робота в формате URDF. Именно там мы добавим элементы, необходимые для взаимодействия с ros_control.

8.1.2 Изменение модели urdf

Сейчас мы разберём изменения, которые нам необходимо внести в urdf-файл, чтобы ros_control работал в Gazebo.

Во-первых, мы должны добавить плагин ros_control для Gazebo, который создаёт интерфейс между физическим движком Gazebo и контроллерами ROS, и добавить мы его должны сразу после открывающего описание робота тега <robot name="ur5">. Итак, для того, чтобы добавить плагин, вставим строки:

<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
</plugin>

У настроек плагина могут быть определены необязательные параметры <robotNamespace>, <controlPeriod>, <robotParam> и <robotSimType>, но нам сейчас достаточно их установок по умолчанию.

Во-вторых, для каждого шарнира, управление которым мы хотим моделировать в Gazebo, мы должны включить блок механической передачи <transmission>, с помощь которого мы добавляем информацию, необходимую ros_control, чтобы он мог управлять шарнирами через Gazebo. Блок <transmission> связывает шарнир (joint) с исполнительным механизмом (actuator). Это необходимо, чтобы ros_control понимал, как управлять физическим движком Gazebo через ROS, если забыть указать <transmission>, контроллер не сможет управлять шарниром.

Давайте посмотрим на такой блок для шарнира shoulder_lift_joint:

<transmission name="shoulder_lift_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="shoulder_lift_joint">
    <hardwareInterface>EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="shoulder_lift_motor">
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>

Что мы здесь видим:

параметр name – это имя нашей передачи,
параметр под тегом <type> – тип передачи, здесь выбран transmission_interface/SimpleTransmission механический редуктор, SimpleTransmission – это простой механический редуктор, который связывает двигатель с шарниром, он используется чаще всего для базовых манипуляторов, если требуется более сложная механика (например, планетарный редуктор), используются другие типы передач.
параметр под тегом <joint> – шарнир, к которому подключена передача (сторона выходного вала):
- параметр name – имя шарнира, к которому подключена передача,
- параметр под тегом <hardwareInterface> – тип управления, здесь мы выбрали управление по усилию EffortJointInterface, также возможно управление по положению PositionJointInterface и скорости VelocityJointInterface,
параметр под тегом <actuator> – исполнительный механизм, к которому подключена передача (сторона мотора):
- параметр name – имя мотора,
- параметр под тегом <mechanicalReduction> – передаточное число редуктора, здесь оно установлено равным 1.

Как я сказал ранее, такие блоки должны быть созданы для каждого шарнира, которым мы хотим управлять. Надо также отметить, что в описании должны присутствовать оба тега, и <joint> и <actuator>, иначе при выполнении может возникнуть ошибка.

Файл ur5_jnt_pos_ctrl.urdf с urdf-моделью в нашем ROS-пакете лежит в каталоге urdf.

Теперь, когда модель робота подготовлена к работе с ros_control, перейдём к настройке контроллеров. Для этого нам понадобится файл конфигурации в формате YAML, где будут указаны типы контроллеров и их параметры.

8.1.3 Файл конфигурации контроллеров

Коэффициенты усиления ПИД-регулятора и настройки контроллера должны быть сохранены в файле конфигурации в формате YAML (мы с ним уже знакомились), который будет загружен на сервер параметров при запуске с помощью launch-файла. Файл конфигурации ur5_jnt_pos_ctrl.yaml лежит в подкаталоге config нашего ROS-пакета. Для настройки контроллеров положения шарниров в ROS используется файл конфигурации в формате YAML. Файл конфигурации ur5_jnt_pos_ctrl.yaml, с которым мы работаем, хранится в подкаталоге config нашего пакета и загружается на сервер параметров при запуске с помощью launch-файла.

# Publish all joint states -----------------------------------
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

# Position Controllers ---------------------------------------
shoulder_pan_joint_position_controller:
  type: effort_controllers/JointPositionController
  joint: shoulder_pan_joint
  pid: {p: 500.0, i: 0.01, d: 50.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
shoulder_lift_joint_position_controller:
  type: effort_controllers/JointPositionController
  joint: shoulder_lift_joint
  pid: {p: 500.0, i: 100.0, d: 30.0, i_clamp_min: -400.0, i_clamp_max: 400.0}
elbow_joint_position_controller:
  type: effort_controllers/JointPositionController
  joint: elbow_joint
  pid: {p: 10000.0, i: 0.01, d: 50.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
wrist_1_joint_position_controller:
  type: effort_controllers/JointPositionController
  joint: wrist_1_joint
  pid: {p: 200.0, i: 10.0, d: 20.0, i_clamp_min: -400.0, i_clamp_max: 400.0}
wrist_2_joint_position_controller:
  type: effort_controllers/JointPositionController
  joint: wrist_2_joint
  pid: {p: 100.0, i: 0.1, d: 10.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
wrist_3_joint_position_controller:
  type: effort_controllers/JointPositionController
  joint: wrist_3_joint
  pid: {p: 100.0, i: 0.1, d: 10.0, i_clamp_min: -100.0, i_clamp_max: 100.0}

Как мы видим, в начале описывается некий joint_state_controller. С его помощью будет создан паблишер, публикующий с частотой publish_rate состояние шарниров в топик /joint_states. Это необходимо для:

Визуализации в RViz,
Работы с TF,
Подписки других узлов на состояние манипулятора.

Далее мы видим описание контроллеров. Для каждого шарнира определён свой контроллер с параметрами:

параметр type - здесь для контроллера, управляющего по усилию, мы выбрали JointPositionController – контроллер управляет положением через усилие, то есть, получает входные данные о желаемом положении и формирует, используя ПИД-регулятор, сигнал, пропорциональный моменту, который должен развивать двигатель,
параметр joint – имя шарнира, которым управляет контроллер,
параметр pid – настройка ПИД-регулятора.

Для ПИД-регулятора мы задаём следющие параметры: - p – пропорциональная составляющая; - i – интегральная составляющая; - d – дифференциальная составляющая; - i_clamp_min/max – ограничения интегральной части, чтобы избежать перерегулирования.

Почему используются разные значения ПИД? Для разных шарниров требуется разная реакция: например, плечевой шарнир (shoulder_lift_joint) имеет высокую инерцию, поэтому ему нужен более высокий i, а шарнир кисти (wrist_1_joint) чувствителен к изменениям, поэтому p меньше, чем у других. Эти значения были подобраны экспериментально, после того, как мы подготовим всё, что нужно для запуска, мы будем разбираться, как изменение этих значений влияет на движение робота.

Теперь, когда мы знаем, как выглядит файл конфигурации контроллеров, можно перейти к тому, как эти контроллеры запускать. Для этого используется launch-файл — удобный инструмент для одновременного запуска нескольких узлов и загрузки параметров. В этом разделе мы разберём launch-файл, который подгружает модель робота, активирует контроллеры и обеспечивает их работу. Вы увидите, как с помощью простых тегов <node> и <rosparam> организовать взаимодействие ros_control и Gazebo.

8.1.4 Файл запуска

Теперь мы должны разобрать launch-файл, который используется для запуска подобной симуляции. В нашем пакете файл ur5_joint_position_control.launch лежит в каталоге launch:

<launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find ur5-joint-position-control)/config/ur5_jnt_pos_ctrl.yaml" command="load"/>

  <param name="robot_description" textfile="$(find ur5-joint-position-control)/urdf/ur5_jnt_pos_ctrl.urdf"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" args="shoulder_pan_joint_position_controller shoulder_lift_joint_position_controller elbow_joint_position_controller wrist_1_joint_position_controller wrist_2_joint_position_controller wrist_3_joint_position_controller joint_state_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/ur5/joint_states" />
  </node>

</launch>

Здесь мы загружаем параметры контроллеров из нашего файла конфигурации на сервер параметров ROS, а затем загружаем файл с urdf-моделью. Эта строка загружает файл конфигурации контроллеров (ur5_jnt_pos_ctrl.yaml) на сервер параметров ROS. Именно отсюда controller_manager будет считывать информацию о типах контроллеров и их параметрах (например, значения ПИД-коэффициентов). Здесь я также хочу обратить внимание на макрос ROS для поиска пакета – $(find ur5-joint-position-control). С его помощью ROS найдёт за нас путь к каталогу, в котором лежит пакет ur5-joint-position-control. От этого пути, зная структуру пакета, мы легко можем обратиться к нужному нам файлу, не зная точного пути к пакету.

Далее с помощью пакета controller_manager мы автоматически загружаем контроллеры, перечисленные в качестве аргументов: shoulder_pan_joint_position_controller shoulder_lift_joint_position_controller elbow_joint_position_controller wrist_1_joint_position_controller wrist_2_joint_position_controller wrist_3_joint_position_controller joint_state_controller, то есть, контроллеры, которые мы описали в файле конфигурации контроллеров.

Важно понимать, что controller_spawner не просто загружает контроллеры в controller_manager, но и запускает их, если не указан флаг --stopped. Поэтому, хотя в launch-файле нет явной команды «запустить», контроллеры начинают работать сразу после запуска spawner. Таким образом, контроллеры становятся активными и готовыми к приёму команд, например, через топики вида /shoulder_pan_joint_position_controller/command.

И последним мы запускаем узел публикации состояний шарниров robot_state_publisher.

Теперь, когда мы знаем, как устроен launch-файл, мы можем запустить всю систему целиком: модель робота, контроллеры и интерфейсы взаимодействия с ROS. В следующем разделе мы проверим работу системы и научимся управлять положением шарниров.

8.1.5 Управление положением шарниров

Теперь, когда модель робота загружена, а контроллеры активированы, вы можете отправлять команды на движение прямо из терминала или графических инструментов ROS. Этот раздел покажет, как это сделать без написания собственного кода. В нём мы научимся управлять шарниром и настраивать ПИД-регуляторы через GUI.

Хоть мы и до этого не написали ни строчки кода, наш launch-файл запустил узлы, которые публикуют топики управления command для контроллеров, которые мы описали в файле ur5_jnt_pos_ctrl.yaml. Давайте выведем список всех топиков, содержащих слово command, используя grep Откроем новое окно терминала, подключимся к контейнеру и поищем топики command:

# В новом терминале подключимся к контейнеру
docker exec -it ur5roscontrol bash
# После подключения выполним команды
. /home/.bashrc
rostopic list | grep command

Мы нашли шесть топиков, в которые мы можем публиковать команды управления шарнирами в формате std_msgs/Float64. Не спешите использовать rostopic pub, давайте воспользуемся средством rqt, о котором мы говорили ранее.

Пакет rqt – это графическая оболочка ROS, которая объединяет множество полезных инструментов для отладки и управления узлами. В этом разделе мы воспользуемся rqt для отправки команд управления шарнирам через плагин Message Publisher. Для визуализации переходных процессов будем использовать плагин Plot. Плагин Dynamic Reconfigure даст нам возможность менять параметры контроллеров (например, коэффициенты ПИД-регулятора) в реальном времени без перезапуска узлов. Благодаря своей модульности и удобству использования, rqt широко применяется как при разработке, так и при отладке робототехнических систем. Одним из “побочных эффектов” этого урока станет то, что вы достаточно близко познакомитесь с rqt – мощным инструментом для тестирования и настройки систем управления роботами.

Давайте теперь запустим rqt. Запустить его можно командой rqt или rosrun rqt_gui rqt_gui. Запустим и посмотрим в меню плагинов, где выберем плагин Message Publisher:

Вызов плагина Message Publisher через меню rqt

После чего нужный нам плагин откроется в rqt. Давайте выберем из списка топиков /wrist_1_joint_position_controller/command и нажмём клавишу добавления (Плюс) топика:

Теперь мы можем с нужной нам частотой публиковать нужные нам значения. Здесь, например, я установил частоту 100 Гц и с этой частотой посылаю целевое положение шарнира в -1,57 радиана, что робот и отработает:

Положение UR5 после отработки команды перехода в целевую точку

Зачем я использую приложение, если мог просто воспользоваться rostopic? Например, у rqt есть отличная возможность задать выражение, значения которого будут публиковаться. Если мы напишем sin(0.1*i), где i – переменная внутреннего времени rqt, то будем подавать синусоиду, хотя опять не напишем ни строчки кода. Но это мелочи. Главное, что мы воспользуемся и другими плагинами rqt, когда будем настраивать ПИД-регулятор. Зачем я использую приложение, если мог просто воспользоваться rostopic? Дело в том, что у rqt есть гораздо больше возможностей, чем просто отправка команд. Например, плагин Message Publisher позволяет не только вручную задавать значения, но и автоматически генерировать сигналы – достаточно указать выражение, например, sin(0.1*i), где i– это внутреннее время rqt. Таким образом, вы можете легко тестировать реакцию системы на сложные входные сигналы без написания отдельного узла.

Однако основная польза rqt проявится для нас на этом этапе при настройке систем управления, таких как ПИД-регуляторы. Именно для этого мы сейчас подготовим всё необходимое, чтобы наглядно продемонстрировать влияние каждого параметра регулятора на движение шарнира.

Подготовка к настройке ПИД-регулятора контроллера

Ранее мы подключили контроллеры положения шарниров через ros_control. Сейчас используется тип JointPositionController, который получает команду – желаемое угловое положение шарнира, а выдаёт сигнал управления, который по сути является усилием (моментом), которое должен развить двигатель, чтобы достичь этой позиции. За это отвечает алгоритм, называемый ПИД-регулятором (Пропорционально-Интегрально-Дифференцирующим).

Что же такое ПИД-регулятор?

Представьте упражнение “Эстакада” на экзамене по вождению: нужно привести машину по наклонной поверхности в определённое положение на вершине эстакады, не скатываясь назад и не переезжая верхнюю отметку. ПИД-регулятор работает аналогично — он динамически регулирует “газ” (усилие на мотор), используя три компоненты:
- пропорциональная составляющая, - интегральная составляющая, - дифференциальная составляющая.

Далее будут формулы, для которых введёмм следующие обозначения: - $p_{error}$ – разница между целевым и фактическим положением шарнира, - $K_{p}$, $K_{i}$, $K_{d}$ – настраиваемые П, И, Д-коэффициенты усиления, - $τ_{p}, τ_{i}, τ_{d}$ – П, И, Д-компоненты сигнала, - $τ$ – выходной сигнал регулятора (усилие).

Пропорциональная составляющая (P) сразу даёт усилие пропорционально текущей ошибке по принципу “Чем дальше от цели – тем сильнее газ”. Пока вершина эстакады далеко, давим на газ не стесняясь: \[\tau_p = K_p \times p_{error}\]

Интегральная составляющая (I) корректирует систематические ошибки. Если в нашем примере машина постепенно останавливается или даже скатывается назад из-за скользкой поверхности, вы плавно добавляете газ: \[\tau_i = K_i \times \int_0^t p_{error}(t) \, dt\]

Дифференциальная составляющая (D) сглаживает резкие изменения, предотвращая “перелёт”. Приближаясь к вершине или попав на более шероховатую поверхность газ убираем, чтобы не перескочить. Реагирует на скорость изменения ошибки: \[\tau_d = -K_d \times \frac{dp_{error}}{dt}\] Важно понимать, что компоненты не работают изолированно – они непрерывно взаимодействуют. P-составляющая даёт основное усилие, I-составляющая корректирует долговременные отклонения, D-составляющая стабилизирует процесс. ПИД-регулятор – это баланс между реакцией на ошибку (P), компенсацией помех (I), плавностью движения (D).

Все три компоненты работают одновременно и взаимосвязано, создавая точное управляющее воздействие:

\[ \tau = K_p \cdot p_{error} + K_i \cdot \int_0^t p_{error}(t) \, dt - K_d \cdot \frac{dp_{error}}{dt} \]

Теперь, когда мы получили представление, что именно регулируем, пришло время посмотреть на реакцию привода нашего манипулятора на управляющие команды. Для этого воспользуемся мощным инструментом ROS – плагином Plot из интерфейса rqt. Он покажет нам графики переходных процессов, протекающих в симуляции, и поможет понять, какие коэффициенты требуют корректировки.

Почему важна корректировка?

Если мы подберём недостаточные коэффициенты: - P – система будет медленно реагировать, может не достичь цели, - I – останется постоянная ошибка, не компенсируются возмущения, - D – возникнут колебания, перерегулирование.

Для избыточных коэффициентов: - P – в системе появятся резкие колебания вокруг цели, - I – возникнет “перегревание” – нарастающие колебания, - D – заторможенность, вялая реакция системы.

Для того чтобы посмотреть графики процессов, протекающих в симуляции, воспользуемся плагином рисования графиков по данным из топиков ROS Plot, вызвав его через меню rqt. В меню выберем плагин Plot (Visualization → Plot):

В поле Topic мы имеем возможность выбрать топик из списка публикуемых сейчас топиков, а знак Плюс позволяет добавить вывод данных этого топика на график. Добавим следующие топики:

/wrist_1_joint_position_controller/state/process_value – текущее положение шарнира;
/wrist_1_joint_position_controller/command/data – целевое значение положения.

Нажмите кнопку ✚ (Add Topic), чтобы добавить оба значения на график. Теперь мы сможем увидеть, как робот реагирует изменением текущего положения шарнира на управляющий сигнал – целевое положение шарнира.

Если топик не отображается или не добавляется, проверьте его имя и убедитесь, что данные имеют числовой формат. Иногда ошибки возникают из-за опечаток или неправильных типов сообщений.

После добавления топиков график будет выглядеть примерно так:

Также подключим плагин Dynamic Reconfigure, который позволит менять параметры контроллера в реальном времени. Он находится в разделе Configuration → Dynamic Reconfigure. После загрузки выберите контроллер wrist_1_joint_position_controller, чтобы получить доступ к его коэффициентам ПИД-регулятора:

Вызов плагина Dynamic Reconfigure через меню rqt

Теперь настроим отображение плагинов так, чтобы нам было удобно. У плагина Dynamic Reconfigure выберем настройку pid для контроллера wrist_1_joint_position_controller, а у плагина Plot выберем инструмент масштабирования и перемещения и перемещая мышь с зажатой правой клавишей справа налево изменим масштаб по оси времени, чтобы нам было видно, что происходит:
Готовый к нашим экспериментам rqt
Теперь установите шарнир в начальное положение (например, 0 радиан), заменив синусоиду в Message Publisher на значение 1. Это подготовит систему к началу настройки ПИД-регулятора.

Настройка пропорциональной части

Пропорциональная часть нашего контроллера просто умножает ошибку $p_{error}$ между желаемой позицией и текущей позицией на коэффициент $K_p$. Так что, если мы увеличим $K_p$, контроллер быстрее и точнее достигнет нужного положения, но колебания увеличатся. Кроме того, если имеется только $K_p$, нам будет необходимо установить его очень высоким, чтобы иметь возможность достичь точного целевого положения. Чтобы продемонстрировать влияние различных коэффициентов усиления, давайте установим p = 20, i = 0,0 и d = 0,0 в нашем плагине Dynamic Reconfigure

Пропорциональная составляющая $K_p$ контроллера управляет скоростью достижения целевого положения шарнира. Она вычисляет ошибку между желаемым и текущим положением шарнира $p_{error}$ и умножает её на коэффициент усиления $K_p$. Это создаёт сигнал управления, который заставляет двигатель развивать усилие, необходимое для корректировки позиции.

Вспомним, как $K_p$ влияет на систему: - При низком $K_p$ контроллер медленно реагирует на ошибку, что приводит к длительному времени переходного процесса. - При высоком $K_p$ контроллер быстрее достигает целевой точки, но может возникнуть перерегулирование (колебания вокруг целевой позиции), так как система становится слишком чувствительной к ошибкам.

Теперь давайте откроем плагин Dynamic Reconfigure в rqt, выберем контроллер wrist_1_joint_position_controller и установим следующие параметры: - p=20, - i=0, - d=0.

Мы установили интегральную и дифференциальную части ПИД-регулятора в ноль, чтобы изолировать влияние пропорциональной части на поведение системы. Это позволяет нам чётко увидеть, как реагирует шарнир при управлении только за счёт текущей ошибки положения, без сглаживания колебаний Д-частью и компенсации статической ошибки И-частью. Такой подход помогает лучше понять роль каждого параметра по отдельности.

Настройка параметров ПИД-регулятора в плагине Dynamic Reconfigure:

Интерфейс динамической настройки ПИД-регулятора

С помощью плагина Message Publisher подадим целевое значение положения шарнира равное -1 радиан:

Процесс отработки целевого положения только с P-компонентой

Кстати, если вы используете инструмент масштабирования и перемещения, вы сможете изучить график более детально. А если процесс на графике от вас “убежал”, снимите галочку с автоперемещения (autoscroll) и перемещайте график вручную. С помощью инструмента масштабирования выделенного (лупа), можно обвести область, которую мы хотим увеличить:

На графике показан переходный процесс шарнира при настройке ПИД-регулятора с параметрами p = 20, i = 0, и d = 0. Шарнир начинает движение от начального положения 0 радиан к целевому значению -1 радиан. Фактически, мы можем наблюдать реакцию системы, определяемую исключительно пропорциональной частью.

При заданном коэффициенте p = 20, шарнир быстро реагирует на ошибку (разницу между текущим и целевым положением). Однако, из-за отсутствия интегрирования и демпфирования, система становится чувствительной к динамике – то есть к скорости и величине отклонения от целевого положения. После достаточно быстрого достижения целевой точки шарнир проскакивает её, затем также быстро возвращается и в результате начинает колебаться вокруг неё. Это происходит потому, что пропорциональная составляющая продолжает генерировать сигналы управления даже после достижения цели, вызывая перерегулирование. Со временем колебания затухают благодаря внутренним демпфирующим свойствам системы или физическим ограничениям, но они остаются заметными.

Система достигает целевой точки быстро, но имеет значительные колебания из-за отсутствия интегрирования и демпфирования. Поскольку $K_d=0$, нет демпфирующей силы, которая могла бы “гасить” скорость движения, и механизм по инерции проскакивает заданную точку. Затем ошибка меняет знак, и процесс повторяется в обратном направлении, создавая затухающие колебания. Без интегральной составляющей $K_i$ система также не компенсирует статическое трение (управляющего сигнала не хватает для преодоления силы трения в шарнире), что может усиливать неточность. Для улучшения стабильности рекомендуется добавить коэффициенты дифференцирующей и интегрирующей компонент.

Настройка дифференцирующей части

Дифференциальная составляющая используется для демпфирования системы, то есть для снижения колебаний после достижения целевой точки. Она учитывает скорость изменения положения шарнира, которая рассчитывается как производная ошибки положения шарнира по времени. Затем эта скорость умножается на коэффициент дифференцирующей части, и результат вычитается из выходного сигнала контроллера:

\[\tau_d = -K_d \times \frac{dp_{error}}{dt}\] Дифференциальная составляющая действует как “гидравлический амортизатор” в системе - она создает управляющий сигнал, противоположный скорости движения. Чем быстрее шарнир приближается к цели, тем сильнее D-компонента его “притормаживает”, позволяя системе меньше реагировать на резкие изменения скорости, что снижает вероятность перерегулирования, но и увеличивая общее время выхода на заданную позицию.

Давайте настроим дифференциальную компоненту с помощью плагина Dynamic Reconfigure, установив $K_d = 5$ (пропорциональную часть оставим такой, как в предыдущем подразделе), зададим целевое положение -1 рад и проследим за поведением системы.

При увеличении d до 5 мы наблюдаем следующее: - Шарнир стремится к целевому положению со значительно меньшей скоростью, так как демпфирование становится более активным. - Колебания практически исчезают, что делает движение более плавным.

Процесс с добавлением дифференциальной части

Для сравнения посмотрим, как будет вести себя система при отсутствии дифференцирующей компоненты.

Давайте вернём шарнир в нулевой положение, задав с помощью плагина Message Publisher целевое положение равным нулю и дождёмся окончания переходных процессов. Теперь установим в плагине Dynamic Reconfigure $K_d=0$ и подадим снова целевое положение -1 рад.

Процесс без дифференциальной части для сравнения

Сравним два графика:

Первый график для d = 5. Мы видим, что система достигает целевой точки без заметных колебаний, но время переходного процесса увеличивается.

Второй график. Здесь только пропорциональная часть, а d = 0. Колебания вокруг целевой точки очевидны, хотя система быстрее достигает цели почти в три раза.

Добавление ненулевого $K_d$ значительно снижает колебания, но увеличивает время движения к цели почти в три раза. Это демонстрирует важную характеристику дифференциальной части: она обеспечивает плавность движения, но может замедлять систему.

Настройка интегрирующей части

Интегрирующая составляющая используется для устранения статической ошибки, то есть постоянного отклонения системы от целевой точки, которое не устраняется пропорциональной частью. Она рассчитывается как интеграл ошибки положения шарнира по времени: \[\tau_i = K_i \times \int_0^t p_{error}(t) \, dt\] Это позволяет системе накапливать информацию об ошибках и корректировать движение даже при малых отклонениях, но также может привести к перерегулированию, если выбрать $K_i$ неверно. Например, добавив к предыдущим настройкам (p = 20 и d = 5) коэффициент $K_i=10$, мы получим такой переходной процесс для отработки целевого положения -1 рад:

Процесс с добавлением интегрирующей части

Можно отметить, что система быстро реагирует на команду и стремится достичь целевой точки, однако статическая ошибка остаётся заметной даже после достижения целевой точки. Вспомним, что в при настройке на предыдущих этапах мы добились того, что фактическое положение не убегало за целевое. Теперь же красная линия (фактическое положение) уходит “под” голубую линию (команда), что указывает на статическую ошибку. Это происходит потому, что система продолжает “накапливать” ошибку, даже когда достигла целевой точки, из-за высокого значения коэффициента.

Попробуем от перерегулирования избавиться. Вернём шарнир в нулевое положение, задав с помощью плагина Message Publisher целевое положение равным нулю и дождёмся, пока система стабилизируется.

В плагине Dynamic Reconfigure есть возможность настроить параметры i_clamp_min и i_clamp_max. Эти параметры используются в ПИД-регуляторах для ограничения интегральной части регулятора, чтобы предотвратить интегральное насыщение. Они задают минимальное и максимальное значения, до которых может расти накопленная ошибка (интеграл), влияя на выходной сигнал контроллера. Как вы думаете, какие значения для этих параметров установлены на данный момент? Могу точно сказать, какие не установлены – там точно не нули, так как мы изменяли $K_i$, и видели явное влияние его изменения на процесс отработки целевой точки. Нули “зажали” бы интегрирующую часть так, что смысла в ней бы не было.

Давайте коэффициенты для пропорциональной и дифференциальной части оставим прежними, 20 и 5 соответственно, а для интегрирующей компоненты установим: - i = 0.1 - i_clamp_min = -200 - i_clamp_min = 200

Теперь подадим целевое положение в -1 рад и получим такой график:

Процесс после настройки интегрирующей части

Видим, что система также быстро реагирует на команду и стремится достичь целевой точки. Однако благодаря более низкому значению $K_i$ и ограничениям i_clamp_min и i_clamp_max , статическая ошибка становится меньше. Фактическое положение лучше следует за целевым, что говорит о том, что статическая ошибка значительно снижена. В отличии от первого графика, где красная линия уходила под голубую, здесь она остается близко к целевому положению.

И всё же, не очень понятно, какую статическую ошибку должна устранить интегрирующая часть. Статическая ошибка – это постоянное отклонение реального положения шарнира от целевого значения, которое остаётся даже после достижения цели. Это может происходить из-за внешних факторов, таких как трение, которое мы моделируем в системе.

В исходной модели URDF все шарниры имеют нулевое трение (friction=0). Однако в реальных роботах трение всегда присутствует. Если использовать только пропорциональную часть без интегрирующей, система будет стремиться достичь целевой точки, но не сможет полностью компенсировать постоянные силы трения – рассчитанный управляющий сигнал будет слишком мал. В результате шарнир останется в состоянии статической ошибки – он будет находиться чуть ниже или выше целевого значения.

Я провёл эксперимент. Чтобы продемонстрировать, как интегрирующая часть помогает устранить эту ошибку, я искусственно добавил трение в один из шарниров (wrist_1_joint), оставил только пропорциональную часть ПИД-регулятора с $K_p=20$ и подал целевое положение в -1 радиан.

Переходной процесс только с пропорциональной частью выглядел так:

Шарнир приближается к целевой точке, но не достигает её полностью из-за трения. Система “застывает” в небольшом отклонении от цели.

Затем я добавил интегрирующую часть с $K_i=10$, которая позволяет системе накапливать ошибку со временем и формировать дополнительный сигнал управления, чтобы компенсировать постоянные силы трения. Это помогает шарниру полностью достичь целевой точки, даже если есть трение.

При подключении интегрирующей части из того же исходного положения команда отработала так:
Процесс с пропорциональной и интегрирующей компонентами для шарнира с трением

После добавления интегрирующей части шарнир полностью достигает целевой точки, хотя трение всё ещё присутствует. Интегрирующая часть компенсирует постоянную ошибку, которую не могла устранить пропорциональная часть. Интегрирующая часть (Ki ) помогает устранить статическую ошибку , которая возникает из-за трения или других постоянных сил. Она накапливает ошибку со временем и формирует дополнительный сигнал управления, который компенсирует эти силы. Однако важно правильно настроить Ki , чтобы избежать перерегулирования.

Мы научились управлять шарнирами манипулятора с помощью ПИД-контроллеров и rqt. Настройка параметров $K_p, K_i, K_d$ позволила генерировать различные переходные процессы, за которыми мы могли наблюдать с помощью плагина Plot, визуализирущего поведение системы. Это дало понимание влияния каждой составляющей регулятора на стабильность и точность движения. После того как вы научились управлять отдельными шарнирами манипулятора, пришло время перейти к более сложной и практической задаче – управлению всей конечной точкой робота в декартовом пространстве.

8.2 Управление движением конечной точки робота с помощью Orocos’ KDL

Ранее мы научились управлять положением подвижных соединений робота. Однако одной из основных “способностей” манипулятора является возможность управлять некой конечной точкой (tool center point – TCP), с которой у нас может быть жестко связано захватное устройство или любой другой инструмент, взаимодействующий с окружением. Наша цель – задать позицию TCP в декартовом пространстве и обеспечить перемещение робота в эту точку. Вот здесь, на стыке управления отдельными соединениями робота и непосредственного управления позицией TCP, появляются понятия прямой и обратной кинематики.

Прямая кинематика позволяет вычислить положение конечной точки (TCP) по известным углам шарниров. Обратная кинематика решает противоположную задачу – находит углы шарниров, при которых TCP попадёт в заданную точку.

Для понимания того, как в ROS можно решать кинематические задачи, мы создадим ROS-пакет с кодом на C++, который позволит нам задавать точку TCP в декартовом пространстве, а шарниры будут отрабатывать перемещение в эту точку. Мы создадим узел ROS, который использует KDL (Kinematics and Dynamics Library) для кинематических расчетов и взаимодействует с роботом, который мы смоделировали ранее в Gazebo. Существуют разные библиотеки для решения обратной кинематики: KDL, Trac-IK, IKFast. Мы выбрали KDL как одну из наиболее распространённых и простых для освоения.

8.2.1 Подготовка и настройка

Давайте снова создадим docker-файл для наших опытов. Мы возьмём docker-файл, который мы приготовили для работы с ros_control и немного дополним его. Склонируем пакет управления конечной точкой робота UR5:

# Не надо выполнять эту команду в терминале, поместим её в docker-файл
git clone https://github.com/dairal/ur5-tcp-position-control.git

Итак, после строк:

WORKDIR /home/catkin_ws/src
RUN git clone https://github.com/dairal/ur5-joint-position-control.git

добавим строку:

RUN git clone https://github.com/dairal/ur5-tcp-position-control.git

Ещё я в прошлом файле напрасно поставил клонирование ROS-пакетов до установки ROS-зависимостей, в этот раз такая последовательность приведёт к ошибке, поэтому сдвинем строки клонирования ниже строки работы с rosdep. Финальная версия docker-файла у меня получилась такая:

FROM osrf/ros:noetic-desktop-full

# RUN apt update -y && apt upgrade -y && apt install git -y
RUN apt update -y && \
    apt upgrade -y && \
    apt install -y \
        git \
        ros-$ROS_DISTRO-ros-control \
        ros-$ROS_DISTRO-ros-controllers && \
    rm -rf /var/lib/apt/lists/*
    
# RUN echo "source /opt/ros/noetic/setup.bash" >> /home/.bashrc
RUN echo "source /opt/ros/noetic/setup.bash" >> /home/.bashrc && \
    echo "source /home/catkin_ws/devel/setup.bash" >> /home/.bashrc
# RUN apt update -y && apt upgrade -y && apt install ros-$ROS_DISTRO-ros-control ros-$ROS_DISTRO-ros-controllers -y

# WORKDIR /home/catkin_ws/src
# RUN git clone -b $ROS_DISTRO-devel https://github.com/ros-industrial/universal_robot.git

WORKDIR /home/catkin_ws
RUN rosdep update && rosdep install --rosdistro $ROS_DISTRO --ignore-src --from-paths src -y

WORKDIR /home/catkin_ws/src
RUN git clone -b $ROS_DISTRO-devel https://github.com/ros-industrial/universal_robot.git && \
    git clone https://github.com/dairal/ur5-joint-position-control.git && \
    git clone https://github.com/dairal/ur5-tcp-position-control.git
# RUN git clone https://github.com/dairal/ur5-joint-position-control.git
# RUN git clone https://github.com/dairal/ur5-tcp-position-control.git

# WORKDIR /home/catkin_ws
RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make'
# RUN echo "source /home/catkin_ws/devel/setup.bash" >> /home/.bashrc

COPY launch.sh /home/launch.sh
RUN chmod +x /home/launch.sh

ENTRYPOINT [ "/home/launch.sh" ]

Ещё мы исправим наш скрипт запуска, он запустит launch-файл, который запустит Gazebo, подгрузит модель робота и поставит Gazebo на паузу. Давайте я всё-таки приведу текст launch-файла:

<launch>

  <include file="$(find gazebo_ros)/launch/empty_world.launch" >
    <arg name="paused" value="true" />
  </include>

  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-file $(find ur5-joint-position-control)/urdf/ur5_jnt_pos_ctrl.urdf -urdf -x 0 -y 0 -z 0.1 -model ur5 -J shoulder_lift_joint -1.5 -J elbow_joint 1.0" />

  <include file="$(find ur5-joint-position-control)/launch/ur5_joint_position_control.launch" />

</launch>

Здесь запускается пустой мир в Gazebo, в него спавнится робот в нужном положении, а потом запускается внешний launch-файл (да, так можно), созданный для работы с контроллерами.

Чтобы запустить этот пакет в Docker, создадим скрипт запуска, который будет выглядеть так:

#!/usr/bin/env bash

. /home/.bashrc
export LIBGL_ALWAYS_SOFTWARE=1

roslaunch ur5-joint-position-control ur5_gazebo_joint_position_control.launch

Построим образ и запустим его:

docker build ~/dockerfiles/ur_gaz_kdl --tag ur5_gaz_kdl
docker run --name ur5kinemat -it --rm --env="DISPLAY" --env="QT_X11_NO_MITSHM=1" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" ur5_gaz_kdl

У меня получилось так, что робот находится в положении с нулевыми координатами в шарнирах (лежит на земле) и Gazebo на паузе:

Робот в Gazebo после запуска – все углы шарниров нулевые

И после нажатия на кнопку “воспроизведения”, то есть, начала симуляции, шарниры робота находятся в положении, заданном в launch-файле:

Робот в Gazebo после старта симуляции – углы шарниров заданные

После того как Docker-образ собран, Gazebo запущен, модель робота загружена, и все необходимые контроллеры активированы, можно приступать к управлению движением конечной точки (TCP).

Для этого мы будем использовать специальный ROS-узел, реализованный в C++. Он использует библиотеку Orocos’ KDL для расчёта обратной кинематики и взаимодействует с контроллерами, чтобы обеспечить точное перемещение TCP в заданную точку пространства.

Узел называется tcp_position_controller и находится в пакете ur5-tcp-position-control. Чтобы запустить его, выполните команду:

# В новом терминале подключимся к контейнеру
docker exec -it ur5kinemat bash
# После подключения выполним команды
. /home/.bashrc
rosrun ur5-tcp-position-control tcp_position_controller

Если нода стартовала, как положено, она даст нам текущее положение и ориентацию робота в мировой СК Gazebo и предложит ввести смещение положения относительно текущего и время, за которое робот должен его отработать:

Давайте по осям x и y смещаться не будем, установим значение сдвига равным нулю, а по оси z сместимся на -0.6 м, что должно привести наш робот своим инструментом практически на “землю”. Пусть движение длится 10 секунд. При выполнении перемещения робот двигался довольно некрасиво, контроллеры шарниров у нас настроены не оптимально, но в ожидаемую точку робот очевидно пришёл:

Робот в Gazebo после отработки целевого положения

После того как мы подготовили Docker-образ, загрузили модель робота в Gazebo и успешно запустили узел управления конечной точкой (TCP), мы получили возможность наблюдать за движением манипулятора в ответ на заданное смещение TCP. Робот двигался, пусть и не идеально плавно — это связано с тем, что контроллеры шарниров пока настроены не оптимально.

Но как именно работает этот узел?

Писать подобный контроллер с нуля на данном этапе ещё рано — для этого нужно понимание работы с библиотекой Orocos’ KDL, а также хорошее знание принципов обратной кинематики. Однако разобрать основную логику работы этого узла будет очень полезно.

Как мы уже знаем, этот ROS-узел реализует управление движением TCP (tool center point) с использованием библиотеки KDL, которая позволяет решать задачи прямой и обратной кинематики. В основе всего — построение кинематической цепочки из URDF-модели, вычисление текущего положения TCP и расчёт новых углов шарниров, чтобы переместить TCP в целевую точку.

Перейдём к подробному разбору.

8.2.2 Код узла и его обсуждение

Давайте рассмотрим главную функцию main() этого узла, в которой реализованы следующие ключевые шаги: 1. Загрузка модели робота из URDF. 2. Создание кинематической цепочки с помощью KDL. 3. Подписка на текущие состояния шарниров. 4. Публикация команд управления шарнирами. 5. Использование решателей KDL для прямой и обратной кинематики. 6. Интерполяция траектории движения и отправка целевых позиций шарнирам.

Этот код поможет нам понять, как взаимодействуют между собой ROS, Gazebo, модель робота и библиотека KDL, чтобы обеспечить управление в декартовом пространстве.

int main(int argc, char **argv)
{
//Получаем строку с путём, по которому расположен пакет ur5-joint-position-control
//Если пакета нет, прекращаем работу с сообщением об ошибке. Кстати, где мы здесь прекращаем работу -- решительно непонятно. Ошибка.
//Иначе дополняем строку именем каталога urdf и urdf-файла
  std::string urdf_path = ros::package::getPath("ur5-joint-position-control");
  if(urdf_path.empty()) {
    ROS_ERROR("ur5-joint-position-control package path was not found");
  }
  urdf_path += "/urdf/ur5_jnt_pos_ctrl.urdf";
//Инициируем узел с именем tcp_control
  ros::init(argc, argv, "tcp_control");
//Всё не так, как в Питоне. Переменная n нам нужна, чтобы связать ROS с функциями узла, паблишер или сабскрайбер прицепить, например
  ros::NodeHandle n;
//Как и для rospy, спооб управления приостановкой потока
  ros::Rate loop_rate(loop_rate_val);

//Создать подписчики для состояний всех шарниров, это те топики, по данным которых мы выше рисовали графики.
//Первый параметр -- имя топика, второй параметр -- размер очереди, третий -- callback-функция на появление нового сообщения.
//В callback-функциях мы будем присваивать полученное значение соответствующему элементу массива положений шарниров KDL::JntArray jnt_pos_start
  ros::Subscriber shoulder_pan_joint_sub = n.subscribe("/shoulder_pan_joint_position_controller/state", 1000, get_shoulder_pan_joint_position);
  ros::Subscriber shoulder_lift_joint_sub = n.subscribe("/shoulder_lift_joint_position_controller/state", 1000, get_shoulder_lift_joint_position);
  ros::Subscriber elbow_joint_sub = n.subscribe("/elbow_joint_position_controller/state", 1000, get_elbow_joint_position);
  ros::Subscriber wrist_1_joint_sub = n.subscribe("/wrist_1_joint_position_controller/state", 1000, get_wrist_1_joint_position);
  ros::Subscriber wrist_2_joint_sub = n.subscribe("/wrist_2_joint_position_controller/state", 1000, get_wrist_2_joint_position);
  ros::Subscriber wrist_3_joint_sub = n.subscribe("/wrist_3_joint_position_controller/state", 1000, get_wrist_3_joint_position);

//Создать паблишеры для управления приводами, это те топики, в один из которых мы посылали выше сигнал управления
//Первый параметр -- имя топика, второй параметр -- размер очереди.
//Чтобы ниже использовать в цикле, паблишеры соберём в массив
  ros::Publisher joint_com_pub[6];
  joint_com_pub[0] = n.advertise<std_msgs::Float64>("/shoulder_pan_joint_position_controller/command", 1000);
  joint_com_pub[1] = n.advertise<std_msgs::Float64>("/shoulder_lift_joint_position_controller/command", 1000);
  joint_com_pub[2] = n.advertise<std_msgs::Float64>("/elbow_joint_position_controller/command", 1000);
  joint_com_pub[3] = n.advertise<std_msgs::Float64>("/wrist_1_joint_position_controller/command", 1000);
  joint_com_pub[4] = n.advertise<std_msgs::Float64>("/wrist_2_joint_position_controller/command", 1000);
  joint_com_pub[5] = n.advertise<std_msgs::Float64>("/wrist_3_joint_position_controller/command", 1000);

//Библиотека KDL разбирает, что написано в urdf-файле и составляет для себя древовидную представление нашего робота ur5_tree
//На если не получилось -- не судьба, то есть, сообщение об ошибке и выход
    KDL::Tree ur5_tree;
  if (!kdl_parser::treeFromFile(urdf_path, ur5_tree)){
    ROS_ERROR("Failed to construct kdl tree");
       return false;
  }
//Теперь из дерева строится кинематическая цепь ur5_chain
//Мы можем построить кинематическую цепь от звена до звена, значит мы должны указать начальное и конечное звено.
//здесь это base_link и wrist_3_link соответственно
  KDL::Chain ur5_chain;
  ur5_tree.getChain("base_link", "wrist_3_link", ur5_chain);

//Теперь создадим решатели:
// для прямой кинематики fk_solver от цепи ur5_chain
// вспомгательный vel_ik_solver для расчета скоростей обратной кинематики
// для расчёта положений обратной кинематики ik_solver, ему мы отдадим vel_ik_solver и два "магических числа", с которыми разберёмся позже

    //Create solvers
  KDL::ChainFkSolverPos_recursive fk_solver(ur5_chain);
  KDL::ChainIkSolverVel_pinv vel_ik_solver(ur5_chain, 0.0001, 1000);
  KDL::ChainIkSolverPos_NR ik_solver(ur5_chain, fk_solver, vel_ik_solver, 1000);

//Крутанём спиннер, чтобы гарантировать получение углов шарниров (так себе решение, но ... окей)
  for(int i=0; i< 2; i++) {
    ros::spinOnce();
     loop_rate.sleep();
  }

//Посчитаем шаг для интерполяции траектории шарниров. Основной цикл узла будет работать с частотой 100 Гц,
//loop_rate_val был задан в основном теле программы const int loop_rate_val = 100;
    const float t_step = 1/((float)loop_rate_val);
  int count = 0;
//Основной цикл
    while (ros::ok()) {

//Вычислим стартовое положение TCP
//Используем для этого функцию JntToCart решателя ПЗ.
//Дадим ему массив jnt_pos_start, который получаем в подписчиках,
//а получим положение в переменную tcp_pos_start через параметр функции С++
    KDL::Frame tcp_pos_start;
    fk_solver.JntToCart(jnt_pos_start, tcp_pos_start);

//Распечатаем текущее положение
        ROS_INFO("Current tcp Position/Twist KDL:");
    ROS_INFO("Position: %f %f %f", tcp_pos_start.p(0), tcp_pos_start.p(1), tcp_pos_start.p(2));
    ROS_INFO("Orientation: %f %f %f", tcp_pos_start.M(0,0), tcp_pos_start.M(1,0), tcp_pos_start.M(2,0));

//Получить от пользователя данные о времени t_max и смещении,
//на основе которых будет сформировано целевое положение vec_tcp_pos_goal
        float t_max;
    KDL::Vector vec_tcp_pos_goal(0.0, 0.0, 0.0);
//Данные формируются в этой "самописной" функции. Приводить я её не стал, она очень простая:
//Получаем данные о желаемом смещении по осям x, y, z от пользователя,
//к текущим положениям по осям добавляем введённое смещение, например, (*vec_tcp_pos_goal)(0) = (tcp_pos_start.p(0) + x) для x,
//и получаем вектор с целевым положением vec_tcp_pos_goal возвращаем через параметр
        get_goal_tcp_and_time(tcp_pos_start, &vec_tcp_pos_goal, &t_max);
//На основе матрицы поворота стартовой позы (назовём так перемещение и поворот вместе) и вектора смещения, строим целевую позу
//Отсюда понятно, что повороты в декартовом пространстве мы делать не будем
    KDL::Frame tcp_pos_goal(tcp_pos_start.M, vec_tcp_pos_goal);

//Вычисляем положение шарниров для целевой позы с помощью решателя ik_solver
//Строим на основе текущих положений шарниров jnt_pos_start, целевой позы tcp_pos_goal, результат функция вернёт через параметр jnt_pos_goal,
//где jnt_pos_goal -- массив из Joints = 6 элементов
        KDL::JntArray jnt_pos_goal(Joints);
    ik_solver.CartToJnt(jnt_pos_start, tcp_pos_goal, jnt_pos_goal);

//Цикл управления шарнирами
        float t = 0.0;
//Пока не вышло заданное время
        while(t<t_max) {
      std_msgs::Float64 position[6];
//Вычислим и опубликуем сообщение с координатами шести шарниров
      for(int i=0; i<Joints; i++) {
//C помощью самописной функции вычисляем положение шарнира на текущем шаге по формуле
//((jnt_pos_goal(i) - jnt_pos_start(i)) * (t/t_max) + jnt_pos_start(i));
        position[i].data = compute_linear(jnt_pos_start(i), jnt_pos_goal(i), t, t_max);
//Посылаем вычесленное значение в соответствующий шарниру паблишер
        joint_com_pub[i].publish(position[i]);
      }
//Крутанём спиннер
      ros::spinOnce();
//Притормозим поток
            loop_rate.sleep();
      ++count;
//Сдвинем время на шаг
      t += t_step;
    }
  }
  return 0;
}

Я постарался подробно прокомментировать код, но всё предлагаю обсудить его.

Функция main() реализует узел ROS для управления движением конечной точки (TCP) робота UR5 с использованием библиотеки Orocos’ KDL.

Сначала программа получает путь к URDF-файлу модели робота и проверяет его наличие. Если путь не найден, выводится сообщение об ошибке. Затем инициализируется ROS-узел tcp_control, создаются подписчики на топики состояния шарниров и паблишеры команд для контроллеров.

Далее загружается модель робота в виде дерева KDL из URDF-файла. Если это не удалось — выводится ошибка. Из этого дерева строится кинематическая цепочка от base_link до wrist_3_link.

Создаются решатели:
- прямой кинематики (ChainFkSolverPos_recursive) — для вычисления положения TCP по углам шарниров;
- обратной кинематики (ChainIkSolverVel_pinv, ChainIkSolverPos_NR) — для расчёта углов шарниров по целевой позиции TCP.

После начальной синхронизации данных с датчиков шарниров запускается основной цикл работы узла. В нём:
1. Вычисляется текущее положение TCP с помощью решателя прямой кинематики.
2. Получается целевое положение TCP на основе пользовательского ввода (смещение и время движения).
3. С помощью решателя обратной кинематики определяются целевые углы шарниров.
4. Для интерполяции траектории используется линейная функция, которая постепенно перемещает шарниры за заданное время.
5. Рассчитанные значения отправляются через паблишеры на контроллеры шарниров.

В конце цикл повторяется, обеспечивая непрерывное управление.

Код демонстрирует базовый подход к управлению в декартовом пространстве, но имеет ограничения: 1. Мы управляем шарниром, только подавая ему команду перехода в точку и не проверяя обратную связь по положению шарнира, а в конце просто останавливаем робот, не убедившись, что шарниры достигли своего целевого положения. По сути, мы доверились контроллерам, считая, что они смогут точно следовать нашим командам. Однако мы знаем, что контроллеры у нас настроены плохо и очевидно могут отставать или промахиваться. 2. Траектория вычисляется как линейная интерполяция между стартовым и целевым положением шарниров. Здесь нет фаз разгона и торможения, а TCP перемещается не по прямой в декартовом пространстве. 3. Этого не видно из кода, но экспериментальным путём можно установить, что сам решатель не всегда может корректно решить обратную задачу.

Тем не менее, у нас есть очень простая программа, которая позволяет нам контролировать позицию TCP робота. Хотя, для случая, когда мы пишем код конкретного робота, я бы заменил в ней обращение к универсальному решателю на обращение к собственной функции решения обратной задачи кинематики, что в принципе несложно сделать для кода выше, заменив вызов ik_solver.CartToJnt на вызов собственного решателя.

8.3 Заключение

Сегодня вы сделали важный шаг в освоении управления роботами с помощью ROS. Вы познакомились с двумя ключевыми аспектами — управлением отдельными шарнирами манипулятора и перемещением конечной точки (TCP) в трёхмерном пространстве. Это похоже на то, как музыкант сначала учится играть отдельные ноты, а затем переходит к исполнению целой мелодии: вы научились “общаться” с каждым двигателем по отдельности и одновременно понимать, как двигать всю руку целиком.

Вы освоили библиотеку ros_control — мощный инструмент для создания контроллеров, который работает одинаково как в симуляции, так и на реальных роботах. С её помощью вы добавили в модель робота ПИД-регуляторы, которые позволяют точно управлять положением шарниров. Настройка этих регуляторов стала вашим первым опытом в подстройке систем управления под конкретную динамику механизма — будто бы вы подкручивали “педаль газа” и “амортизаторы” автомобиля, чтобы добиться плавного хода.

Кроме того, вы познакомились с библиотекой KDL (Kinematics and Dynamics Library), которая помогает решать задачи обратной кинематики. Теперь вы можете задавать роботу точку в пространстве, и он сам рассчитывает, как повернуть свои суставы, чтобы достичь этой цели. Это похоже на то, как человек тянется за чашкой с кофе: не задумываясь о каждом мышечном сокращении, вы просто фокусируетесь на цели.

Все эти технологии лежат в основе реальных промышленных роботов, используемых в производстве, автоматизации, медицине и даже в космических аппаратах. Например, когда вам нужно запрограммировать робота, чтобы он аккуратно собирал детали или сваривал конструкции, вы будете использовать те же подходы — точное управление шарнирами и контроль конечной точки.

Теперь вы можете: - Настроить ПИД-регуляторы для стабильного движения робота, - Визуализировать переходные процессы с помощью rqt_plot, - Менять параметры управления в реальном времени через rqt_reconfigure, - Программировать движение TCP в декартовых координатах, используя KDL.

Это открывает возможности для дальнейших экспериментов: планирования траекторий, управления захватом объектов, работы с датчиками и даже взаимодействия с окружающей средой. И хотя сегодняшние контроллеры ещё не идеальны, теперь вы знаете, где и как их улучшать.

Сегодня вы не просто изучили новый материал — вы стали ближе к настоящей робототехнике. То, что вы делали, — это основа для создания автономных систем, способных выполнять сложные задачи. Двигайте дальше — новые горизонты уже ждут вас!