puma560的运动学及matlab实现(正解+逆解)

表1 PUMA560机器人的连杆参数

关节i

 

 

 

 

变化范围/(o)

1

90

0

0

0

-160~160

2

0

-90

0

149.09

-225~45

3

-90

0

431.8

0

-45~225

4

0

-90

20.32

443.07

-110~170

5

0

90

0

0

-100~100

6

0

-90

0

0

-266~266

 

正解源码

DEG = pi/180;

cta1=-70.4385

cta2=182.6918

cta3=-90.0000

cta4=-82.4708

cta5=-19.7387 

cta6=-97.9933

T01=[cosd(cta1),-sind(cta1),0,0;

    sind(cta1), cosd(cta1),0,0;

    0,0,1,0;

    0,0,0,1];

 

T02=T01*[cosd(cta2),-sind(cta2),0,0;

    0,0,1, 149.09;

    -sind(cta2),-cosd(cta2),0,0;

    0,0,0,1] ;

 

T03=T02*[cosd(cta3),-sind(cta3),0,431.8;

    sind(cta3), cosd(cta3),0,0;

    0,0,1,0;

    0,0,0,1];

T04=T03*[cosd(cta4),-sind(cta4),0,20.32;

    0,0,1,433.07;

    -sind(cta4),-cosd(cta4),0,0;

    0,0,0,1];

T05=T04*[cosd(cta5),-sind(cta5),0,0;

    0,0,-1,0;

    sind(cta5), cosd(cta5), 0,0;

    0,0,0,1];

T06=T05*[cosd(cta6),-sind(cta6),0,0;

    0,0,1,0;

    -sind(cta6),-cosd(cta6),0,0;

    0,0,0,1];

O=T06*[0;0;0;1];

=====================================================

逆解源码

fid = fopen('inverseout.txt','w');%逆解的保存文件
%赋初值
T06 =[0.0000    1.0000    0.0000 -149.0900;
     0.0000   -0.0000    1.0000  864.8700;
     1.0000         0   -0.0000   20.3200;
         0         0         0    1.0000] ; 
a0=0;  a1=0;  a2=431.8;  a3=20.32;  a4=0;  a5=0;
d1=0;  d2=149.09;  d3=0;  d4=433.07;  d5=0; d6=0;
n_x=T06(1);   n_y=T06(2);  n_z=T06(3);
o_x=T06(5);   o_y=T06(6);  o_z=T06(7);
a_x=T06(9);   a_y=T06(10);  a_z=T06(11);
p_x=T06(13);  p_y=T06(14);   p_z=T06(15);
  disp(['八组解分别是:']);
  for i=1:2
      for j=1:2
          for k=1:2 
                %求解theta1(为弧度)
                sqr1=[sqrt(p_x^2+p_y^2-d2^2),-sqrt(p_x^2+p_y^2-d2^2)];
                ta1=atan2(p_y,p_x)-atan2(d2,sqr1(i));
                %求解theta3(弧度表示)
                k1=(p_x^2+p_y^2+p_z^2-a2^2-a3^2-d2^2-d4^2)/(2*a2);
                sqr3=[sqrt(a3^2+d4^2-k1^2),-sqrt(a3^2+d4^2-k1^2) ];
                ta3=atan2(a3,d4)-atan2(k1,sqr3(j));
                fs23=-((a3+a2*cos(ta3))*p_z)+(cos(ta1)*p_x+sin(ta1)*p_y)*(a2*sin(ta3)-d4);
                sc23=(-d4+a2*sin(ta3))*p_z+(cos(ta1)*p_x+sin(ta1)*p_y)*(a2*cos(ta3)+a3);
                ta23=atan2( fs23,sc23);
                %求解theta2 (弧度表示)
                ta2=ta23-ta3;
                 %求解theta4 (弧度表示)
                fs4=[ -a_x*sin(ta1)+a_y*cos(ta1),a_x*sin(ta1)-a_y*cos(ta1)];
                sc4=[ -a_x*cos(ta1)*cos(ta23)-a_y*sin(ta1)*cos(ta23)+a_z*sin(ta23),
                     a_x*cos(ta1)*cos(ta23)+a_y*sin(ta1)*cos(ta23)-a_z*sin(ta23)];
                fprintf(fid,'%d,',sc4(1,1));
                fprintf(fid,'\t');
                fprintf(fid,'%d,',sc4(2,1));
                fprintf(fid,'\t');
                fprintf(fid,'%d,',fs4(1,1));
                fprintf(fid,'\t');
                fprintf(fid,'%d,',fs4(1,2));
                fprintf(fid,'\t');
                fprintf(fid,'\n');
                ta4=atan2(fs4(k),sc4(k));
                %求解theta5 (弧度表示)
                fs5=-a_x*(cos(ta1)*cos(ta23)*cos(ta4)+sin(ta1)*sin(ta4))...
                   -a_y*(sin(ta1)*cos(ta23)*cos(ta4)-cos(ta1)*sin(ta4))...
                   +a_z*(sin(ta23)*cos(ta4));
                sc5=a_x*(-cos(ta1)*sin(ta23))+a_y*(-sin(ta1)*sin(ta23))+a_z*(-cos(ta23));
                ta5=atan2(fs5,sc5);
                %求解theta6 (弧度表示)
                fs6=-n_x*(cos(ta1)*cos(ta23)*sin(ta4)-sin(ta1)*cos(ta4))...
                  -n_y*(sin(ta1)*cos(ta23)*sin(ta4)+cos(ta1)*cos(ta4))...
                   +n_z*(sin(ta23)*sin(ta4));

                sc6= n_x*(cos(ta1)*cos(ta23)*cos(ta4)+sin(ta1)*sin(ta4))*cos(ta5)...
                  -n_x*cos(ta1)*sin(ta23)*sin(ta5)...
                  +n_y*(sin(ta1)*cos(ta23)*cos(ta4)+cos(ta1)*sin(ta4))*cos(ta5)...
                  -n_y*sin(ta1)*sin(ta23)*sin(ta5)...
                  -n_z*(sin(ta23)*cos(ta4)*cos(ta5)+cos(ta23)*sin(ta5));
                ta6=atan2(fs6,sc6);
                %save 

                %将其化为角度
                Theta=[ta1 ta2 ta3 ta4 ta5 ta6]./pi*180
          end
      end   
  end
 

 

关于C++版本的运动学正解和逆解的代码,可以在以下链接下载

https://download.csdn.net/download/kobesdu/12099712

 

评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

kobesdu

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值