三应答器SINS/USBL紧耦合导航算法

张亚文，莫明岗，马小艳，邓继权

(北京自动化控制设备研究所, 北京 100074)



三应答器SINS/USBL紧耦合导航算法

张亚文，莫明岗，马小艳，邓继权

(北京自动化控制设备研究所, 北京 100074)

SINS/USBL组合导航误差因素主要包括USBL测距精度误差、USBL测角误差、USBL到SINS的安装误差以及应答器的位置误差，且现有USBL设备的普遍特点为：测距精度高而测角精度相对较差。针对上述误差因素和特点，提出一种三应答器SINS/USBL紧耦合导航算法，避免了USBL测角误差以及USBL到SINS的安装角误差等对导航精度的影响，仿真结果表明能够较大程度上提高组合导航定位精度。此外，该算法无需对安装角进行标定，使用更加便捷。

三应答器；捷联惯性导航；超短基线；紧耦合

0 引言

捷联惯导系统(SINS)能够为水下载体提供完备的导航信息，但随着时间推移，系统误差会累积发散[1]。超短基线(USBL)定位系统能够测量应答器相对于基阵的位置信息[2-3]，可以作为辅助导航系统来抑制惯导系统误差的发散。目前，SINS/USBL组合导航已成为水下导航技术研究的热点。针对传统单应答器SINS/USBL组合导航受USBL测角误差和安装角误差影响较大的问题，以及现有USBL设备测距精度高而测角精度相对较低的普遍特点，本文提出一种三应答器SINS/USBL紧耦合导航算法，能够避免USBL测角误差和安装角误差对导航精度带来的影响，且无需对安装角进行标定，使用方便，理论仿真结果表明本算法能够有效提高导航精度。

图1 三应答器USBL定位原理示意图Fig.1 The scheme of tri-responder USBL position theory

1 三应答器超短基线绝对定位原理及定位误差分析

(1)

利用事先标定测量得到的应答器位置和USBL实时测量的斜距值，便可求解该方程组得到基阵的绝对位置。这便是三应答器USBL绝对定位的几何工作原理。

传统超短基线系统的定位误差源可分为以下两类：

1) 传感器误差：USBL测距误差、USBL基阵姿态测量误差、USBL测角误差、测距交会求解造成的误差、超短基线基阵阵元相位差引起的误差、声传播引起的误差、声学噪声引起的误差等，此类误差大都受制于超短基线设备本身的性能指标。

2)系统误差：超短基线基阵和应答器安装引起的误差、校准误差，此类误差常存在于使用过程中，包括：基阵坐标系和姿态传感器坐标系安装不完全一致；应答器的初始绝对位置标校不准确。

本文提出的三应答器SINS/USBL紧耦合导航算法将避免USBL测角误差以及系统误差中的两坐标系间的安装角误差等误差项对组合导航精度的影响，并对USBL测距误差进行在线估计补偿。

2 紧耦合导航算法滤波模型及滤波方程

2.1 三应答器SINS紧耦合导航状态模型

三应答器SINS/USBL紧耦合导航算法误差状态模型包括SINS的误差状态模型和USBL的误差状态模型。

2.1.1SINS误差状态模型

(2)

捷联惯性导航误差状态方程为

(3)

2.1.2USBL误差状态模型

和单应答器USBL绝对定位误差相比，三应答器USBL绝对定位误差因素相对减小，主要包括应答器初始位置误差和USBL实时测距误差。应答器初始位置误差一般经过布放时的标定测量便可以达到较高的精度。本算法将USBL测距精度误差扩展为系统状态，即

(4)

(5)

其中

(6)

2.1.3 紧耦合导航系统误差状态模型

结合2.1.1和2.1.2可得，18维的三应答器SINS/USBL紧耦合导航算法误差状态量为：

(7)

SINS/USBL组合导航的卡尔曼滤波状态方程为

(8)

其中：

(9)

2.2 三应答器SINS/USBL紧耦合算法观测方程

本算法以载体(SINS)相对三个应答器的距离信息和USBL直接测量得到的载体相对于三个应答器的斜距信息之差为观测量。

(10)

对式(10)求偏微分可得

(11)

则有

(12)

其中

(13)

化简得

(14)

其中

联立式(12)和式(14)可得

(15)

(16)

综上可得，三应答器SINS/USBL紧耦合导航算法的观测量为

(17)

三应答器SINS/USBL紧耦合导航算法观测方程为

(18)

三应答器SINS/USBL紧耦合导航算法观测矩阵为

(19)

三应答器SINS/USBL紧耦合导航算法量测方程为

Z=HX+v

(20)

3 仿真与分析

以某浅水区域探测为应用背景，设计仿真条件为：惯导初始地理位置为北纬39.8°，东经116.2°，高度为0，以3m/s(约6节)的前向速度绕应答器阵，行驶约2700s，理想初始航向姿态角都为0°。惯导采样周期为5ms，USBL数据更新周期为2s，组合导航周期为2s。三应答器位置依次为：(39.8010，-50，116.1967)，(39.8020，-50，116.1997)，(39.8028，-50，116.1966)，如图 2所示。

图2 三应答器SINS/USBL紧耦合算法仿真轨迹Fig.2 Tightly integrated navigation simulation track

各传感器误差源和初始误差设置如表 1。按照本节提出的三应答器SINS/USBL紧耦合算法进行仿真，并与传统单应答器SINS/USBL组合导航仿真结果进行对比。

表1 传感器误差和导航初始误差Tab.1 Error of the sensor and initial navigation

对比三应答器SINS/USBL紧耦合导航算法和单应答器SINS/USBL组合导航算法。其定位精度(1)对比如表 2，位置、速度和航向姿态误差对比曲线如图 3～图 8。

表2 定位精度对比Tab.2 The comparison of position precision

图3 北向位置误差曲线 Fig.3 North position error

图4 天向位置误差曲线 Fig.4 Up position error

图5 东向位置误差曲线Fig.5 East position error

图6 水平位置误差曲线Fig.6 Level position error

图7 速度误差曲线对比 Fig.7 The comparison of velocity error

从表 2可以看出，三应答器SINS/USBL紧耦合算法和单应答器SINS/USBL组合导航算法相比，定位精度(1)有了很大的提高。从图 3～图 6位置误差曲线图可以看出，三应答器SINS/USBL紧耦合算法的定位误差更小，且更加平滑。从图 7速度误差曲线和图 8航向姿态误差曲线可以看出，三应答器SINS/USBL紧耦合算法速度精度更高，航向姿态误差的估计和修正能力更强。

图8 航向姿态误差曲线对比Fig.8 The comparison of attitude error

4 结论

本文针对传统单应答器SINS/USBL组合导航受USBL测角误差和安装角误差影响较大问题，以及现有USBL设备测距精度高而测角精度相对较低的普遍特点，提出一种三应答器SINS/USBL紧耦合导航算法，避免了USBL测角误差以及USBL到SINS安装角误差等对组合导航精度带来的影响。仿真结果表明，和传统单应答器SINS/USBL组合导航算法相比，本文提出的三应答器SINS/USBL紧耦合导航算法，能够提高导航精度，具有可行性。

[1] 兰华林,孙大军,张殿伦,李想.单应答器导航深海实验[J].海洋工程,2007,25(2):110-113.

[2] 隋海琛,田春和,韩德忠,王崇明. 水下定位系统误差分析[J].水道港口,2010,31(1):69-72.

[3] LinkQuest Inc. TrackLink 1500 USBL Tracking System, User’s Guide Version 1500.7.2.

[4] 秦永元.惯性导航[M].北京:科学出版社,2006.

An Underwater Tightly Integrated Navigation Algorithm of Tri-responder SINS/USBL

ZHANG Ya-wen, MO Ming-gang, MA Xiao-yan, DENG Ji-quan

(Beijing Institute of Automatic Control Equipment，Beijing 100074，China)

The errors of SINS/USBL integrated navigation mainly include USBL range-measuring error, angle-measuring error, the fixation error and the position error of the responder. Besides, most USBL instruments have quite high precision in range-measuring and quite short precision in angle-measuring. For these errors and features, the paper designs an underwater tightly integrated navigation algorithm of tri-responder SINS/USBL, avoiding the angle-measuring error and the fixation error. The theory simulation proves the algorithm can improve the navigation precision. Besides, it can be more convenient because no angle-fixation calibration is needed.

Tri-responder; Strap-down inertial navigation; Ultra short base line；Tight integration

2015 - 03 - 10；

2015 - 04 - 02。

张亚文(1990 - )，男，硕士，助理工程师，主要从事惯性/水声组合导航技术研究。

E-mail: zhangyawen0927@126.com

V249.32+2

A

2095-8110(2015)03-0029-05