自適應(yīng)互補(bǔ)濾波水下高度測量技術(shù)

張海鵬 李杰 張波 胡陳君 姜海洋

摘要：針對傳統(tǒng)利用GPS，溫度、聲吶、壓力等手段，對水下高度測量容易受洋流、太陽輻射、季節(jié)變化等海洋環(huán)境干擾因素的影響而導(dǎo)致測量局限性、精度低的問題，設(shè)計一種基于自適應(yīng)互補(bǔ)濾波的水下高度測量系統(tǒng)。該系統(tǒng)以IMU為信息采集單元，搭建以FPGA+DSP為架構(gòu)的信息處理解算平臺，結(jié)合自適應(yīng)補(bǔ)償系數(shù)的互補(bǔ)濾波算法，在低通濾波環(huán)節(jié)加入PI控制器，依據(jù)加速度計測得的姿態(tài)數(shù)據(jù)判斷水下彈體的運(yùn)動情況，實(shí)時調(diào)整PI參數(shù)，實(shí)現(xiàn)水下系統(tǒng)的高度測量。試驗結(jié)果表明：該水下高度測量系統(tǒng)操作方便、獨(dú)立性實(shí)時性好、測量精度高，具有一定的工程應(yīng)用價值。

關(guān)鍵詞：自適應(yīng);互補(bǔ)濾波;水下;高度測量

中圖分類號：TN108.7 文獻(xiàn)標(biāo)志碼：A文章編號1674-5124（2019）07-0046-05

收稿日期：2018-12-08;收到修改稿日期：2019-01-18

基金項目：國家自然科學(xué)基金項目（51575500）

作者簡介：張海鵬（1992-），男，山西忻州市人，碩士研究生，專業(yè)方向為微系統(tǒng)集成、慣性測量。

0 引言

地球表面積約7o%。被海洋覆蓋，有著豐富的資源和能源，對國家政治、經(jīng)濟(jì)、安全等方面有著重大意義，水下彈體的發(fā)射多以冷發(fā)射的方式彈射出倉，發(fā)射過程中彈體姿態(tài)、高度等的測量現(xiàn)已成為國內(nèi)外水下彈體發(fā)射技術(shù)研究的一個重要方向。文獻(xiàn)[1]中用多波束聲吶測深系統(tǒng)，以傳統(tǒng)的掃床方式對一定深度水域進(jìn)行反復(fù)掃測，能夠詳細(xì)反映所測區(qū)域水下的地形情況，但其應(yīng)用范圍易受水下地形的影響而具有局限性，且易受到水深和波束角的限制。文獻(xiàn)[2]利用側(cè)掃聲吶實(shí)時數(shù)據(jù)采集與地貌圖像構(gòu)建方法，雖然可以通過聲學(xué)成像技術(shù)描述海底形態(tài)，但在計算海底高度時，是根據(jù)影子長度進(jìn)行估計的，對數(shù)據(jù)解譯人員的要求很高。文獻(xiàn)[3]提到的GPS高程測量，利用全球定位系統(tǒng)測量技術(shù)可直接測定地面某點(diǎn)的高程，且基于GPS的定位與測高系統(tǒng)在陸地覆蓋廣泛、精度高，但其在水下信號較弱甚至無法使用?；跍囟取⒓す?、壓力等的測高系統(tǒng)極易受到洋流、太陽輻射、季節(jié)變化等海洋環(huán)境干擾因素的影響而使測量精度低、準(zhǔn)確性差。該水下測高系統(tǒng)以IMU為核心信息采集單元，搭建基于FPGA與DSP的硬件電路平臺，結(jié)合自適應(yīng)互補(bǔ)濾波算法，依據(jù)水下彈體的運(yùn)動情況，實(shí)時動態(tài)調(diào)整P1參數(shù)，利用對所得姿態(tài)數(shù)據(jù)的解算得到水下彈體的高度信息，實(shí)現(xiàn)對水下彈體的實(shí)時高度測量，有效提高了測量準(zhǔn)確度，實(shí)用性高。

1 姿態(tài)描述

彈體在水下運(yùn)動過程中，需要建立坐標(biāo)系來描述其運(yùn)動姿態(tài)和所處位置。本文建立以彈體為參考的載體坐標(biāo)系（b系），建立以所在地地理位置為參考的導(dǎo)航坐標(biāo)系（n系），其中導(dǎo)航坐標(biāo)系的X、Y、Z三軸分別對應(yīng)當(dāng)?shù)氐臇|、北和天方位。慣性測量單元IMU固連在彈體上，基于慣性傳感器所得彈體的姿態(tài)和位置信息都是基于導(dǎo)航坐標(biāo)系進(jìn)行測量和計算的，系統(tǒng)姿態(tài)由俯仰角θ、橫滾角γ、偏航角φ來描述[4-6]。導(dǎo)航坐標(biāo)系與載體坐標(biāo)系之間的坐標(biāo)關(guān)系及彈體水下運(yùn)動模擬圖如圖1所示。

導(dǎo)航坐標(biāo)系保持不變，在解算彈體姿態(tài)過程中，將載體坐標(biāo)系獲得的位置和姿態(tài)數(shù)據(jù)轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下進(jìn)行計算。坐標(biāo)變換過程中，根據(jù)兩個坐標(biāo)系之間的位置關(guān)系，導(dǎo)航坐標(biāo)系經(jīng)過3次旋轉(zhuǎn)變換即可得到載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的角位置關(guān)系。3次旋轉(zhuǎn)變換分別為：繞導(dǎo)航系Z軸旋轉(zhuǎn)得到偏航角φ、繞Y軸得到俯仰角。、繞X軸得到滾轉(zhuǎn)角γ。坐標(biāo)系之間的關(guān)系可用旋轉(zhuǎn)矩陣C_n^b表示降[7-9]，表達(dá)式為

在求解彈體在水下運(yùn)動的姿態(tài)時，為防止求解的結(jié)果出現(xiàn)奇點(diǎn)，本文采用解算過程中不產(chǎn)生奇點(diǎn)的四元數(shù)法。四元數(shù)計算過程相對簡單，不需要大量計算，滿足彈體姿態(tài)求解要求。

定義q為一個常規(guī)四元數(shù)向量，數(shù)學(xué)公式為

q=q₀+q₁i+q₂j+q₃k（2）其中q₀、q₁、q₂、q₃為實(shí)數(shù)，i、j、k為兩兩正交的向量。當(dāng)彈體在水下運(yùn)動，載體坐標(biāo)系與導(dǎo)航坐標(biāo)系的坐標(biāo)變換用四元數(shù)法時，其變化過程可看作是一次無中間過程的等效旋轉(zhuǎn)，而這個變化過程可以由四元數(shù)向量表示。由b系變換到R系的變換矩陣用四元數(shù)表示為C_b^R，其公式表述為

且由C_n^b=C_b^R歐拉角與四元數(shù)的關(guān)系，可得俯仰角。、偏航角φ、滾轉(zhuǎn)角γ的數(shù)學(xué)表述公式：

2 互補(bǔ)濾波及彈體高度測量算法

2.1 互補(bǔ)濾波

水下彈體高度測量系統(tǒng)是以慣性測量單元為信息采集單元的捷聯(lián)慣導(dǎo)系統(tǒng)，該信息采集單元包括傳感器MEMS陀螺儀和加速度計。其中陀螺儀能夠測量彈體在水下運(yùn)動時的角速度，加速度計能夠測量彈體在水下運(yùn)動時的三軸方向的加速度。由慣性測量單元采集得到水下彈體的角速度和加速度數(shù)據(jù)都可以通過坐標(biāo)變換和積分計算處理，得到水下彈體運(yùn)動所需的姿態(tài)和位置信息。陀螺儀在測量姿態(tài)和位置信息時，其動態(tài)響應(yīng)特性較好而靜態(tài)特性差，當(dāng)被測物體處在高速運(yùn)動過程中，其所測得的姿態(tài)數(shù)據(jù)和動態(tài)位置精度較高，但是由于陀螺儀存在溫漂和零漂，當(dāng)被測物體在低速運(yùn)動或靜止時，陀螺儀響應(yīng)特性較差。與陀螺儀特性相反，加速度計在測量姿態(tài)和位置信息時，其動態(tài)響應(yīng)特性較差而靜態(tài)特性好。當(dāng)被測物體靜止或低速運(yùn)動時，其能測得相對精確的姿態(tài)和較為穩(wěn)定的位置信息。

因此陀螺儀和加速度計兩者之間的動態(tài)響應(yīng)特性相互補(bǔ)充，互補(bǔ)濾波就是充分利用這一點(diǎn)將所得的數(shù)據(jù)進(jìn)行融合，進(jìn)而得到高精度的彈體姿態(tài)和位置信息?；パa(bǔ)濾波原理如圖2所示。

設(shè)f₁（S）=K/（s+K）為低通濾波器，f₂（s）=s/（s+K）為高通濾波器，其中1/s為濾波器截止頻率，頻率高于1/s的波形通過低通濾波器時，將被濾除，而在高通濾波器中則可以通過。加速度計所測得數(shù)據(jù)通過低通濾波器時，可以濾除高頻抖動信息。而陀螺儀所測得姿態(tài)信息通過高通濾波器時，由漂移導(dǎo)致的累積誤差將被濾除。因此在測量過程中，陀螺儀和加速度計分別在高頻段和低頻段工作。

2.2 自適應(yīng)互補(bǔ)濾波

在測量和計算俯仰角。時，低通濾波器和高通濾波器傳遞函數(shù)分別為f₁（s）和f₂（s），且f₁（s）+f₂（s）=1。則根據(jù)互補(bǔ)濾波原理可表示為其中θ為根據(jù)式（4）計算得到的姿態(tài)角預(yù)估值。通過式（5）利用互補(bǔ)濾波能同時消除高頻和低頻干擾，實(shí)現(xiàn)姿態(tài)數(shù)據(jù)的融合。濾波過程中，需要合理選取參數(shù)K，才能實(shí)現(xiàn)較好的濾波。由于測量和濾波過程中存在較大噪聲，且互補(bǔ)濾波低通阻帶衰減速度較慢，會導(dǎo)致較大誤差。因此為減小誤差，在以上基礎(chǔ)上增加了PI控制器。在測量的不同階段，根據(jù)物體運(yùn)動速度來動態(tài)調(diào)整補(bǔ)償系數(shù)K_p、K_i。以上經(jīng)過改良的濾波器為自適應(yīng)互補(bǔ)濾波器[10-12]，其測量和計算所得的位置和姿態(tài)數(shù)據(jù)精度相對較高。自適應(yīng)互補(bǔ)濾波原理如圖3所示。

在自適應(yīng)互補(bǔ)濾波器中，由加速度計解算得到的姿態(tài)角預(yù)估值θ，通過公式（5）實(shí)現(xiàn)加速度計姿態(tài)角θ與陀螺儀角速率ω數(shù)據(jù)信息融合，從而得到當(dāng)前姿態(tài)信息。在處理過程中，將融合所得數(shù)據(jù)反饋給PI控制器，實(shí)時更新得到新的姿態(tài)角。

2.3 水下彈體的運(yùn)動高度測量

通過式（1）得到C_nb，且利用坐標(biāo)變化矩陣將加速度計輸出的水下彈體運(yùn)動在載體系中的比力信息轉(zhuǎn)換到導(dǎo)航坐標(biāo)系中，同時進(jìn)行有害重力加速度補(bǔ)償，得到導(dǎo)航系中彈體運(yùn)動的加速度方程為式中：f^b——載體系中加速度計測得的比力信息;

ω_enⁿ——掛囪球自轉(zhuǎn)角速度在導(dǎo)航坐標(biāo)系的值，rad/s;

ω_ien——導(dǎo)航坐標(biāo)系相對地球坐標(biāo)系旋轉(zhuǎn)的角

速度在導(dǎo)航坐標(biāo)系的投影值，rad/s;

gⁿ——重力力口速度在導(dǎo)航坐標(biāo)系的投影值，m/s²。

將式（6）的比力方程寫成矩陣形式為式中：L——當(dāng)?shù)鼐暥?，（°?

U_x、V_y、V_z——三維速度在地理坐標(biāo)系下的分值，對應(yīng)東、北、天三個方向的速度大小，m/s;

R_e、R_n——卯酉圈和子午圈的曲率徑，m^-1。

利用公式（1）所得姿態(tài)數(shù)據(jù)及公式（7）得到水下彈體在地理坐標(biāo)系下的天向速度V_z，進(jìn)而可得水下彈體運(yùn)動位置高度H：式中：H₀——初始位置高度，m;

t——彈體運(yùn)動時間，s。

3 系統(tǒng)硬件平臺設(shè)計

系統(tǒng)結(jié)構(gòu)框圖如圖4所示，本文搭建了基于FPGA與DSP的硬件電路平臺，主要由慣性測量模塊（三軸陀螺儀、三軸加速度計、信號調(diào)理單元等）、參數(shù)設(shè)置模塊（外部觸發(fā)單元、通信接口單元等）、MCU主控模塊（信號采集單元、數(shù)據(jù)解算單元、數(shù)據(jù)存儲單元、數(shù)據(jù)輸出單元等）組成。

其中，三軸陀螺儀與三軸加速度計組合構(gòu)成姿態(tài)傳感器，測試水下模型發(fā)射后的三維加速度信息和對三維角速率信息并利用MCU對采集信息進(jìn)行實(shí)時解算與存儲。主控芯片采用XILINX公司生產(chǎn)的Spartan-Ⅱ系列的XC2S100，該FPGA內(nèi)核采用2.5V低電壓供電，工作頻率最高達(dá)200MHz，擁有4個時鐘輸入端，176個I/O口，10×10⁴個門電路，600個可編程邏輯塊，內(nèi)部集成10個雙口RAM，共40kbits，可以實(shí)現(xiàn)系統(tǒng)中數(shù)據(jù)的高速緩存;同時該芯片具有豐富的門陣列資源、靈活的可配置性和良好的抗干擾性，可以實(shí)現(xiàn)復(fù)雜的邏輯控制。

信號調(diào)理模塊采用德州儀器公司的軌至軌運(yùn)算放大器OPA4340，增強(qiáng)模擬信號的驅(qū)動能力;模數(shù)轉(zhuǎn)換芯片采用TI公司的ADS8365實(shí)現(xiàn)6路傳感器信號的同步采集，該型A/D轉(zhuǎn)換器具備16位采集精度、高達(dá)250kS/s的采樣率、并口數(shù)據(jù)傳輸及單5V供電的特點(diǎn)，能夠?qū)崿F(xiàn)6通道模擬信號的同步采集，系統(tǒng)硬件電路如圖5所示。

4 實(shí)驗分析及算法驗證

如圖6所示，本節(jié)采用實(shí)際車載實(shí)驗利用高精度定位定姿系統(tǒng)輸出的姿態(tài)角和位置信息作為標(biāo)準(zhǔn)，對系統(tǒng)姿態(tài)角和位置信息的正確性與準(zhǔn)確性進(jìn)行驗證與評估。高精度定位定姿系統(tǒng)為加拿大NovAtel公司高精度光纖組合導(dǎo)航系統(tǒng)span-lci，其姿態(tài)角準(zhǔn)確度為0.008°、速度準(zhǔn)確度為0.03m/s、位置準(zhǔn)確度為0.1m。

基于上述硬件平臺，實(shí)驗場地選取東西向空曠平直路段，將硬件電路系統(tǒng)朝小車前進(jìn)方向水平放置模擬彈體在水下運(yùn)動時的上升過程，速度在5m/s以內(nèi)，采樣頻率為100Hz。在此實(shí)驗基礎(chǔ)上對實(shí)驗數(shù)據(jù)進(jìn)行存儲采集和互補(bǔ)濾波、自適應(yīng)互補(bǔ)濾波算法處理，以及同高精度定位定姿系統(tǒng)數(shù)據(jù)進(jìn)行對比，實(shí)驗數(shù)據(jù)經(jīng)互補(bǔ)濾波、自適應(yīng)互補(bǔ)濾波算法處理后與高精度定位定姿系統(tǒng)所得姿態(tài)角、位置對比情況如圖7、圖8所示。

通過圖7和圖8可以看出，互補(bǔ)濾波線條波動幅度較大，誤差較大，姿態(tài)及位置推算效果較差。自適應(yīng)互補(bǔ)濾波誤差明顯低于互補(bǔ)濾波，且響應(yīng)速度快，對噪聲的過濾效果更好，線條波動小，且通過對數(shù)據(jù)實(shí)際計算得，自適應(yīng)互補(bǔ)濾波算法計算結(jié)果相比傳統(tǒng)互補(bǔ)濾波方法，誤差有所下降，其中俯仰角平均誤差下降了58.4%、偏航角的平均誤差下降了64.7%、滾轉(zhuǎn)角的平均誤差下降了73.6%、位置推算的平均誤差下降了65.8%。在準(zhǔn)確計算姿態(tài)角的基礎(chǔ)上進(jìn)行位移推算，與高精度定位定姿系統(tǒng)精度較為接近。

5 結(jié)束語

針對現(xiàn)有幾種水下高度測量方法中存在的問題，設(shè)計了基于自適應(yīng)互補(bǔ)濾波的水下高度實(shí)時測量系統(tǒng)。對傳統(tǒng)的互補(bǔ)濾波方法進(jìn)行改進(jìn)，在低通濾波環(huán)節(jié)加入PI控制器，依據(jù)加速度計測得的姿態(tài)數(shù)據(jù)判斷水下彈體的運(yùn)動情況，得到實(shí)時更新的姿態(tài)數(shù)據(jù)信息，進(jìn)而實(shí)現(xiàn)載體高度的高精度解算。搭建了以FPGA+DSP為核心的信息處理解算硬件電路平臺，且通過車載實(shí)驗與高精度定位定姿系統(tǒng)進(jìn)行實(shí)際對比實(shí)驗驗證，驗證表明，該系統(tǒng)以慣性單元為信息采集單元，獨(dú)立性、抗干擾能力好，且相比傳統(tǒng)互補(bǔ)濾波算法，測量精度高、實(shí)時性好。

參考文獻(xiàn)

[1]胡玗晗.基于多波束聲吶系統(tǒng)的水深測量技術(shù)研究[J].科技資訊，2018，16（19）：36-37.

[2]王麗娜.側(cè)掃聲吶數(shù)據(jù)采集與地貌圖像構(gòu)建[J].北京測繪，2018，32（8）：965-969.

[3]楊春，劉遠(yuǎn)征，蔡文生.GPS高程測量及在水利測繪工程中的應(yīng)用[J].工程技術(shù)研究，2018（6）：83-84.

[4]羅莉.水下航行器捷聯(lián)慣導(dǎo)系統(tǒng)粗對準(zhǔn)方法研究[D].哈爾濱：哈爾濱工程大學(xué)，2017.

[5]張濤，石宏飛，陳立平，等.基于UKF的SINS/LBL水下AUV緊組合定位技術(shù)[J].中國慣性技術(shù)學(xué)報，2016，24（5）：638-642.

[6]黃浩乾.水下滑翔器慣性組合導(dǎo)航定位關(guān)鍵技術(shù)研究[D].南京：東南大學(xué)，2015.

[7]陳孟元，謝義建，陳躍東.基于四元數(shù)改進(jìn)型互補(bǔ)濾波的MEMS姿態(tài)解算[J].電子測量與儀器學(xué)報，2015，29（9）：1391-1397.

[8]李向楠.水下姿態(tài)解算的算法研究[D].太原：中北大學(xué)，2014.

[9]孔祥斌.某產(chǎn)品水下運(yùn)動姿態(tài)測量系統(tǒng)設(shè)計[D].太原：中北大學(xué)，2014.

[10]余星.基于捷聯(lián)慣性的水下載體姿態(tài)檢測系統(tǒng)研制[D].杭州：杭州電子科技大學(xué)，2013.

[11]何東旭.AUV水下導(dǎo)航系統(tǒng)關(guān)鍵技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué)，2013.

[12]陳雷，卞鴻巍.基于模糊自適應(yīng)互補(bǔ)濾波的姿態(tài)解算算法[J].兵器裝備工程學(xué)報，2017，38（4）：183-187.

（編輯：商丹丹）