CN117549908A - 一种基于前馈lqr的车辆横向控制方法 - Google Patents

Abstract

本发明公开了一种基于前馈LQR的车辆横向控制方法，解决了现有技术的不足，包括以下步骤：步骤1，通过LQR算法得到车辆的反馈控制增益；步骤2，根据车辆位置与状态和规划轨迹点，得出车辆真实位置的横向误差；步骤3，使用自修正方法，根据反馈控制增益、车辆真实位置的横向误差、整车的参数求出车辆的前馈控制；步骤4，得出最终控制指令，对于车辆横向进行控制。

Description

一种基于前馈LQR的车辆横向控制方法

技术领域

本发明涉及车辆自动驾驶领域，尤其是指一种基于前馈LQR的车辆横向控制方法。

背景技术

如今智能化、电动化、轻量化是汽车的主流趋势，越来越多整车厂商投入到智能驾驶、自动驾驶技术中，而轨迹跟踪是其核心关键技术，目前一般使用较多的是MPC控制和PID方法控制，也有少数技术使用LQR算法做横向控制。自动驾驶中所涉及的车辆横向控制技术，一方面要控制车辆的位置，保持和路线一致，无横向便偏移，另一方面要控制车辆姿态，保持和道路方向一致，无方向误差。

PID、MPC、LQR算法都是比较成熟的技术，广泛地应用在工业生产、自动化控制中，都属于反馈型控制，这些算法的主要功能都是根据当前车辆姿态，车辆位置以及实际地图和实际道路方向，依靠方向盘控制车辆位置和姿态，实现轨迹跟踪。

现有的纯跟踪控制算法的好处是计算量简单，对路径无连续要求，但只适用于低速场景。由于该算法没有考虑到车辆模型特性和车辆速度，所以存在精度不高的问题，且在转弯时易发生内切，存在稳态误差，且在速度增大时稳态误差也增大，速度增大时，超调也增大，并不适用于中高速工况。

而前轮反馈控制算法避免了转弯时发生内切的问题，但仍然存在稳态误差，且速度越大，稳态误差越大，从而导致控制效果不佳，因此也只适用于中低速跟踪工况，且要求路径平滑。

发明内容

本发明的目的是克服现有技术中的车辆横向控制算法精度不高，稳态误差较大，导致控制效果不佳的缺点，提供一种基于前馈LQR的车辆横向控制方法。

本发明的目的是通过下述技术方案予以实现：

一种基于前馈LQR的车辆横向控制方法，包括以下步骤：

步骤1，通过LQR算法得到车辆的反馈控制增益；

步骤2，根据车辆位置与状态和规划轨迹点，得出车辆真实位置的横向误差；

步骤3，使用自修正方法，根据反馈控制增益、车辆真实位置的横向误差、整车的参数求出车辆的前馈控制；

步骤4，得出最终控制指令，对于车辆横向进行控制。

作为优选，所述的步骤1具体为：

步骤1a，通过二自由度车辆模型构建车辆状态方程：

当车辆侧偏角比较小的时候，轮胎侧偏力表示为：

式中，C_cf,C_rf代表前后轮的轮胎侧偏刚度，s_f,s_r分别为前后轮滑移率，α_f,α_r分别为前后轮的侧偏角；

考虑车辆的质心侧偏角β，横摆角和航向角/>车辆的质心速度为V_c，推导车辆在大地坐标系下的速度：

车辆的转弯半径为R,

结合上述公式，主要考虑横摆和横向速度的影响，忽略纵向车速的影响，模型将变化如下

写成状态方程为：

步骤1b，根据状态方程计算反馈控制增益：

LQR算法表达式如下：

y＝Cx+Du

式中，A代表状态转移矩阵，描述了系统状态的演化规律，B为输入矩阵，描述外部输入对系统状态的影响，C为输出矩阵，描述状态变量如何映射到输出信号，D为直接传递矩阵，描述了外部输入直接传递到输出信号的影响，Q为控制效果矩阵，R为状态效果矩阵，表示控制器对控制输入的重视程度；

设定线性反馈控制器u＝-Kx，得到输入参数和状态量的关系，因此状态方程写成：

让该系统稳定的条件是矩阵A-BK＝A_cl的特征值为负数，LQR方法通过设计代价函数J来实现求解反馈控制增益K。

作为优选，所述的步骤2具体为：

设置两个向量，车辆方向为垂向方向为/>误差模块为:/> 故/>

根据这些关系表达式，可以求出误差状态方程：

作为优选，所述的步骤3具体为：

引入前馈控制，令：

u＝-ke_rr+δ_f

前馈控制的引入是为了消除稳态误差；可以得到/>引入前馈控制之后：/>稳定后，/>可以得到/>因此通过前馈控制使得稳态误差为0。

作为优选，所述的步骤4具体为：

考虑到路径的曲率及车辆的转向不足特性，所以前轮反馈角度补偿包括：

其中R为转向半径，L为轮距，K_v为转向不足特性，a_y为横向加速度，k₃,e₃rr分别代表K矩阵中角度偏差反馈控制增益部分和角度偏差；其中

由于载荷转移、轮胎气压变化等导致前后轮的侧偏刚度变化，K_v并不会为定值，因此对参数K_v使用自修正方法，使用较为传统的动量优化法，令J为代价函数，/>为角速度度的偏差，α为修正系数，设动量ε_t+1＝με_t+J_t，则：

K_Vt+1＝K_Vt-ε_t+1

初值：一般α＝0.01,μ＝0.9；

因此最终输入的控制指令为：

本发明的有益效果是：本发明使用前馈控制的LQR横向控制算法，LQR算法的优点在于，通过与转向前馈进行有效结合，LQR能够很好的解决曲线行驶时的稳态跟踪误差。在中等速度曲线行驶时其稳态误差趋近于零，从而极大提升跟踪性能。LQR非常适用于路径平滑的高速公路及城市驾驶场景，具有较好的车辆高速控制性能。本发明的前馈控制算法，考虑了道路信息和车辆特性，消除LQR算法在追踪时产生的稳态误差。

附图说明

图1是本发明的一种流程图；

图2是二自由度车辆动力学模型示意图。

具体实施方式

下面结合附图和实施例对本发明进一步描述。

实施例：

一种基于前馈LQR的车辆横向控制方法，如图1所示，包括以下步骤：

步骤1，通过LQR算法得到车辆的反馈控制增益；

步骤2，根据车辆位置与状态和规划轨迹点，得出车辆真实位置的横向误差；

步骤3，根据反馈控制增益、车辆真实位置的横向误差、整车的参数和自修正算法求出车辆的前馈控制；

步骤4，得出最终控制指令，对于车辆横向进行控制。

所述的步骤1具体为：

步骤1a，通过二自由度车辆模型构建车辆状态方程，二自由度模型具体为：

如图2所示，使用XOY为惯性坐标系，xoy为车辆坐标系，由物理学知识可得：

式中，m为整车质量；分别表示车辆坐标系xoy下纵向车速和横向车速；/>表示横摆角速度；F_cf,F_cr分别为前轴和后轴轮胎受到的侧向合力；F_lf,F_lr分别为前轴和后轴轮胎受到的纵向合力；lf,lr分别为质心到前轴和后轴的距离；δ_f分别为前轮转角。

假设前后轮均为刚体，只沿着车轮滚动方向前进，没有侧向华东，由此可假设自行车模型的微分方程如下：

纯跟踪控制算法(Pure Pursuit)是一种典型的横向控制方法，对外界干扰鲁棒性较好，核心思想是基于车辆的后轮中心位置，在参考路径上向一个自定义的距离匹配一个预瞄点，假设车辆后轮中心可以按照一定转弯半径R行驶到预瞄点，然后根据转弯半径R，预瞄距离Ld，车辆坐标系下预瞄点的朝向角α之间的几何关系，来确定前轮转角。

R,α,l_d的关系为：

再根据汽车动力学方程：而/>结合上式得到前轮转角：

当车辆侧偏角比较小的时候，轮胎侧偏力表示为：

式中，C_cf,C_rf代表前后轮的轮胎侧偏刚度，s_f,s_r分别为前后轮滑移率，α_f,α_r分别为前后轮的侧偏角；

考虑车辆的质心侧偏角β，横摆角和航向角/>车辆的质心速度为V_c，推导车辆在大地坐标系下的速度：

车辆的转弯半径为R,

结合上述公式，主要考虑横摆和横向速度的影响，忽略纵向车速的影响，模型将变化如下

写成状态方程为：

步骤1b，根据状态方程计算反馈控制增益：

LQR算法表达式如下：

y＝Cx+Du

式中，A代表状态转移矩阵，描述了系统状态的演化规律，B为输入矩阵，描述外部输入对系统状态的影响，C为输出矩阵，描述状态变量如何映射到输出信号，D为直接传递矩阵，描述了外部输入直接传递到输出信号的影响，Q为控制效果矩阵，R为状态效果矩阵，表示控制器对控制输入的重视程度；

设定线性反馈控制器u＝-Kx，得到输入参数和状态量的关系，因此状态方程写成：

让该系统稳定的条件是矩阵A-BK＝A_cl的特征值为负数，为了求解反馈控制增益K，LQR方法通过设计代价函数J来实现：

为了选择合适的反馈控制增益K，通常再定义一个矩阵P，

P是对称矩阵，P＝P^T＞0，可以代入代价函数得到：J＝x^T(0)Px(0)，当系统稳定时，t→∞,x→0，可见代价函数只与状态矩阵P有关；

联系上述公式，继续推导可得：

x^T(A_cl ^TP+PA_cl+Q+K^TRK)x＝0

代入A-BK＝A_cl可得：

A^TP+PA+Q+K^TRK-K^TB^TP-PBK＝0

将K＝R^-1B^TP代入有：

A^TP+PA+Q-PBR^-1B^TP＝0

上式也被称为微分Riccati方程，解该方程即可求解P，进而求解K。

根据轨迹信息然后计算横向误差，该误差主要包括横向距离的误差和车身姿态的误差，既要保证车辆位置准确，也要保证车辆方向准确。

故所述的步骤2具体为：

设置两个向量，车辆方向为垂向方向为/>误差模块为:/> 故/>

根据这些关系表达式，可以求出误差状态方程：

前轮反馈控制，又称Stanly控制，主要利用前轮中心和路径之间的横向偏差量反馈到方向盘转角控制上。前轮的中心横向位置跟踪的偏差e变化率为：因此横向位置偏差变化率可以通过前轮转角进行控制，定义/>保证前轮的横向偏差e满足李雅普诺夫的指数化收敛。

故所述的步骤3具体为：

若只使用LQR算法，u＝-Ke_rr，得到无论k取何值，/>e_rr都不能同时为0。所以引入前馈控制，令：

u＝-ke_rr+δ_f

前馈控制的引入是为了消除稳态误差；可以得到/>引入前馈控制之后：/>稳定后，/>可以得到/>因此通过前馈控制使得稳态误差为0。

所述的步骤4具体为：

考虑到路径的曲率及车辆的转向不足特性，所以前轮反馈角度补偿包括：

其中R为转向半径，L为轮距，K_v为转向不足特性，a_y为横向加速度，k₃,e₃rr分别代表K矩阵中角度偏差反馈控制增益部分和角度偏差；其中由于载荷转移、轮胎气压变化等导致前后轮的侧偏刚度变化，K_v并不会为定值，因此对参数K_v使用自修正方法，使用较为传统的动量优化法，令/>J为代价函数，/>为角速度度的偏差，α为修正系数，设动量ε_t+1＝με_t+J_t，则：

K_Vt+1＝K_Vt-ε_t+1

初值：一般α＝0.01,μ＝0.9；

因此最终输入的控制指令为：

根据本申请的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本申请的实施例包括计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的计算机程序。在这样的实施例中，该计算机程序可以通过通信部分从网络上被下载和安装，和/或从可拆卸介质被安装。在该计算机程序被中央处理单元(CPU)执行时，执行本申请的系统中限定的各种功能。

需要说明的是，本申请实施例所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(Erasable Programmable Read Only Memory，EPROM)、闪存、光纤、便携式紧凑磁盘只读存储器(Compact Disc Read-Only Memory，CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本申请中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本申请中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的计算机程序。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的计算机程序可以用任何适当的介质传输，包括但不限于：无线、有线等等，或者上述的任意合适的组合。

附图中的流程图和框图，图示了按照本申请各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。其中，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

描述于本申请实施例中所涉及到的单元可以通过软件的方式实现，也可以通过硬件的方式来实现，所描述的单元也可以设置在处理器中。其中，这些单元的名称在某种情况下并不构成对该单元本身的限定。

作为另一方面，本申请还提供了计算机可读介质，该计算机可读介质可以是上述实施例中描述的电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被一个该电子设备执行时，使得该电子设备实现上述实施例中所述的方法。

应当注意，尽管在上文详细描述中提及了用于动作执行的设备的若干模块或者单元，但是这种划分并非强制性的。实际上，根据本申请的实施方式，上文描述的两个或更多模块或者单元的特征和功能可以在一个模块或者单元中具体化。反之，上文描述的一个模块或者单元的特征和功能可以进一步划分为由多个模块或者单元来具体化。

通过以上的实施方式的描述，本领域的技术人员易于理解，这里描述的示例实施方式可以通过软件实现，也可以通过软件结合必要的硬件的方式来实现。因此，根据本申请实施方式的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中或网络上，包括若干指令以使得一台计算设备(可以是个人计算机、服务器、触控终端、或者网络设备等)执行根据本申请实施方式的方法。

本领域技术人员在考虑说明书及实践这里公开的实施方式后，将容易想到本申请的其它实施方案。本申请旨在涵盖本申请的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本申请的一般性原理并包括本申请未公开的本技术领域中的公知常识或惯用技术手段。

应当理解的是，本申请并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本申请的范围仅由所附的权利要求来限制。

Claims (5)

1.一种基于前馈LQR的车辆横向控制方法，其特征是，包括以下步骤：

步骤1，通过LQR算法得到车辆的反馈控制增益；

步骤2，根据车辆位置与状态和规划轨迹点，得出车辆真实位置的横向误差；

步骤3，使用自修正方法，根据反馈控制增益、车辆真实位置的横向误差、整车的参数求出车辆的前馈控制；

步骤4，得出最终控制指令，对于车辆横向进行控制。

2.根据权利要求1所述的一种基于前馈LQR的车辆横向控制方法，其特征是，所述的步骤1具体为：

步骤1a，通过二自由度车辆模型构建车辆状态方程：

当车辆侧偏角比较小的时候，轮胎侧偏力表示为：

式中，C_cf,C_rf代表前后轮的轮胎侧偏刚度，s_f,s_r分别为前后轮滑移率，α_f,α_r分别为前后轮的侧偏角；

考虑车辆的质心侧偏角β，横摆角和航向角/>车辆的质心速度为V_c，推导车辆在大地坐标系下的速度：

车辆的转弯半径为R,

结合上述公式，主要考虑横摆和横向速度的影响，忽略纵向车速的影响，模型将变化如下

写成状态方程为：

步骤1b，根据状态方程计算反馈控制增益：

LQR算法表达式如下：

y＝Cx+Du

式中，A代表状态转移矩阵，描述了系统状态的演化规律，B为输入矩阵，描述外部输入对系统状态的影响，C为输出矩阵，描述状态变量如何映射到输出信号，D为直接传递矩阵，描述了外部输入直接传递到输出信号的影响，Q为控制效果矩阵，R为状态效果矩阵，表示控制器对控制输入的重视程度；

设定线性反馈控制器u＝-Kx，得到输入参数和状态量的关系，因此状态方程写成：

让该系统稳定的条件是矩阵A-BK＝A_cl的特征值为负数，LQR方法通过设计代价函数J来实现求解反馈控制增益K。

3.根据权利要求2所述的一种基于前馈LQR的车辆横向控制方法，其特征是，所述的步骤2具体为：

设置两个向量，车辆方向为垂向方向为/>误差模块为:/> 故/>

根据这些关系表达式，可以求出误差状态方程：

4.根据权利要求3所述的一种基于前馈LQR的车辆横向控制方法，其特征是，所述的步骤3具体为：

引入前馈控制，令：

u＝-ke_rr+δ_f

前馈控制的引入是为了消除稳态误差；可以得到/>引入前馈控制之后：/>稳定后，/>可以得到/>因此通过前馈控制使得稳态误差为0。

5.根据权利要求4所述的一种基于前馈LQR的车辆横向控制方法，其特征是，所述的步骤4具体为：

考虑到路径的曲率及车辆的转向不足特性，所以前轮反馈角度补偿包括：

其中R为转向半径，L为轮距，K_v为转向不足特性，a_y为横向加速度，k₃,e₃rr分别代表K矩阵中角度偏差反馈控制增益部分和角度偏差；其中

由于载荷转移、轮胎气压变化等导致前后轮的侧偏刚度变化，K_v并不会为定值，因此对参数K_v使用自修正方法，使用较为传统的动量优化法，令J为代价函数，/>为角速度度的偏差，α为修正系数，设动量ε_t+1＝με_t+J_t，则：

K_Vt+1＝K_Vt-ε_t+1

初值：一般α＝0.01,μ＝0.9；

因此最终输入的控制指令为：