一种基于视觉里程计的移动机器人定位方法与流程

本发明涉及移动机器人定位领域，具体涉及一种基于视觉里程计的移动机器人定位方法。

背景技术

随着移动机器人技术的不断发展，人工智能渗透进了各行各业，给人们的生活带来了诸多的方便。无论是在工业界还是学术界，具有移动能力，自主定位和导航的机器人得到了广泛关注和大力支持。智能机器人作为当代高科技产品，代表着科技和生产力水平，21世纪以来传感器技术和通信技术的发展更是为移动机器人的应用打下了坚实的基础。

作为移动机器人的核心模块，定位导航有着举足轻重的作用，精确的定位信息可以满足许多上层任务的需求，比如自动驾驶、路径规划等。提高移动机器人定位精度的就显得越发重要，具备高精度定位系统的移动机器人在众多领域具有许多人类无法企及的优势，能够在危险性高、任务复杂等环境下出色的完成任务。目前，对于室内机器人普遍采用惯性测量单元(inertialmeasurementunit,imu)和编码器组合定位导航，室外则采用全球定位系统(globalpositioningsystem,gps)进行定位。由于移动轮式机器人在自动行驶过程中会发生车轮打滑现象，容易对编码器产生干扰，造成误差累积并直接导致定位不准，车体处于静止状态下的时候也会有零点漂移等因素干扰，除此之外，gps必须运用在室外场景，而当建筑物遮挡或者信号不好时会造成数据丢失，造成定位失败。

技术实现要素：

针对现有技术中的上述不足，本发明提供的一种基于视觉里程计的移动机器人定位方法提供了一种移动机器人的高效定位方法。

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

提供一种基于视觉里程计的移动机器人定位方法，其包括以下步骤：

s1、采集移动机器人周围环境图像信息；

s2、通过光流法计算图像的先验运动估计，得到基于光流法的移动机器人的运动轨迹；

s3、将至少由前100帧图像得到的基于光流法的移动机器人运动轨迹进行三维地图化，得到局部地图；

s4、通过局部地图获取实时图像中的下一个关键帧，并获取该关键帧中的特征点；

s5、通过特征点获取对应图像的后验运动估计，得到基于特征点的移动机器人的运动轨迹；

s6、将基于光流法的移动机器人的运动轨迹和基于特征点的移动机器人的运动轨迹进行卡尔曼融合，得到该关键帧处对应的移动机器人的位置；

s7、判断是否继续定位，若是则进入步骤s8，否则结束定位；

s8、根据该关键帧更新局部地图，采用与步骤s2相同的方法持续获取基于光流法的移动机器人的运动轨迹并返回步骤s4。

进一步地，步骤s2的具体方法包括以下步骤：

s2-1、采用光流法获取每两帧图像之间的旋转矩阵ri和平移向量ti；

s2-2、将从每两帧图像之间得到的旋转矩阵ri和平移向量ti进行累计得到先验运动估计，即位姿偏移量uk-1＝[δx,δy,δθ]^t，其中δx为x方向的偏移量，δy为y方向的偏移量，δθ为行进方向的偏移量。

s2-3、根据离散卡尔曼滤波器的预测方程：

得到基于光流法的移动机器人的运动轨迹其中a是状态转移矩阵，为常数；b是输入增益，为常数；uk-1是位置输入函数，即位姿偏移量；wk和q分别是运动过程噪声和噪声协方差矩阵，为常数；pk-1是上一时刻的协方差矩阵；(·)^t为转置矩阵；为当前时刻的协方差矩阵估计；为当前的预测位置，即基于光流法得到的移动机器人位置；为上一时刻的预测位置。

进一步地，步骤s4的具体方法包括以下步骤：

s4-1、将局部地图中每一帧的特征点作为局部地图的特征点；

s4-2、以至少20帧为间隔帧数，将局部地图后第一个特征点数量少于局部地图的特征点数量60％的帧作为局部地图的下一个关键帧；

s4-3、判断该关键帧中是否有30％以上的特征点未匹配至局部地图中，若是则忽略该帧并进入步骤s4-2，否则进入步骤s5。

进一步地，步骤s5的具体方法为：

根据公式

zk＝hxk+vk

通过特征点匹配得到基于特征点的对应图像的运动轨迹zk，即基于特征点的移动机器人的运动轨迹；其中h为单位矩阵；vk为观测噪声；xk为移动机器人的位姿数据。

进一步地，步骤s6的具体方法包括以下步骤：

根据公式

将基于光流法的移动机器人的运动轨迹和基于特征点的移动机器人的运动轨迹进行卡尔曼融合，得到该关键帧对应的移动机器人的运动估计(pk)，即该关键帧对应的移动机器人位置；其中i为单位矩阵。

进一步地，步骤s8中根据该关键帧更新局部地图的具体方法包括以下步骤：

s8-1、获取局部地图中每个特征点出现的次数；

s8-2、将局部地图中的所有特征点投影到该局部地图的下一个关键帧上，将未在该局部地图的下一个关键帧上出现的现有特征点的出现次数减1，将在该局部地图的下一个关键帧上出现的现有特征点的出现次数加1；

s8-3、将该局部地图的下一个关键帧中未出现在当前局部地图中的特征点加入当前局部地图并将其出现次数记为1，且去掉出现次数小于0的特征点，完成该关键帧对局部地图的更新。

本发明的有益效果为：本发明通过将光流法和特征点匹配法进行结合，通过建立并不断更新局部地图来提高定位的准确性，本方法计算速度快、定位频率高，定位精度高，适用于小运动规模场景的移动机器人定位。

附图说明

图1为本发明的流程示意图；

图2为本定位方法、传统光流法与实际轨迹的对比图；

图3为传统光流法与本方法的误差对比图。

具体实施方式

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

如图1所示，该基于视觉里程计的移动机器人定位方法包括以下步骤：

s1、采集移动机器人周围环境图像信息；

s2、通过光流法计算图像的先验运动估计，得到基于光流法的移动机器人的运动轨迹；

s3、将至少由前100帧图像得到的基于光流法的移动机器人运动轨迹进行三维地图化，得到局部地图；

s4、通过局部地图获取实时图像中的下一个关键帧，并获取该关键帧中的特征点；

s5、通过特征点获取对应图像的后验运动估计，得到基于特征点的移动机器人的运动轨迹；

s6、将基于光流法的移动机器人的运动轨迹和基于特征点的移动机器人的运动轨迹进行卡尔曼融合，得到该关键帧对应的移动机器人的位置；

s7、判断是否继续定位，若是则进入步骤s8，否则结束定位；

s8、根据该关键帧更新局部地图，采用与步骤s2相同的方法持续获取基于光流法的移动机器人的运动轨迹并返回步骤s4。

步骤s2的具体方法包括以下步骤：

s2-1、采用光流法获取每两帧图像之间的旋转矩阵ri和平移向量ti；

s2-2、将从每两帧图像之间得到的旋转矩阵ri和平移向量ti进行累计得到先验运动估计，即位姿偏移量uk-1＝[δx,δy,δθ]^t，其中δx为x方向的偏移量，δy为y方向的偏移量，δθ为行进方向的偏移量；

s2-3、根据离散卡尔曼滤波器的预测方程：

得到基于光流法的移动机器人的运动轨迹其中a是状态转移矩阵，为常数；b是输入增益，为常数；uk-1是位置输入函数，即位姿偏移量；wk和q分别是运动过程噪声和噪声协方差矩阵，为常数；pk-1是上一时刻的协方差矩阵；(·)^t为转置矩阵；为当前时刻的协方差矩阵估计；为当前的预测位置，即基于光流法得到的移动机器人位置；为上一时刻的预测位置。

步骤s4的具体方法包括以下步骤：

s4-1、将局部地图中每一帧的特征点作为局部地图的特征点；

s4-2、以至少20帧为间隔帧数，将局部地图后第一个特征点数量少于局部地图的特征点数量60％的帧作为局部地图的下一个关键帧；

s4-3、判断该关键帧中是否有30％以上的特征点未匹配至局部地图中，若是则忽略该帧并进入步骤s4-2，否则进入步骤s5。

步骤s5的具体方法为：根据公式

zk＝hxk+vk

通过特征点匹配得到基于特征点的对应图像的运动轨迹zk，即基于特征点的移动机器人的运动轨迹；其中h为单位矩阵；vk为观测噪声；xk为移动机器人的位姿数据。

步骤s6的具体方法包括以下步骤：根据公式

将基于光流法的移动机器人的运动轨迹和基于特征点的移动机器人的运动轨迹进行卡尔曼融合，得到该关键帧对应的移动机器人的运动估计(pk)，即该关键帧对应的移动机器人位置；其中i为单位矩阵。

步骤s8中根据该关键帧更新局部地图的具体方法包括以下步骤：

s7-1、获取局部地图中每个特征点出现的次数；

s7-2、将局部地图中的所有特征点投影到该局部地图的下一个关键帧上，将未在该局部地图的下一个关键帧上出现的现有特征点的出现次数减1，将在该局部地图的下一个关键帧上出现的现有特征点的出现次数加1；

s7-3、将该局部地图的下一个关键帧中未出现在当前局部地图中的特征点加入当前局部地图并将其出现次数记为1，且去掉出现次数小于0的特征点，完成该关键帧对局部地图的更新。

在本发明的一个实施例中，如图2所示，当移动机器人刚开始运动时，局部地图初始化，直线行走时光流法与真实轨迹误差不大，与特征点匹配算法进行视觉融合使得位置估计更能接近于真实情况。而当机器人发生转弯，这时候光流法估计出的位姿信息与真实情况相差较大，如图2中的两个比较明显的拐点，光流法估计出的位置与真实位置最远相差0.15m，并且随着时间的推移，误差累积有变大的趋势，而本方法由于局部地图的存在，关键帧与关键帧之间的特征点匹配可以看做一个很强的运动估计约束，尽可能的消除了误差，将估计的位置重新靠近于真实轨迹，并作为初始值传递给下一时刻的算法融合阶段，避免误差累积。

融合后的结果比单纯的特征点匹配定位算法细节上更加丰富，能够提供更多的信息，特别是当纹理不明显的地方可以借助于光流跟踪法实现定位。而相应的比起单纯的光流法，由于本方法引进了特征点匹配算法定为修正，机器人转外角度估计更为准确，在光照强度变化较大时可借助特征点匹配辅助定位，提高了算法的鲁棒性，进而提高了移动机器人的定位精度。

由于本方法引入了关键帧，不需要对每一帧相机图像进行特征点匹配，留给特征点匹配和算法融合的时间较为充裕，因此可以为进一步的分析视觉融合算法带来可靠的精度。误差曲线是计算视觉里程计算法得到的移动机器人位置点和运动捕捉系统(真值)之间的欧氏距离，如图3所示，图中e为算法的绝对误差，f为采样帧号，可以看出直线行走时光流法在保证精度的条件下提供了快速定位的方法，而当发生转弯时，视觉里程计融合算法修正之后的定位精度误差要明显小于光流法造成的误差。

综上所述，本发明通过将光流法和特征点匹配法进行结合，通过建立并不断更新局部地图来提高定位的准确性，本方法计算速度快、定位频率高，定位精度高，适用于小运动规模场景的移动机器人定位。