Upload
conglx
View
78
Download
35
Embed Size (px)
DESCRIPTION
d
Citation preview
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
LỜI NÓI ĐẦU
Ngày nay cùng với sự phát triển không ngừng trong các lĩnh vực cơ khí, điện tử, tin học thì sự tích hợp của ba lĩnh vực đó là cơ điện tử cũng phát triển và được coi là một trong những ngành mũi nhọn trong quá trình hiện đại hóa và công nghiệp hóa đất nước. Sản phẩm đặc trưng của cơ điện tử là ROBOT và CNC.
Trong khuôn khổ nội dung môn học Đồ án: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ em được nghiên cứu một trong hai sản phẩm đặc trưng của cơ điện tử đó là robot, cụ thể ở đây là robot bốn bậc tự do RRTR. Khả năng làm việc của robot thì có rất nhiều ưu điểm: Chất lượng và độ chính xác cao, hiệu quả và kinh tế cao, làm việc trong môi trường độc hại mà con người không thể làm được, trong các công việc mà đòi hỏi phải cận thận không được nhầm lẫn, thao tác nhẹ nhàng, tinh tế và chính xác nên cần có thợ tay nghề cao và phải làm việc căng thẳng suốt ngày thì robot có khả năng thay thế hoàn toàn …
Qua đồ án giúp em bước đầu làm quen với việc tính toán robot: Về cấu trúc động học của robot, cơ sở lý thuyết tính toán, thiết kế, lập trình mô phỏng hoạt động của robot.
Trong quá trình học môn Đồ án : THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ chúng em còn nhiều thiết sót, mong thầy chỉ bảo thêm cho em .
Em chân thành cảm ơn!
Sinh viên thực hiện:
LÊ XUÂN CÔNG
CHƯƠNG 1: THIẾT KẾ MÔ HÌNH 3D
Sử dụng phần mềm solidworks 2010 để thiết kế mô hình robot RRTR
GVHD: ĐỖ ĐỨC NAM Page 1
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
1. Chi Tiết các khâu
Đế
GVHD: ĐỖ ĐỨC NAM Page 2
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Khâu 1
Khâu 2
GVHD: ĐỖ ĐỨC NAM Page 3
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Cặp bánh răng truyền động khâu 2
GVHD: ĐỖ ĐỨC NAM Page 4
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Cặp bánh răng truyền động cho khâu 3
GVHD: ĐỖ ĐỨC NAM Page 5
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Khâu 3
GVHD: ĐỖ ĐỨC NAM Page 6
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Khâu 4 và tay kẹp
GVHD: ĐỖ ĐỨC NAM Page 7
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Tổng Thể robot
GVHD: ĐỖ ĐỨC NAM Page 8
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
PHẦN 2
GIẢI BÀI TOÁN ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC ROBOT SCARA
I. Bài toán động học thuận
1.1 Xác định ma trận chuyển vị thành phần mô tả vị trí của khâu thứ i so với khâu thứ i-1
Dạng tổng quát:
Quy ước viết tắt :
GVHD: ĐỖ ĐỨC NAM Page 9
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Ta có bảng thông số Danevit- Hatenberg của robot scara:
Bảng hệ tọa độ Danevit-Hatenberg
d A
1 0 a1 0
2 0 a2
3 0 0 0
4 0 0
Các ma trận Ai
Ma trận chuyển vị thuần nhất mô tả hướng và vị trí của các khâu:
Khâu 1 so với khâu cơ sở:
GVHD: ĐỖ ĐỨC NAM Page 10
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Khâu 2 so với khâu cơ sở:
Khâu 3 so với khâu cơ sở
Khâu 4 so với khâu cơ sở
Vậy tọa độ điểm E có dạng:
1.2 Hệ phương trình động học
GVHD: ĐỖ ĐỨC NAM Page 11
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Khâu công tác của robot có phương trình động học cơ bản có dạng:
Ta có TE = T4 suy ra:
GVHD: ĐỖ ĐỨC NAM Page 12
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Hệ phương trình động học :
II.Bài toán động học ngược
Nghiệm của bài toán động học ngược của robot Scara là nghiệm của phương trình :
(*)Với :
; là tọa độ của khâu công tác
Ta thấy (*) chỉ giải được khi A có dạng :
GVHD: ĐỖ ĐỨC NAM Page 13
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Chiếu vị trí của tay máy lên mặt phẳng Ox0y0 ta có
Ta phân tích :
Đặt (1)
Hệ phương trình tương đương :
Giải hệ phương trình ta được :
Với các hệ số A, B hoàn toàn xác định khi giá trịnh xác định.
Do đó
Lại có :
GVHD: ĐỖ ĐỨC NAM Page 14
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Vậy bộ nghiệm của phương trình(*) :
PHẦN III
THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA ROBOT
1. CÁC KHÁI NIỆM VỀ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA ROBOT
Vấn đề thiết kế quỹ đạo chuyển động liên quan mật thiết đến bài toán điều khiển robot di chuyển từ vị trí này đến vị trí khác trong không gian làm việc. Đường đi và quỹ đạo được thiết kế là các lượng đặt cho hệ thống điều khiển vị trí của robot. Do đó độ chính xác của quỹ đạo sẽ ảnh hưởng đến chất lượng di chuyển của robot.
Thông thường, quỹ đạo ở dạng đa thức bậc cao sẽ đáp ứng được các yêu cầu về vị trí, tốc độ, gia tốc ở mỗi điểm giữa 2 đoạn di chuyển.
Yêu cầu của thiết kế quỹ đạo là :
- Khâu chấp hành phải đảm bảo đi qua lần lượt các điểm trong không gian làm việc hoặc di chuyển theo một quỹ đạo xác định.- Quỹ đạo của robot phải là đường cong đảm bảo tính liên tục về vị trí trong một khoảng nhất định.- Không có bước nhảy về vận tốc, gia tốc.- Quỹ đạo thường là đường cong thông thường.
Trên thực tế hiên nay có nhiều quỹ đạo là dạng đường cong dạng :
- Đa thức bậc 2 :x(t)=a+bt+ct2
- Đa thức bậc 3 : x(t)=a+bt+ct2+dt3
- Đa thức bậc cao : x(t)=a+bt+……..ktn
Trong đồ án này em sử dụng dạng quỹ đạo là đa thức bậc 3 có dạng:
X(t)=a+bt+ct2+dt3
GVHD: ĐỖ ĐỨC NAM Page 15
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
2. THIẾT KẾ QUỸ ĐẠO ĐA THỨC BẬC BA
Từ vị trí ban đầu và hướng của tay robot, sử dụng phương trình động học ngược ta xác định các giá trị biến khớp tương ứng. Bài toán thiết kế quỹ đạo cho khớp là xác định đường biểu diễn của vị trí khớp (góc quay của khớp quay hoặc khoảng tịnh tiến của khớp tịnh tiến) theo thời gian khi di chuyển từ vị trí ban đầu qo
đến vị trí cuối cùng qc trong thời gian tc với q là biến khớp tổng quát. Quỹ đạo di chuyển của khớp giữa 2 vị trí sẽ thỏa mãn 4 điều kiện: vị trí ban đầu và vị trí cuối cùng, tốc độ tại vị trí ban đầu và vị trí cuối cùng. Do đó đa thức bậc 3 sẽ thích hợp cho quỹ đạo chuyển động của khớp robot:
(2.1)
Với các điều kiện đầu, cuối là:
(2.2a)
(Với i là biến khớp, i=1,2,3)
Trong thực tế, tốc độ tại vị trí ban đầu và vị trí cuối cùng của khớp bằng không:
(2.2b)
Đạo hàm phương trình (3.1) ta có:
(2.3)Sử dụng các điều kiện đầu và cuối (3.2) ta nhận được 4 phương trình sau:
(2.4)
GVHD: ĐỖ ĐỨC NAM Page 16
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Giải các phương trình trên ta được các hệ số:
Từ 4 hệ số trên ta sẽ có quỹ đạo dạng đa thức (3.1) rồi từ đó ta xác định vị trí của các khớp tại thời điểm bất kỳ. Những giá trị đó là tín hiệu đặt cho bộ điều khiển vị trí để truyển động khớp di chuyển đến vị trí tương ứng.
GVHD: ĐỖ ĐỨC NAM Page 17
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
3. VÍ DỤ THIẾT KẾ QUỸ ĐẠO ĐA THỨC BẬC BA CHO ROBOT SCARA
Bài toán: Thiết kế quỹ đạo chuyển động cho robot RRTR từ vị trí A đến vị trí
B trong khoảng thời gian 10s. Với điểm: A điểm B
Từ phương trình động học ngược ta tính được giá trị các biến khớp tại các điểm A, B
Tại điểm A
Tại điểm B
Theo (3.3) ta xác định được các hệ số của các khớp: Khớp 1
Suy ra:
Khớp 2
Ta có :
GVHD: ĐỖ ĐỨC NAM Page 18
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Khớp 3
Ta có :
Khớp 4
Phần IV. VÍ DỤ VỀ ĐỘNG HỌC 1. Động học thuận
Từ quỹ đạo của các biến khớp trên dùng maple ta vẽ được đồ thị vị trí và vận tốc góc của khâu thao tác
Hình 1.1 : Đồ thị Xe hình 1.2 :đồ thị Ye
GVHD: ĐỖ ĐỨC NAM Page 19
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Hình 1.3 : Đồ thị Ze hình 1.4 : vị trí khâu thao tác
Hình 1.5 Đồ thị Wx hình 1.6:đồ thị Wy
GVHD: ĐỖ ĐỨC NAM Page 20
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Hình 1.7: đồ thị Wz hình 1.8: đồ thị vận tốc góc 2. động học ngược Chọn khâu thao tác di chuyển theo phương trình
Sử dụng phần mềm maple ta xác định được đồ thị khâu thao tác và các khớp
Hình 2.1: Đồ thị khâu thao tác
GVHD: ĐỖ ĐỨC NAM Page 21
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Đồ thị và vận tốc các khớp
Hình 2.2: Đồ thị vị trí các khâu
Hình 2.3: Đồ thị vận tốc
GVHD: ĐỖ ĐỨC NAM Page 22
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
PHẦN V
KHẢO SÁT BÀI TOÁN ĐỘNG LỰC HỌC
1. CƠ SỞ LÝ THUYẾT
1.1. Biểu thức động năng và thế năng của rôbôt công nghiệp xác định từ
các ma trận Denavit – Hartenberg
1.1.1. Biểu thức động năng
Xét mô hình rôbôt có n bậc tự do, ma trận Denavit – Hartenberg chuyển toạ
độ một điểm từ hệ qui chiếu Ri về hệ qui chiếu R0 như sau:
Ti =Di = (3.1)
Hình 3.1
Gọi là véc tơ vị trí trọng tâm của khâu thứ i đối với hệ toạ độ khớp đặt tại
khớp thứ i. Khi đó vị trí trọng tâm khâu thứ i đối với hệ toạ độ cơ sở được xác
định theo công thức sau
GVHD: ĐỖ ĐỨC NAM Page 23
Khâu i
Ci
y0
x0
z0
xi
zi
yi-1
yi
zi-1
Oi
Khâu n
xi-1
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
(3.2)
Viết dưới dạng ma trận ta có
(3.3)
Suy ra vận tốc trọng tâm khâu thứ i
(3.4)
Toán tử sóng biểu diễn vận tốc góc khâu thứ i đối với hệ toạ độ cơ sở, theo
công thức (2.1) trang 118, [1]
(3.5)
(3.6)
Suy ra vận tốc góc khâu thứ i trong hệ toạ độ cơ sở
(3.7)
Biểu thức động năng của khâu thứ i
(3.8)
Trong đó
mi : khối lượng khâu thứ i
ICi : ma trận của tenxơ quán tính của khâu tứ i đối với khối tâm C i của
nó ở trong hệ toạ độ R0
GVHD: ĐỖ ĐỨC NAM Page 24
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Biểu thức động năng của hệ n vật rắn
(3.9)
1.1.2. Biểu thức thế năng
Biểu thức thế năng của khâu thứ i
(3.10)
Trong đó
(3.11)
Với gx, gy, gz là các thành phần của véctơ gia tốc trọng trường so với hệ toạ độ
cơ sở. Nếu hướng của gia tốc trọng trường trùng z0 thì gz = g, còn gx = gy = 0 .
Biểu thức tổng thế năng của hệ n khâu
(3.12)
1.2. Thiết lập phương trình vi phân chuyển động của rôbôt công nghiệp
bằng phương pháp Lagrange loại hai
GVHD: ĐỖ ĐỨC NAM Page 25
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Hình 3.2
Xét một hệ gồm n bậc tự do, ta sẽ dựa vào khái niệm ma trận Jacobi , thiết lập
dạng thức Lagrange loại hai cho hệ nhiều vật hôlônôm.
Ký hiệu các toạ độ suy rộng tối thiểu là
q =[q1, …, qn]T (3.13)
Vị trí của vật rắn Bi được xác định bởi vị trí của điểm định vị Ci và ma trận
côsin chỉ hướng Ai của nó.
rci =rci(q,t) ; Ai = Ai(q,t) (3.14)
Để đơn giản, sau đây ta chỉ xét hệ hôlônôm giữ và dừng. Khi đó ta sẽ có
rci = rci(q) ; Ai = Ai(q) (3.15)
Trạng thái vận tốc của vật rắn Bi được xác định bởi vận tốc khối tâm và vận
tốc góc của nó.
; (3.16)
GVHD: ĐỖ ĐỨC NAM Page 26
P
O0
z0
Oi On
Khâu i
Khớp 1
Khâu n
Ci
x0
y0
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Ma trận Jacôbi tịnh tiến và ma trận Jacôbi quay như sau
; (3.17)
Công thức vận tốc khối tâm vật rắn và vận tốc góc của nó có dạng
; (3.18)
Như phần trên ta có biểu thức động năng của khâu thứ i như sau
Ti = (3.19)
Biểu thức tổng động năng của hệ n khâu như sau
T = (3.20)
Với Ii là ma trận tenxơ quán tính của vật rắn thứ i đối với khối tâm C i của nó
trong hệ qui chiếu R0. Trong thực tế ta có thể tính Ii theo công thức sau:
Ii =AiIi(i)Ai
T (3.21)
Việc tính biểu thức có thể được thay bằng tính hay
. Trong đó, Ii(i) là ma trận của tenxơ quán tính khối của vật rắn
thứ i đối với khối tâm Ci của nó ở trong hệ qui chiếu động Cixiyizi gắn liền vào
vật rắn Bi, còn là véc tơ đại số của trong hệ qui chiếu Cixiyizi .
Kết hợp giữa (3.18) và (3.20), biểu thức động năng của hệ có thể được tính
theo công thức sau
T = (3.22)
GVHD: ĐỖ ĐỨC NAM Page 27
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
hay T = (3.23)
Ta đưa vào ký hiệu
M(q) = (3.24)
Biểu thức (3.24) gọi là ma trận khối lượng suy rộng. Khi đó biểu thức động
năng của hệ vật rắn có dạng như sau
T = (3.25)
Trong đó
M(q) = (3.26)
q = ; ; (3.27)
Biểu thức động năng (3.25) có thể viết lại như sau
T= (3.28)
Đạo hàm biểu thức động năng (3.28) theo các vận tốc suy rộng và các toạ
độ suy rộng qi ta sẽ có
;
(3.29)
Từ đó suy ra
GVHD: ĐỖ ĐỨC NAM Page 28
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
(3.30)
Thế các biểu thức trên vào phương trình Lagrange loại 2
(i =1,...,n) (3.31)
ta có
(i =1,...,n) (3.32)
Trong đó
là các lực có thế
là các lực không thế tác dụng vào khâu thứ
i
là lực suy rộng của các lực hao tán tác dụng vào khâu i
lực suy rộng của các lực phát động tác dụng vào khâu thứ i
Đưa vào ký hiệu
(3.33)
thì phương trình (3.32) trở thành
(i = 1,...,n)
(3.34)
Ký hiệu
GVHD: ĐỖ ĐỨC NAM Page 29
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
(3.35)
thì ta có phương trình sau
(i = 1,...,n)
(3.36)
Dùng ký hiệu ma trận
M(q) = [mij(q)]
= [cij( )]
Hệ phương trình vi phân chuyển động được viết lại như sau
M(q) + C + g(q) = τ(t) (3.37)
GVHD: ĐỖ ĐỨC NAM Page 30
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
2. THIẾT LẬP PHƯƠNG TRINH ĐỘNG LỰC HỌC
Chọn tọa độ suy rộng là :
Bảng mô tả vị trí trọng tâm, khối lượng, momen quán tính khối của từng khâu của robot :
Khâu Vị trí trọng tâm so với tọa độ gắn trên khâu
Khối lượng
Momen quán tính khối từng khâu
xGi yGi ZGi Ixx Iyy Izz Ixy Ixz Iyz
1 -a1+L1 0 0 M1 I1x I1y I1z 0 0 02 -a1+L2 0 0 M2 I2x I2y I2z 0 0 03 0 0 -L3 M3 I3x I3y I3z 0 0 04 0 0 -L4 M4 I4x I4y I4z 0 0 0
Theo kết quả của phần tính toán động học thuận ta có các ma trận DH biến đổi các khâu về khâu cơ sở như sau :
GVHD: ĐỖ ĐỨC NAM Page 31
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Từ các ma trận T
i
(i=1,2,3,4), ta xác định được tọa độ khối tâm của các khâu đối với
hệ tọa độ R
0
.
Vị trí khối tâm của khâu thứ nhất:
(3.1)
Đạo hàm biểu thức trên theo thời gian ở trong hệ quy chiếu cố định ta được vận tốc
khối tâm
GVHD: ĐỖ ĐỨC NAM Page 32
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Suy ra vận tốc khối tâm khâu thứ nhất:
(3.2)
Ma trận Jacobi tịnh tiến của khâu thứ nhất:
(3.3)
Vị trí khối tâm của khâu thứ 2:
(3.4)
Vận tốc khối tâm của khâu thứ 2:
(3.5)
Ma trận Jacobi tịnh tiến của khâu thứ 2:
GVHD: ĐỖ ĐỨC NAM Page 33
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
(3.6)
Vị trí khối tâm của khâu thứ 3:
(3.7)
Vận tốc khối tâm của khâu thứ 3:
(3.8)
Ma trận Jacobi tịnh tiến của khâu thứ 3:
(3.9)
Vị trí khối tâm của khâu thứ 4:
(3.10)
GVHD: ĐỖ ĐỨC NAM Page 34
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Vận tốc khối tâm của khâu thứ 4:
(3.11)
Ma trận Jacobi tịnh tiến của khâu thứ 4:
(3.12)
Từ các ma trận T
i
ta suy ra được các ma trận cosin chỉ hướng của các khâu là:
Ma trận cosin chỉ hướng của khâu thứ nhất:
Ma trận cosin chỉ hướng của khâu thứ 2:
Ma trận cosin chỉ hướng của khâu thứ 3:
GVHD: ĐỖ ĐỨC NAM Page 35
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Ma trận cosin chỉ hướng của khâu thứ 4:
Toán tử sóng véc-tơ vận tốc góc khâu thứ nhất:
(3.13)
Suy ra vec-to vận tốc góc trọng hệ R
0
là:
(3.14)
Ma trận Jacobi quay của khâu thứ nhất:
(3.15)
Toán tử sóng véc-tơ vận tốc góc khâu thứ 2:
(3.16)
GVHD: ĐỖ ĐỨC NAM Page 36
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Suy ra:
(3.17)
Ma trận Jacobi quay của khâu thứ 2:
(3.18)
Toán tử sóng của véc-tơ vận tốc góc khâu thứ 3:
(3.19)
Suy ra:
(3.20)
Ma trận Jacobi quay của khâu thứ 3:
(3.21)
GVHD: ĐỖ ĐỨC NAM Page 37
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Toán tử sóng của véc-tơ vận tốc góc khâu thứ 4:
(3.22)
(3.23)
Ma trận Jacobi quay của khâu thứ 4:
(3.24)
Các ma trận momen quán tính của các khâu, tính đối với các hệ tọa độ đi qua khối
tâm tương ứng và có các trục song song với các trục của hệ tọa độ R
i
Đối với khâu thứ nhất:
(3.25)
Đối với khâu thứ 2:
GVHD: ĐỖ ĐỨC NAM Page 38
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
(3.26)
Đối với khâu thứ 3:
(3.27)
Đối với khâu thứ 4:
(3.28)
Ma trận khối lượng xác định
GVHD: ĐỖ ĐỨC NAM Page 39
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Ma trận C xác định theo công thức : nên ta có :
Biểu thức động năng của hệ có dạng :
(3.29)
Biểu thức thế nắng của hệ có dạng :
(3.30)
Đạo hàm biểu thức theo qi ta sẽ thu được các lực suy rộng có thế như sau :
Xem như không có lực cản nhớt
(3.31)
Gọi là các lực (nếu đặt tại khớp tịnh tiến ) hoặc ngẫu lực (nếu nó đặt tại khớp
quay) trực tiếp làm cho khớp chuyển động . Như vậy ta ký hiệu véc tơ lực động cơ
tạo nên dịch chuyển ở các khớp động là :
GVHD: ĐỖ ĐỨC NAM Page 40
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
(3.32)
Thay vào phương trình :
Ta thu được hệ phương trình chuyển động vi phân của robot như sau :
Nếu viết dưới dạng ma trận thì thu được hệ phương trình như sau:
(3.33)
PHẦN V
THUẬT TOÁN THIẾT KẾ BỘ DIỀU KHIỂN
1. THIẾT KẾ BỘ ĐIỀU KHIỂN
Nhiêm vụ của bài toán điều khiển là tìm ra quy luật của lực/ mô men do các động cơ điện tạo ra tác dụng lên các khâu để đảm bảo robot chạy đúng theo quy luật qd(t) cho trước, nhằm thực hiện một số nhiệm vụ nào đó. Trên cơ sở chuyển động mong muốn qd(t) được định nghĩa trước và chuyển động hiện tại của robot được đo bởi các cảm biến đặt tại khớp, bọ điều khiển có nhiệm vụ đưa ra các lực/mômen cần thiết. các lực/mômen này tác động làm cho robot thực hiện chuyển động mong muốn một cách ổn định và chính xác. Sơ đồ khối của bộ điều khiển cho robot có
GVHD: ĐỖ ĐỨC NAM Page 41
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
dạng như hình 3.Từ phần trên ta có phương trình vi phân lagrange loại II mô tả mô hình vi phân của robot:
( , ) ( ) Mq C q q q g q τ
Trong đó q là tập các biến trạng thái của robot, là vecto đặt lực vào
các biến khớp. gọi quỹ đạo của các khâu mong muốn là (t), để đơn giản ta
giả thuyết rằng xác định tại mọi thời điểm và ít nhất có đạo hàm cấp hai.
Các tính chất của phương trình chuyển động robot:+, M(q) là ma trận đối xứng và xác định dương+, 2M C là mà trận đối xứng lệch
Để điều khiển chuyển động của robot chuyển động trong không gian
làm việc, cần xác định n thành phần lực của vecto lực để cho chuyển động
của q(t) gần hay trùng với chuyển dộng mong muốn của (t)
với mục tiêu này, ta có thể xem robot công nghiệp như một hệt thống mà có đầu vào là điện áp tạo các động cơ và đầu ra là chuyển động của các khớp mà ta có thẻ đo được nhờ các sensor đặt tại các khớp tương ứng.
Hình 3.Nhiệm vụ của bài toán điều khiển là cần phải tính toán lực điều khiển
tại các khớp từ chuyển động mong muốn và chuyển động hiện tại của robot
mà được thu từ các sensor để đưa các khâu về vị trí và quỹ đạo mong muốn.Phần điều khiển và robot được tích hợp theo sơ đồ sau:
Hình 4. Sơ đồ điều khiển
GVHD: ĐỖ ĐỨC NAM Page 42
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Với sơ đồ này , người sử dụng robot chỉ việc định nghĩa quỹ đạo mong muốn rồi sau đó robot sẽ tự động thực hiện chuyển động đó
Tùy thuộc vào từng ứng dụng cụ thể của robot mà bài toán điều khiển robot có thể phân thích thành 2 nhóm chính:
+ điều khiển điểm đến điểm mà không quan tâm đến quỹ đạo bàn kẹp+ điều khiển bàn kẹp chuyển động theo một quỹ đạo cho trước
- Điều khiển điểm đến điểmGiả sử bàn kẹp E đang ở một vị trí (xE,yE,zE) nào đó ứng với các tọa độ
khớp (i=1…n), nhiệm vụ của bộ điều khiển là phải đưa ra được điện áp đặt
vào các động cơ để bàn kẹp di chuyển đến đúng hay gần đến vị trí mong
muốn với sai lệch cho phép, tương ứng vị trí mong muốn của bàn kẹp
là các tọa độ khớp .
Sau đây là xét luật điều khiển đơn giản nhất đáp ứng được yêu cầu trên, đó là luật điều khiển PD cùng với bù trọng lực.Lực điều khiển được tính từ
các vị trí mong muốn (ứng với tọa đọ biến khớp ) và trạng thái chuyển
động của robot tại thời điểm hiện tại mà thu thập từ các sensor đặt tại các khớp:
(2.01)Trong đó e=qd-q là sai số mong muốn với thực tế ,KP,KD là các ma
trận đường chéo, xác định dương.Để khảo sát sự ổn định của luật điều khiển PD ta xét hàm Lyapunov:
1 1
2 2T T
P V e Me e K e (2.02)
(6)Vì ma trận M(q) đối xứng và xác định dương , ma trận KP được chọn là
ma trận đối xứng, xác định dương nên V>0 với mọi e, q #0Đạo hàm theo thời gian ta được:
1
2T T T
P V q Mq q Mq e K q
( ( )) Mq τ Cq g q
1
2T T T T
P D P V q K e K q q Cq q Mq e K q
= 12 0
2T T T
D D q K q q M C q q K q (2.03)
GVHD: ĐỖ ĐỨC NAM Page 43
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Căn cứ vào biểu thức trên ta thấy hàm V giảm khi khi q #0 và robot sẽ
đạt đến vị trí q=qd mong muốn. Hàm V =0 chỉ khi q =0 khi đó e=const
Ta có : (2.04)
Sơ đồ khối của hệ cùng với bộ điều khiên PD
Hình 5. Sơ đồ điều khiển vị trí
2. Mô Phỏng
Sử dụng matlab để mô phỏng
Các thông số động học :
a1 = 0,2m ; a2 = 0,15m ; d4 = 0,8m
L1 = 0,1m ; L2 = 0,08m ; L4 = 0,04m
Các thông số động lực học :
I1x = 0,01 ; I1y = 0,06 ; I1z = 0,16 (kg.m2)
I2x = 0 ; I2y = 0,05; I2z = 0,05;
I3x = 0 ; I3y = 0 I3z = 0,002;
I4x = 0 ; I4y = 0 ; I4z = 0,04; g =9,81m/s2
m1 = 9,5kg m2 = 5kg m3 = 2,6kg m4 = 1,8kg
Phương trình vi phân động lực học robot có dạng:
GVHD: ĐỖ ĐỨC NAM Page 44
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Luật điều khiển có dạng:
Với quỹ đạo của các khớp đã thiết lập trong phần thiết lập quỹ đạo. đảm bảo
điểm tác động cuối đi từ điểm A điểm B trong
thời gian 2(s).
Các hệ số khuếch đại tỉ lệ, tích phân và đạo hàm được lựa chọn như sau :
Mô hình bộ điều khiển PD
GVHD: ĐỖ ĐỨC NAM Page 45
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Trong đó :
, là tín hiệu quỹ đạo đặt và vận tốc đặt vào các khớp của robot đươc thiết lập
trong phần thiết lập quỹ đạo
q, v là các tín quỹ đọa và vận tốc phản hồi lại từ robot(quỹ đạo thực)
ở đây ta sử dụng các khối:
khối Gain dùng để khếch đại các tín hiệu đầu vào
khối mux chập các tín hiệu đơn thành các tín hiệu tổng hợp và tách thành nhiều tín hiệu
khối input, ouput đầu vào và ra của tín hiệu
khối hàm biểu diễn một hàm toán học khi có tín hiệu đi vào là các biến và tín hiệu đầu ra là giá trị của hàm
khối scope hiển thị tín hiệu đầu ra cảu quá trình mô phỏng theo thời gian.
khối thời gian đặt giá trị bằng 2(s)
3. Kết quả mô phỏng
GVHD: ĐỖ ĐỨC NAM Page 46
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Hình 3.1: Đồ thị vận tốc
GVHD: ĐỖ ĐỨC NAM Page 47
ĐA: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Hình 3.2: Đồ thị vị trí
GVHD: ĐỖ ĐỨC NAM Page 48