动态场景下的轨迹规划（轨迹生成/代码实现/未来挑战）！

最新推荐文章于 2023-05-29 20:41:43 发布

自动驾驶之心

最新推荐文章于 2023-05-29 20:41:43 发布

阅读量4.4k

点赞数 5

文章标签：机器人

原文链接：https://mp.weixin.qq.com/s?__biz=Mzg2NzUxNTU1OA==&mid=2247512677&idx=3&sn=81d79029e26263e12980a7816d295da4&chksm=ceb89facf9cf16ba8af8c18ee992e2d45e1d7e5a02c4dd40cff63a84540a9981071c5dbc9281&scene=126&&sessionid=0

版权

作者 | Young 编辑 | 半杯茶的小酒杯

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【轨迹预测】技术交流群

后台回复【轨迹预测综述】获取行人、车辆轨迹预测等相关最新论文！

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

“

巡航模式转跟车模式
”

自动驾驶决策系统

论文【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函数如下:

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

在横向规划的目标位置选择上，论文【2】中使用、，即总是假设车辆的目标状态一定是平行于车道中心线的。

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

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

2.1.2 Low Speed Trajectories

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

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

Cost函数也调整为:

其中：

2.2 纵向轨迹生成

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

2.2.1 Following、Merging & Stopping

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

它的Cost函数如下：

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

图片来源【2】

Following

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

其中和都是常数项
和速度都是前车的位置和速度

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

Merging and Stopping

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

在Stopping时：

2.2.2 Velocity Keeping

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

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

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

图片来源【2】

2.3 合并横纵向轨迹

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

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

曲线拟合

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

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是采样间隔；

速度采样

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

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

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
    elifnot 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

是车辆的停车位置，车辆停止时，纵向的速度和加速度都是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下，初始状态: -> 目标状态: ，目标状态的计算公式如下:

其中，是两辆车之间的最小距离，是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(如下图所示)。其中是怎么计算出来的？了解的同学可以帮忙解个惑，不胜感谢...

后台回复【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

往期回顾

一文尽览 | 轨迹预测二十年发展全面回顾！（基于物理/机器学习/深度学习/强化学习）

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称

自动驾驶之心【知识星球】

想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！

自动驾驶之心

关注

5
点赞
踩
87

收藏

觉得还不错? 一键收藏
10
评论
动态场景下的轨迹规划（轨迹生成/代码实现/未来挑战）！

作者| Young编辑| 半杯茶的小酒杯点击下方卡片，关注“自动驾驶之心”公众号ADAS巨卷干货，即可获取点击进入→自动驾驶之心【轨迹预测】技术交流群后台回复【轨迹预测综述】获取行人、车辆轨迹预测等相关最新论文！一些场景下的轨迹规划效果：“巡航模式转跟车模式”自动驾驶决策系统论文【1】中提出的自动驾驶决策系统(Decision-Making System)包含三层Behavior Plann...
复制链接

扫一扫