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

说明书

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

技术领域

本发明属于机器人和计算机图形学技术领域，具体涉及一种基于多次位姿校正的移动机器人快速精确定位算法。

背景技术

随着人类进入信息产业革命时代，人工智能技术迅速发展，移动机器人技术正以前所未有的速度发展革新，逐渐渗透各个行业。定位技术作为移动机器人认知自己在环境中所处位置的根本方式，一直都是机器人领域的研究热点。然而，很多定位算法只针对单个传感器，有的算法又仅用单个算法融合众多传感器的数据。这两种方式在某种特殊环境下，可以使机器人达到一定的定位精度，但是在复杂的环境中常常由于传感器的累积误差导致定位精度降低，或者是由于环境太复杂导致定位时间不能满足实时性要求。

目前很多研究提出扩展卡尔曼滤波(EKF,Extended Kalman Filter)、UKF、蒙特卡洛定位(MCL,Monte Carlo Localization)、点云匹配(Point Clouds Matching)等一系列算法。其中，扩展卡尔曼滤波和无迹卡尔曼滤波由于存在误差累积现象，随着时间的累积其算法定位精度会越来越低；蒙特卡洛通常会得到一个比卡尔曼滤波方法更加精确的定位结果，但由于算法粒子数量越多，其算法对工控机性能要求越高，通常不能达到一个非常精确的定位结果；最后，点云匹配技术适用于机器人精确定位，然而点云匹配技术普遍存在对初始值依赖大的情况，同时在复杂的环境中，一些常用的点云匹配算法如迭代最近点(ICP,Iterative Closest Point)往往会消耗掉大量的时间。

为此，采用无迹卡尔曼滤波对机器人位姿进行一次校正，然后利用增强蒙特卡洛进行二次校正，最后采用D2D 2D-NDT来进行最后一次校正，既能够有效地克服里程计和IMU累计误差，还能够在复杂的环境中快速定位机器人位姿。

发明内容

本发明的目的是解决现有算法在机器人定位过程中存在的误差累积，定位精度低，复杂环境中定位耗时高等问题，提供一种基于多次位姿校正的移动机器人快速精确定位算法。

本发明所提出的技术问题是这样解决的：

一种基于多次位姿校正的移动机器人快速精确定位算法，其特征在于，包括以下步骤：

步骤1.利用SLAM算法建立复杂环境的二维栅格地图；在每次算法执行前，先加载已经建好的栅格地图；

步骤2.栅格地图上的黑色像素点代表激光扫描到的障碍物，把相应的黑色像素点转化为图像标系下的激光点云数据格式；再将图像坐标系下的激光点云转换至地图坐标系下，从而得到全局点云地图；

步骤3.选择尺寸为S的栅格将全局点云规则的划分到各个栅格中，计算点数大于3的栅格中的点的均值和方差，用此均值和方差表示当前栅格内点的正态分布，从而将全局点云地图转换为全局NDT地图；

步骤4.根据UKF原理，用机器人的差分运动模型预测机器人位姿，将里程计和IMU数据作为UKF的观测校正机器人位姿，从而得到一个融合机器人运动模型、里程计数据和IMU数据的融合位姿；