张森， 郭锦标， 吴媛媛

(1.海军工程大学 电子工程学院， 湖北 武汉 430033； 2.31003部队， 北京 100089)

收稿日期：2019-05-07

基金项目：国家高技术研究发展计划项目(2014AA093405)；国家自然科学基金项目(61101205、41976177)

作者简介：张森(1982—)，男，副教授，硕士生导师。E-mail：johnson_xh@sina.com

0 引言

在国家“863”计划项目支持下，作者团队提出了一种新型水下导航系统，称之为多正交信号水下导航系统[1]。该系统由发射端和接收端组成，发射端包含4个发射阵元，同时发射一组相互正交的信号，用一个水听器作为接收端接收信号。多正交信号水下导航系统可为水下用户提供广播式的定位信息，能实现潜艇、无人水下航行器、潜水员及水下平台(如固定式水声监听阵等)等用户的自身隐蔽定位，有广阔的应用前景。

发射端可安装于运动平台，称为运动平台型，也可布放于海底，称为海底布放型。当海底布放型系统布放发射端后，无法直接获取该发射端的精确位置以及姿态角信息，因而需要通过校准得到这些参数，用以实现位置解算。即使发射端安装高精度的姿态传感器，可测量基阵的姿态，但基阵与姿态传感器安装时通常存在一定的偏差，也需要通过校准进行估计得出。

目前应用于水下导航定位系统校准的方法多是采用非线性最小二乘法[2-4]，该方法的核心是将非线性方程用泰勒公式展开，非线性方程线性化后采用迭代的计算方法得到方程未知量的解。姿态角估计时需要设置初始值，一般都是小角度的近似，而多正交信号水下导航系统的发射端位于水下，无法获得发射端姿态的先验知识，并且其角度也可能很大，使得需要初始值的传统非线性最小二乘法可能无法收敛。归纳起来，这是一个在无姿态先验知识条件下对发射端姿态的估计问题。

在忽略声线弯曲或进行等效声速近似[5]的情形下，发射端姿态的估计问题是一个空间坐标变换问题，除了最小二乘法外，还有学者利用四元数[6-7]、奇异值分解[8]、罗德里格矩阵变换[9-11]等方法进行相关研究，但罗德里格矩阵法只需估计3个参数，具有计算方便、适应任意旋转角的估计等优点。为此，本文将研究基于罗德里格矩阵变换的多正交信号水下导航系统姿态角估计，以此完成系统校准。

1 水下导航系统校准模型

图1所示为本系统的校准模型，主要包括水下的发射端和接收端以及试验船上的数据处理部分，船上安装的全球定位系统(GPS)用于测量接收端的位置信息，进行性能的比对。

图1 多正交信号水下导航系统校准模型示意图Fig.1 Calibration model of underwater navigation system based on multi-orthogonal signals

需要说明的是，图1为校准示意图，在实际应用时，将发射端置于水底，经过系统校准获取校准参数后，接收端和信号处理模块将安装在水下运动载体上，为其实时解算自身位置，不再需要GPS.

接收端通过接收信号完成自身位置的解算，自身位置分为两种：一种是相对于发射端的位置，称为相对位置；另外一种是绝对位置，即在地理坐标系下的位置。利用声学原理解算相对位置，采用坐标间的转换关系将相对位置转换为绝对位置，即采用(1)式完成：

(1)

2 基于罗德里格矩阵变换的姿态角估计算法

基于罗德里格矩阵变换的旋转角估计在三维坐标转换中表现出比非线性最小二乘法更好的效果[9]，下面分析其姿态角估计的过程。

旋转矩阵是一个正交矩阵，故而有下列的关系：

(2)

式中：上标T表示矩阵转置。

(3)

式中：I为单位向量。

坐标转换(1)式用罗德里格矩阵变换代替旋转矩阵，则可以得到

LXR=LXB+(I+S)(I-S)-1AXJ,

(4)

通过数学处理，可以将(4)式改写为

(I-S)(LXR-LXB)=(I+S)AXJ,

(5)

化简可得

S(AXJ+LXR-LXB)=LXR-LXB-AXJ.

(6)

为表达方便，令

AXJ+LXR-LXB=[p1,p2,p3]T，LXR-LXB-AXJ=[q1,q2,q3]T，

(7)

式中：p1、p2、p3、q1、q2、q3为实数。

此时，将(6)式展开可化简得

(8)

令

(9)

这样便可建立方程：

q=p×A.

(10)

在校准路径上进行n次测量，则可列n个方程，写成矩阵形式则可表示为

Q=P×A,

(11)

其中，

P=[p1;p2;…;pn]3n×3,Q=[q1;q2;…;qn]3n×1，

(12)

式中：p1、p2、…、pn，q1、q2、…、qn均为(9)式中的矩阵，下标表示测量的序号。

采用最小二乘方法解算出未知量[11-12]，则可求得

A=(PTP)-1PTQ.

(13)

(14)

最后再结合旋转矩阵的表达式即可解算出发射端的姿态角参数。

3 两种姿态角估计方法性能比较

本节将通过仿真比较两种姿态角估计方法的性能。仿真采用MATLAB 2014a软件，具体的仿真环境为：选择文献[2]的“⊕”字型的校准航迹，船绕着发射端航行，发射端的水平位置位于绕行轨迹内部，接收端在航迹上均匀取测量点，在每个测量点上记录下数据。假设水面深度为0，向下为正。图2所示为接收端的航行轨迹与发射端的位置关系，轨迹上“⊕”字型的点代表航迹上的测量点，下方一个单独点表示为发射端位置，经验声速值取1 500 m/s. 假设发射端的地理坐标为LXB=[xB,yB,zB]=[50 m,100 m,500 m]，接收端绕行的半径为400 m.

图2 “⊕”字型校准航迹Fig.2 Calibrated track of ‘⊕’font

3.1 可收敛性比较

文献[2]研究了非线性最小二乘法的校准过程，姿态角估计是基于小角度近似展开，而本系统在布放信标时不能保证其姿态角为小角度，因而基于本系统的非线性最小二乘法姿态角估计采用非小角度近似。由于非线性最小二乘姿态角估计算法的开始需要预先设置一个初始值，不失一般性，可设为(0°，0°，0°). 下面比较非线性最小二乘法与罗德里格矩阵变换法估计角度的可收敛性，假设系统信号传播模型不存在测量误差，仅用上述两种方法分别估计发射端的姿态角，得到估计结果如表1所示。

非线性最小二乘法的迭代控制精度为1×10-8，在小角度估计时其估计结果也是十分接近真实值，但随着角度的增大，所需迭代次数越来越多，到了较大的角度时出现错误，最后一行则是由于迭代发散而得不到估计结果。而罗德里格矩阵法没有非线性的近似，估计角度不存在误差，没有估计角度大小的限制，并且仅需要一次计算，简便高效。实际的水下发射端姿态角，往往可能是横摇角和纵摇角为较小的值，而航向角的值就可能很大。下面将横摇角和纵摇角设定为某个较小的值(不失一般性地设定为2°和3°)，改变航向角，两种方法估计结果如图3所示。从图3可以看出，当航向角超过±60°左右，非线性最小二乘算法会出现无法收敛的情况，而罗德里格矩阵变换法则能稳定地估计。

表1 两种算法姿态角估计结果比较Tab.1 Estimated attitude angles of different algorithms

图3 改变航向角时估计误差对比Fig.3 Comparison of estimated errors at different heading angles

3.2 估计精度比较

下面将研究不同GPS测量误差、不同信噪比(SNR)和发射端不同的姿态角时算法的估计精度，其中SNR决定了时延测量误差和阵元间时延差测量误差，如图4所示，图中估计误差为500次蒙特卡罗仿真的均方根误差。

图4(a)是SNR为20 dB、发射端姿态横摇角、纵摇角和航向角分别为5°、10°和25°时，估计误差随着GPS定位误差变化的情况，从中可以看出，随着GPS误差的增大，两种方法估计误差均增大，但罗德里格矩阵变换法估计误差小于非线性最小二乘法。

图4(b)是GPS定位误差为0.5 m、发射端姿态横摇角、纵摇角和航向角分别为5°、10°和25°时，估计误差随着SNR变化的情况，从中可以看出，随着SNR增大，二者估计误差均下降，且罗德里格矩阵变换法优于非线性最小二乘方法。当SNR大于25 dB时，误差下降变得平缓。

图4 不同条件时姿态角估计误差的对比Fig.4 Comparisons of estimated errors

图4(c)和图4(d)是GPS定位误差为0.5 m、SNR为20 dB时，发射端姿态角(横摇角、纵摇角和航向角)为不同时估计误差的变化情况。图中横坐标为图中参考角度的倍数，如图4(c)和图4(d)中参考角度分别为1°、3°、5°和2°、3°、5°，若当选择倍数为4时，则发射端姿态角分别为4°、12°、20°和8°、12°和20°. 比较图4(c)和图4(d)姿态角发生小的变化，非线性最小二乘方法对于不同的姿态角估计出现较大误差起伏，而罗德里格矩阵变换法起伏很小。从总体上看，姿态角越大，非线性最小二乘方法估计误差越大，而罗德里格矩阵变换法估计误差随角度变化较小。因此，罗德里格矩阵变换法对发射端姿态角具有更优的适应性和稳健性。

基于罗德里格矩阵变换的姿态角估计，不需要姿态角的先验信息，没有非线性化方程的线性近似，消除线性化过程带来的误差，计算过程简便、高效，不存在待估计角度大小的限制，因而与传统非线性最小二乘法的姿态角估计方法相比，具有更高的估计效率、估计精度和更强的适应性。

4 试验分析

2016年12月在湖北宜昌的隔河岩水库对多正交信号水下导航系统进行了湖上试验。试验区域水深约100 m.

试验时信标发射端布放于湖底，通过浮球标识信标位置，信号接收端测量船绕信标航行，船上同时采用差分GPS进行位置测量，试验现场如图5所示。测量船采取绕行的方式收集数据，经过数据处理可以获得校准参数，以此进行位置解算。与未经过校准的位置解算对比时，可以看出校准前后的变化。

图5 湖上试验图Fig.5 Lake trial

图6是测量船所采取的校准航迹，“GPS”代表的路线是由测量船上GPS测量出的航迹，“解算”代表的路线是水下导航系统解算出的接收端位置，此时的解算结果是系统未经过校准所得，可以看到解算结果偏离真实位置。

图6 未经过校准的位置解算与GPS测量的比较Fig.6 Comparison of calculated and GPS-measured results of position without calibration

图7 非线性最小二乘法校准结果Fig.7 Result of calibration by nonlinear least square method

图8 罗德里格矩阵法校准结果Fig.8 Result of calibration by Rodrigo matrix method

发射端位置估计采用相同的方法[13]，而姿态角估计则分别采用非线性最小二乘法和罗德里格矩阵法。水下航行体的深度信息一般会由自身携带的深度计等设备获取，因此这里仅比较水平方向的位置解算结果。将通过校准得到的参数用于校准接收端的位置解算结果，经校准后的结果与GPS测量结果比较，观察解算结果的误差分布，如图7、图8所示，最大作用距离Rmax约390 m. 从图7、图8上可以看出，采用非线性最小二乘法和罗德里格矩阵变换法得到的校准参数均能达到校准位置解算结果的目的，采用罗德里格矩阵变换法的校准结果误差点分布更为集中在原点附近。为更直观比较校准后的误差点分布，采用均方根误差统计校准的位置解算误差大小，如表2所示。从表2中可以看出，采用本文所研究的罗德里格矩阵姿态角估计方法，比非线性最小二乘法具有更佳的校准性能。

表2 校准结果及校准误差的比较Tab.2 Comparison of calibration results and errors

为验证校准参数的可用性，将两种校准方法得到的校准参数用于解算新的航迹，得到结果如图9、图10所示。该航迹最大作用距离Rmax约305 m. 采用均方根误差统计校准后的位置解算误差大小，如表3所示。从图9、图10和均方根误差均可看出，罗德里格矩阵变换法的测量误差明显小于非线性最小二乘法。

图9 非线性最小二乘法校准结果Fig.9 Result of calibration by nonlinear least square method

图10 罗德里格矩阵法校准结果Fig.10 Result of calibration by Rodrigo matrix method

表3 校准后误差比较

Tab.3 Comparison of errors after calibration

校准算法水平方向均方根误差/m相对定位精度非线性最小二乘法2.370.78%Rmax罗德里格矩阵变换法1.700.56%Rmax

5 结论

本文基于多正交信号水下导航系统，研究了利用罗德里格矩阵变换的系统校准算法，进行了姿态角估计。得到以下结论：

1)采用罗德里格矩阵变换法的姿态角估计，无需姿态角的先验信息，无需线性化近似，算法简单、高效，避免了线性化过程的误差。

2)非线性最小二乘法姿态角估计具有角度大小的限制，适用于3个方向的角度(横摇角、纵摇角、航向角)均为小角度的情况，而罗德里格矩阵变换法的姿态角估计不存在限制，适用性更强。

3)湖试结果表明，采用本文研究的姿态角估计方法校准效果更好。