Tartalomjegyzék
\[ \newenvironment{dcases}{\left\{\begin{array}{ll}}{\end{array}\right.} \]Crane model derivation
Teljes Matlab script
(és live script)
kiegészítő függvényekkel.
Tekintsd meg LiveEditor nézetben is!
File: d2017_10_30_levezetesek.mlx
Author: Peter Polcz <mailto:ppolcz@gmail.com ppolcz@gmail.com>
Created on 2017. October 30.
Rakodó daru állapottérmodelljének levezetése a nemlineáris mozgásegyenletekből. Érdemesebb Matlab Live View nézetben megtekinteni, ugyanis ott a szimbolikus képletek LaTeX-esítve jelennek meg.
Equations of the motion
Declare symbolic variables and functions
syms R(t) L(t) Theta(t) m M F T rho g J L0 l(t) R0 r(t) Theta0 theta(t) F0 f T0 tau
State and input vector
pcz_generateSymStateVector(6,'x');
u = [f ; tau];
Equations
eqn1 = m*(...
diff(R,2) ...
+ diff(L,2)*sin(Theta) ...
+ 2*diff(L)*diff(Theta)*cos(Theta) ...
+ L*diff(Theta,2)*cos(Theta) ...
- L*diff(Theta)^2*sin(Theta)) + M*diff(R,2) - F
eqn2 = m*(diff(R,2)*sin(Theta) + diff(L,2)) ...
+ J/rho^2*diff(L,2) ...
- M*L*diff(Theta)^2 ...
- m*g*cos(Theta) + T/rho
eqn3 = m*(diff(R,2)*cos(Theta) + 2*diff(L)*diff(Theta) + L*diff(Theta,2)) + m*g*sin(Theta)
Output:
eqn1(t) = m*(sin(Theta(t))*diff(L(t), t, t) + diff(R(t), t, t) - sin(Theta(t))*L(t)*diff(Theta(t), t)^2 + cos(Theta(t))*L(t)*diff(Theta(t), t, t) + 2*cos(Theta(t))*diff(L(t), t)*diff(Theta(t), t)) - F + M*diff(R(t), t, t) eqn2(t) = T/rho + m*(sin(Theta(t))*diff(R(t), t, t) + diff(L(t), t, t)) - g*m*cos(Theta(t)) - M*L(t)*diff(Theta(t), t)^2 + (J*diff(L(t), t, t))/rho^2 eqn3(t) = m*(L(t)*diff(Theta(t), t, t) + 2*diff(L(t), t)*diff(Theta(t), t) + cos(Theta(t))*diff(R(t), t, t)) + g*m*sin(Theta(t))
Centered system parameters. Operating point values.
Theta0 = 0;
F0 = 0;
T0 = m*g*rho;
L(t) = L0 + l(t);
R(t) = R0 + r(t);
Theta(t) = Theta0 + theta(t);
F = F0 + f;
T = T0 + tau;
Substitute into the motion equations:
eqn1 = subs(eqn1)
eqn2 = subs(eqn2)
eqn3 = subs(eqn3)
Output:
eqn1(t) = m*(sin(theta(t))*diff(l(t), t, t) + 2*cos(theta(t))*diff(l(t), t)*diff(theta(t), t) + diff(r(t), t, t) - sin(theta(t))*(L0 + l(t))*diff(theta(t), t)^2 + cos(theta(t))*(L0 + l(t))*diff(theta(t), t, t)) - f + M*diff(r(t), t, t) eqn2(t) = m*(sin(theta(t))*diff(r(t), t, t) + diff(l(t), t, t)) + (tau + g*m*rho)/rho - M*(L0 + l(t))*diff(theta(t), t)^2 - g*m*cos(theta(t)) + (J*diff(l(t), t, t))/rho^2 eqn3(t) = m*((L0 + l(t))*diff(theta(t), t, t) + 2*diff(l(t), t)*diff(theta(t), t) + cos(theta(t))*diff(r(t), t, t)) + g*m*sin(theta(t))
In order to trace back the problem to a system of linear equations, we collect the coefficients of the second derivatives $\ddot{l},\, \ddot{r},\, \ddot{\theta}$.
VARS = [diff(l,2) ; diff(r,2) ; diff(theta,2)];
[c1,t1] = coeffs(eqn1,VARS); c1 = c1(t); [ transpose(t1) transpose(c1) ]
[c2,t2] = coeffs(eqn2,VARS); c2 = c2(t); [ transpose(t2) transpose(c2) ]
[c3,t3] = coeffs(eqn3,VARS); c3 = c3(t); [ transpose(t3) transpose(c3) ]
Output:
ans(t) = [ diff(l(t), t, t), m*sin(theta(t))] [ diff(r(t), t, t), M + m] [ diff(theta(t), t, t), m*cos(theta(t))*(L0 + l(t))] [ 1, m*(2*cos(theta(t))*diff(l(t), t)*diff(theta(t), t) - sin(theta(t))*(L0 + l(t))*diff(theta(t), t)^2) - f] ans(t) = [ diff(l(t), t, t), m + J/rho^2] [ diff(r(t), t, t), m*sin(theta(t))] [ 1, (tau + g*m*rho)/rho - M*(L0 + l(t))*diff(theta(t), t)^2 - g*m*cos(theta(t))] ans(t) = [ diff(r(t), t, t), m*cos(theta(t))] [ diff(theta(t), t, t), m*(L0 + l(t))] [ 1, 2*m*diff(l(t), t)*diff(theta(t), t) + g*m*sin(theta(t))]
Express the second derivatives $\ddot{l},\, \ddot{r},\, \ddot{\theta}$ using coeffs.
A_ = [
c1(1) c1(2) c1(3)
c2(1) c2(2) 0
0 c3(1) c3(2)
]
b_ = -[
c1(end)
c2(end)
c3(end)
]
Output:
A_ =
[ m*sin(theta(t)), M + m, m*cos(theta(t))*(L0 + l(t))]
[ m + J/rho^2, m*sin(theta(t)), 0]
[ 0, m*cos(theta(t)), m*(L0 + l(t))]
b_ =
f - m*(2*cos(theta(t))*diff(l(t), t)*diff(theta(t), t) - sin(theta(t))*(L0 + l(t))*diff(theta(t), t)^2)
M*(L0 + l(t))*diff(theta(t), t)^2 - (tau + g*m*rho)/rho + g*m*cos(theta(t))
- 2*m*diff(l(t), t)*diff(theta(t), t) - g*m*sin(theta(t))
Only for LaTeX reasons
Sorrend: $l,r,\theta$.
A = sym('a%d%d',[3,3]);
b = sym('b',[3,1]);
A(2,3) = 0;
A(3,1) = 0;
SOL = simplify(A\b)*det(A)
DET = det(A)
pcz_latex(SOL), pcz_latex(DET)
pcz_latex(A), pcz_latex(b)
Output:
SOL =
a13*a32*b2 - a12*a33*b2 - a13*a22*b3 + a22*a33*b1
a13*a21*b3 + a11*a33*b2 - a21*a33*b1
a11*a22*b3 - a12*a21*b3 - a11*a32*b2 + a21*a32*b1
DET =
a11*a22*a33 - a12*a21*a33 + a13*a21*a32
SOL = \left(\begin{array}{c} a_{13}a_{32}b_{2}-a_{12}a_{33}b_{2}-a_{13}a_{22}b_{3}+a_{22}a_{33}b_{1} \\ a_{13}a_{21}b_{3}+a_{11}a_{33}b_{2}-a_{21}a_{33}b_{1} \\ a_{11}a_{22}b_{3}-a_{12}a_{21}b_{3}-a_{11}a_{32}b_{2}+a_{21}a_{32}b_{1} \end{array}\right)
DET = a_{11}a_{22}a_{33}-a_{12}a_{21}a_{33}+a_{13}a_{21}a_{32}
A = \left(\begin{array}{ccc} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & 0 \\ 0 & a_{32} & a_{33} \end{array}\right)
b = \left(\begin{array}{c} b_{1} \\ b_{2} \\ b_{3} \end{array}\right)
Express the second derivatives $\ddot{l},\, \ddot{r},\, \ddot{\theta}$ - manual
clear m M rho g F0 T0 L0 R0 J
syms R(t) L(t) Theta(t) m M F T rho g J L0 l(t) R0 r(t) Theta0 theta(t) F0 f T0 tau
b1 = f - 2*m*diff(l)*diff(theta)*cos(theta) + m*diff(theta)^2*(L0+l)*sin(theta);
b2 = M*(L0 + l)*diff(theta)^2 - m*g*(1-cos(theta)) - tau/rho;
b3 = -2*m*diff(l)*diff(theta) - m*g*sin(theta);
a11 = m*sin(theta);
a12 = M+m;
a13 = m*(L0 + l)*cos(theta);
a21 = m + J/rho^2;
a22 = m*sin(theta);
a32 = m*cos(theta);
a33 = m*(L0 + l);
A = [
a11 a12 a13
a21 a22 0
0 a32 a33
];
b = [
b1
b2
b3
];
simplify([ A - A_ , b - b_ ])
SOL = simplify(A\b);
STATE = [
l
diff(l)
r
diff(r)
theta
diff(theta)
];
fxu_analytic = simplify(subs(SOL,STATE,x))
Output:
ans(t) =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
fxu_analytic(t) =
-(rho*(M*tau + m*tau + g*m^2*rho - m*tau*cos(x5)^2 - g*m^2*rho*cos(x5)^2 + f*m*rho*sin(x5) - L0*M^2*rho*x6^2 + L0*m^2*rho*x6^2 - M^2*rho*x1*x6^2 + m^2*rho*x1*x6^2 + M*g*m*rho - m^2*rho*x1*x6^2*cos(x5)^2 - L0*M*m*rho*x6^2 - M*g*m*rho*cos(x5) - M*m*rho*x1*x6^2 - L0*m^2*rho*x6^2*cos(x5)^2 + M*m*rho*x1*x6^2*cos(x5)^2 + L0*M*m*rho*x6^2*cos(x5)^2))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2)
(J*f + f*m*rho^2 + g*m^2*rho^2*sin(x5) + m*rho*tau*sin(x5) + (J*g*m*sin(2*x5))/2 + L0*m^2*rho^2*x6^2*sin(x5) + J*L0*m*x6^2*sin(x5) + m^2*rho^2*x1*x6^2*sin(x5) + J*m*x1*x6^2*sin(x5) - M*m*rho^2*x1*x6^2*sin(x5) - L0*M*m*rho^2*x6^2*sin(x5))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2)
-(2*(J*f*cos(x5) + J*m*x2*x6 + J*M*g*sin(x5) + J*g*m*sin(x5) + (g*m^2*rho^2*sin(2*x5))/2 + f*m*rho^2*cos(x5) + 2*J*M*x2*x6 + (m*rho*tau*sin(2*x5))/2 + M*g*m*rho^2*sin(x5) - J*m*x2*x6*cos(2*x5) + (L0*m^2*rho^2*x6^2*sin(2*x5))/2 + (J*L0*m*x6^2*sin(2*x5))/2 + (m^2*rho^2*x1*x6^2*sin(2*x5))/2 + (J*m*x1*x6^2*sin(2*x5))/2 + 2*M*m*rho^2*x2*x6 - (L0*M*m*rho^2*x6^2*sin(2*x5))/2 - (M*m*rho^2*x1*x6^2*sin(2*x5))/2))/((L0 + x1)*(2*M*m*rho^2 + J*m + 2*J*M - J*m*cos(2*x5)))
Using odeToVectorField
[V,S] = odeToVectorField(eqn1,eqn2,eqn3);
Generate symbolic function $\dot{\text{ }x} =F\left(x,u\right)$
symvar_V = num2cell(symvar(V));
symvar_V(symvar(V) == 'Y') = {x};
fh_tmp = matlabFunction(V,'vars', num2cell(symvar(V)));
Fxu = simplify(fh_tmp(symvar_V{:}));
fxu_ode2vf = Fxu([2 4 6])
fxu_analytic - fxu_ode2vf
Output:
fxu_ode2vf =
-(rho*(M*tau + m*tau + g*m^2*rho - m*tau*cos(x5)^2 - g*m^2*rho*cos(x5)^2 + f*m*rho*sin(x5) - L0*M^2*rho*x6^2 + L0*m^2*rho*x6^2 - M^2*rho*x1*x6^2 + m^2*rho*x1*x6^2 + M*g*m*rho - m^2*rho*x1*x6^2*cos(x5)^2 - L0*M*m*rho*x6^2 - M*g*m*rho*cos(x5) - M*m*rho*x1*x6^2 - L0*m^2*rho*x6^2*cos(x5)^2 + M*m*rho*x1*x6^2*cos(x5)^2 + L0*M*m*rho*x6^2*cos(x5)^2))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2)
(J*f + f*m*rho^2 + g*m^2*rho^2*sin(x5) + m*rho*tau*sin(x5) + (J*g*m*sin(2*x5))/2 + L0*m^2*rho^2*x6^2*sin(x5) + J*L0*m*x6^2*sin(x5) + m^2*rho^2*x1*x6^2*sin(x5) + J*m*x1*x6^2*sin(x5) - M*m*rho^2*x1*x6^2*sin(x5) - L0*M*m*rho^2*x6^2*sin(x5))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2)
-(2*(J*f*cos(x5) + J*m*x2*x6 + J*M*g*sin(x5) + J*g*m*sin(x5) + (g*m^2*rho^2*sin(2*x5))/2 + f*m*rho^2*cos(x5) + 2*J*M*x2*x6 + (m*rho*tau*sin(2*x5))/2 + M*g*m*rho^2*sin(x5) - J*m*x2*x6*cos(2*x5) + (L0*m^2*rho^2*x6^2*sin(2*x5))/2 + (J*L0*m*x6^2*sin(2*x5))/2 + (m^2*rho^2*x1*x6^2*sin(2*x5))/2 + (J*m*x1*x6^2*sin(2*x5))/2 + 2*M*m*rho^2*x2*x6 - (L0*M*m*rho^2*x6^2*sin(2*x5))/2 - (M*m*rho^2*x1*x6^2*sin(2*x5))/2))/((L0 + x1)*(2*M*m*rho^2 + J*m + 2*J*M - J*m*cos(2*x5)))
ans(t) =
0
0
0
Jacobian matrix, linearization
Linearization formula: $\dot x = F(x.u) \simeq F(x_0,u_0) + \frac{\partial F(x_0,u_0)}{\partial x} (x - x_0) + \frac{\partial F(x_0,u_0)}{\partial u} (u - u_0)$, where $F(x_0,u_0) = 0$ and $x_0 =0$ and $u_0 =0$.
Output:
DFx = [ 0, 1, 0, 0, 0, 0] [ (rho*(M^2*rho*x6^2 - m^2*rho*x6^2 + M*m*rho*x6^2 + m^2*rho*x6^2*cos(x5)^2 - M*m*rho*x6^2*cos(x5)^2))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2), 0, 0, 0, (2*J*m*rho*cos(x5)*sin(x5)*(M*tau + m*tau + g*m^2*rho - m*tau*cos(x5)^2 - g*m^2*rho*cos(x5)^2 + f*m*rho*sin(x5) - L0*M^2*rho*x6^2 + L0*m^2*rho*x6^2 - M^2*rho*x1*x6^2 + m^2*rho*x1*x6^2 + M*g*m*rho - m^2*rho*x1*x6^2*cos(x5)^2 - L0*M*m*rho*x6^2 - M*g*m*rho*cos(x5) - M*m*rho*x1*x6^2 - L0*m^2*rho*x6^2*cos(x5)^2 + M*m*rho*x1*x6^2*cos(x5)^2 + L0*M*m*rho*x6^2*cos(x5)^2))/(M*m*rho^2 - J*m*cos(x5)^2 + J*m + J*M)^2 - (rho*(2*m*tau*cos(x5)*sin(x5) + f*m*rho*cos(x5) + 2*g*m^2*rho*cos(x5)*sin(x5) + M*g*m*rho*sin(x5) + 2*L0*m^2*rho*x6^2*cos(x5)*sin(x5) + 2*m^2*rho*x1*x6^2*cos(x5)*sin(x5) - 2*L0*M*m*rho*x6^2*cos(x5)*sin(x5) - 2*M*m*rho*x1*x6^2*cos(x5)*sin(x5)))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2), (rho*(2*L0*M^2*rho*x6 - 2*L0*m^2*rho*x6 + 2*M^2*rho*x1*x6 - 2*m^2*rho*x1*x6 + 2*L0*M*m*rho*x6 + 2*M*m*rho*x1*x6 + 2*L0*m^2*rho*x6*cos(x5)^2 + 2*m^2*rho*x1*x6*cos(x5)^2 - 2*L0*M*m*rho*x6*cos(x5)^2 - 2*M*m*rho*x1*x6*cos(x5)^2))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2)] [ 0, 0, 0, 1, 0, 0] [ (sin(x5)*m^2*rho^2*x6^2 - M*sin(x5)*m*rho^2*x6^2 + J*sin(x5)*m*x6^2)/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2), 0, 0, 0, (g*m^2*rho^2*cos(x5) + m*rho*tau*cos(x5) + J*g*m*cos(2*x5) + J*L0*m*x6^2*cos(x5) + m^2*rho^2*x1*x6^2*cos(x5) + J*m*x1*x6^2*cos(x5) + L0*m^2*rho^2*x6^2*cos(x5) - M*m*rho^2*x1*x6^2*cos(x5) - L0*M*m*rho^2*x6^2*cos(x5))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2) - (2*J*m*cos(x5)*sin(x5)*(J*f + f*m*rho^2 + g*m^2*rho^2*sin(x5) + m*rho*tau*sin(x5) + (J*g*m*sin(2*x5))/2 + L0*m^2*rho^2*x6^2*sin(x5) + J*L0*m*x6^2*sin(x5) + m^2*rho^2*x1*x6^2*sin(x5) + J*m*x1*x6^2*sin(x5) - M*m*rho^2*x1*x6^2*sin(x5) - L0*M*m*rho^2*x6^2*sin(x5)))/(M*m*rho^2 - J*m*cos(x5)^2 + J*m + J*M)^2, (2*L0*m^2*rho^2*x6*sin(x5) + 2*J*L0*m*x6*sin(x5) + 2*m^2*rho^2*x1*x6*sin(x5) + 2*J*m*x1*x6*sin(x5) - 2*L0*M*m*rho^2*x6*sin(x5) - 2*M*m*rho^2*x1*x6*sin(x5))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2)] [ 0, 0, 0, 0, 0, 1] [ (2*(J*f*cos(x5) + J*m*x2*x6 + J*M*g*sin(x5) + J*g*m*sin(x5) + (g*m^2*rho^2*sin(2*x5))/2 + f*m*rho^2*cos(x5) + 2*J*M*x2*x6 + (m*rho*tau*sin(2*x5))/2 + M*g*m*rho^2*sin(x5) - J*m*x2*x6*cos(2*x5) + (L0*m^2*rho^2*x6^2*sin(2*x5))/2 + (J*L0*m*x6^2*sin(2*x5))/2 + (m^2*rho^2*x1*x6^2*sin(2*x5))/2 + (J*m*x1*x6^2*sin(2*x5))/2 + 2*M*m*rho^2*x2*x6 - (L0*M*m*rho^2*x6^2*sin(2*x5))/2 - (M*m*rho^2*x1*x6^2*sin(2*x5))/2))/((L0 + x1)^2*(2*M*m*rho^2 + J*m + 2*J*M - J*m*cos(2*x5))) - (2*((sin(2*x5)*m^2*rho^2*x6^2)/2 - (M*sin(2*x5)*m*rho^2*x6^2)/2 + (J*sin(2*x5)*m*x6^2)/2))/((L0 + x1)*(2*M*m*rho^2 + J*m + 2*J*M - J*m*cos(2*x5))), -(2*(2*M*m*x6*rho^2 + 2*J*M*x6 + J*m*x6 - J*m*x6*cos(2*x5)))/((L0 + x1)*(2*M*m*rho^2 + J*m + 2*J*M - J*m*cos(2*x5))), 0, 0, (4*J*m*sin(2*x5)*(J*f*cos(x5) + J*m*x2*x6 + J*M*g*sin(x5) + J*g*m*sin(x5) + (g*m^2*rho^2*sin(2*x5))/2 + f*m*rho^2*cos(x5) + 2*J*M*x2*x6 + (m*rho*tau*sin(2*x5))/2 + M*g*m*rho^2*sin(x5) - J*m*x2*x6*cos(2*x5) + (L0*m^2*rho^2*x6^2*sin(2*x5))/2 + (J*L0*m*x6^2*sin(2*x5))/2 + (m^2*rho^2*x1*x6^2*sin(2*x5))/2 + (J*m*x1*x6^2*sin(2*x5))/2 + 2*M*m*rho^2*x2*x6 - (L0*M*m*rho^2*x6^2*sin(2*x5))/2 - (M*m*rho^2*x1*x6^2*sin(2*x5))/2))/((L0 + x1)*(2*M*m*rho^2 + J*m + 2*J*M - J*m*cos(2*x5))^2) - (2*(J*M*g*cos(x5) - J*f*sin(x5) + J*g*m*cos(x5) + g*m^2*rho^2*cos(2*x5) + m*rho*tau*cos(2*x5) - f*m*rho^2*sin(x5) + M*g*m*rho^2*cos(x5) + 2*J*m*x2*x6*sin(2*x5) + L0*m^2*rho^2*x6^2*cos(2*x5) + J*L0*m*x6^2*cos(2*x5) + m^2*rho^2*x1*x6^2*cos(2*x5) + J*m*x1*x6^2*cos(2*x5) - L0*M*m*rho^2*x6^2*cos(2*x5) - M*m*rho^2*x1*x6^2*cos(2*x5)))/((L0 + x1)*(2*M*m*rho^2 + J*m + 2*J*M - J*m*cos(2*x5))), -(2*(2*J*M*x2 + J*m*x2 + 2*M*m*rho^2*x2 - J*m*x2*cos(2*x5) + L0*m^2*rho^2*x6*sin(2*x5) + J*L0*m*x6*sin(2*x5) + m^2*rho^2*x1*x6*sin(2*x5) + J*m*x1*x6*sin(2*x5) - M*m*rho^2*x1*x6*sin(2*x5) - L0*M*m*rho^2*x6*sin(2*x5)))/((L0 + x1)*(2*M*m*rho^2 + J*m + 2*J*M - J*m*cos(2*x5)))] DFu = [ 0, 0] [ -(m*rho^2*sin(x5))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2), -(rho*(M + m - m*cos(x5)^2))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2)] [ 0, 0] [ (m*rho^2 + J)/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2), (m*rho*sin(x5))/(J*m + J*M - J*m*cos(x5)^2 + M*m*rho^2)] [ 0, 0] [ -(2*(m*cos(x5)*rho^2 + J*cos(x5)))/((L0 + x1)*(2*M*m*rho^2 + J*m + 2*J*M - J*m*cos(2*x5))), -(m*rho*sin(2*x5))/((L0 + x1)*(2*M*m*rho^2 + J*m + 2*J*M - J*m*cos(2*x5)))]
DFx = jacobian(Fxu,x)
DFu = jacobian(Fxu,u)
A_lin = simplify(subs(DFx,[x;u],zeros(8,1)))
B_lin = simplify(subs(DFu,[x;u],zeros(8,1)))
pcz_latex(A_lin), pcz_latex(B_lin)
Output:
A_lin =
[ 0, 1, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 1, 0, 0]
[ 0, 0, 0, 0, (g*m)/M, 0]
[ 0, 0, 0, 0, 0, 1]
[ 0, 0, 0, 0, -(g*(M + m))/(L0*M), 0]
B_lin =
[ 0, 0]
[ 0, -rho/(m*rho^2 + J)]
[ 0, 0]
[ 1/M, 0]
[ 0, 0]
[ -1/(L0*M), 0]
A_lin = \left(\begin{array}{cccccc} 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & \frac{gm}{M} & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & -\frac{g(M+m)}{L_{0}M} & 0 \end{array}\right)
B_lin = \left(\begin{array}{cc} 0 & 0 \\ 0 & -\frac{\rho }{m\rho ^2+J} \\ 0 & 0 \\ \frac{1}{M} & 0 \\ 0 & 0 \\ -\frac{1}{L_{0}M} & 0 \end{array}\right)
g = 9.82;
params = [m M rho J R0 L0];
u = [f ; tau];
V = subs(V);
matlabFunction(V,'vars',{'Y',u,params},'File','crane_model')
Output:
ans =
function_handle with value:
@crane_model
Simulate
m = 4;
M = 1.6;
J = 2e-4;
rho = 0.04;
L0 = 1.2;
R0 = 0.8;
g = 9.82;
V = subs(V);
V_fh = matlabFunction(V,'vars', {'t','Y',f,tau});
V_ode = @(t,x) V_fh(t,x,0,0);
ode45(V_ode, [0 4], [0 0 0 0 0.1 0]')
A = subs(A_lin), A = double(A);
B = subs(B_lin), B = double(B);
Output:
A = [ 0, 1, 0, 0, 0, 0] [ 0, 0, 0, 0, 0, 0] [ 0, 0, 0, 1, 0, 0] [ 0, 0, 0, 0, 491/20, 0] [ 0, 0, 0, 0, 0, 1] [ 0, 0, 0, 0, -3437/120, 0] B = [ 0, 0] [ 0, -200/33] [ 0, 0] [ 5/8, 0] [ 0, 0] [ -25/48, 0]
Quick LQR design
Output:
[ 0 , 0 , 3.1623 , 6.7813 , -0.8826 , 1.629 ; -3.1623 , -3.3232 , 0 , 0 , 0 , 0 ]
K = lqr(A,B,10*eye(6),eye(2));
pcz_num2str(K)
Extract subexpression (not very useful).
Output:
V1 =
Y[2]
sigma1*((28*tau)/125 - (27496*cos(Y[5]))/78125 + (96*sin(Y[5])^2*Y[6]^2)/3125 + (3928*cos(Y[5])*sin(Y[5])^2)/15625 - (1344*Y[6]^2)/78125 - (224*Y[1]*Y[6]^2)/15625 - (4*tau*cos(Y[5])^2)/25 - (3928*cos(Y[5])^2)/15625 + (3928*cos(Y[5])^3)/15625 + (4*f*sin(Y[5]))/625 + (192*cos(Y[5])^2*Y[6]^2)/15625 + (32*cos(Y[5])^2*Y[1]*Y[6]^2)/3125 + (16*sin(Y[5])^2*Y[1]*Y[6]^2)/625 + 27496/78125)
Y[4]
-sigma1*((33*f)/5000 + (3928*sin(Y[5]))/15625 + (303*sin(Y[5])*Y[6]^2)/15625 + (491*cos(Y[5])*sin(Y[5]))/62500 + (4*tau*sin(Y[5]))/25 + (101*sin(Y[5])*Y[1]*Y[6]^2)/6250)
Y[6]
(sigma1*((113421*sin(Y[5]))/312500 - (3928*cos(Y[5])^2*sin(Y[5]))/15625 - (3928*sin(Y[5])^3)/15625 + (3928*cos(Y[5])*sin(Y[5]))/15625 + (231*Y[2]*Y[6])/3125 + (33*f*cos(Y[5]))/5000 + (4*tau*cos(Y[5])*sin(Y[5]))/25 - (33*cos(Y[5])^2*Y[2]*Y[6])/625 - (32*sin(Y[5])^2*Y[2]*Y[6])/625 + (303*cos(Y[5])*sin(Y[5])*Y[6]^2)/15625 + (101*cos(Y[5])*sin(Y[5])*Y[1]*Y[6]^2)/6250))/(Y[1] + 6/5)
sigma1 =
1/((33*cos(Y[5])^2)/1250 + (16*sin(Y[5])^2)/625 - 231/6250)
[V1,sigma1] = subexpr(V,'sigma1')