CN108151737A - 一种动态互观测关系条件下的无人机蜂群协同导航方法 - Google Patents
一种动态互观测关系条件下的无人机蜂群协同导航方法 Download PDFInfo
- Publication number
- CN108151737A CN108151737A CN201711371353.2A CN201711371353A CN108151737A CN 108151737 A CN108151737 A CN 108151737A CN 201711371353 A CN201711371353 A CN 201711371353A CN 108151737 A CN108151737 A CN 108151737A
- Authority
- CN
- China
- Prior art keywords
- unmanned plane
- navigation
- error
- state
- bee colony
- 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.)
- Granted
Links
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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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
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
其中,为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
步骤57,利用步骤56得到的惯导误差状态量估计值进行惯性导航修正,并令Pi(k)=Pij(k);
步骤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
其中,为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
步骤57,利用步骤56得到的惯导误差状态量估计值进行惯性导航修正,并令Pi(k)=Pij(k);
步骤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 (5)
1.一种动态互观测关系条件下的无人机蜂群协同导航方法,其特征在于包括如下步骤:
步骤1,初始化,对无人机蜂群中的无人机按照1~n进行编号;
步骤2,选取无人机蜂群导航状态量,建立导航状态方程和独立导航量测方程;
步骤3,建立协同导航量测方程;
步骤4,无人机蜂群中的无人机分别进行独立导航解算;
步骤5,无人机蜂群中的无人机分别进行协同导航解算;
步骤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-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.如权利要求1所述的一种动态互观测关系条件下的无人机蜂群协同导航方法,其特征在于:所述步骤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.如权利要求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时刻惯导误差状态量估计值;
步骤44,利用步骤43估计的惯导误差进行惯性导航修正;
步骤45,输出惯性导航系统指示的经度λi、纬度Li、高度hi作为无人机i的独立导航解算结果。
5.如权利要求1所述的一种动态互观测关系条件下的无人机蜂群协同导航方法,其特征在于:所述步骤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
步骤57,利用步骤56得到的惯导误差状态量估计值进行惯性导航修正,并令Pi(k)=Pij(k);
步骤58:判断是否j<n,如果是则令j=j+1,并执行步骤52,否则执行步骤59;
步骤59,输出惯性导航系统指示的经度λ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 true CN108151737A (zh) | 2018-06-12 |
CN108151737B 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) |
Cited By (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109813311A (zh) * | 2019-03-18 | 2019-05-28 | 南京航空航天大学 | 一种无人机编队协同导航方法 |
CN110146108A (zh) * | 2019-05-10 | 2019-08-20 | 南京航空航天大学 | 一种用于无人机蜂群协同导航的故障在线评估方法 |
CN110243377A (zh) * | 2019-07-19 | 2019-09-17 | 南京航空航天大学 | 一种基于分层式结构的集群飞行器协同导航方法 |
CN110285800A (zh) * | 2019-06-10 | 2019-09-27 | 中南大学 | 一种飞行器集群的协同相对定位方法及系统 |
CN110426029A (zh) * | 2019-07-31 | 2019-11-08 | 南京航空航天大学 | 用于无人机蜂群协同导航的动态互观测在线建模方法 |
CN111473784A (zh) * | 2020-04-16 | 2020-07-31 | 南京航空航天大学 | 基于分布节点信息区块的无人机集群协同导航系统及方法 |
CN112985391A (zh) * | 2021-04-19 | 2021-06-18 | 中国人民解放军国防科技大学 | 一种基于惯性和双目视觉的多无人机协同导航方法和装置 |
CN113376664A (zh) * | 2021-05-25 | 2021-09-10 | 南京航空航天大学 | 一种无人蜂群协同导航多故障检测方法 |
CN113916261A (zh) * | 2021-10-09 | 2022-01-11 | 上海交通大学 | 基于惯性导航优化对准的姿态误差评估方法 |
CN114608578A (zh) * | 2022-03-07 | 2022-06-10 | 北京航空航天大学 | 一种加权不确定性无人机集群协同导航方法 |
CN116358564A (zh) * | 2023-06-01 | 2023-06-30 | 中国人民解放军战略支援部队航天工程大学 | 无人机蜂群质心运动状态跟踪方法、系统、设备及介质 |
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 协同导航问题的研究现状与进展", 《自动化学报》 * |
Cited By (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109813311A (zh) * | 2019-03-18 | 2019-05-28 | 南京航空航天大学 | 一种无人机编队协同导航方法 |
CN110146108A (zh) * | 2019-05-10 | 2019-08-20 | 南京航空航天大学 | 一种用于无人机蜂群协同导航的故障在线评估方法 |
CN110146108B (zh) * | 2019-05-10 | 2020-11-03 | 南京航空航天大学 | 一种用于无人机蜂群协同导航的故障在线评估方法 |
CN110285800A (zh) * | 2019-06-10 | 2019-09-27 | 中南大学 | 一种飞行器集群的协同相对定位方法及系统 |
CN110285800B (zh) * | 2019-06-10 | 2022-08-09 | 中南大学 | 一种飞行器集群的协同相对定位方法及系统 |
CN110243377A (zh) * | 2019-07-19 | 2019-09-17 | 南京航空航天大学 | 一种基于分层式结构的集群飞行器协同导航方法 |
CN110426029B (zh) * | 2019-07-31 | 2022-03-25 | 南京航空航天大学 | 用于无人机蜂群协同导航的动态互观测在线建模方法 |
CN110426029A (zh) * | 2019-07-31 | 2019-11-08 | 南京航空航天大学 | 用于无人机蜂群协同导航的动态互观测在线建模方法 |
WO2021018113A1 (zh) * | 2019-07-31 | 2021-02-04 | 南京航空航天大学 | 用于无人机蜂群协同导航的动态互观测在线建模方法 |
CN111473784A (zh) * | 2020-04-16 | 2020-07-31 | 南京航空航天大学 | 基于分布节点信息区块的无人机集群协同导航系统及方法 |
CN111473784B (zh) * | 2020-04-16 | 2023-06-20 | 南京航空航天大学 | 基于分布节点信息区块的无人机集群协同导航系统及方法 |
CN112985391A (zh) * | 2021-04-19 | 2021-06-18 | 中国人民解放军国防科技大学 | 一种基于惯性和双目视觉的多无人机协同导航方法和装置 |
CN113376664A (zh) * | 2021-05-25 | 2021-09-10 | 南京航空航天大学 | 一种无人蜂群协同导航多故障检测方法 |
CN113916261A (zh) * | 2021-10-09 | 2022-01-11 | 上海交通大学 | 基于惯性导航优化对准的姿态误差评估方法 |
CN113916261B (zh) * | 2021-10-09 | 2023-06-27 | 上海交通大学 | 基于惯性导航优化对准的姿态误差评估方法 |
CN114608578A (zh) * | 2022-03-07 | 2022-06-10 | 北京航空航天大学 | 一种加权不确定性无人机集群协同导航方法 |
CN114608578B (zh) * | 2022-03-07 | 2024-04-09 | 北京航空航天大学 | 一种加权不确定性无人机集群协同导航方法 |
CN116358564A (zh) * | 2023-06-01 | 2023-06-30 | 中国人民解放军战略支援部队航天工程大学 | 无人机蜂群质心运动状态跟踪方法、系统、设备及介质 |
CN116358564B (zh) * | 2023-06-01 | 2023-07-28 | 中国人民解放军战略支援部队航天工程大学 | 无人机蜂群质心运动状态跟踪方法、系统、设备及介质 |
Also Published As
Publication number | Publication date |
---|---|
CN108151737B (zh) | 2021-08-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108151737A (zh) | 一种动态互观测关系条件下的无人机蜂群协同导航方法 | |
CN109556632B (zh) | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 | |
Groves | Principles of GNSS, inertial, and multisensor integrated navigation systems, [Book review] | |
CN104729506B (zh) | 一种视觉信息辅助的无人机自主导航定位方法 | |
CN103697889B (zh) | 一种基于多模型分布式滤波的无人机自主导航与定位方法 | |
CN105606094B (zh) | 一种基于mems/gps组合系统的信息条件匹配滤波估计方法 | |
CN101793523B (zh) | 一种组合导航和光电探测一体化系统 | |
CN109813311A (zh) | 一种无人机编队协同导航方法 | |
CN110501024A (zh) | 一种车载ins/激光雷达组合导航系统的量测误差补偿方法 | |
CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
CN108871336A (zh) | 一种车辆位置估算系统及方法 | |
CN103674021A (zh) | 基于捷联惯导与星敏感器的组合导航系统及方法 | |
CN104697520B (zh) | 一体化无陀螺捷联惯导系统与gps系统组合导航方法 | |
Zhao | GPS/IMU integrated system for land vehicle navigation based on MEMS | |
CN102353377A (zh) | 一种高空长航时无人机组合导航系统及其导航定位方法 | |
CN109032153A (zh) | 基于光电-惯性组合导引的无人机自主着舰方法和系统 | |
Huang et al. | Attitude estimation fusing quasi-Newton and cubature Kalman filtering for inertial navigation system aided with magnetic sensors | |
Yun et al. | IMU/Vision/Lidar integrated navigation system in GNSS denied environments | |
CN105910623B (zh) | 利用磁强计辅助gnss/mins紧组合系统进行航向校正的方法 | |
CN108731674A (zh) | 一种基于单轴旋转调制的惯性天文组合导航系统及计算方法 | |
CN113503892A (zh) | 一种基于里程计和回溯导航的惯导系统动基座初始对准方法 | |
CN104501809B (zh) | 一种基于姿态耦合的捷联惯导/星敏感器组合导航方法 | |
CN108151765A (zh) | 一种在线实时估计补偿磁强计误差的定位测姿方法 | |
CN112880669A (zh) | 一种航天器星光折射和单轴旋转调制惯性组合导航方法 | |
CN104406592A (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 | ||
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 |