FC-Planner方案实现细节与难点讲解
1. 骨架提取
骨架提取是FC-Planner的核心模块之一,其目的是从输入的点云数据中提取出场景的骨架结构。这一步的关键是如何准确高效地计算每个点的ROSA点。
1.1 ROSA点计算
ROSA点的计算涉及到两个优化问题:
-
ROSA点方向 v p v_p vp的优化:
v p v_p vp的优化目标是最小化其与邻域点法线方向的方差。这可以通过求解一个二次规划问题来实现:
v p i + 1 = arg min v p v p T Σ p i v p , s.t. ∣ ∣ v p ∣ ∣ 2 = 1 , v p ⋅ v p i > 0. \begin{aligned} v^{i+1}_p = \underset{v_p}{\arg \min}~ v^T_p \Sigma^i_p v_p, \\ \text{s.t.} ~~ ||v_p||_2 = 1,~ v_p \cdot v^i_p > 0 . \end{aligned} vpi+1=vpargmin vpTΣpivp,s.t. ∣∣vp∣∣2=1, vp⋅vpi>0.
其中, Σ p i \Sigma^i_p Σpi是邻域点法线的协方差矩阵。这个二次规划问题可以用标准的优化库高效求解。
-
ROSA点位置 x p x_p xp的优化:
x p x_p xp的优化目标是最小化其到邻域点法线延长线的距离平方和:
arg min x p ∈ R 3 ∑ p k ∈ N p ∣ ∣ ( x p − p k ) × n ( p k ) ∣ ∣ 2 2 . \underset{x_p \in \mathbb{R}^3}{\arg \min} \sum_{p_k \in N_p} ||(x_p - p_k) \times n(p_k)||^2_2. xp∈R3argminpk∈Np∑∣∣(xp−pk)×n(pk)∣∣22.
这个问题有解析解,可以直接计算得到最优的 x p x_p xp。
1.2 骨架生成
在得到所有点的ROSA点后,作者用一维移动最小二乘法将这些离散的ROSA点连接成骨架曲线。这一步的难点在于如何选择合适的参数化方法和基函数,使得生成的骨架曲线既光滑又能很好地拟合ROSA点。在实现中采用了线性参数化和多项式基函数,取得了不错的效果。
2. 视点生成
视点生成模块的任务是在骨架的引导下,生成一组数量最少但能完全覆盖场景的视点。其中的难点主要有两个:如何避免视点之间的冗余,以及如何优化视点的姿态使其覆盖更多的区域。
2.1 视点采样
为了避免冗余的视点采样,作者利用骨架提取出一组视点采样射线。这些射线起始于骨架,穿过占用的体素,延伸到自由空间。只在这些射线上采样视点,而不是在整个空间中随机采样。这种有针对性的采样策略可以显著减少冗余视点的数量。
2.2 视点姿态优化
提出了一种基于引力模型的视点姿态优化方法。其基本思想是,将视点看作带电粒子,覆盖区域多的视点带正电,覆盖区域少的视点带负电。然后让带负电的视点在带正电视点的吸引下移动,直到达到平衡状态。这个过程可以用下面的公式描述:
p q = p q + ∑ v p a ∈ V P a c a c q ∗ ( p a − p q ) , s.t. c a < c q . p_q = p_q + \sum_{vp_a \in VP_a} \frac{c_a}{c_q} * (p_a - p_q), ~~ \text{s.t.}~~ c_a < c_q. pq=pq+vpa∈VPa∑cqca∗(pa−pq), s.t. ca<cq.
其中, p q p_q pq是当前视点的位置, V P a VP_a VPa是其吸引视点集合, c a c_a ca和 c q c_q cq分别是吸引视点和当前视点的覆盖区域数量。通过这种优化,可以使每个视点的姿态都朝向覆盖区域最多的方向调整,从而减少视点总数。
3. 分层覆盖规划
分层覆盖规划是FC-Planner的另一个核心模块,其目的是在生成的视点集合上规划一条最短的覆盖路径。采用分治的策略,将整个问题分解为全局规划、局部规划和轨迹优化三个层次。
3.1 全局序列规划
在全局层,通过求解一个非对称TSP问题,得到一个遍历所有子空间的最优序列。这里的难点在于如何快速求解TSP问题。采用了启发式的LKH算法,该算法能在可接受的时间内给出近似最优解。
3.2 局部路径规划
在局部层,为每个子空间并行地规划覆盖路径。这里的关键是如何在保证覆盖完整性的同时最小化路径长度。将其表述为一个带时间窗约束的TSP问题:
min ∑ i = 1 R ∑ j = 1 R c ( v p i , v p j ) ⋅ x ( v p i , v p j ) s.t. ∑ j = 1 R x ( v p i , v p j ) = 1 , ∀ i ∈ { 1 , ... , R } , ∑ i = 1 R x ( v p i , v p j ) = 1 , ∀ j ∈ { 1 , ... , R } , x ( v p i , v p j ) ∈ { 0 , 1 } , v p 1 = v p s t a r t ( g i ) , v p R = v p e n d ( g i ) . \begin{aligned} \min &\sum_{i=1}^{R} \sum_{j=1}^{R} c_{(vp_i,vp_j)} \cdot x_{(vp_i,vp_j)} \\ \text{s.t.} & \sum_{j=1}^{R} x_{(vp_i,vp_j)} = 1, \forall i \in \{1,\dots,R\}, \\ & \sum_{i=1}^{R} x_{(vp_i,vp_j)} = 1, \forall j \in \{1,\dots,R\}, \\ & x_{(vp_i,vp_j)} \in \{0,1\}, \\ & vp_1 = vp^{(g_i)}{start}, ~ vp_R = vp^{(g_i)}{end}. \end{aligned} mins.t.i=1∑Rj=1∑Rc(vpi,vpj)⋅x(vpi,vpj)j=1∑Rx(vpi,vpj)=1,∀i∈{1,...,R},i=1∑Rx(vpi,vpj)=1,∀j∈{1,...,R},x(vpi,vpj)∈{0,1},vp1=vpstart(gi), vpR=vpend(gi).
其中, x ( v p i , v p j ) x_{(vp_i,vp_j)} x(vpi,vpj)是决策变量,表示是否从视点 v p i vp_i vpi飞到 v p j vp_j vpj,而 c ( v p i , v p j ) c_{(vp_i,vp_j)} c(vpi,vpj)是两视点间的代价。最后两个约束条件保证了路径的起点和终点分别是该子空间的入口和出口视点。
3.3 轨迹优化
得到覆盖路径后,需要进一步将其优化为一条无碰撞、动态可行的轨迹。这里利用了凸优化的方法。首先,在覆盖路径上生成一系列的安全飞行走廊(SFC)。每个SFC都是一个凸多面体,可以保证无人机在其中飞行时不会与障碍物碰撞。然后,在SFC的约束下,求解一个最小化时间的轨迹优化问题:
min T , t p ∑ i = 1 M T i s.t. t p i ( t ) ∈ C P ( i ) , ∀ t ∈ [ 0 , T i ] , ∥ t p ˙ i ( t ) ∥ 2 2 ≤ v m a x 2 , ∥ t p ¨ i ( t ) ∥ 2 2 ≤ a m a x 2 , ∥ d 3 t p i ( t ) d t 3 ∥ 2 2 ≤ j m a x 2 , t p i ( 0 ) = p R ( i − 1 ) , t p i ( T i ) = p R i . \begin{aligned} \min_{T,tp} & \sum_{i=1}^{M} T_i \\ \text{s.t.} & \quad tp_i(t) \in CP(i), \quad \forall t \in [0,T_i], \\ & \quad \| \dot{tp}i(t) \|^2_2 \leq v^2{max}, \\ & \quad \| \ddot{tp}i(t) \|^2_2 \leq a^2{max}, \\ & \quad \left\| \frac{d^3 tp_i(t)}{dt^3} \right\|^2_2 \leq j^2_{max}, \\ & \quad tp_i(0) = p^{(i-1)}_R, \quad tp_i(T_i) = p^i_R. \end{aligned} T,tpmins.t.i=1∑MTitpi(t)∈CP(i),∀t∈[0,Ti],∥tp˙i(t)∥22≤vmax2,∥tp¨i(t)∥22≤amax2, dt3d3tpi(t) 22≤jmax2,tpi(0)=pR(i−1),tpi(Ti)=pRi.
这个问题可以用凸优化的方法高效求解,得到一条平滑、安全、满足动力学约束的轨迹。
4. 总结
FC-Planner的实现涉及到多个领域的技术,包括计算几何、组合优化、凸优化等。其中的难点主要集中在两个方面:一是如何在保证覆盖完整性的同时最小化视点数量和路径长度;二是如何高效地求解由此产生的各种优化问题。针对第一个难点,提出了骨架引导的空间分解和视点生成策略,可以避免大量的冗余计算。针对第二个难点,巧妙地利用了问题的结构特点,将其分解为多个易于并行求解的子问题,同时采用了各种启发式算法和凸优化技术,大大提高了求解效率。这些思想和技术不仅限于无人机覆盖规划,在其他路径规划问题中也有广泛的应用前景。
具体细节可以阅读下下面的论文原文（中文版）：
FC-Planner: 一个基于骨架引导的快速覆盖复杂3D场景的规划框架
摘要
UAV的3D覆盖路径规划是多种实际应用中的一个关键问题。然而,现有方法在大型复杂场景中的系统简洁性、计算效率和路径质量方面表现不尽如人意。为了应对这些挑战,我们提出了FC-Planner,一个基于骨架引导的规划框架,可以在无需预处理的情况下快速实现对复杂3D场景的空中覆盖。我们通过基于骨架的空间分解(SSD)将场景分解为几个简单的子空间。此外,骨架引导我们轻松确定自由空间。我们利用骨架高效生成一组最小的专门化和信息丰富的视点,以实现完全覆盖。基于SSD,分层规划器有效地将大规划问题划分为独立的子问题,实现每个子空间的并行规划。然后,我们精心设计的全局和局部规划策略被整合,以保证路径生成的高质量和高效率。我们进行了广泛的基准测试和真实世界测试,其中FC-Planner的计算速度比最先进的方法快10倍以上,路径更短,覆盖更完整。源代码将公开发布以造福社区。项目页面: https://hkust-aerial-robotics.github.io/FC-Planner。
I. 引言
近来,由于无人机(UAV)的灵活性和机动性,它们在结构检查[1, 2]和3D重建[3]等各种任务中收集目标场景信息方面变得越来越受欢迎。为了完成这些任务,UAV需要找到一条最短路径来完全覆盖3D场景,这通常被表述为3D覆盖路径规划问题[4]。
现有的工作[1, 2, 5]-[10]通常通过将这个问题分为两个子任务来解决:1)确定一组覆盖目标场景的视点,2)确定具有完全覆盖的最短路径。然而,当前这些子任务的解决方案存在一些限制,阻碍了系统简洁性、计算效率和路径质量的满意实现。对于第一个子任务,现有方法需要使用额外的工具(如Blender [11], CloudCompare [12])进行预处理以提取自由空间,确保视点的安全性,这增加了系统的复杂性。此外,缺乏一种能够产生最小的信息丰富的视点集,同时全面覆盖目标场景的高效策略。现有方法在视点数量和完整性之间存在权衡,即少量视点无法保证完全覆盖,而大量视点则会带来显著的计算成本。另一方面,大多数框架中的路径规划器全局优化一条完全覆盖场景的最短路径,通常被表述为组合优化问题(COP) [13]。然而,由于规划问题的大规模,现有方法在大型复杂场景中经常难以迅速找到高质量的解决方案。目前,在复杂3D场景覆盖的背景下,迫切需要一种高效的规划方法,该方法应能够将艰巨的规划问题分解为可并行化的子问题,同时确保解决方案的质量和计算效率。
为了解决上述限制,我们提出了FC-Planner,这是一个为快速覆盖大型复杂3D场景量身定制的基于骨架引导的规划框架。我们精心设计的全局和局部规划程序,最终生成高质量的覆盖路径,同时保持高计算效率。
我们将提出的方法与最先进的工作进行了仿真比较。结果表明,我们的方法在计算效率方面取得了显著提升(快10倍以上),同时实现了更完整的覆盖和更短的路径。为了进一步验证FC-Planner,我们在一个具有挑战性的场景中进行了真实世界的覆盖测试。仿真和真实世界的实验都证明了我们的系统相比于最先进方法在简洁性和性能方面的优越性。本文的贡献如下:
- 一种高效提取复杂3D场景骨架并指导覆盖规划的SSD方法。
- 一种高效且专门化的视点采样方法,由骨架引导,以及一个引力模型,可有效筛选出最小的信息丰富视点集。
- 一种分层覆盖规划器,将整个规划问题分解为可并行化的子问题。结合精心设计的全局和局部规划模块,它确保了高路径质量和计算效率。
- 大量实验验证了所提出方法的有效性。我们的实现源代码将公开发布。
II. 相关工作
3D场景中的覆盖路径规划问题(CPP)一直是UAV研究的活跃话题,有广泛的实际应用。现有方法通常采用基于采样的框架,分两步解决3D CPP问题,即视点生成和路径规划。
视点生成: 这指的是获得一组视点以满足目标场景覆盖需求的过程。现有方法通常需要预处理来识别自由空间,从中采样安全有效的视点。一些早期方法[1, 5]-[8]利用网格三角形来采样视点。另外,其他工作[9, 14]采用了点云的形式。然而,这两种方法都落入了后一类。通常,这些方法首先采样大量视点候选,然后迭代选择一个视点子集以实现完全覆盖。这种策略导致了大量的计算时间用于选择,并且很难确定所需的最少视点数量。
相比之下,我们的方法通过利用骨架来确定自由空间并采样场景周围的视点,避免了预处理。为了高效找到最小的视点集以实现完全覆盖,我们开发了一种策略,通过重复更新一组视点的姿态而不依赖耗时的选择过程。
路径规划: 此步骤的目标是在全面覆盖场景的同时规划最短路径。以前的方法[1, 5]-[10]将这个问题表述为COP的具体形式,例如,旅行商问题(TSP)[15]、子模块方向TSP(SDOP)[16]等。众所周知,这些问题是NP难的,计算时间随着场景复杂度的增加而急剧增加。为解决这个问题,HCPP[2]提出了一种将场景划分为八叉树的方法。每个叶节点在八叉树中包含最多N个视点,定义一个子空间。然后,规划问题被转化为解决每个子空间的局部TSP。然而,将视点限制在单个子空间内并不能保证该子空间的完全覆盖。此外,来自其他子空间的视点可能需要实现完全覆盖。因此,每个子空间的覆盖区域不能预先分配,下一个子空间的覆盖目标只能在前一个子空间规划完成后更新。这在为不同子空间实现并行规划时带来了挑战。
相反,我们的SSD将场景分解为几个由骨架引导的简单子空间。这确保了每个子空间内的视点避免与该子空间不匹配。因此,可以为每个子空间并行生成路径,从而显著节省计算时间。
III. 系统概述
所提出的框架以目标场景的点云作为输入,这在各种应用中被广泛采用。如图2所示,它由SSD(Sect.IV)、高效视点生成(Sect.V)和分层覆盖规划(Sect.VI)组成。SSD提取场景的骨架,并将点云分解为具有简单几何形状的子空间。在骨架的引导下,我们生成采样射线来采样安全的初始视点,无需预处理。然后该方法最小化视点数量,并使用引力模型更新它们的姿态(Sect.V)。之后,分层规划器找出子空间的全局访问序列,并行优化每个子空间的局部路径,优化局部路径之间的连接部分,并生成一条实现完全覆盖的轨迹(第IV节)。
注释:本章概述了FC-Planner的三大模块:基于骨架的空间分解(SSD)、高效视点生成和分层覆盖规划。SSD利用骨架将复杂场景分解为简单子空间,视点生成模块在骨架引导下高效采样视点并优化姿态,最后分层规划器实现了子问题的并行优化,从而显著提高计算效率。这三个模块协同工作,快速生成高质量的覆盖路径。
IV. 基于骨架的空间分解
在本节中,我们分三步介绍基于骨架的空间分解。1) 给定目标场景的点云,计算由广义旋转对称轴(ROSA)点[17]组成的骨架(第IV-A节)。2) 将骨架分解为几个简单形状,称为分支(第IV-B节)。3) 将点云分配到每个分支中,形成相应的子空间(第IV-C节)。
A. 骨架提取
首先,我们通过法线估计[18]从输入点云获得定向点云 P O P_O PO。受[17]启发,我们使用一组ROSA点表示骨架,每个点由位置和方向表示为r=(x, v)。为加快计算过程,我们对 P O P_O PO进行下采样得到 P D P_D PD,并计算 P D P_D PD中每个点p的ROSA点 r p = ( x p , v p ) r_p=(x_p, v_p) rp=(xp,vp)。对于每个 p p p,其对应的ROSA点在由 v p v_p vp和p构成的平面内对p的邻域 N p N_p Np进行操作。ROSA点在 N p N_p Np周围表现出高度旋转对称性。这一特性要求方向 v p v_p vp最小化 v p v_p vp与 N p N_p Np中法线之间角度的方差,而位置 x p x_p xp最小化到 N p N_p Np中法线延长线的平方距离之和。首先,给定初始方向 v p 0 v^0_p vp0,我们通过求解以下二次规划问题迭代优化 v p v_p vp:
v p i + 1 = arg min v p v p T Σ p i v p , s.t. ∣ ∣ v p ∣ ∣ 2 = 1 , v p ⋅ v p i > 0 , \begin{aligned} v^{i+1}_p = \underset{v_p}{\arg \min}~ v^T_p \Sigma^i_p v_p, \\ \text{s.t.} ~~ ||v_p||_2 = 1,~ v_p \cdot v^i_p > 0 , \end{aligned} vpi+1=vpargmin vpTΣpivp,s.t. ∣∣vp∣∣2=1, vp⋅vpi>0,
其中 v p i v^i_p vpi表示第i次迭代的方向。 Σ p i \Sigma^i_p Σpi是p邻域 N p i N^i_p Npi中所有点法线的协方差矩阵。找到ROSA点的方向后,我们使用以下程序计算 x p x_p xp:
arg min x p ∈ R 3 ∑ p k ∈ N p ∣ ∣ ( x p − p k ) × n ( p k ) ∣ ∣ 2 2 , \underset{x_p \in \mathbb{R}^3}{\arg \min} \sum_{p_k \in N_p} ||(x_p - p_k) \times n(p_k)||^2_2, xp∈R3argminpk∈Np∑∣∣(xp−pk)×n(pk)∣∣22,
其中 n ( p k ) n(p_k) n(pk)表示 p k ∈ N p p_k \in N_p pk∈Np的点法线。最后,我们对所有ROSA点应用一维移动最小二乘法[19]生成骨架。骨架存储在无向图 G = ( V s , E s ) G = (V_s, E_s) G=(Vs,Es)中,其中顶点 V s V_s Vs包含所有ROSA点,边 E s E_s Es表示它们之间的连接。
注释:骨架提取利用了ROSA点的旋转对称性,通过优化ROSA点的位置和方向来生成骨架。其核心是一个二次规划问题,用于优化ROSA点的方向,使其与邻域点的法线方向一致。这一步骤高效提取了场景的骨架结构。
B. 骨架分解
如算法1所述,我们将提取的骨架分解为几个分支,每个分支包含G中的一组边。这些分支都包含在分支集B中。在第2行,我们识别G上的关节J,定义为度数大于2的顶点。由于简单几何要求,一个分支通常从一个关节开始,以一个关节或叶顶点(度等于1)结束。为了获得满足上述需求的分支,我们在第3-14行开发了一个类似深度优先搜索(DFS)的程序[20]。此外,为确保每个分支尽可能简单易覆盖,我们要求一个分支内的边应具有相似的方向。因此,在第16-26行,我们将方向变化较大的分支分解为更简单的分支。
注释:骨架分解利用深度优先搜索,将骨架分解为多个简单分支。为了确保分支的简单性,算法还会将方向变化较大的分支进一步分解。这一步骤简化了骨架结构,为后续的空间分配和路径规划提供了基础。
C. 空间分配
给定分解后的骨架,我们的目标是将 P O P_O PO中的点分配到多个子空间中,每个子空间对应B中的一个分支。我们将这些分支中的边离散化为几个定向点,其方向是它所在边的方向。由这些定向点确定的平面记为Γ。然后,对于每个平面,我们从 P O P_O PO中查询该平面内的点。如果一个点p∈ P O P_O PO位于多个平面上,p将被分配到其定向点最接近它的平面。之后,由同一分支的平面查询的点(称为分配点A)被包含在一个子空间中。所有子空间包含在 S = s 1 , . . . , s N S = {s_1, ..., s_N} S=s1,...,sN中。
注释:空间分配步骤将点云中的点按照它们与骨架分支的距离分配到不同的子空间。这种分配方式简单高效,同时保证了子空间的独立性,为并行路径规划创造了条件。
V. 基于骨架的视点生成
本节介绍高效生成完全覆盖的视点。骨架允许识别内部空间,确保视点的安全性,无需预处理。此外,骨架产生一组视点采样射线,指导视点的生成(第V-A节)。为高效找到最小视点集,我们提出了一个类似引力的模型,迭代调整视点的姿态并移除冗余视点,筛选出更具代表性的子集(第V-C节)。这个过程通过允许更快速可见性检查的双向光线投射(BiRC)进一步加速(第V-B节)。
A. 内部空间和视点采样射线
内部空间用体素地图表示,所有体素初始化为自由状态。包含 P O P_O PO的体素被更新为占用状态。对于每个平面 γ i ∈ Γ γ_i ∈ Γ γi∈Γ(第IV-C节),我们从其定向点 o i o_i oi(位于骨架上)到每个分配点A_i进行光线投射。在穿过占用体素之前的射线上的体素被标记为内部。射线继续延伸到自由空间,我们将这些射线从占用体素开始的部分定义为视点采样射线。沿着它们采样安全和信息丰富的视点,减少冗余采样的视点数量。这些专门的视点还提供了更好的覆盖,因为骨架引导它们都指向表面。此外,从 o i o_i oi投射的视点采样射线属于与 o i o_i oi对应的子空间。
B. 双向光线投射
光线投射广泛用于视点的可见性检查。然而,这种方法从射线的一端开始单向遍历体素。当占用体素位于射线的另一端附近时,它可能无法有效识别遮挡。为解决这个问题,我们实现了双向光线投射(BiRC),其中体素从两端向射线中点遍历。这种方法在遇到上述情况时更早停止,消除了冗余检查。
注释:本小节介绍了两个加速可见性检查的技术。首先,利用骨架和光线投射快速识别内部空间和视点采样射线,减少冗余采样。其次,双向光线投射通过从两端遍历体素的方式,进一步提高了可见性检查效率。
C. 视点姿态的迭代更新
我们采用5自由度视点,表示为 v p = [ p , θ , φ , i d ] vp = [p, θ, φ, id] vp=[p,θ,φ,id],其中 p p p是传感器的3D位置, θ θ θ和 φ φ φ分别表示俯仰角和偏航角。参数id是视点所属的子空间。视点采样射线 r v s r_vs rvs的起点和方向分别描述为 s r = [ x s r , y s r , z s r ] sr = [x_{sr}, y_{sr}, z_{sr}] sr=[xsr,ysr,zsr]和 d r = [ n x d r , n y d r , n z d r ] dr = [nx_{dr}, ny_{dr}, nz_{dr}] dr=[nxdr,nydr,nzdr]。
我们沿每个r_vs在距离D处采样一个视点,其id指的是r_vs所属的子空间。采样视点的位置、俯仰角和偏航角按如下方式确定:
p = s r + D ∗ d r , θ = arcsin ( − n z d r / ∣ ∣ d r ∣ ∣ 2 ) , ϕ = arctan ( − n y d r / − n x d r ) . \begin{aligned} p &= sr + D * dr, \\ \theta &= \arcsin(-nz_{dr} / ||dr||2), \\ \phi &= \arctan(-ny{dr} / -nx_{dr}). \end{aligned} pθϕ=sr+D∗dr,=arcsin(−nzdr/∣∣dr∣∣2),=arctan(−nydr/−nxdr).
这些采样的视点称为初始视点 V P i n i VP_ini VPini。第一步是使用提出的BiRC确定 V P i n i VP_ini VPini中每个视点覆盖的体素。接下来,为减少视点数量,如果一个体素被两个以上的视点观察到,我们将其分配给覆盖体素最多的视点。然后,我们从 V P i n i VP_ini VPini中移除未分配任何体素的视点。之后,我们为 V P i n i VP_ini VPini构建一个kd树[21] T i n i T_ini Tini,并为每个视点定义二元状态(活跃或休眠),初始将所有视点设置为活跃。
为确保每个视点覆盖尽可能多的体素,我们使用引力模型将覆盖体素较少的视点合并到覆盖体素较多的视点中。具体来说,从覆盖体素最多的视点开始,到覆盖体素最少的视点,每个视点(表示为 v p q vp_q vpq)使用半径搜索从 T i n i T_ini Tini中查询其邻域 V P q VP_q VPq。半径 r q r_q rq由最大可见距离 d v d_v dv和视场 ( F o V ) [ f h , f w ] (FoV) [f_h, f_w] (FoV)[fh,fw]确定:
r q = d v ∗ tan ( min ( f h , f w ) / 2 ) . r_q = d_v * \tan(\min(f_h, f_w)/2). rq=dv∗tan(min(fh,fw)/2).
我们使用 V P q VP_q VPq中所有活跃视点 V P a VP_a VPa通过引力模型更新查询视点 v p q vp_q vpq的姿态:
p q = p q + ∑ v p a ∈ V P a c a c q ∗ ( p a − p q ) , s.t. c a < c q , p_q = p_q + \sum_{vp_a \in VP_a} \frac{c_a}{c_q} * (p_a - p_q), ~~ \text{s.t.}~~ c_a < c_q, pq=pq+vpa∈VPa∑cqca∗(pa−pq), s.t. ca<cq,
其中 c q c_q cq和 c a c_a ca分别表示 v p q vp_q vpq和 V P a VP_a VPa中每个视点覆盖的体素数量。 p q p_q pq表示 v p q vp_q vpq的更新位置。类似地,我们获得更新后的俯仰角 θ q θ_q θq和偏航角 φ q φ_q φq。它们进一步调整以使 v p q vp_q vpq覆盖的体素分布在FoV的中心周围。 v p q vp_q vpq所属的子空间与 V P i n i VP_ini VPini中最接近 v p q vp_q vpq的视点相同。接下来,我们将 V P a VP_a VPa中用于更新 v p q vp_q vpq的视点状态设置为休眠。之后,我们通过对所有活跃视点执行第一步来识别未覆盖的体素,然后采样新的视点来覆盖它们,表示为 V P u n c VP_unc VPunc。最后,我们对 V P u n c VP_unc VPunc重复上述步骤以实现完全覆盖。经过这种视点姿态的迭代更新后,所有剩余的活跃视点根据它们的id分配到S中的每个子空间,每个视点子集表示为 V = V P 1 , . . . , V P N V = {VP_1, ..., VP_N} V=VP1,...,VPN。
注释:本小节提出了一种基于引力模型的视点姿态优化方法。通过将视点向覆盖体素更多的方向移动,并去除冗余视点,该方法可以在保证覆盖完整性的同时最小化视点数量。优化后的视点集为路径规划提供了高质量的输入。
VI. 分层覆盖规划
给定上述视点,这个阶段致力于找到穿过它们的最短无碰撞路径。在SSD的支持下,我们将大规模规划问题分解为几个独立且可并行的子问题,分为五个步骤。
全局序列规划。它试图解决一个最优巡游问题,即从UAV当前位置出发访问所有子空间,这被定义为非对称旅行商问题(ATSP)[22]。我们首先计算每个子空间中视点的质心,然后构建这些质心和UAV当前位置之间的欧几里得距离矩阵M_G。Lin-Kernighan-Helsgaun (LKH)求解器[23]以M_G作为输入,获得全局序列[g_1, ..., g_N]。
局部边界选择 。我们期望为每个子空间选择合适的起始和结束视点,确保局部规划的结果与全局序列兼容。根据全局序列,我们将所有质心排序为 S e q C = [ k 0 , . . . , k N ] Seq_C = [k_0, ..., k_N] SeqC=[k0,...,kN],其中 k 0 k_0 k0是UAV的当前位置,其他是排序后的质心。基于 S e q C Seq_C SeqC,第i个访问子空间的起始 v p s t a r t ( g i ) vp^{(g_i)}{start} vpstart(gi)和结束 v p e n d ( g i ) vp^{(g_i)}{end} vpend(gi)通过以下方式获得:
v p s t a r t ( g i ) = arg min v p g i ∈ V P g i ∣ ∣ p g i − k i − 1 ∣ ∣ 2 2 + ∣ ∣ p g i − k i ∣ ∣ 2 2 , v p e n d ( g i ) = arg min v p g i ∈ V P g i ∣ ∣ p g i − k i ∣ ∣ 2 2 + ∣ ∣ p g i − k i + 1 ∣ ∣ 2 2 . \begin{aligned} vp^{(g_i)}{start} &= \underset{vp{g_i} \in VP_{g_i}}{\arg \min}~ ||p_{g_i} - k_{i-1}||^2_2 + ||p_{g_i} - k_i||^2_2, \\ vp^{(g_i)}{end} &= \underset{vp{g_i} \in VP_{g_i}}{\arg \min}~ ||p_{g_i} - k_i||^2_2 + ||p_{g_i} - k_{i+1}||^2_2. \end{aligned} vpstart(gi)vpend(gi)=vpgi∈VPgiargmin ∣∣pgi−ki−1∣∣22+∣∣pgi−ki∣∣22,=vpgi∈VPgiargmin ∣∣pgi−ki∣∣22+∣∣pgi−ki+1∣∣22.
特别地,最后访问的子空间将没有结束视点,因为没有后续子空间。
并行局部路径规划。一旦确定了边界视点,我们就可以为每个子空间单独规划局部路径。我们将这个问题表述为TSP的一个变体,增加了给定起点和终点的约束。假设一个子空间有R个视点,这个TSP的成本矩阵M_L如下:
M L ( i , j ) = { 0 , i = = j 或 j = 0 + ∞ , i = R − 1 且 j ∈ { 1 , . . . , R − 2 } c ( v p i , v p j ) , 其他 , M_L(i,j) = \begin{cases} 0, & i == j \text{ 或 } j = 0 \\ +\infty, & i = R-1 \text{ 且 } j \in \{1, ..., R-2\} \\ c_{(vp_i,vp_j)}, & \text{其他} \\ \end{cases}, ML(i,j)=⎩ ⎨ ⎧0,+∞,c(vpi,vpj),i==j 或 j=0i=R−1 且 j∈{1,...,R−2}其他,
c ( v p i , v p j ) = max ( max ( L ( p i , p j ) v m a x , A n g ( θ i , θ j ) ω m a x ) , A n g ( ϕ i , ϕ j ) ω m a x ) , c_{(vp_i,vp_j)} = \max\left(\max\left(\frac{L(p_i, p_j)}{v_{max}}, \frac{Ang(\theta_i, \theta_j)}{\omega_{max}}\right), \frac{Ang(\phi_i, \phi_j)}{\omega_{max}}\right), c(vpi,vpj)=max(max(vmaxL(pi,pj),ωmaxAng(θi,θj)),ωmaxAng(ϕi,ϕj)),
A n g ( a 1 , a 2 ) = min ( ∣ a 1 − a 2 ∣ , 2 π − ∣ a 1 − a 2 ∣ ) , Ang(a_1, a_2) = \min(|a_1 - a_2|, 2\pi - |a_1 - a_2|), Ang(a1,a2)=min(∣a1−a2∣,2π−∣a1−a2∣),
其中 L ( ⋅ ) L(\cdot) L(⋅)是通过A*算法搜索的 v i v_i vi和 v j v_j vj之间的无碰撞路径长度, v m a x v_{max} vmax和 ω m a x \omega_{max} ωmax是速度和俯仰角、偏航角角速度的限制。对于最后访问的子空间, M L M_L ML中所有 + ∞ +\infty +∞条目都设置为 c ( v p i , v p j ) c_{(vp_i,vp_j)} c(vpi,vpj)。对于每个 V P i ∈ V VP_i \in V VPi∈V,我们使用多线程编程并行生成覆盖其对应子空间的局部路径。假设总共有 m ∈ R + m \in \mathbb{R}^+ m∈R+个视点,所有子问题中最大的视点数是 e ∈ R + e \in \mathbb{R}^+ e∈R+, m ≫ e m \gg e m≫e。由于使用LKH求解器解决TSP的时间复杂度为 O ( n 2.2 ) O(n^{2.2}) O(n2.2)[24],不进行问题分解直接解决所有视点的TSP需要 O ( m 2.2 ) O(m^{2.2}) O(m2.2)时间。得益于我们的并行规划,解决所有子问题的时间复杂度降低到 O ( e 2.2 ) O(e^{2.2}) O(e2.2)。因此,我们的方法有效降低了时间复杂度并提高了计算效率。我们按全局序列的顺序组合所有局部路径,得到整个覆盖路径 P C P_C PC。
注释:分层覆盖规划的核心是将大规模TSP分解为多个子问题并行求解。全局序列规划决定了子空间的访问顺序,局部边界选择确定了每个子空间的起始和结束视点,从而实现子问题的独立性。并行局部路径规划利用多线程编程同时优化各个子空间内的路径,大大减少了计算时间。
局部路径优化。由于前一步中的覆盖路径是在每个子空间内优化的,它不能保证全局最优性,因此可能产生不必要的绕路。我们观察到这些绕路通常发生在局部路径的连接处附近。因此,我们提出了一个局部优化策略,以进一步改善连接处的整体覆盖路径,如算法2所示。在第1行,连接点被定义为两个分支相交的顶点。第2-16行展示了使用局部搜索进行迭代优化的程序,共进行K次迭代。它首先选择整个覆盖路径中靠近连接点的部分。然后,每次迭代从这些部分随机选择三个视点,如果交换它们可以缩短覆盖路径,就使用2-opt[25]进行交换。
注释:局部路径优化通过局部搜索和2-opt操作改进覆盖路径质量。它集中优化相邻子空间连接处的路径,消除不必要的绕路,同时保留子空间内部已优化的路径,在提高路径质量的同时将计算成本降到最低。
无碰撞轨迹优化 。它的目标是将优化后的覆盖路径 P R = v R 0 , . . . P_R = {v^0_R, ...} PR=vR0,... , 为一条平滑、动态可行、安全且时间最短的轨迹,通过所有视点。具体来说,我们将P_R分成M段轨迹,然后生成3D安全飞行走廊(SFCs),其中每个凸飞行走廊被分配一组连续的轨迹段。对于位置轨迹,SFCs用于确保安全性:
t p i ( t ) ∈ C P ( i ) , ∀ t ∈ [ 0 , T i ] , ∀ 1 ≤ i ≤ M , tp_i(t) \in CP(i), ~~ \forall t \in [0, T_i], ~~ \forall 1 \leq i \leq M, tpi(t)∈CP(i), ∀t∈[0,Ti], ∀1≤i≤M,
其中 t p i tp_i tpi是第 i i i段轨迹,持续时间为 T i T_i Ti, C P ( i ) CP(i) CP(i)表示SFCs中分配给 t p i tp_i tpi的凸飞行走廊。为了避免激进飞行导致的视觉模糊并确保动态可行性,还考虑了速度、加速度和加加速度限制:
KaTeX parse error: Undefined control sequence: \dddot at position 215: ...i \leq M, \\ ||\̲d̲d̲d̲o̲t̲{tp}_i(t)||^2_2...
其中 v m a x v_{max} vmax、 a m a x a_{max} amax和 j m a x j_{max} jmax是动态限制。我们还为俯仰角和偏航角生成轨迹,其中最大角速度的限制类似于式(9)。
我们利用MINCO[26]优化覆盖轨迹的时间分配,同时满足上述约束。
注释:无碰撞轨迹优化基于覆盖路径生成平滑、安全且动态可行的飞行轨迹。它利用安全飞行走廊(SFC)确保无碰撞性,同时考虑无人机的动力学约束,如速度、加速度限制等。此外,它还优化了轨迹的时间分配,使执行时间最短。生成的轨迹可直接用于无人机控制。
VII. 实验
A. 实现细节
在骨架提取步骤中,我们将输入点云归一化到一个单位球内,这增强了其对场景尺度变化的鲁棒性。我们在算法1中设置 δ δ δ= 45°,局部路径优化中的迭代次数K = 10000,因为每次迭代仅消耗约0.01ms。在所有实验中,我们选择一个2轴云台相机作为装备在UAV上的传感器。使用几何控制器[27]进行 ( p , θ , φ ) (p, θ, φ) (p,θ,φ)轨迹的跟踪控制。所有模块在Intel Core i9-13900K CPU上运行。
B. 基准比较和分析
为验证我们提出的框架,我们在模拟中进行基准测试,包括三个大型复杂场景：管道(73×66×41 m³),基督救世主像(10×36×38 m³),和滨海湾金沙(100×25×49 m³)。我们比较了两种最先进的方法：SIP[1]和HCPP[2]。由于HCPP[2]没有开源代码,所以我们使用了自己的实现。在模拟中,云台的俯仰角限制在[-90°, 70°]范围内,相机视场为[75°, 55°]。我们为所有方法设置动态限制为 v m a x = 2.0 m / s v_{max} = 2.0 ~ \mathrm{m/s} vmax=2.0 m/s, ω m a x = 1.0 r a d / s ω_{max} = 1.0 ~ \mathrm{rad/s} ωmax=1.0 rad/s, a m a x = 1.0 m / s 2 a_{max} = 1.0 ~ \mathrm{m/s^2} amax=1.0 m/s2, j m a x = 0.5 m / s 3 j_{max} = 0.5 ~ \mathrm{m/s^3} jmax=0.5 m/s3。表I报告了平均比较结果,其中Exec. Time表示轨迹执行时间。
从评估结果来看,我们的方法在计算效率、覆盖完整性和路径质量方面都显著优于其他方法。由于提出的基于骨架引导的视点生成,我们的框架以最少的视点实现了最大的覆盖。得益于我们有效的SSD和并行规划,我们的方法运行速度比现有方法快10倍以上。在路径质量方面,我们生成了更短的覆盖路径和轨迹,实现了更少的执行时间。此外,随着场景复杂度和规模的增加,这些优势变得更加明显。
我们进行了消融研究,以证明局部路径优化和分层规划的有效性。我们将不使用局部优化的框架表示为 N R NR NR,而 G O GO GO则用全局优化所有视点的路径替代分层规划。表II显示了局部路径优化和分层规划对路径质量和计算效率的贡献。与 N R NR NR相比,我们的方法在最小额外计算的情况下缩短了路径。与 G O GO GO相比,我们的方法显著减少了计算时间,同时路径质量损失最小。
此外,表III提供了每个模块的运行时间和空间分解详情。更多关于每个模块的详细性能信息可以在我们的项目页面上找到。
注释:本章通过大量实验全面评估了FC-Planner的性能。基准测试表明,无论在计算效率、覆盖完整性还是路径质量方面,FC-Planner都大大优于现有方法。消融研究进一步证明了局部路径优化和分层规划这两个关键模块的有效性。这些实验结果充分说明了我们提出方法的优越性。
C. 真实世界测试
为进一步验证所提出的框架,我们在一个具有挑战性的场景中进行了真实世界的覆盖测试,其大小为10×5×2.5 m³。对于这次测试,我们设计了一个定制的四旋翼飞行器。云台在[-90°, 20°]范围内旋转,相机视场为[60°, 80°]。动态限制设置为v_max = 0.7 m/s,ω_max = 1.0 rad/s,a_max = 0.5 m/s²,j_max = 0.5 m/s³。我们首先使用激光雷达收集场景的点云。然后,我们的框架生成覆盖轨迹,其中我们的方法合理地将场景分解为五个子空间进行并行规划。之后,四旋翼飞行器在执行轨迹时捕获图像。最后,我们使用这些图像重建场景,结果表明我们的方法确实实现了完全覆盖。更多关于这次测试的细节可以在我们的视频中找到。
注释:在真实场景中的测试进一步证实了FC-Planner的实用性和鲁棒性。整个系统的工作流程,包括场景采集、覆盖规划、轨迹执行和场景重建,都按预期顺利进行。最终重建的场景展示了我们方法出色的覆盖效果。这一测试表明FC-Planner在实际应用中具有巨大潜力。
VIII. 结论
在本研究中,我们提出了一个专门为快速覆盖大型复杂3D场景设计的基于骨架引导的规划框架。提出的SSD有效地提取场景的骨架,实现了将场景合理分解为简单子空间,并在无需预处理的情况下引导高效生成专门化和信息丰富的视点。基于SSD,分层覆盖规划器将整个规划问题分解为可并行化的子问题,结合设计的全局规划和局部优化方法,高效地生成高质量的覆盖路径。大量实验证明了我们框架的效率和优越性。未来,我们计划将我们的框架扩展到多UAV系统,以实现在更大和未知的3D场景中的协作覆盖。
注释:本文提出了FC-Planner,一个创新的基于骨架引导的三维覆盖路径规划框架。它通过骨架提取、视点生成和分层规划三个紧密协作的模块,实现了对复杂大型场景的高效覆盖。实验结果充分证明了该方法在各个方面的卓越表现。未来,FC-Planner有望扩展到多无人机系统和未知环境,解决更加复杂和具有挑战性的实际问题。总的来说,本研究为无人机三维覆盖路径规划提供了一个新的思路和基准。
参考文献
[1] A. Bircher, K. Alexis, M. Burri, P. Oettershagen, S. Omari, T. Mantel, and R. Siegwart, "Structural inspection path planning via iterative viewpoint resampling with application to aerial robotics," in 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015, pp. 6423--6430.
[2] C. Cao, J. Zhang, M. Travers, and H. Choset, "Hierarchical coverage path planning in complex 3d environments," in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 3206--3212.
[3] C. Feng, H. Li, F. Gao, B. Zhou, and S. Shen, "Predrecon: A prediction-boosted planning framework for fast and high-quality autonomous aerial reconstruction," in 2023 IEEE International Conference on Robotics and Automation (ICRA), 2023, pp. 1207--1213.
[4] E. Galceran and M. Carreras, "A survey on coverage path planning for robotics," Robotics and Autonomous systems, vol. 61, no. 12, pp. 1258--1276, 2013.
[5] W. Jing, J. Polden, W. Lin, and K. Shimada, "Sampling-based view planning for 3d visual coverage task with unmanned aerial vehicle," in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2016, pp. 1808--1815.
[6] R. Almadhoun, T. Taha, D. Gan, J. Dias, Y. Zweiri, and L. Seneviratne, "Coverage path planning with adaptive viewpoint sampling to construct 3d models of complex structures for the purpose of inspection," in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 7047--7054.
[7] W. Jing, D. Deng, Z. Xiao, Y. Liu, and K. Shimada, "Coverage path planning using path primitive sampling and primitive coverage graph for visual inspection," in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 1472--1479.
[8] H. W. Tong, B. Li, H. Huang, and C.-y. Wen, "Uav path planning for complete structural inspection using mixed viewpoint generation," in 2022 17th International Conference on Control, Automation, Robotics and Vision (ICARCV). IEEE, 2022, pp. 727--732.
[9] X. Huang, Y. Liu, L. Huang, S. Stikbakke, and E. Onstein, "Bim-supported drone path planning for building exterior surface inspection," Computers in Industry, vol. 153, p. 104019, 2023.
[10] M. Roberts, D. Dey, A. Truong, S. Sinha, S. Shah, A. Kapoor, P. Hanrahan, and N. Joshi, "Submodular trajectory optimization for aerial 3d scanning," in Proceedings of the IEEE International Conference on Computer Vision, 2017, pp. 5324--5333.
[13] C. H. Papadimitriou and K. Steiglitz, Combinatorial optimization: algorithms and complexity. Courier Corporation, 1998.
[14] R. M. Claro, M. I. Pereira, F. S. Neves, and A. M. Pinto, "Energy efficient path planning for 3d aerial inspections," IEEE Access, vol. 11, pp. 32 152--32 166, 2023.
[15] G. Dantzig, R. Fulkerson, and S. Johnson, "Solution of a large-scale traveling-salesman problem," Journal of the operations research society of America, vol. 2, no. 4, pp. 393--410, 1954.
[16] C. Chekuri and M. Pal, "A recursive greedy algorithm for walks in directed graphs," in 46th annual IEEE symposium on foundations of computer science (FOCS'05). IEEE, 2005, pp. 245--253.
[17] A. Tagliasacchi, H. Zhang, and D. Cohen-Or, "Curve skeleton extraction from incomplete point cloud," in ACM SIGGRAPH 2009 papers, 2009, pp. 1--9.
[18] R. B. Rusu and S. Cousins, "3d is here: Point cloud library (pcl)," in 2011 IEEE international conference on robotics and automation. IEEE, 2011, pp. 1--4.
[19] I.-K. Lee, "Curve reconstruction from unorganized points," Computer aided geometric design, vol. 17, no. 2, pp. 161--177, 2000.
[20] R. Tarjan, "Depth-first search and linear graph algorithms," SIAM journal on computing, vol. 1, no. 2, pp. 146--160, 1972.
[21] K. Zhou, Q. Hou, R. Wang, and B. Guo, "Real-time kd-tree construction on graphics hardware," ACM Transactions on Graphics (TOG), vol. 27, no. 5, pp. 1--11,v^M_R}$转换2008.
[22] Z. Meng, H. Qin, Z. Chen, X. Chen, H. Sun, F. Lin, and M. H. Ang, "A two-stage optimized next-view planning framework for 3-d unknown environment exploration, and structural reconstruction," IEEE Robotics and Automation Letters, vol. 2, no. 3, pp. 1680--1687, 2017.
[23] K. Helsgaun, "An effective implementation of the lin--kernighan traveling salesman heuristic," European journal of operational research, vol. 126, no. 1, pp. 106--130, 2000.
[24] C. H. Papadimitriou, "The complexity of the lin--kernighan heuristic for the traveling salesman problem," SIAM Journal on Computing, vol. 21, no. 3, pp. 450--465, 1992.
[25] S. M. McGovern and S. M. Gupta, "2-opt heuristic for the disassembly line balancing problem," in Environmentally conscious manufacturing iii, vol. 5262. SPIE, 2004, pp. 71--84.
[26] Z. Wang, X. Zhou, C. Xu, and F. Gao, "Geometrically constrained trajectory optimization for multicopters," IEEE Transactions on Robotics, vol. 38, no. 5, pp. 3259--3278, 2022.
[27] T. Lee, M. Leoky, and N. H. McClamroch, "Geometric tracking control of a quadrotor uav on se (3)," in Proc. of the IEEE Control and Decision Conf. (CDC), Atlanta, GA, Dec. 2010, pp. 5420--5425.