基于立体视觉的移动机器人定位方法
【专利摘要】一种基于立体视觉的移动机器人定位方法。具体包含以下步骤：利用安装在机器人上的双目相机拍摄图像序列；提取立体图像序列中的尺度不变特征，利用圈匹配技术对特征匹配和跟踪；采用单位四元数法求解相邻帧间的旋转平移矩阵；通过最小化重投影误差法多次迭代优化获得最优运动参数。本发明首先针对某些环境下GPS等传统定位方法失效的情况，提出基于双目立体视觉的定位方法；其次本发明针对特征匹配容易出现错误匹配的问题，提出了圈匹配技术对特征匹配和跟踪，提高特征匹配的准确率，并用单位四元数法求解旋转平移矩阵，提高计算精度；最后本发明提出最小化重投影误差法，多次迭代优化获得较为准确的运动参数。
【专利说明】基于立体视觉的移动机器人定位方法
【技术领域】
[0001] 本发明属于计算机视觉【技术领域】，涉及一种基于立体视觉的移动机器人定位方 法，解决移动机器人利用GI^系统定位精度不高、室内环境无法利用GI^定位的困难，并提 高定位的准确性和鲁棒性，可W用于室内环境移动机器人定位，W及某些环境下GI^定位 不准确甚至失效情况下的移动机器人定位。
【背景技术】
[0002] 基于视觉的定位技术是利用安装在机器人或者车辆上的相机拍摄的图像序列对 车体进行定位的技术。传统的定位技术基于车轮编码器、惯性导航仪或者GI^系统对车辆 估算距离，在某些环境下（如车轮打滑、室内环境）该些方法会失效，基于视觉的定位技术 能够很好的克服该些问题，适用于室内外、斜坡、沙地或者其他未知环境（如月球、火星表 面）。
[0003] 基于视觉的定位技术依靠视觉输入，通过提取图像序列的尺度不变特征点，计算 前后连续峽间的旋转平移关系，估计车体的位置姿态和运动轨迹，主要步骤包括特征提取、 特征匹配与跟踪和运动估计。目前基于视觉定位的研究主要分为单目视觉定位和双目视觉 定位两种。单目视觉定位系统不易重建出图像点深度信息，不能为后续诸如同时定位与地 图构建等视觉导航提供足够有效的信息。双目视觉相对于单目视觉更容易获取精确的场景 深度信息，使得视觉定位技术的实现相对可靠，因此更多的研究者投入到了双目立体视觉 定位技术的研究中。
[0004] 如何选取合适的鲁棒特征，如何准确的进行特征匹配和跟踪，如何精确的计算出 前后峽间的旋转平移矩阵是研究的重点和难点。由于视觉定位是基于图像特征的，因此选 取的特征是否可靠、特征匹配和跟踪是否准确对最终结果有很大影响；运动估计本质是求 解运动参数，即前后峽的特征点间的旋转平移矩阵，因此旋转平移矩阵计算的精度影响了 最后的定位精度。
【发明内容】
[0005] 本发明的目的是克服现有技术存在的上述问题，提出一种基于双目立体视觉的移 动机器人定位方法。由于某些环境下（如车轮打滑、室内环境）GI^等传统移动机器人定位 方法不准确甚至失效，基于视觉的定位技术能够很好的克服该些问题，适用于室内外各类 环境，双目立体视觉能很好的获得场景深度信息，有利于提高定位的精度。
[0006] 本发明提供的基于双目立体视觉的移动机器人定位方法，该方法具体包含W下步 骤：
[0007] 第1、图像采集和预处理
[0008]首先在移动机器人上安装双目立体相机，在场景中漫游并拍摄立体图像序列；
[0009] 第2、尺度不变特征提取
[0010]结合化enCV提取图像序列每一峽图像的SIFT特征点；
[0011] 定位的准确度与图像特征的鲁棒性有很大的关系，提取图像序列的SIFT(Scale InvariantFea化reTransform)特征点,SIFT特征具有尺度、旋转不变性，提取的特征点较 多，并且能精确到亚像素，在鲁棒性和精度上明显优于其他特征；
[0012] 第3、特征匹配与跟踪
[0013] 在第2步获得图像序列的尺度不变特征后，对同一峽左图右图的特征进行匹配并 对前后峽图像的特征进行跟踪，用圈匹配技术同时进行特征匹配和跟踪。首先得到四幅图 像的特征点，四幅图像分别为当前峽的左图、右图和上一峽的左图、右图，然后按照该样的 顺序进行匹配：当前峽左图-〉上一峽左图-〉上一峽右图-〉当前峽右图-〉当前峽左图， 完成一圈匹配。圈匹配技术获得的匹配特征点更可靠，误配点较少，有利于准确进行运动估 计；
[0014] 第4、运动估计
[0015] 机器人的运动属于刚体运动，可分解为旋转运动和平移运动，根据第3步获得的 前后峽匹配的特征点，用单位四元数法计算特征点间的旋转矩阵和平移向量；
[0016] 在二维平面上，一个单位圆上任意一点可W表示绕一个轴旋转的角度；在H维空 间中，一个单位球面上任意一点可W表示绕两个轴旋转的角度；因此可W假设存在一个四 维空间中的一个单位球，球面上任一点可W表示绕H个轴旋转的角度，四维空间中的单位 球定义如下：
[0017] q〇'+qi'+q/+q3^ = 1
[001引球面上任意一点的四元坐标即单位四元数q= [q。，屯，屯，Q3]t，旋转矩阵可表示 为：
【权利要求】
1. 一种基于立体视觉的移动机器人定位方法，其特征在于该方法具体包含以下步骤： 第1、图像采集和预处理 首先在移动机器人上安装双目立体相机，在场景中漫游并拍摄图像序列； 第2、尺度不变特征提取 结合OpenCV提取图像序列每一帧图像的SIFT特征点； 第3、特征匹配与跟踪 在第2步获得图像序列的尺度不变特征后，用圈匹配技术同时进行特征匹配和跟踪； 首先得到四幅图像的特征点，四幅图像分别为当前帧的左图、右图和上一帧的左图、右图， 然后按照这样的顺序进行匹配：当前帧左图_>上一帧左图_>上一帧右图_>当前帧右图_> 当前帧左图，完成一圈匹配； 第4、运动估计 机器人的运动属于刚体运动，可分解为旋转运动和平移运动，根据第3步获得的前后 帧匹配的特征点，用单位四元数法计算特征点间的旋转矩阵和平移向量： 在二维平面上,一个单位圆上任意一点可以表示绕一个轴旋转的角度；在三维空间中， 一个单位球面上任意一点可以表示绕两个轴旋转的角度；因此可以假设存在一个四维空间 中的一个单位球，球面上任一点可以表示绕三个轴旋转的角度，四维空间中的单位球定义 如下： 2 I 2 I 2 I 2 - Qo +? +? +? 一 1 球面上任意一点的四元坐标即单位四元数q = Iiqtl, qp q2, q3]T，旋转矩阵可表示为：
单位四元数法是描述三维空间旋转较好的方法，该方法没有奇异点，并且适用较大角 度旋转，本发明采用单位四元数法表示并计算旋转矩阵； 第5、最小化重投影误差 计算得到的运动参数和实际运动参数不可避免存在误差，需要对误差最小化才能得到 准确的运动参数，运动参数即旋转矩阵和平移向量；三维欧拉空间中误差分布各向异性，噪 声分布不均匀，因此提出在二维图形空间下最小化误差的方法；根据运动参数计算得到运 动后的三维特征点，重投影到二维图像空间下，然后计算与第2步中提取到的真实特征点 之间的误差，多次迭代使得误差距离最小，即可得到较为准确的运动参数； 二维图像上的像素点可以根据相机参数计算得到三维空间下的坐标，同样，三维空间 下的点也可以根据相机参数计算得出投影到二维图像上的像素坐标，由三维空间点计算得 到二维图像像素点的过程称为重投影； 设上一帧图像的某个特征点的三维坐标为P(x，y，z)，变化到当前帧坐标系的旋转平移 矩阵分别为R和T，重投影到当前帧图像上的像素坐标为（u，V)，则重投影公式为：
其中，（U，v)是图像坐标；CjPcu是相机透镜光心在图像上的投影所在的行和列；f是 相机的焦距；(X，y，Z)是前一帧特征点的三维坐标；r和t是一次迭代计算得到的旋转矩阵 和平移向量；如果投影到左图S = 0,如果投影到右图S = B ;B是相机基线。
2. 根据权利要求1所述的方法，其特征在于第3步特征匹配与跟踪的具体方法包括： 第3. 1、圈匹配 在步骤02中获得图像序列各图像特征的基础上，本发明采用圈匹配技术同时完成左 右图的特征匹配和前后帧的特征跟踪； 第3. 2、计算特征点三维坐标 同一帧左图和右图特征匹配是为了计算特征点的三维坐标，假设左图上一个特征点位 于图像上的u列V行，则该特征点的坐标为（Ul，Vl)，右图上所匹配的特征点为（w，')，则左 图上的特征点由图像坐标转换到三维坐标（x，y，z)有下列公式：
其中，d是视差，即该特征点在右图对应列坐标之差d = U1-U1^ ;cv和Cu是相机透镜光 心在图像上的投影所在的行和列，f是相机的焦距，单位为像素，B为相机基线长度，单位为 米，通过上式能够计算得到所有特征点的三维坐标。
3. 根据权利要求1所述的方法，其特征在于该方法第4步采用单位四元数法计算特征 点间的旋转矩阵和平移向量的方法包括： 第4.1、数学模型的建立 假设在前一帧与当前帧间有n个特征匹配对，对应的三维坐标分别为（Pi I i = 1，--?!!}， {P/ I i = 1，…n},其中Pi = (Xi, y。Zi), P/ = (x/，y/，z/ ), P为前一巾贞特征点的三维坐 标，P'为当前帧特征点的三维坐标，则具有如下关系 Pi' = RPj+T (3) 其中，R代表旋转矩阵，T代表平移矢量，表明了机器人在前一巾贞与当前巾贞间的相对位 姿变化；R是3X3的正交旋转矩阵，自由度为3 ; 第4. 2、运动参数求解 为了求得旋转矩阵R和平移向量T，最少需要三对前后帧中相对应的特征点，单位四元 数法求解步骤如下： (a)求取匹配的特征点集（Pi (Xi, Zi) I i = 1，…n}, {P/ (x/，y/，z/ ) I i = 1，…n} 的质心坐标：
(b)两个特征点集的对应点均减去各自的质心坐标，得到新的点集 {可无，另，?；.)|/ = 1，.__?}，拷供，，:?；'，?；，)|/ = 1，...《}，其中，
其中，tr (Q)是矩阵 Q 的迹，E3 是 3X3 的单位矩阵，A= [A23 A31 A12LAij = (Q-Qt) ij; (e) 计算矩阵K的特征值和特征向量，其中最大特征值对应的特征向量即为单位四元 数 q = [q〇, Qi，％，(?]; (f) 把q代入⑴式计算旋转矩阵R ; (g) 最后计算平移向量;T =
4.根据权利要求1所述的方法，其特征在于该方法第5步所述把三维特征点重投影到 二维图像空间下，多次迭代最小化重投影误差的方法包括： 第5.1、（3)式是运动估计模型的理想表达式，而实际误差是不可避免的，对于实际情 况可改写为：P/ = RPi+T+e，其中e为误差；误差e是代表了由旋转平移矩阵计算得到的三 维坐标和实际坐标之间的误差，使误差最小化，才能得到比较正确的运动估计； 求得旋转平移矩阵后，根据（2)式把前一帧特征点的三维坐标变换到当前帧坐标系下 并重投影成图像坐标，计算重投影成的图像坐标和当前帧特征点的实际图像坐标之间的误 差，多次迭代求得使重投影误差最小的旋转矩阵和平移向量，即为所求运动参数； 第5. 2、令(1) (Pi ;r，t)表示前一帧左图上的第i个特征APi根据旋转矩阵r和平移向 量t重投影到当前帧的左图平面上，同理Jiw (Pi ;r，t)表示Pi重投影到右图平面上，Ii'(1)和I/ W分别表示当前帧左图和右图上特征点的实际像素坐标，则通过最小化下式来选择最 优运动参数，
E为残差平方和，通过多次迭代求得使E最小的r和t作为最终的运动参数；具体步骤 如下： (1) 在n个匹配特征点对中随机抽取w个点对；由于三个非共线的特征点可以唯一的 确定运动参数，所以这里我们取w = 3 ; (2) 如果3点共线则重新选取，直到不共线时用四元数法计算出运动参数（rk，tk); (3) 根据步骤（2)求得的运动参数计算特征点匹配集的重投影误差Ek; (4) 重复以上步骤（1)至步骤（3)m次，在m个重投影误差中寻找最小值，Eniin = HiinEkk = 1，2, ".m ; (5) 此时Emin对应的（r，t)便是我们要求得的最优运动参数。
【文档编号】G01C11/08GK104359464SQ201410604569
【公开日】2015年2月18日 申请日期:2014年11月2日 优先权日:2014年11月2日
【发明者】刘少强, 张桦, 徐光平, 薛彦兵, 高赞, 徐珂琼 申请人:天津理工大学