摘要：1.SLAM问题定义同时定位与建图的本质是一个估计问题，它要求移动机器人利用传感器信息实时地对外界环境结构进行估计，并且估算出自己在这个环境中的位置，Smith和Cheeseman在上个世纪首次将EKF估计方法应用到SLAM。
1. SLAM问题定义
同时定位与建图（SLAM）的本质是一个估计问题，它要求移动机器人利用传感器信息实时地对外界环境结构进行估计，并且估算出自己在这个环境中的位置，Smith 和Cheeseman在上个世纪首次将EKF估计方法应用到SLAM。
以滤波为主的SLAM模型主要包括三个方程：
1）运动方程：它会增加机器人定位的不确定性
2）根据观测对路标初始化的方程：它根据观测信息，对新的状态量初始化。
3）路标状态对观测的投影方程：根据观测信息，对状态更新，纠正，减小不确定度。
2. EKF-SLAM维护的数据地图
系统状态x是一个很大的向量，它包括机器人的状态和路标点的状态，
[ mathbf{x}=left[egin{array}{c} {mathcal{R}} \ {mathcal{M}} end{array} ight]=left[egin{array}{c} {mathcal{R}} \ {mathcal{L}_{1}} \ {vdots} \ {mathcal{L}_{n}} end{array} ight] ]
其中({mathcal{R}})是机器人状态，({mathcal{M}} = left({mathcal{L}_{1}}， dots，{mathcal{L}_{n}} ight))是n个当前已经观测过的路标点状态集合。
在EKF中，x被认为服从高斯分布，所以，EKF-SLAM的地图被建模为x的均值(overline{x})与协方差(mathbf{P}),
[ overline{mathbf{x}}=left[frac{overline{mathcal{R}}}{mathcal{M}} ight]=left[egin{array}{c} {overline{mathcal{R}}} \ {overline{mathcal{L}}_{1}} \ {vdots} \ {overline{mathcal{L}}_{n}} end{array} ight] quad mathbf{P}=left[egin{array}{cc} {mathbf{P}_{mathcal{R} mathcal{R}}} & {mathbf{P}_{mathcal{R} mathcal{M}}} \ {mathbf{P}_{mathcal{M R}}} & {mathbf{P}_{mathcal{M M}}} end{array} ight]=left[egin{array}{cccc} {mathbf{P}_{mathcal{R R}}} & {mathbf{P}_{mathcal{R} mathcal{L}_{1}}} & {cdots} & {mathbf{P}_{mathcal{R} mathcal{L}_{n}}} \ {mathbf{P}_{mathcal{L}_{1} mathcal{R}}} & {mathbf{P}_{mathcal{L}_{1} mathcal{L}_{1}}} & {cdots} & {mathbf{P}_{mathcal{L}_{n}} mathcal{L}_{n}} \ {vdots} & {vdots} & {ddots} & {vdots} \ {mathbf{P}_{mathcal{L}_{n} mathcal{R}}} & {mathbf{P}_{mathcal{L}_{n} mathcal{L}_{1}}} & {cdots} & {mathbf{P}_{mathcal{L}_{n} mathcal{L}_{n}}} end{array} ight] ]
因此EKF-SLAM的目标就是根据运动模型和观测模型及时更新地图量(left{overline{mathbf{x}},mathbf{P} ight})
3. EKF-SLAM算法实施
3.1 地图初始化
显而易见，在机器人开始之前是没有任何路标点的信息的，因此此时地图中只有机器人自己的状态信息，因此({n} = 0,{x} = {mathcal{R}})。SLAM中经常把机器人初始位姿认为是地图的原点，其初始协方差可以按实际情况设定，比如：
[ overline{mathbf{x}}=left[egin{array}{l} {x} \ {y} \ { heta} end{array} ight]=left[egin{array}{l} {0} \ {0} \ {0} end{array} ight] quad mathbf{P}=left[egin{array}{lll} {0} & {0} & {0} \ {0} & {0} & {0} \ {0} & {0} & {0} end{array} ight] ]
3.2 运动模型
在EKF中如果x是状态量，u是控制输入，n是噪声变量，那么我们可以得到一般的状态更新函数：
[mathbf{x} leftarrow f(mathbf{x}, mathbf{u}, mathbf{n}) ]
EKF的预测步骤为：
[egin{array}{l} {overline{mathbf{x}} leftarrow f(overline{mathbf{x}}, mathbf{u}, 0)} \ {mathbf{P} leftarrow mathbf{F}_{mathbf{x}} mathbf{P} mathbf{F}_{mathbf{x}}^{ op}+mathbf{F}_{mathbf{n}} mathbf{N} mathbf{F}_{mathbf{n}}^{ op}} end{array} ]
其中雅克比矩阵(mathbf{F}_{mathbf{x}}=frac{partial f(overline{mathbf{x}}, mathbf{u})}{partial mathbf{x}})，(mathbf{F}_{mathbf{n}}=frac{partial f(overline{mathbf{x}}, mathbf{u})}{partial mathbf{n}})，({mathbf{N}})是随机变量n的协方差。
但是在EKF-SLAM中，只有一部分状态({mathcal{R}})是随运动而改变的，其余所有路标状态不改变，所以SLAM的运动方程为：
[egin{array}{l} {mathcal{R} leftarrow f_{mathcal{R}}(mathcal{R}, mathbf{u}, mathbf{n})} \ {mathcal{M} leftarrow mathcal{M}} end{array} ]
因此我们可以得到稀疏的雅克比矩阵：
[ mathbf{F}_{mathbf{x}}=left[egin{array}{cc} {frac{partial f_{mathcal{R}}}{partial mathcal{R}}} & {0} \ {0} & {mathbf{I}} end{array} ight] quad mathbf{F}_{mathbf{n}}=left[egin{array}{c} {frac{partial f_{mathcal{R}}}{partial mathbf{n}}} \ {0} end{array} ight] ]
最终我们得到了用于运动模型的EKF稀疏预测公式
[overline{mathcal{R}} leftarrow f_{mathcal{R}}(overline{mathcal{R}}, mathbf{u}, 0) ]
[ mathbf{P}_{mathcal{R} mathcal{R}} leftarrow frac{partial f_{mathcal{R}}}{partial mathcal{R}} mathbf{P}_{mathcal{R} mathcal{R}} frac{partial f_{mathcal{R}}^{T}}{partial mathcal{R}}+frac{partial f_{mathcal{R}}}{partial mathbf{n}} mathbf{N} frac{partial f_{mathcal{R}}^{T}}{partial mathbf{n}} ]
[mathbf{P}_{mathcal{R} mathcal{M}} leftarrow frac{partial f_{mathcal{R}}}{partial mathcal{R}} mathbf{P}_{mathcal{R} mathcal{M}} ]
[ mathbf{P}_{mathcal{M} mathcal{R}} leftarrow mathbf{P}_{mathcal{R} mathcal{M}}^{ op} ]
3.3 已经加入地图的状态量观测更新
在EKF中，我们有以下一般的观测方程
[ mathbf{y}=h(mathbf{x})+mathbf{v} ]
其中(mathbf{y})是测量噪声，(mathbf{x})是全状态，(h())是观测函数，(v)是测量噪声。
典型的EKF观测更新为：
[ overline{mathbf{z}}=mathbf{y}-h(overline{mathbf{x}}) ]
[mathbf{Z}=mathbf{H}_{mathbf{x}} mathbf{P} mathbf{H}_{mathbf{x}}^{ op}+mathbf{R} ]
[ mathbf{K}=mathbf{P} mathbf{H}_{mathbf{x}}^{ op} mathbf{Z}^{-1} ]
[overline{mathbf{x}} leftarrow overline{mathbf{x}}+mathbf{K} overline{mathbf{z}} ]
[ mathbf{P} leftarrow mathbf{P}-mathbf{K} mathbf{Z} mathbf{K}^{ op} ]
雅克比矩阵(mathbf{H}_{mathbf{x}}=frac{partial h(overline{mathbf{x}})}{partial mathbf{x}})，(mathbf{R})是测量噪声的协方差矩阵。
在SLAM中，观测指的是机器人上的传感器观测到某些路标点，并对路标点进行参数化的输出。每次可能对路标点有多个观测值，这里每使用一个观测值，就进行一次状态更新。
观测的结果依赖于机器人的状态(mathcal{R}),传感器的状态(mathcal{S})和路标点的状态(mathcal{L}_{i})，并且这里假设，传感器的状态与机器人的状态差了一个固定的坐标变化，其实也就是外参固定。当观测到路标点(i)时，可以得到如下关系：
[ mathbf{y}_{i}=h_{i}left(mathcal{R}, mathcal{S}, mathcal{L}_{i} ight)+mathbf{v} ]
这就是观测方程，它不依赖于除了(mathcal{L}_{i})外的任何路标点状态。因此EKF-SLAM的雅克比(mathbf{H}_{mathbf{x}})也是稀疏的：
[ mathbf{H}_{mathbf{x}}=left[egin{array}{lllllllll} {mathbf{H}_{mathcal{R}}} & {0} & {cdots} & {0} & {mathbf{H}_{mathcal{L}_{i}}} & {0} & {cdots} & {0} end{array} ight] ]
其中(mathbf{H}_{mathcal{R}}=frac{partial h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i} ight)}{partial mathcal{R}})，(mathbf{H}_{mathcal{L}_{i}}=frac{partial h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i} ight)}{partial mathcal{L}_{i}})。由于这里的稀疏性，EKF-SLAM的观测更新变成：
[overline{mathbf{z}}=mathbf{y}_{i}-h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i} ight) ]
[ mathbf{Z}=left[egin{matrix}mathbf{H}_{mathcal{R}} &mathbf{H}_{mathcal{L}_{i}}end{matrix} ight]left[egin{array}{cc} {mathbf{P}_{mathcal{R} mathcal{R}}} & {mathbf{P}_{mathcal{R} mathcal{L}_{i}}} \ {mathbf{P}_{mathcal{L}_{i} mathcal{R}}} & {mathbf{P}_{mathcal{L}_{i} mathcal{L}_{i}}} end{array} ight]left[egin{array}{c} {mathbf{H}_{mathcal{R}}^{ op}} \ {mathbf{H}_{mathcal{L}_{i}}^{ op}} end{array} ight]+mathbf{R} ]
[mathbf{K}=left[egin{array}{ll} {mathbf{P}_{mathcal{R} mathcal{R}}} & {mathbf{P}_{mathcal{R} mathcal{L}_{i}}} \ {mathbf{P}_{mathcal{M} mathcal{R}}} & {mathbf{P}_{mathcal{M} mathcal{L}_{i}}} end{array} ight]left[egin{array}{l} {mathbf{H}_{mathcal{R}}^{ op}} \ {mathbf{H}_{mathcal{L}_{i}}^{ op}} end{array} ight] mathbf{Z}^{-1} ]
[ overline{mathbf{x}} leftarrow overline{mathbf{x}}+mathbf{K} overline{mathbf{z}} ]
[mathbf{P} leftarrow mathbf{P}-mathbf{K} mathbf{Z} mathbf{K}^{ op} ]
3.4 观测方程可逆时的状态增广
这里的状态增广指的是新发现的路标点的初始化。当机器人发现了曾经未观测到的路标点时，会利用观测方程将新的路标状态加入地图，这一步操作会增大总状态向量的大小。可以看到EKF-SLAM中的滤波器大小动态变化的。
当传感器信息可以提供新发现路标点的所有自由度，也就是观测方程是双射时，只需要根据观测方程(h())的逆运算，即可以得到机器人状态(mathcal{R})，传感器状态(mathcal{S})，观测量(mathbf{y}_{n+1})，观测噪声(v)，它们与新路标点状态的关系：
[ mathcal{L}_{n+1}=gleft(mathcal{R}, mathcal{S}, mathbf{y}_{n+1},v ight) ]
上式是单个路标点的逆观测模型。
路标点的均值和雅克比：
[ overline{mathcal{L}}_{n+1}=gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0 ight) ]
[mathbf{G}_{mathcal{R}}=frac{partial gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0 ight)}{partial mathcal{R}} ]
[ mathbf{G}_{mathbf v}=frac{partial gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0 ight)}{partial mathbf{v}} ]
显然，新加路标点状态的协方差(mathbf{P}_{mathcal{L} mathcal{L}})，以及该状态与地图其它状态的互协方差为：
[ mathbf{P}_{mathcal{L} mathcal{L}}=mathbf{G}_{mathcal{R}} mathbf{P}_{mathcal{R} mathcal{R}} mathbf{G}_{mathcal{R}}^{ op}+mathbf{G}_{mathbf v} mathbf{R} mathbf{G}_{mathbf v}^{ op} ]
[mathbf{P}_{mathcal{L} mathbf{x}}=mathbf{G}_{mathcal{R}} mathbf{P}_{mathcal{R} mathbf{x}}=mathbf{G}_{mathcal{R}}left[egin{matrix}mathbf{P}_{mathcal{R} mathcal{R}} &mathbf{P}_{mathcal{R} mathcal{M}}end{matrix} ight] ]
然后将这些结果加入到地图中，可以得到总状态的均值与协方差矩阵：
[ overline{mathbf{x}} leftarrow left[egin{array}{c} {overline{mathbf{x}}} \ {overline{mathcal{L}}_{n+1}} end{array} ight] ]
[ mathbf{P} leftarrowleft[egin{array}{cc} {mathbf{P}} & {mathbf{P}_{mathcal{L} mathbf{x}}^{ op}} \ {mathbf{P}_{mathcal{L}mathbf{x}}} & {mathbf{P}_{mathcal{L}mathcal{L}}} end{array} ight] ]
4. 仿真实验
4.1 模型设置
4.1.1 传感器模型
传感器是一个360度的雷达，可以探测发现周围一定范围内的路标点及其类型，其探测半径为r，其观测值为((xi)，s)。(xi)为在当前雷达坐标系中路标点与x轴的的夹角，s为路标点与当前雷达坐标系原点的距离。
4.1.2 运动模型
将当前时刻雷达局部坐标系中的（({u}_{1}),0）点作为下一时刻雷达的位置，所以运动方程可以设为：
[ left[egin{array}{cc} {x_{n}} \ {y_{n}} end{array}ight] = left[egin{array}{cc} cos heta_{n-1} & -sin heta_{n-1} \ sin heta_{n-1} & cos heta_{n-1} end{array} ight] left[egin{array}{cc} {u}_{1} \ 0 end{array} ight] + left[egin{array}{cc} {x_{n-1}} \ {y_{n-1}} end{array}ight] + left[egin{array}{cc} {n}_{1} \ 0 end{array}ight] ]
其方位每次增加固定的(u_2):
[ { heta_{n+1}} = { heta_n} + {u}_{2} + {n}_{2} ]
其中(n_1,n_2)为系统噪声。
4.1.3 观测模型
设路标点(i)的状态为(x_{L_i}=)((m_1)，({m}_{2}))，则其在当前雷达坐标系的坐标为：
[ left[egin{array}{cc} {{{ladar}_{1}}} \ {{{ladar}_{2}}} end{array}ight] = {left[egin{array}{cc} cos heta_n & -sin heta_n \ sin heta_n & cos heta_n end{array} ight]}^{-1} left[egin{array}{cc} {{{m}_{1}}} \ {{{m}_{2}}} end{array}ight] - left[egin{array}{cc} {x_n} \ {y_n} end{array}ight] ]
则观测量为：
[ xi = atan2left({{{ladar}_{2}}},{{{ladar}_{1}}} ight) + {v}_{1} ]
[ s = sqrt{{left({{{ladar}_{1}}} ight)}^{2} + {left({{{ladar}_{2}}} ight)}^{2}} + {v}_{2} ]
其中(v_1,v_2)为测量噪声。
4.2 实验结果
使用MATLAB编写程序进行仿真。
代码地址:https://github.com/liuzhenboo/EKF-2D-SLAM
4.2.1 第一次状态增广
其中左图黑色的点表示滤波器新加入的路标点状态均值，绿色椭圆表征路标状态的协方差，其椭圆方程为：
[ (mathbf{x}-overline{mathbf{x}})^{ op} mathbf{P}^{-1}(mathbf{x}-overline{mathbf{x}})= ext { const } ]
其中x为路标状态, P为路标的协方差。
程序中是对P进行SVD分解，得到椭圆的方向R以及半轴到的长度，进行绘图。
4.2.2 状态增广，观测更新
如上图，黑色椭圆对应的路标点表示雷达曾经观测过它，但是当前时刻没有观测到；红色椭圆对应的路标点表示雷达曾经观测过它，并且当前时刻也观测到了；蓝色代表当前刚刚初始化的新路标点。
注意：由于数值计算的原因，图中几何元素的位置关系可能和实际有些许差别；比如有的路标点明明不在雷达范围内，却开始初始化（绿色），这是因为计算过程中matlab四舍五入导致的结果。
4.2.3 状态不再增广，只有观测更新
如上图，不再存在蓝色的协方差椭圆，代表状态增广停止，滤波器的大小不再改变。