基于Cholesky分解的改进自适应EKF-SLAM算法

程 璐，黄宜庆

(安徽工程大学 电气工程学院，安徽 芜湖 241000)

即时定位与地图构建(SLAM)是近几年用于定位导航与智能体地图构建的一种广泛而有效的算法，它被用来解决在未知环境中进行创建和更新地图以及定位计算未知环境下智能体位置的计算问题[1]。在进行地图构建和同时定位时，寻找最佳或次优路径规划。在移动机器人路径规划和地图构建的领域，提出了很多优化算法。其中路径规划分为全局路径规划和局部路径规划，全局路径规划方法主要是栅格法和A*算法[2]。其中，栅格法将工作空间分解成多个简单区域，并且用0和1来表示障碍物信息，从而将复杂度物理环境转变成为计算机算法能够处理的抽象空间。并且在该抽象空间里通过搜索栅格图找到一条从起始栅格点到目标栅格点的最优路径。其中，栅格的大小影响着最优路径规划的速度和实时性，栅格较小，地图中所储存的环境信息比较多，规划速度会降低，实时性会下降；栅格较大，地图中所储存的环境信息比较少，规划速度加快，但是地图信息的减少会导致地图建立的模糊，不利于路径规划。因此，栅格的划分和路径规划速度实时性需要进行取舍。文献[3]采用栅格法建模，通过在遗传算法中进行改进负反馈来设计全局路径，并根据设计的全局路径来将自由栅格和障碍栅格互相转换从而进行局部避障。文献[4]通过将栅格法和群智能算法结合来对焊接机器人进行路径规划，以防止其与工件碰撞。通过蚁群算法来对自由栅格进行最短路径规划，设计出两个焊点之间的避障方法，然后通过分组竞争粒子群优化算法优化机器人的焊接时间，从而实现全局的路径规划。文献[5]通过简化路径坐标点和计算拐点处的旋转方向和旋转角度来优化A*算法，只保留路径点中的起点、拐点和终点来减少坐标点，从而减少运算次数。通过计算出拐点处机器人的旋转方向和旋转角度来保证机器人能够及时根据拐点信息来调整自身姿态。局部路径规划方法主要是人工势场法[6]和动态窗口法DWA[7]。人工势场法是将地图环境抽象出人造受力场，其中目标点产生“引力”，而障碍物产生“斥力”。最后通过求合力来控制移动机器人的运动。人工势场法主要用于局部环境中躲避动态障碍物，该方法结构简单，便于底层运算的实时控制，但其存在局部极值点，在狭窄环境中移动时，容易出现当临近目标点位置存在障碍物时不能发现路径等问题[8-9]。而地图构建所使用的SLAM算法种类很多，EKF-SLAM算法结合了卡尔曼滤波和SLAM算法，用来提高路径规划和地图构建的准确性和快速性，文献[10]中证明了该算法的一致性。将扩展卡尔曼滤波(EKF)的思想引入SLAM算法中形成的EKF_SLAM算法[11-12]可以很好地处理定位导航中的路径规划问题。文献[13]中通过加入时变调节因子来优化EKF-SLAM算法，提高了算法精度，但是相对运行时间有所增加。文献[14]通过引入最优平滑滤波来减少地图更新的计算量，减少算法的复杂度，但是引入的噪声会让滤波器在接近边缘处效果不佳。文献[15]通过判断量测数据中是否存在粗差来进行抗差迭代计算，提高了计算效率，但是粗差的引入可能会导致系统的收敛性降低。针对EKF-SLAM中存在的问题，使用基于改进Sage-Husa自适应算法来优化系统分析中的发散问题并提高系统的鲁棒稳定性。

1 算法研究

1.1 改进Sage-Husa自适应算法

改进Sage-Husa自适应算法是由Sage和Husa提出的自适应滤波算法[16]，有较好的应用成果[17-18]。通过在传统EKF-SLAM中加入遗忘因子，并且证明收敛性和一致性[19]，来进行对传统EKF-SLAM的优化和改进，具体过程如下：

(1)初始化。

(1)

式中，X(0)是状态方程的初始状态；P(0)是协方差估计的初始状态。

(2)当k=1,2…时，开始迭代。

①时间更新：

(2)

(3)

(4)

式中,Pk是先验协方差估计矩阵；Xk是先验估计；Q是加入的误差矩阵；ε是用来计算卡尔曼增益的残差值。

②状态更新：

(5)

(6)

(7)

(8)

式中，R为观测噪声的协方差；K为卡尔曼增益。

③噪声估计更新：

(9)

(10)

从上式可以看出，Sage-Husa自适应算法通过增加遗忘因子b来实时修正过程激励噪声和观测噪声，从而提高算法的收敛性。

1.2 基于Cholesky分解改进Sage-Husa自适应EKF-SLAM算法

EKF-SLAM算法过程是将智能体传感器获得的数据使用扩展卡尔曼滤波进行预测更新，从而获得智能体的位姿的联合概率密度。在线性代数中，Hermitian正定矩阵通过Cholesky分解形成一个下三角矩阵及其共轭转置矩阵的乘积。Hermitian正定矩阵A的Cholesky分解形式为，A=LLT，其中L是具有实对角线和正对角线的下三角矩阵。Cholesky分解和LU分解[20]相比，具有更高的稳定性和更好的精确性。

(11)

式中，矩阵L的计算公式如下：

(12)

式中，要求i>j。通过式(12)就可以计算出Hermitian正定矩阵A的Cholesky分解矩阵L。

扩展卡尔曼滤波通过对非线性系统的一阶线性化，很好地解决了非线性模型下时间更新和状态更新的收敛性和稳定性。但是由于在扩展卡尔曼滤波中的线性化是局部线性化，会导致连续随机变量的密度不再是正态分布。初始状态估计和预测模型的误差都会导致扩展卡尔曼滤波发散，并且扩展卡尔曼滤波的预测协方差矩阵值比真实协方差矩阵值低，需要增加稳定噪声来减小统计意义上不一致的风险。因此提出基于Cholesky分解改进Sage-Husa自适应EKF-SLAM算法，过程如下：

(1)初始化智能体位置、运动速度，转向角等动态性能。增加观测量的随机噪声为高斯白噪声。规划路标位置以及地标轨迹。

(2)地标提取和数据关联。利用传感器的数据来提取地标数据，并且通过数据关联来分析该地标是否为之前观测过的地标。如果已观测过，则忽略这个地标；如果未观测过，则将这个地标加入到状态更新矩阵中。

(3)位姿预测。使用Sage-Husa自适应滤波方式对位姿进行预测，其中，式(13)是预测状态估计，式(14)是预测协方差估计，两者为时间更新方程。

(13)

(14)

当迭代次数增加的时候，如果整个滤波过程是收敛的，那么误差的后验协方差估计矩阵Pk会逐渐减小并趋向零，则可以将式(5)改为式(15)

(15)

此时暂时忽略不计测量噪声的协方差估计矩阵的无偏性，即忽略不计Pk的影响，从而提高算法的运行速度并且其收敛性也不受影响。接着按照式(3)、式(4)进行预测。

(4)数据更新。假设P=SST，矩阵P经过Cholesky分解，其中S是下三角矩阵。将扩展卡尔曼滤波中的误差后验协方差估计矩阵Pk用S的积分来代替，可以使滤波器的精度提高。则基于Cholesky分解改进Sage-Husa自适应的更新方程如下：

(16)

(17)

(18)

(19)

(20)

(5)增加新特征到状态矩阵中。将新的地标加到状态矢量X和协方差矩阵P中，用于更新状态预测。

2 仿真实验

分别设置障碍物个数为65个、105个，路径点个数为17个，移动智能体从原点(0,0)的位置沿着路径点规划好的线路进行运动，其中添加的噪声是服从(0,1)分布式高斯白噪声。分别采用EKF-SLAM算法，改进自适应EKF-SLAM算法和基于Cholesky分解改进自适应EKF-SLAM算法来进行实验。在65个障碍物的情况下3个算法比较如表1所示。在65个障碍物的情况下3个算法路径规划的轨迹图如图1所示。在65个障碍物的情况下，分别使用3种不同算法进行路径规划和给定实际路径规划的最大误差值随时间的变化如图2、图3所示。在105个障碍物的情况下3个算法比如表2所示。在105个障碍物的情况下3个算法路径规划的轨迹图如图4所示。由于起始和终止的最大误差跳跃幅度比较大，因此选取最大误差相对平稳的时间段进行绘图。在105个障碍物的情况下，分别使用3种不同算法进行路径规划和给定实际路径规划的最大误差值随时间的变化如图5、图6所示。

通过表1、表2，图1～图6分析可得，随着障碍物数量的增加，3个算法的运行时间都有所增加。改进自适应EKF-SLAM算法相对于传统EKF-SLAM算法，由于添加了遗忘因子，预测路径和智能体实际运动路径的最大误差有所减小，并且其运行时间也会有所降低。而基于Cholesky分解的改进自适应EKF-SLAM算法，通过Cholesky因式分解的方法提高了运行精度，并且通过改进自适应算法优化了运行速度，使得运行时间得以减少，其在X轴和Y轴的平均最大误差也比传统EKF-SLAM和改进自适应SLAM要小，精确性和稳定性更好。

表1 65个障碍物的情况下3个算法比较

表2 105个障碍物的情况下3个算法比较

图1 65个障碍物的情况下3个算法的轨迹图图2 65个障碍物的情况下3个算法X轴最大误差

图3 65个障碍物的情况下3个算法Y轴最大误差图4 105个障碍物的情况下3个算法的轨迹图

图5 105个障碍物的情况下3个算法X轴最大误差图6 105个障碍物的情况下3个算法Y轴最大误差

3 总结

对于传统的EKF-SLAM算法所产生的运行时间较长和运行精度较低的问题，Sage和Husa提出了自适应滤波算法，通过在传统EKF-SLAM算法中增加遗忘因子来优化算法，提高算法的计算速度和计算精度。在此基础上，对自适应EKF-SLAM中的误差后验协方差估计矩阵进行Cholesky分解，能够更进一步提高算法的运行速度。通过MATLAB的仿真实验得出，基于Cholesky分解的Sage-Husa自适应算法的运行结果更接近于真实值，估计准确性更高，收敛速度也更快，运行时间更短。