数値計算：常微分方程式

(1)

平井慎一

立命館大学ロボティクス学科

(2)

講義の流れ

1 常微分方程式の標準型 2 オイラー法，ホイン法，ルンゲクッタ法 3 ルンゲクッタフェールベルグ法 4 制約を有する常微分方程式 5 制約安定化法 6 まとめ

(3)

講義の目標

講義の内容常微分方程式の標準型オイラー法，ホイン法，ルンゲクッタ法ルンゲクッタフェールベルグ法講義の目標常微分方程式の標準型を理解する標準型へ変換することができる平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 3 / 82

(4)

常微分方程式の標準型

単振り子

m x mg C l λ θ y O

(5)

単振り子

θ 時刻 t における単振り子の振れ角 支点 C まわりの慣性モーメント J = ml2 重力により支点 C まわりに作用するモーメント −mgl sin θ 振れ角の角加速度 _θ¨ 支点 C まわりの回転に関する運動方程式 J ¨θ = −mgl sin θ =⇒ 変数 θ に関する二階の常微分方程式 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 5 / 82

(6)

単振り子

運動方程式を一階の微分方程式に変換 新しい変数 ω= ˙△ θ を導入 J ˙ω =−mgl sin θ 新しい変数 ω の定義式と上式の両辺を J = ml2_{で割った式} ˙ θ = ω, ˙ ω = −g l sin θ =⇒ 二つの変数 θ と ω に関する一階の微分方程式

(7)

標準型

常微分方程式の標準型 ˙ θ = ω, ˙ ω = −g l sin θ 変数 θ と ω の値を与える． ⇓ 時間微分 ˙θ と ˙ω の値を上式より計算することができる． 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 7 / 82

(8)

標準型

状態変数ベクトル x =△ [ θ ω ] ベクトル値関数 f (x , t)=△ [ ω −g_l sin θ ] 常微分方程式の標準型 ˙ θ = ω, ˙ ω = −g l sin θ ⇓ ˙ x = f (x , t)

(9)

標準型

常微分方程式を数値的に解く Step 1 常微分方程式を標準型に変換 Step 2 変換した標準型をアルゴリズムに適用 (ルンゲクッタ法，ルンゲクッタフェールベルグ法など) 標準型に変換すれば，既存のソフトウェアを用いることができる平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 9 / 82

(10)

MATLAB

ファイル pendulumConstants.m classdef pendulumConstants % 単振り子のパラメータ properties (Constant) gravity = 9.8; % g length = 2.0; % l end end

(11)

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

(12)

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),’--’); % グラフ表示

(13)

MATLAB

>> pendulum time 0 1 2 3 4 5 6 7 8 9 10 -2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 13 / 82

(14)

MATLAB

>> time time = 0 0.1000 0.2000 0.3000 0.4000 0.5000 0.6000 0.7000 0.8000 0.9000 1.0000 1.1000 1.2000 1.3000 1.4000 1.5000 1.6000 1.7000 1.8000 1.9000 2.0000 2.1000 2.2000 2.3000 2.4000 2.5000 2.6000 2.7000 2.8000 2.9000 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 14 / 82

(15)

MATLAB

>> q q = 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 -0.6829 -1.6427 -0.8308 -1.3055 -0.9426 -0.9245 -1.0148 -0.5167 -1.0457 -0.0958 -1.0341 0.3280 -0.9803 0.7434 -0.8859 1.1377 -0.7538 1.4971 -0.5883 1.8028 -0.3958 2.0341 -0.1845 2.1743 0.0358 2.2111 0.2543 2.1410 0.4604 1.9689 0.6450 1.7089 0.8001 1.3834 0.9202 1.0113 1.0014 0.6085 1.0413 0.1894 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 15 / 82

(16)

質点・バネ・ダンパー系

k b m f(t) 運動方程式 m¨x =−b ˙x − kx + f (t)

(17)

質点・バネ・ダンパー系

新しい変数 v = ˙△ x 運動方程式 m ˙v =−bv − kx + f (t) ⇓ ˙ x = v m ˙v = −bv − kx + f (t) =⇒ 二つの変数 x と v に関する一階の微分方程式 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 17 / 82

(18)

LCR

回路

R L C V(t) 回路方程式 V (t)− L˙i − 1 C ∫ t 0 i (τ ) dτ − Ri = 0

(19)

LCR

回路

新しい変数 q(t)=△ ∫ t 0 i (τ ) dτ 上式を時間微分 ˙ q = i 回路方程式 V (t)− L˙i − 1 Cq− Ri = 0 ⇓ ˙ q = i L ˙i = −Ri − 1 Cq + V (t) =⇒ 二つの変数 q と i に関する一階の微分方程式 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 19 / 82

(20)

ロボットアーム

θ1 θ2 l1 l2 link 1 link 2 joint 1 joint 2 lc2 lc1 x y

(21)

ロボットアーム

ロボットアームの運動方程式 H11θ¨1+ H12θ¨2 = h12θ˙22+ 2h12θ˙1θ˙2 − G1− G12+ τ1, H22θ¨2+ H12θ¨1 =−h12θ˙12− G12+ τ2 ただし H11= J1+ m1lc12 + J2+ m2(l12+ l 2 c2+ 2l1lc2cos θ2) H12= J2+ m2(lc22 + l1lc2cos θ2) H22= J2+ m2lc22 h12= m2l1lc2sin θ2 G1 = (m1lc1+ m2l1) g cos θ1 G12 = m2lc2g cos(θ1+ θ2) 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 21 / 82

(22)

ロボットアーム

ω1 △ = ˙θ1，ω2 △ = ˙θ2 常微分方程式の標準型 [ _˙ θ1 ˙ θ2 ] = [ ω1 ω2 ] , [ H11 H12 H12 H22 ] [ ˙ ω1 ˙ ω2 ] = [ h12ω22 + 2h12ω1ω2− G1− G12+ τ1 −h12ω2 1− G12+ τ2 ] 変数 θ1, θ2, ω1, ω2の値を与える． =⇒ 上式左辺の行列の要素と右辺のベクトルの要素の値を計算． =⇒ 連立一次方程式を解き時間微分 ˙θ1, ˙θ2, ˙ω1, ˙ω2の値を計算する．

(23)

ロボットアーム

状態変数ベクトル x =△     θ1 θ2 ω1 ω2     常微分方程式の標準型 ˙ x = f (x , t) θ1 θ2 ω1 ω2 x θ1 θ2 ω1 ω2 ∙ ∙ ∙ ∙ x∙ solve linear equations t 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 23 / 82

(24)

常微分方程式の標準型

状態変数に関する一階の微分方程式

状態変数の値を与えると，その時間微分の値を計算できる．

(25)

常微分方程式の標準型

標準形か否かを判定せよ． (a) x = 3˙ √x (b) x˙2 _{= 9x} (c) x˙2 _{= 9x} _{( ˙}_x _{≤ 0)} (d) { ˙ x + ˙y = x ˙ x− ˙y = y (e) { ˙ x− 2 ˙y = x −2 ˙x + 4 ˙y = y (f)    2 ˙x + p = x 2 ˙y + 3p = y ˙ x + 3 ˙y = x− y 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 25 / 82

(26)

オイラー法，ホイン法，ルンゲクッタ法

常微分方程式の数値解法

常微分方程式 (1 変数) の標準型 ˙ x = f (x , t) を数値的に解く．微分方程式を数値的に解く 離散的な時刻 tn = nT (n = 0, 1, 2,· · · ) における x の値を求める． ステップ幅 T ：時間間隔を表す定数

(27)

常微分方程式の数値解法

オイラー法 (Euler method) ホイン法 (Heun method) ルンゲクッタ法 (Runge-Kutta method) 時刻 tnにおける x の値 xn = x (tn) ⇓ 時刻 tn+1における x の値 xn+1 = x (tn+1) を計算する漸化式 常微分方程式の初期値 x0 = x (0) から始めて漸化式を繰り返し適用 順次 xn= x (nT ) (n = 1, 2,· · · ) の値を求める． 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 27 / 82

(28)

常微分方程式の数値解法

微分方程式 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

(29)

常微分方程式の数値解法

長所解析的に解けない微分方程式を解くことができるシミュレーション（力学，回路，CG）短所方法によって解が異なるステップ幅によって解が異なるステップ幅が大きい計算した解が誤っているステップ幅が小さい計算時間が要するステップ幅を小さく (たとえば半分) にして解が一致 ⇓ もとのステップ幅で計算してよい．平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 29 / 82

(30)

オイラー法

オイラー法 xn+1 = xn+ Tf (xn, tn) 1 段解法 x と t の一つの組 (xn, tn) で f (x , t) を計算 xn+1= x ((n + 1)T ) = x (nT + T ) = x (tn+ T ) を一次の項まで展開 xn+1 = x (tn) + ˙x (tn)T = x (tn) + f (x (tn), tn)T = xn+ Tf (xn, tn) オイラー法の両辺は一次のオーダまで一致

(31)

オイラー法

t x O _{t n} _{t n+1} x n x n+Tk1 Euler method k1=f(xn,tn) 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 31 / 82

(32)

ホイン法

ホイン法 xn+1 = xn+ T 2(k1+ k2), k1 = f (xn, tn), k2 = f (xn+ Tk1, tn+ T ) 2 段解法二つの組 (xn, tn) と (xn+ Tk1, tn+ T ) で f (x , t) を計算

(33)

ホイン法

t x O _{t n} _{t n+1} k1 k2 x n x n+Tk1 Heun method Euler method k1 < k2のとき ¨x > 0 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 33 / 82

(34)

ホイン法

t x O _{t n} _{t n+1} k1 k2 x n x n+Tk1 Heun method Euler method k1 > k2のとき ¨x < 0

(35)

ホイン法

xn+1= x (tn+ T ) を二次の項まで展開 xn+1 = x (tn) + ˙x (tn)T + 1 2x (t¨ n)T 2 ˙ x = f (x , t) の両辺を時間微分 ¨ x = ∂f ∂x dx dt + ∂f ∂t dt dt = ∂f ∂xx +˙ ∂f ∂t = fxf + ft ホイン法の左辺 xn+1 = xn+ f (xn, tn)T + 1 2(fxf + ft) T 2 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 35 / 82

(36)

ホイン法

k2を展開 k2 = f (xn+ Tk1, tn+ T ) = f (xn, tn) + fxTk1+ ftT = f + (fxf + ft)T ホイン法の右辺 xn+ T 2(k1+ k2) = xn+ T 2 (f + f + (fxf + ft)T ) = xn+ fT + 1 2(fxf + ft) T 2 ホイン法の両辺は二次のオーダまで一致

(37)

ルンゲクッタ法

ルンゲクッタ法 xn+1 = xn+ T 6(k1 + 2k2+ 2k3+ k4), k1 = f (xn, tn), k2 = f (xn+ 1 2Tk1, tn+ 1 2T ), k3 = f (xn+ 1 2Tk2, tn+ 1 2T ), k4 = f (xn+ Tk3, tn+ T ) 4 段解法 x と t の四つの組で f (x , t) を計算 ルンゲクッタ法の両辺は四次のオーダまで一致平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 37 / 82

(38)

ルンゲクッタ法

t x O t n t n+ T t n+1 2 k1 k2 k3 k4 x n x n+T k3 x n+ k1T 2 x n+ k2T 2

(39)

階数とオーダ

階数 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

(40)

常微分方程式の数値解法

常微分方程式 (多変数) の標準型 ˙ x = f (x , t) を数値的に解く． ⇓ オイラー法，ホイン法，ルンゲクッタ法の漸化式で 状態変数 x =⇒ 状態変数ベクトル x スカラー関数 f =⇒ ベクトル関数 f スカラー k =⇒ ベクトル k

(41)

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

(42)

MATLAB

>> pendulum time 0 1 2 3 4 5 6 7 8 9 10 -2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5

(43)

MATLAB

>> time time = 0 0.1000 0.2000 0.3000 0.4000 0.5000 0.6000 0.7000 0.8000 0.9000 1.0000 1.1000 1.2000 1.3000 1.4000 1.5000 1.6000 1.7000 1.8000 1.9000 2.0000 2.1000 2.2000 2.3000 2.4000 2.5000 2.6000 2.7000 2.8000 2.9000 3.0000 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 43 / 82

(44)

MATLAB

>> q q = 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 -0.6829 -1.6427 -0.8308 -1.3055 -0.9426 -0.9245 -1.0148 -0.5167 -1.0457 -0.0958 -1.0341 0.3280 -0.9803 0.7434 -0.8859 1.1377 -0.7538 1.4971 -0.5883 1.8028 -0.3958 2.0341 -0.1845 2.1743 0.0358 2.2111 0.2543 2.1410 0.4604 1.9689 0.6450 1.7089 0.8001 1.3834 0.9202 1.0113 1.0014 0.6085 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 44 / 82

(45)

ルンゲクッタフェールベルグ法

オイラー法ホイン法ルンゲクッタ法   ステップ幅固定ルンゲクッタフェールベルグ法 _→ステップ幅可変平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 45 / 82

(46)

ルンゲクッタフェールベルグ法

t x O 変動が小さい変動が大きい変動が小さい：大きいステップ幅変動が大きい：小さいステップ幅

(47)

ルンゲクッタフェールベルグ法

6 段階法 k1 = f (xn, tn), k2 = f (xn+ T 4k1, tn+ 1 4T ), k3 = f (xn+ T 32(3k1+ 9k2), tn+ 3 8T ), k4 = f (xn+ T 2179(1932k1− 7200k2+ 7296k3), tn+ 12 13T ), k5 = f (xn+ T ( 439 216k1− 8k2+ 3680 513 k3− 845 4104k4), tn+ T ), k6 = f (xn+ T (− 8 27k1+ 2k2− 3544 2565k3+ 1859 4104k4− 11 40k5), tn+ 1 2T ) 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 47 / 82

(48)

ルンゲクッタフェールベルグ法

5 次のオーダの解 xn+1 = xn+ T ( 16 135k1+ 6656 12825k3+ 28561 56430k4 − 9 50k5+ 2 55k6) 4 次のオーダの解 x_n+1∗ = xn+ T ( 25 216k1 + 1408 2565k3+ 2197 4104k4− 1 5k5) 変動が小さい：xn+1と xn+1∗ の差が小さい→ 大きいステップ幅 変動が大きい：xn+1と xn+1∗ の差が大きい→ 小さいステップ幅

(49)

ルンゲクッタフェールベルグ法

ルンゲクッタフェールベルグ公式 Step 1 5 次のオーダの解 xn+1を計算する． Step 2 4 次のオーダの解 x_n+1∗ を計算する． Step 3 次式で表される ˆT を計算する． ˆ T = αT { ϵ ∥xn+1− xn+1∗ ∥ }1 5 ここで，ϵ は許容量を表す小さい正の定数，α は安全 率で 0.8 から 0.9 の範囲から選ぶ． Step 4 時間ステップ T の値を ˆT 以下で選ぶ． 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 49 / 82

(50)

MATLAB

ファイル pendulumVar.m pendulumConstants; timeinterval=[0,10]; % 可変ステップ q0=[pi/3;0]; [time,q]=ode45(’dotpendulum’,timeinterval,q0); plot(time,q(:,1),time,q(:,2),’--’);

(51)

MATLAB

>> pendulumVar time 0 1 2 3 4 5 6 7 8 9 10 -2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 51 / 82

(52)

MATLAB

>> time time = 0 0.0000 0.0000 0.0000 0.0000 0.0001 0.0002 0.0002 0.0003 0.0006 0.0009 0.0012 0.0015 0.0029 0.0044 0.0059 0.0074 0.0148 0.0222 0.0296 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 52 / 82

(53)

MATLAB

>> q q = 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 1.0472 -0.0050 1.0472 -0.0062 1.0472 -0.0125 1.0472 -0.0188 1.0471 -0.0251 1.0471 -0.0313 1.0467 -0.0627 1.0462 -0.0941 1.0453 -0.1255 1.0443 -0.1569 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 53 / 82

(54)

衝突のシミュレーション

質点 m を床に落す． 床に原点をおき，時刻 t における質点の位置を x(t) で表す． 質点と床との接触力が弾性力 (弾性係数 k) で表されると仮定する． 質点の運動方程式 m¨x = fcollision− mg, fcollision = { −kx x < 0 0 otherwise パラメータ m = 1，k = 100，g = 9.8 初期値 x(0) = 100， ˙x(0) = 0

(55)

MATLAB

ファイル collideConstants.m classdef collideConstants % 衝突のパラメータ properties (Constant) gravity = 9.8; mass = 1.0; spring = 100.0; end end 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 55 / 82

(56)

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

(57)

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),’--’);

(58)

MATLAB

time 0 5 10 15 20 25 30 35 40 45 50 -50 0 50 100 位置 x(t)，速度 ˙x(t)

(59)

MATLAB

time 0 5 10 15 20 25 30 35 40 45 50 time interval 0 0.2 0.4 0.6 0.8 1 1.2 1.4 時間ステップ T 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 58 / 82

(60)

講義の目標

講義の内容制約を有する常微分方程式制約安定化法講義の目標制約を有する常微分方程式を理解する制約安定化法で標準型に変換できる

(61)

単振り子

(

直交座標

)

m x mg C l R=0 R<0 R>0 λ y O 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 60 / 82

(62)

制約を有する常微分方程式

単振り子

(

直交座標

)

時刻 t における質点の位置 (x, y ) 制約 振り子の支点 C から質点までの距離は l に等しい． R(x , y )=△ {x2+ (y − l)2} 1 2 − l = 0 R(x , y )：長さの次元．軌道円の外側で正，内側で負の値． 勾配ベクトル [Rx, Ry]T Rx(x , y ) =△ ∂R ∂x = x { x2+ (y − l)2}− 1 2_, Ry(x , y ) △ = ∂R ∂y = (y− l) { x2+ (y − l)2}− 1 2_. 制約式 R(x, y ) = 0 が表す円軌跡の外向き法線ベクトル

(63)

単振り子

(

直交座標

)

張力の方向 勾配ベクトル [Rx, Ry]T 張力の大きさ 未知数 λ 質点の運動方程式 m¨x = λ Rx(x , y ), m ¨y = λ Ry(x , y )− mg ただし R(x , y ) = 0 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 62 / 82

(64)

単振り子

(

直交座標

)

vx △ = ˙x と vy △ = ˙y 制約付きの常微分方程式 ˙ x = vx, ˙ y = vy, m ˙vx = λ Rx(x , y ), m ˙vy = λ Ry(x , y )− mg, R(x , y ) = 0 未知変数：x，y ，vx，vy，λ 4 個の常微分方程式と 1 個の代数方程式

(65)

閉リンク機構

x y O link 1 link 2 link 3 link 4 θ1 θ3 θ2 θ4 tip point l1 l2 l3 l4 (x1,y1) (x3,y3) joint 1 joint 2 joint 3 joint 4 joint 5 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 64 / 82

(66)

閉リンク機構

link 1 link 2 θ1 θ2 tip point l1 l2 (x1,y1) joint 1 joint 2 xleft yleft

[

]

link 3 link 4 θ3 θ4 tip point l3 l4 (x3,y3) joint 3 joint 4 xright yright

[

]

左アーム右アーム

(67)

閉リンク機構

左アームの端点の座標 [ x1 y1 ] + l1 [ C1 S1 ] + l2 [ C1+2 S1+2 ] ∥ 右アームの端点の座標 [ x3 y3 ] + l3 [ C3 C3 ] + l4 [ C3+4 S3+4 ] 制約 P(x , y )= l△ 1C1+ l2C1+2− l3C3− l4C3+4+ x1− x3 = 0 Q(x , y ) = l△ 1S1+ l2S1+2− l3S3− l4S3+4+ y1− y3 = 0 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 66 / 82

(68)

閉リンク機構

運動方程式 H11ω˙1+ H12ω˙2 = f1+ λx(−l1S1− l2S1+2) + λy(l1C1+ l2C1+2), H22ω˙2+ H12ω˙1 = f2+ λx(−l2S1+2) + λyl2C1+2, H33ω˙3+ H34ω˙4 = f3+ λx(l3S3+ l2S3+4) + λy(−l3C3− l4C3+4), H44ω˙4+ H34ω˙3 = f4+ λxl4S3+4 + λy(−l4C3+4) ここで f1 = h12ω₂2+ 2h12ω1ω2− G1− G12+ τ1, f2 =−h12ω₁2− G12, f3 = h34ω42+ 2h34ω3ω4− G3− G34+ τ3, f4 =−h34ω32− G34

(69)

閉リンク機構

制約付きの常微分方程式 ˙ θ1 = ω1, θ˙2 = ω2, θ˙3 = ω3, θ˙4 = ω4, H11ω1˙ + H12ω2˙ = f1+ λx(−l1S1− l2S1+2) + λy(l1C1+ l2C1+2), H22ω˙2+ H12ω˙1 = f2+ λx(−l2S1+2) + λyl2C1+2, H33ω˙3+ H34ω˙4 = f3+ λx(l3S3+ l2S3+4) + λy(−l3C3− l4C3+4), H44ω˙4+ H34ω˙3 = f4+ λxl4S3+4 + λy(−l4C3+4), P(x , y ) = 0, Q(x , y ) = 0 未知変数：θ1, θ2, θ3, θ4，ω1, ω2, ω3, ω4，λx，λy 8 個の常微分方程式と 2 個の代数方程式平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 68 / 82

(70)

制約安定化法

代数方程式 R = 0 ⇓ 微分方程式 ¨ R + 2ν ˙R + ν2R = 0 ν：あらかじめ定める大きい正の定数 数値計算の過程で幾何制約 R が破られる． =⇒ 上式 (臨界減衰の式) により R の値は再び 0 に収束 =⇒ 結果的に制約式 R = 0 が保たれる．

(71)

制約安定化法

制約式 R(x, y ) = 0 の制約安定化の式を計算 R(x , y ) の時間微分 ˙R ˙ R = ∂R ∂x dx dt + ∂R ∂y dy dt = Rxx + R˙ yy˙ 偏微分 Rx(x , y ) の時間微分 ˙ Rx = ∂Rx ∂x dx dt + ∂Rx ∂y dy dt = Rxxx + R˙ xyy˙ 偏微分 Ry(x , y ) の時間微分 ˙ Ry = ∂Ry ∂x dx dt + ∂Ry ∂y dy dt = Ryxx + R˙ yyy˙ 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 70 / 82

(72)

制約安定化法

R(x , y ) の二階時間微分 ¨R ¨ R = R˙xx + R˙ xx + ˙¨ Ryy + R˙ yy¨ = Rxx + R¨ yy + (R¨ xxx + R˙ xyy ) ˙˙ x + (Ryxx + R˙ yyy ) ˙˙ y = Rxx + R¨ yy +¨ [ ˙ x y˙ ] [ Rxx Rxy Ryx Ryy ] [ ˙ x ˙ y ] ¨ R + 2ν ˙R + ν2R = 0 ⇓ −Rxx¨− Ryy = C (x , y , ˙¨ x , ˙y ) ただし C (x , y , ˙x , ˙y )=△ [ x˙ y˙ ] [ Rxx Rxy Ryx Ryy ] [ ˙ x ˙ y ] + 2ν(Rxx + R˙ yy ) + ν˙ 2R

(73)

制約安定化法

単振り子 (直交座標) m¨x = λ Rx(x , y ), m ¨y = λ Ry(x , y )− mg, R(x , y ) = 0 ⇓ ˙ x = vx, ˙ y = vy, m ˙vx− λ Rx(x , y ) = 0, m ˙vy − λ Ry(x , y ) = −mg, −Rxv˙x − Ryv˙y = C (x , y , vx, vy) 未知変数：x，y ，vx，vy，λ 5 個の常微分方程式平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 72 / 82

(74)

制約安定化法

P(x , y )= x△ 2+ (y − l)2 R(x , y ) = P12 − l, Rx(x , y ) =△ ∂R ∂x = xP −1 2, Ry(x , y ) =△ ∂R ∂y = (y − l)P −1 2, Rxx(x , y ) =△ ∂2_R ∂x2 = P −1 2 − x2P− 3 2, Ryy(x , y ) =△ ∂2_R ∂y2 = P −1 2 − (y − l)2P− 3 2, Rxy(x , y ) = Ryx(x , y ) =△ ∂2_R ∂x ∂y =−x(y − l)P −3 2.

(75)

制約安定化法

mv˙x−λRx(x , y ) = 0, mv˙y −λRy(x , y ) = −mg, −Rxv˙x − Ryv˙y = C (x , y , vx, vy) ⇓   m0 m0 −R−Rxy −Rx −Ry 0     vv˙˙xy λ   =   _−mg0 C (x , y , vx, vy)   変数 x, y , vx, vyの値を与える． =⇒ 上式左辺の行列の要素と右辺のベクトルの要素の値を計算． =⇒ 連立一次方程式を解き時間微分 ˙vx, ˙vyの値を計算する．平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 74 / 82

(76)

制約安定化法

状態変数ベクトル q =△     x y vx vy     常微分方程式の標準型 ˙ q = f (q, t) x y vx vy q x∙ y∙ vx∙ vy∙ q∙ solve linear equations t

(77)

MATLAB

m = 0.01，l = 2，g = 9.8

初期値 θ(0) = (π/3)，ω(0) = 0 に対応する初期値

x (0) = l sin θ(0)，y (0) = l (1− cos θ(0))，vx(0) = 0，vy(0) = 0

制約付きの常微分方程式を制約安定化法で標準型に変換標準型をルンゲクッタ法で数値的に解く．

(78)

制約安定化法

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 +...

(79)

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

(80)

制約安定化法

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

(81)

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

(82)

制約安定化法

MATLAB

time 0 1 2 3 4 5 6 7 8 9 10 position -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 位置 x, y

(83)

MATLAB

time 0 1 2 3 4 5 6 7 8 9 10 velocity -5 -4 -3 -2 -1 0 1 2 3 4 5 速度 vx, vy 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 81 / 82

(84)

制約安定化法

MATLAB

time 0 1 2 3 4 5 6 7 8 9 10 angle -1.5 -1 -0.5 0 0.5 1 1.5 振れ角 θ

(85)

MATLAB

time 0 1 2 3 4 5 6 7 8 9 10 constraint ×10-3 -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 制約 R 平井慎一 (立命館大学ロボティクス学科) 数値計算：常微分方程式 81 / 82

(86)

まとめ

常微分方程式の標準型状態変数に関する一階の微分方程式状態変数の値を与えると，その時間微分を計算できる ˙ x = f (x , t) 常微分方程式の数値解法標準型を数値的に解くステップ幅固定：オイラー法，ホイン法，ルンゲクッタ法ステップ幅可変：ルンゲクッタフェールベルグ法制約安定化法制約式を微分方程式に変換