机器人学回炉重造（3）：matlab复现最基本的六自由度机械臂逆运动学推导.pdfVIP

机器⼈学回炉重造 （3）：matlab复现最基本的六⾃由度机械臂逆运动学推导 写在前 学习代码都记录在上，欢迎关注~ 本⽂使⽤改进D-H参数法建⽴模型，使⽤的是前 ABB机械臂模型，正运动学建模过程可见前 博客。 我使⽤最⼀般的矩阵逆乘解析法来求解六⾃由度机械臂的运动学逆解，过程仅供参考。 推导过程 Matlab仿真程序 % 适⽤于Modified DH参数法 function ik_T = myikine(fk_T) d(1) = 0; a(1) = 0; d(2) = 0; a(2) = 0.320;%a(2)=a1 d(3) = 0; a(3) = 0. 75 ;%a(3)=a2 d(4) = 0.887; a(4) = 0.2 ;%a(4)=a3 d(5) = 0; a(5) = 0; d(6) = 0; a(6) = 0; nx = fk_T(1, 1) ; ox = fk_T(1, 2) ; ax = fk_T(1, 3) ; px = fk_T(1, 4) ; ny = fk_T(2, 1) ; oy = fk_T(2, 2) ; ay = fk_T(2, 3) ; py = fk_T(2, 4) ; nz = fk_T(3, 1) ; oz = fk_T(3, 2) ; az = fk_T(3, 3) ; pz = fk_T(3, 4) ; %% theta1有两个解 theta1_ 1 = atan2((d(3)+d(2)), (px^2+py^2-(d(3)+d(2))^2)^0.5) + atan2(py, px) ; theta1_2 = atan2((d(3)+d(2)), -(px^2+py^2-(d(3)+d(2))^2)^0.5) + atan2(py, px) ; %% theta3有四个解 K_ 1 = ((a(2)-cos(theta1_ 1)*px-sin(theta1_ 1)*py)^2+pz^2-a(4)^2-d(4)^2-a(3)^2) / 2*a(3) ; K_2 = ((a(2)-cos(theta1_2)*px-sin(theta1_2)*py)^2+pz^2-a(4)^2-d(4)^2-a(3)^2) / 2*a(3) ; theta3_ 1 = atan2(K_ 1, (a(4)^2+d(4)^2-K_ 1^2)^0.5) - atan2(a(4), d(4)) ;%theta1_ 1 theta3_2 = atan2(K_ 1, -(a(4)^2+d(4)^2-K_ 1^2)^0.5) - atan2(a(4), d(4)) ;%theta1_ 1 theta3_3 = atan2(K_2, (a(4)^2+d(4)^2-K_2^2)^0.5) - atan2(a(4), d(4)) ;%theta1_2 theta3_4 = atan2(K_2, -(a(4)^2+d(4)^2-K_2^2)^0.5) - atan2(a(4), d(4)) ;%theta1_2 %% theta2(分别对应theta1和theta3 ，theta1和theta3各⾃也有对应) ，有四个解 e21 = a(4)*cos(theta3_ 1) + d(4)*sin(theta3_ 1) + a(3) ;%theta3_ 1 f21 = a(4)*sin(theta3_ 1) - d(4)*cos(theta3_ 1) ; e22 = a(4)*cos(theta3_2) + d(4)*sin(theta3_2) + a(3) ;%theta3_2 f22 = a(4)*sin(theta3_2) - d(4)*cos(theta3_2) ; e23 = a(4)*cos(theta3_3) + d(4)*sin(theta3_3) + a(3) ;%theta3_3 f23 = a(4)*sin(theta3_3) - d(4)*cos(theta3_3) ; e24 = a(4)*cos(theta3_4) + d(4)*sin(theta3_4) + a(3) ;%theta3_4 e24 = a(4)*cos(theta3_4) + d(4)*sin(theta3_4) + a(3) ;%theta3_4 f24 = a(4)*sin(theta3_4) - d(4)*cos(theta3_4) ; g 1 = cos(theta1_ 1)*px + sin(theta1_ 1)*py - a(2) ;%theta1_ 1 g2 = cos(theta1_2)*px + sin(theta