发明内容
为了解决现有技术存在的上述问题,本发明提供一种基于危险程度变权重的人机协同转向控制方法,其通过判别驾驶员和车辆危险程度来改变驾驶权重,采用约束模型预测控制,在满足避障安全的前提下,使车辆能尽可能满足驾驶员驾驶意图。
本发明得目的是通过以下技术方案实现的:
1.一种基于危险程度变权重的人机协同转向控制方法,包括以下步骤:
步骤一、建立综合车辆动力学和运动学的简化模型:
式中,
x=[yo ψ β r]T,u=δf.
式中,x为***的状态向量;u为***控制量;A为***矩阵;B为输入矩阵;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;
步骤二、确定车辆安全行驶道路边界:
式中,fl(x)为通过感知***扫描后处理得到的前方可行道路区域的左边界;fr(x)为通过感知***扫描后处理得到的前方可行道路区域的右边界;w为车辆宽度,单位,m;lf为车辆质心o到车辆前端点F的距离,单位,m;lr为车辆质心o到车辆后端点R的距离,单位,m;ψ为车辆航向角,单位,rad;
步骤三、确定自动驾驶权重系数:
首先确定驾驶环境危险指数和驾驶员操作危险指数表达式,然后确定驾驶环境危险指数和驾驶员操作危险指数以及自动驾驶权重系数的隶属度函数,根据模糊规则划分不同危险程度等级,得到自动驾驶权重系数关于驾驶环境危险指数和驾驶员操作危险指数的三维map,最后实时确定驾驶环境危险指数和驾驶员操作危险指数,利用所述三维map得到自动驾驶权重系数;
步骤四、利用步骤三得到的自动驾驶权重系数,采用MPC方法进行基于危险程度变权重的人机协同转向控制器设计:
满足:x(k+i+1)=Acx(k+i)+Bcu(k+i)
式中:
Cψ=[0 1 0 0].
其中,J为优化函数的目标函数;δh为驾驶员期望的前轮转角,单位:rad;lf为车辆质心o到车辆前端点F的距离,单位:m;lr为车辆质心o到车辆后端点R的距离,单位:m;u(k+i)为k+i时刻的***控制量,即为车辆的前轮转向角,单位:rad;x(k+i)为k+i时刻的***状态向量;y(k+i)为k+i时刻的***输出量;P为预测时域,N为控制时域;Γd和Γh分别为控制量增量的权重系数和驾驶员目标权重系数;Γy和Γβ分别为道路中心线跟随权重系数和质心侧偏角权重系数;Γ为自动驾驶权重系数;fl(k+i)为前方可行道路区域左边界线fl(x)在时刻k+i的采样值,单位:m;fr(k+i)则为前方可行道路区域右边界线fr(x)在时刻k+i的采样值,单位:m;Ts为采样时间,单位:s;x为***的状态向量;A为***矩阵;B为输入矩阵;
步骤五、选取控制量并完成控制:
选取控制率u为:
u=U*(1) (4)
其中,U*为优化得到的最优控制序列;即选取最优控制序列的第一个量作为控制量作用到被控车辆上;到下一时刻,基于模型预测控制的共享转向控制器将根据当前车辆状态重新计算一个最优控制量;以此往复,实现滚动优化控制。
通过以上方案的实施,本发明的有益效果为:
1、本发明使车辆可以避开障碍物,在安全区域行驶。
2、本发明在避障安全约束选取时考虑了车辆的形状。
3、本发明考虑了驾驶员意图和驾驶环境危险程度。
具体实施方式
以下结合附图对本发明进行详细说明:
本发明是一种基于危险程度变权重的人机协同转向控制方法,如图1所示,具体实施步骤如下:
步骤一、建立综合的车辆动力学和运动学的简化模型
(1)车辆动力学模型建立
车辆动力学模型如图2所示,其中车辆质心o为坐标原点,在这里只考虑车辆的侧向运动动力学和横摆运动运动学,忽略车辆的纵向动力学。于是我们可以得到一个简化的二自由度车辆模型。车身前进方向为x轴正方向,垂直于x轴向上的为y轴正方向。根据动力学知识可得二自由度动力学方程如式5所示。
其中,β为质心侧偏角,单位,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)车辆运动学建模
车辆动力学方程示意图如图2所示,假设车辆为一个刚体,由于车辆在行驶过程中感知到的道路曲率较小,并且航向角ψ和质心侧偏角β也都在较小范围内变化,所以我们可以得到简化后的车辆运动学方程如式(6)所示:
式中,β为质心侧偏角,单位,rad;xo为车辆质心o的纵向位置,单位,m;yo为车辆质心o的侧向位置,单位,m;r为车辆的横摆角速度,单位,rad/s;ψ为车辆航向角,单位,rad;
(3)建立车辆模型
我们假设车辆的纵向速度v保持不变,结合式(5)和式(6)可以得到车辆动力学和运动学的微分方程如式(7)所示:
我们选取[yoψβr]作为***状态变量,选取前轮转角δf作为***控制输入。于是我们可以得到***状态方程如式(8)所示:
其中:
x=[yo ψ β r]T,u=δf.
其中,x为***的状态向量;u为***控制量;A为***矩阵;B为输入矩阵;
步骤二、确定车辆安全行驶道路边界:
如图3所示为二维车路关系模型,此图中忽略车身宽度,把车辆考虑成一个刚性杆,其中F为车辆前端点,R为车辆后端点,o为车辆质心。同时,为了保证简化合理性,我们同样把上下道路边界各缩短二分之一车长。于是我们可以得到简化后的道路边界如式(9)所示:
其中fl'(x)和fr'(x)为简化后的道路边界。fl(x)和fr(x)为原来的道路边界,w为车身宽度。
我们只要保证车辆的前后端点F和R在道路边界内就可以实现防撞。图4给出了刚性杆端点和质心之间的关系,同样我们假设车辆在行驶过程中航向角ψ和质心侧偏角β也都在较小范围内变化。我们可以得到最终的道路边界表达式如式(10)所示:
步骤三、确定自动驾驶权重系数
自动驾驶权重系数Γ是通过考虑道路和驾驶员综合情况而得出的。
(1)驾驶环境危险指数表达式如式(11)所示:
其中,Yvehicle表示车辆质心当前坐标;Ycenter表示道路中心线坐标;EA是一个大于0的确定系数。这样将车辆的位置转化为安全程度的具体表达。驾驶环境危险指数越大代表车辆距离道路边界越近,危险程度越高。
(2)驾驶员操作危险指数表达式如式(12)所示:
其中,δhuman表示驾驶员期望的前轮转角,δprediction表示***预测的前轮转角,它是上一时刻预测控制中预测得到的,EB是调节系数。该指数表征了驾驶员实际操作符合***预期操作的程度。
(3)自动驾驶权重系数Γ关于驾驶环境危险指数和驾驶员操作危险指数的三维map:利用驾驶环境危险指数Eroad和驾驶员操作危险指数Edriver,采用模糊化方法,获得自动驾驶权重系数Γ关于Eroad和Edriver的三维map。
首先将驾驶环境危险指数Eroad和驾驶员操作危险指数Edriver分别模糊化为5个集合:S(安全),MS(较安全),M(中),MD(较危险),D(危险)。如图4、图5所示,利用调节参数EA将Eroad变化范围设定为[0,1],其隶属度函数的表达式如式(13)所示;利用调节参数EB将Edriver的变化范围设为[0,1],其隶属度函数的表达式如式(14)所示:
同样将自动驾驶权重系数Γ也模糊化为5个集合:S(小),MS(较小),M(中),MB(较大),B(大)。如图6所示,其对应的隶属度函数表达式如式(15)所示:
其中Aij、Bij、Cij(i=1,2,3;j=1,2,3,4,5)为常数。
然后进行模糊规则的建立。具体模糊规则如表1所示:
表1模糊规则
最后,通过模糊规则与上述隶属度函数确定自动驾驶权重系数Γ关于Eroad和Edriver的三维map,如图7所示;
(4)自动驾驶权重系数Γ的实时确定
最后实时确定驾驶环境危险指数和驾驶员操作危险指数,利用上述的三维map得到自动驾驶权重系数Γ;
步骤四、利用步骤三得到的自动驾驶权重系数,采用MPC(Model PredictiveControl,模型预测控制)方法进行基于危险程度变权重的人机协同转向控制器设计
(1)本发明的控制目标如下:
1)使***控制尽可能跟随驾驶员意图,同时控制量保持平顺。
2)行驶过程中保证车辆在道路边界线内。
(2)基于危险程度变权重的人机协同转向控制器设计
本发明做出如下假设:假设自主驾驶车辆在一个预测时域内保持恒速行驶。式(8)为车辆***的连续模型,为用于基于模型预测控制的区域型路径跟踪控制算法的设计,需要将式(8)离散化,得到离散时间的车辆***模型,如式(13)所示:
x(k+1)=Acx(k)+Bcu(k) (16)
式中,其中Ts为采样时间。
假定预测时域为P,控制时域为N,且满足N≤P。同时假定控制时域之外的控制量保持不变,即u(k+N)=u(k+N+1)=…=u(k+P-1),可推导出P步的状态预测方程,如式(17):
定义:
于是我们可以得到驾驶员目标函数如式(18)所示:
其中,δh为驾驶员期望的前轮转角,单位,rad。u(k)为控制输入,Δu(k+i)=u(k+i)-u(k+i-1)为前轮转角在采样区间的增量。Γd和Γh分别为控制量增量的权重系数和驾驶员目标权重系数。
于是我们可以得到控制器目标函数如式(19)所示:
其中yc(k+i)为道路中心线的侧向位置,作为车辆侧向位置的期望y(k+i)为车辆实际侧向位置,β(k+i)=Cβx(k+i),Cβ=[0 0 1 0]。Γy和Γβ分别为道路中心线跟随权重系数和质心侧偏角权重系数。
再根据图7中道路危险参数和驾驶员危险参数所确定的自动驾驶权重系数相结合,我们得到最终的控制目标函数如式(20)所示:
J=JH+ΓJA (20)
其中,JH为驾驶员目标函数,JA为自动控制器目标函数,Γ为自动驾驶权重系数。
车辆质心的侧向位置满足式(10)中的约束,该输出约束可被写成如式(21)所示的形式:
式中,ψ(k+i)=Cψx(k+i),Cψ=[0 1 0 0],fl(k+i)为前方可行道路区域左边界线fl(x)在时刻k+i的采样值,单位,m;fr(k+i)则为前方可行道路区域右边界线fr(x)在时刻k+i的采样值,单位,m。
采用约束模型预测方法进行共享转向控制器设计,整理为;
满足:x(k+i+1)=Acx(k+i)+Bcu(k+i)
式中:
Cψ=[0 1 0 0].
其中,J为优化函数的目标函数;δh为驾驶员期望的前轮转角,单位,rad;lf为车辆质心o到车辆前端点F的距离,单位,m;lr为车辆质心o到车辆后端点R的距离,单位,m;u(k+i)为k+i时刻的***控制量,即为车辆的前轮转向角,单位,rad;x(k+i)为k+i时刻的***状态向量;y(k+i)为k+i时刻的***输出量;P为预测时域,N为控制时域;Γd和Γh分别为控制量增量的权重系数和驾驶员目标权重系数;Γy和Γβ分别为道路中心线跟随权重系数和质心侧偏角权重系数;Γ为自动驾驶权重系数;fl(k+i)为前方可行道路区域左边界线fl(x)在时刻k+i的采样值,单位,m;fr(k+i)则为前方可行道路区域右边界线fr(x)在时刻k+i的采样值,单位,m;Ts为采样时间,单位s;x为***的状态向量;A为***矩阵;B为输入矩阵。
步骤五、选取控制量并完成控制,选取控制率u为:
u=U*(1) (23)
其中,U*为优化得到的最优控制序列;
即选取最优控制序列的第一个量作为控制量作用到被控车辆上。到下一时刻,基于模型预测控制的共享转向控制器将根据当前车辆状态重新计算一个最优控制量,以此往复,即实现了滚动优化控制。