Đ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

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN ĐIỆN
BỘ MÔN TỰ ĐỘNG HOÁ CÔNG NGHIỆP
====o0o====

ĐỒ ÁN TỐT NGHIỆP
ĐỀ TÀ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Ụ

2


Lời Cam Đoan

LỜI CAM ĐOAN

3


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

4


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

5


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
6


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

7


Lời Nói Đầu

LỜI NÓI ĐẦU

8


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
ROBOT
1.1. Động học vị trí robot
1.1.1. Biểu diễn ma trận
Ma trận được sử dụng để biểu diễn một điểm, một vector và một khung tọa độ
trong không gian.
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 đó:

(

)

(1.1)

(

)

(1.2)

– 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:

̅

[ ]

[

[ ] ̅

[

]

]

9

(1.3)

(1.4)


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].

(1.5)
[

]

1.1.2. Phép biến đổi
Phép biến đổi dùng để mô tả chuyển động của một khung tọa độ hay một vector
trong không gian tương đối so với khung tọa độ chuẩn (khung tọa độ gốc). Có 3 dạng
phép biến đổi [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:
10


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

(

Trong đó:

)

[

]

(1.6)

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:
(

)

(1.8)

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.

11


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:
( )

( )

(1.9)

( )

( )

(1.10)
(1.11)

Các phương trình trên viết dưới dạng ma trận sẽ trở thành:

[

]

[

] [ ]

(1.12)

Từ đó ta nhận được ma trận biểu diễn phép quay xung quanh trục z như công
thức:
(

)

[

]

(1.13)

Hoàn toàn tương tự ta sẽ tính được ma trận biểu diễn các phép quanh quanh các
trục tọa độ x và y như sau:
(

)

[

]

12

(1.14)


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

(

)

[

]

(1.15)

c)Phép biểu diễn kết hợp
Thực ra phép biểu diễn kết hợp là một tập hợp các phép biến đổi tịnh tiến và
biến đổi quay đơn so với khung tọa độ cố định hoặc khung tọa độ gốc theo một thứ tự
được xác định sẵn.
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 .
Quay quanh trục x một góc .

Khi đó ta sẽ nhận được một phép biến đổi kết hợp chính là tích các ma trận của
các phép biến đổi đơn đã được liêt kê ở trên:
(

)

(

)

(

)

(

)

(1.16)

1.2. Động học thuận robot
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à
giữa hai pháp tuyến chung của trục khớp i.

. Góc

.
là góc

Đố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.

13


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:

14


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

(

)

(

)

(

)

(

)

(1.17)

Ma trận của các phép biến đổi đơn được xác định:

(

)

[

(

)

[

]

(1.19)

(

)

[

]

(1.20)

)

[

(

Trong đó:

]

(1.18)

]

(1.21)

-

,

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:

[

]

(1.22)

1.3. Phương trình động lực học của robot
1.3.1. Phương trình Lagrange
Trong phương trình Lagrange, các biến tọa độ tổng quát được sử dụng để xác
định duy nhất vị trí của vật. Phương trình Lagrange sẽ nhận được từ phương trình
Newton [2].
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à:
15

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

(

̅̅̅

)

Khối lượng của mỗi phần tử vật chất
trình chuyển động Newton có dạng:
̅̅̅

là

(1.23)
và lực đặt lên

, phương

là

̅̅̅̈

(1.24)

Lấy đạo hàm và lấy tổng đối với tất cả các phần tử vật chất của hệ thống, ta
nhận được phương trình sau:
∑ ̅̅̅

̅̅̅

̅̅̅

̅̅̅̈

∑

(1.25)

Mặt khác, từ phương trình tính ̅̅̅ ta có:
̅̅̅̇

̅̅̅

∑

̅̅̅̇
̇

̇

̅̅̅

(1.26)

̅̅̅

(1.27)

Như vậy phương trình tổng quát nhận được là:
(
Trong đó:

∑

̇

)

(1.28)

̅̅̅̇ ̅̅̅̇ – động năng của hệ thống.

∑ ̅̅̅

̅̅̅̅

– lực tổng quát tương ứng với tọa độ

.

Ngoài ra ̅̅̅ gồm hai thành phần: thành phần trọng lực và các thành phần còn lại.
Thành phần trọng lực đượct ính từ thế năng của phần tử:
(1.29)
Khi đó nhận được thành phần

có dạng sau:

∑ ̅̅̅̅̅

̅̅̅
(1.30)

Hàm Lagrange là hiệu của động năng và thế năng của vật có dạng:
(1.31)

16


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.32)

1.3.2. Phương trình động lực học cho cánh tay robot
a)Ma trận Jacobian
[
] và một l vecto
[
]
là hàm của thời gian và có đạo hàm cấp 1 [2].

Giả sử tồn tại một vecto

(
) và ,
trong đó:
Khi đó:

(1.33)

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

̅

(1.34)
[

Kí hiệu ma trận (

) là

]

( ̅) – ma trận Jacobian của

( ̅)

đối với ̅ :

(1.35)
[

]

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

( ̅)

̅

(1.36)

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].
17


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

̅

̅̅̅̅

(1.37)

Hay:

[
[

Trong đó:

]

]

(1.38)
[

]

- dx,dy,dz là các dịch chuyển vi sai của tay robot tương ứng dọc theo
các trục x,y,z.
-

,

,

tương ứng là các góc quay vi sai của tay xung quanh các trục

x,y,z.
- ̅ vecto dịch chuyển vi sai của tay Robot và ̅̅̅̅ là vecto dịch chuyển vi
sai của khớp.
Nếu chia hai vế của công thức trên cho dt ta sẽ nhận được phương trình quan hệ
tốc độ của tay robot và tốc độ khớp.
Ngoài ra, do chúng ta chỉ làm mô phỏng về luật điều khiển robot và không quan
tâm tới cấu trúc phần cứng cũng như cơ cấu chấp hành của robot (chúng ta coi cơ cấu
chấp hành và cấu trúc phần cứng như một phần tử quán tính bậc nhất). Chính vì vậy
công thức trên còn có cách biến đổi khác:

̅
̅

̅

(1.39)
̅

(1.40)

Hay:
̅

̅

18

(1.41)


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.

19


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 đó:

( )

( )

(2.1)

( ): 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:
( )
Trong đó:

( )

[

( )

[

( )

( )

(2.2)

( )]
( )]

( ) chính là lực hút cho phép một điểm từ vị trí x có thể di chuyển tới vị trí
đích

.

( ) chính là lực gây ra bởi một lực đẩy nhân tạo từ bề mặt của vật cản
(FIRAS) tạo ra bởi trường ( ).
2.1.1. Lực hút tới điểm đích
( ) cần là một hàm tỉ lệ với khoảng cách:

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

(

)

(2.3)

( ) sẽ có dạng:

Khi đó trường thế hút
( )

⁄

(

20

)

(2.4)


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.5)

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 ( )
trường vật cản:
( )

{

(

để miêu tả vật cản ta có biểu thức cơ bản của

( )

( )

)

( )

( )

( )

( )

(2.6)

Từ biểu thức trên ta nhận thấy, trường thế năng của vật cản bị giới hạn bởi hai
mặt ( )
và ( )
( ) trong đó là một điểm gần vật cản, còn 𝜂 là hệ số tỉ lệ.

Tuy nhiên biểu thức trên rất khó sử dụng với các vật cản có hình dáng không ở
dạng cơ bản (hay không thể tìm hàm miêu tả một cách dễ dàng), từ đó gây khó khăn
trong việc xây dựng các mặt đẳng thế xung quanh vật cản. Khi đó bằng việc sử dụng
khoảng cách tới vật cản O ta sẽ có trường thế nhân tạo mới:
( )
Trong đó:

{

(

)

(2.7)

: 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:

21


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

(

Trong đó:

)

{

(

)

(2.8)

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:
)

√(
Trong đó:

,

,

(

)

(

)

(2.9)

: 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:
[

]

(2.10)

Với :
)

√(

(

)

(

)

(2.11)

2.1.3. Tổng lực tác dụng vào chất điểm
Như đã nói ở trên, lực tác dụng vào chất điểm là lực do trường thế nhân tạo
trong môi trường tác dụng (bao gồm trường lực hút tới đích và trường lực đẩy từ vật
cản) .
) khi đó lực tác động vào một điểm chỉ có

Nếu khoảng cách tới vật cản (
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 (
vào chất điểm có dạng:
( )

(

)

√(

)

̇

(

) khi đó lực tác động

)

(2.13)

Trong đó:
Với:

(

22

)

(

)


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:
( )

(

)

23

(2.14)


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

Với:

√(

)

(

)

(

[

)

]

Các thông số trong phương trình gồm có: hệ số của lực đẩy từ vật cản ( phụ
thuộc vào kích thước, khả năng gia tốc của điểm tác dụng ),
khoảng cách từ vật cản
[
] là tọa
tới phần tử thứ n, là khoảng giới hạn của trường lực đẩy,
[
]
độ 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:
(

( )
Với:

√(

)

(

)
)

(

[

(2.15)
)

]

Các thông số trong phương trình gồm có: hệ số của lực đẩy từ vật cản ( phụ
thuộc vào kích thước, khả năng gia tốc của điểm tác dụng ),

khoảng cách từ vật cản
tới điểm M gần nhất tới vật cản,
là khoảng giới hạn của trường lực đẩy,
[
] là tọa độ của điểm M gần nhất tới vật cản, [
] là tọa độ của
điểm gần nhất từ vật cản tới cánh tay robot.
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
24


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
(
) và bán kính r. Ta
Xét vật cản có dạng hình tròn với tâm có tọa độ
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:
(2.16)
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:
√(

)

(

)

(2.17)

 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:
25