CN106802143A - 一种基于惯性仪器和迭代滤波算法的船体形变角测量方法 - Google Patents

Abstract

本发明属于惯性导航中的传递对准技术领域，具体涉及一种基于陀螺和加速度计等惯性仪器和迭代滤波算法的船体形变角测量方法。本发明包括以下步骤：第一步，利用卡尔曼滤波方法求形变角的估计值第二步，分析角速度观测残差的功率谱；第三步，将角速度观测残差和比力观测残差作为卡尔曼滤波观测变量迭代滤波；第四步，通过重复第二步和第三步计算，不断地对第一步的估计结果进行修正，直到形变角的修正值接近但不等于真值本发明所述方法针对大型舰船速度低，摇摆弱的环境特点，能够显著提高垂直方向形变角的测量精度，并且计算量较小，适合在线实时测量。

Description

一种基于惯性仪器和迭代滤波算法的船体形变角测量方法

技术领域

本发明属于惯性导航中的传递对准技术领域，具体涉及一种基于陀螺和加速度计等惯性仪器和迭代滤波算法的船体形变角测量方法。

背景技术

惯性导航需要精确知道运动载体的初始姿态，否则运动载体的定位误差将随着距离的增大而急剧增大。对于舰船来说，高精度的主惯性导航系统(以下简称主惯导)可以给出精确的姿态信息，其他设备诸如导弹、雷达的子惯性导航系统(以下简称子惯导)都将引用主惯导的姿态信息。然而船体并非理想的刚体，由于发动机震动、海浪拍打、昼夜温度变化等原因，船体将产生形变角，主惯导的姿态与子惯导的姿态并不严格相等。因此，船体形变角测量技术成为提高子惯导姿态精度的必要手段。

许多学者围绕船体形变角测量技术开展了研究，《航天测量船船姿船位测量技术》(北京国防工业出版社，潘良，2009年)介绍了一种光学自准直方法测量船体形变角，该方法有很高的精度，但是量程小，设备体积庞大，只能用于航天测量船这样特殊的科考船上，对于其他舰船应用受限。

《Kalman filter formulations for transfer alignment of strapdowninertial units》(《Navigation》，A.M.Schnider，1983年第30期)最早引入了捷联惯导系统测量船体形变角，通过匹配主惯导与子惯导的角速度差值，结合卡尔曼滤波，可以实时估计船体形变角，该方法只利用了捷联惯导的陀螺，并未利用加速度计，因此估计结果并未达到最优。

《F-16flight tests of a rapid transfer alignment procedure》(刊于《IEEEPLANS会议文集》，Shortelle,K.J.等，1998年)引入了加速度计，通过匹配主惯导与子惯导的速度差和姿态差，结果得到了很高的精度。该方法基于机载平台进行研究，需要飞机进行大幅度的机动来加速收敛速度并提高收敛精度，因此并不适合舰载平台运动速度慢、摇摆幅度小的应用场合。

针对大型舰载平台的运动特点，《Performance enhancement of large-shiptransfer alignment:a moving horizon approach》(《Journal of Navigation》，Yang,D等，2013年)提出了一种滚动时域算法，大幅缩短了船体形变角估计的收敛时间，并改善了收敛精度。但该方法在计算处理的过程中，随着计算时间的增加，计算量会急剧增大，因此不适合长期在线测量。

发明内容

针对目前大型舰船形变角测量的不足，本发明基于惯性导航系统，充分利用陀螺和加速度计的测量信息，提出了一种迭代滤波算法测量船体形变角，该方法针对大型舰船速度低，摇摆弱的环境特点，能够显著提高垂直方向形变角的测量精度，并且计算量较小，适合在线实时测量。

本发明的技术方案大体如下：第一步，利用船上主惯导与子惯导测量到的角速度和比力矢量值，按照角速度矢量和比力矢量的匹配方程，结合卡尔曼滤波方法，求得船体形变角的估计值；第二步，将角速度残差和比力残差作为观测变量，启动第二路卡尔曼滤波器进行迭代滤波，求得船体形变角估计值的残差。第三步，利用该残差可以修正第一次滤波的估计值，得到形变角的修正值，使得形变角的估计精度获得提升。第四步，重复第二步和第三步，直到形变角的修正值接近但不等于真值。

为了方便描述该方法，首先给出所有符号含义。

主惯导角速度，主惯导角加速度；其中上标m表示投影在主惯导坐标系，下标im表示主惯导坐标系相对于惯性坐标系。

子惯导角速度；其中上标s表示投影在子惯导坐标系，下标is表示子惯导坐标系相对于惯性坐标系。

主惯导比力；上下标定义与类似。

子惯导比力；上下标定义与类似。

形变角，静态形变角，动态形变角，动态形变角速度。

形变角估计值，形变角估计残差，静态形变角估计残差，动态形变角估计残差。

r,r₀,r_d, 杆臂，静态杆臂，动态杆臂，动态杆臂速度，动态杆臂加速度，动态杆臂三阶时间微分。

δω 角速度观测残差。

δf 比力观测残差。

X 卡尔曼滤波状态向量，

F 卡尔曼滤波状态转移矩阵。

W 卡尔曼滤波状态噪声。

Z 卡尔曼滤波观测向量，

H 卡尔曼滤波观测转移矩阵。

V 卡尔曼滤波观测噪声。

α 二阶高斯马尔科夫模型不规则系数。

ω₀,f₀ 二阶高斯马尔科夫模型中心圆频率，二阶高斯马尔科夫模型中心频率。

噪声方差。

单位白噪声。

以上为本发明的主要符号含义。

下面为本发明的主要步骤：

第一步，利用卡尔曼滤波方法求形变角的估计值

根据学术论文《A Method for Measurement of Static Lever Arm》(刊于2016年Selected Proceedings of the Chinese Society for Optical EngineeringConferences Held November(Vol.9796).International Society for Optics andPhotonics,作者Xianglu Ma人)，角速度矢量匹配方程和比力矢量匹配方程可以写作：

其中形变角可以分为动态形变角和静态形变角之和：

同理，杆臂r可以分为动态杆臂r_d和静态杆臂r₀之和：

r＝r₀+r_d (4)

其中，静态形变角和静态杆臂r₀几乎不随时间变化，因此可以认为它们对时间的微分近似为零：

根据学术论文《Determination of dynamic flexure model parameters forship angular deformation measurement》(刊于2012年Ukacc InternationalConference on Control，作者W Wu等)和《Coupling influence of ship dynamicflexure on high accuracy transfer alignment》(刊于2013年International Journalof Modelling Identification&Control，作者W Wu等)，动态形变角可以由二阶高斯马尔科夫模型来描述：

而动态杆臂三阶时间微分可以由高斯白噪声来描述：

综合(1)～(8)式，可以得到卡尔曼滤波的观测方程和状态方程：

Z＝HX+V (10)

方程中各个向量和矩阵被定义为：

W＝[O O W₁ O O W₂]^T (14)

V＝[V₁ V₂]^T (15)

其中I为3×3单位矩阵，O为3×3全零矩阵，F和H中的空元素都表示零，其他子矩阵被定义为：

其中符号表示对应的反对称矩阵，其他类似符号也都表示对应向量的反对称矩阵。各个符号中下标含有xyz表示该向量对应xyz轴向的分量。基于上述观测方程和状态方程的卡尔曼滤波方法，将观测方程(9)与状态方程(10)代入卡尔曼滤波公式中(卡尔曼滤波公式的推导与使用可见著作《Kalman滤波理论及其在导航系统中的应用》一书，科学出版社出版，付梦印等人编著)，通过不停更新观测变量Z，可以实时得到状态变量X的估计值。由于状态变量X包含了动态形变角和静态形变角，因此可以进一步求得形变角的估计值符号^表示该变量为由卡尔曼滤波方法的估计值。

第二步，分析角速度观测残差的功率谱。

根据著作《Applied Optimal Estimation》(作者Gelb,A.,MIT出版社出版)可知，二阶高斯马尔科夫模型的功率谱密度为：

其中ω₀＝2πf₀，为二阶高斯马尔科夫模型的中心圆频率，由于该模型的带宽是有限的，因此在第一步进行卡尔曼滤波的过程中，二阶高斯马尔科夫模型不能将动态形变角的所有频谱包含在内。这导致动态形变角频谱发生泄露，进而导致形变角的估计产生了误差。为了提取动态形变角泄露的频谱，修正第一步的估计误差，首先以第一步估计的结果为基础，求解角速度观测残差δω和比力观测残差δf：

角速度观测残差δω包含了动态变形角泄露的频谱，利用窗口函数法(或相关法，参考文献《数字信号处理——原理、算法与应用》，电子工业出版社出版，John G.Proakis著，电子工业出版社，2014年出版)分析角速度观测残差δω的频谱，确定谱峰附近的二阶高斯马尔科夫模型中心频率f₀。

第三步，将角速度观测残差和比力观测残差作为卡尔曼滤波观测变量迭代滤波。

滤波前，将滤波器中二阶高斯马尔科夫模型的中心频率更改为第二步中确定的中心频率(中心频率位于矩阵F₁'中对角线上)。观测方程和状态方程为：

Z'＝HX'+V' (23)

方程中各个向量和矩阵被定义为：

W'＝[O O W₁ O O W₂]^T (27)

V'＝[V₁' V₂']^T (28)

子矩阵被定义为：

通过以上滤波方程，得到形变角估计值残差利用形变角估计值残差修正形变角估计值得到形变角的修正值

第四步，通过重复第二步和第三步计算，不断地对第一步的估计结果进行修正。同时对形变角估计残差样本的标准差进行观察，当形变角估计残差样本的标准差不再减小时，说明迭代算法已经达到极限精度，此时形变角的修正值接近但不等于真值作为停止迭代的判定依据。

本发明的有益效果在于：

1.提高了舰船作为载体测量变形的鲁棒性。由于舰船在海上航行时，不能像飞机那样做复杂和剧烈的机动，因此船体形变角的可观测度较低，尤其是在平静的海面，或者锚泊状态下，形变角的测量结果会出现较大的误差，甚至估计结果无法收敛。使用本发明算法可以提高舰船形变角低可观测度条件下测量的鲁棒性。

2.大大提高了垂直方向形变角的精度。由于垂直方向的形变角无法获得重力调制，即便在引入加速度计的条件下，它的测量精度依然受限于陀螺的角速度测量值幅度。通过本发明算法分析角速度残差中的功率谱，再通过迭代滤波大大提升垂直方向形变角的估计精度。

3.提高了水平方向形变角的动态精度。由于水平方向形变角在首次滤波中依然含有动态误差，经过迭代滤波后，角速度残差中的剩余信号同样被提取出来，水平方向形变角的动态精度得到了进一步的提升。

附图说明

图1是本发明实施步骤的流程图；

图2是实施例中主子惯导安装示意图；

1为主惯导及主惯导坐标系的三个轴向；

2为子惯导及子惯导坐标系的三个轴向；

3为舰船。

图3是实施例中船体形变角估计值；

图4为实施例中第一步滤波后误差；

图5为实施例中最终修正后的误差。

具体实施方式

图1是本发明的实施步骤的流程图。如图所示，本发明包括四步：

第一步：利用卡尔曼滤波方法求形变角估计值。本步骤中，观测方程和状态方程由式(9)到(18)给出。观测变量Z中的由主惯导和子惯导测量得到，观测变量Z中的r_s可以由激光测距仪测量得到。启动卡尔曼滤波器后，可以实时算出形变角的估计值。

第二步：分析角速度观测残差的功率谱。本步骤紧接第一步，在求得形变角估计值的同时，同步计算由式(20)到(21)定义的角速度观测残差和比力观测残差。每隔10-15分钟，缓存一段角速度观测残差数据，并通过窗口函数法或相关法分析角速度观测残差的频谱分布，确定角速度观测残差谱峰位置处的中心频率。

第三步：将角速度观测残差和比力观测残差作为卡尔曼滤波观测变量迭代滤波。本步骤也紧接第二步，在求得角速度观测残差和比力观测残差后，替换第一步中的原观测变量，观测方程和状态方程由(22)到(31)给出，然后启动第二路卡尔曼滤波器，可以实时算出形变角的估计残差。再利用该残差实时修正形变角的估计值。在修正估计值的同时，及时更新第二步确定的中心频率f₀，通过中心频率与中心圆频率的关系式：ω₀＝2πf₀，进一步确定中心圆频率。将第二路滤波器中将矩阵F₁'中对角线的中心圆频率ω₀'改为第二步确定的中心圆频率。

第四步：重复第二步到第三步的步骤，不断地对第一步中的形变角估计值进行修正，同时观察每一次迭代后的形变角估计残差样本标准差，形变角估计残差的样本标准差不再减小时，停止迭代计算，得到形变角估计值的最终修正值。

图2至图5是展示了实施例的计算结果，其中的主惯导数据和形变角数据基于某航天测量船8个小时的实测数据低通滤波后得到。图2展示了主惯导与子惯导的安装示意图，其中主惯导1安装在航天测量船3的重心位置，主惯导2安装在航天测量船3船艏位置。主惯导与子惯导的坐标轴X轴指向船艏，Y轴指向船左舷，Z轴垂直于甲板，构成右手笛卡尔坐标系。在主惯导与子惯导之间安装了一套光学自准直系统，该系统测量船体形变角的长期精度为5角秒，它可以用来鉴定本发明算法的效果。图3～图5展示了使用迭代算法求船体形变角的结果，其中每幅图的横坐标均表示采样时刻，纵坐标表示形变角(或误差)的大小。图3展示了第一次滤波船体形变角的估计值，图4展示了第一次滤波估计值与光学系统测量值之差，可以看出来Z方向的误差很大，而且不稳定，在0～120角秒内波动，X和Y方向的误差在零附近波动，但是仍然含有动态误差。图5展示了一次迭代滤波后的结果，可以看出来Y方向的动态误差得到了抑制，而Z方向的误差也收敛至30角秒以内。通过比较可以发现，相比传统滤波匹配算法，本发明的算法在Z方向的提升是显著的。

Claims (1)

1.一种基于惯性仪器和迭代滤波算法的船体形变角测量方法，其特征在于，该方法包括以下步骤：

第一步，利用卡尔曼滤波方法求形变角的估计值

角速度矢量匹配方程和比力矢量匹配方程可以写作：

其中形变角可以分为动态形变角和静态形变角之和：

同理，杆臂r可以分为动态杆臂r_d和静态杆臂r₀之和：

r＝r₀+r_d (4)

其中，静态形变角和静态杆臂r₀几乎不随时间变化，因此可以认为它们对时间的微分近似为零：

${\stackrel{\·}{r}}_0=0---\left(6\right)$

动态形变角可以由二阶高斯马尔科夫模型来描述：

而动态杆臂三阶时间微分可以由高斯白噪声来描述：

${\stackrel{\·\·\·}{r}}_d={\mathrm{\σ}}_r{e}_r---\left(8\right)$

综合(1)～(8)式，可以得到卡尔曼滤波的观测方程和状态方程：

$\stackrel{\·}{X}=FX+W---\left(9\right)$

Z＝HX+V (10)

方程中各个向量和矩阵被定义为：

$Z=\left[\begin{array}{c}{\mathrm{\ω}}_{is}^s-{\mathrm{\ω}}_{im}^m\\ f_{is}^s-f_{im}^m-{\mathrm{\ω}}_{im}^m\×{\mathrm{\ω}}_{im}^m\×r_s-{\stackrel{\·}{\mathrm{\ω}}}_{im}^m\×r_s\end{array}\right]---\left(12\right)$

$F=\left[\begin{array}{cccc}& I& & \\ F_1& F_2& & \\ & & I& \\ & & & I\end{array}\right],H=\left[\begin{array}{cccccc}{H}_1& H_1& I& & & \\ H_2& H_2& & H_3& H_4& I\end{array}\right]---\left(13\right)$

W＝[O O W₁ O O W₂]^T (14)

V＝[V₁ V₂]^T (15)

其中I为3×3单位矩阵，O为3×3全零矩阵，F和H中的空元素都表示零，其他子矩阵被定义为：

$F_1=\left[\begin{array}{ccc}-{\mathrm{\ω}}_{0x}^2& & \\ & -{\mathrm{\ω}}_{0y}^2& \\ & & -{\mathrm{\ω}}_{0z}^2\end{array}\right],F_2=\left[\begin{array}{ccc}-2{\mathrm{\α}}_x& & \\ & -2{\mathrm{\α}}_y& \\ & & -2{\mathrm{\α}}_z\end{array}\right]---\left(16\right)$

$\begin{array}{c}{H}_1=\[{\mathrm{\ω}}_{is}^s\×\]\\ H_2=\[f_{is}^s\×\]\\ H_3=\[{\mathrm{\ω}}_{im}^m\×\]\[{\mathrm{\ω}}_{im}^m\×\]+\[{\stackrel{\·}{\mathrm{\ω}}}_{im}^m\×\]\\ H_4=2\[{\mathrm{\ω}}_{im}^m\×\]\end{array}---\left(17\right)$

其中符号表示对应的反对称矩阵，其他类似符号也都表示对应向量的反对称矩阵，各个符号中下标含有xyz表示该向量对应xyz轴向的分量；基于上述观测方程和状态方程的卡尔曼滤波方法，将观测方程(9)与状态方程(10)代入卡尔曼滤波公式中，通过不停更新观测变量Z，可以实时得到状态变量X的估计值；由于状态变量X包含了动态形变角和静态形变角，因此可以进一步求得形变角的估计值符号^表示该变量为由卡尔曼滤波方法的估计值；

第二步，分析角速度观测残差的功率谱：

二阶高斯马尔科夫模型的功率谱密度为：

其中ω₀＝2πf₀，为二阶高斯马尔科夫模型的中心圆频率；为了提取动态形变角泄露的频谱，修正第一步的估计误差，首先以第一步估计的结果为基础，求解角速度观测残差δω和比力观测残差δf：

角速度观测残差δω包含了动态变形角泄露的频谱，利用窗口函数法或相关法分析角速度观测残差δω的频谱，确定谱峰附近的二阶高斯马尔科夫模型中心频率f₀；

第三步，将角速度观测残差和比力观测残差作为卡尔曼滤波观测变量迭代滤波：

滤波前，将滤波器中二阶高斯马尔科夫模型的中心频率更改为第二步中确定的中心频率，所述中心频率位于矩阵F₁'中对角线上；观测方程和状态方程为：

${\stackrel{\·}{X}}^{\′}=F^{\′}{X}^{\′}+W^{\′}---\left(22\right)$

Z'＝HX'+V' (23)

方程中各个向量和矩阵被定义为：

$Z^{\′}=\left[\begin{array}{c}\mathrm{\δ}\mathrm{\ω}\\ \mathrm{\δ}f\end{array}\right]---\left(25\right)$

$F^{\′}=\left[\begin{array}{cccc}& I& & \\ {F^{\′}}_1& {F^{\′}}_2& & \\ & & I& \\ & & & I\end{array}\right],H=\left[\begin{array}{cccccc}{H}_1& H_1& I& & & \\ H_2& H_2& & H_3& H_4& I\end{array}\right]---\left(26\right)$

W'＝[O O W₁ O O W₂]^T (27)

V'＝[V₁' V₂']^T (28)

子矩阵被定义为：

${F_1}^{\′}=\left[\begin{array}{ccc}-{{\mathrm{\ω}}_{0x}^2}^{\′}& & \\ & -{{\mathrm{\ω}}_{0y}^2}^{\′}& \\ & & -{{\mathrm{\ω}}_{0z}^2}^{\′}\end{array}\right],F_2=\left[\begin{array}{ccc}-2{{\mathrm{\α}}_x}^{\′}& & \\ & -2{{\mathrm{\α}}_y}^{\′}& \\ & & -2{{\mathrm{\α}}_z}^{\′}\end{array}\right]---\left(29\right)$

$\begin{array}{c}{H}_1=\[{\mathrm{\ω}}_{is}^s\×\]\\ H_2=\[f_{is}^s\×\]\\ H_3=\[{\mathrm{\ω}}_{im}^m\×\]\[{\mathrm{\ω}}_{im}^m\×\]+\[{\stackrel{\·}{\mathrm{\ω}}}_{im}^m\×\]\\ H_4=2\[{\mathrm{\ω}}_{im}^m\×\]\end{array}---\left(30\right)$

通过以上滤波方程，得到形变角估计值残差利用形变角估计值残差修正形变角估计值得到形变角的修正值

第四步，通过重复第二步和第三步计算，不断地对第一步的估计结果进行修正；同时对形变角估计残差样本的标准差进行观察，当形变角估计残差样本的标准差不再减小时，说明迭代算法已经达到极限精度，此时形变角的修正值接近但不等于真值作为停止迭代的判定依据。