CN106895855B - 一种惯性导航初始基准的估计与补偿方法 - Google Patents

一种惯性导航初始基准的估计与补偿方法 Download PDF

Info

Publication number
CN106895855B
CN106895855B CN201710239543.2A CN201710239543A CN106895855B CN 106895855 B CN106895855 B CN 106895855B CN 201710239543 A CN201710239543 A CN 201710239543A CN 106895855 B CN106895855 B CN 106895855B
Authority
CN
China
Prior art keywords
angle
inertial navigation
initial
speed
error
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.)
Expired - Fee Related
Application number
CN201710239543.2A
Other languages
English (en)
Other versions
CN106895855A (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.)
Beijing Aerospace Automatic Control Research Institute
Original Assignee
Beijing Aerospace Automatic Control Research Institute
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 Aerospace Automatic Control Research Institute filed Critical Beijing Aerospace Automatic Control Research Institute
Priority to CN201710239543.2A priority Critical patent/CN106895855B/zh
Publication of CN106895855A publication Critical patent/CN106895855A/zh
Application granted granted Critical
Publication of CN106895855B publication Critical patent/CN106895855B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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

Landscapes

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

Abstract

本发明公开了一种惯性导航初始基准的估计与补偿方法，通过卫星导航提供的位置、速度信息，和惯性导航计算得到的位置、速度信息采用卡尔曼滤波的方式，估计惯性导航初始基准误差，修正时结合姿态控制要求采用程序角调姿的方式，对姿态角进行修正，同时对速度和位置进行修正，最终实现提高姿态角解算精度，提高中制导与末制导交班精度的目的。

Description

一种惯性导航初始基准的估计与补偿方法
技术领域
本发明涉及惯性导航系统领域，具体涉及一种惯性导航初始基准的估计与补偿方法。
背景技术
在正式开始导航工作之前，惯性导航系统需要知道一些初始条件，例如初始姿态、初始速度和初始位置；通常情况初始速度和初始位置易于获得，而初始姿态中水平两个方向的初始姿态角通过自对准比较容易获得，但是航向角通过自对准获得则需要较长的时间或者需借助外部瞄准等辅助系统；若是为实现快速性的要求的情况下，航向角的初值误差会比较大，这样就会导致惯性导航的误差快速积累，导致导航的速度和位置精准度过低；因此需要利用惯性/卫星组合导航系统对姿态初值进行修正，以提高姿态角解算精度，达到提高中制导与末制导交班精度的目的。
发明内容
本发明的目的是提供一种惯性导航初始基准的估计与补偿方法，适用于惯性/卫星组合导航系统中对姿态角误差有约束条件的飞行器导航制导方案选取，解决在惯性导航初始对准误差较大的情况下，对姿态角进行修正的问题。
为了解决上述技术问题，本发明的实施例提供了一种惯性导航初始基准的估计与补偿方法，惯性导航系统接收卫星导航提供的位置、速度信息，以及惯性导航系统计算得到的位置、速度信息，该方法包括如下步骤：
步骤a)建立初始姿态误差估计状态方程：
步骤b)建立姿态初值误差估计观测方程：
Z＝HX+V；
步骤c)计算观测量，将惯性导航速度和位置与卫星导航给出的速度和位置作差作为观测量：
步骤d)将状态方程和观测方程离散化：
Xk＝Φk,k-1Xk-1+Wk-1
Zk＝HkXk+Vk；
步骤e)使用卡尔曼滤波方程计算：
Pk＝(I-KkHk)Pk|k-1；
步骤f)姿态误差角的修正
姿态角偏差：
修正后的姿态角：
程序角计算：
ψcx＝ψcx-δψ·k
γcx＝γcx-δγ·k；
步骤g)速度和位置误差的修正
导航速度修正：V＝V-W·Φ0，
导航位置修正：X＝X-S·Φ0。
其中，所述步骤a中：
X＝[φx0 φy0 φz0 δVx0 δVy0 δVz0 δX0 δY 0δZ0]T
F＝O9×9；
初始姿态误差角：φx0、φy0、φz0，
速度初始误差：δVx0、δVy0、δVz0，
位置初始误差：δX0、δY0、δZ0。
其中，所述步骤b中：
其中，所述I3×3为3行3列的单位矩阵，Wx、Wy、Wz为视速度，Sx、Sy、Sz为视位置。
其中，所述步骤c中，VxINS，VyINS，VzINS为惯性导航速度，XINS，YINS，ZINS为惯性导航位置，VxGNSS，VxGNSS，VxGNSS为卫星导航给出的速度，XGNSS，YGNSS，YGNSS为卫星导航给出的位置。
其中，所述步骤d中，Φk,k-1＝I9×9，Qk-1＝O9×9，Hk＝H。
其中，所述步骤e中，滤波方程初值为：X0＝O9×1，P0和R0根据具体情况选取。
其中，所述步骤f中，ψ0、γ0为当前俯仰角、当前偏航角，当前滚动角，δψ，δγ为俯仰姿态角偏差、偏航姿态角偏差、滚动姿态角偏差；ψcx、γcx为俯仰程序角、偏航程序角、滚动程序角。
其中，所述步骤f中，系数k计算公式如下：
其中，t为当前时间，t0为修正起始时刻，Δt为修正时间。
其中，所述步骤g中，
本发明通过卫星导航提供的位置、速度信息，和惯性导航计算得到的位置、速度信息采用卡尔曼滤波的方式，估计惯性导航初始基准误差，修正时结合姿态控制要求采用程序角调姿的方式，对姿态角进行修正，同时对速度和位置进行修正，最终实现提高姿态角解算精度，达到提高中制导与末制导交班精度的目的。
与现有技术相比，本发明的有益之处在于：1)满足惯性导航快速初始对准的要求，缩短运载火箭和导弹武器的发射准备时间；2)在飞行过程中对初始姿态误差进行估计和补偿，以消除初始姿态误差；3)还可以消除在飞行过程中对初始姿态误差带来的速度和位置误差。
附图说明
图1为本发明实施例的惯性导航初始基准的估计与补偿方法流程图。
具体实施方式
为使本发明的目的、技术方案及优点更加清楚明白，以下参照附图并举实施例，对本发明进一步详细说明。
本发明的原理如下。本发明根据卫星导航提供的位置、速度信息，和惯性导航计算得到的位置、速度信息采用卡尔曼滤波的方式，估计惯性导航初始基准误差，修正时结合姿态控制要求采用程序角调姿的方式，对姿态角进行修正，同时对速度和位置进行修正。
下面结合说明书附图对本发明作进一步详细描述。图1示出了本发明实施例的惯性导航初始基准的估计与补偿方法流程图。参见图1，根据本发明实施例的惯性导航初始基准的估计与补偿方法包括以下步骤：
步骤a)建立初始姿态误差估计状态方程：
步骤b)建立姿态初值误差估计观测方程：
Z＝HX+V；
步骤c)计算观测量，将惯性导航速度和位置与卫星导航给出的速度和位置作差作为观测量：
步骤d)将状态方程和观测方程离散化：
Xk＝Φk,k-1Xk-1+Wk-1
Zk＝HkXk+Vk；
步骤e)使用卡尔曼滤波方程计算：
Pk＝(I-KkHk)Pk|k-1；
步骤f)姿态误差角的修正，其中
姿态角偏差：
修正后的姿态角：
程序角计算：
ψcx＝ψcx-δψ·k
γcx＝γcx-δγ·k；
步骤g)速度和位置误差的修正，其中
导航速度修正：V＝V-W·Φ0，
导航位置修正：X＝X-S·Φ0。
其中，所述步骤a中：
X＝[φx0 φy0 φz0 δVx0 δVy0 δVz0 δX0 δY0 δZ0]T
F＝O9×9；
初始姿态误差角：φx0、φy0、φz0，
速度初始误差：δVx0、δVy0、δVz0，
位置初始误差：δX0、δY0、δZ0。
其中，所述步骤b中：
其中，所述I3×3为3行3列的单位矩阵，Wx、Wy、Wz为视速度，Sx、Sy、Sz为视位置。
其中，所述步骤c中，VxINS，VyINS，VzINS为惯性导航速度，XINS，YINS，ZINS为惯性导航位置，VxGNSS，VxGNSS，VxGNSS为卫星导航给出的速度，XGNSS，YGNSS，YGNSS为卫星导航给出的位置。
其中，所述步骤d中，Φk,k-1＝I9×9，Qk-1＝O9×9，Hk＝H。
其中，所述步骤e中，滤波方程初值为：X0＝O9×1，P0和R0根据具体情况选取。
其中，所述步骤f中，ψ0、γ0为当前俯仰角、当前偏航角，当前滚动角，δψ，δγ为俯仰姿态角偏差、偏航姿态角偏差、滚动姿态角偏差；ψcx、γcx为俯仰程序角、偏航程序角、滚动程序角。
其中，所述步骤f中，系数k计算公式如下：
其中，t为当前时间，t0为修正起始时刻，Δt为修正时间。
其中，所述步骤g中，
本发明可以满足惯性导航快速初始对准的要求，在飞行过程中对初始姿态误差进行估计和补偿，以消除初始姿态误差，以及由此带来的速度和位置误差，对缩短导弹武器发射准备时间具有重要意义。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

Claims (3)

1.一种惯性导航初始基准的估计与补偿方法，惯性导航系统接收卫星导航提供的位置、速度信息，以及惯性导航系统计算得到的位置、速度信息，其特征在于，该方法包括如下步骤：
步骤a)建立初始姿态误差估计状态方程：
步骤b)建立姿态初值误差估计观测方程：
Z＝HX+V；
步骤c)计算观测量，将惯性导航速度和位置与卫星导航给出的速度和位置作差作为观测量：
步骤d)将状态方程和观测方程离散化：
Xk＝Φk,k-1Xk-1+Wk-1
Zk＝HkXk+Vk；
步骤e)使用卡尔曼滤波方程计算：
Pk＝(I-KkHk)Pk|k-1；
步骤f)姿态误差角的修正
姿态角偏差：
修正后的姿态角：
程序角计算：
ψcx＝ψcx-δψ·k
γcx＝γcx-δγ·k；
步骤g)速度和位置误差的修正
导航速度修正：V＝V-W·Φ0，
导航位置修正：X＝X-S·Φ0；
其中，所述步骤a中：
X＝[φx0 φy0 φz0 δVx0 δVy0 δVz0 δX0 δY0 δZ0]T
F＝O9×9；
初始姿态误差角：φx0、φy0、φz0，
速度初始误差：δVx0、δVy0、δVz0，
位置初始误差：δX0、δY0、δZ0；
其中，所述步骤b中：
其中，所述I3×3为3行3列的单位矩阵，Wx、Wy、Wz为视速度，Sx、Sy、Sz为视位置；
其中，VxINS，VyINS，VzINS为惯性导航速度，XINS，YINS，ZINS为惯性导航位置，VxGNSS，VxGNSS，VxGNSS为卫星导航给出的速度，XGNSS，YGNSS，YGNSS为卫星导航给出的位置；
其中，所述步骤d中，Φk,k-1＝I9×9，Qk-1＝O9×9，Hk＝H；
其中，所述步骤f中，ψ0、γ0为当前俯仰角、当前偏航角，当前滚动角，δψ，δγ为俯仰姿态角偏差、偏航姿态角偏差、滚动姿态角偏差；ψcx、γcx为俯仰程序角、偏航程序角、滚动程序角。
2.根据权利要求1所述的一种惯性导航初始基准的估计与补偿方法，其中，所述步骤e中，滤波方程初值为：X0＝O9×1，P0和R0根据具体情况选取。
3.根据权利要求1所述的一种惯性导航初始基准的估计与补偿方法，其特征在于：所述步骤g中
CN201710239543.2A 2017-04-13 2017-04-13 一种惯性导航初始基准的估计与补偿方法 Expired - Fee Related CN106895855B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710239543.2A CN106895855B (zh) 2017-04-13 2017-04-13 一种惯性导航初始基准的估计与补偿方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710239543.2A CN106895855B (zh) 2017-04-13 2017-04-13 一种惯性导航初始基准的估计与补偿方法

Publications (2)

Publication Number Publication Date
CN106895855A CN106895855A (zh) 2017-06-27
CN106895855B true CN106895855B (zh) 2019-05-28

Family

ID=59197363

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710239543.2A Expired - Fee Related CN106895855B (zh) 2017-04-13 2017-04-13 一种惯性导航初始基准的估计与补偿方法

Country Status (1)

Country Link
CN (1) CN106895855B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111156105B (zh) * 2019-12-31 2020-08-25 北京星际荣耀空间科技有限公司 一种火箭发动机参数校准方法及火箭
CN112284186B (zh) * 2020-09-24 2022-08-12 北京航天自动控制研究所 一种通过减小运载火箭滚动角偏差确保起飞安全的方法
CN112810834B (zh) * 2020-12-23 2022-11-11 北京航天自动控制研究所 一种同时考核惯性导航和模拟飞行的地面试验方法
CN114253282B (zh) * 2021-12-21 2023-09-22 航天科工火箭技术有限公司 运载火箭的姿态控制方法、装置、设备及存储介质

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4534000A (en) * 1975-08-29 1985-08-06 Bliss John H Inertial flight director system
US8359139B2 (en) * 2008-09-15 2013-01-22 Cnh America Llc Method and system for vehicle orientation measurement
CN102486377B (zh) * 2009-11-17 2014-10-22 哈尔滨工程大学 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法
JP2012207919A (ja) * 2011-03-29 2012-10-25 Toyota Central R&D Labs Inc 異常値判定装置、測位装置、及びプログラム
CN104635251B (zh) * 2013-11-08 2017-07-07 中国地质大学（北京） 一种ins/gps组合定位定姿新方法
CN103925917B (zh) * 2014-05-05 2016-10-26 上海新跃仪表厂 一种运载火箭姿态角速率信号的测量系统及方法
CN104567545B (zh) * 2014-10-30 2016-08-24 中国运载火箭技术研究院 Rlv大气层内主动段的制导方法
CN104748763B (zh) * 2015-03-19 2017-11-28 北京航天自动控制研究所 适用于车载晃动的捷联惯组快速对准方法
CN105371844B (zh) * 2015-12-02 2018-02-16 南京航空航天大学 一种基于惯性/天文互助的惯性导航系统初始化方法

Also Published As

Publication number Publication date
CN106895855A (zh) 2017-06-27

Similar Documents

Publication Publication Date Title
CN106895855B (zh) 一种惯性导航初始基准的估计与补偿方法
US8519313B2 (en) Projectile navigation enhancement method
CN105115508B (zh) 基于后数据的旋转制导炮弹快速空中对准方法
CN106052716B (zh) 惯性系下基于星光信息辅助的陀螺误差在线标定方法
KR20160127734A (ko) 관성 항법 시스템
CN109724624B (zh) 一种适用于机翼挠曲变形的机载自适应传递对准方法
CN106931967B (zh) 一种助推-滑翔式临近空间飞行器的捷联惯性导航方法
CN113503894B (zh) 基于陀螺基准坐标系的惯导系统误差标定方法
CN103256928A (zh) 一种分布式惯性导航系统及其姿态传递对准方法
CN108627154B (zh) 极地区域操作姿态与航向参考系统
US20120209455A1 (en) Autopilot with Adaptive Rate/Acceleration Based Damping
CN111351481A (zh) 一种基于发射惯性坐标系的传递对准方法
CN111551175A (zh) 一种航姿参考系统的互补滤波姿态解算方法
CN109489661B (zh) 一种卫星初始入轨时陀螺组合常值漂移估计方法
CN106840194A (zh) 一种大方位失准角线性对准方法
US8346466B2 (en) Systems and methods for determining heading
CN108627152A (zh) 一种微型无人机基于多传感器数据融合的导航方法
Bryne et al. Attitude and heave estimation for ships using mems-based inertial measurements
Avrutov et al. Gyrocompassing mode of the attitude and heading reference system
Lupton et al. Efficient integration of inertial observations into visual SLAM without initialization
CN108955671A (zh) 一种基于磁偏角、磁倾角的卡尔曼滤波导航方法
Zhang et al. A multi-position calibration algorithm for inertial measurement units
CN107764268B (zh) 一种机载分布式pos传递对准的方法和装置
CN105486305B (zh) 一种估计加速度计漂移的近程相对导航滤波方法
CN104330092B (zh) 一种基于双模型切换的二次传递对准方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20190528

Termination date: 20200413