[发明专利]基于全局最优数据融合的分布式SLAM方法

摘要

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

主权项

1.基于全局最优数据融合的分布式SLAM方法，其特征在于：本方法是利用分布式的结构将整个状态向量分为机器人位姿估计和路标估计共五维状态，将本应以矩阵形式集中计算的描述机器人位姿的观测分布概率公式进行分布化处理，依据每个有效路标点单独建立多个相互平行独立的子滤波器，然后将子滤波器的机器人的位姿估计结果在主滤波器中进行融合，并将子滤波器的融合结果通过全局预测器反馈修正，最后得到全局最优的机器人位姿估算结果；由于状态方程和测量方程均为非线性，所以采用扩展卡尔曼滤波，扩展卡尔曼是对非线性函数的泰勒展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性，将卡尔曼线性滤波算法应用于非线性系统中；SLAM系统状态模型可看作一个马尔科夫过程，即t时刻状态与t‑1时刻前的状态无关；系统中机器人的运动控制信息由里程计提供；机器人的状态st描述如下：st＝(x,y,θ)T，其中，x，y表示对应横纵坐标值，θ表示偏航角，θ＝0表示指向x轴正向，θ＝π/2表示指向y轴正向；机器人的状态转移模型如下：式中XL(t)＝(xL(t) yL(t) φL(t))T为机器人在t时刻的位姿,下标L表示的是里程计的信息，xL(t)，yL(t)表示t时刻对应的横纵坐标值，φL(t)表示t时刻航向角，ΔT为时间变化量，vc为机器人移动速度，α为机器人变化的角度，L为两轮轴间距，ω是过程噪声协方差为Q均值为零的高斯白噪声；机器人位姿的估计是由激光传感器测得的观测信息来推断的，本方法针对激光传感器给出机器人的观测模型，测距激光传感器的观测量z是某个环境特征相对于传感器的距离以及角度，在扫描目标时容易出现误差，实际状况中，误差密度函数由各种分布组合而成，包括：指数分布、均匀分布、高斯分布，本方法采用高斯分布描述误差的分布情况；将机器人状态信息与观测信息相关联，获得路标点的观测模型：式中，为观测量包括传感器测得的机器人与路标点间距离z_r和角度z_θ，上标K表示1到K次观测值；X_i＝(x_i y_i)^T为路标点坐标，下标i表示的是路标点的信息；v是协方差为R均值为0的高斯白噪声；m_i,x m_i,y为已存到地图里路标点的横纵坐标，x_i,x，y_i,x表示根据小车坐标估计的路标点坐标；当观测到n个有效特征点时观测方程为：式中，h(xr(t),mi)为上述路标点观测模型，V(t)为协方差为R均值为0的高斯白噪声；这里构建的SALM状态向量分为机器人位姿估计和路标估计共五维状态，即xv＝[xL,yL,φL,xi,yi]T，系统的状态方程如下：将小车航向φL加入观测，并利用扩展卡尔曼线性化后观测模型的雅可比矩阵如下：其中Δx＝[xi‑xL]  (9)Δy＝[yi‑yL]  (10)然后利用分布式模型，将利用扩展卡尔曼线性化后观测模型改写为如下形式：可观测性分析是系统设计与控制领域的重要研究内容与步骤；本方法状态方程是由小车状态和一个路标点的五维状态构成，观测除测量距离和角度外引入小车航向角；首先为了证明模型的可行性和滤波器的稳定性，就要先分析模型的可观性；首先描述什么是可观测性；系统可观测性的定义是：如果系统在t时刻的状态矢量X(t0)能够从时间区间[t0,t1]内系统的输出函数Y(t)中唯一确定出来，则称该系统是可观测的，否则称为不完全可观测或不可观测；通过扩展卡尔曼线性化以后，只需计算系统的过程噪声协方差Q的秩即可判断系统的可观测性；即过程噪声协方差Q为：Q＝[HT,(HA)T,...,(HAn‑1)T]T  (13)如果过程噪声协方差Q的可观测性矩阵Pc的秩等于n，则称系统是完全可观测的，如果Pc的秩小于n，则称系统是不完全可观测的；对于完全可观测的模型，可观测性矩阵Pc为：Pc＝[HT,(H)T,...,(H)T]T  (14)那么因此系统模型是可观的；从模型(3)和(12)看出，SALM问题是非线性非高斯的状态估计问题，首先通过扩展卡尔曼算法将非线性问题转化为线性，再利用卡尔曼算法进行估计，其次在状态模型和观测模型中分别加入均值为零的高斯白噪声，然后建立分布式多子滤波器结构，大大优化了SLAM建模和估计过程；集中式SALM算法是采用了单一滤波器实现对机器人位姿和路标点的估计，不可避免的存在计算量大、复杂度高、容错性差、实时性差的问题；而分布式SALM算法是将本应以矩阵形式集中计算的描述机器人位姿的观测分布概率公式进行分布化处理，之后在子滤波器中处理概率计算，最后根据相应的融合算法进行主滤波器融合；这样大大减少了计算复杂度，提高系统容错能力，下面着重阐述滤波融合方法的实现；全局最优的滤波融合算法的基本概念是通过多个独立平行的子滤波器对传感器测量的处理，实现全局状态及其协方差的预测和更新，然后在主滤波器中融合更新后的本地信息，输出最优估计；假设子滤波器和主滤波器是不相关的，那么全局估计是：下角标“f”，“i”，和“m”各自表示融合滤波器，子滤波器和主滤波器；P表示协方差矩阵，x表示状态量；如果子滤波器和主滤波器是独立的，那么通过全局预测器将次最优的输出反馈修正来实现全局的最优估计；在分布式卡尔曼中，由于子滤波器和主滤波器共用相同的系统模型，所以它们的估计结果是必然相关的，为了消除这种相关，通过下面的式子对协方差矩阵P和过程噪声协方差Q进行约束：β_i是相关因子，β_i≥0，满足每个子滤波器预测过程为：子滤波器和主滤波器中的量测更新为：主滤波器中的融合采用加权平均法，其中wi为加权系数，加权平均法将来自不同子滤波器的机器人位姿估计信息计算权值，进行加权平均，结果作为融合值；由以上可推导出反馈修正的全局最优融合结果：对误差的补偿及机器人动态调整过程不确定噪声的干扰有较好的估计，对存在相关性过程具有很好的估计性能，达到全局最优的估计效果；具体步骤如下：步骤1：建立模型和线性化；将整个状态向量分为机器人位姿估计和路标估计共五维状态，其状态向量为：xv＝[xL,yL,φL,xi,yi]T，xL,yL,φL是小车的状态，xi,yi是路标点的状态；扩展卡尔曼线性化后观测模型的雅可比矩阵H为(5)式；步骤2：初始化；建立坐标系，以机器人初始时刻的位置为坐标原点，以正东和正北方向为x轴和y轴的正方向；全局地图初始化，机器人在初始位置，利用传感器扫描测得的路标点位置信息建立环境地图，并把它与机器人初始时刻的位姿即机器人的位置和角度一起作为全局地图进行存储；步骤3：地图匹配并建立分布式子滤波器；获取机器人t‑1时刻测得的路标点状态信息，并与已在全局地图中存储的路标点信息相匹配，对于t‑1时刻测得的未匹配上的路标点状态信息，直接将其添加至全局地图；对于t‑1时刻测得的能够匹配上的路标点分别对其建立子滤波器，用于并行分布式滤波器对机器人下一时刻的状态和路标点信息进行估计；步骤4：通过扩展卡尔曼滤波算法更新状态；通过扩展卡尔曼滤波算法对上述步骤中建立的各个子滤波器的状态向量和协方差分别进行状态更新和协方差更新，包括时间更新和量测更新两个方面；时间更新：首先把t‑1时刻的每个子滤波器的估计状态和协方差矩阵，经过一步预测方程(19)和(20)进行时间更新；量测更新：现在有了现在状态的预测结果，然后再收集现在状态的测量值；结合预测值和测量值，通过式(21)，(22)和(23)得到现在状态的最优化估算值和协方差的更新值；通过上述时间更新和量测更新，对机器人位姿和路标点进行了更新；步骤5：计算权值并归一化；根据步骤4中各个子滤波器得到的机器人状态的最优估算值，计算每个子滤波器状态和协方差的权重，然后根据式子(24)和(25)，利用加权平均法归一化权值；步骤6：建立主滤波器，并对子滤波器数据融合；把子滤波器中机器人位姿的状态向量及其权值传送给主滤波器，通过主滤波器对子滤波器中所有数据进行融合计算，见式子(26)和(27)最终输出t时刻机器人的反馈修正全局状态估计值和方差值；步骤7：输出最终结果；把所有子滤波器得到的最后一次t时刻机器人的全局状态估计和方差作为最终结果输出，然后把所有子滤波器得到的最后一次t时刻机器人的全局估计存入全局地图；步骤8：判断机器人是否继续运动，如果继续则跳转至步骤3，否则程序终止。