《移动机器人目标定位跟踪滤波算法研究》9800字

移动机器人目标定位跟踪滤波算法研究目录TOC\o"1-1"\h\u5972移动机器人目标定位跟踪算法研究 117422第一章绪论 127677第二章目标跟踪模型及滤波器算法理论 430191第三章基于卡尔曼滤波（KF）的目标跟踪研究 1127759第四章基于扩展卡尔曼滤波（EKF）的目标跟踪研究 1416185第五章基于无迹卡尔曼滤波（UKF）的目标跟踪研究 183432第六章结论与展望 21第一章绪论1.1课题背景随着计算机技术的进一步发展,机器人在军用领域和民用领城也会发挥巨大的功能。机器人能够替代人类进行"无聊,重复,危险"的工作任务,从而减少了人员的工作压力以及暴露于危险的环境条件下的时间。在当今社会,由于科技的迅勐猛进,机器人在我们日常生活中早已无处不在,并几乎走进了各行各业,如家庭生活中的清扫机器人,制造业中的搬运机器人，，快递行业中的分拣机器人，军事领域的拆弹机器人等。特别是随着世界人口呈现严重的老龄化，人口红利逐步下降，人工成本加剧，多数行业呈现用工难的问题，机器人取代人类完成工作的需要日益加大。近年来的席卷全球的新冠疫情，更是让机器人的无人化，智能化成为热点话题。移动机器人是机器人技术最开始的分支，在军事，工程，医疗，教育等方面有着广阔前景，所以世界上都开始重视研究移动机器人。因此。不论在未来的民用还是军用方面，若移动机器人能根据设定的任务，准确的进行目标定位跟踪，就可以极大的提高任务完成效率。1.2研究现状移动机器人的目标定位跟踪主要的两个主要研究方面是：一是地面轨迹跟踪问题，对自身系统状态的估计，调整传感器平台位置确保对目标的覆盖：二是移动机器人路径规划问题，对为了满足既定任务要求，如生成最优路径和规避威胁区域等。1.2.1移动机器人目标定位跟踪研究概述目标定位跟踪的要解决的第一个问题就是目标状态估计，是指通过传感器信息收集系统得到的观测状态和目标的观测信息，从而确定目标设定坐标系下信息：位置、方向、速度等。目标定位跟踪的目的是为了，准确估计目标状态，然后移动机器人将收集到的目标状态估计值作为其控制模块的参考。当前在目标跟踪定位方面常用的估计技术有，a-B和a-B-y法波，Wiener法波卡尔曼滤波，扩展卡尔曼滤波，无迹卡尔曼滤波，加权最小二乘法等滤波器。（1）Wiener滤波：是一种用在频域设计的最优秀的滤波器，利用最小均方误差条件试验和确定滤波器的冲激响应H和系统两数W，转换为求取方程最优解的的问题。Wiener滤波对于一维的稳定的信号比较适用。（2）加权最小二乘法，是一种根据事先设定的信息的收集使用的滤波，当先验统计特性未知时，采用最小二乘法滤波，当只知道观测误差的统计特性，选择马尔可夫估计。（3）a-B和a-B-y法波，是两种工程常用的跟踪滤波器算法，优点是少量的循环滤波计算和离线计算的增益矩阵。（4）卡尔曼滤波，扩展卡尔曼滤波，无迹卡尔曼滤波的滤波器将在后续章节进行详细说明。1.2.2各国移动机器人发展现状第一是美国，曲线的发展道路。第一个工业机器人是美国制造的，60年代就有商品机器人销售，但是之后没有重视机器人技术，偏重于理论，战略意识不强，导致在1973年美国开始从日本进口机器人，来满足自身市场需求。2006年之后，由于机器人市场份额大，发展前景好，美国又开始重视机器人的技术发展。第二是日本，技术和国家经济发展相融合的实用的发展道路。日本于1967年开始在美国引进机器人，到80年代初，已经发展成为机器人王国，机器人众多，机器人生产线规范，工业体系完善，处于世界领先水品。第三是德国，技术促进经济的发展道路。1986年，德国机器人数量位列世界前三，超过10000台.1987年德国已经有42家机器人公司。现在机器人产业给德国产生的经济收入占到国家收入的1.2％。第四是中国，政策作用下的高速发展道路。2018年开始，5年内年均增速接近30%，发展非常强劲，同时伴随着市场规模的不断扩大、应用领域的快速拓展、不断加快核心部位零件，部分领域的突出，特别是在创新型企业的大量涌现、企业，我国移动机器人机器人在产业发展上呈现三个明显的特征：是三类机器人发展不断提速。是产业集群现象愈发明显。区域产业各自优势加速凸显。在国内的产业集聚过程中，。1.2.3目标定位跟踪的传感器移动机器人的目标定位跟踪往往需要满足较高的定位精度、较低的定位延迟、较高的定位输出频率、易融合多种传感器数据，以及安装部署容易，对环境改动小等要求。激光雷达传感器很符合移动机器人目标定位跟踪的要求。激光传感器就是利用激光技术来测量信息的一种传感器。它由三部分组成，一是发出激光信号的激光器，二是接收激光信号的激光检测器，三是对收集信息进行测量的测量电路。他的显著特色就速度快，精度高，测量程度高，抵抗光和温度的能力强。移动机器人定位方面的激光传感器最多的就是激光雷达传感器。激光雷达数据进行各种分类识别，进行跟踪处理，能够非常有效的获得周围环境最主要信息。1.3本文主要工作内容及结构安排本文主要研究的是基于卡尔曼滤波的移动机器人目标定位跟踪的方法，基于匀速直线运动模型，进行真实轨迹的预测，并且在PC平台上实现定位过程的仿真。然后比较卡尔曼滤波，扩展卡尔曼滤波，无迹卡尔曼滤波对移动机器人目标定位的仿真效果。本文分为六章，各章的具体研究内容如下：第一章：简要介绍了本课题研究的意义和背景，分别介绍了主流国家机器人发展现状和趋势，以及移动机器人目标定位的传感器，并说明本文的研究内容和结构。第二章：详细介绍了目标跟踪的滤波原理，重点阐述了卡尔曼滤波器，扩展卡尔曼滤波器，无迹卡尔曼滤波器的原理和算法步骤；设定移动机器人的仿真运动模型和主要参数设置。第三章基于卡尔曼滤波的目标跟踪研究，主要介绍了卡尔曼滤波的算法实现，基于MATLAB的仿真实现，并对卡尔曼滤波前后的实验数据进行分析和比较。第四章基于扩展卡尔曼滤波的目标跟踪研究，主要介绍了扩展卡尔曼滤波的算法实现，基于MATLAB的仿真实现，并对卡尔曼滤波和EKF的实验数据进行分析和比较。第五章基于无迹卡尔曼滤波的目标跟踪研究，主要介绍了无迹卡尔曼滤波的算法实现，基于MATLAB的仿真实现，并比较UKF和EKF的实验数据进行分析和比较。第六章主要是对全文的总结，分析实验不足，并且在之后工作的改进。第二章目标跟踪模型及滤波器算法理论2.1机动目标跟踪原理目标跟踪过程为：量测值Y和状态预测向量HX(K+1/k)构成残差向量ｄ，机动检测或机动辨识根据ｄ的改变进行更新，然后设定的算法逻辑调整滤波增益与协方差矩阵，最后由卡尔曼滤波算法计算目标的系统估计状态和预测状态数值，就完成了整个的目标跟踪过程。表2-1机动目标跟踪原理图如图2-1所示，图中目标动态特性由包含位置、速度和加速度的状态向量x表示，量测值Y被假定为含有量測噪声V的状态向量的线性组合(HX＋V)。残差向量d为量测值丫与状态预测量HX(k+1/6)之差。2.2建立移动机器人移动模型在移动机器人领域中，移动机器人的路径优劣与移动机器人的模型的精准程度关系重大。通常把移动机器人当成一个设定坐标系中的质点，但是移动机器人的真实物理特征影响，因此本文采用简单的匀速直线运动模型。表2-1坐标系统为了实现更好的实现移动机器人的定位，我选用的是匀速直线运动的移动机器人来做运动的模型，使用二维的平面展示，提供四个维度的参数：X方向的机器人的速度和距离，Y方向的机器人的速度和距离，即（x,y,V(x),V(y))。2.3滤波器算法理论本节主要是介绍基于卡尔曼滤波目标定位跟踪中的数学原理知识，包含了卡尔曼滤波的数学理论知识，扩展卡尔曼滤波的数学理论知识，无迹卡尔曼滤波的数学理论知识，对于线性的目标跟踪用到是卡尔曼滤波，对于非线性的目标跟踪用到EKF和UKF。2.3.1卡尔曼滤波(KF)卡尔曼滤波常用于动态多变化系统中的状态估计，是一种通用性强的自回归滤波器。其发明者鲁道夫.E.卡尔曼在一次访问NASA的时候，发现阿波罗计划中一个难点，所以提出一种用来预测轨迹的滤波器，他可以帮助高效预测轨迹，辅助导航。NASA最终使用了这个滤波器，完成阿波罗登月任务，卡尔曼滤波器由此得名。卡尔曼滤波（KF）在数学中是用来估计线性系统最优状态的一种方法。在移动机器人定位方面主要是通过传感器的观测信号，形成的观测状态，和系统算法估计出来的估计状态进行分配比例，从而达到最接近真实的实际定位的参数，并且配合进行位置预测和观察系统的输入输出数据，从而构成系统真实状态的线性方程。传统的卡尔曼滤波算法（FK）适用于高斯分布的系统模型上，不适用于复杂的非线性系统.状态方程描述：线性卡尔曼滤波的状态方程： X(k+1)=δX(k)+σΓW(k) （2-1）线性卡尔曼滤波的观测方程： Y(k)=HX（k)+V(k) （2-2）公式（2-1），（2-2）中，ｋ代表的是离散时间，系统在时刻k的状态就是X(k)，系统在ｋ是时刻的观测状态为Y（ｋ），系统输入为W(k)，系统状态转移矩阵为δ，系统噪声驱动举证为Γ，系统观测矩阵为H。首先预测下一状态，根据系统给定的过程模型：对状态方程取影射： X(k+1|k)=ϕX对观测方程取影射： Y(k+1|)=HX于是就有了 Y(k+1|k)=HX正常运行时卡尔曼录波器运算过程如下：状态的一步预测方程： X(k+1|k)=ϕX然后是经过系统，进行状态更新： X(k+1|k+1)=X(k+1|k)+K(k+1)ε滤波的增益矩阵： K(k+1)=P(k+1|k)HT[HP(k+1|k)H进行下一步的协方差阵： P(k+1|K)=ϕP(k|k)ϕ协方差更新： X(0|0)=u在一个卡尔曼滤波的周期里面，卡尔曼滤波有两方面的更新，时间的更新换代和观测的更新换代。上面的公式2-6在计算过程中对于状态的移动数学矩阵和高斯白噪声输入数学矩阵和过程噪声方差的数学矩阵，都是进一步转移的。噪声的特殊情况处理，当含有控制量的线性系统描述时改为下列公式：线性卡尔曼滤波状态方程： X(k)=AX(k−1)+BU(k−1)+σW(k-1) （2-10）线性卡尔曼滤波观测方程： Y(k)=HX(k)+V(k) （2-11）2.3.2扩展卡尔曼滤波（EKF）扩展卡尔曼滤波（EKF）是对卡尔曼滤波的进一步创新，是对非线性体统进行线性化，通过展开Taylor级数，然后忽略二阶以上的高级项，以低介导师对非线性系统逼近，再利用卡尔曼滤波理论求取近似的线性系统方程。基于EKF的目标定位跟踪需要主要有三个方面：系统的状态预测、系统的观测更新和系统的状态放大。对于移动机器人的目标定位跟踪来说，首先确定移动机器人的初始位置，然后经过系统的预测更新算法得出预测位置，然后观察收集关于周围环境的标志信息，利用之前计算的观测值和实际得到的观测值进行推导，计算的到参数K，再把K带入到系统进行矫正，得到更新的协方差方程，运动位置和运动速度。再这个过程中，系统在不断循环矫正，从而确保了移动机器人的精确定位。扩展卡尔曼滤波（EKF）算法是对非线性系统线性化，基本过程是：Taylor级数展开，舍弃非线性部分，然后对状态变量求取雅各比矩阵，进行替代系统的状态矩阵和观测矩阵，这时系统的就可以按照卡尔曼滤波算法进行处理。非线性函数在Taylor展开式的矩阵形式中会用到Hessian矩阵，所以以下先列出使用EKF算法所需要用到的数学理论。Ａ：泰勒(Taylor）级数：在数学中，泰勒级数（英语：Taylorseries）用无限项连加式——级数来表示一个函数，这些相加的项由函数在某一点的导数求得.Taylor级数展开的数学理论是将一个在（x=x0)关于（x=x0)的n次多项式通近函 f(x)=f(x0)0!+ Rn(x)=Taylor展开式在等式右边，剩余的RnxＢ：雅可比（Jacobian）矩阵向量分析中，雅可比矩阵是函数的一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式。是一个从n维欧氏空间映射到到m维欧氏空间的函数。这个函数由m个实函数组成，这些函数的偏导数(如果存在)可以组成一个m行n列的矩阵，这个矩阵就是所谓的雅可比矩阵状态空间模型： ∂y1∂x1⋯状态和观测方程 F(x)≈F(p)+J Xk+1=f(Xk Zk=ℎ(Xk为了取得状态转移矩阵，要在Wk=0点取值；为了取得观测矩阵，要在Ｃ：海森矩阵（Hessianmatrix）是一个多变量实值函数的二阶偏导数组成的方块矩阵，假设有一实数函数f(x1,x2,…,xn),其中x=(x1,x2,...xn),即H(f)为： ∂2fD：扩展卡尔曼滤波（EKF）算法实现步骤如下：非线性系统局部线性化：离散非线性系统动态的方程可以表示为： X(k+1)=f[k,X(k)]+G(k)W(k) （2-19） Z(k)=h[k,X(k)]+V(k) （2-20）扩展卡尔曼滤波是利用非线性数学函数的局部线性化的特殊性质，将非线性系统线性化。将公式进行泰勒级数展开得状态方程为： X(k+1)=ϕ(k+1|k)X(k)+G(k)W(k)+ϕ同卡尔曼滤波相比，增加了状态方程ϕ(k)将公式进行泰勒级数展开得观测方程为： Z(k)=H(k)X(k)+y(k)+V(k) （2-22）扩展卡尔曼滤波的递推公式为： X(k|k+1)=f(X(k|k)) P(k+1|k)=ϕ(k+1|k)P(k|k)ϕT K(k+1)=P(k+1|k)HT(k+1)[H(k+1)P(k+1|k) P(k+1)=[I−K(k+1)H(k+1)]P(k+1|k) （2-26）上述公式中，滤波初值和滤波误差方差矩阵的初值分别为： X(0)=E(X(0)],P(0)=var[X(0)] （2-27）假设状态变量有n维，X=[ ϕ(k+1)=∂F∂ H(k+1)=∂h∂x2.3.3无迹卡尔曼滤波（UKF）无迹Kalman滤波，是对于卡尔曼滤波中的一步预测方程，使用UT变换，用一系列确定样本来逼近状态的后验概率密度，不需要对Jacobian矩阵进行求导。是一种采样点逐渐逼近真实非线性分布的方法。Ａ：UT变化UT变换是用固定数量的参数支近似一个高斯分布，其实现原理为：确定点集Q来近似代表系统输入状态的特征，然后对每一个SIGMA点进行非线性变换，再根据加权计算得到变化后的统计特征。Ｂ：UT变化的算法实现非线性系统，状态向量为N，用他的均值和方差计算2n+1个Sigma点X的权值ω来计算y.计算2n+1个Sigma点的计算公式：x公式中，（P）T计算采样点的权值：ω其中降低预期状态误差的是：λ=a2(n+k)−n,要想控制分布状态，需要对a进行适当的选取，k备用参数，是保障矩阵（n+对于非线性系统：X(k+1)=f(x(k)),W(k)计算UKF的几步步骤为：第一步：获得采样点和权值X第二步：计算2n+1个点的预测X第三步：状态一步预测和其对应的协方差，对比传统的卡尔曼滤波，多出加权平均值，根据一步预测的值，进行UT变换，通过加权算法，计算出系统的均值和系统的协方差。第四步：计算增益矩阵。第五步：计算得系统新的状态和协方差第三章基于卡尔曼滤波（KF）的目标跟踪研究3.1卡尔曼滤波（KF）目标跟踪算法运动模型状态包括纵向、横向的速度和位置，设定系统状态算法方程如下： X(k)=[x(k)x(k)y(k)y机器人所在的直角坐标系的存在这二维的两个分量，即X和方向的位置和速度分量，机器人状态有四个，分别是：X方向的位置、X方向的速度、Y方向的位置、Y方向的速度。系统方程用下面的公式表示： x(k)x(k)y(k)y(k)+ Z(k)=1003.2卡尔曼滤波（KF）目标跟踪实验移动机器人是在一个二维的平面进行匀速直线运动，初始的位置我设定为（-50米，100米），对于X方向的移动机器人的速度设定为2米每秒，对于Y方向的移动机器人的速度设定为100米每秒，传感器的扫描的T=0.5秒，观测的噪声均值是零，方差设定为5，根据设定的数学模型和FK的数学算法，应用MATLAB仿真软件，进行FK算法对目标定位跟踪研究。研究结果如下：3.2.1卡尔曼滤波（KF）滤波轨迹和真实轨迹比较图片的横坐标参数代表的是移动机器人在匀速直线运动时的X方向的位移和Y方向的位移。图3-1卡尔曼滤波（KF）轨迹、观测轨迹、真实轨迹对比经过仿真图3-1可以看出：移动机器人通过传感器的收集到的观测轨迹跟实际轨迹相差比较大，振荡很明显，不适用精准的移动机器人定位。虽然运动模型是简单的匀速直线运动也存在较大误差。而相对经过卡尔曼滤波（KF）滤波之后的轨迹，就会明显好于直接观测的值。点（-15，450）为零界点，这个点之前的卡尔曼滤波（FK）几乎于真实轨迹重合，很符合移动机器人对于精准定位的要求，但是在点（15，450）之后，FK滤波轨迹的振荡开始增加，并且随着距离的增加变得偏离真实的轨迹。3.2.2卡尔曼滤波（KF）滤波前后位置误差曲线图片是滤波前的系统误差和滤波后的系统误差，X轴是观测的时间，Y轴是滤波前后的系统误差值。图3-2卡尔曼滤波（KF）滤波前后系统误差通过仿真图3-2可以看出：对于没有滤波的系统，整体的误差较大，在10秒时，误差达到最大值，为7.683米，在83秒时有一个最小值，为0.2米，整体普遍的位置估计误差在2.603，说明滤波前振荡严重，误差波动频繁，且单次差距很大，最大差距可达2.3米。而经过卡尔曼滤波的滤波后的位置误差，在0时刻误差最小，在148秒时，误差达到最大值，为2.929米。整体误差在1.324米，单次最大误差可以达到1.16米，位置误差振荡相对不严重，但是误差波动也较频繁。说明对于移动机器人目标跟踪中，卡尔曼滤波能起到优化误差和轨迹的效果，但是适合相对粗糙的要求，在一定误差范围内是不错的选择。3.2.3KF滤波前误差跟KF滤波后误差的仿真实验数据对比选取地44到50采样点进行数据的收集，进行对经过卡尔曼滤波（FK）系统前后误差的比较，对比性能的好坏。滤波前误差4.28281.440743.897642.080611.298841.39213.70329滤波后误差1.174560.862141.038820.7539890.2947270.2547020.62507表3-1KF滤波前后的误差仿真实验数据表3-1是随机抽取的，主要是位于中前半段的的滤波前和滤波后的数据。根据所采集的样本数据能够看出，在滤波运行的过程中，KF滤波前的偏差都要小于滤波后和偏差。第四章基于扩展卡尔曼滤波（EKF）的目标跟踪研究4.1扩展卡尔曼滤波（EKF）目标跟踪算法机器人所在的直角坐标系的存在这二维的两个分量，即X和方向的位置和速度分量，机器人状态有四个，分别是：X方向的位置、X方向的速度、Y方向的位置、Y方向的速度。机器人在移动过程中的随机的扰动定义为U（k），系统表示为： x(k+1)=x(k)+vx(k)T+1移动机器人的系统状态方程为： x(k+1)=ϑx(k+1)+γu(k) （4-2）状态方程的矩阵分别为： ϑ=1T0 γ=T2激光雷达传感器进行信息的搜集和探测，设定位置为（x0,观测方程为： Z(k)=(x(k)−x0)公式（4-5）中的V（k）为激光雷达传感器自己本身的测量误差，我把R设定为测量误差方差。4.2扩展卡尔曼滤波（EKF）目标跟踪实验移动机器人是在一个二维的平面进行匀速直线运动，初始的位置我设定为（-50米，100米），对于X方向的移动机器人的速度设定为2米每秒，对于Y方向的移动机器人的速度设定为100米每秒，传感器的扫描的T=0.5秒，观测的噪声均值是零，方差设定为5，根据设定的数学模型和EKF的数学算法，应用MATLAB仿真软件，进行EKF算法对目标定位跟踪研究。研究结果如下：4.2.1扩展卡尔曼滤波（EKF）滤波轨迹和真实轨迹比较图4-1的横坐标参数代表的是移动机器人在匀速直线运动时的X方向的位移和Y方向的位移。图4-1扩展卡尔曼滤波（EKF）轨迹与真实轨迹对比从图4-1可以看出，在横坐标-20之前，扩展卡尔曼滤波（EKF）轨迹几乎和真实轨迹重合，并且滤波的波动比较小，符合移动机器人目标跟踪要求。但是在横坐标-20点之后，滤波轨迹开始偏离真实轨迹，并且波动增加，在横坐标点40左右波动明显加大。在横坐标-60点之后，滤波轨迹已经完全偏离真实轨迹，这时波动已经超出了之前数倍。可以看出，对于匀速直线运动的机器人，扩展卡尔曼滤波在一定的距离内是很准确和稳定的，但是随着运动距离变化，开始逐步失真，所以扩展卡尔曼滤波适合短距离的移动机器人目标跟踪。4.2.2扩展卡尔曼滤波（EKF）位置误差曲线图4-2为扩展卡尔曼滤波位置误差曲线，横坐标时时间，纵坐标是位置估计偏差：图4-2扩展卡尔曼滤波（EKF）位置误差曲线从图4-2可以看出，EKF滤波效果一般，在前20秒时间内，位置误差很小，几乎为零，但是20秒之后误差开始变大，并且振荡加剧，在109秒这个点，达到最高误差距离23.9米，可以说在40秒之后，滤波轨迹已经理真实轨迹偏离的很严重。对于机器人定位来说，在短时间的运动跟踪，EKF可以做好的很完美的精确定位，但是经过几次算法的迭代，一段时间之后滤波轨迹就会偏离真实轨迹，误差也会随着时间继续加大。在一定的时间内，扩展卡尔曼滤波是很好的选择。想要一直跟踪移动机器人定位是比较苦难的。4.2.3EKF滤波误差跟FK滤波误差的仿真实验数据对比选取地16到23采样点进行数据的收集，进行UKF和EKF位置偏离误差的比较，对比性能的好坏：EKF误差0.161290.3690610.538080.5466220.2139450.1797520.152328FK误差1.3140.862141.38821.96932.049682.139682.45538表4-1EKF滤波误差与FK滤波误差的实验仿真数据从上表可以看出，扩展卡尔曼滤波有效的降低了位置误差，并且在20秒之内的定位很误差小，对比卡尔曼滤波定位也更准确，并且稳定。不过根据之前的仿真可以看出，扩展卡尔曼滤波虽然比卡尔曼滤波目标跟踪更准确，振幅小和稳定，但是不超过设定范围，超过了时间范围误差就会加大，轨迹就会偏离真实轨迹。第五章基于无迹卡尔曼滤波（UKF）的目标跟踪研究5.1无迹卡尔曼滤波（UKF）目标跟踪算法运动模型方程为： X(k+1)=ϕX(k)+ΓW(k) （5-1） Z(k)=(x(k)−x0)过程驱动矩阵： ϕ=110研究移动机器人的目标运动方程为： Γ=0.5010目标状态方程： Xukf=误差的平凡根： RMSM(k)=(XUKF5.2无迹卡尔曼滤波（UKF）目标跟踪实验移动机器人是在一个二维的平面进行匀速直线运动，初始的位置我设定为（-50米，100米），对于X方向的移动机器人的速度设定为2米每秒，对于Y方向的移动机器人的速度设定为100米每秒，传感器的扫描的T=0.5秒，观测的噪声均值是零，方差设定为5，根据上文设定的数学模型和FK的数学算法，应用MATLAB仿真软件，进行FK算法对目标定位跟踪研究。研究结果如下：5.2.1无迹卡尔曼滤波（EKF）滤波轨迹和真实轨迹比较图5-1的横坐标参数代表的是移动机器人在匀速直线运动时的X方向的位移和Y方向的位移图5-1无迹卡尔曼滤波（UKF）轨迹与真实轨迹对比从图5-1可以看出，扩展卡尔曼滤波（UKF）轨迹整体很符合真实轨迹。在横坐标0之前，UKF轨迹是有波动的，离实际轨迹有一定误差，但是随着迭代算法的更新，UKF轨迹越来越符合真实轨迹，能