下面在前面的ur5機械臂的DH參數(shù)基礎(chǔ)是對其正逆解進行求解，為了后面能在MATLAB中利用stl文件進行實際顯示，這里以標準DH參數(shù)為例進行講解。（修正DH參數(shù)在用plot3d函數(shù)是顯示失敗，不知道是不是這個函數(shù)只能顯示標準dh參數(shù)的機械臂模型，有知道的網(wǎng)友可以在評論里告知一下，謝謝。）

一、運動學正解：

機器人正運動學是在已知各連桿相對位置關(guān)系（關(guān)節(jié)角）的情況下，得到末端執(zhí)行器的位姿。在標準DH參數(shù)下相鄰坐標系之間的齊次變換矩陣為：

?則，正解代碼如下：

function    [T06,Pos]=ForwardSolver_MDH(theta)

    DH_JXB =[90    0      144  0; 
             0     264    0    90;
             0     236    0    0; 
             -90   0      106  -90; 
             90    0      114  0; 
             0     0      67   0];
    d=DH_JXB(1:6,3); 
    a=DH_JXB(1:6,2); 
    DH_JXB(1:6,1)=DH_JXB(1:6,1)/180*pi; %度數(shù)轉(zhuǎn)化為弧度
    alp=DH_JXB(1:6,1); 
    offset=[0 90 0 -90 0 0];
    theta=(theta+offset)*pi/180;

for i=1:6
        T{i}=[     cos(theta(i)),       -sin(theta(i))*cos(alp(i)),                  sin(theta(i))*sin(alp(i)),     a(i)*cos(theta(i));
                   sin(theta(i)),       cos(theta(i))*cos(alp(i)),       
-cos(theta(i))*sin(alp(i)),    a(i)*sin(theta(i));
0,             sin(alp(i)),                      cos(alp(i)),                  d(i);
0,                   0,                               0,                          1
             ]

    end

    disp('Homogeneous transformation matrix T06:')
    T06=T{1}*T{2}*T{3}*T{4}*T{5}*T{6}
    %% 求末端位置
    X=T06(1,4);Y=T06(2,4);Z=T06(3,4);
    %% 求末端姿態(tài)Rotations about X, Y, Z axes (for a robot gripper)
    R=T06;
    if abs(abs(R(1,3)) - 1) < eps  % when |R13| == 1
        % singularity
        rpy(1) = 0;  % roll is zero
        if R(1,3) > 0
        rpy(3) = atan2( R(3,2), R(2,2));   % R+Y
        else
             rpy(3) = -atan2( R(2,1), R(3,1));   % R-Y
        end
        rpy(2) = asin(R(1,3));
    else
        rpy(1) = -atan2(R(1,2), R(1,1));
        rpy(3) = -atan2(R(2,3), R(3,3));

        rpy(2) = atan(R(1,3)*cos(rpy(1))/R(1,1));
    end
    RPY=rpy*180/pi;
    Rall=RPY(1);Pitch=RPY(2);Yaw=RPY(3);
    Pos=[X,Y,Z,Rall,Pitch,Yaw];
end

這里對姿態(tài)的描述進行說明：在MATLAB中RPY歐拉角是世界坐標系下的XYZ歐拉角；

RPY角的定義如下：

?輸入端位姿形式為：

MATLAB中對應的RPY旋轉(zhuǎn)矩陣如下：

?這是個旋轉(zhuǎn)矩陣，與齊次矩陣中的旋轉(zhuǎn)矩陣等價，所以根據(jù)齊次矩陣中的旋轉(zhuǎn)矩陣便可以得到末端的姿態(tài)RPY。

二、運動學逆解：

機器人逆運動學是已知機器人末端執(zhí)行器的位姿，通過變換矩陣T得到機器人各關(guān)節(jié)的角度。求解逆運動學有解析法、幾何法、迭代法。這里介紹解析法。如果機器人末端三軸的軸線始終交于一點則該機器人必有解析解。

1、得到齊次變換矩陣：

利用MATLAB求解其齊次變換矩陣：文章來源地址http://www.zghlxwxcb.cn/news/detail-406542.html

syms a1 a2 a3 a4 a5 a6 d1 d2 d3 d4 d5 d6 a1p1 a1p2 a1p3 a1p4 a1p5 a1p6 th1 th2 th3 th4 th5 th6;
a=[0 a2 a3 0 0 0];
d=[d1 0 0 d4 d5 d6];
alp=[90 0 0 -90 90 0]*pi/180;
theta=[th1 th2 th3 th4 th5 th6];
% theta(1)=t1;theta(2)=t2;theta(3)=t3;theta(4)=t4;theta(5)=t5;theta(6)=t6;

 T01=[         cos(theta(1)),       0,      sin(theta(1)),     a(1)*cos(theta(1));
               sin(theta(1)),       0,      -cos(theta(1)),    a(1)*sin(theta(1));
               0,                   1,      0,                 d(1);
               0,                   0,      0,                 1
         ];

 T12=[         cos(theta(2)),       -sin(theta(2)),      0,     a(2)*cos(theta(2));
               sin(theta(2)),       cos(theta(2)),       0,     a(2)*sin(theta(2));
               0,

到了這里，關(guān)于（6）六軸機械臂的運動學正、逆解的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！