[PR-3]ArUco EKF SLAM 扩展卡尔曼SLAM

转载自:https://zhuanlan.zhihu.com/p/45207081?utm_source=qq&utm_medium=social&utm_oi=726911971793838080 

原文有视频

这篇文章实现了《概率机器人》第10章中提到的EKF-SLAM算法,更确切的说是实现了已知一致性的EKF-SLAM算法。

 

 

ArUco EKF SLAM

EKF-SLAM一般是基于路标的SLAM系统。本文使用了一种人工路标——ArUco码。每个ArUco码有一个独立的ID,通过PnP方法还可以计算出码和相机之间的相对位姿。OpenCV中集成了ArUco码库,提供了检测和位姿估计的功能。大家可以参考:

https://docs.opencv.org/3.3.0/d9/d6d/tutorial_table_of_content_aruco.html。​docs.opencv.org

 

[PR-3]ArUco EKF SLAM 扩展卡尔曼SLAM_第1张图片

Aruco Marker

首先在房间的地面上贴若干ArUco码作为路标,然后遥控一个带有摄像头+编码器的机器人在房间内运动。本文的目标就是通过EKF算法同时估计出这些码的位置和机器人的位姿。

[PR-3]ArUco EKF SLAM 扩展卡尔曼SLAM_第2张图片

实验环境

要实现EKF-SLAM,最关键的就是建立运动模型和观测模型,将这两个模型直接带进EKF算法框架就是EKF-SLAM。EKF-SLAM算法使用扩展的状态空间: 

前3项是机器人位姿,后2N项是 N个路标点的位置。

1. 运动模型

1.1 里程计模型

我比较喜欢采用《自主移动机器人导论》中的里程计模型作为运动模型。具体的,如果t-1时刻机器人的位姿是  ,那么t时刻的机器人位姿为: \eqalign{   & {\left[ {\matrix{    x  \cr     y  \cr     \theta   \cr    } } \right]_t}{\rm{ = }}{\left[ {\matrix{    x  \cr     y  \cr     \theta   \cr    } } \right]_{t{\rm{ - }}1}} + \left[ {\matrix{    {\Delta s\cos (\theta  + \Delta \theta /2)}  \cr     {\Delta s\sin (\theta  + \Delta \theta /2)}  \cr     {\Delta \theta }  \cr    } } \right],\;  \cr    & \left\{ \matrix{   \Delta \theta  = {{\Delta {s_r} - \Delta {s_l}} \over b}\; \hfill \cr    \Delta s = {{\Delta {s_r}{\rm{ + }}\Delta {s_l}} \over 2} \hfill \cr}  \right.,\Delta {s_{l/r}} = {k_{l/r}} \cdot \Delta {e_{l/r}}  \cr    & \Delta {s_{l/r}} \sim N\left( {\matrix{    {{{\widehat {\Delta s}}_{l/r}},} & {{{\left\| {k{{\widehat {\Delta s}}_{l/r}}} \right\|}^2}}  \cr    } } \right). \cr} \ \  (1)\\  为左右轮系数,把编码器增量转化为左右轮的位移,  是轮间距。左右轮位移的增量服从高斯分布,均值就是编码器计算出的位移增量,标准差与增量大小成正比。如果t-1时刻机器人位姿的协方差为,控制的协方差也就是左右轮位移增量的协方差为,那么机器人位姿在t时刻的协方差就是:   是(1)式关于机器人位姿的雅克比:

{{\bf{G}}_\xi } = {{\partial {\xi _t}} \over {\partial {\xi _{t{\rm{ - }}1}}}} = \left[ {\matrix{    1 & 0 & { - \Delta s\sin (\theta  + \Delta \theta /2)}  \cr     0 & 1 & {\Delta s\cos (\theta  + \Delta \theta /2)}  \cr     0 & 0 & 1  \cr    } } \right] \ \ (3) \\

 是(1)式关于控制)的雅克比:

{{\bf{G'}}_u} = {{\partial {\xi _t}} \over {\partial {\bf{u}}}}{\rm{ = }}\left[ {\matrix{    {{1 \over 2}\cos \left( {\theta {\rm{ + }}{{\Delta \theta } \over 2}} \right) - {{\Delta s} \over {2b}}\sin \left( {\theta  + {{\Delta \theta } \over 2}} \right)} & {{1 \over 2}\cos \left( {\theta {\rm{ + }}{{\Delta \theta } \over 2}} \right) + {{\Delta s} \over {2b}}\sin \left( {\theta  + {{\Delta \theta } \over 2}} \right)}  \cr     {{1 \over 2}\sin \left( {\theta {\rm{ + }}{{\Delta \theta } \over 2}} \right) + {{\Delta s} \over {2b}}\cos \left( {\theta  + {{\Delta \theta } \over 2}} \right)} & {{1 \over 2}\sin \left( {\theta {\rm{ + }}{{\Delta \theta } \over 2}} \right) - {{\Delta s} \over {2b}}\cos \left( {\theta  + {{\Delta \theta } \over 2}} \right)}  \cr     {{1 \over b}} & { - {1 \over b}}  \cr    } } \right] \ \ (4) \\

 

1.2 EKF-SLAM运动更新

上面说的还是只考虑机器人位姿的情况,但是SLAM系统还需要考虑路标点。扩展路标点之后,运动方程为:

\underbrace {{{\left[ {\matrix{    x  \cr     y  \cr     \theta   \cr     {{m_{x,1}}}  \cr     {{m_{y,1}}}  \cr      \vdots   \cr     {{m_{x,N}}}  \cr     {{m_{y,N}}}  \cr    } } \right]}_t}}_{{{\bf{X}}_t}} = \underbrace {\underbrace {{{\left[ {\matrix{    x  \cr     y  \cr     \theta   \cr     {{m_{x,1}}}  \cr     {{m_{y,1}}}  \cr      \vdots   \cr     {{m_{x,N}}}  \cr     {{m_{y,N}}}  \cr    } } \right]}_{t{\rm{ - }}1}}}_{{{\bf{X}}_{t{\rm{ - }}1}}} + \underbrace {\left[ {\matrix{    1 & 0 & 0  \cr     0 & 1 & 0  \cr     0 & 0 & 1  \cr     0 & 0 & 0  \cr     0 & 0 & 0  \cr      \vdots  &  \vdots  &  \vdots   \cr     0 & 0 & 0  \cr     0 & 0 & 0  \cr    } } \right]}_{\bf{F}}\left[ {\matrix{    {\Delta s\cos (\theta  + \Delta \theta /2)}  \cr     {\Delta s\sin (\theta  + \Delta \theta /2)}  \cr     {\Delta \theta }  \cr    } } \right]}_{g\left( {{{\bf{X}}_{t{\rm{ - }}1}},{{\bf{u}}_t}} \right)} \ \ (5) \\系统状态的均值 更新利用(5)式,下面看状态的方差  更新。

 是  关于  的雅克比:

是关于  的雅克比:

把(6)式展开看一下:

\eqalign{   {\overline {\bf{\Sigma }} _t} & {\rm{ = }}\left[ {\matrix{    {{{\bf{G}}_\xi }} & {\bf{0}}  \cr     {\bf{0}} & {\bf{I}}  \cr    } } \right]{{\bf{\Sigma }}_t}\left[ {\matrix{    {{{\bf{G}}_\xi }^T} & {\bf{0}}  \cr     {\bf{0}} & {\bf{I}}  \cr    } } \right] + {\bf{F}}{{{\bf{G'}}}_u}{{\bf{\Sigma }}_u}{{{\bf{G'}}}_u}^T{{\bf{F}}^T} \cr   &= \left[ {\matrix{    {{{\bf{G}}_\xi }{{\bf{\Sigma }}_{xx}}{{\bf{G}}_\xi }^T} & {{{\bf{G}}_\xi }{{\bf{\Sigma }}_{xm}}}  \cr     {{{\left( {{{\bf{G}}_\xi }{{\bf{\Sigma }}_{xm}}} \right)}^T}} & {{{\bf{\Sigma }}_{mm}}}  \cr    } } \right] + {\bf{F}}{{{\bf{G'}}}_u}{{\bf{\Sigma }}_u}{{{\bf{G'}}}_u}^T{{\bf{F}}^T} \cr}  \ \ (9) \\可以看出,运动更新同时影响了机器人位姿的协方差,以及位姿与地图之间的协方差。

2. 测量模型

首先解决测量值的问题。虽然可以获得ArUco码相对于机器人的6自由度位姿信息,但是为了与书上的观测统一,本文还是把相机作为Range-bearing传感器使用,也就是转换成距离和角度。1个ArUco码作为一个路标点  ,坐标为 。

先说一下如何转化成距离和角度。下图是示意图,码与相机的相对位姿为,相机与机器人的相对位姿为  ,那么码相对于机器人的位姿为  。的平移项和  就是码的原点在机器人坐标系下的坐标。转化成距离信息就是,角度就是  。这样就得到了测量值。这里再做一个近似假设,认为观测的方差与距离和角度成线性关系:

[PR-3]ArUco EKF SLAM 扩展卡尔曼SLAM_第3张图片

第  个路标点的观测模型为:

展开来看:

\left\{ \matrix{   r_t^i = \sqrt {{{\left( {{m_{x,i}} - x} \right)}^2} + {{\left( {{m_{y,i}} - y} \right)}^2}}  \hfill \cr    \phi _t^i = {\rm{atan2}}\left( {{m_{y,i}} - y, \ {m_{x,i}} - x} \right) - \theta  \hfill \cr}  \right.\ \ (12)\\

根据扩展卡尔曼滤波,需要求解观测  相对于的雅克比,实际上一个路标点观测只涉及到机器人的位姿和这个路标点的坐标,组合在一起就是五个量:  。于是,观测相对于的雅克比是:

{{\bf{H}}_\nu } = {{\partial h} \over {\partial \nu }} = {1 \over q}\left[ {\matrix{    { - \sqrt q {\delta _x}} & { - \sqrt q {\delta _y}} & 0 & {\sqrt q {\delta _x}} & {\sqrt q {\delta _y}}  \cr     {{\delta _y}} & { - {\delta _x}} & { - q} & { - {\delta _y}} & {{\delta _x}}  \cr    } } \right]\left( {\left\{ \matrix{   {\delta _x} = {m_{x,i}} - x \hfill \cr    {\delta _y} = {m_{y,i}} - y \hfill \cr}  \right.q = {\delta _x}^2 + {\delta _y}^2} \right) \ \ (13)\\

由于实际的状态空间是3+2N维的,要求的观测雅克比应该是2x(3+2N)维的。对(13)进行转换得到观测相对于全状态空间  的雅克比:

{\bf{H}}_t^i{\rm{ = }}{{\bf{H}}_\nu }{{\bf{F}}_i}{\rm{ = }}{1 \over q}\left[ {\matrix{    { - \sqrt q {\delta _x}} & { - \sqrt q {\delta _y}} & 0 & {\sqrt q {\delta _x}} & {\sqrt q {\delta _y}}  \cr     {{\delta _y}} & { - {\delta _x}} & { - q} & { - {\delta _y}} & {{\delta _x}}  \cr    } } \right]\left[ {\matrix{    1 & 0 & 0 & {0 \cdots 0} & 0 & 0 & {0 \cdots 0}  \cr     0 & 1 & 0 & {0 \cdots 0} & 0 & 0 & {0 \cdots 0}  \cr     0 & 0 & 1 & {0 \cdots 0} & 0 & 0 & {0 \cdots 0}  \cr     0 & 0 & 0 & {0 \cdots 0} & 1 & 0 & {0 \cdots 0}  \cr     0 & 0 & 0 & {\underbrace {0 \cdots 0}_{2i - 2}} & 0 & 1 & {\underbrace {0 \cdots 0}_{2N - 2i}}  \cr    } } \right] \ \ (14)\\

 

下面就可以按照EKF的框架进行操作了。

\eqalign{   & {\bf{K}}_t^i = {\overline {\bf{\Sigma }} _t}{\bf{H}}{_t^i}^T{\left( {{\bf{H}}_t^i{{\overline {\bf{\Sigma }} }_t}{\bf{H}}{{_t^i}^T}{\rm{ + }}{{\bf{Q}}_t}} \right)^{ - 1}}  \cr    & {{\bf{\mu }}_t}{\bf{ = }}{{{\bf{\bar \mu }}}_t} + {\bf{K}}_t^i\left( {{\bf{z}}_t^i - {\bf{\hat z}}_t^i} \right)  \cr    & {{\bf{\Sigma }}_t} = \left( {{\bf{I}} - {\bf{K}}_t^i{\bf{H}}_t^i} \right){\overline {\bf{\Sigma }} _t} \cr} \ \ (15) \\

其中, {\bf{\hat z}}_t^i{\rm{ = }}\left[ \matrix{   \sqrt {{{\left( {{{\bar m}_{x,i}} - \bar x} \right)}^2} + {{\left( {{{\bar m}_{y,i}} - \bar y} \right)}^2}}  \hfill \cr    {\rm{atan2}}\left( {{{\bar m}_{y,i}} - \bar y,\ {{\bar m}_{x,i}} - \bar x} \right) - \bar \theta  \hfill \cr}  \right] \ \ (16)\\

就是由路标点和机器人位姿的均值获取。对每个观测到的路标点进行上述操作就完成了观测更新。

3. 地图构建

上文所说的操作都是假设路标点的数量是已知的,这个值也可以认为是不知道的,可以边运行边加入路标点:当看到一个新的地图点时就扩展状态空间和协方差。当观测到一个新的路标点,其观测为,根据机器人的位姿可以计算地图点的坐标为: \left[ {\matrix{    {{m_x}}  \cr     {{m_y}}  \cr    } } \right] = \left[ {\matrix{    {\cos (\theta )} & { - \sin (\theta )}  \cr     {\sin (\theta )} & {\cos (\theta )}  \cr    } } \right]\left[ {\matrix{    {r\cos \left( \phi  \right)}  \cr     {r\sin \left( \phi  \right)}  \cr    } } \right] + \left[ {\matrix{    x  \cr     y  \cr    } } \right] = r\left[ {\matrix{    {\cos \left( {\theta  + \phi } \right)}  \cr     {\sin \left( {\theta  + \phi } \right)}  \cr    } } \right]{\rm{ + }}\left[ {\matrix{    x  \cr     y  \cr    } } \right] \ \ (17) \\地图点的协方差为:   是(17)式关于机器人位姿的雅克比:   是(17)式关于观测  的雅克比: {G_z} = \left[ {\matrix{    {\cos \left( {\theta  + \phi } \right)} & { - r\sin \left( {\theta  + \phi } \right)}  \cr     {\sin \left( {\theta  + \phi } \right)} & {r\cos \left( {\theta  + \phi } \right)}  \cr    } } \right] \ \ (20) \\通过以上各式,算出新路标的均值和协方差,加入到均值向量和协方差矩阵中即可。至此,EKF算法中所有的模型都已建立完毕。下面给出具体的实施代码。

4. 算法实现

  • 全部工程代码:

https://github.com/ydsf16/aruco_ekf_slam​github.com

 

  • 我利用Falconbot机器人,采集了两组实验数据,大家可以在这里下载:

https://pan.baidu.com/s/1EX9CYmdEUR2BJh7v5dTNfA​pan.baidu.com

 

 

5. 参考文献

《概率机器人》

《自主移动机器人导论》

Freiburg SLAM Course:

http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/​ais.informatik.uni-freiburg.de

 

 

 

 

 

 

 

 

 

 

 

 

你可能感兴趣的:(SLAM)