CN111895995B - 基于pso的飞行器编队多维地磁匹配导航方法及系统 - Google Patents

基于pso的飞行器编队多维地磁匹配导航方法及系统 Download PDF

Info

Publication number
CN111895995B
CN111895995B CN202010493550.7A CN202010493550A CN111895995B CN 111895995 B CN111895995 B CN 111895995B CN 202010493550 A CN202010493550 A CN 202010493550A CN 111895995 B CN111895995 B CN 111895995B
Authority
CN
China
Prior art keywords
aircraft
navigation system
geomagnetic
inertial navigation
value
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
Application number
CN202010493550.7A
Other languages
English (en)
Other versions
CN111895995A (zh
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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN202010493550.7A priority Critical patent/CN111895995B/zh
Publication of CN111895995A publication Critical patent/CN111895995A/zh
Application granted granted Critical
Publication of CN111895995B publication Critical patent/CN111895995B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • Manufacturing & Machinery (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于PSO的飞行器编队多维地磁匹配导航方法及系统，飞行器编队包括一个主飞行器和多个从飞行器，主飞行器上设置有惯性导航系统，所有的飞行器上安装有地磁探测单元，每个飞行器相对于主飞行器的飞行坐标固定；其中导航方法包括：1、在地磁匹配时间段内分别多次采集每个飞行器上地磁探测单元的地磁测量值以及同时刻的惯性导航系统指示坐标；计算每个飞行器与地磁测量值同时刻的惯导推算坐标；3、采用PSO获取惯性导航系统的最佳修正参数；3、用所述最佳修正参数对惯性导航系统的指示坐标进行修正。该方法能够对惯性导航系统的指示位置坐标进行修改，以校正惯性导航系统的误差，且没有累积误差。

Description

基于PSO的飞行器编队多维地磁匹配导航方法及系统
技术领域
本发明属于地磁导航技术领域，具体涉及一种对飞行器编队的惯性导航指示坐标进行修正的方法以及系统。
背景技术
地磁导航作为一种无源自主导航方式，具有无累计误差、隐蔽性良好等优点，是航行器导航定位的重点研究方向之一。在实际工作中，单航行器地磁匹配采用一维航迹匹配，存在定位概率低、使用范围有限等不足。随着空战环境日益复杂，作战武器性能也逐步提升，多飞行器协同制导已成为空战中的主要作战方式，高精度的导航定位系统是飞行器顺利执行任务的关键。飞行器通过信息共享，使各飞行器都具有误差有界的定位能力。当某些飞行器受到环境影响或系统故障丧失导航能力时，通过协同导航可以使这些飞行器恢复一定的导航能力。地磁导航作为一种无源自主导航方式，在水下深空和制导等领域具有较多的应用。作为一种辅助导航方式，地磁导航没有累积误差，能有效校正惯性导航系统误差。
地磁匹配定位是地磁导航系统的关键技术，其目的是根据惯性导航系统指示航迹与实际地磁测量序列来计算确定系统的初始定位误差与航迹变换关系。多飞行器组网飞行条件下，地磁量测值成倍增加，比单飞行器具有更好的并行性和冗余性，同时，随着测量数据的增多，对匹配算法实时性与稳定性提出了更高的要求。常用的地磁匹配算法有磁场轮廓匹配方法和ICCP。轮廓匹配方法在当匹配初始位置误差较大时，不确定域随之增大，计算量不断增加。ICCP算法常用的地磁匹配算法多是基于单维匹配，不适用于多飞行器编队的多维匹配场景。
发明内容
发明目的：本发明旨在提供一种多飞行器组成编队飞行时的导航定位方法，该方法能够对惯性导航系统的指示位置坐标进行修改，以校正惯性导航系统的误差，且没有累积误差。
技术方案：本发明一方面公开了一种基于PSO的飞行器编队多维地磁匹配导航方法，所述飞行器编队包括一个主飞行器和多个从飞行器，所述主飞行器上设置有惯性导航系统，所有的飞行器上安装有地磁探测单元，第k个飞行器相对于主飞行器的飞行坐标为
Figure GDA0003346730530000021
Figure GDA0003346730530000022
M为飞行器总数；包括以下步骤：
(1)在地磁匹配时间段内分别多次采集每个飞行器上地磁探测单元的地磁测量值
Figure GDA0003346730530000023
以及与
Figure GDA0003346730530000024
同时刻的惯性导航系统指示坐标P1 ins(k,i)；i＝1,2,…,Nk，Nk为第k个飞行器在地磁匹配时间段内的采样点数；
计算第k个飞行器与地磁测量值
Figure GDA0003346730530000025
同时刻的惯导推算坐标
Figure GDA0003346730530000026
Figure GDA0003346730530000027
(2)采用PSO获取惯性导航系统的最佳修正参数p＝(Δpx,Δpy,θ,a)，Δpx,Δpy,θ,a分别为主飞行器上惯性导航系统指示坐标的x，y方向的平移误差、航向角误差与缩放因子；所述最佳修正参数为使飞行器编队中所有飞行器地磁测量值与基准图上地磁值的误差和最小时的修正参数；
(3)用所述最佳修正参数对惯性导航系统的指示坐标进行修正：
Figure GDA0003346730530000028
其中P1 ins为修正前的惯性导航系统的指示坐标值；P1 t为修正后的惯性导航系统的指示坐标值。
所述步骤(2)具体步骤包括：
(2.1)定义N个粒子组成粒子集Z＝{zj|j＝1..N}，每个粒子的位置用p表示，p＝(Δpx,Δpy,θ,a)；第j个粒子zj的位置为pj＝(Δpxj,Δpyj,θj,aj)，速度为vj；在搜索空间内随机初始化N个粒子的位置；所述搜索空间由惯性导航系统的误差约束：Δpx,min<Δpxj<Δpx,max，Δpy,min<Δpyj<Δpy,max，θmin<θ<θmax，amin<a<amax；其中Δpx,min、Δpx,max分别为惯性导航系统在x方向的位置误差最小值和最大值；Δpy,min、Δpy,max分别为惯性导航系统在y方向的位置误差最小值和最大值；θmin、θmax分别为惯性导航系统航向误差的最小值和最大值；amin、amax分别为惯性导航系统速度误差的最小值和最大值；
粒子速度vj初始化为
Figure GDA0003346730530000031
其中σx、σy、σθ、σa分别为N个粒子初始位置中Δpx,Δpy,θ,a的标准差；
设置最大迭代次数Hth；定义位置在p＝(Δpx,Δpy,θ,a)粒子的适应度为飞行器编队中所有飞行器地磁测量值与基准图上地磁值的误差和E(p)，计算如下：
Figure GDA0003346730530000032
Figure GDA0003346730530000033
为第k个飞行器在地磁匹配时间段的第i个地磁测量值，
Figure GDA0003346730530000034
为对
Figure GDA0003346730530000035
修正后的坐标；
Figure GDA0003346730530000036
为基准图上位置
Figure GDA0003346730530000037
处的地磁值；
初始化迭代次数h＝0，初始化第j个粒子的最佳位置pj,best为pj；
(2.2)将N个粒子的位置pj＝(Δpxj,Δpyj,θj,aj)分别作为参数计算适应度E(pj)，选择其中适应度值最小的粒子位置作为初始的粒子集最佳位置pbest；
(2.3)按如下更新方程更新粒子集Z中粒子zj的位置和速度：
Figure GDA0003346730530000038
Figure GDA0003346730530000039
上式中，h表示迭代次数；c1、c2为学习因子；r1、r2为[0,1]范围内均匀分布的随机数；ωh为惯性权值，
Figure GDA00033467305300000310
更新完成后重新计算每个粒子的适应度；如果
Figure GDA00033467305300000311
则将pj,best的值更新为
Figure GDA00033467305300000312
如果
Figure GDA00033467305300000313
则将pbest更新为
Figure GDA00033467305300000314
迭代次数h加一；
(2.4)重复步骤(2.3)，直到迭代次数h达到预设的阈值Hth，或pbest连续H′th次迭代都不发生更新，结束迭代；此时pbest的值(Δpxbest,Δpybest,θbest,abest)即为当前时刻主飞行器惯性导航系统的最佳修正参数。
所述学习因子c1、c2为[0,2]范围内的常数。
所述基准图为区域地磁异常图。
另一方面，本发明还公开了实现上述基于PSO的飞行器编队多维地磁匹配导航方法的系统，所述导航系统包括飞行器相对飞行坐标存储模块，用于存储每个飞行器相对于主飞行器的飞行坐标；还包括：
从飞行器惯导推算坐标计算模块，用于根据主飞行器上的惯性导航系统指示坐标
Figure GDA0003346730530000041
和从飞行器相对于主飞行器的飞行坐标
Figure GDA0003346730530000042
计算从飞行器的惯导推算坐标
Figure GDA0003346730530000043
Figure GDA0003346730530000044
最佳修正参数优化模块，用于采用PSO获取惯性导航系统的最佳修正参数p＝(Δpx,Δpy,θ,a)，Δpx,Δpy,θ,a分别为主飞行器上惯性导航系统指示坐标的x，y方向的平移误差、航向角误差与缩放因子；
惯性导航系统指示坐标修正模块，用于根据最佳修正参数对惯性导航系统的指示坐标进行修正：
Figure GDA0003346730530000045
其中P1 ins为修正前的惯性导航系统的指示坐标值；P1 t为修正后的惯性导航系统的指示坐标值。
所述最佳修正参数优化模块按照步骤(2.1)-(2.4)来获取最佳修正参数。
有益效果：本发明公开的基于PSO的飞行器编队多维地磁匹配导航方法具有以下优点：1、飞行器编队飞行过程中可以多次修正惯性导航系统的指示坐标，且每次修正之间不相关，有效地减少了惯性导航系统的累积误差；2、采用多维地磁匹配，提高了导航定位的精度；3、采用粒子群优化算法来获取最佳修正参数，可以快速收敛，且运算较小。
附图说明
图1为飞行器编队结构示意图；
图2为本发明公开的基于PSO的飞行器编队多维地磁匹配导航方法的流程图；
图3为区域地磁异常图；
图4为实施例中地磁匹配中惯导指示坐标与真实位置的轨迹对比图。。
具体实施方式
下面结合附图和具体实施方式，进一步阐明本发明。
多飞行器组成的飞行器编队包括一个主飞行器和多个从飞行器，其中主飞行器上设置有惯性导航系统，所有的飞行器上安装有地磁探测单元，用于测量飞行器所在位置处的地磁值。将主飞行器的编号设为1，从飞行器从2开始编号。第k个飞行器相对于主飞行器的飞行坐标为
Figure GDA0003346730530000051
以主飞行器为原点构建网间坐标系，则第k个飞行器在网间坐标系的坐标为
Figure GDA0003346730530000052
Figure GDA0003346730530000053
M为飞行器总数。飞行器编队的结构如图1所示，图中有4个飞行器，第1个飞行器为主飞行器，其余飞行器与主飞行器保持固定的位置飞行，即
Figure GDA0003346730530000054
为固定值，由飞行器编队的结构确定。在飞行过程中，主飞行器上的惯性导航系统会给出指示坐标P1 i，但由于惯性导航系统存在误差，其与主飞行器的真实位置P1 r之间存在着定位误差
Figure GDA0003346730530000055
由于惯性导航系统的误差是累积的，飞行过程中会导致定位误差增大，导致整个飞行器编队的导航定位精度下降。针对此问题，本发明公开了一种基于PSO的飞行器编队多维地磁匹配导航方法，在飞行过程中对惯性导航系统给出的指示坐标进行修正，使惯性导航系统的指示坐标接近主飞行器的真实坐标。在飞行过程中可以进行多次修正，相邻两次修正的时间间隔可以相等也可以不等，每次修正之间是互不相关相互独立的，如图2所示，具体包括：
步骤1、在地磁匹配时间段内分别多次采集每个飞行器上地磁探测单元的地磁测量值
Figure GDA0003346730530000056
以及与
Figure GDA0003346730530000057
同时刻的惯性导航系统指示坐标P1 ins(k,i)；i＝1,2,…,Nk，Nk为第k个飞行器在地磁匹配时间段内的采样点数，不同飞行器的采样点数可以相同也可以不同；
计算第k个飞行器与地磁测量值
Figure GDA0003346730530000058
同时刻的惯导推算坐标
Figure GDA0003346730530000059
Figure GDA00033467305300000510
步骤2、采用PSO获取惯性导航系统的最佳修正参数p＝(Δpx,Δpy,θ,a)，Δpx,Δpy,θ,a分别为主飞行器上惯性导航系统指示坐标的x，y方向的平移误差、航向角误差与缩放因子；所述最佳修正参数为使飞行器编队中所有飞行器地磁测量值与基准图上地磁值的误差和最小时的修正参数；具体步骤包括：
(2.1)定义N个粒子组成粒子集Z＝{zj|j＝1..N}，每个粒子的位置用p表示，p＝(Δpx,Δpy,θ,a)；第j个粒子zj的位置为pj＝(Δpxj,Δpyj,θj,aj)，速度为vj；在搜索空间内随机初始化N个粒子的位置；所述搜索空间由惯性导航系统的误差约束：Δpx,min<Δpxj<Δpx,max，Δpy,min<Δpyj<Δpy,max，θmin<θ<θmax，amin<a<amax；其中Δpx,min、Δpx,max分别为惯性导航系统在x方向的位置误差最小值和最大值；Δpy,min、Δpy,max分别为惯性导航系统在y方向的位置误差最小值和最大值；θmin、θmax分别为惯性导航系统航向误差的最小值和最大值；amin、amax分别为惯性导航系统速度误差的最小值和最大值；
粒子速度vj初始化为
Figure GDA0003346730530000061
其中σx、σy、σθ、σa分别为N个粒子初始位置中Δpx,Δpy,θ,a的标准差；
设置最大迭代次数Hth；定义位置在p＝(Δpx,Δpy,θ,a)粒子的适应度为飞行器编队中所有飞行器地磁测量值与基准图上地磁值的误差和E(p)，计算如下：
Figure GDA0003346730530000062
Figure GDA0003346730530000063
为第k个飞行器在地磁匹配时间段的第i个地磁测量值，
Figure GDA0003346730530000064
为
Figure GDA0003346730530000065
修正后的坐标；
Figure GDA0003346730530000066
为基准图上位置
Figure GDA0003346730530000067
处的地磁值；本实施例中基准图为区域地磁异常图，如图3所示，区域地磁异常图覆盖范围25km*25km，网格间距为50m。给出区域内的位置点，采用读图函数Bmap(·)可以读取到该点处的地磁值。
初始化迭代次数h＝0，初始化第j个粒子的最佳位置pj,best为pj；
(2.2)将N个粒子的位置pj＝(Δpxj,Δpyj,θj,aj)分别作为参数计算适应度E(pj)，选择其中适应度值最小的粒子位置作为初始的粒子集最佳位置pbest；
(2.3)按如下更新方程更新粒子集Z中粒子zj的位置和速度：
Figure GDA0003346730530000071
Figure GDA0003346730530000072
上式中，h表示迭代次数；c1、c2为学习因子，本实施例中为[0,2]范围内的常数；r1、r2为[0,1]范围内均匀分布的随机数；ωh为惯性权值，随着迭代次数的增加，惯性权值不断减小，本发明中计算式为：
Figure GDA0003346730530000073
更新完成后重新计算每个粒子的适应度；如果
Figure GDA0003346730530000074
则将pj,best的值更新为
Figure GDA0003346730530000075
如果
Figure GDA0003346730530000076
则将pbest更新为
Figure GDA0003346730530000077
迭代次数h加一；
(2.4)重复步骤(2.3)，直到迭代次数h达到预设的阈值Hth，或pbest连续H′th次迭代都不发生更新，结束迭代；此时pbest的值(Δpxbest,Δpybest,θbest,abest)即为当前时刻主飞行器惯性导航系统的最佳修正参数。
步骤3、用所述最佳修正参数对惯性导航系统的指示坐标进行修正：
Figure GDA0003346730530000078
其中P1 ins为修正前的惯性导航系统的指示坐标值；P1 t为修正后的惯性导航系统的指示坐标值。
实施例一：
本实施例以图3所示的区域地磁异常图所覆盖区域内飞行器编队的仿真飞行实验为例，其中主飞行器上惯性导航系统的技术指标为：陀螺零偏0.01°/h，加表零偏50ug；初始状态下飞行器编队的惯导定位误差为东向900m，北向1300m；磁传感器测量误差为2nT；在飞行过程中采用本发明公开的导航方法对惯导指示坐标进行多次修正，当修正后的惯导指示坐标与主飞行器真实位置的东向定位误差小于150m，同时北向误差小于200m时，认为匹配成功。
飞行器飞行速度为60m/s，每次修正的地磁匹配时间段内以5s为采样周期采集数据，共采集5次，即Nk＝5。飞行器数量分别选择1，2，3，5，7进行多次试验，飞行结束后计算本次飞行的匹配成功率，即匹配成功次数与飞行过程中修正次数的比值，并计算匹配成功误差，即匹配成功时东向误差、北向误差和适应度的均值；同时计算匹配失败误差，即匹配失败时东向误差、北向误差和适应度的均值。惯导指示坐标与真实位置的轨迹如图4所示，仿真结果如表1。
表1不同飞行器数量匹配结果
Figure GDA0003346730530000081
在相同的匹配区域，随着组网飞行器个数的增加，匹配成功率逐步提高，在当前条件下，7个飞行器同时进行地磁匹配，匹配成功率可达到95％，定位误差在百米内；从匹配失败误差数据来看，多飞行器匹配失败的定位精度明显高于单维匹配；同时，在多飞行器组网下，适应度能更好的区分匹配是否成功。
实施例二：
飞行器飞行速度为60m/s，飞行器数量为3个，每次修正的地磁匹配时间段内以1s为采样周期采集数据，采集次数Nk分别取3、6、9，11，15、20、30进行多次仿真实验，仿真结果如表2。
表2不同采样点多维匹配结果
Figure GDA0003346730530000091
表2为不同采样点数的导航定位结果。从表中可以看出，当采样周期固定时，东向匹配误差与采样点数呈正相关，北向匹配误差呈负相关；平均适应度随着采样点数的增加不断增大。随着采样点数的增加，匹配成功率逐渐提高。
为了对比在相同采样次数和飞行距离下，多维与单维的匹配差异，本实施例设计了如下试验：单飞行器飞行速度为60m/s，地磁匹配时间段内采样周期为1s，采样点数为5、9、15、36、45、60、80、90个。
表3不同采样点单维匹配结果
Figure GDA0003346730530000092
从表3可以看出，随着单维匹配的采样点的增加，匹配成功率不断上升。对比表2与表3，在采样点较少的情况下，多维匹配的成功率要明显高于单维，匹配误差也低于单维。与单维匹配相比，多维匹配能在较短的匹配周期内达到较高的匹配概率和定位精度；从适应度分布来看，多维匹配可从适应度分布来判断匹配成功与否，单维匹配则需要更多的参数。
实施例三：
飞行器飞行速度为60m/s，飞行器数量为3个，每次修正的地磁匹配时间段内以3s为采样周期采集数据，采集次数Nk为12。飞行器编队中各飞行器间的距离分别设置为东向50m，100m，150m，200m，300m，400m，600m，800m，进行多次仿真实验。仿真结果如表4：
表4不同飞行器轨迹间隔匹配结果
Figure GDA0003346730530000101
表4列出了多维匹配飞行器间距不同的导航定位仿真结果。从中可以看出，当航迹间距较小时，匹配成功率较低，间距为400m，为地磁基准图的5倍网格间距时，匹配成功率最高。
实施例四：
飞行器飞行速度为60m/s，其中，单飞行器地磁匹配时间段内采样周期为0.4s，采样点数为25个；多飞行器地磁匹配时间段内采样周期为1s，每条轨迹采样点为10个。对地磁测量值分别叠加标准差为1、5、8、12nT的高斯测量白噪声，仿真试验结果如表5：
表5不同地磁测量噪声下匹配结果
Figure GDA0003346730530000102
表5为不同地磁测量噪声下飞行器编队导航定位结果。在飞行距离相等的情况下，随着地磁测量噪声幅值的增加，单飞行器匹配成功率下降迅速，定位误差也随着噪声幅值的增加不断变大；在多飞行器编队的情况下，匹配成功率受地磁测量噪声幅值影响较小，并且出现在特定噪声环境下的匹配成功峰值，从表中可以看出，在地磁测量噪声标准差为5nT时，匹配结果优于其他情况。
上述仿真试验结果表明多维地磁匹配能够显著提高匹配定位成功率、匹配定位精度，较短的采样周期与较少的采样点数下，匹配定位精度优于一维地磁匹配，具有较强的抗干扰能力。从实施例二和实施例三可以看出，采样点数对定位精度的影响无强相关性，在相同的采样点下，采样周期越短，匹配结果越好。

Claims (6)

1.基于PSO的飞行器编队多维地磁匹配导航方法，所述飞行器编队包括一个主飞行器和多个从飞行器，所述主飞行器上设置有惯性导航系统，所有的飞行器上安装有地磁探测单元，第k个飞行器相对于主飞行器的飞行坐标为
Figure FDA0003346730520000011
k＝1,2,…,M，M为飞行器总数；其特征在于，包括：
(1)在地磁匹配时间段内分别多次采集每个飞行器上地磁探测单元的地磁测量值
Figure FDA0003346730520000012
以及与
Figure FDA0003346730520000013
同时刻的惯性导航系统指示坐标
Figure FDA0003346730520000014
i＝1,2,…,Nk，Nk为第k个飞行器在地磁匹配时间段内的采样点数；
计算第k个飞行器与地磁测量值
Figure FDA0003346730520000015
同时刻的惯导推算坐标
Figure FDA0003346730520000016
Figure FDA0003346730520000017
(2)采用PSO获取惯性导航系统的最佳修正参数p＝(Δpx,Δpy,θ,a)，Δpx,Δpy,θ,a分别为主飞行器上惯性导航系统指示坐标的x，y方向的平移误差、航向角误差与缩放因子；
所述最佳修正参数为使飞行器编队中所有飞行器地磁测量值与基准图上地磁值的误差和最小时的修正参数；
(3)用所述最佳修正参数对惯性导航系统的指示坐标进行修正：
Figure FDA0003346730520000018
其中P1 ins为修正前的惯性导航系统的指示坐标值；P1 t为修正后的惯性导航系统的指示坐标值；
所述步骤(2)具体步骤包括：
(2.1)定义N个粒子组成粒子集Z＝{zj|j＝1..N}，每个粒子的位置用p表示，p＝(Δpx,Δpy,θ,a)；第j个粒子zj的位置为pj＝(Δpxj,Δpyj,θj,aj)，速度为vj；在搜索空间内随机初始化N个粒子的位置；所述搜索空间由惯性导航系统的误差约束：Δpx,min<Δpxj<Δpx,max，Δpy,min<Δpyj<Δpy,max，θmin<θ<θmax，amin<a<amax；其中Δpx,min、Δpx,max分别为惯性导航系统在x方向的位置误差最小值和最大值；Δpy,min、Δpy,max分别为惯性导航系统在y方向的位置误差最小值和最大值；θmin、θmax分别为惯性导航系统航向误差的最小值和最大值；amin、amax分别为惯性导航系统速度误差的最小值和最大值；
粒子速度vj初始化为
Figure FDA0003346730520000021
其中σx、σy、σθ、σa分别为N个粒子初始位置中Δpx,Δpy,θ,a的标准差；
设置最大迭代次数Hth；定义位置在p＝(Δpx,Δpy,θ,a)粒子的适应度为飞行器编队中所有飞行器地磁测量值与基准图上地磁值的误差和E(p)，计算如下：
Figure FDA0003346730520000022
Figure FDA0003346730520000023
为第k个飞行器在地磁匹配时间段的第i个地磁测量值，
Figure FDA0003346730520000024
为对
Figure FDA0003346730520000025
修正后的坐标；
Figure FDA0003346730520000026
为基准图上位置
Figure FDA0003346730520000027
处的地磁值；
初始化迭代次数h＝0，初始化第j个粒子的最佳位置pj,best为pj；
(2.2)将N个粒子的位置pj＝(Δpxj,Δpyj,θj,aj)分别作为参数计算适应度E(pj)，选择其中适应度值最小的粒子位置作为初始的粒子集最佳位置pbest；
(2.3)按如下更新方程更新粒子集Z中粒子zj的位置和速度：
Figure FDA0003346730520000028
Figure FDA0003346730520000029
上式中，h表示迭代次数；c1、c2为学习因子；r1、r2为[0,1]范围内均匀分布的随机数；ωh为惯性权值，
Figure FDA00033467305200000210
更新完成后重新计算每个粒子的适应度；如果
Figure FDA0003346730520000031
则将pj,best的值更新为
Figure FDA0003346730520000032
如果
Figure FDA0003346730520000033
则将pbest更新为
Figure FDA0003346730520000034
迭代次数h加一；
(2.4)重复步骤(2.3)，直到迭代次数h达到预设的阈值Hth，或pbest连续Ht′h次迭代都不发生更新，结束迭代；此时pbest的值(Δpxbest,Δpybest,θbest,abest)即为当前时刻主飞行器惯性导航系统的最佳修正参数。
2.根据权利要求1所述的基于PSO的飞行器编队多维地磁匹配导航方法，其特征在于，所述学习因子c1、c2为[0,2]范围内的常数。
3.根据权利要求1所述的基于PSO的飞行器编队多维地磁匹配导航方法，其特征在于，所述基准图为区域地磁异常图。
4.基于PSO的飞行器编队多维地磁匹配导航系统，所述飞行器编队包括一个主飞行器和多个从飞行器，所述主飞行器上设置有惯性导航系统，所有的飞行器上安装有地磁探测单元；所述导航系统包括飞行器相对飞行坐标存储模块，用于存储每个飞行器相对于主飞行器的飞行坐标；
其特征在于，还包括：
从飞行器惯导推算坐标计算模块，用于根据主飞行器上的惯性导航系统指示坐标
Figure FDA0003346730520000035
和从飞行器相对于主飞行器的飞行坐标
Figure FDA0003346730520000036
计算从飞行器的惯导推算坐标
Figure FDA0003346730520000037
Figure FDA0003346730520000038
最佳修正参数优化模块，用于采用PSO获取惯性导航系统的最佳修正参数p＝(Δpx,Δpy,θ,a)，Δpx,Δpy,θ,a分别为主飞行器上惯性导航系统指示坐标的x，y方向的平移误差、航向角误差与缩放因子；
惯性导航系统指示坐标修正模块，用于根据最佳修正参数对惯性导航系统的指示坐标进行修正：
Figure FDA0003346730520000039
其中P1 ins为修正前的惯性导航系统的指示坐标值；P1 t为修正后的惯性导航系统的指示坐标值；。
最佳修正参数优化模块采用如下步骤获取最佳修正参数：
(2.1)定义N个粒子组成粒子集Z＝{zj|j＝1..N}，每个粒子的位置用p表示，p＝(Δpx,Δpy,θ,a)；第j个粒子zj的位置为pj＝(Δpxj,Δpyj,θj,aj)，速度为vj；在搜索空间内随机初始化N个粒子的位置；所述搜索空间由惯性导航系统的误差约束：Δpx,min<Δpxj<Δpx,max，Δpy,min<Δpyj<Δpy,max，θmin<θ<θmax，amin<a<amax；其中Δpx,min、Δpx,max分别为惯性导航系统在x方向的位置误差最小值和最大值；Δpy,min、Δpy,max分别为惯性导航系统在y方向的位置误差最小值和最大值；θmin、θmax分别为惯性导航系统航向误差的最小值和最大值；amin、amax分别为惯性导航系统速度误差的最小值和最大值；
粒子速度vj初始化为
Figure FDA0003346730520000041
其中σx、σy、σθ、σa分别为N个粒子初始位置中Δpx,Δpy,θ,a的标准差；
设置最大迭代次数Hth；定义位置在p＝(Δpx,Δpy,θ,a)粒子的适应度为飞行器编队中所有飞行器地磁测量值与基准图上地磁值的误差和E(p)，计算如下：
Figure FDA0003346730520000042
Figure FDA0003346730520000043
为第k个飞行器在地磁匹配时间段的第i个地磁测量值，
Figure FDA0003346730520000044
为对
Figure FDA0003346730520000045
修正后的坐标；
Figure FDA0003346730520000046
为基准图上位置
Figure FDA0003346730520000047
处的地磁值；
初始化迭代次数h＝0，初始化第j个粒子的最佳位置pj,best为pj；
(2.2)将N个粒子的位置pj＝(Δpxj,Δpyj,θj,aj)分别作为参数计算适应度E(pj)，选择其中适应度值最小的粒子位置作为初始的粒子集最佳位置pbest；
(2.3)按如下更新方程更新粒子集Z中粒子zj的位置和速度：
Figure FDA0003346730520000051
Figure FDA0003346730520000052
上式中，h表示迭代次数；c1、c2为学习因子；r1、r2为[0,1]范围内均匀分布的随机数；ωh为惯性权值，
Figure FDA0003346730520000053
更新完成后重新计算每个粒子的适应度；如果
Figure FDA0003346730520000054
则将pj,best的值更新为
Figure FDA0003346730520000055
如果
Figure FDA0003346730520000056
则将pbest更新为
Figure FDA0003346730520000057
迭代次数h加一；
(2.4)重复步骤(2.3)，直到迭代次数h达到预设的阈值Hth，或pbest连续Ht′h次迭代都不发生更新，结束迭代；此时pbest的值(Δpxbest,Δpybest,θbest,abest)即为当前时刻主飞行器惯性导航系统的最佳修正参数。
5.根据权利要求4所述的基于PSO的飞行器编队多维地磁匹配导航系统，其特征在于，所述学习因子c1、c2为[0,2]范围内的常数。
6.根据权利要求4所述的基于PSO的飞行器编队多维地磁匹配导航系统，其特征在于，所述基准图为区域地磁异常图。
CN202010493550.7A 2020-06-03 2020-06-03 基于pso的飞行器编队多维地磁匹配导航方法及系统 Active CN111895995B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010493550.7A CN111895995B (zh) 2020-06-03 2020-06-03 基于pso的飞行器编队多维地磁匹配导航方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010493550.7A CN111895995B (zh) 2020-06-03 2020-06-03 基于pso的飞行器编队多维地磁匹配导航方法及系统

Publications (2)

Publication Number Publication Date
CN111895995A CN111895995A (zh) 2020-11-06
CN111895995B true CN111895995B (zh) 2022-01-28

Family

ID=73206970

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010493550.7A Active CN111895995B (zh) 2020-06-03 2020-06-03 基于pso的飞行器编队多维地磁匹配导航方法及系统

Country Status (1)

Country Link
CN (1) CN111895995B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113514062B (zh) * 2021-06-18 2023-10-27 中国科学院空天信息创新研究院 一种多维地磁匹配定位方法及装置
CN113932806B (zh) * 2021-10-15 2023-08-25 北京航空航天大学 高速飞行器惯性/地磁匹配搜索区域自适应组合导航方法
CN114279438B (zh) * 2021-12-17 2023-11-17 东南大学 基于pso和iccp的地磁匹配导航方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106342186B (zh) * 2008-12-12 2012-09-19 天津航海仪器研究所 地磁导航定位匹配误差确定方法
CN103115624B (zh) * 2013-01-24 2014-12-10 南京航空航天大学 一种基于地磁匹配的地磁日变修正方法
CN104697523B (zh) * 2015-03-31 2017-05-31 哈尔滨工业大学 基于迭代计算的惯性/地磁匹配定位方法
CN104833373A (zh) * 2015-04-21 2015-08-12 西安理工大学 一种基于Procrustes的地磁匹配导航方法
CN106052688B (zh) * 2016-08-08 2019-01-08 西安电子科技大学 基于地形轮廓匹配的惯性导航系统速度累积误差修正方法
CN107270891B (zh) * 2017-05-05 2020-05-19 哈尔滨工业大学 基于抗差估计的惯性地磁匹配定位方法
CN107543541B (zh) * 2017-08-25 2019-11-08 北京麦钉艾特科技有限公司 一种适合室内自由运动载体的地磁定位方法

Also Published As

Publication number Publication date
CN111895995A (zh) 2020-11-06

Similar Documents

Publication Publication Date Title
CN111895995B (zh) 基于pso的飞行器编队多维地磁匹配导航方法及系统
CN109254591B (zh) 基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法
CN106885576B (zh) 一种基于多点地形匹配定位的auv航迹偏差估计方法
CN104807460B (zh) 无人机室内定位方法及系统
CN111024092A (zh) 一种多约束条件下智能飞行器航迹快速规划方法
CN113252038B (zh) 基于粒子群算法的航迹规划地形辅助导航方法
CN107270891B (zh) 基于抗差估计的惯性地磁匹配定位方法
CN108061889A (zh) Ais与雷达角度系统偏差的关联方法
CN113411744B (zh) 一种高精度的室内定位追踪方法
CN111397599A (zh) 基于三角形匹配算法改进的iccp水下地磁匹配方法
CN116642482A (zh) 基于固态激光雷达和惯性导航的定位方法、设备和介质
CN110243377A (zh) 一种基于分层式结构的集群飞行器协同导航方法
CN108225307A (zh) 一种惯性测量信息辅助的星图匹配方法
CN113532438B (zh) 一种大初始定位误差下的改进iccp地形匹配方法
RU2471152C1 (ru) Способ навигации летательных аппаратов
CN111381605A (zh) 一种应用于多无人机大范围海域水下多目标协同搜索方法
Xu et al. 3D pseudolinear Kalman filter with own-ship path optimization for AOA target tracking
CN112729277B (zh) 基于动态夹角匹配的星敏感器星图识别方法
CN116661469B (zh) 机器人轨迹误差修正方法及系统
CN107543541B (zh) 一种适合室内自由运动载体的地磁定位方法
CN113252039B (zh) 面向地形辅助导航的粒子群快速匹配方法
CN115950414A (zh) 一种不同传感器数据的自适应多重融合slam方法
CN113503891B (zh) 一种sinsdvl对准校正方法、系统、介质及设备
CN103954287B (zh) 深空探测自主导航的一种路标规划方法
CN111998847A (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