CN109613520B - 一种基于滤波的超短基线安装误差在线标定方法 - Google Patents

一种基于滤波的超短基线安装误差在线标定方法 Download PDF

Info

Publication number
CN109613520B
CN109613520B CN201811532244.9A CN201811532244A CN109613520B CN 109613520 B CN109613520 B CN 109613520B CN 201811532244 A CN201811532244 A CN 201811532244A CN 109613520 B CN109613520 B CN 109613520B
Authority
CN
China
Prior art keywords
error
matrix
ultra
state
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
Application number
CN201811532244.9A
Other languages
English (en)
Other versions
CN109613520A (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.)
Southeast University
Original Assignee
Southeast 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 Southeast University filed Critical Southeast University
Priority to CN201811532244.9A priority Critical patent/CN109613520B/zh
Publication of CN109613520A publication Critical patent/CN109613520A/zh
Application granted granted Critical
Publication of CN109613520B publication Critical patent/CN109613520B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/52—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S15/00
    • G01S7/52004—Means for monitoring or calibrating
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于滤波的超短基线安装误差在线标定方法，步骤为：1、建立安装角误差以及杆臂误差的状态模型；2、结合RTK、姿态测量传感器的数据，以及超短基线的输出原始数据，建立系统的观测模型；3、通过状态模型与观测模型，对船体在水面的航行轨迹下进行误差状态的可观性分析，确保当前运动轨迹能够充分激励出误差状态；4、用卡尔曼滤波方程对误差进行实时的在线估计安装误差；5、将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度。本发明的有益效果为：基于滤波的方法可以同时在线的估计出USBL与船体之间的安装误差，方法易于实施。

Description

一种基于滤波的超短基线安装误差在线标定方法
技术领域
本发明属于导航技术领域，特别涉及一种基于滤波的超短基线安装误差在线标定方法。
背景技术
水下定位技术在海洋资源开发，海底光缆铺设等方面有着广泛的需求。水下定位精度的提高是一切海下开发活动以及海下科研工作展开的根本保障。由于水下特殊的环境，基于水声的定位技术是水下导航的主要方式之一。按照基线长短，又可以分为长基线水下定位系统(LBL,Long Base Line)，短基线水下定位系统(SBL,Short Base Line)和超短基线水下定位系统(USBL,Ultra Short Base Line)。在这三种声学系统中，USBL由于其部署方便和成本低等优点，被广泛应用于海洋领域的开发，尽管它的定位精度是这其中最低的一个。
USBL系统利用安装在船体的信号收发器以及位于海底的应答器进行定位。影响USBL系统定位精度的因素有很多，包括系统测量误差，海洋环境影响以及USBL与船体其他传感器的安装误差等。在长航时，高精度的导航中，安装误差不容忽略。通常USBL在安装时，可以通过光学仪器精确的测出它与船体之间的安装角以及杆臂误差。然而当其中一个传感器需要维护或修理时，安装位置会发生改变，因此，每一次传感器的变动都需要进行一次安装误差的标定。有效并且容易实施的标定方法对于航海中的导航至关重要。
现有的USBL安装误差海上标定需要载体执行一系列复杂的航行轨迹，而载体在海上由于环境原因无法理想的按预定轨迹航行。因此一种简便且高效的超短基线安装误差标定是提高水下定位精度的关键。
发明内容
发明目的：针对上述问题缺陷，本发明提供一种基于滤波的超短基线安装误差在线标定方法，根据船体安装的RTK以及姿态测量传感器测出的船体位置信息以及姿态信息，结合超短基线输出的斜距以及方位角信息作为观测数据，实时估计出超短基线相对于船体的安装误差。
技术方案：本发明提出一种基于滤波的超短基线安装误差在线标定方法，包括如下步骤：
步骤1：建立安装角误差以及杆臂误差的状态模型；
步骤2：结合RTK、姿态测量传感器的数据，以及超短基线的输出原始数据，建立系统的观测模型；
步骤3：通过步骤(1)中建立的状态模型与步骤(2)中建立的观测模型，对船体在水面的航行轨迹下进行误差状态的可观性分析，确保当前运动轨迹能够充分激励出误差状态；
步骤4：用卡尔曼滤波方程对误差进行实时的在线估计安装误差；
步骤5：将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度。
所述步骤1中建立安装角误差以及杆臂误差的状态模型具体为：
选取安装角误差以及杆臂误差为系统的状态量，
Xusbl＝[δθx δθy δθz δLx δLy δLz]
其中，δθx,δθy,δθz为角度安装误差，δLx,δLy,δLz为杆臂安装误差。
由于安装误差为常值，其微分方程如下：
Figure GDA0003812688860000021
可以得到系统状态方程为：
Figure GDA0003812688860000022
其中Fusbl(t)＝06×6。
所述步骤2中，结合RTK、姿态测量传感器的数据，以及超短基线的输出原始数据，建立系统的观测模型推导如下：
以USBL输出的斜距和方位角，以及用已知的应答器位置与船体位置，解算应答器相对于船心之间的斜距与方位角，两者的差值作为系统的观测量。
已知斜距与方位角为[α β R]。应答器在超短基线基阵坐标系中的相对位置坐标为[x y z]。两者有如下关系：
Figure GDA0003812688860000031
对其求偏微分可得：
Figure GDA0003812688860000032
其中，
Figure GDA0003812688860000033
下面计算已知绝对位置的应答器在船心坐标系(obxbybzb)中的相对位置坐标。应答器的绝对位置为pr＝[lr λr hr]，通过长基线的方式确定。GPS测得船心的绝对位置坐标为pc＝[lc λc hc]。其表示方法为经纬高。通过姿态测量系统得到的姿态转换矩阵为
Figure GDA0003812688860000034
则考虑USBL安装的角度误差与杆臂误差后的应答器在基阵坐标系中的相对位置关系为：
Figure GDA0003812688860000035
其中，
Figure GDA0003812688860000041
安装角误差的姿态矩阵，Lb是超短基线相对于船体在船心坐标系下的杆臂距离。Δpe是在地球直角坐标系(oexeyeze)下应答器位置相对于船体的位置的坐标表示。表示为
Figure GDA0003812688860000042
对上述公式求微分，由于状态只有安装误差，因此可得：
Figure GDA0003812688860000043
其中，[Δx Δy Δz]是应答器在USBL基阵坐标系下的位置误差，δθ＝[δθx δθy δθz]T是安装角误差，δL＝[δLx δLy δLz]T是杆臂误差。(·×)是反对称矩阵的表示。由于
Figure GDA0003812688860000044
是二阶小量，忽略它的影响。结合[xu yu zu]的表达式，可以得到
Figure GDA0003812688860000045
其中
Figure GDA0003812688860000046
因此，综合上述公式，系统量测方程为：
Figure GDA0003812688860000047
其中[αgps βgps Rgps]是根据已知应答器的位置坐标与GPS测得的船心坐标，计算得到的应答器相对于船心的斜距与方位角。观测噪声为V。
观测矩阵
Figure GDA0003812688860000051
因此系统量测方程为Zk＝HkX+V。
所述步骤3中，误差状态的可观性分析，其具体步骤为：
1)3.对待分析的导航系统进行分段处理，获取初始时间段的状态转移矩阵φ1与H1。
2)由φ1与H1计算系统总的可观性矩阵M1.
3)以此类推。依次计算r个时间段内的各状态转移矩阵φj与观测矩阵Hj，并求可观性矩阵
Figure GDA0003812688860000052
4)计算系统可观性矩阵Ms＝[M1 M2 ... Mr]
5)对矩阵Ms进行可观性分解，求出各个状态量对应的奇异值，状态量对应的奇异值反映了该状态的可观测度。
所述步骤4中，应用卡尔曼滤波方程进行实时估计安装误差，其具体方程为：
状态一步预测方程：
Figure GDA0003812688860000053
状态估值方程：
Figure GDA0003812688860000054
滤波增益方程：
Figure GDA0003812688860000055
一步预测均方误差方程：
Figure GDA0003812688860000056
估计均方误差方程：Pk＝[I-KkHk]Pk|k-1[I-KkHk]T+KkRkKk T。
其中，Φk,k-1表示k-1时刻到k时刻的状态转移矩阵，本文中Φk,k-1＝I6×6，I6×6是6维的单位矩阵。
Figure GDA0003812688860000057
表示k-1的滤波器的状态估计值。
Figure GDA0003812688860000058
表示利用
Figure GDA0003812688860000059
计算得到的对状态的一步预测。
Kk表示卡尔曼滤波器的增益。Zk表示系统的观测值，
Figure GDA00038126888600000510
表示在一步预测的基础上根据量测值计算得到的对k时刻系统状态的估计值。Rk表示观测噪声矩阵。
Pk|k-1为估计值
Figure GDA00038126888600000511
的均方误差阵。Pk-1表示k-1时刻状态估计值的均方误差阵。Pk表示当前时刻状态估计值的均方误差阵。I表示单位矩阵，其维度与系统状态量相同。
所述步骤5中，将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度。其具体步骤如下：
(5.1)水下潜航器装备超短基线定位系统，姿态测量传感器，并在水底安放应答器，利用长基线原理计算应答器在地理坐标系下的位置坐标
Figure GDA0003812688860000061
(5.2)潜航器在水下行驶过程中，超短基线系统测得并输出应答器相对于超短基线水听器基阵中心的斜距(R)与方位角(α,β)。
(5.3)传统的超短基线系统对潜航器的定位坐标表示如下：
Figure GDA0003812688860000062
其中，
Figure GDA0003812688860000063
为姿态矩阵，由姿态传感器测得，
Figure GDA0003812688860000064
为通过超短基线系统输出计算得到的潜航器在地理系下的位置坐标。
Figure GDA0003812688860000065
表示应答器在超短基线水听器基阵中心坐标系下的位置，通过斜距与方位角计算得到，具体公式如下：
Figure GDA0003812688860000066
(5.4)由于安装误差的存在，超短基线水听器基阵中心坐标系与船体的坐标系并不重合，上述公式存在较大误差，利用上述步骤(1)-(4)中估计出的安装角度误差与杆臂误差，补偿超短基线对载体的定位结果，得到如下关系：
Figure GDA0003812688860000067
其中，L是估计出的杆臂误差，L＝[Lx Ly Lz]表示在载体坐标系下超短基线中心相对于船心的三维杆臂距离。
Figure GDA0003812688860000068
表示安装误差角的姿态矩阵。
补偿安装误差后，将提高超短基线对水下载体的定位精度。
本发明采用上述技术方案，具有以下有益效果：
根据步骤1所建立的系统状态模型，本发明方案可以同时的估计出安装角度误差与杆臂误差，相比于现有的方案下先标定杆臂误差再标定角度误差，极大的提高了标定的效率。基于滤波的安装误差估计方法，可以在载体行驶过程中在线标定出安装误差并实时补偿到定位结果中，并且合理的噪声矩阵设置，可以避免现有最小二乘法对于噪声敏感的缺点，提高了估计精度。
附图说明
图1为具体实施例中USBL接收基阵安装误差角示意图。
具体实施方式
下面结合具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
一种基于滤波方法的水下超短基线安装误差在线标定方法，根据船体安装的RTK以及姿态测量传感器测出的船体位置信息以及姿态信息，结合超短基线输出的斜距以及方位角信息作为观测数据，实时估计出超短基线相对于船体的安装误差。图1是USBL接收基阵安装误差角示意图，标定过程包括如下具体步骤：
步骤1：建立安装角误差以及杆臂误差的状态模型：
选取安装角误差以及杆臂误差为系统的状态量，
Xusbl＝[δθx δθy δθz δLx δLy δLz]
其中，δθx,δθy,δθz为角度安装误差，δLx,δLy,δLz为杆臂安装误差。
由于安装误差为常值，其微分方程如下：
Figure GDA0003812688860000071
可以得到系统状态方程为：
Figure GDA0003812688860000072
其中Fusbl(t)＝06×6。
步骤2结合RTK、姿态测量传感器的数据，以及超短基线的输出原始数据，建立系统的观测模型：
以USBL输出的斜距和方位角，以及用已知的应答器位置与船体位置，解算应答器相对于船心之间的斜距与方位角，两者的差值作为系统的观测量。
已知斜距与方位角为[α β R]。应答器在超短基线基阵坐标系中的相对位置坐标为[x y z]。两者有如下关系：
Figure GDA0003812688860000081
对其求偏微分可得：
Figure GDA0003812688860000082
其中，
Figure GDA0003812688860000083
下面计算已知绝对位置的应答器在船心坐标系(obxbybzb)中的相对位置坐标。应答器的绝对位置为pr＝[lr λr hr]，通过长基线的方式确定。GPS测得船心的绝对位置坐标为pc＝[lc λc hc]。其表示方法为经纬高。通过姿态测量系统得到的姿态转换矩阵为
Figure GDA0003812688860000084
则考虑USBL安装的角度误差与杆臂误差后的应答器在基阵坐标系中的相对位置关系为：
Figure GDA0003812688860000091
其中，
Figure GDA0003812688860000092
安装角误差的姿态矩阵，Lb是超短基线相对于船体在船心坐标系下的杆臂距离。Δpe是在地球直角坐标系(oexeyeze)下应答器位置相对于船体的位置的坐标表示。表示为
Figure GDA0003812688860000093
对上述公式求微分，由于状态只有安装误差，因此可得：
Figure GDA0003812688860000094
其中，[Δx Δy Δz]是应答器在USBL基阵坐标系下的位置误差，δθ＝[δθx δθy δθz]T是安装角误差，δL＝[δLx δLy δLz]T是杆臂误差。(·×)是反对称矩阵的表示。由于
Figure GDA0003812688860000098
是二阶小量，忽略它的影响。结合[xu yu zu]的表达式，可以得到
Figure GDA0003812688860000095
其中
Figure GDA0003812688860000096
因此，综合上述公式，系统量测方程为：
Figure GDA0003812688860000097
其中[αgps βgps Rgps]是根据已知应答器的位置坐标与GPS测得的船心坐标，计算得到的应答器相对于船心的斜距与方位角。观测噪声为V。
观测矩阵
Figure GDA0003812688860000101
因此系统量测方程为Zk＝HkX+V。
步骤3对误差状态可观性分析，其具体步骤为：
1)对待分析的导航系统进行分段处理，获取初始时间段的状态转移矩阵φ1与H1。
2)由φ1与H1计算系统总的可观性矩阵M1.
3)以此类推。依次计算r个时间段内的各状态转移矩阵φj与观测矩阵Hj，并求可观性矩阵
Figure GDA0003812688860000102
4)计算系统可观性矩阵Ms＝[M1 M2 ... Mr]
5)对矩阵Ms进行可观性分解，求出各个状态量对应的奇异值，状态量对应的奇异值反映了该状态的可观测度。
步骤4应用卡尔曼滤波方程进行实时估计安装误差：
状态一步预测方程：
Figure GDA0003812688860000103
状态估值方程：
Figure GDA0003812688860000104
滤波增益方程：
Figure GDA0003812688860000105
一步预测均方误差方程：
Figure GDA0003812688860000106
估计均方误差方程：Pk＝[I-KkHk]Pk|k-1[I-KkHk]T+KkRkKk T。
其中，Φk,k-1表示k-1时刻到k时刻的状态转移矩阵，本文中Φk,k-1＝I6×6，I6×6是6维的单位矩阵。
Figure GDA0003812688860000107
表示k-1的滤波器的状态估计值。
Figure GDA0003812688860000108
表示利用
Figure GDA0003812688860000109
计算得到的对状态的一步预测。
Kk表示卡尔曼滤波器的增益。Zk表示系统的观测值，
Figure GDA00038126888600001010
表示在一步预测的基础上根据量测值计算得到的对k时刻系统状态的估计值。Rk表示观测噪声矩阵。
Pk|k-1为估计值
Figure GDA0003812688860000111
的均方误差阵。Pk-1表示k-1时刻状态估计值的均方误差阵。Pk表示当前时刻状态估计值的均方误差阵。I表示单位矩阵，其维度与系统状态量相同。
步骤5将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度
(1)水下潜航器装备超短基线定位系统，姿态测量传感器，并在水底安放应答器，利用长基线原理计算应答器在地理坐标系下的位置坐标
Figure GDA0003812688860000112
(2)潜航器在水下行驶过程中，超短基线系统测得并输出应答器相对于超短基线水听器基阵中心的斜距(R)与方位角(α,β)。
(3)传统的超短基线系统对潜航器的定位坐标表示如下：
Figure GDA0003812688860000113
其中，
Figure GDA0003812688860000114
为姿态矩阵，由姿态传感器测得，
Figure GDA0003812688860000115
为通过超短基线系统输出计算得到的潜航器在地理系下的位置坐标。
Figure GDA0003812688860000116
表示应答器在超短基线水听器基阵中心坐标系下的位置，通过斜距与方位角计算得到，具体公式如下：
Figure GDA0003812688860000117
(4)由于安装误差的存在，超短基线水听器基阵中心坐标系与船体的坐标系并不重合，上述公式存在较大误差，利用上述步骤(1)-(5)中估计出的安装角度误差与杆臂误差，补偿超短基线对载体的定位结果，得到如下关系：
Figure GDA0003812688860000118
其中，L是估计出的杆臂误差，L＝[Lx Ly Lz]表示在载体坐标系下超短基线中心相对于船心的三维杆臂距离。
Figure GDA0003812688860000119
表示安装误差角的姿态矩阵。
补偿安装误差后，将提高超短基线对水下载体的定位精度。

Claims (1)

1.一种基于滤波的超短基线安装误差在线标定方法，其特征在于，包括如下步骤：
(1)建立安装角误差以及杆臂误差的状态模型；
(2)结合RTK、姿态测量传感器的数据，以及超短基线的输出原始数据，建立系统的观测模型；
(3)通过步骤(1)中建立的状态模型与步骤(2)中建立的观测模型，对船体在水面的航行轨迹下进行误差状态的可观性分析，确保当前运动轨迹能够充分激励出误差状态；
(4)用卡尔曼滤波方程对误差进行实时的在线估计安装误差；
(5)将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度；
所述步骤(1)中建立安装角误差以及杆臂误差的状态模型的具体步骤如下：
选取安装角误差以及杆臂误差为系统的状态量，
Xusbl＝[δθx δθy δθz δLx δLy δLz]
其中，δθx,δθy,δθz为角度安装误差，δLx,δLy,δLz为杆臂安装误差；
由于安装误差为常值，其微分方程如下：
Figure FDA0003812688850000011
可以得到系统状态方程为：
Figure FDA0003812688850000012
其中Fusbl(t)＝06×6；
所述步骤(2)中建立系统的观测模型的具体步骤如下：
以USBL输出的斜距和方位角，以及用已知的应答器位置与船体位置，解算应答器相对于船心之间的斜距与方位角，两者的差值作为系统的观测量；
其中斜距与方位角为[αβR]；应答器在超短基线基阵坐标系中的相对位置坐标为[x yz]；两者有如下关系：
Figure FDA0003812688850000021
对其求偏微分可得：
Figure FDA0003812688850000022
其中，
Figure FDA0003812688850000023
计算已知绝对位置的应答器在船心坐标系(obxbybzb)中的相对位置坐标；应答器的绝对位置为pr＝[lr λr hr]，通过长基线的方式确定；GPS测得船心的绝对位置坐标为pc＝[lc λchc]；其表示方法为经纬高；通过姿态测量系统得到的姿态转换矩阵为
Figure FDA0003812688850000024
则考虑USBL安装的角度误差与杆臂误差后的应答器在基阵坐标系中的相对位置关系为：
Figure FDA0003812688850000025
其中，
Figure FDA0003812688850000026
表示安装角误差的姿态矩阵，Lb是超短基线相对于船体在船心坐标系下的杆臂距离；Δpe是在地球直角坐标系(oexeyeze)下应答器位置相对于船体的位置的坐标表示，表示为
Figure FDA0003812688850000031
对上述公式求微分，由于状态只有安装误差，因此可得：
Figure FDA0003812688850000032
其中，[Δx Δy Δz]是应答器在USBL基阵坐标系下的位置误差，δθ＝[δθx δθy δθz]T是安装角误差，δL＝[δLx δLy δLz]T是杆臂误差；(·×)是反对称矩阵的表示；由于
Figure FDA0003812688850000033
是二阶小量，忽略它的影响；结合[xu yu zu]的表达式，得到
Figure FDA0003812688850000034
其中
Figure FDA0003812688850000035
因此，综合上述公式，系统量测方程为：
Figure FDA0003812688850000036
其中[αgps βgps Rgps]是根据已知应答器的位置坐标与GPS测得的船心坐标，计算得到的应答器相对于船心的斜距与方位角；观测噪声为V；
观测矩阵
Figure FDA0003812688850000037
因此系统量测方程为Zk＝HkX+V；
所述步骤(3)中误差状态的可观性分析，其具体步骤为：
(4.1)对待分析的导航系统进行分段处理，获取初始时间段的状态转移矩阵φ1与H1；
(4.2)由φ1与H1计算系统总的可观性矩阵M1；
(4.3)以此类推；依次计算r个时间段内的各状态转移矩阵φj与观测矩阵Hj，并求可观性矩阵
Figure FDA0003812688850000041
(4.4)计算系统可观性矩阵Ms＝[M1 M2...Mr]；
(4.5)对矩阵Ms进行可观性分解，求出各个状态量对应的奇异值，状态量对应的奇异值反映了该状态的可观测度；
所述步骤(4)中用卡尔曼滤波方程对误差进行实时的在线估计安装误差的具体步骤方程：
状态一步预测方程：
Figure FDA0003812688850000042
状态估值方程：
Figure FDA0003812688850000043
滤波增益方程：
Figure FDA0003812688850000044
一步预测均方误差方程：
Figure FDA0003812688850000045
估计均方误差方程：Pk＝[I-KkHk]Pk|k-1[I-KkHk]T+KkRkKk T；
其中，Φk,k-1表示k-1时刻到k时刻的状态转移矩阵，Φk,k-1＝I6×6，I6×6是6维的单位矩阵；
Figure FDA0003812688850000046
表示k-1的滤波器的状态估计值；
Figure FDA0003812688850000047
表示利用
Figure FDA0003812688850000048
计算得到的对状态的一步预测；
Kk表示卡尔曼滤波器的增益；Zk表示系统的观测值，
Figure FDA0003812688850000049
表示在一步预测的基础上根据量测值计算得到的对k时刻系统状态的估计值；Rk表示观测噪声矩阵；Hk为观测矩阵；
Pk|k-1为估计值
Figure FDA00038126888500000410
的均方误差阵；Pk-1表示k-1时刻状态估计值的均方误差阵；Pk表示当前时刻状态估计值的均方误差阵；I表示单位矩阵，其维度与系统状态量相同；
所述步骤(5)中将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度；具体步骤如下：
(5.1)水下潜航器装备超短基线定位系统，姿态测量传感器，并在水底安放应答器，利用长基线原理计算应答器在地理坐标系下的位置坐标
Figure FDA0003812688850000051
(5.2)潜航器在水下行驶过程中，超短基线系统测得并输出应答器相对于超短基线水听器基阵中心的斜距R与方位角α,β；
(5.3)传统的超短基线系统对潜航器的定位坐标表示如下：
Figure FDA0003812688850000052
其中，
Figure FDA0003812688850000053
为姿态矩阵，由姿态传感器测得，
Figure FDA0003812688850000054
为通过超短基线系统输出计算得到的潜航器在地理系下的位置坐标；
Figure FDA0003812688850000055
表示应答器在超短基线水听器基阵中心坐标系下的位置，通过斜距与方位角计算得到，具体公式如下：
Figure FDA0003812688850000056
(5.4)由于安装误差的存在，超短基线水听器基阵中心坐标系与船体的坐标系并不重合，上述公式存在较大误差，利用上述步骤(1)-(4)中估计出的安装角度误差与杆臂误差，补偿超短基线对载体的定位结果，得到如下关系：
Figure FDA0003812688850000057
其中，L是估计出的杆臂误差，L＝[Lx Ly Lz]表示在载体坐标系下超短基线中心相对于船心的三维杆臂距离；
Figure FDA0003812688850000058
表示安装误差角的姿态矩阵；
补偿安装误差后，将提高超短基线对水下载体的定位精度。
CN201811532244.9A 2018-12-14 2018-12-14 一种基于滤波的超短基线安装误差在线标定方法 Active CN109613520B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811532244.9A CN109613520B (zh) 2018-12-14 2018-12-14 一种基于滤波的超短基线安装误差在线标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811532244.9A CN109613520B (zh) 2018-12-14 2018-12-14 一种基于滤波的超短基线安装误差在线标定方法

Publications (2)

Publication Number Publication Date
CN109613520A CN109613520A (zh) 2019-04-12
CN109613520B true CN109613520B (zh) 2022-10-14

Family

ID=66008645

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811532244.9A Active CN109613520B (zh) 2018-12-14 2018-12-14 一种基于滤波的超短基线安装误差在线标定方法

Country Status (1)

Country Link
CN (1) CN109613520B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109975760B (zh) * 2019-03-28 2020-11-03 东南大学 基于双矢量重构的基阵安装误差角高精度标定方法和装置
CN110057383B (zh) * 2019-05-05 2023-01-03 哈尔滨工程大学 一种auv推位导航系统杆臂误差标校方法
CN110294080B (zh) * 2019-05-13 2021-06-01 自然资源部第一海洋研究所 一种利用超短基线实现水下精确作业的方法
CN110568407B (zh) * 2019-09-05 2023-06-27 武汉理工大学 一种基于超短基线和航位推算的水下导航定位方法
CN111025333B (zh) * 2019-12-05 2021-11-16 中国人民解放军63620部队 基于导航卫星信号短基线定姿的安装偏差标定与修正方法
CN111625009B (zh) * 2020-06-05 2021-06-25 上海中车艾森迪海洋装备有限公司 水下机器人在布放回收过程中的自动运动控制方法及装置
CN114265047B (zh) * 2021-12-31 2022-11-01 哈尔滨工程大学 一种大潜深auv的定位基阵联合标校方法
CN116295538B (zh) * 2023-05-24 2023-08-01 河海大学 一种基于改进粒子滤波的usbl安装误差标定方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH07260922A (ja) * 1994-03-25 1995-10-13 Oki Electric Ind Co Ltd 水中航走体位置検出装置
CN1837848A (zh) * 2006-04-27 2006-09-27 国家海洋局第一海洋研究所 用于超短基线声学定位系统的校准方法
CN103927442A (zh) * 2014-04-15 2014-07-16 哈尔滨工程大学 一种基于测角变换的超短基线安装角度误差抗粗差校准方法
CN106483498A (zh) * 2015-09-01 2017-03-08 北京自动化控制设备研究所 一种sinsusbl紧耦合算法
CN108490396A (zh) * 2018-03-16 2018-09-04 哈尔滨工业大学（威海） 一种基于水声信号卡尔曼滤波的超短基线定位方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH07260922A (ja) * 1994-03-25 1995-10-13 Oki Electric Ind Co Ltd 水中航走体位置検出装置
CN1837848A (zh) * 2006-04-27 2006-09-27 国家海洋局第一海洋研究所 用于超短基线声学定位系统的校准方法
CN103927442A (zh) * 2014-04-15 2014-07-16 哈尔滨工程大学 一种基于测角变换的超短基线安装角度误差抗粗差校准方法
CN106483498A (zh) * 2015-09-01 2017-03-08 北京自动化控制设备研究所 一种sinsusbl紧耦合算法
CN108490396A (zh) * 2018-03-16 2018-09-04 哈尔滨工业大学（威海） 一种基于水声信号卡尔曼滤波的超短基线定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
《水下定位系统传感器安装误差标定技术研究》;李铁 等;《传感器与微系统》;20101231;第29卷(第6期);第50-54页 *

Also Published As

Publication number Publication date
CN109613520A (zh) 2019-04-12

Similar Documents

Publication Publication Date Title
CN109613520B (zh) 一种基于滤波的超短基线安装误差在线标定方法
CN107990891B (zh) 基于长基线和信标在线标定的水下机器人组合导航方法
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN105823480B (zh) 基于单信标的水下移动目标定位算法
CN106679662B (zh) 一种基于tma技术的水下机器人单信标组合导航方法
CN110132308B (zh) 一种基于姿态确定的usbl安装误差角标定方法
CN107390177B (zh) 一种基于纯测向的被动水下声学定位方法
CN102829777A (zh) 自主式水下机器人组合导航系统及方法
CN105547290B (zh) 一种基于超短基线定位系统的从潜器导航方法
CN109738902B (zh) 一种基于同步信标模式的水下高速目标高精度自主声学导航方法
CN103968838B (zh) 一种基于极坐标系的auv曲线运动状态下的协同定位方法
CN110765686B (zh) 利用有限波段海底地形进行船载声呐测深测线设计的方法
CN103697910A (zh) 自主水下航行器多普勒计程仪安装误差的校正方法
CN109085655B (zh) 一种水下平台重力测量方案与验证方法
CN110837086B (zh) 一种基于侧扫声呐的海底目标定位方法和定位系统
CN108871379B (zh) 一种dvl测速误差在线标定方法
Zhang et al. Use of the Jiaolong manned submersible for accurate mapping of deep-sea topography and geomorphology
CN110389318B (zh) 一种基于立体六元阵的水下移动平台定位系统及方法
CN111220146B (zh) 一种基于高斯过程回归学习的水下地形匹配定位方法
US20210270607A1 (en) Calibration of a Magnetometer in a Towed Object Telemetry Unit Based on Turn Data
CN108761470B (zh) 一种基于拖缆形态方程解析的目标定位方法
CN112147578B (zh) 一种高精度深水发射阵及多元垂直接收阵阵元定位系统与方法
CN116699581B (zh) 基于深海潜水器的海底地形测量方法及装置
Yu In-situ calibration of transceiver alignment for a high-precision USBL system
CN114397644B (zh) 一种基于四元数法的超短基线安装误差校准方法

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