Алгоритм локализации робота
После завершения тестирования второго варианта головы робота были получены результаты измерения расстояний вокруг робота и появилась возможность заняться программированием алгоритма локализации робота (SLAM). Поиск в интернете подходящих готовых алгоритмом результатов не дал. Вернее алгоритмов много, как и их реализаций, и все они достаточно сложны и трудоемки (например, большой список таких алгоритмов есть на openslam.org ). Так или иначе многие алгоритмы SLAM используют показания датчиков расстояния и одометрию уточняя измерения при помощи фильтра Калмана. Формулы фильтра Калмана позволяют рекурсивно найти некий коэффициент Калмана, который определяет, кому мы доверяем больше — датчикам расстояния или одометрии. Самое интересное, что фильтр Калмана позволяет на основе неточных измерений из разных источников вычислять гораздо более точные данные о положении робота ( вот еще одна статья о фильтре Калмана). Я же посчитал, что для моей задачи фильтр Калмана не очень подходит, так он все же расчитан на некое равномерное движение робота в одинаковых условиях, но стоит скажем взять робота и перенести на некоторое расстояние или робот выйдет на скользкую поверхность, стройный фильтр Калмана вряд ли это сможет учесть. Вернее сам формулы фильтра позволяют учитывать известное управляемое воздействие, но вот случайное воздействие учесть сложно. Поэтому пришлось разделить задачу на две — локализация робота на существующей карте, и построение/уточнение карты по мере движения робота.
Как раз правильная локализация на карте по моему мнению позволил роботу более осмысленно определять что с ним происходит. Например, робот выполняет команду перемещения, скажем на 10 см. Для этого он выполняет измерение расстояний вокруг себя, сдвигается на нужное расстояние и снова выполняет измерение расстояний. В этом случае, мы получаем два многоугольника, совместив которые получим на сколько робот реально переместился/повернулся. Таким образом получается некая система с обратной связью, которую можно будет в будущем сделать самообучающейся. Если же в процессе определения перемещения робот выяснит, что он сместился не на 10 см, как ожидалось, а скажем на 30 или вообще не переместился, это уже повод для беспокойства, значит что то не так.
Задачу локализации я начал решать с самого простого варианта — наиболее точном совмещении двух многоугольников — результатов двух последовательных измерений датчика расстояния. После выполнения такого совмещения, становится точно известно, на какое расстояние переместился робот и/или повернулся. Для начала нужен был критерий совмещения многоугольников. Наиболее подходящим сначала показался критерий совмещения площадей, но мне не удалось придумать достаточно быстрый алгоритм расчета пересечения площадей. Следующим критерием была близость точек одного многоугольника к точкам другого. От него так же пришлось отказаться, так как в результате погрешностей измерений и из за того, что измерения ведутся с другой позиции, точки одного многоугольника не имеют точного соответствия точкам другого. Наиболее подходящим на этот момент исследований оказался критерий минимального среднеквадратичного расстояния перпендикуляра проведенного из точек второго многоугольника к грани первого. То есть, из каждой точки второго многоугольника находим перпендикуляр к каждой грани первого многоугольника, и находим перпендикуляр с минимальной длинной. Если такого перпендикуляра не существует вычисляем минимальное расстояние до ближайшей точки. Такие минимальные расстояния возводим в квадрат и суммируем по всем точкам второго многоугольника. Соответственно, чем меньше получается цифра, тем точнее точки одного многоугольника лежат на гранях другого. В идеале, если каждая точка второго многоугольника лежит на прямой соединяющей точки первого — они полностью совпадают. На самом деле это конечно же спорное утверждение, но пока примем его как верное.
Далее встала проблема времени расчета, ведь для оптимизации приходится вычислять расстояние от каждой точки второго многоугольника к каждой грани первого, и количество вычислений растет существенно даже при небольшом количестве точек. Пришлось подумать над тем, как сократить количество точке не очень нарушая геометрию фигур. К счастью такой алгоритм есть, реализуется очень просто и называется Алгоритм Рамера — Дугласа — Пекера. Благодаря этому алгоритму существенно уменьшается количество точек многоугольника, так как он позволяет выбрасывать точки лежащие на одной прямой с заданной точностью.
Вот как это выглядит на картинке:
Здесь толстая синяя линия — это результаты первого измерения (считаем что это карта), соответственно жирные синие точки — замеры первого измерения уже после уменьшения количества точек. Тонкая синяя линия и мелкие точки — необработанные результаты второго измерения, более крупные точки — результаты второго измерения после сокращения количества точек. Зеленые линии кратчайшие перпендикуляры от точек второго измерения к граням первого, соответственно, красные точки — точка пересечения перпендикуляра с гранью.
Затем пришлось повозиться с различными вариантами алгоритмов оптимизации. В частности было протестирован один из вариантов алгоритма Монте-Карло и Метод последовательного спуска. Метод последовательного спуска оказался быстрее и проще и самое главное при одинаковых входных данных дает один и тот же результат. Метод же Монте Карло оказался более трудоемким по количеству вычислений и менее точным. Поэтому пока пришлось остановиться на методе последовательного спуска, хотя подозреваю, что в некоторых случаях он будет не таким уж и хорошим, в случае существования нескольких локальных минимумов. Несмотря на это пока остановился на этом методе локализации. Вот результат работы алгоритма локализации:
Теперь встает проблема непосредственно создания карты — как по двум замерам — двум многоугольникам создать один — карту…..
Второй вариант головы робота Построение карты роботом