CN110274598B - 一种机器人单目视觉鲁棒定位估计方法 - Google Patents

Abstract

本发明公开了一种机器人单目视觉鲁棒定位估计方法，包括：在第一、二关键帧图像特征匹配点对中选择一个随机样本进行初始定位估计，建立两关键帧图像的极几何关系，以匹配点与极线之间的距离作为初始距离误差进行聚类剔除误匹配点，计算初始定位估计的偏置距离修正初始距离误差作为该匹配点权重依据，采用最小化误差平方和重新鲁棒估计机器人位姿，利用三角测量原理建立局部路标三维地图；在第三关键帧及其后续关键帧图像的特征点与局部路标匹配点对中选择一个随机样本估计初始定位，利用重投影初始距离误差进行聚类剔除误匹配点，采用优化方法重新鲁棒估计机器人位姿。本发明可以快速、可靠地去除误匹配点，实现机器人定位的鲁棒估计。

Description

一种机器人单目视觉鲁棒定位估计方法

技术领域

本发明涉及视觉定位技术领域，更具体的涉及一种机器人单目视觉鲁棒定位估计方法。

背景技术

单目视觉定位是机器人导航中的一种便携、性价比高的定位技术，主要利用视觉传感器感知场景，获取序列图像进行定位，其定位准确度依赖于特征点匹配的正确性。而由于噪声、相似纹理等原因，可能出现错误的特征点匹配，从而影响机器人定位估计。

现有的单目视觉定位鲁棒估计方法主要有RANSAC法、1-point RANSAC法。RANSAC法可以用在定位估计之前，剔除误匹配点，但多次随机抽样的策略，使得剔除的效率不高，尤其当一个定位样本所需的数据点较多的时候。1-point RANSAC定位法借助机器人的运动方程，改进了RANSAC的多次随机抽样策略，剔除错误匹配点，但没有考虑到预测的位姿对于观测存在偏置，使得距离阈值确定受到影响，从而影响正确匹配点的判断，降低定位估计的准确度。

发明内容

本发明实施例提供一种机器人单目视觉鲁棒定位估计方法，用以解决上述背景技术中存在的问题。

本发明实施例提供一种机器人单目视觉鲁棒定位估计方法，包括：

1)利用随机样本进行机器人初始位姿估计；

将第一帧图像作为第一关键帧图像，并提取第一关键帧图像中局部不变性ORB特征点；随机选取第二关键帧图像，并提取第二关键帧图像中局部不变性ORB特征点；采用汉明距离法，对第一关键帧图像中局部不变性ORB特征点m′_i和第二关键帧图像中局部不变性ORB特征点m″_i进行匹配；在匹配点对中选择一个随机样本估计机器人的初始位姿；

采用8对匹配点(m′_i,m″_i)，基于直接线性方法，求解式(1)最小化问题，估计两关键帧间的基本矩阵，解算初始位姿R₂、T₂，建立极几何关系；

其中，F为3×3的基本矩阵；

对于第三关键帧图像和第三关键帧图像以后的关键帧图像，利用局部路标三维地图中的三维路标和当前关键帧图像的匹配点(M_i,m″′_i)进行匹配；在匹配点对中选择一个随机样本估计机器人的初始位姿；

采用3对匹配点(M_i,m″′_i)，基于三点透视P3P方法和3D点到3D点的位姿估计，获取当前帧图像的机器人初始位姿R_k和T_k；其中，i＝1,…,3；

2)利用匹配点重新估计机器人位姿，进行鲁棒定位估计；

利用初始位姿估计建立第一、二关键帧图像的极几何关系，以匹配点与极线之间的距离作为初始距离误差进行聚类剔除误匹配点，计算初始位姿估计的偏置距离修正初始距离误差作为该匹配点的权重依据，采用最小化误差平方和重新鲁棒估计机器人位姿，并利用三角测量原理建立匹配点的局部路标三维地图；

对于第三关键帧图像和第三关键帧图像以后的关键帧图像，利用局部路标三维地图中的三维路标和当前关键帧图像的特征点进行匹配，在匹配点对中选择一个随机样本进行初始位姿估计；利用初始位姿估计计算三维路标的重投影初始距离误差进行聚类剔除误匹配点，计算初始位姿估计的偏置距离修正初始距离误差作为该匹配点权重依据，采用优化方法重新鲁棒估计机器人位姿。

进一步地，所述随机样本，是从匹配点对中随机选择位姿估计所需最少数量的匹配点对组成的一个随机样本。

进一度地，对第一关键帧图像和第二关键帧图像的匹配点，重新估计机器人位姿，进行鲁棒定位估计，具体包括：

1)对于第一关键帧图像和第二关键帧图像，将所有匹配点(m_i′,m_i″)到各自关键帧图像极线之间的距离之和作为距离误差e_i，如式(2)所示，并计算距离误差e_i的标准差σ；

2)采用基于密度的聚类算法对距离误差e_i进行密度聚类，剔除误差距离超过3σ的匹配数据，给出距离误差偏置e₀，获取正确匹配数据；

3)根据式(3)获得正确匹配数据与距离误差偏置比较后的修正距离误差e′，并将修正距离误差e′作为正确匹配数据权重w_i的依据，如式(4)所示；

e′_i＝e_i-e₀ (3)

4)采用修正距离误差e′建立正确匹配数据损失函数，如式(5)所示，并通过最小化加权的损失函数重新估计机器人位姿；

进一度地，对第三关键帧图像和第三关键帧图像以后的关键帧图像匹配点，重新估计机器人位姿，进行鲁棒定位估计；具体包括：

1)通过式(6)确定n个匹配点(M_i,m″′_i)的三维路标的重投影距离误差ε_i，并计算距离误差ε_i的标准差；其中，i＝1,…,n；

其中，s_i为M_i到相机的物距；K为相机的内参数矩阵；

2)采用基于密度的聚类算法对距离误差进行密度聚类，剔除误差距离超过的匹配数据，给出距离误差偏置，获取正确的配数据；

3)获取匹配数据与距离误差偏置比较后的修正距离误差，并将修正距离误差作为正确匹配数据权重的依据；

4)采用修正距离误差建立正确匹配数据损失函数，并通过最小化加权的损失函数重新估计机器人位姿。

本发明实施例提供一种机器人单目视觉鲁棒定位估计方法，与现有技术相比，其有益效果如下：

为了提高机器人视觉定位估计的鲁棒性，本发明给出了一种单次随机抽样误差聚类的机器人定位估计方法，利用随机选择的一个样本进行初始定位作为参考，省掉了多次随机选择的过程，去掉了由其引起的不确定性以及可能的次优性；对距离误差的聚类修正，校正了初始定位参考引起的偏置，降低了对初始定位参考的依赖性；以修正的距离误差对正确的匹配数据进行加权，提高了机器人位姿估计的准确度；机器人鲁棒定位估计方法不但有效剔除了误匹配点，而且具有自适应性，同时对正确匹配点的观测误差也具有区别性，降低了观测误差对定位估计的影响。本发明利用单次随机抽样，加速了图像特征点匹配，采用误差聚类的方法，快速地剔除了误匹配数据，并对正确的匹配数据进行了加权分配，有效地提高了机器人定位的精度，即本发明可以快速、可靠地去除误匹配点，实现机器人定位的鲁棒估计。

附图说明

图1为本发明实施例提供的一种机器人单目视觉鲁棒定位估计方法流程示意图；

图2为本发明实施例提供的初始两连续关键帧的机器人鲁棒定位估计流程示意图；

图3为本发明实施例提供的后续关键帧的机器人鲁棒定位估计流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参见图1～3，本发明实施例提供一种机器人单目视觉鲁棒定位估计方法，该方法包括：

(1)机器人通过单目摄像头获取视频流信息，并将视频流转化为序列帧图像。

(2)以第一幅图作为第一关键帧图像，提取图像中的ORB特征点。

(3)确定第二关键帧图像，并提取ORB特征点。

(4)采用汉明距离，对第一关键帧图像的ORB特征点m′_i和第二关键帧图像的ORB特征点m″_i进行匹配；

(5)利用第一、二关键帧的匹配点(m′_i,m″_i)，i＝1,…,n进行鲁棒定位估计，其中n为匹配点的数量，流程图如图2所示，具体过程如下：

1)采用8对匹配点(m′_i,m″_i)，i＝1,…,8，基于直接线性方法，求解式(1)最小化问题，估计两关键帧间的基本矩阵，建立极几何关系；

2)对于两幅图像，计算所有匹配点(m′_i,m″_i)到各自极线l和l′之间的距离之和作为距离误差e_i，并计算距离误差e_i的标准差σ；

3)对距离误差e_i采用DBSCAN算法进行密度聚类，剔除误差距离超过3σ的匹配数据，给出距离误差偏置e₀，获取正确的匹配数据；

4)按照式(3)给出匹配数据与距离误差偏置比较后的修正距离误差e′，作为该匹配数据权重w_i的依据，如式(4)所示；

e′_i＝e_i-e₀ (3)

5)采用修正距离误差e′建立正确匹配数据损失函数：

通过最小化加权的损失函数重新估计机器人位姿，并以当前的位移作为参考单位，利用三角测量原理建立局部路标三维地图。

(6)对于第三帧等后续的关键帧图像，利用局部地图中的三维路标和当前关键帧图像的匹配点(M_i,m″′_i)，i＝1,…,n进行鲁棒定位估计，其中n为匹配点的数量，流程图如图3所示，具体过程如下：

1)采用3对匹配点(M_i,m″′_i)，i＝1,…,3，基于P3P方法，和3D点到3D点的位姿估计，获取当前图像帧的机器人初始位姿R_k和T_k；

2)计算n个匹配点(M_i,m″′_i)，i＝1,…,n的三维路标的重投影距离误差ε_i，并计算距离误差ε_i的标准差σ；

3)对距离误差ε_i采用DBSCAN算法进行密度聚类，剔除误差距离超过3σ的匹配数据，给出距离误差偏置ε₀，获取正确的匹配数据；

4)参照式(3)给出匹配数据与距离误差偏置比较后的修正距离误差ε′，作为该匹配数据权重w_i的依据，参照式(4)；

5)采用修正距离误差ε′参照式(5)建立正确匹配数据损失函数，最小化加权的损失函数重新估计机器人位姿，并对位姿与匹配点进行BA优化，同时对新匹配的二维特征点，利用三角测量原理建立局部增量三维地图。

本发明实施例中，随机选择一个样本进行机器人初始定位估计作为参考，作为参考的初始定位估计可以是错误的，也可以是正确的。两关键帧图像匹配点相互计算匹配点与极线的距离之和作为距离误差e；地图中的三维路标和当前关键帧图像匹配点计算三维路标的重投影点与观测点位置距离作为距离误差ε，距离误差e和ε的聚类情况与初始定位估计无关，距离误差e和ε的偏置可以通过聚类进行估计。距离误差e和ε进行偏置修正后，作为该匹配数据权重的依据，以修正后的距离误差e′和ε′建立正确匹配数据的损失函数。利用正确的匹配数据，通过最小化加权的损失函数重新估计机器人位姿。

综上所述，为了提高机器人视觉定位估计的鲁棒性，本发明给出了一种单次随机抽样误差聚类的机器人定位估计方法，利用随机选择的一个样本进行初始定位作为参考，省掉了多次随机选择的过程，去掉了由其引起的不确定性以及可能的次优性；对距离误差的聚类修正，校正了初始定位参考引起的偏置，降低了对初始定位参考的依赖性；以修正的距离误差对正确的匹配数据进行加权，提高了机器人位姿估计的准确度；机器人鲁棒定位估计方法不但有效剔除了误匹配点，而且具有自适应性，同时对正确匹配点的观测误差也具有区别性，降低了观测误差对定位估计的影响。本发明利用单次随机抽样，加速了图像特征点匹配，采用误差聚类的方法，快速地剔除了误匹配数据，并对正确的匹配数据进行了加权分配，有效地提高了机器人定位的精度，即本发明可以快速、可靠地去除误匹配点，实现机器人定位的鲁棒估计。

以上公开的仅为本发明的几个具体实施例，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (3)

1.一种机器人单目视觉鲁棒定位估计方法，其特征在于，包括：

1)利用随机样本进行机器人初始位姿估计；

将第一帧图像作为第一关键帧图像，并提取第一关键帧图像中局部不变性ORB特征点；随机选取第二关键帧图像，并提取第二关键帧图像中局部不变性ORB特征点；采用汉明距离法，对第一关键帧图像中局部不变性ORB特征点m′_i和第二关键帧图像中局部不变性ORB特征点m″_i进行匹配；在匹配点对中选择一个随机样本估计机器人的初始位姿；

采用8对匹配点(m′_i,m″_i)，基于直接线性方法，求解式(1)最小化问题，估计两关键帧间的基本矩阵，解算初始位姿R₂、T₂，建立极几何关系；

其中，F为3×3的基本矩阵；

对于第三关键帧图像和第三关键帧图像以后的关键帧图像，利用局部路标三维地图中的三维路标和当前关键帧图像的匹配点(M_i,m″′_i)进行匹配；在匹配点对中选择一个随机样本估计机器人的初始位姿；

采用3对匹配点(M_i,m″′_i)，基于三点透视P3P方法和3D点到3D点的位姿估计，获取当前帧图像的机器人初始位姿R_k和T_k；其中，i＝1,…,3；

2)利用匹配点重新估计机器人位姿，进行鲁棒定位估计；

利用初始位姿估计建立第一、二关键帧图像的极几何关系，以匹配点与极线之间的距离作为初始距离误差进行聚类剔除误匹配点，计算初始位姿估计的偏置距离修正初始距离误差作为该匹配点的权重依据，采用最小化误差平方和重新鲁棒估计机器人位姿，并利用三角测量原理建立匹配点的局部路标三维地图；对第一关键帧图像和第二关键帧图像的匹配点，重新估计机器人位姿，进行鲁棒定位估计，具体包括：

1)对于第一关键帧图像和第二关键帧图像，将所有匹配点(m′_i,m″_i)到各自关键帧图像极线之间的距离之和作为距离误差e_i，如式(2)所示，并计算距离误差e_i的标准差σ；

2)采用基于密度的聚类算法对距离误差e_i进行密度聚类，剔除误差距离超过3σ的匹配数据，给出距离误差偏置e₀，获取正确匹配数据；

3)根据式(3)获得正确匹配数据与距离误差偏置比较后的修正距离误差e′_i，并将修正距离误差e′_i作为正确匹配数据权重w_i的依据，如式(4)所示；

e′_i＝e_i-e₀ (3)

4)采用修正距离误差e′_i建立正确匹配数据损失函数，如式(5)所示，并通过最小化加权的损失函数重新估计机器人位姿；

对于第三关键帧图像和第三关键帧图像以后的关键帧图像，利用局部路标三维地图中的三维路标和当前关键帧图像的特征点进行匹配，在匹配点对中选择一个随机样本进行初始位姿估计；利用初始位姿估计计算三维路标的重投影初始距离误差进行聚类剔除误匹配点，计算初始位姿估计的偏置距离修正初始距离误差作为该匹配点权重依据，采用优化方法重新鲁棒估计机器人位姿。

2.如权利要求1所述的机器人单目视觉鲁棒定位估计方法，其特征在于，所述随机样本，是从匹配点对中随机选择位姿估计所需最少数量的匹配点对组成的一个随机样本。

3.如权利要求1所述的机器人单目视觉鲁棒定位估计方法，其特征在于，对第三关键帧图像和第三关键帧图像以后的关键帧图像匹配点，重新估计机器人位姿，进行鲁棒定位估计；具体包括：

1)通过式(6)确定n个匹配点(M_i,m″′_i)的三维路标的重投影距离误差ε_i，并计算重投影距离误差ε_i的标准差；其中，i＝1,…,n；

其中，s_i为M_i到相机的物距；K为相机的内参数矩阵；

2)采用基于密度的聚类算法对距离误差进行密度聚类，剔除误差距离超过的匹配数据，给出距离误差偏置，获取正确的配数据；

3)获取匹配数据与距离误差偏置比较后的修正距离误差，并将修正距离误差作为正确匹配数据权重的依据；

4)采用修正距离误差建立正确匹配数据损失函数，并通过最小化加权的损失函数重新估计机器人位姿。

Images