EGO planner 规划算法流程与ROS仿真
EGO planner 规划算法流程与ROS仿真
EGO planner作为一种先进的路径规划算法，在解决复杂环境下的机器人导航问题上展现出了卓越的能力。本文将深入探讨EGO planner的核心原理、规划算法流程，并通过ROS（Robot Operating System）平台上的实际案例来展示其在仿真环境中的应用效果。
1. 参考资料
- 课程与ppt：移动机器人运动规划-深蓝学院 - 专注人工智能与自动驾驶的学习平台
- 代码链接：EGO-planner
- 论文链接：https://arxiv.org/abs/2008.08835
2. 前言
在机器人技术迅速发展的今天，自主导航已成为实现机器人智能化的关键技术之一。EGO planner作为一种先进的路径规划算法，在解决复杂环境下的机器人导航问题上展现出了卓越的能力。它特别适用于需要高精度避障和动态适应性的场景，如无人机、无人驾驶车辆等。本文旨在深入探讨EGO planner的核心原理、规划算法流程，并通过ROS（Robot Operating System）平台上的实际案例来展示其在仿真环境中的应用效果。希望通过本文能够让读者对EGO planner有更加全面而深刻的理解，并为相关领域的研究人员提供参考价值。
3. FAST-planner与EGO-planner
3.1 两算法的特点
前面已经介绍过FAST planner算法，FAST planner 强调快速响应和计算效率，在时间敏感的任务中表现出色。它利用高效的搜索算法来迅速生成路径，适合需要即时决策的应用场景。尽管FAST planner在速度上有优势，但在高度动态变化的环境中可能需要额外的调整以维持性能。
相比之下，EGO planner 专长于处理动态环境中的局部避障问题，通过结合全局路径规划与局部轨迹优化，能够在复杂场景下保持高效的安全性和路径质量。其自适应机制使得EGO planner能够灵活应对各种障碍物分布情况，增强了系统的鲁棒性。
3.2 两算法的对比
EGO planner是一种基于梯度的局部规划器，与FAST planner相比，最大的不同是它不依赖于ESDF，从而显著减少了计算时间。
ESDF（欧几里得符号距离场）是一种栅格地图，其中每个格子存储着该格子到最近障碍物的距离。从下图右下角的柱状图可以看出，EWOK 和 Fast-Planner 算法都需要较长的时间来构建这个 ESDF 地图。相比之下，Ego Planner 算法则不需要构建 ESDF，从而显著缩短了规划时间。
同时，在生成地图的过程中，系统仅能识别障碍物的表面，而无法感知到障碍物背后的区域或其内部结构，正如示例图所示。这意味着，在利用ESDF进行路径规划优化时，算法倾向于产生一条远离障碍物边缘的安全路径。然而，在某些情况下，比如当上方有障碍物将轨迹向下挤压，同时下方又有障碍物向上推挤时，可能会导致规划出的路径陷入障碍物之间，从而造成路径“卡住”的现象。因此，ESDF不仅面临构建耗时的问题，还存在着对于复杂避障场景处理不够灵活的局限性。
对两个算法进行比较可以得到下表：
4. EGO-plaanner框架
4.1 如何取代ESDF
EGO Planner不使用环境距离场（ESDF）来规划路径。那么，它是如何确保轨迹避开障碍物的呢？
EGO Planner的核心思想是在优化过程中，一旦检测到轨迹与障碍物发生碰撞，系统就会基于碰撞点产生一个反向力，将轨迹从障碍物表面推开，引导其进入无障碍区域。这种方法能够动态响应环境变化，有效避免障碍物，保证路径的安全性。相比ESDF，这种策略更加灵活，适用于需要快速适应环境变化的应用场景。
接下来，我们来看一下轨迹与障碍物发生碰撞后，与碰撞反方向的推力是如何产生的：
首先，检查轨迹上的所有控制点，找出所有位于障碍物内部的控制点及其邻近的控制点（如图左所示）。接下来，使用传统的A*算法，以这两个不在障碍物内的邻近控制点为起点和终点，搜索一条连接这两点且不穿过障碍物的可行路径（如图中蓝色路径所示）。
对于每个位于障碍物内部的点Q，计算其速度方向，并以此方向作为法向量，找到一个垂直于该速度方向并通过点Q的平面（如图中绿色平面所示）。接着，确定这个平面与障碍物表面的交点P（如图中P点所示），并定义由点Q指向点P的方向上的单位向量v，从而得到针对点Q的一个{P, v}对。
接下来，定义障碍物距离 d=(Q−P)。例如，如果要求障碍物距离 d 大于0.6米，而实际测量得到的 Q 点与 P 点之间的距离为0.8米，则此时 Q 点处的 d 值为-0.8米。为了使优化过程逐渐将点Q移动到沿着向量v的方向上，距离P点前方0.6米的位置，可以施加一个
的代价项。这鼓励优化器在每次迭代中调整轨迹，直到满足所需的安全距离。
通过这种方式，EGO Planner能够在发现碰撞后动态地调整轨迹，确保最终生成的路径既安全又平滑。这种方法不仅有效地解决了碰撞问题，还保证了轨迹优化的整体效率和质量。
4.2 总体EGO planner框架
与 Fast Planner大致相似，但有几点不同：
前端规划
Fast Planner：前端采用Hybrid A*算法来生成一条初步的、不与障碍物发生碰撞的轨迹。这种方法虽然能确保初始路径的安全性，但计算复杂度较高。
EGO Planner：不需要一个完全避开障碍物的初始轨迹。它仅需生成一条满足起点和终点状态要求的简单轨迹，即使这条轨迹可能与障碍物相交。这样的设计简化了前端处理，使得EGO Planner能够更快地进入后端优化阶段。
后端轨迹优化
Fast Planner：侧重于直接优化轨迹以达到平滑性和效率，假设前端提供的轨迹已经避开了所有障碍物。
EGO Planner：在每次迭代中都会检查优化后的轨迹是否与障碍物发生碰撞（见图中的“check collision”步骤）。如果检测到碰撞，则向优化问题中加入一个新的排斥力成本项，促使轨迹远离障碍物。这种动态调整意味着每次发现碰撞时都需要重新定义并解决新的优化问题，直到找到一条无碰撞的轨迹为止。
时间重分配
两者都基于控制点使用均匀B样条来生成最终轨迹，并且都需要进行时间重分配以确保轨迹的速度和加速度符合物理约束。然而，EGO Planner与Fast Planner在这方面的具体实现方法存在差异，EGO Planner采用了不同的策略来进行时间上的调整，以适应其独特的优化流程需求。
通过这种方式，EGO Planner能够在保持高效的同时，灵活应对复杂的环境条件，尤其是在需要快速响应变化的情况下表现出色。
5. 轨迹优化
在EGO-planner的设计中，前端的任务是生成一条能够满足给定起点与终点状态要求的基础轨迹。因此，后端的轨迹优化成为了整个规划过程中的关键环节。
由于轨迹采用B样条进行描述，这意味着在构建优化问题时，我们只需要专注于调整控制点Q的位置即可。并且，在EGO-planner框架内，控制点所对应的时间段下标是从1开始计数的，这一点与FAST-planner从0开始不同。
速度、加速度以及jerk（即加速度的变化率）轨迹的控制点可由下式计算：
EGO-planner的目标函数同样由平滑项，碰撞项，动力学三项构成。
平滑项：
与FAST planner中使用相邻点之间的变化来定义平滑度不同，EGO planner使用加速度和加速度变化率来定义平滑度，这会使路径更加平滑。
代价函数：
碰撞项：
由于我们采用生成力的方式来替代ESDF的作用，因此需要重新定义碰撞项。为了确保轨迹不会与障碍物发生碰撞，我们规定障碍物间的最小距离
必须大于预设的安全距离Sf。基于这一条件，我们定义了碰撞成本函数:
由于一个控制点可能碰到多个障碍物，定义控制点的代价值(Np代表该控制点(p,v)对的个数):
所有控制点的代价：
对碰撞项进行优化时，只需对控制点求偏导就可以求出解析解，具体步骤如下：
对控制点求偏导：
将碰撞项成本函数带入上式：
解得：
动力学可行项：
EGO Planner 的动力学可行性约束与 FAST Planner 的理念相似，两者都确保路径在每个维度上的高阶导数不超过设定的限制值。然而，EGO Planner 在此基础之上增加了对 jerk（即加速度变化率）项的考量，因此它不仅检查速度和加速度，还额外关注 jerk，以保证路径更加平滑且符合物理实现的可能性。得益于B样条曲线的特性，EGO Planner 仅需在控制点处进行这些检查，即可确保整个路径的动力学可行性。
通过这种方式，EGO Planner 能够生成既满足严格的动力学限制又具有高度平滑性的路径，而这种增强的检查机制是其相较于 FAST Planner 的一个显著区别。FAST Planner 主要聚焦于快速找到可行解，并通过简化模型来提高计算效率；相比之下，EGO Planner 则更侧重于优化路径的质量，即使这可能会增加一定的计算成本。
其中：
6. 时间重分配
在 FAST Planner 执行时间重分配的过程中，首先将轨迹转换为非均匀B样条表示。随后，算法会检查那些超出预设限制的控制点，并通过迭代方式调整相应的时间区间，以确保轨迹符合动力学约束。然而，如果对轨迹初始状态附近的时间进行调整，可能会导致速度、加速度等高阶导数发生变化，进而引起无人机飞行时的抖动现象。
因此，在EGO Planner中，为了规避非均匀B样条参数化曲线中时间分配对路径上各点速度、加速度及更高阶导数的直接影响，会根据安全轨迹重新生成具有合理时间分配的均匀B样条轨迹。这种方法确保了时间参数化过程不会引起不必要的高阶导数变化，从而避免无人机在飞行过程中可能出现的抖动现象，保证了运动的平滑性和稳定性。
实现框架：
首先，得到合理的时间间隔，然后，以优化后的轨迹作为初值
新轨迹的时间间隔：
检查每个控制点各维度的速度，加速度，jerk是否超出限制，根据超出限制的量定义时间扩展系数：
这样就得到一个合理的时间间隔作为新轨迹的
生成新轨迹：
接下来，我们需要根据新的时间间隔
生成一条新的均匀B样条轨迹。这条新轨迹不仅要紧密贴合初次优化所得的轨迹，还需尽可能满足平滑性要求和运动学约束。
在将初次优化出的轨迹转换为具有重新分配时间的新轨迹时，我们注意到两条轨迹之间的主要差异在于时间间隔和控制点的位置。我们的目标是确定这些关键的控制点，以确保新轨迹既能保持原有的路径特性，又能在时间和空间上符合预期的平滑性和动力学可行性。
那么如何确保新轨迹与旧轨迹在特定时间点上的位置、速度和加速度保持一致，从而实现平滑过渡？以下是详细的过程：
首先，假设最开始的时间节点 t1=0，最终时间点 tm′=re*tm ，其中 re是时间重分配的比例因子。
我们希望新的轨迹 Φf在每一个新扩展的时间 ret上与之前优化出来的轨迹 Φs 在旧的时间 t 上的位置均相同，即
将上述等式展开，代入时间区间中的每个时间，构建如下等式约束：
同时，期望这两个等式的初始和终末状态也是一样的，即在起始点和终点处满足
将这些条件综合整理后，得到了一个闭型最小二乘问题：
通过求解该问题，可以找到一组新的控制点 Q′，使得新轨迹 Φf在每个新扩展的时间点上与旧轨迹 Φs 在旧的时间点上的位置、速度和加速度保持一致，从而实现平滑过渡和动力学一致性。
接着，利用最小二乘的闭式解求解新轨迹的控制点：
在进行第二次优化时，我们采用第一次优化后得到的新轨迹的控制点作为初始解。此时，由于第一次优化已确保生成的轨迹不存在碰撞，因此第二次优化的目标函数中不再包含碰撞检测项，而是引入了曲线拟合项：
在Ego Planner的设计中，我们的目标是在与车辆速度垂直的方向上，确保新规划的轨迹尽可能贴近原始轨迹，而在沿着速度方向上，则允许有更大的灵活性，以换取更平滑的路径。具体来说，如图所示，黑色线条代表原始轨迹，绿色线条代表新生成的轨迹。我们希望最小化垂直于速度方向的位置偏差dr，而对沿速度方向的位置偏差 da 则可以相对宽容。
为了实现这一目标，在计算代价函数时，我们应该增加 dr 相关的代价权重，同时减少 da 的代价权重。这可以通过调整公式中的参数来完成：增大第三个公式分母中与 da 相关的系数 a 的值，减小与 dr相关的系数 b 的值。这样做的结果是，垂直于速度方向上的偏差将被赋予更高的代价，从而促使新轨迹在此方向上更紧密地跟随原轨迹；而在速度方向上的偏差则会因为较低的代价而得到更多的容忍度，有助于创建更加平滑自然的新轨迹。
最后，Ego Planner框架的流程可以总结如下：
首先，前端我们采用一种简化的初始方法，不考虑任何障碍物的影响，生成一条从起点到目标点的平滑B样条曲线作为初步轨迹。这一阶段的目标是获得一个基础解，该解具备良好的平滑性，为后续优化提供一个合理的起点。
接下来，使用均匀B样条对这条初值轨迹进行优化。在此过程中，若检测到控制点与障碍物发生碰撞，则将由此碰撞引入的新成本项纳入优化目标函数中。通过迭代调整，直至找到既无碰撞又满足所有需求的最优解。
完成上述静态轨迹优化后，进入时间重分配阶段。此阶段依然依赖于均匀B样条技术，但重点转向了时间和空间上的同步优化。我们首先计算出新的时间间隔 Δt ，然后利用最小二乘法估算出一组新的轨迹控制点作为初值。随后，针对这组新初值进行进一步优化，确保新轨迹在对应的时间点上尽可能地贴合原有轨迹的位置，并同时保持其平滑性和符合动力学约束条件。这一过程本质上是一个高级曲线拟合问题，旨在实现时空两方面的最佳匹配。
最终，经过这样一系列精心设计的步骤，Ego Planner能够输出一条既安全又高效的行驶轨迹，不仅避开了所有障碍物，而且在动态性能和乘坐舒适度方面都达到了优化标准。
7. ros仿真实现
快速实现
安装库：
sudo apt-get install libarmadillo-dev
下载源码：
git clone https://github.com/ZJU-FAST-Lab/ego-planner.git
编译运行：
cd ego-planner
catkin_make
source devel/setup.bash
roslaunch ego_planner simple_run.launch
标准编译
安装Armadillo：
sudo apt-get install libarmadillo-dev
从 github 或 gitee 克隆代码。这两个仓库会自动同步：
git clone https://github.com/ZJU-FAST-Lab/ego-planner.git //github
git clone https://gitee.com/iszhouxin/ego-planner.git //gitee
编译：
cd ego-planner
catkin_make -DCMAKE_BUILD_TYPE=Release
运行：
在一个终端里，在ego-planner/文件夹下打开 RViz 进行可视化和交互：
source devel/setup.bash
roslaunch ego_planner rviz.launch
然后在另一个终端里，在 ego-planner/ 下运行模拟中的规划器：
source devel/setup.bash
roslaunch ego_planner run_in_sim.launch
选择目标点：