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

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

背景技术：

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

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

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

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

技术实现思路

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

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

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

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

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

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

7、在一些实施方式中，可见光模组内参采用棋盘格的方式采用张正友标定法进行标定，得到x轴和y轴的焦距fx和fy,图像中心沿x轴和y轴的像素坐标cx和cy,径向畸变系数k1、k2、k3,切向畸变系数p1、p2,设可见光模组坐标系下一个点的三维坐标为(xc，yc，zc)，计算得到该点在可见光模组坐标系下的像素坐标u、v的公式如下：

8、x＝xc/zc

9、y＝yc/zc

10、

11、xcorrected＝x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)

12、ycorrected＝y(1+k1r2+k2r4+k3r6)+2p2xy+p1(r2+2y2)

13、u＝fxxcorrected+cx

14、v＝fyycorrected+cy。

15、在一些实施方式中，可见光模组与雷达的外参(r∈r3×3，t∈r3×1)标定时采用不同位置的aruco的矩形标定板，分别获取标定板的矩形角点在可见光模组坐标系下的坐标、标定板的矩形角点在雷达坐标系下的坐标、以及雷达坐标系与可见光坐标系的相对的旋转矩阵r和相对平移量t，标定完成后雷达坐标系中的一个点(xl，yl，zl)转换至可见光模组坐标系下的公式为：

16、

17、在一些实施方式中，

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

19、在一些实施方式中，

20、排管机坐标系中x坐标轴的方向与排管机上抓取钻杆的开口方向相同记为z坐标轴与排管机平面垂直，方向向上记为y坐标轴为x和z两个坐标轴叉乘的结果记为雷达坐标系中的一个点(xl，yl，zl)到排管机坐标系下的点(xp,yp,zp)的变换公式如下式所示：

21、

22、在一些实施方式中，

23、吊卡坐标系中x坐标轴的方向与吊卡上抓取钻杆的虎口的开口方向相同记为z坐标轴与吊卡平面垂直，方向向上记为y坐标轴为前两个坐标轴叉乘的结果记为雷达坐标系中的一个点(xl，yl，zl)到吊卡坐标系下的点(xd,yd,zd)的变换公式如下所示：

24、

25、在一些实施方式中，

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

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

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

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

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

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

32、在一些实施方式中，

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

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

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

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

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

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

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

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

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