CN117308982A - 一种核电站水下清洁机器人的定位方法及装置 - Google Patents

一种核电站水下清洁机器人的定位方法及装置 Download PDF

Info

Publication number
CN117308982A
CN117308982A CN202311230669.5A CN202311230669A CN117308982A CN 117308982 A CN117308982 A CN 117308982A CN 202311230669 A CN202311230669 A CN 202311230669A CN 117308982 A CN117308982 A CN 117308982A
Authority
CN
China
Prior art keywords
cleaning robot
positioning information
underwater
similarity
underwater cleaning
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
CN202311230669.5A
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.)
Shanghai Cgn Engineering Technology Co ltd Beijing Branch
China General Nuclear Power Corp
CGN Power Co Ltd
Guangdong Nuclear Power Joint Venture Co Ltd
China Nuclear Power Operation Co Ltd
Original Assignee
Shanghai Cgn Engineering Technology Co ltd Beijing Branch
China General Nuclear Power Corp
CGN Power Co Ltd
Guangdong Nuclear Power Joint Venture Co Ltd
China Nuclear Power Operation Co Ltd
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 Shanghai Cgn Engineering Technology Co ltd Beijing Branch, China General Nuclear Power Corp, CGN Power Co Ltd, Guangdong Nuclear Power Joint Venture Co Ltd, China Nuclear Power Operation Co Ltd filed Critical Shanghai Cgn Engineering Technology Co ltd Beijing Branch
Priority to CN202311230669.5A priority Critical patent/CN117308982A/zh
Publication of CN117308982A publication Critical patent/CN117308982A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/05—Underwater scenes
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/20—Image preprocessing
    • G06V10/30—Noise filtering
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Image Processing (AREA)

Abstract

本发明公开了一种核电站水下清洁机器人的定位方法及装置，该方法包括：实时获取水下环境图像数据，并对实时获取的水下环境图像数据进行处理，以获取水下清洁机器人的平面运动轨迹；对水下环境图像数据进行降噪处理；根据降噪后的水下环境图像数据计算相邻帧图像的结合相似度，并判断结合相似度是否超出预设值；如果结合相似度超出预设值，则根据平面运动轨迹确定水下清洁机器人在运动过程中的第一视觉定位信息；将第一视觉定位信息与第一车道线定位信息进行融合，在水下环境地图中确定水下清洁机器人的定位位置。通过该方法及装置能够避免水下清洁机器人偏离目标清洁位置，实现在水下环境地图中对水下清洁机器人的准确定位。

Description

一种核电站水下清洁机器人的定位方法及装置
技术领域
本发明涉及机器人定位技术领域，具体涉及一种核电站水下清洁机器人的定位方法及装置。
背景技术
随着计算机视觉技术的发展，基于单目立体视觉的同步定位与地图创建逐渐成为计算机视觉领域的热点问题之一。然而，由于单目立体视觉的尺度不确定性的问题，基于单目立体视觉构建的地图只能是相对精度的地图，即使地图中的各个物体的相对位置关系准确，该地图与真实地理环境之间仍然存在一个可能的比例关系。而且单目立体视觉在平面运动位移及轨迹测量、位姿估计、惯性导航、桥梁及建筑结构安全测试等领域具有广泛应用。平面运动测量是多自由度运动测量的基础，其测量精度直接影响实际工程应用***或装置的整体性能。特别地，水下环境对于工程顺利进行阻碍很大，主要原因是人类在水下活动受限，水下清洁机器人的诞生主要是潜入水中代替人类完成一些操作。目前在核电站水下清洁机器人执行任务的过程中，对水下清洁机器人所在位置定位及运行轨迹测量精度较低，影响机器人按照预设轨迹行进，导致水下清洁任务无法顺利完成。
现有专利CN109887032A公开了一种基于单目视觉SLAM的车辆定位方法及***，该方法包括：首先从车辆的单目摄像头拍摄到的真实环境图像序列中确定出至少两个初始帧，并通过车辆的定位模块获取在至少两个初始帧的拍摄时刻之间车辆的实际移动距离；结合车辆的实际移动距离和至少两个初始帧进行单目视觉SLAM初始化；如果初始化成功，根据车辆的实际移动距离和真实环境图像序列构建SLAM地图；SLAM地图包括单目摄像头的位姿序列，以及真实环境图像序列中各个图像包含的特征点的三维空间位置；在重定位时，根据特征点在单目摄像头拍摄到的目标图像中的图像位置以及特征点在SLAM地图中的三维空间位置确定单目摄像头在SLAM地图中的重定位位姿；根据单目摄像头在SLAM地图中的重定位位姿确定车辆在SLAM地图中的重定位位姿。该专利侧重于SLAM地图构建及车辆的位姿定位，未解决水下清洁机器人所在位置定位及运行轨迹测量精度较低的问题。
现有专利CN106970620A公开了一种基于单目视觉的机器人控制方法，该方法包括以下两个模块：图像处理模块：主要是利用单目相机从不同角度采集到的两幅图像对周围环境进行三维重构；轨迹规划模块：根据三维重构的结果，对环境进行地图构建，并利用轨迹规划算法对所构建的地图进行路径搜索，并按照一定的评估标准选择最优路径，在实际应用过程中，根据环境信息的复杂程度、机器人所要完成任务的难易程度可以选择不同的轨迹规划算法，也可以结合其他的人工智能算法进行轨迹搜索，其目的就是为了获得最优路径。该方法侧重于对机器人进行轨迹规划，选择最优路径到达目的地，未解决水下清洁机器人所在位置定位及运行轨迹测量精度较低的问题。
综上所述，上述两个现有专利均未解决现有技术中水下清洁机器人所在位置定位不准确及运行轨迹测量精度较低的问题。
发明内容
基于上述技术问题，本发明提出一种核电站水下清洁机器人的定位方法及装置，解决现有技术中水下清洁机器人所在位置定位不准确及运行轨迹测量精度较低的问题。
为实现上述目的，本发明提出一种核电站水下清洁机器人的定位方法，该方法包括：
实时获取水下环境图像数据，并对实时获取的所述水下环境图像数据进行处理，以获取所述水下清洁机器人的平面运动轨迹；
对所述水下环境图像数据进行降噪处理；
根据降噪后的水下环境图像数据计算相邻帧图像的结合相似度，并判断所述结合相似度是否超出预设值；
如果所述结合相似度超出预设值，则根据所述平面运动轨迹确定所述水下清洁机器人在运动过程中的第一视觉定位信息；
将所述第一视觉定位信息与第一车道线定位信息进行融合，在水下环境地图中确定所述水下清洁机器人的定位位置。
进一步地，获取所述水下清洁机器人的平面运动轨迹，包括：
通过摄像头采集参照物的运动序列图像；
对所述参照物的运动序列图像进行解析，获取所述参照物在X方向与Y方向的运动位移，所述参照物在X方向的运动位移sX(t)与Y方向的运动位移sY(t)可表示为，
其中，ωv为振动角频率，与/>分别为sX(t)与sY(t)的峰值，/>与/>分别为sX(t)与sY(t)的初相；
所述参照物在X方向与Y方向的运动位移形成所述水下清洁机器人的平面运动轨迹。
进一步的，对所述水下环境图像数据进行降噪处理，包括：
通过四阶累积量矩阵对所述水下环境图像数据进行高斯去噪，所述四阶累积量矩阵如公式一，
其中，C4s为四阶累积量信号矩阵，C4v为四阶累积量噪声矩阵。
进一步的，根据降噪后的水下环境图像数据计算相邻帧图像的结合相似度，并判断所述结合相似度是否超出预设值，包括：
获取所述水下清洁机器人运动过程中当前帧及前一帧的图像信息；
根据所述当前帧的图像信息和所述前一帧的图像信息计算SITF特征描述符相似度及显著向量相似度；
根据所述SITF特征描述符相似度和所述显著向量相似度计算得到结合相似度。
进一步的，根据所述SITF特征描述符相似度和所述显著向量相似度计算得到结合相似度，包括：
根据所述SITF特征描述符相似度和所述显著向量相似度通过公式二计算得到所述结合相似度，公式二，
Dr＝exp(dm+ds)
其中，Dr为结合相似度，dm为SITF特征描述符相似度，ds为显著向量相似度，exp为指数e的幂。
进一步的，根据所述平面运动轨迹确定所述水下清洁机器人在运动过程中的第一视觉定位信息，包括：
根据所述平面运动轨迹通过公式三和公式四计算所述水下清洁机器人在运动过程中的第一视觉定位信息，公式三，
其中，sX(t)为t时刻所述参照物在X方向的运动位移，sX(t-1)为t-1时刻所述参照物在X方向的运动位移，sY(t)为t时刻所述参照物在Y方向的运动位移，sY(t-1)为t-1时刻所述参照物在Y方向的运动位移，PX(t)为所述水下清洁机器人在X方向的视觉定位信息与PY(t)为所述水下清洁机器人在Y方向的视觉定位信息；
公式四，其中，Pi-1为所述水下清洁机器人在运动过程中的第一视觉定位信息。
进一步地，将所述第一视觉定位信息与第一车道线定位信息进行融合后，还包括：
获取第二视觉定位信息与第二车道线定位信息，根据所述第二视觉定位信息在水下环境地图中确定所述水下清洁机器人的定位位置。
进一步地，获取第二视觉定位信息与第二车道线定位信息，包括：
根据公式五计算卡尔曼增益，公式五，K＝Pi-1HT(HPi-1HT+V)-1，其中，K为卡尔曼增益，Pi-1为第一视觉定位信息，H为函数h的雅克比矩阵，V为观测误差，T表示矩阵的转置；
根据公式六计算第二视觉定位信息，公式六，Pi＝(I-KH)Pi-1，Pi-1为第一视觉定位信息，Pi为第二视觉定位信息，H为函数h的雅克比矩阵，I为单位矩阵；
根据公式七计算所述第二车道线定位信息，公式七，x＝K(h(xf)-h(xp))，其中，K为卡尔曼增益，h(xf)表示预测的角度；h(xp)表示观测的角度。
进一步的，还包括：将所述视觉定位信息与所述车道线定位信息进行融合后，通过最小二乘法获取下一帧的运动轨迹参数。
为实现与上述方法相同的目的，本发明还提出一种核电站水下清洁机器人的定位装置，包括：
获取模块，用于实时获取水下环境图像数据，并对实时获取的所述水下环境图像数据进行处理，以获取所述水下清洁机器人的平面运动轨迹；
处理模块，用于对所述水下环境图像数据进行降噪处理；
计算模块，用于根据降噪后的水下环境图像数据计算相邻帧图像的结合相似度，并判断所述结合相似度是否超出预设值；
确定模块，用于如果所述结合相似度超出预设值，则根据所述平面运动轨迹确定所述水下清洁机器人在运动过程中的第一视觉定位信息；
定位模块，用于将所述第一视觉定位信息与第一车道线定位信息进行融合，在水下环境地图中确定所述水下清洁机器人的定位位置。
基于上述技术方案，本发明至少具有如下有益效果：
1、本发明通过平面运动轨迹确定水下清洁机器人在运动过程中的第一视觉定位信息，将第一视觉定位信息与第一车道线定位信息进行融合，以修正水下清洁机器人的第一视觉定位信息，并更新第一车道线定位信息，避免水下清洁机器人偏离目标清洁位置；根据修正后的第二视觉定位信息在水下环境地图中实现对水下清洁机器人的准确定位。
2、本发明通过四阶累积量矩阵对水下环境图像数据进行降噪处理，能够将信号数据和噪声数据分离，得到四阶累积量信号矩阵和四阶累积量噪声矩阵，从而根据高斯累积量中任何一个高斯有色噪声的高阶累积量恒等于零的性质，使得四阶累积量噪声矩阵为零，该方法能够提升降噪处理的精度，使得该水下环境图像数据更真实，提升对水下清洁机器人定位的精度。
3、本发明通过相邻帧图像的SITF特征描述符相似度和显著向量相似度计算得到结合相似度，通过结合相似度判断当前时刻水下清洁机器人的运动方向是否发生偏移，便于及时修正水下清洁机器人的运动方向。
附图说明
构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1为本发明一个实施例的一种核电站水下清洁机器人的定位方法流程图；
图2为本发明一个实施例中获取所述水下清洁机器人的平面运动轨迹的流程图；
图3为本发明一个实施例中计算相邻帧图像的结合相似度的流程图；
图4为本发明一个实施例中一种核电站水下清洁机器人的定位装置示意图。
具体实施方式
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
以下结合具体实施例对本发明作进一步详细描述，这些实施例不能理解为限制本发明所要求保护的范围。
实施例
为解决现有技术中的水下清洁机器人所在位置定位不准确及运行轨迹测量精度较低的问题，本发明提出一种核电站水下清洁机器人的定位方法及装置。
如图1中示出了本发明一个实施例的一种核电站水下清洁机器人的定位方法流程图，如图1所示，该方法包括：
S1，实时获取水下环境图像数据，并对实时获取的所述水下环境图像数据进行处理，以获取所述水下清洁机器人的平面运动轨迹。
在水下清洁机器人进入水下环境后，获取水下清洁机器人的初始位置坐标，而后以该初始位置坐标为起点，实时获取并处理水下环境图像数据，以获取所述水下清洁机器人的平面运动轨迹。
如图2所示，获取所述水下清洁机器人的平面运动轨迹，包括如下子步骤：
S101，通过摄像头采集参照物的运动序列图像。
所述参照物为具有高对比度特征标志的物体，所述高对比度特征标志指的是具有明显、显著的亮度、颜色或纹理变化的特征点、特征区域或特征对象。本发明将该参照物的运动序列图像作为水下清洁机器人的运动序列图像，从而用于计算水下清洁机器人的运动轨迹。
S102，对所述参照物的运动序列图像进行解析，获取所述参照物在X方向与Y方向的运动位移。
所述参照物在X方向的运动位移sX(t)与Y方向的运动位移sY(t)可表示为，
其中，ωv为振动角频率，与/>分别为sX(t)与sY(t)的峰值，/>与/>分别为sX(t)与sY(t)的初相。
具体来说，上述参照物在X方向的运动位移sX(t)与Y方向的运动位移sY(t)的计算过程如下：
首先通过正弦逼近法拟合求得tj时刻在X与Y方向的运动位移sX(tj)与sY(tj)，所述正弦逼近法拟合公式为：
其中，参数AX、BX、CX及AY、BY、CY可分别通过求解对应的N个方程获得，根据求解得到的参数AX、BX、CX及AY、BY、CY可计算得到所述平面运动在X与Y方向的位移峰值与/>与初相与/>为，
进一步地，本发明定义相位差PD为:
当满足PD＝0°或180°条件时，所述参照物的平面运动轨迹为直线轨迹；当且仅当满足且PD＝90°条件时,所述参照物的平面运动轨迹为圆轨迹；其余平面运动均为椭圆轨迹。
S103，所述参照物在X方向与Y方向的运动位移形成所述水下清洁机器人的平面运动轨迹。
基于上述步骤S102计算得到的所述参照物在X方向与Y方向的运动位移，可得到所述水下清洁机器人的平面运动轨迹。
S2，对所述水下环境图像数据进行降噪处理。
高阶累积量是信号处理领域中一种重要方法，其对加性高斯白噪声和高斯色噪声均具有较强的抑制能力。本发明采用四阶累积量矩阵对所述水下环境图像数据进行高斯去噪，所述四阶累积量矩阵如公式一，
其中，C4s为四阶累积量信号矩阵，C4v为四阶累积量噪声矩阵。
通过四阶累积量矩阵对所述水下环境图像数据进行降噪处理，能够将信号数据和噪声数据分离，得到四阶累积量信号矩阵和四阶累积量噪声矩阵。根据高斯累积量的性质：对于任何一个高斯有色噪声，高阶累积量恒等于零，在利用公式一对所述水下环境图像数据进行降噪处理时，可使得四阶累积量噪声矩阵C4v为零，从而提升降噪处理的精度，使得该水下环境图像数据更真实。
进一步地，所述公式一的推导过程如下：
对于零均值的n维平稳随机过程x(n)的四阶累积量为：
C4x(τ1,τ2,τ3)＝cum[x*(n),x*(n+τ1),x(n+τ2),x(n+τ3)]
＝E{x*(n)x*(n+τ1)x(n+τ2)x(n+τ3)}-Rx(τ1)Rx(τ3-τ2)-Rx(τ2)Rx(τ3-τ1)-Rx(τ3)Rx(τ2-τ1)
由样本数据x1,x2...xn估计的四阶累积量可表示如下:
其中，
Rx(τ)为二阶矩，即自相关函数。
通过上述公式整合可得到四阶累积量矩阵。
S3，根据降噪后的水下环境图像数据计算相邻帧图像的结合相似度，并判断所述结合相似度是否超出预设值。
具体来说，本发明中主要通过计算相邻帧图像的结合相似度，并判断所述结合相似度是否超出预设值，来判断所述水下清洁机器人在某时刻的运动方向是否发生较大偏移。
如图3中示出了本发明一个实施例中根据上述步骤S2降噪处理后的水下环境图像数据计算相邻帧图像的结合相似度的流程，具体包括如下子步骤：
S301，获取所述水下清洁机器人运动过程中当前帧及前一帧的图像信息。
S302，根据所述当前帧的图像信息和所述前一帧的图像信息计算SITF特征描述符相似度及显著向量相似度。
所述SITF特征描述符相似度指的是图像中不同特征点之间的相似度或匹配程度，SITF特征描述符相似度计算过程如下：
首先，检测当前帧的图像信息和前一帧的图像信息中的关键特征点，这些特征点具有不受缩放、旋转和亮度变化影响的特性。
其次，计算每个关键特征点周围区域的描述符。
最后，使用SITF描述符的相似度度量来衡量相邻帧关键特征点的描述符之间的相似性。常见的相似度度量包括欧氏距离、汉明距离或相关性度量等。
所述显著向量指的是图像中在视觉上比较突出的特征，所述显著向量相似度指的是图像中两个显著向量之间相似性。具体可通过获取当前帧和前一帧图像信息中的显著向量，然后通过欧氏距离或余弦相似度等度量方式计算两个显著向量之间的相似性。
S303，根据所述SITF特征描述符相似度和所述显著向量相似度计算得到结合相似度。
具体来说，在本发明的一个具体实施例中，根据上述步骤S302中计算得到的SITF特征描述符相似度和显著向量相似度，通过公式二计算得到所述结合相似度，公式二，
Dr＝exp(dm+ds)
其中，Dr为结合相似度，dm为SITF特征描述符相似度，ds为显著向量相似度，exp为指数e的幂。
根据上述步骤S301-S303计算得到的结合相似度，可用于判断所述水下清洁机器人在某时刻的运动方向是否发生较大偏移。
此外，由于所述水下清洁机器人的第一帧为初始帧，没有前一帧的图像信息，该情况下可通过计算第一帧对应的位置坐标与初始位置坐标的插值来判断水下清洁机器人在某时刻的运动方向是否发生较大偏移。
S4，如果所述结合相似度超出预设值，则根据所述平面运动轨迹确定所述水下清洁机器人在运动过程中的第一视觉定位信息。
当所述结合相似度超出预设值时，则表示当前时刻水下清洁机器人的运动方向发生较大偏移。所述第一视觉定位信息指的是水下清洁机器人在运动过程中的实际视觉定位信息，具体确定方式如下：
根据所述平面运动轨迹通过公式三和公式四计算所述水下清洁机器人在运动过程中的第一视觉定位信息，公式三，
其中，sX(t)为t时刻所述参照物在X方向的运动位移，sX(t-1)为t-1时刻所述参照物在X方向的运动位移，sY(t)为t时刻所述参照物在Y方向的运动位移，sY(t-1)为t-1时刻所述参照物在Y方向的运动位移，PX(t)为所述水下清洁机器人在X方向的视觉定位信息与PY(t)为所述水下清洁机器人在Y方向的视觉定位信息。
公式四，其中，Pi-1为所述水下清洁机器人在运动过程中的第一视觉定位信息。
S5，将第一视觉定位信息与第一车道线定位信息进行融合，在水下环境地图中确定所述水下清洁机器人的定位位置。
所述第一车道线定位信息指的是预设车道线定位信息，通过上述步骤S4可确定当前时刻的水下清洁机器人的运动方向发生了较大偏移，本发明通过步骤S5将第一视觉定位信息与第一车道线定位信息进行融合，修正水下清洁机器人的第一视觉定位信息，避免水下清洁机器人偏离目标清洁方向，无法完成清洁任务。
在该步骤中，将所述第一视觉定位信息与第一车道线定位信息进行融合后，还包括：获取第二视觉定位信息与第二车道线定位信息，根据所述第二视觉定位信息在水下环境地图中确定所述水下清洁机器人的定位位置。所述第二视觉定位信息与第二车道线定位信息是经过第一视觉定位信息与第一车道线定位信息融合获得的。
进一步地，获取第二视觉定位信息与第二车道线定位信息，包括：
根据公式五计算卡尔曼增益，公式五，K＝Pi-1HT(HPi-1HT+V)-1，其中，K为卡尔曼增益，Pi-1为第一视觉定位信息，H为函数h的雅克比矩阵，V为观测误差，T表示矩阵的转置。
基于第一视觉定位信息和上述计算得到的卡尔曼增益，根据公式六计算第二视觉定位信息，公式六，Pi＝(I-KH)Pi-1，Pi-1为第一视觉定位信息，Pi为第二视觉定位信息，H为函数h的雅克比矩阵，I为单位矩阵。
根据公式七计算第二车道线定位信息，公式七，x＝K(h(xf)-h(xp))，其中，K为卡尔曼增益，h(xf)表示预测的角度，h(xp)表示观测的角度。
通过上述计算方式可获得融合后的第二视觉定位信息和第二车道线定位信息，根据第二视觉定位信息在水下环境地图中确定水下清洁机器人的运动轨迹，完成对水下清洁机器人的位置定位。所述第二车道线定位信息可作为下一帧的第一车道线定位信息，用于下一帧水下清洁机器人的第一视觉定位信息与车道线定位信息的融合。
此外，如果通过所述结合相似度未超出预设值，则表示当前时刻水下清洁机器人的运动方向未发生较大偏移，行进正常，无需获取水下清洁机器人在运动过程中的视觉定位信息与车道线定位信息，可直接根据第一视觉定位信息在水下环境地图中确定所述水下清洁机器人的定位位置。
进一步的，在该步骤中还包括：将所述视觉定位信息与所述车道线定位信息进行融合后，通过最小二乘法获取下一帧的运动轨迹参数。
所述最小二乘拟原理如下：
已知圆的一般方程为：(x-a)2+(y+b)2＝r2，令待定系数A＝-2a，B＝-2b，C＝a2+b2-r2，则可将圆的方程转换为：x2+y2+Ax+Bx+C＝0，则边缘点集中的点到圆心的距离di满足
di与半径r的平方间的偏差为：
所有偏差的平方和Q为：
根据误差平方和最小化原则，想要获得最佳的拟合圆，则必须找到使得偏差平方和Q(A,B,C)取最小值时对应的待定参数A、B和C，即通过求Q(A,B,C)的极小值确定待定参数。因此，将Q(A,B,C)分别对A、B、C求偏导，并令其偏导值等于0，从而求得待定参数A，B，C，最佳圆心坐标(a,b)为(-A/2，-B/2)，半径r的值为结果如下：
基于上述最小二乘原理，以误差平方和最小化为目标分别拟合X与Y方向直线运动坐标，根据拟合后的直线运动坐标可计算得到水下清洁机器人在X与Y方向的运动位移及下一帧的运动轨迹参数，所述运动轨迹参数包括：水下清洁机器人的运动角度、运动速度及运动加速度。
通过执行步骤S5，将所述视觉定位信息与所述车道线定位信息进行融合，并通过最小二乘法获取下一帧的运动轨迹参数，可使得所述水下清洁机器人的运动轨迹逐渐接近所述车道线定位信息，避免水下清洁机器人偏离清洁位置，导致无法完成清洁任务；此外，将所述视觉定位信息与所述车道线定位信息进行融合后，可在水下环境地图中确定所述水下清洁机器人的定位位置。
具体来说，所述水下环境地图是通过障碍物三维模型、水下清洁机器人三维模型以及所述水下清洁机器人前后四个端角的三维坐标构建完成的，所述水下清洁机器人前后四个端角的三维坐标用于准确定位水下清洁机器人的位置，避免水下清洁机器人在行进过程中发生磕碰。
其中，所述障碍物三维模型是基于kinect深度传感器实现的。具体来说，首先通过kinect深度传感器获取水下环境中的障碍物深度图像；然后将所获得的障碍物深度图像进行三角化，在尺度空间中融合所有三角化的深度图像构建分层有向距离场；其次，应用整体三角剖分算法对距离场中所有的体素产生涵盖所有体素的凸包，使得障碍物三维模型更加立体；最后利用MarchingTetrahedra算法构造等值面，完成障碍物三维模型的构建。
为实现与上述方法相同的目的，本发明还提出一种核电站水下清洁机器人的定位装置。
如图4中示出了本发明一个实施例的一种核电站水下清洁机器人的定位装置示意图，如图4所示，该装置包括：获取模块41、处理模块42、计算模块43、确定模块44、定位模块45，以下将对各功能模块进行详细描述。
获取模块41，用于实时获取水下环境图像数据，并对实时获取的所述水下环境图像数据进行处理，以获取所述水下清洁机器人的平面运动轨迹。
处理模块42，用于对所述水下环境图像数据进行降噪处理。
计算模块43，用于根据降噪后的水下环境图像数据计算相邻帧图像的结合相似度，并判断所述结合相似度是否超出预设值。
确定模块44，用于如果所述结合相似度超出预设值，则根据所述平面运动轨迹确定所述水下清洁机器人在运动过程中的第一视觉定位信息。
定位模块45，用于将所述第一视觉定位信息与所述第一车道线定位信息进行融合，在水下环境地图中确定所述水下清洁机器人的定位位置。
应当理解的是，一种核电站水下清洁机器人的定位装置与其对应的一种核电站水下清洁机器人的定位方法实施例描述一致，故本实施例不再赘述。
总之，从以上的描述中可以看出，本发明上述的实施例实现如下技术效果：
1、本发明通过平面运动轨迹确定水下清洁机器人在运动过程中的第一视觉定位信息，将第一视觉定位信息与第一车道线定位信息进行融合，以修正水下清洁机器人的第一视觉定位信息，并更新第一车道线定位信息，避免水下清洁机器人偏离目标清洁位置；根据修正后的第二视觉定位信息在水下环境地图中实现对水下清洁机器人的准确定位。
2、本发明通过四阶累积量矩阵对水下环境图像数据进行降噪处理，能够将信号数据和噪声数据分离，得到四阶累积量信号矩阵和四阶累积量噪声矩阵，从而根据高斯累积量中任何一个高斯有色噪声的高阶累积量恒等于零的性质，使得四阶累积量噪声矩阵为零，该方法能够提升降噪处理的精度，使得该水下环境图像数据更真实，提升对水下清洁机器人定位的精度。
3、本发明通过相邻帧图像的SITF特征描述符相似度和显著向量相似度计算得到结合相似度，通过结合相似度判断当前时刻水下清洁机器人的运动方向是否发生偏移，便于及时修正水下清洁机器人的运动方向。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、物品或者设备中还存在另外的相同要素。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行***、装置或设备(如基于计算机的***、包括处理器的***或其他可以从指令执行***、装置或设备取指令并执行指令的***)使用，或结合这些指令执行***、装置或设备而使用。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行***执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
需要说明的是，在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

Claims (10)

1.一种核电站水下清洁机器人的定位方法，其特征在于，包括：
实时获取水下环境图像数据，并对实时获取的所述水下环境图像数据进行处理，以获取所述水下清洁机器人的平面运动轨迹；
对所述水下环境图像数据进行降噪处理；
根据降噪后的水下环境图像数据计算相邻帧图像的结合相似度，并判断所述结合相似度是否超出预设值；
如果所述结合相似度超出预设值，则根据所述平面运动轨迹确定所述水下清洁机器人在运动过程中的第一视觉定位信息；
将所述第一视觉定位信息与第一车道线定位信息进行融合，在水下环境地图中确定所述水下清洁机器人的定位位置。
2.根据权利要求1所述的方法，其特征在于，获取所述水下清洁机器人的平面运动轨迹，包括：
通过摄像头采集参照物的运动序列图像；
对所述参照物的运动序列图像进行解析，获取所述参照物在X方向与Y方向的运动位移，所述参照物在X方向的运动位移sX(t)与Y方向的运动位移sY(t)可表示为，
其中，ωv为振动角频率，与/>分别为sX(t)与sY(t)的峰值，/>与/>分别为sX(t)与sY(t)的初相；
所述参照物在X方向与Y方向的运动位移形成所述水下清洁机器人的平面运动轨迹。
3.根据权利要求1所述的方法，其特征在于，对所述水下环境图像数据进行降噪处理，包括：
通过四阶累积量矩阵对所述水下环境图像数据进行高斯去噪，所述四阶累积量矩阵如公式一，
其中，C4s为四阶累积量信号矩阵，C4v为四阶累积量噪声矩阵。
4.根据权利要求1所述的方法，其特征在于，根据降噪后的水下环境图像数据计算相邻帧图像的结合相似度，并判断所述结合相似度是否超出预设值，包括：
获取所述水下清洁机器人运动过程中当前帧及前一帧的图像信息；
根据所述当前帧的图像信息和所述前一帧的图像信息计算SITF特征描述符相似度及显著向量相似度；
根据所述SITF特征描述符相似度和所述显著向量相似度计算得到结合相似度。
5.根据权利要求4所述的方法，其特征在于，根据所述SITF特征描述符相似度和所述显著向量相似度计算得到结合相似度，包括：
根据所述SITF特征描述符相似度和所述显著向量相似度通过公式二计算得到所述结合相似度，公式二，
Dr＝exp(dm+ds)
其中，Dr为结合相似度，dm为SITF特征描述符相似度，ds为显著向量相似度，exp为指数e的幂。
6.根据权利要求2所述的方法，其特征在于，根据所述平面运动轨迹确定所述水下清洁机器人在运动过程中的第一视觉定位信息，包括：
根据所述平面运动轨迹通过公式三和公式四计算所述水下清洁机器人在运动过程中的第一视觉定位信息，公式三，
其中，sX(t)为t时刻所述参照物在X方向的运动位移，sX(t-1)为t-1时刻所述参照物在X方向的运动位移，sY(t)为t时刻所述参照物在Y方向的运动位移，sY(t-1)为t-1时刻所述参照物在Y方向的运动位移，PX(t)为所述水下清洁机器人在X方向的视觉定位信息与PY(t)为所述水下清洁机器人在Y方向的视觉定位信息；
公式四，其中，Pi-1为所述水下清洁机器人在运动过程中的第一视觉定位信息。
7.根据权利要求6所述的方法，其特征在于，将所述第一视觉定位信息与第一车道线定位信息进行融合后，还包括：
获取第二视觉定位信息与第二车道线定位信息，根据所述第二视觉定位信息在水下环境地图中确定所述水下清洁机器人的定位位置。
8.根据权利要求7所述的方法，其特征在于，获取第二视觉定位信息与第二车道线定位信息，包括：
根据公式五计算卡尔曼增益，公式五，K＝Pi-1HT(HPi-1HT+V)-1，其中，K为卡尔曼增益，Pi-1为第一视觉定位信息，H为函数h的雅克比矩阵，V为观测误差，T表示矩阵的转置；
根据公式六计算所述第二视觉定位信息，公式六，Pi＝(I-KH)Pi-1，Pi-1为第一视觉定位信息，Pi为第二视觉定位信息，H为函数h的雅克比矩阵，I为单位矩阵；
根据公式七计算所述第二车道线定位信息，公式七，x＝K(h(xf)-h(xp))，其中，K为卡尔曼增益，h(xf)表示预测的角度；h(xp)表示观测的角度。
9.根据权利要求1所述的方法，其特征在于，还包括：将所述视觉定位信息与所述车道线定位信息进行融合后，通过最小二乘法获取下一帧的运动轨迹参数。
10.一种核电站水下清洁机器人的定位装置，其特征在于，包括：
获取模块，用于实时获取水下环境图像数据，并对实时获取的所述水下环境图像数据进行处理，以获取所述水下清洁机器人的平面运动轨迹；
处理模块，用于对所述水下环境图像数据进行降噪处理；
计算模块，用于根据降噪后的水下环境图像数据计算相邻帧图像的结合相似度，并判断所述结合相似度是否超出预设值；
确定模块，用于如果所述结合相似度超出预设值，则根据所述平面运动轨迹确定所述水下清洁机器人在运动过程中的第一视觉定位信息；
定位模块，用于将所述第一视觉定位信息与第一车道线定位信息进行融合，在水下环境地图中确定所述水下清洁机器人的定位位置。
CN202311230669.5A 2023-09-21 2023-09-21 一种核电站水下清洁机器人的定位方法及装置 Pending CN117308982A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311230669.5A CN117308982A (zh) 2023-09-21 2023-09-21 一种核电站水下清洁机器人的定位方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311230669.5A CN117308982A (zh) 2023-09-21 2023-09-21 一种核电站水下清洁机器人的定位方法及装置

Publications (1)

Publication Number Publication Date
CN117308982A true CN117308982A (zh) 2023-12-29

Family

ID=89236498

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311230669.5A Pending CN117308982A (zh) 2023-09-21 2023-09-21 一种核电站水下清洁机器人的定位方法及装置

Country Status (1)

Country Link
CN (1) CN117308982A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117934317A (zh) * 2024-01-23 2024-04-26 北京世航智能科技有限公司 基于多传感器的水下机器人在线定位方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117934317A (zh) * 2024-01-23 2024-04-26 北京世航智能科技有限公司 基于多传感器的水下机器人在线定位方法

Similar Documents

Publication Publication Date Title
CN112985416B (zh) 激光与视觉信息融合的鲁棒定位和建图方法及***
CN110686677B (zh) 一种基于几何信息的全局定位方法
Davison et al. 3D simultaneous localisation and map-building using active vision for a robot moving on undulating terrain
Zuo et al. Visual-inertial localization with prior LiDAR map constraints
JP5012615B2 (ja) 情報処理装置、および画像処理方法、並びにコンピュータ・プログラム
CN105910604A (zh) 一种基于多传感器的自主避障导航***
CN112444246B (zh) 高精度的数字孪生场景中的激光融合定位方法
Zhuang et al. 4d iriom: 4d imaging radar inertial odometry and mapping
Zhou et al. A lidar odometry for outdoor mobile robots using ndt based scan matching in gps-denied environments
Konrad et al. Localization in digital maps for road course estimation using grid maps
Demim et al. Simultaneous localisation and mapping for autonomous underwater vehicle using a combined smooth variable structure filter and extended kalman filter
Kmiotek et al. Representing and tracking of dynamics objects using oriented bounding box and extended kalman filter
CN104166995A (zh) 一种基于马步测度的Harris-SIFT双目视觉定位方法
Anousaki et al. Simultaneous localization and map building of skid-steered robots
Yang et al. Simultaneous estimation of ego-motion and vehicle distance by using a monocular camera
Amarasinghe et al. Landmark detection and localization for mobile robot applications: A multisensor approach
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及***
CN116380079A (zh) 一种融合前视声呐与orb-slam3的水下slam方法
CN117308982A (zh) 一种核电站水下清洁机器人的定位方法及装置
CN105890589B (zh) 一种水下机器人单目视觉定位方法
Kolu et al. A mapping method tolerant to calibration and localization errors based on tilting 2D laser scanner
Spears et al. Determining underwater vehicle movement from sonar data in relatively featureless seafloor tracking missions
Benmansour et al. Stereovision-based 3D lane detection system: a model driven approach
Uno et al. Deep Inertial Underwater Odometry System.
Krishnaswamy et al. Sensor fusion for GNSS denied navigation

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