CN102829777A - 自主式水下机器人组合导航系统及方法 - Google Patents

Abstract

本发明公开了一种自主式水下机器人组合导航系统，由捷联惯性导航系统、全球定位导航系统、多普勒测速仪、磁航向仪组成，所述捷联惯性导航系统计算出相对地球沿地理坐标系的速度信息、位置信息和姿态信息；所述全球定位导航系统获取自主式水下机器人的初始绝对位置信息和速度信息；所述多普勒测速仪计算出速度信息；所述磁航向仪计算出航向信息；所述全球定位导航系统对捷联惯性导航系统定时重调，系统对捷联惯导系统输出的速度信息、位置信息和姿态信息，多普勒测速仪所计算的速度信息，磁航向仪所计算的航向信息用自适应模糊卡尔曼滤波进行数据融合，得到精确组合导航信息。

Description

自主式水下机器人组合导航系统及方法

技术领域

本发明涉及一种导航技术，特别是一种自主式水下机器人组合导航系统及方法，属于海洋工程领域。

背景技术

伴随着现代科学技术的发展，自主式水下机器人（Autonomous UnderwaterVehicle，AUV）无论在军用还是民用方面都有广泛的应用，例如战区侦查、探测扫除水雷、海上预警、水下搜索和测绘、近海石油工程作业等。就当前的发展而言，导航技术是实现AUV自主航行的关键，导航系统必须提供远距离及长时间范围内的精确定位、速度及姿态信息。但由于受其大小、重量、电源使用的限制及水介质的特殊性、隐蔽性等因素的影响，实现AUV的精确导航是一项艰难的任务。

就AUV而言，可供应用的主要有两类导航方式：基于外部信号的非自主导航和基于传感器的自主导航。非自主导航方式，如罗兰、欧米加、GPS等，仅在接收机能接收到信号时能够完成导航，且由于电波在水中快速衰减的原因，这些基于无线电的导航方式，在AUV上的使用受到很大限制。基于传感器的自主导航方式靠AUV自身携带的装备如惯性测量装置（IMU）、声换能器、地形匹配或地磁传感等手段完成导航。

目前，国际上水下导航已由单一导航方式向高精度、高可靠、综合化、智能化的组合导航系统发展；信息处理方法也由单一数据源的处理，向多导航传感器多数据源的信息融合发展，总的来说，组合导航代表了未来水下导航的发展方向，克服了传统导航的缺陷和不足，使水下导航领域呈现出崭新的面貌，具有无比广阔的发展前景。

现有的水下机器人通常利用惯性导航系统（Inertial Navigation System，INS）采集的姿态及加速度信息或磁罗经（Magnctic Compass，MC）采集姿态信息，以及多普勒计程仪（Doppler Velocity Log，DVL）采集的速度信息进行组合导航。但是由于上述姿态和速度数据存在一定误差，且这些误差会随时间积累到不可接受的程度。而研究结果表明基于定时接收GPS修正的组合导航系统可提供比常规惯性导航技术高一到两个数量级的测量精度，并且能够把横滚角误差和俯仰角误差抑制到一定的精度，但是很难抑制组合导航系统航向角误差的积累。磁航向仪（MCP）可以输出地磁场矢量在载体坐标系的投影，且精度不发散，故利用磁航向仪的信息与捷联惯性导航系统（SINS）、GPS、多普勒计程仪进行组合，可以抑制航向角误差的发散，可显著提高AUV整体导航的精度。

发明内容

本发明的目的在于提供一种自主式水下机器人组合导航系统及方法，针对现有技术存在的缺陷，将磁航向仪的信息与捷联惯性导航系统（SINS）、GPS、多普勒计程仪进行组合，抑制航向角误差的发散，显著提高AUV导航系统整体的精度。

本发明的目的通过以下技术方案予以实现：

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

一种自主式水下机器人组合导航系统的导航数据融合方法，步骤如下：

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(.)表示对矩阵求迹；残差实测方差 $C_r=\frac{1}{N}\underset{i=i_0}{\overset{k}{\mathrm{\Σ}}}{r}_i{r}_i^T,i_0=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)，对量测噪声方差阵在线调整一次。

与现有技术相比，本发明的有益效果是：利用GPS定时重调SINS，然后应用卡尔曼滤波器整合SINS估计测出的速度、姿态信息以及DVL、MCP测出的速度和航向信息,得到高精度的位置、速度和姿态航向信息，提高组合导航的系统精度。

附图说明

图1为组合导航系统结构图；

图2为SINS/DVL/磁航向仪组合导航滤波模型结构图；

图3为SINS/DVL/磁航向仪/GPS组合导航滤波模型结构图；

图4为基于模糊自适应卡尔曼滤波器控制解算流程图；

图5为FIS输出与输入的隶属度函数曲线图；

图6为模糊逻辑推理器原理框图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

如图1所示，自主式水下机器人组合导航系统，由捷联惯性导航系统1、全球定位导航系统2、多普勒测速仪3、磁航向仪4组成，所述捷联惯性导航系统1利用陀螺仪和加速度计计算出相对地球沿地理坐标系的速度信息、位置信息和姿态信息；所述全球定位导航系统2获取自主式水下机器人的初始绝对位置信息和速度信息；所述多普勒测速仪3计算出速度信息；所述磁航向仪4计算出航向信息；所述全球定位导航系统2对捷联惯性导航系统1定时重调。对于GPS信号需在AUV上浮状态下才可进行信息交换，故此系统导航方案分水下和水上部分。水下部分采用SINS/DVL/MCP组合导航的形式进行；水上部分采用GPS重调惯导方式，实际上就是在GPS工作期间，惯导显示的是GPS的位置和速度，在GPS停止期间，惯导在原显示的基础上变化，即GPS停止工作瞬间的位置和速度作为惯导系统的初值，然后再根据重调得到的SINS信息与DVL、MCP的进行信息融合。通过下列步骤实现：

1）捷联惯性导航系统（SINS）系统误差模型的建立包括姿态误差、速度误差的模型；多普勒测速仪（DVL）误差模型的建立即速度误差的模型；磁航向仪（MCP）误差模型的建立即磁航向误差模型。列出各自的状态方程。

2）SINS系统的输出的速度信息与DVL输出的速度信息相减得量测量，SINS输出的航向信息与MCP输出的航向信息相减得量测量

3）由步骤1）、2），设计出集中滤波器的状态方程、量测方程，通过自适应模糊卡尔曼滤波器估计误差量，一方面对主系统SINS进行反馈校正，一方面输出得到组合修正的导航信息。

4)通过定时上浮利用GPS重调SINS，运用重调后的SINS输出信息再次进行步骤1)、2)，设计出集中滤波器的状态方程、量测方程，通过自适应模糊卡尔曼滤波器估计误差量，一方面对主系统进行更精确的反馈校正，一方面输出得到更为精确的组合导航信息。

系统对捷联惯导系统l输出的速度信息、位置信息和姿态信息，多普勒测速仪3所计算的速度信息，磁航向仪4所计算的航向信息进行数据融合，得到组合导航信息。如图2所示，利用数据融合方法对各个导航信息进行融合，将捷联惯导分别与其余子系统形成的所有量测量作为滤波器的量测量，利用捷联解算得到载体的导航信息；多普勒测速仪进行速度解算，得到导航信息；磁航向仪计算测出的航向信息，对捷联惯性导航系统误差和多普勒测速仪、磁航向仪误差进行建模，建立系统方程，通过自适应模糊卡尔曼滤波器估计误差量，对系统进行反馈校正，得到组合修正的导航信息。具体实现步骤如下：

1)捷联惯性导航系统SINS系统误差模型的建立：

根据捷联惯性导航系统长期工作时的误差特点，选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量

$X_{\mathrm{SINS}}=[{\mathrm{\δV}}_E\mathrm{\δV}.{\mathrm{\φ}}_E{\mathrm{\φ}}_N{\mathrm{\φ}}_U.\mathrm{\δL\δ\λ}.{\▿}_{\mathrm{bx}}{\▿}_{\mathrm{by}}.{\mathrm{\ϵ}}_{\mathrm{bx}}{\mathrm{\ϵ}}_{\mathrm{by}}{\mathrm{\ϵ}}_{\mathrm{bz}}]$

式中，δV_E、δV_N分别是东向、北向速度误差；φ_E、φ_N、φ_U分别是东向、北向、天向失准角；δL、δλ分别是纬度、经度误差；

分别是x、y向的加速度计偏置；ε_bx、ε_by、ε_bz分别是x、y、z向的陀螺漂移。选取东北天向坐标系作为导航坐标系，载体坐标系x轴沿水下潜器横轴指向右舷，y轴沿航行器纵轴指向前，z轴垂直于x与y轴所确定的平面构成右手坐标系。状态方程为 $X_{\mathrm{SINS}}^{^}=F_{\mathrm{SINS}}{X}_{\mathrm{SINS}}+W_{\mathrm{SINS}}$

系统噪声矩阵 $W_{\mathrm{SINS}}=[{\mathrm{\ω}}_{V_E}{\mathrm{\ω}}_{V_N}{\mathrm{\ω}}_{{\mathrm{\φ}}_E}.{\mathrm{\ω}}_{{\mathrm{\φ}}_N}{\mathrm{\ω}}_{{\mathrm{\φ}}_U}.{\mathrm{\ω}}_{{\mathrm{\ϵ}}_L}{\mathrm{\ω}}_{{\mathrm{\ϵ}}_{\mathrm{\λ}}},00000]$

$F_{\mathrm{SINS}}=\left[0\begin{array}{cccccccccccc}\frac{V_N}{R}\mathrm{tgL}& F_{12}& 0& -f_U& f_N& F_{16}& 0& C_{11}& C_{21}& 0& 0& 0\\ F_{21}& 0& f_U& 0& -f_E& F_{26}& 0& C_{12}& C_{22}& 0& 0& 0\\ 0& -\frac{1}{R}& 0& F_{34}& F_{35}& 0& 0& 0& 0& -C_{11}& -C_{21}& -C_{31}\\ \frac{1}{R}& 0& F_{43}& 0& -\frac{V_N}{R}& F_{46}& 0& 0& 0& -C_{12}& -C_{22}& -C_{32}\\ \frac{\mathrm{tgL}}{R}& 0& F_{53}& \frac{V_N}{R}& 0& F_{56}& 0& 0& 0& -C_{13}& -C_{23}& -C_{33}\\ 0& \frac{1}{R}& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ \frac{\sec L}{R}& 0& 0& 0& 0& F_{76}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$ 其中：

$F_{12}=2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan {\mathrm{LF}}_{16}=(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}{\sec }^{2}L)V_N$

$F_{21}=-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)F_{26}=-(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}{\sec }^{2}L)V_E$

$F_{34}=-F_{43}={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L{F}_{35}=-F_{53}=-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R})$

$F_{46}=-{\mathrm{\ω}}_{\mathrm{ie}}\sin L{F}_{56}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}{\sec }^{2}L$

$F_{76}=\frac{V_E}{R}{\sec }^{2}L\sin L{C}_{\mathrm{ij}}$ 为姿态转移矩阵

的元素

2)多普勒测速仪DVL误差模型的建立：

多普勒测速仪的优点是能够完全自主导航、反应速度快、隐蔽性好、测得的平均速度精度高，并且可以测定浅水和低速时的航速，这对于远程水下潜器是非常重要的。在实际应用中多普勒测速仪需要外部的航向信息和垂直基准信息，所以利用多普勒测速仪与捷联惯导系统进行组合，可以构成精度高，可靠性好的高自主性水下组合导航系统。由于测速仪的天向测速精度相对较低，选取多普勒测速仪的水平方向测速误差和刻度系数误差作为状态变量

X_DVL=[δV_DxδV_Dy.δK_DxδK_Dy]

其中多普勒测速误差δV_Dx、δV_Dy可用一阶马尔可夫过程近似描述，刻度系数误差δK_Dx、δK_Dy为随机常数。

状态方程X＾_DVL=F_DVLX_DVL+W_DVL

其中系统噪声矩阵W_DVL=[W_DxW_Dy.00]，τ_dx、τ_dy、W_DxW_Dy分别是一阶马尔可夫过程的相关时间和驱动白噪声。

3）磁航向仪MCP误差模型的建立：

磁航向仪是利用磁敏感元件感测地磁场的方向，即磁子午线方向，并给出航向器纵轴相对磁子午线在水平面内的夹角。磁航向仪很容易受外界的影响，精度也不易提高但由于结构简单，仪表本身的可靠性高、成本低，所以仍被很多使用场合采用。磁航向仪只取一个状态：

X_MCP＝δΨ_MCP

状态方程可用一阶马尔可夫过程描述之：

${X^{^}}_{\mathrm{MCP}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{MCO}}}{X}_{\mathrm{MCP}}+W_{\mathrm{MCP}}$

其中δΨ_MCP为MCP的测量误差，τ_MCP为相关时间参数，W_MCP为MCP系统噪声。

4）由步骤1)、2)、3)中各子系统的状态方程，可得集中滤波器的状态方程：

取状态向量为：X＝[X_SINS，X_DVL，X_MCP]

则状态方程为X＾_总=F_总X_总+W_总

式中：

X_总=[X_SINS，X_DVL，X_MCP]^T

W_总=[W_SINS，W_DVL，W_MCP]^T

5)量测方程的建立：

a.捷联惯导SINS与多普勒测速仪DVL的量测量：

多普勒测速仪分别测出两个速度V_x和V_y。而在本发明的应用中，需要测量航行器东向及北向的移动速度，多普勒测速仪直接测出的是航行器在载体坐标系的速度分量V_x、V_y，故必须先将多普勒测出的速度分量变换到导航坐标系，然后与捷联惯导的相应输出信息相减才能到到量测量

DVL速度输出为

$\left\{\begin{array}{c}{V}_{\mathrm{DE}}=V_E+\mathrm{\δ}{V}_{\mathrm{DE}}=V_E+\cos \mathrm{\ψ}\×\mathrm{\δ}{V}_{\mathrm{dx}}-\sin \mathrm{\ψ}\×\mathrm{\δ}{V}_{\mathrm{dy}}+V_x\cos \mathrm{\ψ}\×\mathrm{\δ}{K}_x-V_y\sin \mathrm{\ψ}\×\mathrm{\δ}{K}_y-(V_x\sin \mathrm{\ψ}+V_y\cos \mathrm{\ψ})\×{\mathrm{\φ}}_U+{\mathrm{\η}}_E\\ V_{\mathrm{DN}}=V_N+\mathrm{\δ}{V}_{\mathrm{DN}}=V_N+\sin \mathrm{\ψ}\×\mathrm{\δ}{V}_{\mathrm{dx}}+\cos \mathrm{\ψ}\×\mathrm{\δ}{V}_{\mathrm{dy}}+V_x\sin \mathrm{\ψ}\×\mathrm{\δ}{K}_x+V_y\cos \mathrm{\ψ}\×\mathrm{\δ}{K}_y+(V_x\sin \mathrm{\ψ}+V_y\cos \mathrm{\ψ})\×{\mathrm{\φ}}_U+{\mathrm{\η}}_N\end{array}\right.$ 其中 $\left[\begin{array}{c}{V}_E\\ V_N\end{array}\right]=\left[\begin{array}{c}{V}_x\cos \mathrm{\ψ}+V_y\sin \mathrm{\ψ}\\ V_s\sin \mathrm{\ψ}+V_y\cos \mathrm{\ψ}\end{array}\right],$ V_E，V_N分别为东向、北向真实速度；η_E、η_N为多普勒测速白噪声，ψ是方位角的真值。

SINS速度输出为：

$\left\{\begin{array}{c}{V}_{\mathrm{IE}}=V_E+{\mathrm{\δV}}_E\\ V_{\mathrm{IN}}=V_N+{\mathrm{\δV}}_N\end{array}\right.$

SINS/DVL形成的速度量测向量为

$Z_{\mathrm{ID}}=\left[\begin{array}{c}{V}_{\mathrm{IE}}-V_{\mathrm{DE}}\\ V_{\mathrm{IN}}-V_{\mathrm{DN}}\end{array}\right]=\left[\begin{array}{c}(V_E+{\mathrm{\δV}}_E)-(V_E+{\mathrm{\δV}}_{\mathrm{DE}})\\ (V_N+{\mathrm{\δV}}_N)-(V_N+{\mathrm{\δV}}_{\mathrm{DN}})\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δV}}_E-{\mathrm{\δV}}_{\mathrm{DE}}\\ {\mathrm{\δV}}_N-{\mathrm{\δV}}_{\mathrm{DN}}\end{array}\right]$

$=\left[\begin{array}{cccccccccc}1& 0& 0& 0& -(V_x\sin \mathrm{\π}+V_y\cos \mathrm{\π})& 0_{1\×7}& \cos \mathrm{\π}& \sin \mathrm{\ψ}& V_x\cos \mathrm{\ψ}& V_y\cos \mathrm{\ψ}\\ 0& 1& 0& 0& V_x\cos \mathrm{\ψ}-V_y\sin \mathrm{\ψ}& 0_{1\×7}& \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& V_x\sin \mathrm{\ψ}& -V_y\sin \mathrm{\ψ}\end{array}\right]\left[\begin{array}{c}{X}_{\mathrm{SINS}}\\ X_{\mathrm{DVL}}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\η}}_E\\ {\mathrm{\η}}_N\end{array}\right]$ 式中：

$H_1=\left[\begin{array}{cccc}{I}_{2\×2}& 0_{2\×2}& \begin{array}{c}-(V_s\sin \mathrm{\ψ}+V_y\cos \mathrm{\ψ})\\ V_x\cos \mathrm{\ψ}-V_y\sin \mathrm{\ψ}\end{array}& \begin{array}{c}{0}_{1\×7}\\ 0_{1\×7}\end{array}\end{array}\right]$ $H_{\mathrm{ID}}=\left[\begin{array}{cccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& V_x\cos \mathrm{\ψ}& V_y\cos \mathrm{\ψ}\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& V_x\sin \mathrm{\ψ}& -V_y\sin \mathrm{\ψ}\end{array}\right]$

$V_{\mathrm{ID}}=\left[\begin{array}{c}{\mathrm{\η}}_E\\ {\mathrm{\η}}_N\end{array}\right]$

b.捷联惯导SINS与磁航向仪MCP的量测量：

磁航向仪MCP航向输出及捷联惯导航向信息分别为：

ψ_MCP＝ψ_t+δψ_MCP，ψ_SIMS＝ψ_t+δψ_SINS，其中ψ_t为真实航向值，δψ_SINS为SINS航向测量误差。

量测量为：

$Z_{\mathrm{IM}}=[{\mathrm{\ψ}}_{\mathrm{SINS}}-{\mathrm{\ψ}}_{\mathrm{MCP}}]=[{\mathrm{\δ\ψ}}_{\mathrm{SINS}}-{\mathrm{\δ\ψ}}_{\mathrm{MCP}}]=\left[\begin{array}{cccc}{0}_{1\×4}& 1& 0& -1\end{array}\right]\left[\begin{array}{c}{X}_{\mathrm{SINS}}\\ X_{\mathrm{MCP}}\end{array}\right]+\left[\begin{array}{c}{W}_{\mathrm{SINS}}\\ W_{\mathrm{MCP}}\end{array}\right]$

式中H₂＝[0_1×4 1 0_1×7]，H_IM＝-1

c.集中滤波器的量测方程为

Z＝HX+V

式中 $Z=\left[\begin{array}{c}{Z}_{\mathrm{ID}}\\ Z_{\mathrm{IM}}\end{array}\right],$ $V=\left[\begin{array}{c}{V}_{\mathrm{ID}}\\ V_{\mathrm{IM}}\end{array}\right],$ $H=\left[\begin{array}{ccc}{H}_1& H_{\mathrm{ID}}& 0\\ H_2& 0& H_{\mathrm{IM}}\end{array}\right]$

如图3所示，然后通过定时上浮接收GPS提供的位置、速度信息，重调主系统sINS，然后再进一步和DVL、MCP信息组合，通过模糊白适应卡尔曼滤波器得到精确的估计值输出，并修正反馈主系统sINS

以下说明介绍本发明所采用的数据融合方法：

考虑到实际应用过程中各子系统量测噪声统计特性可能发生变化，而标准卡尔曼滤波算法则无法对上述变化进行检测和调整，使得估计误差变大，故提出在卡尔曼滤波算法中使用模糊自适应滤波代替标准卡尔曼滤波，形成模糊白适应卡尔曼信息融合算法，模糊白适应卡尔曼滤波算法原理是通过监视理论残差与实际残差的比值是否在1附近，应用模糊推理系统（FIS）不断地调整量测噪声协方差阵的加权系数，同时也自适应地调整了系统和量测的噪声值，使系统模型和实际模型更接近，也使卡尔曼滤波具有了自适应能力并使最终的估计精度更高。具体步骤如下：

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

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

b．模糊自适应卡尔曼滤波算法编排：

设计矩阵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个残差向量求平均值，M由经验根据具体情况选定，主要起平滑作用。

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）

上述控制流程如图4所示。

上式中α(k)为对量测噪声阵R(k)的调整系数，b是一个正值常数，表示对量测噪声矩阵调整系数α(k)的放大程度。若b>1,表示放大α(k)对R(k)的调整作用，这样在量测噪声变化的情况下，R(k)可以在较少的步数内迅速逼近真实的量测噪声值。若b<1表示缩小α(k)对R(k)的调整作用，这样可以使自适应调整的R(k)非常精确地稳定在真实量测噪声值上。但会使R(k)调整到真实量测噪声值的过渡时间稍长。若b=0，意味着放弃α(k)对R(k)的调整作用。此时上述滤波方程等同于常规卡尔曼滤波。同时由滤波算法编排中（5）、（6）、（7）三步可知α(k)的变化相当于对增益系数K(k)的变化，自适应调整了系统和测量的噪声值，进而使系统模型和实际模型更接近，也就使得卡尔曼滤波具有了自适应能力并使最终的估计状态值X(k)精度更高。

c．模糊推理系统设计

α(k)的取值由模糊推理系统得到，这里的模糊推理系统(FIS)采用单输入单输出模式，此时FIS的输入为每一步的残差实测方差与理论方差比值q(k)，由上面模糊滤波算法编排中可知

其中T_r(.)表示对矩阵求迹。残差实测方差 $C_r=\frac{1}{N}\underset{i=i_0}{\overset{k}{\mathrm{\&Sigma;}}}{r}_i{r}_i^T,$ 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）。由FIS的输入，得到系统输出α(k)，对输入、输出变量模糊化，两者的隶属度函数分别为两个“trimf”和一个“trapmf”，即两个三角形隶属度函数和一个梯形隶属度函数。隶属度函数曲线见图5。采用一个梯形隶属度函数的目的是为了避免q(k)在近似等于1的情况下，FIS仍然调α(k)而引起不必要的滤波误差。

定义FIS模糊规则如下：

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模糊输出的求解，采用FIS直接输出。这样每滤波一次，FIS输出一个α(k)，对量测噪声方差阵在线调整一次。如图6所示为模糊推理系统原理框图。

除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。

Claims (2)

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(.)表示对矩阵求迹；残差实测方差 $C_r=\frac{1}{N}\underset{i=i_0}{\overset{k}{\mathrm{\&Sigma;}}}{r}_i{r}_i^T,$ 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)，对量测噪声方差阵在线调整一次。

Images