基于改进IEKF的室内移动机器人定位算法研究.pdf

众创空间科技创新与应用Technology Innovation and Application2023 年 27 期基于改进 IEKF 的室内移动机器人定位算法研究刘钰芃，王斌宇，张淞淏，杨伊帆，王晓燕*（西安建筑科技大学 机电工程学院，西安 710055）移动机器人定位技术是衡量移动机器人自主导航能力的重要指标1。近年来，随着国内外学者对多传感器信息融合定位技术研究的深入，多传感器信息融合定位算法得到了快速发展，包括扩展卡尔曼滤波定位2、无迹卡尔曼滤波定位3和粒子滤波定位4。其中，基于扩展卡尔曼滤波的多传感器信息融合定位技术在无人机探测、水下搜索和无人驾驶等领域的应用最为广泛。扩展卡尔曼滤波可以有效解决非线性系统估计的问题，但线性化过程会带来误差。为了解决这个问题，文献5提出了一种基于多传感器信息融合的迭代扩展卡尔曼滤波定位算法，将每次系统测量更新得到的系统后验估计作为下一个扩展卡尔曼滤波测量更新过程的先验估计进行更新计算，提高了滤波精度。文献6提出了一种基于新息估计的 AEKF 算法。该算法利用系统的新息矩阵，自适应调整状态噪声和观测噪声的协方差矩阵，从而抑制状态误差的增加。自适应 IEKF 算法修正了噪声协方差，但不可避免地会累积计算误差，可能导致卡尔曼滤波增益失去加权调整的功能。本文提出一种改进的 IEKF 定位算法。该算法通过新息估计方法自适应调整系统的状态噪声协方差和观测噪声协方差，减小恒定噪声协方差的影响；引入遗忘因子调整误差协方差预测值，限制了卡尔曼滤波的长度，减小了旧数据的累积计算误差；最后，采用 Lev原enberg-Marquardt（LM）方法优化 IEKF 的迭代过程，以提高全局估计的收敛性。1系统模型本文以两轮差分移动机器人为研究对象。系统的状态模型由里程计模型建立，观测模型由激光雷达测距传感器建立。模型图（图 1）和计算公式如下。基金项目：西安建筑科技大学大学生创新创业训练计划项目（S20220701080）*通信作者：王晓燕（1976-），女，博士，副教授。研究方向为机器人智能控制。摘要：该文提出一种基于改进 IEKF 的室内移动机器人定位算法。该算法通过基于新息的协方差估计器自适应实时调整状态噪声和观测噪声;引入遗忘因子调整误差预测协方差矩阵，提高观测数据的利用率，消除旧数据累积误差对系统的影响;采用 Leven原berg-Marquardt（LM）方法优化 IEKF 的迭代过程。最后，以两轮差分移动机器人为研究对象，对仿真结果进行验证。仿真结果表明，所提定位算法收敛性好，定位精度高。关键词：定位算法；迭代扩展卡尔曼滤波（IEKF）；自适应迭代扩展卡尔曼滤波（AIEKF）；Levenberg-Marquardt；遗忘因子中图分类号院TP183文献标志码院A文章编号院2095-2945渊2023冤27-0037-04Abstract:This paper proposes an indoor mobile robot positioning algorithm based on the improved iterative extended Kalmanfilter(IEKF).The algorithm adaptively adjusts the state noise and observation noise in real time through an innovation covarianceestimator;and a forgetting factor is introduced to adjust the error prediction covariance matrix to increase the utilization of theobserved data and eliminate the influence of the accumulated error of the old data on the system;and Levenberg-Marquardt(LM)method is used to optimize the iterative process of IEKF.Finally,two wheeled differential mobile robot is taken as the researchobject to verify the simulation results.The simulation results show that the proposed positioning algorithm has good convergenceand high positioning accuracy.Keywords:location algorithm;iterative extended Kalman filter(IEKF);adaptive iterative extended Kalman filter(AIEKF);Levenberg-Marquardt;forgetting factorDOI：10.19981/j.CN23-1581/G3.2023.27.00937-2023 年 27 期众创空间科技创新与应用Technology Innovation and Application图 1两轮差速移动机器人的状态模型两轮差速移动机器人的模型如图 1 所示。R 是机器人运动的弧半径，A 为左右轮轴的中心点，D 为移动机器人左右轮之间的距离，驻x、驻y、驻兹 分别为移动机器人位置在采样时间 驻t 内的横坐标、纵坐标和方向角的变化。移动机器人左右驱动轮的行驶距离为 驻SL和 驻SR。两轮差速移动机器人在横坐标和纵坐标的位置变化如下驻x=AA忆cos（兹+驻兹/2）=2Rsin（驻兹/2）cos（兹+驻兹/2），（4）驻y=AA忆sin（兹+驻兹/2）=2Rsin（驻兹/2）sin（兹+驻兹/2）。（5）将系统的输入定义为 U（k）=驻S，驻兹T，系统在 k 时刻的状态向量由 Xk沂xkyk兹kT表示，则系统的状态方程可表示为。（6）观测模型和计算过程如图 2 所示。图 2两轮差分移动机器人的测量模型由图 2 所知，激光雷达观测到的地标（xi，yi）与机器人在 k 时刻位置xkyk兹kT的相对位置关系。d 是地标与移动机器人的相对距离、琢 是地标相对移动机器人的夹角、琢-兹k是地标相对于机器人移动方向的角度。激光雷达返回的地标观测信息是机器人与地标之间的相对位置信息（d，琢）。观测模型可以表示为。（7）2改进的IEKF定位算法2.1迭代扩展卡尔曼滤波本文将两轮差分移动机器人建立的状态方程和观察方程离散化后表示如下7，（8），（9）式中：Xk沂xkyk兹kT是系统的状态向量；Zk沂d，琢T是系统的观察向量；椎k，k-1是系统的状态转换矩阵；h（）是测量函数；wk-1是状态噪声，并且 wk-1沂N（0，Qk-1）；vk是系统观测噪声，和 vk沂N（0，Rk）；wk-1并且 vk是高斯白噪声，E（wk-1，vk）=0。基于 IEKF 算法的多传感器信息融合过程如下8，（10），（11），（14），（15）式中：是系统的一步态估计，P1k,k-1是系统的一步误差协方差预测；Kik是卡尔曼滤波的增益；eik是创新估计向量；是系统的后验状态估计，是系统的预测值是后验误差协方差；是观察方程的雅可比矩阵；i 是迭代的 IEKF 编号（i=1，2，3，n）。IEKF 将每个扩展卡尔曼滤波过程的后验估计作为先验估计，代入多次迭代的测量更新过程中，使测量信息得到充分利用，估计值在计算中接近实值。2.2基于遗忘因子的改进自适应 IEKF 算法由于计算误差的累积，系统误差协方差可能会失去其非负定性和对称性，从而导致卡尔曼滤波增益矩阵的损失。为了解决这一问题，本文在改进自适应IEKF 的基础上引入遗忘因子来调整系统误差协方差，限制卡尔曼滤波的存储长度，减少过去测量数据的内RLRL/2/SSSSSDRSqq ，（1），（2）。（3）1T,sinsin/coscos/kkkkkkkkXF X UwXxySxySqqqqqqqqqqq 22arctanikikkikkikxxyydzyyxxaq ,111hkk kkkkkkXwxvXZ1,1,111T,1,11,111TT,1,1,11,1,11,1k kk kkk kk kkk kkiiiikk kkkk kkkiikkk kiiiiiiiiikk kkkkkkk kkiiikkkk kXXPAPAQKPPRh XXXhXXXPIKPHHHeZKeHH，（12），（13）1ikP1,1k kX1ikX1ikX/iiikkkh XXH38-众创空间科技创新与应用Technology Innovation and Application2023 年 27 期存，增加当前测量数据的应用，从而进一步提高系统的定位精度。目前，计算遗忘因子常用的方法有 2 种：一步法和简化算法。为了减少计算量，提高滤波器的估计速度，本文选取简化算法作为遗忘因子的计算方法，计算过程如下计算遗忘因子：，（16）式中：姿k是遗忘因子；trace（）是矩阵迹线的计算；Nk计算方法 Mk如下，（17），（18）式中：Ck是 k 时刻新息协方差的理论值，计算如下。（19）新息协方差一般用窗口法估计即，（21）式中：en是新息残差。（22）为了更好地利用新的测量值来修改状态估计值，避免模型误差对滤波新息值的影响，本文采用公式（22）作为遗忘因子加法，因为其可以更好地把握系统预测误差协方差的整体变化。由公式（21）可知，C赞k会随着 en的增大而增大，同时遗忘因子也会增大，从而提高滤波精度。2.3基于 LM 优化的改进自适应 IEKF 算法由于 IEKF 的一阶泰勒展开中的线性化误差，系统状态模型与实际测量结果不匹配。在迭代过程中，系统的误差协方差将小于实际值，这将影响估计的稳定性。针对该问题，本文采用 Levenberg Marquardt（LM）方法优化了 IEKF 的迭代过程，提高了 IEKF 算法迭代过程的稳定性和全局收敛性。LM 方法的主要思想是引入修正参数 琢i，在每次迭代中调整系统误差协方差预测矩阵，然后使用调整后的预测协方差矩阵迭代更新计算。系统的误差协方差预测矩阵调整如下。（23）综上所述，基于 LM 优化的具有遗忘因子的改进自适应 IEKF 算法的流程表示如下。步骤 1：初始化系统状态估计X赞0和系统误差协方差预测矩阵 P0；步骤 2：通过方程（10）和（11）计算系统状态的一步预测值和一步误差协方差预测值，完成时间更新过程；步骤 3：Kk从系统状态预测值和优化的系统预测误差协方差计算卡尔曼滤波增益、新息残差向量 ek和观测方程雅可比矩阵 Hk，然后更新X赞k,k-1误差协方差P赞k,k-1估计和状态估计，完成测量更新过程；步骤 4：通过卡尔曼滤波增益 Kk、新息残差向量 ek和观测方程雅可比矩阵 Hk，通过自适应调整状态噪声协方差 Qk-1和测量噪声协方差 Rk，分别对步骤 2 和步骤 3 进行调整；步骤 5：通过方程（16），（17），（20）和方程（21）从新息残差向量 ek和观测方程雅可比矩阵 Hk计算遗忘因子 姿k；然后将遗忘因子 姿k应用于步骤 2；步骤 6：利用 LM 算法优化P赞k,k-1系统误差协方差的预测值；步骤 7：重复步骤 3 至步骤 6，当迭代次数达到最大迭代次数时，退出迭代，输出和。3仿真实验与分析3.1仿真设计假设在静态室内环境（障碍物保持静止）下，两轮差分移动机器人的运动速度为 v=3.0 m/s、角速度为 rateG=60伊仔/180 rad，激光雷达测距传感器的最大观测距离20 m，观察角度范围 360毅。状态噪声为 滓v=0.6 m/s，滓g=1.0伊仔/180 rad，观测噪声 滓R=0.2 m/s，滓B=1.0伊仔/180 rad，两轮差速移动机器人 2 个驱动轮之间的距离 D=0.5 m。总采样时间为 60 s，采样间隔为 0.1 s，即 驻T=0.1 s 步长。设置 LM 校正参数 琢i=0.1，滑动窗口 S=5。开展50 次蒙特卡罗实验，以采样时间内 IEKF、LM-IEKF、LM-AIEKF、LM-AIEKF 和 LM-FAIEKF 的估计值和实际值的 RMSE（均方根误差）平均值为标准，测量算法的定位精度，RMSE 越小，定位精度越高。RMSE 平均值计算如下，（24），（25）式中：Xji,k是 k 时刻第 j 次蒙特卡罗模拟的第 i 个状态向量的实际值，Xji,k第 j 次蒙特卡罗模拟在 k 时刻的第i 状态向量的估计值，蒙特卡洛模拟的次数 M，以及模拟步骤的总数 N。11,1,1,1,1iiiik kk kk kik kIPPIPaPtracemax 1,tracekkkNMlTT,11|,1T1kkk kkk kkkkkkkkMHPHNCH QHRTT,1kkkkk kkkCE e eH PHRT11=kknnnCe ek，（20）T1kkkkkkNCH QHRT-1,11,11k,kkk kkk kkPAPAQl,ik kX|ik kP1/22,21,111=MjjRMSEki ki kjNMERMSE kkEXXMEEN，1,1k kX1,1k kP39-2023 年 27 期众创空间科技创新与应用Technology Innovation and Application3.2仿真结果随着定位过程的进行，状态噪声和观测噪声会受到环境因素的影响。在本实验中，前 300 个仿真步长中状态噪声和观测噪声的协方差设置为原始步骤的一半，即 0.5Q 和 0.5R；后 300 个仿真步长中的状态噪声协方差和观测噪声协方差与初始值一致，通过 50 次蒙特卡罗实验验证了每种算法的收敛性和定位精度。结果如图 3 所示。渊a冤IEKF 误差渊b冤LM-IEKF 误差渊c冤LM-AIEKF 误差渊d冤LM-FAIEKF 误差图 3噪声变化情况下不同算法定位误差图 3 比较了状态噪声和观测噪声变化时 IEKF、LM-IEKF、LM-AIEKF 和 LM-FAIEKF 的定位均方根误差。从图 3 可以看出，4 种算法的定位误差随着系统状态噪声误差和观测噪声误差的增加而增大。其中，LM-IEKF 算法比 IEKF 算法具有更好的收敛性和稳定性；LM-AIEKF 算法的定位精度优于 LM-IEKF 算法。50 次蒙特卡罗实验中不同算法的 RMSE 平均值和平均运行时间见表 1。表 1RMSE 平均值和平均运行时间由表 1 可知，通过 50 次蒙特卡罗实验，与 IEKF算法相比，LM-IEKF、LM-AIEKF 和 LM-FAIEKF 的定位精度分别提高了 39.59%、46.57%、84.62%。本文提出的改进 IEKF 算法（LM-FAIEKF）的定位精度明显优于其他 3 种算法。虽然该算法的平均运行时间高于 IEKF和 LM-IEKF 算法，略小于 LM-AIEKF 算法，但具有良好的收敛性和鲁棒性，符合预期要求。4结论本文提出一种改进的室内移动机器人多传感器融合定位 IEKF 算法。解决了姿态估计中恒定噪声协方差误差、前一数据累积计算误差、IEKF 算法收敛性差等问题。在 2 种仿真场景中，比较了 IEKF、LM-IEKF、LM-AIEKF 和 LM-FAIEKF 的性能。仿真结果表明，所提算法具有较高的稳定性和定位精度，符合室内移动机器人的定位要求。参考文献院1 CAMPBELL S，OMAHONY N，CARVALHO A，et al.Lo原calization techniques for Mobile Robots A Review C/pro原ceedingsofthe20206thInternationalConferenceonMechatronics and Robotics Engineering（ICMRE），F，2020.2 张雨婷，陈璟.采用 EKF 与 PF 的室内融合定位技术J.传感技术学报，2020，33（2）：245-251.3 YAN J，ZHAO H，LUO X，et al.Asynchronous Localiza原tion of Underwater Target Using Consensus-Based UnscentedKalman FilteringJ.IEEE Journal of Oceanic Engineering，2019，PP（99）：1-16.4 HUMBERTO M，PABLO B，DAVID H.Sensor Modeling forUnderwaterLocalizationUsingaParticleFilter J.Sensors（Basel，Switzerland），2021，21（4）.5 张敏利，张万绪.IEKF 滤波在移动机器人定位中的应用J.电子技术应用，2013，39（2）：74-77.6 张志勇，张淑芝，黄彩霞，等.基于自适应扩展卡尔曼滤波的分布式驱动电动汽车状态估计J.机械工程学报，2019，55（6）：156-165.7 曲志昱，王超然，孙萌，基于改进迭代扩展卡尔曼滤波的 3 星时频差测向融合动目标跟踪方法J.电子与信息学报，2021，43（10）：2871-2877.8 任其亮，程昊东.针对小车状态感知的卡尔曼滤波多传感器融合算法J.计重庆理工大学学报（自然科学），2022，36（11）：176-182.算法 RMSE 平均值/m 运行时间/ms IEKF 0.161 9 0.026 7 LM-IEKF 0.097 8 0.053 3 LM-AIEKF 0.086 5 0.092 6 LM-FAIEKF 0.024 9 0.087 4 0100200300400500600Simulation step size21.510.500100200300400500600Simulation step size21.510.500100200300400500600Simulation step size21.510.500100200300400500600Simulation step size21.510.50error of IEKFerror of LM-IEKFerror of LM-FAIEKFerror of LM-AIEKF40-