機械電子制御工学演習 テキスト
中島 明
理工学部 機械電子制御工学科
南山大学
改定日：平成 27 年 11 月 3 日
ii
はじめに
このテキストは，名古屋大学井上剛志先生が授業で使用されているテキストおよび MBD
協議会のセミナーで使用されたテキストを，南山大学大学院理工学研究科機械電子制御工
学専攻の機械電子制御工学演習でのテキストとして再編し，補足などを加えたものになり
ます．
iv
はじめに
目次
第1章
講義概説
1
第2章
2.1
2.2
2.3
2.4
2 次元空間での剛体の運動学
剛体の位置・姿勢の表現 . . . . . . . . . . . .
回転行列の性質 . . . . . . . . . . . . . . . . .
剛体の運動学 . . . . . . . . . . . . . . . . . .
剛体の静力学 –異なる座標系での等価な力 [4]
.
.
.
.
3
3
4
5
6
第3章
3.1
3.2
3.3
2 次元空間での剛体の動力学
ニュートン・オイラーの運動方程式 . . . . . .
拘束の定式化 [5, 6] . . . . . . . . . . . . . . .
拘束の取り扱い手法 [5, 6] . . . . . . . . . . .
3.3.1 消去法 . . . . . . . . . . . . . . . . . .
3.3.2 拡大法 . . . . . . . . . . . . . . . . . .
拘束式の取り込み –ダランベールの原理 [5, 6]
ラグランジュの運動方程式 . . . . . . . . . . .
.
.
.
.
.
.
.
9
9
10
11
11
12
12
15
.
.
.
.
.
.
.
.
.
.
.
17
17
17
18
18
19
20
20
21
22
22
23
.
.
.
.
25
25
26
27
28
3.4
3.5
第4章
4.1
4.2
4.3
第5章
5.1
5.2
5.3
5.4
数値積分法
数値積分法の概説 [8] . . . . . . . . .
4.1.1 取り扱う微分方程式の形 . . .
4.1.2 時間積分の考え方 . . . . . .
4.1.3 時間積分手法の要求条件 . . .
4.1.4 時間積分手法の種類 . . . . .
微分代数方程式の求解法 [5, 6] . . . .
4.2.1 拘束式の時間微分 . . . . . .
4.2.2 拘束力の算出 . . . . . . . . .
拘束式の安定化 . . . . . . . . . . . .
4.3.1 バウムガルデの安定化法 [5, 6]
4.3.2 拘束式の修正 . . . . . . . . .
演習問題
剛体振子 (消去法) . . . . . . .
剛体振子 (拡大法) . . . . . . .
剛体振子 (拡大法・駆動拘束)
剛体 2 重振子 (拡大法) . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
vi
目次
第 1 章 講義概説
自動車，列車，航空機，ロボット，建築機械，建築物，宇宙構造物などの機械システム
(電子機械システム) において，設計にコンピュータを用いた解析技術は必要不可欠である．
これらは様々な部品 (ボディ) が相互に連結した複雑な機構系，すなわちマルチボディシ
ステムと呼ばれている．本講義では，このような非常に広い適用範囲を持つマルチボディ
システムの運動学と動力学を取り扱うマルチボディダイナミクスについて，定式化および
プログラミング実装までを行うことができることを目指す．取り扱う数式が複雑になるこ
とを避けるため，2 次元空間に限定した剛体運動の定式化を取り扱う．また，プログラミ
ングでは汎用制御系 CAD である Matlab を用いて行い，アルゴリズムの構造により注視
できるようにする．
扱う内容は大まかに以下のようになる．
• 2 次元空間での剛体運動
• 拘束式の定式化
• 拘束力の運動方程式への取り込み手法
• 数値解析のための定式化および数値積分法
• いくつかの具体例を用いたプログラミング演習
参考にした文献は [1] [2] [3] [4] [5] [6] [7] [8] であり，より深く学びたい人はこれらを参
照されたい．
2
第1章
第 2 章 2 次元空間での剛体の運動学
2.1 剛体の位置・姿勢の表現
Figure 2.1: 平面内の剛体の位置・姿勢表現
Fig. 2.1 のように，平面内において ΣA を基準座標系とする剛体を考える．剛体の位置・
姿勢を表現するためには，剛体上のある点，すなわち代表点に物体座標系と呼ばれる ΣB
を固定する必要がある．代表点はどこでも良いが，重心とすることが多い1 ．これら 2 つ
の座標系を用いて，剛体の位置・姿勢はそれぞれ
]
[
A
p
B
A
(2.1)
pB = A x ∈ R2 , A θB ∈ R
pBy
で表現される．ここで A pB は座標系 ΣA の原点から ΣB の原点に向かう位置ベクトルで
あり，その成分は ΣA で表現されている．また A θB は ΣA の XA 軸から ΣB の XB 軸まで
のなす角である．なお，変数の左肩添字は基準となる座標系を表している．これらの変数
をまとめて，剛体の位置・姿勢を表す座標ベクトル A rB ∈ R3 として定義する．
[
]
A
p
B
A
rB := A
∈ R3
(2.2)
θB
1
剛体と一定距離を保ちつつ連動するならば，剛体上でなくとも良い．
第2章
4
Figure 2.2: 平面内の剛体の位置・姿勢表現
剛体の姿勢は姿勢角 A θB で表現されるが，次の回転行列を用いる場合もある．
[
]
A
A
A
RB := xB yB ∈ R2×2
(2.3)
ここで A xB ，A yB ∈ R2 は ΣA を基準にして ΣB の XB ，YB 軸を表した単位方向ベクトル
であり，添字の意味は同様である．したがって，それぞれは以下で与えられる．
[
]
[
]
A
A
cos θB
− sin θB
A
xB =
, A yB =
(2.4)
A
sin θB
cos A θB
よって，回転行列 (2.3) 式は次のようになる．
[
]
cos
θ
−
sin
θ
A
RB = R(A θB ), R(θ) :=
sin θ cos θ
(2.5)
このような回転行列 A RB と位置ベクトル A pB をあわせて (A pB , A RB ) を位置・姿勢を
表す数学的量としても良い．また，これらは Fig. 2.2 のように，空間上の点 e を異なる座
標系で表現した際の座標変換に用いることができる．
A
2.2
pe = A pB + A RB B pe
(2.6)
回転行列の性質
ここでは，回転行列 (2.5) 式のいくつかの性質についてまとめる．まず，簡単な検算か
ら明らかに
R(θ)RT (θ) = R(θ)T R(θ) = I2
(2.7)
2.3 剛体の運動学
5
が成立することから，逆行列と転置行列は等しい．
R−1 (θ) = RT (θ)
(2.8)
また，任意の回転角度 θ1 ，θ2 ∈ R について積の交換則が成り立つ．
R(θ1 )R(θ2 ) = R(θ2 )R(θ1 ) = R(θ1 + θ2 )
(2.9)
このことから，逆行列については，
R−1 (θ) = R(−θ)
(2.10)
のようにも表現できる．
次に，時間変化について考える．回転角度 θ が時間 t に関する関数 θ(t) であるとする
と，回転行列の時間微分について次の関係式が成り立つ．
[
]
d
dθ
0
−a
R(θ) = R( π2 )R(θ) = S(θ̇)R(θ) = R(θ)S(θ̇), S(a) :=
(2.11)
dt
dt
a 0
ここで S(a) ∈ R2×2 (a は任意の変数) は歪対称行列である．
2.3 剛体の運動学
Figure 2.3: 平面内の剛体の位置・姿勢表現
Fig. 2.3 のように，剛体上に点 e があり，この点に座標系 Σe を設置する．この座標系
は時間的に変化しないとする．このとき，2 つの座標系での点 e の関係は (2.6) 式となる．
また，ΣA ，ΣB を基準とした作用点 e の姿勢 A θe と B θe の関係は以下のようになる．
A
θe = A θB + B θe
(2.12)
第2章
6
まず位置に関する運動学を考える．(2.6) 式を時間微分すると
A
ṗe = A ṗB + A ṘB B pe
(2.13)
となり，(2.11) 式を用いれば (2.13) 式は次のようになる．
A
ここで
ṗe = A ṗB + S(A θ̇B )A RB B pe = A ṗB + S(1)A RB B pe A θ̇B
[
][ ] [
]
0 −1 px
−py
V (p) := S(1)p =
=
∈ R2×1
1 0
py
px
(2.14)
(2.15)
とおけば，作用点 e の並進速度は一般化座標の速度を用いて以下のように表される．
A
ṗe = A ṗB + V (A RB B pe )A θ̇B
(2.16)
変換行列 Je ∈ R2×3 で用いられている V (A RB B pe ) は，ΣB の原点での回転速度 A θ̇B を
A
RB B pe だけ離れた位置での並進速度に変換するものであり，3 次元空間での外積に似た
作用と考えれば良い．
一方，姿勢の運動学については，(2.12) 式を時間微分すれば以下を得る．
A
θ̇e = A θ̇B
(2.17)
すなわち，剛体上の任意の点の回転速度は等しいことが分かる．
以上から，(2.16)，(2.17) 式をまとめれば，作用点 e と代表点の間の速度に関する関係
式が得られる．
A
ṙe = Je A ṙB
(2.18)
ただし
2.4
[
]
I2 V (A RB B pe )
Je :=
∈ R3
01×2
1
(2.19)
剛体の静力学 –異なる座標系での等価な力 [4]
剛体上の点 e に外力，外モーメントが作用するとき，それと等価な剛体の代表点での並
進力，モーメントを求める静力学を考える．なお，釣り合い状態とは異なることに注意す
べきである．
Fig. 2.4 は，剛体の作用点 e に外力・外モーメント A Fe ∈ R3 が作用する場合である．こ
のとき，この外力・外モーメントと等価な代表点に力・モーメントを A FB ∈ R3 とする．
[
]
[
]
A
A
f
f
e
B
A
Fe = A
, A FB = A
(2.20)
ne
nB
また，B pe ∈ R2 は物体座標系 ΣB で表現された作用点 e の位置ベクトルである．このと
き，作用点と代表点での仮想変位をそれぞれ
]
]
[
[
δ A pB
δ A pe
A
A
(2.21)
, δ rB = A
δ re = A
δ θB
δ θe
2.4 剛体の静力学 –異なる座標系での等価な力 [4]
7
Figure 2.4: 点 e に作用する外力・外モーメント
とすれば，仮想仕事の原理から，作用点と代表点での仮想仕事は等しくなる．
TA
δ A reT A Fe = δ A rB
FB
(2.22)
ここで，(2.18) 式の時間微分を仮想変位に置き換えた関係式
δ A re = Je δ A rB
(2.23)
を用いると
T TA
TA
δ A rB
Je Fe = δ A rB
FB
となる．この式は任意の δ A rB について成り立つため，互いに等価な作用点と代表点での
力とモーメントの関係式が得られる．
A
FB = JeT A Fe
(2.24)
このように，速度に関する変換行列 Je は静力学の関係式にも用いられる．(2.18)，(2.24)
式を比較すると，速度と力では作用点 e と代表点 B の関係が逆であり，変換行列 Je につ
いては転置の関係にある．このように，速度と力は双対関係にある．
なお，仮想仕事の原理 (仮想変位の原理) は本来は以下のように記述される [2] 2 ．
「拘束が仕事を行わないような系が釣り合い状態にある必要十分条件は，拘束を破らない
任意の仮想変位およびそこに作用する力による仕事の和が 0 になること」
したがって，外力 A Fe と釣り合うような代表点に作用する力を A FB′ とすれば，剛体系に
作用する外力による仮想仕事 δ ′ W は
TA ′
δ ′ W = δ A reT A Fe + δ A rB
FB
2
(2.25)
拘束および「拘束が仕事をしない」ことに関する説明は次章で詳しく述べるので，ここでは特に解説し
ない．
8
第2章
となる．釣り合い条件は δ ′ W = 0 であるため，仮想変位の関係 (2.23) 式を考慮すれば，釣
り合いに関する関係式を得る．
A ′
FB = −JeT A Fe
(2.26)
よって，等価な力・モーメント A FB と釣り合う力・モーメント A FB′ は逆向きの関係に
ある．
A ′
FB = −A FB
(2.27)
このように，力学としては剛体系を釣り合い状態として扱うことが本来である．一方ロ
ボットの教科書では，前述した通り，等価な力の変換として捉える場合がほとんどであり，
教科書によっては「釣り合い状態」と「等価変換」を間違えて取り扱っている場合 [3] も
あるため，注意が必要である．
第 3 章 2 次元空間での剛体の動力学
3.1 ニュートン・オイラーの運動方程式
Figure 3.1: 平面内の並進・回転運動の運動モデル
一般的な剛体の運動方程式は，剛体の代表点の並進運動および回転運動について考え
ればよい．なお，鉛直下向き，すなわち YA の負方向に重力が作用するとする．ここで
g = [0 − g]T ∈ R2 は重力加速度ベクトル，g = 9.81 [m/s2 ] は重力加速度定数である．
Fig. 3.1 は一般的な剛体の運動を表すモデルである．前章と同様に，代表点は質量中心
とし，物体座標系 ΣB を代表点を原点として剛体に固定すれば，剛体の位置・姿勢はそれ
ぞれ A pB ∈ R2 ，A θB ∈ R で表現される．剛体の質量を mB ，代表点まわりの慣性モーメ
ントを JB とする．このとき，基準座標系 ΣA における剛体の運動方程式は次のように与
えられる．
{
mB A p̈B = A fB
(3.1)
JB A θ̈B = A nB
ここで A fB ∈ R2 ，A nB ∈ R は座標系 ΣB の原点に作用する外力，外モーメントである．
並進および回転の運動方程式をまとめて表記すると以下のようになる．
MB A r̈B = A FB
ここに
]
[
mB I2 02×1
∈ R3×3
MB :=
01×2 JB
(3.2)
(3.3)
第3章
10
ここで M ∈ R3×3 は剛体の慣性行列である．このような並進運動に関する運動方程式を
ニュートン方程式，回転運動に関する運動方程式をオイラー方程式と呼び，2 つをあわせ
てニュートン・オイラーの運動方程式と呼ぶ．
なお，外力・外モーメント A FB が重力および作用点 e への外力・外モーメントで与え
られる場合は，次のようになる．
[
]
m
g
B
A
FB = A Fg + JeT A Fe , A Fg :=
∈ R3
(3.4)
0
ここで Fg は重力の一般化力である．
3.2
拘束の定式化 [5, 6]
Figure 3.2: ピンジョイント拘束を受ける剛体運動
ここでは剛体運動に何らかの拘束が存在する場合を考える．具体例として，Fig. 3.2 の
ようなピンジョイント拘束を扱う．ここで，拘束点 C には座標系 ΣC が設置されている
とすると，拘束座標系 ΣC と物体座標系 ΣB の関係は以下で与えられる．
{
A
pC = A pB + A RB B pC
(3.5)
A
θC = A θB + B θC
今回はピンジョイント拘束であるため，剛体上の点 C の位置が空間上に固定されるが，回
転は自由に運動できる．したがって，拘束は次のような代数式として定式化される．
C(A rB ) = 0
(3.6)
C(A rB ) := A pC = A pB + A RB B pC
(3.7)
ただし
このような拘束を幾何拘束と呼ぶ．ピンジョイント拘束 (3.7) 式を具体的に展開してみる．
剛体の代表点から拘束点 C までの長さを l とすれば，
] [
][
[
] [
(
)]
A
A
pBx + l cos A θB + B θC
cos A θB − sin A θB l cos B θC
p Bx
A
(
) (3.8)
= A
C( rB ) = A
+
l sin B θC
pBy + l sin A θB + B θC
sin A θB cos A θB
pBy
3.3 拘束の取り扱い手法 [5, 6]
11
Figure 3.3: XA 方向の駆動拘束
となる．
次に，アクチュエータなどの駆動機械を用いて，ピンジョイント部の XA 方向に運動さ
せることを考えてみる．このとき，駆動により与える運動の時間関数を d(t) とすると，拘
束 (3.25) 式は時間に依存した次の形式で定式化される．
C(A rB , t) = 0
ただし
]
d(t)
C(A rB , t) := A pC −
0
(3.9)
[
(3.10)
このような拘束を駆動拘束と呼ぶ1 ．
3.3 拘束の取り扱い手法 [5, 6]
3.3.1
消去法
ピンジョイント拘束 (3.6)，(3.8) 式より，代表点の位置 A pB = [A pBx A pBy ]T はいずれも
姿勢角 A θB のみを用いて表現できる．
{
)
(
A
pBx = −l cos A θB + B θC
(3.11)
)
(
A
pBy = −l sin A θB + B θC
A
T
A
A
A
T
したがって，剛体の位置・姿勢 A rB = [A pT
B θB ] の 3 変数うち pB = [ pBx pBy ] の
2 変数を消去して，A θB の 1 変数のみで系を記述することが可能である．このように幾何
拘束式を用いて変数を消去する手法を消去法と呼ぶ．なお，消去された変数を従属座標，
残った座標を独立座標と呼んだりもする．さらに，一般化座標の次元から拘束式の本数を
引いた数を系の自由度と呼ぶ．
1
制御工学に慣れている人にとっては，外部入力による系の駆動をこのような形で考慮することに違和感
を覚えるかもしれない．制御工学により同様の運動を実現しようとすれば，ピンジョイント部の XA 方向
への制御入力 (外力) uXA (t) を導入して，uXA (t) による軌道追従制御がすぐに思いつくだろう．そのように
実現しても問題ないが，後述するように，系の数値積分において拘束を満足するための修正法であるバウ
ムガルデ法と類似した微分方程式を解いていることになる．
第3章
12
3.3.2
拡大法
消去法は系の構造が単純な場合は非常に有効であるが，剛体の数が多くなり結合 (拘束)
が増えると変数を消去する手順が一気に複雑になる2 ．そこで変数を消去せず，運動方程
式と拘束式を連立させて定式化する手法のことを，拡大法と呼ぶ．後に詳細を述べるが，
拘束を成立させるための乗数 (拘束式の本数と同じ次元) を導入する．したがって，取り
扱う変数は 3 変数に加えて乗数 2 変数の合計 5 変数となる．そのため，変数を消去する必
要がなく運動方程式と拘束式を別々に定式化すれば良いだけであり，どのような機構でも
同一の手順で定式化が可能である．
3.4
拘束式の取り込み –ダランベールの原理 [5, 6]
ここからは力学系の一般論を取り扱うので，運動が一般化座標 q ∈ Rn で表される系を
考える．一般化座標とは，系の配位 (位置・姿勢) を完全に記述する変数の組を意味する3 ．
これまで取り扱った平面の剛体を 1 つだけ取り扱う場合，一般化座標は q = A rB ，n = 3
である．なお，全ての変数は同一の座標系で表現されているとして，左肩添字は省略する．
平面の剛体の運動方程式である (3.2) 式を一般化座標表現で表記する．
M q̈ = Fq
(3.12)
ここで Fq ∈ Rn は一般化座標 q へ作用する外力である．拘束式を満足する (拘束を力学的
に強制する) ためには，(3.12) 式に外力として拘束力 FC を加える必要がある．
M q̈ = Fq + FC
(3.13)
ここで，全ての項を一方にまとめて，
FB + FC − M q̈ = 0
(3.14)
のように，−M q̈ を仮の外力とみなす．これを慣性力 (慣性抵抗) と呼ぶ．したがって，運
動方程式は，
「外力 Fq ，拘束力 FC ，慣性力 −M q̈ が釣り合っている状態」
と解釈することができる．これをダランベールの原理と呼ぶ [2]．この原理により，運動
方程式は静力学の問題として取り扱うことができる．したがって，仮想仕事の原理より，
「力が釣り合うことの必要十分条件はその仮想仕事が 0 である」ことであるため，以下の
関係式が成り立つ．
(Fq + FC − M q̈)T δq = 0
(3.15)
この式のことを，ラグランジュの変分方程式，もしくはこの式を指してダランベールの原
理と呼ぶこともある [2]．
2
開リンク機構のロボットマニピュレータなどはデナビットハーテンベルクの記法 [3, 4] など，関節の自
由度，すなわち最小自由度で系を表現する手法が確立しているが，より複雑な機構 (閉ループリンク機構な
ど) ではケースバイケースにならざるを得ない．
3
系の本質的な自由度より多くても良い．
3.4 拘束式の取り込み –ダランベールの原理 [5, 6]
13
ここからは，拘束力の表現方法について考える．(3.6) 式のような代数拘束4
C1 (q)
C(q) = ... ∈ Rm , m < n
(3.16)
Cm (q)
が成り立つとして，その変分を取る．
δC(q) = Cq (q)δq = 0
ただし
∂C1
Cq (q) :=
∂C
,
∂q
∂q1
∂C
:= ...
∂q
∂C
m
∂q1
···
..
.
···
(3.17)
∂C1
∂qn
.. ∈ Rm×n
.
(3.18)
∂Cm
∂qn
ここで，Cq ∈ Rm×n は q に関する偏微分の省略形であり，Cq を特に拘束ヤコビ行列と呼
ぶ．なお，スカラーをベクトルで偏微分する場合は，ベクトルの各要素で偏微分したもの
を横に並べるように定義し，ベクトルを偏微分する場合は各要素を偏微分して横に並べた
ものを縦に並べるように定義する5 ．得られた拘束の変分 (3.17) 式は，仮想変位 δq が常に
(3.17) 式を満足する方向にしか変動しないことを意味している．このとき，拘束力 FC は
どのように表現されるだろうか．
Figure 3.4: 拘束を満たす仮想変位の例
このことを Fig. 3.4 にあるように，地面を転がる球の簡単な例で考えてみよう．一般化
座標は球の中心位置 q = [q1 q2 ]T ∈ R2 とする．球は地面から垂直抗力を受け，一定の高
さで地面の上を転がりながら運動することができる．この場合，地面を表す曲線が 1 次元
の拘束式
C(q) = 0, C ∈ R
となる．この変分が
∂C [ ∂C
Cq (q)δq = 0, Cq =
= ∂q1
∂q
4
∂C
∂q2
]
∈ R1×2
駆動拘束 C(q, t) でも同様の議論で進めることができる．なぜならば，ここで考えるべきはあくまで微
小変分 δq による変動のみであり，微小時間 dt による変動はないためである．実際，駆動拘束 C(q, t) の変
∂C
分をとっても δC(q, t) = ∂C
∂q δq であり，時間による変動の ∂t は現れない．
5
この定義は絶対ではなく，分野や教科書により異なる．
第3章
14
となるわけであるから，地面の垂直方向を表すベクトルを nC ∈ R2 とすると，
nC = CqT
で与えられることは明らかである．また，nC 方向にある垂直抗力が拘束力そのものであ
るから，拘束力 FC は
FC = λCqT
で表現されることになる．ここで λ ∈ R は拘束力の Cq 方向の大きさを表す．
以上の事実を一般化すれば，拘束ヤコビ行列の (3.18) 式は
∂C1
C1q
∂q
Cq = ... = ...
Cmq
(3.19)
∂Cm
∂q
と表記されるため，拘束の変分 (3.17) 式は，この行ベクトルを転置した
{
}
T
C1Tq , · · · , Cm
q
(3.20)
の全てに変分 δq が直交する方向のみ変動することが許される，ということを意味してい
る．また，拘束力は変分 δq に影響を及ぼさない方向にのみ作用するため，(3.20) 式のベ
クトルの線形和で表現される．
[
] λ1
..
T
T
T
FC = λ1 C1Tq + · · · + λm Cm
= C1Tq · · · Cm
(3.21)
. = Cq (q)λ,
q
q
λm
ここで λ := [λ1 , · · · , λm ]T ∈ Rm はラグランジュ乗数と呼ばれる．なお，このラグラン
ジュ乗数は λ の代わりに −λ としても数理的には何も問題もない．しかしながら，物理
的な解釈はまったく異なるため，細心の注意が必要である．今回は +λ を採用したことに
より，拘束力 FC は “環境から受ける” 力となっている．一方もし −λ を採用した場合は
その反作用力となり，剛体から環境に作用している力として解釈される．
さて，このような拘束力は拘束を満たすように運動する系，すなわち (3.17) 式を満た
す系に対して仕事をしない．実際，ラグランジュの変分方程式 (ダランベールの原理) の
(3.15) 式に拘束力 (3.21) 式を代入すれば，
(
Fq + CqT λ − M q̈
)T
δq
=0
⇔ (Fq − M q̈)T δq + λT Cq δq = 0
⇔ (Fq − M q̈)T δq
=0
(3.22)
となり，拘束力は消去されることから，拘束は仕事はしないことが分かる．このように拘
束を満たす系に仕事をしない拘束を，滑らかな拘束，固い拘束と呼ぶ [2] 6 ．力学系のモデ
6
具体的には，(a) 2 点間の距離が一定のときの相互作用の力の仕事，(b) 質点が滑らかな釘にかけた糸で
連結されてたるみなく運動するときの糸の張力の仕事，(c) 2 つの剛体が摩擦のない滑らかな面で接すると
きの相互作用の力の仕事，(d) 2 つの剛体が摩擦有りの面で接しており滑らず転がるときの相互作用の力の
仕事，(e) 滑車にかかった糸が滑り・弛みないときの張力の仕事，などが挙げられる [2]．
3.5 ラグランジュの運動方程式
15
ル化においては，特に拘束の種類を定めずに，はじめから「拘束は仕事をしない」という
仮定として導入してしまうことも多い．
なお，(3.22) 式は以下の式と同値ではないことに注意すべきである．
Fq − M q̈ = 0
(3.22) 式はあくまで拘束の変分 (3.17) 式を満たす運動方程式であり，一方，上記の式は拘
束を受けない自由運動の運動方程式である．
以上から，拘束条件 (代数拘束，駆動拘束) を考慮した剛体系は，拘束力を含む運動方
程式および拘束式の連立方程式により表される．
{
M q̈ = Fq + CqT λ
(3.23)
C(q, t) = 0
このような運動方程式と拘束方程式からなる連立方程式を微分代数方程式と呼ぶ．
3.5 ラグランジュの運動方程式
これまでの並進運動に関する運動方程式は，いわゆるニュートンの第 2 法則から導かれ
たものである．また回転に関する運動方程式はオイラーにより導かれたものであり，合
わせてニュートン・オイラーの運動方程式と呼ばれ，古典力学で導出されたものである．
ここでは解析力学で導出されたラグランジュの運動方程式について紹介する7 ．ラグラン
ジュの運動方程式は，今回のような単純な剛体運動ではあまり便利さは感じられないが，
多数の剛体が複雑に連動する系では非常に有効である．
運動が一般化座標 q ∈ Rn で表される系を考える．このとき，系の運動エネルギーを K ，
ポテンシャルエネルギーを U とおくと，ラグランジュ関数 (運動ポテンシャル) は以下で
定義される．
L := K − U
(3.24)
このとき，系のラグランジュの運動方程式は次で与えられる．
d
dt
(
∂L
∂ q̇
)T
(
=
∂L
∂q
)T
+Υ
(3.25)
ここで Υ ∈ Rn は一般化外力のうち非保存力となるものである．
平面の剛体の運動方程式をラグランジュの方法によって導くことを考える．この場合，
系の運動エネルギー K およびポテンシャルエネルギー U は以下で与えられる．
K=
1A T
TA
Fg
ṙB MB A ṙB , U = A rB
2
(3.26)
これをラグランジュの運動方程式の (3.25) 式に代入すれば
(
7
∂L
∂ q̇
(
)T
A
= MB ṙB ,
∂L
∂q
)T
= A Fg
(3.27)
Lagrange の運動方程式は D’Alembert の原理，仮想変位の原理，Lagrange の変分方程式，Hamilton の
原理を経て一般化座標，一般化力の表現を導入することで得られる [2]．
第3章
16
となり，また，非保存外力 Υ は
Υ = JeT A Fe
(3.28)
であるから，以下の運動方程式が得られる．
MB A r̈B = A Fg + JeT A Fe
(3.29)
次に，拘束 (代数拘束，駆動拘束) が存在する場合を考える．この場合は，ラグランジュ
関数 L = T − U に拘束式 C(q, t) = 0 の影響を繰り込むことを考える．もし拘束を破る
場合，すなわち
C(q, t) ̸= 0
のように C(q, t) がゼロでない値を持つとき，物理的には拘束状態からの変位を表してい
ることになる．したがって，拘束力を表すラグランジュ乗数 λ はその変位を妨げる仮想
的な力として解釈することができる．以上から，拘束が破られたときになされる仕事，す
なわち拘束によるポテンシャルエネルギー
−λT C
を考慮することを考える8 ．この拘束のポテンシャルエネルギーを含んだラグランジュ関
数を拡張ラグランジュ関数 L̄ と呼ぶ．
L̄ := T − (U − λT C) = L + λT C
(3.30)
このように，条件式 C(q, t) = 0 が破られたときの仮想的なポテンシャルを，評価関数 (ラ
グランジュ関数) に足し合わせることで条件式を満たすように解くことをラグランジュの
未定乗数法と呼ぶ．この拡張ラグランジュ関数をラグランジュ方程式の (3.25) 式に代入す
れば
( )T ( )T ( )T ( )T
∂ L̄
∂L
∂L
∂ L̄
=
=
,
+ CqT λ
(3.31)
∂ q̇
∂ q̇
∂q
∂q
よって，拘束を受ける系のラグランジュの運動方程式は次のように与えられる．
d
dt
8
(
∂L
∂ q̇
)T
(
=
∂L
∂q
)T
+ CqT λ + Υ
(3.32)
未定乗数 λ は拘束を破る変位 C ̸= 0 を妨げるように逆方向に働くため，仕事としては負になる．
第 4 章 数値積分法
4.1 数値積分法の概説 [8]
ここでは数値積分法についての基本事項を簡潔に説明する．詳細な内容を知りたい場合
は，他の教科書 [8] を読むことを勧める．
4.1.1
取り扱う微分方程式の形
世の中の様々な現象の多くは微分方程式で記述されるが，特に力学系は一般に以下の形
式で表現される [9]．
g(ẍ, ẋ, x, t) = 0
(4.1)
ここで，x(t) は未知変数，t は独立変数であり，それぞれ位置と時間を表す．この (4.1) 式
の形を陰関数表現 (非正規形) と呼ぶ．多くの場合は最高次数 ẍ を取り出して
ẍ = f (ẋ, x, t)
(4.2)
と表現できる．この形式を陽関数表現 (正規形) と呼ぶ．また，状態変数 X として
[ ]
x
X :=
(4.3)
ẋ
のように，位置変数 x と速度変数 ẋ をまとめることで，(4.2) 式は 1 階の常微分方程式に
変換できる．
Ẋ = F (X, t)
(4.4)
ここまでは一般的な非線形系を扱って来たが，線形系の場合では (4.2) 式は以下の MCK
形として表現される．
M ẍ + C ẋ + Kx = fe
(4.5)
ここで，M ，C ，K は系の特性である慣性，減衰，弾性を表す行列であり，fe は外力を
表す．この MCK 形の (4.5) 式を (4.4) 式のように 1 階の常微分方程式に変換すると
][ ] [ ]
[ ] [
d x
0
0
I
x
+
(4.6)
=
−1
−1
dt ẋ
fe
−M K −M C ẋ
となるため，以下のように線形系の表現に帰着される．
Ẋ(t) = AX(t) + b(t)
(4.7)
第4章
18
非線形系，線形系のいずれも (4.4)，(4.7) 式のように 1 階の常微分方程式で表現される
ため，より一般的であり，標準形方程式と呼ばれる．しかしながら，振動・構造などの実
際に取り扱う問題ではその多くが (4.5) 式のような MCK 形として表現されるため，標準形
方程式のみならず MCK 形に対しても多くの数値積分の手法が開発されている．したがっ
て，(4.5) 式を特に MCK 形方程式と呼んで区別している．
4.1.2
時間積分の考え方
系の応答を解析しようとする場合，次のような微分方程式の初期値問題を時間積分して
いくことになる．
Ẋ(t) = F (X, t), X(t0 ) = X0
(4.8)
時間積分では，既知である現在および過去の状態の値に基づいて未来の状態を予測 (計算)
することが目的である．例えば，時刻 tn の時点での状態量 Xn が計算により求められて
いるとすれば，微小時間 h 後の時刻 tn + h の近未来の時点での状態量 Xn+1 は (4.8) 式に
基づき
Xn+1 ≈ Xn + hF (Xn , tn )
(4.9)
として算出することができる．この計算法をオイラー法と呼ぶ．時間幅の h が十分小さ
ければこの予測値は正解に近いであろう．
したがって，数値解析的に時間積分を行う方法としては，時間を間隔 h に細かく分割し
て，初期時刻 t0 から t1 ，t2 ，· · · の各時刻での状態量 X1 ，X2 ，· · · を (4.9) 式の予測式を
反復適用することで順次求めていくことになる．このような手法は，逐次前進法 (step by
step marching method)，差分法 (difference method)，離散化法 (discrete variable method) な
どと呼ばれている．(4.9) 式のオイラー法は最も基本的な手法であり，t = tn から t = tn h
までの状態量推移を予測する様々な数値積分法が考案されている．手法それぞれで向き・
不向きがあるため，実際の運用では各手法の特徴をよく理解した適切な使用が大切である．
4.1.3
時間積分手法の要求条件
微分方程式を解くためのアルゴリズムには以下の特性が要求される．
1) 適合性
2) 収束性
5) 高次モードの抑制能力
3) 安定性
4) 精度
6) 非線形問題への適用性
また問題によっては，さらに次の特性が必要な場合もある．
7) 内容の理解やプログラムのしやすさ
8) 計算時間
9) コンピュータの容量
いずれも非常に大事な特性であるが，ここでは詳細な説明はしない．特性 (1)∼(6) につい
ては文献 [8] を参照されたい．
4.1 数値積分法の概説 [8]
4.1.4
19
時間積分手法の種類
時間積分の手法としてどのような種類があるか紹介する．それぞれの詳細な内容にはこ
こでは立ち入らない．
1 階微分形と 2 階微分形 前述したように，時間積分の手法は 1 階微分方程式と 2 階微分
方程式に対して考案されたものがある．
1 階微分方程式 (標準形方程式) : ẋ = F (x, t)
2 階微分方程式 (MCK 形方程式) : M ẍ + C ẋ + Kx = fe
標準形方程式の方が一般的である反面，MCK 形方程式では 2 階微分形専用に開発されて
おり効率が良い．
単段形と多段形 単段形は時刻 tn の時点から次の時点 tn+1 の状態量 Xn+1 を求めるため
に，現時点の状態量 Xn のみから直接求める手法を指す．特徴を以下にまとめる．
• 代表的な手法：ルンゲクッタ法，ニューマーク β 法
• 過去の状態量の値を用いないため開始時や途中での時刻刻みの変更が容易であり，
折れ線の非線形要素などが存在する不連続系の解析に向く
一方，多段形は現時点 tn の状態量 Xn だけでなく，過去の時点にさかのぼった状態量
Xn−1 ，· · · も用いて時刻 tn+1 の時点での状態量 Xn+1 を求める手法を意味する．特徴は
以下に示す．
• 代表的な手法：アダムス法 (Adams method)，フーボルト法
• 過去の状態量の値も用いるため，計算制度が良い
• 計算開始時では過去の値がないため，スタータと呼ばれる解析法で数点の解析を進
めることで過去の値を用意する必要がある
• 時刻刻みの変更や不連続系の扱いが難しい
陽解法と陰解法 1 階の微分方程式の (4.4) 式である Ẋ = F (X, t) をオイラー法で解く場
合の積分公式は前述したように以下で与えられる．
Xn+1 = Xn + hF (Xn , tn )
(4.10)
オイラー法は解 X(t) を時刻 t = tn まわりで Tayler 展開して 1 次で打ち切った近似手法で
あり，時刻 tn の時点の状態量 Xn は既に求められているため，次の状態量 Xn+1 は (4.10)
式の右辺に既知の状態量を代入するだけで求められる．このように右辺に未知量を含まず
状態量 Xn+1 が陽な形で求められる解法を陽解法と呼ぶ．陽解法の特徴を以下に述べる．
• プログラム作成が容易であり，かつ計算時間も短い
• 数値積分の安定性に問題があり，時刻の刻み幅が大きくなると発散する
• 無条件安定な解法はない
第4章
20
一方，後方オイラー法と呼ばれる手法がある．
Xn+1 = Xn + hF (Xn+1 , tn+1 )
(4.11)
後方オイラー法はオイラー法と似ているが，解 X(t) を予測すべき時刻 t = tn+1 まわりで
Tayler 展開しているため，右辺の F (Xn+1 , tn+1 ) は予測すべき先の時刻 tn+1 の時点での
未知な状態量 Xn+1 の関数となっており，Xn+1 を求めるために陰形式の方程式を解く必
要がある．このように右辺に未知量 Xn+1 を含む解法を陰解法と呼ぶ．
陰解法の特徴を以下に述べる．
• 数値積分の安定性に優れており，パラメータを上手く選べば無条件安定となる
• 毎時刻の計算において，余計に代数方程式を解く必要があり，計算に時間がかかる
• 代数方程式は線形問題ならば 1 次方程式，非線形問題ならば非線形方程式となり，特
に非線形方程式はニュートン法などにより数値的に解を求めなければならない
• 自由度の大きい大規模問題には適用しづらい
微分代数方程式の求解法 [5, 6]
4.2
4.2.1
拘束式の時間微分
前章で述べたように，拘束条件付きの剛体系は (3.23) 式のように微分代数方程式となる．
{
M q̈ = Fq + CqT λ
(3.23)
C(q, t) = 0
しかしながら，拘束式 C(q, t) = 0 は代数式であるため，微分方程式である運動方程式と
の連立には不向きである．そこで，一般化座標 q の 2 階微分 q̈ が現れるまで拘束式の微
分を繰り返す．
d
C(q, t) =
dt
d2
C(q, t) =
dt2
∂C
∂C
q̇ +
= Cq q̇ + Ct
(4.12)
∂q
∂t
(
)
(
)
(
)
(
)
∂ ∂C
∂ ∂C
∂ ∂C
∂ ∂C
q̇ q̇ +
q̇ +
q̇ +
∂q ∂q
∂t ∂q
∂q ∂t
∂t ∂t
(
)
(
)
(
)
(
)
∂ ∂C
∂ ∂C
∂C
∂ ∂C
∂ ∂C
q̇ q̇ +
q̇ +
q̈ +
q̇ +
=
∂q ∂q
∂t ∂q
∂q
∂q ∂t
∂t ∂t
= (Cq q̇)q q̇ + 2Cqt q̇ + Cq q̈ + Ctt
(4.13)
ここで，以下の偏微分に関する略式の表記を用いた．
)
(
(
)
(
)
(
)
∂ ∂C
∂C
∂ ∂C
∂ ∂C
∂ ∂C
(Cq q̇)q :=
q̇ , Ct :=
, Ctt :=
, Cqt :=
=
∂q ∂q
∂t
∂t ∂t
∂t ∂q
∂q ∂t
(4.14)
なお，(Cq q̇)q q̇ を展開すると以下のようになっている．
( )T
T
T
∂C1
1
q̇ T ∂C
q̇
q̇ C1qq q̇
q̇ C1qq
∂q
∂q
∂ .
∂
.
..
..
(Cq q̇)q q̇ =
q̇ = .. q̇ =
(4.15)
.. q̇ =
.
.
∂q ∂C
∂q (
)T
T
T
m
q̇ Cmqq q̇
q̇ Cmqq
q̇
m
∂q
q̇ T ∂C
∂q
4.2 微分代数方程式の求解法 [5, 6]
21
ただし Ciqq ∈ Rn×n は Ci ∈ R のヘシアン行列である．
Ciqq
∂
:=
∂q
(
∂Ci
∂q
)T
(i = 1, · · · , m)
よって，微分代数方程式の (3.23) 式は次のようになる．
[
][ ] [ ]
M −CqT
q̈
Fq
=
Cq 0m×m λ
γ
(4.16)
(4.17)
ただし
γ(q, q̇, t) := −(Cq q̇)q q̇ − 2Cqt q̇ − Ctt
4.2.2
(4.18)
拘束力の算出
得られた変形後の (4.17) 式において，求めるべき目的変数およびその係数行列のサイズ
はそれぞれ
[ ]
[
]
T
q̈
M
−C
q
∈ R(n+m) ,
∈ R(n+m)×(n+m)
λ
Cq 0m×m
であるため，目的変数と連立方程式の本数は一致しており，係数行列が正則である限り解
を求めることができる1 ．さて，(4.17) 式を実際に数値的に解く際には，大まかに 2 つの方
法が考えられる．
1. (4.17) 式を直接的に目的変数 [q̈ T λT ]T について解く．
2. (4.17) 式から未定乗数の λ を消去して，q̈ のみの方程式にしてから数値積分を行う．
上記いずれの方法においても，未定乗数 λ を求める必要がある．運動方程式の部分を q̈
について解くと
q̈ = M −1 (Fq + CqT λ)
これを拘束式の部分 (2 階微分) に代入すれば
Cq M −1 (Fq + CqT λ) = γ
⇔ Cq M −1 CqT λ = γ − Cq M −1 Fq
)
)−1 (
(
γ − Cq M −1 Fq
⇔ λ = Cq M −1 CqT
(4.19)
となり，未定乗数を得ることができる．
(4.19) 式から分かる通り，未定乗数 λ は (q, q̇, t) のみの関数であり，q̈ を含まない．
λ = λ(q, q̇, t)
(4.20)
したがって，数値積分の際には t = 0 における初期値 (q0 , q̇0 ) を決めれば未定乗数の初期
値 λ0 = λ(q0 , q̇0 , 0) も定まることになる．
1
慣性行列 M は必ず正則であるため，拘束ヤコビ行列 Cq が非特異 (行フルランク) であれば常に正則で
ある．
第4章
22
4.3
4.3.1
拘束式の安定化
バウムガルデの安定化法 [5, 6]
前節での変形より，数値積分においては本来満たすべき代数式 C(q, t) = 0 の代わり
に，2 階微分した導関数 C̈ = 0 を扱っている．しかしながら，数値積分では計算誤差を避
けることは不可能である．またその誤差は累積していく．結果，導関数 C̈ = 0 を繰り返
し数値積分していくと，誤差が累積した結果 (ドリフト)，無視できない誤差が発生して，
C(q, t) ̸= 0 となり拘束が破られることになってしまう．
このことを，直線上の物体の位置 x が静止している状況の数値積分を例に用いて説明
する．物体が原点に静止しており，外力が働かない状況を定式化すると次のようになる．
x(t) = 0, t ∈ [0, ∞)
(4.21)
しかしながら，実際にはこの式は微分形として取扱う必要がある．(4.21) 式を 2 階微分す
れば
ẍ(t) = 0, t ∈ [0, ∞)
(4.22)
となり，時間積分する際は次の初期問題を解くことになる．
Solve ẍ(t) = 0 subject to x(0) = 0, ẋ(0) = 0
(4.23)
この (4.23) 式の数値積分における挙動を調べるため，初期値を x(tk ) = xk ，ẋ(tk ) = ẋk
としたとき，微小時間 h だけ経過後の xk+1 = x(tk + h)，ẋk+1 = ẋ(tk + h) を求めると
1
x(tk + h) = ẍ(tk )h2 + ẋ(tk )h + x(tk ), ẋ(tk + h) = ẍ(tk )h + ẋ(tk )
2
となるが，加速度 ẍ(t) は常に (4.23) 式を満たし ẍ(t) ≡ 0 であるため，
xk+1 = ẋk h + xk , ẋk+1 = ẋk
(4.24)
を得る．したがって，初期値が x0 = x(0) = 0，ẋ0 = ẋ(0) = 0 であり計算誤差がなければ
∀
k, xk = 0, ẋk = 0
(4.25)
が成り立ち，拘束条件を満足することになる．しかしながら，ある時点 k = k ′ で計算誤
差が発生して
ẋk′ = ε, ε ̸= 0
(4.26)
のように速度が 0 でなくなれば，それ以降の位置 xk は以下のように誤差が累積して行く
ことが分かる．
xk′ +n = nεh
(4.27)
よって，たとえ一度の計算誤差であっても，いつしか無視できないほど誤差が大きくなり，
拘束が成り立たなくなってしまうことが分かる．
そこで，このような計算誤差による拘束式の破れを防ぐため，人工的な復元力を (4.23)
式に加えた次の修正式を解くことを考える．
ẍ(t) = −2αẋ(t) − β 2 x(t) ⇔ ẍ(t) + 2αẋ(t) + β 2 x(t) = 0
(4.28)
4.3 拘束式の安定化
23
ここで，β 2 x(t) は x(t) = 0 への復元力を表すバネ項であり，2αẋ(t) は減衰力を表すダン
パ項である．このように，数値積分の積分誤差による拘束式の不安定現象を抑制する手法
をバウムガルデの安定化法 (Baumgarte’s stabilization method) と呼ぶ．この式の形は 1 自
由度の自由振動を表す振動系 (マスバネダンパ系)
mẍ(t) + cẋ(t) + kx(t) = 0
(4.29)
と等価であり，係数比較すれば
c
α=
, β=
2m
√
k
m
であり，特にパラメータ係数の β は固有角振動数を表していることが分かる．拘束の修
正式の (4.28) 式が x(t) = 0 に振動せずに最も速く収束するような系の特性を臨界減衰と
呼び，特性方程式の根が重根となる場合であり，そのようなパラメータ α，β の関係は以
下で与えられる．
α=β
(4.30)
この条件を満たしつつ α = β が大きいほど収束速度が大きい．実際の運用では値として
10 ∼ 20 程度が良く用いられている．
4.3.2
拘束式の修正
以上から，微分代数方程式を数値積分する際に用いるべき拘束の修正式は以下で与えら
れる．
d2
d
C(q, t) + 2α C(q, t) + β 2 C(q, t) = 0
(4.31)
2
dt
dt
拘束式の時間微分の (4.12)，(4.13) 式を考慮すれば
(Cq q̇)q q̇ + 2Cqt q̇ + Cq q̈ + Ctt + 2α (Cq q̇ + Ct ) + β 2 C = 0
となり，これを以下の形式に整理する．
Cq q̈ = γb ,
γb (q, q̇, t) := −(Cq q̇)q q̇ − 2Cqt q̇ − Ctt − 2α (Cq q̇ + Ct ) − β 2 C
(4.32)
したがって，微分代数方程式の (4.17) 式の γ を γb に置き換えれば，拘束式の安定化を考
慮した数値積分を行うことができる．なお，γ と γd の関係は以下のようになる．
γd = γ − 2α (Cq q̇ + Ct ) − β 2 C
(4.33)
第 5 章 演習問題
ここに演習問題をレポートにまとめて提出せよ．期日などについては講義の中で指示す
る．最低限，演習問題 5.1， 5.2 については実施せよ．それ以外については，自分のでき
そうな (好きな) 問題を選んで実施して良い．
5.1 剛体振子 (消去法)
Figure 5.1: 剛体振子
Fig. 5.1 のように，基準座標系 Σ0 の原点 O0 に振子の点 C がピンジョイントで固定さ
れた振子を考える．振子は長方形で質量分布は一様であるとする．質量は m，ピンジョイ
ント部から重心までの距離は l とする．振子の運動を表すための座標系 Σ1 は，原点 O1
を重心，X1 軸を長手方向に取るように設置してある．したがって，振子の位置・姿勢は
3
0
0
T
r1 = [0 pT
1 θ1 ] ∈ R で表現される．
ここではまず消去法による定式化を行う．拘束式は以下のように表現される．
C(0 r1 ) = 0
(5.1)
ただし
]
][ ] [
] [
0
0
0
0
p
−
l
cos
θ
−l
p
cos
θ
−
sin
θ
x
1
x
1
1
(5.2)
= 0 1
C(0 r1 ) := 0 p1 + 0 R1 1 pC = 0 1 +
0
0
0
py1 − l sin θ1
0
sin θ1 cos θ1
py1
[
0
第5章
26
したがって明らかに位置 0 p1 は姿勢角 0 θ1 のみで表現できる．
{
0
px1 = l cos 0 θ1
0
py1 = l sin 0 θ1
(5.3)
よって，振子の運動方程式は姿勢角 0 θ1 のみで記述される．
J 0 θ̈1 = −mgl cos 0 θ1
(5.4)
ここで J [kg·m2 ] はピンジョイントまわりの慣性モーメントである．ピンジョイント部に
粘性抵抗を考慮すれば
J 0 θ̈1 = −mgl cos 0 θ1 − c0 θ̇1
(5.5)
となる．ここで c [N·m·s] は粘性摩擦係数である．
[演習 5.1]
• 慣性モーメント J を求めよ．
• 運動方程式の (5.5) 式を x = [x1 x2 ]T := [0 θ1 0 θ˙1 ]T として 1 階の常微分方程式に変
形せよ．
• 質量 m，粘性摩擦係数 c，初期値 x(0) を適当に設定し，Matlab の 微分方程式の数
値解法の関数 ode45 などを用いてシミュレーションを行え．
• 振子の重心位置 0 p1 (t)，姿勢角 0 θ1 (t) の時間応答をプロットせよ．また，可能なら
振子の運動の様子をアニメーションにして表示せよ．
5.2
剛体振子 (拡大法)
前節のピンジョイント拘束を受ける剛体振子について，拡大法に基づいた定式化を行
う．一般化座標を q := 0 r1 ∈ R3 とする．この場合の振子の位置・姿勢に関する運動方程
式は次で与えられる．
Mq q̈ = Fg + Fv + CqT λ
(5.6)
ただし
]
[
Mq :=
mI2 02×1
01×2 J
0
0
, Fg := −mg , Fv := 0
−c0 θ̇1
0
(5.7)
ここで Fg ∈ R3 は重力の一般化力，Fv ∈ R3 は粘性摩擦の一般化力，λ ∈ R2 はラグラン
ジュの未定乗数である．また，拘束式の (5.1) 式を微分代数方程式として連立させるため，
1 階微分，2 階微分の (4.12)，(4.13) 式を求める．(5.2) 式より，
]
[
]
[
]
[
0
0 0 l0 θ̇1 cos 0 θ1
ṗx1 + l0 θ̇1 sin 0 θ1
1 0 l sin 0 θ1
, (Cq q̇)q =
, Cq q̇ = 0
Cq =
0 0 l0 θ̇1 sin 0 θ1
ṗy1 − l0 θ̇1 cos 0 θ1
0 1 −l cos 0 θ1
(5.8)
5.3 剛体振子 (拡大法・駆動拘束)
27
となる．なお，ピンジョイント拘束は時刻 t に依存しない代数拘束であるため，Ct ，Cqt ，
Ctt は全て 0 である．したがって，拘束の 2 階微分における (4.18) 式の γ は以下のように
なる．
[
]
−l0 θ̇12 cos 0 θ1
γ = −(Cq q̇)q q̇ =
(5.9)
−l0 θ̇12 sin 0 θ1
[演習 5.2]
• 運動方程式の (5.6) 式を x := [q T q̇ T ]T ∈ R6 として 1 階の常微分方程式に変形せよ．
• 質量 m，粘性摩擦係数 c，初期値 x(0) を適当に設定し，(5.9) 式を用いてシミュレー
ションを行え．
• 振子の重心位置 0 p1 (t)，姿勢角 0 θ1 (t)，拘束力 λ(t) の時間応答をプロットせよ．ま
た，可能なら振子の運動の様子をアニメーションにして表示せよ．
• バウムガルデの安定化法の (4.32) 式を用いて拘束を安定化したシミュレーションを
行え．
5.3 剛体振子 (拡大法・駆動拘束)
Figure 5.2: 剛体振子 (駆動拘束)
ここでは，前節までのピンジョイント拘束を受ける剛体振子の問題を拡張して，振子の
拘束点 C が基準座標系 Σ0 の X0 軸方向に時間関数 d(t) に従って運動する状況を考える．
この場合，拘束は駆動拘束となり，以下のように与えられる．
C d (q, t) = 0
(5.10)
第5章
28
ただし
[
]
d(t)
C d (q, t) := C(q) −
0
(5.11)
ここで C(q) は前節までで用いていた幾何拘束 (5.2) 式である．したがって，1 階微分，2
階微分の (4.12)，(4.13) 式を構成する各項は以下のようになる．
[
]
[
]
˙
¨
d(t)
d(t)
d
Cqd = Cq , (Cqd q̇)q = (Cq q̇)q , Ctd =
, Cqt
= 0, Cttd =
(5.12)
0
0
したがって，拘束の 2 階微分における (4.18) 式の γ は以下のようになる．
[
]
0 2
0
¨
−l
θ̇
cos
θ
−
d(t)
1
1
γ = −(Cqd q̇)q q̇ − Cttd =
−l0 θ̇12 sin 0 θ1
(5.13)
[演習 5.3]
• 駆動拘束内の d(t) を d(t) = Ad sin(2πfd t) として Ctd ，Cttd を求めよ．
• 質量 m，粘性摩擦係数 c，初期値 x(0) および駆動拘束の振幅 Ad ，周波数 fd を適当
に設定し，(5.13) 式を用いてシミュレーションを行え．
• 振子の重心位置 0 p1 (t)，姿勢角 0 θ1 (t)，拘束力 λ(t) の時間応答をプロットせよ．ま
た，可能なら振子の運動の様子をアニメーションにして表示せよ．
• バウムガルデの安定化法の (4.32) 式を用いて拘束を安定化したシミュレーションを
行え．
• 周波数 fd を様々な値に変化させて，応答の様子について考察せよ．
5.4
剛体 2 重振子 (拡大法)
Fig. 5.3 のような 2 重振り子を考える．振り子部を根本から順に振り子 1，振り子 2 と
する．このような 2 重振り子において，基準座標系 Σ0 の原点 O0 に振り子 1 の点 C1 が
ピンジョイントで固定されており，また，振り子 1 と振り子 2 が点 C2 においてピンジョ
イントで固定されているとする．なお，振子はいずれも長方形で質量分布は一様であると
する．
振り子 1，2 のそれぞれについて，質量は m1 ，m2 ，ピンジョイント部 C1 ，C2 から重心
までの距離は l1 ，l2 とする．振り子 1 の運動を表すための座標系 Σ1 は，原点 O1 を重心，
X1 軸を長手方向に取るように設置してあり，振り子 2 についても同様であるとする．した
3 0
3
0
T
0 T0
T
がって，振り子 1，2 の位置・姿勢はそれぞれ 0 r1 = [0 pT
1 θ1 ] ∈ R ， r2 = [ p2 θ2 ] ∈ R
で表現される．これらをまとめて一般化座標を
[ ]
0
r1
∈ R6
q := 0
r2
とする．
5.4 剛体 2 重振子 (拡大法)
29
Figure 5.3: 剛体 2 重振子
まず，点 C1 におけるピンジョイント拘束は (5.1)，(5.2) 式と同様に以下のように表現さ
れる．
C1 (q) = 0
(5.14)
ただし
[
px − l1 cos 0 θ1
= 0 1
py1 − l1 sin 0 θ1
0
C1 (q) := 0 pC1 = 0 p1 + 0 R1 1 pC1
]
(5.15)
また，点 C2 におけるピンジョイント拘束は，点 C2 を振り子 1 の原点 O1 から見た場合
と，振り子 2 の原点 O2 から見た場合で一致することから定式化される．
C2 (q) = 0
(5.16)
ただし
(
)
C2 (q) := 0 p1 + 0 R1 1 (−1 pC1 ) − 0 p2 + 0 R2 2 pC2
]
[
0
px1 + l1 cos 0 θ1 − 0 px2 − l2 cos 0 θ2
= 0
py1 + l1 sin 0 θ1 − 0 py2 − l2 sin 0 θ2
(5.17)
以上の 2 つのピンジョイント拘束 (5.14)，(5.16) 式をまとめたものが系全体の拘束式となる．
C(q) = 0
(5.18)
px1 − l1 cos 0 θ1
0
py1 − l1 sin 0 θ1
C(q) := 0
0
0
0
px1 + l1 cos θ1 − px2 − l2 cos θ2
0
py1 + l1 sin 0 θ1 − 0 py2 − l2 sin 0 θ2
(5.19)
ただし
0
第5章
30
振り子 1，2 をまとめた系全体の運動方程式は以下のようになる．
Mq q̈ = Fg + Fv + CqT λ
ただし
(5.20)
[
]
[ ]
[ ]
M1 03×3
F g1
Fv1
M :=
, Fg :=
, Fv :=
03×3 M2
F g2
Fv2
]
[
0
0
mi I2 02×1
, Fgi := −mi g , Fvi := 0 (i = 1, 2)
Mi :=
01×2 Ji
0
−ci 0 θ̇i
(5.21)
ここで Fgi ∈ R3 は重力の一般化力，Fvi ∈ R3 は粘性摩擦の一般化力，λ ∈ R4 はラグラン
ジュの未定乗数である．
[演習 5.4]
• 微分代数方程式として定式化するために必要な Cq ，Cq q̇ ，(Cq q̇)q を求めよ．
• 質量 mi ，粘性摩擦係数 ci (i = 1, 2)，初期値 x(0) を適当に設定し，バウムガルデの
安定化法の (4.32) 式を用いて拘束を安定化したシミュレーションを行え．
• 振子の重心位置 0 pi (t)，姿勢角 0 θi (t) (i = 1, 2)，拘束力 λ(t) の時間応答をプロット
せよ．また，可能なら振子の運動の様子をアニメーションにして表示せよ．
• 姿勢角 0 θ1 ，0 θ2 に外部トルク τ1 (t)，τ2 (t) が作用する形式として定式化し，適当な
トルク入力を入れて強制運動させたシミュレーションを行え．またその結果の時間
応答をプロットし，可能ならアニメーションにして表示せよ．
参考文献
[1] 原島鮮. 力学 I —質点・剛体の力学—. 裳華房, 東京, Japan, 2 月 1973.
[2] 原島鮮. 力学 II —解析力学—. 裳華房, 東京, Japan, 2 月 1973.
[3] John J. Craig. ロボティクス : 機構・力学・制御. 共立出版, 東京, Japan, 1 月 1991.
[4] 吉川恒夫. ロボット制御基礎論. コロナ社, 東京, Japan, 11 月 1988.
[5] 日本機械学会. マルチボディダイナミクス (1) —基礎理論—. コロナ社, 東京, Japan, 4
月 2006.
[6] 日本機械学会. マルチボディダイナミクス (2) —数値解析と実際—. コロナ社, 東京,
Japan, 9 月 2007.
[7] R. M. Murray, Z. Li, and S. S. Sastry. A Mathematical Introduction to ROBOTIC
MANIPULATION. CRC Press, New York, U.S.A., 1994.
[8] 日本機械学会. 数値積分法の基礎と応用. コロナ社, 東京, Japan, 7 月 2003.
[9] 足立俊明. 工科のための常微分方程式. 数理工学社, 東京, Japan, 3 月 2013.
© Copyright 2024 Paperzz
