一種微型矢量水聽器姿態測量系統

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

（海軍潛艇學院，青島 266199）

一種微型矢量水聽器姿態測量系統

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

（海軍潛艇學院，青島 266199）

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

微型；姿態測量系統；擴展卡爾曼濾波；矢量水聽器；目標方位估計

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

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

1 矢量水聽器姿態測量系統

1.1 矢量水聽器坐標系及姿態角定義

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

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

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

1.2 姿態更新算法

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

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

其中，

1.3 擴展卡爾曼濾波器

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

該系統過程模型的狀態變量為

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

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

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

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

1.4 系統實現

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

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

圖2 矢量水聽器姿態測量系統結構Fig.2 Structure of attitude measurement system

圖3 矢量水聽器姿態測量系統Fig.3 Attitude measurement system of vector hydrophone

2 實驗測試

2.1 實驗室測試

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

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

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

表1 橫滾角靜態精度測試結果Tab.1 Test results of roll angle’s static accuracy of

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

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

表2 橫滾角動態精度測試結果Tab.2 Test results of roll angle’s dynamic accuracy

圖4 本系統與TRAX系統橫滾角動態誤差對比Fig.4 Comparison on dynamic errors of roll angles

2.2 海上試驗

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

圖5矢量水聽器各通道信號的頻譜圖，其中黑線為姿態矯正前的信號頻譜，紅線為經過姿態矯正后的信號頻譜。這里為了抑制聲場中的低頻噪音，已將150 Hz以下的信號濾除。可見信號的頻率主要集中在750 Hz。由頻譜可見，經過姿態矯正后，Vx和Vy通道的目標信號得到增強，Vz通道的目標信號被減弱。

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

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

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

3 結 論

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

（References）：

[1] 師俊杰, 呂云飛, 孫大軍, 等. 潛標姿態變化對矢量水聽器目標方位估計的影響[J]. 聲學技術, 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] 牛嗣亮, 張振宇, 胡永明, 等. 單矢量水聽器的姿態修正測向問題探討[J]. 國防科技大學學報, 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] 王文龍, 孫芹東, 侯文姝, 等. 姿態測量系統在單矢量水聽器方位估計中的應用[J]. 聲學技術, 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] 秦永元. 慣性導航[M]. 北京: 科學出版社, 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] 王學斌, 徐建宏, 張章, 等. 卡爾曼濾波器參數分析與應用方法研究[J]. 計算機應用與軟件, 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

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

笪良龍（1967—），男，教授，博士生導師，從事海軍作戰環境、水聲環境效應研究。E-mail: wilon7521@qq.com