EKF SLAM (Extended Kalman Filter SLAM)

Класс алгоритмов SLAM, использующих расширенный фильтр Калмана.

Впервые предложен в 1986 году Smith and Cheeseman [50], впервые разработан в рамках рабочей системы в 1989 году Moutarlier and Chaitila [51].

На рисунке 12 продемонстрирована упрощенная схема алгоритма EKF SLAM.

Рисунок 12 — Алгоритм SLAM с использованием EKF [24].

Процесс оценки состояния системы в контексте SLAM разбивают на [22]:

• 1. Обновление оценки состояния системы на основе одометрических данных;
• 2. Обновление оценки состояния системы на основе повторно обнаруженных ориентиров;
• 3. Добавление новых ориентиров в систему.

Расширенный фильтр Калмана (EKF) отличается от простого фильтра Калмана тем, что может быть использован в нелинейных процессах. Он позволяет не только уточнять оценку положения робота на карте, но и положение всех обнаруженных ориентиров. Тем не менее, EKF имеет серьезный недостаток в виде ограничения на количество обнаруженных ориентиров [22].

Как отмечается в работе Michael Monremerlo:

«While the EKF has become the dominant approach to SLAM, it suffers from two well-known problems that complicate its application in large, real-world environments: quadratic complexity and sensitivity to failures in data association» [49].

Для пояснения первой проблемы примем следующие обозначения [22]:

— оценка вектора состояния динамической системы в момент времени ;

— ковариационная матрица ошибок в момент времени .

Матрица ошибок имеет размерность, где — количество обнаруженных ориентиров. На каждом этапе обновления матрицы должен быть обновлен каждый её элемент, в связи с чем сложность алгоритма будет функцией от квадрата, т. е. [22].

«The first drawback of the EKF as a solution to the SLAM problem is computational complexity. Both the computation time and memory required by the EKF scale quadratically with the number of landmarks in the map. SLAM algorithms based on the full EKF generally do not scale beyond a few hundred landmarks. In contrast, reasonably large environment models might contain millions of features» [49].

Таким образом, EKF применим в ситуации, когда окружающая среда имеет небольшое (не более нескольких сотен) легко различимых ориентиров [22].

На рисунке 13 проиллюстрировано применение расширенного фильтра Калмана в задаче SLAM.

Рисунок 13 — EKF applied to a simulated data set [49].

Рассмотрим вкратце математически аппарат метода.

Состояние мобильного робота в произвольный момент времени можно представить с помощью вектора оценки его текущего местоположения и ковариационной матрицы [22] Прим.: рассматривается пример для 2D движения.:

где — оценка координат робота по осям абсцисс и ординат, — оценка ориентации робота.

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

Состояние обнаруженных ориентиров (при условии, что они являются статическими) можно представить в виде вектора оценки их координат и ковариационной матрицы [22]:

где — количество ориентиров, обнаруженных роботом, — оценка координатыго ориентира. По аналогии с матрицей матрица отражает меру зависимости оценки координат ориентиров друг от друга [22].

Общее состояние системы определяется вектором, отражающим оценки положения робота и ориентиров, и ковариационной матрицей :

где — ковариационная матрица размерностью, отражающая зависимость между оценкой координат робота и оценками положения ориентиров [22].

При инициализации фильтра задаются следующие значения по умолчанию [22]:

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

На следующем этапе происходит обновление оценки состояния системы на основе одометрических данных в EKF (этап предсказания), т. е. оценка состояния обновляется на основе состояния системы в предыдущий момент времени, модели движения и одометрических данных. Оценка состояния, полученная на этом шаге, будет неточна из-за ошибок одометрической системы робота и из-за его движения (например, проскальзывания колес). Используя ориентиры, можно компенсировать эту неточность. Имея две оценки состояния, полученные разным путём, можно вычислить рассогласование между ними и использовать его для уточнения параметров системы. Данный процесс повторяется для каждого ориентира в отдельности. По мере изучения местности могут быть обнаружены новые ориентиры, которые требуется добавить в систему на заключительном этапе оценки состояния системы. Эти действия также повторяются для каждого нового найденного ориентира [22].

Полную модель EKF SLAM в доступной форме можно найти в работе [22].

На этом моменте, с учётом постановке задачи выше, можно более развёрнуто пояснить проблему, возникающую в связи с большим количеством ориентиров:

«Quadratic complexity is a consequence of the Gaussian representation employed by the EKF. The uncertainty of the SLAM posterior is represented as a covariance matrix containing the correlations between all possible pairs of state variables. In a two-dimensional world, the covariance matrix contains entries, where is the total number of landmarks in the map. Thus, it is easy to see how the memory required to store this covariance matrix grows with. Moreover, since the correlations between all pairs of state variables are maintained, any sensor observation incorporated into the EKF will necessarily affect all of the other state variables. To incorporate a sensor observation, the EKF algorithm must perform an operation on every element in the covariance matrix, which requires quadratic time» [49].

Также существует и вторая проблема:

«The second problem with EKF-based SLAM approaches is related to data association, the mapping between observations and landmarks. The SLAM problem is most commonly formulated given known data association […] In the real world, the associations between observations and landmarks are hidden variables that must be determined in order to estimate the robot pose and the landmark positions» [49].

Т.е. принципиально неустраняемые систематические ошибки рано или поздно приведут к расхождению фильтра (will eventually cause the filter to diverge).

Данный алгоритм был актуален до появления FastSLAM, в последние десятилетия XX-го века.