CN109885891B - 一种智能车gpu并行加速轨迹规划方法 - Google Patents

Abstract

本发明公开了一种智能车GPU并行加速轨迹规划方法，在CPU端，获取实时感知障碍物栅格图、全局参考路径、智能车实时GPS信息和上层决策指令，沿参考路径采样，从而获取一系列目标采样终端；在GPU端，从CPU端获取采样终端序列、智能车实时状态信息、感知环境栅格以及历史帧规划轨迹参数，设计轨迹生成内核函数，为每一个目标采样终端生成一条连接初始状态和终端状态的智能车可稳定跟踪轨迹，设计轨迹评估内核函数，评估每条轨迹的代价。回传轨迹数据到CPU端，选取最优轨迹并匹配速度值，得到轨迹规划结果传输给控制执行机构。本发明用于提高智能车轨迹实时规划效率，达到实时高效生成大量轨迹，进一步改善轨迹规划结果的目的。

Description

一种智能车GPU并行加速轨迹规划方法

技术领域

本发明涉及智能交通系统技术领域，尤其涉及一种智能车GPU并行加速轨迹规划方法，特别涉及无人驾驶系统中的实时轨迹规划方法，具体为在cuda架构中实现轨迹生成和代价评估的并行设计，用于提高轨迹规划效率，在满足智能车实时性的前提下规划出尽可能多的轨迹以改善最终的规划结果。

背景技术

近年来，大量的工作致力于解决无人驾驶车辆的运动规划问题。这些方法大致可以分为两类：基于图搜索的方法和基于轨迹生成的方法。基于图搜索的方法关注于使用确定性图搜索计算无碰撞路径，如混合A*，状态栅格法，随机采样算法(如快速搜索随机树(RRT))。这些图搜索方法大多数能够在混乱的环境中计算长期无碰撞路径，并防止车辆陷入局部最小值,但仅适用于在复杂的未知环境中的低速情况。图搜索法通常在计算上过于复杂，无法实时运行或在城市环境中对典型的动态交通状况做出反应，且采样盲目性大、生成路径迂回曲折，易造成控制跟踪震荡。

基于轨迹生成的方法大多数遵循离散优化方案，即轨迹生成、碰撞检测和代价评估。部分研究人员采用线性和非线性模型预测控制(MPC)方法来解决车辆轨迹生成和跟踪问题。由于其在长期预测层面上解决连续控制空间中的约束优化问题，通常涉及复杂的优化过程，包括矩阵运算和数值迭代运算，容易导致过多的计算负担，实时环境中不规则的障碍物分布所施加的限制也难以处理，难以达到实时轨迹规划需求。

发明内容

本发明目的就是为了弥补已有技术的缺陷，提供一种智能车GPU并行加速轨迹规划方法。

本发明是通过以下技术方案实现的：

一种智能车GPU并行加速轨迹规划方法，包括如下步骤：

步骤1：离线构建轨迹初始猜测参数查找表，为每条轨迹建立索引，存储轨迹索引以及轨迹生成参数；

步骤2：在线并行轨迹规划，在CPU端，获取系统信息，沿参考路径分层采样终端状态，通过查找表获取轨迹初始猜测参数，将系统信息和轨迹初始猜测参数拷贝至GPU端；

步骤3：在GPU端读入数据，设计轨迹生成核函数，并行加速模型预测方法生成大量轨迹，存储轨迹参数到显存；

步骤4：在GPU端设计轨迹评估核函数，并行评估轨迹的障碍物代价、一致性代价、横向偏移代价和长度代价；

步骤5:CPU端从GPU显存中拷贝结果，释放GPU显存空间,CPU端归一化轨迹代价并筛选代价最小轨迹作为最佳轨迹，并为最优轨迹匹配速度值。

步骤1所述的离线构建轨迹初始猜测参数查找表，为每条轨迹建立索引，存储轨迹索引以及轨迹生成参数，具体方法如下：基于以自车车身为中心的坐标框架，坐标框架中每个点被描述为四维状态[x,y,θ,κ₀]，其中x、y分别为x轴和y轴坐标值，θ为智能车航向，κ为曲率，车身状态坐标作为初始状态X₀＝[0,0,0,κ₀]，κ₀为初始曲率，在状态空间中均匀采样终端状态X_f＝[x_f,y_f,θ_f,κ_f]，其中，x_f、y_f分别为终端的坐标值，θ_f是相对于初始航向的航向变化量，κ_f是智能车辆终端状态的曲率值，对于空间I＝[κ₀,x_f,y_f,θ_f,κ_f]进行均匀稀疏采样，使用模型预测方法连接初始状态和终端状态生成轨迹，为每一条轨迹建立索引，存储每条轨迹的索引值和轨迹生成参数到查找表。轨迹索引值index表示如下：

index＝x_f*10¹²+y_f*10⁹+θ_f*10⁶+κ_f*10³+κ₀

步骤2所述的在线并行轨迹规划，在CPU端，获取系统信息，沿参考路径分层采样终端状态，通过查找表获取轨迹初始猜测参数，将系统信息和轨迹初始猜测参数拷贝至GPU端，具体如下：

在CPU端，加载查找表到内存，采用串行方式读入CPU端的全局参考路径、实时感知环境栅格、GPS信息和上层决策指令，上层决策指令作为车道级规划结果，根据上层决策指令对全局参考路径进行处理，全局参考路径L是四维状态序列，具有以下形式：

L＝{x_i,y_i,κ_i,κ_i|i＝1,2,…,total}，i为参考路径的total个状态点里第i个状态点,x_i、y_i、θ_i、κ_i分别为第i个状态点的坐标值、航向和曲率；

沿参考路径在不同预瞄距离下纵向采样，并沿纵向采样点进行横向采样，来避开意外障碍物，采样点的航向和曲率状态设置为参考路径上对应点的状态，确保生成的轨迹和参考路径对齐；

对所有的初始状态和终端状态点对：[x₀,y₀,θ₀,κ₀]和[x_f,y_f,θ_f,κ_f]进行坐标变换，使起点成为[0,0,0,κ₀]的形式，并将终点改为[x_f,y_f,θ_f′,κ_f],其中θ′_f＝rad(θ_f-θ₀)，使用五维数据[κ₀,x_f,y_f,θ_f′,κ_f]作为查找索引，利用最近邻查找方法kd树在查找表中进行模糊查找，以匹配距离查找索引最近的轨迹，获取该轨迹的生成参数作为初始猜测；

从CPU端拷贝采样终端状态集、轨迹初始猜测集、车辆初始状态、全局参考路径、上一帧的执行轨迹、实时感知栅格数据到GPU显存中，若当前帧为第一帧，则上一帧的执行轨迹为空。

步骤3所述的在GPU端读入数据，设计轨迹生成核函数，并行加速模型预测方法生成大量轨迹，存储轨迹参数到显存，具体如下：

在GPU端分配线程块，为每条轨迹分配一个线程，每个线程从显存中读取一个终端状态和对应的初始猜测，以及车辆的初始状态作为核心处理函数的输入，核心处理函数采用模型预测方法迭代优化初始猜测，得到该轨迹的参数；

所述的加速模型预测方法生成大量轨迹的具体方法为：利用车辆运动学模型作为预测运动模型，为确保每条轨迹曲率和曲率速度的连续性，曲率模型表示为弧长s的三次多项式函数，并约束曲率和曲率速度的上界：

κ(s)＝κ₀+κ₁s+κ₂s²+κ₃s³

其中，κ_max表示车辆最大转向曲率值；|κ(s)|表示曲率函数的绝对值；

表示曲率函数一阶导数即曲率速度的绝对值；v表示车辆速度；

表示车辆转向曲率速度的最大值；

轨迹上任意弧长s处的车身航向计算公式为：

其中κ(s)为轨迹上任意弧长s处的曲率，

轨迹上任意弧长s处的车身横坐标x和纵坐标y的计算公式分别为：

则轨迹的生成参数表示为P＝[κ₁,κ₂,κ₃,s_f]，s_f为路径长度，使用牛顿迭代法对方程进行数值求解，使得误差值ΔX P(k))小于预定阈值，迭代过程如下：

ΔX(P(k))←X_F-X_F(P(k)) (1)

P(k+1)←P(k)+ΔP (3)

其中，k表示当前是第k次迭代过程；X_F表示轨迹实际终端状态；X_F(P(k))表示使用轨迹参数代入轨迹公式计算得到的计算终端状态；ΔX(P(k))表示计算终端状态值和实际终端状态值之间的误差，式(2)和式(3)利用牛顿迭代法使ΔX(P(k))值趋近于0；ΔP表示参数变化量；参数变化量ΔP用于更新当前参数P(k)得到新的参数P(k+1)；

其中，雅各比矩阵

是4*4阶矩阵，ΔX(P(k))和ΔP是4*1阶矩阵，GPU为每条线程分配的寄存器大小是有限的，没有为矩阵的加法和乘法运算额外分配线程，运算过程中的中间矩阵空间由全局存储器分配。

涉及4*4阶矩阵求逆运算，在轨迹生成核函数内调用动态并行，分配4*4数量的子线程利用高斯消元法求解；

雅各比矩阵

表示为：

其中，元素

没有封闭解，使用复合辛普森规则进行数值积分：

其中,h＝(b-a)/n；a、b分别表示积分的上下限；n表示积分区间等分点数、h表示积分步长、m表示积分区间的第m个等分点。

步骤4所述的在GPU端设计轨迹评估核函数，并行评估轨迹的障碍物代价、横向偏移代价、长度代价和一致性代价，具体如下：

GPU端为每条轨迹分配一个线程计算轨迹代价，离散采样弧长s，根据轨迹公式计算N个轨迹状态点，提取状态点计算代价项：C_o,C_d,C_s,C_c；C_o,C_d,C_s,C_c分别对应上述的四个代价；

代价C_o表示轨迹与障碍物的接近程度，在实时感知栅格中，若栅格被障碍物占据则该栅格障碍物代价为1，否则代价为0，遍历候选轨迹的N个点，以每个轨迹状态点为中心在栅格图上构造大于车身的障碍物搜索矩形框，搜索框内车身距离最近障碍物的欧式距离作为该点障碍物代价，代价C_o通过并行规约求和N个轨迹点的障碍物代价来计算：

其中，τ表示待评估轨迹候选；j表示候选轨迹N个轨迹点中的第j个点；

为了避免过度横向运动，代价C_d惩罚路径相对于参考路径的横向偏差，

其中，D(τ(j))是轨迹候选和参考路径的横向偏差；

代价C_s表现出对更长轨迹的偏好，

C_s＝L(τ)

其中，L(τ)是路径候选的轨迹长度；

考虑重规划过程中规划结果和历史结果的一致性，代价C_c惩罚当前轨迹和历史轨迹之间的不一致，通过对路径和历史路径之间的距离积分计算，

其中，τ_pre(j)记录历史帧路径点数据，dis(τ(j)，τ_pre(j))是候选和历史帧路径的距离，如果当前帧是规划的第一帧，则代价值C_c设置为0。

步骤5所述的CPU端从GPU显存中拷贝结果，释放GPU显存空间，CPU端归一化轨迹代价并筛选代价最小轨迹作为最佳轨迹，并为最优轨迹匹配速度值，具体如下：

将GPU显存中轨迹参数以及轨迹代价值拷贝回CPU内存，CPU端归一化代价值，使不同层面考虑的代价值处于同一量级，过程如下：

C_o′、C_d′、C_s′、C_c′是针对C_o,C_d,C_s,C_c的归一化处理，使四个不同意义的评估值处于同一量级(最终值都位于[0,1])。其中，C_omax是所有候选轨迹中的障碍物最大代价值，D_max是最大横向偏差值，L_max和L_min分别对应最大长度和最小长度，dis_max是和历史帧轨迹之间的最大偏移距离；

加权求和评估每个候选轨迹的性能，

Cost＝w_oC_o′+w_dC_d′+w_sC_s′+w_cC_c′

其中w_o,w_d,w_s,w_c是权重项，根据场景进行实际调整；选择总成本Cost值最小的轨迹作为最佳轨迹；

为最佳轨迹匹配速度曲线，使用多项式函数获得加速度连续的速度分布，速度表示为时间的三次多项式函数:

v(t)＝v₀+v₁t+v₂t²+v₃t³

其中，v₁,v₂,v₃是速度方程的未知参数，t表示时间；

由车辆gps信息得到初始速度v₀和初始加速度a₀，上层决策指令根据交通规则和场景推理给定终端速度v_f和终端加速度a_f，已知路径长度s_f，通过以下方程组求解三次多项式中的未知参数{v₁,v₂,v₃,t_f}，从而得到匹配连续速度的最佳轨迹以传递给智能车控制执行，求解方程组如下：

本发明的优点是：本发明根据GPU(图形处理器)具备多线程并行高性能浮点计算能力以及CUDA编程模型，设计轨迹规划计算流程及存储分配方法，可有效利用GPU大规模并行计算能力提高无人驾驶车辆轨迹规划处理效率，达到实时轨迹规划并筛选最优规划结果，确保规划轨迹的可行性、安全性和舒适性。

附图说明

图1为轨迹规划在线GPU并行过程示意图。

图2为离线查找表采样构建过程示意图。

具体实施方式

如图1所示，一种智能车GPU并行加速轨迹规划方法，包括如下步骤：

步骤1：离线构建轨迹初始猜测参数查找表，为每条轨迹建立索引，存储轨迹索引以及轨迹生成参数；

步骤2：在线并行轨迹规划，在CPU端，获取系统信息，沿参考路径分层采样终端状态，通过查找表获取轨迹初始猜测参数，将系统信息和轨迹初始猜测参数拷贝至GPU端；

步骤3：在GPU端读入数据，设计轨迹生成核函数，并行加速模型预测方法生成大量轨迹，存储轨迹参数到显存；

步骤4：在GPU端设计轨迹评估核函数，并行评估轨迹的障碍物代价、一致性代价、横向偏移代价和长度代价；

步骤5:CPU端从GPU显存中拷贝结果，释放GPU显存空间,CPU端归一化轨迹代价并筛选代价最小轨迹作为最佳轨迹，并为最优轨迹匹配速度值。

步骤1所述的离线构建轨迹初始猜测参数查找表，为每条轨迹建立索引，存储轨迹索引以及轨迹生成参数，具体方法如下：如图2所示，基于以自车车身为中心的坐标框架，坐标框架中每个点被描述为四维状态[x,y,θ,κ₀]，其中x、y分别为为x轴和y轴坐标值，θ为智能车航向，κ为曲率，车身状态坐标作为初始状态X₀＝[0,0,0,κ₀]，κ₀为初始曲率，在状态空间中均匀采样终端状态X_f＝[x_f,y_f,θ_f,κ_f]，其中，x_f、y_f分别为终端的坐标值，θ_f是相对于初始航向的航向变化量，以降低采样空间大小，κ_f是智能车辆终端状态的曲率值，对于空间I＝[κ₀,x_f,y_f,θ_f,κ_f]进行均匀稀疏采样，表1显示了采样方案，使用模型预测方法连接初始状态和终端状态生成轨迹，为每一条轨迹建立索引，存储每条轨迹的索引值和轨迹生成参数到查找表。轨迹索引值index表示如下：

index＝x_f*10¹²+y_f*10⁹+θ_f*10⁶+κ_f*10³+κ₀

表1查找表采样方案

步骤2所述的在线并行轨迹规划，如图1所示，在CPU端，获取系统信息，沿参考路径分层采样终端状态，通过查找表获取轨迹初始猜测参数，将系统信息和轨迹初始猜测参数拷贝至GPU端，具体如下：

在CPU端，加载查找表到内存，采用串行方式读入CPU端的全局参考路径、实时感知环境栅格、GPS信息和上层决策指令，上层决策指令作为车道级规划结果，根据上层决策指令对全局参考路径进行处理，如接收指令为向左换道时向左平移参考路径一个车道。全局参考路径L是四维状态序列，具有以下形式：L＝{x_i,x_i,θ_i,κ_i|i＝1,2,…,total}，i为参考路径的total个状态点里第i个状态点,x_i、y_i、θ_i、κ_i分别为第i个状态点的坐标值、航向和曲率；

沿参考路径在不同预瞄距离下纵向采样，并沿纵向采样点进行横向采样，来避开意外障碍物，采样点的航向和曲率状态设置为参考路径上对应点的状态，确保生成的轨迹和参考路径对齐；

对所有的初始状态和终端状态点对：[x₀,y₀,θ₀,κ₀]和[x_f,y_f,θ_f,κ_f]进行坐标变换，使起点成为[0,0,0,κ₀]的形式，并将终点改为[x_f,y_f,θ_f′,κ_f],其中θ′_f＝rad(θ_f-θ₀)，使用5维数据[κ₀,x_f,y_f,θ_f′,κ_f]作为查找索引，利用最近邻查找方法kd树在查找表中进行模糊查找，以匹配距离查找索引最近的轨迹，获取该轨迹的生成参数作为初始猜测；

从CPU端拷贝采样终端状态集、轨迹初始猜测集、车辆初始状态、全局参考路径、上一帧的执行轨迹、实时感知栅格数据到GPU显存中，若当前帧为第一帧，则上一帧的执行轨迹为空。

步骤3所述的在GPU端读入数据，设计轨迹生成核函数，并行加速模型预测方法生成大量轨迹，存储轨迹参数到显存，具体如下：

在GPU端分配线程块，为每条轨迹分配一个线程，每个线程从显存中读取一个终端状态和对应的初始猜测，以及车辆的初始状态作为核心处理函数的输入，核心处理函数采用模型预测方法迭代优化初始猜测，得到该轨迹的参数；

所述的加速模型预测方法生成大量轨迹的具体方法为：利用车辆运动学模型作为预测运动模型，为确保每条轨迹曲率和曲率速度的连续性，曲率模型表示为弧长s的三次多项式函数，并约束曲率和曲率速度的上界：

κ(s)＝κ₀+κ₁s+κ₂s²+κ₃s³

其中，κ_max表示车辆最大转向曲率值；|κ(s)|表示曲率函数的绝对值；

表示曲率函数一阶导数即曲率速度的绝对值；v表示车辆速度；

表示车辆转向曲率速度的最大值；

轨迹上任意弧长s处的车身航向计算公式为：

其中κ(s)为轨迹上任意弧长s处的曲率，

轨迹上任意弧长s处的车身横坐标x和纵坐标y的计算公式分别为：

则轨迹的生成参数表示为P＝[κ₁,κ₂,κ₃,s_f]，s_f为路径长度，使用牛顿迭代法对方程进行数值求解，使得误差值ΔX(P(k)）小于预定阈值，迭代过程如下：

ΔX(P(k))←X_F-X_F(P(k)) (1)

P(k+1)←P(k)+ΔP (3)

其中，k表示当前是第k次迭代过程；X_F表示轨迹实际终端状态；X_F(P(k))表示使用轨迹参数代入轨迹公式计算得到的计算终端状态；ΔX(P(k))表示计算终端状态值和实际终端状态值之间的误差，式(2)和式(3)利用牛顿迭代法使ΔX(P(k))值趋近于0；ΔP表示参数变化量；参数变化量ΔP用于更新当前参数P(k)得到新的参数P(k+1)；

其中，雅各比矩阵

是4*4阶矩阵，ΔX(P(k))和ΔP是4*1阶矩阵。GPU为每条线程分配的寄存器大小是有限的，迭代中要处理的矩阵规模较小，且针对矩阵的访问次数不多，因此，没有为矩阵的加法和乘法运算额外分配线程，运算过程中的中间矩阵空间由全局存储器分配。

涉及4*4阶矩阵求逆运算，求逆过程计算复杂，矩阵规模较小需要处理的数据量不多，因此直接在轨迹生成核函数内调用动态并行，分配4*4数量的子线程利用高斯消元法求解。

全局存储器高带宽、高延迟的特性可能会带来数据读写延迟，当生成轨迹量大、分配线程总数多时，部分线程在等待显存时，GPU可利用延迟隐藏切换其他线程占用处理器，从而达到利用其他线程束的计算隐藏访存延迟的效果。

雅各比矩阵

表示为：

其中，元素

等是广义菲涅尔(Fresnel)积分的特例，并且没有封闭解。使用复合辛普森规则进行数值积分：

其中,h＝(b-a)/n；a、b分别表示积分的上下限；h表示积分区间等分点数、h表示积分步长、m表示积分区间的第m个等分点。CPU端的辛普森积分运算使用递归运算自适应调节积分步长，考虑到GPU不适用递归运算，采用固定步长，使用n＝10，实践中发现可满足精确度要求。

步骤4所述的在GPU端设计轨迹评估核函数，并行评估轨迹的障碍物代价、横向偏移代价、长度代价和一致性代价，具体如下：

GPU端为每条轨迹分配一个线程计算轨迹代价，离散采样弧长s，根据轨迹公式计算N个轨迹状态点，提取状态点计算代价项：C_o,C_d,C_s,C_c；C_o,C_d,C_s,C_c分别对应上述的四个代价；

代价C_o表示轨迹与障碍物的接近程度，在实时感知栅格中，若栅格被障碍物占据则该栅格障碍物代价为1，否则代价为0，遍历候选轨迹的N个点，以每个轨迹状态点为中心在栅格图上构造略大于车身的障碍物搜索矩形框，搜索框内车身距离最近障碍物的欧式距离作为该点障碍物代价，代价C_o通过并行规约求和N个轨迹点的障碍物代价来计算：

其中，τ表示待评估轨迹候选；j表示候选轨迹N个轨迹点中的第j个点；

为了避免过度横向运动，代价C_d惩罚路径相对于参考路径的横向偏差，

其中，D(τ(j))是轨迹候选和参考路径的横向偏差；

代价C_s表现出对更长轨迹的偏好，可以有效地防止短预测层面引起的侵略性动作。

C_s＝L(τ)

其中，L(τ)是路径候选的轨迹长度；

考虑重规划过程中规划结果和历史结果的一致性，代价C_c惩罚当前轨迹和历史轨迹之间的不一致，通过对路径和历史路径之间的距离积分计算，

其中，τ_pre(j)记录历史帧路径点数据，dis(τ(j)，τ_pre(j))是候选和历史帧路径的距离，如果当前帧是规划的第一帧，则代价值C_c设置为0。

步骤5所述的CPU端从GPU显存中拷贝结果，释放GPU显存空间，CPU端归一化轨迹代价并筛选代价最小轨迹作为最佳轨迹，并为最优轨迹匹配速度值，具体如下：

将GPU显存中轨迹参数以及轨迹代价值拷贝回CPU内存，CPU端归一化代价值，使不同层面考虑的代价值处于同一量级，过程如下：

C_o′、C_d′、C_s′、C_c′是针对C_o,C_d,C_s,C_c的归一化处理，使四个不同意义的评估值处于同一量级(最终值都位于[0,1])。其中，C_omax是所有候选轨迹中的障碍物最大代价值，D_max是最大横向偏差值，L_max和L_min分别对应最大长度和最小长度，dis_max是和历史帧轨迹之间的最大偏移距离；

加权求和评估每个候选轨迹的性能，

Cost＝w_OC_o′+w_dC_d′+w_sC_s′+w_cC_c′

其中w_o,w_d,w_s,w_c是权重项，根据环境进行实际调整；选择总成本Cost值最小的轨迹作为最佳轨迹；

为最佳轨迹匹配速度曲线，使用多项式函数获得加速度连续的速度分布，速度表示为时间的三次多项式函数:

v(t)＝v₀+v₁t+v₂t²+v₃t³

其中，v₁,v₂,v₃是速度方程的未知参数，t表示时间；

由车辆gps信息得到初始速度v₀和初始加速度a₀，上层决策指令根据交通规则和场景推理给定终端速度v_f和终端加速度a_f，已知路径长度s_f，通过以下方程组求解三次多项式中的未知参数{v₁,v₂,v₃,t_f}，从而得到匹配连续速度的最佳轨迹以传递给智能车控制执行，速度规划通过强制执行v₂＝v₃＝0，保留在危险场景下生成线性速度分布以紧急制动。求解方程组如下：

Claims (6)

1.一种智能车GPU并行加速轨迹规划方法，其特征在于：包括如下步骤：

步骤1：离线构建轨迹初始猜测参数查找表，为每条轨迹建立索引，存储轨迹索引以及轨迹生成参数；

步骤2：在线并行轨迹规划，在CPU端，获取系统信息，沿参考路径分层采样终端状态，通过查找表获取轨迹初始猜测参数，将系统信息和轨迹初始猜测参数拷贝至GPU端；

步骤3：在GPU端读入数据，设计轨迹生成核函数，并行加速模型预测方法生成大量轨迹，存储轨迹参数到显存；

步骤4：在GPU端设计轨迹评估核函数，并行评估轨迹的障碍物代价、一致性代价、横向偏移代价和长度代价；

步骤5:CPU端从GPU显存中拷贝结果，释放GPU显存空间,CPU端归一化轨迹代价并筛选代价最小轨迹作为最优轨迹，并为最优轨迹匹配速度值。

2.根据权利要求1所述的一种智能车GPU并行加速轨迹规划方法，其特征在于：步骤1所述的离线构建轨迹初始猜测参数查找表，为每条轨迹建立索引，存储轨迹索引以及轨迹生成参数，具体方法如下：基于以自车车身为中心的坐标框架，坐标框架中每个点被描述为四维状态[x,y,θ,κ₀]，其中x、y分别为x轴和y轴坐标值，θ为智能车航向，κ为曲率，车身状态坐标作为初始状态X₀＝[0,0,0,κ₀]，κ₀为初始曲率，在状态空间中均匀采样终端状态X_f＝[x_f,y_f,θ_f,κ_f]，其中，x_f、y_f分别为终端的坐标值，θ_f是相对于初始航向的航向变化量，κ_f是智能车辆终端状态的曲率值，对于空间I＝[κ₀,x_f,y_f,θ_f,κ_f]进行均匀稀疏采样，使用模型预测方法连接初始状态和终端状态生成轨迹，为每一条轨迹建立索引，存储每条轨迹的索引值和轨迹生成参数到查找表，轨迹索引值index表示如下：

index＝x_f*10¹²+y_f*10⁹+θ_f*10⁶+κ_f*10³+κ₀。

3.根据权利要求2所述的一种智能车GPU并行加速轨迹规划方法，其特征在于：步骤2所述的在线并行轨迹规划，在CPU端，获取系统信息，沿参考路径分层采样终端状态，通过查找表获取轨迹初始猜测参数，将系统信息和轨迹初始猜测参数拷贝至GPU端，具体如下：

在CPU端，加载查找表到内存，采用串行方式读入CPU端的全局参考路径、实时感知环境栅格、GPS信息和上层决策指令，上层决策指令作为车道级规划结果，根据上层决策指令对全局参考路径进行处理，全局参考路径L是四维状态序列，具有以下形式：L＝{x_i,y_i,θ_i,κ_i|i＝1,2,…,total}，i为参考路径的total个状态点里第i个状态点,x_i、y_i、θ_i、κ_i分别为第i个状态点的坐标值、航向和曲率；

沿参考路径在不同预瞄距离下纵向采样，并沿纵向采样点进行横向采样，采样点的航向和曲率状态设置为参考路径上对应点的状态，确保生成的轨迹和参考路径对齐；

对所有的初始状态和终端状态点对：[x₀,y₀,θ₀,κ₀]和[x_f,y_f,θ_f,κ_f]进行坐标变换，使起点成为[0,0,0,κ₀]的形式，并将终点改为[x_f,y_f,θ_f′,κ_f],其中θ′_f＝rad(θ_f-θ₀)，使用五维数据[κ₀,x_f,y_f,θ_f′,κ_f]作为查找索引，利用最近邻查找方法kd树在查找表中进行模糊查找，以匹配距离查找索引最近的轨迹，获取该轨迹的生成参数作为初始猜测；

从CPU端拷贝采样终端状态集、轨迹初始猜测集、车辆初始状态、全局参考路径、上一帧的执行轨迹、实时感知栅格数据到GPU显存中，若当前帧为第一帧，则上一帧的执行轨迹为空。

4.根据权利要求3所述的一种智能车GPU并行加速轨迹规划方法，其特征在于：步骤3所述的在GPU端读入数据，设计轨迹生成核函数，并行加速模型预测方法生成大量轨迹，存储轨迹参数到显存，具体如下：

在GPU端分配线程块，为每条轨迹分配一个线程，每个线程从显存中读取一个终端状态和对应的初始猜测，以及车辆的初始状态作为核心处理函数的输入，核心处理函数采用模型预测方法迭代优化初始猜测，得到该轨迹的参数；

所述的加速模型预测方法生成大量轨迹的具体方法为：利用车辆运动学模型作为预测运动模型，为确保每条轨迹曲率和曲率速度的连续性，曲率模型表示为弧长s的三次多项式函数，并约束曲率和曲率速度的上界：

κ(s)＝κ₀+κ₁s+κ₂s²+κ₃s³

|κ(s)|≤κ_max，

其中，κ_max表示车辆最大转向曲率值；|κ(s)|表示曲率函数的绝对值；

表示曲率函数一阶导数即曲率速度的绝对值；v表示车辆速度；

表示车辆转向曲率速度的最大值；

轨迹上任意弧长s处的车身航向计算公式为：

其中κ(s)为轨迹上任意弧长s处的曲率，

轨迹上任意弧长s处的车身横坐标x和纵坐标y的计算公式分别为：

则轨迹的生成参数表示为P＝[κ₁,κ₂,κ₃,s_f]，s_f为路径长度，使用牛顿迭代法对方程进行数值求解，使得误差值ΔX(P(k))小于预定阈值，迭代过程如下：

ΔX(P(k))←X_F-X_F(P(k))

P(k+1)←P(k)+ΔP

其中，k表示当前是第k次迭代过程；X_F表示轨迹实际终端状态；X_F(P(k))表示使用轨迹参数代入轨迹公式计算得到的计算终端状态；ΔX(P(k))表示计算终端状态值和实际终端状态值之间的误差；ΔP表示参数变化量；参数变化量ΔP用于更新当前参数P(k)得到新的参数P(k+1)；

其中，雅各比矩阵

是4*4阶矩阵，ΔX(P(k))和ΔP是4*1阶矩阵，GPU为每条线程分配的寄存器大小是有限的，没有为矩阵的加法和乘法运算额外分配线程，运算过程中的中间矩阵空间由全局存储器分配，

涉及4*4阶矩阵求逆运算，直接在轨迹生成核函数内调用动态并行，分配4*4数量的子线程利用高斯消元法求解；

雅各比矩阵

表示为：

其中，元素

没有封闭解，使用复合辛普森规则进行数值积分：

其中,h＝(b-a)/n；a、b分别表示积分的上下限；n表示积分区间等分点数、h表示积分步长、m表示积分区间的第m个等分点。

5.根据权利要求4所述的一种智能车GPU并行加速轨迹规划方法，其特征在于：步骤4所述的在GPU端设计轨迹评估核函数，并行评估轨迹的障碍物代价、横向偏移代价、长度代价和一致性代价，具体如下：

GPU端为每条轨迹分配一个线程计算轨迹代价，离散采样弧长s，根据轨迹公式计算N个轨迹状态点，提取状态点计算代价项：C_o,C_d,C_s,C_c；

代价C_o表示轨迹与障碍物的接近程度，在实时感知栅格中，若栅格被障碍物占据则该栅格障碍物代价为1，否则代价为0，遍历候选轨迹的N个点，以每个轨迹状态点为中心在栅格图上构造大于车身的障碍物搜索矩形框，搜索框内车身距离最近障碍物的欧式距离作为该点障碍物代价，代价C_o通过并行规约求和N个轨迹点的障碍物代价来计算：

其中，τ表示待评估轨迹候选；j表示候选轨迹N个轨迹点中的第j个点；

为了避免过度横向运动，代价C_d惩罚路径相对于参考路径的横向偏差，

其中，D(τ(j))是轨迹候选和参考路径的横向偏差；

代价C_s表现出对更长轨迹的偏好，

C_s＝L(τ)

其中，L(τ)是路径候选的轨迹长度；

考虑重规划过程中规划结果和历史结果的一致性，代价C_c惩罚当前轨迹和历史轨迹之间的不一致，通过对路径和历史路径之间的距离积分计算，

其中，τ_pre(j)记录历史帧路径点数据，dis(τ(j)，τ_pre(j))是候选和历史帧路径的距离，如果当前帧是规划的第一帧，则代价值C_c设置为0。

6.根据权利要求5所述的一种智能车GPU并行加速轨迹规划方法，其特征在于：步骤5所述的CPU端从GPU显存中拷贝结果，释放GPU显存空间，CPU端归一化轨迹代价并筛选代价最小轨迹作为最佳轨迹，并为最优轨迹匹配速度值，具体如下：

将GPU显存中轨迹参数以及轨迹代价值拷贝回CPU内存，CPU端归一化代价值，使不同层面考虑的代价值处于同一量级，过程如下：

其中，C_omax是所有候选轨迹中的障碍物最大代价值，D_max是最大横向偏差值，L_max和L_min分别对应最大长度和最小长度，dis_max是和历史帧轨迹之间的最大偏移距离；

加权求和评估每个候选轨迹的性能，

Cost＝w_OC_o′+w_dC_d′+w_sC_s′+w_cC_c′

其中w_o,w_d,w_s,w_c是权重项，根据场景进行实际调整；选择总成本Cost值最小的轨迹作为最佳轨迹；

为最佳轨迹匹配速度曲线，使用多项式函数获得加速度连续的速度分布，速度表示为时间的三次多项式函数:

v(t)＝v₀+v₁t+v₂t²+v₃t³

其中，v₁,v₂,v₃是速度方程的未知参数，t表示时间；

由车辆gps信息得到初始速度v₀和初始加速度a₀，上层决策指令根据交通规则和场景推理给定终端速度v_f和终端加速度a_f，已知路径长度s_f，通过以下方程组求解三次多项式中的未知参数{v₁,v₂,v₃,t_f}，从而得到匹配连续速度的最佳轨迹以传递给智能车控制执行，求解方程组如下：

Images