[发明专利]基于航向辅助分布式SLAM的机器人自主导航方法

摘要

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

主权项

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为加权系数，加权平均法将来自不同子滤波器的机器人位姿估计信息计算权值，进行加权平均后的结果作为融合值。