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

Лохин Валерий Михайлович - д.т.н.; профессор.

Манько Сергей Викггорович - д.т.н.; профессор.

Романов Михаил Петрович - д.т.н.; профессор.

Александрова Римма Ивановна - ведущий электроник.

Makarov Igor Mikhajlovich - Moscow State Technical University of Radioengineering, Electronics and Automation (MSTU MIREA); e-mail: [email protected]; 78, Vernadskiy av., Moscow, 119454, Russia; phone: +74954349232; the department of control problems; dr. of eng. sc.; professor; academician of the Russian Academy of Sciences.

Lokhin Valerij Mikhajlovich - dr. of eng. sc.; professor.

Man’ko Sergej Victorovich - dr. of eng. sc.; professor.

Romanov Mikhail Petrovich - dr. of eng. sc.; professor.

Aleksandrova Rimma Ivanovna - leading electronickname.

УДК 004.896:621.86

P.B. Кучерский, С.В. Манько

АЛГОРИТМЫ ЛОКАЛЬНОЙ НАВИГАЦИИ И КАРТОГРАФИИ ДЛЯ БОРТОВОЙ СИСТЕМЫ УПРАВЛЕНИЯ АВТОНОМНОГО МОБИЛЬНОГО

РОБОТА

Обосновываются принципы построения системы локальной навигации и картогра-

, -,

FastSLAM.

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

FastSLAM , -

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

Система локальной навигации; SLAM (Simultaneous Localization And Mapping); расши-(extended Kalman filter, EKF); FastSLAM.

R.V. Kuchersky, S.V. Man’ko

LOCAL NAVIGATION AND MAPPING ALGORITHMS FOR THE ONBOARD CONTROL SYSTEM OF AUTONOMOUS MOBILE ROBOT

The principles of local navigation and mapping system construction are justified in this article, a comparative performance and accuracy characteristics of systems based on an extended Kalman filter and FastSLAM algorithms analysis is held.

Here are presented results of modeling the cavities building structures mapping process by autonomous mobile robot equipped with extended Kalman filter based local navigation and cartography system.

The selection of FastSLAM method for deeper research, field experiments conduction and analysis of possibility to create a multi-agent robotic co-operative areamapping system based on the method explained.

Local navigation system; SLAM (Simultaneous Localization And Mapping); extended Kalman filter (EKF); FastSLAM.

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

Картографирование местности автономными мобильными роботами представляет собой комплексную проблему, возможность решения которой находится в непосредственной зависимости от качества имеющихся информационноизмерительных средств, а также условий окружающей среды. Круг задач, требующих одновременного решения проблемы локализации и построения карты в условиях несовершенных информационно-измерительных средств, носит общее название SLAM (Simultaneous Localization And Mapping). В настоящее время существует несколько основных подходов к решению этих задач: расширенный фильтр Калмана (extended Kalman filter, EKF), FastSLAM, DP-SLAM.

В последние десятилетия XX-го века практически единственным методом решения подобных задач был расширенный фильтр Калмана. Основным недостатком данного подхода является квадратическая зависимость сложности алгоритма от количества наблюдаемых ориентиров (фактически не превышающего несколь-). -тернативный подход, названный FastSLAM, в основе которого лежит так называемый фильтр частиц (Particle Filter, Monte Carlo methods). В отличие от EKF в FastSLAM -

,

значительно сократить время пересчета оценки состояния системы.

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

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

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

Если в исследуемой среде нет возможности найти ориентиры, то ее проще всего представлять в виде сетки с заполнением ячеек, занятых препятствиями. Хранить такую карту удобно в виде масси-, , препятствий, имеют значение 1, а все остальные - 0 (рис. 1) Такое представление карты применяется, например, в алгоритме DP-SLAM.

, -

ности есть многочисленные ориентиры, карта представляет собой массив оценок .

Dim = R*m, где R - размерность пространства, am - количество ориентиров. Для хранения структуры такой карты проще всего использовать картографическую , , взаимосвязи. Матрица оценок состояния динамической системы на основе расширенного Фильтра Калмана использует именно этот вариант представления карты.

Использование расширенного Фильтра Калмана для решения задачи SLAM. - ,

динамической системы, используя ряд неполных и зашумленных измерений. Назван в честь Р. Калмана. Фильтр Калмана предназначен для рекурсивного дооце-нивания вектора состояния априорно известной динамической системы, т.е. для расчёта текущего состояния системы необходимо знать текущее измерение, а также предыдущее состояние самого фильтра. Состояние фильтра находится в двух :

Хк - оценка вектора состояния динамической системы в момент времени к;

Рк - ковариационная матрица ошибок (мера точности оценивания вектора состояния) в момент времени к.

Расширенный фильтр Калмана (EKF) очень похож на простой фильтр Калмана, за тем исключением, что он может быть использован в нелинейных процессах. EKF - это один из наиболее распространенных методов решения задачи SLAM. Он позволяет не только уточнять оценку положения робота на карте, но и положение всех обнаруженных ориентиров. Обычно процесс оценки состояния системы, в SLAM, :

1) обновление оценки состояния системы на основе одометрических данных;

2)

;

3) добавление новых ориентиров в систему.

, EKF ,

к которым можно отнести в первую очередь ограничение на количество ориенти-. , Р m m, m -

количество обнаруженных ориентиров. На каждом этапе обновления матрицы Р, должен быть обновлен каждый ее элемент, в связи с чем сложность алгоритма O(m2). Таким образом, EKF наиболее применим в ситуации, когда среда имеет не очень большое количество (несколько сотен) легко различимых ориентиров.

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

Рис. 1. Дискретное представление

.

*р=[ Хр Ур Р,Ч; (1)

—хрУр —2 Хррр , (2)

рр = —у2рур —у2рРр

—2 Хр<Рр —у2рРр 1

где хр - оценка координаты робота по оси абсцисс, ур - оценка координаты робота по оси ординат, <рр - оценка ориентации робота.

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

Состояние обнаруженных ориентиров, при условии, что они являются статическими, можно представить в виде вектора оценки их координат Х0, и ковариационной матрицы Р,„ имеющих вид:

(3)

Р =

^о = У Уі ■■ Хп Уп Ґ;

■ —ХіХп

—ХіУі < У1 •• • < Уп

< •• • —« —^пУп

1 пу ^ Уп " • —^пУп —У2пУп_

(4)

где п - количество ориентиров, обнаруженных роботом, х, - оценка координаты г'-го ориентира по оси абсцисс, у( - оценка координаты г'-го ориентира по оси ординат.

По аналогии с матрицей Рр матрица Ри призвана отражать меру зависимости оценки координат ориентиров друг от друга.

Общее состояние системы определяется вектором X, отражающим оценки положения робота и ориентиров, и ковариационной матрицей Р, имеющими вид:

X =[ ХТр Х1]Т ; (5)

□ р

Р =

ро

Р

(6)

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

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

X = Хр = 0,

Р = Рп

В матрице Рр значения диагональных элементов устанавливаются отличными , . Обновление оценки состояния системы на основе одометрических данных в расширенном фильтре Калмана. На это м этапе, называемом шагом пред,

в предыдущий момент времени Хк.1 (р^деленных дискретаым временем Л(), модели движения и одометрических данных. Сигнал управления, используемый для расчета оценки нового положения робота, можно представить в виде матрицы:

и = [у Д^]Г ,

где у - линейная скорость робота,

А<р - угол поворота робота.

Тогда состояние системы на текущем шаге можно представить как:

Ур (^ и )'

X

(7)

Рк =

к -1 т

* РРК-1 * ^8. + б * РР

(У8.

,)Т

(8)

(9)

- ,

Хрк-1 - оценка состояния робота на предыдущем шаге,

Хок.1 - оценка состояния ориентиров на предыдущем шаге, fp - функция, определяющая зависимость между сигналом управления и изменением состояния робота.

(10)

Р - матрица шума процесса, вычисляющаяся по формуле [11]:

б = У8и *и *У81. (11)

Якобианы и и, матрица ковариации и, вычисляются следующим образом:

Хр + Дt * У *С08(®„ рк-1 4' рк- + Дф) I + Дх

II ар * < УРк-1 +Д * у *81п(^Рк- + Д^) = УРк- + Ду

9рк-1 + Д9 Р-1к I + Д^

?8 =^ 8хр дХр

и =

<гу

0

0

(12)

У8, =-^

(Хрк-Ги) *' (X,,

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

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

Наблюдение г-го ориентира, полученное от информационно-измерительной , :

Ъ = [г в]Т , (13)

где г - расстояние от робота до ориентира,

- , , робота против часовой стрелки.

Теперь попробуем предсказать вектор наблюдения, основываясь на оценке положения робота и координатах г-го ориентира, которые находятся в векторе состояния системы на текущем шаге:

к (Хк) =

- хр*)2 +(Уі, - УРК )2

аге1ап(-

Уіг - Ур

X - Х„

(14)

После этого можно посчитать рассогласование между наблюдением ожидаемым (к,) и полученным от информационно-измерительной системы (г,) по сле-:

у, = г. - \(X,), 015)

у, также называется инновацией. Матрица ковариации для нее рассчитывается по :

Sі = Укх * Р. * УК + Я,

где

Я =

0

УК =-

дк;

Эх

(16)

(17)

Матрица И показывает, насколько точна информационно-измерительная сис-.

Наконец, может быть посчитан коэффициент усиления Калмана (^), показы, , -

измеренным должно повлиять на изменение оценки текущего состояния системы:

ЧТ, = Рк * УкТ * 5,-1 + Я. (18)

Единственное, что остается сделать на этом этапе - это обновить оценку состояния системы и соответствующую ей ковариационную матрицу по следующим :

5 у., (19)

Хк = Хк + Wi

Рк = Рк - Wl * Б

(20)

Далее все действия, произведенные на этом этапе, повторяются для каждого повторно обнаруженного ориентира. Таким образом, каждое наблюдение вносит свой вклад в изменение оценки состояния системы.

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

Прежде всего необходимо добавить в вектор оценки состояния системы два новых элемента - оценку х и у координат обнаруженного ориентира. Делается это :

Хк =

Хк

Гі (Х^, ^)

(21)

Здесь /■ - функция, которая переводит вектор наблюдения 7І из полярной системы координат в декартову.

(22). -

Рк , -

ентира от координат робота и координат всех ориентиров, находящихся в системе на данный момент:

Хк

Pk

oT

Vgx

Vgx

* P

PoK

Vgx

P * VgT

Pk Sxp

PT * Vgr

PoK 6 xp

* Vg! +Vgz * R * VgT

(22)

Vgv =

Эх,

(23)

(Хрк.г,)

V*- =1

(XpK.z,)

Это был последний этап процесса SLAM на основе фильтра Калмана.

Использование алгоритма FastSLAM для решения задачи SLAM. В предыдущем разделе, в котором шла речь о применении расширенного фильтра Калмана для решения задачи картографии местности, была обозначена основная про, , его использования на картах, насчитывающих более нескольких сотен ориентиров. Именно для решения этой проблемы в 2002 году Montemerlo, Trun, Koller, и Wegbreit разработали новый подход к решению задачи SLAM. FastSLAM разделяет задачу локализации и картографии на множество подзадач, используя независимость состояния отдельных элементов модели SLAM. B основе алгоритма FastSLAM . , SLAM

, . 2.

Рис. 2. Диаграмма процесса SLAM: XV - Оценка положения робота;

V - текущее положение робота; E(xv) - ошибка оценки положения робота; E(si) - ошибка в показании датчиков; Ot - i-e наблюдение

Из диаграммы справа следует, что все наблюдения независимы друг от друга. В действительности, единственное, что их связывает - это ошибка определения положения робота E(xvi). Таким образом, если положение робота определено абсолютно точно, то в этом случае не должно существовать никаких зависимостей между отдельными наблюдениями. Естественно, в реальности положение робота никогда не известно абсолютно точно, в этом и заключается сама сущность проблемы SLAM, но, тем не менее, сам факт независимости ориентиров друг от

FastSLAM -

дый ориентир по отдельности.

FastSLAM. ,

, x. P(x)

x, . , x

отражает положение робота и ориентиров в многомерном пространстве, то рас-P(x) -

менных состояния. Таким образом, p(x | {ыо, ы}, ... u}, {zo, h, ■■■ z}) описывает вероятности всех величин текущего состояния системы, таких как показания датчи-

, i.

Pi Xi , -

ставлены в значительно более сложной форме. Для удобочитаемости введем обозначения Ut = {ы0, Ы], ... ut} и Zi = {z0, zi, ... z} x в свою очередь включает в себя

положение робота v и положения ориентиров p0, p1, ... pm. p(x I Ui,Zi) можно представить в следующем виде:

p(x | Ui,Zi) = p(v,p0,p1,.. .pm | Ui,Zi). (24)

Воспользуемся основами теории вероятностей для упрощения задачи SLAM. , A B.

что p(A, B) = p(A) * p(B). Но это выражение несправедливо для случая, когда A зависит от B. В этом случае оно будет иметь вид p(A, B) = p(A) * p(B|A). Как из, , значит что p(v,p0,p1,.pm | Ui,Zi) можно представить в следующем виде:

p(v,po,pi,.pm | Ui,Zi) = p(v | Ui,Zi) * p(po,pi,.pm | Ui,Zi,v). (25)

В силу независимости наблюдений ориентиров друг от друга можно разделить p(p0,p1,.pm | Ui,Zi,v) на m независимых выражений: p(v | Ui,Zi) * p(po,p1,.pm | Ui,Zi,v)=

= p(v | Ui,Zi) * p(po | Ui,Zi,v) * p(p1 | Ui,Zi,v) * ... p(pm | Ui,Zi,v). (26)

, : p(x | Ui,Zi) = p(v | Ui,Zi) * nm p(pm | Ui,Zi,v). (27)

Если посмотреть на полученное выражение, то становится очевидным, что проблема SLAM разделена на m+1 задач, и ни одна из оценок положения ориентира не зависит от других. Этот факт, в свою очередь, позволяет решить проблему полиномиальной сложности алгоритма на основе расширенного фильтра Калмана и избежать ее в FastSLAM. Единственная цена, которую приходится платить за такое упрощение - это возможность падения точности, связанная с игнорированием корреляции ошибок оценки положений ориентиров.

FastSLAM ,

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

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

системы локальной навигации и картографии был выбран расширнный фильтр .

, -.

Для оценки точности построения карты был произведен расчет максимального и среднего отклонения координат углов на построенной карте от координат соответствующих углов на исходной карте (рис. 3). Испытания проводились на трех картах различной конфигурации и протя-,

:

24x16 см; максимальная линейная скорость 0,5 м/с; максимальная угловая скорость 90 град/с; угол обзора видеокамеры 120 град; коэффициент проскальзывания колес 0,1; масштаб карты 1 пиксель = 1 см.

Рис. 3. Отклонение координат углов на построенной карте от координат соответствующих углов на реальной карте

Результаты эксперимента представлены в табл. 1, где прияты следующие обозначения: £да(. - суммарная протяженность коридоров (м); £.мж..- максимальная протяженность коридора (м); гср. - среднее отклонение координат (м); гмакс. -

( ).

1

Результаты оценки точности построения карт

Реальная карта

Построенная карта

Характеристики ______карты______

Ьсум. = 19,46 Ь„акс. = 3,67 г<.р. = 0,09

гмакс. = 0,2

Естм. = 31,11

Емакс. = 1,

гср. = 0,06

Г„акс. = 0,16

Ьстм. = 33,83

Емакс. = 2,61

Гср. = 0,13

Гмакс. = 0,48

Для расчета отклонения координат построенной карты от координат реаль-,

г. = ^[(х~~-х~)2^(у~~^у~У, (28)

1 \ Р1 П1 рг ’ 4 '

где хР1 и уР1 - координаты г-го угла на реальной карте, а хп1 и уп1 - координаты соответствующего угла на построенной карте.

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

(

0,2 м), даже при достаточно большой суммарной протяженности коридоров. Также , , суммарной и максимальной протяженности коридоров.

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

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

БИБЛИОГРАФИЧЕСКИЙ СПИСОК

1. Лохин В.М., Манько С.В., Романов МЛ., Александрова Р.И. и др. Универсальная бортовая система управления для автономных мобильных объектов ВВТ // Материалы III научно-практической конференции «Перспективные системы и задачи управления». Т. 1. - Таганрог: Изд-во ТТИ ЮФУ, 2008.

2. Романов МЛ., Гарцеев И.Б. Интеллектуальная система навигации для малоразмерных подвижных объектов ВВТ // Материалы III научно-практической конференции «Пер». . 2. - : - , 2008.

3. Castellanos J.A., Neira J., Tardos J.D. Limits to the consistency of the EKF-based SLAM. In Intelligent Autonomous Vehicles (IAV-2004) (Lisboa, PT, July 2004).

4. Durrant-Whyte, Hugh. Localization, Mapping, and the Simultaneous Localization and Mapping Problem. Australian Center for Field Robotics. Sydney. 2002.

5. Montemerlo M., Thrun S., Koller D., Wegbreit B. Fastslam: A factored solution to the simultaneous localization and mapping problem. In AAAI-2002 (Vancouver, BC, July 2002).

Статью рекомендовал к опубликованию д.т.н., профессор О.С. Колосов.

Кучерекий Роман Владимирович - Федеральное государственное бюджетное образовательное учреждение высшего профессионального образования «Московский государственный технический университет радиотехники, электроники и автоматики» (МГТУ М'ИРЭА); e-mail: [email protected], [email protected]; 119454, г. Москва, пр-кт Вернадского, 78; тел. 84954349232; кафедра «Проблемы управления»; аспирант.

Манько Сергей Викторович - д.т.н.; профессор.

Kuchersky Roman Vladimirovich - Moscow State Technical University of Radioengineering, Electronics and Automation (MSTU MIREA); e-mail: [email protected], [email protected]; 78, Vernadskiy av., Moscow, 119454, Russia; phone: +74954349232; control problems department; post-graduate student.

Man’ko Sergej Victorovich - dr. of eng. sc.; professor.

УДК 681.51:004.7:621.865

..

ЗАДАЧА ФОРМИРОВАНИЯ СТРОЯ В ГРУППЕ БПЛА*

Статья посвящена рассмотрению актуальной темы группового применения беспилотных летательных аппаратов (БЛА). Данная задача является перспективной и имеет множество практических применений. Группы БЛА могут применяться для мониторинга окружающей обстановки, для картографирования местности, для поиска объектов на больших территориях и т. п.

* Работа выполнена при поддержке РФФИ, грант № 11-08-01196-а. 22