EGOSwarm—Prometheus代码阅读笔记【四】——EGOPlannerManager类实现

Prometheusv2中的EGOSwarm代码阅读笔记，个人学习用。
目前还有部分代码没看明白，欢迎大家交流！
EGOPlanner—Prometheus代码阅读笔记汇总

主要参考资料：
Fast Planner——代码解读参考资料整理
EGO-Swarm代码阅读笔记汇总
代码来源：
https://github.com/ZJU-FAST-Lab/ego-planner
https://github.com/amov-lab/Prometheus

---

---

（二）EGOPlannerManager类【规划管理】

1.源文件位置

ego_planner_swarm\plan_manage\src\planner_manager.cpp

2.主要功能

• 整个规划过程进行管理：响应FSM层，EGOReplanFSM接口；调用算法层计算，实现算法拼接

3.成员变量

4.成员函数

EGOPlannerManager::initPlanModules

通过advanced_param.xml传入变量。
初始化GridMap类、BsplineOptimizer类和PlanningVisualization类

EGOPlannerManager::reboundReplan【重要】

路径规划模块，调用算法层规划从start_pt–>local_target_vel的路径
如果始末点距离小于0.2，则continous_failures_count_++ 并返回false
得到全局/局部轨迹的路径点，将轨迹转化为均匀B样条曲线，得到紧邻障碍物的控制点，使用A*得到反向/正向牵引点和牵引方向，对正/反牵引点进行组合，得到不同安全轨迹控制点，使用L-BFGS优化控制点，得到最优轨迹控制点

---

Step1：初始化
如果第一次进入循环或flag_polyInit或flag_force_polynomial，
1.则规划初始点到局部重点的多项式轨迹。分配的时间为以最大加速度和速度飞行所需的时间。如果flag_randomPolyTraj==false ，则调用PolynomialTraj::one_segment_traj_gen 计算出始末两点的多项式轨迹；否则在始末位置之间随机插入一个点，调用PolynomialTraj::minSnapTraj() 计算出轨迹。
将得到的轨迹以ts=ctrl_pt_dist/max_vel_ 为时间间隔分段，如果分段的距离大于1.5倍定义的控制点距离ctrl_pt_dist ，则ts/=1.5 再继续分，直到分段的距离都小于1.5倍定义的控制点距离ctrl_pt_dist 并且分段点的数量大于7。最后将轨迹的始末速度和加速度赋值给start_end_derivatives 。
2. 否则将局部路径local_data_ 以时间间隔ts分段，将分段距离累加赋值到pseudo_arc_length，并将分段点保存到segment_point，如果pseudo_arc_length为空，则continous_failures_count_++ 并返回false。poly_time 为local_data_ 结束点到局部目标local_target_pt 以max_vel_/2 的速度行驶的时间。如果该时间大于ts ，则用PolynomialTraj::one_segment_traj_gen() 规划从local_data_ 结束点到局部目标local_target_pt 的轨迹gl_traj 。接着在gl_traj上等时间间隔ts的距离累加赋值到pseudo_arc_length 并将点保存到segment_point 。
令cps_dist =ctrl_pt_dist1.5接着对于segment_point 中每两个相邻点，如果距离大于cps_dist ，则在这两点间加入几个点，使得相邻两点距离小于等于cps_dist 。如果最终路径点的个数小于7，则减小cps_dist 重新加点，直到路径点的个数大于等于7。最后将local_data_现在的速度加速度和局部终点的速度加速度压入start_end_derivatives 。如果路径点的个数大于planning_horizen_/pp_.ctrl_pt_dist3 则重新使用规则1 进行初始化。
-------------------------------路径点、始末速度加速度设置完成------------------
调用UniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts)得到控制点ctrl_pts。
调用bspline_optimizer_->initControlPoints(ctrl_pts,true)得到调整后的障碍物控制点对segments（障碍物两边紧挨的控制点对，再向两边扩展后得到调整后的障碍物控制点）

---

Step2：优化
1.如果use_distinctive_trajs==true，调用bspline_optimizer_->distinctiveTrajs(segments)得到多条安全控制点集trajs。
对于每一条控制点集，调用bspline_optimizer_->BsplineOptimizeTrajRebound(ctrl_pts_temp, final_cost, trajs[i], ts)得到该条最优路径的costfinal_cost和优化后的控制点ctrl_pts_temp。最后得到最小cost的控制点ctrl_pts
2. 给定时间间隔，调用BsplineOptimizer::BsplineOptimizeTrajRebound(ctrl_pts, ts) 优化得到最优的控制点ctrl_pts。
如果优化不成功，continous_failures_count_++ 返回false。定义新的均匀B样条类pos=UniformBspline(ctrl_pts,3,ts) 并设置动力学约束。

---

Step3：重新分配时间
swarm里没有使用

---

Step4：保存结果
调用updateTrajInfo(pos,ros::Time::now())更新local_data_
continous_failures_count_清零，优化成功，返回true。

EGOPlannerManager::EmergencyStop

规划出一条在stop_pos处停止的三阶均匀B样条轨迹并赋值给local_data_

EGOPlannerManager::checkCollision

以0.03秒为步长，通过遍历的方法，判断当前无人机的轨迹是否会和 uav_id 架无人机的距离小于安全距离

EGOPlannerManager::planGlobalTrajWaypoints

把start_pos和waypoints压入points ，如果路标点间距太大，则在两个路标点间等间隔插入几个点。之后对新的路标点每段赋予时间（以最大速度飞行，始末段分配的时间为2倍原来的时间）。然后，如果路标点大于等于3，则用PolynomialTraj::minSnapTraj minimum snap方法计算出连续轨迹，否则使用PolynomialTraj::one_segment_traj_gen多项式拟合计算出连续轨迹。最后使用GlobalTrajData::setGlobalTraj 将轨迹和开始时间赋值给global_data_

EGOPlannerManager::planGlobalTraj

和planGlobalTrajWaypoints类似只不过这里只有始末两个点，调用的是PolynomialTraj::one_segment_traj_gen多项式拟合计算出连续轨迹。最后使用GlobalTrajData::setGlobalTraj 将轨迹和开始时间赋值给global_data_

EGOPlannerManager::refineTrajAlgo

调用reparamBspline() 将traj 的时间延长为原来的ratio倍，得到新的控制点ctrl_pts 、新的时间间隔ts 和新轨迹增加的时间t_inc 。之后将traj初始化为UniformBspline(ctrl_pts,3,ts) 类。在traj上等时间间隔取ctrl_pts-3 个点，调用BsplineOptimizer::BsplineOptimizeTrajRefine()（拟合项、光滑项、动力学可行项）来得到优化后的控制点optimal_control_points

EGOPlannerManager::updateTrajInfo

把position_traj和time_now赋值给local_data_的position_traj_和start_time_ ，并且通过UniformBspline类的函数设置local_data_ 的速度、加速度轨迹，轨迹开始时间和时长。最后轨迹的id加一

EGOPlannerManager::reparamBspline

把bspline的时长延长为原来的ratio倍，轨迹增加的时长赋值给time_inc，并等间隔取N个点，N为bspline原来的控制点数-3。调用UniformBspline::parameterizeToBspline() 。拟合得到新的均匀B样条轨迹bspline、始末start_end_derivative、控制点ctrl_pts、dt和轨迹增加的时长time_inc