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

摘要

本发明公开了一种救援清障车姿态角估计方法。本方法首先根据救援清障车的工作特点，建立了救援清障车的运动学模型。在此模型基础上，利用鲁棒滤波算法获得救援清障车俯仰角与侧倾角的精确估计值。本发明方法可以在无法确切知道干扰信号的统计特性的情况下，估计救援清障车在较大坡度率工况下的俯仰角与侧倾角，具有精度高、成本低、实时性好、适用范围广等显著优点，可用于救援清障车姿态监控及危险姿态预警。

主权项

一种基于鲁棒滤波的救援清障车姿态角估计方法，其特征在于：根据救援清障车工作特点，对其进行运动学建模，然后通过鲁棒滤波算法实现对救援清障车俯仰角与侧倾角等姿态角的实时、准确估计，该方法可以在无法确切知道干扰信号的统计特性的情况下，估计救援清障车在较大坡度率工况下的俯仰角与侧倾角，且仅需两个低成本微电子机械系统(Micro Electro Mechanical systems，MEMS)车载加速度传感器，具有低成本、高精度的特点；具体步骤包括：1)建立救援清障车的运动学模型由于救援清障车在工作时的俯仰角速度、侧倾角速度、垂向速度均为零，且忽略地球旋转速度，则可建立救援清障车的运动学方程：${\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y+gsin\mathrm{\θ}---\left(1\right)$${\stackrel{\·}{v}}_y=a_y+{\mathrm{\ω}}_z{v}_x+gs\mathrm{in\φ}\cos \mathrm{\θ}$式(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+g sinθ＝0   (2)a_y‑g sinφcosθ＝0由式(2)可得$\mathrm{\θ}=\arcsin \left(\frac{-a_x}{g}\right)---\left(3\right)$$\mathrm{\φ}=\arcsin \left(\frac{a_y}{gcos\mathrm{\θ}}\right)$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为$\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$离散化后的H_∞滤波观测方程的矩阵形式为：Z_k＝H_k·X_k+V_k   (5)式(5)中，Z为观测向量，H为观测阵，V表示观测噪声向量，V＝[n_θ n_φ]，其中n_θ是俯仰角的观测噪声，n_φ是侧倾角的观测噪声；由于观测向量与状态向量都是指俯仰角与侧倾角，所以$H_k=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];Z_k=\left[\begin{array}{c}{\mathrm{\θ}}_{m_k}\\ {\mathrm{\φ}}_{m_k}\end{array}\right],$其中和分别为通过传感器测量值直接推算得出的俯仰角与侧倾角值，根据式(3)、有：${\mathrm{\θ}}_{m_k}=\arcsin \left(\frac{-a_{x\_m_k}}{g}\right)---\left(6\right)$${\mathrm{\φ}}_{m_k}=\arcsin \left(\frac{a_{y\_m_k}}{g{\mathrm{cos\θ}}_{m_k}}\right)$式(6)中，分别表示利用低成本MEMS加速度传感器所测得的纵向加速度、横向加速度；3)H_∞滤波递推：对于式(4)和式(5)所描述的系统状态方程和测量方程，建立次优H_∞滤波的递推过程，对于一定的正数γ，次优H_∞滤波递推过程主要包括以下三个步骤：状态线性组合的估计${\hat{Y}}_{k-1}=L_{k-1}{\hat{X}}_{k-1}---\left(7\right)$是Y_k‑1的估计值，Y_k‑1为系统的待估计的向量，是X_k‑1的估计值，L_k‑1为给定的状态量线性组合矩阵，由于需要估计的量就是状态变量X_k‑1，所以取L_k‑1为$\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$时间修正状态一步预测：估计误差方差阵：其中$R_{e,k-1}=\left[\begin{array}{cc}I& 0\\ 0& -{\mathrm{\γ}}^{2}I\end{array}\right]+\left[\begin{array}{c}{H}_{k-1}\\ L_{k-1}\end{array}\right]P_{k-1}\[\begin{array}{cc}{H}_{k-1}^T& L_{k-1}^T\end{array}\],$P_k的初始值P₀可选为$\left[\begin{array}{cc}{(\mathrm{\π}/180)}^2& 0\\ 0& {(\mathrm{\π}/180)}^2\end{array}\right];$量测修正滤波增益矩阵：$K_k=P_k{H}_k^T{(I+H_k{P}_k{H}_k^T)}^{-1}---\left(10\right)$状态估计：${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})---\left(11\right)$经过上述递推计算后，可实时估计出救援清障车在每个离散时刻k的俯仰角和侧倾角。