笪良龍，王文龍，孫芹東，侯文姝

（海軍潛艇學(xué)院，青島 266199）

一種微型矢量水聽器姿態(tài)測量系統(tǒng)

笪良龍，王文龍，孫芹東，侯文姝

（海軍潛艇學(xué)院，青島 266199）

矢量水聽器姿態(tài)校正的通常做法是將姿態(tài)測量系統(tǒng)捷聯(lián)安裝在聲納平臺上，這種方案無法準(zhǔn)確測量矢量水聽器的姿態(tài)變化。針對這個問題，設(shè)計了一種微型姿態(tài)測量系統(tǒng)，并將其捷聯(lián)封裝在矢量水聽器內(nèi)部。系統(tǒng)采用MEMS陀螺測量角速度，用畢卡迭代算法解四元數(shù)姿態(tài)更新方程。用MEMS加速度計和磁力計分別測量重力方向和磁北方向，再使用擴展卡爾曼濾波對解算姿態(tài)角進行實時校正。經(jīng)測試，該系統(tǒng)橫滾角和俯仰角的靜態(tài)誤差小于0.2°，航向角的靜態(tài)誤差小于0.8°。搖擺實驗中，橫滾角和俯仰角的動態(tài)相對誤差小于 2.9%，航向角的動態(tài)相對誤差小于 3.6%。海上試驗結(jié)果證明該姿態(tài)測量系統(tǒng)應(yīng)用于矢量水聽器可明顯提高目標(biāo)方位估計的精度。

微型；姿態(tài)測量系統(tǒng)；擴展卡爾曼濾波；矢量水聽器；目標(biāo)方位估計

矢量水聽器可以同步共點地測量水下聲場中的聲壓標(biāo)量信號和振速矢量信號，能夠抗各向同性干擾和抗相干干擾、空間增益大，單個水聽器就可實現(xiàn)對目標(biāo)的定向，且具有體積小、重量輕、功耗低等優(yōu)點，非常適合應(yīng)用在晃動小平臺上。矢量水聽器探測到的振速是相對于其自身坐標(biāo)系的矢量信息，然而由于海洋中洋流、內(nèi)波、潮汐等復(fù)雜環(huán)境的影響，聲納平臺以及矢量水聽器的姿態(tài)會發(fā)生不間斷的變化。為了得到目標(biāo)相對于地理坐標(biāo)系的方位，還需獲取矢量水聽器相對于地理坐標(biāo)系的實時姿態(tài)信息。傳統(tǒng)做法是將姿態(tài)測量系統(tǒng)捷聯(lián)安裝在聲納平臺上，但是由于柔性懸掛系統(tǒng)的存在，矢量水聽器的姿態(tài)變化與懸掛平臺的姿態(tài)變化并不完全一致[1]；另外，在矢量水聽器成陣應(yīng)用的時候，陣中各矢量水聽器的姿態(tài)也不完全一致，聲納平臺上的姿態(tài)測量系統(tǒng)無法準(zhǔn)確感測每一個矢量水聽器的姿態(tài)變化，給波束形成帶來較大誤差。可以說矢量水聽器姿態(tài)的實時準(zhǔn)確獲取是其走向工程實用的一個關(guān)鍵問題[2]。

要解決這個問題，可以將微型姿態(tài)測量系統(tǒng)捷聯(lián)封裝在矢量水聽器內(nèi)部[3]。但已有的姿態(tài)測量系統(tǒng)或體積大成本高，如光纖陀螺姿態(tài)測量系統(tǒng)等；或精度較低[4]，動態(tài)響應(yīng)差，如加速度計電子羅盤姿態(tài)測量系統(tǒng)，均不適用于小型矢量水聽器姿態(tài)測量。本文設(shè)計了一種基于 MEMS慣性傳感器的微型矢量水聽器姿態(tài)測量系統(tǒng)。該系統(tǒng)通過采集角速度信息進行姿態(tài)解算，檢測重力加速度方向和地磁北方向來進行姿態(tài)校正。該系統(tǒng)具有體積小、質(zhì)量輕、功耗低、動態(tài)響應(yīng)好等特點，可以捷聯(lián)安裝在矢量水聽器內(nèi)部，實時感測矢量水聽器姿態(tài)的動態(tài)變化。

1 矢量水聽器姿態(tài)測量系統(tǒng)

1.1 矢量水聽器坐標(biāo)系及姿態(tài)角定義

矢量水聽器的姿態(tài)角是水聽器坐標(biāo)系（b系）相對于參考坐標(biāo)系（n系）的旋轉(zhuǎn)關(guān)系。水聽器坐標(biāo)系原點位于矢量水聽器質(zhì)心，坐標(biāo)軸bx、by、bz分別指向矢量水聽器的右、前、上方向。參考坐標(biāo)系原點位于矢量水聽器質(zhì)心，坐標(biāo)軸nx、ny、nz分別指向矢量水聽器所在地的東、北、天方向（與地理坐標(biāo)系一致）。參考坐標(biāo)系n通過式(1)所示三次旋轉(zhuǎn)可以變換到載體坐標(biāo)系b[5]，ψ、θ、γ分別為航向角、俯仰角和橫滾角，其旋轉(zhuǎn)正方向與相應(yīng)的坐標(biāo)軸構(gòu)成右手螺旋關(guān)系。

姿態(tài)可以用歐拉角、方向余弦矩陣、四元數(shù)等方式描述，這幾種姿態(tài)表示方式之間存在固定的轉(zhuǎn)換關(guān)系，如式(2)和式(3)所示[5]：

由式(2)及式(3)可解得航向角、俯仰角和橫滾角分別為

1.2 姿態(tài)更新算法

受復(fù)雜海洋環(huán)境的影響，矢量水聽器在海水中的運動可以看作是隨重心的平動與繞重心的轉(zhuǎn)動的合成。矢量水聽器隨重心的平動對矢量水聽器測量遠場聲場基本沒有影響，這里只考慮其繞重心的轉(zhuǎn)動。b系相對于n系的姿態(tài)可以用n系至b系的旋轉(zhuǎn)四元數(shù)Q來描述，其微分方程為[5]

本系統(tǒng)的姿態(tài)解算速率較快（實測為 540 Hz），可認為在姿態(tài)更新周期內(nèi)角速度不變，可選擇畢卡算法來求解四元數(shù)微分方程。四元數(shù)方程的畢卡三階近似解如下[5]：

其中，

1.3 擴展卡爾曼濾波器

考慮到MEMS陀螺隨溫度、時間變化的非線性導(dǎo)致其精度較低[6]，長時間積分誤差會迅速放大，本系統(tǒng)引入三軸MEMS加速度計和三軸MEMS磁力計檢測重力加速度方向和地磁北方向來對解算姿態(tài)進行實時校正[7-8]。重力加速度方向和地磁北方向正交，從而唯一確定一個三維空間的姿態(tài)，將其作為校準(zhǔn)源，用擴展卡爾曼濾波器(EFK)對陀螺積分出來的姿態(tài)不斷地進行最優(yōu)估計[9]，抑制其漂移。

該系統(tǒng)過程模型的狀態(tài)變量為

式中：0q、1q、2q、3q為姿態(tài)四元數(shù)，xε、yε、zε為陀螺漂移誤差。系統(tǒng)狀態(tài)方程為其中1k-X 為上一時刻狀態(tài)變量，Q為過程激勵噪聲協(xié)方差矩陣，選為一個非常小但不為零的矩陣[10]，A為采用四元數(shù)畢卡算法三階近似的狀態(tài)矩陣：

該系統(tǒng)過程模型的觀測變量設(shè)計為k=Z其中xa、ya、za為加速度計測量值，xm、ym、zm為磁力計測量值。系統(tǒng)觀測方程為其中R為測量噪聲協(xié)方差矩陣，其值設(shè)定為測定的噪聲協(xié)方差[10]，kH 為當(dāng)前觀測矩陣：

圖1 擴展卡爾曼濾波器工作流程Fig.1 Process of extended Kalman filter

擴展卡爾曼濾波器工作流程包括時間更新（預(yù)測）和測量更新（校正）[11]。本系統(tǒng)的擴展卡爾曼濾波器工作流程如圖1。圖1中，第①步是由前文的姿態(tài)更新算法得到的狀態(tài)矩陣A來向前估計狀態(tài)變量，其中為本次估計值，1k-X 為上次后驗估計值。第②步是向前估計誤差協(xié)方差矩陣，其中為本次估計值，1k-P為上次后驗估計值。這兩步是時間更新。第③步為計算卡爾曼增益kK。第④步是由本次觀測值kZ來更新狀態(tài)變量估計kX ，即是本輪的輸出。第⑤步為更新誤差協(xié)方差矩陣kP，供下次使用。這三步是測量更新。

1.4 系統(tǒng)實現(xiàn)

矢量水聽器姿態(tài)測量系統(tǒng)主要由傳感器模塊、計算控制模塊、電源穩(wěn)壓模塊、接口轉(zhuǎn)換電路以及上位機構(gòu)成，系統(tǒng)結(jié)構(gòu)如圖2。傳感器模塊由三軸MEMS數(shù)字陀螺、三軸MEMS數(shù)字加速度計和三軸MEMS數(shù)字磁力計組合而成，可認為它們的測量坐標(biāo)系重合。計算控制模塊采用ARM-cortexM3內(nèi)核的高性能單片機，用于完成對各子模塊的控制、數(shù)據(jù)采集、姿態(tài)解算等工作，并通過RS232接口向上位機發(fā)送數(shù)據(jù)，上位機則接收姿態(tài)測量系統(tǒng)測得的數(shù)據(jù)，并完成姿態(tài)數(shù)據(jù)的顯示、保存等工作。外圍支持電路包括電源穩(wěn)壓電路和接口轉(zhuǎn)換電路。電源穩(wěn)壓電路由數(shù)字電源穩(wěn)壓電路和模擬電源穩(wěn)壓電路構(gòu)成，數(shù)模電源分開供電的設(shè)計，提高了AD轉(zhuǎn)換的精度。接口轉(zhuǎn)換電路實現(xiàn)TTL轉(zhuǎn)RS232總線。

制作完成的姿態(tài)測量系統(tǒng)如圖3所示，其尺寸僅為25 mm×25 mm×3 mm，可以很容易封裝進矢量水聽器內(nèi)部。

圖2 矢量水聽器姿態(tài)測量系統(tǒng)結(jié)構(gòu)Fig.2 Structure of attitude measurement system

圖3 矢量水聽器姿態(tài)測量系統(tǒng)Fig.3 Attitude measurement system of vector hydrophone

2 實驗測試

2.1 實驗室測試

功耗方面，在5 V供電情況下實際測試工作電流為48 mA，即功率為0.23 W。

精度測試在中國航空工業(yè)集團公司北京長城計量測試技術(shù)研究所（國防科技工業(yè)第一計量測試研究中心）完成。分別做了靜態(tài)精度測試和動態(tài)精度測試。

靜態(tài)精度測試對3個軸向進行分別測試。橫滾角測量范圍[-180°, 180°]，每間隔30°取一個點，每個點循環(huán)測試兩次，共測試了13個點26組數(shù)據(jù)。結(jié)果表明橫滾角靜態(tài)誤差最大值為0.2°。俯仰角測量范圍[-80°, 80°]，測試時每間隔 10°取一個點，每個點循環(huán)測試兩次，共測試了17個點34組數(shù)據(jù)。結(jié)果表明俯仰角靜態(tài)誤差最大值為0.1°。航向角測量范圍[0°, 360°]，每間隔30°取一個點，每個點循環(huán)測試兩次，共測試了13個點26組數(shù)據(jù)。結(jié)果表明航向角靜態(tài)誤差最大值為0.8°。三個軸向的兩次測量均重復(fù)性較好，表明測試結(jié)果是可信的。限于篇幅僅將橫滾角測試數(shù)據(jù)列出，見表1。

表1 橫滾角靜態(tài)精度測試結(jié)果Tab.1 Test results of roll angle’s static accuracy of

動態(tài)精度測試采用搖擺實驗。由于海洋中洋流、內(nèi)波、潮汐、浪涌等頻率主要集中在低頻段，因此分別測量了3個軸向在0.1 Hz、0.3 Hz、0.5 Hz、0.7 Hz、0.9 Hz、1 Hz和2 Hz頻率搖擺下的動態(tài)精度。表2為橫滾角動態(tài)精度測試結(jié)果。測試結(jié)果顯示，在振幅小于 20°、頻率低于2 Hz時，橫滾角的動態(tài)誤差小于0.5°，動態(tài)相對誤差為振幅的2.8%以內(nèi)；俯仰角動態(tài)誤差小于0.4°，動態(tài)相對誤差為振幅的2.9%以內(nèi)；航向角動態(tài)誤差小于0.5°，動態(tài)相對誤差為振幅的3.6%以內(nèi)。

作為對比，在同等條件下測試了美國進口PNI公司的TRAX微型姿態(tài)測量系統(tǒng)的羅盤模式，該系統(tǒng)是TCM系列姿態(tài)傳感器的升級產(chǎn)品，是目前市面上可獲取的精度最高的微型姿態(tài)測量系統(tǒng)。測試結(jié)果顯示，該姿態(tài)測量系統(tǒng)的靜態(tài)精度稍好，橫滾角、俯仰角和航向角的最大誤差分別為 0.2°、0.0°和 0.4°；動態(tài)精度方面，圖 4為兩系統(tǒng)橫滾角動態(tài)誤差測量結(jié)果對比。由圖可見TRAX在0.1 Hz、0.3 Hz時精度較高，但隨著頻率升高誤差均迅速增大，其他兩個軸向也是如此。結(jié)果表明TRAX動態(tài)精度隨頻率上升迅速變差，而本系統(tǒng)在低頻范圍內(nèi)動態(tài)精度基本穩(wěn)定。

表2 橫滾角動態(tài)精度測試結(jié)果Tab.2 Test results of roll angle’s dynamic accuracy

圖4 本系統(tǒng)與TRAX系統(tǒng)橫滾角動態(tài)誤差對比Fig.4 Comparison on dynamic errors of roll angles

2.2 海上試驗

2014年9月，在青島外海某海域進行了封裝有該姿態(tài)測量系統(tǒng)的單矢量水聽器的目標(biāo)方位估計海上試驗。目標(biāo)方位估計方法采用的是復(fù)聲強法和直方圖統(tǒng)計法。海試時，封裝有姿態(tài)測量系統(tǒng)的單矢量水聽器由彈簧懸掛在剛性支架上吊放在接收艦舷側(cè)10 m水深處，聲源吊放在發(fā)射艦舷側(cè)10 m水深處，海深55 m。兩艦相距8.2 km，發(fā)射艦相對接收艦的GPS推算方位角約為165°。聲源信號為單頻信號，頻率為750 Hz，持續(xù)2 s，聲源級為192 dB，估算信噪比約為74 dB。

圖5矢量水聽器各通道信號的頻譜圖，其中黑線為姿態(tài)矯正前的信號頻譜，紅線為經(jīng)過姿態(tài)矯正后的信號頻譜。這里為了抑制聲場中的低頻噪音，已將150 Hz以下的信號濾除?？梢娦盘柕念l率主要集中在750 Hz。由頻譜可見，經(jīng)過姿態(tài)矯正后，Vx和Vy通道的目標(biāo)信號得到增強，Vz通道的目標(biāo)信號被減弱。

圖6為矢量水聽器姿態(tài)矯正前后目標(biāo)方位角估計值與GPS艦位推算值的對比。其中綠線為姿態(tài)矯正前的目標(biāo)方位估計值，紅線為姿態(tài)矯正后的目標(biāo)方位估計值，黑線為由GPS艦位推算的目標(biāo)方位。由圖可見未進行姿態(tài)矯正時，目標(biāo)方位角估計值與推算值極不相符且變化很大，最大誤差超過180°，這是因為此時的目標(biāo)方位角估計值是矢量水聽器坐標(biāo)系下的結(jié)果，由于受海流海浪等影響，矢量水聽器坐標(biāo)系相對參考坐標(biāo)系在不斷變化，導(dǎo)致目標(biāo)方位估計結(jié)果基本不可用。在進行了姿態(tài)矯正后，在第2.3 s左右目標(biāo)方位角估計值即很好地收斂到推算值附近，一直到第6.2 s左右，且不隨矢量水聽器姿態(tài)的變化而變化。方位角估計值的最大誤差小于3°。之所以目標(biāo)方位估計有效時間會比信號發(fā)生時間前后都多出1 s左右，是因為目標(biāo)方位估計中采用了1 s時長的滑動時間平均窗。

圖5 各通道信號頻譜Fig.5 Signal spectrum of each channel

圖6 目標(biāo)方位角GPS推算值與實驗估計結(jié)果Fig.6 Direction-of-arrival results of GPS calculation and test estimation

3 結(jié) 論

本文設(shè)計實現(xiàn)了一種微型姿態(tài)測量系統(tǒng)，可直接封裝在矢量水聽器內(nèi)部，采用捷聯(lián)方式測量矢量水聽器的姿態(tài)變化。系統(tǒng)能以540 Hz的速度進行姿態(tài)測量，并以20 Hz的速度輸出矢量水聽器的姿態(tài)數(shù)據(jù)。經(jīng)測試，本系統(tǒng)的橫滾角和俯仰角的靜態(tài)誤差小于0.2°，動態(tài)相對誤差小于2.8%，航向角的靜態(tài)誤差小于 0.8°，動態(tài)相對誤差小于 3.5%。該姿態(tài)測量系統(tǒng)體積小，重量輕，功耗低，精度較高，適合用于矢量水聽器內(nèi)部的姿態(tài)測量。海上試驗證明該姿態(tài)測量系統(tǒng)應(yīng)用于矢量水聽器可大大提高單矢量水聽器目標(biāo)方位估計的精度。該微型姿態(tài)測量系統(tǒng)在矢量水聽器姿態(tài)測量方面具有較好的應(yīng)用價值。

（References）：

[1] 師俊杰, 呂云飛, 孫大軍, 等. 潛標(biāo)姿態(tài)變化對矢量水聽器目標(biāo)方位估計的影響[J]. 聲學(xué)技術(shù), 2014, 33(2): 125.

Shi Jun-jie, Lü Yun-fei, Sun Da-jun, et al. Influence of attitude changes of subsurface buoy on DOA estimation of vector hydrophone[J]. Technical Acoustics, 2014, 33(2): 125.

[2] 牛嗣亮, 張振宇, 胡永明, 等. 單矢量水聽器的姿態(tài)修正測向問題探討[J]. 國防科技大學(xué)學(xué)報, 2011, 33(6): 105.

Niu Si-liang, Zhang Zhen-yu, Hu Yong-ming, et al. Direction of arrival estimation from a single vector hydrophone with attitude correction[J]. Journal of National University of Defense Technology, 2011, 33(6): 105.

[3] 王文龍, 孫芹東, 侯文姝, 等. 姿態(tài)測量系統(tǒng)在單矢量水聽器方位估計中的應(yīng)用[J]. 聲學(xué)技術(shù), 2015, 34(2): 308-310.

Wang Wen-long, Sun Qin-dong, Hou Wen-shu, et al. The application of attitude measurement system on DOA estimation of single vector hydrophone[J]. Technical Acoustics, 2015, 34(2): 308-310.

[4] Zhu Rong, Zhou Zhao-ying. A small low-cost hybrid orientation system and its error analysis[J]. IEEE Sensors Journal, 2009, 9(3): 220-230.

[5] 秦永元. 慣性導(dǎo)航[M]. 北京: 科學(xué)出版社, 2006.

[6] Dah J J. Ta S. Critical remarks on the linearized and extended kalman filters with geodetic navigation examples [J]. Measurement, 2010, 43(9): 1077-1089.

[7] Angrisano A. GNSS/INS integration methods[D]. Canada: University of Calcary, 2010.

[8] Guerrero C J F. Design and implementation of an attitude and heading reference system[C]//8th International Conference on Electrical Engineering. Computing Science and Automatic Control, Program and Abstract Book, 2011: 1-5.

[9] Klein I, Filin S, Toledo T. Pseudo-measurements as aiding to INS during GPS outages[J]. Navigation, 2010, 57(1): 25-34.

[10] 王學(xué)斌, 徐建宏, 張章, 等. 卡爾曼濾波器參數(shù)分析與應(yīng)用方法研究[J]. 計算機應(yīng)用與軟件, 2012, 29(6): 212-215. Wang Xue-bin, Xu Jian-hong, Zhang Zhang, et al. On analysis and application approach for Kalman filter parameters[J]. Computer Applications and Software, 2012, 29(6): 212- 215.

[11] Welch G, Bishop G. An Introduction to the Kalman Filter[C]//Proc of the Acm Siggraph. ACM Press, Addison -Wesley, 2001.

Miniaturized attitude measurement system of vector hydrophone

DA Liang-long, WANG Wen-long, SUN Qin-dong, HOU Wen-shu
(Navy Submarine Academy, Qingdao 266199, China)

The attitude measurement system is usually installed on the platform of the vector hydrophone to eliminate the influence of attitude change, while the attitude of the platform is not completely equal to that of the vector hydrophone. To solve this problem, a new miniaturized attitude measurement system was developed, which could be easily mounted inside the small vector hydrophone. The strap-down mode was applied to measure the angular speed by directly using MEMS gyroscope in this system. The Picard iteration was used to resolve the quaternion attitude updating equation. A MEMS accelerometer and a MEMS dynamometer were also used to find the direction of gravity and magnetic north, based on which the extended Kalman filter can correct the attitude angle in real time. Test results show that the static error of pitch and roll angle of this system are less than 0.2°, and the static error of yaw angle is less than 0.8°. The dynamic relative error of pitch and roll angle is less than 2.9% in low frequency rolling experiment, and the dynamic relative error of yaw angle is no more than 3.6%. Sea trial results prove that the direction-of-arrival estimation system of single vector hydrophone with this attitude measurement system can effectively improve the estimation accuracy.

miniaturized; attitude measurement system; extended Kalman filter; vector hydrophone; direction of arrival

U666.1

A

1005-6734(2016)01-0020-06

10.13695/j.cnki.12-1222/o3.2016.01.005

2015-10-05；

2016-01-08

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

笪良龍（1967—），男，教授，博士生導(dǎo)師，從事海軍作戰(zhàn)環(huán)境、水聲環(huán)境效應(yīng)研究。E-mail: wilon7521@qq.com