基于单轴MEMS惯性传感器的车身倾角测量方法与流程

本发明涉及一种智能农业机械领域，特别涉及一种基于单轴MEMS(Micro electro mechanical Systems，微电子机械系统)惯性传感器的车身倾角测量方法。

背景技术：

智能农业机械的车身倾角测量精度直接影响作业机具自动调平的工作性能。采用基于重力摆与阻尼油结构的电液式倾角传感器由于电液式传感器响应速度慢，易受振动影响，工作稳定性不高。目前最常用的为采用MEMS加速度计和陀螺仪互补融合，简单的切换方式或权重方式融合加速度计和陀螺仪数据能提高测量精度，静态测量精度较高，但由于陀螺仪长时间积分误差较大，加速度计在振动大的情况下测量不准，测量误差明显增加，难以满足农业机械车身倾角动态测量。商品化的高精度IMU(Inertial Measurement Unit，惯性测量单元)和AHRS(Attitude and Heading Reference System，姿态航向参考系统)可准确测量到农业机械动态倾角，但价格太高在农业机械成本中占比过高。采用高精度三轴陀螺仪和三轴加速度计，通过三轴卡尔曼滤波角度融合算法，可获得较好的测量精度，但设计成本仍较高，且在水田平地机较高运行速度下倾角测试数据精度降低。

为降低智能农业机械车身倾角测量方案设计成本，同时保证倾角的测试精度，选用低成本单轴MEMS惯性传感器(单轴陀螺仪和单轴加速度计)是一种较优方案，然而在农业机械作业过程中受外部加速度影响，其动态性能较差。因此，通过合适的算法以提高低成本车身倾角测量精度，有利于低成本农业机械车身倾角测量传感器的推广应用。

技术实现要素：

本发明的目的在于克服现有技术的缺点与不足，提供一种基于单轴MEMS惯性传感器的车身倾角测量方法，本发明车身倾角测量方法估算出农业机械作业过程中外部加速度，从而消除外部加速度影响，具有车倾角测量精度高以及适用于低成本的现代农业装备车身倾角测量的优点。

本发明的目的通过下述技术方案实现：一种基于单轴MEMS惯性传感器的车身倾角测量方法，所述单轴MEMS惯性传感器包括设置于车身的单轴加速度计和单轴陀螺仪，车身倾角测量方法具体如下：

S1、将单轴加速度计和单轴陀螺仪输出的数据分别进行低通滤波处理；

S2、计算外部加速度估计值：根据上一时刻卡尔曼滤波器输出的最优车身倾角以及上一时刻单轴加速度计的加速度输出值计算上一时刻外部加速度估计值；然后根据上一时刻外部加速度估计值，通过低通滤波器计算出当前时刻的外部加速度估计值；

S3、将当前时刻的外部加速度估计值作为外部加速度值，从当前时刻单轴加速度计的加速度输出值中去除外部加速度值，得到当前时刻单轴加速度计重力所产生的重力加速度；

S4、将当前时刻单轴加速度计重力所产生的重力加速度代入卡尔曼滤波器的测量方程，卡尔曼滤波器融合步骤S1中低通滤波处理后的数据，然后根据该测量方程计算出当前时刻的最优车身倾角。

优选的，所述步骤S1中采用一阶IIR低通数字滤波对单轴加速度计和单轴陀螺仪输出的数据分别进行数字滤波处理，其中数字滤波后输出为：

Y(k)＝(1-α)Y(k-1)+αX(k)；

其中X(k)为第k次低通滤波的输入值；Y(k-1)为第(k-1)次低通滤波结果输出值；Y(k)为第k次低通滤波结果输出值；α为低通滤波系数。

优选的，所述步骤S2中计算的上一时刻k-1时刻的外部加速度估计值为：

其中为卡尔曼滤波器上一时刻k-1时刻输出的最优车身倾角，g为重力加速度常量，a_k-1为上一时刻k-1时刻的单轴加速度计的加速度输出值。

更进一步的，卡尔曼滤波器输出的最优车身倾角的初始值为0，单轴加速度计的加速度输出值的初始值a₀为0。

更进一步的，所述步骤S2中根据上一时刻k-1时刻的外部加速度估计值，通过一阶低通滤波器计算出当前时刻k时刻的外部加速度估计值为：

$a_{e,k}^{-}=c_a{a}_{e,k-1}^{+};$

其中c_a为固定的滤波系数。

更进一步的，所述c_a在0～1之间。

更进一步的，所述步骤S3中从当前时刻k时刻单轴加速度计的加速度输出值中去除外部加速度值，获取到的当前时刻k时刻的单轴加速度计重力所产生的重力加速度a_g，k为：

$a_{g,k}=a_k-c_a{a}_{\mathrm{\ϵ},k-1}^{+}-n_k;$

其中a_k为当前时刻k时刻单轴加速度的加速度输出值；a_g,k为当前时刻k时刻单轴加速度计重力所产生的重力加速度；n_k为当前时刻k时刻测量噪声。

更进一步的，所述步骤S4中，将当前时刻k时刻单轴加速度计重力所产生的重力加速度a_g,k代入卡尔曼滤波器的测量方程中，得到当前时刻k时刻测量方程Z_k为：

其中v_k为当前时刻k时刻单轴加速度计测量的高斯白噪声。

更进一步的，所述步骤S4最优车身倾角获取过程具体如下：通过单轴加速度计估计单轴陀螺仪测量车身倾角的零位偏差，然后根据车身倾角的零位偏差，通过卡尔曼滤波器融合步骤S1中低通滤波处理后的数据，最后根据当前时刻k时刻测量方程Z_k计算出当前时刻k时刻的最优车身倾角其中

其中：

其中T为更新周期，Δb为单轴加速度计估计的单轴陀螺仪的零位偏差；ω_k为当前时刻k时刻单轴陀螺仪输出角速度；w_g,k为出当前时刻k时刻单轴陀螺仪测量的高斯白噪声；w_b,k为出当前时刻k时刻单轴加速度计估计单轴陀螺仪零位偏差的高斯白噪声；w_a，k为出当前时刻k时刻单轴加速度计测量的高斯白噪声；K_k为出当前时刻k时刻卡尔曼增益；

其中为当前时刻k时刻卡尔曼滤波器输出的最优车身倾角和当前时刻k时刻单轴加速度计估计的单轴陀螺仪的零位偏差构成的矩阵；

其中为上一时刻k-1时刻卡尔曼滤波器输出的最优车身倾角和上一时刻k-1时刻单轴加速度计估计的单轴陀螺仪的零位偏差构成的矩阵；

其中为上一时刻k-1时刻车身倾角的预测结果和上一时刻k-1时刻单轴加速度计估计的单轴陀螺仪的零位偏差的预测结果构成的矩阵。

本发明相对于现有技术具有如下的优点及效果：

本发明车身倾角测量方法首先根据上一时刻卡尔曼滤波器输出的最优车身倾角以及上一时刻单轴加速度计的加速度输出值计算上一时刻外部加速度估计值；然后根据上一时刻外部加速度估计值，通过一阶低通滤波器计算出当前时刻的外部加速度估计值；然后将当前时刻的外部加速度估计值作为外部加速度值，在前时刻单轴加速度计的加速度输出值中去除该外部加速度值得到单轴加速度计重力所产生的重力加速度。最后将上述重力加速度代入卡尔曼滤波器的测量方程中，根据测量方程计算出当前时刻的最优车身倾角。可见，本发明方法根据上一时刻卡尔曼滤波器输出的最优车身倾角估算出当前时刻的外部加速度，并且在测量方程中将该外部加速度去除，在外部加速度去除的情况下计算得到当前时刻的最优车身倾角，因此本发明方法有效减少了外部加速度的影响，提高了车身倾角测量的精确度；并且仅仅需要单轴MEMS惯性传感器测试的数据即可实现最优车身倾角的测量，不需要增加额外的其他传感器，适用于低成本的现代农业装备的车身倾角测量。

附图说明

图1是本发明车身倾角测量方法流程图。

图2是测试车在Y轴方向直线运动时，各种测试方法中外部加速度及车身倾角曲线图。

图3是图2曲线的局部放大图。

图4是测试车在X轴方向直线运动时，各种测试方法中外部加速度及车身倾角曲线图。

图5是测试车在Z轴方向直线运动时，各种测试方法中外部加速度及车身倾角曲线图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例

本实施例公开了一种基于单轴MEMS惯性传感器的车身倾角测量方法，单轴MEMS惯性传感器包括设置于车身的单轴加速度计和单轴陀螺仪，在本实施例中单轴MEMS惯性传感器中采用的加速度计为ADIS16300，采用的单轴陀螺仪为ADXL453，其中算法运行处理器为TI公司TMS320F28069。

如图1所示，本实施例的车身倾角测量方法具体如下：

S1、将单轴加速度计和单轴陀螺仪输出的数据分别进行低通滤波处理；

在本实施例中采用一阶IIR低通数字滤波对单轴加速度计和单轴陀螺仪输出的数据分别进行数字滤波处理，其中数字滤波后输出为：

Y(k)＝(1-α)Y(k-1)+αX(k)；

其中X(k)为第k次低通滤波的输入值；Y(k-1)为第(k-1)次低通滤波结果输出值；Y(k)为第k次低通滤波结果输出值；α为低通滤波系数。

S2、计算外部加速度估计值：根据上一时刻卡尔曼滤波器输出的最优车身倾角以及上一时刻单轴加速度计的加速度输出值计算上一时刻外部加速度估计值；然后根据上一时刻外部加速度估计值，通过一阶低通滤波器计算出当前时刻的外部加速度估计值；

本步骤中计算的上一时刻k-1时刻的外部加速度估计值为：

其中为卡尔曼滤波器上一时刻k-1时刻输出的最优车身倾角，g为重力加速度常量，a_k-1为上一时刻k-1时刻的单轴加速度计的加速度输出值。其中卡尔曼滤波器输出的最优车身倾角的初始值定义为0，单轴加速度计的加速度输出值的初始值a₀为0，即k＝1时，定义上一时刻卡尔曼滤波器输出的最优车身倾角为0，上一时刻单轴加速度计的加速度输出值为0。

本步骤S2中根据上一时刻k-1时刻的外部加速度估计值，通过一阶低通滤波器计算出当前时刻k时刻的外部加速度估计值为：

$a_{e,k}^{-}=c_a{a}_{e,k-1}^{+};$

其中c_a为固定的滤波系数，其中c_a在0～1之间。

S3、将当前时刻的外部加速度估计值作为外部加速度值，从当前时刻单轴加速度计的加速度输出值中去除外部加速度值，得到当前时刻单轴加速度计重力所产生的重力加速度；

本步骤中从当前时刻k时刻单轴加速度计的加速度输出值中去除外部加速度值，获取到的当前时刻k时刻的单轴加速度计重力所产生的重力加速度a_g,k为：

$a_{g,k}=a_k-c_a{a}_{\mathrm{\ϵ},k-1}^{+}-n_k;$

其中a_k为当前时刻k时刻单轴加速度的加速度输出值；a_g,k为当前时刻k时刻单轴加速度计重力所产生的重力加速度；n_k为当前时刻k时刻测量噪声。

S4、将当前时刻单轴加速度计重力所产生的重力加速度a_g,k代入卡尔曼滤波器的测量方程Z_k，卡尔曼滤波器融合步骤S1中低通滤波处理后的数据，然后根据当前时刻测量方程Z_k计算出当前时刻的最优车身倾角。其中：

本步骤中将当前时刻k时刻单轴加速度计重力所产生的重力加速度a_g,k代入卡尔曼滤波器的测量方程中，得到当前时刻k时刻测量方程Z_k为：

其中v_k为当前时刻k时刻单轴加速度计测量的高斯白噪声。

本步骤S4最优车身倾角获取过程具体如下：通过单轴加速度计估计单轴陀螺仪测量车身倾角的零位偏差，然后根据车身倾角的零位偏差，通过卡尔曼滤波器融合步骤S1中低通滤波处理后的数据，最后根据当前时刻k时刻测量方程Z_k计算出当前时刻k时刻的最优车身倾角其中

其中：

其中T为更新周期，Δb为单轴加速度计估计的单轴陀螺仪的零位偏差；ω_k为当前时刻k时刻单轴陀螺仪输出角速度；w_g,k为出当前时刻k时刻单轴陀螺仪测量的高斯白噪声；w_b,k为出当前时刻k时刻单轴加速度计估计单轴陀螺仪零位偏差的高斯白噪声；w_a,k为出当前时刻k时刻单轴加速度计测量的高斯白噪声；K_k为出当前时刻k时刻卡尔曼增益；

其中为当前时刻k时刻卡尔曼滤波器输出的最优车身倾角和当前时刻k时刻单轴加速度计估计的单轴陀螺仪的零位偏差构成的矩阵；

其中为上一时刻k-1时刻卡尔曼滤波器输出的最优车身倾角和上一时刻k-1时刻单轴加速度计估计的单轴陀螺仪的零位偏差构成的矩阵；

其中为上一时刻k-1时刻车身倾角的预测结果和上一时刻k-1时刻单轴加速度计估计的单轴陀螺仪的零位偏差的预测结果构成的矩阵。

本实施例的车身倾角测量方法首先根据上一时刻卡尔曼滤波器输出的最优车身倾角以及上一时刻单轴加速度计的加速度输出值计算上一时刻外部加速度估计值；然后根据上一时刻外部加速度估计值，通过一阶低通滤波器计算出当前时刻的外部加速度估计值；然后将当前时刻的外部加速度估计值作为外部加速度值，在前时刻单轴加速度计的加速度输出值中去除该外部加速度值得到单轴加速度计重力所产生的重力加速度。最后将上述重力加速度代入卡尔曼滤波器的测量方程中，根据测量方程计算出当前时刻的最优车身倾角。可见，本发明方法根据上一时刻卡尔曼滤波器输出的最优车身倾角估算出当前时刻的外部加速度，并且在测量方程中将估算出的该外部加速度去除，在外部加速度去除的情况下计算得到当前时刻的最优车身倾角，因此本实施例方法有效减少了外部加速度的影响，提高了车身倾角测量的精确度；并且仅仅需要单轴MEMS惯性传感器测试的数据即可实现最优车身倾角的测量，不需要增加额外的其他传感器，适用于低成本的现代农业装备的车身倾角测量。

如图2所示为测试车进行Y轴直线运动时各种测量方法获取到的车身倾角和外部加速度曲线图，其中KF为卡尔曼滤波器在未去除外部加速度情况下的车身倾角测量方法，EAIAK为本实施例中卡尔曼滤波器在去除外部加速度情况下的车身倾角测量方法，AHRS为采用参考姿态传感器的车身倾角测量方法，ACC为采用加速度计的车身倾角测量方法。其中图3为图2的局部放大图。Y轴直线运动输出的外部加速度数据中，从ACC整体曲线中可以直观地观测到Y轴直线运动测量到的外部加速度发生了很大的变化，主要是由Y轴方向加速运动引起，从图3中的局部放大曲线中可以明显地观测到KF方法的误差角度较AHRS方法的误差大，达到5°，而EAIAKF方法最大误差不到1°。

如图4所示为测试车进行X轴直线运动时各种测量方法获取到的车身倾角和外部加速度曲线图，其中KF为卡尔曼滤波器在未去除外部加速度情况下的车身倾角测量方法，EAIAK为本实施例中卡尔曼滤波器在去除外部加速度情况下的车身倾角测量方法，AHRS为采用参考姿态传感器的车身倾角测量方法，ACC为采用加速度计的车身倾角测量方法。X轴直线运动输出的外部加速度数据中，从ACC整体曲线中可直观地观测到X轴直线运动测量到的外部加速度发生了很大的变化，主要是由X轴方向加速运动引起，但各个方法的车倾角测量均保持在0.5°以内。其中KF方法和EAIAKF方法两种方式下精度比较接近，说明X轴直线运动外部加速度引入对倾角传感系统的倾角测量影响较小。

如图5所示为测试车进行Z轴直线运动时各种测量方法获取到的车身倾角和外部加速度曲线图，其中KF为卡尔曼滤波器在未去除外部加速度情况下的车身倾角测量方法，EAIAK为本实施例中卡尔曼滤波器在去除外部加速度情况下的车身倾角测量方法，AHRS为采用参考姿态传感器的车身倾角测量方法，ACC为采用加速度计的车身倾角测量方法。Z轴直线运动输出的加速度数中，ACC整体曲线中可以直观地观测到Z轴直线运动测量到的外部加速度发生了很大的变化，主要由Z轴方向加速运动引起，但各个方法的倾角测量均保持在0.4°以内。其中KF方法和EAIAKF方法的精度比较接近。说明Z轴直线运动时，外部加速度引入对倾角传感系统的倾角测量影响较小。

以AHRS方法为参考，计算得到了KF和EAIAKF两种方法的平均绝对误差MAE(Mean Absolute Error)、均方根误差RMSE(Root Mean Square Error)和最大误差ME(Maximum Error)，如下表1所示：

从表中可以看出，X轴、Y轴和Z轴直线运动下KF方法的数据中可以得到X轴直线运动和Z轴直线运动在外部加速度引入后其倾角测量误差最大不超过0.4307°；而Y轴直线运动中，外部加速度的引入使得车身倾角测量误差最大达到5.3873°。数据表明：外部加速度在X轴直线运动和Z轴直线运动情况下影响较小可以忽略，从Y轴直线运动的数据可以得到KF方法在外部加速度影响下误差最大达到5.3873°而EAIAKF算法的误差最大为1.5265°。可见本实施例的车身倾角测量方法精确较度。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。