CN109633664B - 基于rgb-d与激光里程计的联合定位方法 - Google Patents

Abstract

本发明公开了一种基于RGB‑D与激光里程计的联合定位方法。在本发明中，视觉定位基于RGB‑D，根据线特征匹配进行运动估计，激光方法使用EKF‑SLAM进行运动估计，当两个算法在同一时刻定位成功时，***会有两个位姿，本发明通过对两个位姿做组合滤波得到最优估计；若某些场景，如低纹理等情况导致定位失败，则将激光雷达定位获得的位姿作为全局滤波器的最优位姿。因此本发明采用组合定位的方式，综合雷达***和深度相机***的优缺点，将基于线特征的视觉SLAM与EKF‑SLAM结合起来，极大地提高了***定位建图的精度。

Description

基于RGB-D与激光里程计的联合定位方法

技术领域

本发明属于移动机器人自主定位领域，特别涉及一种基于RGB-D与激光里程计的联合定位方法。

背景技术

随着智能移动机器人的应用领域的扩大，人们期望智能移动机器人在更多领域为人类服务，代替人类完成更复杂的工作。然而,智能移动机器人所处的环境往往是未知的、很难预测，首要考虑的问题就是自主导航，而SLAM(Simultaneous Localization andMapping,同时定位与建图)问题是移动机器人导航中的核心问题之一,是实现机器人路径规划任务的前提。在移动机器人导航中，无论是局部实时避障还是全局规划，都需要精确知道机器人或障碍物的当前状态及位置，以完成导航、避障及路径规划等任务。

近年来，视觉SLAM以其丰富的应用场景在SLAM研究中逐渐成为主流。视觉 SLAM主要是通过摄像头来采集数据信息，在室内外环境下均能开展工作。但是视觉SLAM存在以下缺点：但是对光的依赖程度高，在暗处或者一些无纹理区域是无法进行工作的。

1、对速度存在约束。视觉SLAM单纯依靠图像，在运动过快的时候，必然会出现图像模糊的问题。

2、对环境要求高，对光线依赖程度高。它在面临到弱(无)纹理(比如白墙)、因没有特征可以提取，将难以工作下去；遇到重复纹理(比如相似的建筑物群) 时，会造成误匹配，从而导致错误的关联关系。在遇到明暗光交替的环境时，容易因为相机曝光成像的质量跟不上，从而造成匹配出错。

相比而言，激光SLAM测量更快更准，信息更丰富。激光雷达采集到的物体信息呈现出一系列分散的、具有准确角度和距离信息的点，被称为点云。通常，激光SLAM***通过对不同时刻两片点云的匹配与比对，计算激光雷达相对运动的距离和姿态的改变，也就完成了对机器人自身的定位。激光雷达距离测量比较准确，误差模型简单，在强光直射以外的环境中运行稳定，点云的处理也比较容易。同时，点云信息本身包含直接的几何关系，使得机器人的路径规划和导航变得直观。但是激光SLAM不擅长动态环境中的定位，比如有大量人员遮挡其测量的环境，也不擅长在类似的几何环境中工作，比如在一个又长又直、两侧是墙壁的环境。由于重定位能力较差，激光SLAM在追踪丢失后很难重新回到工作状态。

发明内容

激光SLAM和视觉SLAM各有其优缺点，单独使用都有其局限性，因此，本发明提出一种基于RGB-D与激光里程计的联合定位算法，用以解决在光照严重不足或纹理缺失的环境中，发挥激光里程计准确的点云匹配功能，为视觉SLAM 可以提供帮助。

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

一种基于RGB-D与激光里程计的联合定位方法，包括以下步骤：

步骤一：使用RGB-D相机对环境进行观测，得到深度图和彩色图，根据深度图和彩色图进行特征匹配得到第一特征信息，使用激光雷达对环境进行观测，得到2D激光数据从而得到第二特征信息，将第一特征信息送往局部滤波器1中，将第二特征信息送往局部滤波器2中；

步骤二：局部滤波器1使用基于线特征的RGB-D位姿估计方法得到局部最优估计

局部滤波器2使用扩展卡尔曼滤波算法进行运动估计得到局部最优估计/>

在全局滤波器中用以下方式进行融合：

其中P₁₁和P₂₂分别为局部滤波器1和局部滤波器2的误差协方差矩阵，

为融合后的无偏最小方差估计即全局最优估计值，/>

为融合后的误差协方差矩阵；

步骤三：在k时刻部滤波器1的位姿估计值为

局部滤波器2的位姿估计值为/>

k-1时刻全局滤波器的位姿估计值为/>

则

其中Δθ_k为k时刻激光雷达相对于k-1时刻的旋转角度，ΔS为k时刻激光雷达相对于k-1时刻的平移距离，如果满足式(22)和(23)，说明在同一时刻RGB-D的位姿估计值与激光雷达的位姿估计值差距不大，采用公式(16)和(17) 进行融合，

其中上式中的λ₁取0.2，λ₂取0.15，

如果不满足式(22)和(23)，说明在同一时刻RGB-D的位姿估计值与激光雷达的位姿估计值差距较大，此时将激光雷达定位的估计值作为RGB-D相机定位的估计值，即

P₁₁(k)＝P₂₂(k) (25)

然后采用公式(16)和(17)进行融合。

进一步地，所述RGB-D相机是Kinect相机。

进一步地，所述局部滤波器1是线性滤波器，所述局部滤波器2是扩展卡尔曼滤波器。

假设在机器人移动的过程中，遇到纹理较少的情况而导致特征缺失，这时视觉定位得到的位姿和实际位姿会有较大的偏差，如果将视觉定位的位姿和激光雷达定位的位姿继续融合，会导致全局滤波器的结果并不是最优估计，相反在这种情况下激光定位得到的位姿才是需要的最优估计。于是将此时刻激光雷达定位的位姿传递给视觉定位当做当前时刻的位姿，然后继续执行融合过程。

本发明与现有的技术相比，其显著的优点为：(1)视觉SLAM在纹理丰富的动态环境中稳定工作，并能为激光SLAM提供非常准确的点云匹配；(2)激光里程计可提供精确的方向和距离信息，提高正确匹配率；(3)在光照严重不足或纹理缺失的环境中，能保证定位的准确率与鲁棒性。

附图说明

图1为本发明基于RGB-D与激光里程计的联合定位方法融合方案流程。

图2为联邦滤波器算法的流程图。

图3为卡尔曼滤波流程图。

图4为Kinect与激光里程计的组合滤波示意图。

具体实施方式

下面结合附图对本发明作进一步详细描述。

首先结合图2，描述联邦滤波的具体过程。

联邦滤波的主要思想是各局部滤波器利用局部传感器提供的信息独立运算，将得到的局部估计值和协方差送入全局滤波器，经过全局滤波器的计算进而得到全局最优估计。接着再把全局最优估计反馈给局部滤波器，将局部滤波器的估计值重置，以便进行下一次的更新融合。

假设机器人的运动方程和观测方程分别为

X_i(k)＝F(k-1)X_i(k-1)+W(k) (1)

Z_i(k)＝H_i(k-1)X_i(k)+V_i(k) (2)

其中X_i(k)为时刻k的状态向量，F(k-1)和H_i(k-1)分别为状态转移矩阵和观测矩阵。W(k)为***噪声，是均值为0、方差为Q的高斯白噪声；V_i(k)为测量噪声、是均值为0、方差为R的高斯白噪声。

联邦滤波的估计过程如下：

P_g、Q_g分别是各局部滤波器融合到全局滤波器之后的状态估计矢量、***协方差矩阵、状态矢量协方差矩阵。同理，/>

P_ii、Q_i分别表示局部滤波器的三项数值。

经过一次融合后，全局估计值P_g、Q_g分别被放大

倍重新反馈给局部滤波器，将局部滤波器的三项值重置，以便进行下一次的滤波器融合。更新后的/>

P_i、Q_i值分别为/>

其中β_i是信息分配系数，即为各局部滤波器分配权值，需要满足如下条件：

不同的信息分配系数β_i对各滤波器的结构和特征有着不同的影响，选择一个合适的分配系数可以大大减少计算量，并且能使滤波器的性能达到最优。因此如何确定β_i对于联邦滤波器来说是至关重要的一步。

分析RGB-D与激光里程计的组合滤波过程中的全局滤波器，具体工作原理如下：

机器人的定位***包含两个局部滤波器1、2，并行处理RGB-D相机(Kinect 相机)和2D激光雷达产生的数据，根据各自的观测信息进行局部最优估计，然后将局部滤波器得到的结果送入全局滤波器做全局最优估计。结合图3，局部滤波器2的滤波过程使用常规的卡尔曼滤波器来实现，其中滤波方程为：

新息方程为：

预测方程为：

增益方程为：

预测误差为：

P_i(k+1|k)＝F(k)P_i(k|k)F^T(k)+G(k)Q(k)GT(k) (14)

误差更新：

P_i(k+1|k+1)＝[1-K_i(k+1)H_i(k+1)]×P_i(k+1|k) (15)

传统的卡尔曼滤波过程是从初始值开始逐步递推，设初始值为X(0)和P(0)，它们在滤波过程开始之前是已知的。结合图3卡尔曼滤波图，知道k+1时刻的滤波值X(k+1)，根据输入的k时刻的协方差和滤波值P(k)、X(k)按照上述的公式逐步就算，最终可以得到k+1时刻的协方差和滤波值P(k+1)、X(k+1)。值得注意的是，P(k+1)、P(k)的计算过程和观测值并没有关系，因此为了提高计算速度，通常会将这些值事先算好存入计算机。

卡尔曼滤波过程主要包括预测和更新两个部分，通过不断重复两个过程推进计算过程。首先根据预测方程计算出预测值，再用预测值计算出新息矩阵ω(k)和卡尔曼增益K(k)，最后用这两项和观测值一起修正预测值，从而得到下一时刻的协方差和滤波值P(k+1)、X(k+1)。

实现组合定位算法，具体内容如下：

首先讨论联邦滤波结构，由于***中没有参考***，因此主滤波器的信息分配系数为零，即β_m＝0。

组合过程结合图4Kinect与激光里程计的组合滤波流程图。两个传感器 (Kinect相机和2D激光雷达)都分别对观测目标进行独立观测以获取特征信息和计算，Kinect相机获取的特征信息是观测目标图像，2D激光雷达获取的特征信息是观测目标点云，将各自获取的特征信息传入各自的局部滤波器中，相应的产生两个局部滤波器。在局部滤波器1中，使用基于线特征的RGB-D位姿估计方法，在局部滤波器2中，使用扩展卡尔曼滤波器进行运动估计，设两个局部滤波器的信息分配系数分别为β₁和β₂。

设***噪声和量测噪声都服从高斯分布且均值为0，在局部滤波器估计中不相关，经过并行运行两个局部滤波器，最终能够得到RGB-D-SLAM和EKF-SLAM 的两个局部最优估计

和/>

因此在全局滤波器中用以下方式进行融合，融合后的结果为：

其中P₁₁和P₂₂分别为局部滤波器1和局部滤波器2的误差协方差矩阵

为融合后的无偏最小方差估计，/>

为融合后的误差协方差矩阵。

信息分配系数β₁、β₂分别为：

在机器人实际运动过程中，有时会遇到一些低纹理场景(如直通空白走廊)，比如在机器人转弯时一些特征不会出现在下一帧的图像里，因此基于RGB-D的定位会因为特征缺失或特征较少导致定位不准确。这时，使用以上的融合方式是不合理的。因为跟踪失败的视觉位姿与机器人实际位姿相差较大，如果继续做融合滤波，会导致全局滤波器估计值的准确度低于激光定位的局部最优估计值。所以此时的最优选择是将激光雷达定位获得的位姿作为全局滤波器的最优位姿。

综合以上情况考虑，改进后的组合算法如下。假设k时刻RGB-D的位姿估计值为

局部滤波器2的位姿估计值为/>

k-1时刻全局滤波器的位姿估计值为/>

则

其中Δθ_k为k时刻激光雷达相对于k-1时刻的旋转角度，ΔS为k时刻激光雷达相对于k-1时刻的平移距离。如果RGB-D定位成功，那么在同一时刻RGB-D 的位姿估计值与激光雷达的位姿估计值差距不应过大，可由式(22)和式(23)来判断。

/>

上式中的λ₁、λ₂为相关系数，根据经验值分别设为0.2和0.15。

当RGB-D定位结果不满足式(22)和式(23)时，说明其误差较大，此时将激光定位的估计值作为RGB-D定位的估计值，即

P₁₁(k)＝P₂₂(k) (25)

然后再使用式(16)和(17)的方法将两组局部最优估计值送入全局滤波器进行全局最优估计。

完整的算法如下：

算法先把前一时刻的全局最优估计值反馈给局部滤波器，经过局部滤波器的独立运算会得到两组局部最优估计值，最后将局部最优值送入全局滤波器运算。在这个过程中需要对视觉定位的准确性进行条件判断，如果视觉定位失败，则使用激光雷达的估计值。

Claims (3)

1.一种基于RGB-D与激光里程计的联合定位方法，其特征在于，包括以下步骤：

步骤一：使用RGB-D相机对环境进行观测，得到深度图和彩色图，根据深度图和彩色图进行特征匹配得到第一特征信息，使用激光雷达对环境进行观测，得到2D激光数据从而得到第二特征信息，将第一特征信息送往局部滤波器1中，将第二特征信息送往局部滤波器2中；

步骤二：局部滤波器1使用基于线特征的RGB-D位姿估计方法得到局部最优估计

局部滤波器2使用扩展卡尔曼滤波算法进行运动估计得到局部最优估计/>

在全局滤波器中用以下方式进行融合：

其中P₁₁和P₂₂分别为局部滤波器1和局部滤波器2的误差协方差矩阵，

为融合后的无偏最小方差估计即全局最优估计值，/>

为融合后的误差协方差矩阵；

步骤三：在k时刻局部滤波器1的位姿估计值为

局部滤波器2的位姿估计值为/>

时刻全局滤波器的位姿估计值为/>

则

其中Δθ_k为k时刻激光雷达相对于k-1时刻的旋转角度，ΔS为k时刻激光雷达相对于k-1时刻的平移距离，如果满足式(22)和(23)，说明在同一时刻RGB-D的位姿估计值与激光雷达的位姿估计值差距不大，采用公式(16)和(17)进行融合，

其中上式中的λ₁取0.2，λ₂取0.15，

如果不满足式(22)和(23)，说明在同一时刻RGB-D的位姿估计值与激光雷达的位姿估计值差距较大，此时将激光雷达定位的估计值作为RGB-D相机定位的估计值，即

P₁₁(k)＝P₂₂(k) (25)

然后采用公式(16)和(17)进行融合。

2.根据权利要求1所述的方法，其特征在于，所述RGB-D相机是Kinect相机。

3.根据权利要求1或2所述的方法，其特征在于，所述局部滤波器1是线性滤波器，所述局部滤波器2是扩展卡尔曼滤波器。

Images