Определение областей обновления в пространстве состояний

Рассмотрим алгоритмы определения областей обновления в пространстве состояний. Область обновления характеризуется ее типом (по количеству ори­ентиров), местоположением на карте и размером.

Области обновления для одного обнаруженного ориентира

Геометрические выражения для вычисления местоположения робота в слу­чае с одним обнаруженным ориентиром даны в разделе 4.2.4. Для одного обна­руженного ориентиравсе возможные местоположения робота лежат

на окружностях, центрами которых являются все ориентиры, входящие в под­множество L_kи заданные уравнениями (4.21) и (4.22). Из-за погрешностей за­дания координат ориентиров на карте и погрешностей вычисления относитель­ных координат обнаруженных ориентиров возможные местоположения робота лежат не на окружности, а в некоторой ее окрестности.

При одном обнаруженном ориентиредля каждого ориентира на

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

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

где J_z, матрица Якоби, описывающая локально линеаризованное преобразова­ние между двумя системами координат (декартовской и полярной) в виде

Матрица Якоби для обратного преобразования (из полярной системы коор­динат в декартову) имеет следующий вид

Области обновления для двух обнаруженных ориентиров

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

стоположение робота может быть вычислено при помощи выражений (4.23)­(4.26). Таким образом, первым вопросом при определении областей обновления для пары ориентиров является отбор пар ориентиров на карте, подобных обнару­женным. При таком отборе следует учитывать погрешности в задании координат ориентиров на карте и при вычислении координат обнаруженных ориентиров.

Отбор пар ориентиров

Для выбора пары ориентировна карте, подобной паре обнаруженных

ориентиров, необходимо учесть следующие критерии:

• Ориентиры в выбранной паре должны быть топологически близки друг другу.

• Класс каждого обнаруженного ориентира должен совпадать с классом со­ответствующего ориентира в выбранной паре на карте.

• Расстояния между ориентирами в обнаруженной и выбираемой парах должны быть подобными. При сравнении расстояний должны учитываться имеющиеся погрешности.

Рис. 6.8. Граф топологии рабочего пространства

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

Критерий отбора по соответствию типов ориентиров в паре можно сформулиро­вать как комбинаторную задачу. Имеяиз ориентиров под­

множеств L_kи L_nсоставляются комбинации пар, которые затем отбираются по то­пологической близости и путем сравнения расстояний между ориентирами в паре.

Рис. 6.9. Пара обнаруженных ориентиров

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

сумме погрешностей координат этих ориентиров

Погрешность вычисления расстояниямежду обнаруженными ориентира­ми обозначим какЭта погрешность определяется ковариационными мат­рицамикоторые даны в полярных координатах. Для вычисления

необходимо дважды выполнить преобразования координат для ковариацион­ных матриц, используя матрицы Якоби (6.16) и (6.17). Сначала C_biпереводятся в декартову систему координат робота и суммируются, а потом в полярную систему координат с центром в одном из обнаруженных ориентиров. Оценка

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

где θ - угол между линией, соединяющей два ориентира и осью Xв системе координат робота.

Коэффициент подобия расстояний для пар ориентиров вычисляется как

В процессе отбора пар ориентиров отбираются все пары для которых S < S_max, где S_max- некоторое максимальное значение коэффициента подобия.

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

и его погрешностьТаблица включает все пары ориентиров, для

которых верно условиемаксимальный радиус обзора робота.

Вычисление местоположения робота

Выражения для вычисления местоположения робота ξ по паре ориентиров получаются из уравнений (4.23)-(4.26)

Знак В определяет одно из двух возможных решений и вычисляется в зави­

симости от типа опорного ориентира в обнаруженной паре. Если обнаруженные ориентиры одного типа, то вычисляются оба решения.

Полученное местоположение робота ξ задает центр области обновления ти­па, изображенной на рис.6.7,б. Из-за наличия погрешностей в значениях коор­динат ориентиров в выражении (6.19) мобильный робот будет находиться не точно в состоянии ξ, в некоторой его окрестности. Размер этой окрестности в пространстве состояний определяется ковариационной матрицей, вы­

числяемой в системе координат карты.

Вычисление размера области обновления.

При получении выражения для вычисления ковариационной матрицы C_rзапи­шем выражение (6.19) в виде некоторой функции со следующими параметрами

Поскольку C_rвыражает общую погрешность в вычислении ξ, то на ос­новании такой общей формулировки выражение для C_rможет быть получено по правилам распространения ошибки в виде

Выражения для элементов матрицы Якоби J₂были получены в явном виде путем дифференцирования (6.19) с помощью пакета Mathematica,а также ал­гебраических и тригонометрических упрощений полученных выражений.

C использованием введенных обозначений элементы J₂вычисляются по следующим выражениям

304

Видно, что отдельные элементы матрицы Якоби находятся в обратно про­порциональной зависимости от расстояний между ориентирамиЭто оз­

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

Области обновления для трех и более обнаруженных ориентиров

Данный случай имеет много общего со случаем двух обнаруженных ориен­тиров. Здесь также сначала следует решить задачу по выбору из карты комби­наций ориентиров, похожих по своему расположению на обнаруженные. Затем следует вычислить местоположение робота, используя классические методики триангуляции [206,232,314] в виде некоторой функции

И, в заключение, вычислить погрешность вычисленияв виде ковариаци­онной матрицыОбщее выражение для C_r, согласно правилам рас­

пространения ошибок, будет иметь вид

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

Причин использования такого подхода несколько. Во-первых, упрощается алгоритм. Во-вторых, для вычисления матриц Якобитребуется чис­ленное дифференцирование функции, что требует до­

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

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

6.3.3.