一种基于极坐标模型的多AUV协同导航与定位算法

张淏酥，王 盛，王 涛，苗建明，龚 喜

（1.中山大学海洋工程与技术学院，广州 510275；2.南方海洋科学与工程广东省实验室(珠海)，珠海 519000；3.华中光电技术研究所‒武汉光电国家研究中心，武汉 430223；4.中山大学智能工程学院，广州 510006；5.广东省消防科学与智能应急技术重点实验室，广州 510006）

1 引言

水下无人航行器（Unmanned Underwater Vehicle，UUV）最近成为一个研究的热点。它在军事侦察、水下打击、水下搜救与打捞、水下资源勘探与开采等领域都有十分重要的应用。UUV又可以分为自主式水下无人航行器（Autonomous Underwater Vehicle，AUV）、远程操控式水下无人航行器（Remotely Operated Vehicle，ROV）两大类。ROV 由于需要一根连接到母船的电缆/光缆进行操控和通信，所以限制了其应用范围。AUV 具有更高的自主性，可以完成更多更复杂的水下作业任务。AUV 的导航定位是AUV 总体研制技术中的关键技术，对于AUV 十分重要。水下可以利用的导航信息十分稀少，所以水下的导航比空中导航研究的难度更大，面临更大的挑战。AUV 的导航定位分系统通常由惯性导航装置、多普勒计程仪和卫星定位装置组成。单独一个AUV作业时，其导航定位技术比较成熟，精度已经达到一个较高的水平。定位精度与导航系统的成本是一对矛盾，低成本的导航系统实现高精度的定位对于AUV 意义重大：可以大大地降低成本并极大地拓展AUV 的应用范围。但是，对于单体AUV，低成本导航系统很难实现高精度的定位。

对于AUV 集群，通过较优的协同导航算法可以实现集群中所有低成本的从AUV 的导航定位精度接近主AUV 的水平。在这样的集群中，主AUV 配备常规的高精度惯性导航系统（Inertial Navigation System，INS）+高精度多普勒计程仪（Doppler Velocity Log，DVL）+全球导航卫星系统（Global Navigation Satellite System，GNSS），从AUV 通常配备磁罗盘（Magnetic Compass，MC）+低精度DVL+GNSS 或者低精度INS+MC+低精度DVL+GNSS。由于高精度INS 占整个导航系统成本的60%～70%左右，高精度DVL 与低精度DVL 的差价一般在10 万元以上，所以从AUV的导航定位系统成本将大大降低，从而可以实现整个较低成本的AUV 集群达到较高的导航定位精度。协同导航的核心是：利用水声定位系统、INS、MC 等测量和计算得到的主从AUV 相对距离，相对方位等信息，再利用水声通信设备交互共享这些信息并通过合理的协同导航模型与算法实现所有从AUV 的高精度定位。采用这种技术，可以让一个制造成本较低的AUV 集群去执行一些需要高精度导航定位的水下作业任务（例如：水下资源勘探、测绘、打捞与搜救、探潜与反潜、反水雷作战、水下高精度的水雷布放等），从而可以完全取代传统的高成本AUV 集群（集群中所有AUV 都配置高精度INS）。所以，这种协同导航技术将大大扩展AUV 的应用，有效地降低各种水下作业的成本以及提高作业效率，对于国家大力发展海洋经济有重要的意义。

最常用的协同导航方法就是基于AUV 之间的距离或者距离+方位角的观测量在直角坐标系（笛卡尔坐标系）下建立扩展卡尔曼滤波（Extended Kalman Filter，EKF）的模型来提升每个AUV 的导航定位精度[1-4]。协同导航的模式一般分为无领航者模式（去中心化模式）[4]和主从式（领航者模式）。主从式一般又分为单领航者[1-3,5]、双领航者[6]和多领航者模式[2]。在无领航者模式中，所有AUV 配置同样精度的惯导，通过协同导航来对导航精度进行微弱的提升。而在有领航者模式中，领航者AUV 携带的惯导精度比跟随者AUV 的至少高一个数量级，通过协同导航，将整个AUV 团体的精度提升至与领航者的导航精度相当的水平。主从式模式还可以用来估算未知的定常洋流信息[7]。一般AUV 之间的距离/方位角信息通过声通机、水听器[8]、长基线水声定位系统[9]等设备来测量并进行交互。声学设备都存在声波通信延迟带来的各种问题，所以需要专门针对声学通信延迟进行补偿[10-11]。对于长期作曲线运动的AUV，运用直角坐标系建模将带来较大误差，有学者提出了基于极坐标系的协同导航算法[12]。

传统的AUV 协同导航算法都是基于直角坐标系，对于长期作直线或近似直线运动的AUV集群具有较高的精度。但是，大量的可观性分析表明：想要提高AUV 集群的可观性，必须依靠曲线运动等这类机动。另外，有时受限于水下地形（例如：必须不停地拐弯进行避障）或任务的特殊要求（例如：按某种特定的曲线运动进行高效的搜索与探测），AUV 集群必须长期处于机动运动状态。在这种情况下，采用常规的基于直角坐标系的协同导航算法，定位精度将变差，定位误差将有较大的增长。所以，需要开发一种适用于这种运动情形的新协同导航算法来提高精度。本文提出了在极坐标系下建立协同导航模型和相应的算法来解决定位精度不高的问题。

除了AUV，协同导航技术在其他各类无人装备/机器人中也得到了广泛的应用。例如，有学者提出将最优通信网络模型[13]、车联网[14]、概率估计算法和优化算法[15]、协同编队[16]、自适应协同[17]等用于无人系统的协同导航与定位。跨域协同[18-19]、有人/无人装备的协同[20-21]等也是协同导航领域未来的发展方向。

2 物理模型

2.1 直角坐标系情形

协同导航主要是利用AUV 之间的距离，方位角信息，所以建模也是针对这些信息。以仅观测距离信息的情况为例（假设AUV 的俯仰角接近零，忽略它的影响），模型如下：

其中，[xk,yk]T是k时刻AUV 在导航坐标系（一般采用东北天的直角坐标系）里的位置；Ψk是AUV 的航向角（相对于地理北向）；vk是AUV 的前向速度；uk是AUV 的横向速度；Δt是导航计算的时间间隔。

需要说明的是，由于惯导的高度通道是不稳定的，一般深度信息都通过AUV 搭载的深度计获得，所以建模时只用考虑AUV 实际位置投影到同一水平面（一般选择水域的表面），只对水平距离建模即可。

如果需要观测AUV 之间相对方位角信息，则模型中需要加入相对方位角的变化规律（与AUV 航向角变化速率相关）。有的情况下还可以获得AUV 的准确航速信息，航速当作观测量时，模型中需要加入速度相关的变化规律（与加速度相关）。另外，建立卡尔曼滤波模型时还需要考虑AUV 的IMU 中陀螺和加速度计的噪声。由于协同导航中相对距离是最重要的关键量，所以建模时加速度计的噪声更为重要。详细模型方程可以参考文献[1-4]。

对于在深海区航行的AUV，多普勒计程仪往往只能测出相对海流的速度，这将导致组合导航精度下降，所以洋流估计也是一个重要的研究方向。如前文分析，由于领航者携带了高精度惯导，所以通过协同导航在提高跟随者AUV 导航定位精度的同时，还可以估算出定常洋流的大小。利用主从式协同导航方式还可以估算定常未知洋流。这种情形下的建模如下[7]：

式中，上标S 代表从AUV，也就是跟随者；Uxk和Uyk分别为k时刻的x和y方向的定常洋流大小；δt是协同定位导航的更新时间；N=δt/Δt。为从AUV 在ti时刻的前向航行速度；为从AUV 在ti时刻的偏航角。

利用以上这种模型就可以估算出定常洋流信息，估算出洋流以后，再补偿到多普勒计程仪的测速信息上，就可以改进每个AUV 自身的导航定位精度。

任何水声设备都有一定的延时，为了提高协同导航的精度，需要对延时进行补偿。考虑水声延时的模型和常规情况的模型一样。考虑水声通信延迟的主从式多AUV 协同定位具体过程如下：①AUVM（主AUV）、从AUVS（从AUV）进行时钟同步准备；②AUVM在TS时刻对AUVS进行水声测距；③由于水声设备信息处理、激励声波信号需要一定的时间，所以AUVM延迟一段时间向AUVS广播自身的定位信息、AUVM与AUVS的间距信息，以及信息发送时刻的时间戳；④水声信息经过一定时间的传播，到达AUVS。AUVS接收到信息后，通过自身携带的世界标准时间系统计算本次接收的信息延迟时长，按照信息发送时间顺序，依次利用AUVM的相对定位信息实现对自身航推定位误差的修正，完成AUVS的协同定位；⑤AUVM与各AUVS重复完成过程②～④，实现主从式多AUV 协同定位。

2.2 极坐标系情形

对于长时间作曲线运动的AUV，利用直角坐标系会带来一些不便。这时，用极坐标系更加简便。以主从模式为例，AUVM和AUVS的两组极坐标与航向角分别记为ρM，φM，ΨM和ρS，φS，ΨS。可以找到如下的关系式[12]：

在惯性系下研究主和从AUV 的运动时，Ψ就是相对于惯性系y轴的方位角；在东北天的导航系下研究主和从AUV 的运动时，Ψ就是相对地理北向的方位角。

当AUVM在AUVS前面时：

当AUVM在AUVS后面时：

式中，R为主从AUV 之间的距离；φ为主从AUV之间的相对方位角。将ρM，φM，ΨM和ρS，φS，ΨS这6 个量作为状态变量，就可以建立卡尔曼模型。其中，状态转移阵为：

式中，T为卡尔曼滤波器的离散化时的采样周期。这种方法在极坐标体系下描述曲线运动十分简便，也可以保证较高的导航精度。

3 导航算法

3.1 直角坐标系情形

整个协同导航算法的核心就是模型的建立。通常可以选择的观测量为两个AUV 之间的距离、两个AUV 之间的相对方位角和两个AUV 各自的速度等。最常用的就是距离，距离+相对方位角，距离+主和从AUV 的速度，距离+相对方位角+主和从AUV 的速度这4 种情况。建模时应该根据观测量的情况和所需协同导航精度来选择状态变量进行建模。一般观测量越多并且精度要求越高，则需要选择的状态变量就越多，计算也就越慢。模型的系统噪声、量测噪声、初值等也十分重要。在AUV 不会长时间做曲线运动的大多数情况下，还是选择直角坐标系更加简便。

整个模型建立好以后，开始选择合适的滤波算法。由于协同导航的卡尔曼模型是一个典型的非线性模型，只能使用非线性的卡尔曼滤波算法，最常见的是EKF（扩展卡尔曼）算法。由于采用的都是标准的EKF 计算流程，这里不再赘述。对于需要补偿声学设备的延时的情况，可以使用延时EKF 算法（DEKF）[10]。DEKF 是在水声通信延迟EKF 方法的基础上，利用延迟滤波状态存储器保存的延迟过程中各时刻的滤波状态值，基于状态估计均方误差最小对量测信息延迟产生的协同定位误差进行修正。

除了EKF，还可以采用更先进的自适应滤波（AF）[3]、粒子滤波（PF）[22-23]、因子图算法[24]、M 估计算法[25]、无迹卡尔曼滤波（UKF）算法[26]等。一般情况下，越先进的算法意味着更高的精度和更大的计算量。需要根据AUV 搭载的运算硬件资源来选择合适的滤波算法。另外，在设计滤波算法时，如果在使用距离和速度等量测量之前对它们进行滤波，剔除掉其中的野值[27-28]，则可以进一步提升协同导航的精度和可靠性。

3.2 极坐标系情形

在极坐标情形下，同样是选择合适的观测量，建立极坐标下的卡尔曼模型，然后推导滤波算法。以选择相对距离+相对方位角为例，可以得到AUVM在AUVS前方时的量测矩阵为：

AUVM在AUVS后方时的量测矩阵与式(8)几乎相同，只是第二行的元素全部取反。建立完模型以后就可以采用常规的EKF 算法或自适应滤波等进行解算。

4 可观性分析

对于一个卡尔曼系统，其可观性分析十分重要。选择不同的观测量，系统就有不同的可观性。先分析直角坐标系情形下的可观测性问题。建立的坐标系关系如图1所示。

图1 中，ΣI、ΣM和ΣS分别是惯性坐标系、主AUV 坐标系和从AUV 坐标系。注意图1 中的航向角Ψ为相对于主AUV 的X轴向（主AUV 的前进方向）的相对方位角。基于移动矢径单领航AUV 协同导航定位示意图见图2。根据图2 可以得到表示式(9)。

图1 单领航者AUV 协同导航各个坐标系关系Fig.1 Schematic diagram of multi-AUV cooperative navigation and positioning system for single leader

图2 基于移动矢径单领航AUV 协同导航定位示意图Fig.2 Schematic diagram of cooperative navigation of multiple AUVs based on the MRV(Moving Radius Vector)

如果记ΣI中主AUV 和从AUV 的状态变量分别为XM=[xM,yM,ΨM]T和XS=[xS,yS,ΨS]T，则有式(10)所示的关系。

式中，ωM(t)和ωS(t)是航向角变化的角速率。注意此处的航向角都是相对于XI轴的方位角。如果把ΣI中的运动状态XS=[xS,yS,ΨS]T转换到ΣM中，记为[x,y,Ψ]T。考虑到极坐标下运算更加便捷，转换为极坐标后记为[ρ,φ,Ψ]T，则有式(11)所示的表达式。

一个典型的非线性系统如式(12)所示。

式中，x=[x1,…,xn]T为该系统的状态向量；u=[u1,…,ul]T为该系统的输入向量；y=[y1,…,ym]T为该系统的输出向量，其中，yk=hk(x)，k=1,…,m。

非线性函数hk(x)关于fi(x)的n阶Lie 导数为：

式中，fi(x)=[fi1(x),…,fin(x)]T；∇是梯度运算符，∙是向量的内积运算符。

有了以上Lie 导数定义，就可以建立非线性系统的观测性矩阵：

如果这个矩阵是满秩的，则该系统是局部弱能观的。下面分析仅观测距离的非线性系统的能观性。为了简化计算，把主从AUV 之间距离的测量值设为h(x)=(1/2)∙d2(t)。把式(11)写成式(12)的形式，则有：

式中，γ=Ψ‒φ。设该系统的 4 个输入分别为：u1=vM，u2=ωM，u3=vS，u4=ωS，则可得到如下观测性矩阵O：

如果只考虑u1和u3，则可得到如下观测性矩阵Ov：

类似地，只考虑u2和u4，则可得到观测性矩阵Oω。对O阵或者Ov+Oω阵进行分析，可以得到如下结论。在如下两种情况下系统是可观的[29]：①vM≠0，vS≠0，ωM=ωS=0，同时两个AUV 的运动轨迹不共线。②vM≠0，vS≠0，而且ωM和ωS中至少一个不为0。当在某一段时间内，两个AUV的航迹平行且运动速度相同，或者两个AUV 的航迹共线，则此段时间内系统是不可观测的，所以应该尽量避免这两种运动情况。总结以上情况，当主从AUV 之间距离为0（主和从AUV 位置重合）或不变时，该系统是不可观的。

下面，以极坐标系下的非线性系统为研究对象，推导该系统以距离和航向/方位角作为观测量时的可观测性。极坐标系下的非线性系统如式(18)和(19)所示。

当AUVM在AUVS前面时，相对距离和相对方位角的表达式h1和h2如下面所示：

注意，此处假定是在导航系中建立模型，所以ΨS是相对于地理北向的航向角，与式(10)和(11)中ΨS含义不同。主和从AUV 之间的相对方位角如式(20)中的h2所示。基于以上理论可以建立如式(21)所示的观测性矩阵。

式中，相关变量的表达式为：a1=ρS‒ρM∙cos(φS‒φM)；a2=ρS∙ρM∙sin(φS‒φM)；b1=a2/ρS；b2=‒a1∙ρS；c1=(h1)2∙f1‒(a1)2∙f1‒a1∙a2∙f2；c2=(h1)2∙(rS)2∙f2‒a1∙a2∙f1‒(a2)2∙f2；c3=(h1)2∙(b2∙f2‒b1∙f1)；d1=(h1)2∙rS∙f2‒2a1∙b1∙f1‒2a2∙b2∙f2；d2=‒(h1)2∙rS∙f1‒2a2∙b1∙f1‒2a2∙b2∙f2；d3=(h1)2∙(a2∙f2+a1∙f1)，f1是矢量f1(x)中的第1 个分量，f2是矢量f2(x)中的第2 个分量；OR和Oφ分别是相对距离和相对方位角的子块观测矩阵。

类似地，当AUVM在AUVS后面时，相对距离和相对方位角的表达式h1和h2如下所示：

式中，相关变量的表达式为：a1=ρS‒ρM∙cos(φS‒φM)；a2=ρS∙ρM∙sin(φS‒φM)；b1=a2/ρS；b2=a1∙ρS；c1=(h1)2∙f1‒(a1)2∙f1‒a1∙a2∙f2；c2=(h1)2∙(rS)2∙f2‒a1∙a2∙f1‒(a2)2∙f2；c3=(h1)2∙(b2∙f2‒b1∙f1)；d1=‒(h1)2∙rS∙f2‒2a1∙b1∙f1‒2a2∙b2∙f2；d2=(h1)2∙rS∙f1‒2a2∙b1∙f1‒2a2∙b2∙f2；d3=‒(h1)2∙(a2∙f2+a1∙f1)。

通过对式(21)和(23)分析可知：①当主和子AUV 之间的相对距离不变时，相对距离观测子块不可观。②当主和子之间的相对方位角不变时,相对方位角观测子块不可观。③当主和子AUV的位置重合时，系统整体不可观（两个子块都不可观）。④当相对距离和相对方位角都不变时，系统整体不可观。该结论与前面直角坐标系情形下的分析是一致的。被观测量只有在变化时，可观测性才比较强。所以，协同导航中应该尽量避免相对距离或相对方位角保持为恒定的情况。

5 基于实航数据的仿真测试

基于以上模型和理论，利用主/从AUV 实航的试验数据进行了协同导航的离线仿真，分析了不同模型和观测量对协同导航精度的影响。试验在某个湖中进行，主AUV 的口径为324 mm，搭载高精度光纤INS（其中光纤陀螺的零偏稳定性ε=0.008°/h，石英挠性加速度计的零偏稳定性∇=10 μg）、测速精度为0.4%±4 mm/s 的DVL（多普勒计程仪）、定位误差≤3 m 的全球定位系统（Global Positioning System，GPS）接收机。从AUV 的口径为200 mm，搭载低精度光纤惯导（光纤陀螺的零偏ε=0.1°/h，石英挠性加速度计的零偏∇=500 μg）、测速精度为2%±150 mm/s 的DVL（多普勒计程仪）、定位误差≤3 m 的GPS、航向角误差≤0.2°的磁罗盘。从AUV 搭载磁罗盘是为了获得自身较准确的航向角信息，从而可以得到相对方位角观测量。用于测量主/从AUV 相对距离的水声设备的主要指标为：测距误差≤0.1 m、量程≥4000 m、声波波束角≥200°。考虑到AUV在水面航行时水声定位测距系统的测量精度较差，所以在水面时使用无线电定位测距系统。由主AUV 的组合导航推算以及GPS 量测得到的航迹如图3所示。该试验中主和从AUV 的GPS 位置信息误差较小，可以认为是准确位置，在本研究中作为Benchmark 数据。本研究中，将从AUV自身进行航位推算、从AUV 自身进行组合导航、基于距离信息的传统（基于直角坐标系）协同导航算法和基于距离+方位角信息的传统协同导航算法这4 种算法作为对比对象，重点研究了基于距离信息（基于极坐标系）的新式协同导航算法和基于距离+方位角信息的新式协同导航算法这两种算法的定位精度。为了能够得到Benchmark数据，本研究总是在整个AUV 集群都处于水面能够接收到GPS 信息时才统计定位误差。这种对比研究和衡量误差的方法是协同导航研究中最常用的方法。

图3 主AUV 的航迹图Fig.3 Tracks of the master AUV

整个航行试验的时间持续了约25620 s（约7.11 h）。图3 是出发后约1000 s 时间内的一段航迹。当在水面航行时，主AUV 处于INS/DVL/GPS组合导航状态，在水下航行时，处于INS/DVL 组合导航模式。试验中，从AUV 始终在主AUV 附近1 km 左右跟随前进，主AUV 水下航行时，从AUV 也跟随进行水下航行。主AUV 和从AUV的巡航速度都尽量保持在2.5 kn 左右。图中有一段没有显示GPS 轨迹，就是因为主AUV 在水下航行。主AUV 的在某段时间内的姿态信息如图4（俯仰和横滚角）和图5（航向角）所示。

该信息为截取的一段长度为350s 的姿态角数据。图4 表明AUV 在上浮或下潜时俯仰角变化幅度较大，而横滚角变化不大，始终在0°附近波动。为了保持较好的可观性，主AUV 的航向角一直在变化，如图5所示。图5 中，0°与360°之间的跃变是由航向角的0°～360°的周期性引起的。该航向角依靠INS/DVL/GPS 组合导航（水面）或INS/DVL 组合导航（水下）计算得到，精度比较高。

图4 主AUV 的俯仰和横滚角Fig.4 Roll and pitch angles of the master AUV

图5 主AUV 的航向角Fig.5 Yaw angle of the master AUV

从AUV 的航向角由磁罗盘量测得到。本研究通过事后处理，综合低精度光纤惯导与磁罗盘量测信息，对磁罗盘的野值进行了识别。航向角一旦被判定为野值，就认为此时的量测值无效，卡尔曼滤波只进行时间更新而不进行量测更新。这样处理可以避免野值造成卡尔曼估算误差的快速增长。针对野值的判别与剔除，本研究采用统计方法(“3σ准则”)。根据高斯误差的理论，当测量值服从正态分布时，残差落在[−3σ,3σ]区间的概率≥99.7%，落在此区间外的概率<0.3%。因此，可认为残差落于该区间之外的测量值就是野值（即：异常值），这就是“3σ准则”。设Xi(i=1,2,…,n)为等精度独立测量的数值（可以是DVL 测量的速度、磁罗盘测量的角度、水声定位系统测量的距离和方位等信息），平均之后得均值，标准差σ可以按照贝塞尔公式进行计算。3σ准则定义为：凡是残差大于3 倍标准偏差的测量值判定为野值，应进行剔除。表达如下：

σ的计算公式（贝塞尔公式）如下：

大量的测试和数值分析表明：“3σ准则”可以成功地剔除大部分野值点。

相对方位角的观测信息（参考图1 和式(15)中的γ）就是从AUV 的艏向与主从AUV 连线矢量（主AUV 质心指向从AUV 质心的矢量）之间的夹角。有2 种计算方法：①由从AUV 的航向角减去主从AUV 连线矢量与地理北向的夹角（该夹角由测距定位系统得到）；②由从AUV 与主AUV 的航向角之差、主从AUV 连线矢量与主AUV 艏向的夹角（由测距定位系统得到）这两个角相减计算得到。本研究选用更加简便的方法①进行计算。正如前面的分析，本算法对磁罗盘、GPS、水声定位设备、水声通信设备等的输出进行野值判识。当传感器/设备的输出被判定为野值时，EKF 只进行时间更新而不进行量测更新。

主AUV 的DVL 测量的离底高度如图6所示。

图6 主AUV 的DVL 测量的离底高度Fig.6 Height from the lakebed measured by the DVL of the master AUV

图6 中，301s 处存在一个野值，此时的测速信息也为一个野值，在主AUV 的INS 的组合导航算法里、离线仿真中都剔除了这个野值。DVL出现测速野值时，主AUV 的组合导航滤波器只进行时间更新，不进行量测更新。除了为数不多的几个野值，DVL 的测速信息（AUV 在水面航行时）与GPS 的测速信息基本吻合，最大误差不超过0.12m/s，从而保证了INS/DVL 较高的组合导航精度。

图7 是从AUV 搭载的某款国产磁罗盘产品的实物照片，其核心指标如下：

图7 某款磁罗盘的照片Fig.7 Photo of a magnetic compass

（1）方位角：测量范围‒180°～+180°，分辨率0.01°，精度0.2°；

（2）俯仰角：测量范围‒90°～+90°（0°～+180°），分辨率0.01°，精度±0.1°；

（3）横滚角：测量范围‒180°～+180°，分辨率0.01°，精度±0.1°。

在实航试验中，主和从AUV 的时间系统都依靠各自惯导中的高精度晶振（两个晶振的精度相同），同时在水面时还利用了GPS 发送的时间信息和秒脉冲等进行校准，所以保证了较高的时间同步性。试验中，INS 的惯性测量单元（IMU）的数据更新周期为5 ms，GPS 和DVL 和磁罗盘为1s，定位测距系统为240s。主/从AUV 内INS的卡尔曼滤波器的时间更新周期和量测更新周期分别为5 ms 和1s。协同导航的EKF 滤波器的时间更新周期和量测更新周期分别为1s 和240s。当GPS 等传感器的量测信息丢失或出现野值时，卡尔曼滤波器只进行时间更新而不进行量测更新。从图3 可知，主AUV 的组合导航精度较高，轨迹几乎与GPS 点航迹重合。而从AUV 的惯导中惯性器件和DVL 的精度比主AUV 都低1～2 个数量级，所以不依靠协同导航时，从AUV 的组合导航定位精度较差，如表1所示。

从AUV 的导航定位有6 种方法：（1）从AUV进行自主航位推算。利用磁罗盘的航向角和DVL的测速信息进行航位推算。（2）从AUV 进行自主INS/DVL/磁罗盘组合导航。注意从AUV 即使在水面接收到GPS 信息，也只是存储当作准确位置，而不进行参与组合导航，以模拟真实的从AUV 长期水下航行无GPS 信息的情况。（3）在从AUV 组合导航的基础上，利用笛卡尔坐标系下的EKF 模型和相对距离观测量进行协同导航，记为C-EKF1。（4）利用笛卡尔坐标系下的EKF模型以及相对距离+相对方位角观测量进行协同导航，记为C-EKF2。（5）利用极坐标系下的EKF模型和相对距离观测量进行协同导航，记为P-EKF1。（6）利用极坐标系下的EKF 模型以及相对距离+相对方位角观测量进行协同导航，记为P-EKF2。离线仿真的结果见表1。

表1 仿真结果Table 1 Simulation results

从AUV 的准确位置选定为从AUV 的GPS轨迹点。所以，只有从AUV 在水面运动时才能够计算定位误差大小。表1 中选取的4 个点都是从AUV 在水面航行的时刻。表1 中的从AUV 总航程由水面GPS 轨迹点+水下P-EKF2 协同导航算法推算得到的。由表1 的结果可知：航位推算和组合导航算法都有误差随时间增大的特点。C-EKF1 算法依然服从这个误差随时间增大的规律，只是误差大小得到了一定的抑制。C-EKF2、P-EKF1 和P-EKF2 这3 种算法的误差都不再随时间单调增加，而是波动或保持大致恒定。特别是P-EKF2 算法可以大大地减小误差。

以上离线仿真测试表明：当主和从AUV 频繁做曲线运动等各种机动时，与基于相对距离的直角坐标系下的EKF 算法（C-EKF1）相比，基于相对距离+相对方位角观测量和曲线坐标系下协同导航算法（P-EKF2）可将从AUV 的导航定位误差减小至20m 以内。

6 结论

以基于单领航者的主从式AUV 集群为研究对象，利用AUV 的运动学方程和量测方程建立了极坐标系下的协同导航模型，设计了基于 EKF的导航滤波算法。该算法适用于长时间机动航行的AUV 集群。与常规的航位推算方法、低精度惯导的组合导航方法、基于距离信息的传统（基于直角坐标系）协同导航算法、基于距离+方位角信息的传统协同导航算法进行对比，研究了基于距离信息的新式（基于极坐标系）协同导航算法、基于距离+方位角信息的新式协同导航算法。另外，为了提高本算法的鲁棒性，本文模型考虑了各个传感器输出存在野值问题。当某个输出被判定为野值时，EKF 只进行时间更新而不进行量测更新。这种技术有效克服了水声设备、磁罗盘等的异常量测噪声对于协同导航系统的影响。数值仿真验证了本文算法的有效性，相对传统的从AUV 自身进行航位推算方法，本文提出的算法可以将误差减小至1.27%（15.9m/ 1251.3m），而基于距离信息的常规协同导航算法只能将误差减小至49.6%（621.1m/1251.3m）。