Phương pháp cuốn chiếu trong điều khiển chuyển động robot

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - ĐOÀN THỊ XUÂN QUỲNH PHƯƠNG PHÁP CUỐN CHIẾU (BACKSTEPPING) TRONG ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT Chuyên ngành: ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA LUẬN VĂN THẠC SĨ KHOA HỌC ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA NGƯỜI HƯỚNG DẪN KHOA HỌC : TS NGUYỄN PHẠM THỤC ANH Hà Nội - 2011 LỜI CAM ĐOAN Tôi xin cam đoan luận văn tốt nghiệp “ Phương pháp chiếu (Backstepping) điều khiển chuyển động robot” tự thiết kế hướng dẫn cô giáo TS.Nguyễn Phạm Thục Anh, tham khảo thêm tài liệu liệt kê mục tài liệu tham khảo Nếu phát có chép, tơi xin hoàn toàn chịu trách nhiệm Học viên : Đoàn Thị Xuân Quỳnh Môc lôc Trang CHƯƠNG :TỔNG QUAN VỀ CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN ROBOT 1.1 Các phương pháp điều khiển robot 1.2 Điều khiển chuyển động không gian khớp 1.2.1 Điều khiển PD bù trọng trường 1.2.2 Điều khiển PD bù trọng trường 1.2.3 Phương pháp tuyến tính hóa xác 1.3 Điều khiển chuyển động không gian làm việc 1.3.1 Phương pháp Jacoby đảo 1.3.2 Phương pháp Jacoby chuyển vị 10 1.4 Các phương pháp điều khiển nâng cao 11 1.4.1 Phương pháp thích nghi Lee_Slotine 11 1.4.2 Điều khiển thích nghi theo động lực học đảo 12 1.4.3 Phương pháp điều khiển bền vững thích nghi 15 CHƯƠNG CƠ SỞ LÝ THUYẾT PHƯƠNG PHÁP BACKSTEPPING 20 2.1 Đặt vấn đề 20 2.2 Lý thuyết ổn định Lyapunov 21 2.2.1 Hệ phi tuyến biến đổi theo thời gian 21 2.2.2 Hệ phi tuyến không biến đổi theo thời gian 22 2.3 Phương pháp Backstepping tổng quát 24 2.4 Phương pháp Backstepping tổng quát cho robot n dof 26 2.4.1 Phân tích tham số phương trình động lực học 26 2.4.2 Điều khiển không gian khớp 27 2.4.3 Luật điều khiển không gian làm việc 29 CHƯƠNG 3: ÁP DỤNG PHƯƠNG PHÁP ĐIỀU KHIỂN BACKSTEPPING CHO ROBOT SCARA SEPENT BẬC TỰ DO 33 3.1 Giải toán động học thuận 33 3.2 Giải toán động học ngược 36 3.3 Tính toán ma trận Jacoby 37 3.4 Phương trình động lực học 40 3.5 Điều khiển Backstepping cho robot Scara dof không gian khớp 46 3.6 Điều khiển Backstepping cho robot Scara không gian làm việc 47 CHƯƠNG : KẾT QUẢ MÔ PHỎNG ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT BẰNG PHƯƠNG PHÁP BACKSTEPPING VÀ KẾT LUẬN 49 4.1 Điều khiển không gian khớp 49 DANH MỤC CHỮ VIẾT TẮT Chữ viết tắt Nội dung ĐLH : Động lực học DOF : Bậc tự (degree of freedom) PD : Proportion-Derivative Controller PID : Proportion-Intergral-Derivative Controller DANH MỤC HÌNH VẼ Hình 1.1 Các phương pháp điều khiển robot Erro r! Bookmark not defined Hình 1.2 Sơ đồ điều khiển phương pháp PD bù trọng trường Hình 1.3 Sơ đồ điều khiển phương pháp PID Hình 1.4 Sơ đồ điều khiển phương pháp tuyến tính hóa xác Erro r! Bookmark not defined Hình 1.5 Sơ đồ điều khiển trực tiếp không gian làm việc Hình 1.6 Sơ đồ điều khiển phương pháp Jacoby đảo Hình 1.7 Sơ đồ điều khiển phương pháp Jacoby chuyển vị Erro r! Bookmark not defined Hình 1.8 Sơ đồ điều khiển phương pháp động lực học ngược thích nghi Erro r! Bookmark not defined Hình 2.1 Minh họa khái niệm ổn định Lyapunov 26 Hình 2.2 Kiểm tra tính ổn định hệ 27 Hình 2.3 Đối tượng truyền thẳng qua khâu tích phân 30 Hình 2.4 Cấu trúc hệ truyền ngược 33 Hình 3.1 Hình ảnh robot Scara 42 Hình 3.2 Khơng gian làm việc robot Serpent 43 Hình 3.3 Chọn hệ tọa độ cho robot Scara 44 Hình 3.4 Tọa độ khối tâm nối 49 Hình 4.1 Vị trí đầu cuối robot………………………………………………… ………… 62 Hình 4.2 Đáp ứng khớp γ µ nhỏ ………………………………………………… 63 Hình 4.3 Đáp ứng khớp γ µ tăng 64 Hình 4.4 Đáp ứng khớp γ = µ =50 64 Hình 4.5 Đáp ứng khớp γ = µ =50 65 Hình 4.6 Đáp ứng khớp γ = µ =50 65 Hình 4.7 Đáp ứng momen khớp 66 CHƯƠNG TỔNG QUAN VỀ CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN ROBOT 1.1 Các phương pháp điều khiển Robot Có nhiều phương pháp dùng để điều khiển chuyển động Robot Ta phân hai tốn điều khiển chính: điều khiển quỹ đạo chuyển động điều khiển đồng thời quỹ đạo, lực Trong chia tốn điều khiển khơng gian khớp điều khiển không gian làm việc Điều khiển chuyển động Rôbôt Điều khiển quỹ đạo chuyển động Không gian khớp Điều khiển phi tuyến mơ hình PD bù trọng trường Điều khiển quỹ đạo lực Không gian làm việc PID Jacoby chuyển vị Điều khiển lai Jacoby đảo Điều khiển trở kháng … Hình 1.1 Các phương pháp điều khiển robot Các phương pháp địi hỏi phải biết xác thông số động học, động lực học Robot Điểm hạn chế phương pháp chưa thể đáp ứng thích nghi được, tức chúng cần phải có mơ hình động học cụ thể , xác Robot để thực Do đó, thực tế, người ta dùng phương pháp nâng cao khác như: - Điều khiển thích nghi (Theo mơ hình MRAC Lee Slotine) - Điều khiển mờ - Mạng Nơron - Điều khiển học (Learning Control) - Điều khiển truợt - Điều khiển chiếu Backstepping - … 1.2 Điều khiển chuyển động không gian khớp Quỹ đạo khớp : , giá trị đặt : Mục đích tốn tìm momen điều khiển để Bài toán động học ngược giải trước để chuyển thông số từ không gian công tác sang không gian khớp Mạch điều khiển nhận giá trị đặt biến khớp điều khiển khớp theo sát diễn tiến thời gian biến khớp Mạch kiểu đơn giản độ xác bị hạn chế đối tượng cần giám sát trực tiếp phần cơng tác lại nằm ngồi mạch điều khiển 1.2.1 Điều khiển PD – bù trọng trường * Tay máy gồm nhiều khâu, khớp cấu liên quan Mỗi phận có trọng lượng Trong q trình làm việc, phận thay đổi, nên trọng lực đặt lên chúng thay đổi không theo quy luật định trước Nhiệm vụ điều khiển ổn định trạng thái tay máy dù làm việc Bộ điều khiển phải thường xuyên giám sát trạng thái hệ thống tác dụng trọng trường, phát tín hiệu điều khiển thích hợp để bù lại tác dụng IR Hình 1.2 Sơ đồ điều khiển với phương pháp PD – bù trọng trường * Để thực điều này, người ta sử dụng lý thuyết Lyapunov: trình hệ thống tiệm tiến đến trạng thái cân lượng giảm dần trạng thái cân bằng, lượng hệ đạt cực tiểu Và trường hợp này, lượng phần công tác Về lý thuyết Lyapunov,chúng ta làm rõ chương sau ∗ Phương trình động lực học: (1.1) Bài toán đặt từ , giá trị thực đo sensor ta phải tính tốn phù hợp ∗ Luật điều khiển: (1.2) ; Với : , ma trận đường chéo xác định dương * Luật điều khiển gồm thành phần : + Thành phần trọng lực + Thành phần tỉ lệ , có tác dụng bù trọng lực robot có tác dụng điều khiển để 10 Khối lượng nối Khối lượng nối Khối lượng nối Khối lượng nối Chiều dài nối Chiều dài nối ∗ Giả sử ta điều khiển cho robot chạy từ vị trí đầu động theo quỹ đạo tới vị trí Hình 4.1.Ví dụ vị trí robot ban đầu vị trí cuối ∗ Sau chỉnh thơng số ∗ , ta số kết sau: Với 75 chuyển Goc quay khop Goc quay khop Goc quay khop 2(rad) 0.8 q1 thuc q1 dat 1.5 0.5 0 0.5 1.5 thoi gian (s) Dich chuyen khop 0.25 0.4 0.2 0.5 1.5 thoi gian (s) Goc quay khop 0.15 0.5 1.5 thoi gian (s) Ta thấy thỏa mãn ngưỡng q4 thuc q4 dat 0.5 Hình 4.2 Kết ∗ 1.5 d3 thuc d3 dat 0.2 0.1 q2 thuc q2 dat 0.6 goc quay khop 4(rad) Dich chuyen khop 3(rad) Goc quay khop 1(rad) 0.5 1.5 thoi gian (s) nhỏ nhỏ chưa đủ để làm cho hàm CLF nên giá trị thực xa vời Khi tăng dần đến quỹ đạo thực bắt đầu bám quỹ đạo đặt , nhiên chưa đủ lớn gây dao động ∗ Khi ta tăng hệ số đủ lớn, sai lệch giảm nhiều, quỹ đạo thực bám sát quỹ đạo đặt, cụ thể với hệ số sau : 76 Goc quay khop 1(rad) Sai lech goc quay khop 1(rad) q1 thuc q1 dat 1.5 0.5 0 0.5 1.5 thoi gian (s) Sai lech goc quay khop 0.02 -0.02 -0.04 0.5 1.5 thoi gian (s) Sai lech toc goc quay khop 1(rad/s)Van toc goc quay khop 1(rad/s) Goc quay khop Van toc goc quay khop 1.5 dq1 thuc dq1 dat 0.5 -0.5 0.5 1.5 thoi gian (s) Sai lech toc quay khop 1 0.5 0 0.5 1.5 thoi gian (s) Goc quay khop Goc quay khop 2(rad) 0.8 0.4 0.2 Sai lech goc quay khop 2(rad) q2 thuc q2 dat 0.6 0.5 1.5 thoi gian (s) -3 x 10 Sai lech goc quay khop 2 -5 -10 -15 0.5 1.5 thoi gian (s) Sai lech toc goc quay khop 2(rad/s)Van toc goc quay khop 2(rad/s) Hình 4.3 Đáp ứng góc quay tăng 77 Van toc goc quay khop 0.6 dq2 thuc dq2 dat 0.4 0.2 -0.2 0.5 1.5 thoi gian (s) Sai lech toc quay khop 2 0.4 0.3 0.2 0.1 0 0.5 1.5 thoi gian (s) 0.2 0.15 0.1 0.5 thoi gian (s) 1.5 -3 x 10 Sai lech dich chuyen khop -1 -2 -3 0.5 thoi gian (s) 1.5 Van toc dich chuyen khop 0.08 dd3 thuc dd3 dat 0.06 0.04 0.02 -0.02 Sai lech toc dich chuyen khop 3(m/s) d3 thuc d3 dat Sai lech dich chuyen khop (m/s) Do dich chuyen khop (m) Do dich chuyen khop 0.25 Van toc dich chuyen khop 3(m/s) Hình 4.4 Đáp ứng góc quay tăng 0.5 thoi gian (s) Sai lech toc dich chuyen khop 0.4 0.3 0.2 0.1 0 0.5 thoi gian (s) Hình 4.5 Đáp ứng khớp tịnh tiến tăng 78 1.5 1.5 0.5 0.5 -3 x 10 1.5 thoi gian (s) Sai lech goc quay khop -5 -10 0.5 thoi gian (s) 1.5 Van toc goc quay khop 0.6 dq4 thuc dq4 dat 0.4 0.2 -0.2 0.5 1.5 thoi gian (s) Sai lech toc quay khop 0.3 0.2 0.1 0 0.5 thoi gian (s) 1.5 Momen khop 6.08 Momen khop (N.m) Momen cua khop (N.m) Momen cua khop 6.4 6.3 6.2 6.1 5.9 0.5 1.5 thoi gian (s) Luc khop Luc cua khop (N) -20.46 -20.48 -20.5 -20.52 -20.54 6.06 6.04 6.02 5.98 0.5 1.5 thoi gian (s) 79 0.4 Hình 4.6 Đáp ứng khớp quay tăng Momen cua khop (N.m) goc quay khop 4(rad) Sai lech goc quay khop 4(rad) q4 thuc q4 dat Sai lech toc goc quay khop 4(rad/s)Van toc goc quay khop 4(rad/s) Goc quay khop 1.5 0.5 1.5 thoi gian (s) Momen khop 6.04 6.02 5.98 0.5 1.5 thoi gian (s) 2 Hình 4.7 Đáp ứng momen khớp tăng PHỤ LỤC Robot.m ( Các thông số robot) function [q,dq,ddq]= Robot(T,x0,T0) %Mo phong robot sara noi %Cac thong so cua Robot m1=4;% Khoi luong cua noi [Kg] m2=1.5;% Khoi luong cua noi [Kg] m3=2;% Khoi luong cua noi [Kg] m4=0.6;% Khoi luong cua noi [Kg] l1=0.25;% Chieu dai cua noi [m] l2=0.15;% Chieu dai cua noi [m] lc1=l1/2; lc2=l2/2; I1=0.15;% Momen quan tinh cua noi [Kg.m^2] I2=0.032;% Momen quan tinh cua noi [Kg.m^2] I4=0.032;% Momen quan tinh cua noi [Kg.m^2] %Luc masat dong (Fd), masat tinh (Fs),nhieu Td Fd=[1 0 0;0 0;0 0;0 0 1]; Fs=[1;1;1;1]; Td=[5;5;4;5]; g=9.81; 80 %==================================================================== == T1=T(1);%Momen dieu khien khop T2=T(2);%Momen dieu khien khop F3=T(3);%Luc dieu khien khop T4=T(4);%Momen dieu khien khop x11=x0(1);%Goc theta1 x12=x0(2);%Toc goc khop x21=x0(3);%Goc theta2 x22=x0(4);%Toc goc khop x31=x0(5);%do dich chuyen d3 x32=x0(6);%Toc dich chuyen khop x41=x0(7);%Goc theta4 x42=x0(8);%Toc goc khop %==================================================================== == c2=cos(x21); s2=sin(x21); %Ma tran quan tinh M11=m1*lc1^2+(m2+m3+m4)*l1^2+(m2+m3+m4)*l2^2+I1+I2+I4+2*l1*l2*(m2+m3+ m4)*c2; M12=(m2+m3+m4)*lc2^2+I2+I4+(m2+m3+m4)*l1*lc2*c2; M14=I4; M21=(m2+m3+m4)*lc2^2+I2+I4+(m2+m3+m4)*l1*lc2*c2; M22=(m2+m3+m4)*lc2^2+I2+I4; M24=I4; M33=m3+m4; M41=I4; M42=I4; M44=I4; M=[M11 M12 M14; M21 M22 M24; 0 M33 ; M41 M42 M44]; %Momen nhot , huong tam va truong h1=-2*l1*lc2*(m2+m3+m4)*s2*x12*x22; h2=l1*lc2*(m2+m3+m4)*s2*x12^2; h3=-(m3+m4)*g; h4=0; %Tinh ma tran Hk(known) co ca masat va nhieu H=[h1;h2;h3;0]+Fd*[x12;x22;x32;x42]+Fs+Td; %Nghich dao ma tran M Minv=inv(M); %Gia toc khop dX=-Minv*H+Minv*[T1;T2;F3;T4]; %Phuong trinh trang thai x11p=x12; x12p=dX(1); x21p=x22; x22p=dX(2); x31p=x32; x32p=dX(3); x41p=x42; 81 x42p=dX(4); %Tinh gan dung phuong trinh vi phan x11=x11+T0*x11p; x12=x12+T0*x12p; x21=x21+T0*x21p; x22=x22+T0*x22p; x31=x31+T0*x31p; x32=x32+T0*x32p; x41=x41+T0*x41p; x42=x42+T0*x42p; %==================================================================== == q=[x11;x21;x31;x41]; dq=[x12;x22;x32;x42]; ddq=[x12p;x22p;x32p;x42p]; end quidaokhop.m ( thiết kế quỹ đạo chuyển động cho tay robot) function [q,dq]=quidaokhop(q0,qc,ddq,t1,t2,tc,t) %Chuong trinh tinh quy dao 2-1-2 va toc khop (do thi hinh thang) if tt1)&(tt2)&(t