Применение модифицированного алгоритма "чистого преследования" для управления беспилотным вилочным погрузчиком

Одной из ключевых проблем при разработке системы управления движением беспилотной техники является интеграция алгоритма следования заданной траектории. Сегодня существуют три наиболее распространенных подхода в решении данной проблемы: метод динамического окна, управление на основе предсказаний модели и метод "чистого преследования" [1].

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

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

Основное преимущество метода "чистого преследования" заключается в относительной простоте геометрических вычислений, что упрощает его интеграцию и отладку. Как следствие, повышает его безопасность, надежность и отказоустойчивость по сравнению с другими методами [3]. Основная идея метода заключается в поиске точки на маршруте, которая расположена на выбранном расстоянии (радиусе прогнозирования) от начала отсчета робота (как правило, его центра вращения) и геометрическом расчете мгновенной траектории, проходящей через нее [4]. В случае с погрузчиком алгоритм вычисляет угол поворота ведущего колеса, определяющий мгновенный радиус кривизны траектории относительно середины колесной опоры вил. Данный метод успешно применяется для различных видов колесных интеллектуальных транспортных систем и демонстрирует соизмеримую точность с другими подходами, что делает его приемлемым выбором для вилочных погрузчиков в условиях эксплуатации на промышленных складах. Современные методы "чистого преследования", как правило, изменяют расстояние прогнозирования в зависимости от скорости. Достоинством такого алгоритма является снижение риска потери управления. С другой стороны, при длинном радиусе прогнозирования сближение с истинной траекторией происходит на большей дистанции, что особенно критично в случае двустороннего движения в узком проезде между стеллажами. Поэтому в представленном нами методе сохранение управляемости задается ограничением скорости. При альтернативных подходах также регулируется линейная скорость движения в зависимости от радиуса кривизны траектории, что используется для замедления перед поворотом во избежание заноса и переворачивания робота, а не как средство контроля управляемости. В ряде исследований метод "чистого преследования" применялся на скоростях до 1.5 м/c [5], тогда как предложенный метод позволил осуществлять беспилотное управление погрузчиком на его максимальной скорости: до 2.2 м/c. В исследованиях [5] рассмотрение методов следования траектории ограничивается моделированием или применением в качестве системы позиционирования высокоточных лидаров. Проведенное исследование показало применимость метода "чистого преследования" для управления промышленными вилочными погрузчиками с использованием переносной платформы оптической навигации по потолочным меткам. При этом точность следования траектории оказалась соизмеримой с точностью других современных методов [2, 5, 6]. В работе представлены модификации алгоритма "чистого преследования", позволившие осуществлять подбор и размещение паллеты с произвольной позиции. Такой подход имеет преимущества по сравнению с вычислением траектории до паллеты методом оптимизации [7] или с использованием сплайнов [8], благодаря меньшему числу параметров, вычислительной эффективности, детерминированности, что важно для промышленной техники. Предложены адаптивные компоненты управления, повышающие точность следования траектории при наличии системной ошибки по углу поворота колеса, и они уже внедрены в практическое применение. В статье представлены результаты на основе симуляции движения погрузчика, а также результаты натурных экспериментов.

ОПИСАНИЕ АЛГОРИТМА

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

Зададим состояние погрузчика в глобальной системе координат с центром в (·) О , определив его ключевые геометрические точки в плоскости пола. Пусть точка R — центр вращения погрузчика (середина задней колесной оси), W — серед ин а ведущего колеса. Тогда длина вектора w = RW есть длина колесной базы. Вектор r отражает

Рис. 1. Векторная схема погрузчика (а) и паллеты (б)

направление переднего ведущего колеса. Нормаль n образует с w базис. Теперь определим параметры маршрута. Складские помещения преимущественно имеют клеточную структуру, поэтому маршруты удобно описывать через прямые отрезки путем задания координат их начала и конца. Введем обозначение S для геометрической точки начала отрезка, а T — для конца отрезка. Такие же обозначения будут использованы для задания линии. Без ограничения общности можно сказать, что маршрут произвольной формы задается множеством отрезков { S i T i : S i = T i– 1 }. Введем геометрические величины, задающие отношение положения погрузчика к целевому маршруту. Вектор n₁ отражает поперечное отклонение от точки траектории. Его длина эквивалентна расстоянию до траектории, сам вектор направлен в сторону точки траектории P . Тогда величина поперечного отклонения есть проекция вектора n 1 на ось n . Аналогично | n₂ | есть знаковое расстояние от точки M (| RM^ = 1 ) до касательной к траектории в точке Р . Через данные расстояния можно выразить угловое отклонение от траектории, они взяты в качестве первичных величин из-за простоты интерпретации. Продольное отклонение s от целевой точки T есть проекция TW на направление WR (при операции с паллетой) или проекция TR на направление WR (при движении по целевому маршруту). Отметим, что в случае, если целевая траектория есть прямая линия, то касательная в точке Р совпадет с траекторией. В случае подбора или размещения паллеты целевая траектория задае тс я в виде линии, определяемой вектором t = ST , где S есть предполагаемое место середины переднего торца паллеты, а T — заднего. Далее мы будем придерживаться именно таких обозначений. Формально качество достижения погрузчиком цели можно представить в виде формулы:

^ (InJ ⁺ In 2l Н ⁰ ,              ⁽¹⁾

где s — расстояние от R до T .

Критерием качества следования траектории может выступать функционал в формуле (2):

Рис. 2. Геометрическая модель движения по прямой

Рассмотрим движение погрузчика по маршруту в контек сте выхода его на целевой маршрут в виде вектора ST . Геометрическая модель "чистого преследования" в данном случае представлена на рис. 2.

Подробный анализ данной задачи позволяет затем обобщить движение погрузчика по всему маршруту путем планирования движения между его отрезками. Пусть известны точки S , T , R , W на плоскости и задано расстояние прогнозирования L . Тогда угол направления колеса вычисляется по формулам:

8 = f_t ( S , T , R , W , L ) = Z ( w , r ) = z ( w , WP ) =

= sign ( ^ w -WP J ) - Arccos Wi,WP^ ,           (3)

где { s_k } — дискретное множество длин отрезков пройденного пути, на которых проводилось измерение поперечных отклонений погрузчика во время движения.

Зададим управляющее состояние погрузчика. В нашем случае оно задается углом ведущего рулевого колеса δ и линейной скоростью этого колеса v .

WP = «

- AC - B^L² - C²

Й

- bc - a4l - c ²

I STI2

,

где A , B , C — коэффициенты уравнения прямой, вычисляемые из координат точек направляющего вектора ST .

а

б

Рис. 3. Траектории робота при сближении с целевой прямой (ось абсцисс совпадает с прямой). Симуляции: без ограничений скорости (а), с ограничением скорости (б); L — расстояние прогнозирования

На рис. 3 представлены результаты симуляции метода "чистого преследования".

Начальное продольное отклонение робота было выбрано 20 м, поперечное — 2 м. Начальное угловое отклонение от прямой равно 0.25π рад. В первом случае была задана фиксированная скорость 2.2 м/с как максимально достижимая, и исследовался характер отклонений при различном расстоянии прогнозирования L . Видно, что при L < 2 м наблюдается потеря контроля над управлением — погрузчик начинает "вилять". Потеря контроля над управлением вызвана ограничением угловой скорости вращения рулевого колеса и недопустимо большой скоростью сближения с прямой. С другой стороны, при увеличении L растет средняя величина поперечных отклонений n 1 , n 2 на всем маршруте. Поэтому предложено ограничивать линейную скорость вращения ведущего колеса при больших отклонениях от целевой траектории:

Рис. 4. Устойчивое смещенное положение погрузчика относительно истинной траектории при наличии системной ошибки δ по углу поворота рулевого колеса

v = 5

ν 1 ,

ν ₂,

(I « J <  Q ^ (|| « J -1 « 2II <  C 2 ), otherwise.

При получении данных на рис. 3, б, было выбрано C 1 = 0.05 м, С 2 = 0.13 м, v 1 = 2.2 м/с, v 2 = 1 м/с, что позволило избежать потери управления даже при L < 2 м.

В ходе исследования возникла проблема, связанная с тем, что метод "чистого преследования" не позволяет выполнить условие сходимости (1) при наличии системного смещения между ожидаемым углом поворота рулевого колеса и реальным. Проблема проиллюстрирована на рис. 4.

При наличии системной ошибки δ существует такое положение погрузчика относительно истинной прямой, при котором рассчитанное методом "чистого преследования" значение угла δ в сумме с системной ошибкой не приводит к сближению погрузчика с истинной траекторией. Методом от противного легко показать, что при этом вектор w будет параллелен прямой t (целевой траектории). В таком случае была предложена адаптивная поправка Θ к углу поворота рулевого колеса на основе координат, полученных по оптической навигации:

8 = f 1 +© ,

0 = 0 0 + £ kf \ £ i , i =A s

1, (I n 1 l i <  C 3 K'( n 1 - n 2 | i <  C 4 ) ,

0, otherwise,

где Θ 0 — предобученная поправка, i — итерация обучения (например, обновление поправки может происходить с шагом Δ s = 0.2 м пройденного пути), ℓ i — индикатор близости и параллельности корпуса погрузчика целевой прямой. С 3 , С 4 определяют окрестность положения погрузчика, в пределах которой поправка может обновляться. Коэффициент k определяет вес поправки на каждом шаге обновления.

На рис. 5 представлен результат моделирования сближения с прямой при тех же начальных условиях, с ограничением скорости, но с введением навигационной поправки. Кроме того, искусственно введена системная ошибка δ = 0.17 рад (с учетом знака). В эксперименте С 3 = 1 м, С 4 = 0.05 м.

Из рис. 5 видно, что при k = 0 поперечное отклонение с уменьшением продольной дистанции до 7.5 м перестает уменьшаться. При 0 <  k ≤ 1 наблюдается сближение с истинной траекторией по значению поперечного отклонения. При этом, чем меньше k , тем поправка более инертна, что актуально в случае шума или выбросов в координатах оптической навигации.

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

При подборе или размещении паллеты алгоритм "чистого преследования" был модифицирован таким образом, чтобы обеспечить возможность заезда под паллету вилами вперед из произвольной позиции перед ней. С геометрической точки зрения данная задача рассматривается как плавный выход на прямую, проходящую через середины переднего и заднего торцов паллеты (точки S , T на рис. 1, б). При этом погрузчик своим центром вращения R должен достигнуть заднего торца паллеты T , при этом должно выполниться условие (1). На рис. 6 представлены три разновидности алгоритма "чистого преследования" в зависимости от положения оси робота относительно целевой прямой.

Изначально реализован базовый алгоритм "чистого преследования" с модификацией для решения задачи движения к цели вилами вперед. Алгоритм осуществляет сближение с прямой ST (фаза сближения). Для этого векторы w , r по принципу симметрии были отражены в векторы w ′ , r ′ . Тогда по принципу велосипедной модели движения, исходные векторы и отраженные формируют идентичные мгновенные радиусы кривизны и, следовательно, мгновенные траектории в виде окружности. Поэтому движение вектора w можно рассматривать как классическую задачу "чистого преследования" для его симметричного отражения. Угол рулевого колеса при этом рассчитывается по (9):

8 = f 2 ( S , T , R , W , L ) = Z ( w , r ) = -Z ( w ‘ , r ‘ ) =

= - Arctan

2 |w| sin ( z (w ' , RP

L

Рис. 5. Влияние веса k поправки Θ на поперечное отклонение погрузчика

\                     7

При этом L = p 0 5 _;1 ] ( d ), где d — продольная дистанция от переднего торца паллеты до центра вращения погрузчика. Выбор такой зависимости длины прогнозирования позволяет совершать сближение вдали от паллеты более плавно, т.к. есть запас дистанции для плавного маневра. Вблизи паллеты поперечное отклонение критично, т.к. робот-погрузчик может промахнуться, завести вилы не точно в пазы паллеты и повредить ее. Радиус кривизны поворота в данном случае должен быть меньше. У базового алгоритма существует недостаток: при сближении с целевой прямой под ненулевым углом траектория робота один или несколько раз пересекает ее. Такие пересечения приводят к излишним зигзагообразным отклонениям от прямой, что, как следствие, влечет за собой неоправданное увеличение значения

а

б

Рис. 6. Геометрические схемы движения погрузчика при подборе паллеты по линии ST . а — фаза сближения, б — фаза выхода на касательную, в — фаза выравнивания

в

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

| qp\ = QR. .                  (io)

Тогда угол поворота рулевого колеса выражается формулой:

5 = Z( w, r) =

= f 3 ( S , T , R , W ) = f 2 ( S , T , R , W ,| RP |) .       (11)

В случае если корпус погрузчика w пересекает линию ST , касательную построить невозможно, однако можно осуществить сближение точки W c прямой (фаза выравнивания на рис. 6, в). Аналогично фазе сближения движение вектора w можно выразить через движение его симметричного отражения. При этом чистое преследование будет вычисляться так же, как и при обычном движении по маршруту. Угол поворота рулевого колеса будет вычисляться по формуле:

5 = Z( w, r) =

= f 4 ( S , T , R , W , L ) = - f 2 ( S , T , R , W ‘ , L ) ,     (12)

где L = 0.5.

Объединяя все представленные методы, общая формула приобретает следующий вид:

5 ( S , T , R , W , L ) =

| n 2| < | n 1 |)) ^П | n 1| <  ^C 5 ,

f 3 , ( ( n 1 n ₂ < 0 ) u ( n 1 n ₂ > 0 n

= 4 f4, n1 n2 < 0)u(n1 n2 > 0n

f ₂ , otherwise.

В формуле (13) константа С 5 определяет порог величины поперечного отклонения, начиная с которого фаза сближения сменяется фазой движения по касательной. В экспериментах С ₅ выбрана равной 0.02 м — допустимая погрешность параллельного смещения относительно паллеты. Введение фаз движения по касательной и выравнивания позволили улучшить сходимость поперечных отклонений к нулю по сравнению с классическим методом "чистого преследования" (фазой сближения) и, как следствие, уменьшить величину функционала при одинаковых начальных условиях.

Рис. 7. Траектории робота при подборе или размещении паллеты (ось абсцисс совпадает с продольной осью симметрии паллетного места).

Классическое "чистое преследование" (а) , "чистое преследование" с фазой касания и выравнивания (б)

Чистое преследование (фаза сближения)

На рис. 7 приведены графики симуляции траекторий для устранения поперечных отклонений.

Для операции подбора или размещения паллеты была выбрана постоянная скорость 0.5 м/с при подъезде к паллетному месту и 0.1 м/с в момент заезда на него. Для минимизации системной ошибки продольного смещения при финальном позиционировании паллеты было предложено использовать адаптивный метод сброса газа. Идея заключается в том, что при парковке на паллетное место с одинаковой скоростью точность остановки определяется продольной дистанцией до цели, на которой газ сбрасывается до нуля (формулы (14), (15)).

| v 0, Ax > Axi, v = (

[ 0, otherwise,

A x = A x, , - 0.5 A d ,

— 1

где Δ x — продольное отклонение на момент измерения, Δ x i — порог сброса газа, полученный на предыдущих итерациях, Δ d — продольная ошибка после полной остановки. Во избежание выбросов при обновлении Δ x i ее величину можно ограничить по модулю оценкой сверху.

ЭКСПЕРИМЕНТАЛЬНАЯ ЧАСТЬ

Кроме математического моделирования проведены натурные эксперименты с роботом-погруз- чиком. Алгоритмы сближения с траекторией, подбора и размещения паллеты были внедрены в систему автономного управления стандартным промышленным вилочным погрузчиком, оснащенным переносной системой оптической навигации [9, 10]. Схема компонентов системы управления роботом-погрузчиком представлена на рис. 8.

Микроконтроллер (STM32 Cortex-M4) обеспечивает обработку пакетов CAN-интерфейса бортового компьютера Curtis и одновременно принимает управляющее состояние и отправляет обратную связь через serial-порт микрокомпьютера Jetson Orin NX. В качестве обратной связи доступны угол направления ведущего колеса и его линейная скорость. Оба значения из безразмерных величин переводятся в систему СИ в соответствии с полученными экспериментально таблицами. Информация о задачах и маршруте поступает на каждый робот по сети через сервер принятия решений с использованием инфраструктуры ROS2 FastDDS Middleware [11].

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

а

б

Рис. 8. Блок схема управления роботом-погрузчиком (а) и его прототип (б)

Рассчитанный управляющий сигнал по скорости подается на вход программного модуля безопасности, который либо не меняет его, либо сбрасывает его до нуля в случае большого расхождения между входными и выходными параметрами. Таким образом снижаются риски столкновения на большой скорости, что актуально в случае кратковременных ошибок в системе распознавания меток и резких поворотов. Полученное состояние отправляется в микроконтроллер с частотой 10 Гц. Опытные образцы промышленных погрузчиков обладают длиной колесной базы 1.5 м, максимальной скоростью до 2.2 м/с, предельной скоростью поворота рулевого колеса 1.57 рад/с.

Проведено две серии экспериментов. В первой серии погрузчик стартовал из выбранной позиции ( x , y ) = (-20, -2) под углом 0.75π рад в глобальной системе координат и двигался направлении отрезка { S ; T }={(0,0); (0,20)}. В определенный момент робот выходил на стабильное движение вдоль прямой. Начиная с этого момента, проводились замеры его поперечного отклонения. Внедрение адаптивной ошибки совместно с ПИД-регулятором позволило сократить ошибку почти в 2 раза (табл. 1).

На рис. 9 представлена полученная зависимость поперечного отклонения на продольной дистанции в серии экспериментов в сравнении с результатом моделирования.

При проведении эксперимента использовались параметры: L = 2 C 1 = 0.2, С 2 = 0.13, v 1 = 2.2, v 2 =1, С 3 = 1, С 4 = 0.05 (5). Вес геометрической поправки k = 0.01. Пропорциональный, интегрирующий, дифференциальный коэффициенты ПИД-регулятора соответственно: K p = 0.5, K i = 0.2, K d = 0.

Табл. 1. Ошибка поперечного отклонения при следовании по целевой траектории

Описание режима	Значение ошибки, см
	медианное	максимальное
"Чистое преследование" без адаптивного управления	10.0	12.0
"Чистое преследование" с адаптивной ошибкой	4.5	7.0

Рис. 9. Траектория робота при сближении с целевой прямой (ось абсцисс совпадает с прямой)

Рис. 10. Траектории робота-погрузчика при подборе или размещении паллеты с одной и той же позиции (ось абсцисс совпадает с продольной осью симметрии паллетного места)

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

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

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

Представленные алгоритмы интегрированы в систему автономного управления транспортировкой паллет на одном из промышленных складов.

Рис. 11. Зависимости поперечного (а) и углового (б) отклонений робота-погрузчика от продольной дистанции к цели при подборе или размещении паллеты (разные исходные позиции)

а

Табл. 2. Величины ошибок при операции с паллетой

Вид ошибки	Средняя ошибка	Дисперсия	Максимальное значение
Продольная ошибка, м	0.002	0.06	0.012
Поперечная ошибка, м	0.016	0.006	0.03
Угловая ошибка, рад	0.007	0.005	0.021

ЗАКЛЮЧЕНИЕ

В статье предложен модифицированный метод "чистого преследования" в применении к промышленным роботам-погрузчикам. Рассмотрены задачи движения по заданному отрезку пути, подбор или размещение паллеты. В первом случае удалось достичь устойчивого движения при скоростях до 2.2 м/с со средним медианным значением поперечного отклонения 0.45 м. Точность метода сравнима с точностью других современных подходов, однако развиваемая крейсерская скорость робота-погрузчика оказывается выше. Предложены геометрические методы поправки угла рулевого колеса. Разработанный геометрический метод подбора имеет точность финального позиционирования менее 2 см. Преимуществом метода является возможность подстраиваться под паллету с произвольной позиции перед ней, тогда как большинство альтернативных методов рассматривает данную задачу исключительно при положении "напротив нее". Разработанный метод прошел этап симуляции и верификации в рамках натурных испытаний на нескольких прототипах беспилотных роботов-погрузчиков.