基于模糊自适应控制技术的捷联惯导非线性对准方法

摘要

本发明公开了一种基于模糊自适应控制技术的捷联惯导非线性对准方法，本发明利用模糊逻辑善于表达界限不清晰的定性知识与经验、推理解决常规方法难于解决的规则型模糊信息问题特点，通过在滤波时间更新和滤波量测更新之间添加一个专门用于动态优化弱化因子矩阵的模糊逻辑控制模块实现次优渐消因子的自适应调整。本发明具有在大失准角和运动基座条件下，模糊逻辑控制器利用残差所包含的AUV的运动信息，通过线选取次优渐消因子，调整滤波器增益，保持对舰载设备运动状态的强跟踪能力，满足复杂水下环境的AUV高精度对准要求。

说明书

技术领域

本发明主要涉及舰载导航技术领域，尤其涉及一种基于模糊自适应控制技术 的捷联惯导非线性对准方法。

背景技术

惯导系统在进入导航工作状态以前都要进行初始对准，捷联惯导系统SINS 将惯性传感器与载体直接固联，采用计算的数学平台代替物理平台，因此SINS 的初始对准就是确定初始时刻的姿态矩阵。初始对准误差是SINS的主要误差源， 对准精度直接影响SINS的工作性能；自主水下航行器（AUV）是一种依靠自身 的自治能力来管理和控制自己的智能化无人航行器，精确的导航定位的支持是 AUV可靠、持续工作的保证，从AUV所处的自然环境和应用环境出发，其所配 置的SINS只能采取动基座对准，因此SINS动基座对准技术成为SINS的关键技 术之一。海洋环境下，AUV受到阵风、洋流和海浪等各种因素干扰，特别是在 大失准角和剧烈晃动条件下，使得建立在线性小失准角模型基础上的经典 Kalman滤波方法受到制约；而传统的EKF、UKF等非线性滤波方法具有高维条 件下对准精度低，应对不确定因素能力差等缺点，又由于GPS在水下无法使用， 因此，发明具有高精度，有效应对复杂环境的多普勒测速仪DVL辅助SINS动 基座对准的非线性智能滤波方法具有重要的意义。

发明内容

发明目的：为了克服现有技术中存在的不足，本发明提供一种提高舰载捷联 惯导系统对准精度的基于模糊自适应控制技术的捷联惯导非线性对准方法。

技术方案：为解决上述技术问题，本发明提供的一种基于模糊自适应控制技 术的捷联惯导非线性对准方法，其步骤包括如下：

步骤1：建立DVL辅助SINS动基座对准模型，所述对准模型包括SINS的 非线性误差模型、非线性滤波状态模型和非线性滤波量测模型；

所述SINS非线性误差模型建立过程如下：

步骤1.1：记AUV航行的右-前-上方建立的右手坐标系为载体坐标系b，记 东-北-天当地地理坐标系为导航坐标系n，则AUV在n系下的真实姿态为 真实速度为AUV真实的地理坐标为 p＝[LλH]^T，SINS实际解算出的姿态为速度为 地理坐标为记SINS解算的地理位置建立 的坐标系为计算导航坐标系n′，定义SINS姿态误差和速度误差分别为则φ、δvⁿ的微分方程如下：

$\stackrel{.}{\phi }=C_{\omega }^{-1}[(I-C_n^{n^{\prime }}){\tilde{\omega }}_{\mathrm{in}}^n+C_n^{n^{\prime }}{\mathrm{\delta \omega }}_{\mathrm{in}}^n-C_b^{n^{\prime }}({ϵ}^b+w_g^b)]$

$\delta {\stackrel{·}{v}}^n=[I-{\left(C_n^{n^{\prime }}\right)}^T]C_b^{n^{\prime }}{\tilde{f}}^b+{\left(C_n^{n^{\prime }}\right)}^T{C}_b^{n^{\prime }}({▿}^b+w_a^b)-(2\delta {\omega }_{\mathrm{ie}}^n+\delta {\omega }_{\mathrm{en}}^n)\times {\tilde{v}}_{\sin s}^n-(2{\tilde{\omega }}_{\mathrm{ie}}^n+{\tilde{\omega }}_{\mathrm{en}}^n)\times {\mathrm{\delta v}}^n$

其中，φ＝[φ_eφ_nφ_u]^T为纵摇角、横摇角和航向角误差，δvⁿ＝[δv_eδv_nδv_u]^T为东向速度、北向速度和天向速度误差，为b系下三轴陀螺的 常值误差，为b系下三轴陀螺的随机误差，为b系下三 轴加速度计的常值误差，为b系下三轴加速度计的随机误差，为加速度计 的实际输出，为SINS解算的速度，为计算的导航坐标系的旋转角速度； 为计算的地球旋转角速度，导航坐标系相对地球的旋转角速度，为对应于的计算误差，是n系依次旋转角度φ_u、φ_e、 φ_n得到n′系所形成的方向余弦矩阵，为b系到n′系的转移矩阵，即计算的姿 态矩阵，为欧拉角微分方程的系数矩阵，其具体为：

$C_n^{n^{\prime }}=\left(\begin{array}{ccc}\cos {\phi }_n\cos {\phi }_u-\sin {\phi }_n\sin {\phi }_e\sin {\phi }_u& \cos {\phi }_n\sin {\phi }_u+\sin {\phi }_n\sin {\phi }_e\cos {\phi }_u& -\sin {\phi }_n\cos {\phi }_e\\ -\cos {\phi }_e\sin {\phi }_u& \cos {\phi }_e\cos {\phi }_u& \sin {\phi }_e\\ \sin {\phi }_n\cos {\phi }_u+\cos {\phi }_n\sin {\phi }_e\sin {\phi }_u& \sin {\phi }_n\sin {\phi }_u-\cos {\phi }_n\sin {\phi }_e\cos {\phi }_u& \cos {\phi }_n\cos {\phi }_e\end{array}\right)$

,$C_w=\left(\begin{array}{ccc}\cos {\phi }_n& 0& -\sin {\phi }_n\cos {\phi }_e\\ 0& 1& \sin {\phi }_e\\ \sin {\phi }_n& 0& \cos {\phi }_n\cos {\phi }_e\end{array}\right)$上标T表示转置；

所述非线性滤波状态模型建立过程如下：步骤1.2：选取SINS的欧拉平台 误差角φ_e、φ_n、φ_u，速度误差δv_e、δv_n，陀螺常值误差加速度计 常值误差组成状态量$x={\left[{\phi }_e{\phi }_n{\phi }_u{\mathrm{\delta v}}_e{\mathrm{\delta v}}_n{ϵ}_x^b{ϵ}_y^b{ϵ}_z^b{▿}_x^b{▿}_y^b\right]}^T,$则非线性滤波状态方程为：

$\left(\begin{array}{c}\stackrel{·}{\phi }=C_{\omega }^{-1}[(I-C_n^{n^{\prime }}){\hat{\omega }}_{\mathrm{in}}^n+C_n^{n^{\prime }}\delta {\omega }_{\mathrm{in}}^n-C_b^{n^{\prime }}{ϵ}_b]+w_g\\ \delta {\stackrel{·}{v}}^n=[I-{\left(C_n^{n^{\prime }}\right)}^T]C_b^{n^{\prime }}{\hat{f}}^b+{\left(C_n^{n^{\prime }}\right)}^T{C}_b^{n^{\prime }}{▿}^b-(2\delta {\omega }_{\mathrm{ie}}^n+\delta {\omega }_{\mathrm{en}}^n)\times {\hat{v}}^n-(2{\hat{\omega }}_{\mathrm{ie}}^n+{\hat{\omega }}_{\mathrm{en}}^n)\times {\mathrm{\delta v}}^n+w_a\\ {\stackrel{·}{ϵ}}^b=0\\ {\stackrel{·}{▿}}^b=0\end{array}\right)$

其中，只取前两维状态，并将该非线性滤波状态方程简记为 且w(t)＝[w_gw_a0_1×30_1×2]^T为零均值高斯白噪声过程；

所述非线性量测模型的建立过程如下：

步骤1.3：记AUV在b系下的真实速度为DVL测得AUV在b系下的 实际速度为利用SINS解算的姿态矩阵将变换为以和中的 东向速度和北向速度分量作为匹配信息源，则非线性滤波量测方程为：

$z={\tilde{v}}_{\sin s}^n-C_b^{n^{\prime }}{\tilde{v}}_{\mathrm{dvl}}^b={\mathrm{\delta v}}^n-[I-{\left(C_n^{n^{\prime }}\right)}^T]C_b^{n^{\prime }}{v}_{\mathrm{dvl}}^b+v$

其中，取z前两维为观测值，v为零均值高斯白噪声过程，并将该非线性滤波量 测方程简记为z(t)＝h(x,t)+v(t)；

步骤2：以DVL的输出周期T_dvl作为滤波周期，并以T_dvl为步长对非线性滤 波模型x和z(t)＝h(x,t)+v(t)进行离散化，依据得到的离散化模 型在平方根容积卡尔曼滤波的框架下进行时间更新；

所述非线性滤波模型的离散化过程为：

步骤2.1：离散化为x_k＝x_k-1+[f(x_k-1,t_k-1)+w(t_k-1)]T_dvl并简 记为x_k＝f(x_k-1)+w_k-1，z(t)＝h(x,t)+v(t)离散化为z_k＝h(x_k,t_k)+v(t_k)并简记为 z_k＝h(x_k)+v_k；

步骤3：利用当前SINS和DVL输出计算的量测值减去相同时刻的量测预测 值得到当前时刻的残差ε_k，并计算一段时间内的残差序列第1个分量和第2个分 量的统计值；

所述第1个残差分量计算和统计：

步骤3.1.1：计算残差ε_k的第1个分量ε_1k，即其中z_1k/k-1为z_k/k-1的第1个分量；

步骤3.1.2：计算包括当前时刻残差在内的前20个时刻的残差第1个分量绝 对值的平均值μ_1k和标准差σ_1k：

${\mu }_{1k}=\frac{1}{r}\underset{i=k-r+1}{\overset{k}{\Sigma }}\left|{ϵ}_{i,1k}\right|,{\sigma }_{1k}=\frac{1}{r}\sqrt{\underset{i=k-r-1}{\overset{k}{\Sigma }}{\left(\right|{ϵ}_{i,1k}|-{\mu }_{1k})}^2}$

其中，ε_i,1k表示i时刻的第1个残差分量，k代表当前时刻，r=20；

所述第2个残差分量计算和统计：

步骤3.2.1：计算残差ε_k的第2个分量ε_2k，即其中z_2k/k-1为 z_k/k-1的第2个分量；

步骤3.2.2：计算包括当前时刻残差在内的前20个时刻的残差第2个分量绝 对值的平均值μ_2k和标准差σ_2k：

${\mu }_{2k}=\frac{1}{r}\underset{i=k-r+1}{\overset{k}{\Sigma }}\left|{ϵ}_{i,2k}\right|,{\sigma }_{2k}=\frac{1}{r}\sqrt{\underset{i=k-r-1}{\overset{k}{\Sigma }}{\left(\right|{ϵ}_{i,2k}|-{\mu }_{2k})}^2}$

其中，ε_i,2k表示i时刻的第2个残差分量，r=20；

步骤4：将μ_1k和σ_1k作为模糊逻辑控制器1的输入量，μ_2k和σ_2k作为模糊逻 辑控制器2的输入量，经过模糊逻辑运算，输出精确量弱化因子l_1k和l_2k，并将 其组成弱化因子对角阵l_k=diag[l_1k l_2k]；

步骤5：依据强跟踪滤波原理计算次优渐消因子λ_k，然后利用λ_k修正滤波时 间更新过程，最后完成滤波量测更新；

步骤6：利用当前获得的欧拉平台误差角估计值和速度估计值修正 SINS解算的姿态矩阵和速度将修正之后的值作为下一次捷联解算的初 值，利用当前获得的陀螺的常值误差估计值和加速度计的常值误差估计值修正下一时刻的陀螺输出和加速度计输出具体修正公式按下式计算：

$C_b^n={\hat{C}}_{n^{\prime }}^n{C}_b^{n^{\prime }},v_{\sin s}^n={\tilde{v}}_{\sin s}^n-\delta {\hat{v}}_k^n,{\omega }_{\mathrm{ib}}^b={\tilde{\omega }}_{\mathrm{ib}}^b-{\widehat{ϵ}}_k^b,f^b={\tilde{f}}^b-{\widehat{▿}}_k^b$

若姿态精度达到要求，对准结束，否则继续递推执行步骤2至步骤6，直到对准 结束。

进一步地，所述步骤2中，据得到的离散化模型在平方根容积卡尔曼滤波的 框架下进行时间更新过程步骤为：

步骤2.2：设置滤波状态初值和初始误差协方差阵P₀，并对P₀进行cholesky 分解，得到初始误差协方差阵的特征平方根S₀；

步骤2.3：利用上一时刻的S_k-1估算容积点X_i,k-1并计算传播容积点

$X_{i,k/k-1}=S_{k/k-1}{\xi }_i+{\hat{x}}_{k/k-1},Z_{i,k/k-1}=h\left(X_{i,k/k-1}\right);$

其中，S_k-1是上一时刻误差协方差阵的特征平方根，是上一时刻的状态估计 值，ξ_i表示第i个容积点，2c个容积点为：${\xi }_i=\left(\begin{array}{cc}\sqrt{{\mathrm{ce}}_i},& i=\mathrm{1,2}...,c\\ -\sqrt{{\mathrm{ce}}_{i-c}},& i=c+1,c+2...,2c\end{array}\right),$e_i为c维的初等列向量，c是状态量个数，即c=10；

步骤2.4：计算状态一步预测值和一步预测误差协方差阵特征平方根 S_k/k-1：

${\hat{x}}_{k/k-1}=\frac{1}{2c}\underset{i=1}{\overset{2c}{\Sigma }}{X}_{i,k/k-1}^{*}$

${\chi }_{k/k-1}^{*}=\frac{1}{\sqrt{2c}}[X_{1,k/k-1}^{*}-x_{k/k-1}{X}_{2,k/k-1}^{*}-{\hat{x}}_{k/k-1}...X_{2c,k/k-1}^{*}-{\hat{x}}_{k/k-1}]$

$\left(\begin{array}{cc}A& B\end{array}\right)=\mathrm{qr}\left\{\right[{\chi }_{k.k-1}^{*}\sqrt{Q_k}{]}^T\}$

S_k/k-1＝B(1:c,:)^T

其中，是加权中心矩阵，是系统噪声方差阵Q_k的特征平方根，qr{·} 表示对矩阵进行qr分解，B(1:c,:)表示取矩阵B的第1行至第c行形成的c×c矩 阵；

步骤2.5：计算容积点X_i,k/k-1并更新量测方程传播容积点Z_i,k/k-1：

$X_{i,k/k-1}=S_{k/k-1}{\xi }_i+{\hat{x}}_{k/k-1},Z_{i,k/k-1}=h\left(X_{i,k/k-1}\right);$

步骤2.6：计算量测预测值量测预测误差协方差阵特征平方根S_zz,k/k-1：

${\hat{z}}_{k/k-1}=\frac{1}{2c}\underset{i=1}{\overset{2c}{\Sigma }}{z}_{i,k/k-1}$

${\eta }_{k/k-1}=\frac{1}{\sqrt{2c}}[Z_{1,k/k-1}-{\hat{z}}_{k/k-1}{Z}_{2,k/k-1}-{\hat{z}}_{k/k-1}...Z_{2c,k/k-1}-{\hat{z}}_{k/k-1}]$

$\left(\begin{array}{cc}C& D\end{array}\right)=\mathrm{qr}\left\{{\eta }_{k/k-1}\sqrt{R_k}{]}^T\right\}$

S_zz,k/k-1＝D(1:m,:)^T

其中，η_k/k-1是加权中心矩阵，是系统量测方差阵R_k的特征平方根，D(1:m,:) 表示取矩阵D的第1行至第m行形成的m×m矩阵，m是量测状态个数，即m= 2；

步骤2.7：计算互协方差阵P_xz,k/k-1：

${\chi }_{k/k-1}=\frac{1}{\sqrt{2c}}[X_{1,k/k-1}-{\hat{x}}_{k/k-1}{X}_{2,k/k-1}-{\hat{x}}_{k/k-1}...X_{2c,k/k-1}-{\hat{x}}_{k/k-1}]$

$P_{\mathrm{xz},k/k-1}={\chi }_{k/k-1}{\eta }_{k/k-1}^T$

其中，χ_k/k-1是加权中心矩阵。

进一步地，所述步骤4中：

所述模糊逻辑控制器1的模糊逻辑运算过程为：

步骤4.1.1：确定μ_1k、σ_1k和l_1k的论域集并划分论域，建立μ_1k、σ_1k和l_1k的 三角形隶属度函数MF(μ₁)、MF(σ₁)和MF(l₁)；

步骤4.1.2：分别将μ_1k和σ_1k带入MF(μ₁)和MF(σ₁)计算得到对应的输入模糊 集μ_{1k_set}和σ_{1k_set}；

步骤4.1.3：建立Sugeno型模糊推理规则，对μ_{1k_set}和σ_{1k_set}进行模糊关系合 成和模糊推理合成得到输出模糊集l_{1k_set}；

步骤4.1.4：依据MF(l₁)采用重心法进行解模糊化得到输出精确值l_1k，其中 重心法计算式如下：

$v_0=\frac{\underset{k=1}{\overset{m}{\Sigma }}{v}_k{\mu }_v\left(v_k\right)}{\underset{k=1}{\overset{m}{\Sigma }}{\mu }_v\left(v_k\right)}$

其中，v_k是模糊集合元素，μ_v(v_k)是元素v_k的隶属度，v₀是精确值；

所述模糊逻辑控制器2的模糊逻辑运算过程为：

步骤4.2.1：确定μ_2k、σ_2k和l_2k的论域集并划分论域，建立μ_2k、σ_2k和l_2k的 三角形隶属度函数MF(μ₂)、MF(σ₂)和MF(l₂)；

步骤4.2.2：分别将μ_2k和σ_2k带入MF(μ₂)和MF(σ₂)计算得到对应的输入模糊 集μ_{2k_set}和σ_{2k_set}；

步骤4.2.3：建立Sugeno型模糊推理规则，对μ_{2k_set}和σ_{2k_set}进行模糊关系合 成和模糊推理合成得到输出模糊集l_{2k_set}；

步骤4.2.4：依据MF(l₂)采用步骤4.1.4所用重心法进行解模糊化得到输出精 确值l_2k。

进一步地，所述步骤5中：

所述计算次优渐消因子λ_k的过程为：

步骤5.1.1：若k=1，$V_k={ϵ}_k{ϵ}_k^T,$即$V_1={ϵ}_1{ϵ}_1^T;$若k>1，$V_k=\frac{\rho V_{k-1}+{ϵ}_k{ϵ}_k^T}{1+\rho },$其中0.95≤ρ≤0.995为遗忘因子；

步骤5.1.2：计算$N_k=V_k-{\left[P_{\mathrm{xz},k/k-1}\right]}^T{Q}_{k-1}{\left[S_{k/k-1}{S}_{k/k-1}^T\right]}^{-1}{P}_{\mathrm{xz},k/k-1}-l_k{R}_k$和$M_k=S_{\mathrm{zz},k/k-1}{S}_{\mathrm{zz},k/k-1}^T-V_k+N_k,$其中N_k和M_k为中间值；

步骤5.1.3：计算若λ_0,k<1，则λ_k=1；若λ_0,k≥1，则λ_k=λ_0,k， 其中trace(·)表示矩阵的迹；

所述λ_k修正滤波时间更新过程为：

步骤5.2.1：利用式代替步骤2.4中的式 $\left(\begin{array}{cc}A& B\end{array}\right)=\mathrm{qr}{\left\{\right[\chi }_{/k-1}^{*}\sqrt{Q_k}{]}^T\};$

步骤5.2.2：再次执行步骤2.5至步骤2.7；

所述滤波量测更新过程为：

步骤5.3.1：计算滤波增益矩阵K_k，即

步骤5.3.2：利用前述步骤计算的变量值更新状态和误差协方差阵的特征 平方根Sk：

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(z_k-{\hat{z}}_{k/k-1})$

$\left(\begin{array}{cc}E& F\end{array}\right)=\mathrm{qr}\left\{\right[{\chi }_{k/k-1}-K_k{\eta }_{k/k-1}{K}_k\sqrt{R_k}{]}^T\}$

S_k＝F(1:c,:)^T。

本发明利用模糊逻辑善于表达界限不清晰的定性知识与经验、推理解决常规 方法难于解决的规则型模糊信息问题特点，通过在滤波时间更新和滤波量测更新 之间添加一个专门用于动态优化弱化因子矩阵的模糊逻辑控制模块实现次优渐 消因子的自适应调整。本发明具有在大失准角和晃动基座条件下，模糊逻辑控制 器利用残差所包含的舰载设备的运动信息，通过线选取次优渐消因子，调整滤波 器增益，保持对舰载设备运动状态的强跟踪能力，满足复杂水下环境的AUV高 精度对准要求。

有益效果：本发明相对于现有技术具有以下优点：

1)解决了大失准角、晃动基座复杂条件下的舰载捷联惯导系统对准精度下 降问题，为舰载捷联惯导系统提供高精度的初始姿态信息，保证舰载捷 联惯导系统提供可靠的导航定位信息；

2)采用适用于高维、强非线性条件下的容积卡尔曼滤波器以及引入强跟踪 滤波思想；创造性的提出弱化因子矩阵的概念，有效区别各观测信息的 差异，并引入模糊逻辑控制技术实现次优渐消因子的在线调整，实现对 舰载设备的运动状态的强跟踪，提高滤波和对准的精度；

3)使用DVL提供高精度可靠的速度观测信息，有助于从残差中提取更多有 关于滤波状态量的信息，提高对准精度和速度。

附图说明

图1为本发明DVL辅助SINS动基座对准方案图。

图2为本发明四波束Janus配置DVL测速示意图。

图3为本发明基于模糊逻辑控制技术的非线性智能滤波方法原理图。

图4为本发明模糊逻辑控制器的结构图。

图5为本发明AUV航行纵摇角、横摇角和航向角摇摆模拟图。

图6为本发明SINS三轴陀螺输出模拟图。

图7为本发明SINS三轴加速度计输出模拟图。

图8为本发明DVL辅助SINS动基座对准纵摇误差图。

图9为本发明DVL辅助SINS动基座对准横摇误差图。

图10为本发明DVL辅助SINS动基座对准航向误差图。

具体实施方式

下面结合附图对本发明作更进一步的说明。

一种基于模糊自适应控制技术的捷联惯导非线性对准方法：

步骤1：建立DVL辅助SINS动基座对准模型，所述对准模型包括SINS的 非线性误差模型、非线性滤波状态模型和非线性滤波量测模型；

所述SINS非线性误差模型建立过程如下：

步骤1.1：记AUV航行的右-前-上方建立的右手坐标系为载体坐标系b，记 东-北-天当地地理坐标系为导航坐标系n，则AUV在n系下的真实姿态为 真实速度为AUV真实的地理坐标为 p＝[LλH]^T，SINS实际解算出的姿态为速度为 地理坐标为记SINS解算的地理位置建立 的坐标系为计算导航坐标系n′，定义SINS姿态误差和速度误差分别为则φ、δvⁿ的微分方程如下：

$\stackrel{.}{\phi }=C_{\omega }^{-1}[(I-C_n^{n^{\prime }}){\tilde{\omega }}_{\mathrm{in}}^n+C_n^{n^{\prime }}{\mathrm{\delta \omega }}_{\mathrm{in}}^n-C_b^{n^{\prime }}({ϵ}^b+w_g^b)]$

$\delta {\stackrel{·}{v}}^n=[I-{\left(C_n^{n^{\prime }}\right)}^T]C_b^{n^{\prime }}{\tilde{f}}^b+{\left(C_n^{n^{\prime }}\right)}^T{C}_b^{n^{\prime }}({▿}^b+w_a^b)-(2\delta {\omega }_{\mathrm{ie}}^n+\delta {\omega }_{\mathrm{en}}^n)\times {\tilde{v}}_{\sin s}^n-(2{\tilde{\omega }}_{\mathrm{ie}}^n+{\tilde{\omega }}_{\mathrm{en}}^n)\times {\mathrm{\delta v}}^n$其中，φ＝[φ_eφ_nφ_u]^T为纵摇角、横摇角和航向角误差，δvⁿ＝[δv_eδv_nδv_u]^T为东向速度、北向速度和天向速度误差，和为b系下三轴陀螺 的常值误差，为b系下三轴陀螺的随机误差，为b系下 三轴加速度计的常值误差，为b系下三轴加速度计的随机误差，为加速度 计的实际输出，为SINS解算的速度，为计算的导航坐标系的旋转角速度； 为计算的地球旋转角速度，导航坐标系相对地球的旋转角速度，为对应于的计算误差，是n系依次旋转角度φ_u、φ_e、 φ_n得到n′系所形成的方向余弦矩阵，为b系到n′系的转移矩阵，即计算的姿 态矩阵，为欧拉角微分方程的系数矩阵，其具体为：

$C_n^{n^{\prime }}=\left(\begin{array}{ccc}\cos {\phi }_n\cos {\phi }_u-\sin {\phi }_n\sin {\phi }_e\sin {\phi }_u& \cos {\phi }_n\sin {\phi }_u+\sin {\phi }_n\sin {\phi }_e\cos {\phi }_u& -\sin {\phi }_n\cos {\phi }_e\\ -\cos {\phi }_e\sin {\phi }_u& \cos {\phi }_e\cos {\phi }_u& \sin {\phi }_e\\ \sin {\phi }_n\cos {\phi }_u+\cos {\phi }_n\sin {\phi }_e\sin {\phi }_u& \sin {\phi }_n\sin {\phi }_u-\cos {\phi }_n\sin {\phi }_e\cos {\phi }_u& \cos {\phi }_n\cos {\phi }_e\end{array}\right)$，$C_w=\left(\begin{array}{ccc}\cos {\phi }_n& 0& -\sin {\phi }_n\cos {\phi }_e\\ 0& 1& \sin {\phi }_e\\ \sin {\phi }_n& 0& \cos {\phi }_n\cos {\phi }_e\end{array}\right)$上标T表示转置；

所述非线性滤波状态模型建立过程如下：

步骤1.2：选取SINS的欧拉平台误差角φ_e、φ_n、φ_u，速度误差δv_e、δv_n， 陀螺常值误差加速度计常值误差组成状态量 $x={\left[{\phi }_e{\phi }_n{\phi }_u{\mathrm{\delta v}}_e{\mathrm{\delta v}}_n{ϵ}_x^b{ϵ}_y^b{ϵ}_z^b{▿}_x^b{▿}_y^b\right]}^T,$则非线性滤波状态方程为：

$\left(\begin{array}{c}\stackrel{·}{\phi }=C_{\omega }^{-1}[(I-C_n^{n^{\prime }}){\hat{\omega }}_{\mathrm{in}}^n+C_n^{n^{\prime }}\delta {\omega }_{\mathrm{in}}^n-C_b^{n^{\prime }}{ϵ}_b]+w_g\\ \delta {\stackrel{·}{v}}^n=[I-{\left(C_n^{n^{\prime }}\right)}^T]C_b^{n^{\prime }}{\hat{f}}^b+{\left(C_n^{n^{\prime }}\right)}^T{C}_b^{n^{\prime }}{▿}^b-(2\delta {\omega }_{\mathrm{ie}}^n+\delta {\omega }_{\mathrm{en}}^n)\times {\hat{v}}^n-(2{\hat{\omega }}_{\mathrm{ie}}^n+{\hat{\omega }}_{\mathrm{en}}^n)\times {\mathrm{\delta v}}^n+w_a\\ {\stackrel{·}{ϵ}}^b=0\\ {\stackrel{·}{▿}}^b=0\end{array}\right)$

其中，只取前两维状态，并将该非线性滤波状态方程简记为 且w(t)＝[w_gw_a0_1×30_1×2]^T为零均值高斯白噪声过程；

所述非线性量测模型的建立过程如下：

步骤1.3：记AUV在b系下的真实速度为DVL测得AUV在b系下的 实际速度为利用SINS解算的姿态矩阵将变换为以和中的 东向速度和北向速度分量作为匹配信息源，则非线性滤波量测方程为：

$z={\tilde{v}}_{\sin s}^n-C_b^{n^{\prime }}{\tilde{v}}_{\mathrm{dvl}}^b={\mathrm{\delta v}}^n-[I-{\left(C_n^{n^{\prime }}\right)}^T]C_b^{n^{\prime }}{v}_{\mathrm{dvl}}^b+v$

其中，取z前两维为观测值，v为零均值高斯白噪声过程，并将该非线性滤波量 测方程简记为z(t)＝h(x,t)+v(t)；

步骤2：以DVL的输出周期T_dvl作为滤波周期，并以T_dvl为步长对非线性滤 波模型和z(t)＝h(x,t)+v(t)进行离散化，依据得到的离散化模 型在平方根容积卡尔曼滤波的框架下进行时间更新；

所述非线性滤波模型的离散化过程为：

步骤2.1：离散化为x_k＝x_k-1+[f(x_k-1,t_k-1)+w(t_k-1)]T_dvl并简 记为x_k＝f(x_k-1)+w_k-1，z(t)＝h(x,t)+v(t)离散化为z_k＝h(x_k,t_k)+v(t_k)并简记为 z_k＝h(x_k)+v_k；

所述滤波时间更新过程为：

步骤2.2：设置滤波状态初值和初始误差协方差阵P₀，并对P₀进行cholesky 分解，得到初始误差协方差阵的特征平方根S₀；

步骤2.3：利用上一时刻的S_k-1估算容积点X_i,k-1并计算传播容积点

$X_{i,k-1}=S_{k-1}{\xi }_i+{\hat{x}}_{k-1},X_{i,k/k-1}^{*}=f\left(X_{i,k-1}\right)(i=\mathrm{1,2}...2c)$

其中，S_k-1是上一时刻误差协方差阵的特征平方根，是上一时刻的状态估计 值，ξ_i表示第i个容积点，2c个容积点为：${\xi }_i=\left(\begin{array}{cc}\sqrt{{\mathrm{ce}}_i},& i=\mathrm{1,2}...,c\\ -\sqrt{{\mathrm{ce}}_{i-c}},& i=c+1,c+2...,2c\end{array}\right),$e_i为c维的初等列向量，c为状态量个数，即c=10；

步骤2.4：计算状态一步预测值和一步预测误差协方差阵特征平方根 S_k/k-1：

${\hat{x}}_{k/k-1}=\frac{1}{2c}\underset{i=1}{\overset{2c}{\Sigma }}{X}_{i,k/k-1}^{*}$

${\chi }_{k/k-1}^{*}=\frac{1}{\sqrt{2c}}[X_{1,k/k-1}^{*}-x_{k/k-1}{X}_{2,k/k-1}^{*}-{\hat{x}}_{k/k-1}...X_{2c,k/k-1}^{*}-{\hat{x}}_{k/k-1}]$

$\left(\begin{array}{cc}A& B\end{array}\right)=\mathrm{qr}\left\{\right[{\chi }_{k.k-1}^{*}\sqrt{Q_k}{]}^T\}$

S_k/k-1＝B(1:c,:)^T

其中，是加权中心矩阵，是系统噪声方差阵Q_k的特征平方根，qr{·} 表示对矩阵进行qr分解，B(1:c,:)表示取矩阵B的第1行至第c行形成的c×c 矩阵；

步骤2.5：计算容积点X_i,k/k-1并更新量测方程传播容积点Z_i,k/k-1：

$X_{i,k/k-1}=S_{k/k-1}{\xi }_i+{\hat{x}}_{k/k-1},Z_{i,k/k-1}=h\left(X_{i,k/k-1}\right);$

步骤2.6：计算量测预测值量测预测误差协方差阵特征平方根S_zz,k/k-1：

${\hat{z}}_{k/k-1}=\frac{1}{2c}\underset{i=1}{\overset{2c}{\Sigma }}{z}_{i,k/k-1}$

${\eta }_{k/k-1}=\frac{1}{\sqrt{2c}}[Z_{1,k/k-1}-{\hat{z}}_{k/k-1}{Z}_{2,k/k-1}-{\hat{z}}_{k/k-1}...Z_{2c,k/k-1}-{\hat{z}}_{k/k-1}]$

$\left(\begin{array}{cc}C& D\end{array}\right)=\mathrm{qr}\left\{{\eta }_{k/k-1}\sqrt{R_k}{]}^T\right\}$

S_zz,k/k-1＝D(1:m,:)^T

其中，η_k/k-1是加权中心矩阵，是系统量测方差阵R_k的特征平方根，D(1:m,:) 表示取矩阵D的第1行至第m行形成的m×m矩阵，m是量测状态个数，m=2；

步骤2.7：计算互协方差阵P_xz,k/k-1：

${\chi }_{k/k-1}=\frac{1}{\sqrt{2c}}[X_{1,k/k-1}-{\hat{x}}_{k/k-1}{X}_{2,k/k-1}-{\hat{x}}_{k/k-1}...X_{2c,k/k-1}-{\hat{x}}_{k/k-1}]$

$P_{\mathrm{xz},k/k-1}={\chi }_{k/k-1}{\eta }_{k/k-1}^T$

其中，χ_k/k-1是加权中心矩阵；

步骤3：利用当前SINS和DVL输出计算的量测值减去相同时刻的量测预测 值得到当前时刻的残差ε_k，并计算一段时间内的残差序列第1个分量和第2个分 量的统计值；

所述第1个残差分量计算和统计：

步骤3.1.1：计算残差ε_k的第1个分量ε_1k，即其中z_1k/k-1为z_k/k-1的第一个分量；

步骤3.1.2：计算包括当前时刻残差在内的前20个时刻的残差第1个分量绝 对值的平均值μ_1k和标准差σ_1k：

${\mu }_{1k}=\frac{1}{r}\underset{i=k-r+1}{\overset{k}{\Sigma }}\left|{ϵ}_{i,1k}\right|,{\sigma }_{1k}=\frac{1}{r}\sqrt{\underset{i=k-r-1}{\overset{k}{\Sigma }}{\left(\right|{ϵ}_{i,1k}|-{\mu }_{1k})}^2}$

其中，ε_i,1k表示i时刻的第1个残差分量，k是当前时刻，r=20；

所述第2个残差分量计算和统计：

步骤3.2.1：计算残差ε_k的第2个分量ε_2k，即其中z_2k/k-1为 z_k/k-1的第2个分量；

步骤3.2.2：计算包括当前时刻残差在内的前20个时刻的残差第2个分量绝 对值的平均值μ_2k和标准差σ_2k：

${\mu }_{2k}=\frac{1}{r}\underset{i=k-r+1}{\overset{k}{\Sigma }}\left|{ϵ}_{i,2k}\right|,{\sigma }_{2k}=\frac{1}{r}\sqrt{\underset{i=k-r-1}{\overset{k}{\Sigma }}{\left(\right|{ϵ}_{i,2k}|-{\mu }_{2k})}^2}$

其中，ε_i,2k表示i时刻的第2个残差分量,r=20；

步骤4：将μ_1k和σ_1k作为模糊逻辑控制器1的输入量，μ_2k和σ_2k作为模糊逻 辑控制器2的输入量，经过模糊逻辑运算，输出精确量弱化因子l_1k和l_2k，并将 其组成弱化因子对角阵l_k=diag[l_1k l_2k]

所述模糊逻辑控制器1的模糊逻辑运算过程为：

步骤4.1.1：确定μ_1k、σ_1k和l_1k的论域集并划分论域，建立μ_1k、σ_1k和l_1k的 三角形隶属度函数MF(μ₁)、MF(σ₁)和MF(l₁)；

步骤4.1.2：分别将μ_1k和σ_1k带入MF(μ₁)和MF(σ₁)计算得到对应的输入模糊 集μ_{1k_set}和σ_{1k_set}；

步骤4.1.3：建立Sugeno型模糊推理规则，对μ_{1k_set}和σ_{1k_set}进行模糊关系合 成和模糊推理合成得到输出模糊集l_{1k_set}；

步骤4.1.4：依据MF(l₁)采用重心法进行解模糊化得到输出精确值l_1k，其中 重心法计算式如下：

$v_0=\frac{\underset{k=1}{\overset{m}{\Sigma }}{v}_k{\mu }_v\left(v_k\right)}{\underset{k=1}{\overset{m}{\Sigma }}{\mu }_v\left(v_k\right)}$

其中，v_k是模糊集合元素，μ_v(v_k)是元素v_k的隶属度，v₀是精确值；

所述模糊逻辑控制器2的模糊逻辑运算过程为：

步骤4.2.1：确定μ_2k、σ_2k和l_2k的论域集并划分论域，建立μ_2k、σ_2k和l_2k的 三角形隶属度函数MF(μ₂)、MF(σ₂)和MF(l₂)；

步骤4.2.2：分别将μ_2k和σ_2k带入MF(μ₂)和MF(σ₂)计算得到对应的输入模糊 集μ_{2k_set}和σ_{2k_set}；

步骤4.2.3：建立Sugeno型模糊推理规则，对μ_{2k_set}和σ_{2k_set}进行模糊关系合 成和模糊推理合成得到输出模糊集l_{2k_set}；

步骤4.2.4：依据MF(l₂)采用步骤4.1.4所用重心法进行解模糊化得到输出精 确值l_2k；

步骤5：依据强跟踪滤波原理计算次优渐消因子λ_k，然后利用λ_k修正滤波时 间更新过程，最后完成滤波量测更新

所述计算次优渐消因子λ_k的过程为：

步骤5.1.1：若k=1，$V_k={ϵ}_k{ϵ}_k^T,$即$V_1={ϵ}_1{ϵ}_1^T;$若k>1，$V_k=\frac{\rho V_{k-1}+{ϵ}_k{ϵ}_k^T}{1+\rho },$其中0.95≤ρ≤0.995为遗忘因子；

步骤5.1.2：计算$N_k=V_k-{\left[P_{\mathrm{xz},k/k-1}\right]}^T{Q}_{k-1}{\left[S_{k/k-1}{S}_{k/k-1}^T\right]}^{-1}{P}_{\mathrm{xz},k/k-1}-l_k{R}_k$和$M_k=S_{\mathrm{zz},k/k-1}{S}_{\mathrm{zz},k/k-1}^T-V_k+N_k,$其中N_k和M_k为中间值；

步骤5.1.3：计算若λ_0,k<1，则λ_k=1；若λ_0,k≥1，则λ_k=λ_0,k， 其中trace(·)表示矩阵的迹；

所述λ_k修正滤波时间更新过程为：

步骤5.2.1：利用式代替步骤2.4中的式 $\left(\begin{array}{cc}A& B\end{array}\right)=\mathrm{qr}{\left\{\right[\chi }_{/k-1}^{*}\sqrt{Q_k}{]}^T\};$

步骤5.2.2：再次执行步骤2.5至步骤2.7；

所述滤波量测更新过程为：

步骤5.3.1：计算滤波增益矩阵K_k，即

步骤5.3.2：利用前述步骤计算的变量值更新状态和误差协方差阵的特征 平方根S_k：

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(z_k-{\hat{z}}_{k/k-1})$

$\left(\begin{array}{cc}E& F\end{array}\right)=\mathrm{qr}\left\{\right[{\chi }_{k/k-1}-K_k{\eta }_{k/k-1}{K}_k\sqrt{R_k}{]}^T\}$

S_k＝F(1:c,:)^T；

步骤6：利用当前获得的欧拉平台误差角估计值和速度估计值修正 SINS解算的姿态矩阵和速度将修正之后的值作为下一次捷联解算的初 值，利用当前获得的陀螺的常值误差估计值和加速度计的常值误差估计值修正下一时刻的陀螺输出和加速度计输出具体修正公式按下式计算：

$C_b^n={\hat{C}}_{n^{\prime }}^n{C}_b^{n^{\prime }},v_{\sin s}^n={\tilde{v}}_{\sin s}^n-\delta {\hat{v}}_k^n,{\omega }_{\mathrm{ib}}^b={\tilde{\omega }}_{\mathrm{ib}}^b-{\widehat{ϵ}}_k^b,f^b={\tilde{f}}^b-{\widehat{▿}}_k^b$

若姿态精度达到要求，对准结束，否则继续递推执行步骤2至步骤6，直到对准 结束。

SINS的惯性测量组件安装在AUV内部，DVL安装在AUV底部，DVL辅 助SINS动基座对准原理如图1所示，四波束Janus配置的DVL测速示意图如图 2所示，基于模糊逻辑控制技术的非线性智能方法原理图如图3所示。

以下叙述均针对水下航行器，即载体为一般AUV。

使用如下的实例说明本发明的有益效果：

1)舰船运动参数设置

仿真初始时刻AUV在北纬32°，东经118°的水下10m处；AUV在海浪的 激励下分别绕纵摇轴、横摇轴和航向轴以正弦函数作三轴摇摆运动，纵摇角θ、 横摇角γ和航向角ψ的摇摆幅值为6°、12°、10°，摇摆周期分别是6s、8s、9s， 初始航向角为45°，其模拟曲线图如图5所示；同时AUV做线运动，初始东向 速度和北向速度均为5m/s，0～10s为匀加速直线运动，东向加速度和北向加速度 均为0.5m/s²，之后为匀速直线运动，航行时间为300s；

2)模糊逻辑控制器设计

本发明采用SINS输出的东向速度和北向速度与DVL输出的东向速度和北 向速度之差作为观测值，因此需要设计两个模糊逻辑控制器，模糊逻辑控制器的 结构如图4所示，模糊控制器采用Sugeno型模糊推理规则，如表1所示：

表1

其语法规则为：Ifμ_jk is…,andσ_jk is…,then l_jk is…；

3)传感器参数设置

舰载捷联惯导系统采用光纤陀螺和挠性加速度计，陀螺常值漂移为0.02°/h， 陀螺随机漂移0.01°/h，加速度计偏置为100×10^-6g（g为重力加速度），加速度 计随机误差为50×10^-6g，模拟AUV的三轴陀螺输出ω_x、ω_y、ω_z和三轴加速度计 输出f_x、f_y、f_z如图6、图7所示；所采用的DVL测速误差为0.1m/s；

4)仿真结果分析

5)对准的初始失准角为10°、10°、10°，使用本发明提出的基于模糊控制技术 的非线性智能滤波方法进行DVL辅助SINS动基座对准，图8、9、10为使 用本发明方法完成对准的纵摇角误差φ_x、横摇角误差φ_y和航向角误差φ_z曲线，仿真结果表明，AUV在运动状态下，对于存在大失准角、晃动基座 及DVL速度在载体坐标系和导航坐标系之间变换而产生的噪声时变等恶劣 海洋条件下，使用本发明的非线性智能滤波方法仍然能够保证具有很高的对 准精度，满足AUV的水下导航定位需求。

6)以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技 术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰， 这些改进和润饰也应视为本发明的保护范围。