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

摘要

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

主权项

一种基于模糊自适应控制技术的捷联惯导非线性对准方法，其特征在于：其步骤包括如下：步骤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 }}){\bar{\omega }}_{in}^n+C_n^{n^{\prime }}{\mathrm{\delta \omega }}_{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{\mathrm{\delta \omega }}_{ie}^n+{\mathrm{\delta \omega }}_{en}^n)\times {\tilde{v}}_{\sin s}^n-(2{\tilde{\omega }}_{ie}^n+{\tilde{\omega }}_{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}{\mathrm{cos\phi }}_n{\mathrm{cos\phi }}_u-{\mathrm{sin\phi }}_n{\mathrm{sin\phi }}_e{\mathrm{sin\phi }}_u& {\mathrm{cos\phi }}_n{\mathrm{sin\phi }}_u+{\mathrm{sin\phi }}_n{\mathrm{sin\phi }}_e{\mathrm{cos\phi }}_u& -{\mathrm{sin\phi }}_n{\mathrm{cos\phi }}_e\\ -{\mathrm{cos\phi }}_e{\mathrm{sin\phi }}_u& {\mathrm{cos\phi }}_e{\mathrm{cos\phi }}_u& {\mathrm{sin\phi }}_e\\ {\mathrm{sin\phi }}_n{\mathrm{cos\phi }}_u+{\mathrm{cos\phi }}_n{\mathrm{sin\phi }}_e{\mathrm{sin\phi }}_u& {\mathrm{sin\phi }}_n{\mathrm{sin\phi }}_u-{\mathrm{cos\phi }}_n{\mathrm{sin\phi }}_e{\mathrm{cos\phi }}_u& {\mathrm{cos\phi }}_n{\mathrm{cos\phi }}_e\end{array}\right]$上标T表示转置；所述非线性滤波状态模型建立过程如下：步骤1.2：选取SINS的欧拉平台误差角φ_e、φ_n、φ_u，速度误差δv_e、δv_n，b系下三轴陀螺常值误差b系下三轴加速度计常值误差组成状态量则非线性滤波状态方程为：$\left\{\begin{array}{c}\stackrel{·}{\phi }=C_{\omega }^{-1}[(I-C_n^{n^{\prime }}){\hat{\omega }}_{in}^n+C_n^{n^{\prime }}{\mathrm{\delta \omega }}_{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{\mathrm{\delta \omega }}_{ie}^n+{\mathrm{\delta \omega }}_{en}^n)\times {\hat{v}}^n-(2{\hat{\omega }}_{ie}^n+{\hat{\omega }}_{en}^n)\times {\mathrm{\delta v}}^n+w_a\\ {\stackrel{·}{ϵ}}^b=0\\ {\stackrel{·}{▿}}^b=0\end{array}\right.$其中，取前两维状态，并将该非线性滤波状态方程简记为且w(t)[w_g w_a 0_1×3 0_1×2]^T为零均值高斯白噪声过程；所述非线性量测模型的建立过程如下：步骤1.3：记AUV在b系下的真实速度为DVL测得AUV在b系下的实际速度为利用SINS解算的姿态矩阵将变换为以和中的东向速度和北向速度分量作为匹配信息源，则非线性滤波量测方程为：$z={\tilde{v}}_{\sin s}^n-C_b^{n^{\prime }}{\tilde{v}}_{dvl}^b={\mathrm{\delta v}}^n-[I-{\left(C_n^{n^{\prime }}\right)}^T]C_b^{n^{\prime }}{v}_{dvl}^b+v$其中，取z前两维为观测值，v为零均值高斯白噪声过程，并将该非线性滤波量测方程简记为z(t)＝h(x，t)+v(t)；步骤2：以DVL的输出周期T_dvl作为滤波周期，并以T_dvl为步长对非线性滤波模型进行离散化，依据得到的离散化模型在平方根容积卡尔曼滤波的框架下进行时间更新；所述非线性滤波模型的离散化过程为：步骤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]；所述模糊逻辑控制器1的模糊逻辑运算过程为：步骤4.1.1：确定μ_1k、σ_1k和l_1k的论域集并划分论域，建立μ_1k、σ_1k和l_1k的三角形隶属度函数MF(μ₁)、MF(σ₁)和MF(l₁)；步骤4.1.2：分别将μ_1k和σ_1k带入MF(μ₁)和MF(σ₁)计算得到对应的输入模糊集μ_{1k_seset}和σ_{1k_sset}；步骤4.1.3：建立Sugeno型模糊推理规则，对μ_{1k_set}和σ_{1k_sets}进行模糊关系合成和模糊推理合成得到输出模糊集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修正滤波时间更新过程，最后完成滤波量测更新；步骤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 }_{ib}^b={\tilde{\omega }}_{ib}^b-{\widehat{ϵ}}_k^b,f^b={\tilde{f}}^b-{\widehat{▿}}_k^b$若姿态精度达到要求，对准结束，否则继续递推执行步骤2至步骤6，直到对准结束。