(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210593947.2 (22)申请日 2022.05.27 (71)申请人 平安普惠企业管理有限公司 地址 518000 广东省深圳市前海深港合作 区前湾一路1号A栋201室 (72)发明人 吴绍锋　 (74)专利代理 机构 深圳市沃德知识产权代理事 务所(普通 合伙) 44347 专利代理师 高杰　于志光 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 机器人位姿估计方法、 装置、 电子设备及存 储介质 (57)摘要 本发明涉及人工智能技术， 揭露了一种机器 人位姿估计方法， 包括： 获取周围环境的二维图 像及三维点 云， 从所述二维图像中提取二维直线 段以及从所述三维点云中提取三维直线段， 将所 述三维直线段映射在Plucker坐标系中， 得到六 维向量， 根据所述六维向量及所述二维直线 段构 造机器人的初始位姿参数矩阵， 通过所述初始位 姿参数矩阵构建线性可导的损失函数， 根据预设 的损失值对 所述损失函数求解， 得到所述机器人 的当前位姿参数矩阵， 根据所述 当前位姿参数矩 阵获取机器人的旋转矩阵和平移向量。 本发明还 提出一种机器人位姿估计装置、 电子设备以及存 储介质。 本发明可以提高机器人位姿估计精确 度。 权利要求书3页 说明书10页 附图3页 CN 114789446 A 2022.07.26 CN 114789446 A 1.一种机器人位姿估计方法， 其特 征在于， 所述方法包括： 获取机器人周围环境的二维图像， 在预构建的图像坐标系中提取所述二维图像中的二 维直线段并提取 所述二维直线段的首尾端点 坐标； 采集所述周围环境的三维点云， 在预构建的世界坐标系中提取所述三维点云中的三维 直线段； 计算所述三维直线段的法 向量和方向向量， 由所述法 向量和方向向量将所述三维直线 段映射为Plucker坐标系下的六维向量； 根据所述六维向量及所述 二维直线段构造所述机器人的初始位姿参数矩阵； 由所述二维直线段及三维直线段构建二维 ‑三维直线对， 提取所述二维 ‑三维直线对的 匹配误差， 并根据所述二 维‑三维直线对、 二 维直线段的首尾端点、 二 维‑三维直线对的匹配 误差及初始位姿参数矩阵构造损失函数； 根据预设的损失值对所述损失函数中的初始位姿参数矩阵求解， 得到所述机器人当前 位姿参数矩阵， 由所述机器人当前位姿参数矩阵拆解得到所述机器人的旋转矩阵和平移向 量。 2.如权利要求1所述的机器人位姿估计方法， 其特征在于， 所述在预构建的图像坐标系 中提取所述二维图像中的二维直线段， 包括： 利用中值滤波算法去除所述 二维图像中的图像噪声， 得到去噪二维图像； 利用融合Canny算子和小波变换的边缘检测方法对所述去噪二维图像进行边缘检测， 获得多组图像边 缘直线段； 基于边缘像素拟合的直线提取算法提取所述二维图像边缘直线段的直线特征， 得到所 述二维直线段。 3.如权利要求1所述的机器人位姿估计方法， 其特征在于， 所述在预构建的世界坐标系 中提取所述三维点云中的三维直线段， 包括： 将所述三维点云沿不同方向投影到二维空间， 生成多张具有深度信息的二维图像； 采用非真实感渲染技 术将所述深度信息转 化为所述二维图像的灰度值； 在所述二维图像中利用所述图像灰度值提取二维直线支持区， 将所述二维直线支持区 反投影到三维空间， 得到直线特 征点集； 将所述直线特征点集拟合成多个LSHP结构， 将所述的多个LSHP结构进行合并及扩展， 得到世界坐标系下的三维直线段。 4.如权利要求1所述的机器人位姿估计方法， 其特征在于， 所述构造的损失函数表达式 为： 其中， n为所述二维 ‑三维直线对的数量， d(li， Li)表示所述二维直线li和所述三维直线 Li的匹配误差， 为所述二维直线段的首部端点， 为所述二维直线段的尾部端点， L为所 述六维向量， T为所述初始位姿参数矩阵。 5.如权利要求4所述的机器人位姿估计方法， 其特征在于， 根据 预设的损失值对所述损 失函数中的初始位姿参数矩阵求 解， 得到所述机器人当前位姿参数矩阵， 包括：权　利　要　求　书 1/3 页 2 CN 114789446 A 2求出所述初始位姿参数矩阵中各个参数值ti的梯度 更新所述初始位姿参数矩阵中的各个参数值： 其中， β 为学习率； 根据n对二维 ‑三维直线对计算出 所述损失函数的损失值； 当所述损失值大于或者等于预设的损失值阈值 时， 返回上述获取机器人周围环境的二 维图像的步骤； 当所述损失值小于 所述预设的损失值阈值时， 根据更新后参数值t ′i得到所述机器人当 前位姿参数矩阵。 6.如权利要求1所述的机器人位姿估计方法， 其特征在于， 所述计算所述三维直线段的 法向量和方向 向量， 包括： 利用下述公式计算得到所述 三维直线段的法向量uT和方向向量vT： uT＝(L1 L2 L3)＝(ax ay az)×(bx by bz)， vT＝(L4 L5 L6)＝bw(ax ay az)‑aw(bx by bz)， 其中， L1为所述法向量uT在x轴的投影坐标， L2为所述法向量uT在y轴的投影坐标， L3为所 述法向量uT在z轴的投影坐标， L4为所述方向向量vT在x轴的投影坐标， L5为所述方向向量vT 在y轴的投影坐标， L6为所述方向向量vT在z轴的投影坐标， ax和bx分别为所述三维直线段 中 任意两点的x轴坐标， ay和by为分别为所述三维直线段中任意两点的y轴坐标， az和bz分别为 所述三维直线段中任意两点的z轴坐标， aw和bw为常数。 7.如权利要求1所述的机器人位姿估计方法， 其特征在于， 所述由所述二维直线段及三 维直线段构建二维 ‑三维直线对， 包括： 对所述二维直线和三维直线进行SURF特征提取， 并对提取的特征进行特征匹配， 获得 特征点对； 对所述特 征点对进行匹配筛 选， 获取相匹配的二维 ‑三维特征点对。 8.一种机器人位姿估计装置， 其特 征在于， 所述装置包括： 信息获取模块， 用于获取机器人周围环境的二维图像， 在预构建的图像坐标系中提取 所述二维图像中的二 维直线段并提取所述二 维直线段的首尾端点坐标； 采集所述周围环境 的三维点云， 在预构建的世界坐标系中提取 所述三维点云中的三维直线段； 信息处理模块， 用于计算所述三维直线段的法向量和方向向量， 由所述法向量和方向 向量将所述三维直线段映射为Plucker坐标系下的六维向量； 根据所述六维向量及所述二 维直线段构造所述机器人的初始位姿参数矩阵； 损失函数构建模块， 用于由所述二维直线段及三维直线段构建二维 ‑三维直线对， 提取 所述二维‑三维直线对的匹配误差， 根据所述二 维‑三维直线对、 二 维直线段的首尾端点、 二 维‑三维直线对的匹配误差及 初始位姿参数矩阵构造损失函数； 位姿求解模块， 用于根据预设的损 失值所述损 失函数中的初始位姿参数矩阵求解， 得 到所述机器人当前位姿参数矩阵， 由所述机器人当前位姿参数矩阵拆解得到所述机器人的 旋转矩阵和平 移向量。 9.一种电子设备， 其特 征在于， 所述电子设备包括： 至少一个处 理器； 以及， 与所述至少一个处 理器通信连接的存 储器； 其中，权　利　要　求　书 2/3 页 3 CN 114789446 A 3