CN112212889A - 基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法 - Google Patents

基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法 Download PDF

Info

Publication number
CN112212889A
CN112212889A CN202010975160.3A CN202010975160A CN112212889A CN 112212889 A CN112212889 A CN 112212889A CN 202010975160 A CN202010975160 A CN 202010975160A CN 112212889 A CN112212889 A CN 112212889A
Authority
CN
China
Prior art keywords
attitude
matrix
orthogonal group
equation
special orthogonal
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.)
Pending
Application number
CN202010975160.3A
Other languages
English (en)
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 University of Technology
Original Assignee
Beijing University of 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 Beijing University of Technology filed Critical Beijing University of Technology
Priority to CN202010975160.3A priority Critical patent/CN112212889A/zh
Publication of CN112212889A publication Critical patent/CN112212889A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

本发明公开了基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法，该方法采用特殊正交群描述代替传统四元数描述实现对SINS姿态变换的计算，利用特殊正交群微分方程建立基于特殊正交群描述的线性粗对准模型，设计了一种基于矢量间余弦关系的新的姿态误差函数。本发明采用姿态误差函数的梯度作为新息项，使得姿态误差沿着梯度方向快速收敛，从而大幅度缩短了对准时间；使用特殊正交群描述姿态，避免了传统四元数描述初始姿态矩阵而产生的非唯一性和非线性问题，有效提高了对准精度。本发明在实际工程中具有实用价值。

Description

基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座 粗对准方法
技术领域
本发明公开了一种基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法，该方法属于导航方法及应用技术领域。
背景技术
捷联惯性导航系统是一种自主式的导航系统，在工作时不依靠外界信息，也不向外界辐射任何能量，隐蔽性好、抗扰性强，能够全天时、全天候为载体提供完备的运动信息。初始对准是SINS中最重要的技术之一，其目的是获得初始姿态矩阵。惯性导航系统的初始对准精度直接关系到系统的工作精度。初始对准时间是惯性导航系统的一项重要技术指标。传统的初始对准方法通常采用粗对准加精对准的方式。
粗对准过程的研究在捷联惯导系统中占有重要意义，尤其是晃动基座下的粗对准方法更是当下的研究热点。晃动基座条件下粗对准可以采用双矢量定姿和基于Wahba问题求解两类完成粗对准过程。但这些方法仍然具有不可忽视的缺陷。双矢量确定方法仅利用两个重力矢量来确定初始姿态，舍弃了大量的测量信息，导致粗对准过程精度不高。此外，该方法要求确保姿态的两个重力矢量不能共线，这就使得选择的两个重力矢量之间的时间间隔必须足够长。这意味着基于双向量确定的粗对准过程需要更长的对准时间。虽然Wahba方法可以充分利用所有的测量信息，但它不能保证全局最优，这将影响粗对准过程中的对准精度和时间。
为进一步提高初始对准的性能，本发明针对现有的晃动基座粗对准方法存在的问题，借鉴系最优估计的思想，设计了一种新的特殊正交群最优估计算法。得益于特殊正交群理论的完善，通过提出一种新的姿态误差函数来衡量姿态误差，然后将姿态误差函数的梯度作为更新项来补偿姿态。基于这种方法构建的估计模型避免了传统最优化方法中的奇异值问题，并且姿态估计沿着梯度方向下降，从而快速达到收敛。该方法有效地提高了对准的速度，并且在精度方面也有所提升。仿真实验证明了该算法的可行性，可以作为四元数粗对准的上位替代进行晃动基座粗对准。
发明内容
由于初始对准过程中，载体容易受到外界的各种干扰因素的影响，在对准过程中很难保持静态不动。因此，晃动基座下的粗对准算法具有很高的研究意义与应用价值。本发明的目的是为了应对现有晃动基座粗对准方法存在的问题：(1)本发明通过特殊正交群代替四元数描述初始姿态矩阵，避免了传统四元数描述方法的非唯一性和非线性问题；(2)本发明利用特殊正交群微分方程建立基于特殊正交群描述的线性粗对准模型；(3)本发明设计了一种新的姿态误差函数来衡量姿态的估计误差，然后将姿态误差函数的梯度作为更新项使得姿态沿着梯度方向快速收敛。
为了达到上述目的，本发明提供如下技术方案：
基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法，其特征在于，该方法通过下述步骤实现：
步骤(1)：SINS捷联惯性导航系统进行系统预热准备，启动系统，获得载体所在位置的经度λ、纬度L、当地重力加速度在导航系下的投影gn基本信息，采集惯性测量单元IMU中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影
Figure BDA0002685497890000021
和加速度计输出的载体系加速度信息fb；
步骤(2)：对采集到的陀螺仪和加速度计的数据进行预处理，基于特殊正交群微分方程，建立基于特殊正交群描述的线性晃动基座粗对准系统模型：
本方法的详细描述中坐标系定义如下：
地球坐标系e系，选取地球中心为原点，X轴位于赤道平面内，从地心指向本初子午线，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；
地心惯性坐标系i系，选取地球中心为原点，X轴位于赤道平面内，从地心指向春分点，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系；
导航坐标系n系，表示载体所在位置的地理坐标系，选取舰载机重心为原点，X轴指向东向E，Y轴指向北向N，Z轴指向天向U；本方法中导航坐标系选取为地理坐标系；
载体坐标系b系，表示捷联惯性导航系统三轴正交坐标系，选取舰载机重心为原点，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；
初始导航坐标系n(0)系，表示SINS开机运行时刻的导航坐标系，并在整个对准过程中相对于惯性空间保持静止；
初始载体坐标系b(0)系，表示SINS开机运行时刻的载体坐标系，并在整个对准过程中相对于惯性空间保持静止；
导航坐标系n'系，表示由特殊正交群最优估计算法计算得到的初始导航坐标系，本坐标系和真实导航坐标系n系之间存在转动关系；
基于特殊正交群微分方程，建立基于特殊正交群描述的线性粗对准系统模型：
根据SINS捷联惯性导航系统原理，SINS晃动基座粗对准问题转化为姿态估计问题，姿态变换为两个坐标系之间的旋转变换，导航的姿态矩阵可以用一个3×3的正交变换矩阵表示；该正交变换矩阵符合特殊正交群的特殊正交群SO(n)的性质，构成了三维旋转群SO(3)：
Figure BDA0002685497890000022
其中，R∈SO(3)表示特定的导航姿态矩阵，
Figure BDA0002685497890000023
表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(R)表示为矩阵R的行列式；
晃动基座粗对准姿态估计问题转化为对基于特殊正交群描述的姿态矩阵R的求解问题；根据基于特殊正交群描述的姿态矩阵的链式法则，将导航姿态矩阵
Figure BDA0002685497890000031
分解为三个矩阵的乘积形式：
Figure BDA0002685497890000032
其中，t表示时间变量，
Figure BDA0002685497890000033
表示当前导航系相对于当前载体系的姿态矩阵，
Figure BDA0002685497890000034
表示当前导航系相对于初始导航系的姿态矩阵，初始姿态矩阵
Figure BDA0002685497890000035
表示初始导航系相对于初始载体系的姿态矩阵，
Figure BDA0002685497890000036
表示初始载体系相对于当前载体系的姿态矩阵；
根据特殊正交群微分方程，姿态矩阵
Figure BDA0002685497890000037
和
Figure BDA0002685497890000038
随时间变化更新过程为：
Figure BDA0002685497890000039
Figure BDA00026854978900000310
其中，
Figure BDA00026854978900000311
表示初始载体系相对于当前载体系的姿态矩阵，
Figure BDA00026854978900000312
表示导航系相对于惯性系的旋转角速率在导航系的投影，在晃动基座条件下其等于地球自转角速率
Figure BDA00026854978900000313
L表示当地纬度，
Figure BDA00026854978900000314
表示陀螺仪输出的载体系相对于惯性系的旋转角速率在载体系的投影，符号(·×)表示将一个三维向量转换成一个反对称矩阵的运算，运算规则如下：
Figure BDA00026854978900000315
符号(·×)的逆运算符(·)V表示从反对称矩阵到三维向量的操作：S∨＝ω。
由公式(2)-(5)可以看出，
Figure BDA00026854978900000316
和
Figure BDA00026854978900000317
可由IMU传感器数据实时计算得到，而
Figure BDA00026854978900000318
表示初始时刻的姿态矩阵，其不随时间变化；因此，SINS晃动基座粗对准过程中姿态矩阵
Figure BDA00026854978900000319
的求解问题，转化为对基于特殊正交群描述的初始姿态矩阵
Figure BDA00026854978900000320
的求解问题；
在晃动基座条件下，加速度计测量信息fb表示为：
fb＝-gb (6)其中，gb表示当地重力加速度在载体系下的投影
根据SINS惯性导航基本原理和特殊正交群链式法则，gn和gb之间存在如下关系：
Figure BDA00026854978900000321
对公式(7)进行移项、整理操作，可得：
Figure BDA00026854978900000322
公式(8)简化表示为：
Figure BDA0002685497890000041
其中，
Figure BDA0002685497890000042
表示重力加速度在初始导航系下的投影，
Figure BDA0002685497890000043
表示重力加速度在初始载体系下的投影；
对公式(9)在[0,t]上做积分，可得：
Figure BDA0002685497890000044
其中，
Figure BDA0002685497890000045
表示重力加速度
Figure BDA0002685497890000046
对应的速度矢量，
Figure BDA0002685497890000047
表示重力加速度
Figure BDA0002685497890000048
对应的速度矢量；
公式(10)可以简化为：
y(t)＝Rx(t) (11)
其中
Figure BDA0002685497890000049
Figure BDA00026854978900000410
Figure BDA00026854978900000411
由于将求解姿态矩阵
Figure BDA00026854978900000412
的问题转化为求解初始姿态矩阵
Figure BDA00026854978900000413
的问题，且
Figure BDA00026854978900000414
为符合特殊正交群特性的常值矩阵，因此建立晃动基座粗对准系统的线性状态方程如下：
Figure BDA00026854978900000415
根据上述内容，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，建立具有特殊正交群结构的晃动基座粗对准系统方程，表示为：
Figure BDA00026854978900000416
步骤(3)：根据特殊正交群最优估计算法，直接对基于特殊正交群描述的初始姿态矩阵
Figure BDA00026854978900000417
进行最优估计：
对于公式(10)中的测量方程，R可以将n坐标系中的向量x(t)转换为b坐标系中的向量y(t)。仅考虑向量方向的投影，式(10)可表示为:
r(t)＝Ru(t) (17)其中，
Figure BDA00026854978900000418
因此r和u都是单位向量。
向量r(t)的预测值
Figure BDA00026854978900000419
可以下式获得：
Figure BDA00026854978900000420
其中，
Figure BDA00026854978900000421
是R的预测值。
为了更好地描述姿态估计的误差，设计一种新的姿态误差函数
Figure BDA00026854978900000422
如下：
Figure BDA00026854978900000423
将公式(17)和(18)带入公式(19)得：
Figure BDA0002685497890000051
Figure BDA0002685497890000052
的无穷小变化量
Figure BDA0002685497890000053
如下：
Figure BDA0002685497890000054
其中，
Figure BDA0002685497890000055
从
Figure BDA0002685497890000056
的定义可知,
Figure BDA0002685497890000057
在
Figure BDA0002685497890000058
处的切映射如下:
Figure BDA0002685497890000059
其中，Γ(∈)＝exp(∈S)R是在R周围的一条曲线。
将公式(21)带入公式(22)可得：
Figure BDA00026854978900000510
对于任意的
Figure BDA00026854978900000511
有下式成立：
Figure BDA00026854978900000512
其中，
Figure BDA00026854978900000513
Figure BDA00026854978900000514
是反对称投影算子，
Figure BDA00026854978900000515
是对称投影算子。
将公式(24)带入公式(23)得：
Figure BDA00026854978900000516
一个任意对称矩阵和一个反对称矩阵的乘积的迹等于零，因此
Figure BDA00026854978900000517
其中，
Figure BDA00026854978900000518
在S上的内积<·,·>定义如下：
<A,B>＝tr(ATB) (27)
其中，A,B∈S.
在S上黎曼度量<·,·>R定义如下：
Figure BDA00026854978900000519
将公式(27)带入公式(26)得：
Figure BDA00026854978900000520
将公式(28)带入公式(29)得：
Figure BDA00026854978900000521
Figure BDA00026854978900000522
的切映射和
Figure BDA00026854978900000523
的梯度有以下的关系:
Figure BDA00026854978900000524
其中，
Figure BDA00026854978900000525
是
Figure BDA00026854978900000526
的梯度。
由于公式(29)和(31)左边是相等的，所有右边也是相等的，因此：
Figure BDA0002685497890000061
我们选择姿态误差函数的梯度
Figure BDA0002685497890000062
作为新的创新项补偿姿态特殊正交群,这使得姿态估计沿梯度下降的方向传播,并最终使姿态特殊正交群可以快速收敛。因此，基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准算法归纳为：
Figure BDA0002685497890000063
步骤(4)：求解导航系统所需的姿态矩阵
Figure BDA0002685497890000064
从而完成晃动基座粗对准过程：
根据之前步骤中求解得到的姿态变化矩阵
Figure BDA0002685497890000065
和
Figure BDA0002685497890000066
信息，通过公式(2)即可求解导航姿态矩阵，完成SINS捷联惯性导航系统晃动基座粗对准。
与现有技术相比，本发明具有如下优点和有益效果：
(1)本发明采用特殊正交群描述初始姿态矩阵
Figure BDA0002685497890000067
能够有效避免传统四元数描述初始姿态矩阵
Figure BDA0002685497890000068
产生的非唯一性和非线性问题。
(2)本发明利用特殊正交群微分方程建立基于特殊正交群描述的线性粗对准模型。
(3)本发明设计了一种新的姿态误差函数来衡量姿态的估计误差，然后将姿态误差函数的梯度作为更新项使得姿态沿着梯度方向快速收敛。相较于传统的粗对准方法，不仅能够大幅度缩短对准时间，还能够提高对准的精度。
附图说明
图1捷联惯性导航系统装置总体简图。
图2捷联惯性导航系统流程图。
图3晃动基座粗对准仿真结果图。
图4晃动基座粗对准实验结果图。
图5上位机采集真实导航数据页面示意图。
具体实施方式
本发明是基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法设计，下面结合本发明系统流程图对本发明的具体实施步骤进行详细的描述：
本发明提供的基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法，首先获取传感器实时数据；对采集到的数据进行处理，基于特殊正交群微分方程，建立基于特殊正交群描述的线性粗对准系统模型；使用特殊正交群最优估计算法，估计得到基于特殊正交群描述的初始姿态矩阵
Figure BDA0002685497890000069
并求解姿态矩阵
Figure BDA00026854978900000610
在粗对准期间，经过多次估计解算，最终得到精确的初始姿态矩阵
Figure BDA0002685497890000071
和姿态矩阵
Figure BDA0002685497890000072
完成粗对准过程。
步骤2：对采集到的陀螺仪和加速度计的数据进行处理，基于特殊正交群微分方程，建立基于特殊正交群描述的线性晃动基座粗对准系统模型，
步骤(2.1)：通过
Figure BDA0002685497890000073
更新计算
Figure BDA0002685497890000074
基于特殊正交群描述的姿态矩阵链式法则，姿态矩阵
Figure BDA0002685497890000075
可以分解为：
Figure BDA0002685497890000076
由于
Figure BDA0002685497890000077
表示导航系相对于惯性系的旋转角速率，且
Figure BDA0002685497890000078
变化通常是十分缓慢的，根据基于特殊正交群描述的姿态矩阵更新求解过程，tk-1时刻到tk时刻的姿态矩阵
Figure BDA0002685497890000079
可以近似为：
Figure BDA00026854978900000710
其中，
Figure BDA00026854978900000711
根据公式(25)-(27)，姿态矩阵
Figure BDA00026854978900000712
迭代过程可以近似为：
Figure BDA00026854978900000713
步骤(2.2)：通过陀螺仪输出的角速率信息
Figure BDA00026854978900000714
更新计算
Figure BDA00026854978900000715
tk-1时刻到tk时刻的姿态矩阵
Figure BDA00026854978900000716
可以近似为：
Figure BDA00026854978900000717
其中，根据双子样旋转矢量法，可得：
Figure BDA00026854978900000718
其中，Δθ1和Δθ2分别表示两个相邻半采样周期
Figure BDA00026854978900000719
内由陀螺仪输出计算所得的角增量；
根据公式(29)-(30)，姿态矩阵
Figure BDA00026854978900000720
迭代过程可以近似为：
Figure BDA00026854978900000721
步骤(2.3)：建立基于特殊正交群描述的关于初始旋转矩阵
Figure BDA00026854978900000722
的粗对准系统模型方式为：
在晃动基座条件下，gn和gb之间存在如下关系：
Figure BDA00026854978900000723
对公式(32)进行移项、整理操作，可得：
Figure BDA00026854978900000724
公式(34)可以表示为：
Figure BDA0002685497890000081
其中，
Figure BDA0002685497890000082
表示重力加速度在初始导航系下的投影，
Figure BDA0002685497890000083
表示重力加速度在初始载体系下的投影；
对公式(35)在[0,t]上做积分，可得量测方程如下：
Figure BDA0002685497890000084
公式(36)可以表示成如下形式：
y(t)＝Rx(t) (45)
其中，
Figure BDA0002685497890000085
Figure BDA0002685497890000086
Figure BDA0002685497890000087
由于姿态矩阵
Figure BDA0002685497890000088
为常值矩阵，因此建立晃动基座粗对准系统的线性状态方程如下：
Figure BDA0002685497890000089
根据上述内容，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，建立具有特殊正交群结构的晃动基座粗对准系统方程，表示为：
Figure BDA00026854978900000810
3、根据权利要求1所述的基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法，其特征在于，步骤(3)中使用特殊正交群最优估计算法直接估计
Figure BDA00026854978900000811
特殊正交群最优估计算法为了更好地表示姿态误差，建立了新的姿态误差函数。为了使姿态误差的变化反映在姿态补偿中，采用新的姿态误差函数的梯度作为创新项，对姿态进行实时补偿，使系统快速收敛。晃动基座粗对准整体过程如下：
Figure BDA00026854978900000812
4、根据权利要求1所述的基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法，其特征在于，步骤(4)中求解姿态矩阵
Figure BDA00026854978900000813
并解算姿态信息；
在步骤(2)中，将SINS粗对准过程中
Figure BDA00026854978900000814
的求解问题转化为对
Figure BDA00026854978900000815
的求解问题，并将
Figure BDA00026854978900000816
分解为三个矩阵乘积的形式：
Figure BDA00026854978900000817
根据公式(28)、公式(31)和公式(47)求解得到的
Figure BDA00026854978900000818
和
Figure BDA00026854978900000819
姿态矩阵
Figure BDA00026854978900000820
的求解方式为：
Figure BDA0002685497890000091
并根据求解得到的姿态矩阵
Figure BDA0002685497890000092
解算姿态信息。
本发明的有益效果如下：
(1)在以下的仿真条件下，对该方法进行仿真实验：
步骤(1)中，晃动基座条件下模拟载体收到风浪影响，其航向角ψ、俯仰角θ、横滚角γ作周期变化，姿态变化情况如下：
Figure BDA0002685497890000093
Figure BDA0002685497890000094
Figure BDA0002685497890000095
步骤(1)中，初始地理位置：东经118°，北纬40°；
步骤(1)中，传感器输出频率为100Hz；
步骤(1)中，陀螺仪漂移：三个方向轴上的陀螺常值漂移为0.1°/h，随机漂移为0.01°/h；
步骤(1)中，加速度计零位偏置：三个方向轴上的加速度计常值偏置为1mg，随机偏置为0.1mg；
步骤(2)中，地球自转角速率7.2921158e-5rad/s；
步骤(2)中，时间间隔T为0.02s；
步骤(3)中，特殊正交群最优估计算法初始值
Figure BDA0002685497890000096
方法仿真结果如下：
进行了200s仿真，以姿态角的估计误差作为衡量指标，仿真结果如图(3)所示。从图中可以看出，俯仰姿态在36s左右完成对准，收敛到0.40′；横滚姿态在32s左右完成对准，收敛到0.48′；航向姿态在40s左右完成对准，收敛到4.23′。由仿真结果可知，本方法可以快速有效的完成晃动基座下的粗对准任务。
(2)通过真实实验对本发明提出的基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法进行验证。真实试验中，不提任何供外界辅助信息，实验历时200s。上位导航计算机控制导航系统，以100HZ的数据更新速率，115200bps的波特率，采集航向精度达0.1度、姿态精度达0.05度的实际三轴姿态信息，解算获得的载体姿态信息与本步骤中得到的高精度真实载体姿态信息做比较，证明本方法和系统的可行性和有效性。
系统实验结果如下：
进行了200s的真实实验，以姿态角的估计误差作为衡量对准精度的指标，结果如图(4)所示。从图中可以看出，俯仰姿态在60s左右完成对准，收敛到4.02′；横滚姿态在64s左右完成对准，收敛到3.28′；航向姿态在80s左右完成对准，收敛到24.93′。本方法可以快速有效的完成晃动基座下的粗对准任务。
本发明将晃动基座粗对准问题转化为初始旋转矩阵的最优估计问题，采用基于特殊正交群方法描述初始姿态矩阵，能够有效避免传统基于四元数方法描述初始姿态矩阵产生的非唯一性和和非线性问题；本发明利用特殊正交群微分方程建立线性粗对准系统模型；本发明设计了一种新的姿态误差函数来衡量姿态的估计误差，然后将姿态误差函数的梯度作为更新项使得姿态沿着梯度方向快速收敛。相较于传统的粗对准方法，不仅能够大幅度缩短对准时间，还能够提高对准的精度。
以上所述仅是本发明的优选实施方式，并不用于限制本发明。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。

Claims (4)

1.基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法，其特征在于，该方法通过下述步骤实现：
步骤(1)：SINS捷联惯性导航系统进行系统预热准备，启动系统，获得载体所在位置的经度λ、纬度L、当地重力加速度在导航系下的投影gn基本信息，采集惯性测量单元IMU中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影
Figure FDA0002685497880000011
和加速度计输出的载体系加速度信息fb；
步骤(2)：对采集到的陀螺仪和加速度计的数据进行预处理，基于特殊正交群微分方程，建立基于特殊正交群描述的线性晃动基座粗对准系统模型：
基于特殊正交群微分方程，建立基于特殊正交群描述的线性粗对准系统模型：
根据SINS捷联惯性导航系统原理，SINS晃动基座粗对准问题转化为姿态估计问题，姿态变换为两个坐标系之间的旋转变换，导航的姿态矩阵用一个3×3的正交变换矩阵表示；该正交变换矩阵符合特殊正交群的特殊正交群SO(n)的性质，构成三维旋转群SO(3)
Figure FDA0002685497880000012
其中，R∈SO(3)表示特定的导航姿态矩阵，
Figure FDA0002685497880000013
表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(R)表示为矩阵R的行列式；
晃动基座粗对准姿态估计问题转化为对基于特殊正交群描述的姿态矩阵R的求解问题；根据基于特殊正交群描述的姿态矩阵的链式法则，将导航姿态矩阵
Figure FDA0002685497880000014
分解为三个矩阵的乘积形式：
Figure FDA0002685497880000015
其中，t表示时间变量，
Figure FDA0002685497880000016
表示当前导航系相对于当前载体系的姿态矩阵，
Figure FDA0002685497880000017
表示当前导航系相对于初始导航系的姿态矩阵，初始姿态矩阵
Figure FDA0002685497880000018
表示初始导航系相对于初始载体系的姿态矩阵，
Figure FDA0002685497880000019
表示初始载体系相对于当前载体系的姿态矩阵；
根据特殊正交群微分方程，姿态矩阵
Figure FDA00026854978800000110
和R
Figure FDA00026854978800000111
随时间变化更新过程为：
Figure FDA00026854978800000112
Figure FDA00026854978800000113
其中，
Figure FDA00026854978800000114
表示初始载体系相对于当前载体系的姿态矩阵，
Figure FDA00026854978800000115
表示导航系相对于惯性系的旋转角速率在导航系的投影，在晃动基座条件下其等于地球自转角速率
Figure FDA00026854978800000116
L表示当地纬度，
Figure FDA00026854978800000117
表示陀螺仪输出的载体系相对于惯性系的旋转角速率在载体系的投影，符号(·×)表示将一个三维向量转换成一个反对称矩阵的运算，运算规则如下：
Figure FDA00026854978800000118
符号(·×)的逆运算符(·)∨表示从反对称矩阵到三维向量的操作：S∨＝ω；
由公式(2)-(5)看出，
Figure FDA0002685497880000021
和
Figure FDA0002685497880000022
由IMU传感器数据实时计算得到，而
Figure FDA0002685497880000023
表示初始时刻的姿态矩阵，SINS晃动基座粗对准过程中姿态矩阵
Figure FDA0002685497880000024
的求解问题，转化为对基于特殊正交群描述的初始姿态矩阵
Figure FDA0002685497880000025
的求解问题；
在晃动基座条件下，加速度计测量信息fb表示为：
fb＝-gb (6)
其中，gb表示当地重力加速度在载体系下的投影；
根据SINS惯性导航基本原理和特殊正交群链式法则，gn和gb之间存在如下关系：
Figure FDA0002685497880000026
对公式(7)进行移项、整理操作，可得：
Figure FDA0002685497880000027
公式(8)简化表示为：
Figure FDA0002685497880000028
其中，
Figure FDA0002685497880000029
表示重力加速度在初始导航系下的投影，
Figure FDA00026854978800000210
表示重力加速度在初始载体系下的投影；
对公式(9)在[0,t]上做积分，可得：
Figure FDA00026854978800000211
其中，
Figure FDA00026854978800000212
表示重力加速度
Figure FDA00026854978800000213
对应的速度矢量，
Figure FDA00026854978800000214
表示重力加速度
Figure FDA00026854978800000215
对应的速度矢量；
公式(10)简化为：
y(t)＝Rx(t) (11)
其中
Figure FDA00026854978800000216
Figure FDA00026854978800000217
Figure FDA00026854978800000218
由于将求解姿态矩阵
Figure FDA00026854978800000219
的问题转化为求解初始姿态矩阵
Figure FDA00026854978800000220
的问题，且
Figure FDA00026854978800000221
为符合特殊正交群特性的常值矩阵，因此建立晃动基座粗对准系统的线性状态方程如下：
Figure FDA00026854978800000222
将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，建立具有特殊正交群结构的晃动基座粗对准系统方程，表示为：
Figure FDA00026854978800000223
步骤(3)：根据特殊正交群最优估计算法，直接对基于特殊正交群描述的初始姿态矩阵
Figure FDA0002685497880000031
进行最优估计：
对于公式(10)中的测量方程，R可以将n坐标系中的向量x(t)转换为b坐标系中的向量y(t)；仅考虑向量方向的投影，式(10)可表示为:
r(t)＝Ru(t) (17)
其中，
Figure FDA0002685497880000032
因此r和u都是单位向量；
向量r(t)的预测值
Figure FDA0002685497880000033
可以下式获得：
Figure FDA0002685497880000034
其中，
Figure FDA0002685497880000035
是R的预测值；
为描述姿态估计的误差，设计一种姿态误差函数
Figure FDA0002685497880000036
如下：
Figure FDA0002685497880000037
将公式(17)和(18)带入公式(19)得：
Figure FDA0002685497880000038
Figure FDA0002685497880000039
的无穷小变化量
Figure FDA00026854978800000310
如下：
Figure FDA00026854978800000311
其中，
Figure FDA00026854978800000312
从
Figure FDA00026854978800000313
的定义可知,
Figure FDA00026854978800000314
在
Figure FDA00026854978800000315
处的切映射如下:
Figure FDA00026854978800000316
其中，Γ(∈)＝exp(∈S)R是在R周围的一条曲线；
将公式(21)带入公式(22)可得：
Figure FDA00026854978800000317
对于任意的
Figure FDA00026854978800000318
有下式成立：
Figure FDA00026854978800000319
其中，
Figure FDA00026854978800000320
Figure FDA00026854978800000321
是反对称投影算子，
Figure FDA00026854978800000322
是对称投影算子；
将公式(24)带入公式(23)得：
Figure FDA00026854978800000323
一个任意对称矩阵和一个反对称矩阵的乘积的迹等于零，因此
Figure FDA00026854978800000324
其中，
Figure FDA00026854978800000325
在S上的内积<·,·>定义如下：
<A,B>＝tr(ATB) (27)
其中，A,B∈S.
在S上黎曼度量<·,·>R定义如下：
Figure FDA00026854978800000422
将公式(27)带入公式(26)得：
Figure FDA0002685497880000041
将公式(28)带入公式(29)得：
Figure FDA0002685497880000042
Figure FDA0002685497880000043
的切映射和
Figure FDA0002685497880000044
的梯度有以下的关系:
Figure FDA0002685497880000045
其中，
Figure FDA0002685497880000046
是
Figure FDA0002685497880000047
的梯度；
由于公式(29)和(31)左边是相等的，所有右边也是相等的，因此：
Figure FDA0002685497880000048
选择姿态误差函数的梯度
Figure FDA0002685497880000049
作为新的创新项补偿姿态特殊正交群,这使得姿态估计沿梯度下降的方向传播，并最终使姿态特殊正交群收敛；基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准算法归纳为：
Figure FDA00026854978800000410
步骤(4)：求解导航系统所需的姿态矩阵
Figure FDA00026854978800000411
从而完成晃动基座粗对准过程：
根据求解得到的姿态变化矩阵
Figure FDA00026854978800000412
和
Figure FDA00026854978800000414
信息，通过公式(2)即可求解导航姿态矩阵，完成SINS捷联惯性导航系统晃动基座粗对准。
2.根据权利要求1所述的基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法，其特征在于，步骤(2)中基于特殊正交群微分方程，建立基于特殊正交群描述的线性晃动基座粗对准系统模型；
步骤(2.1)：通过
Figure FDA00026854978800000415
更新计算
Figure FDA00026854978800000416
基于特殊正交群描述的姿态矩阵链式法则，姿态矩阵
Figure FDA00026854978800000417
分解为：
Figure FDA00026854978800000418
由于
Figure FDA00026854978800000419
表示导航系相对于惯性系的旋转角速率，且
Figure FDA00026854978800000421
变化通常是十分缓慢的，根据基于特殊正交群描述的姿态矩阵更新求解过程，tk-1时刻到tk时刻的姿态矩阵
Figure FDA00026854978800000420
可以近似为：
Figure FDA0002685497880000051
其中，
Figure FDA0002685497880000052
根据公式(25)-(27)，姿态矩阵
Figure FDA0002685497880000053
迭代过程可以近似为：
Figure FDA0002685497880000054
步骤(2.2)：通过陀螺仪输出的角速率信息
Figure FDA0002685497880000055
更新计算
Figure FDA0002685497880000056
tk-1时刻到tk时刻的姿态矩阵
Figure FDA0002685497880000057
可以近似为：
Figure FDA0002685497880000058
其中，根据双子样旋转矢量法，可得：
Figure FDA0002685497880000059
其中，Δθ1和Δθ2分别表示两个相邻半采样周期
Figure FDA00026854978800000510
内由陀螺仪输出计算所得的角增量；
根据公式(29)-(30)，姿态矩阵
Figure FDA00026854978800000511
迭代过程近似为：
Figure FDA00026854978800000512
步骤(2.3)：建立基于特殊正交群描述的关于初始旋转矩阵
Figure FDA00026854978800000513
的粗对准系统模型方式为：
在晃动基座条件下，gn和gb之间存在如下关系：
Figure FDA00026854978800000514
对公式(32)进行移项、整理操作，可得：
Figure FDA00026854978800000515
公式(34)可以表示为：
Figure FDA00026854978800000516
其中，
Figure FDA00026854978800000517
表示重力加速度在初始导航系下的投影，
Figure FDA00026854978800000518
表示重力加速度在初始载体系下的投影；
对公式(35)在[0,t]上做积分，得量测方程如下：
Figure FDA00026854978800000519
公式(36)表示成如下形式：
y(t)＝Rx(t) (45)
其中，
Figure FDA00026854978800000520
Figure FDA00026854978800000521
Figure FDA0002685497880000061
由于姿态矩阵
Figure FDA0002685497880000062
为常值矩阵，因此建立晃动基座粗对准系统的线性状态方程如下：
Figure FDA0002685497880000063
根据上述内容，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，建立具有特殊正交群结构的晃动基座粗对准系统方程，表示为：
Figure FDA0002685497880000064
3.根据权利要求1所述的基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法，其特征在于，步骤(3)中使用特殊正交群最优估计算法直接估计
Figure FDA0002685497880000065
采用新的姿态误差函数的梯度作为创新项，对姿态进行实时补偿，使系统快速收敛；晃动基座粗对准整体过程如下：
Figure FDA0002685497880000066
4.根据权利要求1所述的基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法，其特征在于，步骤(4)中求解姿态矩阵
Figure FDA0002685497880000067
并解算姿态信息；
在步骤(2)中，将SINS粗对准过程中
Figure FDA0002685497880000068
的求解问题转化为对
Figure FDA0002685497880000069
的求解问题，并将
Figure FDA00026854978800000610
分解为三个矩阵乘积的形式：
Figure FDA00026854978800000611
根据公式(28)、公式(31)和公式(47)求解得到的
Figure FDA00026854978800000612
和
Figure FDA00026854978800000613
姿态矩阵
Figure FDA00026854978800000614
的求解方式为：
Figure FDA00026854978800000615
并根据求解得到的姿态矩阵
Figure FDA00026854978800000616
解算姿态信息。
CN202010975160.3A 2020-09-16 2020-09-16 基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法 Pending CN112212889A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010975160.3A CN112212889A (zh) 2020-09-16 2020-09-16 基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010975160.3A CN112212889A (zh) 2020-09-16 2020-09-16 基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法

Publications (1)

Publication Number Publication Date
CN112212889A true CN112212889A (zh) 2021-01-12

Family

ID=74049506

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010975160.3A Pending CN112212889A (zh) 2020-09-16 2020-09-16 基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法

Country Status (1)

Country Link
CN (1) CN112212889A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113672137A (zh) * 2021-08-17 2021-11-19 北京字节跳动网络技术有限公司 光标位置更新方法、装置和电子设备
CN114329943A (zh) * 2021-12-23 2022-04-12 哈尔滨工业大学（深圳） 基于姿态旋转矩阵的控制性能边界设计方法、装置及介质

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106871928A (zh) * 2017-01-18 2017-06-20 北京工业大学 基于李群滤波的捷联惯性导航初始对准方法
CN107588771A (zh) * 2017-08-28 2018-01-16 北京工业大学 基于李群描述的捷联惯性导航解算方法
CN108921895A (zh) * 2018-06-12 2018-11-30 中国人民解放军军事科学院国防科技创新研究院 一种传感器相对位姿估计方法
CN109931957A (zh) * 2019-03-24 2019-06-25 北京工业大学 基于lgmkf的sins捷联惯性导航系统自对准方法
CN109931955A (zh) * 2019-03-18 2019-06-25 北京工业大学 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN110595503A (zh) * 2019-08-05 2019-12-20 北京工业大学 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN110702143A (zh) * 2019-10-19 2020-01-17 北京工业大学 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN110926499A (zh) * 2019-10-19 2020-03-27 北京工业大学 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN111399023A (zh) * 2020-04-20 2020-07-10 中国人民解放军国防科技大学 基于李群非线性状态误差的惯性基组合导航滤波方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106871928A (zh) * 2017-01-18 2017-06-20 北京工业大学 基于李群滤波的捷联惯性导航初始对准方法
CN107588771A (zh) * 2017-08-28 2018-01-16 北京工业大学 基于李群描述的捷联惯性导航解算方法
CN108921895A (zh) * 2018-06-12 2018-11-30 中国人民解放军军事科学院国防科技创新研究院 一种传感器相对位姿估计方法
CN109931955A (zh) * 2019-03-18 2019-06-25 北京工业大学 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN109931957A (zh) * 2019-03-24 2019-06-25 北京工业大学 基于lgmkf的sins捷联惯性导航系统自对准方法
CN110595503A (zh) * 2019-08-05 2019-12-20 北京工业大学 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN110702143A (zh) * 2019-10-19 2020-01-17 北京工业大学 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN110926499A (zh) * 2019-10-19 2020-03-27 北京工业大学 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN111399023A (zh) * 2020-04-20 2020-07-10 中国人民解放军国防科技大学 基于李群非线性状态误差的惯性基组合导航滤波方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
HAO XU: "An In-Motion Initial Alignment Algorithm for SINS Using Adaptive Lie Group Filter", PROCEEDINGS OF THE 37TH CHINESE CONTROL CONFERENCE *
徐浩: "一种基于李群描述的深空探测器姿态估计方法", 深空探测学报纸, vol. 7, no. 1 *
裴福俊: "动态干扰条件下的旋转式捷联 惯导系统自对准方法", 自动化学报, vol. 40, no. 9 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113672137A (zh) * 2021-08-17 2021-11-19 北京字节跳动网络技术有限公司 光标位置更新方法、装置和电子设备
CN114329943A (zh) * 2021-12-23 2022-04-12 哈尔滨工业大学（深圳） 基于姿态旋转矩阵的控制性能边界设计方法、装置及介质

Similar Documents

Publication Publication Date Title
CN110702143B (zh) 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN106871928B (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN110595503B (zh) 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN109931957B (zh) 基于lgmkf的sins捷联惯性导航系统自对准方法
CN107588771B (zh) 基于李群描述的捷联惯性导航解算方法
JP5068531B2 (ja) 測定及び記憶された重力傾度を用いて慣性航法測定値の精度を改善する方法及びシステム
CN106767797B (zh) 一种基于对偶四元数的惯性/gps组合导航方法
CN105698822B (zh) 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法
CN110440830A (zh) 动基座下车载捷联惯导系统自对准方法
CN111895988A (zh) 无人机导航信息更新方法及装置
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
Xue et al. In-motion alignment algorithm for vehicle carried SINS based on odometer aiding
CN112212889A (zh) 基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法
CN110285815A (zh) 一种可在轨全程应用的微纳卫星多源信息姿态确定方法
CN113503892B (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN110926499A (zh) 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN108627152A (zh) 一种微型无人机基于多传感器数据融合的导航方法
CN110388942B (zh) 一种基于角度和速度增量的车载姿态精对准系统
CN112902956A (zh) 一种手持式gnss/mems-ins接收机航向初值获取方法、电子设备、存储介质
CN111207773B (zh) 一种用于仿生偏振光导航的姿态无约束优化求解方法
CN111156986A (zh) 一种基于抗差自适应ukf的光谱红移自主组合导航方法
CN112229421B (zh) 基于李群最优估计的捷联惯性导航晃动基座粗对准方法
Chang et al. Pseudo open-loop unscented quaternion estimator for attitude estimation
CN105606093B (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
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20210112