Một phương pháp xây dựng đường đặc tính vận tốc cho quỹ đạo chuyển động của tay máy 6 bậc tự do

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 17
MỘT PHƯƠNG PHÁP XÂY DỰNG ĐƯỜNG 
ĐẶC TÍNH VẬN TỐC CHO QUỸ ĐẠO CHUYỂN ĐỘNG 
CỦA TAY MÁY 6 BẬC TỰ DO 
Phạm Minh Tân*, Trần Trung Kiên, Đặng Nam Kiên, Vũ Đức Tuấn 
Tóm tắt: Bằng phương pháp xấp xỉ hàm sẽ xây dựng được 6 quỹ đạo chuyển 
động của 6 trục tay máy 6 bậc tự do. Từ đó điều khiển đồng bộ cho 6 trục tay máy 
bám theo 6 quỹ đạo đã được xây dựng, để đưa đầu công tác tay máy di chuyển bám 
theo quỹ đạo chuyển động mong muốn trong không gian. Vấn đề đặt ra là tại các 
điểm gián đoạn trên đường quỹ đạo, như điểm bắt đầu chạy hay điểm dừng chạy đầu 
công tác, vận tốc và gia tốc của đầu công tác tay máy cũng như của các trục tay máy 
có sự biến thiên đột ngột, gây dao động và làm đầu công tác di chuyển sai lệch so 
với quỹ đạo mong muốn. Bài báo giới thiệu một phương pháp xây dựng đường đặc 
tính vận tốc cho quỹ đạo chuyển động của tay máy 6 bậc tự do, nhằm đảm bảo các 
trục tay máy vẫn chuyển động đồng bộ trên quỹ đạo mong muốn trong quá trình 
tăng tốc độ hoặc giảm tốc độ. 
Từ khóa: Tay máy 6 bậc tự do; Quỹ đạo chuyển động; Đặc tính vận tốc chuyển động. 
 1. ĐẶT VẤN ĐỀ 
Xây dựng quỹ đạo chuyển động là bài toán quan trọng để có thể điều khiển đầu 
công tác tay máy 6 bậc tự do di chuyển theo những đường cong liên tục bất kỳ 
trong không gian: đường thẳng, đường tròn, ... Việc này có thể được thực hiện 
bằng phương pháp tính trực tiếp từ việc giải bài toán động học ngược trong quá 
trình chạy, hoặc nhận dạng bằng mạng nơ ron [7] để xây dựng quỹ đạo chuyển 
động cho các trục. Tuy nhiên hay dùng hơn cả là sử dụng các phương pháp xấp xỉ 
để xây dựng phương trình chuyển động cho các trục theo thời gian, bởi tính đơn 
giản và dễ dàng triển khai thực hiện trong thực tế. Theo [1] và [3] các bước xây 
dựng quỹ đạo chuyển động cho các trục tay máy như sau: 
- Xây dựng phương trình quỹ đạo chuyển động của đầu công tác tay máy trong 
không gian theo thời gian. 
- Rời rạc hóa quỹ đạo chuyển động thành nhiều điểm cách đều nhau, thu được 
quỹ đạo chuyển động rời rạc của đầu công tác. 
- Giải bài toán động học ngược cho từng điểm rời rạc ấy, chọn nghiệm theo 
phương án tối ưu khoảng cách dịch chuyển, sẽ thu được quỹ đạo chuyển động rời 
rạc cho mỗi trục. 
- Xấp xỉ hàm cho từng trục từ chuỗi những điểm rời rạc tương ứng, bằng 
phương pháp bình phương tối thiểu [1], chuỗi đường cong bậc 3 [3],  sẽ thu 
được quỹ đạo chuyển động liên tục cho mỗi trục theo thời gian, có dạng như sau: 
 θ1 = f1(t); θ2 = f2(t); θ3 = f3(t); θ4 = f4(t); θ5 = f5(t); θ6 = f6(t); (1) 
Trong đó: 
 + t: thời gian chuyển động quỹ đạo (0 ≤ t ≤ Tqd) 
+ θi: góc quay trục thứ i (i = 1 ÷ 6) 
 + fi(t): hàm xấp xỉ của trục thứ i (i = 1 ÷ 6) 
 + Tqd: tổng thời gian tính quỹ đạo. 
Tuy nhiên, với phương pháp xây dựng quỹ đạo như trên, quỹ đạo chuyển động 
của các trục tay máy được xây dựng trên cơ sở đầu công tác tay máy chuyển động 
Kỹ thuật điều khiển & Tự động hóa 
P. M. Tân, , V. Đ. Tuấn, “Một phương pháp xây dựng  của tay máy 6 bậc tự do.” 18 
đều với vận tốc không đổi từ điểm đầu đến điểm cuối của quỹ đạo. Chưa có sự 
kiểm soát vận tốc cũng như gia tốc tại các điểm gián đoạn, ví dụ như điểm bắt đầu 
chạy quỹ đạo, điểm kết thúc quỹ đạo, vận tốc và gia tốc chuyển động của các trục 
có sự biến thiên đột ngột, gây dao động và làm cho đầu công tác di chuyển sai lệch 
so với quỹ đạo mong muốn. Bài toán đặt ra là từ các phương trình quỹ đạo chuyển 
động theo thời gian đã được xây dựng, phải xây dựng được đường đặc tính vận tốc 
cho đầu công tác cũng như các trục tay máy, để hệ thống vận hành trơn tru, êm ái 
từ điểm bắt đầu đến điểm kết thúc quỹ đạo, nhưng vẫn đảm bảo các trục chuyển 
động đồng bộ với nhau trên đường quỹ đạo đã xây dựng. 
2. XÂY DỰNG ĐƯỜNG ĐẶC TÍNH VẬN TỐC CHO 
QUỸ ĐẠO CHUYỂN ĐỘNG CỦA ĐẦU CÔNG TÁC TAY MÁY 
 2.1. Đường đặc tính vận tốc mong muốn 
Để giải quyết vấn đề vận tốc và gia tốc bị gián đoạn tại điểm đầu và điểm cuối 
của quỹ đạo, hai phương pháp thường được sử dụng gồm: 
 Xây dựng thêm 1 đoạn nữa tại điểm đầu và cuối của quỹ đạo đã xây dựng 
[4]. Đoạn quỹ đạo kéo dài này là những đường cong bậc cao theo thời gian, 
thỏa mãn yêu cầu về vị trí, tốc độ, gia tốc tại hai điểm đầu mút. Giải pháp 
này thực hiện rất tốt khi cho phép kéo dài thêm quỹ đạo chuyển động. 
 Xây dựng quỹ đạo chuyển động cho đầu công tác và các trục tay máy theo 
thời gian nhưng có tính cả đến vận tốc và gia tốc trong quá trình chuyển 
động. Giải pháp này không cần kéo dài quỹ đạo nhưng rất phức tạp khi 
triển khai trong thực tế. 
Bài báo giới thiệu phương pháp xây dựng đường đặc tính vận tốc tựa theo 
đường đặc tính vận tốc mong muốn, trực tiếp trong quá trình chạy trong từng chu 
kỳ trích mẫu, từ các phương trình quỹ đạo chuyển động (1) đã được xây dựng trên 
cơ sở vận tốc chuyển động đều của đầu công tác. Việc xây dựng quỹ đạo chuyển 
động và đường đặc tính vận tốc trở lên đơn giản hơn rất nhiều do được tách thành 
2 bài toán độc lập. 
Trong khuôn khổ bài báo, nhóm tác giả lựa chọn xây dựng đường đặc tính vận 
tốc mong muốn của đầu công tác có dạng chữ S, để có thể kiểm soát được cả vận 
tốc, gia tốc và đạo hàm của gia tốc chuyển động, cụ thể như sau: 
Hình 1. Đường đặc tính vận tốc mong muốn của đầu công tác tay máy. 
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 19
Quá trình tăng tốc độ (xây dựng tương tự cho quá trình giảm tốc độ), trải qua 3 
giai đoạn như sau [6]: 
Bảng 1. Đường đặc tính tăng tốc độ quỹ đạo chuyển động mong muốn. 
Giai đoạn 1 
(0 ≤  ≤ ) 
Giai đoạn 2 
(0 ≤  ≤ ) 
Giai đoạn 3 
(0 ≤  ≤ ) 
() =  + 
/2 () =  +  () =  +  − 
/6 
() =  () =  () =  −  
() =  () = 0 () = − 
Trong đó: 
 (), (): Vận tốc và gia tốc đầu công tác tay máy. 
 (): Đạo hàm của gia tốc, đặc trưng cho độ giật của chuyển động. Giá trị 
 thường chọn nhỏ hơn giá trị độ giật tối đa cho phép đối với từng cấu 
trúc tay máy cụ thể. 
 1, 2, : Các khoảng thời gian chuyển động của 3 giai đoạn. 
 , , : Vận tốc đầu của các giai đoạn chuyển động. 
 : Gia tốc trong giai đoạn nhanh dần đều. 
2.2. Giải pháp thực hiện 
Trên cơ sở ứng dụng công nghệ điều khiển số hiện đại, bao gồm máy tính 
nhúng, FPGAđể thực hiện hệ thống điều khiển tay máy 6 bậc tự do, chu kỳ trích 
mẫu tính toán của hệ thống là rất nhỏ (dưới 1ms), nên trong mỗi chu kỳ trích mẫu, 
quãng đường đi được của đầu công tác là rất nhỏ, ta có thể xấp xỉ là 1 đoạn thẳng. 
Giả sử sau mỗi chu kỳ trích mẫu T, đầu công tác di chuyển được quãng đường 
S1, S2, S3, tương ứng với các điểm mốc thời gian t1, t2, t3, để tính quỹ đạo các 
trục từ các phương trình (1). Sơ đồ biểu diễn mối quan hệ giữa thời gian trích mẫu 
(thời gian thực) và thời gian tính quỹ đạo như sau: 
Hình 2. Thời gian tính quỹ đạo theo chu kỳ trích mẫu. 
Trong đó: 
 t: Trục thời gian thực tế. 
 T: Thời gian cho 1 chu kỳ trích mẫu. 
 tqd: Trục thời gian quy đổi để tính quỹ đạo từ phương trình chuyển 
động đã được xây dựng. 
Kỹ thuật điều khiển & Tự động hóa 
P. M. Tân, , V. Đ. Tuấn, “Một phương pháp xây dựng  của tay máy 6 bậc tự do.” 20 
 Tqd: Tổng thời gian tính quỹ đạo. 
 Tvd: Tổng thời gian vận động thực tế. 
 S1, S2, S3, , Sn: Quãng đường đi được của đầu công tác tay máy theo 
quỹ đạo chuyển động trong 1 chu kỳ trích mẫu. 
Tại chu kỳ trích mẫu thứ n, quãng đường đi được của đầu công tác: 
 Tính theo thời gian thực tế là: 
 Tính theo thời gian tính quỹ đạo: 
∆ =  ()

()
∆ = ∆

(2) 
(3) 
Trong đó: 
 : Vận tốc mong muốn của đầu công tác tại chu kỳ trích mẫu n. 
 : Vận tốc chuyển động đều của đầu công tác khi xây dựng quỹ đạo. 
 ∆

: Khoảng thời gian tăng thêm để tính quỹ đạo tại chu kỳ n. 
Do thời gian của 1 chu kỳ trích mẫu là rất nhỏ (dưới 1ms), nên có thể coi vận 
tốc của đầu công tác không đổi trong từng chu kỳ trích mẫu để tính quãng đường đi 
được cho mỗi chu kỳ. Công thức (2) trở thành: 
 ∆ =  (4) 
 Từ (3) và (4): ∆

=


 (5) 
Như vậy, để điều khiển đầu công tác của tay máy vừa di chuyển theo phương 
trình chuyển động đã được xây dựng, vừa đáp ứng đường cong vận tốc có dạng tại 
hình 2, thì giải pháp thực hiện tại từng chu kỳ trích mẫu của bộ điều khiển như sau: 
 Xác định vận tốc vận động yêu cầu của đầu công tác () theo các phương 
trình ở bảng 1. 
 Thay vào (5) tính được lượng thời gian tăng thêm để tính quỹ đạo ∆

. 
 Xác định thời gian tính quỹ đạo 

= 	 

+ ∆

 Thay vào (1) để tính vị trí đặt cho từng trục tay máy. 
3. MÔ PHỎNG 
3.1. Số liệu đầu vào 
- Mô phỏng hệ thống sử dụng Matlab & Simulink với bộ tham số thực tế của 
tay máy 6 bậc tự do Motoman HP6 như sau: 
Bảng 2. Bảng tham số động học của tay máy Motoman HP6. 
TT θ d(mm) a(mm) α(0) θ0(
0) 
1 q1 450 150 90
 90 
2 q2 0 570 0 90 
3 q3 0 155 90
 0 
4 q4 640 0 -90 0 
5 q5 0 0 90 0 
6 q6 95 0 0 0 
Trong đó: - qi là biến góc quay của trục thứ i (i = 1 ÷6). 
 - d, a, α, θ0 là các tham số động học của tay máy. 
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 21
- Mô phỏng quỹ đạo chuyển động của tay máy 6 bậc tự do Motoman HP6 là 
đường tròn trên mặt phẳng song song với mặt phẳng XOY, bán kính R = 200 mm, 
tọa độ tâm (0, 800,1080) mm trong hệ trục tọa độ gốc của tay máy. Tọa độ điểm 
đầu và cuối của quỹ đạo là (-200, 800, 1080) mm. 
- Tốc độ chuyển động đầu công tác là Vqd = 200 mm/s. Từ tổng chiều dài của 
quỹ đạo Sqd, sẽ xác định được tổng thời gian tính quỹ đạo Tqd = Sqd/Vqd. 
- Chia quỹ đạo trên thành 100 điểm cách đều nhau, giải bài toán động học 
ngược cho 100 điểm đó sẽ thu được tương ứng tập hợp 100 điểm quỹ đạo cho mỗi 
trục tay máy. Áp dụng phương pháp đường cong bậc 3 [1], mỗi trục tay máy sẽ 
được xấp xỉ thành 100 đường cong bậc 3 nối tiếp nhau. 
- Giải bài toán động học thuận từ các quỹ đạo của các trục tay máy đã được xấp 
xỉ, thu được quỹ đạo chuyển động của đầu công tác tay máy sau khi xấp xỉ hàm. 
Kết quả mô phỏng được thể hiện ở hình 3 và hình 4. 
- Đặt as = 75mm/s
2, jm = 50mm/s
3. Cập nhật thời gian tính quỹ đạo theo công 
thức (4) ở trong từng chu kỳ trích mẫu T = 0.001s cho mỗi trục. Giải bài toán động 
học thuận để xác định vị trí của đầu công tác tay máy. Vẽ quỹ đạo các trục và đầu 
công tác tay máy sau khi bổ sung đường đặc tính vận tốc. Kết quả mô phỏng thể 
hiện ở hình 5. 
- Tính vận tốc, gia tốc, đạo hàm gia tốc của các trục và đầu công tác tại từng 
chu kỳ trích mẫu. Kết quả mô phỏng thể hiện ở hình 6 ,7, 8. 
3.2. Kết quả mô phỏng xây dựng quỹ đạo các trục 
 Hình 3a. Quỹ đạo các trục. Hình 3b. Quỹ đạo đầu công tác. 
 Hình 4a. Vận tốc các trục. Hình 4b. Vận tốc đầu công tác. 
Kỹ thuật điều khiển & Tự động hóa 
P. M. Tân, , V. Đ. Tuấn, “Một phương pháp xây dựng  của tay máy 6 bậc tự do.” 22 
- Nhận xét: 
+ Quỹ đạo đầu công tác sau khi áp dụng phương pháp xấp xỉ hàm đường cong 
bậc 3 có sai số lớn nhất tại điểm đầu và cuối quỹ đạo khoảng 0.01mm. Sai số này 
càng nhỏ khi số điểm rời rạc càng lớn. 
+ Vận tốc đầu công tác trên toàn quỹ đạo là 200mm/s, thời gian di chuyển quỹ 
đạo từ 0s đến 6.283s. 
+ Vấn đề cần giải quyết đó là tại điểm đầu và điểm cuối quỹ đạo, vận tốc của 
các trục và của đầu công tác tay máy có sự biến thiên đột ngột dạng xung (hình 4a, 
hình 4b). Cần xây dựng lại đường đặc tính vận tốc chuyển động của các trục để 
đưa đầu công tác di chuyển mềm mại từ đầu đến cuối quỹ đạo. 
3.3. Kết quả mô phỏng sau khi bổ sung đường đặc tính vận tốc 
 Hình 5a. Quỹ đạo các trục. Hình 5b. Quỹ đạo đầu công tác. 
 Hình 6a. Vận tốc các trục Hình 6b. Vận tốc đầu công tác 
 Hình 7a. Gia tốc các trục. Hình 7b. Gia tốc đầu công tác. 
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 23
 Hình 8a. Đạo hàm gia tốc các trục. Hình 8b. Đạo hàm gia tốc đầu công tác. 
Nhận xét: 
+ Dạng đường cong vận tốc thu được đúng như mong muốn (hình 6b), vận tốc 
các trục tay máy là những hàm liên tục theo thời gian từ điểm đầu đến điểm cuối 
quỹ đạo chuyển động (hình 6a). 
+ Hình 5b thể hiện đồng thời cả quỹ đạo tay máy sau khi xấp xỉ hàm (đã thể 
hiện ở hình 3b) và quỹ đạo tay máy có bổ sung đường cong vận tốc trong từng chu 
kỳ trích mẫu. Hai quỹ đạo này trùng khít nhau, chứng minh tính đúng đắn của giải 
pháp. Đó là phân bố lại thời gian tính quỹ đạo để có được đường cong vận tốc 
mong muốn nhưng không làm thay đổi quỹ đạo chuyển động của đầu công tác 
cũng như các trục của tay máy. 
+ Hình 5a cho thấy hình dạng quỹ đạo chuyển động đầu công tác không thay đổi 
khi xây dựng đường cong vận tốc trên quỹ đạo đã xấp xỉ, nhưng thời gian vận động 
từ 0s đến 10.452s. Thời gian vận động kéo dài hơn do có bổ sung đường đặc tính 
vận tốc. Các giá trị lựa chọn (as và jm) càng nhỏ thì thời gian vận động càng dài. 
4. KẾT LUẬN 
Bài báo đã đưa ra một giải pháp để xây dựng đường đặc tính vận tốc mong 
muốn cho quỹ đạo chuyển động của tay máy 6 bậc tự do, từ các phương trình 
chuyển động theo thời gian của các trục đã được xây dựng. Bản chất của giải pháp 
là phân bố lại thời gian tính quỹ đạo chuyển động của các trục tay máy trong từng 
chu kỳ trích mẫu của bộ điều khiển, để đạt được đường đặc tính vận tốc mong 
muốn nhưng không làm thay đổi quỹ đạo chuyển động của tay máy. Giảm thiểu sự 
dao động, giật cục cơ khí của các trục tại các điểm gián đoạn như điểm đầu và 
điểm cuối của quỹ đạo chuyển động. 
Các nội dung trình bày trong bài báo cũng là thành quả của Đề tài cấp Viện 
Khoa học và Công nghệ Quân sự “Nghiên cứu, thiết kế, chế tạo hệ thống nhúng 
chuyên dụng điều khiển cơ cấu tay máy 6 trục tự do định hướng ứng dụng trong 
dây chuyền sản xuất”. Các nội dung nghiên cứu đã được cài đặt và chạy thử 
nghiệm trên tay máy Motoman HP6. 
TÀI LIỆU THAM KHẢO 
[1]. 
Trần Tân Tiến, Phạm Minh Tân, Trần Trung Kiên, Đặng Nam Kiên, 
Nguyễn Duy Trung, “Điều khiển bám quỹ đạo cho đầu công tác tay máy 6 
bậc tự do,” TC. Nghiên cứu KHCNQS, số 49 (06-2017), tr. 44-52. 
Kỹ thuật điều khiển & Tự động hóa 
P. M. Tân, , V. Đ. Tuấn, “Một phương pháp xây dựng  của tay máy 6 bậc tự do.” 24 
[2]. 
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 (04-2014), tr. 63-69. 
[3]. Báo cáo tổng kết đề tài, “Nghiên cứu, thiết kế, chế tạo hệ thống nhúng 
chuyên dụng điều khiển cơ cấu tay máy 6 trục tự do định hướng ứng dụng 
trong dây chuyền sản xuất”, Viện Tự động hóa KTQS - Viện Khoa học và 
Công nghệ Quân sự (2017). 
[4]. Hongjun ZHU, “Motion trajectory planning and simulation of 6-dof 
manipulator arm robot,” Academic journal of manufacturing engineering, 
VoL.15 (3/2017), pp.66-71. 
[5]. S.Yauldegar, Hootan Ghiasi, Mohammad Hadi Mazloom, Amirzubir 
Sahamijoo, Mohammad Reza Avazpour, Farzin Piltan, “Trajectory Tracking 
Control of Multi Degrees of Freedom Joints: Robust Fuzzy Logic-Based 
Sliding Mode Approach,” International Journal of Control and Automation, 
Vol. 7, No. 12 (2014), pp. 323-338 
[6]. Edward Red, “A dynamic optimal trajectory generator for Cartesian Path 
following”, BYU SholarAchive, Brigham Young University, (2000) 
[7]. Yavuz Sari, “Performance Evaluation of the Various Training Algorithms 
and Network Topologies in a Neural-network-based Inverse Kinematics 
Solution for Robots,” International Journal of Advanced Robotic Systems 
(2014). 
ABSTRACT 
A METHOD OF CREATING VELOCITY PROFILE FOR THE TRAJECTORY 
OF A 6-DOF MANIPULATOR 
 By the approximation method, 6 movement trajectories of 6 axes of a 6-dof 
manipulator will be built. From that, synchronous control for 6 axes of a 6-dof 
manipulator that follow 6 trajectories has been built, to bring the end effector to 
follow the desired trajectory in space. The problem is at the interrupt points in the 
trajectory, such as the starting point or the stopping point, the speed and the 
acceleration of the end effector, as well as the 6 axes, change suddenly as a pulse. 
This causes oscillating and the end effector will move in the wrong path from the 
desired trajectory. The paper introduces a method of creating velocity profile for the 
trajectory of a 6-dof manipulator, to ensure that 6 axes still move synchronously in 
the desired trajectories in the process of increasing or reducing speed. 
Keywords: 6-dof manipulator; Trajectory; Velocity profile. 
Nhận bài ngày 27 tháng 11 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 KH-CN Quân sự. 
 * Email: minhtanbka@gmail.com.