CN116954225A - 基于多波束声呐的城市管道环境中潜航器避障系统与方法 - Google Patents
基于多波束声呐的城市管道环境中潜航器避障系统与方法 Download PDFInfo
- Publication number
- CN116954225A CN116954225A CN202310939783.9A CN202310939783A CN116954225A CN 116954225 A CN116954225 A CN 116954225A CN 202310939783 A CN202310939783 A CN 202310939783A CN 116954225 A CN116954225 A CN 116954225A
- Authority
- CN
- China
- Prior art keywords
- submarine
- pipeline
- sonar
- information
- camera
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 30
- 238000011217 control strategy Methods 0.000 claims abstract description 6
- 238000005259 measurement Methods 0.000 claims description 33
- 238000003384 imaging method Methods 0.000 claims description 24
- 230000033001 locomotion Effects 0.000 claims description 19
- 238000012545 processing Methods 0.000 claims description 13
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000004891 communication Methods 0.000 claims description 8
- 230000001133 acceleration Effects 0.000 claims description 6
- 238000007781 pre-processing Methods 0.000 claims description 5
- 230000000694 effects Effects 0.000 claims description 4
- 230000007613 environmental effect Effects 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 230000005540 biological transmission Effects 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 230000016776 visual perception Effects 0.000 claims description 3
- 238000001514 detection method Methods 0.000 abstract description 15
- 230000006870 function Effects 0.000 description 5
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 5
- VNWKTOKETHGBQD-UHFFFAOYSA-N methane Chemical compound C VNWKTOKETHGBQD-UHFFFAOYSA-N 0.000 description 4
- 238000005457 optimization Methods 0.000 description 4
- 230000007547 defect Effects 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 3
- 238000000827 velocimetry Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 239000003345 natural gas Substances 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 241000239290 Araneae Species 0.000 description 1
- 241000282412 Homo Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 210000004204 blood vessel Anatomy 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 239000002131 composite material Substances 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000007599 discharging Methods 0.000 description 1
- 238000003912 environmental pollution Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000011010 flushing procedure Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 239000007789 gas Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 210000005036 nerve Anatomy 0.000 description 1
- 239000003208 petroleum Substances 0.000 description 1
- 238000009428 plumbing Methods 0.000 description 1
- 230000000284 resting effect Effects 0.000 description 1
- 239000004576 sand Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000010865 sewage Substances 0.000 description 1
- 239000004575 stone Substances 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 239000002699 waste material Substances 0.000 description 1
- 239000002351 wastewater Substances 0.000 description 1
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
-
- 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/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/1656—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 passive imaging devices, e.g. cameras
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Acoustics & Sound (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本发明公开了基于多波束声呐的城市管道环境中潜航器避障系统与方法，基于IMU、DVL、摄像头和十波束声呐传感数据，建立多数融合的模型，优化求解获得潜航器的位姿信息、实时速度、前方管道的图像信息，以及潜航器在管道环境中的位置和航向信息；根据潜航器的前方管道的图像信息，检测和识别管道和障碍物的位置、形状和尺寸并生成环境地图；基于环境地图和潜航器的位姿信息、实时速度、六组声呐的测距信息、潜航器在管道环境中的位置和航向信息，规划出避开管道和障碍物的安全路径；潜航器根据安全路径动态调整姿态和控制策略，实现安全路径的轨迹跟踪。可高效而准确的生成潜航器避障策略，辅助城市管线自动化检测。
Description
技术领域
本发明属于城市管线自动化检测技术领域，具体涉及基于多波束声呐的城市管道环境中潜航器避障系统与方法。
背景技术
城市管网又名城市地下管线，是指埋设在城市地下的各类管线及其附属设施，作为运输手段，在石油、化工、天然气、排污等多个领域应用广泛，可用于供应水资源、排放废水、处理污水、输送能源以及推动城市的规划与发展，极大方便了人类社区，同时也带来了巨大的经济效益，被喻为城市的“神经”和“血管”，是保障城市运行的“生命线”，是消除“马路拉链”“空中蜘蛛网”、提高城市承载力的有效形式。
随着综合国力的提升，油气需求量也与日俱增，我国城市管道的大规模建设与保护等水下工程在不断增加。管道的安全性也愈发受到重视，特别是城市管道，一旦发生事故，不但带来巨大的经济损失，而且会造成严重的环境污染，引起生态破坏。由于水流冲刷及人类活动的影响，管道覆盖层正逐年变薄，甚至出现裸露，此时管道极易受到砂石的撞击和水流的冲击，并且随着管道服役时间的增加，其内壁会附着大量黏性污物，除此之外，在腐蚀和重压等因素耦合作用下，管道内壁不可避免地出现脱节、破裂、错位、塌陷现象。根据统计，2020年我国城市管道长度约310万公里，其中供水管道总长度为100.69万公里，天然气管道长度为85.06万公里，供热管道长度为42.60万公里，排水管道长度为80.27万公里，管道缺陷检测的社会需求与日俱增。
目前管道缺陷检测分为两大类：人工检测、智能检测；人工检测存在成本高昂、效率低下、检测滞后性以及存在安全隐患等问题。采用机器人进行智能检测可以较好的环节人工检测存在的诸多难点。在管道探测任务中，让潜航器进行自主避障是非常重要的。管道内部通常存在各种障碍物，例如沉积物、弯曲处、阀门等，它们可能对潜航器造成碰撞或阻塞的风险。因此，通过让潜航器进行自主避障，可以有效提高任务的安全性和成功率。首先，管道环境中的障碍物往往具有复杂的形状和结构。在传统的管道探测中，人工操作潜航器进行避障是非常困难的，因为障碍物的位置和形状可能随时变化。相比之下，让潜航器进行自主避障可以通过使用各种传感器和算法来快速感知和识别障碍物，更加灵活和高效地应对复杂的管道环境。其次，自主避障可以减少人为操作所引入的误判和错误决策。人类在繁重的管道探测任务中，可能会因为疲劳、视野受限或个人判断的主观性，导致错过或错误评估障碍物的位置和风险。而潜航器通过搭载各种传感器(如相机、激光雷达、声纳等)和使用先进的算法，能够实时、准确地感知管道环境，自主决策并采取适当的避障策略。
发明内容
本发明所要解决的技术问题是针对上述现有技术的不足，提供基于多波束声呐的城市管道环境中潜航器避障系统与方法，通过合理规划避障路径，潜航器可以更加高效地穿越城市管道，完成探测任务，避免额外的时间和资源浪费。
为实现上述技术目的，本发明采取的技术方案为：
基于多波束声呐的城市管道环境中潜航器避障系统，包括：
推进器、DVL、主控板、IMU、摄像头、电池仓、通讯板、十波束声呐；
推进器按照矢量布局的方式安装在潜航器机身周围，用于驱动潜航器沿管道前进；
DVL安装在潜航器底部，用于基于多普勒效应测量潜航器在管道环境中的实时速度，以用于导航和运动控制；
主控板位于潜航器的内部，负责接收和处理传感器数据，并将控制信号发送到推进器；
IMU安装在潜航器内部，用于测量潜航器在运动过程中的角速度和加速度信息，以提供准确的姿态估计；
摄像头通过支架固定在潜航器前方，镜头朝向潜航器的运动方向以捕捉前方管道的图像信息，实现潜航器的视觉感知；
电池仓用于提供潜航器的动力，通讯板用于与地面系统进行数据传输和通信；
十波束声呐分布在潜航器的周围，用于检测管道环境，其中，四组声呐的测距信息用于解算潜航器在管道环境中的位置和航向信息，六组声呐的测距信息用于避障。
为优化上述技术方案，采取的具体措施还包括：
上述的IMU采用激光捷联惯导系统，陀螺仪和加速度计的零偏稳定性分别为0.01°/h和10-5g，满足管道实时定位的要求；
所述DVL传感器采用A50声学多普勒计程仪；
所述摄像头选用OV7670图像传感器和二自由度摄像头云台；
所述主控板嵌入式处理器选用STM32H743vit6；
所述协处理器选择Jetson NX。
基于多波束声呐的城市管道环境中潜航器避障方法，包括：
步骤1：采集IMU、DVL、摄像头和十波束声呐传感器所获取的数据并进行数据预处理；
步骤2：通过校准和标定，将步骤1各传感器所获取的数据映射到统一的参考坐标系中；
步骤3：建立IMU的惯性测量方程、DVL测速模型、摄像头成像模型以及十波束声呐的测距模型；
步骤4：采用步骤2处理后得到的数据中IMU、DVL、摄像头传感器所获取的数据、四组声呐的测距信息，优化求解IMU的惯性测量方程、DVL测速模型、摄像头成像模型以及十波束声呐的测距模型，获得潜航器的位姿信息、实时速度、前方管道的图像信息，以及潜航器在管道环境中的位置和航向信息；
步骤5：根据步骤4得到的潜航器的前方管道的图像信息，检测和识别管道和障碍物的位置、形状和尺寸，并生成环境地图；
步骤6：基于步骤5得到的环境地图和潜航器的位姿信息、步骤4得到的实时速度和潜航器在管道环境中的位置和航向信息、步骤2处理后得到的数据中六组声呐的测距信息，采用改进的A星算法，规划出避开管道和障碍物的安全路径；
步骤7：潜航器根据步骤6得到的安全路径动态调整姿态和控制策略，实现安全路径的轨迹跟踪。
上述的步骤2所述校准和标定包括：
采用标定板法对摄像头的内参和外参进行标定；
基于椭圆校准法标定IMU的零偏参数以及安装误差；
对DVL进行垂直速度和水平速度标定；
对声呐进行延迟和束宽误差标定。
上述的步骤3所述IMU惯性测量方程基于惯性导航的比力方程构建，并基于此计算潜航器的实时位姿信息，包括姿态、速度、位置信息；
摄像头成像模型采用小孔成像模型描述摄像头模组的成像。
上述的步骤4采用牛顿迭代算法进行IMU的惯性测量方程、DVL测速模型、摄像头成像模型以及十波束声呐的测距模型求解。
上述的步骤4中，十波束声呐的测距模型，基于四组声呐的测距信息得出潜航器在管道环境中的位置和姿态的具体步骤如下：
(1)获取十波束声呐中5号、9号、6号、10号声呐的测距结果d5、d9、d6、d10；
(2)基于d5、d9、d6、d10计算潜航器的偏航姿态，计算公式为：
tanθ1＝(d5-d9)/M1,tanθ2＝(d10-d6)/M2；
其中M1和M2分别是5号、9号以及6号、10号声呐的距离值；
(3)基于(2)获得θ1和θ2，当θ1和θ2之间的差值小于阈值u时，将θ＝(θ1+θ2)/2作为最终潜航器的航向；
(4)采用(3)获得的潜航器航向角，计算潜航器偏离管道环境中心的坐标，计算公式如下：dy＝d5cosθ-d6cosθ；
上述的步骤4中，优化求解摄像头成像模型，获得潜航器前方管道的图像信息的具体步骤如下：
从每个图像帧中提取关键点并计算其特征描述子；
采用快速最近邻匹配器将当前帧的特征描述子与前一帧的特征描述子进行匹配；
使用RANSAC算法剔除匹配中的错误匹配点对；
利用剩下的正确匹配点对，计算相邻帧之间的相机运动；
使用已经估计好的相机运动，通过迭代最小化重投影误差三角化方法计算地图点的位置。
本发明具有以下有益效果：
本发明基于IMU、DVL、摄像头和十波束声呐传感器所获取的数据，建立多数融合的模型，优化求解获得潜航器的位姿信息、实时速度、前方管道的图像信息，以及潜航器在管道环境中的位置和航向信息；根据潜航器的前方管道的图像信息，检测和识别管道和障碍物的位置、形状和尺寸，并生成环境地图；基于环境地图和潜航器的位姿信息、实时速度、利用得到的数据中六组声呐的测距信息、潜航器在管道环境中的位置和航向信息，采用改进的A星算法，规划出避开管道和障碍物的安全路径；潜航器根据安全路径动态调整姿态和控制策略，实现安全路径的轨迹跟踪。可高效而准确的生成潜航器避障策略，辅助城市管线自动化检测。
附图说明
图1为本发明系统的整体结构示意图；
图2为本发明中传感器模组的标定步骤；
图3为本发明中基于十波束声呐的测距模型构建；
图4为本发明中摄像头图像数据帧间匹配步骤。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
本发明中的步骤虽然用标号进行了排列，但并不用于限定步骤的先后次序，除非明确说明了步骤的次序或者某步骤的执行需要其他步骤作为基础，否则步骤的相对次序是可以调整的。可以理解，本文中所使用的术语“和/或”涉及且涵盖相关联的所列项目中的一者或一者以上的任何和所有可能的组合。
如图1所示，本发明的基于多波束声呐的城市管道环境中潜航器避障系统，主要包括：推进器、DVL传感器、主控板、IMU、摄像头、电池仓、声呐。潜航器采用内包式的结构，其中潜航器的推进器按照矢量布局的方式安装在潜航器机身周围，通过提供推力来驱动潜航器沿管道前进。DVL传感器是安装在潜航器底部的重要组件，基于多普勒效应测量潜航器在管道环境中的实时速度，并提供给系统用于导航和运动控制；主控板是潜航器的核心控制单元，位于潜航器的内部，负责接收和处理传感器数据，并将相应的控制信号发送到推进器和其他执行机构。IMU(惯性测量单元)是安装在潜航器内部的重要装置，用于测量潜航器在运动过程中的角速度和加速度信息，以提供准确的姿态估计。摄像头是通过支架固定在潜航器前方的，它的镜头一直朝向潜航器的运动方向。这个摄像头的作用是捕捉前方管道的图像信息，为潜航器的视觉感知提供数据支持。另外还有电池仓用于提供潜航器的动力来源，通讯板用于与地面系统进行数据传输和通信。最重要的是，系统布置了十波束声呐，这些声呐分布在潜航器的周围，用于检测管道环境并提供避障所需的关键信息；其中，四组声呐数据被用于解算潜航器在管道环境中的位置和航向信息，而余下的六组声呐数据则被用于后续的避障操作。通过以上精心设计的机械结构布局，之后结合本专利所提出的潜航器避障策略，以确保潜航器在运动过程中的连续和顺畅。
所述IMU采用激光捷联惯导系统，陀螺仪和加速度计的零偏稳定性分别为0.01°/h和10-5g，满足管道实时定位的要求；
所述DVL传感器采用Water linked公司生产的A50声学多普勒计程仪；
所述摄像头选用OV7670图像传感器和亚博智能二自由度摄像头云台；
所述主控板嵌入式处理器选用意法半导体Cortex-M7内核的STM32H743vit6；
所述协处理器选择NVIDIA开发的Jetson NX；
本发明的基于多波束声呐的城市管道环境中潜航器避障方法，包括：
步骤1：采集IMU、DVL、摄像头和十波束声呐传感器所获取的数据并进行数据预处理；
数据采集和预处理：收集来自各个传感器的数据，包括IMU、DVL、摄像头和十波束声呐的信息。针对每个传感器数据，进行预处理，包括去噪、滤波、坐标转换等，以确保数据的准确性和一致性；
步骤2：通过校准和标定，将各传感器所获取的数据映射到统一的参考坐标系中；
坐标系对齐：由于各个传感器采用不同的坐标系，需要进行坐标系对齐。
通过校准和标定，将各个传感器的数据映射到统一的参考坐标系中，确保它们在相同的坐标系下进行融合；
如图2所示，所述校准和标定包括：
采用标定板法对摄像头的内参和外参进行标定；
基于椭圆校准法标定IMU的零偏参数以及安装误差；
对DVL进行垂直速度和水平速度标定；
对声呐进行延迟和束宽误差标定。
对管道潜航器上的传感器进行标定是确保传感器测量准确性和精度的重要步骤。相关传感器包括摄像头、IMU、DVL、十波束声呐，具体标定过程如下
(1)摄像头标定：
a.相机内参标定：使用标定板或者角点检测算法来获取摄像头的内参，包括焦距、主点坐标和畸变系数。
b.相机外参标定：通过在不同位置和角度下拍摄标定板获取摄像头的外参，包括相机位置和姿态信息。
(2)IMU标定：
a.零偏标定：将IMU静止在平稳的表面上，通过采集一段时间的数据来估计零偏误差，包括加速度计和陀螺仪的零偏。
b.尺度因子标定：将IMU在已知重力加速度下运动，并与真实值进行比较，估计尺度因子的误差。
c.非正交性标定：通过旋转IMU进行数据采集，并估计非正交误差。
(3)DVL标定：
a.垂直速度标定：将潜航器垂直运动，并与DVL测量值进行比较，估计垂直速度的尺度因子和偏差。
b.水平速度标定：将潜航器水平运动，并与DVL测量值进行比较，估计水平速度的尺度因子和偏差。
(4)声呐标定：
a.传感器延迟标定：通过发射声波并记录返回时间来估计声呐的传感器延迟。
b.声呐束宽标定：通过在已知距离上进行目标探测，并与真实位置进行比较，估计声呐束宽的误差。
步骤3：建立IMU的惯性测量方程、DVL测速模型、摄像头成像模型以及十波束声呐的测距模型；
建立图优化模型：根据各个传感器的特点和系统需求，建立相应的传感器模型和观测模型，包括IMU的惯性测量方程，DVL测速模型、摄像头成像模型(帧间匹配)、十波束声呐的测距模型。
所述IMU惯性测量方程基于惯性导航的比力方程构建，并基于此计算潜航器的实时姿态、速度、位置信息；
摄像头成像模型采用小孔成像模型描述摄像头模组的成像。
IMU的惯性测量方程如下：
式中，fb表示加速度计的输出，表示有害加速度，包含哥氏加速度和向心加速度。
DVL测速模型如下：
vb＝MDVLvb DVL+QDVL
式中，vb表示潜航器机体坐标系下的速度，MDVL是坐标系变换矩阵，QDVL表示DVL的测量协方差矩阵
摄像头成像模型如下：
式中，u和v是相机成像平面上的像素坐标，fx、fy、cx、cy是相机的内参，Z表示相机坐标系下的深度值，表示姿态变换矩阵，Pn是三维地理坐标系中的一个路标点；
十波束声呐的测距模型如下：
式中，di表示第i个声呐的真实测距值，表示实际的测距值，δdi表示第i个声呐的测距误差。
传感器模型(即上述的IMU的惯性测量方程、DVL测速模型、摄像头成像模型以及十波束声呐的测距模型)描述了传感器的测量特性和误差模型。图优化模型(即上述的IMU的惯性测量方程、DVL测速模型、摄像头成像模型以及十波束声呐的测距模型)描述了各个节点之间的关系；最终构建出如下各模型的代价函数：
式中，ei表示各个传感器量测模型的误差项，将各个误差项进行累积，最终得到优化的目标代价函数。
步骤4：采用步骤2处理后得到的数据中IMU、DVL、摄像头传感器所获取的数据、四组声呐的测距信息，优化求解IMU的惯性测量方程、DVL测速模型、摄像头成像模型以及十波束声呐的测距模型，获得潜航器的位姿信息、实时速度、前方管道的图像信息，以及潜航器在管道环境中的位置和航向信息；
优化求解：基于构建的图优化模型，位置、速度、姿态作为优化的变量，采用牛顿迭代算法对图模型进行求解，获得潜航器的位姿信息，包括位置、姿态和速度等。通过将位姿信息输出，为后续的避障规划模块使用；
如图3所示，所述步骤4中，十波束声呐的测距模型，基于四组声呐的测距信息得出潜航器在管道环境中的位置和姿态的具体步骤如下：
(1)获取十波束声呐中5号、9号、6号、10号声呐的测距结果d5、d9、d6、d10，其中5号、9号声呐位于潜航器的左方位置，6号、10号声呐位于潜航器对称的右边位置，并采用滑动平均滤波器去除数据中的高频噪声以及异常值；
(2)基于d5、d9、d6、d10计算潜航器的偏航姿态，计算公式为：
tanθ1＝(d5-d9)/M1,tanθ2＝(d10-d6)/M2；
其中M1和M2分别是5号、9号以及6号、10号声呐的距离值，为一个常数；
(3)基于(2)可以获得θ1和θ2，θ1和θ2分别是基于左侧5号、9号声呐以及右侧6号和10号声呐计算的航向姿态值，理想情况下二者相等，但是由于声呐存在误差以及外界存在混响干扰，导致θ1和θ2的值不相等。因此，为了更加可靠的判断潜航器的偏航位置，当θ1和θ2之间的差值小于阈值u时，将θ＝(θ1+θ2)/2作为最终潜航器的航向，否则将计算的结果作为异常值，不参与后续的运算
(4)借助(3)获得的潜航器航向角，计算潜航器偏离管道环境中心的坐标，计算公式如下：dy＝d5cosθ-d6cosθ。
所计算的θ和dy将作为约束参与到步骤5的路径规划中。
如图4所示，所述步骤4中，优化求解摄像头成像模型，获得潜航器前方管道的图像信息的具体步骤如下：
(1)特征提取与描述：从每个图像帧中提取关键点(如角点、斑点等)并计算其特征描述子；
(2)特征匹配：采用快速最近邻(FLANN)匹配器将当前帧的特征描述子与前一帧的特征描述子进行匹配；
(3)外点剔除：使用RANSAC算法剔除匹配中的错误匹配点对；
(4)运动估计：利用剩下的正确匹配点对，计算相邻帧之间的相机运动；
(5)地图点三角化：使用已经估计好的相机运动，通过迭代最小化重投影误差三角化方法计算地图点的位置。
步骤5：根据步骤4得到的潜航器的前方管道的图像信息，检测和识别管道和障碍物的位置、形状和尺寸，并生成环境地图；
利用摄像头和其他传感器的数据，对潜航器周围的环境进行感知和分析。通过图像处理、点云处理、十波束声呐处理等算法，检测和识别管道和其他障碍物的位置、形状和尺寸，并生成环境地图；
步骤6：基于步骤5得到的环境地图和潜航器的位姿信息、步骤4得到的实时速度和潜航器在管道环境中的位置和航向信息、步骤2处理后得到的数据中六组声呐的测距信息，采用改进的A星算法，规划出避开管道和障碍物的安全路径；具体流程如下：
(1)基于步骤5得到的环境地图和潜航器的位姿信息、步骤4得到的实时速度和潜航器在管道环境中的位置和航向信息、步骤2处理后得到的数据中六组声呐的测距信息，将管道抽象成一个图形，其中节点表示潜航器在管道中的位置，边表示相邻节点之间的连接关系。每个节点都有一个与之相关的代价(或距离)。
(2)初始化：将起始节点设置为潜航器的当前位置，并将其添加到一个待处理的节点列表中。同时，创建一个空的已处理节点列表。
(3)选择节点：从待处理节点列表中选择一个节点，该节点的选择标准是根据节点的代价和启发式函数(估计到目标节点的距离)计算得出的综合评估值。这个综合评估值越小，表示该节点越有可能是最佳路径的一部分。其中代价函数采用上述的
(4)目标检查：如果选择的节点是目标节点(即潜航器的目标位置)，则算法结束，找到了最佳路径。否则，继续执行下一步。
(5)扩展节点：对于选择的节点，扩展它的相邻节点。计算每个相邻节点的代价和启发式函数值，并将它们添加到待处理节点列表中。
(6)更新节点：将选择的节点从待处理节点列表中移除，并将其添加到已处理节点列表中。
(7)重复步骤(3)至(6)，直到找到目标节点或者待处理节点列表为空(表示无法到达目标节点)。
(8)生成路径：如果找到了目标节点，从目标节点开始，沿着父节点指针回溯到起始节点，就可以得到最佳路径。
路径规划和避障路线计算：基于环境地图和潜航器的位姿信息，采用改进的A星算法，计算出一条避开管道和障碍物的安全路径；
采用B样条法生成可供潜航器移动的平滑路径；
步骤7：潜航器根据步骤6得到的安全路径动态调整姿态和控制策略，实现安全路径的轨迹跟踪。
采用模型预测控制实现潜航器路径跟踪，从而有效避开障碍物。
轨迹跟踪和动态调整：潜航器根据避障路线执行轨迹跟踪，通过动态调整姿态和推进器的控制策略，以保持在规划好的路径上运动。实时监测潜航器在管道环境中的位置和速度，根据传感器反馈和避障算法的输出，进行轨迹的实时调整和修正；
障碍物回避和避障决策：在遇到突发障碍物或无法预测的环境变化时，潜航器根据实时传感器数据和避障算法的决策，进行快速的障碍物回避动作。根据障碍物的距离、速度和尺寸等信息，调整姿态和推进器的控制，以避免碰撞，并通过安全的路径继续前进。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。
Claims (8)
1.基于多波束声呐的城市管道环境中潜航器避障系统，其特征在于，包括：推进器、DVL、主控板、IMU、摄像头、电池仓、通讯板、十波束声呐；
推进器按照矢量布局的方式安装在潜航器机身周围，用于驱动潜航器沿管道前进；
DVL安装在潜航器底部，用于基于多普勒效应测量潜航器在管道环境中的实时速度，以用于导航和运动控制；
主控板位于潜航器的内部，负责接收和处理传感器数据，并将控制信号发送到推进器；
IMU安装在潜航器内部，用于测量潜航器在运动过程中的角速度和加速度信息，以提供准确的姿态估计；
摄像头通过支架固定在潜航器前方，镜头朝向潜航器的运动方向以捕捉前方管道的图像信息，实现潜航器的视觉感知；
电池仓用于提供潜航器的动力，通讯板用于与地面系统进行数据传输和通信；
十波束声呐分布在潜航器的周围，用于检测管道环境，其中，四组声呐的测距信息用于解算潜航器在管道环境中的位置和航向信息，六组声呐的测距信息用于避障。
2.根据权利要求1所述的基于多波束声呐的城市管道环境中潜航器避障系统，其特征在于，所述IMU采用激光捷联惯导系统，陀螺仪和加速度计的零偏稳定性分别为0.01°/h和10-5g，满足管道实时定位的要求；
所述DVL传感器采用A50声学多普勒计程仪；
所述摄像头选用OV7670图像传感器和二自由度摄像头云台；
所述主控板嵌入式处理器选用STM32H743vit6；
所述协处理器选择Jetson NX。
3.基于权利要求1或2所述系统实现的基于多波束声呐的城市管道环境中潜航器避障方法，其特征在于，包括：
步骤1：采集IMU、DVL、摄像头和十波束声呐传感器所获取的数据并进行数据预处理；
步骤2：通过校准和标定，将步骤1各传感器所获取的数据映射到统一的参考坐标系中；
步骤3：建立IMU的惯性测量方程、DVL测速模型、摄像头成像模型以及十波束声呐的测距模型；
步骤4：采用步骤2处理后得到的数据中IMU、DVL、摄像头传感器所获取的数据、四组声呐的测距信息，优化求解IMU的惯性测量方程、DVL测速模型、摄像头成像模型以及十波束声呐的测距模型，获得潜航器的位姿信息、实时速度、前方管道的图像信息，以及潜航器在管道环境中的位置和航向信息；
步骤5：根据步骤4得到的潜航器的前方管道的图像信息，检测和识别管道和障碍物的位置、形状和尺寸，并生成环境地图；
步骤6：基于步骤5得到的环境地图和潜航器的位姿信息、步骤4得到的实时速度和潜航器在管道环境中的位置和航向信息、步骤2处理后得到的数据中六组声呐的测距信息，采用改进的A星算法，规划出避开管道和障碍物的安全路径；
步骤7：潜航器根据步骤6得到的安全路径动态调整姿态和控制策略，实现安全路径的轨迹跟踪。
4.根据权利要求3所述的基于多波束声呐的城市管道环境中潜航器避障方法，其特征在于，步骤2所述校准和标定包括：
采用标定板法对摄像头的内参和外参进行标定；
基于椭圆校准法标定IMU的零偏参数以及安装误差；
对DVL进行垂直速度和水平速度标定；
对声呐进行延迟和束宽误差标定。
5.根据权利要求3所述的基于多波束声呐的城市管道环境中潜航器避障方法，其特征在于，步骤3所述IMU惯性测量方程基于惯性导航的比力方程构建，并基于此计算潜航器的实时位姿信息，包括姿态、速度、位置信息；
摄像头成像模型采用小孔成像模型描述摄像头模组的成像。
6.根据权利要求3所述的基于多波束声呐的城市管道环境中潜航器避障方法，其特征在于，所述步骤4采用牛顿迭代算法进行IMU的惯性测量方程、DVL测速模型、摄像头成像模型以及十波束声呐的测距模型求解。
7.根据权利要求3所述的基于多波束声呐的城市管道环境中潜航器避障方法，其特征在于，所述步骤4中，十波束声呐的测距模型，基于四组声呐的测距信息得出潜航器在管道环境中的位置和姿态的具体步骤如下：
(1)获取十波束声呐中5号、9号、6号、10号声呐的测距结果d5、d9、d6、d10；
(2)基于d5、d9、d6、d10计算潜航器的偏航姿态，计算公式为：
tanθ1＝(d5-d9)/M1,tanθ2＝(d10-d6)/M2；
其中M1和M2分别是5号、9号以及6号、10号声呐的距离值；
(3)基于(2)获得θ1和θ2，当θ1和θ2之间的差值小于阈值u时，将θ＝(θ1+θ2)/2作为最终潜航器的航向；
(4)采用(3)获得的潜航器航向角，计算潜航器偏离管道环境中心的坐标，计算公式如下：dy＝d5cosθ-d6cosθ。
8.根据权利要求3所述的基于多波束声呐的城市管道环境中潜航器避障方法，其特征在于，所述步骤4中，优化求解摄像头成像模型，获得潜航器前方管道的图像信息的具体步骤如下：
从每个图像帧中提取关键点并计算其特征描述子；
采用快速最近邻匹配器将当前帧的特征描述子与前一帧的特征描述子进行匹配；
使用RANSAC算法剔除匹配中的错误匹配点对；
利用剩下的正确匹配点对，计算相邻帧之间的相机运动；
使用已经估计好的相机运动，通过迭代最小化重投影误差三角化方法计算地图点的位置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310939783.9A CN116954225B (zh) | 2023-07-28 | 2023-07-28 | 基于多波束声呐的城市管道环境中潜航器避障系统与方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310939783.9A CN116954225B (zh) | 2023-07-28 | 2023-07-28 | 基于多波束声呐的城市管道环境中潜航器避障系统与方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116954225A true CN116954225A (zh) | 2023-10-27 |
| CN116954225B CN116954225B (zh) | 2024-03-05 |
Family
ID=88444255
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310939783.9A Active CN116954225B (zh) | 2023-07-28 | 2023-07-28 | 基于多波束声呐的城市管道环境中潜航器避障系统与方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116954225B (zh) |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20120089977A (ko) * | 2011-01-11 | 2012-08-16 | (주)아모스텍 | 잠수함의 지능형 무인 수중 자동항해 시스템과 잠수함의 무인 수중 자동 항해를 제어하는 방법 |
| US20170192439A1 (en) * | 2015-04-15 | 2017-07-06 | Shanghai Maritime University | An underwater body for remotely operated vehicle and an autonomous obstacle avoidance method thereof |
| CN206950128U (zh) * | 2013-12-18 | 2018-02-02 | 艾罗伯特公司 | 自主移动机器人 |
| CN107918399A (zh) * | 2017-11-06 | 2018-04-17 | 哈尔滨工程大学 | 一种适用于水下机器人的快速融合避障方法 |
| JP2018177074A (ja) * | 2017-04-18 | 2018-11-15 | 国立大学法人 東京大学 | 自律型水中ロボット及びその制御方法 |
| CN109240315A (zh) * | 2018-08-27 | 2019-01-18 | 西北工业大学 | 一种新型水下自主避障系统及水下避障方法 |
| CN111090279A (zh) * | 2018-10-24 | 2020-05-01 | 大连永航科技有限公司 | 基于声呐监测规避的自动导航系统 |
| CN211442686U (zh) * | 2020-01-07 | 2020-09-08 | 四川雁鑫农业发展有限公司 | 一种基于太阳能供电和超声波避障的无人船 |
| CN112660310A (zh) * | 2020-12-09 | 2021-04-16 | 威海海洋职业学院 | 一种智能无人船自动定位投料装置及系统 |
| KR20220166014A (ko) * | 2021-06-09 | 2022-12-16 | 주식회사 모션다이나믹스 | 수중 운동체의 수중 구동 제어를 위한 구동 제어 시스템 |
| CN115657683A (zh) * | 2022-11-14 | 2023-01-31 | 中国电子科技集团公司第十研究所 | 一种可用于巡检作业任务的无人无缆潜水器实时避障方法 |
| CN116295511A (zh) * | 2022-12-16 | 2023-06-23 | 南京安透可智能系统有限公司 | 一种用于管道潜航机器人的鲁棒初始对准方法及系统 |
-
2023
- 2023-07-28 CN CN202310939783.9A patent/CN116954225B/zh active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20120089977A (ko) * | 2011-01-11 | 2012-08-16 | (주)아모스텍 | 잠수함의 지능형 무인 수중 자동항해 시스템과 잠수함의 무인 수중 자동 항해를 제어하는 방법 |
| CN206950128U (zh) * | 2013-12-18 | 2018-02-02 | 艾罗伯特公司 | 自主移动机器人 |
| US20170192439A1 (en) * | 2015-04-15 | 2017-07-06 | Shanghai Maritime University | An underwater body for remotely operated vehicle and an autonomous obstacle avoidance method thereof |
| JP2018177074A (ja) * | 2017-04-18 | 2018-11-15 | 国立大学法人 東京大学 | 自律型水中ロボット及びその制御方法 |
| CN107918399A (zh) * | 2017-11-06 | 2018-04-17 | 哈尔滨工程大学 | 一种适用于水下机器人的快速融合避障方法 |
| CN109240315A (zh) * | 2018-08-27 | 2019-01-18 | 西北工业大学 | 一种新型水下自主避障系统及水下避障方法 |
| CN111090279A (zh) * | 2018-10-24 | 2020-05-01 | 大连永航科技有限公司 | 基于声呐监测规避的自动导航系统 |
| CN211442686U (zh) * | 2020-01-07 | 2020-09-08 | 四川雁鑫农业发展有限公司 | 一种基于太阳能供电和超声波避障的无人船 |
| CN112660310A (zh) * | 2020-12-09 | 2021-04-16 | 威海海洋职业学院 | 一种智能无人船自动定位投料装置及系统 |
| KR20220166014A (ko) * | 2021-06-09 | 2022-12-16 | 주식회사 모션다이나믹스 | 수중 운동체의 수중 구동 제어를 위한 구동 제어 시스템 |
| CN115657683A (zh) * | 2022-11-14 | 2023-01-31 | 中国电子科技集团公司第十研究所 | 一种可用于巡检作业任务的无人无缆潜水器实时避障方法 |
| CN116295511A (zh) * | 2022-12-16 | 2023-06-23 | 南京安透可智能系统有限公司 | 一种用于管道潜航机器人的鲁棒初始对准方法及系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116954225B (zh) | 2024-03-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Wu et al. | Survey of underwater robot positioning navigation | |
| Agnisarman et al. | A survey of automation-enabled human-in-the-loop systems for infrastructure visual inspection | |
| Özaslan et al. | Inspection of penstocks and featureless tunnel-like environments using micro UAVs | |
| Kazeminasab et al. | Localization, mapping, navigation, and inspection methods in in-pipe robots: A review | |
| Menna et al. | Towards real-time underwater photogrammetry for subsea metrology applications | |
| Thielemann et al. | Pipeline landmark detection for autonomous robot navigation using time-of-flight imagery | |
| CN113532439B (zh) | 输电线路巡检机器人同步定位与地图构建方法及装置 | |
| CN116958439B (zh) | 一种满水环境中基于多传感融合的管道三维重建方法 | |
| Xu et al. | A localization algorithm based on pose graph using Forward-looking sonar for deep-sea mining vehicle | |
| Horgan et al. | Review of machine vision applications in unmanned underwater vehicles | |
| Feng et al. | Automatic tracking method for submarine cables and pipelines of AUV based on side scan sonar | |
| Liu et al. | Implementation and analysis of tightly integrated INS/stereo VO for land vehicle navigation | |
| Hoang et al. | A simplified solution to motion estimation using an omnidirectional camera and a 2-D LRF sensor | |
| CN116954225B (zh) | 基于多波束声呐的城市管道环境中潜航器避障系统与方法 | |
| Wang et al. | Micro aerial vehicle navigation with visual-inertial integration aided by structured light | |
| Ma et al. | AUV bathymetric simultaneous localisation and mapping using graph method | |
| hyeuk Lee et al. | Elbow detection for localization of a mobile robot inside pipeline using laser pointers | |
| Peel et al. | An improved robot for bridge inspection | |
| Ma et al. | A robust fusion terrain-aided navigation method with a single Beam Echo Sounder | |
| Menendez et al. | Uslam implementation for autonomous underground robot | |
| Kumar | Development of SLAM algorithm for a Pipe Inspection Serpentine Robot | |
| Li et al. | Multisensor data fusion approach for sediment assessment of sewers in operation | |
| Liu et al. | Error modelling and optimal estimation of laser scanning aided inertial navigation system in GNSS-denied environments | |
| Yamaji et al. | Point Cloud Mapping Using Only Onboard Lidar in GNSS Denied and Dynamic Environments | |
| Hansen et al. | Vessel inspection in-the-wild: Practical planning in large-scale industrial environments |
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 |