CN112099378B - 考虑随机测量时滞的前车侧向运动状态实时估计方法 - Google Patents

Abstract

一种考虑随机测量时滞的前车侧向运动状态实时估计方法，属于车辆控制技术领域。本发明的目的是对考虑测量时滞的车辆侧向运动系统进行建模描述，之后根据所建立的模型，在滚动时域估计(Moving Horizon Estimation‑MHE)框架下设计前车侧向运动状态实时估计的考虑随机测量时滞的前车侧向运动状态实时估计方法。本发明的步骤是：高保真车辆模型搭建、考虑测量时滞的车辆侧向运动建模与描述、估计器设计。本发明提高了模型精度，对外部干扰具有更好的鲁棒性。

Description

考虑随机测量时滞的前车侧向运动状态实时估计方法

技术领域

本发明属于车辆控制技术领域。

背景技术

随着人工智能技术和信息技术的不断发展，汽车的智能和信息化程度也在不断地提高，各种先进驾驶辅助系统和自动驾驶系统也得到了发展和应用。对于智能车辆或先进驾驶辅助系统而言，车辆的决策、规划与控制系统都需要获得必要的信息支持。因此，在此背景下有必要对前车的侧向运动状态进行获取。但由于车辆是一个复杂的非线性系统，在现有的传感器技术的基础上，无法对一些关键的运动信息进行直接的测量。因此，需要设计估计算法来根据有限的测量信息来对无法直接测量的信息进行估计。现有的针对前车侧向运动状态的估计算法存在以下问题：

1.在实际的系统中，测量信号并不是实时更新的，而是存在一定时间滞后的。而现有的估计算法大多都将时滞忽略掉了，从而降低了估计的精度。而当时滞较大时，甚至会造成估计器的不稳定和估计值的发散。

2.实际的车辆是一个复杂的非线性系统，而在传统的估计算法中，如卡尔曼滤波(KF)和扩展卡尔曼滤波中(EKF)中，车辆系统往往都被简化为一个线性系统。线性简化处理会引入估计误差，甚至在系统的非线性较强时，会造成估计器不稳定以及估计值的发散。

3.在传统的估计器的设计中，如卡尔曼滤波(KF)和扩展卡尔曼滤波中(EKF)中，只考虑当前时刻的测量值，严重限制了估计器的精度。首先，当时滞较大时，当前时刻的测量值是不可获取的，只能用最近时刻的可用测量值，这会造成估计误差的增大。其次，若只考虑当前时刻的测量值，当传感器受到短时扰动的影响时，会造成估计误差大幅增加，同时扰动消失后收敛速度也较慢。

发明内容

本发明的目的是对考虑测量时滞的车辆侧向运动系统进行建模描述，之后根据所建立的模型，在滚动时域估计(Moving Horizon Estimation-MHE)框架下设计前车侧向运动状态实时估计的考虑随机测量时滞的前车侧向运动状态实时估计方法。

本发明步骤是：

步骤一、高保真车辆模型搭建：高保真车辆模型模拟真实的被控对象，主要作用是能够精确的模拟实际车辆的侧向运动特征；

步骤二、估计器设计：根据推导所得的考虑测量时滞的系统状态空间方程和滚动时域估计原理设计估计器，对前车的侧向速度以及横摆角速度进行估计；

1)前车侧向运动模型建立

车辆二自由度模型：

建立模型时只考虑了车辆的侧向运动和横摆运动，在假设两个前轮转动角度相同的条件下，根据车辆动力学得到车辆的二自由度模型如下：

代表车辆侧向速度的导数，

代表了车辆的横摆角速度的导数，v_x表示车辆沿道路行驶的纵向速度，C_f为车辆前轮的等效侧偏刚度，C_r为车辆后轮的等效侧偏刚度，L_f和L_r分别表示车辆度的前半轴距和后半轴距，m是车辆的质量，I_z是车辆绕质心旋转的转动惯量，δ_f为驾驶员转动的前轮转角；

考虑道路信息的车辆侧向运动模型：

在建立模型时可以假设前轮转角在各个小的采样周期内为恒定的，可得：

综合考虑侧向位移偏差e、车辆航向角和道路中心线的切线方向的航向角偏差Ψ、车辆沿道路行驶的纵向速度v_x、车辆的侧向速度v_y以及所行驶的道路的曲率c，并结合车辆的运动特性，便可推导出描述车辆跟踪目标道路中心线的误差动态方程：

代表车辆沿道路中心线的侧向位移偏差的导数，

代表了车辆航向角沿道路中心线切线方向的航向角偏差的导数；

在每个小的采样周期内的道路曲率是不变的，则此时便可得到：

综合考虑式(1)和(2)中描述的车辆二自由度方程以及式(3)-(7)中描述的考虑道路信息的运动模型，可得考虑行驶道路信息的前车侧向运动的系统的连续状态空间方程为：

状态向量为x＝(v_y,γ,δ_f,e,Ψ,c)^T，由传感器测量到的测量值为y＝(e,Ψ,c)^T，其中的测量值y＝(e,Ψ,c)可以由后车搭载的视觉系统进行测量；过程噪声ω代表了系统模型的不确定性，测量噪声ν代表了系统传感器的测量不确定性；

假设系统模型的离散化时间为T_s，经欧拉离散化后的系统模型便可表示为：

在经离散化后的模型中x_k+1和x_k分别为表示前车侧向运动系统在第k+1和第k时刻的状态向量，ω_k表示在第k时刻的系统过程噪声，υ_k则表示在第k时刻的系统的测量噪声，设协方差矩阵分别为Q和R，H为测量矩阵，y_k为该系统在第k时刻的测量值；

2)考虑测量时滞与多速率的系统模型建立：

假设系统模型的离散化时间为T_s，设系统的测量系统的采样时间为nT_s，即每隔n个系统状态的更新周期进行一次测量值的采样，结合系统的离散化模型可得如下方程：

对缓存器的N个存储单元进行编号，编号为0,1,…,N-1，之后定义一个随机变量

其中j＝0,1,...,N-1为存储单元的编号，t代表了时间，当第j个存储单元的测量值在时间t或t之前到达时

否则

将随机变量与系统的状态空间模型相结合，便可得到描述缓存器内状态和测量数据的模型如下：

在上述方程中

k∈[0,n(N-1)]代表了在缓存器中在时刻t的状态值，

则表示在时刻t缓存器中包含随机测量时滞的测量值，

表示在时刻t缓存器中第j个存储单元对应的测量矩阵，

则表示在时刻t相应存储单元的测量噪声，同时定义新的测量噪声协方差矩阵为

3)前车侧向运动状态估计器设计：

由滚动时域估计的原理可以将估计问题概括为如下形式：

其中

以及

分别是状态变量、过程噪声和测量噪声序列；

为在整个估计起点的初始状态估计；

4)考虑随机时滞的扩展卡尔曼滤波器设计：

步骤1：预测协方差矩阵更新

步骤2：计算卡尔曼增益

步骤3：计算状态估计值

步骤4：估计协方差矩阵更新

k∈[0,N-1]表示缓存器的第一个到最后一个的编号，t则表示当前的时刻，

表示第k个存储单元的卡尔曼增益，

和

分别表示预测值的协方差矩阵和估计值的协方差矩阵；

表示在第k号存储单元的雅克比矩阵，其计算过程如下：

本发明的有益效果是：

1.本发明在进行系统建模时考虑了测量值的时间滞后的影响，对包含时滞的车辆系统进行了建模，该模型可以更加精确地描述车辆沿道路行驶的非线性特性；

2.本发明在对前车的侧向运动进行建模时，充分地考虑了车辆运动的非线性特性，提高了模型精度；

3.本发明采用的前车侧向运动状态估计模块是基于滚动时域估计原理的，滚动时域估计将车辆自身以及车辆沿道路行驶的非线性特性考虑在内，因此可以得到更高的估计精度；此外由于引进了估计窗口，可以综合考虑系统在某一时间段内的测量信息，因此对外部干扰具有更好的鲁棒性。

附图说明

图1是跟车情景下的示意图；

图2是本发明所述的基于滚动时域估计原理所设计的估计器的估计方法框图；

图3是本发明中所使用的车辆二自由度模型示意图；

图4是本发明用于构建考虑道路信息的车辆运动模型所采用的Serret-Frenet的示意图；

图5是本发明中所构建的用于存储包含随机测量时滞测量值的缓存器示意图；

图6是本发明中考虑随机测量时滞的基于扩展卡尔曼滤波原理的迭代状态估计器示意图；

图7是本发明所述的仿真实施例1中关于前车侧向速度的估计结果曲线；

图8是本发明所述的仿真实施例1中关于前车横摆角速度的估计结果曲线；

图9是本发明所述的仿真实施例2中关于系统受到扰动后的估计结果曲线。

具体实施方式

本发明涉及一种考虑随机测量时滞的针对前车侧向运动状态实时估计方法，具体涉及一种基于多速率车辆运动模型的侧向运动状态滚动时域估计方法。更具体地说，随着车辆智能化程度的不断提高，需要对周围车辆尤其是前方车辆的运动状态进行实时获取，为本车的决策、规划与控制系统提供必要的信息支持。

在跟车情景下，本车的先进驾驶辅助系统或自动驾驶系统需要对前方车辆侧向运动状态进行获取的需求，本发明在考虑测量时滞的条件下，设计了一种针对前车侧向运动状态的实时估计方法。在实际的车辆系统中，尤其是描述车辆的侧向运动时，车辆是一个复杂的非线性系统，同时传感器获取前车信息也有时间滞后的特点。因此，本发明首先对考虑测量时滞的车辆侧向运动系统进行建模描述，之后根据所建立的模型，在滚动时域估计(Moving Horizon Estimation-MHE)框架下设计前车侧向运动状态的实时估计方法。

本发明是采用如下技术方案实现的：

步骤一、高保真车辆模型搭建：在CarSim软件中选择车辆模型，并将车辆的运动状态参数读取到Simulink中，基于所选的车辆模型构建跟车情景下的仿真工况，模拟实际车辆的侧向运动特征；

步骤二、考虑测量时滞的车辆侧向运动建模与描述：在考虑测量值随机时间滞后的条件下，结合车辆动力学，对车辆的侧向运动进行建模与描述，为估计器的设计提供系统模型；

步骤三、估计器设计：设计基于滚动时域估计原理的前车侧向运动状态估计器，实现对前车的侧向速度以及横摆角速度的估计。

本发明的应用场景的结构简图如图1所示，从后方跟随车的角度出发，由搭载在跟随车上的传感器对前方车辆的信息进行测量，根据这些测量信息对前方车辆的侧向速度和横摆角速度进行估计。其中前方车辆的纵向速度可通过车载雷达测量或车辆间的通讯系统(V2V)传递得到。在本发明方案中，假设前方车辆的速度已知。而前方车辆的固有参数，如质量、轴距、轮胎侧偏刚度等可由车载视觉系统识别或通过V2V传递得到。在本发明中跟随车的测量值为前车的侧向位移和航向角相对于道路中心线的偏差，以及道路的曲率。在本发明中，估计器的搭建是在MATLAB/Simulink中完成的。

本发明的估计目标是，在跟车情景下，且在考虑传感器测量时滞的条件下，根据跟随车的车载传感器的测量信息，基于滚动时域估计原理设计估计器，准确地估计前方车辆的侧向运动状态。

本发明所述的跟车情景下的前车侧向运动的状态估计是通过软件系统的联合仿真实现的。

1、软件选择

本发明所述的估计器的设计是通过软件Matlab/Simulink进行搭建的，版本为Matlab R2019b，而被估计对象，即车辆模型是通过专业车辆动力学仿真软件CarSim进行搭建的，版本为CarSim2016.1。在整个仿真测试系统中，系统的工作周期为0.01s，求解器为ODE1欧拉求解器。上述软件中，CarSim软件是一个商用的高保真车辆动力学仿真平台，它在本发明中的主要作用是提供高保真的车辆动力学模型以及相应的仿真工况，在仿真实验中这一模型代替了真实的车辆作为所设计的估计方法的实施对象；MATLAB/Simulink软件则是用于估计器的仿真模型搭建，即通过MATLAB/Simulink编程来完成该方法中估计器的运算。

2、联合仿真设置

要实现两者的联合仿真，首先需要在Matlab的路径设置中添加CarSim的路径；其次在CarSim界面中添加输出接口模块；然后将CarSim中的模型信息经过系统编译之后以CarSimS-function的形式保留在Simulink中，最后再进行Simulink中CarSim模块的参数设置。在运行Simulink仿真模型时，CarSim模型也在同时进行计算和求解。仿真过程中两者之间不断进行数据的交换。如果对CarSim中的模型结构或者参数设置进行了修改，则需要重新编译，之后将新的包含最新设定信息的CarSim模块重新发送至Simulink中。

本发明所述的一种考虑测量时滞的前车侧向运动状态估计方法，首先根据车辆动力学知识推导出可以对车辆侧向运动进行描述的系统模型，并在考虑测量时滞的条件下推导出相应的状态空间方程；其次，在高保真车辆动力学仿真软件CarSim中选择合适的车辆模型并获取相应参数；之后基于所选的车辆模型构建跟车情景下的仿真工况；然后根据推导所得的状态空间方程和滚动时域估计原理进行估计器设计，从而实现对前方车辆侧向速度和横摆角速度的估计。

本发明具体包括以下步骤：

步骤一、高保真车辆模型搭建：高保真车辆模型模拟真实的被控对象，主要作用是能够精确的模拟实际车辆的侧向运动特征。

在本发明中，由于使用了联合仿真，因此在CarSim中，主要用到的是车辆模型选择以及仿真工况的构建。

首先选择典型的乘用车模型，之后对模型的相关参数进行修改并获取，将车辆模型参数添加到Simulink仿真模型中。车辆的主要模型参数有车辆质量、前后轴距轮胎侧偏刚度等。在选择相应的车辆模型和参数之后，需要构建相应的仿真工况，仿真工况中可以选择车辆的行驶路线，行驶环境以及驾驶员模型等。由于在本发明中只是对车辆的行驶运动状态进行估计，因此选用CarSim中自带的驾驶员模型，并将车辆的运动状态参数读取到Simulink中，基于所选的车辆模型构建跟车情景下的仿真工况。

步骤二、估计器设计：根据推导所得的考虑测量时滞的系统状态空间方程和滚动时域估计原理设计估计器，对前车的侧向速度以及横摆角速度进行估计。

本发明的被估计对象是在跟车情景下的前方车辆，因此估计目标就是根据可用的传感器测量信息估计出前方车辆的侧向速度以及横摆角速度。主要设计过程描述如下。首先，针对前车的侧向运动进行建模，建立合适的数学模型。

1)前车侧向运动模型建立

车辆二自由度模型：

在本发明中，假设前方车辆的纵向速度可由车载雷达或V2V获取，因此在建立模型时只考虑了车辆的侧向运动和横摆运动。在描述车辆的侧向运动和横摆运动时，车辆的二自由度模型是一种被普遍采用的车辆模型。如图3所示，一个车辆的前轴和后轴轮胎(浅灰色轮胎)分别压缩至一个轮胎(黑色轮胎)。在该模型中假设车辆的纵向速度是不变的，且驾驶员通过方向盘来转动前轮。在假设两个前轮转动角度相同的条件下，根据车辆动力学的相关知识便可得到车辆的二自由度模型如下：

在上述方程中中，

代表车辆侧向速度的导数，

代表了车辆的横摆角速度的导数，v_x表示车辆沿道路行驶的纵向速度，C_f为车辆前轮的等效侧偏刚度，C_r为车辆后轮的等效侧偏刚度，L_f和L_r分别表示车辆度的前半轴距和后半轴距，m是车辆的质量，I_z是车辆绕质心旋转的转动惯量，δ_f为驾驶员转动的前轮转角。

考虑道路信息的车辆侧向运动模型：

由于车辆在行驶过程中是沿着道路行驶的，因此不仅要考虑车辆自身的动力学特性，还要考虑车辆的运动状态和道路之间的相对关系，例如在行驶过程中道路的中心线和车辆中心的垂直距离以及车辆航向角和道路中心线切线方向的角度偏差。由于前方车辆的前轮转角是不可测量的，因此可以参考在车辆状态估计中广泛采用的恒速度和恒加速度模型的建模方法，在建立模型时可以假设前轮转角在各个小的采样周期内为恒定的，可得：

为了描述车辆运动状态和道路之间的相对关系，使用Serret-Frenet方程对车辆路径跟踪误差动态进行建模与描述。如图4所示，当驾驶员驾驶车辆沿着目标路径的中心线行驶时，车辆的中心与道路中心线并不会完全重合，从而导致了侧向位移偏差e以及车辆航向角和道路中心线的切线方向的航向角偏差Ψ。除此之外还要同时考虑车辆沿道路行驶的纵向速度v_x、车辆的侧向速度v_y以及所行驶的道路的曲率c。综合考虑以上因素并结合车辆的运动特性，便可推导出描述车辆跟踪目标道路中心线的误差动态方程：

在方程(4)和方程(5)中，

代表车辆沿道路中心线的侧向位移偏差的导数，

代表了车辆航向角沿道路中心线切线方向的航向角偏差的导数。由于在车辆正常行驶过程中，车辆的侧向速度一般都远远小于车辆的纵向速度，且车辆沿道路中心线的航向角误差Ψ也比较小，因此可以将方程中的v_ysinΨ忽略掉，且类似于恒速度模型和恒加速度模型，认为在每个小的采样周期内的道路曲率是不变的，则此时便可得到：

综合考虑式(1)和(2)中描述的车辆二自由度方程以及式(3)-(7)中描述的考虑道路信息的运动模型，可得考虑行驶道路信息的前车侧向运动的系统的连续状态空间方程为：

此系统中的状态向量为x＝(v_y,γ,δ_f,e,Ψ,c)^T，由传感器测量到的测量值为y＝(e,Ψ,c)^T。其中的测量值y＝(e,Ψ,c)可以由后车搭载的视觉系统进行测量。在式(8)所描述的系统方程中，过程噪声ω代表了系统模型的不确定性，测量噪声ν代表了系统传感器的测量不确定性。

由于计算机系统是一个离散化的系统，因此需要对式(8)中所描述的系统模型进行离散化处理，现假设系统模型的离散化时间为T_s，经欧拉离散化后的系统模型便可表示为：

在经离散化后的模型中x_k+1和x_k分别为表示前车侧向运动系统在第k+1和第k时刻的状态向量，ω_k表示在第k时刻的系统过程噪声，υ_k则表示在第k时刻的系统的测量噪声，在本发明中系统的过程噪声和测量噪声均为高斯白噪声序列，且设它们的协方差矩阵分别为Q和R，H为测量矩阵，y_k为该系统在第k时刻的测量值。

2)考虑测量时滞与多速率的系统模型建立：

在本发明中，由于系统的测量值是由车载视觉系统测量得到的，因此需要对对所采集到的图像进行处理。同时由于采集到的图像不同，因此所需要的处理时间也不同，而图像处理的时间会造成测量值的滞后，因此本系统中的测量值会伴随着随机时滞。而另一方面，由于车载视觉系统的有限的采集能力，因此对图像的采集频率不能过高。同时为了保证离散化的模型有较高的精度，模型的离散时间T_s应选择为一个较小的值。综合考虑上述两点，本发明便提出了一个多速率系统，即系统模型的状态向量以较高的频率进行更新，而系统的测量系统则以较低的频率进行采样。

首先需要对系统的多速率特性进行建模与描述，现假设系统模型的离散化时间为T_s，设系统的测量系统的采样时间为nT_s，即每隔n个系统状态的更新周期进行一次测量值的采样。结合系统的离散化模型可得如下方程：

在本发明中，系统的测量值由一个缓存器来存贮，其中缓存器的存储单元个数为N个，即每个存储单元可以存储一次测量采样的测量值。由于测量系统工作在一个较低的频率下，即每隔n个系统更新周期采样一次，因此缓存器中存储的数据也随着测量系统的频率来更新。如图5所示，存储器每隔n个系统更新周期更新一次，当每个存储单元的测量值到达后，该单元存储相应的测量值，否则将存储一个0在存储单元内。由于在初始时刻，系统的测量值个数不足N个，因此空置的存储单元被标记为N/A。随着测量值的更新，缓存器中的第一个存储单元内的数据在下一次更新时被删除，其余单元的数据依次向左移动一个存储单元。空出的最后一个存储单元将被用来存储新数据。

为了更好地对包含时滞的系统进行描述同时也为估计器的设计提供模型基础，需要对包含随机时滞的系统进行建模与描述。首先对缓存器的N个存储单元进行编号，编号为0,1,…,N-1，之后定义一个随机变量

其中j＝0,1,...,N-1为存储单元的编号，t代表了时间。当第j个存储单元的测量值在时间t或t之前到达时

否则

通过该随机变量的定义可知，当某一存储单元的测量值到达之后，该测量值在未来时刻内一直可用直到随着缓存器的更新而将其替换掉。之后将该随机变量与系统的状态空间模型相结合，便可得到描述缓存器内状态和测量数据的模型如下：

在上述方程中

k∈[0,n(N-1)]代表了在缓存器中在时刻t的状态值，

则表示在时刻t缓存器中包含随机测量时滞的测量值，

表示在时刻t缓存器中第j个存储单元对应的测量矩阵，

则表示在时刻t相应存储单元的测量噪声，同时定义新的测量噪声协方差矩阵为

3)前车侧向运动状态估计器设计：由于所建立的系统模型为一个非线性模型，因此在本发明中为了获得更高的估计精度以及相对初始误差和测量扰动更好的鲁棒性，本发明基于滚动时域估计原理并结合本发明中所述的考虑随机测量时滞的系统模型，进行估计器的设计来对前方车辆的侧向速度和横摆角速度进行估计。其中估计器的测量值则由车载的视觉系统进行测量，测量值包括前方车辆相对道路中心线的位移偏差e，相对道路中心线的航向角偏差Ψ以及道路的曲率c。估计器中的前方车辆的纵向速度在每次估计时都实时更新。

根据滚动时域估计的原理可知，在估计器的设计中首先要定义一个估计窗口长度N，与上述的数据缓存器相对应，该窗口代表了用来存储过去N个测量值的N个存储单元。对着估计器的初始化，估计器的窗口长度随着时间而增长，在到达设定的长度之前，此时的滚动时域估计相当于是一个全息的估计问题，而当到达设定的长度后，状态估计问题便需要进行近似与简化，此时的估计相当于近似估计问题。此时由滚动时域估计的原理可以将估计问题概括为如下形式：

其中

以及

分别是状态变量、过程噪声和测量噪声序列。在上述的目标函数中，第一项称为到达代价，它是用来对过去所丢失的测量信息的一种近似，其中的

为在整个估计起点的初始状态估计。在上述目标函数中，三个子项的权重分别为初始状态估计值的协方差矩阵

的逆矩阵，过程噪声协方差矩阵Q的逆矩阵以及测量噪声协方差矩阵

的逆矩阵。协方差矩阵的逆矩阵代表着在目标函数中各个子项的信任程度，即越大的协方差矩阵则代表对该部分越不信任，相对的越小的协方差矩阵则表示对该部分越信任。由于随着时间不断向前滚动，测量值也在不断更新，因此到达代价也需要不断更新，在本发明中到达代价的更新是通过考虑随机时滞的展卡尔曼滤波器进行的，关于该滤波器的设计过程在下文给出。

4)考虑随机时滞的扩展卡尔曼滤波器设计：为了对滚动时域估计器的到达代价进行更新，同时也为了设计出一个用于对比估计性能的估计器，在本发明中设计了一个考虑随机测量时滞的扩展卡尔曼滤波器，用于估计前车的侧向运动状态。

如图6所示，在当前时刻的状态估计值可根据缓存器中的测量值不断迭代计算而得到。其更新过程如下所示：

步骤1：预测协方差矩阵更新

步骤2：计算卡尔曼增益

步骤3：计算状态估计值

步骤4：估计协方差矩阵更新

在上述的状态估计步骤中，k∈[0,N-1]表示缓存器的第一个到最后一个的编号，t则表示当前的时刻，

表示第k个存储单元的卡尔曼增益，

和

分别表示预测值的协方差矩阵和估计值的协方差矩阵。上述的迭代过程的初值由上一测量采样时刻的第一个存储单元的估计结果提供。通过上述的迭代计算，便可得到当前时刻的状态估计值

同时在滚动时域估计器中，到达代价中的预测值即为第一个存储单元中的估计值。

表示在第k号存储单元的雅克比矩阵，其计算过程如下：

该滤波器的性能也将被用于对比估计性能。

实验对比验证

为了验证本发明中所设计估计器的性能，在本发明中设计了仿真实验进行了验证。在仿真中分别验证了滚动时域估计器以及考虑测量随机时滞的扩展卡尔曼滤波器。在仿真实验中的最大测量时滞为5个测量值采样周期，最小时滞为1个测量值采样周期。

1)侧向运动状态估计实验

在本发明的估计仿真实验中，所用到的车辆模型是CarSim中的高保真车辆模型。为了充分验证本发明所述的估计器的有效性，采用了双移线工况来进行验证，且在双移线工况中分别令车辆在低速、高速、加速和减速的工况下行驶。

在行驶中的低速和高速工况的速度分别为25m/s和30m/s，加速和减速工况的加速度分别为0.6m/s²和-1.2m/s²。在该仿真中，模型的离散时间为T_s＝0.001s，系数n＝4，缓存器单元数N＝12，仿真模型的仿真步长设定为0.01s，求解器选用为欧拉求解器。系统噪声协方差矩阵设定为Q＝diag(9.6E-6,1.02E-16,2.4E-9,8.2E-7,1.26E-15,7.4E-13)，系统的观测噪声的协方差矩阵设定为R＝diag(3.2E-3,6.9E-6,4.5E-6)，系统估计协方差矩阵的初值设定为P＝diag(1,1,1,1,1,1)×0.000001。车辆的主要参数为车辆质量m＝1270kg，车辆前半轴距L_f＝1.015m，车辆后半轴距L_r＝1.895m，车辆绕质心的转动惯量I_z＝1536.7kg·m²，前轮等效侧偏刚度C_f＝135000N/rad，后轮等效侧偏刚度C_r＝85000N/rad。

关于估计精度的仿真实验图7和图8中给出，其中图7是关于前方车辆的侧向速度的估计结果，图8中给出的是关于前方车辆的横摆角速度的估计结果。从图中可以看出，相比于基于卡尔曼估计原理的估计器，本发明中所述的基于滚动时域估计原理的估计器拥有较高的估计精度。

2)加入干扰实验，在本实验中，为了验证本发明所提出的滚动时域估计器的抗干扰性和基于考虑随机测量时滞的扩展卡尔曼估计算法的抗干扰性。在第9s时，对系统的测量值加入干扰，其干扰大小为[-6,-0.3,-0.1]，干扰的持续时间为0.15s。

实验结果如图9所示，在干扰的作用下基于卡尔曼滤波的估计器和本发明所设计的滚动时域估计器的估计误差均会大幅上升，但相比之下，本发明所设计的滚动时域估计器的误差变化较小。且当干扰消失后，本发明所设计的滚动时域估计器可以更快地收敛到真实值附近，体现了本发明所提出的滚动时域估计器的优越性。

Claims (1)

1.一种考虑随机测量时滞的前车侧向运动状态实时估计方法，其步骤是：

步骤一、高保真车辆模型搭建：高保真车辆模型模拟真实的被控对象，主要作用是能够精确的模拟实际车辆的侧向运动特征；

其特征在于：

步骤二、估计器设计：根据推导所得的考虑测量时滞的系统状态空间方程和滚动时域估计原理设计估计器，对前车的侧向速度以及横摆角速度进行估计；

1)前车侧向运动模型建立

车辆二自由度模型：

建立模型时只考虑了车辆的侧向运动和横摆运动，在假设两个前轮转动角度相同的条件下，根据车辆动力学得到车辆的二自由度模型如下：

代表车辆侧向速度的导数，

代表了车辆的横摆角速度的导数，v_x表示车辆沿道路行驶的纵向速度，C_f为车辆前轮的等效侧偏刚度，C_r为车辆后轮的等效侧偏刚度，L_f和L_r分别表示车辆度的前半轴距和后半轴距，m是车辆的质量，I_z是车辆绕质心旋转的转动惯量，δ_f为驾驶员转动的前轮转角；

考虑道路信息的车辆侧向运动模型：

在建立模型时假设前轮转角在各个小的采样周期内为恒定的，得：

综合考虑侧向位移偏差e、车辆航向角和道路中心线的切线方向的航向角偏差Ψ、车辆沿道路行驶的纵向速度v_x、车辆的侧向速度v_y以及所行驶的道路的曲率c，并结合车辆的运动特性，推导出描述车辆跟踪目标道路中心线的误差动态方程：

代表车辆沿道路中心线的侧向位移偏差的导数，

代表了车辆航向角沿道路中心线切线方向的航向角偏差的导数；

在每个小的采样周期内的道路曲率是不变的，则此时得到：

综合考虑式(1)和(2)中描述的车辆二自由度方程以及式(3)-(7)中描述的考虑道路信息的运动模型，得考虑行驶道路信息的前车侧向运动的系统的连续状态空间方程为：

状态向量为x＝(v_y,γ,δ_f,e,Ψ,c)^T，由传感器测量到的测量值为y＝(e,Ψ,c)^T，其中的测量值y＝(e,Ψ,c)由后车搭载的视觉系统进行测量；过程噪声ω代表了系统模型的不确定性，测量噪声ν代表了系统传感器的测量不确定性；

假设系统模型的离散化时间为T_s，经欧拉离散化后的系统模型表示为：

在经离散化后的模型中x_k+1和x_k分别为表示前车侧向运动系统在第k+1和第k时刻的状态向量，ω_k表示在第k时刻的系统过程噪声，υ_k则表示在第k时刻的系统的测量噪声，设协方差矩阵分别为Q和R，H为测量矩阵，y_k为该系统在第k时刻的测量值；

2)考虑测量时滞与多速率的系统模型建立：

假设系统模型的离散化时间为T_s，设系统的测量系统的采样时间为nT_s，即每隔n个系统状态的更新周期进行一次测量值的采样，结合系统的离散化模型得如下方程：

对缓存器的N个存储单元进行编号，编号为0,1,…,N-1，之后定义一个随机变量

其中j＝0,1,...,N-1为存储单元的编号，t代表了时间，当第j个存储单元的测量值在时间t或t之前到达时

否则

将随机变量与系统的状态空间模型相结合，得到描述缓存器内状态和测量数据的模型如下：

在上述方程中

代表了在缓存器中在时刻t的状态值，其中k∈[0,N-1]，

则表示在时刻t缓存器中包含随机测量时滞的测量值，

表示在时刻t缓存器中第j个存储单元对应的测量矩阵，

则表示在时刻t相应存储单元的测量噪声，同时定义新的测量噪声协方差矩阵为

3)前车侧向运动状态估计器设计：

由滚动时域估计的原理将估计问题概括为如下形式：

其中

以及

分别是状态变量、过程噪声和测量噪声序列；

为在整个估计起点的初始状态估计；

4)考虑随机时滞的扩展卡尔曼滤波器设计：

步骤1：预测协方差矩阵更新

步骤2：计算卡尔曼增益

步骤3：计算状态估计值

步骤4：估计协方差矩阵更新

k∈[0,N-1]表示缓存器的第一个到最后一个的编号，t则表示当前的时刻，

表示第k个存储单元的卡尔曼增益，

和

分别表示预测值的协方差矩阵和估计值的协方差矩阵；

表示在第k号存储单元的雅克比矩阵，其计算过程如下：

Images