CN108413923B - 一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法 - Google Patents

一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法 Download PDF

Info

Publication number
CN108413923B
CN108413923B CN201810060029.7A CN201810060029A CN108413923B CN 108413923 B CN108413923 B CN 108413923B CN 201810060029 A CN201810060029 A CN 201810060029A CN 108413923 B CN108413923 B CN 108413923B
Authority
CN
China
Prior art keywords
matrix
representing
denotes
equation
gaussian noise
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.)
Active
Application number
CN201810060029.7A
Other languages
English (en)
Other versions
CN108413923A (zh
Inventor
冯宇
聂煊赫
高佳斌
俞晓天
陈焯明
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN201810060029.7A priority Critical patent/CN108413923B/zh
Publication of CN108413923A publication Critical patent/CN108413923A/zh
Application granted granted Critical
Publication of CN108413923B publication Critical patent/CN108413923B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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)所示：
Figure BDA0001555038400000021
其中，vx,vy分别表示纵向速度和横向速度，上标"·"表示微分，ax,ay分别表示纵向加速度和横向加速度，wz表示横摆角速度，g表示重力加速度，α,β分别表示侧倾角与俯仰角；
1.2)由式(1)得到侧倾角α和俯仰角β的表达式如式(2)所示：
Figure BDA0001555038400000031
实际车辆行驶过程中，相对于车辆的纵向速度vx和纵向加速度
Figure BDA0001555038400000032
车辆的横向速度vy和横向加速度
Figure BDA0001555038400000033
均很小能忽略不计，因此式(2)简化后如式(3)所示：
Figure BDA0001555038400000034
其中α,β分别表示侧倾角与俯仰角，纵向加速度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)所示：
Figure BDA0001555038400000049
其中，k表示离散化时刻，z,z0表示系统输出，x表示估计对象，C1,C0表示估计对象x的输出矩阵；
2.3)基于上述系统设计一个滤波器，如式(7)所示：
Figure BDA0001555038400000041
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，A表示估计对象 x的状态转移矩阵，
Figure BDA0001555038400000042
表示估计对象x的估计值，L表示需要设计的滤波器增益， C2表示估计对象x的观测矩阵，y表示观测向量，
Figure BDA0001555038400000043
分别表示系统输出z,z0的估计值，C1,C0表示估计对象x的输出矩阵；
该滤波器的作用是使得估计值
Figure BDA0001555038400000044
接近系统输出z,z0，从而实现对估计对象 x,即侧倾角α和俯仰角β的实时高精度估计。
进一步，步骤3)中，给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益具体包括以下步骤：
3.1)通过式(4)(5)(6)(7)得到误差系统模型如式(8)所示：
Figure BDA0001555038400000045
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，ex表示估计对象 x与对应估计值
Figure BDA0001555038400000046
的差值,e表示系统输出z与对应估计值
Figure BDA0001555038400000047
的差值，e0表示系统输出z0与对应估计值
Figure BDA0001555038400000048
的差值，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)所示：
Figure BDA0001555038400000051
定义中间矩阵AL和AW分别如式(11)、式(12)所示：
AL＝A+LC2 (11)
AW＝A+B1W (12)
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，γ为预设的常数， ex表示估计对象x与对应估计值
Figure BDA0001555038400000052
的差值,e表示系统输出z与对应估计值
Figure BDA0001555038400000053
的差值，e0表示系统输出z0与对应估计值
Figure BDA0001555038400000054
的差值，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)所示：
Figure BDA0001555038400000055
3.4)基于误差系统，由H∞滤波算法得到最差的非随机有界扰动信号w如式(14)所示：
Figure BDA0001555038400000061
因此如式(15)所示：
Figure BDA0001555038400000062
其中，k表示当前离散化时刻,w表示非随机有界扰动信号，γ为预设的常数，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，B1表示非随机有界扰动信号w的输入矩阵，C1,C0表示估计对象x的输出矩阵，W,AL和P1均为中间矩阵，ex表示估计对象x与对应估计值
Figure BDA0001555038400000063
的差值；
3.5)基于最差非随机扰动下的系统误差模型，通过H2滤波算法得到滤波器增益L的表达式如式(16)所示：
Figure BDA0001555038400000064
其中，上标"-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)
Figure BDA0001555038400000065
Figure BDA0001555038400000066
其中，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)所示：
Figure BDA0001555038400000071
因此得到中间矩阵P1(1)如式(22)所示：
Figure BDA0001555038400000072
其中，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)所示：
Figure BDA0001555038400000073
因此得到中间矩阵P2(1)如式(24)所示：
Figure BDA0001555038400000074
其中，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)不考虑地球自转速度的情况下，假定侧倾角速度和俯仰角速度为零，建立四轮车行驶过程中的连续动力学方程：
Figure BDA0001555038400000091
其中，vx,vy分别表示纵向速度和横向速度，上标"·"表示微分，ax,ay分别表示纵向加速度和横向加速度，wz表示横摆角速度，g表示重力加速度，α,β分别表示侧倾角与俯仰角；
1.2)由式(1)得到侧倾角α和俯仰角β的表达式：
Figure BDA0001555038400000092
实际车辆行驶过程中，相对于车辆的纵向速度vx和纵向加速度
Figure BDA0001555038400000093
车辆的横向速度vy和横向加速度
Figure BDA0001555038400000094
很小可以忽略不计，因此可以简化式(2)为：
Figure BDA0001555038400000095
其中α,β分别表示侧倾角与俯仰角，纵向加速度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的状态转移矩阵为
Figure BDA0001555038400000101
w0表示均值为零方差为1的高斯白噪声，高斯白噪声w0的输入矩阵为
Figure BDA0001555038400000102
非随机有界扰动信号由w＝|0.1*sin(0.5*k)|模拟，非随机有界扰动信号w的输入矩阵为
Figure BDA0001555038400000103
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]，
离散化后的输出方程为：
Figure BDA0001555038400000104
其中，k表示离散化时刻，z,z0表示系统输出，x表示估计对象，估计对象 x的输出矩阵为C1＝[0.5 0.4]，C0＝[0.6 0.4]。
2.3)基于上述系统设计一个滤波器：
Figure BDA0001555038400000111
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，
Figure BDA0001555038400000112
表示估计对象x的估计值，估计对象x的状态转移矩阵为
Figure BDA0001555038400000113
L表示需要设计的滤波器增益，估计对象x的观测矩阵为C2＝[0.5-0.65]，y表示观测向量，
Figure BDA0001555038400000114
分别表示系统输出z,z0的估计值，估计对象x的输出矩阵为C1＝[0.5 0.4]， C0＝[0.6 0.4]；
该滤波器的作用是使得估计值
Figure BDA0001555038400000115
接近系统输出z,z0，从而实现对估计对象 x,即侧倾角α和俯仰角β的实时高精度估计；
3)给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益，具体包括以下步骤：
3.1)通过式(4)(5)(6)(7)得到误差系统模型：
Figure BDA0001555038400000116
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，ex表示估计对象 x与对应估计值
Figure BDA0001555038400000117
的差值,e表示系统输出z与对应估计值
Figure BDA0001555038400000118
的差值，e0表示系统输出z0与对应估计值
Figure BDA0001555038400000119
的差值，估计对象x的状态转移矩阵为
Figure BDA00015550384000001110
L表示需要设计的滤波器增益，w0表示均值为零方差为1的高斯白噪声，估计对象x 的观测矩阵为C2＝[0.5-0.65]，高斯白噪声w0的输入矩阵为
Figure BDA00015550384000001111
高斯白噪声w0的观测矩阵为D＝[1.2 1.6]，w表示未知的非随机有界扰动信号，非随机有界扰动信号w的输入矩阵为
Figure BDA0001555038400000121
估计对象x的输出矩阵为 C1＝[0.5 0.4]，C0＝[0.6 0.4]；
3.2)基于误差系统，定义最差的非随机有界扰动信号w：
w(k)＝Wex(k) (9)
将式(9)代入式(8)，进一步得到最差非随机扰动下的系统误差模型：
Figure BDA0001555038400000122
定义中间矩阵AL和AW：
AL＝A+LC2 (11)
AW＝A+B1W (12)
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，预设的常数为γ＝2.5，ex表示估计对象x与对应估计值
Figure BDA0001555038400000123
的差值,e表示系统输出z与对应估计值
Figure BDA0001555038400000124
的差值，e0表示系统输出z0与对应估计值
Figure BDA0001555038400000125
的差值，估计对象x的状态转移矩阵为
Figure BDA0001555038400000126
L表示需要设计的滤波器增益，估计对象x的观测矩阵为 C2＝[0.5-0.65]，w0表示均值为零方差为1的高斯白噪声，高斯白噪声w0的输入矩阵为
Figure BDA0001555038400000127
高斯白噪声w0的观测矩阵为D＝[1.2 1.6]，w表示非随机有界扰动信号，非随机有界扰动信号w的输入矩阵为
Figure BDA0001555038400000128
W，AL和AW均为中间矩阵，估计对象x的输出矩阵为C1＝[0.5 0.4]，C0＝[0.60.4]；
3.3)当k＝0时,对中间矩阵P1和P2以及中间矩阵W和滤波器增益L赋初值，即：
Figure BDA0001555038400000131
3.4)基于误差系统，由H∞滤波算法得到最差的非随机有界扰动信号w：
Figure BDA0001555038400000132
因此:
Figure BDA0001555038400000133
其中，k表示当前离散化时刻,w表示非随机有界扰动信号，预设的常数为γ＝2.5，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，非随机有界扰动信号w的输入矩阵为
Figure BDA0001555038400000134
估计对象x的输出矩阵为 C1＝[0.5 0.4]，W,AL和P1均为中间矩阵，ex表示估计对象x与对应估计值
Figure BDA0001555038400000135
的差值；
3.5)基于最差非随机扰动下的系统误差模型，通过H2滤波算法得到滤波器增益L的表达式：
Figure BDA0001555038400000136
其中，上标"-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)
Figure BDA0001555038400000137
Figure BDA0001555038400000138
其中，
估计对象x的状态转移矩阵为
Figure BDA0001555038400000141
非随机有界扰动信号w 的输入矩阵为
Figure BDA0001555038400000142
L表示需要设计的滤波器增益，估计对象x的观测矩阵为C2＝[0.5-0.65]，预设的常数为γ＝2.5，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，高斯白噪声w0的输入矩阵为
Figure BDA0001555038400000143
高斯白噪声w0的观测矩阵为D＝[1.21.6]，w表示非随机有界扰动信号，估计对象x的输出矩阵为C1＝[0.5 0.4]，W,AL,AW,P1和P2均为中间矩阵；
3.7)中间矩阵P1满足如下Riccati(黎卡提)方程：
Figure BDA0001555038400000144
因此得到中间矩阵P1(1)：
Figure BDA0001555038400000145
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，非随机有界扰动信号w的输入矩阵为
Figure BDA0001555038400000146
L表示需要设计的滤波器增益，估计对象x的观测矩阵为C2＝[0.5-0.65]，预设的常数为γ＝2.5，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，高斯白噪声w0的输入矩阵为
Figure BDA0001555038400000147
高斯白噪声w0的观测矩阵为D＝[1.2 1.6]，w表示非随机有界扰动信，估计对象x的输出矩阵为C1＝[0.5 0.4]，AL和P1均为中间矩阵；
3.8)中间矩阵P2满足如下Riccati(黎卡提)方程：
Figure BDA0001555038400000148
因此得到中间矩阵P2(1)：
Figure BDA0001555038400000151
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，估计对象x的观测矩阵为C2＝[0.5-0.65]， AW和P2均为中间矩阵，高斯白噪声w0的输入矩阵为
Figure BDA0001555038400000152
高斯白噪声 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，得到：
Figure BDA0001555038400000153
Figure BDA0001555038400000154
同样，在k＝170时刻，矩阵P2(170)和矩阵P2(169)差值的二范数小于误差 0.0001，得到：
Figure BDA0001555038400000155
Figure BDA0001555038400000156
其中，AL,AW,P1和P2均为中间矩阵；
3.10)将式(27)(28)中间矩阵P2和中间矩阵AW代入式(16)得到滤波器增益矩阵L。从而由滤波器(7)，实现对估计对象x(即侧倾角α和俯仰角β) 的实时高精度估计。
本发明设计的一种基于混合滤波的车辆侧倾角与俯仰角估计方法,通过迭代算法求解两个线性Riccati(黎卡提)方程，构造滤波器来实现在最差非随机扰动信号下对车辆侧倾角α和俯仰角β进行实时高精度的估计。
本发明的优点是：考虑了复杂环境的影响，针对四轮车动力学模型建立了系统状态方程和观测方程，进一步构建滤波器，在保障系统鲁棒性和抗干扰能力的前提下，对车辆侧倾角α和俯仰角β进行实时高精度的估计。估计结果可以满足实际应用的精度与实时性要求，并且所需的相关参数均可以通过低成本的传感器测得。
以上结合附图详细说明和陈述了本发明的实施方式，但并不局限于上述方式。在本领域的技术人员所具备的知识范围内，只要以本发明的构思为基础，还可以做出多种变化和改进。

Claims (1)

1.一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法,其特征在于包括如下步骤：
1)对四轮车行驶情况建立连续动力学模型,具体包括以下步骤：
1.1)不考虑地球自转速度的情况下，假定侧倾角速度和俯仰角速度为零，建立四轮车行驶过程中的连续动力学方程如式(1)所示：
Figure FDA0002467590590000011
其中，vx,vy分别表示纵向速度和横向速度，上标"·"表示微分，ax,ay分别表示纵向加速度和横向加速度，wz表示横摆角速度，g表示重力加速度，α,β分别表示侧倾角与俯仰角；
1.2)由式(1)得到侧倾角α和俯仰角β的表达式如式(2)所示：
Figure FDA0002467590590000012
实际车辆行驶过程中，相对于车辆的纵向速度vx和纵向加速度
Figure FDA0002467590590000013
车辆的横向速度vy和横向加速度
Figure FDA0002467590590000014
均很小能忽略不计，因此式(2)简化后如式(3)所示：
Figure FDA0002467590590000015
其中α,β分别表示侧倾角与俯仰角，纵向加速度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)所示：
Figure FDA0002467590590000021
其中，k表示离散化时刻，z,z0表示系统输出，x表示估计对象，C1,C0表示估计对象x的输出矩阵；
2.3)基于上述系统设计一个滤波器，如式(7)所示：
Figure FDA0002467590590000022
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，A表示估计对象x的状态转移矩阵，
Figure FDA0002467590590000023
表示估计对象x的估计值，L表示需要设计的滤波器增益，C2表示估计对象x的观测矩阵，y表示观测向量，
Figure FDA0002467590590000024
分别表示系统输出z,z0的估计值，C1,C0表示估计对象x的输出矩阵；
该滤波器的作用是使得估计值
Figure FDA0002467590590000026
接近系统输出z,z0，从而实现对估计对象x,即侧倾角α和俯仰角β的实时高精度估计；
3)给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益，具体包括以下步骤：
3.1)通过式(4)(5)(6)(7)得到误差系统模型如式(8)所示：
Figure FDA0002467590590000025
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，ex表示估计对象x与对应估计值
Figure FDA0002467590590000036
的差值,
e表示系统输出z与对应估计值
Figure FDA0002467590590000037
的差值，e0表示系统输出z0与对应估计值
Figure FDA0002467590590000038
的差值，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)所示：
Figure FDA0002467590590000031
定义中间矩阵AL和AW分别如式(11)、式(12)所示：
AL＝A+LC2 (11)
AW＝A+B1W (12)
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，γ为预设的常数，ex表示估计对象x与对应估计值
Figure FDA0002467590590000032
的差值,e表示系统输出z与对应估计值
Figure FDA0002467590590000033
的差值，e0表示系统输出z0与对应估计值
Figure FDA0002467590590000034
的差值，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)所示：
Figure FDA0002467590590000035
3.4)基于误差系统，由H∞滤波算法得到最差的非随机有界扰动信号w如式(14)所示：
Figure FDA0002467590590000041
因此如式(15)所示：
Figure FDA0002467590590000042
其中，k表示当前离散化时刻,w表示非随机有界扰动信号，γ为预设的常数，I表示单位矩阵，上标"-1"表示矩阵的逆，上标"T"表示矩阵的转置，B1表示非随机有界扰动信号w的输入矩阵，C1,C0表示估计对象x的输出矩阵，W,AL和P1均为中间矩阵，ex表示估计对象x与对应估计值
Figure FDA0002467590590000046
的差值；
3.5)基于最差非随机扰动下的系统误差模型，通过H2滤波算法得到滤波器增益L的表达式如式(16)所示：
Figure FDA0002467590590000043
其中，上标"-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)
Figure FDA0002467590590000044
Figure FDA0002467590590000045
其中，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)所示：
Figure FDA0002467590590000051
因此得到中间矩阵P1(1)如式(22)所示：
Figure FDA0002467590590000052
其中，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)所示：
Figure FDA0002467590590000053
因此得到中间矩阵P2(1)如式(24)所示：
Figure FDA0002467590590000054
其中，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为侧倾角α和俯仰角β。
CN201810060029.7A 2018-01-22 2018-01-22 一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法 Active CN108413923B (zh)

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 CN108413923A (zh) 2018-08-17
CN108413923B true 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)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109282819B (zh) * 2018-11-26 2021-07-27 浙江工业大学 基于分布式混合滤波的超宽带定位方法
CN109282820B (zh) * 2018-11-26 2021-05-11 浙江工业大学 基于分布式混合滤波的室内定位方法

Citations (5)

* Cited by examiner, † Cited by third party
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 东南大学 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
基于非线性映射的自适应混合Kalman-H∞滤波器;张勇刚 等;《系统工程与电子技术》;20130930;第35卷(第9期);第1948-1952页 *

Also Published As

Publication number Publication date
CN108413923A (zh) 2018-08-17

Similar Documents

Publication Publication Date Title
US8457832B2 (en) Vehicle body speed estimating device
CN106394561B (zh) 一种车辆的纵向车速的估计方法和装置
CN109606378B (zh) 面向非高斯噪声环境的车辆行驶状态估计方法
CN110341714B (zh) 一种同时估计车辆质心侧偏角和扰动的方法
CN105151047A (zh) 一种汽车质心侧偏角测量方法
CN111547059A (zh) 一种分布式驱动电动汽车惯性参数估计方法
CN103279675B (zh) 轮胎-路面附着系数与轮胎侧偏角的估计方法
Li et al. Vehicle sideslip angle estimation based on hybrid Kalman filter
CN109033017B (zh) 一种丢包环境下的车辆侧倾角与俯仰角估计方法
JP2009073466A (ja) 車両姿勢角推定装置及びプログラム
CN111796522B (zh) 一种车辆状态估计方法
CN111688715B (zh) 四轮驱动电动汽车基于融合技术的质心侧偏角观测方法
CN108413923B (zh) 一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法
CN115406446A (zh) 基于神经网络与无迹卡尔曼滤波的多轴特种车状态估计方法
CN111006884B (zh) 基于傅立叶变换的车轮车轴侧偏角与侧偏刚度的测量方法
CN112270039A (zh) 基于分布式异步融合的线控底盘车辆非线性状态估计方法
CN113682282A (zh) 一种车辆稳定性控制方法、系统、车辆和存储介质
CN104819717A (zh) 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN112287289A (zh) 一种面向云控智能底盘的车辆非线性状态融合估计方法
CN111361571B (zh) 一种基于gpi的同时估计车辆质心侧偏角和扰动的方法
CN113063414A (zh) 一种用于视觉惯性slam的车辆动力学预积分构建方法
CN111231976B (zh) 一种基于变步长的车辆状态估计方法
CN105571595A (zh) 一种基于鲁棒滤波的救援清障车姿态角估计方法
CN117068184A (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