一种基于高斯伪谱法的智能车变道超车轨迹规划方法

1.本发明属于智能驾驶领域，涉及一种基于高斯伪谱法的智能车变道超车轨迹规划方法。


背景技术：

2.与人类自主驾驶相比，智能车主动变道系统能够更加清晰的了解道路实时的交通情况从而对车辆的运行状态进行合适的调整，从而最大限度地避免道路交通事故的发生，其发展与推广在保障道路车辆行车安全，改善道路交通情况，构建智能化与网联化的智慧交通体系等方面有重要意义。
3.车辆轨迹规划方法的优越与否对对智能车安全、通畅、迅速的在实际道路中行进其关键作用，是体现智慧驾驶水平的关键环节，传统的基于周围交通状态在车辆换道期间生成的恒定参考轨迹规划方法在复杂的交通情况和道路环境中出现突发随机状况时往往会发生错误决策，造成交通事故的发生。


技术实现要素：

4.本发明的目的在于克服上述现有技术的缺点，提供一种基于高斯伪谱法的智能车变道超车轨迹规划方法，具有高效、稳定、计算精度高的优点，能够得到具有更高安全性的超车轨迹。
5.为达到上述目的，本发明采用以下技术方案予以实现：
6.一种基于高斯伪谱法的智能车变道超车轨迹规划方法，包括以下过程：
7.s1：将车路信息和自身车辆感知信息交换，并采用x-y函数建立局部路径规划的静态和动态障碍物的数学模型，通过交换后的信息建立车辆的可行驶区域模型和运动学模型；
8.s2：根据静态和动态障碍物的数学模型、可行驶区域模型和运动学模型，采用伪谱法对最优控制问题进行超车轨迹规划求解，通过对最优控制问题离散化，转化为非线性规划问题，求解该非线性规划问题得到规划后的超车轨迹。
9.优选的，s1中，车路信息包括汽车行驶路段的道路交通情况，自身车辆感知信息包括汽车行进中周围车辆车速、自身车速、拥堵状况及道路行人状况。
10.优选的，s1中，静态和动态障碍物的数学模型的建立过程为：根据结构化道路，确定静态障碍物和动态障碍物，在车辆坐标系下，通过x-y函数建立静止和动态障碍物数学模型，将车辆坐标系和世界坐标系转换，在新的坐标系下得到转换后的静止和动态障碍物数学模型。
11.优选的，s1中，可行驶区域模型的建立过程为：根据自身车辆感知信息和车路信息交流感知的不可行驶区域的位置信息，通过x-y函数完成道路可行驶区域的数学表达，车辆的可行驶区域为除去不可行驶的区域后所剩余的其他区域。
12.优选的，s2中，采用伪谱法对最优控制问题进行轨迹规划求解，将最优控制问题转
化为非线性规划问题，具体内容包括：
13.确定最优控制问题，确定初始时间区域并进行时域变换，定义新的时间变量，求出在新的时间变量下最优控制问题的性能函数和相关约束，进行最优控制问题离散化，得到非线性规划问题的数学表达。
14.优选的，得到规划后的超车轨迹后，通过伪谱法建立第一级安全校验模型得到轨迹，再通过自身车辆感知信息和车路信息，对轨迹进行二次校验，最终得到最优的变道超车轨迹。
15.进一步，采用x-y函数刻画车辆边界，对于车辆边界上任意一点确定约束表达式，对自车边界和障碍物边界进行离散化处理并进行坐标系转换，并进行两级校验的安全距离检测，确定是否符合安全轨迹要求。
16.一种基于高斯伪谱法的智能车变道超车轨迹规划系统，包括：
17.模型建立模块：用于将车路信息和自身车辆感知信息交换，并采用x-y函数建立局部路径规划的静态和动态障碍物的数学模型，通过交换后的信息建立车辆的可行驶区域模型和运动学模型；
18.超车轨迹规划模块：用于根据静态和动态障碍物的数学模型、可行驶区域模型和运动学模型，采用伪谱法对最优控制问题进行超车轨迹规划求解，通过对最优控制问题离散化，转化为非线性规划问题，求解该非线性规划问题得到规划后的超车轨迹。
19.一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述任意一项所述基于高斯伪谱法的智能车变道超车轨迹规划方法的步骤。
20.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述任意一项所述基于高斯伪谱法的智能车变道超车轨迹规划方法的步骤。
21.与现有技术相比，本发明具有以下有益效果：
22.本发明首先根据感知信息建立了障碍物场景的轨迹规划数学模型，并使用高斯伪谱法的算法原理，通过插值离散将最优控制问题转化为离散的非线性规划问题，具有高效、稳定、计算精度高的优点，能够得到具有更高安全性的超车轨迹。
23.进一步，通过两种安全距离模型分别进行建模计算和轨迹校验，提高了超车的安全性。
附图说明
24.图1为本发明的伪谱法车辆轨迹规划流程图；
25.图2为本发明的安全校验机制流程图；
26.图3为本发明的可行驶区域数学建模示意图；
27.图4为本发明的二自由度车辆模型示意图；
28.图5为本发明的伪谱法校验安全距离模型示意图；
29.图6为本发明的轨迹校验安全距离模型示意图。
具体实施方式
30.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例；基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
31.需要说明的是，下面描述中使用的词语“前”、“后”、“左”、“右”、“上”和“下”指的是附图中的方向，词语“内”和“外”分别指的是朝向或远离特定部件几何中心的方向。
32.除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组合。
33.本发明所述的基于高斯伪谱法的智能车变道超车轨迹规划方法，包括以下步骤：
34.步骤s1、将车路信息和自身车辆感知获得的信息交换，采用x-y函数建立局部路径规划静态障碍物和动态障碍物的数学模型，建立车辆二由度非线性运动学模型和可行驶区域模型，如图3和图4所示。
35.车路信息包含汽车行驶路段的道路交通情况，如：车辆拥堵情况、施工路段位置情况，道路允许驶入的车辆类型、车道数量以及变化状况等。自身车辆感知信息有汽车行进中周围车辆车速、自身车速、拥堵状况、道路行人状况等；
36.选取简单结构化道路为研究场景，确定静态障碍物和动态障碍物，在车辆坐标系下，通过x-y函数建立静止和动态障碍物数学模型，将车辆坐标系和世界坐标系转换，在新的坐标系下得到转换后的静止和动态障碍物数学模型。
37.根据自身感知和车路协同信息交流感知的不可行驶区域的位置信息，通过x-y函数完成道路可行驶区域的数学表达，车辆的可行驶区域为除去不可行驶的区域后所剩余的其他区域。
38.步骤s2、根据步骤s1的模型，采用伪谱法对最优控制问题进行轨迹规划求解，通过对最优控制问题离散化，将最优控制问题转化为非线性规划问题并建立非线性规划问题的数学表达式，求解得到规划轨迹，流程如图1所示。
39.确定最优控制问题，确定初始时间区域并进行时域变换，定义新的时间变量，求出在新的时间变量下最优控制问题的性能函数和相关约束，进行最优控制问题离散化，得到非线性规划问题的数学表达。
40.最优控制问题即轨迹规划的优化问题，寻找最优控制变量，状态变量，静态参数和时间满足条件极值的要求，根据之前采集到的信息和建立的障碍物和可行驶区域模型等生成轨迹，最优控制问题利用一些约束条件如边界约束，路径约束，和参数的数值范围等，使得轨迹规划达到安全性高、路程短、车速变化达可到超车要求、轨迹无其他障碍物等要求。
41.步骤s3、提出一种基于两种安全距离模型的安全校验机制，通过伪谱法建立第一级安全校验模型得到轨迹，再通过自车传感器和车路感知获得的信息，对轨迹进行二次校验，最终得到合适的变道超车轨迹，如图5和图6所示。
42.采用x-y函数刻画车辆边界，对于车辆边界上任意一点确定约束表达式，对自车边界和障碍物边界进行离散化处理并进行坐标系转换，并进行两级校验的安全距离检测，确
定是否符合安全轨迹要求。
43.伪谱法所的轨迹的第一级安全校验时，将本车近似为圆形时，以车辆俯视图的形为中心，车辆俯视图对角线的一半为半径，画一个圆近似为车辆边界，车辆安全距离满足本车的车辆中为与障碍车辆边界上任意一点的距离要大于本车等效圆的半径的要求，确定安全距离。
44.自车传感器和车路感知信息对轨迹进行的二次校验时，将本车俯视图形心依据计算所得轨迹点依次做矩形边界刻画，本车上任意边界点与障碍车辆上任意边界点的距离大于0，确定安全距离。
45.根据自车传感器和车路感知信息采用第二种安全距离模型进行校验，判定所有本车边界点均不与障碍车辆、道路边界和其他不可跨越的交通元素相碰时，则认为通过二次安全距离模型校验，轨迹可跟随。若出现碰撞，则不执行轨迹跟随。
46.步骤s1的采用x-y函数建立局部路径规划静态障碍物和动态障碍物的数学模型，其具体内容包括：
47.选取简单结构化道路为研究场景，确定静态和动态障碍物，对静态障碍物c进行建模，在车辆坐标系(xc,yc)下，所得静止车辆边界模型为
[0048][0049]
其中，(a,b)为车辆坐标系下车辆后轴中心坐标，也为原点坐标为车辆相对于坐标系的比例系数；
[0050]
将车辆坐标系和世界坐标系转换，转换矩阵为
[0051][0052]
其中，世界坐标系下，车辆后轴中心坐标为车辆边界坐标(0,0)
[0053]
联立公式(2-1)(2-2)得到x-y函数下静止车辆边界模型为
[0054][0055]
同理，动态障碍物r也采用以上方式表达，车辆在t时刻，世界坐标系下，车辆后轴中心坐标为(x
dt
,y
dt
)，在车辆坐标系(xr,yr)下，动态车辆边界模型为
[0056][0057]
其中，(c,d)为车辆坐标系下车辆后轴中心坐标，为车辆相对于坐标系的比例系数；
[0058]
进行坐标转换得到动态车辆障碍物边界模型
[0059][0060]
其中x
dt
＝f
γ
(t),y
dt
＝g
γ
(t),动态车辆的位置和航向角为随时间变
化的函数；
[0061]
步骤s1，建立可行驶区域模型，其具体内容包括：
[0062]
根据感知的不可行驶区域的位置信息，通过x-y或区间范围完成道路可行驶区域的数学表达，车辆的可行驶区域u为道路边沿内除去施工路段的区域，表示为
[0063][0064]
步骤s1，建立车辆而二由度非线性运动学模型，其具体内容包括：
[0065]
根据整车运动学模型，车辆后轴中心坐标(x,y)，车辆前轴中心坐标(xq,yq)，车辆轴距l，车速v，转向轮转角δ，车辆横摆角θ，车辆加速度a，车辆前后轮中心坐标与前轮转角和横摆角关系为：
[0066][0067]
前后轮坐标关系为
[0068][0069]
得到车辆运动学模型为：
[0070][0071]
式中，x，y，θ，v为状态变量，δ，a为控制变量。
[0072]
物理约束，如下：
[0073][0074]
边界约束，如下
[0075][0076]
步骤s2中，采用伪谱法对最优控制问题进行轨迹规划求解，具体内容包括：
[0077]
确定最优控制问题，包括最优控制变量u(t)，状态变量x(t)，静态参数p；确定初始时间区域t∈[t0,tf]，其中最优控制变量和状态变量均在该时间区域内，同时将时间区域t∈[t0,tf]转换为τ∈[-1,1]，定义时间变量τ为
[0078][0079]
确定最优控制问题的性能函数
[0080][0081]
约束条件为：
[0082][0083]
步骤s2中，通过对最优控制问题离散化，将最优控制问题转化为非线性规划问题，其具体内容包括：
[0084]
进行最优控制问题离散化，包括：动力学微分方程约束转化，终端状态约束离散和性能函数离散；
[0085]
微分方程离散：
[0086][0087]
终端状态约束离散：
[0088][0089]
其中，是积分权重，τn为lg点；
[0090]
性能函数离散：
[0091][0092]
经过上述离散，得到非线性规划问题的数学表达，其中状态变量为x(τn)，控制变量为u(τn)，n＝0,1,
…
,n；边界约束为b(x(τ0),t0,x(τf),tf)＝0，路径约束为g(x(τn),u(τn),τn；t0,tf)≤0，d
ni
(τn)为微分矩阵确定的多项式微分得到的状态变量代数表达式,具体如下：
[0093][0094]
约束条件：
[0095][0096]
b(x(τ0),t0,x(τf),tf)＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6-3)
[0097][0098]
g(x(τn),u(τn),τn；t0,tf)≤0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6-5)
[0099]
步骤s3中，通过伪谱法建立第一级安全校验模型得到轨迹，其具体内容包括：
[0100]
采用x-y函数刻画障碍车辆s边界，对于车辆边界上任意一点有：
[0101]
[0102]
其中，障碍车辆s中心坐标(x
s0
,y
s0
)，r
x
，ry为车辆的长度和宽度；
[0103]
对车辆边界进行离散化处理：
[0104][0105][0106][0107][0108]
其中j＝1,2,
…
,k；进行从车辆坐标系转换为世界坐标系；
[0109]
将本车b近似为圆形时，以车辆俯视图的形心(x
bo
,y
bo
)为中心，车辆俯视图对角线的一半为半径r
bo
，画一个圆近似为车辆边界，车辆安全距离要满足本车b的车辆中心点(x
bo
,y
bo
)与障碍车辆s边界上任意一点ei(x
si
,y
si
)∈h的距离要大于本车等效圆的半径r
bo
的要求，其差值要大于安全距离ds，从而确定距离ds：
[0110][0111]
安全距离应该满足：
[0112]
d＝ds-r
bo
》dsꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7-7)
[0113]
步骤s3中，通过自车传感器和车路感知获得的信息，对轨迹进行二次校验，其具体内容包括：
[0114]
轨迹校验安全距离模型，如图6左边为本车b，采用间隔对车辆长度和宽度边界进行离散，对本边界x-y函数表达并离散化：
[0115][0116][0117][0118][0119]
其中，j＝1,2,
…
,k，(x
′
b0
,y
′
b0
)为本车俯视图形心，r
xb
、r
yb
为车辆长度和宽度，对本车b上任意边界点均满足上式。
[0120]
同理，障碍车辆s采用x-y函数离散化，车辆s边界上任意一点ei(x
′
si
,y
′
si
)也满足离散化方程，将本车俯视图形心(x
′
b0
,y
′
b0
)依据计算所得轨迹点依次做矩形边界刻画，本车b上任意边界点(x
bi
,y
bi
)与障碍车辆s上任意边界点ei(x
′
si
,y
′
si
)的距离ds
′
大于0，ds
′
的表达为：
[0121][0122]
其中，n＝1,2,
…
,n，为伪谱法轨迹规划后得到的轨迹点数；
[0123]
根据自车传感器和车路感知信息采用第二种安全距离模型进行校验，将本车俯视图形心依据计算所得轨迹点依次做矩形边界刻画，判定所有本车边界点均不与障碍车辆、道路边界和其他不可跨越的交通元素相碰，则认为通过二次安全距离模型校验，轨迹可跟随。若出现碰撞，则不执行轨迹跟随。
[0124]
下述为本发明的装置实施例，可以用于执行本发明方法实施例。对于装置实施例中未纰漏的细节，请参照本发明方法实施例。
[0125]
本发明再一个实施例中，提供了一种基于高斯伪谱法的智能车变道超车轨迹规划系统，该基于高斯伪谱法的智能车变道超车轨迹规划系统可以用于实现上述基于高斯伪谱法的智能车变道超车轨迹规划方法，具体的，该基于高斯伪谱法的智能车变道超车轨迹规划系统包括模型建立模块以及超车轨迹规划模块。
[0126]
其中，模型建立模块用于将车路信息和自身车辆感知信息交换，并采用x-y函数建立局部路径规划的静态和动态障碍物的数学模型，通过交换后的信息建立车辆的可行驶区域模型和运动学模型。
[0127]
超车轨迹规划模块用于根据静态和动态障碍物的数学模型、可行驶区域模型和运动学模型，采用伪谱法对最优控制问题进行超车轨迹规划求解，通过对最优控制问题离散化，转化为非线性规划问题，求解该非线性规划问题得到规划后的超车轨迹。
[0128]
本发明再一个实施例中，提供了一种终端设备，该终端设备包括处理器以及存储器，所述存储器用于存储计算机程序，所述计算机程序包括程序指令，所述处理器用于执行所述计算机存储介质存储的程序指令。处理器可能是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor、dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等，其是终端的计算核心以及控制核心，其适于实现一条或一条以上指令，具体适于加载并执行一条或一条以上指令从而实现相应方法流程或相应功能；本发明实施例所述的处理器可以用于基于高斯伪谱法的智能车变道超车轨迹规划方法的操作，包括：s1：将车路信息和自身车辆感知信息交换，并采用x-y函数建立局部路径规划的静态和动态障碍物的数学模型，通过交换后的信息建立车辆的可行驶区域模型和运动学模型；s2：根据静态和动态障碍物的数学模型、可行驶区域模型和运动学模型，采用伪谱法对最优控制问题进行超车轨迹规划求解，通过对最优控制问题离散化，转化为非线性规划问题，求解该非线性规划问题得到规划后的超车轨迹。
[0129]
再一个实施例中，本发明还提供了一种计算机可读存储介质(memory)，所述计算机可读存储介质是终端设备中的记忆设备，用于存放程序和数据。可以理解的是，此处的计算机可读存储介质既可以包括终端设备中的内置存储介质，当然也可以包括终端设备所支持的扩展存储介质。计算机可读存储介质提供存储空间，该存储空间存储了终端的操作系统。并且，在该存储空间中还存放了适于被处理器加载并执行的一条或一条以上的指令，这些指令可以是一个或一个以上的计算机程序(包括程序代码)。需要说明的是，此处的计算
机可读存储介质可以是高速ram存储器，也可以是非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。
[0130]
可由处理器加载并执行计算机可读存储介质中存放的一条或一条以上指令，以实现上述实施例中有关基于高斯伪谱法的智能车变道超车轨迹规划方法的相应步骤；计算机可读存储介质中的一条或一条以上指令由处理器加载并执行如下步骤：s1：将车路信息和自身车辆感知信息交换，并采用x-y函数建立局部路径规划的静态和动态障碍物的数学模型，通过交换后的信息建立车辆的可行驶区域模型和运动学模型；s2：根据静态和动态障碍物的数学模型、可行驶区域模型和运动学模型，采用伪谱法对最优控制问题进行超车轨迹规划求解，通过对最优控制问题离散化，转化为非线性规划问题，求解该非线性规划问题得到规划后的超车轨迹。
[0131]
本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或计算机程序产品。因此，本技术可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0132]
本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0133]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0134]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0135]
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。
[0136]
应该理解，以上描述是为了进行图示说明而不是为了进行限制。通过阅读上述描述，在所提供的示例之外的许多实施例和许多应用对本领域技术人员来说都将是显而易见的。因此，本教导的范围不应该参照上述描述来确定，而是应该参照前述权利要求以及这些权利要求所拥有的等价物的全部范围来确定。出于全面之目的，所有文章和参考包括专利
申请和公告的公开都通过参考结合在本文中。在前述权利要求中省略这里公开的主题的任何方面并不是为了放弃该主题内容，也不应该认为申请人没有将该主题考虑为所公开的发明主题的一部分。