一种定位方法及系统与流程

本发明涉及图像处理和计算机视觉技术领域，尤其涉及一种定位方法及系统。

背景技术：

视觉里程计仅仅通过获取运动物体或机器人相机获取的视觉信息来完成对运动物体或机器人的定位。传统里程计的缺点在于测量精度较差、数据不稳定，而视觉里程计可以有效克服传统里程计车轮打滑、远距离误差的问题，并可以解决机器人的精确自主定位问题。此技术已被广泛用于对地面机器人、水下机器人、微型无人机的定位中，它可以分为双目立体视觉里程计和单目视觉里程计，于2004年由nister等首次提出。

目前，单目视觉里程计的工作方法可以分为三类，即基于特征的方法、基于面的方法以及两种混合的方法。其中，基于特征的方法是目前主流的选择，本文也是选择基于特征的方法。对于单目来说，运动估计分为两类，一种是2d-2d方式，它仅需要相邻两帧之间匹配的特征点点集；另一种是3d-2d方式，它需要先通过前相邻两帧来得到3d点，并与第三帧的2d特征点进行配对。

目前，针对单目视觉里程计的研究大部分集中在2d-2d方式上，小部分使用了3d-2d方式。这是因为，相比于2d-2d来说，3d-2d方法不仅需要点的三角化，计算法量相对复杂，而且还要得到精确的3d特征集以及考虑相机外参数的噪音问题。但是，相比于2d-2d来说，3d-2d方法具有更高的精度。

因此，需要提出一种兼具快速和高精度的定位方法。

技术实现要素：

本发明通过提供一种定位方法及系统，实现了快速和高精度的技术效果。

本发明提供了一种定位方法，至少包括：

获取图像序列；

对所述图像序列中相邻两帧图像进行2d特征点的提取和匹配；

获取提取和匹配后的2d特征点的3d特征点；

通过帧间运动位移度量值和内点数量选择通过光流法或特征匹配法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；

基于所述当前时刻的旋转矩阵和所述当前时刻的平移向量得到定位位置。

进一步地，所述获取提取和匹配后的2d特征点的3d特征点，具体包括：

获得所述相邻两帧图像的相机矩阵和其中，r1为前一帧图像的旋转矩阵，为所述旋转矩阵r1的取值；t1为前一帧图像的平移向量，为所述平移向量t1的取值；r2为后一帧图像的旋转矩阵，为所述旋转矩阵r2的取值；t2为后一帧图像的平移向量，为所述平移向量t2的取值；

通过方程组计算得到xk、yk、zk，完成对所述3d特征点的提取；

其中，x1和x2为一对匹配的2d特征点，x1＝[u1,v1,1]^t，x2＝[u2,v2,1]^t；k1和k2为比例系数；x为所述3d特征点，x＝[xk,yk,zk,1]^t。

进一步地，所述获得所述相邻两帧图像的相机矩阵和具体包括：

通过八点算法对公式x′＝ex中的本质矩阵e采用奇异值分解分别得到所述r1、t1、r2和t2。

进一步地，所述通过帧间运动位移度量值和内点数量选择通过光流法或特征匹配法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量，具体包括：

如果只有所述光流法的帧间运动位移度量值在预设范围内，通过所述光流法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到所述当前时刻的旋转矩阵和所述当前时刻的平移向量；

如果只有所述特征匹配法的帧间运动位移度量值在所述预设范围内，通过所述特征匹配法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到所述当前时刻的旋转矩阵和所述当前时刻的平移向量；

如果所述光流法和所述特征匹配法的帧间运动位移度量值均在所述预设范围内，比较所述光流法的内点数量n1与所述特征匹配法的内点数量n2的大小；

若n1大于n2，通过所述光流法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到所述当前时刻的旋转矩阵和所述当前时刻的平移向量；

若n1小于n2，通过所述特征匹配法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到所述当前时刻的旋转矩阵和所述当前时刻的平移向量；

若n1等于n2，通过所述光流法或所述特征匹配法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到所述当前时刻的旋转矩阵和所述当前时刻的平移向量。

进一步地，所述基于所述当前时刻的旋转矩阵和所述当前时刻的平移向量得到定位位置，具体包括：

通过公式计算得到rpos和tpos；

其中，rcurr为所述当前时刻的旋转矩阵，tcurr为所述当前时刻的平移向量，rpos为当前时刻与上一时刻的旋转矩阵累积乘，tpos为当前时刻的累积位移；

基于所述rpos和所述tpos得到所述定位位置。

本发明提供的定位系统，至少包括：

图像序列获取模块，用于获取图像序列；

2d特征点提取和匹配模块，用于对所述图像序列中相邻两帧图像进行2d特征点的提取和匹配；

3d特征点获取模块，用于获取提取和匹配后的2d特征点的3d特征点；

运算模块，用于通过帧间运动位移度量值和内点数量选择通过光流法或特征匹配法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；

定位模块，用于基于所述当前时刻的旋转矩阵和所述当前时刻的平移向量得到定位位置。

进一步地，所述3d特征点获取模块，具体包括：

相机矩阵获取单元，用于获得所述相邻两帧图像的相机矩阵和其中，r1为前一帧图像的旋转矩阵，为所述旋转矩阵r1的取值；t1为前一帧图像的平移向量，为所述平移向量t1的取值；r2为后一帧图像的旋转矩阵，为所述旋转矩阵r2的取值；t2为后一帧图像的平移向量，为所述平移向量t2的取值；

特征点提取执行单元，用于通过方程组计算得到xk、yk、zk，完成对所述3d特征点的提取；其中，x1和x2为一对匹配的2d特征点，x1＝[u1,v1,1]^t，x2＝[u2,v2,1]^t；k1和k2为比例系数；x为所述3d特征点，x＝[xk,yk,zk,1]^t。

进一步地，所述相机矩阵获取单元，具体用于通过八点算法对公式x′＝ex中的本质矩阵e采用奇异值分解分别得到所述r1、t1、r2和t2。

进一步地，所述运算模块，具体包括：

第一判断单元，用于如果判断结果为只有所述光流法的帧间运动位移度量值在预设范围内，通过所述光流法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到所述当前时刻的旋转矩阵和所述当前时刻的平移向量；

第二判断单元，用于如果判断结果为只有所述特征匹配法的帧间运动位移度量值在所述预设范围内，通过所述特征匹配法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到所述当前时刻的旋转矩阵和所述当前时刻的平移向量；

第三判断单元，用于如果判断结果为所述光流法和所述特征匹配法的帧间运动位移度量值均在所述预设范围内，比较所述光流法的内点数量n1与所述特征匹配法的内点数量n2的大小；若n1大于n2，通过所述光流法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到所述当前时刻的旋转矩阵和所述当前时刻的平移向量；若n1小于n2，通过所述特征匹配法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到所述当前时刻的旋转矩阵和所述当前时刻的平移向量；若n1等于n2，通过所述光流法或所述特征匹配法将所述3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到所述当前时刻的旋转矩阵和所述当前时刻的平移向量。

进一步地，所述定位模块，具体包括：

计算单元，用于通过公式计算得到rpos和tpos；其中，rcurr为所述当前时刻的旋转矩阵，tcurr为所述当前时刻的平移向量，rpos为当前时刻与上一时刻的旋转矩阵累积乘，tpos为当前时刻的累积位移；

定位执行单元，用于基于所述rpos和所述tpos得到所述定位位置。

本发明中提供的一个或多个技术方案，至少具有如下技术效果或优点：

光流法的计算速度比特征法快，定位频率高，它对于帧间小范围视角旋移有很高的进度，但累计误差会随着移动总量的增加而不断增大，并且容易受到光照的干扰。特征匹配方法对大范围视野变化有更好的鲁棒性，精确性比光流法高，特征点匹配法进度较高，但是速度较慢，定位频率低。本发明通过分别求出这两种方法所使用的帧间运动位移度量值和内点数量来灵活交替选择相应算法的计算结果，不仅加快了定位速度，而且提高了定位精度，在复杂环境下有很强的实用性。

附图说明

图1为本发明实施例提供的定位方法的流程图；

图2为本发明实施例提供的定位方法中特征点提取与匹配的对比示意图；

图3为本发明实施例提供的定位方法中重投影点与特征点比较的示意图；

图4为本发明实施例提供的定位系统的模块图。

具体实施方式

本发明实施例通过提供一种定位方法及系统，实现了快速和高精度的技术效果。

本发明实施例中的技术方案为解决上述问题，总体思路如下：

光流法的计算速度比特征法快，定位频率高，它对于帧间小范围视角旋移有很高的进度，但累计误差会随着移动总量的增加而不断增大，并且容易受到光照的干扰。特征匹配方法对大范围视野变化有更好的鲁棒性，精确性比光流法高，特征点匹配法进度较高，但是速度较慢，定位频率低。本发明实施例通过分别求出这两种方法所使用的帧间运动位移度量值和内点数量来灵活交替选择相应算法的计算结果，不仅加快了定位速度，而且提高了定位精度，在复杂环境下有很强的实用性。

为了更好地理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。

参见图1，本发明实施例提供的定位方法，至少包括：

步骤s110：获取图像序列；

步骤s120：对图像序列中相邻两帧图像进行2d特征点的提取和匹配；

对本步骤进行说明：

对图像序列中相邻两帧图像使用surf描述算子进行特征点提取和匹配。为了提高匹配的准确性和运行效率，采用最近邻搜索算法在欧式空间搜索查找每个特征点的两个最近邻特征点。在这两个特征点中，如果最近距离与次近距离的比值小于预设比例阈值，则接受这对匹配点。此外，为了加快匹配速度，细化、剔除冗余的特征匹配对，并对特征点进行筛选，结合平面的极线几何约束，得到满足极线方程l＝fx的特征点x。其中，x＝[u,v,1]^t，f为基础矩阵。这种方法不仅提高了匹配的精确度，而且还提高了匹配的效率。图2为对数据集中某相邻两帧使用surf算法进行特征检测与匹配的对比示意图，发现特征点丰富且配对准确率高。

步骤s130：获取提取和匹配后的2d特征点的3d特征点；

对本步骤进行说明：

获得相邻两帧图像的相机矩阵和其中，r1为前一帧图像的旋转矩阵，为旋转矩阵r1的取值；t1为前一帧图像的平移向量，为平移向量t1的取值；r2为后一帧图像的旋转矩阵，为旋转矩阵r2的取值；t2为后一帧图像的平移向量，为平移向量t2的取值；旋转矩阵和平移向量都代表着两帧之间的相对运动；

通过方程组计算得到xk、yk、zk，完成对3d特征点的提取；

其中，x1和x2为一对匹配的2d特征点，x1＝[u1,v1,1]^t，x2＝[u2,v2,1]^t；k1和k2为比例系数；x为3d特征点，x＝[xk,yk,zk,1]^t。

具体地，将表达式x1＝[u1,v1,1]^t、表达式x2＝[u2,v2,1]^t、表达式和表达式带入方程组中，得到

和

联立方程可以得到关于xk、yk和zk最小二乘意义上的解，三维点x便可由相邻帧的2d点求解得到。

其中，获得相邻两帧图像的相机矩阵和具体包括：

通过八点算法对公式x′＝ex中的本质矩阵e采用奇异值分解分别得到r1、t1、r2和t2；其中，本质矩阵e的大小为3*3，该矩阵可以由帧间图像中的八个匹配点建立的方程来唯一确定。

这里需要说明的是，图3为某帧重投影2d点与该帧特征点的比较示意图，发现大部分重投影点与当前帧特征点重合或者接近，少部分偏差较大，因此，需要将这些偏差较大的点剔除，从而找到更精准的3d点以及对应的2d点。具体方法如下：首先将生成的3d点重投影到图像平面得到重投影2d点，计算每个重投影2d点与当前帧对应的特征点x的欧式距离。若欧式距离小于预设阈值，则将该3d点以及对应的2d点丢弃；若欧式距离大于或者等于预设阈值，则将该3d点以及对应的2d点保留。接着，在保留的3d点以及对应的2d点中，取所有对应的欧式距离的平均值，将所有大于平均值的3d点及对应2d丢弃，实现二次筛选。

步骤s140：通过帧间运动位移度量值和内点数量选择通过光流法或特征匹配法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；其中，光流法为光流lucas-kanda算法，特征匹配法为最近邻bruteforce算法。

对本步骤进行说明：

在每个时刻，如果只有光流法的帧间运动位移度量值在预设范围内，通过光流法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；

在每个时刻，如果只有特征匹配法的帧间运动位移度量值在预设范围内，通过特征匹配法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；

在每个时刻，如果光流法和特征匹配法的帧间运动位移度量值均在预设范围内，比较光流法的内点数量n1与特征匹配法的内点数量n2的大小；

若n1大于n2，通过光流法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；

若n1小于n2，通过特征匹配法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；

若n1等于n2，通过光流法或特征匹配法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量。

在每个时刻，如果光流法和特征匹配法的帧间运动位移度量值均不在预设范围内，则跳过当前帧。

这样就可以灵活交替地选择相应算法在各个时刻的当前时刻的旋转矩阵和当前时刻的平移向量的计算结果，从而保证了本发明实施例定位的高速和准确性。

步骤s150：基于当前时刻的旋转矩阵和当前时刻的平移向量得到定位位置。

对本步骤进行说明：

通过公式计算得到rpos和tpos；

其中，rcurr为当前时刻的旋转矩阵，tcurr为当前时刻的平移向量，rpos为当前时刻与上一时刻的旋转矩阵累积乘，tpos为当前时刻的累积位移。

基于rpos和tpos得到定位位置。

具体地，既然已知相机在各个时刻的姿态rcurr和tcurr，就可以求解出每个时刻相机的位置，并累积计算相机的位移。假设以初始位置为坐标原点(0，0)，仅仅考虑相机在xoz平面下的运动情况，不考虑垂直方向y上的位移，便可得到相机的平面运动轨迹。具体的路径累积如式所示：

参见图4，本发明实施例提供的定位系统，至少包括：

图像序列获取模块100，用于获取图像序列；

2d特征点提取和匹配模块200，用于对图像序列中相邻两帧图像进行2d特征点的提取和匹配；

3d特征点获取模块300，用于获取提取和匹配后的2d特征点的3d特征点；

在本实施例中，3d特征点获取模块300，具体包括：

相机矩阵获取单元，用于获得相邻两帧图像的相机矩阵和其中，r1为前一帧图像的旋转矩阵，为旋转矩阵r1的取值；t1为前一帧图像的平移向量，为平移向量t1的取值；r2为后一帧图像的旋转矩阵，为旋转矩阵r2的取值；t2为后一帧图像的平移向量，为平移向量t2的取值；

特征点提取执行单元，用于通过方程组计算得到xk、yk、zk，完成对3d特征点的提取；其中，x1和x2为一对匹配的2d特征点，x1＝[u1,v1,1]^t，x2＝[u2,v2,1]^t；k1和k2为比例系数；x为3d特征点，x＝[xk,yk,zk,1]^t。

其中，相机矩阵获取单元，具体用于通过八点算法对公式x′＝ex中的本质矩阵e采用奇异值分解分别得到r1、t1、r2和t2。其中，本质矩阵e的大小为3*3，该矩阵可以由帧间图像中的八个匹配点建立的方程来唯一确定。

运算模块400，用于通过帧间运动位移度量值和内点数量选择通过光流法或特征匹配法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；其中，光流法为光流lucas-kanda算法，特征匹配法为最近邻bruteforce算法。

在本实施例中，运算模块400，具体包括：

第一判断单元，用于如果在每个时刻的判断结果为只有光流法的帧间运动位移度量值在预设范围内，通过光流法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；

第二判断单元，用于如果在每个时刻的判断结果为只有特征匹配法的帧间运动位移度量值在预设范围内，通过特征匹配法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；

第三判断单元，用于如果在每个时刻的判断结果为光流法和特征匹配法的帧间运动位移度量值均在预设范围内，比较光流法的内点数量n1与特征匹配法的内点数量n2的大小；若n1大于n2，通过光流法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；若n1小于n2，通过特征匹配法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量；若n1等于n2，通过光流法或特征匹配法将3d特征点对应的2d特征点与当前帧的特征点进行匹配，得到当前时刻的旋转矩阵和当前时刻的平移向量。

定位模块500，用于基于当前时刻的旋转矩阵和当前时刻的平移向量得到定位位置。

在本实施例中，定位模块500，具体包括：

计算单元，用于通过公式计算得到rpos和tpos；其中，rcurr为当前时刻的旋转矩阵，tcurr为当前时刻的平移向量，rpos为当前时刻与上一时刻的旋转矩阵累积乘，tpos为当前时刻的累积位移；

定位执行单元，用于基于rpos和tpos得到定位位置。

【技术效果】

1、光流法的计算速度比特征法快，定位频率高，它对于帧间小范围视角旋移有很高的进度，但累计误差会随着移动总量的增加而不断增大，并且容易受到光照的干扰。特征匹配方法对大范围视野变化有更好的鲁棒性，精确性比光流法高，特征点匹配法进度较高，但是速度较慢，定位频率低。本发明实施例通过分别求出这两种方法所使用的帧间运动位移度量值和内点数量来灵活交替选择相应算法的计算结果，不仅加快了定位速度，而且提高了定位精度，在复杂环境下有很强的实用性。

2、通过surf描述算子对图像序列中相邻两帧图像进行特征点提取和匹配，提高了本发明实施例的抗干扰能力，进一步地提高了本发明实施例的定位精度。

本发明实施例考虑到室外场景的复杂性，多变性提出了结合光流和特征点匹配的方法，能够很好地完成机器人的自身定位。此外，本发明实施例还可以应用于各种移动机器人系统，如太空机器人、地面机器人、空中机器人和水下机器人的定位中，而且速度快、精度高，具有很强的适用性。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。