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

正向運動學：

function [ T06 ] = fkine(JD)

%JD為輸入的1*6的數組，代表6個關節變量

% trans( theta, d, a, alpha )

if nargin<2; end  

T01 =trans( JD(1), 0.0955,  0.007,   pi/2 );

T12 =trans( JD(2)+pi/2, 0,    0.135,   0);

T23 =trans( JD(3)-pi/2, 0,    0.145, 0);

T34 =trans( JD(4), 0,    0.0535, pi/2 );

T45 =trans( JD(5), 0.176,    0, -pi/2 );

T06 =T01*T12*T23*T34*T45;

end

3機器人模型建立

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

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

具體程序編制如下：

Clear %情況matlab軟件的數據緩存，避免影響本次運行

Clc %清空運行窗口的數據

L(1) = Link(  'd',     0.0955,  'a' , 0.007  , 'alpha',   pi/2 ,'offset',0);

L(2) = Link(  'd',     0 ,  'a' ,0.135 , 'alpha',    0, 'offset',pi/2);

L(3) = Link( 'd',  0 ,  'a' , 0.145 , 'alpha',0 ,'offset',-pi/2);

L(4) = Link( 'd', 0 ,  'a' ,0.0535 , 'alpha', pi/2 ,'offset',0);

L(5) = Link( 'd',  0.176 ,  'a' , 0 , 'alpha', -pi/2 ,'offset',0);

robot = SerialLink(L, 'name' , '機械臂');   %建立四自由度模型

% RRP.plotopt = {'workspace',[-10,10,-10,10,-10,10],'tilesize',10};  %設置模型空間大小和地磚大小

robot.teach;       %畫出模型并進行調控

robot.display();  %顯示建立的機器人的DH參數robot.plot([0 0 0 0 0])%輸入一定參數后機器人圖形

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

圖 33機器臂模型

4機器臂正運動驗證

init_ang=[0,-pi/4,0,-pi/5,0,0];%p1起點

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

robot.fkine(init_ang)

%%

%自定義矩陣

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

fkine(init_ang)

robot.plot([0,-pi/4,0,-pi/5,0,0])

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

軌跡規劃程序

L(1) = Link(  'd',     0.0955,  'a' , 0.007  , 'alpha',   pi/2 ,'offset',0);

L(2) = Link(  'd',     0 ,  'a' ,0.135 , 'alpha',    0, 'offset',pi/2);

L(3) = Link( 'd',  0 ,  'a' , 0.145 , 'alpha',0 ,'offset',-pi/2);

L(4) = Link( 'd', 0 ,  'a' ,0.0535 , 'alpha', pi/2 ,'offset',0);

L(5) = Link( 'd',  0.176 ,  'a' , 0 , 'alpha', -pi/2 ,'offset',0);

% L(6) = Link( 'd',  0.134 ,  'a' ,0  , 'alpha', 0 ,'offset',0);

robot = SerialLink(L, 'name' , '機械臂');   %建立四自由度模型

% RRP.plotopt = {'workspace',[-10,10,-10,10,-10,10],'tilesize',10};  %設置模型空間大小和地磚大小

robot.teach;       %畫出模型并進行調控

robot.display();  %顯示建立的機器人的DH參數

% robot.plot([0 0 0 0 0])%輸入一定參數后機器人圖形

%利用五次多項式軌跡規劃

T1=transl(0.3,-0.3,0.5)%起點

q1=robot.ikunc(T1);

T2=transl(-0.3,-0.2,0.4)%終點

q2=robot.ikunc(T2);

[qt1,qt2,qt3]=jtraj(q1,q2,50);

hold on

robot.plot(qt1, 'trail','-b')

qt=robot.fkine(qt1)

T=transl(qt);

plot3(T(:,1),T(:,2),T(:,3) ,'*b');%輸出末端軌跡