一种水下航行器对准方法及系统

技术领域

本发明涉及水下航行器自动对准技术领域，特别是涉及一种水下航行器对准方法及系统。

背景技术

自主水下航行器作为人类探索和开发海洋资源的重要工作平台，在军事和民用领域都有着广泛的应用，具备快速的高精度初始对准技术是水下航行器能够顺利完成水下作业的重要前提和保证，同样也是评价水下航行器工程实用性的关键指标之一。在复杂海况下，水下航行器难以快速地完成自主粗对准，当前所采用的技术方案为非线性大失准角一步对准方法，其中一步对准中常用的非线性滤波方法有扩展卡尔曼滤波、无迹卡尔曼滤波、容积卡尔曼滤波等等。

但是非线性大失准角一步对准方法在运算中存在力学编排方程的许多重复计算的问题，而且无法规避计算中存在的欧拉角的奇异问题，导致水下航行器在复杂海况下的自主对准速度较慢，且对准精度不高。

发明内容

本发明的目的是提供一种水下航行器对准方法及系统，能够避免力学编排方程的许多重复计算，规避欧拉角的奇异问题，提高水下航行器在复杂海况下的自主对准精度和速度。

为实现上述目的，本发明提供了如下方案：

一种水下航行器对准方法，包括：

获取水下航行器在当前时刻的航行数据；所述航行数据包括惯性器件数据和量测数据；所述惯性器件数据包括角速度数据和加速度数据；所述量测数据为基于全球定位系统的速度数据或基于多普勒计程仪的速度数据；

根据由所述角速度数据确定的欧拉角微分方程，以及由所述加速度数据确定的比力方程，构建直接法滤波的系统方程；

根据上一时刻的协方差、上一时刻的状态估计值、所述直接法滤波的系统方程和所述量测数据对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值和当前时刻的协方差；所述状态估计值包括姿态数据和速度数据；所述姿态数据包括俯仰角、横滚角和航向角；

根据上一时刻的欧拉角切换标志位对当前时刻的所述姿态数据进行欧拉角切换，得到所述水下航行器在当前时刻更新后的姿态数据和当前时刻的欧拉角切换标志位。

可选地，在所述根据上一时刻的欧拉角切换标志位对当前时刻的所述姿态数据进行欧拉角切换，得到所述水下航行器在当前时刻更新后的姿态数据和当前时刻的欧拉角切换标志位之后，还包括：

将当前时刻更新后的姿态数据和当前时刻的所述速度数据输出。

可选地，所述根据上一时刻的协方差、上一时刻的状态估计值、所述直接法滤波的系统方程和所述量测数据对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值和当前时刻的协方差，具体包括：

根据上一时刻的状态估计值和所述直接法滤波的系统方程进行状态一步预测，得到状态一步预测值和一步预测协方差；

当所述量测数据和所述惯性器件数据采集时刻相同时，则根据所述量测数据、所述状态一步预测值和所述一步预测协方差对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值。

可选地，所述当所述量测数据和所述惯性器件数据采集时刻相同时，则根据所述量测数据、所述状态一步预测值和所述一步预测协方差对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值，具体包括：

当所述量测数据和所述惯性器件数据采集时刻相同，且所述量测数据为基于全球定位系统的速度数据，则根据所述基于全球定位系统的速度数据、所述状态一步预测值和所述一步预测协方差，采用无迹卡尔曼滤波方法对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值。

可选地，所述当所述量测数据和所述惯性器件数据采集时刻相同时，则根据所述量测数据、所述状态一步预测值和所述一步预测协方差对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值，具体包括：

当所述量测数据和所述惯性器件数据采集时刻相同，且所述量测数据为基于多普勒计程仪的速度数据，则根据所述基于多普勒计程仪的速度数据、所述状态一步预测值和所述一步预测协方差，采用迭代无迹卡尔曼滤波方法对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值。

可选地，所述根据上一时刻的欧拉角切换标志位对当前时刻的所述姿态数据进行欧拉角切换，得到所述水下航行器在当前时刻更新后的姿态数据和当前时刻的欧拉角切换标志位，具体包括：

若上一时刻的所述欧拉角切换标志位为0，且当前时刻的俯仰角绝对值大于或等于90°，则将当前时刻的所述姿态数据切换为反欧拉角，并将当前时刻的欧拉角切换标志位置1；

若上一时刻的所述欧拉角切换标志位为1，且当前时刻的横滚角绝对值大于135°或小于45°时，则将当前时刻的所述姿态数据切换为正欧拉角，并将当前时刻的欧拉角切换标志位置0。

本发明还提供了一种水下航行器对准系统，包括：

数据采集模块，用于获取水下航行器在当前时刻的航行数据；所述航行数据包括惯性器件数据和量测数据；所述惯性器件数据包括角速度数据和加速度数据；所述量测数据为基于全球定位系统的速度数据或基于多普勒计程仪的速度数据；

系统方程构建模块，用于根据由所述角速度数据确定的欧拉角微分方程，以及由所述加速度数据确定的比力方程，构建直接法滤波的系统方程；

状态预测模块，用于根据上一时刻的协方差、上一时刻的状态估计值、所述直接法滤波的系统方程和所述量测数据对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值和当前时刻的协方差；所述状态估计值包括姿态数据和速度数据；所述姿态数据包括俯仰角、横滚角和航向角；

切换更新模块，用于根据上一时刻的欧拉角切换标志位对当前时刻的所述姿态数据进行欧拉角切换，得到所述水下航行器在当前时刻更新后的姿态数据和当前时刻的欧拉角切换标志位。

可选地，还包括：

输出模块，用于将当前时刻更新后的姿态数据和当前时刻的所述速度数据输出。

可选地，所述状态预测模块包括：

一步预测单元，用于根据上一时刻的状态估计值和所述直接法滤波的系统方程进行状态一步预测，得到状态一步预测值和一步预测协方差；

状态预测单元，用于当所述量测数据和所述惯性器件数据采集时刻相同时，则根据所述量测数据、所述状态一步预测值和所述一步预测协方差对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值。

可选地，所述切换更新模块包括：

第一切换单元，用于若上一时刻的所述欧拉角切换标志位为0，且当前时刻的俯仰角绝对值大于或等于90°，则将当前时刻的所述姿态数据切换为反欧拉角，并将当前时刻的欧拉角切换标志位置1；

第二切换单元，用于若上一时刻的所述欧拉角切换标志位为1，且当前时刻的横滚角绝对值大于135°或小于45°时，则将当前时刻的所述姿态数据切换为正欧拉角，并将当前时刻的欧拉角切换标志位置0。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明公开了一种水下航行器对准方法及系统，所述方法包括获取水下航行器的当前时刻的航行数据，所述航行数据包括惯性器件数据和量测数据；根据惯性器件数据分别构建欧拉角微分方程和比力方程，并根据欧拉角微分方程和比力方程构建直接法滤波的系统方程，再根据上一时刻的协方差、上一时刻的状态估计值、所述直接法滤波的系统方程和量测数据对所述水下航行器进行状态预测，得到水下航行器在当前时刻的状态估计值和当前时刻的协方差，以及根据状态估计值中姿态角包括的俯仰角和横滚角进行欧拉角切换，进而得到水下航行器在当前时刻更新后的姿态数据和当前时刻的欧拉角切换标志位。其中，所述量测数据为基于全球定位系统的速度数据或基于多普勒计程仪的速度数据。本发明通过选用量测数据及状态预测方法，避免了力学编排方程的许多重复计算，并且通过对欧拉角的切换，规避了欧拉角的奇异问题，保证水下航行器在复杂海况下能够实现快速、高精度的对准效果。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明水下航行器对准方法的方法流程图；

图2为本发明水下航行器对准方法的逻辑示意图；

图3为本发明水下航行器对准系统的模块示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种水下航行器对准方法及系统，能够避免力学编排方程的许多重复计算，规避欧拉角的奇异问题，提高水下航行器在复杂海况下的自主对准精度和速度。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1-图2所示，本发明实施例提供的一种水下航行器对准方法，包括：

步骤100：获取水下航行器在当前时刻的航行数据；所述航行数据包括惯性器件数据和量测数据；所述惯性器件数据包括角速度数据和加速度数据；所述量测数据为基于全球定位系统的速度数据或基于多普勒计程仪的速度数据。

步骤200：根据由所述角速度数据确定的欧拉角微分方程，以及由所述加速度数据确定的比力方程，构建直接法滤波的系统方程。

步骤300：根据上一时刻的协方差、上一时刻的状态估计值、所述直接法滤波的系统方程和所述量测数据对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值和当前时刻的协方差；所述状态估计值包括姿态数据和速度数据；所述姿态数据包括俯仰角、横滚角和航向角。

步骤400：根据上一时刻的欧拉角切换标志位对当前时刻的所述姿态数据进行欧拉角切换，得到所述水下航行器在当前时刻更新后的姿态数据和当前时刻的欧拉角切换标志位。

其中在步骤100中，在获取水下航行器在当前时刻的航行数据后，根据水下航行器在当前时刻的航行数据构建卡尔曼滤波的状态估计值和系统噪声向量。

首先，构建卡尔曼滤波的状态估计值x：

在进行第一次滤波循环时，除当前时刻的航行数据之外，还需采集当前时刻水下航行器的姿态数据，即选取俯仰角、横滚角和航向角φ＝[θγψ]

从进行第二次滤波循环开始，滤波循环中运算的姿态数据为上一时刻滤波循环结束的结果。

其次，构建卡尔曼滤波的系统噪声向量ω：

其中，ω

在步骤200之前，还包括：根据当前时刻水下航行器的姿态数据设置初始欧拉角切换标志位。其中，0表示“312”旋转顺序的正欧拉角，1表示“321”旋转顺序的反欧拉角，“321”旋转顺序的反欧拉角适合初始俯仰角较大的载体，“312”旋转顺序的正欧拉角适合初始横滚角较大的载体，一般选择θ＝±45°和γ

步骤300具体包括：

S1：根据上一时刻的状态估计值和所述直接法滤波的系统方程进行状态一步预测，得到状态一步预测值和一步预测协方差。所述一步预测方法包括根据加速度计输出的比力f

所述一步预测方法包括基于系统噪声的一步预测方法和基于量测噪声的一步预测方法。其中，所述基于系统噪声的一步预测方法用于当量测数据为基于全球定位系统的速度数据时采用的方法；所述基于量测噪声的一步预测方法用于当量测数据为基于多普勒计程仪的速度数据时采用的方法。

所述基于系统噪声的一步预测方法具体步骤如下：

第一步：取无迹卡尔曼滤波参数α为1e-3，κ为0，β为2，其中α表示各个Sigma点与系统状态均值的紧密程度，取值越小则距离越近，κ的取值和系统状态维数相关，其性质为比例因子，β的性质同κ，其作用是将传播前Sigma点的先验统计特性合并起来，然后根据式

第二步：根据第一步中得到的γ和k-1时刻的状态估计值、协方差选取k-1时刻的Sigma点并组成矩阵：

其中，

第三步：将第二步中的Sigma点代入非线性系统方程计算状态一步预测值：

其中，f(·)表示非线性状态转移函数，i表示2L+1个sigma点中的第i个，

λ＝α

W

第四步：计算一步预测协方差：

其中，Q

W

所述基于量测噪声的一步预测方法具体包括：

第一步：根据量测数据和非线性系统方程计算状态一步预测值：

η

其中，h(·)为非线性量测函数，χ

第二步：计算一步预测协方差：

其中，R

S2：当所述量测数据和所述惯性器件数据采集时刻相同时，则根据所述量测数据、所述状态一步预测值和所述一步预测协方差对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值。

由于为了保持对高动态载体的实时跟踪，时间更新频率选取为惯性器件采样频率，因此量测更新的频率远低于时间更新的频率，当所述量测数据和所述惯性器件数据采集时刻相同时，此时量测数据为有效数据，并设置量测量有效位为1；否则量测数据无效，且量测量有效位为0。

若量测数据无效，则将所述状态一步预测值赋值给水下航行器在当前时刻的状态估计值并输出：

若量测数据有效，作为一种实施方式，S2具体包括：

当所述量测数据和所述惯性器件数据采集时刻相同，且所述量测数据为基于全球定位系统的速度数据，则根据所述基于全球定位系统的速度数据、所述状态一步预测值和所述一步预测协方差，采用无迹卡尔曼滤波方法对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值。具体步骤如下：

第一步：确定量测数据z和对应的量测矩阵H：

z＝[v

第二步：计算卡尔曼滤波增益值K

第三步：计算状态估计值

若量测数据有效，作为另一种实施方式，S2具体包括：

当所述量测数据和所述惯性器件数据采集时刻相同，且所述量测数据为基于多普勒计程仪的速度数据，则根据所述基于多普勒计程仪的速度数据、所述状态一步预测值和所述一步预测协方差，采用迭代无迹卡尔曼滤波方法对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值。具体步骤如下：

第一步：计算卡尔曼滤波增益：

其中，W

第二步：计算状态估计值和协方差：

第三步：设置迭代数n＝0，在n＝0时设：

第四步：在n

其中，ut(·)表示UT变换函数，是基于系统噪声的一步预测方法中第二步到第四步的概括，V

第五步：在i＝N时或者满足

第六步：在迭代停止后，得到状态估计值和协方差：

在上述步骤中得到的状态估计值的前六项为运载体的姿态数据和速度数据。

在步骤300之后，还包括：

将当前时刻更新后的姿态数据和当前时刻的所述速度数据输出。

步骤400具体包括：

S1：若上一时刻的所述欧拉角切换标志位为0，且当前时刻的俯仰角绝对值大于或等于90°，则将当前时刻的所述姿态数据切换为反欧拉角，并将当前时刻的欧拉角切换标志位置1。具体步骤如下：

若切换标志位为0，如果条件|θ|＜π/2不满足，将估计量x的前三项φ＝[θ γ ψ]

其中，C

S2：若上一时刻的所述欧拉角切换标志位为1，且当前时刻的横滚角绝对值大于135°或小于45°时，则将当前时刻的所述姿态数据切换为正欧拉角，并将当前时刻的欧拉角切换标志位置0。

若切换标志位为1，如果条件π/4＜|γ

至此就完成了双欧拉角无迹卡尔曼滤波初始对准的一次滤波循环，根据这一时刻得到的状态估计值和协方差进行下一时刻的滤波更新。

本发明还提供了一种水下航行器对准系统，包括：数据采集模块1、系统方程构建模块2、状态预测模块3和切换更新模块4。

具体地，数据采集模块1用于获取水下航行器在当前时刻的航行数据；所述航行数据包括惯性器件数据和量测数据；所述惯性器件数据包括角速度数据和加速度数据；所述量测数据为基于全球定位系统的速度数据或基于多普勒计程仪的速度数据；系统方程构建模块2用于根据由所述角速度数据确定的欧拉角微分方程，以及由所述加速度数据确定的比力方程，构建直接法滤波的系统方程；状态预测模块3用于根据上一时刻的协方差、上一时刻的状态估计值、所述直接法滤波的系统方程和所述量测数据对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值和当前时刻的协方差；所述状态估计值包括姿态数据和速度数据；所述姿态数据包括俯仰角、横滚角和航向角；切换更新模块4用于根据上一时刻的欧拉角切换标志位对当前时刻的所述姿态数据进行欧拉角切换，得到所述水下航行器在当前时刻更新后的姿态数据和当前时刻的欧拉角切换标志位。

进一步的方案，所述水下航行器对准系统还包括：输出模块。

具体地，输出模块用于将当前时刻更新后的姿态数据和当前时刻的所述速度数据输出。

进一步的方案，所述状态预测模块包括：一步预测单元和状态预测单元。

具体地，一步预测单元用于根据上一时刻的状态估计值和所述直接法滤波的系统方程进行状态一步预测，得到状态一步预测值和一步预测协方差；状态预测单元用于当所述量测数据和所述惯性器件数据采集时刻相同时，则根据所述量测数据、所述状态一步预测值和所述一步预测协方差对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值。

进一步的方案，所述切换更新模块包括：第一切换单元和第二切换单元。

具体地，第一切换单元用于若上一时刻的所述欧拉角切换标志位为0，且当前时刻的俯仰角绝对值大于或等于90°，则将当前时刻的所述姿态数据切换为反欧拉角，并将当前时刻的欧拉角切换标志位置1；第二切换单元用于若上一时刻的所述欧拉角切换标志位为1，且当前时刻的横滚角绝对值大于135°或小于45°时，则将当前时刻的所述姿态数据切换为正欧拉角，并将当前时刻的欧拉角切换标志位置0。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。