• 
    

    
    

      99热精品在线国产_美女午夜性视频免费_国产精品国产高清国产av_av欧美777_自拍偷自拍亚洲精品老妇_亚洲熟女精品中文字幕_www日本黄色视频网_国产精品野战在线观看 ?

      基于卡爾曼濾波的MIMU姿態(tài)解算算法研究

      2021-01-07 04:56:24李維剛張駿虎張傳莊
      計(jì)算機(jī)測(cè)量與控制 2020年12期
      關(guān)鍵詞:陀螺儀航向卡爾曼濾波

      張 靜,李維剛,張駿虎,聶 鵬,張傳莊

      (1.中北大學(xué) 軟件學(xué)院,太原 030051; 2.山西江淮重工有限責(zé)任公司,山西 晉城 048026)

      0 引言

      MEMS慣性測(cè)量單元(MIMU),采用MEMS制造工藝制作,借助內(nèi)置的MEMS加速度傳感器和MEMS陀螺儀,測(cè)量來自3個(gè)方向的線性加速度和旋轉(zhuǎn)角速率,通過解算MIMU的角速率和加速度輸出,得到載體的姿態(tài)信息[1]。MIMU以其功耗低、重量輕、低成本和小體積使得它廣泛應(yīng)用在智能駕駛、機(jī)器人控制、 平臺(tái)穩(wěn)定、飛行器姿態(tài)控制等民用、商用級(jí)軍事領(lǐng)域。其數(shù)字化和高可靠性的特點(diǎn)使其有漸漸代替其他中低精度慣性測(cè)量單元的趨勢(shì)。

      但是受制于制造極限和體積的約束,MEMS慣性測(cè)量單元中檢測(cè)電路電子元件性能隨溫度發(fā)生漂移、交叉軸存在耦合誤差,嚴(yán)重影響系統(tǒng)的輸出精度[2]。捷聯(lián)慣性導(dǎo)航系統(tǒng)的精度主要取決于慣性器件的精度、導(dǎo)航算法的精度、導(dǎo)航計(jì)算機(jī)的處理能力及載體外界環(huán)境。因此本文研究對(duì)解算算法進(jìn)行優(yōu)化來提升MIMU的工作精度具有非常重要的實(shí)用意義。

      MEMS慣性導(dǎo)航系統(tǒng)自主獲取載體完整運(yùn)動(dòng)信息進(jìn)行導(dǎo)航時(shí),需要確定載體的指向軸與真北的方向,以提供系統(tǒng)初始對(duì)準(zhǔn)過程的航向角。對(duì)于中低精度光纖陀螺和MEMS陀螺儀而言,其精度無法精確測(cè)量到地球自轉(zhuǎn)角速度,在靜置狀態(tài)下,尋北精度相對(duì)較低,甚至不能尋北。要實(shí)現(xiàn)導(dǎo)航任務(wù)就必須外接絕對(duì)航向的傳感器,如GPS、磁傳感器,來解決系統(tǒng)初始對(duì)準(zhǔn)問題。目前,慣性導(dǎo)航定位系統(tǒng)中常用的尋北方式主要有3種:1)基于單軸旋轉(zhuǎn)調(diào)制的MEMS慣性導(dǎo)航系統(tǒng)及其尋北方法,解析地球自轉(zhuǎn)角速度與地球重力加速度兩個(gè)不共線的矢量,利用MEMS慣性系統(tǒng)進(jìn)行自主、快速、準(zhǔn)確尋北[3];2)采用兩個(gè)或更多天線以構(gòu)成基線向量,用于測(cè)量航向和俯仰角,采用不在一條線上的3個(gè)天線組合能測(cè)量航向、俯仰和橫滾角;3)磁力計(jì)尋北,通過利用磁力計(jì)測(cè)得磁場(chǎng)強(qiáng)度進(jìn)而得到所需的載體方位角信息。因此在MEMS慣性測(cè)量組合中基本都集成了經(jīng)過內(nèi)部誤差、應(yīng)用安裝誤差以及羅差補(bǔ)償后的三軸磁力計(jì)用于尋北[4]。

      MIMU工作過程中,陀螺儀存在低頻積分漂移累積的問題,加速度計(jì)受高頻振動(dòng)影響較大,磁力計(jì)測(cè)量得到的磁場(chǎng)信息除了包含地磁導(dǎo)航所需的地磁場(chǎng)信息,還有載體自身存在的線圈、鐵磁物質(zhì)等產(chǎn)生的載體磁場(chǎng),干擾磁力計(jì)的測(cè)量精度,造成航向角漂移。因此,要建立了各自慣性器件輸出誤差模型,發(fā)揮MIMU系統(tǒng)中各傳感器各自的優(yōu)勢(shì),利用Kalman濾波對(duì)基于MIMU的導(dǎo)航系統(tǒng)進(jìn)行信息融合處理[5]。

      本文設(shè)計(jì)了一種基于卡爾曼濾波的四元數(shù)姿態(tài)解算算法來解決MEMS慣性測(cè)量單元器件特性導(dǎo)致的解算精度較低的問題。本文設(shè)計(jì)的四階卡爾曼濾波器復(fù)雜度相對(duì)解四元數(shù)微分方程組并不高,實(shí)驗(yàn)證明該算法可以有效提高M(jìn)IMU姿態(tài)解算精度,明顯改善測(cè)量?jī)x器精度低、數(shù)據(jù)不穩(wěn)定的狀況,具有較強(qiáng)的工程實(shí)用價(jià)值。

      1 MIMU姿態(tài)解算原理

      由三軸MEMS陀螺儀、三軸MEMS加速度傳感器和三軸磁力計(jì)組成的MIMU固連在載體上,MIMU姿態(tài)角解算原理如圖1所示。

      MEMS陀螺儀輸出的旋轉(zhuǎn)角速度信號(hào)經(jīng)過數(shù)字濾波、信號(hào)采集后轉(zhuǎn)化為數(shù)字信號(hào),通過四元數(shù)姿態(tài)解算后,獲得載體運(yùn)載體的姿態(tài)角。這種姿態(tài)角獲取方法是由陀螺儀測(cè)得的角速率,能保證系統(tǒng)的動(dòng)態(tài)性能,不受載體的工作狀態(tài)和環(huán)境影響,但隨時(shí)間積累姿態(tài)角誤差變大;三軸MEMS加速度計(jì)輸出的加速度信號(hào)經(jīng)過數(shù)字濾波、信號(hào)采集后,選用方向余弦矩陣在載體坐標(biāo)系和導(dǎo)航坐標(biāo)系之間進(jìn)行變換,得到俯仰角和橫滾角;三軸磁力計(jì)輸出的磁場(chǎng)信號(hào)同樣經(jīng)過數(shù)字濾波、信號(hào)采集后,根據(jù)方向余弦矩陣,得到地球磁場(chǎng)矢量在x、y軸上的分量,計(jì)算后得到載體相對(duì)地磁北極的航向角,考慮地理北極和地磁北極夾角,可修正得到載體相對(duì)地理北極的航向角。由三軸加速度計(jì)和磁強(qiáng)計(jì)計(jì)算得到的姿態(tài)角,適合運(yùn)載體靜態(tài)時(shí)的測(cè)量,測(cè)量誤差不隨時(shí)間累計(jì),但動(dòng)態(tài)性能差,尤其是加速度信號(hào)噪聲較大。

      因此,本文結(jié)合兩種獲取姿態(tài)角優(yōu)勢(shì),既能保證系統(tǒng)動(dòng)態(tài)時(shí)的性能,又對(duì)補(bǔ)償靜態(tài)時(shí)陀螺儀的零點(diǎn)漂移。將陀螺儀獲取到的姿態(tài)角與角速度計(jì)和磁力計(jì)獲得的姿態(tài)角做差,得到姿態(tài)角誤差。利用設(shè)計(jì)的卡爾曼濾波器對(duì)姿態(tài)誤差角進(jìn)行優(yōu)化估計(jì),得到姿態(tài)角誤差角的最優(yōu)估計(jì)值。對(duì)陀螺儀得到的姿態(tài)角進(jìn)行優(yōu)化校正,從而得到較高精度的姿態(tài)角信息。

      1.1 坐標(biāo)系建立

      在捷聯(lián)慣性導(dǎo)航系統(tǒng)中,進(jìn)行姿態(tài)解算的目的是利用陀螺儀角速率積分和加速度計(jì)瞬時(shí)值確定載體坐標(biāo)系(b系)與導(dǎo)航坐標(biāo)系(n系)的三次轉(zhuǎn)角即航向角ψ、俯仰角θ和橫滾角γ,為慣性導(dǎo)航系統(tǒng)的速度解算、位置解算時(shí)提供姿態(tài)矩陣。姿態(tài)矩陣的實(shí)時(shí)計(jì)算和更新是慣性導(dǎo)航系統(tǒng)的主要計(jì)算內(nèi)容。其解算的實(shí)時(shí)性、解算的精度直接決定著影響著慣性導(dǎo)航的精度及任務(wù)的成敗[6]。航向角ψ、俯仰角θ和橫滾角γ三個(gè)姿態(tài)角定義如圖2所示。

      圖2 載體姿態(tài)角定義

      捷聯(lián)慣性導(dǎo)航系統(tǒng)中慣性儀表直接固連在載體上,可以直接給出軸向的線加速度和角速度。通過其建立的“數(shù)學(xué)平臺(tái)”,將空間每個(gè)矢量在導(dǎo)航坐標(biāo)系下進(jìn)行分解,計(jì)算坐標(biāo)系之間的相對(duì)位置關(guān)系,獲得轉(zhuǎn)換到導(dǎo)航坐標(biāo)系的參量[7],進(jìn)行導(dǎo)航解算,進(jìn)而得到運(yùn)載體的速度和位置。

      載體坐標(biāo)系(簡(jiǎn)稱b系)是一個(gè)正交坐標(biāo)系,采用右手定則確立的三維笛卡爾坐標(biāo)系, 記作OXbYbZb[8]。與載體固連,隨載體的轉(zhuǎn)動(dòng)一起運(yùn)動(dòng)[9],其原點(diǎn)與載體的重心重合,Xb軸沿載體的俯仰軸向右、Yb軸沿載體的橫滾軸向前、Zb軸沿載體的偏航軸向上。

      導(dǎo)航坐標(biāo)系(簡(jiǎn)稱n系),一種站內(nèi)坐標(biāo)系,與地理坐標(biāo)系軸向完全平行,只是原點(diǎn)不同。采用傳統(tǒng)的“東北天”地理坐標(biāo)系,記作OXnYnZn。文中均以地理坐標(biāo)系作導(dǎo)航坐標(biāo)系,進(jìn)行導(dǎo)航解算時(shí)的參考坐標(biāo)系,導(dǎo)航坐標(biāo)系原點(diǎn)位于載體的質(zhì)心,始終取觀測(cè)點(diǎn)為原點(diǎn)。建立的載體坐標(biāo)系和導(dǎo)航坐標(biāo)系如圖3所示。

      圖3 坐標(biāo)系示意圖

      1.2 姿態(tài)更新算法

      捷聯(lián)慣性導(dǎo)航對(duì)導(dǎo)航算法的實(shí)時(shí)性、精確性要求較高。導(dǎo)航算法誤差包括姿態(tài)算法誤差、速度算法誤差、位置算法誤差。其中姿態(tài)算法的精度最為關(guān)鍵,直接決定比力積分的精度,進(jìn)而影響速度、位置解算的精度。姿態(tài)算法本身引入的誤差應(yīng)控制在慣性器件引入誤差的5%內(nèi)。

      載體運(yùn)動(dòng)中高頻成分產(chǎn)生不可交換性誤差,對(duì)姿態(tài)解算精度影響很大,需采用執(zhí)行頻率高的姿態(tài)算法才能精確消除誤差;而高階姿態(tài)算法具有較高的精度,需要更多的計(jì)算量,對(duì)計(jì)算機(jī)處理能力提出較高的要求。因此在姿態(tài)算法的選擇上必須兼顧實(shí)時(shí)性和精確性。相對(duì)于方向余弦矩陣方法和歐拉角法,四元數(shù)方法以其非奇異性、簡(jiǎn)便性和計(jì)算有效性成為姿態(tài)更新的主要方法,能在高頻角速率環(huán)境下姿態(tài)更新精度更高。本文選用四元數(shù)方法對(duì)MEMS陀螺儀輸出的動(dòng)態(tài)旋轉(zhuǎn)角速度信號(hào)進(jìn)行實(shí)時(shí)姿態(tài)解算。

      1.2.1 陀螺儀數(shù)據(jù)姿態(tài)解算

      1.2.1.1 四元數(shù)初始化

      選“東-北-天(E-N-U)”地理坐標(biāo)系(e系)作為捷聯(lián)慣導(dǎo)系統(tǒng)的導(dǎo)航參考坐標(biāo)系,重新記為n系。

      (1)

      1.2.1.2 四元數(shù)更新

      四元數(shù)微分方程為:

      (2)

      寫成矩陣形式為:

      (3)

      即:

      (4)

      要進(jìn)行姿態(tài)矩陣的實(shí)時(shí)計(jì)算和更新,必須實(shí)時(shí)求解四元數(shù)微分方程,這里采用龍格-庫(kù)塔法(Runge-Kutta)[9]。

      X(t+T)=X(t)+hf(X(t),ω(t))

      式中,h為采樣周期[10]。根據(jù)上文,四元數(shù)的更新方程可表示為:

      (5)

      式中,ωX,ωY,ωZ表示陀螺儀在規(guī)定周期內(nèi)完成的3個(gè)軸向的角速度采樣,T是采樣周期。由于四元數(shù)是一個(gè)四維單位向量,在完成四元數(shù)的更新后需要將其進(jìn)行單位化處理,計(jì)算公式如下:

      (6)

      1.2.1.3 四元數(shù)轉(zhuǎn)換為姿態(tài)矩陣與姿態(tài)角

      四元數(shù)運(yùn)算過程中也需要經(jīng)過姿態(tài)變換矩陣,用四元數(shù)常量代替三角函數(shù)運(yùn)算,求解姿態(tài)角。在完成四元數(shù)矩陣的更新之后,需要將得到的四元數(shù)變量轉(zhuǎn)換為可以表示載體運(yùn)動(dòng)的方向余弦陣表達(dá)式,再轉(zhuǎn)換成歐拉角[11]。由四元數(shù)變量組成的方向余弦陣如式(8)所示:

      (7)

      (8)

      至此,得到了只用四元數(shù)中的4個(gè)變量表示的載體姿態(tài)角公式:

      1.2.2 角速度計(jì)和磁力計(jì)數(shù)據(jù)姿態(tài)解算

      捷聯(lián)慣性系統(tǒng)載體坐標(biāo)系和導(dǎo)航(地理)坐標(biāo)系相互重合時(shí),MEMS加速度計(jì)的測(cè)量值為An=[0 0g]T,磁力計(jì)的測(cè)量值為:Mn=[MN0MD]T。

      若載體處于處在任意一個(gè)位置時(shí),MEMS加速度計(jì)的測(cè)量值為:Ab=[axayaz]T;磁力計(jì)的測(cè)量值為:Mb=[MxMyMz]T。

      依據(jù)方向余弦矩陣,b系與n系的變換關(guān)系有:

      (9)

      (10)

      由上式可解算出俯仰角θ和橫滾角γ:

      (11)

      將式代入得到地磁場(chǎng)矢量在x、y方向的分量:

      (12)

      從而計(jì)算出載體相對(duì)地磁北極的航向角:

      (13)

      故航向角φ=φm+Δφ,其中Δφ為地磁北極與地理北極的夾角。

      1.3 卡爾曼濾波器構(gòu)建

      慣性測(cè)量過程中存在隨機(jī)干擾的問題,在觀測(cè)的信號(hào)中常常夾雜著隨機(jī)噪聲。這就需要采用最優(yōu)估計(jì)理論來得到所需的狀態(tài)變量[12-14],這里采用卡爾曼濾波器(Kalman filter)進(jìn)行狀態(tài)估計(jì),該算法的原理如圖4所示,X0代表初始狀態(tài)值,P0代表初始協(xié)方差矩陣,一般選取為單位矩陣I。

      圖4 卡爾曼濾波算法原理圖

      使用該算法時(shí),系統(tǒng)狀態(tài)方程中的參量選為姿態(tài)角偏差和陀螺儀的零漂誤差;測(cè)量方程由加速度計(jì)與磁強(qiáng)計(jì)輸出數(shù)據(jù)解算的姿態(tài)角和四元數(shù)法計(jì)算出的姿態(tài)角的差值構(gòu)成。

      建立的狀態(tài)方程如下:

      (14)

      狀態(tài)轉(zhuǎn)移矩陣:

      (15)

      噪音驅(qū)動(dòng)矩陣:

      測(cè)量方程為:

      Z(t)=H(t)X(t)+V(t)

      其中:H(t)=(I3×303×3)為測(cè)量矩陣,測(cè)量噪聲V(t)為零均值的高斯白噪聲,它的協(xié)方差矩陣為R(t)。

      一般地,卡爾曼濾波器在計(jì)算機(jī)上運(yùn)行,對(duì)狀態(tài)方程和測(cè)量方程采取離散化處理后,為了表述方便,這里狀態(tài)和測(cè)量方程的變量表示不變??柭鼮V波器可以看作狀態(tài)變量在由觀測(cè)生成的線性空間的映射,采用時(shí)域狀態(tài)空間法,對(duì)系統(tǒng)可觀測(cè)信號(hào)進(jìn)行測(cè)量,以觀測(cè)量作為卡爾曼濾波器的輸入量,狀態(tài)量作為輸出量,根據(jù)觀測(cè)量修正狀態(tài)估計(jì)量進(jìn)行迭代從而達(dá)到最優(yōu)估計(jì),實(shí)現(xiàn)對(duì)系統(tǒng)的狀態(tài)進(jìn)行估計(jì)的目的[15]。

      2 實(shí)驗(yàn)與測(cè)試

      2.1 實(shí)驗(yàn)條件

      為了驗(yàn)證算法對(duì)MEMS慣性測(cè)量單元姿態(tài)解算精度補(bǔ)償?shù)男Ч褂米藨B(tài)精度為0.05°的三軸無磁轉(zhuǎn)臺(tái)對(duì)MIMU進(jìn)行靜態(tài)姿態(tài)測(cè)試。利用安裝夾具將MEMS慣性測(cè)量單元固定在無磁轉(zhuǎn)臺(tái)上。

      使用位置控制精度為0.1°的多功能三軸轉(zhuǎn)臺(tái)進(jìn)行動(dòng)態(tài)姿態(tài)測(cè)試。通過對(duì)比算法補(bǔ)償前后的姿態(tài)精度得出實(shí)驗(yàn)結(jié)論。

      被測(cè)MIMU如圖5所示,其主要器件包括:3個(gè)單軸MEMS陀螺儀CRG20、三軸MEMS加表ADIS16210、三軸MEMS磁強(qiáng)計(jì)選用HMC5883L磁力計(jì),該芯片具有體積小、精度高、自帶數(shù)字接口的優(yōu)點(diǎn)。HMC5883L內(nèi)置偏差校準(zhǔn)裝置,可以使得羅盤精度在1~2°。

      圖5 被測(cè)MIMU實(shí)物圖

      2.2 測(cè)試步驟

      1)將被測(cè)MIMU安裝在無磁轉(zhuǎn)臺(tái)臺(tái)面中心,保證其軸向與轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)軸平行。

      2)接通慣組電源,對(duì)被測(cè)MIMU完成預(yù)熱。

      3)以固定步長(zhǎng)分別轉(zhuǎn)動(dòng)無磁轉(zhuǎn)臺(tái)3個(gè)軸向,在每一個(gè)靜止角位置采集被測(cè)器件原始數(shù)據(jù),利用USB-TTL將數(shù)據(jù)發(fā)送至上位機(jī)進(jìn)行采集并保存。

      4)將被測(cè)MIMU安裝在慣性測(cè)量單元測(cè)試標(biāo)定系統(tǒng)臺(tái)面中心,系統(tǒng)調(diào)至搖擺模式,采集被測(cè)器件原始數(shù)據(jù)并保存。

      5)對(duì)原始數(shù)據(jù)進(jìn)行處理并繪圖,取最大姿態(tài)誤差作姿態(tài)精度。

      3 結(jié)果分析

      在傳統(tǒng)的姿態(tài)解算方法中,加速度計(jì)并不參與航向角的解算,航向角的測(cè)量值完全依賴陀螺儀積分得到。受到陀螺儀的低頻積分噪聲干擾以及梯形近似積分引入的計(jì)算誤差影響,航向角漂移嚴(yán)重。因此,航向角的測(cè)量精度可以很好的反映器件姿態(tài)精度。

      以靜態(tài)航向角和動(dòng)態(tài)航向角的補(bǔ)償前后解算結(jié)果繪圖,得到圖6和圖7。

      圖6 航向軸靜態(tài)姿態(tài)角

      圖7 航向軸動(dòng)態(tài)姿態(tài)角

      圖6可以看出,經(jīng)過卡爾曼濾波對(duì)四元數(shù)的補(bǔ)償,四元數(shù)解算得到的航向角精度得到了明顯提高,在10°范圍內(nèi)并未發(fā)生明顯漂移。

      從圖7可以看出,在動(dòng)態(tài)測(cè)試過程中,經(jīng)過補(bǔ)償?shù)暮较蚪钦?fù)峰值均趨近于轉(zhuǎn)臺(tái)搖擺幅度,具有良好的動(dòng)態(tài)跟隨效應(yīng)。相較于補(bǔ)償之前解算精度有了明顯改善。

      4 結(jié)束語

      綜上所述,通過卡爾曼濾波器對(duì)四元數(shù)的修正,MIMU的姿態(tài)解算精度得到了顯著提升。采用基于卡爾曼濾波的四元數(shù)姿態(tài)解算算法能夠增強(qiáng)MIMU的動(dòng)態(tài)跟隨性能、提升姿態(tài)解算精度。通過實(shí)驗(yàn)證明了該算法的可行性,在工程實(shí)踐中具有一定的應(yīng)用價(jià)值。

      猜你喜歡
      陀螺儀航向卡爾曼濾波
      基于EMD的MEMS陀螺儀隨機(jī)漂移分析方法
      知坐標(biāo),明航向
      考慮幾何限制的航向道模式設(shè)計(jì)
      我國(guó)著名陀螺儀專家——林士諤
      基于遞推更新卡爾曼濾波的磁偶極子目標(biāo)跟蹤
      基于干擾觀測(cè)器的船舶系統(tǒng)航向Backstepping 控制
      電子制作(2017年24期)2017-02-02 07:14:16
      微機(jī)械陀螺儀概述和發(fā)展
      MEMS三軸陀螺儀中不匹配干擾抑制方法
      基于模糊卡爾曼濾波算法的動(dòng)力電池SOC估計(jì)
      基于擴(kuò)展卡爾曼濾波的PMSM無位置傳感器控制
      宣汉县| 葫芦岛市| 洪雅县| 阿拉善右旗| 永顺县| 吉木乃县| 个旧市| 清水县| 增城市| 罗源县| 改则县| 鄂托克旗| 界首市| 奇台县| 开江县| 玉环县| 祁阳县| 渭源县| 曲阜市| 阿坝县| 什邡市| 南丹县| 泸水县| 城口县| 成安县| 玛纳斯县| 洪泽县| 漳平市| 从江县| 汽车| 南康市| 义马市| 乌拉特前旗| 常宁市| 大田县| 涞源县| 临安市| 清丰县| 荆州市| 凤山县| 天峨县|