图优化slam以及非线性最小二乘

滤波方法不能处理大场景，在线slam

图优化方法可以处理大场景，完全slam

1.Graph—based SLAM

• 用一个图（Graph）来表示SLAM问题
• 图中的节点来表示机器人的位姿
• 两个节点之间的边表示两个位姿的空间约束
• Graph—based SLAM：构建图，并且找到一个最优的配置，让预测与观测的误差最小（非线性最小二乘）

2.非线性最小二乘(Non-Linear Least Square)

1.要解决的问题

• 给定一个系统，其状态方程有f(x)=z描述，其中：

​ x为该系统的状态向量——即需要估计的值

​ f(x)是一个非线性的映射函数

​ 状态向量x，可以通过非线性函数f(x)映射得到z

​ z表示系统的观测值，可以通过传感器进行直接观测

• 给定该系统的n个会有噪声的观测值$(z_1,...,z_2)$，估计状态向量x，使其经过f(x)映射之后的预测值和观测值得误差最小
• 跟非线性最小二乘基本相同，不同之处在于f(x)是一个非线性函数

2.过程

• x为机器人的位置
• f(x)为传感器的观测模型（似然场模型或者重投影模型）
• z为传感器的观测值，激光数据或者图像特征点
• 找到最优的x，让预测和观测的误差最小

3.误差函数

• 目标为最小化预测和观测的差，因此误差即为预测和观测的差

$$e_i(x)=f_i(x)-z_i$$

• 假设误差服从高斯分布，因此其对应的信息矩阵为${\sum}_i $，因此该观测值误差的平方定义为：

$$E_i(x)=e_i(x{)}^T{\sum }_i{e}_i(x)$$

• 因此，非线性最小二乘的目标函数为：

$$\begin{gathered} F(x)=\sum E_i(x)=\sum e_i(x{)}^T{\sum }_i{e}_i(x) \\ \underset{x}{\min }F(x) \end{gathered}$$

4.解决问题

• 目标函数：

$$\underset{x}{\min }F(x)$$

• 直接想法：求F(X)关于变量x的导数，另其等于0，求解方程即可
• 对于线性问题，该方法可以正确，但是对于非线性问题不正确
• F(x)为关于x的非线性方程，能否把其化为关于x的线性方程——泰勒展开

5.线性化

• F(x)是关于x的非线性函数的原因是：误差函数$e_i(x)$是一个非线性函数。因此直接对误差函数$e_i(x)$进行线性化即可。

$$e_i(x+△x)=e_i(x)+J_i(x)△x$$

• 其中J为映射函数对状态向量x的导数，称之为Jacobian矩阵（雅克比矩阵）

$$J_i(x)=(\frac{\partial f_i(x)}{△x{x}_1},\frac{\partial f_i(x)}{\partial x_2},....,\frac{\partial f_i(x)}{\partial x_n})$$

• 因此函数F（x）的可化解为：

$$F(x+△x)=e_i(x+△x{)}^T{\sum }_i{e}_i(x+△x)$$

• F(x)的化解：
  

• F(x+△x)为关于变量△x的二次函数

• F(x+△x)的极值可通过令其关于△x的导数等于0求解得到：

$$\begin{gathered} \frac{\partial F(x+△x)}{\partial △x}=2b+2H△x=0 \\ H△x=-b \\ △x^{\ast }=-H^{-1}b \end{gathered}$$

6.流程

①线性化误差函数
$$e_i(x+△x)=e_i(x)+J_i(x)△x$$
②构建线性系统
$$\begin{gathered} b_T=\sum (e_i^T{\sum }_i{J}_i) \\ H=\sum (J_i^T{\sum }_i{J}_i) \end{gathered}$$
③求解线性系统

$$△x^{\ast }=-H^{-1}b$$

④更新解，并不断迭代直至收敛

$$x=x+△x^{\ast }$$