FC-Planner: A Skeleton-guided Planning Framework for Fast Aerial Coverage of Complex 3D Scenes

空中机器人快速覆盖复杂3D场景的骨架引导规划框架：
利用骨架有效地生成一组专业和信息丰富的最小视点，以实现完整的覆盖。基于SSD，分层规划器将大规划问题有效地分为独立的子问题，实现了每个子空间的并行规划。然后，采用精心设计的全局和局部规划策略，以保证路径生成的高质量和高效率。

有效提取目标场景的框架

• 基于骨架的空间划分
• 视点采样策略，类引力模型筛选信息量最小的视点集；
• 分层覆盖规划器，利用空间划分将规划空间变成可并行的子空间；

前言 ROSA 旋转对称点

ROSA点集的识别和提取通常用于点云分析中，以便识别点云中的旋转对称结构或对象。例如，它们可以用于识别自然场景中的树干、管道、轮胎等具有旋转对称性的物体。 在无人机巡检场景中，对环境点云进行骨架提取，其中环境点云包含位置和法向量信息。在已知的点云信息中，随机采样点云，根据法向量获取对应的切割平面，取距离切割平面一定阈值的点云作为邻居点云，对邻居点云求ROSA点生成骨架点。 迭代采样生成大量骨架点构成巡检环境的基本骨架。

* ROSA和骨架点云构建

取点云样本采样点pi，以采样点方向vi构建切割平面。识别切割平面内一定距离内的点云样本，通过ROSA找到旋转对称中心点。在点云数据缺失的情况下，点法线可以有效的补偿缺失数据。两个带方向点云的Mahalanobis距离小于阈值时，定义两点之间有一条边。 Mahalanobis 距离结合了欧几里得和方向空间信息，方向相关性由协方差矩阵表示。

1.基于骨架的空间分解

①　在给定目标检测空间的点云信息，生成计算由广义旋转对称轴（ROSA）点组成的骨架；
利用点云采样点，转换为ROSA点，将所有ROSA点应用一维移动最小二乘生成骨架，将骨架存储在无向图中，包含所有ROSA点和之间的连接。
②　将骨架分割为几个简单的形状；对①中生成的无向图进行分割，每一个分割包含无向图中的一些边。节点阶数Degree表示与该顶点相邻边的数量。

如上图中第二步所示骨架分割，在以下算法1中：
首先对无向图保存关节点，其中关节点的度数大于2，表示该关节点有2个以上的搜索方向，将关节点保存在J中。遍历每一个关节点，并采用深度优先搜索用于搜索分支，每一个方向的分支搜索碰到节点阶数不为2时，当前分支搜索完毕。由关节点的下一个方向邻域节点继续开始搜索，完成后得到当前关节点的所有分支；再搜索下一个关节点，知道将所有的分支搜索完毕。
第二步对当前搜索的每一个分支进行方向变化检查，当方向变化量较大，对当前分支进一步分割，保存在新的分支集合中。完成骨架分割。

③　将点云分配到每个分支中，形成相应的子空间。将②中获得的分支，对每一个分支上的边离散为几个有向点，有向点的方向为边的方向，以此能确定一个平面。如果点云中的点位于平面上，将该点分配到该分支。如果一个点位于多个平面中，就将该点分配到距离近的分支中。

2.骨架引导的视点策略

类引力模型 迭代找到合适的视点，减少冗余的视点。采用双向光线投射(BiRC)的方法，更快的访问检查。骨架上的有向点引导视点指向曲面；并且视点采样光线属于有向点所在的子空间中。所有视点的方向指向骨架上的点，并且垂直于骨架上的边。

如上图中，红色的点表示骨架上的一个有向点，在有向点方向作为法方向的所在平面中向外投射射线，在碰到已占用的体素网格前，射线经过的体素为内部空间，当射线碰到已占用的体素网格（即输入点云，物体表面）时，继续延申到自由空间，作为视点采样射线。

双向光线投射 检查视点的可见性，体素网格从射线的两端向射线中点遍历。
根据采样射线上按距离D取采样视点。一个视点包含5个自由度，vp =[p,θ, ϕ, id], 其中p表示视点位置，θ, ϕ分别表示相机的俯仰角和偏航角，id表示视点所属的子空间编号。

首先采样的视点作为初始化视点，用双向射线投射的方法使体素网格被视点覆盖。减少视点数量，如果体素有两种以上的视点体素分配给覆盖最多体素的视点，移除没有分配任何体素的视点。处理完冗余的视点之后将保留的视点构建一个KD-Tree，并定义二进制状态，初始化每一个视点的状态为active。用类引力模型将体素较少的视点合并为体素较多的视点，确保视点尽可能多的覆盖体素网格。从覆盖体素较多的视点开始，一直到覆盖体素最少的视点，每个视点使用半径搜索从KD-Tree结构中查询邻近视点，其中搜索半径由最大可见距离和视场决定。

如上图中，通过邻居视点中的活跃视点来更新查询视点，红色的为查询视点，在查询视点的邻域中找到活跃视点（蓝色），再采用类引力模型更新查询视点（紫色）。更新查询视点的状态后，将视点状态置为休眠（黑色）。更新查询视点后的子空间与离它最近的活跃视点所属的子空间相同。再对所有的活动视点识别未覆盖的体素，对新视点进行采样重新覆盖。
重复以上步骤，对视点姿态进行迭代更新实现完全覆盖。

3.分层覆盖规划

5个步骤将整个路径规划分未几个独立的并行子问题。
a. 用旅行商问题求解每一个子空间的访问顺序：计算每一个子空间视点的质心位置，结合无人机当前的位置，构成一个欧拉距离矩阵，通过LKH求解器求解最优的全局访问序列。
b. 确定每个子空间中的开始视点和终点视点（边界视点），遍历子空间中的每一个视点，根据视点到上一个子空间质心的距离与到所属子空间的质心的距离之和最小的作为起始视点，到下一个子空间质心的距离与到所属子空间的质心的距离之和最小的作为终点视点：

最后一个子空间由于没有下一个子空间，所有没有固定的终点视点。
c. 并行局部规划 ，局部规划每一个子空间中的视点路径，采用带起始点和终点约束的TSP旅行商的算法，以两个节点之间的状态转变所需的时间生成代价矩阵，用TSP求解器求解最优路径。每一个子空间中的规划采用多线程编程的方式同步进行，有效地减少了TSP全局规划所需地时间。
d. 更新局部路径 在每一个子空间地局部规划完成后，路径在各自地子空间中最优，但连接后会产生一些弯路，这些弯路通常分布在骨架分支的交点附近，所以需要一个局部的更新策略优化这部分的路径。以下算法2中，将每两个分支的交点作为连接点，对连接点附近的视点进行K次取样，它首先从整个覆盖路径中选择靠近交叉点的部分。然后，如果这种交换可以缩短覆盖路径，则每次迭代从这些部分随机选择三个视点，并使用2-opt进行交换。

e. 无碰撞轨迹优化 对优化后的路径进行轨迹优化，形成一个无碰撞、动力学可行、时间最优的轨迹，在空间中生成的很多个路径段，采用安全飞行走廊确保轨迹的安全性。用MINCO轨迹优化算法生成最终的全局轨迹。