[发明专利]一种移动机器人同步定位与地图构建方法

说明书

本发明提供了一种移动机器人同步定位与地图构建方法，包括如下步骤：（1）建立系统模型，包括机器人运动模型，观测模型，环境特征模型；并将有色过程噪声模型转化为高斯白噪声模型；（2）设定移动机器人同步定位与地图构建算法的初始参数，包括航位推算系统参数，激光雷达参数；（3）状态预测；（4）观测；（5）数据关联；（6）更新；（7）状态增广。其特征在于步骤（6）中，在标准容积卡尔曼滤波的量测更新过程中，选取自适应因子，自适应调整自相关协方差矩阵和互相关协方差矩阵，完成更新过程，同时能够平衡系统模型预测信息与量测信息之间的权比，从而提高定位精度与地图构建精度。

技术领域

本发明涉及的是一种移动机器人同步定位与地图构建方法，特别是涉及一种基于自适应容积卡尔曼滤波的移动机器人同步定位与地图构建方法。

背景技术

同步定位与地图构建(simultaneous localization and mapping，简称：SLAM)是指移动机器人从一个未知的位置出发，在不断运动的过程中利用自身传感器对环境的动态感知信息递增式构建环境地图，并且利用环境中目标的位置计算自身的位姿。同步定位与地图构建，最早由Smith和Cheeseman于1988年提出，用于解决移动机器人的定位与地图构建问题，他们运用扩展卡尔曼滤波(extended Kalman filter，EKF)对状态空间中的机器人位姿和地图特征同时进行估计。

目前SLAM问题的解决方法以概率估计为主。最早的概率估计方法是基于扩展卡尔曼滤波的SLAM算法，此类算法存在难以解决的数据关联问题，且计算量过大、精度不高。文献《Unscented filtering and nonlinear estimation》(Julier S J,Uhlmann JK.Proceeding of the IEEE,2004,92(3):401-422)基于无迹卡尔曼滤波(unscentedKalman filter，简称：UKF)的SLAM算法，但利用UKF处理高维数(维数n≥4)系统时，为了避免非正定协方差矩阵的传播，需要仔细“调整”UKF中的参数。为了克服UKF在高维系统中出现的数值不稳定及精度降低问题，英国莱斯特大学的研究人员在文献《Cubature KalmanFilter based Localization and Mapping》(Kumar Pakki,Bharani Chandra,Da-Wei Gu,et al.2011, 18(part 1):2121-2125.)提出了基于容积卡尔曼滤波(Cubature KalmanFilter，简称：CKF)的SLAM算法，与基于无迹卡尔曼滤波的SLAM算法(简称：UKF-SLAM算法)相比，基于容积卡尔曼滤波的SLAM算法(简称：CKF-SLAM算法)对状态的最大估计误差降低了40％。

CKF是一种新型的非线性滤波方法，由加拿大学者Arasaratnam在文献《CubatureKalman Filters》(IEEE Transactions on Automatic Control,2009,54(6):1254-1269.)中提出。 CKF根据容积准则，通过一组具有相同权重的点经过非线性系统方程转换后产生新的点来给出下一时刻系统状态的预测，避免了对非线性模型的线性化处理，其精度达3阶。然而标准 CKF用于SLAM时，在递推过程中，存在计算量大、数值不稳定等缺点。CKF-SLAM算法在精度方面仍然有提高的余地。

本发明结合容积卡尔曼滤波，提出了一种基于自适应容积卡尔曼滤波的SLAM算法(简称：自适应CKF-SLAM算法)。该算法通过在容积卡尔曼滤波中引入自适应因子实时调整互相关协方差矩阵和自相关协方差矩阵，自适应地平衡系统模型预测信息与量测信息之间的权比，提高滤波精度，进而提高地图构建精度。通过实验验证了自适应CKF-SLAM算法，其导航定位性能优于CKF-SLAM算法和UKF-SLAM算法，证明了该算法的有效性。

发明内容