CN104061899B - 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法 - Google Patents

Abstract

本发明公开了一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法。本方法针对陆地四轮车辆，建立符合其行驶特征的运动学模型，进一步通过卡尔曼滤波算法实现对车辆侧倾角与俯仰角的实时、准确估计。本发明方法对于较大角度的侧倾角与俯仰角估计依然适用，满足复杂工况下的应用需求，且仅需低成本车载传感器。所估计的侧倾角与俯仰角信息是车辆组合导航与定位的关键参数。

Description

一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法

技术领域

本发明涉及一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法，其目的在于为车辆组合导航与定位提供高精度、低成本、高实时性、适用范围广的车辆姿态角估计，属于汽车定位导航领域。

背景技术

智能交通***(IntelligentTransportationSystems,ITS)日益受到欧洲、日本、美国等发达国家的重视并成为其研究热点。发展智能车辆及车辆自主行驶***，通过提高车辆自身智能的方案是实现安全、高效的自主行驶的最佳选择。发展智能交通***的一个重要基础是实时获取车辆准确可靠的定位信息。为了弥补GPS在天线受遮挡时无法进行定位的不足，发展GPS/INS组合导航***受到了国内外学者的广泛关注。

由于道路纵向和横向坡度的存在以及车辆悬架的运动，导致陆地四轮车辆在行驶过程中会存在一定的侧倾角和俯仰角，其值一般较小，在很多情况下可以忽略不计。但是在GPS/INS组合导航***中，侧倾角与俯仰角是进行航位推算的关键参数，不能将其忽略。这是由于陆地四轮车辆在行驶过程中，车辆的加速度一般远小于重力加速度，这样就会导致较小的侧倾角和俯仰角在测量纵向和横向加速度时也会产生较大的误差，这些误差随时间累积会导致推算速度和位置信息时产生更大的误差。所以，对侧倾角和俯仰角等姿态角进行准确实时的测量或估计是进行高精度GPS/INS组合导航定位的基础。

目前，基于惯性测量单元(IMU，InertialMeasurementUnit)的捷联导航技术已经发展的很成熟。一个IMU一般包含了三个单轴的加速度计和三个单轴的陀螺，这也导致其价格比较昂贵，特别是三个陀螺仪的价格。专利“一种基于递归最小二乘的车辆侧倾角与俯仰角估计方法”(公开号：CN103625475A)提出了一种基于低成本MEMS(Micro-Electro-MechanicSystem，微机电***)传感器的车辆姿态角估算方法。该方法仅需两个MEMS加速度传感器，一个MEMS陀螺仪以及车速传感器即可完成车辆侧倾角与俯仰角的估计。但是该方法仅考虑路面横向坡度率与纵向坡度率小于20％的工况，此时俯仰角和侧倾角都较小，所以在估计过程中采用了约等于处理。但是在复杂道路环境下，路面的横向坡度率与纵向坡度率可能会达到30％。一些汽车试验场的特殊路段上，路面的横向坡度率与纵向坡度率甚至会达到55％。这些情况下，俯仰角和侧倾角较大，是不能进行约等于处理的，上述方法在实际应用过程中会受到限制。

为了解决上述技术在应用中的不足，本发明专利提出一种基于卡尔曼滤波的车辆俯仰角和侧倾角估算方法。卡尔曼滤波器是以最小均方差为准则的最优状态估计滤波器，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值进行递推计算，便可实现对实时信号的估计，具有数据存储量小、算法简便的特点。本发明方法可以用于在复杂工况下估计较大角度的俯仰角和侧倾角，适用范围更广，为车辆导航定位提供一种更可靠的姿态角估算方法。

发明内容

本发明的目的是克服现有技术的不足，提出一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法，该方法精度高、成本低、实时性好、适用范围广，能够满足车辆在复杂环境下的应用需求。

本发明采用的技术方案如下：一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法，其特征在于：本发明针对陆地行驶四轮车辆，建立符合其行驶特征的***运动模型，进一步通过卡尔曼滤波算法实现对车辆侧倾角与俯仰角的实时、准确估计，可以用于在复杂道路环境下估计较大角度的俯仰角和侧倾角，且仅需低成本车载传感器，成本较低。具体步骤包括：

1)建立汽车行驶过程的***运动模型。

忽略地球旋转速度，假设车辆的俯仰角速度、侧倾角速度与垂向速度均为零，则可建立车辆行驶过程的动力学方程：

$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y+g\sin \mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_x-g\sin \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(1\right)$

式(1)中，v_x,v_y分别表示车辆的纵向速度和横向速度，a_x,a_y分别表示车辆的纵向加速度和横向加速度，ω_z表示车辆的横摆角速度，上述变量的定义都是在车体坐标系中定义的，g表示重力加速度，φ,θ分别表示车辆的侧倾角与俯仰角，上标志“·”表示微分，如表示v_x的微分。

由(1)式可得

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_x-a_x-{\mathrm{\ω}}_z{v}_y}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x-{\stackrel{\·}{v}}_y}{g\cos \mathrm{\θ}}\right)\end{array}---\left(2\right)$

式(2)中，纵向车速的微分可通过纵向车速对时间求导获取。

考虑到车辆正常行驶过程中，v_y与的数值都较小，因而可以忽略，则式(2)可简化为：

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_x-a_x}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x}{g\cos \mathrm{\θ}}\right)\end{array}---\left(3\right)$

2)建立卡尔曼滤波的状态方程和观测方程。

离散化后的卡尔曼滤波状态方程的矩阵形式为：

式(4)中，k表示离散化时刻；***状态向量为X＝[x₁x₂]′且x₁＝θ，x₂＝φ，即X＝[θφ]′，本发明中上角标'表示对矩阵转置；W(k-1)表示零均值的***高斯白噪声向量且W＝[w₁w₂]′，其中w₁、w₂分别表示两个***高斯白噪声分量，W(k-1)对应的***噪声协方差阵Q(k-1)为： $Q(k-1)=\left[\begin{array}{cc}{\mathrm{\σ}}_{w_1}^2& 0\\ 0& {\mathrm{\σ}}_{w_2}^2\end{array}\right],$ 其中分别表示***高斯白噪声w₁、w₂对应的方差；状态转移矩阵这是由于车辆行驶过程中，侧倾角与俯仰角都是连续缓慢变化的，可以认为当前采样时刻的侧倾角与俯仰角等于下一采样时刻的侧倾角与俯仰角。

卡尔曼滤波观测方程离散化的矩阵形式为：

Z(k)＝H(k)·X(k)+V(k)(5)

式(5)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量。由于观测向量与状态向量都是指侧倾角与俯仰角，所以 $H\left(k\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ $Z\left(k\right)=\left[\begin{array}{c}{\mathrm{\θ}}_m\left(k\right)\\ {\mathrm{\φ}}_m\left(k\right)\end{array}\right],V=\left[\begin{array}{c}{n}_{{\mathrm{\θ}}_m}\\ n_{{\mathrm{\φ}}_m}\end{array}\right],$ 其中θ_m(k)和φ_m(k)分别为通过传感器测量值直接计算得出的俯仰角与侧倾角值，根据式(3)，有：

$\begin{array}{c}{\mathrm{\θ}}_m=\arcsin \left(\frac{{\stackrel{\·}{v}}_{x\_m}-a_{x\_m}}{g}\right)\\ {\mathrm{\φ}}_m=\arcsin \left(\frac{a_{y\_m}-{\mathrm{\ω}}_{z\_m}{v}_{x\_m}}{g\cos {\mathrm{\θ}}_m}\right)\end{array}---\left(6\right)$

式(6)中，a_{x_m}、a_{y_m}与ω_{z_m}分别表示利用低成本MEMS(Micro-Electro-MechanicSystem，微机电***)传感器所测得的纵向加速度、横向加速度与横摆角速度，v_{x_m}表示通过车速传感器所获取的车辆纵向速度，表示v_{x_m}的微分，由车速传感器所测得的纵向速度信号对时间求导来获取，即在每一离散时刻k，有：

${\stackrel{\·}{v}}_{x\_m}\left(k\right)=\frac{v_{x\_m}\left(k\right)-v_{x\_m}(k-1)}{\mathrm{dt}}---\left(7\right)$

式(7)中，dt表示采样时间间隔，本发明中，dt＝0.01(秒)；表示由式(6)计算获得的车辆俯仰角的观测噪声且是均值为0、方差为的高斯白噪声，表示通过式(6)计算获得的车辆侧倾角的观测噪声且是均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为 $R=\left[\begin{array}{cc}{{\mathrm{\σ}}_{{\mathrm{\θ}}_m}}^2& 0\\ 0& {{\mathrm{\σ}}_{{\mathrm{\φ}}_m}}^2\end{array}\right];$

3)所需车载传感器安装

由式(6)可知，只需测得车辆的纵向速度、纵向加速度、横向加速度与横摆角速度，即可获取卡尔曼滤波观测方程所需的观测量，从而利用卡尔曼滤波递推算法来估计车辆的俯仰角和侧倾角；因此，仅需要两个低成本MEMS加速度传感器，一个低成本MEMS陀螺仪以及车速传感器即可满足测量要求；

其中两个低成本MEMS加速度传感器安装于车辆质心位置附近，一个沿车体坐标系纵轴，用以测量纵向加速度，一个沿车体坐标系横轴，用以测量横向加速度。低成本MEMS陀螺仪也安装于车辆质心位置附近，沿车体坐标系垂向轴安装，用以测量横摆角速度。车速传感器用于测量纵向车速，霍尔传感器或光电码盘等传感器均可采用，在此不做限定，但要求车速测量精度误差小于0.05米/秒。在测得纵向车速信号后，按照式(7)求得其微分，用于计算观测量；

4)卡尔曼滤波递推：

对于式(4)和式(5)所描述的***状态方程和测量方程，运用卡尔曼滤波理论，建立下面的标准滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新：

时间更新：

状态一步预测方程

一步预测误差方差阵

测量更新：

滤波增益矩阵K(k)＝P(k,k-1)·H^T(k)·[H(k)P(k,k-1)H′(k)+R(k)]^-1

状态估计 $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\·\hat{X}(k,k-1)]]$

估计误差方差阵P(k)＝[I-K(k)·H(k)]·P(k,k-1)

经过上述递推计算后，即可实时估计出车辆在每个离散时刻k的侧倾角与俯仰角。

本发明的优点及显著效果：

(1)本发明提出的估计方法可以估算较大角度的车辆侧倾角和俯仰角，在保证估算精度的同时，满足复杂工况下的应用需求；

(2)本发明是对车辆动力学模型根据车辆行驶特点进行合理简化，从而建立卡尔曼滤波的***状态方程和观测方程，并利用卡尔曼滤波递推算法进行侧倾角和俯仰角的估计，估计结果可以满足实际应用的精度与实时性要求；

(3)本发明所使用的传感器均为低成本传感器，便于大规模推广。

具体实施方式

智能交通***(IntelligentTransportationSystems,ITS)日益受到欧洲、日本、美国等发达国家的重视并成为其研究热点。发展智能车辆及车辆自主行驶***，通过提高车辆自身智能的方案是实现安全、高效的自主行驶的最佳选择。发展智能交通***的一个重要基础是实时获取车辆准确可靠的定位信息。为了弥补GPS在天线受遮挡时无法进行定位的不足，发展GPS/INS组合导航***受到了国内外学者的广泛关注。

由于道路纵向和横向坡度的存在以及车辆悬架的运动，陆地四轮车辆在行驶过程中会存在一定的侧倾角和俯仰角，其值一般较小，在很多情况下可以忽略不计，但是在进行GPS/INS组合导航的航位推算时却不能将其忽略。这是由于陆地车辆在行驶过程中的加速度一般远小于重力加速度，以至于较小的侧倾角和俯仰角可能导致测量纵向和横向加速度时产生较大的误差，这些误差随时间累积会导致推算速度和位置信息时产生更大的误差。所以，对侧倾角和俯仰角进行准确的测量或估计是进行GPS/INS组合导航的重要保障，其准确性是影响影响车辆定位精度的一个重要因素。

通常用来确定侧倾角和俯仰角等姿态角信息的方法是使用完整的六维惯性测量单元IMU(InertialMeasurementUnit)，该IMU包括3个加速度计和3个角速度陀螺仪，利用IMU输出量和角度信息的微分之间的运动学关系，并忽略地球旋转速度，车辆动力学过程可建模为[此处可参考文献：H.EricTsenga,LiXu，DavorHrovat，Estimationoflandvehiclerollandpitchangles[J].VehicleSystemDynamics:InternationalJournalofVehicleMechanicsandMobility,2007,45(5):433-443.]：

$\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}={\mathrm{\ω}}_x+({\mathrm{\ω}}_y\sin \mathrm{\φ}+{\mathrm{\ω}}_z\cos \mathrm{\φ})\tan \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}={\mathrm{\ω}}_y\cos \mathrm{\φ}-{\mathrm{\ω}}_z\sin \mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=({\mathrm{\ω}}_y\sin \mathrm{\φ}+{\mathrm{\ω}}_z\cos \mathrm{\φ})/\cos \mathrm{\θ}\end{array}---\left(1\right)$

$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y-{\mathrm{\ω}}_y{v}_z+g\sin \mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_x+{\mathrm{\ω}}_x{v}_z-g\sin \mathrm{\φ}\cos \mathrm{\θ}\\ {\stackrel{\·}{v}}_z=a_z+{\mathrm{\ω}}_y{v}_x-{\mathrm{\ω}}_x{v}_y-g\cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(2\right)$

式中，ω_x,ω_y和ω_z分别表示围绕车体坐标系纵轴、横轴以及垂向轴的角速度，v_x,v_y和v_z分别表示沿车体坐标系纵轴、横轴以及垂向轴的线速度，a_x,a_y和a_z分别表示沿车体坐标系纵轴、横轴以及垂向轴的加速度；φ,θ,和ψ分别表示侧倾、俯仰和横摆三个欧拉角；g表示重力加速度；上标志“·”表示微分，如表示v_x的微分。

车辆的姿态角信息可以通过六维IMU的导航解算算法求得，这在大量车辆定位文献中都有涉及。然而，六维IMU价格昂贵，特别是三个陀螺仪的价格。虽然MEMS(Micro-Electro-MechanicSystem，微机电***)传感器的测量精度相比于传统传感器较低，但是由于其具有成本低的突出特点，其应用依然越来越广泛。本发明即研究如何使用尽量少的MEMS传感器并结合数据处理方法来准确估计车辆的侧倾角和俯仰角，以克服MEMS传感器精度低的缺点。同时，尽量少的传感器也可以进一步降低成本。所使用的数据处理方法也要保证对大角度的侧倾角和俯仰角同样适用，以满足复杂工况下的应用需求。

由式(1)，可以看出为了估计侧倾角和俯仰角，并不需要横摆角信息ψ。同时，由于车辆的侧倾角与俯仰角都是连续缓慢变化的，其相应的角速度值较小，且车辆垂向速度一般也较小，都可以忽略。因此，可以合理的认为ω_x≈0,ω_y≈0,ν_z≈0。则式(1)和(2)可以简化为：

$\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}={\mathrm{\ω}}_z\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=-{\mathrm{\ω}}_z\sin \mathrm{\φ}\end{array}---\left(3\right)$

$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_t+g\sin \mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_x-g\sin \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(4\right)$

根据式(3)可知，如果车辆的初始状态已知且车辆的横摆角速度可获取，则车辆的侧倾角和俯仰角可以通过积分方法求得。然而事实上，直接积分方法由于传感器误差和不可避免的数值运算误差，会导致较大的漂移，特别是在使用低成本的MEMS传感器时。因此，本发明并不采用直接积分方法，而是利用式(4)提出一种基于卡尔曼滤波算法的车辆侧倾角和俯仰角估计方法。卡尔曼滤波器是以最小均方差为准则的最优状态估计滤波器，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值进行递推计算，便可实现对实时信号的估计，具有数据存储量小、算法简便的特点。

由(4)式可得

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_x-a_x-{\mathrm{\ω}}_z{v}_y}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x-{\stackrel{\·}{v}}_y}{g\cos \mathrm{\θ}}\right)\end{array}---\left(5\right)$

式(5)中，纵向车速的微分可通过纵向车速对时间求导获取，考虑到车辆正常行驶中，v_y与数值较小，因而可以忽略，则式(5)可简化为：

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_x-a_x}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x}{g\cos \mathrm{\θ}}\right)\end{array}---\left(6\right)$

路面的纵向和横向坡度是影响车辆侧倾角与俯仰角的主要因素，纵向和横向坡度越大，侧倾角与俯仰角也越大。一般路面的纵向和横向坡度都较小，进行侧倾角与俯仰角求解时，可以进行约等于处理，即：

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_s-a_x}{g}\right)\≈\frac{{\stackrel{\·}{v}}_x-a_x}{g}\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x}{g\cos \mathrm{\θ}}\right)\≈\frac{a_y-{\mathrm{\ω}}_z{v}_x}{g\cos \mathrm{\θ}}\end{array}---\left(7\right)$

但是考虑到在复杂道路环境下，路面的横向坡度率与纵向坡度率可能会达到30％。在汽车试验场的一些特殊路段上，路面的横向坡度率与纵向坡度率甚至会达到55％。这些情况下，侧倾角与俯仰角都较大，在求解arcsin运算时，不能进行约等于处理。本专利就是考虑到复杂工况下应用需求，在没有进行约等于处理的前提下进行卡尔曼滤波推算。下面建立卡尔曼滤波的状态方程和观测方程。

离散化后的卡尔曼滤波状态方程的矩阵形式为：

式(8)中，k表示离散化时刻；***状态向量为X＝[x₁x₂]′且x₁＝θ，x₂＝φ，即X＝[θφ]′，本发明中上角标'表示对矩阵转置；W(k-1)表示零均值的***高斯白噪声向量且W＝[w₁w₂]′，其中w₁、w₂分别表示两个***高斯白噪声分量，W(k-1)对应的***噪声协方差阵Q(k-1)为 $Q(k-1)=\left[\begin{array}{cc}{\mathrm{\σ}}_{w_1}^2& 0\\ 0& {\mathrm{\σ}}_{w_2}^2\end{array}\right],$ 其中分别表示***高斯白噪声w₁、w₂对应的方差；状态转移矩阵为这是由于车辆行驶过程中，其侧倾角与俯仰角都是连续缓慢变化的，可以认为上一采样时刻的侧倾角与俯仰角等于下一采样时刻的侧倾角与俯仰角。

卡尔曼滤波观测方程离散化的矩阵形式为：

Z(k)＝H(k)·X(k)+V(k)(9)

式(9)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量。由于观测向量与状态向量都是指侧倾角与俯仰角，所以 $H\left(k\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ $Z\left(k\right)=\left[\begin{array}{c}{\mathrm{\θ}}_m\left(k\right)\\ {\mathrm{\φ}}_m\left(k\right)\end{array}\right],V=\left[\begin{array}{c}{n}_{{\mathrm{\θ}}_m}\\ n_{{\mathrm{\φ}}_m}\end{array}\right],$ 其中θ_m(k)和φ_m(k)分别为通过传感器测量值直接计算得出的俯仰角与侧倾角值，根据式(6)有：

$\begin{array}{c}{\mathrm{\θ}}_m=\arcsin \left(\frac{{\stackrel{\·}{v}}_{x\_m}-a_{x\_m}}{g}\right)\\ {\mathrm{\φ}}_m=\arcsin \left(\frac{a_{y\_m}-{\mathrm{\ω}}_{z\_m}{v}_{x\_m}}{g\cos {\mathrm{\θ}}_m}\right)\end{array}---\left(10\right)$

式(9)中，a_{x_m}、a_{y_m}与ω_{z_m}分别表示利用低成本MEMS传感器所测得的纵向加速度、横向加速度与横摆角速度，v_{x_m}表示通过车速传感器所获取的车辆纵向速度，表示v_{x_m}的微分，由车速传感器所测得的纵向速度信号对时间求导来获取，即在每一离散时刻k，有：

${\stackrel{\·}{v}}_{x\_m}\left(k\right)=\frac{v_{x\_m}\left(k\right)-v_{x\_m}(k-1)}{\mathrm{dt}}---\left(11\right)$

式(11)中，dt表示采样时间间隔，本发明中，dt＝0.01(秒)；表示由式(10)计算获得的车辆俯仰角的观测噪声且是均值为0、方差为的高斯白噪声，表示通过式(10)计算获得的车辆侧倾角的的观测噪声且是均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为 $R=\left[\begin{array}{cc}{{\mathrm{\σ}}_{{\mathrm{\θ}}_m}}^2& 0\\ 0& {{\mathrm{\σ}}_{{\mathrm{\φ}}_m}}^2\end{array}\right];$

由式(10)可知，只需测得车辆的纵向速度、纵向加速度、横向加速度与横摆角速度，即可获取卡尔曼滤波观测方程所需的观测量，从而利用卡尔曼滤波递推算法来估计车辆的俯仰角和侧倾角。因此，仅需要两个低成本MEMS加速度传感器，一个低成本MEMS陀螺仪以及车速传感器即可满足测量要求。

其中两个低成本MEMS加速度传感器安装于车辆质心位置附近，一个沿车体坐标系纵轴，用以测量纵向加速度，一个沿车体坐标系横轴，用以测量横向加速度，低成本MEMS陀螺仪也安装于车辆质心位置附近，沿车体坐标系垂向轴安装，用以测量横摆角速度，车速传感器用于测量纵向车速，霍尔传感器或光电码盘等传感器均可采用，在此不做限定，但要求车速测量精度误差小于0.05米/秒，在测得纵向车速信号后，根据式(11)对其进行求导即可获取其微分；

事实上，若车辆安装有电子稳定控制或横摆稳定控制***，则这些信息可以通过车辆的CAN(ControllerAreaNetwork，控制器局域网络)总线获取。

对于式(8)和式(9)所描述的***状态方程和测量方程，运用卡尔曼滤波理论，建立下面的标准滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新：

时间更新：

状态一步预测方程

一步预测误差方差阵

测量更新：

滤波增益矩阵K(k)＝P(k,k-1)·H^T(k)·[H(k)P(k,k-1)H′(k)+R(k)]^-1

状态估计 $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\·\hat{X}(k,k-1)]]$

估计误差方差阵P(k)＝[I-K(k)·H(k)]·P(k,k-1)

经过上述递推计算后，即可实时估计出车辆在每个离散时刻k的侧倾角与俯仰角。

在上述整个侧倾角与俯仰角估计的过程中，本专利利用式(6)而非式(7)进行侧倾角与俯仰角的估计，即没有采用侧倾角与俯仰角的约等于处理。这样一方面可以保证估算的精度，另一方面使本发明方法对于大角度的侧倾角与俯仰角估计同样适用，满足复杂工况下的应用需求。本发明方法为车辆组合导航与定位领域提供一种高精度、低成本、高实时性、适用范围广的车辆姿态角估计方法。

Claims (1)

1.一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法，其特征在于：针对陆地行驶四轮车辆，建立符合其行驶特征的***运动模型，进一步通过卡尔曼滤波递推算法实现对车辆侧倾角与俯仰角的实时、准确估计，可以用于在复杂道路环境下估计较大角度的俯仰角和侧倾角，且仅需低成本车载传感器，具体步骤包括：

1)建立汽车行驶过程的***运动模型：

忽略地球旋转速度，假设车辆的俯仰角速度、侧倾角速度与垂向速度均为零，则可建立车辆行驶过程的动力学方程：

$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y+gsin\mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_x-g\sin \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(1\right)$

式(1)中，v_x,v_y分别表示车辆的纵向速度和横向速度，a_x,a_y分别表示车辆的纵向加速度和横向加速度，ω_z表示车辆的横摆角速度，上述变量的定义都是在车体坐标系中定义的，g表示重力加速度，φ,θ分别表示车辆的侧倾角与俯仰角，上标志“·”表示微分，

由(1)式可得

$\begin{array}{c}\mathrm{\θ}=arcsin\left(\frac{{\stackrel{\·}{v}}_x-a_x-{\mathrm{\ω}}_z{v}_y}{g}\right)\\ \mathrm{\φ}=arcsin\left(\frac{a_y-{\mathrm{\ω}}_z{v}_x-{\stackrel{\·}{v}}_y}{g\cos \mathrm{\θ}}\right)\end{array}---\left(2\right)$

式(2)中，纵向车速的微分可通过纵向车速对时间求导获取，

考虑到车辆正常行驶过程中，v_y与的数值都较小，因而可以忽略，则式(2)可简化为：

$\begin{array}{c}\mathrm{\θ}=arcsin\left(\frac{{\stackrel{\·}{v}}_x-a_x}{g}\right)\\ \mathrm{\φ}=arcsin\left(\frac{a_y-{\mathrm{\ω}}_z{v}_x}{g\cos \mathrm{\θ}}\right)\end{array}---\left(3\right)$

2)建立卡尔曼滤波的状态方程和观测方程：

离散化后的卡尔曼滤波状态方程的矩阵形式为：

式(4)中，k表示离散化时刻；***状态向量为X＝[x₁x₂]′且x₁＝θ，x₂＝φ，即X＝[θφ]′，上角标'表示对矩阵转置；W(k-1)表示零均值的***高斯白噪声向量且W＝[w₁w₂]′，其中w₁、w₂分别表示两个***高斯白噪声分量，W(k-1)对应的***噪声协方差阵Q(k-1)为： $Q(k-1)=\left[\begin{array}{cc}{\mathrm{\σ}}_{w_1}^2& 0\\ 0& {\mathrm{\σ}}_{w_2}^2\end{array}\right],$ 其中分别表示***高斯白噪声w₁、w₂对应的方差；状态转移矩阵这是由于车辆行驶过程中，侧倾角与俯仰角都是连续缓慢变化的，可以认为当前采样时刻的侧倾角与俯仰角等于下一采样时刻的侧倾角与俯仰角，

卡尔曼滤波观测方程离散化的矩阵形式为：

Z(k)＝H(k)·X(k)+V(k)(5)

式(5)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量，由于观测向量与状态向量都是指侧倾角与俯仰角，所以 $H\left(k\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ $Z\left(k\right)=\left[\begin{array}{c}{\mathrm{\θ}}_m\left(k\right)\\ {\mathrm{\φ}}_m\left(k\right)\end{array}\right],V=\left[\begin{array}{c}{n}_{{\mathrm{\θ}}_m}\\ n_{{\mathrm{\φ}}_m}\end{array}\right],$ 其中θ_m(k)和φ_m(k)分别为通过传感器测量值直接计算得出的俯仰角与侧倾角值，根据式(3)，有：

$\begin{array}{c}{\mathrm{\θ}}_m=arcsin\left(\frac{{\stackrel{\·}{v}}_{x\_m}-a_{x\_m}}{g}\right)\\ {\mathrm{\φ}}_m=arcsin\left(\frac{a_{y\_m}-{\mathrm{\ω}}_{z\_m}{v}_{x\_m}}{g{\mathrm{cos\θ}}_m}\right)\end{array}---\left(6\right)$

式(6)中，a_{x_m}、a_{y_m}与ω_{z_m}分别表示利用低成本MEMS传感器所测得的纵向加速度、横向加速度与横摆角速度，v_{x_m}表示通过车速传感器所获取的车辆纵向速度，表示v_{x_m}的微分，由车速传感器所测得的纵向速度信号对时间求导来获取，即在每一离散时刻k，有：

${\stackrel{\·}{v}}_{x\_m}\left(k\right)=\frac{v_{x\_m}\left(k\right)-v_{x\_m}(k-1)}{dt}---\left(7\right)$

式(7)中，dt表示采样时间间隔，dt＝0.01(秒)；表示由式(6)计算获得的车辆俯仰角的观测噪声且是均值为0、方差为的高斯白噪声，表示通过式(6)计算获得的车辆侧倾角的观测噪声且是均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为 $R=\left[\begin{array}{cc}{{\mathrm{\σ}}_{{\mathrm{\θ}}_m}}^2& 0\\ 0& {{\mathrm{\σ}}_{{\mathrm{\φ}}_m}}^2\end{array}\right];$

3)所需车载传感器安装：

由式(6)可知，只需测得车辆的纵向速度、纵向加速度、横向加速度与横摆角速度，即可获取卡尔曼滤波观测方程所需的观测量，从而利用卡尔曼滤波递推算法来估计车辆的俯仰角和侧倾角；因此，仅需要两个低成本MEMS加速度传感器，一个低成本MEMS陀螺仪以及车速传感器即可满足测量要求；

其中两个低成本MEMS加速度传感器安装于车辆质心位置附近，一个沿车体坐标系纵轴，用以测量纵向加速度，一个沿车体坐标系横轴，用以测量横向加速度，低成本MEMS陀螺仪也安装于车辆质心位置附近，沿车体坐标系垂向轴安装，用以测量横摆角速度，车速传感器用于测量纵向车速，要求车速测量精度误差小于0.05米/秒，在测得纵向车速信号后，按照式(7)求得其微分，用于计算观测量；

4)卡尔曼滤波递推：

对于式(4)和式(5)所描述的***状态方程和测量方程，运用卡尔曼滤波理论，建立下面的标准滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新：

时间更新：

状态一步预测方程

一步预测误差方差阵

测量更新：

滤波增益矩阵K(k)＝P(k,k-1)·H^T(k)·[H(k)P(k,k-1)H′(k)+R(k)]^-1

状态估计 $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)\[Z\left(k\right)-H\left(k\right)\·\hat{X}(k,k-1)\]\]$

估计误差方差阵P(k)＝[I-K(k)·H(k)]·P(k,k-1)

经过上述递推计算后，即可实时估计出车辆在每个离散时刻k的侧倾角与俯仰角。