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

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

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

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

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

基金項(xiàng)目：國(guó)家自然科學(xué)基金項(xiàng)目（51575500）

作者簡(jiǎn)介：張海鵬（1992-），男，山西忻州市人，碩士研究生，專業(yè)方向?yàn)槲⑾到y(tǒng)集成、慣性測(cè)量。

0 引言

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

1 姿態(tài)描述

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

導(dǎo)航坐標(biāo)系保持不變，在解算彈體姿態(tài)過程中，將載體坐標(biāo)系獲得的位置和姿態(tài)數(shù)據(jù)轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下進(jìn)行計(jì)算。坐標(biāo)變換過程中，根據(jù)兩個(gè)坐標(biāo)系之間的位置關(guān)系，導(dǎo)航坐標(biāo)系經(jīng)過3次旋轉(zhuǎn)變換即可得到載體坐標(biāo)系相對(duì)導(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)動(dòng)的姿態(tài)時(shí)，為防止求解的結(jié)果出現(xiàn)奇點(diǎn)，本文采用解算過程中不產(chǎn)生奇點(diǎn)的四元數(shù)法。四元數(shù)計(jì)算過程相對(duì)簡(jiǎn)單，不需要大量計(jì)算，滿足彈體姿態(tài)求解要求。

定義q為一個(gè)常規(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)動(dòng)，載體坐標(biāo)系與導(dǎo)航坐標(biāo)系的坐標(biāo)變換用四元數(shù)法時(shí)，其變化過程可看作是一次無中間過程的等效旋轉(zhuǎn)，而這個(gè)變化過程可以由四元數(shù)向量表示。由b系變換到R系的變換矩陣用四元數(shù)表示為C_b^R，其公式表述為

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

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

2.1 互補(bǔ)濾波

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

因此陀螺儀和加速度計(jì)兩者之間的動(dòng)態(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ì)所測(cè)得數(shù)據(jù)通過低通濾波器時(shí)，可以濾除高頻抖動(dòng)信息。而陀螺儀所測(cè)得姿態(tài)信息通過高通濾波器時(shí)，由漂移導(dǎo)致的累積誤差將被濾除。因此在測(cè)量過程中，陀螺儀和加速度計(jì)分別在高頻段和低頻段工作。

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

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

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

2.3 水下彈體的運(yùn)動(dòng)高度測(cè)量

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

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

ω_ien——導(dǎo)航坐標(biāo)系相對(duì)地球坐標(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)系下的分值，對(duì)應(yīng)東、北、天三個(gè)方向的速度大小，m/s;

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

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

t——彈體運(yùn)動(dòng)時(shí)間，s。

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

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

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

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

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

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

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

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

5 結(jié)束語

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

參考文獻(xiàn)

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

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

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

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

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

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

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

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

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

[10]余星.基于捷聯(lián)慣性的水下載體姿態(tài)檢測(cè)系統(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é)報(bào)，2017，38（4）：183-187.

（編輯：商丹丹）