隨著服務機器人市場和技術的發展,導航已成為研究和應用中的一個熱點。與車輛、船舶或飛機相比,服務機器人體積小,成本低,因此它們的導航系統應該具有捷聯(Strapdown)和低成本的特點。傳統的穩定平台導航系統通常要採用獨立的加速器和光纖或雷射陀螺儀,所有元件都機械性、且剛性地安裝在與正在移動車輛隔離的穩定平台上,而這導致了尺寸大、可靠性差、成本高的缺點。相反的,在捷聯導航系統中,慣性感測器直接固定在車輛車身上而意味著感測器與車輛一起旋轉,這種捷聯方法消除了穩定平台導航的缺點。

然而,平台導航的準確性通常高於捷聯慣性導航系統(SINS)。平台導航往往可以達到戰略級(0.0001°/時的陀螺儀偏置,1ΜG的加速器偏置)或軍用級(0.005°/時的陀螺儀偏置,30ΜG的加速器偏置),而多數SINS只能到達導航級(0.01°/時的陀螺儀偏置,50ΜG的加速器偏置)或戰略級(10°/時的陀螺儀偏置,1MG的加速器偏置)。對於大多數服務機器人或AGV導航應用,這一個精準度已經足夠了。

導航方法有許多,包括機器視覺、GPS、UWB、SLAM型光達(Lidar)等。慣性導航始終是導航的重要組成部分,其採用的是IMU。然而,由於這種感測器的限制——例如偏置誤差、橫軸誤差、雜訊,特別是偏置不穩定性——慣性導航通常需要採用一個夥伴感測器,定期為它提供參考或校準,本文將這種情況稱為感測器融合。許多感測器都可以與IMU融合,例如攝影機和里程表,但在這些感測器中,地磁感測器是一種低成本的方案,可與IMU配合獲得姿態資訊。

在本文中,使用ADI的IMU ADIS16470和地磁感測器來開發平台和演算法,實現SINS。但是,感測器只能提供姿態資訊。對於航位推算或距離測量,只能使用IMU中的加速度感測器。

IMU簡介

ADI的ADIS16470是一款微型MEMS IMU,整合了3軸陀螺儀和3軸加速度計。其陀螺儀偏置穩定性為8°/時,加速計偏置穩定性為13μg,其關鍵參數都經過工廠校準。在本文中,使用微控制器與IMU透過SPI介面進行通訊。

地磁感測器介紹

地磁感測器是用於測量羅盤體座標(即坐標系)中的地磁場的感測器,可為航向提供絕對參考。其x、y和z分量值由本地地磁場投影而來。這種感測器有兩個主要缺點——一是精準度和解析度不高——例如,常用的Honeywell羅盤感測器的解析度僅為12位元;另一個缺點是感測器容易受到周圍環境的干擾,因為地磁場非常弱,強度範圍為毫高斯(gauss)到8高斯。

儘管具有這些缺點,但其仍然可以在許多情況下使用,例如戶外、低EMI環境等。將地磁感測器鬆散地耦合到IMU,就可以在大多數環境中使用這類感測器。在本文中,使用高性能羅盤感測器(PNI感測器的RM3100)提供24位元解析度,PNI使用主動激勵法來提高抗雜訊能力。

羅盤感測器的校準

在使用羅盤感測器之前,需要對其進行校準以消除兩個主要誤差。一個是失調誤差,這原本是由感測器和電路的失調誤差引起的;另一個是標度誤差。這兩種誤差都容易受到周圍磁環境的干擾,例如,如果有一個x軸向的外部磁場施加到感測器上,就會提供外部x軸失調誤差,同時,x軸標度也將與y軸和z軸不同。

通常用於校準磁感測器的方法是在xy平面上轉動感測器繞圈,之後擷取資料。一個地點的地磁場強度是一個常數值,因此繪製的資料應該是一個圓;然而,事實上,將看到一個橢圓形,這意味著需要移動橢圓並重新縮放到以零為中心的圓。

20190808TA71P1 圖1 原始羅盤資料分佈(左)和使用橢球擬合後的羅盤資料(右)。

上述2D校準方法有一些缺點,並且需要用加速器來測量其傾斜度。使用3D球面擬合方法來校準羅盤感測器。首先,需要將感測器旋轉到x-y-z空間中的每個方向,並在3D座標中繪製其值。之後需要使用最小平方誤差(MSE)方法將資料擬合為橢圓體。

橢球方程式可以表示為:

20190808TA71P1-1

其中,X、Y和Z是羅盤輸出在三個方向上的地磁分量。將這些值擬合為橢圓體意味著,需要獲得係數的最優值集合。可將係數定義為:

20190808TA71P1-2

在擬合時,將向量定義為:

20190808TA71P1-3

所以需要計算最佳σ,並使用公式(2)來找出最小值:

20190808TA71P1-4

如此,就可以得到圖1所示的擬合結果。

為了校準感測器,需要拉伸擬合的橢圓體並將其移至以零為中心的球體上。使用矩陣奇異值分解(SVD)方法來進行這種校準,校準後的球體如圖2所示。

20190808TA71P2 圖2 用SVD方法進行球體校準後的羅盤資料。

校準後,可以看到,測得的磁場強度(球半徑)幾乎恆定不變,如圖3所示。

20190808TA71P3 圖3 預校準和後校準的磁場比較。

使用IMU/羅盤姿態和航向參考系統(AHRS)

AHRS由三個軸上的感測器組成,提供姿態資訊,包括翻滾角、俯仰角和偏航角。AHRS是一個來自飛機導航的概念,用它來描述方向,即姿態。在介紹本文的方法之前,有必要首先解釋為什麼確定姿態需要進行融合。事實上,系統現在有三種感測器:陀螺儀、加速器和羅盤。

陀螺儀提供圍繞各軸的旋轉角速度。透過角速率積分計算,可以得到旋轉角度。如果知道初始航向,就始終都能得到航向姿態,出於積分的原因,可以累計陀螺儀的不穩定偏置,這將導致角度誤差。此外,來自陀螺儀的高斯分佈雜訊將積分成一個布朗運動過程,並導致隨機行走誤差,因此,很難長時間使用陀螺儀,陀螺儀需要定期校準。

加速器提供每個軸方向的移動加速率。在靜態狀態下,可以得到每個軸與重力加速率之間的角度。由於重力加速度在方向和值上恆定不變,因此可以獲得相對於重力方向的航向姿態。然而,該方法使用重力加速度作為參考,因此不能解出圍繞重力加速度旋轉的角度。

羅盤提供從地磁場投影的每個軸的值。可以從每個軸與同為常數向量的地磁場方向之間的關係推導出角度值。如前文所述,由於對外部磁場的抗擾性較差,羅盤需要一個低干擾的環境。

從這一個解釋中,可以瞭解很難靠單一個感測器來找到姿態,需要組合使用兩個或三個感測器,並將資訊融合起來。本文運用加速器、陀螺儀和地磁羅盤查找姿態,這種融合稱為磁、角速率和重力(MARG)系統。

擴展卡爾曼濾波器(EKF)設計與感測器融合

有多種方法可以將IMU和羅盤資料融合,例如互補濾波器、統計ARMA濾波器,卡爾曼濾波器等,在本文中使用的是EKF。首先,需要介紹本文中使用的一些定義。

座標定義

航向或方向是兩個座標(即坐標系)之間的關係。一個座標總在變化,另一個座標保持不變。對於座標定義方法,使用導航座標和體座標。與東北地(NED)坐標系或地理方法相反,將測量的初始體座標值定義為導航座標,此後該座標為恆定座標,從體座標到導航座標的映射(投影)矩陣定義為:

20190808TA71P3-1

姿態定義

與歐拉角或方向余弦矩陣(DCM)不同,在這裡使用四元數,定義為:

20190808TA71P3-2

常用於導航以避免爭議。

運用卡爾曼濾波器更新姿態

在本文中使用的運動學方程 (即狀態轉移方程)是偏差方程而非線性方程,因此需要使用一個EKF,用於求該偏差方程式的一階線性近似值。對於EKF設計,本文提出以下定義:

20190808TA71P3-3

一個1×7向量作為狀態變數,其中:

20190808TA71P3-4

為角速率;

20190808TA71P3-5

為姿態四元數;

20190808TA71P3-6

一個1×7向量作為觀察變數,與狀態變數具有相同的分量。

20190808TA71P3-7

一個7×7矩陣作為狀態轉移矩陣,其中,A的第一部分是角速率的數位化微分方程,第二部分是數位化四元數更新方程,後者從運動學方程推導而來。

20190808TA71P3-8

一個7×7矩陣作為觀察矩陣。

20190808TA71P3-9

為誤差協方差矩陣,這是一個7×7矩陣,其中:

e為從真實的x值估算而來的誤差向量x̂。在測試中將初始誤差設為相對較小的值。該值會自動收斂到一個小值。

R, Q被設為狀態轉移雜訊和觀察雜訊的雜訊協方差。得到它們的初始值,R0和Q0。透過測量陀螺儀和加速器的交流均方根值的平方。在保持IMU和羅盤處於靜止狀態的同時,可設定:

R0 = Q0

根據該定義,卡爾曼濾波器將透過以下五個步驟完成:

步驟1:使用公式(3)計算卡爾曼增益K:

20190808TA71P3-10

步驟2:計算誤差協方差矩陣,P:

20190808TA71P3-11

步驟3:輸出估算狀態x̂:

20190808TA71P3-12

步驟4:投影狀態x̂–:

20190808TA71P3-13

步驟5:投影誤差協方差矩陣P -:

20190808TA71P3-14

該過程可以簡單地描述為圖4中的框架圖。

20190808TA71P4 圖4 用於更新姿態的卡爾曼濾波器流程圖。

基於MSE的感測器融合

在前文中,觀察變數是:

20190808TA71P4-1

其中沒有來自羅盤的資訊。由於ω是角速率,只能使用四元數來導入羅盤資料q。使用MSE方法獲得q,即觀察變數中的組分。

將各變數定義如下:

mb和ab:體坐標系裡的羅盤磁值和加速度值;

mn和an:導航坐標系裡的羅盤磁值和加速度值;

mn0和an0:導航坐標系裡的初始靜態羅盤磁值和加速度值。

20190808TA71P4-2

為從體坐標系到導航坐標系的姿態轉換矩陣,用四元數表示,可以寫成:

20190808TA71P4-3

其給出了導航坐標系中初始值與即時從身坐標系映射到導航坐標系的值之間的誤差ε。

根據之前的定義,MSE方法可用於獲得最佳:

20190808TA71P4-4

透過過求公式(8)的最小值:

20190808TA71P4-5

透過推導f(q)並使其等於零:

20190808TA71P4-6

將獲得方差意義上的最佳q。使用高斯-牛頓方法,用一階梯度收斂來求解以上非線性方程透過組合角速率,將得到觀察變數:

20190808TA71P4-7

其中融合了卡爾曼濾波器中的羅盤資料和IMU資料。該過程可以簡單地描述為圖5中的框架圖。

20190808TA71P5 圖5 使用MSE方法的感測器融合框架圖。

鬆耦合

如前所述,經常遇到無法使用羅盤感測器的情況。如果磁資料受到干擾,則求解的姿態精準度將比僅使用IMU時更差。因此,使用鬆耦合來判斷磁感測器是否可用。當磁感測器不可用時,只用IMU來查找姿態;當磁感測器可用時,可使用融合演算法找到姿態,如圖6所示。

20190808TA71P6 圖6 姿態計算流程圖。

在獲得新資料之後,或者在求解新的姿態時(在某些系統中,採樣週期與姿態解析週期不同,但在此處進行的是單採樣週期解析)計算加速度的大小。如果結果不等於1g,就不會使用加速器的輸出進行姿態計算。之後計算羅盤輸出的大小並將其與初始值進行比較,如果它們彼此不相等,就不會在此週期中使用地磁感測器的資料,當滿足兩個條件時,則使用卡爾曼濾波器並執行MSE融合。

使用IMU進行航位推算(DR)

在導航中,航位推算是計算目前位置的過程,先使用先前確定的位置,之後在解析週期中基於已知或估計的速度或加速度推進該位置,這裡將使用IMU內的加速器。基於上文解出的姿態,可以得到捷聯系統的移動方向,之後需要計算該方向上的距離,最後確定位置。

航位推算方法介紹

捷聯航位元推算需要使用基於加速度測量的比力方程來追蹤INS的位置。比力方程可以簡單描述為公式(10)、公式(11)和公式(12):

20190808TA71P6-1

其中,ae是地球坐標系裡的加速度,ab是體坐標系裡的加速度,ve是地球坐標系裡的速度,se是地球坐標系裡的距離,ge是地球坐標系裡的重力加速度,為[0 0 1],單位為g。需要強調的是,地球坐標系與導航坐標系不同——地球坐標系是針對NED的。該δt是解析週期。

運用第一個公式可以得到從IMU體坐標系到地球坐標系的加速度投影,如以下所示。

20190808TA71P6-2

第二個公式將加速度積分或累加為速度;然而,由於測量的加速度涉及重力分量,所以需要減去重力。與公式(11)類似,公式(12)將速度積分成距離。不過,傳統方法存在幾個問題。

加速器輸出總是有偏置,與重力相結合後,難以從公式(10)中減去,因此更準確的公式應為:

20190808TA71P6-3

除非使用一些專業設備,例如分度頭。

20190808TA71P7 圖7 速度計算中的零階與一階積分方法比較。

作為傳統方法的數值積分方法通常使用零階保持器方法(前一個值)進行積分。但是,對於連續移動,這將帶來重大的誤差。例如,比較以下方法:

方法1

20190808TA71P7-1

方法2

20190808TA71P7-2

在5秒內加速度為0.5m/s2時,位移最高將相差4m。模擬結果如圖7所示。基於前面的討論,根據應用要求,修改了傳統比力方程的兩個點:

不使用地球座標作為導航坐標系。相反的,正如在計算先前姿態時所做的,使用初始姿態:

20190808TA71P7-3

作為導航坐標系。透過這種方式,偏置和重力都可以輕鬆取消,如公式(14)所示:

20190808TA71P7-4

雖然初始姿態中包含偏置和重力分量,但這樣就不需要將它們分開成單獨的分量,而是直接減去它們。基於零階保持器與一階插值之間的比較,使用一階方法來獲得更準確的積分結果。

運動學模式和零速度更新技術(ZUPT)

透過使用IMU的初始值作為導航坐標系,可以部分取消加速器的初始偏置影響。然而,即使在使用設備之前能用分度頭準確測量偏置,仍然很難取消,除非使用另一個精確的感測器來定期校準它。這主要是由兩個原因引起的:一是偏置不穩定,這意味著之前測量的偏置不是現在的實際偏置;二是速度隨機行走,這是加速度的組成部分。前面提到的無用特性會使計算的距離顯著漂移,即使停止移動並保持靜止,從加速度積分而來的速度仍然存在,距離仍會增加。

要解決這個問題,需要找到一種透過使用ZUPT技術重置速度的方法。ZUPT技術與具體的應用密切相關,因此需要獲得系統和應用的運動學特徵,之後給出一些演算法規則。可發現的運動學模式越多,結果就越準確。

透過移動帶有SINS系統的轉椅來進行實驗。由於本文的研究不僅限於特定應用,使用以下運動學假設:

˙對於航位推算,導航坐標系中沒有z軸移動。此限制僅適用於航位推算,不適用於姿態求解。顯然是在二維空間中移動系統,這有助於消除z軸誤差;

˙所有轉彎都發生在停止後。如果在移動時發生轉彎,則會因為涉及額外加速而干擾姿態求解;

˙如果系統正在移動,加速度不能保持不變超過500毫秒。速度不能保持不變超過2秒。由於在推動或拉動轉椅,因此很難手動使力精確地保持不變超過500毫秒,並且人們很難以均勻的速度持續推動轉椅2秒以上。事實上,在實施ZUPT時即運用了這一規則;

˙加速度不能大於±1m/s2。該規則用於一些噪音過濾,後者基於施加於椅子上、不會很大的拉力或推力。

如圖8所示,當系統在X方向上移動時(投影到導航坐標系後),Y方向也會產生加速度;積分後,Y方向速度不會為零,這意味著即使只是在X方向上移動,航位元推算系統仍然會帶來Y分量。

20190808TA71P8 圖8 導航坐標系中三個方向的加速度。

基於第三運動學假設,可以使用ZUPT來消除此誤差。經ZUPT處理之後的積分速度如圖9所示。

20190808TA71P9 圖9 導航坐標系中三個方向的速度。

雖然使用了第三假設,如前所示,誤差仍然無法完全取消。誤差消除取決於設定的零加速度和零速度的閾值。但是,大多數誤差已得到糾正。

20190808TA71P10 圖10 體坐標系(左)和導航坐標系(右)的加速度。

基線偏移的消除

雖然使用了ZUPT,但有時仍然無法達到零加速度。這會導致兩個因素:

˙無法用ZUPT完全消除偏置不穩定誤差和速度隨機行走;

˙求出的姿態有一些誤差,結果將導致投影(從體坐標系到導航坐標系)加速度誤差。

以圖10為例。圖10中的左圖是IMU的原始資料(體坐標系),圖10中的右圖是導航坐標系中投影的加速度。可以看出,停止移動時,投影加速度不為零,由於它總是在變化,可稱之為基線偏移。為了消除基線偏移,需要即時連續獲得偏移偏置並從投影加速度中減去該值。結果如圖11所示。

20190808TA71P11 圖11 基線偏移消除之前(頂)和之後(底)的加速度。

圖11是基線偏移消除前的加速度,底圖中的綠色軌跡是計算的基線偏移,紅色軌跡是基線偏移消除後的加速度。可以使用圖12中的框架圖簡要描述航位推算過程。我們將體坐標系加速度ab和姿態轉換矩陣(來自AHRS)輸入:

20190808TA71P11-1

到DR系統。完成後,將獲得導航坐標系中的位置。

20190808TA71P12 圖12 航位推算流程圖。

實驗結果

使用SPI埠,將IMU評估板和羅盤評估板連接到ADI的ADuCM4050電路板,構建出本文的系統,如圖13所示。電路板調整資料格式並進行時間同步(因為IMU和羅盤的資料速率不同)。之後使用UART將擷取的資料傳輸到電腦,所有計算(包括校準、AHRS和DR)均在MATLAB中執行。

20190808TA71P13 圖13 實驗平台設置。

20190808TA71P14 圖14 四元數格式(左)和DCM格式(右)的姿態。

20190808TA71P15 圖15 位置計算結果。

將評估板和電腦放在轉椅上,並在實驗室中推著轉椅繞圈。

˙AHRS輸出:姿態以四元數格式和DCM格式表示,如圖14所示;

˙DR輸出:帶XYZ位置的航位推算結果和三維圖如圖15所示。

結論

本文使用ADI的IMU和地磁感測器構建導航系統的基本過程,內容包含了使用的校準、AHRS和DR方法。在平台和實驗環境等條件有限的情況下,很難進一步測試平台和演算法。有很多方法可用於改善結果,例如:

˙使用里程表或UWB距離測量方法將加速器與IMU融合,以在DR中獲得更準確的距離值;

˙使用更複雜的運動學模型,該模型在AHRS和DR中在感測器和系統層次涉及更多特性,例如系統的振動、加速度和減速度模型、地面平整度等。這意味著為了提高計算結果的準確性,需要給出更多的邊界條件;

˙使用更精確的數值計算方法,比如用辛普森規則或三次樣條插值在DR中進行積分,或者使用牛頓方法而非高斯-牛頓方法求解非線性MSE方程等。

在實驗中發現的最後、但也是最重要的一點是,INS與應用或運動學模式密切相關。例如,在兩個地方進行了實驗:未鋪設地毯的實驗室和鋪設地毯的辦公室。如果使用相同的參數集,DR結果會顯示出巨大的差異。因此,無論哪種應用,例如患者追蹤、AGV導航或停車定位,或者對於同一應用中的不同條件,都需要全面瞭解其運動學模型。

限量免費席位!年度感測+無線智慧時代雙論壇8月就在文華東方酒店,請盡快由此報名