CN108151737B - 一种动态互观测关系条件下的无人机蜂群协同导航方法 - Google Patents
一种动态互观测关系条件下的无人机蜂群协同导航方法 Download PDFInfo
- Publication number
- CN108151737B CN108151737B CN201711371353.2A CN201711371353A CN108151737B CN 108151737 B CN108151737 B CN 108151737B CN 201711371353 A CN201711371353 A CN 201711371353A CN 108151737 B CN108151737 B CN 108151737B
- Authority
- CN
- China
- Prior art keywords
- navigation
- unmanned aerial
- aerial vehicle
- state quantity
- error state
- 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
- 238000000034 method Methods 0.000 title claims abstract description 23
- 238000005259 measurement Methods 0.000 claims abstract description 42
- 239000011159 matrix material Substances 0.000 claims description 23
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000012937 correction Methods 0.000 claims description 6
- 238000001914 filtration Methods 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 4
- 230000017105 transposition Effects 0.000 claims description 2
- 230000008859 change Effects 0.000 abstract description 6
- 238000013461 design Methods 0.000 abstract description 2
- 230000008569 process Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000010586 diagram Methods 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/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
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying
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)
- Aviation & Aerospace Engineering (AREA)
- Navigation (AREA)
Abstract
本发明公开一种动态互观测关系条件下的无人机蜂群协同导航方法,包括如下步骤:步骤1,初始化,对无人机蜂群中的无人机按照1~n进行编号;步骤2,选取无人机蜂群导航状态量,建立导航状态方程和独立导航量测方程;步骤3,建立协同导航量测方程;步骤4,无人机蜂群中的无人机分别进行独立导航解算;步骤5,无人机蜂群中的无人机分别进行协同导航解算;步骤6,判断导航是否结束,如结束则停止导航,否则返回步骤4。此种方法可在各无人机利用既有自身量测信息进行独立导航的基础上,设计适用于视线矢量测量信息动态变化的协同导航方法对导航性能进行进一步优化,有效提高无人机蜂群中无人机的导航精度。
Description
技术领域
本发明属于定位与导航技术领域,特别涉及一种动态互观测关系条件下的无人机蜂群协同导航方法。
背景技术
导航技术是无人机实现可靠控制和稳定飞行的关键技术之一。传统无人机主要依靠自身携带的组合导航***,在卫星导航等测量信息的辅助下通过不断修正惯导误差进行持续导航。在卫星导航信号干扰或遮蔽条件下,惯导误差难以得到有效抑制,使得导航性能下降。
由多架微无人机组成的蜂群不仅能够完成更为丰富的任务,而且有望引入蜂群中无人机之间的视线矢量测量信息,通过蜂群中微无人机的相互协同提高导航性能。但需要考虑各无人机自身既有独立导航和无人机之间协同导航两者之间的协调运行,并兼顾各无人机在飞行过程中相互位置变化所引起的视线矢量测量信息数量和实现矢量观测关系的动态变化。
发明内容
本发明的目的,在于提供一种动态互观测关系条件下的无人机蜂群协同导航方法,其可在各无人机利用既有自身量测信息进行独立导航的基础上,设计适用于视线矢量测量信息动态变化的协同导航方法对导航性能进行进一步优化,有效提高无人机蜂群中无人机的导航精度。
为了达成上述目的,本发明的解决方案是:
一种动态互观测关系条件下的无人机蜂群协同导航方法,包括如下步骤:
步骤1,初始化,对无人机蜂群中的无人机按照1~n进行编号;
步骤2,选取无人机蜂群导航状态量,建立导航状态方程和独立导航量测方程;
步骤3,建立协同导航量测方程;
步骤4,无人机蜂群中的无人机分别进行独立导航解算;
步骤5,无人机蜂群中的无人机分别进行协同导航解算;
步骤6,判断导航是否结束,如结束则停止导航,否则返回步骤4。
上述步骤2的详细内容是:
对于其中任意一架无人机i,i∈{1,2,…,n},导航状态量表达式为:
其中,φE,φN,φU分别表示无人机i机载惯性导航***误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量;δvE,δvN,δvU分别表示无人机i机载惯性导航***误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量;δL,δλ,δh分别表示无人机i机载惯性导航***误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量;εbx,εby,εbz,εrx,εry,εrz分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量;分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴和Z轴方向加速度计零偏,T为转置;
导航状态方程表达式为:
Xi(k)=Φi(k,k-1)Xi(k-1)+Γi(k,k-1)Wi(k-1)
其中,Φi(k-1)为tk-1时刻至tk时刻***的状态转移矩阵,Wi(k-1)为tk时刻***的噪声矢量,Γi(k,k-1)为tk-1时刻至tk时刻***的噪声驱动矩阵;Xi(k-1)为tk-1时刻无人机i机载惯性导航***误差状态量,Xi(k)为tk时刻无人机i机载惯性导航***误差状态量;
独立导航量测方程表达式为:
Zi(k)=Hi(k)Xi(k)+Vi(k)
其中Zi(k)为tk时刻无人机i惯导输出的飞行器速度矢量,Vi(k)为tk时刻无人机i的卫星导航接收机噪声,其协方差矩阵为Ri(k),Hi(k)为tk时刻无人机i的惯性/卫星组合量测矩阵。
上述步骤3中,协同导航量测方程的表达式为:
Zij(k)=Hij(k)Xi(k)+Vij(k)
其中Zij(k)为tk时刻无人机j对于无人机i的协同导航位置偏差,Vij(k)为tk时刻无人机j对于无人机i的协同导航位置偏差测量噪声,Hij(k)为tk时刻无人机j对于无人机i的协同导航量测矩阵,其表达式为:
Hij=[03×6 diag[RN RM 1] 03×9]
其中RN为无人机i所在当地卯酉圈曲率半径,RM为无人机i所在当地子午圈曲率半径,hi为无人机i所在高度。
上述步骤4的详细内容是:对于其中任意一架无人机i,i∈{1,2,…,n},独立导航解算包括如下子步骤:
步骤41,采集惯性器件数据,进行惯性导航解算;
步骤42,判断是否有卫星导航数据,如果有,则继续执行步骤43,否则执行步骤45;
步骤43,采集卫星导航数据,根据步骤2建立的独立导航量测方程进行惯性/卫星组合滤波并计算惯导误差状态估计值及惯导误差状态估计协方差,其表达式为:
Pi(k)=[I-Ki(k)Hi(k)][Φi(k,k-1)Pi(k)Φi(k,k-1)T+Γi(k,k-1)QiΓi(k,k-1)T]
Ki(k)=Pi(k)Hi(k)[Hi(k)Pi(k)Hi(k)+Ri(k)]-1
步骤44,利用步骤43估计的惯导误差进行惯性导航修正;
步骤45,输出惯性导航***指示的经度λi、纬度Li、高度hi作为无人机i的独立导航解算结果。
上述步骤5的详细内容是:对于其中任意一架无人机i,i∈{1,2,…,n},协同导航解算包括如下子步骤:
步骤51,令j=1;
步骤52,判断是否j=i,如果是则执行步骤58,否则继续执行步骤53;
步骤53,判断是否可以获得无人机i到无人机j的视线矢量数据,如是则执行步骤54,否则执行步骤58;
步骤54,采集无人机i机体坐标系下无人机i到无人机j的视线矢量数据dij,并根据步骤45获得的无人机i和无人机j的独立导航解算结果,进行协同导航位置偏差解算,其表达式为:
其中rj为无人机j在无人机i所在当地地理坐标系下的位置矢量,其表达式为
rj=[(λj-λi)RN (Lj-Li)RM hj-hi]T
其中λi、Li、hi分别为无人机i的独立导航解算结果中的经度、纬度、高度;λj、Lj、hj分别为无人机j的独立导航解算结果中的经度、纬度、高度;
步骤55,进行协同导航位置偏差协方差解算,其表达式为:
其中σdλ,ij,σdL,ij,σdh,ij分别为无人机j在无人机i所在当地地理坐标系经度、纬度、高度方向的为相对视线矢量测量误差协方差,其表达式为:
其中上标(s)表示矩阵对角线上的第s个元素,δθ表示相对视线矢量方向测量误差,ε表示相对距离测量误差;
其中σrλ,ij,σrL,ij,σrh,ij分别为在无人机i所在当地地理坐标系经度、纬度、高度方向的无人机j独立导航解算误差协方差,其表达式为:
步骤56,利用步骤54获得的协同导航位置偏差和步骤55获得的协同导航位置偏差协方差,根据步骤3建立的协同导航量测方程进行协同导航滤波并更新惯导误差状态量估计值和惯导误差状态量估计协方差,其表达式为:
Pij(k)=[I-Kij(k)Hij(k)]Pi(k)
Kij(k)=Pi(k)Hij(k)[Hij(k)Pi(k)Hij(k)+Rij(k)]-1
步骤58:判断是否j<n,如果是则令j=j+1,并执行步骤52,否则执行步骤59;
步骤59,输出惯性导航***指示的经度λi、纬度Li、高度hi作为无人机i的协同导航解算结果。
采用上述方案后,本发明能够适应无人机蜂群在飞行过程中相互视线矢量观测条件的不断变化,有效实现协同导航与自身既有独立导航***的协调运行,与无人机独立导航相比可有效提升导航精度,具有良好的应用前景。
附图说明
图1是本发明的原理流程示意图;
图2是采用本发明协同导航和未优化的无人机蜂群中无人机平均导航误差对比图。
具体实施方式
以下将结合附图,对本发明的技术方案及有益效果进行详细说明。
如图1所示,本发明提供一种动态互观测关系条件下的无人机蜂群协同导航方法,包括如下步骤:
步骤1,初始化,对无人机蜂群中的无人机按照1~n进行编号;
步骤2,选取无人机蜂群导航状态量,建立导航状态方程和独立导航量测方程;对于其中任意一架无人机i,i∈{1,2,…,n},导航状态量表达式为:
其中,φE,φN,φU分别表示无人机i机载惯性导航***误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量;δvE,δvN,δvU分别表示无人机i机载惯性导航***误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量;δL,δλ,δh分别表示无人机i机载惯性导航***误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量;εbx,εby,εbz,εrx,εry,εrz分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量;分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴和Z轴方向加速度计零偏,T为转置;
导航状态方程表达式为:
Xi(k)=Φi(k,k-1)Xi(k-1)+Γi(k,k-1)Wi(k-1)
其中,Φi(k-1)为tk-1时刻至tk时刻***的状态转移矩阵,Wi(k-1)为tk时刻***的噪声矢量,Γi(k,k-1)为tk-1时刻至tk时刻***的噪声驱动矩阵;Xi(k-1)为tk-1时刻无人机i机载惯性导航***误差状态量,Xi(k)为tk时刻无人机i机载惯性导航***误差状态量;
独立导航量测方程表达式为:
Zi(k)=Hi(k)Xi(k)+Vi(k)
其中Zi(k)为tk时刻无人机i惯导输出的飞行器速度矢量,Vi(k)为tk时刻无人机i的卫星导航接收机噪声,其协方差矩阵为Ri(k),Hi(k)为tk时刻无人机i的惯性/卫星组合量测矩阵;
步骤3,建立协同导航量测方程,其表达式为:
Zij(k)=Hij(k)Xi(k)+Vij(k)
其中Zij(k)为tk时刻无人机j对于无人机i的协同导航位置偏差,Vij(k)为tk时刻无人机j对于无人机i的协同导航位置偏差测量噪声,Hij(k)为tk时刻无人机j对于无人机i的协同导航量测矩阵,其表达式为:
Hij=[03×6 diag[RN RM 1] 03×9]
其中RN为无人机i所在当地卯酉圈曲率半径,RM为无人机i所在当地子午圈曲率半径,hi为无人机i所在高度;
步骤4,无人机蜂群中的无人机分别进行独立导航解算,对于其中任意一架无人机i,i∈{1,2,…,n},独立导航解算包括如下子步骤:
步骤41,采集惯性器件数据,进行惯性导航解算;
步骤42,判断是否有卫星导航数据,如果有,则继续执行步骤43,否则执行步骤45;
步骤43,采集卫星导航数据,根据步骤2建立的独立导航量测方程进行惯性/卫星组合滤波并计算惯导误差状态估计值及惯导误差状态估计协方差,其表达式为:
Pi(k)=[I-Ki(k)Hi(k)][Φi(k,k-1)Pi(k)Φi(k,k-1)T+Γi(k,k-1)QiΓi(k,k-1)T]
Ki(k)=Pi(k)Hi(k)[Hi(k)Pi(k)Hi(k)+Ri(k)]-1
步骤44,利用步骤43估计的惯导误差进行惯性导航修正;
步骤45,输出惯性导航***指示的经度λi、纬度Li、高度hi作为无人机i的独立导航解算结果;
步骤5,无人机蜂群中的无人机分别进行协同导航解算,对于其中任意一架无人机i,i∈{1,2,…,n},协同导航解算包括如下子步骤:
步骤51,令j=1;
步骤52,判断是否j=i,如果是则执行步骤58,否则继续执行步骤53;
步骤53,判断是否可以获得无人机i到无人机j的视线矢量数据,如是则执行步骤54,否则执行步骤58;
步骤54,采集无人机i机体坐标系下无人机i到无人机j的视线矢量数据dij,并根据步骤45获得的无人机i和无人机j的独立导航解算结果,进行协同导航位置偏差解算,其表达式为:
其中rj为无人机j在无人机i所在当地地理坐标系下的位置矢量,其表达式为
rj=[(λj-λi)RN (Lj-Li)RM hj-hi]T
其中λi、Li、hi分别为无人机i的独立导航解算结果中的经度、纬度、高度;λj、Lj、hj分别为无人机j的独立导航解算结果中的经度、纬度、高度;
步骤55,进行协同导航位置偏差协方差解算,其表达式为:
其中σdλ,ij,σdL,ij,σdh,ij分别为无人机j在无人机i所在当地地理坐标系经度、纬度、高度方向的为相对视线矢量测量误差协方差,其表达式为:
其中上标(s)表示矩阵对角线上的第s个元素,δθ表示相对视线矢量方向测量误差,ε表示相对距离测量误差;
其中σrλ,ij,σrL,ij,σrh,ij分别为在无人机i所在当地地理坐标系经度、纬度、高度方向的无人机j独立导航解算误差协方差,其表达式为:
步骤56,利用步骤54获得的协同导航位置偏差和步骤55获得的协同导航位置偏差协方差,根据步骤3建立的协同导航量测方程进行协同导航滤波并更新惯导误差状态量估计值和惯导误差状态量估计协方差,其表达式为:
Pij(k)=[I-Kij(k)Hij(k)]Pi(k)
Kij(k)=Pi(k)Hij(k)[Hij(k)Pi(k)Hij(k)+Rij(k)]-1
步骤58:判断是否j<n,如果是则令j=j+1,并执行步骤52,否则执行步骤59;
步骤59,输出惯性导航***指示的经度λi、纬度Li、高度hi作为无人机i的协同导航解算结果;
步骤6,判断导航是否结束,如结束则停止导航,否则返回步骤4。
为了验证本发明所提出的用于动态观测关系条件的无人机蜂群协同导航方法的有效性,进行数字仿真分析。仿真中采用的无人机蜂群中无人机数量为60~240架,视线矢量的方向测量精度为0.1度,视线矢量的距离测量精度为20厘米。图2为采用本发明方法优化和未优化的无人机蜂群中无人机平均导航误差对比图。
由图2可以看出,采用本发明所提出的一种用于动态观测关系条件的无人机蜂群协同导航方法后,蜂群中无人机平均定位误差与独立导航相比降低10.4%~30.9%。此外,采用本发明方法能够适应无人机蜂群在飞行过程中相互视线矢量观测条件的不断变化,且随着蜂群中无人机数量的增加定位误差可以通过协同持续显著降低,具有良好的应用价值。
以上实施例仅为说明本发明的技术思想,不能以此限定本发明的保护范围,凡是按照本发明提出的技术思想,在技术方案基础上所做的任何改动,均落入本发明保护范围之内。
Claims (4)
1.一种动态互观测关系条件下的无人机蜂群协同导航方法,其特征在于包括如下步骤:
步骤1,初始化,对无人机蜂群中的无人机按照1~n进行编号;
步骤2,选取无人机蜂群导航状态量,建立导航状态方程和独立导航量测方程;
步骤3,建立协同导航量测方程;
步骤4,无人机蜂群中的无人机分别进行独立导航解算;
步骤5,无人机蜂群中的无人机分别进行协同导航解算;
所述步骤5的详细内容是:对于其中任意一架无人机i,i∈{1,2,…,n},协同导航解算包括如下子步骤:
步骤51,令j=1;
步骤52,判断是否j=i,如果是则执行步骤58,否则继续执行步骤53;
步骤53,判断是否可以获得无人机i到无人机j的视线矢量数据,如是则执行步骤54,否则执行步骤58;
步骤54,采集无人机i机体坐标系下无人机i到无人机j的视线矢量数据dij,并根据步骤4获得的无人机i和无人机j的独立导航解算结果,进行协同导航位置偏差解算,其表达式为:
其中rj为无人机j在无人机i所在当地地理坐标系下的位置矢量,其表达式为
rj=[(λj-λi)RN (Lj-Li)RM hj-hi]T
其中λi、Li、hi分别为无人机i的独立导航解算结果中的经度、纬度、高度;λj、Lj、hj分别为无人机j的独立导航解算结果中的经度、纬度、高度;RN为无人机i所在当地卯酉圈曲率半径,RM为无人机i所在当地子午圈曲率半径;
步骤55,进行协同导航位置偏差协方差解算,其表达式为:
其中σdλ,ij,σdL,ij,σdh,ij分别为无人机j在无人机i所在当地地理坐标系经度、纬度、高度方向的相对视线矢量测量误差协方差,其表达式为:
其中上标(s)表示矩阵对角线上的第s个元素,δθ表示相对视线矢量方向测量误差,ε表示相对距离测量误差;
其中σrλ,ij,σrL,ij,σrh,ij分别为在无人机i所在当地地理坐标系经度、纬度、高度方向的无人机j独立导航解算误差协方差,其表达式为:
步骤56,利用步骤54获得的协同导航位置偏差和步骤55获得的协同导航位置偏差协方差,根据步骤3建立的协同导航量测方程进行协同导航滤波并更新惯导误差状态量估计值和惯导误差状态量估计协方差,其表达式为:
Pij(k)=[I-Kij(k)Hij(k)]Pi(k)
Kij(k)=Pi(k)Hij(k)[Hij(k)Pi(k)Hij(k)+Rij(k)]-1
其中,Zij(k)为tk时刻无人机j对于无人机i的协同导航位置偏差,Hij(k)为tk时刻无人机j对于无人机i的协同导航量测矩阵;
步骤58:判断是否j<n,如果是则令j=j+1,并执行步骤52,否则执行步骤59;
步骤59,输出惯性导航***指示的经度λi、纬度Li、高度hi作为无人机i的协同导航解算结果;
步骤6,判断导航是否结束,如结束则停止导航,否则返回步骤4。
2.如权利要求1所述的一种动态互观测关系条件下的无人机蜂群协同导航方法,其特征在于:所述步骤2的详细内容是:
对于其中任意一架无人机i,i∈{1,2,…,n},导航状态量表达式为:
其中,φE,φN,φU分别表示无人机i机载惯性导航***误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量;δvE,δvN,δvU分别表示无人机i机载惯性导航***误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量;δL,δλ,δh分别表示无人机i机载惯性导航***误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量;εbx,εby,εbz,εrx,εry,εrz分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量;分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴和Z轴方向加速度计零偏,T为转置;
导航状态方程表达式为:
Xi(k)=Φi(k,k-1)Xi(k-1)+Γi(k,k-1)Wi(k-1)
其中,Φi(k,k-1)为tk-1时刻至tk时刻***的状态转移矩阵,Wi(k-1)为tk-1时刻***的噪声矢量,Γi(k,k-1)为tk-1时刻至tk时刻***的噪声驱动矩阵;Xi(k-1)为tk-1时刻无人机i机载惯性导航***误差状态量,Xi(k)为tk时刻无人机i机载惯性导航***误差状态量;
独立导航量测方程表达式为:
Zi(k)=Hi(k)Xi(k)+Vi(k)
其中Zi(k)为tk时刻无人机i惯导输出的飞行器速度矢量,Vi(k)为tk时刻无人机i的卫星导航接收机噪声,其协方差矩阵为Ri(k),Hi(k)为tk时刻无人机i的惯性/卫星组合量测矩阵。
3.如权利要求1所述的一种动态互观测关系条件下的无人机蜂群协同导航方法,其特征在于:所述步骤3中,协同导航量测方程的表达式为:
Zij(k)=Hij(k)Xi(k)+Vij(k)
其中,Xi(k)为tk时刻无人机i机载惯性导航***误差状态量,Zij(k)为tk时刻无人机j对于无人机i的协同导航位置偏差,Vij(k)为tk时刻无人机j对于无人机i的协同导航位置偏差测量噪声,Hij(k)为tk时刻无人机j对于无人机i的协同导航量测矩阵,其表达式为:
Hij=[03×6 diag[RN RM 1] 03×9]
其中RN为无人机i所在当地卯酉圈曲率半径,RM为无人机i所在当地子午圈曲率半径,hi为无人机i所在高度。
4.如权利要求1所述的一种动态互观测关系条件下的无人机蜂群协同导航方法,其特征在于:所述步骤4的详细内容是:对于其中任意一架无人机i,i∈{1,2,…,n},独立导航解算包括如下子步骤:
步骤41,采集惯性器件数据,进行惯性导航解算;
步骤42,判断是否有卫星导航数据,如果有,则继续执行步骤43,否则执行步骤45;
步骤43,采集卫星导航数据,根据步骤2建立的独立导航量测方程进行惯性/卫星组合滤波并计算惯导误差状态估计值及惯导误差状态估计协方差,其表达式为:
Pi(k)=[I-Ki(k)Hi(k)][Φi(k,k-1)Pi(k)Φi(k,k-1)T+Γi(k,k-1)QiΓi(k,k-1)T]
Ki(k)=Pi(k)Hi(k)[Hi(k)Pi(k)Hi(k)+Ri(k)]-1
其中,为k-1时刻惯导误差状态量估计值,Φi(k,k-1)为tk-1时刻至tk时刻***的状态转移矩阵,Zi(k)为tk时刻无人机i惯导输出的飞行器速度矢量,Hi(k)为tk时刻无人机i的惯性/卫星组合量测矩阵,Γi(k,k-1)为tk-1时刻至tk时刻***的噪声驱动矩阵,Ri(k)为tk时刻无人机i的卫星导航接收机噪声的协方差矩阵;
步骤44,利用步骤43估计的惯导误差进行惯性导航修正;
步骤45,输出惯性导航***指示的经度λi、纬度Li、高度hi作为无人机i的独立导航解算结果。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711371353.2A CN108151737B (zh) | 2017-12-19 | 2017-12-19 | 一种动态互观测关系条件下的无人机蜂群协同导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711371353.2A CN108151737B (zh) | 2017-12-19 | 2017-12-19 | 一种动态互观测关系条件下的无人机蜂群协同导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108151737A CN108151737A (zh) | 2018-06-12 |
CN108151737B true CN108151737B (zh) | 2021-08-10 |
Family
ID=62467621
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711371353.2A Active CN108151737B (zh) | 2017-12-19 | 2017-12-19 | 一种动态互观测关系条件下的无人机蜂群协同导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108151737B (zh) |
Families Citing this family (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109813311B (zh) * | 2019-03-18 | 2020-09-15 | 南京航空航天大学 | 一种无人机编队协同导航方法 |
CN110146108B (zh) * | 2019-05-10 | 2020-11-03 | 南京航空航天大学 | 一种用于无人机蜂群协同导航的故障在线评估方法 |
CN110285800B (zh) * | 2019-06-10 | 2022-08-09 | 中南大学 | 一种飞行器集群的协同相对定位方法及*** |
CN110243377B (zh) * | 2019-07-19 | 2022-09-30 | 南京航空航天大学 | 一种基于分层式结构的集群飞行器协同导航方法 |
CN110426029B (zh) * | 2019-07-31 | 2022-03-25 | 南京航空航天大学 | 用于无人机蜂群协同导航的动态互观测在线建模方法 |
CN111473784B (zh) * | 2020-04-16 | 2023-06-20 | 南京航空航天大学 | 基于分布节点信息区块的无人机集群协同导航***及方法 |
CN112985391B (zh) * | 2021-04-19 | 2021-08-10 | 中国人民解放军国防科技大学 | 一种基于惯性和双目视觉的多无人机协同导航方法和装置 |
CN113376664B (zh) * | 2021-05-25 | 2022-07-26 | 南京航空航天大学 | 一种无人蜂群协同导航多故障检测方法 |
CN113916261B (zh) * | 2021-10-09 | 2023-06-27 | 上海交通大学 | 基于惯性导航优化对准的姿态误差评估方法 |
CN114608578B (zh) * | 2022-03-07 | 2024-04-09 | 北京航空航天大学 | 一种加权不确定性无人机集群协同导航方法 |
CN116358564B (zh) * | 2023-06-01 | 2023-07-28 | 中国人民解放军战略支援部队航天工程大学 | 无人机蜂群质心运动状态跟踪方法、***、设备及介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102628690A (zh) * | 2012-04-19 | 2012-08-08 | 清华大学 | 两架无人机任务协同可视导航方法 |
CN102707693A (zh) * | 2012-06-05 | 2012-10-03 | 清华大学 | 一种时空联合的多架无人机协同控制***的构建方法 |
CN103076808A (zh) * | 2012-12-27 | 2013-05-01 | 清华大学 | 一种自主协同的航天器集群体系及运行方法 |
CN106441300A (zh) * | 2016-09-08 | 2017-02-22 | 哈尔滨工程大学 | 一种具有自适应的协同导航滤波方法 |
CN107238384A (zh) * | 2017-05-26 | 2017-10-10 | 高武保 | 一种基于多模式协同的军民两用智能导航*** |
-
2017
- 2017-12-19 CN CN201711371353.2A patent/CN108151737B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102628690A (zh) * | 2012-04-19 | 2012-08-08 | 清华大学 | 两架无人机任务协同可视导航方法 |
CN102707693A (zh) * | 2012-06-05 | 2012-10-03 | 清华大学 | 一种时空联合的多架无人机协同控制***的构建方法 |
CN103076808A (zh) * | 2012-12-27 | 2013-05-01 | 清华大学 | 一种自主协同的航天器集群体系及运行方法 |
CN106441300A (zh) * | 2016-09-08 | 2017-02-22 | 哈尔滨工程大学 | 一种具有自适应的协同导航滤波方法 |
CN107238384A (zh) * | 2017-05-26 | 2017-10-10 | 高武保 | 一种基于多模式协同的军民两用智能导航*** |
Non-Patent Citations (1)
Title |
---|
多AUV 协同导航问题的研究现状与进展;徐博等;《自动化学报》;20150331;第41卷(第3期);第2.1节 * |
Also Published As
Publication number | Publication date |
---|---|
CN108151737A (zh) | 2018-06-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108151737B (zh) | 一种动态互观测关系条件下的无人机蜂群协同导航方法 | |
CN104181572B (zh) | 一种弹载惯性/卫星紧组合导航方法 | |
CN106871928B (zh) | 基于李群滤波的捷联惯性导航初始对准方法 | |
CN105043415B (zh) | 基于四元数模型的惯性系自对准方法 | |
CN102353378B (zh) | 一种矢量形式信息分配系数的组合导航***自适应联邦滤波方法 | |
CN112097763A (zh) | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 | |
CN103837151B (zh) | 一种四旋翼飞行器的气动模型辅助导航方法 | |
CN106979781B (zh) | 基于分布式惯性网络的高精度传递对准方法 | |
CN110243377B (zh) | 一种基于分层式结构的集群飞行器协同导航方法 | |
CN105021183A (zh) | 多旋翼飞行器gps和ins低成本组合导航*** | |
CN113295162B (zh) | 基于无人机状态信息的广义因子图融合导航方法 | |
CN110017837A (zh) | 一种姿态抗磁干扰的组合导航方法 | |
CN103363992A (zh) | 基于梯度下降的四旋翼无人机姿态航向参考***解算方法 | |
CN110849360B (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
CN104697520A (zh) | 一体化无陀螺捷联惯导***与gps***组合导航方法 | |
CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
CN108592911A (zh) | 一种四旋翼飞行器动力学模型/机载传感器组合导航方法 | |
Liu et al. | Tightly coupled modeling and reliable fusion strategy for polarization-based attitude and heading reference system | |
CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
Yang et al. | Multilayer low-cost sensor local-global filtering fusion integrated navigation of small UAV | |
CN104406592A (zh) | 一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法 | |
CN111337056B (zh) | 基于优化的LiDAR运动补偿位置姿态***对准方法 | |
CN103616026A (zh) | 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法 | |
CN111060140A (zh) | 一种地球椭球模型下的极区惯性导航误差获得方法 | |
CN114111840B (zh) | 一种基于组合导航的dvl误差参数在线标定方法 |
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 | ||
EE01 | Entry into force of recordation of patent licensing contract |
Application publication date: 20180612 Assignee: AEROSPACE TIMES FEIHONG TECHNOLOGY Co.,Ltd. Assignor: Nanjing University of Aeronautics and Astronautics Contract record no.: X2023980035613 Denomination of invention: A Method for Unmanned Aerial Vehicle Bee Colony Collaborative Navigation under Dynamic Mutual Observation Relations Granted publication date: 20210810 License type: Common License Record date: 20230517 |
|
EE01 | Entry into force of recordation of patent licensing contract |