自动驾驶运动规划-Hybird A*算法

YoungTimes

发布于 2022-04-28 15:28:08

1.8K0

发布于 2022-04-28 15:28:08

下面的视频展示了DARPA Urban Challenge(DARPA 2007)中Stanford Racing Team的无人车Junior使用的运动规划(Motion Planning)算法Hybird A*在增量构建的迷宫场景、阻断的道路场景和停车场狭窄停车位场景的实际表现。

http://mpvideo.qpic.cn/0bf25qaawaaabiamzpekofpfb3gdbpwaacya.f10002.mp4?dis_k=e3ddf2b2da5e0a9ee59455b9dfaf00f2&dis_t=1651130724&vid=wxv_1284483385053822976&format_id=10002&support_redirect=0&mmversion=false

在迷宫场景中，可以看到随着车辆的运动，周围在不断的做增量构建,这也就意味着，迷宫中的障碍物是通过车端的传感器实时感知结果得到的。车辆只能看到它周围的环境，随着车辆的持续运动，周围的环境被增量式的构建出来。车辆根据增量构建的场景，实时的调整自身的运动规划策略。

Hybird A*算法在迷宫场景的规划效果。图片来源：参考材料2

视频中黄色的小短线是Hybird A*搜索树，可以看到该算法在不同位置、不同转向角度的情况下都可以实时的为车辆规划出可行的运动路径。

在道路阻断导致车辆无法继续前行的场景下，Hybird A*算法可以规划出掉头曲线，从而避开阻塞的道路，从其它道路继续前进。

Hybird A*算法在道路阻断场景的规划效果。图片来源：参考材料2

最后是一个在停车场进入狭窄停车位的场景，可以看到Hybird A*算法可以规划出复杂的运动路线，使得车辆先前进，再后退，再一次性的进入到狭窄的空车位中。

Hybird A*算法在狭窄停车位场景的规划效果。图片来源：参考材料2

既然是A*算法，Hybird A*算法具有A*算法的基本特征，即通过当前状态到目标状态的代价(Cost)预估，引导车辆更快的收敛到目标状态。

1、搜索空间离散化

传统的开放空间(Open Space)中的A*路径搜索的算法，一般将空间划分为小网格，使用网格中心作为A*路径规划的节点，在这些节点中寻求一条规避障碍物的路径。求解的路径只保证连通性，不保证车辆实际可行。

传统A*算法 VS Hybird A*算法。图片来源：参考材料2

Hybird A*算法同时考虑空间连通性和车辆运动学属性，将二维平面空间和角度同时进行二维离散化。论文《Practical Search Techniques in Path Planning for Autonomous Driving》中设置的二维网格大小为1m x 1m，角度分辨率为5^o 。在(X,Y,\theta )三个维度上进行搜索树(Search Tree)扩展时，Hybird A*将车辆的运动学约束引入其中，路径节点可以是二维小网格内的任意一点，保证了搜索出的路径一定是车辆实际可以行驶的。

2、搜索树(Search Tree)扩展

2.1 满足车辆运动学约束

搜索树扩展过程需要基于车辆运动模型，不同类型的车辆运动模型有差异。这里以以前提到的Simple Car Model为例，不熟悉的小伙伴可以看看这篇文章:

自动驾驶运动规划(Motion Planning)-车辆运动学模型

Simple Car车辆运动模型。图片来源：Planning Algorithm-http://planning.cs.uiuc.edu/node658.htm

Simple Car Model的车辆运动学约束的实现如下，其中(x,y, yaw)是车辆的当前姿态；distance是车辆在当前行驶方向上前进的距离；steer是方向盘与车辆行驶方向的夹角；函数返回的是满足车辆运动学约束的下一个姿态点。

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, yaw

2.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)为输入，从车辆的当前姿态为输入，不断采样生成增量扩展的搜索树的过程。

在生成搜索树的过程中，有两个细节：

1）对采样扩展的结果进行碰撞检测，并剔除不满足碰撞检测的扩展。碰撞检测的过程不仅考虑障碍物的位置和形状，还需要考虑车辆自身的位置和形状；

2）最大程度的保证采样扩展的起点和终点不在同一个网格中。可以将采样扩展的长度设置为比对角线长度大一点；

Search Tree With Reed-Shepp Expansion，黄绿色的是增量扩展的搜索树，紫色的是从当前位置到目标位置的Reed-Shepp扩展路径。图片来源：参考材料3

采样扩展的示例代码如下：

...
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)

Hybird A*算法依赖如下两种Heuristics：Non Holonomic Without Obstacles和Obstacles Without Holonomic，对两种Heuristics Cost进行加权求和，作为Hybird A*的估计Cost。

3.1 Non Holonomic Without Obstacles

Non Holonomic Without Obstacles只考虑车辆运动的非完整约束特性，而不考虑障碍物对车辆运动的限制，即认为车辆在完全没有障碍物的开放空间上运动。

Heuristics Cost = Max(non holonomic without obstacles cost, 2D Euclidean distance)

之所以使用Non Holonomic Without Obstacles Cost和2D Euclidean distance的原因在于，它可以对靠近目标附近的错误Heading搜索进行大量有效的剪枝。

Non Holonomic Without Obstacles Cost的计算过程中，对车辆的运动方向变化、车辆转向角度变化、车辆方向盘转角大小等行为施加一定的惩罚，保证车辆按照预期的行为进行运动。

Hybird A*算法在停车场的路径规划效果。图片来源：参考材料3

3.2 Obstacles Without Holonomic

Obstacles Without Holonomic只考虑环境中的障碍物，不考虑车辆的运动约束。这种情况的处理就非常常见了，先基于已知环境和已知障碍物构建网格地图，再采用动态规划算法(Dynamic Programming)计算每个网格到达目的地所在网格的Cost，Cost一般使用欧式距离就够了。

使用该Heuristic的好处是，可以提前发现所有的U型障碍物和Dead Ends，从而引导车辆尽早避开这些区域。

动态规划算法(Dynamic Programming)

4 Analytic Expansions

前面提到的Hybird A*算法中对运动空间(X, Y, \theta )和车辆控制参数(Steering Angle)进行了离散化处理，这就决定了它永远不可能精确的到达连续变化的目标姿态。

为了解决这一问题，论文《Practical Search Techniques in Path Planning for Autonomous Driving》中提出使用基于Reed Shepp模型的Analytic Expansions，即选出一些节点，使用Reed Shepp曲线计算从该节点到目标姿态的路径，如果该路径在已知的环境中不与任何障碍物发生碰撞，则将其作为可选的行驶路径。

不了解Reed Shepp路径规划算法可以看看这篇文章：

自动驾驶运动规划-Reeds Shepp曲线

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

到目前为止，我们通过Hybird A*算法得到了一条实际可行驶的运动路径，但这样的路径往往需要进一步的优化才能得到更好的预期驾驶行为。论文《Practical Search Techniques in Path Planning for Autonomous Driving》提出的优化分为两个步骤：

1）应用非线性优化算法(non-linear optimization)对路径的长度(length)和平滑性(smoothness)进行优化；

2）对优化后的路径进行非参数化的插值(non-parametric interpolation)。

如何对规划出的路径进行继续优化下周继续研究！To Be Continued...