基于双微机电惯性测量单元的船用自主水平姿态估计算法

顾元鑫，吴文启，王茂松，郭 航

（国防科技大学智能科学学院，长沙 410073）

船载惯性姿态基准系统应用广泛[1]，与采用光纤陀螺、激光陀螺的系统相比，采用微机电惯性测量单元具有体积小、功耗低的优势，由于成本低，可在船上不同区域分布式联网布设多套系统，与少量的光学陀螺惯性姿态基准系统配合，建立高性价比的全船统一姿态参考基准。但微机电陀螺、加速度计精度较低，存在零偏误差和高频噪声[2]，为解决这一问题，可以采用与卫星定位系统或其他导航传感器组合[3]或引入惯性测量单元（IMU）旋转调制技术的方案[4,5]，目前法国的MEMS姿态航向参考系统Ellipse-A（在卫星数据有效的情况下）的水平角精度可达到0.2°，航向角精度可以达到1°，文献[6]中采用互补滤波与卡尔曼滤波的融合姿态测量算法，在卫星数据有效的情况下可以实现水平姿态精度0.2°，航向角精度0.5°。在实际应用中，有些应用背景要求能在卫星信号拒止的条件下实现自主姿态测量。另外，IMU旋转调制虽然可以抑制陀螺、加速度计零偏误差影响，但旋转本身会造成系统水平姿态解算输出结果出现锯齿状误差，对于陀螺、加速度计零偏误差较大的低精度系统更加显著，且IMU转动过程中，即使在有测角系统辅助的情况下所提供的载体姿态精度也往往会受到一定影响。

本文提出一种基于双微机电IMU的水平姿态测量系统方案，系统中两个IMU安装在一起，一个惯性测量单元以捷联方式与船体相固联，为姿态输出IMU；另一个惯性测量单元通过单轴旋转机构可绕方位轴180 °转位。基于双微机电惯性测量单元相对关系约束，以两套IMU之间相对位置、速度和姿态为观测量[7]，采用状态变换卡尔曼滤波算法[8-11]实现了无卫星辅助信息下的陀螺、加速度计零偏估计，提高了水平姿态动态测量精度；由于姿态输出IMU与船体相固联，克服了IMU旋转调制造成的系统水平姿态解算输出结果锯齿状误差的缺陷。

在无外界参考速度信息时，姿态参考系统相当于工作在内阻尼模式中，传递函数分析与实验结果表明，虽然船体机动时，未知的船体质心速度变化会影响水平姿态相对于当地水平面的绝对精度，但不会影响船体各分布水平姿态基准之间的相对水平姿态精度，而当舰船上有高精度主惯导系统可提供速度参考时，即使有常值速度偏差和舒勒周期速度误差，也可确保绝对姿态测量精度。

1 基于双微机电惯性测量单元相对关系约束的状态变换卡尔曼滤波算法

1.1 基于状态变换的系统误差微分方程

与船体相固联的MEMS-IMU记为捷联惯性测量单元，可通过旋转机构转位的MEMS-IMU记为旋转调制惯性测量单元，两个惯性测量单元在北东地地理坐标系（NED）下的速度误差微分方程、姿态误差微分方程、位置误差微分方程形式是一致的，式(1)为状态变换后新的速度误差向量与原速度误差向量的关系[12]。状态变换卡尔曼滤波通过将传统滤波模型中的速度误差状态变换为新的考虑姿态误差的速度误差状态，使状态转移矩阵中动态变化的比力项变换为相对稳定的重力项，从而使误差状态协方差矩阵的更新计算更准确，可以有效地克服滤波估计协方差与实际协方差不一致造成的滤波误差状态估计不准确的问题。

位置误差向量定义为：

状态变换后速度、姿态、位置误差微分方程如式(3)-(5)所示[12]：

其中Frr和Frv如下所示：

由于微机电惯性测量单元的陀螺及加速度计零偏稳定性较差，建模为随机常值不适合长时间连续工作的情况，因此将陀螺及加速度计的零偏建模为一阶马尔科夫过程：

其中τg、τa为相关时间常数，为过程噪声。

将船舶质心运动的水平速度的估计值建模为一阶马尔科夫过程：

其中τov为一阶马尔科夫过程中相关时间常数，wovN、wovE为过程噪声。

1.2 系统状态方程

双微机电惯性测量单元联合的状态变换卡尔曼滤波的系统方程可以表示为：

其中：

注意到两个IMU安装在一起，其速度、位置是一致的，则状态转移矩阵如下所示：

其中：

过程噪声向量如下所示：

系统噪声矩阵如下所示：

其中：

1.3 系统观测方程

观测方程可表示如下：

其中z(t)为观测向量，H(t)为量测矩阵，υ（t）为量测噪声向量。

其中观测向量选取如下：

可通过求解式(30)得到：

可通过式(32)得到求解：

可通过式(34)，根据两个微机电惯性测量单元1b和2b位置测量值进行求解：

υr12=[υr12Lυr12λυr12h]T为相应的观测噪声。

舰船在水面运动，对于舰船，观测方程中IMU高度有外界参考信息，则可以得到高度观测值则有：υh1为相应的观测噪声。

量测矩阵如下所示：

系统的量测噪声向量如下所示：

在旋转调制微机电惯性测量单元处于转动状态时，只进行滤波的时间更新方程解算，不进行测量更新方程解算；在0 °、180 °位置锁定时，才进行测量更新方程解算。

2 基于传递函数的水平姿态动态误差分析

状态变换卡尔曼滤波稳定后，滤波增益矩阵近似为常值，可用传递函数分析水平通道的动态姿态误差[13]。

以捷联微机电惯性测量单元b1为对象，并忽略船的速度与姿态误差的乘积，不区分RE，RN，均用R表示，则有：

图1 东向水平通道误差传递函数框图Fig.1 Block diagram of error transfer function of eastbound horizontal channel

图2 北向水平通道误差传递函数框图Fig.2 Block diagram of error transfer function of northbound horizontal channel

根据两个微机电惯性测量单元b1和b2之间的相对位置、相对速度和相对姿态观测信息，可估计出水平加速度计零偏误差和水平陀螺零偏误差。由于纬度及垂向速度误差小，对水平姿态角测量误差的影响可忽略。由传递函数可知，稳态后由初始速度及初始姿态角所引起的姿态误差均为零。杆臂参数已知的情况下，杆臂速度所导致的水平姿态角误差也可得到补偿，因此姿态测量误差的主要影响因素为舰船质心速度，下面分两种情况进行研究：

1) 无主惯导提供速度参考信息：

此时系统实际上工作在内阻尼状态，式(46)可简化为如下所示：

其中

根据式(50)，只要船舶的质心运动的动态性足够小，通过时域状态变换卡尔曼滤波结合频域设计优化滤波增益矩阵中参数KE1、KE2，即可在内阻尼状态下仍能得到足够精确的水平姿态测量结果。根据姿态基准的传递函数可得舰船质心正弦运动速度的幅值及角频率对姿态测量精度的影响，如图3所示。

图3 船质心速度正弦运动幅值及角频率对姿态测量精度的影响Fig.3 The influence of ship′s center of mass velocity sinusoidal motion amplitude and angular frequency on the accuracy of attitude measurement

由图3可知，随着船体质心正弦运动幅值的增加，姿态基准系统的姿态解算误差逐渐变大。基于本文的微机电传感器精度指标和滤波参数，当船质心运动角频率为舒勒振荡角频率 1.24× 1 0-3rad/s 时（周期约为84.4×60 s），要想姿态测量绝对精度优于0.02°，则要求船体的质心正弦运动速度的幅值不超过2.76m/s。另随着船体质心正弦运动角频率的增加，姿态基准系统的姿态解算误差幅值先逐渐变大后又逐渐变小最后趋于收敛，其衰减原因是水平姿态测量系统相当于带通滤波器，当船体质心运动频率高时会被过滤掉，基于本文的微机电传感器精度指标和滤波参数，当船体质心运动角频率约为 6× 1 0-3rad/s 时（约为5倍的舒勒角频率，周期约为950秒），其姿态测量误差幅值最大。当舰船质心运动导致姿态测量精度低于应用要求时，可通过不同位置处的姿态基准系统间求相对姿态的方法，获得精度较高的相对姿态值，其分析过程如下，舰船上不同位置处水平姿态基准东向水平通道误差传递函数如下：

式(52)及式(53)分别表示舰船上不同位置处姿态基准系统的水平通道东向回路误差传递函数，可知其中造成姿态测量值差异的主要因素为传递函数中的参数KE1、KE2以及舰船东向质心速度，由于滤波算法及参数相同，且环境因素也几乎相同，所以不同位置处水平姿态基准传递函数中的参数K1E、KE2分别近似一致，船上各处水平姿态基准可采用相同的舰船质心水平速度估计值，故滤波稳定后舰船质心速度所造成的动态姿态误差是近似相同的，不同位置处水平姿态基准北向通道传递函数中的参数KN1、KN2也分别近似一致，有同样结论，即有：

各分布式水平姿态基准之间的相对姿态精度，即使在内阻尼状态下，其一致性也不受舰船的质心运动影响。2) 有主惯导提供速度参考信息：

由式(56)可知此时影响姿态测量精度的主要因素为主惯导系统的速度测量精度，姿态测量精度最差发生在主惯导无阻尼的情况下，主惯导速度误差呈舒勒周期振荡。根据传递函数式(56)可以计算出主惯导系统速度测量精度对姿态测量误差的影响，如图4所示。由于舰船上的主惯导系统的速度测量精度都高于2m/s，由图4可知，当有舰船高精度主惯导系统提供的速度为观测量时，姿态基准系统的水平姿态测量精度总可以优于 0.02°（1σ）。

图4 主惯导速度误差舒勒振荡幅值对姿态测量精度影响Fig.4 The influence of main inertial navigation velocity error Schuler oscillation amplitude on attitude measurement accuracy

3 实验系统设计

本姿态基准系统中的旋转微机电惯性测量单元采用0 °→ 180 °→ 0°每个位置停留5min的旋转方案，通过往复旋转，以两套惯性测量单元间的相对速度、相对位置、相对姿态为观测量，实现两套惯性测量单元陀螺、加速度计零偏的自主估计，进而减弱微机电惯性仪表零偏误差对船体姿态测量精度的影响。系统采用的微机电惯性测量单元的性能指标如表1所示。

表1 微机电惯性测量单元性能指标参数Tab.1 Specifications of the MEMS-IMU

4 实验结果与分析

4.1 海况模拟实验设计

本节通过利用三轴角运动模拟转台来实时模拟舰船在海上的摇摆情况，进而验证基于双微机电惯性测量单元船用自主水平姿态基准系统的姿态测量精度。实验装置图如图5所示。

图5 海况模拟实验Fig.5 Sea state simulation experiment

本实验的实验过程如表2所示。

表2 海况模拟参数Tab.2 Sea state simulation parameter

图5 中1为与运载体固联的微机电惯性测量单元，2为单轴旋转的微机电惯性测量单元，3为数据采集处理模块。为进行动态姿态验证，角运动模拟转台与微机电惯性测量单元已实现姿态数据时间同步。图5中4为三轴角运动模拟转台。

4.2 海况模拟实验结果

基于三轴角运动模拟转台按照上述实验流程进行了三组海况模拟实验，重点验证无主惯导信息的恶劣情况。求取各阶段实验水平姿态角误差均方差如表3所示。

表3 海况模拟实验水平姿态角误差均方差Tab.3 Mean square error of horizontal attitude angle in sea state simulation experiment

从实验结果可知，本姿态测量方案所测得的三轴角运动模拟转台的水平姿态输出值与三轴角运动模拟转台的实际水平姿态角输出值间误差的标准差小于0.02°。

4.3 仿真试验

由于捷联惯性测量单元北向水平通道与东向水平通道相似，下面以东向水平通道为例进行质心速度对姿态测量精度影响的仿真试验。在姿态基准系统仿真试验中对运载体加以0.1°的模拟载体初始纬度误差及的船体东向速度，即海浪作用下，舰船质心运动周期为50 s。由于姿态基准系统输出与捷联微机电惯性测量单元输出一致，因此下文中捷联微机电惯性测量单元也代表姿态基准系统，由于捷联微机电惯性测量单元与旋转微机电惯性测量单元的基本指标相同，因此为了在陀螺零偏及加速度计零偏估计时将二者的相应零偏值更好地区分，这里对捷联微机电惯性测量单元加以1°/h的常值陀螺偏移，以及1mg的常值加速度计偏移，对旋转微机电惯性测量单元加以-1°/h 的常值陀螺偏移，以及-1mg的常值加速度计偏移，在引入上述误差后姿态解算的结果如图6所示。由图6可知，一方面，由姿态基准系统所解算出的水平姿态角要比单独采用旋转微机电惯性测量单元解算出的水平姿态角的精度要高；另一方面，由于单轴旋转测量单元0 °→ 180 °→ 0°往复旋转，使得旋转测量单元在其旋转过程中所测得的水平姿态值不能反映船体实际姿态，而采用双微机电协同转位方案的姿态基准系统可以很好地解决这一问题，使得解算出的水平姿态角的精度及可信性更高。从图6中可知，当仿真试验条件与4.1节中海况模拟实验一致，运载体存在质心速度，而质心加速度变化较小且纬度信息有误差的情况下，姿态基准系统的水平姿态角测量精度可维持在 002°（1σ）以内，与前述载体无质心运动速度情况下实验所得的结论相同。

图6 姿态基准系统与旋转惯性测量单元滚转角误差对比Fig.6 Comparison of roll angle error between attitude reference system and rotary inertial measurement unit

图7 -8为捷联微机电惯性测量单元及旋转微机电惯性测量单元陀螺零偏、加速度计零偏的滤波估计值，可知微机电惯性测量单元的陀螺及加速度计零偏由于相对观测量的引入均得到了较好地估计，其中由于天向陀螺的可观测性较差，其估计的情况相较于东向和北向陀螺也较差，但最终也已经收敛。

图7 陀螺零偏估计曲线Fig.7 Gyro bias estimation curve

图8 加速度计零偏估计曲线Fig.8 Accelerometer bias estimation curve

在姿态基准系统试验中，将船体质心速度的角频率设置为造成最大姿态误差的情况，将船体东向速度设置为其仿真试验结果如图9中红色曲线所示。

由仿真试验结果图9中红色曲线可知，当进一步加大运载体质心速度误差后，姿态基准系统的水平姿态测量精度有所降低，其误差超过了0.02°。由于在船上，纵使处在不同位置的姿态基准系统其所感应到的船体质心速度也应为同一值，因此当船体质心速度在一定范围内增大时即使会对单套姿态基准系统的姿态测量精度产生影响，但对两套姿态基准系统所测得的相对水平姿态值的精度应无影响，二者所测得的水平姿态角的差值应保持高精度。为验证上述结论，取同一姿态基准系统不同组别实验数据来模拟船上不同位置处的姿态基准系统，试验中运载体的质心速度与前述一致，为纬度初始误差为0.1°。位置1处及位置2处的滚转角误差值如图9所示，二者间的相对滚转角误差如图10所示。

图9 姿态基准系统滚转角误差曲线Fig.9 Error curve of roll angle of attitude reference system

图10 位置1与位置2间姿态基准系统相对滚转角误差Fig.10 Relative roll angle error of attitude reference system between position 1 and position 2

由图9及图10可知，即使由于载体质心速度加大导致单个姿态基准系统水平姿态测量精度下降，但不同的姿态基准系统间的相对姿态测量精度在1200 s滤波稳定后仍保持在相对较高的水平，在0.02°左右，造成这一相对姿态测量误差的主要原因是由于船上不同位置处环境因素不同，对于船上不同位置处的姿态基准系统，舰船质心速度到水平姿态误差间的传递函数中KE1、KE2、KN1、KN2分别存在差异，进而导致船上不同位置处姿态基准系统的水平姿态测量误差值不同，最终导致了相对姿态测量误差，但其精度仍可维持在较高的水平，这一结论将为后续的多套姿态基准系统协同工作提供一定的理论可行性基础，也进一步说明了本姿态基准系统的自主性。

5 结 论

本文完成了一种不需要依赖外界信息的基于双微机电惯性测量单元的船用自主式水平姿态基准系统算法设计，通过采用双惯性测量单元协同转位的方式，以两套惯性测量单元间相对位置、速度、姿态关系为观测约束，基于状态变换卡尔曼滤波完成载体姿态测量，并通过基于三轴角运动模拟转台的海况模拟实验进行了精度验证。本姿态测量方案在船体质心加速度变化较小时，在不依赖外界信息的情况下可以实现水平姿态角测量绝对精度优于 0.02°（1σ），而在载体质心加速度变化较大时，虽然姿态基准系统的水平姿态角测量绝对精度超出 0.02°（1σ），但是不同姿态基准系统间的姿态测量相对精度仍可维持在 002°（1σ），进而可以利用两套姿态基准系统来实现不同位置间相对水平姿态的测量。而当舰船上有主惯导系统可提供速度参考时，尽管主惯导存在常值速度误差和舒勒周期速度误差，微机电姿态基准系统的水平姿态测量精度仍可优于 0.02°（1σ）。综上所述，采用本系统及相应算法可以在尽可能微型化、低功耗、低成本的前提下，实现在海况较差且无外界参考信息的情况下较好地完成舰船上子系统姿态的高精度测量，为舰船上子系统的高精度姿态测量提供了一种可行的技术方案。