[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

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

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

Info

Publication number
CN114596360B
CN114596360B CN202210161358.7A CN202210161358A CN114596360B CN 114596360 B CN114596360 B CN 114596360B CN 202210161358 A CN202210161358 A CN 202210161358A CN 114596360 B CN114596360 B CN 114596360B
Authority
CN
China
Prior art keywords
path
local
point
global
graph
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210161358.7A
Other languages
English (en)
Other versions
CN114596360A (zh
Inventor
徐伯辰
郭子萱
郑可凡
班超
韩紫叙
卢星霖
曾宪琳
杨庆凯
方浩
陈杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202210161358.7A priority Critical patent/CN114596360B/zh
Publication of CN114596360A publication Critical patent/CN114596360A/zh
Application granted granted Critical
Publication of CN114596360B publication Critical patent/CN114596360B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G06T19/003—Navigation within 3D models or images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Software Systems (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Graphics (AREA)
  • Remote Sensing (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Geometry (AREA)
  • Computer Hardware Design (AREA)
  • General Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Multimedia (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开一种基于图拓扑的双阶段主动即时定位与建图算法，该算法分为全局/局部两阶段；首先，算法获取到无人平台当前的位姿，位姿图和导航地图，计算局部前沿点和候选回环点。若上述点存在，则进入局部探索阶段，通过快速随机生成树算法构建局部拓扑图，利用Dijkstra算法求解到达局部拓扑图的任意顶点的最短路径，对路径集根据局部探索与主动回环联合目标函数进行打分，求解最优路径，同时根据局部拓扑图和局部前沿点更新全局拓扑图和全局前沿点。反之，在局部范围无局部前沿点且无回环需求时，算法进入全局探索阶段，无人平台依据全局拓扑图前往未探索区域重新进行局部探索，直到完成指定区域探索之后，返回出发点，探索任务完成。

Description

一种基于图拓扑的双阶段主动即时定位与建图算法
技术领域
本发明属于地面无人平台感知与自主规划技术领域，具体涉及一种基于图拓扑的双阶段主动即时定位与建图算法。
背景技术
未知环境主动探索(Unknown Environment Autonomous Exploration)技术是指无人平台在缺乏先验信息的情况下自主地对未知环境进行搜索探测，收集环境信息，为后续处置提供有利支撑，可广泛应用于城市排爆排险、核生化环境勘测、矿洞救援、小行星开发等危险场景中。其意义在于不依赖于外部定位器件(如北斗定位)或人员操纵即可实现对复杂环境的勘测，大大降低了探测任务的危险性。即时定位与建图(SimultaneouslyLocalization and Mapping,SLAM)算法是指利用环境感知传感器去确定自身位置的增量，从而确定本体在环境地图中的位置，同时根据位置信息和环境感知数据建立环境点云地图。其定位精度和建图质量直接决定了无人平台是否能够完成探索任务与完成探索任务的质量。因此如何通过主动路径规划方法去提升SLAM方法的精度是目前学者们研究的热点。
针对地面无人平台的未知环境自主探索算法，已有的解决方案有如下几种：
方案1：文献(Dang T,Mascarich F,Khattak S,et al.Graph-based pathplanning 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 ExploringRandom Graph，RRG)来进行全局/局部探索路径规划算法实现，对探索信息和导航代价进行了目标函数设计，同时针对自主返航方法进行了特殊设计，实现了大范围多分支未知隧道环境的覆盖搜索。但仍然存在计算复杂度大、未考虑SLAM算法特性等问题。
方案2：文献(H.Zhu,C.Cao,S.Scherer,J.Zhang,and W.Wang.DSVP:Dual-StageViewpoint Planner for Rapid Exploration by Dynamic Expansion.IEEE/RSJIntl.Conf.on Intelligent Robots and Systems(IROS).Prague,Czech,Sept.2021)文献中提出的DSVP算法针对了GPB planner算法进行了改进，提出了利用启发式迭代修剪的快速扩展随机树(Rapidly Exploring Random Tree，RRT)算法来取代GBP planner局部探索阶段的RRG算法，提高了算法执行效率，同时，该文献还开源了一个基于Gazebo的仿真环境，可供研究人员评估自己的探索算法效率。
方案3：文献(Chen Y,Huang S,Zhao L,et al.Cramér–Rao Bounds and OptimalDesign Metrics for Pose-Graph SLAM[J].IEEE Transactions on Robotics,2021,PP(99):1-15.)该文献提出了利用费舍尔信息矩阵和最小生成树数量来衡量SLAM算法中位姿图的不确定度，但并没有将该尺度指标应用到主动探索之中。
本发明受上述三种方案的启发，根据所要解决问题的本身特点，加以实验验证，提出了一种基于图拓扑的大范围未知环境主动定位与建图方法，针对基于位姿图的SLAM算法，设计了一种考虑SLAM算法精度的自主探索算法，该算法利用SLAM算法提供定位信息和位姿图信息以及导航地图提供的未知区域信息和障碍物信息，对探索问题和主动SLAM问题进行联合建模，使用RRT方法进行求解，在确保安全、快速探索的同时增加了位姿图中的回环约束，提高探索过程中的SLAM算法精度。此外，算法使用全局/局部双阶段的算法架构，降低了算法的时间复杂度和空间复杂度，实现了未知环境下的无碰撞路径生成。最终，无人平台可通过动态滑动窗口方法等控制器对该路径进行跟踪控制，实现在未知环境下的自主定位与建图。
发明内容
有鉴于此，本发明提供了一种基于图拓扑的双阶段主动即时定位与建图算法，能够利用SLAM算法提供定位信息和位姿图信息以及导航地图提供的未知区域信息和障碍物信息，对探索问题和主动SLAM问题进行联合建模，使用RRT方法进行求解，在确保安全、快速探索的同时增加了位姿图中的回环约束，提高探索过程中的SLAM算法精度。
实现本发明的技术方案如下：
一种基于图拓扑的双阶段主动即时定位与建图算法，包括以下步骤：
步骤一、获得无人平台实时的位姿图和导航地图，利用位姿图筛选候选回环点，利用位姿图中的当前位置信息和导航地图生成局部前沿点；若存在局部前沿点或候选回环点，则证明局部区域中存在可探索区域或可提升SLAM精度区域，执行步骤二；反之，则证明当前区域无局部规划需求，需到别的区域执行探索或回环任务，执行步骤三；
步骤二、无人平台根据自身位置和导航地图在局部规划范围
Figure BDA0003514848900000031
中利用RRT算法在空间中生成一个无碰撞的局部拓扑图，在局部拓扑图上针对局部拓扑的每一个顶点使用Dijkstra算法求解局部拓扑图上最短路径，将每一个路径填充入路径集B；针对路径集B中的每一条路径bi利用路径信息增益进行打分，路径集B中得分最高的路径即为最优路径；然后，利用局部前沿点和局部拓扑图更新全局前沿点和全局拓扑图，路径集B中若存在分支Gain(bi)＞0，则将该分支的所有采样点加入全局拓扑图，全局前沿点集更新时，将/>
Figure BDA0003514848900000041
加入上一次求得的全局前沿点集中，然后对该点集中的每一个点进行判断，是否满足自身所处栅格为可通行栅格且与未知栅格交界，同时能够被全局拓扑图中至少一个顶点进行观测，满足条件的点组成的集合称之为全局前沿点集/>
Figure BDA0003514848900000042
步骤三、遍历全局前沿点集合
Figure BDA0003514848900000043
对于某一全局前沿点fg，找到全局拓扑图上与该点欧式距离最近点，利用Dijkstra算法对该最近点与机器人当前位置/>
Figure BDA0003514848900000044
求解全局拓扑图上最短路径，最终可获得一个路径集；该路径集中长度最短路径对应的前沿点fg即为目标点，该点对应的最短路径即为最优路径；若不存在可行解，说明全局区域探索完成，无人平台通过全局拓扑图规划最短路径返回起始点。
进一步地，候选回环点集
Figure BDA0003514848900000045
由局部规划范围/>
Figure BDA0003514848900000046
内存在潜在回环信息增益的位姿图Gp上的顶点组成；其意义在于判断/>
Figure BDA0003514848900000047
内是否存在有价值回环行为，降低后续回环信息增益的计算复杂度。
进一步地，候选回环点筛选步骤如下：
1、遍历顶点集Vp中的所有顶点，若顶点位于
Figure BDA0003514848900000048
中，则置入顶点集Vl p中；
2、利用DBSCAN(Density-Based Spatial Clustering of Applications withNoise，DBSCAN)聚类算法删去Vl p中距离当前位姿点时间序列较近的顶点，得到顶点集
Figure BDA0003514848900000049
3、对于
Figure BDA00035148489000000410
中每个顶点/>
Figure BDA00035148489000000411
计算/>
Figure BDA00035148489000000412
其中，/>
Figure BDA00035148489000000413
是/>
Figure BDA00035148489000000414
中每个顶点在位姿图Gp对应的协方差矩阵∑，/>
Figure BDA00035148489000000415
是无人平台当前位姿点/>
Figure BDA00035148489000000416
的协方差矩阵，/>
Figure BDA00035148489000000417
若该值大于一定阈值，则可选取为候选回环点/>
Figure BDA0003514848900000051
其中，eigValuek(*)是对矩阵*取特征值，k表示第k个特征值，/>
Figure BDA0003514848900000052
表示方阵的维数。
进一步地，步骤二中，路径信息增益包括三个部分：1、未知区域探索信息增益；2、主动回环SLAM信息增益；3、导航代价。
进一步地，主动回环SLAM信息增益LoopGain(*)是指当前路径能够降低SLAM位姿图不确定度的程度，
Figure BDA0003514848900000053
其中，
Figure BDA0003514848900000054
代表了模拟执行第i条规划路径bi后，形成的位姿图，Dopt(*)＝log det(*)为矩阵的行列式进行对数运算，/>
Figure BDA0003514848900000055
表示位姿图Gp的费舍尔信息矩阵，/>
Figure BDA0003514848900000056
表示位姿图
Figure BDA0003514848900000057
的费舍尔信息矩阵。
进一步地，步骤二中，将选取的最优路径交给路径跟踪算法求解出无人平台底盘的线速度与角速度，平台可沿当前规划处出的最优路径行进。
进一步地，步骤三中，将规划出的路径发布给路径跟踪算法实现无人平台的运动。
有益效果：
本发明针对大尺度未知环境下地面无人平台主动即时定位与建图问题，提出了一种基于图拓扑的双阶段主动即时定位与建图算法框架，其创新点主要体现在以下几方面：
一，本发明方法提出了一种基于图拓扑的自主探索和主动回环的联合路径规划算法，解决了自主探索过程中SLAM算法长时无回环造成的误差累积问题，提高了主动建图过程中的无人平台的定位精度和建图质量。
二，本发明方法设计了一种双阶段的主动定位与建图路径规划框架，大幅降低了算法的计算复杂度和空间复杂度，能够完成大尺度未知复杂场景的自主探索任务，兼顾SLAM精度，且能够实现自主返航。
附图说明
图1为双阶段主动即时定位与建图算法框架图。
图2为未知环境主动探索过程示意图，(a)仿真环境，(b)仿真环境自主探索与建图。
图3为无人平台当前位置误差平方和变化对比图。
图4为无人平台当前位置协方差矩阵D最优尺度变化对比图。
图5为地库环境无人平台主动探索所得导航地图对比图，(a)实物环境与实物无人平台，(b)双阶段探索算法所得导航地图，(c)双阶段主动即时定位算法所得导航地图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本发明提出了一种基于图拓扑的双阶段主动即时定位与建图算法框架，算法整体框架如图1所示，算法效果如图2所示，其中，图2(a)为仿真环境与仿真小车，图2(b)展示了无人平台在自主探索过程中的建立的环境模型与生成的多种拓扑图。
步骤一、局部/全局阶段决策算法
首先，双阶段主动即时定位与建图算法通过一种激光惯性紧耦合SLAM算法(lio-sam)给出了无人平台的位姿图Gp(Vp，Ep)，其中，Vp为无人平台的历史关键位姿点集，Ep为关键位姿之间的观测边集，每一个位姿点(包括机器人当前位置
Figure BDA0003514848900000061
)或观测边都带有其协方差矩阵(不确定度)∑。同时，双阶段主动即时定位与建图算法利用激光雷达点云和无人平台当前位姿建立导航地图(二维栅格地图，用于可通行路径生成和局部前沿点的生成，该地图作用为将空间划分为0.1m*0.1m的栅格，栅格点可分为三种：可通行栅格，障碍栅格和未探索栅格)。随后，基于前面给出的信息，双阶段主动即时定位与建图算法进行候选回环点的筛选和局部前沿点的生成，若存在局部前沿点或候选回环点，则证明局部区域中存在可探索区域或可提升SLAM精度区域，执行步骤二；反之，则证明当前区域无局部规划需求，需到别的区域执行探索或回环任务，执行步骤三。
(1)候选回环点筛选
候选回环点集
Figure BDA00035148489000000717
由局部规划范围/>
Figure BDA0003514848900000071
(/>
Figure BDA0003514848900000072
定义为/>
Figure BDA0003514848900000073
为中心，长宽为15m*15m的正方形)内存在潜在回环信息增益的位姿图Gp上的顶点组成。其意义在于判断/>
Figure BDA0003514848900000074
内是否存在有价值回环行为，降低后续回环信息增益的计算复杂度。
候选回环点筛选步骤如下：
1、遍历顶点集Vp中的所有顶点，若顶点位于
Figure BDA0003514848900000075
中，则置入顶点集Vl p中；
2、利用DBSCAN(Density-Based Spatial Clustering ofApplications-withNoise，DBSCAN)聚类算法删去Vl p中距离当前位姿点时间序列较近的顶点，得到顶点集
Figure BDA0003514848900000076
3、对于
Figure BDA0003514848900000077
中每个顶点/>
Figure BDA0003514848900000078
计算/>
Figure BDA0003514848900000079
其中，/>
Figure BDA00035148489000000710
是/>
Figure BDA00035148489000000711
中每个顶点在位姿图Gp对应的协方差矩阵∑，/>
Figure BDA00035148489000000712
是无人平台当前位姿点/>
Figure BDA00035148489000000713
的协方差矩阵，/>
Figure BDA00035148489000000714
若该值大于一定阈值，则可选取为候选回环点/>
Figure BDA00035148489000000715
其中，eigValuek(*)是对矩阵*取特征值，k表示第k个特征值，/>
Figure BDA00035148489000000716
表示方阵的维数。
其意义在于：
1、满足局部区域约束；
2、距离当前位姿点时间序列较近的点存在少量的回环信息增益，但会极大的降低探索效率，故删去；
3、基于位姿图的SLAM算法，位姿图上的每个顶点都服从于高斯分布v～N(μ，∑)，其中，μ为顶点的位姿，∑为该顶点的协方差矩阵，协方差矩阵越大，则误差值越大。dopt(*)将待判断顶点协方差矩阵转换为标量，与当前位姿的协方差矩阵dopt(*)进行判断，若差异较大，则证明可以通过回环约束来提高自身的定位精度或降低建图误差，可列为候选回环点。
(2)局部前沿点生成
局部前沿点集
Figure BDA0003514848900000081
是指局部规划范围/>
Figure BDA0003514848900000082
内与未探索栅格交界的已知栅格点，其作用是判断/>
Figure BDA0003514848900000083
范围内是否还存在无人平台可到达的未探索区域。
步骤二、局部主动即时定位与建图路径规划算法
若进入步骤二，无人平台应执行局部主动即时定位与建图路径规划算法，该阶段算法目的在于进行主动的未知环境探索或基于位姿图的主动回环。如程序框图图1所示，算法流程分为建立局部RRT、使用Dijkstra算法对所有顶点寻找最短路径、评估路径信息增益、选取最优路径和全局前沿点、全局拓扑图更新五个部分。无人平台根据自身位置和导航地图在局部规划范围
Figure BDA0003514848900000084
中利用RRT算法在空间中生成一个无碰撞的局部拓扑图，在局部拓扑图上针对局部拓扑的每一个顶点使用Djjkstra算法求解局部拓扑图上最短路径，将每一个路径填充入路径集B。针对路径集B中的每一条路径bi利用路径信息增益进行打分，路径集B中得分最高的路径即为最优路径。然后，利用局部前沿点和局部拓扑图更新全局前沿点和全局拓扑图，路径集B中若存在分支Gain(bi)＞0，则将该分支的所有采样点加入全局拓扑图，全局前沿点集更新时，将/>
Figure BDA0003514848900000085
加入上一次求得的全局前沿点集中，然后对该点集中的每一个点进行判断，是否满足自身所处栅格为可通行栅格且与未知栅格交界，同时能够被全局拓扑图中至少一个顶点进行观测，满足条件的点组成的集合称之为全局前沿点集/>
Figure BDA0003514848900000091
最终，将选取的最优路径交给路径跟踪算法求解出无人平台底盘的线速度与角速度，平台可沿当前规划处出的最优路径行进。本发明的创新性主要体现在路径信息增益的提出。
该路径信息增益主要包括三个部分：1、未知区域探索信息增益；2、主动回环SLAM信息增益；3、导航代价。
Figure BDA0003514848900000092
其中，Gain(bi)为第i条规划路径bi所代表的信息增益，λ1...λ4为权重系数，LoopGain(bi)为bi所表示的回环信息增益；ExplorationGain(bi)为bi所表示的未知区域探索信息增益；
Figure BDA0003514848900000093
为导航代价的倒数。
(1)未知区域探索信息增益
未知区域探索信息增益需满足公式如下
Figure BDA0003514848900000094
其中，bi是第i条规划路径，即为局部拓扑图上多个采样点的集合。
Figure BDA0003514848900000095
是bi上的第j个采样点。VoxelGain(*)代表计算该采样点视场范围内可看到的所有未知栅格的个数。在栅格地图中可使用计算机图形学中的RayTrace算法进行实现。
(2)主动回环SLAM信息增益
主动回环SLAM信息增益LoopGain(*)是指当前路径能够降低SLAM位姿图不确定度的程度，其目的在于通过SLAM图优化算法的回环约束功能实现对于SLAM定位精度和建图精度的提升。
1.SLAM测量模型定义：
无人平台状态空间为
Figure BDA0003514848900000101
由3维平移向量和旋转李代数构成。待估计状态向量为/>
Figure BDA0003514848900000102
其中，/>
Figure BDA0003514848900000103
Figure BDA0003514848900000104
两个节点i，j之间的测量方程h可表示为：
Figure BDA0003514848900000105
其中，
Figure BDA0003514848900000106
和/>
Figure BDA0003514848900000107
为观测噪声，
Figure BDA0003514848900000108
Figure BDA0003514848900000109
2.回环信息增益定义：
Figure BDA00035148489000001010
其中，
Figure BDA00035148489000001011
代表了模拟执行第i条规划路径bi后，形成的位姿图。Dopt(*)＝log det(*)为矩阵的行列式进行对数运算。
Figure BDA00035148489000001012
如果bi上的采样点
Figure BDA00035148489000001013
与图上任意一关键点nr的欧式距离小于一定阈值τloop，则认为该位姿图可能产生回环，构建边/>
Figure BDA00035148489000001014
Eloop即为上述所有边的集合。
Figure BDA00035148489000001023
表示了位姿图Gp的费舍尔信息矩阵，该费舍尔信息矩阵可由文献3给出。
Figure BDA00035148489000001015
Figure BDA00035148489000001016
其中，L为位姿图Gp的拉普拉斯矩阵，
Figure BDA00035148489000001017
是位姿图Gp平移分量/>
Figure BDA00035148489000001018
的加权拉普拉斯矩阵，/>
Figure BDA00035148489000001019
是位姿图Gp旋转分量/>
Figure BDA00035148489000001020
的加权拉普拉斯矩阵。
相似地，
Figure BDA00035148489000001021
表示位姿图/>
Figure BDA00035148489000001022
的费舍尔信息矩阵，计算过程与上述过程相同。
(3)导航代价
DTW(*)意为动态时间规整算法，该算法计算了当前选择路径与上一次选择路径的相似程度，同时也反应了探索方向是否一致。dist(*)是当前路径的欧式长度。因此，探索方向与当前无人平台探索方向一致且路径长度最短，则导航代价最低，探索收益最高。
步骤三、全局主动即时定位与建图算法
当在局部规划范围
Figure BDA0003514848900000111
内不存在局部前沿点/>
Figure BDA0003514848900000112
或候选回环点/>
Figure BDA0003514848900000113
时，算法转入步骤三。全局规划算法的目的在于引导无人平台到达全局范围内的未探索区域重新进行探索。
遍历全局前沿点集合
Figure BDA0003514848900000114
对于某一全局前沿点fg，找到全局拓扑图上与该点欧式距离最近点，利用Dijkstra算法对该最近点与机器人当前位置/>
Figure BDA0003514848900000115
求解全局拓扑图上最短路径，最终可获得一个路径集。该路径集中长度最短路径对应的前沿点fg即为目标点，该点对应的最短路径即为最优路径。若不存在可行解(/>
Figure BDA0003514848900000116
或/>
Figure BDA0003514848900000117
都不可达)，说明全局区域探索完成，无人平台通过全局拓扑图规划最短路径返回起始点。类似于步骤二，将规划出的路径发布给路径跟踪算法实现无人平台的运动。
在本发明的算法运行中，算法起始阶段，由于局部(以机器人为中心的正方形区域)可达未知区域存在，算法会持续执行步骤二，直至当前局部范围无可探索区域，算法转入步骤三，前往当前导航地图中的其余可达未知区域探索，即再次执行步骤二。
最终，本发明算法与文献2中的双阶段探索算法进行了比较，其性能提升如图3、图4、图5所示。具体而言，图3中横轴为算法执行时间，纵轴为定位误差平方和，虚线(---)代表双阶段探索算法，实线(-)代表本发明提出方法，可以看到，该方法大幅提高了SLAM算法在探索过程中的定位精度；图4中横轴代表时间，纵轴为
Figure BDA0003514848900000121
即为位姿图的不确定度，虚线(---)代表双阶段探索算法，实线(-)代表本发明提出方法，可以看到该方法降低了位姿图在探索过程中的不确定度；图5通过实物实验(无人平台与实物环境如5(a)所示)，证明了算法的有效性，可以看到图5(b)中双阶段探索算法建立的点云图与栅格地图存在较大差异，而在图5(c)中本发明提出方法建立的点云图与栅格地图不存在差异，提高了算法的建图精度。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于图拓扑的双阶段主动即时定位与建图算法，其特征在于，包括以下步骤：
步骤一、获得无人平台实时的位姿图和导航地图，利用位姿图筛选候选回环点，利用位姿图中的当前位置信息和导航地图生成局部前沿点；若存在局部前沿点或候选回环点，则证明局部区域中存在可探索区域或可提升SLAM精度区域，执行步骤二；反之，则证明当前区域无局部规划需求，需到别的区域执行探索或回环任务，执行步骤三；
步骤二、无人平台根据自身位置和导航地图在局部规划范围
Figure FDA0004146259510000014
中利用RRT算法在空间中生成一个无碰撞的局部拓扑图，在局部拓扑图上针对局部拓扑的每一个顶点使用Dijkstra算法求解局部拓扑图上最短路径，将每一个路径填充入路径集B；针对路径集B中的每一条路径bi利用路径信息增益进行打分，路径集B中得分最高的路径即为最优路径；然后，利用局部前沿点和局部拓扑图更新全局前沿点和全局拓扑图，路径集B中若存在分支Gain(bi)>0，则将该分支的所有采样点加入全局拓扑图，全局前沿点集更新时，将局部前沿点集/>
Figure FDA0004146259510000015
加入上一次求得的全局前沿点集中，然后对该点集中的每一个点进行判断，是否满足自身所处栅格为可通行栅格且与未知栅格交界，同时能够被全局拓扑图中至少一个顶点进行观测，满足条件的点组成的集合称之为全局前沿点集/>
Figure FDA0004146259510000011
Gain(bi)为第i条规划路径bi所代表的信息增益；
步骤三、遍历全局前沿点集合
Figure FDA0004146259510000012
对于某一全局前沿点fg，找到全局拓扑图上与该点欧式距离最近点，利用Dijkstra算法对该最近点与机器人当前位置/>
Figure FDA0004146259510000013
求解全局拓扑图上最短路径，最终可获得一个路径集；该路径集中长度最短路径对应的前沿点fg即为目标点，该点对应的最短路径即为最优路径；若不存在可行解，说明全局区域探索完成，无人平台通过全局拓扑图规划最短路径返回起始点；
候选回环点集
Figure FDA0004146259510000021
由局部规划范围/>
Figure FDA0004146259510000022
内存在潜在回环信息增益的位姿图Gp上的顶点组成；
候选回环点筛选步骤如下：
(1)遍历顶点集Vp中的所有顶点，若顶点位于
Figure FDA0004146259510000023
中，则置入顶点集Vl p中；
(2)利用DBSCAN聚类算法删去Vl p中距离当前位姿点时间序列较近的顶点，得到顶点集
Figure FDA0004146259510000024
(3)对于
Figure FDA0004146259510000025
中每个顶点/>
Figure FDA0004146259510000026
计算/>
Figure FDA0004146259510000027
其中，/>
Figure FDA0004146259510000028
是/>
Figure FDA0004146259510000029
中每个顶点在位姿图Gp对应的协方差矩阵Σ，/>
Figure FDA00041462595100000210
是无人平台当前位姿点/>
Figure FDA00041462595100000211
的协方差矩阵，
Figure FDA00041462595100000212
若该值大于一定阈值，则可选取为候选回环点/>
Figure FDA00041462595100000213
其中，eigValuek(*)是对矩阵*取特征值，k表示第k个特征值，l表示方阵的维数；
步骤二中，路径信息增益包括三个部分：(1)未知区域探索信息增益；(2)主动回环SLAM信息增益；(3)导航代价；
主动回环SLAM信息增益LoopGain(*)是指当前路径能够降低SLAM位姿图不确定度的程度，
Figure FDA00041462595100000214
其中，
Figure FDA00041462595100000215
代表了模拟执行第i条规划路径bi后，形成的位姿图，Dopt(*)＝(*)为矩阵的行列式进行对数运算，/>
Figure FDA00041462595100000216
表示位姿图Gp的费舍尔信息矩阵，/>
Figure FDA00041462595100000217
表示位姿图/>
Figure FDA00041462595100000218
的费舍尔信息矩阵。
2.如权利要求1所述的一种基于图拓扑的双阶段主动即时定位与建图算法，其特征在于，步骤二中，将选取的最优路径交给路径跟踪算法求解出无人平台底盘的线速度与角速度，平台可沿当前规划处出的最优路径行进。
3.如权利要求1所述的一种基于图拓扑的双阶段主动即时定位与建图算法，其特征在于，步骤三中，将规划出的路径发布给路径跟踪算法实现无人平台的运动。
CN202210161358.7A 2022-02-22 2022-02-22 一种基于图拓扑的双阶段主动即时定位与建图算法 Active CN114596360B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210161358.7A CN114596360B (zh) 2022-02-22 2022-02-22 一种基于图拓扑的双阶段主动即时定位与建图算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210161358.7A CN114596360B (zh) 2022-02-22 2022-02-22 一种基于图拓扑的双阶段主动即时定位与建图算法

Publications (2)

Publication Number Publication Date
CN114596360A CN114596360A (zh) 2022-06-07
CN114596360B true CN114596360B (zh) 2023-06-27

Family

ID=81804656

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210161358.7A Active CN114596360B (zh) 2022-02-22 2022-02-22 一种基于图拓扑的双阶段主动即时定位与建图算法

Country Status (1)

Country Link
CN (1) CN114596360B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114998540A (zh) * 2022-06-12 2022-09-02 哈尔滨工程大学 一种智慧城市传感器探测主动同步定位与建图方法
CN115617034B (zh) * 2022-09-01 2024-06-14 清华大学 多智能体的环境探索方法、装置、电子设备及存储介质
CN116182840B (zh) * 2023-04-28 2023-07-25 科大讯飞股份有限公司 地图构建方法、装置、设备及存储介质

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110703747B (zh) * 2019-10-09 2021-08-03 武汉大学 一种基于简化广义Voronoi图的机器人自主探索方法
CN111427047B (zh) * 2020-03-30 2023-05-05 哈尔滨工程大学 一种大场景下自主移动机器人slam方法
CN111583136B (zh) * 2020-04-25 2023-05-23 华南理工大学 一种救援场景下自主移动平台同时定位与建图方法
CN111638526B (zh) * 2020-05-20 2022-08-26 电子科技大学 一种陌生环境下机器人自主建图的方法

Also Published As

Publication number Publication date
CN114596360A (zh) 2022-06-07

Similar Documents

Publication Publication Date Title
CN114596360B (zh) 一种基于图拓扑的双阶段主动即时定位与建图算法
Lacaze et al. Path planning for autonomous vehicles driving over rough terrain
Jian et al. Putn: A plane-fitting based uneven terrain navigation framework
Ström et al. Predictive exploration considering previously mapped environments
Schmid et al. A unified approach for autonomous volumetric exploration of large scale environments under severe odometry drift
Zhang et al. Fast active aerial exploration for traversable path finding of ground robots in unknown environments
Ab Wahab et al. Path planning for mobile robot navigation in unknown indoor environments using hybrid PSOFS algorithm
CN111487960A (zh) 一种基于定位能力估计的移动机器人路径规划方法
CN114296474A (zh) 一种基于路径时间代价的无人机路径规划方法及系统
Balan et al. Optimal trajectory planning for multiple waypoint path planning using tabu search
Zheng et al. A hierarchical approach for mobile robot exploration in pedestrian crowd
Zhang et al. Dual-Layer path planning with pose SLAM for autonomous exploration in GPS-denied environments
CN112857370A (zh) 一种基于时序信息建模的机器人无地图导航方法
Soni et al. Multi-robot unknown area exploration using frontier trees
Wang et al. Virtual maps for autonomous exploration with pose SLAM
Collins et al. Efficient planning for high-speed mav flight in unknown environments using online sparse topological graphs
Huang et al. A real-time fast incremental SLAM method for indoor navigation
Zhang et al. 2D map building and path planning based on LiDAR
Zhang et al. An adaptive artificial potential function approach for geometric sensing
Luo et al. Planning optimal trajectory for histogram-enabled mapping and navigation by an efficient PSO algorithm
Domınguez et al. Internal simulation for autonomous robot exploration of lava tubes
CN111912411A (zh) 一种机器人导航定位方法、系统及存储介质
Wang et al. Localization, Planning, and Control of a UAV for Rapid Complete Coverage Bridge Inspection in Large-Scale Intermittent GPS Environments
Burguera et al. A solution for integrating map building and self localization strategies in mobile robotics
Gao et al. Near-ground trajectory planning for UAVs via multi-resolution hybrid voxel-surfel map

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant