CN108052103B - 基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法 - Google Patents

Abstract

一种基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法，利用深度相机和惯性测量单元进行松耦合，通过深度相机采集的深度图获取点云信息，提取平面特征；将深度相机采集的RGB图像转换成灰度图和平面特征融合，利用迭代最近点算法进行优化；迭代最近点优化后的数据和惯性测量单元数据进行松耦合，利用回环检测提高位姿图精度，得到巡检机器人运行轨迹、点云地图和树跳表地图，达到巡检机器人在室内同时定位和地图构建的效果。通过此方法提高巡检机器人在地下空间的同时定位精度和鲁棒性，达到巡检机器人在地下空间同时定位和地图构建的效果。巡检机器人在地下空间进行作业时，在强旋转环境下本发明采用的方法具有良好鲁棒性。

Description

基于深度惯性里程计的巡检机器人地下空间同时定位和地图 构建方法

技术领域

本发明涉及巡检机器人同时定位领域，尤其涉及一种基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法。

背景技术

随着科学技术的进步，巡检机器人在工业、军事等领域的应用越来越广泛。在很多情况下，巡检机器人的作业空间的信息是复杂未知的。巡检机器人想要实现在室内自主导航、目标识别、自动避障等功能，其精确同时定位显着尤为重要。传统的同时定位方法大多以GPS、北斗等全球卫星同时定位为主，但普通的GPS传感器同时定位精度较低，无法满足巡检机器人精确同时定位。

虽然差分GPS在室外同时定位精度较高，但是价格高昂同时无法在隧道、巷道、地下室这些GPS失效环境下工作。隧道、巷道、地下室这些地下空间常年无法接受太阳光照射，光照较低。在视觉定位方面，现在普遍使用单纯的相机进行定位精度较低，无法达到巡检机器人有效定位的效果。

伴随计算机视觉、图像处理技术发展的同时，机器视觉方法通过感知环境进行导航，同时在机器人实时定位方面得到广泛应用。视觉同时定位方法的原理是，通过安装在机器人身上的摄像机实时采集运动过程中的图像，并从图像中提取相关信息，进而判断并计算机器人的运行姿态和轨迹，最终实现导航和实时定位。然而视觉传感器容易受到光照的影响，同时在曝光较强、低亮度等情况下同时定位容易丢失。除此之外，单纯的单目视觉传感器没有尺度信息，无法感知机器人所处周围环境深度，并且在机器人原地转弯时特征丢失，容易导致机器人实时定位失效。

巡检机器人使用惯性测量单元进行定位发展较早，惯性定位是利用惯性测量单元测量的线加速度和旋转角速率来计算载体的六自由度同时定位信息。载体的角速率通过陀螺仪测量，主要用于计算机器人的旋转矩阵，并且提供载体坐标系和导航坐标系的转化关系；载体的线加速度通过加速度计测量，通过对得到的加速度积分求解机器人的速度信息和位移信息，最后通过将机器人六自由度信息转换到导航坐标系中完成定位。然而单纯的惯性测量单元在重复路径下误差累计较大，并且无法进行有效的回环检测。除此之外，因惯性测量单元随机游走等性质，在巡检机器人起步以及加速度变化较大时会产生大量迟滞误差。

以华硕xtion和微软Kinect为代表的消费机深度相机能够获取RGB图像和深度图，广泛应用于室内机器人领域。但是这种深度相机视野范围普遍较窄，导致算法跟踪目标容易丢失，同时深度数据往往存在大量噪声，甚至导致有些数据无法使用。在传统的方法中，视觉特征的提取算法往往基于像素的差异，但是在深度相机测量的深度数据中，位于边角处的点，不易被识别出来。并且移动机器人在大旋转情况下，采用单独深度相机进行定位容易丢失。

同时定位和建图(simultaneous location and mapping，SLAM)最初应用于机器人领域。使用单独的传感器的方法虽然计算量较小，但是定位精度不高、鲁棒性不强。使用多种传感器融合的同时定位和地图构建方法已经成为发展的主流，并且缺乏有效的深度相机和惯性测量单元融合的同时定位和地图构建。

发明内容

根据现有技术的不足，本发明提供一种基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法，通过此方法提高巡检机器人在地下空间的定位精度和鲁棒性，达到巡检机器人在地下空间同时定位和地图构建的效果。巡检机器人在地下空间进行作业时，在强旋转环境下本发明采用的方法具有良好鲁棒性。

为实现上述目的，本发明采用的技术方案为：

一种基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法，该方法为：

利用深度相机和惯性测量单元进行松耦合，通过深度相机采集的深度图获取点云信息，提取平面特征；将深度相机采集的RGB图像转换为灰度图和平面特征融合，利用迭代最近点算法进行优化；迭代最近点优化后的数据和惯性测量单元数据进行松耦合，利用回环检测提高位姿图精度，得到巡检机器人运行轨迹、点云地图和树跳表地图，达到巡检机器人在室内同时定位和地图构建的效果。

优选的是，深度相机采集相邻两个帧为场景S和模型M，两组匹配点分别记为P＝{p_i|i＝1,...,n}和Q＝{q_i|i＝1,...,n}；

其中p_i和q_i都表示三维空间点，能够参数化成

优选的是，深度相机模型为：

其中(u,v)为空间点(x,y,z)^T对应的像素位置，d为深度值，C为相机内参。

优选的是，M到S的运动用旋转R和平移t描述，利用迭代最近点算法求解：

优选的是，从深度图获得的点云中提取平面特征，用四个参数来描述三维空间的平面：

p＝(a,b,c,d)＝{x,y,z|ax+by+cz+d＝0}；

令d＝0，将每个拟合的平面投影在成像平面上，得到平面点的成像位置(u,v),利用投影方程求解：

其中f_x,f_y,c_x,c_y为深度相机的内参，s是深度数据的缩放因子。

优选的是，对每个平面图做一次灰度直方图归一化增强它的对比度，然后再提取特征点并计算特征点的深度：

优选的是，太多的关键帧会给后端和回环检测带来额外的计算量，而太少的关键帧就会导致关键帧之间运动太大，特征匹配数量不够，导致容易丢失。在提取图像的平面，计算它和上一个关键帧之间的相对运动超过某个阈值，就认为这是一个新的关键帧。

优选的是，阈值的计算是通过评价平移和欧拉角旋转实现的：

这里(Δx,Δy,Δz)是相对平移，而(α,β,γ)是相对的欧拉角；

w₁＝(m,m,m),m∈(0.6,0.7)；w₂∈(0.95,1.05)。

本发明有益效果：

通过此方法提高巡检机器人在地下空间作业的同时定位精度和鲁棒性，达到巡检机器人在地下空间环境下的同时定位和地图构建的效果。巡检机器人在地下空间进行作业时，在强旋转环境下本发明采用的方法具有良好鲁棒性。

附图说明

图1为本发明的方法框架示意图；

图2为本发明的深度相机采集RGB图转换成的灰度图示意图；

图3为本发明的深度相机采集深度图像示意图；

图4为本发明的构建环境三维点云地图；

图5为本发明的构建环境三维树跳表地图；

图6为本发明的机器人运行轨迹。

具体实施方式

以下结合附图，通过具体实施例对本发明作进一步的说明。

如图1至图6所示，一种基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法，该同时定位方法为：利用深度相机和惯性测量单元进行松耦合，通过深度相机采集的深度图获取点云信息，提取平面特征。将深度相机采集的RGB图像和平面特征融合，利用迭代最近点(ICP)算法进行优化；ICP优化后的数据和惯性测量单元(IMU)数据进行松耦合，利用回环检测(Loop closure)提高位姿图精度，得到巡检机器人运行轨迹、点云地图和树跳表地图。

深度相机采集相邻两个帧为场景S和模型M，两组匹配点分别记为P＝{p_i|i＝1,...,n}和Q＝{q_i|i＝1,...,n}。其中p_i和q_i都表示三维空间点，可以参数化成

深度相机模型为：

其中(u,v)为空间点(x,y,z)^T对应的像素位置，d为深度值，C为相机内参。

M到S的运动用旋转R和平移t描述，利用ICP算法求解：

从深度图获得的点云中提取平面特征，用四个参数来描述三维空间的平面：

p＝(a,b,c,d)＝{x,y,z|ax+by+cz+d＝0}

令d＝0，将每个拟合的平面投影在成像平面上，得到平面点的成像位置(u,v),利用投影方程求解：

其中f_x,f_y,c_x,c_y为深度相机的内参，s是深度数据的缩放因子。

对每个平面图做一次灰度直方图归一化增强它的对比度，然后再提取特征点并计算特征点的深度：

三维空间的相机位姿，以平移和单位四元数表示：x＝{x,y,z,q_x,q_y,q_z,q_w}；

从该帧提取的平面组P＝{P_i},每个平面包含它的平面参数和所属的特征点。

太多的关键帧会给后端和回环检测带来额外的计算量，而太少的关键帧就会导致关键帧之间运动太大，特征匹配数量不够，导致容易丢失。在提取图像的平面，计算它和上一个关键帧之间的相对运动超过某个阈值，就认为这是一个新的关键帧。阈值的计算是通过评价平移和欧拉角旋转实现的：

这里(Δx,Δy,Δz)是相对平移，而(α,β,γ)是相对的欧拉角。

w₁＝(m,m,m),m∈(0.6,0.7)，w₂∈(0.95,1.05)。

以上所述，仅仅是对本发明的较佳实施例，并非是对本发明做其他形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改变形式为等同变化的等效实施例。但是，凡是未脱离本发明方案内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化与改型，仍属于本发明的保护范围。

Claims (6)

1.一种基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法，其特征在于，该方法为：

利用深度相机和惯性测量单元进行松耦合，通过深度相机采集的深度图获取点云信息，提取平面特征；

将深度相机采集的RGB图像转换成灰度图和平面特征融合，利用迭代最近点算法进行优化；

迭代最近点优化后的数据和惯性测量单元数据进行松耦合，利用回环检测提高位姿图精度，得到巡检机器人运行轨迹、点云地图和树跳表地图，达到巡检机器人在地下空间同时定位和地图构建的效果；

深度相机采集相邻两个帧为场景S和模型M，两组匹配点分别记为P＝{p_i|i＝1,...,n}和Q＝{q_i|i＝1,...,n}；

其中p_i和q_i都表示三维空间点，能够参数化成

M到S的运动用旋转R和平移t描述，利用迭代最近点算法求解：

2.根据权利要求1所述的基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法，其特征在于：

深度相机模型为：

其中(u,v)为空间点(x,y,z)^T对应的像素位置，d为深度值，C为相机内参。

3.根据权利要求1所述的基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法，其特征在于：

从深度图获得的点云中提取平面特征，用四个参数来描述三维空间的平面：

p＝(a,b,c,d)＝{x,y,z|ax+by+cz+d＝0}；

令d＝0，将每个拟合的平面投影在成像平面上，得到平面点的成像位置(u,v),利用投影方程求解：

d＝z·s

其中f_x,f_y,c_x,c_y为深度相机的内参，s是深度数据的缩放因子。

4.根据权利要求3所述的基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法，其特征在于：

对每个平面图做一次灰度直方图归一化增强它的对比度，然后再提取特征点并计算特征点的深度：

5.根据权利要求1所述的基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法，其特征在于：

太多的关键帧会给后端和回环检测带来额外的计算量，而太少的关键帧就会导致关键帧之间运动太大，特征匹配数量不够，导致容易丢失；在提取图像的平面，计算它和上一个关键帧之间的相对运动超过某个阈值，就认为这是一个新的关键帧。

6.根据权利要求5所述的基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法，其特征在于：

阈值的计算是通过评价平移和欧拉角旋转实现的：

这里(Δx,Δy,Δz)是相对平移，而(α,β,γ)是相对的欧拉角；

w₁＝(m,m,m),m∈(0.6,0.7)；w₂∈(0.95,1.05)。

Images