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

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

【技术实现步骤摘要】

【技术保护点】
一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法，其特征在于：本专利技术针对陆地行驶四轮车辆，建立符合其行驶特征的系统运动模型，进一步通过卡尔曼滤波递推算法实现对车辆侧倾角与俯仰角的实时、准确估计，可以用于在复杂道路环境下估计较大角度的俯仰角和侧倾角，且仅需低成本车载传感器，成本较低，具体步骤包括：1)建立汽车行驶过程的系统运动模型，忽略地球旋转速度，假设车辆的俯仰角速度、侧倾角速度与垂向速度均为零，则可建立车辆行驶过程的动力学方程：v·x=ax+ωzvy+gsinθv·y=ay-ωzvx-gsinφcosθ---(1)]]>式(1)中，vx,vy分别表示车辆的纵向速度和横向速度，ax,ay分别表示车辆的纵向加速度和横向加速度，ωz表示车辆的横摆角速度，上述变量的定义都是在车体坐标系中定义的，g表示重力加速度，φ,θ分别表示车辆的侧倾角与俯仰角，上标志“·”表示微分，如表示vx的微分，由(1)式可得θ=arcsin(v·x-ax-ωzvyg)φ=arcsin(ay-ωzvx-v·ygcosθ)---(2)]]>式(2)中，纵向车速的微分可通过纵向车速对时间求导获取，考虑到车辆正常行驶过程中，vy与的数值都较小，因而可以忽略，则式(2)可简化为：θ=arcsin(v·x-axg)φ=arcsin(ay-ωzvxgcosθ)---(3)]]>2)建立卡尔曼滤波的状态方程和观测方程，离散化后的卡尔曼滤波状态方程的矩阵形式为：式(4)中，k表示离散化时刻；系统状态向量为X＝[x1 x2]′且x1＝θ，x2＝φ，即X＝[θ φ]′，本专利技术中上角标'表示对矩阵转置；W(k‑1)表示零均值的系统高斯白噪声向量且W＝[w1 w2]′，其中w1、w2分别表示两个系统高斯白噪声分量，W(k‑1)对应的系统噪声协方差阵Q(k‑1)为：Q(k-1)=σw1200σw22,]]>其中分别表示系统高斯白噪声w1、w2对应的方差；状态转移矩阵这是由于车辆行驶过程中，侧倾角与俯仰角都是连续缓慢变化的，可以认为当前采样时刻的侧倾角与俯仰角等于下一采样时刻的侧倾角与俯仰角，卡尔曼滤波观测方程离散化的矩阵形式为：Z(k)＝H(k)·X(k)+V(k)      (5)式(5)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量，由于观测向量与状态向量都是指侧倾角与俯仰角，所以H(k)=1001;]]>Z(k)=θm(k)φm(k),V=nθmnφm,]]>其中θm(k)和φm(k)分别为通过传感器测量值直接计算得出的俯仰角与侧倾角值，根据式(3)，有：θm=arcsin(v·x_m-ax_mh)φm=arcsin(ay_m-ωz_mvx_mgcosθm)---(6)]]>式(6)中，ax_m、ay_m与ωz_m分别表示利用低成本MEMS(Micro‑Electro‑Mechanic System，微机电系统)传感器所测得的纵向加速度、横向加速度与横摆角速度，vx_m表示通过车速传感器所获取的车辆纵向速度，表示vx_m的微分，由车速传感器所测得的纵向速度信号对时间求导来获取，即在每一离散时刻k，有：v·x_m(k)=vx_m(k)-vx-m(k-1)dt---(7)]]>式(7)中，dt表示采样时间间隔，本专利技术中，dt＝0.01(秒)；表示由式(6)计算获得的车辆俯仰角的观测噪声且是均值为0、方差为的高斯白噪声，表示通过式(6)计算获得的车辆侧倾角的观测噪声且是均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为R=σθm200σφm2;]]>3)所需车载传感器安装由式(6)可知，只需测得车辆的纵向速度、纵向加速度、横向加速度与横摆角速度，即可获取卡尔曼滤波观测方程所需的观测量，从而利用卡尔曼滤波递推算法来估计车辆的俯仰角和侧倾角；因此，仅需要两个低成本MEMS加速度传感器，一个低成本MEMS陀螺仪以及车速传感器即可满足测量要求；其中两个低成本MEMS加速度传感器安装于车辆质心位置附近，一个沿车体坐标系纵轴，用以测量纵向加速度，一个沿车体坐标系横轴，用以测量横向加速度，低成本MEMS陀螺仪也安装于车辆质心位置附近，沿车体坐标系垂向轴安装，用以测量横摆角速度，车速传感器用于测量纵向车速，霍尔传感器或光电码盘...

【技术特征摘要】

【专利技术属性】
技术研发人员：李旭，徐启敏，宋翔，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏;32