(19)中华 人民共和国 国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202111659480.9 (22)申请日 2021.12.31 (71)申请人 北京航空航天大 学杭州创新研究院 地址 310000 浙江省杭州市滨江区长河街 道创慧街18号 (72)发明人 豆渊博　刘雪峰　李青锋　牛建伟　 (74)专利代理 机构 北京超凡宏宇专利代理事务 所(特殊普通 合伙) 11463 代理人 戴尧罡 (51)Int.Cl. B25J 11/00(2006.01) B25J 9/08(2006.01) B25J 9/16(2006.01) B25J 15/08(2006.01) G06V 20/10(2022.01)G06K 9/62(2022.01) G06N 3/04(2006.01) G06N 3/08(2006.01) (54)发明名称 位姿优化方法、 装置、 抓取设备和计算机可 读存储介质 (57)摘要 本发明实施例提出一种位姿优化方法、 装 置、 抓取设备和计算机可读存储介质， 涉及计算 机技术领域。 该方法由于利用了目标物体在前一 时刻的优化位姿， 则将目标物体在当前时刻的目 标点云数据对应的全局特征、 目标物体在前一时 刻的目标点云数据对应的全局特征输入预设的 位姿估计网络时， 即可获取目标物体在当前时刻 和估计位姿相对于真实位姿的相对位姿， 进而根 据相对位姿进行优化， 获得目标物体在当前时刻 的优化位姿， 即目标物体在当前时刻的真实位 姿， 避免了初始化位姿精度不高时， 引起的局部 最优的问题， 并且对位姿进行优化时， 仅使用了 点云数据， 相对于现有技 术， 推理速度更 快。 权利要求书3页 说明书17页 附图6页 CN 114310951 A 2022.04.12 CN 114310951 A 1.一种位姿优化方法， 其特 征在于， 所述方法包括： 根据目标物体在 当前时刻的图像， 获取所述目标物体在当前时刻的估计位姿和点云数 据； 根据所述估计位姿对所述目标物体在 当前时刻的点云数据进行变换， 得到所述目标物 体在当前时刻的目标点云数据； 根据所述目标物体在前一时刻的优化位姿对所述目标物体在前一时刻的点云数据进 行变换， 得到所述目标物体在前一时刻的目标点云数据； 提取所述目标物体在当前时刻的目标点云数据对应的全局特征， 以及提取所述目标物 体在前一时刻的目标点云数据对应的全局特 征； 根据所述目标物体在当前时刻的目标点云数据对应的全局特征、 所述目标物体在前一 时刻的目标点云数据对应的全局特征以及预设的位姿估计网络， 获取所述目标物体在当前 时刻的相对位姿； 根据所述相对位姿对所述估计位姿进行优化， 以获得所述目标物体在当前时刻的优化 位姿。 2.根据权利要求1所述的方法， 其特征在于， 所述估计位姿和所述目标物体在前一 时刻 的优化位姿中均包括旋转矩阵和偏移矩阵， 所述根据所述估计位姿对所述目标物体在当前 时刻的点云数据进行变换， 得到所述目标物体在当前时刻的目标点云数据， 包括： 根据所述估计位姿的旋转矩阵和偏移矩阵， 对所述目标物体在当前时刻的点云数据进 行旋转和平 移变化， 以得到所述目标物体在当前时刻的目标点云数据； 所述根据所述目标物体在前一时刻的优化位姿对所述目标物体在前一时刻的点云数 据进行变换， 得到所述目标物体在前一时刻的目标点云数据， 包括： 根据所述目标物体在前一 时刻的优化位姿的旋转矩阵和偏移矩阵， 对所述目标物体在 前一时刻的点云数据进行旋转和平移变化， 以得到所述目标物体在前一时刻的目标点云数 据。 3.根据权利要求1所述的方法， 其特征在于， 所述提取所述目标物体在当前时刻的目标 点云数据对应的全局特 征， 包括： 根据预设的近邻算法， 获取所述目标物体在 当前时刻的目标点云数据中距离每个点最 近的预设数量个点； 针对所述目标物体在当前时刻的目标点云数据中的每个点， 计算所述预设数量个点分 别到所述每 个点的边向量和法向量； 根据所述目标物体在当前时刻的目标点云数据中的每个点、 所述每个点对应的预设数 量个点以及所述每个点对应的边向量和法向量， 计算所述目标物体在当前时刻的目标点云 数据中的每 个点的初始点云特 征； 对所述目标物体在当前时刻的目标点云数据中的每个点的初始点云特征进行池化处 理， 以获得 所述目标物体在当前时刻的目标点云数据中的每 个点的点云特 征； 对所述目标物体在 当前时刻的目标点云数据中的每个点的点云特征进行加权处理， 得 到所述目标物体在当前时刻的目标点云数据对应的全局特 征。 4.根据权利要求3所述的方法， 其特征在于， 所述对所述目标物体在 当前时刻的目标点 云数据中的每个点的点云特征进 行加权处理， 得到所述目标物体在当前时刻的目标点云数权　利　要　求　书 1/3 页 2 CN 114310951 A 2据对应的全局特 征， 包括： 计算所述目标物体在当前时刻的目标点云数据中的每个点的点云特征的平均值， 获得 所述目标物体在当前时刻的目标点云数据中的每 个点的平均特 征； 利用预设的第一卷积神经网络对所述目标物体在当前时刻的目标点云数据中的每个 点的平均特征进 行非线性变化， 获得所述目标物体在当前时刻的目标点云数据中的每个点 的非线性特 征； 根据预设的第二卷积神经网络和所述目标物体在当前时刻的目标点云数据中的每个 点的非线性特征， 计算所述目标物体在当前时刻的目标点云数据中的每个点的点云特征的 权重； 将所述目标物体在当前时刻的目标点云数据中的每个点的点云特征与对应的权重相 乘， 并对乘积结果进行池化处理， 以获得所述 目标物体在当前时刻的目标点云数据对应的 全局特征。 5.根据权利要求1所述的方法， 其特征在于， 所述提取所述目标物体在前一 时刻的目标 点云数据对应的全局特 征， 包括： 根据预设的近邻算法， 获取所述目标物体在前一 时刻的目标点云数据中距离每个点最 近的预设数量个点； 针对所述目标物体在前一 时刻的目标点云数据中的每个点， 计算所述预设数量个点分 别到所述每 个点的边向量和法向量； 根据所述目标物体在前一 时刻的目标点云数据中的每个点、 所述每个点对应的预设数 量个点以及所述每个点对应的边向量和法向量， 计算所述目标物体在前一时刻的目标点云 数据中的每 个点的初始点云特 征； 对所述目标物体在前一时刻的目标点云数据中的每个点的初始点云特征进行池化处 理， 以获得 所述目标物体在前一时刻的目标点云数据中的每 个点的点云特 征； 对所述目标物体在前一 时刻的目标点云数据中的每个点的点云特征进行加权处理， 得 到所述目标物体在前一时刻的目标点云数据对应的全局特 征。 6.根据权利要求5所述的方法， 其特征在于， 所述对所述目标物体在前一 时刻的目标点 云数据中的每个点的点云特征进 行加权处理， 得到所述目标物体在前一时刻的目标点云数 据对应的全局特 征， 包括： 计算所述目标物体在前一 时刻的目标点云数据中的每个点的点云特征的平均值， 获得 所述目标物体在前一时刻的目标点云数据中的每 个点的平均特 征； 利用预设的第一卷积神经网络对所述目标物体在前一时刻的目标点云数据中的每个 点的平均特征进 行非线性变化， 获得所述目标物体在前一时刻的目标点云数据中的每个点 的非线性特 征； 根据预设的第二卷积神经网络和所述目标物体在前一时刻的目标点云数据中的每个 点的非线性特征， 计算所述目标物体在前一时刻的目标点云数据中的每个点的点云特征的 权重； 将所述目标物体在前一时刻的目标点云数据中的每个点的点云特征与对应的权重相 乘， 并对乘积结果进行池化处理， 以获得所述 目标物体在前一时刻的目标点云数据对应的 全局特征。权　利　要　求　书 2/3 页 3 CN 114310951 A 3