CN114397644A - 一种基于四元数法的超短基线安装误差校准方法 - Google Patents

一种基于四元数法的超短基线安装误差校准方法 Download PDF

Info

Publication number
CN114397644A
CN114397644A CN202111588847.2A CN202111588847A CN114397644A CN 114397644 A CN114397644 A CN 114397644A CN 202111588847 A CN202111588847 A CN 202111588847A CN 114397644 A CN114397644 A CN 114397644A
Authority
CN
China
Prior art keywords
coordinate system
quaternion
coordinate
coordinates
installation 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.)
Pending
Application number
CN202111588847.2A
Other languages
English (en)
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.)
Shandong University of Science and Technology
Original Assignee
Shandong University of Science and Technology
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 Shandong University of Science and Technology filed Critical Shandong University of Science and Technology
Priority to CN202111588847.2A priority Critical patent/CN114397644A/zh
Publication of CN114397644A publication Critical patent/CN114397644A/zh
Pending legal-status Critical Current

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
    • 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

Abstract

本发明涉及一种超短基线安装误差校准算法，尤其为一种基于四元数法的超短基线安装误差校准方法，包括根据坐标变换公式和应答器在地理坐标系下的坐标，求解得到应答器在船坐标系下的坐标；根据四元数与旋转矩阵的变换关系，求解得到基阵坐标系向船坐标系转换的旋转矩阵；根据基阵坐标系向船坐标系转换的旋转矩阵并进行坐标变换和最小二乘原理可求解得到基阵坐标系与船坐标系之间的四元数参数；基于四元数法，求解得到坐标系旋转后的坐标；根据坐标变换公式，求解得到超短基线安装误差校准后的应答器地理坐标。本发明基于四元数法，利用最小二乘原理求解四元数微分方程得到姿态角，实现超短基线安装误差校准。

Description

一种基于四元数法的超短基线安装误差校准方法
技术领域
本发明涉及一种超短基线安装误差校准算法，具体为一种基于四元数法的超短基线安装误差校准方法。
背景技术
海洋测绘为我国海洋科学研究、海洋资源开发和海洋环境保护等重大需求提供数据支撑。超短基线水声定位技术是海洋测绘的关键技术，其系统通过集成声学定位系统、高精度惯性导航系统和全球导航卫星系统(Global Navigation Satellite System，GNSS)来获得目标地理位置。超短基线(Ultra Short Baseline，USBL)水声定位系统在海洋资源开发和勘探中的作用越来越重要、应用越来越广泛，对定位精度的要求也越来越高。
影响USBL定位精度的主要因素有系统自身误差、GNSS误差、噪声误差、海洋环境参数测量误差、声线弯曲误差以及换能器基阵安装误差等。其中，换能器基阵安装误差不容忽略，是USBL的主要误差源，会严重影响水下目标的定位精度，制约水下作业的应用范围，需在实际作业前进行精确校准。目前USBL基阵安装方法大多基于旋转矩阵求解其欧拉角提出的，但在俯仰角θ＝±90°时，用欧拉角计算可能会出现奇异点，导致数值不稳定，故欧拉角法不能全姿态工作。因此，针对以上问题本发明提出一种基于四元数法的超短基线安装误差校准方法。
发明内容
本发明的目的在于提供一种基于四元数法的超短基线安装误差校准方法，以解决上述背景技术中提出的问题，是上述设备的必要补充和支持，对提高水下目标定位精度具有重要意义。
为实现上述目的，本发明提供如下技术方案：
一种基于四元数法的超短基线安装误差校准方法，包括以下步骤：
步骤一：根据坐标变换公式，求解换能器基阵中心的地理坐标；
步骤二：根据距离交汇原理，建立观测方程；
步骤三：将步骤一的坐标变换公式代入步骤二的观测方程，根据最小二乘原理可估计到应答器在地理坐标系下的坐标和GNSS天线中心与USBL基阵中心的位置偏差；
步骤四：根据坐标变换公式和步骤四得到的应答器在地理坐标系下的坐标，求解得到应答器在船坐标系下的坐标；
步骤五：根据四元数与旋转矩阵的变换关系，可得基阵坐标系向船坐标系转换的旋转矩阵；
步骤六：进行坐标变换；
步骤七：根据基阵坐标系向船坐标系转换的旋转矩阵并进行坐标变换和最小二乘原理可求解得到基阵坐标系与船坐标系之间的四元数参数；
步骤八：基于四元数法，求解得到坐标系旋转后的坐标；
步骤九：根据坐标变换公式，求解得到超短基线安装误差校准后的应答器地理坐标。
作为本发明的优选，所述根据坐标变换公式，求解换能器基阵中心的地理坐标(XT，YT，ZT)具体为：
Figure BDA0003428474440000021
其中，(XG，YG，ZG)为测量船的地理坐标；RU为地理坐标系向船坐标系转换的旋转矩阵； (ΔX，ΔY，ΔZ)为GNSS天线中心与USBL基阵中心的位置偏差。
作为本发明的优选，所述根据距离交汇原理，建立观测方程具体为：
|XJ|＝|XD-XT|+V
其中，XJ为应答器在基阵坐标系下的坐标；XD为应答器在地理坐标系下的坐标；V为估计残差。
作为本发明的优选，所述求解得到应答器在船坐标系下的坐标(XC，YC，ZC)具体为：
Figure BDA0003428474440000022
作为本发明的优选，所述得基阵坐标系向船坐标系转换的旋转矩阵具体为：
Figure BDA0003428474440000031
作为本发明的优选，所述进行坐标变换具体为：
Figure BDA0003428474440000032
作为本发明的优选，所述求解得到坐标系旋转后的坐标具体为：
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]；
Figure BDA0003428474440000033
q*＝(0，-XJ，-YJ，-ZJ)。
作为本发明的优选，所述求解得到超短基线安装误差校准后的应答器地理坐标(X′D，Y′D，Z′D)具体为：
Figure BDA0003428474440000034
与现有技术相比，本发明的有益效果是：
本发明基于四元数法，利用最小二乘原理求解四元数微分方程得到姿态角，实现超短基线安装误差校准。本发明既克服了欧拉角法描述姿态存在奇异性的缺点，又降低了计算量，能够保证超短基线水声定位系统全姿态工作，显著提高系统定位精度。一种基于四元数法的超短基线安装误差校准算法具有良好的工程实用性和通用性。
附图说明
图1为本发明实施例中超短基线水声定位原理图；
图2为本发明实施例中超短基线安装误差示意图；
图3为本发明实施例中基于四元数法的超短基线安装误差校准流程图。
具体实施方式
下面将结合本发明实施例对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明提供的技术方案：
实施例1
参照图1-3，本发明优选实施例提供了一种基于选取某湖泊进行超短基线水声定位实验，采集本次算法所需实验数据。将换能器固定安装在测量船上，尽量保持换能器基阵中心坐标轴与测量船重心坐标轴平行一致，应答器放置在水下固定深度处，尽量保持应答器位置固定不变。测量船沿预设圆形航迹航行，船艏方向尽量保持不变，用声学定位方法测量得到应答器分别到x轴、y轴相邻阵元的相位差φ、ψ，用高精度差分GPS测量得到测量船位置的地理坐标(XG，YG，ZG)，与此同时记录测量时延、声线初始掠射角以及辅助传感器测得测量船的瞬时姿态。
步骤一：使用中国Argo实时资料中心下载的不同深度海水的电导率S、温度T、深度D数据，采用Chen Millero经验公式计算得到对应深度下的声速。
步骤二：根据超短基线定位原理，求解应答器在基阵坐标系下的坐标(XJ，YJ，ZJ)：
Figure BDA0003428474440000041
Figure BDA0003428474440000042
Figure BDA0003428474440000043
步骤三：对坐标数据中测量船地理坐标和应答器相对于基阵坐标进行统计处理，剔除异常数据点。
步骤四：根据坐标变换公式，求解换能器基阵中心的地理坐标(XT，YT，ZT)：
Figure BDA0003428474440000051
步骤五：根据距离交汇原理，建立观测方程：
|XJ|＝|XD-XT|+V
步骤六：将式(11)代入方程(12)，根据最小二乘原理可估计得到XD和ΔX。
步骤七：根据坐标变换公式和步骤四得到的XD，求解得到应答器在船坐标系下的坐标 (XC，YC，ZC)：
Figure BDA0003428474440000052
步骤八：根据四元数q＝[q0，q1，q2，q3]与旋转矩阵的变换关系，可得基阵坐标系向船坐标系转换的旋转矩阵RJ：
Figure BDA0003428474440000053
步骤九：根据坐标变换公式，可得：
Figure BDA0003428474440000054
步骤十：将式(14)代入公式(15)，根据最小二乘原理可求解得到基阵坐标系与船坐标系之间的四元数参数[q0，q1，q2，q3]。
步骤十一：基于四元数法，求解得到坐标系旋转后的坐标：
X′C＝qX′Jq-1
步骤十二：根据坐标变换公式，求解得到超短基线安装误差校准后的应答器地理坐标 (X′D，Y′D，Z′D)：
Figure BDA0003428474440000055
在本实施例的上述步骤中，基于四元数法，利用最小二乘原理求解四元数微分方程得到姿态角，实现超短基线安装误差校准。本发明既克服了欧拉角法描述姿态存在奇异性的缺点，又降低了计算量，能够保证超短基线水声定位系统全姿态工作，显著提高系统定位精度。一种基于四元数法的超短基线安装误差校准算法具有良好的工程实用性和通用性。
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的仅为本发明的优选例，并不用来限制本发明，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (8)

1.一种基于四元数法的超短基线安装误差校准方法，其特征在于：包括以下步骤：
步骤一：根据坐标变换公式，求解换能器基阵中心的地理坐标；
步骤二：根据距离交汇原理，建立观测方程；
步骤三：将步骤一的坐标变换公式代入步骤二的观测方程，根据最小二乘原理可估计得到应答器在地理坐标系下的坐标和GNSS天线中心与USBL基阵中心的位置偏差；
步骤四：根据坐标变换公式和步骤四得到的应答器在地理坐标系下的坐标，求解得到应答器在船坐标系下的坐标；
步骤五：根据四元数与旋转矩阵的变换关系，可得基阵坐标系向船坐标系转换的旋转矩阵；
步骤六：进行坐标变换；
步骤七：根据基阵坐标系向船坐标系转换的旋转矩阵并进行坐标变换和最小二乘原理可求解得到基阵坐标系与船坐标系之间的四元数参数；
步骤八：基于四元数法，求解得到坐标系旋转后的坐标；
步骤九：根据坐标变换公式，求解得到超短基线安装误差校准后的应答器地理坐标。
2.根据权利要求1所述的基于四元数法的超短基线安装误差校准方法，其特征在于：所述根据坐标变换公式，求解换能器基阵中心的地理坐标(XT，YT，ZT)具体为：
Figure FDA0003428474430000011
其中，(XG，YG，ZG)为测量船的地理坐标；RU为地理坐标系向船坐标系转换的旋转矩阵；(ΔX，ΔY，ΔZ)为GNSS天线中心与USBL基阵中心的位置偏差。
3.根据权利要求1所述的基于四元数法的超短基线安装误差校准方法，其特征在于：所述根据距离交汇原理，建立观测方程具体为：
|XJ|＝|XD-XT|+V
其中，XJ为应答器在基阵坐标系下的坐标；XD为应答器在地理坐标系下的坐标；V为估计残差。
4.根据权利要求1所述的基于四元数法的超短基线安装误差校准方法，其特征在于：所述求解得到应答器在船坐标系下的坐标(XC，YC，ZC)具体为：
Figure FDA0003428474430000021
5.根据权利要求1所述的基于四元数法的超短基线安装误差校准方法，其特征在于：所述得基阵坐标系向船坐标系转换的旋转矩阵具体为：
Figure FDA0003428474430000022
6.根据权利要求1所述的基于四元数法的超短基线安装误差校准方法，其特征在于：所述进行坐标变换具体为：
Figure FDA0003428474430000023
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]；
Figure FDA0003428474430000024
q*＝(0，-XJ，-YJ，-ZJ)。
8.根据权利要求6所述的基于四元数法的超短基线安装误差校准方法，其特征在于：所述求解得到超短基线安装误差校准后的应答器地理坐标(X′D，Y′D，Z′D)具体为：
Figure FDA0003428474430000025
CN202111588847.2A 2021-12-23 2021-12-23 一种基于四元数法的超短基线安装误差校准方法 Pending CN114397644A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111588847.2A CN114397644A (zh) 2021-12-23 2021-12-23 一种基于四元数法的超短基线安装误差校准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111588847.2A CN114397644A (zh) 2021-12-23 2021-12-23 一种基于四元数法的超短基线安装误差校准方法

Publications (1)

Publication Number Publication Date
CN114397644A true CN114397644A (zh) 2022-04-26

Family

ID=81226635

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111588847.2A Pending CN114397644A (zh) 2021-12-23 2021-12-23 一种基于四元数法的超短基线安装误差校准方法

Country Status (1)

Country Link
CN (1) CN114397644A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115685165A (zh) * 2022-10-31 2023-02-03 浙江大学 一种立体阵超短基线定位系统安装偏差校准方法及装置

Citations (4)

* Cited by examiner, † Cited by third party
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 交通运输部天津水运工程科学研究所 一种基于多信标的超短基线标定系统及标定方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
徐博;郝芮;王超;张勋;张娇;: "水下潜航器的惯导/超短基线/多普勒测速信息融合及容错验证", 光学精密工程, no. 09, 15 September 2017 (2017-09-15) *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115685165A (zh) * 2022-10-31 2023-02-03 浙江大学 一种立体阵超短基线定位系统安装偏差校准方法及装置

Similar Documents

Publication Publication Date Title
CN1837848A (zh) 用于超短基线声学定位系统的校准方法
CN111350214B (zh) 多波束水下钢管桩桩位测量方法
CN114397643B (zh) 一种基于超短基线水声定位系统的声线修正方法
CN112540371A (zh) 一种近底多波束坐标转换处理方法
CN114397644A (zh) 一种基于四元数法的超短基线安装误差校准方法
CN110926459A (zh) 一种多波束数据的处理方法、设备及其存储介质
CN110824430A (zh) 基于北斗定位系统的水下定位方法
CN115856898B (zh) 一种适应于全海深的面阵三维成像声呐点位归算方法
CN108413983A (zh) 一种sins/usbl一体化定位系统安装误差标定的机械方法
CN104965102B (zh) 一种有效提高声学多普勒流速剖面仪流向测量精度的方法
CN102818566A (zh) 一种船舶定位方法及装置
CN110275139B (zh) 一种基于旋转式基元复用的超短基线定位系统及方法
CN117008177B (zh) 基于一体化平台的海底控制点三维坐标标定方法
CN113253285B (zh) 定点式三维全景成像声呐系统升级为走航式系统的方法
Yu In-situ calibration of transceiver alignment for a high-precision USBL system
CN114234932A (zh) 一种获取海底控制点数据的水下导线测量方法及装置
CN109975760B (zh) 基于双矢量重构的基阵安装误差角高精度标定方法和装置
CN113156369B (zh) 一种海上声学相位中心校准方法
CN113064208B (zh) 一种高精度的极浅水目标磁探系统
Wang et al. Measurement error analysis of multibeam echosounder system mounted on the deep-sea autonomous underwater vehicle
JP6321914B2 (ja) 移動体情報算出装置、移動体情報取得装置、移動体、移動体情報算出方法、移動体情報取得方法、移動体情報算出プログラム、および移動体情報取得プログラム
CN112462322A (zh) 一种水下蛙人定位方法及定位系统
Hill Motion compensation for shipborne radars and lidars
CN117665782B (zh) 一种多探头多波束的实时声线弯曲改正方法
JPH11304486A (ja) 広域測位システムを使用した測量装置

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