Система управления мобильного колесного робота Текст научной статьи по специальности «Математика»

МЕХАТРОНИКА И РОБОТОТЕХНИКА j

УДК 621.865.8.001.5

С. Л. Зенкевич, А. В. Назарова

СИСТЕМА УПРАВЛЕНИЯ МОБИЛЬНОГО КОЛЕСНОГО РОБОТА

Рассмотрены вопросы управления автономным мобильным колесным роботом. Приведены кинематическая и динамическая модели робота. Динамическая модель получена с использованием уравнений Аппеля. Для определения декартовых координат объектов внешней среды использованы методы рекуррентной фильтрации, основанные на непрерывной модели объекта и дискретных измерениях, полученных системой зрения. Приведены алгоритмы управления и некоторые результаты моделирования.

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

В настоящей работе рассмотрен самый простой представитель мобильных роботов, а именно робот, имеющий два ведущих и одно ведомое (пассивное) колесо и движущийся по плоскому рельефу. Роботы именно такой конструкции участвуют в традиционных соревнованиях, ежегодно проводимых в рамках молодежного фестиваля "Мобильные роботы" в Институте механики МГУ им. М.В. Ломоносова. В общих чертах регламент соревнований выглядит следующим образом [1].

Соревнования проводятся на ограниченной ровной горизонтальной площадке. Над полигоном подвешено 10... 15 маяков, имеющих излучатели и включающихся последовательно или одновременно в зависимости от вида выполняемого задания. Робот должен иметь на борту все необходимые датчики и систему управления движением. Выполняемые роботами задания состоят в следующем.

1. "Маяки-ворота''. В момент старта включен один маяк. Задача робота — двигаться так, чтобы войти в контакт с маяком. Маяк выключается и включается либо один маяк, либо одновременно два маяка. В последнем случае задача робота — пройти между маяками в образуемые ими ворота, после чего процедура повторяется.

2. "Восьмерка". То же упражнение, но маяки, образующие ворота, не выключаются после их прохождения, и тогда робот движется по "восьмерке", объезжая вокруг маяков.

3. "Змейка". В момент старта и в процессе заезда постоянно включено несколько маяков, образующих последовательность из ворот. Роботу необходимо двигаться так, чтобы пройти "змейку", не задев маяки.

4. "Куча". В момент старта включено больше двух маяков; робот должен двигаться, чтобы войти в контакт с каким-либо из них, после чего маяк выключается. Требуется погасить все маяки. Выбор последовательности объезда маяков произвольный1.

Критерием оценки всех заданий является затраченное время.

Выполнение перечисленных задач, несмотря на их кажущуюся простоту, требует решения широкого круга задач, являющихся базовыми для мобильных роботов вообще [2-5]. Отметим среди них следующие:

— получение информации о внешней среде, поступающей с сенсорной системы, и преобразование ее в форму, удобную для использования в целях управления;

— планирование движения робота в динамически меняющейся среде;

— управление роботом, реализующее сформированный план.

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

Кинематическая модель мобильного робота. Поскольку робот движется в горизонтальной плоскости, то достаточно рассмотреть плоский случай. На рис. 1 изображена соответствующая система координат. Оси вращения колеса совпадают с отрезком ЬЯ пути. Свяжем с роботом систему координат О'Х'У', расположив О' посередине ЬЯ и направив ось X' перпендикулярно ЬЯ в направлении движения. Тогда положение основания робота как твердого тела задается тройкой чисел х, у, где р — угол между осями X и X', х, у — координаты точки О' в абсолютной системе координат ОХУ.

Введем однородную матрицу 3x3 перехода от О'Х'У' к ОХУ:

1 Следует заметить, что регламент соревнований постоянно усложняется. Так со времени написания статьи были введены дополнительно езда на подвижный маяк, формирование карты активных маяков и т.д.

ния (СТЗ).

(1)

где

у х' V /

/\ч> vr

Г\Г хуг

/ / ¿Л. 1 1 1 1 1 1

О у X

Рис. 1. Схема движения мобильного робота в горизонтальной плоскости

В выражении (1) Я и p представляют собой соответственно матрицу ориентации и вектор положения начала системы координат О' в абсолютной системе координат.

Если обозначить векторы линейных скоростей точек О', Ь и Я как V, V;, уг соответственно, а через ш — угловую скорость, то можно записать следующие соотношения:

VI = V + шПг;;

(2)

Vr = V + шПгг,

где матрица П имеет вид

/ 0 -1

П =

VI 0

Нетрудно заметить, что П является ортогональной матрицей, обеспечивающей поворот произвольного вектора на п/2. Запишем теперь следующие очевидные соотношения:

1

Г1 = 2 У ; 1 ,

rr = - 2У;

Vi = vix';

(3)

Vr = Уг х',

где х' и у' — единичные орты системы координат О'Х'У', а V, ьг — проекции соответствующих векторов на ось Х'. Вектор линейной скорости точки О' в силу соотношений (3) совпадает с осью Х'. Вос-

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

v = 1(vr + v);

1 (4)

Ш = 1 (vr - Vi).

Если теперь ввести в рассмотрение координаты x, y, ф, нетрудно видеть, что

X = v cos ф;

y = v sin ф; (5)

ф = Ш.

Подставив выражения (4) в соотношения (5), получим

X = 2(vr + vi) cos ф;

1

2' 1

< y = 7> (^г + v)sin<p; (6)

Ф = l (vr - Vi).

Соотношения (6) представляют собой систему нелинейных дифференциальных уравнений 3-го порядка относительно фазового вектора с компонентами (ж, y, Скаляры vl и vr, заданные тем или иным образом (как функции времени или как функции фазовых координат), определяют движение платформы. Таким образом, соотношения (6) можно рассматривать как кинематическую модель мобильного трехколесного робота. Линейная и угловая скорости робота определяются из соотношений (4) .

Динамическая модель. Прежде чем перейти к выводу уравнений движения, рассмотрим более детально связи, наложенные на робот.

Анализ связей. Мобильный робот перемещается по плоскому рельефу, имея два ведущих колеса радиуса р. Колеса вращаются без проскальзывания, и оси их вращения совпадают с линией LR. Учет последнего условия обеспечивается выполнением очевидного соотношения Ж sin p — y cos p = 0, полученного из выражения (5).

Это соотношение устанавливает ограничения на координаты и скорости и представляет собой одно из уравнений связи, геометрическая интерпретация которого состоит в том, что вектор скорости v = (Ж, у)т перпендикулярен орту y'. Таким образом, налагаемая связь является неголономной.

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

Условия непроскальзывания получим, воспользовавшись соотношением (6), а также тем обстоятельством, что

уг = р • фг;

V = р • ф. Тогда уравнения связей имеют вид

Р

X —

2

Р

У —

Р

ф — l

из которых последнее интегрируется как р = р (дг — д) + С где С — некоторая постоянная.

Окончательно уравнения связи имеют вид

(7)

Таким образом, рассмотренная система имеет 5 — 1=4 обобщенные координаты, например (х, у, ф,дг), и 5 — 3 = 2 степени свободы.

Уравнения движения. Для вывода уравнений движения воспользуемся уравнением Аппеля в виде

Р

x—

2

Р

У—

р

Ф— i

dS = Q • -то

о — Qi, г — 2,

dqi

,n — r,

где п — число обобщенных координат; г — число неголономных уравнений связей; Б — энергия ускорения; д — обобщенные координаты; ^ — обобщенные силы.

Пусть М — произвольная точка массы т„, а ги и р^ — однородные векторы, характеризующие ее положение в абсолютной и связанной системах координат (рис. 2). Тогда имеет место соотношение

^ = Три, (8)

где Т — матрица 3x3, задающая положение и ориентацию системы координат О'Х'У' относительно ОХУ и заданная соотношением (1).

r

r

r

r

r

r

Известно, что энергия ускорения Б твердого тела выражается следующим образом:

1

S =

2

mv rv г^.

(9)

Продифференцировав дважды уравнение (8), запишем

г* = Трр

и подставляя в выражение (9), после несложных преобразова-

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

в горизонтальном плоскости

где Н — матрица инерции, имеющая вид

/ i

H = £

= У , mv Pv Pv =

xx ±rxy ^x

TT S

Tyx Tyy Sy

S = 2tr(fHfr), (10) 2

Sx

\ Бх Б у т )

где 1им = ^^ типи— моменты 2-го порядка (и, V Е (ж, у}); Би = ^^ типи — моменты 1-го порядка (и Е (ж, у}).

Связь между моментами 1ии и моментами 2-го порядка вокруг осей 1и имеет вид:

!хх + % + ^);

1

2 1

2

Tyy о (ix «y + )j

Сделаем теперь следующие предположения относительно конструктивных особенностей робота:

1. Моменты инерции колес значительно меньше соответствующих моментов платформы;

2. Центр массы платформы лежит на оси X' в точке ОСы с координатами (—Ь, 0) (см. рис. 1);

3. Матрица инерции платформы в системе координат ОсмX''У'',

связанной с центром масс, имеет вид

H" =

( Irxx 0

V 0

0

Lyy

0

0 0

m )

где т — масса платформы.

Эти предположения кажутся естественными, имея в виду симметрию платформы относительно продольной оси. С другой стороны, их невыполнение не является существенным, но только усложняет процедуру вывода.

Пусть теперь Н' — матрица инерции платформы в связанной системе координат О'Х'У'2', а А — матрица перехода от О'Х'У' к ОсиХ"У". Тогда, используя известное соотношение, связывающее значения матрицы инерции в различных системах координат [7]

Н' = АН ''Ат, а также то, что матрица перехода А имеет вид

A =

/ 1 0 -Ь\ 0 1 0 \0 0 1 )

получим

H' =

' 1 iz 0 -mb 2

1

V

0 2Ь

—mb 0

m

(11)

/

Учитывая соотношения (10) и (11), после преобразований получим выражение для энергии ускорения:

S = 1 [(ф4 + ф2)г2 + m(X2 + y2)] +

+ bm [Х(ф sin ф + ф2 cos ф) + у(ф2 sin ф — ф cos ф)] .

Выберем из обобщенных координат x,y,ql,qr две координаты ql,qr, а для нахождения скоростей и ускорений X, y, X, y воспользуемся уравнениями связи (7) . После соответствующих преобразований получим

S = 1

2

(ф4 + ф2)гг + mp (qr + qi)

+ РФ2(<ír + qi) - £ФФ(г + qi)

2

В соответствии с уравнением Аппеля имеем

^ = О.

гл Ц;г)

Щт

7Г = О''

В предположении, что обобщенными силами О т, О' являются моменты тт и Т', развиваемые моторами, которые приводят в движение колеса, после преобразования получим

шь = -Ьшш2 +—(т' + тт);

Р

iz ш = bmwv + — (тг — т ). ¿p

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

В целях моделирования удобной формой представления является следующая:

x = v cos р;

y = v sin р; р =

V = -bu2 + —(r¡ + Tr); pm

bmuV l

(12)

u =

+ w,(Tr - Tl)'

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

Известно, что уравнение двигателя постоянного тока имеет вид

LI + RI + Kw u.m = U,

(13)

где Ь — индуктивность обмотки якоря; I — ток якоря; Я — сопротивление обмотки якоря; шгп — угловая скорость вращения вала двигателя; и — напряжение, подаваемое на обмотку якоря.

Угловые скорости вращения вала шгп и колеса связаны так:

um uw

где i — передаточное отношение редуктора.

Выражение для момента т, развиваемого двигателем, имеет вид

т = Кш1. (14)

Учитывая соотношение (14), представим уравнение (13) в виде

R

Т = - LT -

К К i ~L

+ Км тт Uw + —т~ U

L

или окончательно для правого и левого колес

Ti =

R L

R

Ti

К К i

L

К К i

+ Км тт U + —Ui;

тг LTr

+ Км тт

L Ur + —Ur.

(15)

(16)

Заметим, что в соотношении (15) мы предполагаем, что параметры правого и левого моторов одинаковы (что, вообще говоря, не обязательно). Выразим теперь щ и шг через фазовые координаты. Воспользовавшись соотношением (4) и учитывая, что v¡ = щр и vr = шгр, получим:

U ш1 Щ = - v + —

р V 2

W шГ Щ = " v - "Т

А 2

Подставив соотношения (16) в выражение (15) и пополняя систему (12), получим следующую систему дифференциальных уравнений, описывающую движение мобильного робота:

x = v cos ф;

y = v sin ф;

ф = щ;

v = -Ьш2 +--(т + Tr);

pm

bmwv l ш = —--+ (Tr - t);

iz 2piz

Ti =

R

L

R

Ti

2pi К К i

L

К К i

Ul \ , Кмп

Tr l Tr

L

ul

Км

v + -fUr.

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

Нахождение координат объектов. Одна из проблем, возникающих при решении задачи навигации мобильного робота, состоит в

Y

О

X2 X(4 Xj

X

Рис. 3. К методу оценки декартовых координат

определении декартовых координат объектов внешней среды на основе некоторых измерений, при этом используются датчики различного типа. Рассмотрим следующую задачу: по измерениям, производимым телевизионной системой, требуется определить координаты маяков, попавших в поле зрения ГГ-камеры. Эта задача является весьма важной для планирования движения робота. В основе ее решения лежит простой метод триангуляции, состоящий в следующем. Если известны координаты двух точек Я1 и Я2 (рис. 3), в которые помещены измерители, а также углы а и а2 на некоторый объект М, то нахождение координат последнего является простой тригонометрической задачей. Она сводится к нахождению координат точки пересечения прямых 1\ и 12, проведенных через точки и Я2 под углами а и а2.

Специфика применения этого подхода для решения задачи навигации состоит в следующем:

1. ГГ-камера, установленная на роботе, — один измеритель.

2. Измеритель перемещается в плоскости, измеряя углы. Результаты являются дискретными, поскольку роботу требуется конечное время на обработку видеоинформации, поступающей с СТЗ.

Поэтому использование рекуррентных методов фильтрации случайных процессов представляется весьма целесообразным.

Фильтр Калмана для линейного непрерывного объекта с дискретными измерениями. Для удобства приведем известные соотношения для рекуррентного оценивания фазового вектора непрерывной линейной системы по дискретным измерениям.

Пусть система описывается следующим образом:

x = A(t)x + f (t);

z k = Hk Xk + .

(17)

Уравнения (17) описывают поведение объекта и измерения. Здесь х — фазовый вектор; f (£) — известная функция времени; хк = х(£к) — значение фазового вектора в момент £ = £к; хк — вектор измерений.

Вектор х(£) представляет собой случайный процесс со следующими характеристиками:

Измерения содержат в своем составе шум £к, представляющий собой случайную ^-коррелированную нормально распределенную последовательность со следующими характеристиками:

Известно, что уравнения, описывающие поведение оценки X фазового вектора х, имеют следующий вид: 1. Между измерениями (£к < £ < £к+1)

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

Уравнение объекта и измерений. Построим модель объекта. Воспользуемся кинематической моделью мобильного робота в форме

E (x(to)) = m; E(x(to) ■ xT(to)) = Ro.

E(L) = 0;

E(£k ■ £k) = Rk.

(18)

(19)

(20)

x = v cosp:

у = v sinp:

(21)

p = ш

Рис. 4. Поведение оценок в случае непрерывного объекта с дискретными измерениями

где

v = р (шЕ + шь)\ ш = Р (Ше - шь);

(22)

(23)

х, у — декартовы координаты робота в абсолютной системе; V, ш — линейная и угловая скорости робота соответственно; шд, шь — угловые скорости правого и левого колеса соответственно; р — радиус колеса; I — расстояние между ведущими колесами.

Пусть хм, ум — декартовы координаты маяка. Поскольку маяк неподвижен, то имеют место соотношения

х м 0.

(24)

Ум = 0.

Системы (21) и (24), вообще говоря, представляют собой уравнения объекта. Однако воспользоваться ими весьма затруднительно по двум причинам. Во-первых, модель является нелинейной. Фильтр, оценивающий фазовый вектор нелинейной системы, использует разложение в ряд в окрестности текущей оценки и является весьма чувствительным по отношению к начальным условиям. Во-вторых, размерность системы дифференциальных уравнений, описывающих фильтр, высока и равна 20 (5 — для объекта, 15 — для матрицы ковариаций ошибки оценки). Учитывая, что в ряде заданий, выполняемых мобильным роботом, необходимо оценивать координаты нескольких маяков, становится ясно, что этот подход является бесперспективным.

Воспользуемся тем обстоятельством, что для управления движением мобильного робота, вообще говоря, не обязательно знать абсолютные координаты маяков, но достаточно знать их координаты в связанной системе О'Х'У'.

Пусть однородная матрица перехода от связанной системы координат О'Х'У' к абсолютной системе ОХУ имеет вид матрицы (1).

О X

Рис. 5. Получение уравнений объекта в относительной системе координат

Обозначим через р' = (х' у' 1)т, рм = (хм Ум 1)т — однородные векторы неподвижной точки М в связанной и абсолютной системах координат соответственно (рис. 5), тогда имеем

Рм = т • Р'-

Дифференцируя это выражение по времени и учитывая соттношение (24), получим

р' = -т-1 • Т • р'.

Учитывая, что

а также уравнение объекта в абсолютной системе координат (21), после простых преобразований получим

х' = шу' — V;

(25)

у' = —шх',

где ш и V — угловая и линейная скорости мобильного робота соответственно, заданные соотношениями (22), (23). Таким образом, соотношения (25) представляют собой уравнения объекта, фазовыми координатами которого являются декартовые координаты маяка х' и у' в связанной системе координат. Их эволюция вызывается собственным движением мобильного робота, обусловленным вращением ведущих колес с угловыми скоростями шд и шь. Если предположить, что шд и шь представляют собой программные управления, т.е. шд = шд(£),

= wL(t), а следовательно, и v = v(t), w = w(t), тогда уравнения объекта представляют собой систему линейных нестационарных дифференциальных уравнений второго порядка.

Получим теперь уравнения измерений. Считаем, что мобильный робот измеряет угол а, используя TV-систему, установленную в точке С с координатами x' = 0, y' = —d:

y'

а = arctg^£7 • (26)

x' + d

Однако это неудобно, поскольку уравнение (26) представляет собой нелинейную функцию относительно фазовых координат маяка x' и y'.

Воспользуемся другим подходом, состоящим в следующем. Ясно, что каждое измерение угла а означает, что маяк лежит на прямой, уравнение которой имеет вид

y' = (x' + d)tga

или

d sin а = —x' sin а + y' cos а. (27)

Заметим, что соотношение (27) является прямым следствием уравнения (26). В терминах уравнения измерений это означает, что

z = —x' sin а + y' cos а +

а само измерение формируется как

z = d sin а.

Таким образом, окончательно уравнения объекта и измерений имеют вид

ж' = wy' — v;

y' = — wx';

(28)

Zk = d sin ак = — x'k sin ак + yk cos ак + £k. (29)

Оценивание координат маяка. Будем строить фильтр Калмана в соответствии с соотношениями (18)-(20). Согласно выражениям (28), (29) матрицы А и Н имеют вид

A = ^ _W(t) ^ ) ' Hk = ( — s^k cos ак ) . (30)

Выполняя все необходимые операции, окончательно получим следующие уравнения фильтра: 1. Между измерениями

ж' = wy' — v;

y' = —wx';

(31)

pii = 2шры ;

p12 = ш{'Р22 - Pli) (32)

P22 = -2ШР12-

Начальные условия для систем (31) и (32) ищутся в соответствии с уравнениями (19). Заметим, что в этом случае сумма дисперсий ошибок оценок координат маяка постоянна. 2. В момент измерений

( 'Р++Ик Р+12* \ = Л ( кЛ \ ( Рпк .

V Р+21к Р+22к ) V V ) ) \ Р21к Р22к )

( к1 ) = ( Р11к Р12к ) Щ (щ ( Р11к Р12к ) Щ + а2^-1.

„ \ к2 ) \ Р21к Р22к ) к \ \ Р21к Р22к ) )

(33)

В уравнениях (31)-(33) XX, у' — оценки координат маяка в связанной системе координат; р^ — элементы матрицы ковариации ошибки оценки; гк — измерения, формируемые в соответствии с соотношением (29) (напомним, что в действительности мы считаем, что измеряется угол на маяк); ш, V — программные угловая и линейная скорости робота соответственно; а — среднеквадратическая ошибка в измерениях.

Введем следующие допущения.

1. Матрица измерений Н, представленная отношением (30), является случайной, что, вообще говоря, не соответствует классической модели фильтра Калмана.

2. Качество оценки, определяемое соотношениями (32), (33) (элементами р11 и р22), зависит от собственного движения робота. Это кажется естественным: например, если робот неподвижен, то очевидно, что измерения не улучшают начальную оценку. Однако вопрос выбора движения V = v(t), ш = ш(£), обеспечивающего наиболее быструю сходимость оценок, выходит за рамки настоящей работы.

На рис. 6 показаны результаты моделирования приведенного алгоритма оценивания координат маяков1. Робот совершал вращательное движение с угловой скоростью ш = соэ(3,5£), при этом линейная скорость V = 0. Измерения угла на маяк проводились с частотой 5 Гц. Как видно, в этих условиях время сходимости оценок составляет 3,5. ..4с.

1 Моделирование проводил В.В. Лепилкин

Рис. 6. Результаты моделирования поведения оценок декартовых координат маяка в относительном движении

Управление мобильным роботом. Приведем основные принципы, положенные в основу управления мобильным роботом, в предположении, что оценку декартовых координат маяков в связанной системе координат мы получаем, пользуясь описанными методами рекуррентной фильтрации.

Считаем, что закон управления активными колесами имеет вид

Ши = Шо + шс;

шь = шо - Шс,

где ш0 = const, а шс — выбираемое управление. В этом случае в соответствии с соотношениями (22), (23) имеем

v = ршо; 2 Р

ш = — шс . l

Это означает, что движение робота происходит с постоянной линейной скоростью, а управление осуществляется его угловой скоростью.

Будем считать, что задачей управления шс является следование предварительно заданной программной траектории. Эта траектория формируется в связанной с роботом системе координат в виде

f (x',y')= 0, (34)

при этом потребуем, чтобы она проходила через начало связанной системы координат, т.е.

f (0, 0) = 0. (35)

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

Прежде чем перейти к формированию закона управления, заметим, что при движении робота вдоль любой гладкой траектории ось X' связанной системы координат всегда направлена вдоль касательной к траектории, проведенной в точке О' (начало связанной системы). Обоснованием этого служит то обстоятельство, что, как отмечено в выше, ось X' совпадает с вектором скорости точки О', которая, в свою очередь, совпадает с касательным вектором.

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

ш = кф, (36)

где ф — угол между осью хХ и касательным вектором т, проведенным в точке О' к программной траектории (рис. 7). Программная траектория всегда строится так, чтобы выполнялось соотношение (35). При этом, безусловно, желательно, чтобы угол ф был небольшим. Учитывая соотношение (34), имеем

ф = -/X(0, 0)//У(0, 0). (37)

Здесь предполагается, что /'(0, 0) = 0. Это условие в действительности может нарушаться, о чем речь пойдет ниже.

Соотношения (36), (37) в предположении малых углов ф и определяют закон управления.

Y'A

Рис. 8. Программная траектория при движении на маяк

Рассмотрим два примера задач из числа тех, которые входят в регламент соревнований мобильных роботов [1], о которых шла речь ранее.

1. Движение на маяк. Задание состоит в том, чтобы коснуться активного маяка, используя информацию о его относительном угловом положении, поступающую с бортовой сенсорной системы. Будем считать для простоты, что ГГ-камера установлена в точке О' и поставляет в систему управления угол а на маяк. Будем считать далее, что программной траекторией в каждый момент времени является отрезок прямой О'М, соединяющей начало системы координат робота и маяк (рис. 8). Поскольку касательный вектор к прямой совпадает с ней самой, то ошибка управления ф = а. Такой же результат мы получим и при использовании соотношения (36).

Таким образом, соотношения (36) и (37) приводят к очевидному результату, состоящему в том, что ошибкой управления является отклонение оси робота Х' от направления на маяк, получаемого от системы зрения.

2. Въезд в ворота. Задача состоит в том, чтобы робот въехал в створ ворот, задаваемых двумя активными маяками А и В. Выберем в качестве программной траектории дугу окружности ш:

/(г) = (г - Го)2 - Я2 = 0,

где г = (х',у')т, (г — г0)2 = (г — г0)т(г — г0), Я — радиус окружности.

Окружность ш обладает следующими свойствами (рис. 9):

1) ш проходит через О' (что соответствует соотношению (35));

Рис. 9. Программная траектория при прохождении ворот

2) ш пересекает линию ворот АВ в точке С так, что АС = ВС и АВ перпендикулярна ш.

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

Пусть г о, га, гв — радиус-векторы, проведенные из точки О' в точки О (центр окружности), А, В (створы ворот) и заданные в относительной системе координат. После стандартных преобразований

А = г|)

получим следующие соотношения (при г А = гВ):

Л =

rо = Лга + (1 - Л)гв;

4(га - гВ)2 - rB

(38)

rA - rB

Из соотношения (37), определяющего ошибку управления, и из закона управления (36) следует

tg ф = -Хо/уо,

(39)

где хо, уо — компоненты вектора го. Таким образом, в соответствии с уравнением (39) нам необходимо вычислить отношение координат центра окружности, задаваемого вектором го. Вычисляя этот вектор

1

Рис. 10. Результаты моделирования въезда робота в ворота:

а — координаты ворот известны точно; б — координаты ворот зашумлены

в соответствии с соотношением (38), получим вектор и, пропорциональный г о:

и = (га + г в )т(гА - 3гв )га + (га + гв )(Зга - гв )га,

отношение компонент которого совпадает с отношением компонент (39) вектора г0 и позволяет сформировать закон управления (36):

tg ф = -Хи/уп- (40)

Заметим, что в тех случаях, когда гА = г| (см. рис. 9), т.е. программная траектория представляет собой отрезок прямой (эта ситуация возникает всегда в момент въезда в ворота), мы не можем воспользоваться соотношением (40), поскольку и = 0. Однако нетрудно показать, что при этом

Нт ^ ф = ус/хо,

где ус, хс — декартовы координаты точки С центра ворот, что эквивалентно движению на маяк, расположенный в центре ворот, так что эта проблема носит скорее вычислительный характер.

На рис.10, а, б приведены результаты моделирования этого алгоритма при въезде робота в ворота. В случае, представленном на рис. 10, б, декартовы координаты ворот гА, гв, используемые в соответствии с соотношением (38), зашумлены2.

Выводы. В работе представлены математические модели, а также методы управления мобильным колесным роботом, основанные на

2 Моделирование проводил А. Орлов

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

Авторы благодарят аспирантов и студентов, в разное время входивших в состав команды МГТУ им.Н.Э.Баумана: Д.Киселева, А. Орлова, Н. Федорова, И. Сандлера, В. Лепилкина, Д. Лисицына.

СПИСОК ЛИТЕРАТУРЫ

1. Регламент соревнований мобильных роботов. www.imec.msk.ru/fmrobot, www.robot.ru.

2. Д е в я н и н Е. А. О движении колесных роботов // Труды конф. "Мобильные роботы и мехатронные системы". - М., 1998. - С. 169-200.

3. Кобрин А. И., Мартыненко Ю. Г. Неголономная динамика мобильных роботов и ее моделирование в реальном времени // Труды конф. "Мобильные роботы и мехатронные системы". - М., 1998. - С. 107-123.

4. З е н к е в и ч С. Л., Назарова А. В., Лисицын Д. М. Моделирование движения робота по сложному маршруту // Труды конф. "Мобильные роботы и мехатронные системы". - М., 2000. - С. 14-27.

5. Управление мобильным роботом с использованием фильтра Калмана / С.Л. Зенкевич и др. // Труды конф. "Мобильные роботы и мехатронные системы". - М., 2000. - С. 28-41.

6. А о к и М. Оптимизация стохастических систем. - М.: Наука, 1971.

7. Зенкевич С. Л., Ющенко А. С. Основы управления манипуляционными роботами. - М.: Изд-во МГТУ им. Н.Э. Баумана, 2005.

Статья поступила в редакцию 24.11.2005

Станислав Леонидович Зенкевич родился в 1945 г., окончил в 1969 г. МВТУ им. Н.Э. Баумана и в 1975 г. МГУ им. М.В.Ломоносова. Д-р физ.-мат. наук, профессор кафедры "Робототехнические системы" МГТУ им. Н.Э. Баумана. Автор около 80 работ в области теории управления.

S.L. Zenkevich (b. 1945) graduated from the Bauman Moscow Higher Technical School in 1969 and Moscow State University n.a. M.V. Lomonosov in 1975. D.SC. (Phys.-Math.), professor of "Robotic Systems" department of the Bauman Moscow State Technical University. Author of about 80 publications in the field of control theory.

Анаид Вартановна Назарова окончила МВТУ им. Н.Э. Баумана в 1974 г. и МГУ им. М.В. Ломоносова. Канд. техн. наук, доцент кафедры "Робототехнические системы" МГТУ им. Н.Э. Баумана. Специализируется в области управления и моделирования распределенных систем.

A.V. Nazarova graduated from the Bauman Moscow Technical Scholl in 1974 and the Moscow State University n.a. M.V. Lomonosov. Assoc. professor of the "Robotic systems" department of the BMSTU. Specialised in the the field of control and simulation of distributed systems.