及磁罗盘给出机器人的观测模型，观测量 zf分别t时刻第k个路标点相对于传感器的距离，角度以及所测得的航向；获得路标点的观 测模型如下：(2) 其中，mL,x，mL,y表示测量到的路标点相对于Xv，yv的测量信息； 在分布式框架下，带有航向信息的各子滤波器的模型如下：Ο) 每个子滤波器中对应的SLAM状态向量分为机器人位姿估计和路标估计共五维状态，即 每次状态转移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息决定;其 中各子滤波器状态向量xv = [xv，yv，Φ v，xl，yL]T，h(xv(t)，mi)表示t时刻，根据该时刻机器人 状态向量χν (t)和对应的测量信息mn所估计的第η个路标点的测量信息； 分布式结构的滤波方式主要分为粒子滤波(PF)和扩展卡尔曼滤波(EKF)，PF属于非线 性滤波方法，解决SLAM这种非线性模型问题具有精度上的优势，但其计算量较大，并较难移 植到实时性较高的移动平台上； 对于PF，根据各子滤波器中粒子群的预测分布，针对各种传感器的模型近似利用蒙特 卡洛法模拟粒子分布，然后对每个子滤波器中的粒子进行重要性权值估计如式(4)所示；(4) 根据公式(4)得到的权值可见，权值反映各个子滤波器中粒子的分布情况，并且由观 测信息zt和根据状态向量估计的所近似的分布计算得来，每个子滤波器虽然只利用 单一路标点进行滤波估计，但由于PF的权值由概率分布模型决定，并不是基于线性模型迭 代计算得到，而是模拟概率统计的模型得到，故可观测性可以得到保证； 根据权重值更新，可输出t时刻第j个子滤波器的位姿估计结果如式(5)(5) 根据子滤波器位姿结果，进行主滤波器的融合;如式(6)及式(7)所示；(6) (7) 其中，最终位姿估计Xt由子滤波器估计结果χ/融合而来，融合系数9/由各子滤波器有 效粒子数4决定； 根据以上各式可知，PF算法整个计算过程均与子滤波器估计的概率分布有关，可观测 性的问题并不像EKF算法影响显著，但其精度随着滤波器所使用的粒子数的增加而增加，所 以，若想要保证良好的精度，往往滤波器所需的粒子数也较多，即计算的次数要远远大于其 他非统计型的滤波方法； 考虑到计算量的问题，选用相对计算量可控的EKF算法，提高系统的实时性，而分布式 架构又能很好的提高系统的容错性及计算性能;故采用分布式架构上使用EKF的方法可以 充分发挥两者优势，即每个滤波器采用固定的路标点个数，防止过多动态重构的发生，并且 每个滤波器使用EKF进行滤波;该种解决方案虽然最大程度地发挥了两者的优点，但不可忽 视的是，EKF方法将非线性模型转化为线性模型，根据线性模型的可观测性的特质，如果滤 波器中只有一个观测点或多个观测点但共线时，系统的可观测性会受到影响，无法满足滤 波的可观要求，为了兼容每个滤波器中输入路标点相对较少且固定，并降低其对可观测性 的影响，加入航向对可观测性进行改善，当每个子滤波器中存在一个路标点且存在当前时 刻机器人本体的航向时，整个系统是可观的；综上，以下描述基于分布式架构下，结合航向 信息辅助的分布式EKF算法； 首先，卡尔曼滤波解决的是线性系统模型，而SLAM模型属于非线性模型，将其进行线性 化处理如下，航向信息作为观测量引入：(12) (8) 其中 (9) (10) (11)(13) 在模型的线性化操作后，针对每个路标点建立相应的子滤波器，并匹配相关t-ι时刻的 路标点，将各个子滤波器中代入相应的航向值;子滤波器的位姿估计过程为：(14) (15) 根据将公式（1)线性化后的姿态转移矩阵进行以上的位姿预测估计，得到的结果作为 以下量测更新的输入； 量测更新的过程在各子滤波器中进行，结合线性化后的观测模型斤?，进行以下的卡尔 (16) 曼滤波量测更新过程，更新卡尔曼滤波增益1(〇和协方差/;(n:(17) (18)得到子滤波器更新的结果后，根据协方差矩阵进行融合过程，从而输出最终的位置：(19) (20) 其中m为加权系数，加权平均法将来自不同子滤波器的机器人位姿估计信息计算权值， 进行加权平均后的结果作为融合值。2.根据权利要求1所述的基于航向辅助分布式SLAM的机器人自主导航方法，其特征在 于： 步骤1:模型的建立 系统模型的建立是基于机器人、环境特征和传感器这三个坐标系来定义的，通过状态 转移方程和坐标系建立里程计预测模型，本发明选用笛卡尔坐标系统作为直角坐标的表示 形式，把传感器坐标系统一到机器人坐标系中，只采用全局坐标系和机器人坐标系;状态转 移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息决定；即 Xv=[a，yu 4H，Xl，yi]T，建立运动模型和观测模型见公式（1)(2)，两者分别利用里程计的输出和激光 传感器的输出来估计相应的机器人及路标点位置信息； 步骤2:滤波初始化及数据预处理 设定卡尔曼滤波的P/Q/R矩阵初值，并进行全局坐标下机器人位置的初始化，对准磁罗 盘激光传感器等传感器时钟以及读取数据路径;设置匹配路标点阈值等初始化参数;此外， 对传感器数据进行预处理，根据每一时刻扫描的点云(511个）中设计函数提取激光传感器 特征点，并进行航向弧度值的计算等； 步骤3:路标点匹配 在机器人运行过程中，将t时刻观测到路标点与已在全局地图中存储的路标点相匹配， 进行相关的数据关联运算，若匹配成功，则送入所匹配上的t-Ι时刻对应路标点的子滤波器 进行后续运算;若没有匹配成功，则说明该路标点为新增路标点，直接将其添加至全局地图 并建立相关子滤波器;对于t时刻未能观测到t-Ι时刻路标点的子滤波器，进行删除； 步骤4:滤波器运行过程 将运动模型及观测模型进行线性化处理，将传感器输入的结果通过线性化的模型处理 后进行卡尔曼滤波，每个路标点的信息及t时刻的机器人航向信息送入同一个子滤波器，通 过卡尔曼滤波算法对上述步骤中建立的各个子滤波器的状态向量和协方差进行处理，分别 进行状态转移矩阵和相应协方差矩阵的迭代更新，每个子滤波器中进行如下两个关键的计 算步骤，： 预测过程： 首先将t-i时刻的每个子滤波器的估计状态和协方差矩阵，结合t时刻里程计的输出数 据，代入运动模型（1)中，再经过一步预测方程（14)和（15)进行一步递推，得到由里程计和 状态转移矩阵推算出的机器人位置； 量测更新过程： 通过第一步递推的方式，得到机器人位置的预测值后，根据预测值的结果，加入激光传 感器和磁罗盘的观测信息进行量测更新的计算，具体见式（16)，（17)和（18)，输出每个子滤 波器对位置的量测修正值及路标点的更新值，并同时输出相应的协方差矩阵供后续迭代； 每次的滤波重复以上两个过程，能够持续观测到的路标点所建立的滤波器，在t时刻继 承t-Ι时刻的协方差矩阵作为t时刻的初始输入矩阵，新观测到的路标点对应的子滤波器则 米用t_l时刻最终估计得协方差矩阵作为初始输入矩阵； 步骤5:滤波器融合输出最终结果 将各个子滤波器的估计结果输入到主滤波器中，根据各子滤波器协方差确定各子滤波 器在主滤波器中所占的权重并归一化，最后利用信息融合公式(20)输出最终位置估计结 果。
【专利摘要】本发明公开了基于航向辅助分布式SLAM的机器人自主导航方法，属于机器人自主导航的范畴。本方法旨在结合机器人运动模型及观测模型的特点，在分布式滤波运算的框架下，分别比较粒子滤波与扩展卡尔曼滤波的诸如计算量等对系统性能有较大影响的因素，考虑到实时系统中对计算量的要求，采用分布式EKF算法设计滤波器而非采用PF算法，为了同时保证系统的可观测性，在传统的里程计+激光传感器作为传感器组合方案之外，引入磁罗盘作为辅助传感器从而给系统加入机器人的航向信息。该设计针对分布式滤波结构的观测模型，通过引入航向信息重新规划了滤波方法从而达到实用化机器人自主导航的目的，从而增加系统了的稳定性和精度。
【IPC分类】G01C21/20
【公开号】CN105606104
【申请号】CN201610154634
【发明人】裴福俊, 程雨航, 武小平, 严鸿
【申请人】北京工业大学
【公开日】2016年5月25日
【申请日】2016年3月17日