基于法向量位置模型的船舶惯导容错阻尼方法及系统技术方案

本发明专利技术公开了一种基于法向量位置模型的船舶惯导容错阻尼方法及系统。方法包括：S1、判断外部参考速度的误差变化是否大于第一预设值，若是则从阻尼惯导解算模型切换至无阻尼惯导解算模型，由无阻尼惯导解算模型输出；S2、判断外部参考速度的误差变化是否小于第二预设值，若是则继续使用无阻尼惯导解算模型输出，直至重新阻尼的舒勒回路恢复稳定再切换阻尼惯导解算模型输出。本发明专利技术同时具备无阻尼惯导解算和阻尼惯导解算，阻尼切换方便，可避免阻尼切换过程引起的超调振荡；阻尼惯导解算在无阻尼惯导解算外部完成，解算过程以及结果不影响无阻尼惯导解算，以输出校正形式实现了反馈校正效果，扩大了算法的工程适用范围。扩大了算法的工程适用范围。扩大了算法的工程适用范围。

【技术实现步骤摘要】
基于法向量位置模型的船舶惯导容错阻尼方法及系统


[0001]本专利技术涉及导航
，更具体地说，特别涉及一种基于法向量位置模型的船舶惯导容错阻尼方法及系统。

技术介绍

[0002]导航的准确性和可靠性对海上航行至关重要。受海洋环境的限制，导航主要依靠INS/DVL/GNSS。在强干扰或水下时，无法使用GNSS，只能依靠INS/DVL导航。当船舶在深海航行时，DVL的速度信息是相对于水的。由于洋流流速难以实时精确建模和测量，组合导航滤波算法缺乏准确的流速观测，滤波效果不佳。
[0003]相比之下，阻尼算法对外部参考速度要求较低。可以证明，当参考速度没有恒定偏差时，阻尼算法等价于卡尔曼滤波算法。当参考速度具有一定偏差时，阻尼算法性能较好。然而，当外部参考速度误差变化较大时，阻尼算法并不有效。为了保证导航的可靠性，传统的措施是切断舒勒回路的阻尼，以保持惯导系统的精度。当外部参考速度误差变化较小时，外部参考速度由无阻尼切换为有阻尼。在变阻尼时刻，舒勒回路会出现较大的超调振荡，影响阻尼效果。传统的阻尼算法不适用于极区，船舶进出极区时，需要在传统导航算法和极区算法之间进行切换，开关过程会影响阻尼过程的连续性和一致性。为此，有必要开发一种基于法向量位置模型的船舶惯导容错阻尼方法及系统。

技术实现思路

[0004]本专利技术的目的在于提供一种基于法向量位置模型的船舶惯导容错阻尼方法及系统，以克服现有技术所存在的缺陷。
[0005]为了达到上述目的，本专利技术采用的技术方案如下：
[0006]一种基于法向量位置模型的船舶惯导容错阻尼方法，该方法基于无阻尼惯导解算模型和阻尼惯导解算模型实现，所述无阻尼惯导解算模型的输入为加表和陀螺采样值，无阻尼惯导解算模型的输出为纯惯导解算的速度、位置和姿态，所述阻尼惯导解算模型的输入为纯惯导解算的速度、位置和反馈的阻尼速度、位置，阻尼惯导解算模型的输出是按惯导解算频率更新的速度、高度，所述阻尼惯导解算模型中阻尼环节包括高度阻尼和速度阻尼，所述高度阻尼的输入为外部参考高度和按惯导解算频率更新的高度，高度阻尼的输出为阻尼高度，所述速度阻尼的输入为外部参考速度和按惯导解算频率更新的速度，速度阻尼的输出阻尼速度和法向量，所述法向量和阻尼高度合成为阻尼位置，该方法包括以下步骤：
[0007]S1、判断外部参考速度的误差变化是否大于第一预设值，若是则从阻尼惯导解算模型切换至无阻尼惯导解算模型，由无阻尼惯导解算模型输出；
[0008]S2、判断外部参考速度的误差变化是否小于第二预设值，若是则继续使用无阻尼惯导解算模型输出，直至重新阻尼的舒勒回路恢复稳定再切换阻尼惯导解算模型输出。
[0009]进一步地，所述阻尼位置的合成公式为D为阻尼。
[0010]进一步地，在阻尼环节中，若有外部参考速度时，阻尼速度的微分方程为：
[0011][0012]式中，表示阻尼速度，表示阻尼速度的微分，表示载体坐标系到地球坐标系的方向余弦矩阵，表示比力，表示地球自转角速度，表示阻尼重力，表示外部参考速度，C是速度阻尼函数，符号上带～表示为观测值；
[0013]在无阻尼惯导解算模型中，无阻尼速度的微分方程为：
[0014][0015]式中，表示无阻尼速度，表示无阻尼速度的微分，表示无阻尼速度，表示无阻尼重力；
[0016]阻尼速度的微分方程减去无阻尼速度的微分方程为：
[0017][0018][0019]令是串联校正网络传递函数，式中则阻尼速度公式为：
[0020][0021]式中，t
k
‑1表示前一时刻，t
k
表示当前时刻；
[0022]选取Q为相位滞后
‑
超前串联校正网络，ε为阻尼比，ω
s
为舒勒周期频率。
[0023]进一步地，在阻尼环节中，若无外部参考速度时，阻尼速度按惯导解算频率更新，其微分方程为：
[0024][0025]在无阻尼惯导解算模型中，无阻尼速度的微分方程为：
[0026][0027]阻尼速度的微分方程减去无阻尼速度的微分方程为：
[0028][0029]两边积分，得：
[0030][0031]式中，为当前阻尼速度，为前一时刻阻尼速度，为当前无阻尼速度，为前一时刻无阻尼速度，为当前阻尼位置，为前一时刻阻尼位置，为当前无阻尼位置，为前一时刻无阻尼位置，为当前阻尼重力，为前一时刻阻尼重力，为当前时刻无重力，前一时刻阻尼重力；
[0032]阻尼速度初始值计算：
[0033][0034]阻尼位置初始值计算：
[0035][0036]根据阻尼位置初始值计算阻尼重力初始值
[0037]将和代入阻尼速度公式，求得阻尼速度更新值
[0038][0039]阻尼位置更新值：
[0040][0041]根据阻尼位置更新值计算
[0042]将更新值和作为t
k
时刻按惯导解算频率更新的阻尼速度和阻尼位置。
[0043]本专利技术还提供一种根据上述的基于法向量位置模型的船舶惯导容错阻尼方法的系统，该系统基于无阻尼惯导解算模型和阻尼惯导解算模型实现，所述无阻尼惯导解算模型的输入为加表和陀螺采样值，无阻尼惯导解算模型的输出为纯惯导解算的速度、位置和姿态，所述阻尼惯导解算模型的输入为纯惯导解算的速度、位置和反馈的阻尼速度、位置，阻尼惯导解算模型的输出是按惯导解算频率更新的速度、高度，所述阻尼惯导解算模型中阻尼环节包括高度阻尼和速度阻尼，所述高度阻尼的输入为外部参考高度和按惯导解算频率更新的高度，高度阻尼的输出为阻尼高度，所述速度阻尼的输入为外部参考速度和按惯导解算频率更新的速度，速度阻尼的输出阻尼速度和法向量，所述法向量和阻尼高度合成为阻尼位置，该系统包括：
[0044]第一判断模块，用于判断外部参考速度的误差变化是否大于第一预设值，若是则从阻尼惯导解算模型切换至无阻尼惯导解算模型，由无阻尼惯导解算模型输出；
[0045]第二判断模块，用于判断外部参考速度的误差变化是否小于第二预设值，若是则继续使用无阻尼惯导解算模型输出，直至重新阻尼的舒勒回路恢复稳定再切换阻尼惯导解算模型输出。
[0046]与现有技术相比，本专利技术的优点在于：本专利技术同时具备无阻尼惯导解算和阻尼惯导解算，阻尼切换方便，可避免阻尼切换过程引起的超调振荡；阻尼惯导解算在无阻尼惯导解算外部完成，解算过程以及结果不影响无阻尼惯导解算，以输出校正形式实现了反馈校正效果，扩大了算法的工程适用范围。
附图说明
[0047]为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0048]图1是本专利技术基于法向量位置模型的船舶惯导容错阻尼方法中阻尼系统的框图。
[0049]图2是本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于法向量位置模型的船舶惯导容错阻尼方法，其特征在于，该方法基于无阻尼惯导解算模型和阻尼惯导解算模型实现，所述无阻尼惯导解算模型的输入为加表和陀螺采样值，无阻尼惯导解算模型的输出为纯惯导解算的速度、位置和姿态，所述阻尼惯导解算模型的输入为纯惯导解算的速度、位置和反馈的阻尼速度、位置，阻尼惯导解算模型的输出是按惯导解算频率更新的速度、高度，所述阻尼惯导解算模型中阻尼环节包括高度阻尼和速度阻尼，所述高度阻尼的输入为外部参考高度和按惯导解算频率更新的高度，高度阻尼的输出为阻尼高度，所述速度阻尼的输入为外部参考速度和按惯导解算频率更新的速度，速度阻尼的输出阻尼速度和法向量，所述法向量和阻尼高度合成为阻尼位置，该方法包括以下步骤：S1、判断外部参考速度的误差变化是否大于第一预设值，若是则从阻尼惯导解算模型切换至无阻尼惯导解算模型，由无阻尼惯导解算模型输出；S2、判断外部参考速度的误差变化是否小于第二预设值，若是则继续使用无阻尼惯导解算模型输出，直至重新阻尼的舒勒回路恢复稳定再切换阻尼惯导解算模型输出。2.根据权利要求1所述的基于法向量位置模型的船舶惯导容错阻尼方法，其特征在于，所述阻尼位置的合成公式为D为阻尼。3.根据权利要求1所述的基于法向量位置模型的船舶惯导容错阻尼方法，其特征在于，在阻尼环节中，若有外部参考速度时，阻尼速度的微分方程为：式中，表示阻尼速度，表示阻尼速度的微分，表示载体坐标系到地球坐标系的方向余弦矩阵，表示比力，表示地球自转角速度，表示阻尼重力，表示外部参考速度，C是速度阻尼函数，符号上带～表示为观测值；在无阻尼惯导解算模型中，无阻尼速度的微分方程为：式中，表示无阻尼速度，表示无阻尼速度的微分，表示无阻尼重力；阻尼速度的微分方程减去无阻尼速度的微分方程为：阻尼速度的微分方程减去无阻尼速度的微分方程为：令是串联校正网络传递函数，式中则阻尼速度公式为：式中，t
k
‑1表示前一时刻，t
k
表示当前时刻；选取Q为相位滞后
‑
超前串联校正网络，ε为阻尼比，ω
s

【专利技术属性】
技术研发人员：冯国虎，吴文启，曾观林，
申请(专利权)人：中国人民解放军国防科技大学，
类型：发明
国别省市：