1 引言
计算机和通信技术的日趋成熟，促进了网络化系统的发展。组网导航系统(networked navigation system，NNS)是网络化系统在导航领域的一个典型应用，通过无线网络将集群运动体关联起来组成导航网络，参与组网的节点按照设定的通信协议进行信息交互，协作完成定位、测姿等导航任务，能够最大化地利用导航资源，提高系统的导航性能[1-4]。NNS是对传统单平台导航方式的拓展，对提高导航的“性价比”具有重要意义。
姿态信息的获取对运动体的导航与控制是至关重要的。现有的姿态测量系统主要包括惯性导航系统、视觉导航系统、地磁导航系统、GNSS测姿系统等[5-12]。惯性导航系统具有自主导航、短时精度高、抗干扰性强等优点，但其姿态估计误差会随时间累积[5-6]。视觉测量技术具有成本低、精度高等优点，但其测量范围受限，更适合用于近距离位姿测量[7-8]。地磁导航系统以地球磁场矢量为参考基准进行姿态解算，具有隐蔽性强、无累积误差、响应速度快等特点，但在使用过程中容易受到电磁干扰的影响[9]。GNSS测姿系统具有成本低、无累计误差、精度高等优点，但在姿态估计过程中需要进行复杂的模糊度解算，且无法在室内等GNSS信号遮挡严重的环境中使用[10-11]。本文研究基于NNS的姿态估计技术，是在定位功能基础上，对NNS导航功能的拓展,既可以作为独立的导航信源为运动体提供姿态数据，也可以与其他测姿系统进行信息融合，以获取更好的导航性能。
2 NNS工作流程
NNS是以组网运动体为载体，以运动体的导航、定位、测姿和信息共享为核心任务，由导航传感器、组网协议栈、导航处理器等通过实时网络构成闭环反馈的分布式导航系统[3-4]。假设NNS中共有M个节点，采用有向图G=(V,A)对系统进行描述，其中，V和A分别为G的节点集和边集。对于V中的任意节点i(i=1,2，…,M)，其协作邻节点可表示为N[i]={j∈V∶(i, j)∈A}。
典型的组网导航流程可作如下描述：
1) 节点i通过网络接收来自其邻节点j(j∈N[i])的协作数据包，每个数据包包含发送者的导航信息和信号的基本信息。具备目标跟踪功能的节点发送的数据包中，还可包含对目标状态的估计。
2) 节点i利用邻节点的导航信息和节点间的相对测量值进行自身导航状态的更新。
3) 节点i将自身的导航信息打包，并在其发送时序通过网络发送给其他节点，作为其他节点的协作信息。
3 基于NNS的姿态解算模型
3.1 坐标系与姿态
载体的姿态，是指载体坐标系相对于当地地理坐标系的方位关系[13]。基于NNS进行运动体姿态估计，需要在运动体的中心处安装一个节点作为主节点。如图1所示，测姿系统的载体系以主节点所在位置为原点，以运动体主轴方向为Yb轴，Zb轴垂直于节点安装平面指向上方，Xb轴由Yb轴围绕Zb轴顺时针旋转90°得到。
图1 天线安装及载体坐标系示意图
Fig.1 Diagram of antennas installation and body system
本文采用“东北天”坐标系作为当地地理坐标系，以主节点所在的位置为坐标原点，Xn轴指向东(E)，Yn轴指向北(N)，Zn轴指向天顶方向。载体坐标系可由地理坐标系经过3次旋转得到，3次旋转分别对应偏航角ψ、俯仰角θ、滚转角γ。3次旋转可用以下变换矩阵表示[14]：
于是可得，由地理坐标系到载体坐标系的变换矩阵为
3.2 基线矢量的求解
按照图1所示，在运动体的主轴上安装2个组网导航节点。导航节点在安装时会出现安装误差，工程应用中，一方面要采用尽可能高的安装工艺来减小安装误差，另一方面可通过一定的标校手段获取安装误差值，并在解算过程中加入修正量抵消安装误差造成的影响。
用na、nb分别表示节点a和节点b，用nj表示邻节点j。如图2所示，k时刻，na和nb的连线构成基线矢量bk，na处观测nj的单位视线矢量为
与
的夹角为θk。根据矢量运算的知识可得：
图2 基于基线矢量的姿态估计示意图
Fig.2 Attitude estimation based on baseline vector
(1)
由三角函数的相关性质可得：
(2)
式(2)中，
表示k时刻na与nj间的距离；
表示k时刻nb与nj间的距离；l=|bk|，表示基线长度。
在地理坐标系中，将bk和作如下表示：
结合式(1)、式(2)可得：
(3)
设当前时刻na和nj的坐标分别为
和
则视线矢量可由以下式子得到：
(4)
由式(4)可知，和
的误差(即主节点和邻节点定位误差)会影响的计算，进而对后续的姿态解算造成影响。
将式(4)代入式(3)可得：
式中，
组网导航系统可直接测量节点间的相对距离。na、nb与nj间的相对距离测量方程为
(5)
式中，
为满足高斯分布的测量噪声，即
定义如下变量：
(6)
则根据协方差传播定律可得：
(7)
其中，
结合式(3)、式(5)、式(6)可得：
式中，
工程上可采用超宽带(Ultra-wide band，UWB)技术进行组网测距。考虑实际的安装条件，有时会借助辅助天线来实现测距功能，此时，可将天线相位中心视为节点所在位置。另外，本文在建模过程中将节点视为质点，在组网导航过程中，信号的收发和导航信息的共享都是以组网模块(或天线)的相位中心为终端完成的。
若在同一时刻2个天线都能接收到n个邻居节点的协作信息，则可将姿态测量模型写成如下形式：
zk=Bkbk+vk
(8)
式中的参数如下：
结合式(8)可利用最小二乘法求解基线矢量：
考虑不同测量值间的质量差异，可采用加权最小二乘法[15]估计基线矢量：
(9)
3.3 姿态角求解
结合姿态变换矩阵，导航体的偏航角和俯仰角可利用基线矢量通过下列式子求出：
(10)
(11)
俯仰角的范围一般为
可直接由式(11)得到。而偏航角的范围为0～2π，故还需结合式(10)，参照表1中的数据进行多值分析。
表1 航向角
Table 1 Reference table of yaw
bxk的符号byk的符号航向角(弧度)++ψk+-ψk+π--ψk+π-+ψk+2π
为了获取3个姿态角，需考虑双基线的情况，假设天线布局如图3所示， na安装在导航体中心，nb安装在导航体主轴上，na、nb构成基线1； nc安装在XBOYB平面上，na、nc构成基线2，两条基线间的夹角为φ。
图3 三节点布局示意图
Fig.3 Layout of three antennas
利用基线1，可根据式(10)、式(11)得到导航体的偏航角ψk和俯仰角θk。对基线2进行分析，利用式(9)可得其坐标为
在载体坐标系下，基线2的坐标为(l2sinφk,l2cosφk,0)，由坐标变换可得：
(12)
由式(12)可得:
式中，
需要说明的是，3个天线中对主天线(天线a)的要求较高，除了基本的测距功能外，还需具备测角功能或者较高的定位精度；而辅助天线只需具有测距功能即可。
4 基于EKF的姿态估计
第3节提出的算法只利用了观测信息，如果能够充分利用运动体的运动模型信息，将大大提高导航体姿态解算的效率和精度。本节从状态方程的角度，采用EKF算法进行状态估计，首先，构造如下状态向量：
式中，
表示k时刻偏航角和俯仰角的变化率。可得以下状态方程：
xk+1=Fxk+Gwk
(13)
式中，wk表示过程噪声，假设其方差为Q；F、G为系数矩阵，满足：
k时刻，使用导航体的姿态角，在当地地理坐标系下，可将主基线表示为
代入式(8)可得观测方程：
zk=h(ψk,θk)+vk
(14)
式中，
根据式(13)和式(14)，可按照以下步骤进行运动体的姿态估计：
步骤1：初始化：给定
步骤2：一步预测：
xk|k-1=Fxk-1
Pk|k-1=FPk-1FT+GQGT
步骤3：根据式(6)、式(7) 计算Rk
步骤4：计算增益矩阵：
式中，
步骤5：更新：
Pk=(I-KkHk)Pk|k-1
5 仿真验证
本节对所提算法的有效性进行仿真验证。同时，对主节点定位精度、邻节点定位精度和基线长度对测姿结果的影响进行研究。仿真中，设定初始状态 
误差参数Q=0.12I，σaj=σbj=0.1，滤波周期T=0.1 s。进行100次仿真，计算姿态角的均方根误差(root mean square errors，RMSE)来描述估计算法的性能，RMSE定义如下：
其中， N表示仿真次数; ek, j表示第j次仿真中k时刻的误差。
设定主节点和邻节点定位误差0.1 m、基线长度2 m，分别采用加权最小二乘法和EKF算法进行姿态解算，仿真结果如图4所示。显然，算法2(EKF算法)的测姿精度优于算法1(加权最小二乘法)。
图4 算法性能曲线
Fig.4 Comparison of algorithm performance
对比2种方法可以发现，加权最小二乘法具有使用范围广、约束条件少的特点；EKF算法能获得更高的精度，但需要初始值和准确的运动方程建模为前提。在实际应用中，可先根据加权最小二乘法确定姿态解算的初始值，然后根据EKF算法进行姿态估计。
设定主节点定位误差0.1 m、邻节点定位误差0.1 m，基线长度分别取1 m、2 m、5 m，研究基线长度对姿态估计的影响，结果如图5所示。随着基线长度的增加，测姿精度明显提高，但在工程应用中，还需要根据运动体的实际安装条件来确定基线长度。
图5 基线长度对测姿结果的影响曲线
Fig.5 Influence of baseline length on the result of attitude determination
令基线长度为2 m、邻节点定位误差0.1 m，主节点定位误差分别取5 cm、10 cm、15 cm，仿真结果如图6所示。随着主节点定位误差的增大，测姿误差明显增大，说明主节点定位误差对测姿精度有较大的影响。
图6 主节点定位误差对测姿结果的影响曲线
Fig.6 Influence of the main node positioning error on the result of attitude determination
令基线长度为2 m，主节点定位误差5 cm，邻节点定位误差分别取5 cm、10 cm、15 cm，仿真结果如图7所示，3种情况下的测姿精度变化不大，说明邻节点定位误差在一定范围内的变化基本不影响测姿精度。
图7 邻节点定位误差测姿结果的影响曲线
Fig.7 Influence of the neighbor node positioning error on the result of attitude determination
6 结论
基于测距信息建立组网姿态解算模型，根据加权最小二乘法求解基线矢量坐标，结合坐标转换关系得到姿态角。建立了运动体姿态变化的运动学方程和观测方程，并基于EKF方法设计了组网姿态和姿态角速度估计算法。仿真结果表明，相比最小二乘法，本文提出的姿态估计精度更高。另外，基线长度、主节点定位误差对滤波精度影响较大，基线长度越长、主节点定位误差越小，则滤波精度越高，而邻节点定位误差对精度几乎无影响。
[1] WIN M Z，CONTI A，MAZUELAS S，et al.Network localization and navigation via cooperation[J].IEEE Communications Magazine，2011(05)：56-62.
[2] SHEN Y，MAZUELAS S，WIN M Z.Network navigation：theory and interpretation[J].IEEE Journal on Selected Areas in Communications，2012，30(09)：1823-1835.
[3] GAO C，ZHAO G R，LU J H，et al.Decentralized state estimation for networked spatial-navigation systems with mixed time-delays and quantized complementary measurements：The moving horizon case[J].Proceeding of the Institution of Mechanical Engineers，Part G：Journal of Aerospace Engineering，2018，232(11)：2160-2177.
[4] GAO C，ZHAO G R，LU J H，et al.Decentralised moving-horizon state estimation for a class of networked spatial-navigation systems with random parametric uncertainties and communication link failures[J].IET Control Theory and Applications，2015，9(18)：2666-2677.
[5] 王巍.惯性技术研究现状及发展趋势[J].自动化学报，2013，39(6)：723-729.
WANG W.Status and development trend of inertial technology[J].Acta Automatic Sinica，2013，39(06)：723-729.
[6] 李子月，张林，陈善秋，等.捷联惯性/卫星超紧组合导航技术综述与展望[J].系统工程与电子技术，2016，38(04)：866-874.
LI Z Y，ZHANG L，CHEN S Q，et al.Ultra-tightly coupled SINS/GNSS navigation technology review and prospect[J].Systems Engineering and Electronics，2016，38(04)：866-874.
[7] HONG Y Z.Research on pose measurement of space non cooperative target based on binocular vision[D].Beijing：University of Chinese Academy of Sciences，2017：1-8.
[8] 马晓东，吕昊，张杰，等.基于双目视觉的固定翼无人机自主着陆技术研究[J].兵器装备工程学报，2019，40(11)：193-198.
MA X D，LV H，ZHANG J，et al.Research on autonomous landing of fixed wing UAV based on binocular vision[J].Journal of Ordnance Equipment Engineering，2019，40(11)：193-198.
[9] MA H L，XU S J.Magnetometer-only attitude and angular velocity filtering estimation for attitude changing spacecraft[J].Acta Astronautica，2014，102(04)：89-102.
[10] ZHU F，HU Z K，LIU W K，et al.Dual-antenna GNSS integrated with MEMS for reliable and continuous attitude determination in challenged environments[J].IEEE Sensors Journal，2019，19(09)：3449-3461.
[11] WU J，WANG C Y，CHEN Y Z.Single-epoch GPS L1 phase measurements for determining rigid-body attitudes[J].Journal of the Chinese Institute of Engineers，2015，38(03)：382-390.
[12] 许廷金，李杰，胡陈君，等.基于MEMS惯性器件的新型车载导航融合算法研究[J].兵器装备工程学报，2019，40(07)：159-163.
XU T J，LI J，HU C J，et al.Research on innovative vehicle navigation fusion algorithm based on MEMS inertial devices[J].Journal of Ordnance Equipment Engineering，2019，40(07)：159-163.
[13] CORDEIRO T F K，COSTA J P C L D，JUNIOR R T D S，et al.Improved Kalman-based attitude estimation framework for UAVs via an antenna array[J].Digital Signal Processing，2016，59：49-65.
[14] 秦永元.惯性导航[M].北京：科学出版社，2006：5-8.
[15] KIM E，KIM K.Distance estimation with weighted least squares for mobile beacon-based localization in wireless sensor networks[J].IEEE Processing letters，2010，17(06)：559-562.