CN115031728A - 基于ins与gps紧组合的无人机组合导航方法 - Google Patents
基于ins与gps紧组合的无人机组合导航方法 Download PDFInfo
- Publication number
- CN115031728A CN115031728A CN202210483843.6A CN202210483843A CN115031728A CN 115031728 A CN115031728 A CN 115031728A CN 202210483843 A CN202210483843 A CN 202210483843A CN 115031728 A CN115031728 A CN 115031728A
- Authority
- CN
- China
- Prior art keywords
- error
- inertial navigation
- navigation system
- airborne
- unmanned aerial
- 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.)
- Withdrawn
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 238000005259 measurement Methods 0.000 claims abstract description 39
- 238000012216 screening Methods 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 19
- 238000004364 calculation method Methods 0.000 claims description 8
- 150000001875 compounds Chemical class 0.000 claims description 8
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 2
- 235000000391 Lepidium draba Nutrition 0.000 claims 1
- 238000004088 simulation Methods 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 238000012271 agricultural production Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 125000004432 carbon atom Chemical group C* 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
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
- 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
-
- 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
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
本发明公开了基于INS与GPS紧组合的无人机组合导航方法,包括筛选状态量并构建机载惯性导航***误差模型、GPS接收机时钟误差模型和***状态模型;构建***的伪距差量测模型和伪距率量测模型;通过卡尔曼滤波器对机载惯性导航***误差以及GPS接收机时钟误差信息进行最优估计,并通过负反馈的方式对***进行校正与状态更新。在GPS卫星数量少于4颗时,本发明所提出的基于INS与GPS紧组合的无人机组合导航方法相较于INS与GPS松组合能够较好的提高无人机的定位精度,有一定的工程实际意义。
Description
技术领域
本发明属于无人机导航技术领域,具体涉及基于INS与GPS紧组合的无人机组合导航方法。
背景技术
现阶段无人机在军民用领域得到了广泛的应用,在军用领域无人机用于军事侦察、战略打击等任务,在民用领域用于替代人们从事农业生产然而无人机导航技术作为无人机得以广泛应用的关键技术之一得到研究学者的广泛关注。
现阶段无人机导航大多采用捷联惯导与GPS松组合的方式即位置、速度组合的方式,此种方式组合原理较简单、易于工程实现。但缺点也很明显首先INS与GPS松组合的前提是GPS接收机是处于定位模式下按照GPS定位原理可见星数量至少为4颗时才能用于定位解算,然而在工程实践中受天气、环境遮挡以及人为因素的干扰的影响,可见星的数量不一定能够满足GPS接收机定位需求。GPS接收机不能长时间定位,INS与GPS松组合的模式将长时间处于纯惯性导航的方式下,导航误差长时间累积发散严重影响导航***的定位精度。其次捷联惯导与GPS松组合的方式***观测量为GPS接收机内部一般通过卡尔曼滤波器解算得到无人机载体的速度、位置信息,此种方式下易受到误差串联影响***的估计精度,从而影响组合导航***的定位精度。
发明内容
本发明所要解决的技术问题是针对上述现有技术的不足,提供基于INS与GPS紧组合的无人机组合导航方法,基于INS与GPS紧组合实现无人机导航,以解决上述INS与GPS松组合存在的问题。
为实现上述技术目的,本发明采取的技术方案为:
基于INS与GPS紧组合的无人机组合导航方法,包括:
步骤1、筛选机载惯性导航***状态量并构建机载惯性导航***误差模型、GPS接收机时钟误差模型和***状态模型;
步骤2、以GPS接收机输出的伪距、伪距率为紧组合导航***的基准信息,无人机载体与卫星之间的伪距、伪距率作为紧组合导航***的量测信息,二者做差构成INS与GPS紧组合导航***的观测信息,从而构建***的伪距差量测模型和伪距率量测模型;
步骤3:基于步骤1和步骤2的模型,通过卡尔曼滤波器对机载惯性导航***误差以及GPS接收机时钟误差信息进行最优估计,并通过负反馈的方式对***进行校正与状态更新。
为优化上述技术方案,采取的具体措施还包括:
上述的步骤1所述机载惯性导航***误差包括:
(1)捷联机载惯性导航***平台误差角:
式中,δ表示误差;为机载惯导***平台误差角微分形式。δvN为机载惯性导航***北向速度误差,RM为地球子午圈曲率半径;wie为地球自转角速度;h为无人机高度信息;L为无人机地理坐标系下纬度信息;RN为地球卯酉圈曲率半径;εN为机载北向陀螺噪声;εU为机载陀螺天向噪声;vE为机载惯性导航***东向速度,为机载惯性导航***北向平台误差角;为机载惯性导航***天向平台误差角,δvE为机载惯性导航***东向速度误差;δL为机载惯性导航***纬度误差,为机载惯性导航***东向平台误差角;vN为机载惯性导航***北向速度,εE为东向陀螺噪声,E,N,U代表地理坐标系下东、北、天三个方向;
(2)机载惯性导航***速度误差,其模型为:
上式中,分别为机载惯导***东向、北向、天向速度误差的微分形式。vN为机载惯性导航***北向速度;fN为无人机机载加速度计北向比力信息;fU为无人机机载加速度计天向比力信息;fE为无人机机载加速度计东向比力信息;R为地球半径;g为地球重力加速度;vU为机载惯性导航***天向速度;δvU为机载惯性导航***天向速度误差;▽E为东向加速度计一阶马尔可夫过程白噪声;▽N为北向加速度计一阶马尔可夫过程白噪声;▽U为天向加速度计一阶马尔可夫过程白噪声;δh为机载惯性导航***高度误差;
(3)机载惯性导航***位置误差,其模型为:
式中,为无人机机载惯导***纬度、经度、高度误差的微分形式;δvN为机载惯性导航***北向速度误差;δvE为机载惯性导航***东向速度误差;δvU为机载惯性导航***天向速度误差;δL为机载惯性导航***纬度误差;
(4)惯性器件误差,包括陀螺误差和加速度计误差,其模型为:
陀螺误差:
ε=εb+εr+wg (4)
其中,εb为陀螺随机常数;εr为陀螺一阶马尔科夫过程;wg为陀螺白噪声;
陀螺随机常数和一阶马尔科夫过程可表示为:
式中,Tg为陀螺一阶马尔可夫相关时间,wr陀螺一阶马尔可夫过程驱动白噪声;
加速度计误差为:
Ta为加速度计一阶马尔科夫过程相关时间,wa为加速度计驱动白噪声。
上述的步骤1中通过将GPS接收机时钟偏差转换成相应的距离以及等效时钟频率误差相应的距离率作为***状态量进行最优估计,进而对GPS接收机时钟误差进行实时估计与在线补偿,具体的:
GPS接收机等效时钟误差相应的距离和等效时钟频率误差相应的距离率作为***状态量进行估计,其微分模型可表示为:
式中为GPS接收机时钟误差等效的相应距离和等效时钟频率误差相应的距离率的微分形式。βtru为GPS接收机等效时钟频率误差系数。(wru,wtru)为GPS接收机时钟误差等效的相应距离以及相应距离率高斯白噪声。
上述的步骤1构建如下***连续时间状态递推模型:
在***状态递推模型中,状态量主要包括:
δvE,δvN,δvU为地理坐标系下,无人机机载惯导***东向速度误差、北向速度误差、天向速度误差;
δL,δλ,δh为地理坐标系下,无人机机载惯导***纬度误差、经度误差、高度误差;
εbx,εby,εbz为机体坐标系下陀螺三轴随机常数;
εrx,εry,εrz为机体坐标系下三轴陀螺一阶马尔科夫过程随机噪声;
▽x,▽y,▽z为机体坐标系下三轴加速度计一阶马尔科夫过程;
δtu,δtru为GPS接收机等效时钟误差相应的距离和等效时钟频率误差相应的距离率;
其中,FI(t)为机载惯性导航***误差矩阵;
FG(t)为GPS接收机时钟误差矩阵;
G(t)为***噪声输入矩阵。
上述的步骤2构建***的伪距差量测模型,具体为:
GPS接收机输出的对应某颗卫星i的测量伪距为:
ρGi=ri-δtu-wρi (10)
式中,ri为卫星i到无人机GPS接收机的距离真值,δtu为等效时钟误差相应的距离,wρi为白噪声的伪距测量误差;
机载惯性导航***对应于某颗卫星i的计算伪距可表示为:
式中,(xsi,ysi,zsi)为GPS第i颗卫星t时刻在ECEF上的位置;ρIi为无人机机载惯性与卫星i之间的计算伪距;(xI,yI,zI)为无人机机载惯性解算得到的位置信息。
将式(11)在载***置真值(x,y,z)处泰勒级数展开并取前两项可得:
同理可得:
将式(12)与式(10)相减可得相对某颗卫星i=1,2,3,4的伪距测量差矢量模型为:
由于***状态量为地理系下惯导纬度误差、经度误差、高度误差,将ECEF坐标系下δx,δy,δz转换用δL,δλ,δh表示对应的转换关系为:
将式(15)代入式(14)可得***伪距量测模型为:
Zρ(t)=δρ=Hρ(t)X(t)+Wρ(t)(16)
式中Hρ=[04×6 Hρ1 04×9 Hρ2]4×20
上述的步骤2构建***的伪距率量测模型,具体为:
利用机载GPS接收机的伪距率构建伪距率量测模型;
GPS接收机输出对应的某颗卫星i的测量伪距率为:
对式(11)求导得:
则有:
机载惯性导航***输出的位置和速度对应于某颗卫星i的计算伪距率为:
将式(23)与式(19)做差可得相对某颗卫星i,i=1,2,3,4的伪距率量测模型为:
由ECEF坐标系与地理坐标系的转换关系可得:
将式(25)代入式(24)可得伪距率量测模型为:
上述的步骤3中,卡尔曼滤波器对机载惯性导航***误差以及GPS接收机时钟误差信息进行最优估计,并通过负反馈的方式对***进行校正与状态更新,具体的:
状态一步预测模型为:
一步预测均方误差模型为:
状态更新模型为:
滤波增益模型为:
估计均方误差模型为:
本发明具有以下有益效果:
本发明在对机载惯性导航***精确建模的基础上,采用GPS接收机输出的相对GPS卫星的伪距、伪距率作为***观测量,通过卡尔曼滤波能够较好的估计机载惯性导航***的误差,抑制机载惯性导航***长时间误差累积。
1、本发明采用的INS与GPS紧组合导航技术是GPS接收机针对每颗卫星的伪距、伪距率为观测量的组合导航技术,从算法原理上仅有一颗星即可进行滤波解算,从***定位精度层面上当可见星数量少于4颗仍然可以利用仅有的2、3颗星进行有效的组合避免了INS与GPS松组合时GPS接收机不能定位解算而转为纯惯性解算状态,能够较好的抑制机载惯性导航***误差累积,提高***的定位精度;
2、本发明INS与GPS紧组合导航的观测量为GPS接收机输出的相对某颗卫星的伪距、伪距率原始信息,并不需要GPS接收机进行定位解算,因此与INS与GPS松组合相比,避免了滤波器级联的情况,从而避免了观测量与观测噪声的时间相关问题,提高了组合导航***的定位精度与误差估计精度。在工程实践上由于GPS信号容易受遮挡或干扰等因素的影响,INS与GPS紧组合的无人机导航方式在工程上具有较高的工程实践意义。
3、通过搭建实验仿真平台验证本发明的可行性,仿真结果表明在GPS卫星数量少于4颗时,本发明所提出的基于INS与GPS紧组合的无人机组合导航方法相较于INS与GPS松组合能够较好的提高无人机的定位精度,有一定的工程实际意义。
附图说明
图1为本发明方法流程图;
图2为无人机航迹真值与INS与GPS紧组合航迹;
图3INS与GPS紧组合位置误差曲线;
图4INS与GPS紧组合速度误差曲线。
具体实施方式
以下结合附图对本发明的实施例作进一步详细描述。
本发明基于INS与GPS紧组合的无人机组合导航方法,INS与GPS伪距、伪距率紧组合的基本原理是以GPS接收机输出的伪距、伪距率为紧组合导航***的基准信息,通过计算无人机载体与卫星之间的伪距、伪距率作为紧组合导航***的量测信息,二者做差构成INS与GPS紧组合导航***的观测信息,在对机载惯性导航***精确建模的基础上,通过卡尔曼滤波器对机载惯性导航***误差以及GPS接收机时钟误差信息进行最优估计,最后通过负反馈的方式对***进行校正。如图1所示,具体包括以下步骤:
步骤1、筛选机载惯性导航***状态量并构建机载惯性导航***误差模型、GPS接收机时钟误差模型和***状态模型;
1、所述机载惯性导航***误差包括:
(1)捷联机载惯性导航***平台误差角:
式中,δ表示误差;为机载惯导***平台误差角微分形式。δvN为机载惯性导航***北向速度误差,RM为地球子午圈曲率半径;wie为地球自转角速度;h为无人机高度信息;L为无人机地理坐标系下纬度信息;RN为地球卯酉圈曲率半径;εN为机载北向陀螺噪声;εU为机载陀螺天向噪声;vE为机载惯性导航***东向速度,为机载惯性导航***北向平台误差角;为机载惯性导航***天向平台误差角,δvE为机载惯性导航***东向速度误差;δL为机载惯性导航***纬度误差,为机载惯性导航***东向平台误差角;vN为机载惯性导航***北向速度,εE为东向陀螺噪声,E,N,U代表地理坐标系下东、北、天三个方向;
(2)机载惯性导航***速度误差,其模型为:
式中,分别为机载惯导***东向、北向、天向速度误差的微分形式。vN为机载惯性导航***北向速度;fN为无人机机载加速度计北向比力信息;fU为无人机机载加速度计天向比力信息;fE为无人机机载加速度计东向比力信息;R为地球半径;g为地球重力加速度;vU为机载惯性导航***天向速度;δvU为机载惯性导航***天向速度误差;▽E为东向加速度计一阶马尔可夫过程白噪声;▽N为北向加速度计一阶马尔可夫过程白噪声;▽U为天向加速度计一阶马尔可夫过程白噪声;δh为机载惯性导航***高度误差;
(3)机载惯性导航***位置误差,其模型为:
式中,为无人机机载惯导***纬度、经度、高度误差的微分形式;δvN为机载惯性导航***北向速度误差;δvE为机载惯性导航***东向速度误差;δvU为机载惯性导航***天向速度误差;δL为机载惯性导航***纬度误差;
(4)惯性器件误差,包括陀螺误差和加速度计误差,其模型为:
惯性器件的误差一般指加速度计和陀螺零偏,考虑三轴加速度计和陀螺误差模型相同的。则陀螺的误差模型可由以下模型进行表示:
陀螺误差:
ε=εb+εr+wg (4)
其中,εb为陀螺随机常数;εr为陀螺一阶马尔科夫过程;wg为陀螺白噪声;
陀螺随机常数和一阶马尔科夫过程可表示为:
式中,Tg为陀螺一阶马尔可夫相关时间,wr陀螺一阶马尔可夫过程驱动白噪声;
加速度计误差一般考虑加速度计一阶马尔可夫过程,数学表达式可表示为:
Ta为加速度计一阶马尔科夫过程相关时间,wa为加速度计驱动白噪声。
2、通过将GPS接收机时钟偏差转换成相应的距离以及等效时钟频率误差相应的距离率作为***状态量进行最优估计,进而对GPS接收机时钟误差进行实时估计与在线补偿,具体的:
GPS接收机等效时钟误差相应的距离和等效时钟频率误差相应的距离率作为***状态量进行估计,其微分模型可表示为:
式中为GPS接收机时钟误差等效的相应距离和等效时钟频率误差相应的距离率的微分形式。βtru为GPS接收机等效时钟频率误差系数。(wru,wtru)为GPS接收机时钟误差等效的相应距离以及相应距离率高斯白噪声。
公式(7)作为GPS接收机时钟误差等效的相应距离以及距离率误差微分方程。即GPS接收机与卫星之间的时钟偏差,基于无线电信号达到时间(TOA)原理,将导致GPS接收机与卫星的伪距误差。本发明通过将卫星接收机时钟偏差转换成相应的距离以及距离率作为***状态量进行最优估计,进而对GPS接收机时钟误差进行实时估计与在线补偿,降低GPS接收机时钟误差对定位精度的影响,提高***的绝对定位精度。
3、构建如下***连续时间状态递推模型:
在***状态递推模型中,状态量主要包括为:
δvE,δvN,δvU为无人机机载惯导地理坐标系下,东向速度误差、北向速度误差、天向速度误差;
δL,δλ,δh为无人机机载惯导地理坐标系下,纬度误差、经度误差、高度误差;
εbx,εby,εbz为无人机机体坐标系下,陀螺三轴随机常数;
εrx,εry,εrz为无人机机体坐标系下,陀螺三轴一阶马尔科夫过程随机噪声;
▽x,▽y,▽z为无人机机体坐标系下,三轴加速度计一阶马尔科夫过程;
δtu,δtru为GPS接收机等效时钟误差相应的距离和等效时钟频率误差相应的距离率。
将***误差模型按照(8)式展开可得***状态转移矩阵为:
其中,FI(t)为机载惯性导航***误差矩阵;
FG(t)为GPS接收机时钟误差矩阵;
G(t)为***噪声输入矩阵。
步骤2、以GPS接收机输出的伪距、伪距率为紧组合导航***的基准信息,无人机载体与卫星之间的伪距、伪距率作为紧组合导航***的量测信息,二者做差构成INS与GPS紧组合导航***的观测信息,从而构建***的伪距差量测模型和伪距率量测模型;
1、构建***的伪距差量测模型,具体为:
由于采用了GPS接收机的伪距信号,因此需要建立***伪距量测模型。
GPS接收机输出的对应某颗卫星i的测量伪距为:
ρGi=ri-δtu-wρi (10)
式中,ri为卫星i到无人机GPS接收机的距离真值,δtu为等效时钟误差相应的距离,wρi为白噪声的伪距测量误差;
机载惯性导航***对应于某颗卫星i的计算伪距可表示为:
式中,(xsi,ysi,zsi)为GPS第i颗卫星t时刻在地球固定坐标系(Earth CenteredEarth Fixed,ECEF)上的位置;ρIi为无人机机载惯性与卫星i之间的计算伪距;(xI,yI,zI)为无人机机载惯性解算得到的位置信息。
将式(11)在载***置真值(x,y,z)处泰勒级数展开并取前两项可得:
同理可得:
将式(12)与式(10)相减可得相对某颗卫星i=1,2,3,4的伪距测量差矢量模型为:
由于***状态量为地理系下惯导纬度误差、经度误差、高度误差,将ECEF坐标系下δx,δy,δz转换用δL,δλ,δh表示对应的转换关系为:
将式(15)代入式(14)可得***伪距量测模型为:
Zρ(t)=δρ=Hρ(t)X(t)+Wρ(t) (16)
式中Hρ=[04×6 Hρ1 04×9 Hρ2]4×20
2、构建***的伪距率量测模型,具体为:
利用机载GPS接收机的伪距率构建伪距率量测模型;
GPS接收机输出对应的某颗卫星i的测量伪距率为:
对式(11)求导得:
则有:
机载惯性导航***输出的位置和速度对应于某颗卫星i的计算伪距率为:
将式(23)与式(19)做差可得相对某颗卫星i,i=1,2,3,4的伪距率量测模型为:
由ECEF坐标系与地理坐标系的转换关系可得:
将式(25)代入式(24)可得伪距率量测模型为:
步骤3:在上述对机载惯性导航***精确建模的基础上,即基于步骤1和步骤2的模型,通过卡尔曼滤波器对机载惯性导航***误差以及GPS接收机时钟误差信息进行最优估计,并通过负反馈的方式对***进行校正与状态更新,具体的:
状态一步预测模型为:
一步预测均方误差模型为:
状态更新模型为:
滤波增益模型为:
估计均方误差模型为:
为了验证所提出算法的可行性,本发明设计了对应的仿真实验。
在仿真中设计了无人机载体不同飞行轨迹包括:加速、减速、转弯、平飞等。
模拟机载惯性导航***陀螺仪及加速度计输出以及GPS接收机输出的伪距、伪距率。
仿真中模拟机载惯性导航***输出频率为50Hz;
GPS接收机伪距、伪距率输出频率为1Hz。
仿真中采用的机载惯性导航***的精度为10度/小时,加速度计精度为0.001g,GPS接收机输出伪距、伪距率误差为1m、1m、1m、1m、0.2m/s、0.2m/s、0.2m/s、0.2m/s,仿真时长3600秒。
无人机载体飞行航迹真值与INS与GPS紧组合得到的无人机航迹图如图2所示。INS与GPS紧组合得到的无人机载体在东、北、天方向的位置、速度信息与无人机位置、速度真值误差如图3和4所示。
由图3和4可知,INS与GPS紧组合得到的无人机航迹能够较好的吻合无人机真实航迹,通过本发明提出的算法能够较好的抑制惯导长时间累积的误差。INS与GPS紧组合导航***稳态时东向、北向、天向的位置误差保持在1m以内,东向、北向、天向的速度误差保持在0.2m/s以内,从而进一步验证本发明的可行性。
以上仅是本发明的优选实施方式,本发明的保护范围并不仅局限于上述实施例,凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理前提下的若干改进和润饰,应视为本发明的保护范围。
Claims (7)
1.基于INS与GPS紧组合的无人机组合导航方法,其特征在于,包括:
步骤1、筛选机载惯性导航***状态量并构建机载惯性导航***误差模型、GPS接收机时钟误差模型和***状态模型;
步骤2、以GPS接收机输出的伪距、伪距率为紧组合导航***的基准信息,无人机载体与卫星之间的伪距、伪距率作为紧组合导航***的量测信息,二者做差构成INS与GPS紧组合导航***的观测信息,从而构建***的伪距差量测模型和伪距率量测模型;
步骤3:基于步骤1和步骤2的模型,通过卡尔曼滤波器对机载惯性导航***误差以及GPS接收机时钟误差信息进行最优估计,并通过负反馈的方式对***进行校正与状态更新。
2.根据权利要求1所述的基于INS与GPS紧组合的无人机组合导航方法,其特征在于,步骤1所述机载惯性导航***误差包括:
(1)捷联机载惯性导航***平台误差角:
上式中,δ表示误差;为机载惯导***平台误差角微分形式。δvN为机载惯性导航***北向速度误差,RM为地球子午圈曲率半径;wie为地球自转角速度;h为无人机高度信息;L为无人机地理坐标系下纬度信息;RN为地球卯酉圈曲率半径;εN为机载北向陀螺噪声;εU为机载陀螺天向噪声;vE为机载惯性导航***东向速度,为机载惯性导航***北向平台误差角;为机载惯性导航***天向平台误差角,δvE为机载惯性导航***东向速度误差;δL为机载惯性导航***纬度误差,为机载惯性导航***东向平台误差角;vN为机载惯性导航***北向速度,εE为东向陀螺噪声,E,N,U代表地理坐标系下东、北、天三个方向;
(2)机载惯性导航***速度误差,其模型为:
上式中,分别为机载惯导***东向、北向、天向速度误差的微分形式。vN为机载惯性导航***北向速度;fN为无人机机载加速度计北向比力信息;fU为无人机机载加速度计天向比力信息;fE为无人机机载加速度计东向比力信息;R为地球半径;g为地球重力加速度;vU为机载惯性导航***天向速度;δvU为机载惯性导航***天向速度误差;为东向加速度计一阶马尔可夫过程白噪声;为北向加速度计一阶马尔可夫过程白噪声;为天向加速度计一阶马尔可夫过程白噪声;δh为机载惯性导航***高度误差;
(3)机载惯性导航***位置误差,其模型为:
式中,为无人机机载惯导***纬度、经度、高度误差的微分形式;δvN为机载惯性导航***北向速度误差;δvE为机载惯性导航***东向速度误差;δvU为机载惯性导航***天向速度误差;δL为机载惯性导航***纬度误差;
(4)惯性器件误差,包括陀螺误差和加速度计误差,其模型为:
陀螺误差:
ε=εb+εr+wg (4)
其中,εb为陀螺随机常数;εr为陀螺一阶马尔科夫过程;wg为陀螺白噪声;
陀螺随机常数和一阶马尔科夫过程可表示为:
式中,Tg为陀螺一阶马尔可夫相关时间,wr陀螺一阶马尔可夫过程驱动白噪声;
加速度计误差为:
Ta为加速度计一阶马尔科夫过程相关时间,wa为加速度计驱动白噪声。
4.根据权利要求1所述的基于INS与GPS紧组合的无人机组合导航方法,其特征在于,所述步骤1构建如下***连续时间状态递推模型:
在***状态递推模型中,状态量包括
δvE,δvN,δvU为无人机机载惯导***地理坐标系下东向速度误差、北向速度误差、天向速度误差;
δL,δλ,δh为无人机机载惯导***地理坐标系下纬度误差、经度误差、高度误差;
εbx,εby,εbz为机体坐标系下陀螺三轴随机常数;
εrx,εry,εrz为机体坐标系下陀螺三轴一阶马尔科夫过程随机噪声;
δtu,δtru为GPS接收机等效时钟误差相应的距离和等效时钟频率误差相应的距离率;
其中,FI(t)为机载惯性导航***误差矩阵;
FG(t)为GPS接收机时钟误差矩阵;
G(t)为***噪声输入矩阵。
5.根据权利要求1所述的基于INS与GPS紧组合的无人机组合导航方法,其特征在于,所述步骤2构建***的伪距差量测模型,具体为:
GPS接收机输出的对应某颗卫星i的测量伪距为:
ρGi=ri-δtu-wρi (10)
式中,ri为卫星i到无人机GPS接收机的距离真值,δtu为等效时钟误差相应的距离,wρi为白噪声的伪距测量误差;
机载惯性导航***对应于某颗卫星i的计算伪距可表示为:
式中,(xsi,ysi,zsi)为GPS第i颗卫星t时刻在ECEF上的位置;ρIi为无人机机载惯性与卫星i之间的计算伪距;(xI,yI,zI)为无人机机载惯性解算得到的位置信息;
将式(11)在无人机位置真值(x,y,z)处泰勒级数展开并取前两项可得:
同理可得:
将式(12)与式(10)相减可得相对某颗卫星i=1,2,3,4的伪距测量差矢量模型为:
由于***状态量为地理系下惯导纬度误差、经度误差、高度误差,将ECEF坐标系下δx,δy,δz转换用δL,δλ,δh表示对应的转换关系为:
将式(15)代入式(14)可得***伪距量测模型为:
Zρ(t)=δρ=Hρ(t)X(t)+Wρ(t) (16)
式中Hρ=[04×6 Hρ1 04×9 Hρ2]4×20
6.根据权利要求1所述的基于INS与GPS紧组合的无人机组合导航方法,其特征在于,所述步骤2构建***的伪距率量测模型,具体为:
利用机载GPS接收机的伪距率构建伪距率量测模型;
GPS接收机输出对应的某颗卫星i的测量伪距率为:
对式(11)求导得:
则有:
机载惯性导航***输出的位置和速度对应于某颗卫星i的计算伪距率为:
将式(23)与式(19)做差可得相对某颗卫星i,i=1,2,3,4的伪距率量测模型为:
由ECEF坐标系与地理坐标系的转换关系可得:
将式(25)代入式(24)可得伪距率量测模型为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210483843.6A CN115031728A (zh) | 2022-05-06 | 2022-05-06 | 基于ins与gps紧组合的无人机组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210483843.6A CN115031728A (zh) | 2022-05-06 | 2022-05-06 | 基于ins与gps紧组合的无人机组合导航方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115031728A true CN115031728A (zh) | 2022-09-09 |
Family
ID=83119947
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210483843.6A Withdrawn CN115031728A (zh) | 2022-05-06 | 2022-05-06 | 基于ins与gps紧组合的无人机组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115031728A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117214933A (zh) * | 2023-11-07 | 2023-12-12 | 中国船舶集团有限公司第七〇七研究所 | 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法 |
-
2022
- 2022-05-06 CN CN202210483843.6A patent/CN115031728A/zh not_active Withdrawn
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117214933A (zh) * | 2023-11-07 | 2023-12-12 | 中国船舶集团有限公司第七〇七研究所 | 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法 |
CN117214933B (zh) * | 2023-11-07 | 2024-02-06 | 中国船舶集团有限公司第七〇七研究所 | 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110487301B (zh) | 一种雷达辅助机载捷联惯性导航***初始对准方法 | |
CN108226980B (zh) | 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法 | |
CN111156994B (zh) | 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 | |
CN103743414B (zh) | 一种里程计辅助车载捷联惯导***行进间初始对准方法 | |
CN109000640B (zh) | 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法 | |
CN102809377B (zh) | 飞行器惯性/气动模型组合导航方法 | |
WO2020114301A1 (zh) | 基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法 | |
CN111947681B (zh) | 一种gnss与惯导组合导航位置输出的滤波校正方法 | |
CN111006675B (zh) | 基于高精度重力模型的车载激光惯导***自标定方法 | |
CN111399023B (zh) | 基于李群非线性状态误差的惯性基组合导航滤波方法 | |
CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
CN110849360B (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
CN103557864A (zh) | Mems捷联惯导自适应sckf滤波的初始对准方法 | |
CN111121766A (zh) | 一种基于星光矢量的天文与惯性组合导航方法 | |
US20100256906A1 (en) | Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering | |
CN113503892A (zh) | 一种基于里程计和回溯导航的惯导***动基座初始对准方法 | |
CN111964675A (zh) | 一种黑障区的飞行器智能导航方法 | |
CN110221331B (zh) | 基于状态变换的惯性/卫星组合导航动态滤波方法 | |
CN115031728A (zh) | 基于ins与gps紧组合的无人机组合导航方法 | |
Wang et al. | Performance analysis of GNSS/INS loosely coupled integration systems under GNSS signal blocking environment | |
CN111290008A (zh) | 一种动态自适应扩展卡尔曼滤波容错算法 | |
CN111076717A (zh) | 基于全球地磁异常场的地磁辅助惯性导航仿真***及方法 | |
CN114111792B (zh) | 一种车载gnss/ins/里程计组合导航方法 | |
Zhou et al. | Integrated INS/GPS system for an autonomous mobile vehicle | |
CN106323226B (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 | ||
WW01 | Invention patent application withdrawn after publication |
Application publication date: 20220909 |
|
WW01 | Invention patent application withdrawn after publication |