(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210445714.8 (22)申请日 2022.04.26 (71)申请人 平安普惠企业管理有限公司 地址 518000 广东省深圳市前海深港合作 区前湾一路1号A栋201室 (72)发明人 吴绍锋　 (74)专利代理 机构 深圳市沃德知识产权代理事 务所(普通 合伙) 44347 专利代理师 高杰　于志光 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 机器人位姿分析方法、 装置、 设备及存储介 质 (57)摘要 本发明涉及智能决策技术领域， 揭露一种机 器人位姿分析方法， 包括： 获取目标机器人的二 维图像， 提取特征， 得到二维直线； 获取目标机器 人的三维点 云信息， 提取特征， 得到三维直线； 根 据设备参数构建设备参数矩阵； 根据二维直线和 三维直线构建 匹配直线对， 获取将三维直线转换 为二维直线的运动矩阵， 基于匹配直线对和设备 参数矩阵构建直线矩阵； 利用分解算法分解直线 矩阵， 获取目标机器人的旋转矩阵和平移向量， 利用运动矩阵计算旋转矩阵和平移向量， 得到目 标机器人的位姿。 本发明还提出一种机器人位姿 分析装置、 设备及存储介质。 本发明还涉及区块 链技术， 所述设备参数可存储于区块链节点中。 本发明可以提升 机器人位姿分析的准确性。 权利要求书2页 说明书10页 附图4页 CN 114800504 A 2022.07.29 CN 114800504 A 1.一种机器人位姿分析 方法， 其特 征在于， 所述方法包括： 获取所述目标机器人的周围环境的多 张二维图像， 并提取各张所述二维图像的直线特 征， 得到多个二维直线； 利用预设的点云获取设备获取所述目标机器人的周围环境的多组三维点云信 息， 提取 各组所述 三维点云信息的直线特 征， 得到多个三维直线； 获取预设的摄 像设备的设备参数 数据， 并构建所述设备参数据对应的设备参数矩阵； 根据所述二维直线及所述三维直线构建多个匹配直线对， 根据所述匹配直线对， 构建 将所述三 维直线转换为对应二维直线的运动矩阵， 并基于所述匹配直线对和所述设备参数 矩阵构建直线矩阵； 利用预设的分解算法对所述直线矩阵进行分解， 得到所述目标机器人的旋转矩阵和平 移向量， 并利用所述运动矩阵、 所述旋转矩阵及所述平移向量， 计算得到所述目标机器人的 位姿。 2.如权利要求1所述的机器人位姿分析方法， 其特征在于， 所述提取各张所述二维图像 的直线特 征， 得到多个二维直线， 包括： 将各张所述 二维图像进行 灰度化处 理， 得到多张二维灰度图像； 通过直线段检测算法从各张所述 二维灰度图像中提取直线特 征， 得到多个二维直线。 3.如权利要求1所述的机器人位姿分析方法， 其特征在于， 所述根据所述二维直线及所 述三维直线构建多个匹配直线对， 包括： 获取所述二维直线的起始点和终止点； 根据所述 起始点和所述终止点， 在所述 三维直线中查找对应点组成的三维直线； 根据所述 二维直线及对应三维直线构建匹配直线对。 4.如权利要求1所述的机器人位姿分析方法， 其特征在于， 所述根据所述匹配直线对， 构建将所述 三维直线转换为对应二维直线的运动矩阵， 包括： 将所述匹配直线对中的二维直线映射在预设的图像坐标系统中， 得到二维直线表达 式； 将所述匹配直线对中的三维直线映射在预构建的普吕克坐标系中， 得到三维直线表达 式； 根据所述二维直线表达 式和所述三维直线表达 式， 计算将所述三维直线表达式映射为 所述二维直线表达式的运动矩阵。 5.如权利要求1所述的机器人位姿分析方法， 其特征在于， 所述利用预设的分解算法对 所述直线矩阵进行分解， 得到所述目标机器人的旋转矩阵和平 移向量， 包括： 通过所述预设的分解算法对所述直线矩阵进行分解， 得到所述直线矩阵的直线分解矩 阵； 根据所述直线分解矩阵计算所述运动矩阵的共轭矩阵， 并对所述共轭矩阵进行转置， 得到所述 运动矩阵的共 轭转置矩阵； 利用所述预设的分解算法将所述共轭转置矩阵进行分解， 得到所述共轭转置矩阵的转 置分解矩阵， 并基于所述直线分解矩阵和所述转置 分解矩阵获取所述目标机器人的旋转矩 阵和平移向量。 6.如权利要求1所述的机器人位姿分析方法， 其特征在于， 所述利用预设的点云获取设权　利　要　求　书 1/2 页 2 CN 114800504 A 2备获取所述目标机器人的周围环境的多组三维点云信息， 包括： 通过所述点云获取设备获取 所述目标机器人周边的扫描数据点； 基于距离滤波算法剔除所述扫描数据点中离群的扫描数据点， 得到所述三维点云信 息。 7.如权利要求1至6中任一项所述的机器人位姿分析方法， 其特征在于， 所述提取各组 所述三维点云信息的直线特 征， 得到多个三维直线， 包括： 计算各组所述 三维点云信息中各个点的切面以及平 滑度； 根据所述各个点的切平面和平滑度， 通过区域增长法获取初始平面集合， 并利用k均值 聚类法从所述初始 平面集合中获取 标准平面 集合； 从所述标准平面集合中提取具备方向向量的平面边缘点， 并根据 所述方向向量将所述 平面边缘点聚类得到多个三维直线。 8.一种机器人位姿分析装置， 其特 征在于， 所述装置包括： 直线特征获取模块， 用于获取所述目标机器人的周围环境的多张二维图像， 并提取各 张所述二维 图像的直线特征， 得到多个二维直线， 利用预设的点云获取设备获取所述 目标 机器人的周围环境的多组三维点云信息， 提取各组所述三维点云信息的直线特征， 得到多 个三维直线； 设备参数获取模块， 用于获取预设的摄像设备的设备参数数据， 并构建所述设备参数 据对应的设备参数矩阵； 直线矩阵构建模块， 用于根据所述二维直线及所述三维直线构建多个匹配直线对， 根 据所述匹配直线对， 构建将所述三维直线转换为对应二维直线的运动矩阵， 并基于所述匹 配直线对和所述设备参数矩阵构建直线矩阵； 位姿计算模块， 用于利用预设的分解算法对所述直线矩阵进行分解， 得到所述目标机 器人的旋转矩阵和平移向量， 并利用所述运动矩阵、 所述旋转矩阵及所述平移向量， 计算得 到所述目标机器人的位姿。 9.一种电子设备， 其特 征在于， 所述电子设备包括： 至少一个处 理器； 以及， 与所述至少一个处 理器通信连接的存 储器； 其中， 所述存储器存储有可被所述至少一个处理器执行的计算机程序， 所述计算机程序被所 述至少一个处理器执行， 以使 所述至少一个处理器能够执行如权利要求 1至7中任一项所述 的机器人位姿分析 方法。 10.一种计算机可读存储介质， 包括存储数据区和存储程序区， 存储数据区存储创建的 数据， 存储程序区存储有计算机程序； 其中， 所述计算机程序被处理器执行时实现如权利要 求1至7中任一项所述的机器人位姿分析 方法。权　利　要　求　书 2/2 页 3 CN 114800504 A 3