CN113306573B - 一种自动驾驶车辆学习型路径跟踪预测控制方法 - Google Patents

Abstract

本发明实施例公开了一种自动驾驶车辆学习型路径跟踪预测控制方法，包括：在当前采样时刻，获取车辆的当前系统状态；获取所述车辆的未来N个期望状态；根据当前状态、学习型预测控制模型预测车辆未来状态，结合未来N个期望状态、预设的目标函数和系统约束，得到最优控制序列；使用求得的最优控制序列中的第一个量控制所述车辆的运行，直到下一采样时刻到达，计算得到下一最优控制序列，如此重复，在每一采样时刻进行计算，直到车辆行驶完整个路径。

Description

一种自动驾驶车辆学习型路径跟踪预测控制方法

技术领域

本发明实施例涉及自动驾驶车辆技术领域，特别是一种自动驾驶车辆学习型路径跟踪预测控制方法。

背景技术

自动驾驶具有减缓交通拥堵、提高交通效率、减少驾驶员工作量的优点、近些年来受到广泛关注，其中，路径跟踪又是实现自动驾驶的一项关键技术。

目前实现路径跟踪的方法主要包括基于几何模型的路径跟踪、无模型反馈控制的路径跟踪和基于模型的反馈控制的路径跟踪。但是，目前的这些跟踪方法，车辆在进行路径跟踪的过程中，容易受到随机噪声、路面起伏、外界干扰等因素影响，不利于跟踪效果。

发明内容

本发明实施例的目的在于提供一种自动驾驶车辆学习型路径跟踪预测控制方法，来克服或至少减轻现有技术的上述缺陷中的至少一个。

为实现上述目的，本发明实施例提供一种自动驾驶车辆学习型路径跟踪预测控制方法，包括：

步骤1，在当前采样时刻，获取车辆的当前系统状态；

步骤2，获取所述车辆的未来N个期望状态，N为正整数；包括：对所述车辆的待行驶路径进行样条插值，以得到的新路径为参考路径；根据所述当前系统状态获取所述参考路径上距离所述车辆位置最近的路点P₀，以P₀为起点，v(k)ΔT为搜索距离，沿所述参考路径向前搜索获得N个参考路点P_i，i＝1，2，...，N，v(k)表示当前时刻k的车辆速度，ΔT为离散控制周期，所述N个参考路点的状态为所述N个期望状态；

步骤3，根据所述当前系统状态和学习型预测控制模型预测车辆未来状态，并与所述未来N个期望状态、预设的目标函数和系统约束结合，得到最优控制序列；

其中，所述预设的目标函数、所述学习型预测模型和所述系统约束包括：

式(1)中：

u_k+i是待求最优控制序列，表示前轮转角或方向盘转角；

表示实际求得的最优控制序列；

ε是松弛因子；

y_k+i是学习型预测模型的k+i时刻输出；其中所述学习型预测模型的表达式为：

x_k+1＝(A_d+F_k)x_k+(B_d+H_k)u_k+G_k (2)

其中，x_k为当前时刻k的系统状态，u_k代表k时刻的前轮转向角或方向盘转角δ；A_d、B_d是离散化后的系统参数矩阵，其值基于车辆纵向速度、前后轮侧偏刚度、前后轴到质心的距离、车辆质量和转动惯量得到；F_k、H_k和G_k是用于表征系统未建模动态的参数，其维度根据所述状态变量的个数确定，其值为0～1之间的随机数；

x_k+i+1是k时刻通过所述学习型预测模型预测的k+i+1时刻的系统状态；

x₀是所述车辆在所述路点P₀的系统状态；

r_k+i是k时刻的期望状态，为所述车辆的未来N个期望状态中的第i个期望状态；

Q，P，R，w_ε为预设权重系数，取值为大于0的实数；

C是预设系数，X是预设系统状态约束，D是预设未建模动态边界，W是预设终端状态约束，U是预设车辆控制约束，ΔU是预设车辆控制增量约束；

步骤4，使用求得的最优控制序列

中的第一个量

控制所述车辆的运行，直到下一采样时刻到达，重复步骤1-4，直到所述车辆行驶完整个路径。

优选的，所述车辆的系统状态包括状态变量：所述车辆的横向位置y、横摆角度ψ、质心侧偏角β和横摆角度变化率

优选的，通过下述方式构建所述学习型预测模型，包括：

步骤11，建立系统名义模型：

式(3)中：

x表示系统状态；

表示表征所述系统状态的变量的变化率，包括车辆的横向位置变化率、横摆角速度、质心侧偏角变化率和横摆角加速度；

u表示前轮转向角或方向盘转角δ：

矩阵A、B中的参数如下：

式(4)中：

v是车辆纵向速度；

C_f是前轮侧偏刚度；

l_f是前轴到质心的距离；

C_r是后轮侧偏刚度；

l_r是后轴到质心的距离；

m是车辆质量；

I_z为转动惯量；

步骤12，对步骤11中所述名义模型进行离散化并考虑扰动得到所述系统预测模型(5)；

x<sub>k+1</sub>＝A<sub>d</sub>x<sub>k</sub>+B<sub>d</sub>u<sub>k</sub>+d<sub>k</sub>

(5)

式(5)中：A_d、B_d是离散化后的系统参数矩阵，d_k是未建模动态引起的状态误差；

步骤13，构建系统未建模动力学模型：

d<sub>k</sub>＝F<sub>k</sub>x+H<sub>k</sub>u+G<sub>k</sub>

(6)

式(6)中：

α₁至α₂₄的值为区间[0，1]中的随机数；

步骤14，合并式(5)和式(6)，以构建式(2)表示的所述学习型预测模型。

优选的，该方法还包括：优化系统状态与更新未建模动态矩阵参数：

用F、H、G中的参数扩展所述式(5)，得到：

x_aug表示扩大的系统预测状态，y_aug是与x_aug对应的输出，C是系统输出矩阵，O是零矩阵，O^T是O的转置矩阵，I是单位矩阵；

对式(7)中f(·)和h(·)取关于x_aug的一阶偏导数，得到：

其中，

表示k时刻扩大的系统预测状态，

表示初步预测的k+1时刻扩大的系统预测状态；

采用下式(9)初步预测下一个采样时刻的系统状态，并更新估计误差协方差矩阵

式(9)中：

dx_aug，k表示位于线性化邻域内的扰动；

是k时刻误差协方差矩阵；

是k+1时刻估计误差协方差矩阵；

Q_k+1是由系统的不确定性引起的参数噪声的协方差；

是系统初步预测输出的下一个采样时刻的系统状态；

采用下式(10)正式预测所述下一个采样区间的系统状态，并更新误差协方差矩阵；

式(10)中：

R_k+1是测量噪声协方差；

K_k+1是卡尔曼增益；

是k+1时刻误差协方差矩阵；

是优化后的系统状态，用于所述步骤3中的所述当前系统状态。

优选的，所述

为28x1的矩阵，所述优化后的系统状态是所述

的前4个参数；

所述α₁至α₂₄的值为区间[0，1]中的随机数包括：所述α₁至α₂₄的初始值为区间[0，1]中的任意数，之后由所述

的后24个参数进行迭代更新。

本发明由于采取以上技术方案，其具有以下优点：

本发明提出的自动驾驶车辆学习型路径跟踪预测控制方法中，考虑了系统未建模动态的影响，能够根据外部扰动实时学习更新未建模动态模型参数，对系统扰动进行了有效补偿，所建模型更加接近实际系统，使得跟踪精度更加精确，同时，系统具有较好鲁棒性。

附图说明

图1为本发明实施例提供的学习型预测模型的构建过程示意图；

图2为本发明实施例提供的自动驾驶车辆学习型路径跟踪预测控制方法的流程示意图；

图3示意性提供了车辆的未来N个参考路点；

图4为本发明实施例提供的系统状态优化和更新过程示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

本发明实施例提供一种自动驾驶车辆学习型路径跟踪预测控制方法，该自动驾驶车辆包括：车辆状态采集器件，其可以为GPS设备、惯性测量单元(IMU)、以及其他传感器，用于测量车辆的系统状态，包括但不限于采集车辆的行驶速度、横向位置、横摆角、质心侧偏角、以及横摆角变化率，以及本文出现的其它需要采集测量的与车辆行驶相关的量。车辆的系统状态仅为一个称呼，用于指示车辆行驶的状态，也可以有其它名称，例如叫做车辆的运行状态等，本文对此不做限制。系统可以指车辆中与本发明相关器件的整体。还可以利用该车辆状态采集器件或者其它定位器件获得车辆的待行驶路径以及实际行驶路径，例如通过GPS定位系统或者其它车载导航系统等获得车辆位置和目的地之间的路径。该车辆还包括控制器，该控制器的功能包括但不限于根据预先配置周期性或非周期性预测车辆的速度和前轮转角或方向盘转角，控制车辆的行驶。容易理解，该自动驾驶车辆系统还包括其它构造，本文不再一一列出。

自动驾驶车辆的行驶涉及对车辆速度、方向等多个参量的控制，本发明实施例提供一种自动驾驶车辆学习型路径跟踪预测控制方法，用于自动驾驶车辆预测前轮转角或方向盘转角，使用前轮转角或方向盘转角控制车辆的行驶。

本发明实施例中，在控制器内构建和/或存储车辆的学习型预测模型。图1示出学习型预测模型的构建过程，包括：

步骤11，建立系统名义模型。

系统的状态变量可以包括y(横向位置)、ψ(横摆角)，β(质心侧偏角)，

(横摆角变化率)，容易理解，在不同应用场景下，或者基于不同应用需求，状态变量的种类和数量可以不同。针对不同状态变量，本发明实施例提供的自动驾驶车辆学习型路径跟踪预测控制方法同样适用。

系统名义模型如下式(3)所示。

式(3)中：

x表示系统状态；

表示表征系统状态的变量的变化率，在本实施例中，例如包括车辆的横向位置变化率、横摆角速度、质心侧偏角变化率和横摆角加速度；

u表示前轮转向角或方向盘转角δ；

矩阵A、B中的参数如下：

式(4)中：

v是车辆纵向速度；

C_f(C_r)是前(后)轮侧偏刚度；

l_f(l_r)是前(后)轴到质心的距离；

m是车辆质量；

I_z为转动惯量。

步骤12，对系统名义模型进行离散化并考虑扰动得到系统预测模型。该系统预测模型的数学表达式如下式(5)所示。

x<sub>k+1</sub>＝A<sub>d</sub>x<sub>k</sub>+B<sub>d</sub>u<sub>k</sub>+d<sub>k</sub>

(5)

式(5)中：

A_d、B_d是离散化后的系统参数矩阵。在一个示例中，可以由

计算得到A_d、B_d的值，T_s是采样周期，其值根据实际需要等预先设置，采样周期越小效果越好，但需要考虑运算量，通常可以取0.01秒至0.1秒之间的量，例如0.05秒。为减小计算量，也可使用向前欧拉、向后欧拉以及中点欧拉法得到离散后的A_d、B_d的值，本文对此不再赘述。

d_k是未建模动态引起的状态误差，其取值通过步骤13获得。

步骤13，构建系统未建模动力学模型。

由于系统未建模动态是非线性的，很难用物理机制描述，但是在很短一段时间内，例如一个控制周期内，可以用线性方式近似描述未建模动态，具体如下式(6)所示。

d<sub>k</sub>＝F<sub>k</sub>x+H<sub>k</sub>u+G<sub>k</sub>

(6)

式(6)中：

F、H、G是矩阵参数，其维度由状态变量个数决定，如果有n个状态变量，则一共会产生n*(n+2)个参数。此处以4个状态变量为例，一共产生24个参数。其中，α₁至α₂₄的值为区间[0，1]中的随机数，包括：α₁至α₂₄的初始值设定为区间[0，1]任意一个数，之后，α₁至α₂₄的值由式(11)中

的后24个参数进行迭代更新。

步骤14，合并式(5)和式(6)，构建学习型预测模型。该学习型预测模型的数学表达式如下式(2)所示。

x<sub>k+1</sub>＝(A<sub>d</sub>+F<sub>k</sub>)x<sub>k</sub>+(B<sub>d</sub>+H<sub>k</sub>)u<sub>k</sub>+G<sub>k</sub>

(2)

其中，x_k为k时刻的系统状态；uk是k时刻的系统输入，即k时刻的前轮转向角或方向盘转角δ；A_d、B_d、F_k、H_k和G_k均为参数，其取值见上述步骤11-13中所述。

本发明实施例提供的自动驾驶车辆学习型路径跟踪预测控制方法，如图2所示，包括如下步骤：

步骤201，获取当前系统状态。

在每一采样时刻，获取车辆的当前系统状态。系统状态包括状态变量，如上所述，状态变量可以包括y(横向位置)、ψ(横摆角)，β(质心侧偏角)，

(横摆角变化率)，在不同应用场景下，或者基于不同应用需求，状态变量的种类和数量可以不同。可以通过车辆上安装的GPS模块、惯性测量单元(IMU)、以及安装的其他传感器获得车辆的系统状态信息。传感器可以包括摄像头、雷达和激光等类型传感器。

步骤202，获取车辆的未来N个期望状态。

获取车辆的待行驶路径，例如通过搭载GPS的小车提前在某一路段进行路径信息采集得到(包括相对于大地坐标系的横纵向坐标，航向角等路径信息)。对车辆的待行驶路径进行三次样条插值，获得一条新路径，命名为参考路径。通过测得的系统状态信息遍历参考路径，获取参考路径上距离当前时刻k最近的参考路点P₀，以P₀为起点，v(k)ΔT为搜索距离，沿着参考路径向前搜索N个参考路点P_i，i＝1，2，...，N。ΔT为离散控制周期，其值可以根据实际需要预先设置，通常可以取0.01秒至0.1秒之间的量，且该离散控制周期可以设置为与采样周期的值相同。v(k)表示当前时刻k的车辆速度。图3示意性提供了车辆的未来N个参考路点。其中，该N个参考点的状态即为N个期望状态。在一种实现方式中，确定参考路径后，即可确定路径上各点的车辆运行状态，知晓N个参考点后即可知晓N个期望状态。N的值可以取5、10、20、30等，具体根据实际应用预先设定。

步骤203，根据当前状态、学习型预测控制模型预测车辆未来状态，结合未来N个期望状态、预设的目标函数和系统约束，得到最优控制序列。

其中，预设的目标函数、学习型预测模型和系统约束包括：

式(1)中：

u_k+i是待求最优控制序列，表示前轮转角或方向盘转角；

表示实际求得的最优控制序列；

ε是松弛因子；

学习型预测模型如式(2)所示，

x_k+i+1是k时刻通过学习型预测模型预测的k+i+1时刻的系统状态；

y_k+i是学习型预测模型的k+i时刻输出；

x₀是车辆在路点P₀的系统状态；

r_k+i是k时刻的期望状态，为车辆的未来N个期望状态中的第i个期望状态；

Q，P，R，w_ε为预设权重系数，取值为大于0的实数；

C是预设系数，X是预设系统状态约束，D是预设未建模动态边界，W是预设终端状态约束，U是预设车辆控制约束，ΔU是预设车辆控制增量约束。

步骤204，使用求得的最优控制序列

中的第一个量

控制所述车辆的运行，直到下一采样时刻到达，重复步骤201-204，直到车辆行驶完整个路径。

通过步骤203中计算得到的控制序列可以表示为：

式(11)中的

表示在k时刻预测的k+i时刻系统控制输入量，对应于前轮车辆转角或方向盘转角

本步骤中，优选的，将

的第一个量

作用于系统，控制车辆转角或方向盘转角，直到下一采样时刻到达。下一采样时刻到达时，重复步骤201至步骤204，得到下一采样时刻的

如此重复，在每一采样时刻进行计算，直到完成整个路径任务。

本步骤中，当前时刻k得到

使用

的第一个量

控制车辆。当下一采样时刻到达时，通过式(1)计算得到下一最优控制序列，使用下一最优控制序列的第一个量作为系统控制输入量，控制车辆行驶，直到下一采样时刻到达，继续计算下一最优控制序列，如此重复，直到完成整个路径任务。

在一个优选的实施例中，还包括：对式(1)中输入的系统状态，包括y(横向位置)、ψ(横摆角)，β(质心侧偏角)，

(横摆角变化率)，进行优化，并更新未建模动态矩阵参数F、H、G。优化后的系统状态作为式(1)中输入的初始状态，所得更新未建模动态矩阵参数作为下一时刻学习型预测模型的系数矩阵的参数。该优化和更新过程如图4所示，包括：

步骤41，用F、H、G中的参数扩展系统预测模型式(5)，得到一个新的扩展系统，如下式(7)所示：

x_aug表示扩大的系统预测状态，y_aug是与x_aug对应的输出，C是系统输出矩阵，O是零矩阵，O^T是O的转置矩阵，I是单位矩阵；

步骤42，对步骤41中f(·)和h(·)取关于x_aug的一阶偏导数，得到式(8)：

其中，

表示k时刻扩大的系统预测状态，

表示初步预测的k+1时刻扩大的系统预测状态。

步骤43，初步预测下一个采样区间的系统状态并更新估计误差协方差矩阵

如式(9)所示：

式(9)中：

dx_aug，k表示位于线性化邻域内的扰动，其取值范围为[0，1]，优先选择其中较小值，例如0-0.5范围内的值，并根据其实际应用例如调试效果确定具体值；

是k时刻误差协方差矩阵，初始值可取为区间[0，1]内值，例如随机值，其后根据式(10)更新得到；

是k+1时刻估计误差协方差矩阵，可由式(9)和

得到；

Q_k+1是由系统的不确定性引起的参数噪声的协方差，其值取值范围为(0，1)；

是系统初步预测输出的下一个采样时刻的系统状态。

步骤44，采用下式(10)正式预测下一个采样区间的系统状态，更新误差协方差矩阵；

式(10)中：

R_k+1是测量噪声协方差，其值取值范围为(0，1)，具体值可以根据实际需求在0～1之间预先确定；

K_k+1是卡尔曼增益由式(10)直接计算得到；

是k+1时刻误差协方差矩阵，由式(10)迭代推导；

是优化后的系统状态，用于步骤403式(1)中输入的初始状态。

在一个优选的实施方式中，

为28x1的矩阵，优化后的系统状态是

的前4个参数；

α₁至α₂₄的值为区间[0，1]中的随机数包括：α₁至α₂₄的初始值为区间[0，1]中的任意数，之后由

的后24个参数进行迭代更新。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (5)

1.一种自动驾驶车辆学习型路径跟踪预测控制方法，包括：

步骤1，在当前采样时刻，获取车辆的当前系统状态；

步骤2，获取所述车辆的未来N个期望状态，N为正整数；包括：对所述车辆的待行驶路径进行样条插值，以得到的新路径为参考路径；根据所述当前系统状态获取所述参考路径上距离所述车辆位置最近的路点P₀，以P₀为起点，v(k)ΔT为搜索距离，沿所述参考路径向前搜索获得N个参考路点P_i，i＝1，2，...，N，v(k)表示当前时刻k的车辆速度，ΔT为离散控制周期，所述N个参考路点的状态为所述N个期望状态；

步骤3，根据所述当前系统状态和学习型预测模型预测车辆未来状态，并与所述未来N个期望状态、预设的目标函数和系统约束结合，得到最优控制序列；

其中，所述预设的目标函数、所述学习型预测模型和所述系统约束包括：

式(1)中：

J表示所述预设的目标函数；

u_k+i是待求最优控制序列，表示前轮转角或方向盘转角；

表示实际求得的最优控制序列；

ε是松弛因子；

y_k+i是学习型预测模型的k+i时刻输出；其中所述学习型预测模型的表达式为：

x_k+1＝(A_d+F_k)x_k+(B_d+H_k)u_k+G_k (2)

其中，x_k为当前时刻k的系统状态，u_k代表k时刻的前轮转向角或方向盘转角δ；A_d、B_d是离散化后的系统参数矩阵，其值基于车辆纵向速度、前后轮侧偏刚度、前后轴到质心的距离、车辆质量和转动惯量得到；F_k、H_k和G_k是用于表征当前时刻k系统未建模动态的参数，其维度根据状态变量的个数确定，其值为0～1之间的随机数；F_k+i、H_k+i、G_k+i用于表征k+i时刻系统未建模动态的参数；

x_k+i+1是k时刻通过所述学习型预测模型预测的k+i+1时刻的系统状态；

x₀是所述车辆在所述路点P₀的系统状态；

r_k+i是k时刻的期望状态，为所述车辆的未来N个期望状态中的第i个期望状态；

Q，P，R，w_ε为预设权重系数，取值为大于0的实数；

C是预设系数，X是预设系统状态约束，D是预设未建模动态边界，W是预设终端状态约束，U是预设车辆控制约束，Δu_k+i表示的是相邻两个控制量u_k+i与u_k+i-1的差值；ΔU是预设车辆控制增量约束；

步骤4，使用求得的最优控制序列

中的第一个量

控制所述车辆的运行，直到下一采样时刻到达，重复步骤1-4，直到所述车辆行驶完整个路径。

2.根据权利要求1所述的方法，其特征在于，所述车辆的系统状态包括状态变量：所述车辆的横向位置y、横摆角度ψ、质心侧偏角β和横摆角度变化率

3.根据权利要求2所述的方法，其特征在于，其中，通过下述方式构建所述学习型预测模型，包括：

步骤11，建立系统名义模型：

式(3)中：

x表示系统状态；

表示表征所述系统状态的变量的变化率，包括车辆的横向位置变化率、横摆角速度、质心侧偏角变化率和横摆角加速度；

u表示前轮转向角或方向盘转角δ；

矩阵A、B中的参数如下：

式(4)中：

v是车辆纵向速度；

C_f是前轮侧偏刚度；

l_f是前轴到质心的距离；

C_r是后轮侧偏刚度；

l_r是后轴到质心的距离；

m是车辆质量；

I_Z为转动惯量；

步骤12，对步骤11中所述名义模型进行离散化并考虑扰动得到所述系统预测模型(5)；

x_k+1＝A_dx_k+B_du_k+d_k (5)

式(5)中：A_d、B_d是离散化后的系统参数矩阵，d_k是未建模动态引起的状态误差；

步骤13，构建系统未建模动力学模型：

d_k＝F_kx+H_ku+G_k (6)

式(6)中：

α₁至α₂₄的值为区间[0,1]中的随机数；

步骤14，合并式(5)和式(6)，以构建式(2)表示的所述学习型预测模型。

4.根据权利要求3所述的方法，其特征在于，还包括：优化系统状态与更新未建模动态矩阵参数：

用F_k、H_k、G_k中的参数扩展所述式(5)，得到：

x_aug表示扩大的系统预测状态，y_aug是与x_aug对应的输出，C是系统输出矩阵，O是零矩阵，O^T是O的转置矩阵，I是单位矩阵；

对式(7)中f(·)和h(·)取关于x_aug的一阶偏导数，得到：

其中，

表示k时刻扩大的系统预测状态，

表示初步预测的k+1时刻扩大的系统预测状态；

采用下式(9)初步预测下一个采样时刻的系统状态，并更新估计误差协方差矩阵

式(9)中：

表示所述初步预测的下一个采样时刻的系统状态；

是k时刻优化后的系统状态；

dx_aug，k表示位于线性化邻域内的扰动；

是k时刻误差协方差矩阵；

是k+1时刻估计误差协方差矩阵；

Q_k+1是由系统的不确定性引起的参数噪声的协方差；

是系统初步预测输出的下一个采样时刻的系统状态；

采用下式(10)正式预测所述下一个采样区间的系统状态，并更新误差协方差矩阵；

式(10)中：

R_k+1是测量噪声协方差；

K_k+1是卡尔曼增益；

是k+1时刻误差协方差矩阵；

是k+1时刻优化后的系统状态，用于所述步骤3中的所述当前系统状态。

5.根据权利要求4所述的方法，其特征在于，所述

为28x1的矩阵，所述优化后的系统状态是所述

的前4个参数；

所述α₁至α₂₄的值为区间[0,1]中的随机数包括：所述α₁至α₂₄的初始值为区间[0,1]中的任意数，之后由所述

的后24个参数进行迭代更新。

Images