[发明专利]基于正向运动学确定并联机器人位置的方法及装置

权利要求书

1.基于正向运动学确定并联机器人位置的方法，其特征在于，包括：

将并联机器人由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭链结构划分为第一开链结构和第二开链结构；所述第一开链结构和所述第二开链结构均是由各关节连接形成的；

将第三关节角位置为圆心和第三关节自身轴长为半径形成的第一圆，与第七关节角位置为圆心和第七关节自身轴长为半径形成的第二圆，进行相交，计算得到第四关节角在基础坐标系中的位置坐标；

基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵

从所述变换矩阵中提取法兰末端对应的位置坐标。

2.根据权利要求1所述的方法，其特征在于，当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节，以及第五关节连接形成时；所述基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵包括：

将所述第四关节角在基础坐标系中的位置坐标与所述第三关节角作为变量的变换矩阵建立元素对应关系，计算得到第三关节角θ₃；

获取第一关节沿基础坐标系Z轴的移动距离d₁、第二关节角θ₂、第四关节角θ₄，以及第五关节角θ₅；

基于基础坐标系、世界坐标系、所述并联机器人的第一标准DH参数、所述第一关节沿基础坐标系Z轴的移动距离d₁、所述第二关节角θ₂、所述第三关节角θ₃、所述第四关节角θ₄，以及所述第五关节角θ₅，确定法兰末端到世界坐标系的变换矩阵

3.根据权利要求2所述的方法，其特征在于，所述基于基础坐标系、世界坐标系、所述并联机器人的第一标准DH参数、所述第一关节沿基础坐标系Z轴的移动距离d₁、所述第二关节角θ₂、所述第三关节角θ₃、所述第四关节角θ₄，以及所述第五关节角θ₅，确定法兰末端到世界坐标系的变换矩阵包括：

基于所述第一关节沿基础坐标系Z轴的移动距离d₁，确定第一关节到基础坐标系的变换矩阵基于所述第二关节角θ₂和所述第二关节对应的第一标准DH参数，确定第二关节到第一关节的变换矩阵基于所述第三关节角θ₃和所述第三关节对应第一标准DH参数，确定第三关节到第二关节的变换矩阵基于所述第四关节角θ₄和所述第四关节对应的第一标准DH参数，确定第四关节到第三关节的变换矩阵基于第五关节角θ₅和所述第五关节对应的第一标准DH参数，确定第五关节到第四关节的变换矩阵基于法兰对应的第一标准DH参数，确定法兰末端到第五关节的变换矩阵

基于所述基础坐标系到世界坐标系的变换矩阵所述变换矩阵所述变换矩阵所述变换矩阵所述变换矩阵所述变换矩阵以及变换矩阵确定法兰末端到世界坐标系的变换矩阵