Robomaster电控入门（6）惯性测量单元IMU

IMU介绍

Inertial measurement unit，简称 IMU，即惯性测量单元，该单元能够反馈自身的运动状态（加速度）和位姿，是机器人身上最重要的传感器之一。IMU能够应用在在机器人导航，机器人控制等诸多方面，可以说IMU的好坏对于一个机器人来说有着近乎决定性的作用。

在Robomaster比赛中，一般各大参赛队会在机器人的云台上搭载IMU，用以反馈云台的yaw轴和pitch轴的角度和角速度。需要注意的是，尽管依靠云台电机6020的编码器同样可以实现以上数据的获取，但是由于云台控制对于灵敏度和精度的要求比较高，而云台电机编码器反馈的数据是离散的，并且速度数据波动较大，用它来做控制并不能够达到很好的效果，所以目前的主流方案依然是通过IMU的数据来做云台的闭环控制。

IMU的选型方案非常多，从几十到几千乃至上万不等。一般IMU内部会包含陀螺仪和加速度计，陀螺仪用来反馈角速度和计算位姿，加速度计用于反馈线速度，陀螺仪和加速度各自提供三个轴向的速度数据，所以一般这种IMU被称为六轴IMU。基于不同原理去制作的陀螺仪和加速度计，能够达到的精度范围也有一定的区别。

但是对于IMU来说，总会有一个难以克服的问题，即累积误差。由于IMU通过是通过对内部的陀螺仪获取的角速度数据对时间进行积分获取角度数据的，无论一个陀螺仪的精度有多高，总是会在每个时刻产生一些误差，随着积分效应，误差逐渐累积，最后就会产生累积误差。

为了解决累积误差问题，一般厂家会在IMU内部再集成一个三轴磁力计，从而构成一个所谓的九轴IMU。磁力计的作用是对IMU计算出的位姿定期进行较准，从而解决累积误差。但是磁力计不适合在电磁环境复杂的场合下使用，如果附近有能够产生强磁场的设备，会对磁力计的数据产生非常大的影响。

可以在下图中看到，比较常用的微机械陀螺仪（MEMS）其累积误差水平大概在每小时10°到100°。但是由于实际上每局比赛时间并没有那么长，所以其实累积误差造成的影响并不是特别严重，没有必要为了过高的精度要求而去购买特别贵的陀螺仪。

关于IMU的选型，不同的队伍有着不同的方案，而且一般来说都经过了若干次的迭代。

IMU选型上的坑很多，其中最严重的问题就是由于各种原因引发的复位/离线问题。在这种情况下，失去反馈的云台会立刻失控，从而发生“疯头”。

以下针对IMU总结一些需要注意的要点

• 确保供电稳定，如果供电电压有较大的波动可能会引发IMU掉线
• 确保物理防护，在赛场上的冲撞/弹丸打击可能会造成IMU的移位/掉线
• 确保陀螺仪量程，如果撞击产生了超出陀螺仪量程的大角速度，可能会引发云台偏移的问题
• 确保线路连接，保证通信线路电连接良好，尤其是当硬件方案涉及到比较长的走线时
• IMU会受到温度影响，如果要在冬天时把机器人带到室外，记得采取一定的保温措施（比如加热电阻）

以下是一些防范IMU离线引发严重问题的方案

• 准备电机闭环方案，检测到IMU离线后可以自动/手动切换成电机编码器反馈
• 在云台上装载多个IMU，检测到一个离线后将反馈源切换成另一个
• 采用官方开发板/自研开发板上集成的IMU，相比独立的IMU模块来说风险更小

最后推荐一个讲IMU的知乎专栏，有兴趣的同学可以看一看。

https://zhuanlan.zhihu.com/p/41299359

开发板板载IMU

官方提供的开发板自带IMU，用户手册中的介绍如下

如果要使用开发板板载IMU，则必须将开发板固定在云台上可以同时随着yaw轴与pitch轴运动的位置。板载IMU的需要自己完成姿态解算，姿态解算是通过SPI读取MPU6500的寄存器数据后，将三轴加速度计和三轴陀螺仪的数据进行数据融合，解算出当前的位姿，其中为了能够使用矩阵进行快速的运算，需要将欧拉角转换成四元数。

姿态解算的推导涉及到比较复杂的数学过程，这里就不加以太多的介绍了，有兴趣的可以自己去看下面的博客。

姿态解算

• https://blog.csdn.net/u010076999/article/details/50922568

• https://blog.csdn.net/qiguizhe/article/details/59133514

滤波

• https://blog.csdn.net/u012814946/article/details/52850400

四元数

• https://blog.csdn.net/shenshen211/article/details/78492055

牛顿迭代快速求根

• http://www.matrix67.com/blog/archives/361

官方车代码里面同样有解算的代码，并且有多种算法。下面这段代码是一个禁用了磁力计数据的算法，也是我自己以前移植到自己的工程里进行过测试的，由于当时我发现磁力计读取到的数据干扰很大，于是选择了禁用了磁力计数据的算法，只使用加速度计和陀螺仪进行数据融合。其中invSqrt是运用牛顿迭代法快速求平方根，是用于归一化处理的，官方给的注释在我看来已经已到位了，因此在这里不去加更多的注脚。

void mahony_ahrs_updateIMU(struct ahrs_sensor *sensor, struct attitude *atti)
{
  float recipNorm;
  float halfvx, halfvy, halfvz;
  float halfex, halfey, halfez;
  float qa, qb, qc;

  gx = sensor->wx;
  gy = sensor->wy;
  gz = sensor->wz;
  ax = sensor->ax;
  ay = sensor->ay;
  az = sensor->az;
  mx = sensor->mx;
  my = sensor->my;
  mz = sensor->mz;
  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
  {

    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Estimated direction of gravity and vector perpendicular to magnetic flux
    halfvx = q1 * q3 - q0 * q2;
    halfvy = q0 * q1 + q2 * q3;
    halfvz = q0 * q0 - 0.5f + q3 * q3;

    // Error is sum of cross product between estimated and measured direction of gravity
    halfex = (ay * halfvz - az * halfvy);
    halfey = (az * halfvx - ax * halfvz);
    halfez = (ax * halfvy - ay * halfvx);

    // Compute and apply integral feedback if enabled
    if (twoKi > 0.0f)
    {
      integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
      integralFBy += twoKi * halfey * (1.0f / sampleFreq);
      integralFBz += twoKi * halfez * (1.0f / sampleFreq);
      gx += integralFBx; // apply integral feedback
      gy += integralFBy;
      gz += integralFBz;
    }
    else
    {
      integralFBx = 0.0f; // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }

    // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
  }

  // Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
  gy *= (0.5f * (1.0f / sampleFreq));
  gz *= (0.5f * (1.0f / sampleFreq));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
  atti->roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // roll     -pi----pi
  atti->pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;                                // pitch    -pi/2----pi/2
  atti->yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 57.3;  // yaw      -pi----pi
}

除了读取寄存器值进行解算之外，MPU6500的姿态数据也可以通过芯片自身集成的DMP模块获取，有兴趣的同学可以自己加以研究。

最后说一下我自己使用板载陀螺仪进行解算的体验，实际解算出来的角速度数据和角度数据都是比较可靠的，波动也很小，我将板子静止放了10分钟左右，只观测到了极小的累积误差。但是可能是由于我的SPI读取没有开DMA，或者是RTOS的任务设置出了一些问题，角度获取的实时性不是很好，所以我最后选择了用电机反馈角度数据，用IMU反馈角速度数据。此外最后由于最后的设计中没有把开发板安装在云台上，所以还是使用了独立IMU模块的方案。

JY901

JY901是维特智能出品的一款小型IMU传感器，介绍它绝不是收了广告费，仅仅是因为去淘宝上搜九轴传感器，第一个能够搜到的就是它，而且在比赛中我们也确实使用了该传感器，并且发现了它的一些问题。这里仅仅是作为一种独立IMU的方案对它进行评价而已，如同前文所说，还有许多厂家提供的独立IMU传感器方案，RM官方也已经推出了自己的IMU模块，只不过目前还在内测中，还没有公开发售。

该传感器大小接近一个硬币，集成了MPU6050/9250和磁力计，可以通过UART和IIC两种方式进行通信，我们使用了UART进行通信，此外该模块在内部完成了数据解算，可以直接通过接口进行读取，并且在模块内部进行了滤波，可以获得更稳定的数据，所以在使用时，相比板载IMU，JY901可以说是非常容易驱动。

最后，该模块可以通过上位机完成波特率，量程等一些参数的设置，可以说是比较方便的。

介绍完这个模块的优点，该说说使用它时遇到的坑了。其中最重要的一点就是模块的自动复位问题，按照官方的说法，模块的复位只能通过硬件拉高复位引脚和软件输入复位指令两种方法，但是在实际使用时，我们还是遇到了自动复位的情况。我们带着问题去询问客服，也没有得到一个好的解答，在官方的技术论坛上同样没有搜索到合理的解释。

最后我们排查出的原因是，当JY901的供电有较大波动时会引发自动复位，由于硬件电路的问题，JY901的供电会产生在5V到0V之间的波动，这个波动引发了复位，而在复位之后，IMU的波特率发生了改变，进而使得反馈数据直接挂掉，云台就失控了。

结语

IMU可以说是整个RM机器人中最不安定的点之一，从每年都能看到很多“疯头”的队伍来看，在部分参赛队中稳定性受到的重视程度还是不够。但实际上IMU的稳定性并不是非常难以解决的问题，如前文所说，双IMU，或者电机闭环之类的冗余设计方案都可以很好地降低风险，甚至在设计电路时，多考虑一些可能诱发风险的因素都可以避免很多问题。

最后一句话送给所有入坑RM的同学————记得多做测试。