Python пишет систему 2D-SLAM на основе нелинейной оптимизации (уже с открытым исходным кодом)

Адрес кода на гитхабе:GitHub.com/Лю Чжэньбо О/…

1. Вероятностная модель задачи SLAM

1.1 Максимум апостериорно к методу наименьших квадратов

Проблема SLAM на самом деле является проблемой оценки состояния, которая заключается в выводе количества состояния на основе серии наблюдений; в общем, мы строим проблему SLAM в рамках теории вероятностей.

Грубо говоря, SLAM должен решить такую задачу апостериорной вероятности:

p(x|z)

в $x$ текущее состояние системы, $z$ — наблюдаемая величина, связанная с величиной состояния; после получения этой условной вероятности наблюдаемая величина равна $Z$ принесите, вы можете получить $p(x|z=Z)$ , то есть то распределение, которое нам нужно.

Разложить по формуле вероятности:

p(x|z=Z)=\frac{p(z=Z|x)p(x)}{p(z=Z)}

где знаменатель - константа, как нормировочный коэффициент $\eta$ , поэтому его также можно записать как:

p(x|z=Z)=\eta{p(z=Z|x)p(x)}

$p(z|x)$ выраженный в $x$ известен, $z$ Распределение вероятности ; при нахождении $p(z|x)$ После этого поставить $z=Z$ вносить, получать $p(z=Z|x)$ , который также известен как термин правдоподобия, он выражается в $x$ В случае принятия различных значений $z=Z$ Вероятность.

$p(x)$ Также известен как априорный, то есть до наблюдения $x$ Какому распределению он следует. Это можно получить из других источников информации, таких как GPS, инерциальная навигация и т. д. Если какая-либо априорная информация о распределении неизвестна заранее, ее можно установить на 1, что означает, что априорной информации не верят, и оценка основана только на измерениях, используемых системой.

Поскольку каждое наблюдение не зависит друг от друга, приведенную выше формулу можно записать в виде:

Отрицательное логарифмирование дает:

Если сделать предположение о наблюдении $p(z_i|x)$ следовать распределению Гаусса $N (h_i (x), \ сумма_i)$ ;априори $p(x)$ подчиниться $N(х_р, \сумма_{х})$ Тогда есть:

Без предварительных знаний это можно записать так:

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

1.2 Решение для нелинейной оптимизации

Пусть вектор текущего состояния равен x, а размерность n, выразим невязку в виде вектора:

e_1(x) \\ ... \\ e_m(x) \end{matrix}\right]

Здесь мы используем норму Махаланобиса, а функция потерь определяется как:

Помните $J_i(x)=\frac {\partial e_i(x)} {\partial x}$ , то можно получить:

J_1(x) \\ ... \\ J_m(x) \end{matrix}\right]

\frac {\partial e_i(x)} {\partial x_1} ... \frac {\partial e_i(x)} {\partial x_n} \end{matrix}\right]

Разложение Тейлора функции невязки дает приближение первого порядка:

Тогда функция потерь может быть аппроксимирована следующим образом:

\approx \frac{1}{2}{e(x)}^T\Sigma _e^{-1}e(x)+{\delta x}^T J^T \Sigma_e^{-1}e(x)+\frac{1}{2}{\delta x}^TJ^T\Sigma_e^{-1}J\delta x

Таким образом, функция потерь преобразуется примерно в $\delta x$ квадратичная функция от , и если $J^T\Sigma _e^{-1}J$ положительно определенный, то функция потерь имеет минимальное значение.

О приведенной выше формуле $\delta x$ Взяв первую производную и приравняв ее к нулю, получим:

также часто пишется как

Таким образом, вы можете получить приращение $\delta x$ , таким образом, текущее решение может непрерывно оптимизироваться до тех пор, пока ошибка не станет меньше порога. Этот метод называется методом Гаусса-Ньютона.

Однако на практике для решения вышеприведенного уравнения требуется $H$ Матрица обратима, и в программировании я часто сталкиваюсь $H$ Ситуация необратима, поэтому я использую метод Левенберга-Марквардта в реальном коде. Он улучшает метод Гаусса-Ньютона и добавляет коэффициент демпфирования следующим образом:

Я не углубляюсь в итерации здесь $\mu$ Стратегия динамического выбора просто дает фиксированное значение 0,1.

2. Постановка задачи

Робот носит какой-то радарный датчик и движется в 2D-плоскости.Как получить собственное позиционирование по данным датчика с шумом?

Предполагать:

(1) В 2D-плоскости есть несколько ориентиров с разными идентификационными номерами.

(2) Датчик используется для наблюдения за информацией о точках дорожной разметки.Диапазон наблюдения представляет собой круг с собственным положением в качестве начала координат и r в качестве радиуса. Существует три наблюдаемых величины: первая наблюдаемая величина — это значение x опорных точек в диапазоне обнаружения в системе координат датчика, удовлетворяющее нормальному распределению, вторая наблюдаемая величина — это значение y опорных точек в области обнаружения. в системе координат датчика , удовлетворяющей нормальному распределению, третье наблюдение – это номер id точки-ориентира.

Как показано ниже:

在这里插入图片描述

2.1 Система координат

Для системы координат датчика $S$ Представлено, глобальная система координат использует $G$ Выражать. На карте есть знак препинания $L=(l_x,l_y)$ , текущее положение датчика $P=(p_x,p_y,\theta)$ , то есть:

в:

\left[\begin{matrix} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{matrix} \right]

2.2 Модель движения

Пусть сумма ввода будет $u=(u_1,u_2,u_3)$ , текущий момент в локальной системе координат РЛС ( ${u}_{1}$ , $u_2$ ) точка как положение радара в следующий момент:

{x} \\ {y} \end{array} \right] = \left[\begin{array}{cc} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right] \left[\begin{array}{cc} {u}_{1} \\ {u}_{2} \end{array}\right] + \left[\begin{array}{cc} {x} \\ {y} \end{array} \right]

Его ориентация меняется как:

2.3 Модель наблюдения

Предположим, что наблюдаемое состояние контрольной точки L равно ( ${{l}_{x}}^{G}$ , ${{l}_{y}}^{G}$ ), текущее состояние датчика $(p_x^G,p_y^G)$ , уравнение наблюдения выглядит следующим образом:

z_1 \\ z_2 \end{array} \right] = {\left[\begin{array}{cc} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]}^{-1} \left(\left[\begin{array}{cc} {{l}_{x}^{G}} \\ {{l}_{y}^{G}} \end{array} \right] - \left[\begin{array}{cc} {p_x^G} \\ {p_y^G} \end{array} \right]\right)+ \left[\begin{matrix} v_1 \\ v_2 \end{matrix}\right]

3. Технические решения

3.1 Отслеживание внешнего интерфейса

Есть две основные цели внешнего отслеживания:

(1) Ассоциация данных о состоянии

(2) Найдите начальное значение нового состояния.

Связь данных здесь может быть установлена в соответствии с идентификационным номером ориентира в информации наблюдения; здесь мы в основном обсуждаем оценку начального значения состояния.

Идея алгоритма: сначала зафиксируйте оценочное значение старого состояния, а затем установите функцию ошибки для нового состояния позы робота через результаты ассоциации данных и получите начальную оценку новой позы; затем напрямую получите новую позу по новой позе и уравнению наблюдения Начальная оценка состояния точки карты.

Предположим, что L_i — это состояние ориентира в старом состоянии, оценка которого известна как $(l_{ix}^G,l_{iy}^G)$ , пусть текущее состояние новой позы будет $P_j = (p_{jx},p_{jy},\theta_j)$ , предыдущее наблюдение известно как $m_{ij}=(m_{ijx},m_{ijy})$ Тогда имеет место следующая зависимость: Пусть текущая поза нового робота в состоянии P будет $(p_x^G,p_y^G,\theta)$ , старый ориентир L, который можно наблюдать в текущий момент, его состояние $({{l}_{x}}^{G}$ , ${{l}_{y}}^{G})$ , что наблюдается как $M=(z_1,z_2)$ Тогда можно построить следующую грубую оптимизационную функцию:

{\left[\begin{array}{cc} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]}^{-1} \left(\left[\begin{array}{cc} {{l}_{x}^{G}} \\ {{l}_{y}^{G}} \end{array} \right] - \left[\begin{array}{cc} {p_x^G} \\ {p_y^G} \end{array} \right]\right) -\left[\begin{array}{cc} z_1 \\ z_2 \end{array} \right] ||_2

О статусе $(p_x^G,p_y^G,\theta)$ Матрица Якоби:

-cos\theta & -sin\theta & -(l_x^G-p_x^G)sin\theta+(l_y-p_y)cos\theta \\ sin\theta &-cos\theta &-(l_x^G-p_x^G)cos\theta-(l_y-p_y)sin\theta \end{matrix}\right]

Наша цель оценить $(p_x^G,p_y^G,\theta)$ , для успешного отслеживания точки можно добавить матрицу субякобиана.Чем больше точек отслеживается, тем точнее, но и больше времени.В коде используется пятиточечный метод и используется алгоритм LM. Начальное значение устанавливается в состояние положения робота в предыдущий момент.

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

3.2 Оптимизация скользящего окна

3.2.1 Структура диаграммы

Отношения между состояниями SLAM можно представить в виде графика, как показано на следующем рисунке:

在这里插入图片描述

По мере того, как робот продолжает исследовать новые среды, этот граф продолжает расширяться, и мы называем его здесь окном состояния.

установить наблюдение в окне $z_i=(\alpha_i,s_i)$ , общее количество состояний равно $\xi$ , $n$ размерность, то остаточный член $e_i(\xi)=h_i(\xi)-z_i$ : Рассчитанные с использованием введенного ранее метода LM, есть:

Предположим, есть $m$ наблюдений, то приведенную выше формулу можно расширить следующим образом:

\frac {\partial e_i(\xi)} {\partial \xi_1} \dots \frac {\partial e_i(\xi)} {\partial \xi_n} \end{matrix}\right]

Таким образом, по мере того, как мы продолжаем получать новые наблюдения, мы продолжаем вводить новые наблюдения в $J_i^T\Sigma^{-1}_{e_i}J_i$ и $-J_i^T\Sigma_{e_i}^{-1}e_i(\xi)$ Он добавляется к обеим частям уравнения решения для итеративной оптимизации.

Основное содержание этого раздела состоит в том, чтобы $J_i(\xi)$ :

\frac {\partial e_i(\xi)} {\partial \xi_1} \dots \frac {\partial e_i(\xi)} {\partial \xi_n} \end{matrix}\right] =\left[\begin{matrix} 0 & \dots & \frac {\partial e_i(\xi)} {\partial P^G} & \dots & \frac {\partial e_i(\xi)} {\partial L^G} & \dots & 0 \end{matrix}\right]_{2\times n}

Матрица субякобиана для состояний положения робота:

\left[\begin{matrix} \frac {\partial e_i^{\alpha}(\xi)} {\partial p_x^G} & \frac {\partial e_i^{\alpha}(\xi)} {\partial p_y^G} & \frac {\partial e_i^{\alpha}(\xi)} {\partial \theta} \\ \frac {\partial e_i^{s}(\xi)} {\partial p_x^G} & \frac {\partial e_i^{s}(\xi)} {\partial p_y^G} & \frac {\partial e_i^{s}(\xi)} {\partial \theta} \end{matrix}\right]_{2\times 3}

Субякобиановая матрица для состояний точек карты:

\left[\begin{matrix} \frac {\partial e_i^{\alpha}(\xi)} {\partial l_x^G} & \frac {\partial e_i^{\alpha}(\xi)} {\partial l_y^G} \\ \frac {\partial e_i^{s}(\xi)} {\partial l_x^G} & \frac {\partial e_i^{s}(\xi)} {\partial l_y^G} \end{matrix}\right]_{2\times 2}

Что касается вышеупомянутых двух субякобиевых матриц в $J_i(\xi)$ Позиция размещения в основана на векторе состояния $\xi$ Определяется порядок, в котором в нем располагаются подсостояния. Здесь необходимо сначала указать, что каждая величина состояния находится в $\xi$ в порядке размещения.

Предположим, что в скользящем окне есть состояния положения робота в m последовательных моментов времени. $(P_1^G,P_2^G,\dots P_m^G)$ , каждый момент $i$ Состояние наблюдаемой новой точки карты (обратите внимание на новую точку карты) $(L^G_1(i),L^G_2(i),\dots,L^G_{n_i}(i))$ , то порядок величин состояния указывается следующим образом.Конечно, эта спецификация предназначена для удобства матричных операций в более поздней маргинализации:

P_1^G \\ L_1^G(1) \\ L_2^G(1) \\ \vdots \\ L_{n_1}^G(1) \\ P_2^G \\ L_1^G(2) \\ L_2^G(2) \\ \vdots \\ L_{n_2}^G(2) \\ \vdots \\ P_m^G \\ L_1^G(m) \\ L_2^G(m) \\ \vdots \\ L_{n_m}^G(m) \\ \end{matrix} \right]= \left[\begin{matrix} \xi_1 \\ \xi_2 \\ \vdots \\ \xi_m \end{matrix}\right]

Следующее уравнение определяет функцию наблюдения $(\alpha,s)^T=h(P^G,L^G)$ :

{{l}_{x}^{S}} \\ {{l}_{y}^{S}} \end{array} \right] = {\left[\begin{array}{cc} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]}^{-1} \left(\left[\begin{array}{cc} {{l}_{x}^{G}} \\ {{l}_{y}^{G}} \end{array} \right] - \left[\begin{array}{cc} {p_x^G} \\ {p_y^G} \end{array} \right]\right)+ \left[\begin{matrix} v_1 \\ v_2 \end{matrix}\right]

Следующие особые требования $\frac{\partial e_i(\xi)}{\partial P^G}$ и $\frac{\partial e_i(\xi)}{\partial L^G}$ :

\frac{\partial h_{ix}}{\partial p^G_x} & \frac{\partial h_{ix}}{\partial p^G_y} & \frac{\partial h_{ix}}{\partial \theta} \\ \frac{\partial h_{iy}}{\partial p^G_x} & \frac{\partial h_{iy}}{\partial p^G_y} & \frac{\partial h_{iy}}{\partial \theta} \end{matrix}\right]=\left[\begin{matrix} -cos\theta & -sin\theta & {(p_x^G-l_x^G)sin\theta+(l_y^G-p_y^G)cos\theta} \\ sin\theta & -cos\theta & {(p_x^G-l_x^G)cos\theta+(p_y^G-l_y^G)sin\theta} \end{matrix}\right]

\frac{\partial h_{ix}}{\partial l^G_x} & \frac{\partial h_{ix}}{\partial l^G_y} \\ \frac{\partial h_{iy}}{\partial l^G_x} & \frac{\partial h_{iy}}{\partial l^G_y} \end{matrix}\right]= \left[\begin{matrix} cos\theta & sin\theta \\ -sin\theta & cos\theta \end{matrix}\right]

3.2.2 Шульбулл и маргинализация

Очевидно, что с течением времени состояние системы становится все больше и больше, $H$ Чем больше массив, тем больше объем вычислений, поэтому здесь используется метод скользящего окна для управления масштабом оптимизации в пределах определенного интервала.

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

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

Принятая здесь стратегия такова: для скользящего окна, которое только что было оптимизировано, если $n$ Если максимальное количество поз превышает указанное число, то самое раннее состояние позы и все состояния точек карты, которые имеют отношение наблюдения с ним, маргинализируются и записываются как $\xi_m$ , остальные оставшиеся состояния записываются как $\xi_r$ .

$\xi_m$ Соответствующие наблюдения и $\xi$ Сформулированная задача наименьших квадратов:

Это:

H_{mm} & H_{mr} \\ H_{rm} & H_{rr} \end{matrix}\right]) \left[\begin{matrix} \delta \xi_m \\ \delta \xi_r \end{matrix}\right] = \left[\begin{matrix} b_m \\ b_r \end{matrix}\right]

С помощью Schurbull мы можем получить:

Это дает нам априорную информацию в отброшенных наблюдениях:

При следующей оптимизации скользящего окна наблюдения в скользящем окне такие же, как и в скользящем окне. $\xi$ Новая задача наименьших квадратов формируется как:

H_{rr} & H_{r,new} \\ H_{new,r} & H_{new,new} \end{matrix}\right]) \left[\begin{matrix} \delta \xi_r \\ \delta \xi_{new} \end{matrix}\right] = \left[\begin{matrix} b_r \\ b_{new} \end{matrix}\right]

Добавление предыдущего:

H_{rr} & H_{r,new} \\ H_{new,r} & H_{new,new} \end{matrix}\right] + H_{prior}) \left[\begin{matrix} \delta \xi_r \\ \delta \xi_{new} \end{matrix}\right] = \left[\begin{matrix} b_r \\ b_{new} \end{matrix}\right] + b_{prior}

3.2.3 FEJ обеспечивает согласованность системы

На самом деле, наша стратегия маргинализации, описанная выше, уже гарантировала $FEJ$ , потому что мы маргинализируем самое раннее состояние позы и все состояния точек карты, которые имеют с ним отношения наблюдения, что гарантирует, что функция ошибки не будет линеаризована в двух разных точках, что также гарантирует, что предыдущий член $H_{prior}$ с текущим $H$ не будет конфликтовать.

4. Дизайн системы

4.1 Типы данных

Класс ориентира: используется для моделирования фактической 2D-среды и хранения различной информации об окружающей среде.

Класс Movemodel: используется для имитации движения робота, сохранения реального положения движения робота и предоставления его классу наблюдения.

Класс меры: хранение информации об измерении и получение информации об измерении.

Класс Frame: это абстрактный класс, предназначенный для узлов поз. У него две основные цели: одна — хранить текущую позу, а другая — устанавливать связи с точками карты.

Класс Mappoint: абстрактный класс, предназначенный для ориентиров, который имеет две основные цели: одна — хранить предполагаемое местоположение точки карты, другая — устанавливать связь с фреймом.

Класс Гауссньютона. Этот класс представляет собой линейный решатель метода наименьших квадратов.

Класс Draw: он хранит адрес основного абстрактного класса и отображает нужные данные.

Класс Slidewindowgraph: класс скользящего окна является наиболее важным членом данных.Он использует указанные выше типы данных для завершения внешней ассоциации данных и внутренней оптимизации скользящего окна.

4.2 Логика кода

Ядром всего процесса системы является поддержка класса Slidewindowgraph, В частности, начиная с main.py, код понятен, а модули различны.

5. Результаты моделирования

Только внешнее отслеживание, внутренняя оптимизация удалена. Откройте файл slidewindow_graph.py, закомментируйте self.Optimize_graph() в функции def Update(self, Measure), и результат будет таким:

在这里插入图片描述

Используется внутренняя оптимизация, но не используется скользящее окно, а количество состояний сохраняется. Откройте файл slidewindow_graph.py и закомментируйте self.Get_prior() и self.Cut_window() в функции def Optimize_graph(self). Результат:

在这里插入图片描述

Используйте внутреннюю оптимизацию, используя скользящие окна, но без предварительной информации. Откройте файл slidewindow_graph.py и закомментируйте self.Get_prior() в функции def Optimize_graph(self).Результат:

在这里插入图片描述

Используйте внутреннюю оптимизацию, используйте скользящие окна, используйте предварительную информацию. Результатом его работы является:

在这里插入图片描述

Описание каждого элемента на рисунке: красная звезда — фиксированный ориентир в окружающей среде, синяя точка — расчетное значение положения ориентира в текущем окне состояния, красный квадрат — расчетное значение положения робота в текущем окно состояния, а синий квадрат — это кадр между предыдущим и предыдущим кадрами.Пять точек состояния для отслеживания времени.

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