rrt运动规划算法流程图_自动驾驶运动规划-Hybird A*算法
该视频对DARPA 2007年举办的城市挑战赛（DARPA Urban Challenge）中斯坦福大学赛车队开发的无车车（Junior）所采用的运动规划算法——Hybrid A*在不同环境下的具体表现进行了详细描述。
无人车Hybird A*算法表现https://www.zhihu.com/video/1242878670462967808
在迷宫场景中可观察到随着车辆运行周围区域持续形成增量结构这一现象表明迷宫中的障碍物信息是由车端传感器实时采集并转换所得。由于车辆仅能感知本地环境因此随着时间推移外部环境将逐步呈现出完整的轮廓结构在此基础上驾驶员系统会基于动态更新的空间认知来优化自身路径规划
视频中黄色的小短线代表Hybird A*搜索树。我们可以观察到该算法在各种不同的位置和转向情况下都能实时规划出可行的运动路径。
当因道路阻塞而车辆无法继续前行时，在此场景下,Hybrid A*算法能够设计出一套掉头曲线方案,从而绕开阻碍的道路,并可由此处其他路径继续前行。
在停车场面临狭窄停车位挑战时（原文：最后是一个在停车场进入狭窄停车位的场景）
可以说它是混合A算法，并且它还具备A算法的核心特性，并且基于当前状态到目标状态的成本评估引导车辆快速接近目标状态。
1、搜索空间离散化
基于传统开放空间模型的A*路径搜索算法，在离散化处理后形成的网格节点间寻求最短通路时，默认会忽略复杂的运动学约束条件。经过动态规划方法优化后得到的结果仅能确保规划区域内的连通性，并不能完全满足车辆的实际运动需求
该算法综合考虑了路径连通性和车辆行驶方向，并对平面区域及其方向角进行了二维离散划分。在研究文献《Practical Search Techniques in Path Planning for Autonomous Driving》中所采用的二维网格划分方案采用了间距为1米的规则，并且其方向分辨率设定为
。在(X,Y,
在三维空间中对搜索树进行扩展时
2、Hybird A*搜索树扩展
2.1 满足车辆运动学约束
该搜索树扩展过程基于车辆运动模型建立。不同类型的车辆运动模型存在区别，在此我们选择之前提到过的Simple Car Model作为示例进行说明。
半杯茶的小酒杯：自动驾驶中的车辆运动学模型zhuanlan.zhihu.com
基于Simple Car Model的车辆运动学约束实现如下：首先定义了当前状态(x, y, \psi)表示汽车的位置及朝向；随后设定沿当前行驶方向移动的距离为d；再者以方向盘转角\delta来表示转向的程度；最终该函数将输出满足上述运动学条件的新状态坐标点。
def move(x, y, yaw, distance, steer, L=WB):
x += distance * cos(yaw)
y += distance * sin(yaw)
yaw += pi_2_pi(distance * tan(steer) / L) # distance/2
return x, y, yaw2.2 车辆控制空间离散化
车辆的控制指令分为两大类：转向幅度(Steering Angle)和行驶方向(direction)。操作者会将转向幅度从最小值(Min Steering Angle)逐步增加到最大值(Max Steering Angle)，每隔固定幅度采集一次数据点以确保全面覆盖范围；车辆的行驶状态仅限于两种情况：正向行进或倒车操作。
for steer in np.concatenate((np.linspace(-MAX_STEER, MAX_STEER, N_STEER),[0.0])):
for d in [1, -1]:
yield [steer, d]2.3 对运动空间进行扩展探索
其过程即为基于车辆当前的姿态状态，在车辆控制参数（Steering Angle和Direction）的基础上系统性地构建动态扩展的搜索树结构的发展过程。
在生成搜索树的过程中，有两个细节：
通过碰撞检测对采样扩展的结果进行处理，并剔除不符合条件的样本。在该过程中，不仅要考虑到障碍物的位置和形状，还需考虑到车辆自身的位置和形状。
2）确保采样扩展在最大可能的情况下避免起终点位于同一单元格。建议将采样扩展长度设定为对角线长度加长一点；
采样扩展的示例代码如下：
...
x, y, yaw = current.xlist[-1], current.ylist[-1], current.yawlist[-1]
arc_l = XY_GRID_RESOLUTION * 1.5
xlist, ylist, yawlist = [], [], []
for dist in np.arange(0, arc_l, MOTION_RESOLUTION):
x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer)
xlist.append(x)
ylist.append(y)
yawlist.append(yaw)
if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
return None
...3 搜索代价预估(Heuristics)
Hybrid A-star algorithm relies on the following two Heuristics rules: Non-holonomic motion planning and Holonomic motion control without obstacles.
3.1 Non Holonomic Without Obstacles
该系统主要关注无障碍环境下车辆的非完整运动约束特性，并未考虑到障碍物存在的潜在影响
The heuristic cost is defined as the maximum of (2D non-holonomic obstacle-free distance and 2D Euclidean distance).
The heuristic cost is defined as the maximum of (2D non-holonomic obstacle-free distance and 2D Euclidean distance).
基于Non-Holonomic Without Obstacles Cost和2D Euclidean distance的算法设计，在路径规划领域具有重要意义。其主要优势在于能够大幅减少邻近目标区域中无效 headings的搜索空间。
在Non Holonomic Without Obstacles Cost的计算过程中，在对车辆的运动方向变化、转向角度变化以及方向盘转角大小等行为给予一定的惩罚以确保车辆能够按照预期的方式进行行驶时
3.2 Obstacles Without Holonomic
该系统仅针对环境中的障碍物而无需理会车辆的运动限制。这一问题通常较为简单，在既知环境下通过基于既知障碍物生成二维栅格地图后即可轻松解决。具体而言, 我们将通过基于既知环境和既知障碍物生成二维栅格地图, 然后再利用动态规划算法(Dynamic Programming)来计算从每个栅格单元到目标单元所需的时间或距离, 其中时间或距离可统一以欧式距离作为衡量标准
采用该启发式方法的优点在于能够迅速且全面地识别所有U型障碍物和死胡同，并有助于车辆及时避开这些潜在危险区域。
4 Analytic Expansions
前面提到的Hybird A*算法中对运动空间(X, Y,
通过离散化处理了’)’以及车辆控制参数(Steering Angle)，从而导致它永远无法精确地抵达连续变化的目标姿态。
通过离散化处理了’)’以及车辆控制参数(Steering Angle)，从而导致它永远无法精确地抵达连续变化的目标姿态。
为了求解这一难题，《Practical Search Techniques in Path Planning for Autonomous Driving》一文中建议采用基于Reed Shepp模型的Analytic Expansions方法。具体而言，该研究选取若干关键点，并通过Reed Shepp曲线计算这些关键点至目标姿态之间的轨迹；若所计算出的轨迹在预先设定的安全区域内没有与任何障碍物发生接触，则将其视为可行的行驶方案。
半杯茶的小酒杯：自动驾驶运动规划-Reeds Shepp曲线zhuanlan.zhihu.com
paths = rs.calc_paths(sx, sy, syaw, gx, gy, gyaw,
max_curvature, step_size=MOTION_RESOLUTION)
if not paths:
return None
best_path, best = None, None
for path in paths:
if check_car_collision(path.x, path.y, path.yaw, ox, oy, kdtree):
cost = calc_rs_path_cost(path)
if not best or best > cost:
best = cost
best_path = path
return best_path目前为止，在我们的研究中运用Hybrid A*算法成功生成了一条可行行驶路线。然而这条路线仍需经过进一步优化处理以实现预期驾驶行为显著提升这一目标。这种优化工作被划分为两个主要阶段：
采用非线性优化算法（non-linear optimization）来改善路径的长度（length）和平滑性（smoothness）。
2） 对优化后的路径进行非参数化的插值(non-parametric interpolation)。
如何对规划出的路径进行继续优化下周继续研究！
参考材料
Introducing the Hybrid A* algorithm, a highly efficient path planning technique, specifically designed for enhancing navigation capabilities in autonomous vehicles. This advanced method integrates principles from both the A* algorithm and Dijkstra's algorithm to achieve optimal path determination while minimizing computational overhead. The algorithm operates by systematically exploring all potential paths, prioritizing those with the lowest cumulative cost. By incorporating heuristic functions, it ensures that the system can quickly converge on the most viable routes, making it particularly suitable for real-time applications such as autonomous driving systems. For further details, please refer to our in-depth analysis at this link.
2、 Udacity的A*算法在动作智能领域应用——人工智能技术在机器人学中的应用（https://www.youtube.com/watch?v=qXZt-B7iUyw）
3、Effective Search Methods in Route Optimization for Autonomous Driving Applications（https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf）
注：本文首发于微信公众号【半杯茶的小酒杯】，转载请注明出处，谢谢！
推荐阅读：
少量茶水的小盏里：AI领域中的动态规划基础学习
半杯茶的小酒杯：自动驾驶路径规划-Voronoi Plannerzhuanlan.zhihu.com
浅酌一杯茶的小酒盏：改进型采样方法下的路径规划技术-RRT改进版
微醺的小酒盏：自动驾驶路径规划 - 基于图论的BFS最短路径规划算法
半杯茶的小酒杯：自动驾驶运动规划-Dubins曲线zhuanlan.zhihu.com
这只半空的小酒杯承载着未知环境中Lidar的概率占据网格图（Occlusion Grid Map）的独特视角
一个小酒杯里的少量茶水：自动驾驶中的占位栅格图（Occupancy Grid Map）zhuanlan.zhihu.com
半杯茶的小酒杯：自动驾驶中的车辆运动学模型zhuanlan.zhihu.com
半杯茶的微小传感器：自适应定位机制（第十第五集）基于多传感器融合的状态重构（muti-Sensors Fusion）知乎子站链接
基于网格的动态路径规划系统：Lattice Path Finder的深入解析