Chinese Journal of Ship Research
大尺度欠驱动高速AUV导航系统研制
翟云峰，王冠学，徐国华，陈柱，张鑫 430074华中科技大学 船舶与海洋工程学院，湖北 武汉
网络首发地址：http://kns.cnki.net/kcms/detail/42.1755.TJ.20181102.1442.005.html期刊网址：www.ship-research.com引用格式：翟云峰，王冠学，徐国华，等. AUV导航系统研制［J］.中国舰船研究，2018，13（6）：78-86，93.大尺度欠驱动高速ZHAI Y F，WANG G X， XUGH ，et al. Development of navigation system for large-scale and high-speed underactuat⁃ ed AUV［J］. Chinese Journal of Ship Research，2018，13（6）：78-86，93.
ZHAI Yunfeng，WANG Guanxue，XU Guohua，CHEN Zhu，ZHANG Xin School of Naval Architecture and Ocean Engineering，Huazhong University of Science and Technology， Wuhan 430074，China
Abstract：［Objectives］ This paper details the development of a navigation system for a new model of large-scale and high-speed underactuated Autonomous Underwater Vehicle （AUV）.［Methods］ First， the hardware composition and software architecture of AUV navigation system are introduced in detail，and the integrated navigation system of the AUV is constructed on the basis of the VxWorks real-time embedded operating system. Considering the non-linear characteristics and fault-tolerance requirements of high-speed AUV navigation systems，Unscented Kalman Filter （UKF） technology is used in the federated filters，and a federated filter based on UKF is designed.［Results］ A two-dimensional plane model of the navigation system is simulated，showing that the federal unscented Kalman filter can obtain higher accuracy and better robustness than the traditional Kalman filter method. Finally，the real-time operation and reliability of the navigation system are verified via a semi-physical simulation debugging experiment，pool test，lake test and boat test.［Conclusions］The results show that this navigation system can meet the needs of the AUV in completing its missions. Key words：Autonomous Underwater Vehicle（AUV）；integrated navigation system；Unscented Kalman Filter（UKF）；federated kalman filter 收稿日期：2017 - 09 - 28 网络首发时间：2018-11-3 12:03基金项目：海洋工程国家重点实验室研究基金资助项目（201504）作者简介：翟云峰，男，1992年生，硕士生。研究方向：电气装备，水下作业技术自动化控制。E-mail：2422208135@qq.com王冠学，男，1992年生，博士生。研究方向：电气装备，水下作业技术自动化控制。E-mail：379759609@qq.com徐国华（通信作者），男，1965年生，博士，教授，博士生导师。研究方向：水下机器人作业技术，智能控制技术。E-mail：hustxu@vip.sina.com
Development of navigation system for large-scale and high-speed underactuated AUV
0引言
自主式水下机器人（AUV）是一种可在水下探测并作业的设备，在民用和军用方面有着广阔的应用前景。民用上，其可用于海洋资源的勘测与开发、海洋救险等；军事上，可用于排雷、侦察及AUV 1］。战术海图等方面［ 但这些都需要 具备高精度的导航定位能力。而精确的导航信息也是AUV实现自主作业并安全返回的必要条件。AUV的工作使命和航行环境对导航系统的可靠性及精度提出了非常高的要求。目前通用的导航模式均为采用多传感器信息融合的组合导航方法，相较单独的导航系统，该方法具备更高的导航精度和容错性能。组合导航系统一般采用卡尔曼滤波器。但是由于导航系统观测数据间的非线性关系，当前多采用扩展卡尔曼EKF滤波（ ）进行导航定位误差处理算法设计。EKF Taylor实际上是利用 函数展开的一阶项对非线性模型进行近似线性化，但当系统高度非线性EKF Julier 等［2-3时， 可能导致滤波发散。而 ］提出的无迹卡尔曼滤波器（UKF）则很好地克服了EKF的缺点，且其计算复杂程度并未增加。AUV，本文将基于某新型大尺度欠驱动高速VxWorks设计一种基于 嵌入式操作系统的水下机器人导航系统，同时考虑到导航系统的容错性和设计灵活性，以及导航系统模型的非线性特性，选UKF择基于 的联邦卡尔曼滤波器作为组合导航系统的数据融合算法。
1 AUV系统组成
AUV大尺度高速 的系统由水面监控系统、通1信传输系统和艇载控制系统组成，其组成图如图AUV所示。艇载控制系统均已嵌入到 内部，在顶层通过水面监控系统进行全艇的状态监控及任务AUV信息下达，从而实现 的自主作业。为了方便系统控制，导航系统要设计成黑盒，仅仅实现测量输入和信息输出。在高航速下，对AUV导航系统的实时性、可靠性和精度提出了更高的要求。
2 导航系统硬件构架
AUV导航系统主要配备了捷联惯导系统（PHINS）、多普勒测速仪（DVL）、卫星定位系统（GPS）、1深度计和高度计，其主要参数如表 所示。导航VxWorks6.6系统采用嵌入式结构设计，在 实时操AUV的导航定位。VxWorks作系统下实现 可以为 导航任务提供高效、实时的任务调度和中断管理，并提供实时的系统资源以及任务通信，从而避免导航软件开发过程中对系统资源的管理，提高系统的实时性和可靠性。 2 AUV图 所示为 导航系统的硬件构架。其中VxWorks CPCI导航计算机以基于 的 系统作为硬Intel Core i7件载体，包括集成第三代 处理器的核心模块、4 24V通道的串口卡以及 的独立电源模块。导航传感器通过串口线与加固机内部的串口CPU卡相连，向加固机发送导航数据。串口卡与CPCI模块之间通过 总线通信。导航计算机通过TCP网线与主控计算机相连，采用 协议向水下主控系统发送解算后的导航数据。另外，加固机内通风截止波导板的设计则保证了导航系统良好的电磁兼容性能。导航系统采用开放式硬件构架［ 5 ］，可以通过增加导航信息源来提高导航系统的精度。
3 导航系统软件构架
AUV导航系统的软件采用模块化设计，以降
低程序复杂度，使程序结构清晰，便于阅读和理解，方便软件的设计与后续的完善和修改。PHINS，DVL导航工控机通过串口实现 和GPS等传感器数据的采集，然后通过组合导航算法解算出所需的导航数据，最后通过以太网形式与上层进行交互，即定时接收到水下主控系统的询问报文后，导航系统立即将最新解算的导航数据发送给水上主控系统。根据组合导航系统的功3能，可设计系统的模块结构图，如图 所示。 3系统主要由 类模块组成： 1 ）串口通信模块。该模块主要通过调用串口驱动程序实现串口通信，主要包括实时采集导航传感器的数据、对收到的数据进行校验及解包、获取组合导航解算所需的数据。2 ）导航滤波模块。该模块主要包括传感器数据处理、组合导航算法等部分。数据处理主要是对采集的传感器数据进行预处理，即野值剔除、平滑滤波等；组合导航的输入即为数据处理后的AUV传感器信息，通过联邦卡尔曼滤波器输出 当前时刻的速度、位置和姿态信息。3 TCP/IP ）网络通信模块。该模块主要基于网络通信协议，将导航系统作为客户机，水下主控系统作为服务器，实现对服务器查询信息实时侦听，并向服务器发送导航信息的功能。VxWorks嵌入式实时操作系统最显著的特点CPU就是多任务，程序在 中都是以任务的方式运H.Gomma行［6］，因此任务的划分至关重要。根据AUV PHINS原则［7］，将 导航系统的任务划分为 数据采集、DVL数据采集、GPS数据采集、深度计数据采集、高度计数据采集、导航滤波处理、网络数 据上传、错误管理及数据存储，此外，在调试阶段还包括导航数据显示。AUV由于 导航系统配备有多个传感器，且各传感器数据刷新比较快，为保证数据采集的实时性，串口数据采集任务均是基于中断共享的。而网络数据则包括导航数据、系统状态信息及校验TCP码信息，通过 协议上传至水下主控系统。为了保证导航系统软件的可靠性，程序内通过错误管理任务实现对各任务的检测及处理，若检测到某任务异常，则及时进行处理并重新发起该任务。导航滤波处理任务在系统功能中处于核心地位，主要通过联邦卡尔曼滤波器进行数据滤波。所有任务基于任务优先级进行，不同优先级任务间以抢占方式进行调度，不同任务间主要通过访问共享数据结构进行任务间的通信。
4 组合导航滤波算法 4.1 无迹卡尔曼滤波理论
Jacobian当系统高度非线性或 矩阵不可用时，EKF EKF可能会导致滤波发散。针对 的这一Julier UT UKF等［2-3缺点， ］提出了基于 变换的 方UT法。 变换通过在原状态分布中以一定的规则选取一些均值及协方差等于原状态分布的点（Sigma点），将这些点代入非线性函数中，得到相应的非线性函数值点，并求出其均值和协方差。UT通过 变换得到的均值和协方差引入到卡尔曼UKF UKF滤波器中，就构成了 滤波。由于 滤波过程中并没有经过线性化，没有忽略其高阶项，因此EKF中因近似线性化引起的问题［8］。避免了对于一个n维随机向量 x ，已知其均值和方UT 2n+1差分别为 xˉ 和 P ，通过 变换，可得到下列Sigma =0，1，2，…，2n）：个 点 χˉ （i i χ0 = xˉ +(( )i，i
χi = xˉ n + λ) P = 1 2 n （1） -(( ) χi = xˉ n + λ) P ，i = n + 1 2n i
Sigma这些 点对应的均值及协方差的权值分别为：
( 0) ω = λ m n +λ ω ( 0) = λ +( - β) （2） 1 α2 + c n + λ ( i) ( i) ωm = ω = nλ+ ; i = 1 2 ，2n c 2 ( λ) 1 2 m表示均值，c式（ ）和式（ ）中：下标 表示协方差； ( )上标表示第几个采样点； P 为矩阵方根的第i i