基于无驱动结构MEMS 陀螺仪的滚转角测量算法

2023-08-28方旭张伟郭子龙

传感器世界 2023年6期

方旭张伟郭子龙

1.北京信息科技大学传感器重点实验室，北京 100101；2.西安工业大学光电工程学院，陕西西安 710021

0 引言

随着现代战争要求不断提高，采用导弹对目标进行精准打击成为目前军备发展的迫切需求。其中，采用单通道控制的旋转弹更是近程武器发展方向，可大量装备部队，实现范围火力覆盖[1-2]。区别于常规战术导弹的三通道控制，旋转弹弹体在自旋情况下具备低通滤波特性。因此，可通过单通道同时控制弹体的俯仰和偏航方向的运动，即通过滚转角位置信息计算出舵机的张开时间与摆幅，实现对旋转弹的弹道修正，从而精准打击目标。这意味着要想实现对旋转弹的精准制导，需要得到测量精度较高的滚转角数据[3-4]。

本文采用的无驱动结构MEMS 陀螺仪，由于其依靠载体的旋转驱动，无自旋耦合，载体自旋转速越高，陀螺仪的性能越稳定，精度越高，这一特点非常适合应用于旋转弹的姿态测量。无驱动结构MEMS 陀螺仪输出的信号为正弦信号，通过峰值点的判断可以进行重新积分，避免了常规陀螺仪的累积误差。无驱动MEMS 陀螺结构图如图1 所示。

1 滤波算法介绍

在滤波算法的选择上，常用算法主要有算术平均滤波、加权平均值滤波[5]、滑动平均算法[6]、中位数平均滤波[7]、限幅滤波[8]、一阶滞后滤波[9]，以及卡尔曼滤波[10]等改进算法。

由于系统的应用场景为高转速的旋转体滚转角解算，电压参数变化较快，中位数滤波和消抖类滤波并不适用；同时，要求解算的实时性要好，一阶滞后滤波会造成相位滞后，影响系统的实时性，这里也不予考虑；陀螺仪的输出信号为正弦周期信号，普通限幅滤波算法适用性较差。综合应用场景以及解算算法从采样数据处理到解算数据处理等各方面的需求，下面介绍几种常用的滤波算法。

1.1 中位数平均算法

中位数平均算法需要先采样N个值，并对N个值进行排序，去掉最大和最小的L个值，最终计算N-2L个值的平均值。在排序算法的选择上，鉴于N值取值较小，且单片机内存较小，考虑到空间复杂度和算法的实现复杂度，一般选择冒泡排序，对N个值进行排序。

1.2 滑动平均滤波

滑动平均滤波结合了算术平均滤波的优势，传感器连续采样N个数据放入缓存队列中，并计算算术平均值，队列长度固定为N，此后，每次通过传感器采样得到一个新的数据就会删除队首数据，并且把新采样数据放入队尾，再计算队列的算术平均值。滑动平均滤波对于周期性干扰具有很好的抑制作用，但灵敏度低。

1.3 加权平均滤波

加权平均滤波是通过连续观测N个采样值，按照其出现的次数为权重，计算观测值的加权算术平均数。给予新采样值的权系数越大，灵敏度越高，信号平滑度越低。加权平均滤波适用于系统纯滞后时间常数τ较大，采样周期较短的过程；对于纯滞后时间常数τ较小，采样周期较长，变化缓慢的信号，不能迅速反应系统当前所受干扰的严重程度，滤波效果差。加权平均算法的公式为：

1.4 卡尔曼滤波

卡尔曼（Kalman）滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

采用的卡尔曼滤波公式如下：

其中，Kk为卡尔曼增益；H 为测量矩阵；R为测量噪声协方差。

其中，I 为单位矩阵。

2 滚转角测量算法设计

滚转角测量算法包括AD 采样滤波算法、波峰波谷点判定算法、滚转角速度和滚转角解算算法。首先，对不同的滤波算法效果进行对比，设计出合适的AD采样滤波算法；通过五点法对采样得到的正弦数字信号的波峰波谷点进行判定；针对判定得到的波峰波谷点可以计算得到当前的滚转角速度，对滚转角速度进行积分，得到滚转角位置信息。

2.1 AD 采样滤波算法设计

AD 采样后得到的陀螺和加速度计的数字电压量会存在一些噪声信号和异常值，需要通过合适的滤波算法对这些数字量进行处理，得到较为平滑的正弦波形，同时由于系统解算的实时性，滤波后的波形与实际波形相比，不能有较大的相位滞后与失真。对于后续算法解算的滚转角速度同样也需要进行滤波处理，使得解算的角速度收敛接近真实值。

对于模拟信号，经过AD 采样后得到的数字信号，数据处理速率要求很高，因此，需要结构简单、高效的数字滤波器。由上文可知，对于AD 采样可以选用加权平均滤波、滑动平均滤波以及中位数平均滤波。

通过测量系统的AD 采样实验后，分析数据可得，静止状态下，未采用滤波算法的采样数据与实际零位电压转换数字量差值分布图如图2 所示，经过计算后得到均方差为571.72，采样值与实际值的平均误差为520.30。

静止状态下，采用加权平均滤波算法的采样数据与实际零位电压转换数字量差值分布图如图3 所示，经过计算后所得均方差为403.04，与实际值的平均误差为544.66。

运动状态下，未采用滤波算法和采用加权平均滤波算法的波形对比图如图4 所示。

静止状态下，采用滑动平均滤波算法的采样数据与实际零位电压转换数字量差值分布图如图5 所示，经过计算后所得均方差为297.79，与实际值的平均误差为430.76。

运动状态下，未采用滤波算法和采用滑动平均滤波算法的波形对比图如图6 所示。

静止状态下，采用中位数平均滤波算法的采样数据与实际零位电压转换数字量差值分布图如图7所示，经过计算后所得均方差为349.20，与实际值的平均误差为494.98。

运动状态下，未采用滤波算法和采用中位数平均滤波算法的波形对比图如图8 所示。

由上文可知，采用加权平均滤波算法处理的采样数据，在静止状态下，数据的均方差减小，但与实际零位电压的误差变大；在运动状态下，噪声点减少，同时也产生了新的噪声点，但数据的曲线总体趋向是更为平滑。采用中位数平均滤波算法处理的采样数据，在静止状态下，数据的均方差与平均误差减小；在运动状态下，噪声点没有明显的减少，正弦曲线的平滑度也没有得到显著地提升。尽管中位数平均滤波在静止状态下数据的滤波效果较好，但系统主要用于测量旋转体运动状态下的滚转角，且该滤波算法需要取多个值经过排序处理后取平均值，需要消耗较多的运算时间，对于算法整体运算实时性有较大影响，所以本文不予考虑。采用滑动平均滤波算法处理的采样数据，在静止状态下，有着显著的提升，均方差与平均误差最小；在运动状态下噪声点明显减少，正弦曲线也变得更加平滑；同时，在算法时间方面，滑动平均滤波仅需在算法末端对取值点进行滑动操作即可，不占用太多运算时间，整体运算速度得到提升。

通过上述分析对比可以看出，滑动平均滤波在静止状态下和运动状态下表现良好。由于滚转角测量系统的应用场景是高转速工况下，滑动取值求平均值的方法尽管保证了正弦曲线的平滑度，但在高转速工况下可能会导致数据有偏差，所以，本文综合滑动平均滤波算法和加权平均滤波算法，对于这一采样电压值采用滑动加权平均算法，定义4 个过去点以及1 个当前点，在循环中进行滑动取值，根据每个点的权重取最终的加权平均值，保证当前点有较高的权重，距离当前点间隔越大，权重越低，从而避免测量值的失真。

静止状态下，采用滑动加权平均滤波算法的零位电压图如图9 所示，经过计算后，所得均方差约为0.0176，转换数字量为280.77。

运动状态下，未采用滤波算法和采用滑动加权平均滤波算法的波形对比局部放大图如图10 所示。

从上文计算数据以及波形图可知，采用滑动加权平均滤波后，均方差减小，运动状态下，异常峰值点被削去，曲线趋于平滑，但与模拟电压的正弦曲线相比仍有出入，需要进一步改进滤波算法。

2.2 卡尔曼滤波算法设计

上文中，陀螺仪和加速度计采样数据的滤波算法需要进行改进，选择卡尔曼滤波算法对采样数据进行二次滤波。卡尔曼滤波算法能够对数据进行实时的更新与处理。

本文采用的卡尔曼滤波算法根据公式（2）～（6）简化变形。在更新加速度计和陀螺仪的数字电压值时，设计了一个k值，并在循环中对k值进行累加。当k值小于等于4 时，卡尔曼滤波状态值设为经过滑动加权平均滤波后的数字电压值。当k值大于4 时，第1 步，计算先验估计协方差P，P为程序预设值加上设置的过程噪声协方差；第2 步，计算卡尔曼增益，代入计算完成的P和设置的测量噪声协方差；第3 步，计算滤波后最优估计值，代入实际观测值（采样得到的数字电压值）和预测值，得到残差；第4 步，将最优估计值赋给观测值并代入后续参数计算中，最后更新P。

采用卡尔曼滤波算法时，需要预先设置几个参数，并通过调整这几个参数的值，改变系统收敛速度趋于正常，使得输出的估计值更加接近真实值。先验估计值，在k小于等于4 的时候，预设为每次循环采样值转换后的数字电压值；先验估计协方差P预设为与理想值同一数量级或较小的数，预设为0.1，来达到较快的初始收敛速度，当系统进入稳态之后，P值会收敛成一个最小的估计方差矩阵，这个时候的卡尔曼增益也是最优的。

参数Q值与R值对于滤波性能的影响：当R值一定时，Q值越大，观测值在滤波值中反应的权重越大，越能表现观测值的状态，反之亦然；当Q值一定时，R值越大，观测值在滤波值中反应的权重越小，对于模型预测值信任度越高，反之亦然；当R值不变时，Q值越小，系统越容易收敛，如果Q值过于小，则容易导致滤波值发散，反之亦然；Q值不变时，R值越小，系统收敛得越快，但是过小的话会导致系统产生振荡，R值太大，卡尔曼滤波响应会变慢。

综合考虑滤波器在随机性和惯性等方面的表现，首先，确定一组参数——Q值和R值；随后，将Q从小往大调整，将R从大往小调整；先固定一个值，再去调整另外一个值，看收敛速度与波形输出；最后，选择收敛速度与波形最佳的参数值，保证数字电压值波形与实际电压波形相位差尽量小，且波形趋于正弦波形。

静止状态下，增加卡尔曼滤波算法的零位电压图如图11 所示，经过计算后所得均方差约为0.0091，转换数字量为145.77。

运动状态下，未采用滤波算法与采用滑动加权平均滤波算法增加尔曼滤波算法的波形对比局部放大图如图12 所示。

通过上述分析，加入卡尔曼滤波算法后，均方差大幅减小，电压曲线平滑，趋近于模拟电压信号的正弦波形。因此，AD 采样算法部分最终采用滑动加权平均滤波算法结合卡尔曼滤波算法，尽管这样做增加了算法总体的时间，但是采样得到数字信号的正弦波形更加平滑且规范，在经过波峰波谷点判定时，减少了误判的可能性，能够提高算法的解算精度。

2.3 波峰波谷点判定

上文对采样信号进行处理和滤波，得到了较为理想的正弦波形后，需要对信号波形进行波峰波谷点的判定，以此来解算旋转体的滚转角速度。对于信号波形可能出现的异常点，采用三点法判定可能会产生误判，所以本文采用了五点法[11]判定波峰波谷点，减少了误判的可能性。

滤波后，每个采样点电压值在五点循环中，每个周期都会得到电压最大值和电压最小值。采用五点法判定需要取5 个点，这5 个点刚好就是滤波算法中滑动加权平均算法中的5 个点。波峰到波谷最少需要获取10 个点，所以当上文所设的k值大于10，开始进入判定阶段，并且设置k值恒等于11，防止不断自增后数字溢出。进入判定阶段，波峰判定需要满足五点判定，即波峰点大于左右点，且左右点分别大于各自左边或右边一点，排除异常点干扰，同时通过判定波峰点电压值与周期内获取的电压值最小值差值的大小来避免异常周期波形的错误判定，波谷点的判定同样如此。判定成功后，进入角速度解算阶段。

2.4 滚转角速度解算

确定无驱动结构MEMS 陀螺或加速度计去噪后的数字信号的波峰或波谷点，并记录波峰或波谷所对应的时间值。通过陀螺仪或加速度计相邻波峰或波谷点的时间信息，由于陀螺仪和加速度计输出电压频率与载体自旋频率相等，通过公式（7）和公式（8）求得滚转角速度：

由于采用五点法判定波峰波谷值，设置五点进行滑动循环，每当判定次数大于4 次时，所判定的下一个点才有可能为波峰或波谷点。通过上文可知，需要知道波峰波谷点的上一个成功判定点是波峰还是波谷点，算法内采用了标记值对上一个有效点进行判定。

在加速过程中，旋转体的转速是在迅速提升的，因此算法设置有效点判定次数这一参数。参数小于预设值时，在不同的参数范围内对解算的角速度乘以相应的倍数；参数大于预设值时，解算角速度与上一次有效角速度求算术平均值，同时对角速度进行卡尔曼滤波，避免初期加速时间短的情况下很快达到稳定状态而导致解算角速度无法回到正常值。

2.5 滚转角解算

通过上文解算得到角速度，对角速度进行积分求取滚转角，滚转角位置求取通过公式（9）解算得到：

其中，φ为测量的滚转角；φ0为上一个时间点的滚转角；为过程中某一时间点的角速度；ti为过程中某一时间间隔。

常规MEMS 陀螺仪具有累积误差，在积分过程中，会将前一刻漂移误差代入到下一刻的角度计算中，形成累积误差，在高转速状态下误差累积较大，无驱动结构MEMS 陀螺仪如果一直不断积分也会存在累积误差。但是，陀螺仪输出的是正弦波形，因此在波峰波谷点的判定环节，如果本次循环判定的点为波峰点或波谷点，此时，φ0则为预设好的波峰波谷点相对应的滚转角位置，通过实验转台输出正弦信号波峰波谷点所对应的滚转角度，可以得到这一预设的滚转角位置信息。陀螺仪与加速度计输出电压对应的转台滚转角波形图如图13 所示。

由图13 可知，当陀螺仪电压处于波峰时，转台滚转角约为200°，加速度计电压处于波峰时，转台滚转角约为20°，设置这些初始值用于滚转角的解算。当该点未判定为波峰波谷点时，滚转角则在上一个时间点解算的滚转角基础上进行积分运算。

3 实验测试

本次实验的测试平台为高精度三轴转台。图14 为测量系统样机，第1 层背面由加速度计以及稳压电源组成，正面由稳压电路以及2 排插针组成，集成化的陀螺仪通过插针嵌入到第1 层中，构成了系统的第1层电路；第2 层电路主要为信号抬升电路与反相器电路，通过第2 层可以将陀螺仪信号与加速度计信号对准，方便算法解算；第3 层电路主要为数字信号电路，包含模数转换与STM32 单片机系统电路。测试时，将样机固定在转台上，并对样机进行供电，以及将样机输出的485 信号传输到上位机进行通讯。

为了验证算法测量结果是否满足精度要求，设计并进行了如下的实验，在PC 机控制的三轴高精度转台上进行：将该测量系统安装在三轴转台的内环轴心上，通过PC 机接收系统输出的滚转角速度和滚转角度，与PC 机获取的三轴转台实时的姿态角度进行对比。通过对5～10 r/s 转速进行测量，计算滚转角速度和滚转角的解算误差，绘制了如图15 所示的误差图。

从图15 可以看出，滚转角测量算法在5～10 r/s稳定转速下，滚转角速度的测量误差基本处于5°/s 以内，仅在5 r/s 转速下误差为5.4°/s，略高于5°/s；滚转角解算误差如图可以看到，在稳定转速范围内，滚转角的解算误差都没有超过8°。因此，测试样机在5～10 r/s 稳定转速下，滚转角速度解算误差基本在5°/s 以内，滚转角测量误差在10°以内。