ĐIỀU KHIỂN THỜI GIAN THỰC TRÁNH VA CHẠM VỚI VẬT CẢN CHO CÁNH TAY ROBOT CÔNG NGHIỆP

Luật không gian trường thế nhân tạo: Cánh tay máy được coi là di chuyển trong một trường lực. Trong đó, vị trí cần, đi tới (điểm đích) phát ra một trường lực hút cánh tay máy, còn vật cản (chướng ngại vật) sẽ phát ra một trường lực đẩy cánh tay máy ra xa(The manipulator moves in a field of forces. The position to breached is an attractive pole for end effector and obstacles are repulsive surfaces for the manipulator parts)

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

ĐIỀU KHIỂN THỜI GIAN THỰC TRÁNH VA CHẠM VỚI VẬT

CẢN CHO CÁNH TAY ROBOT CÔNG NGHIỆP

Hà nội, 12-2015

Nhiệm Vụ

NHIỆM VỤ

Lời Cam Đoan

LỜI CAM ĐOAN

Mục Lục

MỤC LỤC

LỜI MỞ ĐẦU Error! Bookmark not defined

MỤC LỤC 4

HÌNH VẼ 6

CHƯƠNG I: ROBOT VÀ MÔ HÌNH TOÁN HỌC CỦA ROBOT 9

1.1 Động học vị trí robot 9

1.1.1 Biểu diễn ma trận 9

1.1.2 Phép biến đổi 10

1.2 Động học thuận robot 13

1.2.1 Tham số của thanh nối và khớp 13

1.2.2 Phép biểu diễn Danevit-Hartenberg 13

1.2.3 Quan hệ giữa hai khung tọa độ i và i-1 14

1.3 Phương trình động lực học của robot 15

1.3.1 Phương trình Lagrange 15

1.3.2 Phương trình động lực học cho cánh tay robot 17

CHƯƠNG II: XÂY DỰNG LUẬT ĐIỀU KHIỂN ROBOT TRÁNH VẬT CẢN 19

2.1 Luật không gian trường thế nhân tạo 19

2.1.1 Lực hút tới điểm đích 20

2.1.2 Lực đẩy nhân tạo từ bề mặt vật cản (FIRAS) 21

2.1.3 Tổng lực tác dụng vào chất điểm 22

2.2 Lực hút tác dụng vào cánh tay robot 23

2.3 Lực đẩy tác dụng vào cánh tay robot 23

2.3.1 Tính toán khoảng cách gần nhất từ một điểm tới một vật cản 24

2.3.2 Lực đẩy từ vật cản tác dụng vào một phần tử trên robot 29

2.3.3 Lực đẩy từ vật cản tác dụng vào cánh tay robot 29

CHƯƠNG III: XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN TRÊN ROBOT 2 BẬC TỰ DO 33

3.1 Xây dựng đối tượng robot 33

3.1.1 Đối tượng điều khiển 33

3.1.2 Xây dựng mô hình đối tượng điều khiển (Robot 2 bậc tự do) 34

3.2 Xây dựng mô hình điều khiển cho robot 2 bậc tự do 39

Mục Lục

3.2.1 Tính toán các lực tác dụng vào cánh tay robot thông qua luật trường thế

nhân tạo 39

3.2.2 Mô men điều khiển tác động vào từng khớp của cánh tay robot 43

CHƯƠNG 4: XÂY DỰNG MÔ HÌNH MÔ PHỎNG BẰNG MATLAB 50

4.1 Mô hình đối tượng mô phỏng (Robot 2 bậc tự do) 50

4.2 Mô hình điều khiển tránh va chạm vật cản cho đối tượng đã được xây dựng 52

4.2.1 Mô phỏng trong trường hợp không có vật cản 56

4.2.2 Mô phỏng trong trường hợp có vật cản 58

4.3 So sánh với trường hợp điều khiển robot theo quỹ đạo được thiết kế 60

Hình Vẽ

HÌNH VẼ

Hình 1.1 Tọa độ một điểm trong khung tọa độ Đề-Các 9

Hình 1.2 Biểu diễn khung tọa độ p trong khung tọa độ chuẩn 10

Hình 1.3 Biểu diễn một khung tọa độ tịnh tiến 11

Hình 1.4 Biểu diễn một khung tọa độ quay đơn theo trục z 12

Hình 1.5 mô tả mối liên kết giữa các thanh nối của cánh tay robot 14

Hình 2.1 Điểm đích 19

Hình 2.2 Vật cản O 19

Hình 2.3 Trường thế nhân tạo 20

Hình 2.4 Khoảng cách từ 1 điểm tới đường tròn 25

Hình 2.5 Khoảng cách từ 1 điểm tới 1 đường thẳng 26

Hình 2.6 Khoảng cách ngắn nhất từ 1 điểm tới 1 tứ giác bất kì 27

Hình 2.7 Khoảng cách gần nhất giữa cánh tay robot và vật cản hình tròn 30

Hình 2.8 Khoảng cách gần nhất từ một đoạn thẳng tới 31

một liên kết của cánh tay robot 31

Hình 3.1 Mô hình robot 2 bậc tự do được xét 33

Hình 3.2 Mô hình robot 2 bậc tự do 34

Hình 3.3 Mô hình robot hoạt động trong môi trường 39

có vật cản hình tròn và điểm đích P 39

Hình 3.4 Mô hình robot hoạt động trong môi trường có vật cản hình tròn 40

Hình 3.5 Ma trận Jacobian tính toán cho lực hút của điểm đích 45

Hình 3.6 Ma trận Jacobian tính toán cho lực đẩy vào liên kết 2 47

của vật cản 47

Hình 3.7 Ma trận Jacobian tính toán cho lực đẩy vào liên kết 1 48

của vật cản 48

Hình 4.1 Sơ đồ cấu trúc của robot 2 bậc tự do 50

Hình 4.2 Mô hình điều khiển tránh va chạm cho robot 53

Hình 4.3 Quá trình chuyển động của cánh tay robot từ vị trí ban đầu tới điểm đích có tọa độ [0.2,0.3] 56

Hình 4.4 Đồ thị mô tả quá trình góc quay của hai khớp 57

Hình 4.5 Mô tả sự thay đổi của mô men điều khiển 57

Hình 4.6 Quá trình chuyển động tránh vật cản của cánh tay robot 58

Hình Vẽ

Hình 4.7 Đồ thị mô tả quá trình góc quay của hai khớp khi tránh vật cản 59

Hình 4.8 Mô tả sự thay đổi của mô men điều khiển khi tránh vật cản 60

Hình 4.9 So sánh Quá trình tránh vật cản của robot theo quỹ đạo 61

tránh vật cản 61

Hình 4.10 So sánh sự thay đổi của góc các khớp trong hai trường hợp 62

Hình 4.11 So sánh sự thay đổi của mô men tác động trong hai trường hợp 62

Lời Nói Đầu

LỜI NÓI ĐẦU

Chương 1: Robot và mô hình toán học của robot

CHƯƠNG I: ROBOT VÀ MÔ HÌNH TOÁN HỌC CỦA

a)Biểu diễn một vector trong không gian

Hình 1.1 Tọa độ một điểm trong khung tọa độ Đề-Các [4],[6]

Một vector trong không gian được xác định bằng ba hình chiếu của vector trên

ba trục của khung tọa độ chuẩn (khung tọa độ Đề-Các)

Trong đó: – là hình chiếu của điểm p trên 3 trục x,y,z

– là hình chiếu của vector r trên 3 trục x,y,z

Vector ̅ và điểm p cũng có thể biểu diễn dưới dạng ma trận:

Chương 1: Robot và mô hình toán học của robot

Trong robot, một điểm được biểu diễn bằng một ma trận cột với thành phần thứ

tư, nếu là 1 thì dùng để mô tả ma trận vector của một điểm, nếu là 0 thì dùng để mô tả

ma trận vector của một vector

b)Biểu diễn một khung tọa độ trong không gian

Hình 1.2 Biểu diễn khung tọa độ p trong khung tọa độ chuẩn [5]

Một khung tọa độ được biểu diễn bằng 4 thành phần, bao gồm 3 thành phần là biểu diễn hướng của khung tọa độ còn một thành phần để biểu diễn gốc tọa độ của khung tọa độ đang xét [2]

[

- Phép biến đổi tịnh tiến

- Phép biển đổi quay

- Phép biến đổi kết hợp giữa phép biến đổi tịnh tiến và phép biến đổi quay

a)Phép biến đổi tịnh tiến

Khi một khung tọa độ (có thể bao gồm đối tượng) di chuyển tịnh tiến trong không gian làm việc, thì hướng của khung tọa độ đó sẽ không thay đổi trong quá trình chuyển động

Khi đó phép biến đổi tịnh tiến được biểu diễn bằng ma trân vuông như sau:

Chương 1: Robot và mô hình toán học của robot

Trong đó: a,b,c chính là tọa độ của gốc tọa độ B trong khung tọa độ mới

Hình 1.3 Biểu diễn một khung tọa độ tịnh tiến

Dạng ma trận của khung tọa độ mới được xác định bằng phép nhân ma trận biến đổi tịnh tiến và ma trận biểu diễn khung tọa độ ban đầu

[

] [

]

(1.7)

Hay được viết dưới dạng:

b)Phép biến đổi quay đơn

Ta xét một khung tọa độ R(i,j,k) đặt ở gốc của khung tọa độ gốc O(x,y,z) và có các trục trùng với các trục của khung tọa độ gốc Xét điểm P nằm trong khung tọa độ,

có tọa độ (Px, Py, Pz) trong khung tọa độ gốc và tọa độ (PxR, PyR, PzR) trong khung tọa

độ R

Khi quay khung tọa độ R quanh trục z một góc thì điểm P cũng sẽ quanh cùng với khung tọa độ R một góc , khi đó tọa độ của điểm P trong khung tọa độ R sẽ không thay đổi, còn trong khung tọa độ gốc sẽ thay đổi

Chương 1: Robot và mô hình toán học của robot

Hình 1.4 Biểu diễn một khung tọa độ quay đơn theo trục z

Trên hình 1.4 biểu diễn tọa độ của điểm P trên mặt phẳng (Oxy) (do quay trên

trục z) trước và sau khi quay trục z

Từ hình trên ta nhận thấy tọa độ của điểm P trong khung tọa độ gốc sẽ thay đổi thành:

Chương 1: Robot và mô hình toán học của robot

( ) [

Ví dụ: [2] Xét một phép biến đổi kết hợp từ 4 phép biến đổi đơn theo thứ tự như sau:

- Quay quanh trục z một góc

- Tịnh tiến theo trục z một đoạn

- Tịnh tiến theo trục x một đoạn

1.2.1 Tham số của thanh nối và khớp

Xét hai khớp i và i+1, thanh i nối giữa hai khớp i và i+1, được biểu diễn trên hình trên

Như minh họa trên hình 1.5, là độ dài pháp tuyến chung của trục khớp i và i+1; là góc giữa hai trục của khớp i và i+1 (góc giữa trục i+1 và đường thẳng song son trục i nằm trong mặt phẳng chứa trục i+1 và trực giao với pháp tuyến chung )

Tương tự xét trục khớp i-1 Pháp tuyến chung của trục khớp i và i-1 là Khoảng cách giữa hai chân pháp tuyến chung của trục i là Góc là góc giữa hai pháp tuyến chung của trục khớp i

Đối với khớp quay, là góc quay của khớp Do đó là biến của khớp quay Đối với khớp tịnh tiến, là độ dịch chuyển tịnh tiến của khớp, nên đặt là biến của khớp tịnh tiến

1.2.2 Phép biểu diễn Danevit-Hartenberg

Phép biểu diễn D-H (Danevit-Hartenberg ) dùng để mô tả mối quan hệ giữa các thanh nối, khớp và tay robot

Chương 1: Robot và mô hình toán học của robot

Theo phương pháp D-H, khung tọa độ thanh nối i được xây dựng theo nguyên

tắc sau [2]: (theo hình 1.5):

- Gốc khung tọa độ thanh i đặt trùng với chân pháp tuyến chung của trục i và i+1

và nằm trên trục khớp i+1

- Trục đặt theo phương của trục khớp i+1

- Trục đặt theo phương pháp tuyến chung của trục i và i+1 theo hướng đi từ trục i đến i+1

Một số trường hợp đặc biệt:

- Khi hai trục z cắt nhau: sẽ không có pháp tuyến chung giữa hai khớp Khi đó điểm gốc của khung tạo độ là giao điểm của hai trục và trục x được đặt dọc theo đường vuông góc với mặt phẳng chứa hai trục z đó

- Hai trục song song, sẽ có nhiều pháp tuyến chung của khớp trước Gốc khung tọa độ chọn sao cho là nhỏ nhất

- Đối với khớp tịnh tiến: khoảng cách là biến khớp Hướng của trục khớp trùng với hướng di chuyển của khớp Hướng của trục được xác định, nhưng vị trí trong không gian khôn được xác định Khi đó chiều dài không có ý nghĩa nên đặt Gốc tọa độ đặt trùng với gốc thanh nối tiếp theo

Theo nguyên tắc đặt khung tọa độ như trên, bắt đầu gắn khung tọa độ từ bệ (thân) robot và khung tọa độ 0: trục trùng với trục khớp 1

Hình 1.5 mô tả mối liên kết giữa các thanh nối của cánh tay robot [2]

1.2.3 Quan hệ giữa hai khung tọa độ i và i-1

Một cách tổng quát, quan hệ giữa hai khung tọa độ i và i-1 được xác định bằng các phép biến đổi theo thứ tự sau [2]:

- Quay khung tọa độ quanh trục một góc sao cho trục trùng với phương của trục

- Tịnh tiến dọc theo trục một đoạn để gốc khung tọa độ mới trùng với chân pháp tuyến chung của trục i và i+1

- Tịnh tiến dọc theo trục (phương pháp tuyến chung) một đoạn

- Quay xung quanh trục một góc sao cho trùng với

Các phép biến đổi trên được thực hiện so với khung tạo độ hiện tại (khung tọa

độ ngay trước đó) Do đó phép biến đổi tổng hợp được xác định như sau:

Chương 1: Robot và mô hình toán học của robot

Trong đó: - , lần lượt là hàm tính giá trị cos của hai góc và

- , lần lượt là hàm tính giá trị sin của hai góc và Thay các ma trận của phép biến đổi đơn vào phương trình trên, sau một số biến đổi, nhận được ma trân biểu diễn quan hệ giữa hai khung tọa độ i và i-1 như sau:

[

Giả sử hệ thống gồm n phần tử vật chất với n bậc tự do mô tả bởi cá biến tọa độ tổng quát , ,…, (đối với chuyển động của khớp quay, tọa độ tổng quát là góc quay của khớp ; đối với khớp chuyển động tịnh tiến, tọa độ tổng quát là độ dịch chuyển ) Một vector vị trí trong không gian 3 chiều ̅̅̅ đối với mỗi phần

tử vật chất được kí hiệu là:

Chương 1: Robot và mô hình toán học của robot

Chương 1: Robot và mô hình toán học của robot

Sử dụng các biểu thức vừa tính được, sau một vài phép biến đổi đơn giản ta có phương trình chuyển động Lagrange được viết như sau:

(

̇)

(1.33)

Hoặc viết lại ở dạng ma trận:

̅ [

]

]

(1.35)

Từ hai công thức trên ta có thể viết lại dưới dạng:

̅ ( ̅)

̅

b)Ma trận Jacobian trong robot

Sử dụng khái niệm ma trận Jacobian cho cơ cấu robot, có thể viết phương trình biểu diễn quan hệ chuyển dịch vi sai của tay robot và các khớp robot 6DOF (6 bậc tự do) [2]

Chương 1: Robot và mô hình toán học của robot

Hay:

[

]

[ ]

[

]

Hay:

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

CHƯƠNG II: XÂY DỰNG LUẬT ĐIỀU KHIỂN ROBOT

TRÁNH VẬT CẢN

2.1 Luật không gian trường thế nhân tạo

Phương pháp này được xây dựng để giải quyết bài toán chống va chạm trong điều khiển cánh tay máy và robot di động

Nguyên lí của phương pháp “Không gian trường thế nhân tạo” [1] được miêu tả như sau:

“ The manipulator moves in a field of forces The position to b reached is an attractive pole for end effector and obstacles are repulsive surfaces for the manipulator parts ” [1]

Khatib

( Cánh tay máy được coi là di chuyển trong một trường lực Trong đó, vị trí cần,

đi tới (điểm đích) phát ra một trường lực hút cánh tay máy, còn vật cản (chướng ngại vật) sẽ phát ra một trường lực đẩy cánh tay máy ra xa )

Đầu tiên ta xét quá trình tránh một vật cản đơn O của một điểm

Hình 2.1 Điểm đích [7] Hình 2.2 Vật cản O [7]

Khi ta cho một điểm di chuyển tới điểm đích trong một không gian chứa vật cản

O, ta có thể coi điểm đó di chuyển trong một trường thế nhân tạo Trong đó điểm đích coi như một cực hút phát ra một trường lực hút hướng về đích, còn vật cản O coi như một cực đẩy phát ra một trường lực đẩy hướng ra xa khỏi vật cản

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

Hình 2.3 Trường thế nhân tạo [7]

Trường thế nhân tạo được miêu tả bằng công thức:

Trong đó: ( ): là trường thế nhân tạo

( ): là trường lực hút của điểm đích

( ): là trường lực đẩy của vật cản O

: là tọa độ đích của điểm chuyển động

Từ đó ta tìm được vecto lực tác động vào điểm chuyển động trong môi trường không gian trường thế nhân tạo có dạng:

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

Từ đây ta nhận thấy ( ) cần được chọn sao cho trường thế nhân tạo ( ) phải đạt giá trị “bằng không” khi vật đạt tới đích hay , để chắc chắn vật không tiếp tục di chuyển khi đã tới đích [1]

Tuy nhiên nếu sử dụng công thức 2.3 hệ thống có thể không được ổn định và bị dao động, vì vậy để tăng tính ổn định của hệ thống ta cần thêm một thành phần điều khiển vận tốc cho lực hút Khi đó lực hút sẽ có dạng:

2.1.2 Lực đẩy nhân tạo từ bề mặt vật cản (FIRAS)

Trường lực đẩy ( ) được xây dựng sao cho nó sẽ tạo thành một trường thế năng bao quanh vật cản Đồng thời ( ) cần là một hàm liên tục, khả vi và giá trị sẽ tiến tới vô cùng tại ngay sát bề mặt vật cản đảm báo vật khi di chuyển sẽ không va vào vật cản Ngoài ra khi xây dựng ( ) ta cần giới hạn bề mặt thế năng chỉ trong một khu vực xung quanh vật cản giúp đơn giản hóa quá trình chuyển động của vật cần điều khiển, đặc biệt nó sẽ giúp giảm đáng kể mức ảnh hưởng của một vật cản tới không gian hoạt động

Bằng việc sử dụng hàm ( ) để miêu tả vật cản ta có biểu thức cơ bản của trường vật cản:

( ) { ( )

(2.7)

Trong đó: : là khoảng cách giới hạn của trường thế vật cản

: là khoảng cách từ điểm gần nhất của vật cần di chuyển tới vật cản Khoảng cách giới hạn của trường thế vật cản có thể được tìm dựa vào vận tốc lớn nhất của vật di chuyển cũng như khả năng giảm tốc của nó

Khi lực điều khiển một điểm dưới tác dụng của một trường thế (PSP) ứng với vật cản O được thể hiện:

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

( ) { ( )

(2.8)

Trong đó: là vector định hướng vị trí từ PSP tới vật cản:

[

]

Mà khoảng cách giữa hai điểm trong không gian có dạng:

√( ) ( ) ( ) (2.9) Trong đó: , , : là tọa độ trong không gian của vật cản O

, , : là tọa độ của vật cần di chuyển

Vector định hướng vị trí từ PSP tới vật cản trở thành:

Nếu khoảng cách tới vật cản ( ) khi đó lực tác động vào một điểm chỉ có lực hút tới điểm đích:

( ) ( ) ( ) ̇ (2.12) Nếu chất điểm đi vào vùng ảnh hưởng của vật cản ( ) khi đó lực tác động vào chất điểm có dạng:

( ) ( ) ̇ ( )

Trong đó:

Với: √( ) ( ) ( )

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

[

]

2.2 Lực hút tác dụng vào cánh tay robot

Mục này sẽ đi xây dựng lực hút tác dụng vào cánh tay robot khi một cánh tay robot được đặt trong một môi trường phức tạp, nhiều vật cản Bằng việc sử dụng luật trường thế nhân tạo ta nhận thấy, cánh tay robot sẽ bị tác dụng bởi hai lực gồm: lực hút

do trường lực hút của điểm đích tạo ra, lực đẩy từ trường lực đẩy do vật cản tạo ra Tuy nhiên như đã nói mục này chỉ giải quyết vấn đề về lực hút tác dụng vào cánh tay robot

Đối với việc điều khiển hoạt động của cánh tay robot, mục đích lớn nhất của bài toán điều khiển là làm thế nào để đưa đầu cánh tay máy tới điểm chỉ định nhằm thực hiện các nhiệm vụ từ yêu cầu công nghệ, dây chuyền sản xuất

Chính vì vậy, giống như với chất điểm, lực hút do trường lực hút tạo ra sẽ tác dụng vào đầu cánh tay robot (vì mục đích của lực hút là đưa đầu cánh tay robot tới điểm đích (hay điểm chỉ định))

Vì lực chỉ tác dụng vào đầu cánh tay nên công thức tính lực tác dụng vào đầu cánh tay tương tự như công thức tính lực tác dụng vào một điểm

Vì vậy công thức tính lực hút tác dụng vào đầu cánh tay sẽ giống như công thức 2.12

( ) ( ) ̇

2.3 Lực đẩy tác dụng vào cánh tay robot

Ở mục này ta sẽ phân tích, xây dựng lực đẩy từ vật cản tác dụng vào cánh tay robot khi robot được đặt trong một môi trường phức tạp có nhiều vật cản Khi đặt trong môi trường phức tạp, bằng việc sử dụng luật trường thế nhân tạo áp dụng cho vật cản ta sẽ xác định được lực đẩy từ vật cản tác động vào cánh tay robot giúp cánh tay robot tránh va chạm với vật cản

Như đã được xây dựng ở trên với một chất điểm, ta biết được khi một chất điểm

di chuyển trong môi trường phức tạp, nhờ luật trường thế nhân tạo ta sẽ xác định được lực đẩy từ vật cản tác dụng vào chất điểm giúp làm cho chất điểm hướng đi xa ra khỏi vật cản ( hay tránh vật cản )

Hoàn toàn tương tự như vậy ta có thể coi như cánh tay robot là tập hợp của N điểm liên kết với nhau, và việc cần làm là xác định phản lực tác dụng lên các phần tử của cánh tay robot Khi đó ta có công thức xác định lực đẩy vào một phần tử của cánh tay robot sẽ có dạng giống như tác dụng vào một chất điểm:

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

độ của phần tử thứ n, [ ] là tọa độ của điểm gần nhất từ vật cản tới cánh tay robot

Tuy nhiên việc tính toán lực tác dụng lên tất cả các phần tử rất phức tạp và tạo gánh nặng về mặt tính toán cho cấu trúc điều khiển nên ta cần tìm một cách giải quyết khác cho vấn đề này

Ta nhận thấy, mục tiêu của việc xây dựng lực đẩy từ vật cản tác dụng lên cánh tay robot nhằm tránh sự va chạm giữa cánh tay robot với vật cản, vì vậy chúng ta chỉ cẩn tìm một điểm nằm trên cánh tay robot gần nhất tới vật cản và xây dựng lực đẩy sao cho tại bất kì thời điểm nào điểm gần nhất của cánh tay robot cũng không va chạm với vật cản, khi đó toàn bộ cánh tay robot cũng sẽ không va chạm với vật cản Từ đó công thức tính lực đẩy tác dụng lên điểm M nằm trên cánh tay robot gần nhất với vật cản trở thành:

Khi đó ta chỉ cần xác định tọa độ điểm gần nhất M nằm trên cánh tay robot tới vật cản là ta có thể xác định được vector lực đẩy từ vật cản tác dụng vào cánh tay robot

2.3.1 Tính toán khoảng cách gần nhất từ một điểm tới một vật cản

Ở phần trên ta mới chỉ xét tới tránh né một vật cản đơn O của một chất điểm, vì thế ta chỉ cần coi bề mặt đẩy của vật cản là một vành tròn có tâm chính là vật cản đơn

O Tuy nhiên trong thực tế, vật cản có kích thước và vì vậy nó sẽ ảnh hưởng đáng kể tới điểm chuyển động Vì vậy không chỉ xét vật cản là một chất điểm mà ta còn cần phải xét tới một vật cản có kích thước nữa, tuy nhiên nếu áp dụng công thức tính lực

đẩy (ở mục 3.2) ta sẽ gặp khó khăn do tính phức tạp của bề mặt vật cản, vì vậy ở đây

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

chỉ xét tới vật cản phẳng, và phân tích vật cản ra làm các khối hình học đơn giản (trong mặt phẳng)

a)Xét vật có hình dạng tròn

Xét vật cản có dạng hình tròn với tâm có tọa độ ( ) và bán kính r Ta nhận thấy, khi vật cản có dạng hình tròn, bề mặt đẩy của nó cũng sẽ có dạng đường tròn với tâm là tâm đường tròn bao trọn vật cản ở bên trong

Hình 2.4 Khoảng cách từ 1 điểm tới đường tròn

Hình 2.4 mô tả vị trí của một điểm so với một vật cản có dạng hình tròn (như

hình vẽ), ta nhận thấy, theo luật không gian trường thế nhân tạo, trường thế năng do vật cản tạo ra cũng sẽ có dạng hình tròn với tâm chính là tâm của vật cản Cũng chính

vì vậy đường giới hạn vùng từ trường của vật cản cũng có dạng hình tròn (như hình vẽ)

Từ Hình 2.4 ta thấy rằng khoảng cách gần nhất từ một điểm tới vật cản chính là

hiệu của khoảng cách điểm đó tới tâm của vật cản trừ đi bán kính vật cản:

Trong đó: – khoảng cách từ điểm đang xét tới tâm vật cản

– chính là khoảng cách gần nhất từ một điểm tới vật cản hình tròn Ngoài ra khoảng cách từ một điểm tới tâm đường tròn trong tọa độ Decac có công thức dạng:

 Công thức tính khoảng cách gần nhất từ một điểm tới một vật cản hình tròn sẽ

có dạng:

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

b)Xét vật có dạng hình đa giác (tứ giác)

Hình 2.5 Khoảng cách từ 1 điểm tới 1 đường thẳng

Để xét vật cản hình tứ giác, đầu tiên ta cần xét bài toán tìm vị trí của một điểm gần nhất trên một đoạn thẳng đối với một điểm

Gọi l là độ dài đoạn thẳng với hai đầu mút là ( ) và ( ) , xét C là điểm gần m nhất trên đoạn thẳng AB, từ đó ta có thể dễ dàng tính ra tọa độ của điểm C gần nhất tới điểm đang xét:

( )

(2.19) ( )

Trong đó: – là hệ số tỉ lệ giữa độ dài đoạn CA và độ dài đoạn BA

Để xác định điểm C ta cần tính như sau:

{ ( ( )) {( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

Mà do quan hệ vuông góc nên:

( ) ( ) ( ) ( ) Thay vào phương trình trên ta được:

( ) ( ) ( ) ( )

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) [( ) ( ) ]

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

Nếu thì điểm gần nhất sẽ trùng với và khoảng cách ngắn nhất sẽ là , còn nếu thì điểm gần nhất sẽ trùng với từ đó ta có khoảng cách ngắn nhất tới đoạn có dạng:

{[ ]

⁄

(2.22)

Hoàn toàn tương tự với , lần lượt là khoảng cách từ v tới ,

Xét khoảng cách ngắn nhất tới đoạn :

{[ ]

⁄

(2.23)

Trong đó: ( ) ( )

( ) ( )Xét khoảng cách ngắn nhất tới đoạng :

{[ ]

⁄

(2.24)

Trong đó: ( ) ( )

( ) ( )Xét khoảng cách ngắn nhất tới đoạng :

{[ ]

⁄

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

( )

Trong đó: A,B – là hai đầu mút của đoạn thẳng chứa điểm gần nhất tới điểm đang

xét C

C – Điểm nằm trên vật cản gần nhất tới điểm đang xét

Vậy thông qua các bước tính toán như trên với hầu hết các vật cản được bao bởi một bề mặt hình tròn hoặc mặt đa diện (thường có dạng tứ giác) ta có thể xác định được khoảng cách gần nhất từ vật cản tới robot, đồng thời cũng xác định được điểm gần robot nhất của vật cản Qua đó sẽ xác định được lực đẩy từ vật cản tác dụng lên robot nhằm đẩy robot ra xa khỏi vật cản (tránh va chạm với vật cản)

2.3.2 Lực đẩy từ vật cản tác dụng vào một phần tử trên robot

Như ở mục 2.3.1 trên ta đã xác định được khoảng cách gần nhất từ một điểm

cần tránh va chạm (một phần tử của cánh tay robot) là , đồng thời ta cũng xác định được tọa độ của điểm gần nhất nằm trên vật cản với phần tử đang xét [ ]

Từ đó ta có thể xác định được lực đẩy từ vật cản tới phần tử đang xét tương đương với lực đẩy từ điểm gần nhất nằm trên vật cản tới phần tử đang xét (hay một phần tử của cánh tay robot)

( ) { ( )

(2.27)

Trong đó: – là khoảng cách gần nhất từ vật cản tới phần tử đang xét

– là tọa độ của điểm gần nhất nằm trên vật cản tới phần tử đang xét

– Vecto đạo hàm riêng khoảng cách có dạng:

[

]

2.3.3 Lực đẩy từ vật cản tác dụng vào cánh tay robot

Từ mục 2.3.2 ở trên ta đã xác định được lực đẩy từ một vật cản vào một phần tử

của cánh tay robot Tuy nhiên, như đã nói ở đầu mục, nếu phải tính toán lực đẩy vào từng phần tử của robot sẽ tạo gánh nặng cho bộ xử lí của cánh tay robot đồng thời tạo một khoảng thời gian trễ trong quá trình điều khiển cánh tay robot Vì vậy, tương tự như phương pháp xác định điểm gần nhất của vật cản với một phần tử của cánh tay robot, ở mục này ta cũng sẽ xác định điểm gần nhất của cánh tay robot tới vật cản Từ

đó, với việc tránh va chạm giữa hai điểm gần nhau nhất của vật cản và cánh tay robot

ta sẽ tránh được sự va chạm giữa cánh tay robot và vật cản

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

Để xét lực đẩy từ vật cản vào cánh tay robot ta xét lực đẩy từ vật cản vào một liên kết của cánh tay robot Một liên kết của cánh tay robot được định nghĩa là một đoạn thẳng nối giữa hai khớp quay của cánh tay robot

a)Lực đẩy từ một vật cản hình tròn vào một liên kết cánh tay robot

Hình 2.7 Khoảng cách gần nhất giữa cánh tay robot và vật cản hình tròn

Như được thể hiện trên Hình 2.7 ta nhận thấy việc tính khoảng cách gần nhất từ

một vật cản hình tròn tới một liên kết của cánh tay robot hoàn toàn tương tự như việc tính khoảng cách gần nhất từ một điểm tới một đường thẳng đã được nói tới ở trên

Đầu tiên, làm tương tự như với trường hợp tìm khoảng cách từ một điểm tới một đường thẳng ta sẽ tìm được hệ số tỉ lệ xác định điểm gần nhất giữa một liên kết của cánh tay robot tới vật cản hình tròn có dạng:

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

Khi đó lực đẩy từ vật cản hình tròn vào liên kết cánh tay robot sẽ có dạng:

( ) { ( )

(2.30)

Trong đó vector chỉ hướng của lực có dạng:

[

b)Lực đẩy từ một vật cản hình đa giác (tứ giác) vào một liên kết của cánh tay robot

Như đã làm ở mục 3.1 để xét được lực đẩy từ một vật cản hình tứ giác vào một

liên kết của cánh tay robot, đầu tiên ta sẽ đi tìm lực đẩy từ một đoạn thẳng của tứ giác vào một liên kết của cánh tay robot

Như ta thấy trên hình Hình 2.8 để tính khoảng cách gần nhất từ một đoạn thẳng

tới một liên kết của cánh tay robot ta quy về bài toán tìm điểm gần nhất giữa hai đoạn thẳng trong mặt phẳng

Để đơn giản hơn nữa, như được miêu tả ở trên hình Hình 2.8 ta có thể thấy,

cách tìm khoảng cách gần nhất được chuyển về một bài toán đã được nói tới ở trên là tìm khoảng cách gần nhất từ một điểm tới một đoạn thẳng

Như ta thấy: Khoảng cách gần nhất từ điểm tới đoạn thẳng là đoạn , khoảng cách gần nhất từ điểm tới đoạn thẳng là đoạn , khoảng

Hình 2.8 Khoảng cách gần nhất từ một đoạn thẳng tới

một liên kết của cánh tay robot

Chương 2: Xây dựng luật điều khiển robot tránh vật cản

cách gần nhất từ điểm tới đoạn là đoạn , còn khoảng cách gần nhất từ điểm tới đoạn là đoạn Và như chúng ta thấy đoạn chính là đoạn thẳng ngắn nhất hay cũng chính là khoảng cách gần nhất giữa hai đoạn thẳng

Vì là khoảng cách gần nhất giữa hai đoạn thẳng (vật cản và một liên kết của cánh tay robot), nên , là hai điểm gần nhau nhất lần lượt trên vật cản và liên kết của cánh tay robot

Từ đó ta có được công thức tính lực đẩy tác dụng từ một cạnh của vật cản tới một liên kết của cánh tay robot sẽ có dạng:

Trong đó: – là độ dài đoạn

– là giới hạn của trường phản lực

– chiều lực đẩy tác dụng vào liên kết của cánh tay robot

[

]

Chương 3: Xây dựng thuật toán điềuk hiển trên robot 2 bậc tự do

CHƯƠNG III: XÂY DỰNG THUẬT TOÁN ĐIỀU

KHIỂN TRÊN ROBOT 2 BẬC TỰ DO

3.1 Xây dựng đối tượng robot

3.1.1 Đối tượng điều khiển

Trước khi bắt đầu vào công việc tính toán bộ điều khiển tránh vật cản cho robot, điều đầu tiên chúng ta cần làm đó là xác định đối tượng điều khiển cho bộ điều khiển của mình

Như đã được đề cập từ đầu đề tài, phương pháp tránh vật cản được xây dựng ở đây sẽ được áp dụng cho cánh tay máy công nghiệp và cụ thể ở đây xét tới cánh tay

máy 2 bậc tự do quay trong mặt phẳng như được mô tả trên hình 3.1

Hình 3.1 Mô hình robot 2 bậc tự do được xét

Các thông số cơ bản của Robot hai bậc tự do được sử dụng:

- Liên kết 1 được coi là đồng chất khối lượng

- Liên kết 1 có chiều dài

- Liên kết 2 được coi là đồng chất khối lượng

- Liên kết 2 có chiều dài

- Mô men quán tính của liên kết 1 là ⁄

- Mô men quán tính của liên kết 2 là ⁄

- Gia tốc trọng trường áp dụng là