CN115393432A - 消除静态特征误匹配的机器人定位方法、存储介质及设备 - Google Patents

Abstract

本发明公开了消除静态特征误匹配的机器人定位方法、存储介质及设备，机器人定位方法以图像帧为输入，识别分割得到静态平面；静态平面区域中提取点面特征并进行点面特征匹配，采用双重特征约束的误匹配剔除策略；面特征的匹配首先构建出局部坐标系下相邻2帧中平面的匹配关系，利用基于平面几何约束的面特征误匹配算法去除匹配候选中的错误匹配，得到正确的面特征匹配关系以及点特征所属平面信息；根据点特征所属平面信息剔除非对应平面内的点特征匹配，再根据点特征位置剔除对应平面内的点特征误匹配。本发明保证特征匹配的正确率，提高机器人定位精度，从而构建更准确的轨迹图，并在动态场景中依然具有较高鲁棒性。

Description

消除静态特征误匹配的机器人定位方法、存储介质及设备

技术领域

本发明属于同步定位与地图创建(Simultaneous Location And Mapping，SLAM)技术领域，具体涉及消除静态特征误匹配的机器人定位方法、存储介质及设备。

背景技术

移动机器人同步定位与地图构建(Simultaneous Localization And Mapping，SLAM)主要研究移动机器人如何在复杂多变的环境中准确地估计自身位姿并构建当前场景地图。然而传统视觉惯性SLAM方法都假设相机处于静态场景，能够在大规模环境中实现了高精度的定位与构图，但实际场景中运动物体的存在不可避免，对SLAM系统位姿估计精度与地图构建准确性产生影响。剔除动态物体后，视觉前端提取出足够数量的静态特征仍需消除错误匹配影响，在相应的点、面特征匹配的过程中，图像纹理的相似、噪声的影响及阈值的选择不当等可能导致误匹配现象发生。平面特征的匹配错误会导致非对应平面间产生误匹配的点，而误匹配点的存在将会在一定程度上干扰识别结果，降低识别正确率，且误匹配点会在不同场景中有不同程度的影响，必须尽可能将其消除。综上所述传统视觉惯性SLAM方法在动态场景下存在难以准确剔除动态物体以及特征匹配精度低的问题，对SLAM系统定位精度的影响较大，系统位姿估计与地图构建的准确性也受到影响。

发明内容

本发明的目的是提供消除静态特征误匹配的机器人定位方法，用于解决现有技术中由于动态场景下难以在剔除动态物体后，使得移动机器人的位姿估计精度较低，且无法兼顾可靠性和鲁棒性的技术问题。

所述的消除静态特征误匹配的机器人定位方法，以图像帧为输入，识别输出场景中所有静态平面的分割图和空间中每个平面的三维参数，从而在场景中建立分段静态平面；静态平面区域中提取点面特征并进行点面特征匹配，采用双重特征约束的误匹配剔除策略；面特征的匹配首先构建出局部坐标系下相邻2帧中平面的匹配关系，使用基于平面物理参数的匹配方法来直接匹配不同坐标系下的平面，寻找出匹配候选，随后利用基于平面几何约束的面特征误匹配算法去除匹配候选中的错误匹配，得到正确的面特征匹配关系以及点特征所属平面信息；根据点特征所属平面信息剔除非对应平面内的点特征匹配，再根据点特征位置剔除对应平面内的点特征误匹配。

优选的，所述机器人定位方法具体包括：

步骤S1、根据机器人所自带的相机采集环境信息，分割静态平面区域，该区域通过实例分割网络生成；

步骤S2，提取静态平面区域的点面特征进行面特征匹配，剔除面特征误匹配；

步骤S3，记录正确面特征匹配对中所属点特征信息，剔除非对应平面的点特征匹配；

步骤S4，将对应平面的点特征匹配进行平面内误匹配剔除以实现二次筛选，剔除剩余点特征错误匹配。

优选的，所述步骤S2中，平面特征的匹配先分别计算两平面特征的向量夹角之间的欧式距离和两平面特征的重心之间的欧式距离，以此表示相似度，对不同相似度进行打分求和得到匹配分数，设定分数阈值得到面特征匹配候选；相邻图像帧中任意两相同匹配平面间的法向量应当近似相等，在最终匹配结果中的任意两组匹配平面必须满足此条件，即满足夹角相似度约束；将是否满足该约束转化为有向无环图问题解决，从而剔除不满足夹角相似度约束的面特征误匹配。

优选的，所述无环图问题即假设在m组平面特征匹配候选中，包含n(n≤m)组正确的匹配特征，则：

(1)有向无环图中的节点是由特征匹配候选组成，根据得到的匹配分数将匹配候选中分数最高特征匹配候选作为起始节点，分数最低特征匹配候选作为终止节点，并规定有向图中边的方向由起始节点指向终止节点；

(2)完成有向图的构造后，判断节点间是否存在由匹配分数高节点指向匹配分数低节点的边。若两组匹配平面不满足夹角相似约束，将边赋值为0，即这两个节点间的边不存在，否则判断节点间存在边。最终得到n组具有边的节点，其所代表的匹配候选即为正确的面特征匹配。

优选的，在步骤S3中，记录对应平面内的点特征匹配对信息

和

p_i相对于Z^A的位置与p_j相对于Z^B的位置相似，则最终保留的点特征匹配对应满足下式所示：

其中，Z^A、Z^B分别为相邻帧中对应的平面，p_i和p_j是图像中不同平面Z^A和Z^B中的点特征，p_j是p_i对应的匹配点，p_i在图像中的坐标为(u_i，v_i)，平面Z^A的重心坐标为(u_ZA，v_ZA)；将不满足上述表达式的点特征错误匹配剔除，得到正确的面匹配。

优选的，在步骤S4中，定义相机初始位置状态为c_k-1，则另外两张图像帧分别相对于c_k-1的状态为c_k、c_k+1，c_k相对于c_k-1的旋转矩阵和平移矩阵为R_k、t_k，得到相机的c_k姿态矩阵为Q_k＝M[R_k|t_k]，其中M为相机的内参；将任意匹配对(p_k-1，p_k，p_k+1)齐次变换再归一化处理得到

使用RANSAC算法计算得到在三张连续帧中的共同特征点并计算点特征对于平面的相对位置阈值，当共同特征点的相对位置数值大于相对位置阈值H则剔除此匹配，H的算式如下式所示：

其中，p_k-1、p_k、p_k+1为连续帧中c_k-1、c_k、c_k+1对应匹配特征点，

为

的反对称矩阵，

为

的i列元素，r₄、u₄为旋转矩阵R_k、R_k+1的第4列矩阵，R_k+1为c_k+1相对于c_k的旋转矩阵，r_i、u_i为旋转矩阵R_k、R_k+1第i列元素，

为r_i、u_i的转置矩阵，

为p_k-1的第i维坐标。

优选的，机器人定位方法还包括：获取准确视觉特征关联信息后，根据图像间的关联信息利用SFM估计滑动窗口内所有帧的相对位姿，SFM求解的结果与IMU预积分的值进行对齐，完成VINS系统的初始化；

当所有的惯性量被估计出来，相机位姿和三维特征点被重新缩放为度量单位，世界坐标系重新对齐使得Z轴处于重力方向；对于场景中其他的平面，同样计算它们各自的平面单应矩阵并对其分解。由此前端处理的图像帧中获取的特征以及特征关联信息，利用视觉与IMU时间戳对齐获取相邻图像帧之间的IMU测量数据，求解当前状态下所有视觉和惯性量，将这些估计值送入滑动窗口以进行后端非线性优化。

优选的，所述后端非线性优化模块是通过最小化视觉点面特征的重投影误差、IMU测量残差和滑动窗口边缘化先验约束的方式来联合优化相机位姿；优化函数如下式所示，

其中，l_ij表示窗口内两个连续帧i和j之间的所有IMU测量值，κ代表帧的集合，

是IMU预积分误差，i帧中所有观察到的点特征的集合表示为c_i，l表示集合中的点特征，所有观察到的面特征的集合表示为P，p表示集合中的面特征，r_p是边缘化先验误差，

是重投影误差，

是IMU预积分噪声项的协方差，

和

是视觉观测的噪声协方差，ρ(g)是抑制离群点的鲁棒核函数，

为对应图像位置真实测量值，

为对应图像位置期望测量值，

为对应平面法向量的转置矩阵，

为平面法向量的逆矩阵，

为图像帧中特征点坐标，R_ji和t_ji表示将点从t_i坐标系转换到t_j下的旋转和平移，B表示自身坐标系与IMU坐标系重合，C表示相机坐标系，T_BC表示相机坐标系到自身坐标系的旋转和平移。

本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的消除静态特征误匹配的机器人定位方法的步骤。

本发明还提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上所述的消除静态特征误匹配的机器人定位方法的步骤。

本发明具有以下优点：本发明在剔除动态物体后，采用双重特征约束的误匹配剔除策略，对点特征和面特征的误匹配均进行了剔除，利用这些平面的简单几何形状进行运动估计，提高算法鲁棒性。双重特征误匹配剔除的方法首先使用基于平面物理参数的匹配方法来直接匹配不同坐标系下的平面，寻找出匹配候选，随后利用基于平面几何约束的面特征误匹配算法去除匹配候选中的错误匹配。根据点特征所属平面信息剔除非对应平面内的点特征匹配，再根据点特征位置剔除对应平面内的点特征误匹配，点面特征共同使用参与机器人位姿估计，进一步保证特征匹配的正确率。由此能够获取准确的视觉特征关联信息，完成系统初始化后，进行滑动窗口优化时位置漂移误差较小，进而估计出更精确的相机位姿，提高机器人定位精度，从而构建更准确的轨迹图，并在动态场景中依然具有较高鲁棒性。

附图说明

图1为本发明基于消除静态特征误匹配的机器人定位方法的流程示意图。

图2为本发明基于消除静态特征误匹配的机器人定位方法的流程图。

图3为本发明平面分割对比图。

图4为本发明面特征匹配对比图。

图5为本发明双重特征误匹配剔除对比图。

图6为本发明滑动窗口位置漂移误差图。

图7为本发明在OpenLORIS-Scene数据集不同序列下运行得到的轨迹图。

图8为真实实验环境场景和对应的平面布局图。

图9为移动机器人真实场景运行轨迹图。

具体实施方式

下面对照附图，通过对实施例的描述，对本发明具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和伸入的理解。

实施例一：

如图1-9所示，本发明提供了消除静态特征误匹配的机器人定位方法，该方法中，以图像帧为输入，识别输出场景中所有静态平面的分割图和空间中每个平面的三维参数，从而在场景中建立分段静态平面。静态平面区域中提取点面特征并进行点面特征匹配，采用双重特征约束的误匹配剔除策略。面特征的匹配首先构建出局部坐标系下相邻2帧中平面的匹配关系，使用基于平面物理参数的匹配方法来直接匹配不同坐标系下的平面，寻找出匹配候选，随后利用基于平面几何约束的面特征误匹配算法去除匹配候选中的错误匹配，得到正确的面特征匹配关系以及点特征所属平面信息。根据点特征所属平面信息剔除非对应平面内的点特征匹配，再根据点特征位置剔除对应平面内的点特征误匹配。

本机器人定位方法具体包括。

步骤S1、根据机器人所自带的RGB-D相机采集环境信息，分割静态平面区域，该区域通过实例分割网络生成。

步骤S2，提取静态平面区域的点面特征进行面特征匹配，剔除面特征误匹配。

平面特征的匹配通过比较两平面特征的法向量夹角、重心这些特征之间的欧式距离来实现。具体来说，先分别计算两平面特征的向量夹角之间的欧式距离和两平面特征的重心之间的欧式距离，以此表示相似度，对不同相似度进行打分求和得到匹配分数，设定分数阈值得到面特征匹配候选。相邻图像帧中任意两相同匹配平面间的法向量应当近似相等，在最终匹配结果中的任意两组匹配平面必须满足此条件，即满足夹角相似度约束。因此本方案将是否满足该约束转化为有向无环图问题，即假设在m组平面特征匹配候选中，包含n(n≤m)组正确的匹配特征，则：

(1)有向无环图中的节点是由特征匹配候选组成，根据得到的匹配分数将匹配候选中分数最高特征匹配候选作为起始节点，分数最低特征匹配候选作为终止节点，并规定有向图中边的方向由起始节点指向终止节点。

(2)完成有向图的构造后，判断节点间是否存在由匹配分数高节点指向匹配分数低节点的边。若两组匹配平面不满足夹角相似约束，将边赋值为0，即这两个节点间的边不存在，否则判断节点间存在边。最终得到n组具有边的节点，其所代表的匹配候选即为正确的面特征匹配。

这样，该步骤能高效准确地将面特征误匹配进行剔除，避免面特征匹配错误，并为之后平面上的点特征匹配错误的剔除做准备。

步骤S3，记录正确面特征匹配对中所属点特征信息，剔除非对应平面的点特征匹配。

记录对应平面(即正确的面特征匹配中的两个平面)内的点特征匹配对信息

和

p_i相对于Z^A的位置与p_j相对于Z^B的位置相似，则最终保留的点特征匹配对应满足下式所示：

其中，Z^A、Z^B分别为相邻帧中对应的平面，p_i和p_j是图像中不同平面Z^A和Z^B中的点特征，p_j是p_i对应的匹配点，p_i在图像中的坐标为(u_i，v_i)，平面Z^A的重心坐标为(u_ZA，v_ZA)。

将不满足上述表达式的点特征错误匹配剔除，即可得到正确的面匹配，可以进一步筛选对应平面上点特征匹配。

步骤S4，将对应平面的点特征匹配进行平面内误匹配剔除以实现二次筛选，剔除剩余点特征错误匹配。

定义相机初始位置状态为c_k-1，则另外两张图像帧分别相对于c_k-1的状态为c_k、c_k+1，c_k相对于c_k-1的旋转矩阵和平移矩阵为R_k、t_k，得到相机的c_k姿态矩阵为Q_k＝M[R_k|t_k]，其中M为相机的内参。将任意匹配对(p_k-1，p_k，p_k+1)齐次变换再归一化处理得到

使用RANSAC算法计算得到在三张连续帧中的共同特征点并计算点特征对于平面的相对位置阈值，当共同特征点的相对位置数值大于相对位置阈值H则剔除此匹配，H的算式如下式所示。

其中，p_k-1、p_k、p_k+1为连续帧中c_k-1、c_k、c_k+1对应匹配特征点，

为

的反对称矩阵，

为

的i列元素，r₄、u₄为旋转矩阵R_k、R_k+1的第4列矩阵，R_k+1为c_k+1相对于c_k的旋转矩阵，r_i、u_i为旋转矩阵R_k、R_k+1第i列元素，

为r_i、u_i的转置矩阵，

为p_k-1的第i维坐标。

通过上述算法，本方法筛选出静态平面区域并采用双重特征误匹配剔除策略(面特征误匹配剔除和点特征误匹配剔除)，使得本方法能获取准确视觉特征关联信息。根据图像间的关联信息利用SFM(Structure from Motion)估计滑动窗口内所有帧的相对位姿，SFM求解的结果与IMUfInertial Measurement Unit)预积分的值进行对齐，完成VINS(Visual-Inertial Navigation System)系统的初始化。

当所有的惯性量被估计出来，相机位姿和三维特征点被重新缩放为度量单位，世界坐标系重新对齐使得Z轴处于重力方向。对于场景中其他的平面，同样计算它们各自的平面单应矩阵并对其分解。由此前端处理的图像帧中获取的特征以及特征关联信息，利用视觉与IMU时间戳对齐获取相邻图像帧之间的IMU测量数据，求解当前状态下所有视觉和惯性量，将这些估计值送入滑动窗口以进行后端非线性优化。

后端非线性优化模块是通过最小化视觉点面特征的重投影误差、IMU测量残差和滑动窗口边缘化先验约束的方式来联合优化相机位姿。优化函数如下式所示。

其中，l_ij表示窗口内两个连续帧i和j之间的所有IMU测量值，κ代表帧的集合，

是IMU预积分误差，i帧中所有观察到的点特征的集合表示为c_i，l表示集合中的点特征，所有观察到的面特征的集合表示为P，p表示集合中的面特征，r_p是边缘化先验误差，

是重投影误差，

是IMU预积分噪声项的协方差，

和

是视觉观测的噪声协方差，ρ(g)是抑制离群点的鲁棒核函数，

为对应图像位置真实测量值，

为对应图像位置期望测量值，

为对应平面法向量的转置矩阵，

为平面法向量的逆矩阵，

为图像帧中特征点坐标，R_ji和t_ji表示将点从t_i坐标系转换到t_j下的旋转和平移，B表示自身坐标系与IMU坐标系重合，C表示相机坐标系，T_BC表示相机坐标系到自身坐标系的旋转和平移。

这样本方法基于优化结果完成视觉惯性融合，相比传统视觉惯性SLAM方法在动态场景下在剔除动态物体后能基于点面特征均精确匹配，获取准确视觉特征关联信息，进而保证系统位姿估计与地图构建的准确性较高，本定位方法也因此具有较高的定位精度，可靠性和鲁棒性均较好。

说明书附图中，图3-图9的含义具体如下。

图3为本方法在公开数据集OpenLORIS-Scene中静态平面分割效果图，本方法选取数据集中存在动态物的场景来验证本方法的效果。由图可知，本方法将独立平面整合为单个大平面，因此本方法可将静态平面区域完整分割。

图4为本方法在公开OpenLORIS-Scene数据集中对面特征匹配效果进行对比。图中黑色虚线为真实轨迹，浅灰色实线为改进前轨迹，深灰色实线为本方法，可以看出本方法计算得到的轨迹更加接近真实轨迹，这是因为本方法加入基于平面几何约束的判断条件，能够在所有匹配候选中筛选保留正确匹配对，从而构建更准确的轨迹图。

图5为本方法选取公开OpenLORIS-Scene数据集验证双重特征误匹配效果。本方法通过根据点特征所属平面信息及对应平面匹配候选点相对位置剔除其中错误匹配候选点，因此，本方法能精准判断并剔除错误匹配候选。

图6为本方法在公开数据集OpenLORIS-Scene中滑动窗口优化位置漂移误差图。本方法剔除动态物获得静态平面区域并采用双重特征误匹配剔除的算法，能够获取准确的视觉特征关联信息，完成系统初始化后，进行滑动窗口优化时位置漂移误差较小。

图7为本方法算在公开数据集OpenLORIS-Scene不同序列下的轨迹运行图。本方法剔除动态物获得静态平面区域，提取形态平面特征，采用基于平面约束剔除非对应平面信息特征误匹配并再次剔除对应平面特征误匹配进一步筛选特征匹配，估计出更精确的相机位姿。由图可知本方法在动态场景中依然具有较高鲁棒性。

下面将用实验对本实施例的方法进行说明：选取真实实验环境如图8(a)所示，大小为7.8m×7.2m。图8(b)为真实场景平面布局，其中，A到B为机器人运动路线，E-F段和C-D段为行人往返运动路线。

图9所示为本方法真实场景运行轨迹图。由图9可知，在动态场景中本方法构建平面损失函数剔除动态物并检测场景中的静态平面区域，利用基于几何关系的双重特征匹配策略在静态平面区域提取高质量特征，提高特征匹配精度。因此本方法在动态场景中具有较高鲁棒性。

实施例二：

与本发明实施例一对应，本发明实施例二提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时依照实施例一的方法实现以下步骤。

步骤S1、根据机器人所自带的RGB-D相机采集环境信息，分割静态平面区域，该区域通过实例分割网络生成。

步骤S2，提取静态平面区域的点面特征进行面特征匹配，剔除面特征误匹配。

步骤S3，记录正确面特征匹配对中所属点特征信息，剔除非对应平面的点特征匹配。

步骤S4，将对应平面的点特征匹配进行平面内误匹配剔除以实现二次筛选，剔除剩余点特征错误匹配。

按上述方法获取准确视觉特征关联信息，处理的图像帧中获取的特征以及特征关联信息，利用视觉与IMU时间戳对齐获取相邻图像帧之间的IMU测量数据，求解当前状态下所有视觉和惯性量，将这些估计值送入滑动窗口以进行后端非线性优化。基于优化结果完成视觉惯性融合，最终实现机器人的定位和轨迹生成。

上述存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、光盘等各种可以存储程序代码的介质。

上述关于计算机可读存储介质中程序执行后实现步骤的具体限定可以参见实施例一，在此不再做详细说明。

实施例三：

与本发明实施例一对应，本发明实施例三提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时依照实施例一的方法实现以下步骤。

步骤S1、根据机器人所自带的RGB-D相机采集环境信息，分割静态平面区域，该区域通过实例分割网络生成。

步骤S2，提取静态平面区域的点面特征进行面特征匹配，剔除面特征误匹配。

步骤S3，记录正确面特征匹配对中所属点特征信息，剔除非对应平面的点特征匹配。

步骤S4，将对应平面的点特征匹配进行平面内误匹配剔除以实现二次筛选，剔除剩余点特征错误匹配。

按上述方法获取准确视觉特征关联信息，处理的图像帧中获取的特征以及特征关联信息，利用视觉与IMU时间戳对齐获取相邻图像帧之间的IMU测量数据，求解当前状态下所有视觉和惯性量，将这些估计值送入滑动窗口以进行后端非线性优化。基于优化结果完成视觉惯性融合，最终实现机器人的定位和轨迹生成。

上述关于处理器上运行的程序执行后实现步骤的具体限定可以参见实施例一，在此不再做详细说明。

需要说明的是，本发明的说明书附图中的框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与获得机指令的组合来实现。

上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的发明构思和技术方案进行的各种非实质性的改进，或未经改进将本发明构思和技术方案直接应用于其它场合的，均在本发明保护范围之内。

Claims (10)

1.消除静态特征误匹配的机器人定位方法，其特征在于：以图像帧为输入，识别输出场景中所有静态平面的分割图和空间中每个平面的三维参数，从而在场景中分割建立分段静态平面；静态平面区域中提取点面特征并进行点面特征匹配，采用双重特征约束的误匹配剔除策略；面特征的匹配首先构建出局部坐标系下相邻2帧中平面的匹配关系，使用基于平面物理参数的匹配方法来直接匹配不同坐标系下的平面，寻找出匹配候选，随后利用基于平面几何约束的面特征误匹配算法去除匹配候选中的错误匹配，得到正确的面特征匹配关系以及点特征所属平面信息；根据点特征所属平面信息剔除非对应平面内的点特征匹配，再根据点特征位置剔除对应平面内的点特征误匹配。

2.根据权利要求1所述的消除静态特征误匹配的机器人定位方法，其特征在于：具体包括：

步骤S1、根据机器人所自带的相机采集环境信息，分割静态平面区域，该区域通过实例分割网络生成；

步骤S2，提取静态平面区域的点面特征进行面特征匹配，剔除面特征误匹配；

步骤S3，记录正确面特征匹配对中所属点特征信息，剔除非对应平面的点特征匹配；

步骤S4，将对应平面的点特征匹配进行平面内误匹配剔除以实现二次筛选，剔除剩余点特征错误匹配。

3.根据权利要求2所述的消除静态特征误匹配的机器人定位方法，其特征在于：所述步骤S2中，平面特征的匹配先分别计算两平面特征的向量夹角之间的欧式距离和两平面特征的重心之间的欧式距离，以此表示相似度，对不同相似度进行打分求和得到匹配分数，设定分数阈值得到面特征匹配候选；相邻图像帧中任意两相同匹配平面间的法向量应当近似相等，在最终匹配结果中的任意两组匹配平面必须满足此条件，即满足夹角相似度约束；将是否满足该约束转化为有向无环图问题解决，从而剔除不满足夹角相似度约束的面特征误匹配。

4.根据权利要求3所述的消除静态特征误匹配的机器人定位方法，其特征在于：所述无环图问题即假设在m组平面特征匹配候选中，包含n(n≤m)组正确的匹配特征，则：

(1)有向无环图中的节点是由特征匹配候选组成，根据得到的匹配分数将匹配候选中分数最高特征匹配候选作为起始节点，分数最低特征匹配候选作为终止节点，并规定有向图中边的方向由起始节点指向终止节点；

(2)完成有向图的构造后，判断节点间是否存在由匹配分数高节点指向匹配分数低节点的边。若两组匹配平面不满足夹角相似约束，将边赋值为0，即这两个节点间的边不存在，否则判断节点间存在边。最终得到n组具有边的节点，其所代表的匹配候选即为正确的面特征匹配。

5.根据权利要求4所述的消除静态特征误匹配的机器人定位方法，其特征在于：在步骤S3中，记录对应平面内的点特征匹配对信息

和

p_i相对于Z^A的位置与p_j相对于Z^B的位置相似，则最终保留的点特征匹配对应满足下式所示：

其中，Z^A、Z^B分别为相邻帧中对应的平面，p_i和p_j是图像中不同平面Z^A和Z^B中的点特征，p_j是p_i对应的匹配点，p_i在图像中的坐标为(u_i，v_i)，平面Z^A的重心坐标为(u_ZA，v_ZA)；将不满足上述表达式的点特征错误匹配剔除，得到正确的面匹配。

6.根据权利要求5所述的消除静态特征误匹配的机器人定位方法，其特征在于：在步骤S4中，定义相机初始位置状态为c_k-1，则另外两张图像帧分别相对于c_k-1的状态为c_k、c_k+1，c_k相对于c_k-1的旋转矩阵和平移矩阵为R_k、t_k，得到相机的c_k姿态矩阵为Q_k＝M[R_k|t_k]，其中M为相机的内参；将任意匹配对(p_k-1，p_k，p_k+1)齐次变换再归一化处理得到

使用RANSAC算法计算得到在三张连续帧中的共同特征点并计算点特征对于平面的相对位置阈值，当共同特征点的相对位置数值大于相对位置阈值H则剔除此匹配，H的算式如下式所示：

其中，p_k-1、p_k、p_k+1为连续帧中c_k-1、c_k、c_k+1对应匹配特征点，

为

的反对称矩阵，

为

的i列元素，r₄、u₄为旋转矩阵R_k、R_k+1的第4列矩阵，R_k+1为c_k+1相对于c_k的旋转矩阵，r_i、u_i为旋转矩阵R_k、R_k+1第i列元素，

为r_i、u_i的转置矩阵，

为p_k-1的第i维坐标。

7.根据权利要求6所述的消除静态特征误匹配的机器人定位方法，其特征在于：还包括：获取准确视觉特征关联信息后，根据图像间的关联信息利用SFM估计滑动窗口内所有帧的相对位姿，SFM求解的结果与IMU预积分的值进行对齐，完成VINS系统的初始化；

当所有的惯性量被估计出来，相机位姿和三维特征点被重新缩放为度量单位，世界坐标系重新对齐使得Z轴处于重力方向；对于场景中其他的平面，同样计算它们各自的平面单应矩阵并对其分解。由此前端处理的图像帧中获取的特征以及特征关联信息，利用视觉与IMU时间戳对齐获取相邻图像帧之间的IMU测量数据，求解当前状态下所有视觉和惯性量，将这些估计值送入滑动窗口以进行后端非线性优化。

8.根据权利要求7所述的消除静态特征误匹配的机器人定位方法，其特征在于：所述后端非线性优化模块是通过最小化视觉点面特征的重投影误差、IMU测量残差和滑动窗口边缘化先验约束的方式来联合优化相机位姿；优化函数如下式所示，

其中，l_ij表示窗口内两个连续帧i和j之间的所有IMU测量值，κ代表帧的集合，

是IMU预积分误差，i帧中所有观察到的点特征的集合表示为c_i，l表示集合中的点特征，所有观察到的面特征的集合表示为P，p表示集合中的面特征，r_p是边缘化先验误差，

是重投影误差，

是IMU预积分噪声项的协方差，

和

是视觉观测的噪声协方差，ρ(g)是抑制离群点的鲁棒核函数，

为对应图像位置真实测量值，

为对应图像位置期望测量值，

为对应平面法向量的转置矩阵，

为平面法向量的逆矩阵，

为图像帧中特征点坐标，R_ji和t_ji表示将点从t_i坐标系转换到t_j下的旋转和平移，B表示自身坐标系与IMU坐标系重合，C表示相机坐标系，T_BC表示相机坐标系到自身坐标系的旋转和平移。

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现如权利要求1-8中任一所述的消除静态特征误匹配的机器人定位方法的步骤。

10.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于：所述处理器执行所述计算机程序时实现如权利要求1-8中任一所述的消除静态特征误匹配的机器人定位方法的步骤。

Images