4. Bài toán động học.

4.1 Động học thuận và động học nghịch.

Robot 4 bậc tự do:

Bảng D-H

Đặt tt1=θ1 , tt2=θ2, tt3 = θ3 , tt4 = θ4

* Động học thuận:
T1=(Trans(0,0,l1)*Rot_z(tt1+(pi/2))*Trans(0,0,0)*Rot_x(pi/2));
T2=(Trans(0,0,l2)*Rot_z(-pi/2)*Trans(0,0,0)*Rot_x(pi/2));
T3=(Trans(0,0,0)*Rot_z(tt3+(pi/2))*Trans(l3,0,0)*Rot_x(0));
T4=(Trans(0,0,0)*Rot_z(tt4)*Trans(l4,0,0)*Rot_x(0));
T04=T01*T12*T23*T34

* Động học nghịch:

- Ta có:
0
T = 01T* 12T* 23T* 34T
4

- Nhân cả 2 vế cho ma trận đảo 01T:

0
T -1 * 04T = 01T -1 * 01T* 12T* 23T* 34T = 12T* 23T* 34T
1

Với ma trận 04T = [ r11, r12, r13, Px]

[ r21, r22, r23, Py]
[ r31, r32, r33, Pz]
[ 0, 0 , 0 , 1 ]
 Ta tính được
0
T-1 * 04T =
1

1
T* 23T* 34T =
2

{
Py∗cos ( tt 1 )−Px∗sin ( tt 1 )=0
 Pz – l1=l3∗sin ( tt 3 ) +l 4∗sin ( tt 3+ tt 4 )
Px∗cos ( tt 1 )+ Py∗sin ( tt 1 )=l 2+l 4∗cos (tt 3+ tt 4)+l3∗cos (tt 3)

Chọn chế độ đầu công tác luôn vuông góc với mặt đất
π
=>tt3 +tt4 = - 2

Từ đó ta có:

{
tt 1=atan 2( Py , Px)
 Pz – l1=l3∗sin ( tt 3 )−l 4
Px∗cos ( tt 1 )+ Py∗sin ( tt 1 )=l 2+l3∗cos (tt 3)

Mà cos(tt3)=√ 1−(sin(tt 3))2

{
tt 1=atan 2( Py , Px)
sin ( tt 3 )
 tan ( tt 3 )=
cos ( tt 3 )
l2=Px∗cos ( tt 1 ) + Py∗sin ( tt 1 )−l 3∗cos ( tt 3 )

π π
Vì tt3 +tt4 = - 2 => tt4= - 2 – tt3
 Các biến góc
tt1:
tt 1=atan 2( Py , Px )

l2:
l 2=Px∗cos ( tt 1 )+ Py∗sin ( tt 1 )−l3∗cos ( tt 3 )

tt3:
sin ( tt 3 )
tan ( tt 3 )=
cos (tt 3 )

Với cos(tt3)=√ 1−(sin(tt 3))2 và Pz – l 1=l 3∗sin ( tt 3 ) −l 4

tt4
π
tt4= - 2 – tt3

Tính toán bài toán động học vận tốc

Vận tốc dài:
Ta có Px = cos(tt1)*(l2 + l4*cos(tt3 + tt4) + l3*cos(tt3))
Py = sin(tt1)*(l2 + l4*cos(tt3 + tt4) + l3*cos(tt3))
Pz = l1 + l4*sin(tt3 + tt4) + l3*sin(tt3)
Vận tốc động cơ các khớp của robot:

Vận tốc góc:

Ta có công thức 5.32 (trang 174 sách Kỹ Thuật Robot):
0ω
4 = 0ω1 + R 1 1ω2 + R 2 2ω3 + R 33ω4

với

=>

=>

=> ma trận Jacobi:

Mô phỏng không gian hoạt động: