CN106156481A - 一种基于无损卡尔曼滤波的救援清障车姿态角估计方法 - Google Patents

Abstract

本发明公开了一种基于无损卡尔曼滤波的救援清障车姿态角估计方法，本方法根据救援清障车的工作特点，基于车辆运动学模型对姿态角进行估计，并且舍弃了扩展卡尔曼滤波算法在解决非线性系统将其近似线性化的思路，而是通过无损变换使非线性系统适用于线性假设下的标准卡尔曼滤波体系，提高了滤波精度和效率，从而得到更为准确的姿态角估计值，为救援清障车提供姿态安全预警。

Description

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

技术领域

本发明涉及一种基于无损卡尔曼滤波的救援清障车姿态角估计方法，其目的在于通过无损变换使救援清障车姿态角估计的非线性模型适用于线性假设下的标准卡尔曼滤波体系，从而获得准确的救援清障车姿态角估计值，可用于救援清障车姿态监控及危险姿态预警，属于救援清障车安全预警领域。

背景技术

随着我国经济的快速增长，道路交通基础设施条件逐渐改善，机动车保有量逐年增长，道路车辆事故救援、城市违章车辆处理以及自然灾害抢险救援等社会需求也随着快速增长。与此同时，作为汽车后市场的救援清障行业也逐渐被重视，得到了快速的发展。根据相关数据显示，救援清障车在作业过程中发生侧翻或倾翻的事故率也在逐年增加，往往存在着未能正常作业却自身发生事故的情况。究其原因，一方面存在救援清障车操作人员未按规定作业、处理事故经验不足等主观因素；另一方面车辆的姿态监控装置尚未完善，不能为操作人员提供准确的姿态信息以及安全预警也是不可回避的客观因素。

目前，已有专利文献对救援清障车姿态角进行研究：专利文献CN103616013 A公开了一种基于递归最小二乘的救援清障车姿态角估计方法，该方法填补了国内该领域的空白，但是仅适用于横向坡度和纵向坡度较小的路面(即可采用arcsinδ≈δ进行约等于处理的情况，其中δ为较小的坡度值)，应用工况受限；专利文献CN104061899 A公开了一种基于卡尔曼滤波的车辆姿态角估计方法，可应用于较为复杂的工况，但是该方法是在对测量数据计算后进行滤波，并未采用基于非线性滤波的估计方法，滤波精度和效率存在一定的不足。

为了解决上述技术在实际应用中的不足，本发明提供了一种基于无损卡尔曼滤波的救援清障车姿态角估计方法。该方法采用较少的低成本MEMS(Micro-Electro-Mechanical System)加速度传感器采集救援清障车的状态信息，根据救援清障车的工作特点，基于车辆运动学模型对车辆姿态角进行估计，并且舍弃了扩展卡尔曼滤波算法在解决非线性系统将其近似线性化的思路，而是通过无损变换使非线性系统适用于线性假设下的标准卡尔曼滤波体系，提高了滤波精度和效率，从而得到更为准确的姿态角估计值，为救援清障车提供姿态安全预警。

发明内容

本发明的目的是提供一种基于无损卡尔曼滤波的救援清障车姿态角估计方法，该方法使用较少的MEMS加速度传感器，基于车辆运动学模型对姿态角进行估计，可以为救援清障车提供精确可靠的姿态信息并提供有效的安全预警。

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

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

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

$\begin{array}{c}{\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{\θ}\\ {\stackrel{\·}{v}}_z=a_z+w_y{v}_x-w_x{v}_y-g\cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(1\right)$

式(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：输入变量初始化

$\begin{array}{c}{\hat{x}}_0=E\[x_0\]\\ P_0=E\[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\]\end{array}---\left(5\right)$

步骤2：计算Sigma采样点

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

$\begin{array}{c}{\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\\ {\mathrm{\χ}}_{k-1}^i={\hat{x}}_{k-1}-{\left(\sqrt{(n+\mathrm{\λ})P_{k-1}}\right)}_{i-n},i=3,4\end{array}---\left(6\right)$

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

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

$x_{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^m{\mathrm{\γ}}_{k|k-1}^i---\left(11\right)$

相应的权值定义如下：

$\begin{array}{c}{W}_i^m=\frac{\mathrm{\λ}}{n+\mathrm{\λ}},i=0\\ W_i^c=\frac{\mathrm{\λ}}{n+\mathrm{\λ}}+1-{\mathrm{\α}}^2+\mathrm{\β},i=0\\ W_i^m=W_i^c=\frac{1}{2(n+\mathrm{\λ})},i=1,...,4\end{array}---\left(12\right)$

其中，β为比例因数，对于高斯分布，其最优值为2；为均值的权值， 为方差的权值， ${\mathrm{\Σ}}_{i=0}^{2n}{W}_i^c=1;$

步骤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)$

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

具体实施方式

随着我国经济的快速增长，道路交通基础设施条件逐渐改善，机动车保有量逐年增长，道路车辆事故救援、城市违章车辆处理以及自然灾害抢险救援等社会需求也随着快速增长。与此同时，作为汽车后市场的救援清障行业也逐渐被重视，得到了快速的发展。根据相关数据显示，救援清障车在作业过程中发生侧翻或倾翻的事故率也在逐年增加，往往存在着未能正常作业却自身发生事故的情况。究其原因，一方面存在救援清障车操作人员未按规定作业、处理事故经验不足等主观因素；另一方面车辆的姿态监控装置尚未完善，不能为操作人员提供准确的姿态信息以及安全预警也是不可回避的客观因素。

目前，已有专利文献对救援清障车姿态角进行研究：专利文献CN103616013A公开了一种基于递归最小二乘的救援清障车姿态角估计方法，该方法填补了国内该领域的空白，但是仅适用于横向坡度和纵向坡度较小的路面(即可采用arcsinδ≈δ进行约等于处理的情况，其中δ为较小的坡度值)，应用工况受限；专利文献CN104061899A公开了一种基于卡尔曼滤波的车辆姿态角估计方法，可应用于较为复杂的工况，但是该方法是在对测量数据计算后进行滤波，并未采用基于非线性滤波的估计方法，滤波精度和效率存在一定的不足。

为了解决上述技术在实际应用中的不足，本发明提供了一种基于无损卡尔曼滤波的救援清障车姿态角估计方法。该方法采用较少的低成本MEMS(Micro-Electro-Mechanical System)加速度传感器采集救援清障车的状态信息，根据救援清障车的工作特点，基于车辆运动学模型对车辆姿态角进行估计，并且舍弃了扩展卡尔曼滤波算法在解决非线性系统将其近似线性化的思路，而是通过无损变换使非线性系统适用于线性假设下的标准卡尔曼滤波体系，提高了滤波精度和效率，从而得到更为准确的姿态角估计值，为救援清障车提供姿态安全预警。

对于普通车辆而言，通常用来确定姿态角信息的方法是使用完整的六维惯性测量单元IMU(Inertial Measurement Unit)，该IMU包括3个加速度计和3个角速度陀螺仪，利用其输出量和角度信息的微分之间的运动学关系，并忽略地球旋转速度，车辆运动学过程可建模为[此处可参考文献：H.Eric Tseng，Li Xu，Davor Hrovat，Estimation of landvehicle roll and pitch angles[J].Vehicle System Dynamics：InternationalJournal of Vehicle Mechanics and Mobility，2007，45(5)：433-443.]：

$\begin{array}{c}{\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{\θ}\\ {\stackrel{\·}{v}}_z=a_z+w_y{v}_x-w_x{v}_y-g\cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(1\right)$

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

由式(1)可得，车辆的姿态角信息可以通过六维IMU的导航解算算法得出，这一类方法在诸多相关研究文献中都有涉及。然而六维IMU价格较为昂贵，出于成本因素的考虑在实际应用中难以大规模推广。虽然MEMS加速度传感器的测量精度相对较低，但是由于其具有体积小、耐用性好、价格低廉、性能稳定等特点，自八十年代中后期发展极为迅速。本发明结合救援清障车的运动学模型及工作特点，研究如何利用较少的MEMS加速度传感器并通过无损卡尔曼滤波来进行救援清障车的姿态角估计，以达到较高的稳定性和精度。

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

a_x＝-g sinθ (2)

a_y＝g sinφcosθ

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

综合救援清障车的工作特点，根据公式(1)建立的运动学模型及其简化形式，基于无损卡尔曼滤波算法，无损卡尔曼滤波状态方程为：

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₂对应的方差。

无损卡尔曼滤波的具体算法步骤如下：

步骤1：输入变量初始化

$\begin{array}{c}{\hat{x}}_0=E\[x_0\]\\ P_0=E\[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\]\end{array}---\left(5\right)$

为确保算法收敛，初始误差方阵P₀设为二阶单位矩阵；

步骤2：计算Sigma采样点

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

$\begin{array}{c}{\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\\ {\mathrm{\χ}}_{k-1}^i={\hat{x}}_{k-1}-{\left(\sqrt{(n+\mathrm{\λ})P_{k-1}}\right)}_{i-n},i=3,4\end{array}---\left(6\right)$

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

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

由系统状态方程对各个Sigma点进行变换；

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

对变换后的Sigma点集进行加权处理，从而得到状态的一步预测值；

${\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)$

由系统观测方程对变换后的Sigma点集进行非线性变换；

${\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^m{\mathrm{\γ}}_{k|k-1}^i---\left(11\right)$

相应的权值定义如下：

$\begin{array}{c}{W}_i^m=\frac{\mathrm{\λ}}{n+\mathrm{\λ}},i=0\\ W_i^c=\frac{\mathrm{\λ}}{n+\mathrm{\λ}}+1-{\mathrm{\α}}^2+\mathrm{\β},i=0\\ W_i^m=W_i^c=\frac{1}{2(n+\mathrm{\λ})},i=1,...,4\end{array}---\left(12\right)$

其中，β为比例因数，对于高斯分布，其最优值为2；为均值的权值， 为方差的权值， ${\mathrm{\Σ}}_{i=0}^{2n}{W}_i^c=1;$

步骤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)$

其中，P_y，k为输出预测值协方差；P_xy，k为状态值与测量值的协方差；

滤波更新：

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)$

其中，K_k为无损卡尔曼滤波增益；为更新后的状态向量估计值；P_x，k为对应的后验方差；

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

Claims (1)

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)$

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