CN109059930B - 一种基于视觉里程计的移动机器人定位方法 - Google Patents

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

Info

Publication number
CN109059930B
CN109059930B CN201811009213.5A CN201811009213A CN109059930B CN 109059930 B CN109059930 B CN 109059930B CN 201811009213 A CN201811009213 A CN 201811009213A CN 109059930 B CN109059930 B CN 109059930B
Authority
CN
China
Prior art keywords
mobile robot
local map
key frame
motion
optical flow
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811009213.5A
Other languages
English (en)
Other versions
CN109059930A (zh
Inventor
卢燚鑫
张一博
马磊
孙永奎
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southwest Jiaotong University
Original Assignee
Southwest Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Southwest Jiaotong University filed Critical Southwest Jiaotong University
Priority to CN201811009213.5A priority Critical patent/CN109059930B/zh
Publication of CN109059930A publication Critical patent/CN109059930A/zh
Application granted granted Critical
Publication of CN109059930B publication Critical patent/CN109059930B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Image Analysis (AREA)
  • Image Processing (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了一种基于视觉里程计的移动机器人定位方法，其包括以下步骤：S1、采集移动机器人周围环境图像信息；S2、获取基于光流法的移动机器人的运动轨迹；S3、建立局部地图；S4、通过局部地图获取实时图像中的下一个关键帧，并获取该关键帧中的特征点；S5、获取基于特征点的移动机器人的运动轨迹；S6、进行卡尔曼融合，得到移动机器人的位置；S7、判断是否继续定位，若是则进入步骤S8，否则结束定位；S8、根据该关键帧更新局部地图，采用与步骤S2相同的方法持续获取基于光流法的移动机器人的运动轨迹并返回步骤S4。本方法计算速度快、定位频率高，定位精度高，适用于小运动规模场景的移动机器人定位。

Description

一种基于视觉里程计的移动机器人定位方法
技术领域
本发明涉及移动机器人定位领域，具体涉及一种基于视觉里程计的移动机器人定位方法。
背景技术
随着移动机器人技术的不断发展，人工智能渗透进了各行各业，给人们的生活带来了诸多的方便。无论是在工业界还是学术界，具有移动能力，自主定位和导航的机器人得到了广泛关注和大力支持。智能机器人作为当代高科技产品，代表着科技和生产力水平，21世纪以来传感器技术和通信技术的发展更是为移动机器人的应用打下了坚实的基础。
作为移动机器人的核心模块，定位导航有着举足轻重的作用，精确的定位信息可以满足许多上层任务的需求，比如自动驾驶、路径规划等。提高移动机器人定位精度的就显得越发重要，具备高精度定位系统的移动机器人在众多领域具有许多人类无法企及的优势，能够在危险性高、任务复杂等环境下出色的完成任务。目前，对于室内机器人普遍采用惯性测量单元(Inertial Measurement Unit,IMU)和编码器组合定位导航，室外则采用全球定位系统(Global Positioning System,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、根据离散卡尔曼滤波器的预测方程：
Figure BDA0001784652850000031
得到基于光流法的移动机器人的运动轨迹
Figure BDA0001784652850000032
其中A是状态转移矩阵，为常数；B是输入增益，为常数；uk-1是位置输入函数，即位姿偏移量；wk和Q分别是运动过程噪声和噪声协方差矩阵，为常数；Pk-1是上一时刻的协方差矩阵；(·)T为转置矩阵；
Figure BDA0001784652850000033
为当前时刻的协方差矩阵估计；
Figure BDA0001784652850000034
为当前的预测位置，即基于光流法得到的移动机器人位置；
Figure BDA0001784652850000035
为上一时刻的预测位置。
进一步地，步骤S4的具体方法包括以下步骤：
S4-1、将局部地图中每一帧的特征点作为局部地图的特征点；
S4-2、以至少20帧为间隔帧数，将局部地图后第一个特征点数量少于局部地图的特征点数量60％的帧作为局部地图的下一个关键帧；
S4-3、判断该关键帧中是否有30％以上的特征点未匹配至局部地图中，若是则忽略该帧并进入步骤S4-2，否则进入步骤S5。
进一步地，步骤S5的具体方法为：
根据公式
zk＝Hxk+vk
通过特征点匹配得到基于特征点的对应图像的运动轨迹zk，即基于特征点的移动机器人的运动轨迹；其中H为单位矩阵；vk为观测噪声；xk为移动机器人的位姿数据。
进一步地，步骤S6的具体方法包括以下步骤：
根据公式
Figure BDA0001784652850000036
Figure BDA0001784652850000037
将基于光流法的移动机器人的运动轨迹和基于特征点的移动机器人的运动轨迹进行卡尔曼融合，得到该关键帧对应的移动机器人的运动估计(
Figure BDA0001784652850000041
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、根据离散卡尔曼滤波器的预测方程：
Figure BDA0001784652850000061
得到基于光流法的移动机器人的运动轨迹
Figure BDA0001784652850000062
其中A是状态转移矩阵，为常数；B是输入增益，为常数；uk-1是位置输入函数，即位姿偏移量；wk和Q分别是运动过程噪声和噪声协方差矩阵，为常数；Pk-1是上一时刻的协方差矩阵；(·)T为转置矩阵；
Figure BDA0001784652850000063
为当前时刻的协方差矩阵估计；
Figure BDA0001784652850000064
为当前的预测位置，即基于光流法得到的移动机器人位置；
Figure BDA0001784652850000065
为上一时刻的预测位置。
步骤S4的具体方法包括以下步骤：
S4-1、将局部地图中每一帧的特征点作为局部地图的特征点；
S4-2、以至少20帧为间隔帧数，将局部地图后第一个特征点数量少于局部地图的特征点数量60％的帧作为局部地图的下一个关键帧；
S4-3、判断该关键帧中是否有30％以上的特征点未匹配至局部地图中，若是则忽略该帧并进入步骤S4-2，否则进入步骤S5。
步骤S5的具体方法为：根据公式
zk＝Hxk+vk
通过特征点匹配得到基于特征点的对应图像的运动轨迹zk，即基于特征点的移动机器人的运动轨迹；其中H为单位矩阵；vk为观测噪声；xk为移动机器人的位姿数据。
步骤S6的具体方法包括以下步骤：根据公式
Figure BDA0001784652850000066
Figure BDA0001784652850000067
将基于光流法的移动机器人的运动轨迹和基于特征点的移动机器人的运动轨迹进行卡尔曼融合，得到该关键帧对应的移动机器人的运动估计(
Figure BDA0001784652850000068
Pk)，即该关键帧对应的移动机器人位置；其中I为单位矩阵。
步骤S8中根据该关键帧更新局部地图的具体方法包括以下步骤：
S7-1、获取局部地图中每个特征点出现的次数；
S7-2、将局部地图中的所有特征点投影到该局部地图的下一个关键帧上，将未在该局部地图的下一个关键帧上出现的现有特征点的出现次数减1，将在该局部地图的下一个关键帧上出现的现有特征点的出现次数加1；
S7-3、将该局部地图的下一个关键帧中未出现在当前局部地图中的特征点加入当前局部地图并将其出现次数记为1，且去掉出现次数小于0的特征点，完成该关键帧对局部地图的更新。
在本发明的一个实施例中，如图2所示，当移动机器人刚开始运动时，局部地图初始化，直线行走时光流法与真实轨迹误差不大，与特征点匹配算法进行视觉融合使得位置估计更能接近于真实情况。而当机器人发生转弯，这时候光流法估计出的位姿信息与真实情况相差较大，如图2中的两个比较明显的拐点，光流法估计出的位置与真实位置最远相差0.15m，并且随着时间的推移，误差累积有变大的趋势，而本方法由于局部地图的存在，关键帧与关键帧之间的特征点匹配可以看做一个很强的运动估计约束，尽可能的消除了误差，将估计的位置重新靠近于真实轨迹，并作为初始值传递给下一时刻的算法融合阶段，避免误差累积。
融合后的结果比单纯的特征点匹配定位算法细节上更加丰富，能够提供更多的信息，特别是当纹理不明显的地方可以借助于光流跟踪法实现定位。而相应的比起单纯的光流法，由于本方法引进了特征点匹配算法定为修正，机器人转外角度估计更为准确，在光照强度变化较大时可借助特征点匹配辅助定位，提高了算法的鲁棒性，进而提高了移动机器人的定位精度。
由于本方法引入了关键帧，不需要对每一帧相机图像进行特征点匹配，留给特征点匹配和算法融合的时间较为充裕，因此可以为进一步的分析视觉融合算法带来可靠的精度。误差曲线是计算视觉里程计算法得到的移动机器人位置点和运动捕捉系统(真值)之间的欧氏距离，如图3所示，图中e为算法的绝对误差，F为采样帧号，可以看出直线行走时光流法在保证精度的条件下提供了快速定位的方法，而当发生转弯时，视觉里程计融合算法修正之后的定位精度误差要明显小于光流法造成的误差。
综上所述，本发明通过将光流法和特征点匹配法进行结合，通过建立并不断更新局部地图来提高定位的准确性，本方法计算速度快、定位频率高，定位精度高，适用于小运动规模场景的移动机器人定位。

Claims (4)

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、根据离散卡尔曼滤波器的预测方程：
Figure FDA0002703143410000011
得到基于光流法的移动机器人的运动轨迹
Figure FDA0002703143410000021
其中A是状态转移矩阵，为常数；B是输入增益，为常数；uk-1是位置输入函数，即位姿偏移量；wk和Q分别是运动过程噪声和噪声协方差矩阵，为常数；Pk-1是上一时刻的协方差矩阵；(·)T为转置矩阵；
Figure FDA0002703143410000022
为当前时刻的协方差矩阵估计；
Figure FDA0002703143410000023
为当前的预测位置，即基于光流法得到的移动机器人位置；
Figure FDA0002703143410000024
为上一时刻的预测位置；
所述步骤S4的具体方法包括以下步骤：
S4-1、将局部地图中每一帧的特征点作为局部地图的特征点；
S4-2、以至少20帧为间隔帧数，将局部地图后第一个特征点数量少于局部地图的特征点数量60％的帧作为局部地图的下一个关键帧；
S4-3、判断该关键帧中是否有30％以上的特征点未匹配至局部地图中，若是则忽略该关键帧并进入步骤S4-2，否则进入步骤S5。
2.根据权利要求1所述的基于视觉里程计的移动机器人定位方法，其特征在于：所述步骤S5的具体方法为：
根据公式
zk＝Hxk+vk
通过特征点匹配得到基于特征点的对应图像的运动轨迹zk，即基于特征点的移动机器人的运动轨迹；其中H为单位矩阵；vk为观测噪声；xk为移动机器人的位姿数据。
3.根据权利要求2所述的基于视觉里程计的移动机器人定位方法，其特征在于：所述步骤S6的具体方法包括以下步骤：
根据公式
Figure FDA0002703143410000025
Figure FDA0002703143410000031
将基于光流法的移动机器人的运动轨迹和基于特征点的移动机器人的运动轨迹进行卡尔曼融合，得到该关键帧对应的移动机器人的运动估计
Figure FDA0002703143410000032
即该关键帧对应的移动机器人位置；其中I为单位矩阵。
4.根据权利要求3所述的基于视觉里程计的移动机器人定位方法，其特征在于：所述步骤S8中根据该关键帧更新局部地图的具体方法包括以下步骤：
S8-1、获取局部地图中每个特征点出现的次数；
S8-2、将局部地图中的所有特征点投影到该局部地图的下一个关键帧上，将未在该局部地图的下一个关键帧上出现的现有特征点的出现次数减1，将在该局部地图的下一个关键帧上出现的现有特征点的出现次数加1；
S8-3、将该局部地图的下一个关键帧中未出现在当前局部地图中的特征点加入当前局部地图并将其出现次数记为1，且去掉出现次数小于0的特征点，完成该关键帧对局部地图的更新。
CN201811009213.5A 2018-08-31 2018-08-31 一种基于视觉里程计的移动机器人定位方法 Active CN109059930B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811009213.5A CN109059930B (zh) 2018-08-31 2018-08-31 一种基于视觉里程计的移动机器人定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811009213.5A CN109059930B (zh) 2018-08-31 2018-08-31 一种基于视觉里程计的移动机器人定位方法

Publications (2)

Publication Number Publication Date
CN109059930A CN109059930A (zh) 2018-12-21
CN109059930B true CN109059930B (zh) 2020-12-22

Family

ID=64758051

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811009213.5A Active CN109059930B (zh) 2018-08-31 2018-08-31 一种基于视觉里程计的移动机器人定位方法

Country Status (1)

Country Link
CN (1) CN109059930B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110188596A (zh) * 2019-01-04 2019-08-30 北京大学 基于深度学习的监控视频行人实时检测、属性识别与跟踪方法及系统
CN109887033B (zh) * 2019-03-01 2021-03-19 北京智行者科技有限公司 定位方法及装置
CN110310304B (zh) * 2019-06-14 2021-07-23 达闼机器人有限公司 单目视觉建图和定位方法、装置、存储介质及移动设备
CN112230256B (zh) * 2019-07-15 2024-04-09 苏州宝时得电动工具有限公司 自主机器人及其定位校准方法、装置和存储介质
CN111323009A (zh) * 2020-03-09 2020-06-23 西南交通大学 一种磁悬浮列车定位方法及系统
CN112308917A (zh) * 2020-10-15 2021-02-02 江苏大学 一种基于视觉的移动机器人定位方法
CN112799096B (zh) * 2021-04-08 2021-07-13 西南交通大学 基于低成本车载二维激光雷达的地图构建方法
CN114440892B (zh) * 2022-01-27 2023-11-03 中国人民解放军军事科学院国防科技创新研究院 一种基于拓扑地图和里程计的自定位方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103325108A (zh) * 2013-05-27 2013-09-25 浙江大学 一种融合光流与特征点匹配的单目视觉里程计的设计方法
CN105180933B (zh) * 2015-09-14 2017-11-21 中国科学院合肥物质科学研究院 基于直行路口检测的移动机器人航迹推算修正系统及方法
CN107478214A (zh) * 2017-07-24 2017-12-15 杨华军 一种基于多传感器融合的室内定位方法及系统

Also Published As

Publication number Publication date
CN109059930A (zh) 2018-12-21

Similar Documents

Publication Publication Date Title
CN109059930B (zh) 一种基于视觉里程计的移动机器人定位方法
CN106840148B (zh) 室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法
Zhao et al. A robust laser-inertial odometry and mapping method for large-scale highway environments
CN111220153B (zh) 基于视觉拓扑节点和惯性导航的定位方法
Chiu et al. Robust vision-aided navigation using sliding-window factor graphs
Yu et al. Monocular camera localization in prior lidar maps with 2d-3d line correspondences
Bellavia et al. Robust selective stereo SLAM without loop closure and bundle adjustment
CN105719314A (zh) 基于单应性估计和扩展卡尔曼滤波的无人机位置估计方法
CN109443354B (zh) 基于萤火虫群优化pf的视觉-惯性紧耦合组合导航方法
Chen et al. Stereo visual inertial pose estimation based on feedforward-feedback loops
Alliez et al. Real-time multi-SLAM system for agent localization and 3D mapping in dynamic scenarios
Lin et al. A sparse visual odometry technique based on pose adjustment with keyframe matching
CN114623817A (zh) 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法
Motlagh et al. Position Estimation for Drones based on Visual SLAM and IMU in GPS-denied Environment
Hug et al. Continuous-time stereo-inertial odometry
Launay et al. A corridors lights based navigation system including path definition using a topologically corrected map for indoor mobile robots
Françani et al. Dense prediction transformer for scale estimation in monocular visual odometry
Han et al. Human-following of mobile robots based on object tracking and depth vision
CN112348854A (zh) 一种基于深度学习视觉惯性里程检测方法
Zhang et al. Cross-Modal monocular localization in prior LiDAR maps utilizing semantic consistency
CN108469729B (zh) 一种基于rgb-d信息的人体目标识别与跟随方法
CN114216455A (zh) 同时优化时间偏差的视觉惯性里程计系统
CN113673462B (zh) 一种基于车道线的物流agv定位方法
Neyestani et al. Survey and research challenges in monocular visual odometry
Chen et al. Multiple-object tracking based on monocular camera and 3-D lidar fusion for autonomous vehicles

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant