一种基于参数决策框架的轨迹规划控制方法与流程

本发明属于高级辅助驾驶及无人驾驶车辆的汽车安全技术领域，其涉及的是一种通过将决策行为参数化实现不同驾驶任务下统一的轨迹规划控制的基于参数决策框架的轨迹规划控制方法。

背景技术：

随着车辆技术的不断发展，更高自动化级别的高级辅助驾驶及无人驾驶车辆的运动控制系统成为研究热点并得到不断发展。此种情况下，系统的功能性及适用场景不断增多，基于单一驾驶任务及场景的底层运动控制器，如换道，车道保持，难以适用于其他驾驶场景。因此在系统功能不断丰富后，车辆运动控制系统子系统高度冗杂，进一步增加了系统协调控制的难度。因此，对于需要适应多种驾驶任务的高级别辅助驾驶及无人驾驶车辆的运动控制系统，其应同时具备多种任务的轨迹规划及控制能力并能实现不同任务间的平稳切换。

在控制方法层面，各种控制算法被应用于系统开发，如pid(比例-积分-微分)，lqr(线性二次型)，mpc(模型预测控制)等控制方法。在这些方法中，mpc方法可以显性地处理约束条件，并不断滚动优化进行优化计算，实现闭环的稳定性。由于车辆运动控制系统复杂并存在约束，在考虑车辆动力学模型的轨迹规划任务中，具有一定优势。

目前，对于驾驶任务的定义，一般是通过穷举的方法将人类的驾驶行为归纳描述为类人化的行为，如换道，车道保持，转弯。如果采用此种决策方式，在复杂场景下，机器替代的决策系统需要真正达到类人的驾驶决策能力，需要具备与人类同等智力水平的驾驶决策归纳能力，在目前的技术背景下，机器或决策系统的设计相比较于人类具有一定差距，无法很好具备驾驶决策归纳能力；同时，现有的决策描述方法过于单一，如换道时不考虑车辆的加减速行为，依据此种描述所设计的驾驶决策会过于保守。而实际人的驾驶决策应该是丰富多变的，不应该局限于有限个动作的集合。

技术实现要素：

本发明提出一种基于参数决策框架的轨迹规划控制方法，该控制方法考虑不同驾驶决策及多种驾驶场景下决策的复杂多样性，将类人的驾驶决策描述为与轨迹特征密切相关的若干决策参数。并将这些决策参数设计为轨迹规划层的控制器参数，统一了不同决策与不同场景下的轨迹规划层控制器形式，可有效降低子系统的复杂度与协调控制难度，提高系统性能，解决了现有高级辅助驾驶及无人驾驶存在的上述问题。

本发明技术方案结合附图说明如下：

一种基于参数决策框架的轨迹规划控制方法，该控制方法包括以下步骤：

步骤一、决策及规划环境认知；

通过智能感知模块中的车载智能感知原件及其自身配备的识别算法，得到可用于车辆控制系统决策及规划的信息；所述车载智能感知原件包含车载摄像头及雷达，且原件自身包含已标定的融合识别算法，得到的可用于车辆控制系统决策及规划的信息，所述信息包含当前驾驶环境中道路限速、相邻车道中心线间宽度、道路中心线的曲线形式及道路类型即直道或者弯道、周围存在车辆的位置及行驶车速信息；

步骤二、驾驶决策制定；

在驾驶决策模块中，提出使用基于参数决策框架，通过分析，选择不同驾驶决策及多种驾驶场景下决策需要的关键参数，组成驾驶决策簇；在不同的决策场景下，根据场景的特点，建立相应的优化决策问题，对当前场景下的关键决策参数值进行求解，具体方法如下：

21)驾驶决策簇建立：通过分析类人驾驶行为与交通环境的关系，及不同驾驶决策的特点，建立不同决策场景下的驾驶决策蔟，即驾驶决策的关键参数集；

22)不同的场景下，驾驶决策问题建立：在不同场景下，建立驾驶决策优化问题对当前场景的关键决策参数进行求解；

步骤三、优化轨迹规划求解；

通过轨迹规划模块中基于车辆动力学及运动学模型设计的非线性模型，使用非线性模型预测方法；根据决策层关键决策参数，调整优化的目标并得到不同终端约束条件；轨迹的求解过程不基于固定轨迹形式，通过对优化问题在线求解并滚动优化进行轨迹规划，具体方法如下：

31)纵侧向耦合的车辆运动控制非线性模型的建立：基于车辆动力学及运动学模型，考虑纵侧向运动控制，得到非线性模型用于控制车辆运动；

32)基于关键决策参数的轨迹规划控制器：使用非线性模型预测控制方法，基于建立的车辆运动控制模型及决策输出的关键决策参数，建立轨迹规划的优化问题，通过在线求解并滚动优化进行轨迹规划。

所述步骤二中21)的具体方法如下：

首先，由于轨迹规划层使用模型预测控制方法，即通过建立于轨迹规划相关的优化问题，通过在线求解达到轨迹规划的目的，因此根据方法特性，驾驶决策蔟包含目标函数项不等式约束项等式约束项及参数值项di∈{d1,d2,…,d9}，其中目标函数项与不等式约束项等式约束项为激活函数，其值为0或1，0表示该项在优化问题中不考虑，1表示该项在优化问题中考虑；由目标函数项与不等式约束项等式约束项组合成激活函数向量μ^o,μⁱ,μ^e，分别表示为具体的，包括以下步骤：

21.1)类人驾驶行为本身具有一定的属性，根据其持续特性，分为持续性行为即车道保持和暂时性行为即换道由d1的具体值决定；0代表持续性行为，1代表暂时性行为；相应的在模型预测控制中，持续性行为代表固定预测时域控制，暂时性行为代表预测时域递减控制；

21.2)动作行为的时间，由d2的具体值决定；对于不同驾驶场景下的不同驾驶行为，动作行为的持续时间会有所不同；在直道场景中，车道保持的动作时间为1s；而弯道情况下，由于车道中心线不沿车辆前进方向，为保持车辆处于车道中心，车道保持的动作时间小于直道时的动作时间，为0.5s；进行换道时，动作时间根据实际场景进行优化，换道时间为2s～5s之间；进行转弯行为时，根据路口的大小，动作时间为5s～10s；

21.3)目标函数项中预测时域内控制量相关的积分型性能指标项激活项分别为分别对应u(1),u(2),△u(1),△u(2)，其中u(1)为车轮转角，u(2)为车辆加/减速度，△u(1)为离散化后方程中相邻时刻车轮转角的变化量，△u(2)为离散化后方程中相邻时刻车辆加/减速度的变化量；

21.4)目标函数项中预测时域内状态变量车辆横摆角速度ωr，侧向速度vy的积分型性能指标项激活项分别为

21.5)目标函数项中预测时域终端时状态变量纵向车速vx，坐标系x方向位置坐标x，坐标系y方向位置坐标y的终端型性能指标项激活项，分别为对应的终端期望值分别为d3，d4，d5；

21.6)约束项中的不等式约束μⁱ包含地面附着约束，控制变量车轮转角u(1)，车辆加/减速度u(2)，道路限速vx,max.，激活项分别对应其中道路限速vx,max.值为d6；

21.7)约束项中的等式约束对应预测时域终端时状态变量车辆侧向速度vy，横摆角速度ωr，航向角x方向位置坐标x，y方向位置坐标y，对应的激活项分别为其中若侧向速度vy，横摆角速度ωr的终端值被考虑，其期望值仅存在为0情况；航向角x方向位置坐标x，y方向位置坐标y对应的期望值分别为d7，d8，d9。

所述步骤二中22)的具体方法如下：

在此系统中，驾驶场景中将包含不同的环境车辆，当环境车辆与本车的行驶路径存在路径冲突点时，通过路径冲突点的时序差将是评价多车环境下安全性的主要因素；因此，将此类决策问题建模为通过路径冲突点的时序规划问题；首先考虑是紧急避撞场景，在双向车道行驶时，若前车紧急停车，当对向车道无车辆行驶或者对向车辆距离较远时，本车除了采取紧急制动措施外，还在保证安全的前提下，临时借用对向车道进行紧急避撞，其中冲突点c为本车借用对向车道后返回原车道位置点，即通过时序规划环境车o3与本车o1通过冲突点c的时序；在此问题中，本车执行换道行为的时间t1＝2.5s，驾驶决策为ν，本车开始换道时刻的车速为vh,0，本车换向对向车道行为结束的速度为vh,1，在对向车道行驶的时间为t2，换回本车道时的速度为vh,2。控制量为u＝[ν,△vh,1,t2,△vh,2]，其中vh,1＝vh,0+△vh,1，vh,2＝vh,1+△vh,2，即△vh,1，△vh,2分别代表了本车在换道过程中及在对向车道行驶过程中速度变化量。决策的优化问题建立为找到最优控制量u^*，使得

s.t.tr>tr,min,vh,1≤vx,max.,vh,2≤vx,max.,u(3)≥0,△vmin≤u(2)≤△vmax,

其中，驾驶决策ν的值代表不同行为，0为紧急制动，1为紧急换道；tr是环境车o3与本车o1通过冲突点c的时间差；为保证安全，应保证tr>tr,min，tr,min为最小安全时间，为一设定常数值，可取2，vx,max.为道路最高限速，ah是前车紧急制动的制动减速度，这里假设前车制动过程中减速度不变；△vmin,△vmax为换道过程中为保证驾驶舒适性，速度变化最小最大限制值,ks1,1,ks1,2为权重系数；由以上优化问题得到的最优控制量，得到决策模块最终输出的决策参数d为形式为

其中，d中每一列对应决策参数值d1～d9，t1,t2,t3是行为的执行时间，这里t3＝t1为换道行为执行时间，d2＝△s1,2+s2+ssafe是本车返回原车道时在对向车道上的位移，△s1,2本车与紧急制动车辆在t1结束时刻的纵向相对距离，s2是紧急制动车辆在t2时段内的纵向位移，ssafe是同车道前后车的安全距离；d中每一行对应一个动作的决策参数值d1～d9。

其次是无信号灯交叉路口场景，此问题中存在三个潜在路径冲突点，同样地，将此问题建模为路径冲突点的时序问题，其中ooi∈{0,1|i＝1,2,3}表示此路径上存在或不存在环境车，ooi＝0表示不存在环境车，ooi＝1表示存在环境车，opi∈{0,1|i＝1,2,3}表示当前环境车辆在路径中位置是否已经通过了路径冲突点，opi＝1表示未通过，opi＝0表示已通过；由此，得到此场景下决策层的控制变量的维数为：

n＝op1*oo1+op2*oo2+op3*oo3.(3)

以n＝3时为例，此时控制变量为u＝[th,1,th,2,th,3]，其中th,1,th,2,th,3分别表示环境车辆o1，o2，o3到达路径冲突点的时间，以路径上匀速运动计算得到，决策的优化问题建立为找到最优控制量u^*使得

其中，si(i＝1,2,3)分别为相邻路径冲突点间的近似距离，以圆弧近似计算得到，因此总距离为ks2,1,ks2,2,ks2,3,ks2,4为相应优化项的权重系数，分别为本车通过路径冲突点的上下界约束时间，上下界时间的计算以环境车辆o1，o2，o3到达路径冲突点的时间th,1,th,2,th,3为基准，以路径冲突点为圆心，进入及离开rc＝2m为半径的圆的时间为上下界计算得到；为了保证安全性，本车通过路径冲突点的时刻应在此上下界以外；对于路径冲突点1，本车通过此路径点的时刻为或vh为本车进入路口时的车速，vr为此路口的期望车速；最后，由于转弯是一个连续的过程，如果在此过程中本车与环境车辆的交互关系相同，则对得到的优化结果进一步地进行合并，如果本车与环境车的交互关系均相同，则由决策模块输出的决策参数d为形式为

其中，l为路口宽度，由车道数及车道宽计算得到。

所述步骤三中31)的具体方法如下：

首先基于单轨车辆动力学模型，建立微分方程，其可表示为：

其中，m是车辆质量，vx是纵向车速，∑fx为沿x方向纵向力之和，vy为车辆侧向速度，wr是车辆横摆角速度，fyf,fyr分别车辆前轮侧向力和后轮侧向力，iz为车辆沿z轴的转动惯量，lf,lr为前后轴轴距，由于前后轮侧偏角较小，因此可将前后轮侧偏角αf,αr线性化为

其中，δf为前轮转角，进一步地，式(6)中的轮胎侧向力可表示为

其中，cr,cf分别为前后轮侧偏刚度；同时，根据车辆的运动关系有ax为纵向加速度，为车辆的航向角。以及考虑车辆的运动在全局坐标系下的运动方程，可建立为：

其中，x,y是车辆在全局坐标系下的位置。最终，非线性的车辆运动模型建立为

其中，状态变量为fyf,fyr可由式(8)计算得到，控制变量为u＝[a,δf]。其中为简化运动控制方程，纵向速度数值大小的改变由加速度a控制实现，并由一下层跟踪控制器保证跟踪。

所述步骤三中32)的具体方法如下：

在优化轨迹规划器中，目标函数考虑预测时域内控制量车轮转角u(1)，控制量车辆加/减速度u(2)，离散化后方程中相邻时刻车轮转角的变化量△u(1)，离散化后方程中相邻时刻车辆加/减速度的变化量△u(2)的积分型性能指标，预测时域内状态变量车辆横摆角速度ωr，侧向速度vy的积分型性能指标以及预测时域终端时状态变量纵向车速vx，x方向位置坐标x，y方向位置坐标y的终端型性能指标，这些性能指标在不同的决策下被不同程度的考虑；因此，每一项性能指标项由对应的激活项激活，激活向量可以表示为，同时每一个性能指标项由一个对应的权重系数值，权重系数向量可表示为k＝{ki|i＝1,2,...,n}。因此，控制器的目标函数可以表示为：

其中vx(tf),x(tf),y(tf)分别为纵向车速vx，x方向位置坐标x，y方向位置坐标y预测时域终端值，vt,xt,yt分别为期望的纵向车速vx，x方向位置坐标x，y方向位置坐标y终端函数指标值。

同时，由于控制器同时考虑了横向及纵向的车辆运动控制，因此在此过程中，车辆还需满足路面附着约束，即

其中，u为路面附着系数，fx,fy分别为车轮纵向力和横向力，此约束由激活；同时，控制量应满足可执行约束

其中，*min，*max分别代表相应变量的最小值和最大值，此约束由激活；综合以上描述，由于加入地面附着约束条件时，会很大程度影响求解速度，因此，可通过限制amin，amax使得待优化问题满足地面附着约束。相应的路段信息也会带来的约束，路段最大车速限速，表示为

vx≤vx,max.(14)

此约束由激活；

此外，轨迹规划控制器中还包含等式约束，其与驾驶环境相关；不论是车道保持，还是换道任务，在预测时域终端，应满足一定的终端条件，才能任务的完成，即

其中，分别为预测时域终端时刻横摆角速度，侧向速度，航向角，横向位移，yl,f为期望的终端横向位移；在换道任务中，终端横向位移为两车道中心线间的距离，在车道保持任务中，终端横向位移为0；在弯道环境中，由于道路不是直线，因此等式约束条件减少为式(15)中的后两项；其中第三项变为为期望的终端航向角，由实际弯道道路的曲率计算得到；特别地，在一些驾驶任务中，第四个终端条件变为对于x方向位置x的约束，转弯任务，即在驶离路口时完成转弯动作；因此，以上五种可能存在终端约束条件由激活项激活；

决策模块通过当前的环境信息得到决策参数后，输入规划层，建立相应的优化问题，通过使用通用的非线性模型预测方法进行求解，可通过滚动时域，在线优化的方式，进行轨迹规划与控制。

本发明的有益效果为：

1.本发明基于分层的车辆控制系统架构，提出基于参数决策框架，即使用关键参数替代类人化的驾驶决策，可以更好的适应不同驾驶决策及多种驾驶场景下的复杂决策；

2.在提出的基于的参数决策框架下，通过分析类人驾驶行为与交通环境的关系，及不同驾驶决策的特点，建立不同决策场景下的驾驶决策蔟，即驾驶决策的关键参数集；

3.在驾驶决策蔟的基础上，在不同场景下，建立驾驶决策优化问题对当前场景的关键决策参数进行求解。

4.使用非线性模型预测方法进行轨迹规划，基于车辆动力学及运动学模型建立非线性模型，根据决策层关键决策参数，调整优化的目标及终端约束条件。通过对优化问题在线求解并滚动优化，在不基于固定轨迹形式下进行轨迹规划。

附图说明

图1为本发明的系统结构框图；

图2为紧急避撞场景示意图；

图3为无信号灯交叉路口场景示意图；

图4为单轨车辆动力学模型简图；

图5a为直道场景车辆运动示意图；

图5b为弯道场景车辆运动示意图；

图6a为紧急避撞场景对比实验方向盘转角结果图；

图6b为紧急避撞场景对比实验纵向加速度结果图；

图6c为紧急避撞场景对比实验横向位移结果图；

图6d为紧急避撞场景对比实验纵向速度结果图；

图7a为无信号灯交叉路口场景对比实验方向盘转角结果图；

图7b为无信号灯交叉路口场景对比实验纵向加速度结果图；

图7c为无信号灯交叉路口场景对比实验车辆位置结果图；

具体实施方式

一种基于参数决策框架的轨迹规划控制系统,基于分层的车辆控制系统架构，基于参数决策框架，即使用关键参数替代类人化的驾驶决策，可以更好的适应不同驾驶决策及多种驾驶场景下的复杂决策。

一种基于参数决策框架的轨迹规划结构框图如图1所示，主要包括：智能感知模块(a)，驾驶决策模块(b)，轨迹规划模块(c)。

所述智能感知模块(a)用于得到可用于车辆控制系统决策及规划的信息。该模块包括车载摄像头及雷达环境感知元件，使用自身标定的融合识别算法，需要得到的信息包含当前驾驶环境中道路限速，相邻车道中心线间宽度，道路中心线的曲线形式及道路类型(直到或者弯道)，周围存在车辆的位置及行驶车速信息。

所述驾驶决策模块(b)，用于制定驾驶决策，包含驾驶决策蔟及不同的决策场景的优化问题。其中驾驶决策蔟，即驾驶决策的关键参数集是通过分析类人驾驶行为与交通环境的关系，及不同驾驶决策的特点建立。不同的决策场景的优化问题是根据场景的特点，建立的相应优化问题，最终对当前场景下的关键决策参数值进行求解。

轨迹规划模块(c)，用于得到优化规划后的可行轨迹。在模块中包含建立非线性模型及非线性模型预测方法的使用。其中非线性模型的建立基于车辆动力学及运动学模型并考虑纵侧向运动控制。非线性模型预测方法的使用，根据决策层关键决策参数，调整优化的目标并得到不同终端约束条件；并且在不基于固定轨迹形式的情况下，通过对优化问题在线求解并滚动优化进行轨迹规划。

一种基于参数决策框架的轨迹规划控制方法，该控制方法包括以下步骤：

步骤一、决策及规划环境认知；

通过智能感知模块中的车载智能感知原件及其自身配备的识别算法，得到可用于车辆控制系统决策及规划的信息；所述车载智能感知原件包含车载摄像头及雷达，且原件自身包含已标定的融合识别算法，得到的可用于车辆控制系统决策及规划的信息，所述信息包含当前驾驶环境中道路限速、相邻车道中心线间宽度、道路中心线的曲线形式及道路类型即直道或者弯道、周围存在车辆的位置及行驶车速信；

步骤二、驾驶决策制定；

在驾驶决策模块中，提出使用基于参数决策框架，通过分析，选择不同驾驶决策及多种驾驶场景下决策需要的关键参数，组成驾驶决策簇；在不同的决策场景下，根据场景的特点，建立相应的优化决策问题，对当前场景下的关键决策参数值进行求解，具体方法如下：

21)驾驶决策簇建立：首先，由于轨迹规划层使用模型预测控制方法，即通过建立于轨迹规划相关的优化问题，通过在线求解达到轨迹规划的目的，因此根据方法特性，驾驶决策蔟包含目标函数项不等式约束项等式约束项及参数值项di∈{d1,d2,…,d9}，其中目标函数项与不等式约束项等式约束项为激活函数，其值为0或1，0表示该项在优化问题中不考虑，1表示该项在优化问题中考虑；由目标函数项与不等式约束项等式约束项组合成激活函数向量μ^o,μⁱ,μ^e，分别表示为具体的，包括以下步骤：

21.1)类人驾驶行为本身具有一定的属性，根据其持续特性，分为持续性行为即车道保持和暂时性行为即换道由d1的具体值决定；0代表持续性行为，1代表暂时性行为；相应的在模型预测控制中，持续性行为代表固定预测时域控制，暂时性行为代表预测时域递减控制；

21.2)动作行为的时间，由d2的具体值决定。对于不同驾驶场景下的不同驾驶行为，动作行为的持续时间会有所不同。如直道场景中，车道保持的动作时间一般为1s；而弯道情况下，由于车道中心线不沿车辆前进方向，为保持车辆处于车道中心，车道保持的动作时间小于直道时的动作时间，一般为0.5s；进行换道时，动作时间可根据实际场景进行优化，一般换道时间为2s～5s之间；进行转弯行为时，根据路口的大小，动作时间一般为5s～10s。

21.3)目标函数项中预测时域内控制量相关的积分型性能指标项激活项分别为分别对应u(1),u(2),△u(1),△u(2)，其中u(1)为车轮转角，u(2)为车辆加/减速度，△u(1)为离散化后方程中相邻时刻车轮转角的变化量，△u(2)为离散化后方程中相邻时刻车辆加/减速度的变化量；

21.4)目标函数项中预测时域内状态变量车辆横摆角速度ωr，侧向速度vy的积分型性能指标项激活项分别为

21.5)目标函数项中预测时域终端时状态变量纵向车速vx，坐标系x方向位置坐标x，坐标系y方向位置坐标y的终端型性能指标项激活项，分别为对应的终端期望值分别为d3，d4，d5；

21.6)约束项中的不等式约束μⁱ包含地面附着约束，控制变量车轮转角u(1)，车辆加/减速度u(2)，道路限速vx,max.，激活项分别对应其中道路限速vx,max.值为d6；

21.7)约束项中的等式约束对应预测时域终端时状态变量车辆侧向速度vy，横摆角速度ωr，航向角x方向位置坐标x，y方向位置坐标y，对应的激活项分别为其中若侧向速度vy，横摆角速度ωr的终端值被考虑，其期望值仅存在为0情况；航向角x方向位置坐标x，y方向位置坐标y对应的期望值分别为d7，d8，d9。

22)不同的场景下，驾驶决策问题建立；

在此系统中，驾驶场景中将包含不同的环境车辆，当环境车辆与本车的行驶路径存在路径冲突点时，通过路径冲突点的时序差将是评价多车环境下安全性的主要因素；因此，将此类决策问题建模为通过路径冲突点的时序规划问题；首先考虑是紧急避撞场景，在双向车道行驶时，若前车紧急停车，当对向车道无车辆行驶或者对向车辆距离较远时，本车除了采取紧急制动措施外，还在保证安全的前提下，临时借用对向车道进行紧急避撞，其中冲突点c为本车借用对向车道后返回原车道位置点，即通过时序规划环境车o3与本车o1通过冲突点c的时序；在此问题中，本车执行换道行为的时间t1＝2.5s，驾驶决策为ν，本车开始换道时刻的车速为vh,0，本车换向对向车道行为结束的速度为vh,1，在对向车道行驶的时间为t2，换回本车道时的速度为vh,2。控制量为u＝[ν,△vh,1,t2,△vh,2]，其中vh,1＝vh,0+△vh,1，vh,2＝vh,1+△vh,2，即△vh,1，△vh,2分别代表了本车在换道过程中及在对向车道行驶过程中速度变化量。决策的优化问题建立为找到最优控制量u^*，使得

s.t.tr>tr,min,vh,1≤vx,max.,vh,2≤vx,max.,u(3)≥0,△vmin≤u(2)≤△vmax,

其中，驾驶决策ν的值代表不同行为，0为紧急制动，1为紧急换道；tr是环境车o3与本车o1通过冲突点c的时间差；为保证安全，应保证tr>tr,min，tr,min为最小安全时间，为一设定常数值，可取2，vx,max.为道路最高限速，ah是前车紧急制动的制动减速度，这里假设前车制动过程中减速度不变；△vmin,△vmax为换道过程中为保证驾驶舒适性，速度变化最小最大限制值,ks1,1,ks1,2为权重系数；由以上优化问题得到的最优控制量，得到决策模块最终输出的决策参数d为形式为

其中，d中每一列对应决策参数值d1～d9，t1,t2,t3是行为的执行时间，这里t3＝t1为换道行为执行时间，d2＝△s1,2+s2+ssafe是本车返回原车道时在对向车道上的位移，△s1,2本车与紧急制动车辆在t1结束时刻的纵向相对距离，s2是紧急制动车辆在t2时段内的纵向位移，ssafe是同车道前后车的安全距离；d中每一行对应一个动作的决策参数值d1～d9。

其次是无信号灯交叉路口场景，此问题中存在三个潜在路径冲突点，同样地，将此问题建模为路径冲突点的时序问题，其中ooi∈{0,1|i＝1,2,3}表示此路径上存在或不存在环境车，ooi＝0表示不存在环境车，ooi＝1表示存在环境车，opi∈{0,1|i＝1,2,3}表示当前环境车辆在路径中位置是否已经通过了路径冲突点，opi＝1表示未通过，opi＝0表示已通过；由此，得到此场景下决策层的控制变量的维数为：

n＝op1*oo1+op2*oo2+op3*oo3.(3)

以n＝3时为例，此时控制变量为u＝[th,1,th,2,th,3]，其中th,1,th,2,th,3分别表示环境车辆o1，o2，o3到达路径冲突点的时间，以路径上匀速运动计算得到，决策的优化问题建立为找到最优控制量u^*使得

u(i)≥0,i＝1,2,3,

其中，si(i＝1,2,3)分别为相邻路径冲突点间的近似距离，以圆弧近似计算得到，因此总距离为ks2,1,ks2,2,ks2,3,ks2,4为相应优化项的权重系数，分别为本车通过路径冲突点的上下界约束时间，上下界时间的计算以环境车辆o1，o2，o3到达路径冲突点的时间th,1,th,2,th,3为基准，以路径冲突点为圆心，进入及离开rc＝2m为半径的圆的时间为上下界计算得到；为了保证安全性，本车通过路径冲突点的时刻应在此上下界以外；对于路径冲突点1，本车通过此路径点的时刻为或vh为本车进入路口时的车速，vr为此路口的期望车速；最后，由于转弯是一个连续的过程，如果在此过程中本车与环境车辆的交互关系相同，则对得到的优化结果进一步地进行合并，如果本车与环境车的交互关系均相同，则由决策模块输出的决策参数d为形式为

其中，l为路口宽度，由车道数及车道宽计算得到。

步骤三、优化轨迹规划求解；

通过轨迹规划模块中基于车辆动力学及运动学模型设计的非线性模型，使用非线性模型预测方法；根据决策层关键决策参数，调整优化的目标并得到不同终端约束条件；轨迹的求解过程不基于固定轨迹形式，通过对优化问题在线求解并滚动优化进行轨迹规划，具体方法如下：

31)纵侧向耦合的车辆运动控制非线性模型的建立；首先单轨车辆动力学模型如图4所示，建立的微分方程可以表示为：

其中，m是车辆质量，vx是纵向车速，∑fx为沿x方向纵向力之和，vy为车辆侧向速度，wr是车辆横摆角速度，fyf,fyr分别车辆前轮侧向力和后轮侧向力，iz为车辆沿z轴的转动惯量，lf,lr为前后轴轴距，由于前后轮侧偏角较小，因此可将前后轮侧偏角αf,αr线性化为

其中，δf为前轮转角。进一步地，轮胎侧向力可表示为

其中，cr,cf分别为前后轮侧偏刚度；同时，根据车辆的运动关系有ax为纵向加速度，为车辆的航向角。以及考虑车辆的运动在全局坐标系下的运动方程，可建立为：

其中，x,y是车辆在全局坐标系下的位置。最终，非线性的车辆运动模型建立为

其中，状态变量为fyf,fyr可由式(8)计算得到，控制变量为u＝[a,δf]。其中为简化运动控制方程，纵向速度数值大小的改变由加速度a控制实现，并由一下层跟踪控制器保证跟踪。

32)基于关键决策参数的轨迹规划控制器：使用非线性模型预测控制方法，基于建立的车辆运动控制模型及决策输出的关键决策参数，建立轨迹规划的优化问题，通过在线求解并滚动优化进行轨迹规划。在优化轨迹规划器中，目标函数考虑预测时域内控制量车轮转角u(1)，控制量车辆加/减速度u(2)，离散化后方程中相邻时刻车轮转角的变化量△u(1)，离散化后方程中相邻时刻车辆加/减速度的变化量△u(2)的积分型性能指标，预测时域内状态变量车辆横摆角速度ωr，侧向速度vy的积分型性能指标以及预测时域终端时状态变量纵向车速vx，x方向位置坐标x，y方向位置坐标y的终端型性能指标，这些性能指标在不同的决策下被不同程度的考虑。因此，每一项性能指标项由对应的激活项激活，激活向量可以表示为，同时每一个性能指标项由一个对应的权重系数值，权重系数向量可表示为k＝{ki|i＝1,2,...,n}。因此，控制器的目标函数可以表示为：

其中vx(tf),x(tf),y(tf)分别为纵向车速vx，x方向位置坐标x，y方向位置坐标y预测时域终端值，vt,xt,yt分别为期望的纵向车速vx，x方向位置坐标x，y方向位置坐标y终端函数值。

同时，由于控制器同时考虑了横向及纵向的车辆运动控制，因此在此过程中，车辆还需满足路面附着约束，即

其中u为路面附着系数，fx,fy分别为车轮纵向力和横向力，此约束由激活。同时，控制量应满足可执行约束

其中，*min，*max分别代表相应变量的最小值和最大值，此约束由激活。综合以上描述，由于加入地面附着约束条件时，会很大程度影响求解速度，因此，可通过限制amin，amax使得待优化问题满足地面附着约束。相应的路段信息也会带来的约束，如路段限速，可表示为

vx≤vx,max.(14)

此约束由激活。

此外，轨迹规划控制器中还包含等式约束，其与驾驶环境相关。举例来说，直道环境与弯道下的简图如图5a，图5b所示。不论是车道保持，还是换道任务，在预测时域终端，应满足一定的终端条件，才能任务的完成，即

其中分别为预测时域终端时刻横摆角速度，侧向速度，航向角，横向位移，yl,f为期望的终端横向位移。在换道任务中，终端横向位移为两车道中心线间的距离，在车道保持任务中，终端横向位移为0；类似地，在弯道环境中，由于道路不是直线，因此等式约束条件减少为式(15)中的后两项。其中第三项变为为期望的终端航向角，由实际弯道道路的曲率计算得到。特别地，在一些驾驶任务中，第四个终端条件变为对于x方向位置x的约束，如转弯任务，即在驶离路口时完成转弯动作。因此，以上五种可能存在终端约束条件由激活项激活。

决策模块通过当前的环境信息得到决策参数后，输入规划层，建立相应的优化问题，通过使用通用的非线性模型预测方法进行求解，可通过滚动时域，在线优化的方式，进行轨迹规划与控制。

为了证明以上基于参数决策框架的轨迹规划控制系统的有效性，通过仿真实验进行了验证，实验中，仿真问题的求解由matlab非线性规划函数包fmincon求解。

首先是验证轨迹规划控制器的有效性，通过改变控制器参数，实现了车道保持(p1)，换道(p2)，转弯(p3)，掉头(p4)四种驾驶任务。参数值项d，如表1所示，最终轨迹规划的结果如图6a、6b、6c、6d所示。

表1四种不同任务下参数值d

接着在以上的两种决策场景中进一步验证了整个基于参数决策框架的轨迹规划控制系统的有效性，紧急避撞场景不同的初始参数如表2所示，在四组对比试验中，环境车o3的初始位置会有所不同，会影响使用对向车道时的安全情况，同时在第二组和第三组实验中，环境车o3的驾驶意图也发生了改变，进行了加速操作，加速度由表中所示。最后实验结果如图6a，b，c，d中所示，本车的控制量曲线，即车轮转角，加减速度，轨迹及纵向车速如图所示。在不同的情况下，本车采取不同的决策，在初始距离较近时，本车直接采取紧急制动操作；在距离稍远的情况下，本车尝试通过紧急换道避障，但由于环境车o3驾驶意图改变进行了加速操作，为保证安全，本车又返回原车道进行紧急制动；在距离更远的情况下，尽管环境车o3驾驶意图改变进行了加速操作，但由于距离较远，本车没有改变通过紧急换道避障的意图，而是通过适当的加速以保证安全。当距离足够远时，本车进行正常的紧急换道避障操作。

表2紧急避撞场景下环境车o3不同初始条件

第二个场景是无交通信号灯路口转弯问题，不同的场景初始参数如表3所示，在三组平行试验中，三辆环境车具有不同的初始距离及速度，不同的是，在第三组实验中，环境车辆的驾驶意图在某一时刻发生改变。最后实验结果如图7a，b，c中所示，当三辆环境车的距离足够远时，本车先减速然后保持匀速状态转弯；当有环境车辆距离路口较近时，为了尽快转弯，并保证安全，本车以近似匀速的过程进入路口，然后转弯；而当环境车辆距离适中时，本车先减速准备进入路口，当发现有环境车辆进行加速操作，而本车的距离路口更近时，本车停止减速，以均速或者进一步加速的状态快速通过路口。

表3无交通信号灯路口转弯场景下环境车o1，o2，o3不同初始条件

由以上实验可以看出，本文提出的基于参数决策框架的轨迹规划控制系统不论是轨迹规划控制器还是系统功能上，均能满足提出的不同驾驶决策及多种驾驶场景下决策及进行轨迹规划控制的要求。