admin管理员组文章数量:1622629
Prometheusv2中的EGOSwarm代码阅读笔记,个人学习用。
目前还有部分代码没看明白,欢迎大家交流!
EGOPlanner—Prometheus代码阅读笔记汇总
主要参考资料:
Fast Planner——代码解读参考资料整理
EGO-Swarm代码阅读笔记汇总
代码来源:
https://github/ZJU-FAST-Lab/ego-planner
https://github/amov-lab/Prometheus
三、类实现
- (二)EGOPlannerManager类【规划管理】
- 1.源文件位置
- 2.主要功能
- 3.成员变量
- 4.成员函数
- EGOPlannerManager::initPlanModules
- EGOPlannerManager::reboundReplan【重要】
- EGOPlannerManager::EmergencyStop
- EGOPlannerManager::checkCollision
- EGOPlannerManager::planGlobalTrajWaypoints
- EGOPlannerManager::planGlobalTraj
- EGOPlannerManager::refineTrajAlgo
- EGOPlannerManager::updateTrajInfo
- EGOPlannerManager::reparamBspline
(二)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
本文标签: 代码笔记EGOSwarmprometheusEGOPlannerManager
版权声明:本文标题:EGOSwarm—Prometheus代码阅读笔记【四】——EGOPlannerManager类实现 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://www.elefans.com/dongtai/1728870841a1177258.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论