(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210805461.0 (22)申请日 2022.07.08 (71)申请人 工极智能科技 （苏州） 有限公司 地址 215100 江苏省苏州市吴中区太湖东 路9号澹台湖大厦 （武珞科技园） 1708 室 (72)发明人 边金岳　廖智宇　程庆斌　王鑫　 陈俊勇　马宏宾　 (74)专利代理 机构 郑州翊博专利代理事务所 (普通合伙) 41155 专利代理师 周玉青 (51)Int.Cl. G06T 17/20(2006.01) G06F 17/16(2006.01) G06T 7/11(2017.01)G06T 7/73(2017.01) (54)发明名称 一种适用 于自动加油机器人的加油盖精准 感知算法及装置 (57)摘要 本发明属于燃油车自动加油技术领域， 公开 了一种适用于自动加油机器人的加油盖精准感 知算法， 针对加油外盖， 提出一种利用2D粗略定 位、 3D精确定位的粗糙到精细定位方案， 通过计 算外盖点 云表面法线， 基于法线将点云投影为2D 图像进行外盖粗略定位， 利用粗略定位点进一步 进行外盖分割以精确定位。 针对加油内盖， 提出 一种粗糙到精细模板匹配方案， 通过提取点云边 缘关键点， 结合加油内盖几何特征进行粗略位姿 估计； 在粗略配准基础上， 将模板点云手柄和内 盖点云手柄对齐以进行精细配准； 最终对定位位 姿进行修正以计算加油内盖精确定位位姿， 并能 利用少量模板库匹配多种不同型号的加油塞， 避 免构建大 型模板库， 无需 高算力要求。 权利要求书5页 说明书13页 附图8页 CN 115205495 A 2022.10.18 CN 115205495 A 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)与2 55相乘， 乘积结果将作为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相机的位姿的方法为： 对外盖点云进行平面拟合， 获得平权　利　要　求　书 1/5 页 2 CN 115205495 A 2面参数(Ao,Bo,Co,Do)， 平 面参数的前三个 分量即为加油外盖抓取坐标系Z向在3D相机坐标 系下的坐标(Xoz,Yoz,Zoz)； 将(Xoz,Yoz,Zoz)与3D相机Z向(0， 0， 1)对齐， 应用罗德里格斯 公式构建旋转矩阵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， 将模板点云与内盖点云中加油内盖底面对齐， 从而 有： 其中， tmt是3x1的平移矩阵， 由此构造模板点云到内盖点云的粗略变换矩阵， 定义为 Tmt： 7.如权利要求6所述的适用于自动加油机器人的加油盖精准感知算法， 其特征在于， 在 步骤4中， 所述将模板点云手柄主方向对齐至内盖点云手柄主方向， 进行精细配准， 计算加 油内盖相对于 3D相机的位姿的过程包括： 步骤4.1： 根据(Xtn,Ytn,Ztn)法线和(Xtc,Ytc,Ztc)圆心做条件滤波， 提取 内盖点云的权　利　要　求　书 2/5 页 3 CN 115205495 A 3