CN113689501A - 一种基于收敛点的双机协同目标机定位跟踪控制方法 - Google Patents

一种基于收敛点的双机协同目标机定位跟踪控制方法 Download PDF

Info

Publication number
CN113689501A
CN113689501A CN202110985401.7A CN202110985401A CN113689501A CN 113689501 A CN113689501 A CN 113689501A CN 202110985401 A CN202110985401 A CN 202110985401A CN 113689501 A CN113689501 A CN 113689501A
Authority
CN
China
Prior art keywords
machine
target machine
double
machines
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.)
Granted
Application number
CN202110985401.7A
Other languages
English (en)
Other versions
CN113689501B (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.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
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 University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN202110985401.7A priority Critical patent/CN113689501B/zh
Publication of CN113689501A publication Critical patent/CN113689501A/zh
Application granted granted Critical
Publication of CN113689501B publication Critical patent/CN113689501B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • 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
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明公开了一种基于收敛点的双机协同目标机定位跟踪控制方法，包括以下步骤：S1、建立双机单目标机定位数学模型，基于机载雷达获取测角信息得到目标机的位置信息；S2、针对加性观测噪声对双机定位精度的影响，计算双机的测量方位角对定位精度的影响，进而求解得到双机测量方位角的最优配置；S3、建立目标机跟踪中存在的约束，求解收敛点；S4、设计基于收敛点的双机协同定位跟踪控制器。本发明提供了一种应用于复杂约束下双机单目标机定位跟踪的方法，在预测目标机位置的基础上计算两机收敛点，收敛点处双机能够满足相关约束且双机能同时探测到所有目标机，基于收敛点设计双机协同控制方案能够实现对双机的高精度定位。

Description

一种基于收敛点的双机协同目标机定位跟踪控制方法
技术领域
本发明属于目标机跟踪技术领域，具体涉及一种基于收敛点的双击协同目标机定位跟踪控制方法。
背景技术
目前，集群协同目标机跟踪有两个方面的难点，一个是跟踪目标机的不确定性，包括跟踪目标机的数量，目标机的运动方式和目标机的先验信息等，在实际应用场景中，目标机都是非合作的即无法直接获取目标机的位置，姿态等先验信息；另一个难点是执行任务所受的约束，例如，环境的约束可能影响跟踪器和目标机的机动性，还有通信距离的约束，传感器感知的约束等。
协同目标机跟踪问题离不开目标机跟踪协同控制，在执行跟踪任务时，观测机对目标机的持续跟踪和观测机的部署方位问题是协同控制需要重点解决的问题。由于空间中的目标机不是静止不动的，其位置会随着时间发生改变，为实现持续观测与定位，双机位置也需随着目标机位置的移动而移动。传统的方法是双机保持合理的基线长度，以初始状态持续运动，实现对非静止目标机的持续定位。然而，这种方法在目标机定位精度、定位误差收敛时间等方面具有明显的不足。双机若保持初始定位状态对目标机进行定位，双机相对目标机的视线夹角可能较小，由此得到的定位误差较大。因此，需要考虑双机的机动，为双机设计协同控制方案，实现对未知目标机的高精度观测定位。为实现持续定位，需要双机对目标机进行跟踪，即需要双机在此刻目标机位置信息的基础上，预测目标机下一阶段或者是下一时刻的位置信息。
发明内容
本发明的目的在于克服现有技术的不足，提供一种应用于复杂约束下双机单目标机定位跟踪的方法，在预测目标机位置的基础上计算两机收敛点，收敛点处双机能够满足相关约束且双机能同时探测到所有目标机，基于收敛点设计双机协同控制方案能够实现对双机的高精度定位。
本发明的目的是通过以下技术方案来实现的：一种基于收敛点的双机协同目标机定位跟踪控制方法，包括以下步骤：
S1、建立双机单目标机定位数学模型，双机基于机载雷达获取测角信息，对双机单目标机定位数学模型求解得到目标机的位置信息，并在此基础上采用随机加性噪声模型，得到双机单目标机定位的观测值信息；
S2、针对加性观测噪声对双机定位精度的影响，计算双机的测量方位角对定位精度的影响，进而求解得到双机测量方位角的最优配置；
S3、建立目标机跟踪中存在的约束，求解收敛点；
S4、设计基于收敛点的双机协同定位跟踪控制器，基于收敛点位置信息，控制双机朝向收敛点运动，实现定位精度快速收敛的目标机。
进一步地，所述步骤S1包括以下子步骤：
S11、基于测角信息和双机位置信息得到目标机在三维空间坐标系下的坐标分别为：
Figure BDA0003230492770000021
Figure BDA0003230492770000022
双机分别为长机和受控机，O1、O2分别为长机和受控机的位置，T表示目标机的位置；以受控机O2的位置为坐标原点，O1O2连线为x轴，竖直方向为z轴，建立三维坐标系；
Figure BDA0003230492770000026
为长机在三维坐标系下的横坐标，T1为目标机在xoy平面上的投影点；β1为O1O2与O1T1的夹角，β2为O1O2与O2T1的夹角，d为点T1到线段O1O2的距离；
目标机T的纵坐标为T与O1O2平面之间的高度差，即：
Figure BDA0003230492770000023
其中，θ1为O1T1与O1T的夹角，θ2为O2T1与O2T的夹角；
依据坐标转换关系，将目标机坐标转换到惯性坐标系，在已知双机在惯性坐标系下的位置O1＝[x1 y1 z1]T,O2＝[x2 y2 z1]T的情况下，获得目标机信息T＝[xt yt zt]T；
S12、令O1O2连线与惯性坐标系夹角为α，则
Figure BDA0003230492770000024
S13、利用O1O2连线与惯性坐标系夹角α，得到惯性坐标系到三维空间坐标系O2-xyz的转换矩阵
Figure BDA0003230492770000025
Figure BDA0003230492770000031
得到惯性坐标系下，目标机的位置为：
Figure BDA0003230492770000032
S14、在求解的目标机位置基础上采用随机加性噪声模型，进而得到双机单目标机定位下带有噪声的观测值信息。
进一步地，所述步骤S2包括以下子步骤：
S21、取位置估计误差函数为
Figure BDA0003230492770000033
其中，δ(xt)和δ(yt)分别表示目标机在x和y方向的估计误差；
S22、对
Figure BDA0003230492770000034
求期望：
Figure BDA0003230492770000035
其中，
Figure BDA0003230492770000036
和
Figure BDA0003230492770000037
分别为目标机在x和y方向上估计误差的方差，r1、r2分别为双机距目标机的距离，
Figure BDA0003230492770000038
分别为测量方位角的方差，γ为双机与目标机形成的视线夹角；
S23：若
Figure BDA0003230492770000039
即两架观测机测量方位角的方差相同，则
Figure BDA00032304927700000310
公式(9)的物理意义即为位置估计误差的平方的均值，如果观测机的配置使得该误差平方的均值最小，相应的估计精度也就达到了最优；根据(9)，目标机位置估计的误差大小与双机距离目标机的距离以及相对实线夹角有关，在传感器测量方差一定的情况下，最优的配置为
Figure BDA0003230492770000041
该求解结果表示当双机尽可能的靠近目标机，并保持π/2的视线夹角时即为最优配置。
进一步地，所述步骤S3中收敛点计算过程如下：定义双机的位置为O1＝[x1,y1],O2＝[x2,y2]，目标机的位置表示为Ot1＝[xt1,yt1]；虚拟轴线为目标机与双机连线中点的连线；选取目标机位置为坐标原点，地理东向为x轴的正方向，地理北向为y轴的正方向，建立一个基准坐标系；虚拟轴线与基准坐标系的y轴正方向所成的夹角为：
Figure BDA0003230492770000042
坐标转换的矩阵为：
Figure BDA0003230492770000043
以目标机为原点，目标机与双机的中点连线为y轴，得到双机在该坐标系的坐标分别为(xr1,yr1),(xr2,yr2)，经过坐标转换后的收敛点位置为：
Figure BDA0003230492770000045
其中(xc1,yc1),(xc2,yc2)为两个收敛点的坐标。
进一步地，所述步骤S4包括以下子步骤：
S41、将观测机的运动模型简化为二阶模型：
Figure BDA0003230492770000044
对于双机单目标机跟踪模型，i＝1，2，ui(k)、xi(k)和vi(k)分别表示k时刻双机的控制输入、位置和速度；双机单目标机跟踪的最终控制目标机为：limk→∞|xi(k)-xci(k)|＝0且limk→∞|yi(k)-yci(k)|＝0，其中i＝1，2，xi(k)和yi(k)分别表示双机的位置坐标，xci(k)和yci(k)表示实时求解的收敛点坐标；
S42、长机控制输入设计为
u1＝Γ11+Γ12+Γ13
其中
Figure BDA0003230492770000051
Figure BDA0003230492770000052
Figure BDA0003230492770000053
其中以目标机为中心的虚拟斥力势场对长机运动的作用以Γ11表示，式中通过对势场函数V1j(x1,xj)求负梯度
Figure BDA0003230492770000054
得到虚拟斥力，虚拟斥力使其始终运动在安全观测范围之内；Γ12为朝向收敛点运动的控制输入，式中N表示我方观测机的数量，K1和K2为控制系数，TP、TI和TD表示PID的控制增益，xo1(k)表示长机对应的收敛点位置，vo(k)表示目标机运动的速度；Γ13为双机一致性控制输入，式子中j≠i且j＝1或2，aij为双机组成的邻接矩阵，K3表示一致性控制增益；在期望位置控制与目标机势场的共同作用下，长机在运动过程中满足：朝向目标机收敛点运动，同时保持安全距离且框架角满足约束要求；
S43、受控机的运动需要满足以下几个条件：(1)受控机与目标机的距离大于最小安全距离，即受控机受到目标机虚拟斥力势场影响；(2)受控机朝向目标机收敛点运动；(3)长机、受控机之间达到一致性；
此时，受控机的位置控制输入设计为
u2＝Γ21+Γ22
其中
Figure BDA0003230492770000055
Figure BDA0003230492770000056
其由两部分组成，Γ21为目标机势场作用的控制输入，即对势场函数V2j(x2,xj)求负梯度
Figure BDA0003230492770000057
Γ22为朝向收敛点运动的控制输入，xo2(k)表示受控机对应的收敛点位置，vo(k)表示目标机运动的速度；
S44、考虑到观测机的性能约束，引入饱和约束算法，保证在机动性能约束范围内；饱和约束函数具体的表达式为：
Figure BDA0003230492770000061
其中i＝1,2，umax表示控制输入约束大小，K4为控制系数，最终目标机是使得双机的速度vi(k)≤vmax，vmax即为双机最大飞行速度，该约束即保证飞机处于最大机动性能约束下。
本发明的有益效果是：本发明提供了一种应用于复杂约束下双机单目标机定位跟踪的方法，在预测目标机位置的基础上计算两机收敛点，收敛点处双机能够满足相关约束且双机能同时探测到所有目标机，基于收敛点设计双机协同控制方案能够实现对双机的高精度定位。
附图说明
图1为本发明的基于收敛点的双机协同目标机定位跟踪控制方法的流程图；
图2为本发明的三维场景下双机三角定位原理图；
图3为本实施例中复杂约束条件下收敛点求解示意图；
图4为本实施例中基于收敛点的双机协同运动示意图。
具体实施方式
下面结合附图进一步说明本发明的技术方案。
如图1所示，本发明的一种基于收敛点的双机协同目标机定位跟踪控制方法，包括以下步骤：
S1、建立双机单目标机定位数学模型，即在三维地理坐标系下，两架我方观测机和目标机构成三角定位的数学模型；双机基于机载雷达获取测角信息，对双机单目标机定位数学模型求解得到目标机的位置信息，并在此基础上采用随机加性噪声模型，得到双机单目标机定位的观测值信息；包括以下子步骤：
S11、基于测角信息和双机位置信息得到目标机在三维空间坐标系下的坐标分别为：
Figure BDA0003230492770000062
Figure BDA0003230492770000063
双机分别为长机和受控机，O1、O2分别为长机和受控机的位置，T表示目标机的位置；以受控机O2的位置为坐标原点，O1O2连线为x轴，竖直方向为z轴，建立三维坐标系，如图2所示；
Figure BDA0003230492770000064
为长机在三维坐标系下的横坐标，T1为目标机在xoy平面上的投影点；β1为O1O2与O1T1的夹角，β2为O1O2与O2T1的夹角，d为点T1到线段O1O2的距离；
目标机T的纵坐标为T与O1O2平面之间的高度差，即：
Figure BDA0003230492770000071
其中，θ1为O1T1与O1T的夹角，θ2为O2T1与O2T的夹角；
因此，在已知角度θ1,θ2,β1,β2和双机基线长度Δx(即O1O2长度)的情况下，基于三角定位关系，可获得三维空间坐标系O2-xyz下的目标机位置。进一步，依据坐标转换关系，可以将目标机坐标转换到惯性坐标系，在已知双机在惯性坐标系下的位置O1＝[x1 y1 z1]T,O2＝[x2 y2 z1]T的情况下，可获得目标机信息T＝[xt yt zt]T；
S12、在惯性坐标系下，考虑二维平面上的O1,O2，如图2所示，令O1O2连线与惯性坐标系夹角为α，则
Figure BDA0003230492770000072
S13、利用O1O2连线与惯性坐标系夹角α，得到惯性坐标系到三维空间坐标系O2-xyz的转换矩阵
Figure BDA0003230492770000073
Figure BDA0003230492770000074
得到惯性坐标系下，目标机的位置为：
Figure BDA0003230492770000075
S14、考虑到机载雷达对目标机的测角信息会受到环境等因素的影响，会使得观测值和真实值之间存在误差，因此在求解的目标机位置基础上采用随机加性噪声模型，即在目标机真实位置信息基础上加入随机噪声来模拟真实观测信息，进而得到双机单目标机定位下带有噪声的目标机观测位置信息。
S2、针对加性观测噪声对双机定位精度的影响，计算双机的测量方位角对定位精度的影响，进而求解得到双机测量方位角的最优配置；包括以下子步骤：
S21、双机协同跟踪过程中，需要设计双机与目标机形成的视线夹角，达到最佳的探测性能；考虑到视线夹角对位置估计误差的影响，取位置估计误差函数为
Figure BDA0003230492770000081
其中，δ(xt)和δ(yt)分别表示目标机在x和y方向的估计误差，估计误差就是加入噪声后的观测值与真实值之间的差值；
S22、对
Figure BDA0003230492770000082
求期望：
Figure BDA0003230492770000083
其中，
Figure BDA0003230492770000084
和
Figure BDA0003230492770000085
分别为目标机在x和y方向上估计误差的方差，r1、r2分别为双机距目标机的距离，
Figure BDA0003230492770000086
分别为测量方位角的方差，γ为双机与目标机形成的视线夹角；
S23：若
Figure BDA0003230492770000087
即两架观测机测量方位角的方差相同，则
Figure BDA0003230492770000088
公式(9)的物理意义即为位置估计误差的平方的均值，如果观测机的配置可以使得该误差平方的均值最小，相应的估计精度也就达到了最优。根据(9)，目标机位置估计的误差大小与观测机距离目标机的距离以及相对实线夹角有关，在传感器测量方差一定的情况下，最优的配置为
Figure BDA0003230492770000089
其中，最优配置与双机与目标机的距离rmin有关，r1 *，
Figure BDA00032304927700000810
γ*分别表示最优配置下长机和受控机距离目标机的距离和双机与目标机形成的视线夹角的大小；该求解结果表示当两架观测机尽可能的靠近目标机，并保持π/2的视线夹角时即为最优配置。
S3、针对目标机跟踪中存在的诸多约束，对包括观测机动性能约束、通信拓扑变换、传感器测量约束等进行了详细的分析和建模，并完成一种双机协同跟踪路径求解方案设计，求解收敛点；包括以下子步骤：
S31、观测机动过程中，目标机不超过传感器探测框架角，即观测机执行目标机跟踪任务过程中可以在任何时刻对目标机进行观测，除此之外，考虑双机单目标机跟踪时的测量方位角，保证跟踪过程中处于最佳测量方位角范围；
S32、观测机动过程中，个体间距不超过机间链作用范围，即双机在跟踪过程中能够完成信息交互，同时也需要考虑观测机与目标机的距离大小，保证双机一直处于安全的观测距离；
S33、观测机动性能允许约束，即最大飞行速度，最大转弯半径转弯角，确保设计的控制器能够使得观测机的一直处于可接受的机动性能指标范围内。在满足上述约束的情况下，可设计双机协同算法实现对目标机的持续定位。
S34、如图3所示，双机为①，②，目标机为上方圆心中的点。定义双机与目标机形成的视线夹角为γ，定义虚拟轴线为目标机与双机连线中点的连线，且方向由目标机指向双机中点，定义虚拟轴线与目标机速度的夹角为λ，图中还标注了两个收敛点(两个收敛点与目标机所成的夹角为最大最佳夹角，且两收敛点与目标机的距离相同)，收敛点即根据S2得到的测量方位角最优解解算出的长机和受控机的最佳观测位置。虚线圆描述了双机相对于目标机的最小安全距离。从S2可知，当双机与目标机之间的视线夹角为90°且越靠近目标机时，双机协同定位测量误差最小，因此，双机运动控制目标机为两机视线夹角趋于90°，且两机与目标机的距离趋于一致。
S35、在这种方案中，一个基本假设是两机收敛点之间的距离(收敛点基线)处于通信范围约束dcom内最小安全距离的
Figure BDA0003230492770000091
倍，即
Figure BDA0003230492770000092
其中r12为收敛点距离，ds为双机与目标机之间的最小安全距离。
S36、若式(11)无法满足，则可以基于给定的双机通信范围以及最小安全距离，计算出一个最大可能实现的视线夹角，以此角度为期望视线夹角：
Figure BDA0003230492770000093
由于双机与目标机均是机动的，且安全距离也是时变的，因此最大视线夹角γ不一定能达到其最佳值(90°)，需要实时计算其可行较优值。根据预测的目标机下一步位置，基于相关双机通信范围、安全距离约束进行计算。
S37、收敛点计算过程如下：为了得到收敛点的位置，首先定义双机的位置为O1＝[x1,y1],O2＝[x2,y2]，目标机的位置可以表示为Ot1＝[xt1,yt1]；虚拟轴线为目标机与双机连线中点的连线；选取目标机位置为坐标原点，地理东向为x轴的正方向，地理北向为y轴的正方向，建立一个基准坐标系；虚拟轴线与基准坐标系的y轴正方向所成的夹角为：
Figure BDA0003230492770000101
坐标转换的矩阵为：
Figure BDA0003230492770000102
以目标机为原点，目标机与双机的中点连线为y轴，可以得到双机在该坐标系的坐标分别为(xr1,yr1),(xr2,yr2)，经过坐标转换后的收敛点位置为：
Figure BDA0003230492770000103
其中(xc1,yc1),(xc2,yc2)为两个收敛点的坐标。以上过程完成了对收敛点的求解。基于收敛点的双机协同运动如图4所示。
S4、设计基于收敛点的双机协同定位跟踪控制器，基于收敛点位置信息，控制双机朝向收敛点运动，实现定位精度快速收敛的目标机；包括以下子步骤：
S41、将观测机的运动模型简化为二阶模型：
Figure BDA0003230492770000104
对于双机单目标机跟踪模型，i＝1，2，ui(k)、xi(k)和vi(k)分别表示k时刻双机的控制输入、位置和速度；双机单目标机跟踪的最终控制目标机为：limk→∞|xi(k)-xci(k)|＝0且limk→∞|yi(k)-yci(k)|＝0，其中i＝1，2，xi(k)和yi(k)分别表示双机的位置坐标，xci(k)和yci(k)表示实时求解的收敛点坐标；
S42、如图4，其中①为长机，②为受控机。长机在运动过程中，两机均要与目标机保持安全距离，设安全距离为ds，基于胡克定律，以目标机位置为中心设计一个虚拟斥力势场，保证长机在运动过程中始终位于安全范围内；另外，长机也会受到目标机运动带来的影响，考虑到以上运动约束，提出基于PID的收敛点协同控制算法，其控制输入设计为
u1＝Γ11+Γ12+Γ13
其中
Figure BDA0003230492770000111
Figure BDA0003230492770000112
Figure BDA0003230492770000113
其中以目标机为中心的虚拟斥力势场对长机运动的作用以Γ11表示，式中通过对势场函数V1j(x1,xj)求负梯度
Figure BDA0003230492770000114
得到虚拟斥力，虚拟斥力使其始终运动在安全观测范围之内；Γ12为朝向收敛点运动的控制输入，式中N表示我方观测机的数量(本实施例中N＝2)，K1和K2为可调的控制系数，TP、TI和TD表示PID的控制增益，xo1(k)表示长机对应的收敛点位置，vo(k)表示目标机运动的速度；Γ13为双机一致性控制输入，式子中j≠i且j＝1或2，aij为双机组成的邻接矩阵，K3表示一致性控制增益；在期望位置控制与目标机势场的共同作用下，长机在运动过程中满足：朝向目标机收敛点运动，同时保持安全距离且框架角满足约束要求；
S43、受控机的运动需要满足以下几个条件：(1)受控机与目标机的距离大于最小安全距离，即受控机受到目标机虚拟斥力势场影响；(2)受控机朝向目标机收敛点运动；(3)长机、受控机之间达到一致性；
此时，受控机的位置控制输入设计为
u2＝Γ21+Γ22
其中
Figure BDA0003230492770000115
Figure BDA0003230492770000116
其由两部分组成，Γ21为目标机势场作用的控制输入，即对势场函数V2j(x2,xj)求负梯度
Figure BDA0003230492770000117
Γ22为朝向收敛点运动的控制输入，xo2(k)表示受控机对应的收敛点位置，vo(k)表示目标机运动的速度；
S44、考虑到观测机的性能约束，引入饱和约束算法，采用双曲正切函数，利用函数本身的值域对其进行限幅，全局可微，限幅函数为固定形式且削弱了饱和约束前的控制，保证在机动性能约束范围内；饱和约束函数具体的表达式为：
Figure BDA0003230492770000121
其中i＝1,2，umax表示控制输入约束大小，K4为可调的控制系数，最终目标机是使得双机的速度vi(k)≤vmax，vmax即为双机可以达到的最大飞行速度，该约束即可保证飞机处于最大机动性能约束下。
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (5)

1.一种基于收敛点的双机协同目标机定位跟踪控制方法，其特征在于，包括以下步骤：
S1、建立双机单目标机定位数学模型，双机基于机载雷达获取测角信息，对双机单目标机定位数学模型求解得到目标机的位置信息，并在此基础上采用随机加性噪声模型，得到双机单目标机定位的观测值信息；
S2、针对加性观测噪声对双机定位精度的影响，计算双机的测量方位角对定位精度的影响，进而求解得到双机测量方位角的最优配置；
S3、建立目标机跟踪中存在的约束，求解收敛点；
S4、设计基于收敛点的双机协同定位跟踪控制器，基于收敛点位置信息，控制双机朝向收敛点运动，实现定位精度快速收敛的目标机。
2.根据权利要求1所述的一种基于收敛点的双机协同目标机定位跟踪控制方法，其特征在于，所述步骤S1包括以下子步骤：
S11、基于测角信息和双机位置信息得到目标机在三维空间坐标系下的坐标分别为：
Figure FDA0003230492760000011
Figure FDA0003230492760000012
双机分别为长机和受控机，O1、O2分别为长机和受控机的位置，T表示目标机的位置；以受控机O2的位置为坐标原点，O1O2连线为x轴，竖直方向为z轴，建立三维坐标系；x1为长机在三维坐标系下的横坐标，T1为目标机在xoy平面上的投影点；β1为O1O2与O1T1的夹角，β2为O1O2与O2T1的夹角，d为点T1到线段O1O2的距离；
目标机T的纵坐标为T与O1O2平面之间的高度差，即：
Figure FDA0003230492760000013
其中，θ1为O1T1与O1T的夹角，θ2为O2T1与O2T的夹角；
依据坐标转换关系，将目标机坐标转换到惯性坐标系，在已知双机在惯性坐标系下的位置O1＝[x1 y1 z1]T,O2＝[x2 y2 z1]T的情况下，获得目标机信息T＝[xt yt zt]T；
S12、令O1O2连线与惯性坐标系夹角为α，则
Figure FDA0003230492760000021
S13、利用O1O2连线与惯性坐标系夹角α，得到惯性坐标系到三维空间坐标系O2-xyz的转换矩阵
Figure FDA0003230492760000022
Figure FDA0003230492760000023
得到惯性坐标系下，目标机的位置为：
Figure FDA0003230492760000024
S14、在求解的目标机位置基础上采用随机加性噪声模型，进而得到双机单目标机定位下带有噪声的观测值信息。
3.根据权利要求1所述的一种基于收敛点的双机协同目标机定位跟踪控制方法，其特征在于，所述步骤S2包括以下子步骤：
S21、取位置估计误差函数为
Figure FDA0003230492760000025
其中，δ(xt)和δ(yt)分别表示目标机在x和y方向的估计误差；
S22、对
Figure FDA0003230492760000026
求期望：
Figure FDA0003230492760000027
其中，
Figure FDA0003230492760000028
和
Figure FDA0003230492760000029
分别为目标机在x和y方向上估计误差的方差，r1、r2分别为双机距目标机的距离，
Figure FDA00032304927600000210
分别为测量方位角的方差，γ为双机与目标机形成的视线夹角；
S23：若
Figure FDA00032304927600000211
即两架观测机测量方位角的方差相同，则
Figure FDA0003230492760000031
公式(9)的物理意义即为位置估计误差的平方的均值，如果观测机的配置使得该误差平方的均值最小，相应的估计精度也就达到了最优；根据(9)，目标机位置估计的误差大小与双机距离目标机的距离以及相对实线夹角有关，在传感器测量方差一定的情况下，最优的配置为
Figure FDA0003230492760000032
该求解结果表示当双机尽可能的靠近目标机，并保持π/2的视线夹角时即为最优配置。
4.根据权利要求1所述的一种基于收敛点的双机协同目标机定位跟踪控制方法，其特征在于，所述步骤S3中，收敛点计算过程如下：定义双机的位置为O1＝[x1,y1],O2＝[x2,y2]，目标机的位置表示为Ot1＝[xt1,yt1]；虚拟轴线为目标机与双机连线中点的连线；选取目标机位置为坐标原点，地理东向为x轴的正方向，地理北向为y轴的正方向，建立一个基准坐标系；虚拟轴线与基准坐标系的y轴正方向所成的夹角为：
Figure FDA0003230492760000033
坐标转换的矩阵为：
Figure FDA0003230492760000034
以目标机为原点，目标机与双机的中点连线为y轴，得到双机在该坐标系的坐标分别为(xr1,yr1),(xr2,yr2)，经过坐标转换后的收敛点位置为：
Figure FDA0003230492760000035
其中(xc1,yc1),(xc2,yc2)为两个收敛点的坐标。
5.根据权利要求1所述的一种基于收敛点的双机协同目标机定位跟踪控制方法，其特征在于，所述步骤S4包括以下子步骤：
S41、将观测机的运动模型简化为二阶模型：
Figure FDA0003230492760000036
对于双机单目标机跟踪模型，i＝1，2，ui(k)、xi(k)和vi(k)分别表示k时刻双机的控制输入、位置和速度；双机单目标机跟踪的最终控制目标机为：limk→∞|xi(k)-xci(k)|＝0且limk→∞|yi(k)-yci(k)|＝0，其中i＝1，2，xi(k)和yi(k)分别表示双机的位置坐标，xci(k)和yci(k)表示实时求解的收敛点坐标；
S42、长机控制输入设计为
u1＝Γ11+Γ12+Γ13
其中
Figure FDA0003230492760000041
Figure FDA0003230492760000042
Figure FDA0003230492760000043
其中以目标机为中心的虚拟斥力势场对长机运动的作用以Γ11表示，式中通过对势场函数V1j(x1,xj)求负梯度
Figure FDA0003230492760000044
得到虚拟斥力，虚拟斥力使其始终运动在安全观测范围之内；Γ12为朝向收敛点运动的控制输入，式中N表示观测机的数量，K1和K2为控制系数，TP、TI和TD表示PID的控制增益，xo1(k)表示长机对应的收敛点位置，vo(k)表示目标机运动的速度；Γ13为双机一致性控制输入，式子中j≠i且j＝1或2，aij为双机组成的邻接矩阵，K3表示一致性控制增益；在期望位置控制与目标机势场的共同作用下，长机在运动过程中满足：朝向目标机收敛点运动，同时保持安全距离且框架角满足约束要求；
S43、受控机的运动需要满足以下几个条件：(1)受控机与目标机的距离大于最小安全距离，即受控机受到目标机虚拟斥力势场影响；(2)受控机朝向目标机收敛点运动；(3)长机、受控机之间达到一致性；
此时，受控机的位置控制输入设计为
u2＝Γ21+Γ22
其中
Figure FDA0003230492760000045
Figure FDA0003230492760000051
其由两部分组成，Γ21为目标机势场作用的控制输入，即对势场函数V2j(x2,xj)求负梯度
Figure FDA0003230492760000052
Γ22为朝向收敛点运动的控制输入，xo2(k)表示受控机对应的收敛点位置，vo(k)表示目标机运动的速度；
S44、考虑到观测机的性能约束，引入饱和约束算法，保证在机动性能约束范围内；饱和约束函数具体的表达式为：
Figure FDA0003230492760000053
其中i＝1,2，umax表示控制输入约束大小，K4为控制系数，最终目标机是使得双机的速度vi(k)≤vmax，vmax即为双机最大飞行速度，该约束即保证飞机处于最大机动性能约束下。
CN202110985401.7A 2021-08-26 2021-08-26 一种基于收敛点的双机协同目标机定位跟踪控制方法 Active CN113689501B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110985401.7A CN113689501B (zh) 2021-08-26 2021-08-26 一种基于收敛点的双机协同目标机定位跟踪控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110985401.7A CN113689501B (zh) 2021-08-26 2021-08-26 一种基于收敛点的双机协同目标机定位跟踪控制方法

Publications (2)

Publication Number Publication Date
CN113689501A true CN113689501A (zh) 2021-11-23
CN113689501B CN113689501B (zh) 2023-05-23

Family

ID=78582765

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110985401.7A Active CN113689501B (zh) 2021-08-26 2021-08-26 一种基于收敛点的双机协同目标机定位跟踪控制方法

Country Status (1)

Country Link
CN (1) CN113689501B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116774142A (zh) * 2023-06-13 2023-09-19 中国电子产业工程有限公司 一种非等高双机交叉定位中的坐标转换方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20070023582A1 (en) * 2005-07-01 2007-02-01 Steele Daniel W Unmanned air vehicle, integrated weapon platform, avionics system and control method
CN103499809A (zh) * 2013-09-27 2014-01-08 中国人民解放军空军工程大学 一种纯方位双机协同目标跟踪定位路径规划方法
CN106873628A (zh) * 2017-04-12 2017-06-20 北京理工大学 一种多无人机跟踪多机动目标的协同路径规划方法
CN110389651A (zh) * 2018-04-17 2019-10-29 罗克韦尔柯林斯公司 头部可佩戴装置、系统和方法
CN111273687A (zh) * 2020-02-17 2020-06-12 上海交通大学 基于gnss观测量和机间测距的多无人机协同相对导航方法
CN111427379A (zh) * 2020-04-19 2020-07-17 中国人民解放军海军航空大学 观测驱动的多无人机协同standoff目标跟踪方法
CN111596693A (zh) * 2020-06-17 2020-08-28 中国人民解放军国防科技大学 基于云台相机的无人机对地面目标跟踪控制方法及系统
WO2021018113A1 (zh) * 2019-07-31 2021-02-04 南京航空航天大学 用于无人机蜂群协同导航的动态互观测在线建模方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20070023582A1 (en) * 2005-07-01 2007-02-01 Steele Daniel W Unmanned air vehicle, integrated weapon platform, avionics system and control method
CN103499809A (zh) * 2013-09-27 2014-01-08 中国人民解放军空军工程大学 一种纯方位双机协同目标跟踪定位路径规划方法
CN106873628A (zh) * 2017-04-12 2017-06-20 北京理工大学 一种多无人机跟踪多机动目标的协同路径规划方法
CN110389651A (zh) * 2018-04-17 2019-10-29 罗克韦尔柯林斯公司 头部可佩戴装置、系统和方法
WO2021018113A1 (zh) * 2019-07-31 2021-02-04 南京航空航天大学 用于无人机蜂群协同导航的动态互观测在线建模方法
CN111273687A (zh) * 2020-02-17 2020-06-12 上海交通大学 基于gnss观测量和机间测距的多无人机协同相对导航方法
CN111427379A (zh) * 2020-04-19 2020-07-17 中国人民解放军海军航空大学 观测驱动的多无人机协同standoff目标跟踪方法
CN111596693A (zh) * 2020-06-17 2020-08-28 中国人民解放军国防科技大学 基于云台相机的无人机对地面目标跟踪控制方法及系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
OMAR MECHALI 等: "Observer-based fixed-time continuous nonsingular terminal sliding mode control of quadrotor aircraft under uncertainties and disturbances for robust trajectory tracking: Theory and experiment", 《CONTROL ENGINEERING PRACTICE》 *
WENFENG SONG 等: "Cross-View Contextual Relation Transferred Network for Unsupervised Vehicle Tracking in Drone Videos", 《THE WACV 2020 PAPER》 *
施孟佶 等: "高压输电线路多无人机自主协同巡线设计与测试", 《电力系统自动化》 *
陈 熙: "混合约束下多无人机协同目标 攻击占位控制方法研究", 《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116774142A (zh) * 2023-06-13 2023-09-19 中国电子产业工程有限公司 一种非等高双机交叉定位中的坐标转换方法
CN116774142B (zh) * 2023-06-13 2024-03-01 中国电子产业工程有限公司 一种非等高双机交叉定位中的坐标转换方法

Also Published As

Publication number Publication date
CN113689501B (zh) 2023-05-23

Similar Documents

Publication Publication Date Title
CN109634304A (zh) 无人机飞行路径规划方法、装置和存储介质
CN110068335A (zh) 一种gps拒止环境下无人机集群实时定位方法及系统
CN111813113B (zh) 仿生视觉自运动感知地图绘制方法、存储介质及设备
CN112504261B (zh) 一种基于视觉锚点的无人机降落位姿滤波估计方法及系统
CN111812978B (zh) 一种多无人机协作slam方法与系统
CN112000135B (zh) 基于人脸最高温点特征反馈的三轴云台视觉伺服控制方法
CN108303095B (zh) 适用于非高斯系统的鲁棒容积目标协同定位方法
CN105096341A (zh) 基于三焦张量和关键帧策略的移动机器人位姿估计方法
Yeromina et al. Method of reference image selection to provide high-speed aircraft navigation under conditions of rapid change of flight trajectory
CN111025229B (zh) 一种水下机器人纯方位目标估计方法
CN111121770B (zh) 一种交互式多弹多模型航迹融合方法
CN109655059B (zh) 一种基于θ-增量学习的视觉-惯性融合导航系统及方法
CN113689501B (zh) 一种基于收敛点的双机协同目标机定位跟踪控制方法
CN113076634B (zh) 一种多机协同无源定位方法、装置及系统
Barrile et al. Self-localization by laser scanner and GPS in automated surveys
Jiang et al. 3D SLAM based on NDT matching and ground constraints for ground robots in complex environments
Cai et al. Dynamic illumination optical flow computing for sensing multiple mobile robots from a drone
Cristofalo et al. Vision-based control for fast 3-d reconstruction with an aerial robot
CN113503891B (zh) 一种sinsdvl对准校正方法、系统、介质及设备
Boucheloukh et al. UAV navigation based on adaptive fuzzy backstepping controller using visual odometry
CN112034869B (zh) 一种无人机变参神经动力学控制器的设计方法及其应用
Xiaoqian et al. Nonlinear extended Kalman filter for attitude estimation of the fixed-wing UAV
CN113485385A (zh) 基于误差椭圆的uuv集群编队构型设计方法
Wu et al. 360-VIO: A Robust Visual–Inertial Odometry Using a 360° Camera
Chen et al. Kalman filter tracking algorithm simulation based on angle expansion

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