史上最详细讲解EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors论文思路和关键代码

EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors
这是一篇来自浙江大学Fast Lab的关于四旋翼无人机轨迹规划的论文。作者提出了一种新颖的局部规划器EGO-Planner，它不需要使用ESDF(Euclidean Signed Distance Field，欧式带符号距离场)就能实现高效的基于梯度的轨迹优化。下面我将逐段解读这篇论文。

1.引言

本文指出目前基于ESDF的梯度优化轨迹规划方法存在的主要问题是计算ESDF的冗余度很大,因为轨迹只会覆盖ESDF很小的一部分。为了解决这个问题,本文的主要贡献有:

1. 提出了一个无需ESDF的基于梯度的规划框架,显著减少了计算时间。
2. 通过将发生碰撞的轨迹与无碰撞参考路径比较来构建碰撞惩罚项。
3. 引入各向异性曲线拟合算法来优化轨迹。
4. 开源了源代码。

传统的基于梯度的规划器依赖预先构建的ESDF地图来评估梯度方向,然后用数值优化得到局部最优解。虽然这种方法优化收敛速度快,但构建ESDF的计算量很大,统计显示ESDF计算占据了70%的处理时间,已经成为制约这类方法的瓶颈。

2.相关工作

作者介绍了目前两类构建ESDF的方法:全局增量式更新和批量局部计算。前者的代表是FIESTA和Voxblox,后者代表工作是Felzenszwalb提出的。但它们都没有针对轨迹本身来构建ESDF,导致很多不必要的计算。

梯度规划方法在无人机局部规划领域应用广泛。早期有CHOMP等算法,但离散时间的轨迹表示不太适合无人机。后来出现了一些基于ESDF和B样条曲线参数化轨迹的方法,包括EWOK,Fast-Planner等,极大地推动了这一领域的发展。但它们都依赖ESDF提供梯度信息。

3.碰撞规避力估计

本文提出的方法不需要使用ESDF,而是通过将轨迹控制点与参考路径比较得到碰撞规避所需的信息。具体来说,轨迹用3阶均匀B样条曲线表示,控制点为{Q1, Q2, …, Q25},对应的节点向量为{t1, t2, …, t28}。两个相邻控制点的初始距离约0.3m。

如果检测到轨迹的某段[tm,tm+1]发生碰撞,则搜索一条无碰撞参考路径Γ。路径搜索采用A*算法。然后为该段的每个控制点Qi分配一个障碍物表面的锚点pij,以及一个由Qi指向pij的单位向量vij。这里i是控制点索引,j是第j个锚点。一个控制点可能对应多个锚点。轨迹Qi到障碍物表面的距离定义为:

$$d_{ij}=(Q_i-p_{ij})\cdot v_{ij}$$

只有当某个控制点Qi距离其所有锚点的距离都大于0,即完全逃离障碍物后,之后检测到碰撞时才会给该控制点添加新的锚点信息。这样可以避免在轨迹还未逃离当前障碍物时,反复地向控制点添加锚点对,减少不必要的计算。

障碍物梯度信息存储在控制点中,而不需要显式地构建ESDF。这种方法更有针对性,可以避免在与局部轨迹无关的区域计算ESDF。同时,它不需要无碰撞轨迹初始化。

4.基于梯度的轨迹优化

根据上一节得到的障碍物锚点信息,可以构建目标函数进行轨迹优化:

$$\underset{Q}{\min }J={\lambda }_s{J}_s+{\lambda }_c{J}_c+{\lambda }_d{J}_d$$

其中Js是平滑项,Jc是碰撞项,Jd是动力学可行性项。作者在Fast-Planner的基础上对J做了一些改进。

Js不再对加速度等做时间积分,而是利用B样条的凸包性质,直接惩罚控制点的加速度和加加速度:

$$J_s=\sum _{i=1}^{N_c-1}||A_i|{|}_2^2+\sum _{i=1}^{N_c-2}||J_i|{|}_2^2$$

Jc的关键是collision cost函数jc的设计。本文设计了一个分段二次连续可微函数:

j c ( i , j ) = { 0 c i j ≤ 0 c i j 3 0 < c i j ≤ s f 3 s f c i j 2 − 3 s f 2 c i j + s f 3 c i j > s f j_c(i,j)=

$$\begin{cases} 0 & c_{ij} ≤ 0\\ c_{ij}^3 & 0 < c_{ij} ≤ s_f\\ 3s_fc_{ij}^2-3s_f^2c_{ij}+s_f^3 & c_{ij} > s_f \end{cases}$$ jc​(i,j)=⎩ ⎨ ⎧​0cij3​3sf​cij2​−3sf2​cij​+sf3​​cij​≤00<cij​≤sf​cij​>sf​​

其中cij=sf-dij,sf是安全距离阈值。Jc是所有控制点cost的累加:

$$J_c=\sum _{i=1}^{N_c}{j}_c(Q_i)=\sum _{i=1}^{N_c}\sum _{j=1}^{N_p}{j}_c(i,j)$$

相比基于ESDF插值得到梯度,本文直接求Jc对Qi的梯度。

∂ J c ∂ Q i = ∑ i = 1 N c ∑ j = 1 N p v i j { 0 c i j ≤ 0 − 3 c i j 2 0 < c i j ≤ s f − 6 s f c i j + 3 s f 2 c i j > s f \frac{\partial J_c}{\partial Q_i}=\sum_{i=1}^{N_c} \sum_{j=1}^{N_p} v_{ij}

$$\begin{cases} 0 & c_{ij} ≤ 0\\ -3c_{ij}^2 & 0 < c_{ij}≤ s_f\\ -6s_fc_{ij}+3s_f^2 & c_{ij}>s_f \end{cases}$$ ∂Qi​∂Jc​​=i=1∑Nc​​j=1∑Np​​vij​⎩ ⎨ ⎧​0−3cij2​−6sf​cij​+3sf2​​cij​≤00<cij​≤sf​cij​>sf​​

Jd用于确保动力学可行性,对每个维度的导数添加约束。利用凸包性质,只需约束控制点的速度、加速度、加加速度小于某个值即可。

基于以上目标函数,本文采用 L-BFGS 进行数值优化。该算法二阶收敛,且空间复杂度与时间复杂度均为O(n)。

5.时间分配优化和轨迹细化

由于初始时对轨迹总时间的分配往往并不准确,可能会导致优化后的轨迹超出动力学限制。因此作者提出先通过重新分配时间,然后再细化轨迹的方法来提高轨迹平滑性。

首先计算轨迹各段速度、加速度、加加速度的超限程度,取其最大值re作为时间分配的延长倍数,生成一条新的B样条曲线。然后构建一个新的目标函数对该曲线进行形状细化:

$$\underset{Q}{\min }{J}^{\prime }={\lambda }_s{J}_s+{\lambda }_d{J}_d+{\lambda }_f{J}_f$$

其中前两项与之前相同,Jf 是拟合项,用各向异性的度量来度量新旧轨迹的差异。如下图所示,计算新轨迹采样点到旧轨迹的轴向距离da和径向距离dr,构建Jf:

J f = ∫ 0 1 [ d a ( α T ′ ) 2 a 2 + d r ( α T ′ ) 2 b 2 ] d α = ∫ 0 1 [ ( ( Φ f − Φ s ) ⋅ Φ ˙ s / ∣ ∣ Φ ˙ s ∣ ∣ ) 2 a 2 + ∣ ∣ ( Φ f − Φ s ) × Φ ˙ s / ∣ ∣ Φ ˙ s ∣ ∣ ∣ ∣ 2 b 2 ] d α

$$\begin{aligned} J_f &= \int_0^1 [\frac{d_a(\alpha T')^2}{a^2} + \frac{d_r(\alpha T')^2}{b^2}] d\alpha\\ &= \int_0^1 [\frac{((\Phi_{f}-\Phi_{s})·\dot\Phi_s/||\dot\Phi_s||)^2}{a^2} + \frac{||(\Phi_{f}-\Phi_{s}) \times \dot\Phi_s/||\dot\Phi_s||||^2}{b^2}] d\alpha \end{aligned}$$ Jf​​=∫01​[a2da​(αT′)2​+b2dr​(αT′)2​]dα=∫01​[a2((Φf​−Φs​)⋅Φ˙s​/∣∣Φ˙s​∣∣)2​+b2∣∣(Φf​−Φs​)×Φ˙s​/∣∣Φ˙s​∣∣∣∣2​]dα​

其中 a、b 分别是椭圆的长轴和短轴。离轴向距离远的地方惩罚力度更大。

6.实验结果

作者在仿真和实物上进行了大量实验。首先对比了 BB、L-BFGS、截断牛顿法三种优化算法,发现 L-BFGS 的成功率和计算时间表现最好。

然后作者分别用带ESDF和不带ESDF的方法进行轨迹优化,发现不带ESDF的方法成功率与带ESDF但有碰撞检查的效果相当,但轨迹能量略高。不过省去了ESDF更新的时间后,总耗时降低了一个数量级。

最后作者将本文算法与SOTA方法 Fast-Planner 和 EWOK 进行了对比,发现本文算法可以在更短的飞行时间内完成任务,且不需要ESDF,大幅减少了计算开销。实物实验也验证了本文算法的实用性。

7.结论与未来工作

本文针对当前 ESDF 在梯度优化中应用的弊端,提出了一种高效的ESDF-free 轨迹优化方法EGO-Planner。该方法用锚点表示障碍物信息,构建了平滑、避障和动力学约束的目标函数,并结合轨迹细化技术,最终实现了与SOTA方法相媲美但计算量更小的效果。未来作者拟进一步提高算法的鲁棒性和实时性,拓展其在动态环境下的应用。

8.应用示例

本文开源了基于ROS的EGO-Planner,其应用框架如下

其中Traj Server接收目标点和点云,生成最优轨迹并发送至Drone Server。Mapping 模块用于维护环境栅格地图。整个系统可以在机载计算机如Jetson TX2上实时运行。

以下是一些关键代码:

碰撞检测与锚点添加

void checkAndAddCollision(vector<Cube>& cubes, const vector<Vector3d>& pts) 
{
    for (auto pt : pts) {
        for (auto& cube : cubes) {
            if (cube.isInCube(pt)) {
                auto path = searchPath(cube);  // A*搜索
                // 添加锚点和向量
                for (int i = cube.last_pos; i <= cube.first_pos; ++i) {
                    for (auto v : path)  
                        cube.obs_pairs.emplace(pt, v);
                }  
                break;
            }
        }
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

目标函数构建

double BsplineOptimizer::costFunction(const vector<Vector3d>& points, 
                                      const vector<Vector3d>& start, 
                                      const vector<Vector3d>& end) {
    double cost = 0.0;

    // 平滑项
    cost += lambda_s * smoothnessCost(points);

    // 碰撞项
    cost += lambda_c * collisionCost(points);

    // 动力学约束项
    cost += lambda_d * feasibilityCost(points);

    return cost;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

L-BFGS优化

void lbfgsOptimize(vector<Vector3d>& points) {
    lbfgs::lbfgs_parameter_t lbfgs_params;
    lbfgs::lbfgs_load_default_parameters(&lbfgs_params);
    lbfgs_params.mem_size = 16;  // 设置缓存对数

    auto fn = [&](const vector<Vector3d>& x, vector<Vector3d>& grad) {
        double fx = 0.0;
        costFunction(x, grad, fx);
        return fx;
    };

    int n = 3 * points.size();
    lbfgs::lbfgs(n, &points[0][0], &J, fn, nullptr, &lbfgs_params);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14

这个开源项目提供了EGO-Planner核心部分的代码,包括构建ESDF-free梯度信息、目标函数、L-BFGS优化、时间分配和轨迹细化等模块,非常适合学习论文算法的实现。同时它基于ROS,具有良好的系统集成能力,有利于实际部署应用。

总结

本篇论文针对目前 ESDF 在无人机轨迹优化中存在的冗余计算问题,提出了一种高效的 ESDF-free 方法EGO-Planner。通过只存储轨迹实际经过的障碍物信息,构建了考虑平滑、避障和动力学约束的目标函数,并应用 L-BFGS 进行数值优化,最终生成高质量的轨迹。同时还引入轨迹细化技术提高了轨迹可行性。大量实验表明,该方法大幅降低了计算开销,具有很高的工程实用价值。作者开源的项目代码,极大方便了算法的学习和应用。这是一篇在无人机局部规划领域非常有价值的工作。

Q&A

L-BFGS

L-BFGS 是 Limited-memory BFGS 的缩写,是一种拟牛顿优化算法。它通过近似海森矩阵的逆矩阵,实现了二阶收敛速度,但空间复杂度只有 O(n),是求解大规模非线性优化问题的常用算法。

在上面的代码中,lbfgsOptimize 函数实现了用 L-BFGS 算法优化 B 样条曲线控制点的过程。它主要涉及到论文中第 4 节的内容,与目标函数的构建和求解相关。

首先,代码设置了 L-BFGS 的相关参数,尤其是 mem_size,即算法存储的历史梯度对数。这决定了 L-BFGS 对海森矩阵的近似精度。论文里提到 L-BFGS 优化的空间复杂度为 O(n),其中 n 为优化变量维度,这里 n = 3 × N_c。3 表示三维空间,N_c 为控制点数量。

然后定义了目标函数 fn,其内部调用了 costFunction,计算当前轨迹的各项 cost 及梯度。costFunction 对应论文公式(3):

$$\underset{Q}{\min }J={\lambda }_s{J}_s+{\lambda }_c{J}_c+{\lambda }_d{J}_d$$

其中 J_s 表示平滑项,J_c 为避障项,J_d 为动力学可行性项。这三项分别对应论文公式(4)、(6)、(8):

$$J_s=\sum _{i=1}^{N_c-1}||A_i|{|}_2^2+\sum _{i=1}^{N_c-2}||J_i|{|}_2^2$$

$$J_c=\sum _{i=1}^{N_c}{j}_c(Q_i)=\sum _{i=1}^{N_c}\sum _{j=1}^{N_p}{j}_c(i,j)$$

$$J_d=\sum _{i=1}^{N_c}{w}_{v}F(V_i)+\sum _{i=1}^{N_c-1}{w}_{a}F(A_i)+\sum _{i=1}^{N_c-2}{w}_{j}F(J_i)$$

fn 的输入 x 为展平的控制点坐标向量,grad 为 cost 相对于控制点坐标的梯度。L-BFGS 优化过程中,会多次调用 fn,近似估计海森矩阵,并迭代更新 x,最终使目标函数达到局部极小。这里梯度的计算用到了论文公式(7)。

可见,这段代码紧密结合了论文第 4 节的各个公式,体现了 EGO-Planner 的核心思想——构建 ESDF-free 的梯度目标函数,并用高效的数值优化算法进行求解,最终得到光滑、安全和动力学可行的轨迹。代码基于 C++ 模板库实现,具有很高的执行效率。

各向异性曲线拟合算法(Anisotropic Curve Fitting)

各向异性曲线拟合算法(Anisotropic Curve Fitting)是本文提出的一种轨迹细化方法,用于在时间重新分配后调整轨迹,使其更加光滑,同时尽可能保持原有的形状特征。

-算法动机

当检测到初始轨迹的动力学约束(速度、加速度等)超限时,需要重新分配轨迹的时间,通常是延长总时间。简单地将轨迹在时间维度上"拉长"虽然可以降低速度和加速度,但会改变轨迹形状,使其偏离原来的避障路径。因此需要在调整时间的同时,对轨迹形状进行细化,以保持避障特性。

-算法原理

本文受到各向异性材料的启发,提出赋予轨迹不同方向以不同的"刚度"。具体来说,将轨迹上每一点相对原轨迹的偏差分解为切向(axial)和法向(radial)两个方向,如下图所示:

切向偏差 d_a 代表了由时间调整引起的形状改变,法向偏差 d_r 代表了空间位置的偏移。在优化目标函数时,d_a 和 d_r 应当被赋予不同的权重。一方面,为使轨迹满足动力学约束,需容忍一定程度的切向变形;另一方面,为保证安全,要严格控制轨迹在法向上的偏移,使其不会偏离避障区域。

因此,作者构建了一个各向异性度量(anisotropic metric)来定义轨迹偏差,如上图所示的椭球面。椭球的长轴 a 和短轴 b 分别与切向和法向对齐,反映了它们受到不同的"惩罚力度"。椭球面上的任意一点到中心的距离都对应同样大小的偏差代价。

数学上,轨迹细化的目标函数修改为:

$$J^{\prime }={\lambda }_s{J}_s+{\lambda }_d{J}_d+{\lambda }_f{J}_f$$

其中 J_f 为轨迹拟合项(fitness cost),定义为(公式18):

$$J_f={\int }_0^1\left[(\frac{d_a(\alpha T^{\prime })}{a}{)}^2+(\frac{d_r(\alpha T^{\prime })}{b}{)}^2\right]\mathrm{d}\alpha$$

T’ 为调整后的时间,α 为归一化的时间参数。控制短轴 b 远小于长轴 a,可以使轨迹在法向方向上的偏移受到更大的惩罚。

通过引入各向异性度量,可以在时间调整过程中灵活控制轨迹变形,在满足动力学约束的同时最大限度保持轨迹形状,从而兼顾了运动的可行性和安全性。

-算法实现

以下是轨迹细化中关键的拟合代价计算函数:

/* 计算拟合代价 */
double BsplineOptimizer::fitnessCost(const Eigen::MatrixXd &control_points, double ratio) 
{
    double cost = 0;
    int end_idx = control_points.cols() - order_;

    for (int i = 0; i < end_idx; ++i) {
        Eigen::Vector3d pt = evaluateCurve(control_points, i, 0.0);
        Eigen::Vector3d pt_o = evaluateCurve(origin_control_points_, i, 0.0);
        Eigen::Vector3d vel, vel_o;
        evaluateVel(control_points, i, 0.0, vel);
        evaluateVel(origin_control_points_, i, 0.0, vel_o);
        double cos_theta = vel.dot(vel_o) / vel.norm() / vel_o.norm();
        double theta = acos(cos_theta);
        double dist = (pt - pt_o).norm();
        double d_a = dist * cos_theta;
        double d_r = dist * sin(theta);
        cost += pow(d_a/ratio, 2) + pow(d_r, 2);  // 轴向和径向距离的加权平方和
    }
    return cost;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

其中 origin_control_points_ 为初始轨迹的控制点,ratio 为时间延长系数。

算法流程如下:

1. 在新旧轨迹上等间隔取 N 个采样点,两两配对。
2. 对每一对采样点:
   • 计算它们之间的欧氏距离 dist。
   • 计算新轨迹在该点处的速度方向(切向)vel。
   • 计算该切向与连接两点的向量之间的夹角 theta。
   • 将 dist 分解为轴向距离 d_a 和径向距离 d_r。
3. 计算所有配对点的 d_a 和 d_r 的加权平方和作为拟合代价。

可以看到,通过 cos_theta 和 sin_theta,dist 被分解为了轴向和径向两个分量。轴向分量还除以了 ratio 进行归一化。最终它们的加权平方和作为拟合代价,其中径向分量的权重更大(默认为1,而轴向分量权重为1/ratio^2)。

详解EGO-Planner整个规划流程

我将结合论文和代码详细讲解EGO-Planner的避障流程。

EGO-Planner的避障主要包括两个部分:ESDF-free的梯度信息计算和基于梯度的轨迹优化。当出现新的障碍物时,规划器通过添加碰撞代价项并重新优化来生成一条新的无碰撞轨迹。下面我按照流程分步讲解。

1. 初始轨迹生成

首先,EGO-Planner会生成一条从起点到终点的初始轨迹。这条轨迹通常是一条简单的直线或曲线,不考虑障碍物。轨迹由 p 阶 B 样条曲线参数化,包含 N 个控制点。

void initControlPoints(Eigen::MatrixXd &init_points, int num) 
{
    init_points.resize(3, num);
    Eigen::Vector3d start_pt = start_pos_;
    Eigen::Vector3d end_pt = end_pos_;
    double t = 1.0 / (num - 1);
    for (int i = 0; i < num; ++i) {
        init_points.col(i) = (1 - t * i) * start_pt + t * i * end_pt;
    }
}
1
2
3
4
5
6
7
8
9
10

2. 碰撞检测

然后,规划器会对这条初始轨迹进行碰撞检测。具体做法是uniformly地在轨迹上采样 M 个点,检查这些点是否与障碍物发生碰撞。如果发生碰撞,则记录轨迹上的碰撞区间,将该区间内的控制点标记为 in-collision。

碰撞检测相关代码如下:

bool BsplineOptimizer::isInCollision(Eigen::MatrixXd &control_points) 
{
    int end_idx = control_points.cols() - order_;
    double dt = 1.0 / (K_ - 1);  // K为碰撞检测采样点数
    Eigen::VectorXd t_vec = Eigen::VectorXd::LinSpaced(K_, 0, 1);
    for (int i = 0; i < K_; ++i) {
        for (int j = 0; j < end_idx; ++j) {
            Eigen::MatrixXd pts = control_points.block(0, j, 3, order_+1).transpose();  // 提取order_+1个控制点
            Eigen::VectorXd bas = getBasicCoeff(t_vec(i), j);  // 计算样条基函数系数
            Eigen::Vector3d pt = bas.dot(pts);                 // 计算该参数下B样条曲线坐标
            if (checkCollision(pt, obs_distance)) 
                return true;
        }
    }
    return false;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

3. 构建碰撞代价函数

对于每个标记为 in-collision 的控制点 Q_i,规划器搜索一条从该控制点到最近障碍物表面的路径 Γ_i(使用 A* 算法)。然后在Γ_i 上均匀采样 K 个点 {p_ik},以及相应的梯度方向 {v_ik}(从 Q_i 指向 p_ik)。这些信息将用于构建该控制点的碰撞代价。

碰撞代价函数定义如下(公式 5):

j c ( i , k ) = { 0 c i k ≤ 0 c i k 3 0 < c i k ≤ d s 3 d s c i k 2 − 3 d s 2 c i k + d s 3 c i k > d s j_c(i,k)=

$$\begin{cases} 0 & c_{ik} ≤ 0\\ c_{ik}^3 & 0 < c_{ik} ≤ d_s\\ 3d_sc_{ik}^2-3d_s^2c_{ik}+d_s^3 & c_{ik} > d_s \end{cases}$$ jc​(i,k)=⎩ ⎨ ⎧​0cik3​3ds​cik2​−3ds2​cik​+ds3​​cik​≤00<cik​≤ds​cik​>ds​​

其中 c_ik = d_s - d_ik 为安全距离 d_s 与控制点到障碍物的 signed distance d_ik 之差。d_ik 可以很容易地通过 Q_i,p_ik,v_ik 计算得到(公式 1):

$$d_{ik}=(Q_i-p_{ik})\cdot v_{ik}$$

每个控制点的总碰撞代价为其所有采样点代价之和(公式 6):

$$J_c=\sum _{i=1}^{N_c}\sum _{k=1}^K{j}_c(i,k)$$

这部分的关键代码如下:

/* 搜索参考路径,并记录路径点和梯度方向 */
void BsplineOptimizer::setPosGrad(const Eigen::MatrixXd &control_points, int i, int j) 
{
    Eigen::Vector3d pt = control_points.col(i);
    Eigen::Vector3d dir = pt - (pt - vel_dir_ * delta_); // vel_dir_为当前速度方向
    std::vector<Eigen::Vector3d> path_pts, grad_dirs;
    searchPath(pt, dir, obs_distance, path_pts, grad_dirs); //通过A*搜索参考路径点和梯度方向
    pos_(j).assign(path_pts.begin(), path_pts.end());
    grad_(j).assign(grad_dirs.begin(), grad_dirs.end());
}

/* 碰撞代价函数 */
double BsplineOptimizer::collisionCost(const Eigen::MatrixXd &control_points) 
{
    double cost = 0;
    int end_idx = control_points.cols() - order_;

    for (int cp_id = 0; cp_id < end_idx; ++cp_id) {
        for (auto i = 0; i < pos_[cp_id].size(); ++i) {  // 遍历该控制点的所有参考点
            Eigen::Vector3d dist_vec = control_points.col(cp_id) - pos_[cp_id][i];  // Q_i - p_ik
            double dist = dist_vec.dot(grad_[cp_id][i]);   // 投影距离 (Q_i - p_ik)·v_ik                
            double c = d_s - dist;
            if (c > 0) {          
                if (c <= d_s)           // L2 penalty
                    cost += pow(c, 3);
                else                    // L1 penalty
                    cost += 3.0 * d_s * pow(c, 2) - 3.0 * pow(d_s, 2) * c + pow(d_s, 3);
            }
        }
    }
    return cost;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

4. 梯度计算与轨迹优化

有了碰撞代价函数,就可以求解其相对于控制点的梯度(公式 7)。结合其他目标项如平滑项(公式 4)、动力学可行性项(公式 8),即可构建完整的目标函数(公式 3):

$$\underset{Q}{\min }J={\lambda }_s{J}_s+{\lambda }_c{J}_c+{\lambda }_d{J}_d$$

其中Q为所有控制点构成的向量。使用 L-BFGS 优化算法对目标函数进行优化,就可以得到一条新的无碰撞轨迹。L-BFGS 利用历史梯度信息近似目标函数的 Hessian 矩阵,可以快速收敛到最优解。

相关代码如下:

/* 计算碰撞代价函数梯度 */
void BsplineOptimizer::collisionGradient(const Eigen::MatrixXd &control_points, 
                                         Eigen::MatrixXd &gradByCollision) 
{
    gradByCollision = Eigen::MatrixXd::Zero(3, N_);
    int end_idx = control_points.cols() - order_;
    for (int cp_id = 0; cp_id < end_idx; ++cp_id) {
        for (auto i = 0; i < pos_[cp_id].size(); ++i) {
            Eigen::Vector3d dist_vec = control_points.col(cp_id) - pos_[cp_id][i];
            double dist = dist_vec.dot(grad_[cp_id][i]);
            double c = d_s - dist;
            if (c > 0) {  
                if (c <= d_s)
                    gradByCollision.col(cp_id) += -3.0 * pow(c, 2) * grad_[cp_id][i];
                else
                    gradByCollision.col(cp_id) += (6.0 * d_s * c - 3.0 * pow(d_s, 2)) * grad_[cp_id][i];
            }
        }
    }
}

/* L-BFGS 优化 */
void BsplineOptimizer::optimize(Eigen::MatrixXd &control_points)
{
    lbfgs::lbfgs_parameter_t lbfgs_params;
    lbfgs::lbfgs_load_default_parameters(&lbfgs_params);
    lbfgs_params.mem_size = 16;

    auto costFun = [&](const Eigen::MatrixXd &x, Eigen::MatrixXd &grad) {
        // 计算平滑代价和梯度
        double smCost = smoothnessCost(x);
        smoothnessGradient(x, grad);

        // 计算碰撞代价和梯度
        double clCost = collisionCost(x);
        Eigen::MatrixXd gradByCollision;
        collisionGradient(x, gradByCollision);
        grad += lambda_c * gradByCollision;

        // 计算可行性代价和梯度
        double fsCost = feasibilityCost(x);
        feasibilityGradient(x, grad);

        return smCost + lambda_c * clCost + lambda_f * fsCost;
    };

    lbfgs::lbfgs_optimize(control_points, costFun, &lbfgs_params);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

5. 时间分配与轨迹细化

得到无碰撞轨迹后,规划器会检查该轨迹的动力学可行性,即速度、加速度等是否超出限制。如果不满足,则需要重新分配轨迹时间,并细化轨迹。

分配新的时间时,规划器会计算当前轨迹的超限比例系数 r_e(公式 15),然后将总时间延长 r_e 倍。具体做法是将B样条曲线的节点向量的间隔拉长r_e 倍。

然后,规划器会生成一条与原始轨迹形状相似,但更加平滑的轨迹。这是通过最小化新轨迹与原轨迹之间的各向异性距离实现的(公式 18)。该距离由轴向距离和径向距离组成,分别施加不同的权重。这样可以在保持避障的同时,提高轨迹的光滑性。

时间分配和轨迹细化的代码如下:

/* 计算超限比例系数 */
double BsplineOptimizer::checkTrajectoryCost(const Eigen::MatrixXd &control_points, double &ratio) 
{
    int end_idx = control_points.cols() - order_;
    double max_vel_ratio = 0, max_acc_ratio = 0;
    for (int i = 0; i < end_idx; ++i) {
        Eigen::Vector3d vel, acc;
        evaluateVelAcc(control_points, i, vel, acc);
        max_vel_ratio = std::max(max_vel_ratio, vel.norm() / max_vel_);
        max_acc_ratio = std::max(max_acc_ratio, acc.norm() / max_acc_);
    }
    ratio = std::max(max_vel_ratio, sqrt(max_acc_ratio));
    return ratio > 1.0 ? INF : 0;
}

/* 重新分配时间 */
void BsplineOptimizer::reallocateTime(Eigen::MatrixXd &control_points, double ratio) 
{
    int num_cp = control_points.cols();
    int num_pt = num_cp - 2 * order_ + 2;
    double tm, tmp, new_tm;
    Eigen::MatrixXd ctrl_pts_new = control_points;

    for (int i = 1; i < num_pt; ++i) {
        tm = (double)i / (num_pt - 1);
        tmp = (double)(i - 1) / (num_pt - 1);
        new_tm = (tm + tmp) / 2.0 * ratio;  // 延长 ratio 倍
        Eigen::MatrixXd pts_l = control_points.block(0, i-1, 3, order_+1).transpose();
        Eigen::MatrixXd pts_u = control_points.block(0, i, 3, order_+1).transpose();
        Eigen::VectorXd basis_l = getBasicCoeff(new_tm, i-1);
        Eigen::VectorXd basis_u = getBasicCoeff(new_tm, i);
        Eigen::Vector3d pt_on_curve_l = basis_l.dot(pts_l);
        Eigen::Vector3d pt_on_curve_u = basis_u.dot(pts_u); 
        double ratio_l = (pt_on_curve_u - pt_on_curve_l).norm() / (pts_u.row(order_) - pts_l.row(0)).norm();
        for (int j = i; j < i+order_; ++j) {
            ctrl_pts_new.col(j) = ratio_l * control_points.col(j-1) + (1-ratio_l) * control_points.col(j);
        }
    }
    control_points = ctrl_pts_new;
}

/* 轨迹细化 */
void BsplineOptimizer::refineTrajectory(Eigen::MatrixXd &control_points, double ratio) 
{
    int num_cp = control_points.cols();
    int num_pt = num_cp - 2 * order_ + 2;
    Eigen::MatrixXd ctrl_pts_new = control_points;

    auto costFun = [&](const Eigen::MatrixXd &x, Eigen::MatrixXd &grad) {
        // 构建新的平滑、避障、动力学可行的目标函数,类似之前
        double smCost = smoothnessCost(x);
        smoothnessGradient(x, grad);

        Eigen::MatrixXd gradByCollision;
        collisionGradient(x, gradByCollision);
        grad += lambda_c * gradByCollision;

        double fsCost = feasibilityCost(x);
        feasibilityGradient(x, grad);

        // 计算与原轨迹的拟合代价和梯度
        double ftCost = fitnessCost(x, ratio);  
        fitnessGradient(x, grad, ratio);

        return smCost + lambda_c * collisionCost(x) + lambda_f * fsCost + lambda_ft * ftCost;
    };

    lbfgs::lbfgs_optimize(ctrl_pts_new, costFun, &lbfgs_params);
    control_points = ctrl_pts_new;
}

/* 计算拟合代价 */
double BsplineOptimizer::fitnessCost(const Eigen::MatrixXd &control_points, double ratio) 
{
    double cost = 0;
    int end_idx = control_points.cols() - order_;

    for (int i = 0; i < end_idx; ++i) {
        Eigen::Vector3d pt = evaluateCurve(control_points, i, 0.0);
        Eigen::Vector3d pt_o = evaluateCurve(origin_control_points_, i, 0.0);
        Eigen::Vector3d vel, vel_o;
        evaluateVel(control_points, i, 0.0, vel);
        evaluateVel(origin_control_points_, i, 0.0, vel_o);
        double cos_theta = vel.dot(vel_o) / vel.norm() / vel_o.norm();
        double theta = acos(cos_theta);
        double dist = (pt - pt_o).norm();
        double d_a = dist * cos_theta;
        double d_r = dist * sin(theta);
        cost += pow(d_a/ratio, 2) + pow(d_r, 2);  // 轴向和径向距离的加权平方和
    }
    return cost;
}

/* 计算拟合代价梯度,过程类似碰撞代价梯度 */
void BsplineOptimizer::fitnessGradient(const Eigen::MatrixXd &control_points, 
                                       Eigen::MatrixXd &grad, double ratio)
{
    grad = Eigen::MatrixXd::Zero(3, N_);
    int end_idx = control_points.cols() - order_;

    for (int i = 0; i < end_idx; ++i) {
        // 中间步骤省略,计算d_a和d_r相对控制点的梯度
        grad.col(i) += 2.0 * (d_a/pow(ratio,2)) * grad_d_a.col(i) 
                     + 2.0 * d_r * grad_d_r.col(i);
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106

在轨迹细化的目标函数中,除了平滑项、避障项和可行性项之外,还引入了一个新的拟合项(fitness cost)。该项使得新轨迹与原轨迹的形状尽可能接近,同时两者之间的偏差由轴向距离d_a和径向距离d_r 来衡量(公式17,18)。

轴向距离 d_a 是新旧轨迹上对应点沿原轨迹切向的距离,径向距离 d_r 是垂直于切向的距离。由于 d_a 受到时间分配的影响,因此还要除以延长比例 ratio 来归一化。

优化时,d_a 和 d_r 的加权平方和被作为拟合代价。权重的选取使得径向偏差受到更大的惩罚,从而保证避障的同时尽可能贴近原轨迹。

以上就是 EGO-Planner 避障及轨迹优化的完整流程和关键代码实现。总结一下其优点:

1. 不需要显式构建ESDF,大大降低了计算复杂度,提高了规划效率。

2. 直接从参考路径中采样得到梯度信息,避免了ESDF信息不全导致局部最小值的问题。

3. 通过L-BFGS优化平滑、避障、动力学约束等多个目标项,得到高质量轨迹。

4. 引入时间分配和轨迹细化策略,在避障保形的同时提高了轨迹的光滑性和动力学可行性。

5. 采用 B 样条参数化轨迹,利用了其局部支撑性和凸包性等优良特性。

6. 各部分均提供了详细的公式推导和参考代码,具有很强的可复现性。

希望这个详细的讲解能够帮助你理解 EGO-Planner 的核心思想和实现细节。