基于RGBD相机的3维ICP视觉里程计研究

徐碘

摘  要：针对传统的迭代最近点算法在视觉里程计应用中存在的外点干扰与相机偶然大运动导致的位姿估计不准确或失效问题，提岀基于3维ICP匹配的视觉里程计RGBD相机的切换策略，提髙位姿估计准确度和鲁棒性。并在ICP算法起始阶段估计当前帧点云与模型点云重叠率，减少外点对位姿估计的影响。实验在标准测试数据集下，与典型视觉里程计对比有效提髙小规模运动下位姿估计的准确度，同时解决了相机偶然大运动下系统失效问题。

关键词：3维ICP算法  视觉里程计  RGBD相机  位姿估计

中图分类号：TP242                           文献标识码：A                    文章编号：1674-098X（2020）10（b）-0036-03

Abstract： In order to solve the problem of inaccuracy or failure of pose estimation caused by external point interference and occasional big motion of camera in the traditional iterative nearest point algorithm in the application of visual odometer， we present a switching strategy based on 3D ICP matching for visual odometer RGBD camera to improve the accuracy and robustness of pose estimation. In the initial stage of ICP algorithm， the overlap rate of current frame point cloud and model point cloud is estimated to reduce the influence of external points on pose estimation. Compared with the typical visual odometer， the experiment can effectively improve the accuracy of low pose estimation in small-scale motion， and solve the problem of system failure in accidental large motion.

Key Words： 3D ICP algorithm; Visual odometer; RGBD camera; Pose estimation

1  视觉里程计轨迹估计

1.1 算法框架

在估计相机位姿时，本文选择将图像信息以特征点云的形式进行输入。为了增强最终模型的精准性，本文选择首先综合模型DK（当前帧点云模型）和模型MK—1（世界坐标系下的点云模型）的信息估计位姿，其次使用卡尔曼滤镜对模型MK—1进行结合得到MK。

本文采用的当前帧点云位姿计算的初值是实验中前一帧点云计算得到的位姿TK—1，但实验中会产生一些不可控因素，较为突出的是实验中相机的偶然大幅度运动，此时会对计算结果产生较大的影响。判断之后，本文选择跳过当前帧，代之以计算上一帧DK—1与下一帧DK—1的相对位姿，本文利用RANSAC算法剔除外点并在匹配时选用传统的特征点描述，获得相应的特征点后再采用ICP算法（原始）进行位姿计算。

1.2 本文的ICP位姿估计算法

首先使用欧式距离度量在点云模型MK-1与DK之中检索最近点，接着通过确定其配准关系求解距离和及距离和的最小二乘问题获得位姿，再使点云模型DK处于与模型MK-1相同的坐标系下，寻找下一次迭代的最近点再进行位姿运算，重复操作直到位姿收敛，这是传统ICP算法的主要思路。

在ICP算法中必不可缺的一步便是估计当前帧点云与模型点云的最小部分重叠率ξ=[α，β]，这个最小重叠率的值呈现了当前帧点云与模型点云之间的最小重叠百分比，经过优化运算中相应的目标函数可以得到：Ψ（ξ）=e（ξ）/ξ1+γ。

在本文中，位姿解算采用不同于传统ICP算法的各向异性的ICP算法，这是由于在不同特征点中，其确定度有所不同。需做特殊说明的是，本文不采用传统ICP算法思路中经常使用到的欧氏距离寻找点云模型的最近点，而是采用马氏距离，即点di与mj之间的距离，其距离公式为：dist（di，mj）2=（μdi-μmj）

2  实验及分析

本文的实验步骤具体如下：第一，验证引入自适应部分重叠率是否能够增加位姿计算精度;第二，将本文算法与改进前的算法进行对比实验。一般的，从体现系统随时间增加产生累积误差的程度来看，相对轨迹误差对视觉里程计评价更适宜。具体实验如图1所示。

2.1 帧对模型与帧间匹配的切换策略对比实验

上文提到的相机大运动，本文对比了有无切换策略对视觉里程计的影响，实验表明切换策略对相机偶然大运动具有一定的鲁棒性。具体实验数据如表1所示。

2.2 视觉里程计的综合表现能力实验

针对前文提到的整体视觉里程计系统，实验选择采用TUM RGB—D标准数据集中6个常用数据集对其进行整体评估实验并且选取两个典型的算法（DSO[6]、DVO[7]）实行对比。实验验证精度高于典型算法的3个视频序列为相机做小规模运动，而在相机有偶然大运动的视频序列上可以达到与典型算法同等的估计效果。具体实验数据如表2所示。

3  结语

本文对于现有视觉里程计算法中外点影响大、相机偶然大运动鲁棒性欠缺的问题设想并验证了一种改进方法并使用了一种切换策略以提升系统精确度及鲁棒性，实验结果显示运用该改进方法的视觉里程计系统使用效果优异，能一定程度上缓解前文提到的外点影响大、相机偶然运动鲁棒性欠缺的问题。

参考文献

[1] 邢科新，竺海光，林叶贵，等.基于前后端图优化的RGB-D三维SLAM[J].浙江工业大学学报，2018，46（6）：616-621.

[2] 刘强，段富海，桑勇，等.复杂环境下视觉SLAM闭环检测方法综述[J].机器人，2019，41（1）：112-123，136.

[3] 朱永豐，朱述龙，张静静，等.基于ORB特征的单目视觉定位算法研究[J].计算机科学，2016，43（S1）：198-202，254.

[4] 王消为，贺利乐，赵涛.基于激光雷达与双目视觉的移动机器人SLAM研究[J].传感技术学报，2018，31（3）：394-399.

[5] 伊星星.基于RGB-D的移动机器人实时定位与建图研究[D].哈尔滨：哈尔滨工业大学，2017.

[6] 孙建辉，刘坤.基于序列图像特征识别实时反馈伺服跟踪研究[J].浙江工业大学学报，2015，43（2）：185-189.