一种适用于自动加油机器人的加油盖精准感知算法及装置制造方法及图纸

本发明专利技术属于燃油车自动加油技术领域，公开了一种适用于自动加油机器人的加油盖精准感知算法，针对加油外盖，提出一种利用2D粗略定位、3D精确定位的粗糙到精细定位方案，通过计算外盖点云表面法线，基于法线将点云投影为2D图像进行外盖粗略定位，利用粗略定位点进一步进行外盖分割以精确定位。针对加油内盖，提出一种粗糙到精细模板匹配方案，通过提取点云边缘关键点，结合加油内盖几何特征进行粗略位姿估计；在粗略配准基础上，将模板点云手柄和内盖点云手柄对齐以进行精细配准；最终对定位位姿进行修正以计算加油内盖精确定位位姿，并能利用少量模板库匹配多种不同型号的加油塞，避免构建大型模板库，无需高算力要求。无需高算力要求。无需高算力要求。

【技术实现步骤摘要】
一种适用于自动加油机器人的加油盖精准感知算法及装置


[0001]本专利技术属于燃油车自动加油
，具体涉及一种适用于自动加油机器人的加油盖精准感知算法及装置。

技术介绍

[0002]目前为止，加油站主要通过雇佣工人手动给燃油车加油，由于工人接受培训的时间、理论知识掌握情况等因素，使得加油工人操作存在不确定性，有可能会带来一定的安全威胁。此外，手工加油的方式需要大量的人力、物力。3D视觉技术具备很多优势，如数据采集方式丰富且速度快、精度高、非接触性、部分3D相机功耗低且具备防爆性能。因而基于3D视觉研发一种自动加油机器人，应用前景广阔。
[0003]现阶段燃油车自动加油机器人加油盖定位大致有三类解决方案：(1)视觉伺服方案。采用相机粗略定位加油盖位置并引导机器人到加油盖附近，该方式精度较低且实时性不足。(2)采用大视野的激光相机扫描加油盖附近范围三维信息，极易受到周围复杂环境的干扰，鲁棒性不强。(3)通过人机交互的方式实现燃油车加油半自动化。例如对燃油车车型进行识别，或者由车主以及加油工人手动录入车辆的数据信息，进而匹配数据库中事先定义好的车辆信息，其需要花费大量时间，并且数据库中的车辆信息可能不全，只能实现半自动化。
[0004]因此，提供一种适用于自动加油机器人的加油盖精准感知算法。满足自动加油机器人所需的高精度、强鲁棒性要求是本领域技术人员亟待解决的问题。

技术实现思路

[0005]本专利技术的目的在于提供一种融合3D和2D视觉，适用于自动加油机器人的加油盖精准感知算法及装置。
[0006]基于上述目的，本专利技术采用如下技术方案：
[0007]一种适用于自动加油机器人的加油盖精准感知算法，包括如下步骤：
[0008]步骤1：由3D相机采集加油外盖场景点云数据，根据点云法线投影获得2D图像，然后获取加油外盖粗略定位点坐标；
[0009]步骤2：基于加油外盖粗略定位点进行外盖分割，获取加油外盖精确定位点，定位加油外盖相对于3D相机的位姿；
[0010]步骤3：建立加油内盖模板库，获取模板点云；由3D相机采集加油内盖场景点云数据，从加油内盖场景点云数据中提取内盖点云，并从内盖点云和模板点云中提取边缘关键点，基于几何特征进行模板点云和内盖点云的粗略匹配；
[0011]步骤4：基于步骤3粗略匹配，将模板点云手柄主方向对齐至内盖点云手柄主方向，进行精细配准，计算加油内盖相对于3D相机的位姿；
[0012]步骤5：提出定位位姿修正方案，获得加油内盖相对于3D相机的精确位姿；
[0013]步骤6：由系统内坐标变换关系，将加油外盖和加油内盖在相机坐标系下的精确位
姿矩阵变换到机器人基坐标系下。
[0014]进一步的，在步骤1中，获得2D图像的方法为：对加油外盖场景点云进行离群点滤波去除噪点，再对加油外盖场景点云进行降采样，然后采用滑动最小二乘法平滑加油外盖场景点云；利用K
‑
D树快速搜索加油外盖场景点云中每一个点的近邻点，拟合近邻点局部表面进行法向量估计，法向量估计结果作为该点(加油外盖场景点云中每个点)的法线方向，再归一化法线；然后计算加油外盖场景点云边界信息，获得点云在X、Y维度上的最小值与最大值，即Xmin、Ymin、Xmax、Ymax；将3D加油外盖场景点云在X、Y维度的坐标(Xmin，Ymin)对应2D图像像素(0，0)点位置，将加油外盖场景点云在X、Y维度的坐标(Xmax、Ymax)对应2D图像的右下位置坐标，据此建立加油外盖场景点云的每一个点投影到2D图像每一个像素点的位置对应关系；将加油外盖场景点云每一个点处归一化的法线方向(Xn,Yn,Zn)(0≤Xn,Yn,Zn≤1)与255相乘，乘积结果将作为2D图像R、G、B三通道的像素值。
[0015]进一步的，在步骤1中，获取加油外盖粗略定位点坐标的方法为：根据2D图像上加油外盖与其他位置像素点差异，对投影过后的2D图像二值化并进行形态学操作，去除大面积噪点，保留加油外盖完整图像；随机选取图像若干行像素点，查找像素值为255的像素点位置，并对每一行查找到的位置取中心，对若干行各自查找到的中心位置取平均，得到Xox即为加油外盖粗略定位点在2D图像位置上的X坐标，同理获得加油外盖粗略定位点在2D图像Y方向上的坐标Yoy，将(Xox,Yoy)对应到3D加油外盖场景点云中，获得不包含Z维度的加油外盖粗略位置点(Xor,Yor)。
[0016]进一步的，在步骤2中，获取加油外盖精确定位点的方法为：基于区域生长分割将加油外盖场景点云数据分割为两个部分；设置阈值β，检索分割后的点集是否存在(Xor
±
β,Yor
±
β)区域，若不存在，则这部分不是外盖点云；若存在，则这部分是外盖点云，并计算外盖点云的质心，也即加油外盖精确定位点(Xoa,Yoa,Zoa)。
[0017]进一步的，在步骤2中，定位加油外盖相对于3D相机的位姿的方法为：对外盖点云进行平面拟合，获得平面参数(Ao,Bo,Co,Do)，平面参数的前三个分量即为加油外盖抓取坐标系Z向在3D相机坐标系下的坐标(Xoz,Yoz,Zoz)；将(Xoz,Yoz,Zoz)与3D相机Z向(0，0，1)对齐，应用罗德里格斯公式构建旋转矩阵Roc，即有如下关系：
[0018][0019][0020]根据矩阵运算关系，求解Roc矩阵的逆，获得加油外盖抓取坐标系X、Y向在3D相机坐标系下的坐标(Xox,Yox,Zox)和(Xoy,Yoy,Zoy)；进而获得加油外盖抓取坐标系在3D相机坐标系下的矩阵表示，即加油外盖相对于3D相机的精确位姿矩阵Togc：
[0021][0022]得到加油外盖在相机坐标系下的位置和朝向。
[0023]进一步的，在步骤3中，提取边缘关键点的方法为：采用内部形状描述子算法，有效提取内盖点云和模板点云的手柄边缘关键点和加油内盖底部圆面边缘关键点；
[0024]模板点云和内盖点云的粗略匹配方法为：对内盖点云和模板点云的边缘关键点进行3D空间圆拟合，得到内盖点云中拟合加油内盖底面3D圆的圆心(Xtc,Ytc,Ztc)和法线方向(Xtn,Ytn,Ztn)，得到模板点云加油内盖底面3D圆的圆心(Xmc,Ymc,Zmc)和法线方向(Xmn,Ymn,Zmn)，注意模板点云的圆心和法线方向可事先求解，以加快速度；根据法线方向(Xtn,Ytn,Ztn)和(Xmn,Ymn,Zmn)，由罗德里格斯公式计算旋转矩阵Rmt，将模板点云与内盖点云中加油内盖底面对齐，从而有：
[0025][0026]其中，t
mt
是3x1的平移矩阵，由此构造模板点云到内盖点云的粗略变换矩阵，定义为Tmt：
[0027][0028]进一步的，在步骤4中，将模板点云手柄主方向对齐至内盖点云手柄主方向，进行精细配准，计算加油内盖相对于3D相机的位姿的过程包括：
[0029]步骤4.1：根据(Xtn,Ytn,Ztn)法线和(Xtc,Ytc,Ztc)圆心做条本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种适用于自动加油机器人的加油盖精准感知算法，其特征在于，包括如下步骤：步骤1：由3D相机采集加油外盖场景点云数据，根据点云法线投影获得2D图像，然后获取加油外盖粗略定位点坐标；步骤2：基于加油外盖粗略定位点进行外盖分割，获取加油外盖精确定位点，定位加油外盖相对于3D相机的位姿；步骤3：建立加油内盖模板库，获取模板点云；由3D相机采集加油内盖场景点云数据，从加油内盖场景点云数据中提取内盖点云，并从内盖点云和模板点云中提取边缘关键点，基于几何特征进行模板点云和内盖点云的粗略匹配；步骤4：基于步骤3所述粗略匹配，将模板点云手柄主方向对齐至内盖点云手柄主方向，进行精细配准，计算加油内盖相对于3D相机的位姿；步骤5：提出定位位姿修正方案，获得加油内盖相对于3D相机的精确位姿；步骤6：由系统内坐标变换关系，将加油外盖和加油内盖在相机坐标系下的精确位姿矩阵变换到机器人基坐标系下。2.如权利要求1所述的适用于自动加油机器人的加油盖精准感知算法，其特征在于，在步骤1中，获得2D图像的方法为：对加油外盖场景点云进行离群点滤波去除噪点，再对加油外盖场景点云进行降采样，然后采用滑动最小二乘法平滑加油外盖场景点云；利用K
‑
D树快速搜索加油外盖场景点云中每一个点的近邻点，拟合近邻点局部表面进行法向量估计，法向量估计结果作为该点的法线方向，再归一化法线；然后计算加油外盖场景点云边界信息，获得点云在X、Y维度上的最小值与最大值，即Xmin、Ymin、Xmax、Ymax；将3D加油外盖场景点云在X、Y维度的坐标(Xmin，Ymin)对应2D图像像素(0，0)点位置，将加油外盖场景点云在X、Y维度的坐标(Xmax、Ymax)对应2D图像的右下位置坐标，据此建立加油外盖场景点云的每一个点投影到2D图像每一个像素点的位置对应关系；将加油外盖场景点云每一个点处归一化的法线方向(Xn,Yn,Zn)(0≤Xn,Yn,Zn≤1)与255相乘，乘积结果将作为2D图像R、G、B三通道的像素值。3.如权利要求2所述的适用于自动加油机器人的加油盖精准感知算法，其特征在于，在步骤1中，获取加油外盖粗略定位点坐标的方法为：根据2D图像上加油外盖与其他位置像素点差异，对投影过后的2D图像二值化并进行形态学操作，去除大面积噪点，保留加油外盖完整图像；随机选取图像若干行像素点，查找像素值为255的像素点位置，并对每一行查找到的位置取中心，对若干行各自查找到的中心位置取平均，得到Xox即为加油外盖粗略定位点在2D图像位置上的X坐标，同理获得加油外盖粗略定位点在2D图像Y方向上的坐标Yoy，将(Xox,Yoy)对应到3D加油外盖场景点云中，获得不包含Z维度的加油外盖粗略位置点(Xor,Yor)。4.如权利要求3所述的适用于自动加油机器人的加油盖精准感知算法，其特征在于，在步骤2中，获取加油外盖精确定位点的方法为：基于区域生长分割将加油外盖场景点云数据分割为两部分；设置阈值β，检索分割后的点集是否存在(Xor
±
β,Yor
±
β)区域，若不存在，则此部分不是外盖点云；若存在，则此部分是外盖点云，并计算外盖点云的质心，也即加油外盖精确定位点(Xoa,Yoa,Zoa)。5.如权利要求4所述的适用于自动加油机器人的加油盖精准感知算法，其特征在于，在步骤2中，定位加油外盖相对于3D相机的位姿的方法为：对外盖点云进行平面拟合，获得平
面参数(Ao,Bo,Co,Do)，平面参数的前三个分量即为加油外盖抓取坐标系Z向在3D相机坐标系下的坐标(Xoz,Yoz,Zoz)；将(Xoz,Yoz,Zoz)与3D相机Z向(0，0，1)对齐，应用罗德里格斯公式构建旋转矩阵Roc，即有如下关系：公式构建旋转矩阵Roc，即有如下关系：根据矩阵运算关系，求解Roc矩阵的逆，获得加油外盖抓取坐标系X、Y向在3D相机坐标系下的坐标(Xox,Yox,Zox)和(Xoy,Yoy,Zoy)；进而获得加油外盖抓取坐标系在3D相机坐标系下的矩阵表示，即加油外盖相对于3D相机的精确位姿矩阵Togc：得到加油外盖在相机坐标系下的位置和朝向。6.如权利要求1
‑
5任一所述的适用于自动加油机器人的加油盖精准感知算法，其特征在于，在步骤3中，提取边缘关键点的方法为：采用内部形状描述子算法，有效提取内盖点云和模板点云的手柄边缘关键点和加油内盖底部圆面边缘关键点；模板点云和内盖点云的粗略匹配方法为：对内盖点云和模板点云的边缘关键点进行3D空间圆拟合，得到内盖点云中拟合加油内盖底面3D圆的圆心(Xtc,Ytc,Ztc)和法线方向(Xtn,Ytn,Ztn)，得到模板点云加油内盖底面3D圆的圆心(Xmc,Ymc,Zmc)和法线方向(Xmn,Ymn,Zmn)；根据法线方向(Xtn,Ytn,Ztn)和(Xmn,Ymn,Zmn)，由罗德里格斯公式计算旋转矩阵Rmt，将模板点云与内盖点云中加油内盖底面对齐，从...

【专利技术属性】
技术研发人员：边金岳，廖智宇，程庆斌，王鑫，陈俊勇，马宏宾，
申请(专利权)人：工极智能科技苏州有限公司，
类型：发明
国别省市：