CN102435172A - 一种球形机器人视觉定位系统及视觉定位方法 - Google Patents

Abstract

本发明公开了一种球形机器人视觉定位系统及视觉定位方法。视觉定位系统包括双目视觉系统、一维陀螺仪、球形机器人本体、核心控制系统和无线通信模块组成。定位系统通过相机和陀螺仪配合进行定位，定位算法在核心控制系统中在线实现，定位的数据通过无线通信模块进行远程传输，用户可以远程实时监控机器人的位置。定位方法是通过双目相机实时采集左右图像序列，通过双目视觉里程计算法，计算出球形机器人姿态和位置的变化，通过陀螺仪对球形机器人平台不稳定造成的特征定位误差进行补偿，最后采用卡尔曼滤波方法进行状态更新。整个过程不断迭代，实现实时对机器人的姿态和位置进行计算。

Description

一种球形机器人视觉定位系统及视觉定位方法

技术领域

本发明涉及机器人定位导航领域，特别涉及一种球形机器人视觉定位系统及视觉定位方法

背景技术

球形机器人是指一类将运动执行机构、传感器、控制器、能源装置都内置于一个球形壳体内部、利用球形外壳作为滚动行走装置的系统的总称。与轮式，足式和履带式的机器人相比，具有结构新颖，运动灵活和效率高的特定，在军事、民用和工业领域有广泛的应用前景。

球形机器人的定位问题是实现球形机器人智能化的一个关键问题。目前球形机器人主要采用惯性导航系统、里程计、GPS等方式进行定位，但是惯性导航系统需要通过积分来得到位移和角度，存在时间漂移误差；里程计由于球面滑动，跳跃等因素，将产生累计误差，同时球形机器人平台不稳定，难以找到合适的搭载里程计的位置；GPS精度不高，室内无法使用。

目前视觉定位是研究热点，是未来机器人导航定位的一个方向，其具有信息量大、感应时间短的特点，可以通过获取周围环境位置的变化，得到机器人精确的机器人运动和姿态。球形机器人利用视觉定位还可以克服由于球面滑动，跳跃等采用内部传感器定位造成的累计误差。但是目前在球形机器人上采用视觉定位的研究成果以及在球形机器人上搭载完整的视觉定位系统基本没有，主要是球形机器上采用视觉定位存在不少问题：

1、视觉定位算法复杂、计算量大，球形机器人的处理系统难以进行实时处理。

2、球形机器人外面是封闭球壳，相机透过球壳采集图像质量很差甚至无法采集图像。

3、相机安装困难，通常的安装位置会使相机随球360度转动

4、球形机器人平台不稳定，相机坐标系相对与机器人坐标系会不断变化，导致定位不准。

发明内容

本发明的目的是为了克服现有球形机器人导航的技术不足，提出一种精度高、实时性好的球形机器人视觉定位系统及视觉定位方法。

本发明的采用如下技术方案：

一种球形机器人视觉定位系统，包括双目视觉系统、一维陀螺仪、球形机器人本体、核心控制系统、无线通信模块。

本发明中，所述的视觉定位系统的球形机器人本体设计成左右两个半球壳，左右半球独立驱动，同时左右半球之间开缝设计。所述的视觉定位系统的双目视觉系统包括两个型号相同的CCD相机，其主光轴平行放置，双目相机搭载到球形机器人与重摆相连的框架上，保证球静止时，相机光轴与地面平行，并指向正前方；所述的陀螺仪安装在双目相机的正上方。定位系统通过相机和陀螺仪配合进行定位，定位算法在核心控制系统中在线实现，定位的数据通过无线通信模块进行远程传输，用户可以远程实时监控机器人的位置。

一种球形机器人视觉定位方法，包括以下步骤：

步骤1：对相机和陀螺仪距相机中心的位置进行标定。

步骤2：开启双目相机，进行左右图像连续采集，开启陀螺仪，对相机姿态进行测量。

步骤3：如果图像是第一帧，对采集的左右图像提取Shi-Tomasi特征，并利用SIFT描述符进行描述，通过立体视觉坐标转化原理，将二维图像坐标转换为三维空间坐标，利用陀螺仪对特征点的位置进行补偿，初始化旋转矩阵R、平移矩阵T，R设为单位矩阵，T元素全置零，并设当前位置为原点，重新进入步骤3；否则直接进入步骤4。

步骤4：利用KLT算法对前后两帧左图和前后两帧右图图像分别进行特征的跟踪，同时对前后两帧图像中左右图像利用极线约束条件去除误跟踪特征对，将如果跟踪特征点总数小于阈值，重新对前后帧图像进行Shi-Tomasi提取并匹配，将匹配特征点二维图像坐标转换为三维空间坐标，得到前后两帧图像的三维特征点集，并利用陀螺仪对特征点的位置进行补偿；

步骤5：利用前后两帧的三维特征点集，采用解析法并结合RANSAC算法进行运动估计，得到R和T；

其中的详细步骤如下：

5-1.对前后帧三维特征点集进行筛选，选出匹配成功的特征点，组成新的特征点集。

5-2.对于前后帧新的特征点集，随机选择3对三维特征点，通过解析法，进行一次运动估计，得到一个R和T。

5-3.判断当前R和T，能使多少对特征点成为局内点，记录局内点对数。

5-4.针对5-2、5-3，进行N次迭代，得到局内特征对数最多时候的局内特征对。

5-5.将这些特征对，重新根据解析法，进行运动估计，得到更优的R和T。

步骤6：利用卡尔曼滤波算法对R，T进行更新，并得到当前机器人在世界坐标系中的位置，返回步骤3，进行下一次计算。

本发明的优点有：

1)本发明采用双目视觉结合陀螺仪的方式进行定位，降低了球形机器人不稳定带来的定位误差，提高了定位精度。

2)本发明中球形机器人本体采用左右半球之间开缝设计，外部传感器可以摆脱球壳的约束，更好的获取外部的环境信息；

3)本发明视觉系统搭载到相对稳定的主框上，是其保证一直指向球形机器人正前方位置，并只有小范围的偏转，保证采集图像的稳定性。

3)本发明中视觉定位算法，使用Shi-Tomasi特征和SIFT描述符相结合的特征提取方法，既满足了特征点提取的实时性，又提高特征点匹配的鲁棒性。

4)本发明中视觉定位算法采用KLT算法进行特征跟踪，不用每两帧图像进行特征重新提取和匹配，计算量大为减少。

5)本发明中视觉定位算法，使用解析法和RANSAC算法进行运动估计，保证了定位精度和实时性。

附图说明

图1是本发明提出的球形机器人视觉导航系统结构图。

图2是本发明中所定义的各个坐标系。

图3是本发明中机器人运动时，球形机器人坐标系和摄像机坐标系变化示意图。

图4是本发明提出的球形机器人视觉导航算法流程图。

图5是本发明提出的球形机器人视觉导航算法中运动估计算法结合RANSAC算法流程图

具体实施方式

下面结合附图对本发明进行进一步说明

如图1所示为本发明所述的球形机器人视觉导航系统。该系统包括：球形机器人本体1、双目视觉系统2、陀螺仪3、核心控制系统4、无线通信模块5。定位系统通过双目定位系统2和陀螺仪3配合进行定位，定位算法在核心控制系统4中在线实现，定位的数据通过无线通信模块5进行远程传输，用户可以远程实时监控球形机器人本体1的位置。

球形机器人本体1由左右两个半球壳构成球形外壳，中间开缝；在球壳内装有主框架，与重摆固连，重摆设置在竖直平面上，主框架两侧与球壳通过支撑架滑动连接。球形机器人本体1两个半球壳进行分别驱动控制。球形机器人本体1包括机械结构和底层电机驱动电路。

双目视觉系统2由两个同型号的CCD黑白摄像头构成，事先进行标定，已获得其内参数和畸变参数。双目视觉系统2搭载到球形机器人与重摆相连的框架上，保证球静止时，相机光轴与地面平行，并指向正前方。双目视觉系统2与核心控制系统4通过1394数据线相连，核心控制系统4不断接收双目视觉系统2的图像，并进行处理。

陀螺仪3为一维陀螺仪，可以测量指定方向的角度变化。其安装在双目视觉系统上方，保证其可以测量重摆摆起的角度。陀螺仪3与核心控制系统4通过串口相连，核心控制系统4不断接收陀螺仪3反馈的角度进行处理。

核心控制系统4上运行球形机器人视觉导航算法，核心控制系统4采用MINI-ATX主板、I3 3.1GHZ处理器、30G固态硬盘、4G内存，DC-ATX电源模块，极大的提高了系统的数据处理能力和存储能力，同时兼顾了系统稳定性、功率和小尺寸的要求。核心控制系统4运行稳定性、安全性好的Linux操作系统，通过配备的DC12V锂电池，可以持续工作两个小时。

无线通信模块5为宽带宽、大功率的无线网卡，实现与远程主机进行通信，将定位的结果不断发送给远程客户端，实现实时监控。同时无线通信模块也可以将采集的图像远程传输，，实现图像远程监控。无线通信模块5采用USB接口与核心控制系统4相连。

参照图2，对本发明球形机器人视觉定位算法中用到的几种不同的坐标系和坐标系间转换作简单介绍：

(1)世界坐标系{A：o_w-x_wy_wz_w}

用来表示球形机器人和特征点的绝对位置，在空间中垂直地面为Z轴，设单位为米。

(2)球形机器人本体坐标系{B：o_r-x_ry_rz_r}

坐标原点位于球形机器人球心，z轴指向机器人运动方向，y轴指向地面，x轴与y轴、z轴构成右手坐标系，单位为米。

(3)摄像机坐标系{C：o_c-x_cy_cz_c}

坐标原点位于立体视觉系统左相机光心，z轴与立体视觉系统左相机光轴重合，x轴指向右相机光心，y轴与x、z轴构成右手坐标系。单位为米。

(4)图像坐标系{D：o_u-x_uy_u}

在立体视觉系统左相机成像平面内，以左上角为坐标原点，水平方向为x轴，竖直方向为y轴，单位为像素。

由于摄像机位于球形机器人本体上不稳定平台上，如图3，当机器人重摆摆动，球形机器人本体坐标系和摄像机坐标系关系将发生变化，球形机器人本体坐标系和摄像机坐标系转化需要知道重摆摆动的角度θ。θ由陀螺仪测出，由几何关系，可得球形机器人本体和摄像机坐标系转化关系：

$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \mathrm{\θ}& -\sin \mathrm{\θ}& -L\sin (\mathrm{\α}+\mathrm{\θ})\\ 0& \sin \mathrm{\θ}& \cos \mathrm{\θ}& L\cos (\mathrm{\α}+\mathrm{\θ})\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]---\left(1\right)$

其中(x_r，y_r，z_r)为某特征点在机器人坐标系下的坐标，(x_c，y_c，z_c)为该点在摄像机坐标系下的坐标。

由小孔成像和立体视觉原理，可得摄像机坐标系和图像平面坐标系之间的转化关系：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{T}{d}& 0& -\frac{T}{d}\×u_0\\ 0& \frac{T}{d}& -\frac{T}{d}\×v_0\\ 0& 0& \frac{\mathrm{fT}}{d}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_u\\ y_u\\ 1\end{array}\right]---\left(2\right)$

其中(x_c，y_c，z_c)为某特征点在摄像机坐标系下的坐标，(x_u，y_u)为该点在摄像机坐标系下的坐标。T为双目摄像机基线，d为视差，f为相机焦距，(u₀，v₀)为相机中心点坐标。

由此，球形机器人本体坐标系和图像平面坐标系的转化关系可得：

$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \mathrm{\θ}& -\sin \mathrm{\θ}& -L\sin (\mathrm{\α}+\mathrm{\θ})\\ 0& \sin \mathrm{\θ}& \cos \mathrm{\θ}& L\cos (\mathrm{\α}+\mathrm{\θ})\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\frac{T}{d}& 0& -\frac{T}{d}\×u_0\\ 0& \frac{T}{d}& -\frac{T}{d}\×v_0\\ 0& 0& \frac{\mathrm{fT}}{d}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_u\\ y_u\\ 1\end{array}\right]---\left(3\right)$

如图4所示为本发明所述的球形机器人视觉导航算法流程图，详细步骤如下：

步骤1：对相机和陀螺仪距相机中心的位置进行标定。标定立体相机参数，包括：f、T、u₀、v₀以及畸变参数；标定立体相机和陀螺仪位置参数，包括左相机镜头光心与球心距离L，左相机镜头光心与球心距离与水平线夹角α。

步骤2：开启双目相机，进行左右图像连续采集，利用相机参数对图像进行校正，开启陀螺仪，对相机姿态进行测量。

步骤3：如果图像是第一帧，对采集的左右图像提取Shi-Tomasi特征，并利用SIFT描述符进行描述，同时将图像分块，对特征点进行筛选，让每块中的特征点数基本一致，保证其在整个图像平面分布均匀，得到当前帧左右匹配特征点集

并依次求出视差d。利于公式(2)，将匹配特征点集从图像坐标系转化到摄像机坐标系，得到摄像机坐标系下三维匹配特征点集

通过陀螺仪得到当前重摆摆动的角度θ，根据公式(1)，将摄像机坐标系下三维匹配特征点集转化到球形机器人本体坐标系中，得到球形机器人本体坐标系下三维匹配特征点集初始化旋转矩阵R、平移矩阵T，R设为单位矩阵，T元素全置零，并设当前位置为原点，重新进入步骤3；如果图像不是第一帧直接进入步骤4。

步骤4：利用KLT算法，通过前一帧左图特征点集和右图特征点集对当前帧左图像和右图像分别进行特征跟踪，对于当前帧左右图像，利用极线约束，将跟踪的点集中不满足约束的点去掉，得到匹配特征点集

如果匹配特征点集小于阈值N_lim，重新提取Shi-Tomasi特征，并利用SIFT描述符进行描述，并进行立体匹配，将新得到的匹配对添加到原匹配特征点集中，得到新的匹配特征点集

同样根据步骤3的坐标变化原理，并结合陀螺仪得到的角度，得到球形机器人本体坐标系下的三维特征点集

步骤5：利用前后两帧的三维特征点集，采用解析法并结合RANSAC算法进行运动估计，得到R和T，如图5，其中的详细步骤如下：

5-1.对前一时刻三维特征点集和步骤4得到的当前三维特征点集

通过跟踪ID，去除前一帧没有跟踪上的特征点和后一帧新提取的特征点，选出匹配上的前后帧三维特征点集：

5-2.对于匹配上的前一时刻特征点集

和当前时刻特征点集

随机选择3对特征点集，通过解析法，进行一次运动估计，得到一个R和T。

5-3..将当前时刻的特征点集中，除去抽出的3个点的其他所有点，通过：

$\left[\begin{array}{c}{X}_{\mathrm{rp}1}^i\\ Y_{\mathrm{rp}1}^i\\ Z_{\mathrm{rp}1}^i\end{array}\right]=R\left[\begin{array}{c}{X}_r^i\\ Y_r^i\\ Z_r^i\end{array}\right]+T---\left(4\right)$

得到

设定相关阈值ε，对于计算出的每一个点，与原始前一时刻特征点集相应点进行比较，如果：

$\sqrt{{(X_{\mathrm{rp}1}^i-X_{\mathrm{rp}}^i)}^2+{(Y_{\mathrm{rp}1}^i-Y_{\mathrm{rp}}^i)}^2+{(Z_{\mathrm{rp}1}^i-Z_{\mathrm{rp}}^i)}^2}<\mathrm{\&epsiv;}---\left(5\right)$

则将该特征对设为局内点，遍历当前时刻所有点，得到局内特征对集

记录局内特征对数。

5-4.针对5-2、5-3，进行N次迭代，得到局内特征对数最多时候的局内特征对。

5-5.将这些特征对，重新根据解析法，进行运动估计，得到更优的R和T。

其中的解析算法如下：

(1)、根据给定的得到的前一时刻三维特征点集

和当前三维特征点集

利用公式(6)，得到点集中心点坐标

和

$(X_{\mathrm{rp}}^c,Y_{\mathrm{rp}}^c,Z_{\mathrm{rp}}^c)=(\frac{1}{N}\underset{j=1}{\overset{N}{\mathrm{\&Sigma;}}}{X}_{\mathrm{rp}}^j,\frac{1}{N}\underset{j=1}{\overset{N}{\mathrm{\&Sigma;}}}{Y}_{\mathrm{rp}}^j,\frac{1}{N}\underset{j=1}{\overset{N}{\mathrm{\&Sigma;}}}{Z}_{\mathrm{rp}}^j)$

$(X_r^c,Y_r^c,Z_r^c)=(\frac{1}{N}\underset{j=1}{\overset{N}{\mathrm{\&Sigma;}}}{X}_r^j,\frac{1}{N}\underset{j=1}{\overset{N}{\mathrm{\&Sigma;}}}{Y}_r^j,\frac{1}{N}\underset{j=1}{\overset{N}{\mathrm{\&Sigma;}}}{Z}_r^j)---\left(6\right)$

(2)、前后特征点集中所有点均减去对应中心点得到两个新的三维点集

和

${\{\stackrel{\&OverBar;}{X_{\mathrm{rp}}^i},\stackrel{\&OverBar;}{Y_{\mathrm{rp}}^i},\stackrel{\&OverBar;}{Z_{\mathrm{rp}}^i}\}}_{i=1...N}={\{X_{\mathrm{rp}}^i-X_{\mathrm{rp}}^c,Y_{\mathrm{rp}}^i-Y_{\mathrm{rp}}^c,Z_{\mathrm{rp}}^i-Z_{\mathrm{rp}}^c\}}_{i=1...N}$

${\{\stackrel{\&OverBar;}{X_r^i},\stackrel{\&OverBar;}{Y_r^i},\stackrel{\&OverBar;}{Z_r^i}\}}_{i=1...N}={\{X_r^i-X_r^c,Y_r^i-Y_r^c,Z_r^i-Z_r^c\}}_{i=1...N}---\left(7\right)$

(3)、对于每个三维坐标，计算其9个相关值，并进行累加，得到：

$\begin{array}{ccc}{P}_{\mathrm{XX}}^i=\stackrel{\&OverBar;}{X_{\mathrm{rp}}^i{X}_r^i}& P_{\mathrm{XY}}^i=\stackrel{\&OverBar;}{X_{\mathrm{rp}}^i{Y}_r^i}...P_{\mathrm{ZY}}^i=\stackrel{\&OverBar;}{Z_{\mathrm{rp}}^i{Y}_r^i}& P_{\mathrm{ZZ}}^i=\stackrel{\&OverBar;}{Z_{\mathrm{rp}}^i{Z}_r^i}\end{array}---\left(8\right)$

$\begin{array}{ccc}{S}_{\mathrm{XX}}=\underset{i}{\mathrm{\&Sigma;}}{P}_{\mathrm{XX}}^i& S_{\mathrm{XY}}=\underset{i}{\mathrm{\&Sigma;}}{P}_{\mathrm{XY}}^i...S_{\mathrm{ZY}}=\underset{i}{\mathrm{\&Sigma;}}{P}_{\mathrm{ZY}}^i& S_{\mathrm{ZZ}}=\underset{i}{\mathrm{\&Sigma;}}{P}_{\mathrm{ZZ}}^i\end{array}---\left(9\right)$

(4)、通过步骤(3)得到的9个值，生成4*4的对称矩阵N

$N=\left[\begin{array}{cccc}{S}_{\mathrm{XX}}+S_{\mathrm{YY}}+S_{\mathrm{ZZ}}& S_{\mathrm{YZ}}-S_{\mathrm{ZY}}& S_{\mathrm{ZX}}-S_{\mathrm{XZ}}& S_{\mathrm{XY}}-S_{\mathrm{YX}}\\ S_{\mathrm{YZ}}-S_{\mathrm{ZY}}& S_{\mathrm{XX}}-S_{\mathrm{YY}}-S_{\mathrm{ZZ}}& S_{\mathrm{XY}}+S_{\mathrm{YX}}& S_{\mathrm{ZX}}+S_{\mathrm{XZ}}\\ S_{\mathrm{ZX}}-S_{\mathrm{XZ}}& S_{\mathrm{XY}}+S_{\mathrm{YX}}& -S_{\mathrm{XX}}+S_{\mathrm{YY}}-S_{\mathrm{ZZ}}& S_{\mathrm{YZ}}+S_{\mathrm{ZY}}\\ S_{\mathrm{XY}}-S_{\mathrm{YX}}& S_{\mathrm{ZX}}+S_{\mathrm{XZ}}& S_{\mathrm{YZ}}+S_{\mathrm{ZY}}& -S_{\mathrm{XX}}-S_{\mathrm{YY}}+S_{\mathrm{ZZ}}\end{array}\right]---\left(10\right)$

(5)、计算N对于最大特征值的特征向量(q₁，q₂，q₃，q₄)，该特征向量即为以4元数表示的旋转矩阵，由此通过4元数转化规则，可得旋转矩阵R

(6)、利用R以及步骤(1)得到前后点集中心点3维坐标，最后求得平移矩阵T

步骤6：利用卡尔曼滤波算法对R，T进行更新。

设状态向量x_k＝(v a)^T，其中v＝(Δx Δy Δz Δα Δβ Δγ)^T×1/Δt，(Δx Δy Δz)为平移向量，(Δα Δβ Δγ)为用欧拉角表示的旋转角度向量，二者可以与R、T矩阵相互转化。a＝(v-v_p)×1/Δt，状态方程如下：

${\left(\begin{array}{cc}v& a\end{array}\right)}^T=\left(\begin{array}{cc}I& \mathrm{\&Delta;t}\&times;I\\ 0& I\end{array}\right){\left(\begin{array}{cc}{v}_p& a_p\end{array}\right)}^T+{\mathrm{\&epsiv;}}_1---\left(11\right)$

其中(v a)^T为当前帧特征向量，(v_p a_p)^T为前一帧状态向量。I为单位向量，ε₁为噪声。观测向量z_k＝(v_z)，观测方程如下：

(v_z)＝(I 0)(v a)^T+ε₂            (12)

得到状态矩阵和观测矩阵： $F=\left(\begin{array}{cc}I& \mathrm{\&Delta;t}\&times;I\\ 0& I\end{array}\right),H=\left(\begin{array}{cc}I& 0\end{array}\right)---\left(13\right)$

由此，按照标准的卡尔曼滤波算法，可以得到当前时刻的姿态变化速度，由(Δx Δy Δz Δα Δβ Δγ)^T＝v×Δt，可得当前时刻的位置姿态变化，并转化为R和T。利用R和T以及前一时刻机器人位置(X′，Y′，Z′)，根据公式(14)，求出机器人当前时刻在世界坐标系中的位置(X，Y，Z)。

$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=R\left[\begin{array}{c}{X}^{\&prime;}\\ Y^{\&prime;}\\ Z^{\&prime;}\end{array}\right]+T---\left(14\right)$

卡尔曼滤波算法更新算法结束，返回步骤3，进行下一次计算。

Claims (4)

1.一种球形机器人视觉定位系统，其特征在于，包括双目视觉系统、一维陀螺仪、球形机器人本体、核心控制系统和无线通信模块等。定位系统通过相机和陀螺仪配合进行定位，定位算法在核心控制系统中在线实现，定位的数据通过无线通信模块进行远程传输，用户可以远程实时监控机器人的位置。

2.权利要求1所述的一种球形机器人视觉定位系统，其特征在于，球形机器人本体设计成左右两个半球壳，左右半球独立驱动，同时左右半球之间开缝设计。双目视觉定位系统搭载到球形机器人与重摆相连的框架上，通过左右球壳之间的缝隙，伸出球壳外部，保证机器人静止时，相机光轴与地面平行，并指向正前方。陀螺仪安装在双目相机的正上方。

3.一种球形机器人视觉定位方法，其特征在于，包含以下步骤：

步骤1：对相机和陀螺仪距相机中心的位置进行标定。

步骤2：开启双目相机，进行左右图像连续采集，开启陀螺仪，对相机姿态进行测量。

步骤3：如果图像是第一帧，对采集的左右图像提取Shi-Tomasi特征，并利用SIFT描述符进行描述，通过立体视觉三角测量法将特征点的二维图像坐标转换为三维空间坐标，利用陀螺仪对特征点的位置进行补偿，初始化旋转矩阵R、平移矩阵T，R设为单位矩阵，T元素全置零，并设当前位置为原点，重新进入步骤3；如果图像不是第一帧，直接进入步骤4。

步骤4：利用KLT算法对前后两帧左图和前后两帧右图图像分别进行特征的跟踪，同时对前后两帧图像中左右图像利用极线约束条件去除误跟踪对，得到特征匹配对，如果跟踪特征对总数小于阈值，重新对前后帧图像进行Shi-Tomasi提取并匹配，将新匹配的点加至特征匹配对，通过立体视觉三角测量法将匹配特征对二维图像坐标转换为三维空间坐标，得到前后两帧图像的三维特征点集，并利用陀螺仪对特征点的位置进行补偿；

步骤5：利用前后两帧的三维特征点集，采用解析法并结合RANSAC算法进行运动估计，得到R和T；

步骤6：利用卡尔曼滤波算法对R，T进行更新，并得到当前机器人在世界坐标系中的位置。返回步骤3，进行下一次计算。

4.权利要求4所述的一种球形机器人视觉定位方法，其特征在于，步骤5具体步骤如下：

(1).对前后帧三维特征点集进行筛选，选出匹配成功的点，组成新的特征点集。

(2).对于前后帧新的特征点集，随机选择3对三维特征点，通过解析法，进行一次运动估计，得到一个R和T。

(3).判断当前R和T，能使多少对特征点成为局内点，记录局内点对数。

(4).针对(2)、(3)，进行N次迭代，得到局内特征对数最多时候的局内特征对。

(5).将这些特征对，重新根据解析法，进行运动估计，得到更优的R和T。

Images