CN117302190A - 一种面向自动驾驶卡车的预瞄跟踪控制系统及控制方法 - Google Patents

Abstract

本发明公开了一种面向自动驾驶卡车的预瞄跟踪控制系统及控制方法，本系统考虑长轴距卡车的动力学特性，建立二自由度卡车动力学模型，考虑整车载重变化对整车质心位置和横摆转动惯量的影响，以及各轴垂向载荷和轮胎侧偏刚度的变化。通过PLQR控制理论推导，既考虑当前点的横向位置误差和航向位置误差，同时考虑泊车轨迹点的未来道路曲率信息，实现对长轴距卡车的泊车控制。

Description

一种面向自动驾驶卡车的预瞄跟踪控制系统及控制方法

技术领域

本发明属于自动驾驶技术领域，特别涉及一种面向自动驾驶卡车的预瞄跟踪控制系统，及其控制方法。

背景技术

厢式载货车由于轴距长、车体宽的原因，对于狭窄垂直库位的泊入，相对于乘用车需求更加精准的运动控制。尤其定位跳变及信号噪声会影响规划引导线曲率的抖动，进而影响方向盘转角输出的平滑性及车辆泊入的稳定性。

常见的泊车运动控制算法普遍存在以下问题：

(1)基于运动学模型设计控制器，无法考虑车辆的动力学复杂特性；

(2)仅考虑预瞄的单一轨迹点，未充分利用未来轨迹信息。

针对现有技术存在的不足，本发明提供一种自动驾驶卡车泊车预瞄跟踪控制方法，既考虑车辆的动力学特性，同时考虑未来泊车轨迹点信息，保证跟踪控制的精准性及稳定性。

公开于该背景技术部分的信息仅仅旨在增加对本发明的总体背景的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域一般技术人员所公知的现有技术。

发明内容

本发明的目的在于提供一种面向自动驾驶卡车的预瞄跟踪控制系统，从而克服上述现有技术中的缺陷。

为实现上述目的，本发明提供了一种面向自动驾驶卡车的预瞄跟踪控制系统，主要包括：

泊车轨迹点信息处理模块，所述的泊车轨迹点信息处理模块根据上游规划输出的时空轨迹点计算控制所需的状态量，包含横向位置误差和航向位置偏差；

动力学参数求解模块，所述的动力学参数求解模块根据整车载荷分配计算车辆质心位置、横摆转动惯量、垂直载荷及对应的轮胎侧偏刚度；

PLQR控制器求解模块、所述的PLQR控制器求解模块根据前述模块计算的状态量及动力学参数，建立考虑未来泊车轨迹点的动力学模型，求解所需的方向盘转角输出；

控制输出约束模块，控制输出约束模块根据实际转向性能约束平滑转角输出，得到稳定且平滑的方向盘输出。

泊车轨迹点信息处理模块和动力学参数求解模块通讯连接PLQR控制器求解模块，PLQR控制器求解模块通讯连接控制输出约束模块，控制输出约束模块通讯连接EPS执行器

根据长轴距卡车的动力学特性，建立二自由度卡车动力学模型，考虑整车载重变化对整车质心位置和横摆转动惯量的影响，以及各轴垂向载荷和轮胎侧偏刚度的变化；通过PLQR控制理论推导，既考虑当前点的横向位置误差和航向位置误差，又考虑泊车轨迹点的未来道路曲率信息，实现对长轴距卡车的泊车控制。

一种面向自动驾驶卡车的预瞄跟踪控制方法，其特征在于：包括如权1所述的预瞄跟踪控制系统，轨迹点信息处理模块中的控制模块收到来自上游的时空轨迹点信息，其中包含泊车路径每一点的坐标信息(x，y)、参考航向角信息theta、参考曲率信息kappa、弧长s、参考车速v、参考加速度a，自车实时的定位信息(posX，posY)以及自车的航向角信息yaw，同时由于存在规划与定位信息的发送频率不同，先采用时间戳插值的方法求解距离自车最近的参考点，再通过投影计算出控制所需的横向位置误差及航向误差，输出给PLQR控制器求解模块；

动力学参数求解模块控制根据实际卡车前后轴垂向载荷m_f、m_r、前后轴轴距L，计算出所需的横摆转动惯量I_Z：

I_Z＝m_f×l_f ²+m_r×l_r ² (2-3)

轮胎侧偏刚度的主要影响因素为：垂向载荷和路面附着系数。本发明假设轮胎侧偏刚度为垂向载荷的三次多项式函数，通过已知的各轮胎垂向载荷，可以得到对应的轮胎侧偏刚度，即：

k＝p₀+p₁m_z+p₂m_z ²+p₃m_z ³ (2-4)

式中，k表示轮胎侧偏刚度，m_z表示车轮的垂向载荷，p₀、p₁、p₂和p₃为三次多项式的拟合参数。

PLQR控制器求解模块控制二自由度误差动力学模型推导：

定义车辆在跟踪参考轨迹过程中产生的误差：e_y为横向误差，即车辆质心距参考点的距离；为横向误差变化率，即横向速度误差；/>为航向误差，即车辆的航向角与参考轨迹点处航向角的偏差；/>为航向角误差的变化率，即航向角速度误差；

将泊车过程中车辆的位置与参考时空轨迹点之间的横向误差e_y、横向速度误差航向角误差/>以及航向角速度误差/>作为状态变量，建立基于跟踪误差的动力学模型；

假设短时间内车辆位置和速度恒定，车辆转至期望角度时的横向加速度为：

因此横向加速度误差为：

横向速度误差为：

航向角加速度误差为：

航向角速度误差为：

式中：v_x为车辆纵向车速，为参考点的航向角；

根据牛顿第二定律，分析车辆横向和横摆运动，建立如下动力学方程：

ma_y＝2F_yf+2F_yr (3-6)

式中，m为厢式货车的质量；a_y为车辆质心在y轴方向的加速度；F_yf、F_yr为车辆前、后轮受到的侧向力；I_z为车辆绕z轴的转动惯量；为车辆在惯性坐标系下的航向角；l_f、l_r为车辆前、后轴中心到质心的距离。

车辆在y轴方向的横向加速度分解为横向位移加速度和向心加速度：

式中，y为车辆沿y轴方向的横向位移，v_x为沿x轴方向的纵向速度；

在侧偏角及纵向滑移率较小时，车轮受到的侧向力可以用线性函数近似描述：

F_yf＝-C_fα_f (3-9)

F_yr＝-C_rα_r (3-10)

式中，C_f、G_f分别为前后轮侧偏刚度；α_f、α_r为前后车轮轮胎侧偏角；

在小角度下，轮胎侧偏角近似计算为：

将(3-8)至(3-10)带入(3-6)和(3-7)中，可得：

将(3-2)和(3-5)带入(3-13)和(3-14)中得

其中u＝δ，w＝ρ_r

PLQR控制器求解模块预测LQR控制器设计：考虑未来k时刻到k+N时刻泊车参考轨迹点的曲率变化，则式(3-17)改写为：

其中状态变量为

式中，ρ_r＝[k_r(k) k_r(k+1) k_r(k+2)…k_r(k+N)]，k_r(k)为k时刻的泊车参考轨迹点曲率，

定义预测LQR控制的代价方程为：

u_min≤u≤u_max

其中，

则最优反馈控制为：

其中，

黎卡提迭代方程为：

将迭代矩阵表达成如下：

反馈增益矩阵可以改写为：

因此最优反馈控制量为：

由上式可得预测LQR控制包含两部分：考虑车辆状态的系统反馈和考虑参考未来轨迹点曲率的前馈，求得所需的控制量；

控制输出约束模块对实际前轮转角进行约束；考虑卡车的实际侧倾安全，实际前轮转角约束为：

式中，h₁为卡车的质心高度，B₁为卡车的轮距，G_γ为横摆角速度稳态增益

同时考虑实车的转向性能，对输出的转角变化率进行约束，保证输出的平滑性及安全性

优选地，上述技术方案中，泊车轨迹点信息处理模块的输入为规划的时空轨迹点(x，y，theta，kappa，s，v，a)、定位信息(posX，posY，Yaw)；输出为自车相对时空轨迹点的横向位置误差、航向误差；其中由于规划与定位的运行频率不一致，通过线性插值匹配到时空最近点。

优选地，上述技术方案中，动力学参数求解模块的输入为整车轴距、前后轴载荷质量；输出为车辆的质心点位置、横摆转动惯量以及轮胎侧偏刚度；其中轮胎侧偏刚度与垂向载荷的关系用三次多项式表征。

优选地，上述技术方案中，PLQR控制器求解模块，输入为上述两个模块的输出：横向位置误差、航向误差、横摆转动惯量、轮胎侧偏刚度、垂向载荷及车速信息，通过建立的考虑未来泊车轨迹曲率的扩维动力学模型，建立考虑状态误差及控制输出为目标的代价函数，采用黎卡提方程迭代实时求解反馈增益矩阵和前馈增益矩阵，通过反馈增益矩阵与状态量计算反馈的前轮转角，通过前馈增益矩阵与未来泊车轨迹曲率信息计算前馈的前轮转角，两者叠加计算所需的前轮转角，再通过前轮转角与实际方向盘转角的映射关系得到控制所需的目标方向盘转角。

优选地，上述技术方案中，控制输出约束模块基于静态侧倾理论计算的前轮转角极限约束前轮转角的幅值，通过对执行器性能的响应特性，约束输出的方向盘转角变化速率，保证输出的平滑性及安全性。

与现有技术相比，本发明具有如下有益效果：

考虑车辆的动力学特性，保证泊车整体的稳定性及鲁棒性；考虑未来泊车轨迹点信息，保证车辆控制的平滑性和精准性。所提出的泊车预瞄跟踪控制方法，为自动驾驶卡车的泊车控制开拓了新思路。

附图说明：

图1为整体控制架构示意图；

图2为泊车示意图。

具体实施方式：

下面对本发明的具体实施方式进行详细描述，但应当理解本发明的保护范围并不受具体实施方式的限制。

除非另有其它明确表示，否则在整个说明书和权利要求书中，术语“包括”或其变换如“包含”或“包括有”等等将被理解为包括所陈述的元件或组成部分，而并未排除其它元件或其它组成部分。

本发明中的一种面向自动驾驶卡车泊车的预瞄跟踪控制方法的整体架构的具体实施流程见图1，实现思路如下：

1、轨迹点信息处理模块，控制模块收到来自上游的时空轨迹点信息，其中包含泊车路径每一点的坐标信息(x，y)、参考航向角信息theta、参考曲率信息kappa、弧长s、参考车速v、参考加速度a，自车实时的定位信息(posX，posY)以及自车的航向角信息yaw，同时由于存在规划与定位信息的发送频率不同，先采用时间戳插值的方法求解距离自车最近的参考点，再通过投影计算出控制所需的横向位置误差及航向误差，输出给PLQR控制器求解模块。

2、动力学参数求解模块

控制根据实际卡车前后轴垂向载荷m_f、m_r、前后轴轴距L，计算出所需的横摆转动惯量I_z：

I_z＝m_f×l_f ²+m_r×l_r ² (2-3)

轮胎侧偏刚度的主要影响因素为：垂向载荷和路面附着系数。本发明假设轮胎侧偏刚度为垂向载荷的三次多项式函数，通过已知的各轮胎垂向载荷，可以得到对应的轮胎侧偏刚度，即：

k＝p₀+p₁m_z+p₂m_z ²+p₃m_z ³ (2-4)

式中，k表示轮胎侧偏刚度，m_z表示车轮的垂向载荷，p₀、p₁、p₂和p₃为三次多项式的拟合参数。

3、PLQR控制器求解模块

3.1、二自由度误差动力学模型推导

定义车辆在跟踪参考轨迹过程中产生的误差：e_y为横向误差，即车辆质心距参考点的距离；为横向误差变化率，即横向速度误差；/>为航向误差，即车辆的航向角与参考轨迹点处航向角的偏差；/>为航向角误差的变化率，即航向角速度误差。

本发明将泊车过程中车辆的位置与参考时空轨迹点之间的横向误差e_y、横向速度误差航向角误差/>以及航向角速度误差/>作为状态变量，建立基于跟踪误差的动力学模型。

假设短时间内车辆位置和速度恒定，车辆转至期望角度时的横向加速度为：

因此横向加速度误差为：

横向速度误差为：

航向角加速度误差为：

航向角速度误差为：

式中：v_x为车辆纵向车速，为参考点的航向角。

根据牛顿第二定律，分析车辆横向和横摆运动，建立如下动力学方程：

ma_y＝2F_yf+2F_yr (3-6)

式中，m为厢式货车的质量；a_y为车辆质心在y轴方向的加速度；F_yf、F_yr为车辆前、后轮受到的侧向力；I_z为车辆绕z轴的转动惯量；为车辆在惯性坐标系下的航向角；l_f、l_r为车辆前、后轴中心到质心的距离。

车辆在y轴方向的横向加速度分解为横向位移加速度和向心加速度：

式中，y为车辆沿y轴方向的横向位移，v_x为沿x轴方向的纵向速度。

在侧偏角及纵向滑移率较小时，车轮受到的侧向力可以用线性函数近似描述：

F_yf＝-C_fα_f (3-9)

F_yr＝-C_rα_r (3-10)

式中，C_f、G_f分别为前后轮侧偏刚度；α_f、α_r为前后车轮轮胎侧偏角。

在小角度下，轮胎侧偏角近似计算为：

将(3-8)至(3-10)带入(3-6)和(3-7)中，可得：

将(3-2)和(3-5)带入(3-13)和(3-14)中得

其中u＝δ，w＝ρ_r

3.2预测LQR控制器设计

考虑未来k时刻到k+N时刻泊车参考轨迹点的曲率变化，则式(3-17)改写为：

其中状态变量为

式中，ρ_r＝[k_r(k) k_r(k+1) k_r(k+2)…K_r(k+N)]，k_r(k)为k时刻的泊车参考轨迹点曲率，

定义预测LQR控制的代价方程为：

u_min≤u≤u_max

其中，

则最优反馈控制为：

其中，

黎卡提迭代方程为：

将迭代矩阵表达成如下：

反馈增益矩阵可以改写为：

因此最优反馈控制量为：

由上式可得预测LQR控制包含两部分：考虑车辆状态的系统反馈和考虑参考未来轨迹点曲率的前馈，求得所需的控制量

4、控制输出约束模块

为了保证卡车的侧倾安全性，本发明中对实际前轮转角进行约束。因此，考虑卡车的实际侧倾安全，实际前轮转角约束为：

式中，h₁为卡车的质心高度，B₁为卡车的轮距，G_γ为横摆角速度稳态增益

同时考虑实车的转向性能，对输出的转角变化率进行约束，保证输出的平滑性及安全性。

前述对本发明的具体示例性实施方案的描述是为了说明和例证的目的。这些描述并非想将本发明限定为所公开的精确形式，并且很显然，根据上述教导，可以进行很多改变和变化。对示例性实施例进行选择和描述的目的在于解释本发明的特定原理及其实际应用，从而使得本领域的技术人员能够实现并利用本发明的各种不同的示例性实施方案以及各种不同的选择和改变。本发明的范围意在由权利要求书及其等同形式所限定。

Claims (6)

1.一种面向自动驾驶卡车的预瞄跟踪控制系统，其特征在于：主要包括：

泊车轨迹点信息处理模块，泊车轨迹点信息处理模块根据上游规划输出的时空轨迹点计算控制所需的状态量，包含横向位置误差和航向位置偏差；

动力学参数求解模块，动力学参数求解模块根据整车载荷分配计算车辆质心位置、横摆转动惯量、垂直载荷及对应的轮胎侧偏刚度；

PLQR控制器求解模块，PLQR控制器求解模块根据前述模块计算的状态量及动力学参数，建立考虑未来泊车轨迹点的动力学模型，求解所需的方向盘转角输出；

控制输出约束模块，控制输出约束模块根据实际转向性能约束平滑转角输出，得到稳定且平滑的方向盘输出；

泊车轨迹点信息处理模块和动力学参数求解模块通讯连接PLQR控制器求解模块，PLQR控制器求解模块通讯连接控制输出约束模块，控制输出约束模块通讯连接EPS执行器。

2.一种面向自动驾驶卡车的预瞄跟踪控制方法，其特征在于：包括如权1所述的预瞄跟踪控制系统，轨迹点信息处理模块中的控制模块收到来自上游的时空轨迹点信息，其中包含泊车路径每一点的坐标信息(x，y)、参考航向角信息theta、参考曲率信息kappa、弧长s、参考车速v、参考加速度a，自车实时的定位信息(posX，posY)以及自车的航向角信息yaw，同时由于存在规划与定位信息的发送频率不同，先采用时间戳插值的方法求解距离自车最近的参考点，再通过投影计算出控制所需的横向位置误差及航向误差，输出给PLQR控制器求解模块；

动力学参数求解模块控制根据实际卡车前后轴垂向载荷m_f、m_r、前后轴轴距L，计算出所需的横摆转动惯量I_z：

I_Z＝m_f×l_f ²+m_r×l_r ² (2-3)

轮胎侧偏刚度的主要影响因素为：垂向载荷和路面附着系数。本发明假设轮胎侧偏刚度为垂向载荷的三次多项式函数，通过已知的各轮胎垂向载荷，可以得到对应的轮胎侧偏刚度，即：

k＝p₀+p₁m_z+p₂m_z ²+p₃m_z ³ (2-4)

式中，k表示轮胎侧偏刚度，m_z表示车轮的垂向载荷，p₀、p₁、p₂和p₃为三次多项式的拟合参数。

PLQR控制器求解模块控制二自由度误差动力学模型推导：

定义车辆在跟踪参考轨迹过程中产生的误差：e_y为横向误差，即车辆质心距参考点的距离；为横向误差变化率，即横向速度误差；/>为航向误差，即车辆的航向角与参考轨迹点处航向角的偏差；/>为航向角误差的变化率，即航向角速度误差；

将泊车过程中车辆的位置与参考时空轨迹点之间的横向误差e_y、横向速度误差航向角误差/>以及航向角速度误差/>作为状态变量，建立基于跟踪误差的动力学模型；

假设短时间内车辆位置和速度恒定，车辆转至期望角度时的横向加速度为：

因此横向加速度误差为：

横向速度误差为：

航向角加速度误差为：

航向角速度误差为：

式中：v_x为车辆纵向车速，为参考点的航向角；

根据牛顿第二定律，分析车辆横向和横摆运动，建立如下动力学方程：

ma_y＝2F_yf+2F_yr (3-6)

式中，m为厢式货车的质量；a_y为车辆质心在y轴方向的加速度；F_yf、F_yr为车辆前、后轮受到的侧向力；I_z为车辆绕z轴的转动惯量；为车辆在惯性坐标系下的航向角；l_f、l_r为车辆前、后轴中心到质心的距离。

车辆在y轴方向的横向加速度分解为横向位移加速度和向心加速度：

式中，y为车辆沿y轴方向的横向位移，v_x为沿x轴方向的纵向速度；

在侧偏角及纵向滑移率较小时，车轮受到的侧向力可以用线性函数近似描述：

F_yf＝-C_fα_f (3-9)

F_yr＝-C_rα_r (3-10)

式中，C_f、C_f分别为前后轮侧偏刚度；α_f、α_r为前后车轮轮胎侧偏角；

在小角度下，轮胎侧偏角近似计算为：

将(3-8)至(3-10)带入(3-6)和(3-7)中，可得：

将(3-2)和(3-5)带入(3-13)和(3-14)中得

其中u＝δ，w＝ρ_r

PLQR控制器求解模块预测LQR控制器设计：考虑未来k时刻到k+N时刻泊车参考轨迹点的曲率变化，则式(3-17)改写为：

其中状态变量为

式中，ρ_r＝[k_r(k) k_r(k+1) k_r(k+2) … k_r(k+N)]，k_r(k)为k时刻的泊车参考轨迹点曲率，

定义预测LQR控制的代价方程为：

u_min≤u≤u_max

其中，

则最优反馈控制为：

其中，

黎卡提迭代方程为：

将迭代矩阵表达成如下：

反馈增益矩阵可以改写为：

因此最优反馈控制量为：

由上式可得预测LQR控制包含两部分：考虑车辆状态的系统反馈和考虑参考未来轨迹点曲率的前馈，求得所需的控制量；

控制输出约束模块对实际前轮转角进行约束；考虑卡车的实际侧倾安全，实际前轮转角约束为：

式中，h₁为卡车的质心高度，B₁为卡车的轮距，G_γ为横摆角速度稳态增益

同时考虑实车的转向性能，对输出的转角变化率δ进行约束，保证输出的平滑性及安全性

3.根据权利要求2所述的面向自动驾驶卡车的预瞄跟踪控制方法，其特征在于：泊车轨迹点信息处理模块的输入为规划的时空轨迹点(x，y，theta，kappa，s，v，a)、定位信息(posX，posY，Yaw)；输出为自车相对时空轨迹点的横向位置误差、航向误差；其中由于规划与定位的运行频率不一致，通过线性插值匹配到时空最近点。

4.根据权利要求2所述的面向自动驾驶卡车的预瞄跟踪控制方法，其特征在于：动力学参数求解模块的输入为整车轴距、前后轴载荷质量；输出为车辆的质心点位置、横摆转动惯量以及轮胎侧偏刚度；其中轮胎侧偏刚度与垂向载荷的关系用三次多项式表征。

5.根据权利要求2所述的面向自动驾驶卡车的预瞄跟踪控制方法，其特征在于：PLQR控制器求解模块，输入为上述两个模块的输出：横向位置误差、航向误差、横摆转动惯量、轮胎侧偏刚度、垂向载荷及车速信息，通过建立的考虑未来泊车轨迹曲率的扩维动力学模型，建立考虑状态误差及控制输出为目标的代价函数，采用黎卡提方程迭代实时求解反馈增益矩阵和前馈增益矩阵，通过反馈增益矩阵与状态量计算反馈的前轮转角，通过前馈增益矩阵与未来泊车轨迹曲率信息计算前馈的前轮转角，两者叠加计算所需的前轮转角，再通过前轮转角与实际方向盘转角的映射关系得到控制所需的目标方向盘转角。

6.根据权利要求2所述的面向自动驾驶卡车的预瞄跟踪控制方法，其特征在于：控制输出约束模块基于静态侧倾理论计算的前轮转角极限约束前轮转角的幅值，通过对执行器性能的响应特性，约束输出的方向盘转角变化速率，保证输出的平滑性及安全性。