????????機械臂的正運動學求解即建立DH參數(shù)表，然后計算出各變換矩陣以及最終的變換矩陣。逆運動學求解，即求出機械臂各關(guān)節(jié)θ角與px,py,pz的關(guān)系，建立θ角與末端姿態(tài)之間的數(shù)學模型，在這里以IRB6700為例，對IRB6700進行正逆運動學求解和驗證。
目錄
正運動學求解
逆運動學求解
正逆運動學模型的驗證
正運動學驗證
逆運動學驗證
總的Matlab代碼，包含正逆運動學求解和驗證
參考文獻
正運動學求解
??????? 首先使用DH法建立坐標系如下：
???????? 查閱IRB6700的參數(shù)如下表
| 連桿i | /mm |
/° | /mm | /° | 關(guān)節(jié)角范圍/° |
|---|---|---|---|---|---|
| 1 | 0 | 0 | 780 | +170 ——? (-170) | |
| 2 | 320 | -90 | 0 | +85 —— (-65) | |
| 3 | 1125 | 0 | 0 | +70 —— (-180) | |
| 4 | 200 | -90 | 1142.5 | +300 —— (-300) | |
| 5 | 0 | 90 | 0 | +130 —— (-130) | |
| 6 | 0 | -90 | 200 | +360 —— (-360) |
根據(jù)變換矩陣公式編寫matlab代碼計算得到正運動學的各變換矩陣
上式中的為,為，為
?計算變換矩陣的MATLAB代碼如下
clear;clc
%%
%導入?yún)?shù)
%注，這里的a,afa的下標在實際使用時均需要減去1
syms a afa d theta [1 6]
a = [0 a2 a3 a4 0 0];
afa = [0 -90 0 -90 90 -90];
d = [d1 0 0 d4 0 d6];
theta = [theta1 theta2 theta3 theta4 theta5 theta6];
syms T [4 4 6]
%%
%正運動學模型求解
%計算各變換矩陣及總的變換矩陣
for i = 1:6
T(:,:,i) = trans_cal( afa(i), a(i), d(i), theta(i)*180/pi );
if(i == 1)
trans_matrix = T(:,:,i);
else
trans_matrix = trans_matrix*T(:,:,i);
end
end
?用到的funcion函數(shù)如下
function T = trans_cal(afa_ii,a_ii,d_i,theta_i)
%%
%計算變換矩陣函數(shù)T_{i-1,i}
%輸入的參數(shù)為，afa_{i-1},a_{i-1},d_i,theta_i,與DH表達法的參數(shù)表對應(yīng)
%ii 為i-1
%注意，這里輸入的角度，均采用角度制，不采用弧度制
T = [cosd(theta_i) -sind(theta_i) 0 a_ii
sind(theta_i)*cosd(afa_ii) cosd(theta_i)*cosd(afa_ii) -sind(afa_ii) -sind(afa_ii)*d_i
sind(theta_i)*sind(afa_ii) cosd(theta_i)*sind(afa_ii) cosd(afa_ii) cosd(afa_ii)*d_i
0 0 0 1 ];
end?各個變換矩陣存儲在T中，其中即為變換矩陣,存儲在trans_matrix中。
逆運動學求解
設(shè)總的變換矩陣設(shè)為下數(shù)：
有等式
根據(jù)上面的正運動學求解可以得到矩陣中的等各元素的值分別為什么
使用matlab的實時計算腳本可以較具體地看到
?然后通過封閉解法求逆，將上述等式中右邊地變換矩陣乘到左邊，然后觀察等式兩邊矩陣中地元素，使等式兩邊的矩陣中的某個元素相等，不斷分離變量，然后求解得到角
?首先求解，構(gòu)造等式
?
?觀察等式，使等式兩邊矩陣的第三行第四列元素相等
上述等式只含有一個未知數(shù)，一個等式一個未知數(shù)，求解過程如下：
注：為了便于計算，均用代替，也用代替，即下列式子中的實際上為，其他的和亦然如此
?用同樣的方式求解
?令等式左右兩邊的第一行第四列，和第三行第四列分別相等，求解過程如下
?
然后是
?令等式兩邊第一行第四列，第二行第四列相等
求解過程如下：
?再然后是的求解
令左右兩邊第三行第三列相等
求解過程如下
?最后是和的求解
對 構(gòu)造
，使等式左右第一行第三列，和第三行第三列相等
對構(gòu)造，使等式左右第三行第一列，和第三行第二列相等
?求解過程如下
?至此完成了所有角的求解
?再次注名：上述推導過程中，為了便于計算，均用代替，也用代替，即上述式子中的實際上為，其他的和亦然如此
正逆運動學模型的驗證
正運動學驗證
隨機選定一組關(guān)節(jié)角進行驗證，
| 關(guān)節(jié)角 | ||||||
| 度數(shù) | 22.12 | -81.4 | 21.25 | -84 | 19.14 | 275.13 |
使用之前求解的正運動學模型求解得到末端姿態(tài)矩陣如下
使用RobotStudio軟件導入IRB6700機器人，輸入選擇的角
注意：由連桿參數(shù)可知，RobotStudio軟件中的角度為原角度加上，為原角度減去，所以RobotStudio中輸入的角度應(yīng)如下：
| 關(guān)節(jié)角 | ||||||
| 度數(shù) | 22.12 | 8.6 | 21.25 | -84 | 19.14 | 95.13 |
?
得到的TCP末端xyz坐標???????
?
對比RobotStudio軟件中的末端姿態(tài)[x,y,z]值與正解求得的矩陣中末端姿態(tài)左邊[px,py,pz]可得，兩者相等，因此正運動學模型正確。
逆運動學驗證
由上述的正運動學驗證繼續(xù)計算，上述正運動學得到的末端姿態(tài)矩陣如下：
根據(jù)此末端姿態(tài)矩陣對逆運動學進行求解，可得到八種不同的角組合，使末端姿態(tài)為[1635.672,594.531,1396.902]
求解的matlab代碼較長，共一個主m文件，以及5個function文件，放在本文最后。
求解得到共八種組合的角結(jié)果，根據(jù)IRB6700機器人的各關(guān)節(jié)角工作范圍減去4個不符合的解后，得到四個符合的角組合，然后保持RobotStudio軟件中的角參數(shù)不變，調(diào)整RobotStudio軟件中的軸配置參數(shù)，將逆運動學模型求解得到的結(jié)果與RobotStudio軟件中的結(jié)果進行對比如下表
| 1 | 2 | 3 | 4 | 5 | 6 | |
| 逆解1 | -157.88 | -41.51 | -134.76 | -30.62 | -39.82 | -144.08 |
| 逆解2 | -157.88 | -41.51 | -134.76 | 149.38 | 39.82 | 35.92 |
| 逆解3 | 22.12 | 8.6 | 21.25 | 96 | -19.14 | -84.87 |
| 逆解4 | 22.12 | 8.6 | 21.25 | -84 | 19.14 | 95.13 |
| 軸配置 （-2,-1,-2,-7） |
-157.88 | -41.51 | -134.76 | -30.62 | -39.82 | -144.07 |
| 軸配置 （-2,1,0,6） |
-157.88 | -41.51 | -134.76 | 149.38 | 39.82 | 35.93 |
| 軸配置 （0,1,-1,1） |
22.12 | 8.6 | 21.25 | 96.01 | -19.14 | -84.87 |
| 軸配置 （0,-1,1,0） |
22.12 | 8.6 | 21.25 | -84 | 19.14 | 95.13 |
由上表的結(jié)果對比可知，除了計算精度問題導致的，部分結(jié)果小數(shù)點后兩位與RobotStudio存在一個單位的差異外，結(jié)果一致，因為可以驗證逆運動學模型正確。
總的Matlab代碼，包含正逆運動學求解和驗證
clear;clc
%%
%導入?yún)?shù)
%注，這里的a,afa的下標在實際使用時均需要減去1
syms a afa d theta [1 6]
a = [0 a2 a3 a4 0 0];
afa = [0 -90 0 -90 90 -90];
d = [d1 0 0 d4 0 d6];
theta = [theta1 theta2 theta3 theta4 theta5 theta6];
syms T [4 4 6]
%%
%正運動學模型求解
%計算各變換矩陣及總的變換矩陣
for i = 1:6
T(:,:,i) = trans_cal( afa(i), a(i), d(i), theta(i)*180/pi );
if(i == 1)
trans_matrix = T(:,:,i);
else
trans_matrix = trans_matrix*T(:,:,i);
end
end
%%
%逆運動學計算求解
syms r [3 3]
syms PX_X PY_Y PZ_Z
T_60 = [r1_1 r1_2 r1_3 PX_X;
r2_1 r2_2 r2_3 PY_Y;
r3_1 r3_2 r3_3 PZ_Z;
0 0 0 1];
left_1 = simplify( inv( T(:,:,2) ) * inv( T(:,:,1) ) * T_60 * inv( T(:,:,6) ) );
right_1 = simplify( T(:,:,3)*T(:,:,4)*T(:,:,5) );
left_2 = simplify( inv( T(:,:,1) )* T_60 * inv( T(:,:,6) ) );
right_2 = simplify( T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5) );
left_3 = simplify( inv( T(:,:,1)*T(:,:,2) ) * T_60 * inv( T(:,:,6) ) );
right_3 = simplify( T(:,:,3)*T(:,:,4)*T(:,:,5) );
left_4 = simplify( inv( T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5) ) * T_60 );
right_4 = simplify( T(:,:,6) );
left_5 = simplify( inv( T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4) ) * T_60 );
right_5 = simplify( T(:,:,5)*T(:,:,6) );
left_6 = simplify( inv( T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5) ) * T_60 );
right_6 = simplify( T(:,:,6) );
%%
%逆運動學驗算
%末端姿態(tài)
T_ni = [-0.5 0 0.866 1635.672;
0 1 0 594.531;
-0.866 0 -0.5 1396.902;
0 0 0 1];
r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X = T_ni(1,4);
r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y = T_ni(2,4);
r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z = T_ni(3,4);
syms a d [1 6]
d1 = 780;d4 = 1142.5;d6 = 200;
a2 = 320;a3 = 1125 ;a4 = 200;
syms A B C D E F [1 6]
%theta1的解
theta1_1 = ( atan2(0,1) - atan2( d6*r2_3-PY_Y , PX_X-d6*r1_3 ) )*180/pi %22.1230
theta1_2 = ( -atan2(0,-1) - atan2( d6*r2_3-PY_Y , PX_X-d6*r1_3 ) )*180/pi %-157.8770
%theta2的解
[theta2_1 , theta2_2] = theta2_calculate(theta1_1) %theta1為22.123°時
[theta2_3 , theta2_4] = theta2_calculate(theta1_2) %theta1為-157.8770°時
%theta3的解
theta3_11_21 = theta3_calculate(theta1_1,theta2_1) %theta1為22.123°theta2為-81.3955°時
theta3_11_22 = theta3_calculate(theta1_1,theta2_2) %theta1為22.123°theta2為22.0674°時
theta3_12_23 = theta3_calculate(theta1_2,theta2_3) %theta1為-157.8770°theta2為172.8834°時
theta3_12_24 = theta3_calculate(theta1_2,theta2_4) %theta1為-157.8770°theta2為228.4872°時
%theta4的解
[theta4_1_11_21,theta4_2_11_21] = theta4_calculate(theta1_1,theta2_1,theta3_11_21) %theta1為22.123°theta2為-81.3955°時
[theta4_1_11_22,theta4_2_11_22] = theta4_calculate(theta1_1,theta2_2,theta3_11_22) %theta1為22.123°theta2為22.0674°時
[theta4_1_12_23,theta4_2_12_23] = theta4_calculate(theta1_2,theta2_3,theta3_12_23) %theta1為-157.8770°theta2為172.8834°時
[theta4_1_12_24,theta4_2_12_24] = theta4_calculate(theta1_2,theta2_4,theta3_12_24) %theta1為-157.8770°theta2為228.4872°時
%theta5的解
theta5_11_21_41 = theta5_calculate(theta1_1,theta2_1,theta3_11_21,theta4_1_11_21) %theta1為22.123°theta2為-81.3955°,theta4為第1種
theta5_11_21_42 = theta5_calculate(theta1_1,theta2_1,theta3_11_21,theta4_2_11_21) %theta1為22.123°theta2為-81.3955°,theta4為第2種
theta5_11_22_41 = theta5_calculate(theta1_1,theta2_2,theta3_11_22,theta4_1_11_22) %theta1為22.123°theta2為22.0674°,theta4為第1種
theta5_11_22_42 = theta5_calculate(theta1_1,theta2_2,theta3_11_22,theta4_2_11_22) %theta1為22.123°theta2為22.0674°,theta4為第2種
theta5_12_23_41 = theta5_calculate(theta1_2,theta2_3,theta3_12_23,theta4_1_12_23) %theta1為-157.8770°theta2為172.8834°,theta4為第1種
theta5_12_23_42 = theta5_calculate(theta1_2,theta2_3,theta3_12_23,theta4_2_12_23) %theta1為-157.8770°theta2為172.8834°,theta4為第2種
theta5_12_24_41 = theta5_calculate(theta1_2,theta2_4,theta3_12_24,theta4_1_12_24) %theta1為-157.8770°theta2為228.4872°,theta4為第1種
theta5_12_24_42 = theta5_calculate(theta1_2,theta2_4,theta3_12_24,theta4_2_12_24) %theta1為-157.8770°theta2為228.4872°,theta4為第2種
% theta6的解
theta6_11_21_41 = theta6_calculate(theta1_1,theta2_1,theta3_11_21,theta4_1_11_21) %theta1為22.123°theta2為-81.3955°,theta4為第1種
theta6_11_21_42 = theta6_calculate(theta1_1,theta2_1,theta3_11_21,theta4_2_11_21) %theta1為22.123°theta2為-81.3955°,theta4為第2種
theta6_11_22_41 = theta6_calculate(theta1_1,theta2_2,theta3_11_22,theta4_1_11_22) %theta1為22.123°theta2為22.0674°,theta4為第1種
theta6_11_22_42 = theta6_calculate(theta1_1,theta2_2,theta3_11_22,theta4_2_11_22) %theta1為22.123°theta2為22.0674°,theta4為第2種
theta6_12_23_41 = theta6_calculate(theta1_2,theta2_3,theta3_12_23,theta4_1_12_23) %theta1為-157.8770°theta2為172.8834°,theta4為第1種
theta6_12_23_42 = theta6_calculate(theta1_2,theta2_3,theta3_12_23,theta4_2_12_23) %theta1為-157.8770°theta2為172.8834°,theta4為第2種
theta6_12_24_41 = theta6_calculate(theta1_2,theta2_4,theta3_12_24,theta4_1_12_24) %theta1為-157.8770°theta2為228.4872°,theta4為第1種
theta6_12_24_42 = theta6_calculate(theta1_2,theta2_4,theta3_12_24,theta4_2_12_24) %theta1為-157.8770°theta2為228.4872°,theta4為第2種
function [theta2_1,theta2_2] = theta2_calculate(theta1)
%%
%導入?yún)?shù)
syms r [3,3]
syms a afa d
syms PX_X PY_Y_Y PZ_Z
T_ni = [-0.5 0 0.866 1635.672;
0 1 0 594.531;
-0.866 0 -0.5 1396.902;
0 0 0 1];
r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X = T_ni(1,4);
r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y = T_ni(2,4);
r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z = T_ni(3,4);
syms a d [1 6]
d1 = 780;d4 = 1142.5;d6 = 200;
a2 = 320;a3 = 1125 ;a4 = 200;
syms A B C D E F [1 6]
theta1 = theta1/180*pi;
%%
%theta2計算
A2 = ( PX_X - d6*r1_3 )*cos(theta1) + ( PY_Y - d6*r2_3 )*sin(theta1) - a2;
B2 = d1 + d6*r3_3 - PZ_Z;
C2 = 2*A2*a3;
D2 = 2*B2*a3;
E2 = A2^2 + B2^2 + a3^2 - a4^2 - d4^2;
F2 = sqrt( C2^2 + D2^2 );
theta2_1 = ( -atan2(C2,D2) + atan2( E2/F2 , sqrt( 1- (E2/F2)^2 ) ) )*180/pi ;
theta2_2 = ( -atan2(C2,D2) + atan2( E2/F2 , -sqrt( 1- (E2/F2)^2 ) ) )*180/pi ;
endfunction theta3 = theta3_calculate(theta1,theta2)
%%
%導入?yún)?shù)
syms r [3,3]
syms a afa d
syms PX_X PY_Y_Y PZ_Z
T_ni = [-0.5 0 0.866 1635.672;
0 1 0 594.531;
-0.866 0 -0.5 1396.902;
0 0 0 1];
r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X = T_ni(1,4);
r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y = T_ni(2,4);
r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z = T_ni(3,4);
syms a d [1 6]
d1 = 780;d4 = 1142.5;d6 = 200;
a2 = 320;a3 = 1125 ;a4 = 200;
syms A B C D E F [1 6]
theta1 = theta1/180*pi;
theta2 = theta2/180*pi;
%%
%theta3計算
A3 = d1*sin(theta2) - a2*cos(theta2) - PZ_Z*sin(theta2) + d6*r3_3*sin(theta2) + PX_X*cos(theta1)*cos(theta2) ...
+ PY_Y*cos(theta2)*sin(theta1) - d6*r1_3*cos(theta1)*cos(theta2) - d6*r2_3*cos(theta2)*sin(theta1) - a3;
B3 = d1*cos(theta2) - PZ_Z*cos(theta2) + a2*sin(theta2) + d6*r3_3*cos(theta2) - PX_X*cos(theta1)*sin(theta2) ...
- PY_Y*sin(theta1)*sin(theta2) + d6*r2_3*sin(theta1)*sin(theta2) + d6*r1_3*cos(theta1)*sin(theta2);
C3 = ( a4*B3-d4*A3 ) / ( a4^2 + d4^2 );
D3 = ( a4*A3+d4*B3 ) / ( a4^2 + d4^2 );
theta3 = atan2( C3,D3 )*180/pi;
endfunction [theta4_1,theta4_2] = theta4_calculate(theta1,theta2,theta3)
%%
%導入?yún)?shù)
syms r [3,3]
syms a afa d
syms PX_X PY_Y_Y PZ_Z
T_ni = [-0.5 0 0.866 1635.672;
0 1 0 594.531;
-0.866 0 -0.5 1396.902;
0 0 0 1];
r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X = T_ni(1,4);
r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y = T_ni(2,4);
r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z = T_ni(3,4);
syms a d [1 6]
d1 = 780;d4 = 1142.5;d6 = 200;
a2 = 320;a3 = 1125 ;a4 = 200;
syms A B C D E F [1 6]
theta1 = theta1/180*pi;
theta2 = theta2/180*pi;
theta3 = theta3/180*pi;
%%
%theta4計算
A4 = r3_3*cos(theta2)*sin(theta3) + r3_3*cos(theta3)*sin(theta2) - r1_3*cos(theta1)*cos(theta2)*cos(theta3) ...
- r2_3*cos(theta2)*cos(theta3)*sin(theta1) + r1_3*cos(theta1)*sin(theta2)*sin(theta3) ...
+ r2_3*sin(theta1)* sin(theta2)*sin(theta3);
B4 = r2_3*cos(theta1) - r1_3*sin(theta1);
theta4_1 = ( atan2( 0,1 ) + atan2( B4,A4 ) )*180/pi;
theta4_2 = ( atan2( 0,-1 ) + atan2( B4,A4 ) )*180/pi;
endfunction theta5 = theta5_calculate(theta1,theta2,theta3,theta4)
%%
%導入?yún)?shù)
syms r [3,3]
syms a afa d
syms PX_X PY_Y_Y PZ_Z
T_ni = [-0.5 0 0.866 1635.672;
0 1 0 594.531;
-0.866 0 -0.5 1396.902;
0 0 0 1];
r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X = T_ni(1,4);
r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y = T_ni(2,4);
r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z = T_ni(3,4);
syms a d [1 6]
d1 = 780;d4 = 1142.5;d6 = 200;
a2 = 320;a3 = 1125 ;a4 = 200;
syms A B C D E F [1 6]
theta1 = theta1/180*pi;
theta2 = theta2/180*pi;
theta3 = theta3/180*pi;
theta4 = theta4/180*pi;
%%
%theta4計算
A5 = r1_3*sin(theta1)*sin(theta4) - r2_3*cos(theta1)*sin(theta4) - r3_3*cos(theta2)*cos(theta4)*sin(theta3) ...
- r3_3*cos(theta3)*cos(theta4)*sin(theta2) + r1_3*cos(theta1)*cos(theta2)*cos(theta3)*cos(theta4) + ...
r2_3*cos(theta2)*cos(theta3)*cos(theta4)*sin(theta1) - r1_3*cos(theta1)*cos(theta4)*sin(theta2)*sin(theta3) ...
- r2_3*cos(theta4)*sin(theta1)*sin(theta2)*sin(theta3);
B5 = r3_3*sin(theta2)*sin(theta3) - r3_3*cos(theta2)*cos(theta3) - r1_3*cos(theta1)*cos(theta2)*sin(theta3) ...
- r1_3*cos(theta1)*cos(theta3)*sin(theta2) - r2_3*cos(theta2)*sin(theta1)*sin(theta3) - ...
r2_3*cos(theta3)*sin(theta1)*sin(theta2);
theta5 = atan2(-A5,B5)*180/pi;
endfunction theta6 = theta6_calculate(theta1,theta2,theta3,theta4)
%%
%導入?yún)?shù)
syms r [3,3]
syms a afa d
syms PX_X PY_Y_Y PZ_Z
T_ni = [-0.5 0 0.866 1635.672;
0 1 0 594.531;
-0.866 0 -0.5 1396.902;
0 0 0 1];
r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X = T_ni(1,4);
r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y = T_ni(2,4);
r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z = T_ni(3,4);
syms a d [1 6]
d1 = 780;d4 = 1142.5;d6 = 200;
a2 = 320;a3 = 1125 ;a4 = 200;
syms A B C D E F [1 6]
theta1 = theta1/180*pi;
theta2 = theta2/180*pi;
theta3 = theta3/180*pi;
theta4 = theta4/180*pi;
%%
%theta4計算
A6 = r2_1*cos(theta1)*cos(theta4) - r1_1*cos(theta4)*sin(theta1) - r3_1*cos(theta2)*sin(theta3)*sin(theta4) ...
- r3_1*cos(theta3)*sin(theta2)*sin(theta4) + r1_1*cos(theta1)*cos(theta2)*cos(theta3)*sin(theta4) + ...
r2_1*cos(theta2)*cos(theta3)*sin(theta1)*sin(theta4) - r1_1*cos(theta1)*sin(theta2)*sin(theta3)*sin(theta4) ...
- r2_1*sin(theta1)*sin(theta2)*sin(theta3)*sin(theta4);
B6 = r2_2*cos(theta1)*cos(theta4) - r1_2*cos(theta4)*sin(theta1) - r3_2*cos(theta2)*sin(theta3)*sin(theta4) ...
- r3_2*cos(theta3)*sin(theta2)*sin(theta4) + r1_2*cos(theta1)*cos(theta2)*cos(theta3)*sin(theta4) ...
+ r2_2*cos(theta2)*cos(theta3)*sin(theta1)*sin(theta4) - r1_2*cos(theta1)*sin(theta2)*sin(theta3)*sin(theta4) ...
- r2_2*sin(theta1)*sin(theta2)*sin(theta3)*sin(theta4);
theta6 = atan2(-A6,-B6)*180/pi;
endfunction T = trans_cal(afa_ii,a_ii,d_i,theta_i)
%%
%計算變換矩陣函數(shù)T_{i-1,i}
%輸入的參數(shù)為，afa_{i-1},a_{i-1},d_i,theta_i,與DH表達法的參數(shù)表對應(yīng)
%ii 為i-1
%注意，這里輸入的角度，均采用角度制，不采用弧度制
T = [cosd(theta_i) -sind(theta_i) 0 a_ii
sind(theta_i)*cosd(afa_ii) cosd(theta_i)*cosd(afa_ii) -sind(afa_ii) -sind(afa_ii)*d_i
sind(theta_i)*sind(afa_ii) cosd(theta_i)*sind(afa_ii) cosd(afa_ii) cosd(afa_ii)*d_i
0 0 0 1 ];
end參考文獻
[1] 尹緒偉. 打磨機器人不同位姿下的剛度特性研究[D]. 武漢: 武漢理工大學, 2019.文章來源：http://www.zghlxwxcb.cn/news/detail-409431.html
[2] 王宇迪. 基于加工姿態(tài)最優(yōu)的發(fā)動機飛輪殼機器人銑削修邊空間路徑規(guī)劃[D]. 武漢: 武漢理工大學, 2022.文章來源地址http://www.zghlxwxcb.cn/news/detail-409431.html
到了這里，關(guān)于機械臂正向與逆向運動學求解的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
