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.