Báo cáo bài tập lớn kỹ thuật robot

Báo cáo bài tập lớn Kỹ thuật Robot

BÀI 1
a. Xây dựng hệ tọa độ cho các thanh nối
Ta có sơ đồ cánh tay Robot như sau:

Hình 1.1. Ký hiệu mô hình Robot

Robot hình 1.1 là robot có cơ cấu tay dạng cầu với 2 khớp quay và 1 khớp tịnh
tiến. Hệ tọa độ các thanh nối được xây dựng theo phương pháp biểu diễn D-H.
Từ sơ đồ cánh tay,ta gắn các hệ trục tọa độ như sau:
Vị trí gốc tọa độ
-Khâu số 0 thân robot là khâu cố định có gốc tọa độ O được đặt tại
khớp 1
-Khung tọa độ 1 có gốc O1 được gắn vào khớp 2
-Khung tọa độ 2 có gốc O2 được gắn vào khớp 3
-Khung tọa độ 3 có gốc O3 được gắn vào khâu tác động đuôi
 Chiều của khung tọa độ


SV thực hiện:

Page 1


Báo cáo bài tập lớn Kỹ thuật Robot
-Khung tọa độ số 0 có trục Z0 trùng với trục của khớp 1,trục X0 được
đặt theo phương pháp tuyến chung của trục 1 và trục 2
-Khung tọa độ số 1 có trục Z 1 trùng với trục của khớp 2,trục X 1 được
đặt đặt theo phương pháp tuyến chung của trục 2 và trục 3
-Khung tọa độ số 2 có trục Z2 trùng với trục của khớp 3,trục X2 được

đặt theo phương pháp tuyến chung của trục 3 và trục 4
-Khung tọa độ số 3 có trục Z 3 trùng với trục của khớp 4,trục X 3 đặt
song song với trục X3
Từ đó ta xây dựng được hệ trục tọa độ như sau:

Hình 1.2. Vị trí và hướng của các khung tọa độ
b. Xác định ma trận T biểu diễn hệ tọa độ tay Robot
Các khung tọa độ các thanh nối của Robot đã được thiết kế ở hình 1.2. Từ
đó, ta xác định được các tham số theo biểu diễn D – H trong bảng sau :

SV thực hiện:

Thanh

ai

1
2
3

0
a2
0

Page 2 90o
o

-90
0

0

0
0
d3


Báo cáo bài tập lớn Kỹ thuật Robot

Bảng 1.1 : Bảng D – H của Robot.
Vị trí của hai khâu liền kề nhau được mô tả bởi một ma trận biến đổi đồng nhất
i 1

Ai . Cấu hình của một Robot bất kì nào đó của Robot có thể thu được bằng cách

nhân một số thích hợp các ma trận

i 1

Ai lại với nhau

Ma trận biểu diễn quan hệ giữa hai khung tọa độ i-1 và i
i 1

Ai

( 1.2 )

Trong đó :
.

Ta sử dụng ma trận ( 1.2 ) và bảng 1.1 thì ta xác định được các ma trận
thành phần
là :

i 1

Ai biểu diễn quan hệ giữa hai khung tọa độ của hai khớp i-1, i

0

A1

1

A

=

2 2 =
ANhư
3 vậy, ta xác định được ma trận T biểu diễn hệ trục tọa độ tay Robot :

T3  0 A1.1 A2 .2 A3
SV thực hiện:

Page 3


Báo cáo bài tập lớn Kỹ thuật Robot
=

T3

=
Trong đó :

c . Ý nghĩa ma trận


Ma trận T3 có 4 hàng 4 cột.



Biểu diễn một khung tọa độ tay Robot trong khung tọa chuẩn .



Biểu diễn vị trí và hướng của tay Robot.



3 cột đầu tiên biểu diễn hướng của tay Robot, cột thứ tư biểu diễn vị trí
của tay Robot.
+

Cột thứ nhất biểu diễn vectơ pháp tuyến n ( normal ).

+

Cột thứ hai biểu diễn vectơ có hướng mà theo đó ngón tay của

bàn tay nắm vào nhau khi cần nắm đối tượng, gọi là vectơ o
(occupation).

+

Cột thứ ba biểu diễn vectơ có hướng mà theo đó bàn tay tiếp
cận với đối tượng, gọi là vectơ a ( approach ).

 Qua ma trận T người ta có thể phần tích sự hoạt động và lập trình điều
khiển cho Robot
 Ma trận T có ý nghĩa rất lớn trong bài toán động học thuận và bài toán
động học ngược:
+ Động học thuận: khi biết giá trị của biến khớp thay đổi theo thời
gian thì vị trí và hướng của tay Rotbot sẽ hoàn toàn xác định tại
mọi thời điểm
+ Động học ngược: khi biết vị trí và hướng của điểm tác động cuối
ta hoàn toàn có thể xác định được giá trị của các biến khớp từ
việc giải hệ phương trình động học T trên
SV thực hiện:

Page 4


Báo cáo bài tập lớn Kỹ thuật Robot
d. Vị trí của tay Robot trong hệ tọa độ gốc khi
+ θ1 = 30o ; θ2 = 30o ; d3 = 0.3m; a2=0.4m
+ Vị trí tay Robot được xác định bởi ma trận T3 như sau:
=

T3

=
=
+ Vậy vị trí tay robot trong hệ tọa độ gốc là: p=[0,17 0,1 0,46 1]T

SV thực hiện:

Page 5


Báo cáo bài tập lớn Kỹ thuật Robot

Bài 2
a, Xác định góc θ của robot ở cuối hành trình chuyển động

y
r
2

y2

m2
r1
y1

1

m1

x2

x1

Hình 2.1. Mô hình Robot θ-r
Tính toán với các số liệu sau:
m1=m2=3.5 kg, r1=0.4m, rmax=0.8m, =

( rad/s ), =0.125m/s.

Theo đầu bài, ta có :
=

( rad/s )

dθ = dt .

=0.125m/s => =0.125m/s
Thời gian chuyển động của khớp quay là :
SV thực hiện:

Page 6


Báo cáo bài tập lớn Kỹ thuật Robot
t===3.2.
 Góccủa Robot ở cuối hành trình chuyển động là
= .3.2=(rad)
b, Xác định Momen khớp quay và lực tổng hợp khớp tịnh tiến khi Robot ở cuối
hành trình chuyển động.

Dạng tổng quát của phương trình động lực học:

Trong đó :


Mi và Fi lần lượt là momen của khớp thứ i và lực tác dụng lên



thanh nối thứ i.
q là biến của khớp quay hoặc khớp tịnh tiến.



L là một hàm của Lagrange.



Ki và Pi lần lược là động năng và thế năng của thanh nối thứ i.



mi và lần lượt là khối lượng và vận tốc dài của thanh nối thứ i.



Ji và lần lượt là momen quán tính của thanh nối thứ i đối với trục
qua tâm khối của thanh nối thứ i và khớp quay của thanh nối thứ
i.

Ta lần lượt xác định các giá trị Ki và Pi với các bước sau:

1. Xác định động năng và thế năng của thanh nối thứ 1.

 Động năng của thanh nối thứ nhất là :
K1 = m1 + J1
SV thực hiện:

Page 7


Báo cáo bài tập lớn Kỹ thuật Robot
Vì J1 = 0 nên ta có :
K1 = m1
Theo hình 2.1, ta có :
&
�x&
�x1  r1 cos
�
1   r1 sin 
�
�
�
&
�y1  r1 sin 
�y&1  r1 cos

Do đó:
v12  r12 .&2
r12&2
K1  m1
2

(2.1)

 Thế năng của thanh nối thứ nhất là :
P1 = m1gr1 sinθ
2.

(2.2)

Xác định động năng và thế năng của thanh nối thứ 2

 Động năng của thanh nối thứ 2 là :
K2 = m2 + J2
Vì J1 = 0 nên ta có :
K2 = m2
Tương tự thanh nối thứ 1:

�
�x2  r cos
�x&2  r&cos  r&sin 
��
�
�y&2  r sin   r&cos 
�y2  r sin 

Do đó:
v22  r&2  r 2 .&2
r&2  r 2&2
K 2  m2
2

SV thực hiện:



2.3


Page 8


Báo cáo bài tập lớn Kỹ thuật Robot
 Thế năng của thanh nối thứ 2 là :
P2 = m2gr sinθ

(2.4)

Từ (2.1) và (2.3), ta có tổng động năng của hai thanh nối là :
K = K 1 + K2 =

( 2.5 )

Từ (2.2) và (2.4), ta có tổng thế năng của hai thanh nối là :
P = P1 + P2 = m1gr1 sinθ + m2gr sinθ
Do vậy, từ (2.5) và (2.6), ta có :

3.

Xác định momen ở khớp quay của Robot θ - r
Theo phương trình động lực học tổng quát :
Đối với khớp quay q1 = θ, ta có :

Với :

Do đó :

Theo đề bài, ta đã biết ở cuối hành trình chuyển động :
rmax = 0.8m
(rad)
Do khớp tịnh tiến chuyển động với tốc độ không đổi
.
SV thực hiện:

Page 9

( 2.6 )


Báo cáo bài tập lớn Kỹ thuật Robot
Do khớp quay chuyển động với tốc độ không đổi
Thay số vào M1 , ta thu được momen ở khớp quay Robot θ – r :
M1= 0 +2*3.5*0.8*0.125* + (3.5*0.4+3.5*0.8)*9.81*cos
=33.47(N.m)

4.

Xác định lực tổng ở khớp tịnh tiến của Robot θ - r
Theo phương trình động lực học tổng quát :

Đối với khớp tịnh tiến q2

= r, ta có :

Với :

Ta có:

=>
Thay số vào F2 , ta thu được lực tổng ở khớp tịnh tiến khi Robot ở cuối
hành trình chuyển động :
F2= 3.5*0- 3.5*0.8*+ 3.5*9.81*sin= 20.29(N)
Vậy momen ở khớp quay và lực tổng ở khớp tịnh tiến khi Robot ở cuối hành trình
chuyển động là:
M = 33.47 N.m
F = 20.29 N
c, Xác định bộ điều khiển “ Momen tính toán” cho Robot.
SV thực hiện:

Page 10


Báo cáo bài tập lớn Kỹ thuật Robot
Phương trình động lực học của cơ cấu Robot đã trình bày ở trên được viết lại
như sau:

Hay viết dưới dạng ma trận:
(2.1)
Với các ma trận thành phần của phương trình động lực học robot:

Nguyên lý thiết kế bộ điều khiển mo men tính toán là khử các thành phần phi
tuyến của phương trình động lực học của Robot và phân ly đặc tính động học của
các thanh nối. Kết quả nhận được một hệ thống tuyến tính, từ đó dễ dàng sử dụng

các phương pháp thiết kế kinh điển của hệ thống tuyến tính.
Từ ý tưởng đó ta có cấu trúc bộ điều khiển:

Hình 2.2. Cấu trúc bộ điều khiển
Để làm được điều đó, ta chọn phương trình mô tả bộ điều khiển mô men tính toán
như sau:
SV thực hiện:

Page 11


Báo cáo bài tập lớn Kỹ thuật Robot
(2-2)
Từ (2-1) và (2-2) ta có:
= (2-3)
Thiết kế luật điều khiển P-D cho Robot ta có:
(2-4)
Từ (2-3) và (2-4) có:
+ =0
Laplace hóa hai vế của phương trình thu được phương trình sau:
+ Kdp + Kp=0 (2-5)
Mong muốn của hệ thống có đáp ứng như khâu dao động bậc 2 có dạng như sau:

n

p+n=0 (2-6)

Trong đó :
- Hệ số suy giảm
n

- Tần số giao động

Cân bằng phương trình (2-5) và (2-6) thu được :
Kdi=n==
Kpi=n2=)2

SV thực hiện:

Page 12


Báo cáo bài tập lớn Kỹ thuật Robot

Hình 2.3 : Sơ đồ cấu trúc hệ thống điều khiển mô men tính toán
Chọn chất lượng điều khiển có hệ số suy giảm =0.7 và thời gian quá độ tqđ=1s thu
được tham số bộ điều khiển của khớp i như sau:
Kd = 8
Kp = (2=32,65
Trong bài này bộ điều khiển thiết kế cho hai khớp là giống nhau nên các thông số
của bộ điều khiển của hai khớp là như nhau
Suy ra các hệ số của bộ điều khiển:

d, Mô phỏng hệ thống
Thiết kế quỹ đạo chuyển động cho hai khớp dạng 2-1-2.
Để đảm bảo tay Robot di chuyển từ vị trí ban đầu A(x0,y0) đến vị trí cuối cùng là
B(xc,yc) trong khoảng thời gian tc (s) ta có thể tính toán quỹ đạo như sau:

SV thực hiện:

Page 13


Báo cáo bài tập lớn Kỹ thuật Robot

Hình 2.4 : Dạng quỹ đạo 2-1-2
Khi đó Robot di chuyển từ A tới B xảy ra 3 quá trình:
- Đầu tiên là quá trình khởi động có dạng bậc 2
- Quá trình chuyển động đuêỳ có dạng là đường thẳng bậc 1
- Quá trình hãm có dạng đường cong bậc 2

Ta giả thuyết

SV thực hiện:

q&0  0
�
�
q&c  0
�

. Mặt khác ta có:

Page 14


Báo cáo bài tập lớn Kỹ thuật Robot
� tc
q  q0
tm  ; qm  c

.
�
2
2
�
qm  q1
�
&
q&1  q&
�
1.t1 
tm  t1
�
�
1 2
&
q1  q0  .q&
�
2
1t1
&
&1.tc .t1  qc  q0  0
� q&
�
2
1.t1  q
tc
tc2 qc  q0
� t1  m


&
2
4
q&
1
Từ đồ thị trên ta nhận thấy t1 < tc /2 (do ta mong muốn chọn như vậy). Do đó ta
có:
tc
tc2 qc  q0
t1  

&
2
4
q&
1

.(*)

tc2 qc  q0

&
4
q&
1
Biểu thức chỉ có nghĩa khi hàm dưới dấu căn lớn hơn không. Vậy:
>
0

&

&
&
q&
1cp �q1 

4. qc  q0
tc2
.(**)

Giới hạn của gia tốc phụ thuộc vào độ bền cơ khí của Robot. Như vậy có thể thấy
cách thiết kế quĩ đạo như sau:
&
1 nằm trong khoảng theo biểu thức (**).
Chọn q&
&
1 , tc, qc, q0 ta tính được t1 theo (*).
Biết q&

Khi đó quĩ đạo trong các khoảng thời gian như sau:

SV thực hiện:

Page 15


Báo cáo bài tập lớn Kỹ thuật Robot
1 2
�
&
0


t
:
q
(
t
)

q

q&
0
1t .
� 1
2
�
t1
�
&
t1  t2 : q(t )  q0  q&
)
�
1t1 (t 
2
�
1
�
2
&
t


t
:
q
(
t
)

q

q&
2
3
c
1t1 (t  tc )
�
2
�
Trong đó :
&
1 nằm trong khoảng theo biểu thức (**).
- Chọn q&
&
1 ,tc,qc,q0 ta tính được t1 theo (*)
- Biết q&
Kết quả mô phỏng

Hình 2.5: Các tín hiệu thu được từ mô phỏng

SV thực hiện:

Page 16


Báo cáo bài tập lớn Kỹ thuật Robot
Nhận xét: Góc quay của các biến khớp là θ và r bám sát quỹ đạo 2-1-2 cho
trước với sai số nhỏ và các khớp thực hiện tới đúng điểm dùng cuối của
mình.

%======================================
% CHUONG TRINH CHINH
%=======================================
% khai bao gia tri bo dieu khien
Kp = 32.65
Kd = 8
% goi ham RobotThetaR
[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] =
RobotThetaR(Kp,Kd)
% ========================================
% do thi tin hieu thu duoc
%=========================================
% ve mo men va luc cua robot tren cung do thi
plot(At1,AM1,'r')% mo mem khop quay (khop 1)
text (1,11,'mo men khop 1')
hold on
plot(At1,AM2)% luc cua khop tinh tien (khop 2)
SV thực hiện:

Page 17

Báo cáo bài tập lớn Kỹ thuật Robot
text(4,11,'luc khop 2')
grid
% ve sai so goc quay va tinh tien tren cung do thi
plot(At1,AeTheta,'r') % mo mem khop quay (khop 1)
text (2.5,-0.11,' khop 2 quay')
hold on
plot(At1,Aer) % luc cua khop tinh tien (khop 2)
text(2,-0.036,'khop 1 tinh tien')
grid
% ve goc quay khop 1
plot(At1,Aq1)
grid
% do dich chuyen cua khop 2 tinh tien
plot(At1,Aq2)
grid
Xây dựng các hàm
+ hàm RobotThetaR
function[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer]
=RobotThetaR(Kp,Kd)
tc = 3.2;%Dat thoi gian di chuyen cua tay Robot.
m1 = 3.5 ;%Khoi luong thanh 1
SV thực hiện:

Page 18


Báo cáo bài tập lớn Kỹ thuật Robot
m2 = 3.5 ;%Khoi luong thanh 2

r1 = 0.4 ;%Chieu dai thanh noi 1
%=== Vi tri cua tay ban dau` va` cuoi ==
S0 = [0.5 0]; % r1=0,5 m
Sc = [0 1]; % rmax =1 m
%Chuyen vi tri ban dau cua tay Robot sang vi tri cac khop
q0 = DHN(S0);
qc = DHN(Sc);
%Khac phuc truong hop chia 0
If (qc (2)==q0(2));
qc (2) = qc(2)-0.0001;
end
if(qc(1)==q0(1));
qc(1) = qc(1)-0.0001;
end
%Tinh toan gia toc hai khop tinh tien va quay
ddq1 = 1.3*4*abs(qc(1)-q0(1))/tc^2; %Gia toc khop quay
ddq2 = 1.3*4*abs(qc(2)-q0(2))/tc^2; %Gia toc khop tinh tien
%Xac dinh cac khoang thoi gian chuyen dong tang toc, deu va giam toc cho%cac
khop
t11 = tc/2 - sqrt((tc^2*ddq1-4*(qc(1)-q0(1)))/ddq1)/2; %Thoi gian tang toc
SV thực hiện:

Page 19


Báo cáo bài tập lớn Kỹ thuật Robot
t21 = tc - t11; %t21 - t11/2 se la thoi gian chuyen dong deu, tc-t21 se la
%thoi gian giam toc ve 0. Tuc la thoi gian tang va giam toc
% deu bang t11/2
t12 = tc/2 - sqrt((tc^2*ddq2-4*(qc(2)-q0(2)))/ddq2)/2;%Thoi gian tang toc

t22 = tc - t12;
Tk = 0.01; %Sau Tk(s) ta se tinh toan cac tham so cua robot
%Dieu kien ban dau (So kien)
q = q0; dq = [0 ;0];
X0 = [q0(1);dq(1);q0(2);dq(2)];%So kien bien trang thai X
file1 = fopen('RobotThetaR.txt','w');
i = 0 ; %Bien dung de dem
for

t = 0:0.001:tc;i = i+1;

At1(i)=t; %Lay thoi gian de ve do thi
%Tinh toan gia tri dat cho cac khop trong tung khoang thoi gian chuyen
%dong
[qd1, dqd1] = quiDaoKhopThetaR(q0(1),qc(1),ddq1,t11,t21,tc,t);%dq d1 la van toc
cua khop 1, ddq1 la gia tockhop 1
[qd2, dqd2] = quiDaoKhopThetaR(q0(2),qc(2),ddq2,t12,t22,tc,t);qd = [qd1; qd2];
dqd = [dqd1; dqd2];
G11 = (m1*r1 + m2*X0(3))*9.81*cos(X0(1));
SV thực hiện:

Page 20


Báo cáo bài tập lớn Kỹ thuật Robot
G12 = m2*9.81*sin(X0(1));
G1 = [G11;G12];
v1 = 2*m2*X0(3)*X0(4)*X0(2);
v2 = -m2*X0(4)*X0(1)*X0(1);
V1 = [v1;v2];

H11 = m1*r1^2 + m2*X0(3)^2;%r = x21
H = [H11, H12; H21, H22]; %Tinh momen can thiet de thuc hien chuyen dong
[M,fe] = ControllerPD(qd,dqd,q,dq,Kp,Kd,G1,V1,H);
%Lay thong so qui dao thuc robot chuyen dong duoc qua M
[q, dq] = RobotModel(M,X0,Tk);%Lay cac thong so de ve do thi
%-----------------------------------------------------qdd1(i)= qd(1);
qdd2(i) = qd(2);
Aq1(i) = q(1); %Goc quay khop quay
Aq2(i) = q(2); %r
Adq1(i) = dq(1); %Toc do khop quay
Adq2(i) = dq(2); %Toc do khop tinh tien
AM1(i) = M(1); %Mo men khop quay
AM2(i) = M(2) ; %Luc truyen dong cho khop tinh tien
AeTheta (i) = qd(1) - q(1);%Sai lech goc quay
SV thực hiện:

Page 21


Báo cáo bài tập lớn Kỹ thuật Robot
Aer(i) = qd(2) - q(2);%Sai lech chuyen dong tinh tien
%-------------------------------------------------------X0 = [q(1); dq(1); q(2); dq(2)];%Dat lai so kien cho lan tinh sau%q(1)-> Theta;
q(2)->r
% plot(At1,q(1))
%Luu du lieu vao file
fprintf(file1,'%2.4f%2.4f%2.4f%2.4f%2.4f%3.4f
%3.4f\n',t,qd(1),q(1),qd(2),q(2),M(1),M(2));
end
fclose(file1);
+ hàm RobotModel :

function[q,dq] = RobotModel(M,X0,Tk)
%------------------------------------------M01 = M(1);%Mo men dieu khien cho khop quay
F02 = M(2);%Luc dieu khien cho khop tinh tien
x11 = X0(1);%Goc Theta1
x12 = X0(2);%Toc do goc khop quay
x21 = X0(3);%Gia tri r
x22 = X0(4);%Toc do khop tinh tien
%------------------------------------------%Cac thong so cua Robot
m1 = 3.5 ;%Khoi luong thanh 1
m2 = 3.5 ;%Khoi luong thanh 2
SV thực hiện:

Page 22


Báo cáo bài tập lớn Kỹ thuật Robot
r1 = 0.4;%Chieu dai thanh noi 1
%------------------------------------------C1 = cos(x11);
S1 = sin(x11);
%--------------------------------------------%Ma tran quan tinh
H11 = m1*r1^2 + m2*x21^2; %r = x21
H12 = 0;
H21 = 0;
H22 = m2;
H = [H11, H12; H21, H22];
%Momen nhot va momen huong tam
v1 = 2*m2*x21*x22*x12;
v2 = -m2*x22*x12*x12;
V = [v1;v2];
%Momen trong luc

G1 = (m1*r1 + m2*x21)*9.81*C1;
G2 = m2*9.81*S1;
G = [G1;G2];
%Nghich dao ma tran H
Hinv = inv(H);
SV thực hiện:

Page 23


Báo cáo bài tập lớn Kỹ thuật Robot
%Tinh toan gia toc khop tu phuong trinh dong luc hoc dang nguoc
dX = -Hinv * (V+G) + Hinv*[M01;F02];
%Phuong trinh trang thai
x11p = x12;%Toc do khop quay
x21p = x22;%Toc do khop tinh tien
x12p = dX(1);%Gia toc khop quay
x22p = dX(2);%Gia toc khop tinh tien
%Tinh gan dung phuong trinh vi phan
x11 = x11 + Tk * x11p;
x21 = x21 + Tk * x21p;
x12 = x12 + Tk * x12p;
x22 = x22 + Tk * x22p;
X = [x11;x12;x21;x22];
%Tinh vi tri tay Robot
s = [x21; 0];
q = [x11; x21];
dq = [x12; x22];
hàm quiDaoKhopThetaR:
function[q,dq] = quiDaoKhopThetaR(q0,qc,ddq,t1,t2,tc,t)

ift<=t1 %Tang toc
SV thực hiện:

Page 24


Báo cáo bài tập lớn Kỹ thuật Robot
q = q0 + ddq*t^2/2;
dq = ddq * t;
elseif((t>t1) & (t<=t2)) %Chuyen dong deu
q = q0 + ddq*t1*(t-t1/2);
dq = ddq * t1;
elseif((t>t2)&(t<=tc)) %Giam toc do ve khong
q = qc - ddq*(t-tc)^2/2;
dq = ddq*t1 - ddq*(t-t2);
end
end
+ Hàm DHN tính toán động học ngược
function[q] = DHN(S)
%Px = S(1); Py = S(2)
%Theta = q(1); r = q(2)
Px = S(1);
Py = S(2);
Theta = atan2(Py,Px);
r = Px*cos(Theta) + Py*sin(Theta);
q = [Theta; r];
+ hàm ControllerPD
function[M,errorTheta, errordTheta]
=ControllerPD(sd,dsd,theta,dtheta,Kp,Kd,G,V,H)
%Cai dat tham so bo dieu khien vi tri phan hoi PD

SV thực hiện:

Page 25