基于SBPL函数库的全局路径规划
- SBPL软件包包含了多种全局规划算法
- 在sbpl_lattice_planner.cpp中,利用replan函数调用全局规划算法
以ARA*算法为例说明调用流程
- 运动基元由matlab代码生成,可以根据需要设置基元的分辨率,基元的终点坐标
- 函数GetSearchPath调用了GetSuccs函数,即全局规划路径点的产生也是基于运动基元的
- 在environment_navxythetalat.cpp中,函数ConvertStateIDPathintoXYThetaPath中调用GetSuccs来计算每一个全局规划点的运动基元
araplanner.cpp函数分析
- replan函数
- 输入:搜索时间
- 输出:(1)规划路径点编号 (2)代价值
- 流程:(1)参数定义 (2)调用Search函数
- Search函数
- 输入:搜索状态空间
- 输出:(1)规划路径编号 (2)代价值
- 流程:
- (1)参数定义
- (2)判断是否函数f是否需要更新
- (3)判断搜索空间是否需要重新初始化
- (4)设置规划优先级参数
- (5)调用ARA*算法函数
- (6)如果能够找到可行解,调用GetSearchPath函数找出全局规划路径点
- GetSearchPath函数
- 输入:搜索空间
- 输出:代价值
- 流程:
- (1)参数定义
- (2)确定搜索方向,设置起点和终点
- (3)开始循环,直到找到目标点编号
- (4)循环中需要调用GetSuccs函数,找出当前路径编号的相邻编号
- (5)采用遍历的方法,对当前点的相邻点进行遍历,找到代价值最小的相邻点并记录为全局规划路径上的最优点,重复(4)-(5)过程
- replan函数
总结
- 在调用ARA*算法时就利用运动基元来生成全局规划路径,输出为路径编号
- 在全局规划主函数中对生成的全局规划路径编号进行了解析,还原为运动基元的形式
- 虽然运动基元划分的个数有限,但是所有点的表示都是基于栅格的,因此基于最保守的运动基元划分都可以找到全局规划路径点