技术特征：
1.一种自动驾驶车辆智能决策及局部轨迹规划方法，其特征在于：步骤一、由道路信息采样给定车辆当前位置及给定的目标位置点；步骤二、通过frenet坐标系对轨迹计算进行简化，利用多项式插值出车辆未来的遍历轨迹对应的路径，通过汽车的初始状态与目标状态求解轨迹集合；步骤三、对上述轨迹集合进行成本代价评估并按最小原则排序，根据排序结果依次进行是否满足约束条件的检查，不能同时满足所有约束条件时则按排序取下一轨迹重复检查，直至确定同时满足各约束条件的最优轨迹；步骤四、获得最优轨迹后对其进行frenet坐标系到笛卡尔坐标的转换，然后将转换结果下发行车命令用于车辆运动控制。2.根据权利要求1所述的一种自动驾驶车辆智能决策及局部轨迹规划方法，其特征在于：所述步骤二具体包括：给定车辆当前位置及给定的目标位置点利用6个约束，使用5次多项式进行拟合，具体如下：若已知t0时刻的状态[f(t0),f'(t0),f”(t0)]
t
及t1时刻的状态[f(t1),f'(t1),f”(t1)]
t
，将已知条件代入上式并用矩阵的形式表达线性方程组如下：ma＝f其中：a＝[a
0 a
1 a
2 a
3 a
4 a5]
t
f＝[f(t0),f'(t0),f”(t0),f(t1),f'(t1),f”(t1)]
t
确定车辆的初始状态d(t0)＝[d0,d'0,d
″0]
t
及车辆的目标状态d(t1)＝[d1,0,0]
t
，令t0＝0；确定车辆纵向的初始状态s(t0)＝[s0,s'0,s
″0]
t
及车辆的目标状态s(t1)＝[s1,s1',s
″1]
t
，令t0＝0，通过上式便可求解线性方程；由上述多项式插值计算分别得到对应的横向轨迹集合{d
i
(t)|i＝0，1，2...}和纵向轨迹集合{s
i
(t)|i＝0，1，2...}，二者结合考虑时间离散{δt|t0，t0+δt，t0+2δt，...，t1}得到横、纵合成轨迹集合{d
i，j
，s
i，j
|i，j＝0，1，2...}。3.根据权利要求2所述的一种自动驾驶车辆智能决策及局部轨迹规划方法，其特征在于：所述步骤二中，目标状态根据场景变化包括下列方法确定目标状态s(t1)：巡航：对于横向目标状态，使用本车的横向速度和更新步长n，计算目标状态的横向偏移，其中横向速度和加速度设置为零，对于纵向，终端速度设置为道路限制速度，终端加速度设置为零，纵向位置不受限制，指定为nan，最终生成在给定时间范围内与道路速度平滑
匹配的轨迹：其中，表示目标纵向速度；d
explane
表示横向位移，其基本更新方法为：d
explane
＝v0+0.5
×
d2t(i),i＝1,2,...,n；其中，v0表示初始速度；变道：首先确定本车辆的当前车道，然后检查相邻车道是否存在，对于每个现有车道，目标状态的定义方式与巡航控制行为相同，将终端速度设置为当前速度：其中，表示车辆目标速度；d
deslane
表示车辆横向目标位置；跟车：首先确定本车当前车道，根据设定的前车时间范围，预测前车的未来状态，并查找与本车占用同一车道的所有车辆，同时确定哪个是最接近本车的车辆，如果前车不存在，则该功能不会返回任何内容，通过前车的位置和速度，将终端纵向位置减少一定的安全距离，计算出自车辆的目标状态：其中，s
lead
‑
l
safety
表示前车与本车的纵向间隔距离；表示前车纵向速度，作为本车的纵向目标速度；d
lead
表示前车的横向位移，作为本车的横向目标位移；表示前车的横向速度，作为本车的横向目标速度。4.根据权利要求1所述的一种自动驾驶车辆智能决策及局部轨迹规划方法，其特征在于：所述步骤三中的约束条件包括，位置条件：需要保证车辆始终在车道之内，如果目标位置超出道路边界，车辆将会冲出道路发生危险，位置条件可以表示为：d(s)＜d
rode
，其中：d
rode
表示道路边界位置；速度条件：车辆在道路上行驶时，速度v
car
不能超过车辆本身的速度阈值v
max
，同时在路上也会有道路限速，因此速度条件可以表示为：v
car
＜v
max
；加速度条件：车辆的行驶的加速度a
car
不能超过加速度的上限a
max
，即：a
car
＜a
max
；曲率条件：轨迹曲率k
x
反应了车辆的转弯效果，若曲率k
x
过大，车辆速度过快，容易导致车辆侧翻，曲率可以表示为：k
x
＜k
max
,k
max
为曲率阈值；碰撞条件：假设以障碍物为中心的安全距离是r
block
，车身外接圆的半径是r
car
，则车辆与障碍物的碰撞限制可以表示为：其中(x
x
,y
x
)为车辆自身的坐标，(x
block
,y
block
)为障碍物的坐标。5.根据权利要求1所述的一种自动驾驶车辆智能决策及局部轨迹规划方法，其特征在于：所述步骤三中的约束条件包括，乘坐舒适度：转化为对车辆加加速度的评估，即在初始状态时刻t0到目标状态时刻t1的加加速度积分，横向加加速度的评估j
d
和纵向加加速度的评估j
s
以及总的乘坐舒适度可以表示为：为：j
comfort
＝k
d
j
d
+k
s
j
s
其中，k
d
、k
s
分别表示横向加加速度的评估j
d
和纵向加加速度的评估j
s
的加权系数；车辆安全性：车辆安全性评估用车辆碰撞时间t
ttc
来描述，当碰撞时间大于碰撞阈值时
间，则车辆是安全的，并且碰撞时间越长车辆越安全，车辆安全评估值表示为：其中：ε是一个正数值，为了避免t
ttc
≈0时产生奇异值，v
x
为车辆自身的速度，v
block
障碍物的速度，由此得到车辆碰撞时间t
ttc
的算式如下：车辆行驶效率：效率的评估由初始状态时刻t0到目标状态时刻t1车辆纵向速度与目标速度差值的积分表示：其中，v
target
为目标速度,为由初始状态时间t0到目标状态时刻t1车辆纵向速度。6.一种自动驾驶车辆智能决策系统，其特征在于：采用如权利要求1
‑
5中任一所述的一种自动驾驶车辆智能决策及局部轨迹规划方法，所述自动驾驶车辆智能决策系统包括，环境感知模块，用于感知车辆及车辆周围环境信息；智能决策与规划模块，用于结合车辆及周围环境、驾驶员命令等决策车辆如何运动下发行车命令，还承担处理并传递信息的任务；执行系统模块，用于根据下发的行车命令完成车辆的运行控制。7.根据权利要求6所述的一种自动驾驶车辆智能决策系统，其特征在于：所述智能决策与规划模块包括智能决策模块和轨迹规划模块；所述智能决策模块与环境感知模块、所述轨迹规划模块通过can总线进行通讯，所述执行系统模块包括控制各个执行系统的车辆控制模块，所述车辆控制模块与智能决策模块通过can总线进行通讯；轨迹规划模块，进行局部轨迹规划，通过汽车的初始状态与目标状态，考虑汽车的动力学与运动学约束及环境影响，求解满足约束条件的轨迹集合，并在集合中根据成本代价条件选择最合适的轨迹，确定最优轨迹后据此下达相应的行车命令。
技术总结
本发明提供了一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统，方法包括下列步骤：步骤一、由道路信息采样给定车辆当前位置及给定的目标位置点；步骤二、通过Frenet坐标系对轨迹计算进行简化，利用多项式插值出车辆未来的遍历轨迹对应的路径，通过汽车的初始状态与目标状态求解轨迹集合；步骤三、对上述轨迹集合进行成本代价评估并按最小原则排序，根据排序结果依次进行是否满足约束条件的检查，直至确定同时满足各约束条件的最优轨迹；步骤四、获得最优轨迹后下发行车命令用于车辆运动控制。本发明将复杂的非线性轨迹规划问题简化，降低了轨迹规划的难度，规划出光滑、平顺的轨迹，令车辆行驶更加平稳。令车辆行驶更加平稳。令车辆行驶更加平稳。
技术研发人员：蒋立伟 吴勇 檀生辉 王东 姜敏 陶振 伍旭东 吴二导 孙鸿健 胡珍珠 何志维 齐红青
受保护的技术使用者：西安电子科技大学芜湖研究院
技术研发日：2021.07.05
技术公布日：2021/9/13