CN105737832A - 基于全局最优数据融合的分布式slam方法 - Google Patents

Abstract

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

Description

基于全局最优数据融合的分布式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可以描述如下：s_t＝(x,y,θ)^T，其中，x，y表示对应的横纵坐标值，θ表示偏航角(θ＝0表示指向x轴正向，θ＝π/2表示指向y轴正向)。机器人的状态转移模型如下：

$X_L(t+1)=\left[\begin{array}{c}{x}_L(t+1)\\ y_L(t+1)\\ {\mathrm{\φ}}_L(t+1)\end{array}\right]=f\left(X_L\right(t\left)\right)+\mathrm{\ω}=\left[\begin{array}{c}{x}_L\left(t\right)+\mathrm{\Δ}T\left(v_c\cos \right(\mathrm{\φ})-\frac{v_c}{L}\tan (\mathrm{\φ}\left)\right(a\sin \left(\mathrm{\φ}\right)+b\cos \left(\mathrm{\φ}\right)\left)\right)\\ y_L\left(t\right)+\mathrm{\Δ}T\left(v_c\sin \right(\mathrm{\φ})-\frac{v_c}{L}\tan (\mathrm{\φ}\left)\right(a\sin \left(\mathrm{\φ}\right)-b\cos \left(\mathrm{\φ}\right)\left)\right)\\ {\mathrm{\φ}}_L\left(t\right)+\mathrm{\Δ}T\frac{v_c}{L}\tan \left(\mathrm{\α}\right)\end{array}\right]+\mathrm{\ω}---\left(1\right)$

式中X_L(t)＝(x_L(t)y_L(t)φ_L(t))^T为机器人在t时刻的位姿，下标L表示的是里程计的信息，x_L(t)，y_L(t)表示t时刻对应的横纵坐标值，φ_L(t)表示t时刻航向角,ΔT为时间变化量，v_c为机器人移动速度，α为机器人变化的角度，L为两轮轴间距，ω是协方差为Q均值为零的高斯白噪声。

机器人位姿的估计是由激光传感器测得的观测信息来推断的，本文针对激光传感器给出机器人的观测模型，测距激光传感器的观测量z是某个环境特征相对于传感器的距离以及角度，在扫描目标时容易出现误差，实际状况中，误差密度函数由各种分布组合而成，一般情况包括：指数分布，均匀分布，高斯分布等，本文采用高斯分布描述误差的分布情况。

将机器人状态信息与观测信息相关联，可以获得路标点的观测模型：

$z_t^k=\left[\begin{array}{c}{z}_r^k\\ z_{\mathrm{\θ}}^k\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_{i,x}-x_i)}^2+{(m_{i,y}-y_i)}^2}\\ {\mathrm{\φ}}_i-a\tan (-\frac{y_{i,y}-y_i}{x_{i,x}-x_i})+\frac{\mathrm{\π}}{2}\end{array}\right]+v---\left(2\right)$

式中，为观测量包括传感器测得的机器人与路标点间距离z_r和角度z_θ，上标K表示1到K次观测值。X_i＝(x_iy_i)^T为路标点坐标，下标i表示的是路标点的信息。v是协方差为R均值为0的高斯白噪声。m_i,x，m_i,y为已存到地图里路标点的横纵坐标，x_i,x，y_i,x表示根据小车坐标估计的路标点坐标。当观测到n个有效特征点时观测方程为：

$z=\left[\begin{array}{c}{z}_1\\ z_2\\ .\\ .\\ .\\ z_n\end{array}\right]\left[\begin{array}{c}h\left(x_r\right(t),m_1)\\ h\left(x_r\right(t),m_2)\\ .\\ .\\ .\\ h\left(x_r\right(t),m_n)\end{array}\right]=+v\left(t\right)---\left(3\right)$

式中，h(x_r(t),m_i)为上述路标点观测模型，V(t)为协方差为R均值为0的高斯白噪声。

这里构建的SALM状态向量分为机器人位姿估计和路标估计共五维状态，即x_v＝[x_L,y_L,φ_L,x_i,y_i]^T，系统的状态方程如下：

$X_L(t+1)=\left[\begin{array}{c}{x}_L(t+1)\\ y_L(t+1)\\ {\mathrm{\φ}}_L(t+1)\\ x_i(t+1)\\ y_i(t+1)\end{array}\right]=f\left(X_L\right(t\left)\right)+\mathrm{\ω}\left[\begin{array}{c}{x}_L\left(t\right)+\mathrm{\Δ}T\left(v_c\cos \right(\mathrm{\φ})-\frac{v_c}{L}\tan (\mathrm{\φ}\left)\right(a\sin \left(\mathrm{\φ}\right)+b\cos \left(\mathrm{\φ}\right)\left)\right)\\ y_L\left(t\right)+\mathrm{\Δ}T\left(v_c\sin \right(\mathrm{\φ})-\frac{v_c}{L}\tan (\mathrm{\φ}\left)\right(a\sin \left(\mathrm{\φ}\right)-b\cos \left(\mathrm{\φ}\right)\left)\right)\\ {\mathrm{\φ}}_L\left(t\right)+\mathrm{\Δ}T\frac{v_c}{L}\tan \left(\mathrm{\α}\right)\\ x_L\left(t\right)+r*\cos \left({\mathrm{\φ}}_L\right(t)+\mathrm{\α})\\ y_L\left(t\right)+r*\sin \left({\mathrm{\φ}}_L\right(t)+\mathrm{\α})\end{array}\right]+\mathrm{\ω}---\left(4\right)$

将小车航向φ_L加入观测，并利用扩展卡尔曼线性化后观测模型的雅可比矩阵如下：

$\frac{\∂h}{\∂X}=\left[\begin{array}{c}\frac{\∂h_r}{\∂X}\\ \frac{\∂h_{\mathrm{\θ}}}{\∂X}\\ \frac{\∂h_{\mathrm{\φ}}}{\∂X}\end{array}\right]=\left[\begin{array}{c}\frac{\∂z_r}{\∂(x_L,y_L,{\mathrm{\φ}}_L,\{x_i,y_i\left\}\right)}\\ \frac{\∂z_{\mathrm{\θ}}}{\∂(x_L,y_L,{\mathrm{\φ}}_L,\{x_i,y_i\left\}\right)}\\ \frac{\∂z_{\mathrm{\φ}}}{\∂(x_L,y_L,{\mathrm{\φ}}_L,\{x_i,y_i\left\}\right)}\end{array}\right]---\left(5\right)$

其中

$\frac{\∂h_r}{\∂X}=\frac{1}{\mathrm{\Δ}}\[-\mathrm{\Δ}x,-\mathrm{\Δ}y,0,0,0\]---\left(6\right)$

$\frac{\∂h_{\mathrm{\β}}}{\∂X}=\[\frac{\mathrm{\Δ}y}{{\mathrm{\Δ}}^2},-\frac{\mathrm{\Δ}x}{{\mathrm{\Δ}}^2},-1,0,0\]---\left(7\right)$

$\frac{\∂h_{\mathrm{\φ}}}{\∂X}=\[0,0,1,0,0\]---\left(8\right)$

Δx＝[x_i-x_L](9)

Δy＝[y_i-y_L](10)

$\mathrm{\Δ}=\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2}---\left(11\right)$

然后利用分布式模型，将模型改写为如下形式：

$\begin{array}{c}\left\{\begin{array}{c}{x}_1\left(t\right)=x_1(t-1)+{\mathrm{\ω}}_1\left(t\right)\\ z_1\left(t\right)=H_1\left(t\right)x_1\left(t\right)+v_1\left(t\right)\end{array}\right.\\ \left\{\begin{array}{c}{x}_2\left(t\right)=x_2(t-1)+{\mathrm{\ω}}_2\left(t\right)\\ z_2\left(t\right)=H_2\left(t\right)x_2+v_2\left(t\right)\end{array}\right.\\ \mathrm{............}\\ \left\{\begin{array}{c}{x}_n\left(t\right)=x_n(t-1)+{\mathrm{\ω}}_n\left(t\right)\\ z_n\left(t\right)=H_n\left(t\right)x_n\left(t\right)+v_n\left(t\right)\end{array}\right.\end{array}---\left(12\right)$

可观测性分析是系统设计与控制领域的重要研究内容与步骤。本算法状态方程是由小车状态和一个路标点的五维状态构成，观测除测量距离和角度外引入小车航向角。首先为了证明模型的可行性和滤波器的稳定性，就要先分析模型的可观性。首先描述什么是可观测性。系统可观测性的定义是：如果系统在t时刻的状态矢量X(t0)能够从时间区间[t0,t1]内系统的输出函数Y(t)中唯一确定出来，则称该系统是可观测的，否则称为不完全可观测或不可观测。通过扩展卡尔曼线性化以后，只需计算系统的可观测性矩阵Q的秩即可判断系统的可观测性。即可观测性矩阵Q为：

Q＝[H^T,(HA)^T,…,(HA^n-1)^T]^T(13)

如果矩阵Q的秩等于n，则称系统是完全可观测的，如果Q的秩小于n,则称系统是不完全可观测的。对于的模型，可观测性矩阵Q为：

Q＝[H^T,(H)^T,…,(H)^T]^T(14)

那么

$rank\left(Q\right)=rank\left(H\right)=rank\left(\left[\begin{array}{c}-\frac{\mathrm{\Δ}x}{\mathrm{\Δ}},-\frac{\mathrm{\Δ}y}{\mathrm{\Δ}},0,0,0\\ \frac{\mathrm{\Δ}y}{{\mathrm{\Δ}}^2},-\frac{\mathrm{\Δ}x}{{\mathrm{\Δ}}^2},-1,0,0\\ 0,0,-1,0,0\end{array}\right]\right)=3---\left(15\right)$

固系统模型是可观的。

从模型(3)和(12)可以看出，SALM问题是非线性非高斯的状态估计问题，首先通过扩展卡尔曼算法将非线性问题转化为线性，再利用卡尔曼算法进行估计，其次在状态模型和观测模型中分别加入均值为零的高斯白噪声，然后建立分布式多子滤波器结构，大大优化了SLAM建模和估计过程。

集中式SALM算法是采用了单一滤波器实现对机器人位姿和路标点的估计，不可避免的存在计算量大、复杂度高、容错性差、实时性差的问题。而分布式SALM算法是将本应以矩阵形式集中计算的描述机器人位姿的观测分布概率公式进行分布化处理，之后在子滤波器中处理概率计算，最后根据相应的融合算法进行主滤波器融合。这样大大减少了计算复杂度，提高系统容错能力，下面着重讲述滤波融合算法的实现。

全局最优的滤波融合算法的基本概念是通过多个独立平行的子滤波器对传感器测量的处理，实现全局状态及其协方差的预测和更新，然后在主滤波器中融合更新后的本地信息，输出最优估计，其流程图如图3所示。

假设子滤波器和主滤波器是不相关的，那么全局估计是：

$P_f^{-1}=\underset{i=1}{\overset{n}{\Σ}}{P}_i^{-1}+P_m^{-1}---\left(16\right)$

$P_f^{-1}{\hat{x}}_f=\underset{i=1}{\overset{n}{\Σ}}{P}_i^{-1}{\hat{x}}_i+P_m^{-1}{\hat{x}}_m---\left(17\right)$

下角标“f”，“i”，和“m”各自表示融合滤波器，子滤波器和主滤波器。P表示协方差矩阵，x表示状态量。

如果子滤波器和主滤波器是独立的，那么通过全局预测器将次最优的输出反馈修正来实现全局的最优估计。在分布式卡尔曼中，由于子滤波器和主滤波器共用相同的系统模型，所以它们的估计结果是必然相关的，为了消除这种相关，可以通过下面的式子对协方差矩阵P和过程噪声Q进行约束：

$\{\begin{array}{c}{\hat{P}}_i^{-1}\left(t\right)={\hat{P}}_f^{-1}{\mathrm{\β}}_i\\ {\hat{Q}}_i^{-1}\left(t\right)={\hat{Q}}_f^{-1}{\mathrm{\β}}_i\end{array},i=1,\mathrm{2...}n,m---\left(18\right)$

β_i(≥0)是相关因子，满足

每个子滤波器预测过程为：

${\hat{P}}_i\left(t\right|t-1)={\hat{P}}_i(t-1)+Q_i\left(k\right),i=1,2...n,m---\left(19\right)$

${\hat{X}}_i\left(t\right|t-1)={\hat{X}}_i(t-1),i=1,2...n,m---\left(20\right)$

子滤波器和主滤波器中的量测更新为：

$K_i\left(t\right)={\hat{P}}_i\left(t\right|t-1)H_i^T\left(k\right){\[H_i\left(k\right){\hat{P}}_i\left(t\right|t-1)H_i^T\left(k\right)+R_i\left(k\right)\]}^{-1},i=1,2...n---\left(21\right)$

${\hat{P}}_i\left(t\right)=\[I-K_i\left(t\right)H_i\left(k\right)\]{\hat{P}}_i\left(t\right|t-1),i=1,2...n---\left(22\right)$

${\hat{X}}_i\left(t\right)={\hat{X}}_i(t-1)+K_i\left(t\right)\[Z_i\left(t\right)-H_i\left(t\right){\hat{X}}_i\left(t\right|t-1)\],i=1,2...n---\left(23\right)$

主滤波器中的融合采用加权平均法，

${\hat{P}}_m\left(t\right)={\hat{P}}_m\left(t\right|t-1)=\underset{i=1}{\overset{n}{\Σ}}{w}_i{\hat{p}}_i\left(t\right),i=1,2...n---\left(24\right)$

${\hat{X}}_m\left(t\right)={\hat{X}}_m\left(t\right|t-1)=\underset{i=1}{\overset{n}{\Σ}}{w}_i{\hat{X}}_i\left(t\right),i=1,2...n---\left(25\right)$

其中w_i为加权系数，加权平均法将来自不同子滤波器的机器人位姿估计信息计算权值，进行加权平均，结果作为融合值。

由以上可推导出反馈修正的全局最优融合结果：

${\hat{P}}_f^{-1}\left(t\right)={\hat{P}}_m^{-1}\left(t\right)+\underset{i=1}{\overset{n}{\Σ}}{\hat{P}}_i^{-1}\left(t\right)---\left(26\right)$

$\hat{X}\left(k\right)={\hat{P}}_f\left(t\right)\[{\hat{P}}_m^{-1}\left(t\right){\hat{X}}_m\left(k\right)+\underset{i=1}{\overset{n}{\Σ}}{\hat{P}}_i^{-1}\left(t\right){\hat{X}}_i\left(k\right)\]---\left(27\right)$

附图说明

图1：机器人的状态转移过程图；

图2：机器人实际坐标系；

图3：算法流程图；

图4：移动小车平台；

图5：长方形实验环境；

图6：椭圆形实验环境；

图7：长方形实验对比图；

图8：椭圆形实验对比图；

具体实施方式

步骤1：建立模型和线性化。

将整个状态向量分为机器人位姿估计和路标估计共五维状态，其状态向量为：x_v＝[x_L,y_L,φ_L,x_i,y_i]^T，x_L,y_L,φ_L是小车的状态，x_i,y_i是路标点的状态。扩展卡尔曼线性化后观测模型的雅可比矩阵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，否则程序终止。

下面通过真实实验对本发明提出的基于全局最优数据融合的分布式扩展卡尔曼SLAM算法进行验证。实验所用平台是自己搭建的两轮小车，传感器使用URG-04LX的激光传感器，移动小车平台的实物图如图4。实验所用的环境是椭圆形和长方形跑道，以长筒作为路标点在跑道两旁随机摆放，长方形实验环境如图5所示，椭圆形实验环境如图6所示，圆点表示小车起始位置，箭头指示方向为小车行驶方向，具体路径如图所示。通过使用本发明算法和集中式算法对比，证明本算法的可行性和有效性。

长方形实验的对比图如图7所示，椭圆形实验的对比图如图8所示，里程计数据如图ODM实线所示，可见随着里程计累加误差的增加，里程计数据越来越发散，经过分布式扩展卡尔曼融合滤波算法和集中式卡尔曼滤波算法的估计，对比图中短虚线和长虚线的估计结果可以明确的看出，分布式扩展卡尔曼算法(DEKF)较集中式扩展卡尔曼(CEKF)算法更平滑稳定且估计结果一致性和收敛性更好，偏差更小。可见本发明算法对误差的补偿及机器人动态调整过程不确定噪声的干扰有较好的估计，对存在相关性过程具有很好的估计性能，可以达到全局最优的估计效果。

Claims (2)

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

由于状态方程和测量方程均为非线性，所以采用扩展卡尔曼滤波，扩展卡尔曼是对非线性函数的泰勒展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性，可以将卡尔曼线性滤波算法应用于非线性系统中；

SLAM系统状态模型可看作一个马尔科夫过程，即t时刻状态与t-1时刻前的状态无关；系统中机器人的运动控制信息由里程计提供；

机器人的状态st描述如下：s_t＝(x,y,θ)^T，其中，x，y表示对应横纵坐标值，θ表示偏航角(θ＝0表示指向x轴正向，θ＝π/2表示指向y轴正向)；机器人的状态转移模型如下：

$X_L(t+1)=\left[\begin{array}{c}{x}_L(t+1)\\ y_L(t+1)\\ {\mathrm{\φ}}_L(t+1)\end{array}\right]=f\left(X_L\right(t\left)\right)+\mathrm{\ω}=\left[\begin{array}{c}{x}_L\left(t\right)+\mathrm{\Δ}T\left(v_{c}cos\right(\mathrm{\φ})-\frac{v_c}{L}tan(\mathrm{\φ}\left)\right(a\sin \left(\mathrm{\φ}\right)+bcos\left(\mathrm{\φ}\right)\left)\right)\\ y_L\left(t\right)+\mathrm{\Δ}T\left(v_c\sin \right(\mathrm{\φ})-\frac{v_c}{L}tan(\mathrm{\φ}\left)\right(a\sin \left(\mathrm{\φ}\right)-bcos\left(\mathrm{\φ}\right)\left)\right)\\ {\mathrm{\φ}}_L\left(t\right)+\mathrm{\Δ}T\frac{v_c}{L}tan\left(\mathrm{\α}\right)\end{array}\right]+\mathrm{\ω}---\left(1\right)$

式中X_L(t)＝(x_L(t)y_L(t)φ_L(t))^T为机器人在t时刻的位姿,下标L表示的是里程计的信息，x_L(t)，y_L(t)表示t时刻对应的横纵坐标值，φ_L(t)表示t时刻航向角，ΔT为时间变化量，v_c为机器人移动速度，α为机器人变化的角度，L为两轮轴间距，ω是协方差为Q均值为零的高斯白噪声；

机器人位姿的估计是由激光传感器测得的观测信息来推断的，本方法针对激光传感器给出机器人的观测模型，测距激光传感器的观测量z是某个环境特征相对于传感器的距离以及角度，在扫描目标时容易出现误差，实际状况中，误差密度函数由各种分布组合而成，一般情况包括：指数分布、均匀分布、高斯分布，本方法采用高斯分布描述误差的分布情况；

将机器人状态信息与观测信息相关联，获得路标点的观测模型：

$z_t^k=\left[\begin{array}{c}{z}_r^k\\ z_{\mathrm{\θ}}^k\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_{i,x}-x_i)}^2+{(m_{i,y}-y_i)}^2}\\ {\mathrm{\φ}}_i-atan(-\frac{y_{i,y}-y_i}{x_{i,x}-x_i})+\frac{\mathrm{\π}}{2}\end{array}\right]+v---\left(2\right)$

式中，为观测量包括传感器测得的机器人与路标点间距离zr和角度z_θ，上标K表示1到K次观测值。X_i＝(x_iy_i)^T为路标点坐标，下标i表示的是路标点的信息。v是协方差为R均值为0的高斯白噪声；m_i,xm_i,y为已存到地图里路标点的横纵坐标，x_i,x，y_i,x表示根据小车坐标估计的路标点坐标。；当观测到n个有效特征点时观测方程为：

$z=\left[\begin{array}{c}{z}_1\\ z_2\\ .\\ .\\ .\\ z_n\end{array}\right]=\left[\begin{array}{c}h\left(x_r\right(t),m_1)\\ h\left(x_r\right(t),m_2)\\ .\\ .\\ .\\ h\left(x_r\right(t),m_n)\end{array}\right]+v\left(t\right)---\left(3\right)$

式中，h(x_r(t),m_i)为上述路标点观测模型，V(t)为协方差为R均值为0的高斯白噪声；

这里构建的SALM状态向量分为机器人位姿估计和路标估计共五维状态，即x_v＝[x_L,y_L,φ_L,x_i,y_i]^T，系统的状态方程如下：

$X_L(t+1)=\left[\begin{array}{c}{x}_L(t+1)\\ y_L(t+1)\\ {\mathrm{\φ}}_L(t+1)\\ x_i(t+1)\\ y_i(t+1)\end{array}\right]=f\left(X_L\right(t\left)\right)+\mathrm{\ω}=\left[\begin{array}{c}{x}_L\left(t\right)+\mathrm{\Δ}T(v_{c}cos\left(\mathrm{\φ}\right)-\frac{v_c}{L}tan\left(\mathrm{\φ}\right)\left(a\sin \left(\mathrm{\φ}\right)+bcos\left(\mathrm{\φ}\right)\right))\\ y_L\left(t\right)+\mathrm{\Δ}T(v_c\sin \left(\mathrm{\φ}\right)-\frac{v_c}{L}tan\left(\mathrm{\φ}\right)\left(a\sin \left(\mathrm{\φ}\right)-bcos\left(\mathrm{\φ}\right)\right))\\ {\mathrm{\φ}}_L\left(t\right)+\mathrm{\Δ}T\frac{v_c}{L}tan\left(\mathrm{\α}\right)\\ x_L\left(t\right)+r*\cos ({\mathrm{\φ}}_L\left(t\right)+\mathrm{\α})\\ y_L\left(t\right)+r*\sin ({\mathrm{\φ}}_L\left(t\right)+\mathrm{\α})\end{array}\right]+\mathrm{\ω}---\left(4\right)$

将小车航向φ_L加入观测，并利用扩展卡尔曼线性化后观测模型的雅可比矩阵如下：

$\frac{\∂h}{\∂X}=\left[\begin{array}{c}\frac{\∂h_r}{\∂X}\\ \frac{\∂h_{\mathrm{\θ}}}{\∂X}\\ \frac{\∂h_{\mathrm{\φ}}}{\∂X}\end{array}\right]=\left[\begin{array}{c}\frac{\∂z_r}{\∂(x_L,y_L,{\mathrm{\φ}}_L,\{x_i,y_i\left\}\right)}\\ \frac{\∂z_{\mathrm{\θ}}}{\∂(x_L,y_L,{\mathrm{\φ}}_L,\{x_i,y_i\left\}\right)}\\ \frac{\∂z_{\mathrm{\φ}}}{\∂(x_L,y_L,{\mathrm{\φ}}_L,\{x_i,y_i\left\}\right)}\end{array}\right]---\left(5\right)$

其中

$\frac{\∂h_r}{\∂X}=\frac{1}{\mathrm{\Δ}}\[-\mathrm{\Δ}x,-\mathrm{\Δ}y,0,0,0\]---\left(6\right)$

$\frac{\∂h_{\mathrm{\β}}}{\∂X}=\[\frac{\mathrm{\Δ}y}{{\mathrm{\Δ}}^2},-\frac{\mathrm{\Δ}x}{{\mathrm{\Δ}}^2},-1,0,0\]---\left(7\right)$

$\frac{\∂h_{\mathrm{\φ}}}{\∂X}=\[0,0,1,0,0\]---\left(8\right)$

Δx＝[x_i-x_L](9)

Δy＝[y_i-y_L](10)

$\mathrm{\Δ}=\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2}---\left(11\right)$

然后利用分布式模型，将模型改写为如下形式：

$\begin{array}{c}\left\{\begin{array}{c}{x}_1\left(t\right)=x_1(t-1)+{\mathrm{\ω}}_1\left(t\right)\\ z_1\left(t\right)=H_1\left(t\right)x_1\left(t\right)+v_1\left(t\right)\end{array}\right.\\ \left\{\begin{array}{c}{x}_2\left(t\right)=x_2(t-1)+{\mathrm{\ω}}_2\left(t\right)\\ z_2\left(t\right)=H_2\left(t\right)x_2\left(t\right)+v_2\left(t\right)\end{array}\right.\\ \mathrm{............}\\ \left\{\begin{array}{c}{x}_n\left(t\right)=x_n(t-1)+{\mathrm{\ω}}_n\left(t\right)\\ z_n\left(t\right)=H_n\left(t\right)x_n\left(t\right)+v_n\left(t\right)\end{array}\right.\end{array}---\left(12\right)$

可观测性分析是系统设计与控制领域的重要研究内容与步骤；本方法状态方程是由小车状态和一个路标点的五维状态构成，观测除测量距离和角度外引入小车航向角；首先为了证明模型的可行性和滤波器的稳定性，就要先分析模型的可观性；首先描述什么是可观测性；系统可观测性的定义是：如果系统在t时刻的状态矢量X(t0)能够从时间区间[t0,t1]内系统的输出函数Y(t)中唯一确定出来，则称该系统是可观测的，否则称为不完全可观测或不可观测；通过扩展卡尔曼线性化以后，只需计算系统的可观测性矩阵Q的秩即可判断系统的可观测性；即可观测性矩阵Q为：

Q＝[H^T,(HA)^T,...,(HA^n-1)^T]^T(13)

如果矩阵Q的秩等于n，则称系统是完全可观测的，如果Q的秩小于n,则称系统是不完全可观测的；对于的模型，可观测性矩阵Q为：

Q＝[H^T,(H)^T,...,(H)^T]^T(14)

那么

$rank\left(Q\right)=rank\left(H\right)=rank\left(\left[\begin{array}{c}-\frac{\mathrm{\Δ}x}{\mathrm{\Δ}},-\frac{\mathrm{\Δ}y}{\mathrm{\Δ}},0,0,0\\ \frac{\mathrm{\Δ}y}{{\mathrm{\Δ}}^2},-\frac{\mathrm{\Δ}x}{{\mathrm{\Δ}}^2},-1,0,0\\ 0,0,-1,0,0\end{array}\right]\right)=3---\left(15\right)$

固系统模型是可观的；

从模型(3)和(12)可以看出，SALM问题是非线性非高斯的状态估计问题，首先通过扩展卡尔曼算法将非线性问题转化为线性，再利用卡尔曼算法进行估计，其次在状态模型和观测模型中分别加入均值为零的高斯白噪声，然后建立分布式多子滤波器结构，大大优化了SLAM建模和估计过程；

集中式SALM算法是采用了单一滤波器实现对机器人位姿和路标点的估计，不可避免的存在计算量大、复杂度高、容错性差、实时性差的问题；而分布式SALM算法是将本应以矩阵形式集中计算的描述机器人位姿的观测分布概率公式进行分布化处理，之后在子滤波器中处理概率计算，最后根据相应的融合算法进行主滤波器融合；这样大大减少了计算复杂度，提高系统容错能力，下面着重阐述滤波融合方法的实现；

全局最优的滤波融合算法的基本概念是通过多个独立平行的子滤波器对传感器测量的处理，实现全局状态及其协方差的预测和更新，然后在主滤波器中融合更新后的本地信息，输出最优估计；

假设子滤波器和主滤波器是不相关的，那么全局估计是：

$P_f^{-1}=\underset{i=1}{\overset{n}{\Σ}}{P}_i^{-1}+P_m^{-1}---\left(16\right)$

$P_f^{-1}{\hat{x}}_f=\underset{i=1}{\overset{n}{\Σ}}{P}_i^{-1}{\hat{x}}_i+P_m^{-1}{\hat{x}}_m---\left(17\right)$

下角标“f”，“i”，和“m”各自表示融合滤波器，子滤波器和主滤波器；P表示协方差矩阵，x表示状态量；

如果子滤波器和主滤波器是独立的，那么可以通过全局预测器将次最优的输出反馈修正来实现全局的最优估计；在分布式卡尔曼中，由于子滤波器和主滤波器共用相同的系统模型，所以它们的估计结果是必然相关的，为了消除这种相关，可以通过下面的式子对协方差矩阵P和过程噪声Q进行约束：

$\left\{\begin{array}{c}{\hat{P}}_i^{-1}\left(t\right)={\hat{P}}_f^{-1}{\mathrm{\β}}_i\\ {\hat{Q}}_i^{-1}\left(t\right)={\hat{Q}}_f^{-1}{\mathrm{\β}}_i\end{array}\right.,i=1,2...n,m---\left(18\right)$

β_i(≥0)是相关因子，满足

每个子滤波器预测过程为：

${\hat{P}}_i\left(t\right|t-1)={\hat{P}}_i(t-1)+Q_i\left(k\right),i=1,2...n,m---\left(19\right)$

${\hat{X}}_i\left(t\right|t-1)={\hat{X}}_i(t-1),i=1,2...n,m---\left(20\right)$

子滤波器和主滤波器中的量测更新为：

$K_i\left(t\right)={\hat{P}}_i\left(t\right|t-1)H_i^T\left(k\right){\[H_i\left(k\right){\hat{P}}_i\left(t\right|t-1)H_i^T\left(k\right)+R_i\left(k\right)\]}^{-1},i=1,2...n---\left(21\right)$

${\hat{P}}_i\left(t\right)=\[I-K_i\left(t\right)H_i\left(k\right)\]{\hat{P}}_i\left(t\right|t-1),i=1,2...n---\left(22\right)$

${\hat{X}}_i\left(t\right)={\hat{X}}_i(t-1)+K_i\left(t\right)\[Z_i\left(t\right)-H_i\left(t\right){\hat{X}}_i\left(t\right|t-1)\],i=1,2...n---\left(23\right)$

主滤波器中的融合采用加权平均法，

${\hat{P}}_m\left(t\right)={\hat{P}}_m\left(t\right|t-1)=\underset{i=1}{\overset{n}{\Σ}}{w}_i{\hat{p}}_i\left(t\right),i=1,2...n---\left(24\right)$

${\hat{X}}_m\left(t\right)={\hat{X}}_m\left(t\right|t-1)=\underset{i=1}{\overset{n}{\Σ}}{w}_i{\hat{X}}_i\left(t\right),i=1,2...n---\left(25\right)$

其中w_i为加权系数，加权平均法将来自不同子滤波器的机器人位姿估计信息计算权值，进行加权平均，结果作为融合值；

由以上可推导出反馈修正的全局最优融合结果：

${\hat{P}}_f^{-1}\left(t\right)={\hat{P}}_m^{-1}\left(t\right)+\underset{i=1}{\overset{n}{\Σ}}{\hat{P}}_i^{-1}\left(t\right)---\left(26\right)$

$\hat{X}\left(k\right)={\hat{P}}_f\left(t\right)\[{\hat{P}}_m^{-1}\left(t\right){\hat{X}}_m\left(k\right)+\underset{i=1}{\overset{n}{\Σ}}{\hat{P}}_i^{-1}\left(t\right){\hat{X}}_i\left(k\right)\]---\left(27\right)$

2.根据权利要求1所述的基于全局最优数据融合的分布式SLAM方法，其特征在于：对误差的补偿及机器人动态调整过程不确定噪声的干扰有较好的估计，对存在相关性过程具有很好的估计性能，可以达到全局最优的估计效果；具体步骤如下：

步骤1：建立模型和线性化；

将整个状态向量分为机器人位姿估计和路标估计共五维状态，其状态向量为：x_v＝[x_L,y_L,φ_L,x_i,y_i]^T，x_L,y_L,φ_L是小车的状态，x_i,y_i是路标点的状态；扩展卡尔曼线性化后观测模型的雅可比矩阵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，否则程序终止。