[发明专利]基于区域虚拟力场的智能汽车横纵向耦合路径规划方法

权利要求书

1.基于区域虚拟力场的智能汽车横纵向耦合路径规划方法，其特征在于，具体步骤如下：

步骤一、建立主车的动力学和运动学模型：

(1)主车动力学模型建立

考虑车辆的侧向运动动力学和横摆运动学以及纵向动力学，建立车身坐标系，车辆质心o为坐标原点，车身前进方向为x轴正方向，垂直于x轴向上的为z轴正方向，根据动力学知识可得二自由度动力学方程如下式(1)所示：

其中，m为车辆的质量，单位，kg；a_y分别为沿y轴方向车辆质心的惯性加速度，称为侧向加速度，单位：m/s²；I_z为车辆绕z轴的转动惯量，单位：kg·m²；a为车辆质心o到车辆前轴的距离，单位：m；b为车辆质心o到车辆后轴的距离，单位：m；F_xf为车辆前轮的纵向力，单位：N；F_yf为车辆前轮的侧向力，单位：N；F_yr为车辆后轮的侧向力，单位：N；δ_f为车辆前轮转向角，单位：rad；r为车辆的横摆角速度，单位：rad/s；

经过小角度假设并利用线性轮胎模型：

式中，C_f为车辆前轮的轮胎侧偏刚度，单位：N·rad；C_r为车辆后轮的轮胎侧偏刚度，单位：N·rad；α_f为车辆前轮的轮胎侧偏角，单位：rad；α_r为车辆后轮的轮胎侧偏角，单位：rad；

利用纵向速度和侧向速度来计算轮胎的侧偏角为：

经过整理可以得到车辆的动力学模型：

(2)主车运动学建模：

仅考虑车辆系统的几何关系，以数学方式来描述车辆的运动情况：

式中，x_o为惯性系下车辆质心o的纵向位置，单位：m；y_o为惯性系下车辆质心o的侧向位置，单位：m；r为车辆的横摆角速度，单位：rad/s；为车辆横摆角，单位：rad；

对车辆转向运动进行分析，可以得到车辆质心的运动加速度在车身坐标系的分量：

整理得到车辆的运动学模型：

(3)建立主车动力学和运动学模型

整理主车动力学模型和运动学模型得到主车动力学和运动学模型下：

v_x为车身坐标系下的纵向速度，单位：m/s；v_y为车身坐标系下的侧向速度，单位：m/s；为车辆的横摆角，单位：rad；r为车辆的横摆角速度，单位：rad/s；δ_f为车辆的前轮转角，单位：rad；a_x为沿x轴方向车辆质心的惯性加速度，称为纵向加速度，单位：m/s²；a_y为沿y轴方向车辆质心的惯性加速度，称为侧向加速度，单位：m/s²；x₀为惯性系下车辆质心o的纵向位置，单位：m；y₀为惯性系下车辆质心o的侧向位置，单位：m；I_z为车辆绕z轴的转动惯量，单位：kg·m²；a为车辆质心o到车辆前轴的距离，单位：m；b为车辆质心o到车辆后轴的距离，单位：m；m为车辆的质量，单位：kg；

选取作为系统状态变量，选取前轮转角δ_f和纵向加速度a_x作为系统控制输入，则式(8)表示为：

在任意点(x_r，u_r)附近进行泰勒展开，保留一阶项，进行线性化得到：

其中J_f(x)和J_f(u)分别为f对x和u的雅克比矩阵，对于上述线性化后的时变系统，写为：

其中Δx＝x-x_r，Δu＝u-u_r，A_c(t)和B_c(t)即为雅可比矩阵，表示如下

步骤二、车辆行驶车道区域划分

以双车道为例，由感知模块对道路环境扫描处理得到道路边界线f₁(x)、f₂(x)和f₃(x)，对上述道路进行车道区域划分，L₁为1车道即f₁(x)和f₂(x)范围内的道路区域，L₂为2车道即f₂(x)和f₃(x)范围内的道路区域，L₁₂′为车道间区域，L₁′、L₂′为车道内区域，d为车辆宽度，数学描述为：

步骤三、建立基于区域虚拟力场的道路环境模型：

考虑步骤二中的区域划分以及障碍车建立区域虚拟力场，包括在障碍车周围沿道路方向的虚拟矩形持斥力场、车道保持区域虚拟引力场两部分；

1)车道区域保持虚拟引力场：

车道区域保持虚拟引力场包括两部分，一部分是使车辆行驶在道路区域内的引力F₁，另一部分是使车辆尽量行驶在车道内区域的引力F₂，引力F₁和引力F₂的作用力定义为：

式中，d_road是车道内区域宽度，单位：m；d_de是主车偏移车道区域的距离，单位：m；v为主车车速，单位：m/s；λ_i,κ_i为调节因子，λ_i决定作用力的幅值，κ_i决定作用力的变化速度；

车道保持区域虚拟引力场f_h定义为：

2)障碍车虚拟矩形斥力场：

为了避免主车与障碍车发生碰撞，建立障碍车虚拟矩形斥力场，障碍车虚拟矩形斥力场的影响区域D由D_s1、D_s2和D_s3三个参数确定，定义为：

式中，d₀为安全距离，a_Obs为障碍车的平均制动加速度，a_host为主车的平均制动加速度，T_s1、T_s2和T_s3为权重系数；

障碍车虚拟矩形斥力定义为：

式中，v_Obs(j)代表第j辆障碍车的速度，x_Obs(j)代表第j辆障碍车的纵向位置，y_Obs(j)代表第j辆障碍车的侧向位置，D为障碍车虚拟矩形斥力场影响区域，η₁、η₂和η₃为调节因子，η₁决定障碍车动态矩形虚拟斥力场势能变化的紧急程度、η₂代表势能变化与相对速度的相关程度、η₃代表势能变化与相对位置的相关程度；

步骤四、利用步骤三建立的基于区域虚拟力场的道路环境模型进行模型预测控制器的设计：

对步骤一中得到的线性系统进行离散化处理，得到增量型状态空间模型：

其中，T为离散系统采样时间，Δx(k)＝x(k)-x(k-1)，Δu(k)＝u(k)-u(k-1)；

假设预测时域为P，控制时域为M，且M≤P，并假设控制时域之外控制量保持不变，然后根据模型预测控制的基本原理，基于当前时刻的测量信息和过程的历史信息，推导出k时刻输出Y_P(k+1)与状态X(k+1)的预测方程为：

其中，

综上所述，优化问题如下：

满足:

其中，J为优化函数的目标函数；v(i)代表主车第i步预测速度，单位：m/s；f_x(i)是主车第i步预测位置的纵向引力，f_y(i)是主车第i步预测位置的侧向引力；Λ₁,Λ₂,Λ₃,Λ₄分别为平衡各目标添加的权重因子；δ_fmax前轮转角的最大值，为前轮转角变化率的最大值，单位：rad；a_xmax为纵向加速度的最大值，为纵向加速度变化率的最大值，单位：m/s²；T为离散系统采样时间；

根据模型预测控制的原理可知，选取求解优化问题得到的控制序列U的第一组控制量(δ_f,a_x)作为跟踪控制器的控制输入，作用到被控车辆上，在下一时刻，根据当前时刻的车辆状态信息再次求解优化问题，得到新的最优控制序列，以此往复，实现了车辆的滚动优化控制。