CN104457754B - 一种基于sins/lbl紧组合的auv水下导航定位方法 - Google Patents
一种基于sins/lbl紧组合的auv水下导航定位方法 Download PDFInfo
- Publication number
- CN104457754B CN104457754B CN201410796735.XA CN201410796735A CN104457754B CN 104457754 B CN104457754 B CN 104457754B CN 201410796735 A CN201410796735 A CN 201410796735A CN 104457754 B CN104457754 B CN 104457754B
- Authority
- CN
- China
- Prior art keywords
- sins
- lbl
- delta
- auv
- difference
- Prior art date
- Legal status (The legal status 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 status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 22
- 238000005259 measurement Methods 0.000 claims abstract description 29
- 238000012545 processing Methods 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 35
- 238000012937 correction Methods 0.000 claims description 13
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000012546 transfer Methods 0.000 claims description 10
- 238000001914 filtration Methods 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 6
- 230000007774 longterm Effects 0.000 claims description 5
- 230000000694 effects Effects 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 3
- 238000005516 engineering process Methods 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000000827 velocimetry Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明提供了一种基于SINS/LBL紧组合的AUV水下导航定位方法,其特征在于:由安装在AUV上的捷联惯性导航系统SINS、布放在海底的长基线水声定位系统LBL和数据处理单元三大部分组成,具体步骤为:首先,对IMU数据捷联解算获得AUV位置等信息,并将解算的位置信息用地球直角坐标表示;其次,根据SINS提供的AUV位置信息和水听器基阵位置坐标推算SINS斜距差;再次,根据LBL的定位特点建立LBL斜距差模型,将SINS斜距差和LBL斜距差的差值作为卡尔曼滤波器的观测量,经滤波估计补偿校正SINS的导航定位信息。该方法解决了SINS系统误差随时间积累的问题,保证了AUV在水下长期自主导航定位的精度,同时避免了GPS及其他无线电定位系统的使用,提高了AUV水下作业效率。
Description
技术领域
本发明主要涉及AUV水下导航技术领域,尤其涉及一种基于SINS/LBL紧组合的AUV水下导航定位方法,特别适用于水下自主航行器AUV的跟踪定位。
背景技术
AUV(Autonomous Underwater Vehicle,自主式水下航行器)是一种可以完成水下探测、攻击、运载、打捞等多种功能的水下工具,因其活动范围广、体积小、重量轻、隐蔽性高等特点,现已成为国内外军事海洋技术研究的一个重要方向。
AUV水下高精度自主导航和定位跟踪技术是完成其水下作业的前提和关键。在现有的定位技术中,SINS(Strapdown Inertial Navigation Systems,捷联式惯性导航系统)因其具有隐蔽性强、自主性、抗干扰、数据更新频率高、且在短时间内具有较高精度等特点,因而成为AUV水下自主导航定位的首选定位方法。目前,尽管捷联惯性导航技术的发展已日趋成熟,其导航定位误差随时间积累发散的这一动态特性却未改变,在远程、长期航行及武器发射等高精度导航时还不能完全满足要求。组合导航技术的出现为这一问题的解决提供了一种有效途径。
LBL(Long Base Line,长基线)水声定位系统是由安装在海底的基线长度为几千米的应答器基阵和安装在载体上的问答机组成,其定位原理是利用载体上的问答机与海底应答器阵之间的距离信息来求解AUV位置。LBL因其作用范围广、定位精度高已广泛应用于水下潜器。
近年来,应用于AUV的水下自主导航技术主要以SINS与DVL(Doppler VelocityLog,多普勒测速仪)的组合导航为主,辅以水面GPS(Global Positioning System,全球定位系统)修正。在若干次实验中取得了良好的导航精度,但航程相对较短,对于DVL,当声纳传感器远离海底时测量速度精度很差,仅仅对AUV贴近海底时精度较好,而对于GPS,AUV需中断潜行,浮到水面才能利用GPS信息,这在深海的情况下将浪费大量的时间与能源,严重影响AUV的水下作业效率。
发明内容
针对现有AUV水下导航精度的问题,本发明提供了一种基于SINS/LBL紧组合的AUV水下导航定位方法。
本发明的目的可以通过以下技术方案实现,具体为:
(1)捷联惯性导航系统SINS(1)通过捷联解算得到相应的包括AUV的位置信息的导航信息,解算的位置信息用地球大地坐标PSINS(LS,λS,hS)表示,并将PSINS(LS,λS,hS)转化为用地球直角坐标PSINS(xS,yS,zS)表示;
(2)SINS两两基元与目标斜距差推算模块(3)根据SINS提供的AUV位置信息PSINS(xS,yS,zS)和水听器基阵位置Pi(xi,yi,zi)推算SINS斜距差ρSINS;
(3)SINS/LBL紧组合模块(4)根据长基线水声定位系统LBL(2)的定位特点建立LBL斜距差模型,将SINS斜距差ρSINS和LBL水听器i(i=1,2,3)与AUV之间的斜距与水听器0与AUV之间的斜距之差ρLBL的差值作为外部观测信息输入到卡尔曼滤波器进行滤波;
(4)校正模块(5)根据SINS/LBL紧组合模块(4)的卡尔曼滤波结果对SINS(1)进行校正,最终得到精确的AUV位置信息PAUV。
所述的SINS两两基元与目标斜距差推算模块(3)计算SINS斜距差的方法如下:
(1)根据长基线水声定位系统LBL中水听器位置Pi(xi,yi,zi)和SINS解算AUV位置PSINS(xs,ys,zs)计算得到水听器i(i=1,2,3)与AUV之间的斜距与水听器0与AUV之间的斜距之差
(2)将ρSINSi利用泰勒级数线性化。设AUV真实位置为PAUV(x,y,z),(δx,δy,δz)为SINS解算AUV位置的误差,则xS=x+δx,yS=y+δy,zS=z+δz。将ρSINSi泰勒级数展开取前两项得:
设
同理
其中,Gij(i=0,1,2,3;j=x,y,z)为已知量,可由SINS解算的概略位置PSINS(xS,yS,zS)和水底应答器阵基元的位置Pi(xi,yi,zi)计算得到,由于SINS解算的概略位置PSINS(xS,yS,zS)可能有较大误差,这样在进行方程线性化时略去高阶项会引起线性误差,可以利用迭代法解算,即在第一次解之后,用它作为近似值再重新计算。
设:eix=Gix-G0x,eiy=Giy-G0y,eiz=Giz-G0z,i=1,2,3
于是:
ρSINSi=Ri-R0+(Gix-G0x)δx+(Giy-G0y)δy+(Giz-G0z)δz
=Ri-R0+eixδx+eiyδy+eizδz
所述的SINS/LBL紧组合模块(4)的具体实现步骤如下:
(1)建立LBL斜距差模型
由于时延差测量、声传播的多途径效应等将引起斜距差测量有误差,为简化模型,可认为斜距差误差是由常值偏置和随机噪声组成,则LBL水听器i(i=1,2,3)与AUV的斜距与水听器0与AUV的斜距之差可表示为:
式中,ΔRmeas为LBL水听器i(i=1,2,3)与AUV的斜距与水听器0与AUV的斜距之差,ΔR为斜距差真值,δR=[δR1 δR2 δR3]T为随机常值,νδR(t)~N(0,QΔR)为高斯白噪声。
(2)建立SINS/LBL紧组合状态方程
SINS/LBL紧组合状态方程描述为:
其中:XSINS为SINS的状态向量,XLBL为LBL的状态向量,FSINS为SINS的转移矩阵,FLBL为LBL的转移矩阵,WSINS为SINS的系统噪声向量,WLBL为LBL的系统噪声向量,F为紧组合系统转移矩阵,X为紧组合系统状态向量,W为紧组合系统噪声向量。
根据捷联惯性导航系统长期工作时的误差特点,选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量:
XSINS=[δVE δVN δVU φE φN φU δL δL δh ▽bx ▽by ▽bz εbx εby εbz]T
式中,δVE、δVN、δVU分别是捷联东向、北向、天向的速度误差,分别是捷联东向、北向、天向的失准角,δL、δλ、δh分别是捷联纬度、经度、高度误差,三个位置误差由地球坐标系描述,▽bx、▽by、▽bz是捷联加表三个轴向的偏置误差,εbx、εby、εbz是捷联陀螺的三个轴向漂移。
XLBL=[δR1 δR2 δR3]T
式中,δR1、δR2、δR3分别为LBL水听器i(i=1,2,3)与AUV的斜距与水听器0与AUV的斜距之差的随机常值漂移。
系统噪声阵
WLBL=[0 0 0]T
系统状态转移矩阵
式中,
其中:Fij为F9×9的元素
RN为参考椭球体子午面內的曲率半径,RN=Re(1-2e+3e sin2 L)
RE为垂直子午面內的曲率半径,RE=Re(1+e sin2 L)
其中:Re为参考椭球体的长轴半径;e为椭球的椭圆度。
F37=-2ωie cos LVE
F57=-ωie sin L
Cij为姿态转移矩阵的元素
FLBL=03×3
(3)建立SINS/LBL紧组合量测方程。
紧组合系统采用SINS推算的水听器与AUV的斜距差与LBL测量得到的斜距差之差作为观测量。在紧组合系统中,设LBL测得的斜距差为ρLBLi,水底应答器阵基元的位置为P(xi,yi,zi),SINS测得的AUV位置为PSINS(xS,yS,zS),由SINS测得的AUV位置PSINS(xS,yS,zS)和水底应答器阵基元的位置为Pi(xi,yi,zi)所确定的斜距差为ρSINSi。
SINS斜距差:
ρSINSi=Ri-R0+(Gix-G0x)δx+(Giy-G0y)δy+(Giz-G0z)δz
=Ri-R0+eixδx+eiyδy+eizδz
LBL斜距差
则量测可写成
则有:
当系统采用地球直角坐标系(Oxeyeze)作为导航坐标系时,可用上式构造系统量测方程。实际应用中是以经纬度和高度定位的,因此要把dx,dy,dz用dl,dλ,dh表示。
由
量测方程为Z3×1=H3×18X18×1+VΔR(3×1)
式中,
设其中aij(i=1,2,3;j=1,2,3)为矩阵H1的元素
H1非零元素如下:
ai1=-(RN+h)sin L cos λei1-(RN+h)sin L sin λei2+[RN(1-e2)+h]ei3
ai2=-(RN+h)cos L sin λei1-(RN+h)cos L cos λei2
ai3=cos L cos λei1+cos L sin λei2+sin Lei3 (i=1,2,3)
所述的校正模块(5)根据SINS/LBL紧组合模块(4)的卡尔曼滤波结果对SINS(1)进行校正,最终得到精确的AUV位置信息PAUV。
与现有技术相比,本发明具有如下优点:
(1)解决了SINS系统误差随时间积累的问题,保证了AUV在水下长期自主导航定位的精度,同时避免了GPS及其他无线电定位系统的使用,为水下作业节约时间和能耗,提高了AUV水下作业效率。
(2)本发明重点介绍了SINS与LBL紧组合,对惯性导航系统与声学系统组合应用的研究有一定的意义。
附图说明
图1为SINS/LBL紧组合定位系统原理框图;
图2为长基线水声定位系统LBL示意图;
图3为水听器节点定位示意图。
具体实施方式
下面结合附图,进一步阐明本发明。
如图1所示,本发明由安装在AUV上的捷联惯性导航系统SINS(1)、布放在海底的长基线水声定位系统LBL(2)和数据处理单元三大部分组成。数据处理单元包括SINS两两基元与AUV斜距差计算模块(3)、SINS/LBL紧组合模块(4)和校正模块(5)。通过采用SINS与LBL紧组合的方法完成AUV水下自主导航,具体实现步骤如下:
(1)对惯性测量元件(IMU)输出数据通过捷联解算获得AUV位置信息,用地球大地坐标PSINS(LS,λS,hS)表示,并将PSINS(LS,λS,hS)转化为用地球直角坐标PSINS(xS,yS,zS)表示。
所述的SINS(1)系统包括IMU(Inertial Measurement Unit,惯性测量单元)元件及捷联解算模块,IMU元件用于得到惯性数据,捷联解算模块用于通过捷联解算,得到导航信息,其中包括位置信息PSINS。
1)SINS姿态矩阵及姿态角计算
采用四元数法计算姿态矩阵,根据欧拉定理,动坐标系相对参考坐标系的方位等效于动坐标系绕某个等效转轴转动一个角度θ,如果用u表示等效转轴方向的单位矢量,则动坐标系的方位完全由u和θ两个参数来确定。
用u和θ可构造一个四元数:
对上式求导并化简可得四元数微分方程:
式中
根据毕卡逼近法求解四元数微分方程得:
式中
式中
令地球坐标系相对惯性坐标系的自转角速度为ωie,(其值为15.04088°/h),L表示当地纬度,λ表示当地经度,则
ωie n:地球坐标系相对惯性坐标系的自转角速度在地理坐标系中的矢量,为:
ωie b:地球坐标系相对惯性坐标系的自转角速度在载体坐标系中的矢量,为:
式中的姿态矩阵在载体静止时,由初始角度决定;当载体相对地理坐标系转动时,姿态矩阵跟着变化,由四元数即时修正后求得(下同)。
ωen n:地理坐标相对地球坐标系转动角速度在地理坐标系中的矢量,为:
VE、VN分别为载体运动的东向和北向速度;
RN为参考椭球体子午面内的曲率半径,RN=Re(1-2e+3e sin2 L);
RE为垂直子午面的法线平面内的曲率半径,RE=Re(1+e sin2 L);
其中Re为参考椭球体的长轴半径;e为椭球的椭圆度。
又因为,则
ωen b:地理坐标相对地球坐标系转动角速度在载体坐标系中的矢量,为:
ωib b:陀螺输出角速度,记为
ωnb b:载体坐标系相对地理坐标系的转动角速度在载体坐标系中的矢量,记为
则可得
ωnb b=ωib b-ωie b-ωen b
四元数即时修正后,根据下式可由四元数的元实时更新姿态矩阵
从姿态阵中即可提取实时姿态角
2)SINS速度计算
载体坐标系中的比力矢量为fb,则地理坐标系中有:
式中的方向余弦矩阵在载体静止时,由初始角度决定;当载体相对地理坐标系转动时,方向余弦矩阵跟着变化,由四元数即时修正后求得。
载体在惯导系内的比力方程为:
写成分量形式有:
式中:fn为载体加速度在导航坐标系上的投影,fn=[fE fN fU]T;Vn表示船体在导航坐标系中的速度矢量,Vn=[VE VN VU]T;gn为重力加速度矢量,gn=[0 0 -g]T。
积分上式,即可求得运载体在导航坐标系上的各个速度分量VE、VN、VU。
3)位置计算
得到经纬度高度的微分方程可表示如下:
式中,h为高度。
积分上式的经纬度高度的更新公式即可得到经纬度和高度:
则得到位置P(λ,L,h)。
4)将3)得到的AUV在地球直角坐标系的坐标PSINS(LS,λS,hS)转化为其在地球大地坐标系的坐标PSINS(xS,yS,zS)。
可由公式
获得PSINS(xS,yS,zS)。
式中:RN为参考椭球体子午面內的曲率半径,RN=Re(1-2e+3e sin2 L)
RE为垂直子午面內的曲率半径,RE=Re(1+e sin2 L)
其中:Re为参考椭球体的长轴半径;e为椭球的椭圆度。
(2)SINS两两基元与目标斜距差推算
1)根据SINS解算的AUV位置PSINS(xs,ys,zs)和长基线水声定位系统LBL中水听器基元位置Pi(xi,yi,zi)计算得到水听器i(i=1,2,3)与AUV之间的斜距与水听器0与AUV之间的斜距之差
所述的长基线水声定位系统LBL(2)由布放在海底的四个位置已知的水听器组成,如图2所示,各水听器之间的距离为4km。如图3所示,利用母船,采用超短基线系统对水听器进行精确定位,计算精确坐标值。母船上安装有GPS、IMU和罗经,母船底安装有换能器基阵。根据超短基线系统计算出每个水听器在换能器基阵坐标下的相对位置,结合母船GPS位置、母船姿态以及各安装误差等因素可以计算出各水听器节点在地球坐标下的绝对位置。
2)将ρSINSi利用泰勒级数线性化。设AUV真实位置为PAUV(x,y,z),(δx,δy,δz)为SINS解算AUV位置的误差,则xS=x+δx,yS=y+δy,zS=z+δz。将ρSINSi泰勒级数展开取前两项得:
设
同理
其中,Gij(i=0,1,2,3;j=x,y,z)为已知量,可由SINS解算的概略位置PSINS(xS,yS,zS)和水底应答器阵基元的位置Pi(xi,yi,zi)计算得到,由于SINS解算的概略位置PSINS(xS,yS,zS)可能有较大误差,这样在进行方程线性化时略去高阶项会引起线性误差,可以利用迭代法解算,即在第一次解之后,用它作为近似值再重新计算。
设:eix=Gix-G0x,eiy=Giy-G0y,eiz=Giz-G0z,i=1,2,3
于是:
ρSINSi=Ri-R0+(Gix-G0x)δx+(Giy-G0y)δy+(Giz-G0z)δz
=Ri-R0+eixδx+eiyδy+eizδz
(3)SINS/LBL紧组合
1)建立LBL斜距差模型
由于时延差测量、声传播的多途径效应等将引起斜距差测量有误差,为简化模型,可认为斜距差误差是由常值偏置和随机噪声组成,则LBL水听器i(i=1,2,3)与AUV的斜距与水听器0与AUV的斜距之差可表示为:
式中,ΔRmeas为LBL水听器i(i=1,2,3)与AUV的斜距与水听器0与AUV的斜距之差,ΔR为斜距差真值,δR=[δR1 δR2 δR3]T为随机常值,νδR(t)~N(0,QΔR)为高斯白噪声。
2)建立SINS/LBL紧组合状态方程
SINS/LBL紧组合状态方程描述为:
其中:XSINS为SINS的状态向量,XLBL为LBL的状态向量,FSINS为SINS的转移矩阵,FLBL为LBL的转移矩阵,WSINS为SINS的系统噪声向量,WLBL为LBL的系统噪声向量,F为紧组合系统转移矩阵,X为紧组合系统状态向量,W为紧组合系统噪声向量。
根据捷联惯性导航系统长期工作时的误差特点,选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量:
XSINS=[δVE δVN δVU φE φN φU δL δL δh ▽bx ▽by ▽bz εbx εby εbz]T
式中,δVE、δVN、δVU分别是捷联东向、北向、天向的速度误差,分别是捷联东向、北向、天向的失准角,δL、δλ、δh分别是捷联纬度、经度、高度误差,三个位置误差由地球坐标系描述,▽bx、▽by、▽bz是捷联加表三个轴向的偏置误差,εbx、εby、εbz是捷联陀螺的三个轴向漂移。
XLBL=[δR1 δR2 δR3]T
式中,δR1、δR2、δR3分别为LBL水听器i(i=1,2,3)与AUV的斜距与水听器0与AUV的斜距之差的随机常值误差。
系统噪声阵
WLBL=[0 0 0]T
系统状态转移矩阵
式中,
其中:Fij为F9×9的元素,
RN为参考椭球体子午面內的曲率半径,RN=Re(1-2e+3e sin2 L)
RE为垂直子午面內的曲率半径,RE=Re(1+e sin2 L)
其中:Re为参考椭球体的长轴半径;e为椭球的椭圆度。
F37=-2ωie cos LVE
F57=-ωie sin L
Cij为姿态转移矩阵的元素
FLBL=03×3
3)建立SINS/LBL紧组合量测方程
紧组合系统采用SINS推算的水听器与AUV的斜距差与LBL测量得到的斜距差之差作为观测量。在紧组合系统中,设LBL测得的斜距差为ρLBLi,水底应答器阵基元的位置为P(xi,yi,zi),SINS测得的AUV位置为PSINS(xS,yS,zS),由SINS测得的AUV位置PSINS(xS,yS,zS)和水底应答器阵基元的位置为Pi(xi,yi,zi)所确定的斜距差为ρSINSi。
SINS斜距差:
LBL斜距差
则量测可写成
则有:
当系统采用地球直角坐标系(Oxeyeze)作为导航坐标系时,可用上式构造系统量测方程。实际应用中是以经纬度和高度定位的,因此要把dx,dy,dz用dl,dλ,dh表示。
由
量测方程为Z3×1=H3×18X18×1+VΔR(3×1)
式中,
其中aij(i=1,2,3;j=1,2,3)为矩阵H1的元素
H1非零元素如下:
ai1=-(RN+h)sin L cos λei1-(RN+h)sin L sin λei2+[RN(1-e2)+h]ei3
ai2=-(RN+h)cos L sin λei1-(RN+h)cos L cos λei2
ai3=cos L cos λei1+cos L sin λei2+sin Lei3 (i=1,2,3)
4)系统状态方程及量测方程的离散化
Xk=φk,k-1Xk-1+Γk-1Wk-1
Zk=HkXk+Vk
式中,Xk为k时刻的状态向量,也就是被估计矢量;Zk为k时刻的测量序列;Wk-1为k-1时刻的系统噪声;Vk为k时刻的测量噪声序列;Φk,k-1为k-1时刻到k时刻的一步状态转移矩阵;Γk-1是系统噪声输入矩阵,Hk为k时刻的测量矩阵,
利用标准卡尔曼滤波方程计算状态的最优估计:
状态一步预测向量
Xk/k-1=φk,k-1Xk-1
状态估值计算
Xk=Xk/k-1+Kk(Zk-HkXk/k-1)
滤波增益
Kk=Pk/k-1Hk T(HkPk/k-1Hk T+Rk)-1
一步预测均方误差矩阵
估计均方误差方程
(4)校正
根据滤波得到的状态估计,通过下述方法进行校正。
1)速度和位置校正
下次滤波前,每次捷联解算得到的速度和位置均通过下式进行校正:
2)惯性仪表输出校正
下次滤波前,每次捷联解算时所需的惯性仪表输出在使用前通过下式进行校正:
3)姿态矩阵、四元数校正
姿态校正:下次滤波前,按下式对每次捷联解算得到的进行校正。
四元数校正:因为捷联解算采用的是四元数算法,算法中是是采用四元数进行迭代更新的,所有还需对四元数进行校正。四元数可由更新的姿态矩阵转换得到。
Claims (1)
1.一种基于SINS/LBL紧组合的AUV水下导航定位方法,其特征在于:所用导航定位系统由安装在AUV上的捷联惯性导航系统SINS(1)、布放在海底的长基线水声定位系统LBL(2)和数据处理单元组成,其中,所述的捷联惯性导航系统SINS(1)包括捷联解算模块,所述的长基线水声定位系统LBL(2)由布放在海底的四个位置已知的水听器基阵组成,所述的数据处理单元包括SINS两两基元与AUV斜距差推算模块(3)、SINS/LBL紧组合模块(4)和校正模块(5),采用SINS/LBL紧组合方法完成组合导航,所述方法通过下列步骤实现:
(1)捷联惯性导航系统SINS(1)通过捷联解算得到相应的包括AUV的位置信息的导航信息,解算的位置信息用地球大地坐标PSINS(LS,λS,hS)表示,并将PSINS(LS,λS,hS)转化为用地球直角坐标PSINS(xS,yS,zS)表示;
(2)SINS两两基元与AUV斜距差推算模块(3)根据SINS提供的AUV位置信息PSINS(xS,yS,zS)和水听器基阵位置Pi(xi,yi,zi)推算SINS斜距差ρSINS;
(3)SINS/LBL紧组合模块(4)根据长基线水声定位系统LBL(2)的定位特点建立LBL斜距差模型,将SINS斜距差ρSINS和LBL水听器i与AUV之间的斜距与水听器0与AUV之间的斜距之差ρLBL的差值作为外部观测信息输入到卡尔曼滤波器进行滤波,其中,i=1,2,3;
(4)校正模块(5)根据SINS/LBL紧组合模块(4)的卡尔曼滤波结果对SINS(1)进行校正,最终得到精确的AUV位置信息PAUV;SINS两两基元与AUV斜距差推算模块(3)计算SINS斜距差的方法如下:
(1)根据SINS解算的AUV位置PSINS(xs,ys,zs)和长基线水声定位系统LBL中水听器基元位置Pi(xi,yi,zi)计算得到水听器i与AUV之间的斜距与水听器0与AUV之间的斜距之差其中,i=1,2,3;
(2)将ρSINSi利用泰勒级数线性化,设AUV真实位置为PAUV(x,y,z),(δx,δy,δz)为SINS解算AUV位置的误差,则xS=x+δx,yS=y+δy,zS=z+δz;将ρSINSi泰勒级数展开取前两项得:
设
同理
其中,Gij为已知量,i=0,1,2,3,j=x,y,z,可由SINS解算的概略位置PSINS(xS,yS,zS)和水底应答器阵基元的位置Pi(xi,yi,zi)计算得到,由于SINS解算的概略位置PSINS(xS,yS,zS)可能有较大误差,这样在进行方程线性化时略去高阶项会引起线性误差,利用迭代法解算,即在第一次解之后,用它作为近似值再重新计算;
设:eix=Gix-G0x,eiy=Giy-G0y,eiz=Giz-G0z,i=1,2,3
于是:
所述SINS/LBL紧组合模块(4)的具体实现步骤如下:
(1)建立LBL斜距差模型;由于时延差测量、声传播的多途径效应等将引起斜距差测量有误差,为简化模型,认为斜距差误差是由常值偏置和随机噪声组成,则LBL水听器i与AUV的斜距与水听器0与AUV的斜距之差可表示为:
式中,ΔRmeas为LBL水听器i与AUV的斜距与水听器0与AUV的斜距之差,ΔR为斜距差真值,δR=[δR1 δR2 δR3]T为随机常值,vΔR~N(0,QΔR)为高斯白噪声,i=1,2,3;
(2)建立SINS/LBL紧组合状态方程;
SINS/LBL紧组合状态方程描述为:
其中:XSINS为SINS的状态向量,XLBL为LBL的状态向量,FSINS为SINS的转移矩阵,FLBL为LBL的转移矩阵,WSINS为SINS的系统噪声向量,WLBL为LBL的系统噪声向量,F为紧组合系统转移矩阵,X为紧组合系统状态向量,W为紧组合系统噪声向量;根据捷联惯性导航系统长期工作时的误差特点,选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量:
式中,δVE、δVN、δVU分别是捷联东向、北向、天向的速度误差,分别是捷联东向、北向、天向的失准角,δL、δλ、δh分别是捷联纬度、经度、高度误差,三个位置误差由地球坐标系描述,是捷联加表三个轴向的偏置误差,εbx、εby、εbz是捷联陀螺的三个轴向漂移;
XLBL=[δR1 δR2 δR3]T
式中,δR1、δR2、δR3分别为LBL水听器i与AUV的斜距与水听器0与AUV的斜距之差的随机常值漂移,其中,i=1,2,3;系统噪声阵
WLBL=[0 0 0]T
系统状态转移矩阵
式中,
其中:Fij为F9×9的元素
RN为参考椭球体子午面內的曲率半径,RN=Re(1-2e+3esin2L)
RE为垂直子午面內的曲率半径,RE=Re(1+esin2L)
其中:Re为参考椭球体的长轴半径;e为椭球的椭圆度,L为当地纬度;
F37=-2ωie cosLVE
F57=-ωie sinL
Cij为姿态转移矩阵的元素
FLBL=03×3;
(3)建立SINS/LBL紧组合量测方程;
紧组合系统采用SINS推算的水听器与AUV的斜距差与LBL测量得到的斜距差之差作为观测量;在紧组合系统中,设LBL测得的斜距差为ρLBLi,水底应答器阵基元的位置为Pi(xi,yi,zi),SINS测得的AUV位置为PSINS(xS,yS,zS),由SINS测得的AUV位置PSINS(xS,yS,zS)和水底应答器阵基元的位置为Pi(xi,yi,zi)所确定的斜距差为ρSINSi;
SINS斜距差
ρSINSi=Ri-R0+(Gix-G0x)δx+(Giy-G0y)δy+(Giz-G0z)δz
=Ri-R0+eixδx+eiyδy+eizδz
LBL斜距差
则量测可写成
则有:
当系统采用地球直角坐标系(Oxeyeze)作为导航坐标系时,用上式构造系统量测方程;实际应用中是以经纬度和高度定位的,因此要把dx,dy,dz用dl,dλ,dh表示;
由
量测方程为Z3×1=H3×18X18×1+VΔR(3×1)
式中,
设其中aij为矩阵H1的元素,i=1,2,3;j=1,2,3;
H1非零元素如下:
ai1=-(RN+h)sin L cosλe1x-(RN+h)sin L sinλe1y+[RN(1-e2)+h]cos Le1z
ai2=-(RN+h)cos L sinλe2x-(RN+h)cos L cosλe2y
ai3=cos L cosλe3x+cos L sinλe3y+sin Le3z i=1,2,3。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410796735.XA CN104457754B (zh) | 2014-12-19 | 2014-12-19 | 一种基于sins/lbl紧组合的auv水下导航定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410796735.XA CN104457754B (zh) | 2014-12-19 | 2014-12-19 | 一种基于sins/lbl紧组合的auv水下导航定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104457754A CN104457754A (zh) | 2015-03-25 |
CN104457754B true CN104457754B (zh) | 2017-04-26 |
Family
ID=52904190
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410796735.XA Active CN104457754B (zh) | 2014-12-19 | 2014-12-19 | 一种基于sins/lbl紧组合的auv水下导航定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104457754B (zh) |
Families Citing this family (25)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106483498A (zh) * | 2015-09-01 | 2017-03-08 | 北京自动化控制设备研究所 | 一种sinsusbl紧耦合算法 |
CN105526931A (zh) * | 2015-12-02 | 2016-04-27 | 中国人民解放军91388部队 | 一种基于单个海底应答器的水下航行器组合导航方法 |
CN105676855B (zh) * | 2016-01-29 | 2018-06-19 | 中国船舶重工集团公司第七一〇研究所 | 一种远程自航水雷近水面的航行姿态校准系统和方法 |
CN106643709B (zh) * | 2016-10-10 | 2019-12-31 | 东南大学 | 一种海上运载体的组合导航方法及装置 |
CN107990891B (zh) * | 2016-10-26 | 2021-05-28 | 中国科学院沈阳自动化研究所 | 基于长基线和信标在线标定的水下机器人组合导航方法 |
CN106767793A (zh) * | 2017-01-19 | 2017-05-31 | 东南大学 | 一种基于sins/usbl紧组合的auv水下导航定位方法 |
CN107656281A (zh) * | 2017-08-17 | 2018-02-02 | 东南大学 | 一种基于倒谱分析的时间差辅助sins的导航方法 |
CN107966145B (zh) * | 2017-12-21 | 2020-12-15 | 中国船舶重工集团公司第七0七研究所 | 一种基于稀疏长基线紧组合的auv水下导航方法 |
CN108344999B (zh) * | 2018-01-09 | 2020-08-11 | 浙江大学 | 一种用于水下机器人导航的声呐地图构建与重定位方法 |
CN108444476B (zh) * | 2018-02-05 | 2021-04-23 | 哈尔滨工程大学 | 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法 |
CN108834050A (zh) * | 2018-06-15 | 2018-11-16 | 哈尔滨工程大学 | 一种长基线定位系统基站及通信方法 |
CN109186593A (zh) * | 2018-09-13 | 2019-01-11 | 深圳市吉影科技有限公司 | 一种水下无人机的运动轨迹记录绘制方法 |
CN109324330B (zh) * | 2018-09-18 | 2022-11-18 | 东南大学 | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 |
CN109596128B (zh) * | 2019-01-14 | 2022-06-21 | 哈尔滨工程大学 | 一种基于多水听器提高多auv协同定位性能的方法 |
CN110132308B (zh) * | 2019-05-27 | 2022-04-29 | 东南大学 | 一种基于姿态确定的usbl安装误差角标定方法 |
CN110244261B (zh) * | 2019-06-28 | 2021-05-18 | 中国科学院声学研究所 | 一种任意阵型的超短基线方位估计方法、系统、设备及存储介质 |
CN110542884B (zh) * | 2019-08-28 | 2020-11-06 | 中国科学院声学研究所 | 一种基于惯导修正的长基线导航定位方法 |
CN110749339A (zh) * | 2019-10-25 | 2020-02-04 | 嘉兴中科声学科技有限公司 | 水下航行器的定位校准方法、装置、船载设备和水下设备 |
CN111380517B (zh) * | 2020-03-04 | 2021-10-29 | 哈尔滨工程大学 | 一种基于usbl软件接收机的sins/usbl深组合导航定位方法 |
CN111982105B (zh) * | 2020-08-21 | 2023-10-24 | 运城学院 | 一种基于sins/lbl紧组合的水下导航定位方法及系统 |
CN112083425B (zh) * | 2020-09-14 | 2024-03-12 | 湖南航天机电设备与特种材料研究所 | 一种引入径向速度的sins/lbl紧组合导航方法 |
CN112184765B (zh) * | 2020-09-18 | 2022-08-23 | 西北工业大学 | 一种用于水下航行器的自主跟踪方法 |
CN113156442A (zh) * | 2021-03-22 | 2021-07-23 | 河海大学 | 一种基于长基线水声系统辅助导航的auv水下定位方法 |
CN113280812B (zh) * | 2021-05-12 | 2023-06-02 | 中山大学 | 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置 |
CN113776525B (zh) * | 2021-09-01 | 2023-12-05 | 东南大学 | 一种基于斜距差匹配的惯性/单声源被动组合导航方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103744098A (zh) * | 2014-01-23 | 2014-04-23 | 东南大学 | 基于sins/dvl/gps的auv组合导航系统 |
CN104061930A (zh) * | 2013-12-05 | 2014-09-24 | 东南大学 | 基于捷联惯性制导和多普勒计程仪的导航方法 |
-
2014
- 2014-12-19 CN CN201410796735.XA patent/CN104457754B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104061930A (zh) * | 2013-12-05 | 2014-09-24 | 东南大学 | 基于捷联惯性制导和多普勒计程仪的导航方法 |
CN103744098A (zh) * | 2014-01-23 | 2014-04-23 | 东南大学 | 基于sins/dvl/gps的auv组合导航系统 |
Non-Patent Citations (3)
Title |
---|
"Autonomous Underwater Vehicle Navigation";Paul A. Miller;《IEEE JOURNAL OF OCEANIC ENGINEERING》;20100731;正文第663-676页 * |
"GAS Tightly Coupled LBL/USBL Position and Velocity Filter for Underwater Vehicles";Pedro Batista;《2013 European Control Conference (ECC)》;20130719;正文第2982-2987页 * |
"长基线声学定位系统发展现状及其应用";宁津生;《海洋测绘》;20140630;第4卷(第1期);正文第221-224页 * |
Also Published As
Publication number | Publication date |
---|---|
CN104457754A (zh) | 2015-03-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104457754B (zh) | 一种基于sins/lbl紧组合的auv水下导航定位方法 | |
CN109782323B (zh) | 一种深海水下自主航行器导航定位及校准方法 | |
CN104316045B (zh) | 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法 | |
CN106767793A (zh) | 一种基于sins/usbl紧组合的auv水下导航定位方法 | |
CN109737956B (zh) | 一种基于双应答器的sins/usbl相位差紧组合导航定位方法 | |
CN102829777B (zh) | 自主式水下机器人组合导航系统及方法 | |
CN106643709B (zh) | 一种海上运载体的组合导航方法及装置 | |
CN102169184B (zh) | 组合导航系统中测量双天线gps安装失准角的方法和装置 | |
CN103674030B (zh) | 基于天文姿态基准保持的垂线偏差动态测量装置和方法 | |
CN106405670B (zh) | 一种适用于捷联式海洋重力仪的重力异常数据处理方法 | |
CN105091907B (zh) | Sins/dvl组合中dvl方位安装误差估计方法 | |
CN210719199U (zh) | 一种水下机器人多设备组合导航系统 | |
CN103245360A (zh) | 晃动基座下的舰载机旋转式捷联惯导系统自对准方法 | |
CN107797125B (zh) | 一种减小深海探测型auv导航定位误差的方法 | |
CN105486313A (zh) | 一种基于usbl辅助低成本sins系统的定位方法 | |
CN103900565A (zh) | 一种基于差分gps的惯导系统姿态获取方法 | |
CN110274591B (zh) | 深潜载人潜水器adcp辅助sins导航方法 | |
CN103017755A (zh) | 一种水下导航姿态测量方法 | |
CN107966145B (zh) | 一种基于稀疏长基线紧组合的auv水下导航方法 | |
CN110806209A (zh) | 一种水下机器人多设备组合导航系统及方法 | |
CN102628691A (zh) | 一种完全自主的相对惯性导航方法 | |
CN103900608A (zh) | 一种基于四元数ckf的低精度惯导初始对准方法 | |
CN103712621B (zh) | 偏振光及红外传感器辅助惯导系统定姿方法 | |
CN103604428A (zh) | 基于高精度水平基准的星敏感器定位方法 | |
CN104061930B (zh) | 一种基于捷联惯性制导和多普勒计程仪的导航方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |