CN115031728A - 基于ins与gps紧组合的无人机组合导航方法 - Google Patents

基于ins与gps紧组合的无人机组合导航方法 Download PDF

Info

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
Application number
CN202210483843.6A
Other languages
English (en)
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.)
Nanjing Institute of Railway Technology
Original Assignee
Nanjing Institute of Railway Technology
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 Nanjing Institute of Railway Technology filed Critical Nanjing Institute of Railway Technology
Priority to CN202210483843.6A priority Critical patent/CN115031728A/zh
Publication of CN115031728A publication Critical patent/CN115031728A/zh
Withdrawn legal-status Critical Current

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
    • 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紧组合的无人机组合导航方法
技术领域
本发明属于无人机导航技术领域，具体涉及基于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)捷联机载惯性导航***平台误差角：
Figure BDA0003628757630000021
式中，δ表示误差；
Figure BDA0003628757630000022
为机载惯导***平台误差角微分形式。δvN为机载惯性导航***北向速度误差，RM为地球子午圈曲率半径；wie为地球自转角速度；h为无人机高度信息；L为无人机地理坐标系下纬度信息；RN为地球卯酉圈曲率半径；εN为机载北向陀螺噪声；εU为机载陀螺天向噪声；vE为机载惯性导航***东向速度，
Figure BDA0003628757630000023
为机载惯性导航***北向平台误差角；
Figure BDA0003628757630000024
为机载惯性导航***天向平台误差角，δvE为机载惯性导航***东向速度误差；δL为机载惯性导航***纬度误差，
Figure BDA0003628757630000025
为机载惯性导航***东向平台误差角；vN为机载惯性导航***北向速度，εE为东向陀螺噪声，E,N,U代表地理坐标系下东、北、天三个方向；
(2)机载惯性导航***速度误差，其模型为：
Figure BDA0003628757630000031
上式中，
Figure BDA0003628757630000032
分别为机载惯导***东向、北向、天向速度误差的微分形式。vN为机载惯性导航***北向速度；fN为无人机机载加速度计北向比力信息；fU为无人机机载加速度计天向比力信息；fE为无人机机载加速度计东向比力信息；R为地球半径；g为地球重力加速度；vU为机载惯性导航***天向速度；δvU为机载惯性导航***天向速度误差；▽E为东向加速度计一阶马尔可夫过程白噪声；▽N为北向加速度计一阶马尔可夫过程白噪声；▽U为天向加速度计一阶马尔可夫过程白噪声；δh为机载惯性导航***高度误差；
(3)机载惯性导航***位置误差，其模型为：
Figure BDA0003628757630000033
式中，
Figure BDA0003628757630000034
为无人机机载惯导***纬度、经度、高度误差的微分形式；δvN为机载惯性导航***北向速度误差；δvE为机载惯性导航***东向速度误差；δvU为机载惯性导航***天向速度误差；δL为机载惯性导航***纬度误差；
(4)惯性器件误差，包括陀螺误差和加速度计误差，其模型为：
陀螺误差：
ε＝εb+εr+wg (4)
其中，εb为陀螺随机常数；εr为陀螺一阶马尔科夫过程；wg为陀螺白噪声；
陀螺随机常数和一阶马尔科夫过程可表示为：
Figure BDA0003628757630000041
式中，Tg为陀螺一阶马尔可夫相关时间，wr陀螺一阶马尔可夫过程驱动白噪声；
加速度计误差为：
Figure BDA0003628757630000042
Ta为加速度计一阶马尔科夫过程相关时间，wa为加速度计驱动白噪声。
上述的步骤1中通过将GPS接收机时钟偏差转换成相应的距离以及等效时钟频率误差相应的距离率作为***状态量进行最优估计，进而对GPS接收机时钟误差进行实时估计与在线补偿，具体的：
GPS接收机等效时钟误差相应的距离和等效时钟频率误差相应的距离率作为***状态量进行估计，其微分模型可表示为：
Figure BDA0003628757630000043
式中
Figure BDA0003628757630000044
为GPS接收机时钟误差等效的相应距离和等效时钟频率误差相应的距离率的微分形式。βtru为GPS接收机等效时钟频率误差系数。(wru,wtru)为GPS接收机时钟误差等效的相应距离以及相应距离率高斯白噪声。
上述的步骤1构建如下***连续时间状态递推模型：
Figure BDA0003628757630000045
式中
Figure BDA0003628757630000048
为***状态量的微分形式；X(t)为***状态量，F(t)为***状态转移矩阵；G(t)为***噪声控制输入矩阵；W(t)为***噪声矩阵；
Figure BDA0003628757630000047
在***状态递推模型中，状态量主要包括：
Figure BDA0003628757630000051
为地理坐标系下，无人机机载惯导***东向、北向、天向三个方向的平台误差角；
δvE,δvN,δvU为地理坐标系下，无人机机载惯导***东向速度误差、北向速度误差、天向速度误差；
δL,δλ,δh为地理坐标系下，无人机机载惯导***纬度误差、经度误差、高度误差；
εbx,εby,εbz为机体坐标系下陀螺三轴随机常数；
εrx,εry,εrz为机体坐标系下三轴陀螺一阶马尔科夫过程随机噪声；
▽x,▽y,▽z为机体坐标系下三轴加速度计一阶马尔科夫过程；
δtu,δtru为GPS接收机等效时钟误差相应的距离和等效时钟频率误差相应的距离率；
Figure BDA0003628757630000052
其中，FI(t)为机载惯性导航***误差矩阵；
FG(t)为GPS接收机时钟误差矩阵；
G(t)为***噪声输入矩阵。
上述的步骤2构建***的伪距差量测模型，具体为：
GPS接收机输出的对应某颗卫星i的测量伪距为：
ρGi＝ri-δtu-wρi (10)
式中，ri为卫星i到无人机GPS接收机的距离真值，δtu为等效时钟误差相应的距离，wρi为白噪声的伪距测量误差；
机载惯性导航***对应于某颗卫星i的计算伪距可表示为：
Figure BDA0003628757630000053
式中，(xsi,ysi,zsi)为GPS第i颗卫星t时刻在ECEF上的位置；ρIi为无人机机载惯性与卫星i之间的计算伪距；(xI,yI,zI)为无人机机载惯性解算得到的位置信息。
将式(11)在载***置真值(x,y,z)处泰勒级数展开并取前两项可得：
Figure BDA0003628757630000061
Figure BDA0003628757630000062
同理可得：
Figure BDA0003628757630000063
将式(12)与式(10)相减可得相对某颗卫星i＝1,2,3,4的伪距测量差矢量模型为：
Figure BDA0003628757630000064
由于***状态量为地理系下惯导纬度误差、经度误差、高度误差，将ECEF坐标系下δx,δy,δz转换用δL,δλ,δh表示对应的转换关系为：
Figure BDA0003628757630000065
将式(15)代入式(14)可得***伪距量测模型为：
Zρ(t)＝δρ＝Hρ(t)X(t)+Wρ(t)(16)
式中Hρ＝[04×6 Hρ1 04×9 Hρ2]4×20
Figure BDA0003628757630000066
Figure BDA0003628757630000067
上述的步骤2构建***的伪距率量测模型，具体为：
利用机载GPS接收机的伪距率构建伪距率量测模型；
GPS接收机输出对应的某颗卫星i的测量伪距率为：
Figure BDA0003628757630000071
式中，
Figure BDA0003628757630000072
为卫星i到机载GPS接收机的伪距率真值，δtru为等效时钟频率误差相应的距离率，vρi为白噪声伪距率测量误差；
对式(11)求导得：
Figure BDA0003628757630000073
则有：
Figure BDA0003628757630000074
机载惯性导航***输出的位置和速度对应于某颗卫星i的计算伪距率为：
Figure BDA0003628757630000075
将上式在
Figure BDA0003628757630000076
处泰勒级数展开并忽略高阶项得：
Figure BDA0003628757630000077
将式(23)与式(19)做差可得相对某颗卫星i,i＝1,2,3,4的伪距率量测模型为：
Figure BDA0003628757630000078
由ECEF坐标系与地理坐标系的转换关系可得：
Figure BDA0003628757630000079
将式(25)代入式(24)可得伪距率量测模型为：
Figure BDA00036287576300000710
Figure BDA0003628757630000081
Figure BDA0003628757630000082
Figure BDA0003628757630000083
上述的步骤3中，卡尔曼滤波器对机载惯性导航***误差以及GPS接收机时钟误差信息进行最优估计，并通过负反馈的方式对***进行校正与状态更新，具体的：
状态一步预测模型为：
Figure BDA0003628757630000084
一步预测均方误差模型为：
Figure BDA0003628757630000085
状态更新模型为：
Figure BDA0003628757630000086
滤波增益模型为：
Figure BDA0003628757630000087
估计均方误差模型为：
Figure BDA0003628757630000088
本发明具有以下有益效果：
本发明在对机载惯性导航***精确建模的基础上，采用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)捷联机载惯性导航***平台误差角：
Figure BDA0003628757630000101
式中，δ表示误差；
Figure BDA0003628757630000102
为机载惯导***平台误差角微分形式。δvN为机载惯性导航***北向速度误差，RM为地球子午圈曲率半径；wie为地球自转角速度；h为无人机高度信息；L为无人机地理坐标系下纬度信息；RN为地球卯酉圈曲率半径；εN为机载北向陀螺噪声；εU为机载陀螺天向噪声；vE为机载惯性导航***东向速度，
Figure BDA0003628757630000103
为机载惯性导航***北向平台误差角；
Figure BDA0003628757630000104
为机载惯性导航***天向平台误差角，δvE为机载惯性导航***东向速度误差；δL为机载惯性导航***纬度误差，
Figure BDA0003628757630000105
为机载惯性导航***东向平台误差角；vN为机载惯性导航***北向速度，εE为东向陀螺噪声，E,N,U代表地理坐标系下东、北、天三个方向；
(2)机载惯性导航***速度误差，其模型为：
Figure BDA0003628757630000106
式中，
Figure BDA0003628757630000107
分别为机载惯导***东向、北向、天向速度误差的微分形式。vN为机载惯性导航***北向速度；fN为无人机机载加速度计北向比力信息；fU为无人机机载加速度计天向比力信息；fE为无人机机载加速度计东向比力信息；R为地球半径；g为地球重力加速度；vU为机载惯性导航***天向速度；δvU为机载惯性导航***天向速度误差；▽E为东向加速度计一阶马尔可夫过程白噪声；▽N为北向加速度计一阶马尔可夫过程白噪声；▽U为天向加速度计一阶马尔可夫过程白噪声；δh为机载惯性导航***高度误差；
(3)机载惯性导航***位置误差，其模型为：
Figure BDA0003628757630000111
式中，
Figure BDA0003628757630000112
为无人机机载惯导***纬度、经度、高度误差的微分形式；δvN为机载惯性导航***北向速度误差；δvE为机载惯性导航***东向速度误差；δvU为机载惯性导航***天向速度误差；δL为机载惯性导航***纬度误差；
(4)惯性器件误差，包括陀螺误差和加速度计误差，其模型为：
惯性器件的误差一般指加速度计和陀螺零偏，考虑三轴加速度计和陀螺误差模型相同的。则陀螺的误差模型可由以下模型进行表示：
陀螺误差：
ε＝εb+εr+wg (4)
其中，εb为陀螺随机常数；εr为陀螺一阶马尔科夫过程；wg为陀螺白噪声；
陀螺随机常数和一阶马尔科夫过程可表示为：
Figure BDA0003628757630000113
式中，Tg为陀螺一阶马尔可夫相关时间，wr陀螺一阶马尔可夫过程驱动白噪声；
加速度计误差一般考虑加速度计一阶马尔可夫过程，数学表达式可表示为：
Figure BDA0003628757630000114
Ta为加速度计一阶马尔科夫过程相关时间，wa为加速度计驱动白噪声。
2、通过将GPS接收机时钟偏差转换成相应的距离以及等效时钟频率误差相应的距离率作为***状态量进行最优估计，进而对GPS接收机时钟误差进行实时估计与在线补偿，具体的：
GPS接收机等效时钟误差相应的距离和等效时钟频率误差相应的距离率作为***状态量进行估计，其微分模型可表示为：
Figure BDA0003628757630000121
式中
Figure BDA0003628757630000122
为GPS接收机时钟误差等效的相应距离和等效时钟频率误差相应的距离率的微分形式。βtru为GPS接收机等效时钟频率误差系数。(wru,wtru)为GPS接收机时钟误差等效的相应距离以及相应距离率高斯白噪声。
公式(7)作为GPS接收机时钟误差等效的相应距离以及距离率误差微分方程。即GPS接收机与卫星之间的时钟偏差，基于无线电信号达到时间(TOA)原理，将导致GPS接收机与卫星的伪距误差。本发明通过将卫星接收机时钟偏差转换成相应的距离以及距离率作为***状态量进行最优估计，进而对GPS接收机时钟误差进行实时估计与在线补偿，降低GPS接收机时钟误差对定位精度的影响，提高***的绝对定位精度。
3、构建如下***连续时间状态递推模型：
Figure BDA0003628757630000123
式中
Figure BDA0003628757630000127
为***状态量的微分形式；X(t)为***状态量，F(t)为***状态转移矩阵；G(t)为***噪声控制输入矩阵；W(t)为***噪声矩阵；
Figure BDA0003628757630000125
在***状态递推模型中，状态量主要包括为：
Figure BDA0003628757630000126
为无人机机载惯导地理坐标系下，东向、北向、天向三个方向的平台误差角；
δvE,δvN,δvU为无人机机载惯导地理坐标系下，东向速度误差、北向速度误差、天向速度误差；
δL,δλ,δh为无人机机载惯导地理坐标系下，纬度误差、经度误差、高度误差；
εbx,εby,εbz为无人机机体坐标系下，陀螺三轴随机常数；
εrx,εry,εrz为无人机机体坐标系下，陀螺三轴一阶马尔科夫过程随机噪声；
▽x,▽y,▽z为无人机机体坐标系下，三轴加速度计一阶马尔科夫过程；
δtu,δtru为GPS接收机等效时钟误差相应的距离和等效时钟频率误差相应的距离率。
将***误差模型按照(8)式展开可得***状态转移矩阵为：
Figure BDA0003628757630000131
其中，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的计算伪距可表示为：
Figure BDA0003628757630000132
式中，(xsi,ysi,zsi)为GPS第i颗卫星t时刻在地球固定坐标系(Earth CenteredEarth Fixed,ECEF)上的位置；ρIi为无人机机载惯性与卫星i之间的计算伪距；(xI,yI,zI)为无人机机载惯性解算得到的位置信息。
将式(11)在载***置真值(x,y,z)处泰勒级数展开并取前两项可得：
Figure BDA0003628757630000141
Figure BDA0003628757630000142
同理可得：
Figure BDA0003628757630000143
将式(12)与式(10)相减可得相对某颗卫星i＝1,2,3,4的伪距测量差矢量模型为：
Figure BDA0003628757630000144
由于***状态量为地理系下惯导纬度误差、经度误差、高度误差，将ECEF坐标系下δx,δy,δz转换用δL,δλ,δh表示对应的转换关系为：
Figure BDA0003628757630000145
将式(15)代入式(14)可得***伪距量测模型为：
Zρ(t)＝δρ＝Hρ(t)X(t)+Wρ(t) (16)
式中Hρ＝[04×6 Hρ1 04×9 Hρ2]4×20
Figure BDA0003628757630000146
Figure BDA0003628757630000147
2、构建***的伪距率量测模型，具体为：
利用机载GPS接收机的伪距率构建伪距率量测模型；
GPS接收机输出对应的某颗卫星i的测量伪距率为：
Figure BDA0003628757630000151
式中，
Figure BDA0003628757630000152
为卫星i到机载GPS接收机的伪距率真值，δtru为等效时钟频率误差相应的距离率，vρi为白噪声伪距率测量误差；
对式(11)求导得：
Figure BDA0003628757630000153
则有：
Figure BDA0003628757630000154
机载惯性导航***输出的位置和速度对应于某颗卫星i的计算伪距率为：
Figure BDA0003628757630000155
将上式在
Figure BDA0003628757630000156
处泰勒级数展开并忽略高阶项得：
Figure BDA0003628757630000157
将式(23)与式(19)做差可得相对某颗卫星i,i＝1,2,3,4的伪距率量测模型为：
Figure BDA0003628757630000158
由ECEF坐标系与地理坐标系的转换关系可得：
Figure BDA0003628757630000159
将式(25)代入式(24)可得伪距率量测模型为：
Figure BDA00036287576300001510
Figure BDA0003628757630000161
Figure BDA0003628757630000162
Figure BDA0003628757630000163
步骤3：在上述对机载惯性导航***精确建模的基础上，即基于步骤1和步骤2的模型，通过卡尔曼滤波器对机载惯性导航***误差以及GPS接收机时钟误差信息进行最优估计，并通过负反馈的方式对***进行校正与状态更新，具体的：
状态一步预测模型为：
Figure BDA0003628757630000164
一步预测均方误差模型为：
Figure BDA0003628757630000165
状态更新模型为：
Figure BDA0003628757630000166
滤波增益模型为：
Figure BDA0003628757630000167
估计均方误差模型为：
Figure BDA0003628757630000168
为了验证所提出算法的可行性，本发明设计了对应的仿真实验。
在仿真中设计了无人机载体不同飞行轨迹包括：加速、减速、转弯、平飞等。
模拟机载惯性导航***陀螺仪及加速度计输出以及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)捷联机载惯性导航***平台误差角：
Figure FDA0003628757620000011
上式中，δ表示误差；
Figure FDA0003628757620000012
为机载惯导***平台误差角微分形式。δvN为机载惯性导航***北向速度误差，RM为地球子午圈曲率半径；wie为地球自转角速度；h为无人机高度信息；L为无人机地理坐标系下纬度信息；RN为地球卯酉圈曲率半径；εN为机载北向陀螺噪声；εU为机载陀螺天向噪声；vE为机载惯性导航***东向速度，
Figure FDA0003628757620000013
为机载惯性导航***北向平台误差角；
Figure FDA0003628757620000014
为机载惯性导航***天向平台误差角，δvE为机载惯性导航***东向速度误差；δL为机载惯性导航***纬度误差，
Figure FDA0003628757620000015
为机载惯性导航***东向平台误差角；vN为机载惯性导航***北向速度，εE为东向陀螺噪声，E,N,U代表地理坐标系下东、北、天三个方向；
(2)机载惯性导航***速度误差，其模型为：
Figure FDA0003628757620000021
上式中，
Figure FDA0003628757620000022
分别为机载惯导***东向、北向、天向速度误差的微分形式。vN为机载惯性导航***北向速度；fN为无人机机载加速度计北向比力信息；fU为无人机机载加速度计天向比力信息；fE为无人机机载加速度计东向比力信息；R为地球半径；g为地球重力加速度；vU为机载惯性导航***天向速度；δvU为机载惯性导航***天向速度误差；
Figure FDA0003628757620000023
为东向加速度计一阶马尔可夫过程白噪声；
Figure FDA0003628757620000024
为北向加速度计一阶马尔可夫过程白噪声；
Figure FDA0003628757620000025
为天向加速度计一阶马尔可夫过程白噪声；δh为机载惯性导航***高度误差；
(3)机载惯性导航***位置误差，其模型为：
Figure FDA0003628757620000026
式中，
Figure FDA0003628757620000027
为无人机机载惯导***纬度、经度、高度误差的微分形式；δvN为机载惯性导航***北向速度误差；δvE为机载惯性导航***东向速度误差；δvU为机载惯性导航***天向速度误差；δL为机载惯性导航***纬度误差；
(4)惯性器件误差，包括陀螺误差和加速度计误差，其模型为：
陀螺误差：
ε＝εb+εr+wg (4)
其中，εb为陀螺随机常数；εr为陀螺一阶马尔科夫过程；wg为陀螺白噪声；
陀螺随机常数和一阶马尔科夫过程可表示为：
Figure FDA0003628757620000031
式中，Tg为陀螺一阶马尔可夫相关时间，wr陀螺一阶马尔可夫过程驱动白噪声；
加速度计误差为：
Figure FDA0003628757620000032
Ta为加速度计一阶马尔科夫过程相关时间，wa为加速度计驱动白噪声。
3.根据权利要求1所述的基于INS与GPS紧组合的无人机组合导航方法，其特征在于，步骤1中通过将GPS接收机时钟偏差转换成相应的距离以及等效时钟频率误差相应的距离率作为***状态量进行最优估计，进而对GPS接收机时钟误差进行实时估计与在线补偿，具体的：
GPS接收机等效时钟误差相应的距离和等效时钟频率误差相应的距离率作为***状态量进行估计，其微分模型可表示为：
Figure FDA0003628757620000033
式中
Figure FDA0003628757620000034
为GPS接收机时钟误差等效的相应距离和等效时钟频率误差相应的距离率的微分形式；βtru为GPS接收机等效时钟频率误差系数；(wru,wtru)为GPS接收机时钟误差等效的相应距离以及相应距离率高斯白噪声。
4.根据权利要求1所述的基于INS与GPS紧组合的无人机组合导航方法，其特征在于，所述步骤1构建如下***连续时间状态递推模型：
Figure FDA0003628757620000035
式中
Figure FDA0003628757620000036
为***状态量的微分形式；X(t)为***状态量，F(t)为***状态转移矩阵；G(t)为***噪声控制输入矩阵；W(t)为***噪声矩阵；
Figure FDA0003628757620000041
在***状态递推模型中，状态量包括
Figure FDA0003628757620000042
为无人机机载惯导***地理坐标系下东向、北向、天向三个方向的平台误差角；
δvE,δvN,δvU为无人机机载惯导***地理坐标系下东向速度误差、北向速度误差、天向速度误差；
δL,δλ,δh为无人机机载惯导***地理坐标系下纬度误差、经度误差、高度误差；
εbx,εby,εbz为机体坐标系下陀螺三轴随机常数；
εrx,εry,εrz为机体坐标系下陀螺三轴一阶马尔科夫过程随机噪声；
Figure FDA0003628757620000043
为机体坐标系下三轴加速度计一阶马尔科夫过程；
δtu,δtru为GPS接收机等效时钟误差相应的距离和等效时钟频率误差相应的距离率；
Figure FDA0003628757620000044
其中，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的计算伪距可表示为：
Figure FDA0003628757620000045
式中，(xsi,ysi,zsi)为GPS第i颗卫星t时刻在ECEF上的位置；ρIi为无人机机载惯性与卫星i之间的计算伪距；(xI,yI,zI)为无人机机载惯性解算得到的位置信息；
将式(11)在无人机位置真值(x,y,z)处泰勒级数展开并取前两项可得：
Figure FDA0003628757620000051
Figure FDA0003628757620000052
同理可得：
Figure FDA0003628757620000053
将式(12)与式(10)相减可得相对某颗卫星i＝1,2,3,4的伪距测量差矢量模型为：
Figure FDA0003628757620000054
由于***状态量为地理系下惯导纬度误差、经度误差、高度误差，将ECEF坐标系下δx,δy,δz转换用δL,δλ,δh表示对应的转换关系为：
Figure FDA0003628757620000055
将式(15)代入式(14)可得***伪距量测模型为：
Zρ(t)＝δρ＝Hρ(t)X(t)+Wρ(t) (16)
式中Hρ＝[04×6 Hρ1 04×9 Hρ2]4×20
Figure FDA0003628757620000056
Figure FDA0003628757620000061
6.根据权利要求1所述的基于INS与GPS紧组合的无人机组合导航方法，其特征在于，所述步骤2构建***的伪距率量测模型，具体为：
利用机载GPS接收机的伪距率构建伪距率量测模型；
GPS接收机输出对应的某颗卫星i的测量伪距率为：
Figure FDA0003628757620000062
式中，
Figure FDA0003628757620000063
为卫星i到机载GPS接收机的伪距率真值，δtru为等效时钟频率误差相应的距离率，vρi为白噪声伪距率测量误差；
对式(11)求导得：
Figure FDA0003628757620000064
则有：
Figure FDA0003628757620000065
机载惯性导航***输出的位置和速度对应于某颗卫星i的计算伪距率为：
Figure FDA0003628757620000066
将上式在
Figure FDA0003628757620000067
处泰勒级数展开并忽略高阶项得：
Figure FDA0003628757620000068
将式(23)与式(19)做差可得相对某颗卫星i,i＝1,2,3,4的伪距率量测模型为：
Figure FDA0003628757620000069
由ECEF坐标系与地理坐标系的转换关系可得：
Figure FDA0003628757620000071
将式(25)代入式(24)可得伪距率量测模型为：
Figure FDA0003628757620000072
Figure FDA0003628757620000073
Figure FDA0003628757620000074
Figure FDA0003628757620000075
7.根据权利要求1所述的基于INS与GPS紧组合的无人机组合导航方法，其特征在于，所述步骤3中，卡尔曼滤波器对机载惯性导航***误差以及GPS接收机时钟误差信息进行最优估计，并通过负反馈的方式对***进行校正与状态更新，具体的：
状态一步预测模型为：
Figure FDA0003628757620000076
一步预测均方误差模型为：
Figure FDA0003628757620000077
状态更新模型为：
Figure FDA0003628757620000078
滤波增益模型为：
Figure FDA0003628757620000079
估计均方误差模型为：
Figure FDA00036287576200000710
CN202210483843.6A 2022-05-06 2022-05-06 基于ins与gps紧组合的无人机组合导航方法 Withdrawn CN115031728A (zh)

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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117214933A (zh) * 2023-11-07 2023-12-12 中国船舶集团有限公司第七〇七研究所 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法

Cited By (2)

* Cited by examiner, † Cited by third party
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