Управление реальным роботом c обратной связью в ROS (часть 5)

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

1. Создание замкнутой системы управления с использованием ПИД-регулятора

Хотя управление с разомкнутым контуром простое в реализации, оно не всегда работает надежно при работе на реальном роботе, потому что комбинация факторов окружающей среды (например есть трение от пола) и внутренних факторов (то есть различий между левым и правым двигателями) играет роль.

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

2. Одометрия с точным расчетом

Одометрия ценна тем, что она:

1) является необходимой информацией для использования стека навигации ROS в будущем

2) позволяет нам видеть траекторию робота

Мы объединим показания энкодеров с обоих колес (точный расчет), чтобы оценить положение робота во времени (одометрия).

1. Обзор: Управление роботом c обратной связью

Цели: 1. Использование данных колесного энкодера для управления с обратной связью и ПИД-регулятором, и для оценки одометрии робота

2. Обучение использованию обратной связи с данными датчиков для усовершенствования управления роботом и для оценки состояния

План:

1. Прочитать данные с колесных энкодеров
2. Ввести ПИД-регулирование в управление на обоих колесах
3. Организовать точный расчет для оценки одометрии робота

Код репозитория (хранилища):

git clone -b closedloop https://github.com/richardw05/gopigo_ws.git

Видео-мануал

2. Архитектура

И опять, у нас есть хост-машина и робот, подключенный через WiFi. Как показано ниже, мы добавляем два узла ROS. gopigo_state_updater запускается на роботе и преобразует данные энкодера колеса в оценки угловой скорости колеса. Мы расширяем gopigo_controller, чтобы использовать эти показания с энкодера для управления с помощью PID. diffdrive_odom запускается на хост-машине и использует оценки энкодера для проведения точного расчета одометрии.

Показания энкодера колеса преобразуются в угловые скорости вращения колеса, которые используются как для ПИД-регулирования, так и для точных расчетов одометрии.

Ниже показаны сообщения ROS, передаваемые между нодами ROS.

Поток сообщений ROS от колесных энкодеров до вычисления одометрии.

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

Учитывая показания колесного энкодера, мы можем вычислить угловые скорости для управления с помощью ПИД-регулятора и точных расчетов одометрии.

3. Реализация

Мы превратим наш контроллер без обратной связьи в контроллер с обратной связью, создав новую ноду gopigo_state_updater и отредактировав ноду gopigo_controller. Затем мы выполним расчеты в новой ноде diffdrive_odom.

1. GoPiGo State Updater

Колесные энкодеры измеряют, на сколько реально повернулось каждое колесо, и оценивают реальные угловые скорости колес. В дальнейшем наш ПИД-регулятор исправит несоответствия между заданной и измеряемой (реальной) скоростью вращения колеса. Но прежде всего, нода gopigo_state_updater должна преобразовать показания колесного энкодера в угловую скорость.

Интересно, что колесные энкодеры GoPiGo возвращают количество пройденных сантиметров; в то время как большинство кодировщиков возвращает количество пройденных тактов (Следует пояснить, что при повороте на фиксированный малый угол энкодер генерирует некий сигнал . Подсчитывая количество этих сигналов, т.е. количество тактов, мы можем получить угол поворота и скорость поворота колеса). Тем не менее, измеряемые сигналы любого типа можгут быть преобразованы в угловые скорости. Для данных с нашего колесного энкодера мы можем вычислить угол поворота колеса в радианах, как величину, пропорциональную пройденному расстоянию, получаемому с энкодера. Затем мы вычисляем угловую скорость путем деления на время, за которое проводилось измерение (т.е. время, за которое робот преодолел расстояние, полученное с энкодеров).

Используются следующие сокращения:

• d: изменения в расстоянии, полученные от энкодеров
• R: радиус колеса робота
• Δt: время перемещения на расстояние, полученное от энкодера
• w: угловая скорость колеса

Для вычисления угловой скорости, мы сначала используем расстояние с энкодера, чтобы вычислить, какую часть от длины окружности колеса представляет это расстояние. А значит можно и вычислить угол поворота колеса, умножая это (ту самую часть, долю) на количество радиан за один оборот (очевидно, 2π). Наконец, мы делим на время, необходимое для перемещения на текущее расстояние, полученное с энкодера.

Уравнение может быть дополнительно упрощено для получения угловой скорости колеса. Основная цель узла gopigo_state_updater состоит в публикации этих угловых скоростей вращения колес.

2. Контроллер GoPiGo с ПИД-регулятором

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

ПИД-регуляторы хорошо изучены, поэтому можно понять принцип их работы интуитивно. Довольно просто можно это понять на примере круиз-контроля автомобиля. Состояние системы - текущая скорость робота. Круиз-контроль устанавливает целевую скорость. Управляющая команда регулирует педаль газа. Если вы двигаетесь слишком медленно, то вы нажмете на педаль газа сильнее (и наоборот). Аналогично за вас контролирует скорость движения и круиз-контроль. Измерение - это одометр, который сообщает нам скорость автомобиля. Ошибка - это разница между измерением и целью. Если ошибка равна 0, то это означает, что мы достигли нашей целевой скорости. В большинстве физических систем мы не хотим мгновенно переходить от 0 до 100 км/ч, поэтому нам нужно вычислить управляющие команды, которые постепенно регулируют скорость автомобиля. ПИД-регулятор в основном представляет собой совокупность трех отдельных методов (составляющих) настройки управления, которые охватывают различные ситуации:

1. (П)ропорциональная - команда управления пропорциональна текущей ошибке (например, достаточно нажать на педаль газа, чтобы медленно увеличить скорость до целевой)
2. (И)нтегральная - команда управления пропорциональна не только текущей ошибке, но и предыдущим (например, добавляется больше "газа", если мы имели большую ошибку на протяжении некоторого времени)
3. (Д)ифференциальная - команда управления пропорциональна будущим изменениям ошибки (благодаря ей мы быстро реагируем на изменения ошибки)

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

• https://www.coursera.org/learn/mobile-robot
• https://en.wikipedia.org/wiki/PID_controller

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

Надеемся, что функция pid_control показывает, насколько просто реализовать контроллер PID. Конечно, необходимо будет экспериментировать, чтобы определить правильные значения коэффициентов усиления (Kp, Ki, Kd) для использования.

3. Одометрия дифференциального привода

Посредством точного расчета мы можем объединить данные энкодеров с обоих колес, чтобы не только восстановить линейную и угловую скорости робота (в идеальной системе мы бы получили то же самое, что и на входе /cmd_vel), но и оценить его положение во времени. Прямая кинематика сводится к последовательности уравнений (на основе моделей того, как физическая система реагирует на заданные входные данные) для преобразования данных датчика в одометрию робота. Наши кинематические уравнения предполагают отсутствие проскальзывания колеса, сопротивления и других факторов, которые по своей природе трудно точно предсказать, поэтому оценки одометрии не будут полностью соответствовать реальности.

Определим следующие входные величины:

• L: расстояние между колесами робота
• w_l: угловая скорость левого колеса
• w_r: угловая скорость правого колеса
• Δt: изменения во времени
• x: предыдущая координата робота по x
• y: предыдущая координата робота по y
• θ: предыдущая ориентация (угол поворота) робота

А также выходные величины:

• R: радиус поворота (расстояние до мгновенного центра скоростей)
• v_l: касательная скорость левого колеса
• v_r: касательная скорость правого колеса
• v_c: линейная скорость робота
• w_c: угловая скорость робота
• x’: текущая координата робота по x
• y’: текущая координата робота по y
• θ’: текущая ориентация (угол поворота) робота

Мы используем простые уравнения прямой кинематики, которые предполагают один из двух вариантов-сценариев движения нашего робота с дифференциальным приводом: либо робот движется прямо вперед, либо движется вокруг «мгновенного центра скоростей» (МЦС или ICC - instantaneous center of curvature, буквально с английского - мгновенный центр кривизны).

Наша модель прямой кинематики учитывает два варианта мгновенных скоростей: при движении прямо или при повороте по некоторому кругу.

• Прямолинейное движение

Динамика прямолинейного движения проста. Угловая скорость равна 0, так как оба колеса движутся с одной скоростью:

Естественно, мы можем представить текущую позицию робота (глобальные позицию и ориентацию) как:

• Криволинейное движение (поворот по кругу)

Сначала мы вычисляем радиус круга, по которому движется робот (в данный момент). Это позволяет нам вычислить угловую скорость робота.

Затем мы можем вычислить координаты центра этого круга.

А затем мы можем представить текущую позицию робота как:

Мы публикуем вычисленную глобальную позицию робота в пространстве как в сообщение tf transform так и в сообщение odometry (одометрии) робота. Сообщение одометрии также содержит информацию о скорости, а значит сообщение одометрии содержит больше информации о состоянии робота (положение, ориентация, линейная скорость, угловая скорость).

Ссылки:

• Реализация одометрии в ROS
• Одометрия ROS в Python

4. Запуск

Мы собираемся использовать данные c колесного энокодера для:

1) управления с обратной связью с использованием ПИД-регулятора

2) оценки одометрии робота

Мы используем одни и те же launch-файлы nav_behaviors.launch и gopigo_interface.launch. Мы отредактировали их для запуска дополнительных нод ROS (diffdrive_odom для nav_behaviors.launch и gopigo_state_updater для gopigo_interface.launch). Попробуйте запустить следующие сценарии.

1. Одометрия в моделировании

В качестве базовой проверки работоспособности мы хотим убедиться, что наши прямые кинематические преобразования верны. Это можно сделать, показав, что мы можем восстановить исходное сообщение cmd_vel.

В gopigo_interface.launch установите следующее для всех:

<param name="gopigo_on" value="False"/>

Теперь запустите launch-файлы. На хост-машине:

roslaunch nav_behaviors nav_behaviors.launch

На Raspberry Pi:

roslaunch gopigo_description gopigo_interface.launch

Введите следующие сообщения, чтобы убедиться в правильности наших вычислений:

rostopic echo /cmd_vel
rostopic echo /lwheel_tangent_vel_target
rostopic echo /rwheel_tangent_vel_target
rostopic echo /lwheel_angular_vel_target
rostopic echo /rwheel_angular_vel_target
rostopic echo /lwheel_angular_vel_control
rostopic echo /rwheel_angular_vel_control
rostopic echo /lwheel_angular_vel_motor
rostopic echo /rwheel_angular_vel_motor


rostopic echo /lwheel_angular_vel_enc
rostopic echo /rwheel_angular_vel_enc
rostopic echo /lwheel_tangent_vel_enc
rostopic echo /rwheel_tangent_vel_enc
rostopic echo /cmd_vel_enc

Отправьте целевые сообщения /cmd_vel и посмотрите, как эти сообщения меняются.

./nav_forward.sh
./nav_rotate.sh

Когда мы устанавливаем для gopigo_on значение False, нода gopigo_state_updater будет копировать целевую угловую скорость (по сути, имитируя показания кодировщика, полностью совпадающие нашей данной моделью). Убедитесь, что /cmd_vel и /cmd_vel_enc совпадают.

2. Одометрия на GoPiGo

Теперь вычислим одометрию, используя фактические данные с колесного энкодера.

В gopigo_interface.launch установите следующее для всех:

<param name="gopigo_on" value="True"/>

Запустите те же lainch-файлы, что и ранее. Также введите те же сообщения ROS.

Убедитесь, что /cmd_vel и /cmd_vel_enc похожи. Это должно показать вам, насколько реальность соответствует нашей ожидаемой модели.

3. Управление с ПИД-регулятором на GoPiGo

ПИД-регулятор должен обеспечивать вращение колес с ожидаемой скоростью.

В gopigo_interface.launch установите:

<param name="pid_on" value="True"/>

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