一种用于自动加油机器人加油盖位姿估计的方法与流程

1.本发明涉及燃油车自动加油技术领域，更具体的说是涉及一种用于自动加油机器人加油盖位姿估计的方法。


背景技术：

2.现阶段加油站绝大多数都是依靠加油工人手工进行加油，需要消耗大量的人力、物力、财力成本，且存在加油工人操作不当等问题，可能会造成安全事故，因此研发一款自动加油机器人具有广阔的应用前景。
3.目前加油盖感知与位姿估计领域已有方案大致分为三类：(1)第一类是通过使用激光扫描仪等昂贵设备来扫描加油盖附近区域来获得其三维信息，该种方法造价昂贵且难以满足加油站的防爆规定；(2)第二类是采用车主主动录入车辆信息的方式来匹配数据库中车辆信息，该种方法只能采集数据库中存在的车型且加油前期需要车主花费大量时间录入车辆信息，不具备灵活性；(3)第三类是采用单目工业相机，通过相机视野中加油盖的相对位置关系引导机械臂移动到加油盖附近，该种方法由于缺乏深度信息只能采取视觉伺服的方式，感知实时性较差且精度较低。
4.因此，如何提供一种满足自动加油感知的高精度要求的用于自动加油机器人加油盖位姿估计的方法是本领域技术人员亟需解决的问题。


技术实现要素：

5.有鉴于此，本发明提供了一种用于自动加油机器人加油盖位姿估计的方法，采用深度学习技术实现了自动加油场景中的智能感知，不需要提前人工录入信息、车辆库匹配等繁琐步骤；采用了双目立体匹配与视觉伺服相结合的方案，保证了自动加油感知的高精度要求。
6.为了实现上述目的，本发明提供如下技术方案：
7.一种用于自动加油机器人加油盖位姿估计的方法，具体步骤包括：
8.主控板与机械臂建立tcp连接，初始化双目相机连接，加载相机内部参数、手眼标定参数并初始化识别程序；
9.获取感兴趣区域的至少两张图片，分别对图片识别油箱外盖，计算得到位于油箱外盖中心的吸取点的位姿信息，主控板将位姿信息发送给机械臂，机械臂吸开外盖；
10.开启条形光源(开启条形光源，提供稳定的打光条件，得到高质量的采图结果，提高图像识别的稳定性)，获取第二感兴趣区域至少两张图片，分别对图片识别内盖把手，计算得到内盖把手抓取点的位姿信息，主控板将位姿信息发送给机械臂，机械臂移动夹爪到抓取点；
11.确定机械臂夹爪与加油内盖抓取点的相对位置关系，机械臂夹爪与内盖把手方向一致时，机械臂拧开加油内盖进行加油。
12.通过上述技术方案，本发明的技术效果在于，提出被动式双目立体视觉与视觉伺
服结合的方案，主动式双目视觉区别于被动式双目视觉，主要指利用激光或者红外光等光信号的往返时间来感知环境深度信息的视觉成像过程，加油站由于防爆要求一般位于开阔无遮挡的场景中，因此环境光干扰极大，采用光信号的主动式双目视觉容易受到环境光干扰，对环境感知能力弱，因此本发明采用被动式双目立体视觉来感知环境信息，利用立体匹配技术获得加油盖吸取点的位姿信息；被动式双目相机由于采用传统的相机成像原理获取视野内图像，对于分辨率有限的相机难以保证目标成像的精度，同时立体匹配过程中存在匹配误差，难以获取到目标的真实位置信息，因此本发明中加入了视觉伺服的方法，被动式双目相机负责前期感知任务的“粗定位”部分，即通过双目立体匹配方法计算抓取点的位姿信息的过程，视觉伺服在“粗定位”的基础上进行“精定位”，即通过视觉伺服技术引导机械臂移动到抓取点位置的过程，保证机械臂移动到加油盖抓取点位置，以便后续拧开加油盖并伸入加油枪加油。
13.优选的，在上述的一种用于自动加油机器人加油盖位姿估计的方法中，所述机械臂为六轴机械臂，所述双目相机安装在第四轴处。
14.优选的，在上述的一种用于自动加油机器人加油盖位姿估计的方法中，所述手眼标定参数加载的具体步骤如下：
15.利用机械臂提供的dh参数来求解第四轴末端的位姿数据，得到相机与第四轴末端之间的手眼标定矩阵b；
16.利用坐标变换，获得相机在机械臂基坐标系下的位姿变换关系；
[0017][0018]
其中是相机在机械臂基坐标系下的位姿矩阵，是机械臂第四轴末端在机械臂基坐标系下的位姿矩阵，是相机在机械臂第四轴末端坐标系下的位姿矩阵。
[0019]
优选的，在上述的一种用于自动加油机器人加油盖位姿估计的方法中，计算得到位于油箱外盖中心的吸取点的位姿信息具体步骤如下：
[0020]
确定油箱外盖中心点作为特征点p；识别油箱外盖，拟合油箱外盖闭合轮廓，得到闭合轮廓的中心点，即油箱外盖的中心点。左、右两张图片都采用上述方法，然后进行点匹配，获得该点的世界坐标。
[0021]
油箱外盖中心点分别在双目相机的左、右相机中成像为p
l
、p
r
，其中p
l
点x轴坐标为x
l
，p
r
点x轴坐标为x
r
；
[0022]
视差为d＝|x
l
‑
x
r
|；o
l
与o
r
表示是两个相机的光心，光心的连线叫做基线，其距离为b；f为两相机的焦距；p点距离基线的距离为称为p点的深度信息；
[0023]
利用z＝z计算得到得到p点的世界坐标(x,y,z)；
[0024]
其中，x为p点在相机中的成像点的x轴坐标，y为p点在相机中的成像点的y轴坐标，f为相机的焦距，x0为相机的像素坐标系的x轴偏移值，y0为相机的像素坐标系的y轴偏移值。
[0025]
优选的，在上述的一种用于自动加油机器人加油盖位姿估计的方法中，计算得到内盖把手抓取点的位姿信息具体步骤如下：
[0026]
首先识别左、右两图片中的内盖把手，在内盖把手区域检测内盖把手上的两条平
行线，选择左视图中两条平行线的中心点，进而根据双目相机立体匹配原理，选取右视图中对应的匹配点；
[0027]
通过检测平行线附近的边缘来判断边缘线与平行线的重合度，若边缘与平行线像素点重合度较高，则认为平行线足够准确，取平行线上的匹配点；反之则将平行线向边缘线靠拢，若重合度大于阈值，则认为靠拢成功，取靠拢后的平行线上的对应点；(计算边缘与平行线上对应像素点的距离，若距离小于设定的阈值，则认为“对应点”属于“相同点”，“对应点”为“相同点”的个数越多，重合度越高。)
[0028]
匹配点分别在双目相机的左、右相机中成像为p
l
、p
r
，其中p
l
点x轴坐标为x
l
，p
r
点x轴坐标为x
r
；
[0029]
视差为d＝|x
l
‑
x
r
|；o
l
与o
r
表示是两个相机的光心，光心的连线叫做基线，其距离为b；f为两相机的焦距；p点距离基线的距离为称为p点的深度信息；
[0030]
利用z＝z计算得到得到p点的世界坐标(x,y,z)；
[0031]
其中，x为p点在相机中的成像点的x轴坐标，y为p点在相机中的成像点的y轴坐标，f为相机的焦距，x0为相机的像素坐标系的x轴偏移值，y0为相机的像素坐标系的y轴偏移值。
[0032]
优选的，在上述的一种用于自动加油机器人加油盖位姿估计的方法中，识别油箱外盖和识别内盖把手具体步骤如下：
[0033]
构建数据集，利用zed相机分别采集1000张油箱外盖图像以及加油内盖图像，然后使用labelme软件进行油箱外盖和加油内盖位置的标注；
[0034]
模型训练，选用识别模型所述识别模型为yolov4，yolov5，nanodet，mobilenetssd中的一种，内盖把手识别选择yolact、yolact 、mask rcnn网络模型中的一种；利用标注好的数据集训练模型网络，使模型网络学习智能识别油箱外盖和内盖把手；
[0035]
模型部署，采用opencv深度学习模块、ncnn、tensorrt网络推理框架将模型部署在jetson嵌入式控制板上，并利用板载gpu进行推理加速。
[0036]
经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种用于自动加油机器人加油盖位姿估计的方法，采用深度学习技术实现了自动加油场景中的智能感知，不需要提前人工录入信息、车辆库匹配等繁琐步骤；采用了双目立体匹配与视觉伺服相结合的方案，保证了自动加油感知的高精度要求。
附图说明
[0037]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
[0038]
图1为本发明的方法流程图；
[0039]
图2为本发明的手眼标定示意图；
[0040]
图3为本发明的手眼标定流程图；
[0041]
图4为本发明的油箱外盖中心点立体匹配方法流程图；
[0042]
图5为本发明的油箱内盖把手立体匹配方法流程图。
具体实施方式
[0043]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0044]
本发明的实施例公开了一种用于自动加油机器人加油盖位姿估计的方法，如图1所示，具体步骤包括：
[0045]
s101主控板与机械臂建立tcp连接，初始化双目相机连接，加载相机内部参数、手眼标定参数并初始化识别程序；
[0046]
s102获取感兴趣区域的至少两张图片，分别对图片识别油箱外盖，计算得到位于油箱外盖中心的吸取点的位姿信息，主控板将位姿信息发送给机械臂，机械臂吸开外盖；
[0047]
s103开启条形光源，获取第二感兴趣区域至少两张图片，分别对图片识别内盖把手，计算得到内盖把手抓取点的位姿信息，主控板将位姿信息发送给机械臂，机械臂移动夹爪到抓取点；
[0048]
s104确定机械臂夹爪与加油内盖抓取点的相对位置关系，机械臂夹爪与内盖把手方向一致时，机械臂拧开加油内盖进行加油。
[0049]
通过上述技术方案，本实施例提出被动式双目立体视觉与视觉伺服结合的方案，主动式双目视觉区别于被动式双目视觉，主要指利用激光或者红外光等光信号的往返时间来感知环境深度信息的视觉成像过程，加油站由于防爆要求一般位于开阔无遮挡的场景中，因此环境光干扰极大，采用光信号的主动式双目视觉容易受到环境光干扰，对环境感知能力弱，因此本发明采用被动式双目立体视觉来感知环境信息，利用立体匹配技术获得加油盖抓取点的位姿信息；被动式双目相机由于采用传统的相机成像原理获取视野内图像，对于分辨率有限的相机难以保证目标成像的精度，同时立体匹配过程中存在匹配误差，难以获取到目标的真实位置信息，因此本发明中加入了视觉伺服的方法，被动式双目相机负责前期感知任务的“粗定位”部分，即通过双目立体匹配方法计算抓取点的位姿信息的过程，视觉伺服在“粗定位”的基础上进行“精定位”，即通过视觉伺服技术引导机械臂移动到抓取点位置的过程，保证机械臂移动到加油盖抓取点位置，以便后续拧开加油盖并伸入加油枪加油。
[0050]
为了进一步优化上述技术方案，机械臂为六轴机械臂，双目相机安装在第四轴处。
[0051]
需要说明的是：六轴机械臂顾名思义，有六个可旋转的关节，由六个伺服电机直接通过减速器、同步带轮等驱动六个关节轴的旋转。从j1开始到j6标明了6个轴，分别代表六轴机械臂的第一轴、第二轴
……
第六轴。一般来说，像相机、夹爪等工具会安装在第六轴末端(即机械臂最末端)。但是在自动加油场景中，最末端需要拧开加油盖，因此机械臂第六轴会携带夹爪旋转多圈来拧开加油盖，若相机安装在机械臂第六轴上，那么会发生相机电源线和数据线缠绕的问题，因此本实施例中将相机安装在第四轴上，避免了线缠绕问题的发生。
[0052]
进一步，需要了解的是：双目相机安装在第四轴上后，双目相机与第六轴不是固定
的，因为位于第四轴上的双目相机与第四轴是固定的，但是第五轴和第六轴会发生转动，所以双目相机与第五轴、第六轴的相对位置是会发生变化的。双目相机与第六轴位置不固定的问题会影响手眼标定任务。
[0053]
具体的，双目相机安装在机械臂第四轴上，双目相机随着机械臂运动而运动，那么可以得到相机与第四轴是固定的，因此相机与第四轴末端的相对位置是确定的，可以通过手眼标定来得到双目相机与机械臂第四轴末端的位姿矩阵(包含平移和旋转)。因此手眼标定的前提便是，相机与被标定的轴之间是固定不动的，没有位姿的变化。手眼标定采用棋盘格标定板标定的方式，棋盘格标定板放置在固定位置。
[0054]
首先来介绍一下图2中各矩阵的含义：
[0055]
a：机械臂第四轴末端在机械臂基坐标系下的位姿，可以通过机械臂的dh参数计算得到
[0056]
b：双目相机在机器机械臂第四轴末端坐标系下的位姿。
[0057]
c：双目相机在标定板坐标系下的位姿，可以通过相机标定来获取到。
[0058]
d：相机在机械臂基坐标系下的位姿。
[0059]
本发明手眼标定的任务就是获取双目相机与机械臂第四轴末端的位姿变换矩阵b，如图3所示，机械臂携带相机采集棋盘格标定板在相机中的成像，同时获取机械臂第四轴末端位姿数据(即图2中的a矩阵)，移动机械臂重新采集图像以及获取机械臂四轴位姿数据，重复采集10次，获取10组图像和机械臂四轴位姿数据。利用10组棋盘格图像进行相机标定，获取10组外参，即10组图2中的c矩阵。利用10组a矩阵和c矩阵的对应关系可以得到如下公式，利用opencv解算得到手眼标定矩阵b；
[0060][0061]
本实施例中，利用坐标转换关系来计算相机在基坐标系中位姿的具体步骤如下：
[0062]
利用机械臂提供的dh参数来求解第四轴末端的位姿数据，得到相机与第四轴末端之间的手眼标定矩阵b；
[0063]
利用坐标变换，获得相机在机械臂基坐标系下的位姿变换关系；
[0064][0065]
其中是相机在机械臂基坐标系下的位姿矩阵，是机械臂第四轴末端在机械臂基坐标系下的位姿矩阵，是相机在机械臂第四轴末端坐标系下的位姿矩阵。
[0066]
为了进一步优化上述技术方案，如图4所示，计算得到位于油箱外盖中心的吸取点的位姿信息具体步骤如下：
[0067]
确定油箱外盖中心点作为特征点p；识别油箱外盖，拟合油箱外盖闭合轮廓，得到闭合轮廓的中心点，即油箱外盖的中心点。左、右两张图片都采用上述方法，然后进行点匹配，获得该点的世界坐标。
[0068]
油箱外盖中心点分别在双目相机的左、右相机中成像为p
l
、p
r
，其中p
l
点x轴坐标为x
l
，p
r
点x轴坐标为x
r
；
[0069]
视差为d＝|x
l
‑
x
r
|；o
l
与o
r
表示是两个相机的光心，光心的连线叫做基线，其距离
为b；f为两相机的焦距；p点距离基线的距离为称为p点的深度信息；
[0070]
利用z＝z计算得到得到p点的世界坐标(x,y,z)；
[0071]
其中，x为p点在相机中的成像点的x轴坐标，y为p点在相机中的成像点的y轴坐标，f为相机的焦距，x0为相机的像素坐标系的x轴偏移值，y0为相机的像素坐标系的y轴偏移值。
[0072]
为了进一步优化上述技术方案，如图5所示，计算得到内盖把手抓取点的位姿信息具体步骤如下：
[0073]
首先识别左、右两图片中的内盖把手，在内盖把手区域检测内盖把手上的两条平行线，选择左视图中两条平行线的中心点，进而根据双目相机立体匹配原理，选取右视图中对应的匹配点；
[0074]
通过检测平行线附近的边缘来判断边缘线与平行线的重合度，若边缘与平行线像素点重合度较高，则认为平行线足够准确，取平行线上的匹配点；反之则将平行线向边缘线靠拢，若重合度大于阈值，则认为靠拢成功，取靠拢后的平行线上的对应点；(计算边缘与平行线上对应像素点的距离，若距离小于设定的阈值，则认为“对应点”属于“相同点”，“对应点”为“相同点”的个数越多，重合度越高。)
[0075]
匹配点分别在双目相机的左、右相机中成像为p
l
、p
r
，其中p
l
点x轴坐标为x
l
，p
r
点x轴坐标为x
r
；
[0076]
视差为d＝|x
l
‑
x
r
|；o
l
与o
r
表示是两个相机的光心，光心的连线叫做基线，其距离为b；f为两相机的焦距；p点距离基线的距离为称为p点的深度信息；
[0077]
利用z＝z计算得到得到p点的世界坐标(x,y,z)；
[0078]
其中，x为p点在相机中的成像点的x轴坐标，y为p点在相机中的成像点的y轴坐标，f为相机的焦距，x0为相机的像素坐标系的x轴偏移值，y0为相机的像素坐标系的y轴偏移值。
[0079]
为了进一步优化上述技术方案，识别油箱外盖和识别内盖把手具体步骤如下：
[0080]
构建数据集，利用zed相机分别采集1000张油箱外盖图像以及加油内盖图像，然后使用labelme软件进行油箱外盖和加油内盖位置的标注；
[0081]
模型训练，选用识别模型识别模型为yolov4，yolov5，nanodet，mobilenetssd中的一种，内盖把手识别选择yolact、yolact 、mask rcnn网络模型中的一种；利用标注好的数据集训练模型网络，使模型网络学习智能识别油箱外盖和内盖把手(使网络具备智能识别油箱外盖和内盖把手的能力。)；
[0082]
模型部署，采用opencv深度学习模块、ncnn、tensorrt网络推理框架将模型部署在jetson嵌入式控制板上，并利用板载gpu进行推理加速。
[0083]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0084]
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。
对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。