本发明涉及一种基于区域虚拟力场的智能汽车模型预测决策方法，以区域虚拟力场方法对存在障碍车的多车道场景进行动态交通场景模拟，并基于此决策出控制智能汽车运动的控制量并作用于汽车。
背景技术：
随着汽车保有量的日益增加，道路交通区域密集化与复杂化，驾驶员的驾驶压力大大增加。智能车辆可以通过先进的技术完成自出避障，规避风险，已经成为智能化的一个重点研究方向。在不同交通环境下的决策是其中的关键技术之一，如何对复杂交通场景进行建模，以及对行驶的障碍车进行动态预测决策出最优的控制量来控制车辆的运动是智能车决策研究的难点问题。而且，决策的控制量不仅要保证智能车辆可以躲避障碍行驶，还要考虑驾驶员的舒适性，车辆运动状态的平顺性以及车辆的操纵稳定性。在完成驾驶任务的同时给乘车人更好的体验。
技术实现要素：
为了解决现有技术存在的上述问题，本发明提供一种基于区域虚拟力场的智能汽车模型预测决策方法，其通过区域虚拟力场进行动态交通环境建模，采用约束模型预测控制进行路径规划。
本发明的目的是通过以下技术方案实现的：
1.一种基于区域虚拟力场的智能汽车模型预测决策方法，包括以下步骤：
步骤一、建立主车的动力学与运动学模型以及障碍车运动学模型，主车动力学与运动学模型：
式中，
x＝[yo ψ β r]T,u＝δf.
由于假设驾驶纵向速度不变，车辆质心的纵向位置xo与状态量无关，只与固定纵向速度有关，所以单独进行考虑，如公式(2)：
式中，x为系统的状态向量；u为系统控制量；A为系统矩阵；B为输入矩阵；C为常数矩阵；xo为车辆质心o的纵向位置，单位：m；yo为车辆质心o的侧向位置，单位：m；ψ为车辆航向角，单位：rad；v为车辆质心处的纵向速度，单位：m/s；β为车辆的质心侧偏角，单位：rad；r为车辆的横摆角速度，单位：rad/s；Cf为车辆前轮轮胎的侧偏刚度，单位：N/rad；Cr为车辆后轮轮胎的侧偏刚度，单位：N/rad；m为车辆的质量，单位：kg；Iz为车辆绕z轴的转动惯量，单位：kg·m2；a为车辆质心o到车辆前轴的距离，单位：m；b为车辆质心o到车辆后轴的距离，单位：m；δf为车辆的前轮转角，单位：rad；
障碍车运动学模型：
式中，xObs是障碍车纵向位置，单位：m；yObs是障碍车侧向位置，单位：m；vObs是障碍车速度，单位：m/s；是障碍车航向角，单位：rad；
步骤二、车辆行驶车道区域划分：
考虑多车道道路每条车道的道宽以及车辆的形状，将道路区域划分为车道间区域L′12以及车道内区域L′1,L′2：
式中，L1,L2表示1车道和2车道；f1(x)为通过感知系统扫描后处理得到的前方1车道的左边界；f2(x)为通过感知系统扫描后处理得到的前方1车道的右边界(即2车道的左边界)；f3(x)为通过感知系统扫描后处理得到的前方2车道的右边界；d为车辆宽度，单位：m。
步骤三、建立基于区域虚拟力场的道路环境模型：
考虑步骤二中的区域划分以及障碍车建立区域虚拟力场，包括在障碍车周围沿道路方向的虚拟矩形持斥力场、车道保持区域虚拟引力场以及行驶目标虚拟引力场三部分。车道内区域L′1,L′2具有虚拟引力场，吸引车辆行驶在浅灰色车道区域内，深灰色区域是障碍车O产生的虚拟斥力场矩形作用域，对行驶其附近的车辆产生虚拟斥力，其区域形状由Ds1,Ds2,Ds3三个参数决定，Ds1为垂直道路方向的矩形作用域宽度，Ds2为以障碍车O为起点沿车道行驶方向反方向矩形作用域长度，Ds3为以障碍车O为起点沿车道行驶方向正方向矩形作用域长度。
1)车道区域保持虚拟引力场：
目标：1.保证车辆行驶在道路区域内。2.保证车辆尽量行驶在车道内区域。
力场方向：1.沿道路法线方向指向道路区域。2.沿道路法线方向指向车道间区域。
受力大小：目标1作用力为F1，目标2作用力为F2，作用力的大小如式(5)：
式中，droad是车道内区域宽度，单位：m；dde是主车偏移车道区域的距离，单位：m；v为主车车速，单位：m/s；λi,κi为调节因子。
2)障碍车虚拟矩形斥力场：
目标：防止主车与障碍车发生碰撞。
力场方向：沿道路方向由障碍车指向主车。
力场区域确定：矩形力场区域参数如式(6)：
式中，d0为两车静止时需要保持的最小距离，aObs为障碍车的平均制动加速度，ahost为主车的平均制动加速度，Ts1,Ts2,Ts3为安全距离调节因子。
受力大小：力场作用力fp(x0,y0,v)表达如式(7)：
式中，O(j)代表第j辆障碍车，vObs(j)代表第j辆障碍车的速度，xObs(j)代表第j辆障碍车的纵向位置，yObs(j)代表第j辆障碍车的侧向位置，η1,η2,η3为调节因子。
3)期望行驶目标虚拟引力场：
目标：使主车趋近行驶目标。
方向：由主车当前的纵向位置与侧向位置指向目标位置的纵向位置和侧向位置。
受力大小：力场纵向作用力fx与侧向作用力fy大小如式(8):
式中，xfinal，yfinal是目标位置的纵向与侧向坐标，η4,η5为调节因子。
步骤四、利用步骤三建立的道路模型进行决策控制器的设计：
满足:
式中：
Cψ＝[0 1 0 0].
其中，J为优化函数的目标函数；x0(i)代表主车第i步预测纵向位置，单位：m；y0(i)代表主车第i步预测纵向位置，单位：m；v(i)代表主车第i步预测速度，单位：m/s；fx(i),fy(i)分别是主车第i步预测位置的纵向引力和侧向引力；Γ1,Γ2,Γ3,Γ4,Γ5分别为平衡各目标添加的权重因子；δfmin为前轮转角最小值，单位：rad；δfmax为前轮转角最大值，单位：rad；βmin为质心侧偏角最小值，单位：rad；βmax为质心侧偏角最大值，单位：rad。
步骤五、选取控制量并完成控制：
选取控制率u为：
u＝U*(1) (10)
其中，U*为优化得到的最优控制序列；即选取最优控制序列的第一个量作为控制量作用到被控车辆上；到下一时刻，基于模型预测的决策器将根据当前车辆状态重新计算一个最优控制量；以此往复，实现滚动优化控制。
通过以上方案的实施，本发明的有益效果为：
1、本发明可以躲避障碍车辆，稳定行驶。
2、本发明考虑了障碍车的动态行驶。
3、本发明对环境进行建模，可以适应动态的交通场景。
附图说明
图1为本发明所述的道路区域划分示意图
图2为区域虚拟力场作用域示意图
图3为主车动力学模型示意图
图4为主车运动学模型示意图
图5为障碍车运动学模型示意图
具体实施方式
以下结合附图对本发明进行详细说明：
本发明是一种基于区域虚拟力场的智能汽车模型预测决策方法，具体实施步骤如下：
步骤一、建立主车的动力学和运动学模型以及障碍车运动学模型
(1)主车动力学模型建立
车辆动力学模型如图3所示，其中车辆质心o为坐标原点，在这里只考虑车辆的侧向运动动力学和横摆运动运动学，忽略车辆的纵向动力学。于是我们可以得到一个简化的二自由度车辆模型。车身前进方向为x轴正方向，垂直于x轴向上的为y轴正方向。根据动力学知识可得二自由度动力学方程如式(11)所示：
其中，β为质心侧偏角，单位：rad；v为车辆质心处的纵向速度，单位：m/s；r为车辆的横摆角速度，单位：rad/s；Cf为车辆前轮轮胎的侧偏刚度，单位：N/rad；Cr为车辆后轮轮胎的侧偏刚度，单位：N/rad；m为车辆的质量，单位：kg；Iz为车辆绕z轴的转动惯量，单位：kg·m2；a为车辆质心o到车辆前轴的距离，单位：m；b为车辆质心o到车辆后轴的距离，单位：m；δf为车辆前轮转向角，单位：rad；
(2)主车运动学建模
车辆动力学方程示意图如图4所示，假设车辆为一个刚体，由于车辆在行驶过程中感知到的道路曲率较小，并且航向角ψ和质心侧偏角β也都在较小范围内变化，所以我们可以得到简化后的车辆运动学方程如式(12)所示：
式中，β为质心侧偏角，单位：rad；xo为车辆质心o的纵向位置，单位：m；yo为车辆质心o的侧向位置，单位：m；r为车辆的横摆角速度，单位：rad/s；ψ为车辆航向角，单位：rad；
(3)建立主车动力学和运动学模型
我们假设车辆的纵向速度v保持不变，结合式(11)和式(12)可以得到主车动力学和运动学的微分方程如式(13)所示：
车辆质心的纵向位置xo与状态量无关，只与固定纵向速度有关，所以单独进行考虑，如公式(14)：
我们选取[yoψβr]作为系统状态变量，选取前轮转角δf作为系统控制输入。于是我们可以得到系统状态方程如式(15)所示：
其中：
x＝[yo ψ β r]T,u＝δf.
其中，x为系统的状态向量；u为系统控制量；A为系统矩阵；B为输入矩阵；
(3)障碍车运动学建模
式中，xObs是障碍车纵向位置，单位：m；yObs是障碍车侧向位置，单位：m；vObs是障碍车速度，单位：m/s；是障碍车航向角，单位：rad；
步骤二、车辆行驶车道区域划分
如图1，考虑多车道道路每条车道的道宽以及车辆的形状将道路区域划分为车道间区域L′12以及车道内区域L′1,L′2。将主车用质点进行考虑，为保证简化合理性，对车道进行处理，把每条车道的左右车道边界缩进车宽的一半，缩进后的区域为车道内区域。其余在道路区域内的其他区域为车道间区域。
式中，L1,L2表示1车道和2车道；f1(x)为通过感知系统扫描后处理得到的前方1车道的左边界；f2(x)为通过感知系统扫描后处理得到的前方1车道的右边界(即2车道的左边界)；f3(x)为通过感知系统扫描后处理得到的前方2车道的右边界；d为车辆宽度，单位：m。
步骤三、建立基于区域虚拟力场的道路环境模型
考虑步骤二中的区域划分以及障碍车建立区域虚拟力场，包括在障碍车周围沿道路方向的虚拟矩形斥力场，车道保持区域虚拟引力场以及行驶目标虚拟引力场三部分。如图2所示。车道内区域L′1,L′2具有虚拟引力场，吸引车辆行驶在绿色车道区域内，红色区域是障碍车O产生的虚拟斥力场矩形作用域，对行驶其附近的车辆产生虚拟斥力，其区域形状由Ds1,Ds2,Ds3三个参数决定，Ds1为垂直道路方向的矩形作用域宽度，Ds2为以障碍车O为起点沿车道行驶方向反方向矩形作用域长度，Ds3为以障碍车O为起点沿车道行驶方向正方向矩形作用域长度。
1)车道区域保持虚拟引力场：
目标：1.保证车辆行驶在道路区域内。2.保证车辆尽量行驶在车道内区域。
力场方向：1.沿道路法线方向指向道路区域。2.沿道路法线法线方向指向车道间区域。
受力大小：目标1作用力为F1，目标2作用力为F2，作用力的大小如式(18)
式中，droad是车道内区域宽度，单位：m；dde是主车偏移车道区域的距离，单位：m；v为主车车速，单位：m/s；λi,κi为调节因子。
2)障碍车虚拟矩形斥力场：
目标：防止主车与障碍车发生碰撞。
力场方向：沿道路方向由障碍车指向主车。
力场区域确定：矩形力场区域参数如式(19)
式中，d0为两车静止时需要保持的最小距离，aObs为障碍车的平均制动加速度，ahost为主车的平均制动加速度，Ts1,Ts2,Ts3为安全距离调节因子。
受力大小：力场作用力fp(x0,y0,v)表达如式(20)。
式中，O(j)代表第j辆障碍车，vObs(j)代表第j辆障碍车的速度，xObs(j)代表第j辆障碍车的纵向位置，yObs(j)代表第j辆障碍车的侧向位置，η1,η2,η3为调节因子。
3)期望行驶目标虚拟引力场：
目标：使主车趋近行驶目标。
方向：由主车当前的纵向位置与侧向位置指向目标位置的纵向位置和侧向位置。
受力大小：力场纵向作用力fx与侧向作用力fy大小如式(21)。
式中，xfinal，yfinal是目标位置的纵向与侧向坐标，η4,η5为调节因子。
步骤四、利用步骤三建立的道路模型进行模型预测控制器的设计
假设主车在一个预测时域内保持恒速行驶，首先将建立的模型离散化，如式(22)：
x(k+1)＝Acx(k)+Bcu(k) (22)
式中，其中Ts为采样时间。
然后根据离散模型推导预测方程。预测步长选取为P，控制步长选取为M，并且M≤P，推导出k时刻输出YP(k+1)与状态X(k+1)的预测方程为式(23)：
其中，
由于假设驾驶纵向速度不变，车辆质心的纵向位置xo与状态量无关，只与固定纵向速度有关，所以单独进行考虑，x0推导如公式(24)：
xo(k+i)＝vTs+x(k+i-1),i＝1…P (24)
为了满足无障碍车情况下车道保持的巡航行驶，根据建立的车道保持虚拟引力场，当车辆处于车道内区域时不受引力作用，当车辆处于车道间和车道区域外时受到不同程度的引力，并且偏离车道内区域距离越大所受引力越大。为了控制车辆行驶在车道内区域，以主车受到的车道保持引力最小为优化目标一；目标一的数学表达为式(25)，式中fh(i)是预测第i步的车道保持区域虚拟力场受力大小。
为了及时变道躲避障碍车，根据建立的障碍车周身矩形虚拟斥力场，当车辆进入障碍车矩形虚拟斥力场时，受到障碍车的斥力影响，且距离障碍车越近斥力越大；当车辆不处于障碍车的虚拟斥力场范围时，不受斥力的影响，为了保证主车不与障碍车发生碰撞，以主车所受障碍车斥力最小为优化目标二，其数学表达式如式(26)，x0(i)代表主车第i步预测纵向位置，y0(i)代表主车第i步预测纵向位置，v(i)代表主车第i步预测速度。
为使规划的车辆行驶方向始终沿道路趋向于既定的目标位置，根据建立期望位置的引力场，引力场中主车距离期望位置越近引力越小，所以以纵向引力和侧向引力最小为优化目标三和四，如式(27)，式中fx(i),fy(i)分别是主车第i步预测位置的纵向引力和侧向引力。
为了保证车辆行驶的稳定性，以车辆的前轮转角尽可能的小为目标五，如式(28)
由于各部分目标之间互相制约，为了协调各目标，加入权重因子Γ1,Γ2,Γ3,Γ4,Γ5，最终的目标如式(29)：
J＝Γ1J1+Γ2J2+Γ3J3+Γ4J4+Γ5J5 (29)
除了建立的虚拟力场外，由于车辆的转向机构存在饱和约束，对车辆前轮转角δf进行限制，如式(30)，δfmin为前轮转角最小值，δfmax为前轮转角最大值，并且为保证车辆行驶过程中的稳定性，对质心侧偏角β进行约束以达到车辆稳定的要求，如式(31)，βmin为质心侧偏角最小值，βmax为质心侧偏角最大值。
δfmin≤δf(k+i)≤δfmax,i＝1…M (30)
βmin≤β(k+i)≤βmax,i＝1…P (31)
综上所述，总的优化问题如下：
满足:x(k+i+1)＝Acx(k+i)+Bcδf(k+i)
δfmin≤δf(k+i)≤δfmax,i＝1…M
βmin≤β(k+i)≤βmax,i＝1…P
(32)
式中：
Cψ＝[0 1 0 0].
其中，J为优化函数的目标函数；x0(i)代表主车第i步预测纵向位置，单位：m；y0(i)代表主车第i步预测纵向位置，单位：m；v(i)代表主车第i步预测速度，单位：m/s；fx(i),fy(i)分别是主车第i步预测位置的纵向引力和侧向引力；Γ1,Γ2,Γ3,Γ4,Γ5为平衡各目标添加的权重因子；δfmin为前轮转角最小值，单位：rad；δfmax为前轮转角最大值，单位：rad；βmin为质心侧偏角最小值，单位：rad；βmax为质心侧偏角最大值，单位：rad。
步骤五、选取控制量并完成控制
选取控制率u为：
其中，为优化得到的最优控制序列；即选取最优控制序列的第一个量作为控制量作用到被控车辆上；到下一时刻，基于模型预测的决策控制器将根据当前车辆状态重新计算一个最优控制量；以此往复，实现滚动优化控制。