CN115727875A - 一种基于修正罗德里格斯参数的无奇异传递对准方法 - Google Patents

一种基于修正罗德里格斯参数的无奇异传递对准方法 Download PDF

Info

Publication number
CN115727875A
CN115727875A CN202211518553.7A CN202211518553A CN115727875A CN 115727875 A CN115727875 A CN 115727875A CN 202211518553 A CN202211518553 A CN 202211518553A CN 115727875 A CN115727875 A CN 115727875A
Authority
CN
China
Prior art keywords
inertial navigation
navigation system
coordinate system
matrix
sub
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
CN202211518553.7A
Other languages
English (en)
Other versions
CN115727875B (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.)
Harbin University of Science and Technology
Original Assignee
Harbin University of Science and 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 Harbin University of Science and Technology filed Critical Harbin University of Science and Technology
Priority to CN202211518553.7A priority Critical patent/CN115727875B/zh
Publication of CN115727875A publication Critical patent/CN115727875A/zh
Application granted granted Critical
Publication of CN115727875B publication Critical patent/CN115727875B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Navigation (AREA)

Abstract

一种基于修正罗德里格斯参数的无奇异传递对准方法，涉及捷联惯性导航系统的初始对准技术领域。本发明是为了解决现有的惯性导航系统传递对准方法不能同时实现对杆臂误差的有效估计以及保证对准模型的无奇异性的问题。本发明对子惯性导航系统的姿态矩阵进行链式分解，从中提取出未知姿态矩阵；充分考虑主、子惯性导航系统之间存在的杆臂误差，利用主、子惯性导航系统输出的角速度测量值和比力测量值构建传递对准模型；采用修正罗德里格斯参数等价表示未知姿态矩阵，在保证模型无奇异的同时，降低模型维数；采用基本的扩展卡尔曼滤波进行状态估计，利用估计出的修正罗德里格斯参数和误差角计算相关姿态矩阵，完成传递对准。

Description

一种基于修正罗德里格斯参数的无奇异传递对准方法
技术领域
本发明属于捷联惯性导航系统的初始对准技术领域。
背景技术
惯性导航系统(INS)是利用线运动传感器(加速度计)测量的比力信息和角运动传感器(陀螺仪)测量的角速度信息，通过导航解算，连续得到载体的位置、速度和姿态信息的一种导航方式。初始对准作为惯性导航系统的一个重要研究内容，不仅是组合导航模型线性化的重要保证，也是决定纯惯导导航精度的关键因素。而传递对准则是一种特殊的动基座初始对准技术，其利用高精度主惯性导航系统传递的导航信息，使低精度子惯性导航系统可以在较短时间内实现初始对准。相比于其他类型的初始对准方法，如卫星导航系统辅助(导航系速度或绝对位置)、多普勒计程仪辅助(载体系速度)、里程计辅助(载体系速度或相对位置)等，传递对准在对准速度和对准精度上都要有明显优势。
基于滤波的传递对准方法是传递对准领域的主流研究方向，而基于滤波的传递对准方法的关键在于构建合适的误差模型。现有的传递对准方法按照模型类型可以分为三种：φm角模型，φn角模型和惯性系传递对准模型。φm角模型和φn角模型是根据不同的失准角(即一种欧拉角)定义而建立的，由于欧拉角自身固有的转动问题以及在大安装误差角情况下一步装订操作导致的大失准角问题，导致φm角模型和φn角模型在实际应用过程中均会面临模型奇异点问题和非线性模型问题。
为了解决上述问题，在《SINS任意失准角无奇异快速传递对准》(发表于期刊《宇航学报》，2018年，第39卷，10期)一文中，提出了一种基于姿态矩阵的惯性系传递对准方法，并利用主惯性导航系统输出的测量参数(即角速度和比力)，通过积分运算获得相关模型参数，实现了任意安装误差角下的传递对准。虽然该方法利用姿态矩阵建立传递对准模型，有效解决了φm角模型和φn角模型中的奇异点问题和非线性模型问题，但是未考虑主、子惯性导航系统之间的杆臂误差，导致其对准精度会在存在杆臂误差的情况下严重下降。在《基于罗德里格斯参数的惯性系传递对准算法》(发表于期刊《系统工程与电子技术》，2022年，第44卷，9期)一文中，利用罗德里格斯参数建立带有杆臂误差的传递对准模型，有效解决了杆臂误差的影响。然而，在旋转角度为π时，罗德里格斯参数存在奇异，导致该模型在实际应用中需要着重考虑奇异点问题。
综上所述，现有的惯性导航系统传递对准方法不能同时实现对杆臂误差的有效估计以及保证对准模型的无奇异性。
发明内容
本发明是为了解决现有的惯性导航系统传递对准方法不能同时实现对杆臂误差的有效估计以及保证对准模型的无奇异性的问题，现提供一种基于修正罗德里格斯参数的无奇异传递对准方法。
一种基于修正罗德里格斯参数的无奇异传递对准方法，包括以下步骤：
步骤一：对t时刻子惯性导航系统体坐标系与导航坐标系之间的姿态矩阵
Figure BDA0003970972870000021
进行链式分解，提取未知项，
所述未知项为：初始时刻子惯性导航系统体坐标系与主惯性导航系统体坐标系之间的姿态矩阵
Figure BDA0003970972870000022
和子惯性导航系统体坐标系在初始时刻与t时刻之间的姿态矩阵
Figure BDA0003970972870000023
所述链式分解的表达式如下：
Figure BDA0003970972870000024
其中，
Figure BDA0003970972870000025
为t时刻主惯性导航系统体坐标系与导航坐标系之间的姿态矩阵，
Figure BDA0003970972870000026
为主惯性导航系统体坐标系在初始时刻与t时刻之间的姿态矩阵，(·)T表示矩阵的转置；
步骤二：将主惯性导航系统和子惯性导航系统的角速度测量值以及比力测量值分别投影在m0坐标系和s0坐标系上，并建立下式：
Figure BDA0003970972870000027
Figure BDA0003970972870000028
其中，
Figure BDA0003970972870000029
为投影在s0坐标系上的子惯性导航系统比力测量值，
Figure BDA00039709728700000210
为投影在m0坐标系上的主惯性导航系统比力测量值，
Figure BDA00039709728700000211
投影在s0坐标系上的子惯性导航系统角速度测量值，
Figure BDA00039709728700000212
为投影在m0坐标系上的主惯性导航系统角速度测量值，
Figure BDA00039709728700000213
为含有误差的
Figure BDA00039709728700000214
φs为
Figure BDA00039709728700000215
的误差角，
Figure BDA00039709728700000216
为子惯性导航系统中加速度计的测量误差值，
Figure BDA00039709728700000217
为子惯性导航系统中陀螺仪的测量误差值，
Figure BDA00039709728700000218
为主惯性导航系统的角速度测量值，
Figure BDA00039709728700000219
为
Figure BDA00039709728700000220
的一阶导数，(·×)表示反对称矩阵，rm为投影在主惯性导航系统体坐标系下的杆臂误差值，
所述m0坐标系和s0坐标系分别为主惯性导航系统体坐标系与子惯性导航系统体坐标系在对准初始时刻凝固得到的惯性坐标系；
步骤三：对公式二和公式三进行积分获得：
Figure BDA0003970972870000031
Figure BDA0003970972870000032
其中，
Figure BDA0003970972870000033
Figure BDA0003970972870000034
Figure BDA0003970972870000035
Figure BDA0003970972870000036
Figure BDA0003970972870000037
Figure BDA0003970972870000038
Figure BDA0003970972870000039
上式中，
Figure BDA00039709728700000310
和
Figure BDA00039709728700000311
分别为子惯性导航系统的比力测量值和角速度测量值；
步骤四：设
Figure BDA00039709728700000312
为姿态矩阵
Figure BDA00039709728700000313
对应的修正罗德里格斯参数，则姿态矩阵
Figure BDA00039709728700000314
能够表达如下：
Figure BDA00039709728700000315
其中，I3为3维单位矩阵；
步骤五：将公式六分别代入公式四和五，变换获得：
Figure BDA00039709728700000316
Figure BDA00039709728700000317
步骤六：定义状态向量
Figure BDA00039709728700000318
状态噪声向量
Figure BDA00039709728700000319
量测向量
Figure BDA00039709728700000320
以及量测噪声向量ν＝[νV；νΩ]，建立无奇异传递对准模型：
Figure BDA0003970972870000041
其中，
Figure BDA00039709728700000424
为x的一阶导数，F为状态转移矩阵，G为状态噪声驱动矩阵，εs为
Figure BDA0003970972870000042
中包含的陀螺仪零偏，
Figure BDA0003970972870000043
为
Figure BDA0003970972870000044
中包含的高斯白噪声，
Figure BDA0003970972870000045
为
Figure BDA00039709728700000425
中包含的加速度计零偏，
Figure BDA0003970972870000046
为
Figure BDA0003970972870000047
中包含的高斯白噪声，νV为速度量测噪声，νΩ为姿态量测噪声，
h(x)为非线性函数且表达式如下：
Figure BDA0003970972870000048
步骤七：采用扩展卡尔曼滤波对无奇异传递对准模型的状态向量进行估计，获得状态向量中的
Figure BDA0003970972870000049
和φs，并利用
Figure BDA00039709728700000410
和φs求解
Figure BDA00039709728700000411
和
Figure BDA00039709728700000412
步骤八：将
Figure BDA00039709728700000413
和
Figure BDA00039709728700000414
代入公式一，获得姿态矩阵
Figure BDA00039709728700000415
进而实现子惯性导航系统的传递对准。
进一步的，上述步骤一中，主惯性导航系统体坐标系初始时刻与t时刻之间的姿态矩阵
Figure BDA00039709728700000416
通过下式获得：
Figure BDA00039709728700000417
其中，
Figure BDA00039709728700000418
为
Figure BDA00039709728700000419
的一阶导数。
进一步的，上述步骤二中，将主惯性导航系统和子惯性导航系统的角速度测量值以及比力测量值分别投影在m0坐标系和s0坐标系上的具体方法为：
Figure BDA00039709728700000420
Figure BDA00039709728700000421
上式中，
Figure BDA00039709728700000422
为主惯性导航系统的比力测量值。
进一步的，上述步骤二中，φs满足下式：
Figure BDA00039709728700000423
其中，
Figure BDA0003970972870000051
为φs的一阶导数。
进一步的，上述步骤二中，
Figure BDA0003970972870000052
通过下式获得：
Figure BDA0003970972870000053
其中，
Figure BDA0003970972870000054
为
Figure BDA0003970972870000055
的一阶导数。
进一步的，上述步骤四中，修正罗德里格斯参数
Figure BDA0003970972870000056
中的元素个数为3，且在旋转角度为(-2π,2π)范围内不存在奇异点。
进一步的，上述步骤六中，状态转移矩阵F和状态噪声驱动矩阵G的表达式分别如下：
Figure BDA0003970972870000057
Figure BDA0003970972870000058
其中，0a×b为a×b维的零矩阵。
进一步的，上述步骤七中，将
Figure BDA0003970972870000059
代入公式六获得姿态矩阵
Figure BDA00039709728700000510
将φs代入下式获得
Figure BDA00039709728700000512
Figure BDA00039709728700000511
本发明具有如下有益效果：
1、本发明方法在惯性系下建立传递对准模型，实现一步式对准，可以避免传统方法存在的大安装误差角导致的强非线性模型问题。
2、本发明考虑主、子惯性导航系统之间杆臂误差的影响，从而提高了在存在杆臂误差情况下的传递对准精度。
3、本发明方法采用修正罗德里格斯参数等价表示姿态矩阵，不仅可以有效降低模型维数，而且可以避免罗德里格斯参数存在的奇异点问题，保证了模型的非奇异性。
附图说明
图1为本发明所述方法的流程图；
图2为本发明所述方法和现有方法的x轴安装误差角估计结果对比图；
图3为本发明所述方法和现有方法的y轴安装误差角估计结果对比图；
图4为本发明所述方法和现有方法的z轴安装误差角估计结果对比图；
图5为本发明所述方法和现有方法的杆臂误差估计结果对比图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其它实施例，都属于本发明保护的范围。需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
具体实施方式一：参照图1具体说明本实施方式，本实施方式所述的一种基于修正罗德里格斯参数的无奇异传递对准方法，包括以下步骤：
步骤一：对t时刻子惯性导航系统体坐标系与导航坐标系之间的姿态矩阵
Figure BDA0003970972870000061
进行链式分解，提取未知项，
所述未知项为：初始时刻子惯性导航系统体坐标系与主惯性导航系统体坐标系之间的姿态矩阵
Figure BDA0003970972870000062
和子惯性导航系统体坐标系在初始时刻与t时刻之间的姿态矩阵
Figure BDA0003970972870000063
所述链式分解的表达式如下：
Figure BDA0003970972870000064
其中，
Figure BDA0003970972870000065
为t时刻主惯性导航系统体坐标系与导航坐标系之间的姿态矩阵，
Figure BDA0003970972870000066
为主惯性导航系统体坐标系在初始时刻与t时刻之间的姿态矩阵，(·)T表示矩阵的转置。
主惯性导航系统体坐标系初始时刻与t时刻之间的姿态矩阵
Figure BDA0003970972870000067
通过下式获得：
Figure BDA0003970972870000068
其中，
Figure BDA0003970972870000069
为
Figure BDA00039709728700000610
的一阶导数，(·×)表示反对称矩阵。
Figure BDA00039709728700000611
为常值矩阵，满足
Figure BDA00039709728700000612
03×3为3维零矩阵。
由于子惯性导航系统中陀螺仪存在测量误差
Figure BDA00039709728700000613
(包含常值零偏εs和高斯白噪声
Figure BDA00039709728700000614
)，所以
Figure BDA0003970972870000071
也会存在计算误差，将含有误差的
Figure BDA0003970972870000072
记为
Figure BDA0003970972870000073
通过下式获得：
Figure BDA0003970972870000074
其中，
Figure BDA0003970972870000075
为
Figure BDA0003970972870000076
的一阶导数。
Figure BDA0003970972870000077
的误差角φs在较短时间内为小角度，φs满足下式：
Figure BDA0003970972870000078
其中，
Figure BDA0003970972870000079
为φs的一阶导数。
步骤二：利用
Figure BDA00039709728700000710
和
Figure BDA00039709728700000711
将主惯性导航系统和子惯性导航系统的角速度测量值以及比力测量值分别投影在m0坐标系和s0坐标系上，具体如下：
Figure BDA00039709728700000712
Figure BDA00039709728700000713
上式中，
Figure BDA00039709728700000714
为主惯性导航系统的角速度测量值，
Figure BDA00039709728700000715
为主惯性导航系统的比力测量值，
Figure BDA00039709728700000716
为子惯性导航系统的角速度测量值，
Figure BDA00039709728700000717
为子惯性导航系统的比力测量值，
Figure BDA00039709728700000718
为投影在s0坐标系上的子惯性导航系统角速度测量值，
Figure BDA00039709728700000719
为投影在m0坐标系上的主惯性导航系统角速度测量值，
Figure BDA00039709728700000720
为投影在s0坐标系上的子惯性导航系统比力测量值，
Figure BDA00039709728700000721
为投影在m0坐标系上的主惯性导航系统比力测量值。
考虑投影在主惯性导航系统体坐标系下的杆臂误差值rm建立下式：
Figure BDA00039709728700000722
Figure BDA00039709728700000723
其中，
Figure BDA00039709728700000724
为子惯性导航系统中加速度计的测量误差值，
Figure BDA00039709728700000725
为
Figure BDA00039709728700000726
的一阶导数。
所述m0坐标系和s0坐标系分别为主惯性导航系统体坐标系与子惯性导航系统体坐标系在对准初始时刻凝固得到的惯性坐标系。
步骤三：对公式二和公式三进行积分获得：
Figure BDA00039709728700000727
Figure BDA00039709728700000728
其中，
Figure BDA0003970972870000081
Figure BDA0003970972870000082
Figure BDA0003970972870000083
Figure BDA0003970972870000084
Figure BDA0003970972870000085
Figure BDA0003970972870000086
Figure BDA0003970972870000087
步骤四：由于
Figure BDA0003970972870000088
和rm之间存在耦合，导致公式四为非线性。虽然姿态矩阵不存在奇异点，但是其元数个数较多，所以为了实现模型低维和非奇异性的目标，本实施方式设
Figure BDA0003970972870000089
为姿态矩阵
Figure BDA00039709728700000810
对应的修正罗德里格斯参数，则姿态矩阵
Figure BDA00039709728700000811
能够表达如下：
Figure BDA00039709728700000812
其中，I3为3维单位矩阵。
所述修正罗德里格斯参数
Figure BDA00039709728700000813
中的元素个数为3，且在旋转角度为(-2π,2π)范围内不存在奇异点。
步骤五：将公式六分别代入公式四和五，变换获得：
Figure BDA00039709728700000814
Figure BDA00039709728700000815
公式七和公式八中多项式函数的最高阶为3，不含有三角函数，不存在奇异点或者其他约束条件，综合而言，要比采用欧拉角、四元数或经典罗德里格斯参数表示姿态矩阵更为合适。
步骤六：定义状态向量
Figure BDA00039709728700000816
状态噪声向量
Figure BDA00039709728700000817
量测向量
Figure BDA00039709728700000818
以及量测噪声向量ν＝[νV；νΩ]，建立具有弱非线性量测方程的无奇异传递对准模型：
Figure BDA0003970972870000091
其中，
Figure BDA0003970972870000092
为x的一阶导数，εs为
Figure BDA0003970972870000093
中包含的陀螺仪零偏，
Figure BDA0003970972870000094
为
Figure BDA0003970972870000095
中包含的高斯白噪声，
Figure BDA0003970972870000096
为
Figure BDA00039709728700000922
中包含的加速度计零偏，
Figure BDA0003970972870000097
为
Figure BDA00039709728700000923
中包含的高斯白噪声，νV为速度量测噪声，νΩ为姿态量测噪声。
h(x)为非线性函数且表达式如下：
Figure BDA0003970972870000098
F为状态转移矩阵，且
Figure BDA0003970972870000099
G为状态噪声驱动矩阵，
Figure BDA00039709728700000910
0a×b为a×b维的零矩阵。
步骤七：采用扩展卡尔曼滤波对无奇异传递对准模型的状态向量进行估计，获得状态向量中的
Figure BDA00039709728700000911
和φs，并利用
Figure BDA00039709728700000912
和φs求解
Figure BDA00039709728700000913
和
Figure BDA00039709728700000914
将
Figure BDA00039709728700000915
代入公式六获得姿态矩阵
Figure BDA00039709728700000916
将φs代入下式获得
Figure BDA00039709728700000917
Figure BDA00039709728700000918
步骤八：将
Figure BDA00039709728700000919
和
Figure BDA00039709728700000920
代入公式一，获得姿态矩阵
Figure BDA00039709728700000921
进而实现子惯性导航系统的传递对准。
为了验证本实施方式的有益效果，利用Matlab程序对设计的基于修正罗德里格斯参数的无奇异传递对准方法进行仿真，为实现对比分析：
将本实施方式所述方法定义为方法1。
将《SINS任意失准角无奇异快速传递对准》(发表于期刊《宇航学报》，2018年，第39卷，10期)一文中方法定义为方法2。
将《基于罗德里格斯参数的惯性系传递对准算法》(发表于期刊《系统工程与电子技术》，2022年，第44卷，9期)一文中方法定义为方法3。
将通过无迹卡尔曼滤波对φm角模型进行仿真的方法定义为方法4。
Matlab仿真条件设置：
1)采用摇摆轨迹生成所需主惯性导航系统和子惯性导航系统输出数据，摇摆幅值分别为：纵摇方向5°，横摇方向4°，航向方向3°，摇摆周期分别为：纵摇方向7s，横摇方向8s，航向方向9s，初始纵摇和横摇均为0°，初始航向为45°，航行速度为10m/s，离散时间为0.01s，仿真时间为35s。子惯性导航系统陀螺仪常值零偏设置为100°/h，角度随机游走系数设置为
Figure BDA0003970972870000101
加速度计常值零偏设置为5mg，速度随机游走系数设置为
Figure BDA0003970972870000102
主、子惯性导航系统之间的安装误差角设置为μ＝[10°；10°；90°]，主、子惯性导航系统之间的杆臂误差设置为rm＝[2m；2m；1m]。状态噪声协方差矩阵根据子惯性导航系统器件随机游走系数设置，量测噪声协方差矩阵中姿态噪声标准差为0.01m/s，速度噪声标准差为0.005°。
2)方法1、方法2和方法3均是惯性系传递对准方法，无需对子惯性导航系统进行初始化，方法4基于传统传递对准模型，子惯性导航系统需要进行惯性导航解算，采用主惯性导航系统导航信息一步装订的方式实现初始化。
以上述仿真条件，仿真得到的结果如表1、图2～图5所示。
表1后10秒安装误差角估计误差均值和均方根(Root mean square,RMS)
Figure BDA0003970972870000103
Figure BDA0003970972870000111
由图2～图4以及表1能够看出，同方法2和方法4相比，方法1和方法3具有更高的安装误差角估计精度。由图5能够看出，同方法4相比，方法1和方法3同样具有更高的杆臂误差估计精度。对于方法2，由于其不具有估计杆臂误差的能力，因此，当存在杆臂误差时，其估计精度会受到严重影响；对于方法4，当主、子惯导之间的安装误差角为大角度时，φm角模型为强非线性模型，其估计精度也会受到严重影响。对比方法1和方法3，虽然本发明方法的基于修正罗德里格斯参数的传递对准模型要比方法3的基于罗德里格斯参数的传递对准模型在量测模型非线性度上略高，但是从估计精度上可以看出，两者几乎一致，并无明显区别，而方法1提出的传递对准模型具有非奇异性，相比方法3要更为适用。
虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其它所述实施例中。

Claims (8)

1.一种基于修正罗德里格斯参数的无奇异传递对准方法，其特征在于，包括以下步骤：
步骤一：对t时刻子惯性导航系统体坐标系与导航坐标系之间的姿态矩阵
Figure FDA0003970972860000011
进行链式分解，提取未知项，
所述未知项为：初始时刻子惯性导航系统体坐标系与主惯性导航系统体坐标系之间的姿态矩阵
Figure FDA0003970972860000012
和子惯性导航系统体坐标系在初始时刻与t时刻之间的姿态矩阵
Figure FDA0003970972860000013
所述链式分解的表达式如下：
Figure FDA0003970972860000014
其中，
Figure FDA0003970972860000015
为t时刻主惯性导航系统体坐标系与导航坐标系之间的姿态矩阵，
Figure FDA0003970972860000016
为主惯性导航系统体坐标系在初始时刻与t时刻之间的姿态矩阵，(·)T表示矩阵的转置；
步骤二：将主惯性导航系统和子惯性导航系统的角速度测量值以及比力测量值分别投影在m0坐标系和s0坐标系上，并建立下式：
Figure FDA0003970972860000017
Figure FDA0003970972860000018
其中，
Figure FDA0003970972860000019
为投影在s0坐标系上的子惯性导航系统比力测量值，
Figure FDA00039709728600000110
为投影在m0坐标系上的主惯性导航系统比力测量值，
Figure FDA00039709728600000111
投影在s0坐标系上的子惯性导航系统角速度测量值，
Figure FDA00039709728600000112
为投影在m0坐标系上的主惯性导航系统角速度测量值，
Figure FDA00039709728600000113
为含有误差的
Figure FDA00039709728600000114
φs为
Figure FDA00039709728600000115
的误差角，
Figure FDA00039709728600000116
为子惯性导航系统中加速度计的测量误差值，
Figure FDA00039709728600000117
为子惯性导航系统中陀螺仪的测量误差值，
Figure FDA00039709728600000118
为主惯性导航系统的角速度测量值，
Figure FDA00039709728600000119
为
Figure FDA00039709728600000120
的一阶导数，(·×)表示反对称矩阵，rm为投影在主惯性导航系统体坐标系下的杆臂误差值，
所述m0坐标系和s0坐标系分别为主惯性导航系统体坐标系与子惯性导航系统体坐标系在对准初始时刻凝固得到的惯性坐标系；
步骤三：对公式二和公式三进行积分获得：
Figure FDA00039709728600000121
Figure FDA00039709728600000122
其中，
Figure FDA00039709728600000123
Figure FDA0003970972860000021
Figure FDA0003970972860000022
Figure FDA0003970972860000023
Figure FDA0003970972860000024
Figure FDA0003970972860000025
Figure FDA0003970972860000026
上式中，
Figure FDA0003970972860000027
和
Figure FDA0003970972860000028
分别为子惯性导航系统的比力测量值和角速度测量值；
步骤四：设
Figure FDA0003970972860000029
为姿态矩阵
Figure FDA00039709728600000210
对应的修正罗德里格斯参数，则姿态矩阵
Figure FDA00039709728600000211
能够表达如下：
Figure FDA00039709728600000212
其中，I3为3维单位矩阵；
步骤五：将公式六分别代入公式四和五，变换获得：
Figure FDA00039709728600000213
Figure FDA00039709728600000214
步骤六：定义状态向量
Figure FDA00039709728600000215
状态噪声向量
Figure FDA00039709728600000216
量测向量
Figure FDA00039709728600000217
以及量测噪声向量ν＝[νV；νΩ]，建立无奇异传递对准模型：
Figure FDA00039709728600000218
其中，
Figure FDA00039709728600000219
为x的一阶导数，F为状态转移矩阵，G为状态噪声驱动矩阵，εs为
Figure FDA00039709728600000220
中包含的陀螺仪零偏，
Figure FDA00039709728600000221
为
Figure FDA00039709728600000222
中包含的高斯白噪声，
Figure FDA00039709728600000223
为
Figure FDA00039709728600000224
中包含的加速度计零偏，
Figure FDA00039709728600000225
为
Figure FDA00039709728600000226
中包含的高斯白噪声，νV为速度量测噪声，νΩ为姿态量测噪声，
h(x)为非线性函数且表达式如下：
Figure FDA0003970972860000031
步骤七：采用扩展卡尔曼滤波对无奇异传递对准模型的状态向量进行估计，获得状态向量中的
Figure FDA0003970972860000032
和φs，并利用
Figure FDA0003970972860000033
和φs求解
Figure FDA0003970972860000034
和
Figure FDA0003970972860000035
步骤八：将
Figure FDA0003970972860000036
和
Figure FDA0003970972860000037
代入公式一，获得姿态矩阵
Figure FDA0003970972860000038
进而实现子惯性导航系统的传递对准。
2.根据权利要求1所述的一种基于修正罗德里格斯参数的无奇异传递对准方法，其特征在于，步骤一中，主惯性导航系统体坐标系初始时刻与t时刻之间的姿态矩阵
Figure FDA0003970972860000039
通过下式获得：
Figure FDA00039709728600000310
其中，
Figure FDA00039709728600000311
为
Figure FDA00039709728600000312
的一阶导数。
3.根据权利要求1所述的一种基于修正罗德里格斯参数的无奇异传递对准方法，其特征在于，步骤二中，将主惯性导航系统和子惯性导航系统的角速度测量值以及比力测量值分别投影在m0坐标系和s0坐标系上的具体方法为：
Figure FDA00039709728600000313
Figure FDA00039709728600000314
上式中，
Figure FDA00039709728600000315
为主惯性导航系统的比力测量值。
4.根据权利要求3所述的一种基于修正罗德里格斯参数的无奇异传递对准方法，其特征在于，步骤二中，φs满足下式：
Figure FDA00039709728600000316
其中，
Figure FDA00039709728600000317
为φs的一阶导数。
5.根据权利要求4所述的一种基于修正罗德里格斯参数的无奇异传递对准方法，其特征在于，步骤二中，
Figure FDA00039709728600000318
通过下式获得：
Figure FDA00039709728600000319
其中，
Figure FDA0003970972860000041
为
Figure FDA0003970972860000042
的一阶导数。
6.根据权利要求1所述的一种基于修正罗德里格斯参数的无奇异传递对准方法，其特征在于，步骤四中，修正罗德里格斯参数
Figure FDA0003970972860000043
中的元素个数为3，且在旋转角度为(-2π,2π)范围内不存在奇异点。
7.根据权利要求1所述的一种基于修正罗德里格斯参数的无奇异传递对准方法，其特征在于，步骤六中，状态转移矩阵F和状态噪声驱动矩阵G的表达式分别如下：
Figure FDA0003970972860000044
Figure FDA0003970972860000045
其中，0a×b为a×b维的零矩阵。
8.根据权利要求1所述的一种基于修正罗德里格斯参数的无奇异传递对准方法，其特征在于，步骤七中，将
Figure FDA0003970972860000046
代入公式六获得姿态矩阵
Figure FDA0003970972860000047
将φs代入下式获得
Figure FDA0003970972860000048
Figure FDA0003970972860000049
CN202211518553.7A 2022-11-29 2022-11-29 一种基于修正罗德里格斯参数的无奇异传递对准方法 Active CN115727875B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211518553.7A CN115727875B (zh) 2022-11-29 2022-11-29 一种基于修正罗德里格斯参数的无奇异传递对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211518553.7A CN115727875B (zh) 2022-11-29 2022-11-29 一种基于修正罗德里格斯参数的无奇异传递对准方法

Publications (2)

Publication Number Publication Date
CN115727875A true CN115727875A (zh) 2023-03-03
CN115727875B CN115727875B (zh) 2023-09-01

Family

ID=85299403

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211518553.7A Active CN115727875B (zh) 2022-11-29 2022-11-29 一种基于修正罗德里格斯参数的无奇异传递对准方法

Country Status (1)

Country Link
CN (1) CN115727875B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107990910A (zh) * 2017-11-06 2018-05-04 哈尔滨工业大学 一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法
CN108981696A (zh) * 2018-08-01 2018-12-11 西北工业大学 一种sins任意失准角无奇异快速传递对准方法
US20220033100A1 (en) * 2019-05-17 2022-02-03 Southeast University Dual-filter-based transfer alignment method under dynamic deformation
CN115371706A (zh) * 2022-09-05 2022-11-22 中国人民解放军海军工程大学 一种基于罗德里格斯参数的李群捷联惯导误差模型统一式

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107990910A (zh) * 2017-11-06 2018-05-04 哈尔滨工业大学 一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法
CN108981696A (zh) * 2018-08-01 2018-12-11 西北工业大学 一种sins任意失准角无奇异快速传递对准方法
US20220033100A1 (en) * 2019-05-17 2022-02-03 Southeast University Dual-filter-based transfer alignment method under dynamic deformation
CN115371706A (zh) * 2022-09-05 2022-11-22 中国人民解放军海军工程大学 一种基于罗德里格斯参数的李群捷联惯导误差模型统一式

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
崔潇 等: "SINS 任意失准角无奇异快速传递对准", 《宇航学报》 *
徐庚 等: "基于罗德里格斯参数的惯性系传递对准算法", 《系统工程与电子技术》 *

Also Published As

Publication number Publication date
CN115727875B (zh) 2023-09-01

Similar Documents

Publication Publication Date Title
CN107990910B (zh) 一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法
CN104655131B (zh) 基于istssrckf的惯性导航初始对准方法
CN105737823B (zh) 一种基于五阶ckf的gps/sins/cns组合导航方法
CN103344260B (zh) 基于rbckf的捷联惯导系统大方位失准角初始对准方法
CN103900574B (zh) 一种基于迭代容积卡尔曼滤波姿态估计方法
CN101949703A (zh) 一种捷联惯性/卫星组合导航滤波方法
CN103575299A (zh) 利用外观测信息的双轴旋转惯导系统对准及误差修正方法
CN105136145A (zh) 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法
CN104567930A (zh) 一种能够估计和补偿机翼挠曲变形的传递对准方法
CN108318038A (zh) 一种四元数高斯粒子滤波移动机器人姿态解算方法
CN103913181A (zh) 一种基于参数辨识的机载分布式pos传递对准方法
CN110186478B (zh) 用于捷联式惯导系统的惯性传感器选型方法及系统
CN108592943A (zh) 一种基于opreq方法的惯性系粗对准计算方法
CN110044385A (zh) 一种大失准角情况下的快速传递对准方法
CN103776449A (zh) 一种提高鲁棒性的动基座初始对准方法
CN103575276A (zh) 一种双轴旋转惯性导航系统初始对准模型降阶方法
CN102168978A (zh) 一种船用惯性导航系统摇摆基座开环对准方法
CN102436437B (zh) 基于角速度的飞行器极限飞行时四元数傅里埃近似输出方法
CN108731702B (zh) 一种基于Huber方法的大失准角传递对准方法
CN112325841B (zh) 一种动中通天线安装误差角的估计方法
CN115727875A (zh) 一种基于修正罗德里格斯参数的无奇异传递对准方法
CN104613984B (zh) 临近空间飞行器传递对准模型不确定性的鲁棒滤波方法
CN103591960A (zh) 一种基于旋转调制的静基座惯性导航系统粗对准方法
CN112649022B (zh) 一种考虑挠曲变形和杆臂效应的大失准角传递对准方法
CN113029197B (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