Asatr+DWA路径规划算法结构梳理

Astar的参数列表

1、输入参数：障碍物位置，起点位置，终点位置，最大范围（X，Y）

2、输出参数：路径点，路径总长度，open列表个数，closed列表中障碍物个数标志位

1、寻找子节点函数：

（1）扩展当前节点周围的节点，检查是否在边界内

（2）检查扩展的节点是否在障碍物。子节点在障碍物上，上下有障碍物存到a1计数器中，左右有障碍物存到b1计数器中，障碍物在父节点上下，舍弃障碍物左右子节点，同理，舍弃左右障碍物上下的子节点。

（3）fn中检查行驶区域内的障碍物个数

2、开始Astar算法

1、寻找到子节点之后，检测是否在openlist中，如果不在，添加到openlist中，如果在，更新gn，hn，完成open列表更新。openlist第一列是有效性1

2、在openlist寻找最小的fn的， 构造fn_min函数

3、将最小的openlist中的数放到closed，同时openlist有效性置0；

4、是否完成路径标志位检测

3、提取路径优化折现

如果到达目标点：将最后一个点提取出来并提取出父节点，一直到达起点，获得路径点

1、将倒置排序转换为正向排序的路径

2、然后判断当前节点，当前节点的子节点，子节点的子节点是否在同一直线上，更新关键节点，从起点寻找子节点，到达终点，裁弯曲直

3、优化折线，每个一段距离取点

DWA的参数列表

输入参数：障碍物，Astar全局路径，初始参数，评价参数

输出参数：速度、角速度、位置

1、判断是否更新局部目标点

2、局部路径规划：找出参考轨迹，进行正则化，找出最优局部路径，更新状态，进行下一次的局部路径搜索