[发明专利]基于模糊自适应控制技术的捷联惯导非线性对准方法

摘要

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

主权项

一种基于模糊自适应控制技术的捷联惯导非线性对准方法，其特征在于：其步骤包括如下：步骤1：建立DVL辅助SINS动基座对准模型，所述对准模型包括SINS的非线性误差模型、非线性滤波状态模型和非线性滤波量测模型；所述SINS非线性误差模型建立过程如下：步骤1.1：记AUV航行的右‑前‑上方建立的右手坐标系为载体坐标系b，记东‑北‑天当地地理坐标系为导航坐标系n，则AUV在n系下的真实姿态为真实速度为AUV真实的地理坐标为p＝[L λ H]T，SINS实际解算出的姿态为速度为地理坐标为记SINS解算的地理位置建立的坐标系为计算导航坐标系n′，定义SINS姿态误差和速度误差分别为则φ、δvn的微分方程如下：φ·=Cω-1[(i-Cnn′)ω‾inn+Cnn′δωinn-Cbn′(ϵb+wgb)]]]>δv·n=[I-(Cnn′)T]Cbn′f~b+(Cnn′)TCbn′(▿b+wab)-(2δωien+δωenn)×v~sinsn-(2ω~ien+ω~enn)×δvn]]>其中，φ＝[φe φn φu]T为纵摇角、横摇角和航向角误差，δvn＝[δve δvn δvu]T为东向速度、北向速度和天向速度误差，为b系下三轴陀螺的常值误差，为b系下三轴陀螺的随机误差，为b系下三轴加速度计的常值误差，为b系下三轴加速度计的随机误差，为加速度计的实际输出，为SINS解算的速度，为计算的导航坐标系的旋转角速度；为计算的地球旋转角速度，导航坐标系相对地球的旋转角速度，为对应于的计算误差，是n系依次旋转角度φu、φe、φn得到n′系所形成的方向余弦矩阵，为b系到n′系的转移矩阵，即计算的姿态矩阵，为欧拉角微分方程的系数矩阵，其具体为：Cnn′=cosφncosφu-sinφnsinφesinφucosφnsinφu+sinφnsinφecosφu-sinφncosφe-cosφesinφucosφecosφusinφesinφncosφu+cosφnsinφesinφusinφnsinφu-cosφnsinφecosφucosφncosφe]]>上标T表示转置；所述非线性滤波状态模型建立过程如下：步骤1.2：选取SINS的欧拉平台误差角φe、φn、φu，速度误差δve、δvn，b系下三轴陀螺常值误差b系下三轴加速度计常值误差组成状态量则非线性滤波状态方程为：φ·=Cω-1[(I-Cnn′)ω^inn+Cnn′δωinn-Cbn′ϵb]+wgδv·n=[I-(Cnn′)T]Cbn′f^b+(Cnn′)TCbn′▿b-(2δωien+δωenn)×v^n-(2ω^ien+ω^enn)×δvn+waϵ·b=0▿·b=0]]>其中，取前两维状态，并将该非线性滤波状态方程简记为且w(t)[wg wa 01×3 01×2]T为零均值高斯白噪声过程；所述非线性量测模型的建立过程如下：步骤1.3：记AUV在b系下的真实速度为DVL测得AUV在b系下的实际速度为利用SINS解算的姿态矩阵将变换为以和中的东向速度和北向速度分量作为匹配信息源，则非线性滤波量测方程为：z=v~sinsn-Cbn′v~dvlb=δvn-[I-(Cnn′)T]Cbn′vdvlb+v]]>其中，取z前两维为观测值，v为零均值高斯白噪声过程，并将该非线性滤波量测方程简记为z(t)＝h(x，t)+v(t)；步骤2：以DVL的输出周期Tdvl作为滤波周期，并以Tdvl为步长对非线性滤波模型进行离散化，依据得到的离散化模型在平方根容积卡尔曼滤波的框架下进行时间更新；所述非线性滤波模型的离散化过程为：步骤2.1：离散化为xk＝xk‑1+[f(xk‑1，tk‑1)+w(tk‑1)]Tdvl并简记为xk＝f(xk‑1)‑wk‑1，z(t)＝h(x，t)+v(t)离散化为zk＝h(xk，tk)+v(tk)并简记为zk＝h(xk)+vk；步骤3：利用当前SINS和DVL输出计算的量测值减去相同时刻的量测预测值得到当前时刻的残差εk，并计算一段时间内的残差序列第1个分量和第2个分量的统计值；所述第1个残差分量计算和统计：步骤3.1.1：计算残差εk的第1个分量ε1k，即其中z1k/k‑1为zk/k‑1的第1个分量；步骤3.1.2：计算包括当前时刻残差在内的前20个时刻的残差第1个分量绝对值的平均值μ1k和标准差σ1k：μ1k=1rΣi=k-r+1k|ϵi,1k|,σ1k=1rΣi=k-r+1k(|ϵi,1k|-μ1k)2]]>其中，εi，1k表示i时刻的第1个残差分量，k代表当前时刻，r＝20；所述第2个残差分量计算和统计：步骤3.2.1：计算残差εk的第2个分量ε2k，即其中z2k/k‑1为zk/k‑1的第2个分量；步骤3.2.2：计算包括当前时刻残差在内的前20个时刻的残差第2个分量绝对值的平均值μ2k和标准差σ2k：μ2k=1rΣi=k-r+1k|ϵi,2k|,σ2k=1rΣi=k-r+1k(|ϵi,2k|-μ2k)2]]>其中，εi，2k表示i时刻的第2个残差分量，r＝20；步骤4：将μ1k和σ1k作为模糊逻辑控制器1的输入量，μ2k和σ2k作为模糊逻辑控制器2的输入量，经过模糊逻辑运算，输出精确量弱化因子l1k和l2k，并将其组成弱化因子对角阵lk＝diag[l1k l2k]；所述模糊逻辑控制器1的模糊逻辑运算过程为：步骤4.1.1：确定μ1k、σ1k和l1k的论域集并划分论域，建立μ1k、σ1k和l1k的三角形隶属度函数MF(μ1)、MF(σ1)和MF(l1)；步骤4.1.2：分别将μ1k和σ1k带入MF(μ1)和MF(σ1)计算得到对应的输入模糊集μ1k_seset和σ1k_sset；步骤4.1.3：建立Sugeno型模糊推理规则，对μ1k_set和σ1k_sets进行模糊关系合成和模糊推理合成得到输出模糊集l1k_set；步骤4.1.4：依据MF(l1)采用重心法进行解模糊化得到输出精确值l1k，其中重心法计算式如下：v0=Σk=1mvkμv(vk)Σk=1mμv(vk)]]>其中，vk是模糊集合元素，μv(vk)是元素vk的隶属度，v0是精确值；所述模糊逻辑控制器2的模糊逻辑运算过程为：步骤4.2.1：确定μ2k、σ2k和l2k的论域集并划分论域，建立μ2k、σ2k和l2k的三角形隶属度函数MF(μ2)、MF(σ2)和MF(l2)步骤4.2.2：分别将μ2k和σ2k带入MF(μ2)和MF(σ2)计算得到对应的输入模糊集μ2k_set和σ2k_set；步骤4.2.3：建立Sugeno型模糊推理规则，对μ2k_set和σ2k_set进行模糊关系合成和模糊推理合成得到输出模糊集l2k_set；步骤4.2.4：依据MF(l2)采用步骤4.1.4所用重心法进行解模糊化得到输出精确值l2k；步骤5：依据强跟踪滤波原理计算次优渐消因子λk，然后利用λk修正滤波时间更新过程，最后完成滤波量测更新；步骤6：利用当前获得的欧拉平台误差角估计值和速度估计值修正SINS解算的姿态矩阵和速度将修正之后的值作为下一次捷联解算的初值，利用当前获得的陀螺的常值误差估计值和加速度计的常值误差估计值修正下一时刻的陀螺输出和加速度计输出具体修正公式按下式计算：Cbn=C^n′nCbn′,vsinsn=v~sinsn-δv^kn,ωibb=ω~ibb-ϵ^kb,fb=f~b-▿^kb]]>若姿态精度达到要求，对准结束，否则继续递推执行步骤2至步骤6，直到对准结束。