2019-05-16

基于无迹卡尔曼滤波的SLAM算法

目的：

求解SLAM问题时一般将移动机器人的位姿状态和环境地图中的路标位置组合成联合的系统状态向量，并利用SLAM算法对联合系统状态向量作最小均方误差估计，并把移动机器人在移动过程中新观测到的路标向量加入到联合状态向量，以此来逐步构建环境地图并同时更新机器人在环境中的位置。本文要研究的问题是通过采用无迹卡尔曼滤波算法来提高SLAM定位估计的精度。

SLAM问题的UKF实现：

在matlab上实现SLAM仿真，在一张自定义地图中放置多个路标点，设置小车的速度以及传感器能够检测的范围，求出系统非线性状态方程和非线性观测方程。初始化，根据K时刻状态估计值 和协方差矩阵对n维状态向量采样获得2n+1个Sigma点。]时间预测，通过非线性模型对Sigma点的状态进行预测，利用这些Sigma点集的状态值加权求和得到均值和协方差预测值。可用和分别表示Sigma点的均值和协方差对应的权值。

观测更新，通过非线性的观测方程对观测量的均值和协方差进行预测，通过加权求和得到。     

状态更新，利用K+1时刻获得的观测值对状态变量进行更新，定义新息，滤波增益矩阵，更新状态和协方差。

目前进展:

在相同的地图下，分别对EKF-SLAM和UKF-SLAM仿真作了比较，由于计算量较大的原因会导致计算机运行速度变慢，所以每隔500个计数点取一次数据求取误差均值，并求取34个点，并计算两种算法的所用时间。结果证明，UKF的误差均值只有1.815m，EKF的误差均值有3.548m，UKF算法明显优于EKF算法，运行时间UKF用了182秒，而EKF用了212秒，运行时间也明显减短了。

总结：

因为相对于扩展卡尔曼算法，无迹卡尔曼算法不需要对非线性系统的线性化过程忽略高阶项，也不需要获得Jacobian矩阵，采用UT变化能提高定位的精度。实验结果无迹卡尔曼算法计算速度和定位平均误差都会减小。所以可以针对于UKF的基础上改进算法来进一步提高定位的精度。