―1―

LEGO Mindstorms EV3を用いた回転型倒立振子の最適ロバストサーボ制御
小松晃

大・木澤悟・宮脇和人The optimal robust servo control of a rotary inverted pendulum
using LEGO Mindstorms EV3
Akihiro KOMATSU, Satoru KIZAWA and Kazuto MIYAWAKI（平成28年11月30日受理）
Control of an inverted pendulum is used as verification of the application of the control
theory well. We used a rotary inverted pendulum for an experiment to verify a control theory
in this laboratory. We needed an expensive input and output device and converter. However,recently, high-performance and low-priced hardware equipped with an input and output device
appeared and a control system design was low-cost and came to be able to perform it simply.
In this study, I really performed the optimal robust servo control of the rotary inverted
pendulum using LEGO Mindstorms EV3 which was one of the hardware.
Keywords : optimal robust servo control, rotary inverted pendulum, LEGO Mindstorms EV3

1.

緒言

近年，エネルギーの省力化，低コスト化を目的にアクチュエータの数を減らした劣駆動システムの制御法に関する研究が行われており，その中でも制御理論の応用の検証として，倒立制御はよく用いられている。本研究室でも制御理論を検証するための実験に回転型倒立振子を利用してきた。しかしながら，一般に制御対象を駆動させる入出力装置，デジタルコントローラは高価である。ところが近年になり，入出力デバイスを搭載した高性能かつ低価格のハードウェアである Arduino や Raspberry Pi，LEGO Mindstorms が出現し，それらで制御系設計・解析ツールである MATLAB/Simulink を扱えるようになった。これにより，様々な制御理論を組み込んだ制御応用が手軽に行えるようになった。しかしながら，研究事例は少なく，制御性能やその精度についての報告はあまり見られない。そこで本研究では，企業から教育機関まで幅広く普及し始めている
LEGO Mindstorms EV3 に着目し，実際に回転型倒立振子制御装置を製作し，最適ロバストサーボ制御理論を適用させ，振子の安定化制御の検証を行う事
秋田高専研究紀要第52号にした。LEGO を用いる事で，制御装置の組み立ても容易であり，MATLAB/Simulink を利用できる事で制御理論の応用をも簡単に実現できる。これにより，制御工学の学習や理解に役立てられると考えた。
2. 回転型倒立振子のシステムの概要
回転型倒立振子のフレームの組み立てには，全て LEGO ブ ロ ッ ク を 用 い た。MATLAB と LEGO

Fig.1

回転型倒立振子システム

―2―
小松晃大・木澤Mindstorms EV3 の 通 信 に は Wi-Fi ド ン グ ル やEthernet アダプタを使用し，LAN による通信を行う必要があり，今回は Ethernet 通信を採用した。
図 1 に構築した回転型倒立振子のシステムを示す。
また，図 2 にこのシステムの模式図を示す。

悟・宮脇和人m2 ：振子の質量J2 ：振子の重心周りのモーメントc2：振子の粘性摩擦係数
g ：重力加速度である。また，アームと振子の長さや重さは実測値を用いる。モータの諸特性のパラメータ a，b および振子に掛かる粘性係数 c2，重心周りのモーメントJ2は角度のステップ応答実験，減衰実験により同定する。取得した値を表 1 に示す。
Table 1Fig.23.

記号
L1L2m2J2c2
gabシステムの模式図

回転型倒立振子のモデル化

図 3 は図 1 の回転型倒立振子システムをモデル化した図である。なお，モデルはアームの質量を考慮していない。

パラメータ

数値
7.50×10-2 [m]16.0×10-2 [m]4.00×10-3 [kg]7.51×10-6 [kg･m2]
1.87×10-5 [kg･m2/s]
9.81 [m/s2]
1.56×101
2.13×100

3.1. 非線形運動方程式の導出
モデル化した図 3 の回転型倒立振子を基に，ラグランジュ方程式を用いて，状態方程式を導出する。
そこで，アームおよび振子とトルクの関係を含めた非線形な運動方程式は次式となる。
（J1＋m1L12）＋ m2L12＋ sin2θ2 0J2＋m2L22
θ̈1m2L1L2 cosθ2θ̈2
1J2＋m2L22＋c1
m2L1L2 sinθ2･θ2 sin 2θ2＋J2＋m2L22J2＋m2L22･－ sin θ2 cosθ2θ1＋Fig.3

回転型倒立振子のモデル図

ここで
θ1 ：アームの角度
L1 ：アームの長さ

θ2 ：振子の角度
L2 ：振子の重心までの長さ

c2
J2＋m2L220
1m2 gL2 sinθ2 ＝ J2＋ m2L22 τ－J2＋m2L220･

θ1･

θ2（1）

しかし，使用している PF-XL モータの内部には，多段のギアが内蔵されており，振子を揺らした時にモータの軸がほとんど回転せず，モータ側から見たアームと振子の負荷の影響をほとんど受けない。そのため，式（1）の 1 行列目は振子の項を無視することができ，アームとトルクの関係式が成り立つ。そして，2 行列目はトルクの影響を受けていない。そこで，式（1）を次式のようにアームと電圧の関係式
平成29年２月

―3―LEGO Mindstorms EV3を用いた回転型倒立振子の最適ロバストサーボ制御に置き換えることができる。
0θ̈11 θ̈2－ sin θ2 cosθ2＋･θ1c2･
θ2J2＋m2L220bm2 gL2 sinθ2 ＝v－
02J2＋m2L2＋m2gL2－J2＋m2L22（2）･（4）の 2 式で与えられる。ただし，μc は動摩擦係数であ･り，sgnθ1は･1 （θ1 > 0）･･
sgnθ1＝ 0 （θ1＝0）･－1（θ1 < 0）と定義される。
3.2. 線形な運動方程式の導出
次に，前節で求めた非線形な運動方程式の式（2）を線形化する。式（3）はμc･u ＝ v － sgnθ1bという入力変換を行い，線形化すると次式が得られる。
･θ̈1＝－ aθ1＋ bu（5）

式（4）をアームが基準姿勢，振子が倒立姿勢で静止しているような平衡状態の近傍で振る舞うとして
cosθ2≃1, sinθ2≃θ2･･
θ12 sinθ2 cosθ2≃θ12θ2≃0≃≃≃≃のように近似的に線形化すると，次式が得られる。
c2m2 gL2  m2L1L2･
θ2＝0（6）2 θ̈1＋θ̈2＋2 θ2－J2＋m2L2
J2＋m2L2
J2＋m2L22
秋田高専研究紀要第52号

（7）

（8）

ここで，00A＝ 00000
m2gL2
J2＋m2L2200
B＝bm2L1L2b－J2＋m2L22

1001－a0m2L1L2ac22－J2＋m2L2J2＋m2L22

C ＝［1 0･･00］

x（t）＝［θ（t）
θ（t）
θ（t）
θ（t）］
1212である。

4. 最適ロバストサーボ制御法の設計方法
ここで，制御則は最適レギュレータの枠組みで設計するので，次の 2 次形式評価関数を考える。

∫｛ x C Cx ＋ ru ｝dt＝∫｛ y（t）＋ ru（t）｝dt

J＝∞0∞0222（9）出力 y が t →∞で 0 でない目標値に一致したとすると，入力 u も t →∞で 0 でない一定値になる。したがって式（9）の評価関数は無限大となってしまい，このままこの評価関数を用いることができない。こ
こで新しい制御入力を v ＝ u･ とし，拡大制御ベクトル xe ＝［x u］ を定義すると，拡大システムは⊥m2 gL2 sinθ2c2･＋θ2＝0J2＋m2L22J2＋m2L22bθ1＝ u0
θ2x･ ＝ Ax ＋ Buy ＝ Cx

（3）

m2L1L2θ̈1 cosθ2･＋θ̈2－ sinθ2 cosθ2θ12J2＋m2L22－･θ1･θ2

3.3. 状態方程式の導出
前節で導出した式（7）を変形すると，以下の状態方程式が得られる。

⊥θ̈1＝－aθ1＋ μc sgnθ1＋ bv0c2J2＋m2L220

よって回転型倒立振子の数学モデルは，･aθ̈1＋1 θ̈20

⊥＋00

⊥

a －μc sgn

1
m2L1L2J2＋m2L22⊥

1m2L1L2 cosθ2J2＋m2L22

式（5）および式（6）から，回転型倒立振子の線形な運動方程式は次式で表される。

―4―小松晃大・木澤x･ e ＝ Ae xe ＋ Be v
（10）
y ＝ Ce xeとなる。ただし
A 0BBe ＝
A＝
C 00
Ce ＝［C 0］
一方，適当なフィードバックによりサーボ系を構成して，出力 y が目標値 r に追従したとき，t →∞における定常状態での x，u，y，v の定常値をそれぞれ
x ∞，u ∞，y ∞，v ∞とすると，式（10）より
x･ ∞＝ Ax ∞＋ Bu ∞＝0
u･ ∞＝ v ∞＝0（11）
y ∞＝ Cx ∞＝ r
ここで定常値からの変動分をx t）－ x ∞
δx（t）＝ （（12）δu（t）＝ u（t）－ u ∞とおき，式（10）から式（11）を引き，偏差系の状態ベクトルをδxe ＝［δx δu］ のように定義するとδx･ e ＝ Aeδxe ＋ Bev（13）e ＝ y － r ＝ Ceδxe
したがって，この拡大偏差システムに対してδxe → 0となるような制御を行うと y → r が達成され，偏差e ＝ y － r は 0 となる。これを実現するために次の評価関数を定義する。
⊥⊥∫｛δx Q δx ＋ R v ｝dt∞02⊥

Je ＝eeee（14）ただし，Qe は準正定行列，Re は正の定数である。ロバストサーボ系の設計問題は最適レギュレータ問題に帰着され，式（14）を最小にする制御入力は，リカッチ方程式
AeT Pe ＋ Pe Ae － Pe Be Re－1Be Pe ＋ Qe ＝0（15）を満たす正定解 Pe > 0 を用いて
v ＝－Re－1Be Peδxe（16）で与えられる。しかしながら，式（16）はδx とδuのフィードバックで表されているため，このままでは実現できない。そこで，状態 x と偏差 e ＝ r － y の積分動作によるフィードバックの形に変換する。状
態フィードバックゲインをKe ＝－Re－1Be Pe（17）とおき，式（16）に式（12）を代入すると，
x∞xv ＝ Ke －Ke（18）uu∞となる。一方，式（11）より次のような行列表示が得られる。
A B x∞
0＝（19）r
C 0 u∞
これより，

悟・宮脇和人A B －1 0x∞＝（20）
C 0ru∞となる。また式（8）はA B xx･＝（21）C 0 uyとなる。これより
A B －1 x･x＝（22）C 0yu
式（18）に式（20）と式（22）を代入すると
x∞xv ＝ Ke －Keuu∞
A B －1 x･
A B －1 0＝ Ke－ KeC 0yr  C 0－1･A Bx＝ Ke（23）C 0－eが得られる。ここでA B －1Ke＝［K1 K2］（24）
C 0とおくと，もともとA B －1 x･x･u･ ＝ v ＝Ke＝［K1 K2］（25）
C 0－e－eであるため，式（25）を 0 から t まで積分すると
e t）dt∫（x t）－K1 （x 0）K2
u＝K1 （（26）t0となり，比例積分型の制御則が得られる。初期状態x 0）＝ 0 のとき制御則のブロック線図は図 4 のよう（になる。

⊥⊥

Fig.45.

最適ロバストサーボシステム

安定化制御実験及び実験結果⊥5.1. 安定化制御実験
ここでは，最適ロバストサーボ制御を用いて実際に倒立実験を行い，振子の安定化とアーム角度を目標角度に追従させることを目指す。なお，目標角度をθr ＝40［deg］とした。はじめにコントローラを設計するためにモデルの物理パラメータを代入した
結果，式（8）に示した回転型倒立振子システムの状
態方程式は
平成29年２月

―5―
LEGO Mindstorms EV3を用いた回転型倒立振子の最適ロバストサーボ制御

00x･ ＝0001.0000
00001.0000
0x＋
u0－15.6000
02.1300
94.8112 11.3078－0.5648－1.5439となる。次に，最適ロバストサーボ制御に基づいたコントローラを設計するために式（15）のリカッチ方程式において重み関数を1.0000Qe ＝ 00001000
000000.01000000.0100000100Re ＝50のように設計した。ここでは，振子角度と，目標角度と現在の角度の偏差に重みを置いている。コントローラのゲインは MATLAB を用いて計算した結果，
K1 ＝［4.9424 347.5033 15.9534 34.6656］
K2 ＝1.4142となった。図 5 に Simulink を用いて設計した最適ロバストサーボのブロック線図を示す。図 5（a）のSubsystem の内部を図 5（b）に示している。アームと振子のそれぞれの角度，角速度の値および，モータへの出力には設計したローパスフィルタを用いて情報を検出し，ノイズ等の不要な高周波成分を取り除いている。
5.2. 実験結果
次に設計した最適ロバストサーボ制御に基づく，回転型倒立振子の安定化の実験結果について述べる。図 6（a）は，アーム角度θ1の時間応答であり，図 6（b）は振子角度の時間応答である。なお，5 秒後に制御を開始させており，振子角度は振子が真下で静止している状態を 0［deg］としている。図 6（a）
から分かるように，アーム角度は10秒付近から目標角度である40［deg］に落ち着き始め，図 （6 b）より，振子は鉛直状態である180［deg］を維持しており，安定した倒立が行えている事が分かる。
以上の実験の結果より，本研究で設計した EV3を用いた回転型倒立振子に対し，最適ロバストサーボ制御に基づいたコントローラを設計して，適用した結果，十分倒立のための安定性を確保できたことが確認できた。

Fig.5

Fig.6
秋田高専研究紀要第52号

simulink のブロック線図

アームと振子角度の時間応答

―6―
小松晃大・木澤6.

悟・宮脇和人結言

本研究では，LEGO Mindstorms EV3 を用いて回 転 型 倒 立 振 子 シ ス テ ム を 製 作 し，MATLAB/Simulink を用いてコントローラを設計した。そして，システムに対して最適ロバストサーボ制御を適用させ，振子を鉛直に倒立状態に保持させること，およびアームを任意の角度に追従させることが可能であることを検証した。その結果，EV3 を用いた回転型倒立振子に対し，最適ロバストサーボ制御が有効であることが確かめられた。
参考文献［1］ 川 田 昌 克 「MATLAB/Simulink と 実 機 で 学
ぶ制御工学－ PID 制御から現代制御まで－」，TechShare 株式会，2013［2］橋本洋志，石井千春，小林裕之，大山恭弘「Scilabで学ぶシステム制御の基礎」， 株式会社オーム社，2007

平成29年２月