Giải bài toán động học Robot là vấn đề kỹ thuật kinh điển. Tài liệu này sẽ trình bày lý thuyết thuật toán giải các bài toán thuận nghịch và thực hiện thuật toán ấy trên công cụ tính toán mô phỏng Matlab.
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI KHOA ĐIỆN BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG ====o0o==== BÁO CÁO ROBOT CÔNG NGHIỆP Giáo viên hướng dẫn : PGS.TS Phan Bùi Khôi Sinh viên thực : Trịnh Văn Vương MSSV : 20073539 Lớp : KSTN - ĐKTĐ Khóa : 52 Đề số : 17 Hà nội, 12-2011 MỤC LỤC GIỚI THIỆU Chương THIẾT LẬP HỆ PHƯƠNG TRÌNH ĐỘNG HỌC ROBOT .2 1.1 Phân tích động học Denavit–Hartenberg 1.2 Thiết lập hệ phương trình động học Ma trận trạng thái khâu thao tác theo cấu trúc động học Dựa bảng tham số động học 1.1, xây dựng ma trận biến đổi tọa độ Denavit-Hartenberg dựa công thức tổng quát sau: Chương BÀI TOÁN ĐỘNG HỌC ROBOT Giải toán động học Robot vấn đề kỹ thuật kinh điển Trong chương trình bày lý thuyết thuật toán giải toán thuận nghịch thực thuật toán công cụ tính toán mô Matlab Do mã nguồn chương trình dài nên không tiện cho việc trình bày nên chủ yếu mô tả cách thực thuật toán phần mềm Mã chương trình tham khảo phụ lục file mềm kèm 2.1 Bài toán động học thuận 2.2 Bài toán động học ngược Chương THIẾT KẾ VÀ MÔ PHỎNG ROBOT 3.1 Thiết kế mô hình Robot 3D Solidworks 3.2 Mô hoạt động Robot Matlab .10 TÀI LIỆU THAM KHẢO 13 PHỤ LỤC 14 Giới thiệu GIỚI THIỆU Số thứ tự đề bài: 17 Ký hiệu tên robot: ‘Robot No.17’ Nội dung báo cáo: Bao gồm tập đề, chia làm chương Đó là: Thiết lập hệ phương trình động học robot.(bắt buộc) Giải toán động học robot.(bắt buộc) Thiết kế mô phỏng.(tự chọn) Công cụ tính toán mô phỏng: Matlab Công cụ thiết kế khí: Solidworks Kết thu được: Tính toán, giải mô thành công động học robot Hướng phát triển: Có thể ứng dụng thực tiễn công nghiệp, hàn, ghép phôi, hay vận chuyển hàng thông minh Giáo viên hướng dẫn Sinh viên thực PGS.TS Phan Bùi Khôi Trịnh Văn Vương Chương Thiết lập hệ phương trình động học Robot Chương THIẾT LẬP HỆ PHƯƠNG TRÌNH ĐỘNG HỌC ROBOT 1.1 Phân tích động học Denavit–Hartenberg Thiết lập hệ tọa độ Denavit-Hartenberg Hình vẽ mô tả cấu trúc Robot No.17 hệ tọa độ Descartes gắn vào khâu theo quy ước Denavit-Hartenberg: Z3 Z2 Zo X2 Z1 X1 Xo Hình 1.1 Robot No.17 hệ tọa độ Descartes Mô hình robot cụ thể hóa từ mô hình túy mang tính cấu trúc đề Robot có bốn khâu (tính đế) với ba khớp động quay - tịnh tiến – quay Các tham số động học Denavit-Hartenberg X3 Chương Thiết lập hệ phương trình động học Robot Dựa hệ tọa độ thiết lập trên, tiến hành tính toán thông số Denavit-Hartenberg thu bảng sau: D-H Parameters of Robot No 17 Joint di θi αi d1 θ1* a1 90o d 2* 0 θ3* a3 90o Hình 1.2 Tham số động học Denavit – Hartenberg Ở đây, biến khớp đánh dấu * Cụ thể, vector biến khớp là: q =θ d θ T 1.2 Thiết lập hệ phương trình động học Ma trận trạng thái khâu thao tác theo cấu trúc động học Dựa bảng tham số động học 1.1, xây dựng ma trận biến đổi tọa độ Denavit-Hartenberg dựa công thức tổng quát sau: i-1 Ai = Cụ thể, ta có ma trận biến đổi tọa độ sau: A1 = A2 = A3 = Từ đó, thiết lập ma trận trạng thái khâu thao tác theo cấu trúc động học ( biến khớp) dựa công thức: 0A3(q) = 0A1.1A2.2A3 Chương Thiết lập hệ phương trình động học Robot Kết tính toán cho: A3(q) = Ma trận trạng thái khâu thao tác theo tọa độ thao tác Dựa theo lý thuyết, trạng thái khâu thao tác End-Effector biểu diễn qua vector định vị (mô tả bới tọa độ góc định hướng góc Euler, Cardan, Roll0 Pitch-Yaw), cụ thể là: A3(t) = Hệ phương trình động học Robot Từ kết thu trên, thiết lập phương trình động học Robot dạng hệ phương trình động học Đây hệ phương trình đại số phi tuyến gồm phương trình độc lập tuyến tính, trình bày phía đây: f1 = cos(θ1 )a 3cos(θ3 )+sin(θ1 )d +a1cos(θ1 )-x E = f = sin(θ1 )a 3cos(θ3 )-cos(θ1 )d +a1sin(θ1 )-y E = f = a 3sin(θ3 )+d1 -z E = f = cos(θ1 )cos(θ3 )-cos(β)cos(η) = f = -cos(θ1 )+sin(α)sin(β)sin(η)-cos(α)cos(η) = f = -cos(θ3 )-cos(α)cos(β) = Ghi Công việc tính toán sử dụng trợ giúp phần mềm Matlab Chương Bài toán động học Robot Chương BÀI TOÁN ĐỘNG HỌC ROBOT Giải toán động học Robot vấn đề kỹ thuật kinh điển Trong chương trình bày lý thuyết thuật toán giải toán thuận nghịch thực thuật toán công cụ tính toán mô Matlab Do mã nguồn chương trình dài nên không tiện cho việc trình bày nên chủ yếu mô tả cách thực thuật toán phần mềm Mã chương trình tham khảo phụ lục file mềm kèm 2.1 Bài toán động học thuận 2.1.1 Bài toán giải thuật Bài toán Cho trước giá trị biến khớp (các góc quay, di chuyển tịnh tiến) Yêu cầu xác vận tốc điểm tác động cuối vận tốc góc khâu thao tác Giải thuật Theo giải thuật giảng Robot công nghiệp ( Tài liệu tham khảo [1]) 2.1.2 Chương trình lập trình Giải thuật thực Matlab: ‘FwdKinematics.m’ ‘RuleGen.m’ Gọi hàm ‘RuleGen.m’ để tạo file liệu số biến khớp ‘RuleDat.mat’ Khai báo quy luật biến khớp ‘MatrixDH.m’ Gọi hàm ‘MatrixDH.m’sử dụng vòng lặp for tính ma trận A Kết Position, Velocity, Omega Hình 2.1 Trình tự giải toán thuận Matlab Chương Bài toán động học Robot Position of End-Efector 4000 xE yE zE -2000 -4000 Velocity of End-Efector vX vY vZ 0.5 Velocity Position 2000 x 10 -0.5 -1 Time Time Rotate Angle Velocity of End-Efector Orbit of End-Efector 20 wX wY wZ 15 Position 10 4000 2000 0 -2000 5000 -5 5000 -10 0 -5000 Time -5000 Hình 2.2 Nghiệm ví dụ toán động học thuận 2.2 Bài toán động học ngược 2.2.1 Bài toán giải thuật Bài toán Cho trước vị trí điểm tác động cuối (và vị rí khâu thao tác) Yêu cầu tìm giá trị biến khớp ứng với vị trí cho trước Giải thuật Ta sử dụng phương pháp số nhờ khai triển Taylor ma trận Jacobi Gọi ma trận x ma trận tọa độ điểm E thời điểm Từ toán động học thuận ta có hệ thức: x = f ( q ) đạo hàm vế ta có: x&= ∂f q&= J ( q ) q& ∂q Chương Bài toán động học Robot ∂f1 ∂q ∂f ∂f = ∂q1 Trong J ( q ) = ∂q ∂f n ∂q1 ∂f1 ∂q2 ∂f ∂q2 ∂f n ∂q2 ∂f1 ∂qn ∂f ∂qn gọi ma trân Jacobi ∂f n ∂qn −1 Từ ta tính q&( t ) = J ( q ( t ) ) x&( t ) Áp dụng khai triển Taylor sử dụng sai phân lùi để tính nghiệm : q k +1 = q ( tk + ∆t ) ≈ q k + q&k ∆t 2.2.2 Chương trình lập trình Giải thuật thực Matlab: Với quỹ đạo chuyển động đường xoắn ốc ‘InvKinematics.m’ ‘PosGen.m’ ‘PosDat.mat’ Gọi hàm ‘PosGen.m’ để tạo file liệu số vị trí khâu thao tác Khai báo quy luật thao tác tìm nghiệm đầu Tính ma trận Jacobi sử dụng vòng lặp for tính nghiệm theo xấp xỉ sai phân lùi Kết Giá trị biến khớp Hình 2.3 Trình tự giải toán ngược Matlab Chương Bài toán động học Robot Inverse Kinematics: Θ Inverse Kinematics: Distance 1500 0.05 Distance Θ1 -0.05 1000 500 -0.1 -0.15 Time Time Inverse Kinematics: Θ Orbit Position 0.04 100 Θ3 0.02 0 -0.02 -100 -0.04 -200 1200 -0.06 1180 1160 Time -100 Hình 2.4 Nghiệm ví dụ toán ngược -50 50 Chương Thiết kế mô Robot Chương THIẾT KẾ VÀ MÔ PHỎNG ROBOT 3.1 Thiết kế mô hình Robot 3D Solidworks Robot No.17 có ba bậc tự thuộc loại robot có khâu nối tiếp hở, có hai bậc xoay bậc tịnh tiến Robot No.17 thiết kế nhờ Solidworks, công cụ phổ biến trương đại học kỹ thuật: Hình 3.1 Mô hình 3D Robot No.17 Solidworks Do hạn chế kiến thức thuộc lĩnh vực khí (không với chuyên ngành đào tạo) nên mô hình robot thiết kế thông qua tham khảo từ hình mẫu robot Stanford, Scara, Puma, Kuka… Kích thước robot tham khảo từ nhiều nguồn, chủ yếu từ vẽ robot thông dụng, phổ biến phòng thí nghiệm, trường đại học giới Chương Thiết kế mô Robot Hình trình bày vẽ CAD hình chiếu 2D Robot : Hình 3.2 Bản vẽ 2D Robot No.17 3.2 Mô hoạt động Robot Matlab 10 Chương Thiết kế mô Robot Hình 3.3 Giao diện mô hoạt động Robot No.17 Tiến hành xây dựng giao diện mô hoạt động Robot No.17 Matlab hình vẽ trên, công cụ kỹ thuật cực mạnh Giao diện mô kế thừa giải thuật toán động học để tính toán hiển thị hoạt động robot Lưu đồ mô tả bước thực : Từ vẽ 3D xuất file định dạng nhị phân ‘*.stl’ mang thông tin vê cấu trúc khí robot Lập trình file hàm ‘stlread.m’ để xử lý file ‘*.stl’, trả vector đỉnh mặt (Vertices & Faces) vật thể Lưu thông số vào file liệu ‘linksdata.mat’ mang thông tin khâu Lập trình file ‘RobotNo17.m’ tạo giao diện GUI (Graphical User Interface) để tiến hành mô phỏng, sử dụng lệnh ‘patch’ để tái tạo lại mô hình 3D robot hiển thị Matlab Giao diện hiển thị hình ảnh thao tác robot công cụ chức phong phú cho phép tùy biến trạng thái không gian robot, cụ thể là: Bốn nút bấm chức Các hộp thoại trượt Demo Giải toán động học ngược để robot thao tác theo quỹ đạo xoắn ốc Các hộp thoại trượt để đặt quan sát thông số biến khớp: Random Move Đặt giá trị ngẫu nhiên cho biến khớp Clr Trail Xóa vết quỹ đạo khâu thao tác cuối Home Đưa robot vị trí ban đầu Hình 3.4 Lưu đồ bước xây dựng giao diện mô Robot No.17 11 Chương Thiết kế mô Robot Giao diện mô kế thừa thuật toán giải toán động học xây dựng chương để giải toán động học trình mô Một số hình ảnh mô phỏng: Hình 3.5 Hình ảnh mô hoạt động Robot No 17 Giao diện mô phát triển them nhiều tính khác, chẳng hạn mô phối hợp thao tác hai robot, hay bổ sung đối tượng thao tác lẫn môi trường làm việc ( băng chuyền, khớp hàn …) 12 Tài liệu tham khảo TÀI LIỆU THAM KHẢO [1] PGS.TS Phan Bùi Khôi, Bài giảng Robot công nghiệp [2] Internet 13 Phụ lục PHỤ LỤC Phụ lục Mã chương trình thiết lập phương trình động học Gồm hai mã chương trình: ‘MatrixDH.m’ function DH11 DH12 DH13 DH14 DH21 DH22 DH23 DH24 DH31 DH32 DH33 DH34 DH41 DH42 DH43 DH44 DH = end DH = MatrixDH(alpha, a, d, theta) = cos(theta); = -sin(theta)*cos(alpha); = sin(theta)*sin(alpha); = a*cos(theta); = sin(theta); = cos(theta)*cos(alpha); = -cos(theta)*sin(alpha); = a*sin(theta); = 0; = sin(alpha); = cos(alpha); = d; = 0; = 0; = 0; = 1; [ DH11 DH12 DH13 DH14; DH21 DH22 DH23 DH24; DH31 DH32 DH33 DH34; DH41 DH42 DH43 DH44]; ‘KinematicsEquation.m’ %%%%%%%%%%%%%%%%%%%%%%%% Equation Kinematics Generate %%%%%%%%%%%%%%%%%%% % Written by: Vuong V Trinh - Automatics Control % % Hanoi University of Science and Technology % % 20/12/2011 % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Equation Kinematics Generate % Preparation clear all clc % Define parameters and variables as symbols syms q1 q2 q3 a1 a3 d1 xE yE zE alpha beta eta real % Define DH parameters alpha = [pi/2 pi/2]'; a = [a1 a3]'; d = [d1 q2 0]'; theta = [q1 q3]'; % Initial and calculate DH matrix A A = eye(4); for j = 1:3 DH = MatrixDH(alpha(j),a(j),d(j),theta(j)); A = A*DH; end 14 Phụ lục % Define Cardan matrix CD11 = cos(beta)*cos(eta); CD14 = xE; CD22 = -sin(alpha)*sin(beta)*sin(eta)+cos(alpha)*cos(eta); CD24 = yE; CD33 = -sin(alpha)*cos(beta); CD34 = zE; % Define and display kinematics equations systems Eqt1 = A(1,4) - CD14 Eqt2 = A(2,4) - CD24 Eqt3 = A(3,4) - CD34 Eqt4 = A(1,1) - CD11 Eqt5 = A(2,2) - CD22 Eqt6 = A(3,3) - CD33 Phụ lục Mã chương trình toán động học thuận Gồm ba mã chương trình: ‘MatrixDH.m’ % View on the previous ‘RuleGen.m’ % This function generates rule of joint variables for % Forward kinematics problems close all clear all clc t = 0:.01:5; q1 = 2*t.^2; q2 = 2.*exp(-t); q3 = 2*t.*cos(t); save('RuleDat.mat'); clear all clc ‘FwdKinematics.m’ %%%%%%%%%%%%%%%%%%%%%%%% Forward Kinematics Solution %%%%%%%%%%%%%%%%%%%% % Written by: Vuong V Trinh - Automatics Control % % Hanoi University of Science and Technology % % 20/12/2011 % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Forward Kinematics Solution close all clear all clc % Load rule of joint variables RuleGen load('RuleDat.mat'); % Initial i = 1; alpha = [pi/2 pi/2]'; a = [800 2600]'; d = [1000 q2(i) 0]'; theta = [q1(i) q3(i)]'; A = eye(4); for j = 1:3 DH = MatrixDH(alpha(j),a(j),d(j),theta(j)); A = A*DH; end 15 Phụ lục r = A(1:3,4); xE = r(1); yE = r(2); zE = r(3); x_E(i) = xE;y_E(i) = yE;z_E(i) = zE; % Calculate for i = 2:length(t) % Assign DH parameters alpha = [pi/2 pi/2]'; a = [800 2600]'; d = [1000 q2(i) 100]'; theta = [q1(i) q3(i)]'; % Compute state matrix through kinematics variables Aold = A; A = eye(4); for j = 1:3 DH = MatrixDH(alpha(j),a(j),d(j),theta(j)); A = A*DH; end % Forward kinematics of position, velocity and rotate angle % Calculate essential matrices from DH matrix Adot = (A - Aold)/(t(i)-t(i-1)); % A dot R = A(1:3,1:3); % R Rdot = Adot(1:3,1:3); % R dot r = A(1:3,4); % r rdot = Adot(1:3,4); % rdot w_wave = Rdot*R'; % w wave w = [w_wave(3,2) w_wave(1,3) w_wave(2,1)]'; % w % Show roots of forward kinematics problem % Position xE = r(1); yE = r(2); zE = r(3); % Velocity vX = rdot(1); vY = rdot(2); vZ = rdot(3); % Rotate angle velocity wX = w(1); wY = w(2); wZ = w(3); % Save data to plot x_E(i) = xE;y_E(i) = yE;z_E(i) = zE; v_X(i) = vX;v_Y(i) = vY;v_Z(i) = vZ; w_X(i) = wX;w_Y(i) = wY;w_Z(i) = wZ; end % Visualize figure1 = figure; plot(t,x_E,'.',t,y_E,'b',t,z_E,' '); grid on hold on xlabel({'Time'}); ylabel({'Position'}); title({'Position of End-Efector'}); legend('xE','yE','zE'); figure2 = figure; plot(t,v_X,'.',t,v_Y,'b',t,v_Z,' '); grid on hold on xlabel({'Time'}); title({'Velocity of End-Efector'}); ylabel({'Velocity'}); legend('vX','vY','vZ'); figure3 = figure; plot(t,w_X,'.',t,w_Y,'b',t,w_Z,' '); grid on hold on 16 Phụ lục xlabel({'Time'}); title({'Rotate Angle Velocity of End-Efector'}); ylabel({'Position'}); legend('wX','wY','wZ'); figure4 = figure; plot3(x_E,y_E,z_E); grid on hold on title({'Orbit of End-Efector'}); % End Phụ lục Mã chương trình toán động học ngược Gồm hai mã chương trinh: ‘PosGen.m’ % This function generates position rule for inverse kinematics problems close all clear all clc t = 0:.01:5; xE = 30*t.*cos(t); yE = 1200-300*t.*(t)/(50*pi); zE = 30*t.*sin(t); save('PosDat.mat'); clear all clc ‘InvKinematics.m’ %%%%%%%%%%%%%%%%%%%%%%%% Inverse Kinematics Solution %%%%%%%%%%%%%%%%%%%% % Written by: Vuong V Trinh - Automatics Control % % Hanoi University of Science and Technology % % 20/12/2011 % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Inverse Kinematics Solution % Preparation close all clear all clc PosGen load('PosDat.mat') tstep = t(2)-t(1); % Step time % Choose initial q1old = 0; q2old = 1000; q3old = 0; q = [q1old q2old q3old]'; for i=2:length(t) q1 = q1old; q2 = q2old; q3 = q3old; % Jacobian matrix calculate J11 = q2*cos(q1)-(1614521220006991075*cos(q1)*sin(q3)) /10141204801825835211973625643008-2600*cos(q3)*sin(q1)-800*sin(q1); J12 = sin(q1); J13 = - 2600*cos(q1)*sin(q3)-(1614521220006991075*cos(q3)* sin(q1))/10141204801825835211973625643008; J21 = 800*cos(q1)+2600*cos(q1)*cos(q3)-(1614521220006991075* sin(q1)*sin(q3))/10141204801825835211973625643008+q2*sin(q1); J22 = -cos(q1); 17 Phụ lục J23 = (1614521220006991075*cos(q1)*cos(q3))/ 10141204801825835211973625643008-2600*sin(q1)*sin(q3); J31 = 0; J32 = 4967757600021511/81129638414606681695789005144064; J33 = 2600*cos(q3); J = [J11 J12 J13; J21 J22 J23; J31 J32 J33]; J_inv = inv(J); % Inverse Jacobian matrix % Position deratives matrix calculate xdot = (xE(i)-xE(i-1))/tstep; ydot = (yE(i)-yE(i-1))/tstep; zdot = (zE(i)-zE(i-1))/tstep; Posdot = [xdot ydot zdot]'; % Approximate use backward algorithm q = q+J_inv*Posdot*tstep; q1old = q(1); q2old = q(2); q3old = q(3); q1_(i) = q(1); q2_(i) = q(2); q3_(i) = q(3); end figure1 = figure; plot(t,q1_,'*'); xlabel('Time'); ylabel('\Theta_1'); title('Inverse Kinematics: \Theta_1'); grid on figure2 = figure; plot(t,q2_,'o'); xlabel('Time'); ylabel('Distance 2'); title('Inverse Kinematics: Distance 2'); grid on figure3 = figure; plot(t,q3_,'d'); xlabel('Time'); ylabel('\Theta_3'); title('Inverse Kinematics: \Theta_3'); grid on figure4 = figure; plot3(xE, yE, zE,'*'); title('Orbit Position'); grid on Phụ lục Mã chương trình tạo giao diện mô Do đoạn mã dài, xấp xỉ 600 dòng lệnh 200 dòng thích nên không tiện đưa vào Có thể tham khảo chi tiết file mềm kèm theo 18 ... Chương Thiết kế mô Robot Chương THIẾT KẾ VÀ MÔ PHỎNG ROBOT 3.1 Thiết kế mô hình Robot 3D Solidworks Robot No.17 có ba bậc tự thuộc loại robot có khâu nối tiếp hở, có hai bậc xoay bậc tịnh tiến Robot. .. Chương Thiết kế mô Robot Hình trình bày vẽ CAD hình chiếu 2D Robot : Hình 3.2 Bản vẽ 2D Robot No.17 3.2 Mô hoạt động Robot Matlab 10 Chương Thiết kế mô Robot Hình 3.3 Giao diện mô hoạt động Robot. .. ngược Chương THIẾT KẾ VÀ MÔ PHỎNG ROBOT 3.1 Thiết kế mô hình Robot 3D Solidworks 3.2 Mô hoạt động Robot Matlab .10 TÀI LIỆU THAM KHẢO