CN110186456B - 一种目标定位的方法、系统、无人机及存储介质 - Google Patents
一种目标定位的方法、系统、无人机及存储介质 Download PDFInfo
- Publication number
- CN110186456B CN110186456B CN201910364492.5A CN201910364492A CN110186456B CN 110186456 B CN110186456 B CN 110186456B CN 201910364492 A CN201910364492 A CN 201910364492A CN 110186456 B CN110186456 B CN 110186456B
- Authority
- CN
- China
- Prior art keywords
- target
- current sampling
- state information
- sampling time
- aerial vehicle
- 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 91
- 238000005070 sampling Methods 0.000 claims abstract description 234
- 238000001914 filtration Methods 0.000 claims abstract description 27
- 238000012545 processing Methods 0.000 claims abstract description 20
- 238000005259 measurement Methods 0.000 claims description 48
- 239000011159 matrix material Substances 0.000 claims description 25
- 230000008569 process Effects 0.000 claims description 19
- 230000033001 locomotion Effects 0.000 claims description 12
- 230000026676 system process Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000006870 function Effects 0.000 description 32
- 238000012360 testing method Methods 0.000 description 13
- 239000004973 liquid crystal related substance Substances 0.000 description 10
- 238000010586 diagram Methods 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 4
- 239000000654 additive Substances 0.000 description 3
- 230000000996 additive effect Effects 0.000 description 3
- 238000013178 mathematical model Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000010845 search algorithm Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000012790 confirmation Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000008685 targeting Effects 0.000 description 1
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/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/12—Target-seeking control
-
- 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)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本申请公开了一种目标定位的方法、系统、无人机及存储介质,该方法包括获取由同一无人机测量得到的多个目标在当前采样时刻的方向数据;利用三维伪线性卡尔曼滤波算法,对每个目标在当前采样时刻的方向数据进行处理,得到每个目标在当前采样时刻的状态信息。通过上述方式,可以实现同时由一个无人机对多个目标进行定位,且可实现对多个目标更为准确的定位。
Description
技术领域
本申请涉及无人机领域,特别是涉及一种目标定位的方法、系统、无人机及存储介质。
背景技术
相比于卫星定位技术,无人机搜索目标在精确性以及搜索速度上更具有优势,故近年来无人机被广泛应用于各个领域的目标跟踪或搜索中。然而当对多个目标执行搜索任务时,由于搜索范围往往很大,现有技术中为快速锁定目标则多会采用多架无人机分散作业。然而由于分散的无人机在协作上难度较大,同时也会限定搜索的精度并增加了成本投入,故需要一种可以解决当前技术问题的方案。
发明内容
本申请主要解决的技术问题是提供一种目标定位的方法、系统、无人机及存储介质,实现由同一无人机同时对多个目标进行更加准确的定位。
为解决上述技术问题,本申请采用的一个技术方案是:提供一种目标定位的方法,所述方法包括:
获取由同一无人机测量得到的多个目标在当前采样时刻的方向数据;
利用三维伪线性卡尔曼滤波算法,对每个所述目标在当前采样时刻的方向数据进行处理,得到每个所述目标在当前采样时刻的状态信息。
为解决上述技术问题,本申请采用的另一个技术方案是,提供一种目标定位的系统,所述系统包括:处理器以及与所述处理器连接的存储器;
其中,所述存储器用于存储程序数据;
所述处理器用于运行所述程序数据,以执行如上所述的目标定位的方法。
为解决上述技术问题,本申请采用的又一个技术方案是,提供一种无人机,所述无人机包括:传感组件、驱动电路和目标定位的系统,所述传感组件和驱动电路分别与所述目标定位的系统的处理器连接;
其中,所述传感组件用于获取多个目标的方向数据;
所述驱动电路用于响应所述处理器的控制指令飞至下一路径点;
所述目标定位的系统用于实现对所述多个目标定位,所述目标定位的系统为如上所述的系统。
为解决上述技术问题,本申请采用的又一个技术方案是,提供一种存储介质,所述存储介质存储有程序数据,所述程序数据被执行时实现如上所述的目标定位的方法。
以上方案,通过获取由同一无人机测量得到的多个目标在当前采样时刻的方向数据,然后利用三维伪线性卡尔曼滤波算法,对每个目标在当前采样时刻的方向数据进行处理,得到每个目标在当前采样时刻的状态信息。基于上述技术方案可以实现由同一无人机对多个目标进行同时定位,且通过本申请所提供的三维伪线性卡尔曼滤波算法对每个目标在当前采样时刻的方向数据进行处理,可以实现对多个目标更为准确的定位。
附图说明
图1是本申请一种目标定位的方法一实施例的流程示意图;
图2是本申请一种目标定位的方法一实施例中目标相对于无人机的方位角和俯仰角的示意图;
图3是本申请一种目标定位的方法另一实施例的流程示意图;
图4是本申请一种目标定位的方法又一实施例的流程示意图;
图5是本申请一种目标定位的方法又一实施例的流程示意图;
图6是本申请一种目标定位的方法一实施例的流程示意图;
图7a是本申请一种目标定位的方法又一实施例中的路径搜索范围示意图;
图7b是本申请一种目标定位的方法一实施例中的成本函数的示意图;
图7c是本申请一种目标定位的方法另一实施例中的成本函数的示意图;
图8是本申请一种目标定位的方法又一实施例的流程示意图;
图9是本申请一种目标定位的方法又一实施例的流程示意图;
图10是本申请一种目标定位的方法再一实施例的流程示意图;
图11是本申请一种目标定位的方法又一实施例的流程示意图;
图12是本申请一种目标定位的系统一实施例的结构示意图;
图13是本申请一种无人机一实施例中的结构示意图;
图14是本申请一种存储介质一实施例的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述。可以理解的是,此处所描述的具体实施例仅用于解释本申请,而非对本申请的限定。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
本申请的描述中,“多个”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。此外,术语“包括”和“具有”以及它们任何变形,意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元,而是可选地还包括没有列出的步骤或单元,或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。
在本文中提及“实施例”意味着,结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例,也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是,本文所描述的实施例可以与其它实施例相结合。
需要首先说明的是,本申请所提供的技术方案可应用于无人机上,无人机可基于自身所装备的传感组件获取目标的初始位置数据,且在本申请所提供的技术方案中单个无人机可用于对多个目标进行定位。其中,在一实施例中,无人机中装备有AOA(Angle-of-arrival,目标方向角)传感器,用于实时跟踪某区域的多个移动的目标。其中,并不限定本申请所提供的方法所应用的无人机的种类。
请参见图1,图1为本申请一种目标定位的方法一实施例的流程示意图。
S10:获取由同一无人机测量得到的多个目标在当前采样时刻的方向数据。
其中,采样时刻是指对目标进行方向数据采样的时间点,相邻采样时刻间隔的时长是根据需要进行设置的,相邻采样时刻间隔的时长定义为时间步长,时间步长的长短可以根据实际的需求进行调整,在此不做限定。本申请所提供的技术方案可同时对同一无人机跟踪的多个目标进行定位,故步骤S10是获取同一无人机测量得到的多个目标在当前采样时刻的方向数据。
在当前实施例中,方向数据包括目标相对于无人机的方位角和俯仰角的测量值。请参见图2,图2为本申请一种目标定位的方法一实施例中目标相对于无人机的方位角和俯仰角的示意图。其中,对于目标Pk,1,目标Pk,1相对于无人机的方位角θk,1是无人机rk与目标Pk,1在xy平面上的正投影P’k,1间的连线与基准线Ark之间的夹角,其中,基准线Ark是过无人机rk且与xy平面上与x轴平行的直线,目标Pk,1相对于无人机rk的俯仰角φk,1是目标Pk,1与无人机rk之间的连线和目标Pk,1在xy平面上的正投影P’k,1与无人机rk之间的连线的夹角,其中,rk是表示是无人机在k采样时刻的位置。其中,由于无人机具有一定的体积,故无人机的位置可以是指无人机上所装备的传感组件的位置。在本申请所提供的技术方案中,是由同一无人机同时获取多个目标在当前采样时刻的方向数据,多个目标相对于无人机的位置信息并不相同,且本申请所提供的技术方案中对于步骤S10中所获得的多个目标在当前采样时刻的方向数据的具体类型并不做限定,也不限定获得方向数据的具体方法。
在当前实施例中,多个目标在当前采样时刻的方向数据是由无人机上所装备的AOA传感器获得。具体地,AOA传感器可在离散的采样时刻k=1,2,3…同时获取多个不同目标的方向数据,在本申请所提供的技术方案中,用k来表示采样时刻,用i来标识无人机同时跟踪的第i个目标或者是目标i,pk,i表示在第k个采样时刻目标i的位置信息。在当前实施例中,目标pk,i理想的方位角θk,i和俯仰角φk,i的数学模型如公式(1):
其中pk,i=[pxk,i,pyk,i,pzk,i]和rk=[rxk,ryk,rzk]分别是第k个采样时刻的目标i和无人机自身的位置信息,pxk,i表示的是第k个采样时刻目标i在x坐标轴上的坐标,pyk,i表示的是第k个采样时刻目标i在y坐标轴上的坐标,pzk,i表示的是第k个采样时刻目标i在z坐标轴上的坐标,rxk,ryk,rzk是表示的无人机在第k个采样时刻在x轴,y轴和z轴方向上的位置数据。||[pxk,i,pyk,i]-[rxk,ryk]||是对[pxk,i,pyk,i]-[rxk,ryk]求取欧几里得范数,tan-1是四象限反正切函数。
在另一实施例中,由于无人机飞行时受风、发动机震动等因素影响,机载传感组件量测中含有较大高斯噪声,而会影响目标定位精度。对应的,在当前实施例中,在构建方位角和俯仰角的数学模型时,将实际应用的过程中会遇到的噪声考虑进去,对应的目标pk,i包含噪声的方位角及俯仰角/>的数学模型方位角和俯仰角实际测量模型如公式(2)所示,可将方位角和俯仰角的实际测量模型简称为实际测量值:
其中nk,i和mk,i分别是均值为零、方差为和/>的独立加性高斯白噪声,θk,i和φk,i表示的是理想的方位角和俯仰角。无人机的位置数据和速度至少可以是由机载导航设备获取,可以理解的,在此并不限定无人机的位置数据和速度的获取方式。
S20:利用三维伪线性卡尔曼滤波算法,对每个目标在当前采样时刻的方向数据进行处理,得到每个目标在当前采样时刻的状态信息。
其中,可同时并行采用多个三维伪线性卡尔曼滤波算法逻辑分别对每个目标在当前采样时刻的方向数据进行处理,也可以是采用同一个三维伪线性卡尔曼滤波算法逻辑依次对每个目标在当前采样时刻的方向数据进行处理,进而获得每个目标在当前采样时刻的状态信息。
其中,如上所述目标在当前采样时刻的方向数据包括在当前采样时刻测量得到的方向角和俯仰角,三维伪线性卡尔曼滤波算法包括xy平面的伪线性卡尔曼滤波算法和z轴的伪线性卡尔曼滤波算法,目标的状态信息至少包括目标的三维坐标位置、目标的三维方向的运动速度,可以理解的在其他实施例中,目标的状态信息还可以包括其他的内容。
图1所示意的实施例中,通过获取由同一无人机测量得到的多个目标在当前采样时刻的方向数据,然后利用三维伪线性卡尔曼滤波算法,对每个目标在当前采样时刻的方向数据进行处理,得到每个目标在当前采样时刻的状态信息,可以较好地实现由同一无人机对多个目标的定位,节省了在对多个目标进行定位和追踪时所需投入的资源。
如上所述,三维伪线性卡尔曼滤波算法包括xy平面的伪线性卡尔曼滤波算法和z轴的伪线性卡尔曼滤波算法。
对应的,在另一实施例中,步骤S20利用三维伪线性卡尔曼滤波算法,对每个目标在当前采样时刻的方向数据进行处理,得到每个目标在当前采样时刻的状态信息包括:利用xy平面的伪线性卡尔曼滤波算法,对每个目标在当前采样时刻的方向数据进行处理,得到每个目标在当前采样时刻的xy平面的状态信息;以及,利用z轴的伪线性卡尔曼滤波算法,对每个目标在当前采样时刻的方向数据进行处理,得到每个目标在当前采样时刻的z轴的状态信息。
其中,在当前实施例中,对于是先利用xy平面的伪线性卡尔曼滤波算法对每个目标在当前采样时刻的方向数据进行处理,得到每个目标在当前采样时刻的xy平面的状态信息,还是先利用z轴的伪线性卡尔曼滤波算法,对每个目标在当前采样时刻的方向数据进行处理,得到每个目标在当前采样时刻的z轴的状态信息不做特别限定,具体可以依据不同实施例中的要求进行设定调整。
进一步地,请参见图3,图3为本申请一种目标定位的方法另一实施例的流程示意图。在当前实施例中,步骤利用xy平面/z轴的伪线性卡尔曼滤波算法,对每个目标在当前采样时刻的方向数据进行处理,得到每个目标在当前采样时刻的xy平面/z轴的状态信息包括步骤S31至步骤S33。
S31:利用目标在前一采样时刻的最终状态信息,预测得到目标在当前采样时刻的预测状态信息。
在当前实施例中,可将三维动态模型分成xy平面和z轴两个部分。目标在xy平面上的动态模型定义为目标在z轴方向的动态模型定义为/>其中,矩阵的上标的T表示的是向量或矩阵的转置。具体地,基于目标在前一采样时刻的xy平面上最终状态信息和公式(3)获得目标在当前采样时刻在xy平面上的预测状态信息,基于目标在前一采样时刻的z轴方向上最终状态信息和公式(4)获得目标在当前采样时刻在z轴方向上的预测状态信息。
ak|k-1,i=Uak-1|k-1,i (3)
bk|k-1,i=Gbk-1|k-1,i (4)
其中,k表示的是当前采样时刻,k-1表示的是前一采样时刻,ak|k-1,i表示的是在当前采样时刻目标i在xy平面上的预测状态信息,ak-1|k-1,i表示的是前一采样时刻的目标i在xy平面上的最终状态信息,bk|k-1,i表示的是在当前采样时刻目标i在z轴方向上的预测状态信息,bk-1|k-1,i表示的是前一采样时刻目标i在z轴方向上的最终状态信息。
在本申请所提供的技术方案中,将三维空间中第i个目标的状态信息定义为该目标的动态模型满足
其中,在极小的时间间隔中,可以将目标在xy平面内的运动看作是匀速直线运动,上述U是目标做匀速直线运动的速度状态,G是z轴方向上的运动转换矩阵,具体地/>其中,/>表示在当前采样时刻目标i在x轴方向上的运动速度,同理,/>和/>分别用于表示在当前采样时刻目标i在y轴方向和z轴方向上的运动速度,矩阵中的元素T表示离散时刻k和k+1之间的时间步长,qk,i表示系统过程噪声。
S32:对目标在当前采样时刻的方向数据进行处理,得到目标在当前采样时刻的测量状态信息。其中,对于目标在当前采样时刻在xy平面的测量状态信息是对目标的方向数据进行非线性处理得到。
其中,对于目标i在xy平面的测量状态信息hk,i是基于公式(5)获得,对于目标i在z轴方向的测量状态信息lk,i是基于公式(6)获得。
lk,i=[1,0]T (6)
其中,为无人机在当前采样时刻k测量得到的目标i的方位角的实际测量值。在z轴上可以直接将目标i的动态模型定义为lk,i=[1,0]T。在本申请所提供的技术方案中,将传感组件测量所得的无人机的实际测量值的非线性运算转换为线性运算,可避免数据近似所造成的数据丢失,进而减少对多个目标进行定位的误差。
S33:基于目标在当前采样时刻的预测状态信息和测量状态信息,获得目标在当前采样时刻的最终状态信息。
在分别求得的目标在当前采样时刻的预测状态信息和目标在当前采样时刻的测量状态信息,进一步求取目标在当前采样时刻的最终状态信息,目标的最终状态信息至少包括目标在xy平面上的状态信息和目标在z轴方向上的状态信息。进一步地,可参见图4中对于步骤S33所包括内容的详细阐述。
进一步地,请参见图4,图4为本申请一种目标定位的方法又一实施例中的流程示意图。在当前实施例中,图3所示意的步骤S33之前还包括步骤S41,步骤S33:基于目标在当前采样时刻的预测状态信息和测量状态信息,获得目标在当前采样时刻的最终状态信息,则包括步骤S42至步骤S45。具体地,当前实施例中该方法包括:
S41:利用目标在前一采样时刻的最终状态准确度,预测得到目标在当前采样时刻的预测状态准确度。
其中,最终状态准确度是用于衡量最终状态信息的准确性的参数,对应的,预测状态准确度是用于衡量预测状态信息准确度的参数。在本申请所提供的技术方案中,在每个采样时刻获得目标的最终状态信息之后,会进一步获得目标在当前采样时刻的最终状态准确度,用W表示目标i在xy平面上测量准确度。具体地,在当前实施例中用Wk|k-1,i表示目标i基于前一采样时刻的最终状态准确度Wk-1|k-1,i所求得的在当前采样时刻的预测状态准确度。具体地,预测状态准确度Wk|k-1,i是基于公式(7)获得。
Wk|k-1,i=UWk-1|k-1,iUT+Mk-1,i (7)
其中,UT表示的是U的转置矩阵,Mk-1,i是在k-1时刻的边程噪声的协方差矩阵。
S42:获取目标在当前采样时刻的测量状态信息相对预测状态信息的误差。
其中,目标在当前采样时刻的测量状态信息相对预测状态信息的误差gk,i是基于公式(8)求得。
其中,表示的是目标在xy平面的测量状态信息的转置矩阵,rxyk表示的是当前采样时刻无人机在xy平面上的位置数据,ak|k-1,i表示的是在当前采样时刻目标i的预测状态信息。在当前实施例中,目标在当前采样时刻的测量状态信息相对预测状态信息的误差,等于目标在xy平面的测量状态信息的转置矩阵和当前采样时刻无人机的位置信息与当前采样时刻目标的预测状态信息差的乘积。
S43:获取目标与无人机之间的距离,并基于目标与无人机之间的距离得到无人机对目标在当前采样时刻的测量准确度。
其中,目标i在当前采样时刻与无人机之间的距离是基于公式(9)获得,目标i在当前采样时刻的测量准确度ξk,i是基于公式(10)求得。
Δ=||[pxk|k-1,i,pyk|k-1,i]T-[rxk,ryk]T|| (9)
其中,用Δ表示当前采样时刻目标与无人机之间的距离,pxk|k-1,i,pyk|k-1,i分别表示的是在当前采样时刻目标i在x轴上的预测状态信息和y轴上的预测状态信息,目标与无人机之间的距离利用距离公式并基于目标在当前采样时刻的预测状态信息与无人机当前位置信息求得。其中,无人机在当前的位置信息是可以基于无人机的运动速度与轨迹求得,为明确已知量,在此不做过多阐述。σθ,i是目标i测量方位角时的高斯白噪声,为已知量。
S44:利用目标在当前采样时刻的测量状态信息、预测状态准确度、测量准确度,获得目标在当前采样时刻的测量状态信息的权重。
其中,目标在当前采样时刻的测量状态信息的权重ωk,i是基于公式(11)求得,公式(11)如下:
其中,权重ωk,i是目标在当前采样时刻的测量状态信息占目标在当前采样时刻的测量状态信息和预测状态信息的和的权重比,公式中其他参数可参见本申请上下文中对应阐述的内容。
S45:利用误差以及权重,获得目标在当前采样时刻的最终状态信息。
进一步地,步骤S45包括:利用下述公式(12),获得在当前采样时刻目标i在xy平面的最终状态信息ak|k,i。在当前实施例中,目标i的最终状态信息ak|k,i等于权重ωk,i与误差gk,i的乘积和目标在当前采样时刻的预测状态信息的和。具体地,公式(12)如下:
ak|k,i=ak|k-1,i+ωk,igk,i (12)
进一步地,在当前实施例中,在图3所示意的实施例中,经过步骤S33获得目标在当前采样时刻的最终状态信息之后,本申请所提供的方法还包括步骤S46,进一步获得当前采样时刻目标i的最终状态准确度。
S46:基于目标在当前采样时刻的预测状态准确度、测量状态信息以及权重,获得目标在当前采样时刻的最终状态准确度。
进一步地,步骤S46包括利用下面公式(13),获得目标i在当前采样时刻的xy平面的最终状态准确度Wk|k,i,并输出保存用于下一采样时刻目标定位时进行调用。
其中,I是单位矩阵,Wk|k-1,i为预测协方差矩阵,用于表示目标在当前采样时刻的最终状态准确度,ωk,i和hk,i均可以依据上述公式求得,在此不再赘述。
在又一实施例中,进一步阐述目标在z轴方向上的最终状态信息的求解过程。
首先,利用目标在前一采样时刻在z轴方向上的最终状态准确度Sk-1|k-1,i,预测得到目标在当前采样时刻在z轴方向上的预测状态准确度Sk|k-1,i。计算公式如公式(14)所示。
Sk|k-1,i=GSk-1|k-1,iGT+Qk-1,i (14)
其中,Qk-1,i表示的是系统过程噪声的协方差矩阵,G是z轴方向上的运动转换矩阵。
再者,获取目标在当前采样时刻的测量状态信息相对预测状态信息的误差ck,i,具体是依据公式(15)求得,
再基于公式(16)获取xy平面上目标与无人机之间的距离并基于目标与无人机之间的距离以及公式(17)得到无人机对目标在当前采样时刻的测量准确度fk,i。
其中,是目标i相对无人机的俯仰角的实际测量值。其中,需要说明的是,当在当前实施例中,是设定先获取xy平面上的目标的最终状态信息,再获取z轴方向上的目标的最终状态信息,则在计算xy平面上目标与无人机之间的距离/>时,采用的是当前采样时刻目标在xy平面上的最终状态信息计算求得更为准确的/>可以理解的,当在另一实施例中,设定同时获取xy平面上的目标的最终状态信息和z轴上目标的最终状态信息,则在计算/>时则会采用的是当前采样时刻目标在xy平面上的预测状态信息。
然后,利用目标在当前采样时刻的测量状态信息lk,i、预测状态准确度Sk|k-1,i、测量准确度fk,i,获得目标在当前采样时刻的测量状态信息的权重tk,i,其中,tk,i表示的是目标的实际测量状态信息占测量状态信息和预测状态信息和的权重比,具体计算公式请参见公式(18)。
进一步地,当计算目标在z轴上的最终状态信息时,图4中步骤S45利用误差以及权重,获得目标在当前采样时刻的最终状态信息包括:利用下面公式(19),获得目标i在当前采样时刻k的z轴的最终状态信息bk|k,i,公式(19)如下所示:
bk|k,i=bk|k-1,i+tk,ick,i (19)
最后,在求得目标在当前采样时刻在z轴方向上的最终状态信息后,进一步求取目标在当前采样时刻的最终状态准确度。
具体地,步骤S46基于目标在当前采样时刻的预测状态准确度、测量状态信息以及权重,获得目标在当前采样时刻的最终状态准确度则包括:利用下面公式(20),获得目标i在当前采样时刻在z轴的最终状态准确度Sk|k,i,具体计算过程请参见公式(20)。
其中,I是单位矩阵,Sk|k-1,i是目标i在当前采样时刻k在z轴方向的预测状态准确度。
当前实施例中,本申请所提供的方法通过将三维动态模型分为xy平面和z轴两个部分,可对目标实现更为准确的定位。
请参见图5,图5为本申请一种目标定位的方法又一实施例中的流程示意图。在当前实施例中,进一步考虑无人机的路径点对于目标定位准确度的影响,故本申请所提供的方法还包括图5所示的内容,该方法包括:
S51:评估无人机分别在若干待定路径点对多个目标进行测量的定位准确情况。
在无人机对目标进行定位的过程中,无人机的搜索路径会影响对目标定位的准确度。故本申请所提供的目标定位的方法,在利用三维伪线性卡尔曼滤波算法对目标在当前采样时刻的方向数据进行处理时或者是处理之后,会进一步确定一个可以使得目标定位更为准确的无人机接下来的路径点。在确定无人机接下来的路径点时会先初步确定若干待定路径点,然后会对待定路径点进行评估,具体是评估无人机分别在若干待定路径点对多个目标进行测量的定位准确情况。在评估得到无人机分别在若干待定路径点对多个目标进行测量的定位准确情况后,会进一步基于定位准确情况确定无人机下一路径点。
其中,定位准确情况是基于无人机处于待定路径点时对应的预测状态准确度求得。对应的,每个目标的定位准确情况具体体现为成本函数,故每个目标的定位准确情况可以基于如公式(21)所对应的目标的成本函数数学模型求得。
Ji(rk)=Jxy,i(rxyk)+Jz,i(rzk)=tr(Wk|k,i)+tr(Sk|k,i) (21)
其中,tr(·)表示矩阵求迹,即tr(Wk|k,i)和tr(Sk|k,i)分别表示的是求取Wk|k,i和Sk|k,i的迹,rxyk是k个采样时刻时在xy平面中无人机位置信息,而rzk是在第k个采样时刻无人机的z轴位置信息。
进一步地,由于本申请所提供的技术方案中,同一无人机同时实现对多个目标进行定位,那么在确定下一路径点时则需要同时评估在该待定路径点时多个目标的定位准确情况。由于目标的成本函数是加性的,因此对于多个目标的定位准确情况则可以将每个目标的成本函数相加求得。在当前实施例中,某个待定路径点对应的成本函数Ji(rk)的值越小,则对应的表示在当前待定路径点的定位准确情况越好。
再进一步地,由于无人机同时跟踪的多个目标的权重比不同,则对应的在计算多个目标的定位准确情况时,则会进一步考虑不同目标对应的权重比例,具体可以依据公式(22)求得多个目标的定位准确情况。
其中,λi表示的是每个目标所占的权重比例,每个目标的权重比例可以根据需要预先设置并调整,Nt=1,2,3,...表示目标的数量。
S52:将定位准确情况符合预设条件的待定路径点作为无人机的下一路径点。
在当前实施例中,进一步设定选取定位准确情况符合预设条件的待定路径点作为无人机的下一路径点。其中,预设条件至少包括待定路径点的定位准确情况大于或等于预设值或定位准确情况最优对应的待定路径点,其中,定位准确情况大于或等于预设值是待定路径点对应的成本函数值小于或等于某一预设值,定位准确情况最优是待定路径点对应的成本函数值最小。如可将对多个目标进行测量的定位准确情况最优的路径点确定为无人机的下一路径点。
S53:控制无人机向下一路径点飞行,并在飞行过程中继续测量得到多个目标在下一采样时刻的方向数据,且利用多个目标在下一采样时刻的方向数据获得每个目标在下一采样时刻的状态信息。
在确定无人机的下一路径点之后,生成控制指令发送至无人机的驱动电路,以控制无人机向下一路径点飞行。在飞行的过程中继续测量得到多个目标在下一采样时刻的方向数据,且利用多个目标在下一采样时刻的方向数据获得每个目标在下一采样时刻的状态信息。
请参见图6,图6为本申请一种目标定位的方法一实施例中的流程示意图。
S61:利用由同一无人机测量得到的多个目标在当前采样时刻的方向数据,获得每个目标在当前采样时刻的状态信息。
其中,多个目标在当前采样时刻的方向数据是由无人机机载的传感组件获得,每个目标在当前采样时刻的状态信息可以按照图1至图4及其所对应的实施例所对应的至少一个获得,获得每个目标在当前采样时刻的状态信息的过程具体可以参见上文,在此不做过多阐述。
在获得每个目标在当前采样时刻的状态信息之后,在获得每个目标在下一采样时刻的状态信息之前,本申请所提供的技术方案会执行如步骤S62至步骤S63所述的相关评估以及优化路径的算法,以获得一个更为准确的下一路径点,进而提高对多个目标的定位准确度。
S62:评估无人机分别在若干待定路径点对多个目标进行测量的定位准确情况。
在获得每个目标在当前采样时刻的状态信息之后,会首先确定若干待定路径点。然后基于步骤S62评估无人机分别在若干待定路径点对多个目标进行测量的定位准确情况。其中,初步确定的若干待定路径点是基于3D网格搜索算法确定。其中,在当前实施例中,定位准确情况是基于对多个目标的定位准确情况进行处理求得。步骤S62所评估的具体是,如果当无人机飞至待定路径点对多个目标进行定位,则所获得是多个目标状态信息的整体的准确情况,具体可以参见上文图5中的步骤S51的阐述。
S63:将定位准确情况符合预设条件的待定路径点作为无人机的下一路径点。
其中,预设条件是根据需求进行设定的,可以根据不同实施例中的要求进行调整。预设条件至少包括:定位准确情况最优,定位准确情况大于设定值的且同时满足其他相关条件的,具体在此不一一阐述。
S64:控制无人机向下一路径点飞行,并在飞行过程中继续测量得到多个目标在下一采样时刻的方向数据,且利用多个目标在下一采样时刻的方向数据获得每个目标在下一采样时刻的状态信息。步骤S64与图5中的步骤S53相同,具体可以参见图5中对应的阐述,在此不再赘述。
请参见图7a,图7a为本申请一种目标定位的方法又一实施例中的路径搜索范围示意图。在当前实施例中,受无人机飞行速度所限,无人机所有可能的下一个航路点构成了一个具有半径长度为vxyzT的球面,而我们需要搜索该球面以找到最佳的下一个路径点。图7a显示了采用3D网格搜索算法所需搜索区域,其中,R1表示的是当前无人机所在的位置,R2中的所有搜索路径点为可能的下一定路径点,在本申请中定义为待定路径点。
其中,球面上所有可能的下一个路径点应满足公式(23)所定义的关系,在当前实施例中,将所有可能的下一个路径点定义为待定路径点。
rk+1=rk+μj,j=1,2,...,Nxy (23)
其中,rk为无人机当前采样时刻的位置,μj为自无人机当前位置至下一路径点的向量,定义Nxy是在xy平面中的搜索数,Nz是z轴中的搜索数,j是用于表示当前所计算的待定路径点序号,其中,μj满足公式(24):
公式(24)中,s=1,2,…,Nz,l=1,2,…,Nxy并且和/>表示待定路径点矢量相对于无人机当前所在路径点的方位角和俯仰角。/>
在进行对下一路径点的均匀的搜索中,各个待定路径点的方位角和俯仰角的角度应满足公式(25)所定义的关系。
在公式(25)中,Nxy>1,Nz>1,且/>
在求得该待定路径点的方位角和俯仰角之后,可基于无人机的待定位置信息[rxk+1,ryk+1,rzk+1]以及在当前采样时刻目标的最终状态信息,在每个待定路径点上重新计算得到当无人机飞至待定路径点时每个目标的待定方位角和待定俯仰角/>然后再将每个目标的待定方位角/>和待定俯仰角/>分别代入下述公式(26)及(27)求得对应待定路径点的待定测量准确度,然后再将所得的待定测量准确度带入上述公式(21),然后利用公式(22)进而求解得到当前待定路径点对应的目标的成本函数值,该待定路径点对应的目标的成本函数值如公式(26)所述。
其中,k+1表示的是下一采样时刻,表示的是下一采样时刻目标i在xy平面上距离无人机所装备的传感组件的距离。
公式(27)如下:
lk+1,i=[1,0]T
Sk+1,i=Sk|k,i (27)
其中,和/>使用[rxk+1,ryk+1,rzk+1]和k时刻的所得的目标最终状态信息计算,后即可求得当前待定路径点的获得最终成本函数值。公式(26)和(27)详细内容可参见图1至图4所对应的实施例,在此不再详述。/>
如上所述,定位准确情况符合预设条件的待定路径点作为无人机的下一路径点。在当前实施例中,预设条件为定位准确情况最优,故下一路径点rk+1则为公式(29)的计算结果。
其中,表示是对成本函数求取最小值,N表示由Nxy和Nz确定的总搜索点数。对于确定的Nxy和Nz,N=Nxy×Nz。
由于本申请所提供的技术方案中,是由同一无人机同时实现对多个目标进行定位,但是在下一路径点的确认过程中,可能会出现局部极小问题,进而影响对目标定位的准确度,故需要对目标搜索的数量和范围进行进一步地优化。
请同时参见图7b和图7c,图7b为本申请一种目标定位的方法一实施例中成本函数为便于理解局部极小的问题,举一个具体数值例子来解释局部极小问题。如在一实施例中,三个目标分别从[0,300,200]Tm,[100,-200,100]Tm和[200,100,300]Tm开始移动。目标的匀速运动,速度v为[3,3,1]Tm/s,[6,4,2]T m/s和[4,0,4]Tm/s。另外,目标运动时具有零均值高斯分布的随机加速度。目标的加速度方差为[0.52,0.12,0.12]T m4/s4,[0.12,0.42,0.22]Tm4/s4和[0.52,0.52,0.12]T m4/s4。对于每个目标,预先设定三维伪线性卡尔曼滤波器中参数有qx=qy=qz=0.01。无人机最初位置为[0,0,0]m,目标估计量的初始状态矩阵是三个目标的3DPLKF(三维伪线性卡尔曼滤波算法)的初始协方差矩阵均相同,diag[W0|0,S0|0]=diag[104,104,104,104,104,104]。无人机速度为vxyz=70m/s,T=1s。使用具有固定搜索数Nxy=Nz=5的网格搜索路径优化方法,当k=6,获得下一步搜索范围在k=7上的点的所有成本函数值。图7b和图7c分别显示了球体上的路径点的成本函数值和相应的等高图。我们使用方位角和俯仰角/>来表示球体上的点,其中,图7b中的Cost function value表示的是路径点的成本函数值,/>表示的是目标相对无人机的方位角,φ(degree)是目标相对无人机的俯仰角。
图7b中k=7时球体上各点的成本函数值在图7c中全局最小值的轮廓图用Q2表示,局部最小值用Q1表示。
由图7b和图7c中,我们可以看到成本函数是非凸函数并且存在局部最小值。受预测精度的影响,局部极小问题在目标追踪的初始阶段,即测量信息较少时较为严重。随着测量的增加及估计算法的运行,定位精度逐步改善,成本函数图将改变并且局部最小点可能消失,这使得在搜索路径时搜索网格的大小应该是可以依据需要进行动态调整的。此外,如果搜索网格大小不合适,则所选择的路径点始终不足以接近全局最小值。因此,需要对目标搜索的数量和范围进行进一步地优化。
具体地,为解决上述问题请参见图8,图8为本申请一种目标定位的方法又一实施例的流程示意图。在当前实施例中,图6所示意的步骤S62之前还包括步骤S81,图6所示意的步骤S62包括步骤S82至步骤S84。在当前实施例中,该方法包括:
S81:采用利普希茨常数估计算法,得到xy平面的搜索数Nxy和z轴中的搜索数Nz。
在当前实施例中,可将路径优化的成本函数看作是关于无人机位置和目标位置的利普希茨连续函数。因此在当前实施例中采用利普希茨常数估计算法用于路径点搜索数的优化,进而得到xy平面的搜索数Nxy和z轴中的搜索数Nz。步骤S81的详细内容可进一步参见图10对应的内容。
S82:确定N个待定路径点,预估位于每个待定路径点的无人机测量得到的每个目标在下一采样时刻的预估方向数据。
在采用利普希茨常数估计算法,得到xy平面的搜索数Nxy和z轴中的搜索数Nz之后,进一步基于xy平面的搜索数Nxy和z轴中的搜索数Nz确定N个待定路径点。其中,N等于Nxy*Nz。然后利用上述图1至图4所述的内容得到每个目标在下一采样时刻的预估方向数据。在其他实施例中,步骤S82还可以包括如图9所示意的步骤S91和步骤S92。
S83:利用对应待定路径点的每个目标在下一采样时刻的预估方向数据,确定对应待定路径点的每个目标在xy平面的定位准确情况以及在z轴的定位准确情况。在其他实施例中,步骤S83还可以包括如图9所示意的步骤S93和步骤S94。
S84:由每个目标在xy平面的定位准确情况以及在z轴的定位准确情况,得到对应待定路径点的多个目标的定位准确情况。在其他实施例中,步骤S84还可以包括如图9所示意的步骤S95。
步骤S83和步骤S84可参见上文图5所对应的实施例部分的内容,具体在此不再赘述。
请参见图9,图9为本申请一种目标定位的方法又一实施例的流程示意图。在当前实施例中,本申请所提供的一种目标定位的方法包括:
S91:确定N个待定路径点。
在无人机获得每个目标在当前采样时刻的状态信息后,进一步确定N个待定路径点,确定N个待定路径点的方式可以是按照一定的规则随机选取,或者是确定待定路径点所在范围然后基于无人机飞行的特点进一步确定待定路径点。
S92:利用无人机当前位置到待定路径点的三维向量以及每个目标在当前采样时刻的状态信息,得到当无人机位于待定路径点时测量得到的每个目标在下一采样时刻的预估方向数据。
其中,每个目标在下一采样时刻的预估方向数据包括:每个目标在下一采样时刻在xy平面上的预估方向数据和每个目标在下一采样时刻在z轴方向的预估方向数据。再进一步地,每个目标下一采样时刻在xy平面上的预估方向数据至少包括预估方位角,每个目标在下一采样时刻在z轴方向的预估方向数据至少包括预估俯仰角。在其他实施例中,目标的预估方向数据也可以理解为目标的预测下一时刻的测量信息,预估方向数据可利用上述公式(3)和(4)求得。
S93:利用xy平面的伪线性卡尔曼滤波算法,对每个目标在下一采样时刻的预估方向数据进行处理,得到每个目标的在xy平面的状态准确度,对每个目标的在xy平面的状态准确度进行矩阵求迹,得到每个目标在xy平面的定位准确情况。其中,目标在xy平面的定位准确情况的函数关系体现为上述的成本函数。
在当前实施例中,可依据公式(30)求取每个目标的在xy平面的定位准确情况,具体地,公式(30)如下所述:
Jxy,i(rxyk)=tr(Wk|k,i) (30)
其中,Ji(rk)是指的是目标i对应的成本函数值,Jxy,i(rxyk)是指的是目标i在xy平面上的成本函数值,Jz,i(rzk)表示的是目标i在z轴方向上的成本函数值,tr(·)表示矩阵求迹,rxyz是在xy平面中无人机信息,而rzk是在第k个采样时刻无人机的z轴方向的位置信息,Wk|k,i是当前采样时刻目标i在xy平面上的准确度,Sk|k,i是当前采样时刻目标i在z轴方向上的准确度。
S94:利用z轴的伪线性卡尔曼滤波算法,对每个目标在下一采样时刻的预估方向数据进行处理,得到每个目标的在z轴的状态准确度,对每个目标的在z轴的状态准确度进行矩阵求迹,得到每个目标在z轴的定位准确情况。在当前实施例中,可依据公式(31)求取每个目标的在z轴的定位准确情况,具体地,公式(31)如下所述:
Jz,i(rzk)=tr(Sk|k,i) (31)
S95:将每个目标在xy平面的定位准确情况以及在z轴的定位准确情况求和,并将每个目标的和进行加权求和,得到对应待定路径点的多个目标的定位准确情况。由于成本函数是加性的,所以可直接利用公式(32)将每个目标在xy平面的定位准确情况和在z轴的定位准确情况相加求得该目标的定位准确情况,然后再利用公式(33)将当前无人机所定位的多个目标的成本函数相加求得多个目标的定位准确情况。
Ji(rk)=Jxy,i(rxyk)+Jz,i(rzk)=tr(Wk|k,i)+tr(Sk|k,i) (32)
S96:将定位准确情况符合预设条件的待定路径点作为无人机的下一路径点。
S97:控制无人机向下一路径点飞行,并在飞行过程中继续测量得到多个目标在下一采样时刻的方向数据,且利用多个目标在下一采样时刻的方向数据获得每个目标在下一采样时刻的状态信息。
步骤S96和步骤S97与图6所示意的步骤S63和步骤S64为相同步骤,具体可参见上文图6中对应部分的阐述,在此不再赘述。
进一步地,请参见图10,图10为本申请一种目标定位的方法再一实施例的流程示意图。在当前实施例中,图8中步骤S81包括:
S101:基于利普希茨常数估计算法求取目标利普希茨常数。为保证全局最优值的可达性,三维球面上的各个下一路径点之间的网格距离应小于等于其中,M是利普希茨常数,故需要得到一个准确的利普希茨常数。
进一步地,请参见图11,图11为本申请一种目标定位的方法又一实施例的流程示意图。在当前实施例中,详细阐述了目标利普希茨常数的求解过程,步骤S101进一步包括:
S111:在预设范围内随机选取L组测试路径点。
其中,每组测试路径点包括多个测试路径点,预设范围为俯仰角搜索范围和方位角搜索范围定义的区域。在当前实施例中,俯仰角的搜索范围方位角的搜索范围[-π,π]。/>
S112:计算出每个测试路径点对多个目标进行测量的定位准确情况,并由每个测试路径点的定位准确情况对测试路径点的位置求偏导。
步骤S112中计算出每个测试路径点对多个目标进行测量的定位准确情况的过程可参见上文公式(26)和(27),具体在此不再详细阐述。在求得每个测试路径点对多个目标进行测量的定位准确情况后,进一步基于公式(34)进行每个测试路径点的定位准确情况对测试路径点的位置求偏导。
其中,γl是每个测试路径点的定位准确情况对测试路径点的位置的偏导值,也可以理解为各个路径点的斜率。
S113:从每组测试路径点的偏导值中,选出一绝对值最大偏导值,以得到L个最大偏导值。如下述公式(35)所述,在求取得到偏导值之后,会进一步求取各个偏导值的绝对值,并选出绝对值最大偏导值输出为Ml,进而得到数据集M={M1,M2,...,MD},其中,sl表示的是路径点的位置信息,sl在当前实施例中至少包括路径点相对当前路径点的方位角和俯仰角。
S114:对L个最大偏导值拟合至反韦伯分布模型中。
在求得数据集M后,将数据集M中的最大值数据得到m,即由于对步骤S111至步骤S113中设定循环次数,从而获得数据集m={m1,m2,...,mL},然后将数据集{m1,m2,...,mL}拟合至反韦伯分布模型中。
S115:基于二次矩方法求取反韦伯分布的位置参数,并将位置参数输出为目标利普希茨常数。
在求取反韦伯分布的位置参数之后,将该位置参数输出为目标利普希茨常数M,然后基于所求取的目标利普希茨常数求取单位搜索距离,集执行图10中的步骤S102。
S102:根据目标利普希茨常数求取单位搜索距离。
进一步地,步骤S102包括:利用公式(36),计算得到单位搜索距离d。
其中,M为目标利普希茨常数,ε为预设定位精度值。
S103:将单位搜索距离转换为单位搜索俯仰角和单位搜索方位角。
S104:将俯仰角搜索范围与单位搜索俯仰角的商作为xy平面的搜索数Nxy,将方位角搜索范围与单位搜索方位角的商作为z轴中的搜索数Nz。其中,俯仰角搜索范围为无人机高度转弯率,方位角的搜索范围为无人机方位角转弯率。
本申请还提供一种目标定位的系统,请参见图12,图12为本申请一种目标定位的系统一实施例的结构示意图。目标定位的系统120包括处理器121,以及与处理器121连接的存储器122。其中,存储器122存储程序数据和处理器121工作执行的结果。而处理器121在运行所存储的程序数据时,用于执行如上述各个实施例中所述的一种目标定位的方法。其中,在一实施例中,本申请所提供的目标定位的系统120可以是直接装载在无人机上。在另一实施例中,本申请所提供的目标定位的系统120可以是装载在可以与无人机进行实时通讯的设备上,具体在此不一一列举。
本申请还提供一种无人机,请参见图13,图13为本申请一种无人机一实施例的结构示意图。本申请所提供的无人机130包括:传感组件133、驱动电路134和目标定位系统135,传感组件133、驱动电路134分别与目标定位系统135中的处理器131连接。
其中,传感组件133用于获取多个目标的方向数据,并反馈至目标定位系统135中的处理器131。进一步地,传感组件133包括AOA(Angle-of-arrival,目标方向角)传感器。
驱动电路134与处理器131连接,驱动电路134用于响应处理器131的控制指令飞至下一路径点。其中,下一路径点时基于上述图6至图11及其所对应的各个实施例中的任意一个确定。
存储器132用于存储程序数据。
目标定位系统135为如图12所示意的系统,目标定位系统135中的处理器131用于运行程序数据,以执行如图1至图11及其所对应的实施例中任一个所述的目标定位的方法,进而实现对多个目标进行定位。
本申请还提供一种存储介质,参见图14,图14为本申请一种存储介质一实施例结构示意图。该存储介质140存储有程序数据141,该程序数据141被执行时实现如上所述一种目标定位的方法。具体的,上述具有存储功能的存储介质140可以是存储器、个人计算机、服务器、网络设备,或者U盘等其中的一种。
以上所述仅为本申请的实施方式,并非因此限制本申请的专利范围,凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换,或直接或间接运用在其他相关的技术领域,均同理包括在本申请的专利保护范围内。
Claims (12)
1.一种目标定位的方法,其特征在于,所述方法包括:
获取由同一无人机测量得到的多个目标在当前采样时刻的方向数据;
利用三维伪线性卡尔曼滤波算法,对每个所述目标在当前采样时刻的方向数据进行处理,得到每个所述目标在当前采样时刻的状态信息;
基于利普希茨常数估计算法求取目标利普希茨常数;
根据所述目标利普希茨常数求取单位搜索距离;
将所述单位搜索距离转换为单位搜索俯仰角和单位搜索方位角;
将俯仰角搜索范围与所述单位搜索俯仰角的商作为xy平面的搜索数,将方位角搜索范围与所述单位搜索方位角的商作为z轴的搜索数;其中,所述俯仰角搜索范围为无人机高度转弯率,所述方位角搜索范围为无人机方位角转弯率;
基于所述xy平面的搜索数和所述z轴的搜索数确定若干待定路径点;
评估所述无人机分别在所述若干待定路径点对所述多个目标进行测量的定位准确情况;
将所述定位准确情况符合预设条件的所述待定路径点作为所述无人机的下一路径点;
控制所述无人机向所述下一路径点飞行,并在飞行过程中继续测量得到多个目标在下一采样时刻的方向数据,且利用所述多个目标在下一采样时刻的方向数据获得每个所述目标在下一采样时刻的状态信息。
2.根据权利要求1所述的方法,其特征在于,所述目标在当前采样时刻的方向数据包括在当前采样时刻测量得到的方位角和俯仰角;所述三维伪线性卡尔曼滤波算法包括xy平面的伪线性卡尔曼滤波算法和所述z轴的伪线性卡尔曼滤波算法;
所述利用三维伪线性卡尔曼滤波算法,对每个所述目标在当前采样时刻的方向数据进行处理,得到每个所述目标在当前采样时刻的状态信息,包括:
利用xy平面的伪线性卡尔曼滤波算法,对每个所述目标在当前采样时刻的方向数据进行处理,得到每个所述目标在当前采样时刻的所述xy平面的状态信息;以及
利用z轴的伪线性卡尔曼滤波算法,对每个所述目标在当前采样时刻的方向数据进行处理,得到每个所述目标在当前采样时刻的所述z轴的状态信息。
3.根据权利要求1所述的方法,其特征在于,所述利用xy平面/z轴的伪线性卡尔曼滤波算法,对每个所述目标在当前采样时刻的方向数据进行处理,得到每个所述目标在当前采样时刻的所述xy平面/z轴的状态信息,包括:
利用所述目标在前一采样时刻的最终状态信息,预测得到所述目标在当前采样时刻的预测状态信息;
对所述目标在当前采样时刻的方向数据进行处理,得到所述目标在当前采样时刻的测量状态信息,其中,对于所述目标在当前采样时刻的xy平面的测量状态信息是对所述目标的方向数据进行非线性处理得到;
基于所述目标在当前采样时刻的预测状态信息和所述测量状态信息,获得所述目标在当前采样时刻的所述最终状态信息。
4.根据权利要求3所述的方法,其特征在于,在所述基于所述目标在当前采样时刻的预测状态信息和所述测量状态信息,获得所述目标在当前采样时刻的所述最终状态信息之前,还包括:
利用所述目标在前一采样时刻的最终状态准确度,预测得到所述目标在当前采样时刻的预测状态准确度;
所述基于所述目标在当前采样时刻的预测状态信息和所述测量状态信息,获得所述目标在当前采样时刻的所述最终状态信息,包括:
获取所述目标在当前采样时刻的测量状态信息相对所述预测状态信息的误差;
获取所述目标与所述无人机之间的距离,并基于所述距离得到所述无人机对所述目标在当前采样时刻的测量准确度;
利用所述目标在当前采样时刻的测量状态信息、所述预测状态准确度、所述测量准确度,获得所述目标在当前采样时刻的测量状态信息的权重;
利用所述误差以及所述权重,获得所述目标在当前采样时刻的所述最终状态信息;
在所述基于所述目标在当前采样时刻的预测状态信息和所述测量状态信息,获得所述目标在当前采样时刻的所述最终状态信息之后,还包括:
基于所述目标在当前采样时刻的预测状态准确度、所述测量状态信息以及所述权重,获得所述目标在当前采样时刻的最终状态准确度。
5.根据权利要求4所述的方法,其特征在于,
对于xy平面,所述目标i在当前采样时刻k的xy平面的测量状态信息、测量准确度、与所述无人机之间的距离/>、所述误差/>、测量状态信息的权重/>具体由下面相应公式得到:
其中,所述和/>分别是当前采样时刻目标在x轴和y轴上的预测状态信息,所述/>和/>是当前采样时刻无人机在x轴和y轴上的位置信息,所述/>是当前采样时刻无人机在xy平面上的位置数据,权重/>为增益矩阵,所述/>为所述无人机在当前采样时刻k测量得到的所述目标i的方位角,所述/>为所述目标i测量方位角时的高斯白噪声的方差,所述/>为所述目标在当前采样时刻的预测状态信息,所述/>为所述目标在当前采样时刻的预测状态准确度。
6.根据权利要求5所述的方法,其特征在于,对于xy平面:
所述利用所述误差以及所述权重,获得所述目标在当前采样时刻的所述最终状态信息,包括:
利用下面公式,获得所述目标i在当前采样时刻k的xy平面的所述最终状态信息:
所述基于所述目标在当前采样时刻的预测状态准确度、所述测量状态信息以及所述权重,获得所述目标在当前采样时刻的最终状态准确度,包括:
利用下面公式,获得所述目标i在当前采样时刻k的xy平面的最终状态准确度:
其中,所述为协方差矩阵,所述I为单位矩阵。
7.根据权利要求4所述的方法,其特征在于,
对于z轴,所述目标i在当前采样时刻k的z轴的测量状态信息、测量准确度/>、与所述无人机之间的距离/>、所述误差/>、测量状态信息的权重/>具体由下面相应公式得到:
其中,所述为所述无人机测量得到的所述目标i的俯仰角,所述/>为当前采样时刻所述目标在z轴方向的预测状态信息,所述/>和/>是当前采样时刻无人机在x轴和y轴上的位置信息,所述/>为当前采样时刻所述无人机在z轴方向的位置信息,所述/>为当前采样时刻所述目标在x轴方向的最终状态信息,所述/>是所述目标i测量俯仰角时的高斯白噪声的方差,所述/>为所述无人机在当前采样时刻测量得到的所述目标i的俯仰角,所述/>为当前采样时刻目标在z轴方向上的所述预测状态准确度,所述/>为当前采样时刻目标在z轴方向上的预测状态信息。
8.根据权利要求7所述的方法,其特征在于,对于z轴:
所述利用所述误差以及所述权重,获得所述目标在当前采样时刻的所述最终状态信息,包括:
利用下面公式,获得所述目标i在当前采样时刻k的z轴的所述最终状态信息:
所述基于所述目标在当前采样时刻的预测状态准确度、所述测量状态信息以及所述权重,获得所述目标在当前采样时刻的最终状态准确度,包括:
利用下面公式,获得所述目标i在当前采样时刻k的z轴的最终状态准确度:
其中,所述I为单位矩阵。
9.根据权利要求4所述的方法,其特征在于,
对于xy平面,所述目标i在当前采样时刻k的xy平面的预测状态信息、所述目标i在前一采样时刻k-1的xy平面上的最终状态信息/>、所述目标做匀速直线运动的速度状态U、预测状态准确度/>具体由下面相应公式得到:
;
对于z轴,所述目标i在当前采样时刻k的z轴的预测状态信息、所述目标i基于前一采样时刻k-1的最终状态准确度/>、在k-1时刻的边程噪声的协方差矩阵/>、预测状态准确度/>具体由下面相应公式得到:
;
其中,所述是z轴方向上的运动转换矩阵,所述/>是系统过程噪声的协方差矩阵,所述/>是所述目标在前一采样时刻在z轴方向上的最终状态准确度,所述/>是所述目标在前一采样时刻的z轴方向上的最终状态信息。
10.一种目标定位系统,其特征在于,所述系统包括处理器以及与所述处理器连接的存储器;
其中,所述存储器用于存储程序数据;
所述处理器用于运行所述程序数据,以执行如权利要求1~9任一项所述的方法。
11.一种无人机,其特征在于,所述无人机包括:传感组件、驱动电路和目标定位系统,所述传感组件和驱动电路分别与所述目标定位的系统中的处理器连接;
其中,所述传感组件用于获取多个目标的方向数据;
所述驱动电路用于响应所述处理器的控制指令飞至下一路径点;
所述目标定位系统用于对所述多个目标进行定位,所述目标定位系统为如权利要求10所述的系统。
12.一种存储介质,其特征在于,所述存储介质存储有程序数据,所述程序数据被执行时实现如权利要求1~9任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910364492.5A CN110186456B (zh) | 2019-04-30 | 2019-04-30 | 一种目标定位的方法、系统、无人机及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910364492.5A CN110186456B (zh) | 2019-04-30 | 2019-04-30 | 一种目标定位的方法、系统、无人机及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110186456A CN110186456A (zh) | 2019-08-30 |
CN110186456B true CN110186456B (zh) | 2023-09-29 |
Family
ID=67715460
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910364492.5A Active CN110186456B (zh) | 2019-04-30 | 2019-04-30 | 一种目标定位的方法、系统、无人机及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110186456B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111459187B (zh) * | 2020-04-20 | 2021-09-07 | 清华大学 | 无人机状态监管方法、装置、系统及可读存储介质 |
CN112113557B (zh) * | 2020-07-29 | 2023-05-02 | 中国人民解放军军事科学院国防科技创新研究院 | 一种基于插值法的多无人机相对定位预测方法及装置 |
CN117817673B (zh) * | 2024-03-05 | 2024-05-03 | 泓浒(苏州)半导体科技有限公司 | 晶圆搬运机械臂动态路径调整系统及方法 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103713285A (zh) * | 2014-01-09 | 2014-04-09 | 西安电子科技大学 | 基于信息融合的分布式米波阵列雷达测角方法 |
CN107703526A (zh) * | 2016-08-08 | 2018-02-16 | 清华大学 | 基线测向方法、装置和系统 |
CN107783555A (zh) * | 2016-08-29 | 2018-03-09 | 杭州海康机器人技术有限公司 | 一种基于无人机的目标定位方法、装置及系统 |
RU2659810C1 (ru) * | 2017-04-07 | 2018-07-04 | федеральное государственное казенное военное образовательное учреждение высшего образования "Военная академия связи имени Маршала Советского Союза С.М. Буденного" Министерства обороны Российской Федерации | Способ и устройство определения координат источников радиоизлучения |
CN108828511A (zh) * | 2018-06-20 | 2018-11-16 | 西南电子技术研究所(中国电子科技集团公司第十研究所) | 全空域多波束协同的目标搜索方法 |
CN108919640A (zh) * | 2018-04-20 | 2018-11-30 | 西北工业大学 | 无人机自适应多目标跟踪的实现方法 |
JP2019027909A (ja) * | 2017-07-31 | 2019-02-21 | 株式会社Aerial Lab Industries | 無人移動体による対象物の位置推定方法 |
CN109540172A (zh) * | 2018-12-27 | 2019-03-29 | 中国船舶重工集团公司第七0研究所 | 一种用于水雷平台的目标运动参数估计方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7765062B2 (en) * | 2006-04-25 | 2010-07-27 | Honeywell International Inc. | Method and system for autonomous tracking of a mobile target by an unmanned aerial vehicle |
-
2019
- 2019-04-30 CN CN201910364492.5A patent/CN110186456B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103713285A (zh) * | 2014-01-09 | 2014-04-09 | 西安电子科技大学 | 基于信息融合的分布式米波阵列雷达测角方法 |
CN107703526A (zh) * | 2016-08-08 | 2018-02-16 | 清华大学 | 基线测向方法、装置和系统 |
CN107783555A (zh) * | 2016-08-29 | 2018-03-09 | 杭州海康机器人技术有限公司 | 一种基于无人机的目标定位方法、装置及系统 |
RU2659810C1 (ru) * | 2017-04-07 | 2018-07-04 | федеральное государственное казенное военное образовательное учреждение высшего образования "Военная академия связи имени Маршала Советского Союза С.М. Буденного" Министерства обороны Российской Федерации | Способ и устройство определения координат источников радиоизлучения |
JP2019027909A (ja) * | 2017-07-31 | 2019-02-21 | 株式会社Aerial Lab Industries | 無人移動体による対象物の位置推定方法 |
CN108919640A (zh) * | 2018-04-20 | 2018-11-30 | 西北工业大学 | 无人机自适应多目标跟踪的实现方法 |
CN108828511A (zh) * | 2018-06-20 | 2018-11-16 | 西南电子技术研究所(中国电子科技集团公司第十研究所) | 全空域多波束协同的目标搜索方法 |
CN109540172A (zh) * | 2018-12-27 | 2019-03-29 | 中国船舶重工集团公司第七0研究所 | 一种用于水雷平台的目标运动参数估计方法 |
Non-Patent Citations (5)
Title |
---|
3D pseudolinear Kalman filter with own-ship path optimization for AOA target tracking;Sheng Xu等;《2016 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP)》;20160519;第3136-3140页 * |
Dubins Orienteering Problem;Robert Pěnička;《IEEE Robotics and Automation Letters 》;全文 * |
K Dogancay等.Profile Likelihood Estimator for Passive Scan-Based Emitter Localization.《2007 IEEE International Conference on Acoustics, Speech and Signal Processing - ICASSP '07》.2007,第1117-1120页. * |
基于目标速度信息的海上编队目标选择技术;姜永华;但波;李敬军;;战术导弹技术(第01期);全文 * |
改进的渐消卡尔曼滤波在GPS动态定位中的应用;王虎;王解先;白贵霞;李浩军;;同济大学学报(自然科学版)(第01期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN110186456A (zh) | 2019-08-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110186456B (zh) | 一种目标定位的方法、系统、无人机及存储介质 | |
Vasconcelos et al. | Discrete-time complementary filters for attitude and position estimation: Design, analysis and experimental validation | |
Fang et al. | Graph optimization approach to range-based localization | |
CN110554376A (zh) | 用于运载工具的雷达测程法 | |
US20140139374A1 (en) | Kalman filtering with indirect noise measurements | |
CN110889808A (zh) | 一种定位的方法、装置、设备及存储介质 | |
CN110285800B (zh) | 一种飞行器集群的协同相对定位方法及系统 | |
Miller et al. | Underwater target tracking using bearing-only measurements | |
CN110220513B (zh) | 一种目标定位的方法、系统、无人机及存储介质 | |
CN110702110A (zh) | 一种基于无迹卡尔曼滤波的舰船升沉运动测量方法 | |
CN111103566A (zh) | 一种航向角确定方法、系统、电子设备及存储介质 | |
CN110908404B (zh) | 一种基于数据驱动的auv智能观测运动方法 | |
RU2232402C2 (ru) | Способ определения дальности до источников радиоизлучений и скорости сближения с ними в однопозиционных радиолокационных системах | |
CN111505573A (zh) | 一种分布式定位系统的航迹生成方法及装置 | |
CN114216463B (zh) | 一种路径优化目标定位方法及装置、存储介质、无人设备 | |
KR101862065B1 (ko) | 비행체를 이용한 영상 기반 바람 추정 장치 및 방법 | |
Shmatko et al. | Estimation of rotation measurement error of objects using computer simulation | |
CN113503891B (zh) | 一种sinsdvl对准校正方法、系统、介质及设备 | |
Manchester et al. | Method for optical-flow-based precision missile guidance | |
CN112230194B (zh) | 一种基于平移阵列的解模糊方法、设备及存储介质 | |
Vlastos et al. | Applied partitioned ordinary kriging for online updates for autonomous vehicles | |
Biswas et al. | Navigation system for a coral reef monitoring robot (C-bot) | |
Liu et al. | Consecutive tracking for ballistic missile based on bearings-only during boost phase | |
Li et al. | Observability analysis for cooperative navigation system in autonomous underwater vehicles | |
RU2758446C1 (ru) | Способ управления положением оси антенны бортовой радиолокационной станции при сопровождении меневрирующей воздушной цели |
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 |