楊小康,嚴(yán)恭敏,李四海
(西北工業(yè)大學(xué)自動(dòng)化學(xué)院,西安 710129)
慣性測量組合(IMU,Inertial Measurement Unit)是慣性導(dǎo)航系統(tǒng)的核心,IMU標(biāo)定的目的是確定其數(shù)學(xué)模型或誤差數(shù)學(xué)模型的參數(shù)[1],標(biāo)定結(jié)果直接影響導(dǎo)航精度[2]。目前常見的是捷聯(lián)慣性測量組合(SIMU),SIMU通常由三個(gè)陀螺和三個(gè)加速度計(jì)組成,陀螺和加速度計(jì)輸出中存在零偏誤差、刻度系數(shù)誤差、安裝誤差和隨機(jī)噪聲[3]。標(biāo)定分為分立式標(biāo)定和系統(tǒng)級(jí)標(biāo)定,前者根據(jù)各個(gè)陀螺和加速度計(jì)輸出與參考輸入間的關(guān)系進(jìn)行標(biāo)定,后者則是基于導(dǎo)航解算誤差原理,利用導(dǎo)航誤差估計(jì)慣性器件誤差[2]。相比于分立式標(biāo)定,系統(tǒng)級(jí)標(biāo)定可以實(shí)現(xiàn)外場標(biāo)定和慣導(dǎo)系統(tǒng)自標(biāo)定,標(biāo)定精度不依賴轉(zhuǎn)臺(tái)精度,不需要記錄陀螺和加速度計(jì)輸出數(shù)據(jù)[4]。無論是高精度SIMU還是消費(fèi)級(jí)SIMU都可以通過多位置轉(zhuǎn)動(dòng)實(shí)現(xiàn)基本誤差參數(shù)標(biāo)定[5]。SIMU標(biāo)定大多在實(shí)驗(yàn)室環(huán)境進(jìn)行,標(biāo)定時(shí)拆卸和搬運(yùn)設(shè)備給SIMU維護(hù)帶來不便[6],因此需要研究非實(shí)驗(yàn)室環(huán)境下外場標(biāo)定和自標(biāo)定技術(shù)。
外場環(huán)境的干擾運(yùn)動(dòng)使標(biāo)定量測不準(zhǔn)確,影響SIMU標(biāo)定結(jié)果,所以外場試驗(yàn)需要對(duì)標(biāo)定方法進(jìn)行改進(jìn)。對(duì)于低精度SIMU可以通過檢測轉(zhuǎn)動(dòng)過程中的靜止?fàn)顟B(tài),在手動(dòng)轉(zhuǎn)動(dòng)條件下完成標(biāo)定,試驗(yàn)表明此方法可以估計(jì)出陀螺和加速度計(jì)刻度系數(shù)誤差及安裝誤差[7]。外場標(biāo)定研究大多在陸上或者艦船系泊狀態(tài)下進(jìn)行[8,9],艦船錨泊條件下的研究尚未見相關(guān)文獻(xiàn),但研究艦船錨泊條件下SIMU外場標(biāo)定方法有很大的應(yīng)用價(jià)值。首先,大型艦艇的自持力不斷提升一般可以達(dá)到60天甚至更長,一次出海周期可達(dá)1年,期間沒有條件對(duì)SIMU進(jìn)行實(shí)驗(yàn)室環(huán)境標(biāo)定。其次,艦船上武器裝備種類多、數(shù)量大,對(duì)使用中低精度SIMU的戰(zhàn)術(shù)武器,簡單標(biāo)定后補(bǔ)償SIMU器件誤差能顯著提升其導(dǎo)航精度。
在地面環(huán)境進(jìn)行外場標(biāo)定時(shí)可保持SIMU靜止,轉(zhuǎn)位機(jī)構(gòu)使SIMU轉(zhuǎn)過不同角位置,利用濾波方法估計(jì)出SIMU器件誤差。但是艦船在水面上會(huì)受到風(fēng)浪等各種因素影響,即使在錨泊狀態(tài)也不是靜止不動(dòng)的。受到風(fēng)和海浪的影響,艦船會(huì)產(chǎn)生沿著船體坐標(biāo)系三個(gè)軸的線運(yùn)動(dòng)和角運(yùn)動(dòng)[10]。錨泊狀態(tài),船體在錨鏈力作用下還會(huì)出現(xiàn)偏蕩運(yùn)動(dòng)。偏蕩運(yùn)動(dòng)是錨泊船在風(fēng)、流、浪和錨鏈力的約束作用下,圍繞錨泊作用點(diǎn)產(chǎn)生的周期性低頻振蕩狀態(tài)[11]。因?yàn)殄^泊條件下艦船會(huì)受到錨鏈力和風(fēng)、浪、流等作用力的綜合影響,所以與系泊條件相比錨泊運(yùn)動(dòng)速度更大且規(guī)律性較差。進(jìn)行錨泊條件系統(tǒng)級(jí)標(biāo)定時(shí)無法消除錨泊運(yùn)動(dòng)的影響,因此錨泊條件下進(jìn)行系統(tǒng)級(jí)標(biāo)定難度更大。
本文針對(duì)在艦船錨泊和無其他導(dǎo)航系統(tǒng)輔助條件下進(jìn)行SIMU系統(tǒng)級(jí)標(biāo)定的問題,首先建立艦船錨泊條件下運(yùn)動(dòng)簡化模型,分析錨泊運(yùn)動(dòng)特點(diǎn),得到標(biāo)定時(shí)干擾運(yùn)動(dòng)的規(guī)律。然后根據(jù)慣導(dǎo)誤差方程建立捷聯(lián)慣導(dǎo)誤差狀態(tài)空間模型,設(shè)計(jì)卡爾曼濾波器,再根據(jù)錨泊運(yùn)動(dòng)干擾速度設(shè)置合適的濾波參數(shù)使SIMU誤差參數(shù)估計(jì)結(jié)果收斂、穩(wěn)定。最后進(jìn)行仿真,并比較靜基座和錨泊條件系統(tǒng)級(jí)標(biāo)定結(jié)果,檢驗(yàn)錨泊條件系統(tǒng)級(jí)標(biāo)定的效果。
艦船在水面運(yùn)動(dòng)時(shí),產(chǎn)生的橫蕩、縱蕩、升沉、艏搖、橫搖和縱搖都是周期性運(yùn)動(dòng),周期一般在1~30 s之間[10]。船體產(chǎn)生偏蕩運(yùn)動(dòng)時(shí),重心將描繪一個(gè)與風(fēng)向橫交的“∞”形軌跡。偏蕩運(yùn)動(dòng)的橫移幅度和船艏轉(zhuǎn)動(dòng)角度都會(huì)隨著風(fēng)速和鏈長增加而增加。圖1是一種真實(shí)的偏蕩運(yùn)動(dòng)軌跡[11],圖中O為偏蕩中心,Ox軸為橫蕩方向,Oy軸為縱蕩方向。為了方便分析,對(duì)偏蕩運(yùn)動(dòng)模型進(jìn)行簡化,以橫縱方向兩個(gè)垂直的周期運(yùn)動(dòng)來描述偏蕩運(yùn)動(dòng)。
圖1 一種偏蕩運(yùn)動(dòng)軌跡Fig.1 A trajectory of the anchoring motion
偏蕩運(yùn)動(dòng)模型包含橫向、縱向位移以及運(yùn)動(dòng)周期三個(gè)參數(shù),分別用sx、sy和T表示。簡化后偏蕩運(yùn)動(dòng)數(shù)學(xué)模型為
式中,x表示橫蕩方向位置坐標(biāo),y表示縱蕩方向位置坐標(biāo)。偏蕩距離sx與平均偏蕩速度關(guān)系為
偏蕩運(yùn)動(dòng)橫向平均速度的估算公式為
式中,vas表示風(fēng)速,Ba/Ld表示艦船水線上下側(cè)面積比,P/Lpp表示風(fēng)作用中心到船中心距離與兩柱間長之比,Lc/H表示鏈長與水深之比。根據(jù)上述參數(shù)計(jì)算出偏蕩運(yùn)動(dòng)平均速度,再由式(2)計(jì)算橫向運(yùn)動(dòng)幅度。艦船縱向偏蕩運(yùn)動(dòng)幅度小于橫向幅度,一般為橫向幅度的0.2~0.6倍,即
根據(jù)式(1)~(4)可以估算出偏蕩運(yùn)動(dòng)速度和位置。
在后文錨泊條件系統(tǒng)級(jí)標(biāo)定仿真中,假設(shè)偏蕩運(yùn)動(dòng)的橫蕩幅度為75 m,縱蕩幅度為30 m,偏蕩運(yùn)動(dòng)周期為600 s。偏蕩運(yùn)動(dòng)簡化模型軌跡如圖2所示。
圖2 簡化后偏蕩運(yùn)動(dòng)軌跡Fig.2 Simplified trajectory of the anchoring motion
系統(tǒng)級(jí)標(biāo)定常用卡爾曼濾波,將SIMU誤差參數(shù)作為狀態(tài)量。陀螺輸出角速度誤差和加速度計(jì)輸出比力誤差為
式中,δKg和δKa分別是陀螺和加速度計(jì)刻度系數(shù)誤差矩陣,εb和 ?b分別是陀螺和加速度計(jì)常值零偏。δKg和δKa均是3階方陣,由于存在安裝誤差角約束,只需要估計(jì)兩個(gè)矩陣中的9個(gè)安裝誤差。所以在誤差模型中設(shè)定
后文用δkgii和δkaii表示刻度系數(shù)誤差,δkgij和δkaij(i≠j)表示安裝誤差。
根據(jù)捷聯(lián)慣導(dǎo)誤差方程,選取卡爾曼濾波狀態(tài)空間模型30維狀態(tài)向量為
式中,φ=[φEφNφU]T為姿態(tài)失準(zhǔn)角,δvn為導(dǎo)航系下速度誤差,δp=[δLδλ δh]T為導(dǎo)航位置誤差(三個(gè)分量依次為緯度誤差、經(jīng)度誤差和高度誤差),Xg和Xa分別為陀螺和加速度計(jì)刻度系數(shù)誤差矩陣中元素組成的列向量
根據(jù)捷聯(lián)慣導(dǎo)誤差方程和式(8)給出的狀態(tài)向量,構(gòu)建濾波器系統(tǒng)方程
式中,F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣,G為系統(tǒng)噪聲分配矩陣,W為陀螺和加速度計(jì)輸出中的白噪聲向量。F中各個(gè)元素根據(jù)捷聯(lián)慣導(dǎo)誤差方程確定。
靜基座系統(tǒng)級(jí)標(biāo)定時(shí)SIMU速度為零且位置不變,所以可以選擇零速度和初始位置作為導(dǎo)航結(jié)果基準(zhǔn),以此構(gòu)建量測方程
式中,vn表示慣導(dǎo)輸出導(dǎo)航系速度,p=[Lλh]T為慣導(dǎo)輸出位置向量(緯度、經(jīng)度和高度),p0表示初始位置,觀測矩陣H=[06×3I6×606×21],V為量測噪聲。
標(biāo)定過程中還需要通過狀態(tài)可觀測性分析,確定合適的轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)方案,使?fàn)顟B(tài)向量中SIMU誤差參數(shù)全部可觀。表1列出了一組包含15次轉(zhuǎn)動(dòng)的轉(zhuǎn)動(dòng)編排方案[12],初始時(shí)刻SIMU坐標(biāo)系“X-Y-Z”軸指向“東-北-天”。靜基座系統(tǒng)級(jí)標(biāo)定實(shí)驗(yàn)中每個(gè)位置通常靜止60s,動(dòng)基座標(biāo)定實(shí)驗(yàn)中可根據(jù)標(biāo)定條件增加每個(gè)位置靜止時(shí)間。
表1 系統(tǒng)級(jí)標(biāo)定轉(zhuǎn)動(dòng)編排Tab.1 Rotation plan of system-level calibration
按照?qǐng)D3所示在三軸轉(zhuǎn)臺(tái)上安裝SIMU,依表1給出的轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)方案進(jìn)行一組15位置轉(zhuǎn)動(dòng),每次轉(zhuǎn)動(dòng)后靜止一段時(shí)間。利用標(biāo)定過程的SIMU輸出完成卡爾曼濾波就可以估計(jì)出SIMU誤差參數(shù)。
圖3 系統(tǒng)級(jí)標(biāo)定SIMU安裝示意圖Fig.3 SIMU install sketch of system-level calibration
系統(tǒng)級(jí)標(biāo)定中,根據(jù)式(11)和式(12)可得離散化狀態(tài)空間模型
式中,Φk/k-1為狀態(tài)一步轉(zhuǎn)移矩陣,Γk/k-1為系統(tǒng)噪聲分配矩陣,Hk為觀測矩陣;Wk為系統(tǒng)噪聲,Vk為量測噪聲,兩者互不相關(guān),且。因?yàn)殪o基座下量測準(zhǔn)確,Rk設(shè)置較小的值即可,而動(dòng)基座標(biāo)定算法中,需要根據(jù)運(yùn)動(dòng)特點(diǎn)選取合適的量測噪聲參數(shù)。
在錨泊條件下,艦船速度和位置不斷變化,在無其他導(dǎo)航系統(tǒng)輔助的條件下無法得到準(zhǔn)確的慣導(dǎo)誤差。如果用慣導(dǎo)速度和位置作為濾波量測,則錨泊運(yùn)動(dòng)的速度和位置都會(huì)影響SIMU誤差參數(shù)的估計(jì)精度,所以錨泊條件系統(tǒng)級(jí)標(biāo)定只用速度誤差作為量測,在算法設(shè)計(jì)時(shí)只需考慮錨泊運(yùn)動(dòng)速度干擾的影響,便于濾波器量測噪聲參數(shù)設(shè)置。在錨泊條件下,SIMU輸出速度為
式中,vn為慣導(dǎo)系統(tǒng)輸出速度,為SIMU器件誤差激勵(lì)出的速度誤差,vw為艦船短周期振蕩速度,va為艦船錨泊偏蕩運(yùn)動(dòng)速度,Vk為量測噪聲。記,則量測向量為
在卡爾曼濾波更新中,濾波增益更新為
狀態(tài)更新公式為
根據(jù)式(15)和式(17),錨泊條件系統(tǒng)級(jí)標(biāo)定的狀態(tài)更新公式為
式中,由錨泊運(yùn)動(dòng)引起的KkZa,k會(huì)影響系統(tǒng)級(jí)標(biāo)定狀態(tài)更新結(jié)果。
按照靜基座系統(tǒng)級(jí)標(biāo)定參數(shù)設(shè)置經(jīng)驗(yàn),量測噪聲方差陣一般取值較小,按式(16)和式(18)計(jì)算卡爾曼濾波過程狀態(tài)更新時(shí),速度誤差量測向量中錨泊運(yùn)動(dòng)產(chǎn)生的干擾會(huì)影響SIMU誤差參數(shù)估計(jì)。以陀螺和加速度計(jì)安裝誤差角為例,當(dāng)速度量測誤差設(shè)置為0.01m/s時(shí)進(jìn)行不同條件的4組錨泊條件系統(tǒng)級(jí)標(biāo)定,補(bǔ)償后的兩個(gè)誤差角δkg31和δka31如圖4和圖5所示。由于量測誤差小,濾波時(shí)錨泊運(yùn)動(dòng)干擾速度會(huì)被錯(cuò)誤地引入慣導(dǎo)系統(tǒng)誤差估計(jì)中,使得誤差參數(shù)估計(jì)結(jié)果隨錨泊運(yùn)動(dòng)波動(dòng),影響誤差參數(shù)估計(jì)。
圖4 系統(tǒng)級(jí)標(biāo)定過程 δkg31的變化Fig.4 Plot of δkg31in system-level calibration process
圖5 系統(tǒng)級(jí)標(biāo)定過程 δka31的變化Fig.5 Plot of δka31in system-level calibration process
根據(jù)以上分析,錨泊條件系統(tǒng)級(jí)標(biāo)定時(shí)應(yīng)當(dāng)按照錨泊運(yùn)動(dòng)速度大小設(shè)置Rk,以減小干擾速度的影響。可以設(shè)置速度量測噪聲方差為最大錨泊運(yùn)動(dòng)速度k倍的平方,即
式中,diag(v)表示向量v作為對(duì)角元素構(gòu)成的對(duì)角陣,Za,max表示錨泊運(yùn)動(dòng)最大速度,參數(shù)k可取不同值進(jìn)行錨泊條件系統(tǒng)級(jí)標(biāo)定仿真,根據(jù)仿真結(jié)果確定合適的數(shù)值。假設(shè)艦船錨泊狀態(tài)下兩個(gè)水平速度波動(dòng)幅度均為0.6 m/s,垂向速度波動(dòng)幅度為0.2 m/s。設(shè)置不同的慣導(dǎo)誤差參數(shù),取k為0.001、0.01、0.1和1分別進(jìn)行20次系統(tǒng)級(jí)標(biāo)定蒙特卡洛仿真,得到SIMU各類誤差項(xiàng)標(biāo)定補(bǔ)償后殘差的最大值,如表2所列。
表2 不同量測噪聲方差下的系統(tǒng)級(jí)標(biāo)定結(jié)果Tab.2 Calibration result with different variance of measurement noise
表2中所示的各標(biāo)定誤差為同類誤差的最大值。根據(jù)表2中的仿真結(jié)果,錨泊條件系統(tǒng)級(jí)標(biāo)定時(shí)應(yīng)取k= 0.1,按0.1倍最大錨泊速度設(shè)置速度量測噪聲方差陣得到的標(biāo)定結(jié)果比較準(zhǔn)確,且濾波器狀態(tài)估計(jì)值不隨干擾運(yùn)動(dòng)波動(dòng)。為了在SIMU標(biāo)定時(shí)得到更準(zhǔn)確的標(biāo)定結(jié)果,還可以在0.1附近繼續(xù)取值進(jìn)行仿真,尋找更優(yōu)的量測噪聲參數(shù)。
進(jìn)行艦船錨泊條件系統(tǒng)級(jí)標(biāo)定實(shí)驗(yàn)時(shí),可根據(jù)海上風(fēng)、浪和艦船特征等參數(shù)估算出錨泊運(yùn)動(dòng)幅度和周期,然后建立海上錨泊運(yùn)動(dòng)模型并進(jìn)行仿真分析,確定出合適的量測噪聲方差參數(shù),以得到錨泊干擾下最準(zhǔn)確的SIMU誤差參數(shù)標(biāo)定結(jié)果。
仿真中設(shè)置4組不同的SIMU誤差參數(shù),每組仿真5次,誤差參數(shù)如表3所列。以每一類誤差標(biāo)定補(bǔ)償后殘差的最大值來進(jìn)行標(biāo)定結(jié)果比較。
表3 SIMU誤差參數(shù)表Tab.3 Error parameter table of SIMU
為了方便比較,分別進(jìn)行靜基座條件和錨泊條件兩組系統(tǒng)級(jí)標(biāo)定仿真。靜基座系統(tǒng)級(jí)標(biāo)定根據(jù)表1的方案進(jìn)行轉(zhuǎn)動(dòng),標(biāo)定時(shí)間為1400 s,標(biāo)定結(jié)果如表4所列,經(jīng)過標(biāo)定補(bǔ)償后捷聯(lián)慣導(dǎo)誤差中陀螺零偏數(shù)值較大,其余誤差顯著降低。
表4 靜基座系統(tǒng)級(jí)標(biāo)定誤差Tab.4 Result of system-level calibration on stationary base
錨泊條件動(dòng)基座系統(tǒng)級(jí)標(biāo)定仿真時(shí)按照第1節(jié)中的簡化偏蕩運(yùn)動(dòng)模型進(jìn)行艦船偏蕩運(yùn)動(dòng)仿真。同時(shí)加入幅值為7°的橫搖運(yùn)動(dòng),15°的縱搖運(yùn)動(dòng),0.25m的橫蕩和縱蕩,以及0.5m的升沉運(yùn)動(dòng),以上運(yùn)動(dòng)的周期為10s。由于錨泊條件系統(tǒng)級(jí)標(biāo)定中量測噪聲方差較大,量測中存在錨泊運(yùn)動(dòng)干擾,因此與靜基座條件相比量測向量中有效信息較少。延長標(biāo)定時(shí)間可增加量測信息,且慣導(dǎo)誤差增大可以提升量測中慣導(dǎo)誤差占比,所以錨泊條件仿真時(shí)延長標(biāo)定時(shí)間至2800s。20組錨泊條件仿真中慣導(dǎo)誤差標(biāo)定后殘差變化如圖6~12所示,圖中左側(cè)為標(biāo)定全程的估計(jì)殘差變化,右側(cè)為最后800 s的變化曲線,標(biāo)定結(jié)果見表5。
圖6 系統(tǒng)級(jí)標(biāo)定陀螺零偏誤差Fig.6 Calibration result of gyroscope bias error
圖7 系統(tǒng)級(jí)標(biāo)定加速度計(jì)零偏誤差Fig.7 Calibration result of accelerometer bias error
圖8 系統(tǒng)級(jí)標(biāo)定陀螺刻度系數(shù)誤差Fig.8 Calibration result of gyroscope scale factor error
圖9 系統(tǒng)級(jí)標(biāo)定加速度計(jì)刻度系數(shù)誤差Fig.9 Calibration result of accelerometer scale factor error
圖10 系統(tǒng)級(jí)標(biāo)定陀螺安裝誤差Fig.10 Calibration result of gyroscope misalignment error
圖11 系統(tǒng)級(jí)標(biāo)定陀螺安裝誤差Fig.11 Calibration result of gyroscope misalignment error
圖12 系統(tǒng)級(jí)標(biāo)定加速度計(jì)安裝誤差Fig.12 Calibration result of accelerometer misalignment error
表5 錨泊條件系統(tǒng)級(jí)標(biāo)定仿真結(jié)果Tab.5 Result of the system-level calibration under anchoring condition
從圖6~12可以看出,在不同仿真條件下SIMU誤差參數(shù)估計(jì)結(jié)果都是收斂的,雖然最后階段受錨泊運(yùn)動(dòng)影響部分結(jié)果依然有波動(dòng),但是都穩(wěn)定在小量范圍內(nèi)。根據(jù)表5中錨泊條件系統(tǒng)級(jí)標(biāo)定結(jié)果可以得出結(jié)論:陀螺零偏標(biāo)定結(jié)果較差,難以通過動(dòng)基座系統(tǒng)級(jí)標(biāo)定補(bǔ)償SIMU陀螺零偏;加速度計(jì)零偏標(biāo)定結(jié)果較準(zhǔn)確,標(biāo)定誤差小于 1× 10-5g;刻度系數(shù)誤差標(biāo)定結(jié)果一般,陀螺刻度系數(shù)誤差標(biāo)定誤差小于29.0 ppm,加速度計(jì)小于20.8 ppm;SIMU安裝誤差角標(biāo)定比較準(zhǔn)確,陀螺和加速度計(jì)安裝誤差角的標(biāo)定誤差分別小于10.0''和9.3''。
SIMU外場標(biāo)定技術(shù)應(yīng)用廣泛,可以利用卡爾曼濾波估計(jì)SIMU誤差參數(shù),但外場標(biāo)定大多都是在地面環(huán)境或艦船系泊狀態(tài)下進(jìn)行,現(xiàn)有標(biāo)定方法尚不能進(jìn)行艦船錨泊條件下SIMU外場標(biāo)定。
本文分析艦船錨泊條件運(yùn)動(dòng)特點(diǎn),提出一種錨泊條件下SIMU系統(tǒng)級(jí)標(biāo)定方法。此方法可以在艦船錨泊條件下完成SIMU誤差參數(shù)標(biāo)定,補(bǔ)償SIMU的加速度計(jì)零偏、陀螺和加速度計(jì)刻度系數(shù)誤差以及陀螺和加速度計(jì)安裝誤差角。仿真結(jié)果表明,錨泊條件下系統(tǒng)級(jí)標(biāo)定方法可以補(bǔ)償SIMU中大部分誤差,提升導(dǎo)航精度。錨泊條件系統(tǒng)級(jí)標(biāo)定方法可以在艦船長時(shí)間出海期間,不依賴其他導(dǎo)航系統(tǒng)實(shí)現(xiàn)艦上SIMU系統(tǒng)級(jí)標(biāo)定,提升SIMU導(dǎo)航精度,有一定的應(yīng)用參考價(jià)值。