基于全局最优数据融合的分布式SLAM方法技术

本发明专利技术公开了基于全局最优数据融合的分布式SLAM方法，本方法是利用分布式的结构将整个状态向量分为机器人位姿估计和路标估计共五维状态，将本应以矩阵形式集中计算的描述机器人位姿的观测分布概率公式进行分布化处理，依据每个有效路标点单独建立多个相互平行独立的子滤波器，然后将子滤波器的机器人的位姿估计结果在主滤波器中进行融合，并将子滤波器的融合结果通过全局预测器反馈修正，最后得到全局最优的机器人位姿估算结果。最后通过真实实验使用本发明专利技术算法和集中式算法对比，证明了本方法的可行性和有效性。

【技术实现步骤摘要】

基于全局最优数据融合的分布式扩展卡尔曼SLAM算法是利用分布式的结构将整个状态向量分为机器人位姿估计和路标估计共五维状态，将本应以矩阵形式集中计算的描述机器人位姿的观测分布概率公式进行分布化处理，依据每个有效路标点单独建立多个相互平行独立的子滤波器，然后将子滤波器的机器人的位姿估计结果在主滤波器中进行融合，并将子滤波器的融合结果通过全局预测器反馈修正，最后得到全局最优的机器人位姿估算结果。属于机器人自主导航领域。
技术介绍
SLAM(SimultaneousLocalizationandMapping)即同步定位与地图构建，其基本思想是：让机器人在未知环境中从未知位置开始移动,通过自身所带传感器扫描到的路标点的信息进行自身位置估计，同时构建增量式地图。在SLAM过程中，根据观测信息进行实时的地图估计与更新，其中滤波器设计，数据融合算法都是关键问题。SLAM中的数据融合是指利用计算机技术对按时序获得的机器人传感器的观测信息在一定准则下加以自动分析、综合以完成所需的决策和估计任务而进行的信息处理过程。机器人传感器系统是数据融合的硬件基础，多源信息是数据融合的加工对象，协调优化和综合处理是数据融合的核心。基于分布式结构的SLAM算法是与集中式SLAM算法相对应的，集中式滤波器结构是用一个滤波器完成对位姿与环境地图的同时估计，这种结构中的状态向量均是包括机器人和路标信息的高维向量，并且状态向量维数将随环境动态时变增加，必然造成集中式SLAM算法存在计算量大、稳定性和容错性差等问题。目前，数据融合方法主要包括基于对象的统计特性和概率模型的方法、直接对数据源操作的方法、基于规则推理的方法这三大类。其中基于对象的统计特性和概率模型的方法主要包括Kalman滤波法、贝叶斯估计法、多贝叶斯估计法和统计决策理论法。直接对数据源操作的方法主要有加权平均法和神经网络法。基于规则推理的方法主要有D-S证据理论、产生式规则法、模糊集理论法和粗糙集理论等算法。上述方法各有特点，也存在一些不足，主要体现在：(1)未形成基本的理论框架和有效广义模型及算法。虽然数据融合的应用已相当广泛，但是，数据融合本身至今未形成基本的理论框架和有效的广义模型及算法。其绝大部分工作都是针对特定应用领域内的问题来开展研究，也就是说，目前对数据融合问题的研究都是根据问题的种类，各自建立直观认识原理(即融合准则)，并在此基础上形成所谓的最佳融合方案，如典型的分布式检测融合，已从理论上解决了最优融合准则、最优局部决策准则和局部决策门限的最优协调方法，并给出了相应的算法。但这些研究反映的只是数据融合所固有的面向对象的特点，也就难以构成数据融合这一独立学科所必需的完整理论体系。这一理论短缺现象阻碍了研究者对数据融合本身的深入认识，也使得数据融合在某种程度上仅被看成是一种多传感器信息处理概念。人们无法对面向对象的融合系统做出综合分析和评估，使得融合系统的设计带有一定的盲目性。(2)关联的二义性是数据融合中的主要障碍。在进行数据融合之前，必须对信息进行关联，以保证所融合的信息是来自同一目标或事件，及保证融合信息的一致性。如果对不同目标或事件的信息进行融合，将难以使系统得出正确的结论，这一问题称为关联的二义性，是数据融合中克服的主要障碍。由于在多传感器信息系统中引起关联二义的原因很多，例如传感器测量的不精确性、干扰等，因此，怎样确立信息可融合性的判断准则，如何进一步降低关联的二义性已成为融合研究领域亟待解决的问题。(3)融合系统的容错性或稳健性没有得到很好的解决。冲突(矛盾)信息或传感器故障所产生的错误信息等的有效处理，即系统的容错性或稳健性也是数据融合理论中必须考虑的问题。诸如机器人移动过程中的动态调整和传感器之间的时钟匹配等问题。
技术实现思路
本专利技术针对现有SLAM数据融合方法的不足，提出一种基于全局最优数据融合的分布式SLAM方法。将整个状态向量分为机器人位姿估计和路标估计共五维状态，依据每个有效路标点单独建立多个子滤波器，然后将子滤波器的机器人的位姿估计结果在主滤波器中进行融合，得到最终的机器人位姿估计。下文将分别讲述分布式SLAM和融合算法的原理和内容。最后通过真实实验证明了本专利技术算法的有效性。由于状态方程和测量方程均为非线性，所以采用扩展卡尔曼滤波，扩展卡尔曼是对非线性函数的泰勒展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性，可以将卡尔曼线性滤波算法应用于非线性系统中。SLAM系统状态模型可看作一个马尔科夫过程，即t时刻状态与t-1时刻前的状态无关。系统中机器人的运动控制信息由里程计提供，如图1所示，为机器人的状态转移过程，图2为实际机器人坐标系。机器人的状态st可以描述如下：st＝(x,y,θ)T，其中，x，y表示对应的横纵坐标值，θ表示偏航角(θ＝0表示指向x轴正向，θ＝π/2表示指向y轴正向)。机器人的状态转移模型如下：XL(t+1)=xL(t+1)yL(t+1)φL(t+1)=f(XL(t))+ω=xL(t)+ΔT(vccos(φ)-vcLtan(φ)(asin(φ)+bcos(φ)))yL(t)+ΔT(vcsin(φ)-vcLtan(φ)(asin(φ)-bcos(φ)))φL(t)+ΔTvcLtan(α)+ω---(1)]]>式中XL(t)＝(xL(t)yL(t)φL(t))T为机器人在t时刻的位姿，下标L表示的是里程计的信息，xL(t)，yL(t)表示t时刻对应的横纵坐标值，φL(t)表示t时刻航向角,ΔT为时间变化量，vc为机器人移动速度，α为机器人变化的角度，L为两轮轴间距，ω是协方差为Q均值为零的高斯白噪声。机器人位姿的估计是由激光传感器测得的观测信息来推断的，本文针对激光传感器给出机器人的观测模型，测距激光传感器的观测量z是某个环境特征相对于传感器的距离以及角度，在扫描目标时容易出现误差，实际状况中，误差密度函数由各种分布组合而成，一般情况包括：指数分布，均匀分布，高斯分布等，本文采用高斯分布描述误差的分布情况。将机器人状态信息与观测信息相关联，可以获得路标点的观测模型：ztk=zrkzθk=(mi,x-xi)2+(mi,y-yi)2φi-atan(-yi,y-yixi,x-xi)+π2+v---(2)]]>式中，为观测量包括传感器测得的机器人与路标点间距离z本文档来自技高网...

【技术保护点】
基于全局最优数据融合的分布式SLAM方法，其特征在于：本方法是利用分布式的结构将整个状态向量分为机器人位姿估计和路标估计共五维状态，将本应以矩阵形式集中计算的描述机器人位姿的观测分布概率公式进行分布化处理，依据每个有效路标点单独建立多个相互平行独立的子滤波器，然后将子滤波器的机器人的位姿估计结果在主滤波器中进行融合，并将子滤波器的融合结果通过全局预测器反馈修正，最后得到全局最优的机器人位姿估算结果；由于状态方程和测量方程均为非线性，所以采用扩展卡尔曼滤波，扩展卡尔曼是对非线性函数的泰勒展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性，可以将卡尔曼线性滤波算法应用于非线性系统中；SLAM系统状态模型可看作一个马尔科夫过程，即t时刻状态与t‑1时刻前的状态无关；系统中机器人的运动控制信息由里程计提供；机器人的状态st描述如下：st＝(x,y,θ)T，其中，x，y表示对应横纵坐标值，θ表示偏航角(θ＝0表示指向x轴正向，θ＝π/2表示指向y轴正向)；机器人的状态转移模型如下：XL(t+1)=xL(t+1)yL(t+1)φL(t+1)=f(XL(t))+ω=xL(t)+ΔT(vccos(φ)-vcLtan(φ)(asin(φ)+bcos(φ)))yL(t)+ΔT(vcsin(φ)-vcLtan(φ)(asin(φ)-bcos(φ)))φL(t)+ΔTvcLtan(α)+ω---(1)]]>式中XL(t)＝(xL(t) yL(t) φL(t))T为机器人在t时刻的位姿,下标L表示的是里程计的信息，xL(t)，yL(t)表示t时刻对应的横纵坐标值，φL(t)表示t时刻航向角，ΔT为时间变化量，vc为机器人移动速度，α为机器人变化的角度，L为两轮轴间距，ω是协方差为Q均值为零的高斯白噪声；机器人位姿的估计是由激光传感器测得的观测信息来推断的，本方法针对激光传感器给出机器人的观测模型，测距激光传感器的观测量z是某个环境特征相对于传感器的距离以及角度，在扫描目标时容易出现误差，实际状况中，误差密度函数由各种分布组合而成，一般情况包括：指数分布、均匀分布、高斯分布，本方法采用高斯分布描述误差的分布情况；将机器人状态信息与观测信息相关联，获得路标点的观测模型：ztk=zrkzθk=(mi,x-xi)2+(mi,y-yi)2φi-atan(-yi,y-yixi,x-xi)+π2+v---(2)]]>式中，为观测量包括传感器测得的机器人与路标点间距离zr和角度zθ，上标K表示1到K次观测值。Xi＝(xi yi)T为路标点坐标，下标i表示的是路标点的信息。v是协方差为R均值为0的高斯白噪声；mi,x mi,y为已存到地图里路标点的横纵坐标，xi,x，yi,x表示根据小车坐标估计的路标点坐标。；当观测到n个有效特征点时观测方程为：z=z1z2...zn=h(xr(t),m1)h(xr(t),m2)...h(xr(t),mn)+v(t)---(3)]]>式中，h(xr(t),mi)为上述路标点观测模型，V(t)为协方差为R均值为0的高斯白噪声；这里构建的SALM状态向量分为机器人位姿估计和路标估计共五维状态，即xv＝[xL,yL,φL,xi,yi]T，系统的状态方程如下：XL(t+1)=xL(t+1)yL(t+1)φL(t+1)xi(t+1)yi(t+1)=f(XL(t))+ω=xL(t)+ΔT(vccos(φ)-vcLtan(φ)(asin(φ)+bcos(φ)))yL(t)+ΔT(vcsin(φ)-vcLtan(φ)(asin(φ)-bcos(φ)))φL(t)+ΔTvcLtan(α)xL(t)+r*cos(φL(t)+α)yL(t)+r*sin(φL(t)+α)+ω---(4)]]>将小车航向φL加入观测，并利用扩展卡尔曼线性化后观测模型的雅可比矩阵如下：∂h∂X=∂hr∂X∂hθ∂X∂hφ∂X=∂zr∂(xL,yL,φL,{xi,yi})∂zθ∂(xL,yL,φL,{xi,yi})∂zφ∂(xL,yL,φL,{xi,yi})---(5)]]>其中∂hr&pa...

【技术特征摘要】
1.基于全局最优数据融合的分布式SLAM方法，其特征在于：本方法是利用分布式的结
构将整个状态向量分为机器人位姿估计和路标估计共五维状态，将本应以矩阵形式集中计
算的描述机器人位姿的观测分布概率公式进行分布化处理，依据每个有效路标点单独建立
多个相互平行独立的子滤波器，然后将子滤波器的机器人的位姿估计结果在主滤波器中进
行融合，并将子滤波器的融合结果通过全局预测器反馈修正，最后得到全局最优的机器人
位姿估算结果；
由于状态方程和测量方程均为非线性，所以采用扩展卡尔曼滤波，扩展卡尔曼是对非
线性函数的泰勒展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为
线性，可以将卡尔曼线性滤波算法应用于非线性系统中；
SLAM系统状态模型可看作一个马尔科夫过程，即t时刻状态与t-1时刻前的状态无关；
系统中机器人的运动控制信息由里程计提供；
机器人的状态st描述如下：st＝(x,y,θ)T，其中，x，y表示对应横纵坐标值，θ表示偏航角
(θ＝0表示指向x轴正向，θ＝π/2表示指向y轴正向)；机器人的状态转移模型如下：
XL(t+1)=xL(t+1)yL(t+1)φL(t+1)=f(XL(t))+ω=xL(t)+ΔT(vccos(φ)-vcLtan(φ)(asin(φ)+bcos(φ)))yL(t)+ΔT(vcsin(φ)-vcLtan(φ)(asin(φ)-bcos(φ)))φL(t)+ΔTvcLtan(α)+ω---(1)]]>式中XL(t)＝(xL(t)yL(t)φL(t))T为机器人在t时刻的位姿,下标L表示的是里程计的
信息，xL(t)，yL(t)表示t时刻对应的横纵坐标值，φL(t)表示t时刻航向角，ΔT为时间变化
量，vc为机器人移动速度，α为机器人变化的角度，L为两轮轴间距，ω是协方差为Q均值为零
的高斯白噪声；
机器人位姿的估计是由激光传感器测得的观测信息来推断的，本方法针对激光传感器
给出机器人的观测模型，测距激光传感器的观测量z是某个环境特征相对于传感器的距离
以及角度，在扫描目标时容易出现误差...

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