【matlab与机器人】PUMA560的SDH逆解

和风寂羽

已于 2022-01-26 16:28:33 修改

阅读量1k

点赞数

分类专栏：自己的工作报告文章标签： matlab 开发语言

于 2022-01-26 15:26:32 首次发布

本文链接：https://blog.csdn.net/weixin_47868675/article/details/122702269

版权

本文介绍了如何使用MATLAB进行PUMA560的SDH（标准笛卡尔到关节）逆解，并提供了相关代码。通过正解与逆解的比较验证，确保解的准确性。在特定误差范围内，可以找到匹配的关节角度。

摘要由CSDN通过智能技术生成

写在前面：工作报告，不一定正确，谨慎观看使用。

数据表格，也是后面的验证代码

%%标准D-H模型
%       theta    d           a        alpha     offset
SL1=Link([0      0           0        -pi/2       0     ],'standard');
SL2=Link([0      0          432        0          0     ],'standard');
SL3=Link([0      149        20        -pi/2        0     ],'standard');
SL4=Link([0      433         0        pi/2        0     ],'standard');
SL5=Link([0      0           0        -pi/2       0     ],'standard');
SL6=Link([0      0           0        0           0     ],'standard');
p560=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','puma560');
p560.teach([ 0 0 0 0 0 0]);

代码1

使用前的说明：

1.目前结果，逆解带入正解和teach，一定有一个结果，考虑误差情况下，完全正确。

2.验证流程：Self_SDH_ikine_try_V1——>Self_SDH_fkine_try_V1 以及 Standard_DH_teach，也就是这里的代码2 -> 挑选逆解的结果（注意，选弧度的那个）-> 代码1和代码3。

3.挑选结果带入，位置是相同的，旋转矩阵可能不一样（该位置上为相反数，其他位置不为0，但远远小于某个十分靠近1的数），但一定有能完全对应上的结果。

4.教材给的例子：T_known=[0 1 0 -149.09;0 0 1 864.87; 1 0 0 20.32;0 0 0 1] ，可以自行带入试试，逆解的第二个就是教材给的θ角度，代入teach最后一行的参数那再看看，XYZ和RPY能否对的上。

SDH的逆解代码：

%自己做的SDH逆解,第一版本，几乎抄的construct_3_V2
%目前结果！逆解带入正解和teach，考虑误差完全正确
%验证流程：Self_SDH_ikine_try_V1——>Self_SDH_fkine_try_V1 以及 Standard_DH_teach
%挑选结果带入，位置是相同的，但是旋转矩阵可能不一样（该位置上为相反数），但一定有能完全对应上的结果
%教材给的例子：T_known=[0 1 0 -149.09;0 0 1 864.87; 1 0 0 20.32;0 0 0 1]
clear
clc
syms y x t1 t2 t3 t4 t5 t6

%输入已知T（你想求解的）矩阵：
%————————————————————

T_known=[0 1 0 -149.09;0 0 1 864.87; 1 0 0 20.32;0 0 0 1];

%————————————————————

%已知参数
    a2=431.8;a3=20.32;d3=149.09;d4=433.07;
%赋值变量，T_known=[nx ox ax px;ny oy ay py; nz oz az pz;0 0 0 1];
    nx=T_known(1,1); ny=T_known(2,1); nz=T_known(3,1);  
    ox=T_known(1,2); oy=T_known(2,2); oz=T_known(3,2); 
    ax=T_known(1,3); ay=T_known(2,3); az=T_known(3,3); 
    px=T_known(1,4); py=T_known(2,4); pz=T_known(3,4);
%求t1
t1_1=atan2(py, px) - atan2(d3, (- d3^2 + px^2 + py^2)^(1/2));
t1_2=atan2(py, px) - atan2(d3, -(- d3^2 + px^2 + py^2)^(1/2));

%求t3（注意方法，(1,4)(2,4)(3,4)的平方和）
t3_1=atan2(a2*a3, a2*d4) - atan2(- a2^2 - a3^2 - d3^2 - d4^2 + px^2 + py^2 + pz^2, (4*a2^2*a3^2 - (a2^2 + a3^2 + d3^2 + d4^2 - px^2 - py^2 - pz^2)^2 + 4*a2^2*d4^2)^(1/2));
t3_2=atan2(a2*a3, a2*d4) - atan2(- a2^2 - a3^2 - d3^2 - d4^2 + px^2 + py^2 + pz^2, -(4*a2^2*a3^2 - (a2^2 + a3^2 + d3^2 + d4^2 - px^2 - py^2 - pz^2)^2 + 4*a2^2*d4^2)^(1/2));

%求t2. t1 t3 独立，顺序：11,12,21,22
%cos(t2)*(px*cos(t1) + py*sin(t1)) - pz*sin(t2) = a2+a3*cos(t3) - d4*sin(t3)
%- pz*cos(t2) - sin(t2)*(px*cos(t1) + py*sin(t1)) = d4*cos(t3) + a3*sin(t3)
%A=[(px*cos(t1) + py*sin(t1)),- pz;- pz,-(px*cos(t1) + py*sin(t1))];
%b=[ a2+a3*cos(t3) - d4*sin(t3);d4*cos(t3) + a3*sin(t3)];
  %11
x= ((px*cos(t1) + py*sin(t1))*(a2 + a3*cos(t3) - d4*sin(t3)))/(px^2*cos(t1)^2 + py^2*sin(t1)^2 + pz^2 + 2*px*py*cos(t1)*sin(t1)) - (pz*(d4*cos(t3)