Привязка траектории движения мобильного объекта к плану помещения

Информация о траектории движения мобильного объекта поступает в виде последовательности шагов, каждый из которых состоит из двух координат: расстояния и угла, на который изменяется направление движения. План здания представлен координатами концов стен. Заданы начальная позиция и направление движения мобильного объекта. Шаг является результатом слияния и интеграции данных, полученных от системы инерционных датчиков (акселерометров и гироскопов) за некоторое время.

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

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

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

Проблема вырождения частиц - одна из требующих решения подзадач при данном подходе. Частицы могут «вымирать» (вес частицы становится равен нулю), если они на текущем шаге, например, пересекли стену. Возможны и другие варианты, при которых вес частицы изменяется: различного рода ограничения или дан¬ные от других источников. Вес частиц-потомков можно связать с весом частиц-предков. Это позволит отделить более вероятные варианты перемещения от менее вероятных.

Еще один подход к данной задаче - дискретизация пространства. План помещения разбивается на достаточно маленькие ячейки, предполагается аппроксимировать все возможные позиции, попавшие в ту или иную ячейку, центром той самой ячейки. На следующем шаге из-за неопределенности множество ячеек, в которых может находиться объект, вырастает.

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

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

С. В. Иванов