CN106197428B - 一种利用测量信息优化分布式ekf估计过程的slam方法 - Google Patents
一种利用测量信息优化分布式ekf估计过程的slam方法 Download PDFInfo
- Publication number
- CN106197428B CN106197428B CN201610541854.XA CN201610541854A CN106197428B CN 106197428 B CN106197428 B CN 106197428B CN 201610541854 A CN201610541854 A CN 201610541854A CN 106197428 B CN106197428 B CN 106197428B
- Authority
- CN
- China
- Prior art keywords
- subfilter
- moment
- formula
- fusion
- robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种利用测量信息优化分布式EKF估计过程的SLAM方法,在利用测量信息优化分布式EKF估计过程的SLAM方法中,根据路标点信息对应建立若干平行的子系统,各子系统单独进行滤波估计,将各自的估计结果送到主滤波器中,最后输出最优的机器人位姿估计结果。本发明针对分布式SLAM系统中EKF子滤波器估计精度存在的局限性问题,采用由传感器直接测得的观测量计算滤波器雅克比矩阵,利用改进的线性化方法提高子滤波器的估计精度。考虑到一致性对系统的影响,采用误差协方差阵和匹配信息共同参与数据融合过程的方法改进系统融合的方案,充分发挥分布式结构下观测信息的作用,保证系统在精度较高或一致性较高的情况下均能合理地做出融合判定。
Description
技术领域
在利用测量信息优化分布式EKF估计过程的SLAM方法中,分布式结构将时变的观测信息分布化处理,根据路标点信息对应建立若干平行的子系统,并且子系统维数不随时间变化。各子系统单独进行滤波估计,将各自的估计结果送到主滤波器中,主滤波器在保证系统在精度较高或一致性较高的情况下合理地做出融合判定,最后输出最优的机器人位姿估算结果。属于机器人自主导航领域。
背景技术
SLAM(Simultaneous Localization and Mapping)即同步定位与地图构建,其基本思想是:让机器人在未知环境中从未知位置开始移动,通过自身所带传感器扫描到的路标点的信息进行自身位置估计,同时构建增量式地图。在SLAM过程中,根据观测信息进行实时的地图估计与更新,其中滤波器设计,数据融合算法都是关键问题。
SLAM问题可以建立在不同的算法结构中,对于传统的集中式SLAM,将机器人位姿和路标点信息均加入到状态向量中,在整个SLAM滤波过程中,无论采用哪种滤波方式,状态向量由于一直附带时变的路标点信息进行后续滤波运算,随着路标点数量的增多状态向量维数也不断增多,计算量相应增大,并且根据路标点的观测情况动态时变。
本发明中SLAM算法采用分布式结构,在分布式结构中将机器人位姿及路标点信息进行分布化处理,每个有效的路标点对应一个子滤波器。因此,状态向量的维数相对固定,不会随着时变的路标点信息而变化,每一个子滤波器都有各自的状态方程和观测方程,各子滤波器的估计结果都被传送到主滤波器进行融合运算。在主滤波器中,根据各个子滤波器的估计结果及相应的误差情况计算机器人位姿的最终结果。由于扩展卡尔曼滤波方法的简单性与有效性,在各子滤波器中通常选用EKF算法。但是,EKF线性化过程忽略高阶项产生的线性误差导致了子滤波器估计精度存在局限性。主滤波器的核心是融合算法,选择单一的引入误差协方差矩阵或新息矩阵信息参与数据融合过于单薄,难以保证系统精度。
发明内容
本发明针对分布式SLAM系统中EKF子滤波器估计精度存在的局限性问题,采用由传感器直接测得的观测量计算滤波器雅克比矩阵,提高了子滤波器估计精度。并且考虑到一致性对系统的影响,改进分布式系统融合的方案,提出利用误差协方差阵和匹配信息共同参与数据融合过程的方式对算法进行优化。下文将具体描述优化分布式结构中的子滤波器以及分布式系统的融合方案的过程。最后通过真实实验证明了本发明算法的可行性与有效性。
机器人的运动模型和观测模型如图1所示。
在分布式SLAM问题中,假设机器人的姿态表示为其中(xr,yr)表示机器人在地图中的坐标,表示航向角。路标点在地图中的位置表示为:mi=(xi,yi)T,i=1:n,其中mi表示第i个路标点对应的位置信息,(xi,yi)表示路标点在地图中的坐标。t时刻机器人的状态矢量表示为Xt=(xt,r T,mt,i T)T。
机器人的运动学模型可用公式(1)描述
st=f(st-1,ut)+wt (1)
其中,移动机器人运动模型描述了t时刻机器人的状态在运动的控制信号ut和噪声干扰wt作用下随时间的变化过程。wt表示过程噪声,用来表示机器人运动过程中存在的不确定性误差。
机器人的观测模型可以用公式(2)表示
Ζ(t)=h(xr(t),mi(t))+v(t) (2)
其中Z为t时刻机器人通过激光测距仪所获得的观测信息(r,β)T,r表示机器人与路标点间的距离,β表示第i个路标点相对机器人的方向角,v为测量误差。
假设t时刻观测到n个有效的特征点,分布式系统可表示为:
可见,在分布式SLAM中,根据观测到的路标点信息建立相互平行的子滤波器,每一个子滤波器都有各自的状态方程和观测方程。最终每一个子滤波器的估计结果都被传送到主滤波器,而在主滤波器中,根据各个子滤波器的估计结果及相应的误差情况计算出机器人位姿的最优结果。分布式滤波器结构图如图2。
在分布式SLAM算法子滤波器估计单元中,EKF算法的第一步是根据运动模型和t-1时刻的状态对t时刻状态进行预测
其中,和是机器人在t-1时刻所估计的机器人位置结果和路标点位置结果。
第i个子系统t时刻的观测方程具体形式如下:
解决SLAM问题时,EKF是在噪声符合零均值的高斯白噪声的假设下,利用将f(xt-1,rut-1)和h(xr(t),mi(t))展成泰勒级数并略去二阶以上高阶项从而得到非线性系统的线性化模型。因此,EKF关键的步骤是准确地求解状态方程和观测方程的两个雅克比矩阵。
计算状态转移雅克比矩阵
计算观测信息雅克比矩阵
预测结果的协方差阵如下:
其中Prr,t-1和Pri,t-1分别是t-1时刻机器人状态向量协方差和机器人与路标点状态向量的协方差。Pii,t-1和Qt-1分别是t-1时刻路标点状态向量的协方差和机器人运动的过程噪声的协方差。
t时刻第i个子系统根据运动模型预测的和Pt *按照以下方式进行更新过程:
其中,Ki(t)和分别是子滤波器t时刻的卡尔曼增益和观测方程的雅克比矩阵,Rt代表观测噪声矩阵,和Pi(t)分别代表子滤波器t时刻对应的状态向量估计值和相应的协方差。
可见,t时刻子系统的位姿估计是根据泰勒一阶展开通过线性化的h(Xt)进而计算得到的,即
其中X0表示观测方程线性化时进行泰勒展开时所在的点,即根据运动模型所预测的点最后,将Z=Zt代入时,得到EKF SLAM的最优估计结果。
当观测方程在进行泰勒展开时的斜率是如下形式时,拥有最高的估计精度,
当将kreal和Zt分别代入Ht和Z中,则Xt能被最精确地估计。如图3所示。
在图3中,X0处的三条曲线A,B,C分别代表采用不同计算方法求出的X0处切线,即中所对应的各项值。其中,曲线A对应的是式(12)中的算法,即理论上最接近要求的计算值kreal,曲线C对应的是传统EKF中的计算值,而曲线B则表示通过利用各种优化的方法,试图将中对应的k在kEKF的基础上靠近真实观测方程雅克比矩阵中各项的kreal。基于此种思想,可以结合系统的观测方式和预测方式,设计适合实际系统的求取方法,从而得到精度更优的滤波器性能。
针对以上非线性系统的线性化误差分析,改进的关键点是采用何处的X值进行线性化。结合分布式独有的结构,即对于各子滤波器观测到的路标点情况,分别计算相应的线性化结果,最终融合得到最优的估计结果。每一个子滤波器对应不同的线性化结果hi,由于各子滤波器的雅克比矩阵hi存在差异性,根据线性化尽可能最优的原则,采用均值逼近的方法,对进行改进。
根据分布式EKF中子滤波器t时刻迭代更新的计算式(14)
其中观测方程的雅克比矩阵可以表示为式(15)的形式
如式(15),在整个扩展卡尔曼滤波过程中,传统算法中将里程计数据、激光传感器数据代入滤波器过程得出状态估计结果,再代入式(15)中。由于得到的机器人位姿、路标点位置并不准确,该处理方式易造成的不准确,故提出如下的算法:
式(15)中的表达式可以借助观测量,通过如下式(16)的表示方法进行求取:
虽然t时刻理想状况的xreal无法直接得到,但可以由观测量间接推导得到,分别将由激光传感器得到的rt、βt和估计的航向角φt代入式(17)中进行计算,进而得到在点xreal下求得的相应观测方程雅克比矩阵
则式(14)中的可以表示为如下形式:
改进后的t时刻第i个子系统的估计过程如下:
考虑到一致性对系统的影响,本发明改进了分布式系统融合的方案,通过协方差矩阵子模块和匹配信息子模块,充分发挥分布式结构下观测信息的作用,保证系统在精度较高或一致性较高的情况下均能合理地做出融合判定。
首先,利用误差协方差阵定义融合权重:设第i个子滤波器协方差阵为Pi,定义相应子滤波器在融合过程中的权重为δp,i。
依据以上定义,协方差阵Pi表征子滤波器的精度情况,子滤波器对应的协方差越大,说明该子滤波器的估计精度相对较低。相反,协方差阵越小,表征子滤波器的估计精度较高,应当令主滤波器更多继承该子滤波器的估计值,即该子滤波器权重值较大。
协方差矩阵Pi描述子滤波器估计的精度情况,其主要描述当前时刻t的子滤波器权重情况,但从时间的维度上,无法反映从时刻t-1到t的子滤波器准确情况,为了衡量在运行过程中t-1到t时刻子滤波器的精度情况,并且使其反映到子滤波器的权重上,考虑在数据融合中引入匹配误差,从而丰富信息融合过程。
同时需要考虑的是,由于子滤波器在运行过程中,其对某一路标点的“置信度”会随着系统运行的时间逐渐增高,这一规律是影响分布式系统一致性的重要原因,当观测噪声R无穷大时,看作近似对系统一致性没有影响,而如此则会降低子滤波器精度。所以,当子系统运行时,既需要精度高时实现合理性的融合,又需要在噪声较大而一致性较好时实现合理性的融合。
从让系统最大限度地挖掘观测信息中所表征的特性,以及考虑一致性影响的角度出发,加入能够反映子滤波器测量噪声情况的匹配误差信息参与融合算法。该方法中的融合系数是以相邻时刻的临域匹配值大小来确定的,在机器人运行过程中,每一时刻的观测量都要进行匹配过程,在该过程中,是以匹配成功的路标点t-1和t时刻在地图上的映射值来衡量子滤波器性质。
设t时刻的观测到的第i个路标点的坐标为(xi,t,yi,t),地图中与之匹配的t-1时刻的路标点的坐标为(mix,t-1,miy,t-1)。定义匹配误差为同一路标点相邻时刻的距离误差,表示为ri。则有
此时,利用匹配误差定义融合权重:设第i个子滤波器匹配误差为ri,定义相应子滤波器在融合过程中的权重为δr,i。
根据以上定义,匹配误差用来衡量各子滤波器的可信赖程度。子滤波器的匹配误差越大,该子滤波器的观测噪声越大,子滤波器的观测精度越低,融合时子滤波器的信任度低,该自滤波器的权重值就越小;相反,子滤波器的匹配误差越小,该子滤波器的观测噪声越小,子滤波器的观测精度越高,融合时给与该子滤波器较高的信任度,即该子滤波器的权重值就越高。
在本文的融合算法中,每个子系统存在以上两个融合子模块,即误差协方差模块和匹配融合模块。两个子模块分别采用协方差阵各项系数以及匹配最小邻域的误差进行权重的求解,最后,按照式(24)可以通过子模块的权重,计算最终的权重分配情况:
δp,i·α+δr,i·γ=ηi (24)
在公式(24)中δp,i是第i个子系统由协方差子模块计算出的权重值,δM,i是第i个子系统由匹配融合模块计算出的权重值。ηi是改进的信息融合方法综合计算出的子系统权重。而α和γ分别是δp,i和δM,i在改进的信息融合方法中所占的比例,其中α和γ服从公式(25)。
α+γ=1 (25)
因此,主滤波器输出的t时刻机器人位姿的最优估计结果满足公式(26)。
附图说明
图1:机器人的运动模型和观测模型
图2:分布式滤波器结构图
图3:EKF线性化误差示意图
图4:优化的分布式SLAM原理框图
图5:移动平台示意图
图6:椭圆形实验环境示意图
图7:IDH-EKF算法和DH-EKF算法路径估计结果
图8:IDH-EKF算法和DH-EKF算法与GPS数据的误差比较(X轴方向)
图9:IDH-EKF算法和DH-EKF算法与GPS数据的误差比较(Y轴方向)
图10:矩形实验环境示意图
图11:IDH-EKF算法和DH-EKF算法路径估计结果
具体实施方式
步骤1:建立模型
机器人模型及坐标系如图1所示,以机器人初始时刻的位置为坐标原点,以正东和正北方向为x轴和y轴的正方向。将整个状态向量分为机器人位姿估计和路标估计共五维状态,t时刻机器人状态矢量表示为Xt=(xt,r T,mt,i T)T。设机器人的姿态表示为其中(xr,yr)表示机器人在地图中的坐标,表示航向角。路标点在地图中的位置表示为mi=(xi,yi)T,i=1:n,其中mi表示第i个路标点对应的位置信息,(xi,yi)表示路标点在地图中的坐标值。
步骤2:初始化
地图初始化,利用激光传感器扫描数据计算得到的路标点位置信息建立环境地图。
步骤3:地图匹配并建立分布式子滤波器。
获取机器人t时刻测得的路标点信息,并与已在全局地图中存储的路标点信息相匹配。对于t时刻匹配失败的路标点信息,直接将其添加至全局地图;对于t时刻成功匹配的路标点,利用公式(3)分别对应建立子滤波器。
步骤4:子滤波器线性化
利用公式(6)、(7)计算得到状态方程和观测方程的雅克比矩阵。根据公式(15)、(16)、(17)给出的新的线性化方法,可以得到子滤波器雅克比矩阵新的计算方法。改进后子滤波器的估计更新过程如公式(18)、(19)、(20)所示。
步骤5:子滤波器滤波过程
通过扩展卡尔曼滤波算法对上述步骤中建立的各个子滤波器的状态向量和协方差分别进行预测和更新。
预测阶段:利用t-1时刻机器人位姿估计和该子滤波器对应的协方差矩阵,预测t时刻状态和协方差的先验估计值,如公式(4)、(8)所示。
更新阶段:有了t时刻状态的预测结果,然后再收集现在状态的测量值。结合预测值和测量值,可以通过式子(9),(10)和(11)得到现在状态的最优化估算值和协方差的更新值。
步骤6:计算融合权值
根据步骤5中各个子滤波器得到的机器人状态的最优估算值以及误差协方差矩阵,利用公式(21)计算每个子滤波器协方差矩阵的权重;根据公式(22)得到子滤波器的匹配误差值,利用公式(23)计算每个子滤波器匹配模块的权重。
步骤7:建立主滤波器,并对各子滤波器的结果进行数据融合
由公式(24)、(25)得到主滤波器中各子滤波器的融合权重。把各子滤波器中机器人位姿的状态向量及其权值传送给主滤波器,利用公式(26)主滤波器对各子滤波器中的数据进行融合计算,最终得到机器人位姿的最优估计,输出结果。
本文利用测量信息优化分布式EKF估计过程的SLAM方法的原理框图如图4所示。
下面通过真实实验对本发明提出的利用测量信息优化分布式EKF估计的SLAM方法进行验证。实验所用平台是自己搭建的两轮小车,传感器使用URG-04LX的激光传感器,移动平台的实物图如图5。实验所用的环境是椭圆形和长方形跑道,以长筒作为路标点在跑道两旁随机摆放,椭圆形实验环境如图6所示,矩形实验环境如图10所示,圆点表示小车起始位置,箭头指示方向为小车行驶方向,具体路径如图所示。通过使用本发明算法和传统的分布式SLAM算法对比,证明本算法的可行性和有效性。
椭圆形跑道实验结果对比如图7,虚线(DH-EKF)表示的是单纯利用航向辅助分布式EKF估计的运动轨迹,图中实线(IDH-EKF)是基于本文利用测量信息优化分布式EKF估计过程的SLAM方法得到的,两者均能在运行一周后停留在起始点附近,图中标注的三个椭圆形区域为GPS的测算区域。利用场地环境的GPS测定结果中搜星数目较为理想的三段GPS数据为基准,三段GPS时间起始点分别是:93.53s,138.42s,318.43s,每个时间段所对应的两种算法在x轴方向和y轴方向上与GPS数据的误差情况如图8和图9所示。
矩形跑道实验结果对比如图11,虚线(DH-EKF)表示的是利用基于航向辅助分布式SLAM的运动轨迹,可以观察出其运行轨迹大致符合矩形轨道,但最终并没有能够回到初始起点;图中实线(IDH-EKF)是基于本文利用测量信息优化分布式EKF估计过程的SLAM方法得到的,其估计轨迹符合矩形轨道规律,同时,轨迹终点接近原点,可见其收敛性更好。
通过上述两组实验证实,本发明中的优化方法能有效地提高分布式SLAM算法在特殊环境变化时的稳定性和精度。
Claims (1)
1.一种利用测量信息优化分布式EKF估计过程的SLAM方法,其特征在于:
本方法针对分布式SLAM系统中EKF子滤波器估计精度存在的局限性问题,采用由传感器直接测得的观测量计算滤波器雅克比矩阵,提高了子滤波器估计精度;并且考虑到一致性对系统的影响,改进分布式系统融合的方案,提出利用误差协方差阵和匹配信息共同参与数据融合过程的方式对算法进行优化;将具体描述优化分布式结构中的子滤波器以及分布式系统的融合方案的过程;最后通过真实实验证明了本方法的可行性与有效性;
在分布式SLAM问题中,假设机器人的姿态表示为其中(xr,yr)表示机器人在地图中的坐标,表示航向角;路标点在地图中的位置表示为:mi=(xi,yi)T,i=1:n,其中mi表示第i个路标点对应的位置信息,(xi,yi)表示路标点在地图中的坐标;t时刻机器人的状态矢量表示为Xt=(xt,r T,mt,i T)T;
机器人的运动学模型用公式(1)描述
st=f(st-1,ut)+wt (1)
其中,移动机器人运动模型描述了t时刻机器人的状态在运动的控制信号ut和噪声干扰wt作用下随时间的变化过程;wt表示过程噪声,用来表示机器人运动过程中存在的不确定性误差;
机器人的观测模型用公式(2)表示
Z(t)=h(xr(t),mi(t))+v(t) (2)
其中Z为t时刻机器人通过激光测距仪所获得的观测信息(r,β)T,r表示机器人与路标点间的距离,β表示第i个路标点相对机器人的方向角,v为测量误差;
假设t时刻观测到n个有效的特征点,分布式系统表示为:
可见,在分布式SLAM中,根据观测到的路标点信息建立相互平行的子滤波器,每一个子滤波器都有各自的状态方程和观测方程;最终每一个子滤波器的估计结果都被传送到主滤波器,而在主滤波器中,根据各个子滤波器的估计结果及相应的误差情况计算出机器人位姿的最优结果;
在分布式SLAM算法子滤波器估计单元中,EKF算法的第一步是根据运动模型和t-1时刻的状态对t时刻状态进行预测
其中,和是机器人在t-1时刻所估计的机器人位置结果和路标点位置结果;
第i个子系统t时刻的观测方程具体形式如下:
解决SLAM问题时,EKF是在噪声符合零均值的高斯白噪声的假设下,利用将f(xt-1, rut-1)和h(xr(t),mi)展成泰勒级数并略去二阶以上高阶项从而得到非线性系统的线性化模型;因此,EKF关键的步骤是准确地求解状态方程和观测方程的两个雅克比矩阵;
计算状态转移雅克比矩阵
计算观测信息雅克比矩阵
预测结果的协方差阵如下:
其中Prr,t-1和Pri,t-1分别是t-1时刻机器人状态向量协方差和机器人与路标点状态向量的协方差;Pii,t-1和Qt-1分别是t-1时刻路标点状态向量的协方差和机器人运动的过程噪声的协方差;
t时刻第i个子系统根据运动模型预测的和Pt *按照以下方式进行更新过程:
其中,Ki(t)和分别是子滤波器t时刻的卡尔曼增益和观测方程的雅克比矩阵,Rt代表观测噪声矩阵,和Pi(t)分别代表子滤波器t时刻对应的状态向量估计值和相应的协方差;
可见,t时刻子系统的位姿估计是根据泰勒一阶展开通过线性化的h(Xt)进而计算得到的,即
其中X0表示观测方程线性化时进行泰勒展开时所在的点,即根据运动模型所预测的点最后,将Z=Zt代入时,得到EKFSLAM的最优估计结果;
当观测方程在进行泰勒展开时的斜率是如下形式时,拥有最高的估计精度,
当将kreal和Zt分别代入Ht和Z中,则Xt能被最精确地估计;
X0处的三条曲线A,B,C分别代表采用不同计算方法求出的X0处切线,即中所对应的各项值;其中,曲线A对应的是式(12)中的算法,即理论上最接近要求的计算值kreal,曲线C对应的是传统EKF中的计算值,而曲线B则表示通过利用各种优化的方法,试图将中对应的k在kEKF的基础上靠近真实观测方程雅克比矩阵中各项的kreal;基于此,结合系统的观测方式和预测方式,设计适合实际系统的求取方法,从而得到精度更优的滤波器性能;
针对以上非线性系统的线性化误差分析,改进的关键点是采用何处的X值进行线性化;结合分布式独有的结构,即对于各子滤波器观测到的路标点情况,分别计算相应的线性化结果,最终融合得到最优的估计结果;每一个子滤波器对应不同的线性化结果hi,由于各子滤波器的雅克比矩阵hi存在差异性,根据线性化尽可能最优的原则,采用均值逼近的方法,对进行改进;
根据分布式EKF中子滤波器t时刻迭代更新的计算式(14)
其中观测方程的雅克比矩阵表示为式(15)的形式
如式(15),在整个扩展卡尔曼滤波过程中,传统算法中将里程计数据、激光传感器数据代入滤波器过程得出状态估计结果,再代入式(15)中;由于得到的机器人位姿、路标点位置并不准确,该处理方式易造成的不准确,故提出如下的算法:
式(15)中的表达式借助观测量,通过如下式(16)的表示方法进行求取:
虽然t时刻理想状况的xreal无法直接得到,但由观测量间接推导得到,分别将由激光传感器得到的rt、βt和估计的航向角φt代入式(17)中进行计算,进而得到在点xreal下求得的相应观测方程雅克比矩阵
则式(14)中的表示为如下形式:
改进后的t时刻第i个子系统的估计过程如下:
考虑到一致性对系统的影响,通过协方差矩阵子模块和匹配信息子模块,充分发挥分布式结构下观测信息的作用,保证系统在精度较高或一致性较高的情况下均能合理地做出融合判定;
首先,利用误差协方差阵定义融合权重:设第i个子滤波器协方差阵为Pi,定义相应子滤波器在融合过程中的权重为δp,i;
依据以上定义,协方差阵Pi表征子滤波器的精度情况,子滤波器对应的协方差越大,说明该子滤波器的估计精度相对较低;相反,协方差阵越小,表征子滤波器的估计精度较高,应当令主滤波器更多继承该子滤波器的估计值,即该子滤波器权重值较大;
协方差矩阵Pi描述子滤波器估计的精度情况,其主要描述当前时刻t的子滤波器权重情况,但从时间的维度上,无法反映从时刻t-1到t的子滤波器准确情况,为了衡量在运行过程中t-1到t时刻子滤波器的精度情况,并且使其反映到子滤波器的权重上,考虑在数据融合中引入匹配误差,从而丰富信息融合过程;
同时需要考虑的是,由于子滤波器在运行过程中,其对某一路标点的“置信度”会随着系统运行的时间逐渐增高,这一规律是影响分布式系统一致性的重要原因,当观测噪声R无穷大时,看作近似对系统一致性没有影响,而如此则会降低子滤波器精度;所以,当子系统运行时,既需要精度高时实现合理性的融合,又需要在噪声较大而一致性较好时实现合理性的融合;
从让系统最大限度地挖掘观测信息中所表征的特性,以及考虑一致性影响的角度出发,加入能够反映子滤波器测量噪声情况的匹配误差信息参与融合算法;该方法中的融合系数是以相邻时刻的临域匹配值大小来确定的,在机器人运行过程中,每一时刻的观测量都要进行匹配过程,在该过程中,是以匹配成功的路标点t-1和t时刻在地图上的映射值来衡量子滤波器性质;
设t时刻的观测到的第i个路标点的坐标为(xi,t,yi,t),地图中与之匹配的t-1时刻的路标点的坐标为(mix,t-1,miy,t-1);定义匹配误差为同一路标点相邻时刻的距离误差,表示为ri;则有
此时,利用匹配误差定义融合权重:设第i个子滤波器匹配误差为ri,定义相应子滤波器在融合过程中的权重为δr,i;
根据以上定义,匹配误差用来衡量各子滤波器的可信赖程度;子滤波器的匹配误差越大,该子滤波器的观测噪声越大,子滤波器的观测精度越低,融合时子滤波器的信任度低,自滤波器的权重值就越小;相反,子滤波器的匹配误差越小,该子滤波器的观测噪声越小,子滤波器的观测精度越高,融合时给与该子滤波器较高的信任度,即该子滤波器的权重值就越高;
在融合算法中,每个子系统存在以上两个融合子模块,即误差协方差模块和匹配融合模块;两个子模块分别采用协方差阵各项系数以及匹配最小邻域的误差进行权重的求解,最后,按照式(24)通过子模块的权重,计算最终的权重分配情况:
δp,i·α+δr,i·γ=ηi (24)
在公式(24)中δp,i是第i个子系统由协方差子模块计算出的权重值,δr,i是第i个子系统由匹配融合模块计算出的权重值;ηi是改进的信息融合方法综合计算出的子系统权重;而α和γ分别是δp,i和δr,i在改进的信息融合方法中所占的比例,其中α和γ服从公式(25);
α+γ=1 (25)
因此,主滤波器输出的t时刻机器人位姿的最优估计结果满足公式(26);
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610541854.XA CN106197428B (zh) | 2016-07-10 | 2016-07-10 | 一种利用测量信息优化分布式ekf估计过程的slam方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610541854.XA CN106197428B (zh) | 2016-07-10 | 2016-07-10 | 一种利用测量信息优化分布式ekf估计过程的slam方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106197428A CN106197428A (zh) | 2016-12-07 |
CN106197428B true CN106197428B (zh) | 2019-03-22 |
Family
ID=57476244
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610541854.XA Active CN106197428B (zh) | 2016-07-10 | 2016-07-10 | 一种利用测量信息优化分布式ekf估计过程的slam方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106197428B (zh) |
Families Citing this family (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106767792A (zh) * | 2017-01-16 | 2017-05-31 | 东南大学 | 一种水下滑翔器导航系统及高精度姿态估计方法 |
CN106680765B (zh) * | 2017-03-03 | 2024-02-20 | 济南大学 | 基于分布式组合滤波ins/uwb行人导航系统及方法 |
CN106989773B (zh) * | 2017-04-07 | 2019-07-16 | 浙江大学 | 一种姿态传感器及姿态更新方法 |
DE102017108107A1 (de) * | 2017-04-13 | 2018-10-18 | Volkswagen Aktiengesellschaft | Verfahren, vorrichtung und computerlesbares speichermedium mit instruktionen zur schätzung einer pose eines kraftfahrzeugs |
CN107391450A (zh) * | 2017-06-30 | 2017-11-24 | 上海德衡数据科技有限公司 | 一种基于gpb算法的运维多模态决策系统原型 |
CN107765242A (zh) * | 2017-09-16 | 2018-03-06 | 太原理工大学 | 基于状态增广迭代扩展卡尔曼滤波的系统状态估计方法 |
CN109542093B (zh) * | 2017-09-22 | 2022-06-07 | 华为技术有限公司 | 一种处理数据的方法和装置 |
US10852443B2 (en) * | 2018-05-10 | 2020-12-01 | Here Global B.V. | Algorithm and architecture for map-matching streaming probe data |
CN108648214B (zh) * | 2018-06-14 | 2022-04-29 | 广东工业大学 | 一种基于宽度学习滤波的视觉slam算法及系统 |
CN109405837B (zh) * | 2018-11-14 | 2021-06-08 | 蔚来(安徽)控股有限公司 | 物体定位方法、应用和车辆 |
CN109520503A (zh) * | 2018-11-27 | 2019-03-26 | 南京工业大学 | 一种平方根容积模糊自适应卡尔曼滤波slam方法 |
CN111123860B (zh) * | 2019-12-11 | 2020-11-10 | 山东科技大学 | 带有非对称信息的网络系统的最优分布式控制方法 |
CN111174782B (zh) * | 2019-12-31 | 2021-09-17 | 智车优行科技(上海)有限公司 | 位姿估计方法、装置、电子设备及计算机可读存储介质 |
CN111595354B (zh) * | 2020-05-27 | 2022-12-20 | 东南大学 | 一种自适应动态观测域的ekf-slam算法 |
CN117848332B (zh) * | 2024-03-07 | 2024-05-03 | 北京理工大学前沿技术研究院 | 一种车载多源融合高精度定位系统的imu噪声消除方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102706342A (zh) * | 2012-05-31 | 2012-10-03 | 重庆邮电大学 | 一种智能移动机器人的定位与环境建模方法 |
CN105606104A (zh) * | 2016-03-17 | 2016-05-25 | 北京工业大学 | 基于航向辅助分布式slam的机器人自主导航方法 |
CN105737832A (zh) * | 2016-03-22 | 2016-07-06 | 北京工业大学 | 基于全局最优数据融合的分布式slam方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20050015912A (ko) * | 2003-08-14 | 2005-02-21 | 삼성전자주식회사 | 무선 호출망 이용한 네비게이션 시스템 및 그 교통정보제공 방법 |
-
2016
- 2016-07-10 CN CN201610541854.XA patent/CN106197428B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102706342A (zh) * | 2012-05-31 | 2012-10-03 | 重庆邮电大学 | 一种智能移动机器人的定位与环境建模方法 |
CN105606104A (zh) * | 2016-03-17 | 2016-05-25 | 北京工业大学 | 基于航向辅助分布式slam的机器人自主导航方法 |
CN105737832A (zh) * | 2016-03-22 | 2016-07-06 | 北京工业大学 | 基于全局最优数据融合的分布式slam方法 |
Non-Patent Citations (2)
Title |
---|
On the Linear and Nonlinear Observability Analysis of the SLAM Problem;L.D.L.Perera;《Proceedings of the 2009 IEEE International Conference on Mechatronics》;20080430;正文第1-6页 |
基于自适应渐消EKF的FastSLAM算法;刘丹等;《系统工程与电子技术》;20160331(第3期);第645-651页 |
Also Published As
Publication number | Publication date |
---|---|
CN106197428A (zh) | 2016-12-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106197428B (zh) | 一种利用测量信息优化分布式ekf估计过程的slam方法 | |
Luo et al. | Enriched indoor map construction based on multisensor fusion approach for intelligent service robot | |
CN105823480B (zh) | 基于单信标的水下移动目标定位算法 | |
CN104809326B (zh) | 一种异步传感器空间配准算法 | |
CN105606104B (zh) | 基于航向辅助分布式slam的机器人自主导航方法 | |
CN106052688B (zh) | 基于地形轮廓匹配的惯性导航系统速度累积误差修正方法 | |
CN101576384B (zh) | 一种基于视觉信息校正的室内移动机器人实时导航方法 | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
CN105704652B (zh) | 一种wlan/蓝牙定位中的指纹库采集和优化方法 | |
CN106597416B (zh) | 一种地面GPS辅助的LiDAR数据高程差的误差修正方法 | |
CN107491070A (zh) | 一种移动机器人路径规划方法及装置 | |
CN106441275A (zh) | 一种机器人规划路径的更新方法及装置 | |
RU2662913C2 (ru) | Способ локализации робота в плоскости локализации | |
CN108592914A (zh) | 无gps情况下的复杂区域巡视机器人定位、导航及授时方法 | |
CN103994765B (zh) | 一种惯性传感器的定位方法 | |
CN109765523A (zh) | 基于自适应akf的单应答器斜距水声定位方法及系统 | |
KR20120046974A (ko) | 이동 로봇 및 이동 로봇의 위치인식 및 지도작성 방법 | |
CN105136054A (zh) | 基于地面三维激光扫描的构筑物精细变形监测方法及系统 | |
CN101520328A (zh) | 地磁场线图匹配自主导航方法 | |
KR20130063915A (ko) | 실내 측위를 위한 경로 추정 방법 및 그 장치 | |
JP2016024598A (ja) | 自律移動装置の制御方法 | |
CN109633664A (zh) | 基于rgb-d与激光里程计的联合定位方法 | |
CN110455309A (zh) | 具备在线时间校准的基于msckf的视觉惯性里程计 | |
CN109059907A (zh) | 轨迹数据处理方法、装置、计算机设备和存储介质 | |
CN106525042A (zh) | 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |