CN113103232B - 一种基于特征分布匹配的智能设备自适应运动控制方法 - Google Patents

Abstract

本发明公开了一种基于特征分布匹配的智能设备自适应运动控制方法，该方法综合考虑了特征的提取匹配过程和SLAM算法前端的跟踪状态，根据实时提取到的ORB特征及实时的跟踪状态，对机器人的行进速度和转向速度进行自适应控制，解决现有恒速控制模型下，可能导致SLAM跟踪丢失进而导致整个SLAM过程失败的问题，具体而言，一方面避免在纹理较少区域，行进速度过快，导致跟踪丢失；另一方面在纹理较丰富区域，提高机器人的行进速度，进而提升整个系统的运行效率。

Description

一种基于特征分布匹配的智能设备自适应运动控制方法

技术领域

本发明涉及移动机器人控制技术领域，具体涉及一种基于特征分布匹配的智能设备自适应运动控制方法。

背景技术

室内环境下，由于缺乏GPS信号覆盖，当前一般采用SLAM(simultaneouslocalization and mapping，同时定位与建图)技术实现室内环境下的地图构建和定位。当前，都是由专人通过遥控器或者远程主机手动操作机器人进行SLAM过程，具体的实施过程为，操作人员控制机器人以尽可能全覆盖的方式在未知环境中移动，机器人根据自身搭载的传感器搜集到的信息，估计出自己的位姿，同时绘制出周围环境的地图。

为了提升机器人同时定位与建图过程的智能性和效率，前人已经在这方面有了一些探索。Feder H.J.S.等人提出了自适应移动机器人导航和制图方法，这种主动探索式地移动和制图的方式可以有效提高制图和导航的效率。因此，考虑当前SLAM中的实际情况，主动SLAM也可以带来效率的提升，并可为后期拓展路径规划和自主导航等应用提供基础。

现有的主动SLAM方法一般都是采用恒速模型来控制机器人的前进和转向，这就带来了一个问题，对于基于特征的SLAM系统，提取特征和匹配特征都需要一定的时间和硬件资源的开销，如果机器人一直按照恒速前进或者转向，则可能由于硬件资源的处理速度跟不上导致跟踪的丢失(对于机器人纯旋转的情形尤甚)，尤其是到了SLAM过程的后期，需要维持较大地图的更新，更需要采用适当的策略来保证SLAM过程的持续运行。

并且在VSLAM(Visual Simultaneous Localization and Mapping，视觉同时定位和制图)研究领域中，基于特征点的方法是一个重要的研究分支。而在基于特征点的VSLAM系统中，前端的视觉里程计又扮演了系统中的一个重要角色。视觉里程计的主要作用可以描述为：通过比较相邻两个图像帧，实现对相机运动状态的估计。而在进行相邻图像帧之间的比较时，首先需要解决的问题就是怎样找出前后两帧图像中相同的部分。而在基于特征的方法中，解决这个问题的方法就是进行帧间的特征匹配。

在图像处理领域中，经过长期的研究和实践，前人得到了一些非常实用的特征提取方法，其中最为人熟知也应用最广泛的两种就是SIFT特征和SURF特征，这两种特征的优点是抗噪性和可重识别性优良，但是二者的短板也非常的明显，就是提取过程比较复杂，计算量非常大，在一些对实时性要求较高的应用中，比如VSLAM中，就不能达到应用要求。而2014年，由Ethan Rublee等人提出的ORB特征为实时性的应用带来了新的思路。此后，在许多的应用中都采用了ORB特征提取方法。而在VSLAM领域中，这一方面的集大成者便是2015年发表的ORB_SLAM，整个ORB_SLAM系统从前端到后端都是由ORB特征进行链接。

原始版本的ORB特征的提取效率和匹配效率相较SIFT和SURF特征已经有了明显得提升，但仍有改进的空间，比如，虽然ORB特征在进行帧间匹配时的匹配率较高，但最终的匹配结果中，存在许多的误匹配，而误匹配对于计算前后帧之间的位姿变换将造成非常不好的影响。当前的特征点提取算法，都是首先提取图片中的关键点，然后计算关键点的描述子。提取关键点的过程中，都将图片中的每个关键点视作一个独立的对象，单独计算每个关键点的描述子，没有对各个关键点在整张图片上的几何分布信息进行有效的利用。

发明内容

针对现有技术中的上述不足，本发明提供了一种基于特征分布匹配的智能设备自适应运动控制方法。

为了达到上述发明目的，本发明采用的技术方案为：

一种基于特征分布匹配的智能设备自适应运动控制方法，包括以下步骤：

S1、对移动机器人进行初始化；

S2、对采集的初始图像进行预处理后提取初始图像ORB特征，并获取初始位姿；

S3、启动移动机器人以预设初始速度匀速行进，提取行进过程中实时图像的ORB特征并计算特征分布；

S4、启动SLAM跟踪线程，采用基于特征分布的方法进行帧间ORB特征匹配，记录匹配的ORB特征数量和特征匹配率；

S5、获取移动机器人当前行进速度，判断移动机器人是否处于正常行进状态；若是，则保持当前行进速度继续行进；否则执行步骤S6；

S6、根据匹配ORB特征数判断SLAM跟踪是否失败；若是，则执行步骤S7；否则调整移动机器人行进速度继续行进；

S7、查询SLAM跟踪线程状态，判断当前SLAM跟踪线程是否丢失；若是，则将移动机器人回退并执行步骤S8；否则调整移动机器人行进速度继续行进；

S8、对移动机器人进行重定位；若重定位成功，则重置移动机器人行进速度继续行进；否则移动机器人停止行进。

进一步地，所述步骤S2和S3中ORB特征的提取方法具体包括：

对图像进行灰度化预处理，并构建图像金字塔；

将图像金字塔按层次分割；

对每一层分割后的图像部分并行提取FAST关键点；

对提取的FAST关键点进行筛选，得到中心区域关键点；

根据中心区域关键点与图像中心像素的距离查找中心关键点；

根据查找的中心关键点计算中心关键区；

计算图像中每个中心关键点与距离图像中心像素最近的中心关键点的相对位置；

计算图像中每个FAST关键点与距离图像中心像素最近的中心关键点的相对位置；

计算图像中每个FAST关键点的主方向；

计算图像中每个FAST关键点的描述子，得到图像的ORB特征。

进一步地，所述步骤S4中基于特征分布的帧间ORB特征匹配的方法具体包括：

S41、获取两幅相邻帧图像的所有特征点；

S42、根据第一图像的中心关键点以及计算得到的中心关键区，在第二图像的中心关键区内搜索第一图像的中心关键点的匹配特征点；若搜索到匹配特征点，则执行步骤S43；否则执行步骤S45；

S43、以搜索到的匹配特征点的坐标，计算在第二图像中进行全图搜索匹配时解算匹配特征点相对位置的起点；

S44、遍历第二图像中所有的可能出现匹配特征点的位置，并搜索该位置设定区域范围内的匹配特征点，保存搜索到的所有匹配特征点；

S45、扩大第二图像的中心关键区的搜索范围，在扩大后的区域内搜索第一图像的中心关键点的匹配特征点；若搜索到匹配特征点，则执行步骤S43；否则采用暴力匹配方式进行帧间ORB特征匹配。

进一步地，所述步骤S5具体包括：

获取移动机器人当前行进速度；

判断移动机器人当前行进速度是否达到预设的最大限定速度或最小限定速度；

若移动机器人当前行进速度达到最大限定速度，且当前帧中包含的匹配ORB特征数大于最小阈值，则保持当前行进速度继续行进；

若移动机器人当前行进速度达到最小限定速度，且当前帧中包含的匹配ORB特征数等于最小阈值，则保持当前行进速度继续行进；

否则，执行步骤S6。

进一步地，所述步骤S6具体包括以下分步骤：

S61、判断当前帧与上一帧匹配ORB特征数是否小于第一阈值；若是，则执行步骤S7；否则执行步骤S62；

S62、判断当前帧与上一帧匹配ORB特征数是否大于第二阈值；若是，则执行步骤S63；否则保持当前行进速度继续行进；

S63、判断是否为连续第一数量的帧间匹配ORB特征数大于第二阈值；若是，则执行步骤S64；否则保持当前行进速度继续行进；

S64、调整移动机器人行进速度，以调整后的行进速度继续行进；

S65、判断是否为连续第二数量的帧间匹配ORB特征数大于第二阈值；若是，则执行步骤S66；否则保持当前第二阈值不变；

S66、计算当前帧之前的第二数量个帧的特征匹配率均值，判断当前帧的特征匹配率是否大于计算的特征匹配率均值；若是，则调整匹配ORB特征数的第二阈值；否则保持当前第二阈值不变。

进一步地，所述步骤S7具体包括以下分步骤：

S71、查询SLAM跟踪线程状态，判断当前SLAM跟踪线程是否丢失；若是，则将移动机器人停止前进，并执行步骤S72；否则执行步骤S73；

S72、从关键帧数据库读取上一个关键帧位姿，计算当前帧位姿和上一关键帧位姿之间的位姿变换矩阵，将移动机器人回退至上一个关键帧位姿，并执行步骤S8；

S73、判断是否为连续第一数量的帧间匹配ORB特征数小于第一阈值；若是，则执行步骤S74；否则保持当前行进速度继续行进；

S74、调整移动机器人行进速度，以调整后的行进速度继续行进；

S75、判断是否为连续第二数量的帧间匹配ORB特征数小于第一阈值；若是，则执行步骤S76；否则保持当前第一阈值不变；

S76、计算当前帧之前的第二数量个帧的特征匹配率均值，判断当前帧的特征匹配率是否大于计算的特征匹配率均值；若是，则调整匹配ORB特征数的第一阈值；否则保持当前第一阈值不变。

进一步地，所述步骤S8具体包括以下分步骤：

S81、在回退过程中进行重定位并记录重定位次数，实时判断重定位是否成功；若是，则停止回退，并将移动机器人的行进速度和匹配ORB特征数的最小阈值进行重置，然后以预设初始速度匀速行进；否则执行步骤S82；

S82、若回退至上一个关键帧位姿后依然没有重定位成功，则在该关键帧处停止回退，并以移动机器人正前方作为主方向，在主方向设定偏航角范围内旋转移动机器人，对移动机器人进行辅助重定位；

S83、判断辅助重定位是否成功；若是，则停止回退，并将移动机器人的行进速度和匹配ORB特征数的最小阈值进行重置，然后以预设初始速度匀速行进；否则从关键帧数据库读取上一个关键帧位姿，计算当前帧位姿和上一关键帧位姿之间的位姿变换矩阵，将移动机器人回退至上一个关键帧位姿；

S84、判断重定位次数是否达到设定次数阈值；若是，则移动机器人停止行进，并输出跟踪丢失；否则返回步骤S81。

进一步地，所述特征匹配率的计算公式为：

其中，ρ表示特征匹配率，γ_n表示第n帧提取的特征数，γ_n-1表示第n-1帧提取的特征数，γ_n∩Υ_n-1表示第n帧和第n-1帧之间匹配的ORB特征数。

进一步地，所述调整移动机器人行进速度的调节公式为：

V_new＝a+b*log₁₀γ

其中，V_new表示调整后的移动机器人行进速度，a、b为常数，γ表示当前帧与上一帧之间匹配的ORB特征数。

进一步地，所述匹配ORB特征数的第一阈值和第二阈值的调节公式为：

其中，Γ_new表示调整后的阈值，γ_i表示前n个帧中第i帧包含的匹配特征数。

本发明的有益效果是：

(1)本发明综合考虑了特征的提取匹配过程和SLAM算法前端的跟踪状态，根据提取到的特征及实时的跟踪状态，对机器人的行进速度和转向速度进行自适应控制，解决现有恒速控制模型下，可能导致SLAM跟踪丢失进而导致整个SLAM过程失败的问题；

(2)本发明根据视觉SLAM中当机器人的运行速度一定且相机输出图像的帧率较高时，视觉SLAM中需要跟踪的是时间间隔非常短的相邻帧，此时相邻两帧之间的变化非常少，而且图像中的点的相对位置关系变化很少，在空间尺度上相邻图像帧之间的特征点的位置分布不会有较大的变化的特性，利用特征在图像帧中的几何位置分布提高相邻帧之间的特征匹配速度和准确度。

(3)本发明可以根据当前环境中环境纹理特征数量，自适应的调整机器人的行进速度，一方面避免在纹理较少区域，行进速度过快，导致跟踪丢失。另一方面在纹理较丰富区域，提高机器人的行进速度，进而提升整个系统的运行效率。

(4)本发明根据滑动窗口方法自适应调整特征阈值，可以利用每个帧的特征匹配率这一指标，衡量当前环境中所提取特征的质量的优劣，并且以环境中特征的分布情况自适应的调节帧间匹配的阈值。

附图说明

图1为本发明基于特征分布匹配的智能设备自适应运动控制方法流程示意图；

图2为本发明基于特征分布匹配的智能设备自适应运动控制方法流程图；

图3为本发明实施例中帧间ORB特征匹配示意图；其中图(a)为第一图像帧间ORB特征匹配，图(b)为第二图像帧间ORB特征匹配；

图4为本发明实施例中行进速度拟合结果示意图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

如图1和图2所示，本发明实施例提供了一种基于特征分布匹配的智能设备自适应运动控制方法，包括以下步骤S1至S8：

S1、对移动机器人进行初始化；

在本实施例中，本发明在控制程序启动后，首先对机器人底座和相机等硬件进行初始化操作；初始化完成后，移动机器人原地待机，并等待相机输出初始图像。

S2、对采集的初始图像进行预处理后提取初始图像ORB特征，并获取初始位姿；

在本实施例中，本发明对相机输出的初始图像进行预处理后提取初始图像的ORB特征。ORB特征的提取方法具体包括：

S21、对图像进行灰度化预处理，并构建图像金字塔；

本发明首先读取待匹配的图像，由于特征提取只需要在灰度图上进行操作，为了节省存储空间同时简化特帧提取流程，若读取的原始图像为彩色图则将其转换为灰度图。并且为了让最终提取到的关键点具有尺度不变性，在成功读取图片后，在原图片之上，以尺度因子α(通常设为1.2)构建k(通常设为8)层图像金子塔。

S22、将图像金字塔按层次分割；

本发明将图像金字塔的每一层均分为m个部分，以便于进行并行处理。

S23、对每一层分割后的图像部分并行提取FAST关键点；

本发明对每一层分割后的m个部分并行提取FAST关键点。为了方便下一个步骤，在提取每一层的m个部分的FAST关键点时，均从距离该层的中心像素最近的地方开始提取FAST关键点。

S24、对提取的FAST关键点进行筛选，得到中心区域关键点；

当对图像金字塔的每一层的m个部分提取到FAST关键点后，由于是从距离每一层的中心像素最近的地方开始提取FAST关键点，那么在存放每一层的每个部分的FAST关键点的容器中，前n个元素就是该部分提取的FAST关键点中距离该层中心像素最近的n个FAST关键点。因此本发明中k层图像金字塔，每层m个部分，每个部分n个中心关键点，最终可以得到P＝k*m*n个中心区域关键点。

S25、根据中心区域关键点与图像中心像素的距离查找中心关键点；

本发明根据得到的P个中心区域关键点，将筛选出其中的c个作为中心关键点。为了兼顾特征提取和后期特征匹配时的效率，本发明设置m为4，n为2，c为5效果较佳。

由于对每一层图像金子塔提取关键点之后，会将关键点的像素坐标映射到最底层的原始图像上，所以可以直接对这P个关键点计算它们到图片中心像素的距离，进而找出c个距离中心像素最近的关键点。

假设某个中心区域关键点的坐标为Pcenter(Pcen_x,Pcen_y)，该关键点到图片几何中心像素Pcenter的距离使用欧式距离表示，计算公式为

最终找到的c个距离图片几何中心最近的关键点称为中心关键点。

S26、根据查找的中心关键点计算中心关键区；

本发明根据查找的c个中心关键点，找出c个中心关键点的横坐标的最小值Xmin和最大值Xmax，以及纵坐标的最小值Ymin和最大值Ymax。以(Xmin–d)作为中心关键区的横坐标的起始值，以(Xmax+d)作为中心关键区的横坐标的终止值，同理，设置纵坐标的起始值为(Ymin-d)，纵坐标的终止值为(Ymax+d)。这样就找到了中心关键区的四个顶点的坐标，分别为：(Xmin–d，Ymin–d)、(Xmin–d，Ymax+d)、(Xmax+d，Ymin–d)、(Xmax+d，Ymax+d)。

S27、计算每个中心关键点与距离图像中心像素最近的中心关键点的相对位置，以及图像中每个关键点与距离中心像素最近的中心关键点的相对位置；

本发明在找到的c个中心关键点中，选择与图片几何中心距离最近的关键点的坐标作为计算相对位置的起始坐标，此处假设距离图片几何中心最近的关键点的坐标为Pnear(Pnear_x，Pnear_y)，假设从图片中提取的某一个FAST关键点的坐标为Pi(Pi_x，Pi_y)，那么FAST关键点Pi与Pnear的相对位置的计算公式为

其中，Δx为相对位置中的横坐标，Δy为相对位置中的纵坐标。

对于c个中心关键点中的其余c-1个中心关键点，需要单独计算这c-1个中心关键点和距离图片几何中心最近的FAST关键点的相对位置并且单独保存。

S28、计算图像中每个FAST关键点的主方向；

为了让最终提取到的FAST关键点具有方向不变形，需要计算图片中每个FAST关键点的主方向，FAST关键点的主方向的计算方法为，在FAST关键点的周围选择一个小的图像块Block，然后计算该图像块的灰度质心，灰度质心的计算公式为

其中，m₁₀、m₀₁和m₀₀都表示图像块的矩，矩的计算方式为

其中，m_pq表示图像的矩，x表示区域内像素的横坐标，y表示区域内像素的纵坐标，I(x,y)表示图形块内部像素坐标为[x,y]的像素的灰度。得到图像块Block的灰度质心后，将它与图像块的几何中心O连接起来，就得到了一个方向向量

此时就将方向向量的方向作为这个FAST关键点的方向，方向向量的方向角的计算公式为

θ＝arctan(m₀₁/m₁₀)

S29、计算图像中每个FAST关键点的描述子，得到图像的ORB特征。

得到FAST关键点后，为了描述这个FAST关键点还需计算FAST关键点的描述子。描述子的计算过程为：在FAST关键点Pi周围的像素邻域内(一般取31x31的一个方形区域)随机地选取128或256对像素点，选取的点对为

其中，x_i表示第i个点的横坐标，y_i表示第i个点的纵坐标，然后利用刚才计算得到的FAST关键点的方向角，可计算得到其旋转矩阵R_θ，然后将该旋转矩阵与D相乘，得到旋转后的点对的坐标位置为

D_θ＝R_θD

然后，对旋转之后得到的像素坐标处的像素的灰度值进行对比，按照设定的对比规则，如果像素对中某一像素的灰度值高于另一像素的灰度值，则取1，反之，则取0，最终得到一个256维由0和1组成的向量，这个向量就是该FAST关键点的描述子。BRIFE使用的是随机选点策略，所以计算速度非常快，同时由于它是二进制形式，存储起来也很方便，特征匹配时速度也很快，改进之后，描述子具有了一定的尺度和旋转不变性。

经过上面的步骤，就得到了具有尺度和旋转不变性以及包含特征点分布的ORB特征。

此外，本发明在机器人开机时，还需要进行位姿初始化操作，以机器人开机处的坐标作为绘制地图的原点，即世界坐标系的[0，0，0]点，然后进行位姿初始化获得机器人的初始位姿。在SLAM系统中，位姿表示机器人在世界坐标系下的位置坐标以及机器人当前的姿态，姿态描述的是以刚体看待机器人时，机器人的俯仰、横滚和偏航，但是在地面应用中，主要考虑机器人的偏航角。而机器人从一个位姿到另一个位姿的变换过程，则是以变换矩阵的形式表示的，即：

其中，T表示变换矩阵，R表示旋转矩阵，t表示平移向量，下方的0^T和1是引入齐次坐标后矩阵中需要的占位符。在后续控制流程中将以此初始位姿作为机器人运动轨迹的起始位姿。

S3、启动移动机器人以预设初始速度匀速行进，提取行进过程中实时图像的ORB特征并计算特征分布；

在本实施例中，本发明启动机器人，使机器人以预设的初始速度匀速前进。并对行进过程中采集的实时图像提取实时图像的ORB特征。实时图像的ORB特征提取方法与步骤S2相同。

S4、启动SLAM跟踪线程，采用基于特征分布的方法进行帧间ORB特征匹配，记录匹配ORB特征数量和特征匹配率；

在本实施例中，本发明在移动机器人开始移动后启动SLAM跟踪线程。SLAM跟踪线程对提取出两幅相邻帧的ORB特征进行帧间ORB特征匹配。匹配过程就是寻找两副图像中对应的特征。ORB特征的描述子是二进制形式的，所以，可以直接用两个描述子间的汉明距离来表示两个特征的相似程度，对两帧之间的特征点进行匹配的最简单方式就是暴力匹配，但是暴力匹配的效率较低，本发明中将使用基于特征点分布的匹配方法，提高帧间匹配的效率和准确率。

基于特征分布的帧间ORB特征匹配的方法具体包括：

S41、获取两幅相邻帧图像的所有特征点；

S42、根据第一图像的c个中心关键点以及计算得到的中心关键区，在第二图像的中心关键区内搜索第一图像的c个中心关键点的匹配特征点；若搜索到匹配特征点，则执行步骤S43；否则执行步骤S45；

具体而言，根据第一图像的中心关键点以及计算得到的中心关键区，如图3(a)所示，设图3(a)中P0为第一图像的中心像素，P1,P2,P3,P4,P5为第一图像中提取的中心关键点，方框为计算所得的中心关键区，在第二图像的中心关键区内搜索第一图像的中心关键点的匹配特征点；若搜索到匹配特征点，则执行步骤S43，如图3(b)所示，此处设第二图像的中心关键区内找到的第一图像的中心关键点P1的匹配特征点为P1-2；若在第二图像的中心关键区内没有找到匹配特征点，则执行步骤S45。

S43、以搜索到的匹配特征点的坐标，计算在第二图像中进行全图搜索匹配时解算匹配特征点相对位置的起点；

具体而言，以搜索到的匹配特征点的坐标，计算在第二图像中进行全图搜索匹配时解算匹配特征点相对位置的起点，如图3(b)所示，以P1-2作为计算相对位置的起点，计算第二图像中可能出现特征点的搜索区域的中心像素Pk的坐标。

假设在第二图像的中心关键区内找到与第一图像的中心关键点P1(P1_x，P1_y)匹配的特征点P12(x₁₂，y₁₂)，则在第二图像中以该特征点的坐标为相对位置的起点，根据从第一图像中计算得到的特征点之间的相对位置，计算出第二图像中可能出现特征点的区域的中心像素位置。此处假设最开始在第一图像中有一个关键点Pk(Pk_x，Pk_y)，在第一图像中，计算出它与距离第一图像的几何中心像素最近的关键点的相对位置为(Δx_k，Δy_k)，那么在第二图像中，这个特征可能出现的区域的中心像素的坐标的计算公式为

其中，x_k和y_k为关键点Pk在第二图像中可能出现的区域的中心像素的横坐标和纵坐标。

S44、遍历第二图像中所有的可能出现匹配特征点的位置，并搜索该位置设定区域范围内的匹配特征点，保存搜索到的所有匹配特征点；

在得到中心像素的坐标后，搜索以该点为中心的w*h范围内的方形区域内的所有特征点，寻找该区域内可能与图一中的第k个特征点匹配的特征点；找到匹配的特征点之后，将匹配的特征点对保存下来。

S45、扩大第二图像的中心关键区的搜索范围，在扩大后的区域内搜索第一图像的中心关键点的匹配特征点；若搜索到匹配特征点，则执行步骤S43；否则采用暴力匹配方式进行帧间ORB特征匹配。

如果在图二的中心关键区内没有找到图一的c个中心关键点的匹配特征点，那将中心关键区的范围扩大s个像素，使用扩大的范围在图二上继续寻找图一的c个中心关键点的匹配特征点。如果扩大搜索范围后，在图二中依然没有找到图一中的c个中心关键点的匹配特征，则放弃搜索匹配，转而使用暴力匹配或FLANN进行两帧之间的特征匹配。

在帧间ORB特征匹配后记录相邻帧之间匹配的ORB特征数，并记录当前帧的特征匹配率。特征匹配率的计算公式为：

其中，ρ表示特征匹配率，γ_n表示第n帧提取的特征数，γ_n-1表示第n-1帧提取的特征数，γ_n∩Υ_n-1表示第n帧和第n-1帧之间匹配的ORB特征数。

S5、获取移动机器人当前行进速度，判断移动机器人是否处于正常行进状态；若是，则保持当前行进速度继续行进；否则执行步骤S6；

在本实施例中，本发明在匹配ORB特征后，读取移动机器人当前行进速度，并判断移动机器人是否处于正常行进状态，这里的正常行进状态是指移动机器人当前行进速度是否已经达到预设的最大限定速度V_max或最小限定速度V_min；若是，则保持当前行进速度继续行进；否则执行步骤S6。

具体而言，步骤S5具体包括：

获取移动机器人当前行进速度；

判断移动机器人当前行进速度是否达到预设的最大限定速度V_max或最小限定速度V_min；

若移动机器人当前行进速度达到最大限定速度V_max，且当前帧中包含的匹配ORB特征数大于最小阈值Γ_min，则保持当前行进速度继续行进；

若移动机器人当前行进速度达到最小限定速度V_min，且当前帧中包含的匹配ORB特征数等于最小阈值Γ_min，则保持当前行进速度继续行进；

否则，执行步骤S6。

本发明中最小阈值Γ_min是保证跟踪线程正常运行所需的最小帧间匹配特征数。

S6、根据匹配ORB特征数判断SLAM跟踪是否失败；若是，则执行步骤S7；否则调整移动机器人行进速度继续行进；

在本实施例中，本发明以最小阈值Γ_min的设定比例作为初始阈值，来判断SLAM跟踪是否失败。具体而言，将初始阈值Γ设置为最小阈值Γ_min的110％比例。

上述步骤S6具体包括以下分步骤：

S61、判断当前帧与上一帧匹配ORB特征数是否小于第一阈值；若是，则执行步骤S7；否则执行步骤S62；

具体而言，设置第一阈值为初始阈值Γ的95％比例，判断当前帧与上一帧匹配ORB特征数是否小于第一阈值；若是，则说明SLAM系统的跟踪已失败，执行步骤S7；否则执行步骤S62；

S62、判断当前帧与上一帧匹配ORB特征数是否大于第二阈值；若是，则执行步骤S63；否则保持当前行进速度继续行进；

具体而言，设置第一阈值为初始阈值Γ的110％比例，判断当前帧与上一帧匹配ORB特征数是否大于第二阈值；若是，则记录当前比较结果，并执行步骤S63；否则将移动机器人保持当前行进速度继续匀速行进；

S63、判断是否为连续第一数量的帧间匹配ORB特征数大于第二阈值；若是，则执行步骤S64；否则保持当前行进速度继续行进；

具体而言，判断是否为连续第m次的帧间匹配ORB特征数大于第二阈值，若是，则执行步骤S64；否则将移动机器人保持当前行进速度继续匀速行进；

S64、调整移动机器人行进速度，以调整后的行进速度继续行进。调整移动机器人行进速度的调节公式为：

V_new＝a+b*log₁₀Υ

其中，V_new表示调整后的移动机器人行进速度，a、b为常数，Υ表示当前帧与上一帧之间匹配的ORB特征数。

本发明基于非均匀量化中的对数压缩方法进行行进速度调节，当帧间匹配的特征数量在一定范围内增加时，以较大的步长提升行进速度，但是当行进速度到达一定程度后，为了保证系统的稳定运行，将减缓速度的增长趋势，最终的行进速度稳定在最大行进速度处。

调整移动机器人行进速度的调节公式的计算过程为：

确定机器人的最大行进速度V_max，此速度为能够保证SLAM系统正常运行时机器人的最大行进速度；确定机器人的最小行进速度V_min，此速度为开机时机器人的默认行进速度；提取当前帧之前的n个帧中特征匹配的最大量Υ_max和最小量Υ_min；然后，基于最小二乘法，利用多项式实现对行进速度调节公式中对数函数的拟合近似，拟合数据为：(Υ_min，V_min)，

(Υ_max，V_max)。已知行进速度调节公式的函数表达式为对数函数，且已知三组函数值，首先按照泰勒级数展开式将原对数函数进行展开，展开至某高次项，得到一个参数多项式p(x)：

p(x)＝a₀+a₁x+...+a_mx^m

其中，a₀，a₁，…，a_m为泰勒展开时对应项的系数；

使得以下误差函数最小：

得到拟合多项式后，将当前帧中的匹配特征数Υ带入拟合多项式计算得到新的行进速度。如图4所示，为行进速度拟合结果示意图。

此外，步骤S64后还包括：

S65、判断是否为连续第二数量的帧间匹配ORB特征数大于第二阈值；若是，则执行步骤S66；否则保持当前第二阈值不变；

具体而言，判断是否为连续第n次的帧间匹配ORB特征数大于第二阈值，第二数量n>第一数量m且为第一数量m的倍数；若是，则执行步骤S66；否则保持当前第二阈值不变，即保持当前初始阈值不变；

S66、计算当前帧之前的第二数量个帧的特征匹配率均值，判断当前帧的特征匹配率是否大于计算的特征匹配率均值；若是，则调整匹配ORB特征数的第二阈值；否则保持当前第二阈值不变。

具体而言，计算当前帧之前的n个帧的特征匹配率均值

判断当前帧的特征匹配率是否大于计算的特征匹配率均值

若是，则调整匹配ORB特征数的第二阈值，即将当前初始阈值调整为新的阈值Γ_new；否则保持当前第二阈值不变。

新的阈值Γ_new的调节公式为：

其中，Γ_new表示调整后的阈值，Υ_i表示前n个帧中第i帧包含的匹配特征数。

S7、查询SLAM跟踪线程状态，判断当前SLAM跟踪线程是否丢失；若是，则将移动机器人回退并执行步骤S8；否则调整移动机器人行进速度继续行进；

在本实施例中，步骤S7具体包括以下分步骤：

S71、查询SLAM跟踪线程状态，判断当前SLAM跟踪线程是否丢失；若是，则将移动机器人停止前进，并执行步骤S72；否则执行步骤S73；

S72、从关键帧数据库读取上一个关键帧位姿，计算当前帧位姿和上一关键帧位姿之间的位姿变换矩阵，将移动机器人回退至上一个关键帧位姿，并执行步骤S8；

具体而言，本发明通过计算位姿变换矩阵，得到移动机器人从上一关键帧到当前帧处的平移向量和旋转矩阵。假设上一关键帧处的位姿为A₁＝[a₁ 1]^T，当前帧的位姿为A₂＝[a₂ 1]^T，则从上一关键帧到当前帧的位姿变换为：

根据计算得到的是正向变换矩阵，而现在需要从A₂处倒退回A₁处，则需要对变换矩阵求逆：

从T^-1变换矩阵中就可得到平移向量和旋转矩阵，将移动机器人慢速退回上一关键帧处。

S73、判断是否为连续第一数量的帧间匹配ORB特征数小于第一阈值；若是，则执行步骤S74；否则保持当前行进速度继续行进；

具体而言，没有丢失对当前环境的跟踪时，判断是否为连续第m次的帧间匹配ORB特征数小于第一阈值；若是，则执行步骤S74；否则将移动机器人保持当前行进速度继续匀速行进；

S74、调整移动机器人行进速度，以调整后的行进速度继续匀速行进。

此外，步骤S74后还包括：

S75、判断是否为连续第二数量的帧间匹配ORB特征数小于第一阈值；若是，则执行步骤S76；否则保持当前第一阈值不变；

具体而言，判断是否为连续第n次的帧间匹配ORB特征数小于第一阈值；若是，则执行步骤S66；否则保持当前第一阈值不变，即保持当前初始阈值不变；

S76、计算当前帧之前的第二数量个帧的特征匹配率均值，判断当前帧的特征匹配率是否大于计算的特征匹配率均值；若是，则调整匹配ORB特征数的第一阈值；否则保持当前第一阈值不变。

具体而言，计算当前帧之前的n个帧的特征匹配率均值

判断当前帧的特征匹配率是否大于计算的特征匹配率均值

若是，则调整匹配ORB特征数的第一阈值，即将当前初始阈值调整为新的阈值Γ_new；否则保持当前第一阈值不变。

S8、对移动机器人进行重定位；若重定位成功，则重置移动机器人行进速度继续行进；否则移动机器人停止行进。

在本实施例中，步骤S8具体包括以下分步骤：

S81、在回退过程中进行重定位并记录重定位次数，实时判断重定位是否成功；若是，则停止回退，并将移动机器人的行进速度和匹配ORB特征数的最小阈值进行重置，然后以预设初始速度匀速行进；否则执行步骤S82；

S82、若回退至上一个关键帧位姿后依然没有重定位成功，则在该关键帧处停止回退，并以移动机器人正前方作为主方向，在主方向设定偏航角范围内旋转移动机器人，对移动机器人进行辅助重定位；具体而言，设定偏航角为正负30°范围内。

S83、判断辅助重定位是否成功；若是，则停止回退，并将移动机器人的行进速度和匹配ORB特征数的最小阈值进行重置，然后以预设初始速度匀速行进；否则从关键帧数据库读取上一个关键帧位姿，计算当前帧位姿和上一关键帧位姿之间的位姿变换矩阵，将移动机器人回退至上一个关键帧位姿；

S84、判断重定位次数是否达到设定次数阈值；若是，则移动机器人停止行进，并输出跟踪丢失；否则返回步骤S81。

具体而言，在移动机器人回退过程中再次进行重定位，判断重定位次数是否达到设定次数阈值，优选地设定为3次；若3次后依然无法重定位成功，则移动机器人停止行进，并输出跟踪丢失；否则返回步骤S81。

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

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

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

本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (5)

1.一种基于特征分布匹配的智能设备自适应运动控制方法，其特征在于，包括以下步骤：

S1、对移动机器人进行初始化；

S2、对采集的初始图像进行预处理后提取初始图像的ORB特征，并获取初始位姿；

S3、启动移动机器人以预设初始速度匀速行进，提取行进过程中实时图像的ORB特征并计算特征分布；

S4、启动SLAM跟踪线程，采用基于特征分布的方法进行帧间ORB特征匹配，记录匹配的ORB特征数量和特征匹配率；其中基于特征分布的帧间ORB特征匹配的方法具体包括：

S41、获取两幅相邻帧图像的所有特征点；

S42、根据第一图像的中心关键点以及计算得到的中心关键区，在第二图像的中心关键区内搜索第一图像的中心关键点的匹配特征点；若搜索到匹配特征点，则执行步骤S43；否则执行步骤S45；

S43、以搜索到的匹配特征点的坐标，计算在第二图像中进行全图搜索匹配时解算匹配特征点相对位置的起点；

S44、遍历第二图像中所有的可能出现匹配特征点的位置，并搜索该位置设定区域范围内的匹配特征点，保存搜索到的所有匹配特征点；

S45、扩大第二图像的中心关键区的搜索范围，在扩大后的区域内搜索第一图像的中心关键点的匹配特征点；若搜索到匹配特征点，则执行步骤S43；否则采用暴力匹配方式进行帧间ORB特征匹配；

所述特征匹配率的计算公式为：

其中，ρ表示特征匹配率，Υ_n表示第n帧提取的特征数，Υ_n-1表示第n-1帧提取的特征数，Υ_n∩Υ_n-1表示第n帧和第n-1帧之间匹配的ORB特征数；

S5、获取移动机器人当前行进速度，判断移动机器人是否处于正常行进状态；若是，则保持当前行进速度继续行进；否则执行步骤S6；

S6、根据匹配ORB特征数判断SLAM跟踪是否失败；若是，则执行步骤S7；否则调整移动机器人行进速度继续行进，具体包括以下分步骤：

S61、判断当前帧与上一帧匹配ORB特征数是否小于第一阈值；若是，则执行步骤S7；否则执行步骤S62；

S62、判断当前帧与上一帧匹配ORB特征数是否大于第二阈值；若是，则执行步骤S63；否则保持当前行进速度继续行进；

S63、判断是否为连续第一数量的帧间匹配ORB特征数大于第二阈值；若是，则执行步骤S64；否则保持当前行进速度继续行进；

S64、调整移动机器人行进速度，以调整后的行进速度继续行进；

S65、判断是否为连续第二数量的帧间匹配ORB特征数大于第二阈值；若是，则执行步骤S66；否则保持当前第二阈值不变；

S66、计算当前帧之前的第二数量个帧的特征匹配率均值，判断当前帧的特征匹配率是否大于计算的特征匹配率均值；若是，则调整匹配ORB特征数的第二阈值；否则保持当前第二阈值不变；

所述调整移动机器人行进速度的调节公式为：

V_new＝a+b*log₁₀Υ

其中，V_new表示调整后的移动机器人行进速度，a、b为常数，Υ表示当前帧与上一帧之间匹配的ORB特征数；

S7、查询SLAM跟踪线程状态，判断当前SLAM跟踪线程是否丢失；若是，则将移动机器人回退并执行步骤S8；否则调整移动机器人行进速度继续行进；

S8、对移动机器人进行重定位；若重定位成功，则重置移动机器人行进速度继续行进；否则移动机器人停止行进，具体包括以下分步骤：

S81、在回退过程中进行重定位并记录重定位次数，实时判断重定位是否成功；若是，则停止回退，并将移动机器人的行进速度和匹配ORB特征数的最小阈值进行重置，然后以预设初始速度匀速行进；否则执行步骤S82；

S82、若回退至上一个关键帧位姿后依然没有重定位成功，则在该关键帧处停止回退，并以移动机器人正前方作为主方向，在主方向设定偏航角范围内旋转移动机器人，对移动机器人进行辅助重定位；

S83、判断辅助重定位是否成功；若是，则停止回退，并将移动机器人的行进速度和匹配ORB特征数的最小阈值进行重置，然后以预设初始速度匀速行进；否则从关键帧数据库读取上一个关键帧位姿，计算当前帧位姿和上一关键帧位姿之间的位姿变换矩阵，将移动机器人回退至上一个关键帧位姿；

S84、判断重定位次数是否达到设定次数阈值；若是，则移动机器人停止行进，并输出跟踪丢失；否则返回步骤S81。

2.根据权利要求1所述的基于特征分布匹配的智能设备自适应运动控制方法，其特征在于，所述步骤S2和S3中ORB特征的提取方法具体包括：

对图像进行灰度化预处理，并构建图像金字塔；

将图像金字塔按层次分割；

对每一层分割后的图像部分并行提取FAST关键点；

对提取的FAST关键点进行筛选，得到中心区域关键点；

根据中心区域关键点与图像中心像素的距离查找中心关键点；

根据查找的中心关键点计算中心关键区；

计算图像中每个中心关键点与距离图像中心像素最近的中心关键点的相对位置；

计算图像中每个FAST关键点与距离图像中心像素最近的中心关键点的相对位置；

计算图像中每个FAST关键点的主方向；

计算图像中每个FAST关键点的描述子，得到图像的ORB特征。

3.根据权利要求1所述的基于特征分布匹配的智能设备自适应运动控制方法，其特征在于，所述步骤S5具体包括：

获取移动机器人当前行进速度；

判断移动机器人当前行进速度是否达到预设的最大限定速度或最小限定速度；

若移动机器人当前行进速度达到最大限定速度，且当前帧中包含的匹配ORB特征数大于最小阈值，则保持当前行进速度继续行进；

若移动机器人当前行进速度达到最小限定速度，且当前帧中包含的匹配ORB特征数等于最小阈值，则保持当前行进速度继续行进；

否则，执行步骤S6。

4.根据权利要求1所述的基于特征分布匹配的智能设备自适应运动控制方法，其特征在于，所述步骤S7具体包括以下分步骤：

S71、查询SLAM跟踪线程状态，判断当前SLAM跟踪线程是否丢失；若是，则将移动机器人停止前进，并执行步骤S72；否则执行步骤S73；

S72、从关键帧数据库读取上一个关键帧位姿，计算当前帧位姿和上一关键帧位姿之间的位姿变换矩阵，将移动机器人回退至上一个关键帧位姿，并执行步骤S8；

S73、判断是否为连续第一数量的帧间匹配ORB特征数小于第一阈值；若是，则执行步骤S74；否则保持当前行进速度继续行进；

S74、调整移动机器人行进速度，以调整后的行进速度继续行进；

S75、判断是否为连续第二数量的帧间匹配ORB特征数小于第一阈值；若是，则执行步骤S76；否则保持当前第一阈值不变；

S76、计算当前帧之前的第二数量个帧的特征匹配率均值，判断当前帧的特征匹配率是否大于计算的特征匹配率均值；若是，则调整匹配ORB特征数的第一阈值；否则保持当前第一阈值不变。

5.根据权利要求1或4所述的基于特征分布匹配的智能设备自适应运动控制方法，其特征在于，所述匹配ORB特征数的第一阈值和第二阈值的调节公式为：

其中，Γ_new表示调整后的阈值，Υ_i表示前n个帧中第i帧包含的匹配特征数。

Images