大尺度欠驱动高速AUV导航系统研制

翟云峰，王冠学，徐国华，陈柱，张鑫

华中科技大学 船舶与海洋工程学院，湖北 武汉 430074

0 引 言

自主式水下机器人（AUV）是一种可在水下探测并作业的设备，在民用和军用方面有着广阔的应用前景。民用上，其可用于海洋资源的勘测与开发、海洋救险等；军事上，可用于排雷、侦察及战术海图等方面［1］。但这些都需要AUV具备高精度的导航定位能力。而精确的导航信息也是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可以为导航任务提供高效、实时的任务调度和中断管理，并提供实时的系统资源以及任务通信，从而避免导航软件开发过程中对系统资源的管理，提高系统的实时性和可靠性。

表1 传感器参数Table 1 Senor parameters

图2所示为AUV导航系统的硬件构架。其中导航计算机以基于VxWorks的CPCI系统作为硬件载体，包括集成第三代Intel Core i7处理器的核心模块、4通道的串口卡以及24 V的独立电源模块。导航传感器通过串口线与加固机内部的串口卡相连，向加固机发送导航数据。串口卡与CPU模块之间通过CPCI总线通信。导航计算机通过网线与主控计算机相连，采用TCP协议向水下主控系统发送解算后的导航数据。另外，加固机内通风截止波导板的设计则保证了导航系统良好的电磁兼容性能。

导航系统采用开放式硬件构架［5］，可以通过增加导航信息源来提高导航系统的精度。

3 导航系统软件构架

AUV导航系统的软件采用模块化设计，以降低程序复杂度，使程序结构清晰，便于阅读和理解，方便软件的设计与后续的完善和修改。

导航工控机通过串口实现PHINS，DVL和GPS等传感器数据的采集，然后通过组合导航算法解算出所需的导航数据，最后通过以太网形式与上层进行交互，即定时接收到水下主控系统的询问报文后，导航系统立即将最新解算的导航数据发送给水上主控系统。根据组合导航系统的功能，可设计系统的模块结构图，如图3所示。

系统主要由3类模块组成：

1）串口通信模块。该模块主要通过调用串口驱动程序实现串口通信，主要包括实时采集导航传感器的数据、对收到的数据进行校验及解包、获取组合导航解算所需的数据。

2）导航滤波模块。该模块主要包括传感器数据处理、组合导航算法等部分。数据处理主要是对采集的传感器数据进行预处理，即野值剔除、平滑滤波等；组合导航的输入即为数据处理后的传感器信息，通过联邦卡尔曼滤波器输出AUV当前时刻的速度、位置和姿态信息。

3）网络通信模块。该模块主要基于TCP/IP网络通信协议，将导航系统作为客户机，水下主控系统作为服务器，实现对服务器查询信息实时侦听，并向服务器发送导航信息的功能。

VxWorks嵌入式实时操作系统最显著的特点就是多任务，程序在CPU中都是以任务的方式运行［6］，因此任务的划分至关重要。根据H.Gomma原则［7］，将AUV导航系统的任务划分为PHINS数据采集、DVL数据采集、GPS数据采集、深度计数据采集、高度计数据采集、导航滤波处理、网络数据上传、错误管理及数据存储，此外，在调试阶段还包括导航数据显示。

由于AUV导航系统配备有多个传感器，且各传感器数据刷新比较快，为保证数据采集的实时性，串口数据采集任务均是基于中断共享的。而网络数据则包括导航数据、系统状态信息及校验码信息，通过TCP协议上传至水下主控系统。为了保证导航系统软件的可靠性，程序内通过错误管理任务实现对各任务的检测及处理，若检测到某任务异常，则及时进行处理并重新发起该任务。导航滤波处理任务在系统功能中处于核心地位，主要通过联邦卡尔曼滤波器进行数据滤波。所有任务基于任务优先级进行，不同优先级任务间以抢占方式进行调度，不同任务间主要通过访问共享数据结构进行任务间的通信。

4 组合导航滤波算法

4.1 无迹卡尔曼滤波理论

当系统高度非线性或Jacobian矩阵不可用时，EKF可能会导致滤波发散。针对EKF的这一缺点，Julier等［2-3］提出了基于UT变换的UKF方法。UT变换通过在原状态分布中以一定的规则选取一些均值及协方差等于原状态分布的点（Sigma点），将这些点代入非线性函数中，得到相应的非线性函数值点，并求出其均值和协方差。通过UT变换得到的均值和协方差引入到卡尔曼滤波器中，就构成了UKF滤波。由于UKF滤波过程中并没有经过线性化，没有忽略其高阶项，因此避免了EKF中因近似线性化引起的问题［8］。

对于一个n维随机向量x，已知其均值和方差分别为和P，通过UT变换，可得到下列2n+1个Sigma点

这些Sigma点对应的均值及协方差的权值分别为：

式（1）和式（2）中：下标m表示均值，c表示协方差；上标表示第几个采样点；为矩阵方根的第i列；β为非负的权系数，可以合并方程中高阶项的动差，当x服从高斯分布时，取β=2最优；λ为缩放比例参数，用来降低总的预测误差，，其中α为尺度参数，控制Sigma点的分布状态，取值范围为0≤α≤1，κ为待选参数，当状态变量为多变量时，一般选择κ=3-n，可以合并方程中高阶项的动差。对于离散化后的非线性系统：

式中：Xk，Xk+1分别为系统k和k+1时刻的状态向量；Zk为系统k时刻的观测量；Wk，Vk分别为系统的过程噪声向量和观测噪声向量；f[·]，h[·]为非线性函数。

采用Sigma点的UKF滤波递推步骤如下［2，9］：

3）计算系统状态量的一步预测及协方差：

式中，Q为系统噪声协方差矩阵。

4）根据状态量的一步预测值，代入式（3）的UT变换，产生新的Sigma点集

5）计算系统观测值的均值和方差：

式中，R为观测噪声协方差矩阵。

6）卡尔曼滤波增益更新：

7）系统状态更新和协方差更新。

4.2 联邦滤波

对于水下组合导航系统，一般采用集中式融合和分布式融合2种结构进行多传感器的信息融合。由于集中式滤波方法计算量大，容错能力低，而分布式滤波方法中，Carlson提出的联邦滤波方法计算量小，信息分配方式灵活，有很强的容错性能［10］，因此联邦滤波器在组合导航中应用更加广泛。

图4为联邦滤波器的一般结构。图中，Xg，Pg为全局状态估计值和协方差；Xi，Pi为第i个子滤波器送入主滤波器的局部估计值和协方差；Xm，Pm为主滤波器的估计值和协方差。联邦滤波器由若干个子滤波器和一个主滤波器构成，其中各子滤波器独立地进行时间更新和测量更新；而主滤波器一方面进行时间更新，另一方面将各子滤波器的结果进行融合，得到全局最优估计。

联邦滤波器算法流程为：

1）信息分配过程：在各子滤波器与主滤波器间分配系统的过程信息，其中，当i=m时表示主滤波器。

式中，βi＞0为第i个子滤波器的信息分配系数，并满足信息守恒原理：

2）时间更新：各子滤波器与主滤波器独立进行时间更新。

式中，ϕ为系统的一步状态转移矩阵。

3）量测更新：主滤波器无量测值，因此没有量测更新，量测更新仅在各子滤波器中独立进行。量测更新如下式所示：

4）信息融合：将各子滤波器的局部估计信息输入至主滤波器进行数据融合计算，得到全局最优估计。

在局部滤波器中，由于方差上界技术引起的信息丢失，导致局部滤波变成次优，但在主滤波器的融合过程中，这种非最优性被重新融合，使得融合后的全局滤波达到最优。

5 基于UKF的联邦滤波器的设计

将联邦滤波算法应用于AUV导航系统中，可以提高系统的容错性和可靠性，同时将UKF方法应用于联邦滤波器中，可处理具有非线性特征的组合导航系统的数据融合问题［11］，使导航系统能够获得更高的滤波精度，具有更好的鲁棒性。赵琳等［12］研究表明，综合考虑精度容错性和数据传输，无反馈联邦滤波器比有反馈联邦滤波器更具优势，因此本文采用无反馈模式的联邦滤波器。

仅考虑AUV在近水面航行，模型简化为在二维平面上的运动，导航系统的滤波器结构如图5所示。

在联邦滤波器中，以PHINS捷联惯导系统作为参考系统，对PHINS/DVL子系统采用速度组合，对PHINS/GPS子系统采用位置组合。

5.1 PHINS/DVL子系统的状态方程和观测方程

对于PHINS/DVL子系统，取状态变量为：

式中：δL，δλ分别为PHINS的纬度和经度误差；δVe，δVn分别 为PHINS东、北向速度误差；φ为PHINS航向角误差；δVde，δVdn分别为DVL测速误差。

PHINS捷联惯导系统的误差方程［13］为：

式中：Re为地球WGS-84模型半长轴，Re=6 378 137 m；f为地球扁率，f=1/298.257；RM为地球子午面半径，RM=Re(1-2f+3fsin2L)；RN为地球卯酉面半径，RN=Re(1+fsin2L)；wie为地球自转角速率。

对于DVL的测量误差，仅考虑其速度偏移误差，其误差方程为

式中：τd为速度偏移误差的相关时间；wd为零均值高斯白噪声。

建立PHINS/DVL子系统的状态方程：

将PHINS输出速度与DVL输出速度之差作为观测量，则PHINS/DVL的观测方程为

式中，H1，V1(k)为测量噪声，近似为高斯白噪声，且

5.2 PHINS/GPS子系统的状态方程和观测方程

PHINS/GPS子系统的状态向量为：

式中，δLg，δλg分别为GPS的纬度和经度误差。

GPS的定位精度受多种物理因素的影响，但相对而言，SA误差才是GPS定位误差中的主要成分。SA在时间域内的表现形式为变化缓慢、相关强烈，其动力学特性可采用一阶马尔科夫过程描述，GPS的东、北向定位误差［11，14］可表示为

式中：相关时间τSAλ，τSAL的取值范围为100～200 s；wSAλ，wSAL为零均值白噪声。

PHINS/GPS子系统的非线性状态方程为

PHINS/GPS的观测方程为

式中，H2，V2(k)为测量噪声，同样视为白噪声，且

AUV导航系统采用无反馈的联邦滤波器进行数据滤波，各子滤波器的信息分配系数分别为：βm=0，β1=β2=0.5。此时，主滤波器不参与信息分配，无需进行独立滤波，2个子滤波器平均分配信息后进行独立滤波，然后将各子滤波器的局部估计值XPHINS（公共状态）传到主滤波器进行融合，各子系统无反馈重置。则系统全局状态估计为：

当AUV潜至水下时，GPS无法接收卫星信号，PHINS/GPS子系统失效，则取信息分配系数βm=β2=0.5，β1=1。

6 仿真与试验

6.1 数值仿真

根据所设计的模型，在Matlab软件中进行数值仿真，对基于UKF的联邦滤波算法进行验证。同时，对于组合导航系统的线性化模型，采用传统的卡尔曼滤波方法进行仿真，仿真结果如图6～图8所示。

图6为实际的航行轨迹与组合导航滤波轨迹对比图。图6右方为局部放大图，可以更清楚地对比各自的轨迹。图7、图8分别为联邦无迹卡尔曼滤波器及传统卡尔曼滤波器下的位置和速度误差曲线。图9为这2种滤波器的对比。从图中可以看出，将UKF技术应用于无反馈的联邦滤波器中，既可以处理组合导航系统模型的非线性问题，避免截断误差，比传统的卡尔曼滤波方法具备更高的估计精度，同时又由于无反馈联邦滤波器良好的容错性能，使得导航系统具备更高的可靠性。

6.2 调试及试验

1）半实物仿真联调试验。该项试验主要是在室内的半实物仿真平台上进行AUV的全系统联合调试。经测试，AUV全系统能稳定运行，导航系统可实时向全艇提供位姿、速度等导航信息。

2）水池试验。试验场地为一条室内长条形水池，由于在室内GPS无法接收到卫星信号，因此仅使用PHINS与DVL进行组合导航试验，主要测试导航系统的测速精度。试验的测速数据如表2所示。

表2 水池试验速度数据Table 2 Velocity data of pool test

试验表明，导航系统在不同航速下的速度误差都较小，在0.02 m/s以内，达到了预期的试验目标。

3）湖上试验。为验证该导航系统应用于实际AUV的可行性，将导航系统固定于游艇上，在湖上进行了长距离航行试验。试验艇为典型的欠驱动系统，其运动特性类似于AUV的近水面航行。试验现场如图10所示。

在导航系统校准完成后，进行了累计里程10.6 km的水面航行试验，历时2 h，最大速度可达10 kn。试验的测量基准为一台定位误差小于2 m的差分GPS。图11为导航系统的运动轨迹曲线，图12为导航系统与差分GPS的位置偏差随巡航里程变化的曲线。

根据试验结果，导航系统连续航行累计10 645.2 m，历时2 h。相比GPS，导航位置相对偏差36.4 m，实际偏差率0.34%。导航系统的定位精度满足AUV的试验任务要求，可应用于大尺度欠驱动高速AUV。

4）艇载试验。将导航系统嵌入到AUV内部进行导航系统的实艇试验。试验过程中，AUV在近水面航行，因此艇内安装的单点GPS可作为导航系统的测量基准。根据GPS数据及导航系统的数据对比，可绘出经、纬度曲线及位置误差曲线，如图13～图15所示。

在AUV近3 h的航行过程中，导航系统的绝对位置误差不超过8 m，表明导航系统应用于AUV中仍具备较高的精度。

7 结 语

本文介绍了某新型高速AUV基于VxWorks嵌入式操作系统的组合导航系统，并对导航系统的硬件、软件构架进行了详述。提出了以捷联式惯导系统和多普勒测速仪为主、卫星定位系统为辅的组合导航方法，并设计了基于UKF的联邦卡尔曼滤波器。仿真结果表明，与传统的卡尔曼滤波器相比，联邦无迹卡尔曼滤波器具备更好的滤波效果，以及更高的容错性能，提高了导航系统的可靠性。通过半实物仿真联调试验、水池试验、湖上试验及艇载试验，充分验证了AUV组合导航系统的实时性、可靠性和可行性，表明导航系统具备较高的测速及定位精度，能够满足大尺度欠驱动高速AUV完成各种航行任务的需要。