拖地机中可以用单轴陀螺仪，例如mint5200中用的就是模拟单轴陀螺仪，但模拟陀螺仪要外接运算放大器，还需要CPU的AD转换器进行高速处理。因为计算的是角速度，因此运算实时更新要求很高。MPU6050是数字3轴陀螺仪和3轴加速度传感器一体的芯片，输出是16位的数字信号。售价并不贵，2012年的价格大约14元人民币左右，而ex3500价格大约8元，加上外接的运放以及多出来的焊点的贴片的费用，其实价格相差不了多少。但使用MPU6050则性能上一个档次。

MPU6050是美国invensense的产品，MPU-6000（6050）的角速度全格感测范围为±250、±500、±1000与±2000°/sec(dps)，可准确测量快速与慢速转动，并且，用户可设置参数让加速度的测量范围为±2g、±4g±8g与±16g。

读取原始数据并不复杂，目前有很多基于arduino的程序，而在STM32上运行的并不多见。在STM32上运行需要通过I2C总线读取内部的数据，然后通过CPU定时中断程序读取，从而计算出角速度。如果计算单轴的数据，直接通过咋定时中断程序中积分可以得出角度，但实验发现，如果拖地机完全水平，那么左右转动的计算是正确的。如果带一定倾斜角度转动，实际转动的角度和计算的角度有差异，具体差异受转动速度和倾斜角度影响。原因是如果不是水平，则在x,y,z轴中其它两个轴具有投影分量，直接影响了水平轴的计算。为了解决这个问题，需要用到旋转矩阵，通过旋转矩阵计算出欧拉角，也就是pitch,yaw ,roll。但此方法也有一定的局限，也就是在90度的时候有参数趋向无穷大，也就是万向节锁的问题。因而改进的方法是四元素法，当然四元素法比较复杂，涉及的知识较多。在MPU6050内部有个DMP，能直接输出四元素，欧拉角。但DPM并不公开，需要和invensense签约。DMP运行前需要加载运行一个固件，也就是在初始化的时候加载固件后，MPU6050开始运行，并输出四元素。如果DPM不运行，则MPU6050的很多优势并不能很好发挥。如果不用DMP,则需要用外部处理器高速处理旋转矩阵，并在每5ms更新一次矩阵的计算，而且矩阵的每个元素包含了一系列不同的运算，因此对处理器资源的占用非常大。

由于单独陀螺仪计算出来的数据也有存在偏差的问题，为了将偏差最小，应该融合加速度传感器的数据，然后分别给予两者不同的权重。最终计算出经过融合的角度数据。

陀螺仪过来的数据存在噪音。比如机器运行的时候的细微震动，因为PWM波导致芯片受到的电源干扰，还有陀螺仪自身的漂移等。如果这些干扰不去除，最终计算出来的角度也不可靠，从拖地机运行的角度看就是角度调整不稳定，而且容易发生偏离。因此必须进行专门的滤波处理。可以参考卡尔曼滤波的设计，也就是通过卡尔曼滤波算法，最终采集的数据收敛到和实际运行符合的曲线上。

另外，拖地机上采用陀螺仪的目的是对运行偏移角度进行校正。但是如果要求对长时间的角度的偏移修正，难度很大。比如在10分钟内，要求角度偏移小于2度。陀螺仪对短时间的角度变化是能检测出来的，对长时间的角度变化检测很困难。因为首先陀螺仪自身的数据漂移会随着时间的积累最终反映在角度的变化上；另外是收到 MEMS本身的最高输出精度的限制。MPU6050具有±250、±500、±1000与±2000°/sec (dps) 四种范围，可选择设置为250这档，这样在FS_SEL=0的情况下，AD为16bit的输出情况下，LSB是131(º/s)。陀螺仪还有零点漂移，而且零点漂移一直会变化。除了零点漂移外，其检测的角速度也存在随着时间的漂移，和温度也有关系。以上这些因素都要深入分析，要非常仔细计算每个干扰因素对整体输出的影响，采取对策使输出尽可能符合设计要求。

以下是MPU6050读取原始数据的程序，首先是初始化

在初始化的时候将陀螺仪和加速度传感器调整到最灵敏也就是+/- 2g 和 +/- 250 degrees/sec, 设置时钟源为 X Gyro参考输入, 这比内部振荡器性能稍微好些。

voidMPU6050_Initialize()

{

  MPU6050_SetClockSource(MPU6050_CLOCK_PLL_ZGYRO);

  MPU6050_SetFullScaleGyroRange(MPU6050_GYRO_FS_250);

  MPU6050_SetFullScaleAccelRange(MPU6050_ACCEL_FS_2);

   MPU6050_SetSleepModeStatus(DISABLE);

}

然后测试MPU6050是否连接正常，这主要用来检测焊接贴片是否有问题。

bool MPU6050_TestConnection()

{

  if(MPU6050_GetDeviceID() == 0x34) //0b110100; 8-bit representation inhex = 0x34

    return TRUE;

  else

    return FALSE;

}

然后在定时中断程序内读取原始输出数据，以下程序读出三个方向的加速度和三个方向的角速度数据

void MPU6050_GetRawAccelGyro(s16*AccelGyro)

{

 inti;

u8 tmpBuffer[14];

  MPU6050_I2C_BufferRead(MPU6050_DEFAULT_ADDRESS,tmpBuffer,MPU6050_RA_ACCEL_XOUT_H, 14);

  /* Get acceleration */

  for( i=0; i<3; i++)

    AccelGyro[i]=((s16)((u16)tmpBuffer[2*i] << 8) + tmpBuffer[2*i+1]);

  /*Get Angular rate */

  for( i=4; i<7; i++)

    AccelGyro[i-1]=((s16)((u16)tmpBuffer[2*i] << 8) +tmpBuffer[2*i+1]);        

}

一种简单的方法是在数据读出以后，经过滤波，在中断程序内积分计算，获得角度偏移。如果单轴的陀螺仪。这样计算在短时间内不太会影响性能，如果要求长时间保持角度的稳定，就不容易实现。因为拖地机运行的时候，特别是转弯的时候，前部的机身角度有起伏，这是一个三维的姿态变化。需要用到前面提到的矩阵运算，因此要做更加复杂的计算。MPU6050内部有个DMP,其实就是个处理器对原始数据处理。如果用了DMP，那么直接输出解算好的数据就可以，外部CPU负担很轻，但假如不用内部的DMP,则还需要自己解算，对软件和硬件都是不少的负担。

在陀螺仪调试正常后，实际测试结果表明，在10分钟内能让方向维持在可接收的范围内，超过就不理想。因此拖地机还配合外部红外线定时修正角度的方法。具体见本章前面导航部分。

直线行走的陀螺仪角度修正在主控程序中进行，当检测到角度发生偏转后，调整左右马达的速度，使角度恢复到偏移前的状态。

侧向滑动的角度检测也通过陀螺仪数据进行，知道侧向滑动就能记录目前的路线，分析障碍物的情况，决定下一步的动作。