【技术实现步骤摘要】
本专利技术涉及水下航行器自动对准，特别是涉及一种水下航行器对准方法及系统。
技术介绍
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°时，则将当前时刻的所述姿本文档来自技高网...
【技术保护点】
1.一种水下航行器对准方法，其特征在于，包括：
2.根据权利要求1所述的水下航行器对准方法，其特征在于，在所述根据上一时刻的欧拉角切换标志位对当前时刻的所述姿态数据进行欧拉角切换，得到所述水下航行器在当前时刻更新后的姿态数据和当前时刻的欧拉角切换标志位之后，还包括：
3.根据权利要求1所述的水下航行器对准方法，其特征在于，所述根据上一时刻的协方差、上一时刻的状态估计值、所述直接法滤波的系统方程和所述量测数据对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值和当前时刻的协方差，具体包括：
4.根据权利要求3所述的水下航行器对准方法，其特征在于，所述当所述量测数据和所述惯性器件数据采集时刻相同时，则根据所述量测数据、所述状态一步预测值和所述一步预测协方差对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值，具体包括：
5.根据权利要求3所述的水下航行器对准方法，其特征在于，所述当所述量测数据和所述惯性器件数据采集时刻相同时，则根据所述量测数据、所述状态一步预测值和所述一步预测协方差对所述水下航行器进
6.根据权利要求1所述的水下航行器对准方法，其特征在于，所述根据上一时刻的欧拉角切换标志位对当前时刻的所述姿态数据进行欧拉角切换，得到所述水下航行器在当前时刻更新后的姿态数据和当前时刻的欧拉角切换标志位，具体包括：
7.一种水下航行器对准系统，其特征在于，包括：
8.根据权利要求7所述的水下航行器对准系统，其特征在于，还包括：
9.根据权利要求7所述的水下航行器对准系统，其特征在于，所述状态预测模块包括：
10.根据权利要求7所述的水下航行器对准系统，其特征在于，所述切换更新模块包括：
...【技术特征摘要】
1.一种水下航行器对准方法，其特征在于，包括：
2.根据权利要求1所述的水下航行器对准方法，其特征在于，在所述根据上一时刻的欧拉角切换标志位对当前时刻的所述姿态数据进行欧拉角切换，得到所述水下航行器在当前时刻更新后的姿态数据和当前时刻的欧拉角切换标志位之后，还包括：
3.根据权利要求1所述的水下航行器对准方法，其特征在于，所述根据上一时刻的协方差、上一时刻的状态估计值、所述直接法滤波的系统方程和所述量测数据对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值和当前时刻的协方差，具体包括：
4.根据权利要求3所述的水下航行器对准方法，其特征在于，所述当所述量测数据和所述惯性器件数据采集时刻相同时，则根据所述量测数据、所述状态一步预测值和所述一步预测协方差对所述水下航行器进行状态预测，得到所述水下航行器在当前时刻的状态估计值，具体包括：
<...【专利技术属性】
技术研发人员：周广涛，吴磊，张云浩，兰贺，李瓷冰，董诗淼，张莹军，
申请(专利权)人：哈尔滨哈船导航技术有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。