一种水下航行器组合导航系统研究_张福斌

图 1 仿真模型结构框图
利用具有定向功能的 GPS 接收机对陀螺罗 经和多普勒速度仪组合导航系统进行校准, 主要 分为以下 3种情况: 水下航行器自航发射过程中, 航行器位置的初始化及陀螺罗经随机常值误差修 正; 水下航行器执行任务过程中, 航行器位置的校 准及陀螺罗经误差的修正; 水下航行器回收过程 中, 航行器位置的校准。
在 k 时刻的位置及航向测量误差, 则量测方程
DU( k )
DUD ( k ) - DUG ( k )
Z ( k ) = DK( k ) = DKD ( k ) - DKG ( k )
DW( k )
DWD ( k ) - DWG ( k )
= H ( k)X (k) + V(k)
( 6)
式中: H ( k )为量测矩阵, 其非零元为 H ( 1, 1) =
多数水下航行器导航的需要, 但其价格较贵, 且使 用需要进行初始对准。从低成本角度考虑, 本文 研究了一种采用陀螺罗经与多普勒速度仪组合, 加 GPS间歇校正的导航系统, 实现了远程自主水 下航行器在浅水条件下的较高精度导航定位。
1 组合导航方案
由于 GPS信号在水下很快衰减, 所以远程自 主航行器在水下执行任务时无法利用其信息。航 行器采用陀螺罗经测量航向, 姿态传感器测量姿 态, 多普勒速度仪测量航行器相对于海底的速度 ( 消除了海流对导航定位精度的不利影响 ) , 航行 深度用高精度压力传感器测量, 通过航位推算进 行水下自主导航。
张福斌, 等: 一种水下航行器组合导航系统研究
4 仿真验证
仿真模型主要包括水下航行器航迹发生器、 陀 螺罗 经模 拟器 、多普 勒速 度仪 模拟 器、GPS 模 拟 器、UKF 滤波器, 其中航迹 发生器采用闭 环水下 航行器控制系统数学仿真模型, 其结构见图 1。
果如图 3~ 图 5所示, 从图中可以看出, 陀螺罗经 的误差值与陀螺罗经误差滤波估计值基本吻合, 陀螺罗经误差滤波估计值的均值为 2b, 即陀螺罗 经的随机常值误差为 2b, 与仿真条件吻合。
G D
=
100
( 5)
010
001
FD 的非零元
FD ( 1, 3) = Q1# co s W, FD ( 1, 4) = - Q1# sin W,
FD ( 1, 5) = - Q1# ( Vx s in W + Vz cos W),
FD ( 2, 3) = Q2# s in W, FD ( 2, 4) = Q2# co s W,
FD ( 2, 5) = Q2# ( Vx co s W - Vz s in W),
FD ( 3, 3)
=-
1 Sx
,
FD ( 4, 4) = -
1 Sz
。
式中, Q1, Q2 分别为子午椭圆和纬线圆的曲率 [ 4] 。
可以看出, 系统的状态方程是非线性的。
2. 2 量测方程
为了提高校准的效果, 采用的 GPS接收机为
第 15卷第 1期 2007年 02月
鱼雷技术 TORPEDO TECHNO LOGY
V o.l 15 N o. 1 Feb. 2007
一种水下航行器组合导航系统研究
张福斌, 高 剑, 崔荣鑫
( 西北工业大学 航海学院, 陕西 西安 710072)
摘 要: 设计了一种采用陀螺罗经和多普勒速度仪组合加 G PS间歇校正的水下航行器组合导航系统, 建立 了该组合导
图 4 经度方向绝对误差 图 5 陀螺罗经误差值、误差滤波估计值
1, H ( 2, 2) = 1, H ( 5, 3) = 1。
3 UKF滤波
过去几十年中, EKF 是应用最多的非线性系 统滤波算法, 但是其性能依赖于局部非线性强度。 由于 EKF 在线性化时需要用到 Jacob ian矩阵, 因 此有时导致了实现上的困难。当违背局部线性假
设, T ay lor展开式中被忽略的高阶项带来较大的 误差时, EKF 算法还可能引起滤波不稳定。
为了简化, 考虑 3种情况校准方法统一为: 航 行器在近水面条件下, GPS接收机收到第一个有 效数据后, 把该值赋给陀螺罗经和多普勒速度仪 组合导航系统, 作为下一时刻航位推算的初值; 启 动 UKF滤波器, 并将滤波器估计出的航行器位置 的纬度、经度误差反馈回航位推算模块, 进行位置 校正; 陀螺罗经的随机常值误差通过对滤波器估 计出的航向误差在滤波时间内求均值得到, 在校 准过程结束后, 对陀螺罗经误差进行一次性校正。
水下导航定位技术对水下航行器完成预定的 使命意义重大, 是水下航行器技术发展与应用的 瓶颈问题。水下精密导航系统一般由精度较高的 捷联惯导系统和多普勒速度仪组成 [ 1- 3] , 全球定 位系统 ( GPS) 间歇辅助。精度较高的捷联惯导系 统, 如文献 [ 1] 的惯性导航系统位置精度为 0. 25 n m ile, 速度精度为 ? 0. 61 m / s, 虽然可以满足大
的状态向量
XD = [ DK( t) DU( t) DVx ( t) DVz ( t) DWD ( t) ]T
( 3)
陀螺罗经和多普勒速度仪组合导航系统的状态方
程
XD = FD XD + GD wD
( 4)
式中, 系统噪声向量
wD = [ wx wz w W ] T
20
系统噪声分配矩阵
000
000
UKF滤波算法, 由 于对均值、协方差和 的均 值、协方差的计算精度比 EKF 高, 且的均值和协 方差的计算不像 EKF 那样需要求出非线性函数 的 Jacob ian 矩阵, 从而容易 实现, 因此正在 取代 EKF 成为非线性系统滤波的主要算法, 组合导航 系统的 UKF 滤波算法参考文献 [ 6] 。
( RM S) ; 7) GPS的定位精度为 1 m 95% ( DGPS); 8) UK F 滤波参数设为 A= 0. 001, B= 2, k = 0; 9) 航位推算周期为 0. 1 s, 滤波周期为 0. 2 s。 水下航行器水平面轨迹如图 2所示。仿真结
图 2 水下航行器仿真轨迹 图 3纬度 方向绝对误差图
2. 1 状态方程 导航坐标系采用北天东坐标系 [ 6 ] ; 载体坐标
系 ox 轴沿航行器纵轴并 指向前方, oy 轴 垂直于 ox 轴并指向上方, oz 轴垂直于 ox 轴与 oy 轴所确 定的平面, 构成右手系。
陀螺罗经的误差模型主要考虑 2种分量, 逐 次启动漂移 ( 陀螺罗经的安装误差可以看作是该 误差的一部分 ) 和快变漂移。陀螺罗经的误差模 型简化为
关键词: 水下航行器; 组合导航系统; 无迹卡尔曼滤波; 误差估计
中图分类号: T J765. 23
文献标识码: A
文章编 号: 1673-1948( 2007) 01-0019-03
An Integrated N avigation System for Underwater V ehicles
ZHANG Fu-b in, GAO J ian, CUI Rong-x in
( Co llege ofM a rine Eng inee ring, N or thwestern Po ly tech ica lU n iversity, X ican 710072, Ch ina)
Abstrac t: A n integrated nav igation sy stem for underwa ter veh ic les is des igned, wh ich integrates gy rocom pass, D oppler and GPS. T he unscented K alman filter ( UK F) m ode l for the system is bu ilt up, and a nume rica l sim ulation is perfo rm ed w ith the softw are M a tlab. T he simu la tion resu lts show tha tG PS can im plem en t the h igh accuracy pos ition correction, and can estim ate the stochastic error o f gy rocompass by rule and line. T he system can m eet the requ irem ents o f underw ate r vehic les for nav igation and position. K ey word s: K eywords: underw ater veh icle; in teg ra ted nav ig ation system; unscented K alm an filte r( UK F); error estim a tion
航系统的无迹卡尔曼 滤波模型, 并利用 M ATLA B软 件对其 进行了 数学仿真 验证。从 仿真结 果看出, 陀 螺罗经 的误差
值与陀螺罗经误差滤波估计值 基本吻合, 这表明, GPS间歇校正的引入可以实现航行器位置高精 度校准和 陀螺罗经随
机常值误差的准确估 计, 该组 合导航系统能够满足一般水下航行器的导航定位的需要。
DWD = DWb + w W
#
( 1)
DWb = 0
式中: DWb 为随机常值误差; w W 为白噪声过程。 多普勒速度仪的测速精度较高, 文献 [ 1] 中,
其精确度为测量速度的 0. 2% , 水下航行器在近
水面利用 GPS进行校准时, 多普勒速度仪的测速
误差用 1阶马尔柯夫过程描述
#
DVx
=-
根据设计的仿真模型, 利用 MATLAB 软件对 该组合导航系统进行了数学仿真。仿真条件为: