多视角几何Rao-Blackwellised SLAM算法
- 格式：pdf
- 大小：399.92 KB
- 文档页数：6
Ｃｏｍｐｕｔｅｒ Ｅｎｇｉｎｅｅｒｉｎｇ ａｎｄＡｐｐｌｉｃａｔｉｏｎｓ＂￣ｆ算机工程与应用 ◎理论研究、研发设计＠ 多视角几何Ｒａｏ．Ｂｌａｃｋｗｅｌｌｉｓｅｄ ＳＬＡＭ算法
弋英民，黄 莹 ＹＩ Ｙｉｎｇｍｉｎ，ＨＵＡＮＧ Ｙｉｎｇ
西安理工大学自动化与信息工程学院，西安７ １ ００４８ Ｆａｃｕｌｔｙ ｏｆＡｕｔｏｍａｔｉｏｎ ａｎｄ Ｉｎｆｏｒｍａｔｉｏｎ Ｅｎｇｉｎｅｅｒｉｎｇ，Ｘｉ’ａｎ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｔｅｃｈｎｏｌｏｇｙ，Ｘｉ’ａｎ ７ １ ００４８，Ｃｈｉｎａ
ＹＩ Ｙｉｎｇｍｉｎ，ＨＵＡＮＧ Ｙｉｎｇ．ＳＬＡＭ ｍｅｔｈｏｄ ｗｉｔｈ ｍｕｌｔｉｐｌｅ ｖｉｓｉｏｎ ｇｅｏｍｅｔｒｉｃ ａｎｄ Ｒａｏ—Ｂｌａｃｋｗｅｌｌｉｓｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒｉｎｇ． Ｃｏｍｐｕｔｅｒ Ｅｎｇｉｎｅｅｒｉｎｇ ａｎｄ Ａｐｐｌｉｃａｔｉｏｎｓ，２０１５，５１（７）：３５－３９．
Ａｂｓｔｒａｃｔ：Ｌｏｗ ｐｒｉｃｅ ｓｅｎｓｏｒ ａｎｄ ｈｉｇｈ ｐｒｅｃｉｓｅ ｌｏｃａｌｉｚａｔｉｏｎ ｉｓ ａ ｈｏｔ ｒｅｓｅａｒｃｈ ｉｓｓｕｅ．Ｉｎ ｔｈｉｓ ｐａｐｅｒ，ｏｎｌｙ ｓｉｎｇｌｅ ｃａｍｅｒａ ｉｓ ｕｓｅｄ ｔｏ ｃａｔｃｈ ｍｕｌｔｉｐｌｅ ｉｍａｇｅｓ．Ｔｈｅ ｒｅｌａｔｉｏｎ ｂｅｔｗｅｅｎ ｉｍａｇｅｓ ｉｓ ｇｏｔｔｅｎ ｂｙ ｆａｃｔｏｒｉｚａｔｉｏｎ ｍｅｔｈｏｄ．Ｆｒｏｍ ｔｈｅ ｍｕｌｔｉｐｌｅ ｇｅｏｍｅｔｒｉｃ ｃａｌｃｕｌａｔｉｎｇ，ｔｈｅ ｄｅｅｐ ｏｎ ｔｈｅ ｅｎｖｉｒｏｎｍｅｎｔｓ ｉｓ ｇｏｔｔｅｎ．Ｔｈｅ ｒｏｂｏｔ ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ ｉｓ ｐｅｒｆｏｒｍｅｄ ｉｔｅｒａｔｉｖｅｌｙ ｂｙ ｔｈｅ Ｒａｏ．Ｂｌａｃｋｗｅｌｌｉｓａｔｅｄ ｆｉｌｔｅｒ．Ｔｈｅ ｏｂｓｅｒｖａｔｉｏｎ ｍｏｄｅ１ ｉＳ ａｐｐｒｏｘｉｍａｔｅｄ ｗｉｔｈ ｆｉｒｓｔ．ｏｒｄｅｒ Ｔａｙｌｏｒ．Ｔｈｅ ｅｘｐｅｒｉｍｅｎｔｓ ａｒｅ ｐｅｒｆｏｒｍｅｄ ｗｉｔｈ ＭＴ－Ｒ ｒｏｂｏｔ ｐｌａｎｔ．Ｔｈｅ ｒｅｓｕｌｔｓ ｓｈｏｗ ｔｈａｔ ｔｈｅ ｌｏｃａｌｉｚａｔｉｏｎ ｐｒｅｃｉｓｅ ｏｆ ｒｏｂｏｔ ｗｉｔｈ ｏｎｌｙ ｓｉｎｇｌｅ ｃａｍｅｒａ ｉｓ ｍｏｒｅ ｔｈａｎ ｔｈｅ ｃｌａｓｓｉｃ ｍｅｔｈｏｄ． Ｋｅｙ ｗｏｒｄｓ：ｍｕｌｔｉｐｌｅ ｖｉｓｉｏｎ ｇｅｏｍｅｔｒｉｃ；ｓｉｎｇｌｅ ｃａｍｅｒａ；Ｒａｏ—Ｂｌａｃｋｗｅｌｌｉｓｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒｉｎｇ；ＳＬＡＭ ａｌｇｏｒｉｔｈｍ
摘 要：低成本传感器、高精度定位是机器人同步定位与地图构建的热点研究问题。采用单视觉传感器来获取多个 图像视觉信息，通过因式分解求解多幅图像的空间关系；利用多视角几何理论获得环境深度信息。机器人的同步定 位与地图构建是通过对观测模型进行一阶泰勒近似，以及Ｒａｏ．Ｂｌａｃｋｗｅｌｌｉｓｅｄ粒子滤波迭代进行的。在ＭＴ－Ｒ移动机 器人研究平台进行实验，实验结果表明所提出的方法在定位精度指标优于经典的ＥＫＦ．ＳＬＡＭ方法，并且只需要单个 摄像头。 关键词：多视角几何；单目视觉；Ｒａｏ Ｂｌａｃｋｗｅｌｌｉｓｅｄ粒子滤波；ＳＬＡＭ算法 文献标志码：Ａ 中图分类号：ＴＰ２４ ｄｏｉ：１０．３７７８／ｊ．ｉｓｓｎ．１００２—８３３１．１３０４—００７９
１ 引言 ＳＬＡＭ问题是移动车辆重建周围环境的空间图，同 时估计其自身位姿（位置和方向）。在ＳＬＡＭ问题中， 随着移动机器人应用环境的不同，从２Ｄ”。 到３Ｄ ，从结 构化环境 到非结构化环境 ，从室内环境 到室外环 境 ， ，从城市环境 川到野外环境 等非结构化的自然 环境的应用。从研究问题看，主要采用传感器为声纳传 感器和激光传感器。但有基于视觉的ＳＬＡＭ算法 ” ， 主要采用ＥＫＦ理论框架的算法。当系统的控制量误差 较大时，系统的非线性特性越突出，ＥＫＦ算法由于对非 线性模型近似计算的缺点，往往发生误差累积，或算法 多次运算发生误差漂移大。目前机器人多采用里程计 或编码器作为控制量，由于这些传感器以轮子的转速作 为测量对象，误差相对于传感器本身的精度较大。 以视觉传感器作为测量数据来源，在控制量精度无 法满足的情况下，本文提出基于多视觉几何的Ｒａｏ．Ｂｌａｃｋ． ｗｅｌｌｉｓｅｄ的ＳＬＡＭ算法。机器人在移动过程中，不断获 取环境的单帧图像，利用多视角几何理论计算出机器人 和环境特征点的相对位置信息。在本文算法中，单视觉 获取距离和角度信息，编码器作为控制量的输出信息， 并把观测到视觉信息加入到观测模型和粒子更新中，通 过粒子滤波更新机器人位姿，利用特征点的近似高斯模
基金项目：国家自然科学基金（Ｎｏ．５１２７５４０５）；陕西省教育厅自然科学专项（Ｎｏ．２０１３Ｊｋ１０７８）。 作者简介：弋英民（１９７６一），男，博士，副教授，研究领域为机器人ＳＬＡＭ研究，信息融合与控制理论；黄莹（１９８９一），女，硕士研究 生，研究领域为机器人运动及地图构建。Ｅ－ｍａｉｌ：ｙｉｙｍ＠ｘａｕｔ．ｅｄｕ．ｃｎ 收稿日期：２０１３－０４．０７ 修回日期：２０１３－０６—０４ 文章编号：１００２—８３３１（２０１５）０７—００３５—０５ ＣＮＫＩ网络优先出版：２０１３—０６—２６，ｈｔｔｐ：／／ｗｗｗ．ｃｎｋｉ．ｎｅｔ／ｋｃｍｓ／ｄｅｔａｉｌ／１１．２１２７．ＴＰ．２０１３０６２６．１５３９．０１６．ｈｔｍｌ ＣｏｍｐｕｔｅｒＥｎｇｉｎｅｅｒｉｎｇ ａｎｄＡｐｐｌｉｃａｔｉｏｎｓ计算机工程与应用 型，对特征点进行ＥＫＦ更新。测量模型中传感器采用单 视觉传感器，可获取丰富的环境信息。
２多视觉几何Ｒａｏ．Ｂｌａｃｋｗｅｌｌｉｓｅｄ ＳＬＡＭ算法 法对单目视觉采集的图像进行几何关系处理，获取环境
２．１ 因式分解的多视角几何方法
矩阵Ｐ 投射到３×１的２Ｄ图像点Ｙ ， 表示尺度因子： Ｗ＝ ２ｍｌｌ Ｙ ２ｍ２ Ｙ
甜Ｙ ＝１１ １＝ Ｉ ｌ ２… Ｉ
［Ｐ Ｐ …Ｐ二］ ｉｖ，ｙ２… ］ （２） Ｙ －（Ｘｐ，Ｙｐ，ｚＰ） 表示在世界坐标系中的特征点，采用
位置和方向，即 ＝ ，Ｙ ， ） 。该参数通过已知第ｉ Ｐ， ＝Ｋｃ Ｒ ｆ ］＝ ｄ ｔ］ （４）
已知Ｐ 利用ＲＱ因式分解方法来求尺度矩阵
Ｒ ＝ＲＱ（Ｍｉ） 这里Ｍ，是Ｐ 左边３×３子阵。转换矩阵可以写为： ｔｉ＝ｅ。
－ ｔ ｜７、，＝ｆ （５）
这里Ⅳ，是Ｐ，，右边３×１的子向量，也就是Ｐ ＝［ ｌⅣ］。 ２．２ Ｒａｏ—Ｂｌａｃｋｗｅｌｌｉｓｅｄ滤波 Ｒａｏ．Ｂｌａｃｋｗｅｌｌｉｓａｔｉｏｎ是Ｍｏｎｔｅ Ｃａｒｌｏ方法的一种变形 方法 。对于固定数目的采样，基于Ｒａｏ．Ｂｌａｃｋｗｅｌｌｉｓｅｄ 采样的随机变量总是小于或等于从整个空间的采样，它 把联合概率密度函数分解为乘积项，从而减少状态空间 的维数。 机器人的控制量Ｕ，，是在时问段［ｔ—ｌ，ｔ）内作用 的。控制机器人位姿演化过程为一动力学运动非线性 模型，如下形式： ｐ（ｓｆｆｕｆ，Ｓｆ一１）＝ｈ（ｕｆ， １）＋ ｆ 这里机器人位姿的一步预测函数ｈ伴随有高斯噪声； 为零均值协方差为Ｐ 的随机变量。ＳＬＡＭ算法的核心 是传感器测量生成的模型，即测量模型为： ｐ（ｚｆＩｓ ，０ ｆ，，ｌｆ）＝ｇ（ ｆ，Ｓｆ）＋８ｆ 这里测量模型是非线性模型，是以机器人位姿 ，特征 点标识ｎ ，以及观测到的特征点０ 为条件分布；观测 模型函数ｇ伴随有零均值噪声ｓ ，协方差为Ｒ，。 Ｒａｏ．Ｂｌａｃｋｗｅｌｌｉｓｅｄ滤波主要解决非线性非高斯系 统问题，滤波过程如下所示。 ２．２．１ 预测步 在预测步，新的机器人姿态Ｓ 由控制量Ｕ，和每个 粒子状态 一中抽样得到。 ｓ －ｐ（ｓ ｌ，ｌｉｔ） （６） 这里 是机器人在ｔ一１时刻的位姿后验估计，采样结 果ｓ 添加到暂态粒子集。 ２．２．２ 更新步 假设特征点是已知数据关联的，更新是通过方程 （１）正则化后： ｐ（Ｏ ， ｎｔ， ）＝ （ｚ Ｉｓ ，０ ｎ ）ｐ（ Ｉｓ ，ｎ ，ｚ卜 ）（７） 概率ｐ（Ｏ Ｉｓ川，，ｌ ， ）是在ｔ一１时刻均值为 一 ，协 方差为∑ 一 的高斯分布。在ｔ时刻新的估计也是高斯 的，通过ＥＫＦ线性化观测模型ｐ（ｚ Ｉｓ ，０ ，ｌ ）。利用一阶 泰勒展开式近似测量函数譬： ｇ（ｏ ，ｓ ） ｇ（．１ｌ ，ｓ ）＋ ｇ ，，ｌ ，一１）（ ，一 一 ）＝ ＋ａｌｍｌ（ 一 ） （８） 这里ｇ 是关于０ 的偏导数。线性化近似是在 州和 Ｉｎ］ 处的ｇ的正切。通过线性化近似，特征点位置 ， 的后验分布为高斯的。新的均值和协方差可通过标准 弋英民，黄 莹：多视角几何Ｒａｏ．Ｂｌａｃｋｗｅｌｌｉｓｅｄ ＳＬＡＭ算法 ＥＫＦ测量更新得到。 ＝∑ 一 Ｇ （Ｇ ｔ州 ∑ Ｇ 州＋ ）
ｆ ＝．ｆｆ 。＋ 州（ 三 １∑ ＝（Ｊ一 州Ｇ ～Ｃｍ¨］一。
ｔ一１时刻的位置和方向角。 （９） ２．３．２环境观测 环境观测要完成对环境特征信息的检测。 （１０） ２．３．３数据关联
第１步和第２步重复 次，得到 个粒子的状态。 ２．２．３重新采样步 第１步抽样没有考虑测量Ｚ ，重采样就是为了弥补
这一缺失。粒子集中的位姿粒子的分布如下： ｐ（ｓｔ＇［ｍ］ｌｚ ，Ⅱ ，ｎｔ－１）＝ｐ（ｓ ｉｓｉ
．ｒｅ
１，／４ｔ）×
ｐ（ｓ 一 ’【 Ｉｚ 一１，Ｕ 一１，ｎ 一 ） （１１）
目标分布考虑到ｚ，和关联 ，： ｐ（ｓｔ＇［ｍ］ｌｚ ，Ｕ ，ｎ ） （１２）
重要性权值由目标分布和建议分布的商得到：
： ，
［￥ｔ，［ｍ］ｚｔ＿１ ｎｔ）（１３）
ｐ（ ’， ，ｌ ）…
上式转化为： ｐ（ｓｔ＇ｔｍ］ｌＺ ，Ⅱ ，ｎｔ）＝ （ｚ，Ｉｓ ’ １，Ｈ ，ｔｌｔ）×
ｐ（ｓｔ，［ｍ］ｌＺ ，Ｕ ，，ｌ ） （１４） 忽略与传感器测量无关的变量： ＝ｒｌＳｐ（ｚ ｆ ，，ｌ ，ｓ ）ｐ（ ｆＳｔ－１，［ｍ］ 一 ，，ｌ ）ｄ （１ ５） ＦａｓｔＳＬＡＭ利用同样的线性化近似，重要性权值因 子可写为： ≈叩ｌ２尢Ｑ ｌ ｅｘｐ｛一 １（ 一乏 叫）ＴＱ 卜 （ 一乏 州）｝（１６）
Ｑ ＝Ｇ ∑ 一 Ｇ ＋ （１７） ２．３多视角几何的Ｒａｏ．Ｂｌａｃｋｗｅｌｌｉｓｅｄ ＳＬＡＭ算法 机器人多视角几何的Ｒａｏ．Ｂｌａｃｋｗｅｌｌｉｓｅｄ ＳＬＡＭ算 法的每一个递推过程都经过５步：状态预测、环境观测、 数据关联、更新、重采样。 ２．３．１ 状态预测 在预测步需要计算机器人姿态的一步预测 ，和预 测协方差矩阵。控制量Ｕ 由机器人速度ｖ和角速度 、． 广，，］ 组成“， ｌ，状态 — 是ｆ一１时刻的机器人状态。
