CN110132308B - 一种基于姿态确定的usbl安装误差角标定方法 - Google Patents

一种基于姿态确定的usbl安装误差角标定方法 Download PDF

Info

Publication number
CN110132308B
CN110132308B CN201910447543.0A CN201910447543A CN110132308B CN 110132308 B CN110132308 B CN 110132308B CN 201910447543 A CN201910447543 A CN 201910447543A CN 110132308 B CN110132308 B CN 110132308B
Authority
CN
China
Prior art keywords
usbl
installation error
vector
attitude
error angle
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
CN201910447543.0A
Other languages
English (en)
Other versions
CN110132308A (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 CN201910447543.0A priority Critical patent/CN110132308B/zh
Publication of CN110132308A publication Critical patent/CN110132308A/zh
Application granted granted Critical
Publication of CN110132308B publication Critical patent/CN110132308B/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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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/40—Correcting position, velocity or attitude
    • 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
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

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

Abstract

一种基于姿态确定的USBL安装误差角标定方法，超短基线定位系统在水下航行器定位中有着重要的应用。而USBL定位系统的安装误差角对USBL定位精度有重要影响。传统的标定方法对安装误差角的估计精度有限，且对路线要求较高。本申请SINS和USBL在应用过程中固联在一起时，USBL的安装误差角是固定不变的，然后利用姿态确定的思想来完成USBL安装误差角的标定。首先建立基于安装误差角矩阵的矢量观测模型，通过构造观测向量和参考向量，该方法可以实时标定SINS和USBL的安装误差角。该方法的优点在于：该方法能实时标定出USBL安装误差角，并且操作简单，对标定路线没有具体要求；USBL定位系统在水听器接收基阵坐标系下的定位精度越高，该方法的标定精度也就越高。

Description

一种基于姿态确定的USBL安装误差角标定方法
技术领域
本发明涉及水下定位技术领域，特别是涉及一种基于姿态确定的USBL安装误差角标定方法。
背景技术
自主水下航行器(AUV)是人类进行海洋资源调查研究不可缺少的载体，水下定位技术是水下机器人水下作业的关键技术之一。海洋的介质环境决定了声音比光波和电磁波更适合作为水下定位技术的传播载体。在现有的水下声学定位技术中，USBL系统以其结构简单、操作方便、体积小等优点，在海洋资源调查和科研领域发挥着越来越重要的作用。
影响USBL定位系统定位精度的主要因素是系统误差、海洋环境参数引起的测量误差和接收阵列的安装误差。安装误差角是指水听器阵列架与惯性导航体架不一致造成的误差。超短基线接收基阵的安装误差是超短基线定位系统的主要误差源，在实际使用前必须对其进行精确标定。因此，研究超短定位系统安装误差角的标定方法，对于提高超短基线系统定位精度具有重要的意义。
虽然安装误差的标定对提高超短基线定位系统的定位精度起着重要作用，但安装误差角标定方法相关发表的文献却很少。2007年，哈尔滨工程大学郑翠娥提出了一种利用最小二乘法估计超短基线定位系统安装误差的标定方法，并进行了海上试验。然而，当该方法用最小二乘法求解安装误差角时，需要对矩阵进行求逆运算，这样会使得该算法存在奇异值的情况。2013年，Chen Hsin-Hung设计了一种迭代标定方法，通过分析三个角度值分别对USBL定位的影响来估计安装误差角。但这种标定方法原理过于理想，标定过程中对路线要求过于复杂。因此，本文提出的标定方法可以实时的标定出超短基线定位系统的安装误差角，且对标定路线没有要求。
发明内容
为了解决现有标定方法技术中标定精度的不足以及标定路线要求高的缺点，本发明提供一种基于姿态确定的USBL安装误差角标定方法，通过具有RTK定位精度的GPS位置来修正SINS的姿态误差，在事先通过LBL计算得到应答器位置的条件下，构造基于USBL安装误差角矩阵的矢量观测模型，利用姿态确定方法求解出USBL安装误差角，为达此目的，本发明提供一种基于姿态确定的USBL安装误差角标定方法，其具体步骤如下：
(1)超短基线系统由安装在AUV上的四个水听器组成的正方形基阵和布放在海底的应答器组成，换能器发送声波信号，应答器接收信号后经过一段延时发送应答信号，四个水听器接收到应答信号后，根据声波到达时间确定水听器与应答器间的斜距信息，以及声波与水听器基阵间的两个方位角信息；
(2)利用具有RTK固定解定位精度的GPS位置信息与捷联惯性导航系统SINS进行松组合算法，以此来修正SINS的姿态角误差，从而为USBL标定方法提供更加精确的姿态矩阵
Figure GDA0003518295820000021
(3)构造基于USBL安装误差角姿态矩阵的矢量观测模型，利用超短基线定位系统构造水声基阵坐标系下的位置矢量，利用SINS/RTK组合导航系统的位置和姿态构造载体系下的位置矢量；
(4)根据步骤(3)构造的基于USBL安装误差角姿态矩阵的矢量观测模型，利用姿态确定方法求解出USBL安装误差角对应的姿态四元数，从而完成USBL安装误差角的实时标定。
作为本发明进一步改进，所述步骤(2)中基于RTK固定解的SINS/GPS松组合导航方法的具体步骤为：
(21)状态量X由以下15个变量组成；
Figure GDA0003518295820000024
其中，φE，φN，φU是捷联惯导的三个失准角，δVE，δVN，δVU是东北天三个方向上的速度误差，δL，δλ，δh是捷联惯导的三个位置误差在导航系下的描述，εx，εy，εz是陀螺的三个轴向漂移，
Figure GDA0003518295820000022
是加速度计三个轴向的零偏误差；
(22)系统状态方程为；
Figure GDA0003518295820000023
其中，X(t)为系统的状态向量，F(t)是惯导系统误差状态方程状态转移矩阵，W(t)是关于捷联惯导系统的噪声向量；
(23)以RTK位置与SINS位置差作为观测量，建立系统观测方程为；
Z(t)＝PSINS-PGPS＝H(t)X(t)+V(t)
其中，Z(t)为观测方程的量测向量，PSINS为SINS得到的位置，PGPS为RTK位置，H(t)为观测方程的量测矩阵，V(t)为观测方程的量测噪声向量；其中量测矩阵的具体表达式为；
H(t)＝[03×3 03×3 I3×3 03×3 03×3]
其中I3x3表示3阶单位矩阵。
作为本发明进一步改进，所述步骤(3)中构造基于USBL安装误差角姿态矩阵的矢量观测模型的具体步骤为：
(31)超短基线坐标系记为a系，捷联惯导载体坐标系记为b系，ra表示应答器在超短基线坐标系下的相对位置，可以通过超短基线定位系统的斜距及方位角信息求解得到，具体计算公式为，
Figure GDA0003518295820000031
其中R表示应答器与水听器接收基阵之间的斜距信息，α，β分别为水听器与应答器之间的声波信号与超短基线坐标系x，y轴的方位角；
(32)利用不同时刻计算得到的ra(t)作差，得到应答器在a系下相对位置的变化量
Figure GDA0003518295820000032
具体计算公式为：
Figure GDA0003518295820000033
(33)LBL计算得到的应答器在导航系下的位置为
Figure GDA0003518295820000034
利用SINS位置和姿态，求解得到应答器在b系下的相对位置rb，具体计算方式为：
Figure GDA0003518295820000035
其中，
Figure GDA0003518295820000036
表示b系与n系之间的姿态矩阵，
Figure GDA0003518295820000037
表示SINS在导航系下的位置。
(34)建立ra与rb之间的关系表达式为：
Figure GDA0003518295820000038
其中，
Figure GDA0003518295820000039
表示b系与a系之间的姿态矩阵，Lb表示超短基线坐标系原点与载体系原点之间的杆臂值在载体系下的投影；
(35)通过将不同时刻计算得到的rb(t)-Lb作差，得到应答器在b系下相对位置的变化量
Figure GDA00035182958200000310
Figure GDA00035182958200000311
(36)通过步骤(32)构造的水声基阵坐标系下的位置矢量
Figure GDA00035182958200000312
及步骤(35)构造的载体坐标系下的位置矢量
Figure GDA00035182958200000313
建立基于USBL安装误差角的矢量观测模型如下：
Figure GDA00035182958200000314
作为本发明进一步改进，所述步骤(4)中利用姿态确定方法求解USBL安装误差角对应的姿态四元数的具体步骤为：
(41)通过步骤(36)建立的基于USBL安装误差角的矢量观测模型，建立对应的基于USBL安装误差姿态四元数的矢量观测模型：
Figure GDA0003518295820000041
其中，
Figure GDA0003518295820000042
为USBL安装误差角姿态矩阵
Figure GDA0003518295820000043
对应的姿态四元数。
Figure GDA0003518295820000044
表示四元数乘法运算。
(42)将观测矢量和参考矢量简记为
Figure GDA0003518295820000045
并构造两个矩阵如下式：
Figure GDA0003518295820000046
Figure GDA0003518295820000047
(43)通过步骤(42)构造的两个矩阵，步骤(14)中的矢量观测模型转化为
Figure GDA0003518295820000048
上式中构造的观测矢量模型可以通过最小二乘约束求得
Figure GDA0003518295820000049
其中，
Figure GDA00035182958200000410
则K矩阵最小特征值对应的特征向量即为USBL安装误差角姿态四元数
Figure GDA00035182958200000411
与现有技术相比，本发明具有如下优点：
(1)相比于传统标定方法的事后处理方式，本方法既可以通过事后处理的方式求解出安装误差角，也可以通过实时构造观测矢量及参考矢量来实时计算出USBL安装误差角。
(2)相比于传统方法中要求标定路线为圆形或者直线型路线，本方法求解安装误差角所需的观测矢量及参考矢量在任意轨迹条件下都能构造得到，因此，本方法在任意路线条件下都能完成超短基线安装误差角的标定。
(3)USBL定位系统计算得到的应答器在声学基阵坐标系下的定位精度越高，观测矢量构造的精度也就越高，则本方法标定的安装误差角精度越高。进一步地，USBL计算得到的载体在导航系下的定位精度也就越高。这是一个逐次递进的效果。
附图说明
图1为本发明的标定方法结构示意图；
图2为本发明中标定方法整体流程图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述：
本发明提供一种基于姿态确定的USBL安装误差角标定方法，通过具有RTK定位精度的GPS位置来修正SINS的姿态误差，在事先通过LBL计算得到应答器位置的条件下，构造基于USBL安装误差角矩阵的矢量观测模型，利用姿态确定方法求解出USBL安装误差角。
下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
如图1所示，本发明由安装在AUV上的捷联惯导系统(SINS)和超短基线定位系统(USBL)组成，其中超短基线系统的水听器接收基阵和惯导系统固联在一起，应答器布放在海底。通过建立安装误差角姿态矩阵的矢量观测模型，并设计基于姿态确定的USBL安装误差角标定方法估计出USBL安装误差角，具体实现步骤如下。
(1)超短基线系统由安装在AUV上的四个水听器组成的正方形基阵和布放在海底的应答器组成。换能器发送声波信号，应答器接收信号后经过一段延时发送应答信号，四个水听器接收到应答信号后，根据声波到达时间确定水听器与应答器间的斜距信息，以及声波与水听器基阵间的两个方位角信息。
(2)利用具有RTK固定解定位精度的GPS位置信息与捷联惯性导航系统SINS进行松组合算法，以此来修正SINS的姿态角误差，从而为USBL标定方法提供更加精确的姿态矩阵
Figure GDA0003518295820000051
(3)构造基于USBL安装误差角姿态矩阵的矢量观测模型，利用超短基线定位系统构造水声基阵坐标系下的位置矢量，利用SINS/RTK组合导航系统的位置和姿态构造载体系下的位置矢量。
(4)根据步骤(3)构造的基于USBL安装误差角姿态矩阵的矢量观测模型，利用姿态确定方法求解出USBL安装误差角对应的姿态四元数，从而完成USBL安装误差角的实时标定。
一、步骤(2)中基于RTK固定解的SINS/GPS松组合导航方法，具体实现方式如下：
(5)状态量X由以下15个变量组成
Figure GDA0003518295820000052
其中，φE，φN，φU是捷联惯导的三个失准角，δVE，δVN，δVU是东北天三个方向上的速度误差，δL，δλ，δh是捷联惯导的三个位置误差在导航系下的描述，εx，εy，εz是陀螺的三个轴向漂移，
Figure GDA0003518295820000053
是加速度计三个轴向的零偏误差。
(6)系统状态方程为
Figure GDA0003518295820000054
其中，X(t)为系统的状态向量，F(t)是惯导系统误差状态方程状态转移矩阵，W(t)是关于捷联惯导系统的噪声向量。
Figure GDA0003518295820000061
Figure GDA0003518295820000062
Figure GDA0003518295820000063
Figure GDA0003518295820000064
Figure GDA0003518295820000065
Figure GDA0003518295820000066
Figure GDA0003518295820000067
(7)以RTK位置与SINS位置差作为观测量，建立系统观测方程为
Z(t)＝PSINS-PGPS＝H(t)X(t)+V(t)
其中，Z(t)为观测方程的量测向量，PSINS为SINS得到的位置，PGPS为RTK位置，H(t)为观测方程的量测矩阵，V(t)为观测方程的量测噪声向量；其中量测矩阵的具体表达式为
H(t)＝[03×3 03×3 I3×3 03×3 03×3]
其中I3x3表示3阶单位矩阵。
(8)系统状态方程及量测方程的离散化
Xk＝Φk，k-1Xk-1+Wk-1
Zk＝HkXk+Vk
式中，Xk为k时刻的状态向量，也就是被估计矢量；Zk为k时刻的测量序列；Wk-1为k-1时刻的系统噪声；Vk为k时刻的测量噪声序列；Φk，k-1为k-1时刻到k时刻的一步状态转移矩阵；Hk为k时刻的测量矩阵，
(9)利用标准卡尔曼滤波方程计算状态的最优估计
状态一步预测向量
Xk/k-1＝Φk，k-1Xk-1
状态估值计算
Xk＝Xk/k-1+Kk(Zk-HkXk/k-1)
滤波增益Kk
Figure GDA0003518295820000071
一步预测均方误差矩阵Pk/k-1
Figure GDA0003518295820000072
其中Qk-1为系统噪声矩阵；
估计均方误差方程Pk
Figure GDA0003518295820000073
其中Rk为量测噪声矩阵；
(10)校正SINS
利用当前的误差最优估计可以立即校正SINS每次的由测量数据得到的状态量。
位置校正可以通过惯导系统对参数的估计值与估计误差简单相减来修正：
Figure GDA0003518295820000074
式中，Xc是校正后的状态量。
二、步骤(3)中建立构造基于USBL安装误差角姿态矩阵的矢量观测模型的具体步骤为：
(11)超短基线坐标系记为a系，捷联惯导载体坐标系记为b系。ra表示应答器在超短基线坐标系下的相对位置，可以通过超短基线定位系统的斜距及方位角信息求解得到，具体计算公式为，
Figure GDA0003518295820000075
其中R表示应答器与水听器接收基阵之间的斜距信息，α，β分别为水听器与应答器之间的声波信号与超短基线坐标系x，y轴的方位角。
(12)利用不同时刻计算得到的ra(t)作差，得到应答器在a系下相对位置的变化量
Figure GDA0003518295820000081
具体计算公式为：
Figure GDA0003518295820000082
(13)LBL计算得到的应答器在导航系下的位置为
Figure GDA0003518295820000083
利用SINS位置和姿态，求解得到应答器在b系下的相对位置rb，具体计算方式为：
Figure GDA0003518295820000084
其中，
Figure GDA0003518295820000085
表示b系与n系之间的姿态矩阵，
Figure GDA0003518295820000086
表示SINS在导航系下的位置。
(14)建立ra与rb之间的关系表达式为：
Figure GDA0003518295820000087
其中，
Figure GDA0003518295820000088
表示b系与a系之间的姿态矩阵，Lb表示超短基线坐标系原点与载体系原点之间的杆臂值在载体系下的投影。
(15)通过将不同时刻计算得到的rb(t)-Lb作差，可以得到应答器在b系下相对位置的变化量
Figure GDA0003518295820000089
Figure GDA00035182958200000810
(16)通过步骤(12)构造的水声基阵坐标系下的位置矢量
Figure GDA00035182958200000811
及步骤(15)构造的载体坐标系下的位置矢量
Figure GDA00035182958200000812
建立基于USBL安装误差角的矢量观测模型如下：
Figure GDA00035182958200000813
三、步骤(4)中利用姿态确定方法求解USBL安装误差角对应的姿态四元数的具体步骤为：
(17)通过步骤(16)建立的基于USBL安装误差角的矢量观测模型，建立对应的基于USBL安装误差姿态四元数的矢量观测模型：
Figure GDA00035182958200000814
其中，
Figure GDA00035182958200000815
为USBL安装误差角姿态矩阵
Figure GDA00035182958200000816
对应的姿态四元数。
Figure GDA00035182958200000817
表示四元数乘法运算。
(18)将位置矢量
Figure GDA00035182958200000818
和位置矢量
Figure GDA00035182958200000819
简记为
Figure GDA00035182958200000820
并构造两个矩阵如下式：
Figure GDA0003518295820000091
Figure GDA0003518295820000092
(19)通过步骤(18)构造的两个矩阵，步骤(14)中的矢量观测模型转化为
Figure GDA0003518295820000093
上式中构造的观测矢量模型可以通过最小二乘约束求得
Figure GDA0003518295820000094
其中，
Figure GDA0003518295820000095
则K矩阵最小特征值对应的特征向量即为USBL安装误差角姿态四元数
Figure GDA0003518295820000096
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (4)

1.一种基于姿态确定的USBL安装误差角标定方法，其特征在于，其具体步骤如下：
(1)超短基线系统由安装在AUV上的四个水听器组成的正方形基阵和布放在海底的应答器组成，换能器发送声波信号，应答器接收信号后经过一段延时发送应答信号，四个水听器接收到应答信号后，根据声波到达时间确定水听器与应答器间的斜距信息，以及声波与水听器基阵间的两个方位角信息；
(2)利用具有RTK固定解定位精度的GPS位置信息与捷联惯性导航系统SINS进行松组合算法，以此来修正SINS的姿态角误差，从而为USBL标定方法提供更加精确的姿态矩阵
Figure FDA0003518295810000011
(3)构造基于USBL安装误差角姿态矩阵的矢量观测模型，利用超短基线定位系统构造水声基阵坐标系下的位置矢量，利用SINS/RTK组合导航系统的位置和姿态构造载体系下的位置矢量；
(4)根据步骤(3)构造的基于USBL安装误差角姿态矩阵的矢量观测模型，利用姿态确定方法求解出USBL安装误差角对应的姿态四元数，从而完成USBL安装误差角的实时标定。
2.根据权利要求1所述的一种基于姿态确定的USBL安装误差角标定方法，其特征在于：所述步骤(2)中基于RTK固定解的SINS/GPS松组合导航方法的具体步骤为：
(21)状态量X由以下15个变量组成；
Figure FDA0003518295810000014
其中，φE，φN，φU是捷联惯导的三个失准角，δVE，δVN，δVU是东北天三个方向上的速度误差，δL，δλ，δh是捷联惯导的三个位置误差在导航系下的描述，εx，εy，εz是陀螺的三个轴向漂移，
Figure FDA0003518295810000012
是加速度计三个轴向的零偏误差；
(22)系统状态方程为；
Figure FDA0003518295810000013
其中，X(t)为系统的状态向量，F(t)是惯导系统误差状态方程状态转移矩阵，W(t)是关于捷联惯导系统的噪声向量；
(23)以RTK位置与SINS位置差作为观测量，建立系统观测方程为；
Z(t)＝PSINS-PGPS＝H(t)X(t)+V(t)
其中，Z(t)为观测方程的量测向量，PSINS为SINS得到的位置，PGPS为RTK位置，H(t)为观测方程的量测矩阵，V(t)为观测方程的量测噪声向量；其中量测矩阵的具体表达式为；
H(t)＝[03×3 03×3 I3×3 03×3 03×3]
其中I3x3表示3阶单位矩阵。
3.根据权利要求1所述的一种基于姿态确定的USBL安装误差角标定方法，其特征在于：所述步骤(3)中构造基于USBL安装误差角姿态矩阵的矢量观测模型的具体步骤为：
(31)超短基线坐标系记为a系，捷联惯导载体坐标系记为b系，ra表示应答器在超短基线坐标系下的相对位置，通过超短基线定位系统的斜距及方位角信息求解得到，具体计算公式为，
Figure FDA0003518295810000021
其中R表示应答器与水听器接收基阵之间的斜距信息，α，β分别为水听器与应答器之间的声波信号与超短基线坐标系x，y轴的方位角；
(32)利用不同时刻计算得到的ra(t)作差，得到应答器在a系下相对位置的变化量
Figure FDA0003518295810000022
具体计算公式为：
Figure FDA0003518295810000023
(33)LBL计算得到的应答器在导航系下的位置为
Figure FDA0003518295810000024
利用SINS位置和姿态，求解得到应答器在b系下的相对位置rb，具体计算方式为：
Figure FDA0003518295810000025
其中，
Figure FDA0003518295810000026
表示b系与n系之间的姿态矩阵，
Figure FDA0003518295810000027
表示SINS在导航系下的位置：
(34)建立ra与rb之间的关系表达式为：
Figure FDA0003518295810000028
其中，
Figure FDA0003518295810000029
表示b系与a系之间的姿态矩阵，Lb表示超短基线坐标系原点与载体系原点之间的杆臂值在载体系下的投影；
(35)通过将不同时刻计算得到的rb(t)-Lb作差，得到应答器在b系下相对位置的变化量
Figure FDA00035182958100000210
Figure FDA00035182958100000211
(36)通过步骤(32)构造的水声基阵坐标系下的位置矢量
Figure FDA00035182958100000212
及步骤(35)构造的载体坐标系下的位置矢量
Figure FDA00035182958100000213
建立基于USBL安装误差角的矢量观测模型如下：
Figure FDA00035182958100000214
4.根据权利要求3所述的一种基于姿态确定的USBL安装误差角标定方法，其特征在于：所述步骤(4)中利用姿态确定方法求解USBL安装误差角对应的姿态四元数的具体步骤为：
(41)通过步骤(36)建立的基于USBL安装误差角的矢量观测模型，建立对应的基于USBL安装误差姿态四元数的矢量观测模型：
Figure FDA0003518295810000031
其中，
Figure FDA0003518295810000032
为USBL安装误差角姿态矩阵
Figure FDA0003518295810000033
对应的姿态四元数，
Figure FDA0003518295810000034
表示四元数乘法运算；
(42)将观测矢量和参考矢量简记为
Figure FDA0003518295810000035
并构造两个矩阵如下式：
Figure FDA0003518295810000036
Figure FDA0003518295810000037
(43)通过步骤(42)构造的两个矩阵，步骤(14)中的矢量观测模型转化为
Figure FDA0003518295810000038
上式中构造的观测矢量模型通过最小二乘约束求得
Figure FDA0003518295810000039
其中，
Figure FDA00035182958100000310
则K矩阵最小特征值对应的特征向量即为USBL安装误差角姿态四元数
Figure FDA00035182958100000311
CN201910447543.0A 2019-05-27 2019-05-27 一种基于姿态确定的usbl安装误差角标定方法 Active CN110132308B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910447543.0A CN110132308B (zh) 2019-05-27 2019-05-27 一种基于姿态确定的usbl安装误差角标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910447543.0A CN110132308B (zh) 2019-05-27 2019-05-27 一种基于姿态确定的usbl安装误差角标定方法

Publications (2)

Publication Number Publication Date
CN110132308A CN110132308A (zh) 2019-08-16
CN110132308B true CN110132308B (zh) 2022-04-29

Family

ID=67582093

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910447543.0A Active CN110132308B (zh) 2019-05-27 2019-05-27 一种基于姿态确定的usbl安装误差角标定方法

Country Status (1)

Country Link
CN (1) CN110132308B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110568407B (zh) * 2019-09-05 2023-06-27 武汉理工大学 一种基于超短基线和航位推算的水下导航定位方法
CN110673115B (zh) * 2019-09-25 2021-11-23 杭州飞步科技有限公司 雷达与组合导航系统的联合标定方法、装置、设备及介质
CN113465599B (zh) * 2021-06-04 2023-08-01 北京信息科技大学 定位定向方法、装置及系统
CN113670302B (zh) * 2021-09-01 2023-12-05 东南大学 一种运动效应影响下的惯性/超短基线组合导航方法
CN114397644A (zh) * 2021-12-23 2022-04-26 山东科技大学 一种基于四元数法的超短基线安装误差校准方法
CN114993242B (zh) * 2022-06-17 2023-03-31 北京航空航天大学 一种基于加速度匹配的阵列式pos安装偏差角标定方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102169184A (zh) * 2011-01-04 2011-08-31 北京航空航天大学 组合导航系统中测量双天线gps安装失准角的方法和装置
CN104457754A (zh) * 2014-12-19 2015-03-25 东南大学 一种基于sins/lbl紧组合的auv水下导航定位方法
CN106483498A (zh) * 2015-09-01 2017-03-08 北京自动化控制设备研究所 一种sinsusbl紧耦合算法
CN106767793A (zh) * 2017-01-19 2017-05-31 东南大学 一种基于sins/usbl紧组合的auv水下导航定位方法
CN108413983A (zh) * 2017-12-21 2018-08-17 中国船舶重工集团公司第七0七研究所 一种sins/usbl一体化定位系统安装误差标定的机械方法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109737956A (zh) * 2018-12-17 2019-05-10 东南大学 一种基于双应答器的sins/usbl相位差紧组合导航定位方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102169184A (zh) * 2011-01-04 2011-08-31 北京航空航天大学 组合导航系统中测量双天线gps安装失准角的方法和装置
CN104457754A (zh) * 2014-12-19 2015-03-25 东南大学 一种基于sins/lbl紧组合的auv水下导航定位方法
CN106483498A (zh) * 2015-09-01 2017-03-08 北京自动化控制设备研究所 一种sinsusbl紧耦合算法
CN106767793A (zh) * 2017-01-19 2017-05-31 东南大学 一种基于sins/usbl紧组合的auv水下导航定位方法
CN108413983A (zh) * 2017-12-21 2018-08-17 中国船舶重工集团公司第七0七研究所 一种sins/usbl一体化定位系统安装误差标定的机械方法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109737956A (zh) * 2018-12-17 2019-05-10 东南大学 一种基于双应答器的sins/usbl相位差紧组合导航定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于SINS/LBL 紧组合的AUV 水下导航定位技术;张涛等;《中国惯性技术学报》;20150831;第23卷(第4期);第500-504页 *

Also Published As

Publication number Publication date
CN110132308A (zh) 2019-08-16

Similar Documents

Publication Publication Date Title
CN110132308B (zh) 一种基于姿态确定的usbl安装误差角标定方法
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN108594272B (zh) 一种基于鲁棒卡尔曼滤波的抗欺骗干扰组合导航方法
CN100535683C (zh) 用于超短基线声学定位系统的校准方法
JP4412381B2 (ja) 方位検出装置
CN109613520B (zh) 一种基于滤波的超短基线安装误差在线标定方法
CN109724599A (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN107966145B (zh) 一种基于稀疏长基线紧组合的auv水下导航方法
CN112284384A (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
CA2256964C (en) Method of locating hydrophones
CN112859133B (zh) 一种基于雷达与北斗数据的船舶深度融合定位方法
CN111982105B (zh) 一种基于sins/lbl紧组合的水下导航定位方法及系统
CN114265047B (zh) 一种大潜深auv的定位基阵联合标校方法
CN115307643A (zh) 一种双应答器辅助的sins/usbl组合导航方法
Zhang et al. A calibration method of ultra-short baseline installation error with large misalignment based on variational Bayesian unscented Kalman filter
Troni et al. Advances in in situ alignment calibration of Doppler and high/low‐end attitude sensors for underwater vehicle navigation: Theory and experimental evaluation
CN110703205A (zh) 基于自适应无迹卡尔曼滤波的超短基线定位方法
CN115979253A (zh) 一种基于抗差滤波的水下机器人多传感器紧组合导航方法
Chen In-situ alignment calibration of attitude and ultra short baseline sensors for precision underwater positioning
Liu et al. SINS/DVL integrated system with current and misalignment estimation for midwater navigation
CN111999747B (zh) 一种惯导-卫星组合导航系统的鲁棒故障检测方法
Morgado et al. Position and velocity USBL/IMU sensor-based navigation filter
CN116222578B (zh) 基于自适应滤波和最优平滑的水下组合导航方法及系统
CN117146830A (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