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

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

背景技术：

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

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

技术实现思路

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

25、可选地，还包括：

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

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

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

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

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

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

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

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

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