(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211214966.6 (22)申请日 2022.09.30 (71)申请人 北京华航唯实机 器人科技股份有限 公司 地址 100000 北京市海淀区望福园东区北 京海青曙光房地产开 发中心产业用房 （办公） 及邮政支局项目A幢八层2-01 号 (72)发明人 李奕杰　 (74)专利代理 机构 北京慧加伦知识产权代理有 限公司 16 035 专利代理师 兰海叶 (51)Int.Cl. B25J 9/00(2006.01) B25J 17/02(2006.01)G06F 17/16(2006.01) (54)发明名称 基于正向运动学确定并联机器人位置的方 法及装置 (57)摘要 本发明实施例公开了一种基于正向运动学 确定并联机器人位置的方法及装置， 该方法首先 将并联机器人的闭链结构划分为第一开链结构 和第二开链结构； 第一开链结构和第二开链结构 均是由各关节连接形成的； 其次基于第三关节角 的位置和自身轴长， 以及第七关节角的位置和自 身轴长， 计算得到第四关节角在基础坐标系中的 位置坐标； 最后基于基础坐标系、 世界坐标系、 第 四关节角的位置坐标第一开链结构的各关节角， 以及并联机 器人的标准DH参数， 确定 变换矩阵 并从变换矩阵 中提取法兰末端对应的位 置坐标。 由此， 通过将闭链结构拆分成两个开链 结构， 并基于第四关节在基础坐标系中的位置坐 标确定从动轴的关节角， 从而提高了并联机器人 位置计算的速率和准确性。 权利要求书4页 说明书14页 附图5页 CN 115488860 A 2022.12.20 CN 115488860 A 1.基于正向运动学确定并联机器人位置的方法， 其特 征在于， 包括： 将并联机器人由第一关节、 第二关节、 第三关节、 第四关节、 第五关节、 第六关节以及第 七关节连接形成的闭链结构划分为第一开链结构和 第二开链结构； 所述第一开链结构和所 述第二开链结构均是由各关节连接形成的； 将第三关节角位置为 圆心和第 三关节自身轴长为半径形成的第 一圆， 与第七关节角位 置为圆心和第七关节自身轴长为半径形成的第二圆， 进行相交， 计算得到第四关节角在基 础坐标系中的位置坐标； 基于基础坐标系、 世界坐标系、 所述第四关节角在基础坐标系中的位置坐标、 所述第一 开链结构的各关节角， 以及 并联机器人 的标准DH参数， 确定法兰末端到世界坐标系的变换 矩阵 从所述变换矩阵 中提取法兰末端对应的位置坐标。 2.根据权利要求1所述的方法， 其特征在于， 当所述第一开链结构是由第一关节、 第二 关节、 第三关节、 第四关节， 以及第五关节连接形成时； 所述基于基础坐标系、 世界坐标系、 所述第四关节角在基础坐标系中的位置坐标、 所述第一开链结构的各关节角， 以及 并联机 器人的标准DH参数， 确定法兰末端到世界坐标系的变换矩阵 包括： 将所述第四关节角在基础坐标系中的位置坐标与所述第三关节角作为变量的变换矩 阵 建立元素对应关系， 计算得到第三关节角 θ3； 获取第一关节沿基础坐标系Z轴的移动距离d1、 第二关节角 θ2、 第四关节角 θ4， 以及第五 关节角 θ5； 基于基础坐标系、 世界坐标系、 所述并联机器人的第一标准DH参数、 所述第一关节沿基 础坐标系Z轴的移动距离d1、 所述第二关节角 θ2、 所述第三关节角 θ3、 所述第四关节角 θ4， 以 及所述第五关节角 θ5， 确定法兰末端到世界坐标系的变换矩阵 3.根据权利要求2所述的方法， 其特征在于， 所述基于基础坐标系、 世界坐标系、 所述并 联机器人的第一标准DH参数、 所述第一关节沿基础 坐标系Z轴的移动距离d1、 所述第二关节 角 θ2、 所述第三关节角 θ3、 所述第四关节角 θ4， 以及所述第五关节角 θ5， 确定法兰末端到世界 坐标系的变换矩阵 包括： 基于所述第一 关节沿基础坐标系Z轴的移动距离d1， 确定第一 关节到基础坐标系的变换 矩阵 基于所述第二关节角 θ2和所述第二关节对应的第一标准DH参数， 确定第二关节到 第一关节的变换矩阵 基于所述第三关节角 θ3和所述第三关节对应第一标准DH参数， 确 定第三关节到第二关节的变换矩阵 基于所述第四关节角 θ4和所述第四关节对应的第 一标准DH参数， 确定第四关节到第三关节的变换矩阵 基于第五关节角 θ5和所述第五关 节对应的第一标准DH参数， 确定第五关节到第四关节的变换矩阵 基于法兰对应的第一权　利　要　求　书 1/4 页 2 CN 115488860 A 2标准DH参数， 确定法兰末端到第五关节的变换矩阵 基于所述基础坐标系到世界坐标系的变换矩阵 所述变换矩阵 所述变换矩阵 所述变换矩阵 所述变换矩阵 所述变换矩阵 以及变换矩阵 确定法兰 末端到世界坐标系的变换矩阵 4.根据权利要求1所述的方法， 其特征在于， 当所述第一开链结构是由第一关节、 第六 关节、 第七关节、 第四关节， 以及第五关节连接形成时； 所述基于基础坐标系、 世界坐标系、 所述第四关节角在基础坐标系中的位置坐标、 所述第一开链结构的各关节角， 以及所述并 联机器人的标准DH参数， 确定法兰末端到世界坐标系的变换矩阵 包括： 将所述位置坐标与所述第七关节角作为变量的变换矩阵 建立元素对应关系， 得到 第七关节角 θ7； 获取第一关节沿基础坐标系Z轴的移动距离d1、 第六关节角 θ6、 第四关节角 θ4， 以及第五 关节角 θ5； 基于基础坐标系、 世界坐标系、 所述并联机器人的第二标准DH参数， 所述第一关节沿基 础坐标系Z轴的移动距离d1、 所述第六关节角 θ6、 所述第七关节角 θ7、 所述第四关节角 θ4， 以 及所述第五关节角 θ5， 确定法兰末端到世界坐标系的变换矩阵 5.根据权利要求4所述的方法， 其特征在于， 所述基于基础坐标系、 世界坐标系、 所述并 联机器人的第二标准DH参数， 所述第一关节沿基础 坐标系Z轴的移动距离d1、 所述第六关节 角 θ6、 所述第七关节角 θ7、 所述第四关节角 θ4， 以及所述第五关节角 θ5， 确定法兰末端到世界 坐标系的变换矩阵 包括： 基于所述第一 关节沿基础坐标系Z轴的移动距离d1， 确定第一 关节到基础坐标系的变换 矩阵 基于所述第六关节角 θ6和所述第六关节对应的第二标准DH参数， 确定第六关节到 第一关节的变换矩阵 基于所述第 七关节角 θ7和所述第 七关节对应第二标准DH参数， 确 定第七关节到第六关节的变换矩阵 基于所述第四关节角 θ4和所述第四关节对应 的第 二标准DH参数， 确定第四关节到第七关节的变换矩阵 基于第五关节角 θ5和所述第五关 节对应的第二标准D H参数， 确定第五关节到第四关节的变换矩阵 基于法兰对应的第二 标准DH参数， 确定法兰末端到第五关节的变换矩阵 基于所述变换矩阵 所述变换矩阵 所述变换矩阵 所述变换矩阵 所述 变换矩阵 所述变换矩阵 以及变换矩阵 确定法兰末端到世界坐标系的变换矩权　利　要　求　书 2/4 页 3 CN 115488860 A 3