УПРАВЛЕНИЕ СФЕРИЧЕСКИМ РОБОТОМ С МАЯТНИКОВЫМ ПРИВОДОМ
прямоугольную систему координат - ) € , центр - которой жестко связан с геометрическим центром сферической оболочки робота. Оси системы координат - ) € параллельны осям неподвижной системы координат - )€.
Определим в координатах неподвижной системы координат - )€ единичный вектор нормали L к опорной поверхности и вектор ® = (0, 0, 1), противоположный направлению силы тяжести. Обозначим координаты центра масс робота и его угловую скорость через ¯ = ( , ), €) и ° соответственно, тогда динамика системы в безразмерных переменных описывается следующими дифференциальными уравнениями:
¯` = ±тр+ l L − ®,
°P = ´ − ¦[L × ±тр], l = 〈L ∙ ®〉 − 〈¯P ∙ LP〉,
±тр= −¸ ¹¡⁄|¹¡|, ¹¡= ¯P − [° × L],
(1)
где ±тр – сила трения между оболочкой и опорной поверхностью, l – величина силы нормального давления, ¸ – коэффициент сухого трения, ¹¡ – скорость точки контакта оболочки с поверхностью, ´ = −¬ – вектор управления и ¦ = * (« + 6Z) †⁄ . Приведенное выражение справедливо, если ¹¡ ≠ 0, в противном случае скольжение отсутствует и движение осуществляется с учетом неголономной связи
¯P = [° × L] (2)
В этом случае уравнения (1) можно упростить. Для этого исключим силу трения ±тр, учтем соотношение (2) и предположим, что отсутствует верчение сферической оболочки относительно вектора нормали L, то есть
〈° ∙ L〉 = 0, тогда
(1 + ¦)°P + ¦ L 〈°P ∙ L〉 = ´ − ¦[L × ®] (3)
Кроме того, в отсутствие скольжения должно выполняться условие ¼±тр¼ < ¸ l, где ±тр= ¯` − l L + ® и величина силы реакции опоры l вычисляется согласно третьему уравнению (1).
Синтез законов управления
Теперь рассмотрим вопрос об организации движения вдоль заданной траектории, которую описывает центр сферической оболочки. Будем считать, что движение по поверхности происходит без проскальзывания и траектория движения описывается параметрическим уравнением ¯•= ½( ), где ½( ) – дважды непрерывно дифференцируемая функция. Требуется найти вектор управления ´, обеспечивающий приближение робота к заданной траектории. Если в начальный момент времени = 0 выполняется соотношение °(0) = [L × ¯P(0)], то вектор ´ находится из уравнения (3):
´ = ´•= (1 + ¦)°P•+ ¦ L 〈°P•∙ L〉 + ¦[L × ®]. (4) Пусть теперь °(0) ≠ tL × ¯P•u, в этом случае обеспечить приближение к заданной траектории можно с помощью дополнительного корректирующего управления, а именно
´ = ´•+ ¾(¿ − ¿•), ¿ = e¯À, ¯PÀfÀ. (5) где матрица коэффициентов обратной связи ¾, вообще говоря, может быть функцией времени. Однако, для использования соотношения (5) требуется знать вектор нормали L, что не всегда возможно, поэтому заменим закон управления (5) на более простой и не требующий знания вектора нормали:
´ = (1 + ¦)[® × ¯P•] + ¾(¿ − ¿•). (6)
Для определения параметров обратной связи ¾ введем в рассмотрение два функционала:
† (¾) = sup
¿Á(•) Â ¿(•) ‖Ä‖ Å•
supÆ•|¿ − ¿•|
?¿À(0) Â ¿(0) + ‖Ç‖ , † (¾) = sup
¿Á(•) Â ¿(•) ‖Ä‖ Å•
supÆ•|¾(¿ − ¿•)|
?¿À(0)  ¿(0) + ‖Ç‖ . где  = ÂÀ> 0 – заданная матрица. Нетрудно видеть, что первый функционал характеризует максимальное отклонение траектории движения робота от заданной траектории ¯• вне зависимости от начальных условий и отличий вектора нормали от вектора ®, а второй – максимальное значение корректирующих управлений.
Таким образом, для определения матрицы обратной связи ¾ требуется для системы (2), (3) найти оптимальные по Парето решения двухкритериальной задачи оптимального управления с функционалами † и † .
Результаты численного моделирования
При проведении числительных экспериментов примем следующие значения параметров ¸ = 0.8, ¦ = 10.5. Будем считать, что робот движется по плоскости € = 0, а требуемая траектория движения центра масс робота является окружностью
•( ) = É cos Ì , )•( ) = É sin Ì , €•( ) = É.
Начальные условия зададим следующим образом:
¯(0) = (É, 0, 1), ¯P(0) = (0, Ì É, 0), °(0) = (−Ì É + 0.1, −0.2,0).
На рис. 2 при значениях параметров É = 2 и Ì = 0.5 представлены результаты численного моделирования приближения сферического робота к движению вдоль окружности: линией черного цвета показана желаемая траектория, а красной линией – траектория движения центра сферического робота. Как можно видеть, построенное управление обеспечивает решение поставленной задачи, то есть траектория движения центра
робота приближается к заданной окружности как в неголономном случае, описываемом уравнениями (2), (3), так и в общем случае, описываемом системой (1).
Рис. 2. Изменение величины отклонения центра оболочки от желаемой траектории при движении вдоль окружности в случае системы, описываемой уравнениями (2), (3)
Рис. 3. Изменение величины отклонения центра оболочки от желаемой траектории при движении вдоль окружности в случае общей системы, описываемой уравнениями (1)
Заключение
В работе рассматривается математическая модель безотрывного движения сферического робота по поверхности. Для обеспечения движения центра сферического робота вдоль заданной траектории предложено управление в форме обратной связи по состоянию. Корректность работы синтезированных законов управления продемонстрирована численными экспериментами.
Литература
1. Д.В. Баландин, М.А. Комаров, Г.В. Осипов // Известия РАН. Теория и системы управления. 2013. № 4. C. 150-163.
2. Д.В. Баландин, М.Ю. Скучилин // Журнал Средневолжского математического общества. 2013. Т. 15. № 4. C. 47-56.
3. R. Chase, A.A. Pandya // Robotics. 2012. Vol. 1. P. 3-21.
4. T.B. Ivanova, A.A. Kilin, E.N. Pivovarova // J. Dyn. Control Syst. 2018. Vol. 24. No. 3. P. 497-510.
5. T.B. Ivanova, A.A. Kilin, E.N. Pivovarova // J. Dyn. Control Syst., 2019. Vol. 25. No. 1. P. 1-16.
6. A.A. Kilin, E.N. Pivovarova, T.B. Ivanova // Regul. Chaotic Dyn. 2015. Vol. 20. No. 6. P. 716-728.
7. A. Koshiyama, K. Yamafuji // International Journal of Robotics Research. 1993. Vol. 12. P. 411-419.
МНОГОГИПОТЕЗНЫЙ АЛГОРИТМ ТРАЕКТОРНОЙ ОБРАБОТКИ Д.А. Бедин, А.Г. Иванов, А.А. Федотов
Институт математики и механики им. Н. Н. Красовского УрО РАН, Екатеринбург iagsoft@imm.uran.ru
Аннотация. Рассматривается задача восстановления траектории воздушного судна (ВС) по входящим радиолокационным измерениям: после поступления очередного замера алгоритм должен «моментально» дать оценку положения ВС. Алгоритм строит пучок траекторий, каждая из которых представляет собой некоторый вариант движения ВС, совместимый с его динамическими возможностями. При получении очередного замера, проводится ряд операций над пучком, в том числе, происходит формирование оценки текущего положения ВС.
Применение описанного подхода позволило создать алгоритм устойчивый к возможным выбросам замеров.
Работа поддержана РФФИ в рамках проекта № 18-01-00410.
Введение
Рассматривается задача реконструкции траектории в режиме реального времени по входящим радиолокационным измерениям: после поступления следующего радиолокационного замера алгоритм должен практически немедленно дать оценку положения ВС. Основная трудность задачи заключается в том, что объект движется нестационарно, то есть выполняет маневры, характеристики и продолжительность которых неизвестны наблюдателю. Возможны длительные участки, где тип движения постоянный, при этом траектория самолета может хорошо аппроксимироваться простой моделью. Кроме того, для работы в реальных ситуациях важна устойчивость к «выбросам» в измерениях.
В настоящей работе рассматривается один из возможных подходов к решению проблемы. Приводятся результаты обработки типичных модельных данных.
Общее описание алгоритма
В основе алгоритма лежит построение набора (пучка) «наиболее вероятных» траекторий летательного аппарата, который строится с учетом имеющихся замеров, ограничений на динамику ВС и возможных выбросов измерений. Конечные точки траекторий в пучке используются для построения оценки положения ВС в текущий момент времени.
Предполагаем, что ВС движется в горизонтальной плоскости в силу стандартной модели простейшего движения самолета [1] (
x
иz
— координаты на плоскости,
— путевой угол,v
— скорость):
w v
v u v z
v x
/ sin cos
В случае постоянных продольного
w
и поперечногоu
управлений эти уравнения могут быть проинтегрированы аналитически, см. [2]. Каждая траектория пучка соответствует динамике и кусочно- постоянным управлениямw
иu
. Предполагается, что длительность участков постоянства не может быть меньше определенного заданного ограничения.В начале работы алгоритма происходит инициализация пучка траекторий по нескольким первым замерам. Далее выполняется основной цикл, где каждая итерация связана с поступлением нового замера.
Пучок пересчитывается с использованием замеров из скользящего временно́го окна фиксированной длительности, который заканчивается последним замером. Пересчет запускается при поступлении каждого нового замера.
Для каждой траектории пучка рассчитывается критерий соответствия замерам. Критерий рассчитывается на основе расстояния между траекторией и замерами и учитывает дополнительные штрафы.
Был протестирован ряд критериев с различными свойствами. Следующие свойства являются общими для всех критериев:
меньшее значение критерия соответствует траектории, расположенной ближе к замерам;
если траектория проходит точно через замеры, значение критерия равно нулю.
Дополнительно штрафуется следующее поведение:
значения продольного и поперечного ускорений на траектории не укладываются в ограничения;
длительность участка постоянного управления меньше установленного значения;
продолжительность двух смежных участков постоянного управления меньше установленного значения;
значение величины скорости самолета слишком мало или слишком велико;
тип движения на отдельных участках траектории не соответствует типу, определенному детектором типа движения (см. ниже).
Основные процедуры алгоритма
Опишем процедуры алгоритма в порядке их применения после поступления очередного замера.
Продление пучка и подрезка пучка. На этом этапе вычисляется прогнозное продолжение всех треков на момент нового замера. Последний участок постоянного управления продлевается до момента текущего замера. С другой стороны треки «подрезаются» по времени со стороны старых замеров так, чтобы общая продолжительность трека не превышала заданную длину расчетного окна.
Ветвление. Это процедура, в которой для каждой траектории создаются возможные варианты ее продления с измененными (по отношению к исходной продленной траектории) значениями управления.
Осуществляется непрерывное «склеивание» ответвления с родительской траекторией в промежуточной точке ветвления. Выбирая разные моменты ответвления на исходной траектории и разные значения управления после ветвления, получаем различные варианты ответвившейся траектории. В пучок войдут траектории с хорошим значением критерия соответствия.
Один из вариантов ветвления — траектория, которая попадает точно в точку последнего замера. Для построения этой траектории используется решение вспомогательной задачи о попадании в точку, описанное в работе [2].
Рассматриваются и другие варианты ветвления: ветвления с крайними значениями управления на участке ответвления; ответвления, которые попадают в случайные точки рядом с последним замером;
ветвление с нулевым управлением на участке ответвления. Последнее используется для улучшения аппроксимации в областях, где ВС завершает свой маневр и начинает равномерное и прямолинейное движение.
На этом же этапе формируются специальные траектории, а именно «прямая по МНК» и «окружность по МНК», которые рассчитываются без использования какой-либо траектории пучка в качестве родительской.
Траектории строятся из условия минимума среднеквадратичного отклонения от замеров. «Прямая по МНК»
предполагает постоянное продольное ускорение и нулевое поперечное ускорение. «Окружность по МНК»
конструируется с нулевым продольным ускорением и постоянным поперечным ускорением.
Определение типа движения. Алгоритм оценивания формирует оценку продольного и поперечного ускорений. Каждое из них анализируется специальным алгоритмом-детектором. Цель детектора —обнаружение того, что входной сигнал близок к постоянному, или наоборот, обнаружение его внезапного изменения после периода постоянства. Тип движения влияет на штрафы, налагаемые на траектории пучка, а также на вес специальных траекторий в алгоритме расчета позиции ВС.
Предварительное прореживание пучка. На этом этапе удаляются траектории, которые не удовлетворяют физическим ограничениям и плохо согласуются с имеющимися замерами.
Выборочная оптимизация. При оптимизации происходит варьирование величин управления и времен переключения между участками постоянного управления. Используется метод прямого поиска для нахождения минимума многомерной функции. Процедура оптимизации, применяемая ко всем траекториям, приводит к плохим результатам из-за «истончения» пучка и потери многогипотезности. Поэтому оптимизация осуществляется только по небольшому количеству траекторий с наилучшим значением критерия соответствия.
Удаление дубликатов. На этом этапе создается матрица взаимных расстояний между траекториями пучка. Если в процессе создания матрицы обнаруживаются две совпадающие или очень близкие траектории, то траектория с худшим критерием соответствия удаляется из пучка.
Расчет оценки положения ВС. В момент поступления замера алгоритм должен производить оценку местоположения ВС. Последняя точка каждой траектории пучка представляет оценку положения ВС, соответствующую этой траектории. Таким образом, имеем множество оценок положения ВС. В качестве ответа формируется средневзвешенная оценка.
В оценке участвуют только те траектории, для которых значение основного критерия соответствия относительно мало. Для каждой траектории рассчитывается ее вес. Формирование веса с использованием того же критерия, что и в основных процедурах, не дает хорошие результаты. В описываемой версии оценка текущего положения генерируется с использованием весов, полученных из других критериев качества.
В зависимости от типа текущего движения, полученного алгоритмом-детектором, вес траекторий «по МНК»
может быть принудительно увеличен.
Группировка и прореживание. Цель этой процедуры — уменьшение количества траекторий в пучке при сохранении репрезентативности различных гипотез о движении ВС.
В матрице взаимных расстояний между траекториями пучка определяется пара траекторий с минимальным расстоянием, при этом траектория с худшим критерием соответствия удаляется из пучка. Далее переходим к следующей паре траекторий с минимальным расстоянием и т.д. Процедура продолжается до тех пор, пока количество траекторий не станет меньше заданного числа.
Результаты моделирования
Сконструирована идеальная модельная траектория, которая состоит из участков устойчивого движения и переходных участков, для которых максимальные ошибки восстановления определены в авиационных стандартах [3]. Для этой траектории было получено 100 треков замеров со среднеквадратичным отклонением 70 м. Для каждого из треков замеров траектория была восстановлена с использованием описанного алгоритма.
Построены графики среднеквадратичного отклонения восстановленных положений от истинного движения.
Временной график продольного отклонения изображен на рис. 1 (толстая сплошная черная линия). Кроме того, приведен аналогичный график для алгоритма IMM [4] (красная линия). Пунктирная синяя линия показывает среднеквадратичное отклонение для замеров.
В случае обработки траекторий с выбросами (редкие большие отклонения, которые не соответствуют регулярному распределению), преимущество предложенного алгоритма становится более очевидным. На рис. 2 представлено продольное отклонение в случае присутствия выбросов в замерах. Здесь с вероятностью 1/20 измерение является выбросом. В случае, если замер объявлен выбросом, ошибка замера увеличивается в пять раз по сравнению с обычным уровнем.
200 400 600 800 1000 1200
0 50 100 150 200
время, с продольное отклонение, м
200 400 600 800 1000 1200
0 50 100 150 200
продольное отклонение, м
время, с Рис. 1. График среднеквадратичного продольного
отклонения как функция времени. В замерах нет выбросов.
Широкая сплошная черная линия показывает результаты по описанному алгоритму. Красная линия — результат по методу IMM. Пунктирная синяя линия — трек замеров
Рис. 2. График среднеквадратичного продольного отклонения как функция времени. В замерах присутствуют
выбросы. Широкая сплошная черная линия показывает результаты по описанному алгоритму. Красная линия — результат по методу IMM. Пунктирная синяя линия — трек
замеров
Заключение
Создан алгоритм восстановления траектории ВС, основанный на построении пучка траекторий, каждая из которых соответствует некоторой гипотезе об истинном движении ВС. Среди предположений о движении ВС есть предположение о движении по прямой и предположение о движении по окружности. Преимущество алгоритма особенно проявляется в случае обработки измерений с выбросами.
Авторы благодарят ООО «Фирма «НИТА» за постановку задачи и обсуждение полученных результатов.
Литература
1. Р.М. Ахмедов, А.А. Бибутов, А.В. Васильев и др. Автоматизированные системы управления воздушным движением: Новые информационные технологии в авиации: учеб. пособие / под ред. С.Г. Пятко, А.И. Красова. СПб.: Политехника, 2004. 446 с.
2. Д.А. Бедин, В.С. Пацко, А.А. Федотов и др. Восстановление траектории самолета по неточным измерениям // Автомат. и телемех., 2010, № 2. С. 17–30.
3. SUR.ET1.ST01.1000-STD-01-01: EuroControl standart document for radar surveillance in en-route airspace and major terminal areas. Edition 1.0, 1997. https://www.eurocontrol.int/publications/eurocontrol-standard-radar-surveillance-en-route-airspace-and- major-terminal-areas
4. Y. Bar-Shalom, X. Li, T. Kirubarajan. Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software.
John Wiley & Sons, 2001. 584 p.