学术交流 移动机器人SLAM位姿估量的改善四元数无迹卡尔曼滤波

发布时间:2022-06-02 16:42:10 | 作者:环球体育竞猜

  在全自主运动操控的移动机器人体系中,本身位姿的估量和校对关于移动机器人的运动至关重要。卡尔曼滤波是处理移动机器人同步定位与地图构建(SLAM)常用办法。相较于卡尔曼滤波,无迹卡尔曼滤波(UKF)无须对杂乱的非线性函数进行雅可比矩阵运算。本文依据无迹卡尔曼滤波,依据先验协方差的平方根挑选sigma点,核算协方差以及加权均值。用四元数表明姿势,将四元数矢量转化为旋转空间进行矩阵运算,在此基础上规划了一种位姿估量算法——依据四元数平方根的无迹卡尔曼滤波(QSR-UKF)算法。实验将EKF、QSR-UKF、SR-UKFEKF 3种算法的位姿估量作用进行仿真剖析,并经过相关定量方针进行了描绘,验证了本文算法的有用性。

  在传统测绘工业数据获取和数据处理办法中,很多重复性的人作业业仍是占有主导地位,这种办法不只功率极低,而且难以到达精度的要求,逐渐无法满意智能化测绘阶段时空信息获取与运用需求。跟着移动机器人技能的广泛老练以及智能化程度的进步,智能化测绘技能将改动现在数字化测绘以人为中心的办法,给测绘范畴供给新的生机,成为未来开展的必然趋势,打破原有数字化测绘由人为要素带来的环境不行及和功率上的瓶颈。智能化测绘工业将以自主化、实时化为技能方针,树立结合自动探究、实时感知和智能决议计划的技能体系。

  近年来,跟着机器人技能的开展,机器人现已被广泛运用于实践生产范畴中,特别是功用强大的移动机器人在测绘范畴中所扮演的作用越来越大。跟着移动丈量技能走向老练,特别是无人机丈量、车载移动丈量等的诞生,更是在必定程度上处理了动态丈量技能问题[1-2]。现在,智能科技工业的开展现已在社会生产和日子中掀起了严重的革新,具有自主化、实时化才能的移动机器人为测绘工业带来了巨大的开展机会,运用机器代替人并完结丈量行为的自主移动机器人也将是往后智能化测绘工业的开展方向。

  自主移动机器人具有显着的智能化和先进性特征,能够完结关于周围信息的感知和处理,自主做出决议计划和规划,并完结既定的移动方针[3]。国外部分高校以及研讨机构很早开端了相关的研讨,在20世纪60年代,斯坦福大学初次发布了机器人Shakey,其具有了必定的自主决议计划功用,引发了较大的重视,能够完结途径规划,而且在不同的环境中适用性杰出[4]。进入21世纪,在机器学习以及大数据等技能持续开展的布景下,这些新技能被运用于移动机器人的规划中。其间LAAS团队研发的HILARE机器人[5]装备了视觉相机和激光测距仪等传感体系,能够在已知场景内完结途径规划和导航功用。移动机器人的开展从开端的工业机器人扩展到面向服务的自主移动机器人,在医疗保健、文化娱乐等日子办法中有很大的需求,显现出较大的运用远景。Massachusetts Institute of Technology的Wheelesley、德国Vamors-P和Caravelle systems、Carnegie Mellon University的CMU漫游者也在移动机器人范畴有较高水平的研讨作用[6]。

  移动机器人在不知道环境下从初始点开端运动到既定方针的进程中,需求精确获取本身的方位以及环境中障碍物的方位。文献[7]针对机器人的导航,将其总结为两个问题,别离是“我处于何地?”“周围环境怎么?”,二者实践上对应着定位、建图的进程。1995年,文献[8-10]提出“SLAM”一词,并给出了移动机器人SLAM的根本技能结构。

  SLAM依据传感器类型的不同,可首要分为视觉SLAM[11-13]及激光SLAM[14]。移动机器人SLAM的研讨首要包含数据相关[15]、环境特征提取与表明[16]、方针检测[17]、闭环[18]等。移动机器人SLAM问题中存在许多不确认要素,例如与观测模型以及所在环境的不确认性等要素有关[19],由此逐渐呈现了依据概率的SLAM算法,详细能够将其划分为不同的类型[20],例如有依据EKF的SLAM算法、依据希望最大化(EM)的SLAM算法等。

  卡尔曼滤波(KF)算法用状况向量表明移动机器人的姿势和特征在地图上的方位。在此算法中认为是线性体系,噪声以及状况均满意高斯散布,可是因为存在显着的线]规划了EKF算法,该算法能够完结关于非线性体系的迫临。

  为了削减EKF-SLAM算法的核算量,文献[23]规划了无迹卡尔曼滤波(UKF)。在此办法中无须对Hessians矩阵进行核算,运用Sigma点集来近似方针状况的后验概率密度。但UKF的核算量跟着观测值的添加也会随之添加。因而,扩展办法的Sigma点滤波办法也得到了广泛的运用。文献[24]规划了Sigma点卡尔曼滤波的平方根。该办法能够到达较高的估量精度,而且稳定性杰出。因为滤波器的平方根具有保证协方差矩阵非负的长处,因而文献[25]在SLAM问题中规划了平方根无迹卡尔曼滤波,改善了算法的运用作用。一起,文献[25]改善了文献[26]的作用,规划了一种迭代平方根的UKF-SLAM,文献[27]规划了AUKF-SLAM。

  依据现有的研讨,四元数的运用具有多方面的长处,首要体现在核算量小,精度高,非奇异性和可全姿势作业等方面。部分学者依据四元数进行了研讨,其间文献[28]针对UKF核算量较大的问题, 提出一种经过改善的四元数无迹卡尔曼滤波算法。经过四元数UKF中四元数和批改罗德里格斯参数之间的线性改换, 避免了频频切换四元数和差错罗德里格斯参数所或许带来的核算过错和测验量增大的现象。文献[29]提出一种依据含糊逻辑四元数的平方根UKF姿势估量算法。选用含糊逻辑的平方根协方差办法作为更新参数,既下降了算法的核算量和杂乱度,又保证了数值的稳定性。文献[30]提出一种用于磁角速率和重力传感器的线性卡尔曼滤波器,该滤波器处理角速率、加速度和磁场数据,以取得四元数表明中的方向估量。该算法将四元数方向核算为两个代数四元数的组合。两个四元数的解耦性质使方向的滚转和俯仰重量不受磁搅扰的影响。文献[31]依据UKF,提出一种四元数衍生无迹卡尔曼滤波的无人机姿势估量办法,该办法实时性及精度方面都强于无迹卡尔曼滤波。文献[32]提出一种四元数状况切换无迹卡尔曼滤波办法。经过罗德里格斯参数与四元数结合的无迹卡尔曼滤波,运用时刻更新和量测更新的进程中切换滤波状况来削减核算量,以此添加体系的实时性、精度和稳定性。

  本文旨在处理移动机器人运动进程中的非线性位姿估量问题。首要列出了扩展卡尔曼滤波和无迹卡尔曼滤波SLAM的推导方程。依据无迹卡尔曼滤波触及高阶矩阵运算导致时刻核算添加的现状,在无迹卡尔曼滤波的基础上添加了平方根滤波。一起,运用四元数全姿势操作,精确性高的特征,经过乘性差错四元数对均匀点和四元数点之间的间隔进行描绘,并下降滤波状况的维数,削减核算耗时。

  针对移动机器人SLAM的研讨比较多,该问题能够描绘为机器人从处于特定环境中的某方位开端运动,实时对邻近环境信息进行勘探和感知,并以增量办法构成地图。关于SLAM问题,在时刻k处界说以下状况向量。

  SLAM进程能够由运动方程和观测方程来描绘。假定在t=0至t=N的时刻内,有位姿x0至xN,而且有路标y1, y2, …, yM。运动和观测方程为

  因为x、y均归于需求估量的变量,因而能够将二者认为是满意特定概率散布的随机变量。已知xk代表k时的悉数不知道量,对应的悉数观测表明为zk

  现在考虑第k时刻的状况。用曩昔0到k中的数据,来估量现在的状况散布:那么SLAM问题能够用式(1)描绘为

  估量后验概率密度散布P(xkzk, uk)详细划分为两个不同的进程,别离是猜测、更新,详细的进程如下所示。

  (1) 猜测阶段:依据机器人的运动模型和当时方位,猜测机器人下一时刻的状况。

  (2) 更新阶段:在核算出观测模型和先验概率密度后,运用传感器所获取到的信息更新体系内的状况估量量。因而,体系的后验概率密度为

  P(zkxk)是观测模型。依据马尔可夫假定,当时状况已知时,前一次观测和当时观测是彼此独立的,因而P(zkzk-1, uk)是归一化常数。

  假如运动模型和观测模型满意线性散布,则可选用卡尔曼滤波算法求解。当不满意时,积分核算很难完结,可选用扩展卡尔曼滤波进行求解。

  在实践进程中,SLAM的方程均为非线性,关于此类体系,需求将非高斯散布近似认为是高斯散布。选用扩展卡尔曼滤波办法能够完结关于非线性方程的线性迫临,在此办法中需求在

  方位来对方程进行打开,然后将对应的高阶项进行疏忽,继而完结转化的方针。在此办法中需求经过Kalman公式进行核算[33]。

  传统的EKF存在着两个难以克服的问题:一是EKF算法的一阶泰勒打开疏忽了非线性函数高阶导的影响,当非线性度较强时,则简略下降滤波精度,特别是不低于二阶的切断差错会发生更显着的影响;二是有的非线性函数并没有雅可比矩阵,这也让EKF算法在某些条件下不行用。近来呈现的无迹卡尔曼滤波办法在运用中显现出必定的优势。该算法依据体系状况的先验均值和方差挑选一组核算特征与之前的核算特征状况共同确实定性Sigma采样点。再经过线性传递函数得到一组新的Sigma点,在此基础上能够得到采样点的核算特征,即为对应的后验核算散布。从本质上来看,该办法实践上迫临的对错线性函数的概率密度散布,所以无须对雅克比矩阵进行核算,以此能够完结对非线性函数的处理,相关于传统的扩展卡尔曼滤波办法具有了显着的优势。

  UKF选用递推估量的办法,求得状况量的最优方差估量值,避免了由高阶项切断而发生的体系切断差错,相较于EKF,进步了体系的解算精度。

  关于无迹改换而言,需求保证在到达采样均值x以及协方差P的基础上选取适宜的Sigma点,Sigma点数量取决于所选的采样战略。现在较为常见的采样战略是2n+1 Sigma点对称采样。采样完结之后依据非线性改换办法对Sigma点进行处理,能够得到对应的均值与协方差,然后将其运用到各个Sigma点中,选用相同的处理办法能够得到新的Sigma点。该办法相关于扩展卡尔曼滤波能够得到更多的观测假定。UT改换的运用体现出显着的优势,下降了切断差错关于数据交融所构成的影响,无须进行线性化处理的进程,能够到达更高的求解精度。因而,状况核算特性的估量比EKF算法更精确。

  式中,α代表与Sigma点分散标准有关的参数;β≥0;x遵守高斯散布;n代表状况向量维度。

  运用非线性丈量函数核算k时刻体系状况、均值和协方差的单步猜测χkk-1(i)=f(χk-1(i)),

  运用非线性丈量函数核算k时刻体系状况单步猜测、均值和方差以及互协方差Zkk-1(i)=h(ξkk-1(i)),有

  无迹卡尔曼滤波相关于扩展卡尔曼滤波在精度上具有必定的优势,而且核算难度较小,无须核算雅克比矩阵,所以能够运用到更多的场景中。

  Sigma点确实定性采样与协方差P的平方根运算有关,假如是高阶矩阵则极易影响到核算的功率。其次,在运算进程中,遭到数值舍入差错的影响导致核算稳定性下降。因而,文献[34]在研讨进程中规划了依据平方根的UKF算法。该办法在平方根核算进程中选用了Cholesky分化等办法,详细的内容如下。

  运用非线性丈量函数核算k时刻体系状况、均值和互协方差的单步猜测Zkk-1(i)=h(ξkk-1(i)),有

  式中,qr{…}代表奇异值分化的进程;cholupdate{…}代表矩阵更新的进程。

  运用非线性丈量函数核算k时刻体系状况、均值和互协方差的单步猜测Zkk-1(i)=h(ξkk-1(i))。

  传统的非线性优化办法在求解移动机器人的位姿估量时,往往面对核算量大的问题,需求花费较多的时刻才能够完结核算。四元数则能够有用地处理上述问题,其优势在于下降了核算量,进步了核算的精度,而且能够到达全姿势作业的方针,所以广泛运用到了姿势表明中[35]。国外部分学者很早开端在此范畴进行了研讨,在19世纪,W.R.Hamilton提出了四元数,可是遭到技能以及硬件条件等要素的影响并未引起广泛的重视,也没有得到较多的运用。后来跟着在此范畴研讨的深化,以及核算机功能的不断进步,关于四元数的研讨和运用逐渐增多,能够充分发挥其运用的优势与特征。别的,其相关于欧拉角更为简略,大幅度下降了核算的杂乱度。正是因为具有了上述优势,使得四元数在扩展卡尔曼滤波中有着广泛的运用。文献[36]提出了一种依据四元数的二阶EKF算法,核算了Taylor打开的二阶精度,进步了姿势估量的精度,但核算量也有所添加。文献[37]提出了一种依据批改Rodriguez参数和姿势四元数转化的无迹Kalman滤波算法。该算法经过参数间的彼此转化有用地进步了姿势估量的精度,但这种彼此转化无疑添加了算法的杂乱度。

  本文提出一种四元数平方根无迹卡尔曼滤波(Quaternion Squareroot-UKF,QSR-UKF)算法。算法将四元数均值问题转化为拉格朗日价值函数的极值问题,无须姿势参数切换,矢量部分归于姿势变量,标量则经过单位四元数核算。这种核算办法处理了四元数Sigma点在选取进程中的或许呈现的搅扰问题,下降了平方根无迹卡尔曼滤波状况的维数。

  在算法中运用了乘性差错四元数,其能够对均匀点和四元数点之间的间隔进行描绘,有助于处理奇异值分化的核算量大和特征向量法烦琐的核算进程等问题。除了上述优势之外,相同有用地处理了四元数加权均值问题,能够坚持明晰的核算流程,并得到精度较高的作用。

  在UKF算法中参加四元数构成Q-UKF算法,相同存在必定的缺乏,包含差错协方差矩阵的对称性等。本文中针对上述问题进行了剖析和规划,在此基础上规划了一种新的四元数平方根UKF(QSR-UKF)算法。

  选定的状况变量x=[ρT, βT]T,其间ρ是姿势四元数的矢量部分;β是陀螺漂移。

  与均匀值核算相同,四元数方差核算也需求作为旋转向量处理。这儿,乘法差错四元数δχkk-1q(i)用于表明状况猜测值χkk-1q(i)到猜测均匀值

  (3) 丈量更新。与时刻更新的Sigma点挑选相似,丈量更新的Sigma点为

  本文别离在公寓环境下以及迷宫环境下对移动机器人SLAM进行了测验,别离对EKF、SR-UKF和QSR-UKF进行了仿真。一切仿实在验均在Ubuntu 18.04中运用ROS Melodic进行,在英特尔Core i7-8750HCPU上运转。在本次研讨中现已事前确认了地图特征和观测特征之间的联系,由此下降了数据相关过错所发生的晦气影响。然后针对3种算法的运用作用进行了比照剖析,经过方针量化剖析,包含运转时刻和精度,继而能够得到客观的作用。

  本文创立两个实验环境,公寓环境巨细为15 m×15 m,迷宫环境巨细为80 m×8 0m,共有18个导航符号和28个停止点的特征以及一些随机设置的点特征。如图 1和图 6所示,移动机器人的初始方位为(0, 0)移动速度为v=3 m/s。别的,激光雷达采样时长为0.2 s,勘探间隔最大值是30 m,间隔差错为0.1 m,进程噪声协方差矩阵Q为[1.900;01.20;0000.8],观测噪声协方差矩阵R为[0.50;000.1]。图 1 公寓仿真环境

  针对仿真中的搅扰要素进行规划,在100个操控周期中恣意设置外部搅扰,详细为3-5次。经过这种办法能够对SLAM精度发生必定的影响。在本次实验中,选用了3种算法来对移动路标进行估量,别离是EKF-SLAM, SR-UKF-SLAM、QSR-UKF-SLAM算法。在各个参数设置完结之后能够进行仿真的进程,详细得到的作用如图 2-图 5、图 7-图 10所示,其间“+”“”别离代表环境特征的实在以及估量的方位;而实践移动道路 扩展卡尔曼滤波在公寓内的仿线 平方根无迹卡尔曼滤波在公寓内的仿线 四元数平方根无迹卡尔曼滤波在公寓内的仿线种在公寓内的仿线 迷宫仿线 扩展卡尔曼滤波在迷宫内的仿线 平方根无迹卡尔曼滤波在迷宫内的仿线 四元数平方根无迹卡尔曼滤波在迷宫内的仿线种在公寓内的仿线

  依据3种办法得到的姿势差错信息即为图 2-图 4及图 7-图 9中所示,而对应的环境方位信息如图 5及图 10中所示。依据图中所示的信息可知,相关于EKF-SLAM,其他两种算法能够到达更高的精度要求。经过调整协方差矩阵和添加噪声差错,能够看出SR-UKF和QSR-UKF-SLAM具有更高的姿势估量精度。依据本文研讨的作用可知,遭到噪声要素的影响,导致EKF简略发散,增大了EKF-SLAM操控的难度,而这种现象与噪声的类型有关,假如不是零均值白噪声即简略呈现发散的问题。SR-UKF-SLAM则能够完结噪声习惯,首要与无迹改换等处理进程有关。尽管3种算法与机器人实在的途径都存在必定的差错,可是QSR-UKFSLAM算法机器人轨道估量和路标估量的差错都小于EKF-SLAM和SR-UKF-SLAM算法,所以QSR-UKFSLAM算法的位姿估量精度最高。

  为了更详细地比较3种算法在不同的运动噪声和观测噪声下的姿势估量精度,在研讨进程中经过RMSE来进行点评,针对3种算法的姿势估量精度进行了比照剖析,为了进步仿真作用的精确性与可靠性,将仿线次,终究得到的信息见表 1。表 1首要包含两个方针,别离是RMSE_P、RMSE_L,二者别离对应着机器人、路标方位估量。

  表1 3种算法在公寓环境中移动机器人方位估量以及路标方位估量的均方根差错

  依据表 1和表 2的信息能够显着地看到,在公寓环境中,QSR-UKF-SLAM算法在机器人方位估量和路标估量上的RMSE比EKF-SLAM别离下降了55.20%、47.29%,比SQ-UKF-SLAM算法在轨道和路标估量的RMSE别离下降了32.81%、31.57%。在迷宫环境中,QSR-UKF-SLAM算法在机器人方位估量和路标估量上的RMSE比EKF-SLAM别离下降了64.78%、67.92%,比SQ-UKF-SLAM算法在轨道和路标估量的RMSE别离下降了20.08%、29.53%。3种算法在路标方位的均方根差错曲线所示,结合图中所示的信息可知,依据SR-UKF、QSR-UKF的SLAM算法能够到达较高的位姿估量精度,估量差错最低的是QSR-UKFSLAM算法,由此验证了算法的运用作用。

  表2 3种算法在迷宫环境中移动机器人方位估量以及路标方位估量的均方根差错

  1.36图11 在公寓环境中3种算法在路标方位上的均方根差错图12 在迷宫环境中3种算法在路标方位上的均方根差错曲线显现了在迷宫环境下,EKF、SR-UKF、QSR-UKF-SLAM 3种算法在相同条件下的X、Y以及姿势角方向的绝对差错均匀值以及方差的比较。一起从表中能够看出,SR-UKF-SLAM和QSR-UKF-SLAM核算精度在全体上要比EKF-SLAM高;SR-UKF-SLAM和QSR-UKF-SLAM核算精度比较照较挨近,因而,SR-UKF-SLAM和QSR-UKF-SLAM能有用下降EKF所带来的线性化差错。整体而言,QSR-UKF进步了机器人运动进程的稳定性,详细功能优于EKF-SLAM和SR-UKF-SLAM。本文未比较在公寓环境下3种算法在X、Y以及姿势角方向的绝对差错均匀值以及方差。原因是公寓环境较小,X、Y以及姿势角方向的绝对差错均匀值以及方差都较小,可比性缺乏。

  依据QSR-UKF-SLAM与EKF-SLAM、SQ-UKF-SLAM算法在公寓以及迷宫环境下的运转时刻见表 4及表 5,结合表中的信息可知,总运转时刻最低的是依据QSR-UKF的SLAM算法。而且,QSR-UKF-SLAM算法在单步运转时刻上也少于其他两种算法。

  本文对移动机器人SLAM算法的位姿估量与校对问题进行了研讨,针对依据非线性优化的扩展卡尔曼滤波和无迹卡尔曼滤波位姿优化算法的局限性,给出了相应的处理方案。本文在无迹卡尔曼滤波中参加Sigma点的平方根以进步其稳定性和估量精度。其次,依据四元数具有核算量小、精度高的长处,将其向量当作姿势变量,处理了四元数Sigma在选取进程中的或许呈现的搅扰问题。而且相较于无迹卡尔曼滤波下降了滤波状况的维数,削减了核算耗时。综上,本文提出了四元数平方根UKFSLAM位姿估量算法。仿实在验作用表明,与EKF和SR-UKF算法比较,QSR-UKF算法选用了相对简略的奇异值分化和Cholesky分化来完结差错方差矩阵的平方根运算,使得算法的数值运算特性得到了必定程度的改善,一起也进步了算法的稳定性。

  原标题:《学术交流 移动机器人SLAM位姿估量的改善四元数无迹卡尔曼滤波》

相关推荐

读取内容中,请等待...

© CopyRight 2019, HUNANGY.COM, Inc.All Rights Reserved.
学院地址:湖南省长沙市岳麓区含浦科教园
湘教QS3-200505-000638 湘ICP备19015321号