室内移动机器人视觉里程计研究

高云峰，李伟超，李建辉

（哈尔滨工业大学机电工程学院，黑龙江哈尔滨 150001）

0 引言

在室内移动机器人的研究中，实时获得机器人高精度的运动参数是极其重要的，关系到机器人定位、三维全局地图构建等重要导航任务的实现。由于机器人行进过程中的碰到障碍物、轮子存在长期磨损、打滑现象，使得光电码盘、测速电机等不能准确测定机器人的位移准确信息;采用GPS定位，存在分辨率低，且在室内的信号弱，不适合于室内移动机器人。视觉里程计可有效弥补这些缺点。

视觉里程计算法是由Matthies L［1］在1987年提出的，之后经过多年的研究，已经有很多优秀的里程计出现。Nister D等人［2，3］分别采用单目和双目实现里程计，并取得较好的效果。

Navid N V等人［4］设计的视觉里程计用在室外马路上的车辆自身定位，这个里程计用摄像机拍摄地面，并采用光流法作图像配准，Zhang Tianguang［5］在随后采用类似的方法设计的里程计，他也采用了光流法，最后他使用了卡尔曼滤波器将常规里程计测得的值和视觉里程计测得的值作了信息融合，在室内环境达到了2.5%的精度，由于他们的里程计的工作模型是二维模型，对摄像机的安装提出了比较高的要求。

1 视觉里程计工作模型

里程计装在室内移动机器人底部拍摄地面，配置合适光源进行照明。

里程计用到的坐标系有:

1）世界坐标系（OwXwYwZw），原点位于标定时棋盘图的某一固定点上，OwXwYw与地平面重合，OwZw竖直向上。

2）机器人坐标系（OrXrYrZr），原点位于机器人的后驱动轮轴心处，OrXr指向与后驱动轮轴线垂直方向，竖直向上。

3）机器人起始点坐标系（OroXroYroZro），原点和方向与机器人坐标系的初始位置重合。

4）摄像机坐标系（OcXcYcZc），原点位于摄像机主点，与帧存坐标系平面平行。

5）摄像机起始点坐标系（OcoXcoYcoZco），原点和方向与摄像机坐标系的初始位置重合。

6）帧存坐标系（OoUV），原点位于图像左上角，OoU向右，OoV向下。

2 视觉里程计软件流程与关键算法

2.1 软件流程

该里程计软件流程为:在t时刻采集一帧图像，然后提取SURF特征点，并与前一帧图像的SURF特征点进行匹配，匹配的准则是最近邻向量匹配法;匹配得到特征点对之后，再将帧存坐标系下的点转换到摄像机坐标系下并进行倾斜变换修正，然后使用RANSAC算法选取高精度的特征点对并计算机器人的转换矩阵，以用于在摄像机起始点坐标系下的运动估计;最后，再将摄像机起始点坐标系下的机器人的坐标转换到机器人起始点坐标系下。

2.2 摄像机模型与摄像机标定

为了得到帧存坐标系、摄像机坐标系与世界坐标系的关系，需要进行摄像机的建模与标定。

摄像机的模型有线性模型和非线性模型2种，由于采用的摄像机畸变较小，所以，选用线性模型，采用张正友的模型来进行标定，得到摄像机的参数为

其中，fx，fy，u0，v0为张正友模型下的摄像机内参，Rc，Tc为摄像机起始位置时的外参。

2.3 SURF算法与帧间图像特征点匹配

SURF算法是Bay H等人［6］提出的，它是对SIFT算法的改进算法，理论基础是尺度空间理论和尺度选择理论，SURF算法中使用箱式滤波器代替二阶高斯滤波器，这样做的好处是，可以使用积分图像来加速卷积过程，极大地减少了卷积所使用的时间，达到比SIFT更快的计算速度。

通过使用SURF算法得到前后两帧的特征点组（u1i，v1i），i=1，…，n1和（u2i，v2i），i=1，…，n2。

然后进行特征点匹配，使用最近邻向量匹配法进行匹配，依次选取第一幅图像中的每一特征点与第二幅图像中的所有特征点分别计算欧氏距离，只要最近的距离与次近的距离的比值小于一个阈值，就认为这对特征点匹配成功，实验结果如图1所示。

图1 前后两帧图像匹配结果Fig 1 Matching result of adjacent frames

2.4 帧存坐标系至摄像机坐标系变换

由SURF算法得到了一些特征点对，这些特征点对的坐标还是在帧存坐标系下表示的，由于需要在摄像机坐标系下进行运动估计，根据摄像机小孔成像模型和标定得到的摄像机的参数进行坐标变换，得到特征点对在摄像机坐标系下的坐标，变换公式如下

2.5 基于透视变换的摄像机倾斜修正

由于CCD所在平面与地面不平行，所以，摄像机得到的图像与实际地平面存在着变形，导致转换到摄像机坐标系中点的坐标与地面的点的实际坐标有所偏移。

由2.4节得到的点所在的平面是与地平面有一定偏角的平面1，平面1与摄像机光轴垂直。需要得到实际的点构成的地平面2。

为了能将点平面 1 上得到的点P'i（x'ci，y'ci，zc，1）转换到地平面2上的实际的点Pi（xci，yci，zci，1），需要求得直线OcP'i的方程和地平面2的方程，这样就可以联立方程组求得Pi（xci，yci，zci，1）。

在求地平面2的时候，由于摄像机的运动只有绕垂直于地面的方向上的轴的转动和在平行于地面的平面内的平移，地平面在前后两帧的摄像机坐标系中的表示都是一致的，所以，所有帧的摄像机倾斜修正的公式都是一致的，都可以由标定得到的参数Rc和Tc.

2.6 帧间坐标变换矩阵求解

求解:已知地面上的采样点在当前帧摄像机坐标系下的坐标和前一帧摄像机坐标系下的坐标，求当前帧的点在前一帧摄像机坐标系下的转换矩阵，即求

其中，Wci=（Rci|Tci）为所求，Rci为旋转矩阵，Tci为平移矩阵。

如果直接使用同一平面上的点来求解上面的Wci中的未知数，那么构成的线性方程组的系数矩阵是病态矩阵，无法求得正确值，所以，采用向量的方式来解决这个问题，先求Rci，然后求Tci。

2.7 RANSAC 算法

在得到特征点对在摄像机坐标系下的坐标后，由于匹配的特征点存在误匹配现象，于是选用RANSAC算法行运动估计［7］。

RANSAC算法应用的基本思路是在特征点对中随机选出3个特征点对，根据2.6所示步骤求得Wci，然后再通过统计适合于Wci的特征点对的个数，即内点个数，选出适合点数目最多的Wci，认为是所求的Wci。算法匹配结果如图2。

图2 RANSAC算法匹配结果Fig 2 Matching result of RANSAC algorithm

2.8 摄像机起始点下的机器人位移求解

当前帧摄像机在OroXroYroZro下的坐标，可根据下面公式求得

2.9 Mcr的标定

得到摄像机起始点坐标系下机器人的坐标之后，需要把这个坐标信息转换到机器人起始点坐标系下，因为机器人姿态变化时，不进行转换，会导致里程计给出的信息不是机器人的正确的坐标信息。

提出一种Mcr的标定方法，基本思路是让机器人分别进行纯平移和绕起点与机器人起始点坐标系原点相同、方向与OwXw轴相同的轴的纯旋转运动，运动中的各个位置的机器人坐标系原点在机器人起始点坐标系下的坐标已知，当机器人进行纯旋转运动时，标定得到摄像机坐标系到机器人坐标系的平移分量Tcr，当机器人进行纯平移运动时标定得到旋转分量Rcr。

先求得当前帧摄像机坐标系的原点在摄像机起始点坐标系下的坐标，公式如下

公式中的Oci为当前帧摄像机坐标系的原点，Ori为当前帧机器人坐标系的原点，求得当前帧机器人在摄像机起始点坐标系下的坐标后，就可以根据公式（7）就能得到当前帧机器人在机器人起始点坐标系下的坐标

第一种方法标定Mcr，对机器人纯平移和机器人纯旋转这两种特殊情形，先分别依据上面的原理算出当前帧机器人在机器人起始点坐标系下的坐标，假定当前帧的机器人在机器人起始点坐标系下的坐标已知且可信，由于测得的该坐标含有Mcr的信息，依此作标定，即可分别求得Mcr的旋转分量和平移分量.最终测得的值如图3、图4所示。

图3 纯平移标定实验Fig 3 Calibration experiment of pure translational motion

图4 纯旋转标定实验Fig 4 Calibration experiment of pure rotational motion

2.10 机器人起始点坐标系下的位移求解

根据Mcr得到机器人起始点坐标系下机器人的位移信息，由式（5）、式（6）、式（7）所示的步骤来求得。

3 实验与结果分析

为了验证视觉里程计算法的可行性，选用Motoman工业机器人进行实验，摄像机固定在机器人末端。

实验步骤:机器人分37步沿一个直径为300 mm的圆周行进，测得37组数据，再根据以上算法得到机器人的末端坐标，如图5所示。

图5 圆周实验Fig 5 Experiment of circular motion

机器人末端起始坐标为（0，0 mm），计算得到的机器人末端终点坐标为（3.021，5.844 mm），相对误差在2%以内，而到终点机器人走过的距离是930.954 mm，理论上走的距离为 942.478 mm，误差为 1.22%。

4 结束语

提出的里程计标定方法可以精确地得到摄像机坐标系与机器人坐标系的转换关系，能较为精确地得到机器人在机器人起始点坐标下的位移信息;因为采用了三维模型，所以，对摄像机的安装位姿无特殊要求。

本视觉里程计是基于单目视觉的，由图像特征点还原到摄像机坐标系下的点时，采用点的深度信息是一个固定值，因此，该视觉里程计只适用于较为平坦的室内地面，若采用双目视觉，可以适应室外地面。

［1］Matthies L，Shafer S.Error modeling in stereo navigation［J］.IEEE J Robot Automat，1987，3（3）:239 －250.

［2］Nistér D，Naroditsky O，Bergen J.Visual odometry［C］//Proceedings of IEEE Computer Society Conference on Computer Vision and Pattern Recognition，2004:652 －659.

［3］Nistér D，Naroditsky O.Visual odometry for ground vehicle applications［J］.Journal of Field Robotics，2006，23（1）:3 －20.

［4］Navid N V，Roberts J，Srinivasan M V.Practical visual odometry for car-like vehicles［C］//2009 IEEE International Conference on Robotics and Automation，Kobe International Conference Center，2009.

［5］Zhang Tianguang，Liu Xiaodong，Kuhnlenz K，et al.Visual odometry for the autonomous city explorer［C］//The 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems，2009:11－15.

［6］Bay H，Ess A，Tuytelaars T，et al.Speeded-up robust features（SURF）［J］.Computer Vision and Image Understanding，2008，110:346－359.

［7］Fischler M A，Bolles R C.Random sample consensus:Aparadigm for model fitting with applications to image analysis and automated cartography［J］.ACM Commun，1981，24（6）:381 －389.