过多动态重构的发生， 并且每个滤波器使用EKF进行滤波。该种解决方案虽然最大程度地发挥了两者的优点，但不 可忽视的是，EKF方法将非线性模型转化为线性模型，根据线性模型的可观测性的特质，如 果滤波器中只有一个观测点或多个观测点但共线时，系统的可观测性会受到影响，无法满 足滤波的可观要求，为了兼容每个滤波器中输入路标点相对较少且固定，并降低其对可观 测性的影响，加入航向对可观测性进行改善，当每个子滤波器中存在一个路标点且存在当 前时刻机器人本体的航向时，整个系统是可观的。综上，以下描述基于分布式架构下，结合 航向信息辅助的分布式EKF算法。
[0030] 首先，卡尔曼滤波解决的是线性系统模型，而SLAM模型属于非线性模型，将其进行 线性化处理如下，航向信息作为观测量引入：
[0036] Λ χ= [xv-xl] Δ y= [yv-yL] (12)
[0038]在模型的线性化操作后，针对每个路标点建立相应的子滤波器，并匹配相关t_l时 刻的路标点，将各个子滤波器中代入相应的航向值。子滤波器的位姿估计过程为：
[0041]根据将公式（1)线性化后的姿态转移矩阵进行以上的位姿预测估计，得到的结果 作为以下量测更新的输入。
[0042]量测更新的过程在各子滤波器中进行，结合线性化后的观测模型丑丨，进行以下的 卡尔曼滤波量测更新过程，更新卡尔曼滤波增益1(〇和协方差/^):
[0046]得到子滤波器更新的结果后，根据协方差矩阵进行融合过程，从而输出最终的位 置：
[0049] 其中IU为加权系数，加权平均法将来自不同子滤波器的机器人位姿估计信息计算 权值，进行加权平均后的结果作为融合值。
【附图说明】
[0050] 图1:机器人的状态转移过程图；
[0051 ]图2:机器人实际坐标系；
[0052] 图3:基于航向辅助分布式SLAM算法流程图；
[0053] 图4:基于航向辅助分布式程序设计流程图
[0054] 图5:移动小车平台实物图；
[0055] 图6:移动小车平台实验环境；
[0056] 图7:移动小车平台形实验结果图。
【具体实施方式】
[0057] 步骤1:模型的建立
[0058]系统模型的建立是基于机器人、环境特征和传感器这三个坐标系来定义的，通过 图1和图2中的状态转移方程和坐标系建立里程计预测模型，本发明选用笛卡尔坐标系统作 为直角坐标的表示形式，把传感器坐标系统一到机器人坐标系中，只采用全局坐标系和机 器人坐标系。状态转移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息 决定。即^^^，(^^…广建立运动模型和观测模型见公式⑴⑵凋者分别利用里 程计的输出和激光传感器的输出来估计相应的机器人及路标点位置信息。
[0059] 步骤2:滤波初始化及数据预处理
[0060] 设定卡尔曼滤波的P/Q/R矩阵初值，并进行全局坐标下机器人位置的初始化，对准 磁罗盘激光传感器等传感器时钟以及读取数据路径。设置匹配路标点阈值等初始化参数。 此外，对传感器数据进行预处理，根据每一时刻扫描的点云(511个)中设计函数提取激光传 感器特征点，并进行航向弧度值的计算等。
[0061] 步骤3:路标点匹配
[0062] 在机器人运行过程中，将t时刻观测到路标点与已在全局地图中存储的路标点相 匹配，进行相关的数据关联运算，若匹配成功，则送入所匹配上的t-i时刻对应路标点的子 滤波器进行后续运算;若没有匹配成功，则说明该路标点为新增路标点，直接将其添加至全 局地图并建立相关子滤波器。对于t时刻未能观测到t-i时刻路标点的子滤波器，进行删除。
[0063] 步骤4:滤波器运行过程
[0064] 将运动模型及观测模型进行线性化处理，将传感器输入的结果通过线性化的模型 处理后进行卡尔曼滤波，每个路标点的信息及t时刻的机器人航向信息送入同一个子滤波 器，通过卡尔曼滤波算法对上述步骤中建立的各个子滤波器的状态向量和协方差进行处 理，分别进行状态转移矩阵和相应协方差矩阵的迭代更新，每个子滤波器中进行如下两个 关键的计算步骤，：
[0065]预测过程：
[0066] 首先将t_l时刻的每个子滤波器的估计状态和协方差矩阵，结合t时刻里程计的输 出数据，代入运动模型（1)中，再经过一步预测方程（14)和（15)进行一步递推，得到由里程 计和状态转移矩阵推算出的机器人位置。
[0067] 量测更新过程：
[0068] 通过第一步递推的方式，得到机器人位置的预测值后，根据预测值的结果，加入激 光传感器和磁罗盘的观测信息进行量测更新的计算，具体见式（16)，（17)和（18)，输出每个 子滤波器对位置的量测修正值及路标点的更新值，并同时输出相应的协方差矩阵供后续迭 代。
[0069] 每次的滤波重复以上两个过程，能够持续观测到的路标点所建立的滤波器，在t时 刻继承t-i时刻的协方差矩阵作为t时刻的初始输入矩阵，新观测到的路标点对应的子滤波 器则采用t-i时刻最终估计得协方差矩阵作为初始输入矩阵。
[0070] 步骤5:滤波器融合输出最终结果
[0071 ]将各个子滤波器的估计结果输入到主滤波器中，根据各子滤波器协方差确定各子 滤波器在主滤波器中所占的权重并归一化，最后利用信息融合公式(20)输出最终位置估计 结果。
[0072] 通过实际场景实验对航向辅助的分布式SLAM进行验证。实验平台是基于R0S操作 系统操控制的两轮小车，传感器使用URG-04LX的激光传感器，磁罗盘使用迈科传感公司生 产的HCM365B全姿态三维电子罗盘，移动小车平台的实物图如图4。实验的测试环境中，以圆 柱形筒作为路标点分别放置于实验跑道两侧，实验环境如图6所示，通过使用本发明算法和 未加入航向的EKF算法对比，证明本算法在精度及保持可观测性的优势。
[0073]实验数据在Matlab中进行绘图，结果如图7所示，在里程计数据无法保证系统收敛 性的要求的情况下，图中给出了传统集中式EKF算法(CEKF)和本发明所应用的基于航向辅 助的分布式EKF算法(DH-EKF)的运算结果，对比图中DH-EKF和传统CEKF算法，可以看出加入 航向辅助的算法更平滑稳定且估计结果一致性和收敛性更好，误差更小。证明加入航向修 正后，对于系统可能存在的动态干扰和观测的不确定性均有很好的稳定性表现。并且，由于 航向的引入，以往分布式结构中单纯路标点扫描带来的可观测性问题也能得到很好解决， 从而增加系统了的稳定性和精度。
【主权项】
1.基于航向辅助分布式SLAM的机器人自主导航方法，其特征在于:在每个路标点信息 送入各自子滤波器的方式下，改变观测模型的结构，在不添加每个分布式子滤波器中路标 点个数的条件下，将航向信息作为状态量融合到整个的扩展卡尔曼滤波过程中，下文将论 述分布式架构中粒子滤波与卡尔曼滤波实现的差异，在采用分布式EKF的条件下，若需保证 精度，与粒子滤波不同的是需要引入航向信息作为观测量，从而在减少计算量同时保证可 观性的条件下，本方法通过航向引入的方式建立分布式扩展卡尔曼滤波的模型，设计相应 的滤波器实现方式，并通过实验验证本算法的真实有效性； 首先，引入航向信息的系统模型阐述如下： 关于引入航向信息的机器人状态矩阵描述如下:xv=[xv，yv, <K，xL，yL]T，其中，xv，yv表 示机器人坐标值，Φ v表示机器人航向角（Φ v=〇表示指向X轴正向，Φ v=V2表示指向y轴正 向）；XL，yL表示探测得到的路标点坐标值，则系统的状态转移模型如下：：1) 以上转移矩阵中的参数与车体及里程计的固定参数有关，式中Xv=[Xv，yv，0 vV，XL，yL]T 为机器人在t时刻的位姿及路标点位置，ΔΤ为时间变化量，v。为机器人移动速度，α为机器 人车轮转角，L为两轮轴间距，γ为高斯白噪声； 扩展卡尔曼滤波的量测更新结果是由激光传感器测得的路标点距离信息及磁罗盘测 得的本体航向信息计算得到的，针对激光传感器