用於精準定位的 IMU：第 2 篇 – 如何使用 IMU 軟體達到更高的精準度

許多嵌入式系統需要追蹤其本身所在地點，或是定位與航向，設計人員往往以全球導航衛星系統 (GNSS) 做為優先解決方案 (請參閱《使用 GNSS 模組快速設計位置追蹤系統》)。但基於一些原因，不可永遠只用 GPS 來取得位置和航向資訊，首要的原因便是耗電量。

GPS 系統會消耗大量的電力。時時刻刻維持 GPS 定位需要很高的功率預算，許多嵌入式系統無法負擔，特別是電池供電式系統。另外，GPS 也會出現訊號受阻與更新速率過慢的情形，對某些應用來說，準確度只達 1 公尺可能不夠。

使用慣性量測單元 (IMU) 來補強 GNSS 定位資料，便能解決上述問題。

前一篇關於 IMU 的文章討論過使用價格相對低廉的 MEM 技術架構 IMU 來判定位置和航向，請參閱《在 GPS 不足以提供精準的位置資料時使用 IMU》。不過，單只有 IMU 感測器資料是無法提供絕對位置的。GNSS 接收器只會提供位置資料，為了將 IMU 的相對運動資料轉換成絕對位置與航向，則需要使用非常多種軟體，這其中會用到感測器濾波、融合以及雙重積分。

本文探討 IMU 的嵌入式應用、各種 IMU 感測器的資料誤差來源，以及需要使用哪些軟體來計算精確位置與來自 IMU 感測器的航向資訊。另外，本文還會探討 STMicroelectronics 的 MotionFX 程式庫，以及如何使用此軟體將 IMU 的感測器資料轉換成嵌入式系統中的位置和航向。

IMU 如何讓 GNSS 接收器更完備

GNSS 的問題分為四個層面：

1. GNSS 的訊號極具方向性，很容易被建築物擋住。因此，GNSS 在室內或都市叢林內運作時可能會發生問題。
2. GNSS 接收器的暖啟動和冷啟動時間以數十秒或更長時間作為測量單位。如果 GNSS 接收器不是連續操作 (例如為了節省電力)，則需要好幾秒才可取得每個新位置的讀數。
3. GNSS 的位置更新速率限制為每秒一次。對於需要快速移動的嵌入式應用來說，這樣的更新速率不夠快。這種嵌入式應用相當多，從虛擬實境到材料處理以及機器人。
4. GNSS 準確度以公尺作為測量單位。此解析度太過粗糙，無法用於許多嵌入式應用中。

IMU 提供更精細的定位解析度以及更快的更新速率，符合許多嵌入式應用的需求。而且，IMU 會從某個已知的起點提供相對位置資料，與 GNSS 接收器提供絕對位置資訊不同，因此這兩種類型的位置感測器能夠相輔相成。這些特性使得 IMU 相當適合用來增強從 GNSS 接收器直接獲得的位置資訊；請參閱《使用 GNSS 模組快速設計位置追蹤系統》。

IMU 用於航太產業的歷史已有數十年之久。但是，這些精確的航太 IMU 是以昂貴的陀螺儀以及其他大型感測器作為基礎，因此難以應用到較注重成本的系統。

現代化的電子 IMU 經過高度整合，可同時含有許多種感測器，包括加速計、陀螺儀以及磁力計，這些產品以微機電系統 (MEMS) 技術作為基礎，因此輕盈小巧，也較耐用。這些新世代的 IMU 可作為板式安裝元件使用，而且已證實相當適合嵌入式應用。

就自由度 (DOF) 而言，IMU 具有許多不同的功能，而且與 GNSS 接收器不同，IMU 不需要仰賴無線電訊號來取得位置資料。此外，IMU 耗電量很低，並有眾多貨源，可提供多種解析度與準確度規格。

這些特性使得 IMU 能用來增強來自 GNSS 接收器模組的定位資訊 (請參閱《使用 GNSS 模組快速設計位置追蹤系統》)，但也必須使用非常多種軟體來過濾 IMU 感測器資料、將 IMU 內部數種感測器的資料相互融合，然後透過雙重積分，根據融合的感測器資料來計算位置。

IMU 誤差來源

結合感測器來提升準確度已經是航太產業長期以來普遍的作法，並且在慣性導航系統中廣泛運用加速計與陀螺儀。在這些系統中，陀螺儀雖然會透過角速度的變化來感測方位，但通常會隨著時間而漂移，這是因為陀螺儀只能感測變化，本身沒有固定的參考座標系。如果將加速計資料加入陀螺儀資料中，就可將陀螺儀的偏誤降到最低，讓位置預估值變得更準確。加速計可感測相對於重力的方向變化，而此資料可用於確定陀螺儀的方位。

加速計進行靜態計算時的準確度，比進行動態計算時更高。在系統已經移動的情況下，陀螺儀可達到更好的方位偵測效果。加速計的反應速度很快，因此若單獨使用該資料，加速計的抖動與雜訊會產生累積性誤差。另外礙於引力等外力，加速計往往會讓加速度失真，而這也會在系統中累積成為雜訊。

如果將陀螺儀的短期準確度與加速計的長期準確度相互結合，截長補短，便能產生出更精確的方位讀數。這兩種類型的感測器相輔相成。

需使用融合濾波技術減少誤差

IMU 軟體使用濾波技術，將 IMU 資料的定位誤差降到最低。若要融合感測器的資料，有幾種濾波方法可以使用，但複雜度各不相同。互補濾波器能讓您輕易將幾個感測器的資料結合在一起。互補濾波是一種線性函數，結合了高通陀螺儀濾波器以及低通加速計濾波器。因此，加速計資料中的高頻雜訊會在短時間內濾除，並透過陀螺儀資料進行平滑處理。

雖然互補濾波器的運算方式很簡單，但現在業界較偏好在 IMU 的應用中結合卡爾曼濾波器等更深入的技術，以達到更高的準確度。卡爾曼濾波器是相當受歡迎的遞迴式感測器融合演算法，這種方法無需耗用太多處理能力，就能打造出更為準確的定位系統。卡爾曼濾波技術有幾種不同的類型，包括標準卡爾曼濾波器、延伸卡爾曼濾波器 (EKF) 以及無跡卡爾曼濾波器 (UKF)。

卡爾曼濾波器會基於以下假設，使用數種數學方程式來預測數值：待濾波的資料採用高斯分布形式，濾波器會對此形式套用線性方程式。但是，現實世界中的移動不像卡爾曼濾波器假設的一樣單純。現實世界中的移動，其遵循的方程式通常涉及非線性的正弦及餘弦函數，因此 EKF 採用泰勒級數以及高斯分布的單點逼近法，將非線性函數轉為線性。這些逼近做法可能會成為明顯的誤差來源。

UKF 解決此誤差問題的做法是，使用確定式採樣法產生一組經過仔細挑選的樣本點來代表資料的預期高斯分布，而不是 EKF 所使用的單點逼近法。這些樣本點會完整擷取預期高斯值的真實平均值與共變異數，從而實現更準確的濾波。

磁力計誤差來源

典型的 IMU 包含三種不同的感測器：測量角速率／速度的陀螺儀、測量線性加速度的加速計，以及測量磁場強度的磁力計。更具體而言，IMU 的磁力計會測量地球的磁場強度，來判定相對於地球磁北極的航向。專為導航應用所設計的 IMU 通常都含有這三種感測器，以涵蓋三個正交動作軸。

但是，磁力計容易發生硬鐵磁與軟磁扭曲。外部磁力影響所導致的地球磁場失真，一般分為硬磁或軟磁效應。如果沒有扭曲效應，將磁力計旋轉 360° 並以產生的資料進行繪製，結果會是一個以 (0, 0) 為中心的圓 (圖 1)。

圖 1：如果附近沒有磁性物質會造成局部磁場扭曲，則將磁力計旋轉 360° 並以產生的資料進行繪製，結果會是一個以位置 (0, 0) 為中心的圓。(圖片來源：sensorsmag.com)

若有硬磁與／或軟磁效應，可能會對圓造成干擾。硬磁效應會導致圓從位置 (0, 0) 偏移 (圖 2)。

圖 2：硬磁扭曲效應導致磁場資料從位置 (0, 0) 偏移。(圖片來源：sensorsmag.com)

軟磁效應會使圓變形為橢圓形 (圖 3)。

圖 3：軟磁扭曲效應導致 360° 磁場資料失真，使其從圓變成橢圓。(圖片來源：sensorsmag.com)

請注意，硬磁與軟磁效應可能會同時出現。

比起補償硬磁扭曲效應，補償軟磁扭曲所進行的計算需要使用更多的運算資源，因此將軟磁材質從 IMU 的磁力計處移開，可能會是更有效率且更具成本效益的方法。但這種做法在許多情況下行不通，因此需要進行補償計算。

積分後再次積分

最後，因為加速計和陀螺儀只會提供物體的線性及旋轉加速力的資訊，因此必須將此資料積分兩次來產生位置資料，因為加速度是物體的速度變化率，速度則是物體的位置變化率。也就是說，IMU 陀螺儀和加速計資料會提供物體絕對位置的二階導數。

雙重積分本身就需要進行大量的運算，但對 3D 定位來說，還必須去除重力的加速度影響。對加速計來說，重力就像是恆定加速度，會導致資料偏移。在 IMU 以及內含 IMU 的物體靜止時，通常必須對重力的加速度影響進行校正，將其排除於系統之外。

請注意，取樣誤差通常會導致雙重積分出現誤差。取樣率越慢，誤差越大。

IMU 軟體的設計考量

在現實世界中的 IMU 軟體實作這幾種演算法時，請考慮採取以下幾項步驟及建議：

• IMU 感測器訊號免不了會有雜訊，需要進行數位濾波。IMU 應用中一般都會使用某種卡爾曼濾波技術。
• 但即使進行濾波，還是會有一些額外的誤差來源，像是 IMU 帶來的機械雜訊，以及上述的硬磁與軟磁扭曲效應。此雜訊必須濾除，或從資料中移除。
• 應用開始時，必須在 IMU 靜止期間執行校正常式，這是為了盡可能取得準確的結果，軟體需要知道在物體不移動時的感測器讀數為何。移動中物體的真實加速度值，是以加速度感測器目前的取樣值減去校正值而得。
• 校正之後，感測器值可能為正，也可能為負。因此，儲存這些值的變數必須帶符號。
• 感測器資料的取樣頻率越高，就可產生更準確的結果，但較多的樣本數需要更多的記憶體、更嚴格的計時要求，並且可能還需要考量其他硬體因素。
• 採樣週期必須始終保持相同。一旦週期有所出入，便會導致資料出現誤差。
• 樣本之間的線性資料逼近 (內插) 可產生較準確的結果，但所需的處理時間也較長。

從上面的討論中可以明顯看出，要從 IMU 的資料中判定航向、位置及移動，所需的計算並不簡單。因此，IMU 的廠商通常會提供軟體應用程式套件，協助自家產品執行這些計算。例如，STMicroelectronics 就開發了 MotionFX 程式庫，為該公司開放下載的 X-CUBE-MEMS1 軟體提供功能擴充，以納入 IMU 的功能。

這個程式庫中的軟體會從加速計、陀螺儀以及磁力計取得資料，並為 STMicroelectronics 的 MEMS 感測器 (包含 IMU 在內) 提供即時的動作感測器資料融合。MotionFX 濾波及預測性軟體使用先進的演算法，可智慧整合多個 MEMS 感測器的輸出，而不受環境狀況所限制。這是專為 STMicroelectronics 的 MEMS IMU 設計的程式庫，因此如果使用其他廠商的 IMU，則無法確保軟體的功能與效能表現。

MotionFX 程式庫可在 STMicroelectronics 的多種處理器核心架構微控制器上運作，包括 Arm® Cortex®-M0+、Cortex-M3 以及 Cortex-M4 處理器核心。STMicroelectronics 的 X-NUCLEO-IKS01A2 擴充板 (圖 4) 安裝在以下開發板時，有一些範例實作可供使用：

• NUCLEO-F401RE 開發板 (以 STM32F4 MCU 為基礎、整合 Arm Cortex-M4 處理器核心)；
• NUCLEO-L476RG 開發板 (以 STM32L4 MCU 為基礎、整合 Arm Cortex-M4 處理器核心)；
• NUCLEO-L152RE 開發板 (以 STM32L1 MCU 為基礎、整合 Arm Cortex-M3 處理器核心)；
• NUCLEO-L073RZ 開發板 (以 STM32L0 MCU 為基礎、整合 Arm Cortex-M0+ 處理器核心)。

STMicroelectronics 的 MotionFX 程式庫會針對從 IMU 的加速計、陀螺儀以及磁力計所取得的資料進行管理與融合。此程式庫提供的常式可用於以下層面：

• 即時的 9 軸動作感測器資料融合 (加速計、陀螺儀、磁力計)
• 即時的 6 軸動作感測器資料融合 (加速計、陀螺儀)
• 旋轉、四元數、重力以及線性加速資料的運算
• 陀螺儀偏誤校正
• 磁力計硬磁校正

STMicroelectronics 建議在使用 MotionFX 軟體時，使用 100 Hz 的感測器資料取樣頻率。

圖 4：STMicroelectronics 的 X-NUCLEO-IKS01A2 擴充板，由該公司的 IMU 專用 MotionFX 軟體提供支援。(圖片來源：STMicroelectronics)

MotionFX 程式庫會實作感測器融合演算法，以估算在空間中的 3D 方位。此程式庫會過濾並融合來自多個感測器的資料，對 IMU 個別感測器的限制進行補償。

清單 1 中的範例程式碼顯示要讓 STMicroelectronics 的 IMU 開始運作所需具備的元素。

複製
[…]
#define VERSION_STR_LENG 35
#define MFX_DELTATIME 10
[…]
/*** Initialization ***/
char lib_version[VERSION_STR_LENG];
char acc_orientation[3];
MFX_knobs_t iKnobs;
/* Sensor Fusion API initialization function */
MotionFX_initialize();
/* Optional: Get version */
MotionFX_GetLibVersion(lib_version);
MotionFX_getKnobs(&iKnobs);
/* Modify knobs settings */
MotionFX_setKnobs(&iKnobs);
/* Enable 9-axis sensor fusion */
MotionFX_enable_9X(MFX_ENGINE_ENABLE);
[…]
/*** Using Sensor Fusion algorithm ***/
Timer_OR_DataRate_Interrupt_Handler()
{
MFX_input_t data_in;
MFX_output_t data_out;
/* Get acceleration X/Y/Z in g */
MEMS_Read_AccValue(data_in.acc[0], data_in.acc[1], data_in.acc[2]);
/* Get angular rate X/Y/Z in dps */
MEMS_Read_GyroValue(data_in.gyro[0], data_in.gyro[1], data_in.gyro[2]);
/* Get magnetic field X/Y/Z in uT/50 */
MEMS_Read_MagValue(data_in.mag[0], data_in.mag[1], &data_in.mag[2]);
/* Run Sensor Fusion algorithm */
MotionFX_propagate(&data_out, &data_in, MFX_DELTATIME);
MotionFX_update(&data_out, &data_in, MFX_DELTATIME, NULL);
}

清單 1：範例程式碼展示如何使用 MotionFX 程式庫，從該公司 X-NUCLEO-IKS01A2 擴充板上的 IMU 元件擷取資訊。(程式碼來源：STMicroelectronics)

在此亦提供範例軟體的流程圖 (圖 5)。

圖 5：此流程圖說明清單 1 中的 MotionFX 範例程式碼所用的步驟順序，以便從 STMicroelectronics 的 X-NUCLEO-IKS01A2 擴充板中擷取資料，並將其融合為 3D 空間方位。(圖片來源：STMicroelectronics)

請注意，陀螺儀的資料可能會漂移，導致 3D 空間方位的估算品質下降。MotionFX 軟體可以使用磁力計資料帶來的絕對方位資訊來進行補償。同時，磁力計不會在高速率下產生航向資料，也會受到磁干擾的影響，但陀螺儀資料會對磁力計的這些缺陷進行補償。

MotionFX 軟體的 9 軸感測器融合計算使用來自加速計、陀螺儀以及磁力計的資料，以便提供 3D 空間中的絕對方位，包括相對於磁北極的航向。

MotionFX 軟體的 6 軸感測器融合運算僅使用加速計和陀螺儀的資料。此運作等級對運算能力的需求較低，而且不會提供關於絕對方位的資訊，但常式的執作速度比 9 軸常式快。6 軸感測器融合技術適合測量快速移動，像是電玩或固定式機器人，也可用於不需要絕對方位的應用。

結論

IMU 在嵌入式應用中相當實用，本身就能提供位置和航向資訊。若需要更高的精準度或更快的更新速率時，利用 IMU 來增強 GNSS 接收器的資訊也很有幫助。但若要將 IMU 整合到嵌入式系統中，則需要用到大量的軟體。IMU 供應商通常會提供這些軟體，協助在真實世界中應用這些元件。