Lab127
ООО "Лаб127"
RUS ENG
Одновременное позиционирование объекта и построение карты его окружения (SLAM)

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

Как правило, положение базовых станций на карте задаётся при инсталляции системы позиционирования. Такая процедура может занимать много времени. Кроме того, координаты базовых станций могут быть заданы с ошибкой, что приведёт к нарушению корректной работы системы позиционирования. Существуют приложения в которых предварительная расстановка базовых станций на карте является затруднительной.

Одним из решений является автоматическая расстановка базовых станций с использованием алгоритмов одновременного решения задач локации объекта и построения его окружения (Simultaneous Localization And Mapping — SLAM).

Основная идея метода SLAM показана на рисунке ниже.

Пример траектории движения человека с МУ вокруг БС
Рис.1. Пример траектории движения человека с мобильным устройством вокруг базовой станции.

На данном рисунке приведён пример траектории движения человека с мобильным устройством в руке. Человек двигался вокруг базовой станции, показанной перечёркнутым квадратом. Траектория регистрировалась встроенным модулем IMU. Расстояния между мобильным устройством и базовыми станциями измерялись в моменты времени, когда пешеход находился в трёх различных положениях, обозначенных цифрами 1, 2 и 3. Очевидно, что положение БС находится в одной из точек внутри области пересечения окружностей. Чем больше расстояний измеряется за время движения пешехода, тем меньше становится область пересечения окружностей и тем точнее определяется местоположение базовой станции.

Для расчёта локации мобильного устройства используется фильтр частиц. В данном методе состояние системы в каждый момент времени \(k\) представляется в виде набора из \(N\) случайных частиц \({\mathbf{x}_k}^{(i)} , i=1 \dots N\) и весов. Каждая отдельная частица соответствует возможному значению вектора состояния системы. Данный набор частиц изменяется рекурсивно в соответствии с уравнениями байесовской фильтрации.

Для вычисления локации базовой станции применяется алгоритм дискретного байесовского фильтра (ДБФ). В данном методе неизвестная плотность распределения аппроксимируется набором точек равномерной дискретной сетки и соответствующим им вероятностям. Данное упрощение позволяет напрямую рассчитать вероятности нахождения базовой станции в конкретной точке дискретной сетки в момент времени \(k\), принимая во внимание все измерения \(\mathbf{z}_{1 \dots k}\) к моменту времени \(k\).

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

ДБФ инициализируется для каждой базовой станции при получении первого расстояния, измеренного между базовой станцией и мобильным устройством. Поскольку измеренное расстояние методом RTT всегда больше или равно истинному расстоянию, базовая станция всегда будет располагаться внутри окружности с центром в данной частице и радиусом, равным измеренному расстоянию. Данная окружность разделяется на дискретные точки с заранее заданным расстоянием между ними. Если положение базовой станции ограничено контуром здания, точки дискретной сетки выбираются внутри данного контура.

Операция коррекции для БС и выбранной частицы
Рис.2. Операция коррекции для базовой станции и выбранной частицы.
Цвета точек соответствуют вероятностям: красный цвет соответствует более высоким вероятностям. Окружность соответствует расстоянию, измеренному между базовой станцией и мобильным устройством. A, B, C, D – четыре последовательных момента времени.

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

Распределение ошибок в расстояниях, измеренных методом RTT
Рис.3. Распределение ошибок в расстояниях, измеренных методом RTT
1 – экспериментальная зависимость, 2 – аппроксимирующая функция.

На данном рисунке также приведен пример функции \(h(e_r)\), аппроксимирующей данное распределение: $$ h(e_r)=\frac{1}{e_r \cdot \sigma \cdot \sqrt{2\pi}} \exp{\frac{-(\ln{e_r}-\mu)^2}{2\cdot \sigma^2}}. $$ Здесь \(e_r\) – ошибка, соответствующая разнице между измеренным и истинным расстоянием, \(e_r \geqslant 0\).

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

Таким образом, после инициализации в момент времени \(k=0\) для каждой отдельной частицы информация о местоположении базовой станции представлена набором из точек \((x_{i,0}, y_{i,0})\) и соответствующих вероятностей \(p_i, i = 1 \dots G_0\), где \(G_0\) – начальное число точек дискретной сетки.

В следующий момент получения новых измерений (\(k=1\)) частица меняет своё положение в соответствии с траекторией, полученной от IMU. После чего строится новая окружность, соответствующая измеренному расстоянию. Те точки дискретной сетки, которые оказались вне области, ограниченной новой окружностью, удаляются из конечного набора. Для оставшихся точек применяется процедура вычисления вероятностей.

Данная процедура повторяется каждый раз при получении новых измеренных расстояний (например, в моменты C и D на рисунке выше). После нескольких итераций для каждой отдельной частицы наблюдается как геометрическая сходимость ДБФ, так и сходимость по вероятности. Геометрическая сходимость достигается благодаря отсечению точек окружностями, построенными по новым измерениям. Сходимость по вероятности достигается благодаря процедуре вычисления вероятностей по описанной выше методике.

Работа ДБФ для данной частицы проиллюстрирована на рисунке выше. После того, как частица переместилась в соответствии показанной с траекторией, количество точек, соответствующих возможным положениям базовой станции, сократилось приблизительно до 10.

Местоположение базовых станций вычисляется как среднее из возможных местоположений базовых станций для всех частиц. Общий набор точек для всех частиц для данной базовой станции можно использовать для оценки точности локации. Чем меньше область, представленная набором точек, тем точнее оценка местоположения данной базовой станции.

Для того, чтобы лоцировать базовые станции и мобильные устройства в пределах заданной карты здания необходимо с заданной точностью рассмотреть все возможные гипотезы о возможных траекториях движения объекта. Поскольку начальное положение мобильного устройства неизвестно, то набор из \(N\) частиц генерируется случайным образом внутри всей области, ограниченной картой здания. Каждая из сгенерированных частиц перемещается в соответствие с траекторией, полученной от встроенного IMU.

Траектория движения человека, определенная встроенным IMU между двумя последовательными промежутками времени \(k-1\) и \(k\) состоит из набора векторов, соответствующих шагам \(\{s_i, i = 1 \dots N_s\}\). Каждый вектор \(s_i\) имеет длину \(l_i\) и направление \(\alpha_i\). Для того, чтобы учесть возможные ошибки определения траектории встроенным IMU, уравнения динамики частиц содержат компоненты, соответствующие шумам измерений: $$ x_{k} = x_{k-1} + \sum_{j=1}^{N_s}{(l_{j} + {\delta_l}_j)\cdot \cos{\alpha_{j}}}, $$ $$ y_{k} = y_{k-1} + \sum_{j=1}^{N_s}{(l_{j} + {\delta_l}_j)\cdot \sin{\alpha_{j}}}, $$ $$ \alpha_k = \alpha_{k-1} + \sum_{j=1}^{N_s}{(\alpha_j + {\delta_\alpha}_j)}, $$ где \((x_k, y_k)\) и \((x_{k-1}, y_{k-1})\) – координаты частицы в моменты времени \(k\) и \(k-1\); \({\delta_l} = N(0, {\sigma^2}_{l})\) – нормально распределённая случайная величина с нулевым математическим ожиданием и дисперсией \({\sigma^2}_{l}\), соответствующая возможным изменениям в длине шага; \({\delta_\alpha} = N(0, {\sigma^2}_{\alpha})\) – нормально распределённая случайная величина с нулевым математическим ожиданием и дисперсией \({\sigma^2}_{\alpha}\), соответствующая возможным изменениям в направлении движения.

Информация о стенах здания учитывается на этапе прогноза для повышения точности локации. Если частица в ходе перемещения пересекает стену (что невозможно на практике), она удаляется из конечного набора. Таким образом из начального числа из \(N\) частиц остается только \(M\).

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

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

Для проверки эффективности разработанного алгоритма были использованы модельные эксперименты. В ходе экспериментов предполагалось, что человек с мобильным устройством в руке двигался по заранее определённой траектории. Моделировалось движение внутри офиса площадью \(800\,м^{2}\). Внутри здания располагалось 9 базовых станций. На рисунке их местоположение обозначено красными квадратами и четырьмя символами, соответствующими двум последним байтам MAC-адреса. Движение начиналось из левой части коридора из точки, расположенной около базовой станции A5B4.

Траектория движения МУ и местоположение БС, рассчитанные с помощью разработанного алгоритма SLAM
Рис.4. Траектория движения мобильного устройства и местоположение базовых станций, рассчитанные с помощью разработанного алгоритма SLAM.

Эталонный трек показан на рисунке красной пунктирной линией. Трек, зарегистрированный встроенным IMU показан серой линией. Рассчитанный трек показан сплошной синей линией.

В ходе перемещения мобильного устройства набор расстояний до базовых станций генерировался с интервалом раз в секунду. Параметры ошибок в измеренном расстоянии и данных IMU моделировались в соответствии с результатами предыдущих экспериментов.

В ходе тестирования разработанный алгоритм запускался со следующими параметрами:

  • начальное число частиц \(N_0=15000\);
  • порог, при котором осуществлялась инициализация ДБФ \(N_{init}=100\);
  • расстояние между точками дискретной сетки в ДБФ – 1 м;
  • стандартное отклонение шумов направления движения составляло \(2^{\circ}\);
  • стандартное отклонение шумов длины шага равнялось 10 см.

Для того, чтобы оценить точность локации, рассчитанные координаты мобильных устройств и базовых станций сравнивались с эталонными координатами для каждой точки экспериментальной траектории. Для приведённого эксперимента точность расчёта локации мобильного устройства (75 % квантиль кумулятивной функции ошибки локации) после сходимости алгоритма составила \(\approx 1.2\,м\). Ошибки в определении местоположении БС для финальной точки траектории варьировались от 3.2 м (A5B4) до 0.2 м (9F3F).

Точность расчёта локации существенно улучшается после сходимости алгоритма. На рисунке ниже показана зависимость точности локации для мобильных устройств и базовых станций от номера итерации (моментов получения измерений расстояний и данных от IMU).

Зависимость точности локации от номера итерации
Рис.5. Зависимость точности локации (75 % квантиль кумулятивной функции ошибки локации) от номера итерации.

Сходимость алгоритма наблюдается приблизительно через 50 итераций. Момент сходимости соответствует проходу человека около базовой станции с MAC-адресом 9F40.

Необходимо отметить, что форма траектории движения человека влияет на точность расчёта локации базовых станций. Форма траектории может быть подобрана таким образом, чтобы обеспечить лучшую точность расчёта локации. Тем не менее результаты, полученные в модельных экспериментах (ошибка 1.2 м для 75 % квантили кумулятивной функции ошибки локации) сравнимы с результатами работы системы RealTrac с заранее заданными координатами базовых станций, продемонстрированными во время участия в международном конкурсе систем позиционирования EVAAL-2013.

По всем вопросам обращаться к Мощевикину Алексею Петровичу
Использование материалов сайта
без разрешения запрещено!