https://mjwhite8119.github.io/Robots/mpu6050

介紹

本文將通過 C++ 程式碼示例和一些說明圖來解釋如何使用來自MPU6050裝置的資料。MPU6050是一款慣性測量單元(IMU)，它結合了 MEMS 陀螺儀和加速度計，並使用標準 I2C 匯流排進行資料通訊。在本文中，我有時會使用術語 IMU 來指代MPU6050 。有許多很棒的文章解釋了陀螺儀和加速度計的基本概念，我發現的最好的文章之一是在CH Robotics網站上。我在本文中使用了該站點的一些影象。在 Dronebot Workshop 上還有一段很棒的介紹性視訊解釋了MPU6050在這裡。

MPU6050的機械部分如下圖所示。

是的，它是微觀的！但是更大的加速度計圖將有助於顯示正在發生的事情。

所謂的證明質量懸掛在彈簧上，並在裝置加速時自由移動。固定的電極梳在自身和檢測質量之間建立電容效應。當裝置移動時，電容的變化會被記錄並由 ADC 轉換為 0 到 32,750 之間的數字值。陀螺儀以類似的方式工作，只是它基於科里奧利效應而不是加速度。

結果是MPU6050會向您丟擲一堆數字，您需要解釋它們以便在您的專案中使用。本文的其餘部分有望幫助您理解這些數字。

裝置靈敏度

正如剛才提到的，從電容感測器讀取的模擬電壓被轉換為 0 到 32750 值範圍內的數字訊號。這些值構成了陀螺儀和加速度計的測量單位。必須拆分測量單位以表示有意義的資訊。

加速度計的預設設定為 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);
}