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

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

Info

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
indicate
matrix
formula
indicates
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
Application number
CN201810060029.7A
Other languages
English (en)
Other versions
CN108413923B (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

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为侧倾角α和俯仰角β。
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 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)

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

* 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∞滤波器", 《系统工程与电子技术》 *

Cited By (3)

* Cited by examiner, † Cited by third party
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
CN103759742B (zh) 基于模糊自适应控制技术的捷联惯导非线性对准方法
CN104787260B (zh) 一种基于融合滤波器的水翼双体船纵向姿态估计方法
CN108715166B (zh) 基于深度学习的车辆稳定性指标估计方法
CN105785999A (zh) 无人艇航向运动控制方法
CN110341714B (zh) 一种同时估计车辆质心侧偏角和扰动的方法
CN106979780A (zh) 一种无人车实时姿态测量方法
Li et al. Vehicle sideslip angle estimation based on hybrid Kalman filter
CN113353085B (zh) 一种基于卡尔曼滤波理论的路面不平度识别方法
CN109033017A (zh) 一种丢包环境下的车辆侧倾角与俯仰角估计方法
CN113447021B (zh) 基于lstm神经网络模型的mems惯性导航系统定位增强方法
Kang et al. A fast in‐motion alignment algorithm for DVL aided SINS
CN108592943A (zh) 一种基于opreq方法的惯性系粗对准计算方法
CN108413923A (zh) 一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法
CN112298354A (zh) 一种无人驾驶汽车转向系统方向盘与前轮转角的状态估计方法
CN102445176A (zh) 高速列车运行姿态参数测量系统
CN112270039A (zh) 基于分布式异步融合的线控底盘车辆非线性状态估计方法
CN104677359B (zh) 一种动态载体运行轨迹预测预报的方法
CN112214014A (zh) 农机自动驾驶控制方法及系统
CN108426584A (zh) 汽车多传感器的校准方法
CN112287289A (zh) 一种面向云控智能底盘的车辆非线性状态融合估计方法
CN111361571B (zh) 一种基于gpi的同时估计车辆质心侧偏角和扰动的方法
CN113063414A (zh) 一种用于视觉惯性slam的车辆动力学预积分构建方法
CN109506674A (zh) 一种加速度的校正方法及装置
CN110095118A (zh) 一种车身姿态角的实时测量方法及系统
CN110048693A (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