[发明专利]基于RGB-D与激光里程计的联合定位方法

摘要

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

主权项

1.一种基于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)进行融合。