python卡尔曼滤波 库 卡尔曼滤波 arduino

介绍

本文将通过 C++ 代码示例和一些说明图来解释如何使用来自MPU6050设备的数据。MPU6050是一款惯性测量单元(IMU)，它结合了 MEMS 陀螺仪和加速度计，并使用标准 I2C 总线进行数据通信。在本文中，我有时会使用术语 IMU 来指代MPU6050 。有许多很棒的文章解释了陀螺仪和加速度计的基本概念，我发现的最好的文章之一是在CH Robotics网站上。我在本文中使用了该站点的一些图像。在 Dronebot Workshop 上还有一段很棒的介绍性视频解释了MPU6050在这里。

MPU6050的机械部分如下图所示。 

是的，它是微观的！但是更大的加速度计图将有助于显示正在发生的事情。

所谓的证明质量悬挂在弹簧上，并在设备加速时自由移动。固定的电极梳在自身和检测质量之间建立电容效应。当设备移动时，电容的变化会被记录并由 ADC 转换为 0 到 32,750 之间的数字值。陀螺仪以类似的方式工作，只是它基于科里奥利效应而不是加速度。

结果是MPU6050会向您抛出一堆数字，您需要解释它们以便在您的项目中使用。本文的其余部分有望帮助您理解这些数字。

设备灵敏度

正如刚才提到的，从电容传感器读取的模拟电压被转换为 0 到 32750 值范围内的数字信号。这些值构成了陀螺仪和加速度计的测量单位。必须拆分测量单位以表示有意义的信息。MPU6050 _通过创建四个灵敏度级别来分配其测量单位，如下面的幻灯片所示。您选择的敏感级别取决于您将如何使用 IMU。例如，如果机器人要进行每秒超过 1000° (167 RPM) 的高速旋转，则应将陀螺仪灵敏度设置为 2000°。在这种情况下，由于陀螺仪必须在很短的时间内覆盖大量旋转地面，因此需要谨慎地拆分其测量单元。然而，对于大多数应用，机器人不太可能旋转得那么快，因此我们可以将灵敏度级别设置为 250°，这是默认设置。这为我们提供了每秒每度 131 个测量单位，从而提供了非常高的精度水平。

加速度计的默认设置为 2g。这应该适用于 F14 以外的大多数应用程序或构建 Tesla 的机械臂。

代码设置

因此，让我们开始查看使用MPU6050所需的代码。我将使用Jeff Rowberg 开发的i2cdev库之一，它显着简化了从MPU6050设备获取数据的工作。我在 Arduino 上安装了示例代码MPU6050_DMP6 。代码示例乍一看有点难以理解，所以我将通过它的关键部分并尝试解释发生了什么。设置代码的关键部分如下所示。

 

void mpu_setup()
{
  
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  } 
} 

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}

　　

 

默认情况下， initialize ()命令将加速度计设置为 +/- 2g，将陀螺仪设置为每秒 250%。这些是最敏感的设置。灵敏度设置已在上一节中进行了说明。testConnection ()将检查它是否可以找到与 IMU 关联的 I2C 设备地址。这应该是 0x68 或 0x69。

下一步是初始化数字运动处理器(DMP)。这是MPU6050的板载处理器，它结合了来自加速度计和陀螺仪的数据。DMP是使用MPU6050的关键，后面会详细说明。

与所有微处理器一样，DMP需要固件才能运行。dmpInitialize ( )命令加载固件并对其进行配置。它还会初始化 FIFO 缓冲区，该缓冲区将保存来自陀螺仪和加速度计的组合数据读数。如果初始化一切顺利，则启用DMP 。

有一些语句为陀螺仪和加速度计提供一些默认偏移量。我将在稍后有关校准的部分中回过头来讨论这些内容。

设置中断的代码的下一部分一开始有点令人困惑，因为我见过很多接线图都没有显示连接的中断引脚。事实证明，您可以在有中断和无中断的情况下使用MPU6050 。如果 IMU 用于需要您向机器人发送控制动作的项目，那么使用中断方法可能是不可取的，因为控制动作可能很难通过。在文章的最后，我将向您展示如何在没有中断的情况下（在轮询模式下）使用MPU6050 。当 MPU 的数据缓冲区已满时，中断仅调用一个设置标志的短 ISR。ISR 显示在上述代码示例的末尾。

校准

在项目中使用 IMU 之前，必须对其进行校准。安装在机器人上的 IMU 不会与地面完美对齐，因此您需要从加速度计和陀螺仪进行一系列数据测量以产生偏移量。从物理学的角度来看，偏移提供了从Body Frame到Inertial Frame 的转换。Body Frame是安装在机器人上的 IMU，而Inertial Frame是完成所有角度和速度计算的框架。



有几个过程可用于校准传感器。我在以下过程中取得了一些成功，它使用了我在此处找到的 Luis Ródenas 编写的代码。将MPU6050安装到机器人上后，只需运行一次代码并记下最终输出。输出将是一组六个偏移值，您可以将其硬编码到脚本中。

在该过程开始时，所有加速度计和陀螺仪偏移都设置为零。然后我们从 IMU 中获取 1000 个读数并计算每个偏移量的平均值。然后将这些值输入 IMU 以成为新的偏移量。校准例程将继续获取平均 IMU 读数，直到校准在某个公差范围内。根据 CPU 的速度，校准可能需要一些时间才能运行。

 因此，让我们看一下我正在使用的校准代码，它是由 Luis Ródenas 编写的。从 IMU 读取原始加速度和陀螺仪数据。如上图所示，此功能用于校准过程中的多个点。

//--------------------------------------------//
// Read the raw sensor values
//--------------------------------------------//
void IMU::readIMU() {

  // Using the MotionApps library
  accX = mpu.getAccelerationX();
  accY = mpu.getAccelerationY();
  accZ = mpu.getAccelerationZ();
  gyroX = mpu.getRotationX();
  gyroY = mpu.getRotationY();
  gyroZ = mpu.getRotationZ();
}

校准过程首先将 IMU 偏移归零。从 IMU 读取最初的 1000 个测量值并计算平均值。然后我们进入主校准程序，循环直到偏移量在公差范围内。

//----------------------------------------------------//
// Calibrate bias of the accelerometer and gyroscope
// Sensor needs to be calibrated at each power cycle.
//----------------------------------------------------//
void IMU::calibrateSensor()
{
  // Initialize the offset values
  setOffsets(0, 0, 0, 0, 0, 0);
  
  ROSLOGString("Reading sensors for first time...");
  meanSensors_();
  delay(1000);

  ROSLOGString("Calculating offsets...");
  calibrate_();
  delay(1000);
}

meanSensor( ) 例程如下所示。丢弃前 100 个读数，然后累积 1000 个测量值并除以样本大小。插入延迟以确保没有重复数据读取。

int buffersize=1000;     // Number of readings used to average
//--------------------------------------------//
// Get the mean values from the sensor 
//--------------------------------------------//
void IMU::meanSensors_() {
  
  long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;

  while (i<(bufferSize+101)) {
    
    // read raw accel/gyro measurements from device
    readIMU();
    
    if (i>100 && i<=(bufferSize+100)) { //First 100 measures are discarded
      buff_ax=buff_ax+accX;
      buff_ay=buff_ay+accY;
      buff_az=buff_az+accZ;
      buff_gx=buff_gx+gyroX;
      buff_gy=buff_gy+gyroY;
      buff_gz=buff_gz+gyroZ;
    }
    if (i==(bufferSize+100)){
      mean_ax=buff_ax/bufferSize;
      mean_ay=buff_ay/bufferSize;
      mean_az=buff_az/bufferSize;
      mean_gx=buff_gx/bufferSize;
      mean_gy=buff_gy/bufferSize;
      mean_gz=buff_gz/bufferSize;
    }
    i++;
    delay(2); //Needed so we don't get repeated measures
  }
}

这是校准过程的主循环。查看代码段，如果您仍然对它的工作原理感到困惑（就像我一样），请继续阅读。

int acel_deadzone=8;     //Accelerometer error allowed
int giro_deadzone=1;     //Giro error allowed
//--------------------------------------------//
// Calibrate sensor
//--------------------------------------------//
void IMU::calibrate_() {
  ax_offset=-mean_ax/8;
  ay_offset=-mean_ay/8;
  az_offset=(16384-mean_az)/8;

  gx_offset=-mean_gx/4;
  gy_offset=-mean_gy/4;
  gz_offset=-mean_gz/4;
  while (1){
    int ready=0;
    mpu.setXAccelOffset(ax_offset);
    mpu.setYAccelOffset(ay_offset);
    mpu.setZAccelOffset(az_offset);

    mpu.setXGyroOffset(gx_offset);
    mpu.setYGyroOffset(gy_offset);
    mpu.setZGyroOffset(gz_offset);

    // Get the mean values from the sensor  
    meanSensors_();
    logMeanValues();

    if (abs(mean_ax)<=acel_deadzone) ready++;
    else ax_offset=ax_offset-mean_ax/acel_deadzone;

    if (abs(mean_ay)<=acel_deadzone) ready++;
    else ay_offset=ay_offset-mean_ay/acel_deadzone;

    if (abs(16384-mean_az)<=acel_deadzone) ready++;
    else az_offset=az_offset+(16384-mean_az)/acel_deadzone;

    if (abs(mean_gx)<=giro_deadzone) ready++;
    else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);

    if (abs(mean_gy)<=giro_deadzone) ready++;
    else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);

    if (abs(mean_gz)<=giro_deadzone) ready++;
    else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);

    if (ready==6) break;
  }
}

开始时，平均值取自上一步并用于提供初始偏移值。为了获得经过良好校准的系统，计划是使这些值尽可能小。这是 IMU 偏移设置为零后的初始平均值示例。

Acclerometer 8531, -31407, 2727
Gyroscope    20, -34, 14

如您所见，还有很长的路要走，因此作为第一次切割，我们采用了平均值的一部分。我不确定除数是如何得出的，但它确实得出了一个有点接近最终偏移值的值：

ax_offset -1066
ay_offset 3925
az_offset 1707
gx_offset -5
gy_offset 8
gz_offset -3

进入循环，将偏移值输入到 IMU，并获取一组新的平均值。使用新的偏移量，下一组平均值开始看起来好多了。

Acclerometer 153, -1084, 16117
Gyroscope    13, -23, 10

　　

将这些除以死区常数并从当前偏移量中减去它会产生 IMU 的新偏移量：

ax_offset -917
ay_offset 3192
az_offset 1469
gx_offset -13
gy_offset 23
gz_offset -9

每次循环运行时，平均值都会变得越来越小。所以它一直持续下去，直到平均值在允许的误差范围内，此时它退出循环，我们就完成了。如果您不想每次打开机器人时都运行校准例程，请将这些偏移值硬编码到项目的设置例程中。

获得方向
有两种方法可以从MPU6050中提取有用的数据。一种方法是读取原始传感器数据值，就像我们在校准过程中所做的那样，并使用该数据计算新方向。第二种方法是从 MPU 的板载数字运动处理器(DMP)中提取数据。要了解这两种方法之间的区别，我们可以看一个如何获得俯仰角的示例。如果您正在为平衡机器人编码，俯仰角将是保持机器人直立的关键。

要使用原始传感器数据获得俯仰角，需要进行多种转换。

EG4YAup2hySgcimq8MkLuB6e34rNB2SF9h94LL1ryzzq

在进行任何计算之前，您必须使用 IMU 偏移量将身体坐标系的读数转换为惯性坐标系。

您需要将原始 IMU 传感器读数转换为度数。

为了减轻漂移的影响，您需要将陀螺仪信息与来自加速度计的读数结合起来。

让我们一一进行。

陀螺仪不报告角度，它们报告设备转动的速度或角速度。为了获得角度位置，您必须随着时间的推移对其进行积分。下面的代码显示了应用于来自陀螺仪的角速率信息的时间积分。

要将机器人的主体坐标系带入惯性坐标系，您必须减去偏移量。偏移值是在校准过程中计算的。如果您要使用原始传感器数据，那么这些偏移值必须存储在您的程序变量中。

在应用积分之前，需要将原始传感器读数转换为度数。这是通过将读数除以灵敏度测量单位来完成的。回想一下，在最高灵敏度设置下，每个度数由 131 个测量单位表示。

 

//--------------------------------------------//
// Get angle from the gyroscope. Uint: degree
//--------------------------------------------//
float IMU::getGyroRoll(int gyroX, int gyroBiasX, uint32_t lastTime)
{
  float gyroRoll;

  //integrate gyroscope value in order to get angle value
  gyroRoll = ((gyroX - gyroBiasX ) / 131) * ((float)(micros() - lastTime) / 1000000); 
  return gyroRoll;
}

以下代码显示了如何计算来自加速度计的俯仰角。可以在网上找到用于计算偏航角、俯仰角和横滚角的公式。同样，您需要减去偏移值。生成的输出以弧度为单位，需要将其转换为度数。

 

//-----------------------------------------------//
//Get angle from the accelerometer. Uint: degree
//-----------------------------------------------//
float IMU::getAccRoll(int accY, int accBiasY, int accZ, int accBiasZ)
{
  float accRoll;

  //calculate angle value
  accRoll = (atan2((accY - accBiasY), (accZ - accBiasZ))) * RAD_TO_DEG;

  if (accRoll <= 360 && accRoll >= 180) {
    accRoll = 360 - accRoll;
  }
  return accRoll;
}

　　

在计算来自加速度计和陀螺仪的俯仰角后，使用互补滤波器来减轻加速度计受到的振动影响，更重要的是，陀螺仪的长期漂移效应。

那么漂移从何而来？正如刚才提到的，陀螺仪不报告角度，它们报告设备转动的速度。为了获得角度位置，您必须随着时间的推移对其进行积分。你可能还记得你在微积分课上学过的，要获得位置，你必须对速度进行积分。由于计算机上使用的时间段有一些定义的长度，例如 10 毫秒，因此积分过程会在位置计算中引入一个小误差。随着时间的推移，这些小错误的累积是导致漂移的原因。当然，时间段越小，漂移就越小，但最终您会遇到 CPU 速度的限制。

互补滤波器计算显示在下面的代码中。有关互补滤波器以及如何调整它们的更多信息，可以在线找到。由于减轻陀螺仪漂移是我们的主要目标，因此代码显示滤波器在该方向上的权重很大。

 

//--------------------------------------------//
// Get current angle of the robot
//--------------------------------------------//
float IMU::currentAngle() {
  
  // Get raw IMU data 
  readIMU();

  // Complementary filter for angle calculation
  float gRoll = getGyroRoll(gyroX, gx_offset, lastTime);
  float aRoll = getAccRoll(accY, ay_offset, accZ, az_offset);
  
  float angleGet = 0.98 * (angleGet + gRoll) + 0.02 * (aRoll);

  lastTime = micros(); // Reset the timer
  
  return angleGet;
}

　　

使用数字运动处理器 (DMP)

所以为了获得俯仰角需要做很多工作！让我们看看如何使用MPU6050的板载数字运动处理器获得俯仰角。DMP卸载了通常必须在微处理器上进行的处理。它维护一个内部缓冲区，该缓冲区结合来自陀螺仪和加速度计的数据并为您计算方向。DMP还负责应用偏移量，因此您不必在项目代码中跟踪这些。这是DMP内部 FiFo 缓冲区的格式。

要将DMP的缓冲区读入您的程序，您可以使用以下语句序列。检查中断状态并将缓冲区读入本地程序变量。一旦完成，就可以访问方向信息。

// Check for DMP data ready interrupt (this should happen frequently)
if (mpuIntStatus & 0x02) {   

	 // wait for correct available data length, should be a VERY short wait    
	 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

   // read a packet from FIFO
   mpu.getFIFOBytes(fifoBuffer, packetSize);
        
   // track FIFO count here in case there is > 1 packet available
   // (this lets us immediately read more without waiting for an interrupt)
   fifoCount -= packetSize;

     // get quaternion values in easy matrix form: w x y z
   mpu.dmpGetQuaternion(&q, fifoBuffer);
}

　　

方向由四元数表示。四元数是一种表示物体在 3D 空间中的方向的方法，可以在计算机上高效计算。它们还避免了旋转角度超过 90° 时出现的问题，称为Gimbal Lock。除非您正在从事无人机项目，否则您可能不会遇到Gimbal Lock问题，因此您可以安全地忽略它。四元数有点难以理解，这就是为什么他们在 100 多年的时间里都被欧拉角所忽视。但是，如果您认真对待机器人技术，就不能太长时间忽视四元数这个主题。有许多学习四元数的在线资源，但我将提供一个我认为有用的可视化资源。

四元数

要理解四元数，将它们与 Yaw、Pitch、Roll 进行比较是很有用的，这是大多数人更熟悉的概念。要表示方向的变化，您首先要指定偏航角，即围绕 z 轴的旋转。然后加上Pitch，也就是绕y轴旋转。最后绕 x 轴滚动。当然，飞机可能会以不同的顺序执行此操作，或者更有可能同时执行此操作，但最终结果仍然是方向发生变化。这里的关键是你只需要三个参数 (ψ, θ, ϕ) 来表示转换。

将此与莫名其妙地需要四个参数的四元数进行对比。所以四元数首先要使用一个向量并将它指向你需要去的方向。这由下图中的红色箭头表示，并且始终是一个单位的长度。由于箭头可以指向 3D 空间中的任何地方，我们需要三个参数来定义它。方向参数以sines形式给出。一旦我们有了方向，我们就可以执行一个滚动来让我们到达最终方向。这就是第四个参数的目的。它以度数（或弧度）指定我们需要旋转多少。



为了确保方向数据对所有应用程序都有用，DMP将其计算存储为四元数。Jeff Rowberg 的程序为您提供了一种将四元数转换为其他有用信息（例如欧拉角和线性加速度）的简单方法。

欧拉角

一旦我们从DMP中检索到数据，我们就可以使用它来获取欧拉角。四元数值被传递到dmpGetEuler()函数以将它们转换为欧拉角。输出以弧度表示，因此如果需要可以转换为度数。可以在线找到将四元数转换为欧拉角的公式，也可以通过检查 Jeff Rowberg 的图书馆找到。

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);

float psi = euler[0] * 180/M_PI;
float theta = euler[1] * 180/M_PI;
float phi = euler[2] * 180/M_PI;

　　

EG4YAup2hySgcimq8MkLuB6e34rNB2SF9h94LL1ryzzq 欧拉角被指定为不考虑重力的纯粹方向变化。

偏航、俯仰、滚转
正如您之前看到的，计算音高信息需要一些编程。将此与从DMP获取音高进行对比，这可以在四个语句中完成。首先需要从预先计算的四元数数据中提取重力分量。然后将重力传递给函数以获得音高。

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

float pitch = ypr[1] * 180/M_PI;

　　

加速度

DMP的缓冲区中也提供加速度数据。然而，由于 IMU 正在移动，加速度计报告的力不仅是地球的力，而且是导致它加速的力。因此，您需要从计算中去除重力以获得线性加速度。如前所述，可以从四元数中提取重力分量，因此我们将它们与加速度读数一起使用来计算线性加速度。

// display real acceleration, adjusted to remove gravity
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
float x = aaReal.x;
float y = aaReal.y;
float z = aaReal.z;

　　

轮询 IMU

MPU6050可以在不使用中断的情况下使用。在此模式下，您将在主控制循环内轮询 IMU。如果您需要向机器人发送其他控制操作，而不希望 IMU 中断使 CPU 不堪重负，则首选此方法。此函数在您的主控制循环中调用。DMP的 FiFo 缓冲区的填充速度比您的控制循环快得多，因此需要清除它，以便我们拥有最新的值。我们等待缓冲区被填满，然后将其返回给程序。

void IMU::readFifoBuffer_() {
  // Clear the buffer so as we can get fresh values
  // The sensor is running a lot faster than our sample period
  mpu.resetFIFO();
  
  // get current FIFO count
  fifoCount = mpu.getFIFOCount();
  
  // wait for correct available data length, should be a VERY short wait
  while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

  // read a packet from FIFO
  mpu.getFIFOBytes(fifoBuffer, packetSize);
}