Github address: github.com/liuzhenboo/…

1. Probabilistic model of SLAM problem

1.1 Maximum posterior to least squares

In fact, the problem of SLAM is a state estimation problem, which is to predict the state quantity according to a series of observations. In general, the SLAM problem is established in the framework of probability theory.

To put it bluntly, SLAM is about solving a class of posterior probability problems:


p ( x ∣ z ) p(x|z)

Where XXX is the current state quantity of the system, ZZZ is the observed quantity related to the state quantity; After calculating the conditional probability, the observed value z into, can obtain p (x ∣ z = z) p (x | z = z) p (x ∣ z = z), which is what we need.

Expand according to probability formula:


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

Where the denominator is constant, it can be regarded as the normalized factor η\etaη, so it can also be written:


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

P (z ∣ x) p (z | x) p (z ∣ x) said in the condition of known XXX, ZZZ probability distribution; In calculate p (z ∣ x) p (z | x) p (z) ∣ x, z = Zz = Zz = z into, get p (z = z ∣ x) p (z = z | x) p (z = z ∣ x), which is also known as likelihood item, it says in the case of XXX to take different values, the probability of z = Zz = Zz = z.

P (x)p(x)p(x) is also known as prior, that is, what distribution XXX obeys before observation. This can be obtained from other information sources such as GPS, INERTIAL navigation, etc. If you do not know any prior distribution information in advance, it can be set to 1, indicating that you do not believe the prior information and only estimate according to the measurements used by the system.

Since each observation is independent, the above equation can be written as:

Take the negative logarithm to get:

If observed p (zi ∣ x) p (z_i | x) p (zi ∣ x) obey gaussian distribution N (hi (x), ∑ I) N (h_i (x), \ sum_i) N (hi (x), ∑ I); A priori p (x) p (x) p (x) obey N (xp, ∑ x) N (x_p, \ sum_ {x}) N (xp, ∑ x) is:

If there is no prior knowledge, it can be written as:

It is obvious that in the process of SLAM, new residuals are constantly added to the system, and the above least scalar problem is constantly expanding.

1.2 Nonlinear optimization solution

Assume that the state vector at the current moment is X and the dimension is N, and we express the residual in the form of a vector:

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

Here, we use Margens norm, and the loss function is defined as:

Ji Ji (x) = partial ei (x) partial xJ_i (x) = \ frac {\ partial e_i (x)} {\ partial x} Ji (x) = partial x partial ei (x), then you can get:

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]

Taylor expansion of the residual function can obtain the first-order approximation:

Then the loss function can be approximated as follows:


material 1 2 e ( x ) T Σ e – 1 e ( x ) + Delta t. x T J T Σ e – 1 e ( x ) + 1 2 Delta t. x T J T Σ e – 1 J Delta t. x \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

So the loss function is converted to a quadratic function about delta x delta x delta x, and if JT σ e−1JJ^T\Sigma _e^{-1}JJT σ e−1J positive definite, then the loss function has a minimum.

Take the first derivative of the above equation with respect to delta x\delta xδx and set it to zero, then:

It’s also often written as

The delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x delta x This method is known as the Gauss-Newton method.

However, in practice, solving the above equation requires the invertibility of HHH matrix, and I often encounter the irreversible situation of HHH in programming, so I used levenberg-Marquardt method in the actual code. It improves the Gauss-Newton method by adding a damping factor, as shown below:

Here I do not delve into the dynamic selection strategy of μ\muμ in the iteration, but simply give a fixed value of 0.1.

2. Question setting

A robot wearing some kind of radar sensor moving in 2D plane, how to get its own positioning through noisy sensor data?

Assumptions:

(1) There are several landmarks with different ID numbers in the 2D plane.

(2) The sensor is used to observe the information of landmark points, and the observation range is a circle with its own position as the origin and r as the radius. There are three observations. The first observation is the x value of landmark points in the sensor coordinate system within the detection range, which satisfies normal distribution. The second observation is the y value of landmark points in the sensor coordinate system within the detection range, which satisfies normal distribution. The third observation is the ID number of the signpost.

As shown below:

2.1 coordinate system

The sensor coordinate system is represented by SSS and the global coordinate system is represented by GGG. A map of all the punctuation = L (lx, ly) L = (l_x l_y) L = (lx, ly), the current sensor position P = (px, py, theta) = P (pressure, p_y \ theta) P = (px, py, theta), there are:

Among them:


[ c o s Theta. – s i n Theta. s i n Theta. c o s Theta. ] \left[\begin{matrix} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{matrix} \right]

2.2 Motion model

Set the input as U =(U1, U2,u3)u=(U_1, U_2, U_3) U =(U1, U2, U3), and take the point (U1 {U}_{1} U1, U2U_2U2) in the local coordinate system of the radar at the current moment as the position of the radar at the next moment:

{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]

Its azimuth changes as follows:

2.3 Observation Model

Observed a waypoint L status to (lxG ^ {{L} _ {x}} {G} lxG, lyG ^ {{L} _ {y}} {G} lyG), the current moment sensor position status to (pxG, pyG) (pressure ^ G, p_y ^ G) (pxG, pyG), observation equations are as follows:

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. Technical options

3.1 Front-end Tracing

There are two main purposes of front-end tracing:

(1) State data association

(2) Solve the initial value of the new state.

Here, the data association can be established according to the landmark id number in the observation information. The initial state estimation is mainly discussed here.

The algorithm idea is as follows: firstly, the estimation value of the old state is fixed, and then the error function of the new pose state is established through the data association results, and the initial estimation of the new pose state is obtained. Then, according to the observation equation, the initial estimate of the state of the new map point is obtained directly.

Suppose L_i is the state of a landmark point in the old state, whose estimated value is known as (lixG,liyG)(l_{ix}^G,l_{iy}^G)(lixG,liyG), Set the current state for Pj = new posture (PJX pjy, theta j) P_j = (p_ {jx}, p_ {jy}, \ theta_j) Pj = (PJX pjy, theta j), Observations before known as mij = m_ (mijx, mijy) {ij} = (m_ {ijx}, m_ {ijy}) mij = (mijx mijy) then has the following relationship: The current state for robot pose new P (pxG, pyG, theta) (pressure ^ G, p_y ^ G, \ theta) (pxG, pyG, theta), can be observed in the current time an old waypoint L, its status to (lxG (^ {{L} _ {x}} {G} (lxG, LyG) {{l} _ {y}} ^ {G}) lyG), the observation of M = (z1, z2) M = (z_1, z_2) M = (z1, z2) can build coarse optimization function below:


[ c o s Theta. – s i n Theta. s i n Theta. c o s Theta. ] – 1 ( [ l x G l y G ] – [ p x G p y G ] ) – [ z 1 z 2 ] ∣ ∣ 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

About state (pxG, pyG, theta) (pressure ^ G, p_y ^ G, \ theta) (pxG, pyG, theta) of Jacobi matrix of the:

-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]

Our goal is to estimate (pxG,pyG,θ)(p_x^G,p_y^G,\theta)(pxG,pyG,θ), successful tracking of a point can be added to a sub-Jacobian matrix, tracking more points, more accurate, but also more time, using the five-point method in the code, using LM algorithm, The initial value is set to the position state of the robot at the previous moment.

The front section is to provide an initial value, so enough is enough, and here the data correlation is determined, there is no external point, back-end optimization will fully compensate for the front-end deficiencies.

3.2 Sliding window optimization

3.2.1 figure structure

The relationship between SLAM states can be represented by a graph, as shown below:

The graph expands as the robot explores new environments, and we call it a state window.

Observations are within the set window zi = (alpha I, si) z_i = (\ alpha_i, s_i) zi = (alpha I, si), for all state traffic factor \ xi factor, NNN d, The residual item ei (factor) – (factor) = hi zie_i (\ xi) = h_i (\ xi) – z_iei (factor) – (factor) = hi zi: with LM method described above, are:

Assuming MMM observations, the above equation can be expanded as:
\frac {\partial e_i(\xi)} {\partial \xi_1} \dots \frac {\partial e_i(\xi)} {\partial \xi_n} \end{matrix}\right]

And so, as new observations continue to be made, We keep introducing new observations into the JiT σ EI −1JiJ_i^T\Sigma^{-1}_{e_I}J_iJiT σ EI −1Ji and – JiT Σ ei ei – 1 (factor) – J_i ^ T \ Sigma_ {e_i} ^ {1} e_i (\ xi) – JiT Σ ei ei – 1 (factor) to join on both sides to solve the equation, then iterative optimization.

The main content of this section is Ji(ξ)J_i(\xi)Ji(ξ):

\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}

Sub-jacobian matrix for robot position state:


[ partial e i Alpha. ( Is deduced ) partial p x G partial e i Alpha. ( Is deduced ) partial p y G partial e i Alpha. ( Is deduced ) partial Theta. partial e i s ( Is deduced ) partial p x G partial e i s ( Is deduced ) partial p y G partial e i s ( Is deduced ) partial Theta. ] 2 x 3 \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}

Sub-jacobian matrix for map point position states:


[ partial e i Alpha. ( Is deduced ) partial l x G partial e i Alpha. ( Is deduced ) partial l y G partial e i s ( Is deduced ) partial l x G partial e i s ( Is deduced ) partial l y G ] 2 x 2 \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}

The position of the two sub-Jacobian matrices in Ji(ξ)J_i(\xi)Ji(ξ) is determined by the order of sub-states in the state vector ξ\xiξ. It is necessary to specify the order of ξ\xiξ of each state quantity.

It is assumed that there are robot position states at m consecutive moments in the sliding window (P1G,P2G… PmG) (P_1 ^ G, P_2 ^ G, \ dots P_m ^ G) (P1G, P2G,… PmG), the state of the new map point observed at each moment III (note that the new map point) is (L1G(I),L2G(I)… , LniG (I)) (L ^ G_1 (I), L ^ G_2 (I), \ dots, L ^ G_ {n_i} (I)) (L1G (I), L2G (I),… ,LniG(I)), so the order of state quantities is specified as follows, of course, this is specified to facilitate the operation of matrix in later Marginalization:

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]

The following relationship equation defines the observation function (alpha s) T = h (PG, LG) (\ alpha, s) ^ T = h (P ^ G, L ^ G) (alpha s) T = h (PG, LG) :

{{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]

The following specific partial ei (factor) had partial PG \ frac {\ partial e_i (\ xi)}} {\ partial P ^ G partial PG partial ei (factor), and partial ei (factor) had partial LG \ frac {\ partial e_i (\ xi)} {\ partial L ^ G} partial LG partial ei (factor) :

\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 Shuerbu and marginalization

Obviously, with the passage of time, the system has more and more states, and the HHH matrix is bigger and bigger, which makes the calculation more and more. So the sliding window method is used to control the optimization scale within a definite interval.

One of the key aspects of the sliding window method is how to deal with discarded observations, whether to throw them away directly or to transform them into constraints on the amount of remaining states.

The intuitive idea is to leave the robot pose variable of the latest NNN frame and the state of map points associated with it, throw away the rest, and rebuild the new error equation. Obviously, you’re throwing away some of the constraints, you’re losing some of the information, so you’re using the marginalization technique to get the priors of the lost observations.

The strategy adopted here is: for the newly optimized sliding window, if NNN exceeds the specified maximum number of pose, the earliest pose state and all map point states related to it are marginalized as ξm\xi_mξm, and the remaining states as ξr\xi_rξr.

The least square problem of ξm\xi_mξm corresponding observation and ξ\xiξ is:

Also known as:

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]

Using Schuur’s complement, it can be obtained:

Thus we have a priori information about discarded observations:

In the next sliding window optimization, the observation in the sliding window and ξ xiξ constitute a new least squares problem:

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]

Plus prior is:

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 ensures system consistency

In fact, our above marginalization strategy already guarantees FEJFEJFEJ, because we marginalize the earliest pose state and all map points associated with it, which ensures that the error function will not be linearized at two different points. HpriorH_{prior}Hprior does not conflict with the current HHH.

4. System design

4.1 Data Types

Landmark: used to simulate the actual 2D environment and store various information about the environment.

Movemodel class: used to simulate robot movement, store the real position of robot movement, provided for observation class.

Measure: Stores and obtains measurement information.

Frame class: This is an abstract class designed for pose nodes. It has two main purposes. One is to store the current pose and the other is to establish a relationship with map points.

Mappoint class: Abstract class designed for Landmark. Its main purpose is to store the estimated location of map points and to establish a relationship with frame

Gaussnewton class: This class is a linear least square solver.

Draw class: this holds the address of the main abstract class and displays the desired data.

Slidewindowgraph class: The slideWindow class, which is the most important data member, uses the above data types to perform front-end data association and back-end slide-window optimization.

4.2 Code Logic

The core of the whole process of the system is to maintain the Slidewindowgraph class. Specifically, from main.py, the code is clear, the module is distinct.

5. Simulation results

Only front-end tracking, back-end optimization removed. Open slideWindow_graph.py and comment out self.optimize_graph () from def Update(self, measure)

With backend optimization, but without sliding Windows, the amount of state is saved. Def Optimize_graph(self) self.get_prior () and self.cut_window ();

Use back-end optimizations, using sliding Windows, but without prior information. Open slideWindow_graph.py and comment out self.get_prior () in def Optimize_graph(self).

Use backend optimization, use sliding Windows, use prior information. Its running results are as follows:

Each element in the figure explains that the red star is the fixed landmark in the environment, the blue point is the estimated value of landmark position in the current state window, the red square is the estimated value of robot pose in the current state window, and the blue square is the five state points used for tracking between the front and rear frames.

According to the simulation results, the global optimization without sliding window is the best. When using sliding window, the prior effect is not obvious. Using only the front five point tracking method is very poor.