0 引　言

地球表面积约70%被海洋覆盖，有着丰富的资源和能源，对国家政治、经济、安全等方面有着重大意义，水下弹体的发射多以冷发射的方式弹射出仓，发射过程中弹体姿态、高度等的测量现已成为国内外水下弹体发射技术研究的一个重要方向。文献[1]中用多波束声呐测深系统，以传统的扫床方式对一定深度水域进行反复扫测，能够详细反映所测区域水下的地形情况，但其应用范围易受水下地形的影响而具有局限性，且易受到水深和波束角的限制。文献[2]利用侧扫声呐实时数据采集与地貌图像构建方法，虽然可以通过声学成像技术描述海底形态，但在计算海底高度时，是根据影子长度进行估计的，对数据解译人员的要求很高。文献[3]提到的GPS高程测量，利用全球定位系统测量技术可直接测定地面某点的高程，且基于GPS的定位与测高系统在陆地覆盖广泛、精度高，但其在水下信号较弱甚至无法使用。基于温度、激光、压力等的测高系统极易受到洋流、太阳辐射、季节变化等海洋环境干扰因素的影响而使测量精度低、准确性差。该水下测高系统以IMU为核心信息采集单元，搭建基于FPGA与DSP的硬件电路平台，结合自适应互补滤波算法，依据水下弹体的运动情况，实时动态调整PI参数，利用对所得姿态数据的解算得到水下弹体的高度信息，实现对水下弹体的实时高度测量，有效提高了测量准确度，实用性高。

1 姿态描述

弹体在水下运动过程中，需要建立坐标系来描述其运动姿态和所处位置。本文建立以弹体为参考的载体坐标系（b系），建立以所在地地理位置为参考的导航坐标系（n系），其中导航坐标系的 $X$ 、 $Y$ 、 $Z$ 三轴分别对应当地的东、北和天方位。惯性测量单元IMU固连在弹体上，基于惯性传感器所得弹体的姿态和位置信息都是基于导航坐标系进行测量和计算的，系统姿态由俯仰角 $\theta $ 、横滚角 $\gamma $ 、偏航角 $\varphi $ 来描述^[4-6]。导航坐标系与载体坐标系之间的坐标关系及弹体水下运动模拟图如图1所示。

导航坐标系保持不变，在解算弹体姿态过程中，将载体坐标系获得的位置和姿态数据转换到导航坐标系下进行计算。坐标变换过程中，根据两个坐标系之间的位置关系，导航坐标系经过3次旋转变换即可得到载体坐标系相对导航坐标系的角位置关系。3次旋转变换分别为：绕导航系 $Z$ 轴旋转得到偏航角 $\varphi $ 、绕 $Y$ 轴得到俯仰角 $\theta $ 、绕 $X$ 轴得到滚转角 $\gamma $ 。坐标系之间的关系可用旋转矩阵 ${{C}}_{{n}}^{{b}}$ 表示^[7-9]，表达式为

在求解弹体在水下运动的姿态时，为防止求解的结果出现奇点，本文采用解算过程中不产生奇点的四元数法。四元数计算过程相对简单，不需要大量计算，满足弹体姿态求解要求。

定义 ${{q}}$ 为一个常规四元数向量，数学公式为

其中 ${q_0}$ 、 ${q_1}$ 、 ${q_2}$ 、 ${q_3}$ 为实数， ${{i}}$ 、 ${{j}}$ 、 ${{k}}$ 为两两正交的向量。当弹体在水下运动，载体坐标系与导航坐标系的坐标变换用四元数法时，其变化过程可看作是一次无中间过程的等效旋转，而这个变化过程可以由四元数向量表示。由b系变换到R系的变换矩阵用四元数表示为 ${{C}}_{{b}}^{{R}}$ ，其公式表述为

且由 ${{C}}_{{n}}^{{b}} = {{C}}_{{b}}^{{R}}$ 欧拉角与四元数的关系，可得俯仰角 $\theta $ 、偏航角 $\varphi $ 、滚转角 $\gamma $ 的数学表述公式：

2 互补滤波及弹体高度测量算法 2.1 互补滤波

水下弹体高度测量系统是以惯性测量单元为信息采集单元的捷联惯导系统，该信息采集单元包括传感器MEMS陀螺仪和加速度计。其中陀螺仪能够测量弹体在水下运动时的角速度，加速度计能够测量弹体在水下运动时的三轴方向的加速度。由惯性测量单元采集得到水下弹体的角速度和加速度数据都可以通过坐标变换和积分计算处理，得到水下弹体运动所需的姿态和位置信息。陀螺仪在测量姿态和位置信息时，其动态响应特性较好而静态特性差，当被测物体处在高速运动过程中，其所测得的姿态数据和动态位置精度较高，但是由于陀螺仪存在温漂和零漂，当被测物体在低速运动或静止时，陀螺仪响应特性较差。与陀螺仪特性相反，加速度计在测量姿态和位置信息时，其动态响应特性较差而静态特性好。当被测物体静止或低速运动时，其能测得相对精确的姿态和较为稳定的位置信息。

因此陀螺仪和加速度计两者之间的动态响应特性相互补充，互补滤波就是充分利用这一点将所得的数据进行融合，进而得到高精度的弹体姿态和位置信息。互补滤波原理如图2所示。

设 ${f_1}\left( s \right) = {K/{\left( {s + K} \right)}}$ 为低通滤波器， ${f_2}\left( s \right) = $ ${s/{\left( {s + K} \right)}} $ 为高通滤波器，其中 ${1/s}$ 为滤波器截止频率，频率高于 ${1/s}$ 的波形通过低通滤波器时，将被滤除，而在高通滤波器中则可以通过。加速度计所测得数据通过低通滤波器时，可以滤除高频抖动信息。而陀螺仪所测得姿态信息通过高通滤波器时，由漂移导致的累积误差将被滤除。因此在测量过程中，陀螺仪和加速度计分别在高频段和低频段工作。

2.2 自适应互补滤波

在测量和计算俯仰角 $\theta $ 时，低通滤波器和高通滤波器传递函数分别为 ${f_1}\left( s \right)$ 和 ${f_2}\left( s \right)$ ，且 ${f_1}\left( s \right) + {f_2}\left( s \right) = 1$ 。则根据互补滤波原理可表示为

其中 $\theta $ 为根据式(4)计算得到的姿态角预估值。通过式(5)利用互补滤波能同时消除高频和低频干扰，实现姿态数据的融合。滤波过程中，需要合理选取参数 $K$ ，才能实现较好的滤波。由于测量和滤波过程中存在较大噪声，且互补滤波低通阻带衰减速度较慢，会导致较大误差。因此为减小误差，在以上基础上增加了PI控制器。在测量的不同阶段，根据物体运动速度来动态调整补偿系数K_p、K_i。以上经过改良的滤波器为自适应互补滤波器^[10-12]，其测量和计算所得的位置和姿态数据精度相对较高。自适应互补滤波原理如图3所示。

在自适应互补滤波器中，由加速度计解算得到的姿态角预估值 $\theta $ ，通过公式(5)实现加速度计姿态角 $\theta $ 与陀螺仪角速率 $\omega $ 数据信息融合，从而得到当前姿态信息。在处理过程中，将融合所得数据反馈给PI控制器，实时更新得到新的姿态角。

2.3 水下弹体的运动高度测量

通过式(1)得到 ${{C}}_{{n}}^{{b}}$ ，且利用坐标变化矩阵将加速度计输出的水下弹体运动在载体系中的比力信息转换到导航坐标系中，同时进行有害重力加速度补偿，得到导航系中弹体运动的加速度方程为

式中： ${ f^{{b}}}$ ——载体系中加速度计测得的比力信息；

$ \omega _{{\rm{en}}}^{{n}}$ ——地球自转角速度在导航坐标系的值，rad/s；

$ \omega _{{\rm{ie}}}^{{n}}$ ——导航坐标系相对地球坐标系旋转的角速度在导航坐标系的投影值，rad/s；

${ g^{{n}}}$ ——重力加速度在导航坐标系的投影值，m/s²。

将式(6)的比力方程写成矩阵形式为

式中：L——当地纬度，（°）；

${V_{{x}}}$ 、 ${V_{{y}}}$ 、 ${V_{{z}}}$ ——三维速度在地理坐标系下的分值，对应东、北、天三个方向的速度大小，m/s；

${{{R}}_{\rm{e}}}$ 、 ${{{R}}_{\rm{n}}}$ ——卯酉圈和子午圈的曲率径， ${{\rm m}^{ - 1}}$ 。

利用公式(1)所得姿态数据及公式(7)得到水下弹体在地理坐标系下的天向速度 ${V_{{z}}}$ ，进而可得水下弹体运动位置高度H：

式中： ${H_0}$ ——初始位置高度，m；

$t$ ——弹体运动时间，s。

3 系统硬件平台设计

系统结构框图如图4所示，本文搭建了基于FPGA与DSP的硬件电路平台，主要由惯性测量模块（三轴陀螺仪、三轴加速度计、信号调理单元等）、参数设置模块（外部触发单元、通信接口单元等）、MCU主控模块（信号采集单元、数据解算单元、数据存储单元、数据输出单元等）组成。

其中，三轴陀螺仪与三轴加速度计组合构成姿态传感器，测试水下模型发射后的三维加速度信息和对三维角速率信息并利用MCU对采集信息进行实时解算与存储。主控芯片采用XILINX公司生产的Spartan-II系列的XC2S100，该FPGA内核采用2.5 V低电压供电，工作频率最高达200 MHz，拥有4个时钟输入端，176个I/O口，10×10⁴个门电路，600个可编程逻辑块，内部集成10个双口RAM，共40 kbits，可以实现系统中数据的高速缓存；同时该芯片具有丰富的门阵列资源、灵活的可配置性和良好的抗干扰性，可以实现复杂的逻辑控制。

信号调理模块采用德州仪器公司的轨至轨运算放大器OPA4340，增强模拟信号的驱动能力；模数转换芯片采用TI公司的ADS8365实现6路传感器信号的同步采集，该型A/D转换器具备16位采集精度、高达250 kS/s的采样率、并口数据传输及单5 V供电的特点，能够实现6通道模拟信号的同步采集，系统硬件电路如图5所示。

4 实验分析及算法验证

如图6所示，本节采用实际车载实验利用高精度定位定姿系统输出的姿态角和位置信息作为标准，对系统姿态角和位置信息的正确性与准确性进行验证与评估。高精度定位定姿系统为加拿大NovAtel公司高精度光纤组合导航系统span-lci，其姿态角准确度为0.008°、速度准确度为0.03 m/s、位置准确度为0.1 m。

基于上述硬件平台，实验场地选取东西向空旷平直路段，将硬件电路系统朝小车前进方向水平放置模拟弹体在水下运动时的上升过程，速度在5 m/s以内，采样频率为100 Hz。在此实验基础上对实验数据进行存储采集和互补滤波、自适应互补滤波算法处理，以及同高精度定位定姿系统数据进行对比，实验数据经互补滤波、自适应互补滤波算法处理后与高精度定位定姿系统所得姿态角、位置对比情况如图7、图8所示。

通过图7和图8可以看出，互补滤波线条波动幅度较大，误差较大，姿态及位置推算效果较差。自适应互补滤波误差明显低于互补滤波，且响应速度快，对噪声的过滤效果更好，线条波动小，且通过对数据实际计算得，自适应互补滤波算法计算结果相比传统互补滤波方法，误差有所下降，其中俯仰角平均误差下降了58.4%、偏航角的平均误差下降了64.7%、滚转角的平均误差下降了73.6%、位置推算的平均误差下降了65.8%。在准确计算姿态角的基础上进行位移推算，与高精度定位定姿系统精度较为接近。

5 结束语

针对现有几种水下高度测量方法中存在的问题，设计了基于自适应互补滤波的水下高度实时测量系统。对传统的互补滤波方法进行改进，在低通滤波环节加入 PI 控制器，依据加速度计测得的姿态数据判断水下弹体的运动情况，得到实时更新的姿态数据信息，进而实现载体高度的高精度解算。搭建了以FPGA+DSP为核心的信息处理解算硬件电路平台，且通过车载实验与高精度定位定姿系统进行实际对比实验验证，验证表明，该系统以惯性单元为信息采集单元，独立性、抗干扰能力好，且相比传统互补滤波算法，测量精度高、实时性好。