改进的LQR横向路径跟踪控制器

2024-01-22马思群王兆强韩博赵佳伟

机械科学与技术 2024年1期

马思群,王兆强,韩博,赵佳伟

(上海工程技术大学机械与汽车工程学院,上海 201620)

随着谷歌、百度、华为、特斯拉等公司的纷纷入局,自动驾驶在近些年迎来了蓬勃发展的时期[1]。自动驾驶车辆以其舒适、安全等优点在全世界范围内收到了广泛的关注[2]。一般来说,自动驾驶系统由环境感知、决策、路径规划和路径跟踪这4部分组成[3]。在这些技术中,路径跟踪控制是实现智能驾驶的核心,是保障车辆行驶安全、操纵稳定性的重要保障[4-5]。由于其直接影响到汽车的安全性和用户体验,所以关于路径跟踪的研究具有很强的工程应用价值和理论研究意义。

路径跟踪是指通过控制车辆的转向以及速度来使其行驶在规划路径上,最小化车辆与参考路径之间的横向距离和航向误差是路径跟踪的主要目的[6]。在过去的半个多世纪里,研究者设计了各式各样的路径跟踪控制器。例如基于车辆运动学模型的纯跟踪法和斯坦利法[7],虽然运动学控制器设计简单,但因为考虑到车辆的动力学因素,所以其在高速情况下的跟踪效果较差。因此目前大家主要的研究方向是基于车辆动力学的路径跟踪控制。这其中有经典的PID控制[8]、线性二次型调节器(LQR)[9]、模糊逻辑控制[10]、滑模控制(SMC)[11]和模型预测控制(MPC)[12]。

LQR控制是最优控制中的一种,在许多领域都有应用[13]。在2009年,斯坦福大学的Snider提出将LQR控制方法应用于车辆的路径跟踪控制,并建立了包含有路面曲率扰动的动力学模型[14]。虽然该控制方法能使系统趋于稳定,但没有考虑到干扰项的影响,跟踪器存在稳态误差。Kapani和Gerdes提出了前馈+反馈的LQR转向控制器[15],该设计在Snider的基础上设计了前馈控制器,结果表明跟踪误差显著的减小。Xu和Peng提出在LQR反馈控制的基础上增加一个包含道路曲率信息的前馈控制部分,以消除控制器的稳态误差[16]。针对LQR的权重系数,高琳琳等提出一种根据路径误差实时调整LQR控制器权重系数的方法,提高了跟踪精度[17],但是其没有具体的分析每个权重系数对跟踪的影响。基于LQR控制方法的研究已经在智能汽车上得到了广泛的应用,但仍需要提高控制器的稳定性。

以现代智能算法为基础的控制方法,主要包括模糊控制、神经网络和遗传算法等。将智能控制算法应用于路径跟跟踪问题是未来的趋势[18]。Onieva等提出了一种自适应模糊控制器,它可以通过迭代遗传算法实时调整模糊控制器的隶属度函数和规则库[19]。Yuan等提出一种根据车辆的横向速度和纵向速度调整预瞄距离的路径跟踪控制器,调整方法是根据驾驶者经验设计的模糊控制器[20]。尽管基于智能控制方法的路径跟踪性能得到了很大的优化,但模糊控制规则的制定仍然是一个棘手的问题。

上文总结了自主车辆路径跟踪控制算法的一些研究成果,并提出了这些控制算法面临的一些问题。虽然LQR控制器可以在路径跟踪时拥有着较高的跟踪精度。但是在车辆远离期望路径或者行驶在极端路况时,由于代价函数的权重固定所引起的驾驶舒适性问题研究较少。为此我们设计了一种基于汽车误差动力学模型的前馈+反馈的离散LQR控制器。并设计了根据跟踪误差实时调整的模糊控制器来控制LQR的权重系数。随后考虑到实时调整将导致控制器计算量太大。于是针对LQR控制器在跟踪精度和计算量之间的平衡问题进行了研究,设计了基于余弦相似度的更新规则。通过在Simulink-Carsim平台下的仿真测试,将改进的LQR控制器与传统LQR控制器进行对比仿真。结果表明:改进的LQR控制器在降低跟踪误差和减少计算负荷方面得到了改善。

1 数学模型

1.1 车辆动力学模型

由于车辆是一个复杂的非线性系统,仅仅使用车辆的运动学模型很难很好的来表述车辆的运动特性。所以本文采用的是经过简化后的二自由度动力学模型。汽车在高速行驶时,轮胎会变形产生侧偏角。当侧偏角较小时,轮胎的侧向力与侧偏角成正比,所以有Fy=Cα。我们假设车是对称的,同时忽略掉车轮的间距,将两车轮合并为一个。其动力学模型如图1所示。

图1 二自由度动力学模型Fig. 1 Dynamics model of two degrees of freedom

图1中:αf与αr为前后轮的侧偏角大小;Fyf和Fyr为前后轮的侧向力;δ为前轮转角;a与b分别为前后车桥到质心处的距离;vf与vr分别为前后轮的速度。由于侧偏角的存在,所以看到车轮的速度方向不与车轮的方向相同。v为质心处的速度,一般情况下不与车身坐标系的x轴重合。

根据图1动力学模型,对汽车坐标系y轴方向进行动量定理分析及对质心处的动量矩定理分析可得:

(1)

考虑到高速情况下前轮转向角δ一般较小,所以我们视cosδ≈1,将式(1)简化为:

(2)

借鉴刚体运动学知识,可以得到前后轮侧偏角的表达式为:

(3)

式中:β为质心侧偏角;vx和vy分别为汽车的纵向速度和横向速度。

(4)

1.2 车辆动力学误差模型

为了实现汽车的横向控制,使用frenet自然坐标系将汽车的动力学模型转换动力学误差模型。在横向跟踪中,定义横向误差为当前汽车所在位置与其在规划路径上的投影点的距离,航向误差则为当前位置的航向角与规划路径上投影点的切线方向的偏差,图2为横向误差和航向误差的定义。

图2 当前位置和期望路径之间的误差示意图Fig. 2 Demonstrated picture for the error between current position and expected path

根据图2可以得到横向误差ed的表达式为

ed=(r-rref)·nref

(5)

将式(5)对时间求导,可以得到横向误差导数的表达式为

(6)

(7)

因为在设计路径时通常将参考的航向角设计的相对顺滑,所以忽略掉θr的高阶导数。此外,因为设计的是横向运动控制器,所以将vx视为一个常数。可以得到动力学模型到动力学误差模型的变换方程为:

(8)

(9)

式中:

2 横向路径跟踪控制器设计

为了使车辆可以较好的跟踪在参考路径上,基于车辆的误差模型,设计了离散LQR控制器对车辆进行横向控制,实现了横向距离误差和航向误差的快速收敛。首先,根据车辆的动力学误差模型设计离散LQR控制器,得到反馈系数。然后根据环境感知系统得到当前车辆的位置,从而计算车辆与规划路径间的状态误差。再通过反馈系数和前馈部分来补偿忽略掉的参考航向角变化率部分。最后,将离散LQR控制器计算出来的反馈系数和环境误差以及前馈部分输入到车辆控制器中去控制车辆的前轮转角,从而实现车辆的横向控制。

(10)

式中:Q和R为对角矩阵,其中参数表示每个变量的权重系数,系数越大,表示对该变量的重视程度,其求解时就越快收敛。Q=diag[q1,q2,q3,q4],q1,q2,q3,q4分别为横向误差、横向误差变化率、航向误差和航向误差变化率的重要性。R=[r*],r*为对输入前轮转角的重视程度。

2.1 反馈控制

(11)

式中dt为采样周期。

根据积分的中值定理,得:

(12)

一般来说采用中点欧拉法系统的准确性较高,但是由于输入只知道当前时刻的输入,所以采用向前欧拉法和中点欧拉法的混合使用,令E(ξ)=[E(t)+E(t+dt)]/2,u(ξ)=u(t),代入到式(12),化简整理后得到离散化的方程为

(13)

由于式(10)中的无穷不好处理,所以将其变换,转换为:

(14)

使用拉格朗日乘数法构造在有约束情况下的代价函数为

(15)

通过构造哈密顿方程来化简方程,哈密顿方程为

(16)

将式(16)代入到式(15)中,对其进行化简可得

(17)

为了求代价函数的最小值,利用向量导数的知识。求式(17)的极值,对其进行求导,并使其等于0,可得:

(18)

根据式(18),可以得到

(19)

式(19)和λn=2QEn拥有相同的格式。从n开始递推,使用同样的方法可以得到λn-1,λn-2,直到λ1的表达式。如果定义λk=2PkEk,并且Pk=Q,根据式(19),可以得到Pk-1的表达式为

(20)

式(20)也被称为Riccati方程,该方程的特性是当P的值迭代很多次后,P的值将收敛,可以认定为一个定值。通过求解Riccati方程可以得到一个收敛的P值,则代入到式(18)可以得到

(21)

将式(13)代入式(21)可以得到

(22)

可以将式(22)简写为uk=-kEk,其中,k为LQR控制器的反馈系数,k=[k1,k2,k3,k4]。

2.2 前馈控制

将式(22)代入到式(9),可以得到

(23)

u=-kE+δf

(24)

(25)

将式(9)中A,B,C代入式(25)中,可以得到系统稳态误差的方程为

(26)

根据式(5),可以得到rref+edVnref=r,对其两边求导,并代入式(7)和弗莱纳公式,可以得到

(27)

(28)

(29)

此外,因为假设车辆是一个刚体,根据等效替换方法Fyr≈(a/a+b)may,则式(29)可以简化为

(30)

二自由度模型示意图如图3所示。图3中:O为车辆的转向半径。后轮的转弯半径和质心的转弯半径近似相等,可以近似为一个等腰三角形。又因为角γ通常是一个较小的值,所以其可以表示为γ≈b/R。

图3 二自由度模型示意图Fig. 3 Model of two degrees of freedom

根据三角形的内角和,可以得到质心侧偏角为

(31)

根据式(30)和式(31),可得eφ=-β,所以航向误差的稳态值就不需要前馈部分消除。综上所述,得到前馈部分为

(32)

3 LQR控制器的实时更新方法

LQR路径跟踪控制器的性能受到权重系数的选择影响,但通常对权重系数选择是依靠经验来决定的。为了更好的观察各权重系数对路径跟踪控制效果的影响,使用Simulink-Carsim平台进行仿真验证,选择双移线路况作为测试路径。针对q1,q2,q3,q4和r这5个权重系数,单独改变各权重系数的数值,并将其他4个权重系数设置为1,测试不同的权重系数对跟踪效果的影响。LQR控制器在不同权重系数下的结果如图4和图5所示。结果表明:权重系数q1和q4的改变对车辆的跟踪性能影响较大,当权重系数改的过大或过小并且跟踪出现较大误差时,车辆不能很好的快速消除误差并且稳定的维持在参考路径上。但是权重系数q2,q3和r对跟踪控制效果的影响则很小。

图4 不同权重系数q1,q2,q3,q4下路径跟踪的结果Fig. 4 The results of path tracking in different weight coefficients q1,q2,q3,q4

图5 不同权重系数r下路径跟踪的结果Fig. 5 The result of path tracking in different weight coefficient r

由于是在计算机上的仿真环境下进行仿真,所以可以在每个计算周期内都根据新的状态矩阵重新进行离散LQR控制器的求解。虽然这样提高了跟踪精度,但是会增大计算机的计算量,其跟踪结果只具有理论参考价值,在实际应用时很难实现。所以为了提高LQR控制器的稳定性以及实时性,本文提出了一种改进的LQR路径跟踪策略。具体的实现过程如图6所示,控制策略主要分为两部分:首先,控制器模块使用离散LQR算法进行转向输入角的计算。然后模糊控制器根据车辆位置来更新控制器的权重系数。其次,根据最新的状态矩阵和权重系数矩阵,更新模块计算其与控制器中的状态矩阵和权重矩阵之间的余弦相似度。若余弦相似度低于设定好的更新阈值,则将最新的状态矩阵和权重系数矩阵替换掉控制模块中的状态矩阵和权重系数矩阵,并重新进行离散LQR控制的计算过程,得到新的反馈系数。若余弦相似度大于更新阈值,则不重新计算,使用之前的反馈系数k。

图6 基于模糊控制和余弦相似度的LQR横向路径跟踪控制器框架Fig. 6 LQR lateral path tracking controller based on the fuzzy control and cosine similarity degree

3.1 基于模糊控制的权重系数的更新

随着模糊控制理论的发展,人们提出了模糊控制算法。它的突出特点是不需要对控制系统进行精确建模,对过程参数变化具有很强的适应性。由于模糊控制采纳了人们的经验理论,可以简化系统的复杂性,是一种易于控制、鲁棒性强的控制方法。将模糊控制与LQR控制器相结合,结合驾驶者的经验设计模糊控制器规则,使用模糊控制实时调整LQR控制器的权重系数。这样可以充分利用LQR控制器和模糊控制器的优点,有效地提高控制器的抗干扰性和鲁棒性。图7为模糊控制的过程。

图7 模糊控制框架Fig. 7 Fuzzy control

根据图4可知:q1和q4对跟踪性能影响较大,所以主要对q1和q4进行模糊控制的调整。根据人们的驾驶经验,横向误差与航向误差并不是所有时候都是越小越好。所以需要根据不同的情况,有时拥有一定的误差反而是好的结果。当横向误差较大时,需要有存在较大的航向误差来消除横向误差,反之当横向误差较小时,应该主要控制航向误差以保证车辆行驶的稳定性。

所以q1和q4需要满足以下需求:

1) 当横向误差较大,航向误差不处于消除横向误差的方向时,q1需要取一个较大的值,q4需要取一个较小的值,如图8a)所示。

2) 当横向误差较大,航向误差处于可以消除横向误差的方向时,q1需要取一个较小的值,q4需要取一个较大的值,如图8b)所示。

3) 当横向误差较小,航向误差较大时,q1需要取一个较大的值,q4需要取一个较小的值,如图8c)所示。

4) 当横向误差较小,航向误差也较小时,q1需要取一个较小的值,q4需要取一个较大的值,如图8d)所示。

图8 车辆在不同横向误差和偏航误差下的位置示意图Fig. 8 The positions of the vehicle under different lateral error and yaw error

因此,q1和q4的数值需要由检测到的横向误差和航向误差来确定。在此基础上,使用控制因子来调整q1和q4的数值。q1和q4被描述为控制因子,横向误差和航向误差的函数分别为:

(33)

式中:τ和σ分别为q1和q4的控制因子。通过式(34)将输入ed和eφ标准化处理在[-2,2]的合适范围内。

(34)

式中:Range(·)为max(·)-min(·)。使用图9中的隶属度函数记性模糊化和逆模糊化处理。隶属度函数由高斯和三角形隶属度函数组成。

图9 ed,eφ,τ和σ的隶属度函数Fig. 9 Membership functions for ed,eφ,τ,σ

根据前面对各种车况下的所需权重系数的分析,在表1和表2中设计了具体的模糊控制规则。然后根据经典的重心法对推理出的结果进行逆模糊运算。

表1 τ的模糊规则Tab. 1 Fuzzy rule of τ

表2 σ的模糊规则Tab. 2 Fuzzy rule of σ

图10为控制因子τ和σ在不同的横向和航向误差情况下通过模糊推理得到的值。

图10 控制因子的τ和σ的模糊推理结果Fig. 10 Fuzzy inference results of control parameters τ and σ

根据控制因子τ和σ的值可以得到q1和q4的计算公式为:

(35)

3.2 基于余弦相似度的更新规则

为了降低计算机的计算时间,使用余弦相似度的计算方法判断是否更新状态矩阵和权重系数矩阵,余弦相似度的计算公式为:

(36)

式中:A*和Q*为控制器正在使用的矩阵;A和Q为当前时刻最新的状态矩阵和权重系数矩阵;sA和sQ为当前时刻的矩阵和控制器中的矩阵之间的余弦相似度,其数值位于0～1之间,数值越接近1表示两矩阵间的差异就越小。

设定一个阈值作为更新的条件,当最新的状态矩阵与参考的状态矩阵的余弦相似度小于设定阈值,则以最新的状态矩阵去求解LQR控制器。若大于设定的阈值则不使用新的状态求解LQR控制器,依旧使用之前的反馈系数作为结果。为了更好的分析在不同的阈值下状态矩阵的更新频率。采用如图11所示正弦形式的纵向速度作为速度输入。分别设置余弦相似度的更新阈值sA,ref为0.8,0.9,0.95和不更新状态矩阵。

图11 正弦形式的纵向速度输入Fig. 11 Longitudinal velocity input in sinusoidal form

如图12所示,当系统的状态矩阵不更新时,其余弦相似度较小,且相似度的变化率较大。从这个角度讲,当实时的纵向速度与控制器默认状态矩阵之间余弦相似度较小时会导致车辆行驶不稳定。将阈值设置为0.8时,总体的余弦相似度减小,更新次数为6次,但是变化率变大,不利于系统的输入。增大更新阈值至0.9,余弦相似度与余弦相似度的变化率均减小,且更新次数没有增加,保持在6次。当进一步增大更新阈值至0.95,则更新次数增大到12次,但是余弦相似度与余弦相似度的变化率都进一步减小。若是继续增大更新阈值,则计算负荷进一步增加。综合考虑后选择状态矩阵的更新阈值为0.9,在保证实时性的同时提高了LQR控制器的实时性。

图12 在正弦输入下状态矩阵A的余弦相似度及变化率Fig. 12 Cosine similarity degree and rate of change of state matrix A under sinusoidal input

使用同样的方法分析权重系数矩阵的更新阈值设置。使用如图13所示的输入作为τ和σ的值。分别设置权重系数矩阵的更新阈值sQ,ref为0.8,0.85,0.9和不更新作为权重系数矩阵。

图13 正弦形式的σ和τ输入Fig. 13 Input of σ and τ in sinusoidal form

如图14所示,当系统的权重系数矩阵不更新时,其余弦相似度很小,不能满足车辆对各种情况的自适应性。当更新阈值设置为0.8时,总体的余弦相似度减小,更新次数为7次,但是变化率变大,不利于系统的稳定性。增大更新阈值至0.85,余弦相似度与余弦相似度的变化率均减小,处于一个合适的范围内,且更新次数保持在7次。当进一步增大更新阈值至0.9时,虽然余弦相似度与余弦相似度的变化率都进一步减小,但是更新次数增大到14次,若是继续增大更新阈值,则计算负荷进一步增加。综合考虑后选择权重系数矩阵的更新阈值为0.85。

图14 在正弦输入下权重系数矩阵Q的余弦相似度及变化率Fig. 14 Cosine similarity and rate of change of weight coefficient matrix Q under sinusoidal input

4 仿真试验分析

为了验证改进的LQR控制器的跟踪控制效果,我们使用Simulink-CarSim联合仿真平台进行仿真验证。其中Simulink软件用来构建横向路径跟踪控制器,Carsim软件用来搭建车辆模型。我们使用双移线轨迹作为参考路径来测试控制器的跟踪性能,控制器及车辆主要参数的设定如表3所示。

表3 仿真参数Tab. 3 Parameters for simulation

为了更好的分析改进的LQR控制器的路径跟踪效果,将其与实时计算更新的LQR控制器进行比较。分别在速度15 m/s和25 m/s的情况下进行测试。如图15所示,在车速15 m/s的情况下,改进的LQR控制器可以和实时更新的LQR控制器一样很好的跟踪在期望路径上。在低速的情况下,横向误差的最大值约为0.2 m,这是较小的数值。改进的LQR控制器虽然没有实时更新反馈系数,但是其跟踪效果与实时更新的LQR控制器相比偏差较小。

图15 在15 m/s速度下改进的LQR控制器和实时更新的LQR控制器的路径跟踪结果Fig. 15 Path tracking results of improved LQR controller and real-time updated LQR controller at 15 m/s speed

图16为在25 m/s的车速下改进的LQR控制器和实时更新的LQR控制器的跟踪效果。显然,当车辆处于高速行驶状态下,车辆已经很难完美的行驶在双移线工况上。在曲率最大的转弯处,两种控制算法均出现了较大的误差,但是改进的LQR控制器的横向误差更小,且当误差较大时可以更快的修正当前状态。改进的LQR控制器相比较于实时更新的LQR控制器,其最大横向误差降低了约28%。这表明当车辆的跟踪误差较大时,改进的LQR控制器可以更快速的消除误差,使车辆行驶在一个安全的情况下。其原因主要是因为当横向误差较大时,模糊控制器通过调整权重系数调整对横向误差的优先级,使LQR控制器更快的消除横向误差。

图16 在25 m/s速度下改进的LQR控制器和实时更新的LQR控制器的路径跟踪结果Fig. 16 Path tracking results of improved LQR controller and real-time updated LQR controller at 25 m/s speed

为了评估改进的LQR控制器的实时性,将其与实时更新的LQR控制器的计算时间进行比较。使用MATLAB中的τ和σ函数记录下在25 m/s的速度下,两个控制器的时间,其结果如图17所示。由图17可知:改进的LQR控制器的计算时间大大缩减,相比于实时更新的LQR控制器计算时间缩减了92%。改进的LQR控制器在某些时刻计算时间突然增加,原因是在这些时刻余弦相似度低于更新阈值,导致控制器重新进行计算求解。但整体观察下来,由于引入了余弦相似度的计算,在大多数时刻改进的LQR控制器的计算时间大大降低,达到了期望的目的。