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

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

Info

Publication number
CN103759742B
CN103759742B CN201410030336.2A CN201410030336A CN103759742B CN 103759742 B CN103759742 B CN 103759742B CN 201410030336 A CN201410030336 A CN 201410030336A CN 103759742 B CN103759742 B CN 103759742B
Authority
CN
China
Prior art keywords
phi
error
matrix
fuzzy
prime
Prior art date
Application number
CN201410030336.2A
Other languages
English (en)
Other versions
CN103759742A (zh
Inventor
徐晓苏
邹海军
刘锡祥
刘义亭
潘永飞
吴亮
Original Assignee
东南大学
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 东南大学 filed Critical 东南大学
Priority to CN201410030336.2A priority Critical patent/CN103759742B/zh
Publication of CN103759742A publication Critical patent/CN103759742A/zh
Application granted granted Critical
Publication of CN103759742B publication Critical patent/CN103759742B/zh

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/0088Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours

Abstract

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

Description

基于模糊自适应控制技术的捷联惯导非线性对准方法
技术领域
[0001] 本发明主要涉及舰载导航技术领域,尤其涉及一种基于模糊自适应控制技术的捷 联惯导非线性对准方法。
背景技术
[0002] 惯导系统在进入导航工作状态以前都要进行初始对准,捷联惯导系统SINS将惯性 传感器与载体直接固联,采用计算的数学平台代替物理平台,因此SINS的初始对准就是确 定初始时刻的姿态矩阵。初始对准误差是SINS的主要误差源,对准精度直接影响SINS的工 作性能;自主水下航行器(AUV)是一种依靠自身的自治能力来管理和控制自己的智能化无 人航行器,精确的导航定位的支持是AUV可靠、持续工作的保证,从AUV所处的自然环境和应 用环境出发,其所配置的SINS只能采取动基座对准,因此SINS动基座对准技术成为SINS的 关键技术之一。海洋环境下,AUV受到阵风、洋流和海浪等各种因素干扰,特别是在大失准角 和剧烈晃动条件下,使得建立在线性小失准角模型基础上的经典Kalman滤波方法受到制 约;而传统的EKF、UKF等非线性滤波方法具有高维条件下对准精度低,应对不确定因素能力 差等缺点,又由于GPS在水下无法使用,因此,发明具有高精度,有效应对复杂环境的多普勒 测速仪DVL辅助SINS动基座对准的非线性智能滤波方法具有重要的意义。
发明内容
[0003] 发明目的:为了克服现有技术中存在的不足,本发明提供一种提高舰载捷联惯导 系统对准精度的基于模糊自适应控制技术的捷联惯导非线性对准方法。
[0004] 技术方案:为解决上述技术问题,本发明提供的一种基于模糊自适应控制技术的 捷联惯导非线性对准方法,其步骤包括如下:
[0005] 步骤1:建立DVL辅助SINS动基座对准模型,所述对准模型包括SINS的非线性误差 模型、非线性滤波状态模型和非线性滤波量测模型;
[0006] 所述SINS非线性误差模型建立过程如下:
[0007] 步骤1.1:记AUV航行的右-前-上方建立的右手坐标系为载体坐标系b,记东-北-天 当地地理坐标系为导航坐标系n,则AUV在η系下的真实姿态为梦f,真实速度为 g <f,AUV真实的地理坐标为p[L λ H]T,SINS实际解算出的姿态为 r #'f,速度为Kf f 地理坐标为卜以名泠 f,记sins解算的 地理位置建立的坐标系为计算导航坐标系?,定义SINS姿态误差和速度误差分别为 #您#^则φ、δνη的微分方程如下:
[0010] 其中,φ = [φη φη (Κ]1为纵摇角
Figure CN103759742BD00071
、横摇角和航向角误差,δνη[δν(3 δνη δνηΓ为东 向速度、北向速度和天向速度误差,为b系下三轴陀螺的常值误差,緣;为 b系下三轴陀螺的随机误差,为b系下三轴加速度计的常值误差,< 为b 系下三轴加速度计的随机误差,、P为加速度计的实际输出,l^SSINS解算的速度,为计 算的导航坐标系的旋转角速度;藏:为计算的地球旋转角速度,导航坐标系相对地球的 旋转角速度,戌<、<^ ;为对应于的计算误差,C:i是η系依次旋转角度 Φυ、Φβ、Φη得到V系所形成的方向余弦矩阵,C::f为b系到V系的转移矩阵,即计算的姿态 矩阵,C.=为欧拉角微分方程的系数矩阵,其具体为:
Figure CN103759742BD00081
[0013] 所述非线性滤波状态模型建立过程如下:
[0014] 步骤1.2:选取SINS的欧拉平台误差角Φ。、Φη、、(K,速度误差δνθ、δνη,ΐ3系下三轴 陀螺常值误差C,b系下三轴加速度计常值误差%、g组成状态量 .、·於4堯<、' 、%,<<< K 、:]',则非线性滤波状态方程为:
Figure CN103759742BD00082
[0016] 其中,,、妒取前两维状态,并将该非线性滤波状态方程简记为紛+)+0:/故如―) 且W(t) = [Wg Wa 01Χ3 01Χ2]Τ为零均值高其if白噪声过程;
[0017] 所述非线性量测模型的建立过程如下:
[0018] 步骤1.3:记AUV在b系下的真实速度为<^:,DVL测得AUV在b系下的实际速度为笔# 利用SINS解算的姿态矩阵将变换为以1¾和?L中的东向速度和北向速度分量作为 匹配信息源,则非线性滤波量测方程为:
[0019] :眾 I - ( 知、I广f 篆魏卢
[0020] 其中,取z前两维为观测值,v为零均值高斯白噪声过程,并将该非线性滤波量测方 程简记为z (t) =h (x,t) +v⑴;
[0021] 步骤2:以DVL的输出周期Tdvl作为滤波周期,并以Tdvl为步长对非线性滤波模型 i^) =/(爲〇 4 和z (t) = h (X,t) +v⑴进行离散化,依据得到的离散化模型在平方根容 积卡尔曼滤波的框架下进行时间更新;
[0022] 所述非线性滤波模型的离散化过程为:
[0023] 步骤2 · 1:雄)=离散化为xk = xk-1+ [f (xk-1,tk-1) +w (tk-1) ] Tdvi并简记 为Xk = f (Xk-1) +Wk-1,Z (t) =h (x,t) +v (t)离散化为Zk = h (Xk,tk) +v (tk)并简记为Zk = h (Xk) + Vk;
[0024] 步骤3:利用当前SINS和DVL输出计算的量测值减去相同时刻的量测预测值得到当 前时刻的残差e k,并计算一段时间内的残差序列第1个分量和第2个分量的统计值;
[0025] 所述第1个残差分量计算和统计:
[0026] 步骤3.1.1:计算残差的第1个分量,即%。% ,其中zik/k-1为Zk/k-1的第 1个分量;
[0027] 步骤3.1.2:计算包括当前时刻残差在内的前20个时刻的残差第1个分量绝对值的 平均值yik和标准差〇ik:
Figure CN103759742BD00091
[0029] 其中,ε i, lk表示i时刻的第1个残差分量,k代表当前时刻,r = 20;
[0030] 所述第2个残差分量计算和统计:
[0031] 步骤3.2.1:计算残差第2个分量e2k,即,.其中Z2k/k-1为Zk/k-1的 第2个分量;
[0032] 步骤3.2.2:计算包括当前时刻残差在内的前20个时刻的残差第2个分量绝对值的 平均值y2k和标准差〇2k:
Figure CN103759742BD00092
[0034] 其中,ei,2k表示i时刻的第2个残差分量,r = 20;
[0035] 步骤4:将ylk和〇lk作为模糊逻辑控制器1的输入量,μ 21ί和〇2k作为模糊逻辑控制器2 的输入量,经过模糊逻辑运算,输出精确量弱化因子ldPli并将其组成弱化因子对角阵lk -diag [lik l2k];
[0036] 所述模糊逻辑控制器1的模糊逻辑运算过程为:
[0037] 步骤4.1.1:确定ylk、olk和llk的论域集并划分论域,建立y lk、olk和llk的三角形隶属 度函数MF如)、MF㈨)和MF (U ;
[0038] 步骤4 · 1 · 2 :分别将ylk和〇lk带入MF (μ!)和MF (σ!)计算得到对应的输入模糊集ylk set 和 〇lk-set;
[0039] 步骤4.1.3:建立Sugeno型模糊推理规则,对ylk_se^0lk set进行模糊关系合成和模 糊推理合成得到输出模糊集llk_set;
[0040] 步骤4.1.4:依据MF (ω采用重心法进行解模糊化得到输出精确值llk,其中重心法 计算式如下:
Figure CN103759742BD00093
[0042] 其中,Vk是模糊集合元素,μν (Vk)是元素 Vk的隶属度,v〇是精确值
[0043] 所述模糊逻辑控制器2的模糊逻辑运算过程为:
[0044] 步骤4.2.1:确定以215、〇 21{和121{的论域集并划分论域,建立以21 {、〇21{和121{的三角形隶属 度函数MF (μ2)、MF (〇2)和MF (h);
[0045] 步骤4 · 2 · 2 :分别将y2k和〇2k带入MF (μ2)和MF (σ2)计算得到对应的输入模糊集y2k set 和 〇2k-set;
[0046] 步骤4 · 2 · 3:建立Sugeno型模糊推理规则,对y2k_se4P〇2k_ set进行模糊关系合成和模 糊推理合成得到输出模糊集l2k_set;
[0047] 步骤4.2.4:依据MF (12)采用步骤4.1.4所用重心法进行解模糊化得到输出精确值 l2k〇
[0048] 步骤5 :依据强跟踪滤波原理计算次优渐消因子,然后利用修正滤波时间更新 过程,最后完成滤波量测更新;
[0049] 步骤6:利用当前获得的欧拉平台误差角估计值^和速度估计值修正SINS解算 的姿态矩阵Cf和速度.,将修正之后的值作为下一次捷联解算的初值,利用当前获得的 陀螺的常值误差估计值f和加速度计的常值误差估计值修正下一时刻的陀螺输出和 加速度计输出,具体修正公式按下式计算:
Figure CN103759742BD00101
[0051] 若姿态精度达到要求,对准结束,否则继续递推执行步骤2至步骤6,直到对准结 束。
[0052] 进一步地,所述步骤2中,据得到的离散化模型在平方根容积卡尔曼滤波的框架下 进行时间更新过程步骤为:
[0053] 步骤2.2:设置滤波状态初值和初始误差协方差阵Po,并对Po进行cholesky分解,得 到初始误差协方差阵的特征平方根So;
[0054] 步骤2.3 :利用上一时刻的Sk-i估算容积点Xi,k-i并计算传播容积点;
[0055] A; (6..} ™ :¾ i %-S 1 ™ ^
[0056] 其中,是上一时刻误差协方差阵的特征平方根,爲..s是上一时刻的状态估计值,
Figure CN103759742BD00102
ξτ表示第i个容积点,2c个容积点为 ,ei为c维的初等列向 量,C是状态量个数,即c = 10;
[0057] 步骤2.4:计算状态一步预测值和一步预测误差协方差阵特征平方根Sk/w:
Figure CN103759742BD00103
[0062] 其中,之"..;是加权中心矩阵,.$/¾是系统噪声方差阵Qk的特征平方根,qr { · }表 示对矩阵进行qr分解,B (1: c,:)表示取矩阵B的第1行至第c行形成的c X c矩阵;
[0063] 步骤2.5:计算容积点Xi.k/k-i并更新量测方程传播容积点Zi.k/k-i:
[0064] '' s i :);
[0065] 步骤2.6:计算量测预测值量测预测误差协方差阵特征平方根Swk/H:
Figure CN103759742BD00111
[0070] 其中,是加权中心矩阵,.#是系统量测方差阵Rk的特征平方根,D (1 :m,:)表 示取矩阵D的第1行至第m行形成的mXm矩阵,m是量测状态个数,即m = 2;
[0071] 步骤2 · 7:计算互协方差阵Pxz.k/k-i:
Figure CN103759742BD00112
[0074] 其中,xkk-1是加权中心矩阵。
[0075] 进一步地,所述步骤5中:
[0076] 所述计算次优渐消因子11{的过程为:
[0077] 步骤5.1.1:若让=1,&«5!%系|,即1^=¥^若1€>1,其中 0·95<ρ <0.995为遗忘因子;
Figure CN103759742BD00113
[0078] 步骤5 · 1 · 2:计算美二
[0079] 步骤5 · 1 · 3:计算4y*若λ〇,k〈 1,则Xk = 1;若λ〇,k彡 1,则Xk = λ〇,k,其中 trace ( ·)表示矩阵的迹;
Figure CN103759742BD00114
[0080] 所述修正滤波时间更新过程为:
[0081] 步骤5.2.1:利用式Μ 代替步骤2.4中的式
Figure CN103759742BD00115
[0082] 步骤5 · 2 · 2:再次执行步骤2 · 5至步骤2 · 7;
[0083] 所述滤波量测更新过程为:
[0084] 步骤5.3.1:计算滤波增益矩阵Kk,即心二f匕
[0085] 步骤5.3.2:利用前述步骤计算的变量值更新状态和误差协方差阵的特征平方根 Sk:
Figure CN103759742BD00121
[0089] 本发明利用模糊逻辑善于表达界限不清晰的定性知识与经验、推理解决常规方法 难于解决的规则型模糊信息问题特点,通过在滤波时间更新和滤波量测更新之间添加一个 专门用于动态优化弱化因子矩阵的模糊逻辑控制模块实现次优渐消因子的自适应调整。本 发明具有在大失准角和晃动基座条件下,模糊逻辑控制器利用残差所包含的舰载设备的运 动信息,通过线选取次优渐消因子,调整滤波器增益,保持对舰载设备运动状态的强跟踪能 力,满足复杂水下环境的AUV高精度对准要求。
[0090] 有益效果:本发明相对于现有技术具有以下优点:
[0091] 1)解决了大失准角、晃动基座复杂条件下的舰载捷联惯导系统对准精度下降问 题,为舰载捷联惯导系统提供高精度的初始姿态信息,保证舰载捷联惯导系统提供可靠的 导航定位信息;
[0092] 2)采用适用于高维、强非线性条件下的容积卡尔曼滤波器以及引入强跟踪滤波思 想;创造性的提出弱化因子矩阵的概念,有效区别各观测信息的差异,并引入模糊逻辑控制 技术实现次优渐消因子的在线调整,实现对舰载设备的运动状态的强跟踪,提高滤波和对 准的精度;
[0093] 3)使用DVL提供高精度可靠的速度观测信息,有助于从残差中提取更多有关于滤 波状态量的信息,提高对准精度和速度。
附图说明
[0094] 图1为本发明DVL辅助SINS动基座对准方案图;
[0095]图2为本发明四波束Janus配置DVL测速示意图;
[0096] 图3为本发明基于模糊逻辑控制技术的非线性智能滤波方法原理图;
[0097] 图4为本发明模糊逻辑控制器的结构图;
[0098] 图5为本发明AUV航行纵摇角、横摇角和航向角摇摆模拟图;
[0099]图6为本发明SINS三轴陀螺输出模拟图;
[0100]图7为本发明SINS三轴加速度计输出模拟图;
[0101] 图8为本发明DVL辅助SINS动基座对准纵摇误差图;
[0102] 图9为本发明DVL辅助SINS动基座对准横摇误差图;
[0103]图10为本发明DVL辅助SINS动基座对准航向误差图。
具体实施方式
[0104] 步骤1:建立DVL辅助SINS动基座对准模型,所述对准模型包括SINS的非线性误差 模型、非线性滤波状态模型和非线性滤波量测模型;
[0105] 所述SINS非线性误差模型建立过程如下:
[0106] 步骤1.1:记AUV航行的右-前-上方建立的右手坐标系为载体坐标系b,记东-北-天 当地地理坐标系为导航坐标系n,则AUV在η系下的真实姿态为铲,真实速度为 么*:«螬,AUV真实的地理坐标为p= [L λ H]T,SINS实际解算出的姿态为 #^|1 f #f ,速度为f ft,地理坐标为4 ,记SINS解算 的地理位置建立的坐标系为计算导航坐标系V,定义SINS姿态误差和速度误差分别为 沴您e @^则Φ、δνη的微分方程如下:
Figure CN103759742BD00131
[0109] 其中,Φ = [Φ0 φη (Κ]1为纵摇角、横摇角和航向角误差,δνη[δν(3 δνη δ^]1为东 向速度、北向速度和天向速度误差,? =¾ 4 <Ί'为13系下三轴陀螺的常值误差,< 为b系下三轴陀螺的随机误差,vtf为b系下三轴加速度计的常值误差, 为b系下三轴加速度计的随机误差,为加速度计的实际输出,f^SSINS解算的速度, 为计算的导航坐标系的旋转角速度;乾:为计算的地球旋转角速度,翁L导航坐标系相对地 球的旋转角速度,#为对应于的计算误差,Γ'是η系依次旋转 角度Φυ、Φβ、Φη得到V系所形成的方向余弦矩阵,(.f为b系到V系的转移矩阵,即计算的 姿态矩阵,(:^为欧拉角微分方程的系数矩阵,其具体为:
Figure CN103759742BD00132
[0112] 所述非线性滤波状态模型建立过程如下:
[0113] 步骤1.2:选取SINS的欧拉平台误差角Φ。、Φη、(K,速度误差S Ve、SVn,b系下三轴陀 螺常值误差<、<、系下三轴加速度计常值误差V;:、%组成状态量
Figure CN103759742BD00133
,则非线性滤波状态方程为:
Figure CN103759742BD00134
[0115] 其中,你 s、令哪前两维状态,并将该非线性滤波状态方程简记为雄》气神s 且W(t) = [Wg Wa 01Χ3 01Χ2]Τ为零均值高其if白噪声过程;
[0116] 所述非线性量测模型的建立过程如下:
[0117] 步骤1.3:记AUV在b系下的真实速度为,DVL测得AUV在b系下的实际速度为 利用SINS解算的姿态矩阵将.变换为^^*以1¾和PL.中的东向速度和北向速度分量作为 匹配信息源,则非线性滤波量测方程为:
[0118] f - ^ ''' P 'KC^' f 1?4
[0119] 其中,取Ζ前两维为观测值,V为零均值高斯白噪声过程,并将该非线性滤波量测方 程简记为z (t) =h (x,t) +v⑴;
[0120] 步骤2:以DVL的输出周期Tdvl作为滤波周期,并以Tdvl为步长对非线性滤波模型 诈}、、/〇· _}和2 (t) =h (X,t) +V⑴进行离散化,依据得到的离散化模型在平方根容 积卡尔曼滤波的框架下进行时间更新;
[0121] 所述非线性滤波模型的离散化过程为:
[0122] 步骤2 · 1:雄^ /C#:H.谈_离散化为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;
[0123] 所述滤波时间更新过程为:
[0124] 步骤2.2:设置滤波状态初值和初始误差协方差阵Po,并对Po进行cholesky分解,得 到初始误差协方差阵的特征平方根So;
[0125] 步骤2.3:利用上一时刻的31{-1估算容积点乂^- 1并计算传播容积点.<_"^
[0126] ίJ: 1 ™ ^ -; i ! s i:;:s ; ~ Ji·^·i,i s) ^ ''' ^ ^
[0127] 其中,是上一时刻误差协方差阵的特征平方根,是上一时刻的状态估计值, ξτ表示第i个容积点,2c个容积点为为c维的初等列向 量,c是状态量个数,即c = 10;
Figure CN103759742BD00141
[0128] 步骤2.4:计算状态一步预测值和一步预测误差协方差阵特征平方根Sk/H:
Figure CN103759742BD00142
[0133] 其中,名_是加权中心矩阵,.J1是系统噪声方差阵Qk的特征平方根,qr { · }表 示对矩阵进行qr分解,B (1: c,:)表示取矩阵B的第1行至第c行形成的c X c矩阵;
[0134] 步骤2.5:计算容积点Xi.k/k-i并更新量测方程传播容积点Zi.k/k-i:
[0135] ί- s ! -1 - i '' ΐ' :);
[0136] 步骤2.6:计算量测预测值量测预测误差协方差阵特征平方根Swk/H:
Figure CN103759742BD00143
Figure CN103759742BD00151
[0141] 其中,%/k-i是加权中心矩阵,^是系统量测方差阵Rk的特征平方根,D (1 :m,:)表 示取矩阵D的第1行至第m行形成的mXm矩阵,m是量测状态个数,即m = 2;
[0142] 步骤2.7:计算互协方差阵Pxz.k/k-i:
Figure CN103759742BD00152
[0145] 其中,xk,k-i是加权中心矩阵。
[0146] 步骤3:利用当前SINS和DVL输出计算的量测值减去相同时刻的量测预测值得到当 前时刻的残差e k,并计算一段时间内的残差序列第1个分量和第2个分量的统计值;
[0147] 所述第1个残差分量计算和统计:
[0148] 步骤3.1.1:计算残差είς的第1个分量eik,即%您% 其中2觀....+;为Zk/k-i的第 1个分量;
[0M9] 步骤3.1.2:计算包括当前时刻残差在内的前20个时刻的残差第1个分量绝对值的 平均值yik和标准差〇ik:
Figure CN103759742BD00153
[0151] 其中,ei,lk表示i时刻的第1个残差分量,k代表当前时刻,r = 20;
[0152] 所述第2个残差分量计算和统计:
[0153] 步骤3.2.1:计算残差的第2个分量e2k,即,_其中Z2k/k-i为Zk/k-i的 第2个分量;
[0154] 步骤3.2.2:计算包括当前时刻残差在内的前20个时刻的残差第2个分量绝对值的 平均值y2k和标准差〇2k:
Figure CN103759742BD00154
[0156] 其中,ei,2k表示i时刻的第2个残差分量,r = 20;
[0157] 步骤4:将μ11{和〇lk作为模糊逻辑控制器1的输入量,μ 21ί和〇2k作为模糊逻辑控制器2 的输入量,经过模糊逻辑运算,输出精确量弱化因子ldPli并将其组成弱化因子对角阵l k -diag [lik l2k];
[0158] 所述模糊逻辑控制器1的模糊逻辑运算过程为:
[0159] 步骤4.1.1:确定ylk、〇lk和llk的论域集并划分论域,建立y lk、〇lk和llk的三角形隶属 度函数MF如)、MF㈨)和MF (U ;
[0160] 步骤4 · 1 · 2 :分别将ylk和〇lk带入MF (μ!)和MF (σ〇计算得到对应的输入模糊集ylk set 和 〇lk-set;
[0161] 步骤4.1.3:建立Sugeno型模糊推理规则,对ylk_se4P〇 lk_set进行模糊关系合成和模 糊推理合成得到输出模糊集llk_ set;
[0162] 步骤4.1.4:依据MF (10采用重心法进行解模糊化得到输出精确值llk,其中重心法 计算式如下:
Figure CN103759742BD00161
[0164] 其中,Vk是模糊集合元素,μν(νΐ〇是元素 Vk的隶属度,VQ是精确值
[0165] 所述模糊逻辑控制器2的模糊逻辑运算过程为:
[0166] 步骤4.2.1:确定以215、〇 21{和121{的论域集并划分论域,建立以21 {、〇21{和121{的三角形隶属 度函数MF (μ2)、MF (〇2)和MF (h);
[0167] 步骤4 · 2 · 2 :分别将y2k和〇2k带入MF (μ2)和MF (σ2)计算得到对应的输入模糊集y2k set 和 〇2k-set;
[0168] 步骤4 · 2 · 3:建立Sugeno型模糊推理规则,对y2k_se4P〇2k_ set进行模糊关系合成和模 糊推理合成得到输出模糊集l2k_set;
[0169] 步骤4.2.4:依据MF (12)采用步骤4.1.4所用重心法进行解模糊化得到输出精确值 l2k〇
[0170] 步骤5:依据强跟踪滤波原理计算次优渐消因子,然后利用修正滤波时间更新 过程,最后完成滤波量测更新;
[0171] 所述计算次优渐消因子11{的过程为:
[0172] 步骤5.1.1:若k= 1,&從,即K ;若k>]
Figure CN103759742BD00162
>其中0.95彡P <0.995为遗忘因子;
[0173] 步骤5 · 1 · 2:计算岭^卜I释.,Α 輯 4«.微 Λ.綠
Figure CN103759742BD00163
[0174] 步骤 5.1.3:计算 4若X〇,k〈l,则Xk=l;若入〇,1{彡1,则\ 1{ = \〇,1{,其中 trace (·)表示矩阵的迹;
[0175] 所述修正滤波时间更新过程为:
[0176] 步骤5. 2.1:利用式
Figure CN103759742BD00164
|代替步骤2. 4中的式 |:/1用哼觀糾:揭:π?
[0177] 步骤5.2.2:再次执行步骤2.5至步骤2.7;
[0178] 所述滤波量测更新过程为:
[0179] 步骤5.3.1:计算滤波增益矩阵Kk,即《二/ ,
[0180] 步骤5.3.2:利用前述步骤计算的变量值更新状态和误差协方差阵的特征平方根 Sk:
[0181] 美為,'.卜 為薄 J
Figure CN103759742BD00171
[0184] 步骤6:利用当前获得的欧拉平台误差角估计值義和速度估计值3¾修正SINS解算 的姿态矩阵qf和速度^^,将修正之后的值作为下一次捷联解算的初值,利用当前获得的 陀螺的常值误差估计值薄和加速度计的常值误差估计值^修正下一时刻的陀螺输出AI和 加速度计输出,具体修正公式按下式计算:
Figure CN103759742BD00172
[0186] 若姿态精度达到要求,对准结束,否则继续递推执行步骤2至步骤6,直到对准结 束。
[0187] SINS的惯性测量组件安装在AUV内部,DVL安装在AUV底部,DVL辅助SINS动基座对 准原理如图1所示,四波束Janus配置的DVL测速示意图如图2所示,基于模糊逻辑控制技术 的非线性智能方法原理图如图3所示。
[0188] 以下叙述均针对水下航行器,即载体为一般AUV。
[0189] 使用如下的实例说明本发明的有益效果:
[0190] 1)舰船运动参数设置
[0191] 仿真初始时刻AUV在北炜32°,东经118°的水下10m处;AUV在海浪的激励下分别绕 纵摇轴、横摇轴和航向轴以正弦函数作三轴摇摆运动,纵摇角Θ、横摇角γ和航向角Φ的摇摆 幅值为6°、12°、10°,摇摆周期分别是6 8、88、98,初始航向角为45°,其模拟曲线图如图5所 示;同时AUV做线运动,初始东向速度和北向速度均为5m/s,0~10s为匀加速直线运动,东向 加速度和北向加速度均为〇. 5m/s2,之后为匀速直线运动,航行时间为300s;
[0192] 2)模糊逻辑控制器设计
[0193] 本发明采用SINS输出的东向速度和北向速度与DVL输出的东向速度和北向速度之 差作为观测值,因此需要设计两个模糊逻辑控制器,模糊逻辑控制器的结构如图4所示,模 糊控制器采用Sugeno型模糊推理规则,如表1所示:
[0194] 表 1
Figure CN103759742BD00173
[0196] 其语法规则为:Iftijk is···,and〇jk is…,then ljk is···;
[0197] 3)传感器参数设置
[0198] 舰载捷联惯导系统采用光纤陀螺和挠性加速度计,陀螺常值漂移为0.02°/h,陀螺 随机漂移0.01 °/h,加速度计偏置为100 X l(T6g (g为重力加速度),加速度计随机误差为50 X l〇-6g,模拟AUV的三轴陀螺输出ωχ、ωγ、ω ζ和三轴加速度计输出fx、fy、fz如图6、图7所示;所 采用的DVL测速误差为0. lm/s;
[0199] 4)仿真结果分析
[0200] 5)对准的初始失准角为10 °、10 °、10 °,使用本发明提出的基于模糊控制技术的非 线性智能滤波方法进行DVL辅助SINS动基座对准,图8、9、10为使用本发明方法完成对准的 纵摇角误差Φ χ、横摇角误差Φγ和航向角误差Φζ曲线,仿真结果表明,AUV在运动状态下,对 于存在大失准角、晃动基座及DVL速度在载体坐标系和导航坐标系之间变换而产生的噪声 时变等恶劣海洋条件下,使用本发明的非线性智能滤波方法仍然能够保证具有很高的对准 精度,满足AUV的水下导航定位需求。
[0201] 6)以上所述仅是本发明的优选实施方式,应当指出:对于本技术领域的普通技术 人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也 应视为本发明的保护范围。

Claims (3)

1. 一种基于模糊自适应控制技术的捷联惯导非线性对准方法,其特征在于:其步骤包 括如下: 步骤1:建立DVL辅助SINS动基座对准模型,所述对准模型包括SINS的非线性误差模型、 非线性滤波状态模型和非线性滤波量测模型; 所述SINS非线性误差模型建立过程如下: 步骤1.1:记AUV航行的右-前-上方建立的右手坐标系为载体坐标系b,记东-北-天当地 地理坐标系为导航坐标系n,贝1JAUV在η系下的真实姿态为#=|# y ,真实速度为 ,AUV真实的地理坐标为p= [L λ H]T,SINS实际解算出的姿态为 0二@ ,速度为謂fi,地理坐标为』彦f ,记SINS解算 的地理位置建立的坐标系为计算导航坐标系V,定义SINS姿态误差和速度误差分别为 •盘·- 1¾班一,则φ、δνη的微分方程如下:
Figure CN103759742BC00021
其中,Φ = [ΦΘ Φη Φυ]Τ为纵摇角、横摇角和航向角误差,δνη=[δνθδνηδνιι]Τ为东向 速度、北向速度和天向速度误差,铲#|:?ί < CT为b系下三轴陀螺的常值误差,^为b 系下三轴陀螺的随机误差,g; 为b系下三轴加速度计的常值误差为b 系下三轴加速度计的随机误差,、^为加速度计的实际输出,为SINS解算的速度,为计 算的导航坐标系的旋转角速度;为计算的地球旋转角速度,爾;导航坐标系相对地球的 旋转角速度,为对应于·、.的计算误差,(:Hn系依次旋转角度 得到V系所形成的方向余弦矩阵,Cf为b系到V系的转移矩阵,即计算的姿态 矩阵,(^为欧拉角微分方程的系数矩阵,其具体为:
Figure CN103759742BC00022
所述非线性滤波状态模型建立过程如下: 步骤1.2:选取SINS的欧拉平台误差角(i>e、Φη、(K,速度误差SVe、S Vn,b系下三轴陀螺常 值误差<、《:、<s.b系下三轴加速度计常值误差组成状态量心《〖為4 4多4 邊% 4 4 n,则非线性滤波状态方程为:
Figure CN103759742BC00031
其中,#沪、炉取前两维状态,并将该非线性滤波状态方程简记为雄h /!爲r;H啤h 且w(t) [Wg Wa 0lX3 0lX2]T为零均值高其if白噪声过程; 所述非线性量测模型的建立过程如下: 步骤1.3:记AUV在b系下的真实速度为&,DVL测得AUV在b系下的实际速度为,利用 SINS解算的姿态矩阵将变换为中的东向速度和北向速度分量作为匹配 信息源,则非线性滤波量测方程为:
Figure CN103759742BC00032
其中,取z前两维为观测值,v为零均值高斯白噪声过程,并将该非线性滤波量测方程简 记为z (t) =h (x,t) +v (t); 步骤2:以DVL的输出周期Tdvl作为滤波周期,并以Tdvl为步长对非线性滤波模型 雄>_'} 、、知进行离散化,依据得到的离散化模型在平方根容积 卡尔曼滤波的框架下进行时间更新; 所述非线性滤波模型的离散化过程为: 步骤2 · 1:雄> =、/(Μ、;Η. ».'{?>离散化为Xk = xk-丨+ [f (Xk-丨,tk-1) +w (tk-1) ] Tdvi并简记为xk = f (xk-1) _wk-i,z ⑴=h (x,t) +v ⑴离散化为zk=h (xk,tk) +v (tk)并简记为zk = h (xk) +vk; 步骤3:利用当前SINS和DVL输出计算的量测值减去相同时刻的量测预测值得到当前时 刻的残差4,并计算一段时间内的残差序列第1个分量和第2个分量的统计值; 所述第1个残差分量计算和统计: 步骤3.1.1:计算残差είς的第1个分量eik,8卩% =+.½ 其中zik/k-i为Zk/k-i的第1个 分量; 步骤3.1.2:计算包括当前时刻残差在内的前20个时刻的残差第1个分量绝对值的平均 值yik和标准差〇ik:
Figure CN103759742BC00033
其中,£iak表示i时刻的第1个残差分量,k代表当前时刻,r = 20; 所述第2个残差分量计算和统计: 步骤3.2.1:计算残差的第2个分量e2k,即%$ =¾. ,其中Z2k/k-i为Zk/k-i的第2个 分量; 步骤3.2.2:计算包括当前时刻残差在内的前20个时刻的残差第2个分量绝对值的平均 值y2k和标准差〇2k:
Figure CN103759742BC00041
其中,£i,2k表示i时刻的第2个残差分量,r = 20; 步骤4:将ylk和〇lk作为模糊逻辑控制器1的输入量,μ21ί和 〇2k作为模糊逻辑控制器2的输 入量,经过模糊逻辑运算,输出精确量弱化因子ldPli并将其组成弱化因子对角阵lk = diag [lik bk]; 所述模糊逻辑控制器1的模糊逻辑运算过程为: 步骤4.1.1:确定以11{、011{和111{的论域集并划分论域,建立以 11{、011{和111{的三角形隶属度函 数MF (μ!)、MF 知)和MF (h); 步骤4.1.2 :分别将ylk和〇lk带入MF (μ!)和MF (σ〇计算得到对应的输入模糊集ylk seset和 〇lk-sset; 步骤4.1.3:建立Sugeno型模糊推理规则,对yik_set和〇ik_sets进行模糊关系合成和模糊推 理合成得到输出模糊集llk_set; 步骤4.1.4:依据MF(h)采用重心法进行解模糊化得到输出精确值llk,其中重心法计算 式如下:
Figure CN103759742BC00042
其中,vk是模糊集合元素,μν(νι〇是元素 vk的隶属度,νο是精确值; 所述模糊逻辑控制器2的模糊逻辑运算过程为: 步骤4.2.1:确定y2k、〇2k和l2k的论域集并划分论域,建立y 2k、〇2k和l2k的三角形隶属度函 数MF (μ2)、MF (〇2)和MF (12) 步骤4 · 2 · 2 :分别将y2k和〇2k带入MF (μ2)和MF (σ2)计算得到对应的输入模糊集y2k set和 〇2k-set; 步骤4.2.3:建立Sugeno型模糊推理规则,对y2k_set和〇2k_set进行模糊关系合成和模糊推 理合成得到输出模糊集l2k_set; 步骤4.2.4:依据MF (12)采用步骤4.1.4所用重心法进行解模糊化得到输出精确值l2k; 步骤5:依据强跟踪滤波原理计算次优渐消因子,然后利用修正滤波时间更新过程, 最后完成滤波量测更新; 步骤6:利用当前获得的欧拉平台误差角估计值|和速度估计值奸f修正SINS解算的姿 态矩阵cy和速度将修正之后的值作为下一次捷联解算的初值,利用当前获得的陀螺 的常值误差估计值#和加速度计的常值误差估计值f丨修正下一时刻的陀螺输出#1和加速 度计输出,具体修正公式按下式计算:
Figure CN103759742BC00043
若姿态精度达到要求,对准结束,否则继续递推执行步骤2至步骤6,直到对准结束。
2.根据权利要求1所述的基于模糊自适应控制技术的捷联惯导非线性对准方法,其特 征在于:所述步骤2中,据得到的离散化模型在平方根容积卡尔曼滤波的框架下进行时间更 新过程步骤为: 步骤2.2:设置滤波状态初值尾和初始误差协方差阵?〇,并对?()进行(*〇1^1^分解,得到 初始误差协方差阵的特征平方根So; 步骤2.3:利用上一时刻的Sh估算容积点XiM并计算传播容积点不:
Figure CN103759742BC00051
其中,㊀^是上一时刻误差协方差阵的特征平方根,毛.、5是上一时刻的状态估计值,【,表 示第i个容积点,2 c个容积点为:
Figure CN103759742BC00052
、e i为c维的初等列向量, c是状态量个数,即c = 10; 步骤2.4:计算状态一步预测值和一步预测误差协方差阵特征平方根Sk/n:
Figure CN103759742BC00053
其中,名是加权中心矩阵,、/(Λ是系统噪声方差阵Qk的特征平方根,qr {· }表示对矩 阵进行qr分解,B (1: c,:)表示取矩阵B的第1行至第c行形成的c X c矩阵; 步骤2.5:计算容积点Xi.k/k-i并更新量测方程传播容积点Zi.k/H: J:.-· '' i iV ; s > ''' Ϊ ' 步骤2.6:计算量测预测值量测预测误差协方差阵特征平方根Szz .k/n:
Figure CN103759742BC00054
其中,nk/k-ι是加权中心矩阵,是系统量测方差阵Rk的特征平方根,D (1 :m,:)表示 取矩阵D的第1行至第m行形成的mXm矩阵,m是量测状态个数,即m = 2; 步骤2.7:计算互协方差阵?\2,1^-1:
Figure CN103759742BC00055
其中,Xk/k-l是加权中心矩阵。
3.根据权利要求1所述的基于模糊自适应控制技术的捷联惯导非线性对准方法,其特 征在于:所述步骤5中: 所述计算次优渐消因子11<的过程为:
Figure CN103759742BC00061
其中0.95彡P<0.995为遗忘因子;
Figure CN103759742BC00062
Figure CN103759742BC00063
s'若入0,1<〈1,则4=1;若λο,ι^Ι JljAk = A(),k,其中trace (·)表示矩阵的迹; 所述Ak修正滤波时间更新过程为:
Figure CN103759742BC00064
步骤5.2.2:再次执行步骤2.5至步骤2.7; 所述滤波量测更新过程为:
Figure CN103759742BC00065
步骤5.3.2:利用前述步骤计算的变量值更新状态和误差协方差阵的特征平方根Sk:
Figure CN103759742BC00066
CN201410030336.2A 2014-01-22 2014-01-22 基于模糊自适应控制技术的捷联惯导非线性对准方法 CN103759742B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410030336.2A CN103759742B (zh) 2014-01-22 2014-01-22 基于模糊自适应控制技术的捷联惯导非线性对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410030336.2A CN103759742B (zh) 2014-01-22 2014-01-22 基于模糊自适应控制技术的捷联惯导非线性对准方法

Publications (2)

Publication Number Publication Date
CN103759742A CN103759742A (zh) 2014-04-30
CN103759742B true CN103759742B (zh) 2017-04-05

Family

ID=50527018

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410030336.2A CN103759742B (zh) 2014-01-22 2014-01-22 基于模糊自适应控制技术的捷联惯导非线性对准方法

Country Status (1)

Country Link
CN (1) CN103759742B (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104655131B (zh) * 2015-02-06 2017-07-18 东南大学 基于istssrckf的惯性导航初始对准方法
CN104808488B (zh) * 2015-03-05 2017-07-07 长安大学 一种适用于不确定性系统的非线性度量方法
CN105893687B (zh) * 2016-04-18 2018-11-16 哈尔滨工业大学 基于遗传算法的惯导平台系统自标定试验优化设计方法
CN106199580B (zh) * 2016-07-01 2018-08-14 中国人民解放军海军航空工程学院 一种基于模糊推理系统的Singer模型改进算法
CN106885569A (zh) * 2017-02-24 2017-06-23 南京理工大学 一种强机动条件下的弹载深组合arckf滤波方法
CN107192995B (zh) * 2017-05-23 2020-11-06 西北工业大学 一种多层次信息融合的纯方位水下目标跟踪算法
CN107193009A (zh) * 2017-05-23 2017-09-22 西北工业大学 一种模糊自适应多交互模型的多uuv协同系统水下目标跟踪算法
CN107783422B (zh) * 2017-10-20 2020-10-23 西北机电工程研究所 采用捷联惯导的火炮瞄准稳定系统控制方法
CN108563210B (zh) * 2017-12-07 2020-11-13 中国航空工业集团公司西安航空计算技术研究所 一种基于微分预测的零位自动标定方法
CN108490472A (zh) * 2018-01-29 2018-09-04 哈尔滨工程大学 一种基于模糊自适应滤波的无人艇组合导航方法
CN109059912A (zh) * 2018-07-31 2018-12-21 太原理工大学 一种基于小波神经网络的gps/ins集成定位方法
CN109579873A (zh) * 2018-12-11 2019-04-05 中国航空工业集团公司北京长城计量测试技术研究所 一种基于模糊逻辑系统的环形激光器温度建模及补偿方法
CN109443355B (zh) * 2018-12-25 2020-10-27 中北大学 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法
CN109752568B (zh) * 2019-01-28 2020-12-04 南京理工大学 基于主成分分析的微电子机械系统加速度计标定方法
CN110567490A (zh) * 2019-08-29 2019-12-13 桂林电子科技大学 一种大失准角下sins初始对准方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102519460A (zh) * 2011-12-09 2012-06-27 东南大学 一种捷联惯性导航系统非线性对准方法
CN102980579A (zh) * 2012-11-15 2013-03-20 哈尔滨工程大学 一种自主水下航行器自主导航定位方法
CN103033186A (zh) * 2012-12-30 2013-04-10 东南大学 一种用于水下滑翔器的高精度组合导航定位方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7418346B2 (en) * 1997-10-22 2008-08-26 Intelligent Technologies International, Inc. Collision avoidance methods and systems
AU2003264048A1 (en) * 2002-08-09 2004-02-25 Intersense, Inc. Motion tracking system and method
US20060241859A1 (en) * 2005-04-21 2006-10-26 Microsoft Corporation Virtual earth real-time advertising

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102519460A (zh) * 2011-12-09 2012-06-27 东南大学 一种捷联惯性导航系统非线性对准方法
CN102980579A (zh) * 2012-11-15 2013-03-20 哈尔滨工程大学 一种自主水下航行器自主导航定位方法
CN103033186A (zh) * 2012-12-30 2013-04-10 东南大学 一种用于水下滑翔器的高精度组合导航定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于采样强跟踪非线性滤波理论的驾驶员眼动跟踪技术研究;张祖涛;《中国博士学位论文全文数据库信息科技辑》;20100915(第09期);66-73,93-99 *
模糊自适应强跟踪卡尔曼滤波器研究;王春柏等;《系统工程与电子技术》;20041020(第10期);1367-1372 *

Also Published As

Publication number Publication date
CN103759742A (zh) 2014-04-30

Similar Documents

Publication Publication Date Title
Hong Fuzzy logic based closed-loop strapdown attitude system for unmanned aerial vehicle (UAV)
Lugo-Cárdenas et al. Dubins path generation for a fixed wing UAV
CN101893445B (zh) 摇摆状态下低精度捷联惯导系统快速初始对准方法
CN104075715B (zh) 一种结合地形和环境特征的水下导航定位方法
CN103675861B (zh) 一种基于星载gnss多天线的卫星自主定轨方法
CN104165642B (zh) 一种用于导航系统航向角直接校正补偿方法
CN105843233B (zh) 一种基于非线性观测器的自主水下航行器运动控制方法
CN104142686B (zh) 一种卫星自主编队飞行控制方法
CN102486377B (zh) 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法
CN101082493B (zh) 一种农业机械导航的组合定位方法
Li et al. GPS/INS/Odometer integrated system using fuzzy neural network for land vehicle navigation applications
Huang et al. High accuracy navigation information estimation for inertial system using the multi-model EKF fusing adams explicit formula applied to underwater gliders
CN101608920B (zh) 一种组合式空间位姿精密动态测量装置及方法
CN103245359B (zh) 一种惯性导航系统中惯性传感器固定误差实时标定方法
CN101881619B (zh) 基于姿态测量的船用捷联惯导与天文定位方法
CN100541135C (zh) 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法
CN103528584B (zh) 基于横向地理坐标系的极区惯性导航方法
CN105676641A (zh) 针对四旋翼无人机非线性模型的基于反步和滑模控制技术的非线性鲁棒控制器的设计方法
CN102353378B (zh) 一种矢量形式信息分配系数的组合导航系统自适应联邦滤波方法
CN104848860B (zh) 一种敏捷卫星成像过程姿态机动规划方法
CN106774410A (zh) 无人机自动巡检方法和装置
CN103744098B (zh) 基于sins/dvl/gps的auv组合导航系统
CN102589552B (zh) 低成本组合导航系统的数据融合方法和装置
CN102706366B (zh) 一种基于地球自转角速率约束的sins初始对准方法
CN103487822A (zh) 北斗/多普勒雷达/惯性自主式组合导航系统及其方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant