CN113050143B - 一种发射惯性坐标系下的紧耦合导航方法 - Google Patents

一种发射惯性坐标系下的紧耦合导航方法 Download PDF

Info

Publication number
CN113050143B
CN113050143B CN202110611333.8A CN202110611333A CN113050143B CN 113050143 B CN113050143 B CN 113050143B CN 202110611333 A CN202110611333 A CN 202110611333A CN 113050143 B CN113050143 B CN 113050143B
Authority
CN
China
Prior art keywords
navigation system
coordinate system
strapdown
inertial navigation
strapdown inertial
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
CN202110611333.8A
Other languages
English (en)
Other versions
CN113050143A (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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical 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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202110611333.8A priority Critical patent/CN113050143B/zh
Publication of CN113050143A publication Critical patent/CN113050143A/zh
Application granted granted Critical
Publication of CN113050143B publication Critical patent/CN113050143B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • 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
    • G01C21/165—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 combined with non-inertial navigation instruments

Abstract

本发明公开了一种发射惯性坐标系下的紧耦合导航方法，该方法通过选择发射惯性坐标系作为高超声速助推‑滑翔飞行器的导航坐标系，基于发射惯性坐标系下的捷联惯性导航系统与卫星导航系统紧耦合的组合导航方法，先利用数值更新算法更新，利用该更新数值与卫星导航系统得到的伪距、伪距率得到量测量，再通过卡尔曼滤波对该量测量进行误差分析，对更新后的捷联惯性导航系统数值进行修正，再利用该修正后的捷联惯性导航系统数值对卫星导航系统进行校正，该方法可以为高超声速助推‑滑翔飞行器提供导航信息，满足其垂直发射的要求，通过紧耦合方式提高了精度和抗干扰能力。

Description

一种发射惯性坐标系下的紧耦合导航方法
技术领域
本发明属于飞行器导航、制导和控制领域，涉及一种捷联惯性导航系统与卫星导航系统的组合导航领域，具体涉及一种发射惯性坐标系下的紧耦合导航方法。
背景技术
高超音速飞行器是一种能够在近空间以5倍以上声速长时间飞行和巡航的飞行器，也称为“近空间高超音速飞行器”。近空间是距离地面20-100公里的大气空间。该空间位于低轨道卫星作业区下方，传统飞机飞行区上方。它具有很大的军事和民用应用价值。由于高超声速助推-滑翔飞行器是垂直发射的，其弹道与运载火箭相似，如果采用当地水平坐标系作为高超声速助推-滑翔飞行器的导航坐标系，在导航计算过程中俯仰角会出现奇异的现象，影响正常导航；而发射惯性坐标系作为运载火箭常用的导航坐标系，不会导致俯仰角出现奇异，选择发射惯性坐标系作为高超声速助推-滑翔飞行器的导航坐标系，可以满足垂直发射的导航需求。
由于捷联惯性导航系统与卫星导航系统的优势互补性，高超声速飞行器均以惯性/卫星组合导航为主要的导航系统。常用的惯性/卫星组合方式为松耦合，但松耦合在有效卫星数小于4时，不能进行正常组合导航。
发明内容
针对现有技术中的上述不足，本发明提供了一种发射惯性坐标系下的紧耦合导航方法。
为了达到上述发明目的，本发明采用的技术方案为：
一种发射惯性坐标系下的紧耦合导航方法，包括以下步骤：
S1、利用数值更新算法更新发射惯性坐标系下的捷联惯性导航系统数值；
S2、利用步骤S1更新后的捷联惯性导航系统数值和卫星导航接收机接收数值计算得到捷联惯性导航系统下伪距和伪距率，并通过卫星导航接收机得到卫星导航系统下伪距和伪距率；
S3、利用卡尔曼滤波对根据步骤S2中捷联惯性导航系统下伪距和伪距率与卫星导航系统下伪距和伪距率计算得到的量测量进行误差分析，得到捷联惯性导航系统误差；
S4、利用步骤S3得到的捷联惯性导航系统误差与步骤S1更新后的捷联惯性导航系统数值进行修正，得到修正后的捷联惯性导航系统数值；
S5、利用步骤S4修正后的捷联惯性导航系统数值对捷联惯性导航系统进行校正与反馈。
该方案的有益效果为：
提出了一种适用于高超声速-助推滑翔飞行器导航方法，可以高超声速助推-滑翔飞行器提供导航信息，满足其垂直发射的要求，并且精度和抗干扰能力上都优于常用的松耦合。
进一步地，所述步骤S1具体为：
根据加速度计敏感的比力信息和陀螺仪敏感的角速度信息采用数值更新算法中姿态数值更新算法、速度数值更新算法和位置数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统姿态、速度和位置信息进行数值更新，得到更新后的捷联惯性导航系统数值，其中数值更新算法采用二子样算法。
该进一步方案的有益效果为：
为捷联惯性导航与卫星组合导航提供捷联惯性导航系统数据支持。
进一步地，所述步骤S1中利用姿态数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统的姿态信息进行更新，具体为：
先利用陀螺仪敏感的角速度
Figure 822404DEST_PATH_IMAGE001
计算
Figure 971626DEST_PATH_IMAGE002
时刻到
Figure 383016DEST_PATH_IMAGE003
时刻的姿态转换四元数
Figure 414426DEST_PATH_IMAGE004
，再利用该姿态转换四元数
Figure 569463DEST_PATH_IMAGE005
对姿态更新前
Figure 369929DEST_PATH_IMAGE006
时刻的姿态进行更新，得到更新后
Figure 952220DEST_PATH_IMAGE003
时刻的姿态四元数
Figure 972391DEST_PATH_IMAGE007
，表示为：
Figure 665541DEST_PATH_IMAGE008
。
该进一步方案的有益效果为：
完成姿态更新算法，得到捷联惯性导航系统的姿态输出。
进一步地，所述步骤S1中利用速度数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统的速度信息进行更新，具体为：
先利用载体坐标系下加速度计敏感的比力
Figure 586092DEST_PATH_IMAGE009
计算捷联卫星导航系比力速度增量
Figure 339285DEST_PATH_IMAGE010
，再利用该采样时段T内捷联卫星导航系统比力速度增量
Figure 345287DEST_PATH_IMAGE011
、采样时段T内捷联卫星导航引力速度增量
Figure 576548DEST_PATH_IMAGE012
对位置更新前
Figure 351606DEST_PATH_IMAGE013
时刻捷联惯性导航系统速度
Figure 275699DEST_PATH_IMAGE014
进行更新，得到更新后
Figure 1954DEST_PATH_IMAGE015
时刻的捷联惯性导航系统速度
Figure 302485DEST_PATH_IMAGE016
，表示为：
Figure 932049DEST_PATH_IMAGE017
。
该进一步方案的有益效果为：
完成速度算法更新，完成捷联惯性导航系统的速度输出。
进一步地，所述步骤S1中利用位置数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统的位置信息进行更新，具体包括：
利用采样时段T内捷联导航系统引力速度增量
Figure 292624DEST_PATH_IMAGE018
、更新前
Figure 742060DEST_PATH_IMAGE019
时刻下的捷联惯性导航系统速度
Figure 580703DEST_PATH_IMAGE020
、载体坐标系b系到发射惯性坐标系a系的转换矩阵
Figure 330353DEST_PATH_IMAGE021
与捷联导航系统比力速度增量引起的位置增量
Figure 596249DEST_PATH_IMAGE022
对更新前
Figure 300025DEST_PATH_IMAGE019
时刻下的捷联导航系统位置
Figure 676780DEST_PATH_IMAGE023
进行更新，得到更新后
Figure 280936DEST_PATH_IMAGE024
时刻的捷联惯性导航系统位置
Figure 717734DEST_PATH_IMAGE025
，表示为：
Figure 407341DEST_PATH_IMAGE026
。
该进一步方案的有益效果为：
完成位置更新算法，完成捷联惯性导航系统的位置输出。
进一步地，所述步骤S2具体为：
利用步骤S1更新后的捷联惯性导航系统速度
Figure 587787DEST_PATH_IMAGE027
和位置
Figure 46450DEST_PATH_IMAGE028
与卫星接收机接收的地心地固坐标系下的卫星速度和位置信息作差，得到捷联惯性导航系统的伪距
Figure 654149DEST_PATH_IMAGE029
和伪距率
Figure 329588DEST_PATH_IMAGE030
，并通过卫星接收机观测得到卫星导航系统的伪距
Figure 313724DEST_PATH_IMAGE031
和伪距率
Figure 626894DEST_PATH_IMAGE032
。
该进一步方案的有益效果为：
为为捷联惯性导航与卫星组合导航提供卫星导航数据支持。
进一步地，所述步骤S3具体包括以下分步骤：
S31、利用步骤S2得到的捷联惯性导航系统的伪距
Figure 405494DEST_PATH_IMAGE033
和伪距率
Figure 69693DEST_PATH_IMAGE034
，分别与卫星导航系统的伪距
Figure 388679DEST_PATH_IMAGE035
和伪距率
Figure 697301DEST_PATH_IMAGE036
作差，得到量测量
Figure 771436DEST_PATH_IMAGE037
、
Figure 798298DEST_PATH_IMAGE038
；
S32、以发射惯性坐标系下的捷联惯性导航系统误差与卫星定位误差作为状态量，构建卡尔曼滤波状态方程，表示为：
Figure 484756DEST_PATH_IMAGE039
其中，
Figure 647885DEST_PATH_IMAGE040
为卡尔曼滤波的状态矢量，
Figure 158500DEST_PATH_IMAGE041
为姿态误差，
Figure 407079DEST_PATH_IMAGE042
为姿态误差的微分，
Figure 395764DEST_PATH_IMAGE043
为速度误差，
Figure 413398DEST_PATH_IMAGE044
为速度误差的微分，
Figure 829336DEST_PATH_IMAGE045
为位置误差，
Figure 565211DEST_PATH_IMAGE046
为位置误差的微分，
Figure 856122DEST_PATH_IMAGE047
为陀螺仪零偏，
Figure 993842DEST_PATH_IMAGE048
为陀螺仪零偏的微分，
Figure 580681DEST_PATH_IMAGE049
为加速度计零偏，
Figure 600590DEST_PATH_IMAGE050
为加速度计零偏的微分，
Figure 806443DEST_PATH_IMAGE051
为卫星收机时钟偏差造成的距离误差，
Figure 923304DEST_PATH_IMAGE052
卫星接收机时钟偏差造成的距离误差的微分，
Figure 821989DEST_PATH_IMAGE053
为卫星接收机时钟漂移，
Figure 657090DEST_PATH_IMAGE054
为卫星接收机时钟漂移的微分，
Figure 401055DEST_PATH_IMAGE055
分别为陀螺仪、加速度计、时钟偏差和时钟漂移的白噪声，
Figure 139466DEST_PATH_IMAGE056
分别表示矩阵维数为
Figure 677895DEST_PATH_IMAGE057
的零矩阵；
S33、利用步骤S31得到的量测量
Figure 265871DEST_PATH_IMAGE058
、
Figure 813527DEST_PATH_IMAGE059
，构建卡尔曼滤波量测方程，表示为：
Figure 904980DEST_PATH_IMAGE060
其中，
Figure 145469DEST_PATH_IMAGE061
为从发射惯性坐标系下的误差矢量转换到地心地固坐标系下的转换矩阵，M为当前可见卫星颗数，
Figure 689582DEST_PATH_IMAGE062
为M行的列向量，
Figure 40929DEST_PATH_IMAGE063
分别表示矩阵维数为
Figure 485424DEST_PATH_IMAGE064
的零矩阵，
Figure 896814DEST_PATH_IMAGE065
为卫星位置的测量误差矩阵，
Figure 928223DEST_PATH_IMAGE066
为卫星速度的测量误差矩阵，
Figure 817682DEST_PATH_IMAGE067
为卡尔曼滤波的状态矢量；
S34、利用步骤S32得到的卡尔曼状态方程与步骤S33得到的卡尔曼滤波量测方程构建卡尔曼滤波系统，并利用该卡尔曼滤波系统计算得到捷联惯性导航系统姿态、速度、位置误差
Figure 618148DEST_PATH_IMAGE068
。
该进一步方案的有益效果为：
得到卡尔曼滤波系统的估计结果，为修正捷联惯性导航系统提供误差估计信息。
进一步地，所述步骤S4具体为：
利用步骤S3得到的捷联惯性导航系统姿态、速度、位置误差
Figure 200439DEST_PATH_IMAGE069
与步骤S1更新后的捷联惯性导航系统姿态、速度和位置信息作差，得到修正后的捷联惯性导航系统姿态、速度和位置信息
Figure 719145DEST_PATH_IMAGE070
。
该进一步方案的有益效果为：
得到修正后的捷联惯性导航系统信息。
进一步地，所述步骤S5具体为：
利用步骤S4得到的修正后的捷联惯性导航系统姿态、速度和位置信息
Figure 412294DEST_PATH_IMAGE071
作为捷联惯性导航系统下一解算周期数值，对捷联惯性导航系统进行校正和反馈。
该进一步方案的有益效果为：
完成一个紧耦合组合导航周期解算，为飞行器提供导航信息。
附图说明
图1为本发明中发射坐标系关系图；
图2为本发明提供的一种发射惯性坐标系下的紧耦合导航方法流程图；
图3为本发明中步骤S3的分步骤流程图；
图4为本发明中高超声速飞行器飞行轨迹三维图；
图5为本发明中松、紧耦合姿态误差对比图；
图6为本发明中松、紧耦合速度误差对比图；
图7为本发明中松、紧耦合位置误差对比图。
具体实施方式
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
如图1所示，本发明提供一种发射惯性坐标系下的紧耦合导航方法基于以下坐标系，并选择发射惯性坐标系作为捷联惯性导航系统的参考坐标系，包括：
1)、地心惯性坐标系（地惯系，i系），原点为地球中心，
Figure 568731DEST_PATH_IMAGE072
、
Figure 321924DEST_PATH_IMAGE073
轴在地球赤道平面内，
Figure 30DEST_PATH_IMAGE074
轴指向春分点，
Figure 621504DEST_PATH_IMAGE075
轴为地球自转轴；
2)、地心地固坐标系（地固系，e系），原点为地球中心，
Figure 271928DEST_PATH_IMAGE076
、
Figure 55076DEST_PATH_IMAGE077
轴在地球赤道平面内，
Figure 423741DEST_PATH_IMAGE078
指向本初子午线，
Figure 583327DEST_PATH_IMAGE079
轴为地球自转轴；
3)、发射坐标系（发射系，g系），坐标原点与发射点固连，
Figure 353837DEST_PATH_IMAGE080
轴在发射点水平面内，指向发射瞄准方向，
Figure 806421DEST_PATH_IMAGE081
轴垂直于发射点水平面指向上方，发射系为前上右坐标系。发射坐标系确定了与地球之间的初始纬度
Figure 396803DEST_PATH_IMAGE082
、初始经度
Figure 360080DEST_PATH_IMAGE083
、初始高度
Figure 719517DEST_PATH_IMAGE084
和发射方位角
Figure 110047DEST_PATH_IMAGE085
；
4)、载体坐标系（载体系，b 系），坐标原点为飞行器的质心，
Figure 187724DEST_PATH_IMAGE086
轴指向头部，
Figure 954692DEST_PATH_IMAGE087
轴在飞行器的主对称面内，向上为正，载体系为前上右坐标系；
5)、发射惯性坐标系（发惯系，a系），飞行器起飞瞬间，坐标原点与发射原点重合，各坐标轴与发射系各轴也相应重合。飞行器起飞后，发惯系各轴方向在惯性空间保持不动。
如图2所示，本发明提供一种发射惯性坐标系下的紧耦合导航方法，包括以下步骤：
S1、利用数值更新算法更新发射惯性坐标系下的捷联惯性导航系统数值；
本实施例中，根据加速度计敏感的比力信息和陀螺仪敏感的角速度信息采用姿态数值更新算法、速度数值更新算法和位置数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统姿态、速度和位置信息进行数值更新，得到更新后的捷联惯性导航系统数值，其中数值更新算法采用二子样算法。
实际中，首先记二子样算法的采样时刻为
Figure 434215DEST_PATH_IMAGE088
和
Figure 497111DEST_PATH_IMAGE089
时刻，解算周期为
Figure 858822DEST_PATH_IMAGE090
，记采样周期为
Figure 39268DEST_PATH_IMAGE091
，记
Figure 497931DEST_PATH_IMAGE092
和
Figure 105630DEST_PATH_IMAGE093
分别是
Figure 282533DEST_PATH_IMAGE094
时间段内的角增量和速度增量，
Figure 1091DEST_PATH_IMAGE095
和
Figure 579839DEST_PATH_IMAGE096
是
Figure 358440DEST_PATH_IMAGE097
时间段内的角增量和速度增量；
再将通过系统装订得到飞行器发射时刻发射惯性坐标系下的捷联惯性导航系统姿态、速度和位置信息
Figure 521174DEST_PATH_IMAGE098
作为初始值，利用加速度计和陀螺仪敏感的比力和角速度信息得到的姿态数值更新算法、速度数值更新算法和位置数值更新算法进行数值更新，得到更新后的捷联惯性导航系统数值。
本实施例中，步骤S1中利用姿态数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统的姿态信息进行更新，具体为：
先利用陀螺仪敏感的角速度
Figure 777843DEST_PATH_IMAGE099
计算姿态转换四元数
Figure 211099DEST_PATH_IMAGE100
，再利用该姿态转换四元数
Figure 160600DEST_PATH_IMAGE101
对姿态更新前
Figure 46516DEST_PATH_IMAGE102
时刻的姿态进行更新，得到更新后
Figure 372456DEST_PATH_IMAGE103
时刻的姿态四元数
Figure 660217DEST_PATH_IMAGE104
，表示为：
Figure 780620DEST_PATH_IMAGE105
。
实际中，姿态更新前
Figure 91516DEST_PATH_IMAGE106
时刻的姿态四元数
Figure 581665DEST_PATH_IMAGE107
由姿态矩阵
Figure 599300DEST_PATH_IMAGE108
得到，以系统装订得到的初始姿态矩阵
Figure 15238DEST_PATH_IMAGE109
为初始值；
利用载体系（
Figure 16692DEST_PATH_IMAGE110
系）相对于发射惯性坐标系（
Figure 809067DEST_PATH_IMAGE111
系）的角速度
Figure 415629DEST_PATH_IMAGE112
，即陀螺敏感的角速度，来计算
Figure 268047DEST_PATH_IMAGE113
时刻到
Figure 491218DEST_PATH_IMAGE114
时刻的姿态转换四元数
Figure 320241DEST_PATH_IMAGE115
，计算过程为：
首先，利用
Figure 312468DEST_PATH_IMAGE116
、
Figure 335787DEST_PATH_IMAGE117
两个时间段内的角增量
Figure 780675DEST_PATH_IMAGE118
计算得到
Figure 914853DEST_PATH_IMAGE119
对应的旋转矢量
Figure 27166DEST_PATH_IMAGE120
，表示为：
Figure 955807DEST_PATH_IMAGE121
再利用旋转矢量
Figure 950308DEST_PATH_IMAGE122
及四元数的对应关系计算得到姿态转换四元数
Figure 497964DEST_PATH_IMAGE123
，表示为：
Figure 90882DEST_PATH_IMAGE124
。
本实施例中，所述步骤S1中利用速度数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统的速度信息进行更新，具体为：
先利用载体坐标系下加速度计敏感的比力
Figure 65791DEST_PATH_IMAGE125
计算捷联卫星导航系比力速度增量
Figure 609905DEST_PATH_IMAGE126
，再利用该采样时段T内捷联卫星导航系统比力速度增量
Figure 961252DEST_PATH_IMAGE127
、采样时段T内捷联卫星导航引力速度增量
Figure 907211DEST_PATH_IMAGE128
对位置更新前
Figure 318601DEST_PATH_IMAGE129
时刻捷联惯性导航系统速度
Figure 350011DEST_PATH_IMAGE130
进行更新，得到更新后
Figure 239469DEST_PATH_IMAGE131
时刻的捷联惯性导航系统速度
Figure 827487DEST_PATH_IMAGE132
，表示为：
Figure 144199DEST_PATH_IMAGE133
。
实际中，以系统装订得到速度更新前的惯导速度
Figure 335009DEST_PATH_IMAGE134
为初始值；
利用发射惯性坐标系a系下的重力矢量
Figure 152792DEST_PATH_IMAGE135
计算采样时段T内捷联惯性导航系统引力速度增量
Figure 948710DEST_PATH_IMAGE136
，表示为：
Figure 826536DEST_PATH_IMAGE137
由于，短时间
Figure 707904DEST_PATH_IMAGE138
内其引起的速度变化量很小的，可以认为
Figure 63799DEST_PATH_IMAGE139
是时间的缓变量，可采用
Figure 714223DEST_PATH_IMAGE140
时刻的值进行近似代替，表示为：
Figure 998836DEST_PATH_IMAGE141
其中，
Figure 367501DEST_PATH_IMAGE142
为
Figure 527087DEST_PATH_IMAGE143
时刻在a系下的重力矢量；
利用载体坐标系b系到发射惯性坐标系a系的转换矩阵
Figure 32017DEST_PATH_IMAGE144
与载体坐标系下加速度计敏感的比力
Figure 251646DEST_PATH_IMAGE145
计算采样时段T内捷联惯性导航系统比力速度增量
Figure 107607DEST_PATH_IMAGE146
，表示为：
Figure 477408DEST_PATH_IMAGE147
亦可通过载体坐标系b系到发射惯性坐标系a系的转换矩阵
Figure 227058DEST_PATH_IMAGE148
与载体系b系下的比力速度增量
Figure 492955DEST_PATH_IMAGE149
计算得到，表示为：
Figure 193801DEST_PATH_IMAGE150
其中，载体坐标系b系的比力速度增量
Figure 836135DEST_PATH_IMAGE151
通过加速度计的速度增量
Figure 440292DEST_PATH_IMAGE152
、速度的旋转效应补偿量
Figure 877089DEST_PATH_IMAGE153
与速度的划桨效应补偿项
Figure 566696DEST_PATH_IMAGE154
计算得到，表示为：
Figure 215984DEST_PATH_IMAGE155
其中，加速度计的速度增量
Figure 940226DEST_PATH_IMAGE156
通过
Figure 547925DEST_PATH_IMAGE157
、
Figure 226293DEST_PATH_IMAGE158
两个时间段内的速度增量计算得到，表示为：
Figure 210430DEST_PATH_IMAGE159
速度的旋转效应补偿量
Figure 789179DEST_PATH_IMAGE160
是由运载体的线运动方向在空间旋转引起，通过加速度计的速度增量
Figure 567779DEST_PATH_IMAGE161
与陀螺仪的角增量
Figure 231978DEST_PATH_IMAGE162
计算得到，表示为：
Figure 488647DEST_PATH_IMAGE163
速度的划桨效应补偿项
Figure 921903DEST_PATH_IMAGE164
当载体同时做线振动和角振动时存在，通过
Figure 871404DEST_PATH_IMAGE165
时间段内的角增量
Figure 521435DEST_PATH_IMAGE166
和速度增量
Figure 847374DEST_PATH_IMAGE167
与
Figure 135136DEST_PATH_IMAGE168
时间段内的角增量
Figure 255539DEST_PATH_IMAGE169
和速度增量
Figure 628751DEST_PATH_IMAGE170
计算得到，表示为：
Figure 492802DEST_PATH_IMAGE171
其中, h为单子样的采样周期。
本实施例中，步骤S1中利用位置数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统的位置信息进行更新，具体包括：
利用采样时段T内捷联导航系统引力速度增量
Figure 635070DEST_PATH_IMAGE172
、更新前
Figure 191954DEST_PATH_IMAGE173
时刻下的捷联惯性导航系统速度
Figure 553927DEST_PATH_IMAGE174
、载体坐标系b系到发射惯性坐标系a系的转换矩阵
Figure 956089DEST_PATH_IMAGE175
与捷联导航系统比力速度增量引起的位置增量
Figure 218444DEST_PATH_IMAGE176
对更新前
Figure 946228DEST_PATH_IMAGE177
时刻下的捷联导航系统位置
Figure 28454DEST_PATH_IMAGE178
进行更新，得到更新后
Figure 499886DEST_PATH_IMAGE179
时刻的捷联惯性导航系统位置
Figure 616747DEST_PATH_IMAGE180
，表示为：
Figure 249853DEST_PATH_IMAGE181
。
实际中，以系统装订得到位置更新前的惯导速度
Figure 757058DEST_PATH_IMAGE182
为初始值；
捷联导航系统比力速度增量引起的位置增量
Figure 593034DEST_PATH_IMAGE183
由捷联导航系统比力的二次积分增量
Figure 564401DEST_PATH_IMAGE184
、位置计算中的旋转效应补偿量
Figure 633988DEST_PATH_IMAGE185
与位置计算中的涡卷效应补偿量
Figure 362910DEST_PATH_IMAGE186
计算得到，表示为：
Figure 300779DEST_PATH_IMAGE187
其中，捷联导航系统比力的二次积分增量
Figure 267598DEST_PATH_IMAGE188
、位置计算中的旋转效应补偿量
Figure 367141DEST_PATH_IMAGE189
与位置计算中的涡卷效应补偿量
Figure 52200DEST_PATH_IMAGE190
分别通过
Figure 29645DEST_PATH_IMAGE191
、
Figure 850971DEST_PATH_IMAGE192
两个时间段内的速度增量
Figure 121415DEST_PATH_IMAGE193
和角速度增量
Figure 28191DEST_PATH_IMAGE194
计算得到，分别表示为：
Figure 307863DEST_PATH_IMAGE195
Figure 983695DEST_PATH_IMAGE196
Figure 690620DEST_PATH_IMAGE197
S2、利用步骤S1更新后的捷联惯性导航系统数值和卫星导航接收机接收数值计算得到捷联惯性导航系统下伪距和伪距率，并通过卫星导航接收机得到卫星导航系统下伪距和伪距率；
利用步骤S1更新后的捷联惯性导航系统速度和位置信息
Figure 84692DEST_PATH_IMAGE198
、
Figure 401011DEST_PATH_IMAGE199
与卫星接收机接收的地心地固坐标系下的卫星速度和位置信息
Figure 196928DEST_PATH_IMAGE200
作差，得到捷联惯性导航系统的伪距
Figure 74754DEST_PATH_IMAGE201
和伪距率
Figure 690543DEST_PATH_IMAGE202
，并通过卫星接收机观测得到卫星导航系统的伪距
Figure 312018DEST_PATH_IMAGE203
和伪距率
Figure 962442DEST_PATH_IMAGE204
。
S3、利用卡尔曼滤波对根据步骤S2中捷联惯性导航系统下伪距和伪距率与卫星导航系统下伪距和伪距率计算得到的量测量进行误差分析，得到捷联惯性导航系统误差；
本实施例中，以发射关系坐标系下捷联惯性导航系统误差和卫星误差作为状态量，以步骤S2得到的捷联惯性导航系统的伪距
Figure 11169DEST_PATH_IMAGE205
和伪距率
Figure 379834DEST_PATH_IMAGE206
和卫星接收机观测得到卫星导航系统的伪距
Figure 40884DEST_PATH_IMAGE207
和伪距率
Figure 545815DEST_PATH_IMAGE208
作差得到的量测量，进行卡尔曼滤波，得到捷联惯性导航系统的姿态、速度和位置误差
Figure 499865DEST_PATH_IMAGE209
。
实际中，卡尔曼滤波系统方程由状态方程和量测量方程组成，状态方程根据发射惯性坐标系下捷联惯性导航系统误差方程得到，包括姿态误差方程、速度误差方程和位置误差方程，分别表示为：
Figure 90246DEST_PATH_IMAGE210
Figure 53523DEST_PATH_IMAGE211
Figure 678539DEST_PATH_IMAGE212
其中，
Figure 69069DEST_PATH_IMAGE213
为姿态误差，由计算坐标系与真实导航坐标系之间的姿态误差得到，
Figure 146747DEST_PATH_IMAGE214
为
Figure 146670DEST_PATH_IMAGE215
的微分；
Figure 626193DEST_PATH_IMAGE216
为速度误差，由计算坐标系与真实导航坐标系之间的速度误差得到，
Figure 187625DEST_PATH_IMAGE217
为
Figure 752598DEST_PATH_IMAGE218
的微分；
Figure 729781DEST_PATH_IMAGE219
为位置误差，由计算坐标系与真实导航坐标系之间的位置误差得到，
Figure 454024DEST_PATH_IMAGE220
为
Figure 61723DEST_PATH_IMAGE221
的微分；
Figure 973047DEST_PATH_IMAGE222
为陀螺测量误差，由IMU器件参数得到；
Figure 957183DEST_PATH_IMAGE223
为载体坐标系b系到发射惯性坐标系a系的转换矩阵；
Figure 37397DEST_PATH_IMAGE224
为相对于发射惯性坐标系下的比力
Figure 815997DEST_PATH_IMAGE225
的反对称矩阵，且
Figure 214618DEST_PATH_IMAGE226
；
Figure 736866DEST_PATH_IMAGE227
为发射惯性坐标系下的重力矢量模型。
实际中，载体坐标系b系到发射惯性坐标系a系的转换矩阵
Figure 170121DEST_PATH_IMAGE228
表示为：
Figure 119623DEST_PATH_IMAGE229
其中，
Figure 271118DEST_PATH_IMAGE230
分别为发射惯性坐标系下的俯仰角、偏航角和滚转角。
实际中，发射惯性坐标系下的重力矢量模型
Figure 331478DEST_PATH_IMAGE231
表示为:
Figure 383354DEST_PATH_IMAGE232
其中，
Figure 503757DEST_PATH_IMAGE233
为万有引力常数，M为地球的质量，
Figure 876970DEST_PATH_IMAGE234
分别为载体在发射惯性坐标系下位置矢量的三轴分量，
Figure 475441DEST_PATH_IMAGE235
为地球平均半径，r为载体在发射惯性坐标系下的相对于地心的位置矢量，表示为
Figure 883289DEST_PATH_IMAGE236
。
如图3所示，本实施例中，步骤S3具体包括以下分步骤：
S31、利用步骤S2得到的捷联惯性导航系统的伪距
Figure 174593DEST_PATH_IMAGE237
和伪距率
Figure 35101DEST_PATH_IMAGE238
，分别与卫星导航系统的伪距
Figure 702843DEST_PATH_IMAGE239
和伪距率
Figure 466662DEST_PATH_IMAGE240
作差，得到量测量
Figure 928867DEST_PATH_IMAGE241
；
S32、以发射惯性坐标系下的捷联惯性导航系统误差与卫星定位误差作为状态量，构建卡尔曼滤波状态方程，表示为：
Figure 276672DEST_PATH_IMAGE242
其中，
Figure 748105DEST_PATH_IMAGE243
为卡尔曼滤波的状态矢量，
Figure 537069DEST_PATH_IMAGE244
为姿态误差，
Figure 294810DEST_PATH_IMAGE245
为姿态误差的微分，
Figure 284238DEST_PATH_IMAGE246
为速度误差，
Figure 293782DEST_PATH_IMAGE247
为速度误差的微分，
Figure 265149DEST_PATH_IMAGE248
为位置误差，
Figure 334736DEST_PATH_IMAGE249
为位置误差的微分，
Figure 893019DEST_PATH_IMAGE250
为陀螺仪零偏，
Figure 440675DEST_PATH_IMAGE251
为陀螺仪零偏的微分，
Figure 266549DEST_PATH_IMAGE252
为加速度计零偏，
Figure 772616DEST_PATH_IMAGE253
为加速度计零偏的微分，
Figure 51151DEST_PATH_IMAGE254
为卫星收机时钟偏差造成的距离误差，
Figure 402498DEST_PATH_IMAGE255
卫星接收机时钟偏差造成的距离误差的微分，
Figure 348457DEST_PATH_IMAGE256
为卫星接收机时钟漂移，
Figure 759847DEST_PATH_IMAGE257
为卫星接收机时钟漂移的微分，
Figure 313229DEST_PATH_IMAGE258
分别为陀螺仪、加速度计、时钟偏差和时钟漂移的白噪声，
Figure 468267DEST_PATH_IMAGE259
分别表示矩阵维数为
Figure 268733DEST_PATH_IMAGE260
的零矩阵；
实际中，卡尔曼滤波系统方程由状态方程和量测量方程组成，状态方程根据发射惯性坐标系下捷联惯性导航系统误差方程得到，包括姿态误差方程、速度误差方程和位置误差方程，分别表示为：
Figure 851024DEST_PATH_IMAGE261
Figure 369730DEST_PATH_IMAGE262
Figure 859617DEST_PATH_IMAGE263
其中，
Figure 655535DEST_PATH_IMAGE264
为姿态误差，由计算坐标系与真实导航坐标系之间的姿态误差得到，
Figure 267782DEST_PATH_IMAGE265
为姿态误差
Figure 149150DEST_PATH_IMAGE266
的微分；
Figure 272089DEST_PATH_IMAGE267
为速度误差，由计算坐标系与真实导航坐标系之间的速度误差得到，
Figure 922513DEST_PATH_IMAGE268
为速度误差
Figure 971241DEST_PATH_IMAGE269
的微分；
Figure 74326DEST_PATH_IMAGE270
为位置误差，由计算坐标系与真实导航坐标系之间的位置误差得到，
Figure 233912DEST_PATH_IMAGE271
为位置误差
Figure 4422DEST_PATH_IMAGE272
的微分；
Figure 224051DEST_PATH_IMAGE273
为陀螺测量误差，由IMU器件参数得到；
Figure 814432DEST_PATH_IMAGE274
为载体坐标系b系到发射惯性坐标系a系的转换矩阵；
Figure 276244DEST_PATH_IMAGE275
为相对于发射惯性坐标系下的比力
Figure 901260DEST_PATH_IMAGE276
的反对称矩阵，且
Figure 26211DEST_PATH_IMAGE277
；
Figure 103889DEST_PATH_IMAGE278
为发射惯性坐标系下的重力矢量模型。
实际中，载体坐标系b系到发射惯性坐标系a系的转换矩阵
Figure 605277DEST_PATH_IMAGE279
表示为：
Figure 84800DEST_PATH_IMAGE280
其中，
Figure 646231DEST_PATH_IMAGE281
分别为发射惯性坐标系下的俯仰角、偏航角和滚转角。
实际中，发射惯性坐标系下的重力矢量模型
Figure 211205DEST_PATH_IMAGE282
表示为：
Figure 17749DEST_PATH_IMAGE283
其中，f为万有引力常数，M为地球的质量，
Figure 351778DEST_PATH_IMAGE284
分别为载体在发射惯性坐标系下位置矢量的三轴分量，
Figure 84111DEST_PATH_IMAGE285
为地球平均半径，r为载体在发射惯性坐标系下的相对于地心的位置矢量，表示为
Figure 136381DEST_PATH_IMAGE286
。
S33、利用步骤S31得到的量测量
Figure 245151DEST_PATH_IMAGE287
、
Figure 230425DEST_PATH_IMAGE288
，构建卡尔曼滤波量测方程，表示为：
Figure 9025DEST_PATH_IMAGE289
其中，
Figure 673224DEST_PATH_IMAGE290
为从发射惯性坐标系下的误差矢量转换到地心地固坐标系下的转换矩阵，M为当前可见卫星颗数，
Figure 195472DEST_PATH_IMAGE291
为M行的列向量，
Figure 330525DEST_PATH_IMAGE292
分别表示矩阵维数为
Figure 404661DEST_PATH_IMAGE293
的零矩阵，
Figure 228260DEST_PATH_IMAGE294
为卫星位置的测量误差矩阵，
Figure 288620DEST_PATH_IMAGE295
为卫星速度的测量误差矩阵，
Figure 576382DEST_PATH_IMAGE296
为卡尔曼滤波的状态矢量；
实际中，从发射惯性坐标系下的误差矢量转换到地心地固坐标系下的转换矩阵
Figure 962364DEST_PATH_IMAGE297
表示为：
Figure 335576DEST_PATH_IMAGE298
其中，
Figure 199627DEST_PATH_IMAGE299
为卫星到捷联惯导解算出的载体位置的视线矩阵，
Figure 108940DEST_PATH_IMAGE300
为a系到e系的转换矩阵，
Figure 400244DEST_PATH_IMAGE301
为绕z轴旋转
Figure 260752DEST_PATH_IMAGE302
角的姿态旋转矩阵。
实际中，卫星到捷联惯导解算出的载体位置的视线矩阵
Figure 928494DEST_PATH_IMAGE303
，表示为：
Figure 190848DEST_PATH_IMAGE304
其中，
Figure 653053DEST_PATH_IMAGE305
为从第m颗卫星到捷联惯导解算出的载体位置的视线单位矢量，表示为：
Figure 735279DEST_PATH_IMAGE306
其中，
Figure 206712DEST_PATH_IMAGE307
为捷联惯导系统解算出的在地心地固坐标系下的载体位置，
Figure 822107DEST_PATH_IMAGE308
为第m颗卫星的在地心地固坐标系下的位置。
实际中，a系到e系的转换矩阵
Figure 455214DEST_PATH_IMAGE309
，表示为：
Figure 290315DEST_PATH_IMAGE310
其中，
Figure 34280DEST_PATH_IMAGE311
为发射方位角，
Figure 271226DEST_PATH_IMAGE312
为发射点地理纬度，
Figure 75234DEST_PATH_IMAGE313
为发射点地理经度。
实际中，绕z轴旋转
Figure 397631DEST_PATH_IMAGE314
角的姿态旋转矩阵
Figure 945287DEST_PATH_IMAGE315
表示为：
Figure 708844DEST_PATH_IMAGE316
其中，
Figure 575431DEST_PATH_IMAGE317
为地球旋转角速度。
S34、利用步骤S32得到的卡尔曼状态方程与步骤S33得到的卡尔曼滤波量测方程构建卡尔曼滤波系统，并利用该卡尔曼滤波系统计算得到捷联惯性导航系统姿态、速度、位置误差
Figure 994911DEST_PATH_IMAGE318
。
S4、利用步骤S3得到的捷联惯性导航系统误差与步骤S1更新后的捷联惯性导航系统数值进行修正，得到修正后的捷联惯性导航系统数值；
本实施例中，利用步骤S3得到的捷联惯性导航系统姿态、速度、位置误差
Figure 470891DEST_PATH_IMAGE319
与步骤S1更新后的捷联惯性导航系统姿态、速度和位置信息作差，得到修正后的捷联惯性导航系统姿态、速度和位置信息
Figure 292217DEST_PATH_IMAGE320
。
S5、利用步骤S4修正后的捷联惯性导航系统数值对捷联惯性导航系统进行校正与反馈。
本实施例中，利用步骤S4得到的修正后的捷联惯性导航系统姿态、速度和位置信息
Figure 828241DEST_PATH_IMAGE321
作为捷联惯性导航系统下一解算周期数值，对捷联惯性导航系统进行校正和反馈。
为了验证发射惯性坐标系下的紧耦合导航方法的准确性和优越性，根据上述实施方案通过数字仿真试验，将发射惯性系SINS/GPS松、紧耦合的组合导航结果进行对比。该数字仿真试验采用一条时长1100s的高超声速飞行器经典飞行轨迹进行仿真验证，该轨迹的初始状态为：速度为0m/s，纬度为34.2°，经度为108.9°，高度为400m，射向为200°，俯仰角为90°，横滚角为0°，偏航角为0°。飞行轨迹如图4所示。
仿真参数，如表1所示：
表 1 仿真参数表
Figure 735017DEST_PATH_IMAGE322
图5-图7为发射惯性系下松、紧耦合组合导航数字仿真结果对比，从图中可以看出，紧耦合的俯仰角和滚转角均收敛到0.015°以内，偏航角在970s漂移到0.16°，之后收敛到0.015°以内；三个方向的速度误差均可收敛到0.05m/s以内，三个方向的位置误差均可收敛到5m以内。松耦合的三个姿态角均收敛到0.02°以内，三个方向的速度误差均可收敛到0.2m/s以内，三个方向的位置误差均可收敛到8m以内。从总体来看，紧耦合的导航精度优于松耦合。
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (8)

1.一种发射惯性坐标系下的紧耦合导航方法，其特征在于，包括以下步骤：
S1、利用数值更新算法更新发射惯性坐标系下的捷联惯性导航系统数值；
S2、利用步骤S1更新后的捷联惯性导航系统数值和卫星导航接收机接收数值计算得到捷联惯性导航系统下伪距和伪距率，并通过卫星导航接收机得到卫星导航系统下伪距和伪距率；
S3、利用卡尔曼滤波对根据步骤S2中捷联惯性导航系统下伪距和伪距率与卫星导航系统下伪距和伪距率计算得到的量测量进行误差分析，得到捷联惯性导航系统误差，具体包括以下分步骤：
S31、利用步骤S2得到的捷联惯性导航系统的伪距
Figure 826935DEST_PATH_IMAGE001
和伪距率
Figure 249827DEST_PATH_IMAGE002
，分别与卫星导航系统的伪距
Figure 699262DEST_PATH_IMAGE003
和伪距率
Figure 396960DEST_PATH_IMAGE004
作差，得到量测量
Figure 349873DEST_PATH_IMAGE005
、
Figure 209244DEST_PATH_IMAGE006
；
S32、以发射惯性坐标系下的捷联惯性导航系统误差与卫星定位误差作为状态量，构建卡尔曼滤波状态方程，表示为：
Figure 880397DEST_PATH_IMAGE007
其中，
Figure 257152DEST_PATH_IMAGE008
为卡尔曼滤波的状态矢量，
Figure 330150DEST_PATH_IMAGE009
为姿态误差，
Figure 360423DEST_PATH_IMAGE010
为姿态误差的微分，
Figure 518872DEST_PATH_IMAGE011
为速度误差，
Figure 292793DEST_PATH_IMAGE012
为速度误差的微分，
Figure 485877DEST_PATH_IMAGE013
为位置误差，
Figure 687051DEST_PATH_IMAGE014
为位置误差的微分，
Figure 598375DEST_PATH_IMAGE015
为陀螺仪零偏，
Figure 910408DEST_PATH_IMAGE016
为陀螺仪零偏的微分，
Figure 223577DEST_PATH_IMAGE017
为加速度计零偏，
Figure 595653DEST_PATH_IMAGE018
为加速度计零偏的微分，
Figure 728694DEST_PATH_IMAGE019
为卫星收机时钟偏差造成的距离误差，
Figure 985363DEST_PATH_IMAGE020
卫星接收机时钟偏差造成的距离误差的微分，
Figure 887460DEST_PATH_IMAGE021
为卫星接收机时钟漂移，
Figure 696016DEST_PATH_IMAGE022
为卫星接收机时钟漂移的微分，
Figure 50774DEST_PATH_IMAGE023
分别为陀螺仪、加速度计、时钟偏差和时钟漂移的白噪声，
Figure 235767DEST_PATH_IMAGE024
分别表示矩阵维数为
Figure 992371DEST_PATH_IMAGE025
的零矩阵；
S33、利用步骤S31得到的量测量
Figure 706249DEST_PATH_IMAGE026
、
Figure 79461DEST_PATH_IMAGE027
，构建卡尔曼滤波量测方程，表示为：
Figure 677933DEST_PATH_IMAGE028
其中，
Figure 289043DEST_PATH_IMAGE029
为从发射惯性坐标系下的误差矢量转换到地心地固坐标系下的转换矩阵，M为当前可见卫星颗数，
Figure 704981DEST_PATH_IMAGE030
为M行的列向量，
Figure 34331DEST_PATH_IMAGE031
、
Figure 295548DEST_PATH_IMAGE032
分别表示矩阵维数为
Figure 26744DEST_PATH_IMAGE033
的零矩阵，
Figure 348004DEST_PATH_IMAGE034
为卫星位置的测量误差矩阵，
Figure 164650DEST_PATH_IMAGE035
为卫星速度的测量误差矩阵，
Figure 963979DEST_PATH_IMAGE036
为卡尔曼滤波的状态矢量；
S34、利用步骤S32得到的卡尔曼状态方程与步骤S33得到的卡尔曼滤波量测方程构建卡尔曼滤波系统，并利用该卡尔曼滤波系统计算得到捷联惯性导航姿态误差
Figure 815260DEST_PATH_IMAGE037
、速度误差
Figure 307421DEST_PATH_IMAGE038
、位置误差
Figure 486730DEST_PATH_IMAGE039
；
S4、利用步骤S3得到的捷联惯性导航系统误差与步骤S1更新后的捷联惯性导航系统数值进行修正，得到修正后的捷联惯性导航系统数值；
S5、利用步骤S4修正后的捷联惯性导航系统数值对捷联惯性导航系统进行校正与反馈。
2.根据权利要求1所述的一种发射惯性坐标系下的紧耦合导航方法，其特征在于，所述步骤S1具体为：
根据加速度计敏感的比力信息和陀螺仪敏感的角速度信息采用数值更新算法中姿态数值更新算法、速度数值更新算法和位置数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统姿态、速度和位置信息进行数值更新，得到更新后的捷联惯性导航系统数值，其中数值更新算法采用二子样算法。
3.根据权利要求2所 述的一种发射惯性坐标系下的紧耦合导航方法，其特征在于，所述步骤S1中利用姿态数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统的姿态信息进行更新，具体为：
先利用陀螺仪敏感的角速度
Figure 89749DEST_PATH_IMAGE040
计算
Figure 61116DEST_PATH_IMAGE041
时刻到
Figure 458600DEST_PATH_IMAGE042
时刻的姿态转换四元数
Figure 515417DEST_PATH_IMAGE043
，再利用该姿态转换四元数
Figure 922128DEST_PATH_IMAGE044
对姿态更新前
Figure 482422DEST_PATH_IMAGE045
时刻的姿态进行更新，得到更新后
Figure 50807DEST_PATH_IMAGE046
时刻的姿态四元数
Figure 470287DEST_PATH_IMAGE047
，表示为：
Figure 415109DEST_PATH_IMAGE048
。
4.根据权利要求2所述的一种发射惯性坐标系下的紧耦合导航方法，其特征在于，所述步骤S1中利用速度数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统的速度信息进行更新，具体为：
先利用载体坐标系下加速度计敏感的比力
Figure 95489DEST_PATH_IMAGE049
计算捷联卫星导航系比力速度增量
Figure 365934DEST_PATH_IMAGE050
，再利用采样时段T内捷联卫星导航系统比力速度增量
Figure 866185DEST_PATH_IMAGE051
、采样时段T内捷联卫星导航引力速度增量
Figure 614698DEST_PATH_IMAGE052
对位置更新前
Figure 149585DEST_PATH_IMAGE053
时刻捷联惯性导航系统速度
Figure 59772DEST_PATH_IMAGE054
进行更新，得到更新后
Figure 188265DEST_PATH_IMAGE055
时刻的捷联惯性导航系统速度
Figure 740469DEST_PATH_IMAGE056
，表示为：
Figure 129862DEST_PATH_IMAGE057
。
5.根据权利要求2所述的一种发射惯性坐标系下的紧耦合导航方法，其特征在于，所述步骤S1中利用位置数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统的位置信息进行更新，具体包括：
利用采样时段T内捷联导航系统引力速度增量
Figure 742109DEST_PATH_IMAGE058
、更新前
Figure 216953DEST_PATH_IMAGE059
时刻下的捷联惯性导航系统速度
Figure 65127DEST_PATH_IMAGE060
、载体坐标系b系到发射惯性坐标系a系的转换矩阵
Figure 574605DEST_PATH_IMAGE061
与捷联导航系统比力速度增量引起的位置增量
Figure 233120DEST_PATH_IMAGE062
对更新前
Figure 460839DEST_PATH_IMAGE063
时刻下的捷联导航系统位置
Figure 89266DEST_PATH_IMAGE064
进行更新，得到更新后
Figure 453252DEST_PATH_IMAGE065
时刻的捷联惯性导航系统位置
Figure 141722DEST_PATH_IMAGE066
，表示为：
Figure 591158DEST_PATH_IMAGE067
。
6.根据权利要求1所述的一种发射惯性坐标系下的紧耦合导航方法，其特征在于，所述步骤S2具体为：
利用步骤S1更新后的捷联惯性导航系统速度
Figure 757697DEST_PATH_IMAGE068
和位置
Figure 241768DEST_PATH_IMAGE069
与卫星接收机接收的地心地固坐标系下的卫星速度和位置信息作差，得到捷联惯性导航系统的伪距
Figure 101139DEST_PATH_IMAGE070
和伪距率
Figure 772292DEST_PATH_IMAGE002
，并通过卫星接收机观测得到卫星导航系统的伪距
Figure 8101DEST_PATH_IMAGE071
和伪距率
Figure 81100DEST_PATH_IMAGE072
。
7.权利要求1所述的一种发射惯性坐标系下的紧耦合导航方法，其特征在于，步骤S4具体为：
利用步骤S3得到的捷联惯性导航系统姿态误差
Figure 111373DEST_PATH_IMAGE073
、速度误差
Figure 269821DEST_PATH_IMAGE038
、位置误差
Figure 778163DEST_PATH_IMAGE074
与步骤S1更新后的捷联惯性导航系统姿态、速度和位置信息作差，得到修正后的捷联惯性导航系统姿态信息
Figure 705668DEST_PATH_IMAGE075
、速度信息
Figure 906842DEST_PATH_IMAGE076
和位置信息
Figure 552587DEST_PATH_IMAGE077
。
8.据权利要求1所述的一种发射惯性坐标系下的紧耦合导航方法，其特征在于，步骤S5具体为：
利用步骤S4得到的修正后的捷联惯性导航系统姿态信息
Figure 599041DEST_PATH_IMAGE078
、速度信息
Figure 974527DEST_PATH_IMAGE079
和位置信息
Figure 81023DEST_PATH_IMAGE080
作为捷联惯性导航系统下一解算周期数值，对捷联惯性导航系统进行校正和反馈。
CN202110611333.8A 2021-06-02 2021-06-02 一种发射惯性坐标系下的紧耦合导航方法 Active CN113050143B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110611333.8A CN113050143B (zh) 2021-06-02 2021-06-02 一种发射惯性坐标系下的紧耦合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110611333.8A CN113050143B (zh) 2021-06-02 2021-06-02 一种发射惯性坐标系下的紧耦合导航方法

Publications (2)

Publication Number Publication Date
CN113050143A CN113050143A (zh) 2021-06-29
CN113050143B true CN113050143B (zh) 2021-08-10

Family

ID=76518691

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110611333.8A Active CN113050143B (zh) 2021-06-02 2021-06-02 一种发射惯性坐标系下的紧耦合导航方法

Country Status (1)

Country Link
CN (1) CN113050143B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113884704B (zh) * 2021-09-23 2023-07-14 中国人民解放军63620部队 运载火箭多源多类测量数据速度基准高精度对齐方法
CN115248038B (zh) * 2022-09-21 2022-12-30 河北斐然科技有限公司 一种发射系下的sins/bds组合导航工程算法
CN115855104A (zh) * 2022-11-25 2023-03-28 哈尔滨工程大学 一种组合导航滤波结果最优在线评价方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106767787A (zh) * 2016-12-29 2017-05-31 北京时代民芯科技有限公司 一种紧耦合gnss/ins组合导航装置
CN111580144A (zh) * 2020-05-07 2020-08-25 西北工业大学 一种mins/gps超紧组合导航系统设计方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
SE0300303D0 (sv) * 2003-02-06 2003-02-06 Nordnav Technologies Ab A navigation Method and Apparatus
US7579984B2 (en) * 2005-11-23 2009-08-25 The Boeing Company Ultra-tightly coupled GPS and inertial navigation system for agile platforms
US7724184B2 (en) * 2006-02-21 2010-05-25 Honeywell International Inc. System and method for detecting false navigation signals
CN108761512A (zh) * 2018-07-28 2018-11-06 南京理工大学 一种弹载bds/sins深组合自适应ckf滤波方法
CN109084762A (zh) * 2018-08-12 2018-12-25 西北工业大学 基于惯导辅助单星定位的卡尔曼滤波动目标定位方法
CN109307879A (zh) * 2018-11-08 2019-02-05 北京理工大学 一种基于ins辅助gnss矢量跟踪环的导航方法
CN112146655B (zh) * 2020-08-31 2023-03-31 郑州轻工业大学 一种BeiDou/SINS紧组合导航系统弹性模型设计方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106767787A (zh) * 2016-12-29 2017-05-31 北京时代民芯科技有限公司 一种紧耦合gnss/ins组合导航装置
CN111580144A (zh) * 2020-05-07 2020-08-25 西北工业大学 一种mins/gps超紧组合导航系统设计方法

Also Published As

Publication number Publication date
CN113050143A (zh) 2021-06-29

Similar Documents

Publication Publication Date Title
CN113050143B (zh) 一种发射惯性坐标系下的紧耦合导航方法
CN106342284B (zh) 一种飞行载体姿态确定方法
CN104344837B (zh) 一种基于速度观测的冗余惯导系统加速度计系统级标定方法
CN105371844B (zh) 一种基于惯性/天文互助的惯性导航系统初始化方法
CN108535755A (zh) 基于mems的gnss/imu车载实时组合导航方法
US7957899B2 (en) Method for determining the attitude, position, and velocity of a mobile device
CN103245360A (zh) 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
Bezick et al. Inertial navigation for guided missile systems
CN109556631A (zh) 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法
CN105180728B (zh) 基于前数据的旋转制导炮弹快速空中对准方法
CN112611394B (zh) 一种在发射坐标系下的飞行器姿态对准方法及系统
CN105258698A (zh) 一种高动态自旋制导炮弹空中组合导航方法
US7248967B2 (en) Autonomous velocity estimation and navigation
CN110243362B (zh) 一种中高空超声速靶标导航方法
CN105115508A (zh) 基于后数据的旋转制导炮弹快速空中对准方法
CN109269526A (zh) 基于阻尼网络的旋转式格网惯导水平阻尼方法
CN110017808B (zh) 利用地磁信息和加速计解算飞行器姿态的方法
CN114993305A (zh) 一种基于发射坐标系的制导炮弹组合导航方法
CN110332933A (zh) 车辆定位方法、终端及计算机可读存储介质
CN113418499B (zh) 一种旋转飞行器滚转角解算方法及系统
CN109029499A (zh) 一种基于重力视运动模型的加速度计零偏迭代寻优估计方法
CN114353784B (zh) 一种基于运动矢量的制导炮弹空中姿态辨识方法
CN106643726B (zh) 一种统一惯性导航解算方法
CN110017809B (zh) 利用地磁信息和光流传感器解算飞行器姿态的方法
CN114509071A (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