Nonlinear optimization: establishment of slam model

SLAM mathematical model

Equation of motion

Set the robot in the process of driving , moment $t$ The pose of is $x_t$, Then the motion equation of the robot can be expressed by the following formula ：
$$x_k=f(x_{k-1},u_k,w_k)$$
among ,$u_k$ It is the control quantity, that is, the reading or input of the motion sensor . Generally, the encoder is used to obtain the travel speed of the robot （ Speed motion model ） Or use odometer to obtain mileage information （ Mileage model ） As input .$w_k$ Is the motion noise item .

The meaning of the equation of motion is , Through the position and posture information and control quantity of the robot at the last moment , It can estimate the current position and posture of the robot .

The observation equation

Vision SLAM The observation model of is usually a feature-based model , Then the map information is composed of some columns of road signs . use $y_1,y_2,\cdots ,y_N$ It indicates each road punctuation .

In the process of robot movement , The sensor will get the information of the surrounding road signs , Then the observation equation of the robot is as follows ：
$$z_{k,j}=h(y_j,x_k,v_{k,j})$$
among ,$v_{k,j}$ Is the observation noise item .

The meaning of observation equation is , In the road sign information $y_j$ And robot posture $x_k$ Next , The observation data of the sensor is $z_{k,j}$

State estimation problem

thus , obtain SLAM Model of ：
$$\{\begin{array}{l}{x}_k=f(x_{k-1},u_k,w_k)\\ z_{k,j}=h(y_j,x_k,v_{k,j})\end{array}$$
among , The current pose of the robot in the equation of motion $x_k$ May by $T_k\in SE(3)$ Describe . The observation equation is modeled by the camera pinhole model ：
$$s{z}_{k,j}=K(R_k{y}_j+t_k)$$
That is, at the moment $t$ Next , The camera sensor is in the robot position $x_k$ It's about , At this time, the road signs $y_j$ The observation made corresponds to the pixel position on the image $z_{k,j}$ It's about . among ,$s$ Is the scale factor ,$K$ For camera internal reference ,$R_k\in SO(3)$ Move peacefully $t_k$ It forms the external parameter matrix of the camera $T_k$.

Usually , Increase with zero as the mean $R_k、Q_{k,j}$ Is the Gaussian distribution of variance $w_k,v_{k,j}$ As motion noise and observation noise ：
$$w_k\sim \mathcal{N}(0,R_k)v_{k,j}\sim \mathcal{N}(0,Q_{k,j})$$
According to the model , Adopt control quantity $u$（ Usually speed information or odometer information ） And sensor observation $z$ Infer the pose of the robot $x$ And map information $y$

There are two ways to deal with the problem of state estimation , One is called filter algorithm , One is batch Estimation Algorithm . The filter algorithm holds an estimate of the current moment , And update the estimation with new sensor data . The batch estimation calculation method accumulates the data into a small batch , Unified processing and estimation .

The commonly used filter algorithm is extended Kalman Algorithm （EKF）、 Unscented Kalman Algorithm （UKF） etc. . Relatively speaking , The filter algorithm only cares about the state estimator at the current moment , The batch estimation is optimized in a larger range . It is generally believed that batch estimation is better than filter estimation .

Batch estimation

For robot motion , Consider from 1 To N The position and posture of the robot at any time $x_t$ And the corresponding landmark information $y_j$：
$$x=\{x_1,\cdots ,x_N\}y=\{y_1,\cdots ,y_M\}$$
Then input $u$、 Sensor observation data $z$ The robot shape is estimated as follows under the condition of ：
$$p(x,y|z,u)=\frac{p(z,u|x,y)p(x,y)}{p(z,u)}=\eta p(z,u|x,y)p(x,y)$$
Bayesian law unfolds , Get the content on the right , among $\eta$ Is the normalization factor ;$p(z,u|x,y)$ by likelihood （Likehood）;$p(x,y)$ by transcendental （Prior）;$p(x,y|z,u)$ by Posttest .

It is difficult to solve the posterior distribution directly , Turn to solving an optimal state estimation , Maximize the posterior probability ：
$$(x,y{)}_{MAP}^{\ast }=arg\max p(x,y|z,u)=arg\max p(z,u|x,y)p(x,y)$$
Normalization factor $\eta$ Independent of distribution , Classified as a constant term , Ignore . From the above equation ： To maximize the posterior probability is to solve the most Large likelihood estimation ：
$$(x,y{)}_{MLE}^{\ast }=arg\max p(z,u|x,y)$$
Likelihood means ： Under the current robot posture , Possible observational data .

Maximum likelihood estimation ： In what state does the robot pose , Most likely to get the currently observed data .

Maximum likelihood estimation

Observation model

$$(x,y{)}_{MLE}^{\ast }=argmaxp(z,u|x,y)$$

For an observation ：
$$z_{k,j}=h(y_j,x_k)+v_{k,j}$$
Modeling observation noise with Gaussian distribution $v_{t,j}\sim \mathcal{N}(0,Q_{k,j})$, Then we can see ：
$$p(z_{k,j}|x_k,y_j)\sim \mathcal{N}(h(y_j,x_k),Q_{k,j})$$
To solve the maximum likelihood estimation, you can use Minimize the negative logarithm In a way that .

Minimum negative logarithm

For an arbitrary high latitude Gaussian distribution $x\sim \mathcal{N}(\mu ,\Sigma )$：
$$p(x)=\frac{1}{\sqrt{(2\pi {)}^{N}det(\Sigma )}}exp(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))$$
Take the negative natural logarithm ：
$$-\ln p(x)=\frac{1}{2}\ln ((2\pi {)}^{N}det(\Sigma ))+\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )$$
Monotonically increasing property of logarithmic function , You can get right $p(x)$ Find the maximization, that is, the negative logarithm $-\ln p(x)$ Minimize .

The first term on the right of the above expression is a constant term , Then the maximum likelihood estimation of the state can be obtained by solving the minimum term of the second term on the right .

Maximum likelihood estimation

Bring the observation equation into the above model ：
$$\begin{array}{rl}(x_k,y_j{)}^{\ast }& =arg\max \mathcal{N}(h(y_j,x_k),Q_{k,j})\\ & =arg\min \left(\right(z_{k,j}-h(x_k,y_j){)}^T{Q}_{k,j}^{-1}(z_{k,j}-h(x_k,y_j)))\end{array}$$
Call the mahalanobi distance described by the above formula , Mahalanobis distance . The expression is equivalent to minimizing the noise term ,$Q_{k,j}^{-1}$ It is called information matrix , That is, the inverse of the covariance matrix of Gaussian distribution .

Batch estimation

Consider the data at the time of batch , Suppose the control quantity input at each time in the batch $u$、 Sensor observation data $z$ Are independent of each other , That is, the control quantities are independent of each other , The observations are independent of each other , Available ：
$$p(z,u|x,y)=\prod _{k}p(u_k|x_{k-1},x_k)\prod _{k,j}p(z_{k,j}|x_k,y_j)$$
thus , It can independently handle the movement at all times 、 observation . Now define the motion control quantity 、 The errors between the sensor observation and the model are as follows ：
$$\begin{array}{rl}{e}_{u,k}& =x_k-f(x_{k-1},u_k)\\ e_{z,k,j}& =z_{k,j}-h(x_k,y_j)\end{array}$$
thus , The objective function is as follows ：
$$\min J(x,y)=\sum _k{e}_{u,k}^T{R}_k^{-1}{e}_{u,k}+\sum _k\sum _j{e}_{z,k,j}^T{Q}_{k,j}^{-1}{e}_{z,k,j}$$
here , In practice, the minimum negative logarithm is used to solve the maximum likelihood estimation problem , Therefore, the cumulative multiplication becomes cumulative in the form of negative logarithm .

Minimize the objective function to nonlinear The least squares problem .