CN112146654B - 基于关键约束帧的前视成像声呐水下定位与导航方法 - Google Patents

基于关键约束帧的前视成像声呐水下定位与导航方法 Download PDF

Info

Publication number
CN112146654B
CN112146654B CN202010864951.9A CN202010864951A CN112146654B CN 112146654 B CN112146654 B CN 112146654B CN 202010864951 A CN202010864951 A CN 202010864951A CN 112146654 B CN112146654 B CN 112146654B
Authority
CN
China
Prior art keywords
frame
sonar
feature
frames
underwater
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
CN202010864951.9A
Other languages
English (en)
Other versions
CN112146654A (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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202010864951.9A priority Critical patent/CN112146654B/zh
Publication of CN112146654A publication Critical patent/CN112146654A/zh
Application granted granted Critical
Publication of CN112146654B publication Critical patent/CN112146654B/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/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • 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
    • 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
    • G01S15/60—Velocity or trajectory determination systems; Sense-of-movement determination systems wherein the transmitter and receiver are mounted on the moving object, e.g. for determining ground speed, drift angle, ground track
    • 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/89—Sonar systems specially adapted for specific applications for mapping or imaging
    • 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)
  • Acoustics & Sound (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本发明属于水下自主定位导航技术领域，公开了一种基于关键约束帧的前视成像声呐水下定位与导航方法。其目的在于识别并充分利用过往声呐图像中的有效约束信息对当前位姿和水下特征进行定位。主要步骤有：前端数据处理，包括二维声呐图像的特征提取与匹配、惯性测量数据处理；后端弹性滑动窗口优化，包括声呐关键约束帧的识别与存储、从关键约束帧库中选择可加入滑动窗口优化的过往帧、窗口大小控制与优化、更新机器人位姿和水下特征位置。本发明方法可提高水下机器人定位和导航系统的精度，同时保证有限的优化规模，在声呐图像前端出现异常值时也可以及时在后端检测并处理，以保证系统的鲁棒性。

Description

基于关键约束帧的前视成像声呐水下定位与导航方法
技术领域
本发明属于水下自主定位导航技术领域，主要涉及一种基于关键约束帧的前视成像声呐水下定位与导航方法。
背景技术
近年来，世界各国对海洋安全、海洋资源和海洋经济等方面日益重视，海洋机器人，尤其是无人潜航器(Autonomous Underwater Vehicle,AUV)，作为执行各类海洋水下任务必不可缺的高端装备，可适应不确定、无法预知的复杂水下环境，在民用和军事上都具备无可比拟的优越性，正受到越来越广泛的关注。由于复杂海洋环境海浪、海流、碍航物、湍流和内波等不确定环境因素，水下精确可靠的导航定位是AUV执行任务及安全航行的关键。
AUV导航定位系统能够为其提供位置、航向、深度、速度和姿态等信息，以保障AUV的安全航行、水下作业。现有的水下定位导航技术一般分为两大类[1]:
从外部定位导航来看，GPS信号在水下衰减迅速，很难依赖；水声定位系统如超短基线定位声呐(Ultra-Short Baseline，USBL)、短基线定位声呐(Short Baseline,SBL)、长基线定位声呐(Long Baseline,LBL)等，需要提前放置声学信标或者声学基站，工作区域受限，而且成本很高；而海洋地形匹配技术往往需要预先得到目标海域的海图，这在未知海域并不现实。
从自主定位导航来看，惯性导航系统往往具有积累误差，随着时间推移，误差变得越来越大，致使定位不可靠；视觉导航成像距离短，穿透力差，仅适用于水质较清、光照条件较好的水下环境，在泥沙较多、水质浑浊、光照较暗的区域难以有效运行；声呐定位导航系统它不受水质、光照等因素的影响，距离较远，穿透能力强，非常适合各种场景下的水下自主定位导航。但是多波束测深声呐和侧扫声呐一般只对海床进行测量建图，不用于向前的避障，而机械扫描式成像声呐由于扫描速度慢，在AUV运动的同时会产生图像残影，精度也受到影响[3][4]。多波束前视成像声呐(以下均简称为“前视成像声呐”)为电子扫描，频率高，非常适合AUV的障碍物识别与避障、水下导航，应用前景广阔。
目前国内外的声呐定位导航系统大多是使用多波束测深声呐、机械扫描式成像声呐和侧扫声呐，而前视成像声呐相关研究仍然较少。前视成像声呐的定位和导航最早一般采用扩展卡尔曼滤波(EKF)及其衍生方法，如稀疏扩展滤波(SEIF)等[5]。然而，EKF系列定位和导航方法的主要缺点在于状态向量和协方差矩阵的维度随着时间不断增大，运算复杂度和代价也相当可观，且没有异常检测机制，这并不利于长时间的水下导航，因此目前常采用基于双视角优化的方法进行前视成像声呐的定位和导航[6]。
目前前视成像声呐的定位和导航问题主要在于：二维声呐图像丢失了水下目标的俯仰角；水下声学特征稀疏，噪声严重，难以提取、匹配特征。这些问题导致了三维空间内的二维前视成像声呐定位和导航极易受到前端图像特征提取和关联效果的影响而出现欠约束现象，对异常值的鲁棒性不强，进而可能导致定位导航误差迅速增大甚至发散；水下环境的特征也因丢失俯仰角而难以确定位置，这也对根据周围环境进行自身定位的水下机器人带来负面效果。目前国内外关于如何根据前视成像声呐关键约束帧进行水下定位和导航的研究十分少见。
参考文献
[1]Paull L,Saeedi S,Seto M,et al.AUV Navigation and Localization:AReview[J].IEEE Journal of Oceanic Engineering,2014,39(1):131-149.
[2]Miller P A,Farrell J A,Zhao Y,et al.Autonomous Underwater VehicleNavigation[J].IEEE Journal of Oceanic Engineering,2010,35(3):663-678.
[3]张书景.大尺度环境中自主式水下机器人同时定位与地图构建算法研究[D].山东:中国海洋大学,2014.
[4]李庆武,霍冠英,周妍.声呐图像处理[M].科学出版社,2015.
[5]Walter M R,Hover F S,Leonard J J,et al.SLAM for ship hullinspection using exactly sparse extended information filters[C]//IEEEInternational Conference on Robotics&Automation(ICRA).IEEE,2008:1463-1470.
[6]Shin Y,Lee Y,Choi H,et al.Bundle adjustment from sonar images andSLAM application for seafloor mapping[C]//IEEE/MTS OCEANS,2015:1-6.
发明内容
本发明提供了一种基于关键约束帧的前视成像声呐水下定位与导航方法。针对目前前视成像声呐定位和导航中存在的上述问题，通过在后端识别因前端数据处理的异常值、噪声、特征匹配过少等造成的欠约束现象，剔除异常声呐帧；同时保存声呐有效约束帧，按照一定策略加入到滑动窗口优化中，有效提高定位和导航精度，以及对异常值和噪声的鲁棒性。
本发明提供了如下技术方案：
所述基于关键约束帧的前视成像声呐水下定位和导航方法，包括以下步骤：
步骤1：前端数据处理：水下机器人将机载惯性测量单元、深度计等导航设备产生的数据处理后，得到惯性导航设备推测的机器人位姿；通过二维前视成像声呐获取二维声呐图像，经过特征提取与匹配后，得到声呐连续两帧之间的特征匹配，并进行特征点全局编号，以feature_id表示，每一帧观测到的所有特征点均以像素坐标形式按feature_id顺序存储到特征点数据库。若匹配成功或检测到已经存在于特征点数据库中，则判定为旧特征点，全局编号不更新；否则判定为当前帧观测到的新特征点，按特征观测的先后顺序依次编号；
步骤2：声呐关键约束帧的识别与存储：读取连续两帧中机器人在世界坐标系下的位姿和所有特征点像素坐标，并将特征点像素坐标转换成基于声呐极坐标系的球面坐标；判定当前帧是否为欠约束帧，若是，则另行处理；若否，则进一步判定当前帧是否为声呐关键约束帧，若是，则将该声呐帧存储至关键约束帧库；
步骤3：从关键约束帧库中选择可加入滑动窗口优化的过往帧：对于关键约束帧库中存储的过往帧，判断其能否加入到当前滑动窗口优化中，若能则选择性加入，若否则不加入；
步骤4：窗口大小控制：判断当前滑动窗口的大小NS是否超过最大限制NSmax，若是，则只保留当前窗口内最小奇异值排序在前NSmax-2的过往帧，并移除其余的过往帧，滑动窗口大小NS在2和最大窗口限制NSmax之间自动调节；若否，则在窗口确定后开始通过非线性优化器对滑动窗口进行优化；优化完成后，得到估计的机器人相对位姿及水下特征点球面坐标，同时更新机器人和水下特征点的全局坐标信息，并反馈至下一次优化的输入中。
所述步骤2的具体方法为：
步骤2.1：读取连续两帧中所有的特征点像素坐标，并将上一帧中的特征点像素坐标转换成基于声呐极坐标系的球面坐标；判断当前帧的特征匹配数量Nmatch是否不低于最小规定值Nth-match，若是，则直接判定为欠约束帧；若否，则进行下一步。其中最小规定值Nth-match一般根据观测方程数量不小于未知量的原则取值：确保连续两帧之间Nth-match个关联的特征点对应的最大声呐观测方程数量4Nth-match不小于待估计的当前位姿和Nth-match个路标点坐标的参数数量，即4Nth-match≥a+bNth-match，一般在三维场景中a＝6,b＝3，二维场景中a＝3,b＝2；
步骤2.2：读取连续两帧对应的机器人在世界坐标系下的位姿数据，根据上一次后端优化得到的上一帧机器人位姿xi-1(i＝1,2,3,...)、惯性导航设备得到的当前帧机器人位姿xi、当前声呐帧和上一帧的所有特征点球面坐标lj(j＝1,...,M),M为连续两帧的特征点总个数，计算对应测量函数的雅克比矩阵A及其奇异值。
对一个典型二维前视成像声呐而言，一个三维特征点lj＝[ψj,rj,θj]T对应的测量函数h(lj)可表示为：
Figure BDA0002649433760000041
其中pj为特征点球面坐标lj对应的笛卡尔坐标，定义在声呐笛卡尔坐标系上，π(g)为将特征点投影到二维声呐成像平面的投影函数。一般而言，特征点球面坐标lj中的θj是无法通过声呐直接测量得到的，由于二维前视成像声呐的垂直开角通常较小(一般10度以内)，故可作θj≈0°近似处理。此外，假设声呐相对于机器人机体坐标系无相对旋转和平移，即二者位姿等价。
对于连续两帧，其对应的闭式雅克比矩阵A可由如下方式求得：
先计算各子块矩阵：
Figure BDA0002649433760000042
其中，声呐预测函数h(xik,ljk)(k＝1,...,N)将关联的特征ljk投影到关联的位姿xik所在坐标系，从而得到当前位姿上预测的特征点二维极坐标，N为连续两帧之间的最大特征关联数量。pjk为上一个位姿xi-1处的声呐笛卡尔坐标，
Figure BDA0002649433760000051
表示pjk在当前位姿xi处对应的重投影坐标，
Figure BDA0002649433760000052
为当前位姿xi的机体坐标系到参考位姿xi-1机体坐标系的旋转矩阵，
Figure BDA0002649433760000053
为参考位姿和当前位姿之间的相对位移。
然后，以位姿编号i和特征点编号j为列，以声呐测量的编号k为行，按上述关联关系将子块矩阵拼接，一般每个声呐测量h(xik,ljk)仅关联一个位姿和一个特征点，故每行仅有两个非零子块矩阵，其余子块则取为零矩阵，即可得到由各子块矩阵组成的雅克比矩阵A。
接下来对雅克比矩阵A进行奇异值分解：
A＝USVT
其中
Figure BDA0002649433760000054
和
Figure BDA0002649433760000055
均为正交矩阵，
Figure BDA0002649433760000056
为包含奇异值对角方阵σn的矩阵，其中奇异值为σ1≥...≥σn。
步骤2.3：若最小奇异值σmin小于一定阈值σth-low，则判定当前帧为欠约束帧；若最小奇异值大于一定阈值σth-high，则判定当前帧为关键约束帧，并存储至关键约束帧库；其中阈值σth-low、σth-high均可根据试验中计算多个欠约束帧和关键约束帧对应的最小奇异值数据进行经验性选取；
步骤2.4：判定为欠约束帧的当前声呐帧，不加入滑动窗口优化，其对应的机器人位姿由惯性导航设备预测更新，对应的水下特征坐标不更新。
所述步骤3的具体方法为：
步骤3.1在关键约束帧库中，对于待加入到滑动窗口优化的过往帧，先判断该过往帧匹配的特征和当前帧匹配的特征中feature_id相同的数量NcoFea是否不小于阈值Nth-coFea，若是，则进行下一步；阈值Nth-coFea可根据经验设置；
步骤3.2在满足步骤3.1条件的所有过往帧中，将所有过往帧按最小奇异值的大小从高到低排序，并计算所有帧最小奇异值的平均值σmin-mean，选择最小奇异值不小于σmin-mean的过往帧加入到滑动窗口优化，即：
Figure BDA0002649433760000057
其中
Figure BDA0002649433760000061
表示满足步骤3.1条件的过往帧的最小奇异值。
与现有技术相比，本发明的有益效果为：
本发明实现了一种基于关键约束帧的前视成像声呐水下定位与导航方法，可以通过闭式雅克比矩阵判定声呐帧的约束情况，实现对异常值的检测与处理，增强了定位和导航系统的鲁棒性；将声呐有效约束帧存储，并从中选取满足一定条件的帧加入滑动窗口，对于特征稀疏的水下而言，可最大化利用过往声呐帧中的有效约束信息，避免稀疏约束信息的浪费，有效提高精度；此外，本发明方法还设计了可根据一定准则自动调节的滑动窗口，保证优化规模在较小范围内，复杂度不随时间增长，有利于水下机器人长时间定位和导航。
附图说明
图1为基于关键约束帧的前视成像声呐水下定位与导航方法原理图。
图2为水下机器人的前视成像声呐和IMU安装位置与坐标系示意图。
图3为应用基于关键约束帧的前视成像声呐水下定位与导航方法和现有方法的水下机器人的轨迹和路标位置对比图。
图4为应用基于关键约束帧的前视成像声呐水下定位与导航方法和现有方法的水下机器人的定位和导航结果对比图。
具体实施方式
下面结合附图对本发明方法的实施方式做详细说明。本发明的原理图如图1所示，其具体步骤包括：
步骤1，水下机器人将机载惯性测量单元、深度计等导航设备产生的数据处理后，得到惯性导航设备推测的全局机器人位姿
Figure BDA0002649433760000062
通过二维前视成像声呐获取二维声呐图像，经过特征提取与匹配后，得到声呐连续两帧之间的特征匹配，并进行特征点全局编号，以feature_id表示，每一帧观测到的所有特征点均以像素坐标形式按feature_id顺序存储到特征点数据库。若匹配成功或检测到已经存在于特征点数据库中，则判定为旧特征点，全局编号不更新；否则判定为当前帧观测到的新特征点，按特征观测的先后顺序依次编号；该步骤方法均为常规方法，其中惯性导航设备推测过程一般由惯性导航系统内部集成的算法完成；特征提取与匹配一般采用A-KAZE算法和RANSAC(随机采样一致性)方法以保证准确性；特征点像素坐标转换到球面坐标l的过程可参考文献[4]，其具体转换过程与声呐实际参数有关；
步骤2，读取连续两帧中机器人在世界坐标系下的位姿和所有特征点像素坐标，并将特征点像素坐标转换成基于声呐极坐标系的球面坐标；判断当前帧的特征匹配数量Nmatch是否不低于最小规定值Nth-match，若是，则直接判定为欠约束帧；若否，则进行下一步；读取连续两帧对应的机器人在世界坐标系下的位姿数据，根据上一次后端优化得到的上一帧机器人位姿xi-1、惯性导航设备得到的当前帧机器人位姿xi、当前声呐帧和上一帧的所有特征点球面坐标lj(j＝1,...,M),计算对应测量函数的雅克比矩阵A及其奇异值；
所述计算雅克比矩阵及其奇异值的具体过程如下：
如图2所示，对一个典型二维前视成像声呐而言，一个三维特征点lj＝[ψj,rj,θj]T对应的测量函数h(lj)可表示为：
Figure BDA0002649433760000071
其中pj为特征点球面坐标lj对应的笛卡尔坐标，定义在声呐笛卡尔坐标系上，π(g)为将特征点投影到二维声呐成像平面的投影函数。一般而言，特征点球面坐标lj中的θj是无法通过声呐直接测量得到的，由于二维前视成像声呐的垂直开角通常较小(一般10度以内)，故可作θj≈0°近似处理。
假设前视成像声呐测量函数和惯性导航设备测量函数的噪声分别服从
Figure BDA0002649433760000072
和
Figure BDA0002649433760000073
的零均值高斯分布，其中Σ和Λ均为协方差矩阵，且假设声呐相对于机器人机体坐标系无相对旋转和平移，即二者位姿等价，则惯性导航设备和前视成像声呐的实际测量函数可分别表示为：
Figure BDA0002649433760000074
其中，
Figure BDA0002649433760000075
表示惯性导航得到的第i个实际位姿，
Figure BDA0002649433760000076
表示第k个关联了位姿xik和特征点ljk的声呐实际测量，N为连续两帧之间的最大特征关联数量。g(g)为惯性导航设备的测量函数，声呐预测函数h(xik,ljk)将关联的特征ljk投影到关联的位姿xik所在坐标系，从而得到当前位姿上预测的特征点二维极坐标
Figure BDA0002649433760000081
对于连续两帧，可令xA＝xi-1＝06×1为参考位姿，
Figure BDA0002649433760000082
表示两帧之间的相对位姿，其中
Figure BDA0002649433760000083
为相对姿态角(欧拉角)，
Figure BDA0002649433760000084
为相对位移。因此对于位姿xA处观测到的每个特征点，其在位姿xB处的声呐预测值可表示为：
Figure BDA0002649433760000085
Figure BDA0002649433760000086
其中pjk为位姿xA处的声呐笛卡尔坐标，qjk是其在位姿xB处对应的重投影坐标。
Figure BDA0002649433760000087
为齐次变换矩阵，其中
Figure BDA0002649433760000088
为参考位姿的机体坐标系到当前位姿机体坐标系的旋转矩阵，相对旋转角度为
Figure BDA0002649433760000089
对于连续两帧，其对应的闭式雅克比矩阵A可由如下方式求得：
先计算各子块矩阵(注：k已省略)：
Figure BDA00026494337600000810
具体而言，对于连续两帧，其位姿xA和xB对应的雅克比子块矩阵分别为：
Figure BDA00026494337600000811
Figure BDA00026494337600000812
Figure BDA00026494337600000813
Figure BDA00026494337600000814
其中
Figure BDA00026494337600000815
为反对称矩阵：
Figure BDA00026494337600000816
然后，以位姿编号A,B和特征点编号j为列，以声呐测量的编号k为行，按上述关联关系将子块矩阵拼接，一般每个声呐测量h(xik,ljk)仅关联一个位姿和一个特征点，故每行仅有两个非零子块矩阵，其余子块则取为零矩阵，即可得到由各子块矩阵组成的雅克比矩阵A。
接下来对雅克比矩阵A进行奇异值分解：
A＝USVT
其中
Figure BDA0002649433760000091
和
Figure BDA0002649433760000092
均为正交矩阵，
Figure BDA0002649433760000093
为包含奇异值对角方阵σn的矩阵，其中奇异值为σ1≥...≥σn。
若最小奇异值σmin小于一定阈值σth-low，则判定当前帧为欠约束帧；若最小奇异值大于一定阈值σth-high，则判定当前帧为关键约束帧，并存储至关键约束帧库；判定为欠约束帧的当前声呐帧，不加入滑动窗口优化，其对应的机器人位姿由惯性导航设备预测更新，对应的水下特征坐标不更新。
步骤3，在关键约束帧库中，对于待加入到滑动窗口优化的过往帧，先判断该过往帧匹配的特征和当前帧匹配的特征中feature_id相同的数量NcoFea是否不小于阈值Nth-coFea，若是，则进行下一步；在满足上一步条件的所有过往帧中，将所有过往帧按最小奇异值的大小从高到低排序，并计算所有帧最小奇异值的平均值σmin-mean，选择最小奇异值不小于σmin-mean的过往帧加入到滑动窗口优化，即：
Figure BDA0002649433760000094
步骤4，判断当前滑动窗口的大小NS是否超过最大限制NSmax，若是，若当前滑动窗口大小超过最大限制时，则只保留当前窗口内最小奇异值排序在前NSmax-2的过往帧，并移除其余的过往帧，滑动窗口大小NS在2和最大窗口限制NSmax之间自动调节；若否，则在窗口确定后开始通过非线性优化器对滑动窗口进行优化；
所述优化的具体过程如下：
上述定位和导航问题即在给定观测值
Figure BDA0002649433760000095
时，找到一个最大化的当前位姿和路标位置的概率集合Θ＝{xi,lj}，即求解最大化后验概率问题(MAP)：
Figure BDA0002649433760000101
其中x0为位姿先验值。根据概率图优化理论，上述MAP问题可以转化为非线性最小二乘形式：
Figure BDA0002649433760000102
注意，先验项为常数项，可以舍去，惯性导航误差项已经包含在连续两帧之间的相对位姿xB中。
对上式在线性化点处Θ0(即迭代优化的初值点处)进行一阶泰勒展开，可得到标准线性最小二乘问题：
Figure BDA0002649433760000103
其中A为与声呐测量函数有关的雅克比矩阵，Δ*为待求解的状态更新向量。
上述优化过程可使用高斯-牛顿法迭代求解。
滑动窗口优化完成后，可得到Δ*＝{xi,Δli}，从而求得机器人相对位姿及其测量的水下特征点坐标变化量集合
Figure BDA0002649433760000104
进而对世界坐标系下的机器人位姿和特征点位置进行更新，机器人全局位姿[Ri w|Pi w]的更新公式如下：
Figure BDA0002649433760000105
其中
Figure BDA0002649433760000106
表示机体坐标系到世界坐标系的旋转矩阵。
特征点全局位置
Figure BDA0002649433760000107
的更新公式如下：
Figure BDA0002649433760000108
更新后的全局位姿和特征点位置将反馈到下一次优化的输入。
结合图2、图3和图4说明本发明的实例仿真验证，仿真水下机器人为一台悬浮在大尺度海洋模拟环境下的遥控无人潜水器(ROV)，其中惯性测量单元(IMU)安装在ROV的质心处，IMU坐标系与ROV机体坐标系重合，多波束二维前视成像声呐安装在ROV机体坐标系的[1.4,0,0.6]m处，且无相对旋转。水下放置了6个全局位置已知的人工路标，沿x轴的上下两侧均匀分布，且深度均为0.5m。ROV在水下保持一个较小的恒定速度沿世界坐标系x轴行进，深度保持在0.5m，采集包含声呐图像、IMU数据和真实值(Ground Truth)的水下数据集。实验过程的计算条件和技术参数如下：
模拟IMU测量频率为50Hz，测量噪声方差均为0.01；
模拟前视成像声呐测量频率为5～10Hz，最大方位角范围为[-45°，45°]，方位角测量噪声方差为0.05，距离测量范围为[1,8]m，距离测量噪声方差为0.05；
ROV在世界坐标系下的初始坐标为[0.14,0,-0.5]m，初始速度为[0,0,0]m/s，模拟海水流速为[-0.01,-0.01,0]m/s。
分别使用本发明的方法和目前的代表性方法(参见参考文献[5]，作为对比方法)进行了正常情况和非正常情况的对比实验。在正常情况中，声呐图像的特征匹配经过人工检查以确保匹配的完全准确性，在非正常情况中，在10s左右向连续两帧声呐图像匹配的特征点(对应路标2)的像素坐标加入人为扰动，以模拟异常值。
基于本发明的定位和导航方法与上述的技术参数，以及目前的代表性方法，正常情况和非正常情况下通过优化估计得到的ROV航行轨迹和人工路标位置如图3所示，实验时间为30s。由图3可以看出本方法和对比方法在两种不同情况下分别估计的轨迹和路标位置，以及真实轨迹和路标位置。由图4可以看出对应的ROV估计位置均方根误差(RMSE)曲线，在正常情况下，两种方法的精度均至少比航位推测法(DR)提高40％以上，而本发明的方法比对比方法的精度高出约10％；在加入异常值后，本方法可以迅速检测到异常并纠正偏差，总体误差变化较小，其路标位置估计的平均绝对误差(MAE)为x方向0.09m，y方向0.13m，均控制在0.15m内，而对比方法在遭遇扰动后纠偏速度很慢，最后发生崩溃，其路标位置估计的MAE为0.84m，y方向1.31m。
由实验结果可知，本发明可以基于关键约束帧进行前视成像声呐水下定位与导航，在正常和存在异常值的情况下均能有效提高水下定位和导航精度。

Claims (3)

1.基于关键约束帧的前视成像声呐水下定位与导航方法，其特征在于，包括以下步骤：
步骤1：前端数据处理：水下机器人依据惯性导航设备产生的数据得到推测的机器人位姿；通过二维前视成像声呐获取二维声呐图像，经过特征提取与匹配后，得到声呐连续两帧之间的特征匹配，并进行特征点全局编号，以feature_id表示，每一帧观测到的所有特征点均以像素坐标形式按feature_id顺序存储到特征点数据库，若匹配成功或检测到已经存在于特征点数据库中，则判定为旧特征点，全局编号不更新；否则判定为当前帧观测到的新特征点，按特征观测的先后顺序依次编号；
步骤2：声呐关键约束帧的识别与存储：读取连续两帧中机器人在世界坐标系下的位姿和所有特征点像素坐标，并将特征点像素坐标转换成基于声呐极坐标系的球面坐标；判定当前帧是否为欠约束帧，若是，则另行处理；若否，则进一步判定当前帧是否为声呐关键约束帧，若是，则将当前帧存储至关键约束帧库；具体方法为：
步骤2.1：读取连续两帧中所有的特征点像素坐标，并将上一帧中的特征点像素坐标转换成基于声呐极坐标系的球面坐标；判断当前帧与上一帧的特征匹配数量Nmatch是否不低于最小规定值Nth-match，若是，则直接判定为欠约束帧；若否，则进行下一步；
步骤2.2：读取连续两帧对应的机器人在世界坐标系下的位姿数据，根据上一次后端优化得到的上一帧机器人位姿xi-1，i＝1,2,3,...、惯性导航设备得到的当前帧机器人位姿xi、当前声呐帧和上一帧的所有特征点球面坐标lj，j＝1,...,M,计算对应测量函数的雅克比矩阵A及其奇异值；
步骤2.3：若最小奇异值σmin小于一定阈值σth-low，则判定当前帧为欠约束帧；若最小奇异值大于一定阈值σth-high，则判定当前帧为关键约束帧，并存储至关键约束帧库；
步骤2.4：判定为欠约束帧的当前声呐帧，不加入滑动窗口优化，其对应的机器人位姿由惯性导航设备预测更新，对应的水下特征坐标不更新；
步骤3：从关键约束帧库中选择可加入滑动窗口优化的过往帧：对于关键约束帧库中存储的过往帧，判断其能否加入到当前滑动窗口优化中，若能则选择性加入，若否则不加入；具体方法为：
步骤3.1在关键约束帧库中，对于待加入到滑动窗口优化的过往帧，先判断该过往帧匹配的特征和当前帧匹配的特征中feature_id相同的数量NcoFea是否不小于阈值Nth-coFea，若是，则进行下一步；
步骤3.2在满足步骤3.1条件的所有过往帧中，将所有过往帧按最小奇异值的大小从高到低排序，并计算所有帧最小奇异值的平均值σmin-mean，选择最小奇异值不小于σmin-mean的过往帧加入到滑动窗口优化；
步骤4：窗口大小控制与优化：判断当前滑动窗口的大小是否超过最大限制，若是，则从当前窗口内移除部分过往帧；若否，则在窗口确定后开始通过非线性优化器对滑动窗口进行优化；优化完成后，得到估计的机器人相对位姿及水下特征点球面坐标，同时更新机器人和水下特征点的全局坐标信息，并反馈至下一次优化的输入中。
2.根据权利要求1所述的基于关键约束帧的前视成像声呐水下定位与导航方法，其特征在于，所述的前视成像声呐为多波束二维前视成像声呐。
3.根据权利要求1所述的基于关键约束帧的前视成像声呐水下定位与导航方法，其特征在于，步骤4所述的滑动窗口大小不是固定的，可以根据是否满足步骤3的判定准则进行自动调节，其大小在2和最大窗口限制NSmax之间取值；若当前滑动窗口大小超过最大限制时，则只保留当前窗口内最小奇异值排序在前NSmax-2的过往帧，并移除其余的过往帧。
CN202010864951.9A 2020-08-25 2020-08-25 基于关键约束帧的前视成像声呐水下定位与导航方法 Active CN112146654B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010864951.9A CN112146654B (zh) 2020-08-25 2020-08-25 基于关键约束帧的前视成像声呐水下定位与导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010864951.9A CN112146654B (zh) 2020-08-25 2020-08-25 基于关键约束帧的前视成像声呐水下定位与导航方法

Publications (2)

Publication Number Publication Date
CN112146654A CN112146654A (zh) 2020-12-29
CN112146654B true CN112146654B (zh) 2022-08-23

Family

ID=73889039

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010864951.9A Active CN112146654B (zh) 2020-08-25 2020-08-25 基于关键约束帧的前视成像声呐水下定位与导航方法

Country Status (1)

Country Link
CN (1) CN112146654B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112802195B (zh) * 2020-12-30 2022-04-29 浙江大学 一种基于声呐的水下机器人连续占据建图方法
CN112946660A (zh) * 2021-01-28 2021-06-11 西北工业大学 一种基于多波束前视声呐的同时定位与建图方法
CN113052940B (zh) * 2021-03-14 2024-03-15 西北工业大学 一种基于声呐的时空关联地图实时构建方法
CN113075665B (zh) * 2021-03-24 2023-06-20 鹏城实验室 水下定位方法、水下载体航行器及计算机可读存储介质
CN113108774B (zh) * 2021-05-21 2023-11-10 深圳市人工智能与机器人研究院 水下机器人及水下机器人导航定位方法
CN114228959B (zh) * 2021-12-29 2023-03-21 中国科学院沈阳自动化研究所 一种基于声学路标与光学路标组合辅助导航的水下机器人极地冰下回收方法
CN117824664B (zh) * 2024-03-05 2024-05-28 河海大学 基于多波束测深声呐的自主无人系统主动slam方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5155706A (en) * 1991-10-10 1992-10-13 Westinghouse Electric Corp. Automatic feature detection and side scan sonar overlap navigation via sonar image matching
CN101672916A (zh) * 2009-09-17 2010-03-17 中国海洋大学 机械扫描式成像声呐的运动失真补偿方法
CA2814833A1 (en) * 2010-10-25 2012-05-10 Sekhar C. Tangirala Estimating position and orientation of an underwater vehicle based on correlated sensor data
CN105856230A (zh) * 2016-05-06 2016-08-17 简燕梅 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法
CN108318034A (zh) * 2018-01-09 2018-07-24 浙江大学 一种基于声呐地图的auv回坞导航方法
JP2018177074A (ja) * 2017-04-18 2018-11-15 国立大学法人 東京大学 自律型水中ロボット及びその制御方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11199414B2 (en) * 2016-09-14 2021-12-14 Zhejiang University Method for simultaneous localization and mapping
US10948297B2 (en) * 2018-07-09 2021-03-16 Samsung Electronics Co., Ltd. Simultaneous location and mapping (SLAM) using dual event cameras

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5155706A (en) * 1991-10-10 1992-10-13 Westinghouse Electric Corp. Automatic feature detection and side scan sonar overlap navigation via sonar image matching
CN101672916A (zh) * 2009-09-17 2010-03-17 中国海洋大学 机械扫描式成像声呐的运动失真补偿方法
CA2814833A1 (en) * 2010-10-25 2012-05-10 Sekhar C. Tangirala Estimating position and orientation of an underwater vehicle based on correlated sensor data
CN105856230A (zh) * 2016-05-06 2016-08-17 简燕梅 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法
JP2018177074A (ja) * 2017-04-18 2018-11-15 国立大学法人 東京大学 自律型水中ロボット及びその制御方法
CN108318034A (zh) * 2018-01-09 2018-07-24 浙江大学 一种基于声呐地图的auv回坞导航方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Stability analysis of discrete-time BAM neural networks based on standard neural network models;Zhang Senlin 等;《Journal of Zhejiang University Science A》;20051231(第7期);第689-696页 *
基于帧差法的多波束前视声呐运动目标检测;崔杰等;《仪器仪表学报》;20180228;第39卷(第02期);第169-176页 *

Also Published As

Publication number Publication date
CN112146654A (zh) 2020-12-29

Similar Documents

Publication Publication Date Title
CN112146654B (zh) 基于关键约束帧的前视成像声呐水下定位与导航方法
CN109239709B (zh) 一种无人船的局部环境地图自主构建方法
CN109781093B (zh) 一种水下目标磁性定位跟踪系统与方法
Guth et al. Underwater SLAM: Challenges, state of the art, algorithms and a new biologically-inspired approach
KR102339465B1 (ko) 딥러닝-영상인식 기반 해상폐기물 제거용 자율운항 선박 시스템
Chen et al. RBPF-MSIS: Toward rao-blackwellized particle filter SLAM for autonomous underwater vehicle with slow mechanical scanning imaging sonar
CN101907711A (zh) 一种自主水下机器人的同时检测与定位方法
CN110378411B (zh) 一种基于交互式多模型的支持向量机辅助水下机动目标跟踪方法
CN113010958A (zh) 自航船舶的模拟系统及其运作方法
Li et al. Terminal underwater docking of an autonomous underwater vehicle using one camera and one light
CN110427030B (zh) 一种基于Tiny-YOLOship目标检测算法的无人艇自主对接回收方法
CN110533650B (zh) 一种基于视觉的auv水下管道检测跟踪方法
Duecker et al. RGB-D camera-based navigation for autonomous underwater inspection using low-cost micro AUVs
Thompson Maritime object detection, tracking, and classification using lidar and vision-based sensor fusion
CN110908404B (zh) 一种基于数据驱动的auv智能观测运动方法
Zalewski et al. Computer Vision-Based Position Estimation for an Autonomous Underwater Vehicle
CN117788572A (zh) 光学图像和激光点云的融合船舶精定位方法及系统
Bjerkeng et al. Absolute localization of an ROV in a Fish Pen using Laser Triangulation
Oleynikova et al. Perimeter patrol on autonomous surface vehicles using marine radar
CN116597141A (zh) 一种基于声呐图像语义分割的海底电缆检测跟踪方法
Maurelli et al. Sonar-based AUV localization using an improved particle filter approach
Kim et al. Simulation and feasibility test of mini-rovs with auv for the manipulation purpose
CN114396921A (zh) 一种基于无人机的钱塘江涌潮高度及传播速度测量方法
Lee et al. A docking and control system for an autonomous underwater vehicle
Xu et al. Keyframe-based imaging sonar localization and navigation using elastic windowed optimization

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