2.2 Tính toán động học ngược robot
3.1.1 Tính toán biến khớp
Căn cứ vào kiểu dáng và kích thước vùng làm việc của robot, bố trí vị trí phôi trong vùng làm việc theo sơ đồ sau:
Hình 3.1 :Sơ đồ chuyển động của robot giữa hai trạm cấp phôi
Nếu lấy điểm gốc của hệ quy chiếu cơ sở làm chuẩn mô tả, đối tượng mô tả là điểm P mút dụng cụ. Vì mút dụng cụ trong quá trình làm việc cần trùng với quỹ đạo gia công trên phôi nên có thể viết được quan hệ này dưới dạng phương trình vòng véc tơ như sau:
Theo phép chuyển đổi thuần nhất thế của khâu chấp hành là hàm của các biến khớp, mô tả bằng ma trận tổng hợp của phép chuyển đổi :
= X.E.R
∏
Trong đó: với i = với i= 1 n, là ma trận chuyển đổi giữa hệ toạ độ thứ i đến hệ i-1, xác định theo quy tắc Denavit-Hartenberg; n là số biến khớp (bậc tự do) của robot. Vị trí và hướng của khâu chấp hành được xác định từ quỹ đạo cho trước:
0 [
0 0 0 1
]
Trong đó: 0 ( 1 2 . 1 các biến khớp; n, s, a là các vec tơ chỉ phương; p là véc tơ chỉ vị trí; oxyz là hệ toạ độ gốc.
Ma trận chuyển đổi tổng hợp có dạng:
[
11 12
21 22
13 1
23 2
31 32
0 0 033 13 ]
Các thành phần với i,j = 1 3là các cosin chỉ phương của n,s,a; 1 2 3 lần lượt là các thành phần chiếu lên hệ oxyz của p. Do tính chất trực giao của các vec tơ chỉ phương, cho nên chỉ có ba thành phần trong các cosin chỉ phương độc lập. Vì vậy ta nhận được:
(3.1)
Giải hệ phương trình trên ta được giá trị các biến khớp . Tuy nhiên đây là bước thường gặp nhiều khó khăn nhất vì không phải với cấu trúc robot nào cũng có lời giải dưới dạng giải tích. Dưới đây giới thiệu phương pháp số dể thực hiện việc này . Do đặc điểm tham gia và mức độ tham gia của các động cơ trên cánh tay vào việc kẹp hoặc nhả phôi ở các tư thế khác nhau là hoàn toàn khác nhau nên bài toán (3.1) cần giải lặp lại trước khi thao tác bàn tay.Để vẽđược đặc tính chuyển vị đầy đủ với chi phí tính toán nhỏ có thể nội suy lời giải qua một số điểm chốt, sử dụng đặc tính chuyển vị bậc ba dạng đa thức với một giải thuật số có tính tổng quát để giải bài toán động học ngược hoàn toàn thỏa mãn được các yêu cầu này.
Trên hình 3.1 lần lượt thiết lập các ma trận chuyển trục giữa robot đến hai trạm A và B như sau:
( ( ( ( ( (
Vậy với bàn tay chuyên dụng lấy vật có kích thước ( 70mm ) và bố trí
trục zv trùng với trục của phôi, hai ma trận thể hiện cách gá vật biểu diễn theo kích thước chi tiết như sau:
Hình 3.2: Hệ quy chiếu đồ gá và hệ quy chiếu vật trên trạm B Vì mô tả chính xác ngay điểm trọng tâm phôi nằm trong bàn tay khi kẹp nênkhông cần đến ma trận R như trong trường hợp tool cần di chuyển
theomộtquỹ tích trên phôi, gốc Ov của phôi trên trạm A được mô tả ngay so với gốc đồ gá OA bằng ma trận EA. Ở trạng thái mang phôi trục x6 của bàn tay trùng với trục xv của phôi.
( [
]
3.2
Tương tự ma trận gá vật trên trạm B với các kích thước thực sẽ là : ( [
]
Như vậy tọa độ của tâm vật trên trạm A và trên trạm B tính theo gốc hệ quy chiếu của robot sẽ là:
( ( ( (
( ( ( ( Vậy ở trạng thái gắp hoặc nhả vật bàn tay phải lấy hai điểm này làm vị trí đầu sau đó ấn trục cẳng tay xuống theo phương thẳng đứng 18 (mm) để điều khiển kẹp hoặc nhả, khi đó tâm bàn tay không chuyển động mà chỉ có cam trong trục cổ tay chuyển động (cam 3, 4, 5 trượt tương đối với nhau), sau khi đã kẹp (hoặc đã nhả vật) cần nâng trục cẳng tay lên 18 (mm) ngược hướng ban đầu, nhấc bàn tay lên khỏi mặt đồ gá của trạm hiện thời 50 (mm) và di
chuyển sang trạm kia theo đường thẳng.
Căn cứ vào toàn bộ quá trình trên cần lấy toàn bộ chu trình tối thiểu cần 9 điểm key mô tả như sau:
P1: (Home) đây là điểm khởi xuất làm việc từ tư thế nghỉ mở kẹpP1
P2: Bàn tay dừng trên trạm A với vật đã được định vị đúng cách vật 40(mm)
P3: Bàn tay đi xuống 40 mm chạm vật.
P4: Bàn tay được ấn xuống 18 (mm) theo phương thẳng đứng kể từ tư thế mô tả bởi P3 nhằm kẹp chặt vật đã định vị đúng.
P5: nhấc thẳng đứng bàn tay 58 (mm) nhằm tránh va đập với đồ gá.
P6: chuyển động theo đường thẳng tới vị trí treo trên trạm B cách mặt đặt vật 40 (mm) chuẩn bị hạ vật thẳng đứng.
P7: hạ vật thẳng đứng xuống 40mm vật ở trạm B lấy điểm tựa (kẹp chưa mở).
P8: bàn tay được ấn xuống 18 (mm) theo phương thẳng đứng kể từ tư thế mô tả bởi P7 nhằm mở kẹp nhả vật vào vị trí đã định.
P9: treo bàn tay về điểm p6 như trước khi hạ vật xuống trạm B để tránh va đập.
P1: về lại vị trí home hoàn thành chu kỳ thao tác.
Tất cả 9 điểm nói trên cần mô tả trong hệ quy chiếu O0 gắn với giá của robot, trong đó điểm p3 chính là tính theo (3.17) và điểm p7 chính là
3.3
tính theo (3.3).
[
] ; [
]
Khi bàn tay được ấn xuống 18 (mm) theo phương thẳng đứng kể từ tư thế mô tả bởi P3 nhằm kẹp chặt vật đã định vị đúng (P4) và bàn tay được ấn xuống 18 (mm) theo phương thẳng đứng kể từ tư thế mô tả bởi P7 nhằm mở kẹp nhả vật vào vị trí đã định.(P8).
[
] ; [
]
Tại hai điểm treo phôi P2 và P6 có thể suy ra từ điểm lấy phôi P2 hoặc nhả phôi p6 bằng cách cộng vào tọa độ lượng treo phôi 40(mm), cụ thể:
[
] ; [
]
Ta có điểm P6 tay kẹp chuyển động theo đường thẳng tới vị trí treo trên trạm B cách mặt đặt vật 40 (mm) chuẩn bị hạ vật thẳng đứng.trong khi đó điểm P9 treo bàn tay về điểm p6 như trước khi hạ vật xuống trạm B . Vì vậy ta có thể dễ dàng suy luận được P6 p9 . Tương tự ta cũng có P2 P5.
Điểm P1 là tư thế home của robot là điểm xác định bởi bộ tọa độ suy rộng như sau:
( (-2.10^-07 , 0.073209,-0.01461, 0.000641,-0,00641, - 3,14945)
Các điểm keypoint nói trên được thay vào hệ phương trình (2.3) để xác định bộ tọa độ suy rộng tương ứng, sử dụng phương pháp GRG để giải bài toán cho từng điểm nói trên sau đó nội suy bậc ba qua các điểm chốt. Do ràng buộc trực giao nên chỉ có ba phần tử tạo thành ba đỉnh của một hình tam giác bất kỳ trong ma trận cosin chỉ hướng là độc lập tuyến tính.Ta chọn các đại lượng đại diện cho ràng buộc định hướng của trục bàn tay gồm ny, nz, nz và các số hạng đảm bảo định vị của bàn tay bao gồm px, py, pz. Hệ phương trình sẽ giải được rút gọn lại với quy mô 6x6.
Thể hiện bài toán lên solver với các giới hạn vật lý của robot cụ thể như sau:
-3,23 3,23 -0.7 1,62 -3,63 1,15
-6,1 6,1 -2,36 2,36
-6,1 6,1
H3.3 Kết quả bài toán động học ngược giải bằng phương pháp GRG tại điểm P2.
H3.4 Kết quả bài toán động học ngược giải bằng phương pháp GRG tại điểm P3.
Kết quả của bài toán ngược tại mỗi điểm keypoint cho thấy như sau:
Bảng 3.1: Kết quả bài toán ngược tại các điểm keypoint.
Điểm Mục tiêu
P1 -2.E-07 0.073209 -0.0146 0.000641 -0,0064 -3,1415 1,99E-12 P2 1,6E-06 -1,1766 0,8219 -3,1415 -0,3546 -3,1415 3,7E-11 P3 4,4E-06 -1,1567 0,79042 -3,1415 -0,3661 -3,1415 7,4E-11 P4 1,7E-06 -1,1476 0,7765 -3,145 -0,3711 -3,1415 6,4E-12 P5 1,6E-06 -1,1766 0,8219 -3,14115 -0,3546 -3,1415 3,7E-11 P6 -1,0517 -0,7616 -0,9771 0,9730 -2,3736 1,5677 6,5E-11 P7 -1,5174 -0,5137 -0,8655 0,0628 -1,7001 -1,5708 5,9E-13 P8 -1,5161 -0,5175 -0,8564 0,06437 -1,7033 -1,5708 8,2E-11 P9 -1,0517 -0,7616 -0,9771 0,9730 -2,3736 1,5677 6,5E-11 Thể hiện kết quả chuyển vị dưới dạng một đặc tính điều khiển biến thiên theo thời gian với các mốc thời gian mô tả trên trục hoành bao gồm:
0-4 (s) Từ home đến P1 (đoạn 1 trên đồ thị).
4-6 (s) Từ P1 đến P2 (đoạn 2 trên đồ thị).
6-8 (s) Kẹp chặt vật, ấn bàn tay 18 (mm) (đoạn 3 trên đồ thị).
8-10 (s) Nhấc bàn tay thẳng đứng 58 (mm) (đoạn 4 trên đồ thị).
10-16 (s) Chạy đến trạm B, treo cao 40 (mm) (đoạn 5 trên đồ thị).
16-18 (s) Định vị vật trong bàn tay nhưng chưa kẹp (đoạn 6 trên đồ thị).
18-20 (s) Nhả vật kẹp, ấn bàn tay 18 (mm) (đoạn 7 trên đồ thị).
20-22 (s) Nhấc bàn tay thẳng đứng 40 (mm) (đoạn 8 trên đồ thị).
22-26 (s) Về vị trí home (đoạn 9 trên đồ thị).
Hình 3.5: Đồ thị chuyển vị q1 trong một chu kì
Hình 3.6: Đồ thị chuyển vị q2 trong một chu kì
-1.80E+00 -1.60E+00 -1.40E+00 -1.20E+00 -1.00E+00 -8.00E-01 -6.00E-01 -4.00E-01 -2.00E-01 0.00E+00 2.00E-01
0 5 10 15 20 25 30
q1
-1.4 -1.2 -1 -0.8 -0.6 -0.4 -0.2 0 0.2
0 5 10 15 20 25 30
Series1
Hình 3.7: Đồ thị chuyển vị q4 trong một chu kì
Hình 3.8: Đồ thị chuyển vị q5 trong một chu kì
-4 -3.5 -3 -2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5
0 5 10 15 20 25 30
Series1
-2.5 -2 -1.5 -1 -0.5 0
0 5 10 15 20 25 30
q5
Hình 3.10: Đồ thị chuyển vị q6 trong một chu kì
Để thuận lợi cho điều khiển, nếu xác định đồ thị gia tốc không có bước nhảy nó cần có bậc tối thiểu là bậc ba với mỗi phân tố trong trường hợp tổng quát. Tính theo mức đó trên cơ sở quan hệ đạo hàm vận tốc cần có dạng bậc 4 và chuyển vị có dạng đa thức bậc 5, một đa thức bậc 5 đầy đủ có 6 ẩn cần dựa vào ba loại điều kiện biên trong đó:
- Điều kiện đi qua dùng cho hai đầu.
- Điều kiện vận tốc liên tục ( điều kiện liên tục tiếp tuyến hay liên tục loại một) áp dụng cho hai đầu.
- Điều kiện liên tục gia tốc áp dụng cho hai đầu.
Xét ba điểm lần lượt là : ( ( ( và gọi là hệ số góc của hai dây cung nối ba điểm đó, công thức tính các hệ sốgóc các dây này cho bởi:
-4 -3 -2 -1 0 1 2
0 5 10 15 20 25 30
q6
Xét hai tình huống xảy ra :
- Nếu ( ( → - Nếu ( ( → 0
Trên cơ sở đó xây dựng bảng hệ số góc chuyển tiếp như trong bảng sau:
F( ) F( ) F( ) F( ) F( ) F( )
4,5E-07 -0,3124 0,2091 -0,785 -0,0087 0 1,4E-06 9,9E-03 -0,0157 0 -5.8E-03 0 -1,4E-06 4,5E-03 -6,9E-03 0 -5,5E-03 0
-5E-08 -0,0145 0,00227 0 0,0112 0
-0,1752 0,06916 -0,2998 0,6857 -0,3365 2,3546 0,2328 0,1239 0,0558 -0,4551 0,3367 -0,5230 6,5E-04 -1.9E-03 4,5E-03 7,8E-04 -1,6E-03 0
0,2322 -0,2298 -0,0603 0,4543 -0,3351 1,5692 0.2629 0,2087 0,2406 -0,2430 0,5918 -1,1773
Bảng 3.2: Hệ số góc tính toán tại các điểm chuyển tiếp trên đồ thị
Xét ba điểm theo thứ tự là: ( ( ( . đa thức bậc năm đầy đủ sau đây là hai phân tố lần lượt đi qua hai điểm , và .
(
Trong đó u là thời gian quy đổi từ t là thời gian thực, cụ thể như sau : ( là phân tố đi qua hai điểm ( ( công thức đổi biến .
trong đó vì t ( nên u ( . Với đọan còn lại :
(
Xét mô hình chuyển tiếp trơn giữa hai đoạn bậc năm nói trên, toàn bộmô hình gồm 12 ẩn số cần có một lượng phương trình độc lập tương ứng:
- Điều kiện đi qua (sử dụng dữ liệu bảng 3.1):
(
(
(
( Điều kiện liên tục vận tốc tại điểm chuyển tiếp (điều kiện liên tục , dữ liệu bảng 3.2):
(
( (
( Điều kiện liên tục gia tốc tại điểm chuyển tiếp:
(
(
(
( Tập hợp tất cả các phương trình từ (3.20) đến (3.31) cho phép xác định chính xác 12 ẩn giả định và vẽ được các đặc tính chuyển vị, vận tốc, gia tốc trơn đều trong khoảng thời gian ( .
Do tính chất của mô hình sử dụng biến thời gian quy đổi nên thay vì kích thước hệ 12.12 thì ở đây hệ chỉ có kích thước 6.6, bằng một nửa so với mô hình gốc.