[发明专利]一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法

摘要

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

主权项

一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法，其特征在于：本发明针对陆地行驶四轮车辆，建立符合其行驶特征的系统运动模型，进一步通过卡尔曼滤波递推算法实现对车辆侧倾角与俯仰角的实时、准确估计，可以用于在复杂道路环境下估计较大角度的俯仰角和侧倾角，且仅需低成本车载传感器，成本较低，具体步骤包括：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}}{h}\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‑Mechanic System，微机电系统)传感器所测得的纵向加速度、横向加速度与横摆角速度，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的侧倾角与俯仰角。