リーマン距離に基づく慣性誘発効果を利用した多関節ロボットの制御○関本 昌紘 ∗ , 有本 卓 ∗,† , 川村 貞夫 ∗∗立命館大学 総合理工学研究機構,†
理化学研究所 理研−東海ゴム人間共存ロボット連携センター (RTC)

Control of a Multi-joint Robot by Using an Inertia-induced Eﬀect
Based on a Riemannian Distance○ Masahiro SEKIMOTO∗ , Suguru ARIMOTO∗,† , and Sadao KAWAMURA∗∗
Research Organization of Science and Engineering, Ritsumeikan University,†
RIKEN-TRI Collaboration Center (RTC), RIKEN
Abstract: The article focuses on motion control of a multi-joint robot arm. For an under-actuated multi-jointsystem during the periods excluding the initial and ﬁnal stages of movements, the proposed control method realizes
PTP (Point-to-Point) motions by using inertia-induced eﬀects. The eﬀectiveness of the PTP control is shown
through computer simulations in the case of planar movements of a four-joint robot arm.

1.

はじめにy

一般的に，ロボットの運動制御では，「制御入力は常に与えられる」との暗の仮定のもと，議論が展開されている．
一方で，生体運動を観察すると，力の拭き・入れといった制御の勘所が，動作生成において極めて重要な役割を果たしている．ロボットと生体はともに多関節構造からなり，それらの運動は，リンク間の複雑な力学的相互作用を介して生じる．そのため，運動の創成には，多関節系全体を協調させて，巧みに操る力学原理が存在しているはずである．
最近，筆者らは，多関節系の姿勢全体の集合がリーマン多様体とみなせること 1) ，その多様体上で定義されるリーマン距離が多関節系の純粋な慣性，遠心，コリオリ力による運動（以下，慣性誘発運動と呼ぶ）に対応すること 2) から，多関節系の慣性誘発運動の度合いを測る「慣性誘発度メジャー (Inertia-induced measure)」を提案した 3) ．この
メジャーにより実際の人間のリーチング動作を計測したところ，滑らかなリーチング時の運動初期と終盤において慣性誘発度が低くなる一方，運動中盤では慣性誘発度が最も高くなった．これより，熟練リーチングにおいて，人間は自身の慣性誘発効果を効率的に利用していることを示唆した．つまり，運動初期と終盤で積極的な運動制御が行われている一方，運動中盤では慣性誘発効果に身を任せた効率的な運動が行われていると考えられる．
これらの考察をヒントに，慣性誘発効果を利用した，多関節ロボットの運動制御を考える．つまり，運動開始時は慣性誘発効果を利用できるように制御を行い，中盤では慣性誘発効果を利用することで制御入力がゼロ，もしくはゼロに近い状態を保ち，目標近傍で目標位置へ収束するよう再び制御を行う．このような運動制御が実現可能かを考える．これは，運動中盤が劣駆動系となる時間区間劣駆動系の制御問題として捉えられる．劣駆動系の制御問題としては，Acrobot4) のように，特定関節部が非駆動である場合が良く扱われるのに対し，本報告では特定の運動区間において全関節が非駆動となる場合を扱う．このような問題の取り扱いは，エネルギーの使用が制限される宇宙ロボット

q4

q3

DesiredPose: qd

q2
q1

Ox

Fig.1 Planar movements by a robot arm with four DOFs.の制御への応用も期待できる．本報告では，慣性誘発効果が支配的にはたらく４関節平面ロボットの二姿勢間の PTP(Point-to-Point) 制御において，慣性誘発運動を利用した運動制御の有効性を，数値シミュレーションにより示す．

2.

リーマン距離と多関節系の慣性誘発運動

Fig.1 に示すような n 個の回転関節がリンクで繋がる多関節系を考える．この多関節系のある姿勢を p で表すと，その取り得る姿勢全体の集合 M は自然に定義できる．このとき，実際に存在する多関節系が扱われる限り，集合 Mは，コンパクトなハウスドルフ空間となる．一方，多関節系の関節角度 qi ∈ (−π, π] (i = 1, . . . , n) による，n 個の実数の組 (q1 , . . . , qn ) の取り得る全体の集合は，ベクトル空間 Rn （配位空間とも呼ばれる）をなす．対象とする多関節系において，Rn の各組は，M の各姿勢と一対一に対応付けでき，M の各点は Rn の開集合と同相な近傍を持つので，M は n 次元位相多様体とみなせる．さらに，M の開集合 U は Rn の開集合 Ω と同相となるので，U と同相
写像 (a homeomorphism) ϕ : U → Ω の組 (U, ϕ)，つまり，M の座標近傍 (a chart) が定義できる．Ui ∩ Uj ̸= ∅ (i, j:任意) を満足する M の二つの座標近傍 (Ui , ϕi )，(Uj , ϕj )について，この座標近傍の族 (Ui , ϕi ) が M の被覆となり，かつ，その座標変換 ϕj ◦ ϕ−1: ϕi (Ui ∩ Uj ) → ϕj (Ui ∩ Uj )i

第10回システムインテグレーション部門講演会(SI2009)（2009年12月24日～26日・東京）-212-

Initial
Pose: qs

SY0013/09/0000 - 0212 © 2009 SICE

が C ∞ 級微分同相写像となるので，M は C ∞ 級微分可能多様体とみなせる 1) ．
次に，任意の ε > 0 に対する区間 I = (−ε, ε) から M への写像 c : I → M によって決まる曲線 c(t) を考える．ここで，c(t) は，t = 0 において p となる (c(0) = p を通る) ものとする．この曲線は，多関節系の姿勢の移り変わり（軌道 (an orbit)）を表す（ここで，t は助変数として用いており，時間変数を意味しない）．こうして定義される二つの曲線 c と c̄ について，点 p のまわりの座標近傍 (U, ϕ) をとり，二つの曲線の間の関係 ∼ を，d(ϕ ◦ c)d(ϕ ◦ c̄)(0) =(0)c ∼ c̄ ⇐⇒dtdt

i=1n∑

gij (p)ui v j(2)i,j=1により定義する．ここで，gij は，n × n の正定対称行列の各成分を表している．すると，この gp は M 上のリーマン計量となり，M はリーマン多様体（さらに，連結リーマン多様体 (a connected Riemannian manifold)）とみなせる．
連結リーマン多様体では，多様体上の 2 点は滑らかな曲線で結ぶことができ，その曲線の長さは測れる．それゆえ，M 内の C ∞ 級曲線 c(t)(a ≤ t ≤ b) に対して，その長さ L(c) は，∫ b∫ b√gc(t) (ċ(t), ċ(t)) dt
L(c) = ∥ċ(t)∥dt =a(3)aにより与えられる．さらに，p, p′ ∈ M に対して，c(a) =p, c(b) = p′ を満たす曲線全体のうち，L(c) の下限

d(p, p′ ) = inf L(c(t))(4)

は，リーマン距離 (a Riemannian distance) となる．特に，(3) 式および (4) 式では，α と β を正の定数として，助変数 t を s = αt + β に置き換えても，その値は変わらない
ことが知られる（文献 5, 6) 参照）．つまり，リーマン距離は，p と p′ のみによって決まる．
さらに，c(a) = p, c(b) = p′ において，リーマン距離に対応する曲線 c(t) は，M の座標近傍 (U, ϕ) における局所座標 ϕ(c(t)) = (q1 (t), · · · , qn (t)) を用いて，二階の微分方
程式q̈k (t) +n∑Γkij (c(t))q̇i (t)q̇j (t) = 0

1234

li

0.28000.28000.09500

0.09000

Center of mass [m]lgi

0.14000.14000.04750

0.04500

Mass [kg]

mi

1.407

1.078

0.24230.02552

Inertia moment [kgm2 ]Igiz

9.758 × 10−3

7.370 × 10−3

2.004 × 10−4

1.780 × 10−5

Table 2 Simulation-parameter settings(5)i,j=1-213-

Link numberi1234

Initial Pose [rad]qsi

0.36691.31720.7714

1.13211.2217

Desired Pose [rad]qdi

1.3963

1.0472

0.6981

D-Gain [Nms/rad]

Kd1ii

8050

100.5

P-Gain [Nm/rad]

Kp1ii

5000

3000

200

100

D-Gain [Nms/rad]

Kd2ii

1015

1.00.05

P-Gain [Nm/rad]

Kp2ii

150

350

151より求められる．ここで，k = 1, · · · , n であり，Γkij はク
リストッフェルの記号 (Christoﬀel symbols) と呼ばれ，行列 (gkh ) の逆行列を (g kh ) とすれば，1Γkij =2m∑
h=1(gkh∂gih∂gjh∂gij+−∂qj∂qi∂qh)(6)

で 表 さ れ る ．(5) 式 を 満 足 す る 曲 線 c(t) は 測 地 線 (ageodesic)，この微分方程式は測地線の方程式 (a geodesicequation) と呼ばれる．
一方，解析力学では，多関節系のラグランジュの運動方
程式は，{ることが知られている．M 上に，Tp M の元の長さを与える写像 gp : Tp M × Tp M → R を，C ∞ 級関数

gp (u, v) =i

Length [m](1)により定義すると，これは同値関係 (equivalence relation)をなす．それゆえ，この同値類は M の p における接ベク
トル，この接ベクトル全体の集合は M の p における接空間 Tp M となる．ここで，この同値関係は座標近傍 (U, ϕ)の選び方に依らないこと，また，Tp M は Rn と同相の実ベクトル空間の構造をとることが知られている 5, 6) ．
微分可能多様体 M の p のまわりの座標近傍 (U, ϕ) では，Tp M の基底 (∂/∂qi )(i = 1, . . . , n) が決まる．そこで，二つの接ベクトル u, v ∈ Tp M を導入する．u, v はそれぞれ，nn∑∑
u=ui (∂/∂qi )p ，v =v i (∂/∂qi )p と一意的に表せi=1

Table 1 Physical parameters of the arms
Link number

H(q(t))q̈(t) +}1Ḣ(q(t)) + S(q(t), q̇(t)) q̇(t) = u(t)2(7)と記述される 7) ．ここで，q(t) = (q1 (t), . . . , qn (t))T は関
節角度ベクトル，H(q(t)) は慣性行列，S(q(t), q̇(t))q̇(t)は遠心・コリオリ力を含む項，S(q(t), q̇(t)) は歪対称行列，u(t) は関節での制御入力トルクである．H(q(t)) の成分 hijは定数もしく関節変数の正弦関数からなるため，H(q(t))
は C ∞ 級微分可能，かつ，正定対称な行列である．
いま，(2) 式のリーマン計量において，多関節系の慣性行列 hij を gij に選んでみる. すでに述べたように，M 上の p は，q ∈ Rn と一対一に対応づけられるので，

H−1{(q)}n∑1
Γkij (c)q̇i q̇jḢ(q) + S(q, q̇) q̇ =2i,j=1(8)の関係が成り立ち（文献 7, 8) を参照），測地線の方程式((5) 式) の左辺は，H −1 を左から掛けた多関節系のダイナ
ミクス ((7) 式) の左辺と一致する．それゆえ，測地線の方程式は，u = 0 とした多関節系のダイナミクスと一致す
る．つまり，力学的には，リーマン計量の gij に慣性行列を選ぶと，測地線 c(t) は，ある初速度で制御入力無しにq(a) = p から q(b) = p′ へ向かう運動，すなわち，多関節系の純粋な慣性，遠心，コリオリ力による運動（便宜上，これを「慣性誘発運動」と呼ぶ）と解釈できる．そして，リーマン距離は，姿勢 p(= q(a)) から別の姿勢 p′ (= q(b))
への運動全体の中で (3) 式を最小にするもので，多関節系の慣性誘発運動に対応している 2) ．

3.

慣性誘発効果を利用した運動制御

いま，Fig.1 の 4 関節アームにおいて，システムの動特性を可能な限り利用し，ある静止姿勢から目標静止姿勢に到達する PTP (Point-to-Point) 制御を考える．よく知られる状態フィードバック制御などの制御法では，通常，運動中は常に制御がかかっている．これに対し，本報告では，

Initial

Initial0.4

Desiredy-component [m]0.20Desired
0.2

0-0.2

00.2

0.4

-0.4

-0.2

x-component [m]
(3)

(2)

(1)

q [rad]q [rad/s]

0.
q3

-1

0.5

0.2

0.4

0.6

0.8

1

1.2

-2

1.4

00.2

.
q2

.
q4

0.4

0.6

time [s]

1.2

01.4

u3
u4

00.2

0.4

0.6

0.8

0.2

0.4

0.6

11.2

time [s](d) Input torque u

1.4

0.8

11.2

(3)

5

0.8

0.6

11.2

01.4

0.2

0.4

11.21.4(3)

(2)

0.2

0.1

0.6

0.8

1

1.2

1.4

00.2

0.4

0.6

time [s]time [s](d) Input torque u(e) Kinetic energy

0.8

0-5
0.6

0.4

(1)

u1
u2
u3
u4

0

0.4

0.2

(c) Angular velocities q̇

(3)

(2)

(1)

0.1

0.2

0

time [s]

100.
q4.
q2

1.4

(b) Joint Angles q

0-50

time [s]u [Nm]

u2

01

0.2

Kinetic Energy [J]u [Nm]

5.
q3.
q1

-50.8

(2)

(1)

u1
u2
u3
u4

u1

(3)

(2)

50.5(c) Angular velocities q̇

(3)

(2)

(1)

q3

time [s](b) Joint Angles q101.

q3

0.4

(1)

q1
q4
q2

1.5

1

10(3)

(2)

(1)

(3)

(2)

.
q1.

q [rad]1.50.2(a) Endpoint trajectories

(1)

2

q1
q4
q2

0

x-component [m](a) Endpoint trajectories

q [rad/s]-0.4

Kinetic Energy [J]y-component [m]

0.4

0.8

1

1.2

1.4

time [s](e) Kinetic energy

Fig.3 Transient responses in the case of a movementwith joint damping

Fig.2 Transient responses in the case of a movement
without joint dampingて，制御入力を，運動中はできる限りシステムの動特性を利用し，運動開始時と終了時のみの肝心なところでのみ制御を行い，それ以外の区間では制御を行わない，運動区間劣駆動系の制御問題を考える．この問題の難しさは，運動開始時の制御が適切でなければ，リンク相互間の作用力の結果，思いもよらない過渡応答を引き起こすことにある．本報告では，多関節構造をなす運動区間劣駆動系において，慣性誘発効果を利用した運動制御法を提案し，その効果を検討する．
対象とする運動は，比較的速い運動で，慣性誘発効果が支配的にはたらく多関節運動とする．始めに，システムのラグランジュの運動方程式が (7) 式で表される場合で，初期姿
勢 q s = (qs1 , · · · , qs4 )T から目標姿勢 q d = (qd1 , · · · , qd4 )Tへ達する運動を考える．まず，t ∈ [0, tg ] においてこの二姿勢間を慣性誘発運動で動く軌道 (q g (t), q̇ g (t)) は，q s とq d を境界条件として，(5) 式の測地線の方程式を解くことで得られる．この軌道は，オフラインで計算・確認できる．
しかし，導出された軌道の両端 (q s と q d ) の速度はゼロではないため，運動開始時から，慣性誘発運動を実現することはできない．そこで，次の 3 つのフェーズにより，慣性誘発運動をできる限り利用できるようにする：(1) 静止
初期姿勢から制御により，慣性誘発運動まで導き，(2) その後は制御を切り，(3) 目標姿勢近傍に近づいたら，再度，制御を開始し，目標姿勢に収束させる．直感的なイメージ
としては，列車の運転に似ており，制御は加速区間，惰性区間，減速区間に大きく分けられる．しかし，多関節系の運動制御ではリンク間の協調に注意を払う必要があり，そのために慣性誘発運動を利用する．具体的には，フェーズ(1) と (2) の切り替え時刻を t1 ，フェーズ (2) と (3) の切り替え時刻を t2 ，慣性誘発運動の軌道を (q g (t), q̇ g (t)) とし-214-−K d1 (q̇(t) − q̇ g (t)) − K p1 (q(t) − q g (t))
u(t) =0−K q̇(t) − K (q(t) − q )d2p2d(t < t1 )(t1 ≤ t < t2 )(9)

(t2 ≤ t)と構成する．ここで，K d1 , K d2 , K p1 , K p2 ∈ ℜ4×4 はゲイン行列で，正定対角行列からなる．
Fig.2 は，Table 1 のアームパラメタ設定，Table 2 のシミュレーションパラメタ設定，t1 = 0.1[s]，t2 = 0.95[s]として，(9) 式の制御入力を用いたときのシミュレーション結果である．慣性誘発運動軌道 (q g (t), q̇ g (t)) は，二姿勢間を tg = 1.0[s] で移動するとして導出した．区間 (2)から (3) への切り替えでは，t = tg となってから区間 (3)に切り替えたのでは遅いので，それより少し早いタイミングで切り替えた．また，過大なトルク出力を防ぐため，
ulimit = ±(10, 5, 1, 0.2)T [Nm] のトルクリミッタをつけた．
Fig.2(d) と Fig.2(e) の示すように，区間 (1) と (3) では制御が行われ，運動エネルギーに変化が見られるのに対し，区間 (2) では入力は無く，運動エネルギーが保存されている．Fig.2(b) と Fig.2(c) からは，区間 (1) でアームは慣性誘発運動軌道に乗り，区間 (2) では制御入力無しにもかかわらずその軌道を追従し，区間 (3) で最終的に目標姿勢へ収束していることがわかる．また，Fig.2(c) の区間 (1)において，実際の速度プロファイルは，一度，慣性誘発運動軌道を越えてから追従している．これは，慣性誘発運動軌道の初速度はゼロでないのに対して実際の運動の初速度はゼロのため，位置と速度の両方を追従させるには，一度オーバーシュートしなければならないことによる．また，Fig.2(a) からは，運動の途中は制御を行っていないにもかかわらず，予期せぬ動きをせずに，滑らかに目標姿勢へ収束していることがわかる．区間 (1) にて慣性誘発運動軌道

への追従が一つのリンクでも実現されなかった場合，次の非制御区間では，その実現されなかったリンクの力学的な相互作用により，他の関節の応答も，慣性誘発運動から逸れてくることを確認した．これより，厳密には，区間 (1)において，すべての関節の位置，速度が慣性誘発運動に乗らなければ，非制御区間での応答は保証されない．
実世界の運動では，慣性誘発効果以外の効果を含む場合がほとんどである．そこで，次に，{H(q)q̈ +}1Ḣ(q) + S(q, q̇) q̇ + C q̇ = u2(10)のように，運動方程式に関節粘性が含まれる場合を考える．Fig.3 は，C = diag(0.03, 0.003, 1.0 × 10−5 , 1.0 ×10−6 )T [Nms/rad] とした運動方程式 ((10) 式) において，Fig.2 と同様の条件で制御を行った際のシミュレーション結果である．Fig.3(b)，Fig.3(c)，Fig.3(e) の示すように，システムに関節粘性を含む場合には，非制御区間において，軌道は，慣性誘発運動から逸れていく．しかし，Fig.3(a)の示すように，実際の姿勢はあまり崩れておらず，想定の範囲内といえる．目標姿勢への収束フェーズ (区間 (3)) では，フェーズの切り替わり直後，大きな速度変化が起きているものの，その後は滑らかに目標姿勢へ収束している．
この結果より，慣性誘発効果が支配的であれば，他の効果が多少含まれる場合でも，この制御法は有効に機能する．
本報告では，非制御区間の効果を確認するために，この区間での制御入力は完全にゼロとしたが，エネルギーの消散分を補うように制御を行えば（当初の時間区間劣駆動系の運動制御の枠組みからは議論が外れるが），Fig.2 のような結果が得られるものと考える．消散エネルギーを補う場
合でも，慣性誘発効果を効率的に使うことができれば，力尽くに制御する手法より，提案手法はエネルギー効率の観点から有利といえる．

4.

おわりに慣性誘発効果が支配的にはたらく多関節運動において，リーマン距離に基づく多関節系の慣性誘発運動を利用した運動制御法を提案し，シミュレーションによりその手法の有効性を確認した．本報告では，提案手法の中心となる「慣性誘発運動の利用効果」に焦点を当てたため，まだ他にも検討すべき事項が多々あることを認識している．たとえば，区間の切り替えポイントは時刻に依存して決めたが，時刻に依存することなく，姿勢間の距離（リーマン距離）に応じて切り替えることも考えられる．また，区間 (1),(3)における制御は，トルクリミッタ付きの PD フィードバッ
クによって行ったが，他の制御法との比較も行うべきであろう．今後は，これらの点も踏まえ，運動制御における慣性誘発運動利用の効果を調べたい．また，この運動制御を実現する機構についても考えていきたい．
[謝辞] 本研究は科研費 若手研究 (B) (20760175) の支援を受けた．
参考文献
1) S. Arimoto, “Modelling and control of multi-body mechanicalsystems Part I: A Riemannian geometry approach”, International Journal of Factory Automation, Robotics and Soft Computing, 2009, (in press).
2) M. Sekimoto, S. Arimoto, S. Kawamura, and J.-H. Bae,
“Skilled-motion plannings of multi-body systems based upon
Riemannian distance”, Proc. of the 2008 IEEE International
Conference on Robotics and Automation (ICRA2008), pp.
1233–1238, Pasadena, CA, May 19-23 2008.

-215-3) M. Sekimoto, S. Arimoto, B. I. Prilutsky, T. Isaka, and S. Kawamura,“Observation of human multi-joint arm movementfrom the viewpoint of a Riemannian distance”, Proc. of theICROS-SICE International Joint Conference 2009 (ICCASSICE2009), pp. 2664–2669, Fukuoka International Congress
Center, Fukuoka, Japan, Aug. 18-21 2009.
4) M. W. Spong, “The swing up control problem for the acrobot”,
IEEE Control Systems Magazine, Vol. 15, No. 1, pp. 49–55,1995.
5) J. Jost,Riemannian Geometry and Geometric Analysis,
Springer, Berlin, 2002.
6) S. Gallot, D. Hulin, and J. Lafontaine, Riemannian Geometory,Springer, Berlin, Germany, 2004.
7) S. Arimoto, Control Theory of Non-linear Mechanical Systems:
A Passivity-based and Circuit-theoretic Approach, Oxford Univ.
Press, Oxford, UK, 1996.
8) S. Arimoto, M. Yoshida, M. Sekimoto, and K. Tahara,“A Riemannian-geometry approach for modeling andcontrol of dynamics of object manipulation under constraints”, Journal of Robotics, Vol. 2009, p. 892801, 2009,(doi:10.1155/2009/892801).