[发明专利]一种基于驾驶状态预测的人机力矩协同控制方法

权利要求书

1.一种基于驾驶状态预测的人机力矩协同转向控制方法，通过建立人-车-路模型实现对驾驶状态的预测，基于人-车-路模型和模型预测控制方法设计基于驾驶状态预测的人机力矩协同转向控制器，为了提高人机一致性，在建立人-车-路模型时引入辅助强度系数，并通过模糊逻辑方法实时确定辅助强度系数；为了进一步减轻驾驶负担，对驾驶员引导系数进行拟合并根据驾驶员引导系数对驾驶员进行引导，减小驾驶员输出力矩，其特征在于，本方法的步骤如下：

步骤一、建立人-车-路的简化模型

建立地面坐标系，原点O固结于当前时刻车辆质心o所处位置，X轴指向当前时刻车身正前方，X轴沿逆时针方向旋转90度的方向为Y轴正方向；

建立车身坐标系，原点与车辆质心o重合，x轴指向车身正前方，x轴沿逆时针方向旋转90度为y轴正方向，z轴指向车身正上方并垂直于x轴和y轴；

(1)驾驶员模型建立

两点预瞄驾驶员模型结合远近道路的两个区域的道路信息，远点F作为预测点，反应未来要达到的大致方向，近点N作为补偿点，使驾驶员逐渐调整到期望轨迹上，主要包括以下环节：

远预瞄角θ_far为车辆质心o到远点F的方向与车身正前方的夹角，单位：rad；远预瞄距离l_far为车辆质心o到远点F的距离，单位：m；根据几何关系和运动学原理，得到远预瞄角θ_far的表达式如下：

其中，R_v为车辆质心o轨迹的曲率半径，单位：m；r为车辆横摆角速度，单位：rad/s；v_x为车辆坐标系下车辆纵向速度，单位：m/s；

近预瞄角θ_near为车辆质心o到近点N的方向与车身正前方的夹角，单位：rad；近预瞄距离l_near为车辆质心o到近点N到车辆前进方向垂点的距离l_near，单位：m；横向偏移量y_L为近点N到车辆前进方向的垂直距离，单位：m；由于近预瞄角θ_near通常较小，得到近预瞄角θ_near表达式如下：

当车辆在直线上行驶时远预瞄角θ_far为零，当车辆在弯道上行驶时，根据道路弯曲程度来确定远预瞄距离l_far，也即确定远预瞄角θ_far；

为了产生正比于远预瞄角θ_far的驾驶员模型输出力矩T_d，引入视觉超前控制环节，视觉超前控制环节的输入为θ_far、输出为T_ant，视觉超前控制环节的传递函数G_a(s)如下：

G_a(s)＝K_α (3)

其中，K_a为远预瞄角θ_far的比例增益，反应驾驶员对远预瞄角θ_far的感知程度，本方法中选取K_a＝22；

为了基于近预瞄角θ_near生成与近预瞄距离l_near相关的纠正转向角，以确保驾驶员模型的路径跟踪精度，引入车道保持补偿控制环节，车道保持补偿控制环节的输入为θ_near、输出为T_com，车道保持补偿控制环节的传递函数G_c(s)如下：

其中，K_c为近预瞄角θ_near的比例增益，反应驾驶员对近预瞄角θ_near的感知程度，本方法中选取K_c＝14；T_L为驾驶员模型的超前时间常数，单位：s，本方法中选取T_L＝2.4；T_I为驾驶员模型的滞后时间常数，单位：s，本方法中选取T_I＝0.2；

为了反映驾驶员感觉器官和神经系统处理信息的延时，引入延时环节进行等效，延时环节的输入为T_ant+T_com、输出为T_r，不同驾驶员的反应时间是不同的，取一次近似得到延时环节的传递函数G_L(s)如下：

其中，τ_p为延时环节时间常数，单位：s，本方法中选取τ_p＝0.04；

为了模拟驾驶员作用于转向系统而产生的肌肉运动的知觉部分，引入感知环节和动作环节两个子系统，感知环节的输入为δ_f、输出为T_f，动作环节的输入为T_f、输出为T_m，感知环节的传递函数G_k1(s)和动作环节的传递函数G_k2(s)如下：

其中，K_D为感知环节的比例增益，本方法中选取K_D＝1；K_G为动作环节的比例增益，本方法中选取K_G＝-0.63；T₁为感知环节时间常数，单位：s，本方法中选取T₁＝2.5；T_k1为动作环节的超前时间常数，单位：s，本方法中选取T_k1＝1.99；T_k2为动作环节的滞后时间常数，单位：s，本方法中选取T_k2＝0.013；

驾驶员手臂输出的转矩作用于转向盘从而控制车辆实现车道保持，驾驶员手臂动力学环节的输入为T_r-T_m-T_f、输出为驾驶员模型输出力矩T_d，驾驶员手臂动力学环节的传递函数G_nm(s)近似后如下：

其中，T_N为驾驶员手臂动力学模型时间常数，单位：s，本方法中选取T_N＝0.11；

同时，为了实现驾驶员模型与自动驾驶控制器人机共驾合作控制，驾驶员模型将被集成到自动驾驶控制器设计过程中，从而获得驾驶员模型在驾驶行为上的预测，驾驶员模型可以等效为一个三输入[θ_near θ_far δ_f]、单输出T_d的模型，由上式可得驾驶员模型的状态空间方程如下：

式中：

x_d＝[T_d x_d2 x_d3 x_d4 x_d5]^T

u_d＝[θ_far θ_near δ_f]^T

C_d＝[1 0 0 0 0]

其中，x_d为驾驶员状态矩阵；T_d为驾驶员模型输出力矩，单位：N·m；x_d2、x_d3、x_d4和x_d5为视觉超前控制环节、车道保持补偿控制环节、延时环节、感知环节、动作环节和驾驶员手臂动力学环节输入输出的线性组合，没有实际物理意义；Α_d为驾驶员系统矩阵；B_d为驾驶员输入矩阵；C_d为驾驶员输出矩阵；u_d为驾驶员控制量矩阵；y_d为驾驶员输出矩阵；δ_f为车辆前轮转角，单位：rad；

(2)车辆动力学模型建立

本方法中车辆动力学模型采用车辆二自由度模型；根据力矩和转矩平衡方程，得到车辆侧向速度v_y和车辆横摆角速度r表达式如下：

其中，m为车辆的质量，单位：kg；v_y为车辆坐标系下车辆侧向速度，单位：m/s；v_x为车辆坐标系下车辆纵向速度，单位：m/s；r为车辆横摆角速度，单位：rad/s；F_yf为车辆前轮侧向力，单位：N；F_yr为车辆后轮侧向力，单位：N；I_z为车辆绕z轴的转动惯量，单位：kg·m²；l_f为车辆质心o到车辆前轴的距离，单位：m；l_r为车辆质心o到车辆后轴的距离，单位：m；

本方法中进行小角度近似，认为车辆轮胎通常工作在线性区域，得到车辆前轮侧向力F_yf和车辆后轮侧向力F_yr如下：

其中，C_f为线性化的车辆前轮轮胎的侧偏刚度，单位：N/rad；C_r为线性化的车辆后轮轮胎的侧偏刚度，单位：N/rad；α_f为车辆前轮胎侧偏角，单位：rad；α_r为车辆后轮胎侧偏角，单位：rad；

车辆前轮胎侧偏角α_f和车辆后轮胎侧偏角α_r近似后如下：

整理可得线性二自由度车辆的状态空间方程如式(12)：

(3)车辆运动学模型

v_Y为地面坐标系下车辆侧向速度，单位：m/s；v_X为地面坐标系下车辆纵向速度，单位：m/s；地面坐标系下车辆侧向速度v_Y、地面坐标系下车辆纵向速度v_X和车辆坐标系下车辆侧向速度v_y、车辆坐标系下车辆纵向速度v_x关系如下：

其中，ψ为车辆横摆角，单位：rad；

假设车辆横摆角ψ较小，如果车辆前轮胎侧偏角α_f和车辆后轮胎侧偏角α_r保持在正常工作范围内，可进一步假设v_y远小于v_x，得：

定义车辆横摆角偏差ψ_L如下：

ψ_L＝ψ-ψ_d (15)

其中，ψ_d为期望的车辆横摆角，单位：rad；

为了跟踪期望路径，选取车辆横摆角偏差ψ_L与横向偏移量y_L来进行建模，这两个量可以很好的反应车辆跟踪期望路径的状态；

对式(15)求导，根据几何关系和运动学原理得车辆横摆角偏差ψ_L的导数如下：

其中，ρ为道路曲率，单位：1/m；

横向偏移量y_L通过几何关系可以近似表示如下：

y_L＝y_vc+l_nearψ_L (17)

其中，y_vc为车辆质心o当前位置与期望路径之间的偏差，单位：m；

对式(17)进行求导可得：

(4)转向系统模型

车辆方向盘转角δ_s和车辆前轮转角δ_f的关系如下：

δ_s＝i_s·δ_f

其中，δ_s为车辆方向盘转角，单位：rad；i_s为车辆转向系统传动比系数；

由车辆动力学原理可得车辆回正力矩的表达式如下：

其中，为车辆回正力矩，单位：N·m；K_aln为车辆回正力矩比例系数，为车辆自身参数；β为车辆质心侧偏角，单位：rad；

外部施加力矩T的表达式如下：

T＝T_d+ωT_c

其中，ω为辅助强度系数；T_c为辅助力矩，单位：N·m；

车辆转向系统的转矩平衡方程如下：

其中，J_s为车辆转向柱转动惯量，单位：kg·m²；b_s为车辆转向柱阻尼比；

整理得到转向系统的状态空间方程如下：

其中，η_t为轮胎印迹常数，为车辆自身参数；

(4)人-车-路模型建立

人-车-路闭环系统的状态空间方程表达式如下：

式中，

u＝ωT_c

C＝[0 0 0 0 0 0 0 0 0 -l_near 1]^T

其中，x为人-车-路系统状态向量；u为人-车-路系统控制量；A为人-车-路系统矩阵；B为人-车-路系统输入矩阵；C为人-车-路系统输出矩阵；D为人-车-路系统曲率适配矩阵；y为人-车-路系统输出向量；

步骤二、确定辅助强度系数

辅助强度系数ω是通过道路危险参数P_road和辅助强度参数P_con按照模糊逻辑得出的；

(1)道路危险参数P_road表达式如下：

其中，E_A为道路危险参数P_road的调节系数，使道路危险参数P_road满足P_road∈[0,1]；车辆质心o当前位置与期望路径之间的偏差y_vc的绝对值越大，道路危险参数P_road越接近于1，车辆危险程度就越高；

(2)辅助强度参数P_con主要包括如下一致状态与非一致状态两种状态，表达式如下：

T_c·T_d≥0时为一致状态，该状态下P_con越接近于1，辅助力矩占比越大，辅助强度越强；T_c·T_d＜0时为非一致状态，该状态下P_con越接近于-1，说明辅助力矩占比越大，辅助强度越强；

(3)采用模糊逻辑方法，获得辅助强度系数ω关于道路危险参数P_road和辅助强度参数P_con的函数f(P_road,P_con)：

道路危险参数P_road范围为[0,1]，将其划分为四个集合，即：s1表示安全，s2表示较安全，d1表示较危险，d2表示危险，道路危险参数P_road的隶属度函数如下：

其中，A₁₁、B₁₁、C₁₁、A₁₂、B₁₂、C₁₂、A₁₃、B₁₃、C₁₃、A₁₄、B₁₄和C₁₄为常数；

辅助强度参数P_con范围为[-1,1]，在一致状态条件下为非负的，在非一致状态条件下为负的，将一致状态和非一致状态合在一起划分为5个集合，即：f2表示较大冲突，f1表示较小冲突，c1表示较小一致，c2表示中度一致，c3表示较大一致，辅助强度参数P_con的隶属度函数如下：

其中，A₂₁、B₂₁、C₂₁、A₂₂、B₂₂、C₂₂、A₂₃、B₂₃、C₂₃、A₂₄、B₂₄、C₂₄、A₂₅、B₂₅和C₂₅为常数；

辅助强度系数ω范围为[0,1]，将其划分为四个集合，即：t1表示轻度辅助，t2表示中度辅助，t3表示较强辅助，以及t4表示高度辅助，辅助强度系数ω的隶属度函数如下：

其中，A₃₁、B₃₁、C₃₁、A₃₂、B₃₂、C₃₂、A₃₃、B₃₃、C₃₃、A₃₄、B₃₄和C₃₄为常数；

建立模糊规则如表1所示：

表1模糊规则

通过模糊规则与道路危险参数P_road的隶属度函数、辅助强度参数P_con的隶属度函数和辅助强度系数ω的隶属度函数确定辅助强度系数ω关于道路危险参数P_road和辅助强度参数P_con的函数f(P_road,P_con)；

(4)辅助强度系数ω的实时确定

实时确定道路危险参数P_road和辅助强度参数P_con，利用辅助强度系数ω关于道路危险参数P_road和辅助强度参数P_con的函数f(P_road,P_con)得到辅助强度系数ω；

步骤三、利用步骤二得到的辅助强度系数ω，采用模型预测控制方法进行基于驾驶状态预测的人机力矩协同控制器设计

(1)本方法的控制目标如下：

1)使车辆尽可能跟随期望路径，即使车辆质心o当前位置与期望路径之间的偏差y_vc尽可能小，提高车辆行驶安全性；

2)人-车-路系统控制量u尽可能小，达到节能目的；

(2)基于驾驶状态预测的人机力矩协同控制器设计：

本方法做出如下假设：假设自动驾驶车辆在一个预测时域内保持恒速行驶，式(22)为人-车-路系统的连续模型，为用于基于驾驶状态预测的人机力矩协同控制算法的设计，需要将式(22)离散化，得到离散时间的人-车-路闭环系统模型如下：

式中，C_c＝C,其中，τ为积分变量；T_s为采样时间，单位：s；A_c为离散化后的人-车-路系统矩阵；B_c为离散化后的人-车-路系统输入矩阵；C_c为离散化后的人-车-路系统输出矩阵；D_c为离散化后的人-车-路道路曲率矩阵；

定义控制量序列U_k为：

假定预测时域为P步，控制时域为N步，且满足N≤P，同时假定控制时域之外的控制量保持不变，即u(k+N)＝u(k+N+1)＝…＝u(k+P-1)，可推导出预测时域P步内的驾驶状态预测方程如下：

其中，x(k+i)为k+i时刻的系统状态量，i＝0,1,…,P；u(k+i)为k+i时刻的优化量，i＝0,1,…,P-1；ρ(k+i)为k+i时刻的道路曲率，i＝0,1,…,P-1；

预测时域P步内的输出预测方程如下：

其中，y_vc(k+i)为k+i时刻的系统输出量，i＝0,1,…,P；

于是可以得到控制器目标函数如下：

其中，J为控制器的目标函数；Γ₁为路径偏差权重系数；Γ₂为控制量权重系数；

同时控制器还应满足一定的执行机构约束和安全性约束如下：

综上所述，得基于驾驶状态预测的人机力矩协同转向控制方法的约束优化问题如下：

满足：x(k+i+1)＝A_cx(k+i)+B_cu(k+i)+D_cρ(k+i)

-1≤y_vc(k+i)≤1

-60≤u(k+i)≤60

求解上述约束优化问题，得k时刻优化解u(k)；

步骤四、确定驾驶员引导系数

定义驾驶员输出力矩为T′_d，单位：N·m；

(1)一致驾驶负担占比α₁的表达式如下：

α₁越接近于1，驾驶员输出力矩T′_d越大，驾驶负担越重；

(2)冲突驾驶负担占比α₂的表达式如下：

α₂越接近于-1，驾驶员误操作程度越大，驾驶负担越重；

(3)抑制驾驶负担占比α₃的表达式如下：

α₃越接近于-1，驾驶员输出力矩T′_d越大，驾驶负担越重；

(4)驾驶员引导系数拟合：

根据上面的定义，可知每个时刻驾驶员与控制器之间的关系必然在这三种关系中的一种，为了刻画三种关系与驾驶员引导系数α之间的关系，按照引导趋势，可以刻画出驾驶员引导系数α如下：

其中，c为驾驶员引导系数α的比例系数，i＝1或2或3；

驾驶员引导系数α的最大值为1，代表着驾驶员按照自己原来的想法输出力矩；驾驶员引导系数α逐渐减小到0，代表着驾驶员在原有输出意愿上减小相应的倍数，驾驶负担减轻；驾驶员引导系数α在[0,1]范围内为在人机一致情况下对驾驶员进行引导，使驾驶员引导系数α逐渐减小到0，驾驶员输出力矩T′_d逐渐减小，驾驶负担减轻；驾驶员引导系数α在[-1，0]的情况下，为在人机冲突或抑制的情况下对驾驶员进行引导，使驾驶员引导系数α逐渐减小到0，驾驶员输出力矩T′_d逐渐减小，冲突程度或抑制程度减轻；

车辆转向系统根据驾驶员引导系数α产生相应的反馈效果作用于驾驶员手臂的神经肌肉环节，驾驶员根据上一时刻反馈效果调节当前时刻驾驶员输出力矩T′_d；

步骤五、选取控制量并完成控制，选取控制量u'如下：

u'＝T′_d+u(k) (36)

将控制量u'作用于车辆转向系统，并根据驾驶员引导系数α产生相应的反馈效果作用于驾驶员手臂，对驾驶员进行引导，到下一时刻，根据当前人-车-路状态重新求解基于驾驶状态预测的人机力矩协同转向控制方法的约束优化问题，选取控制量u'作用于车辆转向系统，并根据驾驶员引导系数α产生相应的反馈效果作用于驾驶员手臂，对驾驶员进行引导，如此往复，实现滚动优化控制。