赞
踩
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
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。