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.
Trang 1TRƯỜ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 hiện : Trịnh Văn Vương
Hà nội, 12-2011
Trang 2MỤC LỤC
GIỚI THIỆU 1
Chương 1 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 2
2.2 Thiết lập hệ phương trình động học 3
Chương 2 Bài toán động học Robot 5
2.1 Bài toán động học thuận 5
2.2 Bài toán động học ngược 6
Chương 3 Thiết kế và mô phỏng Robot 9
3.1 Thiết kế mô hình Robot 3D bằng Solidworks 9
3.2 Mô phỏng hoạt động Robot trên Matlab 10
TÀI LIỆU THAM KHẢO 13
PHỤ LỤC 14
Trang 3Giớ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 3 bài tập trong đề, chia làm 3 chương Đó là:
1 Thiết lập hệ phương trình động học của robot.(bắt buộc)
2 Giải bài toán động học robot.(bắt buộc)
3 Thiết kế và mô phỏng.(tự chọn)
Công cụ tính toán và mô phỏng: Matlab.
Công cụ thiết kế cơ khí: Solidworks.
Kết quả thu được: Tính toán, giải quyết và mô phỏng thành công động học robot.
Hướng phát triển: Có thể ứng dụng trong thực tiễn công nghiệp, như hàn, ghép phôi,
hay vận chuyển hàng thông minh
Trang 4Chương 1 Thiết lập hệ phương trình động học Robot
Chương 1 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ẽ dưới mô tả cấu trúc của Robot No.17 cùng các hệ tọa độ Descartes gắn vào mỗi khâu theo quy ước Denavit-Hartenberg:
Hình 1.1 Robot No.17 và các hệ tọa độ Descartes.
Mô hình robot trên được cụ thể hóa từ mô hình thuần túy mang tính cấu trúc ở đề bài ra Robot có bốn khâu (tính cả đế) với ba khớp động quay - tịnh tiến – quay
Các tham số động học Denavit-Hartenberg
Xo
Zo
X1 Z1
X2 Z2
Z3
X3
Trang 5Chương 1 Thiết lập hệ phương trình động học Robot
Dựa trên các hệ tọa độ đã thiết lập ở trên, tiến hành tính toán các thông số Denavit-Hartenberg thu được bảng sau:
D-H Parameters of Robot No 17
1
2
3
Hình 1.2 Tham số động học Denavit – Hartenberg.
Ở đây, các biến khớp được đánh dấu * Cụ thể, vector biến khớp ở đây là:
T
q
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 trên bảng tham số động học 1.1, xây dựng các ma trận biến đổi tọa độ thuần nhất Denavit-Hartenberg dựa trên công thức tổng quát sau:
Cụ thể, ta có các ma trận biến đổi tọa độ thuần nhất như sau:
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 ( các biến khớp) dựa trên công thức: 0A3(q) = 0A1.1A2.2A3
Trang 6Chương 1 Thiết lập hệ phương trình động học Robot
Kết quả tính toán cho: 0A3(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 có thể biểu diễn qua vector định vị (mô tả bới các tọa độ và góc định hướng như các góc Euler, Cardan, Roll-Pitch-Yaw), cụ thể là: 0A3(t)=
Hệ phương trình động học Robot
Từ các kết quả thu được ở trên, thiết lập phương trình động học Robot dưới dạng
hệ phương trình động học Đây là hệ phương trình đại số phi tuyến gồm các phương trình độc lập tuyến tính, như trình bày phía dưới đây:
f = cos(θdθ )a cos(θdθ )+sin(θdθ )d +a cos(θdθ )-x = 0
E
f = sin(θdθ )a cos(θdθ )-cos(θdθ )d +a sin(θdθ )-y = 0
1 E
3 3 3 -z = 0
f = a sin(θdθ )+d
4 )cos(θdθ )-cos(β)cos(η) = 0)cos(η) = 0) = 0
f = cos(θdθ
1
5 )+sin(α)sin(β)cos(η) = 0)sin(η) = 0)-cos(α)cos(η) = 0) = 0
f = -cos(θdθ
3
6 )-cos(α)cos(β)cos(η) = 0) = 0
f = -cos(θdθ
Ghi chú Công việc tính toán ở đây sử dụng sự trợ giúp của phần mềm Matlab.
Trang 7Chương 2 Bài toán động học Robot
Chương 2 BÀI TOÁN ĐỘNG HỌC ROBOT
Giải bài toán động học Robot là vấn đề kỹ thuật kinh điển Trong chương 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 Do mã nguồn chương trình quá dài nên không tiện cho việc trình bày nên ở đây chủ yếu mô tả cách thực hiện thuật toán trên phần mềm
Mã chương trình có thể tham khảo ở phụ lục hoặc file mềm đi kèm
2.1 Bài toán động học thuận
2.1.1 Bài toán và giải thuật
Bài toán Cho trước giá trị các biến khớp (các góc quay, các 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à vận tốc góc khâu thao tác
Giải thuật Theo giải thuật trong bài 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 hiện trên Matlab:
Hình 2.1 Trình tự giải bài toán thuận trên Matlab
‘FwdKinematics.m’
Gọi hàm ‘RuleGen.m’ để tạo
file dữ liệu số biến khớp
Khai báo quy luật biến khớp
Gọi hàm ‘MatrixDH.m’sử dụng
vòng lặp for tính ma trận A
‘MatrixDH.m’
‘RuleDat.mat’
‘RuleGen.m’
Kết quả
Position, Velocity, Omega
Trang 8Chương 2 Bài toán động học Robot
-4000
-2000
0
2000
4000
Time
Position of End-Efector
xE yE zE
-1 -0.5 0 0.5
1x 10
5
Time
Velocity of End-Efector
vX vY vZ
-10
-5
0
5
10
15
20
Time
Rotate Angle Velocity of End-Efector
wX wY wZ
-5000
0
5000 -5000
0 5000
-2000 0 2000 4000
Orbit of End-Efector
Hình 2.2 Nghiệm của ví dụ về bài 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 và giải thuật
Bài toán Cho trước vị trí điểm tác động cuối (và có thể cả vị rí các khâu thao tác) Yêu
cầu tìm giá trị các biến khớp ứng với vị trí cho trước đó
Giải thuật
Ta sẽ sử dụng phương pháp số nhờ khai triển Taylor và ma trận Jacobi
Gọi ma trận x là ma trận tọa độ điểm E tại các thời điểm.
Từ bài toán động học thuận ta có hệ thức: x f q đạo hàm 2 vế ta có:
f
Trang 9Chương 2 Bài toán động học Robot
Trong đó
n
n
n
f
J q
Từ đó ta tính được t 1 t t
Áp dụng khai triển Taylor sử dụng sai phân lùi để tính các nghiệm tiếp theo :
1
k tk t k k t
2.2.2 Chương trình lập trình
Giải thuật thực hiện trên Matlab: Với quỹ đạo chuyển động là đường xoắn ốc
Hình 2.3 Trình tự giải bài toán ngược trên Matlab
‘InvKinematics.m’
Gọi hàm ‘PosGen.m’ để tạo file
dữ liệu số vị trí khâu thao tác
Khai báo quy luật thao tác và tìm nghiệm đầu
Tính ma trận Jacobi và sử dụng vòng lặp for tính nghiệm theo xấp xỉ sai phân lùi
‘PosDat.mat’
‘PosGen.m’
Kết quả
Giá trị biến khớp
Trang 10Chương 2 Bài toán động học Robot
-0.15
-0.1
-0.05
0
0.05
Time
1
Inverse Kinematics:
1
0 500 1000 1500
Time
Inverse Kinematics: Distance 2
-0.06
-0.04
-0.02
0
0.02
0.04
Time
3
Inverse Kinematics:
3
-100 -50
1160 1180
1200-200 -100 0 100
Orbit Position
Hình 2.4 Nghiệm của ví dụ về bài toán ngược
Trang 11Chương 3 Thiết kế và mô phỏng Robot
Chương 3 THIẾT KẾ VÀ MÔ PHỎNG ROBOT
3.1 Thiết kế mô hình Robot 3D bằng Solidworks
Robot No.17 có ba bậc tự do thuộc loại robot có các khâu nối tiếp hở, có hai bậc xoay và một bậc tịnh tiến Robot No.17 được thiết kế nhờ Solidworks, một công cụ rất phổ biến trong các trương đại học kỹ thuật:
Hình 3.1 Mô hình 3D Robot No.17 trên nền Solidworks.
Do sự hạn chế về kiến thức thuộc lĩnh vực cơ khí (không đúng với chuyên ngành đào tạo) nên mô hình robot ở đây được thiết kế thông qua tham khảo từ hình mẫu các robot như Stanford, Scara, Puma, Kuka… Kích thước robot cũng được tham khảo từ nhiều nguồn, chủ yếu từ các bản vẽ robot thông dụng, phổ biến ở các phòng thí nghiệm, các trường đại học trên thế giới
Trang 12Chương 3 Thiết kế và mô phỏng Robot
Hình dưới trình bày bản vẽ CAD hình chiếu 2D của Robot :
Hình 3.2 Bản vẽ 2D Robot No.17. 3.2 Mô phỏng hoạt động Robot trên Matlab
Trang 13Chương 3 Thiết kế và mô phỏng Robot
Hình 3.3 Giao diện mô phỏng hoạt động Robot No.17
Tiến hành xây dựng giao diện mô phỏng hoạt động của Robot No.17 trên nền Matlab như hình vẽ trên, một công cụ kỹ thuật cực mạnh Giao diện mô phỏng này kế thừa các giải thuật của bài toán động học để tính toán và hiển thị hoạt động của robot Lưu đồ dưới mô tả các bước thực hiện :
Hình 3.4 Lưu đồ các bước xây dựng giao diện mô phỏng Robot No.17
Từ các bản vẽ 3D xuất ra file định dạng nhị phân
‘*.stl’ mang thông tin vê cấu trúc cơ khí của robot.
Lập trình file hàm ‘stlread.m’ để xử lý các file ‘*.stl’, trả về các
vector đỉnh và mặt (Vertices & Faces) của vật thể Lưu các thông
số này vào file dữ liệu ‘linksdata.mat’ mang thông tin về các 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, trong đó sử dụng lệnh ‘patch’ để tái tạo lại mô hình 3D của robot và hiển thị trên Matlab Giao diện này hiển thị hình ảnh thao tác của robot và các công cụ chức năng phong phú cho phép tùy biến các trạng thái không gian của robot, cụ thể là:
Bốn nút bấm chức năng Demo Giải bài toán động học ngược để
robot thao tác theo quỹ đạo xoắn ốc
Random Move Đặt giá trị ngẫu nhiên cho
các biến khớp
Clr Trail Xóa vết quỹ đạo của khâu thao
tác cuối
Home Đưa robot về vị trí ban đầu.
Các hộp thoại và thanh trượt
Các hộp thoại và thanh trượt này để cái đặt và quan sát thông số biến khớp:
Trang 14Chương 3 Thiết kế và mô phỏng Robot
Giao diện mô phỏng này kế thừa các thuật toán giải bài toán động học đã xây dựng
ở chương 2 để giải bài toán động học trong quá trình mô phỏng
Một số hình ảnh mô phỏng:
Hình 3.5 Hình ảnh mô phỏng hoạt động của Robot No 17.
Giao diện mô phỏng này còn đang được phát triển them nhiều tính năng khác, chẳng hạn mô phỏng phối hợp thao tác của 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 …)
Trang 15Tà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
Trang 16Phụ lục
PHỤ LỤC
Phụ lục 1 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 DH = MatrixDH(alpha, a, d, theta)
DH11 = cos(theta);
DH12 = -sin(theta)*cos(alpha);
DH13 = sin(theta)*sin(alpha);
DH14 = a*cos(theta);
DH21 = sin(theta);
DH22 = cos(theta)*cos(alpha);
DH23 = -cos(theta)*sin(alpha);
DH24 = a*sin(theta);
DH31 = 0;
DH32 = sin(alpha);
DH33 = cos(alpha);
DH34 = d;
DH41 = 0;
DH42 = 0;
DH43 = 0;
DH44 = 1;
DH = [ DH11 DH12 DH13 DH14;
DH21 DH22 DH23 DH24;
DH31 DH32 DH33 DH34;
DH41 DH42 DH43 DH44];
end
‘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
clc
% Define parameters and variables as symbols
% Define DH parameters
alpha = [pi/2 0 pi/2]';
a = [a1 0 a3]';
d = [d1 q2 0]';
theta = [q1 0 q3]';
% Initial and calculate DH matrix A
A = eye(4);
DH = MatrixDH(alpha(j),a(j),d(j),theta(j));
A = A*DH;
Trang 17Phụ 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 2 Mã chương trình bài 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
clc
t = 0:.01:5;
q1 = 2*t.^2; q2 = 2.*exp(-t); q3 = 2*t.*cos(t);
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
clc
% Load rule of joint variables
RuleGen
% Initial
i = 1;
alpha = [pi/2 0 pi/2]';
a = [800 0 2600]';
d = [1000 q2(i) 0]';
theta = [q1(i) 0 q3(i)]';
A = eye(4);
DH = MatrixDH(alpha(j),a(j),d(j),theta(j));
A = A*DH;
end
Trang 18Phụ 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
alpha = [pi/2 0 pi/2]';
a = [800 0 2600]';
d = [1000 q2(i) 100]';
theta = [q1(i) 0 q3(i)]';
Aold = A;
A = eye(4);
DH = MatrixDH(alpha(j),a(j),d(j),theta(j));
A = A*DH;
end
xE = r(1); yE = r(2); zE = r(3);
vX = rdot(1); vY = rdot(2); vZ = rdot(3);
wX = w(1); wY = w(2); wZ = w(3);
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
legend( 'xE' , 'yE' , 'zE' );
figure2 = figure;
plot(t,v_X, '.' ,t,v_Y, 'b' ,t,v_Z, ' ' );
grid on
hold on
legend( 'vX' , 'vY' , 'vZ' );
figure3 = figure;
plot(t,w_X, '.' ,t,w_Y, 'b' ,t,w_Z, ' ' );
grid on
Trang 19Phụ lục
legend( 'wX' , 'wY' , 'wZ' );
figure4 = figure;
plot3(x_E,y_E,z_E);
grid on
hold on
% End
Phụ lục 3 Mã chương trình bài 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
clc
t = 0:.01:5;
xE = 30*t.*cos(t);
yE = 1200-300*t.*(t)/(50*pi);
zE = 30*t.*sin(t);
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
clc
PosGen
% Choose initial
q1old = 0; q2old = 1000; q3old = 0;
q = [q1old q2old q3old]';
q1 = q1old; q2 = q2old; q3 = q3old;
/10141204801825835211973625643008-2600*cos(q3)*sin(q1)-800*sin(q1); J12 = sin(q1);
sin(q1))/10141204801825835211973625643008;
sin(q1)*sin(q3))/10141204801825835211973625643008+q2*sin(q1);
J22 = -cos(q1);
Trang 20Phụ lục
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];
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]';
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;
grid on
figure2 = figure;
grid on
figure3 = figure;
grid on
figure4 = figure;
grid on
Phụ lục 4 Mã chương trình tạo giao diện mô phỏng Do đoạn mã quá dài, xấp xỉ
600 dòng lệnh và 200 dòng chú thích nên không tiện đưa vào đây Có thể tham khảo chi tiết ở file mềm kèm theo