Построение карты роботом
Всегда думал, что неплохо знаю математику, в том числе и геометрию, но оказалось что составление карты по замерам расстояний не простая задача. Для построения карты использовались реальные замеры перемещающегося робота. То есть, робот выполнял измерения расстояний вокруг себя, затем перемещался и снова измерял. Так получилась серия измерений. Составление карты производилось последовательно от самого первого измерения к последующим. Перед участием в формировании карты измерение подвергалось обработке алгоритмом локализации, который наиболее точно располагал измерение на карте сформированной на предыдущих шагах. Первое измерение становится картой практически без изменений, за исключением работы алгоритма уменьшения количества точек. Вот как выглядит карта построенная на первом измерении:
Толстая синяя линия — соответственно граница карты, точка в центре с зеленой полоской — центр робота, полоска показывает его ориентацию.
Вот второй шаг:
Здесь толстая жирная линия, это карта сформированная на первом шаге, тонкая синяя — измерения второго шага. Зеленые линии исходящие из центра второго положения робота — лучи дальномера. Красная линия — новая карта.
Новая карта строится следующим образом. Через каждую точку второго измерения проводится луч исходящий из центра робота. Этот луч строится до пересечения с картой или с самой точкой измерения. Затем новой точкой карты становится середина отрезка (красная точка) между точкой пересечения луча с картой и точкой измерения. То есть, мы просто усредняем результаты двух измерений с разных положений робота. Причем, если новая точка находится в непосредственной близости к точке карты, просто смещаем точку карты, если же далеко, добавляем на карту новую точку. Затем запускаем процедуру уменьшения количества точек на карте и повторяем процесс для следующего измерения. Вот как выглядит третий этап:
Здесь отдельные точки — это точки всех измерений, они добавлены для наглядности. В центре слева есть небольшой косяк в построении карты, она почему то завернулась назад, но общей картины для предлагаемой идеи это не портит.
Вот результат десятого шага:
Здесь уже точек гораздо больше и кое где так же есть некоторые проблемы алгоритма, но общая картина сохраняется.
Вот как выглядит эта же карта, без вспомогательных линий построения:
Так же видны некоторые ошибки алгоритма построения карты. Но теперь уже можно оценить, насколько верно сформирована карта. В некоторых местах, которые достаточно прямолинейны и стабильны, алгоритм достаточно правильно отрисовал контур карты, а вот в других местах, где разброс точек большой — карта создана не совсем верно. Почему — видно из следующего рисунка, где изображены только точки измерений:
Здесь видно, почему такое происходит. Карта строилась на реальной комнате (кухне :-)), поэтому там оказались предметы, например, такие как ножки стола и стула, алгоритм все время пытался усреднить расстояние до ножки и до реальной границы комнаты, и в этих местах у него не получилось. Так же хорошо видны большие погрешности дальних измерений — правый верхний угол (это ровная стена, должна быть прямая линия).
Вывод: В целом алгоритм оказался работоспособным, неплохо учитывающим выпуклые объекты (стены, мебель) и плохо учитывающий объекты находящиеся внутри карты (ножки стола, стула). Достоинство его в том, что карта векторная, занимает мало места и во много раз сокращает количество расчетов при работе с ней. Однако глядя на последний рисунок как то хочется строить ее по другому, основываясь не на отрезках, а на точках, местах их консолидации.
И получить, что то примерно такое:
Алгоритм локализации робота Перегрузка или как иногда полезно делать предварительные расчеты