[发明专利]自主式水下机器人组合导航系统及方法

权利要求书

1.一种自主式水下机器人组合导航系统，其特征在于，由捷联惯性导航系统（1）、全球定位导航系统（2）、多普勒测速仪（3）、磁航向仪（4）组成，所述捷联惯性导航系统（1）利用陀螺仪和加速度计计算出相对地球沿地理坐标系的速度信息、位置信息和姿态信息；所述全球定位导航系统（2）获取自主式水下机器人的初始绝对位置信息和速度信息；所述多普勒测速仪(3)计算出速度信息；所述磁航向仪(4)计算出航向信息；所述全球定位导航系统（2）对捷联惯性导航系统（1）定时重调，系统对捷联惯导系统(1)输出的速度信息、位置信息和姿态信息，多普勒测速仪(3)所计算的速度信息，磁航向仪(4)所计算的航向信息进行数据融合，得到组合导航信息。

2.如权利要求1所述的自主式水下机器人组合导航系统的导航数据融合方法，其特征在于，该方法步骤如下：

1）对于水下组合导航系统，线性化后离线系统状态方程和观测方程如下：

X(k)=Φ(k/k-1)X(k-1)+Γ(k-1)W(k-1)

Z(k)=H(k)X(k)+V(k)

式中：X(k)为系统状态向量；Z(k)为量测向量；Φ(k/k-1)为一步转移阵；Γ(k-1)为系统噪声驱动阵；W(k)为系统激励噪声序列；H(k)为量测阵；V(k)为量测噪声序列，W(k)、V(k)是不相关的高斯白噪声序列，其均值、方差分别为E[ω(k)]=0，E[ω(k)ω^T(j)]=Q(k)δ_kj；E[v(k)]=0，E[v(k)v^T(j)]=R(k)δ_kj;cov[ω(k)v(j)]=0

2）模糊自适应卡尔曼滤波算法编排：

设计矩阵Q(k)=Q^T(k)>0，R(k)=R^T(k)>0，Q(k)为系统噪声方差阵，R(k)为量测噪声方差阵，其中时间更新：

（1）状态变量估计：

X(k/k-1)＝Φ(k/k-1)X(k-1)

（2）误差协方差估计：

P(k/k-1)=Φ(k/k-1)P（k-1)Φ^T(k/k-1)+Γ(k-1)Q(k-1)Γ^T(k-1)

测量更新：

（3）定义残差的理论方差阵为

P_r=H(k)(Φ(k/k-1)P（k-1)Φ^T(k/k-1)+Q)H(k)^T+R（k-1），

以及残差的实测方差阵

i₀=k-N+1,式子C_r为对最新的N个残差向量求平均值，r为残差序列定义为：r_k=Z(k)-H(k)X（k/k-1），

进而求得实测方差阵和理论方差阵的比值其中T_r(.)表示对矩阵求迹；

（4）计算加权于量测噪声阵系数：

α^b(k)=FIS(q(k))

（5）量测噪声阵更新：

R(k)=α^b(k)R(k-1)

（6）计算卡尔曼增益阵：

K(k)=P(k/k-1)H^T(k)[H(k)P(k/k-1)H^T(k)+R(k)]^-1

（7）由测量变量z(k)更新得到状态变量估计

X(k)=X(k/k-1)+K(k)[Z(k)-H(k)X(k/k-1)]

（8）误差协方差更新：

P(k)=(I-K(k)H(k))P(k/k-1)

（9）循环：k=k+1，转到步骤（1）

3）模糊推理系统设计

α(k)的取值由模糊推理系统得到，模糊推理系统采用单输入单输出模式，此时模糊推理系统的输入为每一步的残差实测方差与理论方差比值q(k)，由上述模糊滤波算法编排中可知其中T_r(.)表示对矩阵求迹；残差实测方差Cr=1NΣi=i0kririT,]]>i₀=k-N+1，

残差的理论方差P_r=H(k)(Φ(k/k-1)P（k-1)Φ^T(k/k-1)+Q)H(k)^T+R（k-1）；由模糊推理系统的输入，得到系统输出α(k)，对输入、输出变量模糊化，两者的隶属度函数分别为两个“trimf”和一个“trapmf”，即两个三角形隶属度函数和一个梯形隶属度函数；采用一个梯形隶属度函数的目的是为了避免q(k)在近似等于1的情况下，模糊推理系统仍然调α(k)而引起不必要的滤波误差；

定义模糊推理系统模糊规则如下：

if q(k)less0.9，thenα(k)less1

if q(k)more0.9and q(k)less1.1，thenα(k)equal1

if q(k)more1.1，thenα(k)more1

模糊输出的求解，采用模糊推理系统直接输出；这样每滤波一次，模糊推理系统输出一个α(k)，对量测噪声方差阵在线调整一次。