一种无人驾驶车辆跟踪控制方法及装置发明专利

专利名称：一种无人驾驶车辆跟踪控制方法及装置

专利类型：发明专利

专利申请号：CN202011231921.0

专利申请（专利权）人：宇通客车股份有限公司
权利人地址：河南省郑州市管城回族区宇通路6号

专利发明（设计）人：黄琨,苏常军,陈慧勇

专利摘要：本发明属于无人驾驶技术领域，具体涉及一种无人驾驶车辆跟踪控制方法及装置。该方法首先获取车辆当前系统状态量，根据当前系统状态量所在状态分区，确定与系统状态分区对应的控制显式关系式，系统状态量包括车体坐标系下纵向位移的一阶导数、车体坐标系下横向位移的一阶导数、横摆角、横摆角的一阶导数、惯性坐标系下纵向位移、以及惯性坐标系下横向位移，然后将当前系统状态量代入至所述控制显式关系式中，得到与所述当前系统状态量对应的当前最优前轮转角控制量；最后参照所述当前最优前轮转角控制量，控制车辆运行。本发明将反复的在线计算简化为单纯的查表工作，大大减少了在线计算时间，提高了系统控制的实时性，保证了跟踪精度。

主权利要求：
1.一种无人驾驶车辆跟踪控制方法，其特征在于，包括如下步骤：
1)获取车辆当前系统状态量，根据当前系统状态量所在状态分区，确定与系统状态分区对应的控制显式关系式；
其中，采用显式模型预测控制方法得到所述状态分区以及状态分区对应的控制显式关系式；且所述显式模型预测控制方法的系统状态量包括车体坐标系下纵向位移的一阶导数、车体坐标系下横向位移的一阶导数、横摆角、横摆角的一阶导数、惯性坐标系下纵向位移、以及惯性坐标系下横向位移，系统控制量为前轮转角；
2)将当前系统状态量代入至所述控制显式关系式中，得到与所述当前系统状态量对应的当前最优前轮转角控制量；
3)参照所述当前最优前轮转角控制量，控制车辆运行，以使车辆跟踪预设轨迹；
采用显式模型预测控制方法得到所述状态分区以及状态分区对应的控制显式关系式包括：建立车辆动力学模型，并将动力学模型转化为状态空间表达式；
基于所述状态空间表达式，应用线性二次调节器构建车辆控制优化函数；
将车辆控制优化函数转化为最优控制问题，转化得到二次规划的标准形式，进而将二次规划的标准形式转化为二次最优化形式；
将库恩塔克约束条件的公式代入至所述二次最优化形式进行条件约束，最终求解得到最优前轮转角控制量；
采用分段函数表示所述最优前轮转角控制量，以得到各状态分区以及以状态分区对应的控制显式关系式；所述车辆控制优化函数为：*
J(ξ(t))＝minJ(U,ξ(t))*Us.t.ξt+k+1|t＝Aξt+k|t+But+kk≥0
0≤ξt+k≤ξmax
umin≤uk+t≤umax
*
其中，J (U,ξ(t))为基于系统控制向量U和系统状态向量ξ(t)的车辆控制优化函数，取决于优化问题的系统状态向量ξ(t)；P、Q、R为常数权重矩阵；ξmax为ξt+k取值的最大值；umin、umax分别为uk+t取值的最小值和最大值；Xt+k|t表示参数X在t时刻对t+k时刻的预测值；Xt+k表示参数X在t+k时刻的实际值。
2.根据权利要求1所述的无人驾驶车辆跟踪控制方法，其特征在于，所述车辆动力学模型为：其中， 分别为车体坐标系下横向位移、纵向位移的二阶导数， 为车体坐标系下横向位移、纵向位移的一阶导数；为横摆角；为横摆角的一阶导数， 为横摆角的二阶导数；m为车辆质量；Clf、Clr分别为车辆前后轮胎的纵向刚度；Ccf、Ccr分别为车辆前后轮胎的侧偏刚度；sf、sr分别为车辆前后轮胎的滑移率；δf为前轮转角；a为前轴到质心的距离；Iz为z轴转动惯量；b为后轴到质心的距离； 分别为惯性坐标系下横向位移、纵向位移的一阶导数。
3.根据权利要求2所述的无人驾驶车辆跟踪控制方法，其特征在于，所述状态空间表达式为：其中，为系统状态量ξ的一阶导数， 分别为惯性坐标系下横向位移、纵向位移；u＝δf为系统控制量； C(t)＝(0,0,0,0,T
1,0) ，且：
4.根据权利要求1所述的无人驾驶车辆跟踪控制方法，其特征在于，所述二次规划的标准形式为：s.t.GU≤W+Eξ(t)
其中，Y、H、F、G、W、E是常数参数。
5.根据权利要求4所述的无人驾驶车辆跟踪控制方法，其特征在于，所述二次最优化形式为：s.t.Gz(ξ(t))≤W+Sξ(t)
‑1 ‑1 T
其中，S＝E+GH F； z(ξ(t))＝U+H F ξ(t)。
6.根据权利要求1所述的无人驾驶车辆跟踪控制方法，其特征在于，采用分段函数表示的所述最优前轮转角控制量为：* i i
U(ξ(t))＝Fξ(t)+Gi＝1,..,Nri ‑1 T ‑1 T ‑1 i ‑1 T ‑1 T ‑1 ‑1 T *其中，G＝H G (GH G) W，F＝H G (GH G) S‑H F ，Nr为状态分区的总数目；U (ξ(t))为最优前轮转角控制量；ξ(t)为系统状态量。
7.一种无人驾驶车辆跟踪控制装置，其特征在于，包括存储器和处理器，所述处理器用于执行存储在存储器中的指令以实现如权利要求1～6任一项所述的无人驾驶车辆跟踪控制方法。 说明书 : 一种无人驾驶车辆跟踪控制方法及装置技术领域[0001] 本发明属于无人驾驶技术领域，具体涉及一种无人驾驶车辆跟踪控制方法及装置。背景技术[0002] 跟踪控制技术作为无人驾驶系统的关键技术之一，通过控制车辆的转向系统以及制动/驱动系统等，使得车辆能够在指定的时间到达给定的或规划的轨迹点，从而实现车辆的无人驾驶操作。[0003] 目前行业内主流的无人驾驶系统跟踪控制技术有PID控制算法、预瞄式控制算法、前反馈控制算法、传统模型预测算法。PID控制算法需要大量的人工标定，控制精度无法实时保证，同时当车辆状态发生改变时，控制精度会受到较大影响，导致控制误差较大；预瞄式控制算法与前反馈控制算法不需要人工标定，控制精度能够保证车辆正常行驶，但不具备车速扰动抗干扰能力，当车速发生突变时，由于时间滞后等因素影响会导致控制误差变大；相比以上算法，传统模型预测控制算法在保证控制精度的同时，具备良好的车速扰动抗干扰能力，但由于需要反复进行在线优化，随着场景的变化，在线计算量会显著增加，不能满足无人驾驶系统实时跟踪控制的需求。发明内容[0004] 本发明提供了一种无人驾驶车辆跟踪控制方法及装置，用以解决现有技术的控制方法无法满足无人驾驶系统实时跟踪控制需要的问题。[0005] 为解决上述技术问题，本发明的技术方案包括：[0006] 本发明提供了一种无人驾驶车辆跟踪控制方法，包括如下步骤：[0007] 1)获取车辆当前系统状态量，根据当前系统状态量所在状态分区，确定与系统状态分区对应的控制显式关系式；[0008] 其中，采用显式模型预测控制方法得到所述状态分区以及状态分区对应的控制显式关系式；且所述显式模型预测控制方法的系统状态量包括车体坐标系下纵向位移的一阶导数、车体坐标系下横向位移的一阶导数、横摆角、横摆角的一阶导数、惯性坐标系下纵向位移、以及惯性坐标系下横向位移，系统控制量为前轮转角；[0009] 2)将当前系统状态量代入至所述控制显式关系式中，得到与所述当前系统状态量对应的当前最优前轮转角控制量；[0010] 3)参照所述当前最优前轮转角控制量，控制车辆运行，以使车辆跟踪预设轨迹。[0011] 上述技术方案的有益效果为：本发明将显式模型预测控制方法应用于无人驾驶车辆控制上来，在离线预处理阶段，划分系统状态分区，并确定系统状态分区对应的控制显式关系式，该阶段结束后，将划分的状态分区以及对应的控制显式关系式存储在系统运行空间中供在线计算使用，在线阶段时，在每一个采样时刻，一旦确定系统的初始状态，就可以通过查询方式找到当前时刻对应的控制显式关系式，最终计算得到当前时刻的最优前轮转角控制量以对车辆进行控制。该方法将计算复杂度较高的最优控制求解问题转移为离线计算，在线计算只需要一个简单的查询过程和线性函数运算过程，将反复的在线计算简化为单纯的查表工作，大大减少了在线计算时间，提高了系统控制的实时性，保证了跟踪精度，解决了复杂场景下无人驾驶系统高效实时跟踪控制问题。[0012] 进一步的，采用显式模型预测控制方法得到所述状态分区以及状态分区对应的控制显式关系式包括：[0013] 建立车辆动力学模型，并将动力学模型转化为状态空间表达式；[0014] 基于所述状态空间表达式，应用线性二次调节器构建车辆控制优化函数；[0015] 将车辆控制优化函数转化为最优控制问题，转化得到二次规划的标准形式，进而将二次规划的标准形式转化为二次最优化形式；[0016] 将库恩塔克约束条件的公式代入至所述二次最优化形式进行条件约束，最终求解得到最优前轮转角控制量；[0017] 采用分段函数表示所述最优前轮转角控制量，以得到各状态分区以及以状态分区对应的控制显式关系式。[0018] 进一步的，所述车辆动力学模型为：[0019][0020] 其中， 分别为车体坐标系下横向位移、纵向位移的二阶导数， 为车体坐标系下横向位移、纵向位移的一阶导数； 为横摆角； 为横摆角的一阶导数， 为横摆角的二阶导数；m为车辆质量；Clf、Clr分别为车辆前后轮胎的纵向刚度；Ccf、Ccr分别为车辆前后轮胎的侧偏刚度；sf、sr分别为车辆前后轮胎的滑移率；δf为前轮转角；a为前轴到质心的距离；Iz为z轴转动惯量；b为后轴到质心的距离； 分别为惯性坐标系下横向位移、纵向位移的一阶导数。[0021] 进一步的，所述状态空间表达式为：[0022][0023] 其中，为系统状态量ξ的一阶导数， 分别为惯性坐标系下横向位移、纵向位移；u＝δf为系统控制量； C(t)＝(0,0,T0,0,1,0) ，且[0024][0025][0026] 进一步的，所述车辆控制优化函数为：[0027][0028] J*(ξ(t))＝minJ(U,ξ(t))*U[0029] s.t.ξt+k+1t＝Aξt+kt+But+kk≥0[0030] 0≤ξt+k≤ξmax[0031] umin≤uk+t≤umax[0032] 其中，J(U,ξ(t))为系统控制向量U和系统状态向量ξ(t)的车辆控制优化函数，取决于优化问题的系统状态向量ξ(t)；P、Q、R为常数权重矩阵；ξmax为ξt+k取值的最大值；umin、umax分别为uk+t取值的最小值和最大值；Xt+k|t表示参数X在t时刻对t+k时刻的预测值；Xt+k表示参数X在t+k时刻的实际值。[0033] 进一步的，所述二次规划的标准形式为：[0034][0035] s.t.GU≤W+Eξ(t)[0036] 其中，Y、H、F、G、W、E是常数参数。[0037] 进一步的，所述二次最优化形式为：[0038][0039] s.t.Gz(ξ(t))≤W+Sξ(t)[0040] 其中，S＝E+GH‑1F； z(ξ(t))＝U+H‑1 TFξ(t)。[0041] 进一步的，采用分段函数表示的所述最优前轮转角控制量为：[0042] U*(ξ(t))＝Fiξ(t)+Gii＝1,..,Nr[0043] 其中，Gi＝H‑1GT(GH‑1GT)‑1W，Fi＝H‑1GT(GH‑1GT)‑1S‑H‑1FT，Nr为状态分区的总数目。[0044] 本发明还提供了一种无人驾驶车辆跟踪控制装置，包括存储器和处理器，所述处理器用于执行存储在存储器中的指令以实现上述介绍的无人驾驶车辆跟踪控制方法。附图说明[0045] 图1是本发明的车辆三自由度模型示意图；[0046] 图2是本发明的无人驾驶车辆跟踪控制方法原理图；[0047] 图3是本发明的无人驾驶车辆跟踪控制装置结构图。具体实施方式[0048] 本发明采用显式模型预测控制对自动驾驶车辆进行控制，其主要思想是采用多参数二次规划理论求解线性时不变约束系统优化控制问题。该方法将系统状态量作为参数，对状态区域进行凸划分，并离线计算各状态分区上的最优状态反馈控制率与状态的控制显式关系式。在每一个采样时刻，根据当前系统状态量所在状态分区，查表得到对应的控制显式关系式，得到当前最优前轮转角控制量。显式模型预测控制方法无需反复在线优化，可提高在线的计算速度。下面结合附图及实施例，对本发明的一种无人驾驶车辆跟踪控制方法及一种无人驾驶车辆跟踪该控制装置进行详细说明。[0049] 方法实施例：[0050] 本发明的一种无人驾驶车辆跟踪控制方法实施例，如图2所示，步骤如下：[0051] 步骤一，基于显式模型预测控制，按照如下方法步骤，得到车辆的各状态分区以及各状态分区对应的显式控制关系式，以完成离线预处理。具体的：[0052] 1、建立车辆动力学模型。在进行车辆动力学通用化建模时，只需要考虑纵向、横向、横摆三个自由度上的运动。[0053] 车辆的三自由度模型如图1所示，o为车辆质心位置，车辆受到的纵向力、侧向力以及横摆力矩为：[0054][0055] 其中，m为车辆质量；x、y分别为车体坐标系下横向位移、纵向位移；为车体坐标系下纵向坐标的一阶导数； 分别为车体坐标系下横向位移、纵向位移的二阶导数；δ为转向角；为转向角的一阶导数；Iz为z轴转动惯量； 为横摆角； 为横摆角的二阶导数；Fx为车辆所受总的纵向力；Fy为车辆所受总的横向力；Mz为车辆所受总的横摆力矩；Fcf、Fcr分别为车辆前后轮胎所受侧向力；Flf、Flr分别为车辆前后轮胎所受纵向力；Fxf、Fxr分别为车辆前后轮胎在x方向所受力；Fyf、Fyr分别为车辆前后轮胎在y方向所受力；δf为前轮转角；a为前轴到质心的距离；b为后轴到质心的距离。[0056] 车辆的轮胎力可以表示成：[0057][0058] 其中，Ccf、Ccr分别为车辆前后轮胎的侧偏刚度；Clf、Clr分别为车辆前后轮胎的纵向刚度；sf、sr分别为车辆前后轮胎的滑移率； 为横摆角的一阶导数；为车体坐标系下横向位移的一阶导数。[0059] 综合式(1)和式(2)，可以得到车辆动力学模型为：[0060][0061] 其中， 分别为惯性坐标系下横向位移、纵向位移的一阶导数。[0062] 2、选取 为系统状态量，u＝δf为系统控制量，经过线性化处理后，公式(3)的状态空间表达式可表示为：[0063]T[0064] 其中， C(t)＝(0,0,0,0,1,0)，且：[0065][0066][0067] 3、应用线性二次调节器(linear‑quadratic‑regulator，LQR)理论构建车辆跟踪控制优化函数，表达式如下：[0068][0069] 其中，P、Q、R为常数权重矩阵。[0070] 将公式(4)转化为如下形式：[0071][0072] 4、将车辆控制优化函数转化为最优控制问题，从而将公式(5)转化为二次规划的标准形式：[0073][0074] 其中， 是优化控制向量，取决于优化问题的系统状态向量ξ(t)；Y、H、F、G、W、E都是常数参数；这里定义：[0075] z(ξ(t))＝U+H‑1FTξ(t)(8)[0076] 5、将公式(7)转化为二次最优化形式：[0077]‑1[0078] 其中，S＝E+GH F；[0079] 6、求解公式(7)时，附加上库恩塔克(Karush‑Kuhn‑Tucker，KKT)最优化条件作为约束条件，KKT公式如下：[0080] Hz+GTλ＝0(10)[0081] λi(Gz‑W‑Sξ)＝0,i＝1,...,N(11)[0082] 其中，λ≥0，N≥1。[0083] 由公式(10)可得z＝‑H‑1GTλ，并代入至公式(11)中可得：[0084] λ(‑GH‑1GTλ‑W‑Sξ)＝0(12)[0085] 由公式(12)可得：[0086] λ＝‑(GH‑1GT)‑1(W+Sξ)(13)[0087] z＝H‑1GT(GH‑1GT)‑1(W+Sξ)(14)[0088] 将公式(14)代入到公式(8)可得最优控制向量，即最优前轮转角控制向量：[0089] U*＝H‑1GT(GH‑1GT)‑1(W+Sξ)‑H‑1FTξ(15)[0090] 7、对于当前系统状态ξ(t)，最优前轮转角控制向量U*(ξ(t))可以由公式(15)转换为ξ(t)的分段仿射函数表示：[0091] U*(ξ(t))＝Fiξ(t)+Gii＝1,..,Nr(16)[0092] 其中，Gi＝H‑1GT(GH‑1GT)‑1W，Fi＝H‑1GT(GH‑1GT)‑1S‑H‑1FT，Nr为状态分区的总数目。[0093] 至此，便完成离线预处理，将划分的状态分区以及对应的控制显式关系式存储在系统运行空间中供在线计算使用。[0094] 步骤二，在车辆运行过程中，获取车辆当前系统状态量，包括车体坐标系下纵向位移的一阶导数 车体坐标系下横向位移的一阶导数 横摆角 横摆角的一阶导数 惯性坐标系下纵向位移Y、以及惯性坐标系下横向位移X，根据当前系统状态量所在状态分区，确定与系统状态分区对应的控制显式关系式。[0095] 步骤三，将车辆当前系统状态量构成的系统状态向量代入至公式(16)中，得到与当前系统状态量对应的当前最优前轮转角控制量。根据得到的当前最优前轮转角控制量，对车辆进行控制，以使车辆跟踪预设轨迹。[0096] 本发明的无人驾驶车辆跟踪控制方法，在每个控制周期里，只需根据当前系统状态量所在状态分区，确定对应的控制率(控制显式关系式)，最终得到最优前轮转角控制量。相对于反复在线优化方法，该方法能够在保证跟踪精度的同时，避免复杂的在线计算量，提升实时可操作性，且具备车速扰动抗干扰能力，满足复杂场景下无人驾驶系统跟踪控制实时性高、变化速率快的需求。[0097] 装置实施例：[0098] 本发明的一种无人驾驶车辆跟踪控制装置实施例，如图3所示，包括存储器、处理器和内部总线，处理器、存储器之间通过内部总线完成相互间的通信和数据交互。存储器包括至少一个存储于存储器中的软件功能模块，处理器通过运行存储在存储器中的软件程序以及模块，执行各种功能应用以及数据处理，实现本发明的方法实施例中介绍的一种无人驾驶车辆跟踪控制方法。[0099] 其中，处理器可以为微处理器MCU、可编程逻辑器件FPGA等处理装置。存储器用于存储程序，处理器在接收到执行指令后，执行程序。[0100] 存储器可为利用电能方式存储信息的各式存储器，RAM、ROM等；利用磁能方式存储信息的各式存储器，例如硬盘、软盘、磁带、磁芯存储器、磁泡存储器、U盘等；利用光学方式存储信息的各式存储器，例如CD、DVD等。当然，还有其他方式的存储器，例如量子存储器、石墨烯存储器等。

专利地区：河南

专利申请日期：2020-11-06

专利公开日期：2024-06-18

专利公告号：CN114442601B