CN114368381A - 一种基于横摆角速度预估的统一时序卡车横向控制方法 - Google Patents

Abstract

本发明涉及动车自动驾驶技术领域，尤其涉及一种基于横摆角速度预估的统一时序卡车横向控制方法，本发明根据卡车总重通过查询表定标确认此时的转向系统延迟，为应对不同载重下的不同转向系统延迟问题的解决提供关键参数输入；基于转向系统延迟参数预测了预瞄点处的车身的横摆角速度，进而与预瞄点道路信息结合，实现了道路信息与车辆位置的时序统一；采用经典的前馈加反馈的控制方法，将预瞄的前馈输入与基于模型的反馈输入结合，实现了最终的横向控制。

Description

一种基于横摆角速度预估的统一时序卡车横向控制方法

技术领域

本发明涉及动车自动驾驶技术领域，尤其涉及一种基于横摆角速度预估的统一时序卡车横向控制方法。

背景技术

半挂卡车，相比小型车，具有转向时车身响应较慢且响应延迟随载重变化较大的特点。其自动驾驶横向控制方法与小型车差别较大。

而当前自动驾驶半挂卡车横向控制方法，大多基于PID反馈控制或者基于模型的前馈加反馈的控制方法，来实现卡车的横向控制。其中，两大类方法大都采用了选取预瞄点的控制方法，即选取前方一定距离目标点的横向误差和航向误差作为反馈状态量。然而，以上方法虽然选取了前方的预瞄点作为反馈状态，但是并未选择未来在此预瞄点的车辆动力学和运动学参数作为车身的反馈状态。由于预瞄点与车身未统一时序，现有的卡车横向控制方法对不同载重下的不同转向延迟的适应性较差。

发明内容

(一)发明目的

为解决背景技术中存在的技术问题，本发明提出一种基于横摆角速度预估的统一时序卡车横向控制方法。

(二)技术方案

为解决上述问题，本发明提供了一种基于横摆角速度预估的统一时序卡车横向控制方法，包括以下步骤：

步骤S100，接收卡车轴荷传感器的数据，并估算整车总重和当前的牵引状态；

步骤S200，根据估算的整车总重和当前的牵引状态，估算二自由度动力学模型关键参数；

步骤S300，接收输入的卡车轨迹线参数，输出预瞄的轨迹点状态；

步骤S400，依据估算的整车总重和当前牵引状态，通过查询标定表，确定此载重下的转向系统延迟时间；

步骤S500，根据步骤S200中估算的二自由度动力学模型关键参数，预估卡车在预瞄点处的横摆角速度，并结合步骤S400中确定的转向系统延迟时间，积分计算出此段时间内的航向变化，根据步骤S300输出的预瞄的轨迹点状态，计算出统一时序后的预瞄点横向误差和航向误差及其变化率参数；

步骤S600，将步骤S200中估算的二自由度动力学模型关键参数和步骤S500中计算出的统一时序后的预瞄点横向误差和航向误差及其变化率参数，代入二自由度动力学模型中，计算前馈的控制输出量；

步骤S700，将步骤S200中估算的二自由度动力学模型关键参数和步骤S500中计算出的统一时序后的预瞄点横向误差和航向误差及其变化率参数，代入线性二次型控制器中，计算能使横向误差和航向误差取极小的反馈控制输出量；

步骤S800，将步骤S600中的前馈控制输出量与S700中的反馈控制输出量加和并输出，获得方向盘指令，发送给卡车的线控系统；

步骤S900，循环步骤S100至步骤S800，实现卡车横向控制。

优选的，所述步骤S100中，还包括卡车稳定行驶状态判断、轴荷信号滤波、轴荷信号学习、轴荷学习重置判断和轴荷信号到整车质量映射的计算。

优选的，所述步骤S200中，还包括车轴载荷分布计算、车轮侧偏刚度估算和卡车横摆转动惯量估算。

优选的，所述步骤S300中，还包括预瞄距离计算、预瞄点纵向位置计算和预瞄点状态取值。

优选的，所述步骤S500中，还包括以下步骤：

步骤S501，建立卡车二自由度动力学模型，拟将卡车的左右车轮分别合并在卡车车身的中轴线上，公式(1)如下：

其中，L表示卡车前轴到后轴的直线距离，δ表示前轮转角，R表示卡车在此时的瞬时转向半径，u为卡车的行驶速度，K为卡车的稳定性因素，其计算公式(2)如下：

其中，m为卡车质量；a为卡车质心位置距离前轴的距离；b为卡车质心位置距离后轴的距离；k₁、k₂分别为卡车前轮和后轮的侧偏刚度；

步骤S502，确认车轮转角，公式(3)如下：

其中，δ_PW为车轮转角，δ_P为上一个计算步长中步骤S800输出的方向盘转角，i为卡车的转向传动比；

根据确认的车轮转角δ_PW并结合步骤S200中估算的二自由度动力学模型关键参数，代入到步骤S501中公式(1)、公式(2)中，计算得到卡车的行驶曲率ρ_P，并基于如下公式(4)，估算卡车在一个转向延迟之后的横摆角速度ω_P；

ω_P＝u·ρ_P

步骤S503，基于步骤S400中输出的转向延迟时间t_d和步骤S502估算的横摆角速度ω_p，根据如下公式(5)积分计算此段时间预测航向角变化Δψ_P；

步骤S504，根据S502中的横摆角速度ω_p和S503中的航向角变化Δψ_P，根据如下公式(6)计算未来一个转向延迟时间t_d后卡车的横向位移ΔS_P；

ΔS_P＝t_d·u·sinΔψ_P

其中，u为卡车的行驶速度；

步骤S505，根据步骤S300中的轨迹点状态信息，结合步骤S502、步骤S503、步骤S504中统一时序后的卡车状态信息，根据如下公式(7)计算预瞄点横向误差和航向误差及其变化率参数矩阵E：

其中，e_{lat_v}＝e_r-ΔS_p；

θ_{ψ_v}＝θ_r-Δψ_P；θ_{ψ_v}＝θ_r-Δψ_P；e_r、θ_r分别为预瞄的轨迹点的横向距离和航向角。

优选的，所述步骤S600中计算前馈的控制输出量步骤如下：

取步骤S300中轨迹点状态中的目标曲率ρ_r，代入到步骤S501的二自由度动力学模型中，得到如下公式(12)：

δ_FF＝iρ_r(1+Ku²)L

其中，δ_FF即为前馈的控制输出量。

优选的，所述步骤S700中，还包括线性二次型控制的状态方程搭建、反馈增益矩阵的求解和最优反馈输出计算的过程：

线性二次型控制的状态方程，公式(13)：

其中，I_Z为卡车的横摆转动惯量，δ_w为卡车的车轮转角，ω_r为预瞄轨迹点的目标横摆角速度；

对公式(13)进行离散化，再根据里卡蒂方程求解中间矩阵；

利用中间矩阵计算反馈增益矩阵；

根据反馈增益矩阵和步骤S505中预瞄点横向误差和航向误差及其变化率参数矩阵E，计算能使横向误差和航向误差取极小的反馈控制输出量。

优选的，还包括转向零位修正、转向死区修正和输出平滑限制处理。

本发明的上述技术方案具有如下有益的技术效果：

本发明具有很强的适应性，并且在卡车不同载重不同转向延迟下，都能实现精确的横向控制；本发明根据卡车总重通过查询表定标确认此时的转向系统延迟，为应对不同载重下的不同转向系统延迟问题的解决提供关键参数输入；

本发明基于转向系统延迟参数预测了预瞄点处的车身的横摆角速度，进而与预瞄点道路信息结合，实现了道路信息与车辆位置的时序统一；

本发明采用经典的前馈加反馈的控制方法，将预瞄的前馈输入与基于模型的反馈输入结合，实现了最终的横向控制。

附图说明

图1为本发明流程框图；

图2为本发明卡车二自由度动力学模型示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要的混淆本发明的概念。

如图1所示，本发明提出的一种基于横摆角速度预估的统一时序卡车横向控制方法，包括步骤：步骤S100接收卡车轴荷传感器的数据，并估算整车总重和当前的牵引状态；步骤S200根据估算的整车总重和当前的牵引状态，估算二自由度动力学模型关键参数；步骤S300接收输入的卡车轨迹线参数，输出预瞄的轨迹点状态；步骤S400依据估算的整车总重和当前牵引状态，通过查询标定表，确定此载重下的转向系统延迟时间；步骤S500根据步骤S200中估算的二自由度动力学模型关键参数，预估卡车在预瞄点处的横摆角速度，并结合步骤S400中确定的转向系统延迟时间，积分计算出此段时间内的航向变化，根据步骤S300输出的预瞄的轨迹点状态，计算出统一时序后的预瞄点横向误差和航向误差及其变化率参数；

参考图2，在步骤S500中，还包括：步骤S501，建立如图2所示的卡车二自由度动力学模型，图2中将卡车的左右车轮分别合并在卡车车身的中轴线上，构成卡车的二自由度模型，其中，其中L表示卡车前轴到后轴的直线距离，δ表示前轮转角，α_f表示前轮侧偏角，α_r表示后轮侧偏角，R表示卡车在此时的瞬时转向半径，O_v表示卡车的质心位置，O_r为卡车在此时的瞬时转向中心位置；

则卡车在此时的行驶曲率ρ满足以下关系，公式(1)：

式中，u为卡车的行驶速度，K为卡车的稳定性因素，其计算公式(2)如下：

其中，m为卡车质量；a为卡车质心位置距离前轴的距离；b为卡车质心位置距离后轴的距离；k₁、k₂分别为卡车前轮和后轮的侧偏刚度；

步骤S502，根据上一个计算步长中步骤S800输出的方向盘转角δ_P(特殊地，在第一次运行时，代入方向盘转角值为0)和卡车的转向传动比i，通过如下公式(3)确认车轮转角δ_PW：

根据确认的车轮转角δ_PW结合步骤S200中估算的二自由度动力学模型关键参数，代入到步骤S501中公式(1)、公式(2)中，计算得到卡车的行驶曲率ρ_P，并基于如下公式(4)，估算卡车在一个转向延迟之后的横摆角速度ω_P；

ω_P＝u·ρ_P

步骤S503，基于步骤S400中输出的转向延迟时间t_d和步骤S502估算的横摆角速度ω_p，根据如下公式(5)积分计算此段时间预测航向角变化Δψ_P；

步骤S504，根据S502中的横摆角速度ω_p和S503中的航向角变化Δψ_P，根据如下公式(6)计算未来一个转向延迟时间t_d后卡车的横向位移ΔS_P；

ΔS_P＝t_d·u·sinΔψ_P

其中，u为卡车的行驶速度；

步骤S505，根据步骤S300中的轨迹点状态信息，结合步骤S502、步骤S503、步骤S504中统一时序后的卡车状态信息，根据如下公式(7)计算预瞄点横向误差和航向误差及其变化率参数矩阵E：

其中，e_{lat_v}＝e_r-ΔS_p；

θ_{ψ_v}＝θ_r-Δψ_P；θ_{ψ_v}＝θ_r-Δψ_P；e_r、θ_r分别为预瞄的轨迹点的横向距离和航向角。

步骤S600，将步骤S200中估算的二自由度动力学模型关键参数和步骤S500中计算出的统一时序后的预瞄点横向误差和航向误差及其变化率参数，代入二自由度动力学模型中，计算前馈的控制输出量；

具体的，所述步骤S600中计算前馈的控制输出量步骤如下：

取步骤S300中轨迹点状态中的目标曲率ρ_r，代入到步骤S501的二自由度动力学模型中，得到如下公式(12)：

δ_FF＝iρ_r(1+Ku²)L

其中，δ_FF即为前馈的控制输出量。

步骤S700，将步骤S200中估算的二自由度动力学模型关键参数和步骤S500中计算出的统一时序后的预瞄点横向误差和航向误差及其变化率参数，代入线性二次型控制器中，计算能使横向误差和航向误差取极小的反馈控制输出量；

具体的，所述步骤S700中，还包括线性二次型控制的状态方程搭建、反馈增益矩阵的求解和最优反馈输出计算的过程：

线性二次型控制的状态方程，公式(13)：

其中，I_Z为卡车的横摆转动惯量，δ_w为卡车的车轮转角，ω_r为预瞄轨迹点的目标横摆角速度；

为简化表示，令：

则对公式(13)进行离散化后：

A_D＝I+AT

B_D＝B₁T

其中，A_D为离散后的状态矩阵，B_D为离散后的控制矩阵，T为本发明的运算步长时间，I为4*4的单位矩阵。

再根据里卡蒂方程求解中间矩阵P；

利用中间矩阵P计算反馈增益矩阵K_LQR；

其中，Q为状态量的对角加权矩阵，R_LQR为控制量的对角加权矩阵，两者的结构分别如下：

R_LQR＝r

其中，q₁、q₂、q₃、q₄分别为各个状态量的权重系数，r为控制量的权重系数，以上五个系数皆为标定参数；

根据反馈增益矩阵K_LQR和步骤S505中预瞄点横向误差和航向误差及其变化率参数矩阵E，计算能使横向误差和航向误差取极小的反馈控制输出量δ_FB，即：

δ_FB＝iK_LQRE

步骤S800，将步骤S600中的前馈控制输出量与S700中的反馈控制输出量加和并输出，获得方向盘指令，发送给卡车的线控系统；

步骤S900，循环步骤S100至步骤S800，实现卡车横向控制。

在本发明一个实施例中，所述步骤S100中，还包括卡车稳定行驶状态判断、轴荷信号滤波、轴荷信号学习、轴荷学习重置判断和轴荷信号到整车质量映射的计算。在步骤S200中还包括车轴载荷分布计算、车轮侧偏刚度估算和卡车横摆转动惯量估算。在步骤S300中还包括预瞄距离计算、预瞄点纵向位置计算和预瞄点状态取值。在步骤S800中还包括转向零位修正、转向死区修正和输出平滑限制处理。

本发明达到的技术效果：

根据卡车总重通过查询表定标确认此时的转向系统延迟，为应对不同载重下的不同转向系统延迟问题的解决提供关键参数输入；其次，本发明基于转向系统延迟参数预测了预瞄点处的车身的横摆角速度，进而与预瞄点道路信息结合，实现了道路信息与车辆位置的时序统一；最终，本发明采用经典的前馈加反馈的控制方法，将预瞄的前馈输入与基于模型的反馈输入结合，实现了最终的横向控制。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的仅为本发明的优选例，并不用来限制本发明，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。

Claims (8)

1.一种基于横摆角速度预估的统一时序卡车横向控制方法，其特征在于，包括以下步骤：

步骤S100，接收卡车轴荷传感器的数据，并估算整车总重和当前的牵引状态；

步骤S200，根据估算的整车总重和当前的牵引状态，估算二自由度动力学模型关键参数；

步骤S300，接收输入的卡车轨迹线参数，输出预瞄的轨迹点状态；

步骤S400，依据估算的整车总重和当前牵引状态，通过查询标定表，确定此载重下的转向系统延迟时间；

步骤S500，根据步骤S200中估算的二自由度动力学模型关键参数，预估卡车在预瞄点处的横摆角速度，并结合步骤S400中确定的转向系统延迟时间，积分计算出此段时间内的航向变化，根据步骤S300输出的预瞄的轨迹点状态，计算出统一时序后的预瞄点横向误差和航向误差及其变化率参数；

步骤S600，将步骤S200中估算的二自由度动力学模型关键参数和步骤S500中计算出的统一时序后的预瞄点横向误差和航向误差及其变化率参数，代入二自由度动力学模型中，计算前馈的控制输出量；

步骤S700，将步骤S200中估算的二自由度动力学模型关键参数和步骤S500中计算出的统一时序后的预瞄点横向误差和航向误差及其变化率参数，代入线性二次型控制器中，计算能使横向误差和航向误差取极小的反馈控制输出量；

步骤S800，将步骤S600中的前馈控制输出量与S700中的反馈控制输出量加和并输出，获得方向盘指令，发送给卡车的线控系统；

步骤S900，循环步骤S100至步骤S800，实现卡车横向控制。

2.根据权利要求1所述的一种基于横摆角速度预估的统一时序卡车横向控制方法，其特征在于，所述步骤S100中，还包括卡车稳定行驶状态判断、轴荷信号滤波、轴荷信号学习、轴荷学习重置判断和轴荷信号到整车质量映射的计算。

3.根据权利要求1所述的一种基于横摆角速度预估的统一时序卡车横向控制方法，其特征在于，所述步骤S200中，还包括车轴载荷分布计算、车轮侧偏刚度估算和卡车横摆转动惯量估算。

4.根据权利要求1所述的一种基于横摆角速度预估的统一时序卡车横向控制方法，其特征在于，所述步骤S300中，还包括预瞄距离计算、预瞄点纵向位置计算和预瞄点状态取值。

5.根据权利要求1所述的一种基于横摆角速度预估的统一时序卡车横向控制方法，其特征在于，所述步骤S500中，还包括以下步骤：

步骤S501，建立卡车二自由度动力学模型，拟将卡车的左右车轮分别合并在卡车车身的中轴线上，公式(1)如下：

其中，L表示卡车前轴到后轴的直线距离，δ表示前轮转角，R表示卡车在此时的瞬时转向半径，u为卡车的行驶速度，K为卡车的稳定性因素，其计算公式(2)如下：

其中，m为卡车质量；a为卡车质心位置距离前轴的距离；b为卡车质心位置距离后轴的距离；k₁、k₂分别为卡车前轮和后轮的侧偏刚度；

步骤S502，确认车轮转角，公式(3)如下：

其中，δ_PW为车轮转角，δ_P为上一个计算步长中步骤S800输出的方向盘转角，i为卡车的转向传动比；

根据确认的车轮转角δ_PW并结合步骤S200中估算的二自由度动力学模型关键参数，代入到步骤S501中公式(1)、公式(2)中，计算得到卡车的行驶曲率ρ_P，并基于如下公式(4)，估算卡车在一个转向延迟之后的横摆角速度ω_P；

ω_P＝u·ρ_P

步骤S503，基于步骤S400中输出的转向延迟时间t_d和步骤S502估算的横摆角速度ω_p，根据如下公式(5)积分计算此段时间预测航向角变化Δψ_P；

步骤S504，根据S502中的横摆角速度ω_p和S503中的航向角变化Δψ_P，根据如下公式(6)计算未来一个转向延迟时间t_d后卡车的横向位移ΔS_P；

ΔS_P＝t_d·u·sinΔψ_P

其中，u为卡车的行驶速度；

步骤S505，根据步骤S300中的轨迹点状态信息，结合步骤S502、步骤S503、步骤S504中统一时序后的卡车状态信息，根据如下公式(7)计算预瞄点横向误差和航向误差及其变化率参数矩阵E：

其中，e_{lat_v}＝e_r-ΔS_p；

θ_{ψ_v}＝θ_r-Δψ_P；θ_{ψ_v}＝θ_r-Δψ_P；e_r、θ_r分别为预瞄的轨迹点的横向距离和航向角。

6.根据权利要求5所述的一种基于横摆角速度预估的统一时序卡车横向控制方法，其特征在于，所述步骤S600中计算前馈的控制输出量步骤如下：

取步骤S300中轨迹点状态中的目标曲率ρ_r，代入到步骤S501的二自由度动力学模型中，得到如下公式(12)：

δ_FF＝iρ_r(1+Ku²)L

其中，δ_FF即为前馈的控制输出量。

7.根据权利要求5所述的一种基于横摆角速度预估的统一时序卡车横向控制方法，其特征在于，所述步骤S700中，还包括线性二次型控制的状态方程搭建、反馈增益矩阵的求解和最优反馈输出计算的过程：

线性二次型控制的状态方程，公式(13)：

其中，I_Z为卡车的横摆转动惯量，δ_w为卡车的车轮转角，ω_r为预瞄轨迹点的目标横摆角速度；

对公式(13)进行离散化，再根据里卡蒂方程求解中间矩阵；

利用中间矩阵计算反馈增益矩阵；

根据反馈增益矩阵和步骤S505中预瞄点横向误差和航向误差及其变化率参数矩阵E，计算能使横向误差和航向误差取极小的反馈控制输出量。

8.根据权利要求1所述的一种基于横摆角速度预估的统一时序卡车横向控制方法，其特征在于，所述步骤S800中还包括转向零位修正、转向死区修正和输出平滑限制处理。

Images