CN104281148A - 基于双目立体视觉的移动机器人自主导航方法 - Google Patents

Abstract

本发明公开了一种基于双目立体视觉的移动机器人自主导航方法，该方法包括图像处理和运动估计两个过程：图像处理环节中设计了特征提取、立体匹配、特征跟踪等具体处理算法，分析了图像处理过程中的误差；运动估计环节通过鲁棒运动估计方法和非线性运动估计两步估计算法，具有针对性地消除了图像处理中的误匹配及噪声等误差影响，估计了机器人移动过程中的运动状态，提高了机器人在复杂环境下进行自主导航的性能。

Description

基于双目立体视觉的移动机器人自主导航方法

技术领域

本发明涉及图像处理、计算机视觉和自主导航技术领域，尤其是一种应用于表面复杂环境的自主导航技术实现方法。

背景技术

定位是机器人自主移动过程中的关键技术，其任务就是要确定机器人在地表参考坐标系中的姿态和位置信息，随着计算机图像处理技术的高速发展，利用视觉系统进行运动估计逐步成为了机器人进行导航的首选，机器人自运动估计就是通过分析环境图像信息的变化来估计其自身的运动参数，从某些意义上来说，基于视觉序列图像的自运动估计也可称之为视觉里程计运动估计技术，基于立体视觉的导航定位技术可靠性较高，逐步成为了机器人导航定位发展的趋势，根据机器人对于环境的了解情况，当机器人在视野范围内进行巡游探测时，用车上安装的立体视觉相机对其所处的周围环境进行成像，根据图像中的视觉特征，来实时对机器人的位置进行估计。

发明内容

本发明公开了一种基于双目立体视觉的机器人自主导航方法，该方法包括图像处理和运动估计两个过程：图像处理环节中设计了特征提取、立体匹配、特征跟踪等具体处理算法，分析了图像处理过程中的误差；运动估计分为鲁棒运动估计和非线性运动估计两个过程。

本发明的基本原理如下：

机器人立体视觉自运动估计是一套自主定位方案，整个部分分为两大部分，第一部分主要是相机标定、图像采集与预处理过程，需要首先对原始图像进行噪声过滤、灰度校正等图像预处理，最后输出立体图像对；第二部分主要由视觉图像处理和运动估计两个模块组成，由运动估计模块处理图像中特征点位置变化，来估计机器人自身的导航参数，具体如下：

1. 图像处理与分析过程：

1）图像噪声处理：本专利利用一种该进的中值滤波算法，像素灰度计算如下：

其中：分别为像素的邻域上、左、下、右上的像素，为像素的邻域对角方向上的像素，为取中值运算符.

2）特征点提取，本发明采用Harris算子模型中表示像元灰度值，并分别用表示像元在X和Y方向的梯度，Harris算子将角点响应函数表示为该像元的协方差矩阵行列式与方阵的迹：

          (1)

3）立体匹配，本发明采用相关匹配的方法进行匹配，该方法是根据两幅图像中特征点的相关程度来确定其匹配关系，相关值定义为：

      (2)

其中，为灰度均值，且有

                       (3)

为图像k以特征点为中心领域图像的标准偏差，可由以下公式计算

                          (4)

相关值从-1到1，两个相关窗口极度相似时为1，极度不相似时为-1.

4）环境特征深度提取，本发明基于双目视觉测量中视差原理来计算距离，双目相机间的基线距离为B，相机的焦距为f，空间中任意一点P(x,y,z)在左右图像上的像点分别为和，根据三角形的相似关系可推导出点P在摄像机坐标系中的距离信息：

                               (5)

像点和，其对应的像素坐标分别为，，由以上关系可得两个像点在像平面坐标系中的坐标分别为，

          (6)

由（5）得出空间点P在摄像机坐标中的坐标为

             (7)

    最后得出空间点P在世界坐标系中的坐标

                              (8)

机器人运动前空间点以及对应运动后的可建立匹配环境特征点对集合.

 2. 机器人运动估计过程如下：

定义、分别表示空间特征点在运动前后两个摄像机坐标系下的位置信息，则可建立运动估计模型：

                          (10)

其中为第次运动前后的坐标旋转矩阵，为第次运动前后的平移矩阵，为观测误差向量，

，，，           (11)

1）鲁棒运动参数估计：从空间特征点集中随机选取三组不共线的特征点，，，利用奇异值分解方法求出，，对求得的 ，回代求每个特征点对应的误差平方

                   (12)

重复采样m次，求出m个中值的最小值：

                         (13)

2）奇异值分解法确定运动参数：选择三个不共线特征点对，令：

                      (14)

其中、分别为机器人运动前后空间特征点的质心：

                          (15)

构造矩阵B:

                            (16)

奇异值分解,可以得到旋转矩阵：

                   (17)

3）非线性运动参数估计：旋转矩阵用四元数来表示，运动估计就是利用匹配点集通过对以下目标函数最小化来估计运动参数：

                            (18)

其中：.L-M算法通过迭代求解上式来计算的最小值：

                           (19)

其中：

                     

                     

由此可得到双目立体测距条件下的自运动参数。

本发明的技术效果：

    本发明提出了一种可在复杂环境下进行自主移动的导航算法，精确估计机器人在移动过程中的运动状态，从而提高了机器人在复杂环境下进行自主定位的导航性能。

附图说明：

图1为三角测量方法原理模型；

图2为L-M运动估计算法流程图；

图3为机器人的实际运动轨迹；

图4为本发明计算的双目视觉运动估计误差曲线；

图5为本发明计算出的姿态参数曲线；

图6为本专利计算出的姿态参数残差曲线；

图7为本发明与里程计相比较，二者之间的定位误差对比图。

具体实施过程：

参见图1-图7描述本发明具体实施过程

1、首先对基于视觉序列图像的导航参数进行估计：

1）图像噪声处理：本专利利用一种该进的中值滤波算法，像素灰度计算如下：

其中：分别为像素的邻域上、左、下、右上的像素，为像素的邻域对角方向上的像素，为取中值运算符.

2）特征点提取，本发明采用Harris算子模型中表示像元灰度值，并分别用表示像元在X和Y方向的梯度，Harris算子将角点响应函数表示为该像元的协方差矩阵行列式与方阵的迹：

          (1)

3）立体匹配，本发明采用相关匹配的方法进行匹配，该方法是根据两幅图像中特征点的相关程度来确定其匹配关系，相关值定义为：

      (2)

其中，为灰度均值，且有

                       (3)

为图像k以特征点为中心领域图像的标准偏差，可由以下公式计算

                          (4)

相关值从-1到1，两个相关窗口极度相似时为1，极度不相似时为-1.

4）环境特征深度提取，本发明基于双目视觉测量中视差原理来计算距离，如附图1所示，双目相机间的基线距离为B，相机的焦距为f，空间中任意一点P(x,y,z)在左右图像上的像点分别为和，根据三角形的相似关系可推导出点P在摄像机坐标系中的距离信息：

                               (5)

像点和，其对应的像素坐标分别为，，由以上关系可得两个像点在像平面坐标系中的坐标分别为，

          (6)

由（5）得出空间点P在摄像机坐标中的坐标为

             (7)

    最后得出空间点P在世界坐标系中的坐标

                              (8)

机器人运动前空间点以及对应运动后的可建立匹配环境特征点对集合.

 2. 机器人运动估计过程如下：

定义、分别表示空间特征点在运动前后两个摄像机坐标系下的位置信息，则可建立运动估计模型：

                          (10)

其中为第次运动前后的坐标旋转矩阵，为第次运动前后的平移矩阵，为观测误差向量，

，，，           (11)

1）鲁棒运动参数估计：从空间特征点集中随机选取三组不共线的特征点，，，利用奇异值分解方法求出，，对求得的 ，回代求每个特征点对应的误差平方

                   (12)

重复采样m次，求出m个中值的最小值：

                         (13)

2）奇异值分解法确定运动参数：选择三个不共线特征点对，令：

                      (14)

其中、分别为机器人运动前后空间特征点的质心：

                          (15)

构造矩阵B:

                            (16)

奇异值分解,可以得到旋转矩阵：

                   (17)

3）非线性运动参数估计：本发明引入Levenberg-Marquardt算法对运动参数进行迭代求解，根据姿态运动学，本发明选择旋转矩阵用四元数的形式来表示，,空间中一点经过旋转变为，如果把和均用四元数的形式表示，即： 

                          (18)

其中，，整理得

       (19)

其中

，

由以上公式可得：

即

       (20)

又由于得：

             (21)

平移可用表示，假如用表示运动参数估计量：

                    (22)

运动估计就是利用匹配点集通过对以下目标函数最小化来估计运动参数：

                             (23)

其中，.

L-M算法通过迭代求解来计算的最小值：

                            (24)

其中

                       (25)

                                  (26)

为迭代因子，在计算过程中可以实时调整大小以保证算法收敛的速度和精度。

由可知，为的矩阵：

         (237)

L-M算法的迭代初值由鲁棒方法的运算结果、得出，算法的流程如附图2所示,在进行迭代求解的过程中，每一个迭代步骤中的参数矢量利用归一化条件进行化简，即。

最后，本发明利用仿真算例对提出的方法进行验证，在世界坐标系中随机生成10个特征点，设定双目视觉系统的相机焦距为7mm，基线距离为25cm，在机器人运行过程中，在每一时刻都可以通过坐标变换来获得特征点在摄像机坐标系中的坐标，根据双目测距原理，用两个相邻时刻的空间特征点的坐标信息来对火星漫游器的定位信息进行估计，进而利用获得的导航信息来对机器人的运动情况计算,假定机器人的行进轨迹为

其中，A=0.3，B=0.5，φ=-π/3，附图3给出了机器人的实际运动轨迹，附图4为本发明计算的双目视觉运动估计误差曲线，附图5为本发明计算出的姿态参数曲线，附图6为本发明计算出的姿态参数残差曲线，附图7为本发明与里程计相比较，二者之间的定位误差对比图。

Claims (7)

1.本发明提出一种机器人立体视觉自运动估计是一套自主定位方案，可满足复杂地形情况下机器人自主巡游过程中导航定位的需要，不仅如此，此算法可大大实现机器人的智能化需求，整个部分分为两大部分，第一部分主要是相机标定、图像采集与预处理过程，需要首先对原始图像进行噪声过滤、灰度校正等图像预处理，最后输出立体图像对；第二部分主要由视觉图像处理和运动估计两个模块组成，由运动估计模块处理图像中特征点位置变化，来估计机器人自身的导航参数。

2.    本发明在特征点提取和立体匹配过程中权利要求：

1）特征点提取算法采用Harris算子模型，用                                               表示像元灰度值，并分别用表示像元在X和Y方向的梯度，Harris算子将角点响应函数表示为该像元的协方差矩阵行列式与方阵的迹，即

          (1)

3）立体匹配：采用相关匹配的方法进行匹配，该方法是根据两幅图像中特征点的相关程度来确定其匹配关系，相关值定义为：

      (2)

其中，为灰度均值，且有

                       (3)

为图像k以特征点为中心领域图像的标准偏差，可由以下公式计算

                          (4)

相关值从-1到1，两个相关窗口极度相似时为1，极度不相似时为-1。

3.本发明在环境特征深度信息提取中权利要求：

双目视觉测量是根据视差原理来计算距离，设双目相机间的基线距离为B，相机的焦距为f，空间中任意一点P(x,y,z)在左右图像上的像点分别为和，根据三角形的相似关系可推导出点P在摄像机坐标系中的距离信息：

                               (5)

像点和，其对应的像素坐标分别为，，由以上关系可得两个像点在像平面坐标系中的坐标分别为，

          (6)

由（5）得出空间点P在摄像机坐标中的坐标为

             (7)

    最后得出空间点P在世界坐标系中的坐标

                              (8)

机器人运动前空间点以及对应运动后的可建立匹配环境特征点对集合。

4.本发明在机器人运动估计过程权利要求如下：

定义、分别表示空间特征点在运动前后两个摄像机坐标系下的位置信息，则可建立运动估计模型：

                          (10)

其中为第次运动前后的坐标旋转矩阵，为第次运动前后的平移矩阵，为观测误差向量，

，，，           (11)

由上述观测误差向量可得到运动参数估计。

5.本发明权利要求一种鲁棒运动参数估计

从空间特征点集中随机选取三组不共线的特征点，，，利用奇异值分解方法求出，，对求得的 ，回代求每个特征点对应的误差平方

                   (12)

重复采样m次，求出m个中值的最小值：

                         (13)。

6.本发明要求一种奇异值分解法确定运动参数

选择三个不共线特征点对，令：

                      (14)

其中、分别为机器人运动前后空间特征点的质心：

                          (15)

构造矩阵B:

                            (16)

奇异值分解,可以得到旋转矩阵：

                   (17)。

7.本发明权利要求一种非线性运动参数估计

旋转矩阵用四元数来表示，运动估计就是利用匹配点集通过对以下目标函数最小化来估计运动参数：

                            (18)

其中：.L-M算法通过迭代求解上式来计算的最小值：

                           (19)

其中：

                     

                     

由此可得到双目立体测距条件下的自运动参数。