CN108592943B - 一种基于opreq方法的惯性系粗对准计算方法 - Google Patents

一种基于opreq方法的惯性系粗对准计算方法 Download PDF

Info

Publication number
CN108592943B
CN108592943B CN201810217685.3A CN201810217685A CN108592943B CN 108592943 B CN108592943 B CN 108592943B CN 201810217685 A CN201810217685 A CN 201810217685A CN 108592943 B CN108592943 B CN 108592943B
Authority
CN
China
Prior art keywords
matrix
steps
attitude
follows
vector
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
CN201810217685.3A
Other languages
English (en)
Other versions
CN108592943A (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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN201810217685.3A priority Critical patent/CN108592943B/zh
Publication of CN108592943A publication Critical patent/CN108592943A/zh
Application granted granted Critical
Publication of CN108592943B publication Critical patent/CN108592943B/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
    • 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

本发明公开了一种基于OPREQ方法的惯性系粗对准计算方法，将初始姿态矩阵的计算转换为常值姿态矩阵的确定，采用OPREQ方法来求解姿态矩阵所对应的最优四元数。本发明能够通过自适应调节增益大小，能有效地对观测矢量中的噪声进行滤波处理，从而达到提高粗对准收敛性能的效果，作为一种静基座及摇摆基座粗对准方法，具有很好的工程参考和应用价值。

Description

一种基于OPREQ方法的惯性系粗对准计算方法
技术领域
本发明属于捷联惯性导航系统初始对准技术，具体涉及一种基于OPREQ方法的惯性系粗对准计算方法。
背景技术
捷联惯性导航系统(SINS)是利用惯性传感器的测量来计算载体相对于初始点的位置和方位的一种自主系统。因此，用于求解初始姿态的初始对准技术的发展对实现高精度导航具有重要意义。初始对准的有两个重要的性能指标，一个是对准精度，另一个是收敛速度。如何在较短的时间内获得较高精度的初始姿态是惯性导航领域的一个重要的研究热点。
根据定位过程中，初始对准一般分为两个阶段。第一阶段称为粗对准过程，粗对准主要利用地球引力和地球自转角速度来确定一个粗略的初始姿态矩阵，粗对准的贡献主要体现在对准速度上。因此，一种有效的粗对准方法可以减少对准时间，从而使系统快速进入导航状态。第二阶段是精对准过程，精对准是在粗对准的基础上更准确地确定初始姿态。在粗对准过程中，失准角可以收敛到一个小角度范围，从而使捷联惯性导航系统的非线性误差模型近似简化为线性误差模型。然后，在精对准阶段就可以采用线性卡尔曼滤波器来获取精确的初始姿态。另外在采用线性卡尔曼滤波器进行精对准过程中，可以估计出惯性传感器的偏置，从而进一步减小失调角。因此，粗对准过程是精对准的前提和基础，且粗对准的性能将直接影响精对准的结果。在实际应用中，设计具有收敛速度快、对准精度高的粗对准算法具有重要意义。
惯性系粗对准方法将初始对准问题总结为姿态确定问题，姿态确定问题一般有两种解决方案，一种是直接计算姿态矩阵，例如双矢量定姿方法，但该方法精度差，且更新率较低；另一种是求解姿态矩阵对应的姿态四元数，例如REQUEST算法，该方法通过迭代构造K矩阵的方式简化了算法计算量，且K矩阵最大特征值对应的特征向量即为姿态四元数；但该方法的权值是固定的，使得该方法在整个姿态确定过程中无法取得最优的效果。
发明内容
发明目的：本发明的目的在于解决现有技术中存在的由于惯性传感器获取的观测向量通常含有各种噪声，影响了粗对准的收敛速度和精度的不足，提供一种基于OPREQ方法的惯性系粗对准计算方法。
技术方案：本发明提供一种基于OPREQ方法的惯性系粗对准计算方法，通过将初始姿态矩阵
Figure BDA0001598981760000021
的计算转化为常值姿态矩阵
Figure BDA0001598981760000022
的确定，其包括如下步骤：
(1)获取传感器实时数据；
(2)根据地球自转角速度及载体纬度数据，计算出坐标变换矩阵
Figure BDA0001598981760000023
及
Figure BDA0001598981760000024
(3)根据步骤(1)得到的陀螺仪输出数据更新计算坐标变换矩阵
Figure BDA0001598981760000025
(4)根据步骤(2)、(3)得到的坐标变换矩阵、步骤(1)得到的加速度计输出数据、及地球重力矢量，计算出参考矢量及观测矢量；
(5)根据步骤(4)中得到的参考矢量及观测矢量，利用OPREQ方法求解量测更新矩阵Kk+1/k+1；
(6)根据步骤(5)中得到的量测更新矩阵，求解常值姿态矩阵
Figure BDA0001598981760000026
对应的最优姿态四元数；
(7)根据步骤(2)、(3)、(6)中得到的坐标变换矩阵，利用矩阵链式法则计算初始姿态矩阵
Figure BDA0001598981760000027
(8)重复步骤(1)到(7)，实时更新计算初始姿态矩阵直至对准时间结束。
进一步地，初始姿态矩阵
Figure BDA0001598981760000028
的计算转化为常值姿态矩阵
Figure BDA0001598981760000029
的确定，其具体的计算步骤如下：
(1.1)将初始姿态矩阵
Figure BDA00015989817600000210
分解为四个坐标变换矩阵：
Figure BDA00015989817600000211
其中，e0系是e坐标系在初始时刻相对地球自转保持静止的惯性系，b0系是b坐标系在初始时刻相对地球自转保持静止的惯性系；
(1.2)由步骤(1.1)得，导航坐标系n系与地心地球坐标系e系之间的坐标转换矩阵
Figure BDA00015989817600000212
为：
Figure BDA00015989817600000213
其中，L表示载体所在位置的纬度。
(1.3)由步骤(1.1)得，地心地球坐标系e系与地心惯性坐标系e0系之间的坐标变换矩阵
Figure BDA00015989817600000214
为：
Figure BDA00015989817600000215
其中，ωie表示地球自转角速度。
(1.4)由步骤(1.1)得，载体坐标系b系与b0系之间的姿态矩阵可以利用陀螺仪的输出实时计算，即求解如下的姿态矩阵微分方程：
Figure BDA0001598981760000031
其中，
Figure BDA0001598981760000032
表示载体系相对惯性系的旋转角速度在载体系下的投影，可用陀螺仪测量得到；I3表示3阶单位矩阵。
(1.5)由步骤(1.1)、(1.2)、(1.3)、(1.4)得，计算初始姿态矩阵
Figure BDA0001598981760000033
就转化为求解b0系和e0系之间的常值姿态矩阵
Figure BDA0001598981760000034
进一步地根据所述步骤(4)构造惯性系粗对准观测模型，其具体步骤如下：
(2.1)当载体没有线运动的情况下，加速度计的输出fb可以看作由三部分信息组成：重力加速度矢量在b系的投影gb，加速度计偏置
Figure BDA0001598981760000035
和外界干扰加速度ab，即：
Figure BDA0001598981760000036
(2.2)由步骤(2.1)得，fb在b0系中的投影可表示为：
Figure BDA0001598981760000037
(2.3)步骤(2.2)中参数ge0表示地球重力矢量在e0系下的投影，具体计算如下：
Figure BDA0001598981760000038
(2.4)由步骤(2.2)得，为抑制干扰加速度的影响，对上式两端积分：
Figure BDA0001598981760000039
(2.5)由步骤(2.4)得，记
Figure BDA00015989817600000310
并忽略上式右侧第二项可得：
Figure BDA00015989817600000311
其中，
Figure BDA00015989817600000312
(2.6)由步骤(2.5)得，将矢量Vb0(t)和Ve0(t)单位化处理，分别记为b和r，
Figure BDA00015989817600000313
(2.7)由步骤(2.6)得，惯性系粗对准观测模型可表示为：
Figure BDA0001598981760000041
进一步地，所述步骤(5)和步骤(6)中采用OPREQ算法求解常值姿态矩阵
Figure BDA0001598981760000042
所对应的姿态四元数，其具体步骤如下：
(3.1)设一组tk时刻获得的矢量表示为bi,ri,i＝1,2,…,n，对应的权值为ai，其中，
Figure BDA0001598981760000043
定义4×4对称矩阵K如下：
Figure BDA0001598981760000044
其中3×3矩阵Sk，列向量zk，以及标量σk分别定义如下：
Figure BDA0001598981760000045
Figure BDA0001598981760000046
其中tr(·)表示欧几里得范数；
(3.2)设tk+1时刻获得的矢量记为bk+1,rk+1，相应的权值记为ak+1。则对应tk+1时刻的矩阵K定义如下，并用δKk+1表示：
Figure BDA0001598981760000047
其中3阶矩阵Sk+1，列向量zk+1，及标量σk+1分别定义如下：
Figure BDA0001598981760000048
Figure BDA0001598981760000049
(3.3)由步骤(3.1)得，定义4阶矩阵
Figure BDA00015989817600000410
为：
Figure BDA00015989817600000411
其中，
Figure BDA00015989817600000412
是3阶子矩阵，表达式
Figure BDA00015989817600000413
和
Figure BDA00015989817600000414
分别计算如下：
Figure BDA00015989817600000415
Figure BDA00015989817600000416
Figure BDA00015989817600000417
Figure BDA00015989817600000418
其中，μk是观测矢量bk中所包含误差的标准差；
(3.4)初始化设置参数矩阵K0/0、方差矩阵P、及比例系数m0分别为：
K0/0＝δK0
Figure BDA00015989817600000419
m0＝δm0＝1
(3.5)由步骤(3.3)、(3.4)得，量测更新方程的权值计算如下：
Figure BDA0001598981760000051
(3.6)由步骤(3.5)得，比例系数更新方程计算如下：
Figure BDA0001598981760000052
(3.7)由步骤(3.1)、(3.2)、(3.5)、(3.6)得，量测更新方程计算如下：
Figure BDA0001598981760000053
(3.8)由步骤(3.3)、(3.4)、(3.5)、(3.6)得，方程矩阵更新方程计算如下：
Figure BDA0001598981760000054
(3.9)由步骤(3.5)、(3.6)、(3.7)、(3.8)得，可以迭代计算得到tk+1时刻的Kk+1/k+1矩阵，而Kk+1/k+1矩阵最大特征值对应的特征向量即为常值姿态矩阵
Figure BDA0001598981760000055
对应的姿态四元数。
进一步地，利用计算得到的常值姿态矩阵
Figure BDA0001598981760000056
所对应的姿态四元数求解初始姿态矩阵及初始姿态角，其具体步骤如下：
(4.1)由计算得到的姿态四元数，对应的常值姿态矩阵
Figure BDA0001598981760000057
计算如下：
Figure BDA0001598981760000058
其中，q＝[q0 q1 q2 q3]T表示姿态四元数。
(4.2)初始姿态矩阵可计算如下：
Figure BDA0001598981760000059
其中，θ,γ,ψ分别表示载体的纵摇角、横摇角及航向角。
(4.3)由步骤(4.2)得，可以根据初始姿态矩阵
Figure BDA00015989817600000510
计算得到欧拉角：
Figure BDA0001598981760000061
有益效果：本发明与现有技术相比，具备如下优点：
(1)本发明采用OPREQ算法来计算惯性系粗对准中的常值姿态矩阵，能够根据观测噪声自适应调节滤波增益，使得该粗对准方法收敛速度更快且收敛结果更加稳定。
(2)本发明在求解滤波器增益及量测更新时，采用迭代计算的方式，减小了对准算法的计算量，提高了对准算法的实时性。
附图说明
图1为本发明的算法整体流程图；
图2为本发明粗对准纵摇角姿态误差曲线图；
图3为本发明粗对准横摇角姿态误差曲线图；
图4为本发明粗对准航向角姿态误差曲线图。
具体实施方式
本发明提供一种基于OPREQ方法的惯性系粗对准计算方法，其包括如下步骤：
(1)获取传感器实时数据；
(2)根据地球自转角速度及载体纬度数据，计算出坐标变换矩阵
Figure BDA0001598981760000062
及
Figure BDA0001598981760000063
(3)根据步骤(1)得到的陀螺仪输出数据更新计算坐标变换矩阵
Figure BDA0001598981760000064
(4)根据步骤(2)、(3)得到的坐标变换矩阵、步骤(1)得到的加速度计输出数据、及地球重力矢量，计算出参考矢量及观测矢量；
(5)根据步骤(4)中得到的参考矢量及观测矢量，利用OPREQ方法求解量测更新矩阵Kk+1/k+1；
(6)根据步骤(5)中得到的量测更新矩阵，求解常值姿态矩阵
Figure BDA0001598981760000065
对应的最优姿态四元数；
(7)根据步骤(2)、(3)、(6)中得到的坐标变换矩阵，利用矩阵链式法则计算初始姿态矩阵
Figure BDA0001598981760000066
(8)重复步骤(1)到(7)，实时更新计算初始姿态矩阵直至对准时间结束。
如图1所示，初始姿态矩阵
Figure BDA0001598981760000067
的计算转化为常值姿态矩阵
Figure BDA0001598981760000068
的确定，其具体的算法步骤如下：
(1.1)将初始姿态矩阵
Figure BDA0001598981760000069
分解为四个坐标变换矩阵：
Figure BDA0001598981760000071
其中，e0系是e坐标系在初始时刻相对地球自转保持静止的惯性系，b0系是b坐标系在初始时刻相对地球自转保持静止的惯性系；
(1.2)由步骤(1.1)得，导航坐标系n系与地心地球坐标系e系之间的坐标转换矩阵
Figure BDA0001598981760000072
为：
Figure BDA0001598981760000073
其中，L表示载体所在位置的纬度。
(1.3)由步骤(1.1)得，地心地球坐标系e系与地心惯性坐标系e0系之间的坐标变换矩阵
Figure BDA0001598981760000074
为：
Figure BDA0001598981760000075
(1.4)由步骤(1.1)得，载体坐标系b系与b0系之间的姿态矩阵可以利用陀螺仪的输出实时计算，即求解如下的姿态矩阵微分方程：
Figure BDA0001598981760000076
其中，
Figure BDA0001598981760000077
表示载体系相对惯性系的旋转角速度在载体系下的投影，可用陀螺仪测量得到；I3表示3阶单位矩阵。
(1.5)由步骤(1.1)、(1.2)、(1.3)、(1.4)得，计算初始姿态矩阵
Figure BDA0001598981760000078
就转化为求解b0系和e0系之间的常值姿态矩阵
Figure BDA0001598981760000079
进一步地根据所述步骤(4)构造惯性系粗对准观测模型，其具体步骤如下：
(2.1)当载体没有线运动的情况下，加速度计的输出fb可以看作由三部分信息组成：重力加速度矢量在b系的投影gb，加速度计偏置
Figure BDA00015989817600000710
和外界干扰加速度ab，即：
Figure BDA00015989817600000711
(2.2)由步骤(2.1)得，fb在b0系中的投影可表示为：
Figure BDA00015989817600000712
(2.3)步骤(2.2)中参数ge0表示地球重力矢量在e0系下的投影，具体计算如下：
Figure BDA00015989817600000713
(2.4)由步骤(2.2)得，为抑制干扰加速度的影响，对上式两端积分：
Figure BDA0001598981760000081
(2.5)由步骤(2.4)得，记
Figure BDA0001598981760000082
并忽略上式右侧第二项可得：
Figure BDA0001598981760000083
其中，
Figure BDA0001598981760000084
(2.6)由步骤(2.5)得，将矢量Vb0(t)和Ve0(t)单位化处理，分别记为b和r，
Figure BDA0001598981760000085
(2.7)由步骤(2.6)得，惯性系粗对准观测模型可表示为：
Figure BDA0001598981760000086
所述步骤(5)和步骤(6)中采用OPREQ算法求解常值姿态矩阵
Figure BDA0001598981760000087
所对应的姿态四元数，其具体步骤如下：
(3.1)设一组tk时刻获得的矢量表示为bi,ri,i＝1,2,…,n，对应的权值为ai，其中，
Figure BDA0001598981760000088
定义4×4对称矩阵K如下：
Figure BDA0001598981760000089
其中3×3矩阵Sk，列向量zk，以及标量σk分别定义如下：
Figure BDA00015989817600000810
Figure BDA00015989817600000811
其中tr(·)表示欧几里得范数；
(3.2)设tk+1时刻获得的矢量记为bk+1,rk+1，相应的权值记为ak+1。则对应tk+1时刻的矩阵K定义如下，并用δKk+1表示：
Figure BDA00015989817600000812
其中3阶矩阵Sk+1，列向量zk+1，及标量σk+1分别定义如下：
Figure BDA0001598981760000091
Figure BDA0001598981760000092
(3.3)由步骤(3.1)得，定义4阶矩阵
Figure BDA0001598981760000093
为：
Figure BDA0001598981760000094
其中，
Figure BDA0001598981760000095
是3阶子矩阵，表达式
Figure BDA0001598981760000096
和
Figure BDA0001598981760000097
分别计算如下：
Figure BDA0001598981760000098
Figure BDA0001598981760000099
Figure BDA00015989817600000910
Figure BDA00015989817600000911
其中，μk是观测矢量bk中所包含误差的标准差；
(3.4)初始化设置参数矩阵K0/0、方差矩阵P、及比例系数m0分别为：
K0/0＝δK0
Figure BDA00015989817600000912
m0＝δm0＝1
(3.5)由步骤(3.3)、(3.4)得，量测更新方程的权值计算如下：
Figure BDA00015989817600000913
(3.6)由步骤(3.5)得，比例系数更新方程计算如下：
Figure BDA00015989817600000914
(3.7)由步骤(3.1)、(3.2)、(3.5)、(3.6)得，量测更新方程计算如下：
Figure BDA00015989817600000915
(3.8)由步骤(3.3)、(3.4)、(3.5)、(3.6)得，方程矩阵更新方程计算如下：
Figure BDA00015989817600000916
(3.9)由步骤(3.5)、(3.6)、(3.7)、(3.8)得，可以迭代计算得到tk+1时刻的Kk+1/k+1矩阵，而Kk+1/k+1矩阵最大特征值对应的特征向量即为常值姿态矩阵
Figure BDA00015989817600000917
对应的姿态四元数。
利用计算得到的常值姿态矩阵
Figure BDA00015989817600000918
所对应的姿态四元数求解初始姿态矩阵及初始姿态角，其具体步骤如下：
(4.1)由计算得到的姿态四元数，对应的常值姿态矩阵
Figure BDA0001598981760000101
计算如下：
Figure BDA0001598981760000102
其中，q＝[q0 q1 q2 q3]T表示姿态四元数。
(4.2)初始姿态矩阵可计算如下：
Figure BDA0001598981760000103
其中，θ,γ,ψ分别表示载体的纵摇角、横摇角及航向角。
(4.3)由步骤(4.2)得，可以根据初始姿态矩阵
Figure BDA0001598981760000104
计算得到欧拉角：
Figure BDA0001598981760000105
本实施例将本发明提出的一种基于OPREQ方法的惯性系粗对准计算方法通过MATLAB仿真软件进行仿真实验效果,从而证明观测矢量存在噪声时，本发明在粗对准快速性方面的优势。
仿真实验中惯性传感器的性能指标设置如下：陀螺常值漂移：0.01°/h；陀螺随机漂移：0.01°/h；加速度计常值偏置：50μg；加速度计随机偏置：50μg。经纬度设置为：纬度32°(N)，经度118°(E)。
仿真实验在摇摆基座下进行，三轴摇摆的运动方式设置为：内框摆幅为3°，频率为0.15Hz；中框摆幅为3°，频率为0.2Hz；外框摆幅为2°，频率为0.125Hz。三轴同时进行摇摆运动，以此运动状态模拟舰船实际的应用环境，仿真实验进行200s。
图2-4显示了上述实施例基于OPREQ方法的惯性系粗对准计算方法的三个姿态角的误差曲线，
图2-图4中纵轴分别表示纵摇角误差、横摇角误差及航向角误差，单位为度；横轴均为时间，单位为秒。
从图2-图3中可知，粗对准水平角误差保持在极限对准精度范围内；且从图4中航向角误差曲线中可以看出，对准时间20s以后，航向角误差保持在0.05°范围以内，并在对准时间40s以后，航向角误差一直稳定在0.03度左右；相比于传统粗对准方法，该粗对准方法收敛速度明显加快。实验结果表明本发明能够有效提高粗对准收敛速度，并使得收敛结果更加稳定。

Claims (4)

1.一种基于OPREQ方法的惯性系粗对准计算方法，其特征在于：通过将初始姿态矩阵
Figure FDA0002389172800000011
的计算转化为常值姿态矩阵
Figure FDA0002389172800000012
的确定，其包括如下步骤：
(1)获取传感器实时数据；
(2)根据地球自转角速度及载体纬度数据，计算出坐标变换矩阵
Figure FDA0002389172800000013
及
Figure FDA0002389172800000014
(3)根据步骤(1)得到的陀螺仪输出数据更新计算坐标变换矩阵
Figure FDA0002389172800000015
(4)根据步骤(2)、(3)得到的坐标变换矩阵、步骤(1)得到的加速度计输出数据、及地球重力矢量，计算出参考矢量及观测矢量；
(5)根据步骤(4)中得到的参考矢量及观测矢量，利用OPREQ方法求解量测更新矩阵Kk+1/k+1；
(6)根据步骤(5)中得到的量测更新矩阵，求解常值姿态矩阵
Figure FDA0002389172800000016
对应的最优姿态四元数；
(7)根据步骤(2)、(3)、(6)中得到的坐标变换矩阵，利用矩阵链式法则计算初始姿态矩阵
Figure FDA0002389172800000017
(8)重复步骤(1)到(7)，实时更新计算初始姿态矩阵直至对准时间结束；
所述步骤(5)和步骤(6)中采用OPREQ算法求解常值姿态矩阵
Figure FDA0002389172800000018
所对应的姿态四元数，其具体步骤如下：
(3.1)设一组tk时刻获得的矢量表示为bi，ri，i＝1，2，...，n，对应的权值为ai，
其中，
Figure FDA0002389172800000019
定义4×4对称矩阵K如下：
Figure FDA00023891728000000110
其中3×3矩阵Sk，列向量zk，以及标量σk分别定义如下：
Figure FDA00023891728000000111
Figure FDA00023891728000000112
其中tr(·)表示欧几里得范数；
(3.2)设tk+1时刻获得的矢量记为bk+1，rk+1，相应的权值记为ak+1， 则对应tk+1时刻的矩阵K定义如下，并用δKk+1表示：
Figure FDA00023891728000000113
其中3阶矩阵Sk+1，列向量zk+1，及标量σk+1分别定义如下：
Figure FDA00023891728000000114
Figure FDA00023891728000000115
(3.3)由步骤(3.1)得，定义4阶矩阵
Figure FDA0002389172800000021
为：
Figure FDA0002389172800000022
其中，
Figure FDA0002389172800000023
是3阶子矩阵，表达式
Figure FDA0002389172800000024
和
Figure FDA0002389172800000025
分别计算如下：
Figure FDA0002389172800000026
Figure FDA0002389172800000027
Figure FDA0002389172800000028
Figure FDA0002389172800000029
其中，μk是观测矢量bk中所包含误差的标准差；
(3.4)初始化设置参数矩阵K0/0、方差矩阵P、及比例系数m0分别为：
K0/0＝δK0
Figure FDA00023891728000000210
m0＝δm0＝1
(3.5)由步骤(3.3)、(3.4)得，量测更新方程的权值计算如下：
Figure FDA00023891728000000211
(3.6)由步骤(3.5)得，比例系数更新方程计算如下：
Figure FDA00023891728000000212
(3.7)由步骤(3.1)、(3.2)、(3.5)、(3.6)得，量测更新方程计算如下：
Figure FDA00023891728000000213
(3.8)由步骤(3.3)、(3.4)、(3.5)、(3.6)得，方程矩阵更新方程计算如下：
Figure FDA00023891728000000214
(3.9)由步骤(3.5)、(3.6)、(3.7)、(3.8)得，可以迭代计算得到tk+1时刻的Kk+1/k+1矩阵，而Kk+1/k+1矩阵最大特征值对应的特征向量即为常值姿态矩阵
Figure FDA00023891728000000215
对应的姿态四元数。
2.根据权利要求1所述的一种基于OPREQ方法的惯性系粗对准计算方法，其特征在于：初始姿态矩阵
Figure FDA00023891728000000216
的计算转化为常值姿态矩阵
Figure FDA00023891728000000217
的确定，其具体的计算步骤如下：
(1.1)将初始姿态矩阵
Figure FDA0002389172800000031
分解为四个坐标变换矩阵：
Figure FDA0002389172800000032
其中，e0系是e坐标系在初始时刻相对地球自转保持静止的惯性系，b0系是b坐标系在初始时刻相对地球自转保持静止的惯性系；
(1.2)由步骤(1.1)得，导航坐标系n系与地心地球坐标系e系之间的坐标转换矩阵
Figure FDA0002389172800000033
为：
Figure FDA0002389172800000034
其中，L表示载体所在位置的纬度；
(1.3)由步骤(1.1)得，地心地球坐标系e系与地心惯性坐标系e0系之间的坐标变换矩阵
Figure FDA0002389172800000035
为：
Figure FDA0002389172800000036
其中，ωie表示地球自转角速度；
(1.4)由步骤(1.1)得，载体坐标系b系与b0系之间的姿态矩阵可以利用陀螺仪的输出实时计算，即求解如下的姿态矩阵微分方程：
Figure FDA0002389172800000037
其中，
Figure FDA0002389172800000038
表示载体系相对惯性系的旋转角速度在载体系下的投影；I3表示3阶单位矩阵；
(1.5)由步骤(1.1)、(1.2)、(1.3)、(1.4)得，计算初始姿态矩阵
Figure FDA0002389172800000039
就转化为求解b0系和e0系之间的常值姿态矩阵
Figure FDA00023891728000000310
3.根据权利要求1所述的一种基于OPREQ方法的惯性系粗对准计算方法，其特征在于：根据所述步骤(4)构造惯性系粗对准观测模型，其具体步骤如下：
(2.1)当载体没有线运动的情况下，加速度计的输出fb可以看作由三部分信息组成：重力加速度矢量在b系的投影gb，加速度计偏置
Figure FDA00023891728000000313
和外界干扰加速度ab，即：
Figure FDA00023891728000000311
(2.2)由步骤(2.1)得，fb在b0系中的投影可表示为：
Figure FDA00023891728000000312
(2.3)步骤(2.2)中参数ge0表示地球重力矢量在e0系下的投影，具体计算如下：
Figure FDA0002389172800000041
(2.4)由步骤(2.2)得，为抑制干扰加速度的影响，对上式两端积分：
Figure FDA0002389172800000042
(2.5)由步骤(2.4)得，记
Figure FDA0002389172800000043
并忽略上式右侧第二项可得：
Figure FDA0002389172800000044
其中，
Figure FDA0002389172800000045
(2.6)由步骤(2.5)得，将矢量Vb0(t)和Ve0(t)单位化处理，分别记为b和r，
Figure FDA0002389172800000046
(2.7)由步骤(2.6)得，惯性系粗对准观测模型可表示为：
Figure FDA0002389172800000047
4.根据权利要求1所述的一种基于OPREQ方法的惯性系粗对准计算方法，其特征在于：利用计算得到的常值姿态矩阵
Figure FDA0002389172800000048
所对应的姿态四元数求解初始姿态矩阵及初始姿态角，其具体步骤如下：
(4.1)由计算得到的姿态四元数，对应的常值姿态矩阵
Figure FDA0002389172800000049
计算如下：
Figure FDA00023891728000000410
其中，q＝[q0 q1 q2 q3]T表示姿态四元数；
(4.2)初始姿态矩阵可计算如下：
Figure FDA00023891728000000411
其中，θ，γ，ψ分别表示载体的纵摇角、横摇角及航向角；
(4.3)由步骤(4.2)得，可以根据初始姿态矩阵
Figure FDA0002389172800000051
计算得到欧拉角：
Figure FDA0002389172800000052
CN201810217685.3A 2018-03-16 2018-03-16 一种基于opreq方法的惯性系粗对准计算方法 Active CN108592943B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810217685.3A CN108592943B (zh) 2018-03-16 2018-03-16 一种基于opreq方法的惯性系粗对准计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810217685.3A CN108592943B (zh) 2018-03-16 2018-03-16 一种基于opreq方法的惯性系粗对准计算方法

Publications (2)

Publication Number Publication Date
CN108592943A CN108592943A (zh) 2018-09-28
CN108592943B true CN108592943B (zh) 2020-06-02

Family

ID=63626553

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810217685.3A Active CN108592943B (zh) 2018-03-16 2018-03-16 一种基于opreq方法的惯性系粗对准计算方法

Country Status (1)

Country Link
CN (1) CN108592943B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110133702B (zh) * 2019-05-13 2022-12-27 桂林电子科技大学 一种基于正交变换的姿态测量方法和设备
CN111397603B (zh) * 2020-04-24 2022-07-12 东南大学 载体姿态动态情况下的惯性/多普勒动基座粗对准方法
CN112013872A (zh) * 2020-08-13 2020-12-01 哈尔滨工业大学 一种基于特征值分解的静基座自对准方法
CN112747772B (zh) * 2020-12-28 2022-07-19 厦门华源嘉航科技有限公司 一种基于request的惯性里程计动基座粗对准方法
CN114383614B (zh) * 2022-01-20 2023-12-05 东南大学 一种弹道环境下的多矢量空中对准方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1862764A1 (en) * 2006-05-31 2007-12-05 Honeywell International Inc. High speed gyrocompass alignment via multiple kalman filter based hypothesis testing
CN102679978A (zh) * 2012-05-14 2012-09-19 北京理工大学 一种旋转式捷联惯性导航系统静基座初始对准方法
CN105180937A (zh) * 2015-10-15 2015-12-23 常熟理工学院 一种mems-imu初始对准方法
CN106595711A (zh) * 2016-12-21 2017-04-26 东南大学 一种基于递推四元数的捷联惯性导航系统粗对准方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1862764A1 (en) * 2006-05-31 2007-12-05 Honeywell International Inc. High speed gyrocompass alignment via multiple kalman filter based hypothesis testing
CN102679978A (zh) * 2012-05-14 2012-09-19 北京理工大学 一种旋转式捷联惯性导航系统静基座初始对准方法
CN105180937A (zh) * 2015-10-15 2015-12-23 常熟理工学院 一种mems-imu初始对准方法
CN106595711A (zh) * 2016-12-21 2017-04-26 东南大学 一种基于递推四元数的捷联惯性导航系统粗对准方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
摇摆基座上基于信息的捷联惯导粗对准研究;秦永元_等;《西北工业大学学报》;20051031;第23卷(第5期);第682页第2章节 *

Also Published As

Publication number Publication date
CN108592943A (zh) 2018-09-28

Similar Documents

Publication Publication Date Title
CN108592943B (zh) 一种基于opreq方法的惯性系粗对准计算方法
CN105737823B (zh) 一种基于五阶ckf的gps/sins/cns组合导航方法
WO2020087845A1 (zh) 基于gpr与改进的srckf的sins初始对准方法
CN103630137B (zh) 一种用于导航系统的姿态及航向角的校正方法
CN104655131B (zh) 基于istssrckf的惯性导航初始对准方法
CN110398257A (zh) Gps辅助的sins系统快速动基座初始对准方法
CN105806363B (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN106595711A (zh) 一种基于递推四元数的捷联惯性导航系统粗对准方法
CN102538821B (zh) 一种快速、参数分段式捷联惯性导航系统自对准方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
CN106940193A (zh) 一种基于Kalman滤波的船舶自适应摇摆标定方法
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN112857398B (zh) 一种系泊状态下舰船的快速初始对准方法和装置
CN112229421B (zh) 基于李群最优估计的捷联惯性导航晃动基座粗对准方法
CN106840201B (zh) 一种带双轴转位机构捷联惯导的三位置自对准方法
CN108827288A (zh) 一种基于对偶四元数的降维捷联惯性导航系统初始对准方法及系统
CN112902956A (zh) 一种手持式gnss/mems-ins接收机航向初值获取方法、电子设备、存储介质
CN110207694A (zh) 一种基于相对位置信息的极区格网惯导/超短基线组合导航方法
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法
CN109084756B (zh) 一种重力视运动参数辨识与加速度计零偏分离方法
CN112683265B (zh) 一种基于快速iss集员滤波的mimu/gps组合导航方法
CN114543786B (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