MATLAB实现一个EKF-2D-SLAM(已开源)

  • 2020 年 4 月 10 日
  • 筆記

1. SLAM问题定义

同时定位与建图(SLAM)的本质是一个估计问题,它要求移动机器人利用传感器信息实时地对外界环境结构进行估计,并且估算出自己在这个环境中的位置,Smith 和Cheeseman在上个世纪首次将EKF估计方法应用到SLAM。

以滤波为主的SLAM模型主要包括三个方程:

1)运动方程:它会增加机器人定位的不确定性

2)根据观测对路标初始化的方程:它根据观测信息,对新的状态量初始化。

3)路标状态对观测的投影方程:根据观测信息,对状态更新,纠正,减小不确定度。

2. EKF-SLAM维护的数据地图

系统状态x是一个很大的向量,它包括机器人的状态和路标点的状态,

[ mathbf{x}=left[begin{array}{c} {mathcal{R}} \ {mathcal{M}} end{array}right]=left[begin{array}{c} {mathcal{R}} \ {mathcal{L}_{1}} \ {vdots} \ {mathcal{L}_{n}} end{array}right] ]

其中({mathcal{R}})是机器人状态,({mathcal{M}} = left({mathcal{L}_{1}}, dots,{mathcal{L}_{n}}right))是n个当前已经观测过的路标点状态集合。

在EKF中,x被认为服从高斯分布,所以,EKF-SLAM的地图被建模为x的均值(overline{x})与协方差(mathbf{P}),

[ overline{mathbf{x}}=left[frac{overline{mathcal{R}}}{mathcal{M}}right]=left[begin{array}{c} {overline{mathcal{R}}} \ {overline{mathcal{L}}_{1}} \ {vdots} \ {overline{mathcal{L}}_{n}} end{array}right] quad mathbf{P}=left[begin{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}right]=left[begin{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}right] ]

因此EKF-SLAM的目标就是根据运动模型和观测模型及时更新地图量(left{overline{mathbf{x}},mathbf{P}right})

3. EKF-SLAM算法实施

3.1 地图初始化

显而易见,在机器人开始之前是没有任何路标点的信息的,因此此时地图中只有机器人自己的状态信息,因此({n} = 0,{x} = {mathcal{R}})。SLAM中经常把机器人初始位姿认为是地图的原点,其初始协方差可以按实际情况设定,比如:

[ overline{mathbf{x}}=left[begin{array}{l} {x} \ {y} \ {theta} end{array}right]=left[begin{array}{l} {0} \ {0} \ {0} end{array}right] quad mathbf{P}=left[begin{array}{lll} {0} & {0} & {0} \ {0} & {0} & {0} \ {0} & {0} & {0} end{array}right] ]

3.2 运动模型

在EKF中如果x是状态量,u是控制输入,n是噪声变量,那么我们可以得到一般的状态更新函数:

[mathbf{x} leftarrow f(mathbf{x}, mathbf{u}, mathbf{n}) ]

EKF的预测步骤为:

[begin{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}}^{top}+mathbf{F}_{mathbf{n}} mathbf{N} mathbf{F}_{mathbf{n}}^{top}} 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的运动方程为:

[begin{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[begin{array}{cc} {frac{partial f_{mathcal{R}}}{partial mathcal{R}}} & {0} \ {0} & {mathbf{I}} end{array}right] quad mathbf{F}_{mathbf{n}}=left[begin{array}{c} {frac{partial f_{mathcal{R}}}{partial mathbf{n}}} \ {0} end{array}right] ]

最终我们得到了用于运动模型的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}}^{top} ]

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}}^{top}+mathbf{R} ]

[ mathbf{K}=mathbf{P} mathbf{H}_{mathbf{x}}^{top} 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}^{top} ]

雅克比矩阵(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}right)+mathbf{v} ]

这就是观测方程,它不依赖于除了(mathcal{L}_{i})外的任何路标点状态。因此EKF-SLAM的雅克比(mathbf{H}_{mathbf{x}})也是稀疏的:

[ mathbf{H}_{mathbf{x}}=left[begin{array}{lllllllll} {mathbf{H}_{mathcal{R}}} & {0} & {cdots} & {0} & {mathbf{H}_{mathcal{L}_{i}}} & {0} & {cdots} & {0} end{array}right] ]

其中(mathbf{H}_{mathcal{R}}=frac{partial h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i}right)}{partial mathcal{R}})(mathbf{H}_{mathcal{L}_{i}}=frac{partial h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i}right)}{partial mathcal{L}_{i}})。由于这里的稀疏性,EKF-SLAM的观测更新变成:

[overline{mathbf{z}}=mathbf{y}_{i}-h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i}right) ]

[ mathbf{Z}=left[begin{matrix}mathbf{H}_{mathcal{R}} &mathbf{H}_{mathcal{L}_{i}}end{matrix}right]left[begin{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}right]left[begin{array}{c} {mathbf{H}_{mathcal{R}}^{top}} \ {mathbf{H}_{mathcal{L}_{i}}^{top}} end{array}right]+mathbf{R} ]

[mathbf{K}=left[begin{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}right]left[begin{array}{l} {mathbf{H}_{mathcal{R}}^{top}} \ {mathbf{H}_{mathcal{L}_{i}}^{top}} end{array}right] 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}^{top} ]

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},vright) ]

上式是单个路标点的逆观测模型。

路标点的均值和雅克比:

[ overline{mathcal{L}}_{n+1}=gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0right) ]

[mathbf{G}_{mathcal{R}}=frac{partial gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0right)}{partial mathcal{R}} ]

[ mathbf{G}_{mathbf v}=frac{partial gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0right)}{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}}^{top}+mathbf{G}_{mathbf v} mathbf{R} mathbf{G}_{mathbf v}^{top} ]

[mathbf{P}_{mathcal{L} mathbf{x}}=mathbf{G}_{mathcal{R}} mathbf{P}_{mathcal{R} mathbf{x}}=mathbf{G}_{mathcal{R}}left[begin{matrix}mathbf{P}_{mathcal{R} mathcal{R}} &mathbf{P}_{mathcal{R} mathcal{M}}end{matrix}right] ]

然后将这些结果加入到地图中,可以得到总状态的均值与协方差矩阵:

[ overline{mathbf{x}} leftarrow left[begin{array}{c} {overline{mathbf{x}}} \ {overline{mathcal{L}}_{n+1}} end{array}right] ]

[ mathbf{P} leftarrowleft[begin{array}{cc} {mathbf{P}} & {mathbf{P}_{mathcal{L} mathbf{x}}^{top}} \ {mathbf{P}_{mathcal{L}mathbf{x}}} & {mathbf{P}_{mathcal{L}mathcal{L}}} end{array}right] ]

4. 仿真实验

4.1 模型设置

4.1.1 传感器模型

传感器是一个360度的雷达,可以探测发现周围一定范围内的路标点及其类型,其探测半径为r,其观测值为((xi),s)。(xi)为在当前雷达坐标系中路标点与x轴的的夹角,s为路标点与当前雷达坐标系原点的距离。

4.1.2 运动模型

将当前时刻雷达局部坐标系中的(({u}_{1}),0)点作为下一时刻雷达的位置,所以运动方程可以设为:

[ left[begin{array}{cc} {x_{n}} \ {y_{n}} end{array} right] = left[begin{array}{cc} costheta_{n-1} & -sintheta_{n-1} \ sintheta_{n-1} & costheta_{n-1} end{array}right] left[begin{array}{cc} {u}_{1} \ 0 end{array}right] + left[begin{array}{cc} {x_{n-1}} \ {y_{n-1}} end{array} right] + left[begin{array}{cc} {n}_{1} \ 0 end{array} right] ]

其方位每次增加固定的(u_2):

[ {theta_{n+1}} = {theta_n} + {u}_{2} + {n}_{2} ]

其中(n_1,n_2)为系统噪声。

4.1.3 观测模型

设路标点(i)的状态为(x_{L_i}=)((m_1)({m}_{2})),则其在当前雷达坐标系的坐标为:

[ left[begin{array}{cc} {{{ladar}_{1}}} \ {{{ladar}_{2}}} end{array} right] = {left[begin{array}{cc} costheta_n & -sintheta_n \ sintheta_n & costheta_n end{array}right]}^{-1} left[begin{array}{cc} {{{m}_{1}}} \ {{{m}_{2}}} end{array} right] – left[begin{array}{cc} {x_n} \ {y_n} end{array} right] ]

则观测量为:

[ xi = atan2left({{{ladar}_{2}}},{{{ladar}_{1}}}right) + {v}_{1} ]

[ s = sqrt{{left({{{ladar}_{1}}}right)}^{2} + {left({{{ladar}_{2}}}right)}^{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}})^{top} mathbf{P}^{-1}(mathbf{x}-overline{mathbf{x}})=text { const } ]

其中x为路标状态, P为路标的协方差。

程序中是对P进行SVD分解,得到椭圆的方向R以及半轴到的长度,进行绘图。

4.2.2 状态增广,观测更新

如上图,黑色椭圆对应的路标点表示雷达曾经观测过它,但是当前时刻没有观测到;红色椭圆对应的路标点表示雷达曾经观测过它,并且当前时刻也观测到了;蓝色代表当前刚刚初始化的新路标点。

注意:由于数值计算的原因,图中几何元素的位置关系可能和实际有些许差别;比如有的路标点明明不在雷达范围内,却开始初始化(绿色),这是因为计算过程中matlab四舍五入导致的结果。

4.2.3 状态不再增广,只有观测更新

如上图,不再存在蓝色的协方差椭圆,代表状态增广停止,滤波器的大小不再改变。