[发明专利]适用于极地冰架下环境的自主水下机器人的组合导航方法

权利要求书

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的位移动矢量径。