Trajectory Generation of a Quadrotor Transporting a Bulky Payload in the Cluttered Environments 论文解析

无人机在运输质量或者体积较大的物体时，由于惯性增大，无人机常常会处在一个不平稳的状态，特别是在运动状态发生改变的时候，机体会发生较大的摇晃。

为了解决这一问题，论文中提出了一种无人机在运输大型载荷时通过狭窄空间的路径规划算法。

1. 提出了一种基于Voronoi 图的地图生成算法，角落延伸规则，改变地图规则后使用A*算法可以将无人机看作一个质点。
2. 设计了一种航向角优化算法，使通过狭窄通道时角速度最小，最小化摇摆的影响。

Voronoi 图和地图定义

Voronoi 图的顶点是中轴，所以能够为路径规划提供一个安全区域。 在传统的Voronoi 图中，不能考虑有效载荷的大小，文中假设有效载荷是椭球形状，用椭球的长轴长和短轴长决定无人机在空间中的旋转。

提出了地图修改算法：在栅格地图上实现，欧几里得距离图产生Voronoi 图，顶点到障碍物的距离是Voronoi圆的半径。减少了圆的数量：如果两个随机圆之间的距离小于两个圆半径之和的20%，两个圆中的一个被删除。生成路径采用角延伸法。

传统的A*算法不考虑无人机的形状和载荷，转角扩展使用的椭球长轴长和短轴长的长度。

转角扩展算法：

V由圆的位置(Vpos)和半径(Vrad)组成。
Obs由障碍物在网格图中的位置(下图中的Obs1和Obs2)组成。

我们使用distance(point, Vpos[j])函数(第6-8行)找到了半径为障碍物到圆心的距离的圆。例如，如下图所示，Obs1距离最近的圆是C1和C2，Obs2的圆是C3。

接下来，我们找到最近的圆，并将该圆的半径保存到其他圆中(第10行)。
Obs1和Obs2距离最近的圆分别是C2和C3。
如果最短半径大于rl，则障碍物大小以半径rl扩展(第12行)。例如，由于下图中的d3大于rl，所以Obs2的角的延伸半径为rl。在这一区域，无人驾驶飞行器(UA V)可以通过一个狭窄的通道，以任何航向角度。另一方面，当半径小于rl时，障碍物大小扩展为rs(第14行)。由于Obs1的最近圆的半径(即d2)小于rl且大于rs，所以Obs1的角的半径为rs。

在这个区域，UAV应该旋转通过一个狭窄的通道，这意味着只有UAV的细长形状才能通过。
如果半径小于rs，这个区域不用于在这个狭窄通道中有任何头部角度的UAV。

综合映射使用前一种方法可以采用A*算法。如果机器人适当转弯，地图会让UA V通过狭窄的走廊。

重叠区域 选择两个圆

用d和β作为自由空间。当这个方程应用到所有的圆上时，积分区域描述了自由空间。

找出狭窄通道 全局路径由A*生成的离散路径点组成。

在确定航向角之前，我们从每个航路点找到最近的V圆，并将椭圆的长轴和短轴与最近的V圆半径进行比较。当最近的V圆的半径大于椭圆的短轴时，机器人可以通过走廊。当前一个路径点对应的圆半径大于rl，当前路径点对应的圆半径小于rl时，UAV可以旋转通过走廊。它也可以在相反的情况下旋转。因此，我们用旋转标志定义了这个路径点。

优化和航向角决策

在狭窄通道产生平滑轨迹
路径优化 采用松紧带优化，根据弹性力和路径点P，优化到路径点Q。

假设路径点密集分布在轨迹上，且Qk与Qk+1(即θk)的夹角小于1弧度。这一假设很容易通过轨迹插值得到解决。可以将整个轨迹的侧向力Nk的总和最小化，

安全区域的变化取决于最近的V形蛇圆的半径。

航向角的选择与优化 确定了优化路径的方向，利用旋转点。

1. 为了使无人机的移动方向与相机的方向同步，我们添加了9f k和Yk（角）的差小于π/2的约束，使无人机一直向前看。
2. 由于反正切函数在- π和π附近引起的航向角不连续，通过在原始值上加减2π来表示航向角为连续变量。
3. 优化过程最小化了航向角的变化，即使在旋转点的目标航向角应该改变的地方，目标角度也可能不会改变，为了使目标航向角和优化的航向角在通过狭窄通道时具有相同的值，需要一个附加的约束
   
   最小化角加速度，使得角速度变化率最小

航向角决策算法：

• 找到了旋转点ps，给出优化后的路径点 3-5
• 使用ps和下一个旋转标志PE （由nextRotation函数得到）
• 计算中间路径点PC（在两个旋转点之间）
• 为了在狭窄通道中保持旋转，为优化添加约束(第10行)。
• 通过求解优化问题，最终得到了轨迹

速度优化 使用下面论文中的方法
H. Lee, H. Seo, and H. Kim,“Trajectory optimization and replanning framework for a micro air vehicle in cluttered environments,” IEEE Access,vol. 8, pp. 135406–135415, 2020.

实验结果

仿真实验 无人机搭载视觉惯性传感器通过FIESTA 产生地图。

当使用如图7 (b)所示的无人机时，所生成的轨迹如图7 (e)所示。当无人机负载较大时，可以满意地通过狭窄通道。对于图7 (f)所示的两架载荷较大的无人机，可以很容易地从结果中得到每个轨迹，见图7 (e)。结果表明，该方法不仅适用于单架无人机，而且适用于多架无人机。

与其他方法的比较
与RRT-Connect,RRT*,Informed-RRT* 和Fast Marching Tree star(FMT*) 比较，
提出的方法在时间消耗和旋转方面都有令人满意的表现。虽然我们的算法在总长度上比RRT*， I-RRT和FMT稍长一些，但由于有效载荷中心和墙在狭窄的路径上距离更远，提出的算法更安全。

真机实验 深度相机在线生成地图
运载物是一根1米长的碳管。设计了一个通道较窄的障碍物，障碍物的通道宽度为0.85 m。UAV需要旋转头部角度以通过通道。我们将速度约束设置为0.1米/秒，高度设置为0.6米。

一种最小化转弯时不稳定运动的优化方法。通过仿真和实验验证了该方法保证了路径规划的安全性和计算效率。