Назад в библиотеку

---

Управление движением мобильного робота

Автор:А. А. Большаков, Д. Л. Лисицкий
Источник:SyberLeninka

Пусть необходимо, чтобы робот переместился из точки А в точку В горизонтальной плоскости (рис. 1) по траектории, минимально отклоняющейся от прямой, соединяющей эти точки (перемещение по прямой линии невозможно из – за препятствий произвольной формы). Для ясности дальнейших рассуждений предположим, что препятствие сплошное и несимметричное относительно прямой АВ (рис. 1). Робот может огибать препятствие, обходя его с разных стороны, однако одно из направлений обхода будет требовать большего отклонения от прямой АВ и, соответственно, должно быть исключено. Кроме того, в точке траектории, наиболее удаленной от прямой АВ (точки C и C' на рис. 1), вектор скорости объекта должен быть ей параллелен и между роботом и препятствием должно оставаться некоторое заданное расстояние з Y₃.

Для обнаружения препятствий местности на корпусе робота устанавливается специальный датчик. Им может являться лазерный дальномер с лучом, сканирующим в горизонтальной плоскости (ЛДС), угол сканирования которого равен 2φ_max, причем φ_max не меньше максимального угла, который может составлять траектория движения робота с прямой АВ. Предположим, что ось симметрии сектора сканирования луча ЛДС параллельна прямой АВ, а угол отклонения луча ЛДС от оси симметрии сектора – φ изменяется дискретно с интервалом Δφ (рис. 1). Дальномер при каждом i – м измерении определяет угол наклона луча φ_i и расстояние до препятствия l_i, что позволяет вычислить координаты видимых дальномером точек препятствия в связанной с объектом системе координат. Всего за полупериод сканирования будет получена информация о расстоянии до k = (2φ_max / Δφ) +1 точек препятствия, На корпусе робота установлены также два лазерных дальномера с неподвижными лучами, измеряющие расстояние от робота до препятствий в направлении перпендикулярном прямой АВ. 

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

Для реализации системы воспользуемся структурой (рис. 2), описанной в [1], которая позволяет разделить ее на две последовательно включенные подсистемы – «модель–вычислитель программного управления (ВПУ)» и «объект – регулятор» (рис. 3). Под моделью в этом случае понимается упрощенная математическая модель движения центра масс робота. Подсистема «модель – ВПУ» формирует программную траекторию, а подсистема «объект – регулятор» ее отслеживает.

Рисунок 2 – A) Исходная структура САУ;
Б) Эквивалентная структура САУ

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

где Y_M, X_M – координаты упрощенной модели объекта в системе координат АХУ; V_xM, V_yM – соответственно продольная и поперечная составляющие скорости упрощенной модели; n_y – поперечная перегрузка модели; g – ускорение силы тяжести. В (1) X_M, Y_M, V_xM, V_yM – координаты; n_y – управление, которое далее будем называть программным управлением.

На координаты и управления упрощенной модели робота наложены ограничения:

Первое из неравенств (2) ограничивает угол наклона программной траектории, второе – радиус кривизны траектории. Величины ограничений следует выбирать так, чтобы динамические свойства робота позволяли ему отслеживать «движение» модели;

Необходимо синтезировать алгоритм выбора программного управления n_y, который определяет направление обхода препятствия и переводит объект (1),  (2) из произвольного начального состояния в точку, расположенную на расстоянии Y₃ от точки препятствия, наиболее удаленной от оси АХ (рис. 1), с поперечной составляющей вектора скорости, равной нулю (V_yM = 0), таким образом, чтобы минимизировался функционал.

Можно показать ,что минимизация функционала (3) в рамках модели (1), (2) требует создания системы с максимальным быстродействием. Тогда, согласно принципу максимума Понтрягина и теореме об n – интервалах [2], оптимальное управление будет релейным и принимает только значения (n_{y max}, 0, – n_{y max}).

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

Введем новые переменные:

где X_Пi, Y_Пi – координаты i – й точки препятствия местности в неподвижной системе координат АХУ.

Тогда система уравнений (1) примет вид:

После введения новых переменных задача формально сводится к синтезу оптимального в смысле функционала (3) алгоритма выбора управления n_y =  (n_{y max}, 0, – n_{y max}), перемещения изображающей точки из произвольного положения в фазовом пространстве Охуv_y в начало координат – точку О.

Фазовый портрет завершающего этапа такого переходного процесса приведен на рис. 4 (последние три интервала постоянных значений управления). Несложно видеть, что в начало координат можно попасть по двум траекториям – АО с управлением n_y = – n_{y max} и ВО – с управлением n_y = n_{y max}. На координату v_y, согласно обозначениям (4), также наложены ограничения:

где |v_{y max}| = |V_{y max}|, поэтому в точке А с координатами (x_a, y_a, v_y = – v_{y max}) траектория АО переходит в траекторию АС, у которой n_y = 0, а v_y = – v_{y max} = const. Аналогично траектория ВО в точке В с координатами (x_b, y_b, v_y = v_{y max}) переходит в траекторию ВD, у которой n_y = 0, а v_y = v_{y max} = const.

Приблизиться к траектории АО изображающая точка может только по поверхности П1 с управлением n_y = n_{y max}, а к ее продолжению – траектории АC – по поверхности П2 тоже с управлением n_y = n_{y max}. Границей между поверхностями П1 и П2 является траектория АМ.

Приблизиться к траектории ВО изображающая точка может только по поверхности П3 с управлением n_y = – n_{y max}, а к ее продолжению – траектории BD – по поверхности П4 также с управлением n_y = – n_{y max}. Границей между поверхностями П3 и П4 является траектория BN.

Для определения уравнений вышеперечисленных траекторий и поверхностей исключим из уравнений (5) время и проинтегрируем их в обратном направлении – от конца фазовых траекторий к началу при соответствующих постоянных значениях управления (n_{y max}, 0, – n_{y max}).

Получим уравнения:

• траектории ОА:
  
• поверхности П1:
  
• граничной траектории АМ:
  
• поверхности П2:
  
• траектории ОВ:
  
• поверхности П3:
  
• граничной траектории BN:
  
• поверхности П4:
  

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

Любая i – я точка препятствия при Y_Пi > 0 является потенциально опасной для объекта, если изображающая точка попала на поверхности П1, П2, траекторию ОА или пересекла их. Соответственно, при Y_Пi < 0 признаком потенциальной опасности i – й точки препятствия является попадание изображающей точки на поверхности П3, П4, траекторию ОВ или их пересечение.

При попадании изображающей точки на одну из поверхностей или линий переключения, приводящих ее в начало координат (в соответствии со знаком Y_Пi) программное управление должно выбираться равным управлению, для которого получены эта поверхность или линия. Если изображающая точка не попала ни на одну из линий или поверхностей переключения, программное управление должно выбираться таким, чтобы оно переводило модель к траектории АВ за минимальное время.

В соответствии с изложенным подходом сформирован алгоритм выбора управления на каждом шаге интегрирования уравнений (1), однако для стабилизации объекта на прямой АВ использован регулятор с зоной линейности при малых отклонениях.

1. Выбор программного управления, стабилизирующего объект на траектории АВ, осуществляется регулятором с законом управления, учитывающим ограничения (2):
   

   где с₁, с₁ – постоянные коэффициенты; Н1, Н2 – промежуточные переменные; n_yp – выходной сигнал регулятора;Y_П – координата препятствия в системе координат АХУ в месте нахождения объекта.

2. Для каждого положения луча дальномера выбирается программное управление:
   

   где, H_3i, H_4i – промежуточные переменные.

   Каждому значению i соответствует упорядоченная тройка {n_y1i, Y_Пi, h_1i}, где h_1i = (y_i – Н_3i), если Y_Пi > 0, и {n_y2i, Y_Пi, h_2i}, где h_2i = (y_i − Н_4i), если Y_Пi < 0.

3. Если отсутствуют тройки, в которых Y_Пi > 0, то формируется тройка {n_y1 = n_{y max}, Y_Пi = 100, h₁ = 1} и пропускаются пп. 5, 7 алгоритма.
4. Если отсутствуют тройки, в которых Y_П1 < 0, то формируется тройка {n_y2 = – n_{y max}, Y_П1 = 100, h₁ = –1} и пропускаются пп. 6, 8 алгоритма.
5. Из всех троек, соответствующих Y_Пi > 0, отбираем те, у которых n_y1i > – n_{y max}. Если таких пар нет, то формируем тройку {n_y1 = – n_{y max}, Y_П1 = 0, h₁ = – 1} и пропускаем п. 7 алгоритма.
6. Из всех троек, соответствующих Y_П1 < 0, отбираем те, у которых n_y2i < n_{y max}. Если таких пар нет, то формируем тройку {n_y2 = n_{y max}, Y_П2 = 0, h₁ = 1} и пропускаем п. 8 алгоритма.
7. Из оставшихся троек, соответствующих Y_Пi > 0, отбираем ту, у которой Y_Пi максимально. Обозначим параметры этой группы {n_y1, Y_П1, h₁}.
8. Из оставшихся троек, соответствующих Y_Пi < 0, отбираем ту, у которой Y_Пi минимально. Обозначим параметры этой группы {n_y2, Y_П2, h₂}.
9. Выбираем программное управление:

В результате интегрирования уравнений (1) с выбранным значением управления получаем численное значение координат модели Y_M, V_yM, которые и определяют значение программной траектории в каждый момент времени. Координату X_M модели принимаем равной координате X_P центра масс робота.

Работоспособность и эффективность предложенного алгоритма проверялись методом математического моделирования. Для этого разработана программа на языке СИ++. При моделировании использовалась линеаризованная математическая модель плоского движения гипотетического четырехколесного робота шестого порядка [3] с регулятором, обеспечивающим отслеживание программной траектории синтезированным методом АКОР.

Один из результатов моделирования представлен на рис. 5, где показана только та часть препятствия, которую объезжал робот. Программная траектория на этом графике показана сплошной линией, траектория движения центра масс робота – пунктирной.

Из результатов моделирования следует, что программная траектория соответствует предъявляемым требованиям. Робот правильно выбирал направление объезда препятствия, отклонение траектории движения центра масс робота от программной траектории несущественно и не превышало 0,2 м.