测试机构

关于测试机构说明，见陀螺仪小型测试平台

测试工程

整个对陀螺仪测试的代码工程都记录在Github上的一个项目中，项目还会随着进展进行更新
项目URL：https://github.com/HITLIVING/-RT-Thread-.git
可以通过Pull下来整个项目进一步了解
对测试功能的实现需要格外关注
gyroscope_app.c
gyroscope_app.h
两个文件

测试方案与步骤

MPU6050驱动

  • 因为最近在学习RT-Thread，而且正好用Mini开发板写了个操作界面，
  • 于是顺手也就把对陀螺仪模块的驱动写在里面了，如果想把数据反馈在显示屏上也方便
  • MPU6050买的是淘宝上10块多的模块，也只是为了验证算法的正确性，模块直接固定在旋转平台上
  • 关于I2C的驱动直接使用的是RT-Thread的驱动，也很方便，节省了一些时间
  • 同时RT-Thread官方也提供了MPU6050的驱动库，直接添加在工程下
  • 完整驱动代码参照团队的RT-Thread教程【RT-Thread】模拟I2C_MPU6050


这里说一下本次测试设置的MPU6050的关键参数**
陀螺仪（速度计）量程500dps
加速度计量程2g（虽然没用到）
采样率200Hz
调用驱动的函数并修改参数即可完成配置
当前芯片工作环境26℃

读取MPU6050原始数据

  • 通过【方案资料】的分析内容，我们重点关注Z轴角速度的输出数据
  • 直接通过RT-Thread的打印函数，通过串口，将数据打印在Finsh指令窗口内
  • 使用山外多功能调试助手将打印的数据保存在.csv文件内
  • 通过excel打开把.csv转成.xlsx
  • 用matlab读取.xlsx文件内的数据绘制波形分析

image.png

图为采样了3000个原始数据后绘制的波形
可以看到

  • 存在较大的零点偏移
  • 噪声明显

可以通过求和取平均值来消除偏移量，采样多少个数据取平均作为偏移量比较合适，下表通过采集不同数量的采样点比较平均值的变化，采样频率为200Hz

  • 从零计数 | 采样点数/个 | 200 | 500 | 1000 | 2000 | 3000 | 4000 | 5000 | 6000 | | :—-: | :—-: | :—-: | :—-: | :—-: | :—-: | :—-: | :—-: | :—-: | | 采样时间/s | 1 | 2.5 | 5 | 10 | 15 | 20 | 25 | 30 | | 平均值 | 99.72 | 99.99 | 100.348 | 100.5 | 100.637 | 100.826 | 100.939 | 101.054 |

如果采用前200个点平均值99.72作为偏移量，在30s后平均值为101.54，角速度零点就会偏差
（101.54-99.72）*（500/32736）=0.027dps

这样三分钟后角速度的零点大约偏差0.027*6=0.162dps，
这样如果全局定位静止不动，在三分钟后，每秒的角度偏差大约0.162。

可以发现平均值在一直上升，而尤其时刚开机时上升幅度较大，随着时间上升幅度变缓
这种现象有可能是芯片刚供电，数据起步存在坏点，

这次我们采样了15000个点观察数据的变化趋势
image.png

如果抛弃了前10s数据点，我们从10s后采样取平均数

  • 从10s后采样 | 采样点数/个 | 200 | 500 | 1000 | 2000 | 3000 | 4000 | 5000 | 6000 | | :—-: | :—-: | :—-: | :—-: | :—-: | :—-: | :—-: | :—-: | :—-: | | 采样时间/s | 1 | 2.5 | 5 | 10 | 15 | 20 | 25 | 30 | | 平均值 | 101 | 100.8 | 100.8 | 101 | 101.2 | 101.3 | 101.38 | 101.42 |

如果采用前200个点平均值99.72作为偏移量，在30s后平均值为101.54，角速度零点就会偏差
（101.42-101）*（500/32736）=0.0064dps

但是这样全局定位从开机到正常工作需要10+1=11s，这个时间较长

  • 从5s后采样

我们再尝试如果抛弃了前5s数据点，我们从5s后采样取平均数

采样点数/个 200 500 1000 2000 3000 4000 5000 6000
采样时间/s 1 2.5 5 10 15 20 25 30
平均值 100.3 100.4 100.7 100.78 100.98 101.08 101.2 101.27

如果采用前200个点平均值100.3作为偏移量，在30s后平均值为101.27，角速度零点就会偏差
（101.27-100.3）*（500/32736）=0.0148dps

这样全局定位从开机到正常工作需要5+1=6s

从表中可以看出角速度平均值随时间近似为线性增长，三分钟后角速度的零点大约偏差0.0148*6=0.088dps，
这样如果全局定位静止不动，在三分钟后，每秒的角度偏差大约0.088°<0.1°

实际上，当采用除速度计之外数据进行融合后，这种数据漂移会得到抑制，见陀螺仪数据融合

因此折中考虑，我们采用连续采样5s后取平均作为偏差值
如果采用前1000个点平均值100.348作为偏移量，在30s后平均值为101.42，角速度零点就会偏差
（101.42-100.348）（500/32736）=0.0164dps
这样三分钟后角速度的零点大约偏差0.01646=0.098dps，
这样如果全局定位静止不动，在三分钟后，每秒的角度偏差大约0.1°

设计对原始数据的处理过程

这样，对原始数据的处理要经过三个阶段

  • 采样5s
  • 对这5s数据取平均
  • 读取减去平均后的数据并单位换算

MPU6050单位换算的方法见
https://www.cnblogs.com/uestcman/p/9433871.html

  • 处理流程的实现
  1. #define OFFSET_SAMPLE 1000
  2. float offset_sample = 0;                    //偏移量计算采样数
  4. void gyr_schedule(void)
  5. {
  6. //**********************  Sample Period test  **************************//    
  7.     tick_now=rt_tick_get();
  8.     tick_delta = tick_now-tick_last;
  9.     tick_last = tick_now;
  11.     switch(gyr_step)
  12.     {
  13.         case gyr_sample:
  14.             {            
  15.                 gyr_sample_dataGet();
  16.                 offset_sample++;
  17.                 if(offset_sample==OFFSET_SAMPLE)
  18.                     gyr_step = gyr_samplecul;                
  19.                 break;
  20.             }                            
  21.         case gyr_samplecul:
  22.             {
  23.                 gyr_sample_cul();
  24.                 gyr_step = gyr_deal;
  25.                 break;
  26.             }            
  27.         case gyr_deal:
  28.             {
  29.                 gyr_data_deal();            
  30.                 break;
  31.             }            
  32.         default:
  34.             break;
  35.     }
  36. }
  • 处理流程的调度器 void gyr_schedule(void) 放在了一个线程内循环进行
  • 为了控制运算频率，调度器被延迟周期调用，设计为每5ms调用一次
  • 但是会存在程序运行占用时间而计算周期被拉长，此时还当作5ms计算时会导致角度计算结果偏小
  • 因此在调度器使用RT-Thread的tick读取函数，计算连续两次被调用之间的时间间隔作为采样周期参与运算
  • 宏定义OFFSET_SAMPLE 作为采样取平均的点数定义，5s即采样1000个点
  • 详细代码见工程下的 gyroscope_app.c 和 gyroscope_app.h 两个文件

对去除偏移量后的数据做信号分析

去除偏移量并对数据做了单位变换后测试了5614个点
波形如图

  • 时域上

image.png

  • 从时域上可以看出除去偏移量后速度计的返回值基本在0上下波动

  • 这5600多的点的平均值为0.0228°，仍存在向上偏移的趋势

  • 频域上

image.png

  • 对这段信号做傅里叶变换后，可以观察频域上的信号特征
  • 可以看到除了有偏移带来的直流信号（低频信号）外，还有频率丰富的噪声信号
  • 噪声信号满足热噪声信号的特征（高斯白噪声）

添加滤波器

根据前面的分析，我们尝试将信号通过Kalman滤波器，观察滤波后的信号特征
滤波器的结构见
Kalman滤波器
详细代码实现见工程中 kalman_math.c 和 kalman_math.h 两个文件

  • 初始化

在陀螺仪的初始化中完成对Kalman滤波器的初始化，赋予参量的初始值

  1. //**********************  Kalman Filter Struct  **************************//
  2. struct KalmanParam KalParam_gz;
  3. #define    KalParam_gz_LastP     0.02
  4. #define    KalParam_gz_Q           0.001
  5. #define    KalParam_gz_R           0.543
  7. void gyroscope_init(void)
  8. {
  9.     /* init the mpu6050 drive */
  10.     mpu6050_hw_init();
  12.     rt_thread_mdelay(50);
  14.     /* Display the temp of mpu6050*/
  15.     char str_temp[5]={0};
  16.     mpu6050_temperature_get(&temp);
  17.     sprintf(str_temp, "%d", temp/100);
  19.     /* Init the Kalman Filter on g_Z*/
  20.     KalmanParamInit(&KalParam_gz,KalParam_gz_LastP,KalParam_gz_Q,KalParam_gz_R);
  21. }

滤波器的参数是在Matlab的调试中选取了一个滤波效果比较好的，而具体的计算依据还需要研究

在数据处理的调度器的第三个环节数据处理过程中，添加对滤波器的调用

  1. void gyr_data_deal(void)
  2. {         
  3.     mpu6050_gyroscope_get(&orignal_gx, &orignal_gy, &orignal_gz);  
  5.     real_gz = (orignal_gz-offset_gz)/65.534;
  7.     KalmanParamUpdate(&KalParam_gz, real_gz);
  9.     printf("%f,%f\n",real_gz,KalParam_gz.out);
  11. }

并将滤波器处理先后的数据均导出到matlab分析

滤波器作用在去除偏移量后的数据

  • 时域上

image.png

  • 红色为滤波前信号，蓝色为滤波后
  • 可以看到静止状态下滤波前信号的起伏范围为-0.2~0.2，经过滤波后信号的起伏范围仅在0~0.05

  • 滤波后的信号起伏范围负数较少，主要是因为输入信号平均值的漂移影响了滤波器的输出

  • 频域上

image.png

  • 红色为滤波前信号，蓝色为滤波后

  • 可以看到噪声信号得到了有效的抑制，保留了有效的低频数据信号和少量低频噪声

  • 可能存在更好滤波器参数，我们保留当前结果展开下一步

静态角速度积分角度计算

  • 对滤波器输出的角速度乘以采样时间进行积分，即可得到通过角速度计计算所得的陀螺仪当前角度
  • 同样在陀螺仪调度的第三个阶段数据处理中完成此过程
  1. Angle_z+=KalParam_gz.out*tick_delta*0.001;

先测试静止情况下陀螺仪的角度计算返回情况

image.png

可以从图中看到经历了不同时间后，计算所得角度的偏移结果

时间/s 0 20 40 60 80 100 120
角度/° 0 0.02 0.045 0.12 0.198 0.3 0.42

我们与不进行滤波直接积分计算的角度进行对比

image.png

从图中可知，如果不进行滤波在20s后就会偏移0.44°，40s后就会偏移1.05°
也印证了滤波算法的必要性和重要性

动态角速度积分角度计算

我们测试，当转动陀螺仪到达固定角度时，计算所得角度与测试平台上的角度的拟合情况

  • 数据源1：陀螺仪计算返回角度
  • 数据源2：测试平台上指针角度刻度

测试从0°->5°->10°->30°->60°->90°->30°->0°
以上动作在1min内完成

image.png

可以看到在小角度转动5°，10°，30°时拟合状况较好，当转动幅度较大时便会实际角度存在较大差值
最大时会相差5°左右

造成这种现象的原因有以下几种可能

  1. 运算频率较低，当大角度变化的加速过程中，5ms可能捕捉不到一些变化导致累积误差较大
  2. 纯信任角速度计的数据，无法对数据校准，累计误差无法消除，详细解释见从陀螺仪获知角度
  3. 滤波算法还可以再优化
  4. 单次实验有偶然误差