Tải bản đầy đủ
5 Tính toán động học Robot

5 Tính toán động học Robot

Tải bản đầy đủ

Hình 2.21: Mô hình và hệ tọa độ Robot ba bậc tự do
Trong đó các tham số Denavit – Hartenberg (DH) , d i, ai, được xác định
như sau :
: góc quay quanh trục Zi-1 để trục Xi-1 chuyển đến trục Xi (Xi’ // Xi).
di : đoạn dịch chuyển tịnh tiến dọc theo trục Zi-1 để dịch trục Xi-1 đến Xi.
ai : đoạn dịch chuyển tịnh tiến dọc theo trục Xi-1 để dịch trục Zi-1 đến trục Zi.
: góc quay quanh trục Xi để trục Zi-1 chuyển đến trục Zi.
Ta có bảng thông số Denavit - Hartenberg (DH):
Bảng 2.1: Bảng thông số Denavit - Hartenberg (DH)
Khâu

ai

di

1

a1

d1

2

0

a2

0

3

0

a3

0

Khi đó ta có phương trình động học của Robot được xác định như sau:
2.5.1 Phương trình xác định vị trí khâu thao tác (bàn kẹp) của Robot (Bài
toán động học thuận)
Trên cơ sở gắn hệ trục tọa đã ấn định cho tất cả các khâu liên kết của
Robot như trên, ta có thể thiết lập mối quan hệ giữa các hệ tọa độ nối tiếp nhau
(n-1), (n) bởi các phép quay và tịnh tiến sau đây:

46



Quay quanh Zn-1 một góc .



Tịnh tiến dọc theo Zn-1 một khoảng dn.



Tịnh tiến dọc theo Xn-1= Xn một đoạn an.



Quay quanh Xn một góc xoắn .

Bốn phép biến đổi thuần nhất này thể hiện quan hệ của hệ tọa độ thuộc
khâu thứ n so với hệ tọa độ khâu thứ n-1 và tích của chúng được gọi là ma trận A:
An= Rot(z,θ)Trans(0,0,d)Trans(a,0,0)Rot(x,α)
An=
An=Như vậy, sử dụng phương pháp ma trận Denavit - Hartenberg ta có ma
trận chuyển tọa độ từ hệ i (Ox iyizi) về hệ i-1 (Oxi-1yi-1zi-1) ở dạng tổng quát
là:
=
(Với i=[1, n])
Dựa vào bảng thông số DH ta có:


Ma trận mô tả hướng và vị trí của O1X1Y1Z1 đối với O0X0Y0Z0 là:
A1 =



Ma trận mô tả hướng và vị trí của O2X2Y2Z2 đối với O1X1Y1Z1 là:
A2 =



Ma trận mô tả hướng và vị trí của O3X3Y3Z3 đối với O2X2Y2Z2 là:
A3=

Quy ước viết tắt các hàm lượng giác như sau:
C1 =

C2 =

C3 =

S1 =

S2 =

S3 =

C12 = S12 =
Ta có:
A1 = A2 =
A3 =

47

Khi đó, ma trận mô tả vị trí và hướng của khâu thao tác cuối đối với hệ tọa
độ gốc O0X0Y0Z0 là:
T3= A1A2A3 =
=
=
Vậy tọa độ điểm P theo trục OX0Y0Z0 là:
(I)
2.5.2 Bài toán ngược xác định thông số động học Robot
Để xác định các góc quay , , ta tiến hành giải hệ phương trình (I), coi tọa
độ điểm P là tham số.
Bình phương hai vế của hệ phương trình (I) sau đó cộng từng vế hai
phương trình, ta có:
++= ++2
=a
Từ (1.1) và (1.2) suy ra:
Giải hệ phương trình trên với biến C1 và S1 ta có:
Để hệ phương trình có nghiệm thì và phải bằng 0:
=
Từ (1.2) và (1.3) suy ra:

Giải hệ phương trình ta được:
=a
2.6 Mô phỏng chương trình trên OpenGL
48

Sau khi thực hiện xong các bước ở trên như khởi tạo một Project MFC và
include các gói thư viện của OpenGL em đã xây dựng được chương trình mô
phỏng hoạt động của Robot với các giao diện được đưa ra ở chương 3.

49