一种基于图拓扑的双阶段主动即时定位与建图算法

1.本发明属于地面无人平台感知与自主规划技术领域，具体涉及一种基于图拓扑的双阶段主动即时定位与建图算法。


背景技术：

2.未知环境主动探索(unknown environment autonomous exploration)技术是指无人平台在缺乏先验信息的情况下自主地对未知环境进行搜索探测，收集环境信息，为后续处置提供有利支撑，可广泛应用于城市排爆排险、核生化环境勘测、矿洞救援、小行星开发等危险场景中。其意义在于不依赖于外部定位器件(如北斗定位)或人员操纵即可实现对复杂环境的勘测，大大降低了探测任务的危险性。即时定位与建图(simultaneously localization and mapping,slam)算法是指利用环境感知传感器去确定自身位置的增量，从而确定本体在环境地图中的位置，同时根据位置信息和环境感知数据建立环境点云地图。其定位精度和建图质量直接决定了无人平台是否能够完成探索任务与完成探索任务的质量。因此如何通过主动路径规划方法去提升slam方法的精度是目前学者们研究的热点。
3.针对地面无人平台的未知环境自主探索算法，已有的解决方案有如下几种：
4.方案1：文献(dang t,mascarich f,khattak s,et al.graph-based path planning for autonomous robotic exploration in subterranean environments[c]//2019ieee/rsj international conference on intelligent robots and systems(iros).ieee,2019:3105-3112.)文献提出了gbp planner算法。算法主要贡献在于，首次提出了双阶段探索算法框架，利用两个动态扩展的快速随机生成图(rapidly exploring random graph，rrg)来进行全局/局部探索路径规划算法实现，对探索信息和导航代价进行了目标函数设计，同时针对自主返航方法进行了特殊设计，实现了大范围多分支未知隧道环境的覆盖搜索。但仍然存在计算复杂度大、未考虑slam算法特性等问题。
[0005]
方案2：文献(h.zhu,c.cao,s.scherer,j.zhang,and w.wang.dsvp:dual-stage viewpoint planner for rapid exploration by dynamic expansion.ieee/rsj intl.conf.on intelligent robots and systems(iros).prague,czech,sept.2021)文献中提出的dsvp算法针对了gpb planner算法进行了改进，提出了利用启发式迭代修剪的快速扩展随机树(rapidly exploring random tree，rrt)算法来取代gbp planner局部探索阶段的rrg算法，提高了算法执行效率，同时，该文献还开源了一个基于gazebo的仿真环境，可供研究人员评估自己的探索算法效率。
[0006]
方案3：文献(chen y,huang s,zhao l,et al.cram
ér–
rao bounds and optimal design metrics for pose-graph slam[j].ieee transactions on robotics,2021,pp(99):1-15.)该文献提出了利用费舍尔信息矩阵和最小生成树数量来衡量slam算法中位姿图的不确定度，但并没有将该尺度指标应用到主动探索之中。
[0007]
本发明受上述三种方案的启发，根据所要解决问题的本身特点，加以实验验证，提出了一种基于图拓扑的大范围未知环境主动定位与建图方法，针对基于位姿图的slam算
法，设计了一种考虑slam算法精度的自主探索算法，该算法利用slam算法提供定位信息和位姿图信息以及导航地图提供的未知区域信息和障碍物信息，对探索问题和主动slam问题进行联合建模，使用rrt方法进行求解，在确保安全、快速探索的同时增加了位姿图中的回环约束，提高探索过程中的slam算法精度。此外，算法使用全局/局部双阶段的算法架构，降低了算法的时间复杂度和空间复杂度，实现了未知环境下的无碰撞路径生成。最终，无人平台可通过动态滑动窗口方法等控制器对该路径进行跟踪控制，实现在未知环境下的自主定位与建图。


技术实现要素：

[0008]
有鉴于此，本发明提供了一种基于图拓扑的双阶段主动即时定位与建图算法，能够利用slam算法提供定位信息和位姿图信息以及导航地图提供的未知区域信息和障碍物信息，对探索问题和主动slam问题进行联合建模，使用rrt方法进行求解，在确保安全、快速探索的同时增加了位姿图中的回环约束，提高探索过程中的slam算法精度。
[0009]
实现本发明的技术方案如下：
[0010]
一种基于图拓扑的双阶段主动即时定位与建图算法，包括以下步骤：
[0011]
步骤一、获得无人平台实时的位姿图和导航地图，利用位姿图筛选候选回环点，利用位姿图中的当前位置信息和导航地图生成局部前沿点；若存在局部前沿点或候选回环点，则证明局部区域中存在可探索区域或可提升slam精度区域，执行步骤二；反之，则证明当前区域无局部规划需求，需到别的区域执行探索或回环任务，执行步骤三；
[0012]
步骤二、无人平台根据自身位置和导航地图在局部规划范围中利用rrt算法在空间中生成一个无碰撞的局部拓扑图，在局部拓扑图上针对局部拓扑的每一个顶点使用dijkstra算法求解局部拓扑图上最短路径，将每一个路径填充入路径集b；针对路径集b中的每一条路径bi利用路径信息增益进行打分，路径集b中得分最高的路径即为最优路径；然后，利用局部前沿点和局部拓扑图更新全局前沿点和全局拓扑图，路径集b中若存在分支gain(bi)＞0，则将该分支的所有采样点加入全局拓扑图，全局前沿点集更新时，将加入上一次求得的全局前沿点集中，然后对该点集中的每一个点进行判断，是否满足自身所处栅格为可通行栅格且与未知栅格交界，同时能够被全局拓扑图中至少一个顶点进行观测，满足条件的点组成的集合称之为全局前沿点集
[0013]
步骤三、遍历全局前沿点集合对于某一全局前沿点fg，找到全局拓扑图上与该点欧式距离最近点，利用dijkstra算法对该最近点与机器人当前位置求解全局拓扑图上最短路径，最终可获得一个路径集；该路径集中长度最短路径对应的前沿点fg即为目标点，该点对应的最短路径即为最优路径；若不存在可行解，说明全局区域探索完成，无人平台通过全局拓扑图规划最短路径返回起始点。
[0014]
进一步地，候选回环点集由局部规划范围内存在潜在回环信息增益的位姿图g
p
上的顶点组成；其意义在于判断内是否存在有价值回环行为，降低后续回环信息增益的计算复杂度。
[0015]
进一步地，候选回环点筛选步骤如下：
[0016]
1、遍历顶点集v
p
中的所有顶点，若顶点位于中，则置入顶点集v
lp
中；
[0017]
2、利用dbscan(density-based spatial clustering of applications with noise，dbscan)聚类算法删去v
lp
中距离当前位姿点时间序列较近的顶点，得到顶点集
[0018]
3、对于中每个顶点计算其中，是中每个顶点在位姿图g
p
对应的协方差矩阵∑，是无人平台当前位姿点的协方差矩阵，若该值大于一定阈值，则可选取为候选回环点其中，eigvaluek(*)是对矩阵*取特征值，k表示第k个特征值，表示方阵的维数。
[0019]
进一步地，步骤二中，路径信息增益包括三个部分：1、未知区域探索信息增益；2、主动回环slam信息增益；3、导航代价。
[0020]
进一步地，主动回环slam信息增益loopgain(*)是指当前路径能够降低slam位姿图不确定度的程度，
[0021][0022]
其中，代表了模拟执行第i条规划路径bi后，形成的位姿图，dopt(*)＝log det(*)为矩阵的行列式进行对数运算，表示位姿图g
p
的费舍尔信息矩阵，表示位姿图的费舍尔信息矩阵。
[0023]
进一步地，步骤二中，将选取的最优路径交给路径跟踪算法求解出无人平台底盘的线速度与角速度，平台可沿当前规划处出的最优路径行进。
[0024]
进一步地，步骤三中，将规划出的路径发布给路径跟踪算法实现无人平台的运动。
[0025]
有益效果：
[0026]
本发明针对大尺度未知环境下地面无人平台主动即时定位与建图问题，提出了一种基于图拓扑的双阶段主动即时定位与建图算法框架，其创新点主要体现在以下几方面：
[0027]
一，本发明方法提出了一种基于图拓扑的自主探索和主动回环的联合路径规划算法，解决了自主探索过程中slam算法长时无回环造成的误差累积问题，提高了主动建图过程中的无人平台的定位精度和建图质量。
[0028]
二，本发明方法设计了一种双阶段的主动定位与建图路径规划框架，大幅降低了算法的计算复杂度和空间复杂度，能够完成大尺度未知复杂场景的自主探索任务，兼顾slam精度，且能够实现自主返航。
附图说明
[0029]
图1为双阶段主动即时定位与建图算法框架图。
[0030]
图2为未知环境主动探索过程示意图，(a)仿真环境，(b)仿真环境自主探索与建图。
[0031]
图3为无人平台当前位置误差平方和变化对比图。
[0032]
图4为无人平台当前位置协方差矩阵d最优尺度变化对比图。
[0033]
图5为地库环境无人平台主动探索所得导航地图对比图，(a)实物环境与实物无人平台，(b)双阶段探索算法所得导航地图，(c)双阶段主动即时定位算法所得导航地图。
具体实施方式
[0034]
下面结合附图并举实施例，对本发明进行详细描述。
[0035]
本发明提出了一种基于图拓扑的双阶段主动即时定位与建图算法框架，算法整体框架如图1所示，算法效果如图2所示，其中，图2(a)为仿真环境与仿真小车，图2(b)展示了无人平台在自主探索过程中的建立的环境模型与生成的多种拓扑图。
[0036]
步骤一、局部/全局阶段决策算法
[0037]
首先，双阶段主动即时定位与建图算法通过一种激光惯性紧耦合slam算法(lio-sam)给出了无人平台的位姿图g
p
(v
p
，e
p
)，其中，v
p
为无人平台的历史关键位姿点集，e
p
为关键位姿之间的观测边集，每一个位姿点(包括机器人当前位置)或观测边都带有其协方差矩阵(不确定度)∑。同时，双阶段主动即时定位与建图算法利用激光雷达点云和无人平台当前位姿建立导航地图(二维栅格地图，用于可通行路径生成和局部前沿点的生成，该地图作用为将空间划分为0.1m*0.1m的栅格，栅格点可分为三种：可通行栅格，障碍栅格和未探索栅格)。随后，基于前面给出的信息，双阶段主动即时定位与建图算法进行候选回环点的筛选和局部前沿点的生成，若存在局部前沿点或候选回环点，则证明局部区域中存在可探索区域或可提升slam精度区域，执行步骤二；反之，则证明当前区域无局部规划需求，需到别的区域执行探索或回环任务，执行步骤三。
[0038]
(1)候选回环点筛选
[0039]
候选回环点集由局部规划范围(定义为为中心，长宽为15m*15m的正方形)内存在潜在回环信息增益的位姿图g
p
上的顶点组成。其意义在于判断内是否存在有价值回环行为，降低后续回环信息增益的计算复杂度。
[0040]
候选回环点筛选步骤如下：
[0041]
1、遍历顶点集vp中的所有顶点，若顶点位于中，则置入顶点集v
lp
中；
[0042]
2、利用dbscan(density-based spatial clustering ofapplications-with noise，dbscan)聚类算法删去v
lp
中距离当前位姿点时间序列较近的顶点，得到顶点集
[0043]
3、对于中每个顶点计算其中，是中每个顶点在位姿图g
p
对应的协方差矩阵∑，是无人平台当前位姿点的协方差矩阵，若该值大于一定阈值，则可选取为候选回环点其中，eigvaluek(*)是对矩阵*取特征值，k表示第k个特征值，表示方阵的维数。
[0044]
其意义在于：
[0045]
1、满足局部区域约束；
[0046]
2、距离当前位姿点时间序列较近的点存在少量的回环信息增益，但会极大的降低探索效率，故删去；
[0047]
3、基于位姿图的slam算法，位姿图上的每个顶点都服从于高斯分布v～n(μ，∑)，其中，μ为顶点的位姿，∑为该顶点的协方差矩阵，协方差矩阵越大，则误差值越大。dopt(*)将待判断顶点协方差矩阵转换为标量，与当前位姿的协方差矩阵dopt(*)进行判断，若差异较大，则证明可以通过回环约束来提高自身的定位精度或降低建图误差，可列为候选回环点。
[0048]
(2)局部前沿点生成
[0049]
局部前沿点集是指局部规划范围内与未探索栅格交界的已知栅格点，其作用是判断范围内是否还存在无人平台可到达的未探索区域。
[0050]
步骤二、局部主动即时定位与建图路径规划算法
[0051]
若进入步骤二，无人平台应执行局部主动即时定位与建图路径规划算法，该阶段算法目的在于进行主动的未知环境探索或基于位姿图的主动回环。如程序框图图1所示，算法流程分为建立局部rrt、使用dijkstra算法对所有顶点寻找最短路径、评估路径信息增益、选取最优路径和全局前沿点、全局拓扑图更新五个部分。无人平台根据自身位置和导航地图在局部规划范围中利用rrt算法在空间中生成一个无碰撞的局部拓扑图，在局部拓扑图上针对局部拓扑的每一个顶点使用djjkstra算法求解局部拓扑图上最短路径，将每一个路径填充入路径集b。针对路径集b中的每一条路径bi利用路径信息增益进行打分，路径集b中得分最高的路径即为最优路径。然后，利用局部前沿点和局部拓扑图更新全局前沿点和全局拓扑图，路径集b中若存在分支gain(bi)＞0，则将该分支的所有采样点加入全局拓扑图，全局前沿点集更新时，将加入上一次求得的全局前沿点集中，然后对该点集中的每一个点进行判断，是否满足自身所处栅格为可通行栅格且与未知栅格交界，同时能够被全局拓扑图中至少一个顶点进行观测，满足条件的点组成的集合称之为全局前沿点集最终，将选取的最优路径交给路径跟踪算法求解出无人平台底盘的线速度与角速度，平台可沿当前规划处出的最优路径行进。本发明的创新性主要体现在路径信息增益的提出。
[0052]
该路径信息增益主要包括三个部分：1、未知区域探索信息增益；2、主动回环slam信息增益；3、导航代价。
[0053][0054]
其中，gain(bi)为第i条规划路径bi所代表的信息增益，λ1...λ4为权重系数，loopgain(bi)为bi所表示的回环信息增益；explorationgain(bi)为bi所表示的未知区域探索信息增益；为导航代价的倒数。
[0055]
(1)未知区域探索信息增益
[0056]
未知区域探索信息增益需满足公式如下
[0057]
[0058]
其中，bi是第i条规划路径，即为局部拓扑图上多个采样点的集合。是bi上的第j个采样点。voxelgain(*)代表计算该采样点视场范围内可看到的所有未知栅格的个数。在栅格地图中可使用计算机图形学中的raytrace算法进行实现。
[0059]
(2)主动回环slam信息增益
[0060]
主动回环slam信息增益loopgain(*)是指当前路径能够降低slam位姿图不确定度的程度，其目的在于通过slam图优化算法的回环约束功能实现对于slam定位精度和建图精度的提升。
[0061]
1.slam测量模型定义：
[0062]
无人平台状态空间为由3维平移向量和旋转李代数构成。待估计状态向量为其中，其中，
[0063]
两个节点i，j之间的测量方程h可表示为：
[0064][0065]
其中，和为观测噪声，为观测噪声，
[0066]
2.回环信息增益定义：
[0067][0068]
其中，代表了模拟执行第i条规划路径bi后，形成的位姿图。dopt(*)＝log det(*)为矩阵的行列式进行对数运算。
[0069][0070]
如果bi上的采样点与图上任意一关键点nr的欧式距离小于一定阈值τ
loop
，则认为该位姿图可能产生回环，构建边e
loop
即为上述所有边的集合。
[0071]
表示了位姿图g
p
的费舍尔信息矩阵，该费舍尔信息矩阵可由文献3给出。
[0072][0073][0074]
其中，l为位姿图g
p
的拉普拉斯矩阵，是位姿图g
p
平移分量的加权拉普拉斯矩阵，是位姿图g
p
旋转分量的加权拉普拉斯矩阵。
[0075]
相似地，表示位姿图的费舍尔信息矩阵，计算过程与上述过程相同。
[0076]
(3)导航代价
[0077]
dtw(*)意为动态时间规整算法，该算法计算了当前选择路径与上一次选择路径的
相似程度，同时也反应了探索方向是否一致。dist(*)是当前路径的欧式长度。因此，探索方向与当前无人平台探索方向一致且路径长度最短，则导航代价最低，探索收益最高。
[0078]
步骤三、全局主动即时定位与建图算法
[0079]
当在局部规划范围内不存在局部前沿点或候选回环点时，算法转入步骤三。全局规划算法的目的在于引导无人平台到达全局范围内的未探索区域重新进行探索。
[0080]
遍历全局前沿点集合对于某一全局前沿点fg，找到全局拓扑图上与该点欧式距离最近点，利用dijkstra算法对该最近点与机器人当前位置求解全局拓扑图上最短路径，最终可获得一个路径集。该路径集中长度最短路径对应的前沿点fg即为目标点，该点对应的最短路径即为最优路径。若不存在可行解(或都不可达)，说明全局区域探索完成，无人平台通过全局拓扑图规划最短路径返回起始点。类似于步骤二，将规划出的路径发布给路径跟踪算法实现无人平台的运动。
[0081]
在本发明的算法运行中，算法起始阶段，由于局部(以机器人为中心的正方形区域)可达未知区域存在，算法会持续执行步骤二，直至当前局部范围无可探索区域，算法转入步骤三，前往当前导航地图中的其余可达未知区域探索，即再次执行步骤二。
[0082]
最终，本发明算法与文献2中的双阶段探索算法进行了比较，其性能提升如图3、图4、图5所示。具体而言，图3中横轴为算法执行时间，纵轴为定位误差平方和，虚线(
‑‑‑
)代表双阶段探索算法，实线(-)代表本发明提出方法，可以看到，该方法大幅提高了slam算法在探索过程中的定位精度；图4中横轴代表时间，纵轴为即为位姿图的不确定度，虚线(
‑‑‑
)代表双阶段探索算法，实线(-)代表本发明提出方法，可以看到该方法降低了位姿图在探索过程中的不确定度；图5通过实物实验(无人平台与实物环境如5(a)所示)，证明了算法的有效性，可以看到图5(b)中双阶段探索算法建立的点云图与栅格地图存在较大差异，而在图5(c)中本发明提出方法建立的点云图与栅格地图不存在差异，提高了算法的建图精度。
[0083]
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。