EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors（论文笔记）_ego planner 论文

传统上，基于梯度的规划器依赖于预先构建的ESDF地图来评估梯度大小和方向，并使用数值优化来生成局部最优解。尽管优化程序收敛速度快，但由于预先构造所需的ESDF，它们受到很大的影响。构建ESDF已经成为基于梯度的规划器的瓶颈。

在本文中，设计了一个无esdf的基于梯度的本地规划框架，称为EGO，我们结合了仔细的工程考虑，使其轻量级和鲁棒性。该算法由 基于梯度的样条优化器和 细化后程序 组成。

ESDF欧式符号距离场

欧几里德符号距离场（ESDF）可以很方便地对障碍物进行距离和梯度信息的查询，对空中机器人的在线运动规划具有重要意义。如何快速地生成ESDF地图是进行实时运动规划的重点。

港科大开源一款构建ESDF工具FIESTA（https://github.com/HKUST-Aerial-Robotics/FIESTA ），ESDF地图的每个元素，储存了到最近的障碍物的欧氏距离。

避碰力估算

本文的决策变量为b样条曲线的控制点Q，每个Q都独立拥有自己的环境信息。将在障碍物表面分配一个锚点p_ij，其排斥方向向量v_ij，如图所示

从Qi到第j个障碍物的障碍物距离定义为

基于梯度的轨迹优化

优化问题表述如下：
其中J_s为平滑性惩罚，J_c为碰撞，J_d为可行性。λ_s， λ_c， λ_d为各罚项的权重。

平滑惩罚

平滑惩罚被公式化为时间积分除以导数(加速度、加速度等)的平方。在没有时间积分的情况下惩罚加速度和 jerk ：

障碍惩罚

碰撞惩罚将路径点推离障碍物，这是通过采用安全间隙 s_f 和惩罚控制点d_ij < s_f来实现的。构造了一个二次连续可微的惩罚函数j_c，并随着d_ij的减小抑制其斜率：

具体来说，添加到第i个控制点的成本值为

N_p为属于Q_i的{p, v}_j对的数目。综合所有Qi的成本得到总成本J_c：

公式推导如下：

可行性的惩罚

通过在每个一维上限制轨迹的高阶导数来保证可行性，惩罚函数表达式为：

w_v, w_a, w_j 为每一项的权值，F(·)为控制点高阶导数的二次连续可微度量函数。
其中c_r∈C∈{V_i, A_i, J_i}，选择a₁, b₁, c₁, a₂, b₂, c₂满足二阶连续性，c_m为导数极限，c_j为二次区间和三次区间的分裂点。

数值优化

本文提出的问题有两个方面的特点。

• 首先，目标函数 J 根据新发现的障碍进行自适应调整。它要求解算器能够快速重新启动。
• 其次，二次项在目标函数的表达式中占主导地位，使得J近似二次项。这意味着利用黑森信息可以显著加快收敛速度。

采用准牛顿方法，从梯度信息近似逆海森。（inverse Hessian）

ESDF-free Gradient-based