全能編程開發工程師之gimballock

一、什麼是gimballock

在飛行器控制中，經常會使用到互相垂直的三個陀螺儀。由於歷史原因，這三個陀螺儀的輸出信息通常分別稱為pitch、yaw和roll（俯仰、偏航和翻滾）。在飛行器姿態控制時，一般會根據姿態感測器輸出的角度信息計算目標姿態與當前姿態的角度差，進而通過PID等控制方法對飛行器進行控制，使其達到目標姿態。

然而，飛行器姿態控制過程中常常會遭遇一種困境——gimbal lock。當飛行器的姿態遭遇到gimbal lock時，三個陀螺儀的輸出信息就會出現奇異點，控制器很難確定當前準確的姿態信息，因而會導致飛行器失控。

二、gimballock的產生原因

想要深入理解gimballock，需要對矩陣旋轉進行一定的了解。在三維計算中，通常將姿態信息表示為一個3×3的旋轉矩陣。旋轉矩陣有三個要點：旋轉軸、旋轉角度和右手坐標系。如果某一個軸旋轉180°，那麼這個矩陣就出現了奇異點——gimbal lock。

舉例來說，考慮一架飛行器正在執行一次俯仰動作（pitch motion），也就是要將飛行器從平直的飛行狀態調整到一個俯仰的狀態。在這個過程中，飛行器的x軸旋轉了一定角度，而飛行器的y軸恰好垂直於地面，所以y軸這個維度和x軸這個維度相關聯了起來。此時，如果再對z軸（也就是飛行器的水平軸）進行旋轉，就會導致x軸和z軸之間的信息重疊，即使再輸入正確的控制信號，也無法得到正確的控制效果。這樣一來，就出現了gimbal lock。

三、如何避免gimballock

為了避免gimbal lock，我們需要尋找一種方法來解除三維旋轉中的奇異點。最直接的方法，就是使用四元數（quaternion）進行旋轉。四元數可以看作是一種擴展了複數的數學對象，可以用四個數字表示，分別為實數部分和三個虛數部分。通過四元數的乘積運算，我們可以方便地進行旋轉操作，並且不受shear的影響。因此，使用四元數進行旋轉，可以有效地避免gimbal lock。

四、gimballocked和大疆

gimballock問題不僅存在於理論計算中，也是實際應用中遇到的難題。目前，大疆的無人機產品已經採用了基於四元數的感測器融合演算法，從而大大提高了飛行器的姿態控制穩定性，使其更加適合各種複雜的應用場景。

在實現中，我們可以使用大疆的飛控組件進行快速開發。首先，需要使用DJI Assistant 2軟體進行控制器的初始化和固件升級操作。然後，使用DJI SDK提供的高級API介面，可以輕鬆地獲取飛行器的實時姿態信息，並且進行飛行控制的設計和實現。

#include 

// 回調函數，獲取飛行器姿態信息
void AttitudeCallback(const dji_sdk::Attitude::ConstPtr& msg){
  // 獲取當前俯仰、偏航和翻滾的角度信息
  float pitch = msg->pitch;
  float yaw = msg->yaw;
  float roll = msg->roll;

  // 在這裡進行PID等控制方法的實現
  // ...
}

int main(int argc, char** argv){
  ros::init(argc, argv, "gimballock_demo");
  ros::NodeHandle nh;

  // 訂閱飛行器姿態信息
  ros::Subscriber sub_attitude = nh.subscribe(
    "/dji_sdk/attitude", 10, AttitudeCallback);

  // 主循環
  ros::spin();

  return 0;
}

在上面的示例代碼中，我們使用了dji_sdk中的Attitude消息類型，它可以從飛行器實時獲取到俯仰、偏航和翻滾的角度信息。通過這些信息，我們可以進行PID等控制方法的實現，從而完成對飛行器的精準控制。

五、總結

gimballock是飛行器姿態控制中常遇到的難題，但我們可以通過使用四元數進行旋轉，以避免出現奇異點，實現更加精準的飛行器控制。通過使用大疆的飛行控制組件和SDK，在實現過程中可以方便地獲取飛行器的實時姿態信息，並進行PID等控制方法的實現。