Độ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

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:

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

đạ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 q 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 tố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.

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.

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

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 qút bài tố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 tồn phương với các
ràng buộc và sau đó giải bằng những thuật tốn khá phức
tạp. Các thuật tố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

Nêu bài toán
n
Xét tay máy n bậc tự do, gọi q
là véctơ
chứa các tọa độ khớp. Bàn kẹp của robot vận hành trong
m
không gian thao tác, gọi x
là véctơ chứa vị trí và
m
hướng của bàn kẹp
( 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:
m
n
f (x, q ) 0 ,
.

(1)
x, f
,q
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
tố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 tố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 qq 0 ,
(2)
với các ma trận Jacobi như sau
J x (x, q )
f / x, Jq (x, q )
f/ q.
Đối với trường hợp robot phẳng, m 3,
x [x, y, ]T , quan hệ (1,2) có thể được viết ở dạng

tường minh như sau:
x f (q )
(3)
x J (q )q
(4)

End-Effector

z

đượ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 JW (q )x (I JW J )z 0 ,
(8)
n
với z 0
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 z 0 theo công
thức

z0

T

(9)

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:

y

O

(q )
q

x

(q )

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
q (T ) q (0) hoặc ít nhất là || q (T ) q(0) ||
.

det[J (q)J T (q )]

(10)

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

Phương án giải quyết
Giả sử rằng ma trận Jacobi J q cỡ m n có hạng
đầy đủ, rank(Jq ) 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

f

1
2

qTWq

min,

W

0

(5)

Kết quả là
q

JW (q )x

(6)

với
†
J (q )W

W 1J T (q )[J (q )W 1J T (q )]

1

(7)

1 n  q − qi 
(q) = −  ci  i

2 i =1  qiM − qim 

(11)

với qiM ( q im ) 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; c i 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

(q )

min p(q ) o

(12)

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

Độ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

1 n  q − qi (0) 
(q) = −  ci  i

2 i =1  qiM − qim 

2

trình
(13)

Khi đó

x

Jq (q )q

e

Jq (q )q

x

e

(16)

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

z0

(q )
q

T

K (q

q (0)) .

(14)

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
q (t ) sẽ tuần hoàn khi x (t ) tuần hồn, và q (t ) sẽ ln
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 q (0)
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 q (0) 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 tốn
tìm giá trị ban đầu là trở thành việc tìm nghiệm của bài
tố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.

x (t )
x (t )

x(0)

Tính

q

1
s

q

Tìm q(0)
Hình 2. Sơ đồ khối của phương pháp

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

e,
0,
(15)
với e x f (q ) . Rõ ràng là nghiệm của (15) có dạng
e(t ) e(0) exp( t ) 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

q

JW (q )[x

e]

(I

JW J )z 0 ,

(17)

Sơ đồ khối mơ phỏng thuật tốn giải bài tố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.
B

y

q3

C

q2

g

q4


A
q1

E
q5

O

x
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
khâu i

1

2

3

4

5

l [m]

0.55

0.50

0.45

0.40

0.20

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

5

với f (q )
k 1

Lk sin(

i 1

k

5

qi ),

k 1

Lk cos(

i 1

T

5

qi ),

i 1

qi

ở đây x [x, y, ]T là véctơ chứa vị trí (x , y ) và
hướng của bàn kẹp
; q [q1 q2 q 3 q 4 q5 ]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:


Nguyễn Quang Hồng

v(t )

v0 sin

s(t )

v0

2 t
,
T

T
1

2

cos

v0

L /T,

2 t
.
T

Hình 4. Dạng đồ thị vận tốc và dịch chuyển theo thời gian
Hình 7. Các cấu hình tay máy khi di chuyển

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à
K
diag(50, 50, 50, 50, 50)
diag(10, 10, 10, 10, 10)

Đ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 rC (0.5, 0.5),
2 rad. Kết
điểm xuất phát A rA (1.2, 0.5), góc quét
quả mơ phỏng được đưa ra trên các Hình 8-14.

Đ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 rA (1.2, 0.5), rB (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 8. Đồ thị các biến khớp theo thời gian q(t)

Hình 5. Đồ 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
Hình 6. 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 14. Quỹ đạo pha các biến khớp [K = 0,  = 10]

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

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 q 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
z0
K (q q )
trường
hợp
ứng
với
và
z0
K (q q(0)) . 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 13. Quỹ đạo pha các biến khớp, [K = 0,  = 0]
Hình 16. Quỹ đạo pha các biến khớp, z0 = K(q-q(0)), K = 10


Nguyễn Quang Hồ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.

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
0 - tức là không gian bù của ma trận Jacobi
véctơ z 0
khơng được sử dụng, cịn trong mơ phỏng thứ 2 ta cho
z0
K (q q(0)) . Kết quả của hai trường hợp này
được đưa ra trên các Hình 17-19.

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:

x (0)
s(0)

x (T ),
0,

x (0)
s(T )

x (T )

L,

0,
s(0)

s(T )

0

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:

v0
v(t )

L / 2T ,
v0 sin

t
, s(t )
T

v0

T

1

cos

t
.
T

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

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

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. Ngồ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 Hồng Nam. Về
một thuật tốn giải bài tốn động học ngược robot dạng

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 =


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