用于精確定位的 IMU： 如何使用 IMU 軟件實(shí)現(xiàn)更高的精度

許多嵌入式系統(tǒng)需要跟蹤其所在的地點(diǎn)或位置和航向，設(shè)計(jì)人員經(jīng)常將 GNSS（全球?qū)Ш叫l(wèi)星系統(tǒng)）作為首選（參見(jiàn)“使用 GNSS 模塊快速設(shè)計(jì)位置跟蹤系統(tǒng)”）。然而，GPS 不能總是專門用于確定位置和航向信息，原因有多方面，首先是功耗。

GPS 系統(tǒng)需要消耗大量電力。許多嵌入式系統(tǒng)，尤其是電池供電的系統(tǒng)，無(wú)法負(fù)擔(dān) 100% 時(shí)間都維持 GPS 定位所需的功耗預(yù)算。此外，GPS 信號(hào)會(huì)受到阻擋，更新率較慢，且精度為 1 米 (m)，這對(duì)某些應(yīng)用而言可能不夠。

使用慣性測(cè)量裝置 (IMU) 對(duì) GNSS 定位數(shù)據(jù)進(jìn)行補(bǔ)充，則可以克服這些問(wèn)題。

前一篇 IMU 文章討論了如何使用基于 MEMS 技術(shù)的相對(duì)便宜的 IMU 來(lái)確定位置和航向，請(qǐng)參閱“在 GPS 不足以提供精準(zhǔn)的位置數(shù)據(jù)時(shí)使用 IMU”。但是，僅憑 IMU 傳感器數(shù)據(jù)還無(wú)法提供絕對(duì)位置。GNSS 接收器僅提供位置數(shù)據(jù)，但為了將 IMU 的相對(duì)運(yùn)動(dòng)數(shù)據(jù)轉(zhuǎn)換為絕對(duì)位置和航向，則需要大量軟件，其中會(huì)用到傳感器濾波、融合和二重積分。

本文討論 IMU 的嵌入式應(yīng)用、各種 IMU 傳感器所提供數(shù)據(jù)的誤差來(lái)源，以及計(jì)算來(lái)自 IMU 傳感器的精準(zhǔn)定位和航向信息所需的軟件。此外，本文還會(huì)討論STMicroelectronicsMotionFX 庫(kù)，以及如何使用該軟件將 IMU 傳感器數(shù)據(jù)轉(zhuǎn)換為嵌入式系統(tǒng)中的定位和航向。

IMU 如何補(bǔ)充 GNSS 接收器

GNSS 存在四個(gè)問(wèn)題：

GNSS 信號(hào)具有很強(qiáng)的指向性，很容易被建筑物遮擋。因此，室內(nèi)或城市混凝土峽谷深處的 GNSS 操作可能會(huì)有問(wèn)題。

GNSS 接收器有幾十秒甚至更長(zhǎng)時(shí)間的熱啟動(dòng)和冷啟動(dòng)時(shí)間。如果 GNSS 接收器不是連續(xù)工作（例如為了省電），則每個(gè)新位置讀數(shù)可能需要好幾秒才能獲得。

GNSS 的位置更新率被限制為每秒一次。對(duì)于涉及快速運(yùn)動(dòng)的嵌入式應(yīng)用，該更新率不夠快。這樣的應(yīng)用有很多，從虛擬現(xiàn)實(shí)到材料處理和機(jī)器人技術(shù)。

GNSS 精度為米級(jí)。該分辨率對(duì)于許多嵌入式應(yīng)用而言太過(guò)粗糙。

IMU 提供了許多嵌入式應(yīng)用所需的更精細(xì)的定位分辨率和更快的更新率。并且，和 GNSS 接收器提供絕對(duì)定位信息相反，IMU 提供距離已知起點(diǎn)的相對(duì)位置信息，因此這兩種位置傳感器可互為補(bǔ)充。IMU 的這些特點(diǎn)使其對(duì)于增強(qiáng)直接從 GNSS 接收器獲得的定位信息非常有用，請(qǐng)參閱“使用 GNSS 模塊快速設(shè)計(jì)位置跟蹤系統(tǒng)”。

IMU 在航空航天工業(yè)中的應(yīng)用已有數(shù)十年歷史。然而，航空航天用精密 IMU 是基于昂貴的陀螺儀和其他大型傳感器，難以應(yīng)用于對(duì)成本更敏感的系統(tǒng)。

現(xiàn)代電子 IMU 已高度集成化，可包含多種類型的傳感器——加速計(jì)、陀螺儀和磁力儀；其基于微機(jī)電系統(tǒng) (MEMS) 技術(shù)，因此體積小、重量輕且相對(duì)堅(jiān)固。這些新一代 IMU 可作為板安裝元件提供，經(jīng)證明非常適合嵌入式應(yīng)用。

IMU 可提供多種自由度 (DOF)，而且與 GNSS 接收器不同，IMU 不依賴無(wú)線電信號(hào)提供定位數(shù)據(jù)。IMU 耗電量也極低，來(lái)源眾多，并具有廣泛的分辨率和精度規(guī)格可供選擇。

借助這些特性，IMU 可用于增強(qiáng) GNSS 接收器模塊提供的定位信息（參見(jiàn)“使用 GNSS 模塊快速設(shè)計(jì)位置跟蹤系統(tǒng)”），但還需要大量軟件來(lái)過(guò)濾 IMU 傳感器數(shù)據(jù)，融合 IMU 內(nèi)部多種類型傳感器提供的數(shù)據(jù)，并利用二重積分根據(jù)融合的傳感器數(shù)據(jù)計(jì)算位置。

IMU 誤差源

多個(gè)傳感器結(jié)合以提高精度是航空航天工業(yè)長(zhǎng)期以來(lái)的常見(jiàn)做法，其中加速計(jì)和陀螺儀廣泛用于慣性導(dǎo)航系統(tǒng)。在此類系統(tǒng)中，陀螺儀通過(guò)角速度變化檢測(cè)方向，但隨著時(shí)間推移，陀螺儀往往會(huì)漂移，因?yàn)樗鼉H檢測(cè)變化而沒(méi)有固定的參考系。將加速計(jì)數(shù)據(jù)添加到陀螺儀數(shù)據(jù)中，可以最大限度地減小陀螺儀偏置，從而得到更準(zhǔn)確的位置估計(jì)。加速計(jì)檢測(cè)相對(duì)于重力的方向變化，該數(shù)據(jù)可用于給陀螺儀定向。

加速計(jì)對(duì)于靜態(tài)（與動(dòng)態(tài)相反）計(jì)算更準(zhǔn)確。當(dāng)系統(tǒng)已處于運(yùn)動(dòng)狀態(tài)時(shí)，利用陀螺儀檢測(cè)方向要更好。加速計(jì)反應(yīng)迅速，若只使用該數(shù)據(jù)，加速計(jì)抖動(dòng)和噪聲會(huì)產(chǎn)生累積誤差。此外，由于重力之類的外力，加速計(jì)往往會(huì)使加速度失真，這也會(huì)作為噪聲在系統(tǒng)中累積。

將陀螺儀的長(zhǎng)期精度與加速計(jì)的短期精度相結(jié)合，依靠每種傳感器的優(yōu)勢(shì)來(lái)抵消或至少減輕另一種傳感器的弱勢(shì)，可以獲得更精確的方向讀數(shù)。這兩類傳感器相互補(bǔ)充。

此時(shí)需要融合濾波來(lái)減少誤差

IMU 軟件使用濾波來(lái)最大限度地減小 IMU 數(shù)據(jù)的定位誤差。有多種濾波方法可融合傳感器數(shù)據(jù)，每種方法都有不同程度的復(fù)雜性?；パa(bǔ)濾波是合并多個(gè)傳感器所提供數(shù)據(jù)的簡(jiǎn)單方法?；パa(bǔ)濾波是一種將高通陀螺儀濾波和低通加速計(jì)濾波相結(jié)合的線性函數(shù)。因此，加速計(jì)數(shù)據(jù)中的高頻噪聲會(huì)在短期內(nèi)被濾除，并且被陀螺儀數(shù)據(jù)平滑。

雖然互補(bǔ)濾波在計(jì)算上很簡(jiǎn)單，但為了實(shí)現(xiàn)更好的精度，IMU 現(xiàn)在更喜歡使用卡爾曼濾波之類的更復(fù)雜技術(shù)?？柭鼮V波是一種非常受歡迎的遞歸傳感器融合算法，不需要大量處理能力就能構(gòu)建更精確的定位系統(tǒng)?？柭鼮V波有多種類型，包括標(biāo)準(zhǔn)卡爾曼濾波、擴(kuò)展卡爾曼濾波 (EKF) 和無(wú)跡卡爾曼濾波 (UKF)。

卡爾曼濾波使用若干數(shù)學(xué)方程來(lái)預(yù)測(cè)值，并基于以下假設(shè)：被濾波的數(shù)據(jù)為高斯分布形式，對(duì)其應(yīng)用線性方程進(jìn)行濾波。然而，現(xiàn)實(shí)世界中的運(yùn)動(dòng)并不像卡爾曼濾波的假設(shè)那樣中規(guī)中矩。實(shí)際運(yùn)動(dòng)遵循的方程常常涉及非線性的正弦和余弦函數(shù)，因此 EKF 采用泰勒級(jí)數(shù)和高斯分布的單點(diǎn)近似來(lái)線性化非線性函數(shù)。這些近似可能是重大的誤差源。

UKF 使用確定性采樣方法來(lái)解決此誤差問(wèn)題，生成一組精心選擇的采樣點(diǎn)來(lái)代表數(shù)據(jù)的預(yù)期高斯分布，而不是 EKF 所用的單點(diǎn)近似。這些采樣點(diǎn)完全捕獲了預(yù)期高斯值的真實(shí)均值和協(xié)方差，從而實(shí)現(xiàn)更精確的濾波。

磁力儀誤差源

典型 IMU 包含三種不同的傳感器：用于測(cè)量角速度/速度的陀螺儀、用于測(cè)量線性加速度的加速計(jì)以及用于測(cè)量磁場(chǎng)強(qiáng)度的磁力儀。更具體地說(shuō)，IMU 的磁力儀測(cè)量地球的磁場(chǎng)強(qiáng)度，以確定相對(duì)于地磁北極的航向。設(shè)計(jì)用于導(dǎo)航應(yīng)用的 IMU 的每類傳感器通常有三個(gè)，以對(duì)應(yīng)三個(gè)正交運(yùn)動(dòng)軸。

然而，磁力儀易受硬鐵和軟鐵失真的影響。外部磁場(chǎng)影響導(dǎo)致的地球磁場(chǎng)失真一般分為硬鐵效應(yīng)和軟鐵效應(yīng)兩種。如果不存在失真效應(yīng)，將磁力儀旋轉(zhuǎn) 360° 并將結(jié)果數(shù)據(jù)繪制成圖，得到的將是一個(gè)以 (0, 0) 為中心的圓（圖 1）。

圖 1：如果附近的鐵質(zhì)材料未引起局部磁場(chǎng)失真，則將磁力儀旋轉(zhuǎn) 360° 并將結(jié)果數(shù)據(jù)繪制成圖，得到的將是一個(gè)以位置 (0, 0) 為中心的圓。（圖片來(lái)源：sensorsmag.com）

硬鐵和/或軟鐵效應(yīng)的存在可能會(huì)影響該圓。硬鐵效應(yīng)導(dǎo)致其從位置 (0, 0) 發(fā)生簡(jiǎn)單偏移（圖 2）。

圖 2：硬鐵失真效應(yīng)導(dǎo)致磁力數(shù)據(jù)從位置 (0, 0) 發(fā)生偏移。（圖片來(lái)源：sensorsmag.com）

軟鐵效應(yīng)使圓變形，產(chǎn)生一個(gè)橢圓（圖 3）。

圖 3：軟鐵失真效應(yīng)會(huì)扭曲 360° 磁力數(shù)據(jù)，將其從圓形變?yōu)闄E圓形。（圖片來(lái)源：sensorsmag.com）

請(qǐng)注意，硬鐵效應(yīng)和軟鐵效應(yīng)可能同時(shí)出現(xiàn)。

補(bǔ)償軟鐵失真所需的計(jì)算量比補(bǔ)償硬鐵失真的計(jì)算量要大得多，因此，從 IMU 的磁力儀附近移走軟鐵材料可能是更有效、更經(jīng)濟(jì)的辦法。但在很多情況下，這不是一種選擇，而需要進(jìn)行補(bǔ)償計(jì)算。

積分，然后再次積分

最后，加速計(jì)和陀螺儀僅提供關(guān)于物體上的加速力（線性和旋轉(zhuǎn)）的信息，因此必須將該數(shù)據(jù)積分兩次才能確定位置，因?yàn)榧铀俣仁俏矬w速度的變化率，而速度是物體位置的變化率。換言之，IMU 陀螺儀和加速計(jì)數(shù)據(jù)提供的是物體絕對(duì)位置的二階導(dǎo)數(shù)。

二重積分本身就需要相當(dāng)多的處理，但對(duì)于三維定位，還必須消除重力的加速度影響。對(duì)于加速計(jì)，重力表現(xiàn)為恒定加速度，導(dǎo)致其數(shù)據(jù)發(fā)生偏移。通常，重力的加速度影響必須在 IMU 和包含 IMU 的物體處于靜止?fàn)顟B(tài)時(shí)，通過(guò)校準(zhǔn)從系統(tǒng)中消除。

請(qǐng)注意，由于采樣誤差，這種二重積分會(huì)引起誤差。采樣率越慢，誤差越大。

IMU 軟件的設(shè)計(jì)考慮因素

為實(shí)際的 IMU 軟件實(shí)現(xiàn)這些算法時(shí)，請(qǐng)考慮以下措施和建議：

IMU 傳感器信號(hào)有噪聲，需要進(jìn)行數(shù)字濾波。通常，IMU 應(yīng)用中會(huì)使用某種卡爾曼濾波。

即使進(jìn)行濾波，仍不免存在其他誤差源，例如耦合到 IMU 的機(jī)械噪聲，以及上面討論的硬鐵和軟鐵失真。必須濾除這種噪聲，或以其他方式從數(shù)據(jù)中消除此噪聲。

應(yīng)用開(kāi)始時(shí)，需要在 IMU 處于靜止?fàn)顟B(tài)時(shí)執(zhí)行校準(zhǔn)例程，原因是為了盡可能準(zhǔn)確，軟件需要知道當(dāng)物體不運(yùn)動(dòng)時(shí)傳感器讀數(shù)是多少。運(yùn)動(dòng)中物體的實(shí)際加速度值等于加速度傳感器的當(dāng)前采樣值減去校準(zhǔn)值。

校準(zhǔn)后，傳感器值可以是正數(shù)，也可以是負(fù)數(shù)。因此，保存這些值的變量必須有正負(fù)之分。

對(duì)傳感器數(shù)據(jù)的采樣頻率越高，產(chǎn)生的結(jié)果越準(zhǔn)確，但更多樣本將需要更多存儲(chǔ)器和更嚴(yán)格的定時(shí)要求，還可能涉及到其他硬件考慮因素。

采樣周期必須始終完全相同。采樣周期不相等會(huì)引起數(shù)據(jù)誤差。

樣本之間的線性數(shù)據(jù)擬合（插值）可產(chǎn)生更準(zhǔn)確的結(jié)果，但也需要更多的處理時(shí)間。

從上面的討論中可以明顯看出，根據(jù) IMU 數(shù)據(jù)確定航向、位置和運(yùn)動(dòng)所需的計(jì)算并不簡(jiǎn)單。因此，IMU 供應(yīng)商常常為其產(chǎn)品提供軟件應(yīng)用包來(lái)執(zhí)行這些計(jì)算。例如，STMicroelectronics 開(kāi)發(fā)了MotionFX 庫(kù)，此庫(kù)擴(kuò)展了該公司的可下載 X-CUBE-MEMS1 軟件的功能，以包含 IMU 功能。

該庫(kù)中的軟件從加速計(jì)、陀螺儀和磁力儀獲取數(shù)據(jù)，并為 STMicroelectronics 的 MEMS 傳感器（包括 IMU）提供實(shí)時(shí)運(yùn)動(dòng)傳感器數(shù)據(jù)融合。無(wú)論環(huán)境條件如何，MotionFX 濾波和預(yù)測(cè)軟件都使用高級(jí)算法智能集成來(lái)自多個(gè) MEMS 傳感器的輸出。該庫(kù)僅針對(duì) STMicroelectronics 的 MEMS IMU 設(shè)計(jì)，若使用其他供應(yīng)商的 IMU，無(wú)法保證該軟件的功能和性能。

MotionFX 庫(kù)運(yùn)行在 STMicroelectronics 各種基于 Arm? Cortex?-M0+、Cortex-M3 和 Cortex-M4 處理器內(nèi)核的微控制器上。當(dāng)安裝在以下開(kāi)發(fā)板上時(shí)，有針對(duì) STMicroelectronicsX-NUCLEO-IKS01A2擴(kuò)展板（圖 4）的實(shí)例實(shí)現(xiàn)供參考：

NUCLEO-F401RE開(kāi)發(fā)板，基于STM32F4MCU，內(nèi)置 Arm Cortex-M4 處理器內(nèi)核；

NUCLEO-L476RG開(kāi)發(fā)板，基于STM32L4MCU，內(nèi)置 Arm Cortex-M4 處理器內(nèi)核；

NUCLEO-L152RE開(kāi)發(fā)板，基于STM32L1MCU，內(nèi)置 Arm Cortex-M3 處理器內(nèi)核；

或NUCLEO-L073RZ開(kāi)發(fā)板，基于STM32L0MCU，內(nèi)置 Arm Cortex-M0+ 處理器內(nèi)核。

STMicroelectronics 的 MotionFX 庫(kù)管理并融合了來(lái)自 IMU 的加速計(jì)、陀螺儀和磁力儀的數(shù)據(jù)。該庫(kù)提供以下例程：

實(shí)時(shí)、9 軸、運(yùn)動(dòng)傳感器數(shù)據(jù)融合（加速計(jì)、陀螺儀、磁力儀）

實(shí)時(shí)、6 軸、運(yùn)動(dòng)傳感器數(shù)據(jù)融合（加速計(jì)、陀螺儀）

旋轉(zhuǎn)、四元數(shù)、重力和線性加速度數(shù)據(jù)的計(jì)算

陀螺儀偏置校準(zhǔn)

磁力儀硬鐵校準(zhǔn)

使用 MotionFX 軟件時(shí)，STMicroelectronics 建議采用 100 Hz 傳感器數(shù)據(jù)采樣頻率。

圖 4：STMicroelectronics 針對(duì) IMU 的 MotionFX 軟件支持其 X-NUCLEO-IKS01A2 擴(kuò)展板。（圖片來(lái)源：STMicroelectronics）

MotionFX 庫(kù)實(shí)現(xiàn)了一種傳感器融合算法來(lái)估計(jì)空間中的三維方向。它過(guò)濾并融合來(lái)自多個(gè)傳感器的數(shù)據(jù)，以彌補(bǔ) IMU 各個(gè)傳感器的局限性。

清單 1 提供的演示代碼顯示了啟動(dòng)和運(yùn)行 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 庫(kù)來(lái)從該公司的 X-NUCLEO-IKS01A2 擴(kuò)展板上的 IMU 元件提取信息。（代碼來(lái)源：STMicroelectronics）

該演示軟件的流程圖如下所示（圖 5）。

圖 5：此流程圖說(shuō)明了清單 1 中的 MotionFX 演示代碼所使用的步驟順序，以便從 STMicroelectronics 的 X-NUCLEO-IKS01A2 擴(kuò)展板提取數(shù)據(jù)，并將數(shù)據(jù)融合成三維空間方向。（圖片來(lái)源：STMicroelectronics）

請(qǐng)注意，陀螺儀數(shù)據(jù)可能會(huì)漂移，導(dǎo)致三維空間方向估計(jì)的精度降低。MotionFX 軟件可以使用磁力儀數(shù)據(jù)（提供絕對(duì)方向信息）來(lái)補(bǔ)償這種降級(jí)情況。同時(shí)，磁力儀不能以高速率產(chǎn)生航向數(shù)據(jù)，而且磁力儀會(huì)遭受磁干擾，但陀螺儀數(shù)據(jù)可以彌補(bǔ)磁力儀的這些弱點(diǎn)。

MotionFX 軟件的 9 軸傳感器融合計(jì)算使用來(lái)自加速計(jì)、陀螺儀和磁力儀的數(shù)據(jù)，提供三維空間中的絕對(duì)方向，包括航向（相對(duì)于地磁北極）。

MotionFX 軟件的 6 軸傳感器融合計(jì)算僅使用加速計(jì)和陀螺儀的數(shù)據(jù)。此操作級(jí)別對(duì)計(jì)算的要求較低，并且不提供關(guān)于絕對(duì)方向的信息，但該例程比 9 軸例程運(yùn)行得更快。6 軸傳感器融合適合測(cè)量快速運(yùn)動(dòng)，例如游戲或固定機(jī)器人，另外也可用在不需要絕對(duì)方向的場(chǎng)合。

總結(jié)

IMU 在嵌入式應(yīng)用中非常有用，可以自行提供位置和航向信息。IMU 也可用于增強(qiáng) GNSS 接收器信息，以提高定位精度或更新率。然而，將 IMU 集成到嵌入式系統(tǒng)中需要大量軟件，IMU 供應(yīng)商常常會(huì)提供相關(guān)軟件，以幫助實(shí)現(xiàn)這些器件的實(shí)際應(yīng)用。