(機器人技術基礎實驗matlab DH修正引數求旋轉矩陣描述和計算X-Y-Z固定角描述)

simulink效果圖：
)

DH引數到旋轉矩陣程式碼

function R8 = fac(si)
i=1;
 R8=eye(4,4);
 DH=[0,0,0,si(1)*pi/180;pi/2,0,0.2155,si(2)*pi/180;pi,0.408,0.2155,si(3)*pi/180;pi,0.376,0.1215,si(4)*pi/180;-pi/2,0,0.103,si(5)*pi/180;pi/2,0,0,si(6)*pi/180];
while(i<7)  
   
       R(1,1)=cos(DH(i,4
 
));
       R(1,2)=-sin(DH(i,4));
       R(1,3)=0;
       R(1,4)=DH(i,2);
       R(2,1)=sin(DH(i,4))*cos(DH(i,1));
       R(2,2)=cos(DH(i,4))*cos(DH(i,1));
       R(2,3)=-sin(DH(i,1));
       R(2,4)=-sin(DH(i,1))*DH(i,3);
       R(3,1)=sin(DH(i,4))*sin(DH(i,1));
       R(3,2)=cos(DH(i,4))*sin(DH(i,1))
 
;
       R(3,3)=cos(DH(i,1));
       R(3,4)=cos(DH(i,1))*DH(i,3);
       R(4,1)=0;
       R(4,2)=0;
       R(4,3)=0;
       R(4,4)=1;
       R
       R8=R8*R 
    i=i+1
end
R8

旋轉矩陣到X-Y-Z固定角程式碼

function u=routh_xyz(R)
u=zeros(3,1);
r11=R(1,1);
r12=R(1,2);
r13=R(1,3);
r21=R(2,1);
r22=R(2,2);
r23=R(2
 
,3);
r31=R(3,1);
r32=R(3,2);
r33=R(3,3);
u(2)=atan2(-r31,sqrt(r11^2+r21^2));
if cos(u(2))~=0
u(3)=atan2(r21/cos(u(2)),r11/cos(u(2)))*180/pi;
u(1)=atan2(r32/cos(u(2)),r33/cos(u(2)))*180/pi;
u(2)=u(2)*180/pi;
else
    u(1)=0;
    u(2)=90;
    u(3)=atan2(r12,r22)*180/pi;
end
end