CN109324330B - 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 - Google Patents
基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 Download PDFInfo
- Publication number
- CN109324330B CN109324330B CN201811087892.8A CN201811087892A CN109324330B CN 109324330 B CN109324330 B CN 109324330B CN 201811087892 A CN201811087892 A CN 201811087892A CN 109324330 B CN109324330 B CN 109324330B
- Authority
- CN
- China
- Prior art keywords
- error
- equation
- state
- matrix
- transponder
- 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
- 238000001914 filtration Methods 0.000 title claims abstract description 68
- 238000000034 method Methods 0.000 title claims abstract description 28
- 238000005259 measurement Methods 0.000 claims abstract description 33
- 238000004364 calculation method Methods 0.000 claims abstract description 19
- 238000012546 transfer Methods 0.000 claims abstract description 11
- 238000006243 chemical reaction Methods 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 71
- 239000013598 vector Substances 0.000 claims description 64
- 238000004422 calculation algorithm Methods 0.000 claims description 13
- 230000007704 transition Effects 0.000 claims description 10
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 5
- 239000006185 dispersion Substances 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 238000004458 analytical method Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 238000003491 array Methods 0.000 claims 1
- 230000003287 optical effect Effects 0.000 claims 1
- 230000009897 systematic effect Effects 0.000 claims 1
- 230000006872 improvement Effects 0.000 description 3
- 238000001514 detection method Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
Images
Classifications
-
- 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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection
-
- 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/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/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
- 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/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- 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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
- G01S15/06—Systems determining the position data of a target
- G01S15/46—Indirect determination of position data
-
- 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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
- G01S15/06—Systems determining the position data of a target
- G01S15/46—Indirect determination of position data
- G01S2015/465—Indirect determination of position data by Trilateration, i.e. two transducers determine separately the distance to a target, whereby with the knowledge of the baseline length, i.e. the distance between the transducers, the position data of the target is determined
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Acoustics & Sound (AREA)
- Navigation (AREA)
Abstract
本发明提供的是一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法,特别适用于水下设备的定位。本发明由超短基线水声定位***USBL和捷联惯性导航***SINS组成,采用混合无导数扩展卡尔曼滤波HDEKF进行组合导航。超短基线***通过计算超声波信号在应答器和水听器之间单向传播的时间,得到两者之间的斜距测量值,并通过坐标转换公式得到观测方程。再根据捷联惯导***的误差传递公式建立误差状态方程。最后进行混合无导数卡尔曼滤波,使用标准线性卡尔曼滤波进行时间更新,使用无导数扩展滤波进行量测更新。本发明能有效提高USBL/INS组合导航***的导航精度和稳定性,并减少实时计算量。
Description
技术领域
本发明涉及一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法,特别适用于水下设备的定位,且该算法比一般扩展卡尔曼滤波计算量小。
背景技术
随着科技的发展,人们日益看重对海洋资源的开发,对水下设备也越来越来感兴趣。水下设备广泛应用于各类水中探测工作,比如环境检测,港口的水下检查,地质和生物调查等。常用的水下设备,包括各类水中机器人和潜艇。水下设备广阔的应用前景,引起了各国科学家的极大关注。
水下设备与陆地设备一样,需要一个精确的导航***来提供姿态,速度,位置等导航信息。GPS和INS组合导航***,是各类空中飞行器和陆地设备中最常用的导航***,但是由于水中GPS信号的衰减,针对水下设备需要使用不同的导航设备,如超短基线***USBL。USBL***是一种水声定位***,传统的USBL***一般包含安装在载体上的水听器基阵和安装在固定位置的应答器,可以通过水听器基阵和应答器之间的斜距差进行定位。但是USBL***容易受到水中环境的影响,如螺旋桨产生的干扰等,因此一个独立的USBL***一般不足以精确地进行导航。USBL***多作为辅助***,与INS***进行组合导航。
扩展卡尔曼滤波EKF常用于各类紧组合导航***中。但是因为EKF需要利用一阶泰勒展开来计算雅可比矩阵,因此对于某些非线性***来说,该方法将十分复杂而难以应用。为了克服EKF的缺点,科学家提出了无迹卡尔曼滤波UKF,UKF具有二阶甚至更高的精度,如今已经广泛用于各类非线性***中。基于Sigma点的UKF虽然避免了计算雅可比矩阵,且比EKF具有更高的精度。但是无迹卡尔曼滤波计算量巨大,难以用于实时性要求较高的组合导航***中。而且在惯性组合导航***中,***的非线性特性并不强,所以UKF的表现并不比EKF好很多。因此本发明使用了一种由Brendan M.发明的无导数卡尔曼滤波DEKF,针对一些特殊的非线性***,该算法可以很好地替代EKF和UKF,只需要计算n+1个预测向量,具有更高的精度和更小的计算量,为USBL/INS组合导航***提供一种新的研究方法。
发明内容
本发明的目的是为了提高USBL/SINS组合导航***的导航精度和稳定性,并在保证精度的前提下,减少实时计算量,提供一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法。
为达到上述目的,本发明采用的方法是一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法,由超短基线水声定位***USBL和捷联惯性导航***SINS组成,采用混合无导数扩展卡尔曼滤波HDEKF进行组合导航,通过下列步骤实现:
(1)、超短基线***由安装在载体上的水听器基阵和放置在海底已知位置的应答器组成,水听器基阵由4个水听器构成,通过基阵测量超声波信号从声源到应答器,再从应答器回到第i个水听器的往返传播时间tRTTi,计算出应答器和水听器之间单向传播的时间tri,最后根据水速算出应答器与该水听器之间的斜距值ρri;
(2)、根据捷联惯导***的误差传递方程,以东北天方向上的姿态误差角φE,φN,φU,东北天方向上的速度误差δvE,δvN,δvU,经度和纬度误差δL,δλ,高度误差δh以及沿载体坐标系x,y,z轴上的陀螺仪随机漂移εx,εy,εz和加速度计零偏▽x,▽y,▽z作为状态量,建立15维的状态向量,根据姿态误差方差,速度误差方程和位置误差方程建立误差状态方程,再根据基于坐标转换的超短基线导航解算方程建立观测方程;
(3)、根据步骤(2)中建立的误差状态方程和观测方程建立基本的扩展卡尔曼滤波方程,再根据扩展卡尔曼滤波滤波方程推导无导数扩展卡尔曼滤波;最后进行混合滤波,使用标准线性卡尔曼滤波进行时间更新,使用无导数扩展卡尔曼滤波进行量测更新。
作为本发明的一种改进,所述的步骤(1)中的计算应答器和水听器之间的斜距测量值方法如下:
(1-1)、通过基阵测量超声波信号从声源到应答器的时间,再从应答器回到基阵上第i个水听器的往返时间,记为tRTTi;
(1-2)、忽略声源和水听器之间的时差,应答器和第i个水听器之间的声波单向传播时间tri,可表示为tri=tRTTi-td-<tRTT>/2,其中,td为应答器延迟时间,<tRTT>为所有水听器测得的声波往返时间的平均值;
(1-3)、最后考虑水速以及噪声计算得到应答器与该水听器之间的斜距测量值ρri=vptri+η,vp为水速,η为测量噪声。
作为本发明的一种改进,步骤(2)中建立误差状态方程和量测方程的方法如下:
(2-1)、选取东北天坐标系作为导航坐标系,载体坐标系中x轴沿水下载体横轴指向右舷,y轴沿载体纵轴指向前,z轴垂直于x与y轴所确定的平面构成右手坐标系,以惯性导航***的误差量作为滤波器的状态变量,即选取东北天方向上的姿态误差角φE,φN,φU,东北天方向上的速度误差δvE,δvN,δvU,经度和纬度误差δL,δλ,高度误差δh以及沿载体坐标系x,y,z轴上的陀螺仪随机漂移εx,εy,εz和加速度计零偏▽x,▽y,▽z作为状态量,则有状态量:
X=[φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,εx,εy,εz,▽x,▽y,▽z]T
误差状态方程为:
其中,F(t)为状态转移矩阵,G(t)为噪声分配矩阵
根据捷联惯导***的误差传递规律,建立姿态、速度和位置误差方程:
姿态误差方程如下:
速度误差方程如下:
位置误差方程如下:
其中,δvE,δvN,δvU为东北天三个方向的速度误差,RM为子午圈主曲率半径,RN为卯酉圈主曲率半径
加速度计和陀螺仪的误差模型可以看作随机常值误差加上白噪声。所以加速度计误差和陀螺仪误差描述如下:
▽a=▽af+wa
εg=εgf+wg
▽a为加速度计零偏,εg为陀螺仪随机漂移,▽af,εgf为随机常值误差,wa,wg为零均值高斯白噪声,方差分别为Qa,Qg;
(2-2)、根据基于坐标转换的超短基线导航解算算法,建立量测方程,应答器在载体坐标系的位置tb可表示为te表示应答器在地球坐标系的位置,pe为安装在载体上的捷联惯导***的重心在地球坐标系的位置矢量,为载体坐标系到地球坐标系的旋转矩阵;建立基阵坐标系u系,u系原点与超短基线基阵几何中心重合,x,y,z轴与载体坐标系b系的x,y,z轴平行,4个水听器在u系的位置矢量和满足:
应答器与第i个水听器之间的距离ρi表示为:
式中,tu表示为应答器在u系的位置矢量
当忽略声学基阵的偏移和安装误差角等误差项时,b系与u系重合,得:
以第i个水听器和第j个水听器的斜距差作为量测量:
Z=[ρI1-ρI2 ρI1-ρI3…ρIj-1-ρIj]T
因此量测方程为δρij=ρIi-ρIj。
作为本发明的一种改进,步骤(3)中建立基本的扩展卡尔曼滤波方程,并进行混合无导数扩展卡尔曼滤波的方法如下:
(3-1)一个基本的离散时间非线性***描述如下:
xk+1=f(xk,uk)+wk
zk+1=h(zk,uk)+vk
xk为状态量,zk为观测量,wk和vk为协方差矩阵,期望和方差如下
E(wk)=0
var(vk)=Qk
E(wk)=0
var(vk)=Rk
所建立的扩展卡尔曼滤波基本公式如下:
状态一步预测:
状态一步预测均方差:
滤波增益:
状态估计量:
均方差估计量:
Pk=(I-KkHk)Pk/k-1
一般的扩展卡尔曼滤波方程中的状态转移矩阵和量测矩阵由下式求得:
其中传递矩阵和量测矩阵的计算一般具有一阶精度;
(3-2)无导数扩展卡尔曼滤波利用了无迹卡尔曼滤波的原理,不需要进行泰勒展开,避免了一阶截断误差。而且与无迹卡尔曼滤波相比计算量更小,只需要计算n+1个构造得到的预测向量,具体滤波过程如下:
其中,Vxi为n+1个期望为均方误差矩阵Pxx的离散向量,即E[ΔxiΔxi T]=Pxx,Δxi为矩阵的n+1个列向量,即n是状态向量x的维数,α是一个固定参数,1≤α<∞,取决于所使用的非线性模型,与估计量平均值的离散程度成正比,与非线性模型的阶数成反比
计算量测均方误差阵PZZ,协方差阵PXZ:
(3-3)通过分析,可以发现捷联惯导***的误差状态方程是可以线性化的,但超短基线和捷联惯导组合***的量测方程是非线性的,为了简化算法并减少运算量,本专利提出了混合无导数滤波,结合了线性估计和非线性估计算法的优点,在混合滤波中,使用标准线性卡尔曼滤波进行时间更新,使用无导数扩展卡尔曼滤波进行量测更新。
有益效果:
本发明是一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法,弥补了SINS长时间工作误差发散的问题,满足水下设备长时间工作的要求,并提高了定位精度。以应答器和水听器之间的斜距公式作为观测方程,其中考虑到坐标转换方程等问题,保证了超短基线***计算结果的精确度。利用混合无导数扩展卡尔曼滤波算法,没有使用泰勒展开,避免了传统扩展卡尔曼滤波算法存在的一阶截断误差。并且使用了混合滤波,大大减少了计算量,提高了算法实时性。
附图说明
图1为本发明的整体流程图;
图2为本发明中USBL***坐标系转换示意图。
具体实施方式
下面结合附图对本发明进一步详细说明,但是本发明的保护范围不局限于所述实施例。
如图1所示,本发明由超短基线水声定位***USBL和捷联惯性导航***SINS组成,采用混合无导数扩展卡尔曼滤波HDEKF进行组合导航,具体实现步骤如下::
步骤1:超短基线***由安装在载体上的水听器基阵和放置在海底已知位置的应答器组成,水听器基阵由4个水听器构成,通过基阵测量超声波信号从声源到应答器,再从应答器回到第i个水听器的往返传播时间tRTTi,表示为:
tp为声源到应答器的真实时间,tri为应答器到第i个水听器的时间,εt和εc为应答器噪声和水听器接收噪声,εdi为额外的微分误差,应答延迟时间td一般为已知,为取整函数,round(·)为四舍五入函数
假设当接受和返回信号时,载体处于静止状态(一般速度较低),此时可以忽略发射器和接收器之间的时差,因此应答器和接收器之间的单向传播时间可以通过减去延迟时间和平均循环时间的一半得到:
tri=tRTTi-td-<tRTT>/2
其中,td为应答器延迟时间,<tRTT>为所有水听器测得的声波往返时间的平均值
最后根据水速算出应答器和水听器之间的斜距测量值:
ρri=vptri+η
其中,η为测量噪声。
步骤2:选取东北天坐标系作为导航坐标系,载体坐标系x轴沿水下载体横轴指向右舷,y轴沿载体纵轴指向前,z轴垂直于x与y轴所确定的平面构成右手坐标系。以惯性导航***的误差量作为滤波器的状态变量,即选取东北天方向上的姿态误差角φE,φN,φU,东北天方向上的速度误差δvE,δvN,δvU,经度和纬度误差δL,δλ,高度误差δh以及沿载体坐标系x,y,z轴上的陀螺仪随机漂移εx,εy,εz和x,y,z轴上的加速度计零偏▽x,▽y,▽z作为状态量,则有状态量:
X=[φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,εx,εy,εz,▽x,▽y,▽z]T
误差状态方程为:
其中,F(t)为状态转移矩阵,G(t)为噪声分配矩阵
根据捷联惯导***的误差传递规律,建立姿态、速度和位置误差方程:
姿态误差方程如下:
速度误差方程如下:
位置误差方程如下:
其中,δvE,δvN,δvU为东北天三个方向的速度误差,RM为子午圈主曲率半径,RN为卯酉圈主曲率半径
加速度计和陀螺仪的误差模型可以看作随机常值误差加上高斯白噪声。所以加速度计误差和陀螺仪误差描述如下:
▽a=▽af+wa
εg=εgf+wg
▽a为加速度计零偏,εg为陀螺仪随机漂移,▽af,εgf为随机常值误差,wa,wg为零均值高斯白噪声,方差分别为Qa,Qg;
根据误差方程整理可得:
***噪声为:
W(t)=[wa,wg]T
噪声分配矩阵为:
状态转移矩阵为:
其中:
可以看出捷联惯性导航***的误差状态方程是线性的。
根据基于坐标转换的超短基线导航解算算法,建立量测方程
如图2所示,根据坐标变换关系,应答器在载体坐标系的位置,表示为
建立基阵坐标系u系,u系原点与超短基线基阵几何中心重合,x,y,z轴与载体坐标系b系的x,y,z轴平行,4个水听器在u系的位置矢量和满足:
应答器与第i个水听器之间的斜距可表示为:
式中,tu表示为应答器在u系的位置
当忽略声学基阵的偏移和安装误差角等误差项时,b系与u系重合,上式可以简化为:
根据斜距公式,利用捷联惯导***误差补偿公式,建立量测方程
位置误差补偿公式如下:
和姿态误差补偿公式如下:
带入斜距公式中,则可得量测方程为:
以第i个水听器和第j个水听器的斜距差作为量测量:
Z=[ρI1-ρI2 ρI1-ρI3…ρIj-1-ρIj]T
因此量测方程为δρij=ρIi-ρIj。
步骤3:利用误差状态方程和观测方程建立基本的扩展卡尔曼滤波方程,根据基本滤波方程进行无导数扩展卡尔曼滤波。
一个基本的离散时间非线性***描述如下:
xk+1=f(xk,uk)+wk
zk+1=f(zk,uk)+vk
xk为状态量,zk为观测量,wk和vk为均方差矩阵,期望和方差如下
E(wk)=0
var(vk)=Qk
E(wk)=0
var(vk)=Rk
所建立的扩展卡尔曼滤波基本公式如下:
状态一步预测:
状态一步预测均方差:
滤波增益:
状态估计量:
均方差估计量:
Pk=(I-KkHk)Pk/k-1
一般的扩展卡尔曼滤波方程中的状态转移矩阵和量测矩阵由下式求得
其中传递矩阵和量测矩阵的计算一般具有一阶精度;
无导数扩展卡尔曼滤波利用了无迹卡尔曼滤波的原理,不需要进行泰勒展开,避免了一阶截断误差。而且与无迹卡尔曼滤波相比计算量更小,只需要计算n+1个构造得到的预测向量,具体滤波过程如下:
其中,Vxi为n+1个期望为均方误差矩阵Pxx的离散向量,即E[ΔxiΔxi T]=Pxx,Δxi为矩阵的n+1个列向量,即n是状态向量x的维数;在实际计算时,由于计算精度有限,需要对矩阵进行规范化,转换为对称矩阵,具体公式如下:
其中是α一个固定参数,1≤α<∞,取决于所使用的非线性模型。与估计量平均值的离散程度成正比,与非线性模型的阶数成反比。
计算量测均方误差阵PZZ,协方差阵PXZ:
通过分析,可以发现捷联惯导***的误差模型是线性的,但超短基线和捷联惯导组合***的量测方程是非线性的。为了简化算法并减少运算量,本专利提出的混合无导数滤波,结合了线性估计和非线性估计算法的优点。在混合滤波中,使用标准线性卡尔曼滤波进行时间更新,使用无导数扩展卡尔曼滤波进行量测更新。混合滤波的具体过程如下:
利用基本的状态预测方程和均方差预测方程进行时间更新,方程如下:
量测更新部分利用无导数扩展卡尔曼滤波。
计算量测均方误差阵PZZ,协方差阵PXZ:
Claims (3)
1.一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法,其特征在于:由超短基线水声定位***USBL和捷联惯性导航***SINS组成,采用混合无导数扩展卡尔曼滤波HDEKF进行组合导航,通过下列步骤实现:
(1)、超短基线水声定位***由安装在载体上的水听器基阵和放置在海底已知位置的应答器组成,水听器基阵由4个水听器构成,通过基阵测量超声波信号从声源到应答器,再从应答器回到第i个水听器的往返传播时间tRTTi,计算出应答器和水听器之间单向传播的时间tri,最后根据水速算出应答器与该水听器之间的斜距值ρri;
(2)、根据捷联惯性导***的误差传递方程,以东北天方向上的姿态误差角φE,φN,φU,东北天方向上的速度误差δvE,δvN,δvU,经度和纬度误差δL,δλ,高度误差δh以及沿载体坐标系x,y,z轴上的陀螺仪随机漂移εx,εy,εz和加速度计零偏建立15维的状态向量,根据姿态误差方差,速度误差方程和位置误差方程建立误差状态方程,再根据基于坐标转换的超短基线导航解算方程建立量测方程;
(3)、根据步骤(2)中建立的误差状态方程和观测方程建立基本的扩展卡尔曼滤波方程,再根据扩展卡尔曼滤波方程推导无导数扩展卡尔曼滤波方程;最后进行混合滤波,使用标准线性卡尔曼滤波进行时间更新,使用无导数扩展卡尔曼滤波进行量测更新;
步骤(3)的具体步骤如下:
(3-1)建立扩展卡尔曼滤波基本方程一个基本的离散时间非线性***描述如下:
xk+1=f(xk,uk)+wk
zk+1=h(zk,uk)+vk
xk为状态量,zk为观测量,wk为***噪声向量和vk为量测噪声向量,期望和方差如下:
E(wk)=0
var(vk)=Qk
E(wk)=0
var(vk)=Rk
所建立的扩展卡尔曼滤波基本公式如下:
状态一步预测:
状态一步预测均方差:
滤波增益:
状态估计量:
均方差估计量:
Pk=(I-KkHk)Pk/k-1
一般的扩展卡尔曼滤波方程中的状态转移矩阵和量测矩阵由下式求得:
其中传递矩阵和量测矩阵的计算一般具有一阶精度;
(3-2)无导数扩展卡尔曼滤波利用了无迹卡尔曼滤波的原理,不需要进行泰勒展开,避免了一阶截断误差。而且与无迹卡尔曼滤波相比计算量更小,只需要计算n+1个构造得到的预测向量,具体滤波过程如下:
其中,Vxi为n+1个期望为均方误差矩阵Pxx的离散向量,即E[ΔxiΔxi T]=Pxx,Δxi为矩阵的n+1个列向量,即n是状态向量x的维数,α是一个固定参数,1≤α<∞,取决于所使用的非线性模型,与估计量平均值的离散程度成正比,与非线性模型的阶数成反比;
计算量测均方误差阵PZZ,协方差阵PXZ:
(3-3)通过分析,可以发现捷联惯导***的误差状态方程是可以线性化的,但超短基线和捷联惯导组合***的量测方程是非线性的,为了简化算法并减少运算量,所述混合无导数扩展卡尔曼滤波,结合了线性估计和非线性估计算法的优点,在混合滤波中,使用标准线性卡尔曼滤波进行时间更新,使用无导数扩展卡尔曼滤波进行量测更新。
2.根据权利要求1所述的基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法,其特征在于,步骤(1)的具体步骤如下:
(1-1)、通过基阵测量超声波信号从声源到应答器的时间,再从应答器回到基阵上第i个水听器的往返时间,记为tRTTi;
(1-2)、忽略声源和水听器之间的时差,应答器和第i个水听器之间的超声波信号单向传播时间tri,可表示为tri=tRTTi-td-<tRTT>/2,其中,td为应答器延迟时间,<tRTT>为所有水听器测得的声波往返时间的平均值;
(1-3)、最后考虑水速以及噪声计算得到应答器与该水听器之间的斜距测量值ρri=vptri+η,vp为水速,η为测量噪声。
3.根据权利要求1所述的基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法,其特征在于,步骤(2)的具体步骤如下:
(2-1)、选取东北天坐标系作为导航坐标系,载体坐标系中x轴沿水下载体横轴指向右舷,y轴沿载体纵轴指向前,z轴垂直于x与y轴所确定的平面构成右手坐标系,以惯性导航***的误差量作为滤波器的状态变量,即选取东北天方向上的姿态误差角φE,φN,φU,东北天方向上的速度误差δvE,δvN,δvU,经度和纬度误差δL,δλ,高度误差δh以及沿载体坐标系x,y,z轴上的陀螺仪随机漂移εx,εy,εz和加速度计零偏作为状态量,则有状态量:
误差状态方程为:
其中,F(t)为状态转移矩阵,G(t)为噪声分配矩阵
根据捷联惯导***的误差传递规律,建立姿态、速度和位置误差方程:
姿态误差方程如下:
速度误差方程如下:
位置误差方程如下:
其中,δvE,δvN,δvU为东北天三个方向的速度误差,RM为子午圈主曲率半径,RN为卯酉圈主曲率半径;
加速度计和陀螺仪的误差模型可以看作随机常值误差加上高斯白噪声,所以加速度计误差和陀螺仪误差描述如下:
εg=εgf+wg
根据误差方程整理可得:
***噪声为:
W(t)=[wa,wg]T
噪声分配矩阵为:
状态转移矩阵为:
其中:
(2-2)、根据基于坐标转换的超短基线导航解算算法,建立量测方程,应答器在载体坐标系的位置tb可表示为te表示应答器在地球坐标系的位置,pe为安装在载体上的捷联惯导***的重心在地球坐标系的位置矢量,为载体坐标系到地球坐标系的旋转矩阵;建立基阵坐标系u系,u系原点与超短基线基阵几何中心重合,x,y,z轴与载体坐标系b系的x,y,z轴平行,4个水听器在u系的位置矢量和满足:
应答器与第i个水听器之间的距离ρi表示为:
式中,tu表示为应答器在u系的位置矢量
当忽略声学基阵的偏移和安装误差角等误差项时,b系与u系重合,得:
以第i个水听器和第j个水听器的斜距差作为量测量:
Z=[ρI1-ρI2 ρI1-ρI3 … ρIj-1-ρIj]T
因此量测方程为δρij=ρIi-ρIj。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811087892.8A CN109324330B (zh) | 2018-09-18 | 2018-09-18 | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811087892.8A CN109324330B (zh) | 2018-09-18 | 2018-09-18 | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109324330A CN109324330A (zh) | 2019-02-12 |
CN109324330B true CN109324330B (zh) | 2022-11-18 |
Family
ID=65264831
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811087892.8A Active CN109324330B (zh) | 2018-09-18 | 2018-09-18 | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109324330B (zh) |
Families Citing this family (27)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109828296A (zh) * | 2019-03-08 | 2019-05-31 | 哈尔滨工程大学 | 一种ins/usbl非线性紧组合综合校正方法 |
CN109974695B (zh) * | 2019-04-09 | 2022-08-26 | 东南大学 | 基于Krein空间的水面舰艇导航***的鲁棒自适应滤波方法 |
CN110132308B (zh) * | 2019-05-27 | 2022-04-29 | 东南大学 | 一种基于姿态确定的usbl安装误差角标定方法 |
CN110207694A (zh) * | 2019-05-27 | 2019-09-06 | 哈尔滨工程大学 | 一种基于相对位置信息的极区格网惯导/超短基线组合导航方法 |
CN110553653B (zh) * | 2019-08-23 | 2021-04-23 | 上海航天控制技术研究所 | 基于多源数据驱动的航天器轨道确定方法 |
CN110554359B (zh) * | 2019-09-11 | 2021-07-09 | 哈尔滨工程大学 | 一种融合长基线与单信标定位的海底飞行节点定位方法 |
CN110736457A (zh) * | 2019-11-12 | 2020-01-31 | 苏州工业职业技术学院 | 一种基于北斗、gps和sins的组合导航方法 |
CN111290007A (zh) * | 2020-02-27 | 2020-06-16 | 桂林电子科技大学 | 一种基于神经网络辅助的bds/sins组合导航方法和*** |
CN111578928B (zh) * | 2020-05-07 | 2022-04-05 | 北京邮电大学 | 一种基于多源融合定位***的定位方法及装置 |
CN111624552B (zh) * | 2020-05-25 | 2022-08-30 | 中国地质大学(武汉) | 一种基于声波渡越时间测量的地下管道定位***及方法 |
CN111829512B (zh) * | 2020-06-08 | 2024-04-09 | 中国航天空气动力技术研究院 | 一种基于多传感器数据融合的auv导航定位方法及*** |
CN111829511A (zh) * | 2020-06-08 | 2020-10-27 | 中国航天空气动力技术研究院 | 一种基于m估计的auv组合导航方法及*** |
CN111896008A (zh) * | 2020-08-20 | 2020-11-06 | 哈尔滨工程大学 | 一种改进的鲁棒无迹卡尔曼滤波组合导航方法 |
CN111964684B (zh) * | 2020-08-21 | 2023-11-17 | 运城学院 | 一种基于sins/lbl紧组合的水下导航混合定位方法及*** |
CN112083425B (zh) * | 2020-09-14 | 2024-03-12 | 湖南航天机电设备与特种材料研究所 | 一种引入径向速度的sins/lbl紧组合导航方法 |
CN112507281B (zh) * | 2020-11-19 | 2024-01-02 | 东南大学 | 一种基于双状态多因子抗差估计sins/dvl紧组合***的方法 |
CN113155156A (zh) * | 2021-04-27 | 2021-07-23 | 北京信息科技大学 | 运行信息的确定方法及装置、存储介质、电子装置 |
CN113280812B (zh) * | 2021-05-12 | 2023-06-02 | 中山大学 | 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置 |
CN113776525B (zh) * | 2021-09-01 | 2023-12-05 | 东南大学 | 一种基于斜距差匹配的惯性/单声源被动组合导航方法 |
CN114279443B (zh) * | 2021-10-29 | 2024-01-02 | 东南大学 | 一种基于最大熵自适应抗差估计的usbl斜距修正方法 |
CN114485678B (zh) * | 2021-12-31 | 2023-09-12 | 上海航天控制技术研究所 | 天地一体月面着陆导航方法 |
CN115112121B (zh) * | 2022-03-28 | 2023-03-03 | 浙江德清知路导航科技有限公司 | 基于数据与模型结合的多源融合定位方法、***及终端 |
CN114877892A (zh) * | 2022-07-11 | 2022-08-09 | 泉州通维科技有限责任公司 | 一种用于光伏机器人的融合定位方法 |
CN115060274B (zh) * | 2022-08-17 | 2022-11-18 | 南开大学 | 一种水下一体式自主导航装置及其初始对准方法 |
CN116222578B (zh) * | 2023-05-04 | 2023-08-29 | 山东大学 | 基于自适应滤波和最优平滑的水下组合导航方法及*** |
CN116337115B (zh) * | 2023-05-31 | 2023-08-29 | 山东大学 | 一种基于声呐标校惯性传感器的方法及*** |
CN117804444B (zh) * | 2024-01-08 | 2024-07-19 | 河海大学 | 基于ukf和滚动时域估计的水下机器人组合定位方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4941199B2 (ja) * | 2007-09-25 | 2012-05-30 | ヤマハ株式会社 | ナビゲーション装置 |
CN104316045B (zh) * | 2014-11-06 | 2017-06-16 | 东南大学 | 一种基于sins/lbl的auv水下交互辅助定位***及定位方法 |
CN104457754B (zh) * | 2014-12-19 | 2017-04-26 | 东南大学 | 一种基于sins/lbl紧组合的auv水下导航定位方法 |
CN106767793A (zh) * | 2017-01-19 | 2017-05-31 | 东南大学 | 一种基于sins/usbl紧组合的auv水下导航定位方法 |
-
2018
- 2018-09-18 CN CN201811087892.8A patent/CN109324330B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN109324330A (zh) | 2019-02-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109324330B (zh) | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 | |
CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN109556632B (zh) | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 | |
CN103744098B (zh) | 基于sins/dvl/gps的auv组合导航*** | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN106643709B (zh) | 一种海上运载体的组合导航方法及装置 | |
CN108594272B (zh) | 一种基于鲁棒卡尔曼滤波的抗欺骗干扰组合导航方法 | |
CN103697910B (zh) | 自主水下航行器多普勒计程仪安装误差的校正方法 | |
CN108594271B (zh) | 一种基于复合分层滤波的抗欺骗干扰的组合导航方法 | |
CN109724599A (zh) | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 | |
CN104316045A (zh) | 一种基于sins/lbl的auv水下交互辅助定位***及定位方法 | |
CN111829512B (zh) | 一种基于多传感器数据融合的auv导航定位方法及*** | |
CN106405592B (zh) | 车载北斗载波相位周跳检测与修复方法及*** | |
CN106679662A (zh) | 一种基于tma技术的水下机器人单信标组合导航方法 | |
CN110132308A (zh) | 一种基于姿态确定的usbl安装误差角标定方法 | |
CN111982105B (zh) | 一种基于sins/lbl紧组合的水下导航定位方法及*** | |
CN115096303B (zh) | 一种gnss多天线与ins紧组合定位定姿方法和设备 | |
CN104049269A (zh) | 一种基于激光测距和mems/gps组合导航***的目标导航测绘方法 | |
CN104061930B (zh) | 一种基于捷联惯性制导和多普勒计程仪的导航方法 | |
CN113834483A (zh) | 一种基于可观测度的惯性/偏振/地磁容错导航方法 | |
CA2256964C (en) | Method of locating hydrophones | |
Zhang et al. | A calibration method of ultra-short baseline installation error with large misalignment based on variational Bayesian unscented Kalman filter | |
CN106199668A (zh) | 一种级联式gnss/sins深组合导航方法 | |
KR20020080829A (ko) | 오차보정시스템을 구비하는 관성측정유닛-지피에스통합시스템과 미지정수 검색범위 축소방법 및 사이클 슬립검출방법, 및 그를 이용한 항체 위치, 속도,자세측정방법 | |
CN115307643A (zh) | 一种双应答器辅助的sins/usbl组合导航方法 |
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 |