2015年9月22日版

機械電子制御工学演習テキスト
中島明
理工学部機械電子制御工学科
南山大学
改定日：平成 27 年 9 月 21 日
ii
はじめに
このテキストは，名古屋大学井上剛志先生が授業で使用されているテキストおよび MBD
協議会のセミナーで使用されたテキストを，南山大学大学院理工学研究科機械電子制御工
学専攻の機械電子制御工学演習でのテキストとして再編し，補足などを加えたものになり
ます．
iv
はじめに
目次
第1章
講義概説
1
第2章
2.1
2.2
2.3
2.4
2 次元空間での剛体の運動学
剛体の位置・姿勢の表現 . .
回転行列の性質 . . . . . . .
剛体の運動学 . . . . . . . .
剛体の静力学 . . . . . . . .
.
.
.
.
3
3
4
5
6
第3章
3.1
3.2
3.3
3.4
2 次元空間での剛体の動力学
剛体の並進・回転運動の運動方程式 . . .
ラグランジュの運動方程式 . . . . . . . .
拘束の定式化 . . . . . . . . . . . . . . .
拘束の取り扱い手法 . . . . . . . . . . .
3.4.1 消去法 . . . . . . . . . . . . . . .
3.4.2 拡大法 . . . . . . . . . . . . . . .
拘束式の取り込み –ダランベールの原理
.
.
.
.
.
.
.
9
9
10
11
12
12
13
13
3.5
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
vi
目次
第 1 章講義概説
自動車，列車，航空機，ロボット，建築機械，建築物，宇宙構造物などの機械システム
(電子機械システム) において，設計にコンピュータを用いた解析技術は必要不可欠である．
これらは様々な部品 (ボディ) が相互に連結した複雑な機構系，すなわちマルチボディシ
ステムと呼ばれている．本講義では，このような非常に広い適用範囲を持つマルチボディ
システムの運動学と動力学を取り扱うマルチボディダイナミクスについて，定式化および
プログラミング実装までを行うことができることを目指す．取り扱う数式が複雑になるこ
とを避けるため，2 次元空間に限定した剛体運動の定式化を取り扱う．また，プログラミ
ングでは汎用制御系 CAD である Matlab を用いて行い，アルゴリズムの構造により注視
できるようにする．
扱う内容は大まかに以下のようになる．
• 2 次元空間での剛体運動
• 拘束式の定式化
• 拘束力の運動方程式への取り込み手法
• 数値解析のための定式化および数値積分法
• いくつかの具体例を用いたプログラミング演習
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
ṗe = A ṗB + A ṘB B pe
(2.13)
となり，(2.11) 式を用いれば (2.13) 式は次のようになる．
A
ここで
ṗe = A ṗB + S(A θ̇B )A RB B pe = A ṗB + S(1)A RB B pe A θ̇B
]
[
][ ] [
−py
0 −1 px
∈ R2×1
=
V (p) := S(1)p =
px
py
1 0
(2.14)
(2.15)
とおけば，作用点 e の並進速度は一般化座標の速度を用いて以下のように表される．
A
ṗe = A ṗ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
ṙe = Je A ṙB
(2.18)
ただし
2.4
[
]
I2 V (A RB B pe )
Je :=
∈ R3
01×2
1
(2.19)
剛体の静力学
剛体上の点 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 剛体の静力学
7
Figure 2.4: 点 e に作用する外力・外モーメント
とすれば，仮想仕事の原理から次の関係が成り立つ．
TA
δ A reT A Fe = δ A rB
FB
(2.22)
ここで，(2.18) 式の時間微分を仮想変位に置き換えた関係式 δqe = Je δqB を用いると
T TA
TA
δ A rB
Je Fe = δ A rB
FB
となる．この式は任意の δqB について成り立つため，作用点と代表点での力の釣り合い
に関する次の式が得られる．
A
FB = JeT A Fe
(2.23)
このように，速度に関する変換行列 Je は静力学の関係式にも用いられる．(2.18)，(2.23)
式を比較すると，速度と力では作用点 e と代表点 B の関係が逆であり，変換行列 Je につ
いては転置の関係にある．このように，速度と力は双対関係にある．
8
第2章
第 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 は剛体の慣性行列である．外力・外モーメント A FB が重力および作用
点 e への外力・外モーメントで与えられる場合は，次のようになる．
[
]
mB g
A
FB = A Fg + JeT A Fe , A Fg :=
∈ R3
(3.4)
0
ここで Fg は重力の一般化力である．
3.2
ラグランジュの運動方程式
前節の並進運動に関する運動方程式は，いわゆるニュートンの第 2 法則から導かれたも
のである．また回転に関する運動方程式はオイラーにより導かれたものであり，合わせて
ニュートン・オイラーの運動方程式と呼ばれ，古典力学で導出されたものである．ここで
は解析力学で導出されたラグランジュの運動方程式について紹介する1 ．
運動が一般化座標 q ∈ Rn で表される系を考える．一般化座標とは，系の配位 (位置・
姿勢) を完全に記述する変数の組を意味する．このとき，系の運動エネルギーを K ，ポテ
ンシャルエネルギーを U とおくと，ラグランジュ関数は以下で定義される．
L := K − U
(3.5)
このとき，系のラグランジュの運動方程式は次で与えられる．
( )
d ∂L
∂L
=
+Υ
dt ∂ q̇
∂q
(3.6)
ここで Υ ∈ R2 は一般化外力のうち非保存力となるものである．また，スカラーである L
をベクトル q̇ ，q で偏微分するときの演算を以下で定義する．
 ∂L 
 ∂L 
∂ q̇
∂q
1
1
∂L
∂L
 
 
:=  ...  ∈ Rn ,
:=  ...  ∈ Rn
∂ q̇
∂q
∂L
∂L
∂ q̇n
(3.7)
∂qn
前節のように，平面の剛体の運動方程式をラグランジュの方法によって導くことを考え
る．この場合は一般化座標は q = A rB ，n = 3 となる．したがって，系の運動エネルギー
K およびポテンシャルエネルギー U は以下で与えられる．
K=
1A T
TA
Fg
ṙ MB A ṙB , U = A rB
2 B
(3.8)
これをラグランジュの運動方程式の (3.6) 式に代入すれば
∂L A
∂L
= MB A ṙB ,
= Fg
∂ q̇
∂q
(3.9)
となり，また，非保存外力 Υ は
Υ = JeT A Fe
1
(3.10)
Lagrange の運動方程式は D’Alembert の原理，仮想変位の原理，Lagrange の変分方程式，Hamilton の
原理を経て一般化座標，一般化力の表現を導入することで得られる．
3.3 拘束の定式化
11
であるから，以下の運動方程式が得られる．
MB A r̈B = A Fg + JeT A Fe
(3.11)
ラグランジュの運動方程式は，今回のような単純な剛体運動ではあまり便利さは感じら
れないが，多数の剛体が複雑に連動する系では非常に有効である．
3.3 拘束の定式化
Figure 3.2: ピンジョイント拘束を受ける剛体運動
ここでは剛体運動に何らかの拘束が存在する場合を考える．具体例として，Fig. 3.2 の
ようなピンジョイント拘束を扱う．ここで，拘束点 C には座標系 ΣC が設置されている
とすると，拘束座標系 ΣC と物体座標系 ΣB の関係は以下で与えられる．
{
A
pC = A pB + A RB B pC
(3.12)
A
θC = A θB + B θC
今回はピンジョイント拘束であるため，剛体上の点 C の位置が空間上に固定されるが，回
転は拘束がなく自由に運動できる．したがって，ピンジョイント拘束は次のような代数式
として定式化される．
C(q) = 0
(3.13)
ただし
C(q) := A pC = A pB + A RB B pC
(3.14)
このような拘束を幾何拘束と呼ぶ．ピンジョイント拘束 (3.14) 式を具体的に展開してみ
る．剛体の代表点から拘束点 C までの長さを l とすれば，
] [
][
] [
[
(
)]
A
A
pBx + l cos A θB + B θC
cos A θB − sin A θB l cos B θC
pBx
(
)
= A
+
C(q) = A
l sin B θC
pBy + l sin A θB + B θC
sin A θB cos A θB
p By
(3.15)
となる．
第3章
12
Figure 3.3: XA 方向の駆動拘束
次に，アクチュエータなどの駆動機械を用いて，ピンジョイント部の XA 方向に運動さ
せることを考えてみる．このとき，駆動により与える運動の時間関数を d(t) とすると，拘
束 (3.6) 式は時間に依存した次の形式で定式化される．
C(q, t) = 0
ただし
[
]
d(t)
C(q, t) := A pC −
0
(3.16)
(3.17)
このような拘束を駆動拘束と呼ぶ2 ．
3.4
3.4.1
拘束の取り扱い手法
消去法
ピンジョイント拘束 (3.13)，(3.15) 式より，代表点の位置 A pB = [A pBx A pBy ]T はいずれ
も姿勢角 A θB のみを用いて表現できる．
{
(
)
A
pBx = −l cos A θB + B θC
(3.18)
(
)
A
pBy = −l sin A θB + B θC
A
T
A
A
A
T
したがって，一般化座標 q = A rB = [A pT
B θB ] の 3 変数うち pB = [ pBx pBy ] の 2 変
数を消去して，A θB の 1 変数のみで系を記述することが可能である．このように幾何拘束
式を用いて変数を消去する手法を消去法と呼ぶ．なお，消去された変数を従属座標，残っ
た座標を独立座標と呼んだりもする．さらに，一般化座標の次元から拘束式の本数を引い
た数を系の自由度と呼ぶ．
2
制御工学に慣れている人にとっては，外部入力による系の駆動をこのような形で考慮することに違和感
を覚えるかもしれない．制御工学により同様の運動を実現しようとすれば，ピンジョイント部の XA 方向
への制御入力 (外力) uXA (t) を導入して，uXA (t) による軌道追従制御がすぐに思いつくだろう．そのように
実現しても問題ないが，後述するように，系の数値積分において拘束を満足するための修正法であるバウ
ムガルデ法と類似した微分方程式を解いていることになる．
3.5 拘束式の取り込み –ダランベールの原理
3.4.2
13
拡大法
消去法は系の構造が単純な場合は非常に有効であるが，剛体の数が多くなり結合 (拘束)
が増えると変数を消去する手順が一気に複雑になる3 ．そこで変数を消去せず，運動方程
式と拘束式を連立させて定式化する手法のことを，拡大法と呼ぶ．後に詳細を述べるが，
拘束を成立させるための乗数 (拘束式の本数と同じ次元) を導入する．したがって，取り
扱う変数は 3 変数に加えて乗数 2 変数の合計 5 変数となる．そのため，変数を消去する必
要がなく運動方程式と拘束式を別々に定式化すれば良いだけであり，どのような機構でも
同一の手順で定式化が可能である．
3.5 拘束式の取り込み –ダランベールの原理
拘束式を満足するためには (拘束を力学的に強制するためには)，ラグランジュの運動方
程式の (3.6) 式に外力として拘束力 FC を加える必要がある．
( )
d ∂L
∂L
=
+ Υ + FC
(3.19)
dt ∂ q̇
∂q
ここで，拘束について，
「拘束は仕事をしない」(ダランベールの原理) という仮定を導入す
4
る．
3
開リンク機構のロボットマニピュレータなどはデナビットハーテンベルクの記法など，関節の自由度，
すなわち最小自由度で系を表現する手法が確立しているが，より複雑な機構 (閉ループリンク機構など) で
はケースバイケースにならざるを得ない．
4
このような拘束は滑らかな拘束，固い拘束と呼ばれる．具体的には，(a)2 点間の距離が一定のときの相
互作用の力の仕事，(b) 質点が滑らかな釘にかけた糸で連結されてたるみなく運動するときの糸の張力の仕
事，(c)2 つの剛体が摩擦のない滑らかな面で接するときの相互作用の力の仕事，(d)2 つの剛体が摩擦有りの
面で接しており滑らず転がるときの相互作用の力の仕事，(e) 滑車にかかった糸が滑り・弛みないときの張
力の仕事，などが挙げられる．

Download Report