一種新型旋轉(zhuǎn)卡爾曼濾波器的設(shè)計(jì)

一種新型旋轉(zhuǎn)卡爾曼濾波器的設(shè)計(jì)

0旋轉(zhuǎn)卡爾曼濾波器隨著地磁理論的不斷完善和傳感器、檢測(cè)器和解算法的成熟，磁強(qiáng)圖的空間姿態(tài)已成為當(dāng)前振動(dòng)測(cè)量和導(dǎo)航研究領(lǐng)域的熱點(diǎn)。然而采用地磁測(cè)量彈丸的空間姿態(tài)會(huì)產(chǎn)生一定的誤差,文中正是基于此種問(wèn)題設(shè)計(jì)出了一種新型旋轉(zhuǎn)卡爾曼濾波器來(lái)對(duì)誤差參數(shù)進(jìn)行求解。以前對(duì)于地面姿態(tài)測(cè)量系統(tǒng)中的三軸地磁傳感器采用的校準(zhǔn)算法主要是兩種,一種是與姿態(tài)無(wú)關(guān)的校準(zhǔn)算法,另一種是與姿態(tài)相關(guān)的校準(zhǔn)算法,文中主要描述的是將以前的方法組合從而產(chǎn)生一種新的三軸磁傳感器誤差校準(zhǔn)算法。1軸磁傳感器校準(zhǔn)算法文中研究的這種新型的誤差校準(zhǔn)算法特別適合旋轉(zhuǎn)穩(wěn)定的火箭彈。對(duì)于帶有更精確傳感器的三軸穩(wěn)定的火箭彈(如陀螺儀),也可應(yīng)用其他校準(zhǔn)算法。但是這并不妨礙這種算法的實(shí)用性。因?yàn)橥ǔ?lái)講測(cè)量彈體姿態(tài)的傳感器分為兩種,一種是相對(duì)粗糙的測(cè)量方法(如地磁的姿態(tài)測(cè)量),另一種是比較精確的測(cè)量方法(如太陽(yáng)方位傳感器)。而文中介紹的這種方法價(jià)值在于,在精確校準(zhǔn)之前對(duì)原始測(cè)量數(shù)據(jù)進(jìn)行粗校準(zhǔn)。三軸磁傳感器校準(zhǔn)算法的目的是用來(lái)逐步改進(jìn)估計(jì)的參數(shù),通過(guò)把每時(shí)每刻由地磁測(cè)得的數(shù)據(jù),從傳感器的測(cè)量坐標(biāo)系轉(zhuǎn)換到彈體坐標(biāo)系,然后對(duì)數(shù)據(jù)進(jìn)行迭代計(jì)算。對(duì)常用的傳感器模型,這些參數(shù)主要包括傳感器的偏置,每個(gè)軸的比例因子,以及非正交誤差。一般常用的粗校準(zhǔn)模型只可估計(jì)出傳感器的偏置。而文中介紹的三軸磁傳感器校準(zhǔn)算法,當(dāng)有足夠的數(shù)據(jù)之后就可確定出偏置、比例系數(shù)和三軸傾斜角的非正交部分。同時(shí)當(dāng)有來(lái)自其他傳感器的數(shù)據(jù)之后,相對(duì)于其他傳感器的對(duì)準(zhǔn)誤差也可確定出來(lái)。2軸磁傳感器的校準(zhǔn)算法文中采用的新型三軸磁傳感器校準(zhǔn)算法是:將與姿態(tài)無(wú)關(guān)和與姿態(tài)相關(guān)的校準(zhǔn)算法組合起來(lái)實(shí)現(xiàn)磁傳感器的誤差校準(zhǔn),主要包括以下步驟:1)首先用一個(gè)與姿態(tài)無(wú)關(guān)的三軸磁傳感器校準(zhǔn)算法來(lái)估計(jì)出偏置、比例系數(shù)和非正交誤差。用這些參數(shù)對(duì)三軸磁傳感器的數(shù)據(jù)實(shí)現(xiàn)粗處理。2)其次用經(jīng)過(guò)校準(zhǔn)后的磁傳感器和至少一個(gè)其他傳感器數(shù)據(jù)來(lái)求解姿態(tài)歷程,如果有多個(gè)傳感器,指定其中一個(gè)作為主要的傳感器。3)最后使用由上一步得到的姿態(tài)歷程,用一個(gè)姿態(tài)相關(guān)的方法估計(jì)出三軸磁傳感器的對(duì)準(zhǔn)誤差。利用此對(duì)準(zhǔn)誤差重新對(duì)數(shù)據(jù)進(jìn)行處理。重復(fù)上述兩個(gè)步驟直到校準(zhǔn)誤差沒(méi)有顯著的改變?yōu)橹埂?.1軸磁傳感器的自校準(zhǔn)三軸磁傳感器的數(shù)據(jù)本身可以進(jìn)行自校準(zhǔn),這是基于參考磁場(chǎng)信號(hào)幅值的一致性(也就是觀測(cè)到的地磁信號(hào)的幅值是不變的,所以可以根據(jù)這些測(cè)量數(shù)據(jù)對(duì)其本身進(jìn)行自校準(zhǔn)),而不需要姿態(tài)角、姿態(tài)角速度和其他傳感器的信息。除了用于迭代校準(zhǔn)之外,這個(gè)方法本身也是很有用的,特別是在弾箭飛行的初始階段內(nèi),由于在初始階段內(nèi),其他傳感器尚未工作或?qū)?zhǔn)誤差還沒(méi)確定出來(lái),因此在這段時(shí)間內(nèi)三軸磁傳感器本身的自校準(zhǔn)是非常有效的。并不是所有的三軸磁傳感器參數(shù)都可以用這種姿態(tài)無(wú)關(guān)的方法解算出來(lái)。因?yàn)橛绊憸y(cè)量幅值的參數(shù)是可觀測(cè)的;而影響測(cè)量方向的參數(shù)是不可觀測(cè)的。從地面系到測(cè)量系的轉(zhuǎn)換可以用矩陣轉(zhuǎn)換來(lái)表示。這種九元素的矩陣是正交矩陣和對(duì)稱矩陣。正交矩陣表示傳感器的測(cè)量軸相對(duì)于彈體軸的剛體轉(zhuǎn)動(dòng)。對(duì)稱矩陣的對(duì)角線部分表示比例因子,而非對(duì)角線則表示為3個(gè)軸的傾斜角部分。當(dāng)只有測(cè)量幅值數(shù)據(jù)可用時(shí),由于矢量的內(nèi)積隨坐標(biāo)系是不變的,因此傳感器的正交對(duì)準(zhǔn)誤差將是不可觀測(cè)的,但是對(duì)于其偏置,比例因子誤差卻可以估計(jì)出來(lái)。2.2旋轉(zhuǎn)卡爾曼濾波器對(duì)三軸磁傳感器的偏置、比例系數(shù)進(jìn)行校正之后,運(yùn)用三軸磁傳感器及其他傳感器數(shù)據(jù)就可確定姿態(tài)歷程。當(dāng)前對(duì)彈丸的姿態(tài)估計(jì)有三種方法,第一種是最小二乘的差分校正法,第二種是擴(kuò)展卡爾曼濾波法,這種方法對(duì)具有有效陀螺數(shù)據(jù)的三軸穩(wěn)定的火箭彈是非常有用的。最后一種是一個(gè)旋轉(zhuǎn)的卡爾曼濾波器,它專門(mén)用于旋轉(zhuǎn)穩(wěn)定的火箭彈。三軸穩(wěn)定飛行器的姿態(tài)估計(jì)器已被廣泛應(yīng)用。而文中介紹的這個(gè)旋轉(zhuǎn)卡爾曼濾波器則是一種新型的濾波算法。以前對(duì)三軸磁傳感器的校準(zhǔn)算法都是假設(shè)旋轉(zhuǎn)方向是不變的(即在一批數(shù)據(jù)里他的旋轉(zhuǎn)方向是不變的),且不考慮力矩和章動(dòng)力的影響。而文中闡述的這種新型旋轉(zhuǎn)卡爾曼濾波算法則相對(duì)更靈活一些,可以應(yīng)用于姿態(tài)和角速度傳感器輸入數(shù)據(jù)不斷變化的情況。同時(shí)也會(huì)考慮到環(huán)境和控制力矩的影響。這種旋轉(zhuǎn)的卡爾曼估計(jì)器是一種采用了新型的狀態(tài)矢量的濾波算法,這個(gè)狀態(tài)矢量主要基于三部分:一個(gè)是在彈體系下的角動(dòng)量矢量,另一個(gè)是慣性系下的角動(dòng)量矢量,最后一個(gè)是旋轉(zhuǎn)角度。假定外力矩基本不變,這個(gè)角動(dòng)量是慢速變化的,因此所有的快速變化量就被隔離了,這種優(yōu)勢(shì)在濾波狀態(tài)進(jìn)程中是非常重要的,因?yàn)樵谡麄€(gè)計(jì)算過(guò)程中這種算法將使數(shù)值誤差變得很小。2.3姿態(tài)相關(guān)的準(zhǔn)估計(jì)問(wèn)題由以上步驟可獲得近似的姿態(tài)歷程,通過(guò)使殘差最小可以得到三軸磁傳感器對(duì)準(zhǔn)誤差。當(dāng)然姿態(tài)也存在誤差,這種誤差主要是由不完全校準(zhǔn)引起的,但是這種姿態(tài)誤差將隨著每一步的迭代計(jì)算而不斷減小。這種與姿態(tài)相關(guān)的方法用來(lái)求解給定姿態(tài)的對(duì)準(zhǔn)誤差。它是一個(gè)非常高效,可靠的四元數(shù)估計(jì)算法,可用來(lái)求解矢量匹配問(wèn)題以便使損失函數(shù)L最小化。L是一個(gè)關(guān)于姿態(tài)矩陣A的函數(shù):其中:vib為傳感器i所在的彈體坐標(biāo)系下觀測(cè)單位矢量;vir為相應(yīng)慣性系下的參考矢量;A為某時(shí)刻的姿態(tài)矩陣。同樣地,對(duì)姿態(tài)相關(guān)的對(duì)準(zhǔn)估計(jì)問(wèn)題,可以歸結(jié)為一個(gè)相似的形式,也就是:對(duì)一個(gè)給定的傳感器通過(guò)使損失函數(shù)最小來(lái)求解其對(duì)準(zhǔn)誤差其中,L(O)為正交對(duì)準(zhǔn)誤差O的函數(shù);Aj為已知的姿態(tài)歷程;vjb為給定傳感器觀測(cè)單位矢量。一旦O確定下來(lái),新的對(duì)準(zhǔn)誤差在傳感器坐標(biāo)系中通常表示為:其中:N0為校準(zhǔn)矩陣;M0為上一次的對(duì)準(zhǔn)誤差。本方法可能面臨的一個(gè)復(fù)雜性是三軸磁傳感器對(duì)準(zhǔn)誤差中的某一個(gè)分量OTAM不太好確定。這本身就表明了旋轉(zhuǎn)火箭彈關(guān)于旋轉(zhuǎn)軸的對(duì)準(zhǔn)誤差不如其他軸更容易觀測(cè)。這個(gè)問(wèn)題可以通過(guò)求解不相關(guān)的另一個(gè)傳感器的對(duì)準(zhǔn)誤差來(lái)得出。這個(gè)不相關(guān)的傳感器通常相對(duì)于旋轉(zhuǎn)軸具有較高靈敏度,如太陽(yáng)方位傳感器等。通過(guò)把對(duì)準(zhǔn)誤差項(xiàng)組合到一個(gè)單一有效的三軸磁傳感器對(duì)準(zhǔn)誤差中,然后兩個(gè)傳感器通過(guò)Osensor-2的逆矩陣進(jìn)行旋轉(zhuǎn)變換:這樣,三軸磁傳感器的對(duì)準(zhǔn)誤差就是相對(duì)于第二個(gè)傳感器的對(duì)準(zhǔn)誤差。這樣就可以將對(duì)準(zhǔn)誤差計(jì)算出來(lái)并應(yīng)用于每次迭代的數(shù)據(jù)當(dāng)中。這種用來(lái)確定不相關(guān)傳感器對(duì)準(zhǔn)誤差的方法根據(jù)第二個(gè)傳感器的類型而不同。如果在彈體坐標(biāo)系下測(cè)量的數(shù)據(jù)觀測(cè)范圍很寬時(shí),就用姿態(tài)估計(jì)法。但是當(dāng)觀測(cè)范圍很小或者在一個(gè)旋轉(zhuǎn)周期內(nèi)測(cè)量值變化較大時(shí)會(huì)使識(shí)別誤差變得很大。在這種情況下,就選用一個(gè)直接的方法來(lái)求解關(guān)于旋轉(zhuǎn)軸的對(duì)準(zhǔn)誤差。把三軸磁傳感器和不相關(guān)傳感器的觀測(cè)值以及他們的參考矢量都投影到垂直于旋轉(zhuǎn)軸的一個(gè)單位平面上,來(lái)找到從參考矢量到觀測(cè)矢量的平均轉(zhuǎn)角。這些平均的轉(zhuǎn)角就是關(guān)于旋轉(zhuǎn)軸的對(duì)準(zhǔn)誤差角。這樣通過(guò)每一次迭代,就可計(jì)算出一個(gè)新的姿態(tài)歷程,同時(shí)也可以計(jì)算出一個(gè)新的組合校準(zhǔn)誤差。當(dāng)組合校準(zhǔn)的改變量小于給定的誤差范圍時(shí),迭代即可停止。3基于反彈道德性模型的仿真模型假設(shè)三軸磁傳感器關(guān)于3個(gè)旋轉(zhuǎn)軸的誤差如表1所示。采用SIMULINK建立仿真模型,如圖1所示,當(dāng)火箭彈轉(zhuǎn)速為10r/s,彈道傾角在507)~07)之間變化時(shí),傳感器所測(cè)曲線如圖2所