D-H參數表
+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 0| L1| 1.5708| 0|
| 2| q2| 0| L2| 0| 0|
| 3| q3| 0| L3| 0| 0|
| 4| q4| 0| L4| 1.5708| 0|
| 5| q5| 0| L5| 0| 0|
+---+-----------+-----------+-----------+-----------+-----------+
五連杆如圖所示
正解用D-H參數相乘
L1=0.5; L2=0.7;L3=0.8;L4=0.7;L5=0.5;
p1 = [pi/4 0 L1 pi/2];
p2 = [pi/4 0 L2 0];
p3 = [pi/4 0 L3 0];
p4 = [0 0 L4 pi/2];
p5 = [pi/4 0 L5 0];
para = [p1;p2;p3;p4;p5];
pos = fk(para)
function T = fk(links)
%正解
%已知關節角求變換矩陣
% theta kinematic: joint angle 繞Z軸旋轉角度
% d kinematic: link offset 繞Z軸平移距離
% a kinematic: link length 繞X軸平移距離
% alpha kinematic: link twist 繞X軸旋轉角度
T=diag([1 1 1 1]); %機關陣
si=size(links);
for i=1:si(1) %行數
T_link=T_para(links(i,:));
T=T*T_link;
end
end
function T = T_para(p)
%由連杆參數轉化為轉化矩陣
%theta：繞Z軸旋轉角度； a:繞X軸平移距離; d:繞Z軸平移距離; alpha:繞X軸旋轉角度
%傳回4維矩陣
theta=p(1);d=p(2);a=p(3);alpha=p(4);
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
逆解就解方程了
已知末端坐标及歐拉角和杆長 x y z R P Y L1 L2 L3 L4 L5 求五杆的轉角 th1 th2 th3 th4 th5
為了減少變量個數，alpha及 offset作為已知參數給出
由末端坐标及歐拉角求得末端的變換矩陣 T
[ cos(P)*cos(Y), cos(Y)*sin(P)*sin(R) - cos(R)*sin(Y), sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P), x]
[ cos(P)*sin(Y), cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y), cos(R)*sin(P)*sin(Y) - cos(Y)*sin(R), y]
[ -sin(P), cos(P)*sin(R), cos(P)*cos(R), z]
[ 0, 0, 0, 1]
%連杆參數
p1 = [th1 0 L1 pi/2];
p2 = [th2 0 L2 0];
p3 = [th3 0 L3 0];
p4 = [th4 0 L4 pi/2];
p5 = [th5 0 L5 0];
%轉化矩陣
T1 = T_para(p1);
T2 = T_para(p2);
T3 = T_para(p3);
T4 = T_para(p4);
T5 = T_para(p5);
%T1的逆設為T1_inv ,其餘類比
T1_inv = inv(T1 )
T5_inv = inv(T5);
由 T = T1*T2*T3*T4*T5
T1_inv * T = T2*T3*T4*T5
T1_inv * T =
[ cos(P)*cos(Y - th1), sin(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)) - cos(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)), cos(th1)*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)) - sin(th1)*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)), x*cos(th1) - L1 + y*sin(th1)]
[ -sin(P), cos(P)*sin(R), cos(P)*cos(R), z]
[ -cos(P)*sin(Y - th1), - cos(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)) - sin(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)), cos(th1)*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)) + sin(th1)*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)), x*sin(th1) - y*cos(th1)]
[ 0, 0, 0, 1]
T2*T3*T4*T5=
[ cos(th2 + th3 + th4)*cos(th5), -cos(th2 + th3 + th4)*sin(th5), sin(th2 + th3 + th4), L3*cos(th2 + th3) + L2*cos(th2) + L4*cos(th2 + th3)*cos(th4) - L4*sin(th2 + th3)*sin(th4) - L5*cos(th5)*(sin(th2 + th3)*sin(th4) - cos(th2 + th3)*cos(th4))]
[ sin(th2 + th3 + th4)*cos(th5), -sin(th2 + th3 + th4)*sin(th5), -cos(th2 + th3 + th4), L3*sin(th2 + th3) + L2*sin(th2) + L4*cos(th2 + th3)*sin(th4) + L4*sin(th2 + th3)*cos(th4) + L5*cos(th5)*(cos(th2 + th3)*sin(th4) + sin(th2 + th3)*cos(th4))]
[ sin(th5), cos(th5), 0, L5*sin(th5)]
[ 0, 0, 0, 1]
由兩者三行四列相等：
L5*sin(th5) = x*sin(th1) - y*cos(th1)
求得th1與th5關系
由兩者1行1列與2行1列 得到
sin(th2 + th3 + th4) = -sin(P)/cos(th5)
cos(th2 + th3 + th4) = cos(P)*cos(Y - th1) / cos(th5);
由于 sin(th2 + th3 + th4)與 cos(th2 + th3 + th4) 有多種表示方式，在此取不包含R的表達式
又有 T1_inv * T* T5_inv = T2*T3*T4
T1_inv * T* T5_inv =
[ sin(th5)*(cos(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)) - sin(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y))) + cos(P)*cos(th5)*cos(Y - th1), cos(P)*sin(th5)*cos(Y - th1) - cos(th5)*(cos(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)) - sin(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y))), cos(th1)*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)) - sin(th1)*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)), x*cos(th1) - L5*(cos(P)*cos(Y)*cos(th1) + cos(P)*sin(Y)*sin(th1)) - L1 + y*sin(th1)]
[ - sin(P)*cos(th5) - cos(P)*sin(R)*sin(th5), cos(P)*sin(R)*cos(th5) - sin(P)*sin(th5), cos(P)*cos(R), z + L5*sin(P)]
[ sin(th5)*(cos(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)) + sin(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R))) - cos(P)*cos(th5)*sin(Y - th1), - cos(th5)*(cos(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)) + sin(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R))) - cos(P)*sin(th5)*sin(Y - th1), cos(th1)*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)) + sin(th1)*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)), x*sin(th1) - y*cos(th1) - L5*(cos(P)*cos(Y)*sin(th1) - cos(P)*sin(Y)*cos(th1))]
[ 0, 0, 0, 1]
T2*T3*T4 =
[ cos(th2 + th3 + th4), 0, sin(th2 + th3 + th4), L3*cos(th2 + th3) + L2*cos(th2) + L4*cos(th2 + th3 + th4)]
[ sin(th2 + th3 + th4), 0, -cos(th2 + th3 + th4), L3*sin(th2 + th3) + L2*sin(th2) + L4*sin(th2 + th3 + th4)]
[ 0, 1, 0, 0]
[ 0, 0, 0, 1]
由兩者三行四列相等：
x*sin(th1) - y*cos(th1) - L5*(cos(P)*cos(Y)*sin(th1) - cos(P)*sin(Y)*cos(th1)) = 0
則 th1 = atan( (y-L5*cos(P)*sin(Y))/(x-L5*cos(P)*cos(Y)) )
是以 th5=asin( (x*sin(th1)-y*cos(th1)) / L5);
因為五杆隻有五個轉動副，理應隻能控制五個未知量，是以一個末端坐标應該由其它表示
由兩者二行二列相等
cos(P)*sin(R)*cos(th5) - sin(P)*sin(th5) = 0
得到 R =asin( sin(P)*sin(th5)/cos(P)/cos(th5) );
在此可以得到R，但由于sinR已知時 cosR有雙解 是以不使用存在cosR的等式
由二者1行4列與2行4列 聯立解方程
L3*cos(th2 + th3) + L2*cos(th2) + L4*cos(th2 + th3 + th4) = x*cos(th1) - L5*(cos(P)*cos(Y)*cos(th1) + cos(P)*sin(Y)*sin(th1)) + y*sin(th1)
L3*sin(th2 + th3) + L2*sin(th2) + L4*sin(th2 + th3 + th4) = z - L1 + L5*sin(P)
令
m1 = z - L1 + L5*sin(P) - L4* sin(th2 + th3 + th4) ;
n1=x*cos(th1) - L5*(cos(P)*cos(Y)*cos(th1) + cos(P)*sin(Y)*sin(th1)) + y*sin(th1) - L4*cos(th2 + th3 + th4);
方程簡化為
m1 - L2*sin(th2) = L3*sin(th2 + th3)
n1 - L2*cos(th2) = L3*cos(th2 + th3)
兩邊平方相加,得
2*m1*L2*sin(th2) + 2*n1*L2*cos(th2) =m1*m1+n1*n1+L2*L2 - L3*L3
令：
m2=2*m1*L2;
n2=2*n1*L2;
k=m1^2+n1^2+L2^2-L3^2;
解這個方程，可以通過平方消去sin或者cos,如果消去的是sin,則使用acos可以求得th2,得到
th2 =acos(( 2*n2*k-sqrt(4*n2^2*k^2-4*(k^2-m2^2)*(m2^2+n2^2)) ) / ( 2*(m2^2+n2^2) ));
或者 th2 =acos(( 2*n2*k + sqrt(4*n2^2*k^2-4*(k^2-m2^2)*(m2^2+n2^2)) ) / ( 2*(m2^2+n2^2) ));
求解th3與th4時同樣使用acos，這樣使用求解角度範圍在0~pi是唯一的
再代入方程，得
th3 =acos((n1 - L2*cos(th2))/L3) -th2;
th4=acos(cos234)-th2-th3; %cos234為 cos(th2 + th3 + th4)
求解完畢
則可以由 x y z P Y 求五杆的轉角 th1 th2 th3 th4 th5
有多解 ，而此處沒有讨論角度範圍，是以該方法适合求解角度範圍在0~pi
如果消去的是cos,則使用asin可以求得th2,
det =4*m2^2*k^2-4*(k^2-n2^2)*(m2^2+n2^2);
th2(1) =asin(( 2*m2*k-sqrt(det) ) / ( 2*(m2^2+n2^2) ));
th2(2) =asin(( 2*m2*k+sqrt(det) ) / ( 2*(m2^2+n2^2) ));
th3 =asin((m1 -L2*sin(th2))/L3) -th2;
th4=asin(sin234)-th2-th3;
c= [th1,th2(1),th3(1),th4(1),th5];
該方法适合求解角度範圍在-pi/2~pi/2
![auboi5在Ubuntu18.04下執行roslaunch aubo_i5_moveit_config moveit_planning_execution出現[aubo_driver-5]錯誤[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)