Робот с большими картонными колёсами

3. Симонова С.Н. О многолинейной системе с потерями с входящим полумарковским потоком требований // Кибернетика, 1967. № 6. С. 48-53.

4. Сорокин А.С. Математическое моделирование оценки надежности технологических систем // Вестник Кузбасского государственного технического университета, 2008. № 5. С. 28 – 37.

Ларкин Евгений Васильевич, д-р техн. наук, профессор, заведующий кафедрой, elarkin a mail.ni, Россия, Тула, Тульский государственный университет,

Антонов Максим Александрович, аспирант, max0594@,yandex.ru, Россия, Тула, Тульский государственный университет

DISPATCH MODELS E.V. Larkin, M.A. Antonov

The necessity of modeling a command generator for a mobile robot is shown. A characteristic feature of the proposed concept is an interactive mode of interaction between the operator and technical means, which allows the use of simulation results, the results in the simulation of 2 parallel processes.

Key words: command generator, mobile robot, scheduling, 2 parallel process.

Larkin Eugene Vasilevich, doctor of technical science, professor, elarkin@,mail. ru, Russia, Tula, Tula State University,

Antonov Maxim Aleksandrovich, postgraduate, [email protected], Russia, Tula, Tula State University

УДК 629.7.06(082)

ДВИЖЕНИЕ ТРЕХКОЛЕСНОГО МОБИЛЬНОГО РОБОТА

О.А. Лучанский, Т.А. Акименко

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

Ключевые слова: мобильный колесный робот, динамические характеристики, платформа, системы технического зрения.

Мобильные колесные роботы (МКР) применяются для сбора информации в различных областях человеческой деятельности: для предотвращения чрезвычайных ситуаций, экологического мониторинга, разведки полезных ископаемых, в военной сфере и т.п. [3, 4, 5, 6, 7]. Для сбора информации об окружающем пространстве МКР оснащают системами технического зрения (СТЗ), которая, как правило, закреплена на мобильном

133

основании и перемещается вместе с МКР по твердой поверхности. При движении мобильного робота по пересеченной местности СТЗ подвергается различного рода механическим воздействиям, что оказывает существенное влияние на качество изображений [7, 8]. Следовательно, необходимо оценить колебания платформы МКР. Для примера рассмотрим расчет колебаний платформы мобильного трехколесного робота.

Общая кинематическая схема для расчета колебаний платформы трехколесного робота приведена на рис.1, где показаны: точки А, В, С -точки подвеса платформы на опорах; Ял, Яв, Яс – реакции опор А, В, С, соответственно; Mg – вес платформы с установленной СТЗ; сл, св, сс – жесткости пружин подвесок А, В, С; сшл, сшв, сшс – жесткости прокладок (шин) опор А, В, С; щ, Цв, Цс – коэффициенты вязкого трения подвесок А, В, С; ПшЛ, г/шв, ПшС – коэффициенты вязкого трения в шинах А, В, С; тл, тв, тс -приведенные массы опор А, В, С.

Следует отметить, что в трехколесном МКР колеса А, В и С движутся каждое по своей индивидуальной траектории, причем, все траектории параллельны. Поэтому можно считать, что на подрессоренную платформу оказывается три различных механических воздействия, приложенных по углам треугольника АВС [1, 2].

В трехколесном МКР, приведенном на рис. 1, имеется шесть степеней свободы. К ним относятся: вертикальное линейное перемещение платформы г; продольное вращение по углу места поперечное вращение по углу крена у; линейные перемещения приведенных масс тл, тв, тс опор робота.

Рис. 1. Кинематическая схема трехколесного МКР

Колебания в МКР возникают вследствие воздействий со стороны дороги, которые передаются через подрессоренные опоры на платформу.

Положение центра масс относительно точек опоры платформы в пространстве описывается следующим уравнением:

хо

х А

хВ хС

УО У А Ув УС

гО гЛ

гв

гС

1

1

0.

(1)

где хо, уо, го – координаты центра масс платформы; (ха, УА, да), (хв, ув, гв), (хс, Ус, ¿с) – координаты точек А, В, С.

С учетом того, что хл = Ьл; Ул = 0, хв = хс = – Ьвс, Ув = – Ус = Ь, (1) будет иметь вид

хо Уо гО

Ьл 0 гЛ

. Т ^вс Ь гв

Ь ^вс -Ь

1

0.

(2)

Вследствие того, что углы отклонения г и улежат в незначительных пределах, – 15о < &< 15о и – 15о < у< 15о можно считать координаты (хл, ул), (хв, ув), (хс, ус) точек А, в, с постоянными. Это позволяет существенно упростить выкладки при определении местоположения центра масс ¿о, а также углов в и у.

Совместное решение (2) и уравнения х = -Ьвс дает следующие значения угла у и координаты ¿вс, лежащей на пересечении плоскостей у = 0 и

АвС.

¿В ¿с

гвс

у = агС^

2

¿в

¿с

2 Ь

(3)

(4)

Совместное решение (2) и уравнения у = 0, с учетом (3), дает сле дующее значение местоположения центра масс го и угла г.

2Ьвсгл ЬЛ (гв гс).

го

г = агС£

2( Ьл Ьвс )

2гЛ – (гв гс) 2(Ьл Ьвс)

С учетом малости углов г и у, окончательно получим

гв гс.

у:

2 Ь 2Ь 135

(5)

(6)

(7)

1

1

1

& = — —^—^-. (8)

LA LBC 2(LA LBC ) 2(LA LBC )

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

В состав системы дифференциальных уравнений входят:

уравнение сил относительно центра масс платформы, в котором учтены начальные поджатия рессор, уравновешивающих силу тяжести –

MZ O = tfoF ZO СА (ZoA – ZA ) CB (ZoB – ZB ) CC (ZoC – ZC ) (9)

Па (Z oA – ZA ) Пв (Z oB – ZB ) Пс (Z oC – ZC X V ‘

уравнение сил относительно центров масс опор A, B, C –

mAZоА = (ZА – ZoA )Па (НА – ZоА П шА (ZА – ZоА )cA (НА – ZоА )СшА ;

mBZoB = (ZB – ZоВ )Пв (НВ – ZоВ )ПшВ (ZB – ZоВ )CB (hB – ZоВ )СшВ ; (10) mCZ оС = (ZC – ZоС )Пс (НС – ZоС )ПшС (ZC – ZоС )cC (hC – ZоС )СшС

уравнение моментов при вращении МКР относительно центра масс по углу места

(ZoA – Z А ) С А (ZoA – Z А )]А – (11)

– ПВ (ZoB – ZB ) Пс (Z oC – Z С ) СВ (ZoB – ZB ) СС (ZoC – ZC )LBC

уравнение моментов при вращении МКР относительно центра масс по углу крена

JX7 nOMf = [nB (ZoB – ZB ) св (ZoB – ZB )-Пс (Z oC – ZC ) – СС (ZoC – ZC )L ,(12)

где tfoF – коэффициент вязкого трения платформы при поступательном движении; Цом – коэффициент вязкого трения платформы при вращательном движении; Jx, Jy – моменты инерции платформы относительно осей х и у, соответственно; На, Нв, hc – величины микронеровностей дороги под колесами А, В, С, соответственно; ZoA, ZoB, Zoe – координаты центров колес.

Система дифференциальных уравнений, описывающих движение платформы трехколесного мобильного робота в пространстве получается при совместном решение алгебраических уравнений (5), (7), (8) и дифференциальных уравнений (9)-(12).

Решение (5), (7), (8) относительно za, zb, zc имеет вид

Z А = zO ; zb = zo -®lBC Yl ; (13)

zc = z0-®lbc -yl.

Дифференцирование (13) по времени дает

Z А = zo ®lA ;

zb = z0 -®lbc YL; (14)

zc = zo-®lbc -YL.

Подстановка (13) и (14) в (9), (10), (11), (12) дает

[м1о (пж Пл Пв Пс ) ¿о (СЛ Св Сс ) ¿о ]

СС-оС;

СсЬвс )г

(-ПлгоЛ – СЛ-оЛ ) (-ПвгоВ – СВ-оВ ) (-ПсгоС

МЛЬЛ -ПвЬ вс -ПсЬ вс )г (слЬл _ [(Пв -Пс)Ь7 (Св – Сс)ЬГ] = 0;

(-Плго – СЛго ) [тоЛгоЛ (Пл ПшЛ )-оЛ (С Л СшЛ ) ¿оЛ ]

(-ПлЬЛг-СЛЬЛг) = (ПшЛИЛ СшЛИЛ ); (-Пв-о – Св-о ) [тов-ов (Пв Пшв )-оВ (СВ СшВ )-оВ ] (-ПвЬвсг- СвЬвсг) (-ЛвЬ7- СвЬГ) = (ПшвИв СшВИВ); (-Пс-о – Сс-о ) [тос-ос (Пс ПшС ) —оС (сС СшС )-оС ] (-ПсЬВСг-ССЬВС&) (ЯсЬ7 ССЬ7) = (ПшСИС СшСИС );

МАЬЛ – ПВЬВС – ПСЬВС )-о (сАЬЛ – СВЬВС – ССЬВС )-о ] (-ЛаЬЛ-оЛ – СЛЬЛгоЛ ) (1вЬВС-оВ СВЬВСгоВ ) ^ПсЬВС-оС ССЬВС2(

мг (плЬЛ пвЬВс псЬВс г (СлЬл СвЬе

с СсЬвс

[(-Пв Пс ) ЬЬвс 7 (-Св Сс ) ЬЬвс 7] = 0; [(Пв – Пс )Ь-о (сВ – Сс )Ь-о ] (-ПвЬ-оВ – СВЬ-оВ )

(15) )

(ПсЬ-оС СсЬ-оС ) [(-Пв Пс ) ЬЬвсг (-Св Сс УхГ (Пом ПвЬ2 ПсЬ2)Г (СвЬ2 СсЬ2)у Применяя к (15) преобразование Фурье

1 ~

I (®) = .— Ц )ехр(- ), л/2п

ЬЬвс = 0.

г

(16)

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

й( з®) ■

-о ( з®) ” Г 0

-оЛ (з®) ИА ( з®)

-оВ (з®) Ив (з®)

-оС (з®) Ис (з®)

з®) 0

г(з®) У V 0

(17)

Перечисленные параметры могут быть представлены в операторной форме следующим образом:

-О (з®) =

ДгА (з®)

К(з®)

Ав(з®)

К(з®)

Ас (з®)

Аз®) Аз®) Аз®)

(За)ЬА( (За)ЬА( ^с((з а)

К(з®) =

за)

Да (з®)

к(за)

Дгв (з®)

Ив(за)

Дгс (за)

Ис (за)

Д(з’а) Аз®) Д(з’а)

= ( з®)Иа (з ®) ^гв (з®)Ив (з ® ^гс (з ®)Ис (з ®);

К з®> =

(з®)

И А (з®)

а(з”®)

Ив (з®)

(з’®)

Ис (з®) =

А( з®) А( з®) А( з®)

= жгл ( з’®)иа (з ®) (]о)Ив (з ®) ^с (з®)Ис (з®),

Структурная схема [4, 5, 7, 8] трехколесного МКР приведена на рис. 2.

Рис. 2. Структурная схема движения трехколесного МКР

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

Характеристическое уравнение передаточных функций имеет 18-й порядок. Некоторые пары корней характеристического уравнения является комплексно-сопряженными. На частотах, соответствующих комплексно сопряженным корням, при недостаточном демпфировании системы при движении МКР наблюдается явление резонанса. Однако в общем случае порядок числителей передаточных функций меньше порядка знаменателей, вследствие чего амплитуда собственных колебаний платформы МКР затухает с ростом частоты колебаний.

Список литературы

1. Акименко Т.А., Лучанский О.А. Модели механического воздействия на транспортируемую аппаратуру // Системы управления электротехническими объектами. Сб. трудов 4-1 Всероссийской научно-технической конференции СУЭТО-4. Тула: ТулГУ, 2007. С. 27 – 30.

138

2. Акименко Т.А., Лучанский О.А. Продольное движение подвижного наземного объекта с колесными движителями // Приборы и управление. Вып. 5. Тула: Изд-во ТулГУ. C. 6 – 11.

3. Бурдаков С.Ф., Стельмаков Р.Э., Мирошкин И.В. Системы управления движением колесных роботов. С.-Пб: Наука, 2001. 227 с.

4. Ерофеенко В.Г., Козловская И.С. Основы математического моделирования. Минск: БГУ, 2002. 195 с.

5. Краснов М.П., Киселев А.И., Макаренко Г.И. Функции комплексного переменного. Операционное исчисление. Теория устойчивости. М.: Наука, 1971. 304 с.

6. Курочкин С.А., Лучанский О.А. Передаточные функции корректирующих устройств в системах с обратными связями // Известия ТулГУ. Сер. Вычислительная техника. Информационные технологии. Системы управления. Системы управления. Тула: ТулГУ, 2006. Т. 1. Вып. 3. С. 258 -262.

7. Ларкин Е.В., Акименко Т.А., Лучанский О.А. Моделирование движения автономных колесных транспортных средств: монография. Тула: Изд-во ТулГУ, 2023. 160 с.

8. Системы технического зрения мобильных колесных роботов : диссертация … кандидата технических наук: 05.11.16 [Электронный ресурс] URL: http://www.dissforall.eom/_catalog/t8/_science/5/101418.html (дата обращения: 10.01.2023).

Лучанский Олег Алексеевич, канд. техн. наук, tа[email protected], Россия, Министерство обороны Российской Федерации,

Акименко Татьяна Алексеевна, канд. техн. наук, доцент, tаntan72@,mail.ru, Россия, Тула, Тульский государственный университет

MOVEMENT OF THE THREE-WHEEL MOBILE ROBOT О.А. Luchansky T.A. Akimenko

The issues of modeling the movement of a three-wheeled robot are considered. A general kinematic diagram of a three-wheeled mobile robot and the dynamics of a mobile wheeled robot with damping and damping are presented.

Key words: mobile wheeled robot, dynamic characteristics, platform, vision

systems.

Luchansky Oleg Alekseevich, candidate of technical sciences, [email protected], Russia. Ministry of Defense of the Russian Federation,

Akimenko Tatiana Alekseevna, candidate of technical sciences, docent, [email protected], Russia, Tula, Tula State University

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

УДК 621.865.8.001.5

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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)

где Т — матрица 3×3, задающая положение и ориентацию системы координат О’Х’У’ относительно ОХУ и заданная соотношением (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 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.