混合Astar实现_karlkurzer/path_planner

混合Astar实现：
包括路径生成和路径优化两个部分。
路径生成：

1. 结点扩展时即考虑车辆的动力学约束，只考虑可行驶路径上的离散结点作为扩展结点，同时基于离散网格对多余的结点进行剪枝。
2. 依据当前成本+启发函数成本从openlist弹出结点，其中当前成本是路径上每次状态转换的成本和，不仅考虑路径长，也考虑转向、倒车等附加成本；启发函数考虑两种成本函数a. 满足动力学约束、但不考虑障碍物的成本；b. 考虑障碍物，但不考虑车辆动力学约束的成本，取两者的最大值作为混合Astar的启发函数成本。
3. 以一定概率直接解析求解当前节点到目标点的路径，一方面用于快速结束循环，一方面防止离散的状态转换找不到在空间上连续的目标点。

路径优化： 最小化成本函数，成本函数考虑了vonoroi图、与障碍物的碰撞、平滑、曲率四个因素。

github地址：https://github.com/karlkurzer/path_planner
对该代码的注释：https://github.com/teddyluo/hybrid-a-star-annotation
论文地址：http://kth.diva-portal.org/smash/record.jsf?pid=diva2%3A1057261&dswid=-8984

论文部分

Hybrid A star搜索

混合Astar算法类似于Astar算法，关键不同在于，混合Astar的状态转化发生在连续空间中（在连续空间中扩展节点），而非离散空间。虽然混合Astar搜索在离散网格上建立地图，但是每一个路径点不受网格的限制，并且借助网格对多余且相似的路径点进行剪枝。
算法伪代码：

以Astar一致，算法需要两个容器，openlist O存放扩展点，closelist C存放寻访过的点。
算法起始于将起始点$x_s$置入openlist中，并对openlist进行循环。
从openlist中弹出最小代价点，作为当前寻访点，并置入closelist中。
如果寻访点和目标点在同一网格cell中，循环结束，找到目标路径
（注意，该网格是三维网格，对应于配置空间，是对配置空间中的点$(x,y,\theta )$离散化的结果。所以寻访点和目标点网格cell一致，不仅代表工作空间位置(x,y)一致，偏航角$\theta$也一致）
对所有可能的动作展开当前状态，获得后继点 $x_{succ}$。
判断后继点不在closelist中（重复寻访）。
判断后继点不在openlist中，或者后继点的成本小于该cell的成本，那么该后继点可以进入openlist：给该后继点的前继点$Pred(x_{succ})$赋值，更新当前成本和启发函数成本。将后继点添加入openlist中。

结点扩展与剪枝

为了遵守non-holonomic车辆的约束，当前状态/当前结点通过六个动作（action）展开六个后继节点：a.最大转向左侧、b.最大转向右侧、c.直行、d.左向倒车、e.右向倒车、f.正向倒车。 每个控制动作都会持续一段时间，从而生成满足车辆约束的圆弧。 这将确保生成的路径始终是可行驶的。状态转换同时会产生成本（每个节点的当前成本，是其路径上所有变换成本的和），成本不仅包含路径长度（弧长），倒车、改变驾驶方向（倒车转正向）、转弯都为产生额外的成本，与路径长度进行乘算或加算。
通过上述的算法过程，我们可以注意到，如果某后继节点所在单元格已经有节点被寻访或者扩展，且后继节点的g值（当前成本）不低于已有节点的成本值，则放弃该后继节点。这即为剪枝过程。

解析拓展

HAstar规划器会以一定概率（依赖于当前节点到目标节点的距离），直接解析地计算当前节点到目标节点的Dubins曲线。如果获得该解析曲线，且不与障碍物冲突，则可以直接停止搜索。
使用该方法一方面可以提高检索效率，在接近目标时，或者障碍物很稀疏时，可以直接结束循环。另一方面，也是考虑到仅靠离散化的控制动作难以达到连续的目标位置。

启发函数

混合Astar算法采用了两种启发函数，并取两者的最大值作为最终的启发函数代价。
满足动力学约束的启发函数：忽略环境障碍，使用Dubins或者Reeds-Shepp到达目标点的代价。
考虑环境的启发函数：忽略车辆的动力学约束，只考虑障碍物，估计当前节点距离目标点的距离（该距离是通过标准Astar算法实现的），使得节点的扩展原理死胡同与U型弯。

路径平滑

算法主要针对四个成本函数对路径进行平滑。

最后一项时voronoi势场成本。

代码部分

//核心步骤：节选自 planner.cpp

// 混合A*算法找到一条初始路径结
Node3D* nSolution = Algorithm::hybridAStar(nStart, nGoal, nodes3D, nodes2D, width, height,configurationSpace, dubinsLookup, visualization);
// 1. 将start点纳入openlist
// 2. 开始循环
//     a. 从openlist中弹出最小成本点nPred
//     b. 将当前点送入closelist(没有实体容器,仅修改当前点标志位)
//     c. 终止条件1:1.寻访到目标点;2.达到最大循环次数
//        终止条件2:如果车辆在前进且距离目标足够近,直接使用Dubins命中目标.
//     d. 沿六个方向拓展当前点,生成后继点
//         确保后继点:1.位置正常;2.可行驶;3.非close点,还未寻访;4.后继点尚未被被扩展或者已经被扩展过,但代价更小。否者放弃该后继点
//         检查如果后继和当前节点在同一个网格，且总代价C上升，放弃该后继点
//         后继点送入openlist

// 从结束点,循环遍历每个点的前继点 node->getPred(),直到得到完整路径保存在 smoother.path。
smoother.tracePath(nSolution);

// 取出smoother.path 到 planner.path 
path.updatePath(smoother.getPath());

// 对路径进行优化与平滑
smoother.smoothPath(voronoiDiagram);
// 针对每个点的Voronoi代价、碰撞代价、平滑代价、曲率代价求梯度,迭加到每个点上。
// 循环500次

// 取出平滑后的路径,保存为planner.smoothedPath
smoothedPath.updatePath(smoother.getPath());

程序运行

按照github上的提示，编译过程没有遇到问题，直接即可运行。