[发明专利]基于立体视觉的移动机器人定位方法

摘要

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

主权项

一种基于立体视觉的移动机器人定位方法，其特征在于该方法具体包含以下步骤：第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是相机基线。