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

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

Info

Publication number
CN102829777B
CN102829777B CN201210332022.9A CN201210332022A CN102829777B CN 102829777 B CN102829777 B CN 102829777B CN 201210332022 A CN201210332022 A CN 201210332022A CN 102829777 B CN102829777 B CN 102829777B
Authority
CN
China
Prior art keywords
information
error
navigation
sins
inertial navigation
Prior art date
Application number
CN201210332022.9A
Other languages
English (en)
Other versions
CN102829777A (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 CN201210332022.9A priority Critical patent/CN102829777B/zh
Publication of CN102829777A publication Critical patent/CN102829777A/zh
Application granted granted Critical
Publication of CN102829777B publication Critical patent/CN102829777B/zh

Links

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

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

发明内容

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

[0007] 本发明的目的通过以下技术方案予以实现:

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

[0009] -种自主式水下机器人组合导航系统的导航数据融合方法,步骤如下:

[0010] 1)对于水下组合导航系统,线性化后离线系统状态方程和观测方程如下:

[0011] Χ(1〇=Φ (k/k-l)X(k-l) + r (k-l)w(k-l)

[0012] Z(k) =H(k)X(k) +V (k)

[0013] 式中:X(k)为系统状态向量;Z(k)为量测向量;®(k/k-l)为一步转移阵; Γ (k-ι)为系统噪声驱动阵;W(k)为系统激励噪声序列;H(k)为量测阵;V (k)为量测噪声 序列,W(k)、V(k)是不相关的高斯白噪声序列,其均值、方差分别为Ε[ω 00]=0, Ε[ω (k) «T(j)]=Q(k) δ kJ;E[v(k)]=0, E[v(k)vT(j)]=R(k) δ kJ;cov[0 (k)v(j)]=0

[0014] 2 )模糊自适应卡尔曼滤波算法编排:

[0015] 设计矩阵0(1〇=〇1(1〇>0,1?(1〇=矿(1〇>0,〇(1〇为系统噪声方差阵,1?(1〇为量测噪声 方差阵,其中时间更新:

[0016] (1)状态变量估计:

[0017] X(k/k-Ι) =Φ (k/k-1)X(k-1)

[0018] (2)误差协方差估计:

[0019] P(k/k-1) =Φ (k/k-1)P (k-1) Φτ(k/k-1)+ Γ (k-1)Q(k-1) Γτ(k-1)

[0020] 测量更新:

[0021] (3)定义残差的理论方差阵为

[0022] Pr=H (k) (Φ (k/k-1) P (k-1) ΦT (k/k-1) +Q) H (k) T+R (k-1),

[0023] 以及残差的实测方差阵

Figure CN102829777BD00061

[0024]

Figure CN102829777BD00062

式子(;为对最新的N个残差向量求平均值,r为残差序列 定义为:rk=Z(k)-H(k)X (k/k-1),

[0025] 进而求得实测方差阵和理论方差阵的比值< ,其中!;(.)表示对矩阵求 迹;

[0026] (4)计算加权于量测噪声阵系数:

[0027] a bOO=FIS (q(k))

[0028] ( 5 )量测噪声阵更新:

[0029] R(k) = a b(k)R(k-l)

[0030] (6)计算卡尔曼增益阵:

[0031] K (k) =P (k/k-1) Ht (k) [H (k) P (k/k-1) Ht (k) +R (k) Γ1

[0032] (7)由测量变量z (k)更新得到状态变量估计

[0033] X (k) =X (k/k-1) +K (k) [Z (k) -H (k) X (k/k-1)]

[0034] (8)误差协方差更新:

[0035] P (k) = (I-K (k) H (k)) P (k/k-1)

[0036] (9)循环:k=k+l,转到步骤(1)

[0037] 3)模糊推理系统设计

[0038] a (k)的取值由模糊推理系统得到,模糊推理系统采用单输入单输出模式, 此时模糊推理系统的输入为每一步的残差实测方差与理论方差比值q(k),由上述 模糊滤波算法编排中可知

Figure CN102829777BD00071

,其中!;(.)表示对矩阵求迹;残差实测方差

Figure CN102829777BD00072

[0039] 残差的理论方差 Pr=H(k) (Φ (k/k_l)P (k-1) 〇T(k/k_l)+Q)H(k)T+R (k-Ι);由模 糊推理系统的输入,得到系统输出a (k),对输入、输出变量模糊化,两者的隶属度函数分别 为两个"trimf"和一个"trapmf",即两个三角形隶属度函数和一个梯形隶属度函数;采用 一个梯形隶属度函数的目的是为了避免q(k)在近似等于1的情况下,模糊推理系统仍然调 a (k)而引起不必要的滤波误差;

[0040] 定义模糊推理系统模糊规则如下:

[0041] if q (k) IessO. 9, then a (k) lessl

[0042] if q(k)more0. 9and q(k) lessl. I, then a (k) equal I

[0043] if q(k)morel. I, then a (k) more I

[0044] 模糊输出的求解,采用模糊推理系统直接输出;这样每滤波一次,模糊推理系统输 出一个a (k),对量测噪声方差阵在线调整一次。

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

附图说明

[0046] 图1为组合导航系统结构图;

[0047] 图2为SINS/DVL/磁航向仪组合导航滤波模型结构图;

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

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

[0050] 图5为FIS输出与输入的隶属度函数曲线图;

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

具体实施方式

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

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

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

[0055] 2) SINS系统的输出的速度信息与DVL输出的速度信息相减得量测量,SINS输出 的航向信息与MCP输出的航向信息相减得量测量

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

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

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

[0059] 1)捷联惯性导航系统SINS系统误差模型的建立:

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

[0061] ^ = WES^r 4^Ai^LSlNblShy.£hx£by£ bz]

[0062] 式中,δ VE、δ 乂及别是东向、北向速度误差;φ Ε、φΝ、(^分别是东向、北向、天向 失准角;δ L、δ λ分别是炜度、经度误差;Vto、V6y分别是X、y向的加速度计偏置;ε bx、 eby、ebz分别是x、y、z向的陀螺漂移。选取东北天向坐标系作为导航坐标系,载体坐标系 X轴沿水下潜器横轴指向右舷,y轴沿航行器纵轴指向前,z轴垂直于X与y轴所确定的平 面构成右手坐标系。状态方程为= ΑΜίίΒΒ +

[0063] 系统噪声矩阵?心、.、=[叫气气.ί^,4κ%.%ι.00000]

Figure CN102829777BD00091

[0069]

Figure CN102829777BD00092

为姿态转移矩阵Cnb的元素

[0070] 2)多普勒测速仪DVL误差模型的建立:

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

[0072] XDVL=[5VDx5VDy. 5KDx5KDy]

[0073] 其中多普勒测速误差δ VDx、δ VDy可用一阶马尔可夫过程近似描述,刻度系数误差 SKDx、SKlly为随机常数。

[0074]状态方程X ' DVL=F肌X肌+W肌

[0075] 其中

Figure CN102829777BD00101

1,系统噪声矩阵评肌=[^.00],^、、、1、分 别是一阶马尔可夫过程的相关时间和驱动白噪声。

[0076] 3)磁航向仪MCP误差模型的建立:

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

[0078] Xmcp - δ Ψ Mcp

[0079] 状态方程可用一阶马尔可夫过程描述之:

[0080]

Figure CN102829777BD00102

[0081] 其中δ Ψκρ为MCP的测量误差,τ Mep为相关时间参数,WKP为MCP系统噪声。

[0082] 4)由步骤1)、2)、3)中各子系统的状态方程,可得集中滤波器的状态方程:

[0083] 取状态向量为:X = [XSINS,X肌,Xmcp]

[0084] 则状态方程为X =F总X总+W总

[0085] 式中:

[0086]

Figure CN102829777BD00103

[0087] X总 _[Xsins,Xdvl,Xmcp] _] W,&=[WSINS, Wdvl, WmcpIt

[0089] 5)量测方程的建立:

[0090] a.捷联惯导SINS与多普勒测速仪DVL的量测量:

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

[0092] DVL速度输出为

[0093]

Figure CN102829777BD00104

其4

Figure CN102829777BD00105

,vE,vN分别为东向、北向真实速度;η E、n A多普勒测速白 噪声,Φ是方位角的真值。

[0094] SINS速度输出为:

[0095]

Figure CN102829777BD00106

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

Figure CN102829777BD00111

[0101] b.捷联惯导SINS与磁航向仪MCP的量测量:

[0102] 磁航向仪MCP航向输出及捷联惯导航向信息分别为:

[0103] ΦΚΡ= Φ t+δ ΦΚΡ,I^sims= Φ t+δ i])SINS,其中 1]^为真实航向值,δ φ sins为 SINS 航向测量误差。

[0104] 量测量为:

[0105]

Figure CN102829777BD00112

[0106] 式中 H2= [0 1X4 I Oix7],Him=-1

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

[0108] Z = HX+V

[0109] 式中·

Figure CN102829777BD00113

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

[0111] 以下说明介绍本发明所采用的数据融合方法:

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

[0113] a.对于水下组合导航系统,线性化后离线系统状态方程和观测方程如下:

[0114] X(k)=C> (k/k-l)X(k_l) + r (k-l)W(k-l)

[0115] Z(k) =H(k)X(k) +V (k)

[0116] 式中:X(k)为系统状态向量;Z(k)为量测向量;®(k/k-l)为一步转移阵; Γ (k-ι)为系统噪声驱动阵;W(k)为系统激励噪声序列;H(k)为量测阵;V (k)为量测噪声 序列,W(k)、V(k)是不相关的高斯白噪声序列,其均值、方差分别为Ε[ω 00]=0, Ε[ω (k) «T(j)]=Q(k) δ kJ;E[v(k)]=0, E[v(k)vT(j)]=R(k) δ kJ;cov[0 (k)v(j)]=0

[0117] b.模糊自适应卡尔曼滤波算法编排:

[0118] 设计矩阵0(1〇=〇1(1〇>0,1?(1〇=矿(1〇>0,〇(1〇为系统噪声方差阵,1?(1〇为量测噪声 方差阵

[0119] 其中

[0120] 时间更新(预测):

[0121] (1)状态变量估计:

[0122] X(k/k-l)=® (k/k-l)X(k-l)

[0123] (2)误差协方差估计:

[0124] P(k/k-l)=〇 (k/k-l)P (k-1) 〇T(k/k-l) + r (k-l)Q(k-l) rT(k-l)

[0125] 测量更新(校正):

[0126] (3)定义残差的理论方差阵为

[0127] Pr=H(k) (Φ (k/k-l)P (k-1) ®T(k/k-l)+Q)H(k)T+R (k-1),

[0128] 以及残差的实测方差阵

[0129]

Figure CN102829777BD00121

i〇=k-N+l,式子C,为对最新的N个残差向量求平均值,M由经验根据 具体情况选定,主要起平滑作用。

[0130] r 为残差序列定义为:rk=Z(k)_H(k)X (k/k-1),

[0131] 进而求得实测方差阵和理论方差阵的比值

Figure CN102829777BD00122

,其中!;(.)表示对矩阵求 迹。

[0132] (4)计算加权于量测噪声阵系数:

[0133] ab(k) =FIS (q(k))

[0134] (5)量测噪声阵更新:

[0135] R(k) = a b(k)R(k_l)

[0136] (6)计算卡尔曼增益阵:

[0137] K (k) =P (k/k-1) Ht (k) [H (k) P (k/k-1) Ht (k) +R (k) Γ1

[0138] (7)由测量变量z (k)更新得到状态变量估计

[0139] X (k) =X (k/k-1) +K (k) [Z (k) -H (k) X (k/k-1)]

[0140] (8 )误差协方差更新:

[0141] P(k) = (I-K(k)H(k))P(k/k-l)

[0142] (9)循环:k=k+l,转到(1)

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

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

[0145] c.模糊推理系统设计

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

Figure CN102829777BD00131

,其中T1X )表不对矩阵求迹。残差实测方差 i0=k_N+l,

Figure CN102829777BD00132

[0147] 残差的理论方差 Pr=H (k) (Φ (k/k-1) P (k-1) Φτ (k/k-1) +Q) H (k) T+R (k-1)。由 FIS的输入,得到系统输出a (k),对输入、输出变量模糊化,两者的隶属度函数分别为两个 "trimf"和一个"trapmf",即两个三角形隶属度函数和一个梯形隶属度函数。隶属度函数 曲线见图5。采用一个梯形隶属度函数的目的是为了避免q(k)在近似等于1的情况下,FIS 仍然调a (k)而引起不必要的滤波误差。

[0148] 定义FIS模糊规则如下:

[0149] if q (k) IessO. 9, then a (k) lessl

[0150] if q(k)more0. 9and q(k) lessl. I, then a (k) equal I

[0151] if q(k)morel. I,then a (k)morel模糊输出的求解,采用FIS直接输出。这样每 滤波一次,FIS输出一个a (k),对量测噪声方差阵在线调整一次。如图6所不为模糊推理 系统原理框图。

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

Claims (2)

1. 一种自主式水下机器人组合导航系统,由捷联惯性导航系统(1)、全球定位导航系 统(2)、多普勒测速仪(3)、磁航向仪⑷组成,所述捷联惯性导航系统⑴利用陀螺仪和 加速度计计算出相对地球沿地理坐标系的速度信息、位置信息和姿态信息;所述全球定位 导航系统(2)获取自主式水下机器人的初始绝对位置信息和速度信息;所述多普勒测速仪 (3)计算出速度信息;所述磁航向仪⑷计算出航向信息;所述全球定位导航系统⑵对 捷联惯性导航系统(1)定时重调,系统对捷联惯导系统(1)输出的速度信息、位置信息和 姿态信息,多普勒测速仪(3)所计算的速度信息,磁航向仪(4)所计算的航向信息进行数 据融合,得到组合导航信息;其特征在于,系统导航方案分水下和水上部分,水下部分采用 SINS/DVL/MCP组合导航的形式进行;水上部分采用GPS重调惯导方式,在GPS工作期间,惯 导显示的是GPS的位置和速度,在GPS停止期间,惯导在原显示的基础上变化,即GPS停止 工作瞬间的位置和速度作为惯导系统的初值,然后再根据重调得到的SINS信息与DVL、MCP 的进行信息融合,通过下列步骤实现: 1) 捷联惯性导航系统(1)系统误差模型的建立包括姿态误差、速度误差的模型;多普 勒测速仪(3)误差模型的建立即速度误差的模型;磁航向仪(4)误差模型的建立即磁航向 误差模型,列出各自的状态方程; 2. SINS系统的输出的速度信息与DVL输出的速度信息相减得量测量,SINS输出的航向 信息与MCP输出的航向信息相减得量测量; 3) 由步骤1)、2),设计出集中滤波器的状态方程、量测方程,通过自适应模糊卡尔曼滤 波器估计误差量,一方面对主系统SINS进行反馈校正,一方面输出得到组合修正的导航信 息; 4) 通过定时上浮利用GPS重调SINS,运用重调后的SINS输出信息再次进行步骤1)、 2),设计出集中滤波器的状态方程、量测方程,通过自适应模糊卡尔曼滤波器估计误差量, 一方面对主系统进行更精确的反馈校正,一方面输出得到更为精确的组合导航信息。
2. 如权利要求1所述的自主式水下机器人组合导航系统的导航数据融合方法,其特征 在于,该方法步骤如下: 1) 对于水下组合导航系统,线性化后离线系统状态方程和观测方程如下: X(k) = 〇 (k/k-l)X(k-l) +r(k-l)ff(k-l) Z(k) =H(k)X(k)+V(k) 式中:x(k)为系统状态向量;z(k)为量测向量;〇 (k/k-i)为一步转移阵;r(k-1)为 系统噪声驱动阵;w(k)为系统激励噪声序列;H(k)为量测阵;V(k)为量测噪声序列,W(k)、 V(k)是不相关的高斯白噪声序列,其均值、方差分别为E[«(k)] =0,E[«(k)«T(j)]= Q(k) 8kJ;E[v(k)] = 0,E[v(k)vT(j)] =R(k) 8kJ;cov[« (k)v(j)] = 0 ; 其中Skj是kronecker函数,即:
Figure CN102829777BC00021
2) 模糊自适应卡尔曼滤波算法编排: 设计矩阵Q(k) =QT(k) >0,R(k) =RT(k) >0,Q(k)为系统噪声方差阵,R(k)为量测 噪声方差阵,其中时间更新: (1) 状态变量估计: X(k/k-l) = 〇 (k/k-l)X(k-l) (2) 误差协方差估计: P(k/k-l) = 〇 (k/k-l)P(k-l) 〇T(k/k-l) +r(k-l)Q(k-l)rT(k-l) 测量更新: (3) 定义残差的理论方差阵为 Pr=H(k) (〇 (k/k-l)P(k-l) 〇T(k/k-l)+Q)H(k)T+R(k-l), 以及残差的实测方差阵
Figure CN102829777BC00031
、=k-N+1,式子Q为对最新的N个残差向量求平均值,r为残差序列定 义为:rk=Z(k)-H(k)X(k/k-l), 进而求得实测方差阵和理论方差阵的比伯
Figure CN102829777BC00032
,其中!;(.)表示对矩阵求迹; (4) 计算加权于量测噪声阵系数: ab(k) =FIS(q(k)) (5) 量测噪声阵更新: R(k) =ab(k)R(k-l) (6) 计算卡尔曼增益阵: K(k) =P(k/k-1)Ht (k)[H(k)P(k/k-1)HT (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)模糊推理系统设计 a(k)的取值由模糊推理系统得到,模糊推理系统采用单输入单输出模式,此时模糊推 理系统的输入为每一步的残差实测方差与理论方差比值q(k),由上述模糊滤波算法编排中 可知
Figure CN102829777BC00033
,其中TV(.)表不对矩阵求迹;残差实测方差
Figure CN102829777BC00034
,;[(!=k_N+l, 残差的理论方差已=H(k) (〇(k/k-l)P(k-l) 〇T(k/k-l)+Q)H(k)T+R(k-l);由模糊推 理系统的输入,得到系统输出a(k),对输入、输出变量模糊化,两者的隶属度函数分别为 两个"trimf"和一个"trapmf",即两个三角形隶属度函数和一个梯形隶属度函数;采用一 个梯形隶属度函数的目的是为了避免q(k)在近似等于1的情况下,模糊推理系统仍然调 a(k)而引起不必要的滤波误差; 定义模糊推理系统模糊规则如下:ifq(k)less0• 9,thena(k)lessl ifq(k)moreO. 9andq(k)lessl. 1,thena(k)equal1 ifq(k)morel. 1,thena(k)more1 模糊输出的求解,采用模糊推理系统直接输出;这样每滤波一次,模糊推理系统输出一 个a(k),对量测噪声方差阵在线调整一次。
CN201210332022.9A 2012-09-10 2012-09-10 自主式水下机器人组合导航系统及方法 CN102829777B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201210332022.9A CN102829777B (zh) 2012-09-10 2012-09-10 自主式水下机器人组合导航系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201210332022.9A CN102829777B (zh) 2012-09-10 2012-09-10 自主式水下机器人组合导航系统及方法

Publications (2)

Publication Number Publication Date
CN102829777A CN102829777A (zh) 2012-12-19
CN102829777B true CN102829777B (zh) 2015-09-16

Family

ID=47332995

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201210332022.9A CN102829777B (zh) 2012-09-10 2012-09-10 自主式水下机器人组合导航系统及方法

Country Status (1)

Country Link
CN (1) CN102829777B (zh)

Families Citing this family (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103047983B (zh) * 2013-01-14 2016-03-09 哈尔滨工程大学 水下机器人的面地形匹配导航方法
CN103292812A (zh) * 2013-05-10 2013-09-11 哈尔滨工程大学 一种微惯性sins/gps组合导航系统的自适应滤波方法
CN103278163A (zh) * 2013-05-24 2013-09-04 哈尔滨工程大学 一种基于非线性模型的sins/dvl组合导航方法
CN104613979A (zh) * 2013-11-05 2015-05-13 中国科学院沈阳自动化研究所 一种用于标定auv自主导航传感器参数的装置和方法
CN104678929A (zh) * 2013-11-30 2015-06-03 中国科学院沈阳自动化研究所 一种用于自主水下机器人的自动驾驶系统及方法
CN103697887B (zh) * 2013-12-05 2017-03-01 东南大学 一种基于捷联惯导系统和多普勒计程仪的优化导航方法
CN103744100A (zh) * 2014-01-07 2014-04-23 北京航空航天大学 一种基于卫星导航与惯性导航的组合导航方法
CN103744098B (zh) * 2014-01-23 2017-03-15 东南大学 基于sins/dvl/gps的auv组合导航系统
CN104197927B (zh) * 2014-08-20 2017-06-23 江苏科技大学 水下结构检测机器人实时导航系统及方法
CN104316045B (zh) * 2014-11-06 2017-06-16 东南大学 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
CN104330084B (zh) * 2014-11-13 2017-06-16 东南大学 一种水下航行器用神经网络辅助组合导航方法
WO2017095539A1 (en) * 2015-11-30 2017-06-08 Raytheon Company Navigation system for autonomous underwater vehicle based on coherence map
CN105676855B (zh) * 2016-01-29 2018-06-19 中国船舶重工集团公司第七一〇研究所 一种远程自航水雷近水面的航行姿态校准系统和方法
CN105717928B (zh) * 2016-04-26 2018-03-30 北京进化者机器人科技有限公司 一种基于视觉的机器人导航过门方法
CN106597510B (zh) * 2016-05-24 2019-04-09 上海铸天智能科技有限公司 基于模糊判断算法的多旋翼无人机位置数据融合滤波方法
CN106094819B (zh) * 2016-06-17 2019-02-26 江苏科技大学 水下机器人控制系统及基于声纳图像目标识别的航向控制方法
CN106199652A (zh) * 2016-06-24 2016-12-07 南京理工大学 一种gps接收机的自适应矢量跟踪方法
CN106094829B (zh) * 2016-07-11 2018-10-30 中国农业大学 一种自主式海参捕捞机器人系统及方法
JP2018084445A (ja) * 2016-11-22 2018-05-31 株式会社エス・イー・エイ 水中音響測位システム
CN108519620B (zh) * 2018-07-11 2020-06-05 哈尔滨工程大学 一种可自主布放回收的海底地震检波航行器
CN108955678B (zh) * 2018-08-03 2019-08-20 国家深海基地管理中心 一种深海运载器通信定位导航授时一体化方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1916567A (zh) * 2006-09-04 2007-02-21 南京航空航天大学 基于自适应闭环h∞滤波器的对北斗双星/捷联惯导组合导航系统进行修正的方法
CN101261130A (zh) * 2008-04-15 2008-09-10 哈尔滨工程大学 一种船用光纤捷联惯导系统传递对准精度评估方法
TW201024677A (en) * 2008-12-18 2010-07-01 Mingchi Inst Of Technology A method and system for integrating GPS/INS navigation using Kalman filter
CN102052924A (zh) * 2010-11-25 2011-05-11 哈尔滨工程大学 一种小型水下机器人组合导航定位方法
CN102095424A (zh) * 2010-12-06 2011-06-15 国营红峰机械厂 一种适合车载光纤航姿系统的姿态测量方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7643939B2 (en) * 2006-03-08 2010-01-05 Honeywell International Inc. Methods and systems for implementing an iterated extended Kalman filter within a navigation system
JP4941199B2 (ja) * 2007-09-25 2012-05-30 ヤマハ株式会社 ナビゲーション装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1916567A (zh) * 2006-09-04 2007-02-21 南京航空航天大学 基于自适应闭环h∞滤波器的对北斗双星/捷联惯导组合导航系统进行修正的方法
CN101261130A (zh) * 2008-04-15 2008-09-10 哈尔滨工程大学 一种船用光纤捷联惯导系统传递对准精度评估方法
TW201024677A (en) * 2008-12-18 2010-07-01 Mingchi Inst Of Technology A method and system for integrating GPS/INS navigation using Kalman filter
CN102052924A (zh) * 2010-11-25 2011-05-11 哈尔滨工程大学 一种小型水下机器人组合导航定位方法
CN102095424A (zh) * 2010-12-06 2011-06-15 国营红峰机械厂 一种适合车载光纤航姿系统的姿态测量方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
GPS/SINS 组合导航系统中模糊自适应卡尔曼滤波算法研究;胡凡;《煤炭技术》;20100831;第29卷(第8期);142-144 *
GPS/SINS组合导航系统研究;吴小兰;《中国优秀硕士学位论文数据库工程科技辑》;20071115;正文第34页第1段-57页第6段 *
INS /GPS组合导航系统模糊卡尔曼滤波算法研究;姚文国等;《弹箭与制导学报》;20070331;第27卷(第3期);正文第6页第1-3段 *
基于模糊自适应强跟踪滤波的惯性/地磁组合导航方法;刘等睿;《中国惯性技术学报》;20110630;第19卷(第3期);329-334 *
张建会.SINS/DVL/GPS组合导航在远程AUV上的应用研究.《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》.2006,正文第12页第3段-第53页第4段. *

Also Published As

Publication number Publication date
CN102829777A (zh) 2012-12-19

Similar Documents

Publication Publication Date Title
EP3350628B1 (en) Measurement of magnetic field gradients
EP3350543B1 (en) Magnetic field gradient navigation aid
Leonard et al. Autonomous underwater vehicle navigation
Webster et al. Advances in single-beacon one-way-travel-time acoustic navigation for underwater vehicles
Noureldin et al. Fundamentals of inertial navigation, satellite-based positioning and their integration
CN103245360B (zh) 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
US9372255B2 (en) Determining a position of a submersible vehicle within a body of water
Fang et al. Predictive iterated Kalman filter for INS/GPS integration and its application to SAR motion compensation
CN104729506B (zh) 一种视觉信息辅助的无人机自主导航定位方法
AU2011258700B2 (en) Determining spatial orientation information of a body from multiple electromagnetic signals
Whitcomb et al. Advances in Doppler-based navigation of underwater robotic vehicles
CN102401658B (zh) 用于计算垂直位置的系统和方法
US6459990B1 (en) Self-contained positioning method and system thereof for water and land vehicles
US7228230B2 (en) System for autonomous vehicle navigation with carrier phase DGPS and laser-scanner augmentation
CN102519450B (zh) 一种用于水下滑翔器的组合导航装置及方法
Lee et al. Simulation of an inertial acoustic navigation system with range aiding for an autonomous underwater vehicle
US6860023B2 (en) Methods and apparatus for automatic magnetic compensation
CN103630137B (zh) 一种用于导航系统的姿态及航向角的校正方法
Eustice et al. Recent advances in synchronous-clock one-way-travel-time acoustic navigation
Rigby et al. Towards geo-referenced AUV navigation through fusion of USBL and DVL measurements
Georgy et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation
Hegrenas et al. Model-aided inertial navigation for underwater vehicles
CN102506857B (zh) 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法
McEwen et al. Performance of an AUV navigation system at Arctic latitudes
Wu et al. Velocity/position integration formula part I: Application to in-flight coarse alignment

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20150916

Termination date: 20180910