一种适用于水下MEMS航向陀螺航向角解算方法技术

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

【技术实现步骤摘要】
一种适用于水下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ωzWy＝Sxyωx+Syyωy+SzyωzWz＝Sxzωx+Syzωy+Szzωz(1)Ax＝gxxax+gyxay+gzxazAy＝gxyax+gyyay+gzyazAz＝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))计算航向、横滚、俯仰角速率零位误差补偿值，其中为i为5s时间采样值得数量(i＝500)，航向、横滚、俯仰角速率零位误差补偿值。所述的步骤7包括：按公式组(6)进行各轴角速率零偏补偿，角速率Wx、Wy、Wz转换为弧度后参与后续计算；所述的步骤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，角度和角速率计算时采用单位为弧度；按步骤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)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。根据当前四元数系数q0、q1、q2、q3按公式组(15)计算当前航向角为Ψi，并进行航向角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。本专利技术的优点是：由于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。

【技术特征摘要】
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所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤2包括：采集角速率陀螺仪和加速度计信号，并按航行器坐标轴系定义将各传感器数据矢量折算到航向、横滚和俯仰轴上，折算包括测量值极性转换、标度系数换算和轴间安装耦合叠加，按如下公式组(1)计算航向、横滚、俯仰角速率、航向、横滚、俯仰加速度；其中Wx、Wy、Wz为航向、横滚、俯仰角速率，Ax、Ay、Az为航向、横滚、俯仰加速度，ωx、ωy、ωz为三个角速率陀螺仪测量值，ax、ay、az为三个加速度计测量值，S*x、S*y、S*z角速率陀螺在航向、横滚、俯仰轴上的投影系数，g*x、g*y、g*z加速度计在航向、横滚、俯仰轴上的投影系数，各投影系数可通过转台标定测试获得。3.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤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为当前输出航向角。4.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤5包括：进行各轴角速率零位误差补偿计算，计算周期为0.01s，角速率零位误差值取5s内的采样数据进行平均,具体按公式组(5))计算航向、横滚、俯仰角速率零位误差补偿值，其中为i...

【专利技术属性】
技术研发人员：田新兴，李海龙，王薇薇，杨斌，
申请(专利权)人：中船重工西安东仪科工集团有限公司，
类型：发明
国别省市：陕西,61