計測自動制御学会東北支部 第 246 回研究集会 (2008.11.19)
資料番号 246-6

倒立振子型ロボットの加速起立制御
Control of standing up motion by acceleration
for inverted pendulum type robot○木村直 ∗ ，鄭聖熹 ∗∗ ，高橋隆行 ∗○ Sunao Kimura∗ , SeongHee Jeong∗∗ , Takayuki Takahashi∗*福島大学, **産業技術総合研究所*Fukushima University , **AIST
キーワード : 車輪型倒立振子 (Wheeled inverted pendulum)，加速起立 (Standing up motion byacceleration)，追従制御 (Follow-up control)，計算トルク法 (Computed torque method)

連絡先 : 〒 980-8579 福島県福島市金谷川 1 福島大学 理工学群 共生システム理工学群 高橋研究室
木村直，Tel.: (024)548-8428，Fax.: (024)548-8428，E-mail: sunao@rb.sss.fukushima-u.ac.jp

1.

はじめにように I-PENTAR は動的安定状態 (起立状態)
および静的安定状態 (着座状態) での作業が実

近年，人間の生活環境内で対人サービスを提供する人間共存型ロボットの研究が活発に行われている1) 2) ．これらのロボットにおいて，高安全性と高作業性が求められる．しかし，これらは一般的に相反する性質を持ち，両立することが困難な課題である．この課題に対し車輪型倒立振子は，非力なマニピュレータでも自重を利用すれば高作業性が実現可能であ現可能である．本論文では，車輪型倒立振子
ロボットの状態遷移，特に起立動作方法について述べる．起立動作は静的，動的に起立させる２つの方法がある．以前までの I-PENTAR
は Fig.2 上図のように，腕を前方に移動させることにより重心を移動し静的に起立していた
3) ．本論文では，動的に起立させる方法を述べる．

り，更に簡易構造，段差登坂能力，機敏性にも優れているため高安全性，高作業性を両立するのに適した機構だと考えられる．

動的に起立させる方法の一つに，Fig.2 下図のようにロボットを後方に加速させ，起立動作に移行させる方法がある．本論文ではこの筆者らは安全性と作業性の両立を目的とした倒立振子型アシストロボットI-PENTAR3) 4)を提案している．I-PENTARは，人間の生活空間の中で物理的な対人サービスを高作業で安起立方法を加速起立と呼ぶことにする．この方法により，迅速に倒立状態に遷移することが可能になる．また I-PENTAR のようにアームを有するロボットであれば，荷物運搬の際，全に提供することを目的としている．Fig.1 の–1–

Iyy
Mg

ψlg

Zb
Standing state

I-PENTAR

Sitting staterw
ZW

θw X W
Yb m wxc
Fig. 1

I-PENTARΣWΣbFig. 3 The model of a 2DOF wheeled Inverted Pendulumとみなした代表車輪と，１つの胴体を持つもBrake

Staticのとする．ここでの胴体は，車輪以外のパートを合わせて一つにしたものを指す．このモacceleration

デルを基に運動方程式を求める．なお，ロボットの運動は，車軸に垂直な平面内に限るものとする．
DynamicFig. 2

モデルにおける制御変数とパラメータをTa-ble1，2 に示す．

Standing up motions

Table 1

荷物を持ちながら起立状態に遷移できるので，作業性が向上する．

θw
ψ

以下第２章では車輪型倒立振子のモデリン

[rad]
[rad]

Control variables

Rotational angle of wheel
Inclination angle of CoG

グについて述べる．第３章では加速起立の制御方法について述べる．第４章では第３章をふまえた拘束条件の追加と目標軌道について述べる．第５章では構築した制御方法を確認するための小型の実験機について述べる．第６章では実験機を使用した加速起立の実験と結果について述べる．

2.

倒立振子の制御モデリング

2.1

モデリング

車輪型倒立振子移動ロボットのモデルをFig.3に示す．モデルは，２つの車輪を１つの車輪–2–

Table 2Mg
mwlg

[Kg]
[Kg]
[m]rw
Ig
Iyy
Iwa
Iraγ
τwcw
g

[m]
[Kgm2 ]
[Kgm2 ]
[Kgm2 ]
[Kgm2 ]
[Nm]
[Nm/(rad/s)][m/s2 ]

Parameters
Mass of Body
Mass of wheel
Length between the origin ofbody coordinates and CoG
Ridius of wheel
Moment of Inertia of bodyMoment of Inertia of body for Y axis
Wheel inertia(axis)
Motor rotor inertia(axis)Reduction ratio
Motor toruqe of wheel
Viscosity coeff. of wheel axis
Gravity acceleration

2.2

Lagrangian の導出

ERR が含まれ，それぞれ運動方程式を導くために，Fig.3 のロボット

ETM =モデルを用いて Lagrangian を導出する．

ERA =絶対座標系 ΣW から胴体固定座標系 Σb への

ERR =同次変換行列 W T b は，xc を絶対座標系での車輪回転軸の x 座標とすると，

W



Cψ


 0=  −SψTb0

0 Sψ xc

1 00 
0 Cψ rw 
0 0112 2mw r w
θ̇w
212Iwa θ̇w
21Ira γ 2 (θ̇w − ψ̇)22(5)(6)(7)となる．
重心及び車輪の運動エネルギー Tg ，Tw ，散(1)逸エネルギー D，重心のポテンシャルエネル
ギー Ug は各々以下のように求めることができる．なお，xc = rw θw であることに注意する．となる．ここで Cψ は cos ψ ，Sψ は sin ψ とする．

Tg =従って，絶対座標系ΣW に対する胴体の重心座
標W P g 及び並進速度W V g は次式のように求め=られる．
+

0
#"

WP

0 gW


=Tb 1
 lg 
1

Tw =+



xc + lg Sψ
0

= 
 rw + l g C ψ 1WVg= ẋc + ψ̇lg Cψ0−ψ̇lg SψUg =D =(2)11Mg V Tg V g + W ΩTg I g W Ωg
22Mg 2 2{rw θ̇w + 2ψ̇rw θ̇w lg Cψ + ψ̇ 2 lg2 }21 2ψ̇ Iyy(8)
2
122(mw rw+ Iwa )θ̇w
2
1Ira γ 2 (θ̇w − ψ̇)2(9)2Mg glg Cψ
(10)
1cw (θ̇w − ψ̇)2(11)2

従って，LagrangianはL = Tg +Tw −Ug となる．

(3)

2.3

運動方程式の導出

Lagrangian を用いて，ロボットの運動方程また，絶対座標系ΣW に対する胴体重心の角速式を導く．ラグランジュ方程式は次式で示さ

度 W Ωg は，れる．
0WΩg =  ψ̇ 0ddt(4)である．

µ∂L
∂ q̇i

¶−

∂L ∂D+= Qi∂qi∂ q̇ii = 1, 2, · · · n(12)ここで，Q は一般化力，q は一般化座標，n は車輪の運動エネルギー Tw は車輪の並進運動一般化座標数である．一般化座標を ψ, θw とエネルギー ETM ，車軸周りの回転エネルギー定義し，式 (12) に代入してラグランジュ方程ERA ，モータのロータ周りの回転エネルギー–3–

式を整理すると次式のようになる 5) ．

ddt

Ã∂L∂ ψ̇!

−q̈d∂L ∂D+= −τw∂ψ∂ ψ̇q̇dqd∂L∂ θ̇w¶−

τInverse
Dynamicsq̈
Motor1sq̇

1sqė Kv−e Kp+−γ 2 Ira )θ̈w + cw ψ̇ − cw θ̇w − Mg glg Sψ = −τw(13)µ−∗+(Mg lg2 + Iyy + γ 2 Ira )ψ̈ + (Mg rw lg Cψddtq̈
+− −Fig. 4 Block diagram for computed torquemethod
力すべきトルクが計算される．今回は，追従∂L∂D= τw+∂θw∂ θ̇w

制御の中のひとつの方法である計算トルク法

2(Mg rw lg Cψ − γ 2 Ira )ψ̈ + {(Mg + mw )rw+ Iwa

で制御を行う．計算トルク法を利用した制御+Ira γ 2 }θ̈w − cw ψ̇ + cw θ̇w − Mg rw lg Sψ ψ̇ 2 = τw
(14)

則は3.

加速起立制御方法τ = τ ID (q, q̇, q̈ ∗ )である．また，q̈ ∗ は制御変数に対して軌道を与え，それに追従q̈ ∗ = q̈ d − K v ė − K p e

させる制御を行う．筆者らは，計算トルク法(18)である．ここでe = q − q d ，ė = q̇ − q̇ d である．

を利用し制御を行った．

3.1

(17)

この q̈ ∗ を式 (16) の q̈ に代入することにより，制御を行う．Fig.4にブロック線図を示す7) ．計算

計算トルク法について

トルク法は制御過程で逆動力学を利用し，モ一般的に運動方程式は以下のような式で表すことができる．

デルやパラメータが適切であれば，誤差無く制御ができるという特徴がある．

τ = M (q)q̈ + C(q, q̇) + B q̇ + D(q̇) + g(q)(15)

3.2

軌道の拘束

ここで，M (q)q̈ は慣性項，C(q, q̇) は遠心力項

運動方程式 (13),(14) からモータのトルク τwとコリオリ力項，B q̇ は粘性項，D(q̇) は動摩擦項， g(q) は重力項，τ は入力トルク項を表を消去し，ψ̈ と θ̈w の関係を導くとす．

θ̈w = −

物理パラメータに基づいて与えられた運動(19)2 +I
α = (Mg + mw )rw
wa
β = M g rw l g
δ = Ira γ 2
ε = Mg lg2 + Iyy
k2 = Mg glgを実現するのに必要なトルクを求めることを逆動力学と呼ぶ．この推定値 τ ID を

τ ID (q, q̇, q̈) = M̂ (q)q̈ + Ĉ(q, q̇) + B̂ q̇+ D̂(q̇) + ĝ(q)ε + βCψ(k2 + β ψ̇ 2 )Sψψ̈ +α + βCψ
α + βCψ(16)が得られる．式(19)より，ψ とθw どちらかの軌とする．ここで「ˆ」は物理パラメータの推定道が決定すれば，もう片方の軌道が自動的に値を表す．追従すべき軌道が q d (t) で与えられ決定される．よって，２つの軌道を独立に決たとき，逆動力学を計算することにより，出定することができない．そこで，倒立状態に–4–するために重要な ψ について目標軌道を生成0.05

し，式 (19) を利用して θw の軌道を間接的に生Standing
0state成する．

Simulation

Desired

-0.05

3.3

ψ [rad]-0.1

逆動力学

-0.15
-0.2
-0.25
-0.3

制御を行う準備として，運動方程式(13),(14)ψ0 -0.35-0.4

を行列表示し，式 (19) を用いて整理すると"

d11 d12d21 d22#"ψ̈θ̈w#+"

P1P2

#=

"−τw
τw

#Fig. 52.0

1.5

Simulation of ψ

0-2

Simulation

Desired-4

θw [rad]-6-8-10-12となる．この式から θ̈w を消去するとd212 − d11 d12
p2 d12 − p1 d22
ψ̈ +d12 + d22
d12 + d22

1.0t [s]tf(20)d11 = ² + δ
d12 = d21 = βCψ − δ
d22 = α + δ
p1 = cw (ψ̇ − θ̇w ) − k2 Sψ
p2 = −β ψ̇ 2 Sψ − cw (ψ̇ − θ̇w )

τw =

0.5

0-14-16(21)-180となる．この式が計算トルク法における逆動

0.5

1.0tf

t [s]Fig. 6

1.5

2.0

Simulation of θw

力学の式となる．

tf 後も回転し続けていることが分かる．起立

3.4

後その場で静止することが望ましいので，こ目標軌道とその挙動の軌道では不十分である．よって拘束条件をψ̈, ψ̇, ψ の軌道を生成するために，初期時刻ts [s]と加速起立完了時刻tf [s]でのそれぞれの値を与えることにする．よって６つの拘束 条件を設定できる 5 次多項式 (22) を用いる 6) ．

ψ(t) =5X

ai ti(22)追加し，ψ(t) の軌道の次元を増やすことを考える．

4.

拘束条件の追加

4.1

目標軌道の次数の変更

i=0

前述した軌道では倒立後静止はしない．そさらに係数 ai は，ψ(ts )，ψ(tf )，ψ̇(ts )，ψ̇(tf )，こで拘束条件を１つ追加する．

ψ̈(ts )，ψ̈(tf ) によって決定される．
ts = 0，ψ0 = −0.349rad，tf = 0.5s とし，前項と同様ψ̈, ψ̇, ψ の軌道を生成するために，拘束条件を ψ(0) = ψ0 ，ψ(tf ) = 0，ψ̇(0) = 0，

初期時刻 ts と加速起立完了時刻 tf でのそれぞψ̇(tf ) = 0，ψ̈(0) = 0，ψ̈(tf ) = 0 として軌道をれの値を与える．更にtf 時のθ̇w の値が0になる

生成した場合のシミュレーション結果を Fig.5，条件を加える．したがって７つの拘束条件を6 に示す．

設定できる 6 次多項式 (23) を用いる．

Fig.5よりtf 後ψ = 0となり，倒立状態に遷移していることが分かる．しかし Fig.6 より θw は–5–ψ(t) =6X
i=0ai ti(23)係数 ai は，ψ(ts )，ψ(tf )，ψ̇(ts )，ψ̇(tf )，ψ̈(ts )，ψ̈(tf ) 及び後述する追加条件によって決定される．

4.2

次数変更後の軌道生成

ts = 0 とし，７つの拘束条件を
ψ(0) = ψ0 ，ψ(tf ) = 0，ψ̇(0) = 0，ψ̇(tf ) = 0，ψ̈(0) = 0，ψ̈(tf ) = 0，θ̇w (tf ) = 0

Fig. 7とする．最後の θ̇w (tf ) = 0 が追加する条件である．式 (19) を cos ψ ; 1，sin ψ ; ψ ，ψ̇ 2 ; 0Experimental robot

Gaer A

Gaer BV+ α [V]2V− α [V]2と近似し，さらに θ̇w (0) = 0 であることを考慮して積分すると

θ̇w (tf ) =k2α+βZ tf0²+βα+βψ dt −Z tfψ̈ dt (24)MotorA

MotorB0Wheelという θ̇w (tf ) の式が求められる．よって
Z tf

ψ dt =0²+βk2

Z tf

ψ̈ dt

Gear C(25)0Fig. 8という関係式が求められる．式 (25) の右辺はDouble motor method

他の拘束条件 (ψ(tf ) = 0，ψ̇(ts ) = 0) より 0 になるので，θ̇w (tf ) = 0 という条件は，ψ(t) の積5.1分を Ψ(t) とすると
Z tf

実験機のハードウェア構成

本研究用に作成した車輪型倒立振子ロボットを Fig.7 に示す．対向２輪を駆動輪として有ψ dt = 00

Ψ(tf ) − Ψ(0) = 0(26)し，右側面には角速度を計測するためのジャイロセンサが搭載されている．ハードウェアとして近似的に表せる．これらの条件により，の仕様を Table 3 に示す．

軌道を生成すると

ψ(t) =−70ψ0 6 216ψ0 5 225ψ0 4t −t +tt6f
t5f
t4f
80ψ0 3t + ψ0t3f

Table 3DOF
SizeWeightCPU
FPGASensor
Motor(0 ≤ t ≤ tf ) (27)となる．この軌道を計算トルク法に適用する．

5.

実験機についてHardware specification

2300[H] × 200[W] × 270[D] [mm]4.08 [kg]SH7144F
EP1K30TC144-2
Gyro sensor(1), Encoder(2)Maxon (4)I-PENTAR の実機は 32kg であり，新規の制御方法を確認する初期段階で使用するには暴車輪駆動部は，駆動力の増大とバックラッ走などを考慮に入れると使いづらく，そのため小型の実験機を作成した．

シュの低減を目的にダブルモータ方式を採用–6–

軌道の作成誤差が挙げられる．計算トルク法
0.2

は，パラメータやモデルの誤差が非常に小さ0.150.1

いことを前提に行う制御であるため，誤差が0.05

ψ [rad]Standing 0state

存在する分挙動に影響を及ぼす．また，今回-0.05

Error [rad]-0.1のように倒立振子を車軸に垂直な平面上でモDesired-0.15-0.2-0.3

デリングをする場合，車輪のトルク τw のみでExperiment

ψ0 -0.25

ψ と θw を制御するため，２つの軌道を独立に0.50Fig. 9

2.0

1.0

1.5

t [s]tf決定することが不可能である．そしてψ の軌道

Behavior of ψから間接的に θw の軌道を生成したため，ψ の追従性が良い代わりに，誤差のほとんどが θw

0.15の挙動に影響を及ぼしたと考えられる．よっ0.100.05て，パラメータの同定方法やモデリングを再Error [m]0

考，改善することにより，より追従性に優れθw rw [m]-0.05
-0.10-0.15-0.25-0.30た挙動を示すと予想できる．

Experiment-0.20

Desired

7.

-0.35
-0.40

0.5

0

1.01.5t [s]tf

2.0

おわりに

本論文では倒立振子の起立方法として，加Fig. 10

Behavior of θw・rw

速起立制御方法について提案した．結果として，θw の挙動に誤差が生じたが，加速起立後している．
ダブルモータ方式では Fig.8 に示すように，車軸に直結したギア C にモータに直結した 2 つのギア A，B をかませて車輪を回転させる．この時，2 つのモータに電圧差 2α[V] をかけて回転させるのでギア C を引っ張り合うことになりバックラッシュを低減できる 5) ．

起立状態に遷移可能であった．計算トルク法を利用し，優れた加速起立制御を行うには，パラメータの同定の正確性を始めとした実シス
テムとモデルの相違について考慮する必要がある．さらに，大型の実験機で実現する場合は特に，ジャイロの測定可能角速度，モータの限界出力トルク，加速起立に必要な距離や6.

スリップ等も考慮する必要もある．今後，前加速起立の実験

ψ0 = −0.253rad，tf = 1.5s という条件の下，述の改善内容を考慮した上で I-PENTAR で実現させる予定である．

実験機を使用して実験を行った．

Fig.9 が ψ の挙動， Fig.10 が θw に関する挙動である．ψ は追従性が優れているのに対し，θw

参考文献

は追従性が悪い．

θw の追従性が悪い要因として，各パラメータの同定誤差や，モータの時定数を考慮していないために起こるモデル誤差，近似による–7–

1) EMIEW2, 株 式 会 社 日 立 製 作 所:
http://www.hitachi.co.jp/rd/research/robotics/emiew2_01.html，Access:2008/11/19
2) TWENDY-ONE, 早 稲 田 大 学 菅 野 研 究 室:
http://twendyone.com，Access:2008/11/19

3) 鄭 聖 熹 ，高 橋 隆 行: “力 制 御 を 含 む 全 身 動作を用いた倒立振子型アシストロボット IPENTARの起立・着座動作”，第25回日本ロボット学会学術講演会，講演概要CDROM 3J18，(2007)
4) 鄭聖熹，佐々木裕之，高橋隆行: “倒立振子型アシストロボット I-PENTAR 用の８自由度双腕マニピュレータの開発”，計測自動制御学会
東北支部第 240 回研究集会，資料番号 240-16，(2007)
5) 田村晶子: “腰関節を有する人間共存型ロボットの安定制御に関する研究”，東北大学院情報科学研究科修士学位論文，(2005 年度)
6) 高島亨: “車輪型移動ロボットの動的効果を利用した段差昇降に関する研究”，東北大学院情報科学研究科修士学位論文，(2004 年度)
7) 小林尚登 他: ロボット制御の実際，100/102，
コロナ社，(1997)–8–