[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

Aruco Marker

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

实验环境

要实现EKF-SLAM，最关键的就是建立运动模型和观测模型，将这两个模型直接带进EKF算法框架就是EKF-SLAM。EKF-SLAM算法使用扩展的状态空间： ${\bf{X}} = {\left[ {\matrix{ x & y & \theta & {{m_{x,1}}} & {{m_{y,1}}} & {{m_{x,2}}} & {{m_{y,2}}} & \cdots & {{m_{x,N}}} & {{m_{y,N}}} \cr } } \right]^T} \\$

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

1. 运动模型

1.1 里程计模型

我比较喜欢采用《自主移动机器人导论》中的里程计模型作为运动模型。具体的，如果t-1时刻机器人的位姿是 ${\xi _{t{\rm{ - }}1}} = \left[ {\matrix{ x & y & \theta \cr } } \right]_{t{\rm{ - }}1}^T$ ，那么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)\\$ ${k_{l/r}}$ 为左右轮系数，把编码器增量 $\Delta {e_{l/r}}$ 转化为左右轮的位移， $b$ 是轮间距。左右轮位移的增量 $\Delta {s_{l/r}}$ 服从高斯分布，均值就是编码器计算出的位移增量，标准差与增量大小成正比。如果t-1时刻机器人位姿的协方差为 ${{\bf{\Sigma }}_{\xi ,t{\rm{ - }}1}}$ ，控制的协方差也就是左右轮位移增量的协方差为 ${{\bf{\Sigma }}_u}$ ，那么机器人位姿在t时刻的协方差就是： ${{\bf{\Sigma }}_{\xi ,t}} = {{\bf{G}}_\xi }{{\bf{\Sigma }}_{\xi ,t{\rm{ - }}1}}{{\bf{G}}_\xi }^T + {{\bf{G'}}_u}{{\bf{\Sigma }}_u}{{\bf{G'}}_u}^T \ \ (2) \\$ ${{\bf{G}}_\xi }$ 是(1)式关于机器人位姿 ${\xi _{t{\rm{ - }}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) \\$

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

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

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

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

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

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

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

把(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传感器使用，也就是转换成距离 $r$ 和角度 $\phi$ 。1个ArUco码作为一个路标点 ${\bf{m}}$ ，坐标为 ${\left[ {\matrix{ {{m_x}} & {{m_y}} \cr } } \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 {\rm{ = atan2}}\left( {y,x} \right)$ 。这样就得到了测量值 $z = {\left[ {\matrix{ r & \phi \cr } } \right]^T}$ 。这里再做一个近似假设，认为观测的方差与距离和角度成线性关系：

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

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

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

展开来看：

$\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{z}}_t^i$ 相对于 ${{\bf{X}}_t}$ 的雅克比 ${\bf{H}}_t^i$ ，实际上一个路标点观测只涉及到机器人的位姿和这个路标点的坐标，组合在一起就是五个量： $\nu = \left[ {\matrix{ x & y & \theta & {{m_{x,i}}} & {{m_{y,i}}} \cr } } \right]$ 。于是，观测 ${\bf{z}}_t^i$ 相对于 $\nu$ 的雅克比是：

${{\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{z}}_t^i$ 相对于全状态空间 ${{\bf{X}}_t}$ 的雅克比：

${\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. 地图构建

上文所说的操作都是假设路标点的数量是已知的，这个值也可以认为是不知道的，可以边运行边加入路标点：当看到一个新的地图点时就扩展状态空间和协方差。当观测到一个新的路标点，其观测为 $z{\rm{ = }}{\left[ {\matrix{ r & \phi \cr } } \right]^T}$ ，根据机器人的位姿可以计算地图点的坐标为： $\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) \\$ 地图点的协方差为： ${{\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[ {\matrix{ 1 & 1 & { - r\sin \left( {\theta + \phi } \right)} \cr 1 & 1 & {r\cos \left( {\theta + \phi } \right)} \cr } } \right] \ \ (19) \\$ ${{\bf{G}}_z}$ 是(17)式关于观测 $z$ 的雅克比： ${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算法中所有的模型都已建立完毕。下面给出具体的实施代码。