【matlab与机器人】PUMA560运动学逆解

本文主要是介绍【matlab与机器人】PUMA560运动学逆解，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

经观察，PUMA560机器人的逆解在任何参考书中均一致。自己推了一遍，matlab代码使用了

基于Matlab的PUMA 560运动学与轨迹规划仿真 - 灰信网（软件开发博客聚合） (freesion.com)

的参考代码。经实验，该段代码能够正确回推各个θ角度。

下面先总结推导过程中用到的一般方法与思路，供迁移到其他机器人的建模上使用。

方法1：构造sinx与cosx的函数，使用atan2(sinx,cosx)进行求解。

原因是atan2能够考虑到象限的位置关系，得到正确的角度。否则只使用arctan()函数，会造成角度的错乱。

方法2：一般形式三角函数的求解方法

构造出来的形式一般会有：

$C1\cdot sinx+ C2\cdot cosx=C3$

其中C1、C2、C3都是常量。令

$C1=\rho cos\psi , C2=\rho sin\psi, \rho=\sqrt{c1^{2}+c2^{2}}$

则

$\psi =Atan(C1/C2)$

由三角和差公式，就有了

$sin(\theta +\psi )=C3/\rho$

$cos(\theta +\psi )=\pm \sqrt{1-(C3/\rho )^{2})}$

sin()/cos(),得到tan().

两边同时做atan2()的参数，求出θ+ψ，再结合ψ的式子，就可求解。

方法3：观察

求解逆解，一般是构造方法2的式子，用方法2处理。解方程又有一般规律，即注意未知数与方程的个数。解三角函数又要注意，我们求相同未知数往往用两个式子（sinx和cosx，用atan2），要观察好sinx与cosx是否有具体对应，不到万不得已，或者已经进入的方法2的节奏，才使用

$sin^{2}x+cos^{2}x=1$

进行运算，并且必须保留正负号。

每次求解一个值，必须仔细观察矩阵左右两边的对应关系，并且使用和差公式进行简化，看看简化后两边sin和cos的参数是不是只剩相同的，即如果左右两边同时出现s4、s23、c4、c23，且两个角度未知，那基本上解不了。

还要注意，两个式子左右平方后相加，可能出现大量去除三角函数的情况（

$Px\cdot sin^{2}x+Px\cdot cos^{2}x=Px$

），这有利于我们求解。

这篇文章是为了整理毕设工作报告做的，没时间进行更细致讲解。解题过程随意翻阅任意一本机器人运动学相关的书籍即可（尽量书籍，比较连贯），代码我在灰信网给你们找到了，我粘贴在开头和最后，如果侵权就删掉。

使用求解逆解还有几个小要点：（可以自己写成一个m文件）

1.你别老老实实直接 theta_Med=Inverse_kinematics(T) 就上了，你会发现很慢

正确的做法：theta_Med=vpa(Inverse_kinematics(T06_REAL_KNOWN),5)

T06_REAL_KNOWN就是已知位置的矩阵

也就是用vpa控制好精度，求解速度会很快，而且结果输出得漂亮。

2.要是基本参数（a2、d2等）没有被替换掉，添加这么一句：

theta_Med=subs(theta_Med,[a2 a3 d2 d4],[0.4318,0.02032,0.14909,0.43307])

单位是m

3.再约束一下精度

theta_Med=vpa( theta_Med,5)

4.转化为角度

theta_Med=theta_Med/3.1416*180

function theta_Med=Inverse_kinematics(T)
% a--连杆长度，d--连杆偏移量a2=0.4318;a3=0.02032;d2=0.14909;d4=0.43307;nx=T(1,1); ny=T(2,1); nz=T(3,1);  ox=T(1,2); oy=T(2,2); oz=T(3,2); ax=T(1,3); ay=T(2,3); az=T(3,3); px=T(1,4); py=T(2,4); pz=T(3,4);% 为方便计算，定义的m系列向量
% 求解关节角1theta1_1 = atan2(py,px)-atan2(d2,sqrt(px^2+py^2-d2^2));theta1_2 = atan2(py,px)-atan2(d2,-sqrt(px^2+py^2-d2^2));
% 求解关节角3m3_1 = (px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2)/(2*a2);theta3_1 = atan2(a3,d4)-atan2(m3_1,sqrt(a3^2+d4^2-m3_1^2));theta3_2 = atan2(a3,d4)-atan2(m3_1,-sqrt(a3^2+d4^2-m3_1^2));
% 求解关节角2ms2_1 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*...(a2*sin(theta3_1)-d4);mc2_1 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*...(a2*cos(theta3_1)+a3);theta23_1 = atan2(ms2_1,mc2_1);theta2_1 = theta23_1 - theta3_1;ms2_2 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*...(a2*sin(theta3_1)-d4);mc2_2 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*...(a2*cos(theta3_1)+a3);theta23_2 = atan2(ms2_2,mc2_2);theta2_2 = theta23_2 - theta3_1;ms2_3 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*...(a2*sin(theta3_2)-d4);mc2_3 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*...(a2*cos(theta3_2)+a3);theta23_3 = atan2(ms2_3,mc2_3);theta2_3 = theta23_3 - theta3_2;ms2_4 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*...(a2*sin(theta3_2)-d4);mc2_4 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*...(a2*cos(theta3_2)+a3);theta23_4 = atan2(ms2_4,mc2_4);theta2_4 = theta23_4 - theta3_2;    
% 求解关节角4ms4_1=-ax*sin(theta1_1)+ay*cos(theta1_1);mc4_1=-ax*cos(theta1_1)*cos(theta23_1)-ay*sin(theta1_1)*...cos(theta23_1)+az*sin(theta23_1);theta4_1=atan2(ms4_1,mc4_1);ms4_2=-ax*sin(theta1_2)+ay*cos(theta1_2);mc4_2=-ax*cos(theta1_2)*cos(theta23_2)-ay*sin(theta1_2)*...cos(theta23_2)+az*sin(theta23_2);theta4_2=atan2(ms4_2,mc4_2);ms4_3=-ax*sin(theta1_1)+ay*cos(theta1_1);mc4_3=-ax*cos(theta1_1)*cos(theta23_3)-ay*sin(theta1_1)*...cos(theta23_3)+az*sin(theta23_3);theta4_3=atan2(ms4_3,mc4_3);ms4_4=-ax*sin(theta1_2)+ay*cos(theta1_2);mc4_4=-ax*cos(theta1_2)*cos(theta23_4)-ay*sin(theta1_2)*...cos(theta23_4)+az*sin(theta23_4);theta4_4=atan2(ms4_4,mc4_4);
% 求解关节角5ms5_1=-ax*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+...sin(theta1_1)*sin(theta4_1))-...ay*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)-cos(theta1_1)*sin(theta4_1))...+az*(sin(theta23_1)*cos(theta4_1));mc5_1= ax*(-cos(theta1_1)*sin(theta23_1))+ay*(-sin(theta1_1)*sin(theta23_1))...+az*(-cos(theta23_1));theta5_1=atan2(ms5_1,mc5_1);ms5_2=-ax*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+...sin(theta1_2)*sin(theta4_2))-...ay*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)-cos(theta1_2)*sin(theta4_2))...+az*(sin(theta23_2)*cos(theta4_2));mc5_2= ax*(-cos(theta1_2)*sin(theta23_2))+ay*(-sin(theta1_2)*sin(theta23_2))...+az*(-cos(theta23_2));theta5_2=atan2(ms5_2,mc5_2);ms5_3=-ax*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+...sin(theta1_1)*sin(theta4_3))-...ay*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)-cos(theta1_1)*sin(theta4_3))...+az*(sin(theta23_3)*cos(theta4_3));mc5_3= ax*(-cos(theta1_1)*sin(theta23_3))+ay*(-sin(theta1_1)*sin(theta23_3))...+az*(-cos(theta23_3));theta5_3=atan2(ms5_3,mc5_3);ms5_4=-ax*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+...sin(theta1_2)*sin(theta4_4))-...ay*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)-cos(theta1_2)*sin(theta4_4))...+az*(sin(theta23_4)*cos(theta4_4));mc5_4= ax*(-cos(theta1_2)*sin(theta23_4))+ay*(-sin(theta1_2)*sin(theta23_4))...+az*(-cos(theta23_4));theta5_4=atan2(ms5_4,mc5_4);
% 求解关节角6ms6_1=-nx*(cos(theta1_1)*cos(theta23_1)*...sin(theta4_1)-sin(theta1_1)*cos(theta4_1))...-ny*(sin(theta1_1)*cos(theta23_1)*...sin(theta4_1)+cos(theta1_1)*cos(theta4_1))...+nz*(sin(theta23_1)*sin(theta4_1));mc6_1= nx*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)...+sin(theta1_1)*sin(theta4_1))*cos(theta5_1)...-nx*cos(theta1_1)*sin(theta23_1)*sin(theta4_1)...+ny*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)...+cos(theta1_1)*sin(theta4_1))*cos(theta5_1)...-ny*sin(theta1_1)*sin(theta23_1)*sin(theta5_1)...-nz*(sin(theta23_1)*cos(theta4_1)*cos(theta5_1)...+cos(theta23_1)*sin(theta5_1));theta6_1=atan2(ms6_1,mc6_1);ms6_2=-nx*(cos(theta1_2)*cos(theta23_2)*...sin(theta4_2)-sin(theta1_2)*cos(theta4_2))...-ny*(sin(theta1_2)*cos(theta23_2)*...sin(theta4_2)+cos(theta1_2)*cos(theta4_2))...+nz*(sin(theta23_2)*sin(theta4_2));mc6_2= nx*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)...+sin(theta1_2)*sin(theta4_2))*cos(theta5_2)...-nx*cos(theta1_2)*sin(theta23_2)*sin(theta4_2)...+ny*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)...+cos(theta1_2)*sin(theta4_2))*cos(theta5_2)...-ny*sin(theta1_2)*sin(theta23_2)*sin(theta5_2)...-nz*(sin(theta23_2)*cos(theta4_2)*cos(theta5_2)...+cos(theta23_2)*sin(theta5_2));theta6_2=atan2(ms6_2,mc6_2);ms6_3=-nx*(cos(theta1_1)*cos(theta23_3)*...sin(theta4_3)-sin(theta1_1)*cos(theta4_3))...-ny*(sin(theta1_1)*cos(theta23_3)*...sin(theta4_3)+cos(theta1_1)*cos(theta4_3))...+nz*(sin(theta23_3)*sin(theta4_3));mc6_3= nx*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)...+sin(theta1_1)*sin(theta4_3))*cos(theta5_3)...-nx*cos(theta1_1)*sin(theta23_3)*sin(theta4_3)...+ny*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)...+cos(theta1_1)*sin(theta4_3))*cos(theta5_3)...-ny*sin(theta1_1)*sin(theta23_3)*sin(theta5_3)...-nz*(sin(theta23_3)*cos(theta4_3)*cos(theta5_3)...+cos(theta23_3)*sin(theta5_3));theta6_3=atan2(ms6_3,mc6_3);ms6_4=-nx*(cos(theta1_2)*cos(theta23_4)*...sin(theta4_4)-sin(theta1_2)*cos(theta4_4))...-ny*(sin(theta1_1)*cos(theta23_4)*...sin(theta4_4)+cos(theta1_2)*cos(theta4_4))...+nz*(sin(theta23_4)*sin(theta4_4));mc6_4= nx*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)...+sin(theta1_2)*sin(theta4_4))*cos(theta5_4)...-nx*cos(theta1_2)*sin(theta23_4)*sin(theta4_4)...+ny*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)...+cos(theta1_2)*sin(theta4_4))*cos(theta5_1)...-ny*sin(theta1_2)*sin(theta23_4)*sin(theta5_4)...-nz*(sin(theta23_4)*cos(theta4_4)*cos(theta5_4)...+cos(theta23_4)*sin(theta5_4));theta6_4=atan2(ms6_4,mc6_4);% 整理得到4组运动学非奇异逆解theta_Med_1 = [ theta1_1,theta2_1,theta3_1,theta4_1,theta5_1,theta6_1;theta1_2,theta2_2,theta3_1,theta4_2,theta5_2,theta6_2;theta1_1,theta2_3,theta3_2,theta4_3,theta5_3,theta6_3;theta1_2,theta2_4,theta3_2,theta4_4,theta5_4,theta6_4;];
% 将操作关节‘翻转’可得到另外4组解
theta_Med_2 = ...[ theta1_1,theta2_1,theta3_1,theta4_1+pi,-theta5_1,theta6_1+pi;theta1_2,theta2_2,theta3_1,theta4_2+pi,-theta5_2,theta6_2+pi;theta1_1,theta2_3,theta3_2,theta4_3+pi,-theta5_3,theta6_3+pi;theta1_2,theta2_4,theta3_2,theta4_4+pi,-theta5_4,theta6_4+pi;];theta_Med=[theta_Med_1;theta_Med_2];         
end

这篇关于【matlab与机器人】PUMA560运动学逆解的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！