CN102768043B - 一种无外观测量的调制型捷联***组合姿态确定方法 - Google Patents

Abstract

本发明提供的是一种无外观测量的调制型捷联***组合姿态确定方法。采用全球定位***(GPS)确定载体初始位置参数并装订至导航计算机中；采集光纤陀螺仪和石英加速度计输出的数据并进行处理；设定惯性测量单元(IMU)单轴四位置转停方案；将加速度计输出转换到载体半固定坐标系；设计无限冲击响应(IIR)数字高通滤波器，对导航系下解算的载体速度滤波；将滤波后的速度与调制型捷联***解算出的速度作差后作为***观测量，采用卡尔曼滤波技术估计调制型捷联惯导***的姿态信息。本发明不需要外界辅助设备提供观测信息，可有效解决辅助设备提供信息频率与调制型捷联***解算频率不匹配问题，实现调制型捷联惯导***的组合姿态确定。

Description

一种无外观测量的调制型捷联***组合姿态确定方法

(一)技术领域

本发明涉及的是一种测量方法，尤其涉及的是一种无外观测量的调制型捷联***组合姿态确定方法。

(二)背景技术

在捷联惯性导航***中，所有的惯性测量元件直接安装在载体上，惯性元件输出的就是载体相对于惯性空间的角速度和加速度，由计算机将载体坐标系下测得的加速度数据转换到导航坐标系再进行导航解算，相当于利用陀螺仪输出数据在计算机内构建一个数学平台作为导航计算的参考。由于捷联***没有平台框架及相连的伺服机构，因而简化了硬件，与平台惯导相比具有体积小、重量轻、成本低、可靠性比较高等优点。正是由于以上优点，它在航空、航天、航海和很多民用领域得到了广泛应用。

旋转调制技术是惯性导航***的一种自校正方法。调制型捷联惯性导航***在捷联惯导***的外面加上转动机构和测角装置，导航解算仍采用捷联惯导算法。它不需要引入外部校正信息，能够自动对***中惯性器件的常值偏差进行平均，达到抵消漂移对***精度的影响。因而可以提高惯性导航***长时间工作的精度，充分发挥惯性导航“自主式”的优点。应用旋转调制技术，还可以应用较低精度的惯性器件，构成较高精度的惯性导航***，有利于降低惯性导航***的成本，同时由于引入外界运动可以有效地提高惯导***部分参数的可观测度。

初始姿态误差是惯性导航***主要的误差源之一，初始姿态的误差对***误差的影响不仅表现在姿态指标上，而且表现在速度和位置信息的获取上。初始姿态确定的精度直接影响着导航的精度。按基座的运动状态来分，惯导***的初始姿态确定方法可以分为两类，即静基座初始姿态确定和动基座初始姿态确定。所谓静基座初始姿态确定是指惯性测量组合在载体完全静止的情况下确定载体的初始姿态信息。目前，该技术已经比较成熟，通过多位置法和卡尔曼滤波已经能够达到相当高的精度。但是，静基座初始姿态确定技术对于众多的机载和舰载武器***来说已经不能适应它们快速反应的要求，所以当前研究的重点主要集中在动基座初始姿态确定方法。所谓动基座初始姿态确定是指惯性测量组合在载体运动或者外界扰动的情况下完成。其难点在于构造快速、稳定、鲁棒性强的滤波器，并且对滤波器状态的可观测性和可观测度进行评价，从而设计最优滤波器。根据使用外部信息的情况，可以将动基座初始姿态确定方法分为三类：外部阻尼式、传递式和外部辅助姿态信息式。

滤波器的功能就是允许某一部分频率的信号顺利的通过，而另外一部分频率的信号则受到较大的抑制，它实质上是一个选频电路。无限冲击响应(IIR)滤波器属于经典滤波器，即假定输入信号中的有用成分和希望除去的成分各自占有不同的频带。这种假定在一定程度上符合了客观规律，对于高通滤波器来说，采样序列的高频部分包含有用信号，而低频部分则主要是由舒勒周期振荡控制。因此通过设计合理的高通滤波器，就可以达到去除舒勒周期振荡的目的。

(三)发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种无外观测量的调制型捷联***组合姿态确定方法。

本发明的技术解决方案为：一种无外观测量的调制型捷联***组合姿态确定方法，其特征在于采用惯性测量单元单轴四位置转停，提出载体瞬时速度的提取方法，根据瞬时速度的误差特性，采用无限冲击响应(IIR)数字高通滤波器滤除载体速度中的舒勒周期，将滤波后的速度信息和惯导解算出的速度信息作差后作为***的观测量，采用卡尔曼滤波技术实现捷联惯导***的组合姿态，其具体步骤如下：

(1)通过GPS确定载体的初始位置参数，将它们装订至导航计算机中；

(2)调制型捷联惯导***进行预热准备，采集光纤陀螺仪和石英加速度计输出的数据并对数据进行处理；

(3)IMU采用4个转停次序为一个旋转周期的转位方案(如附图2)；

次序1，IMU由位置A出发，顺时针旋转180°到位置B，并在位置B停留时间T_s；次序2，IMU由位置B出发，顺时针旋转90°到达位置C，并在位置C停留时间T_s；次序3，IMU由位置C出发，逆时针旋转180°到位置D，在位置D停留时间T_s；次序4，IMU由位置D出发逆时针旋转90°回到位置A，并在位置A停留时间T_s；然后按照次序1～4的顺序循环运动。

IMU每次转动180°或90°间隔进行。从一个位置转动180°到对称位置，在这两个相互对称的位置上，水平方向上惯性敏感元件的常值漂移在进行导航计算的时候能够相互抵消掉。通过旋转90°到达另外一个新位置。

(4)将加速度计的输出转换到载体半固定坐标系，利用调制型捷联惯导***中的积分环节提取载体瞬时线速度信息；

1)引入载体半固定坐标系

以舰船重心为载体半固定坐标系原点，纵轴OY_d指向舰船的主行向方向，横轴OX_d垂直于纵轴平行于水平面，在舰船无纵摇运动时指向右舷方向。垂直轴OZ_d与前两轴垂直，沿船只竖轴向上为正(如附图3)。其中ψ_G为主航向角，γ角为航向摇摆角(即艏摇角定义其与主航向角同向为正)。载体半固定坐标系的引入使得测量结果和角运动基本脱离，可以准确地描述舰船的瞬时线运动，因此采用载体半固定坐标系作为研究舰船瞬时线运动或称为平动的基础坐标系。

2)建立惯性测量单元坐标系与载体半固定坐标系的转换矩阵

首先建立载体坐标系与载体半固定坐标系之间相差三个旋转角，可视为半固定坐标系经三次旋转后与载体坐标系重合，三个角度分别为：纵摇角α、横摇角β及艏摇角γ(如附图4)。载体坐标系(b系)转换到载体半固定坐标系(d系)的方向余弦矩阵

$C_b^d=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \sin \mathrm{\γ}\sin \mathrm{\α}-\cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ -\sin \mathrm{\γ}\cos \mathrm{\β}& \cos \mathrm{\γ}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \cos \mathrm{\γ}\sin \mathrm{\α}-\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ \sin \mathrm{\β}& -\cos \mathrm{\β}\sin \mathrm{\α}& \cos \mathrm{\β}\cos \mathrm{\α}\end{array}\right]$

现有的惯性导航***已经可以提供较为精确的姿态角信息，其中水平两个姿态角信息即为纵摇角信息和横摇角信息，而航向信息提供的是航向角信息ψ，这有别于艏摇角γ，但舰船的操纵者可以提供准确的主航向信息ψ_G，可得

γ＝ψ-ψ_G

将γ代入的计算方程中，即可得到载体坐标系转换到载体半固定坐标系的方向余弦矩阵。

由于惯性测量单元相对载体存在绕方位轴的转位运动，因此惯性测量坐标系(s坐标系)与载体坐标系之间的转换矩阵可以利用下式进行计算：

$C_s^b=\left[\begin{array}{ccc}\cos \mathrm{\ωt}& -\sin \mathrm{\ωt}& 0\\ \sin \mathrm{\ωt}& \cos \mathrm{\ωt}& 0\\ 0& 0& 1\end{array}\right]$

式中，ωt表示惯性测量单元相对载体坐标系的相对角度关系。因此可以得到惯性测量单元坐标系转换到载体半固定坐标系的方向余弦矩阵：

$C_s^d=C_b^d{C}_s^b$

(5)设计合理的无限冲击响应数字高通滤波器(IIR)，将导航系下解算出的载体速度进行高通滤波处理；

1)确定所设计数字高通滤波器的技术指标

高通数字滤波器f_p1、f_s1、δ_p、δ_s的技术指标是根据信号特征和采样频率f_s给定的。其中，f_p1为通带截止频率，f_s1为阻带截止频率，δ_p为通带波纹，即滤波器通带内偏离单位增益的最大值，通带边缘增益为1-δ_p，δ_s为阻带波纹，即滤波器阻带内偏离单位增益的最大值，阻带边缘处滤波器的增益为δ_s。通带及阻带的衰减α_p、α_s分别定义为-20log(1-δ_p)、-20log(1-δ_s)。

舒勒周期振荡信号相对来说属于低频信号，振荡周期是84.4分钟。而舰船瞬时线运动是由海洋环境因素引起的，最主要的产生原因是海浪的影响，所以舰船瞬时线运动是频率与海浪频率大体一致的往复运动。而且舰船瞬时线运动相对于舰船的航行运动，属于高频运动，运动周期比较短，一般在1.5秒～10秒左右，频率为0.67赫兹。根据升沉横荡纵荡运动和舰船常规工作运动的运动特性上的不同，设计所需要的滤波器的技术指标要求，具体设计指标在试验过程中根据滤波效果进行调整，以达到最优滤波效果为准。

2)将技术指标从数字滤波器转换到模拟滤波器

技术指标从模拟滤波器到数字滤波器的变换采用双线性Z变换法，设计数字高通滤波器的技术指标为f_p1，f_s1，δ_p，δ_s，t_s＝0.0102。首先应得到数字边缘频率Ω，因为2π对应采样频率f_s，而f_s＝1/t_s，所以有：

$\frac{f_{p1}}{f_s}=\frac{{\mathrm{\Ω}}_{p1}}{2\mathrm{\π}}$

$\frac{f_{s1}}{f_s}=\frac{{\mathrm{\Ω}}_{s1}}{2\mathrm{\π}}$

所以求得：

Ω_s1＝2πf_s1/f_s

Ω_p1＝2πf_p1/f_s

按照双线性Z变换法的频率转换关系ω＝2f_stan(Ω/2)继续转换有：

${\mathrm{\Ω}}_p=\tan \left(\frac{{\mathrm{\ω}}_p}{2}\right)$

${\mathrm{\Ω}}_s=\tan \left(\frac{{\mathrm{\ω}}_s}{2}\right)$

以此将数字高通滤波器的技术指标就转换成为模拟高通滤波器的技术指标。

(6)根据调制型捷联惯导***动基座误差方程建立载体系泊状态时的组合姿态误差模型，以高通滤波后得到的速度与调制型捷联惯导***直接解算出的速度作差后作为***观测量。利用卡尔曼滤波技术实现调制型捷联惯导***组合姿态的确定；

建立以经过高通滤波后的水平速度与调制型捷联惯导***直接解算出的速度作差后作为观测量的卡尔曼滤波模型；

用一阶线性微分方程来描述调制型捷联惯导***的状态误差：

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW}$

其中，X为***的状态向量；A和B分别为***的状态矩阵和噪声矩阵；W为***噪声向量；

***的状态向量为：

***的白噪声向量为：

W＝[a_x a_y ω_x ω_y ω_z  0 0 0 0 0]^T

其中δV_e、δV_n分别表示东向、北向的速度误差；分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；ω_x、ω_y、ω_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的白噪声误差；

***的状态转移矩阵为：

$A=\left[\begin{array}{cccc}{F}_{2\×2}^1& f_{2\×3}& {\tilde{T}}_{2\×2}& O_{2\×3}\\ F_{3\×2}^2& F_{3\×3}^3& O_{3\×2}& T_{3\×3}\\ O_{5\×2}& O_{5\×3}& O_{5\×2}& O_{5\×3}\end{array}\right]$

$F_{2\×2}^1=\left[\begin{array}{cc}\frac{V_N}{R_n}\tan L^{\′}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_n}\tan L\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{2V_E}{R_n}\tan L)& 0\end{array}\right]$

$F_{3\×2}^2=\begin{array}{}\left[\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{R_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right]\end{array}$

$F_{3\×3}^3=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n})& 0& -\frac{V_N}{R_m}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n}& \frac{V_N}{R_m}& 0\end{array}\right]$

$f_{2\×3}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& f_E\end{array}\right]$

${\tilde{T}}_{2\×2}=\left[\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right]$

$T_{3\×6}=\left[\begin{array}{ccc}-T_{11}& -T_{12}& -T_{13}\\ -T_{21}& -T_{22}& -T_{23}\\ -T_{31}& -T_{32}& -T_{33}\end{array}\right]$

V_E、V_N分别表示东向、北向的速度；ω_x、ω_y、ω_z分别表示陀螺的三个输入角速度；ω_ie表示地球自转角速度；R_m、R_n分别表示地球子午、卯酉曲率半径；L表示当地纬度；L′表示系泊状态初始时刻载体纬度信息；f_E、f_N、f_U分别表示为导航坐标系下东向、北向、天向的比力。

2)建立卡尔曼滤波的量测方程：

用一阶线性微分方程来描述调制型捷联惯导***的量测方程如下：

Z＝HX+V

其中：Z表示***的量测向量；H表示***的量测矩阵；V表示***的量测噪声；

***量测矩阵为：

$H=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

量测量为调制型捷联惯导***解算的东向速度V_E、北向速度V_N分别和经过高通滤波处理得到的东向速度北向速度之差：

$Z=\left[\begin{array}{c}{V}_E-{\tilde{V}}_E\\ V_N-{\tilde{V}}_N\end{array}\right]$

本发明与现有技术相比的优点在于：本发明打破了调制型捷联惯导***组合姿态确定过程中外部设备提供信息频率与调制型捷联惯导***提供信息频率不匹配这一问题，在惯性测量单元四位置转停方案下的调制型捷联惯导***中，利用载体瞬时速度信息中存在低频的舒勒周期信息和载体系泊状态存在的摇摆及荡运动的高频扰动信息这一特性，提出利用IIR数字高通滤波器滤除载体瞬时速度信息中的舒勒周期项，将滤波后的速度和惯导解算速度作差后作为***的观测量，采用卡尔曼滤波技术实现捷联惯导***的组合姿态。因此不需要外部设备为其提供参考信息。

对本发明有益的效果说明如下：

在VC++仿真条件下，对该方法进行仿真实验：

载体作三轴摇摆运动。载体以正弦规律绕纵摇轴、横摇轴和航向轴摇摆，其数学模型为：

$\left\{\begin{array}{c}\mathrm{\θ}={\mathrm{\θ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\θ}}t+{\mathrm{\φ}}_{\mathrm{\θ}})\\ \mathrm{\γ}={\mathrm{\γ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\γ}}t+{\mathrm{\φ}}_{\mathrm{\γ}})\\ \mathrm{\ψ}={\mathrm{\ψ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\ψ}}t+{\mathrm{\φ}}_{\mathrm{\ψ}})+k\end{array}\right.$

其中：θ、γ、ψ分别表示纵摇角、横摇角和航向角的摇摆角度变量；θ_m、γ_m、ψ_m分别表示相应的摇摆角度幅值；ω_θ、ω_γ、ω_ψ分别表示相应的摇摆角频率；φ_θ、φ_γ、φ_ψ分别表示相应的初始相位；ω_i＝2π/T_i，i＝θ、γ、ψ，T_i表示相应的摇摆周期，k为初始航向角。仿真时取：θ_m＝6°，γ_m＝12°，ψ_m＝10°，T_θ＝8s，T_γ＝10s，T_ψ＝6s，k＝0°。

载体的横荡、纵荡和垂荡引起的线速度为：

式中，i＝x，y，z为地理坐标系的东向、北向、天向。 为[0，2π]上服从均匀分布的随机相位。

载体初始位置：北纬45.7796°，东经126.6705°；

初始姿态误差角：三个初始姿态误差角均为零；

赤道半径：R_e＝6378393.0m；

椭球度：e＝3.367e-3；

由万有引力可得的地球表面重力加速度：g₀＝9.78049；

地球自转角速度(弧度/秒)：7.2921158e-5；

陀螺仪常值漂移：0.01度/小时；

陀螺仪随机游走：

加速度计零偏：10^-4g₀；

加速度计噪声：10^-6g₀；

常数：π＝3.1415926；

IMU四位置转停方案的数学模型参数：

四个位置的停顿时间：T_s＝5min；

转动180°和90°时消耗的时间：T_z＝12s；

转动180°和90°的过程中，每一个转位中的加减速时间各为4s；

利用本发明所述的方法得到调制型捷联***失准角曲线，如图5所示。结果表明有摇摆干扰条件下，采用本发明方法可以获得较高的对准精度。

(四)附图说明

图1为本发明的一种无外观测量的调制型捷联***组合姿态确定方法流程图；

图2为本发明的IMU单轴四位置转停；

图3为本发明的定义载体半固定坐标系；

图4为本发明的载体坐标系与载体半固定坐标系的转换关系；

图5为本发明的无外观测量的调制型捷联***卡尔曼滤波估计的失准角曲线。

(五)具体实施方式

下面结合附图对本发明的具体实施方式进行详细地描述：

(1)通过GPS确定载体的初始位置参数，将它们装订至导航计算机中；

(2)调制型捷联惯导***进行预热准备，采集光纤陀螺仪和石英加速度计输出的数据并对数据进行处理；

(3)IMU采用4个转停次序为一个旋转周期的转位方案(如附图2)；

次序1，IMU由位置A出发，顺时针旋转180°到位置B，并在位置B停留时间T_s；次序2，IMU由位置B出发，顺时针旋转90°到达位置C，并在位置C停留时间T_s；次序3，IMU由位置C出发，逆时针旋转180°到位置D，在位置D停留时间T_s；次序4，IMU由位置D出发逆时针旋转90°回到位置A，并在位置A停留时间T_s；然后按照次序1～4的顺序循环运动。

IMU每次转动180°或90°间隔进行。从一个位置转动180°到对称位置，在这两个相互对称的位置上，水平方向上惯性敏感元件的常值漂移在进行导航计算的时候能够相互抵消掉。通过旋转90°到达另外一个新位置。

(4)将加速度计的输出转换到载体半固定坐标系，利用调制型捷联惯导***中的积分环节提取载体瞬时线速度信息；

1)引入载体半固定坐标系

以舰船重心为载体半固定坐标系原点，纵轴OY_d指向舰船的主行向方向，横轴OX_d垂直于纵轴平行于水平面，在舰船无纵摇运动时指向右舷方向。垂直轴OZ_d与前两轴垂直，沿船只竖轴向上为正(如附图3)。其中ψ_G为主航向角，γ角为航向摇摆角(即艏摇角定义其与主航向角同向为正)。载体半固定坐标系的引入使得测量结果和角运动基本脱离，可以准确地描述舰船的瞬时线运动，因此采用载体半固定坐标系作为研究舰船瞬时线运动或称为平动的基础坐标系。

2)建立惯性测量单元坐标系与载体半固定坐标系的转换矩阵

首先建立载体坐标系与载体半固定坐标系之间相差三个旋转角，可视为半固定坐标系经三次旋转后与载体坐标系重合，三个角度分别为：纵摇角α、横摇角β及艏摇角γ(如附图4)。载体坐标系(b系)转换到载体半固定坐标系(d系)的方向余弦矩阵

$C_b^d=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \sin \mathrm{\γ}\sin \mathrm{\α}-\cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ -\sin \mathrm{\γ}\cos \mathrm{\β}& \cos \mathrm{\γ}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \cos \mathrm{\γ}\sin \mathrm{\α}-\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ \sin \mathrm{\β}& -\cos \mathrm{\β}\sin \mathrm{\α}& \cos \mathrm{\β}\cos \mathrm{\α}\end{array}\right]---\left(1\right)$

现有的惯性导航***已经可以提供较为精确的姿态角信息，其中水平两个姿态角信息即为纵摇角信息和横摇角信息，而航向信息提供的是航向角信息ψ，这有别于艏摇角γ，但舰船的操纵者可以提供准确的主航向信息ψ_G，可得

γ＝ψ-ψ_G                 (2)

将γ代入的计算方程中，即可得到载体坐标系转换到载体半固定坐标系的方向余弦矩阵。

由于惯性测量单元相对载体存在绕方位轴的转位运动，因此惯性测量坐标系(s坐标系)与载体坐标系之间的转换矩阵可以利用下式进行计算：

$C_s^b=\left[\begin{array}{ccc}\cos \mathrm{\ωt}& -\sin \mathrm{\ωt}& 0\\ \sin \mathrm{\ωt}& \cos \mathrm{\ωt}& 0\\ 0& 0& 1\end{array}\right]---\left(3\right)$

式中，ωt表示惯性测量单元相对载体坐标系的相对角度关系。因此可以得到惯性测量单元坐标系转换到载体半固定坐标系的方向余弦矩阵：

$C_s^d=C_b^d{C}_s^b---\left(4\right)$

(5)设计合理的无限冲击响应数字高通滤波器(IIR)，将导航系下解算出的载体速度进行高通滤波处理；

1)确定所设计数字高通滤波器的技术指标

高通数字滤波器f_p1、f_s1、δ_p、δ_s的技术指标是根据信号特征和采样频率f_s给定的。其中，f_p1为通带截止频率，f_s1为阻带截止频率，δ_p为通带波纹，即滤波器通带内偏离单位增益的最大值，通带边缘增益为1-δ_p，δ_s为阻带波纹，即滤波器阻带内偏离单位增益的最大值，阻带边缘处滤波器的增益为δ_s。通带及阻带的衰减α_p、α_s分别定义为-20log(1-δ_p)、-20log(1-δ_s)。

舒勒周期振荡信号相对来说属于低频信号，振荡周期是84.4分钟。而舰船瞬时线运动是由海洋环境因素引起的，最主要的产生原因是海浪的影响，所以舰船瞬时线运动是频率与海浪频率大体一致的往复运动。而且舰船瞬时线运动相对于舰船的航行运动，属于高频运动，运动周期比较短，一般在1.5秒～10秒左右，频率为0.67赫兹。根据升沉横荡纵荡运动和舰船常规工作运动的运动特性上的不同，设计所需要的滤波器的技术指标要求，具体设计指标在试验过程中根据滤波效果进行调整，以达到最优滤波效果为准。

2)将技术指标从数字滤波器转换到模拟滤波器

技术指标从模拟滤波器到数字滤波器的变换采用双线性Z变换法，设计数字高通滤波器的技术指标为f_p1，f_s1，δ_p，δ_s，t_s＝0.0102。首先应得到数字边缘频率Ω，因为2π对应采样频率f_s，而f_s＝1/t_s，所以有：

$\frac{f_{p1}}{f_s}=\frac{{\mathrm{\Ω}}_{p1}}{2\mathrm{\π}}$ (5)

$\frac{f_{s1}}{f_s}=\frac{{\mathrm{\Ω}}_{s1}}{2\mathrm{\π}}$

所以求得：

Ω_s1＝2πf_s1/f_s                                 (6)

Ω_p1＝2πf_p1/f_s

按照双线性Z变换法的频率转换关系ω＝2f_stan(Ω/2)继续转换得到：

${\mathrm{\Ω}}_p=\tan \left(\frac{{\mathrm{\ω}}_p}{2}\right)$ (7)

${\mathrm{\Ω}}_s=\tan \left(\frac{{\mathrm{\ω}}_s}{2}\right)$

以此将数字高通滤波器的技术指标就转换成为模拟高通滤波器的技术指标。

(6)根据调制型捷联惯导***动基座误差方程建立载体系泊状态时的组合姿态误差模型，以高通滤波后得到的速度与调制型捷联惯导***直接解算出的速度作差后作为***观测量。利用卡尔曼滤波技术实现调制型捷联惯导***组合姿态的确定；

建立以经过高通滤波后的水平速度与调制型捷联惯导***直接解算出的速度作差后作为观测量的卡尔曼滤波模型；

1)建立卡尔曼滤波的状态方程：

用一阶线性微分方程来描述调制型捷联惯导***的状态误差：

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW}---\left(8\right)$

其中，X为***的状态向量；A和B分别为***的状态矩阵和噪声矩阵；W为***噪声向量；

***的状态向量为：

***的白噪声向量为：

W＝[a_x a_y ω_x ω_y ω_z 0 0 0 0 0]^T                   (10)

其中δV_e、δV_n分别表示东向、北向的速度误差；分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；ω_x、ω_y、ω_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的白噪声误差；

***的状态转移矩阵为：

$A=\left[\begin{array}{cccc}{F}_{2\×2}^1& f_{2\×3}& {\tilde{T}}_{2\×2}& O_{2\×3}\\ F_{3\×2}^2& F_{3\×3}^3& O_{3\×2}& T_{3\×3}\\ O_{5\×2}& O_{5\×3}& O_{5\×2}& O_{5\×3}\end{array}\right]---\left(11\right)$

$F_{2\×2}^1=\left[\begin{array}{cc}\frac{V_N}{R_n}\tan L^{\′}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_n}\tan L\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{2V_E}{R_n}\tan L)& 0\end{array}\right]---\left(12\right)$

$F_{3\×2}^2=\begin{array}{c}\left[\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{R_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right]---\left(13\right)\end{array}$

$F_{3\×3}^3=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n})& 0& -\frac{V_N}{R_m}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n}& \frac{V_N}{R_m}& 0\end{array}\right]---\left(14\right)$

$f_{2\×3}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& f_E\end{array}\right]---\left(15\right)$

${\tilde{T}}_{2\×2}=\left[\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right]---\left(16\right)$

$T_{3\×6}=\left[\begin{array}{ccc}-T_{11}& -T_{12}& -T_{13}\\ -T_{21}& -T_{22}& -T_{23}\\ -T_{31}& -T_{32}& -T_{33}\end{array}\right]---\left(17\right)$

V_E、V_N分别表示东向、北向的速度；ω_x、ω_y、ω_z分别表示陀螺的三个输入角速度；ω_ie表示地球自转角速度；R_m、R_n分别表示地球子午、卯酉曲率半径；L表示当地纬度；L′表示系泊状态初始时刻载体纬度信息；f_E、f_N、f_U分别表示为导航坐标系下东向、北向、天向的比力。

2)建立卡尔曼滤波的量测方程：

用一阶线性微分方程来描述调制型捷联惯导***的量测方程如下：

Z＝HX+V                      (18)

其中：Z表示***的量测向量H表示***的量测矩阵；V表示***的量测噪声；

***量测矩阵为：

$H=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(19\right)$

量测量为调制型捷联惯导***解算的东向速度V_E、北向速度V_N分别和经过高通滤波处理得到的东向速度北向速度之差：

$Z=\left[\begin{array}{c}{V}_E-{\tilde{V}}_E\\ V_N-{\tilde{V}}_N\end{array}\right]---\left(20\right)$

Claims (1)

1.一种无外观测量的调制型捷联***组合姿态确定方法，其特征在于包括以下步骤： 

(1)通过GPS确定载体的初始位置参数，将它们装订至导航计算机中； 

(2)调制型捷联惯导***进行预热准备，采集光纤陀螺仪和石英加速度计输出的数据并对数据进行处理； 

(3)IMU采用4个转停次序为一个旋转周期的转位方案； 

次序1，IMU由位置A出发，顺时针旋转180°到位置B，并在位置B停留时间T_s；次序2，IMU由位置B出发，顺时针旋转90°到达位置C，并在位置C停留时间T_s；次序3，IMU由位置C出发，逆时针旋转180°到位置D，在位置D停留时间T_s；次序4，IMU由位置D出发逆时针旋转90°回到位置A，并在位置A停留时间T_s；然后按照次序1～4的顺序循环运动； 

IMU每次转动180°或90°间隔进行，从一个位置转动180°到对称位置，在这两个相互对称的位置上，水平方向上惯性敏感元件的常值漂移在进行导航计算的时候能够相互抵消掉；通过旋转90°到达另外一个新位置； 

(4)将加速度计的输出转换到载体半固定坐标系，利用调制型捷联惯导***中的积分环节提取载体瞬时线速度信息； 

1)引入载体半固定坐标系 

以舰船重心为载体半固定坐标系原点，纵轴OY_d指向舰船的主行向方向，横轴OX_d垂直于纵轴平行于水平面，在舰船无纵摇运动时指向右舷方向；垂直轴OZ_d与前两轴垂直，沿船只竖轴向上为正；其中ψ_G为主航向角，γ角为航向摇摆角即艏摇角定义其与主航向角同向为正；载体半固定坐标系的引入使得测量结果和角运动基本脱离，可以准确地描述舰船的瞬时线运动，因此采用载体半固定坐标系作为研究舰船瞬时线运动或称为平动的基础坐标系； 

2)建立惯性测量单元坐标系与载体半固定坐标系的转换矩阵 

首先建立载体坐标系与载体半固定坐标系之间相差三个旋转角，可视为半固定坐标系经三次旋转后与载体坐标系重合，三个角度分别为：纵摇角α、横摇角β及艏摇角γ；载体坐标系b系转换到载体半固定坐标系d系的方向余弦矩阵

现有的惯性导航***已经可以提供较为精确的姿态角信息，其中水平两个姿态角信息即 为纵摇角信息和横摇角信息，而航向信息提供的是航向角信息ψ，这有别于艏摇角γ，但舰船的操纵者可以提供准确的主航向信息ψ_G，可得 

γ＝ψ-ψ_G

将γ代入的计算方程中，即可得到载体坐标系转换到载体半固定坐标系的方向余弦矩阵； 

由于惯性测量单元相对载体存在绕方位轴的转位运动，因此惯性测量坐标系s坐标系与载体坐标系之间的转换矩阵可以利用下式进行计算： 

式中，ωt表示惯性测量单元相对载体坐标系的相对角度关系；因此可以得到惯性测量单元坐标系转换到载体半固定坐标系的方向余弦矩阵： 

(5)设计合理的无限冲击响应数字高通滤波器IIR，将导航系下解算出的载体速度进行高通滤波处理； 

1)确定所设计数字高通滤波器的技术指标 

高通数字滤波器f_p1、f_s1、δ_p、δ_s的技术指标是根据信号特征和采样频率f_s给定的，其中，f_p1为通带截止频率，f_s1为阻带截止频率，δ_p为通带波纹，即滤波器通带内偏离单位增益的最大值，通带边缘增益为1-δ_p，δ_s为阻带波纹，即滤波器阻带内偏离单位增益的最大值，阻带边缘处滤波器的增益为δ_s，通带及阻带的衰减α_p、α_s分别定义为-20log(1-δ_p)、-20log(1-δ_s)； 

舒勒周期振荡信号相对来说属于低频信号，振荡周期是84.4分钟，而舰船瞬时线运动是由海洋环境因素引起的，最主要的产生原因是海浪的影响，所以舰船瞬时线运动是频率与海浪频率大体一致的往复运动，而且舰船瞬时线运动相对于舰船的航行运动，属于高频运动，运动周期比较短，一般在1.5秒～10秒左右，频率为0.67赫兹；根据升沉横荡纵荡运动和舰船常规工作运动的运动特性上的不同，设计所需要的滤波器的技术指标要求，具体设计指标在试验过程中根据滤波效果进行调整，以达到最优滤波效果为准； 

2)将技术指标从数字滤波器转换到模拟滤波器 

技术指标从模拟滤波器到数字滤波器的变换采用双线性Z变换法，设计数字高通滤波器 的技术指标为f_p1，f_s1，δ_p，δ_s，t_s＝0.0102；首先应得到数字边缘频率Ω，因为2π对应采样频率f_s，而f_s＝1/t_s，所以有： 

所以求得： 

Ω_s1＝2πf_s1/f_s

Ω_p1＝2πf_p1/f_s

按照双线性Z变换法的频率转换关系ω＝2f_stan(Ω/2)继续转换有： 

以此将数字高通滤波器的技术指标就转换成为模拟高通滤波器的技术指标； 

(6)根据调制型捷联惯导***动基座误差方程建立载体系泊状态时的组合姿态误差模型，以高通滤波后得到的速度与调制型捷联惯导***直接解算出的速度作差后作为***观测量；利用卡尔曼滤波技术实现调制型捷联惯导***组合姿态的确定； 

建立以经过高通滤波后的水平速度与调制型捷联惯导***直接解算出的速度作差后作为观测量的卡尔曼滤波模型； 

用一阶线性微分方程来描述调制型捷联惯导***的状态误差： 

其中，X为***的状态向量；A和B分别为***的状态矩阵和噪声矩阵；W为***噪声向量； 

***的状态向量为： 

***的白噪声向量为： 

W＝[a_x a_y ω_x ω_y ω_z 0 0 0 0 0]^T

其中δV_E、δV_N分别表示东向、北向的速度误差；分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分 别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；ω_x、ω_y、ω_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的白噪声误差； 

***的状态转移矩阵为： 

V_E、V_N分别表示东向、北向的速度；ω_x、ω_y、ω_z分别表示陀螺的三个输入角速度；ω_ie表 示地球自转角速度；R_m、R_n分别表示地球子午、卯酉曲率半径；L表示当地纬度；L′表示系泊状态初始时刻载体纬度信息；f_E、f_N、f_U分别表示为导航坐标系下东向、北向、天向的比力； 

2)建立卡尔曼滤波的量测方程： 

用一阶线性微分方程来描述调制型捷联惯导***的量测方程如下： 

Z＝HX+V 

其中：Z表示***的量测向量；H表示***的量测矩阵；V表示***的量测噪声； 

***量测矩阵为： 

量测量为调制型捷联惯导***解算的东向速度V_E、北向速度V_N分别和经过高通滤波处理得到的东向速度北向速度之差

。