关于机器人坐标系转换问题（世界坐标系与关节坐标系的互换）

世界坐标系与关节坐标系互换—矩阵求解

前言

前面的文章有提到关节坐标系（θ1…θ6）和世界坐标系（XYZABC或XYZ rxryrz）的基本概念，本文主要从两个坐标系相互转化关系进行探究（附加带工具情况研究）。
以下面puma560工业机器人为例，其坐标系建立如下图：

puma560机器人模型（坐标系）

基本的矩阵旋转关系在此不在啰嗦，直接进入求解思路探讨过程！

1不带刀具的坐标系转化

1.1已知关节坐标系求解世界坐标系

基本思路：不论是关节坐标系还是世界坐标系，其最终的目的都是要实现既定的目标位置和姿态，那么关节坐标系与世界坐标系之间一定存在转换的关系。如R（关节）转换 = 目标姿态 = R（世界坐标）转换。
图片来源于互联网

一、旋转角度转换求解

①相对于世界坐标系的关节坐标系运动描述（为方便描述，我们假设第六轴轴向竖直）
如上所示，机器人第6轴竖直向下，指向Z轴反方向，相对于世界坐标系描述则可用向量表示为[0,0,-1],
对于任意一个工作位置的描述，可由第一到第六轴的旋转角度（θ01-θ6）来得到。那么6轴的运动我们可以进行从末尾姿态到各关节角度的反向推导。
六轴运动可分为6各轴的旋转方向（以下第6轴Z轴方向称为Z轴）：
(1)第六轴旋转θ6，则此时Z轴的方向为：R6 = R关（θ6）*[0,0,-1]

（其实可以看出来，第6轴的旋转对于Z轴的矢量无影响，由于是相对于运动后坐标系参考，所以为左乘矩阵）
(2)第五轴旋转θ5，如上图所示，第五轴的转动是相对第六轴的Y轴进行转动，此时Z轴指向为：R56 = R关（θ5）R关（θ6）[0,0,-1]
（3）第四轴旋转θ4，同理如上图，第4轴的运动是相对于第六轴Z轴方向旋转，则此时Z轴指向为：R 456 = R关z（θ4）R关y（θ5）R关z（θ6）[0,0,-1]


（4）第三轴旋转为θ3，同理，第3轴的运动是相对于第六轴Y轴方向旋转，则此时Z轴指向为：R 3456 =R关y（θ3） R关z（θ4）*R关y（θ5）R关z（θ6）[0,0,-1]

(5)第二轴旋转为θ2，同理，第2轴的运动是相对于第六轴Y轴方向旋转，则此时Z轴指向为：R 23456 =R关y（θ2）R关y（θ3） R关z（θ4）*R关y（θ5）R关z（θ6）[0,0,-1]，

第一轴旋转为θ1，同理，第1轴的运动是相对于第六轴Z轴方向旋转，则此时Z轴指向为：R 123456 =R关z（θ1）*R关y（θ2）R关y（θ3） R关z（θ4）*R关y（θ5）R关z（θ6）[0,0,1]

（以上角度θ等同于j）
假设以上：
R1’ = R关z（θ1）
R2‘ = R关z（θ1）*R关y（θ2）
R3’ = R关z（θ1）*R关y（θ2）*R关y（θ3）
R4‘ = R关z（θ1）*R关y（θ2）R关y（θ3） R关z（θ4）
R5’ = R关z（θ1）*R关y（θ2）R关y（θ3） R关z（θ4）*R关y（θ5）
R6‘ = R关z（θ1）*R关y（θ2）R关y（θ3） R关z（θ4）*R关y（θ5）*R关z（θ6）

在不带工具坐标的情况下， 对于关节坐标为（θ1,θ2,θ3,θ4,θ5,θ6）的位置时，第六轴Z轴指向可描述为R6’*[0,0,1]
②关节坐标系中描述，可忽略中间轴，直接去参考末端点与世界坐标系基点的位姿关系，设，末端点绕世界坐标系X，Y，Z旋转分别为A，B，C角度。
则旋转矩阵为

（rx,ry,rz分别代表A B C）
则其旋转后Z轴方向为：
R ABC = Rotx（A）*Roty（B）Rotz（C）[0,0,-1]

求解

因此如已知θ1-θ6，可令两式相等，即R_123456 = R_ABC
有：

=

三个方程，三个未知数，可求得A,B,C的角度值。
matlab程序如下：

clc
clear
syms theta1 theta2 theta3 theta4 theta5 theta6
syms A B C
Z = [0;0;1]
R_16 = Rotr('Z',theta1)*Rotr('Y',theta2)*Rotr('Y',theta3)* Rotr('Z',theta4)*Rotr('Y',theta5)*Rotr('Z',theta6)
R_ABC = Rotr('X',A)*Rotr('Y',B)*Rotr('Z',C)

r11=R_16(1,1);r12=R_16(1,2);r13=R_16(1,3);
r21=R_16(2,1);r22=R_16(2,2);r23=R_16<