转载自：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算法使用扩展的状态空间：
前3项是机器人位姿，后2N项是 N个路标点的位置。
1. 运动模型
1.1 里程计模型
我比较喜欢采用《自主移动机器人导论》中的里程计模型作为运动模型。具体的，如果t-1时刻机器人的位姿是 ，那么t时刻的机器人位姿为：
为左右轮系数，把编码器增量
转化为左右轮的位移，
是轮间距。左右轮位移的增量
服从高斯分布，均值就是编码器计算出的位移增量，标准差与增量大小成正比。如果t-1时刻机器人位姿的协方差为
，控制的协方差也就是左右轮位移增量的协方差为
，那么机器人位姿在t时刻的协方差就是：
是(1)式关于机器人位姿
的雅克比：
是(1)式关于控制）
的雅克比：
1.2 EKF-SLAM运动更新
上面说的还是只考虑机器人位姿的情况，但是SLAM系统还需要考虑路标点。扩展路标点之后，运动方程为：
系统状态的均值
更新利用(5)式，下面看状态的方差
更新。
是
关于
的雅克比：
是
关于
的雅克比：
把(6)式展开看一下：
可以看出，运动更新同时影响了机器人位姿的协方差，以及位姿与地图之间的协方差。
2. 测量模型
首先解决测量值的问题。虽然可以获得ArUco码相对于机器人的6自由度位姿信息，但是为了与书上的观测统一，本文还是把相机作为Range-bearing传感器使用，也就是转换成距离和角度
。1个ArUco码作为一个路标点
，坐标为
。
先说一下如何转化成距离和角度。下图是示意图，码与相机的相对位姿为，相机与机器人的相对位姿为
，那么码相对于机器人的位姿为
。
的平移项
和
就是码的原点在机器人坐标系下的坐标。转化成距离信息就是
，角度就是
。这样就得到了测量值
。这里再做一个近似假设，认为观测的方差与距离和角度成线性关系：
第 个路标点的观测模型为：
展开来看：
根据扩展卡尔曼滤波，需要求解观测 相对于
的雅克比
，实际上一个路标点观测只涉及到机器人的位姿和这个路标点的坐标，组合在一起就是五个量：
。于是，观测
相对于
的雅克比是：
由于实际的状态空间是3+2N维的，要求的观测雅克比应该是2x(3+2N)维的。对(13)进行转换得到观测相对于全状态空间
的雅克比：
下面就可以按照EKF的框架进行操作了。
其中，
就是由路标点和机器人位姿的均值获取。对每个观测到的路标点进行上述操作就完成了观测更新。
3. 地图构建
上文所说的操作都是假设路标点的数量是已知的，这个值也可以认为是不知道的，可以边运行边加入路标点：当看到一个新的地图点时就扩展状态空间和协方差。当观测到一个新的路标点，其观测为，根据机器人的位姿可以计算地图点的坐标为：
地图点的协方差为：
是(17)式关于机器人位姿的雅克比：
是(17)式关于观测
的雅克比：
通过以上各式，算出新路标的均值和协方差，加入到均值向量和协方差矩阵中即可。至此，EKF算法中所有的模型都已建立完毕。下面给出具体的实施代码。
4. 算法实现
- 全部工程代码：
https://github.com/ydsf16/aruco_ekf_slamgithub.com
- 我利用Falconbot机器人，采集了两组实验数据，大家可以在这里下载：
https://pan.baidu.com/s/1EX9CYmdEUR2BJh7v5dTNfApan.baidu.com
5. 参考文献
《概率机器人》
《自主移动机器人导论》
Freiburg SLAM Course:
http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/ais.informatik.uni-freiburg.de