基于组网导航系统的运动体姿态估计算法

刘 帅，赵国荣，韩 旭，李海君

(1.海军航空大学 岸防兵学院， 山东 烟台 264001； 2.中国人民解放军91001部队， 北京 100048； 3.中国人民解放军91115部队， 浙江 舟山 316000)

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 天线安装及载体坐标系示意图

本文采用“东北天”坐标系作为当地地理坐标系，以主节点所在的位置为坐标原点，Xn轴指向东(E)，Yn轴指向北(N)，Zn轴指向天顶方向。载体坐标系可由地理坐标系经过3次旋转得到，3次旋转分别对应偏航角ψ、俯仰角θ、滚转角γ。3次旋转可用以下变换矩阵表示[14]：

于是可得，由地理坐标系到载体坐标系的变换矩阵为

3.2 基线矢量的求解

按照图1所示，在运动体的主轴上安装2个组网导航节点。导航节点在安装时会出现安装误差，工程应用中，一方面要采用尽可能高的安装工艺来减小安装误差，另一方面可通过一定的标校手段获取安装误差值，并在解算过程中加入修正量抵消安装误差造成的影响。

图2 基于基线矢量的姿态估计示意图

(1)

由三角函数的相关性质可得：

(2)

结合式(1)、式(2)可得：

(3)

(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)

表1 航向角

为了获取3个姿态角，需考虑双基线的情况，假设天线布局如图3所示，na安装在导航体中心，nb安装在导航体主轴上，na、nb构成基线1；nc安装在XBOYB平面上，na、nc构成基线2，两条基线间的夹角为φ。

图3 三节点布局示意图

(12)

由式(12)可得:

需要说明的是，3个天线中对主天线(天线a)的要求较高，除了基本的测距功能外，还需具备测角功能或者较高的定位精度；而辅助天线只需具有测距功能即可。

4 基于EKF的姿态估计

第3节提出的算法只利用了观测信息，如果能够充分利用运动体的运动模型信息，将大大提高导航体姿态解算的效率和精度。本节从状态方程的角度，采用EKF算法进行状态估计，首先，构造如下状态向量：

xk+1=Fxk+Gwk

(13)

式中，wk表示过程噪声，假设其方差为Q；F、G为系数矩阵，满足：

k时刻，使用导航体的姿态角，在当地地理坐标系下，可将主基线表示为

代入式(8)可得观测方程：

zk=h(ψk,θk)+vk

(14)

式中，

根据式(13)和式(14)，可按照以下步骤进行运动体的姿态估计：

步骤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 仿真验证

其中，N表示仿真次数;ek, j表示第j次仿真中k时刻的误差。

设定主节点和邻节点定位误差0.1 m、基线长度2 m，分别采用加权最小二乘法和EKF算法进行姿态解算，仿真结果如图4所示。显然，算法2(EKF算法)的测姿精度优于算法1(加权最小二乘法)。

图4 算法性能曲线

对比2种方法可以发现，加权最小二乘法具有使用范围广、约束条件少的特点；EKF算法能获得更高的精度，但需要初始值和准确的运动方程建模为前提。在实际应用中，可先根据加权最小二乘法确定姿态解算的初始值，然后根据EKF算法进行姿态估计。

设定主节点定位误差0.1 m、邻节点定位误差0.1 m，基线长度分别取1 m、2 m、5 m，研究基线长度对姿态估计的影响，结果如图5所示。随着基线长度的增加，测姿精度明显提高，但在工程应用中，还需要根据运动体的实际安装条件来确定基线长度。

图5 基线长度对测姿结果的影响曲线

令基线长度为2 m、邻节点定位误差0.1 m，主节点定位误差分别取5 cm、10 cm、15 cm，仿真结果如图6所示。随着主节点定位误差的增大，测姿误差明显增大，说明主节点定位误差对测姿精度有较大的影响。

图6 主节点定位误差对测姿结果的影响曲线

令基线长度为2 m，主节点定位误差5 cm，邻节点定位误差分别取5 cm、10 cm、15 cm，仿真结果如图7所示，3种情况下的测姿精度变化不大，说明邻节点定位误差在一定范围内的变化基本不影响测姿精度。

图7 邻节点定位误差测姿结果的影响曲线

6 结论

基于测距信息建立组网姿态解算模型，根据加权最小二乘法求解基线矢量坐标，结合坐标转换关系得到姿态角。建立了运动体姿态变化的运动学方程和观测方程，并基于EKF方法设计了组网姿态和姿态角速度估计算法。仿真结果表明，相比最小二乘法，本文提出的姿态估计精度更高。另外，基线长度、主节点定位误差对滤波精度影响较大，基线长度越长、主节点定位误差越小，则滤波精度越高，而邻节点定位误差对精度几乎无影响。