1.

Sơ đồ động của robot, đặt các hệ tọa độ, chọn kích thước các
khâu và giá trị biến khớp, đưa ra bảng thông số D-H:
Đề bài: Robot 4 khâu 6 bậc tự do
a) Sơ đồ động robot, đặt các hệ tọa độ, chọn kích thước các khâu và giá trị
biến khớp:

Robot có các khớp theo thứ tự sau: trụ, quay, quay, trụ.
Kích thước các khâu như trên hình.
𝜋
Giá trị các biến khớp: 0 ≤ 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 ≤ ; 1 ≤ 𝑑1 , 𝑑4 ≤ 2
4

b) Đưa ra bảng thông số D-H:

 Bảng D-H:
Khâu 𝜃 d a 𝛼
1 𝜃1 d1 0 𝜋
2
2 𝜃2 0 1 0
3 𝜋 0 0 𝜋
𝜃3 − −
2 2
4 𝜃4 d4 0 0

2. Giải bài toán động học thuận:

Ma trận biến đổi giữa các khâu:
cos⁡(𝜃1 ) 0 sin⁡(𝜃1 ) 0
A1 = ( sin⁡(𝜃1) 0 −cos⁡(𝜃1 ) 0
)
0 1 0 𝑑1
0 0 0 1
cos⁡(𝜃2 ) −sin⁡(𝜃2 ) 0 cos⁡(𝜃2 )
A2= ( sin⁡(𝜃2 ) cos⁡(𝜃2) 0 sin⁡(𝜃2 )
)
0 0 1 0
0 0 0 1
cos⁡(𝜃3 − 𝜋⁄2) 0 −sin⁡(𝜃3 − 𝜋⁄2) 0
A3= ( sin⁡(𝜃3 − 𝜋⁄2) 0 cos⁡(𝜃3 − 𝜋⁄2) 0)
0 −1 0 0
0 0 0 1
cos⁡(𝜃4 ) −sin⁡(𝜃4 ) 0 0
A4= ( sin⁡(𝜃4 ) cos⁡(𝜃4 ) 0 0
)
0 0 1 𝑑4
0 0 0 1

Ta có:
T4 = A1*A2*A3*A4
𝑛𝑥 𝑠𝑥 𝑎𝑥 𝑃𝑥
𝑛𝑦 𝑠𝑦 𝑎𝑦 𝑃𝑦
Và T4 = [ ]
𝑛𝑧 𝑠𝑧 𝑎𝑧 𝑃𝑧
0 0 0 1

 𝑛𝑥 = ⁡ cos(𝜃1 ) ∗ cos(𝜃2 ) ∗ cos(𝜃4 ) ∗ sin(𝜃3 ) − sin(𝜃1 ) ∗ sin(𝜃4 ) + cos(𝜃1 ) ∗ cos(𝜃3 ) ∗ cos(𝜃4 ) ∗ sin⁡(𝜃2 )⁡
𝑛𝑦 = ⁡ cos(𝜃2 ) ∗ cos(𝜃4 ) ∗ sin(𝜃1 ) ∗ sin(𝜃3 ) + cos(𝜃1 ) ∗ sin(𝜃4 ) + cos(𝜃3 ) ∗ cos(𝜃4 ) ∗ sin(𝜃1 ) ∗ sin⁡(𝜃2 )
𝑛𝑧 = ⁡ − cos(𝜃2 + 𝜃3 ) ∗ cos⁡(𝜃4 )
𝑠𝑥 = ⁡ −⁡𝑐𝑜𝑠(𝜃4 ) ∗ 𝑠𝑖𝑛(𝜃1 ) ⁡ − ⁡𝑐𝑜𝑠(𝜃1 ) ∗ 𝑐𝑜𝑠(𝜃2 ) ∗ 𝑠𝑖𝑛(𝜃3 ) ∗ 𝑠𝑖𝑛(𝜃4 )⁡ − ⁡𝑐𝑜𝑠(𝜃1 ) ∗ 𝑐𝑜𝑠(𝜃3 ) ∗ 𝑠𝑖𝑛(𝜃2 ) ∗ 𝑠𝑖𝑛(𝜃4 )
𝑠𝑦 = ⁡cos(𝜃1 ) ∗ cos(𝜃4 ) ⁡ − ⁡cos(𝜃2 ) ∗ sin(𝜃1 ) ∗ sin(𝜃3 ) ∗ sin(𝜃4 )⁡ − ⁡cos(𝜃3 ) ∗ sin(𝜃1 ) ∗ sin(𝜃2 ) ∗ sin(𝜃4 )
𝑠𝑧 = ⁡ cos(𝜃2 + 𝜃3 ) ∗ sin⁡(𝜃4 )
𝑎𝑥 = ⁡ cos(𝜃2 + 𝜃3 ) ∗ cos(𝜃1 )
𝑎𝑦 = ⁡⁡ cos(𝜃2 + 𝜃3 ) ∗ sin⁡(𝜃1 )
𝑎𝑧 = ⁡𝑠𝑖𝑛(𝜃2 ⁡ + ⁡ 𝜃3 )
𝑃𝑥 = ⁡cos(𝜃1 ) ∗ (cos(𝜃2 )⁡ + ⁡d4 ∗ cos(𝜃2 ⁡ + ⁡ 𝜃3 ))
𝑃𝑦 = ⁡sin(𝜃1 ) ∗ (cos(𝜃2 )⁡ + ⁡d4 ∗ cos(𝜃2 ⁡ + ⁡ 𝜃3 ))
{ 𝑃𝑧 = ⁡d1⁡ + ⁡sin(𝜃2 )⁡ + ⁡d4 ∗ sin(𝜃2 ⁡ + ⁡ 𝜃3 )

3. Không gian làm việc của robot:

- X-Y:

- Y-Z:
- X-Z:

- 3D: