CN116382330A - 一种目标驱动下的集群无人机协同导航方法 - Google Patents

一种目标驱动下的集群无人机协同导航方法 Download PDF

Info

Publication number
CN116382330A
CN116382330A CN202310262526.6A CN202310262526A CN116382330A CN 116382330 A CN116382330 A CN 116382330A CN 202310262526 A CN202310262526 A CN 202310262526A CN 116382330 A CN116382330 A CN 116382330A
Authority
CN
China
Prior art keywords
node
unmanned aerial
aerial vehicle
slave
navigation
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.)
Pending
Application number
CN202310262526.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.)
Qinhuai Innovation Research Institute Of Nanjing University Of Aeronautics And Astronautics
Nanjing University of Aeronautics and Astronautics
Original Assignee
Qinhuai Innovation Research Institute Of Nanjing University Of Aeronautics And Astronautics
Nanjing University of Aeronautics and Astronautics
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 Qinhuai Innovation Research Institute Of Nanjing University Of Aeronautics And Astronautics, Nanjing University of Aeronautics and Astronautics filed Critical Qinhuai Innovation Research Institute Of Nanjing University Of Aeronautics And Astronautics
Priority to CN202310262526.6A priority Critical patent/CN116382330A/zh
Publication of CN116382330A publication Critical patent/CN116382330A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/12—Simultaneous equations, e.g. systems of linear equations
    • 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
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/004—Artificial life, i.e. computing arrangements simulating life
    • G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N7/00—Computing arrangements based on specific mathematical models
    • G06N7/01—Probabilistic graphical models, e.g. probabilistic networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Algebra (AREA)
  • Remote Sensing (AREA)
  • Databases & Information Systems (AREA)
  • Computing Systems (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Evolutionary Computation (AREA)
  • Automation & Control Theory (AREA)
  • Probability & Statistics with Applications (AREA)
  • Operations Research (AREA)
  • Artificial Intelligence (AREA)
  • Biomedical Technology (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • General Health & Medical Sciences (AREA)
  • Molecular Biology (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Health & Medical Sciences (AREA)
  • Evolutionary Biology (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种目标驱动下的集群无人机协同导航方法，包括以下步骤：获取领机节点的位置信息及测距信息，基于所述位置信息和测距信息，采用最大矢量四面体体积法对从机节点进行空间几何构型；获取所述空间几何构型的GDOP值并进行排序处理，基于最小的GDOP值，采用最小二乘法对所述空间几何构型进行迭代，获得最优空间几何构型；基于所述最优空间几何构型，并结合卡尔曼滤波，对从机节点的位置进行修正，实现集群无人机的协同导航。本发明在集群无人机主从式协同导航的架构下实现了目标驱动下全过程中集群无人机的高精度协同导航。

Description

一种目标驱动下的集群无人机协同导航方法
技术领域
本发明属于协同导航技术领域，特别是涉及一种目标驱动下的集群无人机协同导航方法。
背景技术
集群无人机有着作战能力强、体系生存率高以及攻击成本低等诸多优势。随着无人机种类越来越多，集群无人机的任务领域和任务类型不断拓展，已经逐步涉足到空域对抗、防御、监视、侦察等各个领域。高精度的导航信息是实现无人机集群导航控制的关键信息，而当无人机集群面对高度对抗性、高度不确定性、高度动态性的实际战场环境，制定好科学且清晰的目标驱动策略是多任务目标下无人机集群技术研究中的关键。
当今研究学者针对单一目标驱动下集群无人机的控制问题开展了大量研究工作，如“多无人机协同攻击策略研究”中提出：从多无人机协同空战和多无人机协同对地攻击两个部分进行多无人机协同攻击策略研究，分别建立了不同环境下的作战模型。“异构无人机编队防御及评估策略研究”中提出：将遗传算法应用于无人机集群的防御部署问题中，针对不同规模和不同队形的敌机编队，对我方无人机编队进行优化，并通过绘制多种战况的损失曲线验证其算法有效性。“巡检无人机编队轨迹跟踪控制研究”中提出了一种巡检无人机编队控制巡检方法，针对无人机编队过程中的饱和输入现象，采用领导者-跟随者控制算法进行无人机编队控制，达到有效实现编队控制的目的。
对上述的现有文献分析可知，目前研究学者针对集群无人机单一任务目标场景下的控制问题开展了大量的研究工作，但考虑到复杂的作战环境，并未有文献综合考虑无人机集群接受多任务目标驱动时的情况。此外，集群无人机任务目标执行中编队构型的实时拓扑变化会影响到无人机的导航定位误差，从而影响到集群无人机任务执行的成功率。
因此，亟需提出一种目标驱动下的集群无人机协同导航方法，实现多目标驱动下全过程中集群无人机的高精度协同导航。
发明内容
本发明的目的是提供一种目标驱动下的集群无人机协同导航方法，针对集群无人机接受攻击任务目标、躲避任务目标、巡检任务目标等多目标驱动的情况，采用最小二乘的方法完成集群无人机不同目标驱动下构型的迭代；同时在不考虑无人机间时钟非同步误差的前提下，基于无人机间相对测距方式，通过最小几何精度因子准则实时选择最优空间几何构型中的多架领机对从机节点进行协同定位结算，并在此基础上设计了扩展卡尔曼滤波器以降低无人机执行不同任务目标过程中的定位误差，以解决上述现有技术存在的问题。
为实现上述目的，本发明提供了一种目标驱动下的集群无人机协同导航方法，包括以下步骤：
获取领机节点的位置信息及测距信息，基于所述位置信息和测距信息，采用最大矢量四面体体积法对从机节点进行空间几何构型；
获取所述空间几何构型的GDOP值并进行排序处理，基于最小的GDOP值，采用最小二乘法对所述空间几何构型进行迭代，获得最优空间几何构型；
基于所述最优空间几何构型，并结合卡尔曼滤波，对从机节点的位置进行修正，实现集群无人机的协同导航。
可选的，获取领机节点的位置信息及测距信息的过程包括：基于机载导航系统获取领机的位置信息，并将所述位置信息通过数据链广播给从机节点；所述从机节点接收所述位置信息，并测量与所述领机节点的相对距离，获得测距信息。
可选的，采用最大矢量四面体体积法对从机节点进行空间几何构型的过程包括：获取领机节点与从机节点之间的三维相对距离，对所述三维相对距离进行一阶线性化泰勒展开，进而获得从机节点的协同定位误差方程组；对所述协同定位误差方程组进行最小二乘法的处理，获得误差协方差方程；将领机节点与从机节点的相对测距误差与所述误差协方差方程相结合，获得从机节点与领机节点的空间几何构型。
可选的，获得最优空间几何构型的过程包括：当所述空间几何构型的GDOP值最小时，获取领机节点和从机节点的节点坐标以及各个基站的坐标，进而获得节点坐标到各个基站的距离方程；对所述距离方程进行迭代作差，并用最小二乘法进行求解，获得领机节点对从机节点的协同定位结算，进而获得最优空间几何构型。
可选的，结合卡尔曼滤波，对从机节点的位置进行修正的过程包括：基于所述最优空间几何构型，获得优选的领机节点；基于所述优选的领机节点，构建从机节点的相对测距误差观测模型以及无人机集群状态递推模型，进而利用扩展卡尔曼滤波对从机节点进行时间更新和量测更新，实现对从机节点的位置修正。
可选的，构建从机节点的相对测距误差观测模型的过程包括：构建领机节点与从机节点的伪距方程并进行一阶线性化泰勒展开，进而构造基于相对测距方式下无人机集群的雅可比矩阵，基于所述雅可比矩阵构建从机节点的相对测距误差观测模型。
可选的，构建无人机集群状态递推模型的过程包括：获取无人机搭载的惯性导航系统的导航输出参数误差并建立18维状态量，进而构建系统状态转移矩阵和系统噪声阵，基于所述18维状态量、系统状态转移矩阵和系统噪声阵构建无人机集群状态递推模型。
本发明的技术效果为：
本发明基于迭代最小二乘法构建了集群无人机目标驱动构型变更模型，在此基础上，建立了基于最小几何精度因子准则(Geometric Dilution Of Precision,GDOP)的协同导航节点寻优策略；并基于目标驱动策略下的最优空间构型，通过设计基于相对距离约束的协同导航滤波器，实现集群无人机目标驱动变更下无人机状态的在线估计与补偿，达到提升集群无人机定位精度的目的。本发明在集群无人机主从式协同导航的架构下实现了目标驱动下全过程中集群无人机的高精度协同导航。
附图说明
构成本申请的一部分的附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1为本发明实施例中的集群无人机攻击目标构型示意图；
图2为本发明实施例中的集群无人机躲避目标构型示意图；
图3为本发明实施例中的集群无人机巡检目标构型示意图；
图4为本发明实施例中的集群无人机目标驱动策略流程图；
图5为本发明实施例中的从机节点1的最优空间构型示意图；
图6为本发明实施例中的基于目标驱动下集群无人机协同导航算法设计流程图；
图7为本发明实施例中的从机节点1目标驱动任务下与协同节点实时结算的GDOP值；
图8为本发明实施例中的从机节点1单一构型和最优空间构型下三维位置误差曲线示意图。
具体实施方式
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
实施例一
无人机集群中攻击、躲避、巡检等行为与生物集群往往表现出高度的一致。狼群捕猎时的分工合作行为，鱼群遇险时的群体逃脱行为，再到蚂蚁觅食时的信息传递和搜索行为，生物集群中的每个个体都具有自主决策和信息交流的能力，经过不断地演化，最终在宏观层面上使集群表现出合作性、稳定性以及对复杂环境的适应能力，这与无人机集群协同目标跟踪的分布式、协同性等要求有着紧密的契合之处。因此，本实施例结合不同生物群落协同方式的优点，将其与集群无人机不同目标驱动下的行为形成映射关系并开展相关研究工作。
通过对狼群捕食策略，鱼群躲避策略，蚁群觅食策略的研究总结得出，生物集群策略的实施格外依赖领导者的决策以及生物群落合理的编队构型。基于此研究，本实施例在开展基于目标驱动下集群无人机协同导航研究时，将研究重点落脚于集群无人机主从式协同导航架构下，无人机能否快速且精确的生成相应目标驱动构型，且集群无人机生成构型过程中能否同时保证高精度的导航定位。
集群无人机目标驱动策略的研究采用传统主从式协同导航架构，其中领机节点搭载MEMS(Micro-Electro-Mechanical Systems)惯性测量单元、卫星接收机及数据链通信装置，从机节点搭载MEMS惯性测量单元和数据链通信装置。如图1所示，集群无人机攻击编队的构型采用规则的空间三角形构型；如图2所示，集群无人机躲避构型采用规则球体构型；如图3所示，集群无人机巡检构型采用空间矢量直线。其中，图1-3中箭头方向表示集群无人机的飞行方向。
本实施例基于目标驱动下集群无人机协同导航方面的研究将从提升集群无人机构型迭代过程中定位精度的角度来开展。无人机编队执行目标任务过程中会面临编队构型实时拓扑变化，而构型优选是充分发挥无人机集群优势的关键技术。本实施例在集群无人机目标驱动下的协同导航研究中采用最大失端四面体体积法，借鉴卫星导航的GDOP值的概念，当无人机集群中4架领机与从机节点构成的空间矢量四面体体积最大，即精度因子GDOP最小时，得到领机最佳几何配置，可有效降低从机定位误差。因此，本实施例基于最小GDOP准则，对集群无人机构型最小二乘迭代过程中领机节点进行实时构型优选，同时设计了无人机集群协同导航滤波器，实现集群无人机的协同导航。
如图4所示，本实施例提供一种目标驱动下的集群无人机协同导航方法，首先获取领机节点的位置信息及测距信息，基于所述位置信息和测距信息，采用最大矢量四面体体积法对从机节点进行空间几何构型；获取所述空间几何构型的GDOP值并进行排序处理，基于最小的GDOP值，采用最小二乘法对所述空间几何构型进行迭代，获得最优空间几何构型；基于所述最优空间几何构型，并结合卡尔曼滤波，对从机节点的位置进行修正，实现集群无人机的协同导航。
获取目标驱动下集群无人机的最优空间几何构型：
假设无人机集群协同网络中N架为领机节点，飞行在卫星信号覆盖区域内，k架为待协同定位解算的从机节点。领机通过机载导航系统进行自身导航结果解算，同时将自身位置信息通过数据链广播给从机节点。从机节点实时接收领机节点的位置信息，并基于TOA(time of arrival)的方式实现与领机节点的相对距离测量。根据领机的位置信息及测距信息，采用最大矢量四面体体积法，同时实现从机节点空间构型的实时优选。无人机集群协同网络中从机节点1空间构型四面体如图5所示。
领机与从机节点之间的三维相对距离计算值可表示为：
Figure BDA0004131778900000071
其中，(xsi,ysi,zsi)为领机节点的三维位置，(x,y,z)为从机节点的三维位置。将(1)式进行一阶线性化泰勒展开可得在N架领机节点的情况下，从机节点协同定位误差方程组为：
R-P＝Gu·Xu (2)
其中，R＝(r1 r2 … rn)为从机节点到领机的空间几何距离；P＝(ρ1 ρ2 ... ρn)为从机节点到领机节点的三维相对距离测量值；Gu为从机节点到领机节点的方向余弦矩阵，可表示为：
Figure BDA0004131778900000072
其中，Xu＝[δx δy δz]T，(δx,δy,δz)为从机节点机载惯导系统位置误差。
通过(2)式采用最小二乘法可得：
Figure BDA0004131778900000073
误差协方差方程可表示为：
Figure BDA0004131778900000081
假设从机节点与各领机相对测距误差为白噪声模型，且相互独立：
cov(δ(R-P))＝σ2I (6)
将(6)式带入(5)式可得：
Figure BDA0004131778900000082
令上式中
Figure BDA0004131778900000083
则从机与领机节点空间几何构型GDOP误差系数可表示为：
Figure BDA0004131778900000084
为了实时获取与从机节点最优空间几何构型的四架领机，需实时计算当前时刻所有空间构型的GDOP(几何精度因子)值。采用当前时刻GDOP值最小的空间几何构型作为优选的领机节点结果：
GDOPm＝min(GDOP) (10)
基于目标驱动策略下集群无人机构型迭代过程：
记无人机节点位置坐标为(x,y,z)，求解协同网络中两两无人机节之间的距离dij(i∈(1,N-1),j∈(i+1,N))：
Figure BDA0004131778900000085
为便于迭代最小二乘的进行，本实施例给定集群无人机目标构型迭代完成后无人机节点参考目标点的三维坐标，记为
Figure BDA0004131778900000086
同时计算目标构型迭代后各无人机节点与其他节点的距离，记为/>
Figure BDA0004131778900000091
将集群无人机构型迭代前后，无人机节点两两之间的距离计算值{d12,...,dij,...,dN-1N}和
Figure BDA0004131778900000092
一一对应的带入式(12)进行迭代残差计算：
Figure BDA0004131778900000093
Figure BDA0004131778900000094
若迭代残差满足式(13)所设定的最小残差阈值
Figure BDA0004131778900000096
即证明集群无人机已完成相应目标驱动下的构型迭代。若迭代残差不满足式(13)的阈值，则初始构型下的集群无人机三维坐标值添加位置迭代步长，重复式(11)-式(13)的过程，直至满足所设定的迭代最小残差阈值/>
Figure BDA0004131778900000097
即完成目标驱动任务，同时记录目标驱动下集群无人机迭代航迹用以后续滤波结算。
基于目标驱动策略下集群无人机协同导航滤波器设计：
基于目标驱动下集群无人机协同导航算法设计流程图如图6所示。首先，在协同网络时钟同步的基础上，从机节点通过机载数据链通信系统实时识别不同领机节点的设备ID并完成相对测距以及导航信息互享，机载导航计算机结合自身惯导信息以及相对测距信息完成空间坐标系转换。其次，根据本实施例提出的空间构型优选方法实时选择出不同时刻与从机节点构成最优空间构型的四架领机节点，在此基础上设计扩展卡尔曼滤波器，最终实现系统状态量的在线估计与实时补偿，达到提高从机节点定位精度的目的。
状态方程模型：
基于空间构型优选的无人机集群协同定位算法系统状态量为：
Figure BDA0004131778900000095
Figure BDA0004131778900000101
其中，
Figure BDA0004131778900000102
为从机节点机载惯导系统平台误差角；δvE,δvN,δvU为从机节点机载惯导系统速度误差；δL,δλ,δh为从机节点机载惯导系统位置误差；εbx,εby,εbz为三轴陀螺随机常数；εrx,εry,εrz为三轴陀螺一阶马尔科夫过程随机噪声；▽x,▽y,▽z为三轴加速度计一阶马尔科夫过程随机噪声。
通过从机节点惯导系统误差微分方程，构建协同定位算法状态递推方程为:
Figure BDA0004131778900000103
其中F(t)为系统状态转移矩阵，G(t)为系统噪声矩阵；W(t)为陀螺和加速度计白噪声。
Figure BDA0004131778900000104
FN为平台误差角、速度误差、位置误差对应的系统矩阵：
Figure BDA0004131778900000105
Figure BDA0004131778900000106
其中，
Figure BDA0004131778900000107
为机体坐标系到导航坐标系的姿态矩阵，Tgx,Tgy,Tgz为三轴陀螺仪相关时间常数，Tax,Tay,Taz为三轴加速度计相关时间常数。
观测方程模型：
从机节点与最优空间几何构型下领机节点三维相对距离计算值可表示为：
Figure BDA0004131778900000108
其中，(xs,ys,zs)为从机节点机载惯导系统解算的位置，(xmi,ymi,zmi)为领机节点三维位置。将式(19)一阶泰勒展开并忽略高次项可得：
Figure BDA0004131778900000111
无人机节点之间相对距离计算值在x,y,z三轴的方向余弦可表示为：
Figure BDA0004131778900000112
Figure BDA0004131778900000113
Figure BDA0004131778900000114
Figure BDA0004131778900000115
从机节点通过数据链通讯系统获得最优空间构型下，领机与从机节点相对距离测量值可表示为：
ρDj＝rj-vρj (25)
其中，rj为领机与从机节点相对距离真值，vρj为机载数据链系统相对测距误差白噪声。
将式(20)与式(25)作差可得：
δρi＝ρsmi-ρDj＝ei1δx+ei2δy+ei3δz+vρj (26)
取i＝1,2,3，则数据链通讯系统下相对测距观测方程可表示为：
Figure BDA0004131778900000116
本实施例选取导航坐标系为东、北、天地理坐标系，因此需要将地心地固直角(Earth-Centered Earth Fixed,ECEF)坐标系下惯导位置误差转换到地理坐标系下，两坐标系的空间转换关系数学表达式为：
Figure BDA0004131778900000121
其中，δL,δλ,δh表示从机节点的纬度、经度、高度位置误差；L,λ,h为解算的从机节点纬度、经度、高度值。RN＝Re(1+f(sinL)2),Re＝6378137m，f＝1/298.257。
将式(28)带入式(27)可得基于目标驱动下集群无人机系统观测方程：
Zρ＝δρ＝HρXi+Vρ (29)
其中，Hρ＝[03×6 Hρ1 03×9]，
Figure BDA0004131778900000122
Figure BDA0004131778900000123
卡尔曼滤波器设计：
经状态方程及观测方程建立后卡尔曼滤波进行时间更新和量测更新的过程如下：
Figure BDA0004131778900000124
Figure BDA0004131778900000125
Figure BDA0004131778900000126
Figure BDA0004131778900000127
Figure BDA0004131778900000128
上式中，
Figure BDA0004131778900000129
为t-1到t时刻的估计状态量，Φt,t-1为t-1到t时刻的状态转移矩阵，
Figure BDA00041317789000001210
为上一步的估计状态量，/>
Figure BDA00041317789000001211
为本步的估计状态量，Kt为系统t时刻的滤波增益，Zt为t时刻的量测量，其值根据公式(29)中的Hρ实时变化，Ht为系统t时刻的量测矩阵，其值根据公式(29)中的Zρ实时变化，Pt/t-1为/>
Figure BDA0004131778900000131
对应的均方误差，Rt为系统t时刻的量测噪声矩阵，Pt-1为上一步的均方误差，Γt,t-1为t-1到t时刻的系统输入矩阵，Pt/t为/>
Figure BDA0004131778900000132
对应的均方误差，I为单位阵，上标T表示转置。
最终根据建立的卡尔曼滤波时间更新和量测更新方程开展协同导航算法仿真分析部分。
仿真分析
为了验证算法的可行性及有效性，本实施例通过仿真对算法进行了验证分析。仿真中共设置5架领机和3架从机。所有无人机节点通过迭代最小二乘法拟合出在连续接受不同目标驱动下的航迹以用于后续的滤波解算程序。下文仿真中用以对比的传统单一构型指待定位的1架从机节点与协同网络中4架领机组成的空间构型。须注意的是单一构型中用以协同解算的4架领机节点一旦选择，在整个从机节点协同定位中则不再更换。4架领机节点的选择标准保证初始航迹方向互相不同且有别于从机节点即可。
本实施例进行算法验证的PC(personal computer)配备AMD Ryzen 55600GwithRadeon Graphics，3.90GHzCPU以及16GB机带RAM，算法各项调试在MATLAB2019b中进行。
仿真时的机载惯导系统采样频率为50HZ，领机机载卫星接收机及数据链采样频率为1HZ。仿真的机载传感器性能参数设置如表1所示。整个仿真时长为345s。
表1
Figure BDA0004131778900000141
本实施例同时给出仿真中部分领机与从机节点1的初始飞行参数。相关参数设置由表2给出，其余领机与从机节点的初始参数设置与表2相关参数设置模式类似。
表2
Figure BDA0004131778900000142
表2中无人机节点位置参数按照经度、纬度、高度的顺序依次设置；无人机节点姿态角参数按照横滚角、俯仰角、航向角的顺序依次设置。
表3为单一构型与最优空间几何构型下从机节点三维位置误差均方根(RMSE)：
表3
Figure BDA0004131778900000151
由表3数据可知，相比与传统的单一构型方法，提出的方法可以显著提高从机节点定位精度，从机节点位置精度提升约28％，进而验证了本实施例提出方法的有效性。
为了验证本实施例提出的从机节点实时空间构型优选策略，以从机节点1为例，图7给出了从机节点1与领机实时构成最优空间几何构型对应的GDOP值。
为了更加直观地对比分析本实施例提出算法的性能。图8给出了采用单一构型以及本实施例提出方法得到的从机节点1位置误差曲线。由图8可知，本实施例提出的方法考虑了从机节点与协同节点的空间构型，从机节点能够获得相对于单一构型更好的定位精度。为了定量对比分析本实施例提出的算法相对于传统单一构型协同定位算法性能，表3给出了采用两种算法下从机节点1三维位置误差均方根(Root Mean Squared Error,RMSE)。
本实例有效实现了集群无人机接受多目标驱动时的复杂控制。同时能够结合复杂飞行环境下集群无人机的动态拓扑结构实现对从机节点位置的准确估计。
以上所述，仅为本申请较佳的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应该以权利要求的保护范围为准。

Claims (7)

1.一种目标驱动下的集群无人机协同导航方法，其特征在于，包括以下步骤：
获取领机节点的位置信息及测距信息，基于所述位置信息和测距信息，采用最大矢量四面体体积法对从机节点进行空间几何构型；
获取所述空间几何构型的GDOP值并进行排序处理，基于最小的GDOP值，采用最小二乘法对所述空间几何构型进行迭代，获得最优空间几何构型；
基于所述最优空间几何构型，并结合卡尔曼滤波，对从机节点的位置进行修正，实现集群无人机的协同导航。
2.根据权利要求1所述的目标驱动下的集群无人机协同导航方法，其特征在于，
获取领机节点的位置信息及测距信息的过程包括：基于机载导航系统获取领机的位置信息，并将所述位置信息通过数据链广播给从机节点；所述从机节点接收所述位置信息，并测量与所述领机节点的相对距离，获得测距信息。
3.根据权利要求1所述的目标驱动下的集群无人机协同导航方法，其特征在于，
采用最大矢量四面体体积法对从机节点进行空间几何构型的过程包括：获取领机节点与从机节点之间的三维相对距离，对所述三维相对距离进行一阶线性化泰勒展开，进而获得从机节点的协同定位误差方程组；对所述协同定位误差方程组进行最小二乘法的处理，获得误差协方差方程；将领机节点与从机节点的相对测距误差与所述误差协方差方程相结合，获得从机节点与领机节点的空间几何构型。
4.根据权利要求3所述的目标驱动下的集群无人机协同导航方法，其特征在于，
获得最优空间几何构型的过程包括：当所述空间几何构型的GDOP值最小时，获取领机节点和从机节点的节点坐标以及各个基站的坐标，进而获得节点坐标到各个基站的距离方程；对所述距离方程进行迭代作差，并用最小二乘法进行求解，获得领机节点对从机节点的协同定位结算，进而获得最优空间几何构型。
5.根据权利要求1所述的目标驱动下的集群无人机协同导航方法，其特征在于，
结合卡尔曼滤波，对从机节点的位置进行修正的过程包括：基于所述最优空间几何构型，获得优选的领机节点；基于所述优选的领机节点，构建从机节点的相对测距误差观测模型以及无人机集群状态递推模型，进而利用扩展卡尔曼滤波对从机节点进行时间更新和量测更新，实现对从机节点的位置修正。
6.根据权利要求5所述的目标驱动下的集群无人机协同导航方法，其特征在于，
构建从机节点的相对测距误差观测模型的过程包括：构建领机节点与从机节点的伪距方程并进行一阶线性化泰勒展开，进而构造基于相对测距方式下无人机集群的雅可比矩阵，基于所述雅可比矩阵构建从机节点的相对测距误差观测模型。
7.根据权利要求5所述的目标驱动下的集群无人机协同导航方法，其特征在于，
构建无人机集群状态递推模型的过程包括：获取无人机搭载的惯性导航系统的导航输出参数误差并建立18维状态量，进而构建系统状态转移矩阵和系统噪声阵，基于所述18维状态量、系统状态转移矩阵和系统噪声阵构建无人机集群状态递推模型。
CN202310262526.6A 2023-03-17 2023-03-17 一种目标驱动下的集群无人机协同导航方法 Pending CN116382330A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310262526.6A CN116382330A (zh) 2023-03-17 2023-03-17 一种目标驱动下的集群无人机协同导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310262526.6A CN116382330A (zh) 2023-03-17 2023-03-17 一种目标驱动下的集群无人机协同导航方法

Publications (1)

Publication Number Publication Date
CN116382330A true CN116382330A (zh) 2023-07-04

Family

ID=86968547

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310262526.6A Pending CN116382330A (zh) 2023-03-17 2023-03-17 一种目标驱动下的集群无人机协同导航方法

Country Status (1)

Country Link
CN (1) CN116382330A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117852316A (zh) * 2024-03-07 2024-04-09 中国人民解放军军事科学院国防科技创新研究院 通讯约束下集群协同导航仿真测试系统及方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117852316A (zh) * 2024-03-07 2024-04-09 中国人民解放军军事科学院国防科技创新研究院 通讯约束下集群协同导航仿真测试系统及方法
CN117852316B (zh) * 2024-03-07 2024-05-28 中国人民解放军军事科学院国防科技创新研究院 通讯约束下集群协同导航仿真测试系统及方法

Similar Documents

Publication Publication Date Title
Redding et al. Vision-based target localization from a fixed-wing miniature air vehicle
Mu et al. A practical INS/GPS/DVL/PS integrated navigation algorithm and its application on Autonomous Underwater Vehicle
Kim et al. Autonomous airborne navigation in unknown terrain environments
EP3158293B1 (en) Sensor fusion using inertial and image sensors
Caballero et al. Vision-based odometry and SLAM for medium and high altitude flying UAVs
CN105022401B (zh) 基于视觉的多四旋翼无人机协同slam方法
Campbell et al. Cooperative tracking using vision measurements on seascan UAVs
CN112347840A (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
US9746330B2 (en) System and method for localizing two or more moving nodes
Sabatini et al. Low-cost sensors data fusion for small size unmanned aerial vehicles navigation and guidance
Xu et al. Target location of unmanned aerial vehicles based on the electro-optical stabilization and tracking platform
Cappello et al. A low-cost and high performance navigation system for small RPAS applications
CN107270933B (zh) 一种基于多星协同的空间碎片运动状态联合确定方法
CN111044075B (zh) 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法
CN111238469B (zh) 一种基于惯性/数据链的无人机编队相对导航方法
CN110955261A (zh) 一种基于ros的固定翼无人机自动驾驶仪的仿真方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN110440830A (zh) 动基座下车载捷联惯导系统自对准方法
CN116382330A (zh) 一种目标驱动下的集群无人机协同导航方法
Campbell et al. A vision based geolocation tracking system for uav's
CN110779519A (zh) 一种具有全局收敛性的水下航行器单信标定位方法
Goppert et al. Invariant Kalman filter application to optical flow based visual odometry for UAVs
Whitacre et al. Decentralized geolocation and bias estimation for uninhabited aerial vehicles with articulating cameras
CN115560757B (zh) 随机姿态误差条件下基于神经网络的无人机直接定位校正方法
Lee et al. Analysis on observability and performance of INS-range integrated navigation system under urban flight environment

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