CN116068540B - 一种声学多普勒测速径向波束角校正方法 - Google Patents

一种声学多普勒测速径向波束角校正方法 Download PDF

Info

Publication number
CN116068540B
CN116068540B CN202310133795.2A CN202310133795A CN116068540B CN 116068540 B CN116068540 B CN 116068540B CN 202310133795 A CN202310133795 A CN 202310133795A CN 116068540 B CN116068540 B CN 116068540B
Authority
CN
China
Prior art keywords
coordinate system
dvl
beam angle
angle
beams
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
CN202310133795.2A
Other languages
English (en)
Other versions
CN116068540A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202310133795.2A priority Critical patent/CN116068540B/zh
Publication of CN116068540A publication Critical patent/CN116068540A/zh
Application granted granted Critical
Publication of CN116068540B publication Critical patent/CN116068540B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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/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
    • G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
    • 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/183—Compensation of inertial measurements, e.g. for temperature effects
    • 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/50—Systems of measurement, based on relative movement of the target
    • G01S15/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • 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
    • 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/88—Sonar systems specially adapted for specific applications
    • G01S15/93—Sonar systems specially adapted for specific applications for anti-collision purposes
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • 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/52—Determining velocity
    • 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)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Acoustics & Sound (AREA)
  • Navigation (AREA)

Abstract

一种声学多普勒测速径向波束角校正方法，涉及组合导航及水声定位技术领域。本发明是为了解决目前还无法实现DVL波束角的标定，从而导致不能完全修正DVL的速度量测信息的问题。本发明包括：建立载体坐标系和导航坐标系，定义DVL波束的波束角及波束角初值；然后分别获取整条航迹中作业船在导航坐标系、载体坐标系下的导航信息；获取DVL在计算导航坐标系n’下四组波束输出的作业船三维速度；建立扩展卡尔曼滤波器，获取k+1时刻的状态估计值；根据k+1时刻的状态估计值修正SINS的输出并获得扩展卡尔曼滤波器的输出波束角误差，然后将波束角误差与波束角初值相加，得到修正后的DVL各波束角。本发明用于获取DVL波束角。

Description

一种声学多普勒测速径向波束角校正方法
技术领域
本发明涉及组合导航及水声定位技术领域，特别涉及一种声学多普勒测速径向波束角校正方法。
背景技术
以SINS/DVL为代表的组合导航目前水下应用最为广泛和成熟。然而，随着航行器不断向远程化、智能化方向发展，要求水下作业时间越来越长、活动范围逐渐扩大、操控精度越来越高，对声学多普勒测速声呐校准精度提出了新要求。
紧耦合导航是实现复杂机动状态下精确导航的重要途径，紧耦合导航要求高精度的DVL各波束信息，而传统的基阵系标定模型认为DVL四波束对称且波束角已知，未考虑受机械加工误差影响，声学径向波束真实空间方位、比例因子矢量并非对称一致这一情况。若仍采用传统基阵系标定方法，将人为引入误差、影响紧耦合导航性能；文献[2](Li W,ZhangL,Sun F,et al.Alignment calibration of IMU and Doppler sensors for precisionINS/DVL integrated navigation[J].Optik,2015,126(23):3872-3876.)在导航系下，利用基于SVD的最小二乘法来解算出DVL(多普勒计程仪)基阵的三个安装偏角以及一个速度比例因子，文献[4](高伟,刘亚龙,徐博,池姗姗,肖永平,陈春,王文佳,郭宇,田学林.SINS/DVL组合导航系统一体化误差标定方法[P].黑龙江：CN103389115A,2013-11-13.)则是将GPS位移信息转换至载体系，对DVL进行航位推算得到载体系航迹，利用基于SVD的最小二乘解算出航向角。文献[1](Tang K,Wang J,Li W,et al.A novel INS and Doppler sensorscalibration method for long range underwater vehicle navigation[J].Sensors,2013,13(11):14583-14600.)提出了一种三点标定法，即只需在航迹起点及终点接收GPS位置信息就可标定出部分安装偏角，从而减小对外部数据的依赖性；文献[3](]James C.K,Louis L.Adaptive Identification on the Group of Rigid-Body Rotations and itsApplication to Underwater Vehicle Navigation[J].IEEE TRANSACTIONS ONROBOTICS.2007,23(1):124-136.)则是利用自适应技术对DVL的三个安装偏角进行标定估计；文献[5](熊明磊,陈龙冬,刘兵.标定SINS/DVL组合导航系统误差的方法和装置[P].北京市：CN112987054A,2021-06-18.)对测速误差建立非线性模型，利用高斯牛顿法对DVL速度比例因子以及航向角误差进行标定；文献[6](刘静晓,李海兵,马思乐,罗建刚,李海虎,张峰,罗骋,张同伟,马晓静,王友东,付碧波,刘伟.一种DVL内外误差标定方法[P].山东省：CN113703018A,2021-11-26.)则是采用遗传算法对DVL三个安装偏角及比例因子进行标定。文献[7](王彦国，夏雨，何春海，宫京，王海鹏.SINS/DVL组合导航系统安装误差结构补偿方法[P].天津市：CN110608756A，2019-12-24.)认为速度比例因子不仅有一个，x、y轴速度均需进行标定，且DVL的波束与基阵水平面的夹角也需标定，其在忽略了垂向速度的条件下，对DVL的波束水平面夹角、水平面速度比例因子、基阵的航向角安装偏角进行标定。文献[8](Wang Q，Nie X，Gao C，et al.Calibration of a three-dimensional laser Dopplervelocimeter in a land integrated navigation system[J].Applied optics，201 8，57(29)：8566-8572.)则是认为三维激光LDV不仅存在三个安装偏角，其波束角也存在误差，并采用了卡尔曼滤波算法对其进行标定。这些现有技术中的标定方法大部分都是在各波束严格对称的假设下提出的，然而，受机械加工误差影响，声学径向波束真实空间方位、比例因子矢量并非对称一致，造成载体系三维速度非正交，若仍采用基阵系标定将人为引入误差、影响紧耦合导航性能，因此目前并不能实现波束角的标定，从而不能完全修正DVL的速度量测信息。
发明内容
本发明目的是为了解决目前还无法实现波束角的标定，从而导致不能完全修正DVL的速度量测信息的问题，而提出了一种声学多普勒测速径向波束角校正方法。
一种声学多普勒测速径向波束角校正方法具体过程为：
步骤一、将SINS的陀螺仪、加速度计组件与DVL的声学基座连接，并将连接后的SINS的陀螺仪、加速度计组件与DVL的声学基座通过转台安装在作业船底部，将GPS接收机固定在作业船顶部，然后建立载体坐标系b和导航坐标系n，并定义DVL四个波束在载体坐标系b中的波束角及波束角初值；
所述转台垂直于作业船甲板轴；
步骤二、在航迹中点将转台水平旋转90°，然后分别获取整条航迹中作业船在导航坐标系、载体坐标系下的导航信息；
步骤三、利用步骤二记录的导航信息获取DVL在计算导航坐标系n’下四组波束输出的作业船三维速度
所述DVL四组波束为别为：
a组：1、2、3波束；
b组：1、2、4波束；
c组：1、3、4波束；
d组：2、3、4波束；
所述计算导航坐标系n’为根据作业船姿态角所得的导航坐标系；
步骤四、利用步骤三获得的和步骤二获得的作业船在导航坐标系下的信息建立扩展卡尔曼滤波器；
步骤五、利用步骤四建立的扩展卡尔曼滤波器获取k+1时刻的状态估计值并修正SINS的输出；
步骤六、根据步骤五获得的得到扩展卡尔曼滤波器的输出波束角误差/>然后将/>与波束角初值相加，得到修正后的DVL各波束角。
进一步地，所述步骤一中的建立载体坐标系b和导航坐标系n，并定义DVL四个波束在载体坐标系b中的波束角及波束角初值，包括以下步骤：
步骤一一、建立载体坐标系b和导航坐标系n；
所述载体坐标系b以作业船重心为原点，yb轴沿甲板平面指向作业船的艏向方向，zb轴垂直与甲板向上，xb轴、yb轴与zb轴构成右手坐标系；
所述导航坐标系n以作业船的质点为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直与xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；
步骤一二、定义DVL四个波束在载体坐标系中的每个波束角αi、θi及波束角初值αi0、θi0；
其中，i＝1，2，3，4，表示波束编号，αi为波束i的声轴线与载体坐标系中yoz平面的夹角；θi为波束i的声轴线与载体坐标系中xoy平面的夹角；波束1处于载体坐标系中的第V象限，波束2位于第VI象限，波束3位于第VII象限，波束4位于第VIII象限。
进一步地，所述步骤二中的分别获取整条航迹中作业船在导航坐标系、载体坐标系下的导航信息，具体为：
利用GPS获取作业船在导航坐标系下的速度经度Bgps、纬度信息Lgps、高度hgps；
利用DVL获取DVL四个波束测得的波束径向速度VL＝[v1v2v3v4]T；
其中，v1、v2、v3、v4分别是DVL四个波束的径向速度；
利用SINS获取导航坐标系下作业船的姿态角和作业船加速度计组的输出；
作业船的姿态角包括：航向角A、俯仰角K和横滚角ψ；
所述作业船的加速度计组输出为比力。
进一步地，所述步骤三中的利用步骤二记录的导航信息获取DVL在计算导航坐标系n’下四组波束输出的作业船三维速度包括以下步骤：
步骤三一、利用导航坐标系下作业船的姿态角获取载体坐标系b到计算导航坐标系n’的转换矩阵
步骤三二、利用步骤一定义的波束角初值和步骤二获得的DVL四个波束的径向速度分别获取作业船在载体坐标系b下的三维速度
首先，对DVL四个波束进行分组：
a组为1、2、3波束，b组为1、2、4波束，c组为1、3、4波束，d组为2、3、4波束；
然后，利用步骤一获得的波束角初值和步骤二获得的DVL四个波束的径向速度分别获取作业船在载体坐标系b下的三维速度为：
其中，Ca、Cb、Cc、Cd为中间变量；
步骤三三、利用步骤三一获得的转换矩阵和步骤三二获得的作业船在载体坐标系b下的三维速度/>获取DVL在计算导航坐标系n’下四组波束输出的载体三维速度/>
进一步地，所述DVL在计算导航坐标系n’下四组波束输出的载体三维速度如下式：
进一步地，所述步骤四中的利用步骤三获得的和步骤二记录的导航信息建立扩展卡尔曼滤波器，包括以下步骤：
步骤四一、获得与/>的理论速度误差关系：
其中，φ＝[φx φy φz]T为SINS姿态误差角，×表示叉乘；Ka为在波束角初值处一阶泰勒展开矩阵的负值，Kb为/>在波束角初值处一阶泰勒展开矩阵的负值，Kc为/>在波束角初值处一阶泰勒展开矩阵的负值，Kd为/>在波束角初值处一阶泰勒展开矩阵的负值；[δα1 δα2 δα3 δα4 δθ1 δθ2 δθ3 δθ4]T为波束角误差向量，δα1、δα2、δα3、δα4、δθ1、δθ2、δθ3、δθ4分别是各波束真值与其对应初值的差；
步骤四二、根据步骤四一获得的误差关系建立扩展卡尔曼滤波器状态方程；
步骤四三、根据步骤四二获得的扩展卡尔曼滤波器状态方程建立扩展卡尔曼滤波器观测方程。
进一步地，所述步骤四二中的根据步骤四一获得的误差关系建立扩展卡尔曼滤波器状态方程，如下式：
Xk+1＝Fk+1/kXk+wk+1 (9)
其中，
δL＝Lsins-Lgps
δB＝Bsins-Bgps
其中，是波束角误差向量，δVn＝[δvx δvy]T是SINS速度误差，δPn＝[δL δB]T是SINS位置误差，ε＝[εx εy εz]T是陀螺零偏，/>是加速度计零偏，X为扩展卡尔曼滤波系统的状态变量，εx εy εz分别是SINS坐标系下x、y、z方向的陀螺零偏，/>分别是SINS坐标系下x、y方向的加速度计零偏；Xk为k时刻的状态变量，Xk+1为k+1时刻的状态变量，Fk+1/k为状态转移矩阵，wk+1为扩展卡尔曼滤波系统过程噪声序列，δL是GPS输出作业船在n系下的位置纬度与SINS在n’系下输出的位置纬度差，δB是GPS输出作业船在n系下的位置经度与SINS在n’系下输出的位置经度差，δvx是SINS速度误差的x方向分量，δvy是SINS速度误差的y方向分量。
进一步地，所述步骤四三中的根据步骤四二获得的扩展卡尔曼滤波器状态方程建立扩展卡尔曼滤波器观测方程，如下式：
Zk+1＝Hk+1Xk+1+vk+1 (10)
其中，Zk+1为k+1时刻的观测量，Hk+1为k+1时刻的观测矩阵，vk+1为扩展卡尔曼滤波系统观测噪声序列，Z是扩展卡尔曼滤波系统的观测量。
进一步地，
其中，0m×n为m×n维零矩阵，I4×4为四阶单位矩阵，m取3或4，n取3、4、5或8。
进一步地，所述步骤五中利用步骤四建立的扩展卡尔曼滤波器获取k+1时刻的状态估计值如下式：
其中，
Pk+1/k＝Fk+1/kPkFT k+1/k+Qk
其中，Kk+1是k+1时刻的滤波器增益，Kk是k时刻的滤波器增益，Rk+1为k+1时刻的系统观测噪声协方差矩阵，Hk+1为k+1时刻的观测矩阵，Pk+1/k是k+1时刻的状态预测误差协方差矩阵，Pk为k时刻的状态估计误差协方差矩阵，Qk为k时刻的系统过程噪声协方差矩阵，Pk+1是k+1时刻的状态估计误差协方差矩阵，是k+1时刻的状态预测值。
本发明的有益效果为：
本发明提出了波束角的估计方法。本发明对波束轴的真实空间方位角进行标定，通过波束角误差与载体测速误差的误差传递公式，推导出基于扩展卡尔曼滤波的标定方程，从而得到波束轴的真实空间方位角，解决了由机械加工误差等的影响造成的载体系三维速度非正交的问题，进而提高测速及组合导航精度；现有技术中将波速角定义在基阵系下均需要进行基阵系到载体系转换这一步骤，本发明将波束角直接定义在载体系下，省去转换坐标系的步骤，从而避免了这一过程中产生的误差；本发明利用八个波束角来代替速度比例因子，使误差模型更加符合实际，得到了更为准确的波束角的标定结果，解决了由于传统的速度比例因子只是在最小二乘的条件下对速度误差进行拟合修正，而导致的存在部分误差无法修正的问题，完全修正了DVL的速度量测信息。
附图说明
图1为本发明原理图；
图2为本发明中DVL波束角示意图；
图3为本发明的航迹示意图。
具体实施方式
具体实施方式一：如图1所示，本实施方式一种声学多普勒测速径向波束角校正方法具体过程为：
步骤一、将SINS的陀螺仪和加速度计组件与DVL的声学基座固联，并将其通过转台安装在作业船底部，GPS接收机固定在作业船顶部，然后建立载体坐标系b和导航坐标系n，并定义DVL四个波束在载体坐标系b中的波束角及波束角初值；
所述SINS为捷联惯性导航系统；DVL为声学多普勒计程仪；
所述转台是在垂直于甲板轴上，可进行相对于作业船旋转的平台；
步骤一一、建立载体坐标系b和导航坐标系n；
所述载体坐标系b以作业船重心为原点，yb轴沿甲板平面指向作业船的艏向方向，zb轴垂直与甲板向上，xb轴与yb轴、zb轴构成右手坐标系；
所述导航坐标系n是指“东北天”地理坐标系，该地理坐标系以作业船的质点为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直与xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；
步骤一二、定义DVL四个波束在载体坐标系中的每个波束的波束角αi、θi及波束角初值，如图2所示；
设定波束角的初值为αi0，θi0，。定义每个波束的波束角为αi、θi，其中，i＝1，2，3，4，表示四个波束编号，αi为波束i的声轴线与载体坐标系中yoz平面的夹角；θi为波束i的声轴线与载体坐标系中xoy平面的夹角；
其中波束1处于载体坐标系中的第V象限，波束2位于第VI象限，波束3位于第VII象限，波束4位于第VIII象限，v1v2v3v4分别是DVL四个波束的速度；
步骤二、在航迹中点将安装DVL与INS的转台水平旋转90°，然后分别获取整条航迹中作业船在导航坐标系、载体坐标系下的导航信息：
利用GPS获取作业船在导航坐标系下的速度经度Bgps、纬度信息Lgps、高度hgps；
利用DVL获取DVL四个波束的径向速度VL＝[v1v2v3v4]T；
利用SINS获取导航坐标系下作业船的姿态角和作业船加速度计组的输出；
作业船的姿态角包括：航向角A、俯仰角K和横滚角ψ；
所述作业船的加速度计组输出为比力。
步骤三、利用步骤二记录的导航信息获取DVL在计算导航坐标系n’下四组波束输出的作业船三维速度
所述DVL四组波束分别为：a组为1、2、3波束，b组为1、2、4波束，c组为1、3、4波束，d组为2、3、4波束；
所述计算导航坐标系n’为根据作业船姿态角所得的导航坐标系；
步骤三一、利用作业船的姿态角获取载体坐标系b到计算导航坐标系n’的转换矩阵
所述计算导航坐标系n’为根据作业船姿态角所得的导航坐标系；
步骤三二、利用步骤一获得的波束角的初值和步骤二获得的DVL四个波束的径向速度分别获取作业船在载体坐标系b下的三维速度为：
首先，利用DVL四个波束将四个波束分为四组，a组为1、2、3波束，b组为1、2、4波束，c组为1、3、4波束，d组为2、3、4波束；
然后，利用步骤一获得的波束角的初值和步骤二获得的DVL四个波束的径向速度分别获取作业船在载体坐标系b下的三维速度为：
其中，Ca、Cb、Cc、Cd为中间变量；
步骤三三、利用步骤三一获得的转换矩阵和步骤三二获得的作业船在载体坐标系b下的三维速度/>获取DVL在计算导航坐标系n’下四组波束输出的载体三维速度/>
步骤四、利用步骤三获得的和GPS输出作业船在n系下的导航信息建立扩展卡尔曼滤波器：
步骤四一、获得与/>的理论速度误差关系：
其中，φ＝[φx φy φz]T为n’系与n系的姿态误差角，φx、φy、φz分别是φ的x、y、z方向分量，×表示叉乘；Ka为在波束角初值处一阶泰勒展开矩阵的负值，Kb为/>在波束角初值处一阶泰勒展开矩阵的负值，Kc为/>在波束角初值处一阶泰勒展开矩阵的负值，Kd为/>在波束角初值处一阶泰勒展开矩阵的负值；[δα1 δα2 δα3 δα4 δθ1 δθ2 δθ3 δθ4]T为波束角误差向量，是波束角真值向量减去初值向量，δα1、δα2、δα3、δα4、δθ1、δθ2、δθ3、δθ4分别是各波束角真值与对应初值的差；
步骤四二、根据步骤四一获得的误差关系建立扩展卡尔曼滤波器状态方程：
Xk+1＝Fk+1/kXk+wk+1 (9)
其中，
δL＝Lsins-Lgps
δB＝Bsins-Bgps
其中，是波束角误差向量，φ＝[φx φy φz]T是n’系与n系的姿态误差角即SINS姿态误差角，δVn＝[δvx δvy]T是SINS速度误差，δPn＝[δL δB]T是SINS位置误差，ε＝[εx εyεz]T是陀螺零偏，/>是加速度计零偏，X为扩展卡尔曼滤波系统的状态变量，εxεy εz分别是SINS坐标系下x、y、z方向的陀螺零偏，/>分别是SINS坐标系下x、y方向的加速度计零偏；Xk为k时刻的状态变量，Xk+1为k+1时刻的状态变量，Fk+1/k为状态转移矩阵，wk+1为扩展卡尔曼滤波系统过程噪声序列，δL是GPS输出作业船在n系下的位置纬度与SINS在n’系下输出的位置纬度差，δB是GPS输出作业船在n系下的位置经度与SINS在n’系下输出的位置经度差，δvx是SINS速度误差的x方向分量，δvy是SINS速度误差的y方向分量；
步骤四三、根据步骤四二获得的扩展卡尔曼滤波器状态方程建立扩展卡尔曼滤波器观测方程：
Zk+1＝Hk+1Xk+1+vk+1 (10)
其中，Zk+1为k+1时刻的观测量，Hk+1为k+1时刻的观测矩阵，vk+1为扩展卡尔曼滤波系统观测噪声序列；0m×n为m×n维零矩阵，I4×4为四阶单位矩阵，m取3或4，n取3、4、或5，Z是扩展卡尔曼滤波系统的观测量，δVn只使用水平二维速度误差；
步骤五、利用步骤四建立的扩展卡尔曼滤波器获取k+1时刻的状态估计值
其中，
Kk+1＝Pk+1/kHT k+1(Hk+1Pk+1/kHT k+1+Rk+1)-1
Pk+1/k＝Fk+1/kPkFT k+1/k+Qk
其中，XK为k时刻的状态估计值，Kk+1是k+1时刻的滤波器增益，Kk是k时刻的滤波器增益，Rk+1为k+1时刻的系统观测噪声协方差矩阵，Hk+1为k+1时刻的观测矩阵，Pk+1/k是k+1时刻的状态预测误差协方差矩阵，Pk为k时刻的状态估计误差协方差矩阵，Qk为k时刻的系统过程噪声协方差矩阵，Pk+1是k+1时刻的状态估计误差协方差矩阵，是k+1时刻的状态预测值；
根据修正SINS的输出，进行迭代计算，获得所有时刻的状态预测值；
其中，计算时，Pk+1＝(I-KkHk+1)Pk；I为二十阶单位阵；
步骤六、根据步骤五获得的得到扩展卡尔曼滤波器的输出波束角误差/>然后将/>与波束角初值相加，得到修正后的DVL各波束角，如图3所示。
需要注意的是，具体实施方式仅是对本发明技术方案的解释和说明，不能以此限定权力保范围。凡根据本发明说明书所做的仅仅是局部改变的，仍应落入本发明的保护范围。

Claims (10)

1.一种声学多普勒测速径向波束角校正方法，其特征在于所述方法具体过程为：
步骤一、将SINS的陀螺仪、加速度计组件与DVL的声学基座连接，并将连接后的SINS的陀螺、加速度计组件与DVL的声学基座通过转台安装在作业船底部，将GPS接收机固定在作业船顶部，然后建立载体坐标系b和导航坐标系n，并定义DVL四个波束在载体坐标系b中的波束角及波束角初值；
所述转台垂直于作业船甲板轴；
步骤二、在航迹中点将转台水平旋转90°，然后分别获取整条航迹中作业船在导航坐标系、载体坐标系下的导航信息；
步骤三、利用步骤二记录的导航信息计算DVL在计算导航坐标系n’下四组波束输出的作业船三维速度
所述DVL四组波束为别为：
a组：1、2、3波束；
b组：1、2、4波束；
c组：1、3、4波束；
d组：2、3、4波束；
所述计算导航坐标系n’为根据作业船姿态角所计算得到的导航坐标系；
步骤四、利用步骤三获得的和步骤二记录的导航信息建立扩展卡尔曼滤波器；
步骤五、利用步骤四建立的扩展卡尔曼滤波器获取k+1时刻的状态估计值并修正SINS的输出；
步骤六、根据步骤五获得的得到扩展卡尔曼滤波器的输出波束角误差/>然后将/>与波束角初值相加，得到修正后的DVL各波束角。
2.根据权利要求1所述的一种声学多普勒测速径向波束角校正方法，其特征在于：所述步骤一中的建立载体坐标系b和导航坐标系n，并定义DVL四个波束在载体坐标系b中的波束角及波束角初值，包括以下步骤：
步骤一一、建立载体坐标系b和导航坐标系n；
所述载体坐标系b以作业船重心为原点，yb轴沿甲板平面指向作业船的艏向方向，zb轴垂直与甲板向上，xb轴、yb轴与zb轴构成右手坐标系；
所述导航坐标系n以作业船的质点为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直与xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；
步骤一二、定义DVL四个波束在载体坐标系中的每个波束角αi、θi及波束角初值αi0、θi0；
其中，i＝1,2,3,4，表示波束编号，αi为波束i的声轴线与载体坐标系中yoz平面的夹角；θi为波束i的声轴线与载体坐标系中xoy平面的夹角；波束1处于载体坐标系中的第V象限，波束2位于第VI象限，波束3位于第VII象限，波束4位于第VIII象限。
3.根据权利要求2所述的一种声学多普勒测速径向波束角校正方法，其特征在于：所述步骤二中的分别获取整条航迹中作业船在导航坐标系、载体坐标系下的导航信息，具体为：
利用GPS获取作业船在导航坐标系下的速度经度Bgps、纬度信息Lgps、高度hgps；
利用DVL获取DVL四个波束测得的波束径向速度VL＝[v1v2v3v4]T；
其中，v1、v2、v3、v4分别是DVL四个波束的径向速度；
利用SINS获取导航坐标系下作业船的姿态角和作业船加速度计组的输出；
作业船的姿态角包括：航向角A、俯仰角K和横滚角ψ。
4.根据权利要求3所述的一种声学多普勒测速径向波束角校正方法，其特征在于：所述步骤三中的利用步骤二获得的导航信息计算DVL在计算导航坐标系n’下四组波束输出的作业船三维速度包括以下步骤：
步骤三一、利用导航坐标系下作业船的姿态角获取载体坐标系b到计算导航坐标系n’的转换矩阵
步骤三二、利用步骤一定义的波束角初值和步骤二获得的DVL四个波束的径向速度分别获取作业船在载体坐标系b下的三维速度
首先，对DVL四个波束进行分组：
a组为1、2、3波束，b组为1、2、4波束，c组为1、3、4波束，d组为2、3、4波束；
然后，利用步骤一获得的波束角初值和步骤二获得的DVL四个波束的径向速度分别获取作业船在载体坐标系b下的三维速度为：
其中，Ca、Cb、Cc、Cd为中间变量；
步骤三三、利用步骤三一获得的转换矩阵和步骤三二获得的作业船在载体坐标系b下的三维速度/>获取DVL在计算导航坐标系n’下四组波束输出的载体三维速度/>
5.根据权利要求4所述的一种声学多普勒测速径向波束角校正方法，其特征在于：所述DVL在计算导航坐标系n’下四组波束输出的载体三维速度如下式：
6.根据权利要求5所述的一种声学多普勒测速径向波束角校正方法，其特征在于：所述步骤四中的利用步骤三获得的和步骤二记录的导航信息建立扩展卡尔曼滤波器，包括以下步骤：
步骤四一、获得与/>的理论速度误差关系：
其中，φ＝[φx φy φz]T为SINS姿态误差角，×表示叉乘；Ka为在波束角初值处一阶泰勒展开矩阵的负值，Kb为/>在波束角初值处一阶泰勒展开矩阵的负值，Kc为/>在波束角初值处一阶泰勒展开矩阵的负值，Kd为/>在波束角初值处一阶泰勒展开矩阵的负值；[δα1δα2 δα3 δα4 δθ1 δθ2 δθ3 δθ4]T为波束角误差向量，δα1、δα2、δα3、δα4、δθ1、δθ2、δθ3、δθ4分别是各波束角真值与对应初值的差；
步骤四二、根据步骤四一获得的误差关系建立扩展卡尔曼滤波器状态方程；
步骤四三、根据步骤四二获得的扩展卡尔曼滤波器状态方程建立扩展卡尔曼滤波器观测方程。
7.根据权利要求6所述的一种声学多普勒测速径向波束角校正方法，其特征在于：所述步骤四二中的根据步骤四一获得的误差关系建立扩展卡尔曼滤波器状态方程，如下式：
Xk+1＝Fk+1/kXk+wk+1 (9)
其中，
δL＝Lsins-Lgps
δB＝Bsins-Bgps
其中，是波束角误差向量，δVn＝[δvx δvy]T是SINS速度误差，δPn＝[δL δB]T是SINS位置误差，ε＝[εx εy εz]T是陀螺零偏，/>是加速度计零偏，X为扩展卡尔曼滤波系统的状态变量，εx εy εz分别是SINS坐标系下x、y、z方向的陀螺零偏，/>分别是SINS坐标系下x、y方向的加速度计零偏；Xk为k时刻的状态变量，Xk+1为k+1时刻的状态变量，Fk+1/k为状态转移矩阵，wk+1为扩展卡尔曼滤波系统过程噪声序列，δL是GPS输出作业船在n系下的位置纬度与SINS在n’系下输出的位置纬度差，δB是GPS输出作业船在n系下的位置经度与SINS在n’系下输出的位置经度差，δvx是SINS速度误差的x方向分量，δvy是SINS速度误差的y方向分量。
8.根据权利要求7所述的一种声学多普勒测速径向波束角校正方法，其特征在于：所述步骤四三中的根据步骤四二获得的扩展卡尔曼滤波器状态方程建立扩展卡尔曼滤波器观测方程，如下式：
Zk+1＝Hk+1Xk+1+vk+1 (10)
其中，Zk+1为k+1时刻的观测量，Hk+1为k+1时刻的观测矩阵，vk+1为扩展卡尔曼滤波系统观测噪声序列，Z是扩展卡尔曼滤波系统的观测量。
9.根据权利要求8所述的一种声学多普勒测速径向波束角校正方法，其特征在于：
其中，0m×n为m×n维零矩阵，I4×4为四阶单位矩阵，m取3或4，n取3、4、5或8。
10.根据权利要求9所述的一种声学多普勒测速径向波束角校正方法，其特征在于：所述步骤五中利用步骤四建立的扩展卡尔曼滤波器获取k+1时刻的状态估计值如下式：
其中，
Kk+1＝Pk+1/kHT k+1(Hk+1Pk+1/kHT k+1+Rk+1)-1
Pk+1/k＝Fk+1/kPkFT k+1/k+Qk
其中，Kk+1是k+1时刻的滤波器增益，Kk是k时刻的滤波器增益，Rk+1为k+1时刻的系统观测噪声协方差矩阵，Hk+1为k+1时刻的观测矩阵，Pk+1/k是k+1时刻的状态预测误差协方差矩阵，Pk为k时刻的状态估计误差协方差矩阵，Qk为k时刻的系统过程噪声协方差矩阵，Pk+1是k+1时刻的状态估计误差协方差矩阵，是k+1时刻的状态预测值。
CN202310133795.2A 2023-02-17 2023-02-17 一种声学多普勒测速径向波束角校正方法 Active CN116068540B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310133795.2A CN116068540B (zh) 2023-02-17 2023-02-17 一种声学多普勒测速径向波束角校正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310133795.2A CN116068540B (zh) 2023-02-17 2023-02-17 一种声学多普勒测速径向波束角校正方法

Publications (2)

Publication Number Publication Date
CN116068540A CN116068540A (zh) 2023-05-05
CN116068540B true CN116068540B (zh) 2023-09-12

Family

ID=86171396

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310133795.2A Active CN116068540B (zh) 2023-02-17 2023-02-17 一种声学多普勒测速径向波束角校正方法

Country Status (1)

Country Link
CN (1) CN116068540B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110146075A (zh) * 2019-06-06 2019-08-20 哈尔滨工业大学（威海） 一种增益补偿自适应滤波的sins/dvl组合定位方法
CN111323012A (zh) * 2020-04-24 2020-06-23 东南大学 载体高动态环境下的ins辅助dvl测速误差补偿方法
CN111380519A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种超短基线/捷联惯性导航松组合的导航误差校正方法
CN111380520A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl松组合导航定位方法
CN111380518A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl紧组合导航定位方法
CN112097763A (zh) * 2020-08-28 2020-12-18 西北工业大学 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110887505A (zh) * 2019-09-29 2020-03-17 哈尔滨工程大学 一种冗余式惯性测量单元实验室标定方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110146075A (zh) * 2019-06-06 2019-08-20 哈尔滨工业大学（威海） 一种增益补偿自适应滤波的sins/dvl组合定位方法
CN111380519A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种超短基线/捷联惯性导航松组合的导航误差校正方法
CN111380520A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl松组合导航定位方法
CN111380518A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl紧组合导航定位方法
CN111323012A (zh) * 2020-04-24 2020-06-23 东南大学 载体高动态环境下的ins辅助dvl测速误差补偿方法
CN112097763A (zh) * 2020-08-28 2020-12-18 西北工业大学 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法

Also Published As

Publication number Publication date
CN116068540A (zh) 2023-05-05

Similar Documents

Publication Publication Date Title
CN110031882B (zh) 一种基于sins/dvl组合导航系统的外量测信息补偿方法
CN109443379A (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN111380518B (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
CN111595348B (zh) 一种自主水下航行器组合导航系统的主从式协同定位方法
CN110567454B (zh) 一种复杂环境下sins/dvl紧组合导航方法
CN108088443B (zh) 一种定位定向设备速度补偿方法
CN102506857A (zh) 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法
CN102116634B (zh) 一种着陆深空天体探测器的降维自主导航方法
CN108444476B (zh) 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法
CN109443349A (zh) 一种姿态航向测量系统及其融合方法、存储介质
CN113311436A (zh) 一种移动平台上激光测风雷达运动姿态测风订正方法
CN108594283A (zh) Gnss/mems惯性组合导航系统的自由安装方法
CN105928515B (zh) 一种无人机导航系统
CN110926468A (zh) 基于传递对准的动中通天线多平台航姿确定方法
CN105258698A (zh) 一种高动态自旋制导炮弹空中组合导航方法
CN111829512A (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN115200574A (zh) 一种地球椭球模型下的极区横向组合导航方法
CN104776847B (zh) 一种适用于水下导航系统单点估计陀螺漂移的方法
CN113503892B (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN105547289A (zh) 一种水下航行器组合导航系统及导航信息融合方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN112284412B (zh) 一种避免欧拉转换奇异导致精度下降的地面静态对准方法
CN109813316A (zh) 一种基于地形辅助的水下载体紧组合导航方法
CN112083425A (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN116068540B (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