基于模糊算法改进自动驾驶跟踪控制系统

孙柯阳,王兆强,梁威,刘斌山,孙崇智

（1.上海工程技术大学机械与汽车工程学院,上海 201620；2.甘肃省特种设备检验检测研究院,甘肃 兰州 730050）

自动驾驶汽车因具有可靠性高、行驶速度高及通行效率高等特点,而受到政府与汽车相关产业的广泛关注[1].自动驾驶系统主要有车辆定位与环境感知、决策规划与跟踪控制三大模块[2].跟踪控制模块作为自动驾驶核心模块之一,保证了车辆在不同环境和车况下,可以准确、稳定及鲁棒地跟踪参考路径[3].其中路径跟踪模块在保证行车安全与操作稳定性方面占有主导地位, 因此对于路径跟踪控制器的研究有着实际的应用价值.

在真实的环境中,自动驾驶车辆不会完全按照规定的路线行驶,实际行驶路径和规划路径之间存在误差,控制器的作用就是尽可能地降低此类误差.现有的路径跟踪控制算法有：斯坦福大学开发的stanly控制算法、纯跟踪控制算法（PP）、PID 控制算法、线性二次调节器（LQR）、滑模控制算法（SMC）及模型预测控制算法（MPC）等,其中线性二次规划算法与PID 控制算法由于其良好的鲁棒性和精确性被广泛地应用于自动驾驶跟踪控制系统中.本文设计了一种基于模糊线性二次规划算法和PID 控制算法的联合跟踪控制器,运用Carsim/simulink 进行联合仿真,验证了控制器的有效性.

1 动力学误差模型

1.1 动力学模型

车辆是由发动机、车身及底盘所组成的复杂的非线性系统,用忽略了力学影响的车辆运动学模型难以精确描述车辆的运动特性,因此本文搭建了车辆动力学模型,且根据阿克曼转向几何原理将四轮模型简化为二自由度的自行车模型.简化后的动力学模型如图1 所示.

图1 车辆动力学模型Fig.1 Vehicle dynamics model

图1 中a 和b 分别为前后轴到车辆质心的距离,Vf和Vr是前后车轮的速度.由于前后轮所受侧向力Fyf 和Fyr 的影响,前后轮的速度方向相对于轮胎指向发生了一定的偏转,因此产生的偏转角αf和αr即为前后轮的侧偏角,δ 是车辆的前轮转角,V 是车辆质心处的速度.对车辆整体做受力分析有

式（1）（2）中：m 为整车质量,Iz为汽车绕大地坐标系Z 轴的转动惯量,ay为车辆在自身坐标系y 轴方向上的加速度,φ 为汽车的横摆角,为汽车的角加速度,为了防止高速行驶时转向过度导致车辆侧翻,前轮转角δ 一般较小,因此式（1）和（2）可以化简为

考虑到前轮转角δ 和轮胎侧偏角α 较小,根据图1 所示关系,可以得到如下公式

式（7）（8）中：Vx和Vy分别为车辆的纵向与横向速度.与此同时,车辆沿自身坐标系y 轴方向的侧向加速度ay为

结合公式（3）（4）（7）（8）和（9）,可以得到一个关于Vy和的状态空间表达式

1.2 横向和纵向误差模型

自动驾驶车辆在跟踪规划好的轨迹时,由于道路因素及自身信号传递的延迟,导致汽车不能很好地沿着预定轨迹行驶,此时车辆产生的横向位置误差和航向误差就构成了横向误差,因此需要设计一个横向控制器,降低横向误差给自动驾驶车辆带来的影响,使得车辆能够精确地沿着期望轨迹行驶.横向误差的定义如图2 所示.

图2 横向误差示意图Fig.2 Schematic diagram of lateral error

为了得到横向误差模型,还需对ed进行求导

运用Frenet-Serret 公式对公式（12）进行化简

式（13）中：θ=φ+β,同时,定义eφ=φ-θr,且假设其为小量,对公式（13）展开化简可得

根据eφ=φ-θr与公式（14）,可以推出以下几个关系式

为了方便描述,公式（16）可以简写成

车辆在跟踪期望轨迹时产生的纵向误差主要由车辆当前位置与目标点之间的纵向位置误差和当前车速与规划的目标点之间的速度误差.纵向误差如图3 所示.

图3 纵向误差示意图Fig.3 Schematic diagram of longitudinal error

图3 中es是车辆与目标点之间的纵向位置误差,可由以下公式求出

对于速度误差eV=V−,由于,结合公式（12）可得到式

式（19）中：Kr为规划轨迹的曲率.

2 横向和纵向控制器设计

2.1 基于模糊线性二次规划的横向控制

基于第二节建立的横向误差模型, 通过线性二次规划理论和模糊控制算法设计了一种模糊横向跟踪控制器,整个横向控制系统的设计原理是：结合轨迹信息和车辆的航向信息和横向位置信息,计算出两者之间的误差,将误差输入到模糊控制模块得到控制因子r,利用r 值求得合适的权重系数并通过设计好的线性二次规划器求得前轮转角,将计算结果输入给被控车辆,达到对车辆进行横向跟踪控制的目的.

在本文研究的横向控制系统中,目标是算出最优的输入,即前轮转角,使车辆与目标轨迹之间的横向位置误差和航向误差快速减少直至趋近于零.根据公式（17）车辆横向误差模型,对车辆的横向跟踪控制可以转化为求解LQR 问题,即

式（20）中：Q 为对角矩阵,Q=[q1,q2,q3,q4],其中q1、q2、q3、q4分别对应于公式（16）中误差变量ed、、eϕ、的权重值.变量的权重值越大,表示横向控制器在求解时更侧重让此变量快速收敛.而R 是表示前轮转角在横向控制系统的影响程度.为了优化计算方式,提升计算效率,控制器处理的都是规划好的一系列离散的轨迹点,因此需要将线性二次型控制器离散化,根据公式（20）可得

式（21）中：ζ∈（t,t+dt）.在求解E（ζ）和u（ζ）的过程中,采用向前欧拉法求解u（ζ）、E（ζ）,采用中点欧拉法进行求解,运用混合欧拉法可以得到如下公式

I 表示单位矩阵,式（22）是离散化后的车辆横向误差公式.根据式（20）（22）,可以构造一个带有约束条件的代价函数

式（23）中：n→∞,再通过构造Hamilton 方程,进一步简化公式（23）

求公式（24）中代价函数J 的极值,可以得到如下方程组

根据式（25）,可求得Riccati 方程,对此方程求解后,即可求得横向控制系统的输入

式（26）中：Pk+1为Riccati 方程收敛后的定值.将公式（26）简写为：

式（27）中：k=[k1,k2,k3,k4]为线性二次型控制器求解所得的系数矩阵,用于前轮转角和消除稳态误差的计算.将公式（27）代入横向误差状态空间表达式（17）中,可以得到

令误差变化率E˙为0,则可求得误差E

至此,可以推出系统的稳态误差为

首先,考虑消除横向位置误差的稳态误差,即令公式（31）右侧的第一行方程式等于0,求得

对于航向误差的稳态误差,根据式（25）右侧的第三行方程式,可以求得eφ=-β.当航向误差θ-θr=0 时,φ+β-θr=0,即eφ+β=0,所以系统的航向误差的稳态误差,不需要通过前馈控制模块消除.

在上述线性二次规划控制器的设计过程中,研究发现目标函数的权重矩阵直接影响最优控制的性能,因此有许多关于如何选择这些矩阵的研究已经被讨论.由于智能化技术的发展,各类优化算法被应用于权重矩阵的选择.如蚁群优化算法[5]、遗传算法[6]及粒子群算法[7]用于解决此类问题.为了更好地研究权重矩阵中的各个权重系数对控制效果的影响,本文在一定范围内设置不同的权重系数进行仿真测试.如图4 所示.

从图4 中的a、b、c、d 四图中可以看出,当权重系数范围在[1,1 000]内时q1、q3和q4对于提高控制精度有着较大的影响,其中q1提升效果最为明显.而从图4 中的e、f、g、h 四图可以看出权重系数范围在[0.001,1]内时,q2、q3和q4对于提高控制精度有着较大的影响.综合考虑各个权重系数对车辆控制精度、操作稳定性及行驶平顺性的影响,本文主要研究权重系数q1和q3.而对于权重系数R 的选择,由图5可知,随着R 值的增大,车辆的实际路径与规划路径之间的轨迹越接近,因此取R 值为100.

模糊逻辑控制（FLC）是一种基于人类经验模拟人工行为的智能控制方法,其过程有三步：模糊化、模糊推理和去模糊化[8].首先,根据制定的隶属度函数,将系统的输入值模糊化.然后,通过人为制定的模糊规则进行去模糊化操作,得到一个实际的输出值.据此,本文将横向误差ed和航向误差eφ作为输入并模糊化为五个模糊集：负大（NB）、负小（NS）、零（ZO）、正小（PS）和正大（PB）.然后输出五个模糊集：负大（NB）、负小（NS）、零（ZO）、正小（PS）和正大（PB）.最后去模糊化输出得到red和reφ,并利用q=10r改变权重系数q.

制定的模糊控制输入输出的隶属度函数如图6 所示.

模糊规则库是由人的经验制定的,在制定规则库时,要考虑控制精度和车辆操作平顺性.本文制定的规则库如表1 和表2 所示.

red eφ ed NB NS ZO PS PB NB PB PB PS NB NS NS PB PS ZO NS ZO ZO PS PS NS PS PB PS ZO NS ZO PB PB PB NS NB PS PB PB

图7 表示模糊控制最后一步,根据模糊规则去模糊化后得到的red和reφ的实际数值.

2.2 纵向控制器设计

纵向控制是一个上下层系统结合的一个非线性系统,因此难以用线性系统准确描述系统实际运行的状态[9].可以采用智能控制理论准确有效地控制车辆.在燃油车的控制过程中涉及发动机油门角的控制和变速器速比的选择,为了减少控制的复杂度,我们选取电动汽车作为研究对象.电动车的驱动电机为永磁同步电机,最大功率Pmax=175 kW,最大扭矩Mmax=430 N·m.以油门信号thr 和电机转速n 为索引的电机的转速扭矩特性如图8 所示.

将选取的电机参数代入仿真软件,需要分别做0～50 m/s 的加速实验以及50～0 m/s 的减速实验,通过仿真得到油门与制动查找表.拟合的查找表如图9 所示.

通过查找表系统可以控制车辆的油门刹车,达到对车辆纵向控制的目的.相较于其他控制方式,传统PID 精确性更好[10],因此采用双闭环PID 的控制方式.整个控制流程如图10 所示.

图10 纵向控制流程Fig.10 Vertical control process

PID 控制器的设计如下

式（34）中：C 为控制器输出,e 为误差量,kp、ki和kd分别为比例项,积分项和微分项的权重系数.车辆通过外环计算出纵向位置误差es输入到第一个PID 控制器,得到一个速度补偿量Δv,结合内环系统可以得到车辆的纵向速度误差ev,将ev输入到第二个控制器,可以得到加速度补偿量Δa,联立期望加速度,即可求得车辆所需的加速度.将加速度和速度输入到查找表,就能达到控制车辆加速或刹车的目的.

将上述横向控制器的车轮转角信号和纵向控制器中的油门刹车信号输入到被控车辆, 即可实现对车的横纵向耦合控制.

3 实验验证与分析

Simulink/Carsim 进行联合仿真验证改进后的线性二次型控制器的控制效果,整个控制流程如图11所示.

为了更清楚地分析改进后的横纵向耦合控制器的效果,本节设置了三种测试场景：一种是横向控制,二是横纵向联合控制,三是采用改进横纵向联合控制,在速度为60 km/h 的条件下,以双移线作为参考路径,进行对比分析,仿真结果如图12 所示.

从图12-a 中可以看出在纯横向控制的条件下,高速行驶时,车辆的跟踪误差较大,最大值约为0.5 m左右,而加入了纵向控制后,车辆控制效果得到了改善,跟踪误差下降到了0.4 m 左右,跟踪精度提升了20%,最后通过模糊控制改进的横纵向控制的跟踪效果具有明显提升,最大值仅为0.25 m.而图12-b 中纯横向控制的条件下,车辆横摆角误差波动明显,控制器鲁棒性较低,改进后的横摆角误差明显下降,控制稳定.

4 小结

本文提出了模糊线性二次型与PID 联合控制系统,实验证明改进后的控制器可以自适应调节线性二次规划器的权重系数,使得控制器的鲁棒性和精度都有所提升.在后续的研究工作中,将设计相应的更新策略,用以提升模糊算法在控制器中应用的运算速度.