CN112729274B - 一种组合导航系统及方法 - Google Patents
一种组合导航系统及方法 Download PDFInfo
- Publication number
- CN112729274B CN112729274B CN202011436343.4A CN202011436343A CN112729274B CN 112729274 B CN112729274 B CN 112729274B CN 202011436343 A CN202011436343 A CN 202011436343A CN 112729274 B CN112729274 B CN 112729274B
- Authority
- CN
- China
- Prior art keywords
- speed
- navigation system
- inertial navigation
- speed correction
- axis
- 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
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
-
- 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)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明提出一种组合导航系统及方法,包括状态方程与量测方程确定模块、滤波器、速度修正判断模块和速度修正模块。本发明利用飞行过程中红外成像设备或无线电设备提供的合速度信息作为观测量与惯性导航系统进行组合,实时通过对载体的航迹进行判断实现对惯导速度的准确修正,提高系统的导航整体性能。
Description
技术领域
本发明涉及一种组合导航系统及方法,属于惯性导航技术领域。
背景技术
惯性导航系统精度随时间发散,实际应用中常采用卫星导航接收机输出的位置、速度进行组合导航,但卫星导航接收机易收到干扰,无法满足复杂电磁环境下的组合导航精度。无人飞行器利用红外成像设备或无线电设备可以不依赖卫星导航接收机给出飞行器飞行的合速度,但是无法给出无人飞行器载体系下准确的纵向和侧向速度,因此无法直接利用合速度对惯性导航系统的速度进行修正,难以充分利用外部导航辅助信息。
发明内容
本发明的目的在于克服现有技术不足,提供一种基于合速度、精度高的组合导航系统及方法。
本发明的技术解决方案:一种组合导航系统,包括状态方程与量测方程确定模块、滤波器、速度修正判断模块和速度修正模块;
状态方程与量测方程确定模块根据红外成像设备或无线电设备提供的合速度信息作为观测量,建立滤波器状态方程与量测方程;
滤波器根据状态方程与量测方程确定模块建立的滤波器状态方程与量测方程,进行滤波处理,得到载体速度误差的估计值;
速度修正判断模块对滤波器每次滤波得到的载体速度误差的估计值进行判断,判断是否满足公式max({θ0,θ1,…θi,…θm})-min({θ0,θ1,…θi,…θm})≥T,若满足,则触发速度修正模块,其中θi为第i秒惯导解算得到的载体的方位角,T为速度修正判决门限;
速度修正模块利用满足速度修正判断模块判断公式的载体速度误差的估计值,对惯性导航的速度误差进行修正。
一种组合导航方法,通过以下步骤实现:
第一步,以红外成像设备或无线电设备提供的合速度信息作为观测量,建立滤波器状态方程与量测方程,
滤波器状态向量如公式(1)所示,
X=[φN,φE,δVN,δVE]T (1)
其中φN、φE为惯导解算得到的北向、东向的平台失准角,δVN,δVE为惯导解算得到的导航系下X轴、Z轴的速度误差;
量测向量如公式(3)所示,
Z=[VA,m-VA]T (3)
其中VA为惯导解算得到的导航系下X轴、Z轴的载体合速度,VA,m为红外成像设备或无线电设备提供的导航系下X轴、Z轴的合速度;
第二步,利用第一步建立的滤波器状态方程与量测方程,进行Kalman滤波处理,得到载体速度误差的估计值;
第三步,对第二步Kalman滤波处理后得到的速度误差的估计值进行判决,若满足判决公式(5),则进行第四步,若不满足则返回第一步,
max({θ0,θ1,…θi,…θm})-min({θ0,θ1,…θi,…θm})≥T (5)
其中,θi为第i秒惯导解算得到的载体的方位角,T为速度修正判决门限;
第四步,对惯性导航的速度误差进行修正。
本发明与现有技术相比的有益效果:
(1)本发明利用飞行过程中红外成像设备或无线电设备提供的合速度信息作为观测量与惯性导航系统进行组合,实时通过对载体的航迹进行判断实现对惯导速度的准确修正,提高系统的导航整体性能;
(2)本发明以红外成像设备或无线电设备提供的合速度信息作为观测量,对纯惯性导航的速度误差与姿态误差进行卡尔曼滤波估计,并实时记录载体的航迹变化,确定了满足速度修正的条件,在满足修正条件下,进行速度误差修正,提高速度精度。
附图说明
图1为本发明理论框图;
图2为本发明流程图。
具体实施方式
下面结合具体实例及附图对本发明进行详细说明。
为了充分利用红外成像设备或无线电设备提供的合速度信息对惯性导航系统的位置、速度进行修正。
本发明如图1所示,提供一种基于合速度辅助的组合导航系统,包括状态方程与量测方程确定模块、滤波器、速度修正判断模块和速度修正模块。
状态方程与量测方程确定模块根据红外成像设备或无线电设备提供的合速度信息作为观测量,建立滤波器状态方程与量测方程。
滤波器根据状态方程与量测方程确定模块建立的滤波器状态方程与量测方程,进行Kalman滤波处理,得到载体速度误差的估计值。
速度修正判断模块对滤波器每次滤波得到的载体速度误差的估计值进行判断,判断是否满足公式max({θ0,θ1,…θi,…θm})-min({θ0,θ1,…θi,…θm})≥T,若满足,则触发速度修正模块,若不满足则触发状态方程与量测方程确定模块。
速度修正模块利用满足速度修正判断模块判断公式的载体速度误差的估计值,对惯性导航的速度误差进行修正。
本发明提出了一种基于合速度辅助的组合导航方法,通过以下步骤实现:
第一步,以红外成像设备或无线电设备提供的合速度信息作为观测量,建立滤波器状态方程与量测方程。
滤波器状态向量如公式(1)所示,
X=[φN,φE,δVN,δVE]T (1)
其中φN、φE为惯导解算得到的北向、东向的平台失准角,δVN,δVE为惯导解算得到的导航系下X轴、Z轴的速度误差。
状态方程转移矩阵Φ如公式(2)所示,
Φ=I+F(t)·Δt (2)
其中VN、VU、VE为惯导解算得到的导航系下X轴、Y轴、Z轴的速度,RM为子午圈半径、RN为卯酉圈半径,wie为地球自转角速度,L惯导解算得到的载体纬度,H为惯导解算得到的载体的高度,fU为惯导解算得到的导航系下Y轴的比力信息。
量测向量如公式(3)所示,
Z=[VA,m-VA]T (3)
其中VA为惯导解算得到的导航系下X轴、Z轴的载体合速度,VA,m为红外成像设备或无线电设备提供的导航系下X轴、Z轴的合速度。
量测方程转移矩阵H1×4如公式(4)所示,
其中VINS_N、VINS_E为惯导解算得到的导航系下X轴、Z轴的速度。
第二步,利用第一步建立的滤波器状态方程与量测方程,进行Kalman滤波处理,得到载体速度误差的估计值。
本步骤Kalman滤波为本领域公知技术,根据以下公式确定。
(1)状态一步预测:
(2)状态估计:
(3)滤波增益:
(4)一步预测均方误差:
(5)估计均方误差:
第三步,对第二步Kalman滤波处理后得到的速度误差的估计值进行判决,若满足判决公式(5),则进行第四步,若不满足则返回第一步。
max({θ0,θ1,…θi,…θm})-min({θ0,θ1,…θi,…θm})≥T (5)
其中,θi为第i秒惯导解算得到的载体的方位角,T为速度修正判决门限。
速度修正判决门限T的确定准则,根据惯性导航系统陀螺精度确定,若惯性导航系统陀螺精度高,判决严格,若陀螺精度低,判决标准低一些;速度修正判决门限T值越大,判决越严格,设置不小于90°。优选,若惯性导航系统陀螺精度优于0.1°/h,则速度修正判决门限T设置不小于120°,若陀螺精度低于0.1°/h,则速度修正判决门限T设置不小于90°。
本步骤中,在每次Kalman滤波完成后,对速度误差的估计值进行判决,若满足公式(5)则认为速度误差估计准确,可以用于误差修正,若不满足公式(5),则返回第一步中,进行数据迭代后重新建立滤波器状态方程与量测方程。
第四步,对惯性导航的速度误差进行修正。
采用公式组(6)对速度误差进行修正,返回第一步,直至飞行结束。
其中,VINS_N0、VINS_E0为修正前惯性导航系统在导航系下X轴、Z轴的速度,VINS_N1、VINS_E1为修正后惯性导航系统在导航系下X轴、Z轴的速度,δVN,δVE为第三步满足判决公式(5)的速度误差的估计值。
本发明未详细说明部分为本领域技术人员公知技术。
Claims (8)
1.一种组合导航系统,其特征在于:包括状态方程与量测方程确定模块、滤波器、速度修正判断模块和速度修正模块;
所述的状态方程与量测方程确定模块根据红外成像设备或无线电设备提供的合速度信息作为观测量,建立滤波器状态方程与量测方程;
所述的滤波器根据状态方程与量测方程确定模块建立的滤波器状态方程与量测方程,进行滤波处理,得到载体速度误差的估计值;
所述的速度修正判断模块对滤波器每次滤波得到的载体速度误差的估计值进行判断,判断是否满足公式max({θ0,θ1,…θi,…θm})-min({θ0,θ1,…θi,…θm})≥T,若满足,则触发速度修正模块,其中θi为第i秒惯导解算得到的载体的方位角,T为速度修正判决门限,速度修正判决门限T的确定准则,根据惯性导航系统陀螺精度确定,惯性导航系统陀螺精度越高,速度修正判决门限T值越大,设置不小于90°;
所述的速度修正模块利用满足速度修正判断模块判断公式的载体速度误差的估计值,对惯性导航的速度误差进行修正。
2.根据权利要求1所述的一种组合导航系统,其特征在于:所述的状态方程与量测方程确定模块建立如公式(3)所示的量测向量,
Z=[VA,m-VA]T (3)
其中VA为惯导解算得到的导航系下X轴、Z轴的载体合速度,VA,m为红外成像设备或无线电设备提供的导航系下X轴、Z轴的合速度。
3.根据权利要求1所述的一种组合导航系统,其特征在于:所述的速度修正判决门限T设置,若惯性导航系统陀螺精度优于0.1°/h,则不小于120°,若陀螺精度低于0.1°/h,则不小于90°。
4.根据权利要求1所述的一种组合导航系统,其特征在于:所述的速度修正判断模块若不满足,则触发状态方程与量测方程确定模块。
5.一种组合导航方法,其特征在于,通过以下步骤实现:
第一步,以红外成像设备或无线电设备提供的合速度信息作为观测量,建立滤波器状态方程与量测方程,
滤波器状态向量如公式(1)所示,
X=[φN,φE,δVN,δVE]T (1)
其中φN、φE为惯导解算得到的北向、东向的平台失准角,δVN,δVE为惯导解算得到的导航系下X轴、Z轴的速度误差;
量测向量如公式(3)所示,
Z=[VA,m-VA]T (3)
其中VA为惯导解算得到的导航系下X轴、Z轴的载体合速度,VA,m为红外成像设备或无线电设备提供的导航系下X轴、Z轴的合速度;
第二步,利用第一步建立的滤波器状态方程与量测方程,进行Kalman滤波处理,得到载体速度误差的估计值;
第三步,对第二步Kalman滤波处理后得到的速度误差的估计值进行判决,若满足判决公式(5),则进行第四步,若不满足则返回第一步,
max({θ0,θ1,…θi,…θm})-min({θ0,θ1,…θi,…θm})≥T (5)
其中,θi为第i秒惯导解算得到的载体的方位角,T为速度修正判决门限,速度修正判决门限T的确定准则,根据惯性导航系统陀螺精度确定,惯性导航系统陀螺精度越高,速度修正判决门限T值越大,设置不小于90°;
第四步,对惯性导航的速度误差进行修正。
6.根据权利要求5所述的一种组合导航方法,其特征在于:所述第三步速度修正判决门限T设置,若惯性导航系统陀螺精度优于0.1°/h,则不小于120°,若陀螺精度低于0.1°/h,则不小于90°。
7.根据权利要求5所述的一种组合导航方法,其特征在于:所述第三步在每次Kalman滤波完成后,对速度误差的估计值进行判决,若不满足公式(5),则返回第一步中,进行数据迭代后重新建立滤波器状态方程与量测方程。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011436343.4A CN112729274B (zh) | 2020-12-11 | 2020-12-11 | 一种组合导航系统及方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011436343.4A CN112729274B (zh) | 2020-12-11 | 2020-12-11 | 一种组合导航系统及方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112729274A CN112729274A (zh) | 2021-04-30 |
CN112729274B true CN112729274B (zh) | 2023-01-10 |
Family
ID=75598802
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011436343.4A Active CN112729274B (zh) | 2020-12-11 | 2020-12-11 | 一种组合导航系统及方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112729274B (zh) |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0926328A (ja) * | 1995-07-12 | 1997-01-28 | Tokimec Inc | 位置標定装置 |
CN111189446A (zh) * | 2018-11-15 | 2020-05-22 | 北京自动化控制设备研究所 | 一种基于无线电的组合导航方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH09210720A (ja) * | 1996-02-05 | 1997-08-15 | Yokogawa Denshi Kiki Kk | 船舶用慣性航法装置 |
WO2012048287A2 (en) * | 2010-10-08 | 2012-04-12 | Loctronix Corporation | Doppler aided inertial navigation |
CN102608596B (zh) * | 2012-02-29 | 2013-06-05 | 北京航空航天大学 | 一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法 |
CN104567888A (zh) * | 2014-12-25 | 2015-04-29 | 大连楼兰科技股份有限公司 | 基于速度在线修正的惯性导航车辆姿态测量方法 |
-
2020
- 2020-12-11 CN CN202011436343.4A patent/CN112729274B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0926328A (ja) * | 1995-07-12 | 1997-01-28 | Tokimec Inc | 位置標定装置 |
CN111189446A (zh) * | 2018-11-15 | 2020-05-22 | 北京自动化控制设备研究所 | 一种基于无线电的组合导航方法 |
Also Published As
Publication number | Publication date |
---|---|
CN112729274A (zh) | 2021-04-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108731670B (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
CN112097763B (zh) | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 | |
CN110779521A (zh) | 一种多源融合的高精度定位方法与装置 | |
CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
CN110926468B (zh) | 基于传递对准的动中通天线多平台航姿确定方法 | |
CN101846734B (zh) | 农用机械导航定位方法、系统及农用机械工控机 | |
JP4199553B2 (ja) | ハイブリッド航法装置 | |
CN106979781B (zh) | 基于分布式惯性网络的高精度传递对准方法 | |
CN112346104B (zh) | 一种无人机信息融合定位方法 | |
CN113029139B (zh) | 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法 | |
CN111207745A (zh) | 一种适用于大机动无人机垂直陀螺仪的惯性测量方法 | |
CN113063429A (zh) | 一种自适应车载组合导航定位方法 | |
US8229661B2 (en) | Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering | |
CN110388942B (zh) | 一种基于角度和速度增量的车载姿态精对准系统 | |
CN112212862A (zh) | 一种改进粒子滤波的gps/ins组合导航方法 | |
CN114111767B (zh) | 基于多信息融合对线路设计线型进行优化的方法 | |
CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
CN112729274B (zh) | 一种组合导航系统及方法 | |
CN110375773B (zh) | Mems惯导系统姿态初始化方法 | |
CN112284412A (zh) | 一种避免欧拉转换奇异导致精度下降的地面静态对准方法 | |
CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
CN115950419A (zh) | 超小型无人机用组合导航方法、装置及系统 | |
CN114935345A (zh) | 一种基于模式识别的车载惯导安装角误差补偿方法 | |
CN115014321A (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 |