CN116242366B - 一种球罐内壁爬壁机器人行走空间跟踪与导航方法 - Google Patents

Abstract

本发明公开了一种球罐内壁爬壁机器人行走空间跟踪与导航方法，应用于机器人空间跟踪与导航领域，包括：计算机视觉识别靶标的像素坐标，并结合靶标在机器人坐标系下的坐标，利用pnp模型解算机器人在云台相机坐标系下的位姿；根据位姿和机器人中心点在机器人坐标系下的坐标，计算机器人中心点在云台相机坐标系下的坐标，以控制云台相机pitch和yaw角，使靶标始终位于图像中心范围；获取并计算划线路径中的三维坐标作为图像中心点对应的pitch和yaw角对应的图像，计算机器人中心点与图像中心点间的像素距离，以使机器人中心点与图像中心点重合；重复导航步骤，完成划线自动导航。本发明提升了巡检的效率，安全性和适用性。

Description

一种球罐内壁爬壁机器人行走空间跟踪与导航方法

技术领域

本发明涉及机器人空间跟踪与导航领域，特别涉及一种球罐内壁爬壁机器人行走空间跟踪与导航方法。

背景技术

依据固定式压力容器的相关监察规程，在对球罐进行检验时，需要停机开罐检验。在检验检测之前，通常需要准备相关球罐检验辅助工程工作，如搭设脚手架，脚手架上设置踏板，踏板设置扶手、爬梯，球罐内中上部需要设置安全网等。工人站在脚手架内使用打磨机对球罐内壁焊缝进行手动清洁除锈，然后进行检测。不仅操作不便、效率低、不能保证工作的质量，且具有较大的安全隐患。为此，球罐内壁爬壁机器人应时而生。

上海交通大学的缪松华等人提出一种基于麦克风阵列的大型压力容器机器人定位系统，该方案是在大型压力容器机器人顶部安装两个发声器，发声器各间歇性地循环播放一段声音。通过安装在储罐顶部入口处的一个麦克风阵列拾取两个声源的信号，然后通过采集卡将声源模拟信号转化成数字信号，最后通过声源定位算法对信号做处理，进而得到机器人的位置及转角。此机器人定位方案采用的是声呐定位，声呐容易受罐体回声噪声的影响导致测量漂移、精度受限等缺点，对机器人的定位容易产生较大误差，且不同罐体回声不同，这样导致算法对控制参数敏感，鲁棒性差。

国内的大型压力容器特种机器人的定位和位姿信息大部分采用简单的姿态传感和航位推算方法粗略表示，其中哈工大与大庆石油管理局第一采油厂合作研制的我国首台油田注水站金属罐防腐专用磁吸附爬壁机器人，采用倾斜计、光电编码器感知并控制机器人的姿态。清华大学与燕山石化合作开发的大型储罐罐壁检测机器人TH-Climber I，装有倾斜计能检测机器人姿态偏角，同时还携带光电接近开关能判断本体是否到达罐顶或罐底。上海交通大学与上海炼油厂合作开发的储罐容积检测机器人，利用在罐壁外铺设的磁带导航。上面方案大型压力容器特种机器人定位能力很大程度上受传感器选择影响，而高精度传感器普遍价格高昂，且部分传感器调试安装复杂，如需要铺设的磁带导航等，导致实际落地使用面临困难。

为此，如何提供一种具有高精度的自主定位、跟踪和导航能力，且部署简单，无需复杂的额外铺设，不受传感器精度影响的球罐内壁爬壁机器人行走空间跟踪与导航方法是本领域技术人员亟需解决的问题。

发明内容

有鉴于此，本发明提出了一种球罐内壁爬壁机器人行走空间跟踪与导航方法。

为了实现上述目的，本发明采用如下技术方案：

一种球罐内壁爬壁机器人行走空间跟踪与导航方法，包括：

定位：通过计算机视觉识别靶标在云台相机坐标系下的坐标，并投影到云台相机图像平面，得到像素坐标；根据像素坐标以及靶标在机器人坐标系下的坐标，利用pnp模型解算机器人在云台相机坐标系下的位姿；根据位姿和机器人中心点在机器人坐标系下的坐标，计算机器人中心点在云台相机坐标系下的坐标；

跟踪：根据机器人中心点在云台相机坐标系下的坐标，控制云台相机pitch角和yaw角，使运动的靶标始终位于云台相机图像中心范围；

导航：获取划线路径中的三维坐标，计算三维坐标作为云台相机图像中心点对应的云台相机pitch角和yaw角，得到云台相机图像，计算机器人中心点与云台相机图像中心点间的像素距离，根据所述像素距离控制机器人使机器人中心点与云台相机图像中心点重合；

重复所述导航步骤，完成划线自动导航。

可选的，靶标由安装于球罐内壁爬壁小车上的左右两块板组成，每块板由8个不同的apriltag码组合成图案。

可选的，apriltag码的检测由开源算法库实现。

可选的，通过计算机视觉识别靶标在云台相机坐标系下的坐标，如下：

球罐内壁坐标方程为：X²+Y²+(Z-D/2)²＝(D/2)²；

则，S＝(x,y,z)；

其中，S为靶标在云台相机坐标系下的坐标；θ为云台相机绕y轴旋转的yaw角；为云台相机绕x轴旋转的pitch角；H为相机中心离球罐人孔中心高度；D为球罐直径。

可选的，pnp模型，如下：

e＝sρ-K(RP_W+t)；

其中，e为重投影误差；s为常量系数；ρ为像素坐标；K为云台相机内参矩阵；R为相机相对于机器人坐标系下旋转矩阵；P_W为靶标在机器人坐标系下的坐标；t为平移向量；

根据R和t，得到机器人在云台相机坐标系下的位姿，如下：：

可选的，根据位姿和机器人中心点在机器人坐标系下的坐标，计算机器人中心点在云台相机坐标系下的坐标，如下：

其中，为机器人中心点在云台相机坐标系下的坐标；/>为机器人中心点在机器人坐标系下的坐标。

可选的，控制云台相机pitch角和yaw角，如下：

其中，为云台相机绕x轴旋转的pitch角；θ为云台相机绕y轴旋转的yaw角。

经由上述的技术方案可知，与现有技术相比，本发明提出了一种球罐内壁爬壁机器人行走空间跟踪与导航方法。实现了在大型球罐内爬壁机器人的智能自主巡检，具有出色的自主定位、跟踪和导航能力，且部署简单，定位和跟踪方式无需复杂的额外铺设，能很容易应用到各类现场环境中，提升了巡检的效率，安全性和适用性，节约了每次巡检的成本。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明的方法流程示意图。

图2为本发明的靶标安装方式示意图。

图3为本发明的靶标实物示意图。

图4为本发明的靶标在云台相机坐标系下的坐标解算关系示意图。

图5为本发明计算靶标在云台相机坐标系下的坐标所应用的三角几何关系示意图。

图6为本发明将靶标在云台相机坐标系下的坐标投影到云台相机图像平面，得到像素坐标的示意图。

图7为本发明在云台相机坐标系下控制云台相机pitch角和yaw角对机器人进行跟踪的示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1：

本发明实施例1公开了一种球罐内壁爬壁机器人行走空间跟踪与导航方法，如图1所示，包括：

定位：通过计算机视觉识别靶标在云台相机坐标系下的坐标，并投影到云台相机图像平面，得到像素坐标。

如图2所示，靶标由安装于球罐内壁爬壁小车上的左右两块板组成；如图3所示，每块板由8个不同的apriltag码组合成图案；板背部两侧装有LED条，apriltag码印刷在荧光薄片上，LED点亮后板可以均匀发光，满足黑暗环境下靶标识别的要求，供电电压为12v直流电；每块板设计多个apriltag码的目的是考虑到了现实场景中会存在靶标遮挡的问题，保留多个apriltag码可以提高冗余度，这样只要有一个apriltag码没有被遮挡就能定位到靶标。另外，apriltag码的检测由现有的开源算法库实现。

如图4所示，已知球罐内壁坐标方程为：X²+Y²+(Z-D/2)²＝(D/2)²；云台相机绕y轴旋转的yaw角θ，θ∈[0，2π)；云台相机绕x轴旋转的pitch角相机中心离球罐人孔中心高度为H，球罐直径为D。根据如图5所示的三角几何关系，可得到靶标在云台相机坐标系下的坐标S＝(x,y,z)，如下：

如图6所示，根据像素坐标以及靶标在机器人坐标系下的坐标，利用pnp模型解算机器人在云台相机坐标系下的位姿。

需要了解的是：本发明图6中的机器人以及机器人中心点仅是为了方便表示机器人坐标系，以表示靶标在机器人坐标系下的坐标，并不是表示机器人中心点的投影，图6中的投影为将靶标在云台相机坐标系下的坐标，投影到云台相机图像平面，以得到像素坐标。

pnp模型，如下：

e＝sρ-K(RP_W+t)；

其中，e为重投影误差；s为常量系数；ρ为像素坐标；K为云台相机内参矩阵；R为相机相对于机器人坐标系下旋转矩阵；P_W为靶标在机器人坐标系下的坐标；t为平移向量；

根据R和t，得到机器人在云台相机坐标系下的位姿，如下：：

根据位姿和机器人中心点在机器人坐标系下的坐标，计算机器人中心点在云台相机坐标系下的坐标，如下：

其中，为机器人中心点在云台相机坐标系下的坐标；/>为机器人中心点在机器人坐标系下的坐标。

跟踪：如图7所示，根据机器人中心点在云台相机坐标系下的坐标，控制云台相机pitch角和yaw角，使运动的靶标始终位于云台相机图像中心范围。

控制云台相机pitch角和yaw角，如下：

其中，为云台相机绕x轴旋转的pitch角；θ为云台相机绕y轴旋转的yaw角。

导航：采用划线导航的方式，在球罐内壁任意绘制一条空间导航路径曲线，得到一系列路径三维坐标(x_i，y_i，z_i)；

将一系列路径三维坐标作为机器人中心点在云台相机坐标系下的坐标输入上述定位过程中的公式中，反推得到一系列路径三维坐标作为云台相机图像中心点分别对应的一系列云台相机绕y轴旋转的yaw角θ和绕x轴旋转的pitch角即/>

云台相机从起点开始转动，直到终点/>得到每次转动的云台相机图像；

计算云台相机每次转动中，机器人中心点与云台相机图像中心点的像素距离(δ_ix，δ_iy)；

根据每次转动中机器人中心点与云台相机图像中心点的像素距离(δ_ix，δ_iy)，控制机器人中心点与每次转动的云台相机图像的中心点重合，实现划线自动导航。

本发明实施例公开了一种球罐内壁爬壁机器人行走空间跟踪与导航方法。实现了在大型球罐内爬壁机器人的智能自主巡检，具有出色的自主定位、跟踪和导航能力，且部署简单，定位和跟踪方式无需复杂的额外铺设，能很容易应用到各类现场环境中，提升了巡检的效率，安全性和适用性，节约了每次巡检的成本。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (4)

1.一种球罐内壁爬壁机器人行走空间跟踪与导航方法，其特征在于，包括：

定位：通过计算机视觉识别靶标在云台相机坐标系下的坐标，并投影到云台相机图像平面，得到像素坐标；根据所述像素坐标以及所述靶标在机器人坐标系下的坐标，利用pnp模型解算机器人在云台相机坐标系下的位姿；根据所述位姿和机器人中心点在机器人坐标系下的坐标，计算机器人中心点在云台相机坐标系下的坐标；

跟踪：根据所述机器人中心点在云台相机坐标系下的坐标，控制云台相机pitch角和yaw角，使运动的靶标始终位于云台相机图像中心范围；

导航：获取划线路径中的三维坐标，计算所述三维坐标作为云台相机图像中心点对应的云台相机pitch角和yaw角，得到云台相机图像，计算机器人中心点与云台相机图像中心点间的像素距离，根据所述像素距离控制机器人使机器人中心点与云台相机图像中心点重合；

重复所述导航步骤，完成划线自动导航；

所述pnp模型，如下：

e＝sp-K(RP_W+t)；

其中，e为重投影误差；s为常量系数；ρ为所述像素坐标；K为云台相机内参矩阵；R为相机相对于机器人坐标系下旋转矩阵；P_W为所述靶标在机器人坐标系下的坐标；t为平移向量；

根据R和t，得到机器人在云台相机坐标系下的位姿，如下：

根据所述位姿和机器人中心点在机器人坐标系下的坐标，计算机器人中心点在云台相机坐标系下的坐标，如下：

其中，为机器人中心点在云台相机坐标系下的坐标；/>为机器人中心点在机器人坐标系下的坐标；

控制云台相机pitch角和yaw角，如下：

其中，为云台相机绕x轴旋转的pitch角；θ为云台相机绕y轴旋转的yaw角。

2.根据权利要求1所述的一种球罐内壁爬壁机器人行走空间跟踪与导航方法，其特征在于，所述靶标由安装于球罐内壁爬壁小车上的左右两块板组成，每块所述板由8个不同的apriltag码组合成图案。

3.根据权利要求2所述的一种球罐内壁爬壁机器人行走空间跟踪与导航方法，其特征在于，所述apriltag码的检测由开源算法库实现。

4.根据权利要求1所述的一种球罐内壁爬壁机器人行走空间跟踪与导航方法，其特征在于，通过计算机视觉识别靶标在云台相机坐标系下的坐标，如下：

球罐内壁坐标方程为：X²+Y²+(Z-D/2)²＝(D/2)²；

则，S＝(x,y,z)；

其中，S为靶标在云台相机坐标系下的坐标；θ为云台相机绕y轴旋转的yaw角；为云台相机绕x轴旋转的pitch角；H为相机中心离球罐人孔中心高度；D为球罐直径。