CN108413923A - 一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法 - Google Patents
一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法 Download PDFInfo
- Publication number
- CN108413923A CN108413923A CN201810060029.7A CN201810060029A CN108413923A CN 108413923 A CN108413923 A CN 108413923A CN 201810060029 A CN201810060029 A CN 201810060029A CN 108413923 A CN108413923 A CN 108413923A
- Authority
- CN
- China
- Prior art keywords
- matrix
- representing
- denotes
- equation
- estimation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 30
- 238000001914 filtration Methods 0.000 title claims abstract description 25
- 239000011159 matrix material Substances 0.000 claims description 180
- 230000001133 acceleration Effects 0.000 claims description 16
- 230000007704 transition Effects 0.000 claims description 14
- 230000014509 gene expression Effects 0.000 claims description 6
- 230000000694 effects Effects 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 4
- 230000017105 transposition Effects 0.000 claims description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 2
- 239000011295 pitch Substances 0.000 description 39
- 230000007547 defect Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000013500 data storage Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C9/00—Measuring inclination, e.g. by clinometers, by levels
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Feedback Control In General (AREA)
Abstract
本发明公开了一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法，它包括如下步骤：对四轮车行驶情况建立连续动力学模型；考虑复杂环境下的混合扰动，建立系统状态方程，观测方程和输出方程，构建系统滤波器；给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益。本发明考虑了复杂环境的影响，针对四轮车动力学模型建立了系统状态方程和观测方程，进一步构建滤波器，在保障系统鲁棒性和抗干扰能力的前提下，对车辆侧倾角α和俯仰角β进行实时高精度的估计。估计结果可以满足实际应用的精度与实时性要求，并且所需的相关参数均可以通过低成本的传感器测得。
Description
技术领域
本发明属于汽车定位导航技术领域，具体涉及一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法，它是在最差非随机干扰下对车辆侧倾角与俯仰角进行估计，是为了给导航系统提供在最差非随机扰动下侧倾角与俯仰角的高精度估计。
背景技术
导航是以某种方法或技术引导航行体安全、准确、经济、便捷地按照一定路径或者轨迹行进到目标点，实现上述引导功能的硬件设备以及相应配套软件统称为导航系统。为了弥补GPS信号失锁时无法定位的缺陷，GPS/INS组合导航系统在发挥各自特长的同时，能互相弥补不足，增强导航系统的可靠性并提高导航的精度，是导航技术发展的主要趋势。
由于四轮车在行驶过程中发生侧倾或俯仰运动时，车身的四个悬架吊点相对各轮轮心的垂直位置发生了变化，从而会产生侧倾角与俯仰角，这两个参数在GPS/INS组合导航系统的航位推算中是必不可少的。虽然在实际车辆行驶过程中侧倾角与俯仰角的数值很小，但通常情况下四轮车的加速度远小于重力加速度，导致计算横向和纵向加速度时会产生较大偏差，因此对车辆的侧倾角与俯仰角进行实时估计显得尤为重要。
在对侧倾角与俯仰角进行估计时，通常借助卡尔曼滤波算法来进行估算，此类方法处理随机干扰时具有数据储存量小，算法容易实现，且实施成本低等优点。然而在复杂道路环境下，一些外在非随机干扰(比如传感器故障，碰撞等极端情况)往往会影响卡尔曼滤波算法的估计效果。
发明内容
为解决上述问题，本发明的目的在于提供一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法，该方法在保证系统鲁棒性的前提下对侧倾角与俯仰角进行高精度的估计，能够满足四轮车在复杂环境下的应用需求，给导航系统提供在最差非随机扰动下侧倾角与俯仰角的高精度估计。
本发明为解决上述技术问题提供了如下解决方案：本发明设计了一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法。其工作原理如下：先对四轮车建立连续动力学模型，模拟实际行驶情况；再将复杂环境的影响考虑成由随机扰动和非随机扰动构成的混合扰动信号；进一步用混合滤波算法构造滤波器；同时对侧倾角与俯仰角进行实时高精度估计。
本发明的一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法,具体步骤包括：
1)对四轮车行驶情况建立连续动力学模型；
2)考虑复杂环境下的混合扰动，建立系统状态方程，包括观测方程和输出方程，并构建系统滤波器；
3)给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益。
进一步，步骤1)中，对四轮车行驶情况建立连续动力学模型的方法具体包括以下步骤：
1.1)不考虑地球自转速度的情况下，假定侧倾角速度和俯仰角速度为零，建立四轮车行驶过程中的连续动力学方程如式(1)所示：
其中，vx,vy分别表示纵向速度和横向速度，上标"·"表示微分，ax,ay分别表示纵向加速度和横向加速度，wz表示横摆角速度，g表示重力加速度，α,β分别表示侧倾角与俯仰角；
1.2)由式(1)得到侧倾角α和俯仰角β的表达式如式(2)所示：
实际车辆行驶过程中，相对于车辆的纵向速度vx和纵向加速度车辆的横向速度vy和横向加速度均很小能忽略不计，因此式(2)简化后如式(3)所示：
其中α,β分别表示侧倾角与俯仰角，纵向加速度ax，横向加速度αy，纵向速度vx以及横摆角速度wz由四轮车上安装的传感器测得，而纵向速度vx的微分通过测得的vx对时间求导来得到；
进一步，步骤2)中，考虑复杂环境下的混合扰动，建立系统状态方程和观测方程包括以下步骤：
2.1)离散化后的状态方程如式(4)所示：
x(k+1)＝Ax(k)+B0w0(k)+B1w(k) (4)
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，估计对象x表示侧倾角α和俯仰角β，即x＝[αβ]T，上标"T"表示矩阵的转置，A表示估计对象x 的状态转移矩阵，w0表示均值为零方差为1的高斯白噪声，B0表示高斯白噪声w0的输入矩阵，w表示非随机有界扰动信号，B1表示非随机有界扰动信号w的输入矩阵；
2.2)离散化后的观测方程如式(5)所示：
y(k)＝C2x(k)+Dw0(k) (5)
其中，k表示离散化时刻，y表示观测向量，x表示估计对象,C2表示估计对象x的观测矩阵，w0表示均值为零方差为1的高斯白噪声，D表示高斯白噪声 w0的观测矩阵；离散化后的输出方程如式(6)所示：
其中，k表示离散化时刻，z,z0表示系统输出，x表示估计对象，C1,C0表示估计对象x的输出矩阵；
2.3)基于上述系统设计一个滤波器，如式(7)所示：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，A表示估计对象 x的状态转移矩阵，表示估计对象x的估计值，L表示需要设计的滤波器增益， C2表示估计对象x的观测矩阵，y表示观测向量，分别表示系统输出z,z0的估计值，C1,C0表示估计对象x的输出矩阵；
该滤波器的作用是使得估计值接近系统输出z,z0，从而实现对估计对象 x,即侧倾角α和俯仰角β的实时高精度估计。
进一步，步骤3)中，给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益具体包括以下步骤：
3.1)通过式(4)(5)(6)(7)得到误差系统模型如式(8)所示：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，ex表示估计对象 x与对应估计值的差值,e表示系统输出z与对应估计值的差值，e0表示系统输出z0与对应估计值的差值，A表示估计对象x的状态转移矩阵，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，w0表示均值为零方差为1的高斯白噪声，B0为高斯白噪声w0的输入矩阵，D表示高斯白噪声w0的观测矩阵，w 表示非随机有界扰动信号，B1为非随机有界扰动信号w的输入矩阵，C1,C0表示估计对象x的输出矩阵；
3.2)基于误差系统，定义最差的非随机有界扰动信号w如式(9)所示：
w(k)＝Wex(k) (9)
将式(9)代入式(8)，进一步得到最差非随机扰动下的系统误差模型如式 (10)所示：
定义中间矩阵AL和AW分别如式(11)、式(12)所示：
AL＝A+LC2 (11)
AW＝A+B1W (12)
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，γ为预设的常数， ex表示估计对象x与对应估计值的差值,e表示系统输出z与对应估计值的差值，e0表示系统输出z0与对应估计值的差值，A表示估计对象x的状态转移矩阵，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，w0表示均值为零方差为1的高斯白噪声，B0表示高斯白噪声w0的输入矩阵，D表示高斯白噪声w0的观测矩阵，w表示非随机有界扰动信号，B1表示非随机有界扰动信号w 的输入矩阵，C1,C0表示估计对象x的输出矩阵，W，AL和AW均为中间矩阵；
3.3)当k＝0时,对中间矩阵P1和P2以及中间矩阵W和滤波器增益L赋初值，如式(13)所示：
3.4)基于误差系统，由H∞滤波算法得到最差的非随机有界扰动信号w如式(14)所示：
因此如式(15)所示：
其中，k表示当前离散化时刻,w表示非随机有界扰动信号，γ为预设的常数，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，B1表示非随机有界扰动信号w的输入矩阵，C1,C0表示估计对象x的输出矩阵，W,AL和P1均为中间矩阵，ex表示估计对象x与对应估计值的差值；
3.5)基于最差非随机扰动下的系统误差模型，通过H2滤波算法得到滤波器增益L的表达式如式(16)所示：
其中，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，B0表示高斯白噪声w0的输入矩阵，AW和P2均为中间矩阵，D表示高斯白噪声w0的观测矩阵；
3.6)当k＝1时，由式(11)(12)(15)(16)分别得到式(17)、式(18)、式(19)及式(20)所示：
AW(1)＝A+B1W(0) (17)
AL(1)＝A+L(0)C2 (18)
其中，A表示估计对象x的状态转移矩阵，B1表示非随机有界扰动信号w的输入矩阵，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，γ为预设的常数，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，B0表示高斯白噪声w0的输入矩阵，D表示高斯白噪声w0的观测矩阵，w表示非随机有界扰动信号，C1,C0表示估计对象x的输出矩阵，W,AL,AW,P1和P2均为中间矩阵；
3.7)中间矩阵P1满足如下Riccati黎卡提方程如式(21)所示：
因此得到中间矩阵P1(1)如式(22)所示：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，B1表示非随机有界扰动信号w的输入矩阵，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，γ为预设的常数，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T" 表示矩阵的转置，B0表示高斯白噪声w0的输入矩阵，D表示高斯白噪声w0的观测矩阵，w表示非随机有界扰动信，C1表示估计对象x的输出矩阵，AL和P1均为中间矩阵；
3.8)中间矩阵P2满足如下Riccati(黎卡提)方程如式(23)所示：
因此得到中间矩阵P2(1)如式(24)所示：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，C2表示估计对象 x的观测矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，B0表示高斯白噪声w0的输入矩阵，D表示高斯白噪声w0的观测矩阵，AW和P2均为中间矩阵；
3.9)重复步骤3.6)、步骤3.7)及步骤3.8)：
若k＝T时刻时，矩阵P1(T)和矩阵P1(T-1)差值的二范数小于给定误差，分别得到如式(25)、式(26)所示：
P1＝P1(T)＝P1(T-1) (25)
AL＝AL(T)＝AL(T-1) (26)
同样，若k＝T时刻时，矩阵P2(T)和矩阵P2(T-1)差值的二范数小于给定误差，得到分别得到如式(27)、式(28)所示：
P2＝P2(T)＝P2(T-1) (27)
AW＝AW(T)＝AW(T-1) (28)
其中，AL,AW,P1和P2均为中间矩阵；
3.10)将中间矩阵P2和中间矩阵AW代入式(16)得到滤波器增益矩阵L。从而由滤波器(7)，实现对估计对象x的实时高精度估计，估计对象x为侧倾角α和俯仰角β。
本发明设计的一种基于混合滤波的车辆侧倾角与俯仰角估计方法,通过迭代算法求解两个线性Riccati(黎卡提)方程，构造滤波器来实现在最差非随机扰动信号下对车辆侧倾角α和俯仰角β进行实时高精度的估计，通过采用上述技术，与现有技术相比，本发明的有益效果如下：它考虑了复杂环境的影响，针对四轮车动力学模型建立了系统状态方程和观测方程，进一步构建滤波器，在保障系统鲁棒性和抗干扰能力的前提下，对车辆侧倾角α和俯仰角β进行实时高精度的估计。估计结果可以满足实际应用的精度与实时性要求，并且所需的相关参数均可以通过低成本的传感器测得。
附图说明
图1是本发明方法的实时仿真效果图；
图2是本发明方法的实时估计误差图。
具体实施方式
为使本发明的目的、技术方案和优点更加清晰，下面结合附图和仿真数据对本发明的技术方案作进一步描述。
本发明设计了一种基于混合滤波的车辆侧倾角与俯仰角估计方法。其工作原理如下：先对四轮车建立连续动力学模型，模拟实际行驶情况；再将复杂环境的影响考虑成由随机扰动和非随机扰动构成的混合扰动信号；进一步用混合滤波算法构造滤波器；同时对侧倾角与俯仰角进行实时高精度估计。
本发明的基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法,具体步骤包括：
1)对四轮车行驶情况建立连续动力学模型，具体包括以下步骤：
1.1)不考虑地球自转速度的情况下，假定侧倾角速度和俯仰角速度为零，建立四轮车行驶过程中的连续动力学方程：
其中，vx,vy分别表示纵向速度和横向速度，上标"·"表示微分，ax,ay分别表示纵向加速度和横向加速度，wz表示横摆角速度，g表示重力加速度，α,β分别表示侧倾角与俯仰角；
1.2)由式(1)得到侧倾角α和俯仰角β的表达式：
实际车辆行驶过程中，相对于车辆的纵向速度vx和纵向加速度车辆的横向速度vy和横向加速度很小可以忽略不计，因此可以简化式(2)为：
其中α,β分别表示侧倾角与俯仰角，纵向加速度ax，横向加速度αy，纵向速度vx以及横摆角速度wz由四轮车上安装的传感器测得，而纵向速度vx的微分通过测得的vx对时间求导来得到；
2)考虑复杂环境下的混合扰动，建立系统状态方程，观测方程和输出方程，构建系统滤波器考虑复杂环境下的混合扰动，建立系统状态方程和观测方程包括以下步骤：
2.1)离散化后的状态方程为：
x(k+1)＝Ax(k)+B0w0(k)+B1w(k) (4)
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，估计对象x表示侧倾角α和俯仰角β，即x＝[αβ]T，上标"T"表示矩阵的转置，由于车辆行驶过程中侧倾角与俯仰角都是连续缓慢变化的，可以认为当前采样时刻的侧倾角与俯仰角近似于下一采样时刻的侧倾角与俯仰角，因此取估计对象x的状态转移矩阵为w0表示均值为零方差为1的高斯白噪声，高斯白噪声w0的输入矩阵为非随机有界扰动信号由w＝|0.1*sin(0.5*k)|模拟，非随机有界扰动信号w的输入矩阵为
2.2)离散化后的观测方程为：
y(k)＝C2x(k)+Dw0(k) (5)
其中，k表示离散化时刻，y表示观测向量，x表示估计对象,w0表示均值为零方差为1的高斯白噪声，估计对象x的观测矩阵为C2＝[0.5-0.65]，高斯白噪声w0的观测矩阵为D＝[1.2 1.6]，
离散化后的输出方程为：
其中，k表示离散化时刻，z,z0表示系统输出，x表示估计对象，估计对象 x的输出矩阵为C1＝[0.5 0.4]，C0＝[0.6 0.4]。
2.3)基于上述系统设计一个滤波器：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，表示估计对象x的估计值，估计对象x的状态转移矩阵为L表示需要设计的滤波器增益，估计对象x的观测矩阵为C2＝[0.5-0.65]，y表示观测向量，分别表示系统输出z,z0的估计值，估计对象x的输出矩阵为C1＝[0.5 0.4]， C0＝[0.6 0.4]；
该滤波器的作用是使得估计值接近系统输出z,z0，从而实现对估计对象 x,即侧倾角α和俯仰角β的实时高精度估计；
3)给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益，具体包括以下步骤：
3.1)通过式(4)(5)(6)(7)得到误差系统模型：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，ex表示估计对象 x与对应估计值的差值,e表示系统输出z与对应估计值的差值，e0表示系统输出z0与对应估计值的差值，估计对象x的状态转移矩阵为L表示需要设计的滤波器增益，w0表示均值为零方差为1的高斯白噪声，估计对象x 的观测矩阵为C2＝[0.5-0.65]，高斯白噪声w0的输入矩阵为高斯白噪声w0的观测矩阵为D＝[1.2 1.6]，w表示未知的非随机有界扰动信号，非随机有界扰动信号w的输入矩阵为估计对象x的输出矩阵为 C1＝[0.5 0.4]，C0＝[0.6 0.4]；
3.2)基于误差系统，定义最差的非随机有界扰动信号w：
w(k)＝Wex(k) (9)
将式(9)代入式(8)，进一步得到最差非随机扰动下的系统误差模型：
定义中间矩阵AL和AW：
AL＝A+LC2 (11)
AW＝A+B1W (12)
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，预设的常数为γ＝2.5，ex表示估计对象x与对应估计值的差值,e表示系统输出z与对应估计值的差值，e0表示系统输出z0与对应估计值的差值，估计对象x的状态转移矩阵为L表示需要设计的滤波器增益，估计对象x的观测矩阵为 C2＝[0.5-0.65]，w0表示均值为零方差为1的高斯白噪声，高斯白噪声w0的输入矩阵为高斯白噪声w0的观测矩阵为D＝[1.2 1.6]，w表示非随机有界扰动信号，非随机有界扰动信号w的输入矩阵为W，AL和AW均为中间矩阵，估计对象x的输出矩阵为C1＝[0.5 0.4]，C0＝[0.6 0.4]；
3.3)当k＝0时,对中间矩阵P1和P2以及中间矩阵W和滤波器增益L赋初值，即：
3.4)基于误差系统，由H∞滤波算法得到最差的非随机有界扰动信号w：
因此:
其中，k表示当前离散化时刻,w表示非随机有界扰动信号，预设的常数为γ＝2.5，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，非随机有界扰动信号w的输入矩阵为估计对象x的输出矩阵为 C1＝[0.5 0.4]，W,AL和P1均为中间矩阵，ex表示估计对象x与对应估计值的差值；
3.5)基于最差非随机扰动下的系统误差模型，通过H2滤波算法得到滤波器增益L的表达式：
其中，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，B0表示高斯白噪声w0的输入矩阵，AW和P2均为中间矩阵，D表示高斯白噪声w0的观测矩阵；
3.6)当k＝1时，由式(11)(12)(15)(16)得到：
AW(1)＝A+B1W(0) (17)
AL(1)＝A+L(0)C2 (18)
其中，
估计对象x的状态转移矩阵为非随机有界扰动信号w 的输入矩阵为L表示需要设计的滤波器增益，估计对象x的观测矩阵为C2＝[0.5-0.65]，预设的常数为γ＝2.5，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，高斯白噪声w0的输入矩阵为高斯白噪声w0的观测矩阵为D＝[1.2 1.6]，w表示非随机有界扰动信号，估计对象x的输出矩阵为C1＝[0.5 0.4]，W,AL,AW,P1和P2均为中间矩阵；
3.7)中间矩阵P1满足如下Riccati(黎卡提)方程：
因此得到中间矩阵P1(1)：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，非随机有界扰动信号w的输入矩阵为L表示需要设计的滤波器增益，估计对象x的观测矩阵为C2＝[0.5-0.65]，预设的常数为γ＝2.5，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，高斯白噪声w0的输入矩阵为高斯白噪声w0的观测矩阵为D＝[1.21.6]，w表示非随机有界扰动信，估计对象x的输出矩阵为C1＝[0.5 0.4]，AL和P1均为中间矩阵；
3.8)中间矩阵P2满足如下Riccati(黎卡提)方程：
因此得到中间矩阵P2(1)：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，估计对象x的观测矩阵为C2＝[0.5-0.65]， AW和P2均为中间矩阵，高斯白噪声w0的输入矩阵为高斯白噪声 w0的观测矩阵为D＝[1.2 1.6]；
3.9)重复步骤3.6)、步骤3.7)、步骤3.8)得到：
P1(k),P2(k),W(k),L(k)
当在k＝170时，矩阵P1(170)和矩阵P1(169)差值的二范数小于误差0.0001，得到：
同样，在k＝170时刻，矩阵P2(170)和矩阵P2(169)差值的二范数小于误差 0.0001，得到：
其中，AL,AW,P1和P2均为中间矩阵；
3.10)将式(27)(28)中间矩阵P2和中间矩阵AW代入式(16)得到滤波器增益矩阵L。从而由滤波器(7)，实现对估计对象x(即侧倾角α和俯仰角β) 的实时高精度估计。
本发明设计的一种基于混合滤波的车辆侧倾角与俯仰角估计方法,通过迭代算法求解两个线性Riccati(黎卡提)方程，构造滤波器来实现在最差非随机扰动信号下对车辆侧倾角α和俯仰角β进行实时高精度的估计。
本发明的优点是：考虑了复杂环境的影响，针对四轮车动力学模型建立了系统状态方程和观测方程，进一步构建滤波器，在保障系统鲁棒性和抗干扰能力的前提下，对车辆侧倾角α和俯仰角β进行实时高精度的估计。估计结果可以满足实际应用的精度与实时性要求，并且所需的相关参数均可以通过低成本的传感器测得。
以上结合附图详细说明和陈述了本发明的实施方式，但并不局限于上述方式。在本领域的技术人员所具备的知识范围内，只要以本发明的构思为基础，还可以做出多种变化和改进。
Claims (4)
1.一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法,其特征在于包括如下步骤：
1)对四轮车行驶情况建立连续动力学模型；
2)考虑复杂环境下的混合扰动，建立系统状态方程，包括观测方程和输出方程，并构建系统滤波器；
3)给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益。
2.根据权利要求1所述的一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法,其特征在于步骤1)中，对四轮车行驶情况建立连续动力学模型的方法具体包括以下步骤：
1.1)不考虑地球自转速度的情况下，假定侧倾角速度和俯仰角速度为零，建立四轮车行驶过程中的连续动力学方程如式(1)所示：
其中，vx,vy分别表示纵向速度和横向速度，上标"·"表示微分，ax,ay分别表示纵向加速度和横向加速度，wz表示横摆角速度，g表示重力加速度，α,β分别表示侧倾角与俯仰角；
1.2)由式(1)得到侧倾角α和俯仰角β的表达式如式(2)所示：
实际车辆行驶过程中，相对于车辆的纵向速度vx和纵向加速度车辆的横向速度vy和横向加速度均很小能忽略不计，因此式(2)简化后如式(3)所示：
其中α,β分别表示侧倾角与俯仰角，纵向加速度ax，横向加速度αy，纵向速度vx以及横摆角速度wz由四轮车上安装的传感器测得，而纵向速度vx的微分通过测得的vx对时间求导来得到。
3.根据权利要求1所述的一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法,其特征在于步骤2)中考虑复杂环境下的混合扰动，建立系统状态方程和观测方程包括以下步骤：
2.1)离散化后的状态方程如式(4)所示：
x(k+1)＝Ax(k)+B0w0(k)+B1w(k) (4)
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，估计对象x表示侧倾角α和俯仰角β，即x＝[α β]T，上标"T"表示矩阵的转置，A表示估计对象x的状态转移矩阵，w0表示均值为零方差为1的高斯白噪声，B0表示高斯白噪声w0的输入矩阵，w表示非随机有界扰动信号，B1表示非随机有界扰动信号w的输入矩阵；
2.2)离散化后的观测方程如式(5)所示：
y(k)＝C2x(k)+Dw0(k) (5)
其中，k表示离散化时刻，y表示观测向量，x表示估计对象,C2表示估计对象x的观测矩阵，w0表示均值为零方差为1的高斯白噪声，D表示高斯白噪声w0的观测矩阵；
离散化后的输出方程如式(6)所示：
其中，k表示离散化时刻，z,z0表示系统输出，x表示估计对象，C1,C0表示估计对象x的输出矩阵；
2.3)基于上述系统设计一个滤波器，如式(7)所示：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，A表示估计对象x的状态转移矩阵，表示估计对象x的估计值，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，y表示观测向量，分别表示系统输出z,z0的估计值，C1,C0表示估计对象x的输出矩阵；
该滤波器的作用是使得估计值接近系统输出z,z0，从而实现对估计对象x,即侧倾角α和俯仰角β的实时高精度估计。
4.根据权利要求1所述的一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法,其特征在于步骤3)中，给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益具体包括以下步骤：
3.1)通过式(4)(5)(6)(7)得到误差系统模型如式(8)所示：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，ex表示估计对象x与对应估计值的差值,
e表示系统输出z与对应估计值的差值，e0表示系统输出z0与对应估计值的差值，A表示估计对象x的状态转移矩阵，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，w0表示均值为零方差为1的高斯白噪声，B0为高斯白噪声w0的输入矩阵，D表示高斯白噪声w0的观测矩阵，w表示非随机有界扰动信号，B1为非随机有界扰动信号w的输入矩阵，C1,C0表示估计对象x的输出矩阵；
3.2)基于误差系统，定义最差的非随机有界扰动信号w如式(9)所示：
w(k)＝Wex(k) (9)
将式(9)代入式(8)，进一步得到最差非随机扰动下的系统误差模型如式(10)所示：
定义中间矩阵AL和AW分别如式(11)、式(12)所示：
AL＝A+LC2 (11)
AW＝A+B1W (12)
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，γ为预设的常数，ex表示估计对象x与对应估计值的差值,e表示系统输出z与对应估计值的差值，e0表示系统输出z0与对应估计值的差值，A表示估计对象x的状态转移矩阵，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，w0表示均值为零方差为1的高斯白噪声，B0表示高斯白噪声w0的输入矩阵，D表示高斯白噪声w0的观测矩阵，w表示非随机有界扰动信号，B1表示非随机有界扰动信号w的输入矩阵，C1,C0表示估计对象x的输出矩阵，W，AL和AW均为中间矩阵；
3.3)当k＝0时,对中间矩阵P1和P2以及中间矩阵W和滤波器增益L赋初值，如式(13)所示：
3.4)基于误差系统，由H∞滤波算法得到最差的非随机有界扰动信号w如式(14)所示：
因此如式(15)所示：
其中，k表示当前离散化时刻,w表示非随机有界扰动信号，γ为预设的常数，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，B1表示非随机有界扰动信号w的输入矩阵，C1,C0表示估计对象x的输出矩阵，W,AL和P1均为中间矩阵，ex表示估计对象x与对应估计值的差值；
3.5)基于最差非随机扰动下的系统误差模型，通过H2滤波算法得到滤波器增益L的表达式如式(16)所示：
其中，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，B0表示高斯白噪声w0的输入矩阵，AW和P2均为中间矩阵，D表示高斯白噪声w0的观测矩阵；
3.6)当k＝1时，由式(11)(12)(15)(16)分别得到式(17)、式(18)、式(19)及式(20)所示：
AW(1)＝A+B1W(0) (17)
AL(1)＝A+L(0)C2 (18)
其中，A表示估计对象x的状态转移矩阵，B1表示非随机有界扰动信号w的输入矩阵，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，γ为预设的常数，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，B0表示高斯白噪声w0的输入矩阵，D表示高斯白噪声w0的观测矩阵，w表示非随机有界扰动信号，C1,C0表示估计对象x的输出矩阵，W,AL,AW,P1和P2均为中间矩阵；
3.7)中间矩阵P1满足如下Riccati黎卡提方程如式(21)所示：
因此得到中间矩阵P1(1)如式(22)所示：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，B1表示非随机有界扰动信号w的输入矩阵，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，γ为预设的常数，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，B0表示高斯白噪声w0的输入矩阵，D表示高斯白噪声w0的观测矩阵，w表示非随机有界扰动信，C1表示估计对象x的输出矩阵，AL和P1均为中间矩阵；
3.8)中间矩阵P2满足如下Riccati(黎卡提)方程如式(23)所示：
因此得到中间矩阵P2(1)如式(24)所示：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，C2表示估计对象x的观测矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，B0表示高斯白噪声w0的输入矩阵，D表示高斯白噪声w0的观测矩阵，AW和P2均为中间矩阵；
3.9)重复步骤3.6)、步骤3.7)及步骤3.8)：
若k＝T时刻时，矩阵P1(T)和矩阵P1(T-1)差值的二范数小于给定误差，分别得到如式(25)、式(26)所示：
P1＝P1(T)＝P1(T-1) (25)
AL＝AL(T)＝AL(T-1) (26)
同样，若k＝T时刻时，矩阵P2(T)和矩阵P2(T-1)差值的二范数小于给定误差，得到分别得到如式(27)、式(28)所示：
P2＝P2(T)＝P2(T-1) (27)
AW＝AW(T)＝AW(T-1) (28)
其中，AL,AW,P1和P2均为中间矩阵；
3.10)将中间矩阵P2和中间矩阵AW代入式(16)得到滤波器增益矩阵L。从而由滤波器(7)，实现对估计对象x的实时高精度估计，估计对象x为侧倾角α和俯仰角β。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810060029.7A CN108413923B (zh) | 2018-01-22 | 2018-01-22 | 一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810060029.7A CN108413923B (zh) | 2018-01-22 | 2018-01-22 | 一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108413923A true CN108413923A (zh) | 2018-08-17 |
| CN108413923B CN108413923B (zh) | 2020-07-24 |
Family
ID=63126068
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810060029.7A Active CN108413923B (zh) | 2018-01-22 | 2018-01-22 | 一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108413923B (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109282820A (zh) * | 2018-11-26 | 2019-01-29 | 浙江工业大学 | 基于分布式混合滤波的室内定位方法 |
| CN109282819A (zh) * | 2018-11-26 | 2019-01-29 | 浙江工业大学 | 基于分布式混合滤波的超宽带定位方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6292750B1 (en) * | 1998-11-04 | 2001-09-18 | Ching-Fang Lin | Vehicle positioning method and system thereof |
| CN1361431A (zh) * | 2000-12-23 | 2002-07-31 | 林清芳 | 完全整合式导航定位方法和系统 |
| CN103063212A (zh) * | 2013-01-04 | 2013-04-24 | 哈尔滨工程大学 | 一种基于非线性映射自适应混合Kalman/H∞滤波器的组合导航方法 |
| CN103323005A (zh) * | 2013-03-06 | 2013-09-25 | 郭雷 | 一种sins/gps/偏振光组合导航系统多目标优化抗干扰滤波方法 |
| CN104061899A (zh) * | 2014-06-20 | 2014-09-24 | 东南大学 | 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法 |
-
2018
- 2018-01-22 CN CN201810060029.7A patent/CN108413923B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6292750B1 (en) * | 1998-11-04 | 2001-09-18 | Ching-Fang Lin | Vehicle positioning method and system thereof |
| CN1361431A (zh) * | 2000-12-23 | 2002-07-31 | 林清芳 | 完全整合式导航定位方法和系统 |
| CN103063212A (zh) * | 2013-01-04 | 2013-04-24 | 哈尔滨工程大学 | 一种基于非线性映射自适应混合Kalman/H∞滤波器的组合导航方法 |
| CN103323005A (zh) * | 2013-03-06 | 2013-09-25 | 郭雷 | 一种sins/gps/偏振光组合导航系统多目标优化抗干扰滤波方法 |
| CN104061899A (zh) * | 2014-06-20 | 2014-09-24 | 东南大学 | 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法 |
Non-Patent Citations (1)
| Title |
|---|
| 张勇刚 等: "基于非线性映射的自适应混合Kalman-H∞滤波器", 《系统工程与电子技术》 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109282820A (zh) * | 2018-11-26 | 2019-01-29 | 浙江工业大学 | 基于分布式混合滤波的室内定位方法 |
| CN109282819A (zh) * | 2018-11-26 | 2019-01-29 | 浙江工业大学 | 基于分布式混合滤波的超宽带定位方法 |
| CN109282819B (zh) * | 2018-11-26 | 2021-07-27 | 浙江工业大学 | 基于分布式混合滤波的超宽带定位方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108413923B (zh) | 2020-07-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8457832B2 (en) | Vehicle body speed estimating device | |
| JP5029442B2 (ja) | 車両姿勢角推定装置及びプログラム | |
| CN108715166B (zh) | 基于深度学习的车辆稳定性指标估计方法 | |
| CN110341714B (zh) | 一种同时估计车辆质心侧偏角和扰动的方法 | |
| CN111547059A (zh) | 一种分布式驱动电动汽车惯性参数估计方法 | |
| JP2007182209A (ja) | 車両物理量推定装置及びプロブラム | |
| CN108519090A (zh) | 一种基于优化的ukf算法的双通道组合定姿算法的实现方法 | |
| CN109033017B (zh) | 一种丢包环境下的车辆侧倾角与俯仰角估计方法 | |
| CN111688715B (zh) | 四轮驱动电动汽车基于融合技术的质心侧偏角观测方法 | |
| CN111796522B (zh) | 一种车辆状态估计方法 | |
| CN113799783B (zh) | 应用于车辆的道路横向坡度测量方法及系统 | |
| CN110530361A (zh) | 一种基于农业机械双天线gnss自动导航系统的转向角度估计器 | |
| CN108413923B (zh) | 一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法 | |
| CN115406446A (zh) | 基于神经网络与无迹卡尔曼滤波的多轴特种车状态估计方法 | |
| Low et al. | Maneuverability and path following control of wheeled mobile robot in the presence of wheel skidding and slipping | |
| CN111006884B (zh) | 基于傅立叶变换的车轮车轴侧偏角与侧偏刚度的测量方法 | |
| CN112270039A (zh) | 基于分布式异步融合的线控底盘车辆非线性状态估计方法 | |
| CN104819717A (zh) | 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法 | |
| CN112287289A (zh) | 一种面向云控智能底盘的车辆非线性状态融合估计方法 | |
| CN111361571B (zh) | 一种基于gpi的同时估计车辆质心侧偏角和扰动的方法 | |
| CN113063414A (zh) | 一种用于视觉惯性slam的车辆动力学预积分构建方法 | |
| CN117068184A (zh) | 车身侧偏角的确定方法、装置及设备 | |
| CN111523207A (zh) | 整车平台建模、车辆性能检测的方法、装置、设备和介质 | |
| CN114932909A (zh) | 一种基于互补滤波实现加速度校正的坡度估算方法 | |
| CN108627155B (zh) | 一种农业机械非直线作业向心加速度估算及倾角融合方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |