CN111260725B - 一种面向动态环境的轮速计辅助的视觉里程计方法 - Google Patents

一种面向动态环境的轮速计辅助的视觉里程计方法 Download PDF

Info

Publication number
CN111260725B
CN111260725B CN202010043797.9A CN202010043797A CN111260725B CN 111260725 B CN111260725 B CN 111260725B CN 202010043797 A CN202010043797 A CN 202010043797A CN 111260725 B CN111260725 B CN 111260725B
Authority
CN
China
Prior art keywords
moment
wheel speed
pose
point
speed meter
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
CN202010043797.9A
Other languages
English (en)
Other versions
CN111260725A (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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202010043797.9A priority Critical patent/CN111260725B/zh
Publication of CN111260725A publication Critical patent/CN111260725A/zh
Application granted granted Critical
Publication of CN111260725B publication Critical patent/CN111260725B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10004—Still image; Photographic image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Image Analysis (AREA)
  • Image Processing (AREA)

Abstract

本发明公开了一种面向动态环境的轮速计辅助的视觉里程计方法。深度相机固定安装在轮式机器人上，拍摄得到相邻时刻的图像；根据前后时刻的轮速计读数计算当前时刻的预测位姿，根据预测位姿将上一时刻的特征点的地图点投影到当前时刻的灰度图像，然后在重投影像素附近进行搜索跟踪，计算重投影误差，和像素距离阈值比较获得有效特征点，根据所有的有效特征点进行再次位姿优化计算。本发明能够利用轮式机器人常配备的轮速计，检测环境中的无效特征点，保留有效的静态特征点，更加鲁棒地对机器人进行定位。

Description

一种面向动态环境的轮速计辅助的视觉里程计方法
技术领域
本发明属于视觉里程计领域的一种视觉里程计方法，具体涉及一种面向动态环境的轮速计辅助的视觉里程计方法。
背景技术
现有的视觉里程计方法都假设图像中的特征点为不运动的，然而当运动物体出现在场景中，视觉里程计的结果会受到干扰。与此同时，轮式机器人常装配轮速计，如何利用轮速计来帮助视觉里程计在动态环境下实现更鲁棒的定位具有重要意义。
发明内容
本发明为解决现有算法在动态环境下鲁棒性差的问题，提出了一种面向动态环境的轮速计辅助的视觉里程计方法。
本发明采用的技术方案是包括以下步骤：
步骤一，获取深度相机拍摄的相邻时刻的图像
将深度相机固定安装在轮式机器人上，轮式机器人的轮子上安装有轮速计，采集到前一时刻和当前时刻的灰度图像和深度图像，并读取轮式机器人上当前时刻的轮速计读数；
步骤二，预测相机位姿
根据匀速运动情况，根据上一时刻和当前时刻的轮速计读数预测在当前时刻的轮式机器人位姿，位姿包括位置和姿态的关系；
步骤三，无效特征点去除
对上一时刻的灰度图像提取特征点，以特征点对应的三维空间坐标作为上一时刻的地图点，根据预测所得的轮式机器人位姿将上一时刻的地图点投影到当前时刻的灰度图像上作为投影点，然后在投影点周围的固定大小区域范围内对上一时刻灰度图像提取的角点采用LK光流法进行跟踪获得跟踪点，计算投影点和跟踪点之间的重投影误差；利用重投影误差的大小进行阈值分割获得有效特征点；
步骤四，位姿优化
根据所有的有效特征点进行位姿的优化计算，重新利用PnP算法处理获得当前时刻相机的姿态和位置；
步骤五，对于每相邻两帧图像重复上述步骤二～步骤四，以排除动态特征点的干扰后进行位姿的计算，获得各个时刻的位姿，由各个时刻的位姿衔接作为结果，实现在动态环境下的视觉里程计。
所述的步骤二中，当前时刻位姿的预测采用以下方式处理获得：
Figure BDA0002368649990000021
其中，ωi和ωj表示前一时刻和当前时刻的轮速计角速度测量值，vi和vj表示前一时刻和当前时刻的轮速计速度测量值，
Figure BDA0002368649990000022
和
Figure BDA0002368649990000023
表示上一时刻的相机姿态和位置，
Figure BDA0002368649990000024
和
Figure BDA0002368649990000025
表示当前时刻的相机姿态和位置，Δt表示上一时刻和当前时刻之间的时间间隔。
所述步骤三中，将上一时刻的地图点投影到当前时刻的灰度图像进行跟踪并根据重投影误差的大小进行阈值分割采用以下方式进行处理：
1)将上一时刻的地图点投影到当前时刻的灰度图像：
Figure BDA0002368649990000026
其中，Pi表示上一时刻的特征点xi对应的地图点的3D位置，x′j表示投影后的像素坐标，K表示相机的内参数矩阵，s表示地图点在第j帧中的深度
然后采用LK光流法在当前时刻的灰度图像上的x′j附近针对xi进行跟踪，得到xj。
2)计算投影点x′j和跟踪点xj之间的像素距离d作为重投影误差，利用重投影误差的大小和预设的像素距离阈值进行比较：若重投影误差d小于像素距离阈值Tset，d＜Tset，则以跟踪点作为有效特征点；若重投影误差d大于等于像素距离阈值Tset，d＜Tset，则跟踪点不作为有效特征点。
本发明的有益效果是：
1.本发明采用的轮速计辅助的方法能够帮助预测当前时刻的相机位姿，相比于其他方法，本发明不要求对于动态运动物体的先验知识，只需要轮式机器人中常见的轮速计便可以实现鲁棒的视觉里程计。
附图说明
图1为***流程图；
图2为静态有效特征点的检测示意图。
具体实施方式
下面结合附图对本发明的技术方案做进一步详细的说明。
如图1所示，本发明的具体实施例及其实施过程如下：
步骤一，获取深度相机拍摄的相邻时刻的图像。将深度相机固定安装在轮式机器人上，轮式机器人的轮子上安装有轮速计，采集到前一时刻和当前时刻的灰度图像和深度图像，并读取轮式机器人上当前时刻的轮速计读数。
步骤二，预测相机位姿。根据匀速运动情况，根据上一时刻和当前时刻的轮速计读数预测在当前时刻的轮式机器人位姿，位姿包括位置和姿态的关系。匀速运动的运动模型如下，当前时刻位姿的预测采用以下方式处理获得：
Figure BDA0002368649990000031
其中，ωi和ωj表示前一时刻和当前时刻的轮速计角速度测量值，vi和vj表示前一时刻和当前时刻的轮速计速度测量值，
Figure BDA0002368649990000032
和
Figure BDA0002368649990000033
表示上一时刻的相机姿态和位置，
Figure BDA0002368649990000034
和
Figure BDA0002368649990000035
表示当前时刻的相机姿态和位置，Δt表示上一时刻和当前时刻之间的时间间隔。
步骤三，运动特征点去除。对上一时刻的灰度图像提取角点，以角点对应的三维空间坐标作为上一时刻的地图点，根据预测所得的轮式机器人位姿将上一时刻的地图点投影到当前时刻的灰度图像上作为投影点，然后在投影点周围的固定大小区域范围内对上一时刻灰度图像提取的角点采用LK光流法进行跟踪获得跟踪点，计算投影点和跟踪点之间的重投影误差；利用重投影误差的大小进行阈值分割获得有效特征点。
如图2所示，具体如下：
1)将上一时刻的地图点投影到当前时刻的灰度图像：
Figure BDA0002368649990000036
其中，Pi表示上一时刻的特征点xi对应的地图点的3D位置，x′j表示投影后的像素坐标，
然后采用LK光流法在当前时刻的灰度图像上的x′j附近针对xi进行跟踪，得到xj。
2)计算投影点x′j和跟踪点xj之间的像素距离d作为重投影误差，利用重投影误差的大小和预设的像素距离阈值进行比较：若重投影误差d小于像素距离阈值Tset，d＜Tset，则以跟踪点作为有效特征点；若重投影误差d大于等于像素距离阈值Tset，d＜Tset，则跟踪点不作为有效特征点。
步骤四，位姿优化。根据所有的有效特征点进行位姿的优化计算，重新利用PnP算法处理获得当前时刻相机的姿态和位置。
步骤五，对于相邻两帧图像重复上述步骤二～步骤四，以排除动态特征点的干扰后进行位姿的计算，获得各个时刻的位姿，由各个时刻的位姿衔接作为结果，实现在动态环境下的视觉里程计。
由此实施可见，本发明能够利用轮式机器人常配备的轮速计，检测环境中的动态特征点，保留有效的静态特征点，更加鲁棒地对机器人进行定位。

Claims (3)

1.一种面向动态环境的轮速计辅助的视觉里程计方法，其特征在于，包括以下步骤：
步骤一，获取深度相机拍摄的相邻时刻的图像
将深度相机固定安装在轮式机器人上，轮式机器人的轮子上安装有轮速计，采集到前一时刻和当前时刻的灰度图像和深度图像，并读取轮式机器人上当前时刻的轮速计读数；
步骤二，预测相机位姿
根据匀速运动情况，根据上一时刻和当前时刻的轮速计读数预测在当前时刻的轮式机器人位姿；
步骤三，无效特征点去除
对上一时刻的灰度图像提取特征点，以特征点对应的三维空间坐标作为上一时刻的地图点，根据预测所得的轮式机器人位姿将上一时刻的地图点投影到当前时刻的灰度图像上作为投影点，然后在投影点周围的固定大小区域范围内对上一时刻灰度图像提取的角点采用LK光流法进行跟踪获得跟踪点，计算投影点和跟踪点之间的重投影误差；利用重投影误差的大小进行阈值分割获得有效特征点；
步骤四，位姿优化
根据所有的有效特征点进行位姿的优化计算，重新利用PnP算法处理获得当前时刻相机的姿态和位置；
步骤五，对于每相邻两帧图像重复上述步骤二～步骤四，获得各个时刻的位姿，由各个时刻的位姿衔接作为结果，实现在动态环境下的视觉里程计。
2.根据权利要求1所述的一种面向动态环境的轮速计辅助的视觉里程计方法，其特征在于：所述的步骤二中，当前时刻位姿的预测采用以下方式处理获得：
Figure FDA0002368649980000011
其中，ωi和ωj表示前一时刻和当前时刻的轮速计角速度测量值，vi和vj表示前一时刻和当前时刻的轮速计速度测量值，
Figure FDA0002368649980000012
和
Figure FDA0002368649980000013
表示上一时刻的相机姿态和位置，
Figure FDA0002368649980000014
和
Figure FDA0002368649980000015
表示当前时刻的相机姿态和位置，Δt表示上一时刻和当前时刻之间的时间间隔。
3.根据权利要求1所述的一种面向动态环境的轮速计辅助的视觉里程计方法，其特征在于：所述步骤三中，计算投影点x′j和跟踪点xj之间的像素距离d作为重投影误差，利用重投影误差的大小和预设的像素距离阈值进行比较：若重投影误差d小于像素距离阈值Tset，d＜Tset，则以跟踪点作为有效特征点；若重投影误差d大于等于像素距离阈值Tset，d＜Tset，则跟踪点不作为有效特征点。
CN202010043797.9A 2020-01-15 2020-01-15 一种面向动态环境的轮速计辅助的视觉里程计方法 Active CN111260725B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010043797.9A CN111260725B (zh) 2020-01-15 2020-01-15 一种面向动态环境的轮速计辅助的视觉里程计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010043797.9A CN111260725B (zh) 2020-01-15 2020-01-15 一种面向动态环境的轮速计辅助的视觉里程计方法

Publications (2)

Publication Number Publication Date
CN111260725A CN111260725A (zh) 2020-06-09
CN111260725B true CN111260725B (zh) 2022-04-19

Family

ID=70950651

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010043797.9A Active CN111260725B (zh) 2020-01-15 2020-01-15 一种面向动态环境的轮速计辅助的视觉里程计方法

Country Status (1)

Country Link
CN (1) CN111260725B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111950370B (zh) * 2020-07-10 2022-08-26 重庆邮电大学 动态环境下线拓展视觉里程计方法
WO2022147655A1 (zh) * 2021-01-05 2022-07-14 深圳市大疆创新科技有限公司 定位方法、空间信息获取方法、装置、拍摄设备

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108242079A (zh) * 2017-12-30 2018-07-03 北京工业大学 一种基于多特征视觉里程计和图优化模型的vslam方法
CN109387204A (zh) * 2018-09-26 2019-02-26 东北大学 面向室内动态环境的移动机器人同步定位与构图方法
CN109816696A (zh) * 2019-02-01 2019-05-28 西安全志科技有限公司 一种机器人定位与建图方法、计算机装置及计算机可读存储介质
CN110232308A (zh) * 2019-04-17 2019-09-13 浙江大学 基于手部速度与轨迹分布的跟随机器人手势轨迹识别方法
CN110570449A (zh) * 2019-09-16 2019-12-13 电子科技大学 一种基于毫米波雷达与视觉slam的定位与建图方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10422648B2 (en) * 2017-10-17 2019-09-24 AI Incorporated Methods for finding the perimeter of a place using observed coordinates

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108242079A (zh) * 2017-12-30 2018-07-03 北京工业大学 一种基于多特征视觉里程计和图优化模型的vslam方法
CN109387204A (zh) * 2018-09-26 2019-02-26 东北大学 面向室内动态环境的移动机器人同步定位与构图方法
CN109816696A (zh) * 2019-02-01 2019-05-28 西安全志科技有限公司 一种机器人定位与建图方法、计算机装置及计算机可读存储介质
CN110232308A (zh) * 2019-04-17 2019-09-13 浙江大学 基于手部速度与轨迹分布的跟随机器人手势轨迹识别方法
CN110570449A (zh) * 2019-09-16 2019-12-13 电子科技大学 一种基于毫米波雷达与视觉slam的定位与建图方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"Visual Odometry based on Stereo Image Sequences with RANSAC-based Outlier Rejection Scheme";Andreas Geiger.et al;《researchGate》;20100731;全文 *
"动态场景下基于运动物体检测的立体视觉里程计";林志林等;《光学学报》;20171130;第37卷(第11期);全文 *

Also Published As

Publication number Publication date
CN111260725A (zh) 2020-06-09

Similar Documents

Publication Publication Date Title
CN108986037B (zh) 基于半直接法的单目视觉里程计定位方法及定位***
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN110070615B (zh) 一种基于多相机协同的全景视觉slam方法
CN112734852B (zh) 一种机器人建图方法、装置及计算设备
CN110807809B (zh) 基于点线特征和深度滤波器的轻量级单目视觉定位方法
KR101725060B1 (ko) 그래디언트 기반 특징점을 이용한 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
US9888235B2 (en) Image processing method, particularly used in a vision-based localization of a device
KR101784183B1 (ko) ADoG 기반 특징점을 이용한 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
CN110766785B (zh) 一种地下管道实时定位与三维重建装置及方法
CN113012197B (zh) 一种适用于动态交通场景的双目视觉里程计定位方法
CN111260725B (zh) 一种面向动态环境的轮速计辅助的视觉里程计方法
JP7173471B2 (ja) ３次元位置推定装置及びプログラム
CN112541938A (zh) 一种行人速度测量方法、***、介质及计算设备
CN114964236A (zh) 一种针对地下停车场环境的建图与车辆定位***及方法
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法
CN115077519A (zh) 基于模板匹配与激光惯导松耦合的定位建图方法和装置
CN115147344A (zh) 一种增强现实辅助汽车维修中的零件三维检测与跟踪方法
Qimin et al. A methodology of vehicle speed estimation based on optical flow
CN112862818B (zh) 惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及***
CN111862146B (zh) 一种目标对象的定位方法及装置
CN116804553A (zh) 基于事件相机/imu/自然路标的里程计***及方法
CN111696155A (zh) 一种基于单目视觉的多传感融合机器人定位方法
CN111553342A (zh) 一种视觉定位方法、装置、计算机设备和存储介质
CN111260709B (zh) 一种面向动态环境的地面辅助的视觉里程计方法

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