Điện, điện tử - Động học tổng quát khâu công tác đối với robot đa nhiệm

Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 25
ĐỘNG HỌC TỔNG QUÁT KHÂU CÔNG TÁC 
ĐỐI VỚI ROBOT ĐA NHIỆM 
Đặng Nam Kiên*, Phạm Minh Tân, Vũ Đức Tuấn 
Tóm tắt: Công cụ gắn vào đầu công tác tay máy có các đặc điểm khác nhau nên 
cần có cách tính động học tổng quát cho các loại công cụ khác nhau để rút ngắn 
thời gian gá lắp, thay đổi; đặc biệt là các loại robot đa nhiệm. Bài báo xây dựng 
phương pháp tính toán động học thuận, động học ngược tổng quát cho đầu công tác 
tay máy đa nhiệm trong công nghiệp sau khi gá lắp công cụ. 
Từ khóa: Tự động hóa; Kỹ thuật robot; Robot 6 bậc tự do nối tiếp (6R). 
1. ĐẶT VẤN ĐỀ 
Robot 6 bậc tự do dạng nối tiếp là robot (cánh tay máy) được dùng rộng rãi 
nhất hiện nay trong công nghiệp. Vì thế robot này đã được nghiên cứu rất nhiều, 
rất sâu để ứng dụng trong nhiều loại công việc khác nhau như gắp, hàn, cắt, sơn, ... 
Tuy nhiên chưa có nhiều nghiên cứu về cách gá lắp và tính toán động học cho công 
cụ gắn vào khâu công tác tay máy. Trong thực tế không phải công cụ nào cũng dễ 
dàng gá lắp và có thể sử dụng ngay với việc áp dụng kết quả bài toán động học 
robot đơn thuần. Bài báo này sẽ đưa ra giải pháp tổng quát cho việc tính toán động 
học cho tay máy sau khi gá lắp công cụ. 
2. NỘI DUNG 
2.1. Các loại công cụ gắn vào khâu công tác 
 Robot dùng trong công nghiệp có các công việc như gắp thì dùng bàn kẹp, 
robot hàn thì dùng đầu hàn, robot cắt thì dùng đầu cắt, robot sơn thì dùng vòi phun 
sơn,...Có nhiều công cụ có thể gá lắp thẳng trục của khâu công tác sẽ rất thuận lợi 
cho việc tính toán động học. Những trường hợp này chỉ cần thay thế tham số (d6 
trong phương pháp ma trận Denavit - Hatenberg) bằng giá trị lớn hơn [3] (do đoạn 
công cụ gá lắp thêm vào) là bài toán động học không thay đổi. Trường hợp nếu 
công cụ gá lắp có cấu tạo phức tạp hơn, ví dụ như quá dài nên không thể gá lắp 
thẳng trục khâu công tác, hay bản thân công cụ đã có cấu tạo gấp khúc như đầu hàn 
hay đầu cắt plasma,... như trong hình 1. Khi đó ta cần có cách gá lắp và phải tính 
toán động học thêm cho phần cấu tạo phức tạp này. 
Hình 1. Các loại đầu máy cắt plasma có thể được gá lắp cho robot. 
2.2. Phương pháp tiếp cận 
 Mô hình tổng quát cho công cụ cần gá lắp là một hình gấp khúc, thể hiện 
bằng tam giác PKM. Trong đó điểm P là điểm cuối hay khâu công tác tay máy, PK 
và KM là hai đoạn mô phỏng công cụ gắn vào tay máy. Với một công cụ cần sử 
dụng thì tam giác PKM hoàn toàn xác định, trong bài báo này chúng ta xác định ba 
cạnh PK, KM và PM. Tương ứng là PK = d71, KM = d72, PM = l; 
Kỹ thuật điều khiển & Tự động hóa 
Đ. N. Kiên, P. M. Tân, V. Đ. Tuấn, “Động học tổng quát  đối với robot đa nhiệm.” 26 
Hình 2. Hình mô phỏng công cụ gắn vào tay máy. 
 Để việc tính toán động học được dễ dàng hơn ta gắn tam giác PKM này sao 
cho PK thẳng theo trục 6 (d6 là độ dài trục 6, tương ứng với 1 hệ số trong phương 
pháp Denavit - Hatenberg [1]) của tay máy, khi đó vector hướng bàn kẹp ⃗ là 
vector vuông góc với mặt phẳng PKM (hay vector hướng bàn kẹp ⃗ là vector pháp 
tuyến của mặt phẳng PKM) còn vector hướng tiếp cận 	⃗ trùng hướng vector ⃗ . 
Vector tiếp cận thực sự giờ là vector ⃗ . 
Hình 3. Hình mô phỏng cách gá lắp công cụ vào tay máy. 
 Thuận lợi lớn nhất của cách làm này là ta hoàn toàn tận dụng được cách 
tính động học robot vốn đã được bàn và nghiên cứu rất nhiều bằng các phương 
pháp khác nhau [2]. Việc tính toán động học của tay máy có công cụ khi đó sẽ tiến 
hành theo 2 bước, động học không có công cụ (đã giải quyết và phổ biến) và động 
học phần công cụ gá lắp. 
2.3. Cách giải động học tổng quát cho tay máy 6 bậc tự do dạng nối tiếp có 
công cụ 
2.3.1. Động học thuận cho tay máy có công cụ 
 Với bài toán thuận ta sẽ cần tìm được vector ⃗, vector ⃗ , vị trí điểm cuối 
thực sự M (xM, yM, zM) (điểm cuối E hay P là điểm cuối của tay máy, không phải 
điểm cuối thực sự của công cụ [4]) từ điều kiện ban đầu là Q(q1, q2, q3, q4, q5, q6) 
trong đó qi (i=1, 2, 3, 4, 5, 6) là giá trị các góc quay động cơ các trục 1,2,3,4,5,6. 
 Do đã có lời giải bài toán động học thuận với tay máy không có công cụ rất 
phổ biến nên từ điều kiện ban đầu Q tìm được ngay các giá trị vector ⃗, vector ⃗ 
và vị trí điểm P (xP, yP, zP) (Trong một số tài liệu có thể được gọi là điểm E viết tắt 
của End - Efector). 
 Tiếp theo đến phần động học thuận của công cụ gá lắp. Với tam giác PKM 
trong đó các khoảng cách PK, KM, PM và các góc đã biết ta hoàn toàn có thể tính 
được khoảng cách PN và MN với N là chân đường cao hạ từ đỉnh M xuống PK. 
 PN = PM ∗ cos() 	= 	l ∗ cos() (1) 
 MN=l*sin() (2) 
 Do ⃗ thẳng hướng vector ⃗ nên tính được luôn 
 ⃗ = PN *( ⃗ / |⃗ |) = l*cos() *( ⃗ / |⃗ |) (3) 
Vị trí P đã biết, nên ta xác định được ngay vị trí điểm N. 
P K 
M 
d6 
⃗ 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 27
Tương tự ta cũng xác định được ngay vị trí điểm K do PK= d71 đã biết và ⃗ 
thẳng hướng vector ⃗ . 
Hình 4. Tính động học thuận phần công cụ gá lắp. 
 Ngoài ra ⃗ là vector vuông góc với vector ⃗ và nằm trên mặt phẳng 
PKM nên đồng thời cũng vuông góc với vector ⃗ do đó: 
 ⃗ =MN*(⃗ /⃗ ) (4) 
trong đó, ⃗ =cross( ⃗ , ⃗) có thể lấy giá trị ngay trên ma trận Denavit - Hatenberg. 
 Khi đó: ⃗ = l*sin() *(⃗ /⃗ ) (5) 
 Do vị trí N đã xác định nên vị trí của M hoàn toàn xác định. Và chúng ta 
xác định được vector ⃗ . Như vậy bài toán thuận đã giải xong với việc xác định 
vị trí điểm M, vector ⃗ và vector ⃗. 
2.3.2. Động học ngược cho tay máy có công cụ 
Hình 5. Tính động học ngược phần công cụ gá lắp. 
 Với bài toán ngược ta sẽ cần tìm được Q(q1, q2, q3, q4, q5, q6) từ điều kiện 
ban đầu là vector ⃗, vector 2⃗ (vector hướng tiếp cận mới do phần công cụ tạo 
nên, 2⃗ 	 trùng hướng với ⃗ ), vị trí điểm cuối thực sự M (xM, yM, zM). Như vậy 
cần thiết chia công việc này thành 2 giai đoạn, giai đoạn 1 là tìm được vector ⃗ , 
vector ⃗ và vị trí điểm P. Giai đoạn 2 là từ P, vector ⃗ , vector ⃗ tìm Q(q1, q2, q3, 
q4, q5, q6) hay bài toán động học ngược phổ biến, đã có lời giải và đã thực hiện 
trong [1]. 
 Như vậy để giải bài toán động học ngược tổng quát cho tay máy có công cụ 
ta cần giải quyết giai đoạn 1. 
 Do ⃗ trùng hướng vector 2⃗ nên có thể xác định luôn: 
 ⃗ 	= KM*( 2⃗ /2⃗ ) (6) 
Khi đó tọa độ điểm K cũng xác định luôn do tọa độ M đã biết. 
P K 
M 
d6 
⃗ 
N 
P K 
d6 
⃗ 
M 
R 
⃗ 
⃗ 
Kỹ thuật điều khiển & Tự động hóa 
Đ. N. Kiên, P. M. Tân, V. Đ. Tuấn, “Động học tổng quát  đối với robot đa nhiệm.” 28 
 Với tam giác PKM đã xác định hoàn toàn (thông số của công cụ), ta còn 
biết thêm tọa độ điểm M nên hoàn toàn có thể vẽ thêm đường MR vuông góc với 
KM và cắt PK ở điểm R. 
 MR=KM*tan() = d72*tan( + ) (7) 
 Do MR vuông góc với KM và nằm trên mặt phẳng PKM nên MR cũng 
vuông góc với vector ⃗ . 
 Nếu 2⃗ = cross( ⃗ , ⃗); (8) 
 Thì ⃗ =MR*( 2⃗ /2⃗ ) (9) 
Với tọa độ điểm M đã xác định ta xác định được luôn tọa độ điểm R. 
Do K và R đã xác định nên vector ⃗ được xác định. 
Do vector ⃗ và vector ⃗ thẳng hướng với vector ⃗ nên xác định được luôn 
vector ⃗ , ngoài ra 
 ⃗ =PK*( ⃗ /|⃗ |)=d71*( ⃗ /|⃗ |) (10) 
 Khi đó xác định được vị trí điểm P do K đã được xác định. 
Với vector ⃗ không thay đổi nên bài toàn ngược giai đoạn 1 đã được giải hay bài 
toán ngược tổng quát của tay máy có công cụ đã giải xong. 
3. MÔ PHỎNG, TÍNH TOÁN, THẢO LUẬN 
3.1. Số liệu đầu vào 
 Để thực hiện mô phỏng và kiểm tra phương pháp trên ta sử dụng robot 6 
bậc tự do của hãng Yaskawa Motoman HP6 với thông số động học như sau [5]: 
Bảng 1. Thông số động học của tay máy Motoman HP6. 
 Ngoài ra công cụ gá lắp được thêm vào là một đầu máy cắt plasma với các 
thông số như sau như bảng 2. 
Bảng 2. Thông số công cụ gá lắp vào tay máy. 
 Quỹ đạo thực hiện công việc cắt là một đường tròn trong không gian với các 
thông số sau như bảng 3. 
Bảng 3. Thông số quỹ đạo của tác vụ. 
Trong đó: 
TT a(mm) α(độ) d(mm) 0(độ) 
1 150 90
o 450 90
o 
2 570 0 0 90
o 
3 155 90
o 0 0 
4 0 -90o 640 0 
5 0 90o 0 0 
6 0 0 95 0 
d71 (mm) d72(mm) l(mm) 
100 100
 180 
R(mm) Tâm ⃗ alpha(độ) 
50 [0, 790,880] (mm) [0, -1,-1] -30
o 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 29
 ⃗ là vector pháp tuyến của mặt phẳng chứa đường tròn; 
 alpha là góc giữa đường cao và đường sinh của hình nón lấy đường tròn 
quỹ đạo là đáy. 
3.2. Phương pháp, công cụ mô phỏng 
 Sử dụng phần mềm Matlab phiên bản R2016a để mô phỏng quỹ đạo của 
tay máy và công cụ trên quỹ đạo của tác vụ đã chọn. Với việc tính toán động học 
thuận và ngược tổng quát liên tiếp từ 100 điểm chia trên quỹ đạo tròn đã nêu trên 
ta thu được hình ảnh quỹ đạo trực quan trong không gian 3 chiều của tay máy và 
công cụ, cũng như các tư thế và kết quả thu được khi thực hiện tác vụ. 
 Để đảm bảo cho việc tiện quan sát kết quả thu được (là quỹ đạo chuyển 
động của công cụ) ta coi mỗi trục là một đường thẳng và các khớp là các điểm nối 
(Rất không tiện quan sát khi mô phỏng toàn bộ kết cấu bằng mô hình 3D cũng như 
thời gian mô phỏng tăng lên gấp nhiều lần). Công cụ do vậy cũng được quy về 
thành 2 đoạn thẳng với 1 điểm cuối sẽ được quan sát để tạo nên quỹ đạo của tác 
vụ. Sai số lặp đối với robot hàn phổ biến là 0.1mm để đạt được sai số này thì sai số 
tính toán phải cực nhỏ, dựa vào những nghiên cứu trước đây [1], sai số cho phép là 
khoảng 10-4mm. 
 Để đảm bảo tính trực quan khi thực nghiệm các hệ số tỷ lệ trên 3 trục cần 
đưa về bằng nhau. Ngoài ra có thể tăng giảm số điểm chia quỹ đạo để có thêm các 
góc nhìn thực tế hơn về kết quả thu được. 
3.3. Kết quả mô phỏng và bình luận 
Hình 6. Kết quả mô phỏng và tính toán trên phần mềm Matlab. 
Hình 7. Chi tiết kết quả mô phỏng trên phần mềm Matlab. 
Kỹ thuật điều khiển & Tự động hóa 
Đ. N. Kiên, P. M. Tân, V. Đ. Tuấn, “Động học tổng quát  đối với robot đa nhiệm.” 30 
 a) b) c) 
Hình 8. Chi tiết kết quả tính toán trên phần mềm Matlab. 
8-a: Tọa độ điểm ; 8-b: Vector ⃗ và vector ⃗ ; 8-c: Khoảng cách từ M 
đến tâm đường tròn. 
 a) b) c) 
Hình 9. Chi tiết kết quả tính toán trên phần mềm Matlab. 
9-a: ⃗= (0;1;0),alpha=-450; 9-b: ⃗= (0;0;-1),alpha=-150 ; 9-c: ⃗= (0;0;-1), 
alpha=-150, R=100mm. 
 a) b) c) 
Hình 10. Chi tiết kết quả tính toán trên phần mềm Matlab. 
10-a: thời gian tính toán động học 100 lần (ứng với 100 điểm); 10-b: thời gian 
tính toán động học 100.000 lần; 10-c: Khoảng cách từ M đến tâm đường tròn 
trường hợp 9-c. 
 Kết quả thu được trên phần mềm Matlab ngoài phần mô phỏng rất trực 
quan cho thấy quỹ đạo thu được đúng như yêu cầu đề ra còn được chi tiết bằng kết 
quả tính toán. Kiểm tra bằng tính toán chi tiết ta thấy sai số là không đáng kể, 
khoảng 10-11mm (khoảng cách từ các điểm đến tâm O thu được rơi vào khoảng 
50±10-11mm (hình 8-c) và 100±10-11mm (trường hợp 9-c)), gần như không giảm 
tốc độ tính toán (kiểm tra trên tổng số 100.000 lần tính động học thuận và nghịch 
chỉ mất dưới 8s, tăng lên khoảng 1s so với bài toán động học gốc) của bài toán 
động học tay máy ban đầu. 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 31
4. KẾT LUẬN 
 Đây là một kết quả rất hữu ích cho người làm robot với các tham số d71, d72 
và l thay đổi được ta có thể thử với các công cụ khác nhau hay là đảm bảo được 
tính đa nhiệm cho robot. Ngoài ra tốc độ tính toán không thay đổi đáng kể và tận 
dụng được các phương pháp tính toán động học có sẵn đối với tay máy không có 
công cụ nên có thể áp dụng ngay vào thực tế. Tuy nhiên thêm một lần nữa, nghiên 
cứu này dừng lại ở bài toán động học, với công cụ gắn vào tay máy làm việc trong 
công nghiệp cần xét đến bài toán động lực học vốn phức tạp hơn nhiều và cần 
nhiều công sức và các nghiên cứu tiếp theo để thực hiện. 
TÀI LIỆU THAM KHẢO 
[1]. Trần Trung Kiên, Đặng Nam Kiên, Vũ Đức Tuấn, Nguyễn Duy Trung “Giải 
bài toán động học cho robot 6 bậc tự do Hitachi M6100 bằng phương pháp 
phân đoạn,” TC. Nghiên cứu KHCNQS, số đặc san TĐH (2014), tr. 63-69. 
[2]. Nguyễn Văn Khang, Chu Anh Mỳ “ Cơ sở robot công nghiệp,” NXB Giáo dục 
2010. 
[3]. George Christopher Vosniakos, Zenon Kannas, “Motion coordination for 
industrial robotic systems with redundant degrees of freedom,” Robotics and 
Computer - Intergrated Manufacturing, 2008. 
[4]. Yushenko A.S, Zenkevich S.L, “Basic of manipulation robot,” MGTU 
Baumann, 2004. 
[5]. HP6 Manipulator Manual, 2007, https://www.motoman.com/ 
ABSTRACT 
GENERAL END EFFECTORS KINEMATIC OF 6-D 
OF MULTI-TASKING MANIPULATOR 
The tools attaching to the end effectors have different characteristics, it 
should be general kinematic calculation for different types of tools to shorten the 
installation and substitution time; especially multi-tasking robots. The article 
builds the calculation method of forward kinematics, inverse kinematics for the 
work of multi-tasking manipulator in industry after tools assembling. 
Keywords: Automation; Robotic; 6-dof manipulator. 
Nhận bài ngày 25 tháng 12 năm 2018 
Hoàn thiện ngày 28 tháng 02 năm 2019 
Chấp nhận đăng ngày 15 tháng 3 năm 2019 
Địa chỉ: Viện Tự động hóa KTQS / Viện Khoa học và Công nghệ quân sự. 
 * Email: dangnamkien@gmail.com.