一种用于无人地面车辆（UGV）导航的新型分层拓扑-度量道路图（HTMRG）构建方法 周帅、 徐晓苏、 张涛、 李诺

《Drones》：A Novel Hierarchical Topology-Metric Road Graph (HTMRG) Construction for UGV Navigation Shuai Zhou, Xiaosu Xu, Tao Zhang and Nuo Li

　　**亮点**

**主要发现是什么？**
1. 提出了一种基于网格地图的新型HTMRG（Hierarchical Topology-Metric Road Graph）构建方法。通过骨架简化、冗余枢纽节点合并、交叉区域建模以及生成平滑走廊（SCE）和交叉边缘（SIE），该方法能够创建出结构紧凑、全局可导航且几何连续的道路网络。
2. 基于可见性的起点-终点插入策略在仿真、Gazebo测试和真实世界实验中显著缩短了规划路径长度，提高了整体路径规划效率。

**主要发现的意义是什么？**
HTMRG能够快速计算出几何连续且可直接追踪的路径，与GVD（Grid-Based Distance Graph）、HTG（Hierarchical Topology Graph）和基于网格的方法相比，具有更好的平滑性和执行稳定性。
该框架在结构化和非结构化环境中均表现出强大的性能，并通过仿真和真实世界测试得到了验证，支持在实用的无人地面车辆（UGV）导航任务中的可靠部署。

**摘要**
在复杂环境中的自主导航需要高效可靠的道路网络表示以实现快速路径规划。然而，传统的基于网格和骨架的方法通常计算成本高且路径质量有限。本文提出了一种用于无人地面车辆（UGV）自主导航的层次拓扑度量道路图（HTMRG）框架。该方法通过识别关键交叉结构并生成平滑走廊和交叉连接来自动构建层次道路图。此外，还开发了一种专用的起点-终点插入策略，以在以前未探索的场景中实现高效的基于图的路径规划。广泛的仿真和真实世界实验表明，所提出的方法可以自动构建层次道路图并生成平滑、高质量的路径，同时提高了规划效率和鲁棒性。HTMRG框架已成功集成到UGV系统中，验证了其在实际导航场景中的有效性和实用性。

**1. 引言**
在复杂和大规模环境中的自主导航是无人地面车辆（UGV）的基本能力，应用于自动引导车辆（AGV）运输[1]、无接触配送[2]和自动驾驶[3]等领域。高效的全局路径规划严重依赖于紧凑且信息丰富的机器人自由空间表示，这需要平衡计算效率、路径质量和安全性。
道路图（RG）通过将可导航的自由空间抽象为图结构来提供有前景的表示方式，从而支持快速搜索和决策制定。然而，在同时确保计算效率、几何精度和路径平滑性的道路上构建RG仍然是一个挑战，尤其是在包含结构化和非结构化区域的环境中。在自主机器人技术中，全局路径规划（GPP）通常是运动规划和控制的重要输入。尽管其重要性不言而喻，现有的RG建模方法仍难以在不同环境中平衡图的紧凑性、规划效率和轨迹平滑性。
文献中探索了多种导航地图建模方法。当前主流的导航地图建模方法包括网格地图、几何地图、拓扑地图、路线地图等。网格地图是最常见的地图表示类型，但其精度受到单元格分辨率的限制[4]。几何地图使用形状和坐标来描述环境的自由空间，提供详细的空间信息[5]。路径可以通过空间采样、数值优化方法或最近的学习型和数据驱动型[6]规划方法来规划。然而，基于采样的GPP方法受采样步长的影响[7]，而数值优化方法计算复杂且容易陷入局部最优解。
可见性图（VG）方法假设机器人被表示为一个点，障碍物被近似为多边形[8]。它通过可见线段连接起点、终点和多边形的顶点。这些线段不能与障碍物的多边形相交[9]。然后基于这些点和边形成可见性图进行路径规划。由此产生的路径往往曲折不平，因为它遵循连接障碍物顶点的线段。这使得它容易与障碍物发生碰撞，并且缺乏灵活性，因此直接用于路径引导具有挑战性。因此，需要平滑技术来在后续应用中细化轨迹[10]。此外，环境障碍物数量的增加和更丰富的特征信息会导致搜索时间延长。
概率路线图（PRM）方法通过采样和碰撞检测来创建无向图，以确定自由空间中采样点之间的连通性。然后可以通过图搜索获得可行路径。这种算法的缺点是其在复杂环境中的适用性有限且完整性较低。具体来说，如果参数设置不正确，甚至可能无法发现现有的可行路径。此外，规划路径往往不平坦，难以直接用于跟踪和引导[11]。
高清地图，如Lanelet2、OpenDRIVE和Apollo[12]在自动驾驶应用中广泛使用。Silva[13]利用Clothoids构建了用于自主导航的稀疏道路网络模型。Mi[14]提出了一种从点云中提取和矢量化3D道路边界的方法。Zhang[15]从移动激光扫描点云中重建了3D高速公路曲线。然而，这些方法仅限于规则的结构化城市高速公路场景，并且需要大量的人工努力进行进一步建模。
拓扑度量RG在自由空间中建立了具有度量属性的路线模型，以支持机器人路径规划和导航。Tae使用细化算法从局部网格地图构建了拓扑图（TG）[16]。Stenzel[17]为AGV构建了拓扑图（TG），使路径查找相对容易和快速。然而，缺乏度量信息导致规划路线的精度较低，不适合直接用于机器人跟踪和引导。在[18,19]中，使用基于中心轴拓扑的路线方法提高了PRM构建RG的效率。然而，这些方法仍然是基于采样的。所得到的路线图具有概率完整性，但不适合直接用于机器人跟踪。在[20]中提出了基于样条的拓扑生成方法用于路径规划。尽管考虑了运动学约束，但验证仅在结构化环境仿真中进行。
在[21,22]中，提出了一种用于机器人导航规划的显式走廊拓扑图。通过显著减少搜索空间，可以根据需要从TG快速获得骨架路径，实现轻量级计算。然而，这种方法主要应用于结构化的室内环境，并且仅区分骨架点和拓扑点，而没有进一步建模枢纽交叉点。尽管这提高了搜索速度，但平滑跟踪规划路径可能具有挑战性。
本研究从现实世界中的结构化交通道路网络中汲取灵感，开发了一种快速、平滑、安全且相对较短的GPP方法，可用于大规模结构化和非结构化环境中的轮式或履带式UGV，例如Husky类无人地面车辆和类似的移动机器人平台。本研究的主要贡献如下：
- 通过简化和合并冗余节点来减小TG的大小并识别地图交叉点。
- 通过设计平滑走廊边缘和交叉边缘来构建HTMRG，以生成可导航的道路路线。
- 改进HTMRG中起点和终点节点的插入过程，以提高GPP性能。
- 在各种环境中进行广泛的仿真和真实世界实验，并将所提出的方法集成到UGV自主导航系统中。

本文的其余部分组织如下。第2节介绍了HTMRG的整体工作流程。第3节详细介绍了HTMRG的构建过程。第4节描述了其在路径规划中的应用。第5节报告了仿真结果，而第6节展示了真实世界实验结果。最后，第7节总结了本文并概述了未来的工作。

**2. HTMRG的工作流程**
本章介绍了HTMRG方法的工作流程，并定义了相关组件，为后续章节奠定了基础。如图1所示，HTMRG框架可以分为四个主要过程：传感器数据和感知、预处理、简化和TG提取、HTMRG生成和应用。

**2.1. 传感器数据和感知**
所提出的HTMRG方法以基于网格的2D地图?? ∈???×??作为输入，其中I和J分别表示网格地图的行数和列数。每个单元格是二进制的：0表示自由空间，1表示占用。无人载体平台通常通过集成多种类型的传感器（例如GPS、LiDAR和摄像头）来执行环境数据采集和基于感知的映射。如图1c所示，可以使用SLAM技术构建全局云图[23]，然后将其投影到网格地图中。此外，如图1d所示，可以将图像分割算法应用于无人机航空图像来提取障碍物[24]，从而生成划定可通行区域的网格地图。

**2.2. 预处理**
在此阶段，首先对输入的网格地图进行过滤[25]以去除噪声，然后进行骨架化以生成骨架图（SG）。骨架是地图的形态学表示，与最近的障碍物点等距定位。可以使用各种方法生成骨架，例如Voronoi图、图像细化和平行场变换[22]。图1f显示了过滤后的网格地图，而图1g显示了通过平行场变换得到的骨架图。然后，使用8-邻域搜索方法在骨架节点之间建立邻居关系。结果，SG表示为???? ={????,????}，其中???? ={??????∣??=1,…,??????}，???? ={?????????=(??????,??????)|??????=???????????????????????(??????)}。这里，??????是第i个骨架节点，??????表示骨架节点的总数。函数???????????????????????()表示邻居关系的形成。后续描述的方法依赖于平行场图和SG。

**2.3. 简化和拓扑图提取**
从预处理中得到的SG（????）用于此过程中的建模，得到简化拓扑骨架图（STSG）。相关元素定义如下：
- **枢纽节点（HNs）**：度数为3或更高的骨架节点，表示为????????? ={???????????∣???????????∈????,???(???????????)>=3}，其中???(???????????)是节点???????????的度数，如图1h中的黑色五角星标记所示。
- **终端节点（ENs）**：度数为1的骨架节点，表示为?????????? ={????????????∣????????????∈????,???(????????????)=1}，如图1h中的绿色菱形标记所示。
- **初始拓扑节点（ITNs）**：包括HNs和ENs，表示为????0 ={????0??∣????0??∈?????????∪??????????}。
- **初始拓扑边（ITEs）**：表示相邻ITNs之间的连通性，表示为????0 ={????0?????=(????0??,????0??)|????0??=???????????????????????(????0??)}。因此，初始拓扑图（ITG）可以表示为????0 ={????0,????0}。
- **初始拓扑骨架边（ITSEs）**：连接相邻ITNs的骨架路径，表示为???????????????0 ={??????????????????=????????????(????0??,????0??)|????0??=???????????????????????(????0??)}，其中????????????(????0??,????0??)表示????0??和????0??之间的骨架路径。因此，初始拓扑骨架图（ITSG）定义为???????0 ={????0,????0}。
- **交叉节点（INs）**：根据预定义规则合并HNs得到的节点，表示为???????????????? ={??????????????????∣??=1,…,??????????????????}，如图1i中的红色五角星标记所示。
- **简化拓扑节点（STNs）**：包括INs和ENs的节点，表示为???? ={??????∣??????∈????????????????∪??????????}。
- **简化拓扑图（STG）**：???? ={????,????}。????仅表示拓扑节点之间的相邻关系。
- **简化拓扑骨架图（STSG）**：??????? ={????,???????????????}。???????????????包含相邻拓扑节点之间的所有骨架节点。

**2.4. HTMRG生成和应用**
在此阶段，STSG（??????????）将进一步被建模为走廊和交叉区域，从而形成便于路径规划和跟踪的平滑道路网络。相关元素定义如下：
- **交叉节点（JNs）**：定义为圆形安全区域与骨架之间的交点，其中圆心对应于交叉节点（IN），半径由安全距离阈值确定。JNs集合表示为?????????? ={????????????→??∣????,????∈????,????=???????????????????????(????)}，如图1k中的粉色三角形标记所示。为简单起见，该集合重新定义为?????????? ={????????????∣??=1,2,…,????????????}，其中????????????表示JNs的总数。
- **关键节点（KNs）**：包括JNs和ENs的节点，表示为?????????? ={????????????∣????????????∈??????????∪??????????}。
- **平滑走廊边（SCEs）**：连接同一交叉区域内相邻JNs的平滑路径，如图1l中的绿色虚线所示。这些边表示为?????????????。
- **平滑交叉边（SIEs）**：同一交叉区域内相邻JNs之间的平滑路径，如图1l中的绿色虚线所示，表示为??????????。
- **HTMRG**：所提出的HTMRG定义为?????????? ={??????????,?????????????∪??int,??}，其中W表示边权重，通常由路径长度表示。
最后，使用适当的插入策略（如图1m所示），可以将起点和终点节点纳入HTMRG中，从而实现高效安全的路径规划。HTMRG的构建
本节详细介绍了HTMRG的构建过程，包括通过合并HNs生成STSG的方法，以及SCEs和SIEs的构建方法。

3.1. 简化与拓扑图提取的细节
如图1h所示，HNs可能在同一交叉区域内紧密相邻且重复，这可能会影响后续的RG建模过程。为了识别INs并构建STG，首先确定相邻的HN对。然后通过合并同一区域内的相邻HNs来生成新的INs，同时保持TG中的拓扑关系。最后，移除原始的相邻HN对。
距离场用于计算v?????????的安全阈值半径rA（如图2中的实线箭头所示）和相应的安全圆（如图2中的虚线圆所示），定义为每个枢纽节点与其最近障碍物之间的距离。当两个相邻的HN位于彼此的安全阈值半径内时，它们被归类为相邻的HN，表示为：
v?????????=???????????????(v?????????)
if(∥v??????????v?????????∥< />????+??????*???????*????????+????
otherwise （3）
其中??????? =∥?????????????????? ??????????????????? ∥，?? ∈????????????????????(??)，并且??控制相邻INs的规模，可以在[0.6,0.9]范围内启发式设置。在本文的后续实验中，??被设置为0.8。图4展示了与图3对应的JN识别结果。
图3. JNs的识别。 (a) 情况1：相邻的INs相距足够远，连接半径等于每个IN的安全半径。 (b) 情况2：相邻的INs靠近，它们的安全区域重叠。为了确保有效的JNs，连接半径被定义为两个IN之间距离的加权函数。线条、箭头、三角形和星号的含义与图1中的相同。
图4. JN识别的示例。 (a) 情况1：两个IN的距离圆不相交的简单环境，对应于图3a。 (b) 情况2：两个相邻的距离圆重叠，如蓝色虚线矩形所示，对应于图3b。符号和图例的含义与图1中的相同。随后，构建集合?????????? ={????????????∣??=1,2,…,????????????} 和 ?????????? ={????????????∣????????????∈??????????∪??????????}。然后可以直接获得属于不同交叉点的相邻KNs之间的走廊区域内的骨架路径，表示为????????????(????????????,????????????)。

算法1：STSG生成
输入：???????0, 距离场地图 ??????????
输出：?????????????1：
function GenSTSG (???????0, ??????????)
2:
GenInterNodes (???????0, ??????????)
3:
???????????????? =?????????
4:
???? ={??????∣??????∈????????????????∪??????????}
5:
????????????? ={????,???????????????}
6:
return ?????????????
7:
function GenInterNodes (???????0, ??????????)
8:
? =???????????????(???????0)
9:
?? =?
10:
while ? ≠? do
11:
for ??????????? ∈? do
12:
if ????????????(???????????????(???????????)) =1&&??????????? ??? then
13:
????????????? =???????????????(???????????)
14:
if ????????????(???????????????(?????????????)) >1 then
15:
从?close?(?????????????)中移除???????????
16:
将?close?(?????????????)推入??
17:
end if
18:
?????????_?????????? =???????????????(???????????,?????????????)
19:
℃???????? ??????????_???????????error??????????
20:
????????????(?????????_??????????,????????????????????(?????????_??????????)) →???????0
21:
??????????????????(???????????,?????????????) 从???????0中取出
22:
??????????????????(???????????,?????????????) 从?中取出
23:
end if
24:
end for
25:
? =???????????????(???????0)
26:
?? =?
27:
end while

3.2.2. 基于约束优化的B样条曲线
从道路图中提取的走廊骨架路径????????????(????????????,????????????)由在离散节点处连接的分段线性段组成，如图5a中的蓝色路径所示。这样的路径不适合机器人跟踪，因为它们包含尖锐的角、不连续的曲率，并可能违反车辆的运动学约束。因此，需要一个轨迹平滑和优化步骤将离散的骨架路径转换为连续且动态可行的轨迹。本节提出了一种使用B样条插值生成SCEs的方法，在保证曲率和安全约束的同时优化平滑度。
图5. 走廊边的平滑。 (a) 平滑前的走廊边。 (b) 平滑后的走廊边。B样条曲线可以在不改变整体形状的情况下局部调整路径，非常适合路径平滑。给定n + 1个控制点，这些控制点定义了样条曲线的方向和边界范围，k阶B样条曲线由以下定义给出：
℃?(??)=[??0,??1,??2…,????]∈[??0,???(??)????,???(??)]=??∑??=0?????????,???(??) （4）
其中????,???(??)是第k阶B样条基函数，???(??)对应于控制点，u是变量。基函数具有以下导出公式：
????,0?(??)={1????≤??≤????+1
else
(5)
????,???(??)=???????????+????????????,???1?(??)+????+??+1?????+??+1?????+1?????+1,???1?(??) （6）
为了平衡路径平滑度和计算效率，采用了三阶准均匀B样条（?? =3）。三次B样条保证了生成轨迹的C2连续性，确保移动机器人的连续曲率和平滑转向。低阶样条无法保证曲率连续性，而高阶样条会引入不必要的计算复杂性并可能导致过拟合。因此，三次B样条在平滑度和效率之间提供了实际的折中。在沿同一边上关键节点（KNs）之间的骨架路径选择控制点时，在平滑区域采用稀疏采样，而在高曲率区域采用密集采样。这种策略确保平滑路径紧密跟随骨架的整体趋势。
设???? ={?????(????,????),??∈[1,??]}表示要通过B样条插值生成的路径。目标是最小化一个成本函数，该函数包含与路径平滑度、曲率约束和安全惩罚相关的项。目标函数定义为：
J=??1·???1∑??=1(Δ?????+1?Δ?????)2+??2·???1∑??=1?????(Δ?????∣Δ?????∣???????)+??3·??∑??=1???????????(∣?????????∣???max) （7）
第一项通过最小化曲率来确保路径平滑度，曲率由轨迹的二阶导数表示。第二项对每个节点处的瞬时曲率施加上限，确保符合车辆的非完整约束。Δ????? =∣tan?1?Δ?????+1Δ?????+1 ?tan?1?Δ?????Δ?????∣是顶点处的切线角度变化。这里，????????是路径的最大允许曲率，????是二次惩罚项。第三项惩罚与障碍物的潜在碰撞，其中??????????是二次惩罚项，????表示最接近顶点的障碍物的位置，??max是障碍物的最大有效距离阈值。??1, ??2, ??3是权重。在后续实验中，我们使用差分驱动机器人（最小转弯半径为零）并设置??max =??robot/??map +2，其中??robot表示机器人的物理半径，??map表示网格地图的分辨率。同时，我们经验性地设置??1 =100, ??2 =0, ??3 =50，并在所有场景中保持不变。最后，可以使用梯度下降算法来解决这个问题并获得平滑的道路段。图5展示了在局部地图中优化前后的走廊边对比，优化过程显著改善了相邻KNs之间的骨架路径。结果，构建了一组SCEs，?????????? ={????????????=??????(????????????,????????????)|????????????=????????????????????(????????????)}，其中????????????, ????????????位于不同的交叉点，??????(????????????,????????????)表示所提出的B样条平滑算法。

3.3. 平滑交叉边的构建
在本节中，在同一交叉点内的相邻JNs之间构建SIEs，确保JNs处的切线连续性，并保持交叉区域内的合理性和安全性。为了解决??1 Hermite插值问题，采用了基于Clothoid的方法，确保沿曲线的方向变化逐渐过渡，便于机器人跟踪和控制。
假设曲线的初始坐标为(??0,??0)，可以根据Fresnel积分函数通过以下方程定义Clothoid：
?{ { { { { {????(??)=??0+∫??0cos?(??02???2+??1???+??0)d?????(??)=??0+∫??0sin?(??02???2+??1???+??0)d?? （8）
其中s是弧长参数，??0是控制曲率变化率的参数，??1是控制初始曲率的参数，??0是偏移常数。
曲线上任何点的曲率和切线角度可以表示为：
?{ { {????(??)=??0???+??1???(??)=??02???2+??1???+??0 （9）
基于卷积曲线[26]的??1连续性拟合确保了JNs处的切线连续性，提供了无缝的几何连接并保持了设计的整体平滑性。
因此，SIEs的集合构建为?????????? ={????????????=??????(????????????,????????????)∣????????????=????????????????????(????????????)}，其中????????????, ????????????是位于同一交叉点内的连接节点，??????(????????????,????????????)表示所提出的基于Clothoid的平滑操作符。作为示例，图6a显示了同一交叉点内相邻JNs之间的SIEs。每个SIE的起点和终点与相应的JNs重合，初始方向由平滑走廊边的切线向量确定。
图6. 交叉点建模。符号和图例的含义与图1中的相同。 (a) 交叉点内JNs之间构建的基于Clothoid的SIEs。 (b) 通过蒙特卡洛采样显示生成的SIEs保持在交叉边界内。为了验证无碰撞运动，使用蒙特卡洛采样来证明所提出的SIEs不会超出交叉圆的边界。具体来说，生成一个中心为(0,0)半径为5米的圆。起点(0, ?10)，终点在圆上的任意离散点，方向为??,?? ±5°,…,?? ±40°，其中a是从圆心到终点的方向。经过拟合和平滑后，图6b清楚地显示曲线不会穿过任何障碍物，因为它保持在圆内。最后，构建了HTMRG，表示为?????????? ={??????????,??????????∪??????????,??} ={??????????,??????????,??}，其中W表示相邻KNs之间的距离成本。

4. HTMRG的应用
要在HTMRG上执行路径规划，必须在图搜索之前将起点和终点节点插入图中。典型的方法识别最接近起点和终点位置的骨架点，将它们连接到图中，然后进行搜索[22]。然而，如图7a中的Path2和Path3所示，这种方法可能导致次优路径。相比之下，图7b显示了使用所提出的插入策略获得的结果。图7. 节点插入策略的比较。 (a) 基于最近邻点的传统插入方法。 (b) 基于可见连接的提出的插入方法，能够实现更可靠的路径连接。因此，我们提出了一种策略，通过利用新插入节点与KNs之间的可见性（视线）关系将起点和终点节点插入HTMRG。以插入起点节点为例，新节点插入后的图表示为?????????????，插入目标节点的过程类似。算法2：将节点插入HTMRG
输入：起始节点????，??????????，障碍物Φ
输出：?????????????
1: ????????????? = ??????????
2: 如果 ???? ∈ ??????????，则
? 如果起始节点位于关键节点上，不进行任何操作。
3: 否则
4: 如果 ???? ∈ ??????????，则
? 如果起始节点位于道路上。
5: ??????????(????) → ?????????????
6: ?????????(????,???????????????????????(????)) → ?????????????
7: 否则
? 起始节点位于道路之外。
8: ?????????? = {???????????? | ???????????? ∈ ?????????? ∧ ???????????? ∥ < ?? ∧ ???????????? ∩ Φ = ?}
9: ????????(????) → ?????????????
10: 对于每个 ???????????? ∈ ??????????
11: ?????????(????,????????????) → ?????????????
12: 结束循环
13: ???? = ?????????????????2(????⊥??????????)
14: ????????(????) → ?????????????
15: ?????????(????,???????????????????????(????)) → ?????????????
16: 结束if循环
17: 结束if循环
18: 返回 ?????????????

在算法2中，???????????? 表示从要插入的节点可见的节点。条件 ???????????? ∩ Φ = ? 表示要插入的节点与可见节点之间的线不与任何障碍物相交。约束 ???????????? ∥ < ?? 限制候选可见节点在距离要插入的节点 ?? 的范围内。在后续实验中，?? 被设置为地图对角线长度的一半。此外，???? = ?????????????????2(????⊥??????????) 表示从要插入的节点到RG的最接近点。需要注意的是，在将起始节点或目标节点插入RG时，不考虑新插入段的平滑性，因为这些段通常较短，可以通过机器人系统的局部规划来处理。

在插入了起始节点和目标节点之后，得到了增强图 ?????????????3。最后，使用Dijkstra算法在 ?????????????3 中搜索起始节点和目标节点之间的路径，同时考虑KNs之间的路径长度。图1m展示了使用所提出的插入方法进行的路径规划结果，证明了该方法的有效性、安全性和平滑性。

5. 仿真实验
5.1. MATLAB中的仿真概述
使用了四张网格地图作为仿真地图（图8），每张地图包含以下特定信息：
图8. 用于实验的仿真地图。(a) 地图1：结构化环境。(b) 地图2：室内环境。(c) 地图3：混乱环境。(d) 地图4：块状环境。
(a) 地图1：结构化场景，500 × 500（像素）。(b) 地图2：室内场景，796 × 598（像素）。(c) 地图3：混乱场景，900 × 900（像素）。(d) 地图4：块状场景，796 × 598（像素）。
5.2. 仿真结果和比较分析
5.2.1. HTMRG的结果
所提出的HTMRG与广义Voronoi图（GVD）[27]和层次拓扑图（HTG）[28]进行了比较。图9a–d展示了四种不同类型地图表示的定性比较结果。GVD的特点是节点数量多且连接粗糙不规则。虽然HTG减少了节点数量，但仍包含冗余的INs，其连接缺乏平滑性。相比之下，HTMRG消除了冗余节点，并在SCEs和SIEs之间提供了平滑的连接，实现了紧凑的结构和足够的拓扑表达能力以及高效的导航性。此外，这些结果表明所提出的方法能够处理各种应用场景，对不同环境具有很强的适应性。
图9. GVD、HTG和HTMRG的视觉比较。符号和图例的含义与图1中的相同。(a) 地图1—结构化场景。(b) 地图2—室内场景。(c) 地图3—混乱场景。(d) 地图4—块状场景。
5.2.2. 基于HTMRG的插入方法比较
为了验证所提出的插入方法在HTMRG上的有效性，在每张地图的自由空间中随机生成了50对起始-目标对。为了避免平凡情况，要求起始点和目标点之间的欧几里得距离大于地图较长维度的一半，并且起始点和目标点不可互相看见。对于每对起始-目标对，应用了传统的最短距离插入方法和所提出的插入方法，并记录了规划的路径长度。我们报告了路径长度的平均值和标准差，并进行了配对统计测试以评估改进的显著性。如表1所示，所提出的插入方法在所有地图上一致地减少了规划的路径长度。具体来说，平均路径长度分别在地图1-地图4上减少了6.99%、4.55%、4.13%和4.23%。这些改进在所有地图上都具有统计学意义，p值范围从1.03 ×10?3到4.89 ×10?11。这些结果表明，所提出的插入策略通过考虑所有可行的插入场景，能够有效地生成更短、更合理的路径。图10提供了定性比较，其中所提出方法生成的红色路径比传统方法生成的绿色路径更直观、更短。
表1. 不同插入方法下的平均路径长度比较（像素）。
图10. HTMRG中起始-目标插入方法的视觉比较。绿色路径对应于传统的最短距离插入方法，而红色路径对应于所提出的插入方法。(a) 地图1。(b) 地图2。(c) 地图3。(d) 地图4。
5.2.3. 不同图上的路径规划比较
为了比较不同图表示上的路径规划性能，在每张地图上随机选择了50个起始节点和50个目标节点。为了避免平凡情况，要求起始点和目标点之间的欧几里得距离大于地图较长维度的一半，并且起始点和目标点不可互相看见。对于每对节点，使用Dijkstra算法在Grid、GVD、HTG和HTMRG图上规划了路径。表2报告了50次试验的平均规划时间和标准差。
如表2所示，HTG和HTMRG与Grid和GVD表示相比显著减少了规划时间。例如，在地图3上，平均规划时间从Grid的7810 ±234 ms和GVD的580 ±43 ms减少到HTG的14.1 ±4.1 ms和HTMRG的17.8 ±4.7 ms。这种显著减少归因于层次拓扑结构，它有效地减少了搜索空间并加速了图遍历。与HTG相比，HTMRG在所有地图上的规划时间略有增加，但仍在同一数量级（相差几毫秒），表明丰富的层次拓扑表示仅引入了轻微的计算开销，同时保持了高效的搜索性能。
表2. 不同图表示上的平均路径规划时间比较（毫秒）。此外，还进行了平均路径长度、曲率和安全间隙的定量比较，以进一步评估所提出算法的性能。每条路径的平均离散曲率定义为：
?? = ∑????????????(????????)?1 / (2???????????????(?????1,????,????+1) × ????????????(????????)) (10)
其中 ????????????(????????) 表示路径点的总数，???????????????(?????1,????,????+1) 表示基于三点圆的点????的曲率计算。
平均安全间隙定义为：
?? = ∑????????????(????????) / ???(????) (11)
其中 ???(????) 表示从????到周围障碍物的最近欧几里得距离，可以直接从距离场图中获得。
使用所提出的HTMRG结构规划的路径，如图11、图12和图13所示，展示了路径长度的减少、平滑性的提高以及一致的安全水平。图14展示了四种不同地图上的代表性结果。与使用Grid、GVD和HTG方法生成的路径相比，HTMRG上规划的路径更平滑，转弯更少。
图11. 不同图表示上不同环境下的平均路径长度比较。
图12. 不同图表示上不同环境下的曲率比较。
图13. 不同图表示上不同环境下的安全比较。
图14. 使用不同图表示进行路径规划结果的视觉比较。路径是使用Dijkstra算法在Grid、GVD、HTG和HTMRG上生成的。(a) 地图1。(b) 地图2。(c) 地图3。(d) 地图4。
5.3. 在ROST中的Gazebo仿真
所提出的方法被集成到ROS框架中，并在Gazebo仿真环境中进行了评估，如图15a所示。仿真环境代表了包含稀疏和密集区域、结构化和非结构化区域以及开放和半封闭空间的复合场景。使用了Gmapping SLAM和OctoMap包生成了占用网格图。生成的网格图分辨率为每像素0.1米，大小为897 × 957像素。网格图的分辨率（粒度）直接影响构建的道路图的复杂性和准确性。实际上，网格分辨率根据机器人大小选择，通常设置为机器人半径的大约一半到四分之一，这在安全性和效率之间提供了平衡。
图15. 基于Gazebo的仿真结果。(a) Gazebo仿真环境。(b) 从生成的占用网格图构建的HTMRG。绿线代表SIEs，红线代表SCEs，红点表示INs。图15b展示了构建的HTMRG，其中红点表示INs，绿点表示SIEs，黄线表示SCEs。HTMRG结构清楚地展示了其在系统地构建功能性道路网络方面的有效性。
如图16a所示，与传统的插入方法相比，所提出的插入策略平均路径长度减少了4.02%，这是通过在Gazebo环境中评估的30对随机生成的起始-目标对得出的。此外，如图17所示，使用HTMRG进行路径规划获得了更短的路径长度、更好的平滑性和更高的安全性。图16b展示了HTG和HTMRG生成的直观路径输出比较，突出了所提出方法产生的更平滑、更短的路径。这些结果进一步验证了所提出方法在真实仿真场景中的有效性。
图16. Gazebo环境中的路径规划结果比较。(a) 不同起始-目标插入方法的比较。(b) HTG和HTMRG上的路径规划结果。
图17. 不同图表示的性能比较。
6. 实际实验
在本节中，将所提出的基于HTMRG的路径规划方法集成到UGV系统中，用于真实校园环境中的实验验证。完整系统包括定位、映射、路径规划和运动控制模块。
6.1. 实验环境和条件
开发了一个用于UGV平台的自主导航系统，如图18所示。图19b展示了实际校园环境的一部分，长度为400米，宽度为200米。图19a中显示的跟踪式差动型UGV平台配备了车载计算机（i9-11900）和Ouster-64 LiDAR。该机器人采用了跟踪式滑移转向运动机制，能够在不平坦的室外地形上稳健运行。平台的大致尺寸为1.1米 × 0.7米 × 0.5米（长度 × 宽度 × 高度），最大行驶速度为1.5米/秒。由于采用了滑移转向配置，车辆具有接近零的最小转弯半径，允许原地旋转。
车载计算机通过CAN总线与底盘控制板通信。首先，使用Lio-Sam [29]构建了全局点云图。同时，进行了分辨率为0.2米的网格投影以生成网格图。然后对全局网格图进行了噪声去除。接着，在先前的地图上进行了在线重新定位[30]以估计机器人姿态。此外，图19c展示了另一个实验场景，其中无人机拍摄了停车场的俯视图，随后用于HTMRG的构建。
图18. 基于HTMRG的UGV系统架构概述。
图19. 实际世界实验设置。(a) 配备了车载计算机和64束LiDAR的跟踪式UGV平台。(b) 校园测试场地（大约400米 × 200米）。(c) 用于俯视映射的停车场的无人机航拍视图。
6.2. 实际世界实验结果和比较分析
校园和停车场环境的HTMRG结果分别显示在图20和图21中。对于停车场场景，使用无人机俯视图像处理以提取HTMRG构建之前的可行区域，可以使用语义分割或手动注释。结果表明，所提出的方法在真实世界环境中成功构建了层次化且平滑的道路网络结构。规划的路径可以直接由控制模块用于轨迹跟踪和引导。
图20. 为真实世界校园环境构建的HTMRG表示。绿线代表SIEs，红线代表SCEs，红点表示INs。放大的视图突出了典型的交叉口和弯曲的道路段。
图21. 为真实世界停车场环境构建的HTMRG表示。符号和图例的含义与图1中的相同。将20对起点-终点对插入到HTMRG中后，结果表明与传统方法相比，所提出的插入方法将平均路径长度减少了4.36%。图22展示了一个典型的例子：所提出方法生成的路径（红色）长度为192米，而传统方法生成的路径（绿色）长度为208米。图22展示了HTMRG框架中不同起点-终点插入方法的比较。绿色路径对应于传统插入方法，红色路径对应于所提出的插入方法。此外，图23还展示了三个代表性的自主导航案例。黄色点表示激光雷达点云，用于可视化车辆在环境中的位置，而车辆姿态则是通过定位模块估计得到的。红线表示基于HTMRG的规划生成的平滑路径，绿色线表示基于HTG的规划生成的路径，后者包含更多的急转弯。红色路径是使用我们之前的研究[31]中提出的增强型纯追踪算法进行跟踪的。图24展示了50个不同起点-终点对规划案例的性能比较。在HTMRG上规划的路径比SG和HTG生成的路径更短、更平滑，且安全性方面的折中最小。图23展示了在真实校园环境中HTG和HTMRG的路径规划示例。(a) 案例1。(b) 案例2。(c) 案例3。图24展示了校园环境中不同图表示方法的性能比较。7. 结论与展望在这项研究中，我们研究了一种新颖的RG建模方法及其在自主路径规划中的应用。所提出的HTMRG框架降低了图的复杂性，识别了交叉口，并构建了SCEs和SIEs以生成高质量的道路网络表示。为了进一步提高实际应用性，引入了一种专门的起点-终点插入策略，以在HTMRG上进行高效路径规划。从定量上看，所提出的插入策略将规划路径长度减少了4.13%-6.99%，与传统插入方法相比有显著改进。此外，分层图表示显著加快了图搜索速度，比基于网格的规划快了几个数量级，比基于GVD的规划快了一个数量级。该框架还表现出高可靠性，这里的可靠性指的是在多次起点-终点试验中生成可行且无碰撞路径的成功率。尽管取得了有希望的结果，但仍存在一些局限性。当前框架假设环境是静态的，并未明确处理动态障碍物。此外，所提出的方法主要针对单机器人导航，尚未扩展到多UGV协调场景。另外，道路图是基于现有地图构建的，目前不支持完全在线地图生成和更新。未来的工作将通过集成动态障碍物避让机制和引入模型预测控制（MPC）来改进运动执行，以解决这些局限性。我们还计划将HTMRG框架扩展到多机器人协作导航，并研究在不断变化的环境中进行在线道路图构建。