.Tính toán thiết kế quỹ đạo chuyển động dạng bậc 3- 123docz.net

Xét tổng quát cho một khớp bất kỳ: Quỹ đạo khớp quay bậc 3 có dạng:

𝜃𝑖(𝑡) = 𝑎0𝑖 + 𝑎1𝑖𝑡 + 𝑎2𝑖𝑡2+ 𝑎3𝑖𝑡3

Quỹ đạo của tốc độ:

𝜃.𝑖(𝑡) = 𝑎1𝑖 + 2𝑎2𝑖𝑡 + 3𝑎3𝑖𝑡2

Hai ràng buộc về giá trị góc khớp ban đầu và giá trị góc khớp cuối là:

{𝜃𝑖(0) = 𝜃0𝑖 𝜃𝑖(𝑡𝑓𝑖) = 𝜃𝑓𝑖

Hai ràng buộc về tốc độ khớp ban đầu và tốc độ khớp cuối là:

{𝜃 . 𝑖(0) = 𝜃.0𝑖 𝜃.𝑖(𝑡𝑓𝑖) = 𝜃.𝑓𝑖 Hệ phương trình về ràng buộc: { 𝜃𝑖(0) = 𝑎0𝑖 = 𝜃0𝑖 𝜃𝑖(𝑡𝑓𝑖) = 𝑎0𝑖 + 𝑎1𝑖𝑡𝑓𝑖 + 𝑎2𝑖𝑡𝑓𝑖2 + 𝑎3𝑖𝑡𝑓𝑖3 = 𝜃𝑓𝑖 𝜃.𝑖(0) = 𝑎1𝑖 = 𝜃.0𝑖 𝜃.𝑖(𝑡𝑓𝑖) = 𝑎1𝑖 + 2𝑎2𝑖𝑡𝑓𝑖 + 3𝑎3𝑖𝑡𝑓𝑖2 = 𝜃.𝑓𝑖

Giải hệ phương trình ta thu được:

{ 𝑎0𝑖 = 𝜃0𝑖 𝑎1𝑖 = 𝜃.0𝑖 𝑎2𝑖 = 3 𝑡𝑓𝑖2 (𝜃𝑓𝑖− 𝜃0𝑖) − 2 𝑡𝑓𝑖𝜃.0𝑖 − 1 𝑡𝑓𝑖𝜃.𝑓𝑖 𝑎3𝑖 = − 2 𝑡𝑓𝑖3 (𝜃𝑓𝑖− 𝜃0𝑖) + 1 𝑡𝑓𝑖2 (𝜃.0𝑖 + 𝜃.𝑓𝑖) Trong đó i = 1÷6

Quỹ đạo bậc 3 cho từng biến khớp 𝑞𝑖 là:

𝜃𝑖(𝑡) = 𝜃0𝑖 + 𝜃.0𝑖𝑡 + [3 𝑡𝑓𝑖2 (𝜃𝑓𝑖 − 𝜃0𝑖) − 2 𝑡𝑓𝑖𝜃 . 0𝑖 − 1 𝑡𝑓𝑖𝜃 . 𝑓𝑖] 𝑡2 + [− 2 𝑡𝑓𝑖3 (𝜃𝑓𝑖 − 𝜃0𝑖) + 1 𝑡𝑓𝑖2 (𝜃.0𝑖 + 𝜃.𝑓𝑖)] 𝑡3

Giả sử robot VIPER 650 cần dịch chuyển từ vị trí A(điểm đầu) đến vị trí B(điểm cuối) có tọa độ khớp tương ứng như sau: điểm A(15; 0; 30; 0; 0; 0) và điểm

B(75; 30; 0; 60; 90; 180). (góc ở đây là 𝜃𝑖 – góc quay của khớp, đơn vị ‘độ’.

Từ đó ta xây dựng chương trình MATLAB để mô phỏng quỹ đạo chuyển động, vận tốc và gia tốc của các khớp của robot, do 6 khớp xây dựng phương trình quỹ đạo đa thức bậc 3 giống nhau nên nhóm em xin phép chỉ làm cho khớp 1, với bài toán khớp 1 là chuyển động từ góc 150 đến góc 750 trong vòng 3s và tốc độ tại điểm đầu và điểm cuối bằng 0.

 Phương trình của quỹ đạo cho khớp 1 (i=1) là: Các hệ số phương trình quỹ đạo:

{ 𝑎01= 15 𝑎11 = 0 𝑎21= 3 32(75 − 15) − 2 320 − 1 320 = 20 𝑎31= − 2 33(75 − 15) + 1 32(0 + 0) = −4.44

Khi đó ta có phương trình quỹ đạo vị trí, vận tốc, gia tốc theo thời gian:

{

𝜃1(𝑡) = 15 + 20𝑡2− 4.44𝑡3 𝜃̇1(𝑡) = 40𝑡 − 13.32𝑡2

𝜃̈1(𝑡) = 40 − 26.64𝑡

 Chương trình Matlab (mfile)

Khai báo các điều kiện ban đầu của biến khớp:

% Cac_Dieu_Kien_Dau_Cua_Khop. theta0 = 15;%vi_tri_diem_dau. thetaf = 75;%vi_tri_diem_cuoi. dtheta0 = 0;%van_toc_diem_dau. dthetaf = 0;%van_toc_diem_cuoi. tf = 3;%thoi_gian_di_chuyen.

Với các biến khớp khác khi cần mô phỏng ta chỉ cần thay đổi các điều kiện đầu.

Khai báo các hệ số phương trình:

a0=theta0; a1=dtheta0;

a2=(3/(tf^2))*(thetaf-theta0)-(2/tf)*dtheta0-(1/tf)*dthetaf; a3=(-2/(tf^3))*(thetaf-theta0)+(1/(tf^2))*(dthetaf+dtheta0); t=(0:0.1:tf);

Khai báo các phương trình (quỹ đạo, vận tốc, gia tốc):

% Phuong_Trinh_Quy_Dao. q=a0+a1*t+a2*(t.^2)+a3*(t.^3); % Phuong_Trinh_Van_Toc. dq=a1+2*a2*t+3*a3*t.^2; % Phuong_Trinh_Gia_Toc. ddq=2*a2+6*a3*t; Vẽ đồ thị: sử dụng các hàm để vẽ đồ thị trong matlab: % Do_Thi_Quy_Dao. subplot(3,1,1); plot(t,q,'b','LineWidth',1); title('Quy dao');

ylabel('Position(degree)'); grid on;

% Do_Thi_Van_Toc.

subplot(3,1,2);

plot(t,dq,'r','LineWidth',1); title('Van toc');

ylabel('Velocity(degree/s)'); grid on;

% Do_Thi_Gia_Toc.

plot(t,ddq,'g','LineWidth',1); title('Gia toc');

xlabel('Time(s)');

ylabel('Acceleration(degree/s^2)'); grid on;

Đồ thị quỹ đạo chuyển động bậc 3 của khớp 1:

Hình ảnh mô phỏng của mô hình trên solid work

Đế Thanh nối 1

Thanh nối 2 Thanh nối 3

CHƯƠNG VI. XÂY DỰNG MÔ HÌNH ĐỘNG LỰC HỌC CHO ĐỐITƯỢNG TRÊN TOOLBOX SIMSCAPE/MATLAB

Thanh 4 Thanh 5

Giao diện:

Kéo thả các thanh để nhập góc theta sẽ tính toàn ra Px Py Pz, close: đóng giao diện, reset: đưa robot về tọa độ ban đầu

Chương trình matlab:

function varargout = giaodien(varargin) gui_Singleton = 1;

gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ...

'gui_OpeningFcn', @giaodien_OpeningFcn, ... 'gui_OutputFcn', @giaodien_OutputFcn, ... 'gui_LayoutFcn', [] , ...

'gui_Callback', []); if nargin && ischar(varargin{1})

gui_State.gui_Callback = str2func(varargin{1}); end

if nargout

[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else

gui_mainfcn(gui_State, varargin{:}); end

function giaodien_OpeningFcn(hObject, eventdata, handles, varargin) handles.output = hObject;

% Update handles structure

guidata(hObject, handles);

function varargout = giaodien_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output;

function slider1_Callback(hObject, eventdata, handles)

a1=0.075; a2=0.27; a3=-0.09; d1=-0.335; d4=0.295; d6=0.08; ModelName = 'Assem1';

global var;

theta1=get(handles.slider1,'value'); set(handles.edit1,'string',num2str(theta1));

theta2=get(handles.slider2,'value');

set(handles.edit2,'string',num2str(theta2));

theta3=get(handles.slider3,'value');

set(handles.edit3,'string',num2str(theta3));

theta4=get(handles.slider4,'value');

set(handles.edit4,'string',num2str(theta4));

theta5=get(handles.slider5,'value');

set(handles.edit5,'string',num2str(theta5));

theta6=get(handles.slider6,'value');

set(handles.edit6,'string',num2str(theta6));

set_param([ModelName '/Slider Gain'],'Gain',num2str(theta1)) set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta2)) set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta3)) set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta4)) set_param([ModelName '/Slider Gain4'],'Gain',num2str(theta5)) set_param([ModelName '/Slider Gain5'],'Gain',num2str(theta6))

A1=[cosd(theta1) 0 -sind(theta1) a1*cosd(theta1) ; sind(theta1) 0 cosd(theta1) a1*sind(theta1) ; 0 -1 0 d1 ; 0 0 0 1];

A2=[cosd(theta2) -sind(theta2) 0 a2*cosd(theta2) ; sind(theta2) cosd(theta2) 0 a2*sind(theta2) ; 0 0 1 0 ; 0 0 0 1];

A3=[cosd(theta3) 0 sind(theta3) a3*cosd(theta3) ; sind(theta3) 0 -cosd(theta3) a3*sind(theta3) ; 0 1 0 0 ; 0 0 0 1];

A4=[cosd(theta4) 0 -sind(theta4) 0 ; sind(theta4) 0 cosd(theta4) 0 ; 0 -1 0 d4 ; 0 0 0 1];

A5=[cosd(theta5) 0 sind(theta5) 0 ; sind(theta5) 0 -cosd(theta5) 0 ; 0 1 0 0 ; 0 0 0 1];

A6=[cosd(theta6) -sind(theta6) 0 0 ; sind(theta6) cosd(theta6) 0 0 ; 0 0 1 d6 ; 0 0 0 1]; T=A1*A2*A3*A4*A5*A6; Px=T(1,4); Py=T(2,4); Pz=T(3,4);

set(handles.edit5,'string',num2str(Px)); set(handles.edit6,'string',num2str(Py)); set(handles.edit7,'string',num2str(Pz));

function slider1_CreateFcn(hObject, eventdata, handles)

if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 .9 .9]);

end

function slider2_Callback(hObject, eventdata, handles)

a1=0.075; a2=0.27; a3=-0.09; d1=-0.335; d4=0.295; d6=0.08; ModelName = 'Assem1';

global var;

theta1=get(handles.slider1,'value'); set(handles.edit1,'string',num2str(theta1));

theta2=get(handles.slider2,'value');

set(handles.edit2,'string',num2str(theta2));

theta3=get(handles.slider3,'value');

set(handles.edit3,'string',num2str(theta3));

theta4=get(handles.slider4,'value');

set(handles.edit4,'string',num2str(theta4));

theta5=get(handles.slider5,'value');

theta6=get(handles.slider6,'value');

set(handles.edit6,'string',num2str(theta6));

A1=[cosd(theta1) 0 -sind(theta1) a1*cosd(theta1) ; sind(theta1) 0 cosd(theta1) a1*sind(theta1) ; 0 -1 0 d1 ; 0 0 0 1];

A2=[cosd(theta2) -sind(theta2) 0 a2*cosd(theta2) ; sind(theta2) cosd(theta2) 0 a2*sind(theta2) ; 0 0 1 0 ; 0 0 0 1];

A3=[cosd(theta3) 0 sind(theta3) a3*cosd(theta3) ; sind(theta3) 0 -cosd(theta3) a3*sind(theta3) ; 0 1 0 0 ; 0 0 0 1];

A4=[cosd(theta4) 0 -sind(theta4) 0 ; sind(theta4) 0 cosd(theta4) 0 ; 0 -1 0 d4 ; 0 0 0 1];

A5=[cosd(theta5) 0 sind(theta5) 0 ; sind(theta5) 0 -cosd(theta5) 0 ; 0 1 0 0 ; 0 0 0 1];

A6=[cosd(theta6) -sind(theta6) 0 0 ; sind(theta6) cosd(theta6) 0 0 ; 0 0 1 d6 ; 0 0 0 1]; T=A1*A2*A3*A4*A5*A6; Px=T(1,4); Py=T(2,4); Pz=T(3,4);

set(handles.edit7,'string',num2str(Px)); set(handles.edit8,'string',num2str(Py)); set(handles.edit9,'string',num2str(Pz));

function slider2_CreateFcn(hObject, eventdata, handles)

if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 .9 .9]);

end

function slider3_Callback(hObject, eventdata, handles)

a1=0.075; a2=0.27; a3=-0.09; d1=-0.335; d4=0.295; d6=0.08; ModelName = 'Assem1';

global var;

theta1=get(handles.slider1,'value'); set(handles.edit1,'string',num2str(theta1));

theta2=get(handles.slider2,'value');

theta3=get(handles.slider3,'value');

set(handles.edit3,'string',num2str(theta3));

theta4=get(handles.slider4,'value');

set(handles.edit4,'string',num2str(theta4));

theta5=get(handles.slider5,'value');

set(handles.edit5,'string',num2str(theta5));

theta6=get(handles.slider6,'value');

set(handles.edit6,'string',num2str(theta6));

A1=[cosd(theta1) 0 -sind(theta1) a1*cosd(theta1) ; sind(theta1) 0 cosd(theta1) a1*sind(theta1) ; 0 -1 0 d1 ; 0 0 0 1];

A2=[cosd(theta2) -sind(theta2) 0 a2*cosd(theta2) ; sind(theta2) cosd(theta2) 0 a2*sind(theta2) ; 0 0 1 0 ; 0 0 0 1];

A3=[cosd(theta3) 0 sind(theta3) a3*cosd(theta3) ; sind(theta3) 0 -cosd(theta3) a3*sind(theta3) ; 0 1 0 0 ; 0 0 0 1];

A4=[cosd(theta4) 0 -sind(theta4) 0 ; sind(theta4) 0 cosd(theta4) 0 ; 0 -1 0 d4 ; 0 0 0 1];

A5=[cosd(theta5) 0 sind(theta5) 0 ; sind(theta5) 0 -cosd(theta5) 0 ; 0 1 0 0 ; 0 0 0 1];

A6=[cosd(theta6) -sind(theta6) 0 0 ; sind(theta6) cosd(theta6) 0 0 ; 0 0 1 d6 ; 0 0 0 1]; T=A1*A2*A3*A4*A5*A6; Px=T(1,4); Py=T(2,4); Pz=T(3,4);

set(handles.edit7,'string',num2str(Px)); set(handles.edit8,'string',num2str(Py)); set(handles.edit9,'string',num2str(Pz));

function slider3_CreateFcn(hObject, eventdata, handles)

if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 .9 .9]);

function slider4_Callback(hObject, eventdata, handles)

a1=0.075; a2=0.27; a3=-0.09; d1=-0.335; d4=0.295; d6=0.08; ModelName = 'Assem1';

global var;

theta1=get(handles.slider1,'value'); set(handles.edit1,'string',num2str(theta1));

theta2=get(handles.slider2,'value');

set(handles.edit2,'string',num2str(theta2));

theta3=get(handles.slider3,'value');

set(handles.edit3,'string',num2str(theta3));

theta4=get(handles.slider4,'value');

set(handles.edit4,'string',num2str(theta4));

theta5=get(handles.slider5,'value');

set(handles.edit5,'string',num2str(theta5));

theta6=get(handles.slider6,'value');

set(handles.edit6,'string',num2str(theta6));

A1=[cosd(theta1) 0 -sind(theta1) a1*cosd(theta1) ; sind(theta1) 0 cosd(theta1) a1*sind(theta1) ; 0 -1 0 d1 ; 0 0 0 1];

A2=[cosd(theta2) -sind(theta2) 0 a2*cosd(theta2) ; sind(theta2) cosd(theta2) 0 a2*sind(theta2) ; 0 0 1 0 ; 0 0 0 1];

A3=[cosd(theta3) 0 sind(theta3) a3*cosd(theta3) ; sind(theta3) 0 -cosd(theta3) a3*sind(theta3) ; 0 1 0 0 ; 0 0 0 1];

A4=[cosd(theta4) 0 -sind(theta4) 0 ; sind(theta4) 0 cosd(theta4) 0 ; 0 -1 0 d4 ; 0 0 0 1];

A5=[cosd(theta5) 0 sind(theta5) 0 ; sind(theta5) 0 -cosd(theta5) 0 ; 0 1 0 0 ; 0 0 0 1];

A6=[cosd(theta6) -sind(theta6) 0 0 ; sind(theta6) cosd(theta6) 0 0 ; 0 0 1 d6 ; 0 0 0 1];

T=A1*A2*A3*A4*A5*A6; Px=T(1,4);

Py=T(2,4); Pz=T(3,4);

set(handles.edit7,'string',num2str(Px)); set(handles.edit8,'string',num2str(Py)); set(handles.edit9,'string',num2str(Pz));

function slider4_CreateFcn(hObject, eventdata, handles)

if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 .9 .9]);

end

function slider5_Callback(hObject, eventdata, handles)

a1=0.075; a2=0.27; a3=-0.09; d1=-0.335; d4=0.295; d6=0.08; ModelName = 'Assem1';

global var;

theta1=get(handles.slider1,'value'); set(handles.edit1,'string',num2str(theta1));

theta2=get(handles.slider2,'value');

set(handles.edit2,'string',num2str(theta2));

theta3=get(handles.slider3,'value');

set(handles.edit3,'string',num2str(theta3));

theta4=get(handles.slider4,'value');

set(handles.edit4,'string',num2str(theta4));

theta5=get(handles.slider5,'value');

set(handles.edit5,'string',num2str(theta5));

theta6=get(handles.slider6,'value');

set(handles.edit6,'string',num2str(theta6));

A1=[cosd(theta1) 0 -sind(theta1) a1*cosd(theta1) ; sind(theta1) 0 cosd(theta1) a1*sind(theta1) ; 0 -1 0 d1 ; 0 0 0 1];

A2=[cosd(theta2) -sind(theta2) 0 a2*cosd(theta2) ; sind(theta2) cosd(theta2) 0 a2*sind(theta2) ; 0 0 1 0 ; 0 0 0 1];

A3=[cosd(theta3) 0 sind(theta3) a3*cosd(theta3) ; sind(theta3) 0 -cosd(theta3) a3*sind(theta3) ; 0 1 0 0 ; 0 0 0 1];

A4=[cosd(theta4) 0 -sind(theta4) 0 ; sind(theta4) 0 cosd(theta4) 0 ; 0 -1 0 d4 ; 0 0 0 1];

A5=[cosd(theta5) 0 sind(theta5) 0 ; sind(theta5) 0 -cosd(theta5) 0 ; 0 1 0 0 ; 0 0 0 1];

A6=[cosd(theta6) -sind(theta6) 0 0 ; sind(theta6) cosd(theta6) 0 0 ; 0 0 1 d6 ; 0 0 0 1]; T=A1*A2*A3*A4*A5*A6; Px=T(1,4); Py=T(2,4); Pz=T(3,4);

set(handles.edit7,'string',num2str(Px)); set(handles.edit8,'string',num2str(Py)); set(handles.edit9,'string',num2str(Pz));

function slider5_CreateFcn(hObject, eventdata, handles)

if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 .9 .9]);

end

function slider6_Callback(hObject, eventdata, handles)

a1=0.075; a2=0.27; a3=-0.09; d1=-0.335; d4=0.295; d6=0.08; ModelName = 'Assem1';

global var;

theta1=get(handles.slider1,'value'); set(handles.edit1,'string',num2str(theta1));

theta2=get(handles.slider2,'value');

set(handles.edit2,'string',num2str(theta2));

theta3=get(handles.slider3,'value');

set(handles.edit3,'string',num2str(theta3));

theta4=get(handles.slider4,'value');

set(handles.edit4,'string',num2str(theta4));

theta5=get(handles.slider5,'value');

set(handles.edit5,'string',num2str(theta5));

theta6=get(handles.slider6,'value');

set(handles.edit6,'string',num2str(theta6));

A1=[cosd(theta1) 0 -sind(theta1) a1*cosd(theta1) ; sind(theta1) 0 cosd(theta1) a1*sind(theta1) ; 0 -1 0 d1 ; 0 0 0 1];

A2=[cosd(theta2) -sind(theta2) 0 a2*cosd(theta2) ; sind(theta2) cosd(theta2) 0 a2*sind(theta2) ; 0 0 1 0 ; 0 0 0 1];

A3=[cosd(theta3) 0 sind(theta3) a3*cosd(theta3) ; sind(theta3) 0 -cosd(theta3) a3*sind(theta3) ; 0 1 0 0 ; 0 0 0 1];

A4=[cosd(theta4) 0 -sind(theta4) 0 ; sind(theta4) 0 cosd(theta4) 0 ; 0 -1 0 d4 ; 0 0 0 1];

A5=[cosd(theta5) 0 sind(theta5) 0 ; sind(theta5) 0 -cosd(theta5) 0 ; 0 1 0 0 ; 0 0 0 1];

A6=[cosd(theta6) -sind(theta6) 0 0 ; sind(theta6) cosd(theta6) 0 0 ; 0 0 1 d6 ; 0 0 0 1]; T=A1*A2*A3*A4*A5*A6; Px=T(1,4); Py=T(2,4); Pz=T(3,4);

set(handles.edit7,'string',num2str(Px)); set(handles.edit8,'string',num2str(Py)); set(handles.edit9,'string',num2str(Pz));

function slider6_CreateFcn(hObject, eventdata, handles)

if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 .9 .9]);

end

function edit1_Callback(hObject, eventdata, handles) function edit1_CreateFcn(hObject, eventdata, handles)