CN111007518B - 基于声纳图像处理的水下机器人水下定位和路径规划方法 - Google Patents

基于声纳图像处理的水下机器人水下定位和路径规划方法 Download PDF

Info

Publication number
CN111007518B
CN111007518B CN201911264682.6A CN201911264682A CN111007518B CN 111007518 B CN111007518 B CN 111007518B CN 201911264682 A CN201911264682 A CN 201911264682A CN 111007518 B CN111007518 B CN 111007518B
Authority
CN
China
Prior art keywords
underwater
underwater robot
grid
information
robot
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
CN201911264682.6A
Other languages
English (en)
Other versions
CN111007518A (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.)
Nanjing Institute of Technology
Original Assignee
Nanjing Institute of 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 Nanjing Institute of Technology filed Critical Nanjing Institute of Technology
Priority to CN201911264682.6A priority Critical patent/CN111007518B/zh
Publication of CN111007518A publication Critical patent/CN111007518A/zh
Application granted granted Critical
Publication of CN111007518B publication Critical patent/CN111007518B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
    • 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)
  • Aviation & Aerospace Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Acoustics & Sound (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供了一种基于声纳图像处理的水下机器人水下定位和路径规划方法，包括以下步骤：获取水下结构的声纳图像、方向信息、位置信息和几何信息；获取水下机器人在水下结构中的运行情况；根据水下结构的声纳图像、方向信息、位置信息、几何信息和水下机器人的运行情况得到水下机器人的当前位置；根据水下结构的方向信息、位置信息和几何信息构建占用网格图；根据原始图像和占用网格图检测水下机器人在水下结构中运行遇到的障碍物；根据占用网格图和障碍物规划水下机器人的运行路径。本发明能够降低水下机器人在水下定位和路径规划过程中受到的测量偏差影响和水下噪声干扰的问题，从而提高水下机器人水下定位和路径规划的准确度和适应性。

Description

基于声纳图像处理的水下机器人水下定位和路径规划方法
技术领域
本发明涉及自动定位和自动导航技术领域，具体涉及一种基于声纳图像处理的水下机器人水下定位和路径规划方法。
背景技术
自主水下机器人导航一般是利用惯性测量单元、姿态和航向参考系统和多普勒测速仪进行惯性导航，但是这些传感器容易受到偏差的影响，导致导航的精度较低。
水下机器人自主导航方法当前有使用视觉传感器或声呐传感器两种，其中，基于视觉的算法无法处理已安装的海底结构附近的定位任务，因为该算法需要目标对象的特定配置，例如附加基准标记，因此基于视觉的算法适应性较小；此外，基于声呐的算法容易受到偏差的影响，而且水下噪声干扰很大。
发明内容
本发明旨在至少在一定程度上解决上述技术中的技术问题之一。为此，本发明的目的在于提出一种基于声纳图像处理的水下机器人水下定位和路径规划方法，能够降低水下机器人在水下定位和路径规划过程中受到的测量偏差影响和水下噪声干扰的问题，从而提高水下机器人水下定位和路径规划的准确度和适应性。
为达到上述目的，本发明实施例提出了一种基于声纳图像处理的水下机器人水下定位和路径规划方法，包括以下步骤：获取水下结构的声纳图像、方向信息、位置信息和几何信息；获取水下机器人在所述水下结构中的运行情况；根据所述水下结构的声纳图像、方向信息、位置信息、几何信息和所述水下机器人的运行情况得到所述水下机器人的当前位置；根据所述水下结构的方向信息、位置信息和几何信息构建占用网格图；根据所述原始图像和所述占用网格图检测所述水下机器人在所述水下结构中运行遇到的障碍物；根据所述占用网格图和所述障碍物规划所述水下机器人的运行路径。
根据本发明实施例的基于声纳图像处理的水下机器人水下定位和路径规划方法，首先获取水下结构的声纳图像、方向信息、位置信息和几何信息以及水下机器人在水下结构中的运行情况，其次根据水下结构的声纳图像、方向信息、位置信息、几何信息和水下机器人的运行情况得到水下机器人的当前位置，并根据水下结构的方向信息、位置信息和几何信息构建占用网格图，然后根据原始图像和占用网格图检测水下机器人在水下结构中运行遇到的障碍物，最后根据占用网格图和障碍物规划水下机器人的运行路径，由此，能够降低水下机器人在水下定位和路径规划过程中受到的测量偏差影响和水下噪声干扰的问题，从而提高水下机器人水下定位和路径规划的准确度和适应性。
另外，根据本发明上述实施例提出的基于声纳图像处理的水下机器人水下定位和路径规划方法还可以具有如下附加的技术特征：
根据本发明的一个实施例，根据所述水下结构的声纳图像、方向信息、位置信息、几何信息和所述水下机器人的运行情况得到所述水下机器人的当前位置，包括：根据所述水下结构的声纳图像、方向信息、位置信息和所述水下机器人的运行情况得到所述水下机器人的预估位置和方向信息；根据所述水下结构的方向信息、位置信息、几何信息与所述水下机器人的运行情况和预估位置、方向信息得到所述水下结构的结构水平图像和原始图像；根据所述预估位置和所述结构水平图像得到所述水下机器人的当前位置。
进一步地，根据所述预估位置和所述结构水平图像得到所述水下机器人的当前位置的表达式为：
Figure GDA0004178435760000021
其中，
Figure GDA0004178435760000031
表示所述预估位置，(xupdata，yupdata)表示所述水下机器人位置更新信息，ψ0表示从NED帧到本地帧的旋转角度，λ表示经度，L表示纬度，(δλ，δL)表示所述水下机器人的初始位置，RN和RE是两种地球半径参数，vλ和vL是两个高斯白噪声，对应计算所述水下机器人位置更新信息的公式，其中vλ～N(0,Rvλ)，vL～N(0,RvL)，Rvλ为vλ的协方差，RvL为vL的协方差。
进一步地，计算所述水下机器人位置更新信息的公式为：
Figure GDA0004178435760000032
其中，k表示所述结构水平图像沿垂直轴向下移动的网格编号，l表示所述结构水平图像沿水平轴向右移动的网格编号，Rres表示所述结构水平图像的图像分辨率。
根据本发明的一个实施例，根据所述水下结构的方向信息、位置信息和几何信息构建占用网格图包括：设定水底占据的网格为值1，其余所有网格为值0；设定目标点或目标网格；采用位势函数为所有值为0的网格分配唯一值。
进一步地，所述位势函数为：
Figure GDA0004178435760000033
其中，U(q)表示所有值为0的网格分配的唯一值，rtp表示当前网格到所述目标网格之间的距离，ri表示当前网格到具有1值的网格之间的距离，catt，crep，a和b是设计参数，n表示具有1值的网格编号
根据本发明的一个实施例，根据所述原始图像和所述占用网格图检测所述水下机器人在所述水下结构中运行遇到的障碍物包括：修改所述原始图像分辨率；对修改后的所述原始图像进行坐标转换；对坐标转换后的所述原始图像进行图像滤波；根据所述占用网格图进行所述障碍物检测。
进一步地，搜索所述占用网格图中网格相连的白色网格组，若所述白色网格组中的网格数大于阈值，则判定所述白色网格组为所述障碍物。
根据本发明的一个实施例，根据所述占用网格图和所述障碍物规划所述水下机器人的运行路径包括：搜索所述水下机器人当前位置的相邻网格中未搜索的网格；选择所述未搜索的网格中唯一值最小的网格作为所述水下机器人的运行路径；若所述水下机器人沿所述搜索的运行路径运动时检测到所述障碍物，则将所述障碍物所在网格的值设定为1，并重置所有网格值不为1的网格；根据重置后的占用网格图搜索另一条运动路径以接近所述目标网格；选取所述相邻网格中唯一值最小网格作为所述水下机器人的运行路径。
附图说明
图1为本发明实施例的基于声纳图像处理的水下机器人水下定位和路径规划方法的流程图；
图2为本发明一个实施例的水下机器人在水下结构中的运行情况示意图；
图3为本发明一个实施例的水下结构的结构水平图像；
图4(a)为本发明一个实施例的水下结构的原始图像；
图4(b)为本发明一个实施例的图像分辨率修改后的水下结构的原始图像；
图4(c)为本发明一个实施例的变换到本地帧的的水下结构的原始图像；
图4(d)为本发明一个实施例的滤波处理后的水下结构的原始图像；
图4(e)为本发明一个实施例的障碍物检测的示意图；
图5(a)为本发明一个实施例的初始占用网格图；
图5(b)为本发明一个实施例的没有障碍物的水下机器人的运行路径；
图5(c)为本发明一个实施例的具有障碍物的水下机器人的运行路径；
图6为本发明一个具体实施例的复杂结构的示意图；
图7(a)为本发明一个具体实施例的没有障碍物的水下机器人的运行路径；
图7(b)为本发明一个具体实施例的具有障碍物的水下机器人的运行路径。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
图1为本发明实施例的基于声纳图像处理的水下机器人水下定位和路径规划方法的流程图。
如图1所示，本发明实施例的基于声纳图像处理的水下机器人水下定位和路径规划方法，包括以下步骤：
S1，获取水下结构的声纳图像、方向信息、位置信息和几何信息。
具体地，可通过双频识别声纳获取水下结构的声纳图像、方向信息、位置信息和几何信息。其中，水下结构可为人工设计并安装在海底的设施，并且水下结构的安装位置还可放置正交防波堤，例如图2所示，水下结构为框架结构，并且在水下结构的两边放置两个正交防波堤，通过双频识别声纳可获取图2所示的水下结构的声纳图像，并获取水下结构相对于两个正交防波堤的方向信息和位置信息，同时获取水下结构的几何形状信息。
此外，在无法使用双频识别声纳获取水下结构的声纳图像、方向信息、位置信息和几何信息时，还可通过前视避碰声纳获取水下结构的声纳图像、方向信息、位置信息和几何信息。
S2，获取水下机器人在水下结构中的运行情况。
具体地，可通过运动传感器，例如位姿和航向参考系统、多普勒测速仪获取水下机器人在水下结构中的运行情况，例如位姿和速度信息，具体如图2所示。
S3，根据水下结构的声纳图像、方向信息、位置信息、几何信息和水下机器人的运行情况得到水下机器人的当前位置。
具体地，步骤S3包括：根据水下结构的声纳图像、方向信息、位置信息和水下机器人的运行情况得到水下机器人的预估位置和方向信息；根据水下结构的方向信息、位置信息、几何信息与水下机器人的运行情况和预估位置、方向信息得到水下结构的结构水平图像和原始图像；根据预估位置和结构水平图像得到水下机器人的当前位置。
更具体地，可根据图3所示的结构水平图像的移动，例如可根据图3所示的结构水平图像沿x轴移动得到第一图像和沿y轴移动得到第二图像，并设定偏移值(k,l)对应第一、第二图像匹配的最大相关值，以获得水下机器人位置更新信息，具体可通过下列公式计算水下机器人位置更新信息：
Figure GDA0004178435760000061
其中，k表示结构水平图像沿垂直轴向下移动的网格编号，l表示结构水平图像沿水平轴向右移动的网格编号，Rres表示结构水平图像的图像分辨率。
进一步地，可根据水下机器人位置更新信息和水下机器人预估位置确定水下机器人当前位置，具体可通过下列公式计算水下机器人当前位置：
Figure GDA0004178435760000062
其中，
Figure GDA0004178435760000063
表示预估位置，(xupdata，yupdata)表示水下机器人位置更新信息，ψ0表示从NED帧到本地帧的旋转角度，λ表示经度，L表示纬度，(δλ，δL)表示水下机器人的初始位置，RN和RE是两种地球半径参数，vλ和vL是两个高斯白噪声，对应上述计算水下机器人位置更新信息的公式，其中vλ～N(0,Rvλ)，vL～N(0,RvL)，Rvλ为vλ的协方差，RvL为vL的协方差。
S4，根据水下结构的方向信息、位置信息和几何信息构建占用网格图。
具体地，步骤S4包括：设定水底占据的网格为值1，其余所有网格为值0；设定目标点或目标网格；采用位势函数为所有值为0的网格分配唯一值。
其中，位势函数为：
Figure GDA0004178435760000071
其中，U(q)表示所有值为0的网格分配的唯一值，rtp表示当前网格到目标网格之间的距离，ri表示当前网格到具有1值的网格之间的距离，catt，crep，a和b是设计参数，n表示具有1值的网格编号。
在本发明的一个实施例中，占用网格图中的网格可为设定尺寸的立方体，具体可为0.3m×0.3m×0.3m的立方体。通过选用设定尺寸的立方体作为占用网格图中的网格，能够避免该网格尺寸过大导致难以区分水下结构的位置，从而造成水下结构即使存在水下机器人可通过的路径，但是无法检测出来的问题，同时能够避免该网格尺寸过小导致地图搜索时间成倍增加的问题。
S5，根据原始图像和占用网格图检测水下机器人在水下结构中运行遇到的障碍物。
具体地，步骤S5包括：修改原始图像分辨率；对修改后的原始图像进行坐标转换；对坐标转换后的原始图像进行图像滤波；根据占用网格图进行障碍物检测。
更具体地，在每个时间步，即水下机器人每次进行障碍物检测的时间段，对图4(a)所示的原始图像进行检测分析，即基于占用网格图对图4(a)所示的原始图像进行网格搜索，进一步需要理解的是，前视避碰声纳的图像分辨率为2cm，而水下机器人在实际应用中并不需要前视避碰声纳达到2cm的图像分辨率来检测障碍物，因此，在本发明实施例中可将前视避碰声纳的图像分辨率降低为原来的五倍，即将前视避碰声纳的图像分辨率降低为10cm，以得到图4(b)所示的图像分辨率修改后的水下结构的原始图像。
进一步地，可对图4(b)所示的图像分辨率修改后的水下结构的原始图像进行坐标转换，即从NED帧到本地帧的坐标转换，以得到图4(c)所示的适应本地帧图像，然后可采用适当的阈值设计参数，以对图4(c)所示的适应本地帧图像进行滤波处理，以得到图4(d)所示的滤波图像，最后采用占用网格图根据图4(d)所示的滤波图像检测水下机器人在水下结构中运行遇到的障碍物。
具体地，如图4(e)所示，可通过搜索占用网格图中网格相连的白色网格组，若白色网格组中的网格数大于阈值，则判定白色网格组为水下机器人在水下结构中运行遇到的障碍物。通过该方法还可应用于检测其他局部帧。
S6，根据占用网格图和障碍物规划水下机器人的运行路径。
具体地，步骤S6包括：搜索水下机器人当前位置的相邻网格中未搜索的网格；选择未搜索的网格中唯一值最小的网格作为水下机器人的运行路径；若水下机器人沿搜索的运行路径运动时检测到障碍物，则将障碍物所在网格的值设定为1，并重置所有网格值不为1的网格；根据重置后的占用网格图搜索另一条运动路径以接近目标网格；选取相邻网格中唯一值最小网格作为水下机器人的运行路径。
举例而言，可先通过上述位势函数将所有值为0的网格进行重置并分配唯一值，从而完成图5(a)所示的初始占用网格图的构建，并假设水下机器人处于图5(a)所示的starpoint，即水下机器运行路径的第一网格，需要到达图5(a)所示的endpoint，即水下机器运行路径的最后一个网格，然后可在starpoint所在的网格中，在26个相邻的网格中，搜索未搜索的网格，并将其中唯一值最小的网格作为水下机器人运行路径的第二网格，然后在第二网格相邻的网格中，搜索未搜索的网格，并将其中唯一值最小的网格作为水下机器人运行路径的第三网格，依次搜索并确定第四网格、...、endpoint，即水下机器运行路径的最后一个网格，从而得到图5(b)所示的水下机器人的运行路径。
进一步地，若水下机器人沿图5(b)所示的水下机器人的运行路径运动时检测到图5(c)所示的障碍物，则将障碍物所在网格的值设定为1，并重置所有网格值不为1的网格，然后通过重置后的占用网格图参照上述搜索方法得到图5(c)所示的另一条水下机器人的运行路径以到达endpoint，即水下机器人运行路径的最后一个网格。通过检测障碍物并重新规划运行路径，能够避免与障碍物碰撞。
根据本发明实施例提出的基于声纳图像处理的水下机器人水下定位和路径规划方法，首先获取水下结构的声纳图像、方向信息、位置信息和几何信息以及水下机器人在水下结构中的运行情况，其次根据水下结构的声纳图像、方向信息、位置信息、几何信息和水下机器人的运行情况得到水下机器人的当前位置，并根据水下结构的方向信息、位置信息和几何信息构建占用网格图，然后根据原始图像和占用网格图检测水下机器人在水下结构中运行遇到的障碍物，最后根据占用网格图和障碍物规划水下机器人的运行路径，由此，能够降低水下机器人在水下定位和路径规划过程中受到的测量偏差影响和水下噪声干扰的问题，从而提高水下机器人水下定位和路径规划的准确度和适应性。
为了进一步阐述本发明的基于声纳图像处理的水下机器人水下定位和路径规划方法的适应性，下面将设计制作一个复杂结构并安装在海底，采用本发明的基于声纳图像处理的水下机器人水下定位和路径规划方法引导水下机器人在该复杂结构中运行。
在本发明的一个具体实施例中，上述复杂结构共安装了24个视觉标记，并在该复杂结构附近的海底安装了两个水下双频识别声纳标记，进一步测量该复杂结构相对于两个正交防波堤的位置和方向，进一步如图6所示，可将该防波堤选作本地导航框架的X和Y轴。
进一步地，首先，可手动操作水下机器人并将其航向定向为与两个防波堤壁之一平行，具体可通过监视表面单元上的前视避碰声呐实时图像来执行此操作。通过监视表面单元上的前视避碰声呐实时图像能够获得水下机器人的大致初始航向信息。
其次，水下机器人将从地面单元接收自主运行启动命令，并会运动远离两个防波堤约10米，以避免防波堤附近的海底中的岩石和混凝土结构对多普勒传感器输出上引起测量噪声；水下机器人在远离防波堤的某个水面点后，将运行混合导航算法潜入水下一定深度。
再次，当水下机器人下潜到一定深度后，水下机器人将运行到特定视觉标记前方的位置，并且在运动过程中，由于没有任何人工标记可供使用，因此，难以通过基于视觉的定位方法或基于水下双频识别声呐的定位方法来获取水下机器人位置信息，此时，需要通过本发明基于声纳图像处理的水下机器人水下定位和路径规划方法来进行位置辅助。
然后，水下机器人可通过复杂结构上的视觉标记和位置辅助，以在复杂结构中进出运动，并识别安装在该结构上的一系列视觉标记，视觉标记的数量和顺序信息传输到水下机器。
最后，在完成对给定标记系列的识别之后，水下机器人将返回到指定终点，并同时保持一定深度。在运动过程中，如果水下机器人发现障碍物，将根据本发明的基于声纳图像处理的水下机器人水下定位和路径规划方法重新规划一条路径以避免碰撞，并沿着这条新路径运动。
综上所述，可得到图7(a)和图7(b)所示的实验结果图。
如图7(a)所示，水下机器人的运行路径上没有设置障碍物，水下机器人可从starpoint，即起点运动到点A，然后从点A进入复杂结构，在识别完视觉标记后，水下机器人返回endpoint，即终点。水下机器人从starpoint，即起点运动到点A没有任何人工标记可以帮助确定水下机器人位置信息，在这种情况下，通过使用本发明的基于声纳图像处理的水下机器人水下定位和路径规划方法来提供位置辅助，表明本发明的基于声纳图像处理的水下机器人水下定位和路径规划方法具有较好的水下定位效果。
如图7(b)所示，水下机器人的运行路径上设置障碍物，例如设置了长为2m、直径为0.5m的圆柱体结构，通过使用本发明的基于声纳图像处理的水下机器人水下定位和路径规划方法能够规划一条避免与障碍物发生碰撞的实际运动路径，表明本发明的基于声纳图像处理的水下机器人水下定位和路径规划方法具有较好的路径规划效果。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (6)

1.一种基于声纳图像处理的水下机器人水下定位和路径规划方法，其特征在于，包括以下步骤：
获取水下结构的声纳图像、方向信息、位置信息和几何信息；
获取水下机器人在所述水下结构中的运行情况；
根据所述水下结构的声纳图像、方向信息、位置信息、几何信息和所述水下机器人的运行情况得到所述水下机器人的当前位置，具体根据所述水下结构的声纳图像、方向信息、位置信息和所述水下机器人的运行情况得到所述水下机器人的预估位置和方向信息，并根据所述水下结构的方向信息、位置信息、几何信息与所述水下机器人的运行情况和预估位置、方向信息得到所述水下结构的结构水平图像和原始图像，以及根据所述预估位置和所述结构水平图像得到所述水下机器人的当前位置；
根据所述水下结构的方向信息、位置信息和几何信息构建占用网格图；
根据所述原始图像和所述占用网格图检测所述水下机器人在所述水下结构中运行遇到的障碍物；
根据所述占用网格图和所述障碍物规划所述水下机器人的运行路径；
根据所述水下结构的方向信息、位置信息和几何信息构建占用网格图包括：设定水底占据的网格为值1，其余所有网格为值0；设定目标点或目标网格；采用位势函数为所有值为0的网格分配唯一值，
其中，所述位势函数为：
Figure QLYQS_1
其中，U(q)表示所有值为0的网格分配的唯一值，rtp表示当前网格到所述目标网格之间的距离，ri表示当前网格到具有1值的网格之间的距离，catt，crep，a和b是设计参数，n表示具有1值的网格编号。
2.根据权利要求1所述的基于声纳图像处理的水下机器人水下定位和路径规划方法，其特征在于，根据所述预估位置和所述结构水平图像得到所述水下机器人的当前位置的表达式为：
Figure QLYQS_2
其中，
Figure QLYQS_3
表示所述预估位置，(xupdata，yupdata)表示所述水下机器人位置更新信息，ψ0表示从NED帧到本地帧的旋转角度，λ表示经度，L表示纬度，(δλ，δL)表示所述水下机器人的初始位置，RN和RE是两种地球半径参数，vλ和vL是两个高斯白噪声，对应计算所述水下机器人位置更新信息的公式，其中/>
Figure QLYQS_4
Rvλ为vλ的协方差，RvL为vL的协方差。
3.根据权利要求2所述的基于声纳图像处理的水下机器人水下定位和路径规划方法，其特征在于，其中，计算所述水下机器人位置更新信息的公式为：
Figure QLYQS_5
其中，k表示所述结构水平图像沿垂直轴向下移动的网格编号，l表示所述结构水平图像沿水平轴向右移动的网格编号，Rres表示所述结构水平图像的图像分辨率。
4.根据权利要求3所述的基于声纳图像处理的水下机器人水下定位和路径规划方法，其特征在于，根据所述原始图像和所述占用网格图检测所述水下机器人在所述水下结构中运行遇到的障碍物包括：
修改所述原始图像分辨率；
对修改后的所述原始图像进行坐标转换；
对坐标转换后的所述原始图像进行图像滤波；
根据所述占用网格图进行所述障碍物检测。
5.根据权利要求4所述的基于声纳图像处理的水下机器人水下定位和路径规划方法，其特征在于，其中，搜索所述占用网格图中网格相连的白色网格组，若所述白色网格组中的网格数大于阈值，则判定所述白色网格组为所述障碍物。
6.根据权利要求5所述的基于声纳图像处理的水下机器人水下定位和路径规划方法，其特征在于，根据所述占用网格图和所述障碍物规划所述水下机器人的运行路径包括：
搜索所述水下机器人当前位置的相邻网格中未搜索的网格；
选择所述未搜索的网格中唯一值最小的网格作为所述水下机器人的运行路径；
若所述水下机器人沿所述搜索的运行路径运动时检测到所述障碍物，则将所述障碍物所在网格的值设定为1，并重置所有网格值不为1的网格；
根据重置后的占用网格图搜索另一条运动路径以接近所述目标网格；
选取所述相邻网格中唯一值最小网格作为所述水下机器人的运行路径。
CN201911264682.6A 2019-12-11 2019-12-11 基于声纳图像处理的水下机器人水下定位和路径规划方法 Active CN111007518B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911264682.6A CN111007518B (zh) 2019-12-11 2019-12-11 基于声纳图像处理的水下机器人水下定位和路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911264682.6A CN111007518B (zh) 2019-12-11 2019-12-11 基于声纳图像处理的水下机器人水下定位和路径规划方法

Publications (2)

Publication Number Publication Date
CN111007518A CN111007518A (zh) 2020-04-14
CN111007518B true CN111007518B (zh) 2023-05-26

Family

ID=70114562

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911264682.6A Active CN111007518B (zh) 2019-12-11 2019-12-11 基于声纳图像处理的水下机器人水下定位和路径规划方法

Country Status (1)

Country Link
CN (1) CN111007518B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114489046B (zh) * 2021-12-27 2024-04-19 南方海洋科学与工程广东省实验室(湛江) 水下巡检机器人导航方法、装置、系统及存储介质

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120099400A1 (en) * 2010-10-25 2012-04-26 Lockheed Martin Corporation Estimating position and orientation of an underwater vehicle relative to underwater structures
CN103529844B (zh) * 2013-10-18 2017-03-15 吴宝举 基于前视声纳的水下机器人避碰方法
CN108227744B (zh) * 2018-01-17 2020-07-31 中国农业大学 一种水下机器人定位导航系统及定位导航方法
CN108871364A (zh) * 2018-06-28 2018-11-23 南京信息工程大学 一种基于结点算法的水下机器人路径规划方法
CN109470249B (zh) * 2018-11-07 2021-07-27 河海大学 一种水下航行器的最优路径规划与避障设计方法

Also Published As

Publication number Publication date
CN111007518A (zh) 2020-04-14

Similar Documents

Publication Publication Date Title
CN110850403B (zh) 一种多传感器决策级融合的智能船水面目标感知识别方法
US10006772B2 (en) Map production method, mobile robot, and map production system
CN108519615B (zh) 基于组合导航和特征点匹配的移动机器人自主导航方法
US6453223B1 (en) Infrastructure independent position determining system
Davison et al. Mobile robot localisation using active vision
US6442476B1 (en) Method of tracking and sensing position of objects
CN109282808B (zh) 用于桥梁三维巡航检测的无人机与多传感器融合定位方法
WO2020051923A1 (en) Systems And Methods For VSLAM Scale Estimation Using Optical Flow Sensor On A Robotic Device
Ribas et al. Underwater SLAM in a marina environment
Trucco et al. Feature tracking in video and sonar subsea sequences with applications
CN113454487A (zh) 信息处理装置以及移动机器人
Demim et al. Simultaneous localisation and mapping for autonomous underwater vehicle using a combined smooth variable structure filter and extended kalman filter
CN111007518B (zh) 基于声纳图像处理的水下机器人水下定位和路径规划方法
Hernández et al. Probabilistic sonar scan matching for an AUV
Nunes et al. Occupancy grid mapping from 2d sonar data for underwater scenes
CN111080788A (zh) 一种海底地形绘制方法及装置
Xia et al. Research on Parameter Adjustment Method of Cartographer Algorithm
Goll et al. An active beacon-based tracking system to be used for mobile robot convoying
CN113030901B (zh) 一种结合姿态仪与毫米波雷达的无人船前方多目标跟踪检测方法
CN116954225B (zh) 基于多波束声呐的城市管道环境中潜航器避障系统与方法
JP4879595B2 (ja) 移動ロボット
Hwang SIMULATION OF SIMULTANEOUS LOCALIZATION AND MAPPING BASED ON THE UNSCENTED KALMAN FILTER FOR SMALL UNMANNED UNDERWATER VEHICLES
JP2024022650A (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
AU756108B2 (en) Method of tracking and sensing position of objects
Beom et al. The improvement of sonar-based mobile robot localization method by multiple beacons

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