一种微型矢量水听器姿态测量系统

笪良龙，王文龙，孙芹东，侯文姝

（海军潜艇学院，青岛 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