[发明专利]一种平方根容积模糊自适应卡尔曼滤波SLAM方法

说明书

本发明公开了一种平方根容积模糊自适应卡尔曼滤波SLAM方法，所述方法包括：对移动机器人进行建模，建立动力学模型和观测模型；模糊自适应噪声动态调整算法，对动力学模型和观测模型中的运动噪声和观测噪声设置控制权值，通过动态调整新息均值和方差的方式对噪声的权值进行了模糊调整；通过k‑1时刻的机器人位姿信息预测k时刻机器人位姿信息；经过预设次数迭代后，对计算器位姿信息进行更新。将迭代方式与强跟踪相结合，并针对机器人存在运动噪声和观测噪声的问题，采用改进的模糊自适应方式对不同的噪声进行自适应动态调整。降低了算法复杂度，解决了采样点在非线性情况下存在失真的问题，校正因特征点增加引起的轨迹偏移现象，提高了位姿精度。

技术领域

本发明涉及机器人自主导航技术领域，特别是涉及一种平方根容积模糊自适应卡尔曼滤波SLAM方法。

背景技术

同时定位与地图重建是指移动机器人在未知环境下通过激光雷达进行环境地图的重建并在重建后的地图中表示出该机器人位姿状态的过程。SLAM算法在移动机器人中的应用是该领域的研究热点。传统的算法将扩展卡尔曼滤波引入到SLAM领域，其本质就是使用卡尔曼滤波对线性化的模型进行处理。扩展的卡尔曼滤波(EKF)虽然能够解决非线性系统问题，但是不能很好的处理算法中的数据关联且精度不高。在此基础之上，近年来许多研究学者提出了各种改进的新型算法。针对地图重建过程中受外界环境影响较大的问题，现有技术中提出了一种超声概率栅格地图环境特征点提取算法、以及迭代测量更新的UKF算法，有效解决了无迹卡尔曼滤波(UKF)中Sigma点会逐渐偏离真实状态估计值的问题，但机器人在同时定位与地图重建过程中系统往往存在噪声，随着地图特征点的增加系统的观测维度也会随之增加，导致定位效果不佳。

发明内容

本发明要解决的技术问题在于，针对现有技术的定位效果不佳的问题，提供一种平方根容积模糊自适应卡尔曼滤波SLAM方法。

为解决上述技术问题，本发明采用的一个技术方案是：提供一种平方根容积模糊自适应卡尔曼滤波SLAM方法，其特征在于，所述方法包括：

对移动机器人进行建模，建立动力学模型和观测模型；

模糊自适应噪声动态调整算法，对动力学模型和观测模型中的运动噪声和观测噪声设置控制权值，通过动态调整新息均值和方差的方式对噪声的权值进行了模糊调整；

通过k-1时刻的机器人位姿信息预测k时刻机器人位姿信息；

经过预设次数迭代后，对计算器位姿信息进行更新。

进一步的，所述“对移动机器人进行建模，建立动力学模型和观测模型”的步骤具体为：

定义为移动机器人的位姿信息，为k时刻地图的特征信息，则移动机器人系统状态信息向量为：

对移动机器人进行建模，其动力学模型和观测模型如下：

其中，U_k为n_u维运动信息向量，Z_k为n_z维的观测向量，Q_k、R_k分别表示运动噪声和观测噪声。

进一步的，所述“模糊自适应噪声动态调整算法，对动力学模型和观测模型中的运动噪声和观测噪声设置控制权值，通过动态调整新息均值和方差的方式对噪声的权值进行了模糊调整”的步骤具体为：

对于动力学模型和观测模型中的运动噪声和观测噪声设置控制权值，动力学模型和观测模型变换为：

其中，a_k，b_k分别表示控制的权值；