1 引言
近年来，无人机凭借体积小、成本低以及高度灵活性等优点受到广泛关注[1]。无人机集群概念的提出，进一步促进了无人机技术的发展，借助个体间的交互，实现群体行为的全局一致性协同，可实现单架无人机无法执行的作战任务，譬如集群中某架无人机发生故障时，可由集群编队中的其他无人机执行故障无人机的功能，从而提高整个系统的抗干扰能力和效能[2-3]。随着无人机集群技术的快速发展，无人机集群在农业植保、森林防火、智慧城市、物流运输、战场侦察与作战、目标搜索与追踪等方面将发挥巨大的作用[4-10]。
在复杂环境中，大规模集群无人机近距离飞行时，高精度、高可靠的导航与定位信息是实现高动态飞行的重要保障[11]。目前，诸多学者对相关技术进行了大量研究。文献[12]和文献[13]针对传统单主从式协同导航存在的精度发散问题，提出了分层式协同导航方案。其中，文献[12]以相对导航和惯性导航信息作为量测量，利用卡尔曼滤波算法估计校正惯性导航误差，使精度满足较长时间导航要求。文献[13]则通过增加长机数量，提高集群内部信息利用率，基于联邦滤波算法改善了导航性能。在导航定位过程中，无人机间的相对定位，特别是无依托的自主相对定位，对提高无人机集群的定位精度和抗干扰能力至关重要。
集群内部不同目标间的相对定位不仅应用于无人机集群，在其他集群场景中也大量存在，譬如卫星星座间的相对定位等，研究人员对相关方法开展了大量研究[14-15]，其研究结论对改进无人机集群间相对定位算法具有借鉴意义。文献[14]考虑了故障星对星座几何构型的影响，针对剔除故障星观测数据进行定位解算时存在GDOP值突变的问题，采用加权最小二乘法提高了数据利用率，并改善了接收机自主完好性监测性能。文献[15]仿真研究了LEO星座对GNSS精密单点定位性能的增强作用，相比于单GPS系统，LEO星座的参与，可降低GDOP值、减小PPP滤波矩阵条件数以及加快PPP收敛时间，并且显著缩短了模糊度首次固定所需时间。文献[16]利用测距与站址坐标估计目标位置，3架无人机绕圆编队飞行，在编队成正三角形时，可获得最小GDOP值，提高了定位精度。上述文献通过改善观测几何减小精度因子值，进一步提高了定位精度。在大规模集群系统中，固定构型实现的数据链较为单一，多构型组合能够使集群具有更为灵活的应变能力，有望提高整个集群系统的抗摧毁能力。
针对大规模集群无人机相对定位问题，基于随机择机策略提出一种迭代估值定位解算方法，引入卫星星座间相对定位精度评估常用的精度因子作为评估准则，分析了观测时段内多构型融合对定位性能的影响，利用扩展卡尔曼滤波算法对无人机状态信息进行估计。
2 相对定位模型
2.1 方案设计
采用课题组研究团队提出的一种无人机集群相对定位技术，该技术在无人机机背上安装3个无线电发射/接收装置，每个天线均可实时检测和发射不同频率电波，通过机间观测，构建观测模型并进行导航定位解算，则可获得无人机状态信息。无人机机身上的天线安装示意图如图1所示。
图1 天线安装示意图
Fig.1 Schematic diagram of the antenna configuration
在无人机集群飞行环境中，随机选取一定数量无人机对某一架无人机进行定位解算，从无人机在主机围成区域内飞行，研究观测时段内多构型融合对飞行器定位性能的影响。图2显示了8架主飞行器组成立方体队形示意图。
图2 8架主机组成立方体队形示意图
Fig.2 Diagram of the eight master aircraft in cube formation
2.2 数学模型
每架无人机配备的3个无线电信号收/发装置，在其自身机体坐标系的位置分别为(a,0,0)、(0,0,b)和(0,0,-b)。根据相对位姿参考系，选取某一架主机机体坐标系作为其他无人机的导航坐标系，则从机3个天线位置信息可通过式(1)进行计算，即：
(1)
式(1)中：
表示从机的位置坐标；
表示从机j天线位置坐标，且j=1,2,3；C表示主机的机体坐标系到从机的机体坐标系的转换矩阵，可由欧拉角即方位角(ψ)、俯仰角(ϑ)以及滚动角(γ)组成的旋转矩阵表示；CT则表示从机到主机的转换矩阵。C、CT分别表示为：
(2)
将式(2)代入式(1)，整理得：
(3)
式(3)中：(x,y,z)表示从机位置坐标；(xj,yj,zj)表示从机天线位置。
假设一主机机体坐标系作为其他无人机的导航坐标系，则该主机3个天线位置坐标为：
(4)
式(4)中，Xi0表示领航主机天线坐标，且i=1,2,3。
结合式(3)与式(4)，可得2架无人机机间距离表达式为：
(5)
式(5)中： i表示主机第i个天线； j表示从机第j个天线。
以载波相位作为量测值，其相位角与波长、距离有如下关系，即：
rij=θijλi/2π
(6)
记定位模型的非线性向量函数为：
Z= h(X)
(7)
式(7)中：Z表示相位转换至距离的观测向量； h(X)表示非线性向量函数。
若观测量取
则位置关系方程为：
(8)
式(8)中， v为观测模型噪声，满足均值为零的高斯白噪声序列。
3 相对定位解算
3.1 基本迭代求解
无人机状态向量利用k时刻3个天线的位置参数进行描述，记为
其中
且j=1,2,3；同理，系统观测向量可利用k时刻主飞行器对从飞行器的相位量测值转换至距离值进行表示，记为
假设飞行器在迭代初始时刻具有状态向量估值X及其协方差阵PX，则可通过式(9)的迭代关系对该估值进行迭代校准，即：
(9)
式(9)中：ΔX表示状态向量修正量；ΔZ表示观测向量估值偏差；RZ表示量测误差方差阵；PX表示定位误差方差阵；H表示非线性向量函数h(X)对状态向量X的Jacobian矩阵。
3.1.1 1MA-1FA模型
结合式(8)所示观测方程，可知观测量Z的估值偏差ΔZ具体形式为：
(10)
式(10)中，矩阵H表示非线性向量函数h(X)对状态向量X的Jacobian矩阵，表达式为：
(11)
式(11)中，矩阵Aj具体如下：
(12)
式(12)中：下标i表示主飞行器第i个天线；下标j表示从飞行器第j个天线。
3.1.2 NMA-1FA模型
上述给出了单架主机对一从飞行器的定位估值方法，若是多架无人机对一架无人机进行观测定位解算，则全局观测向量Z为多架无人机观测向量的组合，有：
(13)
式(13)中，Zk表示第k架无人机对从无人机的观测向量，其中k=1,2,…,N。
相应地，非线性向量函数h(X)对状态向量X的Jacobian矩阵为：
(14)
式(14)中，Hk表示第k架无人机对从无人机的观测矩阵，具体如式(11)和式(12)所示。
3.2 导航滤波估计
已知系统在tk-1时刻的系统状态参数最优估计值
和估计值的协方差矩阵Pk-1，根据扩展卡尔曼滤波算法，在最优估计值和一步预测值
处进行泰勒近似并保留一阶项，将非线性问题线性化处理，可知扩展卡尔曼滤波算法(EKF)的递推方程[17]为：
(15)
3.2.1 状态更新
任意无人机3个天线状态方程表达形式为：
(16)
式(16)中：uk-1表示输入矩阵；Bk-1表示控制矩阵，取Bk-1=τI9×9，其中τ=tk-tk-1表示观测采样间隔；Φk|k-1表示状态转移矩阵，取Φk|k-1=I9×9。
3.2.2 观测更新
多架无人机对单架无人机的观测向量和观测矩阵如式(13)、式(14)所示，将状态更新方程和观测更新方程代入递推方程式(15)，可滤波估计出无人机的位置状态信息。
3.2.3 参数更新
利用上述方法可估计出飞行器3个天线的位置信息，再根据天线在机身上固有的安装位置关系，进一步得到飞行器机体坐标系位置信息，即：
(17)
相应地，结合式(3)，可进一步获得该飞行器在导航坐标系中的相对姿态估值，有：
(18)
4 几何构型评估
4.1 权系数阵
式(8)给出了无人机非线性定位方程，仅考虑位置状态参数时，线性化处理后得；
(19)
式(19)中，Δxj表示天线j的状态修正量。式(19)的最小二乘解为：
(20)
考虑测量误差项εr时，式(20)的最小二乘解为：
(21)
将式(20)代入式(21)，则有：
(22)
式(22)表示测量误差与定位误差之间的关系。
假设测量误差呈均值为零的高斯分布，方差为
且不同飞行器测量误差不相关，则测量误差向量协方差阵为：
Rεr=E((εr-E(εr))(εr-E(εr))T)=
(23)
式(23)中，I表示9×9的单位矩阵。
因此，简化的定位误差向量协方差阵为：
(24)
式(24)中，
表示权系数阵，表明测量误差方差被权系数阵放大后转变成定位误差方差阵[18]。式(9)中，若观测噪声
则
4.2 精度因子
在导航学中，精度因子(dilution of precision,DOP)作为评估定位性能的重要指标，指测量误差放大至定位误差的倍数，与卫星的几何分布有关[18]。将其迁移至无人机集群定位系统中，也可作为集群定位性能的评估准则。根据本文状态向量形式，式(24)左右两边对角元素的关系为：
(25)
式(25)中，
表示3个天线的定位误差标准差。
三维空间定位误差标准差定义为：
(26)
式(26)中，P DOP表示空间位置精度因子。
相应地，各方向上的精度因子计算如下：
(27)
式(27)中：(σX,σY,σZ)表示各方向上的定位误差标准差；X DOP、Y DOP和Z DOP分别表示X、Y和Z轴方向上的精度因子。
5 仿真实验
5.1 基本数据
采用数值仿真方法对上述模型进行仿真实验。为便于分析观测时段内多种几何构型对相对定位性能的影响，假设主飞行器处于稳定飞行状态，从飞行器相对于领航无人机的初始位置为(20,30,20) m，在观测时段内其速度状态如表1所示。
表1 各观测时段内速度参数
Table 1 Velocity parameters during each observation period
t/svx/(m·s-1)vy/(m·s-1)vz/(m·s-1)0～2550525～50050
已知每架无人机天线安装参数为a=1.0 m，b=1.0 m，且每个天线发射的电磁波波长分别为λ1=5 m，λ2=6 m，λ3=7 m。选取某一主飞行器机体坐标系作为其他无人机的导航坐标系，所有主飞行器均布置在边长为200 m的立方体顶点处，且姿态保持一致。仿真时间为50 s，每秒随机选择3架(RD-3MA)/5架(RD-5MA)主机进行观测定位，采样频率取20 Hz，分析观测时段内多构型融合对定位性能的影响。
5.2 初始条件
取初始时刻无人机位置状态误差为(-1.335 00, -1.621 60, -1.682 19) m，其位置初始协方差阵为 diag(1.873 06,3.147 71,3.703 52)。结合初始时刻飞行器姿态角与天线安装参数，可得3个天线状态参数组成的初始时刻状态向量与协方差阵。考虑相位量测值服从高斯分布，其转换至距离观测量噪声为0.15(3σ) m。利用迭代估值方法和扩展卡尔曼滤波算法，在初始条件下可估计出无人机位姿参数。
5.3 结果与分析
结合初始条件，从飞行器随机接收3架/5架主机的观测数据，利用迭代估值方法，可估计出飞行器在各自观测时段内的精度因子值和位置估值偏差，结果如图3—图8所示。
图3 各个时段主机观测情况(RD-3MA)
Fig.3 Master aircraft selection by time period (RD-3MA)
图3与图4显示了各个观测时段内主机观测情况。从图3与图4可看出，整个观测时段内，各主机被选中的概率基本一致，任意一架主机观测数据均可能作为从机的定位数据来源。
图4 各个时段主机观测情况(RD-5MA)
Fig.4 Master aircraft selection by time period (RD-5MA)
图5 各个观测时段DOP值(RD-3MA)
Fig.5 DOP values by observation period (RD-3MA)
图6 各个观测时段DOP值(RD-5MA)
Fig.6 DOP values by observation period (RD-5MA)
图7 位置估值偏差曲线(RD-3MA)
Fig.7.Position valuation deviation curves (RD-3MA)
图8 位置估值偏差曲线(RD-5MA)
Fig.8.Position valuation deviation curves (RD-5MA)
图5与图6显示了各个观测时段内精度因子值变化情况。从图5与图6可看出，RD-3MA策略下的DOP值存在剧增的情况，说明选取的3MA组成的几何构型不利于进行定位解算，会导致观测矩阵性质极度不稳定。而RD-5MA策略下各个观测时段的DOP值均较小，3个方向的DOP值均小于1，减弱了量测误差对定位精度的影响，提高了定位性能。图7与图8给出了观测时段内位置估值偏差曲线。从图7与图8可看出，RD-3MA策略在DOP值极高的观测时段，其位置估值偏差也较大，定位误差达到米级，而RD-5MA策略下的DOP值均较小，相应的定位精度较高，可获得厘米精度等级。
上述结果说明，随机选择5架主机可获得较为稳定的观测几何，其定位精度更高，在8架主机中，随机选取至少5架主机(RD-NMA,N≥5)进行观测定位，可估计出精度因子和位置估值偏差，结果如图9—图11所示。此外，利用蒙特卡洛进行100次实验，均方根误差变化曲线如图12所示。
图9 各个时段主机观测情况(RD-NMA)
Fig.9 Master aircraft selection by time period (RD-NMA)
图9给出了各个时段内主机观测情况。从图9可看出，在整个观测时段内，共选取了271机次，比RD-5MA策略多21机次，对全部观测时段选用主机情况进行统计可知，各主机被选用的概率在10.7%～14.0%。图10给出了各个观测时段内精度因子值变化情况。从图10可看出，各方向上的DOP值均小于1，同样削弱了观测误差对定位精度的影响。此外，相比于RD-5MA选取策略，RD-NMA策略下DOP值小于0.4的次数较多，但总体相差不大，说明RD-NMA策略获得的几何构型相对较好，由于主机数量有限，使得效果提升不明显。针对大规模集群无人机执行飞行任务，RD-NMA策略理论上可显著提升定位性能，充分发挥集群内部个体的优势。图11显示了位置估值偏差曲线。从图10与图11中可看出，在DOP值稍高时段，位置偏差同步增大，与图8类似，其定位精度可达厘米量级。图12显示了位置偏差均方根误差变化曲线。
图10 各个观测时段DOP值(RD-NMA)
Fig.10 DOP values by observation period (RD-NMA)
图11 位置估值偏差曲线(RD-NMA)
Fig.11 Position valuation deviation curves (RD-NMA)
图12 位置偏差RMSE值(RD-NMA)
Fig.12 RMSE values of position deviation (RD-NMA)
从图12中可看出，RMSE维持在较低水平，且随时间推移在较小范围内稳定波动，无剧烈震荡趋势。此外，对观测时段内的RMSE值进行统计分析，可知3个方向上的RMSE值均为0.017 m，三维RMSE为0.030 m，可以满足无人机集群相对定位精度要求。
在扩展卡尔曼滤波框架下，选取固定3架主机(UAV1～UAV3,ST-3MA)和随机3架主机(RD-3MA)进行滤波估计，可获得位置和姿态估值偏差，结果如图13—图16所示。
图13 位置估值偏差曲线(ST-3MA)
Fig.13 Position valuation deviation curves (ST-3MA)
图14 位置估值偏差曲线(RD-3MA)
Fig.14 Position valuation deviation curves (RD-3MA)
图13与图14显示了2种择机策略的位置估值偏差曲线。从图13与图14可看出，考虑了状态更新与观测更新的权重分配，2种择机策略均大大提高了状态估计精度，达到了毫米精度量级。相比于ST-3MA策略，RD-3MA策略可获得更高的定位精度，且收敛速度更快，其位置误差可收敛至±0.002 m。
图15与图16显示了2种择机策略的姿态估值偏差曲线。从图13—图16可看出，随着位置估计精度的提高，姿态估值效果也显著提升，RD-3MA策略的姿态角误差可收敛到±0.2°，可满足集群无人机相对姿态精度要求。将ST-3MA与RD-3MA2种择机策略的位置估值误差进行统计分析，其均方根误差统计结果如表2所示。
图15 姿态估值偏差曲线(ST-3MA)
Fig.15 Pose valuation deviation curves (ST-3MA)
图16 姿态估值偏差曲线(RD-3MA)
Fig.16 Pose valuation deviation curves (RD-3MA)
表2 2种择机策略的均方根误差
Table 2 Root mean square error for the two selection strategies
SelectionstrategyRMSE_X/mRMSE_Y/mRMSE_Z/mRMSE_P/mST-3MA0.0040.0040.0030.006RD-3MA0.0020.0010.0020.003
表2给出了2种择机策略的均方根误差统计结果。从表2可看出，相比于ST-3MA策略，RD-3MA策略在3个方向上的RMSE值分别减小了39.8%、65.7%和42.9%，三维RMSE则减小了50%，显著提高了无人机集群定位精度。
6 结论
针对大规模集群无人机相对定位问题，提出了随机择机策略进行观测定位，在传统精度因子基础上，提出了适用于本文定位方法的精度因子评估准则，结合滤波方法给出了高精度位姿状态估计，结果表明：
1) 随机择机策略实现了观测时段内的多构型融合，解决了单一构型限制，适用于大规模集群无人机飞行环境；
2) 随着观测主机数量增多，几何构型愈加稳定，精度因子无剧烈震荡变化，且均方根误差维持在较低水平，显著提高了定位精度；
3) 在扩展卡尔曼滤波框架下，随机择机策略可估计出稳定且高精度的位置状态信息，为相对姿态信息的高精度解算提供了重要依据。
[1] 许晓伟,赖际舟,吕品,等.多无人机协同导航技术研究现状及进展[J].导航定位与授时,2017,4(04):1-9.
Xu X W,Lai J Z,Lv P,et al.A literature review on the research status and progress of cooperative navigation technology for multiple UAVs[J].Navigation Positioning & Timing,2017,4(04):1-9.
[2] 王祥科,刘志宏,丛一睿,等.小型固定翼无人机集群综述和未来发展[J].航空学报,2020,41(04):20-45.
Wang X K,Liu Z H,Cong Y R,et al.Miniature fixed-wing UAV swarms:Review and outlook[J].Acta Aeronautica et Astronautica Sinica,2020,41(04):20-45.
[3] 宗群,王丹丹,邵士凯,等.多无人机协同编队飞行控制研究现状及发展[J].哈尔滨工业大学学报,2017,49(03):1-14.
Zong Q,Wang D D,Shao S K,et al.Research status and development of multi UAV coordinated formation flight control[J].Journal of Harbin Institute of Technology,2017,49(03):1-14.
[4] 田振坤,傅莺莺,刘素红,等.基于无人机低空遥感的农作物快速分类方法[J].农业工程学报,2013,29(07):109-116,295.
Tian Z K,Fu Y Y,Liu S H,et al.Rapid crops classification based on UAV low-altitude remote sensing[J].Transactions of the Chinese Society of Agricultural Engineering,2013,29(07):109-116,295.
[5] Sudhakar S,Vijayakumar V,Kumar C S,et al.Unmanned aerial vehicle (UAV) based forest fire detection and monitoring for reducing false alarms in forest-fires[J].Computer Communications,2020,149:1-16.
[6] Yuan C,Zhang Y,Liu Z.A survey on technologies for automatic forest fire monitoring,detection,and fighting using unmanned aerial vehicles and remote sensing techniques[J].Canadian Journal of Forest Research,2015,45(07):783-792.
[7] Khan N A,Jhanjhi N Z,Brohi S N,et al.Smart traffic monitoring system using unmanned aerial vehicles (UAVs)[J].Computer Communications,2020,157:434-443.
V,Bá
T,Saska M,et al.Cooperative autonomous search,grasping,and delivering in a treasure hunt scenario by a team of unmanned aerial vehicles[J].Journal of Field Robotics,2019,36(01):125-148.
[9] 兰文博,付义伟,李平坤,等.小型战术察打一体无人机武器系统作战应用研究[J].飞航导弹,2013(02):57-61.
Lan W B,Fu Y W,Li P K,et al.Study on the operational application of small tactical inspection and combat UAV weapon systems[J].Winged Missiles Journal,2013(02):57-61.
[10] Meng W,He Z R,Su R,et al.Decentralized multi-UAV flight autonomy for moving convoys search and track[J].IEEE Transactions on Control Systems Technology,2016,25(04):1480-1487.
[11] 王祥科,陈浩,赵述龙.大规模固定翼无人机集群编队控制方法[J].控制与决策,2021,36(09):2063-2073.
Wang X K,Chen H,Zhao S L.Formation control of large-scale fixed-wing unmanned aerial vehicle swarms[J].Control and Decision,2021,36(09):2063-2073.
[12] 刘晓洋,徐胜红.无人机编队分层式协同导航仿真研究[J].计算机仿真,2019,36(10):44-48.
Liu X Y,Xu S H.Simulation research on hierarchical cooperative navigation of UAV formation[J].Computer Simulation,2019,36(10):44-48.
[13] 谷旭平,唐大全.基于联邦滤波算法的无人机集群分层协同导航[J].系统工程与电子技术,2022,44(03):967-976.
Gu X P,Tang D Q.Hierarchical cooperative navigation of uav swarm based on federated filtering algorithm[J].Systems Engineering and Electronics,2022,44(03):967-976.
[14] 张丽,刘硕,张磊.基于星座几何构型的RAIM故障处理方法[C]//第五届中国卫星导航学术年会论文集.南京,2014:8-12.
Zhang L,Liu S,Zhang L.A fault treatment method of RAIM based on constellation’s geometry[C]//Proceedings of the 5th Annual China Satellite Navigation Academic Conference.Nanjing,2014:8-12.
[15] 赵兴隆,钟世明,欧吉坤,等.LEO星座增强GNSS的精密单点定位初步分析[J].大地测量与地球动力学,2020,40(06):624-628.
Zhao X L,Zhong S M,Ou J K,et al.Precise point positioning primary analysis of GNSS enhanced with LEO constellation[J].Journal of Geodesy and Geodynamics,2020,40(06):624-628.
[16] 屈耀红,张峰,谷任能,等.基于距离测量的多无人机协同目标定位方法[J].西北工业大学学报,2019,37(02):266-272.
Qu Y H,Zhang F,Gu R N,et al.Target cooperative location method of multi-UAV based on pseudo range measurement[J].Journal of Northwestern Polytechnical University,2019,37(02):266-272.
[17] 秦永元,张洪钺,汪叔华.卡尔曼滤波与组合导航原理[M].3版.西安:西北工业大学出版社,2015.
Qin Y Y,Zhang H Y,Wang S H.Theory of kalman filter and integrated navigation[M].3rd edition.Xi’an:Northwestern Polytechnical University Press,2015.
[18] 谢钢.GPS原理与接收机设计[M].北京:电子工业出版社,2017.
Xie G.Principles of GPS and receiver design[M].Beijing:Publishing House of Electronics Industry,2017.