CN113815602B - 一种车辆横向自适应控制方法 - Google Patents

Abstract

本发明提供了一种车辆横向自适应控制方法，其利用有限状态机根据车速及状态转移时间间隔，实现了在不同控制算法之间的轮换，由于不同算法能够工作和取得最佳控制效果的速度区间不同，因此这种轮换控制方式足以保证车辆在全速度区间上控制器的稳定；上述LQR‑MPC联合控制算法对应的速度控制区间，一方面利用了MPC的预测作用，一方面利用LQR在各点的线性化避免滚动优化的过长计算耗时，可以取得较好的控制效果，同时减少计算资源的占用。

Description

一种车辆横向自适应控制方法

技术领域

本发明涉及车辆自动驾驶和轨迹追踪领域，尤其是涉及结合多种控制算法混合实现车辆横向自适应控制的方法。

背景技术

车辆的横向控制一般是指在固定的纵向车速下，使车辆稳定追踪所设定的轨迹。当前主流的控制方法有Purepursuit、LQR、MPC、模糊控制以及神经网络控制算法等。一般而言，车辆的纵向控制主要使用自适应巡航控制保持恒定车速，而在车速增大时，往往会导致控制算法失效的情况发生。上述算法中，如Purepursuit算法利用车辆的三自由度线性运动学模型，通过寻找追踪轨迹上的预瞄点，选择前轮转角，从而令未来弧线轨迹经过预瞄点，实现追踪。由于算法的输入量是前轮偏转量而不是偏转变化量，在实际使用中的输入变化非常陡峭时，必须对输入变化速率和输入最大值进行约束。因此Purepursuit算法在高速情况下，控制效果与实际车辆动力学会严重不符，且输入过于陡峭还会使转向轮无法追踪，也容易发生失效。LQR算法利用车辆二自由度线性动力学模型，在每一个状态点计算出反馈矩阵和前馈量，从而生成控制量，但由于LQR算法没有预测功能，当待追踪轨迹曲率过大或者车速过高时，也会偏离轨迹并发生失效。MPC算法利用车辆二自由度线性动力学模型或者非线性动力学模型，能够预测未来一段时间内的轨迹变化，同时对输入控制量和输出量的变化值进行约束，因此控制曲线非常光滑，具有最好的控制效果，但是由于滚动窗口优化对计算机资源消耗大，因此计算时间长。

发明内容

针对上述本领域中所存在的技术问题，本发明提供了一种车辆横向自适应控制方法，结合不同控制算法的优点来克服部分方法容易失效、效率较低的问题，该方法包括以下步骤：

步骤一、测量获取目标车辆的前后悬长度、整车长度、整车质量、质心位置、轮胎参数、轴距以及轮距数据，并在侧向加速度a_y≤0.4g下，利用魔术公式求得车辆前后轮的侧偏刚度C_cf与C_cr和前后轮纵向刚度C_lf，C_lr，其中下标f和r分别表示前后轮；

步骤二、利用步骤一所获取的车辆数据，针对目标车辆分别设置Purepursuit算法的前视距离和前视系数等参数，以及LQR与MPC控制器算法相应的状态转移矩阵，使各算法在各自对应的车速范围内保持稳定；

步骤三、通过车速传感器获取每一控制周期内的车身坐标系下的纵向车速V_x，并记录相邻两次发生从一种算法转换到另一种算法的状态转移触发时间间隔delta_t，以这两个参数的以下阈值条件作为准则切换不同算法来执行横向控制：

若V_x≤10m/s，且delta_t＞2s，则执行Purepursuit算法；

若10m/s＜V_x＜＝16m/s，且delta_t＞2s，则执行LQR算法；

若16m/s＜V_x＜＝20m/s，且delta_t＞2s，则执行LQR-MPC联合算法；

若20m/s＜V_x，且delta_t＞2s，则执行MPC算法。

进一步地，执行Purepursuit算法具体包括以下步骤：

首先根据测得的当前纵向车速与位姿，在行车轨迹上寻找距离当前位置最近的一点作为预瞄点，并确定当前位置至预瞄点之间的预瞄长度：

L_f＝kV_x+L_fc

式中，L_f、k、V_x、L_fc分别为预瞄距离、前视系数、纵向车速、前视距离；设预瞄点坐标为(x0,y0)，当前位姿即位置坐标与姿态角为则该预瞄点与车辆当前行驶方向的夹角为：

输出满足相应约束的前轮偏转角δ_f：

|δ_f|＜δ_max

其中，L为前后轴距，上标·与下标max表示相应参数的导数与最大值约束。

进一步地，执行LQR算法具体包括以下步骤：

针对目标车辆建立二自由度线性动力学的误差方程，在此方程中表示纵向车速:

式中，m、I_z、a、b分别为车辆的质量、横向转动惯量、前悬长度、后悬长度，C_cf、C_cr、C_lf、C_lr分别为前后车轮侧偏刚度和纵向刚度；定义R是此时的路面曲率半径；定义/>R是此时的路面曲率半径；定义e₁、e₂如下：

e₁:车辆当前位置到预瞄点的横向距离，左手为正，右手为负；

e₂:车辆航向角和轨迹在预瞄点上的航向偏差；

执行以下控制律并输出前轮偏转角：

δ_f＝-Kx+δ_ff

其中，Q,S,N为下述优化问题的权重矩阵：

P为满足以下方程的解：

进一步地，执行LQR-MPC联合算法具体包括以下步骤：

获取当前时刻的前轮偏转角作为初始控制量：u_dyn＝δ_f,t-1,单位rad；构造车辆的线性时变模型：

A_dyn(k)＝I+TA_k,t

B_dyn(k)＝TB_k,t

式中,ξ为由车身横向车速纵向车速/>摆角φ，角速度/>惯性坐标下坐标位置(X，Y)构成的状态向量ξ_dyn，T为MPC算法的时间步长，k为某特定时刻，I_x为纵向转动惯量，下标t为某个控制周期，I为单位矩阵；采取增量约束的方法设计控制器，针对每个控制周期求解以下问题：

w.r.t

ΔU_min≤ΔU(k)≤ΔU_max，k＝t,...,t+H_c-1

Y_min-ε＜＝ψ_tξ(t|t)+ΘΔU(t)≤Y_max+ε

ε＞0

其中，△U为控制时域的控制量增量，也即前轮偏转角的增量；ε为二次优化问题的松弛因子，ε>0；矩阵H_t，G_t，H_c，U，Y，ψ_t，Θ则是通过将将动力学约束转化为二次优化系数矩阵得到；

求得各周期的控制量增量的解为：

由线性时变模型dynamics_function求出向后几个时刻的状态量:

接下来令LQR控制器在预测的几个未来时间节点内线性化:

[K_t+1,K_t+2...,K_t+Nc]＝LQR_controller([ξ_t+1,ξ_t+2...,ξ_t+Nc])

其中，K为LQR控制器的反馈矩阵；

在未来Nc个控制周期内，激活LQR控制器，并使用事先已经得到的系数矩阵，输出为：

δ_f＝-Kx+δ_ff

当未来Nc个时间节点追踪完毕后，再重新进入上述循环。

进一步地，执行MPC算法具体包括以下步骤：

采用与前述LQR-MPC联合算法相同的初始控制量及线性时变模型来求得控制时域的控制量增量：

选取得到的第一个控制量作为MPC控制器的输出，并且在随后的周期中不断滚动优化前轮偏转角:

上述本发明所提供的方法，利用有限状态机根据车速及状态转移时间间隔，实现了在不同控制算法之间的轮换，由于不同算法能够工作和取得最佳控制效果的速度区间不同，因此这种轮换控制方式足以保证车辆在全速度区间上控制器的稳定；上述LQR-MPC联合控制算法对应的速度控制区间，一方面利用了MPC的预测作用，一方面利用LQR在各点的线性化避免滚动优化的过长计算耗时，可以取得较好的控制效果，同时减少计算资源的占用。可见，本发明的方法与现有技术相比至少具有以下有益效果：

(1)该方法利用有限状态机，扩大了控制器有效的工作车速范围，在降低计算耗时的前提下保证了控制品质。

(2)该方法中的MLQR-MPC混合控制策略，避免了滚动优化同时利用了MPC控制器的预测特性和LQR控制器的前馈反馈控制特性。

附图说明

图1本发明所提供方法对应的系统模型框图；

图2有本发明限状态机模型。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明所提供的车辆横向自适应控制方法，如图1所示，具体包括以下步骤：

步骤一、测量获取目标车辆的前后悬长度、整车长度、整车质量、质心位置、轮胎参数、轴距以及轮距数据，并在侧向加速度a_y≤0.4g下，利用魔术公式求得车辆前后轮的侧偏刚度C_cf与C_cr和前后轮纵向刚度C_lf，C_lr，其中下标f和r分别表示前后轮；

步骤二、利用步骤一所获取的车辆数据，针对目标车辆分别设置Purepursuit算法的前视距离和前视系数等参数，以及LQR与MPC控制器算法相应的状态转移矩阵，使各算法在各自对应的车速范围内保持稳定；

步骤三、通过车速传感器获取每一控制周期内的车身坐标系下的纵向车速V_x，并记录相邻两次发生从一种算法转换到另一种算法的状态转移触发时间间隔delta_t，以这两个参数的以下阈值条件作为准则，通过如图2所示的有限状态机的方式切换不同算法来执行横向控制：

若V_x≤10m/s，且delta_t＞2s，则执行Purepursuit算法；

若10m/s＜V_x＜＝16m/s，且delta_t＞2s，则执行LQR算法；

若16m/s＜V_x＜＝20m/s，且delta_t＞2s，则执行LQR-MPC联合算法；

若20m/s＜V_x，且delta_t＞2s，则执行MPC算法。

在图2中，符号vel表示与纵向车速V_x相同的含义。

本领域技术人员应当知晓，在上述本发明的教导下如何选择执行Purepursuit算法、LQR算法、LQR-MPC算法以及MPC算法的具体形式与过程并不是唯一的，以下提供的具体过程仅是优选的实施方式，而并不应认为对本发明的权利要求保护范围构成了限制。事实上，本领域技术人员通过其他多样的算法流程均能够有效解决本发明所面对的技术问题。

在本发明的一个优选实施方式中，执行Purepursuit算法具体包括以下步骤：

首先根据测得的当前纵向车速与位姿，在行车轨迹上寻找距离当前位置最近的一点作为预瞄点，并确定当前位置至预瞄点之间的预瞄长度：

L_f＝kV_x+L_fc

式中，L_f、k、V_x、L_fc分别为预瞄距离、前视系数、纵向车速、前视距离；设预瞄点坐标为(x0,y0)，当前位姿即位置坐标与姿态角为则该预瞄点与车辆当前行驶方向的夹角为：

输出满足相应约束的前轮偏转角δ_f：

|δ_f|＜δ_max

其中，L为前后轴距，上标·与下标max表示相应参数的导数与最大值约束。

在本发明的一个优选实施方式中，执行LQR算法具体包括以下步骤：

针对目标车辆建立二自由度线性动力学的误差方程，在此方程中表示纵向车速:

式中，m、I_z、a、b分别为车辆的质量、横向转动惯量、前悬长度、后悬长度，C_cf、C_cr、C_lf、C_lr分别为前后车轮侧偏刚度和纵向刚度；定义R是此时的路面曲率半径；定义e₁、e₂如下：

e₁:车辆当前位置到预瞄点的横向距离，左手为正，右手为负；

e₂:车辆航向角和轨迹在预瞄点上的航向偏差；

执行以下控制律并输出前轮偏转角：

δ_f＝-Kx+δ_ff

其中，Q,S,N为下述优化问题的权重矩阵：

P为满足以下方程的解：

在本发明的一个优选实施方式中，执行LQR-MPC联合算法具体包括以下步骤：

获取当前时刻的前轮偏转角作为初始控制量：u_dyn＝δ_f,t-1,单位rad；构造车辆的线性时变模型：

A_dyn(k)＝I+TA_k,t

B_dyn(k)＝TB_k,t

式中,ξ为由车身横向车速纵向车速/>摆角φ，角速度/>惯性坐标下坐标位置(X，Y)构成的状态向量ξ_dyn，T为MPC算法的时间步长，k为某特定时刻，I_x为纵向转动惯量，下标t为某个控制周期，I为单位矩阵；采取增量约束的方法设计控制器，在各控制周期△U为控制时域的控制量增量，也即前轮偏转角的增量，辆的空间横向和纵向位置以及摆角作为被控量，具体地在一优选实施方式中，设定:

U＝[u_dyn(t) u_dyn(t+1) ... u_dyn(t+Nc)]^T

ΔU_t＝[Δu_dyn(t|t) Δu_dyn(t+1|t) ... Δu_dyn(t+N_c|t)]^T

Y(t)＝[η(t+1|t) η(t+1|t) ... η(t+Np|t)]T

设Q(3X3),R(1X1)为权重矩阵,ρ＞0为权重系数，于是:

定义E(t)矩阵为跟踪时域内的误差矩阵，在每个控制周期内求出。取矩阵如下:

G_t＝[2E(t)^TQΘ_t 0]

然后求解下列问题:

w.r.t

ΔU_min≤ΔU(k)≤ΔU_max，k＝t,...,t+H_c-1

Y_min-ε＜＝ψ_tξ(t|t)+ΘΔU(t)≤Y_max+ε

ε＞0

求得各周期的控制量增量的解为：

由线性时变模型dynamics_function求出向后几个时刻的状态量:

接下来令LQR控制器在预测的几个未来时间节点内线性化:

[K_t+1,K_t+2...,K_t+Nc]＝LQR_controller([ξ_t+1,ξ_t+2...,ξ_t+Nc])

其中，K为LQR控制器的反馈矩阵；在未来Nc个控制周期内，激活LQR控制器，并使用事先已经得到的系数矩阵，输出为：

δ_f＝-Kx+δ_ff

/>

当未来Nc个时间节点追踪完毕后，再重新进入上述循环。

进一步地，执行MPC算法具体包括以下步骤：

采用与前述LQR-MPC联合算法相同的初始控制量及线性时变模型来求得控制时域的控制量增量：

选取得到的第一个控制量作为MPC控制器的输出，并且在随后的周期中不断滚动优化前轮偏转角:

应理解，本发明实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (4)

1.一种车辆横向自适应控制方法，其特征在于：包括以下步骤：

步骤一、测量获取目标车辆的前后悬长度、整车长度、整车质量、质心位置、轮胎参数、轴距以及轮距数据，并在侧向加速度a_y≤0.4g下，利用魔术公式求得车辆前后轮的侧偏刚度C_cf与C_cr和前后轮纵向刚度C_lf，C_lr，其中下标f和r分别表示前后轮；

步骤二、利用步骤一所获取的车辆数据，针对目标车辆分别设置Purepursuit算法的前视距离和前视系数参数，以及LQR与MPC控制器算法相应的状态转移矩阵，使各算法在各自对应的车速范围内保持稳定；执行Purepursuit算法具体包括以下步骤：

首先根据测得的当前纵向车速与位姿，在行车轨迹上寻找距离当前位置最近的一点作为预瞄点，并确定当前位置至预瞄点之间的预瞄长度：

L_f＝kV_x+L_fc

式中，L_f、k、V_x、L_fc分别为预瞄距离、前视系数、纵向车速、前视距离；设预瞄点坐标为(x0,y0)，当前位姿即位置坐标与姿态角为则该预瞄点与车辆当前行驶方向的夹角为：

输出满足相应约束的前轮偏转角δ_f：

|δ_f|＜δ_max

其中，L为前后轴距，上标·与下标max表示相应参数的导数与最大值约束；

步骤三、通过车速传感器获取每一控制周期内的车身坐标系下的纵向车速V_x，并记录相邻两次发生从一种算法转换到另一种算法的状态转移触发时间间隔delta_t，以这两个参数的以下阈值条件作为准则切换不同算法来执行横向控制：

若V_x≤10m/s，且delta_t＞2s，则执行Purepursuit算法；

若10m/s＜V_x＜＝16m/s，且delta_t＞2s，则执行LQR算法；

若16m/s＜V_x＜＝20m/s，且delta_t＞2s，则执行LQR-MPC联合算法；

若20m/s＜V_x，且delta_t＞2s，则执行MPC算法。

2.如权利要求1所述的方法，其特征在于：执行LQR算法具体包括以下步骤：

针对目标车辆建立二自由度线性动力学的误差方程，在此方程中表示纵向车速:

式中，m、I_z、a、b分别为车辆的质量、横向转动惯量、前悬长度、后悬长度，C_cf、C_cr、C_lf、C_lr分别为前后车轮侧偏刚度和纵向刚度；定义R是此时的路面曲率半径；定义e₁、e₂如下：

e₁:车辆当前位置到预瞄点的横向距离，左手为正，右手为负；

e₂:车辆航向角和轨迹在预瞄点上的航向偏差；

执行以下控制律并输出前轮偏转角：

δ_f＝-Kx+δ_ff

其中，Q、S、N为下述优化问题的权重矩阵：

u表示控制量；

P为满足以下方程的解：

3.如权利要求2所述的方法，其特征在于：执行LQR-MPC联合算法具体包括以下步骤：

获取当前时刻的前轮偏转角作为初始控制量：u_dyn＝δ_f,t-1,单位rad；构造车辆的线性时变模型：

A_dyn(k)＝I+TA_k,t

B_dyn(k)＝TB_k,t

式中,ξ为由车身横向车速纵向车速/>摆角φ，角速度/>惯性坐标下坐标位置(X，Y)构成的状态向量ξ_dyn，T为MPC算法的时间步长，k为某特定时刻，I_x为纵向转动惯量，下标t为某个控制周期，I为单位矩阵；采取增量约束的方法设计控制器，针对每个控制周期求解以下问题：

w.r.t

ΔU_min≤ΔU(k)≤ΔU_max，k＝t,...,t+H_c-1

Y_min-ε＜＝ψ_tξ(t|t)+ΘΔU(t)≤Y_max+ε

ε＞0

其中，△U为控制时域的控制量增量，也即前轮偏转角的增量，ε为二次优化问题的松弛因子，ε>0；矩阵H_t，G_t，H_c，U，Y，ψ_t，Θ均是通过将将动力学约束转化为二次优化系数矩阵得到；

求得各周期的控制量增量的解为：

由线性时变模型dynamics_function求出向后几个时刻的状态量:

接下来令LQR控制器在预测的几个未来时间节点内线性化:

[K_t+1,K_t+2...,K_t+Nc]＝LQR_controller([ξ_t+1,ξ_t+2...,ξ_t+Nc])

其中，K为LQR控制器的反馈矩阵；在未来Nc个控制周期内，激活LQR控制器，并使用事先已经得到的系数矩阵，输出为：

δ_f＝-Kx+δ_ff

当未来Nc个时间节点追踪完毕后，再重新进入上述循环。

4.如权利要求3所述的方法，其特征在于：执行MPC算法具体包括以下步骤：

采用与前述LQR-MPC联合算法相同的初始控制量及线性时变模型来求得控制时域的控制量增量：

选取得到的第一个控制量作为MPC控制器的输出，并且在随后的周期中不断滚动优化前轮偏转角: