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

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

關鍵詞：自適應;互補濾波;水下;高度測量

中圖分類號：TN108.7 文獻標志碼：A文章編號1674-5124（2019）07-0046-05

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

基金項目：國家自然科學基金項目（51575500）

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

0 引言

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

1 姿態(tài)描述

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

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

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

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

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

且由C_n^b=C_b^R歐拉角與四元數的關系，可得俯仰角。、偏航角φ、滾轉角γ的數學表述公式：

2 互補濾波及彈體高度測量算法

2.1 互補濾波

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

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

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

2.2 自適應互補濾波

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

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

2.3 水下彈體的運動高度測量

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

ω_enⁿ——掛囪球自轉角速度在導航坐標系的值，rad/s;

ω_ien——導航坐標系相對地球坐標系旋轉的角

速度在導航坐標系的投影值，rad/s;

gⁿ——重力力口速度在導航坐標系的投影值，m/s²。

將式（6）的比力方程寫成矩陣形式為式中：L——當地緯度，（°）;

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

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

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

t——彈體運動時間，s。

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

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

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

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

4 實驗分析及算法驗證

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

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

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

5 結束語

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

參考文獻

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

[2]王麗娜.側掃聲吶數據采集與地貌圖像構建[J].北京測繪，2018，32（8）：965-969.

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

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

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

[6]黃浩乾.水下滑翔器慣性組合導航定位關鍵技術研究[D].南京：東南大學，2015.

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

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

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

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

[11]何東旭.AUV水下導航系統(tǒng)關鍵技術研究[D].哈爾濱：哈爾濱工程大學，2013.

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

（編輯：商丹丹）