CN111928850B - 适用于极地冰架下环境的自主水下机器人的组合导航方法 - Google Patents

Abstract

本发明涉及水下组合导航技术领域，尤其涉及一种自主水下机器人极地冰架下环境的自主水下机器人的组合导航方法组合导航方法，针对极地水下长航程的自主水下机器人长时间航行的累积导航误差消除问题，实现自主水下机器人自主携带声学导航信标，自主水下机器人自主布放声学导航信标，自主标定声学信标布放三维空间位置，最后根据信标标定结果进行单信标组合导航，解决冰下长时间远程自主水下机器人导航位置的累积偏移问题。本发明的重点是解决冰下布放信标的位置标定问题，在信标标定成功后，再根据信标标定位置和单信标测距对冰下自主水下机器人进行导航。

Description

适用于极地冰架下环境的自主水下机器人的组合导航方法

技术领域

本发明涉及水下组合导航技术领域，尤其涉及一种自主水下机器人(简称AUV)极地冰架下环境的自主水下机器人的组合导航方法组合导航方法。

背景技术

在海洋工程、海洋科学调查过程中，自主水下机器人发挥越来越重要的作用，而在极地环境下使用自主水下机器人替代人类完成极地冰下-海洋***观测是当前支撑极地科考的重要工程装备研究的热点。针对极地冰架-海洋***的观测平台可以分为天基、冰基、自主水下机器人等多种类平台。天基观测平台包括卫星遥感和飞机挂载观测设备等，其优点是实现高效大面积观测，局限性是无法获取冰架内部的精细结构；冰基观测平台主要包含雷达、热水钻等，其优点是能够观测冰架，但局限性是空间覆盖率较低、观测灵活性不足。相对以上两类平台，自主水下机器人平台是高效冰下移动平台，既能到达冰架内部观测，又能够灵活大范围移动观测，所以自主水下机器人越来越成为极地冰下海洋观测的重要海洋装备。自主水下机器人需要在冰架下长时间测绘冰架形态、冰架下海底地貌、观测冰架下的海洋水体，其中导航***的作用是精确导引自主水下机器人到达观测目标位置，实现观测数据与观测空间的高精度位置匹配，保证冰架下海洋观测数据的质量，所以冰架下的导航问题是自主水下机器人在冰架下应用所面临的关键技术问题。极地冰架下导航的难点是深水罗经在极区的游移角误差随纬度的增高而放大，即深水罗经的航向精度随纬度升高而下降，航向精度的下降又进一步降低了自主导航航位推算的位置精度，所以在极地冰架环境下实现长时间、远距离范围内的高精度导航定位是水下导航***的重要技术挑战。传统水下组合导航***的外部辅助定位主要采用声学定位方式，根据声基线的距离不同，可将水下声学定位***分为长基线、短基线和超短基线。长基线的基线长度一般为100米到6000米，采用母船布放3个以上长基线定位信标，然后母船以围绕长基线声学定位信标水面航行航行的方式，利用母船自身搭载的大型超短基线和差分GPS对长基线定位信标位置进行导航标定，最后自主水下机器人在长基线阵内航行，不断融合长基线测距信息改进组合导航***位置精度。短基线的基线长度一般为1到50米，一般将短基线信标搭载在母船上，自主水下机器人在母船附近航行，不断融合短基线测距信息改进组合导航***位置精度。超短基线的基线长度小于1米，一般超短基线基阵搭载在母船上，自主水下机器人在母船超短基阵覆盖范围内航行，数据融合超短基线方位/距离测量改进组合导航***位置精度。在极地冰架下环境下，尤其南极冰架的厚度达到上千米，母船无法到达冰架上方使用短基线、超短基线对冰架下的自主水下机器人进行声学辅助定位，更无法在冰架上方布设长基线定位信标，更无法对布放信标进行位置标定，所以需要自主水下机器人自主携带信标从冰架下洞口到达冰架下方的海洋，自主布放信标到海底，自主对信标的位置进行标定，自主水下机器人通过测量与信标的距离对自身的导航误差进行修正，其中如何利用自主水下机器人对信标位置进行标定是极地冰下组合导航的关键步骤，标定的精度直接影响到自主水下机器人的外部量测精度，进而决定冰下作业自主水下机器人的导航精度。近年来，研究人员在不依托母船工况下对布放信标进行位置标定方面取得了一定的进展，但是其平台依然是具有实时接收GPS信号的无人船、具有上浮到水面接收GPS信标的自主水下机器人，而极地冰下环境的自主水下机器人不具备接收GPS定位的客观条件，再考虑到极地冰架下自主水下机器人自主导航精度下降明显，所以需要新的适用于极地冰架环境的自主水下机器人单信标组合导航方法，利用信标与自主水下机器人的纯距离间接观测，将自主水下机器人内部导航传感器计算的移动矢量径引入观测方程，再通过机动要素分析规划观测的机动路径，提高信标标定的精度和可观测度。

发明内容

本发明涉及水下组合导航技术领域，尤其涉及一种自主水下机器人(简称AUV)极地冰架下环境的自主水下机器人的组合导航方法组合导航方法，针对极地水下长航程的自主水下机器人长时间航行的累积导航误差消除问题，实现自主水下机器人自主携带声学导航信标，自主水下机器人自主布放声学导航信标，自主标定声学信标布放三维空间位置，最后根据信标标定结果进行单信标组合导航，解决冰下长时间远程自主水下机器人导航位置的累积偏移问题。极地冰下自主水下机器人单信标组合导航的关键技术问题是解决自主水下机器人精确标定信标问题的难题，信标位置标定也是单信标组合导航的基础，所以本发明的重点是解决冰下布放信标的位置标定问题，在信标标定成功后，再根据信标标定位置和单信标测距对冰下自主水下机器人进行导航。本发明包括：自主水下机器人在冰架下内布放后，自主水下机器人利用自身航位推算位置、自主水下机器人与信标的测距信息，通过自主水下机器人机动航行的方法改善信标标定方程的可观测性，引入相邻时序测距信息递推提高标定计算效率，获得高精度的信标位置估计。最后，将标定结果代入单信标组合导航滤波器，实现对冰架下环境下航行的自主水下机器人的高精度导航。本方法能够有效地处理冰架下环境下自主水下机器人的导航问题，采用机动航行优化观测性策略和移动矢量径策略，提高了***的可观测性，节约了导航计算的时间成本，具有较强的工程应用价值；本方法移植方便，扩展性强，也适用于极地冰下滑翔机、极地有缆遥控水下机器人等的组合导航应用领域。

本发明为实现上述目的所采用的技术方案是：

适用于极地冰架下环境的自主水下机器人的组合导航方法，包括以下步骤：

1)自主水下机器人按照规划轨迹的约束条件对信标进行机动测距；

2)计算信标的初始位置和方差估计；

3)计算自主水下机器人在当前时刻的量测雅各比矩阵；

4)根据雅各比矩阵计算信标在当前时刻的位置估计和方差估计；

5)根据当前时刻信标的位置估计和方差估计，判断是否满足结束条件，若满足，记录当前时刻信标位置；否则，返回步骤3)；

6)水下机器人根据记录的信标位置作为参考点进行导航。

所述规划轨迹的约束条件为：

其中，ρ_i,i+1表示点X_i到点X_i+1的位置矢量，ρ_i+1,i+2表示点X_i+1到点X_i+2的位置矢量，ρ_c是线性化判定常数，fabs为取绝对值。

步骤2)包括：

求解信标的初始位置估计具体为：

其中第1次收到测距信号的时刻为t₁，自主水下机器人在时刻t₁的位置为X₁＝(x₁,y₁,z₁)^T,同理定义第2、3、4、5次数测距时刻的自主水下机器人位置为X₂、X₃、X₄、X₅，它们均由自主水下机器人搭载的多普勒计程仪、深水罗经、深度计直接测量获得，R₁、R₂、R₃、R₄、R₅分别表示自主水下机器人在时刻t₁、t₂、t₃、t₄、t₅与信标的测距，它们由测距仪直接测量获得，H₀和Y是中间变量，；

P₀表示信标的初始位置估计的方差估计，计算方法为：

其中，X_L表示自主水下机器人布放信标时自身的三维位置，E(.)表示数学期望的计算符号。

所述量测雅各比矩阵为：

H_k＝[ρ_i-4,i-3 ρ_i-3,i-2 ρ_i-2,i-1 ρ_i-1,i]^T

其中，H_k为自主水下机器人在时刻t_k的量测雅各比矩阵，ρ_i-4,i-3表示点X_i-4到点X_i-3的移动矢量径；ρ_i-3,i-2表示点X_i-3到点X_i-2的移动矢量径；ρ_i-2,i-1表示点X_i-2到点X_i-1的移动矢量径；ρ_i-1,i表示点X_i-1到点X_i的位移动矢量径。

ρ_i-4,i-3、ρ_i-2,i-1、ρ_i-3,i-2、ρ_i-1,i的计算方法为：

其中，u(t)表示自主水下机器人在时刻t的航行速度，由自主水下机器人搭载的多普勒计程仪直接测量；ψ(t)和θ(t)分别表示自主水下机器人在时刻t的航向角、纵倾角,由自主水下机器人搭载的深水罗经直接测量；z_i、z_i-1、z_i-2、z_i-3、z_i-4分别表示自主水下机器人在时刻t_i、时刻t_i-1、时刻t_i-2、时刻t_i-3、时刻t_i-4的深度,由自主水下机器人搭载的深度传感器直接测量。

步骤4)包括：

计算信标在时刻t_k的方差估计P_k和位置估计计算方法为：

其中，P_k表示信标在时刻t_k的方差估计，表示信标在时刻t_k的位置估计；P_k-1表示信标在时刻t_k-1的方差估计，/>表示信标在时刻t_k-1的位置估计；K_k表示信标在时刻t_k的估计增益，Y_k表示由不同时刻信标测距组成的列向量，R_k表示在信标标定过程中，自主水下机器人因本体航行运动而引入的t_k时刻量测噪声，/>是中间变量，P_k-1表示前一时刻t_k-1的方差估计，u(t_k)表示自主水下机器人在t_k时刻的航行速度，由自主水下机器人搭载的多普勒计程仪直接测量,ψ(t_k)和θ(t_k)分别表示自主水下机器人在时刻t_k的航向角、纵倾角,由自主水下机器人搭载的深水罗经直接测量，/>是深水罗经和多普勒计程仪的矩阵，dialog(.)表示对角线矩阵算子，σ_u是多普勒计程仪的测速精度，σ_ψ是深水罗经的航向精度，/>表示自主水下机器人搭载的测距仪的测量方差，R_i、R_i-1、R_i-2、R_i-3、R_i-4分别表示信标与自主水下机器人的测距，它们由自主水下机器人搭载的测距仪提供，I表示单位矩阵，它是常数，k表示时刻t_k的时间索引。

当k＝0时即当前时刻是初始时刻，P_k和分别等于初始时刻的位置估计/>和方差估计P₀。

所述结束条件为：

其中||·||₂表示2范数算子，表示从时刻t_k-20到时刻t_k的相邻位置估计2范数的期望，α_c是信标标定精度阈值，k表示时刻t_k的时间索引，for是表示期望计算范围的关键字。

步骤6)包括：

根据t_k时刻测距仪提供的自主水下机器人与信标的距离、信标标定位置结合自主水下机器人本身航位推算位置,对自主水下机器人进行导航。

本发明具有以下有益效果及优点：

1.相对于传统组合导航方法，本方法使用自主水下机器人冰下自主标定信标，克服传统组合导航方法只能依托船基标定而使用环境受限的缺点；

2.针对传统信标标定方法的计算空间和时间成本高的问题，本方法将机动航行策略和移动矢量径模型相结合，提高了自主水下机器人组合导航方法在冰架下内应用的实时性和可靠性。

3.应用范围广，本发明不但可以应用于冰架下环境下的自主水下机器人组合导航，还可以用于极地冰下滑翔机、极地有缆遥控水下机器人的组合导航。

附图说明

图1是本发明的组成示意图；

图2是本发明的流程图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

整个***组成包括自主水下机器人、多普勒计程仪、深水罗经、深度计、测距仪、信标。其中自主水下机器人是搭载多普勒计程仪、深水罗经、深度计、测距仪的本体，也是运载信标的运载具。多普勒计程仪的作用是测量自主水下机器人的航行速度；深水罗经的作用是测量自主水下机器人的姿态角；深水计的作用是测量自主水下机器人的深度；测距仪的作用是测量自主水下机器人到信标的距离；信标初始由自主水下机器人携带，到达预定区域后，它从自主水下机器人上分离，被布放在冰架下的海底，它的作用是向自主水下机器人提供声学定位应答信号，***组成如图1所示。

整个***按照如下流程工作：

为了下文描述方便，这里对部分符号变量给出定义：定义当自主水下机器人分别在时刻t_i、时刻t_i-1、时刻t_i-2、时刻t_i-3接收到自身与信标的测距时，各时刻对应的测距分别用R_i、R_i-1、R_i-2、R_i-3表示，其中i表示测距的时间序号；定义X_i表示自主水下机器人在时刻t_i的三维空间位置，X_i＝(x_i,y_i,z_i)^T，其中x_i,，y_i,，z_i分别表示北向位置，东向位置，深度；定义ρ_i-1,i表示点X_i-1到点X_i的位置矢量。

如图2所示为本发明的流程图。

第一步，按规划轨迹对信标进行机动测距

自主水下机器人自主布放信标后，自主水下机器人机动航行并对信标进行测距。对机动航行轨迹要求如下,自主水下机器人将信标布放到预定位置后，以信标为中心进行机动航行同时对信标测距，要求机动航行轨迹满足机动可观测条件，机动可观测的判别依据是满足如下判别公式：

其中定义ρ_i,i+1表示点X_i到点X_i+1的位置矢量；ρ_i+1,i+2表示点X_i+1到点X_i+2的位置矢量；ρ_i+2,i+3表示点X_i+2到点X_i+3的位置矢量,ρ_c是线性化判定常数，ρ_c是工程上可以取经验值0.9。该判别式的物理含义是当相邻位置矢量满足判别式时，那么相邻位置矢量线性无关。所以在实际工程应用过程中，建议机动航行采用多边形轨迹(边数不少于5条)、圆形、椭圆、***数字8字形等规划轨迹，轨迹规划点到信标初始布放位置的三维距离在100米到600米之间。

第二步，计算信标的初始位置和方差估计

定义表示信标三维位置的初始位置估计，它是待求解的未知量，它通过求解如下方程组获得：

其中定义第1次收到测距信号的时刻为t₁，自主水下机器人在时刻t₁的位置为X₁＝(x₁,y₁,z₁)^T,同理定义第2、3、4、5次数测距时刻的自主水下机器人位置为X₂、X₃、X₄、X₅，它们均由自主水下机器人搭载的多普勒计程仪、深水罗经、深度计直接测量获得，是已知输入量。R₁、R₂、R₃、R₄、R₅分别表示自主水下机器人在时刻t₁、t₂、t₃、t₄、t₅与信标的测距，它们由测距仪直接测量获得。

H₀和Y是中间变量。

定义P₀表示信标三维位置的初始位置估计的方差估计，它的计算方法如下：

其中X_L表示自主水下机器人布放信标时自身的三维位置，E(.)表示数学期望的计算符号。

第三步，计算自主水下机器人在时刻t_k的量测雅各比矩阵H_k,计算方法如下：

H_k＝[ρ_i-4,i-3 ρ_i-3,i-2 ρ_i-2,i-1 ρ_i-1,i]^T (4)

其中ρ_i-4,i-3表示点X_i-4到点X_i-3的移动矢量径；ρ_i-2,i-1表示点X_i-2到点X_i-1的移动矢量径；ρ_i-3,i-2表示点X_i-3到点X_i-2的移动矢量径；ρ_i-1,i表示点X_i-1到点X_i的位移动矢量径，它们是中间变量，中间变量ρ_i-4,i-3、ρ_i-2,i-1、ρ_i-3,i-2、ρ_i-1,i的计算方法如下：

其中u(t)表示自主水下机器人在时刻t的航行速度，由自主水下机器人搭载的多普勒计程仪直接测量,是已知输入量；ψ(t)和θ(t)分别表示自主水下机器人在时刻t的航向角、纵倾角,由自主水下机器人搭载的深水罗经直接测量，是已知输入量；z_i、z_i-1、z_i-2、z_i-3分别表示自主水下机器人在时刻t_i、时刻t_i-1、时刻t_i-2、时刻t_i-3的深度,由自主水下机器人搭载的深度传感器直接测量，是已知输入量。

第四步，计算信标在时刻t_k的位置估计和方差估计

定义和P_k分别表示信标在时刻t_k的位置估计和方差估计，它们的计算方法如下：

其中P_k表示信标在时刻t_k的位置估计，表示信标在时刻t_k的方差估计，它们是方程的待求解变量，整个方程通过代入法直接求解即可。特殊情况下，即当k＝0时(表示当前时刻是初始时刻)，那么P_k和/>分别等于初始时刻的位置估计/>和方差估计P₀，不需要求解该方程组，直接取值即可。K_k表示信标在时刻t_k的估计增益，它是中间变量；Y_k表示由不同时刻信标测距组成的列向量，它是中间变量；R_k表示在信标标定过程中，自主水下机器人因本体航行运动而引入的t_k时刻量测噪声，它是中间变量；/>是为描述R_k方便而引入的中间变量，它是由已知输入量构成的矩阵；P_k-1表示前一时刻t_k-1的方差估计，对于t_k时刻的方程组，它是已知量；u(t_k)表示自主水下机器人在t_k时刻的航行速度，由自主水下机器人搭载的多普勒计程仪直接测量,是已知输入量；ψ(t_k)和θ(t_k)分别表示自主水下机器人在时刻的航向角、纵倾角,由自主水下机器人搭载的深水罗经直接测量，是已知输入量；是深水罗经和多普勒计程仪相关矩阵，dialog(.)表示对角线矩阵算子，σ_u是多普勒计程仪的测速精度，σ_ψ是深水罗经的航向精度，σ_u和σ_ψ在设备出厂时由厂家直接提供，是已知参数；/>表示自主水下机器人搭载的测距仪的测量方差，该参数在设备出厂时由厂家直接提供，是已知参数。R_i、R_i-1、R_i-2、R_i-3表示信标与自主水下机器人的测距，它们由自主水下机器人搭载的测距仪提供，是已知输入量。I表示3*3的单位矩阵，它是常数。

第五步，重复步骤三到步骤四，直到t_k时刻计算的信标位置估计满足如下判别式则停止信标标定过程。

其中||·||₂表示2范数算子，表示从时刻t_k-20到时刻t_k的相邻位置估计2范数的期望；α_c是信标标定精度阈值，用户可以自行设置，建议取值为10，k表示时刻t_k的时间索引，for是表示期望计算范围的关键字。

第六步，根据t_k时刻测距仪提供的自主水下机器人与信标的距离、信标标定位置结合自主水下机器人本身航位推算位置，按照标准卡尔曼滤波器构建单信标组合导航滤波器，实时计算自主水下机器人水下位置的最优估计。因为该步骤算法采用标准卡尔曼滤波器，所以该步骤不作为权利诉求。/>

Claims (9)

1.适用于极地冰架下环境的自主水下机器人的组合导航方法，其特征在于，包括以下步骤：

1)自主水下机器人按照规划轨迹的约束条件对信标进行机动测距；

2)计算信标的初始位置和方差估计；

3)计算自主水下机器人在当前时刻的量测雅各比矩阵；

4)根据雅各比矩阵计算信标在当前时刻的位置估计和方差估计；

5)根据当前时刻信标的位置估计和方差估计，判断是否满足结束条件，若满足，记录当前时刻信标位置；否则，返回步骤3)；

6)水下机器人根据记录的信标位置作为参考点进行导航。

2.根据权利要求1所述的适用于极地冰架下环境的自主水下机器人的组合导航方法，其特征在于，所述规划轨迹的约束条件为：

其中，ρ_i,i+1表示点X_i到点X_i+1的位置矢量，ρ_i+1,i+2表示点X_i+1到点X_i+2的位置矢量，ρ_c是线性化判定常数，fabs为取绝对值。

3.根据权利要求1所述的适用于极地冰架下环境的自主水下机器人的组合导航方法，其特征在于，步骤2)包括：

求解信标的初始位置估计具体为：

其中第1次收到测距信号的时刻为t₁，自主水下机器人在时刻t₁的位置为X₁＝(x₁,y₁,z₁)^T,同理定义第2、3、4、5次数测距时刻的自主水下机器人位置为X₂、X₃、X₄、X₅，它们均由自主水下机器人搭载的多普勒计程仪、深水罗经、深度计直接测量获得，R₁、R₂、R₃、R₄、R₅分别表示自主水下机器人在时刻t₁、t₂、t₃、t₄、t₅与信标的测距，它们由测距仪直接测量获得，H₀和Y是中间变量；

P₀表示信标的初始位置估计的方差估计，计算方法为：

其中，X_L表示自主水下机器人布放信标时自身的三维位置，E(.)表示数学期望的计算符号。

4.根据权利要求1所述的适用于极地冰架下环境的自主水下机器人的组合导航方法，其特征在于，所述量测雅各比矩阵为：

H_k＝[ρ_i-4,i-3 ρ_i-3,i-2 ρ_i-2,i-1 ρ_i-1,i]^T

其中，H_k为自主水下机器人在时刻t_k的量测雅各比矩阵，ρ_i-4,i-3表示点X_i-4到点X_i-3的移动矢量径；ρ_i-3,i-2表示点X_i-3到点X_i-2的移动矢量径；ρ_i-2,i-1表示点X_i-2到点X_i-1的移动矢量径；ρ_i-1,i表示点X_i-1到点X_i的位移动矢量径。

5.根据权利要求4所述的适用于极地冰架下环境的自主水下机器人的组合导航方法，其特征在于，ρ_i-4,i-3、ρ_i-2,i-1、ρ_i-3,i-2、ρ_i-1,i的计算方法为：

其中，u(t)表示自主水下机器人在时刻t的航行速度，由自主水下机器人搭载的多普勒计程仪直接测量；ψ(t)和θ(t)分别表示自主水下机器人在时刻t的航向角、纵倾角,由自主水下机器人搭载的深水罗经直接测量；z_i、z_i-1、z_i-2、z_i-3、z_i-4分别表示自主水下机器人在时刻t_i、时刻t_i-1、时刻t_i-2、时刻t_i-3、时刻t_i-4的深度,由自主水下机器人搭载的深度传感器直接测量。

6.根据权利要求5所述的适用于极地冰架下环境的自主水下机器人的组合导航方法，其特征在于，步骤4)包括：

计算信标在时刻t_k的方差估计P_k和位置估计计算方法为：

其中，P_k表示信标在时刻t_k的方差估计，表示信标在时刻t_k的位置估计；P_k-1表示信标在时刻t_k-1的方差估计，/>表示信标在时刻t_k-1的位置估计；K_k表示信标在时刻t_k的估计增益，Y_k表示由不同时刻信标测距组成的列向量，R_k表示在信标标定过程中，自主水下机器人因本体航行运动而引入的t_k时刻量测噪声，/>是中间变量，P_k-1表示前一时刻t_k-1的方差估计，u(t_k)表示自主水下机器人在t_k时刻的航行速度，由自主水下机器人搭载的多普勒计程仪直接测量,ψ(t_k)和θ(t_k)分别表示自主水下机器人在时刻t_k的航向角、纵倾角,由自主水下机器人搭载的深水罗经直接测量，/>是深水罗经和多普勒计程仪的矩阵，dialog(.)表示对角线矩阵算子，σ_u是多普勒计程仪的测速精度，σ_ψ是深水罗经的航向精度，/>表示自主水下机器人搭载的测距仪的测量方差，R_i、R_i-1、R_i-2、R_i-3、R_i-4分别表示信标与自主水下机器人的测距，它们由自主水下机器人搭载的测距仪提供，I表示单位矩阵，它是常数，k表示时刻t_k的时间索引。

7.根据权利要求6所述的适用于极地冰架下环境的自主水下机器人的组合导航方法，其特征在于，当k＝0时即当前时刻是初始时刻，P_k和分别等于初始时刻的位置估计/>和方差估计P₀。

8.根据权利要求7所述的适用于极地冰架下环境的自主水下机器人的组合导航方法，其特征在于，所述结束条件为：

其中||·||₂表示2范数算子，表示从时刻t_k-20到时刻t_k的相邻位置估计2范数的期望，α_c是信标标定精度阈值，k表示时刻t_k的时间索引，for是表示期望计算范围的关键字。

9.根据权利要求1所述的适用于极地冰架下环境的自主水下机器人的组合导航方法，其特征在于，步骤6)包括：

根据t_k时刻测距仪提供的自主水下机器人与信标的距离、信标标定位置结合自主水下机器人本身航位推算位置,对自主水下机器人进行导航。