đồ án Điều khiển robot

Đề bài
Cho một cơ cấu Robot 2 thanh nối đợc truyền động bởi động cơ một chiều.
Động cơ một chiều đợc cấp điện từ một bộ khuếch đại điện áp.
1. Xây dựng phơng trình động học thuận và ngợc biểu diễn mối quan hệ
giữa hệ toạ độ tay Robot (End effector) và hệ toạ độ các khớp (Joints).
2. Xây dựng quan hệ giữa tốc độ của các khớp và tốc độ của tay Robot.
3. Viết hàm MATLAB thực hiện các phơng trình khi tay Robot di chuyển
từ vị trí [0,4: 0,0 m] đến [0,0: 0,4 m] theo một đờng thẳng. Đồ thị tốc
độ đặt trớc của tay Robot dọc theo quỹ đạo cho ở hình 2.
4. Thiết kế bộ điều khiển bù trọng lực cho Robot.
5. Mô phỏng hệ thống với bộ điều khiển ở câu 4 khi góc quay của khớp 1
thay đổi nhảy cấp từ 0 đến 1 rad.

Hình 1. Cơ cấu động học robot hai thanh nối.
Hình 2. Đồ thị tốc độ.
1
2
l
2g
l
1
l
1g
l
2

1

2
x
2

y
y
x
z
1
m
2
m
1
J
2
J
0 0.25 0.75 1.0 t(s)
V (m/s)
Bảng thông số của Robot:
STT Đại lợng Giá trị
1 Chiều dài thanh nối1 (l
1
) 0,4m
2 Chiều dài thanh nối1 (l
2
) 0,3m
3 Khối lợng thanh nối 1 (m
1
) 10Kg
4 Khối lợng thanh nối 2 (m
2
) 5,0Kg
5 Momen quán tính khớp 1 quay quanh tâm khối (J
1

) 0,528 Kgm
2
6 Momen quán tính khớp 2 quay quanh tâm khối (J
2
) 0,19 Kgm
2
7 Khoảng cách từ khớp 1 đến tâm khối 1 (l
g1
) 0,25 m
8 Khoảng cách từ khớp 2 đến tâm khối 2 (l
g2
) 0,15 m
9 Hằng số momen của động cơ khớp 1, 2 (K
M
) 0,5 Nm/A
10 Điện trở phần ứng (r
1
,r
2
)
3
11 Momen lớn nhất của động cơ khớp 1 (M
1max
) 1,2 Nm
12 Momen lớn nhất của động cơ khớp 2 (M
2max
) 0,7 Nm
13
Tốc độ lớn nhất của động cơ khớp 1, 2 (
max

)
90 rad/s
14 Momen quán tính của động cơ (J
Đ
) 0,0004 Kgm
2
15 Khối lợng tải lớn nhất (m
t
) 5 Kg
16 Tỉ số truyền cho cả hai khớp (i) 12
Bài giải
Câu 1. Xây dựng ph ơng trình động học thuận và ng ợc biểu
diễn mối quan hệ giữa hệ toạ độ tay Robot (End effector)
và hệ toạ độ các khớp (Joints)


2
2
l
2g
l
1
l
1g
l
2

1

2

x
2
y
y
x
z
1
m
2
m
1
J
2
J
a. Xây dựng phơng trình động học thuận
Bài toán động học thuận là bài toán đi tìm các thông số về vị trí và hớng của
tay robot so với khung cơ sở (gốc).
Thực hiện phép biến đổi đồng nhất dựa trên hệ tọa độ thanh nối (Denavit -
Hartenberg) mối quan hệ giữa hệ toạ độ tay Robot (End effector) và hệ toạ độ
các khớp (Joints) đợc xác định nh sau:
2
1
1
0
2
0
n
0
AATT ==
(1)

Với:
n
0
T
: biểu diễn hệ toạ độ thanh thứ n so với hệ toạ độ gốc.
E
n
T
: biểu diễn hệ toạ độ tay (điểm kẹp) so với thanh n.












=
1000
paon
paon
paon
T
zzzz
yyyy
xxxx

E
0
i
i
A
1
: đợc định nghĩa là ma trận chuyển đổi đồng nhất, biểu diễn mối
quan hệ vị trí của một điểm trong khung i và vị trí của điểm đó trong
khung thứ i-1.
Từ mô hình cơ cấu động học của robot ta xây dựng đợc bảng Denavit -
Hartenberg nh sau:
Thanh a
i

i

i
d
i
1
2
l
1
l
2
/2
/2

1

2
0
0
Theo đó ta xác định đợc các ma trận biến đổi A của robot nh sau:













=
1000
dcs0
sascccs
casscsc
A
iii
iiiiiii
iiiiiii
i
-1i

=
1000
0100
sl0cs
cl0sc
A
1111
1111
1
0
3

=
1000
0100
sl0cs
cl0sc
A
2222
2222
2
1
( )

sins;cosc ==
Thay vào (1) ta đợc:
( ) ( ) ( )
( ) ( ) ( )

=












++++
++++
=
1000
paon
paon
paon

1000
0100
slsl0cs

clcl0sc
T
zzzz
yyyy
x
ã
xxx
212112121
212112121
E
0
(2)
Với :
( ) ( )
( ) ( )
2121
2121
sins
cosc
+=+
+=+
P là điểm thuộc hệ toạ độ gắn với tay robot, có vị trí đợc xác định bằng
vectơ cột thứ t của
E
0
T
.
Từ (2) ta đợc phơng trình động học thuận biểu diễn mối quan hệ giữa hệ tọa
độ tay Robot và hệ tọa độ các khớp:

=
++=
++=
0z
)sin(lsinly
)cos(lcoslx
T
21211T
21211T
(3)
b. Xây dựng phơng trình động học ngợc
Bài toán động học ngợc là tìm vị trí các khớp khi biết vị trí của tay robot
(End-effector).
Ma trận biểu diễn vị trí tay robot:
4

=
1000
paon
paon
paon
T
zzzz
yyyy
x
ã
xxx
E
0
Theo (1) ta có:
2
1
1
0
E
0
AAT =
Nhân hai vế của (1) với
1
1

A
ta có:
( )
2

1
E
0
1
1
0
ATA =ì

(4)
tơng đơng với:













=

1000
0100
sl0cs
cl0sc
1000
0fff
0fff
0fff
2222
2222
333231
232221
131211


(5)
trong đó:
( )

ì












=

=ì

1000
paon
paon
paon
1000
0100
00cs-
l-0sc
1000
ffff
ffff
ffff
TA
zzzz
yyyy
x
ã
xxx
11
111
34333231
24232221
14131211
E
0
1

1
0












++++
++++
=
1000
paon
cpspcasacosocnsn
lspcpsacasocosncn
zzzz
1y1x1y1x1y1x1y1x
1y1xy1xy1x1y1x


Cân bằng vế trái và phải của (5), ta có:

=
=
2224
2214
slf
clf

5



=+
+=+

221y1x
2211y1x
slcpsp
cllspcp
(6)
Với





=
=
=+
y
x

2
2
2
2
py
px
1sc

Bình phơng hai vế của hai phơng trình trong (6) và cộng vế ta có:
21
2
2
2
1
22
2
l2l
)l(l)y(x
c
++
=
[ ]
)l(l)y(x
)l(l)y(x2)lly(x
tg
2
2
2
1
22

4
2
4
1
22222
2
2
1
22
2
++
++++++
=
(7)
Đặt :
[ ]





++=
++++++=
)l(l)y(xk
)l(l)y(x2)lly(xk
2
2
2
1
22

2
4
2
4
1
22222
2
2
1
22
1

ta có:
)k,atan2(k
212
=
(8)
Thay (7) vào (6) ta đợc:
1
2
2
2
1
22
11
2l
llxy
ysxc
++
=+

Giải ra ta có:

)k,atan2(kx)atan2(y,
311
=
(9)
trong đó:
)l(l)y(xk
2
2
2
1
22
3
++=
.
Vậy phơng trình động học ngợc của hệ đã cho đợc biểu diễn dới dạng



=
=
),(2tana
),(2tana)x,y(2tana
212
311


(10)
Với :

6
[ ]







++=
++=
++++++=
)l(l)y(xk
)l(l)y(xk
)l(l)y(x2)lly(xk
2
2
2
1
22
3
2
2
2
1
22
2
4
2
4

1
22222
2
2
1
22
1
Câu 2: Xây dựng quan hệ giữa tốc độ các khớp và tốc độ của
tay Robot
Đạo hàm hai vế phơng trình động học thuận theo thời gian ta có:





+++=
++=
)cos()l(cosly
)sin()l(sinlx
21221111
21221111




hay

+++
++
=






2
1
21221211
21221211


)cos(l)cos(lcosl
)sin(l)sin(lsinl
y
x

(11)
Đặt:






+++
++
=
)cos(l)cos(lcosl
)sin(l)sin(lsinl
J
21221211
21221211

(J: ma trận Jacobien)
Phơng trình biểu diễn quan hệ giữa tốc độ các khớp và tốc độ của tay Robot
đợc biểu diễn dới dạng:






=

y
x
J


1
2
1




(12)
Với:
(J)det
)(sinlsinl)(coslcosl
)(sinl)(cosl
J
2121121211
212212
1

++
++
=

( ) ( ) ( ) ( ) ( )
1121211212
coslsinlsinlcoslJdet

+++=
7
Câu 3. Viết hàm MATLAB thực hiện các ph ơng trình ở câu 1 và
2 và vẽ đ ờng biểu diễn vị trí và tốc độ khớp khi tay Robot di
chuyển từ vị trí [0,4:0,0] đến [0,0:0,4] theo một đ ờng thẳng. Đồ
thị tốc độ của tay Robot đã cho nh hình 2
a. Chơng trình tính toán động học thuận
Dựa vào các công thức tính toán động học thuận (3) ta xây dựng đợc hàm
tính toán động học thuận cho robot nh sau:
function xy=Thuan(l1,l2,d1,d2)
% Chuyen doi gia tri goc tu do ra radian
r_d1=d1*2*pi/360;
r_d2=d2*2*pi/360;
%Tinh toan
xy(1)=l1*cos(r_d1)+l2*cos(r_d1+r_d2); % Vi tri tay theo truc x
xy(2)=l1*sin(r_d1)+l2*sin(r_d1+r_d2); % Vi tri tay theo truc y
xy(3)=0; % Vi tri tay theo truc z
b. Chơng trình tính toán động học ngợc
Dựa vào hệ phơng trình tính động học ngợc (10) ta có hàm tính toán động

học ngợc robot nh sau:
function d=Nguoc(x,y,l1,l2)
k1=sqrt((x^2+y^2+l1^2+l2^2)^2-2*((x^2+y^2)^2+l1^4+l2^4));
k2=(x^2+y^2)-(l1^2+l2^2);
k3=(x^2+y^2)+(l1^2-l2^2);
d1=atan2(y,x)-atan2(k1,k3); %Goc theta 1 tinh theo radian.
d2=atan2(k1,k2); %Goc theta 2 tinh theo radian.
d(1)=d1*360/(2*pi) %Goc theta 1 tinh theo do.
d(2)=d2*360/(2*pi) %Goc theta 2 tinh theo do.
c. Chơng trình tính tốc độ quay khớp
Dựa vào phơng trình biểu diễn quan hệ giữa tốc độ các khớp và tốc độ của
tay robot ta xây dựng đợc hàm tính toán tốc độ quay của các khớp nh sau:
% Chuong trinh tinh toc do quay khop
function vd=tocdoquay(vx,vy,d1,d2,l1,l2)
% Chuyen doi d1,d2 tu do() sang radian()
r_d1=d1*2*pi/360;
r_d2=d2*2*pi/360;
J=[-l1*sin(d1)-l2*sin(d1+d2),-l2*sin(d1+d2);
l1*cos(d1)+l2*cos(d1+d2),l2*cos(d1+d2)]
% Ma tran Jacobien
8
v=[vx;vy]; % Vecto van toc theo truc x va y.
vd=inv(J)*v; % Van toc goc cua khop 1 va 2 tinh theo radian/s
d. Vẽ đờng biểu diễn vị trí và tốc độ khớp khi tay Robot di chuyển từ vị trí
[0,4:0,0 m] đến [0,0:0,4 m] theo một đờng thẳng
Dựa vào dạng đồ thị tốc độ ở hình 2 ta có quỹ đạo chuyển động của tay
robot là quỹ đạo 2-1-2. Mặt khác tay Robot di chuyển từ vị trí [0,4: 0,0 m] đến
[0,0: 0,4 m] theo một đờng thẳng thực chất là chuyển động tịnh tiến theo cả hai
trục x và y.
Xét khoảng thời gian từ 0ữ0,25s, tay Robot di chuyển theo quỹ đạo bậc 2:

+=
++=
11121
1011
2
121
at2a(t)x
atata(t)x




+=
++=
11121
1011
2
121
bt2b(t)y
btbtb(t)y

Các điều kiện biên gồm:
ymax
xmax
V(0,25)y
0)0(y
0)0(y

V(0,25)x
0)0(x
4,0)0(x
=
=
=
=
=
=





maxymaxy12
11
10
maxxmaxx12
11
10
V25,0Vb
0b
0b
V25,0Va
0a
4,0a
==
=
=
==

=
=

Xét khoảng thời gian từ 0,25ữ0,75s, robot di chuyển theo quỹ đạo bậc 1:



=
+=
212
20212
a(t)x
a0,25)(ta(t)x




=
+=
212
20212
b(t)y
b0,25)(tb(t)y

Từ điều kiện biên suy ra:
maxy21
maxx21
Vb
Va
=

=
Xét khoảng thời gian từ 0,75ữ1s, robot di chuyển theo quỹ đạo bậc 2:
9



+=
++=
31323
3031
2
323
a0,75)(t2a(t)x
a0,75)(ta0,75)(ta(t)x




+=
++=
31323
3031
2
323
b0,75)(t2b(t)y
b0,75)(tb0,75)(tb(t)y

Dựa vào các điều kiện biên có:
0(1)y
V(0,75)y

0(1)x
V(0,75)x
3
ymax3
3
xmax3
=
=
=
=





maxy32
maxy31
maxx32
maxx31
V2b
Vb
V2a
Va
=
=
=
=
Xét khoảng thời gian sau 1 giây ta có:
0,4(1)y
0(1)x

3
3
=
=

0,4b0,25V0,25V2
0a0,25V0,25V2
30ymax
2
ymax
30xmax
2
xmax
=+ì+ì
=+ì+ì


Mặt khác để đảm bảo tính liên tục của quỹ đạo ta có:



=
=
(0,75)x(0,75)x
(0,25)x(0,25)x
23
12

+==
+==
xmax20230
xmax120
0,5Va(0,75)xa
0,125V0,4(0,25)xa




=
=
(0,75)y(0,75)y
(0,25)y(0,25)y
23
12




+==
==
ymax20230
ymax120
0,5Va(0,75)yb
0,125V(0,25)yb

Vậy ta có:
( )

00,5V0,125V0,40,25V0,25V2
xmaxxmaxxmax
2
xmax
=+++ì+ì
( )
0,40,5V0,125V0,25V0,25V2
ymaxymaxymax
2
ymax
=++ì+ì
sm5333,0V
xmax
=
sm5333,0V
ymax
=
Thay vào các hệ số của phơng trình quỹ đạo chuyển động ở trên ta đợc ph-
ơng trình quỹ đạo chuyển động của robot nh sau:
( )
( )
( ) ( )







+

+
+
=
0
0,06660,75t0,53330,75t1,0666
3333,00,25t0,5333
0,41,0666t
tx
2
2
1t
1t0,75
0,75t0,25
0,25t0
>



10
( )
( )
( ) ( )







+−+−−

+−
=
0,4
0,33330,75t5333,00,75t0666,1
0,06670,25t0,5333
1,0666t
ty
2
2

1t
1t0,75
0,75t0,25
0,25t0
>
≤≤
≤≤
≤≤
Ch¬ng tr×nh MATLAB sau thÓ hiÖn quü ®¹o chuyÓn ®éng cña tay robot theo
vÞ trÝ vµ tèc ®é:
Quü ®¹o chuyÓn ®éng cña tay robot:
function cdtay=cdtay(N)
%N so diem quy dao can ve
for i=0:N
t=i/N;
time(i+1)=t;
if t<=0.25
x(i+1)=-1.0666*t*t+0.4;
vx(i+1)=-2.1332*t;
y(i+1)=1.0666*t*t;

vy(i+1)=2.1332*t;
elseif (t>0.25)&(t<=0.75)
x(i+1)=-0.5333*(t-0.25)+0.3333;
vx(i+1)=-0.5333;
y(i+1)=0.5333*(t-0.25)+0.0667;
vy(i+1)=0.5333;
elseif(t>0.75)&(t<=1)
x(i+1)=1.0666*(t-0.75)*(t-0.75)-0.5333*(t-0.75)+0.0666;
vx(i+1)=2.1332*(t-0.75)-0.5333;
y(i+1)=-1.0666*(t-0.75)*(t-0.75)+0.5333*(t-0.75)+0.3333;
vy(i+1)=-2.1332*(t-0.75)+0.5333;
else
x(i+1)=0;
y(i+1)=0.4;
end %if
end %for
plot(time,[x;vx;y;vy]);%ve quy dao chuyen dong va van toc
grid;
xlabel('t(s)');
ylabel('s(m)&v(m/s)');
11
Kết quả thu đợc khi chạy chơng trình với 200 điểm quỹ đạo đợc thể hiện
trên hình vẽ 3.
>> cdtay(200)
Hình 3. Quỹ đạo chuyển động của tay robot
Để xác định tốc độ và vị trí các khớp, cần tính đợc các giá trị x,y rời rạc của
tay Robot sau đó thông qua phơng trình động học ngợc xác định góc quay
1
và

2
. Biểu diễn


1
và
2
trên đồ thị thời gian, ta đợc đồ thị biểu diễn vị trí của khớp.
Đạo hàm chuyển động của từng khớp theo thời gian ta sẽ đợc dạng đờng vận tốc
của khớp.
Quỹ đạo chuyển động khớp:
function cdkhop=cdkhop(N)
%N so diem quy dao can ve
l1=0.4;
l2=0.3;
for i=0:N
t=i/N;
time(i+1)=t;
12
if t<=0.25
x=-1.0666*t*t+0.4;
vx=-2.1332*t;
y=1.0666*t*t;
vy=2.1332*t;
elseif (t>0.25)&(t<=0.75)
x=-0.5333*(t-0.25)+0.3333;
vx=-0.5333;
y=0.5333*(t-0.25)+0.0667;
vy(i+1)=0.5333;
elseif(t>0.75)&(t<=1)

x=1.0666*(t-0.75)*(t-0.75)-0.5333*(t-0.75)+0.0666;
vx=2.1332*(t-0.75)-0.5333;
y=-1.0666*(t-0.75)*(t-0.75)+0.5333*(t-0.75)+0.3333;
vy=-2.1332*(t-0.75)+0.5333;
else
x=0;
y=0.4;
end %if
d=Nguoc(x,y,l1,l2);
theta1(i+1)=d(1);
theta2(i+1)=d(2);
end %for
plot(time,[theta1;theta2]);
grid;
xlabel('t(s)');
ylabel('Goc(do)');
Ch¹y ch¬ng tr×nh víi sè ®iÓm lµ 150 ta ®îc:
>> cdkhop(150)
13
Hình 4. Quỹ đạo chuyển động của các khớp
Chơng trình tính toán và vẽ đồ thị vận tốc chuyển động của khớp:
function vkhop=vkhop(N)
%N so diem quy dao can ve
l1=0.4;
l2=0.3;
for i=0:N
t=i/N;
time(i+1)=t;
if t<=0.25
x=-1.0666*t*t+0.4;

vx=-2.1332*t;
y=1.0666*t*t;
vy=2.1332*t;
elseif (t>0.25)&(t<=0.75)
x=-0.5333*(t-0.25)+0.3333;
vx=-0.5333;
y=0.5333*(t-0.25)+0.0667;
vy(i+1)=0.5333;
elseif(t>0.75)&(t<=1)
x=1.0666*(t-0.75)*(t-0.75)-0.5333*(t-0.75)+0.0666;
14
vx=2.1332*(t-0.75)-0.5333;
y=-1.0666*(t-0.75)*(t-0.75)+0.5333*(t-0.75)+0.3333;
vy=-2.1332*(t-0.75)+0.5333;
else
x=0;
y=0.4;
end %if
d=Nguoc(x,y,l1,l2);
theta1(i+1)=d(1);
theta2(i+1)=d(2);
end %for
% Lay dao ham chuyen dong khop theo goc quay cua tung khop.
for i=1:N-1
omega1(i)=(theta1(i+1)-theta1(i))*N; % dw1/dt
omega2(i)=(theta2(i+1)-theta2(i))*N; % dw2/dt
end;
omega1(N)=0.0001;
omega1(N+1)=0;
omega2(N)=0.0001;

omega2(N+1)=0;
plot(time,[omega1;omega2]);
grid;
xlabel('t(s)');
ylabel('Van toc goc(do/s)');
Ch¹y ch¬ng tr×nh víi sè ®iÓm vÏ N=150 ta ®îc:
>>vkhop(150);
15
Hình 5. Đồ thị vận tốc chuyển động của khớp
Câu 4. Thiết kế bộ điều khiển bù trọng lực cho robot
a. Xây dựng phơng trình động lực học
Phơng trình động lực học của hệ thống:
16
( ) ( ) ( )
MqgqFqq,qCqqH =+++

Ta cần xác định đợc các hệ số của phơng trình trên. Đối với robot Planar ta
có (bỏ qua thành phần F):
( )






=
2221
1211
hh
hh

qH
( )
( )
2
2
222
222g1
2
2g
2
121
2
1g111
cos6,02555,2
19,0cos15,0.4,0.215,04,0.5528,025,0.10
Jcll2llmJlmh


+=
+++++=
+++++=
( )
( )
2
2
2
222g1
2
2g212
cos3,03025,0

19,0cos.15,0.4,015,0.5
Jclllmh


+=
++=
++=
( )
2
222g1
2
2g221
cos3,03025,0
Jclllmh

+=
++=
3025,0
Jlmh
2
2
2g222
=
+=
( )
( )

+
=
0sllm
sllmsllm
q,qC
122g12
2122g12222g12





( )
122g11211g11
clclgmcglmg ++=
122g22
cglmg =
b. Xây dựng luật điều khiển
Các hệ thống điều khiển có phản hồi nhằm tạo ra khả năng chống nhiễu tốt.
Nhng khi cần bám sát quỹ đạo với tốc độ và gia tốc lớn thì chúng không đáp ứng
đợc. Cơ chế điều khiển có bù cho phép giảm sai số quỹ đạo.
Việc xác định tín hiệu điều khiển sao cho hệ thống ổn định xung quanh
điểm cân bằng dựa trên phơng pháp ổn định Lyapunov.
Gọi vector trạng thái của hệ thống là
[ ]
T
TT
,

với = q
đ
- q là sai số giữa tín
hiệu mong muốn và tín hiệu thực tế. Chọn hàm Lyapunov xác định dơng dạng:
( ) ( )

p
TT
K
2
1
qqHq
2
1
,qV +=

với mọi
0,q


, K
p
là ma trận chéo xác định dơng.
Vi phân phơng trình trên theo thời gian:
17
( ) ( ) ( )

p

TTT
KqqqHq
2
1
qqHq,qV




++=
Từ phơng trình động lực học của hệ thống:
( ) ( ) ( )
MqgqFqq,qCqqH =+++

Vậy ta có:
( ) ( )
( )
( )
( )

p
TTT
KqgMqqFqqq,qC2qHq
2
1
V +=

Nhìn vào phơng trình trên ta chọn luật điều khiển nh sau:
( )
qKKqgu
Dp

+=

Trong đó K
D
là số xác định dơng, tơng ứng với phần bù phi tuyến thành phần
trọng trờng với bộ điều khiển PD.
18
Câu 5: Mô phỏng hệ thống với bộ điều khiển ở câu 4 khi góc
quay của khớp 1 thay đổi nhảy cấp từ 0 radian đến 1
radian
Mô phỏng bằng SIMULINK nh hình vẽ.
Hình 10. Mô phỏng hệ bằng Simulink
Hình 11. Kết quả mô phỏng - đáp ứng đầu
19
H×nh 12. KÕt qu¶ m« pháng, sai lÖch tÜnh cña hÖ
Tµi liÖu tham kh¶o
[1]. NguyÔn M¹nh TiÕn. “Bµi gi¶ng m«n Robot cao häc n¨m 2002”.
20
[2]. Lorenzo Sciavicco, Bruno Sciliano. “Modeling and Control of Robot
Manipulators”.
21