模型辅助和洋流估计下的UUV导航定位方法

严浙平, 宋金雪



模型辅助和洋流估计下的UUV导航定位方法

严浙平, 宋金雪

(哈尔滨工程大学自动化学院, 黑龙江哈尔滨, 150001)

针对深海航行中无人水下航行器(UUV)受多普勒测速仪(DVL)测距的限制, 无法进行对底跟踪的问题, 提出了洋流估计下的UUV动力学模型导航定位方法。利用基于扩展卡尔曼滤波(EKF)的洋流估计方法, 解算出UUV作业海域洋流信息, 将洋流信息加入到UUV系统模型中, 对UUV动力学模型无法感知外界环境产生的误差进行了补偿, 从而提高了导航定位的精度。仿真结果证明了该算法的可行性以及有效性, 对未来UUV深海探索具有很好的实用性。

无人水下航行器(UUV); 动力学模型; 扩展卡尔曼滤波; 洋流估计; 导航定位

0 引言

《深海关键技术与装备》重点专项是我国“十三五”科技计划改革实施以来首批部署实施的重点研发计划。围绕进入深海-认知深海-探查深海-保障安全这一主线, 重点突破制约我国在深海领域发展能力的核心共性关键技术, 形成我国深海运载、探测装备谱系化和配套能力, 作业支持能力以及在深海油气和矿产资源开发方面的应用能力, 带动深海技术和装备产业的发展。全海深(最大工作水深11 000 m)无人水下航行器(unmanned undersea vehicle, UUV)研制、定位及探测技术是重点研究方向之一。

在海中执行任务时, UUV只有具备高精度的导航定位系统, 才能保证其安全作业并安全返回[1-2]。UUV常用的传统导航定位系统是利用多普勒测速仪(Doppler velocity log, DVL)、姿态传感器和压力传感器测量得到的UUV的速度信息、姿态角信息和深度信息, 从UUV已知的初始位置进行位置推算, 获得UUV的地理位置[3-4]。受到DVL测量距离的影响, 并不是所有情况下UUV都能进行对底跟踪。以Seatronics公司生产的导航DVL-DI Workhourse为例, 其最大的测量距离为200 m[4]。如图1所示, 当UUV深海作业时, 无法进行对底跟踪, 根据测流原理DVL测得UUV相对海水的速度。此时, 若不考虑洋流对UUV的作用, 采用UUV相对洋流速度进行航位推算, 将产生较大的定位误差。因此通常情况下采用超短基线(ultra short base line, USBL)定位系统实时定位UUV[5], 由于水面船和UUV的实时通信, 大大增加了UUV的能量消耗, 缩短了UUV作业时间。

图1 多普勒测速仪测距及深海无人水下航行器定位

针对上述问题, 文中将USBL定位系统与UUV动力学模型相结合, 并利用扩展卡尔曼滤波(extended Kalman filter, EKF)算法实现洋流信息的估计, 修正UUV系统, 得到洋流干扰下的UUV动力学模型, 提高了模型辅助下UUV导航定位精度。得到精准的UUV动力学模型后, 不再需要USBL实时通信, 从而减小UUV能耗, 缩短了UUV的工作时间, 仿真结果证明了此方法解算洋流信息的有效性, 明显降低了UUV导航定位误差。

1 UUV系统模型

1.1 运动学模型

研究UUV在任意空间中六自由度运动时, 通常采用2种正交坐标系: 大地坐标系000, 运动坐标系, 考虑到对洋流的分析, 建立流体坐标系′′′[6]。大地坐标系000采用右手坐标系原则, 定义0轴指向正北,0轴指向正东及0轴垂直于00面, 指向地心。

文中假设洋流是无旋的, 意味着每一个无穷小的流体角速度为零, 因此流体坐标系相对于大地坐标系不发生旋转[7], 其采用右手坐标系原则, 定义′,′,′ 轴分别指向正北、正东以及地心。运动坐标系的原点通常取在船体的舯纵剖面(一般位于UUV重心),,和轴分别为指向船艏的水平线、指向右舷的横切线及指向船底的纵切线。

各坐标系关系示意图如图2所示。

图2 各坐标系关系示意

根据各坐标系之间的关系, 推导出UUV各速度向量之间关系表达式

1.2 洋流干扰下的动力学模型

深海航行时, 由于洋流对UUV产生冲击力, 航行轨迹会发生偏移, 因此建立UUV动力学模型时需将洋流考虑在内。将UUV视为刚体, 根据动量定理和动量矩定理, 建立UUV六自由度动力学模型[8-9]

根据方程(2)和(3)可得

2 考虑洋流的导航定位算法

2.1 洋流估计

2.1.1 定深航行的UUV模型

在很多实际情况下, 非线性运动方程在一个操作点可线性化, 且基于操作点的扰动量进行线性化, 是对于式(6), 操作点及扰动量定义为

动力学模型(式(6))在操作点(V,η)泰勒展开式

定义:

线性UUV动力学模型

式中: 系统向量(t)[u v r x y z Ψ u v];(t-1)为1时刻系统噪声。

正定矩阵(t)表示系统噪声方差, 且

记

其中

式中:为UUV自身质量;(·),(·),(·)和(·)为黏性水动力系数;I为UUV相对于运动坐标系G的转动惯量;0,0分别为UUV初始横向速度和艏向角;(·)执行机构的增益;为螺旋桨转数;δ表示舵角。

2.1.2 USBL数学模型

UUV定深航行洋流估计时, 采用USBL定位系统实现UUV实时定位, 在UUV安装的收发换能器向水面船发送询问信号, 水面船上安装的声学应答器接收到信号后并发送应答信号, 根据水面船地理位置、传递的时间间隔以及收发换能器中多个基元交汇测量, 可以确定UUV与水面船之间距离以及UUV相对于水面船的方向角、和, 如图3所示。

图3 超短基线数学模型

水面船通过安装全球定位系统(global positioning system, GPS)获取位置, 得到系统的量测方程

根据EKF算法公式得到洋流估计算法, 滤波具体过程如下。

1) 一步状态和方差预测方程

2) 卡尔曼增益和状态更新方程

3) 估计方差方程

2.2 深海环境下UUV导航定位系统

深海环境下, 在海洋中层定深作业过程中, UUV导航定位算法主要由洋流估计模块和导航定位模块组成。洋流估计模块如上节所述; 导航定位模块中, 将洋流信息与UUV动力学模型信息融合计算得到的UUV对底速度、姿态传感器测取的UUV姿态角以及深度计测取的UUV深度信息结合, 经过综合预处理以及卡尔曼滤波剔除数据野值处理后, 作为航位推算算法输入信息, 推算出UUV的空间位置。在洋流估计过程中, UUV位置信息以USBL定位信息为准; 当估计得到洋流真实信息后, UUV位置信息通过航位推算算法获得, 此过程中, USBL不再需要实时通信, 只需当定位误差较大时进行位置校正, 修正定位误差。图4即为深海环境下UUV导航定位系统。

图4 深海环境下UUV导航定位系统

航位推算算法

3 仿真验证

3.1 试验设计

正如上文所述, UUV深海航行时, 洋流是主要环境干扰因素, 洋流参数初始化不正确会严重影响UUV动力学模型的性能。

试验中, 令UUV舵偏转角为常数, 螺旋桨在水平方向上对UUV产生定常推进力, UUV从初始位置(100 m, 100 m, 6 000 m)定深航行。如图5所示, UUV将做回转运动, 仿真中令UUV航行时间为600 s。可以看出, 无洋流干扰时, UUV航行轨迹为一个几乎完美的圆(见图5中实线), 但在洋流干扰下(假设洋流大小u=0.5 m/s,v=0.4), UUV航行轨迹发生漂移, 产生较大的位置偏差(如图5中虚线所示)。

图5 洋流干扰下的UUV航行轨迹

如图6, 由于洋流的干扰, UUV会产生300 m左右的纵向位移以及大概240 m横向位移, 较大的位移误差严重影响UUV作业及返航的安全性。因此, UUV执行作业任务时, 能否准确的估计洋流信息对UUV航位推算及定位至关重要。

图6 UUV横向及纵向的位移轨迹

3.2 洋流估计下UUV导航定位

EKF滤波算法中, 设舵偏转角为10°, 水平推进力为1 000 N; UUV的横向速度、纵向速度、偏航角速度以及艏向角的初始值分别为1 m/s,1 m/s,0°/s0°; UUV航行轨迹的初始位置为(,,)=(100 m, 100 m, 6 000 m); USBL的初始位置为(0, 0, 0)。考虑到洋流为水平面方向上未知的定向流, 且未知洋流不具有任何先验信息,洋流的估计初始速度为u=0 m/s,v=0 m/s, 仿真结果如图7所示。

图7 扩展卡尔曼滤波洋流速度估计

图7表示EKF算法下洋流估计结果, 可以看出, 文中提出的EKF算法能较准确且快速的估计洋流信息, UUV航行时间大概为100 s时, 估计出的洋流信息接近真实值。由艏向角随时间变化曲线(图8)可知, 艏向角变化周期为60 s, 即UUV完成一次回转运动的时间。因此, 定深航行的UUV只需做2次回转运动即可估计出洋流速度信息。

图8 UUV艏向角变化曲线

图9表示深海环境下, 洋流估计、多传感器及模型辅助的导航定位方法的定位误差, 即在定深水平面上的估计位置与实际位置的位移差。由于洋流的干扰, 0~100 s过程中产生的定位误差相对较大, 当洋流估计接近真实值, 航行过程中大部分定位误差小于1 m, 由此可见, 文中所提算法能有效补偿由于洋流对UUV作用所产生的偏移, 算法具有较好的精度和收敛性, 可满足实际情况下UUV在未知海域执行作业任务的需求。

图9 UUV定位误差

4 结束语

针对国家重点研发计划《深海关键技术与装备》重点专项中关于深海UUV定位技术研究的有关精神, 文中提出一种洋流估计和UUV动力学模辅助下的UUV导航定位方法。USBL测得的UUV位置信息, UUV动力学模型输出的状态向量, 利用EKF算法估计出作业海域的洋流信息。实现UUV深海不对底作业情况下的导航定位, 执行任务时不再需要USBL实时测量UUV的位置, 从而降低了UUV能量消耗, 提高UUV的工作时间。仿真结果表明, 文中提出的算法较为精准地估计出洋流的信息, 提高了深海环境下UUV导航定位精度。该方法满足实际需求, 基本可实现UUV深海定位, 对未来定深航行UUV的导航定位方法具有指导性意义。

文中涉及的USBL定位系统测量信息, 未考虑海流、海浪对水面船只的冲击扰动以及水声通信的时间延迟问题, 如何解决这些问题是今后研究的重点。

[1] 严浙平, 吴迪, 周佳加, 等. 基于UUV动力学模型的导航方法[J]. 华中科技大学学报(自然科学版), 2013, 41 (11): 108-113.Yan Zhe-ping, Wu Di, Zhou Jia-jia, et al. UUV Dynamic Model-based Navigation Method[J]. Journal of Huazhong University of Science and Technology(Natural Science Edition), 2013, 41(11): 108-113.

[2] Øyvind H, Oddvar H, Bjørn J. A Framework for Obtaining Steady-State Maneuvering Characteristics of Underwater Vehicles Using Sea-Trial Data[C]//Mediterranean Conference on Control and Automation. Mediterranean: IEEE, 2009.

[3] 周佳加. 基于扁平UUV动力学模型的导航定位与路径跟随控制方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2012.

[4] 张福斌, 鲍鸿杰, 段小伟, 等. 一种考虑洋流影响的AUV组合导航算法[J]. 计算机测量与控制, 2012, 20(2): 513-515.Zhang Fu-bin, Bao Hong-jie, Duan Xiao-wei, et al. A AUV Integrated Navigation Algorithm Considering the Effect of Ocean Current[J]. Computer Measurement & Control, 2012, 20(2): 513-515.

[5] 金鑫. 超短基线定位系统检测技术研究[D]. 哈尔滨: 哈尔滨工程大学, 2008.

[6] 严浙平, 吴迪, 周佳加, 等. 基于UUV动力学模型的实时海流估计方法研究[J]. 中国造船, 2012, 53(1): 99-106.Yan Zhe-ping, Wu Di, Zhou Jia-jia et al. An In-situ Ocean- current Estimating Method Based on UUV Dynamic Model[J]. Shipbuilding of China, 2012, 53(1): 99-106.

[7] Øyvind H, Oddvar H. Model-aided INS with Sea Current Estimation for Robust Underwater Navi- gation[J]. IEEE Journal of Oceanic Engineering, 2011, 36(2): 316-337.

[8] 王宏建, 陈子印, 贾鹤鸣, 等. 基于滤波反步法的欠驱动AUV三维路径跟踪控制[J]. 自动化学报, 2015, 41(3): 631-645.Wang Hong-jian, Chen Zi-yin, Jia He-ming, et al. There- dimensional Path-following Control of Underactuated Au- tonomous Underwater Vehicle with Command Filtered Backstepping[J]. Acta Automatica Sinica, 2015, 41(3): 631-645.

[9] 马骋, 连琏. 水下运载器操纵控制及模拟仿真技术[M]. 北京: 国防工业出版社, 2009.

[10] Martinez A, Hemandez L, Sahli H. Modelaided Navigat- ion with Sea Current Estimation for an Autonomous Underwater Vehicle[J]. International Journal of Advanced Robotic Systems, 2015, 12(12): 1-15.

[11] 边信黔, 周佳加, 严浙平, 等. 基于EKF的无人潜航器航位推算算法[J]. 华中科技大学学报(自然科学版), 2011, 39(3): 100-104.Bian Xin-qian, Zhou Jia-jia, Yan Zhe-ping, et al. Dead Re- ckoning Method for Autonomous Underwater Vehicles Based on EKF[J]. Journal of Huazhong University of Sci- ence and Technology(Natural Science Edition), 2011, 39 (3): 100-104.

(责任编辑: 杨力军)

UUV Navigation and Positioning Method Based on Model-Aided and Ocean Current Estimation

YAN Zhe-ping, SONG Jin-xue

(College of Automation Harbin Engineering University, Harbin 150001, China)

For deep sea navigation, unmanned undersea vehicle(UUV) cannot track to the bottom because of the limitation of Doppler velocity log(DVL) measured distance. Therefore, a navigation and positioning method based on the UUV dynamic model is proposed in this paper. The estimation method of ocean current based on the extended Kalman filter is employed to calculate the ocean current information in UUV operating area, then the information is added into the UUV system model to compensate the errors induced by the model due to its unable perception of external environment, hence the accuracy of navigation and positioning is improved. Simulation proves the feasibility and effectiveness of the proposed method. This method is able to be applied to future UUV deep sea exploration.

unmanned undersea vehicle(UUV); dynamics model; extended Kalman filter(EKF); ocean current estimation; navigation and positioning

严浙平, 宋金雪. 模型辅助和洋流估计下的UUV导航定位方法[J]. 水下无人系统学报, 2017, 25(4): 319-325.

TP6; TP242.6; TN929.3;

A

2096-3920(2017)04-0319-07

10.11993/j.issn.2096-3920.2017.04.003

2017-05-05;

2017-06-07.

国家自然科学基金项目资助(51679057); 2016年黑龙江省杰出青年科学基金项目资助(JC2016007).

严浙平(1972-), 男, 博士, 教授, 研究方向为水下无人潜航器的总体设计、智能控制和运动控制.