無人機開發之三：飛行器入門理論知識

3. 飛行器入門理論知識

3.1. 名詞解釋：

慣性測量單元 IMU (InertialMeasurementUnit)
姿態航向參考系統 AHRS (Attitudeand Heading Reference System)
地磁角速度重力 MARG (Magnetic,Angular Rate, and Gravity)
微機電系統 MEMS (MicroElectrical Mechanical Systems)
自由度維數 DOF (Dimension OfFreedom)
無人駕駛飛行器 UAV (UnmannedAerial Vehicle)
互補濾波算法 ECF （explicit complement filter）
擴展卡爾曼濾波 EKF (Extended Kalman Filter)
無損卡爾曼濾波 UKF (Unscented Kalman Filter)
梯度下降算法 GD （gradient descent）
慣性導航系統 INS (InertialNavigation System)
全球導航衛星系統 GNSS (GlobalNavigation Satellite System)
天文導航系統 CNS (CelestialNavigation System)
可垂直起降 VTOL (VerticalTake-off and Landing)

3.2. 坐標系介紹：

“地理”坐標系（Earth Frame）和“載體”坐標系(Body Frame)。”地理”坐標系指的就是地球上的“東北天（ENU）”坐標系，而“載體”坐標系值的就是四軸自己的坐標系。當我們在實際控制當中，我們關心的顯然是載體坐標系相對於地理坐標系之間的變化，所以我們通常使用的旋轉矩陣是把“地理”坐標系轉到“載體”坐標系的矩陣，兩者之間的轉化的方法就是坐標系的轉換，目前有三種方式：四元數、歐拉角、方向余弦矩陣。其中使用四元數運算比較快，但是它沒有實際的物理含義，純數學推導。

3.2.1. 姿態數據：

姿態的數據來源有5個：重力、地磁、陀螺儀、加速度計、電子羅盤。其中前兩個來自“地理”坐標系，后三個來自“載體”坐標系。在“地理”坐標系中，重力的值始終是（0，0，1g），地磁的值始終是（0，1，x），這里y指向正北方。這些值就是由放置在四軸上的傳感器測量出來的。在單位時間內的位移被定義為速度，速度有線速度和角速度之分，分別對應兩種傳感器測量這兩種不同的速度：線速度傳感器（加速度計）、角速度傳感器（陀螺儀）。

3.3. 傳感器介紹：

3.3.1. 陀螺儀：

• 陀螺儀，是一種用來感測與維持方向的裝置，基於角動量不滅的理論設計出來的。陀螺儀主要是由一個位於軸心可以旋轉的輪子構成。 陀螺儀一旦開始旋轉，由於輪子的角動量，陀螺儀有抗拒方向改變的趨向。萬萬想不到的是雞頭就是一個非常完美的陀螺儀，無論你怎樣動雞的身體，它總是能hold住自己的頭，太強大了!

• ![](https://img-blog.csdn.net/20180521102456246)
  

  以MPU6050為例，MPU6050芯片的座標系是這樣定義的：令芯片表面朝向自己，將其表面文字轉至正確角度，此時，以芯片內部中心為原點，水平向右的為X軸，豎直向上的為Y軸，指向自己的為Z軸。見下圖：
  
  繞X、Y和Z三個座標軸旋轉的角速度分量GYR_X、GYR_Y和GYR_Z均為16位有符號整數。從原點向旋轉軸方向看去，取正值時為順時針旋轉，取負值時為逆時針旋轉。三個角速度分量均以“度/秒”為單位，能夠表示的角速度范圍，即倍率可統一設定，有4個可選倍率：250度/秒、500度/秒、1000度/秒、2000度/秒。以GYR_X為例，若倍率設定為250度/秒，則意味着GYR取正最大值32768時，當前角速度為順時針250度/秒；若設定為500度/秒，取32768時表示當前角速度為順時針500度/秒。顯然，倍率越低精度越好，倍率越高表示的范圍越大。我們用f表示倍率，f=0為250度/秒，f=3為2000度/秒。
  以GYR_X為例，若當前設定的角速度倍率為1000度/秒，那么將GRY_X讀數換算為角速度（順時針）的公式為：g_x=1000 x GYR_X / 32768。

3.3.2. 加速度計：

• 當我們在想象一個加速度計的時候我們可以把它想作一個圓球在一個方盒子中。假定這個盒子不在重力場中或者其他任何會影響球的位置的場中，球處於盒子的正中央。你可以想象盒子在外
• 太空中，遠離任何天體，如果很難想象，那就當做盒子在航天飛機中，一切東西都處於無重力狀態。在下面的圖中你可以看到我們給每個軸分配了一對牆（我們移除了Y+以此來觀察里面的情況）。設想每面牆都能感測壓力。
  

* 如果我們突然把盒子向左移動（加速度為1g=9.8m/s^2），那么球會撞上X-牆。然后我們檢測球撞擊牆面產生的壓力，X軸輸出值為-1g。

* 請注意加速度計檢測到得力的方向與它本身加速度的方向是相反的。這種力量通常被稱為慣性力或假想力 。在這個模型中你你應該學到加速度計是通過間接測量力對一個牆面的作用來加速度的，在實際應用中，可能通過彈簧等裝置來測量力。這個力可以是加速度引起的，但在下面的例子中，我們會發現它不一定是加速度引起的。如果我們把模型放在地球上，球會落在Z-牆面上並對其施加一個1g的力，見下圖：
* 在這種情況下盒子沒有移動但我們任然讀取到Z軸有-1g的值。球在牆壁上施加的壓力是由引力造成的。三軸加速度計的真正價值在於它們能夠檢測全部三個軸的慣性力。讓我們回到盒子模型，並將盒子向右旋轉45度。現在球會與兩個面接觸：Z-和X-，見下圖：
* 在上一個模型中我們引入了重力並旋轉了盒子。在最后的兩個例子中我們分析了盒子在兩種情況下的輸出值，力矢量保持不變。雖然這有助於理解加速度計是怎么和外部力相互作用的，但如果我們將坐標系換為加速度的三個軸並想象矢量力在周圍旋轉，這會更方便計算。

* 新模型中每個軸都分別垂直於原模型中各自的牆面。矢量R是加速度計所檢測的矢量（它可能是重力或上面例子中慣性力的合成）。Rx，Ry，Rx是矢量R在X，Y，Z上的投影。請注意下列關系：
R^2 = Rx^2 + Ry^2 + Rx^2 （三維空間勾股定理）
* 我們感興趣的角度是向量R和X，Y，Z軸之間的夾角，那就令這些角度為Axr，Ayr，Azr。觀察由R和Rx組成的直角三角形：
cos(Axr) = Rx / R , cos(Ayr) = Ry / R， cos(Azr) = Rz / R
從公式1我們可以推導出 R = Sqrt( Rx^2 + Ry^2 + Rz^2)，通過arccos()函數我們可以計算出所需的角度：
Axr = arccos(Rx/R)，Ayr = arccos(Ry/R)，Azr = arccos(Rz/R)

注：加速度計若是繞着重力加速度的軸轉動，則測量值不會改變，也就是說加速度計無法感知這種水平旋轉。

3.3.3. 地磁計：

• 如圖所示，地球的磁場象一個條形磁體一樣由磁南極指向磁北極。在磁極點處磁場和當地的水平面垂直，在赤道磁場和當地的水平面平行，所以在北半球磁場方向傾斜指向地面。用來衡量磁感應強度大小的單位是Tesla或者Gauss（1Tesla=10000Gauss）。隨着地理位置的不同，通常地磁場的強度是0.4-0.6 Gauss。需要注意的是，磁北極和地理上的北極並不重合，通常他們之間有11度左右的夾角。
  
  地磁場是一個矢量，對於一個固定的地點來說，這個矢量可以被分解為兩個與當地水平面平行的分量mx、my和一個與當地水平面垂直的分量mz，這里的mx指向正北方。如果保持電子羅盤和當地的水平面平行，那么羅盤中磁力計的三個軸就和這三個分量對應起來，如圖所示：
  
  實際上對水平方向的兩個分量來說，他們的矢量和總是指向磁北的。羅盤中的偏航角（Yaw）就是當前方向和磁北的夾角。由於羅盤保持水平，只需要用磁力計水平方向兩軸（通常為X軸和Y軸）的檢測數據就可以用式1計算出航向角。當羅盤水平旋轉的時候，航向角在0?- 360?之間變化。同加速度計一樣，我們也可以根據地磁計輸出的三個分量，求出地磁T與三個軸的夾角。

• 注：同加速度計類似的，若是沿着磁場方向的軸旋轉，測量值不會改變，地磁計無法感知這種旋轉。

3.3.4. GPS：

• 空間星座部分的各顆GPS衛星向地面發射信號，然后用戶設備通過接收、測量各顆可見衛星信號，並從信號中獲取衛星的運行軌道信息，進而確定用戶接收機自身的空間位置。空間星座部分與用戶設備的這種聯系是單向的，信號、信息只從空間星座部分向用戶設備部分傳遞。衛星信號從20200Km的高空被播發后，大約需要70ms的時間就到達地球表面。衛星信號中包含着信號發射時間的精確信息，這是用戶設備用來測量其本身到衛星距離的一個必要條件。
  空間星座部分為GPS提供了定位基礎，並且可以支持無數個GPS用戶。然而，它們不會替用戶自己定位，用戶只有通過GPS用戶設備才能實現定位。用戶設備可以簡單地理解為GPS接收機，它主要由接收機硬件、數據處理軟件、微處理器和終端設備組成。用戶設備的主要任務是跟蹤可見GPS衛星，對接收到的衛星無線電信號經過數據處理后獲得定位所需的測量值和導航信息，最后完成對用戶的定位運算和可能的導航任務。
  通過天線接收所有可見GPS衛星的信號后，接收機對這些信號進行數據處理而精確地測量出各個衛星信號的發射時間，接着將其自備時鍾所顯示的信號接收時間與測量所得的信號發射時間相減后再乘以光速，由此得到接收機與衛星之間的距離。同時，接收機還從衛星信號中解譯處衛星的運行軌道參數，並以此准確地計算出衛星的空間位置。如果衛星1、2、3的空間位置在某一直角坐標系中的坐標為：，，。而接收機測得其本身到該衛星的距離為：，那么我們根據高中數學知識可以列出以下方程：
  
  其中，未知數(x,y,z)正是我們想要的用戶接收機位置。從方程組中解出三個未知數(x,y,z)，便得到了用戶自身的三維坐標。因為接收機時鍾通常與衛星時鍾不同步，所以接收機需要有4顆衛星的測量值，然后4個方程一並求解出x,y,z和接收機鍾差(t)這4個未知數，這便是GPS定位、定時的基本原理。
  GPS數據遵循NMEA-0183協議，該數據標准是由NMEA（National Marine Electronics Association，美國國家海事電子協會）於1983年制定的。統一標准格式NMEA-0183輸出采用ASCII 碼，其串行通信的參數為：波特率＝4800bps，數據位＝8bit，開始位=1bit，停止位＝1bit，無奇偶校驗。數據傳輸以“語句”的方式進行，每個語句均以“\(”開頭，然后是兩個字母的“識別符”和三個字母的“語句名”，接着就是以逗號分割的數據體，語句末尾為校驗和，整條語句以回車換行符結束。 NMEA-0183的數據信息有十幾種，這些信息的作用分別是：\)GPGGA：輸出GPS的定位信息；\(GPGLL：輸出大地坐標信息；\)GPZDA：輸出UTC時間信息；\(GPGSV：輸出可見的衛星信息；\)GPGST：輸出定位標准差信息；\(GPGSA：輸出衛星DOP值信息；\)GPALM：輸出衛星星歷信息；$GPRMC：輸出GPS推薦的最短數據信息等。常用的協議類型解釋如下：

• 1）Global Positioning System Fix Data（GGA）GPS 定位信息
  $GPGGA,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,M,<10>,M,<11>,<12>*hh
  <1> UTC 時間，hhmmss（時分秒）格式
  <2> 緯度ddmm.mmmm（度分）格式（前面的0 也將被傳輸）
  <3> 緯度半球N（北半球）或S（南半球）
  <4> 經度dddmm.mmmm（度分）格式（前面的0 也將被傳輸）
  <5> 經度半球E（東經）或W（西經）
  <6> GPS 狀態：0=未定位，1=非差分定位，2=差分定位，6=正在估算
  <7> 正在使用解算位置的衛星數量（00~12）（前面的0 也將被傳輸）
  <8> HDOP 水平精度因子（0.5~99.9）
  <9> 海拔高度（-9999.9~99999.9）
  <10> 地球橢球面相對大地水准面的高度
  <11> 差分時間（從最近一次接收到差分信號開始的秒數，如果不是差分定位將為空
  <12> 差分站ID 號0000~1023（前面的0 也將被傳輸，如果不是差分定位將為空）

• 2）GPS DOP and Active Satellites（GSA）當前衛星信息
  $GPGSA,<1>,<2>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<4>,<5>,<6>*hh
  <1> 模式，M=手動，A=自動
  <2> 定位類型，1=沒有定位，2=2D 定位，3=3D 定位
  <3> PRN 碼（偽隨機噪聲碼），正在用於解算位置的衛星號（01~32，前面的0 也將被傳輸）。
  <4> PDOP 位置精度因子（0.5~99.9）
  <5> HDOP 水平精度因子（0.5~99.9）
  <6> VDOP 垂直精度因子（0.5~99.9）

• 3）GPS Satellites in View（GSV）可見衛星信息
  $GPGSV,<1>,<2>,<3>,<4>,<5>,<6>,<7>,…<4>,<5>,<6>,<7>*hh
  <1> GSV 語句的總數
  <2> 本句GSV 的編號
  <3> 可見衛星的總數（00~12，前面的0 也將被傳輸）
  <4> PRN 碼（偽隨機噪聲碼）（01~32，前面的0 也將被傳輸）
  <5> 衛星仰角（00~90 度，前面的0 也將被傳輸）
  <6> 衛星方位角（000~359 度，前面的0 也將被傳輸）
  <7> 信噪比（00~99dB，沒有跟蹤到衛星時為空，前面的0 也將被傳輸）
  注：<4>,<5>,<6>,<7>信息將按照每顆衛星進行循環顯示，每條GSV 語句最多可以顯示4 顆衛星的信息。其他衛星信息將在下一序列的NMEA0183 語句中輸出。

• 4）Recommended Minimum Specific GPS/TRANSIT Data（RMC）推薦定位信息
  $GPRMC,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>*hh
  <1> UTC 時間，hhmmss（時分秒）格式
  <2> 定位狀態，A=有效定位，V=無效定位
  <3> 緯度ddmm.mmmm（度分）格式（前面的0 也將被傳輸）
  <4> 緯度半球N（北半球）或S（南半球）
  <5> 經度dddmm.mmmm（度分）格式（前面的0 也將被傳輸）
  <6> 經度半球E（東經）或W（西經）
  <7> 地面速率（000.0~999.9 節，前面的0 也將被傳輸）
  <8> 地面航向（000.0~359.9 度，以真北為參考基准，前面的0 也將被傳輸）
  <9> UTC 日期，ddmmyy（日月年）格式
  <10> 磁偏角（000.0~180.0 度，前面的0 也將被傳輸）
  <11> 磁偏角方向，E（東）或W（西）
  <12> 模式指示（僅NMEA0183 3.00 版本輸出，A=自主定位，D=差分，E=估算，N=數據無效）

• 5）Track Made Good and Ground Speed（VTG）地面速度信息
  $GPVTG,<1>,T,<2>,M,<3>,N,<4>,K,<5>*hh
  <1> 以真北為參考基准的地面航向（000~359 度，前面的0 也將被傳輸）
  <2> 以磁北為參考基准的地面航向（000~359 度，前面的0 也將被傳輸）
  <3> 地面速率（000.0~999.9 節，前面的0 也將被傳輸）
  <4> 地面速率（0000.0~1851.8 公里/小時，前面的0 也將被傳輸）
  <5> 模式指示（僅NMEA0183 3.00 版本輸出，A=自主定位，D=差分，E=估算，N=數據無效）

• 6）Geographic Position（GLL）定位地理信息
  $GPGLL,<1>,<2>,<3>,<4>,<5>,<6>,<7>*hh
  <1> 緯度ddmm.mmmm（度分）格式（前面的0 也將被傳輸）
  <2> 緯度半球N（北半球）或S（南半球）
  <3> 經度dddmm.mmmm（度分）格式（前面的0 也將被傳輸）
  <4> 經度半球E（東經）或W（西經）
  <5> UTC 時間，hhmmss（時分秒）格式
  <6> 定位狀態，A=有效定位，V=無效定位
  <7> 模式指示（僅NMEA0183 3.00 版本輸出，A=自主定位，D=差分，E=估算，N=數
  據無效）

3.3.5. 數據融合：

• （1）為什么還需要加速度計和地磁計呢？
  對於上述論述可以看出，導航姿態從理論上講只用陀螺儀是可以完成任務的，對陀螺儀輸出的三個角速度進行積分就能得到飛行器的姿態數據。那為什么還需要加速度計和地磁計呢？主要是由於陀螺儀在積分過程中會產生誤差累計，加上白噪聲、溫度偏差等會造成導航姿態的解算隨着時間的流逝而逐漸增加。 所以就需要用加速度計在水平面對重力進行比對和補償，用來修正陀螺儀的誤差。但是對於豎直軸上的旋轉，加速度計是無能為力的，此時用的是電子羅盤。也可以測量出水平面內的地磁方向用來修正陀螺儀的水平誤差。通過這兩個器件的修正補償，使得陀螺儀更加穩定、可靠的工作。
• （2）為什么還需要陀螺儀呢？
  加速度計在地球上測量的是重力加速度，如果載體沿着Z軸旋轉，加速度計是無法感知他的運動的；類似的，電子羅盤測量的是地球上的磁場方向，如果載體沿着Y軸旋轉，電子羅盤同樣也是無法感知他的運動的。綜上所述，加速度計和電子羅盤只能得到2維的角度關系，通過某種方式的融合，可以得到正確的三維姿態信息。到目前為止，你可能會問自己，如果加速度計和磁力計在任何時間點能給出DCM矩陣，為什么我們需要陀螺儀？ 陀螺儀實際上是一個比加速度計和磁力計更精確的設備，如果直接從加速度計和磁力計估計讀數，其在形式上包含大量的噪聲，包括外部（非重力）慣性力（即加速度）或不是由地球磁場引起的磁力。
• （3）三個傳感器融合
  綜合考慮，加速度計和磁傳感器都是極易受外部干擾的傳感器，都只能得到2維的角度關系，但是測量值隨時間的變化相對較小，結合加速度計和磁傳感器可以得到3維的角度關系。陀螺儀可以積分得到三維的角度關系，動態性能好，受外部干擾小，但測量值隨時間變化比較大。可以看出，它們優缺點互補，結合起來才能有好的效果。
  現在有了三個傳感器，都能在一定程度上測量角度關系，但是究竟相信誰？根據剛才的分析，應該是在短時間內更加相信陀螺儀，隔三差五的問問加速度計和磁傳感器，角度飄了多少了？有一點必須非常明確，陀螺儀才是主角，加速度計和磁傳感器僅僅是跑龍套的。其實加速度計無法對航向角進行修正，修正航向角需要磁力計。 數據融合原理參考這一節。
• （4）導航
  導航的基本原則就是保證兩個基本坐標系的正確轉化，沒有誤差。只有實現了這個原則，載體才可以在自己的坐標系中完成一系列動作而被轉換到地理坐標系中看起來是正確的。為了達到這個目標，需要對兩個坐標系進行實時的標定和修正。因為坐標系有三個軸，偏航yaw的修正由電子羅盤（基於載體）、地磁（基於地理）對比修正誤差補償得到。俯仰pitch和橫滾roll上的修正由加速度計（基於載體）、重力（基於地理）對比修正誤差得到。在完成了基本原則的基礎之后，即保證兩個坐標系的正確轉化后，利用基於載體上的陀螺儀進行積分運算，得到基於載體坐標系的姿態數據，經過一系列PID控制，給出控制量，完成基於載體坐標系上的穩定控制后，反應到地理坐標系上的穩定控制，從而達到我們觀察到的定高、偏航、翻滾、傾仰等動作。
• 注：AHRS和IMU的差異：
  AHRS（Attitudeand Heading Reference System）為姿態航向參考系統，它由加速度計、磁場計、陀螺儀構成，AHRS的真正參考來自於地球的重力場和地球的磁場，它的靜態精度取決於對磁場的測量精度和對重力的測量精度，而陀螺儀決定了他的動態性能。在這種前提下，說明AHRS離開了地球這種有重力和磁場環境的時候是沒法正常工作的。而且特別注意，磁場和重力場越正交，航姿測量效果越好；也就是說如果磁場和重力場平行了，比如在地磁南北極。這里的磁場是向下的，即和重量場方向相同了。這個時候航線交是沒法測出的，這是航姿系統的缺陷所在；在高緯度的地方航線角誤差會越來越大。
  IMU（Inertial measurement unit）學名慣性測量單元，大學的理論力學告訴我們，所有的運動都可以分解為一個直線運動和一個旋轉運動，故這個慣性測量單元就是測量這兩種運動，直線運動通過加速度計可以測量，旋轉運動則通過陀螺儀。假設IMU的陀螺儀和加速度計的測量是沒有任何誤差的，那么通過陀螺儀則可以精確的測量物體的姿態。通過加速度計可以二次積分得出位移，實現完整的6DOF（自由度維數），也就是說你帶着一台這種理論型的IMU在宇宙任何位置運動。我們都可以知道它當前的姿態和相對位移，這將不局限於任何場。
  　　從上面的描述何以看出。實際上AHRS比IMU還多一個磁場傳感器，而為什么AHRS的級別卻低於IMU而需要依賴於重力場和磁場呢？這是由傳感器器件架構所決定的。AHRS的傳感器通常是成本低廉的mems傳感器。這種傳感器的陀螺儀和加速度計的噪聲相對來說很大。以平面陀螺儀為例：用ADI的陀螺儀進行積分一分鍾會漂移2度左右，這種前提下如果沒有磁場和重力場來修正三軸陀螺儀的話。那么基本上3分鍾以后物體的實際姿態和測量輸出姿態就完全變樣了，所以在這種低價陀螺儀和加速度計的架構下必須運用場向量來進行修正，而IMU實際上也是這樣的。因為我們知道沒有絕對精確的傳感器，只有相對精確的傳感器，IMU的陀螺儀用的是光纖陀螺或者機械陀螺，這種陀螺的成本很高。精度相對mems陀螺也很高，精度高不代表准確，IMU的姿態精度參數通常是一小時飄多少度。
  　　而用加速度計積分做位置的話。AHRS是不現實的（1分鍾就能飄出幾十米，而且是成二次方的速度遞增）。AHRS通常要結合GPS和氣壓計做位置，IMU積分做位置的是一天多少海里。這樣的一個參數數量級。也許在海上還能用的到，這就是AHRS和IMU在我的理解里的一個差異。