[发明专利]一种基于自适应无迹卡尔曼滤波的激光SLAM算法

说明书

本发明公开了一种基于自适应无迹卡尔曼滤波的激光SLAM算法。该算法为：首先利用Hector SLAM算法中的匹配方法，基于激光束末端与已知地图的匹配优化实现定位，读取激光数据并滤波转换，得到激光预估位姿；再利用无迹卡尔曼滤波算法，对非线性系统，融合激光预估位姿数据和里程计推算位姿数据，作为实际估计位姿；最后计算自适应向量，更新测量噪声协方差矩阵，使用自适应无迹卡尔曼滤波算法，对非线性系统进行滤波处理。本发明能够在仅依靠激光数据的情况下，得到较高的定位和构图精度，避免了局部极值，减少或消除了激光扫描到的干扰数据，解决了实际系统噪声或量测噪声特性未知或变化的问题，改善了非线性系统的估计效果，提高了定位精度。

技术领域

本发明属于SLAM导航定位算法技术领域，特别是一种基于自适应无迹卡尔曼滤波的激光SLAM算法。

背景技术

即时定位与地图构建(SLAM)技术于1986年的IEEE机器人与自动化会议正式提出，经过曲折而漫长的探索，SLAM技术的发展逐渐成熟。近年来，随着优化理论的引入，SLAM技术进入了新的高速发展期。2011年Stefan Kohlbrecher，Oskar von Stryk等人提出了一种基于优化理论的Hector SLAM算法，Hector SLAM是一款高效快速的在线SLAM算法，在仅依靠激光数据的情况下，也能得到较高的定位和构图精度。但Hector SLAM算法在长廊等特征雷同的环境仍然无法得到准确的定位。此外，激光定位有可能扫描到行人或装饰等干扰特征，此时单纯依靠激光容易产生错误的定位。因此通过卡尔曼滤波算法，融合激光数据与里程计数据，可以消除或减少这些缺陷。

标准的卡尔曼滤波是针对线性系统提出的，但实际机器人系统往往是较为复杂的非线性系统。针对非线性系统，可以使用扩展卡尔曼滤波(EKF)或者无迹卡尔曼滤波(UKF)进行融合估计。前者对非线性部分进行一阶泰勒展开以完成线性化，但对于高阶部分难以忽略，或者求导困难的系统则难以适用。后者则通过无迹变换(UT)来近似得到变量经非线性变换后的概率分布，即对一个随机变量按分布进行sigma点采样，对这些点进行非线性变换，并用得到的结果描述变量非线性变换后的变量的分布。

无迹卡尔曼滤波的最优估计建立在系统噪声和量测噪声特性已知的基础上，而这一点在现实中往往难以满足，系统噪声和量测噪声的统计特性通常未知，甚至可能是变化的。综上，现有技术存在的问题是：实际中的系统噪声和量测噪声的统计特性通常未知，甚至可能是变化的，往往导致陷入局部极值的结果，激光扫描到的干扰数据较多，使最终的滤波结果不理想，定位精度差。

发明内容

本发明的目的在于提供一种基于自适应无迹卡尔曼滤波的激光SLAM算法，能够在线估计系统噪声或者量测噪声，使得非线性系统能得到更好的估计效果。

实现本发明目的的技术解决方案为：一种基于自适应无迹卡尔曼滤波的激光SLAM算法，包括以下步骤：

步骤1、利用Hector SLAM算法，基于激光束末端与已知地图的匹配优化实现定位，读取激光数据，再根据激光传感器坐标系与机器人坐标系的关系，求出机器人在全局坐标中的位姿；

步骤2、利用无迹卡尔曼滤波算法，在非线性系统中，融合激光预估位姿数据和里程计推算位姿数据，作为实际估计位姿；

步骤3、计算自适应向量，更新测量噪声协方差矩阵，然后使用自适应无迹卡尔曼滤波算法，对非线性系统进行滤波处理。

进一步地，步骤1所述的利用Hector SLAM算法，基于激光束末端与已知地图的匹配优化实现定位，读取激光数据，再根据激光传感器坐标系与机器人坐标系的关系，求出机器人在全局坐标中的位姿，具体如下：