CN105606104B - 基于航向辅助分布式slam的机器人自主导航方法 - Google Patents

Abstract

本发明公开了基于航向辅助分布式SLAM的机器人自主导航方法，属于机器人自主导航的范畴。本方法旨在结合机器人运动模型及观测模型的特点，在分布式滤波运算的框架下，分别比较粒子滤波与扩展卡尔曼滤波的诸如计算量等对系统性能有较大影响的因素，考虑到实时系统中对计算量的要求，采用分布式EKF算法设计滤波器而非采用PF算法，为了同时保证系统的可观测性，在传统的里程计+激光传感器作为传感器组合方案之外，引入磁罗盘作为辅助传感器从而给系统加入机器人的航向信息。该设计针对分布式滤波结构的观测模型，通过引入航向信息重新规划了滤波方法从而达到实用化机器人自主导航的目的，从而增加系统了的稳定性和精度。

Description

基于航向辅助分布式SLAM的机器人自主导航方法

技术领域

本发明涉及机器人同时定位与制图(SLAM)属于机器人自主导航的范畴，针对自主移动机器人如何确定自身位置及感知外界环境的问题，SLAM系统旨在通过机器人系统模型，结合相应的滤波方法完成自主移动机器人的定位与周围环境的制图。

背景技术

SLAM(Simultaneous Localization and Mapping)即同步定位与地图构建其基本原理是：当机器人处于未知环境中时，通过自身传感器收集自身运动状况及周围环境情况，无论采用何种滤波方法进行位置的估计，都需经历预测与更新的过程，传统2D-SLAM的设计，一般采用里程计采集的信息作为预测过程的输入，激光传感器采集的信息作为更新过程的输入，具体的滤波器设计可以有多种衍生，传统的以集中矩阵形式描述机器人位姿及路标点状态，本设计采用系统模型的分布化处理，建立由每个有效路标点单独构成子滤波器的模型结构，由各个子滤波器融合为统一的地图估计结果，基于此框架下，对其观测模型特性进行分析，通过其可观测性及收敛性的总结，引入磁罗盘的航向作为辅助信息加入至分布式观测环节的滤波过程中，从而达到改善系统可观测性的目的，系统的输出结果由里程计，激光传感器及磁罗盘的信息进行滤波生成。

由于集中式结构中，描述系统状态的矩阵维数动态时变，并且随着观测路标点的增加，矩阵维度也随之增加，只用单一滤波器完成计算过程会带来诸如计算量大，稳定性差和容错性差等问题，本设计使用的分布式结构的SLAM算法相对于传统集中式算法，所建立的每个滤波器都包含位姿信息及所对应固定路标点的信息，有效减小了计算量并提高了稳定性与容错性。对于分布式算法，有诸多提高精度的方法，主要通过改善滤波的方法及融合的算法来提高系统属性，而单纯地将集中式结构改成分布式结构而不加入其他传感器信息，同样面临可观测性降低的问题，由于单一路标点观测性无法得到保证，针对此类问题，主要有提高子滤波器中观测点个数等方法解决。针对现有技术的缺陷详述如下。

分布式结构的精度缺陷及解决方案：

分布式结构虽然状态矩阵维数相对固定能够解决计算量和不稳定的问题，但是由于子滤波器中的路标点较少，可观测性得不到有效的保证，即当只有一个观测点的信息时，无法保证系统在长时间运行过程中都是可观的，粒子滤波的方法对可观测性影响较小，但计算量的问题导致它不便于应用到实时系统中。当针对两个及以上不共线的观测点建立子滤波器时，可有效保证系统可观性，但此方案会带来子滤波器重构等不稳定因素，影响系统稳定性，故若试图同时考虑计算量，可观性，稳定性，考虑加入航向信息进行扶助，磁罗盘所提供的机器人本体航向信息能够解决可观测性的问题并且不会带来很频繁的子滤波器重构，规避了以上两者的缺陷，同时选用EKF作为滤波方法。综上所述，制定相关的基于航向辅助的分布式EKF算法作为有效解决方案。

发明内容

本方法旨在结合机器人运动模型及观测模型的特点，在分布式滤波运算的框架下，分别比较粒子滤波(PF)与扩展卡尔曼滤波(EKF)的诸如计算量等对系统性能有较大影响的因素，考虑到实时系统中对计算量的要求，采用分布式EKF算法设计滤波器而非采用PF算法，为了同时保证系统的可观测性，在传统的里程计+激光传感器作为传感器组合方案之外，引入磁罗盘作为辅助传感器从而给系统加入机器人的航向信息。该设计针对分布式滤波结构的观测模型，通过引入航向信息重新规划了滤波方法从而达到实用化机器人自主导航的目的。

本发明从分布式粒子滤波带来的计算量问题及分布式卡尔曼滤波所面临的可观测性问题入手，引入磁罗盘提供航向辅助信息，提出一种结合分布式算法结构与航向辅助的SLAM方法，并搭建移动平台进行算法精度和收敛性的验证。在每个路标点信息送入各自子滤波器的方式下，改变观测模型的结构，在不添加每个分布式子滤波器中路标点个数的条件下，将航向信息作为状态量融合到整个的扩展卡尔曼滤波过程中，下文将论述分布式架构中粒子滤波与卡尔曼滤波实现的差异，在采用分布式EKF的条件下，若需保证一定的精度，与粒子滤波不同的是需要引入航向信息作为观测量，从而在减少计算量同时保证可观性的条件下，本文通过航向引入的方式建立分布式扩展卡尔曼滤波的模型，设计相应的滤波器实现方式，并通过实验验证本算法的真实有效性。

首先，引入航向信息的系统模型阐述如下：

关于引入航向信息的机器人状态矩阵描述如下：x_v＝[x_v,y_v,φ_v,x_L,y_L]^T，其中，x_v,y_v表示机器人坐标值，φ_v表示机器人航向角(φ_v＝0表示指向x轴正向，φ_v＝π/2表示指向y轴正向)。x_L,y_L表示探测得到的路标点坐标值，则系统的状态转移模型如下：

以上转移矩阵中的参数与车体及里程计的固定参数有关，式中x_v＝[x_v,y_v,φ_v,x_L,y_L]^T为机器人在t时刻的位姿及路标点位置,ΔT为时间变化量，v_c为机器人移动速度，为机器人车轮转角，L为两轮轴间距，γ为高斯白噪声。

扩展卡尔曼滤波的量测更新结果是由激光传感器测得的路标点距离信息及磁罗盘测得的本体航向信息计算得到的，针对激光传感器及磁罗盘给出机器人的观测模型，观测量分别t时刻第k个路标点相对于传感器的距离，角度以及所测得的航向。获得路标点的观测模型如下：

其中，m_L,x,m_L,y表示测量到的路标点相对于x_v,y_v的测量信息。

在分布式框架下，带有航向信息的各子滤波器的模型如下：

…………

每个子滤波器中对应的SLAM状态向量分为机器人位姿估计和路标估计共五维状态，即每次状态转移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息决定。其中各子滤波器状态向量x_v＝[x_v,y_v,φ_v,x_L,y_L]^T，h(x_v(t),m₁)表示t时刻，根据该时刻机器人状态向量x_v(t)和对应的测量信息m_n所估计的第n个路标点的测量信息。

分布式结构的滤波方式主要分为粒子滤波(PF)和扩展卡尔曼滤波(EKF)，PF属于非线性滤波方法，解决SLAM这种非线性模型问题具有精度上的优势，但其计算量较大，并较难移植到实时性较高的移动平台上。

对于PF，根据各子滤波器中粒子群的预测分布，针对各种传感器的模型近似利用蒙特卡洛法模拟粒子分布，然后对每个子滤波器中的粒子进行重要性权值估计如式(4)所示。

根据公式(4)得到的权值可见，权值反映各个子滤波器中粒子的分布情况，并且由观测信息z_t和根据状态向量估计的所近似的分布计算得来，每个子滤波器虽然只利用单一路标点进行滤波估计，但由于PF的权值由概率分布模型决定，并不是基于线性模型迭代计算得到，而是模拟概率统计的模型得到，故可观测性可以得到保证。

根据权重值更新，可输出t时刻第j个子滤波器的位姿估计结果如式(5)

根据子滤波器位姿结果，进行主滤波器的融合。如式(6)及式(7)所示。

其中，最终位姿估计x_t由子滤波器估计结果融合而来，融合系数由各子滤波器有效粒子数决定。

根据以上各式可知，PF算法整个计算过程均与子滤波器估计的概率分布有关，可观测性的问题并不像EKF算法影响显著，但其精度随着滤波器所使用的粒子数的增加而增加，所以，若想要保证良好的精度，往往滤波器所需的粒子数也较多，即计算的次数要远远大于其他非统计型的滤波方法。

考虑到计算量的问题，选用相对计算量可控的EKF算法，提高系统的实时性，而分布式架构又能很好的提高系统的容错性及计算性能。故采用分布式架构上使用EKF的方法可以充分发挥两者优势，即每个滤波器采用固定的路标点个数，防止过多动态重构的发生，并且每个滤波器使用EKF进行滤波。该种解决方案虽然最大程度地发挥了两者的优点，但不可忽视的是，EKF方法将非线性模型转化为线性模型，根据线性模型的可观测性的特质，如果滤波器中只有一个观测点或多个观测点但共线时，系统的可观测性会受到影响，无法满足滤波的可观要求，为了兼容每个滤波器中输入路标点相对较少且固定，并降低其对可观测性的影响，加入航向对可观测性进行改善，当每个子滤波器中存在一个路标点且存在当前时刻机器人本体的航向时，整个系统是可观的。综上，以下描述基于分布式架构下，结合航向信息辅助的分布式EKF算法。

首先，卡尔曼滤波解决的是线性系统模型，而SLAM模型属于非线性模型，将其进行线性化处理如下，航向信息作为观测量引入：

其中

Δx＝[x_v-x_L]Δy＝[y_v-y_L] (12)

在模型的线性化操作后，针对每个路标点建立相应的子滤波器，并匹配相关t-1时刻的路标点，将各个子滤波器中代入相应的航向值。子滤波器的位姿估计过程为：

根据将公式(1)线性化后的姿态转移矩阵进行以上的位姿预测估计，得到的结果作为以下量测更新的输入。

量测更新的过程在各子滤波器中进行，结合线性化后的观测模型进行以下的卡尔曼滤波量测更新过程，更新卡尔曼滤波增益K_i(t)和协方差

得到子滤波器更新的结果后，根据协方差矩阵进行融合过程，从而输出最终的位置：

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

附图说明

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

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

图3：基于航向辅助分布式SLAM算法流程图；

图4：基于航向辅助分布式程序设计流程图

图5：移动小车平台实物图；

图6：移动小车平台实验环境；

图7：移动小车平台形实验结果图。

具体实施方式

步骤1：模型的建立

系统模型的建立是基于机器人、环境特征和传感器这三个坐标系来定义的，通过图1和图2中的状态转移方程和坐标系建立里程计预测模型，本发明选用笛卡尔坐标系统作为直角坐标的表示形式，把传感器坐标系统一到机器人坐标系中，只采用全局坐标系和机器人坐标系。状态转移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息决定。即x_v＝[x_L,y_L,φ_L,x_i,y_i]^T，建立运动模型和观测模型见公式(1)(2)，两者分别利用里程计的输出和激光传感器的输出来估计相应的机器人及路标点位置信息。

步骤2：滤波初始化及数据预处理

设定卡尔曼滤波的P/Q/R矩阵初值，并进行全局坐标下机器人位置的初始化，对准磁罗盘激光传感器等传感器时钟以及读取数据路径。设置匹配路标点阈值等初始化参数。此外，对传感器数据进行预处理，根据每一时刻扫描的点云(511个)中设计函数提取激光传感器特征点，并进行航向弧度值的计算等。

步骤3：路标点匹配

在机器人运行过程中，将t时刻观测到路标点与已在全局地图中存储的路标点相匹配，进行相关的数据关联运算，若匹配成功，则送入所匹配上的t-1时刻对应路标点的子滤波器进行后续运算；若没有匹配成功，则说明该路标点为新增路标点，直接将其添加至全局地图并建立相关子滤波器。对于t时刻未能观测到t-1时刻路标点的子滤波器，进行删除。

步骤4：滤波器运行过程

将运动模型及观测模型进行线性化处理，将传感器输入的结果通过线性化的模型处理后进行卡尔曼滤波，每个路标点的信息及t时刻的机器人航向信息送入同一个子滤波器，通过卡尔曼滤波算法对上述步骤中建立的各个子滤波器的状态向量和协方差进行处理，分别进行状态转移矩阵和相应协方差矩阵的迭代更新，每个子滤波器中进行如下两个关键的计算步骤，：

预测过程：

首先将t-1时刻的每个子滤波器的估计状态和协方差矩阵，结合t时刻里程计的输出数据，代入运动模型(1)中，再经过一步预测方程(14)和(15)进行一步递推，得到由里程计和状态转移矩阵推算出的机器人位置。

量测更新过程：

通过第一步递推的方式，得到机器人位置的预测值后，根据预测值的结果，加入激光传感器和磁罗盘的观测信息进行量测更新的计算，具体见式(16)，(17)和(18)，输出每个子滤波器对位置的量测修正值及路标点的更新值，并同时输出相应的协方差矩阵供后续迭代。

每次的滤波重复以上两个过程，能够持续观测到的路标点所建立的滤波器，在t时刻继承t-1时刻的协方差矩阵作为t时刻的初始输入矩阵，新观测到的路标点对应的子滤波器则采用t-1时刻最终估计得协方差矩阵作为初始输入矩阵。

步骤5：滤波器融合输出最终结果

将各个子滤波器的估计结果输入到主滤波器中，根据各子滤波器协方差确定各子滤波器在主滤波器中所占的权重并归一化，最后利用信息融合公式(20)输出最终位置估计结果。

通过实际场景实验对航向辅助的分布式SLAM进行验证。实验平台是基于ROS操作系统操控制的两轮小车，传感器使用URG-04LX的激光传感器，磁罗盘使用迈科传感公司生产的HCM365B全姿态三维电子罗盘，移动小车平台的实物图如图5。实验的测试环境中，以圆柱形筒作为路标点分别放置于实验跑道两侧，实验环境如图6所示，通过使用本发明算法和未加入航向的EKF算法对比，证明本算法在精度及保持可观测性的优势。

实验数据在Matlab中进行绘图，结果如图7所示，在里程计数据无法保证系统收敛性的要求的情况下，图中给出了传统集中式EKF算法(CEKF)和本发明所应用的基于航向辅助的分布式EKF算法(DH-EKF)的运算结果，对比图中DH-EKF和传统CEKF算法，可以看出加入航向辅助的算法更平滑稳定且估计结果一致性和收敛性更好，误差更小。证明加入航向修正后，对于系统可能存在的动态干扰和观测的不确定性均有很好的稳定性表现。并且，由于航向的引入，以往分布式结构中单纯路标点扫描带来的可观测性问题也能得到很好解决，从而增加系统了的稳定性和精度。

Claims (2)

1.基于航向辅助分布式SLAM的机器人自主导航方法，其特征在于：在每个路标点信息送入各自子滤波器的方式下，改变观测模型的结构，在不添加每个分布式子滤波器中路标点个数的条件下，将航向信息作为状态量融合到整个的扩展卡尔曼滤波过程中，下文将论述分布式架构中粒子滤波与卡尔曼滤波实现的差异，在采用分布式EKF的条件下，若需保证精度，与粒子滤波不同的是需要引入航向信息作为观测量，从而在减少计算量同时保证可观性的条件下，本方法通过航向引入的方式建立分布式扩展卡尔曼滤波的模型，设计相应的滤波器实现方式，并通过实验验证本算法的真实有效性；

首先，引入航向信息的系统模型阐述如下：

关于引入航向信息的机器人状态矩阵描述如下：x_v＝[x_v,y_v,φ_v,x_L,y_L]^T，其中，x_v,y_v表示机器人坐标值，φ_v表示机器人航向角，φ_v＝0表示指向x轴正向，φ_v＝π/2表示指向y轴正向；x_L,y_L表示探测得到的路标点坐标值，则系统的状态转移模型如下：

以上转移矩阵中的参数与车体及里程计的固定参数有关，式中x_v＝[x_v,y_v,φ_v,x_L,y_L]^T为机器人在t时刻的位姿及路标点位置,ΔT为时间变化量，v_c为机器人移动速度，α为机器人车轮转角，L为两轮轴间距；

扩展卡尔曼滤波的量测更新结果是由激光传感器测得的路标点距离信息及磁罗盘测得的本体航向信息计算得到的，针对激光传感器及磁罗盘给出机器人的观测模型，观测量分别t时刻第k个路标点相对于传感器的距离，角度以及所测得的航向；获得路标点的观测模型如下：

其中，m_L,x,m_L,y表示测量到的路标点相对于x_v,y_v的测量信息；

在分布式框架下，带有航向信息的各子滤波器的模型如下：

…………

其中，γ为高斯白噪声，每个子滤波器中对应的SLAM状态向量分为机器人位姿估计和路标估计共五维状态，即每次状态转移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息决定；其中各子滤波器状态向量x_v＝[x_v,y_v,φ_v,x_L,y_L]^T，h(x_v(t),m_n)表示t时刻，根据该时刻机器人状态向量x_v(t)和对应的测量信息m_n所估计的第n个路标点的测量信息；

分布式结构的滤波方式分为粒子滤波PF和扩展卡尔曼滤波EKF，PF属于非线性滤波方法，解决SLAM这种非线性模型问题具有精度上的优势，但其计算量较大，并较难移植到实时性较高的移动平台上；

对于PF，根据各子滤波器中粒子群的预测分布，针对各种传感器的模型近似利用蒙特卡洛法模拟粒子分布，然后对每个子滤波器中的粒子进行重要性权值估计如式(4)所示；

根据公式(4)得到的权值可见，权值反映各个子滤波器中粒子的分布情况，并且由观测信息z_t和根据状态向量估计的所近似的分布计算得来，每个子滤波器虽然只利用单一路标点进行滤波估计，但由于PF的权值由概率分布模型决定，并不是基于线性模型迭代计算得到，而是模拟概率统计的模型得到，故可观测性得到保证；

根据权重值更新，可输出t时刻第j个子滤波器的位姿估计结果如式(5)

根据子滤波器位姿结果，进行主滤波器的融合；如式(6)及式(7)所示；

其中，最终位姿估计x_t由子滤波器估计结果融合而来，融合系数由各子滤波器有效粒子数决定；

根据以上各式可知，PF算法整个计算过程均与子滤波器估计的概率分布有关，可观测性的问题并不像EKF算法影响显著，但其精度随着滤波器所使用的粒子数的增加而增加，所以，若想要保证良好的精度，往往滤波器所需的粒子数也较多，即计算的次数要远远大于其他非统计型的滤波方法；

考虑到计算量的问题，选用相对计算量可控的EKF算法，提高系统的实时性，而分布式架构又能很好的提高系统的容错性及计算性能；故采用分布式架构上使用EKF的方法可以充分发挥两者优势，即每个滤波器采用固定的路标点个数，防止过多动态重构的发生，并且每个滤波器使用EKF进行滤波；该种解决方案虽然最大程度地发挥了两者的优点，但不可忽视的是，EKF方法将非线性模型转化为线性模型，根据线性模型的可观测性的特质，如果滤波器中只有一个观测点或多个观测点但共线时，系统的可观测性会受到影响，无法满足滤波的可观要求，为了兼容每个滤波器中输入路标点相对较少且固定，并降低其对可观测性的影响，加入航向对可观测性进行改善，当每个子滤波器中存在一个路标点且存在当前时刻机器人本体的航向时，整个系统是可观的；综上，以下描述基于分布式架构下，结合航向信息辅助的分布式EKF算法；

首先，卡尔曼滤波解决的是线性系统模型，而SLAM模型属于非线性模型，将其进行线性化处理如下，航向信息作为观测量引入：

其中

Δx＝[x_v-x_L]Δy＝[y_v-y_L] (12)

在模型的线性化操作后，针对每个路标点建立相应的子滤波器，并匹配相关t-1时刻的路标点，将各个子滤波器中代入相应的航向值；子滤波器的位姿估计过程为：

根据将公式(1)线性化后的姿态转移矩阵进行以上的位姿预测估计，得到的结果作为以下量测更新的输入；

量测更新的过程在各子滤波器中进行，结合线性化后的观测模型H_i(k)，进行以下的卡尔曼滤波量测更新过程，更新卡尔曼滤波增益K_i(t)和协方差

得到子滤波器更新的结果后，根据协方差矩阵进行融合过程，从而输出最终的位置：

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

2.根据权利要求1所述的基于航向辅助分布式SLAM的机器人自主导航方法，其特征在于：

步骤1：模型的建立

系统模型的建立是基于机器人、环境特征和传感器这三个坐标系来定义的，通过状态转移方程和坐标系建立里程计预测模型，本方法选用笛卡尔坐标系统作为直角坐标的表示形式，把传感器坐标系统一到机器人坐标系中，只采用全局坐标系和机器人坐标系；状态转移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息决定；即x_v＝[x_v,y_v,φ_v,x_i,y_i]^T，建立运动模型和观测模型见公式(1)(2)，两者分别利用里程计的输出和激光传感器的输出来估计相应的机器人及路标点位置信息；

步骤2：滤波初始化及数据预处理

设定卡尔曼滤波的P/Q/R矩阵初值，并进行全局坐标下机器人位置的初始化，对准磁罗盘激光传感器传感器时钟以及读取数据路径；设置匹配路标点阈值初始化参数；此外，对传感器数据进行预处理，根据每一时刻扫描的点云中设计函数提取激光传感器特征点，并进行航向弧度值的计算；

步骤3：路标点匹配

在机器人运行过程中，将t时刻观测到路标点与已在全局地图中存储的路标点相匹配，进行相关的数据关联运算，若匹配成功，则送入所匹配上的t-1时刻对应路标点的子滤波器进行后续运算；若没有匹配成功，则说明该路标点为新增路标点，直接将其添加至全局地图并建立相关子滤波器；对于t时刻未能观测到t-1时刻路标点的子滤波器，进行删除；

步骤4：滤波器运行过程

将运动模型及观测模型进行线性化处理，将传感器输入的结果通过线性化的模型处理后进行卡尔曼滤波，每个路标点的信息及t时刻的机器人航向信息送入同一个子滤波器，通过卡尔曼滤波算法对上述步骤中建立的各个子滤波器的状态向量和协方差进行处理，分别进行状态转移矩阵和相应协方差矩阵的迭代更新，每个子滤波器中进行如下两个关键的计算步骤：

预测过程：

首先将t-1时刻的每个子滤波器的估计状态和协方差矩阵，结合t时刻里程计的输出数据，代入运动模型(1)中，再经过一步预测方程(14)和(15)进行一步递推，得到由里程计和状态转移矩阵推算出的机器人位置；

量测更新过程：

通过第一步递推的方式，得到机器人位置的预测值后，根据预测值的结果，加入激光传感器和磁罗盘的观测信息进行量测更新的计算，具体见式(16)，(17)和(18)，输出每个子滤波器对位置的量测修正值及路标点的更新值，并同时输出相应的协方差矩阵供后续迭代；

每次的滤波重复以上两个过程，能够持续观测到的路标点所建立的滤波器，在t时刻继承t-1时刻的协方差矩阵作为t时刻的初始输入矩阵，新观测到的路标点对应的子滤波器则采用t-1时刻最终估计得协方差矩阵作为初始输入矩阵；

步骤5：滤波器融合输出最终结果

将各个子滤波器的估计结果输入到主滤波器中，根据各子滤波器协方差确定各子滤波器在主滤波器中所占的权重并归一化，最后利用信息融合公式(20)输出最终位置估计结果。