ego-planner个人笔记

写在前面：对这个算法目前是属于完全一脸懵状态，所以本篇博客属于是想到啥写啥，毕竟是个人笔记性质的。

1.名字由来:ESDF（欧几里得符号距离场）-free gradient-based IOcal。所以ego-planner就是基于本地无欧几里得符号距离场的梯度规划器。

ESDF先建图后规划，ego-planner大大减少路径规划时间。

2.使用方法：ros功能包，输入odom位置信息，深度数据，给定目标位置，输出路径点。

3.mavros通过mavlink连接飞控和ros。

4.运行ego-planner会产生的相应内容：

节点1：/uav1_ego_planner_node

位置信息：/mavros/local_position/odom

深度图（根据传感器不同而变）：/d400/depth/image_rect_raw

目标位置（根据用户自己定）：/swarm_terminal_control（这里是网上教程，他用的是键盘输入）

输出（B样条曲线）：用户自己定一般叫bspline

bspline输出给第二个节点处理得到速度加速度偏航角等信息

第三个节点是控制器如LQR，用这些数据再输出到mavros节点

5.实际运行中和仿真需要特别注意光线问题还有下雨问题和强风问题和地面材质。

---

接下来是深入原理部分

1.链接：https://github.com/ZJU-FAST-Lab/ego-planner

2.B样条曲线：用于局部路径规划。B是贝塞尔的意思，样条是以前用木条模拟的方法

（1）贝塞尔曲线相关：n从0开始，n+1是顶点。n+1个顶点称为n阶贝塞尔曲线.缺点是阶数太高的话会出现很多峰谷点，曲线不丝滑。而且一旦顶点确定则无法进行局部修改

（2）B样条对其进行改进，是样条基函数的组合：顶点数是n+1，B样条是k阶

对于B：

阶数关系不太懂，大致意思应该是B样条是有很多小段的贝塞尔组合成的。开B样条曲线就是首尾两点不相连。一般采用3阶，另外B样条的曲率是比较连续的这是最后可实现性的提高

3.ESDF：估计梯度和方向

4.原论文：主要改进是通过将碰撞轨迹与无碰撞引导路径进行比较来制定惩罚函数中的碰撞项。只有当轨迹碰到新的障碍物时，才会存储由此产生的障碍物信息，使规划者只提取必要的障碍物数据。

使用平滑度、碰撞和动态可行性项对轨迹进行优化。与查询预先计算的ESDF的传统方法不同，我们通过将障碍物内的轨迹与引导的无碰撞路径进行比较来对碰撞成本进行建模。然后，我们将力投影到碰撞轨迹上，并生成估计的梯度。

如果违反了限制则重新分配，制定具有各向异性误差惩罚的轨迹拟合问题来生成更平滑的轨迹。

ESDF提供了这种重要的碰撞信息，但代价是计算负担沉重。此外，如图2所示，由于ESDF提供的信息不足甚至错误

样条曲线的凸包特性。使用的均匀B样条

只要所有控制点都在可行性边界框（黑色虚线框）内，整个曲线就保持在该框内。在不失一般性的情况下，每个凸包由四个顶点组成。

B样条的k阶导数仍然是B样条，然后可以算出速度，加速度，加加速度。

引入惩罚有：平滑，碰撞，可行性要使得这给J最小。同时搭配拟牛顿法使得收敛速度加快，好像是基于梯度信息近似逆Hessian的方法，只是因为hessian计算太复杂才用这个。

ego是先给一个无碰撞的路径规划再用B样条和J进行优化。无碰撞的规划用的是A*，他的优点是路径贴近障碍物，所以可以直接选定锚点p。而且

A*只需要二值占据信息： A*算法进行路径搜索时，只需要知道地图中哪些网格/体素是占据（障碍物）的，哪些是空闲（可通行）的。这是一个非常离散、二值化（0或1） 的信息。实际上是需要建个简单的图，但是不需要像传统的方法搞个全局的ESDF，那就计算要求太大。

ESDF-free指的是在B样条优化时。

---

A*算法：

1.引入：最短路径相关：广度优先遍历（BFS）是一个万能的算法。简化问题为格栅，BFS从起点开始向四周探索，四周称为边界，探索的过程中记录下来所有路径直到到达终点。他的探索没有方向性太笨拙，因此产生了像A*一样的启发式搜索。

2.A*的方向决定：选择代价最低的边界，当前代价：即起点到这个格子走过的步数。预估代价：用于指导搜索，这是十分关键的，这个代价是估计出了的从当前格子到终点大概需要多少步。常用的预估代价就是欧式距离：即两点间的直线距离；曼哈顿距离：两点在竖直和垂直方向的距离，他不用开方速度快。

总代价就是把这两个代价加起来用于指导

---

在给出无碰撞的路径之前还需要有一条初始的B样条曲线，他不考虑障碍物，通常采用minimum snap。然后再用A*给出一个条无碰撞的路线用于优化，即设置锚点p和推力v

个人疑问：知道地图上的障碍物信息后用全局搜索算法如A*制定出来的路线无人机不能直接用吗？为什么要建立ESDF？

答：A* 等算法规划出的路径是“离散的”、“拓扑的”，而无人机飞行需要的是“连续的”、“考虑动力学和安全的”轨迹。ESDF 提供了关键的空间梯度信息，使得从前者到后者的转换成为可能，并保证了飞行的安全性和高效性。

ESDF 将二值障碍物地图（有/无障碍）转换成了一个连续的、可微的距离场。它量化了空间中任意一点的安全性（距离障碍物有多远）。ESDF 将二值障碍物地图（有/无障碍）转换成了一个连续的、可微的距离场。它量化了空间中任意一点的安全性（距离障碍物有多远）

利用 ESDF 信息可以轻松地沿着路径膨胀出一个安全的飞行走廊。这确保了整个无人机在走廊内飞行都是安全的。

ego是用排斥力优化路径，传统的是用ESDF优化。

---

ros：

1.ego_planner_node:初始化，函数是egoreplanfsm，一看就知道是一个状态机。到时候可以跳去看看枚举里面有哪些状态

planner_manger：采用minimum snap初始化的话若距离过长可能会导致直线，所以在这个文件里会有个贝塞尔插值的部分

plan_container：会计算一下轨迹时间，看看会不会超时之类

planfromglobraltraj：全局路径的优化（烂尾了，不想写了）

---

minimum snap算法：snap是加加加速度的意思，jerk是加加速度的意思

minimum jerk：最小化 Jerk 的平方沿轨迹的积分 ，机器臂，五阶

minimum snap：最小化 Snap 的平方沿轨迹的积分。无人机，七阶

两者的优化问题在数学形式上非常相似，通常都归结为一个二次规划 (Quadratic Programming, QP) 问题，通过求解一组线性方程来得到最优轨迹（通常是分段多项式，如五次多项式用于 Min Jerk，七次多项式用于 Min Snap）

Min Jerk 让加速度的变化更柔和；Min Snap 让加速度变化的速度更柔和。

如果只有两个点，直接带入求导列方程组求解即可。如果有中间点才需要考虑优化，另外划分多段的方法是采用时间划分，因此上一段的终点一定是下一段的起点，称为连续性约束。

同时求导后的速度加速度加加速度等也要上一段终点等于下一段起点，也是连续性约束。

还有起点终点约束。

既然划分了多段n，那么就有n组不同的系数需要求，拼成一个向量x。每一段的约束也是这个向量x的方程。求解这个方程组得到所有段的系数。

但是划分多段会导致约束条件个数远远小于待求系数个数，这样的方程组是无穷多解的，所以需要最优化，最优化的依据就是jerk/snap平方的积分最小值，平方确保非负性和之前LQR一样。

然后这个积分中的平方可以转化成二次型xTQx，Q是hessian矩阵

---

ego-swarm：swarm是集群的意思

1.在未知环境中部署多个四旋翼飞行器的困难包括但不限于障碍物参数化的非保密性、有限的传感范围、不可靠和带宽有限的通信，以及定位不一致引起的定位漂移。

提出了一种系统的解决方案，在存在上述困难的情况下，使四旋翼飞行器群能够在杂乱的环境中进行高性能飞行。

由拓扑规划和相互碰撞避免两部分组成。

非凸配置空间（即存在局部最优和全局最优，往往优化成问题，梯度法会有问题）可能会导致不期望的行为，例如动态不可行或拥挤导航。因此，采用拓扑规划等策略来规避局部最小值是有益的。

通过在目标函数中加入群体碰撞的加权惩罚，实现了分散的相互碰撞避免。通过将未来时间点的代理分布与正在优化的轨迹进行比较来评估这种惩罚。

为了最大限度地减少数据传输并允许不可靠的通信，使用广播网络来共享轨迹。

为了校正可能增加到半米的相对定位漂移，对目击的代理人的观测结果和轨迹评估的预测进行了比较。感知、规划和控制被集成到机载系统中。

拓扑规划用于逃避局部最小值。

去中心化方法。利用速度障碍物来保证点机器人的无碰撞轨迹

EGO planner将轨迹生成表述为一个非线性优化问题，

ego-swarm在原先A*给定的锚点下重新找一个新的锚点用于创造新的路径

（放弃了，好复杂以后再看会不会补充吧）