[发明专利]基于多次位姿校正的移动机器人快速精确定位算法

摘要

本发明公开了一种基于多次位姿校正的移动机器人快速精确定位算法，属于机器人和计算机图形学技术领域。本发明所述算法利用UKF结合机器人运动模型融合IMU和里程计的数据，把这个融合结果作为AMCL的运动模型采样数据源。接着利用已知全局栅格地图和激光雷达测量作为观测对机器人进行第二次位姿校正。然后用NDT算法将激光扫描转换为NDT分布，把AMCL算法校正后的位姿作为初始变换，将当前扫描的NDT分布与事先转化为NDT分布的全局地图进行匹配，校正转移矩阵，实现最后一次位姿校正。本发明所述方法可以使机器人在复杂地形中实现快速精准的定位，解决了里程计和加速度计误差累积、定位算法所用传感器信息来源单一、以及复杂地形中点云匹配算法耗时高等问题。

主权项

1.一种基于多次位姿校正的移动机器人快速精确定位算法，其特征在于，包括以下步骤：步骤1.利用同步定位与构图(SLAM,Simultaneous Localization and Mapping)算法建立复杂环境的二维栅格地图；在每次算法执行前，先加载已经建好的栅格地图；步骤2.栅格地图上的黑色像素点代表激光扫描到的障碍物，把相应的黑色像素点转化为图像标系下的激光点云数据格式；再将图像坐标系下的激光点云转换至地图坐标系下，从而得到全局点云地图；步骤3.选择尺寸为S的栅格将全局点云规则的划分到各个栅格中，计算点数大于3的栅格中的点的均值和方差，用此均值和方差表示当前栅格内点的正态分布，从而将全局点云地图转换为全局正态分布变换(NDT,Normal Distribution Transformation)地图；步骤4.根据无迹卡尔曼滤波(UKF,Unscented Kalman Filter)原理，用机器人的差分运动模型预测机器人位姿，将里程计和惯性测量单元(IMU,Inertial Measurement Unit)数据作为UKF的观测校正机器人位姿，从而得到一个融合机器人运动模型、里程计数据和IMU数据的融合位姿；步骤5.采用增强蒙特卡洛(AMCL,Augment Monte Carlo Localization)算法，从UKF的融合位姿中采样获取一系列机器人全局位姿预测值，然后根据二维激光雷达的观测与全局栅格地图障碍物的分布，从似然域测量模型中获取预测位姿权重，根据权重校正机器人的位姿预测值，从而获得全局位姿；步骤6.将当前时刻二维激光扫描到的点云转换为NDT分布；步骤7.将AMCL输出的全局位姿转换为相对于地图原点的转移矩阵P₀＝(R,t)，其中R为旋转矩阵，t为平移向量；步骤8.采用分布到分布(D2D,Distribution‑to‑Distribution)的二维正态分布变换(2D‑NDT,Two‑Dimensional Normal Distributions Transform)算法，把P₀作为初始转移矩阵，并令最优转移矩阵P_best＝P₀，匹配当前时刻的NDT分布与全局NDT地图，迭代校正(R,t)，机器人最终位姿以P_best＝(R,t)表示；步骤9.将转移矩阵P_best转换为机器人对应全局地图的位姿，即最终定位位姿。