一种基于无损卡尔曼滤波的救援清障车姿态角估计方法与流程

技术特征：

1.一种基于无损卡尔曼滤波的救援清障车姿态角估计方法，其特征在于采用较少的低成本MEMS加速度传感器采集救援清障车的状态信息，根据救援清障车的工作特点，基于车辆运动学模型对姿态角进行估计，并且舍弃了扩展卡尔曼滤波算法在解决非线性系统将其近似线性化的思路，而是通过无损变换使非线性系统适用于线性假设下的标准卡尔曼滤波体系，提高了滤波精度和效率，从而得到更为准确的姿态角估计值，为救援清障车提供姿态安全预警。具体步骤包括：

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

忽略地球旋转速度，车辆运动学方程为：

${\stackrel{\·}{v}}_x=a_x+w_z{v}_y-w_y{v}_z+g\sin \mathrm{\θ}$

${\stackrel{\·}{v}}_y=a_y+w_z{v}_x-w_x{v}_z-g\sin \mathrm{\φ}\cos \mathrm{\θ}---\left(1\right)$

${\stackrel{\·}{v}}_z=a_z+w_y{v}_x-w_x{v}_y-g\cos \mathrm{\φ}\cos \mathrm{\θ}$

式(1)中，w_x、w_y和w_z分别表示围绕车体坐标系纵轴、横轴以及垂向轴的角速度，v_x、v_y和v_z分别表示沿车体坐标系纵轴、横轴以及垂向轴的线速度，a_x、a_y和a_z分别表示沿车体坐标系纵轴、横轴以及垂向轴的加速度；θ、φ分别表示俯仰角、侧倾角；g表示重力加速度；上标“·”表示微分，如表示v_x的微分。

由于救援清障车在工作时，仅利用吊臂对事故车辆开展施救工作，车身部分相对于地面处于静止状态，即可以合理地认为v_x、v_y、v_z、w_x、w_y和w_z均为零，则救援清障车的运动学模型可合理地简化为：

a_x＝-g sinθ (2)

a_y＝g sinφcosθ

2)所需车载传感器安装

由式(2)可以看出，只需测得车辆纵向加速度与横向加速度，即可对救援清障车的姿态角进行估计。因此，两个低成本MEMS加速度传感器即可满足救援清障车姿态角的测量要求。两个低成本MEMS加速度传感器应安装于车身(即除吊臂外救援清障车的其他部分)质心位置附近，一个与车体坐标系纵轴平行，用于测量纵向加速度，记为a_x-m；另一个与车体坐标系横轴平行，用于测量成横向加速度，记为a_y-m。

3)建立无损卡尔曼滤波的状态方程和观测方程

无损卡尔曼滤波状态方程为：

x_k＝f(x_k-1)+W_k-1 (3)

式(3)中，k表示离散化时刻，x_k＝[θ φ]^T，其中x_k为系统状态向量，θ、φ是系统的待估参数；由于救援清障车在工作过程中，侧倾角与俯仰角是连续缓慢变化的，可认为当前采样时刻的侧倾角与俯仰角等于下一采样时刻的侧倾角与俯仰角，所以f(x_k-1)＝x_k-1；W_k-1表示对应k-1时刻零均值的系统高斯白噪声向量且W＝[w₁ w₂]^T，其中w₁、w₂分别表示两个系统高斯白噪声分量，W_k-1对应的系统高斯白噪声协方差阵$Q_{k-1}=$$\left[\begin{array}{cc}{\mathrm{\σ}}_{w_1}^2& 0\\ 0& {\mathrm{\σ}}_{w_2}^2\end{array}\right],$其中分别表示系统高斯白噪声w₁、w₂对应的方差；

无损卡尔曼滤波观测方程为：

y_k＝h(x_k)+V_k (4)

式(4)中，y_k＝[a_{x_m} a_{y_m}]^T；由式(2)可知h(x_k)＝[-g sinθ g sinφcosθ]^T；V_k表示与W_k互不相关的零均值测量高斯白噪声向量且V＝[V₁ V₂]^T，其中v₁、v₂分别表示两个测量高斯白噪声分量，V_k对应的测量高斯白噪声协方差阵$R_k=\left[\begin{array}{cc}{\mathrm{\σ}}_{v_1}^2& 0\\ 0& {\mathrm{\σ}}_{v_2}^2\end{array}\right],$其中分别表示测量高斯白噪声v₁、v₂对应的方差。

4)无损卡尔曼滤波算法：

根据式(3)和式(4)所描述的系统状态方程和观测方程，结合无损变换，建立无损卡尔曼滤波递推过程，如下述步骤所示：

步骤1：输入变量初始化

${\hat{x}}_0=E\[x_0\]---\left(5\right)$

$P_0=E\[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\]$

步骤2：计算Sigma采样点

采用对称采样策略，产生2n+1个均值为方差为P的Sigma点集，n为状态向量维数，本发明中n＝2；

${\mathrm{\χ}}_{k-1}^0={\hat{x}}_{k-1}$

${\mathrm{\χ}}_{k-1}^i={\hat{x}}_{k-1}+{\left(\sqrt{(n+\mathrm{\λ})P_{k-1}}\right)}_i,i=1,2---\left(6\right)$

${\mathrm{\χ}}_{k-1}^i={\hat{x}}_{k-1}-{\left(\sqrt{(n+\mathrm{\λ})P_{k-1}}\right)}_{i-n},i=3,4$

式(6)中，为5个Sigma点；λ＝α²(n+k)-n为调节Sigma点和的距离参数，调节它可提高逼近精度，其中α决定Sigma点围绕均值的散布，通常取一小正数(如：1e-4≤α≤1)，本文取0.01；k为比例因数，本发明取0；为加权协方差矩阵平方根的第i列；

步骤3：无损卡尔曼滤波的时间更新方程

${\mathrm{\χ}}_{k|k-1}^i=f\left({\mathrm{\χ}}_{k-1}^i\right)---\left(7\right)$

${\hat{x}}_k^{-}={\mathrm{\Σ}}_{i=0}^{2n}{W}_i^m{\mathrm{\χ}}_{k|k-1}^i---\left(8\right)$

$P_{x,k}^{-}={\mathrm{\Σ}}_{i=0}^{2n}{W}_i^c({\mathrm{\χ}}_{k|k-1}^i-{\hat{x}}_k^{-}){({\mathrm{\χ}}_{k|k-1}^i-{\hat{x}}_k^{-})}^T+Q_k---\left(9\right)$

${\mathrm{\γ}}_{k|k-1}^i=h\left({\mathrm{\χ}}_{k|k-1}^i\right)---\left(10\right)$

${\hat{y}}_k^{-}={\mathrm{\Σ}}_{i=0}^{2n}{W}_i^n{\mathrm{\γ}}_{k|k-1}^i---\left(11\right)$

相应的权值定义如下：

$W_i^m=\frac{\mathrm{\λ}}{n+\mathrm{\λ}},i=0$

$W_i^c=\frac{\mathrm{\λ}}{n+\mathrm{\λ}}+1-{\mathrm{\α}}^2+\mathrm{\β},i=0---\left(12\right)$

$W_i^m=W_i^c=\frac{1}{2(n+\mathrm{\λ})},i=1,...,4$

其中，β为比例因数，对于高斯分布，其最优值为2；为均值的权值，为方差的权值，

步骤4：无损卡尔曼滤波的测量更新方程

$P_{y,k}={\mathrm{\Σ}}_{i=0}^{2n}{W}_i^c({\mathrm{\γ}}_{k|k-1}^i-{\hat{y}}_k^{-}){({\mathrm{\γ}}_{k|k-1}^i-{\hat{y}}_k^{-})}^T+R_k---\left(13\right)$

$P_{xy,k}={\mathrm{\Σ}}_{i=0}^{2n}{W}_i^c({\mathrm{\χ}}_{k|k-1}^i-{\hat{x}}_k^{-}){({\mathrm{\γ}}_{k|k-1}^i-{\hat{y}}_k^{-})}^T---\left(14\right)$

K_k＝P_xy，k(P_y，k)^-1 (15)

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(y_k-{\hat{y}}_k^{-})---\left(16\right)$

$P_{x,k}=P_{x,k}^{-}-K_k{P}_{y,k}{\left(K_k\right)}^T---\left(17\right)$

经过上述步骤的递推计算，可以准确实时地估计出救援清障车在每个时刻的侧倾角与俯仰角。