动态场景下的轨迹规划

YoungTimes

发布于 2022-09-01 11:36:53

1.7K20

代码可运行

文章被收录于专栏：半杯茶的小酒杯

运行总次数：0

代码可运行

一些场景下的轨迹规划效果：

“

巡航模式转跟车模式”

自动驾驶决策系统

论文【1】中提出的自动驾驶决策系统(Decision-Making System)包含三层Behavior Planner：

High Level Planner: 负责从电子地图生成导航路线；

Middle Level Planner: 负责处理交通规则，如车道限速、路口交通灯等；

Low Level Planner: 负责根据主车周围的动静态障碍物生成运行轨迹。

本文主要讨论Low Level Planner，即轨迹规划(Trajectory Planning)。

1. 轨迹规划的整体流程

轨迹规划的流程，来源【5】

2. 轨迹生成

2.1 横向轨迹生成

2.1.1 High Speed Trajectories

横向规划主要承担车辆的换道、避障等任务，在横向轨迹规划中，主要考虑车辆乘坐的舒适性，车辆到达目标位置的时间，并对偏离车道中心线的行为进行惩罚。

车辆的舒适性：通过Jerk(加速度的变化率)调节，惩罚大的加速度变化率，避免车辆急加速(Acceleration)和急减速(Deceleration)。

车辆到达目标位置的时间：惩罚时间长的轨迹，使得车辆快速到达目标位置。

车辆对中心线的偏离程度：车辆在中心线上时，d = 0；越偏离中心线，d越大，需要施加的惩罚量就越大。

论文【2】中提出的cost函数如下:

C_{d}=k_{j} J_{t}(d(t))+k_{t} T+k_{d} d_{1}^{2}

其中，

k_{j}

、

k_{t}

、

k_{d}

是各个考量因素的权重，需要根据应用场景自行调整。

在横向规划的目标位置选择上，论文【2】中使用

\dot{d} = 0

、

\ddot{d} = 0

，即总是假设车辆的目标状态一定是平行于车道中心线的。

\left[d_{1}, \dot{d}_{1}, \ddot{d}_{1}, T\right]_{i j}=\left[d_{i}, 0,0, T_{j}\right]

灰色是不符合车辆行驶约束的轨迹，黑色是符合车辆行驶约束的轨迹，绿色是cost最小的轨迹

最后，如上图所示，对T和

d_i

进行采样，利用五次多项式计算出一系列的轨迹集合。再通过车辆的最大加速度、最大速度、最大曲率、障碍物碰撞检测等一系列条件对轨迹进行筛选和过滤，最后选择剩下的cost最小的合法轨迹作为车辆的行驶轨迹。

2.1.2 Low Speed Trajectories

在高速场景下，横向运动和纵向运动可以认为是独立的，但是实际上，车辆是不能直接横向运动的(non-holonomic)，所以在低速场景下，需要同时考虑车辆的横向运动和纵向运动。

此时，不再把横向位置d作为t的函数，而是作为s的函数。

Cost函数也调整为:

C_{d}=k_{j} J_{s}(d(s)) + k_{t} S + k_{d} d_{1}^{2}

其中：

S = s_1 - s_0

J_{s}(d(s)) = \int_{s_0}^{s_1} d^{\prime \prime \prime 2}(\delta) d \delta

2.2 纵向轨迹生成

论文中将纵向轨迹的优化场景大致分成如下三类：

2.2.1 Following、Merging & Stopping

纵向轨迹考虑车辆乘坐的舒适性，车辆到达目标位置的时间，并对纵向的长距离进行惩罚。

它的Cost函数如下：

C_{t}=k_{j} J_{t}+k_{t} T+k_{s}\left[s_{1}-s_{d}\right]^{2}

Following、Merging&Stopping的纵向轨迹都是有明确的目标状态的。纵向轨迹生成的过程就是通过采样的方法从当前状态(

S_0=[s_0,\dot s_0,\ddot s_0]

)转换到目标状态

\left[\left[s_{\text {target }}\left(T_{j}\right)+\Delta s_{i}\right], \dot{s}_{\text {target }}\left(T_{j}\right), \ddot{s}_{\text {target }}\left(T_{j}\right), T_{j}\right]

的过程。

\left[ s_{1}, \dot{s}_{1}, \ddot{s}_{1}, T \right]_{ij} = \left[ \left[s_{\text {target }}\left(T_{j}\right)+\Delta s_{i}\right], \dot{s}_{\text {target }}\left(T_{j}\right), \ddot{s}_{\text {target }}\left(T_{j}\right), T_{j} \right]

图片来源【2】

Following

在跟车时，需要与前车保持一定距离，并且在前车急刹时，也保证不发生碰撞。跟车的纵向目标位置如下:

s_{\text {target }}(t):=s_{l v}(t)-\left[D_{0}+\tau \dot{s}_{l v}(t)\right]

其中

D_0

和

\tau

都是常数项

s_{lv}

和速度

\dot s_{lv}

都是前车的位置和速度

其中前车的一些数据都是需要通过感知预测来获得的，其中我们假设前车的加速度保持不变，

\ddot{s}_{l v}(t)=\ddot{s}_{l v}\left(t_{0}\right)=\text { const. }

,得到主车的目标速度和加速度：

\begin{aligned}&\dot{s}_{\text {target }}(t)=\dot{s}_{l v}(t)-\tau \ddot{s}_{l v}(t) \\&\ddot{s}_{\text {target }}(t)=\ddot{s}_{l v}\left(t_{1}\right)-\tau {s}^{'''}_{l v}(t)=\ddot{s}_{l v}\left(t_{1}\right) .\end{aligned}

Merging and Stopping

在Merging时，主车的纵向目标位置是前后两辆车的中间位置。

s_{\text {target }}(t)=\frac{1}{2}\left[s_{a}(t)+s_{b}(t)\right]

在Stopping时：

s_{target} = s_{stop}

\dot{s}_{target} = 0

\ddot{s}_{target} = 0

2.2.2 Velocity Keeping

速度保持主要是针对前方没有车的场景。主车的目标不是到某个位置，而是速度保持。

Cost函数如下，增加了对速度差异的惩罚项。

C_{v}=k_{j} J_{t}(s(t))+k_{t} T+k_{\dot{s}}\left[\dot{s}_{1}-\dot{s}_{d}\right]^{2}

速度保持的目标状态采样如下：

\left[\dot{s}_{1}, \ddot{s}_{1}, T\right]_{i j}=\left[\left[\dot{s}_{d}+\Delta \dot{s}_{i}\right], 0, T_{j}\right]

图片来源【2】

2.3 合并横纵向轨迹

将横纵向轨迹的Cost进行加权求和，然后选择Cost最小的轨迹。

C_{tot}=k_{lat}C_{lat}+k_{lon}C_{lon}

3. Python代码实现

3.1 地图绘制

轨迹规划依赖高精地图，这里绘制一副简单的3车道地图，用于各种轨迹算法测试。

def create_lane_border(ref_line, width):
    tx, ty, tyaw, tc, csp = generate_target_course(ref_line[:, 0], ref_line[:, 1])

    border = []

    s = np.arange(0, csp.s[-1], 0.1)
    for i_s in range(len(s)):
        s_condition = [s[i_s]]
        d_condition = [width]
        lx, ly = frenet_to_cartesian1D(s[i_s], tx[i_s], ty[i_s], tyaw[i_s], s_condition, d_condition)
        border.append([lx, ly])

    return np.array(border)

center_line = np.array([[0.0, 1.0], [10.0, 0.0], [20.5, 5.0], [35.0, 6.5], [70.5, 0.0]])

tx, ty, _, _, csp = generate_target_course(center_line[:, 0], center_line[:, 1])

border_l = [-1.7, 1.7, 5.1, 8.5]
center_l = [3.4, 6.8]
for i in range(len(border_l)):
    border = create_lane_border(center_line, border_l[i])
    borders.append(border)

for i in range(len(center_l)):
    center = create_lane_border(center_line, center_l[i])
    center_lines.append(center)

地图效果如下：

3.2 横向轨迹生成

首先，沿着道路宽度的方向进行横向采样，MIN_ROAD_WIDTH和MAX_ROAD_WIDTH是道路的最大和最小的横向Offset，D_ROAD_W是采样大小。

横向偏移采样

其次，对时间进行采样，MIN_T是最小预测时间，MAX_T是最大预测时间，DT是采样时间间隔。

for di in np.arange(MIN_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
    for Ti in np.arange(MIN_T, MAX_T, DT):

曲线拟合

已知

[d_0, \dot{d}_0, \ddot{d}_0]

和

[d_1, \dot{d}_1, \ddot{d}_1]

，求解五阶多项式曲线，并进行采样得到横向轨迹。

fp = FrenetPath()

lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)

fp.t = [t for t in np.arange(0.0, Ti, DT)]
fp.d = [lat_qp.calc_point(t) for t in fp.t]
fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]

计算Cost

计算横向采样轨迹的Cost，考虑Jerk、横向距离大小和达到目标状态的时间因素。

Jp = sum(np.power(tfp.d_ddd, 2))  # square of jerk

tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1] ** 2

3.3 纵向轨迹生成

3.3.1 Velocity Keeping Mode

纵向轨迹生成需要对时间采样，MIN_T是最小采样时间，MAX_T是最大采样时间，DT是采样间隔；

速度采样

在速度保持场景下，纵向轨迹需要对速度进行采样。

已知

[\dot{s}_0, \ddot{s}_0]

和

[\dot{s}_1, \ddot{s}_1]

，求解四阶多项式曲线，并进行采样得到纵向轨迹。

for Ti in np.arange(MIN_T, MAX_T, DT):
    for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
        tfp = copy.deepcopy(fp)
        lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti)

        tfp.s = [lon_qp.calc_point(t) for t in fp.t]
        tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
        tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
        tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]

Cost计算

计算纵向轨迹的Cost。考虑Jerk、速度差异和到达目标状态的时间因素。

Js = sum(np.power(tfp.s_ddd, 2))  # square of jerk

ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2 # # square of diff from target speed
                
tfp.cv = K_J * Js + K_T * Ti + K_D * ds

横纵向轨迹结合

最后结合横纵向轨迹的Cost。

tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv

轨迹选择

排除掉不适合车辆无法执行的轨迹，比如超出道路限速、超出车辆的最大加速度、超出可接受的最大曲率、与障碍物有潜在的碰撞风险等等。

for i, _ in enumerate(fplist):
    if any([v > MAX_SPEED for v in fplist[i].s_d]):
        continue
    elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]):
        continue
    elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]):
        continue
    elif not check_collision(fplist[i], ob):
        continue

碰撞检测

对于动态障碍物，可以使用车辆的Bounding Box进行碰撞检测；对于静态障碍物，可以采用占位网格图(Occupancy Grid Map)的方法进行碰撞检测。

动态障碍物的碰撞检测

静态障碍物的碰撞检测

def check_collision(fp, ob):
    for i in range(len(ob[:, 0])):
        d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
             for (ix, iy) in zip(fp.x, fp.y)]

        collision = any([di <= ROBOT_RADIUS ** 2 for di in d])

        if collision:
            return False

    return True

效果展示

3.3.2 Stopping Mode

s_{stop}

是车辆的停车位置，车辆停止时，纵向的速度和加速度都是0。

地图构建

当车辆遇到停止线的场景下，按照交通法规，需要在停止线前停下来。我们先构建一个停止线的场景。

stop_line = np.array([[28.70262896, 4.95465598], [28.25703523, 15.1449183]])

stop_line_s = 30.0
...
plt.plot(stop_line[:, 0], stop_line[:, 1], linestyle = '-', color = '#333333')

如下图所示：

决策模块

已知Stop Line位于Offset=30.0位置，因此当s < 20.0时，Ego车保持巡航状态；s > 20.0时，开始减速，并在停止线钱停下来。

def decision(s):
    if s < 20.0:
        return Mode.VELOCITY_KEEPING

    return Mode.STOPPING

纵向轨迹生成

已知当前Ego车的状态[s0, s0_d, s0_dd]和目标位置[stopline_s, 0, 0]，使用五阶多项式拟合生成纵向轨迹。

for delta_s in [s1 - s0]:
    s_target_dd =  s1_dd
    s_target_d =  s1_d
    s_target = s0 + delta_s

    tfp = copy.deepcopy(fp)
    lon_qp = QuinticPolynomial(s0, s0_d, s0_dd, s_target, s_target_d, s_target_dd, Ti)

轨迹筛选和碰撞检测

轨迹筛选和碰撞检测与velocity keeping模式的处理方式完全相同。

效果展示

3.3.3 Following Mode

Tracking Mode下，初始状态:

[s_0, \dot{s}_0, \ddot{s}_0]

-> 目标状态:

[s_1, \dot{s}_1, \ddot{s}_1]

，目标状态的计算公式如下:

其中，

D_0

是两辆车之间的最小距离，

\tau

是Inter-Vehicle Time,保证在前车急刹的条件下，仍然不会发生碰撞。

决策模块

实现一个简陋的决策模块，当前方车辆距离Ego车的距离大于SWITCH_DISTANCE时，按照巡航(Curise)模式行驶；否则，进入跟车模式。

SWITCH_DISTANCE = 20.0 \

def decision(s, s_front): \
    if s_front - s < SWITCH_DISTANCE:
        return Mode.FOLLOWING
    
    return Mode.VELOCITY_KEEPING

纵向轨迹生成

在跟车模式下，Ego车的目标位置 = 前车的当前位置 - 车辆静止时的间隔距离 - Constant Time Gap；目标速度和目标加速度 = Ti时刻前车的速度和加速度。

for delta_s in [s1 - s0]:
    s_target_dd =  s1_dd
    s_target_d =  s1_d
    s_target = s0 + delta_s

    tfp = copy.deepcopy(fp)
    lon_qp = QuinticPolynomial(s0, s0_d, s0_dd, s_target, s_target_d, s_target_dd, Ti)

轨迹筛选和碰撞检测

轨迹筛选和碰撞检测与velocity keeping模式的处理方式完全相同。

效果展示

待解决的问题

论文【1】中提到一种Adjust Mode(如下图所示)。其中

\dot{s}_{adj}

是怎么计算出来的？了解的同学可以帮忙解个惑，不胜感谢...

后台回复【autonomous_algorithms】可获取本文完整代码，如有错误请帮忙修改指正，感谢！

参考材料

Trajectory optimization and state selection for urban automated driving, Keisuke Yoneda1 etc.(https://d-nb.info/1170773907/34)
Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame, 2010. (https://www.researchgate.net/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame)
Python代码地址：(https://gitee.com/mirrors/PythonRobotics/tree/master/PathPlanning/FrenetOptimalTrajectory)
https://www.notion.so/Frenet-Optimal-Trajectory-Generation-for-Dynamic-Street-Scenarios-in-a-Frenet-Frame-96d5bced98c146d2a9036a1d5fc2b21d#09bc3f3ecad74f5faa96865ba2c80959
https://blog.csdn.net/qq_23981335/article/details/102832823#t9

本文参与腾讯云自媒体同步曝光计划，分享自微信公众号。

原始发表：2022-05-28，如有侵权请联系 cloudcommunity@tencent.com 删除

https

网络安全

本文分享自半杯茶的小酒杯微信公众号，前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与腾讯云自媒体同步曝光计划，欢迎热爱写作的你一起参与！

https

网络安全

登录后参与评论

0 条评论

热度