基于激光雷达与可见光融合的排管机回转角度测量方法

技术领域

本发明涉及智能化石油装备领域，特别涉及一种基于激光雷达与可见光融合的排管机回转角度测量方法。

背景技术

管柱处理是钻井施工的主要工作之一，其劳动强度大、安全系数低。国际钻井承包商协会的统计数据显示，管柱处理过程中的钻井事故占总事故数的80％。提高管柱处理的自动化程度，改善其作业环境极为重要。排管机是管柱处理的核心，是平台作业中最繁忙的系统之一。排管机采用推扶式操作，代替钻工将钻具从吊卡中取出放入二层台，或将钻具从二层台取出放入吊卡，替代操作人员登高作业、降低劳动强度，提高作业安全性。

由于吊卡的位置和角度不确定，排管机在和吊卡交接时的位置存在一定的不确定性。目前的自动化钻机中排管机在起下钻作业时，排管机将钻具从吊卡中取出以及将钻具放入吊卡内的操作需要纯人工手动调整机械臂位置，确保能和吊卡完成钻具交接。具体为，司钻工通过操作操作台上的触摸屏和手柄控制排管机的运动，人为操作按钮启停来驱动电机的运动。但是在实际作业过程中这样的人工操作存在如下问题：

一、对操作人员的主观经验、长时间高强度工作有较高要求；

二、经常出现人工观测导致的交接误差，使得操作效率低下，甚至会发生安全事故等。

发明内容

本发明的目的提供基于激光雷达与可见光融合的排管机回转角度测量方法，解决上述现有技术问题中的一个或多个。

本发明提出基于激光雷达与可见光融合的排管机回转角度测量方法，该排管机回转角度测量方法基于排管机自动化作业系统执行，排管机自动化作业系统包括两套可见光与激光雷达融合装置、信号处理计算机、集控PLC以及交换机，两套可见光与激光雷达融合装置、信号处理计算机、集控PLC均与交换机连接，

两套可见光与激光雷达融合装置分别对准排管机和吊卡安装，两套可见光与激光雷达融合装置的视野中均包含被抓取的钻杆，每套可见光与激光雷达融合装置中包括可见光模组和激光雷达，可见光模组用于采集可见光模组前方的图像数据，激光雷达用于采集激光雷达前方的三维点云数据；

信号处理装置用于接收解码可见光与激光雷达融合装置采集到的图像数据和三维点云数据，并执行基于激光雷达与可见光融合的排管机回转角度测量方法的应用程序，并向集控PLC发送测量的回转角度的结果；

集控PLC用于驱动排管机和吊卡工作，并接收测量的回转角度的结果，根据结果驱动排管机旋转。

在一些实施方式中，两套可见光与激光雷达融合装置组装完成，但未安装时，对可见光与激光雷达融合装置内的可见光模组和激光雷达进行标定，标定的内容包括可见光模组内参、可见光模组与雷达的外参。

在一些实施方式中，可见光模组内参采用棋盘格的方式采用张正友标定法进行标定，得到x轴和y轴的焦距f

x＝X

y＝Y

x

y

u＝f

v＝f

在一些实施方式中，可见光模组与雷达的外参(R∈R

在一些实施方式中，

两套可见光与激光雷达融合装置组装完成且安装完成后，利用可见光模组内参、可见光模组与雷达的外参的标定结果，对排管机和吊卡的坐标系进行标定，标定参数包含初始状态时排管机管柱爪和吊卡虎口的中心位置以及坐标轴的方向，初始状态时的排管机管柱爪的中心位置和吊卡虎口的中心位置分别为排管机坐标系的原点和吊卡坐标系的原点，获取雷达坐标系中的一个点转换至排管机坐标系和卡吊坐标系下的公式。

在一些实施方式中，

排管机坐标系中X坐标轴的方向与排管机上抓取钻杆的开口方向相同记为

在一些实施方式中，

吊卡坐标系中X坐标轴的方向与吊卡上抓取钻杆的虎口的开口方向相同记为

在一些实施方式中，

在信号处理计算机中设定排管机坐标系下钻杆取出到位的期望位置，

集控PLC向信号处理计算机发送取钻指令，信号处理计算机接收指令后进行排管机回转角度的计算，具体计算过程包括如下：

初始旋转角度计算，获取当前对准排管机的可见光与激光雷达融合装置获取的图像数据和三维点云数据，根据雷达坐标系中一个点转换排管机坐标系下的公式将三维点云数据转换至排管机坐标系下的点云数据，并筛选钻杆对应的点云数据，结合排管机自身的相关参数，在排管机坐标系下进行钻杆的位置信息的计算和方向的拟合，计算钻杆在排管机坐标系下z＝0平面的投影位置，并结合排管机自身的参数计算钻杆相对于排管机旋转轴的第一位置向量，并计算第一位置向量与x轴方向的夹角，即为排管机的初始旋转角度；

将初始旋转角度发送给集控PLC控制排管机旋转到相应的角度，并向前运动，抓取钻杆，吊卡虎口打开；

抓住钻杆后进行二次旋转角度计算，设置排管机坐标系下钻杆取出到位的期望位置，根据吊卡与排管机的高度差，结合钻杆的方向，求得吊卡完全取出后钻杆在z＝0平面的位置，结合排管机自身的参数计算钻杆相对于排管机旋转轴的第二位置向量，并计算出第一向量坐标与第二向量坐标之间的夹角，该夹角的角度即为排管机的二次旋转的角度；

信号处理计算机将二次旋转的角度传输给集控PLC，集控PLC控制排管机旋转相应的角度，然后向后运动，将钻杆从吊卡中取出。

在一些实施方式中，

集控PLC向信号处理计算机发送下钻指令，此时吊卡虎口保持打开，排管机抓取钻杆并旋转至初始的角度，信号处理计算机接收下钻指令进行排管机回转角度的计算，具体计算过程包括如下：

获取当前对准吊卡的可见光与激光雷达融合装置获取的图像数据和三维点云数据，根据雷达坐标系中一个点转换至吊卡坐标系下的公式将三维点云数据转换至排管机坐标系下的点云数据，筛选钻杆对应的点云数据；

在吊卡坐标系下进行钻杆的位置信息的计算和方向的拟合；

设定钻杆放入吊卡后的期望位置；

根据钻杆的位置信息、方向、吊卡与排管机的高度差、排管机自身的相关参数以及期望位置计算到位时排管机需要的回转角度；

信号处理计算机将回转角度传输给集控PLC，集控PLC控制排管机回转相应的角度，然后向前运动，将钻杆放入吊卡的虎口中，然后关闭吊卡的虎口。

在一些实施方式中，排管机自身的相关参数包括排管机上管柱爪中心点到排管机旋转轴旋转的中心点的距离、排管机距离钻台平面的高度。

本发明所述的基于激光雷达与可见光融合的排管机回转角度测量方法的优点为：

实现了排管机的自动化作业，大大减少了交接过程中认为的参与，解决了排管机在起下钻作业过程中，纯人工操控存在的依赖经验、长时间高强度工作带来的低效高危作业隐患等问题，提高了作业效率。

附图说明

图1为本发明的一些实施方式中排管机自动化作业系统的结构示意图；

图2为本发明的一些实施方式中取钻时排管机回转角度测量流程；

图3为本发明的一些实施方式中下钻工况时排管机回转角度测量流程；

图4为本发明的一些实施方式中排管机坐标系的示意图；

图5为本发明的一些实施方式中吊卡底面坐标系的示意图；

图6为本发明的一些实施方式中排管机前方钻杆识别示意图；

图7为本发明的一些实施方式中吊卡前方钻杆识别示意图。

具体实施方式

本实施例提出基于激光雷达与可见光融合的排管机回转角度测量方法的排管机自动化作业系统，

结合图1所示的内容，排管机自动化作业系统包括两套可见光与激光雷达融合装置、信号处理计算机、集控PLC以及交换机，

每套可见光与激光雷达融合装置中包括一个可见光模组和激光雷达，比如：

选用大疆Livox Mid-70激光探测测距仪传感器作为激光雷达，其输出905纳米波段激光光束，对人不可见，避免对作业人员的视线产生影响，该激光雷达可以最近到0.05米的观测盲区，且工作距离可以到达260米，视场为70.4度圆形视场，点云密度为10万点/秒，可以很好的满足近距离高分辨率的测量和观测；

选用IMX385低光传感器作为可见光模组，满足夜间作业时的拍摄需求，成像分辨率为1920*1080，选择焦距5.1毫米镜头，满足中小视场范围的拍摄需求。

两套可见光与激光雷达融合装置组装完成后针对相机的内参(即可见光模组的内参)、相机与雷达的外参(可见光模组与激光雷达的外参)在实验室环境下进行标定，其中

相机的内参采用棋盘格的方式采用张正友标定法进行标定，得到x轴和y轴的焦距f

x＝X

y＝Y

x

y

u＝f

v＝f

相机与雷达的外参

该标定过程中应用的算法等均可以直接采用现有技术实现，因此在此不做赘述。

标定结束后进行现场安装，将两套可见光与激光雷达融合装置分别对准排管机和吊卡安装，且两套可见光与激光雷达融合装置的视野中均包含被抓取的钻杆。

安装结束后，利用之前获取的相机的内参、相机与雷达的外参的标定结果，通过可见光与激光雷达融合装置对排管机和吊卡的坐标系进行标定，形成如图4所示的排管机坐标系和如图5所示的吊卡底面坐标系，确定钻杆在排管机坐标系和吊卡坐标系下的空间范围，获取雷达坐标系中的一个点(x

排管机坐标系中X坐标轴的方向与排管机上抓取钻杆的开口方向相同记为

吊卡坐标系中X坐标轴的方向与吊卡上抓取钻杆的虎口的开口方向相同记为

上述的排管机和吊卡的坐标系进行标定的过程可以采用现有技术实现，比如：设初始状态时排管机管柱爪和吊卡虎口的中心位置在其对应的可见光模组坐标系内的。

将两套可见光与激光雷达融合装置分别通过两根千兆网与位于司钻房的交换机连接，信号处理计算机与集控PLC也均与交换机连接，利用交换机进行通信，集控PLC与信号处理计算机之间通过获取信号处理计算机心跳包的方式来保证连接的可靠性。可见光与激光雷达融合装置将1080P视频信号以及雷达点云信号实时发送至信号处理计算机。信号处理计算机采用高新能计算服务器，用于运算融合处理算法。

在排管机自动化作业系统运行过程中，信号处理计算机通过网络和交换机对可见光模组获取的视频进行解码获取实时的图像数据，同时通过网络和交换机获取激光雷达实时采集的三维点云数据，

排管机只做伸缩和旋转动作，上下高度不变，

结合图2所示的内容，吊卡到位后进行取钻时，吊卡在排管机上方，在排管机从指梁区向等待区运动的期间，集控PLC向信号处理计算机发送排管机取钻的识别指令，信号处理计算机在接到指令后，启动识别，获取如图6所示的当前时刻对准排管机的可见光与激光雷达融合装置获取的图像数据和对应时间端内的三维点云数据，识别回转角A(回转角A为排管机取钻时第一次旋转的角)，识别过程为：根据雷达坐标系中的一个点到排管机坐标系下的点的变换公式将雷达坐标系下的三维点云数据转换后排管机坐标系下，设置排管机坐标系下的约束关系(即设定x、y、z坐标值的取值范围)，筛选符合约束关系的点云数据，即为钻杆的点云数据，根据钻杆的点云数据，并结合排管机自身的相关参数(相关参数包括排管机上管柱爪中心点到排管机旋转轴的距离，排管机距离钻台平面的高度)，进行排管机坐标系下钻杆的位置计算和方向拟合，获取当前钻杆在排管机坐标系下z＝0平面的投影，计算当前钻杆的投影的中心点，结合排管机自身的参数计算排管机旋转轴的旋转的中心点在排管机坐标系下z＝0平面的投影位置，获取当前钻杆的投影的中心点相对于排管机旋转轴旋转的中心点的投影位置的第一位置向量，并计算第一位置向量与该排管机坐标系下的x轴方向的夹角，该夹角的角度即为回转角A的角度。信号处理计算机将识别的回转角A发送给集控PLC内的司钻控制软件中的目标转角位置，控制排管机旋转到相应的目标角度A处，司钻工控制排管机伸缩机构向前运动(即向着钻杆所在的方向伸出)，排管机成功抓取钻杆，吊卡的虎口打开。在抓住钻杆从吊卡虎口中取出的时候，在信号处理计算机中事先设定排管机坐标系下的钻杆取出到位后的期望位置，设吊卡与排管机的高度差为d_h，识别钻杆取出时排管机的目标角度B,识别过程为：计算在排管机完全取出之后钻杆在z＝d_h平面的水平位置，然后结合钻杆的方向，求得钻杆从吊卡中完全取出到位后的期望位置上时钻杆在排管机坐标系下z＝0平面的位置，结合排管机自身的参数计算当前排管机旋转轴上旋转的中心点在排管机坐标系下z＝0平面的投影位置，获取取出到位后的期望位置时钻杆在排管机坐标系下z＝0平面的位置相对于当前排管机旋转轴上旋转的中心点在排管机坐标系下z＝0平面的投影位置的第二向量坐标，第二向量与排管机坐标系下的x轴的夹角的角度即为目标角度B，计算第一向量坐标于第二向量坐标之间的夹角，该夹角的角度即为第二次的回转的角度。将第二次的回转的角度发送给集控PLC内的司钻控制软件中，集控PLC再次控制排管机旋转对应的角度到达目标角度B，旋转完成后，司钻工控制排管机伸缩机构向后运动(即向着与钻杆所在的方向相反的方向回缩)，排管机将钻杆从吊卡中取出。

结合图3所示的内容，下钻的时候，吊卡停留在二层台附近，吊卡的虎口保持打开的这状态，排管机从指梁区抓取钻杆到准备区并旋转到角0位(角0位指的是排管机旋转轴与吊卡坐标系下z＝0的平面上与x轴之间的夹角的角度为0的位置)，排管机自动化作业系统通过集控PLC向算法发送识别指令，信号处理计算机收到指令后，启动识别，获取如图7所示的当前时刻对准吊卡的可见光与激光雷达融合装置获取的图像数据以及对应时间段内的三维点云数据，根据雷达坐标系中的一个点到吊卡坐标系下的点的变换公式将雷达坐标系下的三维点云数据转换后吊卡坐标系下，设置吊卡坐标系下的约束关系(即设定x、y、z坐标值的取值范围)，筛选符合约束关系的点云数据，即为钻杆的点云数据，根据钻杆的点云数据进行吊卡坐标系下钻杆的位置计算和方向拟合，获取当前钻杆的坐标信息和方向，在信号处理计算机中事先设定钻杆放入吊卡到位后的期望位置，此时吊卡在排管机上方，设吊卡与排管机的高度差为d_h，设置在排管机将钻杆放入吊卡到位后的期望位置后在z＝d_h平面的水平位置，然后结合钻杆的方向，求得将钻杆放入吊卡到位后的期望位置时钻杆在z＝0平面的位置，结合排管机自身的参数计算当前排管机旋转轴上旋转的中心点在吊卡坐标系下z＝0平面的投影位置，获取钻杆放入吊卡到位后的期望位置时钻杆在z＝0平面的位置相对于当前排管机旋转轴上旋转的中心点在排管机坐标系下z＝0平面的投影位置的第三向量坐标，计算第三向量坐标与x轴之间的夹角，该夹角的角度即为当前排管机的回转角度，信号处理计算机将识别结果(即回转角度)发送给集控PLC内的司钻控制软件中目标转角位置，控制排管机旋转对应的角度到达目标角度C，司钻工控制排管机甚多机构向前运动，将钻杆放入吊卡的虎口内，待确认钻杆到位后，关闭吊卡的虎口，打开排管机的管柱爪即可。

以上所述仅是本发明的优选方式，应当指出，对于本领域普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干相似的变形和改进，这些也应视为本发明的保护范围。