一种结构化道路场景下的车辆局部轨迹规划方法技术

本发明专利技术提出的一种结构化道路场景下的车辆局部轨迹规划方法，首先通过建立一种基于车道参考线的参考坐标系，得到车辆当前时刻在该参考坐标系下的运动状态向量；接着分别生成不同速度区间的参考坐标系下的纵向采样轨迹集合和横向采样轨迹集合，纵向采样轨迹集合内的所有采样轨迹的曲线形式相同，横向采样轨迹集合内所有采样轨迹的曲线形式相同；然后将横纵向采样轨迹进行合成；随后将合成后的采样轨迹转换回笛卡尔坐标系下，并剔除不满足轨迹曲率和碰撞检测的采样轨迹；最后计算每一条采样轨迹的代价，其中代价最小的轨迹即为本发明专利技术所得到的最优轨迹。本发明专利技术通过构造高速和低速下的采样轨迹可提高采样有效率，对复杂约束条件适应性强。

A vehicle local trajectory planning method in structured road scene

【技术实现步骤摘要】
一种结构化道路场景下的车辆局部轨迹规划方法
本专利技术属于智能车决策中的轨迹规划
，特别涉及一种结构化道路场景下的车辆局部轨迹规划方法，用于在复杂动态交通场景下在线快速计算生成以自车起始点为起点的无碰撞轨迹。
技术介绍
自动驾驶汽车局部轨迹规划问题根据应用场景的不同可以分为非结构化场景下的局部轨迹规划问题和结构化场景下的局部轨迹规划问题。结构化场景如高速公路、城市主干道等，其特点在于车道形状已知、环境动态性强、汽车行驶速度高、留给自动驾驶系统的响应时间较短。同时，在这种场景下，车辆局部轨迹规划的结果为一时空曲线，因此问题的求解需要同时考虑环境的静态约束条件和动态约束条件。针对这类场景，基于采样的局部轨迹规划方法由于其计算效率高，受环境约束条件影响小而取得了较好的效果。目前，这类基于采样的局部轨迹规划方法包括随机采样和确定性采样两种方式：随机采样法包括PRM(ProbabilisticRoadmap，随机路线图)和RRT(Rapidly-exploringRandomTree，快速搜索随机树)。这类方法通过随机采样的方式实现对环境的本文档来自技高网...

【技术保护点】
1.一种结构化道路场景下的车辆局部轨迹规划方法，其特征在于，包括以下步骤：/n1)根据车辆当前所处车道的车道参考线信息，建立动态的参考坐标系；获取车辆当前时刻在笛卡尔坐标系下的运动状态向量，由车辆初始时刻在笛卡尔坐标系下的纵向和横向位置、速度和加速度组成；根据笛卡尔坐标系与参考坐标系之间的投影关系确定车辆在参考坐标系下的运动状态向量；/n2)将车速划分为第一速度区间和第二速度区间，所述第一速度区间内的各速度均高于第二速度区间内各速度；若车辆当前时刻的速度属于第一速度区间，则执行步骤3)，若车辆当前时刻的速度属于第二速度区间，则执行步骤4)；/n3)生成车辆在第一速度区间的采样轨迹集合Track

【技术特征摘要】
1.一种结构化道路场景下的车辆局部轨迹规划方法，其特征在于，包括以下步骤：
1)根据车辆当前所处车道的车道参考线信息，建立动态的参考坐标系；获取车辆当前时刻在笛卡尔坐标系下的运动状态向量，由车辆初始时刻在笛卡尔坐标系下的纵向和横向位置、速度和加速度组成；根据笛卡尔坐标系与参考坐标系之间的投影关系确定车辆在参考坐标系下的运动状态向量；
2)将车速划分为第一速度区间和第二速度区间，所述第一速度区间内的各速度均高于第二速度区间内各速度；若车辆当前时刻的速度属于第一速度区间，则执行步骤3)，若车辆当前时刻的速度属于第二速度区间，则执行步骤4)；
3)生成车辆在第一速度区间的采样轨迹集合TrackF1，该采样轨迹集合由车辆在参考坐标系下的纵向采样轨迹集合TrackF1_s和横向采样轨迹集合TrackF1_r合成转化获得，具体步骤如下：
3-1)生成车辆在第一速度区间的纵向采样轨迹集合TrackF1_s
车辆的纵向采样轨迹集合TrackF1_s由多条采样轨迹组成，设各采样轨迹的形式均为相同且关于时间的4次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的纵向位置、速度和加速度；对采样轨迹终点处的纵向速度，以及采样轨迹终点与起点之间的时间差进行均匀采样，同时取各采样轨迹终点处的加速度均为0，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定4次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将该所有采样轨迹中含有车速不属于[0，Vmax]范围和该所有采样轨迹中含有加速度不属于[-adec，amax]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的纵向采样轨迹集合TrackF1_s；Vmax为车辆所在车道规定的最大限速，adec，amax分别为车辆的最大制动减速度和最大加速度；
3-2)生成车辆在第一速度区间的横向采样轨迹集合TrackF1_r
车辆的横向采样轨迹集合TrackF1_r由多条采样轨迹组成，设各采样轨迹的形式均为相同且以时间作为变量的5次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的横向位置、速度和加速度；对采样轨迹终点处的横向位置，以及采样轨迹终点与起点之间的时间差进行均匀采样，同时取各采样轨迹终点处的速度和加速度均为0，确定各目标终点的采样轨迹终点约束条件；根据各采样轨迹的边界约束条件确定5次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将该所有采样轨迹中含有横向位置不属于[Rmin，Rmax]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的横向采样轨迹集合TrackF1_r，Rmin，Rmax分别表示当前车道外侧边界与内侧边界在参考坐标系下的横坐标值；
3-3)从步骤3-1)生成的纵向采样轨迹集合TrackF1_s和步骤3-2)生成的横向采样轨迹集合TrackF1_r中选取对应的纵向采样轨迹和横向采样轨迹进行合成，得到车辆在第一速度区间的采样轨迹集合TrackF1，执行步骤5)；
4)生成车辆在第二速度区间的采样轨迹集合TrackF2，该采样轨迹集合由车辆在参考坐标系下的纵向采样轨迹集合TrackF2_s和车辆的横向采样轨迹集合TrackF2_r组成
4-1)生成车辆在第二速度区间的纵向采样轨迹集合TrackF2_s
车辆的纵向采样轨迹集合TrackF2_s由多条采样轨迹组成，设各采样轨迹的形式均为相同且关于时间的3次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的纵向位置、速度和加速度；对采样轨迹终点处的纵向位置，以及采样轨迹终点与起点之间的时间差进行均匀采样，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定3次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将该所有采样轨迹中含有车速不属于[0，Vmax]范围和该所有采样轨迹中含有加速度不属于[-adec，amax]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的纵向采样轨迹集合TrackF2_s；
4-2)生成车辆在第二速度区间的横向采样轨迹集合TrackF2_r
车辆的横向采样轨迹集合TrackF2_r由多条采样轨迹组成，设各采样轨迹均为相同且关于参考坐标系纵坐标的5次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的横向位置以及其横向位移相对于参考坐标系纵轴的一阶导数和二阶导数，对采样轨迹终点处的横向位置，以及采样轨迹终点所对应的纵向位置进行均匀采样，同时取采样轨迹终点处的横向位移相对于参考坐标系纵轴的一阶导数和二阶导数均为0，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定5次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将所有采样轨迹中含有横向位置不属于[Rmin，Rmax]范围内的采样轨迹剔除，由剩余的采样轨迹构成车辆的横向采样轨迹集合TrackF2_r；
4-3)将第二速度区间下车辆的纵向采样轨迹集合TrackF2_s与横向采样轨迹集合TrackF2_r中具有相同纵向终点位置的采样轨迹进行合成，并将合成后的采样轨迹转换为与第一速度区间下的合成轨迹相同的形式，得到车辆在第二速度区间的采样轨迹集合TrackF2，执行步骤5)；
5)将步骤3-3)或步骤4-3)得到的采样轨迹集合中的采样轨迹转换到笛卡尔坐标系下，得到笛卡尔坐标系下的采样轨迹集合。
6)对步骤5)所得到的各采样轨迹进行曲率检测和碰撞检测，将满足无碰撞约束条件的所有采样轨迹作为车辆可行轨迹集合；
7)设步骤6)得到的车辆可行采样轨迹集合中各采样轨迹在参考坐标系下的轨迹代价函数为轨迹纵向加速度波动量、轨迹横向加速度波动量、轨迹终点纵向位置与期望纵向位置的偏差、轨迹终点纵向速度与期望速度的偏差以及轨迹终点横向位置与期望横向位置的偏差之和，将代价最小的车辆可行采样轨迹作为最优轨迹。


2.根据权利要求1所述的车辆局部轨迹规划方法，其特征在于，令步骤3-1)中，纵向采样轨迹集合TrackF1_s内的各采样轨迹均为s1(t)＝a0+a1t+a2t2+a3t3+a4t4，系数a0～a4按照以下公式计算：



式中，Ti表示在范围[Tmin，Tmax]内进行等间隔均匀采样得到的时间采样值，其中Tmin，Tmax分别为车辆完成一次换道操作所需的最短、最长时间；s0为车辆当前时刻距离车道参考线上距离最近的点与车道参考线起点之间的弧长；符号和符号分别表示变量对于时间的一阶导数和二阶...

【专利技术属性】
技术研发人员：李升波，李克强，徐晨翔，孙琪，刘向宏，王义锋，杨璨，
申请(专利权)人：清华大学，北京嘀嘀无限科技发展有限公司，
类型：发明
国别省市：北京;11