[发明专利]基于结构单元编码和多假设跟踪的线激光全局定位方法

摘要

基于结构单元编码和多假设跟踪的线激光全局定位方法。针对利用线激光为移动机器人提供全局定位的任务，本发明提出一个新颖的基于SUES(结构单元编码机制，structural unit encoding scheme)和扩展MHT(多假设跟踪,multiple hypothesis tracking)的全局定位方法。首先通过SUES对有向端点特征进行编码；再利用扩展MHT逐步恢复全局位姿。相比现有方法，本发明所提的SUES无需离线训练，不依赖于词典质量；同时，所提的扩展MHT方法具有收敛更快和不依赖特征数量的优点。实验结果表明，相比于自适应蒙特卡洛定位，本发明具有更高的成功率和效率。

主权项

1.一种基于结构单元编码和多假设跟踪的线激光全局定位方法，其特征在于该方法包括：第1，利用SUES对有向端点特征之间的几何关系进行编码第1.1构造产生结构单元对激光数据进行线段提取，定义P＝{p1,...,pN}为有向端点特征点集，其中N表示特征点数量；第i个有向端点特征以pi＝[xi yi θi]T表示，其中(xi,yi)定义为特征位置，θi定义为特征方向；定义ρj为特征点pl＝[xl yl θl]T与特征点pn＝[xn yn θn]T之间的欧式距离，其中j为索引编号，当满足ρj≤ξ时，定义结构单元sj＝{ρj,θj1,θj2}表示特征点pl和pn之间的几何关系，其中ξ为给定阈值，θj1和θj2为特征点pl和pn之间的方向信息，计算如下：其中θ′l,θ′n为方向信息，位于区间[0,360°)；定义S＝{s1,…,sM}为构造得到的结构单元集合，其中M定义为结构单元数量，根据上述构造方式获得；第1.2通过SUES对结构单元进行编码定义δ_a和δ_d分别为角度分辨率和距离分辨率，将整个角度空间和距离空间以上述分辨率划分成小栅格；定义w_i为由2个角度分割线和1个距离分割线组合形成的结构单词，其中i为索引值，总共能够产生L＝b²d个结构单词，其中采用基于分割线的软投票机制，将产生的结构单元s_j＝{ρ_j,θ_j1,θ_j2}进行编码，首先根据软投票机制产生分割线，其中φ_i、i＝1,...,6定义为具体分割线索引值；然后根据距离分割线的距离大小决定每个分割线的权重；最后通过分割线之间的组合，每个结构单元能够产生8个结构单词，如是将所有结构单元进行编码再归一化，就能够得到激光数据所对应的特征向量；第2，通过扩展MHT逐步恢复全局位姿第2.1建立多假设概率模型通过SUES对当前激光数据进行单元结构特征编码，然后通过相似度计算能够从离线位姿地图中获取候选关键帧并得到候选全局位姿；区别于传统MHT方法，本发明将独立观测获得的候选全局位姿信息作为先验项，将连续的候选位姿和里程计增量信息作为似然项；关于k时刻机器人位姿序列的概率分布模型表示如下：p(X1:k|u1:k‑1,z1:k,m),                            (10)其中u1:k‑1定义为里程计信息序列；z1:k表示观测序列，同时X1:k和m分别表示候选机器人位姿序列和先验地图；为了减少计算量，整个分布使用如下递归进行计算：p(X1:k|u1:k‑1,z1:k,m)＝p(Xk|uk‑1,zk,m,X1:k‑1)·p(X1:k‑1|u1:k‑2,z1:k‑1,m),     (11)其中p(X1:k‑1|u1:k‑2,z1:k‑1,m)表示k‑1时刻的位姿序列概率分布，p(Xk|uk‑1,zk,m,X1:k‑1)表示k时刻机器人位姿的概率分布；利用贝叶斯规则对p(Xk|uk‑1,zk,m,X1:k‑1)进行展开，得到如下：其中p(uk‑1|Xk‑1,Xk)和p(Xk|zk,m)分别表示似然项和先验项，p(uk‑1|zk,m,X1:k‑1)为归一化项；能够得到概率分布函数正比于似然函数p(Xk|uk‑1,zk,m,X1:k‑1)∝p(uk‑1|Xk‑1,Xk)；第2.2利用里程计模型进行似然估计利用里程计模型能够得到两个连续位姿之间的增量参数，包括两个角度和一个距离；定义δ′rot1、δ′tran和δ′rot2为连续候选全局位姿Xk‑1,Xk的增量参数；定义δrot1、δtran和δrot2为里程计信息uk‑1的增量参数；利用里程计模型获得的增量参数之间相互独立，因此p(uk‑1|Xk‑1,Xk)∝p1·p2·p3，其中其中，αi(i＝1,...,4)为里程计模型中的噪声参数，prob()为正态分布函数；机器人候选位姿序列的概率分布模型通过独立观测和里程计模型进行计算；在实际操作的时候，每一次更新概率分布之后，都要检测去除概率小于特定阈值的位姿序列，直至剩下唯一一个位姿序列对应全局定位结果。