CN103335650A - 一种基于惯性测量单元的坐标失配测量方法 - Google Patents

Abstract

本发明公开了一种基于惯性测量单元的坐标失配测量方法，首先选择测量设备；对所选设备进行安装，主子惯性测量单元安装在船体不同部位，并开启设备进行预热；将测量单元通过串口线与数据采集系统相连，检查设备及航行环境，启动设备并进行初始对准，舰船出航，测量单元进行数据采集和保存；对所采集数据进行处理，采用速度加姿态匹配算法，利用卡尔曼滤波估计出坐标失配角。本发明适用于动态环境下的坐标失配角实时测量。本方法能很好的适应动态环境，在船舶实际航行条件下对坐标失配实时检测并进行修正，以惯性测量单元解算的速度信息和姿态信息作为卡尔曼滤波的观测量，对于提高舰船变形角的收敛速度和估计精度具有明显的效果。

Description

一种基于惯性测量单元的坐标失配测量方法

技术领域

本发明涉及一种测量方法，具体涉及一种基于惯性测量单元的坐标失配测量方法。

背景技术

现代大型舰船在火炮、导弹、雷达和舰载飞机等设备协调工作期间，需要中心航姿系统提供高精度导航信息进行初始对准和初始参数装订。由于舰船不是一个绝对的刚体，在日晒夜露和时间老化作用下结构形状会逐渐发生变化，产生长期变形；而在恶劣海情下波浪运动对船体加载会使船体产生动态变形，且任意两点间角位移的幅值随着其间距离的加大而增大；当船体机动或减摇装置作用或船艏升离水面重又入水时也可能产生瞬时船体挠曲变形。因此，多种因素都可引起船体变形，静态变形可达1度，动态变形在角分级，这些变形角将导致中心航姿系统与各用户部分坐标失配。如果用户设备不对失配角进行修正，直接使用中心航姿系统的参数进行初始对准和初始参数装订就会引起误差。

惯性测量单元(陀螺仪和加速度计)可以测量所在位置的角速率和加速度，利用计算机可以解算出相应的导航参数。由于船体不同位置坐标失配，不同位置的导航参数会有所差异，这个差异可以反应两位置的坐标失配关系，因此在船体不同部位安装惯性测量单元，航行时通过惯性测量单元输出数据进行导航参数解算，建立导航参数与坐标失配角的关系，通过滤波就可以实时估计出坐标失配角，从而对其进行补偿以减小或消除坐标失配带来的影响。

发明内容

本发明是为了实现舰船由于变形和固定安装偏差引起的坐标失配测量，从而提供一种基于惯性测量单元的坐标失配测量方法。

其特征是它由以下步骤实现：

步骤一、选取两套光纤捷联惯性导航系统作为主子惯性测量单元，将法国IXSEA公司生产的高精度捷联惯导系统PHINS与GPS组合作为监测系统，选择422串口线、加固计算机构成数据采集系统进行数据的实时采集、保存和处理，采用高可靠性的电源，在此基础上加装3套UPS以确保在进行试验的过程中供电不间断。

步骤二、安装设备。主惯导系统光纤陀螺捷联惯导系统和PHINS固定于同一铁板上，安装在船体中心，子惯导光纤捷联惯导系统安装在甲板上，设备均通过槽钢架高；两套数据采集系统均放置在惯性测量单元正上方的架子上，电源、UPS依次固定于惯性测量单元旁，GPS安装在二层观测仓。

开启测量系统中的惯性测量单元进行预热，预热时间不少于1小时。建立主子惯导载体坐标系，定义东北天地理坐标系为导航系，记为n。

步骤三、信息采集。PHINS/GPS与主惯导系统光纤陀螺捷联惯导系统(数据线长度约为50米)通过422串口线连接到第一数据采集系统。子惯导系统光纤陀螺捷联惯导系统(数据线长度约为50米)通过422串口线连接到第二数据采集系统。数据采集频率为100Hz。

检查设备及航行环境，舰船系泊状态下启动PHINS与光纤惯性导航系统，光纤惯性导航设备启动1小时之后初始对准结束。惯性测量单元进入工作状态后舰船出航，完成包含转弯、变加速、回旋运动的航行。使用数据采集装置实时、准确、完整地同步采集PHINS和两套光纤惯性测量单元的速度和姿态信息，并进行保存。

步骤四、信息实时解算。对采集到的数据用计算机进行实时解算，采用速度加姿态匹配方法，利用卡尔曼滤波估计坐标失配角。

系统从t_k-1时刻转移到t_k时刻的动态模型为

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k,k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

其中X_k是系统的状态向量，Z_k是系统的观测向量，W_k是系统过程噪声序列，V_k是观测噪声序列，Φ_k，k-1是系统的状态转移矩阵，Γ_k，k-1是噪声输入矩阵，H_k是观测矩阵。系统噪声W_k和观测噪声V_k均是零均值高斯白噪声，且相互独立不相关。

一步状态预测

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

其中为状态X在k时刻的预测，为k-1时刻状态X的估计值。

一步量测预测

${\hat{Z}}_{k,k-1}=H_k{\hat{X}}_{k,k-1}$

式中

为观测Z在k时刻的预测。

一步预测误差协方差

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$

式中P_k，k-1为k-1时刻到k时刻的一步预测误差协方差阵，P_k-1为k-1时刻的估计误差协方差阵；

为Φ_k，k-1的转置；Q_k-1是系统过程噪声W_k-1的对称非负定方差矩阵；

为Γ_k，k-1的转置。

根据卡尔曼滤波原理求取k时刻的滤波增益K_k

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$

其中R_k为k时刻系统观测噪声V_k的对称正定方差阵，为H_k的转置。

利用量测量对预测状态进行修正，得到k时刻的状态估计

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]$

计算估计误差阵判断估计效果。k时刻的估计误差协方差P_k为：

$P_k=[I-K_k{H}_k]P_{k,k-1}{[I-K_k{H}_k]}^T+K_k{R}_k{K}_k^T$

重复此递推过程，直到状态估计达到一个稳定值，即得到船体坐标失配角的估计，完成坐标失配角的测量。

步骤五、对上述测量出的坐标失配角进行补偿。子惯导在进行导航实时解算时，各姿态角分别减掉坐标失配角得到新的真实姿态角，以此姿态角进行惯导解算，消除或减小坐标失配对子惯导的影响。

本发明提出了一种基于惯性测量单元的坐标失配测量方法，能快速准确地测量出坐标失配角，进而对其进行补偿以提高导航精度。本发明基于惯性测量单元的坐标失配测量方法采用速度加姿态匹配滤波算法，具有速度快、精度高、容易实现等显著优点。

附图说明

图1是本发明提供的坐标失配测量系统构成；

图2是本发明提供的测量数据采集系统。

具体实施方式

一种基于惯性测量单元的坐标失配测量方法，它由以下步骤实现：

步骤一、选取设备。

选取两套光纤捷联惯性导航系统作为主子惯性测量单元，其中陀螺漂移不高于0.01°/h，加速度计零偏不高于1×10^-4g。将法国IXSEA公司生产的高精度捷联惯导系统PHINS与GPS组合作为监测系统，以便及时发现主子测量系统的故障，提高系统可靠性。选择422串口线、加固计算机构成数据采集系统进行数据的实时采集、保存和处理，保证数据传输速率，并能在海上航行动态环境下有效工作。采用高可靠性的电源，在此基础上加装3套UPS以确保在进行试验的过程中供电不间断。

步骤二、安装设备。

考虑到潮湿、走线等因素，将惯性导航设备、电源和UPS架高。主惯导系统光纤陀螺捷联惯导系统作为主惯导系统和PHINS固定于同一铁板上，安装在船体中心，子惯导系统光纤陀螺捷联惯导系统作为子惯导系统安装在甲板上，设备均通过槽钢架高，惯性测量单元的安装应尽量满足陀螺仪和加速度计的三轴与船体坐标系重合；两套数据采集系统均放置在惯性测量单元正上方的架子上，电源、UPS依次固定于惯性测量单元旁，GPS安装在二层观测仓，为减少设备工作环境中的湿度，在架子(子惯导系统光纤陀螺捷联惯导系统正上方)下方安装了风扇。

开启测量系统中的惯性测量单元进行预热，预热时间不少于1小时。建立坐标系，定义东北天地理坐标系为导航系，记为n。定义y轴指向船艏，z轴垂直于船体甲板平面并向上，x轴指向船体的右舷，x，y，z轴构成右手直角坐标系，即主惯导载体坐标系，用m表示。子惯导坐标系定义与主惯导类似，用s表示。两套惯性测量单元构成坐标m与s失配角的测量系统。

步骤三、信息采集

PHINS/GPS与主惯导系统光纤陀螺捷联惯导系统(数据线长度约为50米)通过422串口线连接到第一数据采集系统。光纤陀螺捷联惯导系统2(数据线长度约为50米)通过422串口线连接到第二数据采集系统。数据采集系统负责实时、准确、完整地采集和保存PHINS/GPS和光纤陀螺捷联惯导系统的输出数据，数据采集频率为100Hz。

确认设备完整、状态良好，惯性导航设备与数据采集装置接口正确、工作正常；码头、航区、气象、通讯及勤务保障满足航行要求；航行海区水域宽阔，测试时海况不大于三级。

舰船系泊状态下启动PHINS与光纤惯性导航系统，其中PINS工作在与GPS组合状态。20分钟后软件重启PHINS，光纤惯性导航设备启动1小时之后初始对准结束。PHINS进入组合导航工作状态，光纤惯性测量单元进入测量工作状态。舰船出航，完成涵盖转弯、变加速运动、回旋等各种运动方式的航行。从导航设备完成启动转入自主导航方式开始，8小时为1航次，期间不得进行校准，共进行1航次。

使用数据采集装置实时、准确、完整地同步采集PHINS和两套光纤惯性测量单元的速度和姿态信息，并进行保存，采样频率为1次/秒。

步骤四、信息实时解算。

对采集到的光纤惯性测量单元输出数据用计算机进行实时解算，采用速度加姿态匹配方法，利用卡尔曼滤波估计坐标失配角。

系统从t_k-1时刻转移到t_k时刻的动态模型为

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k,k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

其中状态量

$X={\left[\begin{array}{ccccccccccccccccccccc}{\mathrm{\δv}}_x& {\mathrm{\δv}}_y& {\mathrm{\δv}}_z& {\mathrm{\φ}}_{\mathrm{mx}}& {\mathrm{\φ}}_{\mathrm{my}}& {\mathrm{\φ}}_{\mathrm{mz}}& {\▿}_x& {\▿}_y& {\▿}_z& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\mathrm{\φ}}_{\mathrm{ax}}& {\mathrm{\φ}}_{\mathrm{ay}}& {\mathrm{\φ}}_{\mathrm{az}}& {\mathrm{\θ}}_x& {\mathrm{\θ}}_y& {\mathrm{\θ}}_z& {\mathrm{\μ}}_x& {\mathrm{\μ}}_y& {\mathrm{\μ}}_z\end{array}\right]}^T,$ δv_x、δv_y、δv_z分别为主惯导系统光纤陀螺捷联惯导系统与子惯导系统光纤陀螺捷联惯导系统之间东向、北向、天向速度差；φ_mx、φ_my、φ_mz分别为主惯导系统光纤陀螺捷联惯导系统与子惯导系统光纤陀螺捷联惯导系统之间东向、北向、天向姿态误差角，即主子惯导载体坐标系失配角；

分别为东向、北向、天向加速度计零偏；ε_x、ε_y、ε_z分别为东向、北向、天向陀螺漂移；φ_ax、φ_ay、φ_az分别为主惯导系统光纤陀螺捷联惯导系统与子惯导系统光纤陀螺捷联惯导系统之间东向、北向、天向安装误差角；θ_x、θ_y、θ_z分别为东向、北向、天向的动态变形角，μ_x、μ_y、μ_z分别为东向、北向、天向动态变形角的微分。X_k，X_k-1分别为k，k-1时刻的状态量，

Γ_k，k-1是k-1时刻到k时刻的噪声输入阵，Γ＝I_21×21；

W_k是k时刻的系统过程噪声序列，W＝[0_9×1 w_gx w_gy w_gz 0_6×1 w_rx w_ry w_rz]^T，w_g，w_r分别为陀螺仪噪声和挠曲变形噪声。

Φ_k，k-1为k-1时刻到k时刻的状态转移矩阵，且Φ_k，k-1＝I-F(t_k)·Δt，I为单位阵，Δt为采样时间间隔。

式中

分别为地球自转角速率在导航系x，y，z轴的投影；

分别为导航系相对地球系的旋转角速率在导航系x，y，z轴的投影；f_x，f_y，f_z分别为主惯导比力输出在导航系x，y，z轴的投影；

分别为主惯导载体系相对于导航系的角速率在导航系x，y，z轴的投影；0_12×1为12×1维的0矩阵；β_x，β_y，β_z分别为子惯导载体系相对于主惯导载体系x，y，z轴的变形系数。

Z_k为k时刻的系统观测量，以两套光纤惯导系统输出的速度差和姿态差作为观测量，即

$Z=\left[\begin{array}{c}\mathrm{\δv}\\ {\mathrm{\φ}}_m\end{array}\right]$

其中δv＝v₁-v₂，v₁为主惯导系统光纤陀螺捷联惯导系统输出的速度，v₂为子惯导系统光纤陀螺捷联惯导系统输出的速度；φ_m＝φ₁-φ₂，φ₁为主惯导系统光纤陀螺捷联惯导系统输出的姿态角，φ₂为子惯导系统光纤陀螺捷联惯导系统输出的姿态角。H_k为k时刻的系统观测矩阵，且

H＝[I_6×6 0_6×6 C_6×6 0_3×3]

$C_{6\×6}\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ C_m^n\left(11\right)& C_m^n\left(12\right)& C_m^n\left(13\right)& C_m^n\left(11\right)& C_m^n\left(12\right)& C_m^n\left(13\right)\\ C_m^n\left(21\right)& C_m^n\left(22\right)& C_m^n\left(23\right)& C_m^n\left(21\right)& C_m^n\left(22\right)& C_m^n\left(23\right)\\ C_m^n\left(31\right)& C_m^n\left(32\right)& C_m^n\left(33\right)& C_m^n\left(31\right)& C_m^n\left(32\right)& C_m^n\left(33\right)\end{array}\right],$ I_6×6为6×6维单位阵，

为主惯导系统姿态矩阵。V_k为k时刻的观测噪声阵，V＝[w_vx w_vy w_vz w_dcmx w_dcmy w_dcmz]^T，w_v，w_dcm分别为速度观测噪声和姿态观测噪声。

一步状态预测

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

其中

为状态X在k时刻的预测，

为k-1时刻状态X的估计值。

一步量测预测

${\hat{Z}}_{k,k-1}=H_k{\hat{X}}_{k,k-1}$

式中

为观测Z在k时刻的预测。

一步预测误差协方差

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$

式中P_k，k-1为k-1时刻到k时刻的一步预测误差协方差阵，P_k-1为k-1时刻的估计误差协方差阵；

为Φ_k，k-1的转置；Q_k-1是系统过程噪声W_k-1的对称非负定方差矩阵；

为Γ_k，k-1的转置。

根据卡尔曼滤波原理求取k时刻的滤波增益K_k

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$

其中R_k为k时刻系统观测噪声V_k的对称正定方差阵，

为H_k的转置。

利用量测量对状态估计进行修正，得到k时刻的状态估计

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]$

计算估计误差阵判断估计效果。k时刻的估计误差协方差P_k为：

$P_k=[I-K_k{H}_k]P_{k,k-1}{[I-K_k{H}_k]}^T+K_k{R}_k{K}_k^T$

重复此递推过程，直到状态估计达到一个稳定值，即得到船体坐标失配角的估计，完成坐标失配角的测量。

步骤五、误差补偿。

对上述测量出的坐标失配角进行补偿。子惯导在进行导航实时解算时，各姿态角分别减掉坐标失配角得到新的真实姿态角，以此姿态角进行惯导解算，消除或减小坐标失配对子惯导的影响。

本发明提供的基于惯性测量单元的坐标失配测量方法具有以下优点：

本方案采用惯性测量匹配法进行坐标失配测量，一是克服了常用结构力学方法实时性差的缺点，能很好的适应动态环境，在船舶实际航行条件下对坐标失配实时检测并进行修正；二是相对于其它各种光学测量船体变形的方案本发明不需要安装光学设备，操作简单，便于实施；三是以惯性测量单元解算的速度信息和姿态信息作为卡尔曼滤波的观测量，而不是直接以角速度差作为观测量，对于提高舰船变形角的收敛速度和估计精度具有明显的效果。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于惯性测量单元的坐标失配测量方法，其特征在于，该坐标失配测量方法包括：

步骤一、选取两套光纤捷联惯性导航系统作为主子惯性测量单元，将PHINS与GPS组合作为监测系统，选择数据采集系统进行数据的实时采集、保存和处理；

步骤二、安装设备，建立主子惯导载体坐标系，定义东北天地理坐标系为导航系，记为n；

步骤三、使用数据采集装置同步采集PHINS和两套光纤惯性测量单元的速度和姿态信息，并进行保存；

步骤四、对采集到的数据用计算机进行实时解算，采用速度加姿态匹配方法，利用卡尔曼滤波估计坐标失配角；

步骤五、对上述测量出的坐标失配角进行补偿。

2.根据权利要求1所述的基于惯性测量单元的坐标失配测量方法，其特征在于，步骤一、选取两套光纤捷联惯性导航系统作为主子惯性测量单元，将捷联惯导系统PHINS与GPS组合作为监测系统，选择422串口线、加固计算机构成数据采集系统进行数据的实时采集、保存和处理，采用电源，在此基础上加装3套UPS以确保在进行试验的过程中供电不间断。

3.根据权利要求1所述的基于惯性测量单元的坐标失配测量方法，其特征在于，步骤二、安装设备；主惯导系统光纤陀螺捷联惯导系统和PHINS固定于同一铁板上，安装在船体中心，子惯导系统光纤陀螺捷联惯导系统安装在甲板上，设备均通过槽钢架高；两套数据采集系统均放置在惯性测量单元正上方的架子上，电源、UPS依次固定于惯性测量单元旁，GPS安装在二层观测仓；

开启测量系统中的惯性测量单元进行预热；建立主子惯导载体坐标系，定义东北天地理坐标系为导航系，记为n。

4.根据权利要求1所述的基于惯性测量单元的坐标失配测量方法，其特征在于，步骤三、信息采集；PHINS/GPS与主惯导系统光纤陀螺捷联惯导系统通过422串口线连接到第一数据采集系统；子惯导系统光纤陀螺捷联惯导系统通过422串口线连接到第二数据采集系统；数据采集频率为100Hz；

检查设备及航行环境，舰船系泊状态下启动PHINS与光纤惯性导航系统，光纤惯性导航设备启动1小时之后初始对准结束；惯性测量单元进入工作状态后舰船出航，完成包含转弯、变加速、回旋运动的航行；使用数据采集装置同步采集PHINS和两套光纤惯性测量单元的速度和姿态信息，并进行保存。

5.根据权利要求1所述的基于惯性测量单元的坐标失配测量方法，其特征在于，步骤四、信息实时解算：对采集到的数据用计算机进行实时解算，采用速度加姿态匹配方法，利用卡尔曼滤波估计坐标失配角；

系统从t_k-1时刻转移到t_k时刻的动态模型为

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k,k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

其中X_k是系统的状态向量，Z_k是系统的观测向量，W_k是系统过程噪声序列，V_k是观测噪声序列，Φ_k，k-1是系统的状态转移矩阵，Γ_k，k-1是噪声输入矩阵，H_k是观测矩阵。系统噪声W_k和观测噪声V_k均是零均值高斯白噪声，且相互独立不相关；

一步状态预测

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

其中

为状态X在k时刻的预测，

为k-1时刻状态X的估计值。

一步量测预测

${\hat{Z}}_{k,k-1}=H_k{\hat{X}}_{k,k-1}$

式中

为观测Z在k时刻的预测。

一步预测误差协方差

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$

式中P_k，k-1为k-1时刻到k时刻的一步预测误差协方差阵，P_k-1为k-1时刻的估计误差协方差阵；

为Φ_k，k-1的转置；Q_k-1是系统过程噪声W_k-1的对称非负定方差矩阵；

为Γ_k，k-1的转置；

根据卡尔曼滤波原理求取k时刻的滤波增益K_k

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$

其中R_k为k时刻系统观测噪声V_k的对称正定方差阵，为H_k的转置；

利用量测量对预测状态进行修正，得到k时刻的状态估计

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]$

计算估计误差阵判断估计效果；k时刻的估计误差协方差P_k为：

$P_k=[I-K_k{H}_k]P_{k,k-1}{[I-K_k{H}_k]}^T+K_k{R}_k{K}_k^T$

重复此递推过程，直到状态估计达到一个稳定值，即得到船体坐标失配角的估计，完成坐标失配角的测量。

6.根据权利要求1所述的基于惯性测量单元的坐标失配测量方法，其特征在于，步骤五、对上述测量出的坐标失配角进行补偿；子惯导在进行导航实时解算时，解算出的各姿态角分别减掉相应的坐标失配角得到新的真实姿态角，以此姿态角进行后续惯导解算，消除或减小坐标失配对子惯导的影响。

7.根据权利要求1所述的基于惯性测量单元的坐标失配测量方法，其特征在于，该坐标失配测量方法用到的测量系统的组成为：

PHINS与GPS组合作为监测系统，两套光纤捷联惯性导航系统作为主子惯性测量单元，422串口线、加固计算机构成数据采集系统进行数据的实时采集、保存和处理，高可靠性电源外加3套UPS构成供电系统。

Images