CN111750887A - 降低事故严重程度的无人驾驶车辆轨迹规划方法及系统 - Google Patents

降低事故严重程度的无人驾驶车辆轨迹规划方法及系统 Download PDF

Info

Publication number
CN111750887A
CN111750887A CN202010529704.3A CN202010529704A CN111750887A CN 111750887 A CN111750887 A CN 111750887A CN 202010529704 A CN202010529704 A CN 202010529704A CN 111750887 A CN111750887 A CN 111750887A
Authority
CN
China
Prior art keywords
vehicle
track
severity
tracks
accident
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010529704.3A
Other languages
English (en)
Other versions
CN111750887B (zh
Inventor
张希
刘磊
赵柏暄
朱旺旺
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN202010529704.3A priority Critical patent/CN111750887B/zh
Publication of CN111750887A publication Critical patent/CN111750887A/zh
Application granted granted Critical
Publication of CN111750887B publication Critical patent/CN111750887B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas

Abstract

本发明提供了一种降低事故严重程度的无人驾驶车辆轨迹规划方法及系统，包括：将车辆及环境信息由笛卡尔坐标系转换到Frenet坐标系；利用三次样条曲线生成参考路径；以撒点的方式沿参考路径进行采样，通过预设次多项式进行拟合，生成轨迹簇；以最大速度、最大加速度、最大曲率作为控制约束，去除不符合控制约束的轨迹；对保留的符合控制约束的轨迹进行碰撞检测，在无碰撞轨迹簇中通过预设的损失函数筛选使损失函数值最小的轨迹作为最优轨迹；在碰撞轨迹簇中，通过事故严重程度预测模型筛选严重程度最低的轨迹作为最优轨迹；本发明解决了现有路径规划方法面临无法避免的事故时不能降低事故严重程度的问题，提高了事故发生时乘员的安全性。

Description

降低事故严重程度的无人驾驶车辆轨迹规划方法及系统
技术领域
本发明涉及无人驾驶及其轨迹规划领域，具体地，涉及一种降低事故严重程度的无人驾驶车辆轨迹规划方法及系统。
背景技术
随着汽车技术的不断发展，车辆智能化的脚步在不断加快，车辆以逐渐融入人们生活的方方面面。与此同时，人们也承受着交通拥堵以及交通事故带来的烦扰。车辆智能化改善了人们的出行效率，提高了行驶安全，但仍旧无法完全避免交通事故的发生，考虑在交通事故发生过程中降低事故严重程度显得很有必要。
当前的轨迹规划方面，通过直线和弧、回旋曲线、Nelson多项式、螺旋多项式、样条曲线、贝塞尔曲线等方式生成曲线，通过代价函数和约束选择无碰撞的最优轨迹。但事实上我们无法完全避免事故的发生，因此在轨迹生成过程中结合发生碰撞的轨迹的事故严重程度，在无法避免的交通事故中选择事故严重程度最低的轨迹亟待研究。
专利文献CN110362096A(申请号：201910746277.1)公开了一种基于局部最优性的无人驾驶车辆动态轨迹规划方法。该方法根据道路上障碍物车辆的不同位置与其不同相应的工况，来进行最优参考轨迹的选取并进行动态的轨迹规划。分析无人车换道意图的产生与换道可执行的条件，根据对周围障碍车位置以及速度的预测在决定避障换道的初始时刻拟合出局部最优换道轨迹，进而把这条最优轨迹作为局部参考轨迹。生成无人车可行驶的轨迹簇，并将设计出的速度距离成本代价函数与损失函数相结合，利用非线性模型预测控制筛选出轨迹簇中的最优轨迹。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种降低事故严重程度的无人驾驶车辆轨迹规划方法及系统。
根据本发明提供的一种降低事故严重程度的无人驾驶车辆轨迹规划方法，包括：
步骤M1：将车辆及环境信息由笛卡尔坐标系转换到Frenet坐标系；
步骤M2：在Frenet坐标系中利用三次样条曲线生成参考路径；
步骤M3：以撒点的方式沿参考路径进行采样，通过预设次多项式进行拟合，生成轨迹簇；
步骤M4：以最大速度、最大加速度、最大曲率作为控制约束，去除不符合控制约束的轨迹，保留符合控制约束的轨迹；
步骤M5：对保留的符合控制约束的轨迹进行碰撞检测，当检测后存在不碰撞的轨迹，则将不碰撞的轨迹记为无碰撞轨迹簇，在无碰撞轨迹簇中通过预设的损失函数筛选使损失函数值最小的轨迹作为最优轨迹；当检测后所有轨迹都将碰撞，即所有轨迹构成碰撞轨迹簇，则在碰撞轨迹簇中，通过事故严重程度预测模型筛选严重程度最低的轨迹作为最优轨迹；
所述事故严重程度预测模型是在车辆面临碰撞风险时，根据感知层获得的信息，提取包括车辆状态参数、车辆固有属性、人员属性以及组合参数作为事故严重程度预测模型的输入，实时预测主车在沿不同候选轨迹行进发生碰撞的事故严重程度，选择事故严重程度最低的轨迹作为最优轨迹。
优选地，所述步骤M1包括：
车辆的姿态向量
Figure BDA0002534949560000027
其中，(x,y)表示车辆在二维平面的位置，θ表示车辆的朝向，k表示曲率，即朝向θ的变化率，v表示车辆的速度；
则车辆的姿态变量满足下述关系：
Figure BDA0002534949560000021
其中，
Figure BDA0002534949560000022
表示x的导数，即x方向速度；
Figure BDA0002534949560000023
表示y的导数，即y方向速度；
Figure BDA0002534949560000024
表示θ的导数，即位于(x,y)位置的航向角的导数；
在上述基础上考虑一条由车辆运行产生的连续轨迹，记沿轨迹方向的位移为s方向，则沿轨迹s方向的位移与车辆姿态向量
Figure BDA0002534949560000025
的关系如下：
Figure BDA0002534949560000026
其中，dx表示微分；θ(s)表示车辆航向角关于沿轨迹s方向的位移的函数；k(s)表示路径曲率关于沿轨迹s方向的位移的函数；s代表道路的中心线切向方向的位移，也称纵向位移s；与此对应的是道路的中心线垂直方向位移l，也称横向位移；则由笛卡尔坐标系转换到Frenet坐标系：
Figure BDA0002534949560000031
其中，定义道路的采样函数为r(s)＝[rx(s)，ry(s)，rθ(s)，rκ(s)]，xr(s,l)表示笛卡尔坐标系下横坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，yr(s,l)表示笛卡尔坐标系下纵坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，θr(s,l)表示笛卡尔坐标系下航向角通过沿轨迹s方向的位移和横向偏移l表示的关系，kr(s,l)表示笛卡尔坐标系下路径曲率通过沿轨迹s方向的位移和横向偏移l表示的关系；
则由上式推出车辆姿态向量
Figure BDA0002534949560000034
在Frenet坐标系下的坐标(s，l，l′，l″)：
Figure BDA0002534949560000032
l′＝(1-rkl)tanΔθ
Figure BDA0002534949560000033
其中，r(s)表示道路采样函数，即参考路径的函数表示；rk′表示沿道路采样函数的曲率导数，(s,l,l′,l″)为车辆的姿态向量转换到Frenet坐标系下的坐标，s表示沿道路参考线的距离，l表示距离参考线的横向距离，l′表示横向距离的导数，l″表示表示沿参考路径横向偏移的二阶导数，表征横向加速度；Δθ＝θ-rθ；(rx,ry,rθ,rk)为参考路径中距离环境中的车辆或障碍物(x,y,θ,k,v)最近的点的笛卡儿坐标系下的坐标。
优选地，所述步骤M2包括：
设道路中心线上的参考点为(x0，y0)，(x1，y1)，…，(xn，yn)，每一个点(xi,yi)到点(xi+1,yi+1)区间的起始点和终点分别为(xi，yi)和(xi+1，yi+1),则三次样条曲线在每一个区间si的定义为fi(x)，表达式如下：
fi(x)＝ai+bi(x-xi)+ci(x-xi)2+di(x-xi)3
其中，
ai＝yi
Figure BDA0002534949560000048
Figure BDA0002534949560000041
Figure BDA0002534949560000042
其中，hi＝xi+1-xi为点与点之间的步长，mi通过数值方法求解。
优选地，所述步骤M4包括：
去除各轨迹中纵向速度超过预设最大速度的轨迹；去除各轨迹中纵向加速度超过预设最大加速度的轨迹；去除各轨迹中曲率超过预设最大曲率的轨迹。
优选地，所述步骤M5中检测方法包括：
(xf1-xf2)2+(yf1-yf2)2≤(R1+R2)2
(xr1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xf1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xr1-xr2)2+(yf1-yf2)2≤(R1+R2)2
其中，c1r、c1f、c2r和c2f的坐标分别为(xr1，yr1)、(xf1，yf1)、(xr2，yr2)和(xf2，yf2)；c1r和c1f表示自车区域的后部和前部的半径为R1的两圆的圆心；c2r和c2f表示除自车之外的车辆的后部和前部的半径为R2的两圆的圆心。
优选地，所述步骤M5中损失函数包括：
对纵向和横向的损失分别进行计算：
Figure BDA0002534949560000043
Cl＝wl1J(l(t))+wl2T+wl3l1 2
Ctotal＝wsCs+wlCl
其中，J(s(t))和J(l(t))分别为纵向和横向在t0～t1时间段内加速度的累计值；T代表当前规划的路径段所需的时间，在损失函数中加入时间项用以表征车辆在制动时能否响应足够快；
Figure BDA0002534949560000044
和l1 2分别表示纵向的速度偏差和横向相对参考路径的偏差，
Figure BDA0002534949560000045
表示车辆执行后在当前时间段最终所要保持的纵向速度；
Figure BDA0002534949560000046
表示每次规划t1时刻车辆的纵向速度，ws1、ws2、ws3以及wl1、wl2、wl3表示权重；Ctotal表示横向损失Cl和纵向损失Cs的加权总和，ws和wl分别表示对纵向和横向损失的权重。
优选地，所述步骤M5中轨迹的事故严重程度计算公式如下：
Figure BDA0002534949560000047
其中，CIS表示连续事故严重程度，pk为所预测的轨迹属于严重程度为k的概率，ek为指数函数，K表示表征事故严重程度的类别个数。
根据本发明提供的一种降低事故严重程度的无人驾驶车辆轨迹规划系统，包括：
模块M1：将车辆及环境信息由笛卡尔坐标系转换到Frenet坐标系；
模块M2：在Frenet坐标系中利用三次样条曲线生成参考路径；
模块M3：以撒点的方式沿参考路径进行采样，通过预设次多项式进行拟合，生成轨迹簇；
模块M4：以最大速度、最大加速度、最大曲率作为控制约束，去除不符合控制约束的轨迹，保留符合控制约束的轨迹；
模块M5：对保留的符合控制约束的轨迹进行碰撞检测，当检测后存在不碰撞的轨迹，则将不碰撞的轨迹记为无碰撞轨迹簇，在无碰撞轨迹簇中通过预设的损失函数筛选使损失函数值最小的轨迹作为最优轨迹；当检测后所有轨迹都将碰撞，即所有轨迹构成碰撞轨迹簇，则在碰撞轨迹簇中，通过事故严重程度预测模型筛选严重程度最低的轨迹作为最优轨迹；
所述事故严重程度预测模型是在车辆面临碰撞风险时，根据感知层获得的信息，提取包括车辆状态参数、车辆固有属性、人员属性以及组合参数作为事故严重程度预测模型的输入，实时预测主车在沿不同候选轨迹行进发生碰撞的事故严重程度，选择事故严重程度最低的轨迹作为最优轨迹。
优选地，所述模块M1包括：
车辆的姿态向量
Figure BDA0002534949560000051
其中，(x,y)表示车辆在二维平面的位置，θ表示车辆的朝向，k表示曲率，即朝向θ的变化率，v表示车辆的速度；
则车辆的姿态变量满足下述关系：
Figure BDA0002534949560000052
其中，
Figure BDA0002534949560000053
表示x的导数，即x方向速度；
Figure BDA0002534949560000054
表示y的导数，即y方向速度；
Figure BDA0002534949560000055
表示θ的导数，即位于(x,y)位置的航向角的导数；
在上述基础上考虑一条由车辆运行产生的连续轨迹，记沿轨迹方向的位移为s方向，则沿轨迹s方向的位移与车辆姿态向量
Figure BDA0002534949560000056
的关系如下：
Figure BDA0002534949560000057
其中，dx表示微分；θ(s)表示车辆航向角关于沿轨迹s方向的位移的函数；k(s)表示路径曲率关于沿轨迹s方向的位移的函数；s代表道路的中心线切向方向的位移，也称纵向位移s；与此对应的是道路的中心线垂直方向位移l，也称横向位移；则由笛卡尔坐标系转换到Frenet坐标系：
Figure BDA0002534949560000061
其中，定义道路的采样函数为r(s)＝[rx(s)，ry(s)，rθ(s)，rk(s)]，xr(s,l)表示笛卡尔坐标系下横坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，yr(s,l)表示笛卡尔坐标系下纵坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，θr(s,l)表示笛卡尔坐标系下航向角通过沿轨迹s方向的位移和横向偏移l表示的关系，kr(s,l)表示笛卡尔坐标系下路径曲率通过沿轨迹s方向的位移和横向偏移l表示的关系；
则由上式推出车辆姿态向量
Figure BDA0002534949560000064
在Frenet坐标系下的坐标(s，l，l′，l″)：
Figure BDA0002534949560000062
l′＝(1-rκl)tanΔθ
Figure BDA0002534949560000063
其中，r(s)表示道路采样函数，即参考路径的函数表示；rk′表示沿道路采样函数的曲率导数，(s,l,l′,l″)为车辆的姿态向量转换到Frenet坐标系下的坐标，s表示沿道路参考线的距离，l表示距离参考线的横向距离，l′表示横向距离的导数，l″表示表示沿参考路径横向偏移的二阶导数，表征横向加速度；Δθ＝θ-rθ；(rx,ry,rθ,rk)为参考路径中距离环境中的车辆或障碍物(x,y,θ,k,v)最近的点的笛卡儿坐标系下的坐标；
所述模块M2包括：
设道路中心线上的参考点为(x0，y0)，(x1，y1)，…，(xn，yn)，每一个点(xi,yi)到点(xi+1,yi+1)区间的起始点和终点分别为(xi，yi)和(xi+1，yi+1),则三次样条曲线在每一个区间si的定义为fi(x)，表达式如下：
fi(x)＝ai+bi(x-xi)+ci(x-xi)2+di(x-xi)3
其中，
Figure BDA0002534949560000071
Figure BDA0002534949560000072
Figure BDA0002534949560000073
其中，hi＝xi+1-xi为点与点之间的步长，mi通过数值方法求解；
所述模块M4包括：
去除各轨迹中纵向速度超过预设最大速度的轨迹；去除各轨迹中纵向加速度超过预设最大加速度的轨迹；去除各轨迹中曲率超过预设最大曲率的轨迹。
优选地，所述模块M5中检测方法包括：
(xf1-xf2)2+(yf1-yf2)2≤(R1+R2)2
(xr1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xf1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xr1-xr2)2+(yf1-yf2)2≤(R1+R2)2
其中，c1r、c1f、c2r和c2f的坐标分别为(xr1，yr1)、(xf1，yf1)、(xr2，yr2)和(xf2，yf2)；c1r和c1f表示自车区域的后部和前部的半径为R1的两圆的圆心；c2r和c2f表示除自车之外的车辆的后部和前部的半径为R2的两圆的圆心；
所述模块M5中损失函数包括：
对纵向和横向的损失分别进行计算：
Figure BDA0002534949560000074
Cl＝wl1J(l(t))+wl2T+wl3l1 2
Ctotal＝wsCs+wlCl
其中，J(s(t))和J(l(t))分别为纵向和横向在t0～t1时间段内加速度的累计值；T代表当前规划的路径段所需的时间，在损失函数中加入时间项用以表征车辆在制动时能否响应足够快；
Figure BDA0002534949560000075
和l1 2分别表示纵向的速度偏差和横向相对参考路径的偏差，
Figure BDA0002534949560000076
表示车辆执行后在当前时间段最终所要保持的纵向速度；
Figure BDA0002534949560000077
表示每次规划t1时刻车辆的纵向速度，ws1、ws2、ws3以及wl1、wl2、wl3表示权重；Ctotal表示横向损失Cl和纵向损失Cs的加权总和，ws和wl分别表示对纵向和横向损失的权重；
所述模块M5中轨迹的事故严重程度计算公式如下：
Figure BDA0002534949560000078
其中，CIS表示连续事故严重程度，pk为所预测的轨迹属于严重程度为k的概率，ek为指数函数，K表示表征事故严重程度的类别个数。
与现有技术相比，本发明具有如下的有益效果：
1、本发明具有应用上的便利性，能够兼容目前针对无碰撞事故的智能车辆轨迹规划方法；
2、本发明能够在车辆面对无法避免的事故时，规划出事故严重程度最小的轨迹；
3、本发明解决了现有路径规划方法面临无法避免的事故时不能降低事故严重程度的问题，提高了事故发生时乘员的安全性。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明提供的轨迹规划方法流程示意图；
图2为本发明提供的轨迹规划采样示意图；
图3为本发明提供的碰撞检测示意图；
图4为本发明提供的车辆行驶过程中无碰撞轨迹簇示意图；
图5为本发明提供的车辆行驶过程中碰撞轨迹簇示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
本发明所要解决的技术问题是，针对现有无人驾驶车辆轨迹规划的技术不足，提供一种使无人驾驶车辆面对无法避免的事故时能够降低事故严重程度的无人驾驶车辆轨迹规划方法，使得所规划的轨迹能够提高乘员的安全性。
实施例1
根据本发明提供的一种降低事故严重程度的无人驾驶车辆轨迹规划方法，包括：
步骤M1：将车辆及环境信息由笛卡尔坐标系转换到Frenet坐标系；
步骤M2：在Frenet坐标系中利用三次样条曲线生成参考路径；
步骤M3：以撒点的方式沿参考路径进行采样，通过预设次多项式进行拟合，生成轨迹簇；
步骤M4：以最大速度、最大加速度、最大曲率作为控制约束，去除不符合控制约束的轨迹，保留符合控制约束的轨迹；
步骤M5：对保留的符合控制约束的轨迹进行碰撞检测，当检测后存在不碰撞的轨迹，则将不碰撞的轨迹记为无碰撞轨迹簇，在无碰撞轨迹簇中通过预设的损失函数筛选使损失函数值最小的轨迹作为最优轨迹；当检测后所有轨迹都将碰撞，即所有轨迹构成碰撞轨迹簇，则在碰撞轨迹簇中，通过事故严重程度预测模型筛选严重程度最低的轨迹作为最优轨迹；
所述事故严重程度预测模型是在车辆面临碰撞风险时，根据感知层获得的信息，提取包括车辆状态参数、车辆固有属性、人员属性以及组合参数作为事故严重程度预测模型的输入，实时预测主车在沿不同候选轨迹行进发生碰撞的事故严重程度，选择事故严重程度最低的轨迹作为最优轨迹。
具体地，所述步骤M1包括：将感知层获得的环境信息转换到Frenet坐标系中：
车辆的姿态向量
Figure BDA0002534949560000091
其中，(x,y)表示车辆在二维平面的位置，θ表示车辆的朝向，k表示曲率，即朝向θ的变化率，v表示车辆的速度；
则车辆的姿态变量满足下述关系：
Figure BDA0002534949560000092
其中，
Figure BDA0002534949560000093
表示x的导数，即x方向速度；
Figure BDA0002534949560000094
表示y的导数，即y方向速度；
Figure BDA0002534949560000095
表示θ的导数，即位于(x,y)位置的航向角的导数；
在上述基础上考虑一条由车辆运行产生的连续轨迹，记沿轨迹方向的位移为s方向，则沿轨迹s方向的位移与车辆姿态向量
Figure BDA0002534949560000096
的关系如下：
Figure BDA0002534949560000097
其中，dx表示微分；θ(s)表示车辆航向角关于沿轨迹s方向的位移的函数；k(s)表示路径曲率关于沿轨迹s方向的位移的函数；s代表道路的中心线切向方向的位移，也称纵向位移s；与此对应的是道路的中心线垂直方向位移l，也称横向位移；则由笛卡尔坐标系转换到Frenet坐标系：
Figure BDA0002534949560000101
其中，定义道路的采样函数为r(s)＝[rx(s)，ry(s)，rθ(s)，rκ(s)]，xr(s,l)表示笛卡尔坐标系下横坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，yr(s,l)表示笛卡尔坐标系下纵坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，θr(s,l)表示笛卡尔坐标系下航向角通过沿轨迹s方向的位移和横向偏移l表示的关系，kr(s,l)表示笛卡尔坐标系下路径曲率通过沿轨迹s方向的位移和横向偏移l表示的关系；
则由上式推出车辆姿态向量
Figure BDA0002534949560000107
在Frenet坐标系下的坐标(s，l，l′，l″)：
Figure BDA0002534949560000102
l′＝(1-rκl)tanΔθ
Figure BDA0002534949560000103
其中，r(s)表示道路采样函数，即参考路径的函数表示；rk′表示沿道路采样函数的曲率导数，(s,l,l′,l″)为车辆的姿态向量转换到Frenet坐标系下的坐标，s表示沿道路参考线的距离，l表示距离参考线的横向距离，l′表示横向距离的导数，l″表示表示沿参考路径横向偏移的二阶导数，表征横向加速度；Δθ＝θ-rθ；(rx,ry,rθ,rk)为参考路径中距离环境中的车辆或障碍物(x,y,θ,k,v)最近的点的笛卡儿坐标系下的坐标。
具体地，所述步骤M2包括：
设道路中心线上的参考点为(x0ty0)，(x1，y1)，…，(xn，yn)，每一个点(xi,yi)到点(xi+1,yi+1)区间的起始点和终点分别为(xi，yi)和(xi+1，yi+1),则三次样条曲线在每一个区间si的定义为fi(x)，表达式如下：
fi(x)＝ai+bi(x-xi)+ci(x-xi)2+di(x-xi)3
其中，
Figure BDA0002534949560000104
Figure BDA0002534949560000105
Figure BDA0002534949560000106
其中，hi＝xi+1-xi为点与点之间的步长，mi通过数值方法求解。
具体地，所述步骤M4包括：
去除各轨迹中纵向速度超过预设最大速度的轨迹；去除各轨迹中纵向加速度超过预设最大加速度的轨迹；去除各轨迹中曲率超过预设最大曲率的轨迹。
具体地，所述步骤M5中检测方法包括：
(xf1-xf2)2+(yf1-yf2)2≤(R1+R2)2
(xr1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xf1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xr1-xr2)2+(yf1-yf2)2≤(R1+R2)2
其中，c1r、c1f、c2r和c2f的坐标分别为(xr1，yr1)、(xf1，yf1)、(xr2，yr2)和(xf2，yf2)；c1r和c1f表示自车区域的后部和前部的半径为R1的两圆的圆心；c2r和c2f表示除自车之外的车辆的后部和前部的半径为R2的两圆的圆心。
具体地，所述步骤M5中损失函数包括：建立无人驾驶车辆轨迹规划问题的损失函数；
对纵向和横向的损失分别进行计算：
Figure BDA0002534949560000111
Cl＝wl1J(l(t))+wl2T+wl3l1 2
Ctotal＝wsCs+wlCl
其中，J(s(t))和J(l(t))分别为纵向和横向在t0～t1时间段内加速度的累计值；T代表当前规划的路径段所需的时间，在损失函数中加入时间项用以表征车辆在制动时能否响应足够快；
Figure BDA0002534949560000112
和l1 2分别表示纵向的速度偏差和横向相对参考路径的偏差，
Figure BDA0002534949560000113
表示车辆执行后在当前时间段最终所要保持的纵向速度；
Figure BDA0002534949560000114
表示每次规划t1时刻车辆的纵向速度，ws1、ws2、ws3以及wl1、wl2、wl3表示权重；Ctotal表示横向损失Cl和纵向损失Cs的加权总和，ws和wl分别表示对纵向和横向损失的权重。
具体地，所述事故严重程度预测模型为基于基于XGBoost的机器学习模型，以自车及其他车辆的质量、速度、航向角等参数以及其组合参数为输入，以事故严重程度为输出，同时能够预测事故各等级所对应的概率。所述事故严重程度为1,2,3,4，分别对应事故发生后车内乘员未受伤、轻伤、重伤及死亡。
因预测的事故严重程度1,2,3,4为离散值，不便于轨迹筛选时进行比较，进而结合其对每一事故严重程度等级的预测概率，对事故严重程度评价值做连续处理。
所述步骤M5中轨迹的事故严重程度计算公式如下：
Figure BDA0002534949560000115
其中，CIS表示连续事故严重程度，pk为所预测的轨迹属于严重程度为k的概率，ek为指数函数。
根据本发明提供的一种降低事故严重程度的无人驾驶车辆轨迹规划系统，包括：
模块M1：将车辆及环境信息由笛卡尔坐标系转换到Frenet坐标系；
模块M2：在Frenet坐标系中利用三次样条曲线生成参考路径；
模块M3：以撒点的方式沿参考路径进行采样，通过预设次多项式进行拟合，生成轨迹簇；
模块M4：以最大速度、最大加速度、最大曲率作为控制约束，去除不符合控制约束的轨迹，保留符合控制约束的轨迹；
模块M5：对保留的符合控制约束的轨迹进行碰撞检测，当检测后存在不碰撞的轨迹，则将不碰撞的轨迹记为无碰撞轨迹簇，在无碰撞轨迹簇中通过预设的损失函数筛选使损失函数值最小的轨迹作为最优轨迹；当检测后所有轨迹都将碰撞，即所有轨迹构成碰撞轨迹簇，则在碰撞轨迹簇中，通过事故严重程度预测模型筛选严重程度最低的轨迹作为最优轨迹；
所述事故严重程度预测模型是在车辆面临碰撞风险时，根据感知层获得的信息，提取包括车辆状态参数、车辆固有属性、人员属性以及组合参数作为事故严重程度预测模型的输入，实时预测主车在沿不同候选轨迹行进发生碰撞的事故严重程度，选择事故严重程度最低的轨迹作为最优轨迹。
具体地，所述模块M1包括：将感知层获得的环境信息转换到Frenet坐标系中：
车辆的姿态向量
Figure BDA0002534949560000121
其中，(x,y)表示车辆在二维平面的位置，θ表示车辆的朝向，k表示曲率，即朝向θ的变化率，v表示车辆的速度；
则车辆的姿态变量满足下述关系：
Figure BDA0002534949560000122
其中，
Figure BDA0002534949560000123
表示x的导数，即x方向速度；
Figure BDA0002534949560000124
表示y的导数，即y方向速度；
Figure BDA0002534949560000125
表示θ的导数，即位于(x,y)位置的航向角的导数；
在上述基础上考虑一条由车辆运行产生的连续轨迹，记沿轨迹方向的位移为s方向，则沿轨迹s方向的位移与车辆姿态向量
Figure BDA0002534949560000126
的关系如下：
Figure BDA0002534949560000127
其中，dx表示微分；θ(s)表示车辆航向角关于沿轨迹s方向的位移的函数；k(s)表示路径曲率关于沿轨迹s方向的位移的函数；s代表道路的中心线切向方向的位移，也称纵向位移s；与此对应的是道路的中心线垂直方向位移l，也称横向位移；则由笛卡尔坐标系转换到Frenet坐标系：
Figure BDA0002534949560000131
其中，定义道路的采样函数为r(s)＝[rx(s)，ry(s)，rθ(s)，rκ(s)]，xr(s,l)表示笛卡尔坐标系下横坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，yr(s,l)表示笛卡尔坐标系下纵坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，θr(s,l)表示笛卡尔坐标系下航向角通过沿轨迹s方向的位移和横向偏移l表示的关系，kr(s,l)表示笛卡尔坐标系下路径曲率通过沿轨迹s方向的位移和横向偏移l表示的关系；
则由上式推出车辆姿态向量
Figure BDA0002534949560000134
在Frenet坐标系下的坐标(s，l，l′，l″)：
Figure BDA0002534949560000132
l′＝(1-rκl)tanΔθ
Figure BDA0002534949560000133
其中，r(s)表示道路采样函数，即参考路径的函数表示；rk′表示沿道路采样函数的曲率导数，(s,l,l′,l″)为车辆的姿态向量转换到Frenet坐标系下的坐标，s表示沿道路参考线的距离，l表示距离参考线的横向距离，l′表示横向距离的导数，l″表示表示沿参考路径横向偏移的二阶导数，表征横向加速度；Δθ＝θ-rθ；(rx,ry,rθ,rk)为参考路径中距离环境中的车辆或障碍物(x,y,θ,k,v)最近的点的笛卡儿坐标系下的坐标。
具体地，所述模块M2包括：
设道路中心线上的参考点为(x0，y0)，(x1，y1)，…，(xn，yn)，每一个点(xi,yi)到点(xi+1,yi+1)区间的起始点和终点分别为(xi，yi)和(xi+1，yi+1),则三次样条曲线在每一个区间si的定义为fi(x)，表达式如下：
fi(x)＝ai+bi(x-xi)+ci(x-xi)2+di(x-xi)3
其中，
Figure BDA0002534949560000141
Figure BDA0002534949560000142
Figure BDA0002534949560000143
其中，hi＝xi+1-xi为点与点之间的步长，mi通过数值方法求解。
具体地，所述模块M4包括：
去除各轨迹中纵向速度超过预设最大速度的轨迹；去除各轨迹中纵向加速度超过预设最大加速度的轨迹；去除各轨迹中曲率超过预设最大曲率的轨迹。
具体地，所述模块M5中检测方法包括：
(xf1-xf2)2+(yf1-yf2)2≤(R1+R2)2
(xr1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xf1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xr1-xr2)2+(yf1-yf2)2≤(R1+R2)2
其中，c1r、c1f、c2r和c2f的坐标分别为(xr1，yr1)、(xf1，yf1)、(xr2，yr2)和(xf2，yf2)；c1r和c1f表示自车区域的后部和前部的半径为R1的两圆的圆心；c2r和c2f表示除自车之外的车辆的后部和前部的半径为R2的两圆的圆心。
具体地，所述模块M5中损失函数包括：
对纵向和横向的损失分别进行计算：
Figure BDA0002534949560000144
Cl＝wl1J(l(t))+wl2T+wl3l1 2
Ctotal＝wsCs+wlCl
其中，J(s(t))和J(l(t))分别为纵向和横向在t0～t1时间段内加速度的累计值；T代表当前规划的路径段所需的时间，在损失函数中加入时间项用以表征车辆在制动时能否响应足够快；
Figure BDA0002534949560000145
和l1 2分别表示纵向的速度偏差和横向相对参考路径的偏差，
Figure BDA0002534949560000146
表示车辆执行后在当前时间段最终所要保持的纵向速度；
Figure BDA0002534949560000147
表示每次规划t1时刻车辆的纵向速度，ws1、ws2、ws3以及wl1、wl2、wl3表示权重；Ctotal表示横向损失Cl和纵向损失Cs的加权总和，ws和wl分别表示对纵向和横向损失的权重。
具体地，所述事故严重程度预测模型为基于XGBoost的机器学习模型，以自车及其他车辆的质量、速度、航向角等参数以及其组合参数为输入，以事故严重程度为输出，同时能够预测事故各等级所对应的概率。所述事故严重程度为1,2,3,4，分别对应事故发生后车内乘员未受伤、轻伤、重伤及死亡。
因预测的事故严重程度1,2,3,4为离散值，不便于轨迹筛选时进行比较，进而结合其对每一事故严重程度等级的预测概率，对事故严重程度评价值做连续处理。
所述模块M5中轨迹的事故严重程度计算公式如下：
Figure BDA0002534949560000151
其中，CIS表示连续事故严重程度，pk为所预测的轨迹属于严重程度为k的概率，ek为指数函数。
实施例2
实施例2是实施例1的变化例
根据本发明提供的一种降低事故严重程度的无人驾驶车辆轨迹规划方法按如下步骤进行：如图1所示，
步骤101：由无人驾驶车辆感知层获得车辆及环境信息，将获得的信息转化至Frenet坐标系下。具体转化如下：
获得的信息在Frenet坐标系下的坐标(s，l，l′，l″)：
s＝r(s)
Figure BDA0002534949560000153
l′＝(1-rκl)tanΔθ
Figure BDA0002534949560000152
其中，(s，l，l′，l″)为车辆的姿态向量转换到Frenet坐标系下的坐标，对应的分别为沿道路参考线的距离，距离参考线的横向距离，横向距离的倒数、二阶导数。Δθ＝θ-rθ。[rx，ry，rθ，rκ]为参考路径中距离环境中车辆或障碍物(x，y，θ，κ，v)最近的点的笛卡儿坐标系下的坐标。
步骤102：根据行驶任务要求，利用感知层获得的行驶区域内重要的航路点利用三次样条曲线插值拟合参考路径。具体如下：
设参考航路点为(x0，y0)，(x1，y1)，…，(xn，yn)，每一个区间的起始点和终点分别为(xi，yi和(xi+1，yi+1),则三次样条曲线在每一个区间si的定义如下：
fi(x)＝ai+bi(x-xi)+ci(x-xi)2+di(x-xi)3
其中，
Figure BDA0002534949560000161
Figure BDA0002534949560000162
Figure BDA0002534949560000163
其中hi＝xi+1-xi为点与点之间的步长，mi可通过数值方法求解。
步骤103：根据步骤102生成的参考路径(一般是道路中心线)，设定纵向和横向的采样范围(Tmin，Tmax)和(lmin，lmax)，并通过Δl和ΔT来限制采样密度，相应的采样示意图如图2所示。
根据不同时刻的采样点集运用插值拟合的方法生成横向和纵向的轨迹，具体插值拟合计算如下：
以横向路径规划为例，初始配置为
Figure BDA0002534949560000164
目标配置为
Figure BDA0002534949560000165
设车辆由t0时刻从初始位置在时间t1到达目标位置。
则可得到下列方程组
Figure BDA0002534949560000166
通过令t0＝0简化上述方程，可求得
Figure BDA0002534949560000167
令T＝t1-t0，则αl3、αl4、αl5可通过下式求得：
Figure BDA0002534949560000168
同理可得纵向的路径规划。
记录横向轨迹的时刻、偏移、速度、加速度、加加速度；纵向轨迹的时刻、位置、速度、加速度、加加速度。根据时间一致性，从Frenet坐标系将规划的轨迹转换至笛卡尔坐标系下的轨迹坐标，形成备选轨迹簇。
步骤104：根据预先设置的最大速度、最大加速度、最大曲率，去除备选轨迹簇中大于预先设置的最大速度、最大加速度、最大曲率的轨迹。
步骤105：针对步骤104中剩下的轨迹进行碰撞检测。碰撞检测如图3所示。为简化计算，绿色车辆通过以c1r和c1f为圆心，以R1为半径的两个圆表示；红色车辆通过以c2r和c2f为圆心，以R2为半径的两个圆表示。计算c1r和c1f同c2r和c2f之间的距离，若该距离的平方不大于(R1+R2)2，则表示该轨迹存在碰撞。具体计算如下：
(xf1-xf2)2+(yf1-yf2)2≤(R1+R2)2
(xr1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xf1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xr1-xr2)2+(yf1-yf2)2≤(R1+R2)2
其中c1r、c1f、c2r和c2f的坐标分别为(xr1，yr1)、(xf1，yf1)、(xr2，yr2)和(xf2tyf2)。
此时分两种情况讨论：
若经步骤105进行碰撞检测后，仍存在轨迹不发生碰撞，转至步骤1061，即不碰撞轨迹的集合为无碰撞轨迹簇。
步骤1071则根据设计的损失函数，比较无碰撞轨迹簇中轨迹的损失Ctotal，选择无碰撞轨迹簇中Ctotal最小的轨迹作为最优轨迹输出，即步骤108。
具体如图4所示，自车在行驶过程中同前车不碰撞的轨迹，经步骤1071后输出最优轨迹。
其中损失函数计算如下：
Figure BDA0002534949560000171
Cl＝wl1J(l(t))+wl2T+Wl3l1 2
Ctotal＝wsCs+wlCl
其中J(s(t))和J(l(t))分别为纵向和横向在t0～t1时间段内加加速度的累计值；T代表当前规划的路径段所需的时间，在损失函数中加入时间项用以表征车辆在制动时能否响应足够快；
Figure BDA0002534949560000172
和l1 2分别表示纵向和横向的速度偏差和相对参考路径的偏差，其中
Figure BDA0002534949560000173
表示车辆执行后在当前时间段最终所要保持的纵向速度，
Figure BDA0002534949560000174
表示每次规划t1时刻车辆的纵向速度。ws1、ws2、ws3以及wl1、wl2、wl3表示权重。
Ctotal表示横向损失Cl和纵向损失Cs的加权总和，ws和wl分别表示对纵向和横向损失的权重。
若经步骤105进行碰撞检测后，不存在轨迹不发生碰撞，转至步骤1062，即碰撞轨迹的集合为碰撞轨迹簇。
步骤1072则根据构建的事故严重程度评价模型，评价碰撞轨迹簇中各轨迹的事故严重程度CIS，选择事故严重程度最小的轨迹作为最优轨迹输出，即步骤108。
所述事故严重程度预测模型是基于XGBoost的机器学习模型。以车辆状态参数(如速度、航向角等)、固有属性(如质量、车辆类型等)、人员属性以及其组合参数为输入(包括但不限于上述示例参数)，以事故严重程度为输出(指乘员未受伤、轻伤、重伤、死亡)。在车辆面临碰撞风险时，即检测的轨迹都将发生碰撞。根据感知层获得的信息，提取上述所需参数作为事故严重程度预测模型的输入，实时预测主车在沿不同候选轨迹行进发生碰撞的事故严重程度，选择事故严重程度最低的轨迹最为最优轨迹。减轻碰撞，提升车辆安全性。
具体如图5所示，自车在行驶过程中同前车发生碰撞的轨迹，步骤1072将选出其中事故严重程度最低，即CIS值最小的轨迹作为最优轨迹。
所述事故严重程度评价模型为基于XGBoost的机器学习模型，其输入为通过感知层获得的环境信息参数及其组合，输出为事故严重程度。
所述事故严重程度为1,2,3,4，分别对应事故发生后车内乘员未受伤、轻伤、重伤及死亡。
因预测的事故严重程度1,2,3,4为离散值，不便于轨迹筛选，进而结合其对每一事故严重程度等级的预测概率，对事故严重程度评价值做连续处理。
所述轨迹的事故严重程度计算如下：
Figure BDA0002534949560000181
其中，CIS表示连续事故严重程度，pk为所预测的轨迹属于严重程度为k的概率，ek为指数函数。
在本申请的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种降低事故严重程度的无人驾驶车辆轨迹规划方法，其特征在于，包括：
步骤M1：将车辆及环境信息由笛卡尔坐标系转换到Frenet坐标系；
步骤M2：在Frenet坐标系中利用三次样条曲线生成参考路径；
步骤M3：以撒点的方式沿参考路径进行采样，通过预设次多项式进行拟合，生成轨迹簇；
步骤M4：以最大速度、最大加速度、最大曲率作为控制约束，去除不符合控制约束的轨迹，保留符合控制约束的轨迹；
步骤M5：对保留的符合控制约束的轨迹进行碰撞检测，当检测后存在不碰撞的轨迹，则将不碰撞的轨迹记为无碰撞轨迹簇，在无碰撞轨迹簇中通过预设的损失函数筛选使损失函数值最小的轨迹作为最优轨迹；当检测后所有轨迹都将碰撞，即所有轨迹构成碰撞轨迹簇，则在碰撞轨迹簇中，通过事故严重程度预测模型筛选严重程度最低的轨迹作为最优轨迹；
所述事故严重程度预测模型是在车辆面临碰撞风险时，根据感知层获得的信息，提取包括车辆状态参数、车辆固有属性、人员属性以及组合参数作为事故严重程度预测模型的输入，实时预测主车在沿不同候选轨迹行进发生碰撞的事故严重程度，选择事故严重程度最低的轨迹作为最优轨迹。
2.根据权利要求1所述的降低事故严重程度的无人驾驶车辆轨迹规划方法，其特征在于，所述步骤M1包括：
车辆的姿态向量
Figure RE-FDA0002636185520000011
其中，(x,y)表示车辆在二维平面的位置，θ表示车辆的朝向，k表示曲率，即朝向θ的变化率，v表示车辆的速度；
则车辆的姿态变量满足下述关系：
Figure RE-FDA0002636185520000012
其中，
Figure RE-FDA0002636185520000013
表示x的导数，即x方向速度；
Figure RE-FDA0002636185520000014
表示y的导数，即y方向速度；
Figure RE-FDA0002636185520000015
表示θ的导数，即位于(x,y)位置的航向角的导数；
在上述基础上考虑一条由车辆运行产生的连续轨迹，记沿轨迹方向的位移为s方向，则沿轨迹s方向的位移与车辆姿态向量
Figure RE-FDA0002636185520000016
的关系如下：
Figure RE-FDA0002636185520000021
其中，dx表示微分；θ(s)表示车辆航向角关于沿轨迹s方向的位移的函数；k(s)表示路径曲率关于沿轨迹s方向的位移的函数；s代表道路的中心线切向方向的位移，也称纵向位移s；与此对应的是道路的中心线垂直方向位移l，也称横向位移；则由笛卡尔坐标系转换到Frenet坐标系：
Figure RE-FDA0002636185520000022
其中，定义道路的采样函数为r(s)＝[rx(s)，ry(s)，rθ(s)，rκ(s)]，xr(s，l)表示笛卡尔坐标系下横坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，yr(s，l)表示笛卡尔坐标系下纵坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，θr(s，l)表示笛卡尔坐标系下航向角通过沿轨迹s方向的位移和横向偏移l表示的关系，kr(s，l)表示笛卡尔坐标系下路径曲率通过沿轨迹s方向的位移和横向偏移l表示的关系；
则由上式推出车辆姿态向量
Figure RE-FDA0002636185520000025
在Frenet坐标系下的坐标(s，l，l′，l″)：
s＝r(s)
Figure RE-FDA0002636185520000023
l′＝(1-rκl)tanΔθ
Figure RE-FDA0002636185520000024
其中，r(s)表示道路采样函数，即参考路径的函数表示；rk′表示沿道路采样函数的曲率导数，(s，l，l′，l″)为车辆的姿态向量转换到Frenet坐标系下的坐标，s表示沿道路参考线的距离，l表示距离参考线的横向距离，l′表示横向距离的导数，l″表示表示沿参考路径横向偏移的二阶导数，表征横向加速度；Δθ＝θ-rθ；(rx，rt，rθ，rk)为参考路径中距离环境中的车辆或障碍物(x，y，θ，k，v)最近的点的笛卡儿坐标系下的坐标。
3.根据权利要求1所述的降低事故严重程度的无人驾驶车辆轨迹规划方法，其特征在于，所述步骤M2包括：
设道路中心线上的参考点为(x0，y0)，(x1，y1)，…，(xn，yn)，每一个点(xi，yi)到点(xi+1，yi+1)区间的起始点和终点分别为(xi，yi)和(xi+1，yi+1)，则三次样条曲线在每一个区间si的定义为fi(x)，表达式如下：
fi(x)＝ai+bi(x-xi)+ci(x-xi)2+di(x-xi)3
其中，
ai＝yi
Figure RE-FDA0002636185520000031
Figure RE-FDA0002636185520000032
Figure RE-FDA0002636185520000033
其中，hi＝xi+1-xi为点与点之间的步长，mi通过数值方法求解。
4.根据权利要求1所述的降低事故严重程度的无人驾驶车辆轨迹规划方法，其特征在于，所述步骤M4包括：
去除各轨迹中纵向速度超过预设最大速度的轨迹；去除各轨迹中纵向加速度超过预设最大加速度的轨迹；去除各轨迹中曲率超过预设最大曲率的轨迹。
5.根据权利要求1所述的降低事故严重程度的无人驾驶车辆轨迹规划方法，其特征在于，所述步骤M5中检测方法包括：
(xf1-xf2)2+(yf1-yf2)2≤(R1+R2)2
(xr1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xf1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xr1-xr2)2+(yf1-yf2)2≤(R1+R2)2
其中，c1r、c1f、c2r和c2f的坐标分别为(xr1，yr1)、(xf1，yf1)、(xr2，yr2)和(xf2，yf2)；c1r和c1f表示自车区域的后部和前部的半径为R1的两圆的圆心；c2r和c2f表示除自车之外的车辆的后部和前部的半径为R2的两圆的圆心。
6.根据权利要求5所述的降低事故严重程度的无人驾驶车辆轨迹规划方法，其特征在于，所述步骤M5中损失函数包括：
对纵向和横向的损失分别进行计算：
Figure RE-FDA0002636185520000034
Cl＝wl1J(l(t))+wl2T+wl3l1 2
Ctotal＝wsCs+wlCl
其中，J(s(t))和J(l(t))分别为纵向和横向在t0～t1时间段内加速度的累计值；T代表当前规划的路径段所需的时间，在损失函数中加入时间项用以表征车辆在制动时能否响应足够快；
Figure RE-FDA0002636185520000035
和l1 2分别表示纵向的速度偏差和横向相对参考路径的偏差，
Figure RE-FDA0002636185520000036
表示车辆执行后在当前时间段最终所要保持的纵向速度；
Figure RE-FDA0002636185520000037
表示每次规划t1时刻车辆的纵向速度，ws1、ws2、ws3以及wl1、wl2、wl3表示权重；Ctotal表示横向损失Cl和纵向损失Cs的加权总和，ws和wl分别表示对纵向和横向损失的权重。
7.根据权利要求6所述的降低事故严重程度的无人驾驶车辆轨迹规划方法，其特征在于，所述步骤M5中轨迹的事故严重程度计算公式如下：
Figure RE-FDA0002636185520000041
其中，CIS表示连续事故严重程度，pk为所预测的轨迹属于严重程度为k的概率，ek为指数函数，K表示表征事故严重程度的类别个数。
8.一种降低事故严重程度的无人驾驶车辆轨迹规划系统，其特征在于，包括：
模块M1：将车辆及环境信息由笛卡尔坐标系转换到Frenet坐标系；
模块M2：在Frenet坐标系中利用三次样条曲线生成参考路径；
模块M3：以撒点的方式沿参考路径进行采样，通过预设次多项式进行拟合，生成轨迹簇；
模块M4：以最大速度、最大加速度、最大曲率作为控制约束，去除不符合控制约束的轨迹，保留符合控制约束的轨迹；
模块M5：对保留的符合控制约束的轨迹进行碰撞检测，当检测后存在不碰撞的轨迹，则将不碰撞的轨迹记为无碰撞轨迹簇，在无碰撞轨迹簇中通过预设的损失函数筛选使损失函数值最小的轨迹作为最优轨迹；当检测后所有轨迹都将碰撞，即所有轨迹构成碰撞轨迹簇，则在碰撞轨迹簇中，通过事故严重程度预测模型筛选严重程度最低的轨迹作为最优轨迹；
所述事故严重程度预测模型是在车辆面临碰撞风险时，根据感知层获得的信息，提取包括车辆状态参数、车辆固有属性、人员属性以及组合参数作为事故严重程度预测模型的输入，实时预测主车在沿不同候选轨迹行进发生碰撞的事故严重程度，选择事故严重程度最低的轨迹作为最优轨迹。
9.根据权利要求8所述的降低事故严重程度的无人驾驶车辆轨迹规划系统，其特征在于，所述模块M1包括：
车辆的姿态向量
Figure RE-FDA0002636185520000042
其中，(x，y)表示车辆在二维平面的位置，θ表示车辆的朝向，k表示曲率，即朝向θ的变化率，v表示车辆的速度；
则车辆的姿态变量满足下述关系：
Figure RE-FDA0002636185520000043
其中，
Figure RE-FDA0002636185520000051
表示x的导数，即x方向速度；
Figure RE-FDA0002636185520000052
表示y的导数，即y方向速度；
Figure RE-FDA0002636185520000053
表示θ的导数，即位于(x，y)位置的航向角的导数；
在上述基础上考虑一条由车辆运行产生的连续轨迹，记沿轨迹方向的位移为s方向，则沿轨迹s方向的位移与车辆姿态向量
Figure RE-FDA0002636185520000054
的关系如下：
Figure RE-FDA0002636185520000055
其中，dx表示微分；θ(s)表示车辆航向角关于沿轨迹s方向的位移的函数；k(s)表示路径曲率关于沿轨迹s方向的位移的函数；s代表道路的中心线切向方向的位移，也称纵向位移s；与此对应的是道路的中心线垂直方向位移l，也称横向位移；则由笛卡尔坐标系转换到Frenet坐标系：
Figure RE-FDA0002636185520000056
其中，定义道路的采样函数为r(s)＝[rx(s)，ry(s)，rθ(s)，rκ(s)]，xr(s，l)表示笛卡尔坐标系下横坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，yr(s，l)表示笛卡尔坐标系下纵坐标通过沿轨迹s方向的位移和横向偏移l表示的关系，θr(s，l)表示笛卡尔坐标系下航向角通过沿轨迹s方向的位移和横向偏移l表示的关系，kr(s，l)表示笛卡尔坐标系下路径曲率通过沿轨迹s方向的位移和横向偏移l表示的关系；
则由上式推出车辆姿态向量
Figure RE-FDA0002636185520000059
在Frenet坐标系下的坐标(s，l，l′，l″)：
s＝r(s)
Figure RE-FDA0002636185520000057
l′＝(1-rκl)tanΔθ
Figure RE-FDA0002636185520000058
其中，r(s)表示道路采样函数，即参考路径的函数表示；rk′表示沿道路采样函数的曲率导数，(s，l，l′，l″)为车辆的姿态向量转换到Frenet坐标系下的坐标，s表示沿道路参考线的距离，l表示距离参考线的横向距离，l′表示横向距离的导数，l″表示表示沿参考路径横向偏移的二阶导数，表征横向加速度；Δθ＝θ-rθ；(rx，ry，rθ，rk)为参考路径中距离环境中的车辆或障碍物(x，y，θ，k，v)最近的点的笛卡儿坐标系下的坐标；
所述模块M2包括：
设道路中心线上的参考点为(x0，y0)，(x1，y1)，…，(xn，yn)，每一个点(xi，yi)到点(xi+1，yi+1)区间的起始点和终点分别为(xi，yi)和(xi+1，yi+1)，则三次样条曲线在每一个区间si的定义为fi(x)，表达式如下：
fi(x)＝ai+bi(x-xi)+ci(x-xi)2+di(x-xi)3
其中，
ai＝yi
Figure RE-FDA0002636185520000061
Figure RE-FDA0002636185520000062
Figure RE-FDA0002636185520000063
其中，hi＝xi+1-xi为点与点之间的步长，mi通过数值方法求解；
所述模块M4包括：
去除各轨迹中纵向速度超过预设最大速度的轨迹；去除各轨迹中纵向加速度超过预设最大加速度的轨迹；去除各轨迹中曲率超过预设最大曲率的轨迹。
10.根据权利要求8所述的降低事故严重程度的无人驾驶车辆轨迹规划系统，其特征在于，所述模块M5中检测方法包括：
(xf1-xf2)2+(yf1-yf2)2≤(R1+R2)2
(xr1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xf1-xf2)2+(yr1-yr2)2≤(R1+R2)2
(xr1-xr2)2+(yf1-yf2)2≤(R1+R2)2
其中，c1r、c1f、c2r和c2f的坐标分别为(xr1，yr1)、(xf1，yf1)、(xr2，yr2)和(xf2，yf2)；c1r和c1f表示自车区域的后部和前部的半径为R1的两圆的圆心；c2r和c2f表示除自车之外的车辆的后部和前部的半径为R2的两圆的圆心；
所述模块M5中损失函数包括：
对纵向和横向的损失分别进行计算：
Figure RE-FDA0002636185520000064
Cl＝wl1J(l(t))+wl2T+wl3l1 2
Ctotal＝wsCs+wlCl
其中，J(s(t))和J(l(t))分别为纵向和横向在t0～t1时间段内加速度的累计值；T代表当前规划的路径段所需的时间，在损失函数中加入时间项用以表征车辆在制动时能否响应足够快；
Figure RE-FDA0002636185520000065
和l1 2分别表示纵向的速度偏差和横向相对参考路径的偏差，
Figure RE-FDA0002636185520000066
表示车辆执行后在当前时间段最终所要保持的纵向速度；
Figure RE-FDA0002636185520000067
表示每次规划t1时刻车辆的纵向速度，ws1、ws2、ws3以及wl1、wl2、wl3表示权重；Ctotal表示横向损失Cl和纵向损失Cs的加权总和，ws和wl分别表示对纵向和横向损失的权重；
所述模块M5中轨迹的事故严重程度计算公式如下：
Figure RE-FDA0002636185520000071
其中，CIS表示连续事故严重程度，pk为所预测的轨迹属于严重程度为k的概率，ek为指数函数，K表示表征事故严重程度的类别个数。
CN202010529704.3A 2020-06-11 2020-06-11 降低事故严重程度的无人驾驶车辆轨迹规划方法及系统 Active CN111750887B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010529704.3A CN111750887B (zh) 2020-06-11 2020-06-11 降低事故严重程度的无人驾驶车辆轨迹规划方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010529704.3A CN111750887B (zh) 2020-06-11 2020-06-11 降低事故严重程度的无人驾驶车辆轨迹规划方法及系统

Publications (2)

Publication Number Publication Date
CN111750887A true CN111750887A (zh) 2020-10-09
CN111750887B CN111750887B (zh) 2023-11-21

Family

ID=72675064

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010529704.3A Active CN111750887B (zh) 2020-06-11 2020-06-11 降低事故严重程度的无人驾驶车辆轨迹规划方法及系统

Country Status (1)

Country Link
CN (1) CN111750887B (zh)

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112577506A (zh) * 2020-10-30 2021-03-30 上汽大众汽车有限公司 一种自动驾驶局部路径规划方法和系统
CN112572472A (zh) * 2020-12-08 2021-03-30 重庆大学 一种基于Frenet坐标系的自动驾驶碰撞预测方法
CN112874509A (zh) * 2021-03-03 2021-06-01 知行汽车科技（苏州）有限公司 基于智能驾驶员模型idm的轨迹规划方法、装置及存储介质
CN113029151A (zh) * 2021-03-15 2021-06-25 齐鲁工业大学 一种智能车辆路径规划方法
CN113247023A (zh) * 2021-06-30 2021-08-13 上海商汤临港智能科技有限公司 一种行驶规划方法、装置、计算机设备及存储介质
CN113386795A (zh) * 2021-07-05 2021-09-14 西安电子科技大学芜湖研究院 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统
CN113483776A (zh) * 2021-07-01 2021-10-08 广州小鹏自动驾驶科技有限公司 路径规划方法、装置及汽车
CN113485370A (zh) * 2021-08-11 2021-10-08 北方工业大学 一种并联机器人动态拾放轨迹规划方法及系统
CN113537606A (zh) * 2021-07-22 2021-10-22 上汽通用五菱汽车股份有限公司 事故预测方法、装置及计算机可读存储介质
CN113581181A (zh) * 2021-08-04 2021-11-02 武汉理工大学 智能车辆超车轨迹的规划方法
CN113701777A (zh) * 2021-08-28 2021-11-26 浙江省测绘科学技术研究院 基于空间向量的高精地图车道关联轨迹线自动生成方法
CN113721637A (zh) * 2021-11-02 2021-11-30 武汉理工大学 智能车动态避障路径连续规划方法、系统及存储介质
CN113870316A (zh) * 2021-10-19 2021-12-31 青岛德智汽车科技有限公司 一种无gps跟车场景下前车路径重建方法
CN114194215A (zh) * 2021-12-30 2022-03-18 江苏大学 一种智能车辆避障换道轨迹规划方法及系统
CN115127576A (zh) * 2022-09-02 2022-09-30 青岛慧拓智能机器有限公司 路径规划方法、装置、芯片、终端、电子设备及存储介质
WO2022222386A1 (zh) * 2021-04-19 2022-10-27 陕西理工大学 车辆换道轨迹规划方法、装置、设备、存储介质及车辆
CN115489548A (zh) * 2022-09-20 2022-12-20 重庆大学 智能汽车园区道路路径规划方法
WO2023092837A1 (zh) * 2021-11-26 2023-06-01 上海仙途智能科技有限公司 碰撞检测
CN117075619A (zh) * 2023-10-17 2023-11-17 之江实验室 一种局部路径规划方法、装置和介质
CN117475090A (zh) * 2023-12-27 2024-01-30 粤港澳大湾区数字经济研究院(福田) 一种轨迹生成模型、轨迹生成方法、装置、终端及介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102011115875A1 (de) * 2011-10-12 2013-04-18 Volkswagen Aktiengesellschaft Verfahren für ein Fahrerassistenzsystem eines Fahrzeugs
CN109035862A (zh) * 2018-08-06 2018-12-18 清华大学 一种基于车车通信的多车协同换道控制方法
CN110103956A (zh) * 2019-05-16 2019-08-09 北方工业大学 一种无人车自动超车轨迹规划方法
CN110362077A (zh) * 2019-07-03 2019-10-22 上海交通大学 无人驾驶车辆紧急避险决策系统、方法及介质
CN110749333A (zh) * 2019-11-07 2020-02-04 中南大学 基于多目标优化的无人驾驶车辆运动规划方法
CN110851958A (zh) * 2019-10-16 2020-02-28 清华大学 碰撞严重性的预测方法
CN111145552A (zh) * 2020-01-06 2020-05-12 重庆大学 基于5g网络的车辆动态换道轨迹的规划方法
US20200149906A1 (en) * 2017-08-31 2020-05-14 Guangzhou Xiaopeng Motors Technology Co., Ltd. Path planning method, system and device for autonomous driving

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102011115875A1 (de) * 2011-10-12 2013-04-18 Volkswagen Aktiengesellschaft Verfahren für ein Fahrerassistenzsystem eines Fahrzeugs
US20200149906A1 (en) * 2017-08-31 2020-05-14 Guangzhou Xiaopeng Motors Technology Co., Ltd. Path planning method, system and device for autonomous driving
CN109035862A (zh) * 2018-08-06 2018-12-18 清华大学 一种基于车车通信的多车协同换道控制方法
CN110103956A (zh) * 2019-05-16 2019-08-09 北方工业大学 一种无人车自动超车轨迹规划方法
CN110362077A (zh) * 2019-07-03 2019-10-22 上海交通大学 无人驾驶车辆紧急避险决策系统、方法及介质
CN110851958A (zh) * 2019-10-16 2020-02-28 清华大学 碰撞严重性的预测方法
CN110749333A (zh) * 2019-11-07 2020-02-04 中南大学 基于多目标优化的无人驾驶车辆运动规划方法
CN111145552A (zh) * 2020-01-06 2020-05-12 重庆大学 基于5g网络的车辆动态换道轨迹的规划方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
郭蓬;吴学易;戎辉;唐风敏;李鑫慧;华一丁: "基于代价函数的无人驾驶汽车局部路径规划算法", 中国公路学报, no. 006 *

Cited By (26)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112577506A (zh) * 2020-10-30 2021-03-30 上汽大众汽车有限公司 一种自动驾驶局部路径规划方法和系统
CN112572472B (zh) * 2020-12-08 2021-12-14 重庆大学 一种基于Frenet坐标系的自动驾驶碰撞预测方法
CN112572472A (zh) * 2020-12-08 2021-03-30 重庆大学 一种基于Frenet坐标系的自动驾驶碰撞预测方法
CN112874509A (zh) * 2021-03-03 2021-06-01 知行汽车科技（苏州）有限公司 基于智能驾驶员模型idm的轨迹规划方法、装置及存储介质
CN113029151A (zh) * 2021-03-15 2021-06-25 齐鲁工业大学 一种智能车辆路径规划方法
WO2022222386A1 (zh) * 2021-04-19 2022-10-27 陕西理工大学 车辆换道轨迹规划方法、装置、设备、存储介质及车辆
CN113247023A (zh) * 2021-06-30 2021-08-13 上海商汤临港智能科技有限公司 一种行驶规划方法、装置、计算机设备及存储介质
CN113483776A (zh) * 2021-07-01 2021-10-08 广州小鹏自动驾驶科技有限公司 路径规划方法、装置及汽车
CN113386795B (zh) * 2021-07-05 2022-07-01 西安电子科技大学芜湖研究院 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统
CN113386795A (zh) * 2021-07-05 2021-09-14 西安电子科技大学芜湖研究院 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统
CN113537606A (zh) * 2021-07-22 2021-10-22 上汽通用五菱汽车股份有限公司 事故预测方法、装置及计算机可读存储介质
CN113581181A (zh) * 2021-08-04 2021-11-02 武汉理工大学 智能车辆超车轨迹的规划方法
CN113485370A (zh) * 2021-08-11 2021-10-08 北方工业大学 一种并联机器人动态拾放轨迹规划方法及系统
CN113701777A (zh) * 2021-08-28 2021-11-26 浙江省测绘科学技术研究院 基于空间向量的高精地图车道关联轨迹线自动生成方法
CN113701777B (zh) * 2021-08-28 2022-11-04 浙江省测绘科学技术研究院 基于空间向量的高精地图车道关联轨迹线自动生成方法
CN113870316A (zh) * 2021-10-19 2021-12-31 青岛德智汽车科技有限公司 一种无gps跟车场景下前车路径重建方法
CN113870316B (zh) * 2021-10-19 2023-08-15 青岛德智汽车科技有限公司 一种无gps跟车场景下前车路径重建方法
CN113721637A (zh) * 2021-11-02 2021-11-30 武汉理工大学 智能车动态避障路径连续规划方法、系统及存储介质
WO2023092837A1 (zh) * 2021-11-26 2023-06-01 上海仙途智能科技有限公司 碰撞检测
CN114194215A (zh) * 2021-12-30 2022-03-18 江苏大学 一种智能车辆避障换道轨迹规划方法及系统
CN115127576A (zh) * 2022-09-02 2022-09-30 青岛慧拓智能机器有限公司 路径规划方法、装置、芯片、终端、电子设备及存储介质
CN115127576B (zh) * 2022-09-02 2022-12-13 青岛慧拓智能机器有限公司 路径规划方法、装置、芯片、终端、电子设备及存储介质
CN115489548A (zh) * 2022-09-20 2022-12-20 重庆大学 智能汽车园区道路路径规划方法
CN117075619A (zh) * 2023-10-17 2023-11-17 之江实验室 一种局部路径规划方法、装置和介质
CN117075619B (zh) * 2023-10-17 2024-01-16 之江实验室 一种局部路径规划方法、装置和介质
CN117475090A (zh) * 2023-12-27 2024-01-30 粤港澳大湾区数字经济研究院(福田) 一种轨迹生成模型、轨迹生成方法、装置、终端及介质

Also Published As

Publication number Publication date
CN111750887B (zh) 2023-11-21

Similar Documents

Publication Publication Date Title
CN111750887B (zh) 降低事故严重程度的无人驾驶车辆轨迹规划方法及系统
CN110488802B (zh) 一种网联环境下的自动驾驶车辆动态行为决策方法
CN108313054B (zh) 自动驾驶自主换道决策方法和装置及自动驾驶车辆
CN109345020B (zh) 一种完全信息下的无信号交叉口车辆驾驶行为预测方法
WO2021077725A1 (zh) 一种基于驾驶意图的周围车辆运动状态预测系统及方法
CN108225364B (zh) 一种无人驾驶汽车驾驶任务决策系统及方法
Bai et al. Hybrid reinforcement learning-based eco-driving strategy for connected and automated vehicles at signalized intersections
Gao et al. Situational assessment for intelligent vehicles based on stochastic model and Gaussian distributions in typical traffic scenarios
CN103754221B (zh) 一种车辆自适应巡航控制系统
CN112002144B (zh) 无信号交叉路口行车风险态势评估方法及系统
WO2019213981A1 (zh) 基于等效力的行车风险实时评估方法及其装置
CN110992695B (zh) 基于冲突消解的车辆城市交叉口通行决策多目标优化方法
CN112249008B (zh) 针对复杂动态环境的无人驾驶汽车预警方法
Wu et al. Supplementary benefits from partial vehicle automation in an ecoapproach and departure application at signalized intersections
WO2022053026A1 (zh) 一种自动驾驶会车场景处理方法及其装置、车辆、存储介质
Zhang et al. A framework for turning behavior classification at intersections using 3D LIDAR
Guo et al. Toward human-like behavior generation in urban environment based on Markov decision process with hybrid potential maps
CN111994075A (zh) 一种基于人工智能的驾驶辅助方法
Aoki et al. Multicruise: eco-lane selection strategy with eco-cruise control for connected and automated vehicles
CN111103882A (zh) 一种无人驾驶电动汽车的自主跟车控制方法
CN114228707A (zh) 一种无人驾驶车辆的防撞方法及系统
CN114442630A (zh) 一种基于强化学习和模型预测的智能车规划控制方法
EP4043309A1 (en) Vehicle control method, device, controller and intelligent vehicle
CN113119945A (zh) 一种基于环境模型的汽车高级辅助驾驶系统
Tran et al. A model predictive control-based lane merging strategy for autonomous vehicles

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant