( ) 1 / 82
1 2 3 4 5 6 ( ) 2 / 82
( ) 3 / 82
C θ l y m O x mg λ ( ) 4 / 82
θ t C J = ml 2 C mgl sin θ θ C J θ = mgl sin θ = θ ( ) 5 / 82
ω = θ J ω = mgl sin θ ω J = ml 2 θ = ω, ω = g l sin θ = θ ω ( ) 6 / 82
θ = ω, ω = g l sin θ θ ω θ ω ( ) 7 / 82
x = [ f (x, t) = [ θ ω ] ω g sin θ l ] θ = ω, ω = g l sin θ ẋ = f (x, t) ( ) 8 / 82
Step 1 Step 2 ( ) ( ) 9 / 82
MATLAB pendulumconstants.m classdef pendulumconstants % properties (Constant) gravity = 9.8; % g length = 2.0; % l end end ( ) 10 / 82
MATLAB dotpendulum.m function dotq = dotpendulum(t,q) % % % theta = q(1); omega = q(2); dottheta = omega; dotomega = -(pendulumconstants.gravity/... pendulumconstants.length)*sin(theta); dotq = [dottheta; dotomega]; end ( ) 11 / 82
MATLAB pendulum.m pendulumconstants; % timeinterval=0:0.1:10; % ( 0.1s) q0=[pi/3;0]; % [time,q]=ode45( dotpendulum,timeinterval,q0); plot(time,q(:,1),time,q(:,2), -- ); % ( ) 12 / 82
MATLAB >> pendulum 2.5 2 1.5 1 0.5 0-0.5-1 -1.5-2 -2.5 0 1 2 3 4 5 6 7 8 9 10 time ( ) 13 / 82
0 0.1000 0.2000 0.3000 0.4000 0.5000 0.6000 0.7000 0.8000 0.9000 1.0000 ( ) 14 / 82 MATLAB >> time time =
1.0472 0 1.0260-0.4226 0.9630-0.8343 0.8600-1.2228 0.7199-1.5715 0.5476-1.8619 0.3500-2.0750 0.1356-2.1942-0.0852-2.2071-0.3019-2.1117-0.5043-1.9174 ( ) 15 / 82 MATLAB >> q q =
k m b f(t) mẍ = bẋ kx + f (t) ( ) 16 / 82
v = ẋ m v = bv kx + f (t) ẋ = v m v = bv kx + f (t) = x v ( ) 17 / 82
LCR L V(t) C R V (t) L i 1 C t 0 i(τ) dτ Ri = 0 ( ) 18 / 82
LCR q(t) = t 0 q = i i(τ) dτ V (t) L i 1 C q Ri = 0 q = i L i = Ri 1 C q + V (t) = q i ( ) 19 / 82
l2 y lc2 link 2 joint 1 lc1 l1 θ1 θ2 joint 2 link 1 x ( ) 20 / 82
H 11 θ1 + H 12 θ2 = h 12 θ2 2 + 2h 12 θ1 θ2 G 1 G 12 + τ 1, H 22 θ2 + H 12 θ1 = h 12 θ2 1 G 12 + τ 2 H 11 = J 1 + m 1 l 2 c1 + J 2 + m 2 (l 2 1 + l 2 c2 + 2l 1 l c2 cos θ 2 ) H 12 = J 2 + m 2 (l 2 c2 + l 1 l c2 cos θ 2 ) H 22 = J 2 + m 2 l 2 c2 h 12 = m 2 l 1 l c2 sin θ 2 G 1 = (m 1 l c1 + m 2 l 1 ) g cos θ 1 G 12 = m 2 l c2 g cos(θ 1 + θ 2 ) ( ) 21 / 82
ω 1 = θ1 ω 2 = θ2 [ H11 H 12 H 12 H 22 [ ] θ1 θ 2 ] [ ] ω1 ω 2 = = [ ω1 ω 2 ], [ h12 ω 2 2 + 2h 12 ω 1 ω 2 G 1 G 12 + τ 1 h 12 ω 2 1 G 12 + τ 2 ] θ 1, θ 2, ω 1, ω 2 = = θ 1, θ 2, ω 1, ω 2 ( ) 22 / 82
x = θ 1 θ 2 ω 1 ω 2 ẋ = f (x, t) x θ1 θ2 ω1 ω2 solve linear equations θ1 θ2 ω1 ω2 x t ( ) 23 / 82
( ) 24 / 82
(a) ẋ = 3 x (b) ẋ 2 = 9x (c) ẋ 2 = 9x (ẋ 0) { ẋ + ẏ = x (d) ẋ ẏ = y { ẋ 2ẏ = x (e) 2ẋ + 4ẏ = y 2ẋ + p = x (f) 2ẏ + 3p = y ẋ + 3ẏ = x y ( ) 25 / 82
(1 ) ẋ = f (x, t) t n = nt (n = 0, 1, 2, ) x T ( ) 26 / 82
(Euler method) (Heun method) (Runge-Kutta method) t n x x n = x(t n ) t n+1 x x n+1 = x(t n+1 ) x 0 = x(0) x n = x(nt ) (n = 1, 2, ) ( ) 27 / 82
dx/dt = 2x x(0) = 1.00 0.1 t x 0.000000 1.000000 0.100000 0.818567 0.200000 0.670052 0.300000 0.548482 0.400000 0.448969 0.500000 0.367511 0.600000 0.300833 0.700000 0.246252 0.800000 0.201573 0.900000 0.165001 1.000000 0.135065 ( ) 28 / 82
CG ( ) ( ) 29 / 82
x n+1 = x n + Tf (x n, t n ) 1 x t (x n, t n ) f (x, t) x n+1 = x((n + 1)T ) = x(nt + T ) = x(t n + T ) x n+1 = x(t n ) + ẋ(t n )T = x(t n ) + f (x(t n ), t n )T = x n + Tf (x n, t n ) ( ) 30 / 82
x Euler method xn+tk1 xn k1=f(xn,tn) O tn tn+1 t ( ) 31 / 82
x n+1 = x n + T 2 (k 1 + k 2 ), k 1 = f (x n, t n ), k 2 = f (x n + Tk 1, t n + T ) 2 (x n, t n ) (x n + Tk 1, t n + T ) f (x, t) ( ) 32 / 82
x Heun method k2 xn+tk1 Euler method xn k1 O tn tn+1 t k 1 < k 2 ẍ > 0 ( ) 33 / 82
x xn+tk1 xn Euler method k2 k1 Heun method O tn tn+1 k 1 > k 2 ẍ < 0 t ( ) 34 / 82
x n+1 = x(t n + T ) ẋ = f (x, t) x n+1 = x(t n ) + ẋ(t n )T + 1 2ẍ(t n)t 2 ẍ = f dx x dt + f dt t dt = f x ẋ + f t = f xf + f t x n+1 = x n + f (x n, t n )T + 1 2 (f xf + f t ) T 2 ( ) 35 / 82
k 2 k 2 = f (x n + Tk 1, t n + T ) = f (x n, t n ) + f x Tk 1 + f t T = f + (f x f + f t )T x n + T 2 (k 1 + k 2 ) = x n + T 2 (f + f + (f xf + f t )T ) = x n + ft + 1 2 (f xf + f t ) T 2 ( ) 36 / 82
x n+1 = x n + T 6 (k 1 + 2k 2 + 2k 3 + k 4 ), k 1 = f (x n, t n ), k 2 = f (x n + 1 2 Tk 1, t n + 1 2 T ), k 3 = f (x n + 1 2 Tk 2, t n + 1 2 T ), k 4 = f (x n + Tk 3, t n + T ) 4 x t f (x, t) ( ) 37 / 82
xn+ xn+ T 2 T 2 x k1 k2 xn k1 k2 k3 xn+tk3 k4 O tn tn+ T 2 tn+1 t ( ) 38 / 82
1 2 3 4 5 6 7 8 9 10 1 2 3 4 4 5 6 6 7 7 1 2 4 5 ( ) 39 / 82
( ) ẋ = f (x, t) x = x f = f k = k ( ) 40 / 82
MATLAB pendulum.m pendulumconstants; % timeinterval=0:0.1:10; % ( 0.1s) q0=[pi/3;0]; % [time,q]=ode45( dotpendulum,timeinterval,q0); plot(time,q(:,1),time,q(:,2), -- ); % ( ) 41 / 82
MATLAB >> pendulum 2.5 2 1.5 1 0.5 0-0.5-1 -1.5-2 -2.5 0 1 2 3 4 5 6 7 8 9 10 time ( ) 42 / 82
0 0.1000 0.2000 0.3000 0.4000 0.5000 0.6000 0.7000 0.8000 0.9000 1.0000 ( ) 43 / 82 MATLAB >> time time =
1.0472 0 1.0260-0.4226 0.9630-0.8343 0.8600-1.2228 0.7199-1.5715 0.5476-1.8619 0.3500-2.0750 0.1356-2.1942-0.0852-2.2071-0.3019-2.1117-0.5043-1.9174 ( ) 44 / 82 MATLAB >> q q =
( ) 45 / 82
x O t ( ) 46 / 82
6 k 1 = f (x n, t n ), k 2 = f (x n + T 4 k 1, t n + 1 4 T ), k 3 = f (x n + T 32 (3k 1 + 9k 2 ), t n + 3 8 T ), k 4 = f (x n + T 2179 (1932k 1 7200k 2 + 7296k 3 ), t n + 12 13 T ), k 5 = f (x n + T ( 439 216 k 1 8k 2 + 3680 513 k 3 845 4104 k 4), t n + T ), k 6 = f (x n + T ( 8 27 k 1 + 2k 2 3544 2565 k 3 + 1859 4104 k 4 11 40 k 5), t n + 1 2 T ) ( ) 47 / 82
5 x n+1 = x n + T ( 16 135 k 1 + 6656 12825 k 3 + 28561 56430 k 4 9 50 k 5 + 2 55 k 6) 4 x n+1 = x n + T ( 25 216 k 1 + 1408 2565 k 3 + 2197 4104 k 4 1 5 k 5) x n+1 xn+1 x n+1 xn+1 ( ) 48 / 82
Step 1 Step 2 Step 3 5 x n+1 4 xn+1 ˆT { ˆT = αt ϵ x n+1 x n+1 } 1 5 Step 4 ϵ α 0.8 0.9 T ˆT ( ) 49 / 82
MATLAB pendulumvar.m pendulumconstants; timeinterval=[0,10]; % q0=[pi/3;0]; [time,q]=ode45( dotpendulum,timeinterval,q0); plot(time,q(:,1),time,q(:,2), -- ); ( ) 50 / 82
MATLAB >> pendulumvar 2.5 2 1.5 1 0.5 0-0.5-1 -1.5-2 -2.5 0 1 2 3 4 5 6 7 8 9 10 time ( ) 51 / 82
0 0.0000 0.0000 0.0000 0.0000 0.0001 0.0002 0.0002 0.0003 0.0006 0.0009 ( ) 52 / 82 MATLAB >> time time =
1.0472 0 1.0472-0.0001 1.0472-0.0001 1.0472-0.0002 1.0472-0.0002 1.0472-0.0005 1.0472-0.0007 1.0472-0.0010 1.0472-0.0012 1.0472-0.0025 1.0472-0.0037 ( ) 53 / 82 MATLAB >> q q =
m t x(t) ( k) mẍ = f collision mg, { kx x < 0 f collision = 0 otherwise m = 1 k = 100 g = 9.8 x(0) = 100 ẋ(0) = 0 ( ) 54 / 82
MATLAB collideconstants.m classdef collideconstants % properties (Constant) gravity = 9.8; mass = 1.0; spring = 100.0; end end ( ) 55 / 82
MATLAB dotcollide.m function dotq = dotcollide(t,q) % x = q(1); v = q(2); dotx = v; if x < 0 dotv = -collideconstants.gravity... -collideconstants.spring*x/collideconstants.mass; else dotv = -collideconstants.gravity; end dotq = [dotx; dotv]; end ( ) 56 / 82
MATLAB collide.m collideconstants; timeinterval=[0,50]; % q0=[100.0;0.0]; % options = odeset( RelTol,1e-8, AbsTol,[1e-8 1e-10]); [time,q]=ode45( dotcollide,timeinterval,q0,options); plot(time,q(:,1),time,q(:,2), -- ); ( ) 57 / 82
MATLAB 100 50 0-50 0 5 10 15 20 25 30 35 40 45 50 time x(t) ẋ(t) ( ) 58 / 82
MATLAB 1.4 1.2 1 time interval 0.8 0.6 0.4 0.2 0 0 5 10 15 20 25 30 35 40 45 50 time T ( ) 58 / 82
( ) 59 / 82
( ) C R<0 y l R=0 m R>0 O x mg λ ( ) 60 / 82
( ) t (x, y) C l R(x, y) = { x 2 + (y l) 2} 1 2 l = 0 R(x, y) [R x, R y ] T R x (x, y) = R x = x { x 2 + (y l) 2} 1 R y (x, y) = R y = (y l) { x 2 + (y l) 2} 1 R(x, y) = 0 ( ) 61 / 82 2, 2.
( ) [R x, R y ] T λ mẍ = λ R x (x, y), mÿ = λ R y (x, y) mg R(x, y) = 0 ( ) 62 / 82
( ) v x = ẋ vy = ẏ ẋ = v x, ẏ = v y, m v x = λ R x (x, y), m v y = λ R y (x, y) mg, R(x, y) = 0 x y v x v y λ 4 1 ( ) 63 / 82
joint 2 θ2 tip point link 2 l2 joint 5 l4 link 4 joint 4 θ4 y O x l1 link 1 θ1 joint 1 (x1,y1) link 3 joint 3 (x3,y3) θ3 l3 ( ) 64 / 82
joint 2 θ2 tip point link 2 l2 xleft [ yleft ] xright [ yright] link 4 tip point l4 joint 4 θ4 link 1 joint 1 l1 θ1 link 3 joint 3 θ3 l3 (x1,y1) (x3,y3) ( ) 65 / 82
[ ] [ ] x1 C1 + l y 1 1 S 1 [ ] [ ] x3 C3 + l y 3 3 C 3 + l 2 [ C1+2 S 1+2 + l 4 [ C3+4 S 3+4 ] ] P(x, y) = l 1 C 1 + l 2 C 1+2 l 3 C 3 l 4 C 3+4 + x 1 x 3 = 0 Q(x, y) = l 1 S 1 + l 2 S 1+2 l 3 S 3 l 4 S 3+4 + y 1 y 3 = 0 ( ) 66 / 82
H 11 ω 1 + H 12 ω 2 = f 1 + λ x ( l 1 S 1 l 2 S 1+2 ) + λ y (l 1 C 1 + l 2 C 1+2 ), H 22 ω 2 + H 12 ω 1 = f 2 + λ x ( l 2 S 1+2 ) + λ y l 2 C 1+2, H 33 ω 3 + H 34 ω 4 = f 3 + λ x (l 3 S 3 + l 2 S 3+4 ) + λ y ( l 3 C 3 l 4 C 3+4 ), H 44 ω 4 + H 34 ω 3 = f 4 + λ x l 4 S 3+4 + λ y ( l 4 C 3+4 ) f 1 = h 12 ω2 2 + 2h 12 ω 1 ω 2 G 1 G 12 + τ 1, f 2 = h 12 ω1 2 G 12, f 3 = h 34 ω4 2 + 2h 34 ω 3 ω 4 G 3 G 34 + τ 3, f 4 = h 34 ω3 2 G 34 ( ) 67 / 82
θ 1 = ω 1, θ2 = ω 2, θ3 = ω 3, θ4 = ω 4, H 11 ω 1 + H 12 ω 2 = f 1 + λ x ( l 1 S 1 l 2 S 1+2 ) + λ y (l 1 C 1 + l 2 C 1+2 ), H 22 ω 2 + H 12 ω 1 = f 2 + λ x ( l 2 S 1+2 ) + λ y l 2 C 1+2, H 33 ω 3 + H 34 ω 4 = f 3 + λ x (l 3 S 3 + l 2 S 3+4 ) + λ y ( l 3 C 3 l 4 C 3+4 ), H 44 ω 4 + H 34 ω 3 = f 4 + λ x l 4 S 3+4 + λ y ( l 4 C 3+4 ), P(x, y) = 0, Q(x, y) = 0 θ 1, θ 2, θ 3, θ 4 ω 1, ω 2, ω 3, ω 4 λ x λ y 8 2 ( ) 68 / 82
R = 0 R + 2νṘ + ν2 R = 0 ν R = ( ) R 0 = R = 0 ( ) 69 / 82
R(x, y) = 0 R(x, y) Ṙ R x (x, y) R y (x, y) Ṙ = R dx x dt + R y = R x ẋ + R y ẏ Ṙ x = R x dx x dt + R x y = R xx ẋ + R xy ẏ Ṙ y = R y dx x dt + R y y = R yx ẋ + R yy ẏ ( ) 70 / 82 dy dt dy dt dy dt
R(x, y) R R = Ṙxẋ + R x ẍ + Ṙyẏ + R y ÿ = R x ẍ + R y ÿ + (R xx ẋ + R xy ẏ)ẋ + (R yx ẋ + R yy ẏ)ẏ = R x ẍ + R y ÿ + [ ẋ ẏ ] [ ] [ ] R xx R xy ẋ R yx R yy ẏ R + 2νṘ + ν 2 R = 0 R x ẍ R y ÿ = C(x, y, ẋ, ẏ) C(x, y, ẋ, ẏ) = [ ẋ ẏ ] [ R xx R xy R yx R yy ] [ ẋ ẏ ] + 2ν(R x ẋ + R y ẏ) + ν 2 R ( ) 71 / 82
( ) mẍ = λ R x (x, y), mÿ = λ R y (x, y) mg, R(x, y) = 0 ẋ = v x, ẏ = v y, m v x λ R x (x, y) = 0, x y v x v y λ 5 m v y λ R y (x, y) = mg, R x v x R y v y = C(x, y, v x, v y ) ( ) 72 / 82
P(x, y) = x 2 + (y l) 2 R(x, y) = P 1 2 l, R x (x, y) = R x = 1 xp R y (x, y) R xx (x, y) R yy (x, y) R xy (x, y) = R yx (x, y) = R y = 2 R x 2 2, 1 = (y l)p 2, = P 1 2 x 2 P 3 2, = 2 R y = P 1 2 2 (y l) 2 P 3 2, = 2 R x y 3 = x(y l)p 2. ( ) 73 / 82
m v x λ R x (x, y) = 0, m v y λ R y (x, y) = mg, R x v x R y v y = C(x, y, v x, v y ) m 0 R x 0 m R y R x R y 0 v x v y λ = 0 mg C(x, y, v x, v y ) x, y, v x, v y = = v x, v y ( ) 74 / 82
q = x y v x v y q = f (q, t) x x y q y vx vy solve linear equations vx vy q t ( ) 75 / 82
MATLAB m = 0.01 l = 2 g = 9.8 θ(0) = (π/3) ω(0) = 0 x(0) = l sin θ(0) y(0) = l(1 cos θ(0)) v x (0) = 0 v y (0) = 0 ( ) 76 / 82
MATLAB dotpendulumcartesian.m function dotq = dotpendulumcartesian (t,q) % x = q(1); y = q(2); vx = q(3); vy = q(4); nu = pendulumcartesianconstants.nu; mass = pendulumcartesianconstants.mass; gravity = pendulumcartesianconstants.gravity; [R,Rx,Ry,Rxx,Ryy,Rxy] = calculate_r_derivatives(x,y); C = Rxx*vx*vx + Ryy*vy*vy + 2*Rxy*vx*vy +... 2*nu*(Rx*vx + Ry*vy) + nu*nu*r; ( ) 77 / 82
MATLAB A = [ mass, 0, -Rx;... 0, mass, -Ry;... -Rx, -Ry, 0 ]; b = [ 0; -mass*gravity; C ]; d = A\b; dotvx = d(1); dotvy = d(2); dotq = [vx; vy; dotvx; dotvy]; end ( ) 78 / 82
MATLAB calculate R derivatives.m function [ R, Rx, Ry, Rxx, Ryy, Rxy ] =... calculate_r_derivatives( x, y ) % R, R_x, R_y, R_xx, R_yy, R_xy length = pendulumcartesianconstants.length; P = x*x + (y-length)*(y-length); Psqrt = sqrt(p); R = Psqrt - length; Rx = x/psqrt; Ry = (y-length)/psqrt; Rxx = 1/Psqrt - x*x/p/psqrt; Ryy = 1/Psqrt - (y-length)*(y-length)/p/psqrt; Rxy = -x*(y-length)/p/psqrt; end ( ) 79 / 82
MATLAB pendulumcartesian.m pendulumconstants; %timeinterval=0:0.1:10; % timeinterval=[0,10]; % length = pendulumcartesianconstants.length; theta0 = pi/3; q0=[length*sin(theta0); length*(1-cos(theta0)); 0; 0]; [time,q]=ode45( dotpendulumcartesian,timeinterval,q0); plot(time,q(:,1),time,q(:,2), -- ); ( ) 80 / 82
MATLAB 2 1.5 1 0.5 position 0-0.5-1 -1.5-2 0 1 2 3 4 5 6 7 8 9 10 time x, y ( ) 81 / 82
MATLAB 5 4 3 2 1 velocity 0-1 -2-3 -4-5 0 1 2 3 4 5 6 7 8 9 10 time v x, v y ( ) 81 / 82
MATLAB 1.5 1 0.5 angle 0-0.5-1 -1.5 0 1 2 3 4 5 6 7 8 9 10 time θ ( ) 81 / 82
MATLAB 10-3 1 0.8 0.6 0.4 constraint 0.2 0-0.2-0.4-0.6-0.8-1 0 1 2 3 4 5 6 7 8 9 10 time R ( ) 81 / 82
ẋ = f (x, t) ( ) 82 / 82