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

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

【技术实现步骤摘要】
基于RGB-D与激光里程计的联合定位方法
本专利技术属于移动机器人自主定位领域，特别涉及一种基于RGB-D与激光里程计的联合定位方法。
技术介绍
随着智能移动机器人的应用领域的扩大，人们期望智能移动机器人在更多领域为人类服务，代替人类完成更复杂的工作。然而,智能移动机器人所处的环境往往是未知的、很难预测，首要考虑的问题就是自主导航，而SLAM(SimultaneousLocalizationandMapping,同时定位与建图)问题是移动机器人导航中的核心问题之一,是实现机器人路径规划任务的前提。在移动机器人导航中，无论是局部实时避障还是全局规划，都需要精确知道机器人或障碍物的当前状态及位置，以完成导航、避障及路径规划等任务。近年来，视觉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使用扩展卡尔曼滤波算法进行运动估计得到局部最优估计在全局滤波器中用以下方式进行融合：其中P11和P22分别为局部滤波器1和局部滤波器2的误差协方差矩阵，为融合后的无偏最小方差估计即全局最优估计值，为融合后的误差协方差矩阵；步骤三：在k时刻部滤波器1的位姿估计值为局部滤波器2的位姿估计值为k-1时刻全局滤波器的位姿估计值为则其中Δθk为k时刻激光雷达相对于k-1时刻的旋转角度，ΔS为k时刻激光雷达相对于k-1时刻的平移距离，如果满足式(22)和(23)，说明在同一时刻RGB-D的位姿估计值与激光雷达的位姿估计值差距不大，采用公式(16)和(17)进行融合，其中上式中的λ1取0.2，λ2取0.15，如果不满足式(22)和(23)，说明在同一时刻RGB-D的位姿估计值与激光雷达的位姿估计值差距较大，此时将激光雷达定位的估计值作为RGB-D相机定位的估计值，即P11(k)＝P22(k)(25)然后采用公式(16)和(17)进行融合。进一步地，所述RGB-D相机是Kinect相机。进一步地，所述局部滤波器1是线性滤波器，所述局部滤波器2是扩展卡尔曼滤波器。假设在机器人移动的过程中，遇到纹理较少的情况而导致特征缺失，这时视觉定位得到的位姿和实际位姿会有较大的偏差，如果将视觉定位的位姿和激光雷达定位的位姿继续融合，会导致全局滤波器的结果并不是最优估计，相反在这种情况下激光定位得到的位姿才是需要的最优估计。于是将此时刻激光雷达定位的位姿传递给视觉定位当做当前时刻的位姿，然后继续执行融合过程。本专利技术与现有的技术相比，其显著的优点为：(1)视觉SLAM在纹理丰富的动态环境中稳定工作，并能为激光SLAM提供非常准确的点云匹配；(2)激光里程计可提供精确的方向和距离信息，提高正确匹配率；(3)在光照严重不足或纹理缺失的环境中，能保证定位的准确率与鲁棒性。附图说明图1为本专利技术基于RGB-D与激光里程计的联合定位方法融合方案流程。图2为联邦滤波器算法的流程图。图3为卡尔曼滤波流程图。图4为Kinect与激光里程计的组合滤波示意图。具体实施方式下面结合附图对本专利技术作进一步详细描述。首先结合图2，描述联邦滤波的具体过程。联邦滤波的主要思想是各局部滤波器利用局部传感器提供的信息独立运算，将得到的局部估计值和协方差送入全局滤波器，经过全局滤波器的计算进而得到全局最优估计。接着再把全局最优估计反馈给局部滤波器，将局部滤波器的估计值重置，以便进行下一次的更新融合。假设机器人的运动方程和观测方程分别为Xi(k)＝F(k-1)Xi(k-1)+W(k)(1)Zi(k)＝Hi(k-1)Xi(k)+Vi(k)(2)其中Xi(k)为时刻k的状态向量，F(k-1)和Hi(k-1)分别为状态转移矩阵和观测矩阵。W(k)为系统噪声，是均值为0、方差为Q的高斯白噪声；Vi(k)为测量噪声、是均值为0、方差为R的高斯白噪声。联邦滤波的估计过程如下：Pg、Qg分别是各局部滤波器融合到全局滤波器之后的状态估计矢量、系统协方差矩阵、状态矢量协方差矩阵。同理，Pii、Qi分别表示局部滤波器的三项数值。经过一次融合后，全局估计值Pg、Qg分别被放大倍重新反馈给局部滤波器，将局部滤波器的三项值重置，以便进行下一次的滤波器融合。更新后的Pi、Qi值分别为其中βi是信息分配系数，即为各局部滤波器分配权值，需要满足如下条件：不同的信息分配系数βi对各滤波器的结构和特征有着不同的影响，选择一个合适的分配系数可以大大减少计算量，并且能使滤波器的性能达到最优。因此如何确定βi对于联邦滤波器来说是至关重要的一步。分析RGB-D与激光里程计的组合滤波过程中的全局滤波器，具体工作原理如下：机器人的定位系统包含两个局部滤波器1、2，并行处理RGB-D相机(Kinect相机)和2D激光雷达产生的数据，根据各自的观测信息进行局部最优估计，然后将局部滤波器得到的结果送入全局滤波器做全局最优估计。结合图3，局部滤波器2的滤波过程使用常规的卡尔曼滤波器来实现，其中滤波方程为：新息方程为：预测方程为：增益方程为：预测误差为：Pi(k+1|k)＝F(k)Pi(k|k)FT(k)+G(k)Q(k)GT(k)(14)误差更新：Pi(本文档来自技高网...

【技术保护点】
1.一种基于RGB‑D与激光里程计的联合定位方法，其特征在于，包括以下步骤：步骤一：使用RGB‑D相机对环境进行观测，得到深度图和彩色图，根据深度图和彩色图进行特征匹配得到第一特征信息，使用激光雷达对环境进行观测，得到2D激光数据从而得到第二特征信息，将第一特征信息送往局部滤波器1中，将第二特征信息送往局部滤波器2中；步骤二：局部滤波器1使用基于线特征的RGB‑D位姿估计方法得到局部最优估计

【技术特征摘要】
1.一种基于RGB-D与激光里程计的联合定位方法，其特征在于，包括以下步骤：步骤一：使用RGB-D相机对环境进行观测，得到深度图和彩色图，根据深度图和彩色图进行特征匹配得到第一特征信息，使用激光雷达对环境进行观测，得到2D激光数据从而得到第二特征信息，将第一特征信息送往局部滤波器1中，将第二特征信息送往局部滤波器2中；步骤二：局部滤波器1使用基于线特征的RGB-D位姿估计方法得到局部最优估计局部滤波器2使用扩展卡尔曼滤波算法进行运动估计得到局部最优估计在全局滤波器中用以下方式进行融合：其中P11和P22分别为局部滤波器1和局部滤波器2的误差协方差矩阵，为融合后的无偏最小方差估计即全局最优估计值，为融合后的误差协方差矩阵；步骤三：在k时刻部滤波器1的位姿估计值为局部滤波器2的位姿估计值为k-1时刻全局滤波器的...

【专利技术属性】
技术研发人员：郭剑辉，周贝贝，蔡球球，吴轶凡，牟升，钟钎钎，
申请(专利权)人：南京理工大学工程技术研究院有限公司，
类型：发明
国别省市：江苏,32