0 引　言

海洋是一个巨大的能源宝库，水下航行器在人类探索、研究、开发深海资源的过程中承担着重要作用。而对水下航行器的定位必不可少，其核心在于对惯性传感器姿态解算的精确性。在传统的姿态解算中，使用MEMS(Micro-Electro-Mechanical System)传感器进行测姿^[1-2]，由于其传感器本身精度低和环境噪声等问题^[3]，使得采集的数据误差漂移大，最终导致姿态角解算精度低，鲁棒性差。

针对上述存在的问题，李景辉^[4]将测量值与观测值之间的偏差作为PI控制器的输入参数，通过互补滤波进行数据融合，增强了系统的稳定性；Zhongxu Hu^[5]将拓展卡尔曼滤波应用于传感器姿态解算和漂移补偿；米刚^[6]采用微惯性传感器单元采集数据，并对四元数和欧拉角进行转换，来消除奇异点所带来的问题。

鉴于以上研究，通过学习惯性导航相关算法，提出了一种信息融合姿态解算方法，选取MEMS传感器型号为MPU9250，对传感器信号处理，然后对传感器进行数据采集，PID控制器对数据进行调节，通过互补滤波进行对陀螺仪修正，最后通过龙格库塔法对四元数微分方程进行更新。该方法计算简单可靠，逻辑清晰。

1 姿态解算方法原理

为了更好描述水下航行器在水下潜行的姿态，建立如图1所示的载体坐标系。

姿态变化是由载体坐标系相对于地平坐标系的空间转动。设水下航行器载体坐标系为 $\left[ {{x_b}}\quad{{y_b}}\quad{{z_b}} \right]$ 系，参考坐标系为 $\left[ {{x_e}}\quad{{y_e}}\quad{{z_e}}\right] $ 系，参考坐标系为北东地方向，构建初始载体和参考坐标系的三轴相重合，参考坐标系经过旋转后与载体坐标系有如下关系^[7]：

$ {\left[ {{x_b}}\quad{{y_b}}\quad{{z_b}} \right]^{\rm{T}}} = C_e^b{\left[ {{x_e}}\quad{{y_e}}\quad{{z_e}} \right]^{\rm{T}}} ，$

(1)

定义横滚角、俯仰角、航向角，欧拉角矩阵为：

$ \begin{split}&\left[ {\begin{array}{*{20}{c}} {\cos \theta \cos \phi }&{\cos \theta \sin \phi } \\ {\sin \gamma \sin \theta \cos \phi - \cos \gamma \sin \phi }&{\sin \gamma \sin \theta \sin \phi + \cos \gamma \cos \phi } \\ {\cos \gamma \sin \theta \cos \phi + \sin \phi \sin \gamma }&{\cos \gamma \sin \theta \sin \phi - \sin \gamma \cos \phi } \end{array}} \right.\\ &\qquad\quad \left. {\begin{array}{*{20}{c}} { - \sin \theta } \\ {\sin \gamma \cos \theta } \\ {\cos \gamma \cos \theta } \end{array}} \right] {\text{ = }}\left[ {\begin{array}{*{20}{c}} {{c_{11}}}&{{c_{12}}}&{{c_{13}}} \\ {{c_{21}}}&{{c_{22}}}&{{c_{23}}} \\ {{c_{31}}}&{{c_{32}}}&{{c_{33}}} \end{array}} \right] 。\\[-20pt]\end{split}$

(2)

通过转换矩阵 $ {\boldsymbol{C}}_e^b $ 可以计算出水下航行器欧拉角姿态解算方程:

$ \left\{ {\begin{split} \theta & = - \arcsin ({c_{13}}) ，\\ \gamma &= \arctan ({{{c_{23}}} \mathord{\left/ {\vphantom {{{c_{23}}} {{c_{13}}}}} \right. } {{c_{13}}}})，\\ \phi &= \arctan ({{{c_{12}}} \mathord{\left/ {\vphantom {{{c_{12}}} {{c_{11}}}}} \right. } {{c_{11}}}}) 。\end{split}} \right. $

(3)

通过水下航行器匀速潜行或者悬停的状态下，载体坐标系 $ b $ 测得加速度和磁场强度分别为 ${a^b} = [{{a_x^b}\;{a_y^b}\;{a_z^b} ]^{\rm{T}}}$ ， ${m^b} = {\left[ {m_x^b}\;{m_y^b}\;{m_z^b} \right]^{\rm{T}}}$ ，参考坐标系下加速度 ${a^e} = {\left[0\;0\;g \right]^{\rm{T}}}$ ，东向磁场强度近似为0，磁场强度 ${m^e} = {\left[ 0\;{m_y^e}\;{m_z^e} \right]^{\rm{T}}}$ ，通过坐标变换矩阵得到姿态角数据^[8]，即

$ \theta = \arcsin ({{ - a_x^b} \mathord{/ {\vphantom {{ - a_x^b} g}} } g})，$

(4)

$ \gamma = \arctan ({{a_y^b} \mathord{/ {\vphantom {{a_y^b} {a_z^b}}} } {a_z^b}})，$

(5)

$ \phi = \arctan \left( - \frac{{m_y^b\cos \gamma - m_z^b\sin \gamma }}{{m_x^b\cos \theta + m_y^b\sin \gamma + m_z^b\sin \theta \cos \gamma }}\right) 。$

(6)

针对上述所述传统的单组MEMS传感器进行姿态解算漂移大，鲁棒性低的问题，提出一种信息融合姿态解算方法。

在水下航行器悬停静止或者匀速潜行的时候，MEMS传感器可以测得加速度，磁场强度以及角速度分别为 $a = {\left[ {a_x}\;{a_y}\;{a_z} \right]^{\rm{T}}}$ ， ${m} = {\left[ {m_x}\;{m_y}\;{m_z} \right]^{\rm{T}}}$ ， ${\omega } = {\left[{\omega _x}\;{\omega _y}\;{\omega _z} \right]^{\rm{T}}}$ ，通过坐标变换可以得到：

$ C_e^b\left[ {\begin{array}{*{20}{c}} 0 \\ 0 \\ g \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {a_x^{}} \\ {a_y^{}} \\ {a_z^{}} \end{array}} \right] - \left[ {\begin{array}{*{20}{c}} {\Delta a_x^{}} \\ {\Delta a_y^{}} \\ {\Delta a_z^{}} \end{array}} \right] 。$

(7)

式中： $\Delta {a_x}$ ， $\Delta {a_y}$ ， $\Delta {a_z}$ 加速度计的测量噪声，可以得到俯仰角 $\theta $ 和横滚角 $\gamma $ 。

$ {\theta _{}} = \arcsin \left(\frac{{a_x^{} - \Delta a_x^{}}}{{ - g}}\right) ，$

(8)

$ {\gamma _{}} = \arcsin \left(\frac{{a_y^{} - \Delta a_y^{}}}{{g\cos {\theta _{}}}}\right) ，$

(9)

在北东地参考坐标系下，东向磁场强度为0，即为 ${\left[ {{h_x}}\;0\;{{h_z}} \right]^{\rm{T}}}$ ，同理可求得航向角 $\phi $ 为：

$ \phi = \arccos \left(\frac{{m_x^{} - \Delta m_x^{} + {h_x}\sin {\theta _{}}}}{{{h_x}\cos {\theta _{}}}}\right)。$

(10)

2 水下航行器姿态解算算法设计

所提的姿态解算总体结构图如图2所示。通过MEMS传感器采集的数据，对采集的数据通过低通滤波器，磁力计通过椭圆补偿法进行校正，将所得数据与四元数估计数据的向量积作为PID控制器进行调节，互补滤波修正角速度，然后通过龙格库塔法对四元数微分方程进行更新，进而转化为精确的姿态角。

3 传感器信号预处理

由于受环境因素和传感器自身物理因素的影响，直接安装在载体上易受到各种高频振动的干扰，对姿态解算不利，可通过低通滤波处理提高信号的信噪比，提高姿态解算的精确性。

磁力计主要检测当地的磁场强度，磁场强度在一定的时间内不会发生改变，具体公式为：

$ {\left| {\left| M \right|} \right|^2} = m_x^2 + m_y^2 + m_z^2，$

(11)

其中 $ {m_x},{m_y},{m_z} $ 为磁力计的采集值，在使用MEMS传感器前，对磁力计进行校正，提高数据采集精度，采用椭圆补偿法对磁力计进行修正，根据磁力计的特点及误差特性原理，建立误差模型为：

$ \left[ {\begin{array}{*{20}{c}} {{M_x}} \\ {{M_y}} \\ {{M_z}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{S_x}}&0&0 \\ 0&{{S_y}}&0 \\ 0&0&{{S_z}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{m_x}} \\ {{m_y}} \\ {{m_z}} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {{m_{x0}}} \\ {{m_{y0}}} \\ {{m_{z0}}} \end{array}} \right]。$

(12)

其中： $ {M_x},{M_y},{M_z} $ 为磁力计的实际采集值； $ {m_{x0}},{m_{y0}}, $ $ {m_{z0}} $ 是固定值； $ {S_x},{S_y},{S_z} $ 是磁力计的安装误差系数，空间运动轨迹是以原点为球心，半径为 $ \left| {\left| M \right|} \right| $ 的圆球，在理想状态下，将磁力计在水平面旋转一周，其 x 轴输出和 y 轴输出画在同一个坐标系里，理论上会得到一个球心在原点的球体，由于受周围磁性物质的 扰，其结果往往是球心不在原点，补偿后所示如图3所示。

4 互补滤波对角速度修正

在实际研究中发现，虽然加速度计和磁力计传感器在进行姿态解算时没有累积误差，但动态响应较差，而陀螺仪在进行姿态解算时^[9-10]，会产生累积误差，但动态响应良好，因此，根据它们在频域上互补的特性，可以采用互补滤波融合所测得的信息数据，提高系统的鲁棒性。互补滤波可变换得：

$ \hat \theta (s) = \frac{K}{{S + K}}{\theta _{ref}} + \frac{{{\omega _m}}}{{S + K}}，$

(13)

式（13）的时域为：

$ \hat \omega = K({\theta _{ref}} - \hat \theta ) + {\omega _m} 。$

(14)

由式（14）可得互补滤波为通过磁罗盘算法和四元数估计法得到的姿态角差值作为输入量调节陀螺仪角速度输出，载体在匀速潜行或者悬停静止状态时采用向量积法代替计算得到俯仰角和横滚角误差量，即

$ {e_{\theta \gamma }} = {[ {{c_{13}}}\quad{{c_{23}}}\quad{{c_{33}}} ]^{\rm{T}}} \times {[ {{a_x}}\quad{{a_y}}\quad{{a_z}} ]^{\rm{T}}} ，$

(15)

其中 ${\left[ {{c_{13}}}\;{{c_{23}}}\;{{c_{33}}} \right]^{\rm{T}}} = C_e^b{a^n}$ ， ${a^n}$ 为参考坐标系下加速度的单位向量。由于航向角定义在地理水平面 $x - y$ 内，所以地理坐标系磁力计输出地磁向量为：

$ \left[ {b_x}\quad {b_y}\quad {b_z} \right]^{\rm{T}} = C_e^{b - 1}\frac{m}{\left| m \right|} 。$

(16)

地磁水平夹角0°，固定方向北，所以在地理坐标系下的磁场测量向量 $h = {\left[ {{h_x}}\;{{h_y}}\;{{h_z}} \right]^{\rm{T}}}$ ，各元素有： ${h_x} = $ $ \sqrt {b_x^2 + b_y^2}$ ， ${h_y} = 0$ ， ${h_z} = {b_z}$ ，航向角的误差可得：

$ {e_\phi } = C_e^b\left[ {\begin{array}{*{20}{c}} {{h_x}} \\ {{h_y}} \\ {{h_z}} \end{array}} \right] \times \frac{m}{{\left| m \right|}} ，$

(17)

可以得出MEMS传感器的系统误差为：

$ e = {e_{\theta \gamma }} + {e_\phi } 。$

(18)

采用PID控制器对角速度进行调节， $ u $ 为其输出值，即

$ {u_{}}(t) = {k_p}{e^{}}(t) + {k_i}\sum\limits_{j = 0}^k {{e^{}}(j) + {k_d}\frac{{{e^{}}(k) - {e^{}}(k - 1)}}{t}} ，$

(19)

$ \mathop {{\omega _{}}}\limits^ \wedge = K{u_{}} + \omega _m^{}。$

(20)

根据四阶龙格库塔法可以求出准确的姿态角数据，当水下航行器在稳定潜行时，频率一般在15 Hz左右，选取 $ {f_c} = 15 $ Hz， $K = 0.7$ 。

提出的信息融合姿态解算方法，采用PID控制器对采集的数据进行稳定调节，互补滤波来校正下次采集的新数据，通过四元数微分方程的四阶龙格库塔法求出准确的姿态角数据。

5 仿真验证

为了验证本文提出的方法，在Matlab中设定参考坐标系，将MEMS传感器安装在水下航行器的电子舱中。设定加速度计的随机误差为 ${\text{1}}{\text{.1}} \times {\text{1}}{{\text{0}}^{{{ - 4}}}}$ g，磁力计的随机误差为45 nt,静态测量陀螺仪随机误差为0.2 $ ^\circ {\text{/h}} $ 。仿真从静态情况下，与传统方法进行对比。静态仿真图如图4～图6所示，姿态角误差参数见表1。

由表1可以得到在静态情况下图4单组传感器无调节测姿的标准差为：0.189°，0.205°，0.335°，可以看出系统误差大，鲁棒性差，原因是传感器本身测量精度低，随着解算时间的增加，传感器误差存在累积。图5使用PI调节对传感器测姿标准差为：0.107°，0.183°，0.201°，而本文方法进行测姿的标准差为：0.087°，0.077°，0.132°,可以看出姿态角误差有了进一步缩小，且趋于稳定，鲁棒性更好。

对MEMS传感器进行简单的动态测试，保持俯仰角不变，横滚角和航向角进行动态运动，理想的运动状态轨迹如图7中浅色轨迹。通过图7可以得到，本文融合解算方法在动态测试中，实际的运动轨迹在理想状态轨迹线的边缘来回波动，接近真实状态。

6 结　语

本文对传统单组MEMS传感器测姿做出分析，其精度已经满足不了实际需求，因此提出一种信息融合姿态解算方法。先将传感器信号进行预处理，磁力计使用椭圆补偿法进行处理，信号处理后的传感器，对载体进行数据采集，通过PID控制器进行调节，采用互补滤波对角速度数据调节来抑制陀螺仪漂移。经仿真结果可得相较于传统的姿态解算本文所提出的方法鲁棒性更好，优势凸出，更能满足实际需求。