1正運動學分析

采用標準的D-h法進行機械腿模型分析：

D-h表如下

（2）通過（1）求解出機器人各位姿變換矩陣后，求解機器人手臂變換矩陣。通過matlab 計算，寫出機器人末端位置。

正運動學分析

根據D-H表規定得到如下變換矩陣為：

由此可得機器人相鄰兩關節位姿分別為：

所以，坐標系{4}相對于基坐標系的變換矩陣為：

相對于基坐標系的旋轉矩陣

                              

位置矢量

根據DH參數求解變換矩陣的函數trans：

%輸入JD，即6個關節變量的值，求解正運動方程

function [ T ] = trans( theta, d, a, alpha )

T =[

     cos(theta),   -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);

     sin(theta),   cos(theta)*cos(alpha),  -cos(theta)*sin(alpha), a*sin(theta);

     0,           sin(alpha),              cos(alpha),            d;

     0,                      0,                      0,           1            ];

end

3機器人模型建立

所設計的機器人由多個連桿機構組成，其關節類型包括旋轉關節和移動關節兩種。利用Matlab中機器人仿真工具箱Robotics Tool中Link和SerialLink兩個函數可建立機器人模型[] 。其中，函數表達式如下：

式中，theta為關節變量；d為偏置距離；alpha為扭轉角；a為連桿長度；sigma表示關節類型（0為旋轉關節，1為移動關節）；前四個參數分別對應表1中的，，，。

具體程序編制如下：

% DH parameters  theta     d        a    alpha  qlim    

L(1) = Link('d', 126, 'a',0, 'alpha', 0,'qlim',[0,pi/2]);L(1).offset=-pi/2;%定義連桿

L(2) = Link('theta', 0, 'a', 0, 'alpha', -pi/2,'qlim',[0,720]);L(2).offset=300;

L(3) = Link('theta', 0, 'a', 0, 'alpha',  0,'qlim',[127.5,707.85]);L(3).offset=300;

L(4) = Link('d', 242, 'a', 0, 'alpha',-pi/2,'qlim',[-pi,pi]);L(4).offset=pi/2;

L(5) = Link('d', 0, 'a', 126.97, 'alpha',0,'qlim',[-pi,pi]);L(5).offset=-pi/2;

robot = SerialLink(L,'name' , '機器人'); %定義機器人模型函數

robot.display() %顯示D-H表

h=1000;%工作空間參數

robot.plotopt = {'workspace',[-h,h,-h,h,-h,h],'tilesize',h}; %設置工作空間

robot.teach()%顯示機器人模型

運行上述程序，即可得到碼垛機器人模型如圖 3-3

圖 33機器臂模型

4機器臂正運動驗證

init_ang=[0,300,300,0 0]; %任意起點

disp('工具箱求解得到旋轉矩陣如下：')

robot.fkine(init_ang)

%%

%自定義矩陣

disp('通過變換矩陣求解旋轉矩陣如下：')

fkine(init_ang)

如圖所示：任意選取某點工具箱仿真姿態結果與變換矩陣計算結果一致。驗證了正運動學方程的正確性。

需要完整代碼看付費內容或者加扣扣2386317960