1.1 mcore-ip -...

改訂 2012.8.9

MCORE-IP 説明書

福井大学大学院工学研究科　川谷　亮治

長野県飯田工業高校　高田　直人

1 はじめに

1.1 MCORE-IP

二輪独立駆動型の移動ロボットは，(前部にキャスターなどを利用して本体を支えることが必

要となるが) 2 個のモータの回転数を適切に制御することで平面内を移動させることができる。

MCORE-IPはそのロボット本体をバランスを取りながら倒立させることを可能にした移動ロボッ

トである (図 1.1 参照)。倒立させている状態では本体が不安定であることは明らかであり，バラ

ンスをとる目的で適切なフィードバック制御が必要となる。その意味で，制御理論の学習用教材

として適していると考える。MCORE-IPと同じような目的で製作，市販されている教材として，

株式会社ヴィストンの Beauto Balancer[1]，株式会社ゼットエムピーの e-nuvo WHEEL[2] などが

ある。

図 1.1: MCORE-IP

本稿は，MCORE-IPに対して，状態空間モデルの導出，物理パラメータの同定，制御器設計，

マイコンへの制御器の実装，についてまとめたものである。

1.2 解析・設計用ソフト

本稿では，いろいろな場面でフリーソフトの Maximaと Scilabを活用する。前者は数式処理

用，後者は数値計算用である。下記のサイトからダウンロードできるので，各自のパーソナルコ

ンピュータ (PC) にインストールしてもらいたい。本稿をまとめている時点での最新バージョンは

Scilab 5.3.3，Maxima 5.27.0 である。これらの使用法については，拙著 [3]，[4] などを参照さ

れたい。

— 1 —

Maxima： http://maxima.sourceforge.net/

Scilab： http://www.scilab.org/

1.3 プログラム開発環境

本稿では，MCORE-IPに搭載されているマイコンボード TK400のプログラムの開発環境と

して YellowIDE ((有)イエローソフト) を利用するものとする。これについても PC にインストー

ルしておいてもらいたい。操作方法などの詳細についてはマニュアル [5] を参照されたい。

1.4 データ通信

MCORE-IPは移動ロボット (しかも不安定) であるので，それに対して何らかの指令を送った

り，あるいはロボット内部の様々な情報を取得しようとする場合，有線よりは無線の方が望ましい

と考える。MCORE-IPでは，XBEEを利用して無線通信を行う。XBEEの利用法に関しては

資料 [6] を参照してもらいたい。

参考までに，筆者の設定例を示しておく。ボーレートは 19200[bps]で，DataBits(8), Parity(NONE),

StopBits(1)，FlowControl(NONE) である。

表 1.1: XBEEの設定

CH ID DH DL MY

本体側 0x0C 0x1000 0x00 0x10 0x55

PC側 0x0C 0x1000 0x00 0x55 0x10

また，PC 上にも通信用のソフトを用意しておく必要がある。WindowsXP の場合は，ターミナ

ルソフト HyperTerm が標準で用意されているが，Windows7 の場合はユーザが用意する必要が

ある。TeraTerm というフリーソフトが利用できるので，必要に応じてインストールしてもらいた

い。インストール後，XBEEと同じ設定にしておくことを忘れないように。

— 2 —

2 モデリング

2.1 基本構成

図 2.2: MCORE-IP のモデル図

MCORE-IPのモデル図を図 2.2 に示す。円で表したのが駆動輪であり，その上に矩形で表し

た本体部が搭載されている。ここでは，議論を容易にする目的で，2個の駆動輪に独立に取り付け

られている駆動用の直流 (DC)モータは同一回転するものと仮定する。つまり，MCORE-IPは

前後 (紙面に対して左右方向) にしか移動しないことを前提とする。

モデリングの際に使用する座標系は図 2.2 に示すとおりである。鉛直上方に対する駆動輪の回転

角を φ，その上に搭載されている本体部の駆動輪に対する相対回転角を θ とする。いずれも時計

回りを正とする。また，モデリングの際に使用する記号の一覧を表 2.2 にまとめておく。

　

MCORE-IPは DCモータとしてマブチモータ RE-260 を使用し，タミヤのハイパワーギヤ

ボックスHE(ギヤ比は 41.7:1) を通して駆動輪と接続している。また，観測量は本体部の鉛直上方

に対する回転角 θ+φ と本体部の駆動輪に対する相対回転角 θ とし，前者はポテンショメータ，後

者はロータリーエンコーダを利用して計測している。

2.2 状態空間モデル

本章で登場する Maximaのコマンドは，ワークシート mcore-ip2.wxm にまとめてある。

2.2.1 (非線形)運動方程式

ラグランジュの運動方程式

d

dt

µ∂T

∂qi

¶− ∂T

∂qi+∂F

∂qi+∂U

∂qi= τi, (i = 1, · · · , n) (2.1)

— 3 —

表 2.2: 記号一覧

本体部質量 m [kg]

重心までの長さ L [m]

重心周りの慣性モーメント J [kgm2]

粘性抵抗係数 μb [Nms]

駆動輪質量 mt [kg]

半径 rt [m]

回転軸周りの慣性モーメント Jt [kgm2]

粘性抵抗係数 μw [Nms]

DCモータトルク定数 kt [Nm]

逆起電力定数 ke [V/(rad/s)]

内部抵抗 R [Ω]

回転子の慣性モーメント Jm [kgm2]

ギヤ比 (41.7:1) N −補正ゲイン η −重力加速度 g [m/s2]

を利用して，MCORE-IPに対する運動 (微分)方程式の導出を行う。ここで，T, F, U は，そ

れぞれ，運動エネルギ，散逸エネルギ，位置エネルギであり，qi は一般化座標，τi は一般化力で

ある。

　

MCORE-IPに対する式 (2.1) 中の各エネルギを順に求める。

　

運動エネルギ T

本体部の重心位置 (Px, Py) は

Px = rtφ+ L sin(θ + φ), Py = L cos(θ + φ) (2.2)

で与えられる。これより，本体部の運動エネルギ K1 は

K1 =m

2

³P 2x + P

2y

´+J

2(θ + φ)2 (2.3)

で与えられる。また，一つの駆動輪に対する運動エネルギ K2 は

K2 =mt

2

³rtφ´2+Jt2φ2 (2.4)

であり，DCモータの回転子に関する運動エネルギ K3 は

K3 =Jm2(N θ)2 (2.5)

となる。以上を総和することで MCORE-IPのもつ運動エネルギ T が得られる。

— 4 —

T = K1 + (K2 +K3)× 2 (2.6)

Maxima 2.1

Px:rt*phi(t)+L*sin(theta(t)+phi(t))$　/* 本体部の x 座標 */

Py:L*cos(theta(t)+phi(t))$　/* 本体部の y 座標 */

K1:m/2*(diff(Px,t)ˆ2+diff(Py,t)ˆ2)+J/2*(diff(theta(t),t)+diff(phi(t),t))ˆ2$

K2:mt/2*(rt*diff(phi(t),t))ˆ2+Jt/2*diff(phi(t),t)ˆ2$

K3:Jm/2*(N*diff(theta(t),t))ˆ2$

T:K1+(K2+K3)*2;　/* 運動エネルギ */

出力結果は省略

　

位置エネルギ U

位置エネルギ U は次式で与えられる。

U = mgL cos(θ + φ) (2.7)

Maxima 2.2

U:m*g*L*cos(theta(t)+phi(t));　/* 位置エネルギ */

g m cos (θ (t) + φ (t)) L

　

散逸エネルギ F

本体部の回転運動ならびに駆動輪と路面間の相対運動において粘性摩擦が生じるとする。各運動

に対する粘性抵抗係数を μb, μw としたとき，散逸エネルギ F は

F =μb2(θ + φ)2 +

μw2φ2 × 2 (2.8)

で与えられる。

Maxima 2.3

F:mub/2*(diff(theta(t),t)+diff(phi(t),t))ˆ2+muw/2*diff(phi(t),t)ˆ2*2; 　/* 散

逸エネルギ */

mub¡dd t θ (t) +

dd t φ (t)

¢22

+muw

µd

d tφ (t)

¶2

　

今，一般化座標を q1 = θ, q2 = φ としよう。DCモータは，相対回転角 θ と駆動輪に対してト

ルク τ を与えることから，一般化力は，τ1 = τ, τ2 = −τ となる。以上を式 (2.1) に代入すること

で次式に示す (非線形)運動方程式を得る。

— 5 —

⎧⎪⎪⎨⎪⎪⎩(J +mL2 + 2JmN

2)θ + (J +mL2 +mrtL cos(θ + φ))φ+ μb(θ + φ)−mgL sin(θ + φ) = τ

(J +mL2 +mrtL cos(θ + φ))θ + (J +mL2 + 2Jt + (m+ 2mt)r2t + 2mrtL cos(θ + φ))φ

−mrtL sin(θ + φ)(θ + φ)2 + μbθ + (μb + 2μw)φ−mgL sin(θ + φ) = −τ(2.9)

Maxima 2.4

qi:[theta(t),phi(t)]$　/* 一般化座標 */

taui:[tau,-tau]$　/* 一般化力 */

nleq:motion2(T,F,U,qi,taui)$　/* ラグランジュの運動方程式 */

trigsimp(%)$　/* 式の簡単化 */

ratsimp(%);　/* 式の簡単化 */


　

2.2.2 線形化

得られた運動方程式 (2.9) は非線形微分方程式であるが，平衡点回り (φ = θ = 0, τ = 0) の微

小運動を仮定することで，線形微分方程式に近似できる。

⎧⎪⎪⎨⎪⎪⎩(J +mL2 + 2JmN

2)θ + (J +mL(rt + L))φ+ μb(θ + φ)−mgL(θ + φ) = τ

(J +mL(rt + L))θ + (J +m(rt + L)2 + 2Jt + 2mtr

2t )φ

+μbθ + (μb + 2μw)φ−mgL(θ + φ) = −τ(2.10)

Maxima 2.5

leq:linearize(nleq,q2avp(qi),zerolist(6));　/* 線形化 */


　

上式において，DCモータが発生するトルク τ は

τ = η

µktN

Rv − ktkeN

2

Rθ

¶× 2

³= 2η(βv − αθ)

´(2.11)

で与えられるので，式 (2.11) を式 (2.10) に代入することで，MCORE-IPに対する運動方程式を

得る。ここで，η は補正ゲインを表す。

— 6 —

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎩

(J +mL2 + 2JmN2)θ + (J +mL(rt + L))φ

+(μb + 2ηktkeN

2

R)θ + μbφ−mgL(θ + φ) = 2η

ktN

Rv

(J +mL(rt + L))θ + (J +m(rt + L)2 + 2Jt + 2mtr

2)φ

+(μb − 2ηktkeN

2

R)θ + (μb + 2μw)φ−mgL(θ + φ) = −2ηktN

Rv

(2.12)

Maxima 2.6

leq1:subst([tau=2*eta*(kt*N/R*v-kt*ke*Nˆ2/R*diff(theta(t),t))],leq);


2.2.3 状態空間モデル

MCORE-IPに対する状態空間モデルを導出するために，式 (2.12) をいったん行列表現する。

M¨Z + D

˙Z + KZ = Fv (2.13)

ここで，Z = [θ, φ]T であり，

M =

"J +mL2 + 2JmN

2 J +mL(rt + L)

J +mL(rt + L) J +m(rt + L)2 + 2Jt + 2mtr

2t

#

D =

"μb + 2ηktkeN

2/R μb

μb − 2ηktkeN2/R μb + 2μw

#, K =

"−mgL −mgL−mgL −mgL

#, F =

"2ηktN/R

−2ηktN/R

#

　

ところで，MCORE-IPに対する観測量 y の一つが本体部回転角 θ+φ であることを考慮して，

式 (2.13) に対して次の正則変換を施す。"θ + φ

θ

#=

"1 1

1 0

#"θ

φ

# Ã"θ

φ

#=

"0 1

1 −1

# "θ + φ

θ

#!

その結果，

MZ +DZ +KZ = Fv (2.14)

ここで，Z = [θ + φ, θ]T であり，

M =

"J +mL(rt + L) 2JmN

2 −mrtLJ +m(rt + L)

2 + 2Jt + 2mtr2t −(mrt(L+ rt) + 2Jt + 2mtr

2t )

#

D =

"μb 2ηktkeN

2/R

μb + 2μw −(2ηktkeN2/R+ 2μw)

#, K =

"−mgL 0

−mgL 0

#

— 7 —

　

さらに，式 (2.11) で示したように，DCモータに関する物理パラメータを

α =ktkeN

2

R, β =

ktN

R(2.15)

と置き，行列表現を簡単化する。

F =

"2ηβ

−2ηβ

#, D =

"μb 2ηα

μb + 2μw −2(ηα+ μw)

#

　

式 (2.14) に対して，状態量 x を

x =

"Z

Z

#=

⎡⎢⎢⎢⎢⎣θ + φ

θ

θ + φ

θ

⎤⎥⎥⎥⎥⎦ (2.16)

と定義し，本体部回転角 θ + φ と相対回転角 θ がセンサにより直接観測できるとすることで，

MCORE-IPに対する状態空間モデルが得られる。⎧⎪⎪⎪⎨⎪⎪⎪⎩x =

"0 I

−M−1K −M−1D

#x+

"0

M−1F

#v

y =hI 0

ix

(2.17)

Maxima 2.7

ssm:motion ssm2(leq1,qi,[v])$

T:matrix([1,1,0,0],[1,0,0,0],[0,0,1,1],[0,0,1,0])$

ssmT:similar(ssm,invert(T));


— 8 —

3 物理パラメータの同定

状態空間モデル (2.17) は，表 2.2 に示す物理パラメータを含む。本章では，これらの物理パラ

メータの同定法をまとめる。なお，本章で示す物理パラメータ値は，筆者のもつ MCORE-IPに

対するものであり，部品の配置などや実験環境によって異なった値となり得る。得られた同定結果

は，適宜，以降のプログラム等に各自で反映してもらいたい。

3.1 センサゲイン

センサゲインは表 2.2 に含まれていないが，次節以降の同定実験や制御実験において必要となる

ので，最初に調べておく。

図 3.3: MCORE-IPに搭載されているセンサ

3.1.1 ポテンショメータ

ポテンショメータは，その軸の回転角に対応した電圧を出力するセンサである。電圧値は A/D

変換器を利用することでマイコン (TK400) が取得できる。したがって，回転角と出力電圧との関

係を予め調べておけば，センサの出力電圧から軸の回転角を知ることができる。特に，ポテンショ

メータでは，これらが (ある角度範囲内で) 線形の関係にあるため，変換ゲイン (単位は [rad/V])

を求めればよい。ちなみに MCORE-IPでは，本体部回転角 θ+ φ を計測するためのセンサとし

て，ポテンショメータ (緑測器：CP-2UX) を利用している (図 3.3 左)。

　

同定実験　 (プログラム id pot を TK400に書き込む)

実験 3.1 (ポテンショメータ変換ゲイン)

1. プログラム id pot を実行 (TK400のメイン電源をオン) すると，SW1 の入力待

ちとなる。SW1 を押すと，ポテンショメータの出力電圧値が LCD に表示される

(表示モード)。

2. 本体部の回転角を分度器等を利用して設定し，LCD に表示される電圧値を読み

取る。なお，表示モード中に SW1 を押すと，そのときのポテンショメータの電圧

値が LCD にセットされ，SW2 を押すと再び表示モードとなる。

3. 2 をいくつかの回転角に対して行う。

4. Scilabを利用して，最小二乗法により，変換ゲインを求める (Scilab 3.1)。

　

— 9 —

Scilab 3.1 (id pot.sce)

--> exec(’id pot.sci’);　// 同定用関数の読み込み

--> sita=[-20,-15,-10,-5,0,5,10,15,20]*%pi/180;　// 設定角度 [rad]

--> pot=[1.9921,2.1044,2.2119,2.3437,2.4658,2.6269,2.788,2.9394,3.1298]; 　

// 計測した電圧値 [V]

--> p0=[0;0];　// 探索の初期値

--> [f,popt,gropt]=leastsq(list(perror,pot,sita),p0);　// 最小二乗探索

--> popt　// 最適値

　 popt =

　　　 0.6120528　// 変換ゲイン [rad/V]

　　　-1.5370686

--> plot(pot,sita,’+’)　// 実験データのプロット

--> ss=1.85:0.01:3.25;

--> plot(ss,popt(1)*ss+popt(2)), xgrid()　// 同定した直線のプロット

--> xtitle("Sensor gain","Voltage [V]","Angle [rad]")

id pot.sci は以下に示す同定用の関数をまとめたものであり，上記のコマンドをまとめたスク

リプトファイルが id pot.sce である。同定した結果を図 3.4 に示す。

// ポテンショメータゲインの同定 (id_pot.sci)

//--- 近似する直線の方程式

function y=plin(x,param)

y=param(1)*x+param(2);

endfunction

//--- 誤差の 2乗和

function e=perror(param,x,y)

e=sum((plin(x,param)-y).^2);

endfunction

3.1.2 ロータリーエンコーダ

MCORE-IPの駆動輪に取り付けられたロータリーエンコーダ (ベストテクノロジ：BTE030)

(図 3.3 右) の分解能は 360 [pps](4逓倍) であるので，パルス数から角度 [rad] への変換ゲインは

Gainenc =π

180= 0.01745 [rad/pulse] (3.18)

で与えられる。

　

動作確認実験 1 　 (プログラム id enc0 を TK400に書き込む)

— 10 —

図 3.4: ポテンショメータゲイン

実験 3.2 (ロータリーエンコーダの動作確認)

1. プログラム id enc0 を実行すると，SW1 の入力待ちとなる。SW1 を押すと，左

右のロータリーエンコーダからの出力パルス数が LCD に表示される。

2. 左右の駆動輪を手で回し，LCD に表示される数字を確認する。駆動輪を正 (負)

方向に 1回転させると 360 だけ数字が増加 (減少)するはずである。

　ロータリーエンコーダが正しく動作していない場合，再調整が必要となる。

　

　

次に，TK400が受け取ったカウンタ値を XBEEを利用して PC に送信する。

　

動作確認実験 2 　 (プログラム id enc を TK400に書き込む)

実験 3.3 (ロータリーエンコーダのパルス数を PC へ送信)

1. PC に XBEEをセットして，通信プログラムを立ち上げる。

2. プログラム id enc を実行すると，SW1 の入力待ちとなる。SW1 を押すと，左右

のロータリーエンコーダからの出力パルス数が PC に送信される。

3. 左右の駆動輪を手で回し，PC に表示される数字を確認する。

　

　

— 11 —

表 3.3: センサゲイン

ポテンショメータ 0.6121 [rad/V] 　　　　　

ロータリーエンコーダ 0.01745 [rad/pulse]

　 (注) 右の空欄は各自が同定した結果を書き入れるために利用してもら

いたい。以下の表においても同様である。

3.2 力学系のパラメータ同定

3.2.1 m, L, rt, mt, Jt, Jm

本体部の質量 m，駆動輪の質量 mt は電子天秤等を利用して計測する。また，本体部の重心位

置 L についてはナイフエッジ法などを利用すれば計測できる。駆動輪の半径 rt はノギス等を利用

して計測し，駆動輪ならびに DCモータの回転子の慣性モーメント Jt, Jm は均一な質量をもつ円

柱と仮定して計算により求める。得られた結果を表 3.4 に示す。

表 3.4: 力学系の物理パラメータ 1

本体部質量 m 0.6270 [kg] 　　　　　

重心までの長さ L 0.0920 [m]

駆動輪質量 mt 0.0270 [kg]

半径 rt 0.0290 [m]

軸周りの慣性モーメント Jt 1.135× 10−5 (*) [kgm2]

DCモータ回転子の慣性モーメント Jm 3.09× 10−7 [kgm2]

ギヤ比 N 41.7 [-]

重力加速度 g 9.81 [m/s2]

(*) Jt = mtr2t /2 を利用して計算している。

3.2.2 J, μb

本体部の重心周りの慣性モーメント J と粘性抵抗係数 μb の同定は自由振動実験を利用して行う。

　

同定原理

本体部の駆動輪軸周りの自由振動運動に対する (線形)微分方程式は，式 (2.10) の第 1式におい

て φ = 0, τ = 0 とおくことによって得られる。

(J +mL2)θ + μbθ +mgLθ = 0 (3.19)

ここで，本体部を倒立状態ではなく，鉛直下方にぶら下げた状態にしているため，上式の左辺第 3

項の重力加速度に関する項の符号を + としている点に注意してもらいたい。

式 (3.19) を変形する。

— 12 —

θ + 2ζωnθ + ω2nθ = 0 (3.20)

ここで，

ω2n =mgL

J +mL2, 2ζωn =

μbJ +mL2

(3.21)

　

と置いた。初期条件 θ(0) = θ0, θ(0) = 0 の下で，式 (3.20) の解 θ を求めると

θ =θ0p1− ζ2

e−ζωnt cos

Ãωnp1− ζ2 t− tan−1 ζp

1− ζ2

!(3.22)

を得る。なお，粘性抵抗係数 μb が微小であるとして，ζ < 1 としている。

Maxima 3.2

deq:diff(theta(t),t,2)+2*zeta*w*diff(theta(t),t)+wˆ2*theta(t)=0$

atvalue(theta(t),t=0,theta0)$

atvalue(diff(theta(t),t),t=0,0)$

desolve(deq,theta(t));

θ (t) = e−t w ζ

⎛⎝θ0 ζ sin³t wp1− ζ2

´p1− ζ2

+ θ0 cos³twp1− ζ2

´⎞⎠　 desolve を実行すると Is wˆ2*(zeta-1)*(zeta+1) positive, negative, or zero? とたず

ねてくる。ここでは，ζ が微小であると仮定しているため n を入力する。

　

式 (3.22) から，自由振動における周期 (角周波数) と振幅の減衰率より ωn, ζ が得られ，それら

から J, μb を算出できる。しかし，実験においては観測ノイズや静摩擦などの影響を受けるため，

ここでは自由振動が

θ = a0 + a1e−a2t cos(a3t+ a4) (3.23)

で与えられると仮定し，自由振動実験の結果に最も適合すると思われるパラメータ a0 ∼ a4 を最

小二乗法により求め，そこから，J と μb を求める，という方法を採用する。

— 13 —

同定実験　 (プログラム id J を TK400に書き込む)

実験 3.4 (自由振動実験)

1. MCORE-IPのタイヤをはずす。また，ギヤボックス内のいずれかのギヤをはず

すことで DCモータと回転軸を切り離す。そして，回転軸を台の上に固定し，回

転軸を中心に本体部が自由振動を行えるようにする (図 3.5 参照)。


3. プログラム id J を実行すると，SW1 の入力待ちとなる。本体部を少し傾けた状

態で SW1 を押して手を離すと，左右のロータリーエンコーダからの出力パルス数

が PC に送信される。

4. Scilabを利用して同定を行う (Scilab 3.3)。

　

Scilab 3.3 (id J.sce)

--> exec(’id J.sci’)　// 同定用関数の読み込み

--> loadmatfile("free 111115.txt","-ascii");　// データファイルの読み込み

--> data=free 111115;

--> dnum=length(data); stime=0.01;　// データ数とサンプリングタイム

--> t=(0:stime:stime*(dnum-1))’;　// 時間

--> x0=[0;0.7;0.1;2*%pi;0];　// 初期条件 [a0;a1;a2;a3;a4]

--> xopt=xfind(x0,t,data*%pi/180)　// 最小二乗法の適用

　

--> m=0.6270; g=9.81; L=0.092;　// 本体部のパラメータ

--> a2=xopt(3); a3=xopt(4);　// 最適値

--> om=sqrt(a2*a2+a3*a3); zeta=a2/om;　// ω, ζ の計算

--> Jhat=m*g*L/(om*om); J=Jhat-m*L*L;　// 重心周りの慣性モーメント

--> mub=2*zeta*om*Jhat;　// 粘性抵抗係数

--> [J,mub]

　 PC に保存したファイル名に合わせて，データを読み込んでもらいたい。なお，データ

のサンプリングタイムは 10 [msec] である。初期条件 x0 の各要素は a0 ∼ a4 に対応する

が，自由応答からおおよその初期値を定めるとよい。

　

id J.sci は以下に示す同定用の関数をまとめたものであり，上記のコマンドをまとめたスクリ

プトファイルが id J.sce である。

// 自由振動実験 (id_J.sci)

//--- フィッティングしたい関数

function y = fsin(t,x)

— 14 —

y = x(1)+x(2)*exp(-x(3)*t).*cos(x(4)*t+x(5));

endfunction

//--- 誤差の 2乗和

function er = error_sin(x,tm,ym,wm)

er = wm.*((fsin(tm,x)-ym).^2);

endfunction

//--- 最小二乗法

function xopt = xfind(x0,t,data)

[f,xopt,gropt] = leastsq(list(error_sin,t,data,ones(t)),x0);

plot(t,data), xgrid()

yy=fsin(t,xopt);

plot(t,yy)

endfunction

図 3.5: 自由振動実験

　

自由振動実験結果の一例を示したのが図 3.6(実線) である。この実験結果に対して得られた最適

パラメータを以下に示す。

a0 = −0.001231, a1 = 0.3415, a2 = 0.1082, a3 = 7.805, a4 = −0.2824

図 3.6 中，点線で描いたのが，これらの a0 ∼ a4 を用いたシミュレーション結果である。実験結

果とよく一致していることがわかる。a2, a3 から算出した J, μb を表 3.5 に示す。

— 15 —

図 3.6: 実験結果

表 3.5: 力学系の物理パラメータ 2

本体部の重心周りの慣性モーメント J 3.981× 10−3 [kgm2] 　　　　　

粘性抵抗係数 μb 2.010× 10−3 [Nms]

3.3 電気系のパラメータ同定

3.3.1 DCモータの内部抵抗 R

DCモータ RE-260 の内部抵抗 R については電圧降下法を利用して同定する。電圧降下法とは，

DCモータの入力端子に加えた電圧に対する電流を計測し，その関係から内部抵抗を求める方法で

ある。ここでは，実験によって得られた結果を表 3.6 に示す。

表 3.6: 電気系の物理パラメータ 1

DCモータの内部抵抗 R 1.78 [Ω] 　　　　　

3.3.2 トルク定数 kt，逆起電力定数 ke，補正ゲイン η

MCORE-IPでは，ハイパーギヤボックス (ギヤ比 41.7:1) を利用して駆動輪を駆動する。その

ため，ギヤボックスを含めたトルク定数 kt，逆起電力定数 ke ならびに補正ゲイン η を同定する

ことが必要となる。ここでは，周波数応答実験によりこれらの同定を行う。

— 16 —

同定原理

駆動輪を空転させたときの (1つの駆動輪に対する)運動方程式は次式で与えられる。

J0φ+ ηαφ = ηβv (3.24)

ここで，

J0 = Jt + JmN2

式 (3.24) に対して，比例制御 v = Kp(r − φ) を施したときの閉ループシステムは

J0φ+ ηαφ+ ηβKpφ = ηβKpr (3.25)

で与えられる (比例制御を施すことで駆動系を安定化できる。ただし Kp > 0)。式 (3.25) におい

て，J0 は既知であるので，周波数応答実験から ηα, ηβ が得られる。これらに対して，式 (2.15)

を利用することで kt, ke, η を得ることができる。

　

　

同定実験　 (プログラム id freq を TK400に書き込む)

実験 3.5 (周波数応答実験 1)

1. MCORE-IPの本体部を横にした状態で床の上に置き，本体の下に適当な台を

置いて駆動輪を空転できる状態にする。


3. プログラム id freq を実行する。DCモータを回転させるので，メインスイッチ

に加えて，モータ用の電源スイッチをオンにすることを忘れないように。

4. 表 3.7 を参考に，SW1 を押して入力周波数を選定後，SW2 を押す。入力周波数と

サンプリングタイムが表示された後，SW1 の入力待ちとなる。

5. SW1 を押すことで周波数応答実験を開始する。

6. 実験終了後，SW1 を押すと計測したデータが PC へ転送される。なお，ここでは，

指定した入力周波数の No. を x としたとき，ファイル datax.txt にデータを保

存するものとする。転送を終了したら，いったん TK400の電源をオフにする。

7. 3 ∼ 6 の操作を繰り返す。


— 17 —

　本プログラムでは比例ゲイン K を 1.0f としている。他の比例ゲイン値を用いてもよ

いが，その場合，後述する Scilabの関数 fgp0 のゲイン値を変更すること。

　また，表 3.7 以外の周波数に対する実験を行いたい場合は，適宜プログラムを修正し

てもらいたい。

表 3.7: id freq 内で割りつけられた入力周波数 [Hz]

No. 　 0　　 1　　 2　　 3　　 4　　 5　　 6　

[Hz] 0.1 0.2 0.303 0.5 0.714 0.833 1.0

No. 7 8 9 10 11 12 13

[Hz] 1.25 1.429 1.667 2.0 2.5 3.333 5.0

　

Scilab 3.4 (id freq0.sci, id freq0.sce)

--> exec(’id freq0.sci’)　// 同定用関数の読み込み

--> exec(’id freq0.sce’)　// パラメータ同定

id freq0.sci の内容を以下に示す。

// 周波数応答 (id_freq0.sci)

// フーリエ積分

function [gg,pp]=ident_freq(t,om,data)

ss = intsplin(t,data.*sin(om*t))*2/t(length(t));

cc = intsplin(t,data.*cos(om*t))*2/t(length(t));

gg = sqrt(ss^2+cc^2); pp=atan(cc,ss);

endfunction

// ゲインと位相の計算

function [g,p]=ident_gp(data,fr,dt)

t=(0:dt:dt*(length(data)-1))’;

[gy,py]=ident_freq(t,2*%pi*fr,data*0.01745);

g=20*log10(gy/0.873);

p=py*180/%pi;

endfunction

　プログラム id freq では，入力の振幅を 50 [deg]=0.873 [rad] としている。

— 18 —

// J0 の計算

function J0=calc_J0()

Jt=1.135e-5; Jm=3.09e-7; N=41.7;

J0=Jt+Jm*N^2 // J0=0.0005487

endfunction

　 J0 を計算するための関数。同定結果が異なる場合は数値を更新してもらいたい。ここ

で得られた J0 を関数 fgp0 で利用している。

// 適合すべき伝達関数

// x(1):eta*alpha, x(2):eta*beta

function y = fgp0(ff,x)

Kp=1.0; J0=0.0005487;

num=Kp*x(2);

den=J0*%s^2+x(1)*%s+Kp*x(2);

P=syslin(’c’,num/den);

[frq,rep]=repfreq(P,ff);

[db,phi]=dbphi(rep);

y=[db,phi*%pi/180];

endfunction

//--- 誤差の 2乗和

function er = error0(x,ff,ym,wm)

er = wm*((fgp0(ff,x)-ym).^2)’;

endfunction


function xopt = xfind0(x0,ff,ggg,ppp,wg,wp)

data=[ggg,ppp];

wm=[wg,wp];

n=length(data); n2=n/2;

[f,xopt,gropt] = leastsq(list(error_ff0,ff,data,wm),x0);

subplot(2,1,1), plot2d1("oln",ff,data(1:n2)), xgrid()

subplot(2,1,2), plot2d1("oln",ff,data(n2+1:n)*180/%pi), xgrid()

yy=fgp0(ff,xopt);

subplot(2,1,1), plot2d1("oln",ff,yy(1:n2))

subplot(2,1,2), plot2d1("oln",ff,yy(n2+1:n)*180/%pi)

endfunction

　

id freq0.sce の内容を以下に示す。

// 実データの読み込み

loadmatfile(’data0.txt’,’-ascii’); dt0=0.2; f0=0.1; // data0


— 19 —













// ゲインと位相の計算

[g0,p0]=ident_gp(data0’,f0,dt0);














// 結果の表示

fff=[f0,f1,f2,f3,f4,f5,f6,f7,f8,f9,f10,f11,f12,f13];

ggg=[g0,g1,g2,g3,g4,g5,g6,g7,g8,g9,g10,g11,g12,g13];

ppp=[p0,p1,p2,p3,p4,p5,p6,p7,p8,p9,p10,p11,p12,p13];

subplot(2,1,1), plot2d1("oln",fff,ggg), xgrid()

subplot(2,1,2), plot2d1("oln",fff,ppp), xgrid()

// 最小二乗法（駆動輪を空転）

wm1=[1,1,1,1,1,1,1,1,1,1,1,1,1,1]; wm2=[1,1,1,1,1,1,1,1,1,1,1,1,1,1];

xopt=xfind0([0.01,0.1],fff,ggg,ppp*%pi/180,wm1,wm2)

// kt とηの計算

N=41.7;

— 20 —

etakt=xopt(2)*1.78/N;

kt=xopt(1)*1.78/etakt/N^2;

eta=etakt/kt;

[kt,eta]

　最小二乗法における wm1 と wm2 は実験データに対する重みを表す。ここでは，すべて

の実験データを等価に評価するために重みを 1 とした。もし，評価したくない，あるい

は評価すべきでない実験データがある場合は，対応する重みを 0 とすればよい。

　

得られた周波数応答実験結果の一例を図 3.7 の実線に示す。これに対して，最小二乗法を適用し

て ηα, ηβ を求めた結果

ηα = 0.003806, ηβ = 0.05295 (3.26)

を得た。これらのパラメータ値に対する周波数応答を図 3.7 中の点線で示す。両者は良好に一致し

ていることがわかる。これより，kt, ke, η を求めると，

kt = ke = 1.724× 10−3, η = 1.311 (3.27)

が得られる。


トルク定数 kt 1.724× 10−3 [Nm] 　　　　　

逆起電力定数 ke 1.724× 10−3 [V/(rad/s)]

補正ゲイン η 1.311 −

　

3.3.3 粘性抵抗係数 μw

前項では駆動輪を空転させて周波数応答実験を行ったが，本項では MCORE-IPの本体下に置

いた台をはずして，駆動輪を床に接地させた状態で同様の実験を行う。これにより駆動輪と床面に

生じる粘性抵抗係数 μw の同定を行う。なお，駆動輪を床に置くことで補正ゲイン η も変わるこ

とが予想されるので，再同定を行う。

　

同定原理

本体部を横にした状態で床に置いたときの MCORE-IPに対する運動方程式は

J1φ+ 2(μw + ηα)φ = 2ηβv (3.28)

ここで，

J1 = (m+ 2mt)r2t + 2(Jt + JmN

2)

— 21 —

図 3.7: 周波数応答実験結果

で与えられる。前項と同様に，式 (3.28) に対して，比例制御 v = Kp(r−φ) を施したときの閉ルー

プシステムに対して周波数応答実験を行うことで，μw, η を同定する。

　

同定実験　 (プログラム id freq を TK400に書き込む)

実験 3.6 (周波数応答実験 2)

1. MCORE-IPの本体部を横にした状態で床の上に置く。本体を (厳密でなくても

よいが)水平に支えるために，キャスターを利用するとよい。

2. プログラム id freq 中の比例ゲイン K を 1.5f として周波数応答実験を行う。


　

Scilab 3.5 (id freq1.sci, id freq1.sce)

--> exec(’id freq1.sci’)　// 同定用関数の読み込み

--> exec(’id freq1.sce’)　// パラメータ同定

id freq1.sci は id freq0.sci とほぼ同様なので，異なる関数のみを以下に示す。

　

// J1, alpha1, beta1 の計算

function [J1,alpha1,beta1]=calc_JJ()

m=0.6270; mt=0.027; Jt=1.135e-5; Jm=3.09e-7; N=41.7; rt=0.029;

— 22 —

kt=1.724e-3; ke=kt; R=1.78;

J1=(m+2*mt)*rt^2+2*(Jt+Jm*N^2) // J1=0.001670

alpha1=ke*kt*N^2/R // kektN^2/R=0.002904

beta1=kt*N/R // ktN/R=0.04039

endfunction

　 J1, alpha1, beta1 を計算するための関数。同定結果が異なる場合は数値を更新して

もらいたい。J1 は関数 fgp1 で利用している。

　

// 適合すべき伝達関数

// x(1):muw, x(2):eta

function y = fgp1(ff,x)

Kp=1.5; J1=0.001670; alpha1=0.002904; beta1=0.04039;

num=Kp*2*x(2)*beta1;

den=J1*%s^2+2*(x(1)+x(2)*alpha1)*%s+Kp*2*x(2)*beta1;

P=syslin(’c’,num/den);

[frq,rep]=repfreq(P,ff);

[db,phi]=dbphi(rep);

y=[db,phi*%pi/180];

endfunction

//--- 誤差の二乗和

function er = error_ff1(x,ff,ym,wm)

er = wm*((fgp1(ff,x)-ym).^2)’;

endfunction


function xopt = xfind1(x0,ff,ggg,ppp,wg,wp)

data=[ggg,ppp];

wm=[wg,wp];

n=length(data); n2=n/2;

[f,xopt,gropt] = leastsq(list(error_ff1,ff,data,wm),x0);

subplot(2,1,1), plot2d1("oln",ff,data(1:n2)), xgrid()

subplot(2,1,2), plot2d1("oln",ff,data(n2+1:n)*180/%pi), xgrid()

yy=fgp1(ff,xopt);

subplot(2,1,1), plot2d1("oln",ff,yy(1:n2))

subplot(2,1,2), plot2d1("oln",ff,yy(n2+1:n)*180/%pi)

endfunction

id freq1.sce の内容を以下に示す。これも id freq0.sce とほぼ同様なので，異なる部分の

みを示す。

// 最小二乗法

— 23 —

wm1=[1,1,1,1,1,1,1,1,1,1,1,1,1,1]; wm2=[1,1,1,1,1,1,1,1,1,1,1,1,1,1];

xopt=xfind1([0.001,1.0],fff,ggg,ppp*%pi/180,wm1,wm2)

// μ wとη

muw=xopt(1)

eta=xpot(2)

　

得られた周波数応答の実験結果の一例を図 3.7 の実線に示す。これに対して，最小二乗法を適用

して μw, η を求めた結果

μw = 1.929× 10−3, η = 1.330 (3.29)

を得た。これらのパラメータ値に対する周波数応答を図 3.7 中の点線で示す。両者は良好に一致し

ていることがわかる。


粘性摩擦係数 μw 1.929× 10−3 [Nms] 　　　　　

補正ゲイン η 1.330 −

図 3.8: 周波数応答実験結果

　

MCORE-IPは本体を直立させてバランスをとることを制御目的としているため，駆動系のパ

ラメータ同定を行う場合，軽量のアームなどを利用して本体を直立させた状態で周波数応答実験を

行うべきであるかもしれない。ただ，本項で行ったような本体を横にして得られた結果と直立させ

— 24 —

て得られた結果を比較すると大きな差が見られない。周波数応答実験を行う場合，本体を横にして

行う方が容易である。そこで，本項で得られた物理パラメータをそのまま MCORE-IPのモデル

に利用することにする。

3.4 数値状態空間モデル

前節までで同定したパラメータを表 3.10 にまとめる。これらを式 (2.17) に代入することで，式

(3.30) に示す数値状態空間モデルを得る。

表 3.10: 物理パラメータ値

本体部質量 m 0.6270 [kg] 　　　　　

重心までの長さ L 0.092 [m]

重心周りの慣性モーメント J 3.981× 10−3 [kgm2]

粘性抵抗係数 μb 2.010× 10−3 [Nms]

駆動輪質量 mt 0.0270 [kg]

半径 rt 0.0290 [m]

回転軸周りの慣性モーメント Jt 1.135× 10−5 [kgm2]

粘性抵抗係数 μw 1.929× 10−3 [Nms]

DCモータトルク定数 kt 1.724× 10−3 [Nm]

逆起電力定数 ke 1.724× 10−3 [V/(rad/s)]

内部抵抗 R 1.78 [Ω]

回転子の慣性モーメント Jm 3.09× 10−7 [kgm2]

補正ゲイン η 1.330 [-]

ギヤ比 N 41.7 [-]

重力加速度 g 9.81 [m/s2]

ポテンショメータゲイン 0.6121 [rad/V]

エンコーダゲイン 0.01745 [rad/pulse]

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

x =

⎡⎢⎢⎢⎢⎣0 0 1 0

0 0 0 1

55.76 0 −0.06189 −1.44275.73 0 2.226 −13.52

⎤⎥⎥⎥⎥⎦ x+⎡⎢⎢⎢⎢⎣

0

0

18.17

153.3

⎤⎥⎥⎥⎥⎦ v

y =

"1 0 0 0

0 1 0 0

#x

(3.30)

ここで，

x = [ θ + φ, θ, θ + φ, θ ]T

　

— 25 —

Scilab 3.6

--> exec(’ip model.sci’)　// 状態空間モデル作成用関数

--> sys=ip model();　// 状態空間モデルの作成

// 状態空間モデル作成用関数 (ip_model.sci)

function sys=ip_model()

// 物理パラメータ

m = 0.6270; // 本体部の質量

L = 0.0920; // 本体部の重心までの距離

J = 3.981e-3; // 本体部の重心周りの慣性モーメント

rt = 0.0290; // 駆動輪の半径

mt = 0.0270; // 駆動輪の質量

Jt = 1.135e-5; // 駆動輪の慣性モーメント

kt = 1.724e-3; // トルク定数

ke = 1.724e-3; // 逆起電力定数

R = 1.780; // DCモータ内部抵抗

Jm = 3.09e-7; // DCモータ回転子の慣性モーメント

N = 41.7; // ギヤ比

mub = 2.010e-3; // 粘性抵抗係数

muw = 1.929e-3; // 粘性抵抗係数

eta = 1.330; // 補正ゲイン

g = 9.81; // 重力加速度

// 状態空間モデル

M=[J+m*L*(rt+L),2*Jm*N^2-m*rt*L;

J+m*(rt+L)^2+2*Jt+2*mt*rt^2,-(m*rt*(L+rt)+2*Jt+2*mt*rt^2)];

D1=[mub,2*eta*kt*ke*N^2/R;mub+2*muw,-2*(eta*kt*ke*N^2/R+muw)];

K1=[-m*g*L,0;-m*g*L,0];

F=[2*eta*N*kt/R;-2*eta*N*kt/R];

I=[eye(2,2)];

A=[zeros(2,2),eye(2,2);-inv(M)*K1,-inv(M)*D1];

B=[0;0;inv(M)*F];

C=[1,0,0,0;0,1,0,0];

D=[0;0];

sys=syslin(’c’,A,B,C,D);

endfunction

— 26 —

4 状態空間モデルの解析と制御器設計

本章では，状態空間モデル (x = Ax+Bu

y = Cx(4.1)

に対する解析ならびに制御器設計についてまとめる。以下では，上式の第 1式を状態方程式，第

2 式を観測方程式と呼ぶ。なお，観測方程式は，より一般的には y = Cx + Du と書けるが，

MCORE-IPの場合，D = 0 であるので，本稿でも D = 0 とする。

Scilab 4.1

--> exec(’ip model.sci’)　// 状態空間モデル作成用関数

4.1 漸近安定性

状態空間モデルの漸近安定性に対して，次に示す定理が成り立つ。

　

定理 4.1 (漸近安定性)

　状態空間モデル (4.1) が漸近安定であるための必要十分条件は，行列 A のすべての

固有値の実部が負であることである。

　

以下では，行列 A の固有値を極と呼ぶ。

　

それでは MCORE-IPに対する極を求めてみよう。

Scilab 4.2


--> spec(sys.A)　// 極の計算

　 ans =

　　　 0

　　　 6.9934087

　　　-6.5919231

　　　-13.981854

実部が正の極 6.993 をもつことから，MCORE-IPが不安定な制御対象であることがわかる。

　

ここで，極のもつ物理的な意味を考えておく。6.993, −6.592は本体部に関する極である。また，

DCモータは本質的に積分特性をもつので，極 0 はその積分特性を表しており，残る −13.98は駆動系の特性を表している。

— 27 —

4.2 可制御性

状態空間モデルの可制御性に対して，次に示す定理が成り立つ。

　

定理 4.2 (可制御性)

　状態空間モデル (4.1) が可制御であるための必要十分条件は，次に示す可制御行列 V

がフル (行)ランクをもつ (rank(V)=n) ことである。ここで，n は状態量 x の要素数

(行列 A の列数もしくは行数) を意味する。

V = [B,AB, · · · , An−1B] (4.2)

　

可制御性は，状態方程式に関連した性質であることから，状態方程式が可制御である，あるい

は，対 (A,B) が可制御である，という言い方をすることもある。

　

それでは MCORE-IPに対して，可制御性を確かめてみよう。

Scilab 4.3

--> V=cont mat(sys);　// 可制御行列の計算

--> rank(V)　// ランクの計算

　 ans =

　　　 4.

MCORE-IPの状態量の数は 4であり，可制御行列 V のランクが 4であることから，MCORE-IP

が可制御であることを示すことができる。

4.3 状態フィードバック制御

4.3.1 極配置法

状態方程式が可制御であるとき，次に示す定理が成り立つ。

　

定理 4.3 (極配置)

　状態方程式が可制御であるならば，行列 A−BK の固有値を任意に指定可能な行列

K が存在する。

　

状態方程式 x = Ax+Bu に対して，状態フィードバック制御

u = −Kx (4.3)

を施したとしよう。このときの閉ループシステムに対する状態方程式は

— 28 —

x = (A−BK)x (4.4)

で与えられる。定理 4.3 は，状態方程式が可制御であるとき，状態フィードバック制御により，閉

ループシステムの極 (A− BK の固有値をレギュレータ極と呼ぶ) を自由に指定できることを意

味している。

　

MCORE-IPは不安定な制御対象であるが可制御であるため，状態フィードバック制御を施す

ことで，すべての極の位置を自由に移動させることができる。つまり，不安定な極を安定な領域

(複素平面上で虚軸の左側の領域) に移動させることができる。このことは，状態フィードバック

制御により，閉ループシステムを漸近安定にできることを意味している。

　

一般に，システムのもつ極は，そのシステムの時間応答に密接に関係している。したがって，希

望する応答からレギュレータ極を定め，行列 A−BK の固有値がそれらになるように状態フィー

ドバックゲイン K を決定することは有効な設計法となり得る。これが極配置法である。極配置

法では，指定する極が設計パラメータとなり，その適切な選定が重要になる。具体的な設計例につ

いては後述する。

4.3.2 最適レギュレータ法

極の実部はシステムの応答速度に強く関係しており，極の絶対値が大きいほど応答速度は速い。

しかし，速い応答を実現するためには，一般的に多くの操作量をシステムに対して投入する必要が

あるが，現実のシステムでは与えることができる操作量には上限がある。そのために，制御システ

ムの設計においては，操作量の大きさについても注意を払う必要がある。応答速度と操作量という

トレードオフの関係にある量を評価関数の中に組み込んで，その評価関数を最小にするように制御

器を設計する方法が最適レギュレータ法である。

　

具体的な評価関数 J を次式に示す。

J =

Z ∞0

(xTQx+ uTRu)dt (4.5)

被積分関数の第 1項目が応答の二乗面積に相当しており，これが小さい方が応答速度が速い。ま

た，第 2項目が操作量の二乗面積に相当しており，これが小さい方が操作量が小さい。行列 Q, R

はいずれも正方行列で，トレードオフを考える際の重みであり，設計者が適切に与えるべき設計パ

ラメータである。なお，Q は準整定行列 (Q ≥ 0)，R は整定行列 (R > 0) であることが要求され

るが，実際の設計においては，これらの行列は対角行列に選ばれることが多い。その場合，Q の

対角要素は 0 もしくは正 (なお，0 とする場合，後述する可観測性が満たされなければならない)，

R の対角要素は正に選ぶ必要がある。

　

今，状態方程式 x = Ax+Bu が与えられ，式 (4.5) 内の行列 Q, R が選定されているとしよう。

このとき，評価関数 J を最小にする制御器は，次式に示す状態フィードバック制御として与えら

れる。

u = −R−1BTXx (4.6)

— 29 —

ここで，X は次式に示すリカッチ方程式の解である。

ATX +XA−XBR−1BTX +Q = 0 (4.7)

具体的な設計例については後述する。

4.4 状態オブザーバ

状態フィードバック制御 u = −Kx を (可制御な)状態方程式に適用することで，レギュレータ

極を自由に与えることができた。あるいは，式 (4.5) の評価関数を最小にすることができた。これ

らの意味で，状態フィードバック制御は有効な制御方策であるといえる。

　

ところで，状態フィードバック制御の実現には，状態量 x のすべてが既知である必要がある。し

かし，一般のシステムにおいては，必ずしもすべての状態量が計測できるわけではない。どの状態

量 (もしくは状態量の線形結合) が計測できるのかを表しているのが観測方程式 y = Cx である。

行列 C が単位行列 (あるいはより一般的に，正則な行列) のとき，状態量がすべて既知であること

を意味するが，通常，行列 C は (フル行ランクをもつ) 横長の行列となる。

　

そこで，本節では観測量 y から状態量 x を推定することを考える。これができれば，状態量の

推定量 x を利用した状態フィードバック制御の実現が可能となる。この役割を果たすのが状態オ

ブザーバである。

4.4.1 全状態オブザーバ

次に示す微分方程式を考える。ここで，H をオブザーバゲインと呼ぶ。

˙x = Ax+Bu+H(y − Cx) (4.8)

もし，式 (4.1) の状態量 x に対して

limt→∞

(x− x) = 0 (4.9)

が成り立てば，式 (4.8) の微分方程式の解である x は状態量 x の推定量として考えてよい。

　

そこで，式 (4.1) の状態方程式と式 (4.8) の差を計算すると，次式を得る。ここで e = x− x で

あり，推定誤差を意味する。

e = (A−HC)e (4.10)

システムの状態量 xが計測できないということは，もちろん，初期状態 x(0)も計測できない。しか

し，微分方程式 (4.8)を解くためには，初期状態 x(0) を定める必要がある。e(0) = x(0)− x(0) 6= 0であることが一般的であり，この条件の下で，式 (4.9) が成り立つためには，行列 A−HC が漸近

安定でなければならない。MCORE-IPもそうであるが，行列 A は必ずしも漸近安定であるわけ

ではないが，行列 A −HC には設計者が定めてよいオブザーバゲイン H が含まれている。これ

— 30 —

を適切に選定することで行列 A−HC を漸近安定にできるかもしれない。このことに関して，次

の定理が成り立つ。

　

定理 4.4 (可観測性)

　状態空間モデル (4.1) が可観測であるための必要十分条件は，次に示す可観測行列

W がフル (列)ランクをもつことである。ここで，n は状態量 x の要素数 (行列 A の

列数もしくは行数) を意味する。

W =

⎡⎢⎢⎢⎢⎣C

CA...

CAn−1

⎤⎥⎥⎥⎥⎦ (4.11)

　

　

定理 4.5 　

　状態空間モデルが可観測であるならば，行列 A−HC の固有値を任意に指定可能な

行列 H が存在する。

　

なお，状態空間モデルが可観測であることを，対 (C,A) が可観測である，という言い方をする

こともある。

　

それでは MCORE-IPの可観測性を確かめてみよう。

Scilab 4.4

--> W=obsv mat(sys);　// 可観測行列の計算

--> rank(W)　// ランクの計算

　 ans =

　　　 4.

可観測行列 W のランクが 4 であることから，MCORE-IPが可観測であることを示すことがで

きる。

　

定理 4.4 と 4.5 から，状態空間モデルが可観測であれば，適切にオブザーバゲイン H を選定す

ることで，式 (4.8) が状態オブザーバになり得ることがわかる。特に，式 (4.8) は，状態量 x すべ

てを推定していることから全状態オブザーバと呼ぶ。全状態オブザーバを利用した制御器は，次

式で与えられる。 (˙x = Ax+Bu+H(y − Cx)u = −Kx

(4.12)

— 31 —

　

ところで，可制御性と可観測性に関して次の定理が成り立つ。

　

定理 4.6 (双対性)

　対 (A,B) が可制御であるとき，対 (BT , AT ) は可観測である。また，対 (C,A) が可

観測であるとき，対 (AT , CT ) は可制御である。

　

　

MCORE-IPに対して，この定理が成り立つことを確かめてみよう。

Scilab 4.5

--> rank(obsv mat(sys.A’,sys.B’))

　 ans =

　　　 4.

--> rank(cont mat(sys.A’,sys.C’))

　 ans =

　　　 4.

　

行列の固有値は転置をとっても変わることはない。したがって，行列 A−HC の固有値と行列

(A −HC)T = AT − CTHT の固有値は等しい。定理 4.6 から，対 (C,A) が可観測であれば，対

(AT , CT ) は可制御である。したがって，状態フィードバック制御で紹介した極配置法もしくは最

適レギュレータ法を利用することで HT (この転置をとればオブザーバゲイン H) を設計すること

が可能となる。

4.4.2 最小次元オブザーバ

制御器 (4.12) で制御対象を制御する場合，微分方程式を解く必要がある。通常，その処理を行

うのが (デジタル)コンピュータである。電子回路を利用して解くことも可能ではあるが，制御器

の変更がプログラムの変更で対応できる，などの利点から，コンピュータを利用する場合が多い。

一般に，微分方程式を解く場合，その本数に対応して，コンピュータが行うべき演算量が増加す

る。演算能力が高いコンピュータを利用できる場合はよいが，演算能力が高くない汎用マイコンを

利用する場合，演算量の低減を考える必要が出てくる場合がある。解くべき微分方程式の本数は，

推定すべき状態量の数に等しい。前述したように，状態量の一部は観測量 y を利用して直接観測

することが可能であることから，直接観測できない状態量のみを推定すればよい，と考えるのは妥

当である。この考え方に基づいて構成されたのが最小次元オブザーバである。

　

今，状態空間モデルが式 (4.13) で与えられるとしよう。なお，どのような状態空間モデルも可

観測性が満たされている場合，式 (4.13) のように変換できる。MCORE-IPに対する状態空間モ

デルは，この構造を有している。

— 32 —

⎧⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎩

"x1

x2

#=

"A11 A12

A21 A22

#"x1

x2

#+

"B1

B2

#u

y =hI 0

i " x1x2

#( = x1)

(4.13)

　

この場合，x1 が直接計測できる状態量で，x2 がそうでない状態量となる。したがって，後者を

推定するオブザーバを構成すればよい。ここでは，詳細については省略するが，状態空間モデル

(4.13) が可制御かつ可観測である場合，x2 を推定する最小次元オブザーバを利用した制御器は次

式で与えられる [3]。⎧⎪⎨⎪⎩w = (A22 −HmA12)w + (B2 −HmB1)u

+(A21 −HmA11 + (A22 −HmA12)Hm)yu = −k2w − (k1 + k2Hm)y

(4.14)

ここで，Hm は最小次元オブザーバのゲインで，行列 A22 −HmA12 が漸近安定となるように選定

する (対 (C,A) が可観測である場合，対 (A22, A12) が可観測となることを示すことができる)。ま

た，状態量 x2 の推定量を x2 としたとき，

w = x2 −Hmx1

である。k1, k2 は状態フィードバックゲイン K を式 (4.13) の状態量 x に対応して分割したゲイ

ンである。つまり，

u = −Kx = −hk1 k2

i " x1x2

#

4.4.3 オブザーバを用いた閉ループシステム

式 (4.1) に対して，全状態オブザーバを利用した制御器 (4.12) を適用したときの閉ループシステ

ムを考える。簡単な計算から，閉ループシステムに対する状態方程式が次式で与えられることを示

すことができる。 "x

e

#=

"A−BK BK

0 A−HC

#"x

e

#(4.15)

これより，閉ループシステムの極は，行列 A − BK と行列 A −HC の固有値の和集合と一致す

ることがわかる (一般に，ブロック三角行列の固有値はブロック行列の固有値の和集合で与えられ

る)。つまり，これらが漸近安定となるように状態フィードバックゲイン K とオブザーバゲイン

H が選定されていれば，理論上は閉ループシステムの安定性が保証されることになる。

ここでは全状態オブザーバについて考えたが，最小次元オブザーバについても同様の結果を得る

ことができる。

— 33 —

4.5 設計例

それでは，MCORE-IPに対して具体的な制御器の設計例を示すとともに，数値シミュレーショ

ンにより，閉ループシステムに対する時間応答の確認を行う。以下の設計において，利用する関数

をまとめたのが ip design.sci である。これをコマンド exec を利用して読み込んでおいてもら

いたい。

Scilab 4.6

--> exec(’ip design.sci’);

// 制御器設計用関数 (ip_design.sci)

// 最適レギュレータ

function K=lqr2(A,B,Q,R)

X=ricc(A,B*inv(R)*B’,Q,’cont’);

K=inv(R)*B’*X;

endfunction

　 Scilabには最適レギュレータを設計するための関数 lqr が用意されているが，重み

Q, R を含めた状態空間モデルを与える必要がある。一方，lqr2 は重みを直接引数とし

て与えることができる。

// 全状態オブザーバ

function sysk=cont(sys,K,H)

[A,B,C,D]=abcd(sys);

ak=A-(B-H*D)*K-H*C; bk=H; ck=-K; dk=D’;

sysk=syslin(’c’,ak,bk,ck,dk);

endfunction

　 Scilabには関数 obscont が用意されているが，本稿とゲインの符号が逆なので，一

致させるために cont を作成した。

// 最小次元オブザーバ

function mobs=min_obs(sys,pp,po)

[A,B,C,D]=abcd(sys); [r,n]=size(C);

A11=A(1:r,1:r); A12=A(1:r,r+1:n);

A21=A(r+1:n,1:r); A22=A(r+1:n,r+1:n);

B1=B(1:r,:); B2=B(r+1:n,:);

C1=C(:,1:r); C2=C(:,r+1:n);

K=ppol(A,B,pp); K1=K(:,1:r); K2=K(:,r+1:n);

H=ppol(A22’,A12’,po)’;

— 34 —

Ak0=A22-H*A12;

Bk1=B2-H*B1; Bk2=A21-H*A11+(A22-H*A12)*H;

Ck1=-K2; Ck2=-(K1+K2*H);

Ak=Ak0+Bk1*Ck1; Bk=Bk1*Ck2+Bk2;

Ck=Ck1; Dk=Ck2;

mobs=syslin(’c’,Ak,Bk,Ck,Dk);

endfunction

　 min obs の第 1引数は状態空間モデルであり，極配置法を利用して制御器の設計を行

う。第 2引数が状態フィードバックゲイン，第 3引数が最小次元オブザーバゲインを設計

するための極である。

4.5.1 極配置法

極配置法では，配置すべきレギュレータ極が設計パラメータとなる。ここでは，開ループ極の位

置 −13.45, −6.543, 7.001, 0 を参考にして，レギュレータ極を次のように定めた。

h−20 −5± 3j −3

i(4.16)

これに対して極配置法により状態フィードバックゲインを設計し，初期値応答を求めた結果を図

4.1 に示す。上図が本体部回転角 θ+ φ であり，下図が相対回転角 θ である。もし，応答が望まし

いものでなければ，極を変更して再設計ならびに応答の評価を繰り返す。

Scilab 4.7 (ip pole.sce)


--> [A,B,C,D]=abcd(sys);

--> P=[-20,-5+3*%i,-5-3*%i,-3];　// レギュレータ極

--> K=ppol(A,B,P)　// 極配置法に基づく状態フィードバックゲインの設計

　 ans =

　　　 19.769866　-0.2843574　 2.9006145　-0.2170713

--> syscl=syslin(’c’,A-B*K,B,C);　// 閉ループシステム

--> t=0:0.01:3;　// シミュレーション時間

--> x0=[0;0.1;0;0];　// 初期状態

--> ycl pl=csim(zeros(t),t,syscl,x0);　// 初期値応答

--> subplot(2,1,1), plot(t,ycl pl(1,:)), xgrid()　// 本体部回転角

--> xtitle(’Initial response’,’time [s]’,’Body angle [rad]’)

--> subplot(2,1,2), plot(t,ycl pl(2,:)), xgrid()　// 相対回転角

--> xtitle(’’,’time [s]’,’Wheel angle [rad]’)

— 35 —

図 4.1: 初期値応答 (極配置法)

4.5.2 最適レギュレータ法

評価関数 (4.5) 中の重みを次のように選んだとしよう。

Q =

⎡⎢⎢⎢⎢⎣5 0 0 0

0 0.5 0 0

0 0 0 0

0 0 0 0

⎤⎥⎥⎥⎥⎦ , R = 1 (4.17)

Scilab 4.8 (ip lqr.sce)



--> Q=diag([5,0.5,0,0]); R=1;　// 重み行列

--> K=lqr2(A,B,Q,R)　// 最適レギュレータ法による設計

　 ans =

　　　 28.330112　-0.7071068　 3.9752077　-0.3377152

--> spec(A-B*K)　// レギュレータ極

　 ans =

　　　-10.88539 + 3.2048384i

　　　-10.88539 - 3.2048384i

　　　-6.1272924 + 1.359948i

　　　-6.1272924 - 1.359948i

　

前項では初期値応答を示したが，ここでは操作量に単位ステップを加えたときの単位ステップ応

答を調べる。また，操作量も表示する。結果を図 4.2 に示す。上図が観測量 y で下図が操作量 u

— 36 —

である。極配置法と同様に，希望する応答が得られるまで重み Q, R の選定を繰り返す。

Scilab 4.9 (ip lqr.sce)

--> syscl=syslin(’c’,A-B*K,B,[C;-K]);　// 閉ループシステム (観測行列に注目)

--> ycl lq=csim(ones(t),t,syscl);　// 単位ステップ応答

--> subplot(2,1,1), plot(t,ycl lq(1:2,:)), xgrid()　// 応答の表示

--> xtitle(’Step response’,’time [s]’,’Resp.’)

--> subplot(2,1,2), plot(t,ycl lq(3,:)), xgrid()　// 操作量の表示

--> xtitle(’’,’time [s]’,’Input’)

図 4.2: 単位ステップ応答 (最適レギュレータ法)

4.5.3 全状態オブザーバ

全状態オブザーバの基本的な役割が，状態量の推定にあることを考えたとき，A− BK の固有

値に対して A−HC の固有値をより安定側に配置することが好ましい。そこで，

h−40 −20 −22 −24

i(4.18)

として極配置法を用いてオブザーバゲイン H を設計する。これに対する初期値応答を図 4.3 に示

す。なお，状態フィードバックゲイン K は極配置法 (4.5.1項) で得られたものを利用している。

比較の意味で，状態フィードバック制御の結果を点線で示している。

— 37 —

Scilab 4.10 (ip fobs.sce)



--> P=[-20,-5+3*%i,-5-3*%i,-3];　// レギュレータ極

--> K=ppol(A,B,P);　// 状態フィードバックゲイン

--> Po=[-40,-20,-22,-24];　// オブザーバ極

--> H=ppol(A’,C’,Po)’;　// オブザーバゲイン

--> sysk=syslin(’c’,A-B*K-H*C,H,-K);　// フィードバック制御器

--> syscl=sys/.(-sysk);　// 閉ループシステム

--> spec(syscl.A)　// 閉ループシステムの極

--> t=0:0.01:3;　// シミュレーション時間

--> x0=[0;0.1;0;0;0;0;0;0];　// 初期状態 (制御器の初期状態は 0)

--> ycl ob=csim(zeros(t),t,syscl,x0);　// 初期値応答

--> subplot(2,1,1), plot(t,[ycl ob(1,:);ycl pl(1,:)]), xgrid()　//本体部回転角

--> xtitle(’Initial response’,’time [s]’,’Body angle [rad]’)

--> subplot(2,1,2), plot(t,[ycl ob(2,:);ycl pl(2,:)]), xgrid()　// 相対回転角

--> xtitle(’’,’time [s]’,’Wheel angle [rad]’)

図 4.3: 初期値応答 (全状態オブザーバ)

4.5.4 最小次元オブザーバ

最後に，最小次元オブザーバの設計例を示す。なお，状態フィードバックゲインならびにオブ

ザーバゲインはいずれも極配置法を利用して設計するものとする。ここでは，制御器の設計例のみ

を示すので，閉ループシステムの応答に関しては各自で調べてもらいたい。

Scilab 4.11

--> sysk=min obs(sys,[-20,-5+3*%i,-5-3*%i,-3],[-15,-20]);　//最小次元オブザーバ

— 38 —

5 制御器のマイコンへの実装

5.1 タイマー割り込み

前章で設計した制御器をマイコンに実装する場合，マイコンが行うべき仕事は，一定の時間間隔

で制御対象に取り付けたセンサからの情報を受け取り，必要な操作量を計算後，それを制御対象に

与える，ことである。この一定の時間間隔 (サンプリングタイム) を実現する方法として，マイコ

ンのもつタイマー割り込み機能を利用する。本章では，制御器の演算時間に加えて XBEEによる

データ送信用の時間を考慮して，サンプリングタイムを 3 [msec] とする。

5.2 状態フィードバック制御

MCORE-IPに搭載しているセンサから，本体部回転角 θ+φ と相対回転角 θ は入手できるが，

それらの角速度情報を直接入手することはできない。そこで，角速度に関しては角度を差分するこ

とで数値的に得ることにする。なお，ロータリーエンコーダの分解能が低いために，平衡点近傍の

微小な動きに対しては，差分操作によって大きな誤差が生じる可能性がある。そこで，その影響を

低減する目的で，操作量にローパスフィルタを入れる。プログラム cont state (リストを 5.4節

で示す) では，関数 u state() 内で F (s) = 30/(s+ 30) のローパスフィルタをサンプリングタイ

ム 3 [msec] で離散化して組み込んでいる。

　

制御実験 1 　 (プログラム cont state を TK400に書き込む)

実験 5.1 (制御実験 1)

1. プログラムを実行すると，LCD にポテンショメータの出力電圧値とロータリー

エンコーダのパルス値が表示される。

2. 本体部を手で支えた状態で SW1 を押すと制御を開始する。

3. ゆっくりと本体部から手を離すとフィードバック制御により安定化が行われてい

ることを確認できる。

— 39 —

1. TK400のロータリーディップスイッチを 0 としてプログラムを実行すると，安

定化制御モードとなり，それ以外にして実行すると，ステップ応答モードとなる。

後者では，6 [sec] 間隔で振幅 1 と 0 を繰り返す入力が操作量に加えられる。

2. PCにXBEEが取り付けられ，通信用のソフトが実行されている場合，MCORE-

IPから送信されたポテンショメータの電圧値 (ただし，電圧に変換する前の整数

値) とロータリーエンコーダのパルス値を受信できる。前者に対しては 5/1024，

後者に対しては π/180 を掛け算することで，それぞれの角度情報に変換できる。

3. 状態フィードバックゲインを設計し直した場合，プログラム cont state 中の関

数 state feedback() 内のゲイン値を変更し，再度 TK400に書き込みなおし

てもらいたい。

　

ここで，ローパスフィルタ F (s) の効果を XCOSを利用して確認しておこう。シミュレーショ

ン用のブロック線図 state fd.xcos を図 5.1 に示す。なお，シミュレーションを開始する前に，

MCORE-IPの状態空間モデル A, B, C, D ならびに状態フィードバックゲイン K を Scilabの

ワークスペース内に用意しておく必要がある。

　

図 5.1: state fd.xcos

— 40 —

　 MCORE-IP と名前が付けられたブロックが MCORE-IPの状態空間モデルを表して

おり，MCORE-IPに搭載されているロータリーエンコーダの分解能を量子化ブロッ

クによって表している。2つのセンサの出力とその差分により得られた状態量を状態

フィードバックゲインに与えることで操作量を作成している。その操作量はローパス

フィルタ LPF を通して MCORE-IP に与えられる。時間応答の評価は，単位ステップ入

力を与えることで行う。

　

ローパスフィルタを F1(s) = 30/(s+30), F2(s) = 300/(s+300) としたときの各操作量を図 5.2

に示す。前者が上図で，制御実験で用いたローパスフィルタである。また，後者が下図で，ローパ

スフィルタの効果を低くした場合に対応していると考えてもらいたい。これらを比較することで，

ローパスフィルタが差分演算によって生じたノイズを低減する効果を理解できる。制御実験におい

ては，ローパスフィルタを入れない (プログラム cont state 中の関数 u state() における u =

xfk; をコメントアウトする) 場合と比較することで，MCORE-IPの動きや駆動部から生じる

音により差分演算の影響を確認できる。

図 5.2: ローパスフィルタ F (s) の効果

5.3 全状態オブザーバを利用した制御器

全状態オブザーバを利用した制御器は式 (4.12) で与えられるが，これを次式のように書き換

える。 (˙x = (A−BK −HC)x+Hyu = −Kx

(5.1)

これをマイコンに組み込む場合，離散化により差分方程式で近似する必要がある。(z[k + 1] = Ad · z[k] +Bd · y[k]u[k] = Cd · z[k] +Dd · y[k]

(5.2)

— 41 —

離散化にはいくつかの方法があるが，ここでは零次ホールド法を利用する。

Scilab 5.1 (ip discrete.sce)



--> P=[-20,-5+3*%i,-5-3*%i,-3];　// レギュレータ極

--> K=ppol(A,B,P);　// 状態フィードバックゲイン

--> Po=[-40,-20,-22,-24];　// オブザーバ極

--> H=ppol(A’,C’,Po)’;　// オブザーバゲイン

--> sysk=syslin(’c’,A-B*K-H*C,H,-K);　// フィードバック制御器

--> dsysk=dscr(sysk,0.003);　// 零次ホールド法による離散化

--> [Ad,Bd,Cd,Dd]=abcd(dsysk);

　

制御実験 2 　 (プログラム cont fobs を TK400に書き込む)

実験 5.2 (制御実験 2)

1. プログラムを実行すると，LCD にポテンショメータの出力電圧値とロータリー

エンコーダのパルス値が表示される。

2. 本体部を手で支えた状態で SW1 を押すと制御を開始する。

3. ゆっくりと本体部から手を離すとフィードバック制御により安定化が行われてい

ることを確認できる。

　

TK400のロータリーディップスイッチを 0 以外 (たとえば 1) にセットして行った実験結果

を図示したのが，図 5.3 と 5.4 である。前者が本体部回転角 θ + φ，後者が相対回転角 θ であ

る。また，図中，点線が実験データ，実線がシミュレーション結果を表している。

両者が類似した結果であることが確認できるが，定常状態において，実験結果にリミットサイク

ルが生じていることがわかる。これは駆動系のもつ非線形特性により生じたと考えられる。そこ

で，その非線形特性が近似的に不感帯で表すことができると仮定して，XCOSを利用してシミュ

レーションしてみる。その目的で作成したブロック線図 output fd.xcos を図 5.5 に示す。シミュ

レーションでは，離散化した制御器を利用しており，MCORE-IPの状態空間モデル A, B, C,

D ならびに離散化した制御器 Ad, Bd, Cd, Dd を Scilabのワークスペース内に用意しておく必

要がある。

　

不感帯の大きさを ±0.1 としたときのシミュレーション結果 (実線) と実験結果 (点線) を図

5.6, 5.7 に示す。不感帯で実験結果をある程度説明できることがわかる。なお，8 [sec] 以降で

実験結果が一定値になっているのは，シミュレーションでは考慮していない静摩擦によるものと推

測する。

— 42 —

図 5.3: 実験結果 (本体部回転角 θ + φ)

図 5.4: 実験結果 (相対回転角 θ)

— 43 —

図 5.5: 不感帯の影響

図 5.6: 不感帯を考慮したときの本体部回転角 θ + φ

— 44 —

図 5.7: 不感帯を考慮したときの相対回転角 θ

5.4 プログラム例

TK400に書き込むプログラムの一例として，状態フィードバック制御用プログラム cont state.c

のリストを以下に示す。

//---------------------------------------------------------

// MCORE-IP 状態フィードバック制御を利用した安定化

// File Name: cont_state.c

// 2011.11.18 R.Kawatani

//---------------------------------------------------------

#include <7125S.h>

#include <stdio.h>

#include <sysio.h>

#include <lcd7125.h>

#include <tk400sh_lib.h>

#include <sci_lib.h>

#define LEDon (1)

#define LEDoff (0)

#define SWon (0)

#define SWoff (1)

#define CR (0x0d)

#define LF (0x0a)

//----------------------------------------------------------

// 測定条件・グローバル変数の定義

//----------------------------------------------------------

#define V2ANGLE (0.6121f) // ポテンショメータゲイン [rad/V]

#define BIT2V (4.8828e-03) // A/D分解能 (5.0V)/1024

#define V2DT (13.89f) // 100/7.2 (モータ電源電圧を 100%とするため)

#define P2RAD (17.45e-03) // 駆動輪の回転角度 [rad/pulse]

#define STIME (0.003f) // サンプリング時間

float x1, x2, x3, x4; // 状態量

— 45 —

float x1old, x2old; // 差分用

float k1, k2, k3, k4; // 状態フィードバックゲイン

float xk, xfk; // フィルタ用

float wh_z1, wh_z2, wh_z0; // 駆動輪回転角

float vad_s, vad_0; // 本体傾斜角 (θ+φ)(電圧値)

float u; // 出力 (操作量)

float r; // 目標入力

short cflag; // 制御開始フラグ

short cnt, rflag, icnt; // 目標入力発生用

short dsw; // ディップスイッチ

short ecnt1, ecnt2, adpot, ad_0, ad_s; // センサ用

//----------------------------------------------------------

// プロトタイプ宣言

//----------------------------------------------------------

void var_init(void); // 変数の初期化

void read_sensor(void); // センサの読み込み

void inti_cmt0(void); // 制御器演算用，cmt0の初期設定

void ctrl_start(void); // 制御器の動作開始 (cmt0のカウンタスタート)

void state_feedback(void); // 状態フィードバックゲインの設定

void u_state(void); // 操作量の計算 (状態フィードバック制御)

void send_cnt(short data); // 4桁の整数値を文字に変換し，送信

void send_cnt2(short data1, short data2); //

//----------------------------------------------------------

// 各種関数の定義

//----------------------------------------------------------

void var_init(void) // 変数の初期化

ad_start(0); // AD0開始

x1old = x2old = 0.0f; // 差分用

xk = xfk = 0.0f; // フィルタ

ad_0 = get_ad(0); // AN0 の読み込み

vad_0 = (float)(ad_0)*BIT2V; // 本体傾斜角 (θ+φ)初期値

wh_z0 = 0.0f; // 駆動輪初期値 (平衡状態)

r = 0.0f; // 目標入力を 0

cnt = 0; rflag = 0; // 目標値用

void read_sensor(void) // センサの読み込み

ad_start(0); // AD 変換開始

ecnt1 = get_enc(1); // 右エンコーダの値取得

ecnt2 = get_enc(2); // 左エンコーダの値取得

wh_z1 = (float)(ecnt1)*P2RAD; // 右駆動輪回転角 (rad)

wh_z2 = (float)(ecnt2)*P2RAD; // 左駆動輪回転角 (rad)

ad_s = get_ad(0); // AN0 の読み込み

vad_s = (float)(ad_s)*BIT2V; // 振子傾斜角 (現在値)の取得 (Volt)

void state_feedback(void) // 状態フィードバックゲインの設定

// P=[-20,-5+3*%i,-5-3*%i,-3]

k1=19.77f; k2=-0.2844f; k3=2.901f; k4=-0.2171f;

void u_state(void) // 操作量の計算 (状態フィードバック制御)

x1 = (vad_s-vad_0)*V2ANGLE; // 状態量 x1(θ+φ)[rad]

x2 = (wh_z1-wh_z0); // 状態量 x2(θ) [rad]

x3 = (x1-x1old)/STIME; // 状態量 x3(θdot+φdot)

x4 = (x2-x2old)/STIME; // 状態量 x4(θdot)

— 46 —

x1old = x1; x2old = x2; // 状態量の更新

u = r - (k1*x1 + k2*x2 + k3*x3 + k4*x4); // 操作量の算出 ( u = -k x )

xfk = 0.9139f*xk + 0.08607f*u; // 30/(s+30) 3msec で離散化

u = xfk; // LPF出力

xk = xfk; // 状態量の更新

void inti_cmt0(void) // 制御器演算用，CMT0の初期設定

STB.CR4.BIT._CMT = 0; // CMTのスタンバイモードの解除

CMT0.CMCSR.WORD = 0x0040; // CMT0 割込許可，φ P/8(3.125MHz)

INTC.IPRJ.BIT._CMT0 = 14; // CMT0の割込優先順位 14(最も高い)

CMT0.CMCOR = 9374; // 3ms毎の割込処理

// CMT0.CMCOR = 3124; // 1ms毎の割込処理

// CMT0.CMCOR = 1562; // 0.5ms毎の割込処理

CMT0.CMCNT = 0; // CMT0 のゼロクリア

void ctrl_start(void) // 制御器の動作開始 (CMT0 のカウンタスタート)

CMT.CMSTR.BIT.STR0 = 1; // CMT0のスタート

void send_cnt(short data) // 4桁の整数値を文字に変換し，送信

char d1,d2,d3,d4;

if(data<0) sendbyte(DSUB,’-’); data = -data;

d1 = data%10; data /= 10;

d2 = data%10; data /= 10;

d3 = data%10; d4 = data/10;

sendbyte(DSUB,d4+’0’); sendbyte(DSUB,d3+’0’);

sendbyte(DSUB,d2+’0’); sendbyte(DSUB,d1+’0’);

void send_cnt2(short data1, short data2)

send_cnt(data1); sendbyte(DSUB,’ ’);

send_cnt(data2); sendbyte(DSUB,CR); sendbyte(DSUB,LF);

//---------------------------------------------------

// CMT0 による 3ms単位の割込　制御器演算処理

//---------------------------------------------------

void interrupt __vectno__ 184 int_cmt0_control(void)

unsigned short duty;

CMT0.CMCSR.WORD &= 0x0043;

// CMF(コンペアマッチフラグ,bit7)のクリア

// リードモディファイライト

LED1 = LEDon; // モニタ LEDの点灯 (割込処理時間計測用)

if(cflag == 1) // 制御フラグがセットされていれば制御器の演算

duty = (unsigned short)(u*V2DT); // 操作量の 0～7.2Vを 0～100%へ

set_pwm_duty(duty,duty); // デューティ値を設定

read_sensor(); // センサ情報の取得

u_state(); // 操作量の計算

icnt++;

if(icnt>=10)

— 47 —

send_cnt2(ecnt1,ad_s-ad_0); icnt = 0; // 計測データの送信

// ステップ目標入力用 (ディップスイッチが 0以外のとき)

if(dsw != 0)

cnt++;

if(cnt>=2000)

cnt = 0;

if(rflag==0) rflag = 1; r = 1.0f; else rflag = 0; r = 0.0f;

// モータの回転方向の設定

if(u < 0)

set_motor_dir(F,F);

u = -u; // 絶対値処理

else set_motor_dir(R,R);

if(u > 7.0f) u = 0.0f; cflag = 0; LED2 = LEDon;

else

LED2 = LEDoff; // 操作量の制限 ± 2.5V,オーバーフローランプ点灯

else // 制御フラグがクリアされている場合

set_pwm_duty(0,0);

LED1 = LEDoff; // モニタ LEDの消灯 (割込処理時間計測用)

//---------------------------------------------------

// メイン関数

//---------------------------------------------------

void main(void)

port_init(); // ポートの初期化

inti_cmt1(); // 時間待用，CMT1 の初期設定

inti_cmt0(); // 制御器演算用，CMT0 の初期設定

initlcd(); // LCD初期化

ad_init(0); // AD0初期化

set_ad_ch(0); // AD0チャネルの設定

setup_sci(DSUB,BR19200); // SCI1モジュールの初期化

LED1 = LED2 = LEDoff; // LED オフ

set_pwm_freq(10000); // PWM周波数は 10kHz

set_motor_dir(S,S); // モータ停止

set_pwm_duty(0,0); // デューティ値は 0%

state_feedback(); // 状態フィードバックゲインの係数設定

initlcd();

locate(0,0); outst("Start ");

locate(0,1); outst("SW1: START / END");

delay_ms(2000); clr_lcd();

var_init(); // 制御変数の初期化

enc_start(); // エンコーダカウンタの動作開始

while(SW1==SWoff) // ポテンショとエンコーダのモニタ表示

read_sensor();

— 48 —

locate(0,0); outst("s="); outf(vad_s); // ポテンショ

locate(0,1); outi(ecnt1); outst(" "); // エンコーダ

;

while(SW1==SWon) clr_lcd(); ; // SW1から手が離れるまで待機

dsw = get_dip_sw(); //ディップスイッチの読み込み

motor_amp_on(); // モータドライバの Ready

locate(0,0); outst("!! Controlled !!");

var_init(); // 制御変数の初期化

cflag = 1; // 制御フラグのセット

ctrl_start(); // 制御器の動作開始

WriteSR(1<<4); // 時間待ち関数の割込を禁止し，制御器用タイマ割込の許可

icnt = 0;

while(1)

if(SW2 == SWon) cflag = 0; // SW2が押されたら制御停止

if(cflag==0) // 操作量オーバフローのため制御停止

WriteSR(15<<4); // 全ての割込禁止

motor_amp_off(); // モータフリー

clr_lcd(); outst("stop");

while(1);

— 49 —

6 おわりに

本稿では，MCORE-IPに対して，状態空間モデルの導出から制御器の実装に至るまでの手続

きについて，その概要をまとめた。紹介した線形制御理論は，制御の教科書では必ず取り上げられ

るという基本的な内容にとどめた。その学習用として MCORE-IPをご活用いただければ幸いで

ある。

　

MCORE-IPの本体部はユニバーサルプレートを利用している。そのために，部品の配置に関

しては自由度がある。この自由度を利用することで，他の形態の制御対象を作り出すことができ

る。具体例をいくつか示そう。ぜひ挑戦してもらいたい。

(1) MCORE-IPでは本体部を直立させてバランスをとることを目的としたが，キャスター等を

利用して本体部を横置きとし，ポテンショメータに棒を取り付け，それのバランスをとるこ

とを考えると，通常の (台車型の)倒立振子系となる。

(2) また，駆動輪と同軸にロータリーエンコーダを取り付けてあるので，移動ロボットの位置制

御実験にも活用が可能である (XBEEを利用すれば無線操縦も可能)。

(3) さらに，2個の駆動輪を進行方向に並べることで自転車や一輪車を模擬した制御対象も製作

できる。バランスをとるためのトルク発生機構を工夫する必要があるが，興味深い制御対象

である。

(4) TK400は 8つの入力ポートをもつが，MCORE-IPではポテンショメータの 1ポートしか

利用していない。測距センサなどの物体の位置を検出できるセンサを配置することで，障害

物回避問題に取り組むことも可能である。

(5) TK400は 2チャンネルのラジコンサーボ用ドライバを持っており，簡易的なアームを組み

込むことができる。さらに，シリアルポートに市販のラジコンサーボ用のコントローラを接

続すれば複数のラジコンサーボを組み込むことも可能である。

　

参考文献

[1] 伊藤，金田，横山: 車輪型倒立振子を用いたメカトロニクス教材の開発 -現代制御理論の基

礎学習教材-

[2] 株式会社ゼットエムピー：倒立二輪ロボット (モータ 1軸)の安定化制御と走行制御-倒立二

輪ロボット e-nuvo WHELL ver.1.0 の開発-

[3] 川谷：フリーソフトで学ぶ線形制御，コロナ社 (2008)

[4] 川谷：「Maxima」と「Scilab」で学ぶ古典制御，工学社 (2011)

[5] イエローソフトプログラマーズ・マニュアル

[6] 川谷：XBEEの使い方

— 50 —

　

MCORE-IPの製作にあたっては，株式会社MCOR が共同研究にて参画し，

ならびにその販売をしております。

MCORE-IP説明書

2011年 11月 23日　第 2版

2012年 8月 9日　第 2.1版

福井大学大学院工学研究科　川谷　亮治

長野県飯田工業高校　高田　直人

　

910-8507 福井県福井市文京 3-9-1

Email : r [email protected]

— 51 —

更新履歴

2012.8.9

(1) 表 3.4 の Jt の値。2.27→ 1.135

(2) 式 (3.24) の下の式。J0 = Jt +mtr2t + JmN

2 → J0 = Jt + JmN2

(3) 同定パラメータ値

(4) (3) にあわせて，数値状態空間モデルや制御器

— 52 —

1.1 mcore-ip -...

Documents