CN106908040A - 一种基于surf算法的双目全景视觉机器人自主定位方法 - Google Patents

Abstract

本发明属于移动机器人视觉定位技术领域，具体涉及一种基于SURF算法的双目全景视觉机器人自主定位方法。本发明包括：(1)设置人工路标作为先验位置信息；(2)改进的MDGHM‑SURF特征匹配；(3)匹配点重心迭代算法消除误匹配；(4)垂直基线式双目全景系统三维逆光路成像测距；(5)全景三角定位；(6)预测检测窗口加快定位速度。本发明针对移动机器人室内自主定位问题，通过设置人工路标作为先验位置信息，采用改进的MDGHM‑SURF算法进行特征点快速检测，并通过特征匹配进行路标识别定位，由匹配点重心迭代算法减小匹配误差，提高路标中心的定位精度，由机器人运动状态预测路标检测区域，提高了运动中定位的快速性。

Description

一种基于SURF算法的双目全景视觉机器人自主定位方法

技术领域

本发明属于移动机器人视觉定位技术领域，具体涉及一种基于SURF算法的双目全景视觉机器人自主定位方法。

背景技术

移动机器人自主定位导航能力是衡量其智能化水平的关键因素，环境感知能力是移动机器人智能化的基础，一直是机器人领域一个的研究热点。室内定位方式比较成熟的是磁轨,但适用范围有限；RFID、蓝牙、WLAN等信号容易衰减,精度差；超声波容易受障碍物干扰、激光雷达成本太高；电机编码脉冲运动学定位存在累计误差，而全景视觉传感器具有探测范围广，获取信息量大、成像旋转不变性、快速、高精度等优点，恰好能够为自主移动机器人提供丰富的环境信息。

移动机器人定位路标有自然路标和人工路标两种，利用自然路标，例如采用双阈值FAST特征检测的SIFT描述算法,解决了在室内非结构化环境中的定位问题，但视觉路标库的采样规则难以满足。而应用路标定位则可通过设置人工路标，将任意环境结构化。针对移动机器人定位过程中视觉图像特征点提取与匹配数量较多，边缘特征点不稳定的问题，在Bay结合SIFT特征点提取过程,提出了一种加速的、具有特征鲁棒性的SURF算法。通过设置多个路标，利用改进SURF算法进行路标定位。根据垂直基线双目全景系统测量精度与基线距成正比、与测量距离平方成反比的特点，建立垂直基线的双目全景系统三维逆光路成像分析模型，通过双目全景视觉获取路标的相对距离和方位等深度信息，从而得到机器人本身的位置的移动机器人视觉定位方法，很好地解决了视觉机器人自主定位的关键问题。

发明内容

本发明目的在于提供一种基于SURF算法的双目全景视觉机器人自主定位方法。

本发明的目的是这样实现的：

种基于SURF算法的双目全景视觉机器人自主定位方法，包括如下步骤:

(1)设置人工路标作为先验位置信息：

在未知非结构化环境中，根据SURF特征点提取算法，采集环境全景图像库测试，设计弱干扰匹配路标，在机器人工作环境内遮挡度低的一侧等间隔、等高设置两个以上不同特征的人工路标；

(2)改进的MDGHM-SURF特征匹配：

采用改进MDGHM-SURF算法进行路标特征匹配定位，对全景图像掩膜进行等间隔采样,并进行像素坐标变换，定义图像I(i,j)的(p，q)阶改进后离散Gaussian-Hermit矩为：

其中利用改进离散Gaussian-Hermit矩描述局部特征点，以描述特征点为中心，沿特征点主方向的正方形区域划分为若干个子区域，各区域获取多个采样点，在X和Y方向上分别描述；

(3)匹配点重心迭代算法消除误匹配：

由步骤(2)得匹配点坐标为A_i(x_i,y_i),匹配度为m_i设计的路标的特征均布，则匹配路标点重心为通过计算A_i距离Z的欧式距离d_i，建立循环求取距离最大值d_imax,设定距离阈值d_H，如d_imax>d_H,则删除该误匹配点，重新迭代计算重心，直到d′_imax≤d_H，实现路标中心精确定位；

(4)垂直基线式双目全景系统三维逆光路成像测距：

由步骤(3)定位出上下全景中各路标位置后，计算图像中路标距全景中心的像素距离，根据全景成像旋转对称性，取一个光轴截面为研究平面，双曲面镜的上支顶点为坐标原点，水平、竖直方向分别为X、Y轴建立坐标系,双曲面镜曲线方程为(y-c)²/a²-x²/b²＝1其中a、b、c分别为双曲面镜实轴、虚轴和焦距长度，在全景图像中测得路标点距全景中心的距离为r₁，相机标定焦距为f，根据折反射成像原理有r/f＝m/n，求解出镜面曲线上反射点的坐标为P(m,n)

根据垂直双目全景系统结构，入射光线的斜率为k＝(n-2c)/m，设上下全景系统的入射光线与双曲面镜的交点分别为Q₁(m₁,n₁)和Q₂(m₂,n₂)，其中L为基线距离，上下全景相机的入射光线方程为

联立方程组求解与路标水平距离：

在实际应用中，当透视相机和反射镜面的焦点没有严格重合时，将不满足单视点约束；若仍按单视点测距算法求解，误差较大,必须对非单视点进行优化；由于入射反射镜面光线的延长线不经过双曲线焦点，设镜面直径为R，相机和双曲面镜的焦点存在偏移量t，则折反射成像原理t＝Rf/r-H；由此可得变化后镜面方程(y-c-t)²/a²-x²/b²＝1,得到双曲面反射点Q(m′,n′)的坐标；同理，可求解上下全景相机的入射光线方程y′₁＝k₁x+n₁-k₁m₁-L，距离由求解；

(5)全景三角定位：

由步骤(4)得到机器人与等高设置在同一墙壁A(x₁,0)，B(x₂,0)位置的2个环境路标距离为d₁,d₂，在全景图像中定位出两路标距中心的夹角β，所以机器人的坐标计算公式为

根据全景图像中路标角度和机器人绝对的位置坐标关系可以实现对于姿态的测量；

(6)预测检测窗口加快定位速度

根据步骤(5)中机器人初始化定位后，机器人的绝对位置坐标和各路标在全景图像中的位置已经确定，由于机器人最大运动速度有限，对应路标在全景图像中移动范围有限，利用机器人当前的运动状态，估测下一刻机器人全景图像中路标检测区域，实现移动中快速定位。

本发明的有益效果在于：

全景视觉传感器具有探测范围广，获取信息量大、成像旋转不变性、快速、高精度等优点，恰好能够为自主移动机器人提供丰富的环境信息。针对移动机器人室内自主定位问题，研究了一种利用双目全景视觉系统实现的机器人绝对定位方法。通过设置人工路标作为先验位置信息，采用改进的MDGHM-SURF算法进行特征点快速检测，并通过特征匹配进行路标识别定位，由匹配点重心迭代算法减小匹配误差，提高路标中心的定位精度，由机器人运动状态预测路标检测区域，提高了运动中定位的快速性；通过垂直基线式双目全景系统三维逆光路成像测距算法实现了对深度信息的测量，并采用三角定位算法计算出机器人的坐标及姿态。该方法能够有效避免环境背景变化干扰，具有较高的定位精度和实时性。

附图说明

图1是基于SURF算法的双目全景视觉机器人自主定位流程方框图；

图2是移动机器人双目全景视觉系统定位原理图；

图3是双目全景视觉系统成像原理图。

具体实施方式

下面结合附图对本发明做进一步描述：

基于SURF算法的人工路标定位方法，包括如下步骤

(1)设置人工路标作为先验位置信息

在未知非结构化环境中，根据SURF特征点提取算法，采集环境全景图像库测试，设计弱干扰匹配路标。在机器人工作环境内遮挡度低的一侧等间隔、等高设置两个以上不同特征的人工路标。

(2)改进的MDGHM-SURF特征匹配算法

采用改进MDGHM-SURF算法进行路标特征匹配定位，对全景图像掩膜进行等间隔采样,并进行像素坐标变换，定义图像I(i,j)的(p，q)阶改进后离散Gaussian-Hermit矩为：

其中利用改进离散Gaussian-Hermit矩描述局部特征点，以描述特征点为中心，沿特征点主方向的正方形区域划分为若干个子区域，各区域获取多个采样点，在X和Y方向上分别描述。为提高算法匹配的正确率，以最近邻距离与次近邻欧式距离的比值作为匹配特征向量之间相似度的准则，比值越小相似度越大。

(3)匹配点重心迭代算法消除误匹配

由步骤(2)得匹配点坐标为A_i(x_i,y_i),匹配度为m_i设计的路标的特征均布，则匹配路标点重心为通过计算A_i距离Z的欧式距离d_i，建立循环求取距离最大值d_imax,设定距离阈值d_H，假如d_imax>d_H,则删除该误匹配点，重新迭代计算重心，直到d′_imax≤d_H，实现路标中心精确定位。

(4)垂直基线式双目全景系统三维逆光路成像测距算法。

由步骤(3)定位出上下全景中各路标位置后，计算图像中路标距全景中心的像素距离，根据全景成像旋转对称性，取一个光轴截面为研究平面，双曲面镜的上支顶点为坐标原点，水平、竖直方向分别为X、Y轴建立坐标系,双曲面镜曲线方程为(y-c)²/a²-x²/b²＝1其中a、b、c分别为双曲面镜实轴、虚轴和焦距长度，在全景图像中测得路标点距全景中心的距离为r₁，相机标定焦距为f，根据折反射成像原理有r/f＝m/n，可求解出镜面曲线上反射点的坐标为P(m,n)

根据垂直双目全景系统结构，入射光线的斜率为k＝(n-2c)/m。假设上下全景系统的入射光线与双曲面镜的交点分别为Q₁(m₁,n₁)和Q₂(m₂,n₂)，其中L为基线距离，上下全景相机的入射光线方程为

联立方程组求解与路标水平距离：

在实际应用中，当透视相机和反射镜面的焦点没有严格重合时，将不满足单视点约束。若仍按单视点测距算法求解，误差较大,必须对非单视点进行优化。由于入射反射镜面光线的延长线不经过双曲线焦点，设镜面直径为R，相机和双曲面镜的焦点存在偏移量t，则折反射成像原理t＝Rf/r-H。由此可得变化后镜面方程(y-c-t)²/a²-x²/b²＝1,得到双曲面反射点Q(m′,n′)的坐标。同理，可求解上下全景相机的入射光线方程y′₁＝k₁x+n₁-k₁m₁-L，距离由求解。

(5)全景三角定位算法

由步骤(4)得到机器人与等高设置在同一墙壁A(x₁,0)，B(x₂,0)位置的2个环境路标距离为d₁,d₂，在全景图像中定位出两路标距中心的夹角β，所以机器人的坐标计算公式为

根据全景图像中路标角度和机器人绝对的位置坐标关系可以实现对于姿态的测量。

(6)预测检测窗口加快定位速度

根据步骤(5)中机器人初始化定位后，机器人的绝对位置坐标和各路标在全景图像中的位置已经确定，由于机器人最大运动速度有限，对应路标在全景图像中移动范围较小，为减小图像SURF特征检测区域，降低运算量，利用机器人当前的运动状态，估测下一刻机器人全景图像中路标检测区域，实现移动中快速定位。

在机器人自主定位导航方面，使用双目立体视觉系统获取空间景物的度量信息是最常用的技术手段，双目全景在机器人定位导航中的应用拥有巨大潜力，该技术满足了智能驾驶、安防监控、运动目标的跟踪、三维测量及重构、视频会议等用途，更广泛用于军事侦察、地形勘测、安全鉴定、空中导航、机器人视觉、虚拟现实等领域，已成为机器视觉研究的热点方向。针对复杂环境下机器人的定位问题，提出了基于SURF算法实现的双目全景机器人自主定位方法。

实现步骤如下：

步骤1设置人工路标作为先验位置信息。根据SURF特征点提取算法，采集未知非结构化环境全景图像库测试，设计弱干扰匹配路标。在机器人工作环境内遮挡度低的一侧设置多个不同特征的路标。

步骤2改进的MDGHM-SURF特征匹配算法。采用改进MDGHM-SURF算法进行路标特征匹配定位，对全景图像掩膜进行等间隔采样,像素坐标变换，利用改进离散Gaussian-Hermit矩描述局部特征点并匹配。

步骤3匹配点重心迭代算法消除误匹配。迭代计算匹配重心，比较匹配距离阈值，删除误匹配点，实现路标中心精确定位。

步骤4垂直基线式双目全景系统三维逆光路成像测距算法。由步骤(3)定位出上下全景中各路标位置，计算图像中路标距全景中心的像素距离，根据成像原理求解机器人与路标水平距离。

步骤5全景三角定位算法

由步骤4得到机器人与路标距离关系，在全景图像中定位出两路标距中心的夹角，解算机器人坐标，由全景图像中路标角度和机器人绝对的位置坐标关系可以实现对于姿态的测量。

步骤6预测检测窗口加快定位速度

根据步骤5中机器人初始化定位后，根据机器人的运动状态，估测机器人全景图像中路标检测区域，加快运动中定位速度。

实施例：

1.设置人工路标作为先验位置信息

由于移动机器人在执行任务过程中，常常工作在未知非结构化环境中，为将陌生环境转化为熟悉环境，使用路标作为先验位置信息，根据SURF特征点提取算法，采集环境图像库测试，设计弱干扰匹配路标。在机器人工作环境内遮挡度低的一侧等间隔、等高设置两个以上不同特征的人工路标。

2.改进的MDGHM-SURF特征匹配算法

使用改进的MDGHM-SURF算法进行路标特征匹配定位，对全景图像掩膜进行等间隔采样,并进行像素坐标变换，全景图像I(i,j)的(p，q)阶改进后离散Gaussian-Hermit矩定义为：

其中利用改进离散Gaussian-Hermit矩描述局部特征点，以描述特征点为中心，沿特征点主方向的正方形区域划分为若干个子区域，各区域获取多个采样点，在X和Y方向上分别描述。为提高算法匹配的正确率，以最近邻距离与次近邻欧式距离的比值作为匹配特征向量之间相似度的准则，比值越小相似度越大。

3.匹配点重心迭代算法消除误匹配

由步骤(2)在全景图像中匹配点坐标为A_i(x_i,y_i),匹配度为m_i设计的路标的特征均布，则匹配路标点重心为通过计算A_i距离Z的欧式距离d_i，建立循环求取距离最大值d_imax,设定距离阈值d_H，假如d_imax>d_H,则删除该误匹配点，重新迭代计算重心，直到d′_imax≤d_H，实现路标中心精确定位。

4.垂直基线式双目全景系统三维逆光路成像测距算法。

由步骤(3)定位出上下全景中各路标位置后，计算图像中路标距全景中心的像素距离，根据全景成像旋转对称性，取一个光轴截面为研究平面，双曲面镜的上支顶点为坐标原点，水平、竖直方向分别为X、Y轴建立坐标系(如图3所示)，双曲面镜曲线方程为(y-c)²/a²-x²/b²＝1其中a、b、c分别为双曲面镜实轴、虚轴和焦距长度，在全景图像中测得路标点距全景中心的距离为r₁，相机标定焦距为f，根据折反射成像原理有r/f＝m/n，可求解出镜面曲线上反射点的坐标为P(m,n)

根据垂直双目全景系统结构，入射光线的斜率为k＝(n-2c)/m。假设上下全景系统的入射光线与双曲面镜的交点分别为Q₁(m₁,n₁)和Q₂(m₂,n₂)，其中L为基线距离，上下全景相机的入射光线方程。

联立方程组求解与路标水平距离：

在实际应用中，透视相机和反射镜面的焦点没有严格重合，不满足单视点约束。若仍按单视点测距算法求解，误差较大,必须对非单视点进行优化。由于入射反射镜面光线的延长线不经过双曲线焦点，设镜面直径为R，相机和双曲面镜的焦点存在偏移量t，则折反射成像原理t＝Rf/r-H。由此可得变化后镜面方程(y-c-t)²/a²-x²/b²＝1,得到双曲面反射点Q(m′,n′)的坐标

上下全景相机的入射光线方程为y′₁＝k₁x+n₁-k₁m₁-L，距离通过公式求解。

5.全景三角定位算法

由步骤(4)得到机器人与等高设置在同一墙壁A(x₁,0)，B(x₂,0)位置的2个环境路标距离为d₁,d₂，在全景图像中定位出两路标距中心的夹角β(如图2所示)，所以机器人的坐标计算公式为

根据全景图像中路标角度和机器人绝对的位置坐标关系可以实现对于姿态的测量。

6.预测检测窗口加快定位速度

根据步骤(5)中机器人初始化定位后，机器人的绝对位置坐标和各路标在全景图像中的位置已经确定，由于机器人最大运动速度有限，对应路标在全景图像中移动范围较小，为减小图像SURF特征检测区域，降低运算量，可根据机器人的运动状态，估测机器人全景图像中路标检测区域。

具体实施例如下：

以路标A为研究对象，全景中心坐标为G(x₀,y₀),，假设t时刻路标A在全景图像中像素位置为A(ρ,θ,t)，以A为圆心，设置半径为r的圆形检测窗口，进行SURF特征检测与路标匹配定位，假设机器人以角速度ω(t)在做原地旋转运动，由于机器人距离路标的距离不变，则路标在全景图像中的运动轨迹为以全景中心G为圆心，中心距离为半径ρ的圆弧。所以t′时刻的检测区域旋转为以A′(ρ,θ+ω(t)Δt,t′)为中心的圆域。考虑机器人有平移速度时，路标在图像中像素变化的最大梯度方向为速度方向，设机器人以最大速度运行时，对应的全景图像中像素改变最快速度为V_max，所以t′时刻的检测区域旋转为以A′(ρ,θ+ω(t)Δt,t′)为中心，半径为V_maxΔt+r的圆域，则预测在此圆域进行路标检测，实现移动中快速定位。

Claims (1)

1.一种基于SURF算法的双目全景视觉机器人自主定位方法，其特征在于，包括如下步骤:

(1)设置人工路标作为先验位置信息：

在未知非结构化环境中，根据SURF特征点提取算法，采集环境全景图像库测试，设计弱干扰匹配路标，在机器人工作环境内遮挡度低的一侧等间隔、等高设置两个以上不同特征的人工路标；

(2)改进的MDGHM-SURF特征匹配：

采用改进MDGHM-SURF算法进行路标特征匹配定位，对全景图像掩膜进行等间隔采样,并进行像素坐标变换，定义图像I(i,j)的(p，q)阶改进后离散Gaussian-Hermit矩为：

$\stackrel{\‾}{{\mathrm{\η}}_{p,q}}=4\underset{u=0}{\overset{K_M-1}{\Σ}}\underset{v=0}{\overset{K_N-1}{\Σ}}L\left(I\right)\stackrel{\‾}{H_p}(x,\mathrm{\σ})\stackrel{\‾}{H_q}(y,\mathrm{\σ})/(M-1)(N-1)$

其中利用改进离散Gaussian-Hermit矩描述局部特征点，以描述特征点为中心，沿特征点主方向的正方形区域划分为若干个子区域，各区域获取多个采样点，在X和Y方向上分别描述；

(3)匹配点重心迭代算法消除误匹配：

由步骤(2)得匹配点坐标为A_i(x_i,y_i),匹配度为m_i设计的路标的特征均布，则匹配路标点重心为通过计算A_i距离Z的欧式距离d_i，建立循环求取距离最大值d_imax,设定距离阈值d_H，如d_imax>d_H,则删除该误匹配点，重新迭代计算重心，直到d′_imax≤d_H，实现路标中心精确定位；

(4)垂直基线式双目全景系统三维逆光路成像测距：

由步骤(3)定位出上下全景中各路标位置后，计算图像中路标距全景中心的像素距离，根据全景成像旋转对称性，取一个光轴截面为研究平面，双曲面镜的上支顶点为坐标原点，水平、竖直方向分别为X、Y轴建立坐标系,双曲面镜曲线方程为(y-c)²/a²-x²/b²＝1其中a、b、c分别为双曲面镜实轴、虚轴和焦距长度，在全景图像中测得路标点距全景中心的距离为r₁，相机标定焦距为f，根据折反射成像原理有r/f＝m/n，求解出镜面曲线上反射点的坐标为P(m,n)

$\left\{\begin{array}{c}m=\frac{b^2{\mathrm{fcr}}_1+{\mathrm{ar}}_1{b}^2\sqrt{f^2+{r_1}^2}}{b^2{f}^2-a^2{r_1}^2}\\ n=\frac{b^2{f}^{2}c+{\mathrm{ab}}^{2}f\sqrt{f^2+{r_1}^2}}{b^2{f}^2-a^2{r_1}^2}\end{array}\right.$

根据垂直双目全景系统结构，入射光线的斜率为k＝(n-2c)/m，设上下全景系统的入射光线与双曲面镜的交点分别为Q₁(m₁,n₁)和Q₂(m₂,n₂)，其中L为基线距离，上下全景相机的入射光线方程为

$\left\{\begin{array}{c}y=\frac{n_1-2c}{m_1}x+2c\\ y=\frac{n_2-2c}{m_2}x+2c-L\end{array}\right.$

联立方程组求解与路标水平距离：

在实际应用中，当透视相机和反射镜面的焦点没有严格重合时，将不满足单视点约束；若仍按单视点测距算法求解，误差较大,必须对非单视点进行优化；由于入射反射镜面光线的延长线不经过双曲线焦点，设镜面直径为R，相机和双曲面镜的焦点存在偏移量t，则折反射成像原理t＝Rf/r-H；由此可得变化后镜面方程(y-c-t)²/a²-x²/b²＝1,得到双曲面反射点Q(m′,n′)的坐标；同理，可求解上下全景相机的入射光线方程y′₁＝k₁x+n₁-k₁m₁-L，距离由求解；

(5)全景三角定位：

由步骤(4)得到机器人与等高设置在同一墙壁A(x₁,0)，B(x₂,0)位置的2个环境路标距离为d₁,d₂，在全景图像中定位出两路标距中心的夹角β，所以机器人的坐标计算公式为

$\left\{\begin{array}{c}x=x_1+r_1(d_{12}^2+d_1^2-d_2^2)/2d_1\left|x_1-x_2\right|\\ y=r_1{r}_{2}sin\mathrm{\β}/\left|x_1-x_2\right|\end{array}\right.$

根据全景图像中路标角度和机器人绝对的位置坐标关系可以实现对于姿态的测量；

(6)预测检测窗口加快定位速度：

根据步骤(5)中机器人初始化定位后，机器人的绝对位置坐标和各路标在全景图像中的位置已经确定，由于机器人最大运动速度有限，对应路标在全景图像中移动范围有限，利用机器人当前的运动状态，估测下一刻机器人全景图像中路标检测区域，实现移动中快速定位。