概述
ego_replan_fsm.cpp订阅实时定位和目标点信息,每隔0.01s执行一次状态机,进行状态切换;每隔0.05s执行一次碰撞检测,按需进行重新规划或者紧急停止。核心为执行变量exec_state_
主要函数及作用
- EGOReplanFSM::init
输入:句柄
作用:1、参数初始化
2、launch文件参数读取
3、规划管理及可视化指针初始化
4、odom和目标点的订阅、定时器、发布声明等
- EGOReplanFSM::planGlobalTrajbyGivenWps
作用:1、获取终点位置
2、子目标点序列可视化
3、采用mimimum snap或者eigen库求解得到全局五次多项式轨迹[成功:全局轨迹可视化 终点速度为0 目标点相关标志位置于true exec_state_状态改为GEN_NEW_TRAJ]
- EGOReplanFSM::waypointCallback
订阅:孤立目标点
作用:1、保存无人机目标位置、初始位置
2、目标点可视化
3、minimum jerk【若插入点】或者采用eigen库规划当前位置与目标位置的多项式全局轨迹表达式,成功则进行全局轨迹采样点可视化、目标速度为0、exec_state_变量状态更改、标志位置于true等
- EGOReplanFSM::odometryCallback
订阅:odom定位
作用:1、设置无人机当前位置、当前速度、当前航向
2、标志位置于true(表示接受到定位数据)
- EGOReplanFSM::changeFSMExecState
输入:给定执行状态 字符串
作用:1、如果给定状态与exec_state_相同,则持续呼叫次数变量+1;若不同,则将持续呼叫次数变量置于1
2、将exec_state_状态改变为给定值,并进行相关提示信息的打印
- EGOReplanFSM::timesOfConsecutiveStateCalls
作用:得到键值对《当前执行状态被连续调用的次数 当前执行状态exec_state_》
- EGOReplanFSM::printFSMExecState
作用:状态机当前状态打印
- EGOReplanFSM::execFSMCallback
作用:0.01s执行一次状态机切换判断,本质是根据各种触发条件完成规划器在各状态间的转移,具体状态切换过程见下图
- EGOReplanFSM::planFromCurrentTraj
作用:从当前局部轨迹的当前状态【PVA】进行新的局部轨迹更新
【依次尝试原始局部轨迹提取b样条分段端点-------》eigen采样提取b样条分段端点----->插值后采用minimum snap采样提取b样条分段端点,只要成功就不执行后续选择】
返回值:新的局部轨迹更新成功返回true;否则false
- EGOReplanFSM::checkCollisionCallback
作用概述:0.05s执行一次避障回调函数
局部轨迹碰撞检测范围:当前时刻-----》当前局部轨迹定义域终点时间,时间采样间隔0.01s:
起始当前时刻小于定义域时间2/3,则仅仅碰撞检测局部轨迹整体的前2/3;
起始当前时刻大于定义域时间2/3,则碰撞检测当前时刻-----》定义域终点时刻
局部轨迹碰撞处理:若某个时刻发生碰撞,则从当前局部轨迹的当前状态【PVA】进行新的局部轨迹更新,更新成功则exec_state_状态改为EXEC_TRAJ退出;
若失败且轨迹碰撞的时刻与当前时刻的间隔小于紧急停止时间:则将则将exec_state_状态改为EMERGENCY_STOP;
若失败且轨迹碰撞的时刻与当前时刻的间隔大于紧急停止时间,执行重新规划:将exec_state_状态改为REPLAN_TRAJ
- EGOReplanFSM::callReboundReplan
作用:从全局路径根据局部规划视野确定局部子目标点的位置速度
调用ego planner论文方法产生满足始末条件约束的三次均匀b样条轨迹
规划成功则进行b样条信息的发布和控制点可视化
返回值:成功规划局部轨迹,返回true;否则false
- 11.1核心函数planner_manager_->reboundReplan
输入:起点位置 起点速度 起点加速度{局部轨迹规划为GEN_NEW_TRAJ状态首次调用为(0,0,0)} 局部规划终点位置 局部规划终点速度
标志位1【是否直接采用五次多项式轨迹,或者基于原轨迹提取b样条端点】
标志位2:局部轨迹规划为GEN_NEW_TRAJ状态首次调用则false;否则为true【是否需要插值『决定用eigen还是minimum jerk』】
作用:经过init、optimal、refine步骤,规划满足始末条件约束的三次均匀b样条控制点位置
备注:起点是起点,终点不一定为给定终点【但是很靠近局部终点,因为需要minimum snap采样
局部规划成功则进行局部轨迹信息local_data_更新
init: 根据无人机当前位置速度、局部子目标位置速度规划局部参考路径【minimum snap或者eigen】;局部参考路径提取采样点或或者基于原轨迹+新生成五次多项式【原轨迹终点-----》给定局部子目标点】组合提取采样点(作为三次b样条分段的始末点);通过采样点进行三次b样条曲线的控制点的解算;控制点序列中碰撞段的提取、无碰A*路径规划、碰撞段的控制点[p、V]对的生成;无碰A*路径和局部轨迹的可视化
optimal:采用lbfgs算法进行三次均匀b样条曲线的轨迹优化【平滑项、障碍物项、动力学项】----------保留优化后的控制点位置
refine:如果无人机不满足动力学约束,则综合考虑平滑项、动力学项及拟合项采用三次均匀b样条进行轨迹优化,如果优化成功则进行相关均匀b样条信息的更新
返回值:成功规划轨迹返回true
若始末距离近、基于原始局部轨迹采样但是采样点过少;2步或3步优化失败返回false
备注:初始局部参考路径采用minimum snap条件:程序启动第一次进入,本次调用需要,基于原轨迹得到采样点路径太长
- EGOReplanFSM::callEmergencyStop
输入:无人机当前位置
作用:确定无人机紧急停止时的三次局部b样条轨迹并发布【控制点位置重合为原无人机位置【多控制点重合】,故速度、加速度也都为0】
- EGOReplanFSM::getLocalTarget
作用:1、全局路径大于局部规划视野时,进行变量last_progress_time_更新【为距离起始点距离最近的轨迹点时间】
2、进行局部规划目标点位置和速度的确定『局部规划视野大于全局路径长度:局部规划目标点为最后终点,速度为0』
【局部规划视野小于全局路径长度:局部规划目标点为局部视野附近的点,如果局部规划目标点距离终点很近【小于V^2/A】则速度为0】
【局部规划视野小于全局路径长度:局部规划目标点为局部视野附近的点,如果局部规划目标点距离终点较远【大于V^2/A】则速度为对应全局轨迹点速度】