ĐỒ án ROBOT CÔNG NGHIỆP SONG SONG

Đồ án robot công nghiệp

Đại học Thái Nguyên
Trường DHKTCN

Cộng hòa xã hội chủ nghĩa Việt Nam
Độc Lập – Tự Do- Hạnh Phúc
------------o0o----------NỘI DUNG ĐỒ ÁN

TÍNH TOÁN, ĐIỀU KHIỂN VÀ THIẾT KẾ ROBOT TRIPOD
Giáo viên hướng dẫn : Phạm Thành Long
Sinh viên thực hiện : Phạm Văn Biên
MSSV
: K125520114053
Lớp
: K48-CĐT.01
Ngày giao đề
: 08/09/2015
Ngày hoàn thành
: 23/12/2015

Trưởng bộ môn

Giáo viên hướng dẫn

Thái Nguyên, ngày 24 tháng 12 năm 2015

SVTH: Phạm Văn Biên
Long

GVHD: Phạm Thành

Đồ án robot công nghiệp

Mục lục

2

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
Chương I: Tổng quan về các bài toán thiết kế và sử dụng Robot
I.

Giới thiệu

Ngày nay, các kỹ thuật robot với sự hỗ trợ của máy tính đã đáp ứng được
độ chính xác cao, thời gian thu nhận và xử lý các tín hiệu nhanh chóng, tin
cậy, đã làm tăng năng suất lao động, hạn chế các tai nạn và độc hại cho con
người… Tuy nhiên, loại robot nối tiếp hiện đang được sử dụng trong nhiều
lĩnh vực đã bộc lộ nhiều nhược điểm như tính linh hoạt thấp, tốc độ xử lý và
khả năng đáp ứng không cao, độ cứng vững cũng như độ chính xác chưa
đảm
bảo. Để khắc phục phần nào các nhược điểm trên, một loại robot mới đã ra
đời, đó là robot (hay còn gọi là tay máy) song song.
Khác hẳn với robot nối tiếp là loại robot liên tiếp có kết cấu hở được liên kết
với các khâu động học và được điều khiển tuần tự hoặc song song thì

robot song song là cơ cấu vòng kín trong đó khâu tác động cuối được liên kết
với nền bởi ít nhất là hai chuỗi động học độc lập . Robot song song có được
những ưu điểm sau: độ cứng vững cơ khí cao, khả năng chịu tải cao, gia tốc
lớn, khối lượng động thấp và kết cấu đơn giản. Với những ưu điểm trên, robot
song song đã được ứng dụng trong rất nhiều lĩnh vực như y học, thiên văn
học, trắc địa, máy mô phỏng, các máy công cụ…
Trong hoạt động sản xuất, những robot công nghiệp thường có hình dạng
của “cánh tay cơ khí”, mô phỏng theo những đặc điểm cấu tạo cơ bản
của
cánh tay người. Vì tinh thần phát triển các hệ thống cơ khí là thực hiện các
thao tác giống con người nên nó không tự nhiên, gò ép nhằm sử dụng
các
chuỗi động học nối tiếp vòng hở, cấu trúc này được gọi là tay máy nối tiếp.
Cấu trúc này có ưu điểm là vùng làm việc trải rộng, có tính linh hoạt, khéo
léo
như tay người nhưng khả năng nâng tải của nó thấp, độ bền thấp và độ
chính
xác chưa cao do các khớp nối trên tay máy cồng kềnh, khối lượng lớn, khi
chuyển động với tốc độ cao thì nó bị rung và lắc.
Vì thế, đối với các ứng dụng mà mục tiêu quan trọng nhất là khả năng

3

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
nâng tải lớn, thực hiện động lực học tốt và định vị chính xác thì rất cần một

sự

thay thế tay máy nối tiếp truyền thống. Để tìm ra

các giải pháp khả thi, các
nhà khoa học đã quan sát thế giới sinh vật và nhận thấy rằng thân hình của
các loài thú có khả năng nâng chở vật nặng có được độ ổn định, vững chắc
hơn trên nhiều chân song song với nhau so với loài người đứng trên hai
chân,loài người cũng sử dụng các cánh tay, ngón tay kết hợp với nhau để
nâng vật nặng và thực hiện các công việc đòi hỏi độ chính xác như khi viết,
loài
người dùng ba ngón tay tác động song song cùng một lúc. Tóm lại, chúng ta
có thể đưa ra kết luận rằng các tay máy có khâu tác động cuối được gắn với
đất bằng nhiều chuỗi động học có các bộ tác động được gắn song song với
nhau sẽ có được độ cứng vững lớn hơn và khả năng định vị tốt hơn. Và dựa
vào nhận xét trên, các nhà khoa học đã tiến hành nghiên cứu một loại cấu trúc
cơ khí mới, đó là cơ cấu song song. Cơ cấu song song là cơ cấu gồm có bệ cố
định và tấm dịch chuyển được gắn kết với bệ cố định thông qua các
chuỗi
truyền động nối tiếp
Cơ cấu chấp hành song song có mặt trong nhiều ứng dụng, các bộ mô hình
máy bay, các khung đỡ kiến trúc có khớp nối điều chỉnh, các máy khai thác
mỏ... Gần đây, chúng còn được phát triển thành cụm gia cong cơ khí nhiều
bậc tự do có độ chính xác cao, tốc độ cao.
II.

Bài toán động học thuận tay máy
Đa số cơ cấu chấp hành song song 6 bậc tự do có chứa 6 nhánh mở rộng. Các
cơ cấu chấp hành song song này có ưu điểm là độ cứng vững cao, quán tính
thấp, và khả năng tải trọng lớn. Tuy nhiên nhược điểm là không gian làm việc

hữu dụng nhỏ và khó thiết kế. Đối với cơ cấu chấp hành song song 3 bậc tự
do ưu điểm là dễ thiết kế, tính toán đơn giản hơn. Tuy nhiên nó có nhiền han
chế như không gian làm việc hạn chế, độ cứng vững, quán tính, và khả năng
tải trọng kém hơn so với cơ cấu song song 6 bậc tự do.

4

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
1. Nội dung bài toán
1.1

Đầu vào:

Đầu vào là các thông số liên quan đến khớp, cho các thông số góc θ1 ,θ 2 của
các chân A,B,C
1.2

Đầu ra:

Xác định hướng và vị trí của tâm tấm di động hay chính là
góc quay
 cβ .cγ
 cβ .sγ

 − sβ

px , p y , pz

và các

α , β , γ của ma trận RRPY (RPY= Roll-Pith-Yaw)

sα .sβ .cγ − cα .sγ
sα .sβ .sγ + cα .cγ
sα .cβ

cα .sβ .cγ + sα .sγ 
cα .sβ .sγ − sα .cγ 

cα .cβ

= RRPY

2. Mục đích bài toán

Xác định hướng và vị tri tâm tấm di động
3. Cách giải bài toán.

Đối với cơ cấu chấp hành song song (đã cho ở trên) thường là cơ cấu đối
xứng. Để tính toán thiết kế hay phân tích cơ cấu trên điều đầu tiên chúng ta
cần phải xác định:
a
b
c
d

Cấu trúc của cơ cấu song song là cơ cấu phẳng, cầu hay không gian.
Loại cấu trúc là đối xứng hay không đối xứng
Số bậc tự do của cơ cấu
Loại khớp có trong cơ cấu

Để phân tích được động học robot song chúng ta cần trải qua các bước sau:
Bước 1: Mô hình hóa động học robot song song
Bước 2: Xác định các thông số liên




Hệ tọa độ động
Hệ tọa độ cố định
Tham số kết cấu tấm động
5

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp




Thâm số tấm tĩnh (cố định)
Tham số vị trí và hướng

Tham số chân

Bước 3: Viết phương trình vòng vecto kín cho các chân
Bước 4: Gán các thông số liên quan đã xác định ở trên vào phương trình vòng
vecto
Bước 5: Tính toán các phương trình đã xác định ở trên
3.1

Minh họa với robot song song

Cho robot như hình vẽ:

6

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
Minh họa với 1 chân C

Phương trình vòng vecto kin :

O0 C + CC1 = O0 P + R RPY PC1

⇔ l = t + RRPY n − m

Khai triển phương trình vòng vecto ta có được hệ phương trình:

7

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
 − l. cos θ 1 
 0 =


 l. sin θ 1 

t x 
 − n. cos θ 2   C x 
 

 − C 
0
t y  + RRPY .
  y
t y 
 n. sin θ 2   C z 
 

Các chân còn lại làm tương tự ta sẽ được 2 hệ phương trình tương ứng với 2
chân còn lại. Khai triển 3 hệ phương trình trên ta được 9 phương trình thể
hiện mối quan hệ giữa thông số đầu vào và các thông số đầu ra
Động học ngược

III.

1. Nội dung bài toán

Đầu vào:

1.1

là các thông số tọa đọ 3điểm P (

px , py , pz

α , β ,γ )
) và các thông số góc RRPY (

Đầu ra:

1.2

Các góc θ1 ,θ 2 của các chân A,B,C
2. Mục đích bài toán:

Xác định các góc quay của các chân
3. Cách giải bài toán

Thực hiện tát cả các bước của bai toan thuận sau đó bình phương các phương
trình tìm hàm mục tiêu sau đó sử dụng phương pháp GRG để giải bài toán
trên
Đặt

f1 = (− l. cos θ 1 + n(cos α . sin β . cos γ + cos α . sin β − cos β . cos α ) − t x + c x ) 2

f 2 = (t y − c y ) 2
f 3 = ( l. sin θ 1 − n(cos α . sin β . sin γ − sin α . cos β − cos β . sin γ ) )

2

Các chân còn lại làm tương tự ta được 9 phương trình
9

f = ∑ fi
i =1

8

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
Đặt hàm mục tiêu MinL = f khi đó hàm mục tiêu sẽ có dạng bậc 4 sẽ rất khó
2

giải được.
Yêu cuu đặt ra là làm sao giải được thuật toán 1 cách dơn giản nhất từ đó ta
đưa ra đề xuất thay thế khớp dẫn động kiểu P bằng khớp dẫn động kiểu R khi
đó bài toán sẽ được hạ bậc về dạng bậc 2
IV.

Động lực học robot song song

1. Mục đích bài toán

Mô hình động lực học dùng để mô phỏng hệ thống robot trên máy tính. Bằng
cách khảo sát hoat động của mô hình trong những điều kiện khac nhau, có thể
tiên đoán hoạt động sau này của robot.
Mô hình động lực học dùng để khai triển các chế độ điều khiển thích hợp. Bộ
điều khiển tinh vi đòi hỏi sử dụng mô hình động lực học thực sự để đạt sự vận
hành tối ưu với tốc độ cao.
Sự phân tích động lực học cho phép xác định các phản lực khớp cần thiết để
thiết kế cơ cấu chấp hành.
2. Nội dung bài toán

Sử dụng các phương pháp Newton-Euler,hệ phương trình Lagrange, nguyên
lý công ảo và vài phương trình khác.
3. Nội dung bài toán

3.1 Phương pháp Newton-Euler
3.1.1 Phân tích vị trí
Vị trii mỗi nhánh truyền động được tính theo vị trí bệ di động từ đó có thể xác
định phương trình vecto vòng

ai + d i si = p + bi (1)
Trong đó

[

ai = a xi , a yi ,0

]

T

là vecto vị trí của

Ai trong hệ tọa độ cố định

bi = [ biu , biv ,0] là vecto viị trí của Bi trong hệ tọa độ di động
9

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
Phân tích vận tốc

3.1.2

Cần tính vận tốc góc và vận tốc tuyến tính của các khâu theo vận tốc góc và
vận tốc tuyến tính của bệ di động. Vận tốc

Vbi của khớp cầu Bi được xac định

bằng cách lấy đạo ham phương trình 1 theo t

Vbi = V p + ω p + bi

V
iVbi = i R AVbi
bi
Biến đổi theo hệ tọa độ nhánh i

[

iVbi = iVbix , iVbiy , iVbiz
i

]

T

i
B
R A = A Ri T
i
là vận tốc của trong hệ tọa đọ nhánh i và

Vbi = d i ω i + i si + d i i si ⇔ i Vbi = i Vbiz

hoặc
3.1.3 Phân tích gia tốc
Gia tốc khớp cầu

Bi được tính bằng cách lấy đạo hàm của phương trình vận

tốc:

Vbi, = V p, + ω p bi + ω p ( ω p + bi )

niA =

i A
i

h

nhánh thứ i

Vbi, = i R A .Vbi,

Động lực các nhánh

3.1.4

i

i

( )

d i A
hi
dt
trong đó

i A

i

n

là tổng moment tác dụng lên khâu i tại điểm

Ai và

là tổng moment khối lượng gó cylider và pittong tại điểm A cả 2 được

tính trong hệ tọa đọ nhánh i.
i

A

[

f bi = i f bix ; i f biy ; i f biz

g = [ 0;0; g c ]

3.1.5

]

T

là lực tác dụng lên bệ chuyển động nhánh i và

là gia tốc trọng lực

Động lực học bệ di động

10

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
Áp dụng phương trình chuyển động Newton đối với bệ di động và tính
phương trình kết quả trong hệ tọa độ cố định:
b

∑

A

i =1

f bi + m p . A g = m pV p,

3.1.6

A

với các

f bi = ARbi A f bi

Bộ tác dụng và phản lực nền

Lực tác động

τi

τ i = i f biz + m z gcθ i + m2 iV2,iz

⇒

Quy trình Newton-Euler.


Tính vị trí vận tốc và gia tốc các khớp cầu chuyển động trong các
phương trình đã nêu ở trên tiếp theo tìm vị trí, vận tốc và gia tốc của



tâm khối lương và vận tốc góc, gia tốc góc mỗi nhánh.
Ký hiệu các lực tác động và phản lực tương ứng tại các điểm nối giữa



bệ và nhánh.
Xét từng nhánh theo hệ thông con, thiết lập phương trình chuyển động



Euler tại khớp cầu ccos định mỗi nhánh

Tìm các phản lực còn lại trên phương trình chuyển động Newton-

Euler.
 Tìm lực tác động
3.2 Quy trình D’lambert
3.2.1 Ma trận Jacubi

Vi = J bi X ,p
1 0 0 0
biz − biy 


J = 0 1 0 − biz
0
bix 
0 0 1 biy − bix
0 

Trong đó

⇒ i Vbi = i J bi . X ,p
3.2.2

.

;

q = J p . X ,p

Sự xoắn quán tính và tác dụng

Giả sử trọng lực là lực duy nhất tác dụng lên cơ cấu chấp hành
11

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
 ^ 

f p   m p − m p .V p,

F=
= A ,
 n p  − I pω p − ω p A I p ω p 

  
^

A

Trong đó

I p = ARB A I p B R A

là ma trận quán tính của bệ di động tai tâm tấm khối

lượng được tính trong hệ tọa độ cố định

Phương trình chuyển động

3.2.3

i Ti ^ i Ti ^ 
J τ + Fp + ∑  J 1i F 1i + J 2i F 2i  = 0
i 

n

^

T
p

i ^

^

i ^

F p được tính trong hệ tọa độ cố định F 1i ; F 2i được tính trong hệ tọa độ
khâu i

⇒

Quy trình D’lambert






3.3

Xác định vị trí, vận tôc, gia tốc bằng cách phân tích tinh động học
Xác định ma trận jacubi bệ di động và khâu
Xác định tổng các quay xoắn tác dụng và quán tính
Giả phương trình động lực học
Phương trình Lagrange

k
∂T j
d  ∂L  ∂L
−
= Q j + ∑ λi
dt  ∂q j  ∂q j
∂q j
i =1

Tj

j=[1;n]

là hàm ràng buộc thứ i. K hệ số hàm ràng buộc

λ i là hệ số nhân Lagrange

Qj

là lực tổng quát hóa do ngoại lực tac dụng

Trong đó T=K-U
K là động năng : U là thế năng
n

K = K p + ∑ ( K ai + K bi )
i =1

Kp

là động năng bệ di động

K ai là động năng roto trên nhánh i K bi động năng

2 thanh nối nhánh i
12

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
n

U = U p + ∑ ( U ai + U bi )
i = ``

Up

thế năng bệ di động

U ai thế năng khâu ngõ vào bên nhánh i, U bi thế năng 2

thanh nối nhánh i
Chương II Động học tay máy
I.

Bậc tự do tối thiểu để hoàn thành nhiệm vụ

Khi cho 1 robot bất kì đầu tiên t cần phải lược đồ được robot sau đó xác định
sốkhâu số khớp của robot để từ đó mới có thể xác định được số bậc tự do của
robot
Bậc từ do của cơ cấu là các thông số đọc lập hoặc các thông số ngõ vào cần
thiết để chuyên biệt cấu hình cấu hình của cơ cấu hoan chỉnh. Có thể xác định
số bậc tự do theo số khâu số khớp trong cơ cấu. Các ký hiệu dùng trong các
phương trình cơ cấu:

C

+ i số ràng buộc của khớp i
+ F số bậc tự do của cơ cấu

f

+ i số chuyển động tương đối được phép của khớp i
+ j số khớp trong cơ cấu, giả sử mỗi khớp là 2 chiều

j

+ i số khớp với 1 bậc tự do

+ L số vòng độc lập trong cơ cấu
+ n số khâu trong cơ cấu kể cả khâu cố định
+

λ số bậc tự do trong không gian làm việc của cơ câu

Do giả thiết rằng các khớp đều hai chiếu, khớp ba chiều được coi là hai khớp
hai chiều, khớp bốn chiều được coi là ba khớp hai chiều.....

λ được dùng cho

các chuyển động của tất cả các khâu chuyển động chúng đều vận hành trong
không gian làm viêc.

λ =6 với cơ cấu không gian, λ =3 với cơ cấu phẳng và
13

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
cơ cấu cầu. Giá trị bậc tự do của cơ cấu bằng bậc tự do liên quan với tất cả
các khâu chuyển động trừ đi số rằng buộc của các khớp. Do đó nếu tất cả các
khâu đều không bị dàng buộc, số bậc tự do của cơ cấu n-khớp với 1 khớp cố
j

định sẽ là λ .(n-1). Tuy nhiên tổng các rằng buộc của khớp băng

∑C
i =1

i

. Do đó
j

bậc tự do của phương trình được tinh theo phương trình:

F = λ .( n − 1) − ∑ C1
i =1

(1.1)
Số ràng buộc của 1 khâu và số bậc tự do của khâu đó bằng thông số chuyển
động

λ do đó:
λ = Ci + f i (1.2)
Do tổng rằng buộc của khâu là:
j

j

∑ C = ∑ (λ − f ) =
i =1

i

i =1

i

j

j.λ − ∑ f i
i =1

(1.3)

Thay (1.3) vào (1.1) ta được:

F = λ .( n − j − 1) + ∑ f i
i

Bậc tự do thụ động nghĩa là hai khớp trùng nhau về phương
Bậc tự do thụ động không thể truyền moment và lực
Số bậc tự do của cơ cấu tính theo công thức sau:

F = λ .( n − j − 1) + ∑ f i − ∑ f b

với

∑f

b

là tổng số bậc tự do thụ động

14

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp

Với cơ cấu chấp hành song song 3RPS ba bậc tự do. Ba nhánh nối với hệ
bằng các khớp cầu tại các điểm A1,B1,C1, và nối với đế bằng các khớp quay
tại các điểm A,B,C. Mỗi nhánh có phần trên và phần dưới, nối với nhau bằng
khớp lăng trụ. Ba khớp lăng trụ này là ngõ vào cơ cấu chấp hành. Toàn bộ có
8 khâu, 3 khớp lăng trụ và ba khớp cầu. Bậc tự do của cơ cấu là:

F = λ .( n − j − 1) + ∑ f i
i

II.

=6(8-9-1)+(3+3+9)=3

Vùng làm việc của Robot

1. Định nghĩa

Vùng làm việc của tay máy là trường làm việc của khâu tác động cuối,
nó là không gian liên tục có hình dạng kích thước và thể tích xác định

15

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
2. Phương pháp xác định vùng làm việc

Chia lưới toàn bộ không gian chứa robot theo một quy luật nào đó
thuận tiện cho việc khảo sát và tọa độ hóa các điểm trong trường vận
động.
Trên mỗi tuyến khảo sát đơn cần chỉ ra theo cả hai chiều tăng và giảm
các ranh giới giữa điểm có nghiệm và không có nghiệm, trung điểm của
hai điểm này được xem gần đúng thuộc mặt giới hạn và mắt lưới đủ nhỏ.
Việc quét hết các điểm thuộc rìa của không gian khảo sát sẽ vạch ra
ranh giới rõ rệt giữa vùng làm việc và vùng còn lại.
Gọi

θ i là các biến khớp thì vị trí của khâu chấp hành được biểu diễn

dưới dạng vector P

P = P( x e , y e , z e , α , β , γ )

trong đó

xe , y e , z e là tọa độ vị trí và hướng của đối

tượng cần thao tác α , β , γ là góc quay của các tọa độ trung gian quanh các
trục tọa độ x,y,z

Đối với tay máy trong đó gồm các khớp quay khi đó xem
khớp,

θ i là biến

ai là kích thước của các khâu thì tọa đọ các khâu chấp hành được

viết dưới dạng

 xe = xe ( ai ,θ i )

 y e = y e ( ai , θ i )
 z = z ( a .θ )
e
i i
 e

và tọa độ vị trí đối tượng cần thao tác

được viết

16

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
 x s = x s ( ai , θ i )


 y z = y s ( ai , θ i )
 z = z ( a ,θ )
s
i
i
 s
vị trí của khâu chấp hành cuối phụ thuộc vào biến

θi ;

P = f ( θ i ) để xác định giá trị biến khớp θ i ta vi phân phương trình theo t
dp df dθ
dθ
df
= . ⇒
= J −1 ( θ ) .
dt dθ dt
dt
dt
1
= J −1 ( θ )
df
(
)
θ
=
θ
θ
,

θ
.....
θ
1 2
n
dθ
Trong đó
Hoặc ta có thể xác định vùng lam việc của robot băng cách ước lượng
⇒

trước vùng lam việc của robot rồi sau đó đưa thuật toán của robot vào
excel và mò nghiêm hay nói cách khác là cho từng điểm rồi tổng hợp tất
cả các điểm có nghiệm rồi vẽ leenvung lam việc của nó. Cách này chính là
giai bài toan ngược có nghiêm. Vói cách này ta mất rất nhiều thời gian mà
kết quả chỉ mang tính tương đối.
Ta có thể mô phỏng vùng lam viêc có dang như sau:

17

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp

III.

Động học robot

1. Động học thuận robot song song

Cho robot như hình vẽ:

hình 1
18

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
Khi sử dụng phương pháp hình học để mô hình hóa phương pháp Gradient
nếu ứng dụng trên robot song song có cấu trúc RPR.SPS hay RPS sẽ vấp phải
dạng mục tiêu bậc 4 mà kỹ thuật đó không thích hợp. Vậy ta sẽ thay thế bằng
1 cấu hình khác nhằm hạ bậc mô hình về dạng bậc 2
Sơ đồ thay thế 1 nhánh:
Chúng ta giả thiết rằng ABC và A1 B1C1 là 2 tam giác đều cạnh a và b
Ta có vị tri tấm di động và cố định

P =O O1 =( Px, Py, Pz )

COo = V c O1C1 = U
;

−a −a 3 
A =  ,
,0 
6

 2


a −a 3 
B =  ,
,0 
2 6


 a 3 
C =  0,
,0 
 3 

−b −b 3 
A1 =  ,
,0 
2
6



b −b 3 
B1 =  ,
,0 
2
6



 b 3 
C1 =  0,
,0 
3



Sơ đồ thay thế 1 nhánh:

19

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp

li

có chức năng thay đổi AA1 , BB1 , CC1 , chức năng của sơ đồ tương đương là

như nhau.
Ta có hướng của O O1 tuân theo quy luật O 1 =R RPY O 0
Trong đó R RPY là ma trận quay Roll-pitch-yow

cc

Ta xét vòng ngũ giác 1 như trên chon C làm chuẩn mô tả O 1 đươc mô tả
Phương trình vòng ngữ giác:

R RPY

U c -V c

+ P =m +n

(2.3.1)

Trong đó : R RPY là hàm của f( ) :

20

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp

li

có chức năng thay đổi AA1 , BB1 , CC1 , chức năng của sơ đồ tương đương là

như nhau.
Ta có hướng của O O1 tuân theo quy luật O 1 =R RPY O 0
Trong đó R RPY là ma trận quay Roll-pitch-yow

cc

Ta xét vòng ngũ giác 1 như trên chon C làm chuẩn mô tả O 1 đươc mô tả

Phương trình vòng ngữ giác:
R RPY

U c -V c

+ P = m + n (1)

Trong đó : R RPY là hàm của f( ) :
 cβ .cγ
 cβ .sγ

 − sβ

R RPY =
Hình
Xét

sα .sβ .cγ − cα .sγ cα .sβ .cγ + sα .sγ 
sα .sβ .sγ + cα .cγ cα .sβ .sγ − sα .cγ 

sα .cβ
cα .cβ

CE = m
CE y

=

CE x

=0

Ta có:

m. cos α 1c

CE z = m. sin α 1c

EC1 = n
EC y = n. cos( 180 − (α 1c + α 2c ) ) = n. cos(α 1c + α 2c )

EC x = 0
EC z = n. sin( 180 − (α 1c + α 2c ) ) = n. sin( α 1c + α 2c )
Thay tất cả vào (2.3.1) ta được

21

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp

 cβ .cγ
 cβ .sγ

 − sβ

sα .sβ .cγ − cα .sγ cα .sβ .cγ + sα .sγ 

sα .sβ .sγ + cα .cγ cα .sβ .sγ − sα .cγ 

sα .cβ
cα .cβ

x

 0 
b 3 


 3 


 0 



-

 0 
a 3


 3 


 0 



 Px 
 
 
 Py 
 
 
 Pz 
 
+  =

0


 m. cos α + n. cos( α + α ) 
1c
1c
2c 

 m. sin α 1c + n. sin( α 1c + α 2c ) 

Tương tự với nhánh

A A1và BB1

Với AA1

RRPY U A − V A + P = m + n

⇔

 cβ .cγ
 cβ .sγ

 − sβ

sα .sβ .cγ − cα .sγ
sα .sβ .sγ + cα .cγ
sα .cβ

 −b 
 2 


− b 3 
cα .sβ .cγ + sα .sγ   6 



cα .sβ .sγ − sα .cγ  
 0 
 
cα .cβ
 x

 −a 
 2 


− a 3

 6 




 0 

 +

 Px 
 
 
 Py 
 
 
 Pz 
 
=

0


 m. cos α + n. cos( α + α ) 
1A
1A
2A 

 m. sin α 1 A + n. sin( α 1 A + α 2 a ) 

Với BB1

RRPY U B − VB + P = m + n

22

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp

⇔

 cβ .cγ
 cβ .sγ

 − sβ

sα .sβ .cγ − cα .sγ
sα .sβ .sγ + cα .cγ
sα .cβ

 a 
 2 


− a 3
 6 





 0 

 +

 b 
 2 


− b 3 
cα .sβ .cγ + sα .sγ   6 



cα .sβ .sγ − sα .cγ  
 0 
 
cα .cβ
 x

 Px 
 
 
 Py 
 
 
 Pz 
 

=

0


 m. cos α + n. cos( α + α ) 
1B
1B
2B 

 m. sin α 1B + n. sin( α 1B + α 2 B ) 
Do chiều dài các chân trên cấu hình thay thế chọn trước và để dơn giản ta

chọn bằng nhau mà không làm mất tính tổng quát của bài toán khi đó tam

giác

AGA1, BHB1 ,CEC1

cạnh là m = n và

là các tam giác cân đỉnh lần lượt là G, H , E và chiều dài

li

l = m + n − 2m.n. cos( E )
Xét tam giác CEC1 thì E được tính theo công thức i
2

2

2

(2.3.2)

 m 2 + n 2 − li 2 

E = arccos

 2.m.n 
Hay

(2.3.3)

α
= 180 o − E
2
c
Theo hình ta có thể thấy được :
Vậy công thức biến đổi của bài toán để hạ bậc hàm mục tiêu là:
 m2 + n2 − l 2i 

α 2i = 180 − arccos
2
.
m
.
n



o

Để giải được bài toán trên chúng ta sử dung phương pháp GPG

23

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
Phương pháp đưa hàm mục tiêu về dạng Min Hay chính là tổng bình phương
cửa hệ 9 phương trình. Chúng ta đòng nhất các ma trận tương ứng của các
chân của robot thì được hệ phương trình.
L1=[( -1/2*cos(q2)*cos(q3)*b-1/6*(sin(q1)*sin(q2)*cos(q3)cos(q1)*sin(q3))*b*3^(1/2)-1/2*a+px)-0 ]^2
L2=[ ( -1/2*cos(q2)*sin(q3)*b1/6*(sin(q1)*sin(q2)*sin(q3)+cos(q1)*cos(q3))*b*3^(1/2)-1/6*a*3^(1/2)+py)
– (m*cos(x1)+n*cos(x1+x2)) ]^2
L3=[ ( 1/2*sin(q2)*b-1/6*sin(q1)*cos(q2)*b*3^(1/2)+pz)( m*sin(x1)+n*sin(x1+x2)) ]^2
L4=[ ( 1/2*cos(q2)*cos(q3)*b-1/6*(sin(q1)*sin(q2)*cos(q3)cos(q1)*sin(q3))*b*3^(1/2)+1/2*a+px)-0 ]^2
L5=[ ( 1/2*cos(q2)*sin(q3)*b1/6*(sin(q1)*sin(q2)*sin(q3)+cos(q1)*cos(q3))*b*3^(1/2)1/6*a*3^(1/2)+py)-( m*cos(y1)+n*cos(y1+y2)) ]^2
L6=[ ( 1/2*cos(q2)*sin(q3)*b1/6*(sin(q1)*sin(q2)*sin(q3)+cos(q1)*cos(q3))*b*3^(1/2)1/6*a*3^(1/2)+py)-( m*sin(y1)+n*sin(y1+y2)) ]^2
L7=[ ( 1/3*(sin(q1)*sin(q2)*cos(q3)-cos(q1)*sin(q3))*b*3^(1/2)+px)-0 ]^2
L8=[ (1/3*(sin(q1)*sin(q2)*sin(q3)+cos(q1)*cos(q3))*b*3^(1/2) +
1/3*a*3^(1/2)+py)-( m*cos(z1)+n*cos(z1+z2)) ]^2
L9=[ ( 1/3*sin(q1)*cos(q2)*b*3^(1/2)+pz)-( m*sin(z1)+n*sin(z1+z2)) ]^2
Với các thông số tương đương là:
q1,q2,q3 = α , β , γ
x1;x2;y1;y2;z1;z2 lần lượt là

α 1 A , α 2 A , α 1B , α 2 B , α 1C , α 2c

9

Lmin = ∑ L1

=0 (2.3.4)
Để giải được bài toán trên chúng ta đưa thuật toán vào Excel để gải.
Với hệ 9 phương trình 12 ẩn chúng ta không thể giải được bằng phương pháp
i =1

thông thường. Từ đó ta phải sử dụng phương pháp GRG để giải bài toán.
Tuy nhiên mục đích của bài toán thuận là tìm lại được điểm đã đưa vào bài
toán ngược trước đó lẫn trong vô số các thế trên cùng một bộ tham số đầu vào
biết trước, vậy trong thực tế điều khiển xác lập quan hệ giữa hai điểm biết
24

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long


Đồ án robot công nghiệp
trước trong hai không gian này là điều hết sức quan trọng chứ không phải tìm
tất cả các nghiệm có thể có của bài toán thuận. Để làm được việc này phương
pháp mà chúng tôi đề xuất là gán ràng buộc tìm nghiệm với các tham số vị trí
và hướng chính là bộ tham số đã đưa vào bài toán ngược, bài toán cho trước
chiều dài 3 chân l1 đến l3 cộng với các tham số

α 1 A , α 2 A , α 1B , α 2 B , α 1C , α 2C bài

toán trở nên biết 6 tham số, tìm 6 tham số là ( px , p y , pz ,α , β , γ ) , kích thước bài
toán giảm xuống đáng kể song quan trọng là thiết lập một quan hệ duy nhất
giữa hai điểm trên hai không gian, lúc này bài toán thuận có ý nghĩa xác định
các tham số kết cấu phụ để tạo ra điểm đã đưa vào bài toán ngược. Hay chính
là cho các thông số

α 1 A , α 2 A , α 1B , α 2 B , α 1C , α 2C (x1,x2,y1,y2,z1,z2) đưa vào

Excel để giải kết quả là nghiệm của bài toàn ngược.
2. Động học ngược robot.

Để giải được bài toan ngược trước hết ta phải xác định được các phuong trình
liên tới các biến khớp của từng chân của robot cho trước. Trong bài toán này
các phương trình liên quan em đã tìm được ở trên
Với bài tóa ngược ta chỉ đưa thuật toán vào excel và sử dụng phương pháp để
giải bài toán. bài toán ngược thì ta biết được 6 thông số ( px , p y , pz ,α , β , γ ) 6

25

SVTH: Phạm Văn Biên

GVHD: Phạm Thành Long