ДЕКОМПОЗИЦИЯ ЗАДАЧИ УПРАВЛЕНИЯ ДВИЖЕНИЕМ МОБИЛЬНОГО РОБОТА ПО ПЕРЕСЕЧЕННОЙ МЕСТНОСТИ
←
→
Транскрипция содержимого страницы
Если ваш браузер не отображает страницу правильно, пожалуйста, читайте содержимое страницы ниже
ДЕКОМПОЗИЦИЯ ЗАДАЧИ УПРАВЛЕНИЯ ДВИЖЕНИЕМ МОБИЛЬНОГО
РОБОТА ПО ПЕРЕСЕЧЕННОЙ МЕСТНОСТИ
Лисицкий Д. Л.
Саратовский государственный технический университет, Саратов, Росси
foxden@yandex.ru
Ключевые слова: робот, препятствие, управление, траектория, регулятор, оптимизация.
Введение
В настоящее время развитие робототехники вплотную подошло к созданию мобильных
роботов автономно перемещающихся по обширным участкам пересеченной местности. Задача
управления движением таких робота обычно ставится следующим образом: робот должен
перемещаться по участку местности размером до 5×5 км по замкнутому маршруту, заданному
координатами ряда контрольных точек. Управление должно быть полностью автоматическим
При этом может использоваться вся доступная на современном этапе технического развития
информация, включая топографическую и аэрофотографическую информацию об участке,
которую можно получить до начала движения робота; информация о текущем положении
робота в инерциальной системе координат, получаемая как с помощью инерциальных
датчиков, так и с помощью GPS (ГЛОНАСС) приемников; информация о положении робота
относительно препятствий местности, получаемая с помощью различного рода оптических
датчиков.
Эффективное использование такой разнородной информации представляет собой сложную
задачу. Поэтому при управлении движением мобильного робота, по нашему мнению, удобно
использовать метод декомпозиции, разделяя общую очень сложную задачу на ряд
последовательно решаемых относительно простых.
Первым этапом решения задачи управления, на наш взгляд, должно являться
формирование по топографической и аэрофотографической информации об участке местности,
по которому должен перемещаться робот, опорной траектории. Под опорной траекторией
будем понимать приблизительно выбранную траекторию движения робота, соединяющую
контрольные точки маршрута и учитывающую возможности робота по преодолению
препятствий местности. Траектория не только должна огибать макро препятствия
непреодолимые для робота типа водоемов, оврагов, зданий, участков местности с повышенным
углом наклона, но и минимизировать опасность возникновения аварийных ситуаций путем
планирования маршрута по наиболее легко проходимым роботом местам. Опорная траектория
может быть аппроксимирована участками прямых, координаты концов которых задаются в
инерциальной системе координат в качестве опорных точек при дальнейшем управлении
движением робота.
На втором этапе решения задачи должна формироваться программная траектория
движения робота от одной опорной точки к другой, которая огибает микро препятствия
местности, минимально отклоняясь от опорной траектории.
Микро препятствиями будем называть не нанесенные на карту относительно мелкие
препятствия непроходимые для робота: камни деревья, ямы. Для обнаружения этих
препятствий должны использоваться датчики, установленные на корпусе робота, например,
лазерный дальномер со сканирующим лучом.
На третьем этапе решения задачи робот под управлением регулятора должен отслеживать
программную траекторию.
1. ФОРМИРОВАНИЕ ОПОРНОЙ ТРАЕКТОРИИ
Для формирования опорной траектории удобно использовать векторные карты местности,
которые представляют собой набор примитивных объектов (точек, линий, полигонов) с
1
Труды конференции "Технические и программные - 000798 - Москва, октябрь 2010
средства систем управления, контроля и измерения"набором атрибутов, описывающих основные характеристики каждого объекта (координаты,
высота над уровнем местности, тип поверхности, наличие растительности, наличие застройки).
Стандартная карта, представленная даже в векторном формате, малопригодно для
непосредственного формирования опорной траектории, так как имеет достаточно
произвольный формат. Поэтому предварительно должна быть проведена определенная
подготовительная работа. Результатом этой работы должна стать карта местности в векторном
формате, которая является совокупность элементарных ячеек, например, разбита на квадраты
определенного размера. Размер каждого из этих квадратов должен позволять свободный проезд
робота при условии возможности объезда микро препятствий местности. Каждая ячейка карты
должна сопровождаться определенными атрибутами, которым относятся: координаты центра
ячейки в декартовой системе координат, количественная оценка степени проходимости данного
участка для робота.
Количественная оценка проходимости должна учитывать характер грунта, угол наклона
поверхности к горизонту, наличие растительности или строений. Автоматическое описание
атрибутов является самостоятельной достаточно сложной научно-технической проблемой.
Однако вполне реальным будет на первых этапах определение количественных оценок в
полуавтоматическом режиме при помощи экспертов. Для количественной оценки
проходимости можно использовать коэффициент проходимости, вычисляемый по аддитивному
критерию.
В качестве формального критерия оптимальности опорной траектории можно предложить
условие минимизации функционал вида:
n
I = ∑ si Ri → min , (1)
1
где i – номер участка по порядку прохождения; Ri - коэффициент проходимости участка с
номером i; si - длина пути робота по участку с номером i; n – количество проходимых
участков.
Для построения оптимального маршрута карту местности предложен ряд методов,
вытекающих из метода динамического программирования. Наиболее известным из них
является алгоритм Дейкстры [1, 4].
Модельный пример работы программы, использующий алгоритм Дейкстры,
продемонстрирован на рис.1. На карте схематично изображена пересечённая местность, для
простоты вся поверхность участка за исключением четырех объектов была выбрана с
одинаковой величиной коэффициента проходимости равной 1. Четыре объекта были выбраны
непроходимыми для робота с коэффициентами проходимости равным 100 и 90.
На карте схематично показано разбиения участка на ячейки. Значения коэффициента
проходимости приведены в центре ячеек. Выбранная опорная траектория показана
непрерывной черной линией.
Опорная траектория аппроксимирована семью отрезками прямых, конечные точки
которых обозначены цифрами от 1 до 8. Аппроксимирующие прямые проводились через
центры ячеек (вершины), удаленных друг от друга на расстояние от одного до пяти
межцентровых расстояний между ячейками. В качестве критерия выбора допустимой длины
аппроксимирующей прямой использовалось условие ограничения кратчайшего расстояния от
любой точки участка опорной траектории, заключенного между вершинами, через которые
проводится аппроксимирующая прямая, до аппроксимирующей прямой.
2
Труды конференции "Технические и программные - 000799 - Москва, октябрь 2010
средства систем управления, контроля и измерения"Рис. 1. Пример работы программы.
2. ФОРМИРОВАНИЕ ПРОГРАММНОЙ ТРАЕКТОРИИ
2.1. Выбор структуры системы управления
Система автоматического управления (САУ) движением мобильного робота должна
осуществлять управление перемещением объекта из одной опорной точки опорной траектории
до другой по траектории, отвечающей какому-либо критерию оптимальности, с учетом
ограничений, наложенных на координаты и управления объекта, в условиях действия внешних
возмущений, помех, нестационарности и неточной идентификации математической модели
объекта. При этом одна часть ограничений заранее известна, так как относится к
математической модели объекта. Другая часть ограничения заранее не известна и
представляют собой препятствия, которые робот должен огибать при движении и которые он
обнаруживает с помощью специальных датчиков, установленных на его корпусе.
В настоящее время неизвестны методы, позволяющие создать алгоритм выбора управления
объекта в описанной постановке задачи. Выход из ситуации можно найти, создавая САУ в
классе систем с моделью, и, разделяя систему на две независимых подсистемы - подсистему
формирования программной траектории и подсистему отслеживания объектом программной
траектории. Причем алгоритм формирования программной траектории синтезировать с
использованием предельно упрощенной математической модели робота.
Непосредственное применение такого подхода затруднено тем, что для формирования
программной траектории необходима информация о «положении» модели относительно
препятствий. Датчики же установленные на корпусе робота выдают информацию о его
положении относительно препятствий. Поэтому обязательной становится операция пересчета
информации датчиков о положении объекта относительно препятствий в информацию о
положении «модели» относительно тех же препятствий.
Исключить все противоречия можно, используя для создания САУ структурную схему,
изображенную на рис. 2, на которой введены следующие обозначения: X - вектор текущего
состояния объекта в стартовой системе координат (СК); X 0 вектор желаемого конечного
состояния объекта в стартовой СК; X М - вектор текущего состояния модели в стартовой СК;
U - вектор управлений объекта; ВПУ – вычислитель программного управления; Объект -
объект управления; Модель – упрощенная математическая модель объекта управления, причем
именно та, исходя из которой был синтезирован алгоритм выбора программного управления;
ПК и КОС – соответственно прямой канал и канал обратной связи регулятора, синтезированные
3
Труды конференции "Технические и программные - 000800 - Москва, октябрь 2010
средства систем управления, контроля и измерения"исходя из условия наилучшего возможного отслеживания объектом координат модели; ∆X 0 -
вектор координат конечной точки в СК, связанной с объектом; ∆X м - вектор точки начала СК,
«связанной» с моделью, в СК, связанной с объектом; δX М - вектор координат конечной точки в
СК модели.
Из структурной схемы видно, что величины ∆X М и δX М вычисляются по формулам:
∆X M = X М − X ; (2)
δX M = ∆X 0 − ∆X М . (3)
Величина ∆X 0 измеряется датчиками, установленными на борту робота:
∆X 0 = X 0 − X . (4)
Подставляя в (3) выражения (4) и (2), получим:
δX М = X 0− X М . (5)
Преобразуя структурную схему рис. 2 с учетом выражения (4), получим эквивалентную
схему рис. 3, из которой следует, что система, построенная по схеме
рис. 2, эквивалентна системе, состоящей из двух последовательно включенных подсистем
«модель-ВПУ» и «объект-регулятор». Подсистема «модель-ВПУ» формирует программную
траекторию, а подсистема «объект-регулятор» ее отслеживает. Причем обоснованным выбором
параметров программной траектории и регулятора можно обеспечить отслеживание объектом
программной траектории с требуемой точностью.
Рис. 2. Исходная структура САУ Рис.3. Эквивалентная структура САУ
Так как в системе в явной форме отсутствует общая обратная связь, то устойчивость и
грубость всей системы определяются устойчивостью и грубостью подсистем; изменение каких-
либо параметров устройств, входящих в одну подсистему, не влияют на устойчивость другой
подсистемы. Устойчивость этих подсистем можно обеспечить при синтезе, а грубость при
изменении параметров объекта в широких пределах либо адаптивным регулятором, либо, строя
регулятор по заведомо «грубой» структуре.
Поскольку отличие динамических свойств модели и робота не влияют на устойчивость
системы, а только на точность отслеживания объектом «движения» модели, ее можно выбирать
предельно простой, что в свою очередь упрощает алгоритм выбора программного управления.
Разбиение системы на две независимые подсистемы дает возможность синтезировать
алгоритм программного управления и регулятор независимо друг от друга, применяя для этого
хорошо разработанные в настоящее время методы. Так как на подсистему «модель-алгоритм»
не действуют внешние возмущения и модель полностью идентифицируема, для синтеза
алгоритма выбора программного управления можно использовать методы теории оптимального
управления. Для синтеза регулятора, обеспечивающего отслеживание объектом программной
траектории, могут быть применены методы синтеза, основанные на теории линейных систем,
позволяющие учитывать влияние внешних возмущений, помех, неточной идентификации
объекта управления.
3.2. Постановка задачи формирования программной траектории движения робота
4
Труды конференции "Технические и программные - 000801 - Москва, октябрь 2010
средства систем управления, контроля и измерения"В идеале мобильный робот должен перемещаться из одной опорной точки до другой по
соединяющей их прямой. Однако это невозможно, так как на пути он обязательно будет
встречать препятствия, заставляющие отклоняться от прямолинейной траектории. Поэтому
мобильный робот должен перемещаться из опорной точки А горизонтальной плоскости в
опорную точку В по траектории, минимально отклоняющейся от прямой, соединяющей эти две
точки (рис. 4). Форма препятствий местности может быть произвольной. При огибании
препятствия между ним и роботом должно оставаться некоторое заданное расстояние (Yз ) .
Для ясности дальнейших рассуждений предположим, что препятствие сплошное и
несимметричное относительно прямой АВ (рис. 4). Робот может огибать препятствие, обходя
его с разных стороны, однако, одно из направлений обхода всегда будет требовать большего
отклонения от прямой АВ и, соответственно, должно быть исключено. Кроме того, в точке
траектории, наиболее удаленной от прямой АВ (точки С и C ′ на рис. 4), вектор скорости
объекта должен быть ей параллелен.
Для обнаружения препятствий местности на борту робота должен быть установлен
специальный датчик. Предположим, что им является лазерный дальномер с лучом,
сканирующим в горизонтальной плоскости (ЛДС), угол сканирования которого равен 2ϕmax,
причем ϕmax не меньше максимального угла, который может составлять траектория движения
робота с прямой АВ. Предположим также, что ось симметрии сектора сканирования луча ЛДС
параллельна прямой АВ, а угол отклонения луча ЛДС от оси симметрии сектора - ϕ изменяется
дискретно с интервалом ∆ϕ (рис. 4). Будем считать, что ЛДС обнаруживает препятствия и при
каждом i-ом измерении определяет угол наклона луча ϕi и расстояние до препятствия li, что
позволяет вычислить координаты всех видимых дальномером точек препятствия в связанной с
объектом системе координат. Всего за полупериод сканирования будет получена информация о
расстоянии до k = (2ϕ max / ∆ϕ ) + 1 точек препятствия.
Предполагается, что положение центра масс робота в неподвижной СК АXY (рис. 4)
( X , Y ) , первые производные этих координат (Vx , V y ) и ориентация робота в той же СК
определяются с помощью комплексной инерциальной системы навигации и ориентации,
корректируемой по показаниям дифференциальных GPS-датчиков.
Рис. 4. Иллюстрация постановки
В соответствии с выбранной структурой системы необходимо сформировать упрощенную
математическую модель робота. Предположим, что продольная составляющая скорости центра
масс робота стабилизируется специальным регулятором и поэтому постоянна. Тогда
упрощенную математическую модель движения центра масс робота можно записать как
кинематические дифференциальные уравнения плоского движения материальной точки:
5
Труды конференции "Технические и программные - 000802 - Москва, октябрь 2010
средства систем управления, контроля и измерения"dYM dV yM dX M
= V yM ; = gn y ; = VxM = const , (6)
dt dt dt
где YM , X M – координаты упрощенной модели объекта в системе координат АХУ; VxM, VyM –
соответственно, продольная и поперечная составляющие скорости упрощенной модели; ny –
поперечная перегрузка модели; g – ускорение силы тяжести. В этой системе ХМ, YM, VxM, VyM –
координаты упрощенной модели робота, ny – управление упрощенной модели объекта, которое
далее будем называть программным управлением.
На координаты и управления упрощенной модели объекта наложены ограничения:
− V y max ≤ V yM ≤ V y max ; − n y max ≤ n y ≤ n y max . (7)
Величины этих ограничений следует выбирать так, чтобы динамические свойства робота
позволяли ему отслеживать «движение» модели.
Так как координаты модели (6) определяют программную траекторию, то для ее
формирования необходимо синтезировать алгоритм выбора программного управления ny,
который определяет направление обхода препятствия и переводит объект (6, 7) из
произвольной начального состояния в точку расположенную на расстоянии Yз от точки
препятствия, наиболее удаленной от оси АХ (рис. 4), с поперечной составляющей вектора
скорости равной нулю ( V yM = 0 ), таким образом, чтобы минимизировался функционал:
∞
I = ∫ YM dt → min . (8)
0
Для удобства использования этот алгоритм должен являться функции координат модели в
неподвижной системе координат и положения модели относительно препятствий местности. В
каждый момент времени программная траектория должна определяться координатами модели в
СК АХУ.
3.3. Синтез алгоритма выбора программного управления
Для решения задачи будем использовать методику, описанную в работе [3], согласно
которой алгоритм выбора оптимального управления синтезируется, исходя из анализа
положения изображающей точки относительно фазового портрета оптимального переходного
процесса.
Введем новые переменные:
xi = X Пi − X M ; yi = YПi + Yз sign(YПi ) − YM ; v y = −V yM , (9)
где X Пi , YПi - координаты i-й точки препятствия местности в неподвижной системе координат
АХУ.
Тогда система уравнений (9) примет вид:
dyi dv y dxi
= vy ; = − gn y ; = −VxM = const , (10)
dt dt dt
Произведенная замена переменных эквивалентна введению новой системы координат Оху
(рис. 5), начало которой расположено в точке О, в которую следует привести объект
управления. Одновременно вводится в рассмотрение 3-мерное фазовое пространство Охуvy
(рис. 6).
Новые переменные xi , yi и координата YПi для каждой i-й точки препятствия могут быть
определены по сигналам ЛДС. С учетом того, что ЛДС установлен на борту робота, а не
модели, формулы для расчета этих переменных приобретают вид:
xi = li cos ϕ i ; yi = li sin ϕ i − (YM − Y ) + Yз sign(YПi );
(11)
YПi = Y + li sin ϕ i ,
где Y - координата центра масс робота в системе координат АХУ.
6
Труды конференции "Технические и программные - 000803 - Москва, октябрь 2010
средства систем управления, контроля и измерения"После введения новых переменных задача формально сводится к определению
оптимального алгоритма в смысле функционала (8) перемещения изображающей точки из
произвольного положения в фазовом пространстве Охуvy с координатами хi , уi , v у в начало
координат – точку О.
Рис. 5. Иллюстрация формализации задачи
Исходя из принципа максимума Понтрягина и теоремы об n интервалах можно показать,
что оптимальное управление будет релейным, то есть ny должно принимать только три
значения ( n y max , 0, −n y max ).
Построим фазовый портрет завершающего этапа такого переходного процесса (рис. 6)
(последние три интервала постоянных значений управления). Несложно видеть, что в начало
координат можно попасть лишь по двум траекториям – траектории АО с управлением
n y = − n y max и траектории ВО - с управлением n y = n y max . На координату vy согласно
обозначениям (9) также наложены ограничения (7), которые приобретают для этой координаты
вид:
− v y max ≤ v y ≤ v y max , (12)
где v y max = −Vy max , поэтому в точке А с координатами ( xa , y a , v y = V y max ) траектория АО
переходит в траекторию АD, у которой n y = 0 , а v y = V y max = const . Аналогично траектория ВО
в точке В с координатами ( xb , yb , v y = −V y max ) переходит в траекторию ВС, у которой n y = 0 , а
v y = −V y max = const .
Рис. 6. Фазовый портрет системы
7
Труды конференции "Технические и программные - 000804 - Москва, октябрь 2010
средства систем управления, контроля и измерения"Перейти к траектории АО изображающая точка может только по поверхности П1 с
управлением n y = n y max , а к ее продолжению – траектории АD – по поверхности П2 тоже с
управлением n y = n y max . Границей между поверхностями П1 и П2 является траектория АМ.
Приблизиться к траектории ВО изображающая точка может только по поверхности П3 с
управлением n y = − n y max , а к ее продолжению – траектории ВС – по поверхности П4 тоже с
управлением n y = − n y max . Границей между поверхностями П3 и П4 является траектория BN.
Переход изображающей точки к описанным фазовым траекториям, приводящим ее к
началу координат, осуществляется также с одним из трех значений управления
( n y max , 0, −n y max ). Фазовые траектории, соответствующие этому этапу управления, не показаны
на фазовом портрете для его наглядности и так как они не имеют непосредственного
отношения к синтезу алгоритма управления.
Для получения уравнений всех перечисленных траекторий и поверхностей, исключим из
уравнений (6) время и проинтегрируем их в обратном направлении – от конца фазовых
траекторий к началу при соответствующих постоянных значениях управления
( n y max , 0, −n y max ).
Уравнения траектории ОА:
v 2y −V xM v у
yi OA = yi = ; xi OA = xi = . (13)
2 gn y max gn y max
Уравнение поверхности П1»:
v 2y (− xi gn y max + VxM v у ) 2
yi П1 = yi = − + 2
; (14)
2 gn y max 4VхM gn y max 1
Уравнение граничной траектории АМ:
v y2 v y2 max
yi АМ = уi = − + ; (15)
2 gn y max gn y max
Уравнение поверхности П2:
v 2y v y max V xM v у V xM v у max 2v y2 max
y i П2 = yi = − + ( xi − − )+ . (1.50)
2 gn y max Vx м gn y max gn y max gn y max
Уравнения траектории ВО:
v 2y V xM v у
yi OB = yi = − ; xi OB = xi = . .(16)
2 gn y max gn y max
Уравнение поверхности П3:
v y2 ( xi gn y max + VxM v у ) 2
yi П3 = yi = − 2
; (17)
2 gn y max 4VхM gn y max
Уравнение граничной траектории BN:
v y2 v y2 max
yi BN = yi = − . (18)
2 gn y max gn y max
Уравнение поверхности П4:
8
Труды конференции "Технические и программные - 000805 - Москва, октябрь 2010
средства систем управления, контроля и измерения"v y2 v y max V xM v у V xM v у max 2v 2y max
yi П4 = yi = + ( xi + − )− . (19)
2 gn y max VxM gn y max gn y max gn y max
При формировании алгоритма переключения управления необходимо иметь в виду
несколько обстоятельств.
Программная траектория будет оптимальна в смысле функционала (8), если она будет
совпадать с прямой АВ, за исключением необходимых отклонений по объезду препятствий.
Алгоритм должен анализировать все точки препятствия для определения момента начала
маневра огибания препятствия, однако, сам маневр должен начинаться только в тот момент,
когда потенциально опасные точки появляются как в положительной части, так и
отрицательной части плоскости АХУ. Причем программная траектория должна огибать ту
сторону препятствия, которая меньше отклоняется от оси АХ.
Любая i-я точка препятствия при Yni > 0 является потенциально опасной для объекта, если
изображающая точки попала на поверхности П1, П2, траекторию ОА или пересекла их.
Соответственно при Yni < 0 признаком потенциальной опасности i-й точки препятствия
является попадание изображающей точки на поверхности П3, П4, траекторию ОВ или их
пересечение.
При попадании изображающей точки на одну из поверхностей или линий переключения,
приводящих ее в начало координат (в соответствии со знаком Yni ), программное управление
должно выбираться равным управлению, для которого получены эта поверхность или линия.
Если изображающая точка не попала ни на одну из линий или поверхностей переключения,
программное управление должно выбираться таким, чтобы оно переводило модель к
траектории АВ за минимальное время.
В соответствии с изложенным подходом был сформирован алгоритм выбора управления на
каждом шаге интегрирования уравнений (6), однако, для стабилизации объекта на прямой АВ
был использован регулятор с зоной линейности при малых отклонениях.
1. Выбор программного управления, стабилизирующего объект на траектории АВ,
осуществляется регулятором с законом управления, учитывающим ограничения (7):
H 1 = c1YM + c2VуM ; (20)
H 1, если H 1 < n y max ;
H2 = (21)
n y max sign( H 1), если H 1 ≥ n y max ;
H 2, если ( V yM < V y max ) ∨ ( H 2 > 0 ∧
n yр = ∧ V yM < V y max ) ∨ ( H 2 < 0 ∧ V yM > −V y max ); (22)
0, в остальных случаях,
где с1 , с2 - постоянные коэффициенты; Н1, Н2 – промежуточные переменные, n yр - выходной
сигнал регулятора.
2. Для каждого положения луча дальномера выбирается программное управление,
минимизирующее функционал (8):
Если YПi ≥ 0 , то:
yiП1 , если yi − yiAM < 0;
H 3i = (23)
yiП2 , если yi − yiAM ≥ 0.
9
Труды конференции "Технические и программные - 000806 - Москва, октябрь 2010
средства систем управления, контроля и измерения"n y max , если [( y i − H 3i ≥ 0) ∧ (V yM < 0)] ∨
∨ [( y i − H 3i ≥ 0) ∧ (V yM ≥ 0) ∧ ( yi − yiOA > 0) ∧ (V yM < V y max )];
0, если [( y − H < 0) ∨ (V < −V
i 3i yM y max )] ∨
n y1i = (24)
∨ [( y i − H 3i ≥ 0) ∧ (V yM ≥ 0) ∧ ( yi − yiOA > 0) ∧ (V yM ≥ V y max )];
− n , если [( y i − H 3i < 0) ∧ (V yM > −V y max )] ∨
y max
∨ [( y i − H 3i < 0) ∧ (V yM ≥ 0) ∧ ( yi − yiOA ≤ 0)].
Если YПi < 0 , то:
yiП3 , если yi − yiBN ≥ 0;
H 4i = (25)
yiП2 , если yi − yiBN < 0.
− n y max , если [( y i − H 4i ≤ 0) ∧ (V yM > 0)] ∨
∨ [( y i − H 4i ≤ 0) ∧ (V yM ≤ 0) ∧ ( yi − yiOB < 0) ∧ (V yM > −V y max )];
0, если [( y − H < 0) ∧ (V ≥ −V
i 4i yM y max )] ∨
n y 2i = (26)
∨ [( y i − H 4i ≥ 0) ∧ (V yM < 0) ∧ ( yi − yiOB < 0) ∧ (V yM ≤ −V y max )];
− n , если [( y i − H 4i > 0) ∧ (V yM < −V y max )] ∨
y max
[( y i − H 4i ≥ 0) ∧ (V yM ≥ 0) ∧ ( yi − yiOA ≥ 0)].
Здесь i = 0, k ; H 3i , H 4i - промежуточные переменные.
Каждому значению i соответствует упорядоченная пара {n y1i , YПi } , если YПi ≥ 0 , и
{n y 2i , YПi } , если YПi < 0 .
3. Из всех пар, соответствующих YПi ≥ 0 отбираем те, у которых n y1i > −n y max . Если таких
пар нет, то задаем n y1 = −n y max и пропускаем п. 5 алгоритма.
4. Из всех пар, соответствующих YПi < 0 отбираем те, у которых n y 2i < n y max . Если таких
пар нет, то задаем n y 2 = n y max и пропускаем п. 6 алгоритма.
5. Из оставшихся пар, соответствующих YПi ≥ 0 отбираем ту, у которой
YПi максимально. Обозначим параметры этой пары {n y1 , YП1} .
6. Из оставшихся пар, соответствующих YПi < 0 отбираем ту, у которой YПi минимально.
Обозначим параметры этой пары {n y 2 , YП2 } .
7. Выбираем программное управление:
n yр , если (n y1 = −n y max ) ∨ (n y 2 = n y max );
n y = n y1 , если (n y1 > −n y max ) ∧ ( YП1 < YП1 ; (27)
n y 2 , если (n y1 < n y max ) ∧ ( YП1 > YП1 .
В результате интегрирования уравнений (6) с выбранными значениями управления,
получаем численное значение координат модели YM , V yM , которые и определяют значение
программной траектории в каждый момент времени. Координату X M модели принимаем
равной координате X Р центра масс робота.
10
Труды конференции "Технические и программные - 000807 - Москва, октябрь 2010
средства систем управления, контроля и измерения"4. РЕЗУЛЬТАТЫ ЧИСЛЕННОГО МОДЕЛИРОВАНИЯ
Работоспособность и эффективность предложенного алгоритма проверялась методом
математического моделирования. Для этого разработана программа на языке СИ++. При
моделировании заданы следующие параметры модели, ограничений и
дальномера: VxM = 2 м/с; n y max = 0,1; V y max = 1м/с; ϕ max = 45 град.; k = 1000 . Коэффициенты с1 , с2 ,
выбраны с использованием метода модального управления, исходя из обеспечения равных и
действительных корней замкнутой системы «модель-регулятор» и времени переходного
процесса 1 с. Также использовано препятствие прямоугольной формы шириной 1 метр и общей
длиной 8 метров. Препятствие располагалось несимметрично относительно прямой,
соединяющей конечную и начальную точки маршрута робота.
При моделировании использовалась линеаризованная математическая модель плоского
движения гипотетического четырехколесного робота [2]:
X& = AX + BU , (28)
где X = [ X Y Vx V yψ ω z ]T - вектор состояния; U = [δ1 δ 2 ] - вектор управления; А, В –
соответственно матрицы размером 6×6 и 6×2; X , Y - вариации координаты центра масс робота
в системе координат АХУ, а Vx V y - их первые производные; ψ , ω z - соответственно вариация
угла курса и ее первая производная; δ1 , δ 2 - соответственно вариации углов отклонения
рулевых колес и органа изменения мощности тягового двигателя.
Регулятор был синтезирован методом аналитического конструирования оптимальных
регуляторов в виде:
U = C( X з − X ) , (29)
где X з = [0 YM VxM V yM 0 0]T - вектор программного управления; С – матрица коэффициентов
регулятора размером 2×6.
Результаты моделирования представлены на графике рис. 7, для экономии места показана
только та часть препятствия, которую объезжал робот. Программная траектория на этом
графике показана сплошной линией, траектория движения центра масс робота – пунктирной.
Из результатов моделирования следует, что программная траектория соответствует всем
предъявляемым требованиям, за исключением небольшой ее «просадки» у задней кромки
препятствия, которая составила 0,2 м. Это объясняется выходом препятствия из сектора обзора
дальномера. Для ее исключения необходимо увеличить сектор сканирования луча дальномера,
или ввести дополнительные лазерные дальномеры с неподвижными лучами, направленными
перпендикулярно к продольной оси робота. Отклонение траектории движения центра масс
робота от программной траектории несущественно и не превышает 0,15 м.
Рис. 7. Результаты численного моделирования объезда препятствия
11
Труды конференции "Технические и программные - 000808 - Москва, октябрь 2010
средства систем управления, контроля и измерения"ЗАКЛЮЧЕНИЕ
Предложенный подход и алгоритм формирования программной траектории позволяют
эффективно управлять движением робота в соответствии с выбранным критерием качества.
Несмотря на кажущуюся сложность, алгоритм управления может быть легко реализован в
бортовом компьютере мобильного робота и использоваться в качестве основной части общего
алгоритма при создании системы управления. При этом предложенный алгоритм
формирования программной траектории может послужить в качестве базового при создании
алгоритмов, позволяющих мобильному роботу проезжать между препятствиями местности,
объезжать группу препятствий, а также может быть положен в основу гибкой системы
управления движением робота.
ЛИТЕРАТУРА
1. Беллман Р. Динамическое программирование. М.: ИЛ, 1960г., 400 С.
2. Буданов В.М., Девянин Е.А. О движении колесных роботов // ПММ. 2003 г., Т.67. вып.2. – С. 244-
255..
3. Лернер А.Я, Розенман Е.А. Оптимальное управление. М.: Энергия, 1970 г., 360 С.
4. Пшеничный Б.Н., Данилин Ю.М. Численные методы в экстремальных задачах. М.: Наука , 1975 г.,
319 С.
12
Труды конференции "Технические и программные - 000809 - Москва, октябрь 2010
средства систем управления, контроля и измерения"Вы также можете почитать
