1正運動學分析
采用标準的D-h法進行機械腿模型分析：
D-h表如下
[外鍊圖檔轉存失敗,源站可能有防盜鍊機制,建議将圖檔儲存下來直接上傳(img-9C5pdIGG-1666495203285)(data:image/svg+xml;utf8, )]
（2）通過（1）求解出機器人各位姿變換矩陣後，求解機器人手臂變換矩陣
**
[外鍊圖檔轉存失敗,源站可能有防盜鍊機制,建議将圖檔儲存下來直接上傳(img-gtcrHorV-1666495203286)(data:image/svg+xml;utf8, )]
**
。通過matlab 計算，寫出機器人末端位置。
正運動學分析
根據D-H表規定得到如下變換矩陣為：
[外鍊圖檔轉存失敗,源站可能有防盜鍊機制,建議将圖檔儲存下來直接上傳(img-8wbryKKc-1666495203286)(data:image/svg+xml;utf8, )]
由此可得機器人相鄰兩關節位姿分别為：
[外鍊圖檔轉存失敗,源站可能有防盜鍊機制,建議将圖檔儲存下來直接上傳(img-qzOOGY6b-1666495203287)(data:image/svg+xml;utf8, )]
[外鍊圖檔轉存失敗,源站可能有防盜鍊機制,建議将圖檔儲存下來直接上傳(img-uzcd9BIR-1666495203287)(data:image/svg+xml;utf8, )]
[外鍊圖檔轉存失敗,源站可能有防盜鍊機制,建議将圖檔儲存下來直接上傳(img-HcWaWcVY-1666495203288)(data:image/svg+xml;utf8, )]
[外鍊圖檔轉存失敗,源站可能有防盜鍊機制,建議将圖檔儲存下來直接上傳(img-a9hnQQRh-1666495203289)(data:image/svg+xml;utf8, )]
[外鍊圖檔轉存失敗,源站可能有防盜鍊機制,建議将圖檔儲存下來直接上傳(img-iQf32tXc-1666495203289)(data:image/svg+xml;utf8, )]
是以，坐标系{4}相對于基坐标系的變換矩陣為：
[外鍊圖檔轉存失敗,源站可能有防盜鍊機制,建議将圖檔儲存下來直接上傳(img-KOpfa58O-1666495203290)(data:image/svg+xml;utf8, )]
相對于基坐标系的旋轉矩陣
[外鍊圖檔轉存失敗,源站可能有防盜鍊機制,建議将圖檔儲存下來直接上傳(img-Aps7PDrH-1666495203290)(data:image/svg+xml;utf8, )]
位置矢量
[外鍊圖檔轉存失敗,源站可能有防盜鍊機制,建議将圖檔儲存下來直接上傳(img-7D25udss-1666495203291)(data:image/svg+xml;utf8, )]
根據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
圖 3-3機器臂模型
4機器臂正運動驗證
init_ang=[0,300,300,0 0]; %任意起點
disp(‘工具箱求解得到旋轉矩陣如下：’)
robot.fkine(init_ang)
%%
%自定義矩陣
disp(‘通過變換矩陣求解旋轉矩陣如下：’)
fkine(init_ang)
如圖所示：任意選取某點工具箱仿真姿态結果與變換矩陣計算結果一緻。驗證了正運動學方程的正确性。
需要完整代碼可以轉此連結中https://www.jishulink.com/content/post/1887203
或者加扣扣2386317960
https://item.taobao.com/item.htm?spm=a1z10.1-c-s.w4004-22529865413.4.75e351a02MUWu0&id=640920926591