一种基于惯性测量单元的大角度船体变形测量方法

摘要

本发明提出基于惯性测量单元的大角度船体变形测量方法，同时采用CKF滤波方法，并能保证相对较高的精度。本发明包括：将两套惯性测量单元IMU1和IMU2分别安装在船体的中央位置和船首位置；建立变形角的非线性模型；推导两载体坐标系间的变换关系；确定IMU的误差模型；建立非线性状态方程；建立系统的观测方程；采用CKF算法进行时间更新与量测更新，对船体变形进行估计和监控。本发明建立了基于惯性测量匹配法的非线性模型，采用CKF滤波，其适用于非线性船体变形建模与测量，滤波精度要好于UKF。分析变形角参数设置不准确对估计的影响，并提出基于舰船姿态频谱分析的主频参数优化方法。

说明书

技术领域

本发明提出基于惯性测量单元的大角度船体变形测量方法，同时采用CKF滤波方法，并 能保证相对较高的精度。

背景技术

现代舰船的多个部位装有相应攻防设备和导航系统等，一般情况下，这些设备的姿态、 位置及运动参数等信息都是由舰船中心的航姿设备来提供的。由于舰船材料刚度并不是绝对 的，随着海浪拍打、舰船上负载的转移各种外力的综合作用，结构必然会逐渐产生长期变形， 船体的某些部位最大会达到1～1.5度的形变量级。安装在舰船上的中心航姿设备也就不可避 免地与确定局部基准的子设备产生基准失调，进而降低武器的命中率，降低作战效率。因此 为保证各个子系统间协调工作时的精度，精确测量出船体变形是非常有必要的。此外,测量船 体变形对于优化船体结构设计、合理布局舰船负载，以及进行舰船材料选取、不同部位的刚 度设计以及提高舰船传递对准精度都有很现实的意义。

随着现代精密仪器仪表技术的蓬勃发展，高精度的新型传感器不断出现，大量测量船体 形变的新方法也应运而生。一般可被分为两大类：(1)第一类为结构力学法，包括偏振光能量 测量法、大钢管基准法、光栅法、液体压力测量法等。由于这类方法的实时性以及实施条件 等受到很大限制。此外，往往还需要因动态变形角引起的线速度变化，此方法无法实时给出。 (2)第二类为姿态匹配测量法，包括惯性测量匹配法、GPS测量法以及多部位安装航姿系统的 办法可以实时给出并修正这些姿态参数。GPS测量法的测量精度目前而言是比较高的，但因 为严重依赖GPS系统，所以这种测量方法的隐蔽性较差，隐蔽性好对于现代战争来说是极其 重要的。多位置安装航姿系统的费用非常高，不符合经济性的原则。惯性测量匹配法利用分 布式惯性测量单元进行变形测量，不仅能够给出航向、姿态信息，还能实时给出角速率信息。 与外界不存在能量交换，能根据预先设定的参数独立自主运行，隐蔽性非常好。此外，近些 年来，光纤陀螺体积小、重量轻、精度高、动态测量范围广。这些独特的优势使得基于光学 陀螺测量单元的惯性匹配法成为最具发展潜力的研究方向。

在实际中，由于船体较大，中心惯性测量单元与局部惯性测量单元之间存在安装误差角。 针对惯性测量单元存在较大安装误差角时，系统模型是非线性的。本发明提出了一种基于惯 性测量单元的大角度船体变形测量方法。

发明内容

本发明提出基于惯性测量单元(IMU)的大角度船体变形测量方法，并能保证相对较高的 精度。

本发明的目的是这样实现的：

基于惯性测量单元的大角度船体变形测量方法，包括如下步骤：

(1)将两套惯性测量单元IMU1和IMU2分别安装在船体的中央位置和船首位置，两套IMU 的三个轴向分别命名为ox_my_mz_m和ox_sy_sz_s，其中oy_m、oy_s轴指向船体的航向，oz_m、oz_s轴 垂直于甲班平面指天，ox_m、ox_s轴与其它两个轴构成右手正交坐标系；

(2)建立变形角的非线性模型

(3)推导两载体坐标系间的变换关系

ox_sy_sz_s首先绕x_s轴旋转η_x角度得到中间坐标系ox_s1y_s1z_s1，然后再绕y_s1轴旋转η_y角度 得到中间坐标系ox_s2y_s2z_s2，最后绕z_s2轴旋转η_z角度得到中心IMU的平台坐标系ox_my_mz_m：

$C=\left(\begin{array}{ccc}{\mathrm{cos\eta }}_z& {\mathrm{sin\eta }}_z& 0\\ -{\mathrm{sin\mu }}_z& {\mathrm{cos\mu }}_z& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}{\mathrm{cos\eta }}_y& 0& -{\mathrm{sin\eta }}_y\\ 0& 1& 0\\ {\mathrm{sin\eta }}_y& 0& {\mathrm{cos\eta }}_y\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\eta }}_x& {\mathrm{sin\eta }}_x\\ 0& -{\mathrm{sin\eta }}_x& {\mathrm{cos\eta }}_x\end{array}\right);$

(4)确定IMU的误差模型

$\left(\begin{array}{c}{\stackrel{·}{\stackrel{‾}{ϵ}}}_i=0\\ {\stackrel{·}{ϵ}}_i+{\mathrm{\mu ϵ}}_i={\sigma }_i\sqrt{2\mu }w\left(t\right)\end{array}\right);$

(5)建立非线性状态方程

选取状态向量其中i＝x,y,z；

非线性状态方程：

$\left(\begin{array}{c}{\stackrel{·}{x}}_1=0\\ {\stackrel{·}{x}}_2=x_3\\ {\stackrel{·}{x}}_3=-2{\mu }_i{x}_3-b_i^2{x}_2+2b_i\sqrt{D_i{\mu }_i}w\left(t\right)\\ {\stackrel{·}{x}}_4=0\\ {\stackrel{·}{x}}_5=0\\ {\stackrel{·}{x}}_6=-{\mu }_{1i}^{\prime }{x}_6+{\sigma }_i\sqrt{2{\mu }_{1i}^{\prime }}w\left(t\right)\\ {\stackrel{·}{x}}_7=-{\mu }_{1i}^{\prime }{x}_7+{\sigma }_i\sqrt{2{\mu }_{2i}^{\prime }}w\left(t\right)\end{array}\right);$

非线性系统的状态方程可以化简为：

$\stackrel{·}{x}=f\left(x\right)+w;$

(6)建立系统的观测方程

观测方程，即两套光纤陀螺测量单元的输出角速率之差为：

$\Delta \omega ={\omega }_1-{\omega }_2={\omega }_1-({\mathrm{C\omega }}_1+\stackrel{·}{\eta })={\omega }_1-{\mathrm{C\omega }}_1-\stackrel{·}{\eta }=(I-C){\omega }_1-\stackrel{·}{\eta }$

(7)采用CKF算法进行时间更新与量测更新，对船体变形进行估计和监控； 时间更新：

$p_{k-1/k-1}=S_{k-1/k-1}{S}_{k-1/k-1}^T$

$X_{i,k-1/k-1}=S_{k-1/k-1}{\xi }_i+{\hat{x}}_{k-1/k-1},(i=1,2,...,m)$

$X_{i,k/k-1}^{*}=f\left(X_{i,k-1/k-1}\right)$

其中[1]_i表示集合[1]的第i列，对于二维状态，即n＝2，有：

$\left[1\right]=\left[\left(\begin{array}{c}1\\ 0\end{array}\right)\left(\begin{array}{c}0\\ 1\end{array}\right)\left(\begin{array}{c}-1\\ 0\end{array}\right)\left(\begin{array}{c}0\\ -1\end{array}\right)\right]$

${\hat{x}}_{k/k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Sigma }}{X}_{i,k/k-1}^{*}$

$p_{k/k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Sigma }}{X}_{i,k/k-1}^{*}{X}_{i,k/k-1}^{*T}-{\hat{x}}_{k/k-1}{\hat{x}}_{k/k-1}^T+Q_{k-1}$

量测更新：

$p_{k/k-1}=S_{k/k-1}{S}_{k/k-1}^T$

$X_{i,k/k-1}=S_{k/k-1}{\xi }_i+{\hat{x}}_{k/k-1},(i=1,2,...,m)$

Z_i,k/k-1＝h(X_i,k/k-1)

${\hat{z}}_{k/k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Sigma }}{Z}_{i,k/k-1}$

$p_{zz,k/k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Sigma }}{Z}_{i,k/k-1}{Z}_{i,k/k-1}^T-{\hat{z}}_{k/k-1}{\hat{z}}_{k/k-1}^T+R_k$

$p_{xz,k/k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Sigma }}{X}_{i,k/k-1}{Z}_{i,k/k-1}^T-{\hat{z}}_{k/k-1}{\hat{z}}_{k/k-1}^T$

$W_k=p_{xz,k/k-1}{p}_{zz,k/k-1}^{-1}$

${\hat{x}}_{k/k}={\hat{x}}_{k/k-1}+W_k(z_k-{\hat{z}}_{k/k-1})$

$p_{k/k}=p_{k/k-1}-W_k{p}_{zz,k/k-1}{W}_k^T$

(8)通过对船体姿态角度进行频谱分析，对动态变形角模型参数进行优化。

本发明的有益效果在于：

(1)针对非线性模型，分别进行了基于UKF和CKF的变形测量仿真与分析，并对仿真结果 进行了对比。总变形角UKF、CKF估计误差曲线如图5所示。本发明建立了基于惯性测量匹配 法的非线性模型，采用CKF滤波，其适用于非线性船体变形建模与测量，滤波精度要好于UKF。

(2)同时针对同一模型，分别用CKF和UKF进行了50次蒙特卡洛仿真，然后对收敛时间 和估计精度进行统计。UKF、CKF各50次滤波收敛时间和估计精度统计图分别如图6和图7 所示。分析变形角参数设置不准确对估计的影响，并提出基于舰船姿态频谱分析的主频参数 优化方法。

附图说明

图1两套IMU安装位置及坐标示意图；

图2不同主频下的变形角估计误差曲线；

图3不同μ值下的变形角估计误差；

图4动态变形角主频优化后的变形角估计流程图；

图5总变形角UKF、CKF估计误差曲线；

图6UKF、CKF各50次滤波收敛时间情况统计；

图7UKF、CKF各50次滤波估计精度统计。

具体实施方式

下面结合附图对本发明做进一步描述：

本发明是一种基于惯性测量单元的大角度船体变形测量角度。该方法通过在船上测量点 分别放置三轴IMU，根据两套IMU测得的角速率之差和相对变形角间的关系，建立状态空间 模型，对其进行实时估计和监测。该方法的特点在于：①对于IMU存在大安装误差角以及光 纤陀螺的随机漂移，建立了非线性的系统模型；②经过理论比较分析与仿真验证，结果表明 随着系统非线性程度的增强，CKF估计有良好的估计效果，并且对于变形角度估计的高维非 线性系统来说，CKF精度更高；③使用频谱分析舰船姿态对动态变形角模型中相应参数进行 修正，提高了估计精度。

本发明通过以下步骤实现：

(1)测量设备的安装

将两套惯性测量单元(IMU1和IMU2)按照图1所示分别安装在船体的中央位置和船首位 置。两套IMU的三个轴向分别命名为ox_my_mz_m和ox_sy_sz_s，其中oy_m、oy_s轴指向船体的航向， oz_m、oz_s轴垂直于甲班平面指天，ox_m、ox_s轴与其它两个轴构成右手正交坐标系。

(2)建立变形角的非线性模型

①静态形变形角视为一个不随时间变化的常数，因此有：

其中，i分别表示x、y、z轴，所以：

两套IMU之间的安装误差角为一固定常值，模型可由下式表示：

φ_i＝m(3)

其中，i分别表示x、y、z轴，m为某一定值。

②船体动态变形角至少一个二阶马尔可夫过程。动态变形角自相关函数为：

$K_{{\theta }_i}\left(\tau \right)=D_i{e}^{-{\mu }_i\left|\tau \right|}({\mathrm{cos\lambda }}_i\tau +\frac{{\mu }_i}{{\lambda }_i}{\mathrm{sin\lambda }}_i|\tau \left|\right)---\left(4\right)$

与此对应的变形滤波器可以写成：

${\stackrel{··}{\theta }}_i+2{\mu }_i{\stackrel{·}{\theta }}_i+b_i^2{\theta }_i=2b_i\sqrt{D_i{\mu }_i}w\left(t\right)---\left(5\right)$

其中i＝x,y,z，μ_i为不规则系数，λ_i为动态变形角变化的主频率，D_i为 常值系数。

变形角的非线性模型

(3)推导两测量单元的载体坐标系间的变换关系

ox_sy_sz_s首先绕x_s轴旋转η_x角度得到中间坐标系ox_s1y_s1z_s1，然后再绕y_s1轴旋转η_y角度 得到中间坐标系ox_s2y_s2z_s2，最后绕z_s2轴旋转η_z角度得到中心IMU的平台坐标系ox_my_mz_m， 由此可得：

$C=\left(\begin{array}{ccc}{\mathrm{cos\eta }}_z& {\mathrm{sin\eta }}_z& 0\\ -{\mathrm{sin\mu }}_z& {\mathrm{cos\mu }}_z& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}{\mathrm{cos\eta }}_y& 0& -{\mathrm{sin\eta }}_y\\ 0& 1& 0\\ {\mathrm{sin\eta }}_y& 0& {\mathrm{cos\eta }}_y\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\eta }}_x& {\mathrm{sin\eta }}_x\\ 0& -{\mathrm{sin\eta }}_x& {\mathrm{cos\eta }}_x\end{array}\right)---\left(7\right)$

(4)建立IMU的误差模型

①常值漂移可视为受白噪声驱动的一阶马尔可夫过程：

${\stackrel{·}{\stackrel{‾}{ϵ}}}_i=-\frac{1}{T}{\overline{ϵ}}_i+{\omega }_{ϵ}---\left(8\right)$

式中，ω_ε为一阶马尔可夫过程白噪声。因为漂移相关时间T比较长，所以IMU漂移可视 为常数。因此：

${\stackrel{·}{\stackrel{‾}{ϵ}}}_i=0---\left(9\right)$

②IMU随机漂移的自相关函数为：

$K_{{ϵ}_i}\left(\tau \right)={\sigma }_i^2{e}^{-{\mu }_i\left|\tau \right|}---\left(10\right)$

其中i＝x,y,z，τ为相关时间，μ_i为不规则系数，σ_i表征IMU漂移离散程度的均方差。 IMU随机漂移的自相关函数对应的滤波器模型为：

${\stackrel{·}{ϵ}}_i+{\mathrm{\mu ϵ}}_i={\sigma }_i\sqrt{2\mu }w\left(t\right)---\left(11\right)$

③IMU的误差模型

$\left(\begin{array}{c}{\stackrel{·}{\stackrel{‾}{ϵ}}}_i=0\\ {\stackrel{·}{ϵ}}_i+{\mathrm{\mu ϵ}}_i={\sigma }_i\sqrt{2\mu }w\left(t\right)\end{array}\right)---\left(12\right)$

(5)建立系统的非线性状态方程

根据静态变形角、动态变形角以及惯性传感器测量单元漂移模型：

选取状态向量其中i＝x,y,z。

非线性状态方程：

$\left(\begin{array}{c}{\stackrel{·}{x}}_1=0\\ {\stackrel{·}{x}}_2=x_3\\ {\stackrel{·}{x}}_3=-2{\mu }_i{x}_3-b_i^2{x}_2+2b_i\sqrt{D_i{\mu }_i}w\left(t\right)\\ {\stackrel{·}{x}}_4=0\\ {\stackrel{·}{x}}_5=0\\ {\stackrel{·}{x}}_6=-{\mu }_{1i}^{\prime }{x}_6+{\sigma }_i\sqrt{2{\mu }_{1i}^{\prime }}w\left(t\right)\\ {\stackrel{·}{x}}_7=-{\mu }_{1i}^{\prime }{x}_7+{\sigma }_i\sqrt{2{\mu }_{2i}^{\prime }}w\left(t\right)\end{array}\right)---\left(14\right)$

非线性系统的状态方程可以化简为：

$\stackrel{·}{x}=f\left(x\right)+w---\left(15\right)$

(6)建立系统的观测方程

两套光纤陀螺测量单元输出角速率间的关系式：

${\omega }_2={\mathrm{C\omega }}_1+\stackrel{·}{\eta }---\left(16\right)$

观测方程，即两套光纤陀螺测量单元的输出角速率之差为：

$\Delta \omega ={\omega }_1-{\omega }_2={\omega }_1-({\mathrm{C\omega }}_1+\stackrel{·}{\eta })={\omega }_1-{\mathrm{C\omega }}_1-\stackrel{·}{\eta }=(I-C){\omega }_1-\stackrel{·}{\eta }---\left(17\right)$

其中，C为坐标变换阵，具体形式如式(7)所示。

(7)利用CKF滤波理论进行时间更新和量测更新，实现对大角度变形角的测量与监控。 CKF算法具体步骤如下：

一、时间更新：

$p_{k-1/k-1}=S_{k-1/k-1}{S}_{k-1/k-1}^T---\left(18\right)$

$X_{i,k-1/k-1}=S_{k-1/k-1}{\xi }_i+{\hat{x}}_{k-1/k-1},(i=1,2,...,m)---\left(19\right)$

$X_{i,k/k-1}^{*}=f\left(X_{i,k-1/k-1}\right)---\left(20\right)$

其中[1]_i表示集合[1]的第i列，对于二维状态，即n＝2，有：

$\left[1\right]=\left[\left(\begin{array}{c}1\\ 0\end{array}\right)\left(\begin{array}{c}0\\ 1\end{array}\right)\left(\begin{array}{c}-1\\ 0\end{array}\right)\left(\begin{array}{c}0\\ -1\end{array}\right)\right]---\left(21\right)$

${\hat{x}}_{k/k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Sigma }}{X}_{i,k/k-1}^{*}---\left(22\right)$

$p_{k/k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Sigma }}{X}_{i,k/k-1}^{*}{X}_{i,k/k-1}^{*T}-{\hat{x}}_{k/k-1}{\hat{x}}_{k/k-1}^T+Q_{k-1}---\left(23\right)$

二、量测更新：

$p_{k/k-1}=S_{k/k-1}{S}_{k/k-1}^T---\left(24\right)$

$X_{i,k/k-1}=S_{k/k-1}{\xi }_i+{\hat{x}}_{k/k-1},(i=1,2,...,m)---\left(25\right)$

Z_i,k/k-1＝h(X_i,k/k-1)(26)

${\hat{z}}_{k/k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Sigma }}{Z}_{i,k/k-1}---\left(27\right)$

$p_{zz,k/k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Sigma }}{Z}_{i,k/k-1}{Z}_{i,k/k-1}^T-{\hat{z}}_{k/k-1}{\hat{z}}_{k/k-1}^T+R_k---\left(28\right)$

$p_{xz,k/k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Sigma }}{X}_{i,k/k-1}{Z}_{i,k/k-1}^T-{\hat{z}}_{k/k-1}{\hat{z}}_{k/k-1}^T---\left(29\right)$

$W_k=p_{xz,k/k-1}{p}_{zz,k/k-1}^{-1}---\left(30\right)$

${\hat{x}}_{k/k}={\hat{x}}_{k/k-1}+W_k(z_k-{\hat{z}}_{k/k-1})---\left(31\right)$

$p_{k/k}=p_{k/k-1}-W_k{p}_{zz,k/k-1}{W}_k^T---\left(32\right)$

(8)利用船体的姿态角度进行频谱分析，利用分析结果对动态变形角模型参数进行优化 处理

动态变形角中有两个主要参数：λ_i影响动态变形角的振荡周期，μ_i影响变形角的不规则 度。经过仿真分析可知：λ_i的值对仿真结果影响更大。不同主频下的变形角估计误差曲线如 图2所示，不同μ值下的变形角估计误差如图3所示。将出海数小时后的舰船横摇、纵摇等 姿态信息进行频谱分析，由试验数据的频谱图可看出，在三个轴向的动态变形角的频率虽然 会出现多个尖锋，但三个方向变形角主频均主要在一个定值α附近，因此，在进行仿真条件 设置时，主频一般要设为定值α。优化后的变形角估计流程图如图4所示。