[发明专利]一种基于鲁棒滤波的救援清障车姿态角估计方法

权利要求书

1.一种基于鲁棒滤波的救援清障车姿态角估计方法，其特征在于：根据救援清障车工 作特点，对其进行运动学建模，然后通过鲁棒滤波算法实现对救援清障车俯仰角与侧倾角 等姿态角的实时、准确估计，该方法可以在无法确切知道干扰信号的统计特性的情况下，估 计救援清障车在较大坡度率工况下的俯仰角与侧倾角，且仅需两个低成本微电子机械系统 (MicroElectroMechanicalsystems，MEMS)车载加速度传感器，具有低成本、高精度的特 点；

具体步骤包括：

1)建立救援清障车的运动学模型

由于救援清障车在工作时的俯仰角速度、侧倾角速度、垂向速度均为零，且忽略地球旋 转速度，则可建立救援清障车的运动学方程：

v·x=ax+ωzvy+g sinθ---(1)]]>

v·y=ay+ωzvx+gsinφcosθ]]>

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

由于救援清障车在工作时，仅利用吊臂开展施救工作，车身相对于地面处于静止状态， 即可以合理地认为v_x、v_y、和ω_z均为零，则式(1)可以简化为：

a_x+gsinθ＝0(2)

a_y-gsinφcosθ＝0

由式(2)可得

θ=arcsin(-axg)---(3)]]>

φ=arcsin(aygcosθ)]]>

2)建立H_∞滤波的状态方程和观测方程

离散化后的H_∞滤波状态方程的矩阵形式为：

式(4)中，k表示离散化时刻；X为系统状态向量，且X_k＝[x₁x₂]^T，x₁＝θ_k，x₂＝φ_k，即X_k＝ [θ_kφ_k]^T，上角标^T表示对矩阵转置；W表示系统噪声向量，且W＝[w₁w₂]^T，其中w₁、w₂分别表 示两个系统噪声分量；状态转移矩阵为这是因为救援清障车在工作过程 中俯仰角与侧倾角是基本保持不变的，可以认为上一采样时刻的俯仰角与侧倾角等于下一 采样时刻的俯仰角与侧倾角；Γ_k-1为系统噪声输入矩阵，取Γ_k-1为

1001;]]>

离散化后的H_∞滤波观测方程的矩阵形式为：

Z_k＝H_k·X_k+V_k(5)

式(5)中，Z为观测向量，H为观测阵，V表示观测噪声向量，V＝[n_θn_φ]，其中n_θ是俯仰角 的观测噪声，n_φ是侧倾角的观测噪声；由于观测向量与状态向量都是指俯仰角与侧倾角，所 以Hk=1001;Zk=θmkφmk,]]>其中和分别为通过传感器测量值直接推算得出的俯 仰角与侧倾角值，根据式(3)、有：

θmk=arcsin(-ax_mkg)---(6)]]>

φmk=arcsin(ay_mkg cosθmk)]]>

式(6)中，分别表示利用低成本MEMS加速度传感器所测得的纵向加速度、 横向加速度；

3)H_∞滤波递推：

对于式(4)和式(5)所描述的系统状态方程和测量方程，建立次优H_∞滤波的递推过程， 对于一定的正数γ，次优H_∞滤波递推过程主要包括以下三个步骤：

状态线性组合的估计

Y^k-1=Lk-1X^k-1---(7)]]>

是Y_k-1的估计值，Y_k-1为系统的待估计的向量，是X_k-1的估计值，L_k-1为给定的状 态量线性组合矩阵，由于需要估计的量就是状态变量X_k-1，所以取L_k-1为1001;]]>

时间修正

状态一步预测：

估计误差方差阵：

其中Re,k-1=I00-γ2I+Hk-1Lk-1Pk-1[Hk-1TLk-1T],]]>P_k的初始值P₀可选为 (π/180)200(π/180)2;]]>

量测修正

滤波增益矩阵：Kk=PkHkT(I+HkPkHkT)-1---(10)]]>

状态估计：X^k=X^k,k-1+Kk(Zk-HkX^k,k-1)---(11)]]>经过上述递推 计算后，可实时估计出救援清障车在每个离散时刻k的俯仰角和侧倾角。