CN104359464A - 基于立体视觉的移动机器人定位方法 - Google Patents

Abstract

一种基于立体视觉的移动机器人定位方法。具体包含以下步骤：利用安装在机器人上的双目相机拍摄图像序列；提取立体图像序列中的尺度不变特征，利用圈匹配技术对特征匹配和跟踪；采用单位四元数法求解相邻帧间的旋转平移矩阵；通过最小化重投影误差法多次迭代优化获得最优运动参数。本发明首先针对某些环境下GPS等传统定位方法失效的情况，提出基于双目立体视觉的定位方法；其次本发明针对特征匹配容易出现错误匹配的问题，提出了圈匹配技术对特征匹配和跟踪，提高特征匹配的准确率，并用单位四元数法求解旋转平移矩阵，提高计算精度；最后本发明提出最小化重投影误差法，多次迭代优化获得较为准确的运动参数。

Description

基于立体视觉的移动机器人定位方法

技术领域

本发明属于计算机视觉技术领域，涉及一种基于立体视觉的移动机器人定位方法，解决移动机器人利用GPS系统定位精度不高、室内环境无法利用GPS定位的困难，并提高定位的准确性和鲁棒性，可以用于室内环境移动机器人定位，以及某些环境下GPS定位不准确甚至失效情况下的移动机器人定位。

背景技术

基于视觉的定位技术是利用安装在机器人或者车辆上的相机拍摄的图像序列对车体进行定位的技术。传统的定位技术基于车轮编码器、惯性导航仪或者GPS系统对车辆估算距离，在某些环境下(如车轮打滑、室内环境)这些方法会失效，基于视觉的定位技术能够很好的克服这些问题，适用于室内外、斜坡、沙地或者其他未知环境(如月球、火星表面)。

基于视觉的定位技术依靠视觉输入，通过提取图像序列的尺度不变特征点，计算前后连续帧间的旋转平移关系，估计车体的位置姿态和运动轨迹，主要步骤包括特征提取、特征匹配与跟踪和运动估计。目前基于视觉定位的研究主要分为单目视觉定位和双目视觉定位两种。单目视觉定位系统不易重建出图像点深度信息，不能为后续诸如同时定位与地图构建等视觉导航提供足够有效的信息。双目视觉相对于单目视觉更容易获取精确的场景深度信息，使得视觉定位技术的实现相对可靠，因此更多的研究者投入到了双目立体视觉定位技术的研究中。

如何选取合适的鲁棒特征，如何准确的进行特征匹配和跟踪，如何精确的计算出前后帧间的旋转平移矩阵是研究的重点和难点。由于视觉定位是基于图像特征的，因此选取的特征是否可靠、特征匹配和跟踪是否准确对最终结果有很大影响；运动估计本质是求解运动参数，即前后帧的特征点间的旋转平移矩阵，因此旋转平移矩阵计算的精度影响了最后的定位精度。

发明内容

本发明的目的是克服现有技术存在的上述问题，提出一种基于双目立体视觉的移动机器人定位方法。由于某些环境下(如车轮打滑、室内环境)GPS等传统移动机器人定位方法不准确甚至失效，基于视觉的定位技术能够很好的克服这些问题，适用于室内外各类环境，双目立体视觉能很好的获得场景深度信息，有利于提高定位的精度。

本发明提供的基于双目立体视觉的移动机器人定位方法，该方法具体包含以下步骤：

第1、图像采集和预处理

首先在移动机器人上安装双目立体相机，在场景中漫游并拍摄立体图像序列；

第2、尺度不变特征提取

结合OpenCV提取图像序列每一帧图像的SIFT特征点；

定位的准确度与图像特征的鲁棒性有很大的关系，提取图像序列的SIFT(ScaleInvariant Feature Transform)特征点，SIFT特征具有尺度、旋转不变性，提取的特征点较多，并且能精确到亚像素，在鲁棒性和精度上明显优于其他特征；

第3、特征匹配与跟踪

在第2步获得图像序列的尺度不变特征后，对同一帧左图右图的特征进行匹配并对前后帧图像的特征进行跟踪，用圈匹配技术同时进行特征匹配和跟踪。首先得到四幅图像的特征点，四幅图像分别为当前帧的左图、右图和上一帧的左图、右图，然后按照这样的顺序进行匹配：当前帧左图->上一帧左图->上一帧右图->当前帧右图->当前帧左图，完成一圈匹配。圈匹配技术获得的匹配特征点更可靠，误配点较少，有利于准确进行运动估计；

第4、运动估计

机器人的运动属于刚体运动，可分解为旋转运动和平移运动，根据第3步获得的前后帧匹配的特征点，用单位四元数法计算特征点间的旋转矩阵和平移向量；

在二维平面上，一个单位圆上任意一点可以表示绕一个轴旋转的角度；在三维空间中，一个单位球面上任意一点可以表示绕两个轴旋转的角度；因此可以假设存在一个四维空间中的一个单位球，球面上任一点可以表示绕三个轴旋转的角度，四维空间中的单位球定义如下：

q₀ ²+q₁ ²+q₂ ²+q₃ ²＝1

球面上任意一点的四元坐标即单位四元数q＝[q₀,q₁,q₂,q₃]^T，旋转矩阵可表示为：

$R=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2(q_1{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]$

单位四元数法是描述三维空间旋转较好的方法，该方法没有奇异点，并且适用较大角度旋转，本发明采用单位四元数法表示并计算旋转矩阵；

第5、最小化重投影误差

计算得到的运动参数和实际运动参数不可避免存在误差，需要对误差最小化才能得到准确的运动参数，运动参数即旋转矩阵和平移向量。三维欧拉空间中误差分布各向异性，噪声分布不均匀，因此提出在二维图形空间下最小化误差的方法。根据运动参数计算得到运动后的三维特征点，重投影到二维图形空间下，然后计算与第2步中提取到的真实特征点之间的距离，多次迭代使得误差距离最小，即可得到较为准确的运动参数。

二维图像上的像素点可以根据相机参数计算得到三维空间下的坐标，同样，三维空间下的点也可以根据相机参数计算得出投影到二维图像上的像素坐标，由三维空间点计算得到二维图像像素点的过程称为重投影；

设上一帧图像的某个特征点的三维坐标为P(x,y,z)，变化到当前帧坐标系的旋转平移矩阵分别为R和T，重投影到当前帧图像上的像素坐标为(u,v)，则重投影公式为：

$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}f& 0& c_u\\ 0& f& c_v\\ 0& 0& 1\end{array}\right)[\left(\begin{array}{cc}r& t\end{array}\right)\left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)-\left(\begin{array}{c}s\\ 0\\ 0\end{array}\right)]$

其中，(u,v)是图像坐标；c_v和c_u是相机透镜光心在图像上的投影所在的行和列；f是相机的焦距；(x,y,z)是前一帧特征点的三维坐标；r和t是一次迭代计算得到的旋转矩阵和平移向量；如果投影到左图s＝0，如果投影到右图s＝B；B是相机基线。

本发明第3步所述的特征匹配与跟踪的具体方法包括：

第3.1、圈匹配

在步骤02中获得图像序列各图像特征的基础上，本发明采用圈匹配技术同时完成左右图的特征匹配和前后帧的特征跟踪；

第3.2、计算特征点三维坐标

同一帧左图和右图特征匹配是为了计算特征点的三维坐标，假设左图上一个特征点位于图像上的u列v行，则该特征点的坐标为(u_l,v_l)，右图上所匹配的特征点为(u_r,v_r)，则左图上的特征点由图像坐标转换到三维坐标(x,y,z)有下列公式：

$\left\{\begin{array}{c}x=\frac{(u_l-c_u)\·B}{d}\\ y=\frac{(v_l-c_v)\·B}{d}\\ z=\frac{f\·B}{d}\end{array}\right.;$

其中，d是视差，即该特征点在右图对应列坐标之差d＝u_l-u_r；c_v和c_u是相机透镜光心在图像上的投影所在的行和列，f是相机的焦距，单位为像素，B为相机基线长度，单位为米，通过上式能够计算得到所有特征点的三维坐标。

本发明第4步所述的采用单位四元数法计算特征点间的旋转矩阵和平移向量的方法包括：

第4.1、数学模型的建立

假设在前一帧与当前帧间有n个特征匹配对，对应的三维坐标分别为{P_i|i＝1,…n}，{P_i'|i＝1,…n}，其中P_i＝(x_i,y_i,z_i)，P_i'＝(x_i',y_i',z_i')，P为前一帧特征点的三维坐标，P'为当前帧特征点的三维坐标，则具有如下关系

P_i'＝RP_i+T     (1)

其中，R代表旋转矩阵，T代表平移矢量，表明了机器人在前一帧与当前帧间的相对位姿变化；R是3×3的正交旋转矩阵，自由度为3；

设单位四元数为q＝[q₀,q₁,q₂,q₃]^T，其中q₀ ²+q₁ ²+q₂ ²+q₃ ²＝1，旋转矩阵表示为：

$R=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2(q_1{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]---\left(2\right)$

第4.2、运动参数求解

为了求得旋转矩阵R和平移向量T，最少需要三对前后帧中相对应的特征点，单位四元数法求解步骤如下：

(a)求取匹配的特征点集{P_i(x_i,y_i,z_i)|i＝1,…n}，{P_i'(x_i',y_i',z_i')|i＝1,…n}的质心坐标：

$\left\{\begin{array}{c}{\stackrel{\‾}{P}}^{\′}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P_i}^{\′}\\ \stackrel{\‾}{P}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_i\end{array}\right.;$

(b)两个特征点集的对应点均减去各自的质心坐标，得到新的点集 $\left\{{\tilde{P}}_i({\tilde{x}}_i,{\tilde{y}}_i,{\tilde{z}}_i)\right|i=1,...n\},\{{{\tilde{P}}_i}^{\′}({{\tilde{x}}_i}^{\′},{{\tilde{y}}_i}^{\′},{{\tilde{z}}_i}^{\′})|i=1,...n\},$ 其中，

$\left\{\begin{array}{c}{\tilde{P}}_i=P_i-{\stackrel{\‾}{P}}_i\\ {{\stackrel{\‾}{P}}_i}^{\′}={P_i}^{\′}-{{\stackrel{\‾}{P}}_i}^{\′}\end{array}\right.;$

(c)计算协方差矩阵其中

$\left\{\begin{array}{c}P=\left[\begin{array}{cccc}{\tilde{P}}_1& {\tilde{P}}_2& ...& {\tilde{P}}_n\end{array}\right]\\ P^{\′}=\left[\begin{array}{cccc}{{\tilde{P}}_1}^{\′}& {{\tilde{P}}_2}^{\′}& ...& {{\tilde{P}}_n}^{\′}\end{array}\right]\end{array}\right.;$

(d)构造4×4的矩阵

$K=\left[\begin{array}{cc}\mathrm{tr}\left(Q\right)& {\mathrm{\Δ}}^T\\ \mathrm{\Δ}& Q+Q^T-\mathrm{tr}\left(Q\right)E_3\end{array}\right];$

其中，tr(Q)是矩阵Q的迹，E₃是3×3的单位矩阵，△＝[A₂₃ A₃₁ A₁₂]，A_ij＝(Q-Q^T)_ij；

(e)计算矩阵K的特征值和特征向量，其中最大特征值对应的特征向量即为单位四元数q＝[q₀,q₁,q₂,q₃]^T；

(f)把q代入(2)式计算旋转矩阵R；

(g)最后计算平移向量

本发明第5步所述把三维特征点重投影到二维图像空间下，多次迭代最小化重投影误差的方法包括：

第5.1、(1)式是运动估计模型的理想表达式，而实际误差是不可避免的，对于实际情况可改写为：P_i'＝RP_i+T+e，其中e为误差；误差e是代表了由旋转平移矩阵计算得到的三维坐标和实际坐标之间的误差，使误差最小化，才能得到比较正确的运动估计；

求得旋转平移矩阵后，根据下面(3)式把前一帧特征点的三维坐标变换到当前帧坐标系下并重投影成图像坐标，计算重投影成的图像坐标和当前帧特征点的实际图像坐标之间的误差，多次迭代求得使重投影误差最小的旋转矩阵和平移向量，即为所求运动参数；

$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}f& 0& c_u\\ 0& f& c_v\\ 0& 0& 1\end{array}\right)[\left(\begin{array}{cc}r& t\end{array}\right)\left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)-\left(\begin{array}{c}s\\ 0\\ 0\end{array}\right)]---\left(3\right)$

其中，(u,v)是图像坐标；c_v和c_u是相机透镜光心在图像上的投影所在的行和列；f是相机的焦距；(x,y,z)是前一帧特征点的三维坐标；r和t是一次迭代计算得到的旋转矩阵和平移向量；如果投影到左图s＝0，如果投影到右图s＝B；B是相机基线；

第5.2、令π^(l)(P_i；r,t)表示前一帧左图上的第i个特征点P_i根据旋转矩阵r和平移向量t重投影到当前帧的左图平面上，同理π^(r)(P_i；r,t)表示P_i重投影到右图平面上，I_i ^'(l)和I_i ^'(r)分别表示当前帧左图和右图上特征点的实际像素坐标，则通过最小化下式来选择最优运动参数，

$E=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|{I_i}^{\′\left(l\right)}-{\mathrm{\π}}^{\left(l\right)}(P_i;r,t)\left|\right|}^2+{\left|\right|{I_i}^{\′\left(r\right)}-{\mathrm{\π}}^{\left(r\right)}(P_i;r,t)\left|\right|}^2$

E为残差平方和，通过多次迭代求得使E最小的r和t作为最终的运动参数；具体步骤如下：

(1)在n个匹配特征点对中随机抽取w个点对。由于三个非共线的特征点可以唯一的确定运动参数，所以这里我们取w＝3；

(2)如果3点共线则重新选取，直到不共线时用四元数法计算出运动参数(r_k,t_k)；

(3)根据步骤(2)求得的运动参数计算特征点匹配集的重投影误差E_k；

(4)重复以上步骤(1)至步骤(3)m次，在m个重投影误差中寻找最小值，E_min＝min E_k k＝1,2,…m；

(5)此时E_min对应的(r,t)便是我们要求得的最优运动参数。

本发明的优点和有益效果：

1)通过引入基于立体视觉的移动机器人定位方法，解决由于环境等因素带来的传统定位方法失效的问题；2)本发明公开的特征圈匹配技术能够同时进行特征匹配和跟踪，且降低了误匹配率，很大程度上提高了定位的准确度；3)本发明采用的单位四元数法计算相邻帧特征点间的旋转平移矩阵，克服了传统的奇异值分解法计算过程中精度损失问题；4)本发明提出的最小化重投影误差法，把三维特征点重投影到二维图形空间下再进行迭代计算使得误差最小化，能够进一步使得运动参数更为准确和鲁棒。

附图说明

图1为本发明的流程图。

图2为本发明纠正畸变后的平行双目视觉系统坐标系示意图。

图3为图像序列中一幅图像提取出的SIFT特征点。

图4为圈匹配技术原理图。

图5为同一帧左图右图特征立体匹配示意图。

图6为室内环境下简单路径本发明方法计算得到的位置与真实位置的比较。

图7为室内环境下复杂路径本发明方法计算得到的位置与真实位置的比较。

图8为室外环境下采用KITTI数据集中较简单路径本发明方法计算得到的位置与真实位置的比较。

图9为室外环境下采用KITTI数据集中较复杂路径本发明方法计算得到的位置与真实位置的比较。

具体实施方式

下面结合附图对本发明作进一步的描述。

实施例1

如图1所示，为本发明基于立体视觉的移动机器人定位方法的操作流程图，该方法的操作步骤包括：

步骤01图像采集和预处理

首先在移动机器人上安装双目立体相机，用张正友相机标定法对相机进行标定，用Bought畸变纠正法对图像进行畸变纠正。

如图2所示，为本发明经过标定和纠正畸变后的平行双目相机坐标系统示意图。设O_l、O_r分别是左右相机的光心，两个相机光心之间的距离为基线B，单位为米；I_l和I_r分别为左右相机获得的图像，设图像分辨率为col×row；相机光心在图像上的投影点坐标为(c_u,c_v)；相机的焦距为f，单位为像素；点P是三维景物上一点，在左图像上的投影像素坐标为(u_l,v_l)，在右图像上的投影像素坐标为(u_r,v_r)。本实施例中B＝0.12，f＝0.76×col，c_u＝0.50×col，c_v＝0.52×row，图像分辨率为512×384。

步骤02尺度不变特征提取

图像的特征是指图像中明显不同于其他像素的点，特征点应具有尺度、旋转、亮度等不变性。图像中常见的特征有点特征、线特征和面特征，线特征是图像中的直线段或者弧线段，面特征指特征块，线特征和面特征容易随图像的变化而变化，而点特征受图像变化的影响较小，并且点特征容易提取和三维重建，因此视觉定位通常用的是图像的点特征。SIFT(Scale Invariant Feature Transform)特征点对于图像的平移、尺度、旋转都具有不变性，并且对亮度变化和放射变换或三维投影变换有部分不变性，另外重要的是提取的特征点较多，并且能精确到亚像素，这对准确计算旋转平移矩阵是十分有利的。

本实例结合OpenCV提取SIFT特征点，如图3所示，是图像序列中一幅图像提取出的SIFT特征点，特征点的坐标集为：

{57.7888 279.0071；93.9108 242.6903；152.8093 259.1888；248.8281 396.2472；264.5953383.0940；271.3508 407.3074；271.3508 407.3074；350.7544 173.9181；57.7196 324.5589；125.1699 338.1086；136.9977 315.4404；150.9456 230.9013；166.7958 290.5070；147.2054255.1984；165.6071 340.8181；123.3978 203.3458；78.8258 244.2944}。

步骤03特征匹配与跟踪

(1)圈匹配

在步骤02中获得图像序列各图像特征的基础上，本发明采用圈匹配技术同时完成左右图的特征匹配和前后帧的特征跟踪。如图4所示，首先得到四幅图像的特征点，四幅图像分别为当前帧的左图、右图和上一帧的左图、右图，然后按照这样的顺序进行匹配：当前帧左图->上一帧左图->上一帧右图->当前帧右图->当前帧左图，完成一圈匹配。圈匹配技术获得的特征点更可靠，误配点较少，使得运动估计更准确。

(2)计算特征点三维坐标

同一帧左图和右图特征匹配是为了计算特征点的三维坐标，如图5所示，是其中一帧图像左图右图特征匹配结果。假设左图上一个特征点位于图像上的u列v行，则其坐标为(u_l,v_l)，右图上所匹配的特征点为(u_r,v_r)，则左图上的特征点由图像坐标转换到三维坐标(x,y,z)有下列公式：

$\left\{\begin{array}{c}x=\frac{(u_l-c_u)\·B}{d}\\ y=\frac{(v_l-c_v)\·B}{d}\\ z=\frac{f\·B}{d}\end{array}\right.$

d是视差，即该特征点在右图对应列坐标之差d＝u_l-u_r。c_v和c_u是相机透镜光心在图像上的投影所在的行和列，f是相机的焦距，单位为像素，B为相机基线长度，单位为米。通过上式可以计算得到所有特征点的三维坐标。本实施例中B＝0.12，f＝0.76×col，c_u＝0.50×col，c_v＝0.52×row，图像分辨率为512×384。如左图中一个特征点(406.64，72.12)，其在右图中匹配的特征点为(390.39，72.25)，视差d＝406.64-390.39＝16.25，代入上式计算得出x＝1.11，y＝-0.95，z＝2.87。

图3中特征点对应的三维坐标集为：

{0.1970 -1.2541 3.4298；-0.1242 -0.9558 3.4985；0.0224 -0.4074 3.3273；1.2954 0.44683.6233；1.0678 0.5397 3.2973；1.3300 0.6238 3.4471；1.3300 0.6238 3.4471；-0.3416 0.62041.6148；0.5495 -1.1550 3.1573；0.6981 -0.6462 3.3449；0.4936 -0.5336 3.2754；-0.2219 -0.42993.3793；0.2887 -0.2879 3.3230；-0.0115 -0.4489 3.2818；0.7462 -0.3103 3.4601；-0.4640 -0.67363.4068；-0.1036 -1.0289 3.3000}。

步骤04运动估计

运动估计，关键是求解旋转矩阵和平移矩阵。机器人运动属于刚体运动，可分解为旋转运动和平移运动，运动参数包括3×3的旋转矩阵和3×1的平移向量。运动估计所要解决的问题，就是从已知同一组点集在坐标系变换前后各自的三维坐标中求出坐标系变换参数的过程。而相应的坐标系变换参数就是摄像机的运动参数也就是机器人的运动参数。

(1)数学模型的建立

假设在前一帧与当前帧间有n个特征匹配对，其对应的三维坐标为{P_i|i＝1,…n}，{P_i'|i＝1,…n}，其中P_i＝(x_i,y_i,z_i)，P为前一帧特征点的三维坐标，P'为当前帧特征点的三维坐标，则具有如下关系

P_i'＝RP_i+T     (1)

R和T分别代表旋转矩阵和平移矢量，表明了机器人在前一帧与当前帧间的相对位姿变化。R是3×3的正交旋转矩阵，自由度为3，传统方法用欧拉角描述旋转矩阵。这种方法存在奇异点，如欧拉角中有一个为90°时，另外两个无法确定，旋转角为0°时，旋转轴为任意方向，而且这种方法只能针对小角度旋转的情况，当旋转角度较大时，会产生较大的误差。为了克服上述缺陷，本发明采用单位四元数法描述旋转矩阵。单位四元数法是描述三维空间旋转较好的方法，该方法没有奇异点，并且适用较大角度旋转。

设单位四元数为q＝[q₀,q₁,q₂,q₃]^T，其中q₀ ²+q₁ ²+q₂ ²+q₃ ²＝1，旋转矩阵可表示为：

$R=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2(q_1{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]---\left(2\right)$

(2)运动参数求解

为了求得旋转矩阵R和平移向量T，最少需要三对前后帧中相对应的特征点。单位四元数法求解步骤如下：

(a)求取匹配的特征点集{P_i(x_i,y_i,z_i)|i＝1,…n}，{P_i'(x_i',y_i',z_i')|i＝1,…n}的质心坐标：

$\left\{\begin{array}{c}{\stackrel{\‾}{P}}^{\′}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P_i}^{\′}\\ \stackrel{\‾}{P}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_i\end{array}\right.;$

(b)两个特征点集的对应点均减去各自的质心坐标，得到新的点集 $\left\{{\tilde{P}}_i({\tilde{x}}_i,{\tilde{y}}_i,{\tilde{z}}_i)\right|i=1,...n\},\{{{\tilde{P}}_i}^{\′}({{\tilde{x}}_i}^{\′},{{\tilde{y}}_i}^{\′},{{\tilde{z}}_i}^{\′})|i=1,...n\}.$ 其中

$\left\{\begin{array}{c}{\tilde{P}}_i=P_i-{\stackrel{\‾}{P}}_i\\ {{\stackrel{\‾}{P}}_i}^{\′}={P_i}^{\′}-{{\stackrel{\‾}{P}}_i}^{\′}\end{array}\right.;$

(c)计算协方差矩阵其中

$\left\{\begin{array}{c}P=\left[\begin{array}{cccc}{\tilde{P}}_1& {\tilde{P}}_2& ...& {\tilde{P}}_n\end{array}\right]\\ P^{\′}=\left[\begin{array}{cccc}{{\tilde{P}}_1}^{\′}& {{\tilde{P}}_2}^{\′}& ...& {{\tilde{P}}_n}^{\′}\end{array}\right]\end{array}\right.;$

(d)构造4×4的矩阵

$K=\left[\begin{array}{cc}\mathrm{tr}\left(Q\right)& {\mathrm{\Δ}}^T\\ \mathrm{\Δ}& Q+Q^T-\mathrm{tr}\left(Q\right)E_3\end{array}\right];$

其中tr(Q)是矩阵Q的迹，E₃是3×3的单位矩阵，△＝[A₂₃ A₃₁ A₁₂]，其中A_ij＝(Q-Q^T)_ij；

(e)计算矩阵K的特征值和特征向量，其中最大特征值对应的特征向量即为单位四元数q＝[q₀,q₁,q₂,q₃]^T；

(f)把q代入(2)式计算旋转矩阵R；

(g)最后计算平移向量

本实例中当前帧图像特征点集为：

{57.7888 279.0071；93.9108 242.6903；152.8093 259.1888；248.8281 396.2472；264.5953383.0940；271.3508 407.3074；271.3508 407.3074；350.7544 173.9181；57.7196 324.5589；125.1699 338.1086；136.9977 315.4404；150.9456 230.9013；166.7958 290.5070；147.2054255.1984；165.6071 340.8181；123.3978 203.3458；78.8258 244.2944}；

对应的三维坐标为：

{0.1970 -1.2541 3.4298；-0.1242 -0.9558 3.4985；0.0224 -0.4074 3.3273；1.2954 0.44683.6233；1.0678 0.5397 3.2973；1.3300 0.6238 3.4471；1.3300 0.6238 3.4471；-0.3416 0.62041.6148；0.5495 -1.1550 3.1573；0.6981 -0.6462 3.3449；0.4936 -0.5336 3.2754；-0.2219 -0.42993.3793；0.2887 -0.2879 3.3230；-0.0115 -0.4489 3.2818；0.7462 -0.3103 3.4601；-0.4640-0.6736 3.4068；-0.1036 -1.0289 3.3000}；

前一帧特征点集为：

{15.3499 50.4045；14.1375 88.7208；14.7866 150.8147；13.5238 251.5305；15.0090268.0500；14.4355 275.4125；14.4355 275.4125；32.7908 368.8174；15.0235 50.5884；14.7601121.5642；14.7800 133.8779；14.7289 148.5287；14.8807 165.2859；15.4413 144.7826；14.2010164.4536；14.4153 119.5611；14.2173 72.6438}；

对应的三维坐标为：

{0.1618 -1.1750 3.0555；-0.1462 -0.9504 3.3176；0.0011 -0.4046 3.1719；1.2753 0.45173.4681；1.0431 0.5391 3.1249；1.2951 0.6217 3.2491；1.2951 0.6217 3.2491；-0.3472 0.61571.4303；0.5513 -1.1991 3.1219；0.6776 -0.6432 3.1776；0.4825 -0.5423 3.1734；-0.2418 -0.42483.1844；0.2671 -0.2853 3.1519；-0.0292 -0.4343 3.0374；0.7267 -0.3060 3.3027；-0.4897 -0.67533.2536；-0.1297 -1.0808 3.2989}；

从前一帧图像三维特征点中随机选出三个点：

P＝[1.2951 1.0431 -0.1462；0.6217 0.5391 -0.9504；3.2491 3.1249 3.3176]；

从当前帧图像三维特征点中随机选出三个点：

P’＝[1.3300 1.0678 -0.1242；0.6238 0.5397 -0.9558；3.4471 3.2973 3.4985]；

计算得出：

Q＝[0.9515 0.5069 2.4154；0.5035 0.5290 0.1808；2.4642 0.1986 11.0367]；

K＝[-10.6142 1.0104 4.8796 -0.0178；1.0104 -11.4592 0.3794 0.0488；4.8796 0.37949.5562 0.0034；-0.0178 0.0488 0.0034 12.5173]；

q＝[1.0000；-0.0006；0.0020；0.0004]；

R＝[1.0000 0.0008 -0.0040；-0.0008 1.0000 -0.0012；0.0040 0.0012 1.0000]；

T＝[0.0402 0.0035 0.1808]；

步骤05最小化重投影误差

(1)式是运动估计模型的理想表达式，而实际误差是不可避免的，对于实际情况可改写为：P_i'＝RP_i+T+e，其中e为误差。误差e是代表了由旋转平移矩阵计算得到的三维坐标和实际坐标之间的误差，使误差最小化，才能得到比较正确的运动估计。

传统最小化误差的方法是在欧拉空间中进行的，但欧拉空间中的误差是各向异性的，即三维空间噪声分布不均匀，不利于最小化误差。为了解决这个问题，本发明提出在二维图像空间下最小化误差，即最小化重投影误差法。

求得旋转平移矩阵后，根据(3)式把前一帧特征点的三维坐标变换到当前帧坐标系下并重投影成图像坐标，计算重投影成的图像坐标和当前帧特征点的实际图像坐标之间的误差，多次迭代求得使重投影误差最小的旋转矩阵和平移向量，即为所求运动参数。

$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}f& 0& c_u\\ 0& f& c_v\\ 0& 0& 1\end{array}\right)[\left(\begin{array}{cc}r& t\end{array}\right)\left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)-\left(\begin{array}{c}s\\ 0\\ 0\end{array}\right)]---\left(3\right)$

(u,v)是图像坐标；c_v和c_u是相机透镜光心在图像上的投影所在的行和列；f是相机的焦距；(x,y,z)是前一帧特征点的三维坐标；r和t是一次迭代计算得到的旋转矩阵和平移向量；如果投影到左图s＝0，如果投影到右图s＝B；B是相机基线。

令π^(l)(P_i；r,t)表示前一帧左图上的第i个特征点P_i根据旋转矩阵r和平移向量t重投影到当前帧的左图平面上，同理π^(r)(P_i；r,t)表示P_i重投影到右图平面上，I_i ^'(l)和I_i ^'(r)分别表示当前帧左图和右图上特征点的实际像素坐标，则通过最小化下式来选择最优运动参数。

$E=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|{I_i}^{\′\left(l\right)}-{\mathrm{\π}}^{\left(l\right)}(P_i;r,t)\left|\right|}^2+{\left|\right|{I_i}^{\′\left(r\right)}-{\mathrm{\π}}^{\left(r\right)}(P_i;r,t)\left|\right|}^2$

E为残差平方和，通过多次迭代求得使E最小的r和t作为最终的运动参数。具体步骤如下：

(1)在n个匹配特征点对中随机抽取w个点对。由于三个非共线的特征点可以唯一的确定运动参数，所以这里我们取w＝3；

(2)如果3点共线则重新选取，直到不共线时用四元数法计算出运动参数(r_k,t_k)；

(3)根据步骤(2)求得的运动参数计算特征点匹配集的重投影误差E_k；

(4)重复以上步骤(1)至步骤(3)m次，在m个重投影误差中寻找最小值，E_min＝min E_k k＝1,2,…m；

(5)此时E_min对应的(r,t)便是我们要求得的最优运动参数。

本实例把前一帧图像的三维特征点根据计算得到的旋转矩阵R和平移向量T变换到当前帧图像的坐标系下，得到：

{0.1887 -1.1752 3.2355；-0.1202 -0.9507 3.4966；0.0282 -0.4048 3.3522；1.3018 0.45003.6545；1.0711 0.5381 3.3105；1.3227 0.6203 3.4358；1.3227 0.6203 3.4358；-0.3123 0.61781.6104；0.5778 -1.1997 3.3034；0.7044 -0.6440 3.3603；0.5094 -0.5430 3.3554；-0.2148 -0.42483.3636；0.2943 -0.2857 3.3334；-0.0017 -0.4344 3.2175；0.7533 -0.3070 3.4860；-0.4632 -0.67523.4316；-0.1037 -1.0811 3.4779}；

然后重投影到当前帧图像的二维平面上，得到：

{58.7348 279.3462；94.4193 243.1323；153.4671 259.8463；248.7603 395.7358；264.1492382.9763；271.1916 406.9735；271.1916 406.9735；350.5265 180.8039；58.7602 324.9051；125.7713 338.4644；137.4267 315.8735；151.3006 231.6132；167.1607 291.0546；147.9037256.3607；166.2461 340.9921；123.7744 203.8224；79.1981 244.9080}；

然后和真实的当前帧特征点集比较，计算得到E＝56.8371；可以多次迭代计算，找到最小的E，其对应的R和T就是最优运动参数，本实例迭代次数设置为50次。

为了说明本发明方法的有效性，在室内环境下做了实验。把Bumblebee2双目相机安装在机器人上，在实验室内自主漫游，拍摄图像序列并记录拍摄图像时机器人所处真实位置，设机器人起点位置为(0 0 0)，拍摄一帧图像(一帧图像包括左图和右图)；机器人沿直线前进0.17m，记录位置为(0 0 0.17)，拍摄一帧图像；然后再沿直线前进0.17m，记录位置为(0 0 0.34)，拍摄一帧图像；如此重复几次，记录的六个位置序列为(0 0 0)，(00 0.17)，(0 0 0.34)，(0 0 0.51)，(0 0 0.68)，(0 0 0.85)。然后用本发明方法计算机器人每次运动的旋转矩阵和平移向量依次为：

[1.00 0.00 0.01；-0.00 1.00 -0.00；-0.01 0.00 1.00]；[-0.00；0.00；0.17]；

[1.00 0.00 -0.00；-0.00 1.00 0.00；0.00 -0.00 1.00]；[-0.00；-0.01；0.16]；

[1.00 -0.00 -0.01；0.00 1.00 -0.00；0.01 0.00 1.00]；[0.01；-0.01；0.16]；

[1.00 0.00 0.00；-0.00 1.00 -0.00；-0.00 0.00 1.00]；[-0.00；0.00；0.17]；

[1.00 0.00 0.01；-0.00 1.00 0.01；-0.01 -0.01 1.00]；[0.00；-0.02；0.16]；

根据旋转矩阵和平移向量推算运行轨迹为(0 0 0)，(-0.003705 0.002664 0.174764)，(-0.006390 -0.008412 0.336822)，(0.005799 -0.019481 0.497356)，(0.002476 -0.0155370.672164)，(0.003201 -0.031773 0.835188)，真实位置进行比较，如图6所示，“O”符号为真实记录位置，“*”符号为本发明计算出的位置，可以看出本发明计算所得位置是比较准确的每个位置与真实位置，计算得出误差均值为0.0255m。

图7所示为室内复杂运动实验结果，可以看出本发明计算得出的位置与真实位置基本吻合，计算得知误差均值为0.13m。由于室内的白墙较多，在图像上特征点较少，不利于运动参数的计算，因此有些位置误差较大，但整体运动轨迹和真实情况相一致。

为了进一步说明本发明方法的有效性，在公共的KITTI数据集上进行了实验。KITTI数据集是德国卡尔斯鲁厄理工学院(Karlsruhe Institute of Technology)提供的，用安装在车辆上的双目相机拍摄卡尔斯鲁厄市的街道场景获得图像序列。

室外环境下图像中纹理丰富，特征点较多，有利于运动参数的准确计算。如图8所示，当运动路径较简单时，本发明计算得到的位置和真实位置基本吻合，定位精度较高，计算得知误差均值为1.98m；如图9所示，当运动路径较为复杂时，本发明计算得出的位置与真实位置略有偏差，但基本符合运动轨迹，与真实情况基本一致，误差均值为3.65m。

综上所述，本发明的提出的基于双目立体视觉的移动机器人定位方法，具有较好的优越性和鲁棒性，且有较高的定位精度。

Claims (4)

1.一种基于立体视觉的移动机器人定位方法，其特征在于该方法具体包含以下步骤：

第1、图像采集和预处理

首先在移动机器人上安装双目立体相机，在场景中漫游并拍摄图像序列；

第2、尺度不变特征提取

结合OpenCV提取图像序列每一帧图像的SIFT特征点；

第3、特征匹配与跟踪

在第2步获得图像序列的尺度不变特征后，用圈匹配技术同时进行特征匹配和跟踪；首先得到四幅图像的特征点，四幅图像分别为当前帧的左图、右图和上一帧的左图、右图，然后按照这样的顺序进行匹配：当前帧左图->上一帧左图->上一帧右图->当前帧右图->当前帧左图，完成一圈匹配；

第4、运动估计

机器人的运动属于刚体运动，可分解为旋转运动和平移运动，根据第3步获得的前后帧匹配的特征点，用单位四元数法计算特征点间的旋转矩阵和平移向量：

在二维平面上，一个单位圆上任意一点可以表示绕一个轴旋转的角度；在三维空间中，一个单位球面上任意一点可以表示绕两个轴旋转的角度；因此可以假设存在一个四维空间中的一个单位球，球面上任一点可以表示绕三个轴旋转的角度，四维空间中的单位球定义如下：

q₀ ²+q₁ ²+q₂ ²+q₃ ²＝1

球面上任意一点的四元坐标即单位四元数q＝[q₀,q₁,q₂,q₃]^T，旋转矩阵可表示为：

$R=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2(q_1{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]---\left(1\right)$

单位四元数法是描述三维空间旋转较好的方法，该方法没有奇异点，并且适用较大角度旋转，本发明采用单位四元数法表示并计算旋转矩阵；

第5、最小化重投影误差

计算得到的运动参数和实际运动参数不可避免存在误差，需要对误差最小化才能得到准确的运动参数，运动参数即旋转矩阵和平移向量；三维欧拉空间中误差分布各向异性，噪声分布不均匀，因此提出在二维图形空间下最小化误差的方法；根据运动参数计算得到运动后的三维特征点，重投影到二维图像空间下，然后计算与第2步中提取到的真实特征点之间的误差，多次迭代使得误差距离最小，即可得到较为准确的运动参数；

二维图像上的像素点可以根据相机参数计算得到三维空间下的坐标，同样，三维空间下的点也可以根据相机参数计算得出投影到二维图像上的像素坐标，由三维空间点计算得到二维图像像素点的过程称为重投影；

设上一帧图像的某个特征点的三维坐标为P(x,y,z)，变化到当前帧坐标系的旋转平移矩阵分别为R和T，重投影到当前帧图像上的像素坐标为(u,v)，则重投影公式为：

$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}f& 0& c_u\\ 0& f& c_v\\ 0& 0& 1\end{array}\right)[\left(\begin{array}{cc}r& t\end{array}\right)\left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)-\left(\begin{array}{c}s\\ 0\\ 0\end{array}\right)]---\left(2\right)$

其中，(u,v)是图像坐标；c_v和c_u是相机透镜光心在图像上的投影所在的行和列；f是相机的焦距；(x,y,z)是前一帧特征点的三维坐标；r和t是一次迭代计算得到的旋转矩阵和平移向量；如果投影到左图s＝0，如果投影到右图s＝B；B是相机基线。

2.根据权利要求1所述的方法，其特征在于第3步特征匹配与跟踪的具体方法包括：

第3.1、圈匹配

在步骤02中获得图像序列各图像特征的基础上，本发明采用圈匹配技术同时完成左右图的特征匹配和前后帧的特征跟踪；

第3.2、计算特征点三维坐标

同一帧左图和右图特征匹配是为了计算特征点的三维坐标，假设左图上一个特征点位于图像上的u列v行，则该特征点的坐标为(u_l,v_l)，右图上所匹配的特征点为(u_r,v_r)，则左图上的特征点由图像坐标转换到三维坐标(x,y,z)有下列公式：

$\left\{\begin{array}{c}x=\frac{(u_l-c_u)\·B}{d}\\ y=\frac{(v_l-c_v)\·B}{d}\\ z=\frac{f\·B}{d}\end{array}\right.;$

其中，d是视差，即该特征点在右图对应列坐标之差d＝u_l-u_r；c_v和c_u是相机透镜光心在图像上的投影所在的行和列，f是相机的焦距，单位为像素，B为相机基线长度，单位为米，通过上式能够计算得到所有特征点的三维坐标。

3.根据权利要求1所述的方法，其特征在于该方法第4步采用单位四元数法计算特征点间的旋转矩阵和平移向量的方法包括：

第4.1、数学模型的建立

假设在前一帧与当前帧间有n个特征匹配对，对应的三维坐标分别为{P_i|i＝1,…n}，{P_i'|i＝1,…n}，其中P_i＝(x_i,y_i,z_i)，P_i'＝(x_i',y_i',z_i')，P为前一帧特征点的三维坐标，P'为当前帧特征点的三维坐标，则具有如下关系

P_i'＝RP_i+T  (3)

其中，R代表旋转矩阵，T代表平移矢量，表明了机器人在前一帧与当前帧间的相对位姿变化；R是3×3的正交旋转矩阵，自由度为3；

第4.2、运动参数求解

为了求得旋转矩阵R和平移向量T，最少需要三对前后帧中相对应的特征点，单位四元数法求解步骤如下：

(a)求取匹配的特征点集{P_i(x_i,y_i,z_i)|i＝1,…n}，{P_i'(x_i',y_i',z_i')|i＝1,…n}的质心坐标：

$\left\{\begin{array}{c}{\stackrel{\‾}{P}}^{\′}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P_i}^{\′}\\ \stackrel{\‾}{P}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_i\end{array}\right.;$

(b)两个特征点集的对应点均减去各自的质心坐标，得到新的点集 $\left\{{\tilde{P}}_i({\tilde{x}}_i,{\tilde{y}}_i,{\tilde{z}}_i)\right|i=1,...n\},$ $\left\{{{\tilde{P}}_i}^{\′}({{\tilde{x}}_i}^{\′},{{\tilde{y}}_i}^{\′},{{\tilde{z}}_i}^{\′})\right|i=1,...n\},$ 其中，

$\left\{\begin{array}{c}{\tilde{P}}_i=P_i-{\stackrel{\‾}{P}}_i\\ {{\tilde{P}}_i}^{\′}={P_i}^{\′}-{{\stackrel{\‾}{P}}_i}^{\′}\end{array}\right.;$

(c)计算协方差矩阵其中

$\left\{\begin{array}{c}P=\left[\begin{array}{cccc}{\tilde{P}}_1& {\tilde{P}}_2& ...& {\tilde{P}}_n\end{array}\right]\\ P^{\′}=\left[\begin{array}{cccc}{{\tilde{P}}_1}^{\′}& {{\tilde{P}}_2}^{\′}& ...& {{\tilde{P}}_n}^{\′}\end{array}\right]\end{array}\right.;$

(d)构造4×4的矩阵

$K=\left[\begin{array}{cc}\mathrm{tr}\left(Q\right)& {\mathrm{\Δ}}^T\\ \mathrm{\Δ}& Q+Q^T-\mathrm{tr}\left(Q\right)E_3\end{array}\right];$

其中，tr(Q)是矩阵Q的迹，E₃是3×3的单位矩阵，△＝[A₂₃ A₃₁ A₁₂]，A_ij＝(Q-Q^T)_ij；

(e)计算矩阵K的特征值和特征向量，其中最大特征值对应的特征向量即为单位四元数q＝[q₀,q₁,q₂,q₃]^T；

(f)把q代入(1)式计算旋转矩阵R；

(g)最后计算平移向量

4.根据权利要求1所述的方法，其特征在于该方法第5步所述把三维特征点重投影到二维图像空间下，多次迭代最小化重投影误差的方法包括：

第5.1、(3)式是运动估计模型的理想表达式，而实际误差是不可避免的，对于实际情况可改写为：P_i'＝RP_i+T+e，其中e为误差；误差e是代表了由旋转平移矩阵计算得到的三维坐标和实际坐标之间的误差，使误差最小化，才能得到比较正确的运动估计；

求得旋转平移矩阵后，根据(2)式把前一帧特征点的三维坐标变换到当前帧坐标系下并重投影成图像坐标，计算重投影成的图像坐标和当前帧特征点的实际图像坐标之间的误差，多次迭代求得使重投影误差最小的旋转矩阵和平移向量，即为所求运动参数；

第5.2、令π^(l)(P_i；r,t)表示前一帧左图上的第i个特征点P_i根据旋转矩阵r和平移向量t重投影到当前帧的左图平面上，同理π^(r)(P_i；r,t)表示P_i重投影到右图平面上，I_i ^'(l)和I_i ^'(r)分别表示当前帧左图和右图上特征点的实际像素坐标，则通过最小化下式来选择最优运动参数，

$E=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|{I_i}^{\′\left(l\right)}-{\mathrm{\π}}^{\left(l\right)}(P_i;r,t)\left|\right|}^2+{\left|\right|{I_i}^{\′\left(r\right)}-{\mathrm{\π}}^{\left(r\right)}(P_i;r,t)\left|\right|}^2$

E为残差平方和，通过多次迭代求得使E最小的r和t作为最终的运动参数；具体步骤如下：

(1)在n个匹配特征点对中随机抽取w个点对；由于三个非共线的特征点可以唯一的确定运动参数，所以这里我们取w＝3；

(2)如果3点共线则重新选取，直到不共线时用四元数法计算出运动参数(r_k,t_k)；

(3)根据步骤(2)求得的运动参数计算特征点匹配集的重投影误差E_k；

(4)重复以上步骤(1)至步骤(3)m次，在m个重投影误差中寻找最小值，E_min＝minE_k k＝1,2,…m；

(5)此时E_min对应的(r,t)便是我们要求得的最优运动参数。