CN113447018B - 一种水下惯性导航系统的姿态实时估计方法 - Google Patents

一种水下惯性导航系统的姿态实时估计方法 Download PDF

Info

Publication number
CN113447018B
CN113447018B CN202110765930.6A CN202110765930A CN113447018B CN 113447018 B CN113447018 B CN 113447018B CN 202110765930 A CN202110765930 A CN 202110765930A CN 113447018 B CN113447018 B CN 113447018B
Authority
CN
China
Prior art keywords
axis
angular velocity
marking
attitude
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110765930.6A
Other languages
English (en)
Other versions
CN113447018A (zh
Inventor
何雪生
蒋志强
海行洲
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Polytechnic Navigation Control Technology Co ltd
Original Assignee
Beijing Polytechnic Navigation Control Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Polytechnic Navigation Control Technology Co ltd filed Critical Beijing Polytechnic Navigation Control Technology Co ltd
Priority to CN202110765930.6A priority Critical patent/CN113447018B/zh
Publication of CN113447018A publication Critical patent/CN113447018A/zh
Application granted granted Critical
Publication of CN113447018B publication Critical patent/CN113447018B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明涉及水下运动载体姿态实时测量领域，且公开了一种水下惯性导航系统的姿态实时估计方法，其包括对IMU中的陀螺传感器的信息进行处理，得到导航坐标系下的角速度，其步骤为：1.1、将三轴陀螺的原始测量值，扣除零偏后得到立体坐标系下的三轴角速度值，并将其标记为ωx、ωy、ωz，三轴陀螺的原始测量值记为G_measure，零偏记为g_bias，立体坐标系下的角速度值记为Gb，则有：Gb＝G_measure–g_bias，Gb、G_measure、g_bias都是三维列矢量，三个分量分别对应X轴、Y轴、Z轴。本发明相比与卡尔曼滤波器，能够明显减少运算量，并且引入经典的闭环负反馈环节，与基于权重的校正方法相比，计算结果精度更高，收敛性更好。

Description

一种水下惯性导航系统的姿态实时估计方法
技术领域
本发明属于水下运动载体姿态实时测量领域，具体为一种水下惯性导航系统的姿态实时估计方法。
背景技术
水下运动载体需要提供高精度而且实时的姿态估计。高精度测量是高精度控制的前提，实时性则是对应了控制周期的需要。
差分GPS接收机虽然可以提供误差不随时间发散的姿态信息，但水下大部分时间无法接收到GPS信号，无法实现实时性测量。
要提供实时的姿态信息，传统的方法就是基于惯性测量单元的捷联惯性导航算法。由于陀螺解算具有发散特性，为了抑制误差的增长，通常利用GPS对惯性导航的误差进行校正，其特点是误差不随时间发散。
利用外部不随时间发散的绝对测量信息抑制惯性导航系统误差的方法主要是采用卡尔曼滤波器法与基于权重的校正方法。
卡尔曼滤波器通过建立状态方程与观测方程对状态方程中的状态量进行估计，得到滤波后的位置、速度、姿态。然而卡尔曼滤波器的运算量偏大，运算量与滤波器的维数平方成正比，而且非白噪声条件下的应用以及参数设置的不合理往往会导致滤波器不稳定，因此设计一种水下惯性导航系统的姿态实时估计方法来解决这种问题很有必要。
发明内容
本发明的目的在于：为解决上述背景技术中提出的问题，本发明提供了一种水下惯性导航系统的姿态实时估计方法。
为实现上述目的，本发明提供如下技术方案：一种水下惯性导航系统的姿态实时估计方法，包括：
(一)对IMU中的陀螺传感器的信息进行处理，得到导航坐标系下的角速度，其步骤为：
1.1、将三轴陀螺的原始测量值，扣除零偏后得到立体坐标系下的三轴角速度值，并将其标记为ωx、ωy、ωz，三轴陀螺的原始测量值记为G_measure，零偏记为g_bias，立体坐标系下的角速度值记为Gb，则有：Gb＝G_measure-g_bias，Gb、G_measure、g_bias都是三维列矢量，三个分量分别对应X轴、Y轴、Z轴；
1.2、通过姿态转换矩阵得到导航坐标系下的角速度值，采用传统成熟方法利用陀螺信息得到姿态转换矩阵
Figure GDA0004100936640000021
导航坐标系下的角速度记为Gr，则有：
Figure GDA0004100936640000022
Figure GDA0004100936640000023
是三维列矢量，且三个分量分别对应X轴、Y轴、Z轴；
(二)基于陀螺角输入速度的进行姿态预测，其公式如下：ZT(n)＝ZT(n-1)+ω(n-1)×T，ω(n-1)代表n-1时刻某单轴脱落输出角速度：并使用数字积分器实现对P2支路以及和P1支路的积分，其步骤为：
2.1、计算预测的姿态：
Figure GDA0004100936640000024
Figure GDA0004100936640000025
是当前时刻姿态预测输出，等效于前面公式的ZT(n)，经过上式积分前的/>
Figure GDA0004100936640000026
等效于上述公式的ZT(n-1)；
2.2、计算当前时刻姿态误差errZT(n)：
Figure GDA0004100936640000027
ZTref(n)表示当前时刻的外部姿态参考输入，/>
Figure GDA0004100936640000028
表示当前时刻的姿态预测，来自上面公式中积分后的/>
Figure GDA0004100936640000029
2.3、根据需要的带宽BD确定支路的系数P1，P2，其中P1＝1.47BD，P2＝1.787BD2；
2.4、根据姿态误差计算P1支路输出path1，path1＝errZT(n)×P1；
2.5、根据姿态误差计算P2支路输出path21，path21(n)＝path21(n-1)+errZT(n)×P2×T；
2.6、通过测量得到的导航坐标系的角速度支路：
Figure GDA00041009366400000210
角速度X轴：
Figure GDA00041009366400000211
角速度Y轴：
Figure GDA00041009366400000212
角速度Z轴：
Figure GDA00041009366400000213
2.7、计算姿态输出：
Figure GDA00041009366400000214
Figure GDA00041009366400000215
与卡尔曼滤波器相比，本发明能够明显减少运算量，并且引入经典的闭环负反馈环节，与基于权重的校正方法相比，计算结果精度更高，收敛性更好，且特点为运行较为平缓，俯仰轴、横滚轴无较大姿态角度变换，易实现，易推广，能够快速准确的对弹体姿态信息进行实时估计。
附图说明
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：
图1为本发明的原理步骤示意图；
图2为本发明的姿态预测模型示意图；
图3为本发明运用的数字积分器原理示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例；基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参阅图1-3，本具体实施方式提供的一种水下惯性导航系统的姿态实时估计方法，以导航坐标系为例进行说明，一共需要三个方位支路，分别实现对俯仰(X轴)、偏航(Y轴)、横滚(Z轴)的参考姿态进行跟踪，然后分别得到俯仰、偏航、横滚的本地姿态估计，以任意一个轴向的姿态估计为例进行说明，其步骤为包括：(一)对IMU中的陀螺传感器的信息进行处理，得到导航坐标系下的角速度，其步骤为：1.1、将三轴陀螺的原始测量值，扣除零偏后得到立体坐标系下的三轴角速度值，并将其标记为ωx、ωy、ωz，三轴陀螺的原始测量值记为G_measure，零偏记为g_bias，立体坐标系下的角速度值记为Gb，则有：Gb＝G_measure-g_bias，Gb、G_measure、g_bias都是三维列矢量，三个分量分别对应X轴、Y轴、Z轴；1.2、通过姿态转换矩阵得到导航坐标系下的角速度值，采用传统成熟方法利用陀螺信息得到姿态转换矩阵
Figure GDA0004100936640000031
导航坐标系下的角速度记为Gr，则有：/>
Figure GDA0004100936640000032
Figure GDA0004100936640000033
是三维列矢量，且三个分量分别对应X轴、Y轴、Z轴；(二)基于陀螺角输入速度的进行姿态预测，其公式如下：ZT(n)＝ZT(n-1)+ω(n-1)×T，ω(n-1)代表n-1时刻某单轴脱落输出角速度：并使用数字积分器实现对P2支路以及和P1支路的积分，其步骤为：2.1、计算预测的姿态：
Figure GDA0004100936640000034
是当前时刻姿态预测输出，等效于前面公式的ZT(n)，经过上式积分前的/>
Figure GDA0004100936640000035
等效于上述公式的ZT(n-1)；2.2、计算当前时刻姿态误差errZT(n)：
Figure GDA0004100936640000036
Figure GDA0004100936640000037
ZTref(n)表示当前时刻的外部姿态参考输入，
Figure GDA0004100936640000038
表示当前时刻的姿态预测，来自上面公式中积分后的/>
Figure GDA0004100936640000039
2.3、根据需要的带宽BD确定支路的系数P1，P2，其中P1＝1.47BD，P2＝1.787BD2；2.4、根据姿态误差计算P1支路输出path1，path1＝errZT(n)×P1；2.5、根据姿态误差计算P2支路输出path21，path21(n)＝path21(n-1)+errZT(n)×P2×T；2.6、通过测量得到的导航坐标系的角速度支路：
Figure GDA0004100936640000041
角速度X轴：/>
Figure GDA0004100936640000042
角速度Y轴：/>
Figure GDA0004100936640000043
角速度Z轴：/>
Figure GDA0004100936640000044
Figure GDA0004100936640000045
2.7、计算姿态输出：/>
Figure GDA0004100936640000046
Figure GDA0004100936640000047
与卡尔曼滤波器相比，本发明能够明显减少运算量，并且引入经典的闭环负反馈环节，与基于权重的校正方法相比，计算结果精度更高，收敛性更好，且特点为运行较为平缓，俯仰轴、横滚轴无较大姿态角度变换，易实现，易推广，能够快速准确的对弹体姿态信息进行实时估计。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (1)

1.一种水下惯性导航系统的姿态实时估计方法，其特征在于，所述方法包括：
(一)对IMU中的陀螺传感器的信息进行处理，得到导航坐标系下的角速度，其步骤为：
1.1、将三轴陀螺的原始测量值，扣除零偏后得到立体坐标系下的三轴角速度值，并将其标记为ωx、ωy、ωz，三轴陀螺的原始测量值记为G_measure，零偏记为g_bias，立体坐标系下的角速度值记为Gb，则有：Gb＝G_measure–g_bias，Gb、G_measure、g_bias都是三维列矢量，三个分量分别对应X轴、Y轴、Z轴；
1.2、通过姿态转换矩阵得到导航坐标系下的角速度值，采用传统成熟方法利用陀螺信息得到姿态转换矩阵
Figure FDA0004100936620000011
导航坐标系下的角速度记为Gr，则有：/>
Figure FDA0004100936620000012
Figure FDA0004100936620000013
是三维列矢量，且三个分量分别对应X轴、Y轴、Z轴；
(二)基于陀螺角输入速度进行姿态预测，其公式如下：ZT(n)＝ZT(n-1)+ω(n-1)×T，ω(n-1)代表n-1时刻某单轴脱落输出角速度：并使用数字积分器实现对P2支路以及和P1支路的积分，其步骤为：
2.1、计算预测的姿态：
Figure FDA0004100936620000014
Figure FDA0004100936620000015
是当前时刻姿态预测输出，等效于前面公式的ZT(n)，经过上式积分前的/>
Figure FDA0004100936620000016
等效于上述公式的ZT(n-1)；
2.2、计算当前时刻姿态误差errZT(n)：
Figure FDA0004100936620000017
ZTref(n)表示当前时刻的外部姿态参考输入，/>
Figure FDA0004100936620000018
表示当前时刻的姿态预测，来自上面公式中积分后的/>
Figure FDA0004100936620000019
2.3、根据需要的带宽BD确定支路的系数P1，P2，其中P1＝1.47BD，P2＝1.787BD2；
2.4、根据姿态误差计算P1支路输出path1，path1＝errZT(n)×P1；
2.5、根据姿态误差计算P2支路输出path21，path21(n)＝path21(n-1)+errZT(n)×P2×T；
2.6、通过测量得到的导航坐标系的角速度支路：
Figure FDA00041009366200000110
角速度X轴：
Figure FDA00041009366200000111
角速度Y轴：
Figure FDA00041009366200000112
角速度Z轴：
Figure FDA0004100936620000021
2.7、计算姿态输出：
Figure FDA0004100936620000022
Figure FDA0004100936620000023
/>
CN202110765930.6A 2021-07-06 2021-07-06 一种水下惯性导航系统的姿态实时估计方法 Active CN113447018B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110765930.6A CN113447018B (zh) 2021-07-06 2021-07-06 一种水下惯性导航系统的姿态实时估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110765930.6A CN113447018B (zh) 2021-07-06 2021-07-06 一种水下惯性导航系统的姿态实时估计方法

Publications (2)

Publication Number Publication Date
CN113447018A CN113447018A (zh) 2021-09-28
CN113447018B true CN113447018B (zh) 2023-05-26

Family

ID=77815264

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110765930.6A Active CN113447018B (zh) 2021-07-06 2021-07-06 一种水下惯性导航系统的姿态实时估计方法

Country Status (1)

Country Link
CN (1) CN113447018B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114018255B (zh) * 2021-11-03 2023-06-27 湖南国天电子科技有限公司 一种水下滑翔机的智能组合导航方法、系统、设备和介质

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1766332A4 (en) * 2004-06-02 2010-03-03 Athena Technologies Inc SYSTEMS AND METHODS FOR EVALUATING THE POSITION, ATTITUDE AND / OR CAP OF A VEHICLE
CN112097763B (zh) * 2020-08-28 2022-07-05 西北工业大学 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN112729279A (zh) * 2020-11-17 2021-04-30 武汉第二船舶设计研究所(中国船舶重工集团公司第七一九研究所) 一种基于cckf的水下无人潜器姿态测量方法
CN112629538B (zh) * 2020-12-11 2023-02-14 哈尔滨工程大学 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法

Also Published As

Publication number Publication date
CN113447018A (zh) 2021-09-28

Similar Documents

Publication Publication Date Title
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN109596018B (zh) 基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN104698485B (zh) 基于bd、gps及mems的组合导航系统及导航方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN111551174A (zh) 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN110044385B (zh) 一种大失准角情况下的快速传递对准方法
CN116817896B (zh) 一种基于扩展卡尔曼滤波的姿态解算方法
CN116147624B (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN108592943A (zh) 一种基于opreq方法的惯性系粗对准计算方法
CN115900770B (zh) 一种机载环境下磁传感器的在线校正方法和系统
CN108871323B (zh) 一种低成本惯性传感器在机动环境下的高精度导航方法
CN108344413A (zh) 一种水下滑翔器导航系统及其低精度与高精度转换方法
CN108627152A (zh) 一种微型无人机基于多传感器数据融合的导航方法
CN108592917A (zh) 一种基于失准角的卡尔曼滤波姿态估计方法
CN111750865A (zh) 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法
CN106595669B (zh) 一种旋转体姿态解算方法
CN113447018B (zh) 一种水下惯性导航系统的姿态实时估计方法
CN111190207B (zh) 基于pstcsdref算法的无人机ins bds组合导航方法
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
CN115200578A (zh) 基于多项式优化的惯性基导航信息融合方法及系统
Guan et al. Sensor fusion of gyroscope and accelerometer for low-cost attitude determination system

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant