[发明专利]一种基于改进的S-H自适应联邦滤波的智能水下机器人多源组合导航方法

权利要求书

1.一种基于改进的S-H自适应联邦滤波的智能水下机器人多源组合导航方法，其特征在于，所述改进的S-H自适应联邦滤波方法包括以下步骤：

S100、进行水下多源组合导航系统建模，得到导航传感器及其误差模型；

S200、以误差模型为基础，提出改进S-H自适应联邦滤波方法。

2.根据权利要求1所述的一种基于改进的S-H自适应联邦滤波的智能水下机器人多源组合导航方法，其特征在于，在S100中，具体包括以下步骤：

S110、首先建立导航系统常用坐标系和坐标转换关系；

S120、在S110的基础上获得SINS、USBL、DVL和MCP四种传感器的误差模型。

3.根据权利要求2所述的一种基于改进的S-H自适应联邦滤波的智能水下机器人多源组合导航方法，其特征在于，在S110中，具体包括以下步骤：

S111、假定SINS、USBL、DVL和MCP四种传感器均安装于水下机器人载体中心，则所述四种传感器所获得的数据都是基于载体坐标系下的，需要将其统一到导航坐标系下；

S112、导航坐标系围绕OZ_n轴旋转ψ，围绕OX_n1轴旋转θ，然后绕OY_n2旋转γ，即：

其中，ψ为航向角，以地理北向为起点沿顺时针方向运动为正，范围为0°至360°；θ为俯仰角，以水平面为基准，向上为正，向下为负，范围为-90°至90°；γ为横滚角，以铅垂平面为基准，右倾为正，左倾为负，范围为-90°至90°；

转换矩阵分别对应于：

则导航坐标系与水下机器人载体坐标系之间的转换矩阵为：

4.根据权利要求3所述的一种基于改进的S-H自适应联邦滤波的智能水下机器人多源组合导航方法，其特征在于，在S120中，具体的：

所述SINS的误差包括速度、位置和姿态误差，其中，

姿态误差方程为：

式中，表示东、北、天三向平台失准角，为地球自转角速度，表示水下机器人载体位置变化导致的角速度，

上式中，V＝[V_E V_N V_U]^T分别表示水下机器人载体在地理坐标系中的东、北、天三向速度，L,λ,h分别表示经度、纬度和高度，R_M、R_N分别表示地球参考椭球子午圈、椭球卯酉圈上各点的曲率半径，

将姿态误差方程展开得：

速度误差方程：

上式中，fⁿ表示比力，

将其展开如下：

位置误差方程:

所述USBL的误差为：

建立基阵坐标系oxyz，声学信标固定在水下机器人载体上，其位置设为(X_a,Y_a,Z_a)，R为位置矢量，三个水听器分别放置在原点、x轴和y轴上，声学信标和原点之间连线与坐标轴夹角分别是θ_mx、θ_my和θ_m，

信标与基阵之间的距离R为：

而

X_a＝Rcosθ_mx (15)

Y_a＝Rcosθ_my (16)

由上两式得：

将式(12)代入式(13)、式(14)得：

再将式(15)代入式(16)得：

解得：

同理得：

θ_mx、θ_my通过相位差获得，

设两个基阵之间的距离为d，声波波长为λ，各个基阵间接收到的声波信号相位差为φ，信号到基阵的入射角为θ_m：

通过其得到的相位差，由上式得到：

c为声波在水下的传播速度，为已知量，T为调制解调器信号发射、接收的时间差，则R为：

R＝0.5cT (26)

最后得到：

下面对超短基线定位系统误差进行分析，由上面得到了X_a、Y_a、Z_a的表达式，以X_a为例，Y_a、Z_a同理得，对X_a进行全微分得：

位置的相对定位精度为：

X轴方向误差相对斜距R的方差为：

同理：

其中ΔT、Δc、Δφ₁₂、Δφ₁₃分别为时间测量误差、声速测量误差以及相位测量误差；

DVL的误差方程：

DVL向海底前后左右各发射一束频率为f₀，波速为c₀的声波，假设AUV以速度V向前运动，

上式中，f_d1,f_d2,f_d3,f_d4分别为四个方向上的声波频移，V_x,V_y,V_z表示AUV的三轴速度分量，

DVL误差包括速度偏移误差δV_EDVL和δV_NDVL、刻度系数误差δC和偏流角误差δΔ组成，用一阶马尔可夫过程表示，则误差方程为：

其中β^-1_EDVL、β^-1_NDVL、β^-1_Δ分别表示相关时间，w_Ed、w_Nd、w_Δ为激励白噪声；

MCP的误差方程：

经过补偿后，MCP的误差近似看作一阶马尔可夫过程：

上式中，δψ_MCP表示MCP航向角误差，表示相关时间，w_MCP表示相应的驱动白噪声。