基于约束最大后验估计的水下航行器组合定位算法

马 朋，胡安平

基于约束最大后验估计的水下航行器组合定位算法

马 朋，胡安平

（中国电子科技集团公司第二十研究所，西安 710068）

针对水下航行器声学/航位推算组合定位系统，利用声信标与水下航行器间的几何定位结构关系，构建获得水下声学距离量测等式与不等式约束条件，通过最大后验（MAP）估计准则与标准扩展Kalman滤波（EKF）方法相结合，设计出声学距离量测约束下的水下航行器组合定位算法，实现了水声距离量测与航位推算信息的有效融合。数值仿真试验结果表明，所设计的声学/航位推算组合定位算法可有效利用水声距离量测信息，实现水下航行器定位精度的提高。

水下航行器；组合定位；声学距离量测；约束最大后验估计

0 引言

高精度的定位能力是水下航行器完成协同作业任务的基础，无论是任务的规划和分配，还是自主行为控制的实现，都是建立在作业水下航行器位置精确已知的前提上。然而，由于无线电信号在水下传播时极易被吸收而急速衰减，导致陆地和空中目标精确定位常用的GPS和北斗系统等难以直接用于水下航行器定位过程[1]。

目前水下航行器定位技术主要可分为三类[2]：惯性/航位推算定位、地球物理学定位和水下声学定位。惯性/航位推算定位技术的优点在于可不依赖外部信息源进行全天候、全地域自主定位，但是各高精度惯性测量传感器价格高昂、体积较大，误差随着时间积累而趋于发散。而地球物理学定位技术则更多地与惯性/航位推算技术相结合，形成水下组合导航定位系统，虽然可以在一定程度上克服惯性/航位推算定位误差发散问题，但是严重依赖于地形、地磁和深度等先验信息，且易受复杂海洋环境影响。水下声学定位技术，则主要通过测量水下声信号发射器与接收器间的距离及方位，解算估计被定位目标位置信息。常用的水下声学定位方法包括长基线定位、短基线定位和超短基线定位等。长基线定位系统由布置在水底或水面的位置已知声信标基阵组成，各信标间的基线长度从上百米到几千米不等，类似于GPS定位原理，可利用曲面相交法来确定水下航行器相对于基阵的相对位置。短基线及超短基线定位系统将声信标基阵布置于固定载体（如水面舰船、作业平台等）上，信标间的基线长度较短，一般短基线长度不超过几十米，而超短基线长度则更是会小到几厘米，在定位过程中需要测量水下航行器与声信标间的距离和相位差。相比较而言，长基线定位精度最高，可靠性好，但是系统构成复杂，布放、标定及维护信标基阵均需大量的财力物力，而短基线及超短基线定位精度均不如长基线定位系统，好处在于系统构成简单，便于操作。

在水下航行器定位过程中，若只采用惯性/航位推算定位法，受限于水下航行器的体积大小与负载能力，其所能携带的惯性传感器定位精度有限，需要水下航行器定时浮出水面，利用GPS、北斗系统等进行位置修正，影响任务执行效率[3]；而若只采用水声信标进行定位，则受水下复杂环境的影响，容易出现水声定位信息时延与丢包等现象，造成部分时刻定位失败[4]。此外，在传统的水声定位方法中，多个声信标间的相对精确位置与距离信息未能被充分利用，声信标基线间的几何结构关系对于距离量测误差的约束作用也未被足够重视。

因此，本文采用声学/航位推算相结合的组合定位方式，在水下声学距离测量模型的基础上，利用三个水声信标与水下航行器间的几何定位结构关系，构建水下航行器声学定位过程中的距离量测等式与不等式约束条件，并将最大后验（Maximum A Posteriori，MAP）估计准则与传统扩展Kalman滤波（Extended Kalman Filtering，EKF）方法应用于水下航行器组合定位算法设计中，以实现外部声学距离与内部传感器测量信息的有效融合，降低水下航行器定位状态估计的不确定性。

1 基于距离量测约束的水下航行器声学定位方法

在水下声学长基线定位过程中，可通过水声通信测量技术得到已知精确位置的声信标与被定位目标（文中为水下航行器）间的相对距离约束信息，继而在声信标个数大于2个的情况下，利用圆相交或曲面相交定位法得到被定位水下航行器的唯一位置解，水下航行器声学定位示意图如图1所示。

图1 水下航行器声学定位示意图

1.1 水下声学距离测量

可以发现，水下声速随温度、盐度和深度的增加而增大，且温度为最主要影响因素。考虑到温度和盐度本身与深度相关，因此也可将水下声速看作水平分层变化下的深度函数，在固定深度差内的平均声速可简化如式（4）所示：

图2 典型水下声速变化示意图

在不考虑声线反转的情况下，以温跃层声速变化为例，如图2（b）～图2（c）所示，根据声线Snell折射定律，则有式（5）：

1.2 水下航行器声学定位中的距离量测约束

将式（10）代入式（9），可等效变换为以下矩阵和向量相乘的形式，如式（11）所示：

图3 圆相交定位原理

将式（13）～式（15）同时代入式（12），得到如式（16）所示：

2 距离量测约束下基于MAP的水下航行器组合定位算法

水下航行器声学/航位推算组合定位的关键在于实现水下航行器外部声学距离量测信息与内部航位推算信息的有效融合，选择合适的信息融合与定位算法将有助于提高水下航行器定位状态估计性能。考虑到所构造声学距离量测约束中含有不等式约束条件，常用的Kalman滤波器等方法难以应对处理，因此可借助MAP估计准则与传统EKF设计相结合的方法来设计水下航行器组合定位算法。

2.1 水下航行器组合定位系统模型

2.2 约束MAP估计在水下航行器组合定位中的应用

在使用MAP估计准则与EKF所设计水下航行器组合定位算法中，EKF方法主要用于状态协方差矩阵的预测与更新，同样也可被其它相类似滤波方法替代，算法具体步骤为：

（8）返回步骤（2）进行下一轮递推求解，直至定位停止。

3 仿真分析与验证

图4 水下航行器定位轨迹

在水下航行器利用航位推算法进行自主定位过程中，受所携带测向与测速传感器固有误差的限制，水下航行器自主定位轨迹会逐渐偏离真实航行轨迹，如图4所示。而在有效融合利用声学距离量测约束信息后，水下航行器自主定位误差发散得到了有效抑制，声学/航位推算组合定位轨迹与真实航行轨迹接近一致，且文中所设计基于约束MAP的组合定位算法相较于常用的无约束EKF算法具有更高的定位精度，如图5～图6所示。

此外，从图5～图6中可以发现，随着航行轨迹的改变，声信标与水下航行器间的几何结构关系也将发生变化，也就是说声学定位过程中的几何精度因子（Geometric Dilution Precision，GDOP）并非固定值，从而影响了声学/航位推算组合定位误差大小随之震荡变化。

图5 X轴组合定位误差

图6 Y轴组合定位误差

5 结论

本文在充分考虑三个声信标基线间几何结构关系的情况下，针对水下航行器声学/航位推算组合定位算法进行了研究。在对基于信号时间到达的水下声学距离测量方法分析的基础上，构建了三个声信标与水下航行器间的声学距离量测等式与不等式约束条件，并通过MAP估计准则和标准EKF相结合，实现了相关距离量测约束条件在水下航行器组合定位过程中的有效利用，继而通过求解约束最优化问题获得水下航行器定位结果。数值仿真试验结果证明了所设计算法相较于常用的无约束EKF组合定位算法具有更好的定位精度。

此外，本文将水下航行器组合定位问题转化为水声距离量测约束下的优化问题进行求解，而优化算法的选择对于组合定位结果具有直接的影响。因此，将粒子群优化、遗传算法等更加智能化的优化方法应用到组合定位结果的解算中，有助于进一步提高水下航行器组合定位性能，也将成为下一步研究工作的重点。

[1] Tan H P, Diamant R, Seah W K, et al. A survey of techniques and challenges in underwater localization [J]. Ocean Engineering, 2011, 38（14）：1663-1676.

[2] Paull L, Saeedi S, Seto M, et al. AUV navigation and localization: A review [J]. IEEE Journal of Oceanic Engineering, 2014, 39（1）：131-149.

[3] Dinc M, Hajiyev C. Integration of navigation systems for autonomous underwater vehicles [J]. Journal of Marine Engineering & Technology, 2015, 14（1）：32-43

[4] 田坦. 水下定位与导航技术[M]. 北京：国防工业出版社，2006.

[5] 邢志刚, 封金星, 刘伯胜. 水声测距数学模型研究[J]. 哈尔滨工程大学学报，2000，21（3）：24-28.

[6] Jensen F B, Kuperman W A, Porter M B, et al. Computational Ocean Acoustics [M]. 2nd edition. New York: Springer, 2011.

[7] Berger C R, Zhou S, Willett P, et al. Stratification effect compensation for improved underwater acoustic ranging [J]. IEEE Transactions on Signal Processing, 2007, 56（8）：3779-3783.

[8] 张宝成，徐雪仙. 声速不均匀修正对水声定位系统测距精度的影响[J]. 声学与电子工程，1992（4）：7-13.

[9] Meng W, Xiao W, Xie L. A projection based fully distributed approach for source localization in wireless sensor networks [J]. Ad Hoc & Sensor Wireless Networks, 2013, 18（1）：131-158.

[10] 秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理[M]. 西安：西北工业大学出版社，1998.

Integrated Positioning Algorithm for Underwater Vehicle Based on Constraint Maximum A Posteriori Estimation

MA Peng, HU Anping

According to acoustic/dead-reckoning integrated positioning system of underwater vehicle, the positioning geometric structure among acoustic beacons and underwater vehicle is used to construct the equation and inequality constraints of underwater acoustic range measurements. By combining the maximum a posterior（MAP）estimation criterion with standard extended Kalman filtering（EKF）method, an integrated positioning algorithm for underwater vehicles under the constraints of acoustic range measurements is designed, which achieves the effective integration of underwater acoustic range measurements and dead-reckoning information. Numerical simulation results show that the proposed integrated positioning algorithm can effectively utilize the underwater acoustic range measurement information to improve the positioning accuracy of underwater vehicle.

Underwater Vehicle; Integrated Positioning; Acoustic Range Measurement; Constraint Maximum a Posteriori Estimation

U674

A

1674-7976-(2021)-04-251-07

2021-04-25。马朋（1987.12-），陕西渭南人，工程师，博士研究生，主要研究方向为组合导航和信息融合。