欢迎光临
我们一直在努力

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

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}\(是机器人状态,\){\mathcal} = \left({\mathcal{1}}, \dots,{\mathcal}\right)$是n个当前已经观测过的路标点状态集合。

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

$$ \overline{\mathbf}=\left[\frac{\overline{\mathcal}}{\mathcal}\right]=\left[\begin {\overline{\mathcal}} \ {\overline{\mathcal}{1}} \ {\vdots} \ {\overline{\mathcal}} \end\right] \quad \mathbf=\left[\begin {\mathbf{\mathcal \mathcal}} & {\mathbf{\mathcal \mathcal}} \ {\mathbf{\mathcal}} & {\mathbf{\mathcal}} \end\right]=\left[\begin {\mathbf{\mathcal}} & {\mathbf{\mathcal \mathcal{1}}} & {\cdots} & {\mathbf{\mathcal \mathcal}} \ {\mathbf{\mathcal{1} \mathcal}} & {\mathbf{\mathcal{1} \mathcal{1}}} & {\cdots} & {\mathbf{\mathcal} \mathcal} \ {\vdots} & {\vdots} & {\ddots} & {\vdots} \ {\mathbf{\mathcal \mathcal}} & {\mathbf{\mathcal \mathcal{1}}} & {\cdots} & {\mathbf{\mathcal \mathcal_}} \end\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{\mathbf}=\frac{\partial f(\overline{\mathbf}, \mathbf)}{\partial \mathbf}\(,\)\mathbf{\mathbf}=\frac{\partial f(\overline{\mathbf}, \mathbf)}{\partial \mathbf}\(,\){\mathbf}$是随机变量n的协方差。

但是在EKF-SLAM中,只有一部分状态${\mathcal}$是随运动而改变的,其余所有路标状态不改变,所以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} \leftarrow f_{\mathcal}(\overline{\mathcal}, \mathbf, 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{\mathcal \mathcal} \leftarrow \frac{\partial f{\mathcal}}{\partial \mathcal} \mathbf_{\mathcal \mathcal}

\[ $$ \mathbf{P}_{\mathcal{M} \mathcal{R}} \leftarrow \mathbf{P}_{\mathcal{R} \mathcal{M}}^{\top} \]

3.3 已经加入地图的状态量观测更新

在EKF中,我们有以下一般的观测方程

$$ \mathbf=h(\mathbf)+\mathbf $$

其中$\mathbf\(是测量噪声,\)\mathbf$是全状态,$h()$是观测函数,$v$是测量噪声。

典型的EKF观测更新为:

$$ \overline{\mathbf}=\mathbf-h(\overline{\mathbf})

\[ $$\mathbf{Z}=\mathbf{H}_{\mathbf{x}} \mathbf{P} \mathbf{H}_{\mathbf{x}}^{\top}+\mathbf{R} \]

$$ \mathbf=\mathbf \mathbf_{\mathbf}{\top} \mathbf{-1} $$

$$\overline{\mathbf} \leftarrow \overline{\mathbf}+\mathbf \overline{\mathbf}

\[ $$ \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\(外的任何路标点状态。因此EKF-SLAM的雅克比\)\mathbf{\mathbf}$也是稀疏的:

$$ \mathbf{\mathbf}=\left[\begin {\mathbf{\mathcal}} & {0} & {\cdots} & {0} & {\mathbf{\mathcal}} & {0} & {\cdots} & {0} \end\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=\left[\begin\mathbf{\mathcal} &\mathbf{\mathcal}\end\right]\left[\begin {\mathbf{\mathcal \mathcal}} & {\mathbf{\mathcal \mathcal}} \ {\mathbf{\mathcal \mathcal}} & {\mathbf{\mathcal \mathcal}} \end\right]\left[\begin {\mathbf{\mathcal}{\top}} \ {\mathbf{\mathcal}{\top}} \end\right]+\mathbf $$

$$\mathbf=\left[\begin {\mathbf{\mathcal \mathcal}} & {\mathbf{\mathcal \mathcal}} \ {\mathbf{\mathcal \mathcal}} & {\mathbf{\mathcal \mathcal}} \end\right]\left[\begin {\mathbf{\mathcal}^{\top}} \ {\mathbf{\mathcal_}{\top}} \end\right] \mathbf{-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\(,传感器状态\)\mathcal\(,观测量\)\mathbf_{n+1}$,观测噪声$v$,它们与新路标点状态的关系:

$$ \mathcal{n+1}=g\left(\mathcal, \mathcal, \mathbf{n+1},v\right)

\[ 上式是单个路标点的逆观测模型。 路标点的均值和雅克比: $$ \overline{\mathcal{L}}_{n+1}=g\left(\overline{\mathcal{R}}, \mathcal{S}, \mathbf{y}_{n+1},0\right) $$ $$\mathbf{G}_{\mathcal{R}}=\frac{\partial g\left(\overline{\mathcal{R}}, \mathcal{S}, \mathbf{y}_{n+1},0\right)}{\partial \mathcal{R}} \]

$$ \mathbf{\mathbf v}=\frac{\partial g\left(\overline{\mathcal}, \mathcal, \mathbf{n+1},0\right)}{\partial \mathbf}

\[ 显然,新加路标点状态的协方差$\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 \leftarrow\left[\begin {\mathbf} & {\mathbf{\mathcal \mathbf}^{\top}} \ {\mathbf{\mathcal\mathbf}} & {\mathbf_{\mathcal\mathcal}} \end\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}
    cos\theta_{n-1} & -sin\theta_{n-1}  \\
    sin\theta_{n-1} & cos\theta_{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_=$(\(m_1\),\({m}_{2}\)),则其在当前雷达坐标系的坐标为:

$$ \left[\begin {{{ladar}{1}}} \ {{{ladar}{2}}} \end \right] =
{\left[\begin cos\theta_n & -sin\theta_n \ sin\theta_n & cos\theta_n \end\right]}^{-1} \left[\begin {{{m}{1}}} \ {{{m}{2}}} \end \right] – \left[\begin \ \end \right]

\[ 则观测量为: $$ \xi = atan2\left({{{ladar}_{2}}},{{{ladar}_{1}}}\right) + {v}_{1} \]

$$ s = \sqrt{{\left({{{ladar}{1}}}\right)}^{2} + {\left({{{ladar}{2}}}\right)}^{2}} + _{2}

\[ 其中$v_1,v_2$为测量噪声。 ### 4.2 实验结果 使用MATLAB编写程序进行仿真。 [代码地址:https://github.com/liuzhenboo/EKF-2D-SLAM](https://github.com/liuzhenboo/EKF-2D-SLAM) #### 4.2.1 第一次状态增广 <img src=”https://img-blog.csdnimg.cn/20200406223739461.PNG?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xpdTIwMTUzMDIwMjY” width=”40%” alt=””/> <img src=”https://img-blog.csdnimg.cn/20200406224842203.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xpdTIwMTUzMDIwMjY” width=”40%” alt=””/> 其中左图黑色的点表示滤波器新加入的路标点状态均值,绿色椭圆表征路标状态的协方差,其椭圆方程为: $$ (\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 状态不再增广,只有观测更新

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

转载本站文章请注明,转载自:IT技术资讯 [https://www.ithothub.com]

本文链接:https://www.ithothub.com/ihh/202004112336840.html

赞(0) 打赏

评论 抢沙发

评论前必须登录!

 

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

微信扫一扫打赏