[论文阅读]: HE-Nav: A High-Performance and Efficient Navigation...

IEEE Robotics and Automation Letters, 2024

主要创新点：该系统包括两个关键组件，第一个是部署在AGR上的轻量级 BEV 引导语义场景补全网络（LBSCNet）。通过处理稀疏点云输入，LBSCNet执行快速推理，以准确预测障碍物分布（即体素占用）和语义。然后将这些预测集成到局部地图中进行路径规划，通过[3]中提出的基于查询的低延迟地图更新方法提供便利，确保及时更新。在规划阶段开发了一个AG-Planner来搜索空中-地面混合路径。具体而言，高效的动力学A*路径搜索前端利用运动原语而不是直线作为图边，通过增加空中目的地的额外能量成本，规划器不仅倾向于搜索地面轨迹，而且只在 AGR 遇到巨大障碍物时才切换到空中模式，从而促进节能。然后我们利用[8]的距离估计方法来绕过障碍物，避免了ESDF的计算。最后，基于梯度的b样条优化器对路径进行细化，生成安全、光滑、动态可行的轨迹.

1. HE-Nav高性能： 在两种模拟场景中，HE-Nav的成功率为98%，平均移动时间最短。
2. HE-Nav高效： HE-Nav在实际测试中实现了7倍的节能，同时相对于基于 ESDF 的基线减少了6倍的规划时间。
3. LBSCNet是准确和高速的推理： LBSCNet在SemanticKITTI基准上实现了最先进的性能（IoU = 59.71），并实现了高速推理（即20.08 FPS）。

[Paper]: https://ieeexplore.ieee.org/document/10694689/metrics#metrics

[Code]: https://github.com/jmwang0117/HE-Nav

---

Abstract

现有的 AGR 导航系统通过使用 3D 语义场景完成网络进行体素占用预测并构建用于无碰撞路径规划的欧几里得符号距离场 (ESDF) 地图，在轻度遮挡场景（例如建筑物）中取得了进展。然而，由于感知网络的预测精度低和路径规划器计算开销高的限制，这些系统在严重遮挡(如密集森林或高墙壁)的杂乱环境中表现出次优性能和效率。在这封信中，我们介绍了 HE-Nav，这是第一个针对杂乱环境中运行的 AGR 量身定制的高性能和高效导航系统。感知模块利用由鸟瞰(BEV)特征融合指导的轻量级语义场景完成网络(LBSCNet)，并通过精心设计的SCBFusion模块和注意机制增强。这使得杂乱区域的实时高效的障碍物预测成为可能，生成一个完整的局部地图。基于这个完成的地图，我们的新 AG-Planner 采用节能的运动学 A* 搜索算法来保证规划是节能的。随后的轨迹优化过程产生安全、平滑、动态可行和无ESDF的空中地面混合路径。大量实验表明，HE-Nav 在现实世界的情况下实现了 7 倍的节能，同时在模拟场景中保持 98% 的规划成功率。

I. INTRODUCTION

II. RELATED WORKS

略

III. PERCEPTION MODULE OF HE-NAV

略

IV. AERIAL-GROUND MOTION PLANNER OF HE-NAV

在本节中，我们将介绍新颖的 AG-Planner。它是建立在EGO-Planner[8]的基础上，包括：1) 节能的运动学A*路径搜索前端，2) 基于梯度的轨迹优化后端，以确保无ESDF，3) 后细化过程。

A. Energy-Efficient Kinodynamic Hybrid A* Path Searching 节能动力学混合A*路径搜索

我们的AG-Planner首先创建了一个简单的“初始轨迹” $l$（如图5(a)所示），该轨迹忽视了障碍物，通过随机添加坐标点来考虑起点和目标点的位置。 随后，对于“碰撞轨迹段”（即障碍物内部的轨迹），我们的规划器后端基于[23]提出了一种节能的运动学A*路径搜索算法，以建立一条安全的“引导轨迹段”$\tau$，该算法在搜索循环中使用运动原语而不是直线作为图的边。在这个算法中，我们为运动原语增加了额外的飞行费用。因此，路径搜索不仅倾向于规划地面轨迹，而且只有当AGRs遇到巨大的障碍物时，才切换到空中模式并飞越障碍物，从而促进了节能。

B. Gradient-Based B-Spline Trajectory Optimization 基于梯度的 B 样条轨迹优化

B 样条轨迹公式： 在轨迹优化中（如图5(b)所示），轨迹由一条统一的B样条曲线$\Theta$参数化，该曲线由其度数 $p_b$、$N_c$ 个控制点 { Q 1 , Q 2 , Q 3 , . . . , Q N c } \{Q_1, Q_2, Q_3, ... ,Q_{N_c}\} {Q1​,Q2​,Q3​,...,QNc​​}和一个结点向量 { t 1 , t 2 , t 3 , . . . t M − 1 , t M } \{t_1,t_2, t_3,...t_{M−1},t_M\} {t1​,t2​,t3​,...tM−1​,tM​}唯一决定，其中$Q_i\in {\mathbb{R}}^3$，$t_m\in \mathbb{R}$，$M=N+p_b$。根据[24]的矩阵表示，B 样条的值可以计算为：

$$\Theta (u)=[1,u,\dots ,u^p]\cdot M_{p_b+1}\cdot {\left[Q_{i-p_b},Q_{i-p_b+1},\dots ,Q_i\right]}^T\text{\hspace{1em}(5)}$$

其中 $M_{p_b+1}$是一个仅依赖于$p_b$的常数矩阵，且$u=\frac{(t-t_i)}{(t_{i+1}-t_i)}$，$t\in [t_i,t_{i+1}]$。

在地面模式下，我们假设AGR正在平坦的地面上行驶，因此可以省略垂直运动，只需考虑二维水平面上的控制点，表示为 Q g r o u n d = { Q t 0 , Q t 1 , … , Q t M } Q_{ground} = \{ Q_{t0}, Q_{t1}, \dots, Q_{tM} \} Qground​={Qt0​,Qt1​,…,QtM​} ，$Q_{ti}=(x_{ti},y_{ti}),i\in [0,M]$ 在空中模式下，控制点表示为$Q_{aerial}$。根据B样条的性质：B样条的第$k$阶导数仍然是具有阶数$p_{b,k}$的B样条，其中$p_{b,k}=p_b-k$，由于$\Delta t$在所有$\Theta$中相同，控制点的速度$V_i$、加速度$A_i$和撞击加速度 $J_i$曲线通过以下公式得到：

$$V_i=\frac{Q_{i+1}-Q_i}{\Delta t},A_i=\frac{V_{i+1}-V_i}{\Delta t},J_i=\frac{A_{i+1}-A_i}{\Delta t}\text{\hspace{1em}(6)}$$

碰撞避免力估算： 与基于ESDF的方法[2], [3]不同，对于碰撞轨迹段上的每个控制点，从$l$到$\tau$生成向量$v$（即从障碍物内部指向外部的安全方向）。并在障碍物表面定义$p$，如图5(a)所示。通过生成的 { p , v } \{ p, v \} {p,v}对，规划器最大化$D_{ij}$并返回优化后的轨迹。第$i$个控制点$Q_i$到第$j$个障碍物的距离$D_{ij}$被定义为：

$$D_{ij}=(Q_i-p_{ij})\times v_{ij}\text{\hspace{1em}(7)}$$

由于引导路径$\tau$是节能的，因此生成的路径也是节能的。

B样条轨迹优化和后细化过程： B样条路径的基本要求有三个方面：平滑度、安全性和动力学可行性。基于AGR 双模的特殊性质，我们首先采用Zhou等人[8]设计的以下成本项：

$$\min J_1={\lambda }_s{J}_s+{\lambda }_c{J}_c+{\lambda }_f\left(J_v+J_a+J_j\right)\text{\hspace{1em}(8)}$$

其中$J_s$是光滑度惩罚，$J_c$是碰撞惩罚，$J_v,J_a,J_j$是限制速度、加速度和 Jerk 的动态可行性代价。${\lambda }_s,{\lambda }_c,{\lambda }_f$是每个代价项的权重。详细的解释可以在[8]中找到。随后，根据我们的观察，AGR 在地面行驶时面临非完整约束，这意味着 AGR 的地面速度向量必须与其偏航角对齐。此外，AGR 需要处理由于在急转弯期间最小化跟踪误差而产生的曲率限制。因此，需要添加曲率代价，$J_n$可以表示为：

$$J_n=\sum _{i=1}^{M-1}{F}_n(Q_{ti})\text{\hspace{1em}(9)}$$

其中$F_n(Q_{ti})$是一个可微代价函数，$C_{\text{max}}$指定曲率阈值：

$$F_n(Q_{ti})=\{\begin{array}{ll}(C_i-C_{\text{max}}{)}^2,& C_i>C_{\text{max}}\\ 0,& C_i\le C_{\text{max}}\end{array}\text{\hspace{1em}(10)}$$

其中，$C_i=\frac{\Delta {\beta }_i}{\Delta Q_{ti}}$是$Q_{ti}$处的曲率，且$\Delta {\beta }_i=\left|{\tan }^{-1}\frac{\Delta y_{ti+1}}{\Delta x_{ti+1}}-{\tan }^{-1}\frac{\Delta y_{ti}}{\Delta x_{ti}}\right|$，一般来说，总体目标函数公式如下：

$$\min J_{\text{all}}={\lambda }_s{J}_s+{\lambda }_c{J}_c+{\lambda }_f\left(J_v+J_a+J_j\right)+{\lambda }_n{J}_n\text{\hspace{1em}(11)}$$

优化问题使用非线性优化求解器 NLopt [25] 解决，使用[8]中的后细化进行约束违反。在路径规划之后，从轨迹中选择一组点并将其发送到控制器。此外，当下一个控制点的 $z$ 轴坐标大于地面阈值时，即当需要模式切换时，将向控制器发送额外的触发信号（即 PX4 Autopilot）。控制器将自动切换到飞行状态。

V. EVALUATION

A. Evaluation Setup

模拟实验： 模拟环境包括一个密集杂乱的20 m × 20 m × 5 m正方形房间，有80个墙壁和20个环，以及一个类似的杂乱的3 m ×30 m × 5 m走廊，包含60个墙壁和10个环，它们都呈现出明显闭塞的空间，如图9所示。与我们之前的工作AGRNav[3]相比，这些场景包含了40%更多的障碍。在这些模拟中，AGR 的任务是在没有碰撞的情况下从起点导航到指定目的地。

B. LBSCNet Comparison Against the State-of-The-Art.

略

C. Simulated Air-Ground Robot Navigation

在方形房间和走廊场景中（图 9），通过 100 次具有不同障碍物放置试验，我们评估了每种方法的平均移动时间、规划时间和成功率（即无碰撞）（图 8）。我们还利用来自表IV的数据，并在模拟中记录飞行和驾驶持续时间(表V)，以计算能耗。

图8展示了我们的HE-Nav系统的卓越性能，在方形房间和走廊中实现了98%的成功率，平均移动时间为12.2秒和16.2秒。由于消除了冗余的ESDF计算，我们的系统显著加快了规划时间，比TABV[2]和 AGRNav[3]快6倍。HE-Nav有效地利用了不含 ESDF 的AG-Planner，解决了TABV在遮挡区域缺乏障碍物感知和AGRNav 障碍物预测精度较低的局限性。利用LBSCNet的精确障碍物预测（图9(b)），结合节能的动力学A*算法，我们的系统实现了最低的平均能耗7447.4 J和5973.7 J。AG-Planner还实现了激进和保守之间的模式切换平衡（如图9(A)的最优着陆位置），进一步提高了节能效果。

消融实验： 在我们的消融实验（表六）中，我们进行了100次试验，每个试验包含20个环和80个墙，每次试验中设置不同的障碍物，以验证HE-Nav组件在杂乱闭塞环境中的有效性。AG-Planner单独实现了95%的成功率。LBSCNet的加入将成功率提高了3%，而SCONet的加入仅提高了1%。这种差异源于SCONet的补全精度比LBSCNet低3.59%，使得它在处理复杂遮挡时效率较低。此外，AG-Planner减少了87%的计算冗余，实现毫秒级的路径规划。

VI. CONCLUSION

我们提出了HE-Nav，这是第一个专门为空中-地面机器人(AGRs)设计的高性能、高效和无ESDF的导航系统。通过整合轻量级BEV引导的语义场景完成网络(LBSCNet)和空中-地面运动规划器(AG-planner)等关键组件，我们的系统能够预测遮挡区域的障碍物分布，实时(≈1 ms)生成低碰撞风险、节能轨迹。通过广泛的模拟和实际实验，HE-Nav 已被证明在性能和效率方面显著优于最近的 AGR 导航系统和多旋翼路径规划器。