Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight 论文解析

无人驾驶飞行器(UA Vs)近年来在工业检测、搜救和包裹递送等领域得到越来越多的应用。为了在这些场景中实现完全自主，运动规划模块在生成安全流畅的运动方面发挥着至关重要的作用。
在有限的时间和机载计算资源条件下，现有工作无法保证生成安全的、运动学可行的高成功率轨迹。然而，轨迹生成的效率和鲁棒性是至关重要的。

在这篇论文中，提出了一种完整的、鲁棒的在线轨迹生成方法来系统地解决效率和鲁棒性这两个问题。

1. 提出了一种鲁棒高效的系统方法，融合了运动学路径搜索、b样条优化和时间调整。
2. 提出了一个基于b样条凸包性质的优化公式，巧妙地融合了梯度信息和动态约束。
3. 采用一种基于关系的时间调整方法来保证运动的可行性和非保守性。

相关工作

在现有的一些轨迹优化硬约束方法中，自由空间中与障碍物的距离被忽略，这通常会导致轨迹接近障碍物。此外，运动动力学约束是保守的，使得轨迹在快速飞行时速度不足。

在一些软约束方法中，将轨迹生成定义为考虑平滑性和安全性的非线性优化问题。通过使用梯度下降方法最小化其平滑性和碰撞代价来生成离散时间轨迹。将它们扩展为连续时间多项式轨迹。由于时间参数化是连续的，避免了数值微分误差，更准确地表示了四旋翼的运动。然而，它的成功率很低。软约束方法利用梯度信息将轨迹推至远离障碍物的位置，但存在局部极小值问题，并不能很好地保证成功率和运动学可行性。

论文中优化方法还利用梯度信息来提高弹道的安全性。然而，不同于以往计算昂贵的沿轨迹线积分的方法，基于b样条的凸包性质重新设计了公式，使其更简单。极大地提高了计算效率和收敛速度。

运动动力学路径搜索

前端运动学路径搜索模块源于 首次为自动驾驶汽车提出 的混合状态A*搜索。

b样条轨迹优化

利用均匀b样条的凸包特性，结合欧几里得距离场和动态约束的梯度信息，在很短的时间内收敛，生成 平滑、安全、动态可行 的轨迹。

轨迹用b样条(pb = 3)表示。每个线段以控制点对应的凸包为界。右图一阶导数(速度)也是b样条，因此具有相同的性质。凸包特性被广泛使用，以确保动态可行性和安全性。
总代价函数定义为
$$f_{total}={\lambda }_1{f}_s+{\lambda }_2{f}_c+{\lambda }_3(f_v+f_v)$$
其中 $f_s$ 和 $f_c$ 分别为平滑度和碰撞代价。$f_v$ 和 $f_a$ 是速度和加速度的软限制。λ为各自的权重。

平滑 我们使用弹性带代价函数

从物理的角度来看，这个公式把轨迹看作是一条松紧带，其中每一项 $F_{i+1,i}=Q_{i+1}-Q_i$ 和 $F_{i-1,i}=Q_{i-1}-Q_i$ 分别是连接节点$Q_{i+1},Q_i$ 和 $Q_{i-1},Q_i$ 的两个弹簧的合力。如果所有项都等于零，则所有控制点将均匀分布在一条直线上，这条直线理想情况下是平滑的。

安全 碰撞代价也可以用作用在每个控制点上的障碍物的排斥力表示:

其中 $d(Q_i)$ 为 $Q_i$ 与最邻近障碍物之间的距离。$F_c$ 是一个可微的潜在代价函数，$d_{thr}$ 指定障碍清除阈值:

距离大于一定的额阈值时，表示障碍物与当前路径点距离较远，清除障碍物不加入碰撞代价考虑。

动态可行 对轨迹上超过最大允许速度或加速度进行惩罚：

应用凸包性质，定义 $f_v$ 和 $f_a$ ，使不可行的速度和加速度控制点被惩罚:

时间分配

采用了一种基于 导数控制点 与 非均匀b样条时间分配(结跨度)关系的时间调整方法。

非均匀b样条
非均匀b样条是一种更一般的b样条。与均匀b样条唯一不同的是，它的每个结点跨度
$∆t_m=t_{m+1}-t_m$ 相互独立。

实验

无人机搭载 Lidar ，估计四旋翼的姿态，并生成一个密集的点云图。
IMU和扩展卡尔曼滤波器(EKF)的声纳测量融合， 获得反馈控制的高速率状态估计。
运动捕捉系统OptiTrack5 提供准确的姿态反馈，并预先构建环境地图。

采用了后退地平线规划方案，只在已知空间内生成轨迹。当运动基元在此范围外结束时，路径搜索终止，然后进行优化和时间调整。

重规划触发机制:在两种情况下触发重规划。

1. 如果当前轨迹与新出现的障碍物发生碰撞，它将被触发，这将确保在检测到任何碰撞时，新的安全轨迹可用。
2. 其次，在固定的时间间隔内调用规划器。它使用最新的环境信息定期更新轨迹。

采用欧式距离场，只使用增量更新策略更新传感范围内的体素网格。
实验比较：

优化比较:对于后端轨迹优化，我们与之前的工作基于梯度的四旋翼飞行安全轨迹在线生成进行了比较。我们之前的方法和提出的方法都利用了EDF进行非线性优化。

为了公平起见，使用路径搜索给出的相同路径作为初始值。首先，我们比较两种方法的目标函数的代价与时间的关系图(a)。显然，该方法的成本在最初几毫秒内迅速下降，而另一种方法的成本下降得慢得多。其次，平滑度(加速度平方积分)对比如图(b)和表ii所示。尽管所提出的方法所用的时间更短，但得到的轨迹更平滑。