开启辅助访问
 找回密码
 立即注册

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

一品花糖果 回答数20 浏览数9142
这篇文章实现了《概率机器人》第10章中提到的EKF-SLAM算法,更确切的说是实现了已知一致性的EKF-SLAM算法。


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



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

上文所说的操作都是假设路标点的数量是已知的,这个值也可以认为是不知道的,可以边运行边加入路标点:当看到一台新的地图点时就扩展状态空间和协方差。当观测到一台新的路标点,其观测为,根据机器人的位姿可以计算地图点的坐标为:
3.1 新地图点的协方差
地图点的协方差为: 是(17)式关于机器人位姿的雅克比: 是(17)式关于观测 的雅克比:
3.2 新地图点与原状态之间的协方差
下面,还需要计算新加入的状态(地图点)与原状态(1个机器人位姿+N个地图点)之间的协方差。



为原状态的协方差矩阵, 为(17)式相对于原状态的雅克比矩阵:


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



协方差扩展

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


  • 全部工程代码:


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

5. 参考文献

《概率机器人》
《自主移动机器人导论》
Freiburg SLAM Course:
planewh
太牛了!参考
用PixPix生成超酷炫电视壁纸
回复 赞 踩
使用道具 举报
花枝俏
看一下是不是公式(19)推倒有问题
回复 赞 踩
使用道具 举报
zxacjs110
非常感谢,已修正。
回复 赞 踩
使用道具 举报
sexx
这个写的非常好啊,请教下楼主如果将里程计换成IMU该如何融合视觉信息?
回复 赞 踩
使用道具 举报
bote8
哈哈,谢谢。IMU和视觉融合有很多著名的工作,你可以看一下这个 http://rpg.ifi.uzh.ch/docs/ICRA18_Delmerico.pdf
回复 赞 踩
使用道具 举报
cool886
作者您好,地图构建方差矩阵时,是不是漏掉了路标点和机器人的协方差?
回复 赞 踩
使用道具 举报
501252535
是漏掉啦,非常感谢,已经修改啦。
回复 赞 踩
使用道具 举报
xiaofangfang
厉害!
回复 赞 踩
使用道具 举报
520123520
作者您好,自己参阅了您的代码。请问您的摄像头—编码器 变换矩阵是如何得来的呢,手动测量还是?近期自己在做摄像头—编码器标定,想向您请教下这个
回复 赞 踩
使用道具 举报
123下一页
快速回复
您需要登录后才可以回帖 登录 | 立即注册

当贝投影