一种基于载体姿态信息的升沉测量方法与流程

本发明属于测量技术领域，尤其涉及一种基于载体姿态信息的升沉测量方法。

背景技术：

水面舰船和水下运载器在执行任务期间会随海浪波动产生升沉运动，然而升沉运动会严重影响载体作业效率和安全。因此海上舰船、作业平台通常需要升沉补偿系统来消除升沉运动的影响。目前通常使用gps和惯性导航系统进行组合实现水面作业载体垂向的定位，但是测量精度仅为m级。此外，惯性导航系统凭借其无源特性可实现全天候、全地域、全方位自主导航，但是由于惯性导航系统存在误差随时间累积而发散的问题，特别是垂向，不能做长时间导航。当海面环境恶劣或执行水下任务时，无法收取gps信号，水面舰船和水下运载体的升沉信息则会失真。为了解决这个问题，采用卡尔曼滤波来减小垂向升沉的测量误差，但是使用卡尔曼滤波存在以下补足：1)需要对系统进行精准建模，而误差源较多，难以实现；2)滤波参数调试的工作量非常大；3)测量精度难以提高。

技术实现要素：

本发明的技术解决问题：克服现有技术的不足，提供一种基于载体姿态信息的升沉测量方法，旨在实现在不依赖外部垂向辅助量测信息时，实现对载体垂向的升沉信息的高精度测量。

为了解决上述技术问题，本发明公开了一种基于载体姿态信息的升沉测量方法，包括：

定期获取载体姿态信息；其中，所述载体姿态信息包括：横摇角θ、纵摇角γ和艏摇角中的至少一种；

根据所述姿态信息获得载体随海浪波动的频率和幅值信息；

根据所述频率和幅值信息进行滤波器设计；其中，所述滤波器的滤波器参数包括通带截止频率fp，阻带截止频率fs、通带衰减ap、阻带衰减as、截止频率ωc、滤波阶数n；

将由惯性导航系统确定的载体垂向加速度信息，通过三次滤波和两次积分，得到并输出载体的升沉运动信息。

在上述基于载体姿态信息的升沉测量方法中，根据所述定期获取载体姿态信息；其中，“定期”指在一定时间范围内获取新的姿态信息；所述载体姿态信息包括：横摇角θ、纵摇角γ和艏摇角中的至少一种，包括外测直接给出的姿态信息或由导航系统确定的姿态信息。

在上述基于载体姿态信息的升沉测量方法中，根据所述载体姿态信息获得载体随海浪波动的频率和幅值，包括：

横摇角θ的1hz以内的谐振主峰对应的幅值和频率；和/或，

纵摇角γ的1hz以内的谐振主峰对应的幅值和频率；和/或，

艏摇角的1hz以内的谐振主峰对应的幅值和频率。

横摇角θ、纵摇角γ、艏摇角在1hz以内谐振主峰对应的频率为海浪波动的频率；横摇角θ、纵摇角γ、艏摇角在1hz以内谐振主峰对应的幅值为海浪波动影响各姿态角变化的幅值。

在上述基于载体姿态信息的升沉测量方法中，根据所述频率和幅值信息进行滤波器设计；其中，所述滤波器的滤波器参数包括通带截止频率fp，阻带截止频率fs、通带衰减ap、阻带衰减as、截止频率ωc、滤波阶数n，包括：

步骤1，将横摇角θ、纵摇角γ和艏摇角的1hz以内对应最小频率的谐振主峰的幅值按照下式确定通带截止频率影响因子：

其中，aep、anp、aup分别为横摇角θ、纵摇角γ和艏摇角的1hz以内谐振频率最小的谐振主峰对应的幅值。kep、knp、kup分别为横摇角θ、纵摇角γ和艏摇角的通带截止频率影响因子；

步骤2，按照下式计算通带截止频率：

fp＝kepfep+knpfnp+kupfup

其中，fep、fnp、fup分别为横摇角θ、纵摇角γ和艏摇角的1hz以内谐振主峰对应的最小频率。

步骤3，将横摇角θ、纵摇角γ和艏摇角的1hz以内最大频率的谐振主峰对应的幅值按照下式确定阻带截止频率影响因子：

其中，aes、ans、aus分别为横摇角θ、纵摇角γ和艏摇角的1hz以内谐振频率最大的谐振主峰对应的幅值。kes、kns、kus分别为横摇角θ、纵摇角γ和艏摇角的阻带截止频率影响因子；

步骤4，按照下式计算阻带截止频率：

fs＝kesfes+knsfns+kusfus

其中，fes、fns、fus分别为横摇角θ、纵摇角γ和艏摇角的1hz以内谐振主峰对应的最大频率；

步骤5，设置通带衰减ap、阻带衰减as；

步骤6，进行巴特沃斯低通滤波器设计，按照下式确定滤波器滤波阶数n和截止频率ωc

其中，f为采样频率。

步骤7，根据滤波阶数n和截止频率ωc确定巴特沃斯低通滤波器传递函数hl(s)；

步骤8，对滤波器进行双线性z变换，得到转移函数hl(z)；

步骤9，按照下式确定高通滤波器hh(z):

hh(z)＝1-hl(z)

在上述基于载体姿态信息的升沉测量方法中，根据所述将由惯性导航系统确定的载体垂向加速度信息，通过三次滤波和两次积分，得到并输出载体的升沉运动信息，包括：

步骤1，按照下式获得垂向加速度信息，

其中，fu为垂向加速度，为载体系到导航系的姿态变换矩阵，fb为载体三个轴向加速度计测得的比力信息，为由地球自转和哥氏加速度带来的有害加速度，gⁿ为当地导航系下的重力加速度；

步骤2，根据hh(z)确定常系数线性差分方程:

其中，x(n)为滤波前信号，y(n)为滤波后信号，bm为hh(z)分子的系统数组，ak为hh(z)分母的系统数组；

步骤3，对垂向加速度fu按照差分方程y(n)进行滤波，得到ful；

步骤4，对滤波后的加速度ful进行积分，得到速度vu；

步骤5，对速度vu按照差分方程y(n)进行滤波，得到vul；

步骤6，对滤波后的速度vul进行积分，得到升沉高度hu；

步骤7，对升沉高度hu按照差分方程y(n)进行滤波，得到hul；

步骤8，输出升沉信息hul。

本发明具有以下优点：

(1)本发明通过定期对载体姿态信息进行频谱分析，可实时获取载体随海浪波动的特性，并结合影响因子实现滤波器的自适应最优化设计，相比参数固定的滤波器，本发明的滤波器参数可自适应调整，载体的升沉测量精度更高。

(2)本发明不依赖于gps等外部测量方式，仅通过载体姿态角信息实现对载体升沉运动的测量，相比由gps和惯性导航组合的测量方式，本发明在无gps信号的情况下，提高了载体升沉测量的测量精度和测量可靠性。

(3)现有卡尔曼滤波算法需要对系统进行精确建模，模型的准确性制约测量精度，本发明方法不依赖于系统的模型，只需通过分析载体姿态信息进行滤波器设计，实现载体升沉的高精度测量。

(4)现有的卡尔曼滤波算法滤波周期较长，对系统运算资源占用大，本发明的方法运算速度快，资源占用小，具有广泛的适用性。

附图说明

图1是本发明实施例中一种基于载体姿态信息的升沉测量方法的步骤流程图；

图2是本发明实施例中载体升沉测量的原理图；

图3是本发明实施例中一种东向通道误差修正的原理图；

图4是本发明实施例中基于横摇角分析的海浪波动特性结果示意图；

图5是本发明实施例中基于纵摇角分析的海浪波动特性结果示意图；

图6是本发明实施例中基于艏摇角分析的海浪波动特性结果示意图；

图7是基于本发明实施例所述的方法实现的升沉测量结果示意图。

具体实施方式

本发明核心思想之一在于：通过分析载体姿态信息的频谱特性来确定载体随海浪波动的特性。根据波动特性自适应调整滤波器参数，实现滤波器设计。通过惯性导航系统原理获得载体垂向加速度信息，然后经过3次滤波，2次积分后获得载体升沉运动信息。

下面结合附图和具体实例对本发明作进一步的详细描述：

参照图1，示出了本发明实施例中一种基于载体姿态信息的升沉测量方法的步骤流程图。在本实施例中，所述载体姿态信息的升沉测量方法包括：

步骤101，定期获取载体姿态信息。

在本实施例中，定期获取载体姿态据信息是指在设定的周期内通过载体导航系统或其他姿态测量装置获得接近载体真实的姿态信息，其中，所述姿态信息包括但不仅限于：横摇角θ、纵摇角γ和艏摇角中的至少一种。

步骤102，根据所述姿态信息获得载体随海浪波动的频率和幅值信息，进行滤波器设计。

在本实施例中，所述载体随海浪波动的频率和幅值是通过分析载体的姿态信息的频率特性来获取，其中海浪的波动特性可由以下内容来表征：

1)横摇角θ的1hz以内的谐振主峰对应的幅值和频率；和/或，

2)纵摇角γ的1hz以内的谐振主峰对应的幅值和频率；和/或，

3)艏摇角的1hz以内的谐振主峰对应的幅值和频率。

横摇角θ、纵摇角γ、艏摇角在1hz以内谐振主峰对应的频率为海浪波动的频率；横摇角θ、纵摇角γ、艏摇角在1hz以内谐振主峰对应的幅值为海浪波动影响各姿态角变化的幅值。

本实施例中，所述的滤波器为互补型巴特沃斯滤波器，滤波参数包括通带截止频率fp，阻带截止频率fs、通带衰减ap、阻带衰减as、截止频率ωc、滤波阶数n。根据所述海浪波动特性来确定滤波参数，实现滤波器设计，具体实施步骤如下：

步骤1，将横摇角θ、纵摇角γ和艏摇角的1hz以内对应最小频率的谐振主峰的幅值按照下式确定通带截止频率影响因子：

其中，aep、anp、aup分别为横摇角θ、纵摇角γ和艏摇角的1hz以内谐振频率最小的谐振主峰对应的幅值。kep、knp、kup分别为横摇角θ、纵摇角γ和艏摇角的通带截止频率影响因子。

为了便于理解，以一个具体实例对所述滤波器设计进行详细说明。

以采用捷联惯性导航系统的某船舶a为例。

试验时，船舶a系泊于码头，船体随海浪波动存在升沉运动。参照图2、图3、图4分别为船舶a在某段时间内横摇角θ、纵摇角γ、艏摇角的频谱特性。参照表1，为船舶a姿态角在1hz以内的频谱特性

表1

参照表1，横摇角θ、纵摇角γ和艏摇角的1hz以内对应最小频率的谐振主峰的幅值分别为aep＝0.036db/hz、anp＝0.00174db/hz、aup＝11.708db/hz。根据上式得到横摇角θ、纵摇角γ和艏摇角的通带截止频率影响因子分别为kep＝0.00306、knp＝0.00015、kup＝0.99679。

步骤2，按照下式计算通带截止频率：

fp＝kepfep+knpfnp+kupfup

其中，fep、fnp、fup分别为横摇角θ、纵摇角γ和艏摇角的1hz以内谐振主峰对应的最小频率。

参照表1，可得fep＝0.057hz、fnp＝0.076hz、fup＝0.059hz，根据上式可知fp＝0.058996hz；

步骤3，将横摇角θ、纵摇角γ和艏摇角的1hz以内最大频率的谐振主峰对应的幅值按照下式确定阻带截止频率影响因子：

其中，aes、ans、aus分别为横摇角θ、纵摇角γ和艏摇角的1hz以内谐振频率最大的谐振主峰对应的幅值。kes、kns、kus分别为横摇角θ、纵摇角γ和艏摇角的阻带截止频率影响因子。

参照表1，横摇角θ、纵摇角γ和艏摇角的1hz以内对应最大频率的谐振主峰对应的幅值分别为aes＝0.02db/hz、ans＝0.004db/hz、aus＝0.15db/hz。

根据上式得到横摇角θ、纵摇角γ和艏摇角的阻带截止频率影响因子分别为kes＝0.1149、kns＝0.02299、kus＝0.86211。

步骤4，按照下式计算阻带截止频率：

fs＝kesfes+knsfns+kusfus

其中，fes、fns、fus分别为横摇角θ、纵摇角γ和艏摇角的1hz以内谐振主峰对应的最大频率。

参照表1，可得fes＝0.46452hz、fns＝0.46424hz、fus＝0.46673hz，根据上式可知fs＝0.4663hz；

步骤5，设置通带衰减ap、阻带衰减as；本具体实例中设置通带衰减ap＝3、阻带衰减as＝20。

步骤6，进行巴特沃斯低通滤波器设计，按照下式确定滤波器滤波阶数n和截止频率ωc

其中，f为采样频率。

根据本具体实例，采样频率f＝200hz，按上式计算可得n＝2，ωc＝0.0093hz；

步骤7，根据滤波阶数n和截止频率ωc确定巴特沃斯低通滤波器传递函数hl(s)。

根据本具体实例，

步骤8，对滤波器进行双线性z变换，得到转移函数hl(z)。

根据本具体实例，

步骤9，按照下式确定高通滤波器hh(z):

hh(z)＝1-hl(z)

根据本具体实例，

步骤103，对载体垂向加速度信息通过三次滤波和两次积分，得到并输出载体的升沉运动信息。

参照图5，示出了本发明实施例中对载体垂向加速度信息通过三次滤波和两次积分，得到并输出载体的升沉运动信息的步骤流程图。

本实施例中，具体实施步骤如下：

步骤1，根据惯性导航速度误差方程获得垂向加速度信息，如下式所示：

其中，fe为东向加速度，fn为北向加速度，fu为垂向加速度，为载体系到导航系的姿态变换矩阵，fb为载体三个轴向加速度计测得的比力信息，为由地球自转和哥氏加速度带来的有害加速度，其中，为地球相对于惯性系在导航系的投影角速度，为载体在导航系相对地球系的投影角速度，ven为载体速度，gⁿ为当地导航系下的重力加速度；

步骤2，根据hh(z)确定常系数线性差分方程

其中，x(n)为滤波前信号，y(n)为滤波后信号，bm为hh(z)分子的系统数组，ak为hh(z)分母的系统数组。

根据本具体实例，线性差分方程

y(n)＝1.999934321y(n-1)-0.999934324y(n-2)+0.9999999995x(n)-1.9999343228x(n-1)+0.999934323x(n-2)；

步骤3，对垂向加速度fu按照差分方程y(n)进行滤波，得到ful；

步骤4，对滤波后的加速度ful进行积分，得到速度vu；

步骤5，对速度vu按照差分方程y(n)进行滤波，得到vul；

步骤6，对滤波后的速度vul进行积分，得到升沉高度hu；

步骤7，对升沉高度hu按照差分方程y(n)进行滤波，得到hul；

步骤8，输出升沉信息hul。

本发明优选为三次滤波两次积分的方式得到升沉信息，若滤波次数为两次，则省略步骤3中的滤波，直接进行积分得到速度。

根据具体实例，使用本发明方法得到升沉测量结果图6。当缺少gps等辅助测量装置时，未使用本发明方法得到升沉测量结果图7。图6与图7相比较可以明显看到，使用本发明方法可以精确测量载体的升沉变化。

综上所述，本发明实施例所述的基于载体姿态信息的升沉测量方法，通过定期分析载体姿态角频谱特性而确定海浪波动特性，进而确定滤波参数，最终实现载体升沉的精确测量。本发明相比于现有组合导航系统，本发明不依赖gps等外部升沉量测装置，测量可靠性更高；相比于现有的卡尔曼滤波算法，本发明不依赖于系统建模精度，提高了测量精度，并且对系统运算资源占用小，运算速度快，具有广泛的适用性。另外，当海浪波动特性发生变化后，本发明可自主调整滤波器参数，提高了测量精度和可靠性。

以上所述，仅为本发明最佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。