CN115598983A - 一种考虑时变时延的无人车横纵向协同控制方法及装置 - Google Patents

Abstract

本发明公开了一种考虑时变时延的无人车横纵向协同控制方法及装置，其方法包括：步骤1，在线获取底层时变CAN通信延时估计值τ_CAN；步骤2，根据寻找到的预测时域N_p内一系列参考点信息，利用预测模型，结合横纵向一体化评价函数J和横、纵向联合约束s.t.进行最优控制问题滚动求解，输出车辆的控制量。本发明通过结合自适应时延估计器与考虑时变时延的MPC横纵向协同控制算法，解决无人车辆因忽略底层时特性导致的极限工况下车辆控制失稳问题，在保证横、纵向控制精度的同时提升车辆稳定性。

Description

一种考虑时变时延的无人车横纵向协同控制方法及装置

技术领域

本发明涉及智能车辆技术领域，特别是关于一种考虑时变时延的无人车横纵向协同控制方法及装置。

背景技术

智能车辆是智能交通系统的重要环节，它能够有效减少交通事故、交通阻塞和环境污染等问题，也因此成为了近年来的研究热点。运动控制是智能车辆的核心技术之一。所谓运动控制是指根据参考轨迹输入和控制律生成车辆执行器(如方向盘、电子油门、制动器、换挡机构等)的控制指令，产生影响车辆运动的力或力矩，使车辆能够最终收敛于参考轨迹。

智能车辆运动控制包括纵向控制和横向控制，纵向控制实现车辆按照预定的速度巡航或与前方动态目标保持一定的距离，横向控制实现车辆沿规划的路径行驶，并保证车辆的行驶安全性、平稳性与乘坐舒适性。下面分别对现有的自动驾驶车辆轨迹跟踪控制主要研究展开说明：

一是横、纵向分层控制。该控制方式是通过忽略横纵向动力学的耦合特性实现对动力学模型解耦，减小单个问题的复杂度从而有利于横、纵向控制律的快速求解。现有横向控制方法可分为无模型控制和基于模型的控制，无模型控制仅依赖误差进行车轮转角计算，例如纯跟踪、Stanley、PID等控制算法。基于模型的控制方法依据系统动态特性设计显式控制率，例如LQR、MPC等控制算法。现有纵向控制方法按控制结构可分为：直接式控制和分层式控制，直接式控制根据车辆模型和参考速度轨迹直接生成期望制动压力或节气门开度，分层式控制包括上位速度控制和下位执行器控制。

另一种是横、纵向耦合控制。这种控制方式充分考虑了智能车辆横纵向动力学间的耦合关联特性，通过对车辆横纵向一体化动力学模型进行直接控制求解得到横纵向运动控制律。横纵向耦合控制在模型方面进行横纵耦合车辆建模，进而对车辆的动态特性进行更准确的描述，性能评价函数方面采用横纵综合的性能评价实现对两个单向的跟踪性能进行统筹协调，控制约束方面采用横纵联合约束形式实现对控制量可行集更完备的构建。

现有的轨迹跟踪控制算法经常忽略的一个关键因素是车辆底层时延，包含CAN通信延时与执行器时滞。延时主要来自于控制模块与执行器之间的通信，其中存在大量的中间环节，例如CAN总线通信延时。时滞主要来自执行器最终响应上层控制指令存在的耗时，例如转向及驱制动执行滞后。延时与时滞主要受硬件性能和底层控制设计的影响，忽略通信延时和执行器时滞会造成控制模型不匹配和性能恶化，导致系统瞬态响应及稳定性下降，进而出现车辆转向振荡甚至失稳等现象。如今，许多学术研究机构使用的无人车辆平台通过安装线控模块或新的执行机构来减少底层时延，然而无法消除的底层时延仍对当前运动控制系统的稳定性构成挑战。

发明内容

本发明的目的在于提供一种考虑时变时延的无人车横纵向协同控制方法及装置，其通过结合自适应时延估计器与考虑时变时延的MPC横纵向协同控制算法，解决无人车辆因忽略底层时延特性导致的极限工况下车辆控制失稳问题，在保证横、纵向控制精度的同时提升车辆稳定性。

为实现上述目的，本发明提供一种考虑时变时延的无人车横纵向协同控制方法，其包括：

步骤1，在线获取底层时变CAN通信延时估计值τ_CAN；

步骤2，根据寻找到的预测时域N_p内一系列参考点信息，利用预测模型，结合横纵向一体化评价函数J和横、纵向联合约束s.t.进行最优控制问题滚动求解，输出车辆的控制量。

进一步地，步骤2中，最优控制问题被描述为下式：

其中，i为预测状态的索引，k为当前时刻，

分别为N_p内k+i时刻车辆的横向偏差代价项、航向角偏差代价项、纵向速度偏差、纵向控制增量、横向控制增量代价项，N_c为控制时域，e_y(k+i)、

分别为N_p内k+i时刻车辆的横向偏差、航向角偏差、纵向速度偏差，Δa_x(k+i)、Δδ_f(k+i)分别为N_p内k+i时刻车辆的纵向加速度增量、前轮转角增量，Q₁、Q₂、Q₃、R₁、R₂均为权重系数，χ₀(k)、χ(k+1)、χ(k+i)、χ(k+i-1)分别为车辆在k、k+1、k+i、k+i-1时刻的状态量，u₀(k)、u(k+i-1)分别为车辆在k、k+i-1时刻的控制量，a_x(k+j)δ_f(k+j)分别为车辆在k+j时刻的横、纵向控制量，δ_f,max、a_x,max分别为车辆允许的最大横、纵向控制量，Δδ_f,max、Δa_x,max分别为车辆允许的相邻时刻最大横、纵向控制增量，Δδ_f(k+j)、Δa_x(k+j)分别为车辆在k+j时刻的横、纵向控制增量，f(χ₀(k),u₀(k))、f(χ(k+i-1),u(k+i-1))分别为变维度时延增广模型利用车辆在k、k+i-1时刻的状态量和初始控制量进行计算的结果。

进一步地，步骤2中，预测模型为利用根据底层时变CAN通信延时估计值与执行器时滞特性的横纵向时延环节构建得到的变维度时延增广模型：

其中，

N_τ表示信号因τ_CAN而延后N_τ个采样时间步长dt，χ(k)和χ(k+1)分别为预测时域N_p内k、k+1时刻车辆的状态量，χ′(k)和χ′(k+1)分别为χ(k)和χ(k+1)关于时间的一阶导数，a_x(k)和a_x(k+1)分别为预测时域N_p内k、k+1时刻车辆的纵向加速度，δ_f(k)和δ_f(k+1)分别为预测时域N_p内k、k+1时刻车辆的前轮转向角，

分别为预测时域N_p内k-N_τ、k-N_τ+1、k-N_τ+2、k-2、k-1、k时刻车辆的期望前轮转向角，

为预测时域N_p内k时刻车辆的期望纵向加速度，dt为采样时间步长，

分别为横、纵向执行器的一阶惯性环节时滞常数，O为零矩阵，

对应为如下简化公式用的过渡矩阵A、B₁、B₂的离散化结果：

B₁＝[1 0 0 0 0]^T

其中，C_cf、C_cr分别为车辆的前、后轮侧偏刚度，l_f、l_r分别为车辆的质心到前、后轴中心的距离，m、v_x、I_z分别为车辆的质量、纵向速度、转动惯量，κ为跟踪目标点处的道路曲率。

进一步地，步骤1具体包括：

将实时输入的期望控制指令作为实时信号，底层实际控制指令作为延时信号，利用基于FIR的自适应全通滤波时延估计器及基于均方误差评价指标的时延估计稳定策略，获得τ_CAN。

进一步地，基于FIR的自适应全通滤波时延估计器表示为式(8)或(9)：

式中：x(k)为实时信号，x(k-N_τ)为延时信号，

表示为向量

表示为向量

X₊(k-N_τ)＝[x(k+1-N_τ),...,x(k+n_max-N_τ)]^T为前向向量，X_-(k)＝[x(k-1),...,x(k-n_max)]^T为后向向量。

进一步地，基于均方误差评价指标获得时延估计稳定策略采用式(18)获得：

式中，τ(k)表示k时刻的底层时延估计值，Thr为更新阈值，MSE_last表示k-1时刻的底层时延估计值

的MSE指标，MSE_new表示k时刻的底层时延估计值

的MSE指标，再根据

获得τ_CAN。

本发明还提供一种考虑时变时延的无人车横纵向协同控制装置，其包括：

自适应全通滤波时延估计器，其用于在线获取底层时变CAN通信延时估计值τ_CAN；

横纵向协同控制器，其用于根据寻找到的预测时域N_p内一系列参考点信息，利用预测模型，结合横纵向一体化评价函数J和横、纵向联合约束s.t.进行最优控制问题滚动求解，输出车辆的控制量，其中：最优控制问题被描述为下式：

其中，i为预测状态的索引，k为当前时刻，

分别为N_p内k+i时刻车辆的横向偏差代价项、航向角偏差代价项、纵向速度偏差、纵向控制增量、横向控制增量代价项，N_c为控制时域，e_y(k+i)、

分别为N_p内k+i时刻车辆的横向偏差、航向角偏差、纵向速度偏差，Δa_x(k+i)、Δδ_f(k+i)分别为N_p内k+i时刻车辆的纵向加速度增量、前轮转角增量，Q₁、Q₂、Q₃、R₁、R₂均为权重系数，χ₀(k)、χ(k+1)、χ(k+i)、χ(k+i-1)分别为车辆在k、k+1、k+i、k+i-1时刻的状态量，u₀(k)、u(k+i-1)分别为车辆在k、k+i-1时刻的控制量，a_x(k+j)δ_f(k+j)分别为车辆在k+j时刻的横、纵向控制量，δ_f,max、a_x,max分别为车辆允许的最大横、纵向控制量，Δδ_f,max、Δa_x,max分别为车辆允许的相邻时刻最大横、纵向控制增量，Δδ_f(k+j)、Δa_x(k+j)分别为车辆在k+j时刻的横、纵向控制增量，f(χ₀(k),u₀(k))、f(χ(k+i-1),u(k+i-1))分别为变维度时延增广模型利用车辆在k、k+i-1时刻的状态量和初始控制量进行计算的结果。

进一步地，预测模型为利用根据底层时变CAN通信延时估计值与执行器时滞特性的横纵向时延环节构建得到的变维度时延增广模型：

其中，

N_τ表示信号因τ_CAN而延后N_τ个采样时间步长dt，χ(k)和χ(k+1)分别为预测时域N_p内k、k+1时刻车辆的状态量，χ′(k)和χ′(k+1)分别为χ(k)和χ(k+1)关于时间的一阶导数，a_x(k)和a_x(k+1)分别为预测时域N_p内k、k+1时刻车辆的纵向加速度，δ_f(k)和δ_f(k+1)分别为预测时域N_p内k、k+1时刻车辆的前轮转向角，

分别为预测时域N_p内k-N_τ、k-N_τ+1、k-N_τ+2、k-2、k-1、k时刻车辆的期望前轮转向角，

为预测时域N_p内k时刻车辆的期望纵向加速度，dt为采样时间步长，

分别为横、纵向执行器的一阶惯性环节时滞常数，O为零矩阵，

对应为如下简化公式用的过渡矩阵A、B₁、B₂的离散化结果：

B₁＝[1 0 0 0 0]^T

其中，C_cf、C_cr分别为车辆的前、后轮侧偏刚度，l_f、l_r分别为车辆的质心到前、后轴中心的距离，m、v_x、I_z分别为车辆的质量、纵向速度、转动惯量，κ为跟踪目标点处的道路曲率。

进一步地，底层时变CAN通信延时估计值τ_CAN的获取方法具体包括：

将实时输入的期望控制指令作为实时信号，底层实际控制指令作为延时信号，利用基于FIR的自适应全通滤波时延估计器及基于均方误差评价指标的时延估计稳定策略进行底层时变CAN通信延时的在线估计，输出底层时变CAN通信延时估计值。

进一步地，基于FIR的自适应全通滤波时延估计器表示为式(8)或(9)：

式中：x(k)为实时信号，x(k-N_τ)为延时信号，

表示为向量

表示为向量

X₊(k-N_τ)＝[x(k+1-N_τ),...,x(k+n_max-N_τ)]^T为前向向量，X_-(k)＝[x(k-1),...,x(k-n_max)]^T为后向向量；

基于均方误差评价指标获得时延估计稳定策略采用式(18)获得：

式中，τ(k)表示k时刻的底层时延估计值，Thr为更新阈值，MSE_last表示k-1时刻的底层时延估计值

的MSE指标，MSE_new表示k时刻的底层时延估计值

的MSE指标，再根据

获得τ_CAN。

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

1.与现有技术中仅通过离线标定获取车辆时延参数，且运行时始终为常数相比，本发明基于FIR的自适应全通滤波时延估计器，能够在线估计底层时变CAN通信延时。

2.本发明根据均方误差评价指标，结合存储的历史控制信号以及时延估计值提供的时延估计稳定策略，能够消除自适应全通滤波器因噪声干扰而产生的错误估计值；

3.与现有技术中的基于MPC模型预测控制框架，预测模型中仅考虑执行器时滞相比，本发明基于底层时延在线估计值，构建考虑底层CAN通信延时与执行器时滞特性的横纵向时延环节，建立基于车辆二自由度动力学模型的变维度时延增广模型，通过变维度时延增广模型，可以单独解决控制算法因所用车辆模型与实车动态时延特性之间不匹配”而造成的控制性能下降的问题；

4.与现有技术中的从横、纵分离的角度出发，单独设计纵向控制器以实现无人车速度跟踪相比，本发明以变维度时延增广模型为预测模型，基于横、纵向耦合特性建立横纵一体化评价函数及联合约束，设计考虑时变时延的MPC横纵向协同控制器，通过一体化最优控制综合保证横、纵控制精度及车辆稳定性。

附图说明

图1为本发明实施例提供的考虑时变时延的无人车横纵向协同控制方法框架的结构示意图。

图2为本发明实施例提供的关于横向控制的底层时延示意图。

图3为本发明实施例提供的考虑时变时延的无人车横纵向协同控制装置的结构示意图。

具体实施方式

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

如图1所示，本发明实施例提供的考虑时变时延的无人车横纵向协同控制装置包括自适应全通滤波时延估计器和考虑时变时延的横纵向协同控制器。其中：

自适应全通滤波时延估计器用于在线获取底层时变CAN通信延时估计值τ_CAN。

横纵向协同控制器用于根据寻找到的预测时域内一系列参考点信息，利用预测模型，结合由横纵向一体化评价函数J和横、纵向联合约束s.t.进行最优控制问题滚动求解，输出车辆的控制量。

本发明通过结合自适应时延估计器与考虑时变时延的MPC横纵向协同控制算法，解决无人车辆因忽略底层时特性导致的极限工况下车辆控制失稳问题，在保证横、纵向控制精度的同时提升车辆稳定性。

在一个实施例中，如图1所示，自适应全通滤波时延估计器包括自适应全通滤波器和时延估计稳定策略。时延估计器用于将实时输入的期望控制指令作为实时信号，底层实际控制指令作为延时信号，利用基于FIR(英文全称为“Finite Impulse Response”，中文全称为“有限脉冲响应滤波器”)的自适应全通滤波时延估计器及基于均方误差评价指标的时延估计稳定策略进行底层时变CAN通信延时的在线估计，输出底层时变CAN通信延时估计值。

基于FIR的自适应全通滤波时延估计器的获取方法包括：

构建信号通道模型：

H(ω)＝e^-jτw (1)

其中：H为全通滤波器h的傅里叶变换，全通即|H(ω)|＝1，j为虚数，ω表示频率，τ为底层时延，即底层时变CAN通信延时与执行器时滞相互耦合的结果。将实时信号(例如：无人车辆期望控制指令)作为输入，延时信号(例如：底层实际控制指令)作为输出，则滤波器h实现信号相位的改变，而不改变幅值。由于相位变化取决于时延，因此通过估计滤波器h可以获取时延值τ。

基于FIR构建全通滤波器H(ω)：

其中：P(e^jω)是FIR滤波器p的前向频率响应，P(e^-jω)为FIR滤波器p的后向频率响应。

令N_τ＝τ/dt，N_τ表示信号因时延特性而延后N_τ个采样时间步长dt，dt为采样时间步长，k表示当前时刻，则基于式(2)，对当前时刻的信号，即实时信号x(k)与延时信号x(k-N_τ)进行线性处理得到式(3)，其中：*为卷积运算符，

意味着箭头两侧的公式等价，p(k)和p(-k)分别为FIR的前向和反向表达式，前向即利用未来信息，反向即利用过去的信息：

设定一个支持n_max个滤波器系数的FIR，n_max为滤波器最大的索引号，该值也定义了时延估计值的上界τ_max＝n_max*dt，即N_τ≤n_max，令n代表滤波器系数的索引，n∈[0,n_max]。

则FIR滤波器响应p(n)可以由滤波器系数a_n描述为式(4)：

因此，式(3)的全通滤波器可以基于FIR转化为线性表达式(5)：

式中，

表示将基于卷积的全通滤波器表达公式转化为基于FIR的线性表达式，才能通过求线性系数得到时延估计值。

当滤波器系数

已知时，在ω0处分别对式(1)和式(2)式进行求导，计算dH(ω)/dω，并令式(1)与式(2)的值相等，则有式(6)，从而可以计算得到时延估计值τ：

由于τ是CAN通信延时与执行器时滞相互耦合的结果，本实施例将两者通过如下方法进行解耦：以横向控制模型中的底层时延为例，利用实车数据离线标定横向执行器的一阶惯性环节时滞常数

并作为定值，则最终输出时变底层时变CAN通信延时估计值τ_CAN：

上式(1)-式(6)将对时延值τ的估计转化为对滤波器系数a_n的估计。设定x(k)为当前控制器下发的实时控制信号，x(k-N_τ)为底层反馈的延时控制信号，令滤波器系数a₀＝1，则式(5)的线性表达式为式(8)：

式中：x(k)为实时信号，x(k-N_τ)为延时信号，

表示为向量

表示为向量

X₊(k-N_τ)＝[x(k+1-N_τ),...,x(k+n_max-N_τ)]^T为前向向量，X_-(k)＝[x(k-1),...,x(k-n_max)]^T为后向向量。

则实时信号x(k)和延时信号x(k-N_τ)可进一步表示为如下线性式(9)：

由于实时信号x(k)和延时信号x(k-N_τ)已知，令d(k)＝x(k-N_τ)-x(k)，

则时延估计问题转变为优化问题：通过最小化测量样本d(k)与由当前滤波器系数

计算得到的y(k)之间的偏差，从而得到最优滤波器系数a，进而将最优滤波器系数a代入式(6)，便可以得到时延估计值τ。

本实施例基于梯度下降法迭代更新滤波器系数a(k)，a(k)表示当前时刻即k时刻的滤波器系数值，而a是滤波器系数的整体表达，而不区分具体哪一个时刻。最新滤波器系数a(k)的数值用于计算时延估计值τ。本实施例定义d(k)与y(k)之间的偏差为：

定义性能指标函数表示为下式(11)：

J(k)＝|e(k)|² (11)

可得当前梯度为：

则更新后的滤波器系数表示为式(13)：

其中：μ为梯度下降法的学习率参数。

在一个实施例中，基于LMS获取时变的学习率参数μ，用于保证优化过程中的误差收敛，最终实现学习率自适应的全通滤波时延估计器。所设置的自适应学习率为式(14)：

结合自适应学习率μ，式(12)变为：

其中：ρ为自适应学习率常数系数，0＜ρ＜2/3；ε为极小的正整数，用于保证分母不为0。

下面介绍基于均方误差指标的时延估计稳定策略的获得方式。

基于实时及延时信号数据对上述时延估计器进行仿真验证时，存在因噪声干扰而使得估计结果在不同值之间高频变化的现象，甚至出现负数等错误估计值，因此上述TDE是不稳定的。考虑到无人车辆底层时延值在短时间内恒定、长时间内时变的特点，基于均方误差评价指标设计时延估计稳定策略，保证在提高TDE估计稳定性的同时消除错误估计值。

时延估计稳定策略的主要思想是使用统计检验。设定：存在包含M个采样时间步长dt内所有信号值的实时信号序列x₁(t)与延时信号序列x₂(t)，时延估计值的数值范围为[τ_min,τ_max]，即上界为τ_max，下界为τ_min。基于上一时刻的时延估计值

和当前时刻的时延估计值

利用式(16)计算MSE_last表示k-1时刻的底层时延估计值

的均方MSE指标，利用式(17)计算MSE_new表示k时刻的底层时延估计值

的均方MSE指标：

实质上，MSE指标可以采用式(16)-(17)提供的均方误差获得，也可以采用方差或标准差等其他评价标准计算获得。

基于上述MSE指标，本实施例提供时延估计更新策略如下式(18)所示：

其中：τ(k)表示k时刻的底层时延估计值，Thr为更新阈值，本实施例中设置Thr＝0.2。通过比较相邻时刻估计值对同一窗口内实时与延时信号的方差，当两者之间差异不大时假定TDE结果不可靠，此时保留上一时刻估计值，否则，对时延结果进行更新。再根据

获得τ_CAN。

在一个实施例中，考虑时变时延的横纵向协同控制器的获取方式包括：

首先，构建变维度时延增广模型：

在一个实施例中，如图1所示，预测模型为通过结合在线时延估计值构建考虑底层CAN通信延时与执行器时滞特性的变维度时延增广模型，进而对车辆进行更准确的动态特性描述与未来时刻状态预测。

车辆三自由度车辆动力学模型如下：

其中，y_e为车辆后轴中心与参考点之间的横向偏差，

为车辆横向偏差变化率，ε_e为车辆纵轴与参考点之间航向角偏差，

为车辆航向角偏差变化率，

分别为车辆纵向速度v_x和横向速度，

为车辆沿x轴的纵向加速度a_x，

为车辆沿y轴的横向加速度，

为车辆横摆角，

为车辆横摆角速度，

为车辆横摆角速度随时间的变化率，

为车辆期望纵向加速度，

为车辆期望前轮转角，κ为跟踪目标点处的道路曲率，m为车辆质量，l_f为质心到前轴中心的距离，l_r为质心到后轴中心的距离，C_cf为前轮侧偏刚度，C_cr为后轮侧偏刚度，I_z为车辆转动惯量。

考虑车辆底层CAN通信延时与执行器时滞特性(为简化模型复杂度，纵向仅考虑执行器时滞特性)，以横向控制为例的底层时延，如下图2所示：

1)关于CAN通信延时τ_CAN

设定为纯滞后环节，当前控制器下发期望控制量为

则实际作用于车辆的期望控制量为

2)关于执行器时滞

设定为一阶惯性环节，当前控制器下发期望控制量为

当前实际控制量为δ_f(t)、a_x(t)，则实际作用于车辆的期望控制量为

基于上述分析，构建横、纵向时延环节表示为下式(20)：

基于三自由度车辆动力学模型与横纵向时延环节，建立用于MPC状态预测的时延增广预测模型(21)：

为了简化上述模型的表达形式，设置如下矩阵A、矩阵B₁和矩阵B₂，并对时延增广预测模型(21)进行离散化处理，采样时间步长dt，从而得到下面的

B₁＝[1 0 0 0 0]^T

此时，上述时延增广预测模型中待求解的最优横向控制量为

需进一步转化使得待求解控制量仍为

因此，基于已知的历史期望横向控制量，即

进一步得到变维度时延增广预测模型：

式中，

N_τ表示信号因τ_CAN而延后N_τ个采样时间步长dt，χ(k)和χ(k+1)分别为预测时域N_p内k、k+1时刻车辆的状态量，χ′(k)和χ′(k+1)分别为χ(k)和χ(k+1)关于时间的一阶导数，a_x(k)和a_x(k+1)分别为预测时域N_p内k、k+1时刻车辆的纵向加速度，δ_f(k)和δ_f(k+1)分别为预测时域N_p内k、k+1时刻车辆的前轮转向角，

分别为预测时域N_p内k-N_τ、k-N_τ+1、k-N_τ+2、k-2、k-1、k时刻车辆的期望前轮转向角，

为预测时域N_p内k时刻车辆的期望纵向加速度，dt为采样时间步长，

分别为横、纵向执行器的一阶惯性环节时滞常数，O为零矩阵，即矩阵中元素均为0，

对应为简化公式用的过渡矩阵A、B₁、B₂的离散化结果，

为式(19)三自由度车辆动力学模型的状态量，是公式(22)χ的一部分，

u(k)为控制量，

χ(k)为状态量，具体表示如下：

最终变维度时延增广模型可表示为：χ(k+1)＝f(χ(k),u(k))，状态量χ的维度基于时延估计值τ_CAN(离散化后为N_τ)动态变化，保证了对车辆模型更为准确的描述。

然后，构建基于MPC的横纵向协同最优控制器：

本发明采用横纵综合的性能评价实现对两个单向的跟踪性能进行统筹协调，最小化由预测时域内的横向偏差、航向角偏差、纵向速度偏差、前轮转角增量以及纵向加速度增量组成的横纵向一体化评价函数(23)：

其中：N_p为预测时域；N_c为控制时域；Q₁、Q₂、Q₃、R₁、R₂为权重系数；e_y(k)为预测时域内k时刻车辆后轴中心(x(k),y(k))到参考点(x_r(k),y_r(k))处切线的垂直距离，即横向偏差，

为参考点期望航向角

与车辆当前航向角之间的偏差，即航向角偏差，两者反应车辆对期望路径的跟踪性能，用于保证车辆横向跟踪精度；

为预测时域内k时刻车辆期望纵向速度v_xr(k)与车辆当前时刻速度v_x(k)的偏差，即纵向速度偏差，反应了车辆对期望速度曲线的跟踪性能，用于保证车辆纵向跟踪精度；Δa_x(k)为纵向加速度增量，Δδ_f(k)为前轮转角增量，两者反应对控制增量的约束，避免车轮转角及加速度大幅变化，保证控制动作平稳。

构建最优控制问题的横纵联合约束条件：

其中：式(24)为变维度时延增广动力学模型约束，在预测时域内车辆横、纵向状态量及控制量均时变；式(25)为横、纵向执行器的机械响应特性约束，包括车轮转角、纵向加速度的极值约束与增量约束。

最终可构建为如下针对横纵向协同控制的非线性规划问题：

其中，J为MPC横纵向协同控制器，i为预测状态的索引，取值范围是[1 N_p]，k为当前时刻，

分别为N_p内k+i时刻车辆的横向偏差代价项、航向角偏差代价项、纵向速度偏差、纵向控制增量、横向控制增量代价项，N_c为控制时域，e_y(k+i)、

分别为N_p内k+i时刻车辆的横向偏差、航向角偏差、纵向速度偏差，Δa_x(k+i)、Δδ_f(k+i)分别为N_p内k+i时刻车辆的纵向加速度增量、前轮转角增量，Q₁、Q₂、Q₃、R₁、R₂均为权重系数，χ₀(k)、χ(k+1)、χ(k+i)、χ(k+i-1)分别为车辆在k、k+1、k+i、k+i-1时刻的状态量，u₀(k)、u(k+i-1)分别为车辆在k、k+i-1时刻的控制量，a_x(k+j)δ_f(k+j)分别为车辆在k+j时刻的横、纵向控制量，δ_f,max、a_x,max分别为车辆允许的最大横、纵向控制量，Δδ_f,max、Δa_x,max分别为车辆允许的相邻时刻最大横、纵向控制增量，Δδ_f(k+j)、Δa_x(k+j)分别为车辆在k+j时刻的横、纵向控制增量，f(χ₀(k),u₀(k))、f(χ(k+i-1),u(k+i-1))分别为变维度时延增广模型利用车辆在k、k+i-1时刻的状态量和初始控制量进行计算的结果，δ_f,max、Δδ_f,max、a_x,max、Δa_x,max的数值由车辆自身特性决定。

对上述构建的多约束非线性优化问题进行求解，得到最优控制序列：

U^*(k)＝[δ_f ^*(k|k),...,δ_f ^*(k|k+N_c-1),a_x ^*(k|k),a_x ^*(k+2|k),...,a_x ^*(k|k+N_c-1)] (27)

取δ_f ^*(k|k)作为当前期望前轮转角，发送给底层转向执行器。取a_x ^*(k|k)作为当前期望纵向加速度，基于所设置的死区进行驱/制动模式切换判断，最终由期望加速度-油门开度/制动压力MAP表获取纵向控制指令，发送至纵向执行器。其判断逻辑如下：

其中：Mode(k)表示k时刻控制模式，a_x ^*为期望纵向加速度，Δa为死区偏置量，由Δa形成的死区可以保证驱/制动模式的平稳切换。

本发明实施例还提供一种考虑时变时延的无人车横纵向协同控制方法，其包括：

步骤1，在线获取底层时变CAN通信延时估计值。其中，底层时变CAN通信延时估计值在上文表示为k时刻的底层时变CAN通信延时估计值τ_CAN(k)。

步骤2，根据寻找到的预测时域N_p内一系列参考点信息(如图中的参考点信息包含参考点的期望横坐标x_r、期望纵坐标y_r、期望航向角

期望纵向速度、该点路径曲率κ)，利用预测模型，结合由横纵向一体化评价函数J和横、纵向联合约束s.t.进行最优控制问题滚动求解，输出车辆的控制量。

在一个实施例中，步骤2中，最优控制问题可以被描述为上述实施例中的式(26)。

在一个实施例中，步骤2中，预测模型为利用根据底层时变CAN通信延时估计值与执行器时滞特性的横纵向时延环节构建得到的变维度时延增广模型，变维度时延增广模型可以采用上述实施例中的式(22)表示。当然，预测模型也可以为常用的车辆动力学模型来获取控制量。但是，需要说明的是，采用变维度时延增广模型，能够获得更精确的MPC横纵向协同控制器，进而为提升整体控制效果提供有利条件。

在一个实施例中，步骤1可以采用如下方法实现：

将实时输入的期望控制指令作为实时信号，底层实际控制指令作为延时信号，利用基于FIR的自适应全通滤波时延估计器及基于均方误差评价指标的时延估计稳定策略进行底层时变CAN通信延时的在线估计，输出底层时变CAN通信延时估计值。

需要说明的是，步骤1也可以采用例如基于线性回归法估计、基于最小二乘法估计、基于最小均方差估计等现有方法在线获取底层时变CAN通信延时估计值。

在一个实施例中，步骤2中，基于FIR的自适应全通滤波时延估计器表示为上述实施例中的式(8)或(9)。

在一个实施例中，步骤2中，基于均方误差评价指标获得时延估计稳定策略采用上述实施例中的式(18)获得。

本发明通过结合自适应时延估计器与考虑时变时延的MPC横纵向协同控制算法，解决无人车辆因忽略底层时特性导致的极限工况下车辆控制失稳问题，在保证横、纵向控制精度的同时提升车辆稳定性。

如图3所示，车辆控制系统包括环境感知单元、决策规划单元、底层执行单元和本发明所建立的底层时延估计单元与横纵向协同控制单元。其中，环境感知单元用于获取环境信息，经处理后下发至决策规划单元；决策规划单元根据环境信息及车辆状态信息进行全局轨迹规划，并将参考轨迹信息输出给横纵向控制单元；底层时延估计单元使用本发明中的方法在线估计时延值并输出给横纵向控制单元；横纵向控制单元接收到所述参考轨迹及时延估计值后，使用本发明中的方法计算横、纵向控制指令，最终下发给底层执行单元来控制车辆，实现无人车辆精确、平稳的横纵向控制。

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

Claims (10)

1.一种考虑时变时延的无人车横纵向协同控制方法，其特征在于，包括：

步骤1，在线获取底层时变CAN通信延时估计值τ_CAN；

步骤2，根据寻找到的预测时域N_p内一系列参考点信息，利用预测模型，结合横纵向一体化评价函数J和横、纵向联合约束s.t.进行最优控制问题滚动求解，输出车辆的控制量。

2.如权利要求1所述的考虑时变时延的无人车横纵向协同控制方法，其特征在于，步骤2中，最优控制问题被描述为下式：

其中，i为预测状态的索引，k为当前时刻，

分别为车辆在k+i时刻的横向偏差代价项、航向角偏差代价项、纵向速度偏差、纵向控制增量、横向控制增量代价项，N_c为控制时域，e_y(k+i)、

分别为车辆在k+i时刻的横向偏差、航向角偏差、纵向速度偏差，Δa_x(k+i)、Δδ_f(k+i)分别为车辆在k+i时刻的纵向加速度增量、前轮转角增量，Q₁、Q₂、Q₃、R₁、R₂均为权重系数，χ₀(k)、χ(k+1)、χ(k+i)、χ(k+i-1)分别为车辆在k、k+1、k+i、k+i-1时刻的状态量，u₀(k)、u(k+i-1)分别为车辆在k、k+i-1时刻的控制量，a_x(k+j)、δ_f(k+j)分别为车辆在k+j时刻的横、纵向控制量，δ_f,max、a_x,max分别为车辆允许的最大横、纵向控制量，Δδ_f,max、Δa_x,max分别为车辆允许的相邻时刻最大横、纵向控制增量，Δδ_f(k+j)、Δa_x(k+j)分别为车辆在k+j时刻的横、纵向控制增量，f(χ₀(k),u₀(k))、f(χ(k+i-1),u(k+i-1))分别为变维度时延增广模型利用车辆在k、k+i-1时刻的状态量和初始控制量进行计算的结果。

3.如权利要求1或2所述的考虑时变时延的无人车横纵向协同控制方法，其特征在于，步骤2中，预测模型为利用根据底层时变CAN通信延时估计值与执行器时滞特性的横纵向时延环节构建得到的变维度时延增广模型：

其中，

N_τ表示信号因τ_CAN而延后N_τ个采样时间步长dt，χ(k)和χ(k+1)分别为车辆在k、k+1时刻的状态量，χ′(k)和χ′(k+1)分别为χ(k)和χ(k+1)关于时间的一阶导数，a_x(k)和a_x(k+1)分别为车辆在k、k+1时刻的纵向加速度，δ_f(k)和δ_f(k+1)分别为车辆在k、k+1时刻的前轮转向角，

分别为车辆在k-N_τ、k-N_τ+1、k-N_τ+2、k-2、k-1、k时刻的期望前轮转向角，

为车辆在k时刻的期望纵向加速度，dt为采样时间步长，

分别为横、纵向执行器的一阶惯性环节时滞常数，O为零矩阵，

对应为如下简化公式用的过渡矩阵A、B₁、B₂的离散化结果：

B₁＝[1 0 0 0 0]^T

其中，C_cf、C_cr、l_f、l_r、m、v_x、I_z分别为车辆的前轮侧偏刚度、后轮侧偏刚度、质心到前轴中心的距离、质心到后轴中心的距离、质量、纵向速度、转动惯量，κ为跟踪目标点处的道路曲率。

4.如权利要求1或2所述的考虑时变时延的无人车横纵向协同控制方法，其特征在于，步骤1具体包括：

将实时输入的期望控制指令作为实时信号，底层实际控制指令作为延时信号，利用基于FIR的自适应全通滤波时延估计器及基于均方误差评价指标的时延估计稳定策略，获得τ_CAN。

5.如权利要求4所述的考虑时变时延的无人车横纵向协同控制方法，其特征在于，基于FIR的自适应全通滤波时延估计器表示为式(8)或(9)：

式中：x(k)为实时信号，x(k-N_τ)为延时信号，

表示为向量

表示为向量

X₊(k-N_τ)＝[x(k+1-N_τ),...,x(k+n_max-N_τ)]^T为前向向量，X_-(k)＝[x(k-1),...,x(k-n_max)]^T为后向向量。

6.如权利要求4或5所述的考虑时变时延的无人车横纵向协同控制方法，其特征在于，基于均方误差评价指标获得时延估计稳定策略采用式(18)获得：

式中，τ(k)表示k时刻的底层时延估计值，Thr为更新阈值，MSE_last表示k-1时刻的底层时延估计值

的MSE指标，MSE_new表示k时刻的底层时延估计值

的MSE指标，再根据

获得τ_CAN。

7.一种考虑时变时延的无人车横纵向协同控制装置，其特征在于，包括：

自适应全通滤波时延估计器，其用于在线获取底层时变CAN通信延时估计值τ_CAN；

横纵向协同控制器，其用于根据寻找到的预测时域N_p内一系列参考点信息，利用预测模型，结合横纵向一体化评价函数J和横、纵向联合约束s.t.进行最优控制问题滚动求解，输出车辆控制量，其中：最优控制问题被描述为下式：

其中，i为预测状态的索引，k为当前时刻，

分别为车辆在k+i时刻的横向偏差代价项、航向角偏差代价项、纵向速度偏差、纵向控制增量、横向控制增量代价项，N_c为控制时域，e_y(k+i)、

分别为车辆在k+i时刻的横向偏差、航向角偏差、纵向速度偏差，Δa_x(k+i)、Δδ_f(k+i)分别为车辆在k+i时刻的纵向加速度增量、前轮转角增量，Q₁、Q₂、Q₃、R₁、R₂均为权重系数，χ₀(k)、χ(k+1)、χ(k+i)、χ(k+i-1)分别为车辆在k、k+1、k+i、k+i-1时刻的状态量，u₀(k)、u(k+i-1)分别为车辆在k、k+i-1时刻的控制量，a_x(k+j)、δ_f(k+j)分别为车辆在k+j时刻的横、纵向控制量，δ_f,max、a_x,max分别为车辆允许的最大横、纵向控制量，Δδ_f,max、Δa_x,max分别为车辆允许的相邻时刻最大横、纵向控制增量，Δδ_f(k+j)、Δa_x(k+j)分别为车辆在k+j时刻的横、纵向控制增量，f(χ₀(k),u₀(k))、f(χ(k+i-1),u(k+i-1))分别为变维度时延增广模型利用车辆在k、k+i-1时刻的状态量和初始控制量进行计算的结果。

8.如权利要求7所述的考虑时变时延的无人车横纵向协同控制装置，其特征在于，预测模型为利用根据底层时变CAN通信延时估计值与执行器时滞特性的横纵向时延环节构建得到的变维度时延增广模型：

其中，

N_τ表示信号因τ_CAN而延后N_τ个采样时间步长dt，χ(k)和χ(k+1)分别为车辆在k、k+1时刻的状态量，χ′(k)和χ′(k+1)分别为χ(k)和χ(k+1)关于时间的一阶导数，a_x(k)和a_x(k+1)分别为车辆在k、k+1时刻的纵向加速度，+_f(k)和δ_f(k+1)分别为车辆在k、k+1时刻的前轮转向角，

分别为车辆在k-N_τ、k-N_τ+1、k-N_τ+2、k-2、k-1、k时刻的期望前轮转向角，

为车辆在k时刻的期望纵向加速度，dt为采样时间步长，

分别为横、纵向执行器的一阶惯性环节时滞常数，O为零矩阵，

对应为如下简化公式用的过渡矩阵A、B₁、B₂的离散化结果：

B₁＝[1 0 0 0 0]^T

其中，C_cf、C_cr、l_f、l_r、m、v_x、I_z分别为车辆的前轮侧偏刚度、后轮侧偏刚度、质心到前轴中心的距离、质心到后轴中心的距离、质量、纵向速度、转动惯量，κ为跟踪目标点处的道路曲率。

9.如权利要求7或8所述的考虑时变时延的无人车横纵向协同控制装置，其特征在于，底层时变CAN通信延时估计值τ_CAN的获取方法具体包括：

将实时输入的期望控制指令作为实时信号，底层实际控制指令作为延时信号，利用基于FIR的自适应全通滤波时延估计器及基于均方误差评价指标的时延估计稳定策略进行底层时变CAN通信延时的在线估计，输出底层时变CAN通信延时估计值。

10.如权利要求9所述的考虑时变时延的无人车横纵向协同控制装置，其特征在于，基于FIR的自适应全通滤波时延估计器表示为式(8)或(9)：

式中：x(k)为实时信号，x(k-N_τ)为延时信号，

表示为向量

表示为向量

X₊(k-N_τ)＝[x(k+1-N_τ),...,x(k+n_max-N_τ)]^T为前向向量，X_-(k)＝[x(k-1),...,x(k-n_max)]^T为后向向量；

基于均方误差评价指标获得时延估计稳定策略采用式(18)获得：

式中，τ(k)表示k时刻的底层时延估计值，Thr为更新阈值，MSE_last表示k-1时刻的底层时延估计值

的MSE指标，MSE_new表示k时刻的底层时延估计值

的MSE指标，再根据

获得τ_CAN。

Images