CN114715363A - 海底地层空间钻进机器人导航方法、系统及电子设备 - Google Patents

海底地层空间钻进机器人导航方法、系统及电子设备 Download PDF

Info

Publication number
CN114715363A
CN114715363A CN202210349815.5A CN202210349815A CN114715363A CN 114715363 A CN114715363 A CN 114715363A CN 202210349815 A CN202210349815 A CN 202210349815A CN 114715363 A CN114715363 A CN 114715363A
Authority
CN
China
Prior art keywords
drilling robot
robot
data information
drilling
space
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.)
Granted
Application number
CN202210349815.5A
Other languages
English (en)
Other versions
CN114715363B (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 CN202210349815.5A priority Critical patent/CN114715363B/zh
Publication of CN114715363A publication Critical patent/CN114715363A/zh
Application granted granted Critical
Publication of CN114715363B publication Critical patent/CN114715363B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63C—LAUNCHING, HAULING-OUT, OR DRY-DOCKING OF VESSELS; LIFE-SAVING IN WATER; EQUIPMENT FOR DWELLING OR WORKING UNDER WATER; MEANS FOR SALVAGING OR SEARCHING FOR UNDERWATER OBJECTS
    • B63C11/00—Equipment for dwelling or working underwater; Means for searching for underwater objects
    • B63C11/52—Tools specially adapted for working underwater, not otherwise provided for
    • E—FIXED CONSTRUCTIONS
    • E21—EARTH DRILLING; MINING
    • E21C—MINING OR QUARRYING
    • E21C50/00—Obtaining minerals from underwater, not otherwise provided for
    • 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • 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
    • 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

Abstract

本发明涉及一种海底地层空间钻进机器人导航方法、系统、电子设备及存储介质，涉及钻进机器人导航领域。该方法包括：获取海底地层空间钻进机器人运动时传感器阵列采集的数据信息以及作业任务；对数据信息进行降噪处理；根据降噪后的数据信息计算机器人各体节的姿态；根据机器人头部体节的姿态计算机器人在海底地层空间中的位置；根据机器人作业任务构建障碍物栅格地图；根据障碍物栅格地图，基于蚁群优化算法规划最优避障运动路径；根据机器人各体节的姿态、在海底地层空间中的位置以及最优避障运动路径对机器人进行导航。采用本发明方法能够实现对机器人的精准、快速导航，使机器人钻进到海底地层空间的特定位置以执行勘探或者开发作业任务。

Description

海底地层空间钻进机器人导航方法、系统及电子设备
技术领域
本发明涉及钻进机器人导航技术领域，特别是涉及一种海底地层空间钻进机器人导航方法、系统、电子设备及存储介质。
背景技术
近年来，人类对海洋资源的勘探、开发和利用不断深入。自2000年来，中国海上油气项目所产出的石油产量占世界石油产量的30％，所产出的天然气产量占世界天然气产量的27％。海洋油气资源的勘探始终是一项战略性任务。深海多金属结核主要由沉淀的铁和锰氧化物组成，镍、钴、铜、钛和稀土等金属元素吸附在其上，深海多金属结核包含了大量关键金属元素，它的勘探与开采工作也十分重要。
针对深海地层空间的勘探、开发与利用，许多研究人员都设计了可以在海底地层空间作业的各种新型深海地层钻进机器人。这些机器人采用仿生结构设计，可在地层中实现蠕动爬行与挖掘作业。在钻进运动时，它们都表现得十分优异。尽管这些机器人都具备了优异的钻进性能，但是机器人在实际作业时，往往需要导航系统引导，钻进到地层空间的特定位置以执行勘探或者开发任务。因此，研制出一种可用于海底地层空间钻进机器人的导航系统，将填补海底地层空间导航领域技术设备的空白，具有广泛的应用场景和重要的战略意义。
发明内容
本发明的目的是提供一种海底地层空间钻进机器人导航方法、系统、电子设备及存储介质，以解决钻进机器人在海底地层空间作业时的导航问题。
为实现上述目的，本发明提供了如下方案：
一种海底地层空间钻进机器人导航方法，包括：
获取海底地层空间钻进机器人运动时传感器阵列采集的数据信息以及所述钻进机器人在海底地层空间中的作业任务；所述传感器阵列包括安装在所述钻进机器人各体节上的多个惯性测量单元IMU以及安装在所述钻进机器人尾部拖缆上的长度传感器；所述传感器阵列采集的数据信息包括所述惯性测量单元采集的所述钻进机器人各体节的IMU数据信息以及所述长度传感器采集的所述钻进机器人尾部拖缆的长度信息；所述钻进机器人在海底地层空间中的作业任务包括所述钻进机器人在所述海底地层空间中的释放点和作业点；
对所述数据信息进行降噪处理，生成降噪后的数据信息；
根据所述降噪后的数据信息计算所述钻进机器人各体节的姿态；所述钻进机器人各体节的姿态包括所述钻进机器人头部体节的姿态；
根据所述钻进机器人头部体节的姿态计算所述钻进机器人在海底地层空间中的位置；
根据所述钻进机器人在海底地层空间中的作业任务构建障碍物栅格地图；
根据所述障碍物栅格地图，基于蚁群优化算法规划所述钻进机器人的最优避障运动路径；
根据所述钻进机器人各体节的姿态、所述钻进机器人在海底地层空间中的位置以及所述最优避障运动路径对所述钻进机器人进行导航，使所述钻进机器人沿所述最优避障运动路径从所述释放点运动至所述作业点。
可选地，所述对所述数据信息进行降噪处理，生成降噪后的数据信息，具体包括：
将所述数据信息输入到基于卡尔曼滤波算法的滤波器中，对所述数据信息进行降噪处理，生成降噪后的数据信息；所述降噪后的数据信息包括降噪后的IMU数据信息和降噪后的长度信息；所述IMU数据信息包括所述钻进机器人各体节的加速度和磁强度；所述长度信息包括所述钻进机器人尾部拖缆的长度。
可选地，所述根据所述降噪后的数据信息计算所述钻进机器人各体节的姿态，具体包括：
根据所述钻进机器人各体节的加速度和磁强度，采用公式
Figure BDA0003579281510000031
和
Figure BDA0003579281510000032
计算所述钻进机器人各体节的姿态；所述钻进机器人各体节的姿态通过各体节的偏航角ψ、俯仰角θ和滚转角φ来表示；其中ax、ay、az分别表示在x、y、z方向上的加速度；mx、my、mz分别表示在x、y、z方向上的磁强度。
可选地，所述根据所述钻进机器人头部体节的姿态计算所述钻进机器人在海底地层空间中的位置，具体包括：
根据所述钻进机器人头部体节的姿态计算由载体系到导航系的旋转矩阵；
根据所述旋转矩阵计算所述钻进机器人头部体节在导航系中的加速度；
根据所述钻进机器人头部体节在导航系中的加速度以及所述钻进机器人尾部拖缆的长度，计算所述钻进机器人在海底地层空间中的位置。
可选地，所述根据所述钻进机器人在海底地层空间中的作业任务构建障碍物栅格地图，具体包括：
当携带所述钻进机器人的基站坐底后，通过所述基站上搭载的地层剖面仪扫描并获取经过所述释放点和作业点且垂直于地层的地层剖面；
基于所述地层剖面构建所述障碍物方形栅格地图。
一种海底地层空间钻进机器人导航系统，包括：
数据采集模块，用于获取海底地层空间钻进机器人运动时传感器阵列采集的数据信息以及所述钻进机器人在海底地层空间中的作业任务；所述传感器阵列包括安装在所述钻进机器人各体节上的多个惯性测量单元以及安装在所述钻进机器人尾部拖缆上的长度传感器；所述传感器阵列采集的数据信息包括所述惯性测量单元采集的所述钻进机器人各体节的IMU数据信息以及所述长度传感器采集的所述钻进机器人尾部拖缆的长度信息；所述钻进机器人在海底地层空间中的作业任务包括所述钻进机器人在所述海底地层空间中的释放点和作业点；
降噪处理模块，用于对所述数据信息进行降噪处理，生成降噪后的数据信息；
机器人姿态计算模块，用于根据所述降噪后的数据信息计算所述钻进机器人各体节的姿态；所述钻进机器人各体节的姿态包括所述钻进机器人头部体节的姿态；
机器人位置计算模块，用于根据所述钻进机器人头部体节的姿态计算所述钻进机器人在海底地层空间中的位置；
地图构建模块，用于根据所述钻进机器人在海底地层空间中的作业任务构建障碍物栅格地图；
路径规划模块，用于根据所述障碍物栅格地图，基于蚁群优化算法规划所述钻进机器人的最优避障运动路径；
机器人导航模块，用于根据所述钻进机器人各体节的姿态、所述钻进机器人在海底地层空间中的位置以及所述最优避障运动路径对所述钻进机器人进行导航，使所述钻进机器人沿所述最优避障运动路径从所述释放点运动至所述作业点。
可选地，所述降噪处理模块具体包括：
降噪处理单元，用于将所述数据信息输入到基于卡尔曼滤波算法的滤波器中，对所述数据信息进行降噪处理，生成降噪后的数据信息；所述降噪后的数据信息包括降噪后的IMU数据信息和降噪后的长度信息；所述IMU数据信息包括所述钻进机器人各体节的加速度和磁强度；所述长度信息包括所述钻进机器人尾部拖缆的长度。
可选地，所述机器人姿态计算模块具体包括：
机器人姿态计算单元，用于根据所述钻进机器人各体节的加速度和磁强度，采用公式
Figure BDA0003579281510000041
和
Figure BDA0003579281510000042
计算所述钻进机器人各体节的姿态；所述钻进机器人各体节的姿态通过各体节的偏航角ψ、俯仰角θ和滚转角φ来表示；其中ax、ay、az分别表示在x、y、z方向上的加速度；mx、my、mz分别表示在x、y、z方向上的磁强度。
一种电子设备，包括一个或更多个处理器以及一个或更多个存储器，所述一个或更多个存储器用于存储一个或更多个程序，当所述一个或更多个程序被所述一个或更多个处理器执行时，使得所述一个或更多个处理器实现所述的海底地层空间钻进机器人导航方法。
一种计算机可读存储介质，其上存储有计算机指令，所述计算机指令被执行时使得所述计算机可读存储介质实现所述的海底地层空间钻进机器人导航方法。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明提供了一种海底地层空间钻进机器人导航方法、系统、电子设备及存储介质，所述方法包括：获取海底地层空间钻进机器人运动时传感器阵列采集的数据信息以及所述钻进机器人在海底地层空间中的作业任务；对所述数据信息进行降噪处理，生成降噪后的数据信息；根据所述降噪后的数据信息计算所述钻进机器人各体节的姿态；根据钻进机器人头部体节的姿态计算所述钻进机器人在海底地层空间中的位置；根据所述钻进机器人在海底地层空间中的作业任务构建障碍物栅格地图；根据所述障碍物栅格地图，基于蚁群优化算法规划所述钻进机器人的最优避障运动路径；根据所述钻进机器人各体节的姿态、所述钻进机器人在海底地层空间中的位置以及所述最优避障运动路径对所述钻进机器人进行导航。采用本发明方法能够实现对钻进机器人的精准、快速导航，使钻进机器人钻进到海底地层空间的特定位置以执行勘探或者开发作业任务。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的海底地层空间钻进机器人导航方法的流程图；
图2为本发明实施例提供的海底地层空间钻进机器人的结构示意图；
图3为本发明实施例提供的地层剖面获取的示意图；
图4为本发明实施例提供的海底地层空间钻进机器人导航系统的结构示意图；
图5为本发明实施例提供的电子设备和计算机可读存储介质的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种海底地层空间钻进机器人导航方法、系统、电子设备及存储介质，以解决钻进机器人在海底地层空间作业时的导航问题。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
图1为本发明一种海底地层空间钻进机器人导航方法的流程图。参见图1，本发明一种海底地层空间钻进机器人导航方法包括：
步骤101：获取海底地层空间钻进机器人运动时传感器阵列采集的数据信息以及所述钻进机器人在海底地层空间中的作业任务。
本发明所提出的导航方法和系统适用于一种海底地层空间钻进机器人。图2为本发明实施例提供的海底地层空间钻进机器人的结构示意图。参见图2，本发明所述的海底地层空间钻进机器人(简称机器人)采用一种多体节(包括体节1到体节n)、线性排布的结构，机器人各体节之间通过转向机构相连，可实现空间二自由度的转向。在机器人的每个体节上，均安装有惯性测量单元(Inertial Measurement Unit，IMU)，用来采集各体节的IMU数据信息，进而计算机器人的当前姿态。在机器人尾部拖缆处，安装有长度传感器，用来采集机器人尾部所拖线缆的长度信息。
因此，本发明所述传感器阵列包括安装在所述钻进机器人各体节上的多个惯性测量单元以及安装在所述钻进机器人尾部拖缆上的长度传感器。所述传感器阵列采集的数据信息包括所述惯性测量单元采集的所述钻进机器人各体节的IMU数据信息以及所述长度传感器采集的所述钻进机器人尾部拖缆的长度信息。所述IMU数据信息具体包括所述钻进机器人各体节的加速度、角速度、磁强度等信息。所述长度信息包括所述长度传感器连续采集的所述钻进机器人尾部拖缆的长度。
所述钻进机器人在海底地层空间中的作业任务包括所述钻进机器人在所述海底地层空间中的释放点和作业点。
步骤102：对所述数据信息进行降噪处理，生成降噪后的数据信息。
将所述数据信息输入到基于卡尔曼滤波算法的滤波器中，对所述数据信息进行降噪处理，生成降噪后的数据信息；所述降噪后的数据信息包括降噪后的IMU数据信息和降噪后的长度信息；所述IMU数据信息包括所述钻进机器人各体节的加速度和磁强度；所述长度信息包括所述钻进机器人尾部拖缆的长度。
具体地，将硬件传感器采集到的每个传感器数据值输入到基于卡尔曼滤波算法的滤波器中，通过滤波器降低所采集数据的噪声，将传感器采集到的测量值zk降噪处理为xk，具体步骤如2.1-2.5所示：
步骤2.1：计算卡尔曼增益，如下式：
Figure BDA0003579281510000071
步骤2.2：对传感器测量值进行降噪处理，如下式：
xk＝x′k+Kk(zk-Hkx′k) (2)
步骤2.3：计算协方差矩阵，如下式：
Pk＝(I-KkHk)P′k (3)
步骤2.4：预估下一步状态，如下式：
x′k+1＝Axk+Buk (4)
步骤2.5：预估下一步协方差矩阵，如下式：
P′k+1＝APkAT+Q (5)
其中，Kk表示第k步的卡尔曼增益，P′k表示第k步传感器数据协方差矩阵的预估值，Pk表示第k步传感器数据的协方差矩阵的计算值，Hk表示第k步的测量矩阵，R表示测量噪声的协方差矩阵，x′k表示第k步传感器数据的预估值，zk表示第k步传感器数据的测量值，xk表示第k步经过降噪后传感器数据的计算值，uk表示第k步的控制输入，I表示单位矩阵，A、B表示转移矩阵，Q表示状态转移的协方差矩阵，P′k+1表示第k+1步传感器数据协方差矩阵的预估值。
步骤103：根据所述降噪后的数据信息计算所述钻进机器人各体节的姿态。
根据经过步骤102降噪处理后的、安装在各体节上的IMU传感器数据，计算机器人各体节及整体姿态。所述钻进机器人各体节的姿态包括所述钻进机器人头部体节的姿态。
本发明采用欧拉角来表示机器人的姿态，定义ψ、θ、φ分别为绕z轴、y轴、x轴的旋转角度，分别称为偏航角(yaw角)，俯仰角(pitch)角和滚转角(roll角)，根据传感器所采集到各轴向的加速度、磁强度信息，可求得：
Figure BDA0003579281510000081
Figure BDA0003579281510000082
Figure BDA0003579281510000083
根据所述钻进机器人各体节的加速度和磁强度，采用公式(6)-(8)计算所述钻进机器人各体节的姿态；所述钻进机器人各体节的姿态通过各体节的偏航角ψ、俯仰角θ和滚转角φ来表示。其中ax、ay、az分别表示在x、y、z方向上的加速度；mx、my、mz分别表示在x、y、z方向上的磁强度。
采用本发明导航方法及系统的机器人是一种多体节、线性排布的结构，机器人各体节之间通过转向机构相连，可实现空间二自由度的转向，传感器载体坐标系的z轴与体节间转向机构的旋转轴平行。因此，机器人体节之间旋转机构的的转向角度Δψ可通过下式计算：
Δψ＝ψk+1-ψk (9)
其中，ψk+1表示第k+1个体节的yaw角，ψk表示第k个体节的yaw角。
步骤104：根据所述钻进机器人头部体节的姿态计算所述钻进机器人在海底地层空间中的位置。
根据经过步骤102处理后的、安装在头部体节的IMU传感器数据和长度传感器数据以及步骤103所计算出的机器人姿态，计算机器人在地层空间中的位置，具体步骤如4.1-4.3所示：
步骤4.1：根据所述钻进机器人头部体节的姿态计算由载体系到导航系的旋转矩阵。
按照步骤103公式(6)-(8)计算出机器人头部体节的旋转姿态角ψ、θ、φ后，根据空间旋转的欧拉角计算由载体系到导航系的旋转矩阵R0，具体如下式：
Figure BDA0003579281510000091
步骤4.2：根据所述旋转矩阵计算所述钻进机器人头部体节在导航系中的加速度。
根据所述旋转矩阵R0将载体系中的加速度值转换至导航系中的加速度值并消去重力加速度g，得到钻进机器人头部体节在导航系中的加速度a'，具体如下式：
Figure BDA0003579281510000092
其中，
Figure BDA0003579281510000101
表示载体系中的加速度值，
Figure BDA0003579281510000102
表示消除重力加速度后导航系中的加速度值。
步骤4.3：根据所述钻进机器人头部体节在导航系中的加速度以及所述钻进机器人尾部拖缆的长度，计算所述钻进机器人在海底地层空间中的位置。
从长度传感器上读取数据l，计算：
Δl＝l-l′ (12)
其中，l和l′为所述长度传感器连续采集的所述钻进机器人尾部拖缆的长度，l为传感器本次读取的数据，l′为传感器上次读取的数据，Δl为连续两次采集的长度数据的差值。
根据所述钻进机器人头部体节在导航系中的加速度a'以及所述钻进机器人尾部拖缆的长度变化量Δl，计算机器人在地层中的空间位置，具体如下式：
Figure BDA0003579281510000103
其中posaxis表示钻进机器人在axis轴上的位置，axis∈{x，y，z}；pos′axis表示上一时刻钻进机器人在axis轴上的位置。
步骤105：根据所述钻进机器人在海底地层空间中的作业任务构建障碍物栅格地图。
所述步骤105主要进行环境建模，构建出的障碍物栅格地图，所述步骤105具体包括：
步骤5.1：当携带所述钻进机器人的基站坐底后，通过所述基站上搭载的地层剖面仪扫描并获取经过所述释放点和作业点且垂直于地层的地层剖面，如图3所示。
步骤5.2：基于所述地层剖面构建所述障碍物方形栅格地图。
完成对特定地层剖面的获取后，再基于所获取的地层剖面，构建障碍物方形栅格地图，并对地图中存在障碍物的方形区域进行特殊的标记。假设地层剖面尺寸为L×C，用r×r的正方形区域表示机器人尺寸(r大于机器人实际长度)，构建L×C的栅格地图，地图全部由正方形区域构成。ng表示栅格编号，(xg，yg)表示第g个栅格的栅格坐标，具体表达式如下：
xg＝ng mod C (14)
yg＝[ng/C] (15)
ng＝yg×C+xg (16)
其中mod C表示对尺寸C取余。
步骤106：根据所述障碍物栅格地图，基于蚁群优化算法规划所述钻进机器人的最优避障运动路径。
根据机器人作业任务的释放点、作业点以及障碍物栅格地图，为钻进机器人规划出一条最优避障运动路径。将机器人运动的释放点、作业点以及地层障碍物地图输入到路径规划模型中，模型为机器人规划出在地层空间的运动路径；所述的路径规划模型基于蚁群优化算法，为钻进机器人在地层空间中规划出一条最优的避障运动路径，具体步骤6.1-4如下：
步骤6.1：初始化蚁群参数，确定蚁群数量以及信息素分布；
步骤6.2：计算概率
Figure BDA0003579281510000111
选择概率值中的最大值为下一步所到达的栅格j，如下式所示；
Figure BDA0003579281510000112
其中，α代表信息素因子，β代表启发函数因子，allowedk代表蚂蚁k可选的栅格点，ηij＝1/dij代表启发信息，dij代表栅格i到栅格j的欧式距离。
步骤6.3：在整个蚁群都完成一轮迭代后，进行信息素τij更新，如下式所示：
Figure BDA0003579281510000121
其中，m代表蚁群中蚂蚁数量，ρ代表信息素挥发因子，
Figure BDA0003579281510000122
代表本次迭代中蚂蚁k从栅格i走向栅格j路径上带来的信息素浓度增量，如下式所示：
Figure BDA0003579281510000123
其中，Q为常值，代表蚂蚁所留轨数的一个常数。
步骤6.4：判断是否满足结束条件，如果不满足，则继续迭代，否则，结束迭代，输出最优避障运动路径。
步骤107：根据所述钻进机器人各体节的姿态、所述钻进机器人在海底地层空间中的位置以及所述最优避障运动路径对所述钻进机器人进行导航，使所述钻进机器人沿所述最优避障运动路径从所述释放点运动至所述作业点。
根据步骤104所获得的地层空间位置信息与步骤106所获得的最优避障路径，实时调整步骤103获得的钻进机器人各体节的姿态角ψ、θ、φ以及机器人体节之间旋转机构的的转向角度Δψ，使所述钻进机器人沿所述最优避障运动路径从所述释放点运动至所述作业点，以执行勘探或者开发任务。
本发明方法通过采集与处理机器人运动时传感器阵列的数据信息，实时计算并输出钻进机器人的姿态信息和在地层空间中的位置信息，再根据机器人作业任务以及障碍物栅格地图，为钻进机器人在地层中规划出一条最优避障运动路径；基于机器人的实时位置信息与最优避障运动路径，导航系统控制器控制机器人运动，进而实现对钻进机器人的引导，使钻进机器人钻进到地层空间的特定位置以执行勘探或者开发作业任务。本发明提出的一种可用于海底地层空间钻进机器人的导航方法，填补了海底地层空间导航领域技术设备的空白，具有广泛的应用场景和重要的战略意义。
基于本发明提供的导航方法，本发明还提供一种海底地层空间钻进机器人导航系统，参见图4，所述系统包括：
数据采集模块401，用于获取海底地层空间钻进机器人运动时传感器阵列采集的数据信息以及所述钻进机器人在海底地层空间中的作业任务；所述传感器阵列包括安装在所述钻进机器人各体节上的多个惯性测量单元以及安装在所述钻进机器人尾部拖缆上的长度传感器；所述传感器阵列采集的数据信息包括所述惯性测量单元采集的所述钻进机器人各体节的IMU数据信息以及所述长度传感器采集的所述钻进机器人尾部拖缆的长度信息；所述钻进机器人在海底地层空间中的作业任务包括所述钻进机器人在所述海底地层空间中的释放点和作业点；
降噪处理模块402，用于对所述数据信息进行降噪处理，生成降噪后的数据信息；
机器人姿态计算模块403，用于根据所述降噪后的数据信息计算所述钻进机器人各体节的姿态；所述钻进机器人各体节的姿态包括所述钻进机器人头部体节的姿态；
机器人位置计算模块404，用于根据所述钻进机器人头部体节的姿态计算所述钻进机器人在海底地层空间中的位置；
地图构建模块405，用于根据所述钻进机器人在海底地层空间中的作业任务构建障碍物栅格地图；
路径规划模块406，用于根据所述障碍物栅格地图，基于蚁群优化算法规划所述钻进机器人的最优避障运动路径；
机器人导航模块407，用于根据所述钻进机器人各体节的姿态、所述钻进机器人在海底地层空间中的位置以及所述最优避障运动路径对所述钻进机器人进行导航，使所述钻进机器人沿所述最优避障运动路径从所述释放点运动至所述作业点。
其中，所述降噪处理模块402具体包括：
降噪处理单元，用于将所述数据信息输入到基于卡尔曼滤波算法的滤波器中，对所述数据信息进行降噪处理，生成降噪后的数据信息；所述降噪后的数据信息包括降噪后的IMU数据信息和降噪后的长度信息；所述IMU数据信息包括所述钻进机器人各体节的加速度和磁强度；所述长度信息包括所述钻进机器人尾部拖缆的长度。
所述机器人姿态计算模块403具体包括：
机器人姿态计算单元，用于根据所述钻进机器人各体节的加速度和磁强度，采用公式
Figure BDA0003579281510000141
和
Figure BDA0003579281510000142
计算所述钻进机器人各体节的姿态；所述钻进机器人各体节的姿态通过各体节的偏航角ψ、俯仰角θ和滚转角φ来表示；其中ax、ay、az分别表示在x、y、z方向上的加速度；mx、my、mz分别表示在x、y、z方向上的磁强度。
在一些实施例中，本发明实施例提供的导航系统具有的功能或包含的模块可以用于执行上文方法实施例所描述的方法，其具体实现可以参照上文方法实施例的描述，为了简洁，这里不再赘述。
根据本发明的实施例，本发明还提供了一种海底地层空间钻进机器人电子设备和可读存储介质。
如图5所示是根据本发明实施例的用以实现海底地层空间钻进机器人导航方法的电子设备和存储介质的示意图。该电子设备包括：一个或更多个处理器501、存储器502、输入装置503、输出装置504，以及用于连接各部件的接口，包括高速接口和低速接口。各个部件利用不同的总线相互连接，并且可以被安装在公共主板上或者根据需要以其它方式安装。
处理器501可以对电子设备内执行的指令进行处理，包括存储在存储器中或者存储器上以在外部输入/输出装置(诸如，耦合至接口的显示设备)上显示GUI的图形信息的指令。在其它实施方式中，若需要，可以连接多个电子设备，各个设备提供部分必要的操作(例如，作为服务器陈列、一组刀片式服务器、或者处理器系统)。
处理器501可以是各种具有处理和计算能力的通用或专用处理组件。处理器单元501的一些示例包括但不限于中央处理单元(CPU)、图形处理单元(GPU)、各种专用的人工智计算芯片、各种运行机器学习模型算法的计算单元、数字信号处理器(DSP)、以及任何适当的处理器、控制器、微控制器等。处理器501执行上文所描的海底地层空间钻进机器人导航方法。
存储器502即为本发明所提供的非瞬时计算机可读存储介质。其中，所述存储器502存储有可由至少一个处理器执行的指令，以使所述至少一个处理器执行本发明所提供的海底地层空间钻进机器人导航方法。本发明的非瞬时计算机可读存储介质存储计算机指令，该计算机指令用于使计算机执行本发明所提供的海底地层空间钻进机器人导航方法。
存储器502可以包括存储程序区和存储数据区，其中，存储程序区用于存储操作系统以及至少一个功能所需要的应用程序；存储数据区可存储根据海底地层空间钻进机器人导航的电子设备使用时创建的数据。并且，存储器502还可以包括高速随机存取存储器以及非瞬时存储器，例如至少一个磁盘存储器件、闪存器件、或其他非瞬时固态存储器件。
在一些实施例中，存储器502可选包括相对于处理器501远程设置的存储器，这些远程存储器可以通过网络连接至海底地层空间钻进机器人导航的电子设备。上述网络的实施例可选择互联网、企业内部网、局域网、移动通信网及其组合但不局限于此。
输入装置503可接收输入的数字或字符指令，以及产生与海底地层空间钻进机器人导航的电子设备的用户设置以及功能控制有关的键信号输入，例如触摸屏、小键盘、鼠标、触摸板、指示杆、一个或者多个鼠标按钮、轨迹球、操纵杆等输入装置。
输出装置504可以包括显示设备(例如，轨迹板)和反馈装置(例如，振动电机)等。还可以增加一些辅助设备可以包括但不限于例如液晶显示器(LCD)、发光二极管(LED)显示器和等离子体显示器等。
在一些实施方式中，显示设备可以是触摸屏。这种实施例中，输入装置与输出装置联系较为紧密。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种海底地层空间钻进机器人导航方法，其特征在于，包括：
获取海底地层空间钻进机器人运动时传感器阵列采集的数据信息以及所述钻进机器人在海底地层空间中的作业任务；所述传感器阵列包括安装在所述钻进机器人各体节上的多个惯性测量单元IMU以及安装在所述钻进机器人尾部拖缆上的长度传感器；所述传感器阵列采集的数据信息包括所述惯性测量单元采集的所述钻进机器人各体节的IMU数据信息以及所述长度传感器采集的所述钻进机器人尾部拖缆的长度信息；所述钻进机器人在海底地层空间中的作业任务包括所述钻进机器人在所述海底地层空间中的释放点和作业点；
对所述数据信息进行降噪处理，生成降噪后的数据信息；
根据所述降噪后的数据信息计算所述钻进机器人各体节的姿态；所述钻进机器人各体节的姿态包括所述钻进机器人头部体节的姿态；
根据所述钻进机器人头部体节的姿态计算所述钻进机器人在海底地层空间中的位置；
根据所述钻进机器人在海底地层空间中的作业任务构建障碍物栅格地图；
根据所述障碍物栅格地图，基于蚁群优化算法规划所述钻进机器人的最优避障运动路径；
根据所述钻进机器人各体节的姿态、所述钻进机器人在海底地层空间中的位置以及所述最优避障运动路径对所述钻进机器人进行导航，使所述钻进机器人沿所述最优避障运动路径从所述释放点运动至所述作业点。
2.根据权利要求1所述的方法，其特征在于，所述对所述数据信息进行降噪处理，生成降噪后的数据信息，具体包括：
将所述数据信息输入到基于卡尔曼滤波算法的滤波器中，对所述数据信息进行降噪处理，生成降噪后的数据信息；所述降噪后的数据信息包括降噪后的IMU数据信息和降噪后的长度信息；所述IMU数据信息包括所述钻进机器人各体节的加速度和磁强度；所述长度信息包括所述钻进机器人尾部拖缆的长度。
3.根据权利要求2所述的方法，其特征在于，所述根据所述降噪后的数据信息计算所述钻进机器人各体节的姿态，具体包括：
根据所述钻进机器人各体节的加速度和磁强度，采用公式
Figure FDA0003579281500000021
和
Figure FDA0003579281500000022
计算所述钻进机器人各体节的姿态；所述钻进机器人各体节的姿态通过各体节的偏航角ψ、俯仰角θ和滚转角φ来表示；其中ax、ay、az分别表示在x、y、z方向上的加速度；mx、my、mz分别表示在x、y、z方向上的磁强度。
4.根据权利要求3所述的方法，其特征在于，所述根据所述钻进机器人头部体节的姿态计算所述钻进机器人在海底地层空间中的位置，具体包括：
根据所述钻进机器人头部体节的姿态计算由载体系到导航系的旋转矩阵；
根据所述旋转矩阵计算所述钻进机器人头部体节在导航系中的加速度；
根据所述钻进机器人头部体节在导航系中的加速度以及所述钻进机器人尾部拖缆的长度，计算所述钻进机器人在海底地层空间中的位置。
5.根据权利要求4所述的方法，其特征在于，所述根据所述钻进机器人在海底地层空间中的作业任务构建障碍物栅格地图，具体包括：
当携带所述钻进机器人的基站坐底后，通过所述基站上搭载的地层剖面仪扫描并获取经过所述释放点和作业点且垂直于地层的地层剖面；
基于所述地层剖面构建所述障碍物方形栅格地图。
6.一种海底地层空间钻进机器人导航系统，其特征在于，包括：
数据采集模块，用于获取海底地层空间钻进机器人运动时传感器阵列采集的数据信息以及所述钻进机器人在海底地层空间中的作业任务；所述传感器阵列包括安装在所述钻进机器人各体节上的多个惯性测量单元IMU以及安装在所述钻进机器人尾部拖缆上的长度传感器；所述传感器阵列采集的数据信息包括所述惯性测量单元采集的所述钻进机器人各体节的IMU数据信息以及所述长度传感器采集的所述钻进机器人尾部拖缆的长度信息；所述钻进机器人在海底地层空间中的作业任务包括所述钻进机器人在所述海底地层空间中的释放点和作业点；
降噪处理模块，用于对所述数据信息进行降噪处理，生成降噪后的数据信息；
机器人姿态计算模块，用于根据所述降噪后的数据信息计算所述钻进机器人各体节的姿态；所述钻进机器人各体节的姿态包括所述钻进机器人头部体节的姿态；
机器人位置计算模块，用于根据所述钻进机器人头部体节的姿态计算所述钻进机器人在海底地层空间中的位置；
地图构建模块，用于根据所述钻进机器人在海底地层空间中的作业任务构建障碍物栅格地图；
路径规划模块，用于根据所述障碍物栅格地图，基于蚁群优化算法规划所述钻进机器人的最优避障运动路径；
机器人导航模块，用于根据所述钻进机器人各体节的姿态、所述钻进机器人在海底地层空间中的位置以及所述最优避障运动路径对所述钻进机器人进行导航，使所述钻进机器人沿所述最优避障运动路径从所述释放点运动至所述作业点。
7.根据权利要求6所述的系统，其特征在于，所述降噪处理模块具体包括：
降噪处理单元，用于将所述数据信息输入到基于卡尔曼滤波算法的滤波器中，对所述数据信息进行降噪处理，生成降噪后的数据信息；所述降噪后的数据信息包括降噪后的IMU数据信息和降噪后的长度信息；所述IMU数据信息包括所述钻进机器人各体节的加速度和磁强度；所述长度信息包括所述钻进机器人尾部拖缆的长度。
8.根据权利要求7所述的系统，其特征在于，所述机器人姿态计算模块具体包括：
机器人姿态计算单元，用于根据所述钻进机器人各体节的加速度和磁强度，采用公式
Figure FDA0003579281500000031
和
Figure FDA0003579281500000032
计算所述钻进机器人各体节的姿态；所述钻进机器人各体节的姿态通过各体节的偏航角ψ、俯仰角θ和滚转角φ来表示；其中ax、ay、az分别表示在x、y、z方向上的加速度；mx、my、mz分别表示在x、y、z方向上的磁强度。
9.一种电子设备，包括一个或更多个处理器以及一个或更多个存储器，所述一个或更多个存储器用于存储一个或更多个程序，其特征在于，当所述一个或更多个程序被所述一个或更多个处理器执行时，使得所述一个或更多个处理器实现如权利要求1所述的海底地层空间钻进机器人导航方法。
10.一种计算机可读存储介质，其上存储有计算机指令，其特征在于，所述计算机指令被执行时使得所述计算机可读存储介质实现如权利要求1所述的海底地层空间钻进机器人导航方法。
CN202210349815.5A 2022-04-02 2022-04-02 海底地层空间钻进机器人导航方法、系统及电子设备 Active CN114715363B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210349815.5A CN114715363B (zh) 2022-04-02 2022-04-02 海底地层空间钻进机器人导航方法、系统及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210349815.5A CN114715363B (zh) 2022-04-02 2022-04-02 海底地层空间钻进机器人导航方法、系统及电子设备

Publications (2)

Publication Number Publication Date
CN114715363A true CN114715363A (zh) 2022-07-08
CN114715363B CN114715363B (zh) 2023-03-24

Family

ID=82242512

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210349815.5A Active CN114715363B (zh) 2022-04-02 2022-04-02 海底地层空间钻进机器人导航方法、系统及电子设备

Country Status (1)

Country Link
CN (1) CN114715363B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115049646A (zh) * 2022-08-12 2022-09-13 山东九商工程机械有限公司 一种用于煤矿用钻进机器人设备的智能控制方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104533287A (zh) * 2014-11-20 2015-04-22 西南石油大学 一种钻鱼刺状多级分支水平井页岩气储层钻完井和增产系统
CN109917786A (zh) * 2019-02-04 2019-06-21 浙江大学 一种面向复杂环境作业的机器人感知系统及系统运行方法
CN110847823A (zh) * 2019-11-26 2020-02-28 浙江大学 一种海底深部地层自主钻进机器人
CN213338437U (zh) * 2020-09-03 2021-06-01 南京信息工程大学 一种无人挖掘机及其远程管理监控系统
US20210174486A1 (en) * 2017-11-06 2021-06-10 Tushar CHOWHAN A machine vision and machine intelligence aided electronic and computer system for assisting on formation drilling, boring, tunneling on-line guidance, assisted decision and dull grading system for drilling tool and associated drill string components
CN113772054A (zh) * 2021-09-07 2021-12-10 山东建筑大学 液态环境下溶洞勘探机器人及其控制系统、控制方法
CN113778099A (zh) * 2021-09-16 2021-12-10 浙江大学湖州研究院 基于NDT算法和Hybrid A*算法的无人船路径规划方法
CN113843806A (zh) * 2021-08-20 2021-12-28 浙江大学 一种海底沉积物地层空间钻探机器人
CN114035568A (zh) * 2021-03-27 2022-02-11 浙江大学 可燃冰试采区地层钻探机器人路径规划方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104533287A (zh) * 2014-11-20 2015-04-22 西南石油大学 一种钻鱼刺状多级分支水平井页岩气储层钻完井和增产系统
US20210174486A1 (en) * 2017-11-06 2021-06-10 Tushar CHOWHAN A machine vision and machine intelligence aided electronic and computer system for assisting on formation drilling, boring, tunneling on-line guidance, assisted decision and dull grading system for drilling tool and associated drill string components
CN109917786A (zh) * 2019-02-04 2019-06-21 浙江大学 一种面向复杂环境作业的机器人感知系统及系统运行方法
CN110847823A (zh) * 2019-11-26 2020-02-28 浙江大学 一种海底深部地层自主钻进机器人
CN213338437U (zh) * 2020-09-03 2021-06-01 南京信息工程大学 一种无人挖掘机及其远程管理监控系统
CN114035568A (zh) * 2021-03-27 2022-02-11 浙江大学 可燃冰试采区地层钻探机器人路径规划方法
CN113843806A (zh) * 2021-08-20 2021-12-28 浙江大学 一种海底沉积物地层空间钻探机器人
CN113772054A (zh) * 2021-09-07 2021-12-10 山东建筑大学 液态环境下溶洞勘探机器人及其控制系统、控制方法
CN113778099A (zh) * 2021-09-16 2021-12-10 浙江大学湖州研究院 基于NDT算法和Hybrid A*算法的无人船路径规划方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115049646A (zh) * 2022-08-12 2022-09-13 山东九商工程机械有限公司 一种用于煤矿用钻进机器人设备的智能控制方法
CN115049646B (zh) * 2022-08-12 2022-11-01 山东九商工程机械有限公司 一种用于煤矿用钻进机器人设备的智能控制方法

Also Published As

Publication number Publication date
CN114715363B (zh) 2023-03-24

Similar Documents

Publication Publication Date Title
CN109540151B (zh) 一种基于强化学习的auv三维路径规划方法
US8612084B2 (en) System and method for autonomous navigation of a tracked or skid-steer vehicle
CN110262478B (zh) 基于改进人工势场法的人机安全避障路径规划方法
Hou et al. Can a simple control scheme work for a formation control of multiple autonomous underwater vehicles?
JPWO2018143003A1 (ja) ロボットパス生成装置及びロボットシステム
CN110799727A (zh) 用于生成下向井眼惯性测量单元的输出的系统和方法
CN108387205B (zh) 基于多传感器数据融合的钻具姿态测量系统的测量方法
CN114715363B (zh) 海底地层空间钻进机器人导航方法、系统及电子设备
US11435497B2 (en) Three dimensional visualization from point-by-point one dimensional inversion with bed azimuth
CN110561419B (zh) 臂型线约束柔性机器人轨迹规划方法及装置
CN110561420B (zh) 臂型面约束柔性机器人轨迹规划方法及装置
CN110345936B (zh) 运动装置的轨迹数据处理方法及其处理系统
CN111412918B (zh) 无人艇全局安全路径规划方法
US10035264B1 (en) Real time robot implementation of state machine
CN114460939A (zh) 复杂环境下智能行走机器人自主导航改进方法
CN114089763A (zh) 用于海底光缆铺设的多水下机器人编队与避碰控制方法
CN114035568A (zh) 可燃冰试采区地层钻探机器人路径规划方法
CN115828463B (zh) 基于数字孪生六维模型的掘进机位姿估计方法、系统
US20210208302A1 (en) Visualization for look-ahead inversion
CN115237117B (zh) 基于变异自适应粒子群算法的移动机器人路径规划方法
Dai et al. Unsupervised feature fusion combined with neural network applied to UAV attitude estimation
Fattah et al. Dynamic map generating rescuer offering surveillance robotic system with autonomous path feedback capability
CN117055586B (zh) 基于自适应控制的水下机器人巡游搜索与抓取方法、系统
CN117590756B (zh) 水下机器人的运动控制方法、装置、设备和存储介质
Aggarwal et al. Autonomous trajectory planning and following for industrial underwater manipulators

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