Определение областей обновления в пространстве состояний
Рассмотрим алгоритмы определения областей обновления в пространстве состояний. Область обновления характеризуется ее типом (по количеству ориентиров), местоположением на карте и размером.
Размер областей обновления определяется через погрешности задания координат ориентиров на карте и погрешности определения роботом относительных координат обнаруженных ориентиров.
Области обновления для одного обнаруженного ориентира
Геометрические выражения для вычисления местоположения робота в случае с одним обнаруженным ориентиром даны в разделе 4.2.4. Для одного обнаруженного ориентиравсе возможные местоположения робота лежат
на окружностях, центрами которых являются все ориентиры, входящие в подмножество Lkи заданные уравнениями (4.21) и (4.22). Из-за погрешностей задания координат ориентиров на карте и погрешностей вычисления относительных координат обнаруженных ориентиров возможные местоположения робота лежат не на окружности, а в некоторой ее окрестности.
При одном обнаруженном ориентиредля каждого ориентира на
карте, входящего в Lk, создается область обновления, показанная на рис. 6.7,а. Область имеет вид полого квадрата, внутри которого находятся возможные местоположения робота в окрестности окружности. Квадратная форма области является следствием того, что функция плотности вероятности в дискретном пространстве состояний представляется прямоугольными трехмерными сетками.
Размер региона обновления (толщина области, в которой располагаются возможные местоположения робота) определяется ковариационной матрицей Матрица Crзадана в полярной системе координат, связанной с находящимся в центре области конкретным ориентиром на карте, и выражает суммарную неопределенность местоположения робота на карте по отношению к ориентиру.
Данная неопределенность складывается из погрешности задания ориентира на карте (заданной матрицей С/) и погрешности измерения координат ориентира роботам (заданной матрицей Cb).Для вычисления Crнеобходимо сложить погрешности путем сложения Ciи Cbв одной системе координат (в полярной). Получим
где Jz, матрица Якоби, описывающая локально линеаризованное преобразование между двумя системами координат (декартовской и полярной) в виде
Матрица Якоби для обратного преобразования (из полярной системы координат в декартову) имеет следующий вид
Области обновления для двух обнаруженных ориентиров
Геометрические выражения для вычисления местоположения робота в случае с двумя обнаруженными ориентирами даны в разделе 4.2.4. Если для пары обнаруженных ориентировна карте найдена схожая пара, то ме
стоположение робота может быть вычислено при помощи выражений (4.23)(4.26). Таким образом, первым вопросом при определении областей обновления для пары ориентиров является отбор пар ориентиров на карте, подобных обнаруженным. При таком отборе следует учитывать погрешности в задании координат ориентиров на карте и при вычислении координат обнаруженных ориентиров.
Отбор пар ориентиров
Для выбора пары ориентировна карте, подобной паре обнаруженных
ориентиров, необходимо учесть следующие критерии:
• Ориентиры в выбранной паре должны быть топологически близки друг другу.
Это подразумевает, что в отборе должны участвовать только те пары ориентиров, которые могут быть одновременно видимы из одной точки рабочего пространства. Данное условие, например, не выполняется для ориентиров, расположенных недалеко друг от друга, но в разных комнатах.• Класс каждого обнаруженного ориентира должен совпадать с классом соответствующего ориентира в выбранной паре на карте.
• Расстояния между ориентирами в обнаруженной и выбираемой парах должны быть подобными. При сравнении расстояний должны учитываться имеющиеся погрешности.
Рис. 6.8. Граф топологии рабочего пространства
При отборе пар ориентиров в случае сложной конфигурации рабочего пространства на карте необходимо иметь информацию о топологии рабочего пространства. Данная информация может быть представлена в виде графа (рис. 6.8), вершины которого представляют собой части рабочего пространства (комнаты, коридоры, проходы), а ребра указывают на топологическую связь между этими частями. При задании ориентира на карте указывается принадлежность ориентира какой-либо вершине графа. Тогда условие топологического отбора ориентира может быть задано следующим образом — оба ориентира в паре должны иметь хотя бы одну общую вершину в топологическом графе.
Критерий отбора по соответствию типов ориентиров в паре можно сформулировать как комбинаторную задачу. Имеяиз ориентиров под
множеств Lkи Lnсоставляются комбинации пар, которые затем отбираются по топологической близости и путем сравнения расстояний между ориентирами в паре.
Рис. 6.9. Пара обнаруженных ориентиров
Сравнение расстояний между обнаруженными и выбираемыми парами ориентиров следует делать с учетом погрешностей в задании координат ориентиров на карте и вычисления координат обнаруженных роботом ориентиров (рис.
6.9). Погрешность определения расстояниямежду ориентирами z и jна карте равнасумме погрешностей координат этих ориентиров
Погрешность вычисления расстояниямежду обнаруженными ориентирами обозначим какЭта погрешность определяется ковариационными матрицамикоторые даны в полярных координатах. Для вычисления
необходимо дважды выполнить преобразования координат для ковариационных матриц, используя матрицы Якоби (6.16) и (6.17). Сначала Cbiпереводятся в декартову систему координат робота и суммируются, а потом в полярную систему координат с центром в одном из обнаруженных ориентиров. Оценка
длявычисляется как суммарная дисперсия ошибок измерения в декартовой системе координат робота, спроецированная на линию, соединяющую оба обнаруженных ориентира, а именно
где θ - угол между линией, соединяющей два ориентира и осью Xв системе координат робота.
Коэффициент подобия расстояний для пар ориентиров вычисляется как
В процессе отбора пар ориентиров отбираются все пары для которых S < Smax, где Smax- некоторое максимальное значение коэффициента подобия.
Для повышения скорости отбора схожих пар ориентиров на карте информация о всех парах ориентиров на карте хранится в предварительно вычисленной таблице, включающей комбинацию их типов
и его погрешностьТаблица включает все пары ориентиров, для
которых верно условиемаксимальный радиус обзора робота.
Вычисление местоположения робота
Выражения для вычисления местоположения робота ξ по паре ориентиров получаются из уравнений (4.23)-(4.26)
Знак В определяет одно из двух возможных решений и вычисляется в зави
симости от типа опорного ориентира в обнаруженной паре. Если обнаруженные ориентиры одного типа, то вычисляются оба решения.
Полученное местоположение робота ξ задает центр области обновления типа, изображенной на рис.6.7,б. Из-за наличия погрешностей в значениях координат ориентиров в выражении (6.19) мобильный робот будет находиться не точно в состоянии ξ, в некоторой его окрестности. Размер этой окрестности в пространстве состояний определяется ковариационной матрицей, вы
числяемой в системе координат карты.
Вычисление размера области обновления.
При получении выражения для вычисления ковариационной матрицы Crзапишем выражение (6.19) в виде некоторой функции со следующими параметрами
Поскольку Crвыражает общую погрешность в вычислении ξ, то на основании такой общей формулировки выражение для Crможет быть получено по правилам распространения ошибки в виде
Выражения для элементов матрицы Якоби J2были получены в явном виде путем дифференцирования (6.19) с помощью пакета Mathematica,а также алгебраических и тригонометрических упрощений полученных выражений.
Для записи элементов матрицы Якоби J2введем следующие обозначения
C использованием введенных обозначений элементы J2вычисляются по следующим выражениям
304
Видно, что отдельные элементы матрицы Якоби находятся в обратно пропорциональной зависимости от расстояний между ориентирамиЭто оз
начает, что погрешность вычисления возрастает при уменьшении расстояния между обнаруженными ориентирами. Это относится и к размеру области обновления. Таким образом, при локализации по двум обнаруженным ориентирам следует избегать пар, где ориентиры находятся близко друг к другу.
Области обновления для трех и более обнаруженных ориентиров
Данный случай имеет много общего со случаем двух обнаруженных ориентиров. Здесь также сначала следует решить задачу по выбору из карты комбинаций ориентиров, похожих по своему расположению на обнаруженные. Затем следует вычислить местоположение робота, используя классические методики триангуляции [206,232,314] в виде некоторой функции
И, в заключение, вычислить погрешность вычисленияв виде ковариационной матрицыОбщее выражение для Cr, согласно правилам рас
пространения ошибок, будет иметь вид
В нашем случае для упрощения алгоритма мы не используем указанных выражений, а решаем задачу классической триангуляции, как комбинационный частный случай двух обнаруженных ориентиров. То есть, если обнаружены 3 ориентира, то из этих трех ориентиров мы формируем 3 пары ориентиров и переходим к вычислению областей обновления для каждой пары по описанному выше алгоритму для двух обнаруженных ориентиров. В данном случае формируется большое количество областей обновления, но это не оказывает значительное влияние на производительность, так как пересекающиеся области обновления объединяются и не обрабатываются повторно алгоритмом вероятностной Марковской локализации.
Причин использования такого подхода несколько. Во-первых, упрощается алгоритм. Во-вторых, для вычисления матриц Якобитребуется численное дифференцирование функции, что требует до
полнительных вычислительных. В-третьих, при данном подходе, классическая
задача триангуляции все же решается, но уже не алгебраическими, а вероятностными методами. Так, для комбинаций пар ориентиров будут получены области обновления в одном месте, которые будут объединены алгоритмом. Для попадающих внутрь нескольких областей состояний будут получены более высокие значения сенсорной функции плотности условной вероятности (см. ниже).
6.3.3.