Đáp án thực hành robot di động (code matlap)

Nội dung thực hành robot di động trên phần mềm Matlab
Bài 1: Cho Robot di động dạng vi sai: a =20cm; r=3cm;.
a) Tính vận tốc dài và vận tốc góc tại thời điểm phi=30độ và vận tốc góc của 2 bánh
xe: theta_r=3 rad/s, theta_l=2rad/s;
Vận tốc dài:
Vận tốc góc:
b) Tính vận tốc góc của hai bánh xe tại thời điểm phi=40 độ, xQ_cham=4cm/s;
yQ_cham=3cm/s, phi_cham=10rad/s.

function [vQ,phi_cham] =
dong_hoc_thuan_mobile_robot_visai(a,r,theta_r,theta_l,phi)
%%== Dong hoc thuan
J=[r*cos(phi)/2 r*cos(phi)/2;r*sin(phi)/2 r*sin(phi)/2;r/
(2*a) -r/(2*a)];
theta=[theta_r;theta_l];
p_cham=J*theta;
vQ=sqrt(p_cham(1,:)^2+p_cham(2,:)^2)
phi_cham=p_cham(3,:)
end
function [q_cham]=dong_hoc_nghich_mobile_robot_visai(a,r,
xQ_cham, yQ_cham, phi_cham, phi)
%%=== Dong hoc nghich
p_cham=[xQ_cham;yQ_cham;phi_cham];
J=[r*cos(phi)/2 r*cos(phi)/2;r*sin(phi)/2 r*sin(phi)/2;r/
(2*a) -r/(2*a)];
J_s=((J'*J)^-1)*J';
q_cham=J_s*p_cham;
theta_r=q_cham(1,:);
theta_l=q_cham(2,:);
end
Bài 2: Bài tập: Có ba con đường khác nhau đều dẫn đến mục tiêu. Các robot di động

chọn đường dẫn đầu tiên 7 lần trong 10 trường hợp, đường dẫn thứ 2 chỉ 1 trong 10
trường hợp, đường dẫn thứ 3 chỉ là 1 trong 5 trường hợp. Có lần lượt 5%, 10%, 8%
khả năng gặp chướng ngại vật trên con đường thứ nhất, thứ 2 và thứ 3.
a)Xác định xác suất để robot gặp chướng ngại vật khi đi về mục tiêu
b) Nếu robot gặp chướng ngại vật thì xác suất xảy ra trên con đường thứ nhất là bao
nhiêu?
% p(A) = [P(A1), P(A2), P(A3)]
p_A = [0.7 0.1 0.2]
% p(B|A) = [P(B|A1), P(B|A2), P(B|A3)]


p_BA = [0.05 0.1 0.08]
P_B = p_BA*p_A'
% path: p(A|B) = [P(A1|B), P(A2|B), P(A3|B)]
p_AB = (p_BA.*p_A)./P_B

Bài 3: Phân rã ô theo phương pháp Triangulation là một phương pháp mà môi trường
được tách thành các ơ hình tam giác.
% vat can
o = 1000*[0.0149, 0.0693; ...
1.6228, 0.0679; ...
1.6241, 1.0867; ...
0.0112, 1.0854];
o1 = 1000*[0.4263, 0.4569; ...
0.6144, 0.6857; ...
0.3097, 0.9414; ...
0.1190, 0.7126];
o2 = 1000*[0.8151, 0.2079; ...
1.0885, 0.3008; ...
0.9644, 0.6574; ...

0.8753, 0.6278; ...
0.9706, 0.3573; ...
0.7838, 0.2927];
o3 = 1000*[1.3319, 0.4865; ...
1.4723, 0.5659; ...
1.3845, 0.7112];
obstacles = {flipud(o), o1, o2, o3};
% tach thanh tam giac
points = cell2mat(obstacles(:));
dt = delaunayTriangulation(points);
triplot(dt, 'b-'); axis equal tight;
Bài 2: Động lực học Lagrange robot di động dạng vi sai:

Động năng:
I
K = m / 2( x 2 + y 2 ) + φ 2
2

Giả thiết robot di chuyển trên mặt phẳng do đó thế năng:
I 2
φ
2
P =0;

L = K − P1 = m / 2( x 2 + y 2 ) +

Ma sát được bỏ qua

P = 0.

=>

d  ∂L 
d  ∂L 
d  ∂L 
  = mx;   = my;    = Iφ
dt  ∂x 
dt  ∂y 
dt  ∂φ 

và

∂L
∂L
∂L
= 0;
= 0;
=0
∂x
∂y
∂φ

1
mx − λ1 sin φ − (τ r + τ l ) cos φ = 0
r
1
=> my + λ2 cos φ − r (τ r + τ l ) sin φ = 0
2a
Iφ −

(τ r − τ l ) = 0
r
 + M T (q)λ = Eτ
D(q)q
 xQ 
m 0 0
cos φ
τ r 
1




q =  yQ  ; τ =   ; D(q) =  0 m 0 ; E =  sin φ
r
τ l 
 φ 
 0 0 I 
 2a
D(q) v + C(q, q ) v + g (q) = Eτ

cos φ 
sin φ  ; M (q) = [ − sin φ
− 2a 

cos φ

0]

1 

m 0
11
D = B T DB = 
E= 
;


r  2a − 2a 
0 I

=> Phương trình động lực học robot di động kiểu vi sai:
1

1  τ r  v = mr (τ r + τ l )
m 0  v1  1  1
 0 I  v  = 2a − 2a  τ  ⇒ 

 2  r 
  l  ω = 2a (τ r − τ l )
Ir


Yêu cầu: Sử dụng matlab tính tốn mơ men xoắn cần thiết để robot di động kiểu vi sai
di dọc theo quỹ đạo tham chiếu: xr=1.2+0.6sin(2pi/30*t), yr=0.8+0.7sin(4pi/30*t).
Thông số của robot: m=0.75kg, I=0.001kgm2, a=0.075m, r=0.025m.
Ts = 0.033;
t = 0:Ts:30;
freq=2*pi/30;
xR= 1.2 + 0.6*sin(freq*t); yR = 0.8 + 0.7*sin(2*freq*t);
dxR = freq*0.6*cos(freq*t); dyR = 2*freq*0.7*cos(2*freq*t);

ddxR =-freq^2*0.6*sin(freq*t); ddyR =-4*freq^2*0.7*sin(2*freq*t);
dddxR =-freq^3*0.7*cos(freq*t); dddyR =8*freq^3*0.7*cos(2*freq*t);
qR = [xR; yR; atan2(dyR, dxR)]
vR = sqrt(dxR.^2+dyR.^2);
wR = (dxR.*ddyR-dyR.*ddxR)./(dxR.^2+dyR.^2);
dvR = (dxR.*ddxR+dyR.*ddyR)./vR;
dwR = (dxR.*dddyR-dyR.*dddxR)./vR.^2 - 2.*wR.*dvR./vR;
q = [qR(:,1); vR(1); wR(2)];
m = 0.75; J = 0.001;a = 0.075; r = 0.025;
for k = 1:length(t)
v = vR(k); w = wR (k); dv = dvR(k); dw = dwR(k);
tongtau=m*r*dvR;
hieutau=dwR*I*r/(2*a);
tau = [(tongtau+hieutau)/2; (tongtau-hieutau)/2]
end


///////////////////////////////////////////////////////////////////////////////////////////////////////////
NỘI DUNG SINH VIÊN TỰ CODE
Câu 1.: Cho mơ hình robot di động bánh xe vi sai như trên hình 1 có: 2a là chiều rộng
của robot; b là khoảng cách từ điểm giữa hai bánh xe Q đến trọng tâm G của robot; φ là
góc định hướng của robot; θ&r , θ&
l lần lượt là vận tốc góc của bánh xe bên phải và bên
trái; vr , vl lần lượt là vận tốc dài của bánh xe bên phải và bên trái; vQ là vận tốc của
robot; hai bánh xe chủ động có bán kính r .

Hình 1
Giả thiết robot di chuyển trên mặt phẳng xoy, các bánh xe của robot lăn không trượt trên
mặt phẳng chuyển động, vận tốc vr > vl . Cho a =b=20cm; r=3cm;. Yêu cầu:
a) Thiết lập phương trình động học thuận của robot.

b) Tính vận tốc dài và vận tốc góc tại thời điểm phi=30độ và vận tốc góc của 2 bánh xe:
theta_r=3 rad/s, theta_l=2rad/s;
c) Xác định khoảng cách từ tâm quay tức thời tới trọng tâm G của robot.
d) Tính vận tốc góc của hai bánh xe tại thời điểm phi=40 độ, xQ_cham=4cm/s;
yQ_cham=3cm/s, phi_cham=10rad/s.
e) Tính tốn mơ men xoắn cần thiết để robot di động kiểu vi sai di dọc theo quỹ đạo
tham chiếu: xr=1.2+0.6sin(2pi/30*t), yr=0.8+0.7sin(4pi/30*t). Thông số của robot:
m=0.75kg, I=0.001kgm2.

HƯỚNG DẪN BÁO CÁO
Câu 1:
a)Viết phương trình động học thuận của robot:
b) Code matlab
function[vQ,phi_cham]=dong_hoc_thuan_mobile_robot_visai(a,r,theta_r,theta_l,phi)
a= 0.2;
r=0.03 ;
theta_r=3 ;
theta_l=2;
phi= pi/6
%%== Dong hoc thuan
J=[r*cos(phi)/2 r*cos(phi)/2;r*sin(phi)/2 r*sin(phi)/2;r/(2*a) -r/(2*a)];
theta=[theta_r;theta_l];
p_cham=J*theta;
vQ=sqrt(p_cham(1,:)^2+p_cham(2,:)^2)
phi_cham=p_cham(3,:)
end
Kết quả tính tốn được
>> bai_1b

phi =
0.5236


vQ =
0.0750

phi_cham =
0.0750

ans =
0.0750
c)
Code matlab
theta_r=3;
theta_l=2;
a=0.2;
b=0.2;
R=a*((theta_r+theta_l)/(theta_r-theta_l));
IccG=sqrt(R^2+b^2)
Kết quả tính tốn được
>> khoang_cach
IccG =
1.0198
d)
Code matlab
a=0.2;
r=0.03;
xQ_cham=0.04;
yQ_cham=0.03;

phi_cham=10;
phi=2*pi/9


p_cham=[xQ_cham;yQ_cham;phi_cham];
J=[r*cos(phi)/2 r*cos(phi)/2;r*sin(phi)/2 r*sin(phi)/2;r/(2*a) -r/(2*a)];
J_s=((J'*J)^-1)*J';
q_cham=J_s*p_cham;
theta_r=q_cham(1,:)
theta_l=q_cham(2,:)
Kết quả tính tốn được
>> bai_1d
phi =
0.6981

theta_r =
68.3308

theta_l =
-65.0025
>>
e)
Phương trình động lực học robot

Code matlab
Ts=0.034;
t=0:Ts:30;
freq=2*pi/30;
xR=1.2+0.6*sin(freq*t);yR=0.8+0.7*sin(2*freq*t);
dxR=freq*0.6*cos(freq*t);dyR=freq*0.7*2*cos(2*freq*t);

ddxR=-freq^2*0.6*sin(freq*t);ddyR=-4*freq^2*0.7*sin(2*freq*t);
dddxR=-freq^3*0.6*cos(freq*t);dddyR=-8*freq^3*0.7*cos(2*freq*t);
qR=[xR;yR;atan2(dyR,dxR)]
vR=sqrt(dxR.^2+dyR.^2);
wR=(dxR.*ddyR+dyR.*ddyR)./(dxR.^2+dyR.^2);
dvR=(dxR.*ddxR+dyR.*ddyR)./vR;
dwR=(dxR.*dddyR-dyR.*dddxR)./vR.^2-2.*wR.*dvR./vR;
q=[qR(:,1);vR(1);wR(2)];
m=0.75;I=0.001;a=0.2;r=0.03;
for k=1:length(t)
v=vR(k);w=wR(k);dv=dvR(k);dw=dwR(k);
tongtau=m*r*dvR;
hieutau=dwR*I*r/(2*a);
tau=[(tongtau+hieutau)/2;(tongtau-hieutau)/2]
end
Kết quả tính tốn được
Columns 793 through 798
0.0009 0.0009 0.0009 0.0010 0.0010 0.0010
0.0010 0.0011 0.0011 0.0011 0.0011 0.0011
///////////////////////////////
Câu 2. Cho mơ hình robot di động ba bánh dẫn động bánh trước như trên hình 1 có: φ là
góc định hướng của robot, ψ là góc lái của robot; θ&w = ωw là vận tốc góc bánh lái; 2a là
chiều rộng robot; D là khoảng cách giữa bánh xe trước và bánh xe sau của robot; bánh
lái có bán kính r .

Hình 1

Giả thiết robot di chuyển trên mặt phẳng xoy, các bánh xe của robot lăn không trượt trên
mặt phẳng chuyển động. Cho: D = 0, 2m ; ψ = 30° . Yêu cầu:
a) Thiết lập phương trình động học thuận của robot.
b) Tính vận tốc dài và vận tốc góc tại thời bất kỳ (SV tự cho số liệu)
c) Xác định khoảng cách R từ tâm quay tức thời ICC tới điểm giữa hai bánh sau Q của
robot.
d) Tính tốn mô men xoắn cần thiết để robot di động di chuyển theo quỹ đạo tham
chiếu: xr=1.2+0.6sin(2pi/30*t), yr=0.8+0.7sin(4pi/30*t). Thông số của robot: m=0.75kg,
I=0.001kgm2.
HƯỚNG DẪN BÁO CÁO
a) Viết phương trình động học thuận của robot:

b)
Code matlab
function [vQ,phi_cham] = dong_hoc_thuan_mobile_robot_3banh(a,r,D,psi)
a=0.3;
r=0.02;
D=0.2;
psi=30*pi/180;
theta_cham=psi;
%%== Dong hoc thuan
J=[r*cos(psi)*cos(theta_cham) 0; r*cos(psi)*sin(theta_cham) 0; (r/D)*(sin(psi)) 0; 0 1;];
Vw=r*theta_cham;
v=Vw*cos(psi)
phi_cham=J(3,:)*theta_cham
end
Kết quả tính tốn được
c)
Code matlab
%%== khoang cach

R=D*tan(pi/2-psi)
Kết quả tính tốn được


d)
Phương trình động lực học robot
Code matlab
%% dong luc hoc
Ts=0.04;
t=0:Ts:30;
fe=2*pi/30;
xR=1.2+0.6*sin(fe*t);yR=0.8+0.7*sin(2*fe*t);
vR=sqrt(diff(xR).^2+diff(yR).^2);
dvR=diff(vR);
m=0.75;I=0.001;
for k=1:length(t)
tau=2*a*r*m*dvR
end
Kết quả tính tốn được