CN108151736B - 一种适用于水下mems航向陀螺航向角解算方法 - Google Patents

一种适用于水下mems航向陀螺航向角解算方法 Download PDF

Info

Publication number
CN108151736B
CN108151736B CN201711339294.0A CN201711339294A CN108151736B CN 108151736 B CN108151736 B CN 108151736B CN 201711339294 A CN201711339294 A CN 201711339294A CN 108151736 B CN108151736 B CN 108151736B
Authority
CN
China
Prior art keywords
angle
course
heading
calculation
calculating
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
CN201711339294.0A
Other languages
English (en)
Other versions
CN108151736A (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.)
Csic Xi'an Dong Yi Science Technology & Industry Group Co ltd
Original Assignee
Csic Xi'an Dong Yi Science Technology & Industry Group Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Csic Xi'an Dong Yi Science Technology & Industry Group Co ltd filed Critical Csic Xi'an Dong Yi Science Technology & Industry Group Co ltd
Priority to CN201711339294.0A priority Critical patent/CN108151736B/zh
Publication of CN108151736A publication Critical patent/CN108151736A/zh
Application granted granted Critical
Publication of CN108151736B publication Critical patent/CN108151736B/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)
  • Gyroscopes (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：MEMS航向陀螺从航行器发射启动、入水、发动机启动供电、稳定航行等过程中的传感器测量数据在坐标轴上矢量投影、航向角速率积分、航向对准、四元数解算、航向角象限拓展、航向角误差系数等计算方法、公式与步骤。该方法满足水下航行器控制的快速航向建立及精确航向测量需要。

Description

一种适用于水下MEMS航向陀螺航向角解算方法
技术领域
本发明属于惯性测量技术领域，涉及一种适用于水下MEMS航向陀螺航向角解算方法。
背景技术
传统水下航行器的航向测量通常是采用双自由度方位状态的机械框架陀螺仪。在航行器发射过程中，由于航行器发动机未启动，无法给航向陀螺供电，所以在机械航向陀螺上设计有有发条驱动机构，发射时通过电脉冲信号点燃电爆管，电爆管爆炸推动陀螺发条解锁键，发条蓄能驱动陀螺转子高速启转并达到额定转速，在没有供电的情况下依靠陀螺效应的定轴性为水下航行器建立初始航向基准。入水后，航行器发动机启动产生陀螺供电电源，驱动陀螺电机转动，为航行器提供持续的航向角测量输出。
随着惯性技术的快速发展，使得采用MEMS等新型惯性元件实现水下航行器发射时的快速航向建立等功能变得具有可能，由于MEMS陀螺仪为角速率传感器，不能直接用于航向角度测量，所以需要通过软件算法进行一系列的积分、空间坐标关系转换和解算，以满足水下航行器控制的快速航向建立及精确航向测量需要。
发明目的
本发明的目的是提供一种适用于水下MEMS航向陀螺航向角解算方法，以满足水下航行器控制的快速航向建立及精确航向测量。
本发明的目的是这样实现的，一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：至少包括：
步骤1：MEMS航向陀螺启动；
步骤2：各传感器测量数据在坐标轴上矢量投影计算；
步骤3：第一时间延时判断，小于6秒，执行下一步；大于等于6秒，执行步骤10；
步骤4：启动后6秒内进行航向角速率积分计算，积分结果作为输出航向角数据，以便快速建立水下航行器的航向基准；
步骤5：进行角速率零位误差值计算；
步骤6：第二时间延时判断，小于5秒，执行步骤9；大于等于5秒，执行下一步；
步骤7：进行各轴角速率零位误差补偿修正；
步骤8：后台进行航向角的四元数解算，解算结果不输出；
步骤9：将准备好的航向角数据输出，重返步骤2；
步骤10：第三时间延时判断，等于6秒，执行下一步；大于6秒，执行步骤12；
步骤11：根据步骤8计算的前5秒航向角进行航向角零位误差值估算；
步骤12：进行正式航向角解算前对各轴角速率零位误差补偿修正；
步骤13：进行航向角的四元数解算；
步骤14：进行航向角4象限拓展计算；
步骤15：进行航向角弧度转角度整理计算，准备输出航向角数据，重返步骤9。
所述的步骤2包括：
采集角速率陀螺仪和加速度计信号，并按航行器坐标轴系定义将各传感器数据矢量折算到航向、横滚和俯仰轴上，折算包括测量值极性转换、标度系数换算和轴间安装耦合叠加，按如下公式组(1)计算航向、横滚、俯仰角速率、航向、横滚、俯仰加速度。
Wx＝Sxxωx+Syxωy+Szxωz
Wy＝Sxyωx+Syyωy+Szyωz
Wz＝Sxzωx+Syzωy+Szzωz (1)
Ax＝gxxax+gyxay+gzxaz
Ay＝gxyax+gyyay+gzyaz
Az＝gxzax+gyzay+gzzaz
其中Wx、Wy、Wz为航向、横滚、俯仰角速率，Ax、Ay、Az为航向、横滚、俯仰加速度，ωx、ωy、ωz为三个角速率陀螺仪测量值，ax、ay、az为三个加速度计测量值，S*x、S*y、S*z角速率陀螺在航向、横滚、俯仰轴上的投影系数，g*x、g*y、g*z加速度计在航向、横滚、俯仰轴上的投影系数，各投影系数可通过转台标定测试获得。
所述的步骤4包括：
初始航向为Ψ0，航向角积分计算包括公式(2)～(4)，公式(2)为首次积分计算，由于数据处理周期为0.01s，所以角速率积分系数为0.01；公式(3)为循环积分计算；公式(4)为航向角360°范围求余计算，防止航向角超范围，Ψ0为初始航向设定值；
ψo1＝0.01Wx1 (2)
ψoi＝ψoi-1+0.01Wxi (3)
ψoi＝(ψo+ψoi)％360 (4)
其中Ψ0为设定航向角，Ψoi为当前输出航向角。
所述的步骤5包括：进行各轴角速率零位误差补偿计算，计算周期为0.01s，角速率零位误差值取5s内的采样数据进行平均,具体按公式组(5))计算航向、横滚、俯仰角速率零位误差补偿值，
Figure BDA0001508013250000041
Figure BDA0001508013250000042
Figure BDA0001508013250000043
其中为i为5s时间采样值得数量(i＝500)，
Figure BDA0001508013250000044
航向、横滚、俯仰角速率零位误差补偿值。
所述的步骤7包括：
按公式组(6)进行各轴角速率零偏补偿，角速率Wx、Wy、Wz转换为弧度后参与后续计算；
Figure BDA0001508013250000045
Figure BDA0001508013250000046
Figure BDA0001508013250000047
所述的步骤8包括：
设定当前时刻的解算航向、横滚、俯仰角为Ψi、Фi、θi，则前一时刻的解算航向角为Ψi-1，规定解算航向角初始值为0°，航向角0～360°范围，横滚角和俯仰角±80°范围，当前时刻的解算横滚和俯仰角计算公式见(7)和(8)；
Фi＝arctg(Az/Ax) (7)
θi＝arctg(Ay/Ax) (8)
航向角为Ψi解算采用四元数发进行，四元数解算首先按公式组(9)计算当前时刻四元系数q0、q1、q2、q3，角度和角速率计算时采用单位为弧度；
Figure BDA0001508013250000051
Figure BDA0001508013250000052
Figure BDA0001508013250000053
Figure BDA0001508013250000054
按步骤7公式组(6)进行各轴角速率零偏补偿后，根据当前四元数系数q0、q1、q2、q3值按公式组见(10)～(13)计算解算中间变量值k1、k2、k3、k4；
k1[0]＝(Wx*q1+Wy*q2+Wz*q3)/(-200)
k1[1]＝(Wy*q3-Wx*q0-Wz*q2)/(-200) (10)
k1[2]＝(Wz*q1-Wy*q0-Wx*q3)/(-200)
k1[3]＝(Wx*q2-Wy*q1-Wz*q0)/(-200)
Figure BDA0001508013250000055
Figure BDA0001508013250000056
Figure BDA0001508013250000061
Figure BDA0001508013250000062
Figure BDA0001508013250000063
Figure BDA0001508013250000064
Figure BDA0001508013250000065
Figure BDA0001508013250000066
k4[0]＝(Wx*(q1+k3[1])+Wy*(q2+k3[1])+Wz*(q3+k3[1]))/(-200)
k4[1]＝(Wy*(q3+k3[3])-Wx*(q0+k3[0])-Wz*(q2+k3[2]))/(-200) (13)
k4[2]＝(Wz*(q1+k3[1])-Wy*(q0+k3[0])-Wx*(q3+k3[3]))/(-200)
k4[3]＝(Wx*(q2+k3[2])-Wy*(q1+k3[0])-Wz*(q0+k3[1]))/(-200)
接上一步，根据解算中间变量值k1、k2、k3、k4按公式组(14)迭代计算当前四元数系数q0、q1、q2、q3。
Figure BDA0001508013250000067
Figure BDA0001508013250000068
Figure BDA0001508013250000069
Figure BDA00015080132500000610
根据当前四元数系数q0、q1、q2、q3按公式组(15)计算当前航向角为Ψi，并进行航向角4个象限拓展计算；
Figure BDA0001508013250000071
所述的步骤11包括：
在启动时间等于6秒时计算航向角零位误差，航向角零位误差与启动后5-6秒时间内航向角解算值有关，航向角零位误差系数Δψ按公式(16)计算，
Δψ＝57.2957795ψ100％360 (16)
其中为Δψ为航向角误差系数，ψ100为5～6秒时间结束时的航向角值，即解算开始1秒钟后的第100个航向角值。
所述的步骤12是入水后航向角解算前对各轴角速率零位误差补偿修参照步骤7公式(6)计算；所述的步骤13是入水后航向角的四元数解算计算参照步骤8公式(7)～(14)计算。
所述的步骤14是入水后是航向角4象限拓展计算参照步骤8公式(15)计算。
所述的步骤15是入水后进行航向角弧度转角度整理计算，包括：
入水后航向陀螺输出航向角用四元数解算得到的航向角替代，按照公式(17)进行航向角弧度转角度整理计算，
ψoi＝(ψo-Δψ+57.2957795ψi)％360 (17)
其中Ψoi为当前输出航向角，Ψ0为设定航向角，ψi为当前航向角解算值。
本发明中所述的循环和输出周期为0.01s。
本发明的优点是：由于MEMS陀螺仪为角速率传感器，不能直接用于航向角度测量，所以采用本发明的解算方法进行一系列的积分、空间坐标关系转换和解算，以满足水下航行器控制的快速航向建立及精确航向测量需要。
附图说明
下面结合实施例附图对本发明作进一步说明：
图1是一种水下MEMS航向陀螺航向角解算方法流程图。
具体实施方式
如图1所示，一种适用于水下MEMS航向陀螺航向角解算方法，至少包括:
步骤2获取陀螺启动后的传感器数据,在坐标轴上的矢量投影计算包括：
采集角速率陀螺仪和加速度计信号，并按航行器坐标轴系定义将各传感器数据矢量折算到航向、横滚和俯仰轴上，折算包括测量值极性转换、标度系数换算和轴间安装耦合叠加，按如下公式组(1)计算航向、横滚、俯仰角速率、航向、横滚、俯仰加速度。
Wx＝Sxxωx+Syxωy+Szxωz
Wy＝Sxyωx+Syyωy+Szyωz
Wz＝Sxzωx+Syzωy+Szzωz (1)
Ax＝gxxax+gyxay+gzxaz
Ay＝gxyax+gyyay+gzyaz
Az＝gxzax+gyzay+gzzaz
其中Wx、Wy、Wz为航向、横滚、俯仰角速率，Ax、Ay、Az为航向、横滚、俯仰加速度，ωx、ωy、ωz为三个角速率陀螺仪测量值，ax、ay、az为三个加速度计测量值，S*x、S*y、S*z角速率陀螺在航向、横滚、俯仰轴上的投影系数，g*x、g*y、g*z加速度计在航向、横滚、俯仰轴上的投影系数，各投影系数可通过转台标定测试获得。
步骤4：启动后进行航向角速率积分计算包括：
初始航向为Ψ0，航向角积分计算包括公式(2)～(4)，公式(2)为首次积分计算，由于数据处理周期为0.01s，所以角速率积分系数为0.01；公式(3)为循环积分计算；公式(4)为航向角360°范围求余计算，防止航向角超范围，Ψ0为初始航向设定值；
ψo1＝0.01Wx1 (2)
ψoi＝ψoi-1+0.01Wxi (3)
ψoi＝(ψo+ψoi)％360 (4)
其中Ψ0为设定航向角，Ψoi为当前输出航向角。
步骤5：角速率零位误差值计算包括：
在发射到入水过程中航向陀螺(航向解算)软件同时在后台进行各轴角速率零位误差补偿计算，计算周期为0.01s，角速率零位误差值取5s内的采样数据进行平均,具体按公式组(5))计算航向、横滚、俯仰角速率零位误差补偿值。
Figure BDA0001508013250000101
Figure BDA0001508013250000102
Figure BDA0001508013250000103
其中为i为5s时间采样值得数量(i＝500)，
Figure BDA0001508013250000104
航向、横滚、俯仰角速率零位误差补偿值。
步骤7：各轴角速率零位误差补偿修正计算包括：
按公式组(6)进行各轴角速率零偏补偿，角速率Wx、Wy、Wz转换为弧度后参与后续计算。
Figure BDA0001508013250000105
Figure BDA0001508013250000106
Figure BDA0001508013250000107
步骤8：后台进行航向角的四元数解算计算包括：
首先设定当前时刻的解算航向、横滚、俯仰角为Ψi、Фi、θi，则前一时刻的解算航向角为Ψi-1。规定解算航向角初始值为0°，航向角0～360°范围，横滚角和俯仰角±80°范围。当前时刻的解算横滚和俯仰角计算公式见(7)和(8)。
Фi＝arctg(Az/Ax) (7)
θi＝arctg(Ay/Ax) (8)
接上一步，航向角为Ψi解算采用四元数发进行，四元数解算首先按公式组(9)计算当前时刻四元系数q0、q1、q2、q3。需要说明的是C语言软件角度和角速率计算时采用单位为弧度(取1弧度＝57.2957795°)。
Figure BDA0001508013250000111
Figure BDA0001508013250000112
Figure BDA0001508013250000113
Figure BDA0001508013250000114
接上一步，按步骤7公式组(6)进行各轴角速率零偏补偿后，根据当前四元数系数q0、q1、q2、q3值按公式组见(10)～(13)计算解算中间变量值k1、k2、k3、k4。
k1[0]＝(Wx*q1+Wy*q2+Wz*q3)/(-200)
k1[1]＝(Wy*q3-Wx*q0-Wz*q2)/(-200) (10)
k1[2]＝(Wz*q1-Wy*q0-Wx*q3)/(-200)
k1[3]＝(Wx*q2-Wy*q1-Wz*q0)/(-200)
Figure BDA0001508013250000115
Figure BDA0001508013250000116
Figure BDA0001508013250000117
Figure BDA0001508013250000118
Figure BDA0001508013250000119
Figure BDA00015080132500001110
Figure BDA00015080132500001111
Figure BDA00015080132500001112
k4[0]＝(Wx*(q1+k3[1])+Wy*(q2+k3[1])+Wz*(q3+k3[1]))/(-200)
k4[1]＝(Wy*(q3+k3[3])-Wx*(q0+k3[0])-Wz*(q2+k3[2]))/(-200) (13)
k4[2]＝(Wz*(q1+k3[1])-Wy*(q0+k3[0])-Wx*(q3+k3[3]))/(-200)
k4[3]＝(Wx*(q2+k3[2])-Wy*(q1+k3[0])-Wz*(q0+k3[1]))/(-200)
接上一步，根据解算中间变量值k1、k2、k3、k4按公式组(14)迭代计算当前四元数系数q0、q1、q2、q3。
Figure BDA0001508013250000121
Figure BDA0001508013250000122
Figure BDA0001508013250000123
Figure BDA0001508013250000124
接上一步，根据当前四元数系数q0、q1、q2、q3按公式组(15)计算当前航向角为Ψi，并进行航向角4个象限拓展计算。
Figure BDA0001508013250000125
步骤8计算获得航向角值不输出，陀螺航向角输出仍然采用步骤4计算得到的积分航向值。
步骤11：航向角零位误差值计算包括：
在启动时间等于6秒时计算航向角零位误差，航向角零位误差与启动后5-6秒时间内航向角解算值有关，航向角零位误差系数Δψ按公式(16)计算。
Δψ＝57.2957795ψ100％360 (16)
其中为Δψ为航向角误差系数，ψ100为5～6秒时间结束时的航向角值，即解算开始1秒钟后的第100个航向角值。
步骤12：(入水后)航向角解算前对各轴角速率零位误差补偿修参照步骤7公式(6)计算。
步骤13：(入水后)航向角的四元数解算计算参照步骤8公式(7)～(14)计算。
步骤14：(入水后)航向角4象限拓展计算参照步骤8公式(15)计算；
步骤15：(入水后)进行航向角弧度转角度整理计算包括：
入水后航向陀螺输出航向角用四元数解算得到的航向角替代，按照公式(17)进行航向角弧度转角度整理计算。
ψoi＝(ψo-Δψ+57.2957795ψi)％360 (17)
其中Ψoi为当前输出航向角，Ψ0为设定航向角，ψi为当前航向角解算值。
所述的循环解算和输出周期为0.01s。

Claims (8)

1.一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：至少包括：
步骤1：MEMS航向陀螺启动；
步骤2：各传感器测量数据在坐标轴上矢量投影计算；
步骤3：第一时间延时判断，小于6秒，执行下一步；大于等于6秒，执行步骤10；
步骤4：启动后6秒内进行航向角速率积分计算，积分结果作为输出航向角数据，以便快速建立水下航行器的航向基准；
步骤5：进行角速率零位误差值计算；
步骤6：第二时间延时判断，小于5秒，执行步骤9；大于等于5秒，执行下一步；
步骤7：进行各轴角速率零位误差补偿修正；
步骤8：后台进行航向角的四元数解算，解算结果不输出；
步骤9：将准备好的航向角数据输出，重返步骤2；
步骤10：第三时间延时判断，等于6秒，执行下一步；大于6秒，执行步骤12；
步骤11：根据步骤8计算的前5秒航向角进行航向角零位误差值估算；
步骤12：进行正式航向角解算前对各轴角速率零位误差补偿修正；
步骤13：进行航向角的四元数解算；
步骤14：进行航向角4象限拓展计算；
步骤15：进行航向角弧度转角度整理计算，准备输出航向角数据，重返步骤9；
所述的步骤2包括：
采集角速率陀螺仪和加速度计信号，并按航行器坐标轴系定义将各传感器数据矢量折算到航向、横滚和俯仰轴上，折算包括测量值极性转换、标度系数换算和轴间安装耦合叠加，按如下公式组(1)计算航向、横滚、俯仰角速率、航向、横滚、俯仰加速度；
Wx＝Sxxωx+Syxωy+Szxωz
Wy＝Sxyωx+Syyωy+Szyωz
Wz＝Sxzωx+Syzωy+Szzωz (1)
Ax＝gxxax+gyxay+gzxaz
Ay＝gxyax+gyyay+gzyaz
Az＝gxzax+gyzay+gzzaz
其中Wx、Wy、Wz为航向、横滚、俯仰角速率，Ax、Ay、Az为航向、横滚、俯仰加速度，ωx、ωy、ωz为三个角速率陀螺仪测量值，ax、ay、az为三个加速度计测量值，S*x、S*y、S*z角速率陀螺在航向、横滚、俯仰轴上的投影系数，g*x、g*y、g*z加速度计在航向、横滚、俯仰轴上的投影系数，各投影系数通过转台标定测试获得；
所述的步骤8包括：
设定当前时刻的解算航向、横滚、俯仰角为Ψi、Фi、θi，则前一时刻的解算航向角为Ψi-1，规定解算航向角初始值为0°，航向角0～360°范围，横滚角和俯仰角±80°范围，当前时刻的解算横滚和俯仰角计算公式见(7)和(8)；
Фi＝arctg(Az/Ax) (7)
θi＝arctg(Ay/Ax) (8)
航向角为Ψi解算采用四元数进行，四元数解算首先按公式组(9)计算当前时刻四元系数q0、q1、q2、q3，角度和角速率计算时采用单位为弧度；
Figure FDA0003243155160000031
按步骤7公式组(6)进行各轴角速率零偏补偿后，根据当前四元数系数q0、q1、q2、q3值按公式组见(10)～(13)计算解算中间变量值k1、k2、k3、k4；
k1[0]＝(Wx*q1+Wy*q2+Wz*q3)/(-200)
k1[1]＝(Wy*q3-Wx*q0-Wz*q2)/(-200) (10)
k1[2]＝(Wz*q1-Wy*q0-Wx*q3)/(-200)
k1[3]＝(Wx*q2-Wy*q1-Wz*q0)/(-200)
Figure FDA0003243155160000032
Figure FDA0003243155160000041
Figure FDA0003243155160000042
Figure FDA0003243155160000043
根据解算中间变量值k1、k2、k3、k4按公式组(14)迭代计算当前四元数系数q0、q1、q2、q3；
Figure FDA0003243155160000044
接上一步，根据当前四元数系数q0、q1、q2、q3按公式组(15)计算当前航向角为Ψi，并进行航向角4个象限拓展计算；
Figure FDA0003243155160000051
2.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤4包括：
初始航向为Ψ0，航向角积分计算包括公式(2)～(4)，公式(2)为首次积分计算，由于数据处理周期为0.01s，所以角速率积分系数为0.01；公式(3)为循环积分计算；公式(4)为航向角360°范围求余计算，防止航向角超范围；
ψ1＝0.01Wx1 (2)
ψi＝ψi-1+0.01Wxi (3)
ψoi＝(ψ0+ψi)％360 (4)
其中，ψ0为设定航向角，ψoi为当前输出航向角；ψi为当前航向角解算值。
3.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤5包括：进行各轴角速率零位误差补偿计算，计算周期为0.01s，角速率零位误差值取5s内的采样数据进行平均，具体按公式组(5)计算航向、横滚、俯仰角速率零位误差补偿值，
Figure FDA0003243155160000061
其中为i为5s时间采样值得数量i＝500，
Figure FDA0003243155160000062
航向、横滚、俯仰角速率零位误差补偿值。
4.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤7包括：
按公式组(6)进行各轴角速率零偏补偿，角速率Wx、Wy、Wz转换为弧度后参与后续计算；
Figure FDA0003243155160000063
5.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤11包括：
在启动时间等于6秒时计算航向角零位误差，航向角零位误差与启动后5-6秒时间内航向角解算值有关，航向角零位误差系数Δψ按公式(16)计算，
Δψ＝57.2957795ψ100％360 (16)
其中为Δψ为航向角误差系数，ψ100为5～6秒时间结束时的航向角值，即解算开始1秒钟后的第100个航向角值。
6.根据权利要求4所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤12是入水后航向角解算前对各轴角速率零位误差补偿修参照步骤7公式(6)计算；所述的步骤13是入水后航向角的四元数解算计算参照步骤8公式(7)～(14)计算。
7.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤14是入水后航向角的4象限拓展计算，计算方法参照公式(15)。
8.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤15是入水后进行航向角弧度转角度整理计算，包括：
入水后航向陀螺输出航向角用四元数解算得到的航向角替代，按照公式(17)进行航向角弧度转角度整理计算，
ψoi＝(ψo-Δψ+57.2957795ψi)％360 (17)
其中ψoi为当前输出航向角，ψo为设定航向角，ψi为当前航向角解算值，Δψ为航向角误差系数。
CN201711339294.0A 2017-12-14 2017-12-14 一种适用于水下mems航向陀螺航向角解算方法 Active CN108151736B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711339294.0A CN108151736B (zh) 2017-12-14 2017-12-14 一种适用于水下mems航向陀螺航向角解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711339294.0A CN108151736B (zh) 2017-12-14 2017-12-14 一种适用于水下mems航向陀螺航向角解算方法

Publications (2)

Publication Number Publication Date
CN108151736A CN108151736A (zh) 2018-06-12
CN108151736B true CN108151736B (zh) 2021-11-19

Family

ID=62466081

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711339294.0A Active CN108151736B (zh) 2017-12-14 2017-12-14 一种适用于水下mems航向陀螺航向角解算方法

Country Status (1)

Country Link
CN (1) CN108151736B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109814649A (zh) * 2019-01-02 2019-05-28 湖北三江航天红峰控制有限公司 一种适用于尾段舵机测试系统的电流补偿方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09292248A (ja) * 1996-04-30 1997-11-11 Matsushita Electric Ind Co Ltd ナビゲーション装置
RU2176780C1 (ru) * 2000-08-30 2001-12-10 Открытое акционерное общество "Раменское приборостроительное конструкторское бюро" Способ определения истинного курса с помощью двухканального гироскопического датчика угловой скорости
CN103712623A (zh) * 2014-01-20 2014-04-09 东南大学 基于角速率输入的光纤陀螺惯导系统姿态优化方法
CN104697526A (zh) * 2015-03-26 2015-06-10 上海华测导航技术股份有限公司 用于农业机械的捷联惯导系统以及控制方法
CN106153042A (zh) * 2015-03-31 2016-11-23 日本电气株式会社 航向角获取方法和装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09292248A (ja) * 1996-04-30 1997-11-11 Matsushita Electric Ind Co Ltd ナビゲーション装置
RU2176780C1 (ru) * 2000-08-30 2001-12-10 Открытое акционерное общество "Раменское приборостроительное конструкторское бюро" Способ определения истинного курса с помощью двухканального гироскопического датчика угловой скорости
CN103712623A (zh) * 2014-01-20 2014-04-09 东南大学 基于角速率输入的光纤陀螺惯导系统姿态优化方法
CN104697526A (zh) * 2015-03-26 2015-06-10 上海华测导航技术股份有限公司 用于农业机械的捷联惯导系统以及控制方法
CN106153042A (zh) * 2015-03-31 2016-11-23 日本电气株式会社 航向角获取方法和装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于四元数EKF算法的小型无人机姿态估计;宋宇等;《吉林大学学报(理学版)》;20150526;第53卷(第03期);511-518 *

Also Published As

Publication number Publication date
CN108151736A (zh) 2018-06-12

Similar Documents

Publication Publication Date Title
CN108827299B (zh) 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
CN107655493B (zh) 一种光纤陀螺sins六位置系统级标定方法
JP4876204B2 (ja) 小型姿勢センサ
JP3970724B2 (ja) 飛翔体の姿勢変更制御装置及び姿勢変更制御方法
CN109373833B (zh) 适用于旋转弹初始姿态和速度联合测量方法
CN109373832B (zh) 基于磁测滚转的旋转弹炮口初始参数测量方法
KR20170067138A (ko) 관성 항법 시스템
CN112630813A (zh) 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN103983276A (zh) 一种基于导航基准系的三框架四轴惯性平台误差标定方法
CN111623768B (zh) 一种基于克雷洛夫角奇异条件下的姿态角解算方法
FR2922300A1 (fr) PROCEDE DE DETERMINATION DE l'ATTITUDE, DE LA POSITION ET DE LA VITESSE D'UN ENGIN MOBILE
JP5338464B2 (ja) 慣性航法装置、飛翔体及び航法データ算出方法
CN109211231B (zh) 一种基于牛顿迭代法的炮弹姿态估计方法
CN108151736B (zh) 一种适用于水下mems航向陀螺航向角解算方法
Xing et al. Quaternion-based Complementary Filter for Aiding in the Self-Alignment of the MEMS IMU
CN112363195A (zh) 基于运动学方程的旋转弹空中快速粗对准方法
CN111780752A (zh) 一种姿态误差可观测的提高惯性制导精度方法
Changey et al. Experimental validation
CN109211232B (zh) 一种基于最小二乘滤波的炮弹姿态估计方法
Zheng et al. Compensation for stochastic error of gyros in a dual-axis rotational inertial navigation system
Hong et al. Application of EKF for missile attitude estimation based on “SINS/CNS” integrated guidance system
CN113865585B (zh) 一种陀螺仪组合高阶误差系数分离与补偿方法和系统
CN111142128A (zh) 导航完好性的监测方法及装置、无人机
CN109540134B (zh) 一种三轴稳定平台系统框架自解锁方法及系统
CN110440984B (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