一种移动机器人室内定位方法

技术领域

本发明涉及视觉定位技术领域，具体涉及一种移动机器人室内定位方法。

背景技术

传统GPS（Global Positioning System，全球定位系统）以及北斗导航等定位技术是通过卫星信号传递确定机器人位姿，而SLAM（Simultaneous Localization AndMapping，即时定位与地图构建）可以在某些接受不到卫星信号的特定场景下，通过机器人自身装配的传感器进行位姿估计和地图创建。

在传统的纯视觉单目SLAM中，由于传感器本身缺少尺度信息，存在无法准确测量初始位姿等局限性，导致随着时间推移，定位误差逐渐增大。在VINS(Visual InertialNavigation Systems，视觉惯性导航系统)中，将单目视觉传感器信息和IMU（Inertialmeasurement unit，惯性测量单元）测量的信息进行融合，移动机器人可以更好地解决单目视觉传感器深度缺失的问题，能够在动态场景下完成定位与建图任务。但在真实环境下，传统的VINS在实验过程中，可能会因为周围白墙太多，存在单目相机无法提取到有效特征点或者移动机器人遇到转弯时，因为帧与帧图像信息变化过大而无法提取有效特征点等问题，从而导致在运行过程中出现定位失败，最终发生运动轨迹漂移。

发明内容

本发明的目的在于提供一种移动机器人室内定位方法，解决了现有技术在定位过程中发生运动轨迹漂移的问题。

本发明通过下述技术方案实现：

一种移动机器人室内定位方法，包括：

当移动机器人在室内移动时，通过移动机器人搭载的单目相机按第一预设频率采集室内环境图像以及通过移动机器人内置的惯性传感器按第二预设频率采集IMU信息；

对室内环境图像进行特征点提取，对提取的特征点进行正反向流光匹配，得到视觉位姿数据；

对IMU信息进行预积分位姿估计，并将预积分位姿估计得到惯性位姿数据与视觉位姿数据进行紧耦合，得到视觉惯性位姿数据；

采用滑动窗口策略对视觉惯性位姿数据进行关键帧优化，并根据优化后的视觉惯性位姿数据进行稀疏建图，获取机器人移动轨迹图像；

对移动机器人的移动进行回环检测，获取回环检测结果，并根据所述回环检测结果对机器人移动轨迹图像进行全局优化，得到移动机器人的室内移动轨迹，完成移动机器人室内定位。

在一种可能的实施方式中，对室内环境图像进行特征点提取，包括：

针对任一帧室内环境图像，选取室内环境图像中的任一像素点

以所述像素点

以选取的16个像素点为基础，将像素点

遍历室内环境图像中的每个像素点，得到室内环境图像的特征点。

在一种可能的实施方式中，针对任一个像素点

以所述像素点

；

其中，

判断像素点

在一种可能的实施方式中，对提取的特征点进行正反向流光匹配，得到视觉位姿数据，包括：

对相邻两帧室内环境图像中特征点进行正向匹配光流处理，得到正向匹配光流结果；

在正向匹配光流结果上进行反向匹配光流处理，得到反向匹配光流结果；

根据所述反向匹配光流结果对正向匹配光流结果对比，反向光流计算后将特征点的正向光流匹配进行抵消，从而使反向光流矢量表征特征点的视觉位姿信息。

在一种可能的实施方式中，所述正向匹配光流处理，包括：

设第

I

其中，（

将上述变化关系左边进行泰勒展开保留一阶项后，根据灰度不变特性，获取特征点在平面上的位移为：

；

其中，

根据高斯牛顿法，获取特征点在图像间的轴方向速度

；

其中，雅可比矩阵

根据像素点的运动速度(

在一种可能的实施方式中，所述反向匹配光流处理，包括：

从第

-(

其中，(

采用逆变换

在一种可能的实施方式中，对IMU信息进行预积分位姿估计，包括：

获取离散时间下

；

其中，

根据离散时间下

在一种可能的实施方式中，所述

；

其中，

在一种可能的实施方式中，将预积分位姿估计得到惯性位姿数据与视觉位姿数据进行紧耦合，得到视觉惯性位姿数据，包括：第一紧耦合策略以及第二紧耦合策略；

所述第一紧耦合策略包括：从IMU坐标系下将第K帧惯性位姿数据

所述第二紧耦合策略包括：从IMU坐标系下将第K帧惯性位姿数据

在一种可能的实施方式中，采用滑动窗口策略对视觉惯性位姿数据进行关键帧优化，包括：第一滑动窗口策略以及第二滑动窗口策略；

第一滑动窗口策略包括：若当前第

其中，若最老的图像帧

第二滑动窗口策略包括：若当前图像帧不是新关键帧，但滑动窗口内能容纳的图像帧已满，则丢弃滑窗中上一图像帧

本发明提供的一种移动机器人室内定位方法，通过对室内环境图像进行特征点提取，对提取的特征点进行正反向流光匹配，能够准确地得到视觉位姿数据，然后采用IMU预积分策略获取惯性位姿数据，将视觉位姿数据与惯性位姿数据进行紧耦合之后，便能够准确地实现机器人的室内定位，在室内的真实场景中能够更进一步提高动态场景特征提取的准确性，且相比于现有技术，拥有更好地移植性以及鲁棒性。

附图说明

为了更清楚地说明本发明示例性实施方式的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本发明实施例提供的前端模块的示意图。

图2为本发明实施例提供的初始化模块的示意图。

图3为本发明实施例提供的后端模块的示意图。

图4为本发明实施例提供的一种移动机器人室内定位方法的流程图。

图5为本发明实施例提供的正反向匹配光流法示意图。

图6为本发明实施例提供的IMU预积分约束示意图。

图7为本发明实施例提供的视觉与惯性初始化示意图。

图8为本发明实施例提供的滑动窗口示意图。

图9（a）为本发明实施例提供的试验数据为MH-04序列时，本发明算法所得轨迹与真实轨迹对比图。

图9（b）为本发明实施例提供的试验数据为MH-04序列时，现有开源算法轨迹与真实轨迹对比图。

图9（c）为本发明实施例提供的试验数据为MH-04序列时，本发明算法所得轨迹与真实轨迹的平移误差对比图。

图9（d）为本发明实施例提供的试验数据为MH-04序列时，现有开源算法轨迹与真实轨迹的平移误差对比图。

图10（a）为本发明实施例提供的试验数据为MH-03序列时，本发明算法所得轨迹与真实轨迹对比图。

图10（b）为本发明实施例提供的试验数据为MH-03序列时，现有开源算法轨迹与真实轨迹对比图。

图10（c）为本发明实施例提供的试验数据为MH-03序列时，本发明算法所得轨迹与真实轨迹的平移误差对比图。

图10（d）为本发明实施例提供的试验数据为MH-03序列时，现有开源算法轨迹与真实轨迹的平移误差对比图。

图11（a）为本发明实施例提供的一种移动机器人室内定位方法在室内的建图轨迹。

图11（b）为本发明实施例提供的现有开源算法在室内的建图轨迹。

其中，现有开源算法指VINS-Fusion算法。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。

为了便于理解本发明的实施例，首先结合图1至图3对本发明实施例所适用的系统架构进行介绍。

如图1至图3共同所示，本发明实施例所适用的系统架构包括前端模块、初始化模块以及后端模块，为了解决移动机器人在室内场景下定位的准确精度较低、系统兼容性较差以及鲁棒性较差的问题，该系统架构可以基于VINS-fusion框架构建，单目相机获取图像信息后，引入FAST-9算法对图像中特征点快速提取后，将提取的特征点进行正反向匹配光流，更好地解决原系统框架不具备的旋转不变性和尺度不变性。再将视觉匹配的位姿信息与IMU预积分测算信息对齐，前端模块的输出作为后端的输入，采用滑动窗口非线性优化模型对关键帧进行判定，减轻后端的运算量。在与开源系统VINS-fusion通过数据集比较中，该系统架构的定位精度有较大提升，特征点提取与匹配改进后，能应对更复杂的情况，在室内的真实场景中能够更进一步提高动态场景特征提取的准确性，最终完成移动机器人建图任务。

如图1所示，前端模块通过携带移动机器人上的单目相机收集到图像信息后，间接测算机器人的位姿数据，最终实现在未知环境中进行定位。在图像信息中需要对特征点进行提取，并根据特征点估计相机运动。本发明提出一种基于FAST-9算法，但不采用原算法中BRIEF描述图像特征点周围信息，而是通过正反向匹配光流跟踪，目的是更好地解决单目视觉在现实场景中定位准确性较低问题。考虑到单目相机缺乏深度信息，在定位和建图过程中无法还原现实场景中三维结构，限制了机器人对环境的感知力。IMU可以提供加速度和角速度信息，通过连续测量数据，运用预积分可以转化为相机的姿态、速度和位置估计，进而改善单目视觉中SLAM系统的定位性能。

由于单目相机本身无法直接测量尺度信息，需要通过本质矩阵计算出前两帧图像之间的相对尺度，这个尺度可以用来将单目相机的位姿估计结果进行尺度校正，以获得更精确的相对尺度信息，再利用PnP算法求解出2D-3D点对匹配，估计相机的初始位姿，这个过程需要将前一帧图像中的特征点投影到3D空间中，然后与当前帧空间中的特征点进行匹配，完成三角化测量，最终实现对场景中特征点三维重建和深度信息获取。

如图2所示，在实际应用场景中，为了更准确地室内定位，初始化模块可以利用IMU与单目相机具有互补性的特点，即视觉能提供更丰富的信息测算出物体的位姿，IMU预积分后可以获得高频运动下，两帧之间的位姿测量数据，在非性优化后，可以提供较准确的位姿估计，最后将视觉初始化信息和IMU预积分进行对齐。

如图3所示，将前端模块的输出信息作为后端的输入信息，采用基于滑动窗口的优化策略对视觉单目与惯性融合的数据进行非线性优化，建立残差模型，最后通过回环检测使整个SLAM系统精度得到进一步提升。

如图4所示，本发明实施例提供一种移动机器人室内定位方法，包括：

S101、当移动机器人在室内移动时，通过移动机器人搭载的单目相机按第一预设频率采集室内环境图像以及通过移动机器人内置的惯性传感器按第二预设频率采集IMU信息；

S102、对室内环境图像进行特征点提取，对提取的特征点进行正反向流光匹配，得到视觉位姿数据；

S103、对IMU信息进行预积分位姿估计，并将预积分位姿估计得到惯性位姿数据与视觉位姿数据进行紧耦合，得到视觉惯性位姿数据；

S104、采用滑动窗口策略对视觉惯性位姿数据进行关键帧优化，并根据优化后的视觉惯性位姿数据进行稀疏建图，获取机器人移动轨迹图像；

S105、对移动机器人的移动进行回环检测，获取回环检测结果，并根据所述回环检测结果对机器人移动轨迹图像进行全局优化，得到移动机器人的室内移动轨迹，完成移动机器人室内定位。

本发明提供的一种移动机器人室内定位方法，通过对室内环境图像进行特征点提取，对提取的特征点进行正反向流光匹配，能够准确地得到视觉位姿数据，然后采用IMU预积分策略获取惯性位姿数据，将视觉位姿数据与惯性位姿数据进行紧耦合之后，便能够准确地实现机器人的室内定位，在室内的真实场景中能够更进一步提高动态场景特征提取的准确性，且相比于现有技术，采用FAST-9即提取像素周围第1、6、11以及16标号进行比较，且判断条件为若周围连续有9个像素点亮度大于或小于提取像素点则认定为特征点，反向光流匹配是直接计算两帧间的变换增量，通过逆变换对正向光流进行有效抵消，通过实验验证，本发明能更精准地实现相邻帧间匹配，拥有更好地移植性以及鲁棒性。

本发明实施例采用FAST-9算法加快特征点提取速度。在进行特征点匹配时，不进行BRIEF描述子匹配，因为该算法对噪声敏感，鲁棒性较差且不具备旋转不变性和尺度不变性，采用正反向匹配光流法，从而提高匹配精度和准确性。

在一种可能的实施方式中，对室内环境图像进行特征点提取，包括：

针对任一帧室内环境图像，选取室内环境图像中的任一像素点

以所述像素点

以选取的16个像素点为基础，将像素点

遍历室内环境图像中的每个像素点，得到室内环境图像的特征点。

在一种可能的实施方式中，针对任一个像素点

以所述像素点

；

其中，

判断像素点

在相邻两关键帧之间，通过FAST-9提取到特征点后，利用稀疏光流法对特征点的匹配进行跟踪，得到不同帧间对应特征点位置。

在一种可能的实施方式中，对提取的特征点进行正反向流光匹配，得到视觉位姿数据，包括：

对相邻两帧室内环境图像中特征点进行正向匹配光流处理，得到正向匹配光流结果。

在正向匹配光流结果上进行反向匹配光流处理，得到反向匹配光流结果。

根据所述反向匹配光流结果对正向匹配光流结果对比，反向光流计算后将特征点的正向光流匹配进行抵消，从而使反向光流矢量表征特征点的视觉位姿信息。提高特征点匹配稳定性，有效去除了光流法中特征点错误跟踪和单目相机抖动干扰。在得到相邻帧间特征点的位移后，建立特征点的运动轨迹。

在一种可能的实施方式中，所述正向匹配光流处理，包括：

设第

I

其中，（

将上述变化关系左边进行泰勒展开保留一阶项后，根据灰度不变特性，获取特征点在平面上的位移为：

；

其中，

根据高斯牛顿法，获取特征点在图像间的轴方向速度

；

其中，雅可比矩阵

根据像素点的运动速度(

为了估算图像相邻两帧之间特征点匹配，需要进行非线性优化，如果所有特征点都重新计算匹配，这会增加计算量，影响该进程的效率。考虑到图像帧与帧之间的特征点匹配光流是可逆的，本发明提出一种反向光流匹配法，在非线性优化迭代过程中，只需进行一次雅可比矩阵计算就能完成特征点的光流匹配。

在一种可能的实施方式中，所述反向匹配光流处理，包括：

从第

-(

其中，(

采用逆变换对正向匹配光流进行验证，得到反向匹配光流结果。

每次迭代中，相邻帧间反向匹配光流的雅可比矩阵都相同，因此采用逆变换

如图5所示，设方框内分别为

如图6所示，IMU通过测得线加速度和陀螺仪角速度，积分后可获得两帧之间的旋转和位移变换。但在SLAM中，随着时间的推移，通过传感器获取到的信息，需要求解机器人的位姿，如果将两帧之间的IMU测量值都进行计算和评估，会造成非常大的计算量且耗时较长。为了避免不断迭代这些状态量，采用IMU预积分策略，其主要思路是在每次迭代中把不变量提取出来，减小每次重新积分的计算量。

连续时间下，IMU积分误差项导数只与偏置有关与其他量无关，可以表示为：

；

其中，

在一种可能的实施方式中，对IMU信息进行预积分位姿估计，包括：

获取离散时间下

；

其中，

根据离散时间下

在一种可能的实施方式中，所述

；

其中，

如图7所示，首先应建立不同坐标系之间的联系，计算出IMU坐标系到相机坐标系对应关键帧的旋转参数，再利用旋转参数分别计算出陀螺仪偏差、重力方向、速度以及尺度的初始值。

在一种可能的实施方式中，将预积分位姿估计得到惯性位姿数据与视觉位姿数据进行紧耦合，得到视觉惯性位姿数据，包括：第一紧耦合策略以及第二紧耦合策略；

所述第一紧耦合策略包括：从IMU坐标系下将第1帧惯性位姿数据

所述第二紧耦合策略包括：从IMU坐标系下将第1帧惯性位姿数据

数学表达式为：

；/>

其中，

将上式改写为四元数形式为：

；

其中，

若将

；

其中，

权重函数主要通过下式得到：

；

其中，G为工程实践中设定的一个阈值。

实际在采集数据中不可避免地产生误差，在此用残差

；

其中，

本发明采用SVD分解法，其最小奇异值对应的向量即为旋转外参

后端模块采用滑动窗口策略进行非线性优化。随着前端输出数据量的增加，后端输入的数据量也会增加。本发明系统需要判断确定新关键帧以及怎样去除旧关键帧是滑动窗口解决的关键问题。

如图8所示，采用滑动窗口策略对视觉惯性位姿数据进行关键帧优化，包括：第一滑动窗口策略以及第二滑动窗口策略。

第一滑动窗口策略包括：若当前第

其中，若最老的图像帧

第二滑动窗口策略包括：若当前图像帧不是新关键帧，但滑动窗口内能容纳的图像帧已满，则丢弃滑窗中上一图像帧

本发明改进后的系统框架主要和现今比较流行的开源算法框架VINS-Fusion进行比较，主要分为数据集实验比较和现实场景中定位建图实验比较。

在数据集实验中，主要选用开源数据集EuRoc里MH-04序列和MH-03序列，具体实验结果如图9以及图10所示。其中图9（a）和图9（b）是本发明算法在MH-04序列中移动机器人运行的轨迹和真实轨迹的比较，开源算法指VINS-Fusion算法运行轨迹，图中虚线表示用动捕设备测算的真实轨迹，图10（a）和图10（b）是在MH-03序列中运行轨迹示意图，综合两个序列的数据集运行轨迹，可以看出经本发明改进后的运动轨迹更贴近真实轨迹，相较于开源算法的准确性更高。

图9（c）和图9（d）和图10（c）和图10（d）分别进一步比较了在MH-04和MH-03序列中，机器人运动时在x、y、z轴的平移误差，在不同轴向的精度上都优于VINS-fusion。

因为周围条件复杂，如：来往行人较多，照明灯是感应式的等，可能造成单目相机捕捉到的光环境不稳定导致提取的特征点不够。用VINS-fusion进行实验时会发生在转角处光强度变化过大，周围白墙导致了机器人定位失败，在Rviz中表现为机器人发生漂移，而本发明的算法因为对前端模块进行改进，提高了机器人在真实场景下的实时性和定位的准确性，室内运行过程和结果如图11（a）和图11（b）所示。

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

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

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

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

本领域普通技术人员可以理解实现上述事实和方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，涉及的程序或者所述的程序可以存储于一计算机所可读取存储介质中，该程序在执行时，包括如下步骤：此时引出相应的方法步骤，所述的存储介质可以是ROM/RAM、磁碟、光盘等等。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。