Fast Planner——代码解读参考资料整理

Fast Planner 原版程序只适合 ubuntu 18.04，如果需要在 ubuntu 20.04 中运行，需要进行稍微的修改，可参考博客Ubuntu20.04 运行Fast-Planner。

1 地图部分

1.1 EGO-Swarm代码解读-地图部分

参数解读
主要函数解读

1.2 EGO-Swarm代码阅读笔记之GridMap类

1.3 EGO-PLANNER代码阅读（地图部分）

1.4 欧几里得距离转换（EDT）算法

1.5 EDT欧式距离变换

1.6 栅格地图建立-Grid-Mapping

概率栅格地图，概率更新过程的公式推导过程
全概率公式、贝叶斯公式

2 路径搜索部分

2.1 Fast-Planner代码阅读-1. Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight

一. kinodynamic a_star（前端hybrid A_star动力学路径搜索）
1.1启发函数的计算
1.2 Compute shot Traj
1.3 节点扩张
1.4 节点剪枝
1.5 返回kinopath与 getsamples
二、B样条曲线设置
2.1 均匀B样条设置
2.2 B样条函数值计算
2.3 控制点获得-前端路径拟合
2.4 非均匀B样条一阶及二阶微分
2.5 可达性检查
2.6 小结
三、B样条优化
3.1 优化项计算
3.2 优化项结合
3.3 Nlopt优化
3.4 小结
四. 规划系统运行逻辑
4.1 plan_mannager
4.2 kino_replan_fsm.cpp
4.3 流程总结

2.2 【项目解读】fast_planner工程解读

一、规划系统运行逻辑【业务部分】
1.Fast_planner_node.cpp【程序入口】
功能
代码实现
2.kino_replan_fsm.cpp【状态机转移】
功能
代码实现
KinoReplanFSM::init()函数
订阅目标点回调函数
订阅里程计回调函数
【重要】执行状态机FSM定时器回调函数
【重要】路径规划并检查规划是否成功（planner_manager的入口）
【重要】判断目标点是否有障碍物或者是轨迹执行过程中是否有障碍物的回调函数
3.planner_manager.cpp【算法拼接部分】
功能
【核心函数–正常情况的规划】FastPlannerManager::kinodynamicReplan()
【核心函数–航向角yaw规划】FastPlannerManager::planYaw()
为什么要进行航向角yaw规划
核心思想
其他函数接口
二、规划系统算法部分
1.前端kinodynamic A*算法动力学路径搜索
2.后端轨迹优化
（1）Bspline曲线拟合
（2）bspline_opt非线性优化器
3.安全检查线程（safetyCallback）
功能
原理
4.轨迹处理控制指令traj_serve
功能
三、Gazebo仿真环境运行步骤
1.使用激光雷达作为传感器
2.使用RGBD相机作为传感器
总结

2.3 【路径生成–基于运动学的A*图搜索方法】运动学/动力学Kinodynamic A*算法

一、前端kinodynamic A*算法动力学路径搜索的功能
二、步骤一：进行实时采样,离散的获得一些轨迹点（节点point_set，即创建open_list）以及起始点的速度与加速度
1.功能
2.代码实现
三：步骤二：设置算法搜索参数setParam()
四、步骤三：整体的搜索过程search()–包括进行节点扩张-剪枝
1.整体搜索的逻辑流程
手撕示例图
步骤详细解释
2.代码实现
五、步骤四：获取规划得到的路径点getKinoTraj（）
1、功能
2、代码实现
六、关键函数分析【重点】
（1）计算启发代价值H estimateHeuristic（）
1、原理
2、代码实现
（2）计算当前节点的真实的代价G值
（3）计算一条直达曲线computeShotTraj（）
1.原理
2.代码实现
（4）节点扩张、节点剪枝流程
1.节点扩张
2.节点剪枝
总结
参考资料

2.4 Faster Planner——Kinodynamic Astar详解

文中给出了启发成本 Heuristic Cost的详细公式推导，可与KinodynamicAstar::estimateHeuristic( ) 、KinodynamicAstar::computeShotTraj( ) 等函数中代码相对应。

2.5 Optimal Boundary Value Problem (OBVP) 学习笔记

给出了OBVP问题求解的详细推导过程，可与上面一篇及下面一篇文章同时阅读。

2.6 运动规划课程第四次作业——考虑运动约束的路径规划代码全解析

2.7 庞特里亚金最小值原理解最优控制问题

2.8 fast planner代码解析–kino_replan_fsm.cpp

KinoReplanFSM::init()函数
void KinoReplanFSM::waypointCallback()订阅目标点回调函数
void KinoReplanFSM::odometryCallback()订阅里程计回调函数
void KinoReplanFSM::changeFSMExecState() 改变执行状态机FSM函数
void KinoReplanFSM::printFSMExecState()输出执行状态机状态函数
void KinoReplanFSM::execFSMCallback()执行状态机FSM定时器回调函数
void KinoReplanFSM::checkCollisionCallback() 回调碰撞检测函数
路径规划并检查规划是否成功

3 后端优化部分

3.1 Fast-Planner代码阅读-1. Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight

与2.1是同一篇博客。

3.2 贝塞尔曲线，B-样条，非均匀有理B样条梳理

一些关于贝塞尔曲线和B样条曲线的基础概念。

3.3 计算机表示物体形状的基本方法、贝塞尔曲线、B样条曲线、NURBS 曲线、Catmull-Rom 样条曲线

一些关于贝塞尔曲线和B样条曲线的基础概念。

3.4 Fast planner 基本原理学习(一)

部分B样条理论公式推导。以及Fast planner原理架构图。

3.5 代码阅读：使用nlopt和b-spline曲线进行优化

3.6 【轨迹优化–梯度下降】使用nlopt优化库进行路径规划的后端轨迹优化–梯度下降的函数优化使得轨迹形变

4 github中参考代码（有部分注释）

• https://github.com/Mesywang/Fast-Planner-Noted

仓库代码注释于2020年2月，只包含Hybrid A* + 基于梯度的B-Spline优化方案，不包括作者后来提交的版本。

• https://github.com/amov-lab/Prometheus/wiki/%E4%BD%BF%E7%94%A8FastPlanner%E8%BF%9B%E8%A1%8C%E8%BD%A8%E8%BF%B9%E4%BC%98%E5%8C%96

阿木实验室Prometheus项目中关于Fast Planner代码的注释。