集群AUV协作系统执行能力和执行效率的显著提升, 都建立在AUV自身的精确定位基础之上。为了提高AUV的定位精度, 近年来, 利用AUV间的相对距离信息, 实现集群AUV的协同定位, 成为协同导航定位的重要研究方向^[1-2]。

目前, 基于距离量测的协同导航定位技术的研究, 主要关注于定位算法的设计。张立川和杨建等^[3-4]分别基于扩展卡尔曼滤波与容积卡尔曼滤波理论研究了多AUV协同导航定位算法, 主要融合内部和外部传感器信息, 并且显著提高集群中AUV的导航定位精度。文献[5]通过研究集群AUV协作系统的可观测性表明, 若系统可观测, 可以有效利用量测信息进行协同定位, 若不可观测, 协同定位误差将趋于发散。文献[6-8]研究表明通过改变控制输入(设计复杂航行路径)可增加系统的可观测性, 进而提高定位精度。文献[9]在可观测性分析的基础上, 对单主AUV编队进行可观测度分析, 研究表明控制相邻两时刻主从AUV之间角度的变化量, 可以提高协同定位系统的可观测度, 进而提高协同定位精度。

除此之外, 集群AUV的编队构型与定位性能的关系被少数学者所研究。AUV的定位过程可以看作是状态预测过程, 利用预测状态的协方差阵评价协同定位性能的优劣, Cramer-Rao不等式下界(CRLB, 其下界的倒数为Fisher信息量)从理论上给出了未知参数无偏估计的协方差矩阵下界, 文献[10]在未考虑先验信息的情况下, 利用此理论建立与编队构型参数有关的定位性能评价函数, 并得出双主AUV的最优编队构型。文献[11]将先验信息考虑在性能评价函数中, 对单主以及双主AUV的最优队形进行了讨论; 提出利用误差椭圆代替后验协方差阵, 并选取误差椭圆面积建立性能评价函数的思想, 但是没有对误差椭圆和融合观测量信息过程进行详细说明与几何解释, 实质依旧利用协方差矩阵的行列式建立性能评价函数。文献[12]研究了最优编队构型的敏感性问题。当集群AUV以最优编队构型进行运动时, 较小的角度值偏差不会对定位性能产生明显的影响。

可以看出, 对于评价协同定位性能的研究, 主要基于协方差阵建立的性能评价函数进行分析, 通过性能评价函数的最大化得到最优编队构型, 从而实现定位误差最小化。虽然此方法可以应用于评价定位误差优劣以及设计编队构型, 但是不能直观理解协同定位误差变化, 也不能作为编队构型的直观设计工具。针对这一问题, 本文基于CRLB/Fisher信息量对协同定位过程的误差变化进行了几何解释, 详细阐述了协方差阵转化为信息椭圆的过程, 基于信息椭圆解释了观测量对先验误差的影响, 分析得出集群AUV协同定位过程最优量测信息来源。除此之外, 本文以信息椭圆面积为基础, 建立性能评价函数, 对单主、双主以及三主集群AUV编队构型进行了深入研究, 并对相应编队构型的定位系统性能进行几何解释以及实验仿真分析。

1 基于距离量测的集群AUV协同定位系统 1.1 基于距离量测的协同定位原理

基于距离量测的协同定位方法是近年来发展起来的一种定位技术, 通过量测主从AUV之间相对距离信息对自身航位推算的状态信息进行修正, 原理如图 1所示。协同定位系统中主AUV配备高精度惯性导航系统, 可以实现精确定位, 从AUV配备低精度航位推算系统、GPS与水声通信设备。为实现协同导航定位, 集群AUV协同定位系统往往配备一艘或几艘主AUV。执行任务前, 主、从AUV利用GPS进行时钟同步, 确保量测信息准确性; 航行过程中, 主AUV周期性发射声脉冲信号, 从AUV利用此信号解算两者相对位置, 并融合主AUV位置信息进行协同定位对从AUV位置误差进行校正。

图 1为基于距离量测的协同导航定位原理示意图, 图中几何椭圆表示AUV的位置误差。当从AUV与主AUV进行距离量测后, 从AUV对自身的位置误差进行协同校正, 使得自身位置的不确定性减小。

对于在水中稳态航行的AUV而言, 求解其位置信息是一个三维问题。然而深度信息可以通过压力传感器精确测量, 并且AUV的俯仰角与横滚角变化较小, 进而将三维问题转化为二维问题。对于二维平面定位问题, 一般至少需要2个量测量确定从AUV的位置。对于多主AUV的协同定位系统, 已经满足2个量测量的要求, 但是对于单主AUV, 要满足此要求, 主要依据移动矢径原理并利用相邻两时刻的量测信息进行协同定位, 原理如图 2所示。

1.2 系统模型

建立从AUV的二维运动学方程

(1)

式中, u_k, φ_k与Δt分别为从AUV在k时刻的前向航速、航向角与航位推算的采样时间。前向航速与航向角均受高斯白噪声干扰, 且系统噪声方差阵为Q_k=diag[σ_μk², σ_φk²]。

进而将从AUV的二维运动学方程进行简写

(2)

式中：x_k=(x_k, y_k, φ_k)^T为状态向量；f为非线性状态方程函数；u_k=(u_k, φ_k)^T为输入向量；w_k表示k时刻从AUV的过程噪声序列为高斯白噪声；G_k是过程噪声驱动矩阵。

建立主、从AUV的距离量测方程为

(3)

式中，, 为k+1时刻第i个主AUV与从AUV的量测信息, i=1, …, m。其中上标iL表示第i个主AUV, v_k为从AUV的量测噪声序列为高斯白噪声, N_k为量测噪声矩阵, h为非线性量测方程函数。

当i=1时, 即单主AUV协同定位系统, 量测距离信息为相邻两时刻的量测信息, 具体为

(4)

式中：r_k与r_k+1为相邻时刻量测量；D_{xk, k+1}与D_{yk, k+1}为从AUV由k至k+1时刻的移动矢径。

1.3 基于扩展卡尔曼的协同导航算法

集群AUV协作系统为非线性系统, 必须首先利用泰勒级数展开为线性系统, 随后利用卡尔曼滤波算法进行AUV状态的最优估计, 即为扩展卡尔曼的协同导航算法。具体步骤为:

状态一步预测

(5)

一步预测协方差阵

(6)

式中: Q_k为系统噪声方差阵; F_{k, k+1}表示f关于x_k的雅可比矩阵, 为f关于x_k在点处的偏导数; C_{k, k+1}表示f关于u_k的雅可比矩阵, 表示f关于u_k在点处的偏导数, 两者表示为

(7)

(8)

滤波增益矩阵

(9)

式中：R_k+1为量测噪声方差阵；H_k+1为距离量测方程的雅克比矩阵, 表示为

(10)

式中，θ_k+1ⁱ为k+1时刻第i个主AUV与从AUV的夹角。

单主AUV协同定位时, 利用相邻两时刻距离量测信息, 类比于从AUV与双主AUV的量测信息。对于量测方程的雅克比矩阵, 利用相邻两时刻从AUV与主AUV的夹角信息取代从AUV与双主AUV的夹角信息即可, 表示为

(11)

式中，θ_k与θ_k+1为相邻两时刻主从AUV的夹角, 分别取代了θ_k+1¹与θ_k+1²。

状态与协方差阵更新为

(12)

(13)

通过(5)~(13)式, 对从AUV的状态信息进行最优估计。其中为协同定位估计的状态信息, P_k+1为衡量利用算法估计出的状态信息精度。为了更清楚地理解位置状态信息精度, 将(9)式代入(13)式, 并求逆可得

(14)

将(14)式利用Fisher信息矩阵表示

(15)

式中：J_k+1为后验信息矩阵；J_{k, k+1}为先验信息矩阵；J_k+1^H为信息增益矩阵, 是对先验信息矩阵的修正。

对于位置状态信息的精度, 本文利用D-最优标准衡量, 即后验信息矩阵的行列式最大时, 定位性能最优

(16)

2 几何解释理论基础 2.1 信息椭圆

本文在考虑协方差阵时, 只考虑状态量x与y的误差, 并且其为正定矩阵。因而对于(15)式中信息矩阵, 可利用信息椭圆对其进行几何解释。本文以后验信息矩阵为例, 对其信息椭圆进行说明。首先为了方便, 可以将后验信息矩阵转化为对角阵

(17)

式中：a与b为后验信息矩阵的特征值(a>b)；P_ϕ为特征向量组成的旋转矩阵, 其定义为

(18)

因而, 定义后验信息椭圆为E(a, b, ϕ), a与b为椭圆长短轴参数, ϕ为旋转角, 具体如图 3所示。

由于信息椭圆参数求解较为繁琐, 因而本文给出信息椭圆的长半轴、短半轴以及旋转角的直接计算形式。若后验信息矩阵如(19)式所示

(19)

则椭圆长短轴参数和椭圆长轴与x轴的夹角a, b, ϕ为

(20)

2.2 协同定位误差的几何解释

由2.1节分析, 可以求得先验信息椭圆E₁(a′, b′, ϕ′)与信息增益椭圆E₂(μ, η, θ)的所有参数。进而可将(15)式利用椭圆参数表示为

(21)

对于后验信息椭圆参数, 可根据矩阵的奇异值分解得到, 具体为

(22)

式中：A=0.5(a′+b′+μ+η)；B=(a′-b′)+(μ-η)cos(2(θ-ϕ′))；C=(μ-η)·sin(2(θ-ϕ′))。

进而协同定位误差的几何解释, 如图 4所示。

图 4中点划线为信息增益椭圆, 虚线为先验信息椭圆, 实线为前两者合成的后验信息椭圆。对于D-最优标准而言, 后验信息椭圆面积越大, 定位性能越优, 进而可直观从后验信息椭圆面积确定协同定位性能的优劣, 并且从增加量可确定量测信息的优劣。根据(21)与(22)式对后验信息椭圆的面积进行分析, 为了简化分析, 定义其如(23)式所示

(23)

在先验条件已知的条件下, 根据(23)式可对观测信息进行选择或者对集群AUV的编队构型进行规划, 使得观测信息最优, 即确定角ϑ与椭圆面积S之间的关系, 最终使得估计状态信息最优。

3 集群AUV协同定位误差的几何解释及编队构型分析

根据(14)式可知, 后验信息矩阵由先验信息矩阵与信息增益矩阵组成, 然而根据(6)式可知, 先验信息由此时刻之前的所有后验信息和此时刻的状态信息组成, 难以确定具体的形式。为了更好地分析, 对后验信息矩阵进行简化, 其中R为量测噪声, 一般取常值, 可以忽略不计^[13-14], 进而后验信息矩阵可定义为

(24)

根据(10)式, 可以得到

(25)

可根据(20)式, 求出若干主AUV的后验信息椭圆参数a, b与ϕ, 并进行几何解释。并根据(23)式, 定义协同定位性能的评价标准

(26)

3.1 单主AUV的协同定位分析

根据第一节分析可知, 对于基于距离量测的单主AUV协同定位系统, 从AUV主要通过移动矢径原理, 使用相邻两时刻量测信息对位置误差进行修正。结合(11)式与(25)式, 可以得到单主AUV协同定位的后验信息矩阵表达式为

(27)

进而根据(20)式可得到后验信息椭圆E(a, b, ϕ)的所有参数：, a=2+2|cos(θ_k-θ_k+1)|, b=2-2|cos(θ_k-θ_k+1)|。

进而根据(26)式, 可定义单主AUV协同定位系统的性能评价标准为

(28)

进而对单主AUV协同定位系统的后验信息矩阵进行几何说明。对于椭圆面积而言, 若长轴与短轴的和为定值时, 当且仅当长轴与短轴相等时, 椭圆面积最大, 即性能指标Γ₁最大。即2+2|cos(θ_k-θ_k+1)|=2-2|cos(θ_k-θ_k+1)|时, 定位性能最优, 信息椭圆的几何形状为圆。考虑到实际情况, 因此可得当θ_k-θ_k+1=±90^o时, 定位性能最优, Γ₁ab=4, 最优编队构型如图 5所示, 几何解释如图 6所示。图 6中虚线表示后验信息椭圆, 实线表示最优后验信息椭圆。当量测信息最优时, 其几何形状变为半径为2的圆。为了证明几何分析的正确性, 对(28)式进行仿真, 取θ_k, θ_k+1∈[-180°, 180°], 如图 7所示。由图 7可知, 当相邻两时刻的量测夹角差的绝对值越接近90°, 定位性能越优, 反之, 定位性能越差。当相邻两时刻的量测夹角绝对值为90°时, 椭圆面积最大, 定位性能最优, 当相邻两时刻的量测夹角差为0°或±180°时, 椭圆面积最小, 定位性能最差。此分析结果与文献[9]基于可观测度分析结果一致, 即随着协同定位系统的可观测度越大, 定位性能越优, 并且当定位性能最差时, 相当于主从AUV的航行路径为不可观测路径, 从侧面证明了性能评价函数的正确性。

3.2 多主AUV集群的协同定位分析

对于二维平面的AUV协同定位, 从AUV仅使用单一时刻的量测信息不能进行状态预测, 至少需要2个量测信息对其位置进行无偏估计。此外, 已有研究表明在一定程度上, 主AUV数量的增加可以抑制协同定位误差的增长^[15-16]。因此, 在设计协同定位系统时, 往往利用多主AUV达到状态预测的要求。根据1.3节的分析, 对于双主AUV的距离量测方程的雅克比矩阵, 只需要将从AUV与单主AUV相邻两时刻的量测信息转换成从AUV与双主AUV的量测信息, 双主AUV与单主AUV协同定位分析过程及结论一致, 此处不再进行推导。但是实际中, 单主AUV需要设计复杂的航行路径, 使从AUV的状态预测性能更优, 但是一般不能达到最优解。而对于双主AUV只需通过设计编队构型, 即可达到理论分析的最优解。本节主要讨论在2个量测信息基础上, 融合第3个量测信息的协同定位系统, 即对三主AUV集群协同定位误差进行几何解释与编队构型进行研究。

本文认为使用2个观测量的量测信息矩阵为先验量测信息, 当第3个量测信息被引入协同定位系统时, 其为后验量测信息。因而根据(24)式, 三主AUV协同定位的后验信息矩阵表示为

(29)

进而后验信息矩阵利用几何椭圆表示为

(30)

进而根据(22)式可得到三主AUV协同定位系统的后验信息椭圆E(a, b, ϕ)的特征值为

式中, θ₁, θ₂与θ₃分别为三主AUV与从AUV的夹角, 并且对于后验量测信息椭圆而言, 椭圆短半轴为0, 定义为退化信息椭圆^[17], 只能提供一个方向的信息量, 为实现后验信息椭圆面积最大化, 确定退化信息椭圆方向尤为重要。

根据(26)式, 定义三主AUV集群协同定位系统的性能评价标准为

(31)

对三主AUV集群协同定位系统的后验信息矩阵进行几何说明, 令其长轴与短轴相等, 椭圆面积最大, 即性能指标Γ₃最大。求得三主AUV的最优量测信息角, 如表 1所示。

表 1中ϑ₁，ϑ₂与ϑ₃分别为三主AUV的量测信息夹角, 定义为ϑ₁=θ₁-θ₂, ϑ₂=θ₁-θ₃, ϑ₃=θ₂-θ₃并且ϑ₁+ϑ₂+ϑ₃=360°。

对于此4种最优位置状态, 考虑实际情况, 后3种可归属为同一种编队构型。因此, 三主AUV集群协同定位系统最优的编队构型有2种, 如图 8至9所示, 几何解释如图 10所示。

图 10中虚线表示先验量测信息椭圆, 实线表示最优后验信息椭圆。从图中可以看出, 此种情况相当于第3个观测量只使得先验量测信息椭圆的短轴增加, 而长半轴没有变化, 使其几何形状变为半径为3的圆。

此外, 对于(31)式及ϑ₁+ϑ₂+ϑ₃=360°而言, 倘若3个主AUV中有2个主AUV的夹角差绝对值为90°时, 性能评价函数为恒值, Γ₃8, 几何解释如图 11所示。

图 11中虚线为表示最优的先验量测信息椭圆, 即3个主AUV中有2个主AUV的夹角差绝对值为90°, 其几何形状是半径为2的圆, 相当于双主AUV最优协同定位系统的后验信息椭圆。实线表示特殊编队构型的后验信息椭圆, 从图中可以看出, 其几何形状的一个方向的轴增加变为椭圆。对于圆形而言, 所有方向的轴长相等, 无论任何方向轴长增加, 其面积增加量将一致, 因此从几何角度证明了理论分析的正确性。

为了对此恒值Γ₃8, 更深入理解, 本文分2种实际情况进行讨论:

第1种:若2个主AUV的夹角绝对值已为90°, 在确定第3个主AUV在协同定位系统中的位置时, 则可以忽略具体位置, 只需考虑AUV之间避障, 从AUV的位置状态预测性能将一致。此情况的编队构型如图 12所示。

第2种:若2个主AUV的夹角为任意值时, 在确定第3个主AUV在协同定位系统中的位置时, 只需使其与其他任意一个主AUV的夹角绝对值为90°, 从AUV的状态预测性能将达到一个较优的状态。此情况的编队构型如图 13所示。

为了证明几何分析的正确性, 对(31)式进行仿真。根据(31)式可知, 椭圆面积只与两两主AUV的量测信息的夹角绝对值有关, 因此当定义一个主AUV与从AUV的夹角时, 对椭圆面积的评价函数没有影响。本文假定从AUV在原点, 主AUV1相对于从AUV的夹角θ₁=10°时, 主AUV2与主AUV3相对于从AUV的夹角分别取θ₂, θ₃∈[0°, 360°], 仿真结果如图 14至15所示。

由图 14和15可知, 椭圆面积最大值为9, 最小值为0。当椭圆面积最大时, 位置状态预测性能最优, 如图 15中黑点所示, 共有8个点, 代表 3个主AUV的8种不同位置状态, 具体如表 2所示。

根据表 2的8种不同位置状态可知, 其与表 1分析的4种最优量测信息角相对应, 都对应着图 8至9中2种编队构型, 验证了上文分析的正确性。

当椭圆面积最小时, 此位置状态预测性能最差, 如图 15中菱形点所示, 共有4个点, 代表了3个主AUV 4种不同的位置状态, 具体如表 3所示。

根据表 3中3个AUV的位置状况可知, 当三主AUV与从AUV的航行路径为同一直线时, 从AUV的预测位置状态将最差, 此情况类比于单主AUV的不可观测路径, 实际航行时, 应该避免此类编队构型。

对于3.2节分析的特殊情况，如图 15中4条虚线所示, 其直线表达式分别为θ₂=100°, θ₂=280°, θ₃=100°与θ₃=280°, 椭圆面积恒为8。此4条虚线表示的实际情况为:对于基于三主AUV集群协同定位系统而言, 任意两主AUV的量测信息夹角差的绝对值为90°时, 无需考虑第3个领航艇的位置, 其椭圆面积始终为8。此情况较三主AUV的最优情况椭圆面积虽有减少, 但是此特殊情况只需要保证协同定位系统中的一个夹角差为90°, 较最优的编队构型少确定2个夹角差, 因而此编队构型将更容易实现。

4 数值仿真

基于扩展卡尔曼算法设计了3组仿真实验对不同状态的协同定位性能进行仿真比较分析。对于单主AUV与双主AUV可参考文献[9-11]对本文的理论结果进行验证。本节主要对三主AUV集群协同定位系统的理论分析结果进行仿真分析。

仿真过程中, 所有主、从AUV间的距离量测噪声为σ_r²=(5 m)²的零均值高斯白噪声; 从AUV的速度量测噪声为σ_u²=(0.4 m/s)²，与航向角量测噪声为σ_φ²=(2°)²的零均值高斯白噪声, 并且两者独立互不相关。采用蒙特卡洛法估算协同定位系统的从AUV的定位误差, 重复仿真计算100次, 取其统计平均值。

实验一  主AUV1、主AUV2、主AUV3与从AUV分别由(0, 500)m, (0, 1 000)m, (0, 1 500)m和(0, 0)m出发, 航向角均为90°并且沿同一直线航行, 航行速度均为2 m/s, 仿真实验的采样周期为1 s, 此情况为不可观测路径, 椭圆面积为0。图 16给出了从AUV的航位推算与协同定位误差比较图, 协同定位误差随时间增大趋于发散, 协同导航算法失效。

实验二  三主AUV分别按照表 4的位置状态出发, 航向角均为90°并且沿平行直线航行, 航行速度均为2 m/s, 仿真实验的采样周期为1 s。并且表 4中给出了不同位置状态的椭圆面积及仿真后的平均定位误差。图 17为主AUV在不同位置状态时, 从AUV的协同定位误差比较图。

根据表 4与图 17可知, 随着椭圆面积的增大, 协同定位误差减小。

实验三  主AUV1、主AUV2、主AUV3与从AUV分别由(-707, 707)m, (707, 707)m, (0, 1 000)m和(0, 0)m出发, 航向角均为90°并且沿平行直线航行, 航行速度均为2 m/s, 仿真实验的采样周期为1 s。此情况的主AUV1与主AUV2的夹角为90°, 随后始终保持此两主AUV的位置不变, 通过改变主AUV3的位置, 使得其与另外两主AUV的夹角改变, 从而设置一系列实验, 验证理论分析正确性。表 5给出了主AUV3的不同位置状态, 椭圆面积及平均滤波误差值。图 18给出了主AUV3在不同位置状态时, 从AUV的协同定位误差比较图。

从表 5与图 18中可以看出, 当两主AUV的量测信息夹角为90°时, AUV3位于不同位置时, 从AUV的协同定位误差基本一致。

因此, 在基于三主AUV集群协同定位时, 应避免不可观测路径, 并且可以通过本文中的后验信息椭圆面积计算公式(性能指标函数), 事先对协同定位误差有初步的了解。除此之外, 可以按照本文给出的最优编队构型设计AUV的具体位置, 并且在AUV的构型不能达到最优时, 给出了一种特殊情况, 只需要使2个主AUV的夹角差达到90°, 而不必考虑第3个主AUV的具体位置, 协同定位误差也能达到一个较优的等级, 但是其编队构型较最优编队构型更容易实现, 进而验证了理论分析的正确性。

5 结论

本文从Fisher信息矩阵角度研究了基于距离量测的集群AUV协同定位算法, 提出了协同定位误差的几何解释方法; 基于后验信息椭圆面积建立了编队构型的性能评价函数, 通过增大后验信息椭圆面积以降低协同定位误差, 从而估计最优的编队构型, 并指出当椭圆长短轴相等时, 编队构型最优。仿真结果表明, 通过最大化后验信息椭圆面积求取的最优编队构型, 相比其他编队构型, 定位性能有明显的提升。此外, 对于三主集群AUV协同定位系统, 以本文提出的特殊编队构型航行时, 定位性能较最优定位性能相差不大的情况下, 更容易实现。