Puma560机器人运动学正逆解

puma560机器人D-H参数

puma560采用的是改进D-H参数,其DH参数表如下:

i

αi

ai

di

θi

1

0

0

0

t1

2

-90

0

0

t2

3

0

r2

d3

t3

4

-90

r3

d4

t4

5

90

0

0

t5

6

-90

0

0

t6

对于参数为(α,a,d, θ)的机械臂,其变换矩阵为:

T = [        cosθ,         -sinθ,       0,           a;

        sinθcosα, cosθcosα, -sinα, -dsinα;

         sinθsinα, cosθsinα,  cosα, dcosα;

                    0,             0,        0,          1]

所以可以根据上表写出6个关节变换矩阵:

T01 = [cos(t1),-sin(t1),0,0;
            sin(t1),cos(t1),0,0;
           0,0,1,0;
           0,0,0,1];

T12 = [cos(t2),-sin(t2),0,0;
           0,0,1,0;
          -sin(t2),-cos(t2),0,0;
           0,0,0,1];
T23 = [cos(t3),-sin(t3),0,r2;
           sin(t3),cos(t3),0,0;
           0,0,1,d3;
           0,0,0,1];
T34 = [cos(t4),-sin(t4),0,r3;
           0,0,1,d4;
           -sin(t4),-cos(t4),0,0;
           0,0,0,1];
T45 = [cos(t5),-sin(t5),0,0;
           0,0,-1,0;
           sin(t5),cos(t5),0,0;
           0,0,0,1];
T56 = [cos(t6),-sin(t6),0,0;
           0,0,1,0;
           -sin(t6),-cos(t6),0,0;
           0,0,0,1];

运动学正解

puma560机器人运动学正解计算为:

T06 = T01*T12*T23*T34*T45*T56,具体就不放结果了

运动学逆解

已知T06=[r11,r12,r13,Px;

                 r21,r22,r23,Py;

                 r31,r32,r33,Pz;

                    0,   0,    0,  1]

求出6个关节角t1,t2,t3,t4,t5,t6,就称之为运动学逆解

关节角t1计算

可以先计算T16 = T12*T23*T34*T45*T56,计算结果为:

T16 =
[-c23*(s4s6-c4c5c6)-s23s5c6,  s23s5s6-c23(c6s4+c4c5s6),-s23c5-c23c4s5,  r3c23-d4s23+r2c2;
                         -c4s6-s4c5c6,                          s4c5s6-c4c6,                  s4s5,                           d3;
   s23(s4s6-c4c5c6)-c23s5c6, s23*(s4c6+c4c5s6)+c23s5s6, s23c4s5-c23c5, -d4c23-r3s23-r2s2;
                                            0,                                             0,                        0,                             1]

这里的c23=cos(t2+t3),s23=sin(t2+t3),s2=sint2,s3=sin(t3),是一种缩写记法。

又有

由T16[1,3]元素相等 ,可得:

-s1Px+c1Py = d3

 

则有:

可以看到t1有2个解。

关节角t3计算

 现在t1已知,根据T16的元素(0,3)、元素(1,3)、元素(2,3)列出等式:

 三式取平方和有:

 

三式相加化简有:

有看到了熟悉的式子,可以计算t3等于:

可以看到t3有2个解。

关节角t2的计算

先计算T03 ,建议用matlab符号运算来计算,matlab符号运算是推算公式的利器。

取T36的(0,3)元素及(1,3)元素列出等式:

c23c1Px+c23s1Py-s23Pz-r2c3 = r3

-s23c1Px-s23s1Py-c23Pz+r2s3 = d4

联立上面等式可以解出c23和s23,进而可以求出t2+t3,进而求出t2。

关节角t4的计算

T36矩阵中取(0,2)元素及(2,2)元素,成立等式:

r13c23c1+r23c23s1-r33s23 = -c4s5

-s1r13+c1r23 = s4s5

只要t5不为0,则有t4=atan2(-s1r13+c1r23,-r13c23c1-r23c23s1+r33s23);

当t5为0时,关节4和关节6出于一条直线,所有可能的接都是t4和t6的和或差。

关节角t5和t6的计算

 先计算T04及T04的逆矩阵

取T46的(0,2)元素和(2,2)元素构成等式:

-s5=r13(s1s4+c1c4c23)+r23(s1c23c4-c1s4)-r33s23c4

c5=-r13c1s23-r23s1s23+c1c4)-r33c23

故有t5=atan2(-(r13(s1s4+c1c4c23)+r23(s1c23c4-c1s4)-r33s23c4),-r13c1s23-r23s1s23+c1c4)-r33c23)

取T46的(1,0)元素和(1,1)元素构成等式:

s6 = r21(-c1c23s4+s1c4)+r21(-s1c23s4-c1c4)+r3123s4

c6=r12(-c1c23s4+s1c4)+r22(-s1c23s4-c1c4)+r32s23s4

求出t6=atan2(r21(-c1c23s4+s1c4)+r21(-s1c23s4-c1c4)+r3123s4,r12(-c1c23s4+s1c4)+r22(-s1c23s4-c1c4)+r32s23s4)

前面计算t1和t3各有2个解,所以共有4组解,再加上腕关节翻转,可以得出8组解。对于计算出的4组解,由腕关节翻转,可以得到:

 

 

 

 

 

 

 

 

                                     

  • 6
    点赞
  • 64
    收藏
    觉得还不错? 一键收藏
  • 3
    评论
PUMA560机器人是一种六自由度的工业机器人,常用于机器人运动学研究和控制。运动学逆解是通过已知机器人末端执行器的位置和姿态,计算出每个关节的角度值,以实现机器人的精确定位和控制。MATLAB是一种常用的数学软件,也被广泛应用于机器人运动学逆解的计算。 以下是一种基于MATLAB的PUMA560机器人运动学逆解的实现方法: 1. 定义机器人的DH参数 DH参数是描述机器人关节之间距离和相对姿态的参数,需要先进行定义。对于PUMA560机器人,其DH参数如下: alpha = [0 -pi/2 0 pi/2 -pi/2 0]; a = [0.4318 0 0.0203 0 0 0]; d = [0.1491 0 0 0.4331 0 0.068]; theta = [q1 q2 q3 q4 q5 q6]; 其中,alpha、a、d分别表示前后相邻关节旋转轴线之间的夹角、相邻关节的距离、相邻关节的偏移量,而theta则是每个关节的角度值,即运动学逆解需要求解的量。 2. 计算机器人各关节的转换矩阵 根据DH参数,可以计算出每个关节的转换矩阵,即T01、T12、T23、T34、T45、T56。这些矩阵可以使用MATLAB中的Tz、Tx、Rz、Rx等函数进行计算。具体实现方法如下: T01 = Rz(theta(1))*Tx(a(1))*Tz(d(1))*Rx(alpha(1)); T12 = Rz(theta(2))*Tx(a(2))*Tz(d(2))*Rx(alpha(2)); T23 = Rz(theta(3))*Tx(a(3))*Tz(d(3))*Rx(alpha(3)); T34 = Rz(theta(4))*Tx(a(4))*Tz(d(4))*Rx(alpha(4)); T45 = Rz(theta(5))*Tx(a(5))*Tz(d(5))*Rx(alpha(5)); T56 = Rz(theta(6))*Tx(a(6))*Tz(d(6))*Rx(alpha(6)); 3. 计算机器人末端执行器的位姿矩阵 根据机器人的DH参数和各关节的转换矩阵,可以计算出机器人末端执行器的位姿矩阵T06,即机器人的正向运动学矩阵。具体实现方法如下: T06 = T01*T12*T23*T34*T45*T56; 4. 计算机器人各关节的角度值 根据机器人末端执行器的位姿矩阵T06,可以计算出机器人各关节的角度值,即机器人的逆向运动学解。具体实现方法如下: T06 = simplify(T06); x = T06(1,4); y = T06(2,4); z = T06(3,4); r = sqrt(x^2 + y^2); q1 = atan2(y, x); q3 = acos((r^2 + (z - d(1))^2 - a(2)^2 - a(3)^2)/(2*a(2)*a(3))); q2 = atan2(z - d(1), r) - atan2(a(3)*sin(q3), a(2) + a(3)*cos(q3))); q5 = acos((T06(3,1)*sin(q1) - T06(2,1)*cos(q1))/sin(q4)); q4 = atan2(-T06(2,3)*cos(q1) - T06(3,3)*sin(q1), T06(2,2)*cos(q1) + T06(3,2)*sin(q1)); q6 = atan2(T06(1,2)*sin(q1) - T06(1,3)*cos(q1), T06(1,3)*sin(q1) + T06(1,2)*cos(q1)); 最终,q1到q6就是机器人各关节的角度值,即为机器人运动学逆解。
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值