CN111721284A - 一种无源模式下的sins/usbl组合导航定位方法 - Google Patents

一种无源模式下的sins/usbl组合导航定位方法 Download PDF

Info

Publication number
CN111721284A
CN111721284A CN202010144829.4A CN202010144829A CN111721284A CN 111721284 A CN111721284 A CN 111721284A CN 202010144829 A CN202010144829 A CN 202010144829A CN 111721284 A CN111721284 A CN 111721284A
Authority
CN
China
Prior art keywords
usbl
sins
propagation delay
coordinate system
error
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.)
Granted
Application number
CN202010144829.4A
Other languages
English (en)
Other versions
CN111721284B (zh
Inventor
郑翠娥
刘云
孙大军
张居成
韩云峰
崔宏宇
张殿伦
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202010144829.4A priority Critical patent/CN111721284B/zh
Publication of CN111721284A publication Critical patent/CN111721284A/zh
Application granted granted Critical
Publication of CN111721284B publication Critical patent/CN111721284B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements

Abstract

一种无源模式下的SINS/USBL组合导航定位方法,涉及组合导航及水声定位技术领域,针对现有技术中无源模式下SINS/USBL紧组合导航定位精度低的问题，本发明引入深度计信息，设计USBL无源工作模式下的时钟误差的计算方法，补偿USBL单通道的时延误差，以提高组合导航定位的精度。本发明能在有限的基阵开角范围内提高SINS/USBL紧组合导航定位精度。

Description

一种无源模式下的SINS/USBL组合导航定位方法
技术领域
本发明涉及组合导航及水声定位技术领域，具体为一种无源模式下的SINS/USBL组合导航定位方法。
背景技术
USBL水声定位系统的无源工作模式是指声源时钟和测量系统时钟不同步，使USBL声学接收基阵的各个通道测量的声信号传播时延均含有相同的时钟误差。在利用两两通道的时延差时，时钟误差的影响可以相互抵消，而单个通道的时延精度仍受时钟误差的影响，降低了USBL的测距和定位精度。传统的SINS/USBL紧组合导航都是建立在USBL有源工作模式下，以传播时延和时延差(斜距和斜距差)作为数据融合的基础，而对无源模式下的SINS/USBL紧组合导航研究较少。
发明内容
本发明的目的是：针对现有技术中无源模式下SINS/USBL紧组合导航定位精度低的问题，提出一种无源模式下的SINS/USBL组合导航定位方法。
本发明为了解决上述技术问题采取的技术方案是：
一种无源模式下的SINS/USBL组合导航定位方法，包括以下步骤：
步骤一：将USBL声学基阵倒置安装在水下航行器上，并将SINS的陀螺组件、加计组件以及深度计固联在水下航行器上，水面布设一个非同步信标，并设置GPS获取信标在导航坐标系下的位置信息；
步骤二：建立声学基阵坐标系，并确定基元在基阵坐标系下的位置；
步骤三：利用USBL测量声信号在信标和基元之间的传播时延及时延差，获得USBL传播时延量测值及USBL传播时延差量测值，并结合有效声速，解算航行器在基阵坐标系下相对于信标的方向角余弦，所述有效声速为信标和接收基阵的几何距离与声信号在两点间传播时间的比值；
步骤四：获取航行器的深度信息，并结合方向角余弦，判断航行器与信标的相对位置关系是否满足基阵开角条件，若满足，则计算得到信标与航行器的斜距预测值和声信号在信标与基阵中心之间的传播时延预测值，当出现连续两个采样时刻都满足基阵开角条件时，计算得到连续两个采样时刻的斜距预测值和传播时延预测值，并执行步骤五，若不满足基阵开角条件，则判断是否存在邻近时刻的时钟误差估计值，若存在，则沿用其估计值后执行步骤六，若不存在，则沿用步骤三中得到的USBL传播时延量测值，并执行步骤七，所述基阵开角为基阵坐标系原点与信标的连线与z轴正向的夹角；
步骤五：建立时钟误差模型，利用连续两个采样时刻的传播时延预测值与对应的USBL 测量的各基元传播时延均值之差，估计误差模型参数，计算时钟误差估计值；
步骤六：利用时钟误差估计值，补偿USBL测量的传播时延，得到新的USBL传播时延测量值；
步骤七：利用SINS计算水下航行器的姿态和位置，并根据得到的姿态和位置将基元在基阵坐标系下的相对位置转换到SINS使用的导航坐标系下；
步骤八：根据SINS转换的基元在导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差，获得SINS传播时延量测值和传播时延差量测值；
步骤九：根据USBL传播时延测量值并结合步骤八中SINS传播时延量测值和传播时延差量测值，建立基于USBL和SINS传播时延量测值之差、传播时延差量测值之差的信息融合扩展卡尔曼滤波器；
步骤十：利用信息融合扩展卡尔曼滤波器计算SINS误差，修正导航输出，重置扩展卡尔曼滤波器状态，重新执行步骤三至步骤十。
进一步的，所述步骤二中建立声学基阵坐标系的具体步骤为：
首先以基阵中心为原点，然后以沿基阵平面指向水下航行器的艏向方向为y轴，z轴垂直于基阵平面向上，x轴与y轴、z轴构成右手坐标系；
所述步骤八中导航坐标系为东北天地理坐标系，所述东北天地理坐标系为以水下航行器质心为原点，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，构成右手坐标系。
进一步的，所述信息融合扩展卡尔滤波器以SINS的姿态误差、速度误差、位置误差和陀螺漂移、加速度计偏置为状态变量，以USBL和SINS的传播时延之差、传播时延差之差作为观测量，在最小均方误差准则下，建立描述系统的状态方程和观测方程。
进一步的，所述基元在基阵坐标系下的位置为：
Figure RE-GDA0002643472050000021
其中，i为基元序号，r为基元1，3或基元2，4的间距。
进一步的，所述步骤三中方向角余弦为：
Figure RE-GDA0002643472050000031
其中，c为有效声速，d为对应基元间距，
Figure RE-GDA0002643472050000032
为USBL定位系统测量的声信号在各基元和信标间的传播时延，
Figure RE-GDA0002643472050000033
和
Figure RE-GDA0002643472050000034
为传播时延差。
进一步的，所述步骤六中利用相邻时刻的传播时延预测值与对应的USBL水声定位系统测量的声信号传播时延均值之差，估计误差模型参数，计算时钟误差估计值，补偿USBL声学基阵的传播时延误差的具体步骤为：
首先使USBL无源工作模式下的时钟误差δτ与USBL测量的声信号传播时延τ满足δτ＝aτ+b，a为时钟误差的变化速率，b为一常值误差，联立式
Figure RE-GDA0002643472050000035
式
Figure RE-GDA0002643472050000036
和式
Figure RE-GDA0002643472050000037
计算a和b的估计值
Figure RE-GDA0002643472050000038
和
Figure RE-GDA0002643472050000039
当基阵开角θz的绝对值小于60度时，
Figure RE-GDA00026434720500000310
和
Figure RE-GDA00026434720500000311
保持与邻近更新值不变，计算时钟误差估计值
Figure RE-GDA00026434720500000312
即补偿USBL声学基阵的传播时延误差公式为：
Figure RE-GDA00026434720500000313
其中，τi是基元i的时延测量真值，δτ是时钟误差真值，
Figure RE-GDA00026434720500000314
是补偿的时钟误差，ni是基元i的测量噪声，τ31是基元1,3的时延差真值，n31是基元1,3的时延差测量噪声，τ42是基元2,4的时延差真值，n42是基元4,2的时延差测量噪声。
进一步的，所述步骤七的具体步骤为：
首先结合基阵坐标系与载体坐标系的安装偏差校准结果，得到基元在导航坐标系下的位置，其中航行器姿态包括航向角A、俯仰角K和横滚角ψ，声学基阵与航行器对应的载体坐标系的角度安装偏差分别为α、β和γ，位置安装偏差为ΔX＝[Δx Δy Δz]T，载体坐标系到导航坐标系的转换矩阵
Figure RE-GDA0002643472050000041
和声学基阵坐标系到载体坐标系的转换矩阵
Figure RE-GDA0002643472050000042
分别为：
Figure RE-GDA0002643472050000043
Figure RE-GDA0002643472050000044
则第i个基元在导航坐标系下的位置表示为
Figure RE-GDA0002643472050000045
进一步的，所述步骤八中传播时延
Figure RE-GDA0002643472050000046
为：
Figure RE-GDA0002643472050000047
Figure RE-GDA0002643472050000048
为SINS计算的基元位置，
Figure RE-GDA0002643472050000049
为基元真实位置，
Figure RE-GDA00026434720500000410
为
Figure RE-GDA00026434720500000411
和
Figure RE-GDA00026434720500000412
之间的误差，
Figure RE-GDA00026434720500000413
为信标位置，c为有效声速；
传播时延差为：
Figure RE-GDA00026434720500000414
进一步的，所述步骤九的具体步骤为：
SINS位置为纬度L、经度λ和高度h的形式，将其转换为地球直角坐标，则
Figure RE-GDA00026434720500000415
其中，RN为地球卯酉圈主曲率半径，
Figure RE-GDA00026434720500000416
Re为地球半径，e为地球偏心率，
Figure RE-GDA00026434720500000417
其中a,b分别是椭圆长轴和短轴半径；
上式的微分形式为：
Figure RE-GDA0002643472050000051
记
Figure RE-GDA0002643472050000052
地球直角坐标系到SINS计算导航坐标系的转换矩阵为：
Figure RE-GDA0002643472050000053
扩展卡尔曼滤波状态变量由SINS姿态误差φ＝[φx φy φz]T，速度误差δν＝[δvxδvy δvz]T，位置误差δp＝[δL δλ δh]T，陀螺漂移误差ε＝[εx εy εz]T，加速度计偏置误差
Figure RE-GDA0002643472050000054
组成，即
Figure RE-GDA0002643472050000055
状态方程为
Xk+1＝Fk+1/kXk+wk+1
其中，Fk+1/k为状态转移矩阵，由SINS的误差方程获得，wk+1为扩展卡尔滤波过程噪声矢量；扩展卡尔曼滤波的观测量为
Figure RE-GDA0002643472050000056
观测方程为
Zk+1＝Hk+1Xk+1+vk+1
其中，vk+1为高斯白噪声序列，
传播时延
Figure RE-GDA0002643472050000057
的全微分表示为：
Figure RE-GDA0002643472050000061
记
Figure RE-GDA0002643472050000062
Figure RE-GDA0002643472050000063
观测矩阵Hk+1为
Figure RE-GDA0002643472050000064
进一步的，所述步骤十的具体步骤为：
步骤十一：根据
Figure RE-GDA0002643472050000065
计算k+1时刻的状态预测值
Figure RE-GDA0002643472050000066
步骤十二：根据Pk+1/k＝Fk+1/kPkFT k+1/k+Qk计算k+1时刻的状态预测误差协方差矩阵Pk+1/k，其中Pk为k时刻的状态估计误差协方差矩阵，Qk为k时刻的系统过程噪声协方差矩阵；
步骤十三：根据Kk+1＝PkHT k+1(Hk+1PkHT k+1+Rk+1)-1计算k+1时刻的滤波器增益Kk+1，其中Rk+1和Hk+1分别是k+1时刻的系统观测噪声协方差矩阵和观测矩阵；
步骤十四：根据Pk+1＝(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1；
步骤十五：根据
Figure RE-GDA0002643472050000067
计算k+1时刻的状态估计值
Figure RE-GDA0002643472050000068
步骤十六：根据
Figure RE-GDA0002643472050000069
的计算结果修正SINS输出，重置扩展卡尔曼滤波器状态，重新执行步骤三至骤十。
本发明的有益效果是：
本发明引入深度计信息，设计USBL无源工作模式下的时钟误差的计算方法，补偿USBL单通道的时延误差，以提高组合导航定位的精度。本发明能在有限的基阵开角范围内提高SINS/USBL紧组合导航定位精度。
附图说明
图1为无源模式下USBL时钟误差估计原理图；
图2为航行器与信标在基阵坐标系下的相对位置关系图；
图3为基阵坐标系、载体坐标系和导航坐标系的示意简图。
具体实施方式
具体实施方式一：参照图具体说明本实施方式，本实施方式所述的一种无源模式下的 SINS/USBL组合导航定位方法，包括以下步骤：
步骤一：将USBL声学基阵倒置安装在水下航行器上，并将SINS的陀螺组件、加计组件以及深度计固联在水下航行器上，水面布设一个非同步信标，并设置GPS获取信标在导航坐标系下的位置信息；
步骤二：建立声学基阵坐标系，并确定基元在基阵坐标系下的位置；
步骤三：利用USBL测量声信号在信标和基元之间的传播时延及时延差，获得USBL传播时延量测值及USBL传播时延差量测值，并结合有效声速，解算航行器在基阵坐标系下相对于信标的方向角余弦，所述有效声速为信标和接收基阵的几何距离与声信号在两点间传播时间的比值；
步骤四：获取航行器的深度信息，并结合方向角余弦，判断航行器与信标的相对位置关系是否满足基阵开角条件，若满足，则计算得到信标与航行器的斜距预测值和声信号在信标与基阵中心之间的传播时延预测值，当出现连续两个采样时刻都满足基阵开角条件时，计算得到连续两个采样时刻的斜距预测值和传播时延预测值，并执行步骤五，若不满足基阵开角条件，则判断是否存在邻近时刻的时钟误差估计值，若存在，则沿用其估计值后执行步骤六，若不存在，则沿用步骤三中得到的USBL传播时延量测值，并执行步骤七。所述基阵开角为基阵坐标系原点与信标的连线与z轴正向的夹角。
步骤五：建立时钟误差模型，利用连续两个采样时刻的传播时延预测值与对应的USBL 测量的各基元传播时延均值之差，估计误差模型参数，计算时钟误差估计值；
步骤六：利用时钟误差估计值，补偿USBL测量的传播时延，得到新的USBL传播时延测量值；
步骤七：利用SINS计算水下航行器的姿态和位置，并根据得到的姿态和位置将基元在基阵坐标系下的相对位置转换到SINS使用的导航坐标系下；
步骤八：根据SINS转换的基元在导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差，获得SINS传播时延量测值和传播时延差量测值；
步骤九：根据USBL传播时延测量值并结合步骤八中SINS传播时延量测值和传播时延差量测值，建立基于USBL和SINS传播时延量测值之差、传播时延差量测值之差的信息融合扩展卡尔曼滤波器；
步骤十：利用信息融合扩展卡尔曼滤波器计算SINS误差，修正导航输出，重置扩展卡尔曼滤波器状态，重新执行步骤三至步骤十。
针对上述步骤需要说明的是：
本发明是连续导航定位的需要，直至导航结束，不然就一直重复。
所述声学基阵是由多个声信号接收换能器(基元)均匀分布的一个圆柱形设备。
所述基阵坐标系是指以基阵中心为原点，沿基阵平面指向水下航行器的艏向方向为y 轴，z轴垂直于基阵平面向上，x轴与y轴、z轴构成右手坐标系。
所述基阵开角是指在基阵坐标系原点与信标的连线与z轴正向的夹角。
所述导航坐标系是指“东北天(ENU)”地理坐标系。
所述“东北天(ENU)”地理坐标系是指以载体质心为原点，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，构成右手坐标系。
所述有效声速为信标和接收基阵的几何距离与声信号在两点间的传播时间的比值。
所述信息融合扩展卡尔滤波器是指以捷联惯性导航的姿态误差、速度误差、位置误差和陀螺漂移、加速度计偏置为状态变量，以超短基线和捷联惯性导航的传播时延之差、传播时延差之差作为观测量，在最小均方误差准则下，建立描述系统的状态方程和观测方程，对状态变量进行估计。
所述扩展卡尔滤波器是指将非线性的状态方程和观测方程进行线性近似处理。
所述重置滤波状态变量是指，在修正捷联惯性导航输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。
本发明中所述的USBL声学基阵为多个声信号接收换能器均匀设置，声信号接收换能器一般大于或等于3个。
本发明首次研究无源模式SINS/USBL紧组合导航中USBL时钟误差的估计与补偿方法，以提高USBL的时延测量精度和SINS/USBL紧组合导航定位精度。
本发明时钟误差的估计精度虽受USBL基阵开角的影响，但能在有限的基阵开角范围内提高SINS/USBL紧组合导航定位精度。
针对每个步骤的具体说明如下：
步骤一：将超短基线声学基阵和捷联惯性导航的陀螺组件、加速度计组件以及深度计安装在水下航行器上，其中信标可以在水下，USBL基阵可以不倒置。
步骤二：结合附图2，图2中基元1-4在基阵坐标系下的坐标为：
Figure RE-GDA0002643472050000091
式(1)中，i为基元序号，r为基元1,3或基元2,4的间距。图中a为基阵坐标系，b 为载体坐标系，n为导航坐标系。
步骤三：超短基线定位系统测量的声信号在各基元和信标间的传播时延为
Figure RE-GDA0002643472050000092
传播时延差为
Figure RE-GDA0002643472050000093
和
Figure RE-GDA0002643472050000094
下标表示对应基元的传播时延之差。将
Figure RE-GDA0002643472050000095
表示为对应真值附加时钟误差和高斯白噪声干扰的形式，将
Figure RE-GDA0002643472050000096
和
Figure RE-GDA0002643472050000097
表示为附加高斯白噪声干扰的形式：
Figure RE-GDA0002643472050000098
τi是基元i的时延测量真值，δτ是时钟误差真值，ni是基元i的测量噪声，τ31是基元1,3的时延差真值，n31是基元1,3的时延差测量噪声，τ42是基元2,4的时延差真值，n42是基元4,2的时延差测量噪声。
附图2的基阵坐标系中，航行器相对于信标的方向角余弦为
Figure RE-GDA0002643472050000101
式(3)中，c为有效声速，d为对应基元间距。
步骤四：t1时刻深度计辅助下的预测斜距和预测传播时延分别为：
Figure RE-GDA0002643472050000102
Figure RE-GDA0002643472050000103
步骤五：重复步骤三，t2时刻深度计辅助下的预测斜距和预测传播时延分别为：
Figure RE-GDA0002643472050000104
Figure RE-GDA0002643472050000105
步骤六：USBL无源工作模式下的时钟误差δτ与USBL测量的声信号传播时延τ满足如下关系：
δτ＝aτ+b (8)
a为时钟误差的变化速率，b为一常值误差。联立式(2)、式(5)和式(7)计算a和 b的估计值
Figure RE-GDA0002643472050000106
和
Figure RE-GDA0002643472050000107
θz在60°内，采用深度计辅助预测的USBL斜距信息和时延信息才是有效的。当基阵开角θz不满足条件时，参数
Figure RE-GDA0002643472050000108
和
Figure RE-GDA0002643472050000109
保持与邻近更新值不变。
计算时钟误差估计值
Figure RE-GDA00026434720500001010
由式(9)对USBL的时延误差进行修正。
Figure RE-GDA00026434720500001011
步骤七：捷联惯性导航解算水下航行器在导航坐标系下的姿态、速度和位置。根据姿态信息，得到SINS对导航坐标系的复现，称为计算导航坐标系。结合基阵坐标系与载体坐标系的安装偏差校准结果，得到基元在计算导航坐标系下的位置。
建立载体坐标系obxbybzb，载体坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，载体坐标系的定义满足右手定则；
其中航行器姿态包括航向角A、俯仰角K和横滚角ψ，声学基阵坐标系与水下航行器载体坐标系对应的三个轴的角度安装偏差分别为α、β和γ，坐标系原点在载体坐标系下的位置安装偏差为ΔX＝[Δx Δy Δz]T。载体坐标系到计算导航坐标系的转换矩阵
Figure RE-GDA0002643472050000111
和声学基阵坐标系到载体坐标系的转换矩阵
Figure RE-GDA0002643472050000112
分别为：
Figure RE-GDA0002643472050000113
Figure RE-GDA0002643472050000114
则第i个基元在计算导航坐标系下的位置表示为
Figure RE-GDA0002643472050000115
由于捷联惯性导航计算的导航坐标系n'与真实导航坐标系n存在角度误差φ＝[φx φy φz]T，二者之间的转换矩阵
Figure RE-GDA0002643472050000116
可近似为
Figure RE-GDA0002643472050000117
其中
Figure RE-GDA0002643472050000118
因此基元在计算导航坐标系下的位置
Figure RE-GDA0002643472050000119
与位置真值
Figure RE-GDA00026434720500001110
的误差为
Figure RE-GDA00026434720500001111
Figure RE-GDA0002643472050000121
ΔXi是
Figure RE-GDA0002643472050000122
与水下航行器的位置真值
Figure RE-GDA0002643472050000123
的差；记
Figure RE-GDA0002643472050000124
Figure RE-GDA0002643472050000125
是在计算导航坐标系中第i个基元相对载体坐标系原点的位置，
Figure RE-GDA0002643472050000126
是
Figure RE-GDA0002643472050000127
沿计算导航坐标系xn′轴、yn′轴和zn′轴分量。
步骤八声信号在第i个基元与信标之间的传播时延为：
Figure RE-GDA0002643472050000128
式(18)中，
Figure RE-GDA0002643472050000129
为SINS计算的基元位置，
Figure RE-GDA00026434720500001210
为基元真实位置，
Figure RE-GDA00026434720500001211
是信标位置，c是有效声速。
||·||是矩阵二范数的表示，即
Figure RE-GDA00026434720500001212
其中，
Figure RE-GDA00026434720500001213
为信标在真实导航坐标系的xn轴方向位置，
Figure RE-GDA00026434720500001214
为信标在真实导航坐标系的yn轴方向位置，
Figure RE-GDA00026434720500001215
为信标在真实导航坐标系的zn轴方向位置，
Figure RE-GDA00026434720500001216
为第i个基元在计算导航坐标系的xn轴方向位置，
Figure RE-GDA00026434720500001217
为第i个基元在计算导航坐标系的yn轴方向位置，
Figure RE-GDA00026434720500001218
为第 i个基元在计算导航坐标系的zn轴方向位置；
声信号的传播时延差为：
Figure RE-GDA00026434720500001219
步骤九：建立基于传播时延、传播时延差融合的扩展卡尔曼滤波器状态方程和观测方程。
首先SINS位置为纬度L、经度λ和高度h的形式，将其转换为地球直角坐标，地球直角坐标系以地心为原点，xe轴指向本初子午线与赤道交点，ye轴指向90°经线与赤道交点，ze轴与xe轴、ye轴构成右手坐标系，则
Figure RE-GDA0002643472050000131
其中，RN为地球卯酉圈主曲率半径
Figure RE-GDA0002643472050000132
Re为地球半径；e为地球偏心率，
Figure RE-GDA0002643472050000133
其中a,b分别是椭圆长轴和短轴半径；
上式的微分形式为：
Figure RE-GDA0002643472050000134
记
Figure RE-GDA0002643472050000135
地球直角坐标系到SINS计算导航坐标系的转换矩阵为：
Figure RE-GDA0002643472050000136
扩展卡尔曼滤波状态变量由SINS姿态误差φ＝[φx φy φz]T，速度误差δν＝[δvxδvy δvz]T，位置误差δp＝[δL δλ δh]T，陀螺漂移误差ε＝[εx εy εz]T，加速度计偏置误差
Figure RE-GDA0002643472050000137
组成，即
Figure RE-GDA0002643472050000138
状态方程为
Xk+1＝Fk+1/kXk+wk+1 (24)
其中Fk+1/k为状态转移矩阵，由SINS的误差方程获得。wk+1为扩展卡尔滤波过程噪声矢量；
扩展卡尔曼滤波的观测量为
Figure RE-GDA0002643472050000141
观测方程为
Zk+1＝Hk+1Xk+1+vk+1 (26)
其中vk+1为高斯白噪声序列。
结合式(16)，式(17)的全微分表示为：
Figure RE-GDA0002643472050000142
观测矩阵Hk+1为
Figure RE-GDA0002643472050000143
步骤十：计算捷联惯性导航的导航误差，滤波解算过程如下。
根据
Figure RE-GDA0002643472050000144
计算k+1时刻的状态预测值
Figure RE-GDA0002643472050000145
根据Pk+1/k＝Fk+1/kPkFT k+1/k+Qk计算k+1时刻的状态预测误差协方差矩阵Pk+1/k，其中Pk为k时刻的状态估计误差协方差矩阵，Qk为k时刻的系统过程噪声协方差矩阵。
根据Kk+1＝PkHT k+1(Hk+1PkHT k+1+Rk+1)-1计算k+1时刻的滤波器增益Kk+1，其中Rk+1和Hk+1分别是k+1时刻的系统观测噪声协方差矩阵和观测矩阵。
根据Pk+1＝(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1。
根据
Figure RE-GDA0002643472050000146
计算k+1时刻的状态估计值
Figure RE-GDA0002643472050000147
根据
Figure RE-GDA0002643472050000151
的计算结果修正捷联惯性导航输出，重置扩展卡尔曼滤波器状态，重新执行步骤三至步骤十。
需要注意的是，具体实施方式仅仅是对本发明技术方案的解释和说明，不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的，仍应落入本发明的保护范围内。

Claims (10)

1.一种无源模式下的SINS/USBL组合导航定位方法，其特征在于包括以下步骤：
步骤一：将USBL声学基阵倒置安装在水下航行器上，并将SINS的陀螺组件、加计组件以及深度计固联在水下航行器上，水面布设一个非同步信标，并设置GPS获取信标在导航坐标系下的位置信息；
步骤二：建立声学基阵坐标系，并确定基元在基阵坐标系下的位置；
步骤三：利用USBL测量声信号在信标和基元之间的传播时延及时延差，获得USBL传播时延量测值及USBL传播时延差量测值，并结合有效声速，解算航行器在基阵坐标系下相对于信标的方向角余弦，所述有效声速为信标和接收基阵的几何距离与声信号在两点间传播时间的比值；
步骤四：获取航行器的深度信息，并结合方向角余弦，判断航行器与信标的相对位置关系是否满足基阵开角条件，若满足，则计算得到信标与航行器的斜距预测值和声信号在信标与基阵中心之间的传播时延预测值，当出现连续两个采样时刻都满足基阵开角条件时，计算得到连续两个采样时刻的斜距预测值和传播时延预测值，并执行步骤五，若不满足基阵开角条件，则判断是否存在邻近时刻的时钟误差估计值，若存在，则沿用其估计值后执行步骤六，若不存在，则沿用步骤三中得到的USBL传播时延量测值，并执行步骤七，所述基阵开角为基阵坐标系原点与信标的连线与z轴正向的夹角；
步骤五：建立时钟误差模型，利用连续两个采样时刻的传播时延预测值与对应的USBL测量的各基元传播时延均值之差，估计误差模型参数，计算时钟误差估计值；
步骤六：利用时钟误差估计值，补偿USBL测量的传播时延，得到新的USBL传播时延测量值；
步骤七：利用SINS计算水下航行器的姿态和位置，并根据得到的姿态和位置将基元在基阵坐标系下的相对位置转换到SINS使用的导航坐标系下；
步骤八：根据SINS转换的基元在导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差，获得SINS传播时延量测值和传播时延差量测值；
步骤九：根据USBL传播时延测量值并结合步骤八中SINS传播时延量测值和传播时延差量测值，建立基于USBL和SINS传播时延量测值之差、传播时延差量测值之差的信息融合扩展卡尔曼滤波器；
步骤十：利用信息融合扩展卡尔曼滤波器计算SINS误差，修正导航输出，重置扩展卡尔曼滤波器状态，重新执行步骤三至步骤十。
2.根据权利要求1所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述步骤二中建立声学基阵坐标系的具体步骤为：
首先以基阵中心为原点，然后以沿基阵平面指向水下航行器的艏向方向为y轴，z轴垂直于基阵平面向上，x轴与y轴、z轴构成右手坐标系；
所述步骤八中导航坐标系为东北天地理坐标系，所述东北天地理坐标系为以水下航行器质心为原点，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，构成右手坐标系。
3.根据权利要求1所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述信息融合扩展卡尔滤波器以SINS的姿态误差、速度误差、位置误差和陀螺漂移、加速度计偏置为状态变量，以USBL和SINS的传播时延之差、传播时延差之差作为观测量，在最小均方误差准则下，建立描述系统的状态方程和观测方程。
4.根据权利要求1所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述基元在基阵坐标系下的位置为：
Figure FDA0002400369390000021
其中，i为基元序号，r为基元1，3或基元2，4的间距。
5.根据权利要求4所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述步骤三中方向角余弦为：
Figure FDA0002400369390000022
其中，c为有效声速，d为对应基元间距，
Figure FDA0002400369390000023
为USBL定位系统测量的声信号在各基元和信标间的传播时延，
Figure FDA0002400369390000024
和
Figure FDA0002400369390000025
为传播时延差。
6.根据权利要求5所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述步骤六中利用相邻时刻的传播时延预测值与对应的USBL水声定位系统测量的声信号传播时延均值之差，估计误差模型参数，计算时钟误差估计值，补偿USBL声学基阵的传播时延误差的具体步骤为：
首先使USBL无源工作模式下的时钟误差δτ与USBL测量的声信号传播时延τ满足δτ＝aτ+b，a为时钟误差的变化速率，b为一常值误差，联立式
Figure FDA0002400369390000031
式
Figure FDA0002400369390000032
和式
Figure FDA0002400369390000033
计算a和b的估计值
Figure FDA0002400369390000034
和
Figure FDA0002400369390000035
当基阵开角θz的绝对值小于60度时，
Figure FDA0002400369390000036
和
Figure FDA0002400369390000037
保持与邻近更新值不变，计算时钟误差估计值
Figure FDA0002400369390000038
即补偿USBL声学基阵的传播时延误差公式为：
Figure FDA0002400369390000039
其中，τi是基元i的时延测量真值，δτ是时钟误差真值，
Figure FDA00024003693900000310
是补偿的时钟误差，ni是基元i的测量噪声，τ31是基元1,3的时延差真值，n31是基元1,3的时延差测量噪声，τ42是基元2,4的时延差真值，n42是基元4,2的时延差测量噪声。
7.根据权利要求6所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述步骤七的具体步骤为：
首先结合基阵坐标系与载体坐标系的安装偏差校准结果，得到基元在导航坐标系下的位置，其中航行器姿态包括航向角A、俯仰角K和横滚角ψ，声学基阵与航行器对应的载体坐标系的角度安装偏差分别为α、β和γ，位置安装偏差为ΔX＝[Δx Δy Δz]T，载体坐标系到导航坐标系的转换矩阵
Figure FDA00024003693900000311
和声学基阵坐标系到载体坐标系的转换矩阵
Figure FDA00024003693900000312
分别为：
Figure FDA00024003693900000313
Figure FDA00024003693900000314
则第i个基元在导航坐标系下的位置表示为
Figure FDA00024003693900000315
8.根据权利要求7所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述步骤八中传播时延
Figure FDA0002400369390000041
为：
Figure FDA0002400369390000042
Figure FDA0002400369390000043
为SINS计算的基元位置，
Figure FDA0002400369390000044
为基元真实位置，
Figure FDA0002400369390000045
为
Figure FDA0002400369390000046
和
Figure FDA0002400369390000047
之间的误差，
Figure FDA0002400369390000048
为信标位置，c为有效声速；
传播时延差为：
Figure FDA0002400369390000049
9.根据权利要求8所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述步骤九的具体步骤为：
SINS位置为纬度L、经度λ和高度h的形式，将其转换为地球直角坐标，则
Figure FDA00024003693900000410
其中，RN为地球卯酉圈主曲率半径，
Figure FDA00024003693900000411
Re为地球半径，e为地球偏心率，
Figure FDA00024003693900000412
其中a,b分别是椭圆长轴和短轴半径；
上式的微分形式为：
Figure FDA00024003693900000413
记
Figure FDA00024003693900000414
地球直角坐标系到SINS计算导航坐标系的转换矩阵为：
Figure FDA0002400369390000051
扩展卡尔曼滤波状态变量由SINS姿态误差φ＝[φx φy φz]T，速度误差δν＝[δvx δvyδvz]T，位置误差δp＝[δL δλ δh]T，陀螺漂移误差ε＝[εx εy εz]T，加速度计偏置误差
Figure FDA0002400369390000052
组成，即
Figure FDA0002400369390000053
状态方程为
Xk+1＝Fk+1/kXk+wk+1
其中，Fk+1/k为状态转移矩阵，由SINS的误差方程获得，wk+1为扩展卡尔滤波过程噪声矢量；扩展卡尔曼滤波的观测量为
Figure FDA0002400369390000054
观测方程为
Zk+1＝Hk+1Xk+1+vk+1
其中，vk+1为高斯白噪声序列，
传播时延
Figure FDA0002400369390000055
的全微分表示为：
Figure FDA0002400369390000056
记
Figure FDA0002400369390000057
Figure FDA0002400369390000058
观测矩阵Hk+1为
Figure FDA0002400369390000061
10.根据权利要求9所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述步骤十的具体步骤为：
步骤十一：根据
Figure FDA0002400369390000062
计算k+1时刻的状态预测值
Figure FDA0002400369390000063
步骤十二：根据Pk+1/k＝Fk+1/kPkFT k+1/k+Qk计算k+1时刻的状态预测误差协方差矩阵Pk+1/k，其中Pk为k时刻的状态估计误差协方差矩阵，Qk为k时刻的系统过程噪声协方差矩阵；
步骤十三：根据Kk+1＝PkHT k+1(Hk+1PkHT k+1+Rk+1)-1计算k+1时刻的滤波器增益Kk+1，其中Rk+1和Hk+1分别是k+1时刻的系统观测噪声协方差矩阵和观测矩阵；
步骤十四：根据Pk+1＝(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1；
步骤十五：根据
Figure FDA0002400369390000064
计算k+1时刻的状态估计值
Figure FDA0002400369390000065
步骤十六：根据
Figure FDA0002400369390000066
的计算结果修正SINS输出，重置扩展卡尔曼滤波器状态，重新执行步骤三至骤十。
CN202010144829.4A 2020-03-04 2020-03-04 一种无源模式下的sins/usbl组合导航定位方法 Active CN111721284B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010144829.4A CN111721284B (zh) 2020-03-04 2020-03-04 一种无源模式下的sins/usbl组合导航定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010144829.4A CN111721284B (zh) 2020-03-04 2020-03-04 一种无源模式下的sins/usbl组合导航定位方法

Publications (2)

Publication Number Publication Date
CN111721284A true CN111721284A (zh) 2020-09-29
CN111721284B CN111721284B (zh) 2021-08-31

Family

ID=72564072

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010144829.4A Active CN111721284B (zh) 2020-03-04 2020-03-04 一种无源模式下的sins/usbl组合导航定位方法

Country Status (1)

Country Link
CN (1) CN111721284B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113155134A (zh) * 2021-04-27 2021-07-23 哈尔滨工程大学 一种基于惯性信息辅助的水声信道跟踪与预测方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106546956A (zh) * 2016-10-25 2017-03-29 中国海洋石油总公司 一种超短基线接收基阵基元位置的精确标定方法
CN109029460A (zh) * 2018-08-03 2018-12-18 国家深海基地管理中心 深海运载器对水面监控平台测距的导航方法、系统及装置
CN109738902A (zh) * 2019-03-06 2019-05-10 哈尔滨工程大学 一种基于同步信标模式的水下高速目标高精度自主声学导航方法
CN110082706A (zh) * 2019-04-23 2019-08-02 哈尔滨工程大学 一种基于时延差和相位差且适用于时钟异步的水下单信标导航方法
CN110132281A (zh) * 2019-05-21 2019-08-16 哈尔滨工程大学 一种基于询问应答模式的水下高速目标高精度自主声学导航方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106546956A (zh) * 2016-10-25 2017-03-29 中国海洋石油总公司 一种超短基线接收基阵基元位置的精确标定方法
CN109029460A (zh) * 2018-08-03 2018-12-18 国家深海基地管理中心 深海运载器对水面监控平台测距的导航方法、系统及装置
CN109738902A (zh) * 2019-03-06 2019-05-10 哈尔滨工程大学 一种基于同步信标模式的水下高速目标高精度自主声学导航方法
CN110082706A (zh) * 2019-04-23 2019-08-02 哈尔滨工程大学 一种基于时延差和相位差且适用于时钟异步的水下单信标导航方法
CN110132281A (zh) * 2019-05-21 2019-08-16 哈尔滨工程大学 一种基于询问应答模式的水下高速目标高精度自主声学导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
詹飞等: "众核处理架构在水下航行器相位编码脉冲回波检测中的应用 ", 《声学学报》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113155134A (zh) * 2021-04-27 2021-07-23 哈尔滨工程大学 一种基于惯性信息辅助的水声信道跟踪与预测方法

Also Published As

Publication number Publication date
CN111721284B (zh) 2021-08-31

Similar Documents

Publication Publication Date Title
CN109443379B (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN111380518B (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN111595348B (zh) 一种自主水下航行器组合导航系统的主从式协同定位方法
CN105823480A (zh) 基于单信标的水下移动目标定位算法
CN110567454B (zh) 一种复杂环境下sins/dvl紧组合导航方法
CN108444476B (zh) 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法
CN109507706B (zh) 一种gps信号丢失的预测定位方法
CN111982105B (zh) 一种基于sins/lbl紧组合的水下导航定位方法及系统
CN112284384A (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
CN110389318B (zh) 一种基于立体六元阵的水下移动平台定位系统及方法
CN111380519B (zh) 一种超短基线/捷联惯性导航松组合的导航误差校正方法
CN113155134B (zh) 一种基于惯性信息辅助的水声信道跟踪与预测方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN110207698B (zh) 一种极区格网惯导/超短基线紧组合导航方法
CN111721284B (zh) 一种无源模式下的sins/usbl组合导航定位方法
CN112083425B (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN114265047A (zh) 一种大潜深auv的定位基阵联合标校方法
Liu et al. SINS/DVL integrated system with current and misalignment estimation for midwater navigation
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN108759864A (zh) 基于单应答器的水下捷联惯导系统初始对准方法
CN111380520B (zh) 一种引入径向速度的sins/usbl松组合导航定位方法
CN111964684B (zh) 一种基于sins/lbl紧组合的水下导航混合定位方法及系统
CN114111840B (zh) 一种基于组合导航的dvl误差参数在线标定方法

Legal Events

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