连杆变换矩阵
概括起来,求正解即是求坐标系{6}相对于坐标系{0}的变换,这个问题可以分解成6个子问题,即确定6个坐标系{i}相对于坐标系{i-1}的表示,每个变换过程又包含4个子过程,即绕X(i-1)轴旋转alpha(i-1),沿X(i-1)轴平移a(i-1),绕Z(i)轴旋转theta(i),沿Z(i)轴平移d(i);
我们可以得到这个变换为:
将旋转算子和平移算子带入上式,即可得到相邻连杆变换矩阵的一般形式:
求解目标连杆坐标系相对于某坐标系的表示,即从某连杆开始的转换矩阵一直右乘相邻变换矩阵,直到目标矩阵为止;比如求坐标系{N}相对于极坐标系{0}的表示:
Matlab实现
以上一篇文章中提到的MDH参数表为例,matlab实现如下:
function [ T06 ] = fkine(theta1, theta2, theta3, theta4, theta5, theta6) L1=160; L2=575; L3=130; L4=645; % theta d a alpha DH=[ theta1 0 0 0; theta2 0 L1 pi/2; theta3 0 L2 0; theta4 L4 L3 pi/2; theta5 0 0 -pi/2; theta6 0 0 pi/2]; T01=[cos(DH(1,1)) -sin(DH(1,1)) 0 DH(1,3); cos(DH(1,4))*sin(DH(1,1)) cos(DH(1,4))*cos(DH(1,1)) -sin(DH(1,4)) -DH(1,2)*sin(DH(1,4)); sin(DH(1,4))*sin(DH(1,1)) sin(DH(1,4))*cos(DH(1,1)) cos(DH(1,4)) DH(1,2)*cos(DH(1,4)); 0 0 0 1]; T12=[cos(DH(2,1)) -sin(DH(2,1)) 0 DH(2,3); cos(DH(2,4))*sin(DH(2,1)) cos(DH(2,4))*cos(DH(2,1)) -sin(DH(2,4)) -DH(2,2)*sin(DH(2,4)); sin(DH(2,4))*sin(DH(2,1)) sin(DH(2,4))*cos(DH(2,1)) cos(DH(2,4)) DH(2,2)*cos(DH(2,4)); 0 0 0 1]; T23=[cos(DH(3,1)) -sin(DH(3,1)) 0 DH(3,3); cos(DH(3,4))*sin(DH(3,1)) cos(DH(3,4))*cos(DH(3,1)) -sin(DH(3,4)) -DH(3,2)*sin(DH(3,4)); sin(DH(3,4))*sin(DH(3,1)) sin(DH(3,4))*cos(DH(3,1)) cos(DH(3,4)) DH(3,2)*cos(DH(3,4)); 0 0 0 1]; T34=[cos(DH(4,1)) -sin(DH(4,1)) 0 DH(4,3); cos(DH(4,4))*sin(DH(4,1)) cos(DH(4,4))*cos(DH(4,1)) -sin(DH(4,4)) -DH(4,2)*sin(DH(4,4)); sin(DH(4,4))*sin(DH(4,1)) sin(DH(4,4))*cos(DH(4,1)) cos(DH(4,4)) DH(4,2)*cos(DH(4,4)); 0 0 0 1]; T45=[cos(DH(5,1)) -sin(DH(5,1)) 0 DH(5,3); cos(DH(5,4))*sin(DH(5,1)) cos(DH(5,4))*cos(DH(5,1)) -sin(DH(5,4)) -DH(5,2)*sin(DH(5,4)); sin(DH(5,4))*sin(DH(5,1)) sin(DH(5,4))*cos(DH(5,1)) cos(DH(5,4)) DH(5,2)*cos(DH(5,4)); 0 0 0 1]; T56=[cos(DH(6,1)) -sin(DH(6,1)) 0 DH(6,3); cos(DH(6,4))*sin(DH(6,1)) cos(DH(6,4))*cos(DH(6,1)) -sin(DH(6,4)) -DH(6,2)*sin(DH(6,4)); sin(DH(6,4))*sin(DH(6,1)) sin(DH(6,4))*cos(DH(6,1)) cos(DH(6,4)) DH(6,2)*cos(DH(6,4)); 0 0 0 1]; T06=T01*T12*T23*T34*T45*T56; end