CN114397644B - 一种基于四元数法的超短基线安装误差校准方法 - Google Patents
一种基于四元数法的超短基线安装误差校准方法 Download PDFInfo
- Publication number
- CN114397644B CN114397644B CN202111588847.2A CN202111588847A CN114397644B CN 114397644 B CN114397644 B CN 114397644B CN 202111588847 A CN202111588847 A CN 202111588847A CN 114397644 B CN114397644 B CN 114397644B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- quaternion
- matrix
- coordinates
- solving
- 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 49
- 238000009434 installation Methods 0.000 title claims abstract description 33
- 239000011159 matrix material Substances 0.000 claims abstract description 43
- 230000009466 transformation Effects 0.000 claims abstract description 29
- 238000005516 engineering process Methods 0.000 description 3
- 230000007547 defect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000013535 sea water Substances 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
- G06F17/12—Simultaneous equations, e.g. systems of linear equations
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Pure & Applied Mathematics (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Theoretical Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Remote Sensing (AREA)
- Operations Research (AREA)
- Algebra (AREA)
- Radar, Positioning & Navigation (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明涉及一种超短基线安装误差校准算法,尤其为一种基于四元数法的超短基线安装误差校准方法,包括根据坐标变换公式和应答器在地理坐标系下的坐标,求解得到应答器在船坐标系下的坐标;根据四元数与旋转矩阵的变换关系,求解得到基阵坐标系向船坐标系转换的旋转矩阵;根据基阵坐标系向船坐标系转换的旋转矩阵并进行坐标变换和最小二乘原理可求解得到基阵坐标系与船坐标系之间的四元数参数;基于四元数法,求解得到坐标系旋转后的坐标;根据坐标变换公式,求解得到超短基线安装误差校准后的应答器地理坐标。本发明基于四元数法,利用最小二乘原理求解四元数微分方程得到姿态角,实现超短基线安装误差校准。
Description
技术领域
本发明涉及一种超短基线安装误差校准算法,具体为一种基于四元数法的超短基线安装误差校准方法。
背景技术
海洋测绘为我国海洋科学研究、海洋资源开发和海洋环境保护等重大需求提供数据支撑。超短基线水声定位技术是海洋测绘的关键技术,其系统通过集成声学定位系统、高精度惯性导航系统和全球导航卫星系统(Global Navigation Satellite System,GNSS)来获得目标地理位置。超短基线(Ultra Short Baseline,USBL)水声定位系统在海洋资源开发和勘探中的作用越来越重要、应用越来越广泛,对定位精度的要求也越来越高。
影响USBL定位精度的主要因素有系统自身误差、GNSS误差、噪声误差、海洋环境参数测量误差、声线弯曲误差以及换能器基阵安装误差等。其中,换能器基阵安装误差不容忽略,是USBL的主要误差源,会严重影响水下目标的定位精度,制约水下作业的应用范围,需在实际作业前进行精确校准。目前USBL基阵安装方法大多基于旋转矩阵求解其欧拉角提出的,但在俯仰角θ=±90°时,用欧拉角计算可能会出现奇异点,导致数值不稳定,故欧拉角法不能全姿态工作。因此,针对以上问题本发明提出一种基于四元数法的超短基线安装误差校准方法。
发明内容
本发明的目的在于提供一种基于四元数法的超短基线安装误差校准方法,以解决上述背景技术中提出的问题,是上述设备的必要补充和支持,对提高水下目标定位精度具有重要意义。
为实现上述目的,本发明提供如下技术方案:
一种基于四元数法的超短基线安装误差校准方法,包括以下步骤:
步骤一:根据坐标变换公式,求解换能器基阵中心的地理坐标;
步骤二:根据距离交汇原理,建立观测方程;
步骤三:将步骤一的坐标变换公式代入步骤二的观测方程,根据最小二乘原理可估计到应答器在地理坐标系下的坐标和GNSS天线中心与USBL基阵中心的位置偏差;
步骤四:根据坐标变换公式和步骤四得到的应答器在地理坐标系下的坐标,求解得到应答器在船坐标系下的坐标;
步骤五:根据四元数与旋转矩阵的变换关系,可得基阵坐标系向船坐标系转换的旋转矩阵;
步骤六:进行坐标变换;
步骤七:根据基阵坐标系向船坐标系转换的旋转矩阵并进行坐标变换和最小二乘原理可求解得到基阵坐标系与船坐标系之间的四元数参数;
步骤八:基于四元数法,求解得到坐标系旋转后的坐标;
步骤九:根据坐标变换公式,求解得到超短基线安装误差校准后的应答器地理坐标。
作为本发明的优选,所述根据坐标变换公式,求解换能器基阵中心的地理坐标(XT,YT,ZT)具体为:
其中,(XG,YG,ZG)为测量船的地理坐标;RU为地理坐标系向船坐标系转换的旋转矩阵; (ΔX,ΔY,ΔZ)为GNSS天线中心与USBL基阵中心的位置偏差。
作为本发明的优选,所述根据距离交汇原理,建立观测方程具体为:
|XJ|=|XD-XT|+V
其中,XJ为应答器在基阵坐标系下的坐标;XD为应答器在地理坐标系下的坐标;V为估计残差。
作为本发明的优选,所述求解得到应答器在船坐标系下的坐标(XC,YC,ZC)具体为:
作为本发明的优选,所述得基阵坐标系向船坐标系转换的旋转矩阵具体为:
作为本发明的优选,所述进行坐标变换具体为:
作为本发明的优选,所述求解得到坐标系旋转后的坐标具体为:
X′C=qX′Jq-1
其中,X′C=[X′0,X′C,Y′C,Z′X],坐标(X′C,Y′C,Z′X)是XJ仅通过旋转变换得到的坐标;
X′J=[0,XJ,YJ,ZJ];q*=(0,-XJ,-YJ,-ZJ)。
作为本发明的优选,所述求解得到超短基线安装误差校准后的应答器地理坐标(X′D,Y′D,Z′D)具体为:
与现有技术相比,本发明的有益效果是:
本发明基于四元数法,利用最小二乘原理求解四元数微分方程得到姿态角,实现超短基线安装误差校准。本发明既克服了欧拉角法描述姿态存在奇异性的缺点,又降低了计算量,能够保证超短基线水声定位系统全姿态工作,显著提高系统定位精度。一种基于四元数法的超短基线安装误差校准算法具有良好的工程实用性和通用性。
附图说明
图1为本发明实施例中超短基线水声定位原理图;
图2为本发明实施例中超短基线安装误差示意图;
图3为本发明实施例中基于四元数法的超短基线安装误差校准流程图。
具体实施方式
下面将结合本发明实施例对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
本发明提供的技术方案:
实施例1
参照图1-3,本发明优选实施例提供了一种基于选取某湖泊进行超短基线水声定位实验,采集本次算法所需实验数据。将换能器固定安装在测量船上,尽量保持换能器基阵中心坐标轴与测量船重心坐标轴平行一致,应答器放置在水下固定深度处,尽量保持应答器位置固定不变。测量船沿预设圆形航迹航行,船艏方向尽量保持不变,用声学定位方法测量得到应答器分别到x轴、y轴相邻阵元的相位差φ、ψ,用高精度差分GPS测量得到测量船位置的地理坐标(XG,YG,ZG),与此同时记录测量时延、声线初始掠射角以及辅助传感器测得测量船的瞬时姿态。
步骤一:使用中国Argo实时资料中心下载的不同深度海水的电导率S、温度T、深度D数据,采用Chen Millero经验公式计算得到对应深度下的声速。
步骤二:根据超短基线定位原理,求解应答器在基阵坐标系下的坐标(XJ,YJ,ZJ):
步骤三:对坐标数据中测量船地理坐标和应答器相对于基阵坐标进行统计处理,剔除异常数据点。
步骤四:根据坐标变换公式,求解换能器基阵中心的地理坐标(XT,YT,ZT):
步骤五:根据距离交汇原理,建立观测方程:
|XJ|=|XD-XT|+V
步骤六:将式(11)代入方程(12),根据最小二乘原理可估计得到XD和ΔX。
步骤七:根据坐标变换公式和步骤四得到的XD,求解得到应答器在船坐标系下的坐标 (XC,YC,ZC):
步骤八:根据四元数q=[q0,q1,q2,q3]与旋转矩阵的变换关系,可得基阵坐标系向船坐标系转换的旋转矩阵RJ:
步骤九:根据坐标变换公式,可得:
步骤十:将式(14)代入公式(15),根据最小二乘原理可求解得到基阵坐标系与船坐标系之间的四元数参数[q0,q1,q2,q3]。
步骤十一:基于四元数法,求解得到坐标系旋转后的坐标:
X′C=qX′Jq-1
步骤十二:根据坐标变换公式,求解得到超短基线安装误差校准后的应答器地理坐标 (X′D,Y′D,Z′D):
在本实施例的上述步骤中,基于四元数法,利用最小二乘原理求解四元数微分方程得到姿态角,实现超短基线安装误差校准。本发明既克服了欧拉角法描述姿态存在奇异性的缺点,又降低了计算量,能够保证超短基线水声定位系统全姿态工作,显著提高系统定位精度。一种基于四元数法的超短基线安装误差校准算法具有良好的工程实用性和通用性。
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解,本发明不受上述实施例的限制,上述实施例和说明书中描述的仅为本发明的优选例,并不用来限制本发明,在不脱离本发明精神和范围的前提下,本发明还会有各种变化和改进,这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。
Claims (8)
1.一种基于四元数法的超短基线安装误差校准方法,其特征在于:包括以下步骤:
步骤一:根据坐标变换公式,求解换能器基阵中心的地理坐标;
步骤二:根据距离交汇原理,建立观测方程;
步骤三:将步骤一的坐标变换公式代入步骤二的观测方程,根据最小二乘原理可估计得到应答器在地理坐标系下的坐标和GNSS天线中心与USBL基阵中心的位置偏差;
步骤四:根据坐标变换公式和步骤四得到的应答器在地理坐标系下的坐标,求解得到应答器在船坐标系下的坐标;
步骤五:根据四元数与旋转矩阵的变换关系,可得基阵坐标系向船坐标系转换的旋转矩阵;
步骤六:进行坐标变换;
步骤七:根据基阵坐标系向船坐标系转换的旋转矩阵并进行坐标变换和最小二乘原理可求解得到基阵坐标系与船坐标系之间的四元数参数;
步骤八:基于四元数法,求解得到坐标系旋转后的坐标;
步骤九:根据坐标变换公式,求解得到超短基线安装误差校准后的应答器地理坐标。
2.根据权利要求1所述的基于四元数法的超短基线安装误差校准方法,其特征在于:所述根据坐标变换公式,求解换能器基阵中心的地理坐标(XT,YT,ZT)具体为:
其中,(XG,YG,ZG)为测量船的地理坐标;RU为地理坐标系向船坐标系转换的旋转矩阵;(ΔX,ΔY,ΔZ)为GNSS天线中心与USBL基阵中心的位置偏差。
3.根据权利要求1所述的基于四元数法的超短基线安装误差校准方法,其特征在于:所述根据距离交汇原理,建立观测方程具体为:
|XJ|=|XD-XT|+V
其中,XJ为应答器在基阵坐标系下的坐标;XD为应答器在地理坐标系下的坐标;V为估计残差。
4.根据权利要求1所述的基于四元数法的超短基线安装误差校准方法,其特征在于:所述求解得到应答器在船坐标系下的坐标(XC,YC,ZC)具体为:
5.根据权利要求1所述的基于四元数法的超短基线安装误差校准方法,其特征在于:所述得基阵坐标系向船坐标系转换的旋转矩阵具体为:
6.根据权利要求1所述的基于四元数法的超短基线安装误差校准方法,其特征在于:所述进行坐标变换具体为:
7.根据权利要求1所述的基于四元数法的超短基线安装误差校准方法,其特征在于:所述求解得到坐标系旋转后的坐标具体为:
X′C=qX′Jq-1
其中,X′C=[X′0,X′C,Y′C,Z′C],坐标(X′C,Y′C,Z′C)是XJ仅通过旋转变换得到的坐标;X′J=[0,XJ,YJ,ZJ];q*=(0,-XJ,-YJ,-ZJ)。
8.根据权利要求6所述的基于四元数法的超短基线安装误差校准方法,其特征在于:所述求解得到超短基线安装误差校准后的应答器地理坐标(X′D,Y′D,Z′D)具体为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111588847.2A CN114397644B (zh) | 2021-12-23 | 2021-12-23 | 一种基于四元数法的超短基线安装误差校准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111588847.2A CN114397644B (zh) | 2021-12-23 | 2021-12-23 | 一种基于四元数法的超短基线安装误差校准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114397644A CN114397644A (zh) | 2022-04-26 |
CN114397644B true CN114397644B (zh) | 2024-05-28 |
Family
ID=81226635
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111588847.2A Active CN114397644B (zh) | 2021-12-23 | 2021-12-23 | 一种基于四元数法的超短基线安装误差校准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114397644B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115685165A (zh) * | 2022-10-31 | 2023-02-03 | 浙江大学 | 一种立体阵超短基线定位系统安装偏差校准方法及装置 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
RU2011152152A (ru) * | 2011-12-20 | 2013-06-27 | Российская Федерация, от имени которой выступает Министерство промышленности и торговли РФ | Уточненное позиционирование в подводном навигационном пространстве, образованном произвольно расставленными гидроакустическими маяками-ответчиками |
CN106199519A (zh) * | 2016-07-26 | 2016-12-07 | 山东省科学院海洋仪器仪表研究所 | 一种超短基线五基元立体空间基阵及其水声定位方法 |
CN110132308A (zh) * | 2019-05-27 | 2019-08-16 | 东南大学 | 一种基于姿态确定的usbl安装误差角标定方法 |
CN110261824A (zh) * | 2019-07-15 | 2019-09-20 | 交通运输部天津水运工程科学研究所 | 一种基于多信标的超短基线标定系统及标定方法 |
-
2021
- 2021-12-23 CN CN202111588847.2A patent/CN114397644B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
RU2011152152A (ru) * | 2011-12-20 | 2013-06-27 | Российская Федерация, от имени которой выступает Министерство промышленности и торговли РФ | Уточненное позиционирование в подводном навигационном пространстве, образованном произвольно расставленными гидроакустическими маяками-ответчиками |
CN106199519A (zh) * | 2016-07-26 | 2016-12-07 | 山东省科学院海洋仪器仪表研究所 | 一种超短基线五基元立体空间基阵及其水声定位方法 |
CN110132308A (zh) * | 2019-05-27 | 2019-08-16 | 东南大学 | 一种基于姿态确定的usbl安装误差角标定方法 |
CN110261824A (zh) * | 2019-07-15 | 2019-09-20 | 交通运输部天津水运工程科学研究所 | 一种基于多信标的超短基线标定系统及标定方法 |
Non-Patent Citations (1)
Title |
---|
水下潜航器的惯导/超短基线/多普勒测速信息融合及容错验证;徐博;郝芮;王超;张勋;张娇;;光学精密工程;20170915(第09期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114397644A (zh) | 2022-04-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN100535683C (zh) | 用于超短基线声学定位系统的校准方法 | |
Jinwu et al. | Study on installation error analysis and calibration of acoustic transceiver array based on SINS/USBL integrated system | |
CN107990891B (zh) | 基于长基线和信标在线标定的水下机器人组合导航方法 | |
CN109613520B (zh) | 一种基于滤波的超短基线安装误差在线标定方法 | |
CN114397643B (zh) | 一种基于超短基线水声定位系统的声线修正方法 | |
CN111350214B (zh) | 多波束水下钢管桩桩位测量方法 | |
CN107390177A (zh) | 一种基于纯测向的被动水下声学定位方法 | |
CN114397644B (zh) | 一种基于四元数法的超短基线安装误差校准方法 | |
CN105547290A (zh) | 一种基于超短基线定位系统的从潜器导航方法 | |
CN112540371A (zh) | 一种近底多波束坐标转换处理方法 | |
Kinsey et al. | Towards in-situ calibration of gyro and doppler navigation sensors for precision underwater vehicle navigation | |
CN108562872B (zh) | 一种超短基线水下声学定位校准时异常值探测的方法 | |
CN104965102B (zh) | 一种有效提高声学多普勒流速剖面仪流向测量精度的方法 | |
Zhang et al. | Use of the Jiaolong manned submersible for accurate mapping of deep-sea topography and geomorphology | |
CN110441736B (zh) | 多关节水下无人潜行器变基线三维空间定位方法 | |
CN113591011B (zh) | 一种快速的多波束声速改正方法及实时处理终端 | |
CN117008177B (zh) | 基于一体化平台的海底控制点三维坐标标定方法 | |
Yu | In-situ calibration of transceiver alignment for a high-precision USBL system | |
CN116699581B (zh) | 基于深海潜水器的海底地形测量方法及装置 | |
Troni et al. | Field performance evaluation of new methods for in-situ calibration of attitude and Doppler sensors for underwater vehicle navigation | |
CN110793507B (zh) | 基于无人艇运动数据的海浪反演方法 | |
CN113156369B (zh) | 一种海上声学相位中心校准方法 | |
Wang et al. | Measurement error analysis of multibeam echosounder system mounted on the deep-sea autonomous underwater vehicle | |
Ding et al. | Underwater acoustic localization approach to an arbitrary distributed multi-element array | |
Liu et al. | An installation angle error calibration method in an Ultra-Short Baseline system based on a dual transponder |
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 |