本发明属于汽车定位导航技术领域，具体涉及一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法，它是在最差非随机干扰下对车辆侧倾角与俯仰角进行估计，是为了给导航系统提供在最差非随机扰动下侧倾角与俯仰角的高精度估计。
背景技术：
导航是以某种方法或技术引导航行体安全、准确、经济、便捷地按照一定路径或者轨迹行进到目标点，实现上述引导功能的硬件设备以及相应配套软件统称为导航系统。为了弥补gps信号失锁时无法定位的缺陷，gps/ins组合导航系统在发挥各自特长的同时，能互相弥补不足，增强导航系统的可靠性并提高导航的精度，是导航技术发展的主要趋势。
由于四轮车在行驶过程中发生侧倾或俯仰运动时，车身的四个悬架吊点相对各轮轮心的垂直位置发生了变化，从而会产生侧倾角与俯仰角，这两个参数在gps/ins组合导航系统的航位推算中是必不可少的。虽然在实际车辆行驶过程中侧倾角与俯仰角的数值很小，但通常情况下四轮车的加速度远小于重力加速度，导致计算横向和纵向加速度时会产生较大偏差，因此对车辆的侧倾角与俯仰角进行实时估计显得尤为重要。
在对侧倾角与俯仰角进行估计时，通常借助卡尔曼滤波算法来进行估算，此类方法处理随机干扰时具有数据储存量小，算法容易实现，且实施成本低等优点。然而在复杂道路环境下，一些外在非随机干扰(比如传感器故障，碰撞等极端情况)往往会影响卡尔曼滤波算法的估计效果。
技术实现要素：
为解决上述问题，本发明的目的在于提供一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法，该方法在保证系统鲁棒性的前提下对侧倾角与俯仰角进行高精度的估计，能够满足四轮车在复杂环境下的应用需求，给导航系统提供在最差非随机扰动下侧倾角与俯仰角的高精度估计。
本发明为解决上述技术问题提供了如下解决方案：本发明设计了一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法。其工作原理如下：先对四轮车建立连续动力学模型，模拟实际行驶情况；再将复杂环境的影响考虑成由随机扰动和非随机扰动构成的混合扰动信号；进一步用混合滤波算法构造滤波器；同时对侧倾角与俯仰角进行实时高精度估计。
本发明的一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法,具体步骤包括：
1)对四轮车行驶情况建立连续动力学模型；
2)考虑复杂环境下的混合扰动，建立系统状态方程，包括观测方程和输出方程，并构建系统滤波器；
3)给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益。
进一步，步骤1)中，对四轮车行驶情况建立连续动力学模型的方法具体包括以下步骤：
1.1)不考虑地球自转速度的情况下，假定侧倾角速度和俯仰角速度为零，建立四轮车行驶过程中的连续动力学方程如式(1)所示：
其中，vx,vy分别表示纵向速度和横向速度，上标"·"表示微分，ax,ay分别表示纵向加速度和横向加速度，wz表示横摆角速度，g表示重力加速度，α,β分别表示侧倾角与俯仰角；
1.2)由式(1)得到侧倾角α和俯仰角β的表达式如式(2)所示：
实际车辆行驶过程中，相对于车辆的纵向速度vx和纵向加速度
其中α,β分别表示侧倾角与俯仰角，纵向加速度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的状态转移矩阵，
该滤波器的作用是使得估计值
进一步，步骤3)中，给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益具体包括以下步骤：
3.1)通过式(4)(5)(6)(7)得到误差系统模型如式(8)所示：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，ex表示估计对象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与对应估计值
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和纵向加速度
其中α,β分别表示侧倾角与俯仰角，纵向加速度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的状态转移矩阵为
2.2)离散化后的观测方程为：
y(k)＝c2x(k)+dw0(k)(5)
其中，k表示离散化时刻，y表示观测向量，x表示估计对象,w0表示均值为零方差为1的高斯白噪声，估计对象x的观测矩阵为c2＝[0.5-0.65]，高斯白噪声w0的观测矩阵为d＝[1.21.6]，
离散化后的输出方程为：
其中，k表示离散化时刻，z,z0表示系统输出，x表示估计对象，估计对象x的输出矩阵为c1＝[0.50.4]，c0＝[0.60.4]。
2.3)基于上述系统设计一个滤波器：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，
该滤波器的作用是使得估计值
3)给出系统误差模型，进一步给出基于最差非随机扰动下的系统误差模型，设计并通过迭代算法求解滤波器增益，具体包括以下步骤：
3.1)通过式(4)(5)(6)(7)得到误差系统模型：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，ex表示估计对象x与对应估计值
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与对应估计值
3.3)当k＝0时,对中间矩阵p1和p2以及中间矩阵w和滤波器增益l赋初值，即：
3.4)基于误差系统，由h∞滤波算法得到最差的非随机有界扰动信号w：
因此:
其中，k表示当前离散化时刻,w表示非随机有界扰动信号，预设的常数为γ＝2.5，i表示单位矩阵，上标"-1"表示矩阵的逆，上标"t"表示矩阵的转置，非随机有界扰动信号w的输入矩阵为
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的状态转移矩阵为
3.7)中间矩阵p1满足如下riccati(黎卡提)方程：
因此得到中间矩阵p1(1)：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，非随机有界扰动信号w的输入矩阵为
3.8)中间矩阵p2满足如下riccati(黎卡提)方程：
因此得到中间矩阵p2(1)：
其中，k表示当前离散化时刻，k+1表示下一离散化时刻，上标"-1"表示矩阵的逆，上标"t"表示矩阵的转置，估计对象x的观测矩阵为c2＝[0.5-0.65]，aw和p2均为中间矩阵，高斯白噪声w0的输入矩阵为
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(黎卡提)方程，构造滤波器来实现在最差非随机扰动信号下对车辆侧倾角α和俯仰角β进行实时高精度的估计。
本发明的优点是：考虑了复杂环境的影响，针对四轮车动力学模型建立了系统状态方程和观测方程，进一步构建滤波器，在保障系统鲁棒性和抗干扰能力的前提下，对车辆侧倾角α和俯仰角β进行实时高精度的估计。估计结果可以满足实际应用的精度与实时性要求，并且所需的相关参数均可以通过低成本的传感器测得。
以上结合附图详细说明和陈述了本发明的实施方式，但并不局限于上述方式。在本领域的技术人员所具备的知识范围内，只要以本发明的构思为基础，还可以做出多种变化和改进。