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

权利要求书

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转换为机器人对应全局地图的位姿，即最终定位位姿。

2.根据权利要求1所述方法，其特征在于步骤8包括：

步骤8-1.令当前迭代次数为k，将当前的NDT分布通过第k次迭代的转移矩阵P_k映射到全局NDT地图坐标系下；

步骤8-2.计算当前的NDT分布与全局NDT地图的相关性评分score(P)_k，其中，μ_i,μ_j为当前NDT分布第i个栅格内的均值和距离该栅格最近的全局NDT地图栅格的均值；∑_i,∑_j为对应方差；d₁,d₂为归一化参数；

步骤8-3.牛顿法最小化函数f＝-score,求解转移矩阵增量ΔP；

步骤8-4.令k+1次迭代的转移矩阵P_k+1＝P_k+ΔP，并计算在P_k+1下，当前的NDT分布与全局NDT地图的相关性评分score(P)_k+1，若score(P)_k+1>score(P)_k，则令P_best＝P_k+1；

步骤8-5.判断转移矩阵增量ΔP是否达到精度要求，或者迭代次数k是否大于最大迭代次数，若是则进行下一步，否则返回执行步骤8-1。