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

摘要

本发明公开了一种基于卡尔曼滤波的救援清障车姿态角估计方法。本方法通过建立救援清障车的动力学模型，根据其工作特点，简化侧倾角和俯仰角的计算公式，并利用卡尔曼滤波算法获得救援清障车侧倾角与俯仰角的估计值。本发明方法使用较少的传感器，且可以用于在复杂救援环境下估计较大角度的侧倾角与俯仰角，具有精度高、成本低、实时性好、适用范围广等显著优点。可用于救援清障车姿态监控及危险姿态预警。

主权项

一种基于卡尔曼滤波的救援清障车姿态角估计方法，其特征在于：本发明根据救援清障车工作特点，对其进行动力学建模，进一步通过卡尔曼滤波算法实现对救援清障车侧倾角与俯仰角等姿态角的实时、准确估计，该方法可以用于在复杂救援环境下估计较大角度的俯仰角和侧倾角，且仅需两个低成本MEMS车载加速度传感器，具体步骤包括：1)建立救援清障车的动力学模型由于救援清障车在工作时的俯仰角速度、侧倾角速度与垂向速度均为零，且忽略地球旋转速度，则可建立救援清障车的动力学方程：$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y+g\sin \mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_y-g\sin \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(1\right)$式(1)中，v_x,v_y分别表示车辆的纵向和横向速度，a_x,a_y分别表示车辆的纵向和横向加速度，ω_z表示车辆的横摆角速度，上述变量的定义都是在车体坐标系中定义的，g表示重力加速度，φ,θ分别表示车辆的侧倾角与俯仰角，上标志“·”表示微分，如表示对v_x的微分；由于救援清障车在工作时，仅利用吊臂开展施救工作，车身相对于地面处于静止状态，即可以合理地认为v_x、v_y、和ω_z均为零，则式(1)可以简化为：$\begin{array}{c}{a}_x+g\sin \mathrm{\θ}=0\\ a_y-g\sin \mathrm{\φ}\cos \mathrm{\θ}=0\end{array}---\left(2\right)$由式(2)可得$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{-a_x}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y}{g\cos \mathrm{\θ}}\right)\end{array}---\left(3\right)$2)所需车载传感器安装由式(3)可知，只需测得车辆纵向加速度与横向加速度，即可对救援清障车的俯仰角和侧倾角进行估计，因此，仅需要两个低成本MEMS加速度传感器即可满足测量要求，两个低成本MEMS加速度传感器安装于车身(即除吊臂外救援清障车辆的其他部分)质心位置附近，一个与车体坐标系纵轴平行，用以测量纵向加速度，另一个与车体坐标系横轴平行，用以测量成横向加速度；3)建立卡尔曼滤波的状态方程和观测方程：离散化后的卡尔曼滤波状态方程的矩阵形式为：式(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],$其中σ_w1²、σ_w2²分别表示系统高斯白噪声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{-a_{x\_m}}{g}\right)\\ {\mathrm{\φ}}_m=\arcsin \left(\frac{a_{y\_m}}{g\cos {\mathrm{\θ}}_m}\right)\end{array}---\left(6\right)$式(6)中，a_{x_m}、a_{y_m}分别表示利用低成本MEMS传感器所测得的纵向加速度、横向加速度；表示由式(6)计算获得的车辆俯仰角的观测噪声且是均值为0、方差为的高斯白噪声，表示通过式(6)计算获得的车辆侧倾角的的观测噪声且是均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为$R=\left[\begin{array}{cc}{{\mathrm{\σ}}_{{\mathrm{\θ}}_m}}^2& 0\\ 0& {{\mathrm{\σ}}_{{\mathrm{\φ}}_m}}^2\end{array}\right];$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的侧倾角与横摆角。