[发明专利]一种丢包环境下的车辆侧倾角与俯仰角估计方法

权利要求书

1.一种丢包环境下的车辆侧倾角与俯仰角估计方法,其特征在于包括如下步骤：

1)对四轮车行驶情况建立连续动力学模型，具体包括以下步骤：

1.1)不考虑地球自转速度的情况下，假定侧倾角速度和俯仰角速度为零，建立四轮车行驶过程中的连续动力学方程如式(1)所示：

其中，v_x,v_y分别表示纵向速度和横向速度，上标·表示微分，a_x,a_y分别表示纵向加速度和横向加速度，w_z表示横摆角速度，g表示重力加速度，α,β分别表示侧倾角与俯仰角；

1.2)由式(1)得到侧倾角α和俯仰角β的表达式如式(2)所示：

实际车辆行驶过程中，相对于车辆的纵向速度v_x和车辆的横向速度v_y和均很小能忽略不计，因此式(2)简化后如式(3)所示：

其中α,β分别表示侧倾角与俯仰角，纵向加速度a_x，横向加速度α_y，纵向速度v_x以及横摆角速度w_z由四轮车上安装的传感器测得，而纵向速度v_x的微分通过测得的v_x对时间求导来得到；

2)考虑复杂环境下的混合扰动和无线通讯时的丢包，建立系统状态方程，包括观测方程和输出方程，并构建系统滤波器，具体包括以下步骤：

2.1)离散化后的状态方程如式(4)所示：

x(k+1)＝Ax(k)+w₀(k)+w(k) (4)

其中，k表示当前离散化时刻，k+1表示下一离散化时刻，x表示估计对象，包括侧倾角α和俯仰角β，即x＝[α β]^T，上标T表示矩阵的转置，A表示估计对象x的状态转移矩阵，w₀表示均值为零方差为Q的高斯白噪声，w表示非随机有界扰动信号；

2.2)离散化后的观测方程如式(5)所示：

y(k)＝C₂x(k)+v₀(k) (5)

其中，k表示当前离散化时刻，y表示观测向量，x表示估计对象,C₂表示估计对象x的观测矩阵，v₀表示均值为零方差为R的高斯白噪声；

离散化后的输出方程如式(6)所示：

其中，k表示当前离散化时刻，z,z₀表示系统输出，x表示估计对象；

2.3)基于上述系统设计一个滤波器，如式(7)所示：

其中，k表示当前离散化时刻，k+1表示下一离散化时刻，x表示估计对象，A表示估计对象x的状态转移矩阵，表示估计对象x的估计值，L表示需要设计的滤波器增益，Φ表示期望为μ的伯努利过程，用来模拟丢包环境，C₂表示估计对象x的观测矩阵，y表示观测向量，分别表示系统输出z,z₀的估计值；

该滤波器的作用是使得估计值接近系统输出z,z₀，从而实现对估计对象x,即侧倾角α和俯仰角β的实时高精度估计；

3)给出系统误差模型，进一步给出丢包环境下基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益，具体包括以下步骤：

3.1)通过式(4)、式(5)、式(6)、式(7)得到误差系统模型如式(8)所示：

其中，k表示当前离散化时刻，k+1表示下一离散化时刻，e_x表示估计对象x与对应估计值的差值,e表示系统输出z与对应估计值的差值，e₀表示系统输出z₀与对应估计值的差值，A表示估计对象x的状态转移矩阵，L表示需要设计的滤波器增益，Φ表示期望为μ的伯努利过程，用来模拟丢包环境，C₂表示估计对象x的观测矩阵，w₀表示均值为零方差为Q的高斯白噪声，v₀表示均值为零方差为R的高斯白噪声，w表示非随机有界扰动信号；

3.2)基于误差系统，定义最差的非随机有界扰动信号w如式(9)所示：

w(k)＝We_x(k) (9)

将式(9)代入式(8)，进一步得到最差非随机扰动下的系统误差模型如式(10)所示：

定义中间矩阵A_L和A_W分别如式(11)、式(12)所示：

A_L＝A+μLC₂ (11)

A_W＝A+W (12)

其中，k表示当前离散化时刻，k+1表示下一离散化时刻，e_x表示估计对象x与对应估计值的差值,e表示系统输出z与对应估计值的差值，e₀表示系统输出z₀与对应估计值的差值，A表示估计对象x的状态转移矩阵，L表示需要设计的滤波器增益，Φ表示期望为μ的伯努利过程，用来模拟丢包环境，C₂表示估计对象x的观测矩阵，w₀表示均值为零方差为Q的高斯白噪声，v₀表示均值为零方差为R的高斯白噪声，w表示非随机有界扰动信号，μ表示伯努利过程Φ的期望值，W，A_L和A_W均为中间矩阵；

3.3)当k＝0时,对中间矩阵P₁和P₂以及中间矩阵W和滤波器增益L赋初值，如式(13)所示：

P₁(0),P₂(0),W(0),L(0) (13)

3.4)基于误差系统，由H_∞滤波算法得到最差的非随机有界扰动信号w如式(14)所示：

w(k)＝(γ²P₁^-1-I_n)^-1A_Le_x(k) (14)

因此如式(15)所示：

W＝(γ²P₁^-1-I_n)^-1A_L (15)

其中，k表示当前离散化时刻,w表示非随机有界扰动信号，γ为预设的常数，I表示单位矩阵，上标-1表示矩阵的逆，上标T表示矩阵的转置，W,A_L和P₁均为中间矩阵，e_x表示估计对象x与对应估计值的差值；

3.5)丢包环境下基于最差非随机扰动下的系统误差模型，通过H₂滤波算法得到滤波器增益L的表达式如式(16)所示：

其中，上标-1表示矩阵的逆，上标T表示矩阵的转置，L表示需要设计的滤波器增益，C₂表示估计对象x的观测矩阵，R表示高斯白噪声v₀的方差，A_W和P₂均为中间矩阵；

3.6)当k＝1时，由式(11)、式(12)、式(15)、式(16)分别得到式(17)、式(18)、式(19)及式(20)所示：

A_W(1)＝A+W(0) (17)

A_L(1)＝A+μL(0)C₂ (18)

W(1)＝W＝(γ²P₁^-1(0)-I_n)^-1A_L(1) (19)

其中，A表示估计对象x的状态转移矩阵，μ表示伯努利过程Φ的期望值，L表示需要设计的滤波器增益，C₂表示估计对象x的观测矩阵，γ为预设的常数，I表示单位矩阵，上标-1表示矩阵的逆，上标T表示矩阵的转置，w表示非随机有界扰动信号，R表示高斯白噪声v₀的方差，W,A_L,A_W,P₁和P₂均为中间矩阵；

3.7)中间矩阵P₁满足如下Riccati黎卡提方程如式(21)所示：

因此得到中间矩阵P₁(1)如式(22)所示：

其中，k表示当前离散化时刻，k+1表示下一离散化时刻，L表示需要设计的滤波器增益，C₂表示估计对象x的观测矩阵，γ为预设的常数，I表示单位矩阵，μ表示伯努利过程Φ的期望值，上标-1表示矩阵的逆，上标T表示矩阵的转置，w表示非随机有界扰动信，A_L和P₁均为中间矩阵；

3.8)中间矩阵P₂满足如下Riccati(黎卡提)方程如式(23)所示：

因此得到中间矩阵P₂(1)如式(24)所示：

其中，k表示当前离散化时刻，k+1表示下一离散化时刻，C₂表示估计对象x的观测矩阵，上标-1表示矩阵的逆，上标T表示矩阵的转置,Q表示高斯白噪声w₀的方差，R表示高斯白噪声v₀的方差,μ表示伯努利过程Φ的期望值,A_W和P₂均为中间矩阵；

3.9)重复步骤3.6)、步骤3.7)及步骤3.8)：

若k＝T时刻时，矩阵P₁(T)和矩阵P₁(T-1)差值的二范数小于给定误差，分别得到如式(25)、式(26)所示：

P₁＝P₁(T)＝P₁(T-1) (25)

A_L＝A_L(T)＝A_L(T-1) (26)

同样，若k＝T时刻时，矩阵P₂(T)和矩阵P₂(T-1)差值的二范数小于给定误差，得到分别得到如式(27)、式(28)所示：

P₂＝P₂(T)＝P₂(T-1) (27)

A_W＝A_W(T)＝A_W(T-1) (28)

其中，A_L,A_W,P₁和P₂均为中间矩阵；

3.10)将中间矩阵P₂和中间矩阵A_W代入式(16)得到滤波器增益矩阵L，从而由滤波器(7)，实现对估计对象x的实时高精度估计，估计对象x为侧倾角α和俯仰角β。