181
Bước 2.
Biến đổi phương trình vi phân cấp hai
( )
( , )
( )
+
+
=
M q q
C q q q
g q
u
ɺɺ
ɺ ɺ
về cấp một như sau
1
( ) [
( , )
( )]
−
=
=
−
−
q
v
v
M q
u
C q v v
g q
ɺ
ɺ
Trên cơ sở hệ phương trình vi phân cấp một này, ta soạn một m-file (với tên file
xvdot.m
)
như dưới đây.
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
function ydot=xvdot(in)
% Phuong trinh vi phan chuyen dong dang ma tran:
% M(q)*q" + C(q,q')q' + g(q) = tau
% M(q) = ma tran khoi luong
% C(q,q')q' = vector chua luc coriolis va luc ly tam
% g(q) = vector chua luc do trong truong
% tau = vector luc/momen dieu khien
% cac bien su dung hay cac toa do va van toc suy rong
q1=in(1); q2=in(2); q1_dot=in(3); q2_dot=in(4);
q_dot=[q1_dot; q2_dot];
% control vector
u1=in(5); u2=in(6); tau=[u1; u2];
% Cac tham so cua he
% khau 1
m1=21.20; %[kg]
J1=0.21; %[kg m^2]
l1=0.25; %[m]
a1=0.15; %[m]
% khau 2
m2=15.22; %[kg]
J2=0.18; %[kg m^2]
l2=0.45; %[m]
a2=0.19; %[m]
% gia toc trong truong
g=9.81; %m/s^2
% ma tran khoi luong hay ma tran quan tinh
m11 = J1+J2+m1*a1^2+m2*l1^2+m2*a2^2+2*m2*l1*a2*cos(q2);
m12 = J2+m2*a2^2+m2*l1*a2*cos(q2);
m21 = m12; m22 = J2+m2*a2^2;
M = [m11, m12; m21, m22];
