系统工程与电子技术Systems Engineering and ElectronicsISSN 1001-506X,CN 11-2422/TN《系统工程与电子技术》网络首发论文题目： 基于辅助信标的无人机协同目标跟踪作者： 史浩然，卢发兴，祁江鑫，杨光收稿日期： 2021-06-01网络首发日期： 2022-01-18引用格式： 史浩然，卢发兴，祁江鑫，杨光．基于辅助信标的无人机协同目标跟踪[J/OL]．系统工程与电子技术.https://kns.cnki.net/kcms/detail/11.2422.TN.20220117.1837.010.html网络首发：在编辑部工作流程中，稿件从录用到出版要经历录用定稿、排版定稿、整期汇编定稿等阶段。录用定稿指内容已经确定，且通过同行评议、主编终审同意刊用的稿件。排版定稿指录用定稿按照期刊特定版式（包括网络呈现版式）排版后的稿件，可暂不确定出版年、卷、期和页码。整期汇编定稿指出版年、卷、期、页码均已确定的印刷或数字出版的整期汇编稿件。录用定稿网络首发稿件内容必须符合《出版管理条例》和《期刊出版管理规定》的有关规定；学术研究成果具有创新性、科学性和先进性，符合编辑部对刊文的录用要求，不存在学术不端行为及其他侵权行为；稿件内容应基本符合国家有关书刊编辑、出版的技术标准，正确使用和统一规范语言文字、符号、数字、外文字母、法定计量单位及地图标注等。为确保录用定稿网络首发的严肃性，录用定稿一经发布，不得修改论文题目、作者、机构名称和学术内容，只可基于编辑规范进行少量文字的修改。出版确认：纸质期刊编辑部通过与《中国学术期刊（光盘版）》电子杂志社有限公司签约，在《中国学术期刊（网络版）》出版传播平台上创办与纸质期刊内容一致的网络版，以单篇或整期出版形式，在印刷出版之前刊发论文的录用定稿、排版定稿、整期汇编定稿。因为《中国学术期刊（网络版）》是国家新闻出版广电总局批准的网络连续型出版物（ISSN 2096-4188，CN 11-6037/Z），所以签约期刊的网络版上网络首发论文视为正式出版。系统工程与电子技术Systems Engineering and Electronics收稿日期：2021-06-01；修回日期：2021-09-23。*通讯作者.基于辅助信标的无人机协同目标跟踪史浩然*，卢发兴，祁江鑫，杨 光（海军工程大学兵器工程学院，湖北 武汉 430033）摘 要：针对无人机姿态角误差与观测误差影响目标定位精度问题，构建基于辅助信标的无人机协同目标跟踪模型，提高了对目标的定位精度。提出基于辅助信标的姿态校正方法，利用辅助信标的精确位置实时校正无人机的姿态角，减小姿态角误差对定位精度的影响。根据双无人机的最优观测构型，设计双无人机协同控制律，得到无人机观测的优化轨迹，以提高无人机对目标的观测质量，最后采用容积卡尔曼滤波（CKF）算法得到目标的状态估计。仿真结果表明该算法能有效减小无人机姿态角误差和观测误差对目标定位的影响，提高目标跟踪精度，具有一定的工程应用价值。关键词：协同控制；轨迹优化；姿态校正；容积卡尔曼滤波（CKF）；目标跟踪中图分类号：TP273 文献标识码：ACooperative target tracking of UAVs based on aided beaconSHI Haoran，LU Faxing，QI Jiangxin，YANG Guang(College of Weaponry Engineering, Naval University of Engineering, Wuhan 430033, China)Abstract: Aiming at the problem that the attitude angle error and observation error of the UAV affectthe target positioning accuracy, the mathematical model of cooperative target tracking model by UAVsbased on aided beacon is constructed, and the positioning accuracy of the target is improved. An attitudecorrection method based on aided beacon is proposed, which uses the accurate position of aided beacon tocorrect the attitude angle of the UAV in real-time to reduce the influence of attitude angle error onpositioning accuracy. According to the optimal observation configuration of two UAVs, the cooperativecontrol law is designed, and the optimal trajectory of two UAVs is obtained to improve the observationquality of the target. Finally, the state estimation of the target is obtained by using the cubature Kalmanfilter (CKF) algorithm. The simulation results show that the algorithm can effectively reduce the influenceof the attitude angle error and observation error on the target location, improve the target tracking accuracy,and has a specific engineering application value.Keywords: cooperative control; trajectory optimization; attitude correction; cubature Kalman filter(CKF); target tracking0 引 言相比较传统武器而言，超高速炮在反应速度、攻击射程以及可控性等方面表现出明显的优势，但如何为超高速炮提供精确的超视距目标指示是亟需解决的关键问题。现有的超视距目标指示手段包括卫星侦察、预警机侦查、前哨侦查等，然而卫星侦查手段实时性、动态性难以满足要求，预警机侦查主要用于保障反舰导弹武器。无人机由于具有较强的机动性能，而且隐身性能好、成本低，因此广泛应用于目标侦察领域。许多文献都对基于无人机的目标定位与跟踪进行了广泛的研究。邵慧等人[1]研究了影响无人机精确定位的因素，其中主要影响因素包括无人机位置、姿态角的导航精度以及无人机和目标之间的相对位置关系，而在现有的导航系统中缺乏较为成熟的对无人机姿态直接进行测量的系统，姿态角的可观测性较差，存在较大的姿态角误差。在无人机姿态估计方面，大多数的姿态估计算法是基于卡尔曼滤波及其衍生形式。文献[2]采用扩展卡尔曼滤波（EKF）提高姿态估计的精度。Garcia 等人[3]采用无迹卡尔曼滤波（UKF）得到小型航天器的位姿估计。蔡安江等人[4]提出网络首发时间：2022-01-18 16:56:21网络首发地址：https://kns.cnki.net/kcms/detail/11.2422.TN.20220117.1837.010.html系统工程与电子技术Systems Engineering and Electronics一种基于四元数衍生UKF 算法的多旋翼无人机姿态估计算法。但是这种基于融合滤波的方法难以建立合适的姿态方程以及确定噪声协方差。影响定位精度的另一个重要因素是无人机和目标之间的相对位置关系，即通过研究多无人机位置配置以增强目标状态估计[5-7]，通常以Fisher 信息矩阵（FIM）的行列式作为目标函数。Adrian N. Bishop 等人[8]研究了仅方位信息、仅距离信息以及仅到达时间信息下多传感器几何配置问题。文献[9-10]研究了二维空间中基于测角测距的双机最优观测配置问题。赵世钰等人[11]基于框架理论研究了二维和三维空间中传感器的最优观测构型，考虑了三种传感器：仅角度、仅范围和接收信号强度。如何保持双机甚至多机之间的观测配置获得最优的目标估计，这是多机协同定位跟踪的又一难点问题。文献[12]研究了通信和测量受限条件下的无人机分布式控制问题，文献[13]基于滚动时域控制的方式实现多机协同最优目标跟踪。姚鹏等人[14]受PSO 和DE思想的启发，提出了一种改进的灰狼优化算法，优化无人机的轨迹以延长对目标的跟踪时间。这种在线优化方法运算量大，实时性是需要解决的问题。Kim S.等人[15]提出了一种非线性模型预测控制框架用于双无人机的协同standoff 跟踪，在模型预测控制方程中，使用罚函数处理无人机性能约束和机间的防碰撞。Song 等人[16]提出一种针对运动目标的微分几何制导法实现对目标的standoff 跟踪。Kokolakis 等人[17]提出一种分布式稳健制导律实现standoff 目标跟踪。但是以上方法只考虑了盘旋阶段使无人机保持最优观测，并未考虑趋近阶段的最优观测问题。Ousingsawat[18]等人基于FIM 性能指标结合滚动时域方法求解无人机趋近目标的最优轨迹，同时考虑了风险区域和终端的限制条件。孙顺等人[19]针对趋近阶段和保持阶段的最优布站配置，通过闭环最优控制方法实现了TDOA定位体制的多机轨迹优化。无人机姿态角是影响无人机定位精确度的主要因素，目前针对无人机协同定位跟踪问题的研究并没有考虑到姿态角的影响。而且基于势场法的多机协同控制问题，并未考虑传感器观测精度、无人机与目标的距离对最优轨迹的影响。为此，本文提出一种基于辅助信标的多无人机协同跟踪定位算法，通过辅助信标校正无人机的姿态角，减小姿态角误差的影响。考虑最优轨迹的影响因素，设计一种双机趋近目标运动的优化控制方法，使双无人机保持在较好的观测位置，从而增加测量信息，减小目标状态估计的不确定度，提高准确性，以达到对超视距目标精确指示的目的。1 基于辅助信标的无人机姿态校正模型研究表明，影响无人机定位精度的因素主要包括无人机位置误差、无人机姿态误差以及无人机测量误差[1]。由于测量误差与无人机探测设备有关，而且可以通过滤波算法减少测量误差的影响；通过设置辅助信标（辅助信标的位置精度相对较高），然后无人机测量出辅助信标与目标之间的相对位置，得到目标的精确位置，从而消除无人机位置误差的影响，因此提高无人机姿态角的导航精度可以有效提高目标的定位精度。本部分采用一种基于辅助信标的无人机姿态校正方法，提高目标的定位精度，基于辅助信标的无人机姿态校正概念图如图1 所示。合作平台无人机目标坐标转换己方平台图1 基于辅助信标的无人机姿态校正概念图Fig.1 Concept figure of attitude correction by the UAVbased on aided beacon1.1 坐标系及变换绝对地理坐标系：原点固定在地面某一位置， xOy 面与地表相切， x 轴向东， y 轴向北，z 轴指向天顶。用下标g 表示。载体地理坐标系：原点固定在无人机质心，坐标轴与绝对地理坐标系坐标轴平行。用下标b表示。载体坐标系：原点固定在无人机质心，y 轴指向无人机朝向，x 轴与y 轴垂直在无人机横截面内，z 轴垂直于xOy 面指向无人机上方。用下标u 表示。当无人机姿态角为J = (y ,q ,f )（偏航角、俯仰角、横滚角）时，设载体地理坐标系坐标与载系统工程与电子技术Systems Engineering and Electronics体坐标系坐标的变换矩阵为us M （ us® ，载体坐标系坐标到载体地理坐标系坐标）( Mus J) = My (y )Mq (q )Mf (f ) (1)其中，cos sin 0( ) sin cos 00 0 1yy yy y yæ öç ÷= ç- ÷ç ÷è øM (2)1 0 0( ) 0 cos sin0 sin cosq q q qq qæ öç ÷=ç ÷ç - ÷ è øM (3)cos 0 sin( ) 0 1 0sin 0 cosff fff fæ - öç ÷=ç ÷ç ÷è øM (4)1.2 基于辅助信标的姿态校正设绝对地理坐标系下，辅助信标位置为, cgX ， 无人机对辅助信标的量测为( ) , , r r b b e e + D + D + D ，其中r 、b 、e分别表示无人机观测的距离、方位角和俯仰角信息，r D 、bD 、e D 为相对应的观测误差，则可得到辅助信标在无人机直角坐标系中的观测值为：( ) ( ) ( )( ) ( ) ( )( ) ( ),cos cossin cossincob ur rr rr rb b e eb b e ee eæ + D + D + D öç ÷= ç + D + D + D ÷ç + D + D ÷ è øX (5)在理想情况下，应当有*, , , ( ) c g f g us cob u X = X + M J X (6)式中： *f ,g X 为绝对地理坐标系下无人机位置（包含误差）。然而，由于姿态角误差、观测误差与无人机位置误差的存在，公式（6）并不成立，因此需要求解J 使I (J)最小，其中*, , , ( ) ( ) f g us cob u c g I J = X + M J X - X (7)在式（7）中只有坐标变换矩阵( ) us M J 未知，因此可以通过梯度下降法[20] 求解min J 使min min I(J) = I(J )，以 min J 作为无人机姿态的校正值，在理论上具有比观测值* J 更高的精度。2 观测航迹优化最优观测航迹是指使目标状态估计误差最小的无人机观测航迹，测量数据的质量与无人机相对目标的观测位置密切相关，不同的观测位置得到的测量数据所提供的目标信息是不同的[21]。测量数据的“好”或“坏”，直接影响目标的状态估计，也影响后续的滤波处理与姿态校正。本部分的主要研究内容是实时规划无人机的轨迹，使无人机处于观测优化的位置，从而有效提高对目标的观测质量。2.1 最优观测配置误差协方差是状态估计常用的性能指标，它表示状态估计的不确定程度，因此误差协方差应尽可能小。Cramer-Rao Lower Bound（CRLB）定义了理想情况下状态估计误差协方差所能达到的最低下界[22-23]，而且CRLB 与系统的固有属性有关，与具体估计算法无关，CRLB 的数学表达式为：{ } 1| | | ˆ ˆTk k k k k k k k k E x x x x CRLB - P = éë - ùû éë - ùû ³ F (8)F 表示FIM，它是CRLB 的逆，它表示在给定的观测值中所包含的有关状态参数的信息量。det( ) F 为F 的矩阵行列式，与状态参数的不确定度成反比，因此，传感器最优观测即为det( ) F 的最大值。在三维空间中，双无人机对目标进行观测时， det( ) F 可以由公式（9）计算得到，具体证明过程见文献[24]。2122 4 4 41 22124 2 2 21 22126 2 4 4 21 2 1 22122 4 2 21 2(1 cos ) 1 1det( ) ( )(1 cos ) 1 1( )(1 cos ) 1 1( )2(1 cos )rrrR RR RR R R RR Rqs sqs sqsqs s+= +-+ +-+ +++F(9)其中r s 、s 分别为无人机观测距离标准差和角度标准差， 1 R 、2 R 分别为无人机1 和无人机2与目标之间的距离， 12 q 为两架无人机与目标之间的视线夹角。当1 2 min R = R = r 时2 2 2 26 4 6 minmin2 2 2 2 2min 122det( ) [( )( ) cos ]rrrrrrs ss ss s q= +- -F(10)由公式（10）可知，当12 cosq = 0时，det(F)取得最大值。因此双无人机观测目标的最优观测配置是无人机与目标之间的视线夹角为12 q =p / 2，此时无人机能够获取更多有效的目系统工程与电子技术Systems Engineering and Electronics标信息。2.2 观测航迹优化控制在双机协同跟踪目标最优观测配置分析的基础上，本节设计协同控制算法优化无人机的观测轨迹。假设无人机在固定高度飞行，因此可简化为无人机与目标水平方向的夹角保持在90°。针对超视距目标跟踪而言，无人机与目标相距较远，需要在趋近阶段优化无人机的观测轨迹，因此设计的协同控制算法在保证无人机趋近目标的同时，也要使得无人机与目标的视线夹角趋近90°，设计如下的双机协同控制算法[25]：( )( )1 1 12 2 21 2sin / 2sin / 2kka b d pa b d pd b b= + -= - -= -(11)公式（11）中， 1a ， 2a 表示无人机的航向角， 1b ， 2b 表示无人机对目标的观测角，d 是两架无人机与目标之间的视线夹角，比例系数1k 、2k 调节视线夹角趋近90°的优先程度。由文献[18]可知，相对距离ˆrrRss= ( r 为无人机与目标的距离)对最优轨迹产生影响，相对距离越远离1，最优轨迹越优先形成90°的观测，因此根据相对距离，设计如下比例系数：111222ˆ 12 ˆˆ 12 ˆeeRkRRkRppæ - ö= ç ÷ ç ÷è øæ - ö= ç ÷ ç ÷è ø(12)式中： 1ˆ R 、2ˆ R 分别为无人机1 和无人机2 的相对距离，指数因子0 e > 用来控制无人机角度分离的程度，本文取0.8 e = 。当1ˆ R 与2ˆR越远离1 时， 1k 与2k 的值越大，加大无人机与目标视线夹角的控制量，以达到最优观测角度。当1ˆ R 与2ˆ R 越趋近1 时， 1k 与2k 的值越小，加大无人机向目标运动方向的控制量，以趋近目标。2.3 融合滤波为了有效利用观测信息，减小观测误差对定位精度的影响，采用三维容积卡尔曼滤波算法(CKF)得到目标的估计状态，目标的状态为( ) [ ( ), ( ), ( ), ( ), ( ), ( )]T x k = x k x k y k y k z k z k 。目标状态方程为x(k) = Fx(k -1) +Gw(k) (13)其中，1 0 0 0 00 1 0 0 0 00 0 1 0 00 0 0 1 0 00 0 0 0 10 0 0 0 0 1sssTTTé ùê úê úê ú=ê úê úê úê úêë úûF,2220.5 0 00 00 0.5 00 00 0 0.50 0ssssssTTTTTTé ùê úê úê ú=ê úê úê úê úêë úûG()k w 是均值为0，方差为Q 的过程噪声， sT表示采样间隔。观测方程为( ) ( ) azeldk q kqé ùê ú= +ê úêë úûZ v (14)其中， ( ) k v 是均值为0，方差为R 的观测噪声。d 、azq 、elq 分别表示无人机观测目标的距离、方位角和俯仰角。CKF 根据Cubature 变换得到具有相同权值的容积点来近似积分运算，即( ) ( )1|mi iiE x z w f x=» å (15)其中，[ ] , 1,2,..., 221, 1,2,..., 2i iimi m ni m nmxwì= = = ïïíï = = =ïî1(16)[ ] 1 表示n 维空间（ n 维状态）的点集，[ ]i1 表示集合的第i 列，对于三维目标跟踪， 6 n =[ ]6 121 0 1 00 0 0 00 1 0 1´éæ ö æ ö æ - ö æ öùêç ÷ ç ÷ ç ÷ ç ÷ú= êç ÷ ç ÷ ç ÷ ç ÷úêç ÷ ç ÷ ç ÷ ç ÷úêç ÷ ç ÷ ç ÷ ç ÷úêëè ø è ø è ø è - øúû1 (17)计算出容积点集后通过预测更新和量测更新便得到CKF 滤波算法[26-28]。假定得到两条航迹i 和j ，他们分别有状态估计ˆi x ， ˆ j x ，误差协方差i P 和j P ，则融合算法为融合状态估计：( ) ( )( )1 11 1ˆ ˆ ˆˆ ˆj i j i i i j ji i j j- -- -= + + += +x P P P x P P P xP P x P x(18)误差协方差：( )( )111 1i i j ji j--- -= += +P P P P PP P(19)根据第1 节和第2 节的分析，双节协同跟踪算法结构如图2 所示。协同控制模块根据当前时刻的无人机状态与目标估计状态，控制无人机在靠近目标的同时，使得无人机与目标之间的夹角接近90°。姿态校正算法模块通过无人机位置、系统工程与电子技术Systems Engineering and Electronics无人机观测信息、辅助信标位置以及无人机原始的姿态角数据求解出校正后的无人机姿态角，从而减小姿态角误差。坐标变换模块将目标观测值变换到地理坐标系，得到减小姿态角误差后的目标观测值。随后将每个无人机观测到的目标状态送到信息融合中心，信息融合中心对目标进行融合滤波处理，消减观测随机误差对目标状态估计的影响。最后将得到的目标状态估计送入协同控制算法模块，控制无人机的飞行，形成闭合环路。无人机1位置( ) 1 1 f Xk-无人机2位置( ) 2 1 f Xk-协同控制无人机轨迹优化无人机1下一时刻位置无人机2下一时刻位置f 1 ( ) X k ( ) f 2 X k姿态校正姿态校正辅助信标坐标变换坐标变换信息融合中心1Z (k) 2Z (k)Xˆ (k)图2 双机协同跟踪算法结构图Fig.2 Structure diagram of cooperative trackingalgorithm by two UAVs3 仿真结果3.1 单机观测设目标做匀速直线运动，目标初始位置位于（0m,200000m,0m）处，速度为8 / ms，方向角为p / 4 ，过程噪声均值为 0，协方差为0.01*diag([1,1,1])；辅助信标位于（0m,0m,0m）处，无人机与辅助信标的距离为50 km ，无人机高度为2500 m ；传感器观测目标的随机误差为（5m，0.3°，0.3°）；观测辅助信标的随机误差为（5m，0.3°，0.3°），假设随机误差服从高斯分布；无人机姿态角固定误差为（0.1°，0.1°，0.1°），随机误差为（0.1°，0.1°，0.1°）；仿真间隔1s，仿真时长1000s。仿真200 次，得到传统方法[2]（包含姿态角误差）、姿态校正方法 （本文方法）以及静态方法[29] （无姿态角误差）情况下目标位置均方根误差（RMSE），如图3 所示：图3 单机观测时目标位置RMSEFig.3 RMSE of target position through observation bythe single UAV图3 可以看出在终端时刻，单无人机采用传统方法观测得到的目标位置RMSE 为451.8m；采用基于辅助信标的姿态校正算法，目标的位置RMSE 为83.7 m，相比较传统观测而言，定位精度得到了很大的提高；在没有姿态误差的理想情况下，目标位置RMSE 为57.5m。可见本文提出的姿态校正算法能够有效减小姿态角误差对定位精度的影响，提高目标定位精度。同时为比较不同滤波算法对目标位置精度的影响，图4 给出了EKF、UKF、CKF 三种滤波算法下的目标位置RMSE，蒙特卡洛仿真次数为200 次。图4 不同滤波算法下目标位置RMSEFig.4 RMSE of target position under different filteralgorithm系统工程与电子技术Systems Engineering and Electronics图4 的结果表明CKF 算法具有较高的滤波精度，特别是在前200 s，CKF 的滤波精度均明显高于EKF、UKF 滤波算法，这十分有利于在尽可能短的时间内跟踪目标。3.2 双机观测采用单机观测的仿真参数，两架无人机距离辅助信标的距离均为50 km，相比较单机观测增加了无人机2 的观测，观测态势如图5 所示。图5 双无人机对目标定位空间态势图Fig.5 Space situation diagram of target positioning bytwo UAVs同样仿真200次，得到双无人机观测条件下传统方法测量、姿态校正方法测量以及无姿态误差测量情况下目标状态估计误差，如图6所示。图6 双机观测时目标位置RMSEFig.6 RMSE of target position through observation bytwo UAVs双无人机观测时，采用传统方法观测得到终端时刻的目标位置RMSE为24.3 m；采用基于辅助信标的姿态校正算法，目标终端时刻的位置RMSE为19.4 m，在没有姿态误差的理想情况下，终端时刻目标位置RMSE为15.8 m。从仿真结果看出双机观测相比较单机观测能有效提高目标的定位精度。3.3 双机协同控制观测下面考虑双无人机协同的情况，无人机飞行速度为100 / ms，仿真运行1300 s，采用公式（11）的控制律得到无人机的运动轨迹如图7(a)、图7(b)所示，分别给出了在三维和二维空间中的双无人机运动轨迹；图7(c)给出了目标真实运动轨迹以及采用本文方法得到的目标估计运动轨迹。图8给出了无人机与目标之间距离、双无人机与目标之间夹角随时间变化曲线图；图9表示仿真200次，仿真时长为1000 s时，无人机飞行过程中对目标观测定位误差的变化曲线。(a) 无人机与目标在三维空间中的运动轨迹(a) Trajectory of the UAV and target in three-dimensionalspace(b) 无人机与目标在二维空间中的运动轨迹(b) Trajectory of the UAV and target in two-dimensionalspace系统工程与电子技术Systems Engineering and Electronics(c) 目标真实运动轨迹与估计轨迹(c) Real trajectory and estimated trajectory of the target图7 无人机与目标的运动轨迹Fig.7 Trajectory of the UAV and target(a) 无人机与目标之间距离变化(a) The distance between the UAV and target(b)无人机与目标视线夹角变化(b) The line of sight between the UAV and target图8 无人机与目标之间距离和视线夹角变化曲线Fig.8 The distance and line of sight between the UAVand target结合图7、图8可以看出，采用公式（11）的控制律，能够较好的控制无人机在靠近运动目标的同时，无人机与目标之间的夹角接近90°，使得无人机处于观测优化的位置。图9表明无人机观测的目标位置误差随着时间的变化而在不断的减小，相比较3.2节双机观测而言，终端时刻目标位置的RMSE变小。仿真结果验证本文的算法能有效提高对目标的定位精度，从而达到对超视距目标精确定位的目的。图9 双机协同观测时目标位置RMSEFig.9 RMSE of target position when two UAVscooperate to observe为分析双机协同控制算法的有效性，与李雅普诺夫（Lyapunov）导航矢量场的方法[30-31]进行了比较，Lyapunov 导航矢量场为：( )2 20 02 2 2 20 0 0( ) (2 ),( ) ( ) (2 )x d d r ry d r rx v x r R y r Rx yy r r R y r R x r Ré ù é ù æ ö é- - - × × ù= ê ú = ê ú = ç ÷ ê úë û ë û è + ø êë- - + × × úûggg(20)通过导引矢量场得到无人机期望航向角：arctan dddyxjæ ö= ç ÷è ø(21)应用反馈控制原理设计比例控制器：( ) d i d u k w =j - j -j (22)( )2022 204 d dR rvr Rj =+(23)两架无人机协同跟踪目标时，两架无人机的速度控制输入分别为系统工程与电子技术Systems Engineering and Electronics1,1 2 1 01,2 3 2 022ddu l R vu l R vpj jpj jæ ö= ç - - ÷ +è øæ ö= - ç - - ÷ +è ø(24)其中， ( ) ( ) 2 2 2 2i T i T r r r = x - x + y - y = x + y ，( , ) i i x y为无人机位置，( , ) TTxy为目标的估计位置， 0R为无人机的盘旋半径， dv 为期望速度，l 为大于0 的常数。在本次仿真中设置0R =20km ，100 / d v m s = 。图10给出了Lyapunov导航矢量场规划的双无人机轨迹，图11给出了本文方法与Lyapunov导航矢量场法得到的目标位置RMSE对比图。从图11看出本文提出的双机协同控制算法能取得较好的目标状态估计， 而且估计性能优于Lyapunov导航矢量场法，验证了本文所提双机协同控制算法的有效性。图10 Lyapunov导航矢量场规划的无人机轨迹Fig.10 UAV trajectories planned by Lyapunov guidancevector fields method图11 本文方法与Lyapunov导航矢量场法RMSEFig.11 RMSE of target position through providedmethod and Lyapunov guidance vector fields method4 结 论本文提出一种基于无人机对超视距目标的定位跟踪算法。提出基于辅助信标的姿态校正算法，通过无人机观测的辅助信标位置以及辅助信标已知的精确位置，减小无人机姿态角误差，从而消减无人机姿态角误差对定位精度的影响，并且得到了仿真验证。多无人机相对目标的观测位置也会影响无人机对目标的定位精度，基于双机协同的最优观测构型与相对距离因子对最优轨迹的影响，设计一种双机协同制导律从而使得无人机处于观测优化的位置，仿真表明所提控制方法能够控制无人机在靠近目标的同时，使得双机与目标之间的夹角接近90°，提高双无人机对目标的定位精度。本文提出的算法能有效提高对超视距目标的定位精度，为超高速炮弹提供信息保障，具有一定的工程应用价值。参考文献[1] 邵慧. 无人机高精度目标定位技术研究[D].南京: 南京航空航天大学, 2014.SHAO H. Research on High Precision TargetLocalization Technology in UAV [D].Nanjing: Nanjing University of Aeronauticsand Astronautics, 2014.[2] LI J, WEI X G, ZHANG G J. An extendedKalman filter-based attitude tracking algorithmfor star sensors [J]. Sensors, 2017, 17(8):1921.[3] GARCIA R V, KUGA H K, ZANARDI M C.Unscented Kalman filter for spacecraft attitudeestimation using modified Rodriguesparameters and real data [J]. Computational &Applied Mathematics, 2016, 35(3): 835-846.[4] 蔡安江, 刘凯峰, 郭师虹, 等. 基于四元数衍生无迹卡尔曼滤波的二段式多旋翼无人机姿态估计算法[J].控制理论与应用, 2020,37(2): 365-373.CAI A J, LIU K F, GUO S H, et al.Quaternion derivative unscented Kalmanfilter-based two-step attitude estimationalgorithm for multi-rotor unmanned aerialvehicle [J]. Control Theory & Applications,2020, 37(2): 365-373.[5] HE S M, SHIN H S, TSOURDOS A.Trajectory optimization for multitarget系统工程与电子技术Systems Engineering and Electronicstracking using joint probabilistic dataassociation filter [J]. Journal of Guidance,Control, and Dynamics, 2019, 43(1): 170–178.[6] ZHANG S, GUO Y, LU Z, et al. Cooperativedetection based on the adaptive interactingmultiple model-information filtering algorithm[J]. Aerospace Science and Technology, 2019,93: 105310.[7] ULUSKAN S. Noncausal trajectoryoptimization for real-time range-only targetlocalization by multiple UAVs [J]. AerospaceScience and Technology, 2020, 99(3): 105558.[8] BISHOP A N, FIDAN B, ANDERSON B, etal. Optimality analysis of sensor-targetlocalization geometries [J]. Automatica, 2010,46(3):479-492.[9] SONIA M, FRANCESCO B. Optimal sensorplacement and motion coordination for targettracking[J]. Automatica, 2006, 42(4):661-668.[10] 王林. 多无人机协同目标跟踪问题建模与优化技术研究[D]. 长沙: 国防科学技术大学, 2011.WANG L. Modeling and optimization forMulti-UAVs cooperative target tracking [D].Changsha: National University of DefenseTechnology, 2011.[11] ZHAO S, CHEN B, LEE T. Optimal sensorplacement for target localization and trackingin 2D and 3D [J]. International Journal ofControl, 2013, 86(10):1687-1704.[12] 孙海波, 周锐, 邹丽, 等. 通信和测量受限条件下异构多UAV 分布式协同目标跟踪方法[J].航空学报, 2011, 32(2): 299-310.SUN H B, ZHOU R, ZOU L, et al.Distributed cooperation target tracking forheterogeneous multi-UAV undercommunication and measurement constrains[J]. Acta Aeronautica et Astronautica Sinica,2011, 32(2): 299-310.[13] DI B, ZHOU R, DUAN H B. Potential fieldbased receding horizon motion planning forcentrality-aware multiple UAV cooperativesurveillance [J]. Aerospace Science andTechnology, 2015, 46:386-397.[14] YAO P, WANG H, JI H. Multi-UAVs trackingtarget in urban environment by modelpredictive control and Improved Grey WolfOptimizer [J]. Aerospace Science andTechnology, 2016, 55:131-143.[15] KIM S, OH H, TSOURDOS A. Nonlinearmodel predictive coordinated standofftracking of a moving ground vehicle [J].Journal of Guidance, Control, and Dynamics,2013, 36(2): 557-566.[16] SONG Z, LI H, CHEN C, et al. Coordinatedstandoff tracking of moving targets usingdifferential geometry [J]. Journal of ZhejiangUniversity-SCIENCE C,2014, 15(4):284-292.[17] KOKOLAKIS N, KOUSSOULAS N T.Robust standoff target tracking withfinite-time phase separation under unknownwind [J]. Journal of Guidance, Control, andDynamics, 2021, 44(6): 1183-1198.[18] OUSINGSAWAT J, CAMPBELL M E.Optimal cooperative reconnaissance usingmultiple vehicles [J]. Journal of Guidance,Control, and Dynamics, 2007, 30(1):122-132.[19] 孙顺, 熊伟, 刘瑜, 等. 基于 TDOA 的多机协同闭环最优控制方法[J]. 电光与控制,2019, 26(3): 1-7.SUN S, XIONG W, LIU Y, et al. A TDOAbased closed-loop optimal control method formultiple UAVs [J]. Electronics Optics &Control. 2019, 26( 3): 1-7.[20] 朱惠民,贾正荣,王航宇, 等. 针对不同视场辅助信标的无人机目标定位方法[J].国防科技大学学报, 2019, 41(3): 125-136.ZHU H M, JIA Z R, WANG H Y, et al. Targetlocation method of unmanned aerial vehicledirect at aided beacon in different field ofview [J]. Journal of National University ofDefense Technology, 2019, 41(3): 125-136.[21] 刘重, 高晓光, 符小卫. 基于通信与观测联合优化的多无人机协同目标跟踪控制[J].控制与决策, 2018, 33(10):1747-1756.LIU C, GAO X G, FU X W. Co-optimizationof communication and observation for系统工程与电子技术Systems Engineering and Electronicsmultiple UAVs in cooperative target tracking[J]. Control and Decision, 2018,33(10):1747-1756.[22] HUNG N T, CRASTA N, MORENO S D, etal. Range-based target localization and pursuitwith autonomous vehicles: An approach usingposterior CRLB and model predictive control[J]. Robotics and Autonomous Systems, 2020,132: 103608.[23] XU S. Optimal sensor placement for targetlocalization using hybrid RSS, AOA and TOAmeasurements [J]. IEEE CommunicationsLetters, 2020, 24(9): 1966-1970.[24] SHI H R, LU F X, WANG H Y, et al. Optimalobservation configuration of UAVs based onangle and range measurements andcooperative target tracking inthree-dimensional space [J]. Journal ofSystems Engineering and Electronics, 2020,31(5): 996-1008.[25] 钟春梅,赵振宇,孙海波, 等. 多无人机协同目标跟踪闭环最优控制方法[J].探测与控制学报, 2012, 34(3): 13-18.ZHONG C M, ZHAO Z Y, SUN H B, et al. Aclosed-loop optimal control for multipleunmanned aerial vehicles cooperative targettracking [J]. Journal of Detection & Control,2012, 34(3): 13-18.[26] IENKARAN A, SIMON H. Cubature kalmanfilters [J]. IEEE Trans. on Automatic Control,2009, 54(6): 1254-1269.[27] 丁家琳, 肖建, 张勇. 基于 CKF 的分布式滤波算法及其在目标跟踪中的应用[J]. 控制与决策，2015, 30(2): 296-302.DING J L, XIAO J, ZHANG Y. Distributedalgorithm-based CKF and its applications totarget tracking [J]. Control and Decision,2015, 30(2): 296-302.[28] SANTOSL J, ORIVE R, ACOSTA D, et al.The cubature kalman filter revisited [J].Automatica, 2021, 127(6): 109541.[29] WANG D, HUANG D, XU C, et al. Aclosed-form method for simultaneous targetlocalization and UAV trajectory optimization[J]. Applied Sciences, 2021, 11(1):114.[30] LAWRENCE D A, FREW E, PISANO W.Lyapunov vector fields for autonomous UAVflight control [J]. Journal of Guidance,Control, and Dynamics, 2008, 31(5):1220–1229.[31] YAO P, WANG H L, SU Z K. Cooperativepath planning with applications to targettracking and obstacle avoidance formulti-UAVs [J]. Aerospace Science andTechnology, 2016, 54(6): 10-22.作者简介史浩然（1992—），男，博士研究生，主要研究方向为多无人机协同控制、目标跟踪。卢发兴（1974—），男，教授，博士，主要研究方向为舰艇指挥与控制。祁江鑫（1995—），男，硕士研究生，主要研究方向为无人机群目标检测。杨 光（1990—），男，博士研究生，主要研究方向为导弹协同制导。