Sử dụng bộ quan sát lực trong điều khiển lực tương tác giữa robot và môi trường

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI o0o -HOÀNG SƠN N SỬ DỤNG BỘ TƯƠNG TÁC LỰC TRONG ĐIỀU KHIỂN LỰC TƯƠNG TÁC GIỮA ROBOT VÀ MÔI TRƯỜNG LUẬN VĂN THẠC SỸ KHOA HỌC Người hướng dẫn khoa học : TS NGUYỄN THỤC ANH HÀ NỘI - 2010 LỜI CẢM ƠN Qua thời gian nghiên cứu, đến luận văn tốt nghiệp tác giả hoàn thành Đạt thành vậy, trước tiên tác giả xin bày tỏ lòng biết ơn chân thành đến cô giáo hướng dẫn, TS Nguyễn Phạm Thục Anh tận tình bảo, cung cấp kiến thức tài liệu giúp đỡ tác giả suốt trình thực Tác giả xin chân thành cảm ơn Thầy, Các cô khoa Điện, môn Tự động hóa Xí nghiệp Công nghiệp Viện Đào tạo sau đại học truyền cho kiến thức quí báu trình theo học trường Do thời gian thực có hạn kiến thức thân nhiều hạn chế, nên luận văn tránh khỏi thiếu sót Rất mong nhận bảo, góp y phê bình Thầy, Cô bạn bè LỜI CAM ĐOAN Tôi xin cam đoan luận văn công trình nghiên cứu riêng Các số liệu, kết nêu luận văn trung thực chưa công bố công trình khác Tác giả luận văn: Hoàng Sơn MỤC LỤC DANH MỤC CÁC KÝ HIỆU, CHỮ VIẾT TẮT……………………………… DANH MỤC CÁC BẢNG DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ MỞ ĐẦU 10 CHƯƠNG 1: TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP .12 1.1 Giới thiệu Robot công nghiệp 12 1.1.1 Lịch sử phát triển robot công nghiệp .12 1.1.2 Một số định nghĩa robot 15 1.1.3 Các đặc tính robot công nghiệp 16 1.2 Cấu tạo robot công nghiệp .18 1.2.1 Hệ thống chuyển động 19 1.2.2 Hệ thống truyền động robot 25 1.2.3 Hệ thống cảm biến 26 1.2.4 Bộ điều khiển 26 1.3 Các ứng dụng robot công nghiệp .28 CHƯƠNG 2: NGHIÊN CỨU CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN LỰC ROBOT CÔNG NGHIỆP 31 2.1 Mô hình động lực học robot 32 2.1.1 Mô hình không gian khớp .32 2.1.2 Mô hình không gian làm việc xét đến lực tác dụng lên môi trường .33 2.2 Điều khiển cứng (Stiffness Control) 34 2.2.1 Luật điều khiển 34 2.2.2 Tóm tắt nguyên lý điều khiển 37 2.2.3 Nhận xét 37 2.3 Điều khiển lai vị trí- lực 38 2.3.1 Phương pháp tuyến tính hóa xác: 38 2.3.2 Điều khiển lai vị trí – lực sở mạch phản hồi kinh điển .41 2.3.3 Nhận xét 43 2.4 Điều khiển lai trở kháng .43 2.4.1 Thiết kế điều khiển 43 2.4.2 Tóm tắt nguyên lý điều khiển sơ đồ điều khiển 45 2.4.3 Nhận xét 46 2.5 Nhận xét chung phương pháp điều khiển lực/ vị trí 46 CHƯƠNG 3: NGHIÊN CỨU, THIẾT KẾ BỘ QUAN SÁT LỰC ĐỂ ĐIỀU KHIỂN LỰC TƯƠNG TÁC GIỮA ROBOT VÀ MÔI TRƯỜNG 48 3.1 Ươc lượng lực sở quan sát 48 3.2 Xây dựng quan sát lực .50 3.2.1 Quan sát lực 50 3.2.2 Động lực học khâu quan sát 51 3.3 Xây dựng quan sát lực cho robot 52 3.4 Xây dựng quan sát lực cho robot Planar 54 3.4.1 Khái quát Robot Planar bậc tự 54 3.4.2 Bài toán động học thuận 54 3.4.3 Xác định ma trận Jacoby robot Planar 57 3.4.4 Động lực học robot Planar 58 3.4.5 Quan sát lực cho robot Planar 61 3.5 Xây dựng điều khiển lai vị trí - lực cho robot Planar sử dụng quan sát lực: ………………………………………………………………………………62 CHƯƠNG 4: KẾT QUẢ MÔ PHỎNG VÀ ĐÁNH GIÁ KẾT QUẢ 64 4.1 Xây dựng mô hình mô cho khâu quan sát 64 4.1.1 Kết mô quan sát lực : 66 4.2 Điều khiển lai vị trí/ lực cho Robot Planar hai bậc tự .69 4.2.1 Bài toán đặt .69 4.2.2 Xây dựng mô hình với điều khiển lai vị trí - lực : .70 4.3 Đánh giá kết mô ……………………….…………………… 77 KẾT LUẬN…………………………………………………………………… 78 TÀI LIỆU THAM KHẢO……………………………………………………… 79 PHỤ LỤC………………………………………………………………………….81 DANH MỤC CÁC KÝ HIỆU, CHỮ VIẾT TẮT M Ma trận mô men H(q) Ma trận chứa thành phần quán tính V ( q , q ) = C ( q, q ) q G(q) f ^ F sθ1 sθ12 = s(θ1 + θ ) cθ1 cθ12 = c(θ1 + θ ) Ma trận chứa thành mô men nhớt hướng tâm Ma trận chứa thành phần trọng trường Ngoại lực tác động Lực quan sát sin θ1 sin(θ1 + θ ) cosθ1 cos(θ1 + θ ) DANH MỤC CÁC BẢNG Bảng 3.1: Bảng thông số D-H cho robot Planar DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ Hình 1.1: Hình dạng robot công nghiệp Hình 1.2: Hệ thống chuyển động robot Hình 1.3: Khớp tịnh tiến Hình 1.4: Các loại khớp quay Hình 1.5: Cơ cấu kẹp robot Hình 1.6: Cấu hình robot dạng Decac Hình 1.7 : Cấu hình robot dạng Decac Hình 1.8 : Cấu hình robot dạng hình trụ Hình 1.9 : Cấu hình robot dạng hình cầu Hình 1.10: Cấu hình robot dạng khớp nối Hình 1.11: Cấu hình Scara Hình 1.12: Sơ đồ khối robot công nghiệp Hình 1.13: Robot phun sơn Hình 1.14: Robot hàn Hình 1.15: Robot lắp ráp Hình 2.1: Các phương pháp điều khiển Robot Hình 2.2: Hệ thống bậc tự Hình 2.3: Sơ đồ điều khiển lai vị trí- lực theo phương pháp tuyến tính hóa xác Hình 2.3: Điều khiển lai vi trí- lực sở mạch phản hồi kinh điển Hình 2.4: Sơ đồ hệ điều khiển lai trở kháng Hình 3.1: Sơ đồ khôi quan sát Hình 3.2: Mô hình robot Planar hai bậc tự Hình 3.3: Chọn hệ tọa độ gắn với Robot Hình 3.4: Bộ điều khiển vị trí- lực cho robot Planar Hình 4.1: Sơ đồ khối quan sát lực Hình 4.2: Mô hình khối quan sát lực robot planar xây matlab/ Simulink Hình 4.3: Mô hình quan sát lực xây dựng matlab/ Simulink Hình 4.4: Góc quay khớp Hình 4.5: Kết mô với K1= K2= Hình 4.6: Kết mô với K1= 20 K2= 100 Hình 4.7: Kết mô với K1= 40 K2 = 400 Hình 4.8: Kết mô với ngoại lực biến thiên theo kiểu xung Hình 4.9: Kết mô với ngoại lực biến thiên dạng hình sin Hình 4.10: Kết mô với ngoại lực dạng bước nhảy Hình 4.11: Yêu cầu chuyển động robot Hình 4.12: Sơ đồ điều khiển lai vị trí- lực robot Planar Hình 4.13 Mô hình điều lai vị trí- lực xây dựng matlab/simulink Hình 4.14: Đáp ứng vị trí với KP= 30 KD= Hình 4.15: Đáp ứng vị trí với KP= 30 KD= 20 Hình 4.16: Đáp ứng vị trí với KP= 300 KD= 20 Hình 4.17: Đáp ứng vị trí với KP= 3000 KD= 200 Hình 4.18: Sai lệch vị trí theo phương X Hình 4.19: Đáp ứng lực với Kf = 100 Hình 4.20: Đáp ứng lực với Kf = 61 Các thông số đưa vào mô hình mô phỏng: Khối lượng nối: m1= 3,5 (kg); m2 = (kg) Khối lượng tải: mt = 0,5 (kg ) Chiều dài: l1 =0,25 (m); l2 = 0,25(m) Robot di chuyển dọc theo trục x với tọa độ : Xd= 0,3+0,25sint (m); Y = (m) Và robot sinh lực theo trục y với : Fd = (N) Vậy theo mục 3.5- chương ta có điều khiển lai vị trí- lực sau ^ M dk = o Rc J T [ K f S c Ro ( Fd − F ) + K p ( I − S ).c Ro ( X d − X ) + K d c Ro ( I − S ) X ] Với o Rc = c Ro = I ⎡0 ⎤ ⎡1 0⎤ Và S = ⎢ ⎥ I − S = ⎢0 0⎥ ⎣0 ⎦ ⎣ ⎦ Sơ đồ điều khiển lai vị trí- lực Hình 4.12: Sơ đồ điều khiển lai vị trí- lực robot Planar 4.2.2 Xây dựng sơ đồ mô điều khiển lai vị trí - lực : 70 Vì cấu tạo điều khiển chia không gian điều khiển cánh tay Robot thành hai phần hai phần hoàn toàn độc lập với nên việc thiết kế lựa chọn thông số cho điều khiển hoàn toàn độc lập Vì thế, chọn thông số cho điều khiển mà không làm ảnh hưởng tới kết điều khiển Trước hết tác giả tính chọn tham số cho điều khiển vị trí 4.2.2.1 Kết mô điều khiển vị trí : Ban đầu ta chon hai thông số KP, KD nhỏ KP= 30 KD= 2, kết mô sau : 71 Có thể thấy với quỹ đạo vị trí đặt quỹ đạo hình sin, tay Robot chưa bám sát, có lượng sai lệch lớn dao động Bởi thế, ta phải tăng dần thông số KP KD lên Giữ nguyên Ktp=30, tăng Ktd=20, ta có kết sau: Hình 4.15: Đáp ứng vị trí với KP= 30 KD= 20 Ta nhận thấy dạng quỹ đạo tay robot giống với dạng quỹ đạo yêu cầu, vấn đề cần phải xử lý phải bảo đảm sai lệch nhỏ Muốn vậy, ta tăng số Kp Với Kp=300, đáp ứng hình dưới: 72 Tuy nhiên, để bảo đảm quỹ đạo thực quỹ đạo đặt bám sát nữa, ta phải tăng KD lên cao, mặt khác KD KP hai tham số độc lập việc tăng KD mà không tác động vào KP lại làm sai lệch cho hệ thống Bởi vậy, việc điều chỉnh phải tiến hành đồng thời Sau trình lựa chọn tham số, mô điều chỉnh thấy với KP = 3000, KD = 200 Kết mô tốt: 4.17: Đáp vị trí vớiđạo KP= = 200 Có thể thấy quỹ Hình đạo bám rấtứng sát với quỹ đặt3000 với thờiKDgian đáp ứng nhanh, cỡ khoảng 1s Đây kết chấp nhận với sai lệch quỹ đạo biểu diễn (hình 4.18 ) 73 4.2.2.2 Kết mô điều khiển lực Với quan sát mô chọn K1 = 40 K2 = 400, độ điều chỉnh khâu quan sát nhỏ 20%, thời gian độ khâu quan sát nhỏ 0,5 s sai lệch khâu quan sát nhỏ 3% Thành phần điều khiển quỹ đạo chọn với Kp= 3000 KD= 200 Trong điều khiển robot tách thành hai thành phần độc lập thành phần điều khiển vị trí thành phần điều khiển lực Do tiến hành chọn tham số cho thành phần điều khiển lực Thành phần điều khiển lực thiết kế thông qua việc chọn hệ số Kf Trước hết, chọn hệ số Kf = 100 ta có đáp ứng quĩ đạo lực sau: Với Kf = 100 ta có kết mô sau 74 Với đáp ứng lực đầu điểm tác động cuối robot tác động lên môi trường lớn 4N phải giảm Kf xuống Sau trình thử nghiệm tác giả chọn Kf = 61 kết mô sau Hình 4.20: Đáp ứng lực theo phương Y với Kf = 61 Với thời gian độ khoảng 2s độ điều chỉnh vào khoảng 20%, kết chấp nhận 4.2.2.3.Kết luận 75 Với thông số tổng hợp trên, điều khiển lai vị trí- lực có hệ số sạu - Hệ số quan sát : K1 = 40 K2 = 400 - Hệ số điều khiển vị trí : KP = 3000 KD = 200 - Hệ số điều khiển lực : Kf = 61 Kết mô vị trí lực có đáp ứng sau Hình 4.21: : Đáp ứng vị trí với KP= 3000 KD= 200 4.3 Hình 4.22: Đáp ứng lực theo phương Y với Kf = 61 Đánh giá kết mô : Thông qua kết mô thu được, ta nhận thấy kết mô sử dụng phương pháp quan sát lực để ước lượng lực tương tác robot 76 môi trường hoàn toàn đáp ứng tiêu điều khiển Điều chứng tỏ xem xét tới việc áp dụng quan sát lực để điều khiển robot công nghiệp cụ thể thực tế Khi sử dụng phương pháp điều khiển đồng thời vị trí- lực trên, ta định phải biết xác mô hình động học robot Phải có mô hình động học ta xây dựng thuật toán điều khiển không gian khớp không gian làm việc Với robot đơn giản mô hình động học đơn giản nên việc xây dựng thuật toán điều khiển không thành vấn đề Tuy nhiên, robot trở nên phức tạp việc tính toán mô hình tương đối khó khăn Các kết mô thực với robot Planar hai bậc tự Đó mô hình robot đơn giản phổ biến Tuy nhiên, việc đáp ứng thu có chất lượng tương đối tốt cho thấy tính đắn thuật toán quan sát thuật toán điều khiển thiết kế Theo kết này, tác giả tin thuật toán quan sát thuật toán điều khiển áp dụng cho mô hình robot phức tạp khác 77 KẾT LUẬN Trong khuôn khổ luận văn này, tác giả tập trung làm rõ vấn đề sau: - Tìm hiểu tổng quan robot công nghiệp phương pháp dùng điều khiển robot công nghiệp - Phân tích đánh giá cụ thể phương pháp điều khiển đồng thời vị trí- lực ứng dụng thực tế - Thiết kế quan sát lực để lấy tín hiệu phản hồi lực thay cho sensor lực để điều khiển đồng thời vị trí- lực cho robot - Khảo sát mô phương pháp điều khiển lai vị trí/lực cho đối tượng robot Planar hai bậc tự Xét mặt lý thuyết kết mô phỏng, luận văn giải cách vấn đề yêu cầu đặt Mặc dù chưa có kết thực nghiệm, xong kết lý thuyết thu tạo sở tốt cho việc thiết kế hệ thống điều khiển chuyển động tay máy thực tế sử dụng quan sát lực Một lần nữa, xin gửi lời cảm ơn chân thành sâu sắc tới TS Nguyễn Phạm Thục Anh, người trực tiếp hướng dẫn bảo giúp đỡ nhiều để hoàn thành tốt luận văn 78 TÀI LIỆU THAM KHẢO Tiếng Việt Đào Văn Hiệp (2004), Kỹ thuật Robot, NXB Khoa học Kỹ thuật, Hà Nội Huỳnh Thái Hoàng (2006), Lý thuyết điều khiển tự động, Đại học Bách Khoa TP HCM Nguyễn Thiện Phúc (2006), Robot công nghiệp, NXB Khoa học Kỹ thuật, Hà Nội Nguyễn Phùng Quang (2005), Matlab & Simulink dành cho kỹ sư điều khiển tự động, NXB Khoa học Kỹ thuật, Hà Nội Nguyễn Thương Ngô (2006), Lý thuyết điều khiển thông thường đại Quyển 1, NXB Khoa học Kỹ thuật, Hà Nội Nguyễn Mạnh Tiến (2007), Điều khiển Robot công nghiệp, NXB Khoa học Kỹ thuật, Hà Nội Tiếng Anh Alcocer, A (2002): “Force estimation and control in vehicles and robots” Master’s thesis, Dept Automatic Control, Lund institute of Technology Anderson, R., and M.Spong (1988), Hybrid impedance control of robotic manipulators, J Robot Autom., vol 4, no 5, pp 549–556 Chae, A., C.Atkeson, and J.Hollerbach (1988), Model-Based Control of a Robot Manipulator, Cambridge, MA: MIT Press 10 Hacksel, P.J and S E Salcudean (1994): “Estimation of environmental force and rigid-body velocities using observers.” In Proc Of 1994 IEEE Int Conf on Robotics and Automation, pp, 931-936 11 Hogan,N (1987), Stable execution of contact tasks using impedance control, Proc IEEE Int Conf Robot Autom., pp 595–601 12 K Ohishi, M Miyazaki, M.F and Y Ogino( 1991): “ Observer Based Force Control without Force Sensor” In Proceedings Of IEEE Int Con On 79 Industrial Electronics, Control and Instrumentation, pp 1049- 1054 13 Kankaanranta, R., and H.Koivo (1988), Dynamics and simulation of compliant motion of a manipulator, IEEE Trans Robot Autom.,vol 4, pp 163–173 14 Lipkin, H., and J.Duffy (1988), Hybrid twist and wrench control for a robot manipulator, Trans AS ME J Median Transmissions Autom Design, vol 110, pp 138–144 15 McClamroch, N., and D.Wang (1988), Feedback stabilization and tracking of constrained robots, IEEE Trans Autom Control,vol 33, no 5, pp 419–426.Raibert, M., and J.Craig (1981), Hybrid position/force control of manipulators, J Dyn Syst Meas Control, vol 102, pp 126–132 16 Nicosia, S and P Tomei (1990): “Robot Control by using only Joint Position Measure ment.” IEEE Transactions on Automatic Control, 35:9,pp.1058.1061 17 Raibert,M and J Craig (1981): “Hybrid position/force control of manipulators.” ASME J.Dynam.Syst Measurement Control 80 PHỤ LỤC Dưới mã nguồn chương trình mô xây dựng Matlab-Function Chương trình vào thông số robot global m1 m2 l1 l2 g I1 I2 m1=3.5; %Khối lượng m2=2+0.5;% Khối lượng ( 2kg) khối lượng tải (0.5 kg) I1=0.018; % gia tốc quán tính I2=0.01; % gia tốc quán tính l1=0.25; % chiều dài l2=0.25;% Chiều dài g=9.81;% gia tốc trọng trường Hàm tính ma trận mô men trọng lực G(q) function out=g_cal(in); parasys; q1=in(1); % góc theta1 q2=in(2); % góc theta2 out(1)=9.81*0.5*m1*l1*cos(q1)+m2*9.81*(l1*cos(q1)+0.5*l2*cos(q1+q2)); out(2)=9.81*m2*0.5*l2*cos(q1+q2); Hàm tính ma trận mômen nhớt hướng tâm V (q, q) = C (q, q) q function out=v_cal(in); parasys; dq1=in(1); % tốc độ góc khớp dq2=in(2);% tốc độ góc khớp q1=in(3); % góc theta1 q2=in(4); % góc theta2 out(1)=-2*m2*0.5*l1*0.5*l2*sin(q2)*dq1*dq2m2*0.5*l1*0.5*l2*sin(q2)*dq2^2; out(2)=m2*l1*0.5*l2*sin(q2)*dq1^2; 81 Hàm tính gia tốc khớp lực F robot sinh function out=acce(in); parasys; om=40; % hệ số bám trục x gm=400; % hệ số bám trục x q1=in(3); q2=in(4); q11=in(5); % gia toc khop q22=in(6);%gia toc khop tvg(1)=in(1); tvg(2)=in(2); tvg(3)=l1*sin(q1)*q11^2+l2*sin(q1+q2)*(q11+q22)^2-om*l1*cos(q1)*q11om*l2*cos(q1+q2)*(q11+q22)-gm*l1*sin(q1)-gm*l2*sin(q1+q2); tvg=tvg'; H(1,1)=0.25*m1*l1^2+m2*(l1^2+l2^2+2*l1*l2*cos(q2))+I1+I2; H(1,2)=m2*(0.25*l2^2+l1*0.5*l2*cos(q2))+I2; H(1,3)=l1*cos(q1)+l2*cos(q1+q2); H(2,1)=H(1,2); H(2,2)=m2*0.25*l2^2+I2; H(2,3)=l2*cos(q1+q2); H(3,1)=l1*cos(q1)+l2*cos(q1+q2); H(3,2)= l2*cos(q1+q2); H(3,3)=0; out=inv(H)*tvg; % Ma trận out[ 1x3] Trong out(1,1) out (1,2) hàng đầu gia tốc khớp1 khớp2 out(1,3) lực robot sinh Hàm tính ngịch đảo ma ( sử dụng quan sát) function out=H_dao(in); parasys; q1=in(1); 82 q2=in(2); H(1,1)=0.25*m1*l1^2+m2*(l1^2+l2^2+2*l1*l2*cos(q2))+I1+I2; H(1,2)=m2*(0.25*l2^2+l1*0.5*l2*cos(q2))+I2; H(2,1)=H(1,2); H(2,2)=m2*0.25*l2^2+I2; out=inv(H) Hàm tính ma trận Jacoby giả đảo (sử dụng quan sát ) function out=Ja_dao_dacbiet(in); parasys; q1=in(1); q2=in(2); j(1,1)=-l1*sin(q1)-l2*sin(q1+q2); j(1,2)=-l2*sin(q1+q2); j(2,1)=l1*cos(q1)+l2*cos(q1+q2); j(2,2)=l2*cos(q1+q2); j1= j'; out = inv(j*j1)*j; ^ Hàm tính lực quan sát F function out=fxfym(in); parasys; Jgiadao(1,1)=in(1); % hệ số ma trận jacoby giả đảo Jgiadao(1,2)=in(2); % hệ số ma trận jacoby giả đảo Jgiadao(2,1)=in(3); % hệ số ma trận jacoby giả đảo Jgiadao(2,2)=in(4); % hệ số ma trận jacoby giả đảo k2x1ng(1)=in(5);% sai lệch ước lượng biến khớp k2x1ng(2)=in(6); sai lệch ước lượng biến khớp k2x1ng=k2x1ng'; tg=Jgiadao*k2x1ng; 83 Hàm tính động học thuận function out= DHT(q); parasys; q1=q(1); q2=q(2); out(1)=l1*cos(q1)+l2*cos(q1+q2); out(2)=l1*sin(q1)+l2*sin(q1+q2); Mô hình robot Planar có kể đến lực F xây dựng Matlab/simulink 84 ... lực để điều khiển lực tương tác robot môi trường: Trong chương tác giả nghiên cứu xây dựng quan sát lực cho robot, xây dựng quan sát lực cho robot Planar, xây dựng điều khiển lực cho robot Planar... lực robot • Nghiên cứu thiết kế quan sát lực để ước lượng lực tương tác robot môi trường • Thiết kế điều khiển lai vị trí- lực cho robot Planar sử dụng quan sát lực để lấy tín hiệu phản hồi lực. .. pháp điều khiển lực/ vị trí 46 CHƯƠNG 3: NGHIÊN CỨU, THIẾT KẾ BỘ QUAN SÁT LỰC ĐỂ ĐIỀU KHIỂN LỰC TƯƠNG TÁC GIỮA ROBOT VÀ MÔI TRƯỜNG 48 3.1 Ươc lượng lực sở quan sát 48 3.2 Xây dựng quan