Động học ngược tay máy chuỗi dư dẫn động: Chuyển động lặp của biến khớp

Bài viết trình bày một phương pháp giải bài toán động học ngược tay máy chuỗi dư dẫn động. Trên cơ sở phương trình động học ở mức vận tốc, véctơ vận tốc suy rộng được giải dựa trên tiêu chuẩn tối ưu. Không gian bù của ma trận Jacobi được khai thác để đảm bảo chuyển động lặp của các tọa độ khớp.

Tuyển tập Hội nghị khoa học toàn quốc lần thứ nhất về Động lực học và Điều khiển

Đà Nẵng, ngày 19-20/7/2019, tr 183-189, DOI 10.15625/vap.2019000276

Động học ngược tay máy chuỗi dư dẫn động: chuyển động lặp

của biến khớp

Nguyễn Quang Hoàng

Bộ môn Cơ ứng dụng – Viện Cơ khí, Đại học Bách khoa Hà Nội

E-mail: hoang.nguyenquang@hust.edu.vn

Tóm tắt

Với các tay máy đủ dẫn động, khi khâu cuối thực hiện các

chuyển động lặp các tọa độ khớp cũng sẽ thực hiện các chuyển

động lặp Tuy nhiên, đối với các tay máy dư dẫn động để các

tọa độ khớp thực hiện chuyển động lặp đòi hỏi phải có phương

pháp xử lý bài toán động học ngược Bài báo trình bày một

phương pháp giải bài toán động học ngược tay máy chuỗi dư

dẫn động Trên cơ sở phương trình động học ở mức vận tốc,

véctơ vận tốc suy rộng được giải dựa trên tiêu chuẩn tối ưu

Không gian bù của ma trận Jacobi được khai thác để đảm bảo

chuyển động lặp của các tọa độ khớp Giá trị biến khớp tìm

được nhờ các phép tính tích phân véctơ vận tốc suy rộng Ngoài

ra, để giảm các sai số tích lũy phương pháp phản hồi sai số động

học được đưa vào Các mô phỏng số được thực hiện với tay

máy phẳng 5 bậc tự do Các kết quả cho thấy các tọa độ khớp

thực hiện các chuyển động lặp khi khâu cuối thực hiện các

chuyển động lặp

Từ khóa: Tay máy dư dẫn động, Động học ngược, Chuyển động

lặp, Phản hồi sai số động học

1 Mở đầu

So với robot đủ dẫn động, robot dư dẫn động có

nhiều ưu điểm vì chúng cho phép tối ưu quỹ đạo chuyển

động, tránh được vật cản, tránh được các điểm kỳ dị,

tránh các giới hạn khớp [1, 9, 10, 11] Robot dư dẫn động

có số tọa độ khớp lớn hơn số bậc tự do của bàn kẹp, điều

này cho phép có nhiều phương án giải quyết bài toán

động học ngược Đối với tay máy đủ dẫn động, khi khâu

cuối thực hiện chuyển động lặp theo chu trình, các biến

khớp cũng sẽ thực hiện chuyển động lặp tương ứng Tuy

nhiên, điều này có thể không đúng với tay máy dư dẫn

động nếu không có sự can thiệp thích hợp trong bài toán

động học ngược Bài toán xác định chuyển động lặp của

biến khớp được được nghiên cứu trong thời gian gần đây

[12] Trong công trình này các tác giả đã thiết lập và đưa

bài toán về dạng quy hoạch dạng toàn phương với các

ràng buộc và sau đó giải bằng những thuật toán khá phức

tạp Các thuật toán này khó có khả năng triển khai theo

thời gian thực

Phương án dựa trên ma trận Jacobi của phương trình

liên kết hay được sử dụng nhất, do tính chất đơn giản của

nó Với phương pháp này ta chỉ cần giải hệ phương trình

đại số tuyến tính có số ẩn nhiều hơn số phương trình Các tọa độ khớp sau đó nhận được bằng cách tích phân các vận tốc biến khớp theo thời gian, với điều kiện đầu tương thích Để hạn chế sai số tích lũy trong quá trình tính tích phân các phương pháp như phản hồi động học [10], hiệu chỉnh gia lượng sai số véctơ tọa độ suy rộng [5,6,7] thường được sử dụng

Trong bài báo này, vấn đề chuyển động lặp của các biến khớp được quan tâm giải quyết bằng việc sử dụng không gian bù của ma trận Jacobi với véctơ tự do được chọn một cách thích hợp

Phần còn lại của bài báo được trình bày như sau: việc thiết lập và phương pháp giải bài toán được trình bày trong mục 2 và 3 Mục 4 trình bày các mô phỏng số đối với một tay máy phẳng 5 bậc tự do Cuối cùng là một số kết luận được đưa ra

2 Đặt bài toán và phương pháp giải

Nêu bài toán

Xét tay máy n bậc tự do, gọi q n là véctơ chứa các tọa độ khớp Bàn kẹp của robot vận hành trong không gian thao tác, gọi x m là véctơ chứa vị trí và hướng của bàn kẹp m ( m 6 ) Bài toán động học thuận robot được giải quyết bằng các phương pháp hình học, quy tắc Denavit-Hartenberg hoặc Craig [2, 3, 4, 9, 10] Kết quả của bài toán động học thuận cho ta liên hệ sau:

( , ) 0

f x q , x f, m,q n (1) Tay máy là đủ dẫn động nếu số chiều của không gian

thao tác bằng số chiều của không gian khớp, tức m n

Bài toán ngược động học của loại tay máy này yêu cầu ta giải phương trình (1) tìm q( )t khi cho biết x( )t Bài toán này có thể được giải bằng các phương pháp sau: 1 Giải hệ phương trình đại số phi tuyến bằng phương pháp lặp Newton-Raphson, 2 Sử dụng phương pháp hình học

để nhận được nghiệm giải tích, 3 Sử dụng phương pháp các nhóm 3

Tay máy là dư dẫn động khi m n , số bậc tự do

của tay máy lớn hơn số bậc tự do của bàn kẹp Các phương pháp nêu trên đối với tay máy đủ dẫn động khó

áp dụng đối với loại tay máy này Thông thường, bài toán

Nguyễn Quang Hoàng

động học ngược được giải ở mức vận tốc

Đạo hàm phương trình (1) theo thời gian ta nhận

được phương trình liên hệ ở mức vận tốc:

J x x J q q 0, (2)

với các ma trận Jacobi như sau

J x q x( , ) f/ x, J x q q( , ) f/ q

Đối với trường hợp robot phẳng, m 3,

[ , , ]x y T

x , quan hệ (1,2) có thể được viết ở dạng

tường minh như sau:

( )

( )

Hình 1 Sơ đồ tay máy chuỗi Bài toán đặt ra ở đây là: Cho biết chuyển động lặp

của bàn kẹp với chu kỳ T, tức là biết các hàm x( ), ( )t x t

thảo mãn x( )T x(0), và x( )T x(0) 0, ta cần

tìm chuyển động của các tọa độ khớp q( )t thỏa mãn

( )T (0)

q q hoặc ít nhất là || ( )q T q(0) ||

Phương án giải quyết

Giả sử rằng ma trận Jacobi J cỡ m n có hạng q

đầy đủ, rank( )J q m Nếu biết x và q từ phương

trình (2) hoặc từ (4) ta sẽ giải được các vận tốc khớp q

Sau đó, thực hiện tích phân và đạo hàm ta nhận được q

và q

3 Giải bài toán động học ngược tay máy dư

dẫn động ở mức vận tốc

3.1 Tối ưu chuẩn của véctơ vận tốc suy rộng

Phần này trình bày việc giải phương trình (4) kết hợp

điều kiện chuẩn của véctơ vận tốc suy rộng nhỏ nhất Ta

cần giải (4) tìm q phụ thuộc x , với điều kiện

Kết quả là

( )

W

với

được gọi là ma trận tựa nghịch đảo có trọng số của ma trận Jacobi J q [8] ( )

Nếu chọn ma trận trong số là ma trận đơn vị,

W I , nghiệm tính theo công thức (5) sẽ có chuẩn nhỏ

nhất

Nếu chú ý đến không gian bù của ma trận Jacobi, nghiệm của (4) sẽ là

q J q x W( ) (I J J z , W ) 0 (8) với z0 n là véctơ tùy ý Véctơ này sẽ tạo ra chuyển động cho các khâu mà không ảnh hưởng đến chuyển động của bàn kẹp Thông thường véctơ này sẽ được chọn

để khai thác thêm các ưu điểm của tay máy dư dẫn động như tránh vật cản, tránh điểm kỳ dị, tránh va vào các giới hạn khớp Thông thường người ta hay tính z0 theo công thức

0

( )q T z

với ( )q là các hàm mục tiêu phụ thuộc vào yêu cầu đặt

ra Chẳng hạn để tránh điểm kỳ dị, ta chọn hàm này là hàm đo khả năng thao tác:

Hàm này triệt tiêu tại các điểm kỳ dị Do đó, việc cực đại hàm này sẽ giúp robot tránh được các điểm kỳ dị trong quá trình hoạt động

Để tránh va vào các giới hạn khớp, người ta đưa vào hàm khoảng cách tới giới hạn khớp:

2

1

1 ( )

2

n

i i i

q q c



=

−



với qiM (qim) là ký hiệu của giới hạn lớn nhất (nhỏ nhất) và qi là giá trị giữa của khoảng làm việc của khớp; ci là các trọng số Do đó, cực đại hàm khoảng cách này, tính dư dẫn động sẽ được khai thác để giữ cho các biến khớp gần giá trị giữa của giới hạn khớp, và tránh được sự va vào các giới hạn khớp

Để tránh va vào vật cản, ta sử dụng hàm khoảng cách tới vật cản

với o là véctơ vị trí của một điểm thích hợp trên chướng ngại vật (ví dụ tâm trong trường hợp mô hình vật cản là hình cầu) và p q là véctơ vị trí suy rộng cấu ( ) trúc của robot Do đó, cực đại khoảng cách này sẽ giúp robot tránh được vật cản trong quá trình hoạt động Trên thực tế robot không gian, việc mô hình các vật cản cũng như xác định giá trị hàm này là khá phức tạp

Trong bài báo này, để các tọa độ khớp lặp lại sau mỗi chu kỳ di chuyển của bàn kẹp, hàm mục tiêu tránh va vào giới hạn khớp (11) được chỉnh lại thành

End-Effector

z

x

y

O

Động học ngược tay máy chuỗi dư dẫn động: chuyển động lặp của biến khớp

n

i i i

q q c

2

1

(0) 1

( )

2

q



=

−

Khi đó

0

( )

T

q

với Kii = ci/( qiM − qim)2, i = 1, 2, , n

Sự lựa chọn theo (14) có thể đảm bảo rằng nghiệm

( )t

q sẽ tuần hoàn khi x( )t tuần hoàn, và q( )t sẽ luôn

bị hút về lân cận q(0) như vậy sẽ tránh được sự tăng

hoặc giảm liên tục của biến khớp và do đó có thể sẽ tránh

được va chạm vào giới hạn khớp

3.2 Xác định giá trị xuất phát (0)q

Việc tích phân q từ (8) để nhận được q( )t ta cần

phải có giá trị xuất phát q(0) ứng với x(0) Do số ẩn

nhiều hơn số phương trình nên (3) sẽ có vô số nghiệm Ở

đây ta sẽ tìm nghiệm (0)q sao cho các tọa độ khớp gần

với giá trị trung bình của mỗi biến khớp Như thế bài toán

tìm giá trị ban đầu là trở thành việc tìm nghiệm của bài

toán tối ưu có ràng buộc sau: cực tiểu hóa hàm ( )q

tính theo (11) với ràng buộc (3) Bài toán này dễ dàng

giải được bằng các công cụ phần mềm

Một phương án khác để xác định giá trị xuất phát đó

là sử dụng phương pháp lặp Newton-Raphson Theo đó ta

chọn xấp xỉ thô ban đầu q(0) sau đó lặp để hiệu chỉnh

dựa trên tựa nghịch đảo của ma trận Jacobi

3.3 Ổn định hóa bằng phản hồi sai số động học

Do có sai số của phương pháp và sai số làm tròn

trong quá trình tìm q( )t từ các phương trình (6) hoặc

(8) bằng các phương pháp số, nghiệm q( ), ( )t q t tìm

được có thể không còn thỏa mãn các phương trình liên

kết (3) Trong phần này trình bày phương pháp phản hồi

sai số động học

Hình 2 Sơ đồ khối của phương pháp

Xét phương trình động học sai số

với e x f q Rõ ràng là nghiệm của (15) có dạng ( )

( )t (0) exp( t)

e e và như thế e( )t 0 khi t

tăng đủ lớn Bằng cách này ta sẽ giải tìm q từ phương

trình

Khi đó nghiệm (8) trở thành

0

Sơ đồ khối mô phỏng thuật toán giải bài toán động học ngược được thể hiện như trên Hình 2

4 Mô phỏng số

Trong phần này, các kết quả mô phỏng số được đưa

ra Đối tượng khảo sát là một tay máy phẳng 5 bậc tự do với các khớp quay chuyển động trong mặt phẳng đứng

Mô hình và các thông số của tay máy được đưa ra như trên Hình 3 và Bảng 1

Hình 3 Tay máy phẳng 5 bậc tự do

Bảng 1 Các thông số của tay máy

Kết quả của bài toán động học thuận cho ta phương trình sau

( )

x f q ,

với

T

f q

ở đây x [ , , ]x y T là véctơ chứa vị trí x y( , ) và hướng của bàn kẹp ; q [q q q q q1 2 3 4 5]T là véctơ chứa các biến khớp

Các mô phỏng được thực hiện với các dạng quỹ đạo của điểm cuối: chuyển động tiến lui trên quỹ đạo là một đoạn thẳng, một cung tròn, chuyển động lặp trên một đường tròn

Chuyển động tiến – lui trên một đoạn thẳng, một cung tròn

Giả sử điểm cuối cần di chuyển từ A đến B và quay lại A trên một đoạn thẳng hoặc một cung tròn, vận tốc tại A và B

bằng 0 Thời gian cho một chu trình chuyển động là T quãng đường dịch chuyển từ A đến B và quay lại A là 2L

Ở đây ta chọn luật vận tốc có dạng hình sin, từ đó suy ra được luật chuyển động như sau:

g

x

q2

q3

q4

q1

O

A

C



E

q5

Tính q

q

x(0)

1

s

( )

( )

t

t

x

x

Tìm q(0)

Nguyễn Quang Hoàng

0

2

2

2

t

T

T

Hình 4 Dạng đồ thị vận tốc và dịch chuyển theo thời gian

Trong trường hợp khảo sát ở đây ma trận trọng số là

ma trận đơn vị Các ma trận K và trong biểu thức

(14) và (17) được chọn là

diag(50, 50, 50, 50, 50)

K

diag(10,10,10,10,10)

Điểm cuối chuyển động trên đoạn thẳng

Xét trường hợp xuất phát từ A đến B và quay lại A,

với r A(1.2, 0.5), r B(0.5,1.2), 0.2 rad Kết quả mô

phỏng được đưa ra trên các Hình 5, 6, 7

Hình 5 Đồ thị các biến khớp theo thời gian q(t)

Hình 6 Quỹ đạo pha của các biến khớp

Hình 7 Các cấu hình tay máy khi di chuyển

Điểm cuối chuyển động trên cung tròn

Xét trường hợp cung tròn có tâm C r C(0.5, 0.5), điểm xuất phát A r A(1.2, 0.5), góc quét 2 rad Kết quả mô phỏng được đưa ra trên các Hình 8-14

Hình 8 Đồ thị các biến khớp theo thời gian q(t)

Hình 9 Quỹ đạo pha của các biến khớp

Động học ngược tay máy chuỗi dư dẫn động: chuyển động lặp của biến khớp

Hình 10 Các cấu hình tay máy khi di chuyển

Hình 11 Quỹ đạo pha các biến khớp, K= 6, z0 = K(q - q0)

Hình 12 Quỹ đạo pha các biến khớp, K = 6, z0= K(q - q_)

Hình 13 Quỹ đạo pha các biến khớp, [K = 0,  = 0]

Hình 14 Quỹ đạo pha các biến khớp [K = 0,  = 10]

Nhận xét: đồ thị quỹ đạo pha khép kín chứng tỏ chuyển động của các góc khớp được lặp lại, trừ trường hợp Hình 12, chuyển động được lặp lại sau một số chu trình Hình 13 và Hình 14 cho thấy, trong trường hợp bàn kẹp chuyển động tiến lui ta không cần phải sử dụng đến không gian bù của ma trận Jacobi cũng như phản hồi sai

số động học nếu số chu trình lặp lại không lớn Do chuyển động lui ngược với chuyển động tiến và do quá trình đi và về giải theo chuẩn tối ưu vận tốc suy rộng cực tiểu, nên chuyển động sẽ lặp lại

Để thấy được ảnh hưởng của điều kiện đầu q(0), ta xét trường hợp điều kiện đầu q(0) được tìm bằng phương

pháp lặp Newton-Raphson với một xấp xỉ thô ban đầu Trong quá trình hiệu chỉnh ta sử dụng tựa nghịch đảo của

ma trận Jacobi Các mô phỏng được thực hiện cho hai trường hợp ứng với z0 K q( q) và

z K q q Quỹ đạo pha được đưa ra trên các Hình 15 và Hình 16

Hình 15 Quỹ đạo pha các biến khớp z0 = K(q - q_), K = 10

Hình 16 Quỹ đạo pha các biến khớp, z0 = K(q-q(0)), K = 10

Nguyễn Quang Hoàng

Nhận xét: Hình 15 cho thấy chuyển động của các

biến khớp được di chuyển về gần với giá trị trung gian

của các biến khớp Sau đó đường quỹ đạo pha mới đóng

kín và thực hiện chuyển động lặp Hình 16 cho thấy quỹ

đạo pha của 5 biến khớp là những đường khép kín, điều

này cho thấy các biến khớp thực hiện chuyển động tuần

hoàn

Chuyển động chu trình một chiều trên đường (tròn)

khép kín

Giả sử thời gian chuyển động một chu trình là T và chiều

dài quãng dịch chuyển là L Đặc điểm của dạng chuyển

động này trên quỹ đạo như sau:

Luật di chuyển trên quỹ đạo có thể được chọn là các đa

thức bậc 3, 5, 7, hoặc cũng có thể chọn với profile vận tốc

dạng hình tam giác cân, hình thang, … Ở đây ta chọn luật

vận tốc có dạng hình sin, từ đó suy ra được luật chuyển

động như sau:

0

/ 2 ,

Hình 17 Quỹ đạo pha các biến khớp, [K = 0,  = 0]

Hình 18 Quỹ đạo pha các biến khớp, z0 =-K(q-q0)

Hai mô phỏng được thực hiện khi cho điểm cuối di

chuyển trên trường tròn tâm C(1.0, 0.5) m, bán kính r =

0.2 m, điểm xuất phát A(1.2, 0.5), thời gian chuyển động

một vòng là T = 4 s Trong mô phỏng thứ nhất, ta cho

véctơ z0 0- tức là không gian bù của ma trận Jacobi không được sử dụng, còn trong mô phỏng thứ 2 ta cho

z K q q Kết quả của hai trường hợp này được đưa ra trên các Hình 17-19

H 1 Cấu hình tay máy chuyển động lặp, z0 =-K(q-q0) Nhận xét: Hình 18 cho thấy, khi không sử dụng đến

z0 chuyển động của các biến khớp bị trôi sau mỗi chu trình, các góc khớp không thực hiện các chuyển động lặp Trái lại, Hình 18 cho thấy quỹ đạo pha là các đường đóng kín – tức là các khớp chuyển động lặp

5 Kết luận

Bài báo tập trung giải quyết bài toán động học ngược robot dư dẫn động dựa trên các phương trình liên kết ở mức vận tốc Trên cơ sở đó đã khảo sát sự lặp lại của các biến khớp khi khâu cuối thực hiện các chuyển động lặp theo chu trình Phương pháp phản hồi sai số động học được đưa vào để giảm sai số tích lũy khi tích phân Ngoài

ra, không gian bù của ma trận Jacobi cũng được khai thác

để đảm bảo cho các biến khớp không tăng khi khâu cuối thực hiện chuyển động lặp Tính đúng đắn và tin cậy của phương pháp đã được khẳng định thông qua các mô phỏng số đối với tay máy phẳng 5 bậc tự do

Tài liệu tham khảo

[1] Nakamura Y.: Advanced Robotics/Redundancy and Optimization Addison-Wesley Publishing Company, Reading 1991

[2] Nguyễn Thiện Phúc: Robot công nghiệp Nhà xuất bản Khoa học và Kỹ thuật, Hà Nội, 2004

[3] Nguyễn Văn Khang: Động lực học hệ nhiều vật Nhà xuất bản Khoa học và Kỹ thuật, Hà Nội, 2007

[4] Nguyễn Văn Khang, Chu Anh Mỳ: Cơ sở robot công nghiệp Nhà xuất bản Giáo dục, Hà Nội, 2011

[5] Nguyễn Văn Khang, Lê Đức Đạt, Trần Hoàng Nam Về một thuật toán giải bài toán động học ngược robot dạng

Động học ngược tay máy chuỗi dư dẫn động: chuyển động lặp của biến khớp

chuỗi Tuyển tập Hội nghị Cơ học toàn quốc lần thứ VIII,

Tập 1, Hà Nội 2008

[6] Nguyen Van Khang, Nguyen Quang Hoang, Tran Hoang

Nam: On an efficient method for improving the accuracy

of the inverse kinematics of robotic manipulators Int

Conference on Engineering Mechanics and Automation

(ICEMA 2010), Hanoi, July 1-2, 2010, pp 186-194

[7] Nguyen Quang Hoang, Nguyen Van Khang: On kinematic

inverse and control of redundant manipulators under

consideration of jammed joint Proceed Iftomm 1

International Symposium on Robotics and Mechatronics,

2009, Hanoi, Vietnam, pp.201-207

[8] Rao, C.R.: Generalized Inverse of Matrices and its

Applications New York, Wiley, 1971

[9] Spong M W.; Hutchinson S and Vidyasagar M.: Robot

Modeling and Control John Wiley & Sons, New York,

2006

[10] Sciavicco L., Siciliano B.: Modelling and Control of Robot

Manipulators, 2nd Edition, Springer-Verlag, London, UK,

2000

[11] Zhang Y and Wang J.: Obstacle Avoidance for

Kinematically Redundant Manipulators Using A Dual

Neural Network IEEE Transactions on systems, man, and

cybernetics–part b: cybernetics, vol 34, no 1, february

2004

[12] Yunong Zhang & Zhijun Zhang: Repetitive Motion

Planning and Control of Redundant Robot Manipulators

Springer-Verlag Berlin Heidelberg 2013