(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211503550.6 (22)申请日 2022.11.29 (71)申请人 北京航空航天大 学杭州创新研究院 地址 310000 浙江省杭州市滨江区长河街 道创慧街18号 (72)发明人 韩冰　于晓龙　郭昱亮　牛建伟　 任涛　杨帆　马群　 (74)专利代理 机构 北京超凡宏宇专利代理事务 所(特殊普通 合伙) 11463 专利代理师 余菲 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 位姿关系标定方法及 装置、 计算机设备和可 读存储介质 (57)摘要 本申请提供一种位姿关系标定方法及 装置、 计算机设备和可读存储介质， 涉及机器人控制技 术领域。 本申请通过连接装置连接目标机器人的 机器人末端和附加轴系统的输出末端， 并控制附 加轴系统通过连接装置牵引目标机器人进行多 次位姿变化运动， 以获取附加轴系统与目标机器 人各自在每次位姿变化运动前后的实际关节位 置数据， 而后直接基于获取到的附加轴系统与目 标机器人各自的实际关节位置数据， 调用与连接 装置对应的位姿关系标定模型进行参数优化求 解， 得到目标机器人与附加轴系统之间的位姿关 系变换矩阵， 从而有效避免现有位姿关系标定过 程中的繁琐示教操作， 并提升位姿关系的标定精 准度及标定效率。 权利要求书3页 说明书13页 附图4页 CN 115533922 A 2022.12.30 CN 115533922 A 1.一种位姿关系标定方法， 其特 征在于， 所述标定方法包括： 控制附加轴系统通过连接装置牵引目标机器人进行多次位姿变化运动， 其中所述连接 装置用于连接所述目标机器人的机器人末端和所述附加轴 系统的输出末端； 获取所述附加轴系统与所述目标机器人各自在每次位姿变化运动前后的实际关节位 置数据； 基于获取到的所述附加轴系统与 所述目标机器人各自的实际关节位置数据， 调用与 所 述连接装置对应的位姿关系标定模型进行参数优化求解， 得到所述目标机器人与所述附加 轴系统之间的位姿关系变换矩阵。 2.根据权利要求1所述的标定方法， 其特征在于， 针对每次位姿变化运动， 控制所述附 加轴系统通过 所述连接装置牵引所述目标机器人实现该位姿变化 运动的步骤， 包括： 控制所述附加轴 系统执行与该位姿变化 运动对应的附加轴运动操作； 获取所述连接装置检测到的与所述附加轴运动操作对应的牵引力数据； 根据所述牵引力数据计算所述目标机器人的机器人末端在机器人基座坐标系下的期 望速度数据； 根据所述机器人末端在机器人基座坐标系下的当前位姿数据及期望速度 数据， 计算所 述机器人末端在机器人基座 坐标系下的与所述附加轴运动操作匹配的期望位姿数据； 按照所述机器人末端在机器人基座坐标系下的期望位姿数据， 基于机器人逆运动学原 理控制所述目标机器人的各机器人关节进行运动， 以完成该位姿变化 运动。 3.根据权利要求2所述的标定方法， 其特征在于， 所述根据 所述牵引力数据计算所述目 标机器人的机器人末端在机器人基座 坐标系下的期望 速度数据的步骤， 包括： 调用所述机器人末端在机器人末端坐标系下的末端作用力参数与末端速度参数之间 的关联关系， 计算所述机器人末端在机器人末端坐标系下的与所述牵引力数据对应的目标 速度数据； 根据所述目标机器人的机器人末端坐标系与机器人基座坐标系之间的当前齐次变换 矩阵， 对所述机器人末端在机器人末端坐标系 下的目标速度数据进行数据变换处理， 得到 所述机器人末端在机器人基座 坐标系下的期望 速度数据。 4.根据权利要求2所述的标定方法， 其特征在于， 所述按照所述机器人末端在机器人基 座坐标系下的期望位姿数据， 基于机器人逆运动学原理控制所述目标机器人的各机器人关 节进行运动的步骤， 包括： 基于机器人逆运动学原理对所述机器人末端在机器人基座坐标系下的期望位姿数据 进行逆运动学求 解， 得到所述目标机器人的各机器人关节的期望关节位置； 针对所述目标机器人的每个机器人关节， 按照该机器人关节的期望关节位置调 整该机 器人关节的当前关节位置 。 5.根据权利要求1 ‑4中任意一项所述的标定方法， 其特征在于， 所述基于获取到的所述 附加轴系统与所述目标机器人各自的实际关节位置数据， 调用与所述连接装置对应的位姿 关系标定模型进 行参数优化求解， 得到所述目标机器人与所述附加轴系统之间的位姿关系 变换矩阵的步骤， 包括： 针对每次位姿变化运动， 根据 所述目标机器人在该位姿变化运动前后的实际关节位置 数据， 基于机器人正向运动学原理计算得到所述目标机器人的机器人末端坐标系与机器人权　利　要　求　书 1/3 页 2 CN 115533922 A 2基座坐标系之间的第一 运动前齐次变换矩阵及第一 运动后齐次变换矩阵； 针对每次位姿变化运动， 根据 所述附加轴系统在该位姿变化运动前后的实际关节位置 数据， 基于改进型D ‑H参数法计算得到所述附加轴系统的输出末端坐标系与附加轴基座坐 标系之间的第二 运动前齐次变换矩阵及第二 运动后齐次变换矩阵； 针对每次位姿变化运动， 将与该位姿变化运动对应的所述第一运动前齐次变换矩阵、 所述第二运动前齐次变换矩阵、 所述第一运动后齐次变换矩阵及所述第二运动后齐次变换 矩阵代入到所述位姿关系标定模型中， 构建形成关于所述附加轴基座坐标系与所述机器人 基座坐标系之间的目标齐次变换矩阵的待求 解方程； 将所有位姿变化运动各自对应的待求解方程集成为目标方程组进行非线性优化求解， 并将求解得到的目标齐次变换矩阵作为所述 位姿关系变换矩阵。 6.根据权利要求5所述的标定方法， 其特征在于， 所述位姿关系标定模型采用如下式子 进行表达： ； 其中， 用于表示与第 i次位姿变化运动对应的机器人末端坐标系 T和机器人基座坐 标系B之间的第一运动前齐次变换矩阵， 用于表示与第 i次位姿变 化运动对 应的机器人 末端坐标系 T和机器人基座坐标系 B之间的第一运动后齐次变换矩阵， 用于表示与第 i 次位姿变化运动对应的输出末端坐标系 C与附加轴基座坐标系 D之间的第二运动前齐次变 换矩阵， 用于表示与第 i次位姿变 化运动对应的输出末端坐 标系C与附加轴基座坐 标系 D之间的第二运动后齐次变换矩阵， 用于表示附加轴基 座坐标系D与机器人基座坐 标系 B之间的目标齐次变换矩阵， 其中所述 目标齐次变换矩阵为所述 目标机器人与所述附加轴 系统之间的位姿关系变换矩阵。 7.一种位姿关系标定装置， 其特 征在于， 所述标定装置包括： 运动控制模块， 用于控制附加轴系统通过连接装置牵引目标机器人进行多次位姿变化 运动， 其中所述连接装置用于连接所述目标机器人的机器人末端和所述附加轴系统的输出 末端； 数据获取模块， 用于获取所述附加轴系统与所述目标机器人各自在每次位姿变化运动 前后的实际关节位置数据； 关系标定模块， 用于基于获取到的所述附加轴系统与 所述目标机器人各自的实际关节 位置数据， 调用与所述连接装置对应的位姿关系 标定模型进行参数优化求解， 得到所述 目 标机器人与所述附加轴 系统之间的位姿关系变换矩阵。 8.根据权利要求7所述的标定装置， 其特征在于， 所述位姿关系标定模型采用如下式子 进行表达： ；权　利　要　求　书 2/3 页 3 CN 115533922 A 3