CN115574817A - 一种基于三轴旋转式惯导系统的导航方法及导航系统 - Google Patents

一种基于三轴旋转式惯导系统的导航方法及导航系统 Download PDF

Info

Publication number
CN115574817A
CN115574817A CN202211569096.4A CN202211569096A CN115574817A CN 115574817 A CN115574817 A CN 115574817A CN 202211569096 A CN202211569096 A CN 202211569096A CN 115574817 A CN115574817 A CN 115574817A
Authority
CN
China
Prior art keywords
measurement unit
error
attitude
auxiliary
matrix
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.)
Granted
Application number
CN202211569096.4A
Other languages
English (en)
Other versions
CN115574817B (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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202211569096.4A priority Critical patent/CN115574817B/zh
Publication of CN115574817A publication Critical patent/CN115574817A/zh
Application granted granted Critical
Publication of CN115574817B publication Critical patent/CN115574817B/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
    • 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/20—Instruments for performing navigational calculations

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

本发明公开了一种基于三轴旋转式惯导系统的导航方法及导航系统，用于解决现有系统中由于旋转调制引起的系统输出姿态波动问题。通过在三轴旋转式惯导系统的外环转动框架上安装辅助惯性测量单元，然后以内环转动框架上的主惯性测量单元解算出的位置、速度、姿态信息作为观测值，对辅助惯性测量单元的导航误差进行实时卡尔曼滤波最优估算，并在旋转调制周期的整数倍时对辅助惯性测量单元的误差进行定期校正，最后以辅助惯性测量单元导航计算的载体姿态角作为导航系统的姿态输出，从而实现对旋转调制引起的姿态波动抑制。本发明可以有效提高旋转惯导系统姿态精度，能够满足多种应用场合对三轴旋转式惯导系统的高稳定性或高精度姿态需求。

Description

一种基于三轴旋转式惯导系统的导航方法及导航系统
技术领域
本发明涉及惯性导航技术领域，具体是一种基于三轴旋转式惯导系统的导航方法及导航系统。
背景技术
惯性导航系统能够实时输出其所安装载体的位置、速度、姿态等导航信息，具有高可靠性、高隐蔽性等诸多优点，广泛地应用在飞机、导弹、车辆、舰船等需要自主导航信息的场合。自从光学陀螺（激光陀螺、光纤陀螺）出现以后，在捷联惯性导航技术的基础上发展了旋转式惯性导航系统，它采用系统级旋转调制技术来降低惯性元件误差对导航结果的影响，从而提升惯导系统的精度。旋转式惯导系统具有精度高、可靠性好、成本适中等突出优点，目前在国内外的航海舰船领域等需要长期自主导航的场合得到了广泛应用。
旋转式惯导系统根据转轴数目的多少可以划分为单轴系统、双轴系统、三轴系统等类型，其中三轴旋转式系统不但可以自动调制抵消掉所有惯性元件漂移误差对导航结果的影响，而且可以实现对载体航向运动的完全隔离，从而能够获得更高的导航定位精度，在航海高精度惯性导航领域具有良好的应用前景。三轴旋转式惯导系统的定位精度远优于单轴系统或者纯捷联式系统，但是其短期姿态精度却表现较差，其原因在于，三轴旋转式惯导系统在正常导航工作时，其核心部件惯性测量单元（IMU）在空间不断翻转，由于轴系转动误差、结构变形误差等因素影响，系统的输出姿态角上叠加了额外的IMU调制姿态误差，从而导致其姿态稳定性或者短时间内姿态波动情况要劣于单轴系统或捷联式系统，并由此限制了三轴旋转式系统在一些对姿态性能要求更高的场合应用。
具体来说，对于三轴旋转式惯导系统，其内部惯性导航解算进行姿态更新直接得到的是IMU（惯性测量单元）的姿态矩阵，需要根据系统实时采集的旋转机构转动角度，将IMU的姿态矩阵转换为系统基座的姿态矩阵，才能得到最终需要的载体姿态角。在这个过程中，调制机构的测角误差、轴系的结构误差、IMU的安装误差，将会引入到最终的姿态输出波动中，例如轴系测角误差或者结构变形角度10角秒，则IMU在空间翻转会导致在一个调制周期中的系统姿态输出附加±10角秒的姿态波动。特别是对于激光陀螺三轴旋转式惯导系统来说，其内部IMU广泛采用的是机械抖动激光陀螺，需要在IMU内部安装橡胶减振器来保障IMU的正常工作，当IMU在空间翻转时，橡胶减振器的变形最大可以达到10到30个角秒范围，这将导致最终的惯导系统输出姿态角（航向角、纵摇角、横摇角）增加了±10～±30角秒的姿态输出波动。采用误差建模软件补偿、光学姿态监控补偿等方法可以减小姿态输出波动，短期效果较好，但是长时间看各种补偿参数会发生变化，导致补偿的稳定性和精度有限，仍然达不到较好的长期效果。
高性能惯性导航系统的姿态精度是其核心性能指标之一，其姿态输出需要满足载体姿态控制、制导控制、传递对准、变形测量、姿态基准等多种应用需求，提高其姿态性能意义重大。对于三轴旋转式惯导系统来说，其定位性能可以长时间保持，但是其短期姿态性能反而不如单轴系统或纯捷联式惯导系统，从而使其姿态精度正成为系统的一个明显短板。为了提升三轴旋转式惯导系统的姿态性能，本发明提供了一种基于三轴旋转式惯导系统的导航方法及导航系统，能够显著消除掉旋转调制时IMU空间翻转引起的姿态输出波动误差，从而满足多种应用场合的高稳定性、高精度姿态性能需求。
发明内容
针对上述现有技术中三轴旋转式惯导系统中存在的姿态输出波动问题，本发明提供一种基于三轴旋转式惯导系统的导航方法及导航系统，能够有效消除由于惯性测量单元旋转调制与系统各种误差耦合引起的惯导系统输出姿态波动，从而显著提升三轴旋转式惯导系统的姿态稳定性。
为实现上述目的，本发明提供一种基于三轴旋转式惯导系统的导航方法及导航系统，在载体上搭载基于三轴旋转式惯导系统的导航系统，所述导航系统包括外环转动框架、中环转动框架、内环转动框架与主惯性测量单元，并在外环转动框架上安装辅助惯性测量单元；
所述导航方法包括如下步骤：
步骤1，对所述主惯性测量单元、所述辅助惯性测量单元的惯性测量数据分别进行实时惯性导航解算，得到主惯性导航结果与辅助惯性导航结果；
步骤2，基于所述主惯性导航结果对所述辅助惯性测量单元的惯性导航误差进行卡尔曼滤波最优估算并定期校正；
步骤3，基于校正后的所述辅助惯性测量单元得到载体姿态，并将所述载体姿态与所述主惯性导航结果中的位置信息、速度信息作为导航结果输出。
在其中一个实施例，步骤2具体为：
步骤2.1，以所述主惯性导航结果为基准值，得到所述辅助惯性测量单元的导航误差观测矢量，并建立所述辅助惯性测量单元的误差状态模型和误差观测模型；
步骤2.2，根据所述导航误差观测矢量，对所述辅助惯性测量单元的状态误差参量进行实时卡尔曼滤波递推计算，并在进行所述卡尔曼滤波递推计算时间T KF 后，得到所述辅助惯性测量单元的状态误差参量最优估计值，其中，T KF 为所述主惯性测量单元旋转调制周期的整数倍；
步骤2.3，根据所述辅助惯性测量单元的状态误差参量最优估计值，对所述辅助惯性测量单元的惯性导航误差进行校正；
步骤2.4，完成上述步骤2.1至步骤2.3后，重新按照步骤2.1至步骤2.3的方法以T KF 时间周期不断循环进行辅助惯性测量单元的误差校正。
在其中一个实施例，步骤2.1中，所述导航误差观测矢量，具体为：
Figure 948247DEST_PATH_IMAGE001
其中，Z为导航误差观测矢量，
Figure 696760DEST_PATH_IMAGE002
为经度误差观测值，
Figure 700488DEST_PATH_IMAGE003
为纬度 误差观测值，
Figure 79517DEST_PATH_IMAGE004
为东向速度误差观测值，
Figure 535906DEST_PATH_IMAGE005
北向速度误差观测值，
Figure 291372DEST_PATH_IMAGE006
、
Figure 884028DEST_PATH_IMAGE007
、
Figure 965116DEST_PATH_IMAGE008
、
Figure 112064DEST_PATH_IMAGE009
分别为所述辅助惯性导航结果经度、纬度、东向速度、北向速度，
Figure 405642DEST_PATH_IMAGE010
、
Figure 115453DEST_PATH_IMAGE011
、
Figure 367443DEST_PATH_IMAGE012
、
Figure 798424DEST_PATH_IMAGE013
分别为所述主惯性导航结果中的经度、纬度、东向速度、北向速度，
Figure 98956DEST_PATH_IMAGE014
、
Figure 666203DEST_PATH_IMAGE015
、
Figure 620253DEST_PATH_IMAGE016
为姿态误 差角度参量，T为矩阵转置；
所述误差状态模型和所述误差观测模型为：
Figure 272951DEST_PATH_IMAGE017
Figure 908332DEST_PATH_IMAGE018
其中，X为所述辅助惯性测量单元的状态误差参量，A为状态矩阵，H为观测矩阵。
在其中一个实施例，步骤2.2中，所述卡尔曼滤波递推计算的具体过程为：
状态一步预测：
Figure 595665DEST_PATH_IMAGE019
状态估计：
Figure 189457DEST_PATH_IMAGE020
滤波增益矩阵：
Figure 329452DEST_PATH_IMAGE021
一步预测误差方差阵：
Figure 34102DEST_PATH_IMAGE022
估计误差方差阵：
Figure 575942DEST_PATH_IMAGE023
其中，
Figure 75057DEST_PATH_IMAGE024
为系统第k步的n维状态向量，
Figure 905609DEST_PATH_IMAGE025
为系统的m维观测序列，
Figure 882793DEST_PATH_IMAGE026
为第k步n维状态向量的预测值，
Figure 810297DEST_PATH_IMAGE027
为系统第k-1步的n维状态向量，
Figure 483243DEST_PATH_IMAGE028
为滤波增益矩阵，
Figure 597829DEST_PATH_IMAGE029
为一步预测误差方差阵，
Figure 581966DEST_PATH_IMAGE030
为第k-1步的估计误差方差阵，
Figure 98398DEST_PATH_IMAGE031
为第k步的估计误差方差 阵，I为单位矩阵，
Figure 204894DEST_PATH_IMAGE032
为
Figure 806777DEST_PATH_IMAGE033
维观测矩阵，
Figure 594604DEST_PATH_IMAGE034
为
Figure 965543DEST_PATH_IMAGE035
维噪声输入矩阵，
Figure 977361DEST_PATH_IMAGE036
为系统 过程噪声
Figure 800961DEST_PATH_IMAGE037
的
Figure 189217DEST_PATH_IMAGE038
维对称非负定方差矩阵，
Figure 680241DEST_PATH_IMAGE039
为系统观测噪声
Figure 862960DEST_PATH_IMAGE040
的
Figure 439435DEST_PATH_IMAGE041
维对称 正定方差阵，
Figure 569065DEST_PATH_IMAGE042
为系统的
Figure 649017DEST_PATH_IMAGE043
维状态转移矩阵，
Figure 533796DEST_PATH_IMAGE044
根据状态矩阵A计算得到；
设所述主惯性测量单元的旋转调制周期为
Figure 331988DEST_PATH_IMAGE045
，从所述三轴旋转式惯导系统的 三轴转动机构处于零位的时刻开始对所述辅助惯性测量单元的状态误差参量X进行卡尔曼 滤波递推计算，当计算时间T KF 为转动周期的整数倍时，所述辅助惯性测量单元的状态误差 参量最优估计值
Figure 796467DEST_PATH_IMAGE046
为：
Figure 199767DEST_PATH_IMAGE047
其中，
Figure 10376DEST_PATH_IMAGE048
、
Figure 295864DEST_PATH_IMAGE049
、
Figure 829613DEST_PATH_IMAGE050
、
Figure 884157DEST_PATH_IMAGE051
为所述辅助惯性测量单元的经度、纬度、东向速度、北 向速度误差参量最优估计值，
Figure 782843DEST_PATH_IMAGE052
、
Figure 555627DEST_PATH_IMAGE053
、
Figure 893067DEST_PATH_IMAGE054
为所述辅助惯性测量单元的姿态角度误差参量最 优估计值，
Figure 802118DEST_PATH_IMAGE055
、
Figure 934022DEST_PATH_IMAGE056
、
Figure 131785DEST_PATH_IMAGE057
为所述辅助惯性测量单元的陀螺漂移误差参量最优估计值，
Figure 7337DEST_PATH_IMAGE058
、
Figure 302052DEST_PATH_IMAGE059
、
Figure 339278DEST_PATH_IMAGE060
为所述辅助惯性测量单元的加速度计漂移误差参量最优估计值。
在其中一个实施例，步骤3中，所述基于校正后的所述辅助惯性测量单元得到载体姿态，具体为：
步骤3.1，基于校正后的所述辅助惯性测量单元的姿态矩阵，得到载体的姿态矩阵；
步骤3.2，基于载体的姿态矩阵，解算得到载体的三个姿态角。
在其中一个实施例，步骤3.1具体为：
首先，基于所述辅助惯性测量单元的状态误差参量最优估计值中的姿态角度误差 参量最优估计值
Figure 86654DEST_PATH_IMAGE061
、
Figure 703580DEST_PATH_IMAGE062
、
Figure 587223DEST_PATH_IMAGE063
，得到姿态误差矩阵
Figure 60929DEST_PATH_IMAGE064
，为：
Figure 30022DEST_PATH_IMAGE065
其次，基于姿态误差矩阵
Figure 450639DEST_PATH_IMAGE066
对包含有姿态误差的辅助惯性测量单元姿态矩阵
Figure 188788DEST_PATH_IMAGE067
进行修正，得到修正后的辅助惯性测量单元姿态矩阵
Figure 836326DEST_PATH_IMAGE068
，为：
Figure 558294DEST_PATH_IMAGE069
最后，利用辅助惯性测量单元的姿态矩阵
Figure 517023DEST_PATH_IMAGE070
，结合所述三轴旋转式惯导系统的三 轴转动机构外环轴的转动角度
Figure 375258DEST_PATH_IMAGE071
，到载体的姿态矩阵
Figure 190767DEST_PATH_IMAGE072
，为：
Figure 134452DEST_PATH_IMAGE073
其中，
Figure 693609DEST_PATH_IMAGE074
为从载体坐标系到辅助惯性测量单元测量坐标系之间的变换矩阵。
在其中一个实施例，在所述辅助惯性测量单元的惯性导航误差经过第一次校正后，步骤3中所述载体姿态保持由所述辅助惯性测量单元的姿态矩阵计算得到并实时输出，同时辅助惯性测量单元的姿态矩阵误差以周期T KF 进行定期校正，从而保证所述载体姿态输出的长期精度。
在其中一个实施例，步骤3.2中，载体的三个姿态角分别为：
Figure 609613DEST_PATH_IMAGE075
Figure 596023DEST_PATH_IMAGE076
Figure 292584DEST_PATH_IMAGE077
其中，
Figure 389853DEST_PATH_IMAGE078
为纵摇角，
Figure 957101DEST_PATH_IMAGE079
为横摇角，
Figure 317675DEST_PATH_IMAGE080
为航向角，
Figure 970373DEST_PATH_IMAGE081
为姿态矩阵
Figure 871333DEST_PATH_IMAGE082
的第3行、第 2列得值，
Figure 824245DEST_PATH_IMAGE083
为姿态矩阵
Figure 152459DEST_PATH_IMAGE084
的第3行、第1列得值，
Figure 495715DEST_PATH_IMAGE085
为姿态矩阵
Figure 934787DEST_PATH_IMAGE086
的第3行、第3列得 值，
Figure 742206DEST_PATH_IMAGE087
为姿态矩阵
Figure 238391DEST_PATH_IMAGE088
的第1行、第2列得值，
Figure 865681DEST_PATH_IMAGE089
为姿态矩阵
Figure 311706DEST_PATH_IMAGE090
的第2行、第2列得值。
为实现上述目的，本发明还提供一种基于三轴旋转式惯导系统的导航系统，包括：
三轴旋转式惯导系统，包括基座、外环转动框架、中环转动框架、内环转动框架与主惯性测量单元，所述主惯性测量单元固联安装在所述内环转动框架上，所述主惯性测量单元包括3个主陀螺仪和3个主加速度计；
辅助惯性测量单元，包括设在所述外环转动框架上的3个辅助陀螺仪和3个辅助加速度计；
导航计算单元，与所述主惯性测量单元、所述辅助惯性测量单元电连接，用于根据上述的导航方法的部分或全部步骤进行惯性导航。
在其中一个实施例，所述辅助陀螺仪为能够与所述外环转动框架刚性固连的全固态陀螺仪。
与现有技术相比，本发明的优点在于：
1、本发明解决了三轴旋转式惯导系统在导航工作时，在一个旋转调制周期内，由惯性测量单元旋转调制与系统各种误差耦合引起的惯导输出姿态波动问题，显著提升三轴旋转式惯导系统的姿态稳定性，为高精度的载体姿态控制、制导控制、传递对准、变形测量、姿态基准等提供了姿态信息保障；
2、本发明的导航方法及导航系统，利用外环轴隔离了载体航向运动对辅助惯性测量单元、主惯性测量单元的影响，并利用主惯性测量单元对辅助惯性测量单元的误差进行了监控校正，因此硬件上只需要增加一套中低精度的辅助惯性测量单元，使整个系统以较小的代价获得了较大的姿态性能提升。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
图1为本发明实施例中现有三轴旋转式惯导系统转动结构示意图；
图2为本发明实施例中基于三轴旋转式惯导系统的导航系统结构示意图；
图3为本发明实施例中导航方法的流程示意图；
图4为本发明实施例中三轴系统传统方法输出姿态角波动误差示意图，其中：(a)为航向角波动误差示意图，(b)为纵摇角波动误差示意图，(c)为横摇角波动误差示意图；
图5为本发明实施例中辅助惯性测量单元纯惯性姿态角误差示意图，其中：(a)为航向角误差示意图，(b)为纵摇角误差示意图，(c)为横摇角误差示意图；
图6为本发明实施例中采用辅助惯性测量单元后系统输出姿态角误差示意图，其中：(a)为航向角误差示意图，(b)为纵摇角误差示意图，(c)为横摇角误差示意图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
本实施例中公开了一种基于三轴旋转式惯导系统的导航方法及导航系统，通过在载体上搭载基于三轴旋转式惯导系统的导航系统，并在三轴旋转式惯导系统的外环转动框架上安装辅助惯性测量单元，利用外环轴隔离了载体航向运动对辅助惯性测量单元、主惯性测量单元的影响，并利用主惯性测量单元对辅助惯性测量单元的误差进行了监控校正，解决了三轴旋转式惯导系统在导航工作时，在一个旋转调制周期内，由惯性测量单元旋转调制与系统各种误差耦合引起的惯导输出姿态波动问题，显著提升三轴旋转式惯导系统的姿态稳定性。
参考图1为现有的三轴旋转式惯导系统。图1中，1为基座，2为外环转动框架，3为中环转动框架，4为内环转动框架，5为惯性测量单元。其中，基座安装在载体上，惯性测量单元（IMU）固联安装在转动机构的内环转动框架上，上面安装有3个高精度陀螺仪和3个加速度计。
本实施例中所基于的三轴旋转式惯导系统如图2所示。图2中，1为基座，2为外环转 动框架，3为中环转动框架，4为内环转动框架，5为主惯性测量单元，6、7、8分别为辅助惯性 测量单元的三个辅助陀螺仪，9为加速度计组件（内含3个辅助加速度计）。辅助惯性测量单 元的辅助陀螺仪和辅助加速度计固联在一个半包围的外环转动框架上，其中，辅助陀螺仪 为能够与外环转动框架刚性固连的全固态陀螺仪，例如四频差动激光陀螺、光纤陀螺、半球 谐振陀螺等。三轴转动机构的外环轴垂直于载体的安装平面，中环轴平行于载体的安装平 面。当三个转轴处于零位时，三个转轴之间相互正交。并且此时载体坐标系
Figure 973631DEST_PATH_IMAGE091
、辅助 惯性测量单元坐标系
Figure 909226DEST_PATH_IMAGE092
、主惯性测量单元坐标系
Figure 758234DEST_PATH_IMAGE093
完全重合。在系统正常导 航工作时，外环轴用于隔离载体的航向运动，中环轴和内环轴用于实现对主惯性测量单元 的旋转调制。
在本实施例中的三轴旋转式惯导系统正常工作时，外环轴带动外环转动框架和辅 助惯性测测量单元一起转动，中环轴带动中环转动框架转动，内环轴带动内环转动框架和 主惯性测量单元一起转动。三轴转动结构的转动角度由对应轴系中的测角元件实时测量得 到，其中，内环转动框架、中环转动框架、外环转动框架的转动角度分别为
Figure 7949DEST_PATH_IMAGE094
、
Figure 524381DEST_PATH_IMAGE095
、
Figure 365298DEST_PATH_IMAGE096
。导航 计算单元分别对主惯性测量单元和辅助惯性测量单元进行2路独立的惯性导航实时解算， 并控制外环轴隔离载体的航向变化，保证外环转动框架上的辅助惯性测量单元不随载体航 向转动；同时控制中环轴和内环轴带动主惯性测量单元在空间旋转或翻转，实现对主惯性 测量单元的旋转调制及长航时导航误差抑制。
本实施例中的导航方法如图3所示，具体包括如下步骤：
步骤1，对主惯性测量单元、辅助惯性测量单元的惯性测量数据分别进行实时惯性导航解算，得到主惯性导航结果与辅助惯性导航结果。
对主惯性测量单元进行实时惯性导航解算得到的主惯性导航结果包括主惯性测 量单元的东向速度
Figure 967181DEST_PATH_IMAGE097
、北向速度
Figure 551746DEST_PATH_IMAGE098
、天向速度
Figure 125947DEST_PATH_IMAGE099
、经度
Figure 137765DEST_PATH_IMAGE100
、纬度
Figure 492523DEST_PATH_IMAGE101
、高度
Figure 615200DEST_PATH_IMAGE102
以及姿态矩阵
Figure 309487DEST_PATH_IMAGE103
，其中，
Figure 492206DEST_PATH_IMAGE104
代表从坐标系
Figure 803102DEST_PATH_IMAGE105
到计算地理坐标系
Figure 995049DEST_PATH_IMAGE106
的变换矩阵。根据姿态 矩阵
Figure 340579DEST_PATH_IMAGE107
得到坐标系
Figure 697130DEST_PATH_IMAGE108
中的姿态矩阵值
Figure 698584DEST_PATH_IMAGE109
，其中
Figure 428643DEST_PATH_IMAGE110
为从坐标系
Figure 628680DEST_PATH_IMAGE111
到坐标系
Figure 418781DEST_PATH_IMAGE112
的变换矩阵，
Figure 704269DEST_PATH_IMAGE113
根据内框架转动角度
Figure 441281DEST_PATH_IMAGE114
和中框架转动角度
Figure 495825DEST_PATH_IMAGE115
实时计算得到。
具体地，坐标系
Figure 191248DEST_PATH_IMAGE116
先绕
Figure 229611DEST_PATH_IMAGE117
轴转动角度
Figure 301473DEST_PATH_IMAGE118
，再绕
Figure 413785DEST_PATH_IMAGE119
轴转动角度
Figure 545689DEST_PATH_IMAGE120
，最后得到 坐标系
Figure 805769DEST_PATH_IMAGE121
，则
Figure 681321DEST_PATH_IMAGE122
可以表示为：
Figure 648140DEST_PATH_IMAGE123
对辅助惯性测量单元进行实时惯性导航解算得到的辅助惯性导航结果包括辅助 惯性测量单元的东向速度
Figure 950946DEST_PATH_IMAGE124
、北向速度
Figure 698322DEST_PATH_IMAGE125
、天向速度
Figure 111986DEST_PATH_IMAGE126
、经度
Figure 198890DEST_PATH_IMAGE127
、纬度
Figure 672597DEST_PATH_IMAGE128
、高度
Figure 638760DEST_PATH_IMAGE129
、以及姿 态矩阵
Figure 121694DEST_PATH_IMAGE130
，其中，
Figure 859843DEST_PATH_IMAGE131
代表从坐标系
Figure 707713DEST_PATH_IMAGE132
到计算地理坐标系
Figure 164103DEST_PATH_IMAGE133
的变换矩阵。
步骤2，基于主惯性导航结果对辅助惯性测量单元的惯性导航误差进行卡尔曼滤波最优估算并定期校正，其具体实施过程为：
步骤2.1，以主惯性导航结果为基准值，得到辅助惯性测量单元的导航误差观测矢量，并建立辅助惯性测量单元的误差状态模型和误差观测模型；
步骤2.2，根据导航误差观测矢量，对辅助惯性测量单元的状态误差参量进行实时卡尔曼滤波递推计算，并在进行卡尔曼滤波递推计算时间T KF 后，得到辅助惯性测量单元的状态误差参量最优估计值，其中，T KF 为主惯性测量单元旋转调制周期的整数倍；
步骤2.3，根据辅助惯性测量单元的状态误差参量最优估计值，对辅助惯性测量单元的惯性导航误差进行校正；
步骤2.4，完成上述步骤2.1至步骤2.3后，重新按照步骤2.1至步骤2.3的方法以T KF 时间周期不断循环进行辅助惯性测量单元的误差校正。
步骤2.1中，导航误差观测矢量具体为：
Figure 185148DEST_PATH_IMAGE134
其中，Z为导航误差观测矢量，
Figure 43383DEST_PATH_IMAGE135
为经度误差观测值，
Figure 593313DEST_PATH_IMAGE136
为 纬度误差观测值，
Figure 740260DEST_PATH_IMAGE137
为东向速度误差观测值，
Figure 299418DEST_PATH_IMAGE138
北向速度误 差观测值，
Figure 277738DEST_PATH_IMAGE139
、
Figure 264149DEST_PATH_IMAGE140
、
Figure 632813DEST_PATH_IMAGE141
、
Figure 730082DEST_PATH_IMAGE142
分别为辅助惯性导航结果经度、纬度、东向速度、北向速度，
Figure 828488DEST_PATH_IMAGE143
、
Figure 985800DEST_PATH_IMAGE144
、
Figure 841760DEST_PATH_IMAGE145
、
Figure 742720DEST_PATH_IMAGE146
分别为主惯性导航结果中的经度、纬度、东向速度、北向速度，
Figure 695633DEST_PATH_IMAGE147
、
Figure 23846DEST_PATH_IMAGE148
、
Figure 367103DEST_PATH_IMAGE149
为姿态 误差角度参量，T为矩阵转置。
在具体实施过程中，
Figure 809104DEST_PATH_IMAGE150
、
Figure 616523DEST_PATH_IMAGE151
、
Figure 115637DEST_PATH_IMAGE152
具体为从坐标系
Figure 946190DEST_PATH_IMAGE153
转动到坐标系
Figure 188953DEST_PATH_IMAGE154
的小角度，由
Figure 585299DEST_PATH_IMAGE155
、
Figure 520894DEST_PATH_IMAGE156
的值计算得到：
Figure 635480DEST_PATH_IMAGE157
其中，I为单位矩阵。
步骤2.1中，辅助惯性测量单元的误差状态模型和误差观测模型为：
Figure 885196DEST_PATH_IMAGE158
Figure 136049DEST_PATH_IMAGE159
其中，X为辅助惯性测量单元的状态误差参量，A为状态矩阵，H为观测矩阵。
状态误差参量X为：
Figure 242545DEST_PATH_IMAGE160
上述状态误差参量X中各个分分量分别为辅助惯性测量单元的经度误差
Figure 844428DEST_PATH_IMAGE161
、纬度 误差
Figure 428993DEST_PATH_IMAGE162
、东速误差
Figure 799931DEST_PATH_IMAGE163
、北速误差
Figure 811750DEST_PATH_IMAGE164
、x方向姿态误差角
Figure 104191DEST_PATH_IMAGE165
、y方向姿态误差角
Figure 226868DEST_PATH_IMAGE166
、z方 向姿态误差角
Figure 717892DEST_PATH_IMAGE167
、陀螺x漂移
Figure 166191DEST_PATH_IMAGE168
、陀螺y漂移
Figure 680349DEST_PATH_IMAGE169
、陀螺z漂移
Figure 603787DEST_PATH_IMAGE170
、加速度计x漂移
Figure 949317DEST_PATH_IMAGE171
、加速 度计y漂移
Figure 568518DEST_PATH_IMAGE172
、加速度计z漂移
Figure 569972DEST_PATH_IMAGE173
。
状态矩阵A由惯性导航系统的位置、速度、姿态误差方程扩展得到。当导航解算采用东北天地理坐标系的指北方位系统时，状态矩阵A可以表示为：
Figure 300030DEST_PATH_IMAGE174
其中，
Figure 500067DEST_PATH_IMAGE175
表示
Figure 290169DEST_PATH_IMAGE176
阶零矩阵，其它矩阵元
Figure 575657DEST_PATH_IMAGE177
的表达式为：
Figure 843827DEST_PATH_IMAGE178
Figure 101633DEST_PATH_IMAGE179
Figure 62636DEST_PATH_IMAGE180
Figure 100999DEST_PATH_IMAGE181
Figure 110543DEST_PATH_IMAGE182
Figure 285173DEST_PATH_IMAGE183
Figure 151497DEST_PATH_IMAGE184
Figure 677157DEST_PATH_IMAGE185
Figure 287130DEST_PATH_IMAGE186
Figure 519528DEST_PATH_IMAGE187
其中，
Figure 822333DEST_PATH_IMAGE188
为地球自转角速度，
Figure 569709DEST_PATH_IMAGE189
、
Figure 986303DEST_PATH_IMAGE190
分别为主惯性测量单元惯性导航计算出的 地球东向曲率半径、地球北向曲率半径，
Figure 73208DEST_PATH_IMAGE191
、
Figure 546914DEST_PATH_IMAGE192
、
Figure 781586DEST_PATH_IMAGE193
分别为主惯性测量单元惯性导航计算 出的地理坐标系中东北天三个方向的绝对加速度或者力大小。
观测矩阵H则取为下列矩阵：
Figure 733362DEST_PATH_IMAGE194
，其中，H矩阵为
Figure 940352DEST_PATH_IMAGE195
阶矩阵， 由
Figure 584960DEST_PATH_IMAGE196
阶单位矩阵
Figure 306929DEST_PATH_IMAGE197
和
Figure 62395DEST_PATH_IMAGE198
阶零矩阵
Figure 655050DEST_PATH_IMAGE199
构成。
步骤2.2中，卡尔曼滤波递推计算的具体实施方式为：
建立线性离线系统的卡尔曼滤波递推方程，对辅助惯性测量单元的状态误差参量X进行实时递推计算。
本实施例中，卡尔曼滤波递推计算的步骤及公式如下：
状态一步预测：
Figure 673822DEST_PATH_IMAGE200
状态估计：
Figure 617507DEST_PATH_IMAGE201
滤波增益矩阵：
Figure 442244DEST_PATH_IMAGE202
一步预测误差方差阵：
Figure 358247DEST_PATH_IMAGE203
估计误差方差阵：
Figure 79079DEST_PATH_IMAGE204
其中，
Figure 510060DEST_PATH_IMAGE205
为系统第k步的n维状态向量，
Figure 872908DEST_PATH_IMAGE206
为系统的m维观测序列，
Figure 705735DEST_PATH_IMAGE207
为第k步n维状态向量的预测值，
Figure 800730DEST_PATH_IMAGE208
为系统第k-1步的n维状态向量，
Figure 719007DEST_PATH_IMAGE209
为滤波增益矩阵，
Figure 885546DEST_PATH_IMAGE210
为一步预测误差方差阵，
Figure 327808DEST_PATH_IMAGE211
为第k-1步的估计误差方差阵，
Figure 859284DEST_PATH_IMAGE212
为第k步的估计误差方差 阵，I为单位矩阵，
Figure 999278DEST_PATH_IMAGE213
为
Figure 703929DEST_PATH_IMAGE214
维观测矩阵，
Figure 511348DEST_PATH_IMAGE215
为
Figure 213725DEST_PATH_IMAGE216
维噪声输入矩阵，
Figure 841015DEST_PATH_IMAGE217
为系统 过程噪声
Figure 349357DEST_PATH_IMAGE218
的
Figure 745703DEST_PATH_IMAGE219
维对称非负定方差矩阵，
Figure 415719DEST_PATH_IMAGE220
为系统观测噪声
Figure 733568DEST_PATH_IMAGE221
的
Figure 514442DEST_PATH_IMAGE222
维对称 正定方差阵，
Figure 296453DEST_PATH_IMAGE223
为系统的
Figure 137370DEST_PATH_IMAGE224
维状态转移矩阵，其计算方法是：
Figure 739253DEST_PATH_IMAGE225
其中，
Figure 527080DEST_PATH_IMAGE226
为递推计算的时间间隔。
设主惯性测量单元的旋转调制周期为
Figure 898019DEST_PATH_IMAGE227
，从三轴旋转式惯导系统的三轴转动机 构处于零位的时刻开始对辅助惯性测量单元的状态误差参量X进行卡尔曼滤波递推计算， 当计算时间T KF 为转动周期的整数倍时(即
Figure 175416DEST_PATH_IMAGE228
)，辅助惯性测量单元的 状态误差参量最优估计值
Figure 999016DEST_PATH_IMAGE229
为：
Figure 590534DEST_PATH_IMAGE230
其中，
Figure 815979DEST_PATH_IMAGE231
、
Figure 1629DEST_PATH_IMAGE232
、
Figure 578103DEST_PATH_IMAGE233
、
Figure 770050DEST_PATH_IMAGE234
为辅助惯性测量单元的经度、纬度、东向速度、北向速度 误差参量最优估计值，
Figure 850002DEST_PATH_IMAGE235
、
Figure 672464DEST_PATH_IMAGE236
、
Figure 470656DEST_PATH_IMAGE237
为辅助惯性测量单元的姿态角度误差参量最优估计值，
Figure 200715DEST_PATH_IMAGE238
、
Figure 135173DEST_PATH_IMAGE239
、
Figure 190853DEST_PATH_IMAGE240
为辅助惯性测量单元的陀螺漂移误差参量最优估计值，
Figure 476341DEST_PATH_IMAGE241
、
Figure 947774DEST_PATH_IMAGE242
、
Figure 2317DEST_PATH_IMAGE243
为辅助惯性 测量单元的加速度计漂移误差参量最优估计值。
步骤2.3中，对辅助惯性测量单元的惯性导航误差进行校正的具体实施方式为：根 据状态参量X的最优估计值
Figure 963320DEST_PATH_IMAGE244
，对辅助惯性测量单元的位置误差、速度误差、姿态矩阵误差、 陀螺漂移误差、加速度计漂移误差进行修正。
经度
Figure 470525DEST_PATH_IMAGE245
、纬度
Figure 807965DEST_PATH_IMAGE246
、东向速度
Figure 982595DEST_PATH_IMAGE247
、北向速度
Figure 52182DEST_PATH_IMAGE248
的修正计算方法如下：
Figure 312262DEST_PATH_IMAGE249
Figure 187814DEST_PATH_IMAGE250
Figure 214020DEST_PATH_IMAGE251
Figure 47984DEST_PATH_IMAGE252
其中，
Figure 795360DEST_PATH_IMAGE253
、
Figure 474603DEST_PATH_IMAGE254
、
Figure 358246DEST_PATH_IMAGE255
、
Figure 35215DEST_PATH_IMAGE256
为修正后的经度、纬度、东向速度、北向速度；
姿态矩阵的修正方法是根据
Figure 4308DEST_PATH_IMAGE257
、
Figure 221662DEST_PATH_IMAGE258
、
Figure 225390DEST_PATH_IMAGE259
计算出到姿态误差矩阵
Figure 807681DEST_PATH_IMAGE260
的估计值，为：
Figure 264071DEST_PATH_IMAGE261
然后根据姿态误差矩阵
Figure 19537DEST_PATH_IMAGE262
对包含有姿态误差的辅助惯性测量单元姿态矩阵
Figure 143351DEST_PATH_IMAGE263
进行修正，得到修正后的辅助惯性测量单元姿态矩阵
Figure 958860DEST_PATH_IMAGE264
，为：
Figure 105808DEST_PATH_IMAGE265
从而得到修正误差后辅助惯性测量单元的姿态矩阵
Figure 399386DEST_PATH_IMAGE266
，用
Figure 377706DEST_PATH_IMAGE267
替代辅助惯性测量 单元导航解算中的姿态矩阵
Figure 367046DEST_PATH_IMAGE266
，即完成了姿态误差矩阵的修正。
步骤3，基于校正后的辅助惯性测量单元得到载体姿态，并将载体姿态与主惯性导航结果中的位置信息、速度信息作为导航结果输出。
本实施例中，基于校正后的辅助惯性测量单元得到载体姿态的具体实施方式为：
步骤3.1，基于校正后的辅助惯性测量单元的姿态矩阵，得到载体的姿态矩阵：
利用辅助惯性测量单元的姿态矩阵
Figure 798028DEST_PATH_IMAGE266
，结合三轴旋转式惯导系统的三轴转动机 构外环轴的转动角度
Figure 98559DEST_PATH_IMAGE268
，到载体的姿态矩阵
Figure 665806DEST_PATH_IMAGE269
，为：
Figure 88698DEST_PATH_IMAGE270
其中，
Figure 6975DEST_PATH_IMAGE271
为从载体坐标系到辅助惯性测量单元测量坐标系之间的变换矩阵，坐标 系
Figure 845618DEST_PATH_IMAGE272
绕
Figure 532951DEST_PATH_IMAGE273
轴转动
Figure 861164DEST_PATH_IMAGE274
得到
Figure 266738DEST_PATH_IMAGE275
，则
Figure 971389DEST_PATH_IMAGE276
可以表示为：
Figure 513229DEST_PATH_IMAGE277
步骤3.2，基于载体的姿态矩阵，解算得到载体的三个姿态角，定义从坐标系
Figure 215605DEST_PATH_IMAGE278
先绕
Figure 842896DEST_PATH_IMAGE279
轴转动角度
Figure 85658DEST_PATH_IMAGE280
，再绕
Figure 747584DEST_PATH_IMAGE281
轴转动角度
Figure 620862DEST_PATH_IMAGE282
，最后绕
Figure 735448DEST_PATH_IMAGE283
转动角度
Figure 781902DEST_PATH_IMAGE284
，最后得 到坐标系
Figure 298334DEST_PATH_IMAGE285
，三个姿态角别为：
Figure 342513DEST_PATH_IMAGE286
Figure 941466DEST_PATH_IMAGE287
Figure 526031DEST_PATH_IMAGE288
其中，
Figure 162549DEST_PATH_IMAGE289
为纵摇角，
Figure 174367DEST_PATH_IMAGE290
为横摇角，
Figure 201229DEST_PATH_IMAGE291
为航向角，
Figure 589485DEST_PATH_IMAGE292
为姿态矩阵
Figure 80509DEST_PATH_IMAGE293
的第3行、第2 列得值，
Figure 263229DEST_PATH_IMAGE294
为姿态矩阵
Figure 42966DEST_PATH_IMAGE295
的第3行、第1列得值，
Figure 969334DEST_PATH_IMAGE296
为姿态矩阵
Figure 314865DEST_PATH_IMAGE297
的第3行、第3列得值，
Figure 934065DEST_PATH_IMAGE298
为姿态矩阵
Figure 935519DEST_PATH_IMAGE299
的第1行、第2列得值，
Figure 399998DEST_PATH_IMAGE300
为姿态矩阵
Figure 600035DEST_PATH_IMAGE301
的第2行、第2列得值。
将
Figure 655716DEST_PATH_IMAGE302
、
Figure 675625DEST_PATH_IMAGE303
、
Figure 412637DEST_PATH_IMAGE304
替代只利用主惯性测量单元导航解算得到的载体姿态角
Figure 467180DEST_PATH_IMAGE305
、
Figure 428183DEST_PATH_IMAGE306
、
Figure 404229DEST_PATH_IMAGE307
，即实现了惯导输出姿态中的旋转调制姿态波动消除。导航系统的位置、速度输出可以 一直按照主惯性测量单元的导航计算结果经度
Figure 479020DEST_PATH_IMAGE308
、纬度
Figure 388070DEST_PATH_IMAGE309
、高度
Figure 785554DEST_PATH_IMAGE310
、东向速度
Figure 780054DEST_PATH_IMAGE311
、北向速度
Figure 858869DEST_PATH_IMAGE312
、天向速度
Figure 888005DEST_PATH_IMAGE313
进行输出。
需要注意的是，在辅助惯性测量单元的第一次卡尔曼滤波校正完成后，导航系统 的姿态输出需要保持一直由辅助惯性测量单元计算得到的载体姿态姿态角
Figure 190810DEST_PATH_IMAGE314
、
Figure 938186DEST_PATH_IMAGE315
、
Figure 351850DEST_PATH_IMAGE316
进行实时输出，并同时按照上述步骤对辅助惯性测量单元开始新一次的卡尔曼滤波误差估 算和校正，并且在每一个滤波周期T KF 完成后，对辅助惯性测量单元的姿态矩阵
Figure 438755DEST_PATH_IMAGE317
进行修 正，从而保证姿态角
Figure 912461DEST_PATH_IMAGE318
、
Figure 881554DEST_PATH_IMAGE319
、
Figure 302171DEST_PATH_IMAGE320
的长期精度。
下面结合具体的仿真示例对本实施例中的导航方法作出进一步的说明。
系统仿真设置经度为112.99°，纬度为28.22°，高度为0；主惯性测量单元的陀螺漂 移0.003°/h，加速度计漂移10μg；辅助惯性测量单元的陀螺漂移0.01°/h，加速度计漂移30μ g；内环轴、中环轴的转动角度误差分别设定为
Figure 40320DEST_PATH_IMAGE321
角秒、
Figure 684928DEST_PATH_IMAGE322
角秒，其中
Figure 406897DEST_PATH_IMAGE323
、
Figure 162363DEST_PATH_IMAGE324
分别为内环轴转角、中环轴转角；系统的内环轴、中环轴采用的是16次序旋转调制方案，旋 转调制周期为400秒，辅助惯性测量单元的卡尔曼滤波校正周期为800秒。
采用传统三轴旋转式惯导系统的导航方法，只利用主惯性测量单元导航解算载体姿态角，得到4000秒内系统输出姿态角误差情况如图4所示。由图4可见，由于轴系转动角度等15个角秒的误差，导致传统方法的输出姿态角（航向角、纵摇角、横摇角）中存在着幅度为15角秒的姿态波动，这将对系统的输出姿态稳定性造成较大影响。
本实施例的导航方法中，新增加的辅助惯性测量单元，如果不与主惯性测量单元进行组合导航滤波，得到4000秒内的纯惯性输出姿态角误差情况如图5所示。由图5可见，由于辅助惯性测量单元的精度一般，并且没有误差旋转调制，其解算得到的载体姿态角虽然没有旋转调制引起的波动，但长期误差较大，特别是4000秒后航向角误差达到20角秒，并且航向角误差还会随时间不断的发散增大。
采用本实施例的导航方法，对辅助惯性测量单元与主惯性测量单元进行组合导航处理，得到4000秒内系统输出载体姿态角误差情况如图6所示。由于卡尔曼滤波校正周期为800秒，因此在开始的800秒内，输出的姿态其实仍是辅助惯性测量单元的纯惯性姿态角数据，在800秒时进行了第1次滤波校正，系统输出姿态波动误差明显降低，在1600秒时进行了第2次滤波校正后，一直到4000秒之间输出姿态误差波动显著降低，其中航向角误差波动为-1.2角秒，纵摇角误差最大波动为0.13角秒，横摇角误差最大波动为-0.36角秒。在2400、3200、4000秒时对辅助惯性测量单元分别进行了第3、4、5次滤波校正，以此类推下去，只要维持辅助惯性测量单元滤波校正的不断进行，从第2次滤波校正后系统输出姿态误差波动就能够一直维持在较小的水平。
从图4至图6可以看出，采用本实施例的导航方法能够将有效抑制由旋转调制引起的姿态误差波动，从而达到消除系统调制姿态波动、提升系统姿态精度的目的。
总之，得益于三轴旋转式惯导系统的旋转调制作用，主惯性测量单元的长期导航精度较高，但一个旋转调制周期内的短期姿态波动较大。由于辅助惯性测量单元不参与空间翻转以及外环轴对辅助惯性测量单元的航向隔离作用，辅助惯性测量单元的短期导航结果稳定性较高，但是长期导航精度较差。因此通过增加辅助惯性测量单元并实施上述导航处理方法，可以很好地将主惯性导航单元与辅助惯性测量单元的惯性导航信息进行深度融合，最终达到消除系统调制姿态波动、提升系统姿态精度的目的。同时，导航系统的位置、速度输出可以直接采用主惯性测量单元的导航计算结果，因此本实施例的导航方法不改变三轴旋转式惯导系统的位置、速度精度。
以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。

Claims (10)

1.一种基于三轴旋转式惯导系统的导航方法，其特征在于，在载体上搭载基于三轴旋转式惯导系统的导航系统，所述导航系统包括外环转动框架、中环转动框架、内环转动框架与主惯性测量单元，并在外环转动框架上安装辅助惯性测量单元；
所述导航方法包括如下步骤：
步骤1，对所述主惯性测量单元、所述辅助惯性测量单元的惯性测量数据分别进行实时惯性导航解算，得到主惯性导航结果与辅助惯性导航结果；
步骤2，基于所述主惯性导航结果对所述辅助惯性测量单元的惯性导航误差进行卡尔曼滤波最优估算并定期校正；
步骤3，基于校正后的所述辅助惯性测量单元得到载体姿态，并将所述载体姿态与所述主惯性导航结果中的位置信息、速度信息作为导航结果输出。
2.根据权利要求1所述的基于三轴旋转式惯导系统的导航方法，其特征在于，步骤2具体为：
步骤2.1，以所述主惯性导航结果为基准值，得到所述辅助惯性测量单元的导航误差观测矢量，并建立所述辅助惯性测量单元的误差状态模型和误差观测模型；
步骤2.2，根据所述导航误差观测矢量，对所述辅助惯性测量单元的状态误差参量进行实时卡尔曼滤波递推计算，并在进行所述卡尔曼滤波递推计算时间T KF 后，得到所述辅助惯性测量单元的状态误差参量最优估计值，其中，T KF 为所述主惯性测量单元旋转调制周期的整数倍；
步骤2.3，根据所述辅助惯性测量单元的状态误差参量最优估计值，对所述辅助惯性测量单元的惯性导航误差进行校正；
步骤2.4，完成上述步骤2.1至步骤2.3后，重新按照步骤2.1至步骤2.3的方法以T KF 时间周期不断循环进行辅助惯性测量单元的误差校正。
3.根据权利要求2所述的基于三轴旋转式惯导系统的导航方法，其特征在于，步骤2.1中，所述导航误差观测矢量，具体为：
Figure 598812DEST_PATH_IMAGE001
其中，Z为导航误差观测矢量，
Figure 47111DEST_PATH_IMAGE002
为经度误差观测值，
Figure 358007DEST_PATH_IMAGE003
为纬度误差 观测值，
Figure 487637DEST_PATH_IMAGE004
为东向速度误差观测值，
Figure 833167DEST_PATH_IMAGE005
北向速度误差观测值，
Figure 186788DEST_PATH_IMAGE006
、
Figure 984980DEST_PATH_IMAGE007
、
Figure 715039DEST_PATH_IMAGE008
、
Figure 180655DEST_PATH_IMAGE009
分别为所述辅助惯性导航结果经度、纬度、东向速度、北向速度，
Figure 708107DEST_PATH_IMAGE010
、
Figure 993595DEST_PATH_IMAGE011
、
Figure 261765DEST_PATH_IMAGE012
、
Figure 519571DEST_PATH_IMAGE013
分别为所述主惯性导航结果中的经度、纬度、东向速度、北向速度，
Figure 480574DEST_PATH_IMAGE014
、
Figure 518937DEST_PATH_IMAGE015
、
Figure 325219DEST_PATH_IMAGE016
为姿态误差角 度参量，T为矩阵转置；
所述误差状态模型和所述误差观测模型为：
Figure 499849DEST_PATH_IMAGE017
Figure 366173DEST_PATH_IMAGE018
其中，X为所述辅助惯性测量单元的状态误差参量，A为状态矩阵，H为观测矩阵。
4.根据权利要求3所述的基于三轴旋转式惯导系统的导航方法，其特征在于，步骤2.2中，所述卡尔曼滤波递推计算的具体过程为：
状态一步预测：
Figure 829516DEST_PATH_IMAGE019
状态估计：
Figure 439489DEST_PATH_IMAGE020
滤波增益矩阵：
Figure 734204DEST_PATH_IMAGE021
一步预测误差方差阵：
Figure 37009DEST_PATH_IMAGE022
估计误差方差阵：
Figure 518806DEST_PATH_IMAGE023
其中，
Figure 932470DEST_PATH_IMAGE024
为系统第k步的n维状态向量，
Figure 19375DEST_PATH_IMAGE025
为系统的m维观测序列，
Figure 493081DEST_PATH_IMAGE026
为第k步状态 向量的预测值，
Figure 727753DEST_PATH_IMAGE027
为系统第k-1步的n维状态向量，
Figure 679529DEST_PATH_IMAGE028
为滤波增益矩阵，
Figure 680327DEST_PATH_IMAGE029
为一步预 测误差方差阵，
Figure 324935DEST_PATH_IMAGE030
为第k-1步的估计误差方差阵，
Figure 984587DEST_PATH_IMAGE031
为第k步的估计误差方差阵，I为单位 矩阵，
Figure 740053DEST_PATH_IMAGE032
为
Figure 332709DEST_PATH_IMAGE033
维观测矩阵，
Figure 148218DEST_PATH_IMAGE034
为
Figure 357482DEST_PATH_IMAGE035
维噪声输入矩阵，
Figure 916640DEST_PATH_IMAGE036
为系统过程噪声
Figure 629381DEST_PATH_IMAGE037
的
Figure 553474DEST_PATH_IMAGE038
维对称非负定方差矩阵，
Figure 984456DEST_PATH_IMAGE039
为系统观测噪声
Figure 81725DEST_PATH_IMAGE040
的
Figure 180131DEST_PATH_IMAGE041
维对称正定方差 阵，
Figure 71863DEST_PATH_IMAGE042
为系统的
Figure 990141DEST_PATH_IMAGE043
维状态转移矩阵，
Figure 94363DEST_PATH_IMAGE044
根据状态矩阵A计算得到；
设所述主惯性测量单元的旋转调制周期为
Figure 516117DEST_PATH_IMAGE045
，从所述三轴旋转式惯导系统的三轴转 动机构处于零位的时刻开始对所述辅助惯性测量单元的状态误差参量X进行卡尔曼滤波递 推计算，当计算时间T KF 为转动周期的整数倍时，所述辅助惯性测量单元的状态误差参量最 优估计值
Figure 844330DEST_PATH_IMAGE046
为：
Figure 249904DEST_PATH_IMAGE047
其中，
Figure 957484DEST_PATH_IMAGE048
、
Figure 499324DEST_PATH_IMAGE049
、
Figure 201701DEST_PATH_IMAGE050
、
Figure 828991DEST_PATH_IMAGE051
为所述辅助惯性测量单元的经度、纬度、东向速度、北向速 度误差参量最优估计值，
Figure 71754DEST_PATH_IMAGE052
、
Figure 733679DEST_PATH_IMAGE053
、
Figure 403695DEST_PATH_IMAGE054
为所述辅助惯性测量单元的姿态角度误差参量最优估 计值，
Figure 518282DEST_PATH_IMAGE055
、
Figure 299156DEST_PATH_IMAGE056
、
Figure 18850DEST_PATH_IMAGE057
为所述辅助惯性测量单元的陀螺漂移误差参量最优估计值，
Figure 859767DEST_PATH_IMAGE058
、
Figure 461650DEST_PATH_IMAGE059
、
Figure 311794DEST_PATH_IMAGE060
为所述辅助惯性测量单元的加速度计漂移误差参量最优估计值。
5.根据权利要求2至4任一项所述的基于三轴旋转式惯导系统的导航方法，其特征在于，步骤3中，所述基于校正后的所述辅助惯性测量单元得到载体姿态，具体为：
步骤3.1，基于校正后的所述辅助惯性测量单元的姿态矩阵，得到载体的姿态矩阵；
步骤3.2，基于载体的姿态矩阵，解算得到载体的三个姿态角。
6.根据权利要求5所述的基于三轴旋转式惯导系统的导航方法，其特征在于，步骤3.1具体为：
首先，基于所述辅助惯性测量单元的状态误差参量最优估计值中的姿态角度误差参量 最优估计值
Figure 682733DEST_PATH_IMAGE061
、
Figure 694551DEST_PATH_IMAGE062
、
Figure 518150DEST_PATH_IMAGE063
，得到姿态误差矩阵
Figure 109669DEST_PATH_IMAGE064
，为：
Figure 335114DEST_PATH_IMAGE065
其次，基于姿态误差矩阵
Figure 783413DEST_PATH_IMAGE066
对包含有姿态误差的辅助惯性测量单元姿态矩阵
Figure 356958DEST_PATH_IMAGE067
进 行修正，得到修正后的辅助惯性测量单元姿态矩阵
Figure 283325DEST_PATH_IMAGE068
，为：
Figure 363277DEST_PATH_IMAGE069
最后，利用辅助惯性测量单元的姿态矩阵
Figure 185739DEST_PATH_IMAGE070
，结合所述三轴旋转式惯导系统的三轴转 动机构外环轴的转动角度
Figure 983931DEST_PATH_IMAGE071
，到载体的姿态矩阵
Figure 713990DEST_PATH_IMAGE072
，为：
Figure 914027DEST_PATH_IMAGE073
其中，
Figure 704128DEST_PATH_IMAGE074
为从载体坐标系到辅助惯性测量单元测量坐标系之间的变换矩阵。
7.根据权利要求6所述的基于三轴旋转式惯导系统的导航方法，其特征在于，在所述辅助惯性测量单元的惯性导航误差经过第一次校正后，步骤3中所述载体姿态保持由所述辅助惯性测量单元的姿态矩阵计算得到并实时输出，同时辅助惯性测量单元的姿态矩阵误差以周期T KF 进行定期校正，从而保证所述载体姿态输出的长期精度。
8.根据权利要求6所述的基于三轴旋转式惯导系统的导航方法，其特征在于，步骤3.2中，载体的三个姿态角分别为：
Figure 989616DEST_PATH_IMAGE075
Figure 257787DEST_PATH_IMAGE076
Figure 515593DEST_PATH_IMAGE077
其中，
Figure 476595DEST_PATH_IMAGE078
为纵摇角，
Figure 983800DEST_PATH_IMAGE079
为横摇角，
Figure 55661DEST_PATH_IMAGE080
为航向角，
Figure 495870DEST_PATH_IMAGE081
为姿态矩阵
Figure 362195DEST_PATH_IMAGE082
的第3行、第2列得 值，
Figure 825537DEST_PATH_IMAGE083
为姿态矩阵
Figure 435510DEST_PATH_IMAGE084
的第3行、第1列得值，
Figure 464646DEST_PATH_IMAGE085
为姿态矩阵
Figure 504802DEST_PATH_IMAGE086
的第3行、第3列得值，
Figure 252178DEST_PATH_IMAGE087
为姿态矩阵
Figure 931421DEST_PATH_IMAGE088
的第1行、第2列得值，
Figure 815063DEST_PATH_IMAGE089
为姿态矩阵
Figure 288770DEST_PATH_IMAGE090
的第2行、第2列得值。
9.一种基于三轴旋转式惯导系统的导航系统，其特征在于，包括：
三轴旋转式惯导系统，包括基座、外环转动框架、中环转动框架、内环转动框架与主惯性测量单元，所述主惯性测量单元固联安装在所述内环转动框架上，所述主惯性测量单元包括3个主陀螺仪和3个主加速度计；
辅助惯性测量单元，包括设在所述外环转动框架上的3个辅助陀螺仪和3个辅助加速度计
导航计算单元，与所述主惯性测量单元、所述辅助惯性测量单元电连接，用于根据权利要求1至8任一项所述的导航方法的部分或全部步骤进行惯性导航。
10.根据权利要求9所述的基于三轴旋转式惯导系统的导航系统，其特征在于，所述辅助陀螺仪为能够与所述外环转动框架刚性固连的全固态陀螺仪。
CN202211569096.4A 2022-12-08 2022-12-08 一种基于三轴旋转式惯导系统的导航方法及导航系统 Active CN115574817B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211569096.4A CN115574817B (zh) 2022-12-08 2022-12-08 一种基于三轴旋转式惯导系统的导航方法及导航系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211569096.4A CN115574817B (zh) 2022-12-08 2022-12-08 一种基于三轴旋转式惯导系统的导航方法及导航系统

Publications (2)

Publication Number Publication Date
CN115574817A true CN115574817A (zh) 2023-01-06
CN115574817B CN115574817B (zh) 2023-03-31

Family

ID=84590396

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211569096.4A Active CN115574817B (zh) 2022-12-08 2022-12-08 一种基于三轴旋转式惯导系统的导航方法及导航系统

Country Status (1)

Country Link
CN (1) CN115574817B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116163882A (zh) * 2023-04-24 2023-05-26 西安中科原子精密制造科技有限公司 一种基于三轴姿态传感器的风机净空距离调整方法及系统

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050022402A1 (en) * 2003-08-01 2005-02-03 Ash Michael E. Compact navigation system and method
JP2007155584A (ja) * 2005-12-07 2007-06-21 Japan Agengy For Marine-Earth Science & Technology 慣性航法システム
CN106767900A (zh) * 2016-11-23 2017-05-31 东南大学 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法
CN108106635A (zh) * 2017-12-15 2018-06-01 中国船舶重工集团公司第七0七研究所 惯性卫导组合导航系统的长航时抗干扰姿态航向校准方法
CN109211269A (zh) * 2018-08-16 2019-01-15 昆山天地睿航智能科技有限公司 一种双轴旋转惯导系统姿态角误差标定方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN111351508A (zh) * 2020-04-22 2020-06-30 中北大学 一种mems惯性测量单元系统级批量标定方法
CN112179340A (zh) * 2020-10-10 2021-01-05 中国人民解放军战略支援部队航天工程大学 一种冗余配置惯性测量单元双轴旋转调制方法
WO2021227012A1 (zh) * 2020-05-11 2021-11-18 中国科学院地质与地球物理研究所 一种姿态测量方法
CN114061572A (zh) * 2021-11-16 2022-02-18 北京航空航天大学 一种用于旋转式惯导系统的双轴旋转调制方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050022402A1 (en) * 2003-08-01 2005-02-03 Ash Michael E. Compact navigation system and method
JP2007155584A (ja) * 2005-12-07 2007-06-21 Japan Agengy For Marine-Earth Science & Technology 慣性航法システム
CN106767900A (zh) * 2016-11-23 2017-05-31 东南大学 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法
CN108106635A (zh) * 2017-12-15 2018-06-01 中国船舶重工集团公司第七0七研究所 惯性卫导组合导航系统的长航时抗干扰姿态航向校准方法
CN109211269A (zh) * 2018-08-16 2019-01-15 昆山天地睿航智能科技有限公司 一种双轴旋转惯导系统姿态角误差标定方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN111351508A (zh) * 2020-04-22 2020-06-30 中北大学 一种mems惯性测量单元系统级批量标定方法
WO2021227012A1 (zh) * 2020-05-11 2021-11-18 中国科学院地质与地球物理研究所 一种姿态测量方法
CN112179340A (zh) * 2020-10-10 2021-01-05 中国人民解放军战略支援部队航天工程大学 一种冗余配置惯性测量单元双轴旋转调制方法
CN114061572A (zh) * 2021-11-16 2022-02-18 北京航空航天大学 一种用于旋转式惯导系统的双轴旋转调制方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
UMAR QURESHI 等: "An Algorithm for the In-Field Calibration of a MEMS IMU", 《IEEE SENSORS JOURNAL》 *
崔加瑞 等: "双惯导联合旋转调制光纤陀螺标度因数误差自校正方法", 《中国惯性技术学报》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116163882A (zh) * 2023-04-24 2023-05-26 西安中科原子精密制造科技有限公司 一种基于三轴姿态传感器的风机净空距离调整方法及系统

Also Published As

Publication number Publication date
CN115574817B (zh) 2023-03-31

Similar Documents

Publication Publication Date Title
CN108180925B (zh) 一种里程计辅助车载动态对准方法
CN110207697B (zh) 基于角加速度计/陀螺/加速度计的惯性导航解算方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN102749079B (zh) 一种光纤捷联惯导双轴旋转调制方法及双轴旋转机构
CN109211269B (zh) 一种双轴旋转惯导系统姿态角误差标定方法
CN110926468B (zh) 基于传递对准的动中通天线多平台航姿确定方法
CN103245360A (zh) 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN102589546B (zh) 一种抑制器件斜坡误差影响的光纤捷联惯组往复式两位置寻北方法
CN110243377B (zh) 一种基于分层式结构的集群飞行器协同导航方法
CN108592946B (zh) 一种基于两套旋转惯导冗余配置下的惯性器件漂移在线监控方法
CN109752000A (zh) 一种mems双轴旋转调制型捷联罗经初始对准方法
CN113503892B (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN114413895B (zh) 光纤陀螺旋转惯导联合定位方法、装置、设备及介质
CN110954102A (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN110058288A (zh) 无人机ins/gnss组合导航系统航向误差修正方法及系统
CN103630146A (zh) 一种离散解析与Kalman滤波结合的激光陀螺IMU标定方法
CN115574817B (zh) 一种基于三轴旋转式惯导系统的导航方法及导航系统
CN109084756B (zh) 一种重力视运动参数辨识与加速度计零偏分离方法
CN112697143B (zh) 高精度载体动态姿态测量方法及系统
CN113959462A (zh) 一种基于四元数的惯性导航系统自对准方法
CN110488853B (zh) 一种降低转轴涡动影响的混合式惯导系统稳定控制指令的计算方法
CN112033438A (zh) 一种基于速度拟合的晃动基座自对准方法
CN111207734A (zh) 一种基于ekf的无人机组合导航方法
CN111964671B (zh) 一种基于双轴旋转调制的惯性天文组合导航系统及方法

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