机器双目视觉里程计定位算法

黄秀珍，王佳斌，高发钦，冉宇瑶

(1.浙江理工大学 科技与艺术学院，浙江 上虞 312369；2.浙江理工大学，浙江 杭州 315700)

0 引言

随着时代进步，人工智能、图像处理及计算机视觉等技术迅速发展，基于这些技术的应用，如智能机器人、无人机和无人驾驶汽车等已经广泛出现在人们生活的各个领域和方面[1]。无论智能机器人还是无人驾驶汽车，空间定位是最基本和关键的问题。视觉图像中含有丰富的位置和空间信息[2]，机器视觉主要通过摄像机来模拟人的视觉系统。相对惯性导航，机器视觉定位误差对累积误差的抑制能力更强，不会受到未知空间、环境和长距离的影响[3]。

机器视觉里程计分为单目视觉里程计和立体视觉里程计，其中单目视觉里程计需要外部信息进行约束才能获取空间尺度，立体视觉里程计可以通过三角测量获得特征点深度信息，根据深度信息和特征点的二维位置求出相机的位姿变化，最终实现空间定位。立体视觉里程计中双目视觉容易实现，仅需2个相机[4]。

本文提出机器双目视觉里程计定位算法，假定相机的初始位置已知，相机的实时位置信息可由初始位置和计算出的位姿变化通过递推计算得到[5]。

1 里程计定位算法设计

本文研究机器双目视觉里程计定位算法，主要经过相机标定、图像处理、特征提取和匹配以及运动估计最终得到定位结果。

相机标定采用Matlab自带的基于张正友标定法的APP实现。图像处理主要是为了减小计算量，加快程序运行速度，包含灰度化和图像校正2步。特征提取本文选择了基于自定义模板的特征点提取方法，能够提取变化明显的特征点，特征匹配采取了鲁棒性较强的环形匹配。运动估计首先使用RANSAC随机抽样3组点，然后使用P3P算法计算它们各自在相机坐标系下的三维位置，然后利用Bundle Adjustment进行优化，利用高斯牛顿法求解位姿，并将重投影误差和最小的位姿变换作为最优位姿变换。然后根据前一帧的位置即可计算出当前帧的位姿完成定位。

1.1 相机标定及图像处理算法

相机标定的目的是获取2个相机的内参模型和外参模型。综合考虑标定精确度、标定快慢以及标定物制作难易程度，本文使用张正友标定法[6]。由于在Matlab 2016b已经集成了该方法的标定APP，故只需准备好标定板，拍摄不同角度的图像，再导入计算机，利用Matlab中的Stereo camera calibrator进行计算即可。

图像处理包含灰度化和校正[7]。相机拍摄得到的图像为彩色图像，为了加快程序运行的速度，需要对图像进行灰度化处理。本文选择加权平均法对图像进行灰度化处理，该方法灰度化效果最好。图像校正是为了使左右相机之间的像素行对准，方便视差计算也能加快特征匹配。本文选择常用的Bouguet算法进行校正。

1.2 特征提取及匹配算法

特征提取是为了提取图像中一些特别的地方即特征[8]，本文选择较易提取和信息量大的点特征。图像特征点提取的过程可以分为2步：首先是找关键点，灰度值发生剧烈变化的点可以作为关键点，然后计算描述子，描述子描述了关键点及其周围像素的信息。

选择基于模板的特征点检测方法，计算速度较快且鲁棒性较高[9]。选用的5×5的Blod模板和5×5的Corner模板，如图1所示。输入图像进行计算，计算Blod算子和Corner算子的核函数响应，获得候选特征点[10]。

接下来计算描述子。本文所选描述子计算方法是计算特征点周围11×11的窗口中16个点的Sobel滤波响应，如图2所示。

图1 Blod模板和Corner模板

图2 描述子和环形匹配

选择最小平方差(SSD)来计算匹配代价。计算方法如式(1)：

(1)

式中，i，j表示图像物理坐标系中某点到特征点(x,y)的水平和垂直距离；w表示16个点的位置集合；L表示图2(a)中某点的Sobel滤波响应；R表示图2(b)中某点的Sobel滤波响应；D表示视差。

环形匹配如图2(b)所示。从前一帧的左图像中的一个特征点Pl1出发，找到该点在前一帧右图像中的对应点Pr1，左右2幅图像之间采用极线搜索和块匹配来寻找对应点，采用最小平方差(SSD)来计算匹配代价。然后根据前一帧右图像中的点Pr1，找到当前帧右图像中的对应点Pr2。再使用极线搜索和块匹配找到当前帧左图像中匹配点Pl2的位置。然后计算Pl1和Pl2的匹配代价，若代价基本一致，则表示实现了环形匹配，否则失败，选另一个特征点进行环形匹配。

1.3 运动估计算法

本文使用的运动估计方法先用P3P算法[11]计算出一个初始位姿变换，然后用Bundle Adjustment算法进行优化。P3P算法只需要3个点对就可以估计得到位姿变化[12]。

如图3所示，A,B,C三点为世界坐标系下特征点位置，世界坐标系是真实世界的坐标系也是系统的绝对坐标系。a,b,c为对应点在图像像素坐标系下的位置。由已知像素坐标，采用余弦定理计算出A,B,C三点在相机坐标系下的三维位置。假设相机坐标系中心点为O。

图3 投影模型

记x=OA/OC,y=OB/OC,v=AB2/OC2,uv=BC2/OC2,wv=AC2/OC2，x表示投影模型OA边和OC边的比值，其他类似，这样方便根据余弦定理推出：

(1-u)y2-ux2-cos〈b,c〉y+2uxycos〈a,b〉+1=0，(1-w)x2-wy2-cos〈a,c〉x+2wxycos〈a,b〉+1=0 ，

(2)

式中，x,y为未知量，用吴消元法求解得到A,B,C在相机坐标系下的3D坐标。如图4所示。

图4 前后图像投影比较

图4中，Pi表示匹配特征点集，A,B,C属于Pi，ui表示Pi的投影点集，a,b,c属于ui。因为同一Pi在前后帧的相机坐标系下的三维位置已知，可以计算Pi的位姿变换，得到相机的位姿变换。

为了提高数据利用率，本文采取RANSAC算法随机选择所使用的特征点，然后求解最小化重投影误差[13]。重投影是相对于相机第一次采集图像而言，所谓重投影是将P点再次投影到成像平面上，如图5所示。

图5 重投影模型

图5中，P1是P点在像素坐标系中的实际位置，P2是相机位姿变化后的P点投影到成像平面上的位置，P2和P1之间的距离就是重投影误差，要不断调整投影角度，使P2和P1之间的距离e最小。

考虑到多个空间点P和它们对应的投影点，将旋转矩阵R和平移向量t用李代数表示为ξ，假设某个空间点表示为Pi=[Xi,Yi,Zi]T，其投影的像素坐标为Ui=(ui,vi)T，式(3)表示对n个投影点误差求和，单位是像素，最终确定误差和最小时的变换矩阵。

(3)

式中，si为深度距离，可由三角测量和视差原理得到；K为内参矩阵由标定得到；exp(ξ∧)是从世界坐标系到相机坐标系的变换矩阵。

最小二乘优化[14]计算过程主要分成2步，第1步用RANSAC采样选取特征点，第2步用高斯牛顿法求解位姿变换Ti。

位姿计算的输入是前面采样得到的一组匹配特征点的像素坐标中的点，一组点有4个像素坐标系下的位置，分别是(u1c,v1c),(u2c,v2c),(u1p,v1p),(u2p,v2p) 。1表示左，2表示右，c表示当前帧，p表示前一帧。(X1c,Y1c,Z1c),(X1p,Y1p,Z1p)由P3P算法计算得到。(X2c,Y2c,Z2c),(X2p,Y2p,Z2p) 由对极几何原理得到：

(4)

旋转矩阵和平移向量可以表示载体的位置和姿态变化。为了简化计算，用欧拉角来表示旋转矩阵，用Ti={αi,βi,γi,txi,tyi,tzi}表示迭代中第i次的位姿变换。前3个参数是相机间关于X,Y,Z轴各自的旋转，后3个是关于X,Y,Z轴方向上各自的平移。旋转矩阵用欧拉角可表示为:

(5)

Ti通过RANSAC迭代，在每次迭代中都从匹配点中随机抽取3个点通过高斯牛顿法求出。

将(X1c,Y1c,Z1c),(X2c,Y2c,Z2c)变换二维坐标的过程就是重投影。依据重投影后的二维坐标(u,v)，其雅克比计算方式为:

(6)

进行高斯牛顿迭代，通过计算3组点中的雅克比矩阵，最终，可以得到：

(7)

式中，N为抽样组数，本文N为3，J为6×12的矩阵。

(8)

往复迭代，直到T收敛(|T(i+1)-T(i)|<ε)。

(9)

式中，τ为人为设定。

得到最优位姿T后再将其变换成R和t，由式(10)计算得到世界坐标系下当前帧的位置。

(10)

对于运动过程中采集的连续图像序列可以采取递进的方式来计算位置，只要知道初始帧的坐标，之后的坐标都可以在前一帧的基础上得到。

2 仿真结果及分析

选取了KITTI数据集，该数据集由德国卡尔斯鲁厄理工学院和丰田美国技术研究院联合创办，是较大的计算机视觉算法评测数据集，取其中不同街道下的5组80帧连续图像作为输入，运用本文算法进行计算，并和汽车自带IMU(惯性测量单元)测得数据进行比较。

将KITTI数据集中的标号为19和20的数据集作为本文数据集1和数据集2，各选取连续80帧图像，在Matlab下运行本文程序得到图6。

图6 不同数据集下汽车轨迹图展示

图6展示了不同数据集下的汽车运动路径及最终位置，其中曲线表示汽车的运动轨迹，以初始帧为初始状态，此时汽车位于(0,0)处，横轴表示汽车较初始位置的左右变化，纵轴表示汽车较初始位置的前后变化，单位为m，高度看作不变。

任选数据集1中的某一帧，展示前后帧的位姿变换矩阵，如下所示：

计算该位姿变换时所用的匹配特征点为241对，局内点为87.6%，精确度较高。本文将汽车初始位置设为(0,0,0)，则最终的位置为(8.39,133.5，-17.1)。

采用绝对轨迹误差ATE进行误差评估，绝对轨迹误差ATE的计算如式(11)所示。

(11)

式中，Ei为第i帧的误差，xi,yi为通过视觉计算得到的第i帧相机的水平位置，Xi,Yi表示IMU测量得到的第i帧相机的水平位置。

统计所有时刻的绝对轨迹误差，然后求平均就可以得到平均绝对误差MATE：

(12)

终点轨迹误差FATE由IMU测量终点和由视觉里程计测量终点依据式(11)计算出。

本文选取了5组不同数据集，来自KITTI数据集中的标号为19～23的数据集，在这5组数据集中各选取80帧连续图像进行计算，结果如表1所示。

表1 不同数据集定位精度比较

数据集MATE/mFATE/m134.667.2218.336.137.426.245.18.7568129平均26.6853.44

由表1可知，在这5组数据集下，本文算法平均MATE在26.68 m，平均FATE为53.44 m。精度最高的是第4组数据，第4组中汽车接近直线行驶，且马路上没有其他车辆，只有路边建筑和地面在变化，因此准确度较高。第5组误差最大，但也是接近直线行驶，不同之处在于马路上汽车较多，因此推测其他车辆对视觉定位精度影响较大。

3 结束语

本文通过研究实现了双目视觉里程计的定位算法，主要经过相机标定、图像处理、特征提取和匹配以及运动估计算法，最终得到行驶汽车的运动轨迹，精度较高。但适应性不强，在马路上没有其他车辆的情况下精度才比较高。不过，可以使用视觉与其他导航方法相结合，如惯性、GPS等结合，以此来提高精确度。