路径规划：基于搜索的算法hybrid A*

1、算法概述

存在问题： 机器人的控制空间、轨迹空间都是连续的，而现存算法基本都是适用于离散情况的，所以生成的路径是不光滑的。不满足机器人的非完整性约束
解决问题：
假设无人车有充分的感知和定位能力，则其能够在线重新规划，生成障碍物地图，并且能够在未知环境中行驶。解决了在未知环境中自动驾驶车辆的路径规划问题。

流程：
第一步在连续坐标系中使用启发式搜索，以保证计算轨迹的运动学可行性。并且能够保证生成的轨迹满足车辆非完整性约束，通常会生成一条位于全局最优解附近的轨迹。

第二步使用数值非线性优化（共轭梯度（CG）下降）来提高局部解的质量，产生一条至少是局部最优的路径，但通常也会达到全局最优。

2、搜索空间离散化

A* 是赋予每个网格的中心点相应的损失并且算法只访问这些中心点，而hybrid A* 是先在这些网格中挑选满足车辆3D连续状态的点，并将损失赋值给这些点。

Hybird A*算法同时考虑空间连通性和车辆朝向，将二维平面空间和角度同时进行二维离散化。

在(X，Y，theta)三个维度上进行搜索树(Search Tree)扩展时Hybird A*将车辆的运动学约束引入其中，路径节点可以是二维小网格内的任意一点，保证了搜索出的路径一定是车辆实际可以行驶的。

3、启发函数

传统的A* 算法的启发函数一般是2D欧几里得距离，而hybrid A* 算法构造了两个启发函数。

第一个启发函数是Constrained heuristics ，只考虑车辆的非完整性约束而不考虑障碍物（相比直接用欧几里得距离损失要好一个数量级，扩展更少的结点）（图a,b）。

该启发函数忽略了环境中的障碍物等信息，只考虑车辆的运动学特性，从终止点开始，计算从该点到其他点的最短路径。具体的返回值就是：

第二个启发函数是第一个的对偶，称为Unconstrained heuristics，只考虑障碍物信息而不考虑车辆的非完整性约束条件

该启发函数使用传统的2D 路径规划算法，如A* 算法，计算每个节点到终点的最短路径。能够发现2D空间中所有的U形障碍物和死胡同dead end，避免在更复杂的环境中，导致浪费性探索（图c)。最终算法使用的损失函数就是两种启发函数的最大值。(图d)

4、搜索树扩展

车辆的控制输入主要有两个：方向盘转角(Steering Angle)和运动方向(direction)。

将方向盘转角从最小转角(Min Steering Angle)到最大转角(Max Steering Angle)按照一定间隔进行采样；车辆的运动方向：向前运动和向后运动。

正向搜索使用离散化的控制动作空间（转向）。搜索永远不会达到精确的连续坐标目标状态（精度取决于A*中网格的分辨率）。

为了提高精度和搜索速度，在Reed-Shepp模型（Reeds and Shepp 1990）的基础上，通过分析扩展来增强搜索。在搜索中，树中的一个节点通过在一小段时间内（对应于网格的分辨率）使用特定的控制动作模拟汽车的运动学模型来扩展。

除此之外，通过计算从当前状态到目标（假设无障碍环境）的最佳Reed-Shepp路径来生成额外的子节点。然后检查Reed and Shepp路径是否与当前障碍物贴图发生碰撞，并且仅将子节点添加到 如果路径是无冲突的，则子节点添加到树中。

5、其他注意

应用场景：
在自动驾驶领域：
APA（辅助泊车）
AVP（代客泊车）
U-Turn（行车场景下的车辆掉头）

参考链接：
算法解析（伪代码流程）：
https://zhuanlan.zhihu.com/p/139489196
原理代码解析：
https://zhuanlan.zhihu.com/p/139489196
ros平台c++/matlab代码带注释实现：
https://github.com/teddyluo/hybrid-a-star-annotation
参考论文：《Practical Search Techniques in Path Planning for Autonomous Driving》
内容皆来源于网络和参考链接，侵权联系删。