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

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

ArUco EKF SLAM

https://www.zhihu.com/video/1027276196236513280

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

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

Aruco Marker

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

实验环境

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

$${\bf{X}} = {\left[ {\begin{matrix} x & y & \theta & {{m_{x,1}}} & {{m_{y,1}}} & {{m_{x,2}}} & {{m_{y,2}}} & \cdots & {{m_{x,N}}} & {{m_{y,N}}} \\ \end{matrix} } \right]^T} \\$$

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

1. 运动模型

1.1 里程计模型

我比较喜欢采用《自主移动机器人导论》中的里程计模型作为运动模型。具体的，如果t-1时刻机器人的位姿是 ${\xi _{t{\mathrm{ - }}1}} = \left[ {\begin{matrix} x & y & \theta \\ \end{matrix} } \right]_{t{\mathrm{ - }}1}^T$ ，那么t时刻的机器人位姿为：

$$\begin{aligned} & {\left[ {\begin{matrix} x \\ y \\ \theta \\ \end{matrix} } \right]_t}{\mathrm{ = }}{\left[ {\begin{matrix} x \\ y \\ \theta \\ \end{matrix} } \right]_{t{\mathrm{ - }}1}} + \left[ {\begin{matrix} {\Delta s\cos (\theta + \Delta \theta /2)} \\ {\Delta s\sin (\theta + \Delta \theta /2)} \\ {\Delta \theta } \\ \end{matrix} } \right],\; \\ & \left\{ \begin{matrix} \Delta \theta = {{\Delta {s_r} - \Delta {s_l}} \over b}\; \quad \\ \Delta s = {{\Delta {s_r}{\mathrm{ + }}\Delta {s_l}} \over 2} \quad \\\end{matrix} \right.,\Delta {s_{l/r}} = {k_{l/r}} \cdot \Delta {e_{l/r}} \\ & \Delta {s_{l/r}} \sim N\left( {\begin{matrix} {{{\widehat {\Delta s}}_{l/r}},} & {{{\left\| {k{{\widehat {\Delta s}}_{l/r}}} \right\|}^2}} \\ \end{matrix} } \right). \end{aligned}$$

${k_{l/r}}$ 为左右轮系数，把编码器增量$\Delta {e_{l/r}}$转化为左右轮的位移， $b$ 是轮间距。左右轮位移的增量$\Delta {s_{l/r}}$服从高斯分布，均值就是编码器计算出的位移增量，标准差与增量大小成正比。如果t-1时刻机器人位姿的协方差为${{\bf{\Sigma }}_{\xi ,t{\mathrm{ - }}1}}$，控制的协方差也就是左右轮位移增量的协方差为${{\bf{\Sigma }}_u}$，那么机器人位姿在t时刻的协方差就是： ${{\bf{\Sigma }}_{\xi ,t}} = {{\bf{G}}_\xi }{{\bf{\Sigma }}_{\xi ,t{\mathrm{ - }}1}}{{\bf{G}}_\xi }^T + {{\bf{G'}}_u}{{\bf{\Sigma }}_u}{{\bf{G'}}_u}^T \ \ (2) \\$ ${{\bf{G}}_\xi }$ 是(1)式关于机器人位姿${\xi _{t{\mathrm{ - }}1}}$的雅克比：

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

${{\bf{G'}}_u}$ 是(1)式关于控制）${\bf{u}} = {\left[ {\begin{matrix} {\Delta {s_r}} & {\Delta {s_l}} \\ \end{matrix} } \right]^T}$的雅克比：

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

1.2 EKF-SLAM运动更新

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

$$\underbrace {{{\left[ {\begin{matrix} x \\ y \\ \theta \\ {{m_{x,1}}} \\ {{m_{y,1}}} \\ \vdots \\ {{m_{x,N}}} \\ {{m_{y,N}}} \\ \end{matrix} } \right]}_t}}_{{{\bf{X}}_t}} = \underbrace {\underbrace {{{\left[ {\begin{matrix} x \\ y \\ \theta \\ {{m_{x,1}}} \\ {{m_{y,1}}} \\ \vdots \\ {{m_{x,N}}} \\ {{m_{y,N}}} \\ \end{matrix} } \right]}_{t{\mathrm{ - }}1}}}_{{{\bf{X}}_{t{\mathrm{ - }}1}}} + \underbrace {\left[ {\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \vdots & \vdots & \vdots \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{matrix} } \right]}_{\bf{F}}\left[ {\begin{matrix} {\Delta s\cos (\theta + \Delta \theta /2)} \\ {\Delta s\sin (\theta + \Delta \theta /2)} \\ {\Delta \theta } \\ \end{matrix} } \right]}_{g\left( {{{\bf{X}}_{t{\mathrm{ - }}1}},{{\bf{u}}_t}} \right)} \ \ (5) \\$$

系统状态的均值 ${{\bf{\bar u}}_t}$更新利用(5)式，下面看状态的方差 ${\overline {\bf{\Sigma }} _t}$ 更新。

$${\overline {\bf{\Sigma }} _t}{\mathrm{ = }}{{\bf{G}}_t}{{\bf{\Sigma }}_{t{\mathrm{ - }}1}}{{\bf{G}}_t}^T + {{\bf{G}}_u}{{\bf{\Sigma }}_u}{{\bf{G}}_u}^T \ \ (6)\\$$

${{\bf{G}}_t}$ 是 $g\left( {{{\bf{X}}_{t{\mathrm{ - }}1}},{{\bf{u}}_t}} \right)$ 关于 ${{\bf{X}}_{t{\mathrm{ - }}1}}$ 的雅克比：

$${{\bf{G}}_t} = \left[ {\begin{matrix} {{{\bf{G}}_\xi }} & {\bf{0}} \\ {\bf{0}} & {\bf{I}} \\ \end{matrix} } \right] \ \ (7)\\$$

${{\bf{G}}_u}$是$g\left( {{{\bf{X}}_{t{\mathrm{ - }}1}},{{\bf{u}}_t}} \right)$关于 ${{\bf{u}}_t}$ 的雅克比：

$${{\bf{G}}_u} = {\bf{F}}\;{{\bf{G'}}_u}\ \ (8) \\$$

把(6)式展开看一下：

$$\begin{aligned} {\overline {\bf{\Sigma }} _t} & {\mathrm{ = }}\left[ {\begin{matrix} {{{\bf{G}}_\xi }} & {\bf{0}} \\ {\bf{0}} & {\bf{I}} \\ \end{matrix} } \right]{{\bf{\Sigma }}_t}\left[ {\begin{matrix} {{{\bf{G}}_\xi }^T} & {\bf{0}} \\ {\bf{0}} & {\bf{I}} \\ \end{matrix} } \right] + {\bf{F}}{{{\bf{G'}}}_u}{{\bf{\Sigma }}_u}{{{\bf{G'}}}_u}^T{{\bf{F}}^T} \\ &= \left[ {\begin{matrix} {{{\bf{G}}_\xi }{{\bf{\Sigma }}_{xx}}{{\bf{G}}_\xi }^T} & {{{\bf{G}}_\xi }{{\bf{\Sigma }}_{xm}}} \\ {{{\left( {{{\bf{G}}_\xi }{{\bf{\Sigma }}_{xm}}} \right)}^T}} & {{{\bf{\Sigma }}_{mm}}} \\ \end{matrix} } \right] + {\bf{F}}{{{\bf{G'}}}_u}{{\bf{\Sigma }}_u}{{{\bf{G'}}}_u}^T{{\bf{F}}^T} \end{aligned}$$

可以看出，运动更新同时影响了机器人位姿的协方差，以及位姿与地图之间的协方差。

2. 测量模型

首先解决测量值的问题。虽然可以获得ArUco码相对于机器人的6自由度位姿信息，但是为了与书上的观测统一，本文还是把相机作为Range-bearing传感器使用，也就是转换成距离$r$和角度$\phi$。1个ArUco码作为一个路标点 ${\bf{m}}$ ，坐标为 ${\left[ {\begin{matrix} {{m_x}} & {{m_y}} \\ \end{matrix} } \right]^T}$。

先说一下如何转化成距离和角度。下图是示意图，码与相机的相对位姿为${}_m^c{\bf{T}}$，相机与机器人的相对位姿为 ${}_c^r{\bf{T}}$ ，那么码相对于机器人的位姿为 ${}_m^r{\bf{T}} = {}_c^r{\bf{T}}{}_m^c{\bf{T}}$ 。${}_m^r{\bf{T}}$的平移项$x$和 $y$ 就是码的原点在机器人坐标系下的坐标。转化成距离信息就是$r = \sqrt {{x^2} + {y^2}}$，角度就是 $\phi {\mathrm{ = atan2}}\left( {y,x} \right)$ 。这样就得到了测量值$z = {\left[ {\begin{matrix} r & \phi \\ \end{matrix} } \right]^T}$。这里再做一个近似假设，认为观测的方差与距离和角度成线性关系：

$${\bf{Q}} = \left[ {\begin{matrix} {{{\left\| {{k_r}r} \right\|}^2}} & {} \\ {} & {{{\left\| {{k_\phi }\phi } \right\|}^2}} \\ \end{matrix} } \right] \ \ (10)\\$$

第 $i$ 个路标点的观测模型为：

$${\bf{z}}_t^i{\mathrm{ = }}h\left( {{{\bf{X}}_t}} \right){\mathrm{ + }}{\bf{\delta }}_t^i,\;{{\bf{\delta }}_t} \sim {\cal N}\left( {{\bf{0}},{{\bf{Q}}_t}} \right) \ \ (11)\\$$

展开来看：

$$\left\{ \begin{matrix} r_t^i = \sqrt {{{\left( {{m_{x,i}} - x} \right)}^2} + {{\left( {{m_{y,i}} - y} \right)}^2}} \quad \\ \phi _t^i = {\mathrm{atan2}}\left( {{m_{y,i}} - y, \ {m_{x,i}} - x} \right) - \theta \quad \\\end{matrix} \right.\ \ (12)\\$$

根据扩展卡尔曼滤波，需要求解观测 ${\bf{z}}_t^i$ 相对于${{\bf{X}}_t}$的雅克比${\bf{H}}_t^i$，实际上一个路标点观测只涉及到机器人的位姿和这个路标点的坐标，组合在一起就是五个量： $\nu = \left[ {\begin{matrix} x & y & \theta & {{m_{x,i}}} & {{m_{y,i}}} \\ \end{matrix} } \right]$ 。于是，观测${\bf{z}}_t^i$相对于$\nu$的雅克比是：

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

由于实际的状态空间是3+2N维的，要求的观测雅克比应该是2x(3+2N)维的。对(13)进行转换得到观测${\bf{z}}_t^i$相对于全状态空间 ${{\bf{X}}_t}$ 的雅克比：

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

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

$$\begin{aligned} & {\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}{\mathrm{ + }}{{\bf{Q}}_t}} \right)^{ - 1}} \\ & {{\bf{\mu }}_t}{\bf{ = }}{{{\bf{\bar \mu }}}_t} + {\bf{K}}_t^i\left( {{\bf{z}}_t^i - {\bf{\hat z}}_t^i} \right) \\ & {{\bf{\Sigma }}_t} = \left( {{\bf{I}} - {\bf{K}}_t^i{\bf{H}}_t^i} \right){\overline {\bf{\Sigma }} _t} \end{aligned}$$

其中，

$${\bf{\hat z}}_t^i{\mathrm{ = }}\left[ \begin{matrix} \sqrt {{{\left( {{{\bar m}_{x,i}} - \bar x} \right)}^2} + {{\left( {{{\bar m}_{y,i}} - \bar y} \right)}^2}} \quad \\ {\mathrm{atan2}}\left( {{{\bar m}_{y,i}} - \bar y,\ {{\bar m}_{x,i}} - \bar x} \right) - \bar \theta \quad \\\end{matrix} \right] \ \ (16)\\$$

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

3. 地图构建

上文所说的操作都是假设路标点的数量是已知的，这个值也可以认为是不知道的，可以边运行边加入路标点：当看到一个新的地图点时就扩展状态空间和协方差。当观测到一个新的路标点，其观测为$z{\mathrm{ = }}{\left[ {\begin{matrix} r & \phi \\ \end{matrix} } \right]^T}$，根据机器人的位姿可以计算地图点的坐标为：

$$\left[ {\begin{matrix} {{m_x}} \\ {{m_y}} \\ \end{matrix} } \right] = \left[ {\begin{matrix} {\cos (\theta )} & { - \sin (\theta )} \\ {\sin (\theta )} & {\cos (\theta )} \\ \end{matrix} } \right]\left[ {\begin{matrix} {r\cos \left( \phi \right)} \\ {r\sin \left( \phi \right)} \\ \end{matrix} } \right] + \left[ {\begin{matrix} x \\ y \\ \end{matrix} } \right] = r\left[ {\begin{matrix} {\cos \left( {\theta + \phi } \right)} \\ {\sin \left( {\theta + \phi } \right)} \\ \end{matrix} } \right]{\mathrm{ + }}\left[ {\begin{matrix} x \\ y \\ \end{matrix} } \right] \ \ (17) \\$$

3.1 新地图点的协方差

地图点的协方差为：

$${{\bf{\Sigma }}_m} = {{\bf{G}}_p}{{\bf{\Sigma }}_\xi }{\bf{G}}_p^T + {{\bf{G}}_z}{\bf{QG}}_z^T \ \ (18)\\$$

${{\bf{G}}_p}$ 是(17)式关于机器人位姿的雅克比： ${G_p} = \left[ {\begin{matrix} 1 & 0 & { - r\sin \left( {\theta + \phi } \right)} \\ 0 & 1 & {r\cos \left( {\theta + \phi } \right)} \\ \end{matrix} } \right] \ \ (19) \\$ ${{\bf{G}}_z}$ 是(17)式关于观测 $z$ 的雅克比： ${G_z} = \left[ {\begin{matrix} {\cos \left( {\theta + \phi } \right)} & { - r\sin \left( {\theta + \phi } \right)} \\ {\sin \left( {\theta + \phi } \right)} & {r\cos \left( {\theta + \phi } \right)} \\ \end{matrix} } \right] \ \ (20) \\$

3.2 新地图点与原状态之间的协方差

下面，还需要计算新加入的状态（地图点）与原状态（1个机器人位姿+N个地图点）之间的协方差。

$${{\mathbf{\Sigma }}_{mx}} = {{\mathbf{G}}_{fx}}{{\mathbf{\Sigma }}_t}\\$$

${{\mathbf{\Sigma }}_t}$为原状态的协方差矩阵，${{\mathbf{G}}_{fx}}$ 为(17)式相对于原状态的雅克比矩阵：

$${{\mathbf{G}}_{fx}}{\text{ = }}\left[ {\begin{array}{c} 1&0&{ - r\sin (\theta + \phi )}&0& \cdots &0 \\ 0&1&{r\cos (\theta + \phi )}&0& \cdots &0 \end{array}} \right] \\$$

通过以上各式，算出新路标的均值和协方差，以及新路标与原状态的协方差。加入到均值向量和协方差矩阵中即可。协方差的扩展如下图所示。

协方差扩展

至此，EKF算法中所有的模型都已建立完毕。下面给出具体的实施代码。

4. 算法实现

• 全部工程代码：

https://github.com/ydsf16/aruco_ekf_slam

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

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

5. 参考文献

《概率机器人》

《自主移动机器人导论》

Freiburg SLAM Course:

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