Bù chuyển động sóng biển cho robot sử dụng cảm biến đo lường quán tính

Nội dung text: Bù chuyển động sóng biển cho robot sử dụng cảm biến đo lường quán tính

1. Bù Chuyển Động Sóng Biển Cho Robot Sử Dụng Cảm Biến Đo Lường Quán Tính Wave Motion Compensation of a Serial Robot Using an Inertial Measurement Unit Lê Mạnh Tuấn Học viên cao học trường ĐHSPKT TPHCM TÓM TẮT Bài báo này mô tả việc phân tích động học và điều khiển bù chuyển động sóng biển cho robot dùng cảm biến đo lường quán tính. Phương pháp đề xuất này được sử dụng cho một cánh tay robot 4 bậc tự do, bù lại một chuyển động có biên độ lớn ở đế của robot sao cho điểm tác động cuối duy trì một vị trí mục tiêu quy định với độ chính xác cao. Toàn bộ hệ thống được thử nghiệm trên mô hình robot PHANTOMX REACTOR và máy tính được dùng để tạo ra các tín hiệu sóng biển thực với biên độ lớn. Từ khóa: Bù chuyển động sóng; cảm biến đo lường quán tính. ABSTRACT This paper describes the kinematical analysis and control of a large spatial robot for wave Motion compensation using an inertial measurement unit. The proposed method is intended for a 4 degrees of freedom serial manipulator, which compensates a large-amplitude spatial motion at its base such that the end effector maintains to a high degree a prescribed target location. The overall system is tested for a PHANTOMX REACTOR system and computer generated as well as real wave signals with large amplitudes. Keywords: Wave Motion Compensation; Inertial Measurement Unit. 1. GIỚI THIỆU robot là thuật toán bám quỹ đạo trực tuyến Ngày nay, Robot được sử dụng rộng rãi trong (OTG). Cảm biến được sử dụng cho việc xác định công nghiệp thường hoạt động trên bệ đỡ tĩnh. dao động tàu là cảm biến IMU 6 trục kết hợp sử Đối với các Robot được cài đặt trên một bệ đỡ dụng phương pháp tổng hợp dữ liệu đa cảm biến động là một thách thức và là một lĩnh vực nghiên dùng bộ lọc bù và bộ lọc Kalman. Kết quả thực cứu đang phát triển. Đối với hệ thống robot cẩu nghiệm cho thấy robot cẩu có thể gắp được hàng xa bờ là những hệ thống hoạt động trong điều hóa trong môi trường có dao động sóng biển. kiện môi trường khắc nghiệt, chịu tác động của Điều này cho thấy phương pháp được đề xuất là dao động sóng biển [1], [2]. Như vậy, bài toán đặt đáng tin cậy. ra là điều khiển hệ thống hoạt động ổn định, an toàn, hiệu quả trong điều kiện khắc nghiệt trên 2. MÔ HÌNH ĐỘNG HỌC biển. Bài báo này trình bày các hệ bù dao động Robot được sử dụng là PhantomX Reactor 4 cho robot cẩu trong môi trường có dao động như: bậc tự do như hình 1. Mô hình động học thuận hệ thống bù thụ động và hệ thống bù chủ động được xây dựng như hình 2 với bộ thông số [3],[4]. Phương pháp hình học được sử dụng để Denavit-Hartenberg của Robot xác định ma trận giải động học nghịch cho robot. Thuật toán xây chuyền như bảng 1. dựng bộ điều khiển bù dao động sóng biển cho
2. hình học. Xét mặt phẳng thẳng đứng của cánh tay khi xoay khớp θ1. Hình 1. Robot PhantomX Khi áp dụng phương pháp Denavit-Hartenberg gắn các hệ trục tọa độ với các khâu. Hình 3. Xác định góc θ1 Dựa vào hình chiếu của điểm tác động cuối lên mặt phẳng oxy ta thu được vị trí góc khớp thứ nhất: 1 arctan 2(yP , xP ) (1) Sau khi tìm được vị trí góc khớp thứ nhất, ta đưa cánh tay Robot về mặt phẳng không gian owz. Hình 2. Xác định tọa độ lên các khâu Bảng tham số Denavit-Hartenberg: Bảng 1.Bảng thông số DH của Robot xác định ma trận chuyền . Khâu θi αi ai di Tầm hoạt động o 1 θ1 0 0 d1 -150 đến 150o o 2 θ2 90 0 0 0 đến 90o Hình 4. Mô hình cánh tay trong không gian 3 θ 0 a 0 -90o đến 3 3 OWZ 90o o 4 θ4- 0 a4 0 -90 đến 90 90o o 5 θ5 -90 0 d5 -150 đến 150o Với d1=11, a3=15, a4=14.5, d5=13.7 (cm) Phương pháp giải động học nghịch theo phương pháp hình học cho robot 4 bậc tự do cho phép phân tích riêng biệt các phương trình vị trí và một hướng, thủ tục đơn giản và hiệu quả tính toán. Giải động học nghịch theo phương pháp Hình 5. Thiết lập hướng cổ tay cho Robot
3. Đơn giản cho việc xét khớp cổ tay, ta thiết lường trước được và không thể đoán trước bất cứ lập giá trị cố định cho khớp cổ tay AG lúc nào và trong bất kỳ trạng thái chuyển động (GripperAngle) trước tùy vào ứng dụng. Từ giá nào. Việc tích hợp cảm biến trong robot, đặc biệt trị biến khớp AG, ta sẽ thu được hệ tọa độ khâu là sự phát triển của các hệ thống điều khiển thứ 2 (w2,z2), sau đó ta sẽ tính giá trị cho các chuyển động được hướng dẫn bởi cảm biến trở khớp còn lại. nên rất đơn giản. w2 wT L3cos(AG) (2) Để đảm bảo hoạt động Robot an toàn trong z2 zT L3sin(AG) (3) điều khiển chuyển động cảm biến dẫn đường và ngăn không cho nó chạy vào điểm dừng khẩn cấp, chuyển động Robot nên bị hạn chế về mặt vị trí khớp, vận tốc khớp và gia tốc khớp. Chiến lược đề xuất ở đây bao gồm trong tiền xử lý chuyển động mục tiêu tính toán trước khi thực sự gửi nó cho các bộ điều khiển. Chúng tôi giới thiệu một loại thuật toán tạo quỹ đạo trực tuyến, còn chi tiết các vấn đề cơ bản toán học của phương pháp mới này trong [15]. Các thuật toán trình bày ở đây bao gồm ba bước: tính thời gian đồng bộ hóa tối thiểu cho tất cả DOFs, đồng bộ cho tất cả các DOFs, và tính toán giá trị đầu ra. Lý thuyết tiếp theo là kết Hình 6. Mô hình sau khi loại bỏ khớp cổ tay quả thử nghiệm thực tế cho thấy khả năng mới trong điều khiển robot chuyển động. Từ đó ta tính được wT: Gọi véc tơ qcomp là các biến khớp tương ứng wT sqrt(sq(xT ) sq(yT )) (4) theo lời giải từ tọa độ cảm biến IMU thu được, véc tơ qtar là các chuyển động khớp thực được gửi L12 w22 z22 (5) đến bộ điều khiển Robot. Thuật toán ánh xạ qcomp A12 arctan 2(z2, w2) vào qtar được xem như là bộ giới hạn (Thuật toán (6) Limiter). Các ràng buộc cho chuyển động mục L12 L122 L22 tar  2 arccos tiêu q là: 2* L1* L12 (7) Với A1 ta tính được w1, z1 và các góc còn lại: Với qmin và qmax là giới hạn không gian làm việc, q và q là giới hạn vận tốc và gia tốc w1 L1*cos(A1) max max (8) lớn nhất tương ứng. Các thông số giới hạn này do z1 L1*sin(A1) 9) nhà sản suất cung cấp trong bảng dữ liệu về 3 arctan 2(z2 z1, w2 w1) A1 (10) Robot.  A3 AG A1 A2 (11) 4 Luật ánh xạ của bộ gới hạn cho mỗi khoảng lấy mẫu là: 3. THUẬT TOÁN ĐIỀU KHIỂN tar comp Phần này giới thiệu một phương pháp mới qk k qk (12) [14] cho việc tạo ra quỹ đạo chuyển động của các Với hệ thống cơ khí với nhiều bậc tự do (DOFs). Các tính năng chính của khái niệm mới này là các quỹ k diag 1,k ,2,k ,3,k ,4,k  (13) đạo chuyển động được tạo trực tuyến, tức là trong Được tính toán theo thuật toán sau: mỗi chu kỳ điều khiển, thông thường mỗi phần Bước 1: tính độ thay đổi ở mỗi biến khớp nghìn giây. Điều này cho phép hệ thống phản ứng comp tar ngay lập tức với (cảm biến) các sự kiện không qk qk qk 1 (14)
4. Bước 2: tính q khả thi do giới hạn vận tốc min min min k q jk q jk q jk  min max , , khớp j,k  q j,k q j,k q j,k  (18) min,vel  max max max qk qmax t max q jk q jk q jk  (15)  j,k min , ,  max,vel  qk qmax t q j,k q j,k q j,k  Với t là khoảng lấy mẫu đọc từ cảm biến IMU Bước 7: Ta có thể chọn  j,k trong khoảng giới Bước 3: tính khả thi do giới hạn gia tốc hạn ở bước 6 cho đường chéo của  k khớp Bước 8: tính các giá trị mục tiêu qtar min,acc qk [ qmax t qk 1] t tar tar qk qk 1  k qk max,acc (16) qk [qmax t qk 1] t tar qk Bước 4: tính q khả thi do giới hạn không qk  k j,k t (19) gian làm việc biến khớp, với j=1, 2, 3, 4. tar 1 qk tar qk  k qk max min t t 2q (q q ) t qmin,pos j j,k 1 j j,k min q j,k 1 q j 4. TỔNG HỢP DỮ LIỆU CẢM BIẾN (17) qmax q Tính toán góc roll và pitch từ cảm biến gia max,pos j j,k 1 q j,k tốc: 2qmax (qmax q ) t j j j,k 1 acc a tan2(ax ,az ) (20) acc a tan2(ay ,az ) (21) Tính toán góc roll và pitch từ cảm biến Gyroscope: gyro  prev gyro t (22)  gyro  prev gyro t (23) Với gyro là ngõ ra vận tốc góc từ cảm biến Gyro. Hình 7. Sơ đồ mô tả định tính cho Sai số hiệp phương sai cho dự đoán ước lượng trong bước cập nhật thời gian công thức (15) và (17) P AP AT Q (24) Bước 5: tính khoảng khả thi k k 1 Trên là giá trị ước lượng tiền nghiệm ở thời min, max điểm k so với thời điểm trước đó k-1. P là dự qk qk qk đoán không chắc chắn, A mô tả không gian trạng Từ các giới hạn đã tính ở các bước trên thái của mô hình. Ma trận không gian trạng thái min,vel max,vel min,acc max,acc qk và qk , qk và qk , A mô tả cho cả hai cảm biến gia tốc và gyro. Với qmin,pos và qmax, pos P00, P01, P10, P11 mô tả sai số hiệp phương sai cho j,k j,k cảm biến gia tốc và gyro, khởi tạo là hằng số khác Bước 6: tính khoảng khả thi không (nó chỉ ảnh hưởng trong khoảng thời gian ngắn của việc dự đoán của bộ lọc). Q là tham số min max Với  j,k  j,k  j,k nhiễu uáq trình và khác không. Mỗi hằng số được khởi tạo là 0.1.
5. Theo thứ tự quá trình bộ lọc Kalman thực tốt tín hiệu cảm biến gia tốc trong trạng thái ổn hiện từ trái qua phải, ta có: định và bám cảm biến Gyro trong trạng thái rung. Nhưng đoạn cuối của đồ thị cho thấy sai biệt giữa    t k(gyro) k 1(gyro) gyro (25) góc ước lượng dùng bộ lọc bù và cảm biến Gyro Với k ( gyro ) là góc được tính toán từ cảm là tương đối lớn. biến gyro. Kết quả thử nghiệm với bộ lọc bù bậc 2 như Độ lợi Kalman Kk giữa trạng thái hiện tại và hình 9. Ước lượng góc dùng bộ lọc bù bậc 2 cho trạng thái được dự đoán là: kết quả tốt hơn bậc 1 khi khắc phục được việc bám dữ liệu góc cảm biến Gyro trong trạng thái Kk,00 P00 /(P00 R) (26) rung tốt hơn. Cập nhật ước lượng góc tại thời điểm k với độ lợi Kalman Kk là k k 1 Kk,00(acc k 1 ) (27) Với k là ngõ ra ước lượng góc của bộ lọc Kalman. Cuối cùng, cập nhật sai số hiệp phương sai trong bước cập nhật đo lường P00 (1 Kk,00)P00 (28) Các bước tính toán độ lợi Kalman Kk và sai Hình 9. Ước lượng góc dùng bộ lọc bù bậc 2 số hiệp phương sai cho ba thành phần còn lại Hình 10 biểu diễn kết quả ước lượng góc được tính toán tương tự. dùng: chỉ với cảm biến gia tốc, chỉ với cảm biến gyro và với bộ lọc Kalman trong trường hợp vừa 5. KẾT QUẢ xoay vừa rung. Cảm biến gia tốc bị rung và nhiễu Tổng hợp cảm biến dùng bộ lọc bù bậc 1 cho mạnh nên không thể cung cấp thông tin đinh kết quả như hình 8. Với ước lượng góc dùng chỉ hướng góc tốt cho hệ thống. Với cảm biến gyro, cảm biến gia tốc (đồ thị màu đỏ-Acc) cho kết quả thông tin định hướng tốt hơn nhưng tạo ra offset, tốt trong trạng thái ổn định nhưng bị nhiễu mạnh trong trường hợp này thì cảm biến gia tốc đáng khi cảm biến trong trạng thái rung. Với cảm biến tin cậy hơn. Với bộ lọc Kalman thì thông tin ước Gyro (đồ thị màu xanh lá-Gyro) lại ngược lại, cho lượng tốt hơn bằng cách kết hợp bám dữ liệu cảm kết quả ước lượng góc tốt trong trạng thái rung biến gyro trong trạng thái động và bám dữ liệu nhưng lại bị trôi theo thời gian do việc tính góc cảm biến gia tốc trong trạng thái tĩnh. dùng tích phân. Hình 8. Ước lượng góc dùng bộ lọc bù bậc 1 Hình 10. Ước lượng góc trong trạng thái rung Bộ lọc bù bậc 1 (đồ thị màu đen-Comp1) cho kết Hình 11 cho kết quả so sánh ước lượng góc quả tổng hợp tốt hơn 2 cảm biến riêng lẻ khi bám dùng bộ lọc bù bậc 1, bộ lọc bù bậc 2 và bộ lọc
6. Kalman. Do cả bộ lọc bù và bộ lọc Kalman điều crane”. In Proceedings of 11th International có thể bám dữ liệu góc của cảm biến gia tốc tốt Conference on Control, Automation and trong trạng thái tĩnh, nên ta chỉ đánh giá trong Systems (ICCAS), Gyeonggi-do, Korea, trạng thái rung và so sánh kết quả với dữ liệu góc Oct. 26–29, pp. 420–423 của cảm biến Gyro. Bộ lọc Kalman bám dữ liệu [6] Fang Y, Wang P, Sun N and Zhang Y cảm biến Gyro tốt nhất, nhưng sai biệt giữa bộ lọc (2014),” Dynamics analysis and nonlinear bù bậc 2 so với Kalman là khá nhỏ. Vậy với control of an offshore boom crane”. IEEE những ứng dụng không đòi ỏh i độ chính cao thì Transactions on Industrial Electronics ta vẫn có thể dùng phương pháp tổng hợp dữ liệu 61(1):414–427. cảm biến dùng bộ lọc bù với ưu điểm là thời gian tính toán rất nhanh. [7] Yingguang Chu, Filippo Sanfilippo, Vilmar Æsøy and Houxiang Zhang (2014) “An Effective Heave Compensation and Anti- sway Control Approach for Offshore Hydraulic Crane Operations” 2014 IEEE International Conference on Mechatronics and Automation. [8] V. Leontjevs, F. G. Flores, J. López, L. Ribickis, A. Kecskeméthy “Large-amplitude Hình 11. Ước lượng góc bộ lọc bù và Kalman base-motion compensation of a serial robot using an inertial measurement unit”, in Proc. TÀI LIỆU THAM KHẢO of the IEEE 17th International Conference Methods and Models in Automation and [1] Nguyễn Thái Dương. Điều khiển ổn định Robotics (MMAR), 2012, pp 166 – 171. robot gắp sản phẩm trong môi trường có dao động. ĐH Sư Phạm Kỹ Thuật TP.HCM, [9] T. Kroger and F. Wahl, “Online trajectory 2016. generation: Basic conc epts for instantaneous reactions to unforseen events,” IEEE [2] ATIP Document ID: 010814AR Transactions on Robotics, vol. 26, no. 1, pp. ATIP01.036, Automation at the Port of 94–111, 2010. Singapore, 2001. [3] Kimiaghalam B, Homaifar A and Sayarrodsari B (2001),” An application of model predictive control for a shipboard crane”. In Proceedings of the American Control Conference, Arlington, VA, June Thông tin liên hệ tác giả chính (người chịu 25–27, vol. 2, pp. 929–934. trách nhiệm bài viết): [4] Schaub H (2008) “Rate-based ship-mounted Họ tên: Lê Mạnh Tuấn crane payload pendulation control system”. Control Engineering Practice 16(1): 132– Đơn vị: Học viên cao học trường ĐHSPKT 145. TPHCM [5] Kim D, Park Y, Park YS, Kwon S and Kim Điện thoại: 01682180652 E (2011),” Dual stage trolley control system Email: leemanhtuan@yahoo.com for anti-swing control of mobile harbor
7. BÀI BÁO KHOA HỌC THỰC HIỆN CÔNG BỐ THEO QUY CHẾ ĐÀO TẠO THẠC SỸ Bài báo khoa học của học viên có xác nhận và đề xuất cho đăng của Giảng viên hướng dẫn B n ti ng Vi t ©, T NG I H C S PH M K THU T TP. H CHÍ MINH và TÁC GI Bản quếy n táệc ph mRƯ ãỜ cĐ bẠ o hỌ b Ưi Lu tẠ xu t Ỹb n vàẬ Lu t S hỒ u trí tu Vi t Nam. NgẢhiêm c m m i hình th c xu t b n, sao ch p, phát tán n i dung khi c a có s ng ý c a tác gi và ả ng ề i h ẩ pđh đưm ợK thuả tộ TP.ở H ậChí Mấinh.ả ậ ở ữ ệ ệ ấ ọ ứ ấ ả ụ ộ hư ự đồ ủ ả Trườ Đạ ọCcÓ Sư BÀI BạÁO KHỹ OA ậH C T ồT, C N CHUNG TAY B O V TÁC QUY N! ĐỂ Ọ Ố Ầ Ả Ệ Ề Th c hi n theo MTCL & KHTHMTCL h c 2017-2018 c a T vi n ng i h c S ph m K thu t Tp. H Chí Minh. ự ệ Năm ọ ủ hư ệ Trườ Đạ ọ ư ạ ỹ ậ ồ