CN110285800B - 一种飞行器集群的协同相对定位方法及系统 - Google Patents

一种飞行器集群的协同相对定位方法及系统 Download PDF

Info

Publication number
CN110285800B
CN110285800B CN201910497499.4A CN201910497499A CN110285800B CN 110285800 B CN110285800 B CN 110285800B CN 201910497499 A CN201910497499 A CN 201910497499A CN 110285800 B CN110285800 B CN 110285800B
Authority
CN
China
Prior art keywords
aircraft
relative
cluster
vector
target
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
CN201910497499.4A
Other languages
English (en)
Other versions
CN110285800A (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.)
Central South University
Original Assignee
Central South 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 Central South University filed Critical Central South University
Priority to CN201910497499.4A priority Critical patent/CN110285800B/zh
Publication of CN110285800A publication Critical patent/CN110285800A/zh
Application granted granted Critical
Publication of CN110285800B publication Critical patent/CN110285800B/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
    • G01C1/00—Measuring angles
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明公开了一种飞行器集群的协同相对定位方法及系统，根据所述飞行器集群中各飞行器的待定坐标分别与所述各飞行器之间的相对距离、飞行器相对于目标的理论俯仰角以及理论方位角的几何关系构建出观测方程组，根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，以及获得的所述各飞行器与其他飞行器之间的相对距离测量值，利用观测方程组，经优化求解得到各飞行器的坐标位置。相比现有的相对定位方法，本发明所提出的方法简化了对飞行器测量设备配置的要求，更便于实现。并且本发明中采用飞行器间相互测距来实现相对定位，相比起现有技术，能获得更高的相对定位精度。

Description

一种飞行器集群的协同相对定位方法及系统
技术领域
本发明涉及飞行器的定位与导航领域，尤其涉及一种飞行器集群的协同相对定位方法及系统。
背景技术
无论在军用还是民用领域，单个飞行器执行任务的能力始终有限，而由多个飞行器相互协同组成集群飞行器则具有单个飞行器无法比拟的优势，如功能分布化、高效费比、大范围态势感知、高任务完成概率等。
集群飞行器的高精度运动协同，如精密编队飞行，离不开飞行器间高精度相对位置信息，而集群飞行器对非合作目标的协同制导攻击不但需要集群飞行器间的精确相对定位，还需要对非合作目标精确相对定位。仅依靠飞行器普遍使用的绝对导航器设备如惯性导航、卫星导航等，受限于绝对定位精度而不能达到高精度相对定位的要求。因此，高精度相对测量，是实现高精度相对定位的必要手段。
测距和测向是飞行器间相对测量的常用手段。利用飞行器间相对距离与相对角度测量，根据几何特性通过卡尔曼滤波估计各飞行器的相对位置信息已经得到了重视和研究。有研究利用飞行器相对距离与相对角度信息，提高飞行器惯导位置输出精度，减缓惯导定位误差发散速度。然而，当前的相对定位研究，都是采用完整测量，即对同一目标既测距又测向，获得完整的相对位置测量。在实际应用中，受平台设备限制，往往不能实现对同一目标既高精度测距又高精度测向。例如导弹对非合作目标往往只能测向，不能高精度测距；而不能实现对同一目标的高精度定位和高精度测距就不能实现对非合作目标的精准定位。
因此，如何实现对非合作目标精准定位已成为本领域技术人员亟待解决的技术问题。
发明内容
本发明提供了一种飞行器集群的协同相对定位方法及系统，用以解决现有的飞行器定位方法不能实现对同一目标精准定位的技术问题。
为解决上述技术问题，本发明提出的技术方案为：
一种飞行器集群的协同相对定位方法，包括以下步骤：
S1、利用安装在各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，并利用安装在各飞行器上的测距传感器获取飞行器集群中各飞行器与其他飞行器之间的相对距离测量值；
S2、构建出以目标为原点的三维空间坐标系，根据待定坐标分别与所述各飞行器之间的理论相对距离、理论俯仰角以及理论方位角几何关系，构建出反映所述待定坐标与所述各飞行器之间的相对距离测量值、俯仰角测量值以及方位角测量值的关系的观测方程组；
S3、根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，以及获得的所述各飞行器与其他飞行器之间的相对距离测量值，利用观测方程组，经优化求解得到各飞行器的坐标位置。
优选的，所述S2步骤中的观测方程组如下：
Figure GDA0003668605850000021
Figure GDA0003668605850000022
Figure GDA0003668605850000023
Figure GDA0003668605850000024
Figure GDA0003668605850000025
Figure GDA0003668605850000026
其中，(xi,yi,zi)为第i个飞行器在坐标系中的坐标位置，(xj,yj,zj)为第j个飞行器在坐标系中的坐标位置，
Figure GDA0003668605850000027
为第i个飞行器相对于目标的俯仰角的测量值，
Figure GDA0003668605850000028
为第i个飞行器相对于目标的方位角的测量值，
Figure GDA0003668605850000029
为第i个飞行器相对于第j个飞行器之间的相互距离的测量值，
Figure GDA00036686058500000210
为第i个飞行器相对于目标的俯仰角的测量误差，
Figure GDA00036686058500000211
为第i个飞行器相对于目标的方位角的测量误差，和vdij为第i个飞行器相对于第j个飞行器之间的相互距离的测量误差，θi为第i个飞行器相对于目标的理论俯仰角，
Figure GDA00036686058500000212
为第i个飞行器相对于目标的理论方位角，dij为第i个飞行器相对于第j个飞行器之间的理论相对距离，其中(i,j＝1,2,...,n；j≠i)。
优选的，所述S3步骤中利用静态相对定位方法来优化求解得到各飞行器的坐标位置，具体为：
根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角和方位角，以及获得的所述各飞行器与其他飞行器之间的相对距离，构建出最优化问题方程：
Figure GDA00036686058500000213
其中，
Figure GDA0003668605850000031
σθi为俯仰角测量误差的均方差值，
Figure GDA0003668605850000032
为方位角测量误差的均方差值，σdij为相对距离测量误差的均方差值，所述σθi、
Figure GDA00036686058500000311
以及σdij由测量设备的精度事先给定；gd为距离综合误差，
Figure GDA0003668605850000034
为角度综合误差，g为总误差。
利用数值优化方法求解出最优化问题方程中使总误差最小的各个飞行器的优化坐标位置(xi,yi,zi)，i＝1,2,...n。
优选的，所述S3步骤中利用动态相对定位方法来优化求解得到各飞行器的坐标位置，具体为：
根据飞行器集群相对目标的运动规律建立描述相对运动的系统状态方程；
根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角和方位角，以及获得的所述各飞行器与其他飞行器之间的相对距离构建出系统观测方程；
根据所获取的系统状态方程和系统观测方程构建扩展卡尔曼滤波方程组，将给定的相对运动状态估计初值
Figure GDA0003668605850000035
及其误差协方差矩阵P0/0初值代入建立的扩展卡尔曼滤波预测状态方程中获得各个飞行器相对于目标的优化坐标位置。
优选的，所述系统状态方程为：
Figure GDA0003668605850000036
其中f(X,t)为系统状态变化规律函数向量，W(t)为系统状态噪声向量；X为系统状态向量；
其中，系统状态向量为
Figure GDA0003668605850000037
其中上标T表示向量的转置，Xi为第i个飞行器相对目标的状态向量，
Figure GDA0003668605850000038
Figure GDA0003668605850000039
为第i个飞行器与非合作目标的相对速度在各坐标方向的分量，i＝1,2,...n。
其中，
Figure GDA00036686058500000310
优选的，所述系统观测方程为Z＝h(X)+V；其中，Z为观测向量，h(X)为观测函数向量，V为观测误差噪声向量；
优选的，所述观测向量
Figure GDA0003668605850000041
其中
Figure GDA0003668605850000042
表示所述飞行器集群中各飞行器相对于目标的俯仰角的测量值组成的向量，
Figure GDA0003668605850000043
Figure GDA0003668605850000044
表示所述飞行器集群中各飞行器相对于目标的方位角的测量值组成的向量，
Figure GDA0003668605850000045
Figure GDA0003668605850000046
表示所述飞行器集群中各飞行器之间的距离测量值组成的向量，
Figure GDA0003668605850000047
Figure GDA0003668605850000048
表示集群中第i个飞行器与其它编号大于i的所有飞行器间的距离测量值组成的向量，
Figure GDA0003668605850000049
所述观测函数向量
Figure GDA00036686058500000410
其中Θ表示所述飞行器集群中各飞行器相对于目标的俯仰角组成的向量，Θ＝[θ1,θ2,…,θn]T，Φ表示所述飞行器集群中各飞行器相对于目标的方位角组成的向量，
Figure GDA00036686058500000411
Δ表示所述飞行器集群中各飞行器之间的距离组成的向量，
Figure GDA00036686058500000412
Δi表示集群中第i个飞行器与其它编号大于i的所有飞行器间的距离组成的向量，Δi＝[di(i+1),di(i+2),…,din]T，i＝1,2,...,n-1；
所述观测误差噪声向量
Figure GDA00036686058500000413
其中，VΘ表示所述飞行器集群中各飞行器相对于目标的俯仰角测量的误差均方差组成的向量，VΘ＝[vθ1,vθ2,…,vθn]T，VΦ表示所述飞行器集群中各飞行器相对于目标的方位角测量的误差均方差组成的向量，
Figure GDA00036686058500000414
VΔ表示所述飞行器集群中各飞行器之间距离测量的误差均方差组成的向量，
Figure GDA00036686058500000415
VΔi表示集群中第i个飞行器与其它编号大于i的所有飞行器间的距离测量的误差均方差组成的向量，VΔi＝[vdi(i+1),vdi(i+2),…,vdin]T，i＝1,2,...,n-1。
优选的，所述扩展卡尔曼滤波方程组具体包括扩展卡尔曼滤波状态预测方程组和扩展卡尔曼滤波状态修正方程组：
其中，扩展卡尔曼滤波状态预测方程组为
Figure GDA0003668605850000051
Figure GDA0003668605850000052
Figure GDA0003668605850000053
Figure GDA0003668605850000054
扩展卡尔曼滤波状态修正方程组为：
Figure GDA0003668605850000055
Figure GDA0003668605850000056
Figure GDA0003668605850000057
Figure GDA0003668605850000058
Pk+1/k+1＝(I-Kk+1Hk+1)Pk+1/k；
其中，
Figure GDA0003668605850000059
Pk/k为tk时刻的状态估计值及其误差协方差矩阵值，
Figure GDA00036686058500000510
Pk+1/k为tk+1时刻的状态预测值及其误差协方差矩阵值，Q(t)为t时刻系统噪声方差阵，Rk+1为tk+1时刻的观测误差噪声向量V的方差阵，
Figure GDA00036686058500000511
为t时刻的状态估计值，
Figure GDA00036686058500000512
为系统状态变化规律函数向量，f(X,t)对系统状态向量的偏导数的雅克比矩阵；上标T为转置符号；Z(tk+1)为tk+1时刻的观测向量，Hk+1为tk+1时刻的观测函数向量h(X)对系统状态向量偏导数的雅克比矩阵，I为单位矩阵，Kk+1为tk+1时刻的滤波增益矩阵，P(t)为t时刻状态估计的方差阵。
优选的，观测方程导出观测函数向量对状态向量偏导数的雅克比矩阵为
Figure GDA00036686058500000513
其中，
Figure GDA00036686058500000514
Figure GDA00036686058500000515
Figure GDA0003668605850000061
Figure GDA0003668605850000062
Figure GDA0003668605850000063
Figure GDA0003668605850000064
Figure GDA0003668605850000065
Figure GDA0003668605850000066
Figure GDA0003668605850000067
Figure GDA0003668605850000068
Figure GDA0003668605850000069
Figure GDA00036686058500000610
Figure GDA00036686058500000611
Figure GDA00036686058500000612
Figure GDA00036686058500000613
Figure GDA00036686058500000614
一种计算机系统，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述任一所述方法的步骤。
与现有技术相比，本发明所具有的有益效果为：
1、本发明构造了一种飞行器集群的协同相对定位方法及系统。利用对目标测向，和集群飞行器间相互测距，进行融合处理，实现集群飞行器及目标间的协同相对定位。
2、本发明提出的一种飞行器集群的协同相对定位方法及系统，不需要对目标进行完整的相对位置测量，相比现有基于完整测量的定位方法，本发明所提出的方法简化了对飞行器测量设备配置的要求，更便于实现。
3、由于通常飞行器间相互测距精度远高于飞行器绝对位置导航精度，因此本发明基于飞行器间相互测距和对非合作目标测向的方案，比基于飞行器绝对位置导航和对非合作目标测向的方案，能获得更高的相对定位精度。
除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照附图，对本发明作进一步详细的说明。
附图说明
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是本发明优选实施例的一种飞行器集群的协同相对定位方法流程图；
图2为本发明集群飞行器对目标协同相对定位的不完整测量几何关系图；
图3为示例问题静态相对定位方法导弹1的相对定位误差曲线图；
图4为示例问题静态相对定位方法导弹2的相对定位误差曲线图；
图5为示例问题静态相对定位方法导弹3的相对定位误差曲线图；
图6为示例问题动态相对定位方法导弹1的相对定位误差曲线图；
图7为示例问题动态相对定位方法导弹2的相对定位误差曲线图；
图8为示例问题动态相对定位方法导弹3的相对定位误差曲线图。
具体实施方式
以下结合附图对本发明的实施例进行详细说明，但是本发明可以由权利要求限定和覆盖的多种不同方式实施。
实施例一：
如图1所示，本发明公开了一种飞行器集群的协同相对定位方法，包括以下步骤：
S1、利用安装在各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，并利用安装在各飞行器上的测距传感器获取飞行器集群中各飞行器与其他飞行器之间的相对距离测量值；
S2、构建出以目标为原点的三维空间坐标系，根据待定坐标分别与所述各飞行器之间的理论相对距离、理论俯仰角以及理论方位角几何关系，构建出反映所述待定坐标与所述各飞行器之间的相对距离测量值、俯仰角测量值以及方位角测量值的关系的观测方程组；
S3、根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，以及获得的所述各飞行器与其他飞行器之间的相对距离测量值，利用观测方程组，经优化求解得到各飞行器的坐标位置。
本发明中的飞行器集群的协同相对定位方法，根据所述待定坐标分别与所述各飞行器之间的理论相对距离、理论俯仰角以及理论方位角几何关系，构建出反映所述待定坐标与所述各飞行器之间的相对距离测量值、俯仰角测量值以及方位角测量值的关系的观测方程组。再将获取到的俯仰角测量值、方位角测量值以及相对距离测量值代到观测方程组中利用优化求解算法求解出坐标位置。
本发明中的飞行器集群的协同相对定位方法由于不需要对目标进行完整的相对位置测量，相比现有基于完整测量的定位方法，简化了对飞行器测量设备配置的要求，更便于实现。并且由于通常飞行器间相互测距精度远高于飞行器绝对位置导航精度，因此本发明基于飞行器间相互测距和对非合作目标测向的方案，比基于飞行器绝对位置导航和对非合作目标测向的方案，能获得更高的相对定位精度。
实施例二：
1、一种飞行器集群的协同相对定位方法，包括以下步骤：
S1、利用安装在各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，并利用安装在各飞行器上的测距传感器获取飞行器集群中各飞行器与其他飞行器之间的相对距离测量值；
S2、构建出以目标为原点的三维空间坐标系，根据所述待定坐标分别与所述各飞行器之间的理论相对距离、理论俯仰角以及理论方位角几何关系，构建出反映所述待定坐标与所述各飞行器之间的相对距离测量值、俯仰角测量值以及方位角测量值的关系的观测方程组；
如图2所示，本实施例中构建了以目标为原点O，正东方向为x轴，正北方向为y轴，竖直向上为z轴的坐标系。
其中，观测方程组如下：
Figure GDA0003668605850000091
Figure GDA0003668605850000092
Figure GDA0003668605850000093
Figure GDA0003668605850000094
Figure GDA0003668605850000095
Figure GDA0003668605850000096
其中，(xi,yi,zi)为第i个飞行器在坐标系中的坐标位置，(xj,yj,zj)为第j个飞行器在坐标系中的坐标位置，
Figure GDA0003668605850000097
为第i个飞行器相对于目标的俯仰角的测量值，
Figure GDA0003668605850000098
为第i个飞行器相对于目标的方位角的测量值，
Figure GDA0003668605850000099
为第i个飞行器相对于第j个飞行器之间的相互距离的测量值，
Figure GDA00036686058500000910
为第i个飞行器相对于目标的俯仰角的测量误差，
Figure GDA00036686058500000911
为第i个飞行器相对于目标的方位角的测量误差，和vdij为第i个飞行器相对于第j个飞行器之间的相互距离的测量误差，θi为第i个飞行器相对于目标的理论俯仰角，
Figure GDA00036686058500000912
为第i个飞行器相对于目标的理论方位角，dij为第i个飞行器相对于第j个飞行器之间的理论相对距离，其中i,j＝1,2,...,n；j≠i。
S3、根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，以及获得的所述各飞行器与其他飞行器之间的相对距离测量值，利用观测方程组，经静态相对定位优化求解得到各飞行器的坐标位置。
采用静态相对定位算法优化求解出坐标位置，具体为：
根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角和方位角，以及获得的所述各飞行器与其他飞行器之间的相对距离，构建出最优化问题方程：
Figure GDA00036686058500000913
其中，
Figure GDA00036686058500000914
σθi为俯仰角测量误差的均方差值，
Figure GDA0003668605850000101
为方位角测量误差的均方差值，σdij为相对距离测量误差的均方差值，所述σθi、
Figure GDA00036686058500001010
以及σdij由测量设备的精度事先给定；gd为距离综合误差，
Figure GDA0003668605850000103
为角度综合误差，g为总误差。
通过利用数值优化方法求解出最优化问题方程中使总误差最小的各个飞行器的优化坐标位置(xi,yi,zi)，i＝1,2,...n。
本实施例中还公开了一种计算机系统，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述的步骤。
实施例三：
一种飞行器集群的协同相对定位方法，包括以下步骤：
S1、利用安装在各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，并利用安装在各飞行器上的测距传感器获取飞行器集群中各飞行器与其他飞行器之间的相对距离测量值；
S2、构建出以目标为原点的三维空间坐标系，根据所述待定坐标分别与所述各飞行器之间的理论相对距离、理论俯仰角以及理论方位角几何关系，构建出反映所述待定坐标与所述各飞行器之间的相对距离测量值、俯仰角测量值以及方位角测量值的关系的观测方程组；
如图2所示，本实施例中构建了以目标为原点O，正东方向为x轴，正北方向为y轴，竖直向上为z轴的坐标系。
其中，观测方程组如下：
Figure GDA0003668605850000104
Figure GDA0003668605850000105
Figure GDA0003668605850000106
Figure GDA0003668605850000107
Figure GDA0003668605850000108
Figure GDA0003668605850000109
S3、根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，以及获得的所述各飞行器与其他飞行器之间的相对距离测量值，利用观测方程组，经动态相对定位算法优化求解得到各飞行器的坐标位置。
其中，利用观测方程组，经动态相对定位算法优化求解得到各飞行器的坐标位置，具体为：
根据飞行器集群相对目标的运动规律建立描述相对运动的系统状态方程，具体为：
构建出相对运动状态方程为：
Figure GDA0003668605850000111
其中，Xi为第i个飞行器相对目标的状态向量，fi(Xi,t)为第i个飞行器的状态变化规律函数，t为时间，Wi(t)为i个飞行器的模型噪声随机过程向量；
其中，
Figure GDA0003668605850000112
Figure GDA0003668605850000113
为第i个飞行器与非合作目标的相对速度在各坐标方向的分量，i＝1,2,...n；
根据构建出的相对运动状态方程构建出系统状态方程：
Figure GDA0003668605850000114
其中f(X,t)为系统状态变化规律函数向量，W(t)为系统状态噪声向量；X为系统状态向量；
其中，系统状态向量X为所有的飞行器的相对于目标的状态向量组成的向量，
Figure GDA0003668605850000115
其中上标T表示向量的转置；
Figure GDA0003668605850000116
根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角和方位角，以及获得的所述各飞行器与其他飞行器之间的相对距离构建出系统观测方程；
所述系统观测方程为Z＝h(X)+V；其中，Z为观测向量，h(X)为观测函数向量，V为观测误差噪声向量。
所述观测向量
Figure GDA0003668605850000117
其中
Figure GDA0003668605850000118
表示所述飞行器集群中各飞行器相对于目标的俯仰角的测量值组成的向量，
Figure GDA0003668605850000119
Figure GDA00036686058500001110
表示所述飞行器集群中各飞行器相对于目标的方位角的测量值组成的向量，
Figure GDA00036686058500001111
Figure GDA00036686058500001112
表示所述飞行器集群中各飞行器之间的距离测量值组成的向量，
Figure GDA00036686058500001113
Figure GDA00036686058500001114
表示集群中第i个飞行器与其它编号大于i的所有飞行器间的距离测量值组成的向量，
Figure GDA0003668605850000121
所述观测函数向量
Figure GDA0003668605850000122
其中Θ表示所述飞行器集群中各飞行器相对于目标的俯仰角组成的向量，Θ＝[θ1,θ2,…,θn]T，Φ表示所述飞行器集群中各飞行器相对于目标的方位角组成的向量，
Figure GDA0003668605850000123
Δ表示所述飞行器集群中各飞行器之间的距离组成的向量，
Figure GDA0003668605850000124
Δi表示集群中第i个飞行器与其它编号大于i的所有飞行器间的距离组成的向量，Δi＝[di(i+1),di(i+2),…,din]T，i＝1,2,...,n-1；
所述观测误差噪声向量
Figure GDA0003668605850000125
其中，VΘ表示所述飞行器集群中各飞行器相对于目标的俯仰角测量的误差均方差组成的向量，VΘ＝[vθ1,vθ2,…,vθn]T，VΦ表示所述飞行器集群中各飞行器相对于目标的方位角测量的误差均方差组成的向量，
Figure GDA0003668605850000126
VΔ表示所述飞行器集群中各飞行器之间距离测量的误差均方差组成的向量，
Figure GDA0003668605850000127
VΔi表示集群中第i个飞行器与其它编号大于i的所有飞行器间的距离测量的误差均方差组成的向量，VΔi＝[vdi(i+1),vdi(i+2),…,vdin]T，i＝1,2,...,n-1。
其中，扩展卡尔曼滤波方程组具体包括扩展卡尔曼滤波状态预测方程组和扩展卡尔曼滤波状态修正方程组：
扩展卡尔曼滤波状态预测方程组为
Figure GDA0003668605850000128
Figure GDA0003668605850000129
Figure GDA00036686058500001210
Figure GDA00036686058500001211
扩展卡尔曼滤波状态修正方程组为：
Figure GDA0003668605850000131
Figure GDA0003668605850000132
Figure GDA0003668605850000133
Figure GDA0003668605850000134
Pk+1/k+1＝(I-Kk+1Hk+1)Pk+1/k；
其中，
Figure GDA0003668605850000135
Pk/k为tk时刻的状态估计值及其误差协方差矩阵值，
Figure GDA0003668605850000136
Pk+1/k为tk+1时刻的状态预测值及其误差协方差矩阵值，Q(t)为t时刻系统噪声方差阵，Rk+1为tk+1时刻的观测误差噪声向量V的方差阵，
Figure GDA0003668605850000137
为t时刻的状态估计值，
Figure GDA0003668605850000138
为系统状态变化规律函数向量，f(X,t)对系统状态向量的偏导数的雅克比矩阵；上标T为转置符号；Z(tk+1)为tk+1时刻的观测向量，Hk+1为tk+1时刻的观测函数向量h(X)对系统状态向量偏导数的雅克比矩阵，I为单位矩阵，Kk+1为tk+1时刻的滤波增益矩阵，P(t)为t时刻状态估计的方差阵。
其中，观测方程导出观测函数向量对状态向量偏导数的雅克比矩阵为
Figure GDA0003668605850000139
其中，
Figure GDA00036686058500001310
Figure GDA00036686058500001311
Figure GDA00036686058500001312
Figure GDA00036686058500001313
Figure GDA0003668605850000141
Figure GDA0003668605850000142
Figure GDA0003668605850000143
Figure GDA0003668605850000144
Figure GDA0003668605850000145
Figure GDA0003668605850000146
Figure GDA0003668605850000147
Figure GDA0003668605850000148
Figure GDA0003668605850000149
Figure GDA00036686058500001410
Figure GDA00036686058500001411
Figure GDA00036686058500001412
其中，模型噪声为白噪声过程，其统计特性为：
E[Wi(t)]＝0,E[Wi(t)Wj(τ)T]＝δijQi(t)δ(t-τ),(i,j＝1,2,...n)
Figure GDA00036686058500001413
系统噪声方差阵为：
Figure GDA0003668605850000151
其中，V的方差阵R为
Figure GDA0003668605850000152
Figure GDA0003668605850000153
根据所获取的系统状态方程和系统观测方程构建扩展卡尔曼滤波方程组，将给定的相对运动状态估计初值
Figure GDA0003668605850000154
及其误差协方差矩阵P0/0初值代入建立的扩展卡尔曼滤波预测状态方程中获得各个飞行器相对于目标的优化坐标位置。具体为：
获取各飞行器在规定的离散时刻开展持续的角度和距离测量，记测量时刻为t0,t1,t2,…,tk,tk+1,…，将给定的相对运动状态估计初值
Figure GDA0003668605850000155
及其误差协方差矩阵P0/0值代入建立的扩展卡尔曼滤波预测状态方程中，预测出各个飞行器下个时刻的相对运动状态估计值；
并将预测出的各个飞行器的下个时刻的相对运动状态估计值代入到扩展卡尔曼滤波预测状态修正方程中，获得修正过的各个飞行器的下个时刻的相对运动状态估计值，并根据各个飞行器的相对运动状态估计值得到各个飞行器相对于目标的优化坐标。
本实施例中还公开了一种计算机系统，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述的步骤。
实施例四：
如图2至图8所示，将本发明的飞行器集群的协同相对定位方法利用到导弹集群协同攻击目标中，导弹集群通过弹间测距和导引头对目标测向来实现导弹对目标协同相对定位的示例问题，给出本发明方法的应用结果。考虑三个导弹的初始位置坐标分别为(4800m，4800m，1500m)，(4800m，6200m，600m)，(6200m，4800m，600m)，非合作目标在坐标原点(0m，0m，0m)。弹群以相同的速度25m/s朝非合作目标飞行。设导弹的GPS位置测量误差标准差δ＝10m，方位角和俯仰角测量误差标准差
Figure GDA0003668605850000161
测距的测量标准差为δd＝0.1m。
对导弹群从初始位置到最后达到目标这个过程中的协同相对定位开展仿真。仿真时间为300s，采样周期为1s。将本发明提出的基于导弹间测距和对目标测向的相对定位结果与靠导弹自身GPS+对目标测向的相对定位结果与进行对比。对比结果如附图3至附图8中所示，可以很清楚的看出本方案中的基于导弹间测距和对目标测向的相对定位结果比现有的导弹自身GPS+对目标测向的相对定位结果更加精确。
综上所述，本发明中的飞行器集群的协同相对定位方法及系统利用对目标测向，和集群飞行器间相互测距，进行融合处理，从而实现集群飞行器及目标间的协同相对定位。由于不需要对目标进行完整的相对位置测量，相比现有基于完整测量的定位方法，本发明所提出的方法简化了对飞行器测量设备配置的要求，更便于实现。并且由于通常飞行器间相互测距精度远高于飞行器绝对位置导航精度，因此本发明基于飞行器间相互测距和对非合作目标测向的方案，比基于飞行器绝对位置导航和对非合作目标测向的方案，能获得更高的相对定位精度。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种飞行器集群的协同相对定位方法，其特征在于，包括以下步骤：
S1、利用安装在各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，并利用安装在各飞行器上的测距传感器获取飞行器集群中各飞行器与其他飞行器之间的相对距离测量值；
S2、构建出以目标为原点的三维空间坐标系，根据待定坐标分别与所述各飞行器之间的理论相对距离、理论俯仰角以及理论方位角几何关系，构建出反映所述待定坐标与所述各飞行器之间的相对距离测量值、俯仰角测量值以及方位角测量值的关系的观测方程组；所述观测方程组如下：
Figure FDA0003668605840000011
Figure FDA0003668605840000012
Figure FDA0003668605840000013
Figure FDA0003668605840000014
Figure FDA0003668605840000015
Figure FDA0003668605840000016
其中，(xi,yi,zi)为第i个飞行器在坐标系中的坐标位置，(xj,yj,zj)为第j个飞行器在坐标系中的坐标位置，
Figure FDA0003668605840000017
为第i个飞行器相对于目标的俯仰角的测量值，
Figure FDA0003668605840000018
为第i个飞行器相对于目标的方位角的测量值，
Figure FDA0003668605840000019
为第i个飞行器相对于第j个飞行器之间的相互距离的测量值，
Figure FDA00036686058400000110
为第i个飞行器相对于目标的俯仰角的测量误差，
Figure FDA00036686058400000111
为第i个飞行器相对于目标的方位角的测量误差，和vdij为第i个飞行器相对于第j个飞行器之间的相互距离的测量误差，θi为第i个飞行器相对于目标的理论俯仰角，
Figure FDA00036686058400000112
为第i个飞行器相对于目标的理论方位角，dij为第i个飞行器相对于第j个飞行器之间的理论相对距离，其中i,j＝1,2,...,n；j≠i；
S3、根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，以及获得的所述各飞行器与其他飞行器之间的相对距离测量值，利用观测方程组，经优化求解得到各飞行器的坐标位置。
2.根据权利要求1所述的一种飞行器集群的协同相对定位方法，其特征在于，所述S3步骤中利用静态相对定位方法来优化求解得到各飞行器的坐标位置，具体为：
根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角和方位角，以及获得的所述各飞行器与其他飞行器之间的相对距离，构建出最优化问题方程：
Figure FDA0003668605840000021
其中，
Figure FDA0003668605840000022
Figure FDA0003668605840000023
σθi为俯仰角测量误差的均方差值，
Figure FDA0003668605840000024
为方位角测量误差的均方差值，σdij为相对距离测量误差的均方差值，所述σθi、
Figure FDA0003668605840000025
以及σdij由测量设备的精度事先给定；gd为距离综合误差，
Figure FDA0003668605840000026
为角度综合误差，g为总误差；
利用数值优化方法求解出最优化问题方程中使总误差最小的各个飞行器的优化坐标位置(xi,yi,zi)，i＝1,2,...n。
3.根据权利要求1所述的一种飞行器集群的协同相对定位方法，其特征在于，所述S3步骤中利用动态相对定位方法来优化求解得到各飞行器的坐标位置，具体为：
根据飞行器集群相对目标的运动规律建立描述相对运动的系统状态方程；
根据获取的所述各飞行器上的测角传感器获取飞行器集群中各飞行器相对于目标的俯仰角测量值和方位角测量值，以及获得的所述各飞行器与其他飞行器之间的相对距离测量值构建出系统观测方程；
根据所获取的系统状态方程和系统观测方程构建扩展卡尔曼滤波方程组，将给定的相对运动状态估计初值
Figure FDA0003668605840000029
及其误差协方差矩阵P0/0初值代入建立的扩展卡尔曼滤波预测状态方程中获得各个飞行器相对于目标的优化坐标位置。
4.根据权利要求3所述的一种飞行器集群的协同相对定位方法，其特征在于，
所述系统状态方程为：
Figure FDA0003668605840000027
其中f(X,t)为系统状态变化规律函数向量，W(t)为系统状态噪声向量；X为系统状态向量；
其中，
Figure FDA0003668605840000028
其中上标T表示向量的转置，Xi为第i个飞行器相对目标的状态向量，
Figure FDA0003668605840000031
Figure FDA0003668605840000032
为第i个飞行器与非合作目标的相对速度在各坐标方向的分量，i＝1,2,...n。
5.根据权利要求3所述的一种飞行器集群的协同相对定位方法，其特征在于，所述系统观测方程为Z＝h(X)+V；其中，Z为观测向量，h(X)为观测函数向量，V为观测误差噪声向量。
6.根据权利要求5所述的一种飞行器集群的协同相对定位方法，其特征在于，
所述观测向量
Figure FDA0003668605840000033
其中
Figure FDA0003668605840000034
表示所述飞行器集群中各飞行器相对于目标的俯仰角的测量值组成的向量，
Figure FDA0003668605840000035
Figure FDA0003668605840000036
表示所述飞行器集群中各飞行器相对于目标的方位角的测量值组成的向量，
Figure FDA0003668605840000037
Figure FDA0003668605840000038
表示所述飞行器集群中各飞行器之间的距离测量值组成的向量，
Figure FDA0003668605840000039
Figure FDA00036686058400000310
表示集群中第i个飞行器与其它编号大于i的所有飞行器间的距离测量值组成的向量，
Figure FDA00036686058400000311
所述观测函数向量
Figure FDA00036686058400000312
其中Θ表示所述飞行器集群中各飞行器相对于目标的俯仰角组成的向量，Θ＝[θ1,θ2,…,θn]T，Φ表示所述飞行器集群中各飞行器相对于目标的方位角组成的向量，
Figure FDA00036686058400000313
△表示所述飞行器集群中各飞行器之间的距离组成的向量，
Figure FDA00036686058400000314
△i表示集群中第i个飞行器与其它编号大于i的所有飞行器间的距离组成的向量，△i＝[di(i+1),di(i+2),…,din]T，i＝1,2,...,n-1；
所述观测误差噪声向量
Figure FDA00036686058400000315
其中，VΘ表示所述飞行器集群中各飞行器相对于目标的俯仰角测量的误差均方差组成的向量，VΘ＝[vθ1,vθ2,…,vθn]T，VΦ表示所述飞行器集群中各飞行器相对于目标的方位角测量的误差均方差组成的向量，
Figure FDA00036686058400000316
V△表示所述飞行器集群中各飞行器之间距离测量的误差均方差组成的向量，
Figure FDA00036686058400000317
V△i表示集群中第i个飞行器与其它编号大于i的所有飞行器间的距离测量的误差均方差组成的向量，V△i＝[vdi(i+1),vdi(i+2),…,vdin]T，i＝1,2,...,n-1。
7.根据权利要求3所述的一种飞行器集群的协同相对定位方法，其特征在于，所述扩展卡尔曼滤波方程组具体包括扩展卡尔曼滤波状态预测方程组和扩展卡尔曼滤波状态修正方程组：
其中，扩展卡尔曼滤波状态预测方程组为
Figure FDA0003668605840000041
Figure FDA0003668605840000042
Figure FDA0003668605840000043
Figure FDA0003668605840000044
扩展卡尔曼滤波状态修正方程组为：
Figure FDA0003668605840000045
Figure FDA0003668605840000046
Figure FDA0003668605840000047
Figure FDA0003668605840000048
Pk+1/k+1＝(I-Kk+1Hk+1)Pk+1/k；
其中，
Figure FDA0003668605840000049
Pk/k为tk时刻的状态估计值及其误差协方差矩阵值，
Figure FDA00036686058400000410
Pk+1/k为tk+1时刻的状态预测值及其误差协方差矩阵值，Q(t)为t时刻系统噪声方差阵，Rk+1为tk+1时刻的观测误差噪声向量V的方差阵，
Figure FDA00036686058400000411
为t时刻的状态估计值，
Figure FDA00036686058400000412
为系统状态变化规律函数向量，f(X,t)对系统状态向量的偏导数的雅克比矩阵；上标T为转置符号；Z(tk+1)为tk+1时刻的观测向量，Hk+1为tk+1时刻的观测函数向量h(X)对系统状态向量偏导数的雅克比矩阵，I为单位矩阵，Kk+1为tk+1时刻的滤波增益矩阵，P(t)为t时刻状态估计的方差阵。
8.根据权利要求7所述的一种飞行器集群的协同相对定位方法，其特征在于，观测方程导出观测函数向量对状态向量偏导数的雅克比矩阵为
Figure FDA00036686058400000413
其中，
Figure FDA0003668605840000051
Figure FDA0003668605840000052
Figure FDA0003668605840000053
Figure FDA0003668605840000054
Figure FDA0003668605840000055
Figure FDA0003668605840000056
Figure FDA0003668605840000057
Figure FDA0003668605840000058
Figure FDA0003668605840000059
Figure FDA00036686058400000510
Figure FDA00036686058400000511
Figure FDA00036686058400000512
Figure FDA0003668605840000061
Figure FDA0003668605840000062
Figure FDA0003668605840000063
Figure FDA0003668605840000064
9.一种计算机系统，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现上述权利要求1至8任一所述方法的步骤。
CN201910497499.4A 2019-06-10 2019-06-10 一种飞行器集群的协同相对定位方法及系统 Active CN110285800B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910497499.4A CN110285800B (zh) 2019-06-10 2019-06-10 一种飞行器集群的协同相对定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910497499.4A CN110285800B (zh) 2019-06-10 2019-06-10 一种飞行器集群的协同相对定位方法及系统

Publications (2)

Publication Number Publication Date
CN110285800A CN110285800A (zh) 2019-09-27
CN110285800B true CN110285800B (zh) 2022-08-09

Family

ID=68003543

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910497499.4A Active CN110285800B (zh) 2019-06-10 2019-06-10 一种飞行器集群的协同相对定位方法及系统

Country Status (1)

Country Link
CN (1) CN110285800B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110764530B (zh) * 2019-11-06 2022-10-28 中国电子科技集团公司电子科学研究院 无人机集群协同探测队形确定方法、装置和存储介质
CN110849377B (zh) * 2019-11-20 2021-10-29 上海航天控制技术研究所 一种星群协同的空间目标相对导航系统及方法
CN112066978A (zh) * 2020-09-17 2020-12-11 西安思丹德信息技术有限公司 一种适用于机载设备的地面目标主动定位方法与装置
CN113340271B (zh) * 2021-06-29 2022-09-06 上海良相智能化工程有限公司 一种基于无人机微群的地面目标定位误差实时估计方法
CN114234982B (zh) * 2021-12-20 2024-04-16 中南大学 基于方位角定位的三维轨迹规划方法、系统、设备及介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8063825B1 (en) * 2009-05-07 2011-11-22 Chun Yang Cooperative position location via wireless data link using broadcast digital transmissions
CN107121666A (zh) * 2017-04-17 2017-09-01 南京航空航天大学 一种基于无人飞行器的临近空间运动目标定位方法
CN107727079A (zh) * 2017-11-30 2018-02-23 湖北航天飞行器研究所 一种微小型无人机全捷联下视相机的目标定位方法
CN108151737A (zh) * 2017-12-19 2018-06-12 南京航空航天大学 一种动态互观测关系条件下的无人机蜂群协同导航方法
CN109708629A (zh) * 2018-11-15 2019-05-03 南京航空航天大学 一种用于差异定位性能条件的飞行器集群协同导航方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8063825B1 (en) * 2009-05-07 2011-11-22 Chun Yang Cooperative position location via wireless data link using broadcast digital transmissions
CN107121666A (zh) * 2017-04-17 2017-09-01 南京航空航天大学 一种基于无人飞行器的临近空间运动目标定位方法
CN107727079A (zh) * 2017-11-30 2018-02-23 湖北航天飞行器研究所 一种微小型无人机全捷联下视相机的目标定位方法
CN108151737A (zh) * 2017-12-19 2018-06-12 南京航空航天大学 一种动态互观测关系条件下的无人机蜂群协同导航方法
CN109708629A (zh) * 2018-11-15 2019-05-03 南京航空航天大学 一种用于差异定位性能条件的飞行器集群协同导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Synergy Decision for Radar and IRST Data Fusion;Dou Lihua 等;《Journal of Beijing Institute of Technology》;20021231;第11卷(第3期);第229-233页 *
三飞行体间欠冗余相对定位;江涛等;《系统工程与电子技术》;20110630;第33卷(第06期);第1346-1351页 *
两弹协同定位的可行性研究;彭锐晖等;《系统仿真学报》;20060531;第18卷(第05期);第1118-1122页 *

Also Published As

Publication number Publication date
CN110285800A (zh) 2019-09-27

Similar Documents

Publication Publication Date Title
CN110285800B (zh) 一种飞行器集群的协同相对定位方法及系统
Taylor et al. Comparison of two image and inertial sensor fusion techniques for navigation in unmapped environments
CN108827343B (zh) 一种基于试验设计与进化寻优的制导工具误差辨识方法
CN107015199A (zh) 一种考虑无人机姿态角的双无人机测向时差定位方法
CN111238469A (zh) 一种基于惯性/数据链的无人机编队相对导航方法
Huang et al. Design and performance analysis of landmark-based INS/Vision Navigation System for UAV
Haoran et al. Optimal observation configuration of UAVs based on angle and range measurements and cooperative target tracking in three-dimensional space
Carroll et al. Terrain‐referenced navigation using a steerable‐laser measurement sensor
Kaniewski et al. Ballistic target tracking with use of cinetheodolites
CN110220513B (zh) 一种目标定位的方法、系统、无人机及存储介质
Liu et al. Multi-UAV cooperative navigation algorithm based on federated filtering structure
Qu et al. Cooperative localization based on the azimuth angles among multiple UAVs
Kauffman et al. Simulation study of UWB-OFDM SAR for navigation with INS integration
Xiaoqian et al. Nonlinear extended Kalman filter for attitude estimation of the fixed-wing UAV
Liu et al. Multi-robot cooperative localization with range-only measurement by UWB
Lu et al. Analysis and application of geometric dilution of precision based on altitude-assisted INS/SAR integrated navigation
Song et al. Relative Positioning Method for UAVs Based on Multi‐Source Information Fusion
Gong et al. Unscented particle smoother and its application to transfer alignment of airborne distributed POS
Sotnikov et al. Calculating method of error calculations of the object coordination by means of conducting platform free inertial navigation systems of a unmanned aerial vehicle
Zeng et al. INS error correction method based on passive radar angle tracking information
Huang et al. Study on multipoint-ranging-target-positioning technologies for UAV
Wu et al. Target Three-Dimensional Localization and Error Analysis for Unmanned Aerial Vehicles
US20230026000A1 (en) Particle filtering method and navigation system using measurement correlation
Wu et al. Modified sage-husa adaptive filter based three-dimensional localization for moving target using two UAVs
Yun et al. A normalized measurement vector model for enhancing localization performance of 6-dof bearing-only slam

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