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

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

Info

Publication number
CN111260709B
CN111260709B CN202010043799.8A CN202010043799A CN111260709B CN 111260709 B CN111260709 B CN 111260709B CN 202010043799 A CN202010043799 A CN 202010043799A CN 111260709 B CN111260709 B CN 111260709B
Authority
CN
China
Prior art keywords
ground
point
image
moment
point pairs
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
CN202010043799.8A
Other languages
English (en)
Other versions
CN111260709A (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 CN202010043799.8A priority Critical patent/CN111260709B/zh
Publication of CN111260709A publication Critical patent/CN111260709A/zh
Application granted granted Critical
Publication of CN111260709B publication Critical patent/CN111260709B/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/50—Depth or shape recovery
    • G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
    • 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
    • G06T2207/10012—Stereo images

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Optics & Photonics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种面向动态环境的地面辅助的视觉里程计方法。深度相机固定安装在地面移动机器人上，拍摄得到相邻时刻的图像；深度图像对应的三维点云提取主平面，计算地面似然参数将主平面完整合并；根据地面区域的匹配点对计算初始位姿，统计概率分布，设定动态阈值然后对非地面区域的匹配点对进行区分，联合所有静态点对优化位姿。本发明能够在地面区域较小的情况下从深度图像对应的点云中快速检测出地面，不依赖于固定阈值对外点进行区分，去除了动态角点的干扰，更加准确地估计机器人的运动轨迹。

Description

一种面向动态环境的地面辅助的视觉里程计方法
技术领域
本发明属于视觉里程计领域的一种视觉里程计方法，具体涉及了一种面向动态环境的地面辅助的视觉里程计方法。
背景技术
机器人和无人驾驶的技术领域包括环境感知，状态估计和规划控制等技术。由于视觉传感器的低成本和小型化的特点，视觉状态估计技术逐渐成为机器人技术中的热点问题。
视觉里程计(Visual Odometry,VO)是视觉状态估计的重要一环，可以分为特征点法和直接法，其作用是根据相邻两帧图像估计相对位姿。现有方法都是基于静态环境的假设，然而当动态物体出现在场景中，关联的图像特征或者像素中既包含静态环境也有动态物体。目前还没有一个成熟和统一的视觉里程计方法可以应用在动态环境下。与此同时，地面在轮式移动机器人的工作环境中是可以利用的先验信息，然而目前基于单目视觉或者深度传感器的地面检测方法均要求地面点的数量相比场景点的总数量占比充足，由于场景中存在的动态物体或者静态障碍物的遮挡，地面在图像中多为不连续且分段的，地面点的数量通常是不足的。
发明内容
本发明为解决现有算法在动态环境下精度差的问题，提出了一种面向动态环境的地面辅助的视觉里程计方法。
本发明采用的技术方案是包括以下步骤：
步骤一，获取深度相机拍摄的相邻时刻的图像
将深度相机固定安装在地面移动机器人上，深度相机的光轴指向机器人正前方，通过深度相机采集到机器人正前方的前一时刻和当前时刻的灰度图像和深度图像；
步骤二，检测完整地面
在前一时刻和当前时刻的深度图像上提取所有主平面，将属于地面部分的主平面合并成完整的地面，作为地面区域；除了地面区域以外均为非地面区域。
步骤三，估计初始位姿
在前一时刻的灰度图像的地面区域提取角点，对角点采用LK光流法进行跟踪得到关联的点对，作为地面点对并归于静态角点集合，然后使用n点透视(PnP)方法进行计算得到当前时刻坐标系在前一时刻坐标系中的初始相对位姿；相机坐标系是以相机光心为原点，z轴指向相机正前方，x轴指向右侧，y轴指向下的三维坐标系。
所述的步骤三中，同一角点在前一时刻和当前时刻的灰度图像中的坐标构成一对关联的点对。
步骤四，筛选角点
根据所有地面点对的重投影误差进行拟合设定动态阈值，然后在前一时刻的灰度图像的非地面区域提取角点，对角点采用LK光流法进行跟踪得到关联的点对作为非地面点对并归于静态角点集合，然后采用步骤三得到的初始相对位姿将当前时刻的角点重投影至前一时刻的图像上求得重投影误差后根据动态阈值进行筛选；
步骤五，估计优化位姿
联合地面点对和非地面点对的静态点对，使用n点透视(PnP)方法进行计算得到优化后的当前时刻相机坐标系在前一时刻相机坐标系中的最终相对位姿；
步骤六，对于相邻两帧图像重复上述步骤二～步骤五，计算获得各个时刻的最终相对位姿，去除动态角点的干扰，由各个时刻的最终相对位姿衔接作为结果，实现了动态环境下的视觉里程计。
所述的步骤二中，检测完整地面采用以下方式进行处理：
1)提取主平面并计算地面似然参数：
根据深度图像生成点云，采用凝聚层次聚类平面检测(PEAC)算法提取点云中可能存在的多个主平面，得到主平面在灰度图像中对应的区域，然后根据每个主平面的质心和法线方程计算以下公式的地面似然参数err，表示为：
Figure BDA0002368652080000021
其中，θa表示主平面的法线和理想地面法线的锐夹角；(cu,cv)T表示主平面对应点云的质心在图像上的二维像素坐标；cols和rows分别表示灰度图像的宽度和高度；α为权重；
2)合并主平面：
选取地面似然参数最小的主平面作为种子面，遍历其余主平面中的每个点，计算每个点到种子面的垂直距离，若每个点的垂直距离均小于预设距离阈值则将该主平面合并到种子面中，直至合并完全获得的平面作为地面区域，表示为：
dp＝Πs·p<Tdis
其中，p表示当前判断的点，Πs表示种子面的法线，Tdis表示距离阈值，dp表示点p到种子面的垂直距离。
所述步骤四中，筛选角点具体为：
1)设定动态阈值：
计算所有地面点对的重投影误差，采用正态分布对所有地面点对的重投影误差进行拟合，取2倍的标准差作为动态阈值Tadp，即Tadp＝2σ，σ表示正态分布拟合后获得的标准差；
2)筛选：在前一时刻的灰度图像的非地面区域提取角点，并对角点采用LK光流法进行跟踪得到关联的点对，作为非地面点对，也归于静态角点集合；
然后采用步骤三得到的初始相对位姿，将后一时刻的角点重投影至前一时刻的图像上求得重投影误差，然后根据动态阈值进行筛选：若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)小于等于动态阈值Tadp，则将该角点p归为静态角点集合；若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)大于动态阈值Tadp，则不将该角点p归为静态角点集合。
所述的步骤三中，同一角点在前一时刻和当前时刻的灰度图像中的坐标构成一对关联的点对。
所述步骤四中，计算所有地面点对的重投影误差，具体是将后一时刻的角点重投影至前一时刻的灰度图像上的投影点，以投影后的投影点和角点之间的像素距离作为重投影误差。
本发明的有益效果是：
1.本发明采用的完整地面检测方法能够从深度图像对应的点云中快速检测出地面，相比于其他方法，本发明不要求地面的区域占据图片的大部分的区域，即使是地面区域像素数量比较少，本发明也可以检测成功。
2.本发明采用的动态角点检测方法，根据地面区域角点的重投影误差的概率分布设定动态阈值，然后对非地面区域的角点进行区分，相比于设定固定阈值和随机采样的外点剔除方法，对不同的传感器和运动噪声更鲁棒。
附图说明
图1为相邻两帧的灰度图像和深度图像；
图2为检测到的所有主平面；
图3为经过合并后的完整地面；
图4为所有角点的重投影误差表示图；
图5为重投影误差的频率直方图和正态分布拟合曲线图；
图6为动态角点和静态角点的区分结果图。
具体实施方式
下面结合附图对本发明的技术方案做进一步详细的说明。
本发明的具体实施例及其实施过程如下：
步骤一，获取深度相机拍摄的相邻时刻的图像。将深度相机固定安装在地面移动机器人上，光轴指向机器人正前方，采集到前一时刻和当前时刻的灰度图像和深度图像，如图1所示；
步骤二，检测完整地面。
1)提取主平面并计算地面似然参数：
根据深度图像生成点云，采用(PEAC)算法提取点云中可能存在的多个主平面，得到主平面在灰度图像中对应的区域，如图2所示；然后根据每个主平面的质心和法线方程计算以下公式的地面似然参数err，表示为：
Figure BDA0002368652080000041
其中，θa表示主平面的法线和理想地面法线的锐夹角；理想地面法线是指垂直于地面向上的单位向量。(cu,cv)T表示主平面对应点云的质心在图像上的二维像素坐标；cols和rows分别表示灰度图像的宽度和高度；α为权重；
2)合并主平面：
选取地面似然参数最小的主平面作为种子面，遍历其余主平面中的每个点，计算每个点到种子面的垂直距离，若每个点的垂直距离均小于预设距离阈值则将该主平面合并到种子面中，直至合并完全获得的平面作为地面区域，表示为：
dp＝Πs·p<Tdis
其中，p表示当前判断的点，Πs表示种子面的法线，Tdis表示距离阈值，dp表示点p到种子面的垂直距离。
如图3所示为合并后的完整地面。
步骤三，估计初始位姿。在前一时刻的灰度图像的地面区域提取角点，对角点采用LK光流法进行跟踪得到关联的点对，作为地面点对并归于静态角点集合，同一角点在前一时刻和当前时刻的灰度图像中的坐标构成一对关联的点对。然后使用n点透视(PnP)方法进行计算得到前一时刻相机坐标系在当前时刻相机坐标系中的初始相对位姿；相机坐标系是以相机光心为原点，z轴指向相机正前方，x轴指向右侧，y轴指向下的三维坐标系。
步骤四，筛选角点。根据所有地面点对的重投影误差进行拟合设定动态阈值，然后在前一时刻的灰度图像的非地面区域提取角点，对角点采用LK光流法进行跟踪得到关联的点对作为非地面点对并归于静态角点集合，然后采用步骤三得到的初始相对位姿将当前一时刻的角点重投影至前一时刻的图像上，如图4所示，白色线段表示重投影误差的大小和方向，求得重投影误差后根据动态阈值进行筛选；
1)设定动态阈值：
计算所有地面点对的重投影误差，具体是将后一时刻的角点重投影至前一时刻的灰度图像上的投影点，以投影后的投影点和角点之间的像素距离作为重投影误差；采用正态分布对所有地面点对的重投影误差进行拟合，拟合曲线如图5所示，取2倍的标准差作为动态阈值Tadp，即Tadp＝2σ，σ表示正态分布拟合后获得的标准差；具体实施中除了地面点对以外的静态角点被正确筛选的置信区间为95.4％。
2)筛选：在前一时刻的灰度图像的非地面区域提取角点，并对角点采用LK光流法进行跟踪得到关联的点对，作为非地面点对，也归于静态角点集合，
然后采用步骤三得到的初始相对位姿，将后一时刻的角点重投影至前一时刻的图像上求得重投影误差，以投影后的像素距离作为重投影误差，然后根据动态阈值进行筛选：
若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)小于等于动态阈值Tadp，则将该角点p为非地面点对；
若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)大于动态阈值Tadp，则将该角点p不为非地面点对。
非地面点对和非地面点对均为静态角点，其余的为动态角点。
如图6所示，非地面区域的静态角点用绿色圆圈表示，红色圆圈表示动态角点。
步骤五，估计优化位姿。联合地面点对和非地面点对的静态点对，使用n点透视(PnP)方法进行计算得到优化后的当前时刻相机坐标系在前一时刻相机坐标系中的最终相对位姿。
步骤六，对于相邻两帧图像重复上述步骤二～步骤五，计算获得各个时刻的最终相对位姿，去除动态角点的干扰，由各个时刻的最终相对位姿衔接作为结果，实现了动态环境下的视觉里程计。
由此实施可见，本发明能够在地面区域较小的情况下从深度图像对应的点云中快速检测出地面，不依赖于固定阈值对外点进行区分，去除了动态角点的干扰，更加准确地估计机器人的运动轨迹。

Claims (5)

1.一种面向动态环境的地面辅助的视觉里程计方法，其特征在于，包括以下步骤：
步骤一，获取深度相机拍摄的相邻时刻的图像
将深度相机固定安装在地面移动机器人上，深度相机的光轴指向机器人正前方，通过深度相机采集到前一时刻和当前时刻的灰度图像和深度图像；
步骤二，检测完整地面
在前一时刻和当前时刻的深度图像上提取所有主平面，将属于地面部分的主平面合并成完整的地面，作为地面区域；
步骤三，估计初始位姿
在前一时刻的灰度图像的地面区域提取角点，对角点采用LK光流法进行跟踪得到关联的点对，作为地面点对并归于静态角点集合，然后使用n点透视方法进行计算得到当前时刻坐标系在前一时刻坐标系中的初始相对位姿；
步骤四，筛选角点
根据所有地面点对的重投影误差进行拟合设定动态阈值，然后在前一时刻的灰度图像的非地面区域提取角点，对角点采用LK光流法进行跟踪得到关联的点对作为非地面点对并归于静态角点集合，然后采用步骤三得到的初始相对位姿将当前时刻的角点重投影至前一时刻的图像上求得重投影误差后根据动态阈值进行筛选；
步骤五，估计优化位姿
联合地面点对和非地面点对的静态点对，使用n点透视方法进行计算得到优化后的当前时刻相机坐标系在前一时刻相机坐标系中的最终相对位姿；
步骤六，对于相邻两帧图像重复上述步骤二～步骤五，计算获得各个时刻的最终相对位姿，由各个时刻的最终相对位姿衔接作为结果，实现了动态环境下的视觉里程计。
2.根据权利要求1所述的一种面向动态环境的地面辅助的视觉里程计方法，其特征在于：所述的步骤二中，检测完整地面采用以下方式进行处理：
1)提取主平面并计算地面似然参数：
根据深度图像生成点云，采用凝聚层次聚类平面检测算法提取点云中可能存在的多个主平面，得到主平面在灰度图像中对应的区域，然后根据每个主平面的质心和法线方程计算以下公式的地面似然参数err，表示为：
Figure FDA0002368652070000021
其中，θa表示主平面的法线和理想地面法线的锐夹角；(cu,cv)T表示主平面对应点云的质心在图像上的二维像素坐标；cols和rows分别表示灰度图像的宽度和高度；a为权重；
2)合并主平面：
选取地面似然参数最小的主平面作为种子面，遍历其余主平面中的每个点，计算每个点到种子面的垂直距离，若每个点的垂直距离均小于预设距离阈值则将该主平面合并到种子面中，直至合并完全获得的平面作为地面区域，表示为：
dp＝Πs·p<Tdis
其中，p表示当前判断的点，Πs表示种子面的法线，Tdis表示距离阈值，dp表示点p到种子面的垂直距离。
3.根据权利要求1所述的一种面向动态环境的地面辅助的视觉里程计方法，其特征在于：所述步骤四中，筛选角点具体为：
1)设定动态阈值：
计算所有地面点对的重投影误差，采用正态分布对所有地面点对的重投影误差进行拟合，取2倍的标准差作为动态阈值Tadp，即Tadp＝2σ，σ表示正态分布拟合后获得的标准差；
2)筛选：在前一时刻的灰度图像的非地面区域提取角点，并对角点采用LK光流法进行跟踪得到关联的点对，作为非地面点对；
然后采用步骤三得到的初始相对位姿，将后一时刻的角点重投影至前一时刻的图像上求得重投影误差，然后根据动态阈值进行筛选：
若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)小于等于动态阈值Tadp，则将该角点p归为静态角点集合；
若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)大于动态阈值Tadp，则不将该角点p归为静态角点集合。
4.根据权利要求1所述的一种面向动态环境的地面辅助的视觉里程计方法，其特征在于：所述的步骤三中，同一角点在前一时刻和当前时刻的灰度图像中的坐标构成一对关联的点对。
5.根据权利要求1所述的一种面向动态环境的地面辅助的视觉里程计方法，其特征在于：所述步骤四中，计算所有地面点对的重投影误差，具体是将后一时刻的角点重投影至前一时刻的灰度图像上的投影点，以投影后的投影点和角点之间的像素距离作为重投影误差。
CN202010043799.8A 2020-01-15 2020-01-15 一种面向动态环境的地面辅助的视觉里程计方法 Active CN111260709B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010043799.8A CN111260709B (zh) 2020-01-15 2020-01-15 一种面向动态环境的地面辅助的视觉里程计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010043799.8A CN111260709B (zh) 2020-01-15 2020-01-15 一种面向动态环境的地面辅助的视觉里程计方法

Publications (2)

Publication Number Publication Date
CN111260709A CN111260709A (zh) 2020-06-09
CN111260709B true CN111260709B (zh) 2022-04-19

Family

ID=70948940

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010043799.8A Active CN111260709B (zh) 2020-01-15 2020-01-15 一种面向动态环境的地面辅助的视觉里程计方法

Country Status (1)

Country Link
CN (1) CN111260709B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113420590B (zh) * 2021-05-13 2022-12-06 北京航空航天大学 弱纹理环境下的机器人定位方法、装置、设备及介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018133119A1 (zh) * 2017-01-23 2018-07-26 中国科学院自动化研究所 基于深度相机进行室内完整场景三维重建的方法及***
CN108460779A (zh) * 2018-02-12 2018-08-28 浙江大学 一种动态环境下的移动机器人图像视觉定位方法
CN108776989A (zh) * 2018-06-08 2018-11-09 北京航空航天大学 基于稀疏slam框架的低纹理平面场景重建方法
CN108955718A (zh) * 2018-04-10 2018-12-07 中国科学院深圳先进技术研究院 一种视觉里程计及其定位方法、机器人以及存储介质
CN110058602A (zh) * 2019-03-27 2019-07-26 天津大学 基于深度视觉的多旋翼无人机自主定位方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10809071B2 (en) * 2017-10-17 2020-10-20 AI Incorporated Method for constructing a map while performing work

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018133119A1 (zh) * 2017-01-23 2018-07-26 中国科学院自动化研究所 基于深度相机进行室内完整场景三维重建的方法及***
CN108460779A (zh) * 2018-02-12 2018-08-28 浙江大学 一种动态环境下的移动机器人图像视觉定位方法
CN108955718A (zh) * 2018-04-10 2018-12-07 中国科学院深圳先进技术研究院 一种视觉里程计及其定位方法、机器人以及存储介质
CN108776989A (zh) * 2018-06-08 2018-11-09 北京航空航天大学 基于稀疏slam框架的低纹理平面场景重建方法
CN110058602A (zh) * 2019-03-27 2019-07-26 天津大学 基于深度视觉的多旋翼无人机自主定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
"Fast Plane Extraction in Organized Point Clouds Using Agglomerative Hierarchical Clustering";Chen Feng.et al;《ResearchGate》;20140630;全文 *
"Real-time Depth Enhanced Monocular Odometry";Ji Zhang.et al;《IEEE》;20141106;全文 *
"基于双向重投影的双目视觉里程计";张涛等;《中国惯性技术学报》;20181231;第26卷(第6期);全文 *

Also Published As

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

Similar Documents

Publication Publication Date Title
CN110349250B (zh) 一种基于rgbd相机的室内动态场景的三维重建方法
KR101776622B1 (ko) 다이렉트 트래킹을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
Veľas et al. Calibration of rgb camera with velodyne lidar
US9465997B2 (en) System and method for detection and tracking of moving objects
CN112233177B (zh) 一种无人机位姿估计方法及***
EP3070430B1 (en) Moving body position estimation device and moving body position estimation method
JP2018522348A (ja) センサーの３次元姿勢を推定する方法及びシステム
US20210103299A1 (en) Obstacle avoidance method and device and movable platform
CN107862735B (zh) 一种基于结构信息的rgbd三维场景重建方法
JP5105481B2 (ja) 車線検出装置、車線検出方法、及び車線検出プログラム
US11727637B2 (en) Method for generating 3D skeleton using joint-based calibration acquired from multi-view camera
CN111915651B (zh) 基于数字影像地图与特征点跟踪的视觉位姿实时估计方法
Wang et al. An improved ArUco marker for monocular vision ranging
Luo et al. Multisensor integrated stair recognition and parameters measurement system for dynamic stair climbing robots
CN116879870A (zh) 一种适用于低线束3d激光雷达的动态障碍物去除方法
CN114119729A (zh) 障碍物识别方法及装置
CN112509054A (zh) 一种相机外参动态标定方法
CN117523461B (zh) 一种基于机载单目相机的运动目标跟踪与定位方法
CN111260709B (zh) 一种面向动态环境的地面辅助的视觉里程计方法
Cavestany et al. Improved 3D sparse maps for high-performance SFM with low-cost omnidirectional robots
CN111260725B (zh) 一种面向动态环境的轮速计辅助的视觉里程计方法
Fucen et al. The object recognition and adaptive threshold selection in the vision system for landing an unmanned aerial vehicle
Cigla et al. Image-based visual perception and representation for collision avoidance
CN115797405A (zh) 一种基于车辆轴距的多镜头自适应跟踪方法
CN108694348B (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