CC BY
213
ш
■-fiw (5)
где x.y - текущие координаты центра колесной оси
робота; К - конструктивный коэффициент передачи привода; а - угол поворота робота в плоскости; V - линейная скорость движения робота; VL,VR -продольные скорости левого и правого колес ро&ота; up - угловая скорость робота; т! - масса
платформы робота; т - общая масса робота; J -момент инерции робота относительно вертикальной оси; р - коэффициент вязкого трения; г - радиус
ведущего колеса, I - половина расстояния между ведущих колес робота,
Информационная система навигации робота состоит из трехосевого магнитометра и акселерометра а также из двух оптических датчиков вращения ведущих колес, каждый из которых выдает 720 импульсов на один оборот колеса.
Предположим, что системе управления известны текущие координаты робота (это могут быть оптические или другие датчики контрольных точек цеха). Определена траектория движения (кривая S на рис Л). Задача - разработать алгоритм управления двигателями, обеспечивающий минимальные ошибки линейного (е) и углового (а - 0) перемещений робота.
После начала движения робота по приращению текущих координат определяется угол азимута. По из вестным в теори и нав и га ци и формулам из показаний акселерометра (3 оси) находятся крен и тангаж по формулам:
Roll - Atfm(Ac.Y, Ac.Z); (6)
Pit = Aeii}{Ac,X j sqrt{Ac.X1 4- AcY1 + Ac.Z2}) (7)
По показаниям магнитометра находится курс по формуле (сначала показания преобразовываются с системы координат Л А в координату NED CNorth-East-Dovvn) - так называемая компенсация наклона): Xh = Mag.Y * Cos(PitH Mag.X * Sin(Roll) * Sin(Pit) (8) Yh = Mag.X * Cos{Roll)-f- Mag.Z * Sin(Roli); (9) a() = Atan(-Yh, Xh): (10)
Для снижения дисперсии угла азимута необходимо использовать ИЧ-фильтр с постоянной времени Т. Динамическое запаздывание, обусловленное фильтрацией, может быть скомпенсировано за счет определения приращения угла азимута по разности приращений за период опроса показаний
левого Д L и правого ДR импульсных датчиков,
установленных на соответствующих колесах робота, разнесенных на расстояние В.
Д L — AR ~ atч
% =<*, --—^—+—-—- 0 0
^ В Т
Учитывая, что робот движется по плоскости,
выражения 6 + 9 могут быть упрошены за счет
отсутствия в них показаний оси 2 .
Приступим собственно к разработке алгоритма управления движения роботом. На основании показаний модуля цифрового компаса ЬБМЗОЗОЬН можно упрощенно определить текущее значение азимута. Система управления верхнего уровня, в свою очередь, передает контроллеру заданное значение, определенное исходя из текущего положения робота и требуемого направления движения. В результате сравнения двух показаний осуществляется поворот робота до достижения заданного направления движения. Поворот осуществляется при полностью остановленном одном колесе и 0,Й задания на другом. Это
обеспечивает поворот робота вокруг своей оси за 8 секунд. Алгоритм начальной ориентации представлен на рис, 2.
После того, как робот сорнентиро&ан на требуемое направление движения начинается движение к заданной точке. Для возможности оперативного маневрирования начальные скорости
колес робота составляют 0.8 К.
Существуют две причини, вызывающее необходимость коррекции азимута направления движения робота: отклонение от заданного курса за счет разности скоростей левого и правого колес, а также изменение заданного курса системой управления верхнего уровня. Рассмотрим алгоритм управления роботом в первом случае.
Первый из возможных вариантов ухода робота с прямолинейной траектории ори равных заданиях скорости вращения колес заключается в неравенстве коэффициентов механической передачи. Так как это не является запланированным параметром, то учесть это можно путем введения в уравнение движения поправочного коэффициента кк . Определить знгчг-
ние коэффициента можно по экспериментальной траектории прямолинейного движения. Для учебной платформы МП-2301 Л, используемой в эксперименте, левый уклон на двух метрах движения составил 10 см Значение поправочного коэффициента можно определить либо по геометрическим данным получаемой круговой траектории, либо по показаниям импульсных колесных датчиков правого и левого колес от пуска до остановки.
Второй вариант отклонения от прямолинейной траектории - возможность проскальзывания одного из колес. Для устранения последствий такого случая можно разработать алгоритм, аналогичный антипробуксовочной системе автомобиля. Однако гораздо проще в этом случае произвести коррекцию траектории системой управления верхнего уровня, используя разность показаний определения текущей позиции по показаниям колесных датчиков и по показаниям акселерометра.
Рассмотрим алгоритм изменения траектории по сигналу системы управления верхнего уровня
Из выражения (11) можно определить разность линейных скоростей левого и правого колес, обеспечивающую поворот робота со скоростью один градус в секунду В алгоритме можно предусмотреть увеличение разности управляющих сигналов при увеличении величины рассогласования Исходя из уравнений (8 И) и параметров учебной платформы МП-2301 Л определим требуемую пропорциональность задания скоростей. Соответствующий алгоритм управления представлен на рис, 3.
Приведенные алгоритмы предназначены, прежде всего, для микропроцессорной реализации, т.е. могут быть использованы при разработке программного обеспечения микропроцессорной системы управления электроприводами ведущих колес. Для формирования заданий в системе управления верхнего уровня алгоритм может быть сложнее и учитывать все особенности динамических режимов робота, что позволит получить более качественное управление траекторией робота, если это необходимо. В большинстве случаев ДЛЯ транспортных роботов нет необходимости в разработке управляющих программ высокой точности, так как в транспортных операциях не ставится цель отработки заданной траектории
Для уточнения алгоритма управления (определение допустимого диапазона изменения питающего напряжения привода колес и, соответственно этому, постоянного коэффициента в алгоритме управления) реализуем математическую модель робота и определим график изменения угла ориентации робота во времени Структура модели представлена на рис. 4. Моделирование производим со следующими параметрами: начальный азимут ориентации робота равен нулю Величина задания азимута составляет 10 градусов Время действия управляющего сигнала - 4 секунды Результаты моделирования представлены на рис. 5
начало
Определение /
текущею азимута /
т
Получение заданного азимута
ик =
Переход к алгоритму Товорот в лево^> у начального поворота
Рис 3 Алгоритм поворота робота по сигналу сисгемы управления верхнего уровня
О гг ера раї
ры
г:
и*
112
V
Отада
V
VI
>Р51
Рис 4 Структура модели робота
20 181 16114 12 10 б
6!
4
2
О
0 2 4 б 8 10 12 14
Рис 5 Результаты моделирования алгоритма управления поворотом по сигналу задания
Результаты моделирования I (оказали, что для рассогласований порядка 10 градусов, скорость поворота робота составляет 6 градусов в секунду. Таким образом, с учетом линейности алгоритма, максимальная скорость поворота в динамике (но алгоритму 2) составит 24 гр/сск, а скорость разворота (алгоритм начальной ориентации) - 48 гр/сек.
Разумеется, что приведенные здесь результаты справедливы только для точных показаний сигналов магнитометра. Для полного управления роботом необходимо разработать алгоритм определения текущего положения робота с использованием сигналов акселерометра. Последнее возможно только в программном обеспечении системы управления верхнего уровня, так как фсбует серьезных вычислений, связанных с фильтрацией
показаний регистров акселерометра, вычислением текущих значений проекций ускорения и скорости по каждой из координат и хранением вычисленных значения до следующей итерации.
Литература
1. Мартыненко, Ю.Г. Динамика мобильных роботов / Ю.Г.Мартыненко // Соросовский образовательный журнал, - 2000. - №5. - С. 110116. '
2. Девянин К. Л. О движении колёсных роботов / Е.А.Девянин I! Докл. Науч. школы-конференции «Мобильные роботы и мехатронные системы», 7-8 декабря 1999 г. - М,: Институт механики МГУ. 1999. - С. 169-200.
Воронежский государственный технический университет
THE ALGORITHM OF CONTROL BY INDUSTRIAL TRANSPORT ROBOT ELECTRIC DRIVE
Ju.S. Slepokurov, K.A. Razinkm, E.N. Korovin
The problem of wheeled mobile robots motion control on the base of electronic magnetometer signal is considered The mathematical model of the three-wheeled mobile robot with two independent driving wheels is presented. The algorithm of the robots motion control on the base of electronic compass signal is offered
Key words; transport robot, mathematical model, algorithm of control
