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
qi
Ui
=
Ki
T i.S 1
a. Thiết kế bộ điều khiển vị trí cho mỗi khớp biết
chỉnh ≤ 5%, thời gian quá độ là nhỏ nhất
Ui ≤
10V, độ quá điều
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
=
atan2[s
=>
Xác định biến khớp
Đặt k1 =
NHÓM 14
và k2 =
khi đó ta có:
Page 5
Trường ĐHBK Hà Nội
BTL Robot
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 …
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
]T
n
Q Qd
khi
t
Động lực học
Robot
M
Page 11
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
X& [x&, y&, z&, &x , &y , &z ]T
Quỹ đạo đặt cho tay máy
X d [xd , yd , zd , 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
& G (Q)
= H (Q)Q
2
(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ư vậy
NHÓM 14
Q&
)=
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
0 khi
t
thay vào phương trình (1-5) ta có :
Page 13
(1 3)
(1 4)
(1 5)
(1 6)
(1 7)
Trường ĐHBK Hà Nội
K pE 0
khi
E
BTL Robot
t
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
Qd +
+
_
KP
_
Q
NHÓM 14
Page 14
ĐLH robot
Trường ĐHBK Hà Nội
BTL Robot
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
−1
Mô hình của H
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
- Xem thêm -
.DOCX 
32 
1584 
87 
