专利名称：一种基于双三视张量的立体视觉快速导航定位方法
技术领域：
本发明属于视觉导航定位技术领域，涉及ー种基于双三视张量的立体视觉快速导航定位方法，可以应用于智能移动机器人、自主导航车等领域。
背景技术：
近年来国内外研究人员对移动机器人的定位方法与技术采用了各种定位传感器，研究出了许多不同的定位系统，主要有里程计法、惯性导航，全球定位导航。里程计法是移动机器人导航技术中最为广泛采用的定位方法。在移动机器人的车轮上装有光电编码器，通过对车轮转动的记录来计算出机器人位置和姿态。这种方法的最大缺点是车轮滑动引起的编码器读数错误。惯性导航法采用陀螺仪和加速度 计来实现定位，陀螺仪測量旋转速度，加速度计测量加速度。根据测量值的一次积分和二次积分可得到机器人的位置信息。这种方法的缺点是惯导设备随时间的误差漂移太大。全球定位系统是用于室外移动机器人导航与定位的技木。它是ー种以空间卫星为基础的高精度导航与定位系统。可是它在卫星信号封闭或受限的环境下，无法提供可靠的信息。比如在室内、水下、地下、战场、外星球等。目前，随着计算速度和视觉传感器性价比的不断提高以及图像处理和视觉计算的飞速发展，基于视觉的导航定位技术受到了研究者的广泛关注。该方法依赖硬件设备少、对体积、功耗、载荷等要求低，适用于各种复杂未知环境，且定位精度高，该方法在“勇气”号与“机遇”号火星车上也得到了成功的应用。视觉的导航定位技术仅依赖视觉输入进行运动估计，其关键是在ニ维图像序列中追踪视觉特征点进行机器人自身的运动估计。由于单目视觉定位存在尺度模糊性，因此，目前广泛采用立体视觉的定位方法。立体视觉导航定位方法一般通过特征提取、立体匹配、特征跟踪、运动估计得出智能车运动的距离和方向信息。由于运动前后的特征点可以通过立体匹配的视差计算得到其三维坐标，所以目前的立体视觉运动估计算法一般都采用基于3D-3D特征对应的运动估计算法。3D-3D特征对应的运动估计算法一般采用线性或非线性最小ニ乘方法使估计的特征点和测量的特征点之间的3D距离最小，但由于特征点(特别对于远距离特征点)3D坐标重建受噪声影响较大，因此这种方法得到的定位精度不高。
发明内容
本发明的目的在于克服现有技术中的不足和缺点，提出一种基于双三视张量的立体视觉快速导航定位方法。该方法包括如下步骤步骤ー、提取立体相机视频流当前帧左、右图像的BRISK特征点和各BRISK特征点对应图像位置的 U-BRISK 描述符(參见 L. Stefan, C. Margarita, s. Roland. BRISK =BinaryRobust Invariant Scalable Keypoints. In Proceedings of the IEEE InternationalConference on Computer Vision(ICCV)，2011)。步骤ニ、对当前帧左、右图像的BRISK特征点集进行--匹配，得到当前帧左、右图像BRISK特征点匹配对，并得到当前帧左、右图像BRISK特征点匹配对的3D坐标。步骤三、对当前帧左图像或右图像和前一帧左图像或右图像的BRISK特征点集进行一一匹配，并联合步骤ニ得到前后帧立体图像特征匹配点集。步骤四、采用RANSAC算法(随机采样一致性算法，參见M. A. Fischler andR.C.Bolles. Random sample consensus a paradigm for model fitting withapplications to image analysis and automated cartography. Commun. ACM，1981，24(6) :381-395.)和基于SVD的位姿估计方法得到匹配内点和当前帧与前一帧之间的立体相机运动參数(即当前巾贞运动參数)；所述运动參数包括相对位置參数、相对姿态參数，所述相对位置參数为平移參数t,所述相对姿态參数 为旋转參数R ；步骤五、建立双三视张量运动约束的非线性优化目标函数，采用Levenberg-Marquard算法对该目标函数当前巾贞运动參数进行迭代优化；运动參数的迭代初值为步骤四计算得到的运动參数值。步骤六、利用前ー帧的全局位姿參数和当前帧的运动參数计算得到当前帧的全局位姿參数，即相对于起始帧的位置和姿态，从而得到立体相机的导航定位信息。该方法采用ICCV2011提出的BRISK特征作为跟踪匹配的特征，该特征计算速度快，定位精度高，鲁棒性。立体匹配和前后帧匹配都采用局部描述符U-BRISK，该描述符计算速度快。由于U-BRISK描述符为ニ进制比特串，所以匹配速度也很快。立体匹配和前后帧匹配都采用U-BRISK描述符进行匹配，鲁棒性和定位的精确度都很高。传统的立体匹配采用如下的方法首先采用极线校正方法对立体图像对进行校正，使左右图像对应的特征点位于同一水平扫描线上，然后，对采用基于SAD(或NCC)的立体匹配方法沿水平方向极线进行一维搜索匹配，从而得到第一图像的特征在第二图像上的对应图像坐标。该方法的缺点是需要对立体视频流进行立体极线校正，对立体极线校正方法的精度依赖也较大。由于初始运动參数精度不高，需要采用优化方法对其进行进ー步的优化。传统的3D-3D优化方法在3D空间进行优化，但由于特征点(特别对于远距离特征点)3D坐标重建受噪声影响较大，因此这种方法得到的定位精度不高。本发明通过双三视张量运动约束方程直接采用立体相机的四个图像坐标进行运动參数优化，所以优化精度高。在本发明的立体视觉快速导航定位方法中，所述步骤一包括以下具体步骤(I)、提取立体相机视频流当前帧左、右图像的BRISK特征点，得到当前帧左、右图像的BRISK特征点的图像坐标(uLjk, vLjk)、(uEj k, vEjk)；(2)、计算在图像坐标(uL,k，vLjk)、(uEjk, vEjk)位置上的U-BRISK描述符；所述步骤ニ包括以下步骤(I)、取当前帧左图像或右图像中的任一个特征点BRISK描述符，和另ー图像中在视差阈值范围(0，Li)内的特征点BRISK描述符，计算二者之间的汉明距离，如果汉明距离小于汉明阀值T时则接受这ー对特征匹配点为匹配对，否则拒绝。(2)、利用已标定的立体相机左相机和右相机内部參数矩阵ん和Kk以及已标定的右相机相对左相机的旋转矩阵和平移向量も和b，采用线性最小ニ乘算法可得到当前帧左、右图像BRISK特征点的图像坐标(uL,k，vLjk)、(uEjk, vEjk)相对应的世界点3D坐标Y。所述步骤三包括以下步骤
(I)、取当前帧左图像或右图像中的任一个特征点U-BRISK描述符，和前一帧左图像或右图像中在视差阈值范围(-L2, L2)内的特征点U-BRISK描述符，计算二者之间的汉明距离，如果该汉明距离小于汉明阀值T时则接受这ー对特征匹配点为匹配点，否则拒绝；(2)、选择当前帧左、右图像特征匹配、前ー帧左、右特征匹配、当前帧和前ー帧图像特征匹配的公共匹配点集作为最后的前后帧立体图像特征匹配点集；设前ー帧左、右图像BRISK特征点的图像坐标分别为(Ul’h，vL, H)、(ιικ,Η，νκ,Η)，设前ー帧图像BRISK特征点的3D坐标为X。所述步骤四包括以下步骤(I)、在所述步骤三得到的前后帧立体图像特征匹配点集中随机采样3对匹配点；(2)、利用基于SVD的3D-3D位姿估计算法计算该3对匹配点的运动參数旋转矩阵R和平移向量t ;
(3)、利用步骤(2)中获得的该运动參数R和t，计算所有的前后帧特征匹配对在该运动模型下的3D坐标误差，即Y- (RX+t) I < τ (I)该误差小于阀值τ的前后帧特征匹配对称为匹配内点，存储匹配内点、运动參数R和t以及匹配内点数；(4)、重复步骤(I)、 (2)、 (3)多次，选取匹配内点数最多相对应的计算结果作为最后的匹配内点和初始运动參数Rtl和b。在本发明的方法中，步骤五所述的双三视张量运动约束的非线性优化目标函数为f ( Θ ) = (P ( Θ ) -m)T (P ( Θ ) -m),采用 Levenberg-Marquard 算法(參见袁亚湘、孙文瑜编著的《最优化理论与方法》，科学出版社)···)对该目标函数中当前帧运动參数进行迭代优化的计算步骤如下(ル计算洲ニゾワ+ガ厂ワ^バの-所卜则沒-=^+ ^。
rSPl(Q) δΡΝ(θ 其中ゾ=^f==叫...%
W SP1(X) dPN(x)
v—…ノ(2)、若 f ( Θ new) く f ( θ )则转向(4);否则进行(3)。(3)、若/(— nm^m < S，停止计算，输出結果。否则置λ = β λ，θ = θ_，转⑵。(4)、若(の·^の<g，停止计算，输出結果。否则置λ = λ/β，Θ = 0new,转⑵。在上述运动參数进行迭代优化步骤中，所述运动參数Θ = Lq1 q2 q3 X y ζ]τ*6维向量；其初始值为旋转矩阵R转化的四元数(q1; q2，q3，q4)，其中W +qf +も2+も2=I，q1;q2, q3为独立变量，所述平移參数t = [x y ζ]τ。在所述的非线性优化目标函数f(9) = (P(0)-m)T(P(0)-m)中，
其中
权利要求
1.一种基于双三视张量的立体视觉快速导航定位方法，其特征在于，包括如下步骤 步骤一、提取立体相机视频流当前帧左、右图像的BRISK特征点和各BRISK特征点对应图像位置的U-BRISK描述符； 步骤二、对当前帧左、右图像的BRISK特征点集进行一一匹配，得到当前帧左、右图像BRISK特征点匹配对，并得到当前帧左、右图像BRISK特征点匹配对的3D坐标； 步骤三、对当前帧左图像或右图像和前一帧左图像或右图像的BRISK特征点集进行一一匹配，并联合步骤二得到前后帧立体图像特征匹配点集； 步骤四、采用RANSAC算法和基于SVD的位姿估计方法得到匹配内点和当前帧与前一帧之间的立体相机运动参数；所述运动参数包括相对位置参数、相对姿态参数，所述相对位置参数为平移参数t,所述相对姿态参数为旋转参数R ； 步骤五、建立双三视张量运动约束的非线性优化目标函数，采用Levenberg-Marquard算法对该目标函数当前帧运动参数进行迭代优化；运动参数的迭代初值为步骤四计算得到的运动参数值； 步骤六、利用前一帧的全局位姿参数和当前帧的运动参数计算得到当前帧的全局位姿参数(移入说明书)，从而得到立体相机的导航定位信息。
2.根据权利要求I所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于，所述步骤一包括以下步骤 (1)、提取立体相机视频流当前帧左、右图像的BRISK特征点，得到当前帧左、右图像的BRISK 特征点的图像坐标(uL,k, vL,k)、(uEjk, vEjk)； (2)、计算在图像坐标(uL,k，vLjk)、(uEjk,vEjk)位置上的U-BRISK描述符。
3.根据权利要求I所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于，所述步骤二包括以下步骤 (1)、取当前帧左图像或右图像中的任一个特征点BRISK描述符，和另一图像中在视差阈值范围(O，L1)内的特征点BRISK描述符，计算二者之间的汉明距离，如果汉明距离小于汉明阀值T时则接受这一对特征匹配点为匹配对，否则拒绝； (2)、利用已标定的立体相机左相机和右相机内部参数矩阵&和Kk以及已标定的右相机相对左相机的旋转矩阵和平移向量&和k，采用线性最小二乘算法可得到当前帧左、右图像BRISK特征点的图像坐标(uL,k，vLjk)、(uEjk, vEjk)相对应的世界点3D坐标Y。
4.根据权利要求I所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于，所述步骤三包括以下步骤 (1)、取当前帧左图像或右图像中的任一个特征点U-BRISK描述符，和前一帧左图像或右图像中在视差阈值范围(-L2, L2)内的特征点U-BRISK描述符，计算二者之间的汉明距离，如果该汉明距离小于汉明阀值T时则接受这一对特征匹配点为匹配点，否则拒绝； (2)、选择当前帧左、右图像特征匹配、前一帧左、右特征匹配、当前帧和前一帧图像特征匹配的公共匹配点集作为最后的前后帧立体图像特征匹配点集；设前一帧左、右图像BRISK特征点的图像坐标分别为(Ul’h，vL, h)、(Ur’h，νκ,h)，设前一帧图像BRISK特征点的3D坐标为X。
5.根据权利要求I所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于，所述步骤四包括以下步骤(1)、在所述步骤三得到的前后帧立体图像特征匹配点集中随机采样3对匹配点； (2)、利用基于SVD的3D-3D位姿估计算法计算该3对匹配点的运动参数旋转矩阵R和平移向量t ; (3)、利用步骤(2)中获得的该运动参数R和t，计算所有的前后帧特征匹配对在该运动模型下的3D坐标误差，即 Y-(RX+t) I < τ (I) 该误差小于阀值τ的前后帧特征匹配对称为匹配内点，存储匹配内点、运动参数R和t以及匹配内点数； (4)、重复步骤(I)、(2)、(3)多次，选取匹配内点数最多相对应的计算结果作为最后的匹配内点和初始运动参数R(|和h。
6.根据权利要求I所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于，步骤五所述的双三视张量运动约束的非线性优化目标函数为f(0) = (P(0)-m)T (P ( θ ) -m),采用Levenberg-Marquard算法对该目标函数中当前巾贞运动参数进行迭代优化的计算步骤如下(1)、计算
7.根据权利要求6所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于，所述运动参数Θ = Lq1 q2 q3 X y ζ]τ为6维向量；其初始值为旋转矩阵R转化的四元数(q1; q2, q3, q4),其中+ +《22 +《32 =1，q1; q2, q3为独立变量，所述平移参数t = [x yζ]τ；并且
8.根据权利要求7所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于，ΡΚΛ = [ΚΛ hR 2 / 为双三视张量运动约束的点-线-点约束I ; Puk = VKi K2为双三视张量运动约束的点-线-点约束2 ； 其中I = TlrlR,k-i^ i = 1，2，3，/^ =Tl1U = I，2, 3，Ik,H 为经过图像点(ιικ,Η，νκ,k-i)的直线，其向量坐标为[-1 -I Um+VmJ'Pulrf为特征点的齐次图像坐标[Um vLjHtk-1丄」ο
9.根据权利要求8所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于， TE, i为前一帧左图像、前一帧右图像、当前帧右图像组成的三视张量 Tr>! = Ct1IZ4 - b4af, i = 1,2,3(2), TL, i为前一帧左图像、前一帧右图像、当前帧左图像组成的三视张量为 Tl>! = CtiCr4 - b4cf, i = 1,2,3(3)。
10.根据权利要求10所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于，a, b, c计算方式如下 把立体相机的前一帧左相机图像坐标系选为世界坐标系，左相机的3*4的投影矩阵为Ml,h = KL[I|0]，则 前一巾贞右相机的投影矩阵:Μκ,η = Ke [Rr I tr] = La1 a2 a3 a4] (4)； 当前巾贞左相机的投影矩阵Wuk = Kl [Rl I tL] = [C1 C2 C3 c4] (5),上式中 Rl = RrT · R，tL = RrT · t-RrT · tr ； 当前巾贞右相机的投影矩阵:MK,k = KE[R| t] = [Id1 b2 b3 b4] (6)。
全文摘要
本发明属于视觉导航定位技术领域，公开了一种基于双三视张量的立体视觉快速导航定位方法。该方法利用运动前后立体相机四幅图像的特征点图像坐标建立双三视张量运动约束对3D-3D位姿估计方法的初始运动参数进行优化，参数优化精确高。该方法采用ICCV2011提出的BRISK特征作为跟踪匹配的特征，该特征计算速度快，定位精度高，鲁棒性。立体匹配和前后帧匹配都采用局部描述符U-BRISK，该描述符计算速度快。由于U-BRISK描述符为二进制比特串，所以匹配速度也很快。立体匹配和前后帧匹配都采用U-BRISK描述符进行匹配，鲁棒性和定位的精确度都很高。
文档编号G06T7/20GK102682448SQ20121006633
公开日2012年9月19日 申请日期2012年3月14日 优先权日2012年3月14日
发明者刘勇, 蒋云良, 许允喜 申请人:浙江大学