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

УДК 519.7

М. В. Сотникова

Вестник СПбГУ. Сер. 10. 2016. Вып. 1

АЛГОРИТМ АВТОМАТИЧЕСКОГО УДЕРЖАНИЯ КОЛЕСНОГО РОБОТА НА ВИЗУАЛЬНО ЗАДАННОЙ ЛИНИИ

Санкт-Петербургский государственный университет, Россия, 199034, Санкт-Петербург, Университетская наб., 7—9

Рассматривается задача синтеза управления, обеспечивающего автоматическое удержание полноприводного колесного робота на визуально заданной линии. Существо современных подходов к решению этой задачи состоит в формализации понятия отклонения текущего положения робота от заданного пути на основе визуальной информации и поиске такого управления с обратной связью, которое обеспечивает сведение к нулю нормы этого отклонения. Основу предлагаемого подхода к синтезу составляет многоцелевая структура закона управления. Данная структура позволяет обеспечить многоцелевую ориентацию системы, учитывающую весь комплекс требований для достижения желаемой динамики в конкретных вариантах функционирования объекта. Многоцелевая структура включает в себя асимптотический наблюдатель и динамический корректор, которые в рамках описанной задачи служат для оценки неизмеряемых компонент вектора скорости и обеспечения астатизма замкнутой системы по отношению к постоянным или медленно меняющимся внешним возмущениям. Для иллюстрации полученных результатов выполняется имитационное моделирование алгоритмов управления в среде MATLAB. Библиогр. 7 назв. Ил. 3.

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

M. V. Sotnikova

ALGORITHM FOR VISUAL PATH FOLLOWING BY WHEELED FULLY ACTUATED MOBILE ROBOT

St. Petersburg State University, 7—9, Universitetskaya nab., St. Petersburg, 199034, Russia

The paper is devoted to control algorithms design for autonomous mobile robots with the use of visual information in the feedback loop. The problem of visual path following by a fully actuated wheeled mobile robot is considered. The essence of the used approach is to minimize the error between desired and actual position of the given path in the image plane of the camera. The basic idea of synthesis is that the observed path in the image plane must pass through the center point of the bottom of the screen and the direction of the tangent to this path must coincide with the direction of the velocity vector of the robot. These requirements should be satisfied after homographic mapping from camera image plane to the horizontal plane, which is parallel to the plane of robot motion. The proposed approach makes use of a multipurpose control structure that provides an estimate for unmeasured component of the velocity vector and gives a compensation for constant or slowly varying disturbances. Refs 7. Figs 3.

Keywords: visual path, camera, multipurpose structure, wheel robot.

1. Введение. В настоящее время все более широкое распространение получают автономные подвижные объекты, способные решать практически важные задачи без участия человека. К ним относятся автономные подводные и надводные подвижные

Сотникова Маргарита Викторовна — кандидат физико-математических наук, доцент; [email protected]

Sotnikova Margarita Viktorovna — candidate of physical and mathematical sciences, associate professor; [email protected]

© Санкт-Петербургский государственный университет, 2016

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

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

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

В п. 2 и 3 рассматривается задача синтеза управления, обеспечивающего автоматическое удержание полноприводного колесного робота на визуально заданной линии. В частности, на практике подобная линия может обозначать контур дороги, трубопровод и т. д. Современные подходы к решению этой задачи базируются на формализации понятия отклонения текущего положения робота от заданного пути на основе визуальной информации и поиске такого управления с обратной связью, которое обеспечивает сведение к нулю нормы этого отклонения [2-4].

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

Для иллюстрации полученных результатов выполняется имитационное моделирование алгоритмов управления в среде MATLAB (см. п. 4).

2. Постановка задачи. Введем следующие системы координат. Пусть Ox'y'z' — неподвижная система координат, связанная с Землей, Orxyz — система координат, связанная с роботом. Обе системы координат являются правыми. При этом плоскость Ox'y' — горизонтальная плоскость движения робота, а ось Oz' направлена вертикально вниз. В связанной системе координат ось Orz сонаправлена с продольной составляющей скорости робота, ось Or y направлена вертикально вниз, а ось Orx — на правый борт робота.

Математическая модель динамики полноприводного колесного робота, движущегося на плоскости, имеет вид

V = т + d(t),

(1)

i) = R(n)v,

где т = (tu,tv,тш)T — управляющие силы и момент; v = (u,v,p)T — вектор скорости, компонентами которого являются проекции линейной и угловой скоростей

робота на оси связанной системы координат: u — продольная и v — поперечная составляющие скорости, p — угловая скорость вращения робота. Вектор d(t) определяет внешние возмущения, вызванные, например, ветровыми воздействиями, а вектор П = (x', y', ф)т — положение центра масс (x', y') и курсовой угол ф движения робота по отношению к неподвижной системе координат. Угол ф образуется осями координат Oz, OxX и отсчитывается от оси OxX против часовой стрелки. Система (1) содержит единственную нелинейность, обусловленную ортогональной матрицей поворота

cos ф — sin ф 0 R(n) = И(ф) = | sin ф cos ф 0 | . (2)

0 0 1

На подвижном объекте (1), (2) установлена видеокамера, которая может перемещаться и вращаться только вместе с корпусом робота, т. е. не имеет дополнительных степеней свободы. Будем считать, что центр масс робота и оптический центр камеры совпадают. Тогда связанная система координат Or xyz одновременно является системой координат камеры. Обозначим (X, Y, Z) пространственные координаты произвольной точки сцены в системе координат камеры (робота). Тогда, согласно модели перспективного преобразования, соответствующие ей координаты в нормированной плоскости изображения равны [6]

- X „ У

y = (3)

Уравнения (3) представляют измеряемые величины. Будем также полагать, что измеряется вектор п координат робота. С учетом (3) отображение точки трехмерного пространства, находящейся в плоскости Ox'y' движения робота, на плоскость изображения камеры осуществляется по формулам

X hcam /.ч

x=—, у = —. 4

Z Z у '

Здесь hcam — высота установки камеры над землей. Будем в дальнейшем полагать, что визуально заданная линия расположена в плоскости движения робота Ox'y'. Такой линией может быть разметка дорожной полосы, линия трубопровода и др. Для решения задачи удержания робота на линии необходимо на каждом такте функционирования цифровой системы управления обрабатывать поступившее вновь изображение с целью извлечения из него искомого контура. Разработка соответствующих алгоритмов представляет отдельную задачу и не предлагается в данной статье.

Отметим также, что в работе рассматривается нормированная плоскость изображения [6], в которой введена система координат Ocxy, причем ее оси Ocx и Ocy сонаправлены с осями Orx и Ory соответственно, а начала координат Or и Oc совпадают. Таким образом, точка Oc находится в центре экрана.

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

x = x(s), y = y(s). (5)

Здесь s — вещественный параметр, определяющий положение точки на визуально заданной кривой. Отметим, что кривая (5) является результатом проекции точек кривой l в трехмерном пространстве, заданной уравнениями

X = X (s), Y = hcam, Z = Z (s),

на плоскость изображения в соответствии с формулами (4). Поставим задачу синтеза управления для колесного робота с уравнениями динамики (1), (2), которое обеспечивает его движение вдоль визуально заданной кривой (5). Иными словами, цель управления — минимизация разности между текущими координатами робота (х'(Ь),у'(Ь)) и координатами ближайшей к роботу точки (хг'(Ь),уг'(Ь)) на кривой I, находящейся в плоскости движения робота, т. е. обеспечение равенства

где p(t) = у/(x'(t) — x'r(t))2 + (y'(t) — i/^(t))2. В дальнейшем будем считать, что робот должен перемещаться вдоль визуально заданной линии с постоянной по модулю линейной скоростью v.

3. Разработка алгоритмов управления для движения по визуально заданной линии. Рассмотрим задачу формирования управления, обеспечивающего достижение цели (6), т. е. движение робота по визуально заданной линии (5), в частности по прямой. Идея синтеза заключается в том, чтобы указанная линия в плоскости изображения проходила через точку с координатами (0, hmax), где hmax — максимальное значение ординаты в системе координат изображения, т. е. через центральную точку внизу экрана. И, более того, направление касательной к визуально заданной линии должно совпадать с направлением вектора скорости робота, если отобразить плоскость изображения на горизонтальную плоскость, параллельную плоскости дороги.

В рамках данной работы предлагается алгоритм управления для полноприводного колесного робота. Распространение методологии такого алгоритма на неполно-приводные объекты, представленные моделью (1), является предметом дальнейших исследований.

Для пояснения разрабатываемого алгоритма рассмотрим вспомогательную виртуальную камеру, расположенную над плоскостью движения робота так, что ее плоскость изображения параллельна плоскости Ox'y'. Будем считать, что с этой камерой связаны новая система координат Ow Xyz и система координат OvXv yv для нормированной плоскости изображения. Положение виртуальной камеры относительно системы координат камеры робота определяется вектором параллельного переноса t и матрицей поворота R следующего вида:

Здесь Ну и гу — высота и смещение вперед виртуальной камеры относительно камеры робота. Как видно из приведенных формул, оптическая ось г виртуальной камеры направлена вертикально вниз. Следовательно, то изображение, которое получается в плоскости этой камеры, является «видом сверху».

Плоскость движения робота в системе координат камеры Огхуг определяется уравнением птг + й = 0, где п =(0 1 0)Т , й = -Ну, г — радиус-вектор произвольной точки плоскости. Тогда координаты соответствующих друг другу точек в плоскости изображения камеры робота и виртуальной камеры связаны соотношением

t—^

lim p(t) = 0,

(6)

xv = Hx,

(7)

в котором Н = И. — 1;—- — матрица томографии [6], а векторы х и хг, представляют однородные координаты точек на изображениях камеры робота и виртуальной камеры соответственно. Компоненты векторов х и х, обозначим следующим образом: х = (члх^лу^л), х, = (Лх, Лу, Л), где Л и и> — вещественные параметры. Тогда с учетом формул (5) и (7) изображение линии, полученное камерой робота, можно взаимно-однозначно отобразить на плоскость виртуальной камеры. Полученное таким образом изображение является «видом сверху» для отслеживаемой линии. В качестве примера на рис. 1 приведено изображение одной и той же линии в плоскости камеры робота и виртуальной камеры. В дальнейшем при разработке алгоритмов управления будем использовать изображение виртуальной камеры.

О

-0.1

-0.2

-0.3

-0.4

-0.5

Л

-1 -0.5 0

0.5 1

0.05

Рис. 1. Сопоставление изображений камеры робота (слева) и виртуальной камеры (справа)

Введем следующие обозначения: ех = х, еу = у — Нтах — разность между фактическим положением (х, у) нижней точки визуально заданной линии в плоскости изображения виртуальной камеры и желаемым ее положением (0,Нтах). Отметим, что координата у может отличаться от величины Нтах, если рассматриваемая линия не пересекает прямую у = Нтах.

На основе законов теоретической механики и модели перспективного преобразования выведем уравнения, которые связывают перемещение камеры и изменение ошибок ех,еу. Рассмотрим систему координат виртуальной камеры, которая перемещается вместе с камерой робота. Пусть V и и — проекции вектора линейной скорости виртуальной камеры на оси связанной системы координат х и Ошу соответственно, а ш — ее угловая скорость. Тогда в соответствии с уравнениями, приведенными в работе [1], и с учетом движения виртуальной камеры только в горизонтальной плоскости в данном случае получаем такую модель:

X = —V + шУ,

У = - и- шХ,

(8)

22 = 0, 2 = Н,

Здесь (X, У, 2) — координаты произвольной точки, находящейся в плоскости движения робота, относительно системы координат виртуальной камеры. Далее, согласно модели перспективного преобразования, имеем

Х-* ~ * у (9)

2 Ну 2 Ну

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

^ V X = -— +иу, Ну

и

у = ----их.

Ну

Отсюда следует, что изменение ошибок ех и еу описывается уравнениями

' 0 ~Г У \ ( й\

1 Ну |( V |. (10)

Заметим, что проекции линейной скорости виртуальной камеры и компонент линейной скорости самого робота связаны соотношениями

и = -и, V = V, р = и. (11)

Итак, уравнения (10) и (11) будем в дальнейшем рассматривать как модель для ошибки смещения нижней точки визуально заданной линии от центральной точки (0, Ну) в плоскости изображения виртуальной камеры.

Обратимся теперь к выводу условия сонаправленности векторов касательной к визуально заданной линии в плоскости изображения виртуальной камеры и линейной скорости робота. Пусть X = X(в), У = Нсат, 2 = 2(в) — параметрические уравнения визуально заданной линии относительно системы координат камеры в текущий момент времени. Тогда направляющий вектор касательной в каждой точке этой кривой равен

а(в) = (Х(в), 0,2(в)) '

Отсюда направление касательной в каждой точке кривой относительно системы координат виртуальной камеры определяется по формуле

а^в^ Иа(в). (12)

В (12) И. — приведенная выше матрица поворота. Введем угол в

0 = аг<^—. (13)

а\

В формуле (13) а1 и а2 — первая и вторая компоненты вектора ау. Нетрудно убедиться в том, что угол в определяет направление касательной к визуально заданной линии в плоскости виртуальной камеры. Причем этот угол отсчитывается от направления оси X против часовой стрелки. Заметим, что угол в равен п/2, если направление

т

касательной совпадает с направлением вектора линейной скорости движения робота. В связи с этим введем величину ошибки расхождения между направлением скорости и направлением касательной

п

ее = --в. (14)

Объединяя формулы (10) и (14), получаем следующие уравнения, которые описывают отклонение нижней точки наблюдаемой кривой от центра экрана и разность между направлениями касательной и скорости робота:

Ь(е)у.

(15)

В формуле (15)

ех \ / 0 1 к еу + ^шах ( и

еУ ) , Ъ = 1 0 , V = V

К 0

ее / \ 0 — ех ) —1 ) \"

В соответствии с методом, предложенным в [1], будем формировать управление таким образом, чтобы обеспечить экспоненциальное убывание ошибки расхождения между фактическим и желаемым положениями визуально заданной линии, по формуле

(16)

—¡Ъ

где л > 0 — вещественное число, характеризующее скорость убывания нормы вектора ошибки. В результате уравнения замкнутой системы (15), (16) примут вид

е = —¡е.

Вектор скорости V = V*, определяемый по формуле (16), можно рассматривать в качестве заданного сигнала, подлежащего обработке при помощи управляющего воздействия т модели (1). Будем считать, что на объект управления действует постоянное или медленно меняющееся возмущение В отсутствие возмущений заданная скорость обеспечивается пропорциональным регулятором вида

т * = — К^ — V*),

(17)

в котором К^ — произвольная положительно определенная матрица. Здесь векторы заданных скоростей V* и V* связаны равенством (11). Но при наличии внешних возмущений данный закон управления приводит к ненулевой ошибке воспроизведения заданного командного сигнала. Кроме того, для реализации закона (17) необходимо измерять или оценивать текущее значение скорости.

В связи с указанными обстоятельствами введем в рассмотрение многоцелевую структуру закона управления

Ъ V = т + Кт (п)Кх(п — ¿п), ¿п = + К2(п — Ъп),

т = — К (¿V — V*) + Р(р)(п — ¿п).

(18)

Первые два уравнения в структуре (18) представляют асимптотический наблюдатель, а второе слагаемое в уравнении, определяющем управляющий сигнал, является

е

т

динамическим корректором. Настраиваемыми параметрами в структуре (18) служат матрицы Ki и K2 асимптотического наблюдателя, а также матрица F(p) динамического корректора. В работе [7] показано, что при отсутствии внешних возмущений сходимость оценок асимптотического наблюдателя обеспечивается для любых поло-жительноопределенных матриц Ki и K2, имеющих диагональную структуру. Будем полагать, что данные матрицы выбраны исходя из этого условия.

Обратимся к вопросу устойчивости положения равновесия в замкнутой системе (1), (18). Вначале запишем уравнение динамического корректора в пространстве состояний

p = ap + - Zn), С = YР + ¡(п - zn),

где p — вектор состояния корректора; F(s) = 7(E;s-a)-1fiКак показано в [7], достаточным условием устойчивости положения равновесия служит гурвицевость матрицы a корректора.

Рассмотрим условие обеспечения астатизма при наличии постоянного возмущения d(t) = do • 1(t) в модели (1). Будем считать, что для этого возмущения замкнутая система имеет положение равновесия, которому соответствует равенство ф = фо. Проводя рассуждения, аналогичные приведенным в [7], получаем следующее условие обеспечения астатизма в замкнутой системе:

F(0) = KA = -KdMiiM-1 - RT(фо)Ki.

Здесь матрицы Мц и M21 определяются из условия

/ Mil М12 N = / 0 RT(фo)Kl \-1 ^ M21 M2^ ^ R(Фo) K2 J .

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

4. Пример имитационного моделирования алгоритмов управления. Используем разработанные алгоритмы управления для случая, когда визуально заданная линия представляет собой прямую линию. Примем следующие значения параметров алгоритма управления: ¡л = 10, hv = 5, zv = 20. Рассмотрим прямую, которая в начальный момент времени в системе координат камеры описывается уравнениями

Z (s) = s, X (s) = 1.

Как видно на рис. 2, на котором показана динамика компонент вектора ошибки ex и ey, нижняя точка визуально заданной линии на изображении устанавливается в центр, т. е. в точку с координатами (0, hmax) примерно за 2 с.

На рис. 3 для сравнения приведены заданная относительно мировой системы координат прямая линия, по которой должен перемещаться робот, и фактический путь робота при движении с использованием алгоритма управления (16), (18). Из рисунка вытекает, что данный алгоритм обеспечивает следование робота вдоль заданного пути.

0.05

0.01

0.04

0.03

0.02

-0.01

-0.02

-0.03

0.01

-0.04

10

и с

-0.05

10

и с

Рис. 2. Изменение ошибок ех (слева) и еу (справа) при движении робота

0.8

0.6

0.4

0.2

0

0 10 20 30 40 50 60

Рис. 3. Фактический путь робота и заданная линия движения

70

г'

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

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

Литература

1. Chaumette F., Hutchinson S. Visual Servo Control: Basic Approaches // IEEE Robotics & Automation Magazine. 2006. Vol. 13, N 4. P. 82-90.

2. Cherubini A., Choumette F., Oriolo G. An image-based visual servoing scheme for following paths with nonholonomic mobile robots // Intern. Conference on Control, Automation, Robotics and Vision (ICARCV-2008). Hanoi, Vietnam, France, 2008. P. 108-113.

3. Moriottini G. L., Oriolo G., Prottichizzo D. Image-Based Visual Servoing for Nonholonomic Mobile Robots Using Epipolar Geometry // IEEE Trans. on Robotics. 2007. Vol. 23, N 1. P. 87-100.

4. Hashimoto K., Noritsugu T. Visual servoing of nonholonomic cart // IEEE Intern. Conference on Robotics and Automation. Albuquerque, NM, USA, 1997. P. 1719-1724.

5. Веремей Е. И., Сотникова М. В. Многоцелевая структура законов управления морскими подвижными объектами // Труды XII Всерос. совещания по проблемам управления (ВСПУ-2014), М.: Ин-т проблем управления РАН им. В. А. Трапезникова, 2014. С. 3289-3300.

6. Szeliski R. Computer Vision: Algorithms and Applications. London: Springer-Verlag, 2011. 812 p.

7. Veremey E. I. Dynamical Correction of Positioning Control Laws // Proc. of the 9th IFAC Conference on Control Applications in Marine Systems (CAMS-2013). Osaka, Japan, Sept. 17-20, 2013. P. 31-36.

References

1. Chaumette F., Hutchinson S. Visual Servo Control: Basic Approaches. IEEE Robotics & Automation Magazine, 2006, vol. 13, no. 4, pp. 82-90.

2. Cherubini A., Chaumette F., Oriolo G. An image-based visual servoing scheme for following paths with nonholonomic mobile robots. Intern. Conference on Control, Automation, Robotics and Vision (ICARCV-2008). Hanoi, Vietnam, France, 2008, pp. 108-113.

3. Mariottini G. L., Oriolo G., Prattichizzo D. Image-Based Visual Servoing for Nonholonomic Mobile Robots Using Epipolar Geometry. IEEE Trans. on Robotics, 2007, vol. 23, no. 1, pp. 87-100.

4. Hashimoto K., Noritsugu T. Visual servoing of nonholonomic cart. IEEE Intern. Conference on Robotics and Automation. Albuquerque, NM, USA, 1997, pp. 1719-1724.

5. Veremey E. I., Sotnikova M. V. Mnogotzelevaja structura zakonov upravlenija morskimi podvizhnimi ob'ektami [Multipurpose structure of control laws for marine ships]. Trudy XII Vseros. soveshchaniia po problemam upravleniia ( VSPU-2014) [Transactions of XII Russ. Conference of control problems (VSPU-2014)]. Moscow, 2014, June 16-19. Moscow, Trapeznikov Institute of control problems RAS, 2014, pp. 3289-3300. (In Russian)

6. Szeliski R. Computer Vision: Algorithms and Applications. London, Springer-Verlag, 2011, 812 p.

7. Veremey E. I. Dynamical Correction of Positioning Control Laws. Proc. of the 9th IFAC Conference on Control Applications in Marine Systems (CAMS-2013). Osaka, Japan, September 17-20, 2013, pp. 31-36.

Статья рекомендована к печати проф. Е. И. Веремеем. Статья поступила в редакцию 26 ноября 2015 г.