技术领域
[0001] 本
发明属于机动车自动驾驶领域，尤其是一种自动驾驶卡车的横向控 制方法、系统及存储介质。
背景技术
[0002] 如图4所示，现有卡车结构分为
牵引车①和
挂车②两部分，其中， 车后轮轴中心③为牵引车①的质点，挂车后轮轴中心④为挂车②的质点。 通常，车满载的重量是普通轿车的20-25倍，卡车长度是普通轿车的4-5 倍，卡车结构非统一的整体(包含牵引车和挂车)，所以，在道路行驶 过程中，卡车对车辆自身的控制较轿车相比更为复杂且要求更高。
[0003] 卡车在车道内行驶，除了需要将牵引车①保持在车道内外，还需要 将挂车②保持在车道内。由于卡车空载、半载、满载重量不同，行驶速 度不同，如果在行驶过程中驾驶操作不当，如，对
方向盘转
角、转向角 速度控制不当，将会出现卡车在直线车道行驶过程中的甩挂现象，如图5 所示，还可能出现过弯行驶过程中的刮碰现象，如图6所示。
[0004] 在卡车的高级自动驾驶中，往往都难以对卡车进行精确建模，对于 不同
质量质心等等情况的变化也难以精确估计，同时在控制的过程中， 存在外部的扰动影响车辆的控制，如
风速、路面情况等等，都是外部对 于车辆控制系统的扰动，影响控制的性能，甚至使系统不稳定，造成安 全事故。传统的控制方法往往都不考虑外部扰动和系统建模不精确的影 响，利用
控制器的稳定裕度来尽可能的保证控制器的稳定，不仅降低系 统的性能指标，而且当外部扰动或者系统发生较大变化的情况下，控制 器难以保证稳定，进而造成车辆的失控。
[0005] 基于此，特提出本发明。
发明内容
[0006] 为了解决
现有技术中存在的上述问题，本发明的目的在于提供一种自 动驾驶卡车的横向控制方法，能够有效提高自动驾驶卡车横向控制的鲁 棒性和抗干扰能
力，进而提高横向控制的
稳定性。
[0007] 本发明的一种自动驾驶卡车的横向控制方法，包括如下的步骤：
[0008] S100：接收输入的参考轨迹，找到距离卡车最近的轨迹点并计算出
跟踪 误差；
[0009] S200：通过扩张状态观测器得到重新构建的卡车状态的估计值和扰动的 等效估计值；
[0010] S300：将步骤S100获得的跟踪误差和步骤S200获得的重新构建的卡车 状态的估计值，作为线性二次型调节器的状态变量输入到LQR闭环反馈控 制器，获得线性二次型调节器控制器输出；
[0011] S400：结合步骤S300获得的线性二次型调节器控制器输出和步骤S200 获得的卡车状态扰动的等效估计值对控制器输出进行补偿，获得卡车的控制
信号，发送给卡车的线控系统；
[0012] S500：循环步骤S100-S400，使得卡车的输出跟期望的输出一致。
[0013] 进一步地，在所述步骤S100之前，还包括步骤S000：初始化，载入线 性化的车辆动力学模型，载入线性二次型调节器的参数，载入扩张状态观测 器的参数。
[0014] 进一步地，在步骤S200中，扩张状态观测器的输入信息包括卡车车辆 的测量信号和上一时刻卡车的
控制信号。
[0015] 进一步地，所述步骤S200中，扩张状态观测器反馈可以直接测量的状 态量，并且估算难以测量的状态量和扰动，然后对外部扰动进行补偿。
[0016] 进一步地，在步骤S400中，所述控制信号为做过滤波和
限幅处理的方 向盘指令。
[0017] 本发明还提供了一种自动驾驶卡车的横向控制系统，能够实现上述的 自动驾驶卡车的横向控制方法，该自动驾驶卡车的横向控制系统包括如 下的控
制模块：
[0018] 初始化模块，用于载入线性化的车辆动力学模型、载入线性二次型调节 器的参数和载入扩张状态观测器的参数；
[0019] 线性二次型调节器，用于执行线性二次型控制
算法，根据设置的性能指 标，计算出使性能指标最优的控制输出；
[0020] 扩张状态观测器，其通过控制信号和车辆的测量信号，反馈可以直接测 量的状态量，并且估算难以测量的状态量和扰动，然后对外部扰动进行补偿。
[0021] 进一步地，所述线性二次型调节器包括LQR闭环反馈控制器。
[0022] 本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质上 存储有
计算机程序，所述计算机程序在被处理器执行时实现上述的自动 驾驶卡车的横向控制方法。
[0023] 本发明具有的有益效果：
[0024] 1、本发明通过LQR(linear quadratic regulation，LQR)+ESO(extended state observer，ESO)可以极大的提高控制算法的抗干扰能力和鲁棒性，使 得卡车在不同载重下和特殊工况下依然具有很好的稳定性。
[0025] 2、通过估计出扰动的大小并进行补偿，可以加强横向控制的一致性， 使得卡车在面对不同扰动(包括车辆自身载重等情况变化的扰动下)系统都 能补偿扰动，使车辆的响应更贴近设计指标，提高乘坐的舒适性。
附图说明
[0026] 图1是本发明一种自动驾驶卡车的横向控制方法的原理图。
[0027] 图2是本发明一种自动驾驶卡车的横向控制系统的结构示意图。
[0028] 图3是本发明一种自动驾驶卡车的横向控制方法一个
实施例的流程 图。
[0029] 图4是现有卡车的结构示意图。
[0030] 图5是现有卡车行驶中直线车道甩挂现象示意图。
[0031] 图6是现有卡车行驶中弯道刮碰现象示意图。
具体实施方式
[0032] 以下参照附图1-3，对本发明一种自动驾驶卡车的横向控制方法、系统 及存储介质做进一步地说明。
[0033] 本发明涉及一种基于扩张状态观测器(ESO)设计的横向稳定控制算法， 既可以在不同载重下具备良好的车道保持功能，也可以在遇到外部扰动时通 过扩张状态观测器(ESO)估计出扰动并进行补偿，达到消除外部扰动的效 果，具有很强的抗干扰能力。具体来说，上游planning提供自动驾驶卡车车 辆行驶所需要的参考轨迹，根据给定的目标
位置、航向角以及卡车自身的状 态信息，利用扩张状态观测器(ESO)充分估计卡车的重量等模型变化以及 外部扰动并进行补偿，结合线性二次型调节器(LQR)控制器的设计，从而 保证卡车在存在干扰和负载的同时能够快速稳定的跟踪输入轨迹。
[0034] 进入线性二次型调节器的是在扩张状态观测器中重新构建的状态量，这 些状态量都会被设计趋于卡车的状态量，通过重新构建状态量将系统等扰动 的等效值分离，在控制器输出端进行补偿。这些估计量会代替测量的卡车状 态量，来计算跟踪误差，因此进入线性二次型调节器的经过“修正”的跟踪 误差。
[0035] 本发明先利用线性二次型调节器(LQR)和精准度比较高的卡车动力学 模型进行闭环反馈控制，使得卡车在正常行驶时的控制性能达到设计要求， 然后利用扩张状态观测器(ESO)对车辆的模型不确定性和外部扰动一起进 行估计，并通过反馈进行补偿，达到在存在路面干扰、载重变化以及未建模 的不确定性的干扰时，依旧能够达到参考模型设计的指标性能要求，使得卡 车的横向控制在较大的干扰存在时，在保证系统鲁棒性的同时，还能够保持 系统响应的一致性。
[0036] 如图1所示，为本发明的一种自动驾驶卡车的横向控制方法的原理图， 该方法的算法主要由两个部分组成：1.扩张状态观测器ESO：通过控制信号 和车辆的测量信号，反馈可以直接测量状态量，并且估算难以测量的状态量 和扰动，然后对外部扰动进行补偿，以达到消除外部扰动和车辆模型不确定 性扰动的目的。2.LQR控制器：在接收到上游输入的参考轨迹后，找到距离 卡车最近的轨迹点并计算出跟踪误差e，然后将跟踪误差以及扩张状态观测 器ESO反馈的信息作为线性二次型调节器的状态变量X，设计LQR闭环反 馈控制器，保证设计系统具有足够的例如60°的
相位裕度和12db的幅值裕度。
[0037] 根据原理图，可以得出本发明的一种自动驾驶卡车的横向控制方法，包 括如下的步骤：
[0038] S100：接收输入的参考轨迹，找到距离卡车最近的轨迹点并计算出跟踪 误差。
[0039] S200：通过扩张状态观测器得到重新构建的卡车状态的估计值和扰动的 等效估计值；
[0040] S300：将步骤S100获得的跟踪误差和步骤S200获得的重新构建的卡车 状态的估计值，作为线性二次型调节器的状态变量输入到LQR闭环反馈控 制器，获得线性二次型调节器控制器输出；
[0041] S400：结合步骤S300获得的线性二次型调节器控制器输出和步骤S200 获得的卡车状态扰动的等效估计值对控制器输出进行补偿，获得卡车的控制 信号，发送给卡车的线控系统；
[0042] S500：循环步骤S100-S400，使得卡车的输出跟期望的输出一致。
[0043] 在所述步骤S100之前，还包括步骤S000：初始化，载入线性化的车辆 动力学模型，载入线性二次型调节器的参数，以及载入扩张状态观测器的参 数。
[0044] 本发明一种自动驾驶卡车的横向控制方法采用的横向稳定控制算法，其 主要组成部分有两个：
[0045] 1.扩张状态观测器，常见的表示形式如下：
[0046]
[0047] 其中，y(t)为输出，z1，z2…zn为ESO估计的状态变量，zn+1为ESO估计的扰 动量，g1，g2…gn+1为非线性函数。
[0048] 2.线性二次型算法
[0049] 线性二次型控制是最优控制的一种，根据设置的性能指标，计算出使性 能指标最优的控制输出。
[0050] 控制律：
[0051] ub＝Kx,K＝-R-1BTP
[0052] 其中，B为建模的系统的输入矩阵，R为控制分量权值矩阵；P通过求 解PA+ATP+Q-PBR-1BTP+Q＝0(Riccati方程)获得，其中，A为建模的系统状 态矩阵，Q为状态变量的权值矩阵，Q为任意正定矩阵。
[0053] 如图2所示，本发明的一种自动驾驶卡车的横向控制系统，能够实现 上述的自动驾驶卡车的横向控制方法，该自动驾驶卡车的横向控制系统 包括如下的
控制模块。
[0054] 初始化模块，用于载入线性化的车辆动力学模型、载入线性二次型调节 器的参数和载入扩张状态观测器的参数。
[0055] 线性二次型调节器，用于执行线性二次型控制算法，根据设置的性能指 标，计算出使性能指标最优的控制输出。线性二次型调节器包括LQR闭环 反馈控制器。
[0056] 扩张状态观测器，其通过控制信号和车辆的测量信号，反馈可以直接测 量的状态量，并且估算难以测量的状态量和扰动，然后对外部扰动进行补偿。
[0057] 本发明的一种计算机可读存储介质，其上存储有计算机程序，该计算 机程序在被处理器执行时能够实现上述的自动驾驶卡车的横向控制方 法。
[0058] 实施例1
[0059] 如图3所示，本发明一种自动驾驶卡车的横向控制方法一个实施例的
流程图，其具体的步骤如下。
[0060] 步骤S000：系统初始化。载入线性化的车辆动力学模型，载入线性二 次型调节器的时变参数，载入扩张状态观测器的参数。
[0061] 以
状态方程的形式建立车辆的动力学模型如下：
[0062]
[0063] 其中的参数都为卡车参数并且可以测量与计算。
[0064] 线性二次型调节器权重矩阵都为对角阵，形式如下：
[0065]
[0066] 扩张状态观测器的估计方程如下：
[0067]
[0068] 其中e1为车辆横向位置误差，e2为车辆横摆角误差，b1，b2为卡车建模的 参数，b1，b2，b3，b4，b5，b6为可选参数，g1()，g2()，g3()，g4()，g5()，g6()为非线性 函数，这是一个常用的非线性函数形式：
[0069] 其中a,d为常数。
[0070] 步骤S100：处理输入的轨迹，找到距离卡车最近的点，计算误差状态变 量，即跟踪误差；
[0071] e＝r-y，其中，r为输入轨迹，y为输出轨迹。
[0072] 步骤S200：根据卡车的状态和上一时刻的控制输入，通过扩张状态观测 器，得到卡车状态的估计值和扰动的等效估计值；
[0073]
[0074] 其中，z1，z2，z3，z4为对系统不含扰动状态量的估计，z5为对系统内外部 扰动的总体估计，分别去补偿来自侧向力和侧向力矩的扰动，b5g5(z1-e1)为 对侧向力扰动影响的补偿，b6g6(z3-e2)为对侧向力矩扰动影响的补偿。
[0075] 步骤S300：根据卡车的线性化模型，计算线性二次型调节器控制律。
[0076] 线性二次型调节器控制器输出：
[0077] ub＝Kz,K＝-R-1BTP。
[0078] 步骤S400：在利用扩张观测器观测获得总体扰动补偿后，将补偿了扰动 的控制输出做滤波、限幅处理，然后作为控制信号以固定
频率发送给卡车线 控。此处的控制信号为方向盘指令。
[0079] 步骤S500：循环步骤S100-S400，使得卡车的输出跟期望的输出一致。
[0080] 对于本领域技术人员而言，显然本发明不限于上述示范性实施例的 细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的 具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是 示范性的，而且是非限制性的，本发明的范围由所附
权利要求而不是上 述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所 有变化囊括在本发明内。
