基于九軸傳感器的慣性導(dǎo)航模塊的設(shè)計

1、基于九軸傳感器的慣性導(dǎo)航模塊的設(shè)計作者:日期:用梯度下降算法估測IMU和MAG方向摘要：本文提出了一種新的定位算法，用于支持高效計算、可穿戴的人體慣性運動跟蹤系統(tǒng) ，用于康復(fù)應(yīng)用。它適用于由三軸陀螺儀和加速度計組成的慣性測量單元（IMUS ），以及還包括三軸磁強計的磁角速度和重力（MARG傳感器陣列。M ARG的實現(xiàn)包括磁失真補償。該算法使用四元數(shù)表示，允許加速度計和磁強計數(shù)據(jù)用于解析推導(dǎo)和優(yōu)化的梯度下降算法，以四元數(shù)導(dǎo)數(shù)計算陀螺儀測量誤差的方向。并對基于卡爾曼濾波的定位傳感器算法進行了性能測試。結(jié)果表明，該算法達到了基于卡爾曼濾波算法的精度匹配水平 ；V 0.8 C的靜態(tài)均方根 誤差，V 1

2、.7 C的動態(tài)均方根誤差，計算量低和以小采樣率工作的能力影響大大降低了可穿戴 慣性運動跟蹤所需的硬件和電源，從而能夠創(chuàng)造出能夠長期工作的輕量級、廉價系統(tǒng)。1.介紹精確測量方向在一系列領(lǐng)域中起著關(guān)鍵作用，包括:航空航天、機器人、導(dǎo)航和人體運動分析和機器交互。在康復(fù)治療中，運動跟蹤是一項重要的使用技術(shù)，特別是用于監(jiān)測臨床外環(huán)境；理想情況下，病人的活動可以連續(xù)監(jiān)測，并隨后得到糾正。雖然已經(jīng)為康復(fù)而進行了大 量的運動跟蹤工作，但還沒有實現(xiàn)一種能夠長時間記錄數(shù)據(jù)的不突出的、可穿戴的系統(tǒng)?，F(xiàn) 有的系統(tǒng)往往需要一臺筆記本電腦或掌上電腦由受試者攜帶，由于處理，數(shù)據(jù)存儲和感官設(shè)備的功率要求，這在實驗室環(huán)境之外是

3、不實際的，因此只能在短時間內(nèi)獲得有限的物體運動的詳細數(shù)據(jù)。在一段較長的時間內(nèi) （例如一整天或甚至一周）代表一個受試者自然行為的更精確的數(shù)據(jù)將在這個領(lǐng)域有著重要的應(yīng)用價值。在最近的一次調(diào)查中，指出實時操作、無線特性、數(shù)據(jù)正確性和可移植性是實現(xiàn)臨床可行系統(tǒng)必須解決的主要缺陷。2 .慣性導(dǎo)航跟蹤系統(tǒng)雖然多種技術(shù)能夠測量方位，但基于慣性的感知系統(tǒng)的優(yōu)點是完全獨立，因此測量實體 既不受運動限制，也不受任何特定環(huán)境或位置的限制。慣性測量單元（IM U）由陀螺儀和加速度計組成，能夠跟蹤旋轉(zhuǎn)和平移運動。 為了進行三維測量，需要由三個相互正交的敏感軸組成 的三軸傳感器。M AFG （磁性，角速度和重力）傳感器是

4、一種混合IMU,它包含三軸磁強計。 單憑IMU就只能測量相對于重力方向的姿態(tài)，這對于許多應(yīng)用來說都是足夠的。MARG系統(tǒng)也被稱為 AHR S （姿態(tài)和航向參考系統(tǒng)），能夠提供相對于重力方向和地球磁場的方向的 完整測量。方位估計算法是任何IMU或MAG系統(tǒng)的基本組成部分。 需要將單獨的傳感器數(shù)據(jù)融合到一個單一的、最優(yōu)的方位估計中?？柭鼮V波已成為大多數(shù)定向算法和商用慣性方向傳感器的公認基礎(chǔ)；Xse ns、微應(yīng)變、，矢量導(dǎo)航、Inte rS ens e、P NI和十字弓，所有的生產(chǎn)系統(tǒng)都建立在它的基礎(chǔ)上。 基于卡爾曼的解決方案的廣泛使用證明了其準(zhǔn)確性和有效性，但是它們有一些缺點，它們的實現(xiàn)是復(fù)雜的

5、，這可以從學(xué)科文獻中看到的眾多解決方案中反映出來。線性回歸迭代是卡爾曼濾波過程的基礎(chǔ)，它要求采樣率遠遠超過目標(biāo)帶寬（例如，512 Hz之間的采樣率），30千赫對于系統(tǒng)可移植性至關(guān)重要的人體運動捕獲應(yīng)用程序來說也許是必要的，描述三維旋轉(zhuǎn)運動學(xué)的狀態(tài)關(guān)系通常需要較大的狀態(tài)向量，擴展的卡爾曼濾波實現(xiàn)將問題線性化。,并提供了一個明確的,有利于在低角速度下定，但是在有限的操作這些挑戰(zhàn)需要大量的計算負荷來實現(xiàn)基于卡爾曼濾波的解決方案 結(jié)果。解決這些問題的先前方法已經(jīng)實現(xiàn)了模糊處理和頻域濾波器 向的加速度計和在高角速度下的集成陀螺儀測量。這是一種簡單的方法 條件下有效，Bachma n和Mahon y提出了

6、兩種采用互補濾波處理的單獨算法，已經(jīng)示出該 算法結(jié)構(gòu)以相對較少的計算成本提供有效的性能。本文介紹了方向估計算法，適用于IMU和MAR係統(tǒng)。該算法采用四元數(shù)表示方向來描述三個方向的耦合性質(zhì)，不受與歐拉角表示相關(guān)的問題奇點的影響，給出了新算法的完整推導(dǎo)和實證評價，它的性能基準(zhǔn)對現(xiàn)有商用過濾系統(tǒng)和光學(xué)跟蹤系統(tǒng)有驗證作用。2.組織研究第1節(jié)描繪的方向估計算法的數(shù)學(xué)推導(dǎo)，包括磁場失真的描述參數(shù)和補償。第4節(jié)描述了用于測試和驗證算法性能的實驗設(shè)備。第5節(jié)量化了該算法的實驗測試和準(zhǔn)確性，并將其與現(xiàn)有系統(tǒng)進行了比較。第7節(jié)簡要介紹了目前在我們實驗室進行的人體運動跟蹤系統(tǒng)的實 現(xiàn)細節(jié)，第6節(jié)總結(jié)了這項工作的結(jié)論

7、和貢獻?？v觀全文，一個符號系統(tǒng)采用領(lǐng)先的上標(biāo)和 下標(biāo)從克雷格是用來表示方向和載體相對幀。前導(dǎo)下標(biāo)表示正在描述的框架，前導(dǎo)上標(biāo)表示引用的框架。例如，？?描述幀B相對于幀 A的方向，？?是幀A中描述的向量。3.算法推導(dǎo)A.從角速度中定向三軸陀螺測量角速度的X，y，和z軸的傳感器框架，分別稱為W? W?、W?分別。如果這些參數(shù)按照方程 向量: rad-1）排列成載體，四元數(shù)的導(dǎo)數(shù)描述率對地球框架相對于傳感器的框架？9變化可以計算為方程單位長度的規(guī)范化向量。（2 ）。操作表示一個四元數(shù) 錯誤 !的產(chǎn)品和八重音表示 0(I)t, ?的方向可以通過數(shù)值積分 5 當(dāng)初始條件已知時，地球框架相對于傳感器幀在時

8、間方程（3）和 所描述的四元數(shù)導(dǎo)數(shù)？?來計算。在這些方程中，？?是在時間t上測量的,是采樣周期,?/%前的定向估計,下標(biāo)3指示四元數(shù)是根據(jù)角速率計算的。Bl專Qz =閔詢丿一 1十甘3 4(41B. 重力場的方向在方位估計算法中，最初是假設(shè)加速度計只測量重力，磁力計只測量地球磁場。如果 一個地球磁場的方向在已知的地球框架中，測量傳感器框架內(nèi)的磁場方向?qū)⒃试S計算傳感器框架相對于地球框架的方向。然而，對于任何給定的測量，不會有唯一的傳感器定向解決方案，反而會有由旋轉(zhuǎn)得到的與場平行的軸，所有方向的無限個解來表示。四元數(shù)表示需要一個單一的解決方案。這可以通過中傳感器的方向制定優(yōu)化問題來解決，？?是地球

9、框架?中的場的預(yù)定參考方向，1?是在傳感器的幀中測量到的磁場；從而解決方程（6 ）目標(biāo)函數(shù)定義的方程（5）。餛務(wù)=討試Ed % ¥q _ %在眾多優(yōu)化算法中，梯度下降算法是實現(xiàn)和計算最簡單的算法之一。方程(7)描述了n次迭代的梯度下降算法:?+1,基于“初值估計”方向？?0和可變步長卩，得到了的方向估 計，方程(8)計算由目標(biāo)函數(shù)定義的解曲面上的誤差方向f和它的雅可比行列式 J。等式(7)和(8)描述了適用于在任意方向上預(yù)定義的字段的算法的一般形式，然而，如果字段的參考方向被定義為僅具有在主軸線的1或2內(nèi)的地球坐標(biāo)框架分量，那么方程就簡化了。假設(shè)定義的重力方向垂直Z軸，如方程(1 0

10、), ?和歸一化的加速度計測量1?分別替換為?和1?得到由方程(12)和(1 3 )定義的簡化目標(biāo)函數(shù)和雅可比行列式：勺=(I II 0'111(”Uh2血竹一91.1)- " r=諂一詰H -叫_凸M ?中 -71打冷=紙 -71 '如(I 4習(xí) 一4宙Zi/li £*? Hi =fl2>可以認為地球磁場在一個水平軸和垂直軸上具有分量A供<1ri?p,英國場地傾斜角在 6 5°到7?0°之間而產(chǎn)生垂直分量， 可以用方程(1 4 )表示。用?和標(biāo)準(zhǔn)化磁強計測量?分別代替?和?給出了由方程(16)和(1 7)定義的簡化目標(biāo)函數(shù)和

11、雅可比行列式。151nt = 0 “心 ttiy tti2f諸）+2bj（勺23 叮珂4）十劉昇尙術(shù)+羊沖4 ）一囚埠（Q2Q4 41也）"b丄h= （'7172 的也）叫2%（吐訕誕rjf (16»-呱伽2切4打十"沁2沙工吐3 + Qb亍»期工也2億 2如？2丄2兀帕 閔jf/i - 4也:t2亦 g -t/2h工41 + 2札臨2f>r71 + 列上 i/3汕72(17)正如之前所說的的那樣，僅僅測量重力或地球磁場并不能提供給傳感器唯一的方向。為 此，這兩個領(lǐng)域的測量和參考方向可以按照方程（18）和（19 ）的描述進行組合。然而，由方

12、程（12）和（16）中的目標(biāo)函數(shù)所創(chuàng)建的解曲面有一個由直線定義的全局極小值。 限定的解表面具有由單個點限定的最小值，只要b?工0。由等式（1 8 ）仇心一 m b.（閔盡尺阿j（r 12(1911'（7 ）進行多傳統(tǒng)的優(yōu)化方法將需要針對每個新的方位和相應(yīng)的傳感器測量計算方程式 次迭代，如果t控制的方向估計的收斂速度等于或大于物理方向的變化速率，則每一次樣本 計算一次迭代是可以接受的。方程（20）根據(jù)先前對方向位的估計，計算在時間t時的方向估計?+1和傳感器測量值所定義的目標(biāo)函數(shù)誤差？?？ ？和 ?在時間t處采樣。？?的形式是根據(jù)使用中的傳感器選擇的，如方程式（2 1）所示。下標(biāo)？表示四

13、元數(shù)是用梯度下降算法計算的。55r "l If flrd M=1嚴3九% '幣八(211)一個適當(dāng)?shù)闹?購是確保？?？ ？?勺收斂速度被物理定向率所限制,因為這避免了超調(diào)造成不必要的大步長。 問計算步驟是方程（22 ），其中t是采樣周期，？是陀螺儀測量的方位變化率，是加速度計和磁強計測量中噪聲的增強。川=na門 > 1C. 算法融合過程實際上，? ?可能從錯誤的初始條件出發(fā)，并由于陀螺測量噪聲而適應(yīng)誤差，而當(dāng)加速度計不是靜止的或磁強計暴露于干擾時，？ ？?各提供錯誤的估計。融合算法的目的是提-, -供一個方向估計，其中? ?用于濾除? ?中的高頻誤差，而？?？ ？用于補

14、償? ?中的積分漂移，并從初始條件提供收斂性地球框架相對于傳感器框架？?的估計方向是通過兩種獨立的取向計算融合得到的，方程(23)描述了 ？，？和？ ？這兩個方向，丫?和(1 - Y?適用于每個方位的計算。二海處+ (I -和鳥口沖0冬和冬1(23)最優(yōu)值Y是確保了加權(quán)發(fā)散率的值，由于積分漂移等于？?的加權(quán)收斂速度。它用方程(24 )表示,其中姑是？勺收斂速度,3是?的發(fā)散率,表示為四元數(shù)導(dǎo)數(shù)與陀螺測量誤差相對應(yīng)的幅度,方程(24)可以重新排列成方程(25)。(241融合過程保證了均衡器的最佳融合?, ?和 ?假設(shè)？的收斂速度受到a的限制等于或大于方向的物理變化率會改變方向，也變得非常大,方程

15、就很簡單。方程(20)中的購?艮大意味著a沒有上限。假設(shè)a很大，由方程(22)定義的 切?st , ?-1,該方程可寫為方程(26)。VV/Eg -出IV/111(26)27),則方程(2 5)中的Y的定義如果分母中3的項可以忽略，并且方程可以寫為方程( 也簡化了，方程(27)中可以假定Y?-0：(27)(28)。需要注意的是，在方程(28 )將方程(4 )、(26)和(27)替換為方程(2 3)直接產(chǎn)生方程 中，Y被替換為方程(2 6 )和0。電qz-t -(=出忖；J+(丄7) (gg胡丄亠gq戶時十=+倉心就、(29)方程(28 )可簡化為方程(29)，其中？ ？是方程(30)定義的估計

16、定向率。(50)Vf可以看出，方程(2 9 )和(30 )，該算法計算方向的 期?St通過數(shù)值積分的 彩?？取向變 化率估計。該算法計算的 ？?？的陀螺儀測量方向的變化率，矽2隨著陀螺儀測量誤差的大 小，B基于加速度計和磁強計測量的方向被移除。圖1顯示了一個I MU的完全方位估計算法實現(xiàn)的框圖表示。捕I if；優(yōu)単：I J,YJ-叩Gviznpi j 善;庁 y J I> jFip. 1 hkxk did盟JIM iryprcgnMUiiiMii «*1 the、win卩Igk rw證m訶hwi cMinuKHn iiLpdfiThm It If sii I ML' i

17、iipli?iKiiti,i<'nD. 磁場失真補償對磁場失真對方位傳感器性能影響的研究表明，建筑物內(nèi)的電器、金屬家具和金屬結(jié)構(gòu)等來源可能會對結(jié)果產(chǎn)生誤差。傳感器框架內(nèi)固定的干擾源，稱為硬性偏置，可通過校準(zhǔn)消除。在地球框架中的干擾源，稱為軟性誤差，只有在有一個額外的方位參考資料時才能消除。 加速度計提供了姿態(tài)參考，因此可用于補償被測地球磁場中的傾斜誤差。地球框架中磁場在時間t時的測量方向，E?可作為方程(31 )計算，如果地球磁場的參?考方向E?是相同的，則可以糾正被測方向地球磁場 E?的錯誤傾斜的影響。 這是通過計算E? ? 作為E?的正規(guī)化來實現(xiàn)的；正如方程 (3 2 )所描

18、述的那樣，地球框架X和z軸中只有分量。 ?"/骯uAy - Eesi.l 1 ® fill 發(fā)'上罠$翼*-_pl)% = |o /覽 + h* IIII bz通過這種方式對磁畸變進行補償，確保磁擾動僅限于影響方向的估計航向分量。該方法 還消除了預(yù)先定義地球磁場參考方向的需要；這是其他方向估計算法的潛在缺點，圖2顯示了 MA RG傳感器陣列的完整算法實現(xiàn)的框圖表示，包括磁失真補償。E .算法可調(diào)參數(shù)方向估計算法需要一個可調(diào)參數(shù)3, B是以四元數(shù)導(dǎo)數(shù)的大小表示的陀螺儀測量誤差，用角度?)?表示每個軸的最大陀螺儀測量誤差，使用方程描述的關(guān)系，可以用方程(3 3 )來定義

19、，其中q是每單位四元數(shù)J V ® OMmi單Jhwj工1沖djJ三 ¥ 丁*">.4.實驗設(shè)備該算法使用xse n s M Tx方位傳感器進行測試，該傳感器包含16位分辨率的三軸陀螺 儀、加速度計和磁力計。原始傳感器數(shù)據(jù)以5 12Hz的速度記錄到一臺 PC上，并引入附帶的軟件以提供經(jīng)過校準(zhǔn)的傳感器測量值，然后采用所提出的方向估計算法對其進行處理。該由于基于卡爾曼濾波的算法和所提出的,每種算法的性能都可以跟另一種算法軟件還使用了一種基于卡爾曼濾波的方位估計算法， 算法的方位估計都是使用相同的傳感器數(shù)據(jù)來計算的 進行比較，與傳感器性能無關(guān)。Ne XU s軟件的8

20、個MX3攝像機組成，Vicon系統(tǒng)，由連接到MX ultr a net服務(wù)器和被用于提供方位傳感器實際方位的參考測量值。為此，傳感器被固定在一個定位測量平臺上。在120 Hz的頻率下，記錄平臺上的光學(xué)標(biāo)記位置 ，然后對其進行后處理，計算出測量平臺 和傳感器的方位。為了使相機坐標(biāo)系中的方位測量與地球坐標(biāo)系中的方位估計算法相比較， 需要一個初步的校準(zhǔn)程序，在攝像機坐標(biāo)系中測量地球磁場和重力場的方向時，使用帶有光學(xué)標(biāo)記的磁羅盤和鐘擺。5. 實驗結(jié)果在描述最高點？，旋轉(zhuǎn)(和航向嘰分別對應(yīng)于圍繞傳感器框架 X, y和z軸的旋轉(zhuǎn))的方 向組成部分的解耦歐拉參數(shù)中將方位傳感器的性能量化為靜態(tài)和動態(tài)均方根誤差

21、是很常見 的，計算了 4組歐拉參數(shù)，分別對應(yīng)于校準(zhǔn)后的方位光學(xué)測量、基于卡爾曼濾波估計的定向算法和所提出的算法對MA G和I M U實現(xiàn)方向的估計。估計歐拉參數(shù)的誤差颶？ e?和e?實驗重復(fù)0.0 3 3,執(zhí)圖3給以及估計值與校準(zhǔn)光學(xué)測量值之間的差值。結(jié)果是由手控制每個軸的旋轉(zhuǎn)得出的。了 8次，編譯了一個數(shù)據(jù)集表示。該算法的可調(diào)參數(shù)閃于M AG的實現(xiàn)被設(shè)置在行I MU被設(shè)置在0.0 41,圖4中總結(jié)的試驗發(fā)現(xiàn)，這些值可以提供最佳的性能。出8個實驗中的典型，基于卡爾曼的算法和執(zhí)行 MA G算法實現(xiàn)結(jié)果。寅 60I。» 二 50一1口and eshrnnted 遼ncfW f*Mrdau

22、red > KalTiiarh-lMied alnthm * P-iouu&ed Til tai glooiicrm - Propizncl filler hIvIaRG*rrorS45&6032礙OCI-F- J«111 沖？駟*療*0M52和tllTW tWQNCJ令I(lǐng)Fig. 3. Typicul resiJt'i tni riie；i、*ired unJ eutimiteil rmgle “<iiiJ cmdxKUHiirTABLE 1Static and dysamic RMS huror of Kalmanbashd algorithm

23、 AKP P RO POSER AL&OEITHM IMl AND VA Rt i I rPLEMHMTAnONSEuler pjidiiULMerKiihiikiii-hninl 汕糾liili恤】MARG 訓(xùn)gori山m(MUilgurHbiinRMS仇、仙itij 7iWItMSiirimicO.T心戶RMS.SlLtlR-0一趙儼KMSdynuini也皿屮RMSsialic1.1駄戶107-N/AFtMS<>dyinaiTiicJ .3-11.1 HPN/A當(dāng)測量的角速度5° /s時，計算幅？ 0?,如靜態(tài)和動態(tài)RMS直來假設(shè)靜態(tài)狀態(tài)；當(dāng)測量的角速度5&#

24、176; / S時,計算幅？ 0?如靜態(tài)和動態(tài)RMS直來假設(shè)動態(tài)狀態(tài)，這個閾值被選擇得比數(shù)據(jù)的噪聲層大得多，結(jié)果在表1中總結(jié)出來，其中每個值代表了所有 8個實驗的平均值。圖4總結(jié)了可調(diào)參數(shù) B對算法性能影響的研究結(jié)果。實驗數(shù)據(jù)通過獨立的算法IMU和MAR G處理，B的值在0到0. 5之間。由于積分漂移，B足夠高時有一個明顯的最優(yōu)值能減少誤差，但3很低的時候，不必要的噪聲不是通過大梯度下降迭代而引入的。圖5總結(jié)了抽樣率對算法性能影響的研究結(jié)果，通過單獨的提議的算法IMU和MAG注入來處理實驗數(shù)據(jù)，使用先前定義的最優(yōu)值 3由取實驗數(shù)據(jù)，以模擬1Hz到51 2 Hz之間 的采樣率。從圖5可以看出，所提出的算法在 50Hz和512Hz性能水平相似，這兩種算法都能 在10 Hz采樣時實現(xiàn)靜態(tài)誤差2°和動態(tài)誤差7