一种利用测量信息优化分布式EKF估计过程的SLAM方法技术

一种利用测量信息优化分布式EKF估计过程的SLAM方法，在利用测量信息优化分布式EKF估计过程的SLAM方法中，根据路标点信息对应建立若干平行的子系统，各子系统单独进行滤波估计，将各自的估计结果送到主滤波器中，最后输出最优的机器人位姿估计结果。本发明专利技术针对分布式SLAM系统中EKF子滤波器估计精度存在的局限性问题，采用由传感器直接测得的观测量计算滤波器雅克比矩阵，利用改进的线性化方法提高子滤波器的估计精度。考虑到一致性对系统的影响，采用误差协方差阵和匹配信息共同参与数据融合过程的方法改进系统融合的方案，充分发挥分布式结构下观测信息的作用，保证系统在精度较高或一致性较高的情况下均能合理地做出融合判定。

【技术实现步骤摘要】

在利用测量信息优化分布式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,rT,mt,iT)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个有效的特征点，分布式系统可表示为： X · r = f ( X r ) + w z 1 = h ( X r ( t ) , m 1 ) + v 1 X · r 本文档来自技高网...

【技术保护点】
一种利用测量信息优化分布式EKF估计过程的SLAM方法，其特征在于：本方法针对分布式SLAM系统中EKF子滤波器估计精度存在的局限性问题，采用由传感器直接测得的观测量计算滤波器雅克比矩阵，提高了子滤波器估计精度；并且考虑到一致性对系统的影响，改进分布式系统融合的方案，提出利用误差协方差阵和匹配信息共同参与数据融合过程的方式对算法进行优化；下文将具体描述优化分布式结构中的子滤波器以及分布式系统的融合方案的过程；最后通过真实实验证明了本算法的可行性与有效性；机器人的运动模型和观测模型如图所示；在分布式SLAM问题中，假设机器人的姿态表示为其中(xr,yr)表示机器人在地图中的坐标，表示航向角；路标点在地图中的位置表示为：mi＝(xi,yi)T,i＝1:n，其中mi表示第i个路标点对应的位置信息，(xi,yi)表示路标点在地图中的坐标；t时刻机器人的状态矢量表示为Xt＝(xt,rT,mt,iT)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个有效的特征点，分布式系统可表示为：X·r=f(Xr)+wz1=h(Xr(t),m1)+v1X·r=f(Xr)+wz2=h(Xr(t),m2)+v2...X·r=f(Xr)+wzn=h(Xr(t),mn)+vn---(3)]]>可见，在分布式SLAM中，根据观测到的路标点信息建立相互平行的子滤波器，每一个子滤波器都有各自的状态方程和观测方程；最终每一个子滤波器的估计结果都被传送到主滤波器，而在主滤波器中，根据各个子滤波器的估计结果及相应的误差情况计算出机器人位姿的最优结果；在分布式SLAM算法子滤波器估计单元中，EKF算法的第一步是根据运动模型和t‑1时刻的状态对t时刻状态进行预测x‾t,r*x‾t,i*=f(x‾t-1,r,ut-1)x‾t-1,i---(4)]]>其中，和是机器人在t‑1时刻所估计的机器人位置结果和路标点位置结果；第i个子系统t时刻的观测方程具体形式如下：解决SLAM问题时，EKF是在噪声符合零均值的高斯白噪声的假设下，利用将f(xt‑1,rut‑1)和h(xr(t),mi)展成泰勒级数并略去二阶以上高阶项从而得到非线性系统的线性化模型；因此，EKF关键的步骤是准确地求解状态方程和观测方程的两个雅克比矩阵；计算状态转移雅克比矩阵▿ft-1=∂f∂Xr|x‾t-1,r---(6)]]>计算观测信息雅克比矩阵▿ht=∂h∂X|x‾t*---(7)]]>预测结果的协方差阵如下：Pt*=Prr,t*Pri,t*Pir,t*Pii,t*=▿ft-1Prr,t-1▿ft-1T+Qt-1▿ft-1Pri,t-1Pir,t-1▿ft-1TPii,t-1]]>Pir,t-1=Pri,t-1T]]>其中Prr,t‑1和Pri,t‑1分别是t‑1时刻机器人状态向量协方差和机器人与路标点状态向量的协方差；Pii,t‑1和Qt‑1分别是t‑1时刻路标点状态向量的协方差和机器人运动的过程噪声的协方差；t时刻第i个子系统根据运动模型预测的和Pt*按照以下方式进行更新过程：Ki(t)=Pi*(t)▿hiT[▿hiPi*(t)▿hiT+Ri]-1,i=1,2...,n---(9)]]>X‾i(t)=X‾i*(t)+Ki(t)[Zi(t)-Hi(t)X‾i*(t)],i=1,2...,n---(10)]]>Pi(t)=[I-Ki(t)▿hi]Pi*(t),i=1,2...,n---(11)]]>其中，Ki(t)和分别是子滤波器t时刻的卡尔曼增益和观测方程的雅克比矩阵，Rt代表观测噪声矩阵，和Pi(t)分别代表子滤波器t时刻对应的状态向量估计值和相应的协方差；可见，t时刻子系统的位姿估计是根据泰勒一阶展开通过线性化的h(Xt)进而计算得到的，即Z=...

【技术特征摘要】
1.一种利用测量信息优化分布式EKF估计过程的SLAM方法，其特征在于：本方法针对分布式SLAM系统中EKF子滤波器估计精度存在的局限性问题，采用由传感器直接测得的观测量计算滤波器雅克比矩阵，提高了子滤波器估计精度；并且考虑到一致性对系统的影响，改进分布式系统融合的方案，提出利用误差协方差阵和匹配信息共同参与数据融合过程的方式对算法进行优化；下文将具体描述优化分布式结构中的子滤波器以及分布式系统的融合方案的过程；最后通过真实实验证明了本算法的可行性与有效性；机器人的运动模型和观测模型如图所示；在分布式SLAM问题中，假设机器人的姿态表示为其中(xr,yr)表示机器人在地图中的坐标，表示航向角；路标点在地图中的位置表示为：mi＝(xi,yi)T,i＝1:n，其中mi表示第i个路标点对应的位置信息，(xi,yi)表示路标点在地图中的坐标；t时刻机器人的状态矢量表示为Xt＝(xt,rT,mt,iT)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个有效的特征点，分布式系统可表示为： X · r = f ( X r ) + w z 1 = h ( X r ( t ) , m 1 ) + v 1 X · r = f ( X r ) + w z 2 = h ( X r ( t ) , m 2 ) + v 2 ... X · r = f ( X r ) + w z n = h ( X r ( t ) , m n ) + v n - - - ( 3 ) ]]>可见，在分布式SLAM中，根据观测到的路标点信息建立相互平行的子滤波器，每一个子滤波器都有各自的状态方程和观测方程；最终每一个子滤波器的估计结果都被传送到主滤波器，而在主滤波器中，根据各个子滤波器的估计结果及相应的误差情况计算出机器人位姿的最优结果；在分布式SLAM算法子滤波器估计单元中，EKF算法的第一步是根据运动模型和t-1时刻的状态对t时刻状态进行预测 x ‾ t , r * x ‾ t , i * = f ( x ‾ t - 1 , r , u t - 1 ) x ‾ t - 1 , i - - - ( 4 ) ]]>其中，和是机器人在t-1时刻所估计的机器人位置结果和路标点位置结果；第i个子系统t时刻的观测方程具体形式如下：解决SLAM问题时，EKF是在噪声符合零均值的高斯白噪声的假设下，利用将f(xt-1,rut-1)和h(xr(t),mi)展成泰勒级数并略去二阶以上高阶项从而得到非线性系统的线性化模型；因此，EKF关键的步骤是准确地求解状态方程和观测方程的两个雅克比矩阵；计算状态转移雅克比矩阵 ▿ f t - 1 = ∂ f ∂ X r | x ‾ t - 1 , r - - - ( 6 ) ]]>计算观测信息雅克比矩阵 ▿ h t = ∂ h ∂ X | x ‾ t * - - - ( 7 ) ]]>预测结果的协方差阵如下： P t * = P r r , t * P r i , t * P i r , t * P i i , t * = ▿ f t - 1 P r r , t - 1 ▿ f t - 1 T + Q t - 1 ▿ f t - 1 P r i , t - 1 P i r , t - 1 ▿ f t - 1 T P i i , t - 1 ]]> P i r , t - 1 = P r i , t - 1 T ]]>其中Prr,t-1和Pri,t-1分别是t-1时刻机器人状态向量协方差和机器人与路标点状态向量的协方差；Pii,t-1和Qt-1分别是t-1时刻路标点状态向量的协方差和机器人运动的过程噪声的协方差；t时刻第i个子系统根据运动模型预测的和Pt*按照以下方式进行更新过程： K i ( t ) = P i * ( t ) ▿ h i T [ ▿ h i P i * ( t ) ▿ h i T + R i ] - 1 , i = 1 , 2 ... , n - - - ( 9 ) ]]> X ‾ i ( t ) = X ‾ i * ( t ) + K i ( t ) [ Z i ( t ) - H i ( t ) X ‾ i * ( t ) ] , i = 1 , 2 ... , n - - - ( 10 ) ]]> P i ( t ) = [ I - K i ( t ) ▿ h i ] P i * ( t ) , i = 1 , 2 ... , n - - - ( 11 ) ]]>其中，Ki(t)和分别是子滤波器t时刻的卡尔曼增益和观测方程的雅克比矩阵，Rt代表观测噪声矩阵，和Pi(t)分别代表子滤波器t时刻对应的状态向量估计值和相应的协方差；可见，t时刻子系统的位姿估计是根据泰勒一阶展开通过线性化的h(Xt)进而计算得到的，即 Z = h ( X t ) ≈ h ( X 0 ) + ▿ h ( X t - X 0 ) - - - ( 12 ) ]]>其中X0表示观测方程线性化时进行泰勒展开时所在的点，即根据运动模型所预测的点最后，将Z＝Zt代入时，得到EKF SLAM的最优估计结果；当观测方程在进行泰勒展开时的斜率是如下形式时，拥有最高的估计精度， k r e a l = Z t - h ( X 0 ) X t - X 0 - - - ( 13 ) ]]>当将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) K t = P t - ▿ h t T ...

【专利技术属性】
技术研发人员：裴福俊，严鸿，程雨航，武小平，
申请(专利权)人：北京工业大学，
类型：发明
国别省市：北京;11