BÀI tập lớn môn ROBOT đại học bách khoa BKA (thầy tiến )

Trường ĐHBK Hà Nội

BTL Robot

BÀI TẬP LỚN MÔN ROBOT
Cho robot Scalar 2DOF các thông số như sau:
l1=0.3; l2=0.3;
lg1=0.15; lg2=0.15;
m1=7;m2=2;
J1=0.0525;J2=0.0525;
g=9.81;
K1=380; T1=0.02;
K2=300; T2=0.02;
1. Yêu cầu:
Giả sử mỗi khớp được điều khiển bằng một hệ ộng cơ và driver có hàm truyền
=
a. Thiết kế bộ điều khiển vị trí cho mỗi khớp biết 10V, độ quá điều chỉnh 5%,
thời gian quá độ là nhỏ nhất
b. Chọn 2 quỹ đạo thẳng và tròn, tính quỹ đạo cho từng khớp.
c. Mô phỏng trên Matlab simulink. So sánh quỹ đạo đặt và quỹ đạo thực tế.
2. Yêu cầu:
a.Xây dựng mô hình toán học của robot trên Matlab simulink.
b. Thiết kế bộ điều khiển momen tính toán cho robot, mô phỏng trên Matlab
simulink, so sánh quỹ đạo đặt và quỹ đạo thực.
c. Thiết kế bộ điều khiển PD bù trọng trường cho robot, mô phỏng trên Matlab
simulink, so sánh quỹ đạo đặt và quỹ đạo thực.

NHÓM 14

Page 1

Trường ĐHBK Hà Nội

BTL Robot

BÀI 1:THIẾT KẾ BĐK VỊ TRÍ,QUỸ ĐẠO ROBOT
I. Cơ sở lý thuyết:
1. Bài toán động học thuận
-Mục đích của bài toán động học thuận là tìm ra vị trí và hướng của cơ cấu
tác động cuối khi đã biết giá trị các biến khớp. Gắn hệ tọa độ lên các khâu
- Chon hệ tọa độ gốc Ooxoyozo : Gốc tọa độ Oo trên trục khớp thứ nhất, trục
xo đặt theo phương của khâu 1, chiều hướng từ trục khớp thứ 1 sang trục khớp
thứ 2 (Hình 1.1.1)
- Hệ tọa độ O1x1y1z1 có gốc tọa độ O1 đặt tại tâm trục khớp thứ hai, trục x 1
đặt theo phương của khâu 2, chiều hướng từ trục khớp thứ hai sang trục khớp
thứ ba.
- Đặt hệ tọa độ O2x2y2z2 đặt tại tâm bàn tay nắm bắt, chiều hướng từ trục
khớp thứ hai đến điểm cuối của khớp thứ 2.

Hình 1.1.1 : Sơ đồ động Robot Planar

NHÓM 14

Page 2


Trường ĐHBK Hà Nội

BTL Robot

Bảng thông số DH
Khâu

⍬i

αi

ai

di

Khâu 1

⍬1

0

a1

0

Khâu 2

⍬2

0

a2

0

Bảng1.1.1 : Bảng thông số DH Robot SCARA

Các ma trận chuyển đổi
Bằng việc thay thế các thông số động học Denavit Hartenberg vào biểu thức
tính ma trận ���−1 ta lần lượt xác định được các ma trận chuyển đồi
.
Trong đó ���−1 là ma trận chuyển đổi giữa hệ trục tọa độ i-1 với hệ trục tọa độ i.
Để đơn giản cho việc trình bày ta sử dụng các kí hiệu s⍬I = sin(⍬i) và
c⍬I = cos(⍬i).

Ma trận biểu thị mối quan hệ giữa hệ tọa độ cuối so với hệ tọa độ gốc là:

NHÓM 14

Page 3


Trường ĐHBK Hà Nội

BTL Robot

(2.1)

Với c(⍬1+⍬2) = cos(⍬1+⍬2) ; s(⍬1+⍬2) = sin(⍬1+⍬2)
Cân bằng các phần tử tương ứng của hai ma trận với nhau ta có phương trình
động học của Robot Planar là:

Ma trận biểu diễn hướng của tay máy Robot:

Ma trận biểu diễn vị trí của Robot

2. Bài toán động học ngược
Trong bài toán động học ngược ta cần xác định giá trị các biến khớp của Robot khi đã
biết vị trí và hướng của điểm tác động cuối. Vị trí và hướng của khâu tác động cuối được
xác định từ quỹ đạo chuyển đông cho trước của robot, dưới dạng ma trận:

NHÓM 14

Page 4


Trường ĐHBK Hà Nội

BTL Robot

Ta biết phương trình động học của Robot Planar

Ma trận

đã biết (biết vị trí và hướng của cơ cấu tác động cuối ) ta cần xác

định giá trị các biến khớp

tương ứng. Bằng cách cân bằng phần tử ở các hàng

1 và hàng 2 cột hai ma trận tương ứng ta xác định được:
(2.3)

Xác định biến khớp

Bình phương hai vế các phương trình (2.9) và cộng lại ta nhận được phương
trình:

c

=> s

=>

NHÓM 14

=

atan2[s

Page 5


Trường ĐHBK Hà Nội

BTL Robot

Xác định biến khớp

Đặt k1 =

và k2 =

khi đó ta có:

và � = atan2 (

Đặt k1 = r. c(�), k2 = r. s(�). Với r =

< = >

=>

Do đó:
= atan2 (px,

Vậy ta có:

) - � = atan2 (px,

atan2 (px,

atan2[s

NHÓM 14

Page 6

) – atan2 (

) – atan2 (


Trường ĐHBK Hà Nội

BTL Robot

II. Tiến hành:
1. Thiết kế bộ điều khiển vị trí:

Hình 1.1.2: Sơ đồ bộ điều khiển vị trí chô mỗi khớp

Hình 1.1.3: Vị trí khớp 1 so với lượng đặt
NHÓM 14

Page 7


Trường ĐHBK Hà Nội

BTL Robot

Hình 1.1.4: Vị trí khớp 1 so với lượng đặt
2. Thiết kế quỹ đạo thẳng và tròn, thiết kế quỹ đạo cho từng khớp:
2.1: Chọn quỹ đạo thẳng :
fid=fopen('quy_dao_thang.txt','w');
for x=1.04:0.01:2.25
y=(3.148-0.7*x)/1.21;
m=(x^2+y^2-l1^2-l2^2)/(2*l1*l2);
q2=acos(m);
MatA=[l1+l2*cos(q2) l2*sin(q2);-l2*sin(q2) l1+l2*cos(q2)];
Q1=[x y]/(MatA);
cq1=Q1(1,1);sq1=Q1(1,2);
q1=atan2(sq1,cq1);
g2=q2*180/pi;g1=q1*180/pi;

fprintf(fid,'[ %3.4f %3.4f %3.4f %3.4f ]',x,y,g1, g2);
end

2.2. Chọn quỹ đạo tròn:
fclose(fid);
fid=fopen('quy_dao_tron.txt','w');
for x=-0.99:0.02:0.99
y=sqrt(5.7762-x^2);
m=(x^2+y^2-l1^2-l2^2)/(2*l1*l2);

NHÓM 14

Page 8


Trường ĐHBK Hà Nội

BTL Robot

q2=acos(m);
MatA=[l1+l2*cos(q2) l2*sin(q2);-l2*sin(q2) l1+l2*cos(q2)];
Q1=[x y]/(MatA);
cq1=Q1(1,1);sq1=Q1(1,2);
q1=atan2(sq1,cq1);
g2=q2*180/pi;g1=q1*180/pi;
fprintf(fid,'[ %3.4f %3.4f %3.4f %3.4f ]',x,y,g1, g2);

end

2.3. Đồ thị mô phỏng:

Hình 1.2.1: Quỹ đạo góc khớp 1 mô phỏng (màu vàng)và tính toán (màu đen)

Hình 1.2.2: Quỹ đạo góc khớp 2 mô phỏng (màu vàng)và tính toán (màu đen)
NHÓM 14

Page 9


Trường ĐHBK Hà Nội

BTL Robot

Hình 1.2.3: Quỹ đạo thẳng của cánh tay robot tính toán và thực tế

Hình 1.2.4: Quỹ đạo tròn của cánh tay robot tính toán và thực tế

NHÓM 14

Page 10


Trường ĐHBK Hà Nội

BTL Robot

BÀI 2: XÂY DỰNG MÔ HÌNH ROBOT, BỘ DDIEUF
KHIỂN MÔ MEN VÀ BỘ ĐIỀU KHIỂN PD BÙ
TRỌNG TRƯỜNG

I. Cơ sở lý thuyết:
1.1. Tổng quan về phương pháp điều khiển robot
Phương trình động lực học robot :
&
&+ V (Q, Q&) + G (Q)
M = H (Q)Q
Chức năng của hệ thống điều khiển chuyển động là tìm M để cánh tay có quỹ
t→∞

đạo bám theo quỹ đạo yêu cầu khi thời gian
. Trên cơ sở đó có hai hệ thống
điều khiển chuyển động : hệ thống điều khiển ở không gian khớp và hệ thống điều
khiển ở không gian làm việc.
Điều khiển trong không gian khớp
Đại lượng điều khiển là vị trí của khớp robot : góc quay đối với khớp quay; độ
dịch chuyển thẳng đối với khớp tinh tiến. Bộ điều khiển được thiết kế đảm bảo vị
trí khớp luôn bám theo vị trí đặt, tức là sai lệch vị trí khớp hội tụ về không với thời
gian nhỏ nhất.
Vecto khớp : Q = [q1, q2,q3….qn]T
= [ 1, 2, 3 … n]T
Giá trị đặt biến khớp :
Qd = [q1d, q2d, …qnd]T
Yêu cầu đặt ra là tìm M để
Qd
Q,
NHÓM 14

Thuật toán
điều khiển
chuyển động

Q → Qd

khi

t →∞

M
Page 11

Động lực học
Robot

Q


Trường ĐHBK Hà Nội

BTL Robot

Điều khiển trong không gian làm việc
Tín hiệu đặt là vị trí bàn tay máy
Quỹ đạo của tay máy :
X = [x, y, z , φx , φ y , φ z ]T
& &T
X&= [x&, y&&
, z , φ&
x , φ y , φz ]

Quỹ đạo đặt cho tay máy

X d = [xd , yd , z d , φxd , φ yd , φzd ]T
X → Xd

t →∞

Yêu cầu đặt ra là tìm M để
khi
.
Theo yêu cầu của đề bài cho ta sẽ thiết kế 2 bộ điều khiển trong không gian khớp
đó là: Điều khiển trực tiếp momen PIDvà bộ điều khiển PD bù trọng trường.
Sau đây em xin được đề cập đến hai phương pháp điều khiển trong không gian
khớp. Ưu điểm của phương pháp này là bộ điều khiển tác động trực tiếp đến hệ
thống truyền động của khớp. Tuy nhiên, hệ thống khó đảm bảo độ chính xác vị trí
tay khi tồn tại các sai lệch trong cơ cấu cơ khí.
1.2. Tiêu chuẩn Lyapunov
Định nghĩa (Lyapunov) : Một hệ thống được gọi là ổn định tiệm cận Lyapunov
tại điểm cân bằng x nếu như hệ đang ở điểm cân bằng x mà có một tác động nhiễu
tức thời (hoặc khoảng thời gian đủ ngắn) đánh bật hệ ra khỏi x thì sau đó hệ có khả
năng tự quay về điểm cân bằng ban đầu .
Tiêu chuẩn Lyapunov là phương pháp xét tính ổn định một cách trực tiếp trong
không gian trạng thái, thích hợp cho những hệ thống mô tả bởi mô hình trạng thái.
Hệ ổn định nếu quá tình tự do của nó tăt dần nên phương pháp Lyapunov sẽ kiểm
tra xem quỹ đạo trạng thái của hệ thống ở quá trình tự do có hướng tiến về gốc tọa
độ và kết thúc tại đó không.
Từ phương diện năng lượng có thể xem như phương pháp Lyapunov được xây
dựng trên cơ sở bảo tồn năng lượng của một hệ vật lý. Năng lượng còn tồn tại bên
trong hệ vật lý do tác động tức thời bên ngoài đưa vào được đo bởi một hàm không
âm.Hệ sẽ ổn định ở trạng thái cân bằng của nó nếu nhưu trong lân cận điểm cân
bằng đó hàm do năng lượng này của hệ luôn có xu hướng giảm về 0.

Định lý Lyapunov : Nếu tồn tại hàm Lyapunov V(x) ,thỏa mãn các điều kiện:
a, Xác định dương, tức là V(x)> 0 với x ≠ 0 và V(x)= 0 ↔
b, dV(x)/dt < 0 với x là nghiệm tự do của hệ thống thì hệ sẽ ổn định.
NHÓM 14

Page 12


Trường ĐHBK Hà Nội
1.3. Phương pháp PD bù trọng trường

BTL Robot

Yêu cầu đặt ra là xác định cấu trúc bộ điều khiển đảm bảo hệ thống ổn định
tuyệt đối xung quanh điểm cân bằng ( Q= Qd ), không phụ thuộc vào khối lượng
của các thanh nối và tải. Luật điều khiển được xây dựng dựa trên tiêu chuẩn ổn
định Lyapunov trực tiếp.
Phương trình động lực học :
&
&+ V (Q, Q
&) + G (Q )
M = H (Q )Q

&
&+  1 H&(Q) + S (Q, Q
&)  Q
&
= H (Q )Q
 2
 + G (Q )

(1-1)

Phương trình điều khiển

M dk = K p E − K d E&+ G (Q )

(1-2)
Trong đó : E = Qd – Q : sai số vị trí khớp robot
E&= Q&d − Q&

: sai số tốc độ khớp robot
Thay (1-2) vào (1-1) ta được phương trình động lực học kín :
&
&+ V (Q, Q&) + G (Q)
K p E − K d E&+ G (Q ) = H (Q )Q

&
&+ V (Q, Q
&
&) = H (Q )Q
&
&+  1 H&(Q) + S (Q, Q&)  Q&
↔ K p E − K d E&= H (Q)Q
 2

&
&+  1 H&(Q ) + S (Q, Q&)  Q&− K E = − K Q&
↔ H (Q)Q
p

d
 2

&
&+ 1 Q&T H&(Q)Q&+ Q&T S (q, q&)Q&− Q&T K E = −Q&T K Q&
↔ Q&T H (Q)Q
p
d
2
1 d &T
Q H (Q)Q&+ E T K p E  = −Q&T K d Q& ( do Q&T S (Q, Q&)Q&= 0 )
→
2 dt

Chọn hàm Lyapunov : V(Q,

Ta thấy rằng :

NHÓM 14

)=

1 &T
Q H (Q)Q&+ E T K p E 
2

V( E ) ≥ 0

&
V( E ) = 0 ↔ E = 0 và Q=0

&&
V (Q) ≤ 0
V&(Q&) = 0 ↔ Q&= 0


Page 13

(1 − 3)
(1 − 4)
(1 − 5)
(1 − 6)
(1 − 7)


Trường ĐHBK Hà Nội
Như vậy

Q& →

K pE → 0

0 khi

khi

BTL Robot
t →∞

thay vào phương trình (1-5) ta có :

t →∞

⇒ E → 0 khi t → ∞

.

Sơ đồ điều khiển :
Qd +

KP

_
Q

+
+

_

ĐLH robot

KD
Q
G(Q)
G(.)

Hình 2.1.1 Sơ đồ điều khiển PD bù trọng trường
Ưu điểm : đơn giản về mặt toán học.
Nhược điểm : Phải biết chính xác các tham số khối lượng trong G(Q).

1.4 Thuật toán PID
Moment điều khiển :
Mdk =KpE - Kd
(1-8)
K p = diag { k p1 , k p 2 ,...k pn }
K d = diag { kd 1 , kd 2 ,...kdn }

+KI

là ma trận đường chéo hệ số khuếch đại .
là ma trận đường chéo hệ số đạo hàm .

Sơ đồ điều khiển :
KI

NHÓM 14

Page 14

ĐLH robot


Trường ĐHBK Hà Nội
Qd +

+
_

BTL Robot
KP

_

Q
KD
Q

Hình 2.1.2. Sơ đồ điều khiển PID
Nhược điểm của phương pháp PID là các thông số K I, Kd, KP chỉ được xác định
bằng thực nghiệm nên rất khó đưa ra được thông số chính xác.

NHÓM 14

Page 15


Trường ĐHBK Hà Nội

BTL Robot

II. Tiến hành:
2.1. Mô hình robot trên matlab:

Trong đó:
• Code khối G:

function y =bai2_codeG(in)
%q(1) goc theta1
%q(2) goc theta2
%dq(1) toc do quay khop 1

%dq(2) toc do quay khop 2
%cac thong so robot
global m1 m2 l1 lg1 lg2 g
q(1) =in(1);
q(2) = in(2);
%
trong luc
y(1,1)=g*m1*lg1*cos(q(1))+g*m2*(l1*cos(q(1))+lg2*cos(q(1)+q(2)));
y(2,1)= g*m2*lg2*cos(q(1)+q(2)) ;
end

NHÓM 14

Page 16


Trường ĐHBK Hà Nội

BTL Robot

• Code khối H:

function y = bai2_codeH(in)
%q(1) goc theta1
%q(2) goc theta2
%dq(1) toc do quay khop 1
%dq(2) toc do quay khop 2
%cac thong so robot
global m1 m2 l1 l2 lg1 lg2 J1 J2
U(1) =in(1);

U(2) =in(2);
q(1) =in(3);
q(2) =in(4);
% ma tran quan tinh
h(1,1) = m1*lg1^2+J1+m2*(l1^2+lg2^2+2*l1*lg2*cos(q(2)))+J2 ;
h(1,2) = m2*(lg2^2 + l1*lg2*cos(q(2)))+J2 ;
h(2,1) = m2*(lg2^2 + l1*lg2*cos(q(2)))+J2 ;
h(2,2) = m2*lg2^2 + J2 ;
% gia toc khop
y=h*[U(1);U(2)];
end

• code khối V:

function y = bai2_codeV(in)
%q(1) goc theta1
%q(2) goc theta2
%dq(1) toc do quay khop 1
%dq(2) toc do quay khop 2
%cac thong so robot
global m2 l1 lg2
q(1) = in(1);
q(2) = in(2);
dq(1) =in(3);
dq(2) =in(4);
%
Detailed explanation goes here
y(1,1) = -m2*l1*lg2*sin(q(2))*(2*dq(1)+dq(2));
y(2,1) = m2*l1*lg2*sin(q(2))*dq(1)^2 ;
end

NHÓM 14

Page 17


Trường ĐHBK Hà Nội

BTL Robot

2.2. Thiết kế bộ điều khiển mô men tính toán cho robot:
a. Mô hình bộ điều khiển mô men tính toán cho robot
Với: KP= [32.65 0;0 32.65]
KI= [8 0;0 8]
LP1= [16 0;0 16]

b. Mô hình tính mô men điều khiển Mđk:

NHÓM 14

Page 18


Trường ĐHBK Hà Nội

BTL Robot

c. Mô hình robot từ Mđk ra q,dq,ddq:

- Trong đó :

• Các mô hình G,V không đổi
• Mô hình của như sau:

function y = bai2_codeH_1(in)
%q(1) goc theta1
%q(2) goc theta2
%dq(1) toc do quay khop 1
%dq(2) toc do quay khop 2

NHÓM 14

Page 19


Trường ĐHBK Hà Nội

BTL Robot

%cac thong so robot
global m1 m2 l1 l2 lg1 lg2 J1 J2
q(1) =in(1);
q(2) =in(2);
M01 =in(3);
M02 =in(4);
% ma tran quan tinh
h(1,1) = m1*lg1^2+J1+m2*(l1^2+lg2^2+2*l1*lg2*cos(q(2)))+J2 ;
h(1,2) = m2*(lg2^2 + l1*lg2*cos(q(2)))+J2 ;
h(2,1) = m2*(lg2^2 + l1*lg2*cos(q(2)))+J2 ;
h(2,2) = m2*lg2^2 + J2 ;
Hinv=inv(h);

% gia toc khop
y=Hinv*[M01;M02];
end

NHÓM 14

Page 20


Trường ĐHBK Hà Nội

BTL Robot

d. Đáp ứng của 2 khớp trong mô phỏng:

Hình 2.2.1: Đáp ứng của góc khớp q so với giá trị đặt

Hình 2.2.2: Đáp ứng của dq trong mô hình
NHÓM 14

Page 21


Trường ĐHBK Hà Nội

BTL Robot

Hình 2.2.3: Đáp ứng của ddq trong mô hình

Hình 2.2.4 : Đáp ứng của mô men điều khiển trong mô hình

NHÓM 14

Page 22


Trường ĐHBK Hà Nội

BTL Robot

2.3. Thiết kế bộ điều khiển PD bù trọng trường:
a. Mô hình của bộ điều khiển:

- Trong đó các thông số của bộ điều khiển như sau:
Kd= [16 0;0 16]
Kp= [100 0;0 100]
b. Các đáp ứng của q,dq,ddq và Mđk:

Hình 2.3.1: đáp ứng của 2 khớp (q)
NHÓM 14

Page 23


Trường ĐHBK Hà Nội

BTL Robot

Hình 2.3.2: Đáp ứng của Mđk 2 khớp

Hình 2.3.4: Đáp ứng vận tốc 2 khớp

NHÓM 14

Page 24


Trường ĐHBK Hà Nội

BTL Robot

Hình 2.3.5: Đáp ứng gia tốc của 2 khớp
Ghi chú:
Quỹ đạo thẳng của bài toán robot
Thông số tính toán
q1
q2
x
y
35.0826 59.9685
1.04
2.
34.7697 60.012
1.05
1.9942
34.4591 60.0501
1.06
1.9884
34.1507 60.0831
1.07
1.9826
33.8445 60.1107

1.08
1.9769
33.5407 60.1331
1.09
1.9711
33.2391 60.1503
1.1
1.9653
32.9399 60.1622
1.11
1.9595
32.643
60.1688
1.12
1.9537
32.3484 60.1702
1.13
1.9479
32.0562 60.1664
1.14
1.9421
31.7664 60.1573
1.15
1.9364
31.4789 60.143
1.16
1.9306
NHÓM 14

Thông số mô phỏng

q1
q2
35.0767 59.9585
34.7629 60.002
34.4536 60.0401
34.145
60.073
33.8387 60.1007
33.5351 60.1231
33.2335 60.1403
32.9344 60.1522
32.6376 60.1587
32.343
60.1602
32.0508 60.1564
31.7611 60.1473
31.4737 60.133
Page 25

x
1.040416
1.050445
1.060399
1.070406
1.08041
1.090401
1.100402
1.110397
1.120396
1.130395

1.140393
1.150388
1.160386

y
1.999911
1.994107
1.988345
1.982555
1.976764
1.970982
1.965194
1.959409
1.953624
1.947836
1.942049
1.936265
1.930478