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

前言

前面的文章有提到关节坐标系（θ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(2,3);
r31=R_16(3,1);r32=R_16(3,2);r33=R_16(3,3);A = -atan(r23/r33)
B = asin(r13)
C = -atan(r12/r11)function R = Rotr( axis ,theta )
%为方便调用旋转矩阵，写此 函数switch axiscase 'X'R=[1,0,0,0;0,cos(theta),-sin(theta),0;0,sin(theta),cos(theta),0;0,0,0,1];case 'Y'R=[cos(theta),0,sin(theta),0;0,1,0,0;-sin(theta),0,cos(theta),0;0,0,0,1];case 'Z'R=[cos(theta),-sin(theta),0,0;sin(theta),cos(theta),0,0;0,0,1,0;0,0,0,1];
endfunction T = Trans( u,v,w )
%为方便调用平移矩阵，写此函数T=[1,0,0,u;0,1,0,v;0,0,1,w;0,0,0,1];
end

求解结果为：R_16 =

[ - sin(theta6)*(cos(theta4)sin(theta1) - sin(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)cos(theta2)cos(theta3))) - cos(theta6)(cos(theta5)(sin(theta1)sin(theta4) + cos(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)cos(theta3))) + sin(theta5)(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)cos(theta3)sin(theta2))), sin(theta6)(cos(theta5)(sin(theta1)sin(theta4) + cos(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)cos(theta3))) + sin(theta5)(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)sin(theta2))) - cos(theta6)(cos(theta4)sin(theta1) - sin(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)cos(theta3))), cos(theta5)(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)sin(theta2)) - sin(theta5)(sin(theta1)sin(theta4) + cos(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)cos(theta3))), 0]
[ sin(theta6)(cos(theta1)cos(theta4) + sin(theta4)(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)cos(theta3)sin(theta1))) + cos(theta6)(cos(theta5)(cos(theta1)sin(theta4) - cos(theta4)(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)sin(theta1))) - sin(theta5)(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)sin(theta2))), cos(theta6)(cos(theta1)cos(theta4) + sin(theta4)(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)cos(theta3)sin(theta1))) - sin(theta6)(cos(theta5)(cos(theta1)sin(theta4) - cos(theta4)(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)sin(theta1))) - sin(theta5)(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)sin(theta2))), sin(theta5)(cos(theta1)sin(theta4) - cos(theta4)(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)sin(theta1))) + cos(theta5)(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), 0]
[ sin(theta4)sin(theta6)(cos(theta2)sin(theta3) + cos(theta3)sin(theta2)) - cos(theta6)(sin(theta5)(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + cos(theta4)cos(theta5)(cos(theta2)sin(theta3) + cos(theta3)sin(theta2))), sin(theta6)(sin(theta5)(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + cos(theta4)cos(theta5)(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))) + cos(theta6)sin(theta4)(cos(theta2)*sin(theta3) + cos(theta3)sin(theta2)), cos(theta5)(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) - cos(theta4)sin(theta5)(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)), 0]
[ 0, 0, 0, 1]

R_ABC =

[ cos(B)*cos©, -cos(B)*sin©, sin(B), 0]
[ cos(A)*sin© + cos©*sin(A)*sin(B), cos(A)*cos© - sin(A)*sin(B)*sin©, -cos(B)*sin(A), 0]
[ sin(A)*sin© - cos(A)*cos©*sin(B), cos©*sin(A) + cos(A)*sin(B)*sin©, cos(A)*cos(B), 0]
[ 0, 0, 0, 1]

设R11 = R_16(1,1),以此类推，有
A = -atan(r23/r33)
B = asin(r13)
C = -atan(r12/r11)
则：A B C的值为：
A =

-atan((sin(theta5)*(cos(theta1)sin(theta4) - cos(theta4)(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)sin(theta1))) + cos(theta5)(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)sin(theta2)))/(cos(theta5)(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) - cos(theta4)sin(theta5)(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))))

B =

-asin(sin(theta5)*(sin(theta1)sin(theta4) + cos(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)cos(theta3))) - cos(theta5)(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)))

C =

-atan((cos(theta6)*(cos(theta4)sin(theta1) - sin(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)cos(theta2)cos(theta3))) - sin(theta6)(cos(theta5)(sin(theta1)sin(theta4) + cos(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)cos(theta3))) + sin(theta5)(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)sin(theta2))))/(sin(theta6)(cos(theta4)sin(theta1) - sin(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)cos(theta2)cos(theta3))) + cos(theta6)(cos(theta5)(sin(theta1)sin(theta4) + cos(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)cos(theta3))) + sin(theta5)(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)))))

二、位置关系转换求解

思路：本部分参考殷卓的《浅谈工业机器人坐标系转化》，其利用了机器人由六个关节组成，末端手臂的位置向量是这六个关节位置向量的矢量和的思路。
如上图中的机器人各关节的尺寸已知（仅提供一个思路）
（注意，以下为0指的是所有关节转角为0）
①第一关节
在θ1为0时，位置向量是向X方向偏移一定的距离，记作x1
则θ1不为0时，位移则为：
R1’[x1,0,0]
②第二关节
在θ2为0时，位置向量是向Y方向偏移了一定的距离，记y2
则在θ2不为0时，位移为：
R2’[0,y2,0]
③第三关节
在θ3为0时，位置向量是向X，Z方向偏移了一定的距离，记X3，Z3
则在θ2不为0时，位移为：
R3’*[X3，0，Z3]
④第四-六关节
由于四到六关节坐标系建于同一点，则该部分无位置变换，仅进行姿态转换，可见该机器人的位置有前三关节决定。

综上所述，如果设世界坐标的位置为x、y、z, 则已知关节坐标求解世界坐标（不带工具坐标系情况下）的位置公式为：
[X，Y，Z] = R1*[x1,0,0]+R2’[0,y2,0]+R3’[X3，0，Z3]

分析：以上等式中含有三个方程，同时存在三个未知数，可求解。
求解：
matlab代码

%% 位置关系
syms x1 y2 x3 z3 X Y Z
T_16 = R_1*[x1;0;0]+R_12*[0;y2;0]+R_13*[x3;0;z3]
T_6 = [X;Y;Z] ; %世界坐标系位移 等于上述关节坐标系运动值
%解得X Y Z的值
X = T_16(1)
Y = T_16(2)
Z = T_16(3)

结果：
X =
z3*(cos(theta1)*cos(theta2)*sin(theta3) +cos(theta1)*cos(theta3)sin(theta2)) - x3(cos(theta1)sin(theta2)sin(theta3) - cos(theta1)cos(theta2) cos(theta3)) + x1cos(theta1) - y2sin(theta1)

Y =
z3*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)sin(theta2)) - x3(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)cos(theta3)sin(theta1)) + y2cos(theta1) + x1sin(theta1)

Z =
z3*(cos(theta2)cos(theta3) - sin(theta2)sin(theta3)) - x3(cos(theta2) sin(theta3) + cos(theta3)*sin(theta2))

总结

以上，通过角度旋转公式和坐标公式，可以由6关节参数转换为XYZABC坐标值，完成计算。
角度公式
R6‘ = R关z（θ1）*R关y（θ2）R关y（θ3） R关z（θ4）*R关y（θ5）R关z（θ6）= R_ABC = Rotx（A）Roty（B）Rotz（C）
位置公式
[X，Y，Z] = R1[x1,0,0]+R2’[0,y2,0]+R3’[X3，0，Z3]

点的运动坐标为：

%% 最终结果
point = [X,Y,Z,A,B,C]

point =

[ z3*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)sin(theta2)) - x3(cos(theta1)*sin(theta2)sin(theta3) - cos(theta1)cos(theta2)cos(theta3)) + x1cos(theta1) - y2sin(theta1), z3(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)sin(theta2)) - x3(sin(theta1)*sin(theta2)sin(theta3) - cos(theta2)cos(theta3)sin(theta1)) + y2cos(theta1) + x1sin(theta1), z3(cos(theta2)*cos(theta3) - sin(theta2)sin(theta3)) - x3(cos(theta2)*sin(theta3) + cos(theta3)sin(theta2)), -atan((sin(theta5)(cos(theta1)sin(theta4) - cos(theta4)(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)sin(theta1))) + cos(theta5)(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)sin(theta2)))/(cos(theta5)(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) - cos(theta4)sin(theta5)(cos(theta2)*sin(theta3) + cos(theta3)sin(theta2)))), -asin(sin(theta5)(sin(theta1)sin(theta4) + cos(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)cos(theta3))) - cos(theta5)(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)sin(theta2))), -atan((cos(theta6)(cos(theta4)sin(theta1) - sin(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)cos(theta2)cos(theta3))) - sin(theta6)(cos(theta5)(sin(theta1)sin(theta4) + cos(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)cos(theta3))) + sin(theta5)(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)sin(theta2))))/(sin(theta6)(cos(theta4)sin(theta1) - sin(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)cos(theta2)cos(theta3))) + cos(theta6)(cos(theta5)(sin(theta1)sin(theta4) + cos(theta4)(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)cos(theta3))) + sin(theta5)(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)))))]

基于标准DH坐标系的转换思路

DH坐标系参数表

（****本图应该是基于MDH建立坐标系，分析用SDH分析，有点问题，参考思路）

%用于标准DH坐标建模计算
function T = T_SDH(theta,d,a,alpha)
T = Rot('Z',theta)*Trans(0,0,d)*Trans(a,0,0)*Rot('X',alpha)function R = Rotr( axis ,theta )
%为方便调用旋转矩阵，写此 函数
%带比例 4*4 角度表示
switch axiscase 'X'R=[1,0,0,0;0,cosd(theta),-sind(theta),0;0,sind(theta),cosd(theta),0;0,0,0,1];case 'Y'R=[cosd(theta),0,sind(theta),0;0,1,0,0;-sind(theta),0,cosd(theta),0;0,0,0,1];case 'Z'R=[cosd(theta),-sind(theta),0,0;sind(theta),cosd(theta),0,0;0,0,1,0;0,0,0,1];
endfunction T = Trans( u,v,w )
%为方便调用平移矩阵，写此函数T=[1,0,0,u;0,1,0,v;0,0,1,w;0,0,0,1];
end

根据DH旋转公式有:
T_01 =

T_12 =

T_23 =

T_34 =

T_45 =

T_56 =

T_16 = T_01T_12T_23T_34T_45T_56
[ sin((pitheta6)/180)(sin((pitheta4)/180)(cos((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180) - cos((pitheta1)/180)cos((pitheta2)/180)cos((pitheta3)/180)) + cos((pitheta4)/180)sin((pitheta1)/180)) - cos((pitheta6)/180)(cos((pitheta5)/180)(cos((pitheta4)/180)(cos((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180) - cos((pitheta1)/180)cos((pitheta2)/180)cos((pitheta3)/180)) - sin((pitheta1)/180)sin((pitheta4)/180)) + sin((pitheta5)/180)(cos((pitheta1)/180)cos((pitheta2)/180)sin((pitheta3)/180) + cos((pitheta1)/180)cos((pitheta3)/180)sin((pitheta2)/180))), cos((pitheta6)/180)(sin((pitheta4)/180)(cos((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180) - cos((pitheta1)/180)cos((pitheta2)/180)cos((pitheta3)/180)) + cos((pitheta4)/180)sin((pitheta1)/180)) + sin((pitheta6)/180)(cos((pitheta5)/180)(cos((pitheta4)/180)(cos((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180) - cos((pitheta1)/180)cos((pitheta2)/180)cos((pitheta3)/180)) - sin((pitheta1)/180)sin((pitheta4)/180)) + sin((pitheta5)/180)(cos((pitheta1)/180)cos((pitheta2)/180)sin((pitheta3)/180) + cos((pitheta1)/180)cos((pitheta3)/180)sin((pitheta2)/180))), sin((pitheta5)/180)(cos((pitheta4)/180)(cos((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180) - cos((pitheta1)/180)cos((pitheta2)/180)cos((pitheta3)/180)) - sin((pitheta1)/180)sin((pitheta4)/180)) - cos((pitheta5)/180)(cos((pitheta1)/180)cos((pitheta2)/180)sin((pitheta3)/180) + cos((pitheta1)/180)cos((pitheta3)/180)sin((pitheta2)/180)), d3cos((pitheta1)/180)cos((pitheta2)/180) - l2sin((pitheta1)/180) + d4cos((pitheta1)/180)cos((pitheta2)/180)cos((pitheta3)/180) - d4cos((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180)]
[ cos((pitheta6)/180)(cos((pitheta5)/180)(cos((pitheta4)/180)(cos((pitheta2)/180)cos((pitheta3)/180)sin((pitheta1)/180) - sin((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180)) - cos((pitheta1)/180)sin((pitheta4)/180)) - sin((pitheta5)/180)(cos((pitheta2)/180)sin((pitheta1)/180)sin((pitheta3)/180) + cos((pitheta3)/180)sin((pitheta1)/180)sin((pitheta2)/180))) - sin((pitheta6)/180)(cos((pitheta1)/180)cos((pitheta4)/180) + sin((pitheta4)/180)(cos((pitheta2)/180)cos((pitheta3)/180)sin((pitheta1)/180) - sin((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180))), - sin((pitheta6)/180)(cos((pitheta5)/180)(cos((pitheta4)/180)(cos((pitheta2)/180)cos((pitheta3)/180)sin((pitheta1)/180) - sin((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180)) - cos((pitheta1)/180)sin((pitheta4)/180)) - sin((pitheta5)/180)(cos((pitheta2)/180)sin((pitheta1)/180)sin((pitheta3)/180) + cos((pitheta3)/180)sin((pitheta1)/180)sin((pitheta2)/180))) - cos((pitheta6)/180)(cos((pitheta1)/180)cos((pitheta4)/180) + sin((pitheta4)/180)(cos((pitheta2)/180)cos((pitheta3)/180)sin((pitheta1)/180) - sin((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180))), - sin((pitheta5)/180)(cos((pitheta4)/180)(cos((pitheta2)/180)cos((pitheta3)/180)sin((pitheta1)/180) - sin((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180)) - cos((pitheta1)/180)sin((pitheta4)/180)) - cos((pitheta5)/180)(cos((pitheta2)/180)sin((pitheta1)/180)sin((pitheta3)/180) + cos((pitheta3)/180)sin((pitheta1)/180)sin((pitheta2)/180)), l2cos((pitheta1)/180) + d3cos((pitheta2)/180)sin((pitheta1)/180) + d4cos((pitheta2)/180)cos((pitheta3)/180)sin((pitheta1)/180) - d4sin((pitheta1)/180)sin((pitheta2)/180)sin((pitheta3)/180)]
[ sin((pitheta4)/180)sin((pitheta6)/180)(cos((pitheta2)/180)sin((pitheta3)/180) + cos((pitheta3)/180)sin((pitheta2)/180)) - cos((pitheta6)/180)(sin((pitheta5)/180)(cos((pitheta2)/180)cos((pitheta3)/180) - sin((pitheta2)/180)sin((pitheta3)/180)) + cos((pitheta4)/180)cos((pitheta5)/180)(cos((pitheta2)/180)sin((pitheta3)/180) + cos((pitheta3)/180)sin((pitheta2)/180))), sin((pitheta6)/180)(sin((pitheta5)/180)(cos((pitheta2)/180)cos((pitheta3)/180) - sin((pitheta2)/180)sin((pitheta3)/180)) + cos((pitheta4)/180)cos((pitheta5)/180)(cos((pitheta2)/180)sin((pitheta3)/180) + cos((pitheta3)/180)sin((pitheta2)/180))) + cos((pitheta6)/180)sin((pitheta4)/180)(cos((pitheta2)/180)sin((pitheta3)/180) + cos((pitheta3)/180)sin((pitheta2)/180)), cos((pitheta4)/180)sin((pitheta5)/180)(cos((pitheta2)/180)sin((pitheta3)/180) + cos((pitheta3)/180)sin((pitheta2)/180)) - cos((pitheta5)/180)(cos((pitheta2)/180)cos((pitheta3)/180) - sin((pitheta2)/180)sin((pitheta3)/180)), - d3sin((pitheta2)/180) - d4cos((pitheta2)/180)sin((pitheta3)/180) - d4cos((pi*theta3)/180)sin((pitheta2)/180)]
[ 0,0,1]

根据得到的T_16的转换矩阵，取旋转矩阵部分直接转换
即T_DH = T_16(3X3)

%ABC的旋转矩阵为（同上）R_ABC

使T_DH = R_ABC,从而解的ABC的值
令T_16 = r(**),同理：
A = -atan(r23/r33)
B = asin(r13)
C = -atan(r12/r11)

（注意，T_DH的解比较复杂,但T_DH中的θ值默认均为已知值）
具体θ值的求解，是由已知位置姿态逆解获得。

（如果已知了终点的姿态，可直接求得ABC的值，无需通过求出θ再求得ABC，此过程仅仅是为了对不同坐标系下的转换关系进行一个探讨）

关于带刀具的坐标系转换，即在以上的基础上，新增一个工具坐标系的位置及姿态，进行求解，后续进行~

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

浅析相机相关坐标系的相互转换（世界坐标系、相机坐标系、图像坐标系、像素坐标系、内参矩阵、外参矩阵、扭转因子）【相机标定计算机视觉】
最近在网上看到了很多关于坐标系转换的帖子,但是其内容很多都是相互转载(甚至还有一部分是错误的),同时大部分的文章内容都是告诉你四种坐标系间的相互转化的数学公式,看完之后很多时候还是不知所云,本文意在深 ...
医学影像坐标系问题（世界坐标系、解剖坐标系和图像坐标系）
文章目录 2. 坐标系方向orientation(RAI → AIL) 2.1 统一转为RAI方向(itk的python代码) 2.2. 统一转为某个方向(itk的C++代码) 2.3 设置成和另一个 ...
[图形学] 坐标系变换——从世界坐标系到相机坐标系
坐标系变换是图形学中最基础的部分,一个物体从建模到显示在屏幕,经历了从物体坐标系到世界坐标系,再从世界坐标系到观察坐标系,最后通过投影到2D平面,再变换到屏幕坐标等一系列过程. 投影矩阵我们可以很容 ...
地图定位偏移以及坐标系转换（二）-不同坐标系的转换
不同坐标系转换 // 定义一些常量 const x_PI = 3.14159265358979324 * 3000.0 / 180.0 const PI = 3.1415926535897932384 ...
unity 坐标系转换_Unity的几种坐标系及相互转换
介绍 1.WorldSpace(世界坐标系) 当我们向场景中添加物体时,他们都是以世界坐标系的方式的方式显示在场景中的通过transform.position可以得到它在世界中的坐标位置,trans ...
wgs84坐标系转换工具_ArcGIS中不同坐标系之间的转换
用户经常会遇到这样的情况,现有的数据坐标不符合要求,为应对需求,不得不为数据转换坐标系.很多人认为转换坐标系只是单纯的使用工具箱中的[投影]工具,那就大错特错了.有很多情况下,只使用[投影]工具并不能 ...
坐标系转换-大地坐标转笛卡尔坐标系，笛卡尔坐标系转东北天坐标系
参考:https://www.cnblogs.com/charlee44/p/15382659.html #include <iostream> #include <eigen3/E ...
关于世界坐标系，相机坐标系，图像坐标系，像素坐标系的一些理解
关于世界坐标系,相机坐标系,图像坐标系,像素坐标系的一些理解前言一.各坐标系的含义二.坐标系转换 1.世界坐标系与相机坐标系(旋转与平移) 2.相机坐标系与图像坐标系(透视) 3.图像坐标系与像 ...
相机成像---世界坐标系、相机坐标系、图像坐标系和像素坐标系之间的转换关系
新学期第一天开始写的这篇文章,看看我啥时候能把他发出去.假期当然是啥也没干了,之前还信誓旦旦说回家一定能学习,学个毛线.开始学习啦,去年年末把环境配置好了之后,实验发现他不准,用的D435i摄像头是红 ...

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