基于航向辅助分布式slam的机器人自主导航方法
【技术领域】
[0001] 本发明涉及机器人同时定位与制图（SLAM)属于机器人自主导航的范畴，针对自主 移动机器人如何确定自身位置及感知外界环境的问题，SLAM系统旨在通过机器人系统模 型，结合相应的滤波方法完成自主移动机器人的定位与周围环境的制图。
【背景技术】
[0002] SLAM(Simultaneous Localization and Mapping)即同步定位与地图构建其基本 原理是：当机器人处于未知环境中时，通过自身传感器收集自身运动状况及周围环境情况， 无论采用何种滤波方法进行位置的估计，都需经历预测与更新的过程，传统2D-SLAM的设 计，一般采用里程计采集的信息作为预测过程的输入，激光传感器采集的信息作为更新过 程的输入，具体的滤波器设计可以有多种衍生，传统的以集中矩阵形式描述机器人位姿及 路标点状态，本设计采用系统模型的分布化处理，建立由每个有效路标点单独构成子滤波 器的模型结构，由各个子滤波器融合为统一的地图估计结果，基于此框架下，对其观测模型 特性进行分析，通过其可观测性及收敛性的总结，引入磁罗盘的航向作为辅助信息加入至 分布式观测环节的滤波过程中，从而达到改善系统可观测性的目的，系统的输出结果由里 程计，激光传感器及磁罗盘的信息进行滤波生成。
[0003] 由于集中式结构中，描述系统状态的矩阵维数动态时变，并且随着观测路标点的 增加，矩阵维度也随之增加，只用单一滤波器完成计算过程会带来诸如计算量大，稳定性差 和容错性差等问题，本设计使用的分布式结构的SLAM算法相对于传统集中式算法，所建立 的每个滤波器都包含位姿信息及所对应固定路标点的信息，有效减小了计算量并提高了稳 定性与容错性。对于分布式算法，有诸多提高精度的方法，主要通过改善滤波的方法及融合 的算法来提高系统属性，而单纯地将集中式结构改成分布式结构而不加入其他传感器信 息，同样面临可观测性降低的问题，由于单一路标点观测性无法得到保证，针对此类问题， 主要有提高子滤波器中观测点个数等方法解决。针对现有技术的缺陷详述如下。
[0004] 分布式结构的精度缺陷及解决方案：
[0005] 分布式结构虽然状态矩阵维数相对固定能够解决计算量和不稳定的问题，但是由 于子滤波器中的路标点较少，可观测性得不到有效的保证，即当只有一个观测点的信息时， 无法保证系统在长时间运行过程中都是可观的，粒子滤波的方法对可观测性影响较小，但 计算量的问题导致它不便于应用到实时系统中。当针对两个及以上不共线的观测点建立子 滤波器时，可有效保证系统可观性，但此方案会带来子滤波器重构等不稳定因素，影响系统 稳定性，故若试图同时考虑计算量，可观性，稳定性，考虑加入航向信息进行扶助，磁罗盘所 提供的机器人本体航向信息能够解决可观测性的问题并且不会带来很频繁的子滤波器重 构，规避了以上两者的缺陷，同时选用EKF作为滤波方法。综上所述，制定相关的基于航向辅 助的分布式EKF算法作为有效解决方案。
【发明内容】
[0006] 本方法旨在结合机器人运动模型及观测模型的特点，在分布式滤波运算的框架 下，分别比较粒子滤波(PF)与扩展卡尔曼滤波(EKF)的诸如计算量等对系统性能有较大影 响的因素，考虑到实时系统中对计算量的要求，采用分布式EKF算法设计滤波器而非采用PF 算法，为了同时保证系统的可观测性，在传统的里程计+激光传感器作为传感器组合方案之 外，引入磁罗盘作为辅助传感器从而给系统加入机器人的航向信息。该设计针对分布式滤 波结构的观测模型，通过引入航向信息重新规划了滤波方法从而达到实用化机器人自主导 航的目的。
[0007] 本发明从分布式粒子滤波带来的计算量问题及分布式卡尔曼滤波所面临的可观 测性问题入手，引入磁罗盘提供航向辅助信息，提出一种结合分布式算法结构与航向辅助 的SLAM方法，并搭建移动平台进行算法精度和收敛性的验证。在每个路标点信息送入各自 子滤波器的方式下，改变观测模型的结构，在不添加每个分布式子滤波器中路标点个数的 条件下，将航向信息作为状态量融合到整个的扩展卡尔曼滤波过程中，下文将论述分布式 架构中粒子滤波与卡尔曼滤波实现的差异，在采用分布式EKF的条件下，若需保证一定的精 度，与粒子滤波不同的是需要引入航向信息作为观测量，从而在减少计算量同时保证可观 性的条件下，本文通过航向引入的方式建立分布式扩展卡尔曼滤波的模型，设计相应的滤 波器实现方式，并通过实验验证本算法的真实有效性。
[0008] 首先，引入航向信息的系统模型阐述如下：
[0009]关于引入航向信息的机器人状态矩阵描述如下:xv=[xv，y v, Φv，XL，yL]T，其中，Xv, yv表示机器人坐标值，Φ v表示机器人航向角（Φ v = 〇表示指向X轴正向，Φ v=V2表示指向y 轴正向）。^表示探测得到的路标点坐标值，则系统的状态转移模型如下：
[0011] 以上转移矩阵中的参数与车体及里程计的固定参数有关，式中Xv=[Xv，yv，(K， XL， yL]T为机器人在t时刻的位姿及路标点位置，△ T为时间变化量，Vc为机器人移动速度，α为机 器人车轮转角，L为两轮轴间距，γ为高斯白噪声。
[0012] 扩展卡尔曼滤波的量测更新结果是由激光传感器测得的路标点距离信息及磁罗 盘测得的本体航向信息计算得到的，针对激光传感器及磁罗盘给出机器人的观测模型，观 测量 < 分别t时刻第k个路标点相对于传感器的距离，角度以及所测得的航向。获得路标点 的观测模型如下：
[0014]其中，mL,x，mL,y表示测量至Ij的路标点相对于x v，yv的测量信息。
[0015]在分布式框架下，带有航向信息的各子滤波器的模型如下：
[0017] 每个子滤波器中对应的SLAM状态向量分为机器人位姿估计和路标估计共五维状 态，g卩每次状态转移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息决 定。其中各子滤波器状态向量x v= [xv，yv, Φ v，a，yL]T，h(xv(t)，mi)表示t时刻，根据该时刻 机器人状态向量x v (t)和对应的测量信息mn所估计的第η个路标点的测量信息。
[0018] 分布式结构的滤波方式主要分为粒子滤波(PF)和扩展卡尔曼滤波(EKF)，PF属于 非线性滤波方法，解决SLAM这种非线性模型问题具有精度上的优势，但其计算量较大，并较 难移植到实时性较高的移动平台上。
[0019] 对于PF，根据各子滤波器中粒子群的预测分布，针对各种传感器的模型近似利用 蒙特卡洛法模拟粒子分布，然后对每个子滤波器中的粒子进行重要性权值估计如式(4)所 不。
[0021] 根据公式(4)得到的权值可见，权值反映各个子滤波器中粒子的分布情况，并 且由观测信息zt和根据状态向量估计的iMxf)所近似的分布计算得来，每个子滤波器虽然 只利用单一路标点进行滤波估计，但由于PF的权值由概率分布模型决定，并不是基于线性 模型迭代计算得到，而是模拟概率统计的模型得到，故可观测性可以得到保证。
[0022] 根据权重值更新，可输出t时刻第j个子滤波器的位姿估计结果< 如式(5)
[0024]根据子滤波器位姿结果，进行主滤波器的融合。如式(6)及式(7)所示。
[0027] 其中，最终位姿估计xt由子滤波器估计结果xf融合而来，融合系数；?/由各子滤波 器有效粒子数w/决定。
[0028] 根据以上各式可知，PF算法整个计算过程均与子滤波器估计的概率分布有关，可 观测性的问题并不像EKF算法影响显著，但其精度随着滤波器所使用的粒子数的增加而增 加，所以，若想要保证良好的精度，往往滤波器所需的粒子数也较多，即计算的次数要远远 大于其他非统计型的滤波方法。
[0029] 考虑到计算量的问题，选用相对计算量可控的EKF算法，提高系统的实时性，而分 布式架构又能很好的提高系统的容错性及计算性能。故采用分布式架构上使用EKF的方法 可以充分发挥两者优势，即每个滤波器采用固定的路标点个数，防止