CN112113557B - 一种基于插值法的多无人机相对定位预测方法及装置 - Google Patents
一种基于插值法的多无人机相对定位预测方法及装置 Download PDFInfo
- Publication number
- CN112113557B CN112113557B CN202010742633.5A CN202010742633A CN112113557B CN 112113557 B CN112113557 B CN 112113557B CN 202010742633 A CN202010742633 A CN 202010742633A CN 112113557 B CN112113557 B CN 112113557B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- sampling point
- aerial vehicle
- relative
- positioning
- 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 144
- 238000005070 sampling Methods 0.000 claims abstract description 302
- 238000009825 accumulation Methods 0.000 claims abstract description 37
- 230000008859 change Effects 0.000 claims description 36
- 238000005259 measurement Methods 0.000 claims description 27
- 238000012937 correction Methods 0.000 claims description 11
- 238000013213 extrapolation Methods 0.000 claims description 10
- 230000006870 function Effects 0.000 description 14
- 230000005284 excitation Effects 0.000 description 11
- 230000002085 persistent effect Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 8
- 238000004590 computer program Methods 0.000 description 7
- 230000008569 process Effects 0.000 description 6
- 238000004088 simulation Methods 0.000 description 5
- 230000004807 localization Effects 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 238000011160 research Methods 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 230000006872 improvement Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000011835 investigation Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 208000027697 autoimmune lymphoproliferative syndrome due to CTLA4 haploinsuffiency Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000010485 coping Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 230000002045 lasting effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect 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
-
- 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
-
- 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)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种基于插值法的多无人机相对定位预测方法及装置,包括:获取当前采样点无人机间的定位数据;根据当前采样点对应的采样点数据更新所述当前采样点无人机间的相对定位;根据当前采样点对应的采样点数和所述当前采样点无人机间的定位数据预测下一采样点无人机间的相对定位;本发明提供的技术方案,在GPS拒止环境下不依赖于任何外部基础设施,通过基于插值的数据增强方法,来提高多无人机在高速飞行应用场景中相对定位的精度,并深入分析该方法在多无人机同速飞行场景中出现的误差累积问题。
Description
技术领域
本发明涉及多智能体协同控制领域,具体涉及一种基于插值法的多无人机相对定位预测方法及装置。
背景技术
随着科学技术飞速发展,人工智能领域与其他学科不断深入交叉融合,迎来了“智能爆炸”时代。单个无人智能系统迅猛发展的同时,在更多复杂任务需求的驱动之下,群体智能系统逐步发展。
无人机(UAV,Unmanned Aerial Vehicle)是一种具备自主控制能力的飞行智能体,可配备不同设备、执行多种任务,且能重复使用。其独有的低成本、低损耗、零伤亡,及高机动性、隐蔽性、灵活性等特点,使在军事、民用和科学研究领域都有很广的应用,由多架无人机组成的蜂群系统在应对复杂多变的任务环境中也越来越发挥着重要作用。导航定位是无人机自主性的决定性因素,而无人机之间的相对定位更是多无人机系统能够运行的先决条件。在当前大部分的应用中,GPS(全球定位系统)仍然是解决无人机导航定位的主要方案。然而,GPS在室内、山间、森林等环境中会出现信号不好、丢失的问题,这一类环境统称为GPS拒止环境。因此,近年来GPS拒止环境下多无人机相对定位方法的研究已经成为多智能体协同控制领域比较受关注的课题。
近年来,这一课题的研究主要包括依赖外部基础设施估算相对定位和不依赖外部基础设施的系统内自主相对定位两种。第一类需先通过外部设施实现自身的绝对定位,再通信共享自身定位信息以此来估算相对定位,包括基于GSM基站的市区协同定位、基于雷达基站的协同定位、基于UWB地面锚点网络的协同定位等。这一类方法的优点是定位快、精度高、无人机功耗低,缺点是在野外探测、灾区救援、战场侦查等场景中难以提前布置基础设施,应用场景受限、基础设施成本高。
第二类相对定位方法主要依靠机载传感器探测周围环境或相互之间的距离、方位角等信息,再通过对应的算法估算无人机之间的相对定位。这一类方法的一大特点就是不依赖任何外部基础设施,完全依据自身传感器实现系统内相对定位。根据使用算法的不同又可分为自主相对定位法和协同相对定位法。自主相对定位法是先通过算法估算自身无人机的绝对定位,再通信共享定位信息估算相对定位,包括:基于视觉的预存地图匹配定位法、基于磁异常地图的磁定位法和SLAM(Simultaneous Localization and Mapping,即时定位与制图)定位技术等。协同相对定位法主要是通过测算无人机之间的相对距离、方位角、俯仰角等参数来估算相对定位,有融合无线电测距和视觉测向的相对定位法、基于视觉的相对定位法和基于相对距离、相对速度持久激励的相对定位法等。但自主相对定位法的大部分研究存在计算量大、成本高、受环境干扰大的问题,难以真正部署在现实场景中,尤其是小型无人机组成的低成本蜂群系统中;协同相对定位法的应用广、计算量低、成本低,但仍存在部分问题:一是在无人机高速飞行场景中受限于传感器量测频率有限导致相对定位误差精度低;二是同速飞行时,由于传感器量测误差差别较大导致相对定位误差累积,通过分析和对比实验发现,当测速传感器误差远大于测距传感器时,误差沿相对定位负方向累积。
现有的协同相对定位法中存在无人机高速飞行时相对定位精度低和同速飞行时误差累积的问题,为有效应对任务过程中的不同飞行场景,研究一种精度高、无误差累积的相对定位方法尤为重要。
发明内容
针对现有技术的不足,本发明的目的是提供一种基于插值法的多无人机相对定位预测方法及装置,能够在GPS拒止环境下不依赖于任何外部基础设施,通过基于插值的数据增强方法,来提高多无人机在高速飞行应用场景中相对定位的精度,并深入分析该方法在多无人机同速飞行场景中出现的误差累积问题。
本发明的目的是采用下述技术方案实现的:
一种基于插值法的多无人机相对定位预测方法,其改进之处在于,所述方法包括:
获取当前采样点无人机间的定位数据;
根据当前采样点对应的采样点数据更新所述当前采样点无人机间的相对定位;
根据当前采样点对应的采样点数和所述当前采样点无人机间的定位数据预测下一采样点无人机间的相对定位;
其中,所述定位数据包括:相对距离、相对距离变化率、相对速度和相对定位。
优选地,所述根据当前采样点对应的采样点数更新所述当前采样点无人机间的相对定位,包括:
若当前采样点对应的采样点数不大于预设点数,则不更新所述当前采样点无人机间的定位数据,否则,利用插值法更新所述当前采样点无人机间的相对定位。
进一步的,所述利用插值法更新所述当前采样点无人机间的定位数据,包括:
基于第k-n个采样点至第k个采样点无人机间的相对距离,采用三次样条法插值法拟合无人机间的相对距离拟合曲线;
基于第k-n个采样点至第k个采样点无人机间的相对速度,采用三次样条法插值法拟合无人机间的相对速度拟合曲线;
其中,令q∈[0,x-1],当q∈[0,x-2]时,当q=x-1时, 为所述无人机间的相对距离拟合曲线中第k-1个采样点的无人机i与无人机j间的相对速度,为所述无人机间的相对距离拟合曲线中第k-1个采样点的无人机i与无人机j间的相对距离,i,j∈[1,X],X为无人机总数量,T为采样间隔时长,k为当前采样点数,n为预设采样点数。
其中,为第k-1个采样点的无人机i与无人机j间的相对定位,∈k-1为第k-1个采样点的无人机i与无人机j间的相对速度的测量误差,为第k-1个采样点的无人机i与无人机j间的相对距离的测量误差,为第k-1个采样点的无人机i与无人机j间的相对距离变化率的测量误差,γ为相对定位的修正因子,S1q为第q个数据点的无人机i与无人机j间的同速飞行系数,S2q为第q个数据点的无人机i与无人机j间的误差累积现象系数。
进一步的,按下式确定所述第q个数据点的无人机i与无人机j间的同速飞行系数S1q:
按下式确定所述第q个数据点的无人机i与无人机j间的误差累积现象系数S2q:
优选的,所述根据当前采样点对应的采样次数和所述当前采样点无人机间的定位数据预测下一采样点无人机间的相对定位,包括:
若当前采样点对应的采样次数不大于预设次数,则利用相对定位法预测下一采样点无人机间的定位数据中的相对定位,否则,利用插值法预测下一采样点无人机间的相对定位。
进一步的,所述利用插值法预测下一采样点无人机间的相对定位,包括:
基于第k-n个采样点至第k个采样点无人机间的相对距离,采用三次样条法插值法拟合无人机间的相对距离拟合曲线;
基于第k-n个采样点至第k个采样点无人机间的相对速度,采用三次样条法插值法拟合无人机间的相对速度拟合曲线;
其中,令p∈[0,y-1],当p∈[0,y-2]时, 当p=y-1时, 为所述无人机间的相对距离拟合曲线中第k个采样点的无人机i与无人机j间的相对速度,为所述无人机间的相对距离拟合曲线中第k个采样点的无人机i与无人机j间的相对距离,i,j∈[1,X],X为无人机总数量,T为采样间隔时长,k为当前采样点数,n为预设采样点数。
其中,为第k个采样点的无人机i与无人机j间的相对定位,∈k为第k个采样点的无人机i与无人机j间的相对速度的测量误差,为第k个采样点的无人机i与无人机j间的相对距离的测量误差,为第k个采样点的无人机i与无人机j间的相对距离变化率的测量误差,γ为相对定位的修正因子,S1p为第p个数据点的无人机i与无人机j间的同速飞行系数,S2p为第p个数据点的无人机i与无人机j间的误差累积现象系数。
进一步的,按下式确定所述第p个数据点的无人机i与无人机j间的同速飞行系数S1p:
按下式确定所述第p个数据点的无人机i与无人机j间的误差累积现象系数S2p:
基于同一发明构思,本发明还提供了一种基于插值法的多无人机相对定位预测装置,其改进之处在于,所述装置包括:
获取模块,用于获取当前采样点无人机间的定位数据;
更新模块,用于根据当前采样点对应的采样点数据更新所述当前采样点无人机间的相对定位;
预测模块,用于根据当前采样点对应的采样点数和所述当前采样点无人机间的定位数据预测下一采样点无人机间的相对定位;
其中,所述定位数据包括:相对距离、相对距离变化率、相对速度和相对定位。
与最接近的现有技术相比,本发明具有的有益效果:
本发明涉及一种基于插值法的多无人机相对定位预测方法及装置,包括:获取当前采样点无人机间的定位数据;根据当前采样点对应的采样点数据更新所述当前采样点无人机间的相对定位;根据当前采样点对应的采样点数和所述当前采样点无人机间的定位数据预测下一采样点无人机间的相对定位;本发明提供的技术方案,能够在GPS拒止环境下不依赖于任何外部基础设施提高多无人机在高速飞行应用场景中相对定位的精度,并深入分析该方法在多无人机同速飞行场景中出现的误差累积问题。
针对现有的基于持久性激励的相对定位法受限于传感器测量频率有限、通信带宽有限,导致在多无人机高速飞行场景中出现相对定位估计误差较大,甚至发散的问题,本发明通过插值法拟合传感器量测数据的变化曲线,再通过内插和外推的方法在数据层面上提高了传感器的测量频率,从而提高多无人机在高速飞行场景中的相对定位精度。
针对现有的基于持久性激励的相对定位法由于测距传感器与测速传感器的误差相差较大,导致在多无人机同速飞行场景中出现相对定位误差累积的现象,且误差累积方向为相对定位负方向的问题,本发明通过构造特定的Sigmoid函数对同速飞行状态和误差累积现象进行自动判断,并根据最新测量的相对距离来修正相对定位,使避免出现误差累积的现象。
附图说明
图1是本发明的一种基于插值法的多无人机相对定位预测方法流程图;
图2是本发明实施例中插值法增强测量数据集示意图;
图3是本发明实施例中在高速飞行场景中相对定位平均误差曲线对比图;
图4是本发明实施例中在同速飞行场景中相对定位累积误差修正效果对比图;
图5是本发明的一种基于插值法的多无人机相对定位预测装置的结构示意图。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步的详细说明。
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
针对现有协同相对定位法中基于持久性激励的相对定位法在无人机高速飞行场景中精度降低和同速飞行场景中误差累积的问题,本发明提供了一种基于插值法的多无人机相对定位预测方法,使多无人机在高速飞行时仍具备较高精度的相对定位输出,在同速飞行时避免出现误差累积的情况;
首先每架无人机通过测距传感器(如UWB)和测速传感器(如IMU)测量与邻域内(测距范围内)其他无人机之间的相对距离和相对速度;每一架无人机通过插值法将相对距离和相对速度进行插值函数拟合,使拟合曲线通过最近n个量测周期内的所有量测值;在插值函数曲线中前一时刻到当前时刻的数据点之间内插q个点,并利用基于持久性激励的相对定位迭代公式重新估算当前时刻的相对定位;在插值函数曲线当前时刻到下一时刻之间外推p个点,利用迭代公式估算下一时刻的相对定位;重复以上步骤,不断为无人机输出与其他无人机之间的相对定位,直至飞行结束,具体的,如图1所示,所述方法包括:
101获取当前采样点无人机间的定位数据;
102根据当前采样点对应的采样点数据更新所述当前采样点无人机间的相对定位;
103根据当前采样点对应的采样点数和所述当前采样点无人机间的定位数据预测下一采样点无人机间的相对定位;
其中,所述定位数据包括:相对距离、相对距离变化率、相对速度和相对定位。
本发明提供的最优实施例中,可以利用测距传感器UWB和测速传感器IMU实现所述步骤101,测量无人机之间的相对距离、相对距离变化率和相对速度,并采用一致性方法(见文献“Consensus-Based Cooperative Source Localization of Multi-Agent Systemswith Sampled Range Measurements[J].Unmanned Systems,2014,02(03):231-241.”,Chai G,Lin C,Lin Z的论文:基于共识的一致性协同定位法),在无人机之间交换数据,数据携带时间戳;
进一步的,若当前采样点对应的采样点数不大于预设点数,则不更新所述当前采样点无人机间的定位数据,该定位数据基于(文献“Ultra-wideband and odometry-basedcooperative relative localization with application to multi-uav formationcontrol.IEEE Transactions on Cybernetics,2019,1-14.”,GuoK,LiX,XieL的论文:基于持久性激励的相对定位法)获取,否则,利用插值法更新所述当前采样点无人机间的相对定位,在更新的过程中,利用状态函数判断在当前采样时刻最近3s内,无人机是否处在同速飞行状态,且是否出现相对定位误差累积问题,若出现误差累积,则利用误差沿相对定位负方向累积的原理利用相对距离修正相对定位。
其中,所述利用插值法更新所述当前采样点无人机间的定位数据的具体过程包括:
基于第k-n个采样点至第k个采样点无人机间的相对距离,采用三次样条法插值法拟合无人机间的相对距离拟合曲线,例如,如图2所示;
基于第k-n个采样点至第k个采样点无人机间的相对速度,采用三次样条法插值法拟合无人机间的相对速度拟合曲线,例如,如图2所;
其中,令q∈[0,x-1],当q∈[0,x-2]时,当q=x-1时, 为所述无人机间的相对距离拟合曲线中第k-1个采样点的无人机i与无人机j间的相对速度,为所述无人机间的相对距离拟合曲线中第k-1个采样点的无人机i与无人机j间的相对距离,i,j∈[1,X],X为无人机总数量,T为采样间隔时长,k为当前采样点数,n为预设采样点数。
其中,为第k-1个采样点的无人机i与无人机j间的相对定位,∈k-1为第k-1个采样点的无人机i与无人机j间的相对速度的测量误差,为第k-1个采样点的无人机i与无人机j间的相对距离的测量误差,为第k-1个采样点的无人机i与无人机j间的相对距离变化率的测量误差,γ为相对定位的修正因子,S1q为第q个数据点的无人机i与无人机j间的同速飞行系数,S2q为第q个数据点的无人机i与无人机j间的误差累积现象系数。
本发明提供的最优实施例中,利用Sigmoid函数判断3s内,无人机i与无人机j是否是同速飞行,并按下式确定所述第q个数据点的无人机i与无人机j间的同速飞行系数S1q:
本发明提供的最优实施例中,利用Sigmoid函数判断是否出现误差累积现象,当出现误差累积时,由于误差往累积,导致相对定位的二范式必然与相对距离相差较大,因此设计Sigmoid函数并按下式确定所述第q个数据点的无人机i与无人机j间的误差累积现象系数S2q:
其中,为第r个采样点的无人机i与无人机j间的相对距离变化率;μ为误差门限值,描述相对定位的二范式与相对距离的偏差大小,当超过该门限时进行误差修正,是统计距离当前采样点3s(即3/T个采样点)内相对距离变化率的平方和,作为同速飞行判断依据,在同速飞行过程中,相对距离变化率因此,当无人机i与无人机j同速飞行时,S1q≈1;当UAVi和UAVj异速飞行时,S1q≈0。且在指数函数中,S1q变化十分剧烈,只有在3s内都处于同速飞行时才会出现S1q≈1(仿真飞行过程中的前3s,因采样数据不够,令S1q=0),μ是误差累积门限值,当 时,S2q≈0,表示相对定位未出现误差累积现象;当时,S2q≈1,表示相对定位出现误差累积现象。
最终,根据当前采样点对应的采样次数和所述当前采样点无人机间的定位数据预测下一采样点无人机间的相对定位,该过程中根据插值法拟合的曲线外推的数据推算在估算的过程中,利用Sigmoid函数判断在3s内,无人机i与无人机j是否处在同速飞行状态,且是否出现相对定位误差累积问题,若出现误差累积,则利用误差沿相对定位负方向累积的原理利用相对距离修正相对定位。对相对速度、相对距离进行插值法拟合,并利用内插、外推扩大采样数据集,旨在通过扩充数据集来降低采样周期T,使相同硬件条件下相对定位的估算更准确,具体包括:
若当前采样点对应的采样次数不大于预设次数,则利用相对定位法预测下一采样点无人机间的定位数据中的相对定位,否则,利用插值法预测下一采样点无人机间的相对定位。
进一步的,所述利用插值法预测下一采样点无人机间的相对定位,包括:
基于第k-n个采样点至第k个采样点无人机间的相对距离,采用三次样条法插值法拟合无人机间的相对距离拟合曲线,例如,如图2所;
基于第k-n个采样点至第k个采样点无人机间的相对速度,采用三次样条法插值法拟合无人机间的相对速度拟合曲线,例如,如图2所;
其中,令p∈[0,y-1],当p∈[0,y-2]时, 当p=y-1时, 为所述无人机间的相对距离拟合曲线中第k个采样点的无人机i与无人机j间的相对速度,为所述无人机间的相对距离拟合曲线中第k个采样点的无人机i与无人机j间的相对距离,i,j∈[1,X],X为无人机总数量,T为采样间隔时长,k为当前采样点数,n为预设采样点数。
其中,为第k个采样点的无人机i与无人机j间的相对定位,∈k为第k个采样点的无人机i与无人机j间的相对速度的测量误差,为第k个采样点的无人机i与无人机j间的相对距离的测量误差,为第k个采样点的无人机i与无人机j间的相对距离变化率的测量误差,γ为相对定位的修正因子,S1p为第p个数据点的无人机i与无人机j间的同速飞行系数,S2p为第p个数据点的无人机i与无人机j间的误差累积现象系数。
进一步的,按下式确定所述第p个数据点的无人机i与无人机j间的同速飞行系数S1p:
按下式确定所述第p个数据点的无人机i与无人机j间的误差累积现象系数S2p:
本发明提供的最优实施例中设计了多无人机高速飞行仿真环境中的实验结果,如图3所示,共设计三架无人机进行飞行仿真实验,平均飞行速度根据传感器采样频率调研结果取采样周期T=0.025s,三架无人机编号为0、1、2,在足够大的空间内进行随机飞行,图中Baseline曲线表示基于(文献“Ultra-wideband and odometry-basedcooperative relative localization with application to multi-uav formationcontrol.IEEE Transactions on Cybernetics,2019,1-14.”,GuoK,LiX,XieL的论文:基于持久性激励的相对定位法)得到的相对定位误差曲线,Enhanced曲线表示使用本专利方法得到的相对定位误差曲线,表示无人机i与无人机j之间的相对定位误差。其中,图3中(a)取γ=0.1,从左往右分别为无人机0与无人机1、无人机0与无人机2、无人机1与无人机2之间的相对定位误差曲线,以及平均误差曲线,实线表示基于持久性激励的相对定位法,虚线表示本方法改进后的方法,从图中可以看出,原始方法出现误差发散的现象,平均误差为无穷大,本方法可持续输出相对定位,平均误差分别为:11.04m、4.75m、7.79m、7.86m;图3中(b)取γ=0.03,从左往右分别为无人机0与无人机1、无人机0与无人机2、无人机1与无人机2之间的相对定位误差曲线,以及平均误差曲线,实线表示基于持久性激励的相对定位法,虚线表示本方法改进后的方法,Baseline方法平均误差分别为:10.78m、5.91m、6.58m、7.75m,本方法平均误差分别为:7.91m、5.77、4.17m、5.95m。从对比实验中可以看出,当无人机高速飞行时,基于持久性激励的相对定位法可能出现误差发散的现象,本方法可缓解这一问题并持续输出相对定位;通过缩小收敛步长γ基于持久性激励的相对定位法可继续工作,但本方法仍然可提供比该方法误差更低的相对定位。
本发明提供的最优实施例中设计了多无人机同速飞行仿真环境中的实验结果,如图4所示,设计了三架无人机进行飞行仿真实验结果,取根据传感器采样频率调研结果去采样周期T=0.025s,根据相对定位误差收敛效果比较去收敛步长γ=0.1,三架无人机编号为0、1、2,在足够大的空间内先随机飞行60s,相对定位在这一阶段将充分收敛;60s之后进行同速飞行至300s,相对定位将出现误差累积现象,图4中(b)为加入本文提出的误差修正方法之后,相对定位误差累积现象消失,如图4中(c)所示,图4中(a)为三架无人机飞行轨迹。
基于同一发明构思,本发明还提供了一种基于插值法的多无人机相对定位预测装置,如图5所示,所述装置包括:
获取模块,用于获取当前采样点无人机间的定位数据;
更新模块,用于根据当前采样点对应的采样点数据更新所述当前采样点无人机间的相对定位;
预测模块,用于根据当前采样点对应的采样点数和所述当前采样点无人机间的定位数据预测下一采样点无人机间的相对定位;
其中,所述定位数据包括:相对距离、相对距离变化率、相对速度和相对定位。
优选地,所述更新模块具体用于:
若当前采样点对应的采样点数不大于预设点数,则不更新所述当前采样点无人机间的定位数据,否则,利用插值法更新所述当前采样点无人机间的相对定位。
进一步的,所述利用插值法更新所述当前采样点无人机间的定位数据,包括:
基于第k-n个采样点至第k个采样点无人机间的相对距离,采用三次样条法插值法拟合无人机间的相对距离拟合曲线;
基于第k-n个采样点至第k个采样点无人机间的相对速度,采用三次样条法插值法拟合无人机间的相对速度拟合曲线;
其中,令q∈[0,x-1],当q∈[0,x-2]时,当q=x-1时, 为所述无人机间的相对距离拟合曲线中第k-1个采样点的无人机i与无人机j间的相对速度,为所述无人机间的相对距离拟合曲线中第k-1个采样点的无人机i与无人机j间的相对距离,i,j∈[1,X],X为无人机总数量,T为采样间隔时长,k为当前采样点数,n为预设采样点数。
其中,为第k-1个采样点的无人机i与无人机j间的相对定位,∈k-1为第k-1个采样点的无人机i与无人机j间的相对速度的测量误差,为第k-1个采样点的无人机i与无人机j间的相对距离的测量误差,为第k-1个采样点的无人机i与无人机j间的相对距离变化率的测量误差,γ为相对定位的修正因子,S1q为第q个数据点的无人机i与无人机j间的同速飞行系数,S2q为第q个数据点的无人机i与无人机j间的误差累积现象系数。
进一步的,按下式确定所述第q个数据点的无人机i与无人机j间的同速飞行系数S1q:
按下式确定所述第q个数据点的无人机i与无人机j间的误差累积现象系数S2q:
优选的,所述预测模块具体用于:
若当前采样点对应的采样次数不大于预设次数,则利用相对定位法预测下一采样点无人机间的定位数据中的相对定位,否则,利用插值法预测下一采样点无人机间的相对定位。
进一步的,所述利用插值法预测下一采样点无人机间的相对定位,包括:
基于第k-n个采样点至第k个采样点无人机间的相对距离,采用三次样条法插值法拟合无人机间的相对距离拟合曲线;
基于第k-n个采样点至第k个采样点无人机间的相对速度,采用三次样条法插值法拟合无人机间的相对速度拟合曲线;
其中,令p∈[0,y-1],当p∈[0,y-2]时, 当p=y-1时, 为所述无人机间的相对距离拟合曲线中第k个采样点的无人机i与无人机j间的相对速度,为所述无人机间的相对距离拟合曲线中第k个采样点的无人机i与无人机j间的相对距离,i,j∈[1,X],X为无人机总数量,T为采样间隔时长,k为当前采样点数,n为预设采样点数。
其中,为第k个采样点的无人机i与无人机j间的相对定位,∈k为第k个采样点的无人机i与无人机j间的相对速度的测量误差,为第k个采样点的无人机i与无人机j间的相对距离的测量误差,为第k个采样点的无人机i与无人机j间的相对距离变化率的测量误差,γ为相对定位的修正因子,S1p为第p个数据点的无人机i与无人机j间的同速飞行系数,S2p为第p个数据点的无人机i与无人机j间的误差累积现象系数。
进一步的,按下式确定所述第p个数据点的无人机i与无人机j间的同速飞行系数S1p:
按下式确定所述第p个数据点的无人机i与无人机j间的误差累积现象系数S2p:
本领域内的技术人员应明白,本申请的实施例可提供为方法、系统、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
最后应当说明的是:以上实施例仅用以说明本发明的技术方案而非对其限制,尽管参照上述实施例对本发明进行了详细的说明,所属领域的普通技术人员应当理解:依然可以对本发明的具体实施方式进行修改或者等同替换,而未脱离本发明精神和范围的任何修改或者等同替换,其均应涵盖在本发明的权利要求保护范围之内。
Claims (8)
1.一种基于插值法的多无人机相对定位预测方法,其特征在于,所述方法包括:
获取当前采样点无人机间的定位数据;
根据当前采样点对应的采样点数据更新所述当前采样点无人机间的相对定位;
根据当前采样点对应的采样点数和所述当前采样点无人机间的定位数据预测下一采样点无人机间的相对定位;
其中,所述定位数据包括:相对距离、相对距离变化率、相对速度和相对定位;
所述根据当前采样点对应的采样点数更新所述当前采样点无人机间的相对定位,包括:
若当前采样点对应的采样点数不大于预设点数,则不更新所述当前采样点无人机间的定位数据,否则,利用插值法更新所述当前采样点无人机间的相对定位;
所述利用插值法更新所述当前采样点无人机间的定位数据,包括:
基于第k-n个采样点至第k个采样点无人机间的相对距离,采用三次样条法插值法拟合无人机间的相对距离拟合曲线;
在所述无人机间的相对距离拟合曲线中第k-1个采样点至第k个采样点之间均匀内插x个数据点并分别获取x个数据点对应的相对距离变化率
基于第k-n个采样点至第k个采样点无人机间的相对速度,采用三次样条法插值法拟合无人机间的相对速度拟合曲线;
在所述无人机间的相对速度拟合曲线中第k-1个采样点至第k个采样点之间均匀内插x个数据点
基于所述和更新所述当前采样点无人机间的定位数据;
其中,令q∈[0,x-1],当q∈[0,x-2]时,当q=x-1时, 为所述无人机间的相对距离拟合曲线中第k-1个采样点的无人机i与无人机j间的相对速度,为所述无人机间的相对距离拟合曲线中第k-1个采样点的无人机i与无人机j间的相对距离,i,j∈[1,X],X为无人机总数量,T为采样间隔时长,k为当前采样点数,n为预设采样点数。
2.如权利要求1所述的方法,其特征在于,所述基于所述和更新所述当前采样点无人机间的定位数据,包括:
步骤1.初始化q=0,
步骤2.按下式确定第q+1个数据点的无人机i与无人机j间的相对定位
步骤3.若q=x-1,则更新第k个采样点的无人机i与无人机j间的相对定位否则,令q=q+1并返回步骤1;
其中,为第k-1个采样点的无人机i与无人机j间的相对定位,∈k-1为第k-1个采样点的无人机i与无人机j间的相对速度的测量误差,为第k-1个采样点的无人机i与无人机j间的相对距离的测量误差,为第k-1个采样点的无人机i与无人机j间的相对距离变化率的测量误差,γ为相对定位的修正因子,S1q为第q个数据点的无人机i与无人机j间的同速飞行系数,S2q为第q个数据点的无人机i与无人机j间的误差累积现象系数。
3.如权利要求2所述的方法,其特征在于,按下式确定所述第q个数据点的无人机i与无人机j间的同速飞行系数S1q:
按下式确定所述第q个数据点的无人机i与无人机j间的误差累积现象系数S2q:
其中,为第r个采样点的无人机i与无人机j间的相对距离变化率;μ为误差门限值。
4.如权利要求1所述的方法,其特征在于,所述根据当前采样点对应的采样点数和所述当前采样点无人机间的定位数据预测下一采样点无人机间的相对定位,包括:
若当前采样点对应的采样点数不大于预设点数,则利用相对定位法预测下一采样点无人机间的定位数据中的相对定位,否则,利用插值法预测下一采样点无人机间的相对定位。
5.如权利要求4所述的方法,其特征在于,所述利用插值法预测下一采样点无人机间的相对定位,包括:
基于第k-n个采样点至第k个采样点无人机间的相对距离,采用三次样条法插值法拟合无人机间的相对距离拟合曲线;
在所述无人机间的相对距离拟合曲线中第k个采样点之后按T/y的时间间隔外推y个数据点并分别获取y个数据点对应的相对距离变化率
基于第k-n个采样点至第k个采样点无人机间的相对速度,采用三次样条法插值法拟合无人机间的相对速度拟合曲线;
在所述无人机间的相对速度拟合曲线中第k个采样点之后按T/y的时间间隔外推y个数据点
基于所述和预测第k+1个采样点无人机间的相对定位;
其中,令p∈[0,y-1],当p∈[0,y-2]时, 当p=y-1时, 为所述无人机间的相对距离拟合曲线中第k个采样点的无人机i与无人机j间的相对速度,为所述无人机间的相对距离拟合曲线中第k个采样点的无人机i与无人机j间的相对距离,i,j∈[1,X],X为无人机总数量,T为采样间隔时长,k为当前采样点数,n为预设采样点数。
6.如权利要求5所述的方法,其特征在于,所述基于所述和预测第k+1个采样点无人机间的相对定位,包括:
步骤4.初始化p=0,
步骤5.按下式确定第p+1个数据点的无人机i与无人机j间的相对定位
步骤6.若p=y-1,则第k+1个采样点的无人机i与无人机j间的相对定位否则,令q=q+1并返回步骤1;
其中,为第k个采样点的无人机i与无人机j间的相对定位,∈k为第k个采样点的无人机i与无人机j间的相对速度的测量误差,为第k个采样点的无人机i与无人机j间的相对距离的测量误差,为第k个采样点的无人机i与无人机j间的相对距离变化率的测量误差,γ为相对定位的修正因子,S1p为第p个数据点的无人机i与无人机j间的同速飞行系数,S2p为第p个数据点的无人机i与无人机j间的误差累积现象系数。
7.如权利要求6所述的方法,其特征在于,按下式确定所述第p个数据点的无人机i与无人机j间的同速飞行系数S1p:
按下式确定所述第p个数据点的无人机i与无人机j间的误差累积现象系数S2p:
其中,为第r个采样点的无人机i与无人机j间的相对距离变化率;μ为误差门限值。
8.一种基于插值法的多无人机相对定位预测装置,其特征在于,所述装置包括:
获取模块,用于获取当前采样点无人机间的定位数据;
更新模块,用于根据当前采样点对应的采样点数据更新所述当前采样点无人机间的相对定位;
预测模块,用于根据当前采样点对应的采样点数和所述当前采样点无人机间的定位数据预测下一采样点无人机间的相对定位;
其中,所述定位数据包括:相对距离、相对距离变化率、相对速度和相对定位;
所述根据当前采样点对应的采样点数更新所述当前采样点无人机间的相对定位,包括:
若当前采样点对应的采样点数不大于预设点数,则不更新所述当前采样点无人机间的定位数据,否则,利用插值法更新所述当前采样点无人机间的相对定位;
所述利用插值法更新所述当前采样点无人机间的定位数据,包括:
基于第k-n个采样点至第k个采样点无人机间的相对距离,采用三次样条法插值法拟合无人机间的相对距离拟合曲线;
在所述无人机间的相对距离拟合曲线中第k-1个采样点至第k个采样点之间均匀内插x个数据点并分别获取x个数据点对应的相对距离变化率
基于第k-n个采样点至第k个采样点无人机间的相对速度,采用三次样条法插值法拟合无人机间的相对速度拟合曲线;
在所述无人机间的相对速度拟合曲线中第k-1个采样点至第k个采样点之间均匀内插x个数据点
基于所述和更新所述当前采样点无人机间的定位数据;
其中,令q∈[0,x-1],当q∈[0,x-2]时,当q=x-1时, 为所述无人机间的相对距离拟合曲线中第k-1个采样点的无人机i与无人机j间的相对速度,为所述无人机间的相对距离拟合曲线中第k-1个采样点的无人机i与无人机j间的相对距离,i,j∈[1,X],X为无人机总数量,T为采样间隔时长,k为当前采样点数,n为预设采样点数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010742633.5A CN112113557B (zh) | 2020-07-29 | 2020-07-29 | 一种基于插值法的多无人机相对定位预测方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010742633.5A CN112113557B (zh) | 2020-07-29 | 2020-07-29 | 一种基于插值法的多无人机相对定位预测方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112113557A CN112113557A (zh) | 2020-12-22 |
CN112113557B true CN112113557B (zh) | 2023-05-02 |
Family
ID=73799533
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010742633.5A Active CN112113557B (zh) | 2020-07-29 | 2020-07-29 | 一种基于插值法的多无人机相对定位预测方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112113557B (zh) |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8487896B1 (en) * | 2012-06-27 | 2013-07-16 | Google Inc. | Systems and methods for improving image tracking based on touch events |
CN104932548B (zh) * | 2015-05-29 | 2017-11-21 | 北京航空航天大学 | 一种无人机定向天线自跟踪系统设计方法 |
CN107085437A (zh) * | 2017-03-20 | 2017-08-22 | 浙江工业大学 | 一种基于eb‑rrt的无人机航迹规划方法 |
JP6589107B2 (ja) * | 2017-10-05 | 2019-10-16 | 有限会社ワイエスデイ | 変調波レゾルバ装置 |
CN108828539A (zh) * | 2018-06-25 | 2018-11-16 | 中国电子科技集团公司第四十研究所 | 一种vco开环线性校正方法 |
CN110186456B (zh) * | 2019-04-30 | 2023-09-29 | 中国科学院深圳先进技术研究院 | 一种目标定位的方法、系统、无人机及存储介质 |
CN111102981B (zh) * | 2019-12-09 | 2022-03-04 | 北京空间机电研究所 | 一种基于ukf的高精度卫星相对导航方法 |
CN111090283B (zh) * | 2019-12-20 | 2023-08-29 | 上海航天控制技术研究所 | 一种无人艇组合定位定向方法和系统 |
-
2020
- 2020-07-29 CN CN202010742633.5A patent/CN112113557B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN112113557A (zh) | 2020-12-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111220153B (zh) | 基于视觉拓扑节点和惯性导航的定位方法 | |
Wüest et al. | Online estimation of geometric and inertia parameters for multirotor aerial vehicles | |
CN112432649A (zh) | 一种引入威胁因子的启发式无人机蜂群航迹规划方法 | |
CN108267731B (zh) | 无人机目标跟踪系统的构建方法及应用 | |
CN108508471A (zh) | 一种无人驾驶车辆定位方法及装置 | |
CN112346104B (zh) | 一种无人机信息融合定位方法 | |
CN109507706B (zh) | 一种gps信号丢失的预测定位方法 | |
CN110969643B (zh) | 地面目标移动轨迹的星上自主预测方法 | |
CN111812669B (zh) | 绕机检查装置及其定位方法、存储介质 | |
CN113960622A (zh) | 融合激光雷达及imu传感器信息的实时定位方法及装置 | |
CN111156986A (zh) | 一种基于抗差自适应ukf的光谱红移自主组合导航方法 | |
Taylor | An analysis of observability-constrained Kalman filtering for vision-aided navigation | |
CN115728803A (zh) | 一种城市驾驶车辆连续定位系统及方法 | |
CN113124882B (zh) | 一种背景磁场未知情形下的多磁偶极子磁源反演定位方法 | |
CN112113557B (zh) | 一种基于插值法的多无人机相对定位预测方法及装置 | |
WO2016165336A1 (zh) | 一种导航的方法和终端 | |
CN109769206B (zh) | 一种室内定位融合方法、装置、存储介质及终端设备 | |
CN115790603A (zh) | 一种用于信息拒止环境下的无人机动态目标估计方法 | |
CN114216463B (zh) | 一种路径优化目标定位方法及装置、存储介质、无人设备 | |
CN116576868A (zh) | 一种多传感器融合精确定位及自主导航方法 | |
Monda et al. | Ground target localization and tracking in a riverine environment from a uav with a gimbaled camera | |
Nielsen et al. | Ground target tracking using a monocular camera and imu in a nonlinear observer slam framework | |
Li et al. | An improved target tracking algorithm based on extended kalman filter for uav | |
Hadji et al. | A Review: Simultaneous localization and mapping algorithms for mobile robot | |
CN110160527B (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 |