エネルギー法を利用した回転型倒立振子の振り上げ制御木澤

(1)

エネルギー法を利用した回転型倒立振子の振り上げ制御

木澤悟・奈良森紹＊

Swing‑UpControloflnvertedPendulumUsingEnergyMethod

SatoruKIzAwAandMoritsuguNARA"

(2006年11月30日受理）

Thestudyofthemotioncontrolofunderactuatedmechanicalsystemsisnowreceivinga

greatdealofattention. UnderactuatedmechanicalsystemscalledtheAcrobotaremechanical

systemswithfeweractuatorsthandegree‑of‑freedom. Becauseofit, thesesystemshave strongnonlinearityanditisverydifficulttocontroltheirmotion. Thispaperpresentsthe controlofanunderactuatedsystemscalledtheRotationallnvertedPendulumwitharmand link. Weproposethecontrolalgorithmusedtoswing‑upandbalancethelinkatunstable equilibriumposition. Aswing‑upcontrollerisdesignedbasedonmechanicalenergyanda stabilizationcontrol isdesignedbasedonLQGcontrol theory. Theperformanceofthe proposedcontrol lawisshowninasimulationexample.

近させ，次の第2ステップ．においては制御則を切り

換えてLQG制御を用いて，振子の安定化制御を

行った。なお，本研究は実機をベースにモデル化しているが，制御手法を検証するためにMatlab/

Simulinkを用いてシミュレーションを行い，提案した制御手法の有効性について示す。

1．緒

一一一一目

劣駆動システム3)'4)は一般化座標の数よりも少ない数のアクチュエータで制御入力以上の数の一般化座標を制御するシステムである。劣駆動システムの例としては第一関節に駆動関節を持たないアクロボッ

トやトレーラーおよび宇宙ロボットなどが挙げられる。劣駆動システムはアクチュエータの数が関節の数より少ないことにより，ロボットの軽量化コストダウンがはかられ，システムの簡素化のメリットが挙げられる。しかしながらその反面，パッシブな関節を有しているため動作空間において，非線形性があり，モデルベースに基づく現代制御理論が直接利用できない，あるいは直接駆動できない関節に対する制御の難しさがあり，数多くの課題を有する難

しい制御問題である。

本研究では，劣駆動システムの一例であるアームと振子から構成される回転型倒立振子を製作し，この振子が真下状態にぶら下がっている状態から，振子を振り上げることにより振子を真上状態に倒立させる安定化制御を目的とした。制御手法は2つのステップにわけ，第1ステップは力学的エネルギに着目した振り上げ制御により安定化領域まで振子を漸

2．システムの概要

図1に製作した回転型倒立振子システムを示す。

また，図2に模式的なシステム構成図を示す。アー

秋田高専専攻科学生

図1 回転型倒立振子システム

(2)

ムに取り付けられたロータリーエンコーダと，振子のDCギヤードモータに取り付けられたロータリー

エンコーダの角度信号はカンサー社製のMultiQ‑

PCI (カウンタ）を介してパソコンへと送られる。

また，指令信号は制御則に基づいてMultiQ‑PCI

(D/Aコンバータ）からモータアンプを介してDC モータへと送られる。

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

Mu耐Q詩PCI ロータリー

エンコーダカウンタＤ／Ａ

である。さらにモータの動特性を考盧した非線形運動方程式である式(1)において，

ロータリー

エンコーダ

α・ａ

ＯＳ１ＪａＣ″だし８●了〃し人脈伽・８α

恥１Ｊ加〃地引皿恥州唖瀝

附恥

十α２．１︐〆郎７や・恥俳一一柵伽勝恥邸兆恥恥恥一

一一一一一一ＩＩＯ

吻岬恥肺肺肺

ブ聡琴ゞ

ンが心万一︺源

釣泌電

モ然鍾一

︽Ｆ

﹄一

︾

︷

・か

︾

︲吋徳誌毛

︽

︑ 叩

︺凹牽いい

？

﹃咽

︒

図2 システム構成図

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

c=G+zfi

^〜

鵬一凡

一一

だ

(3) 3.1 非線形運動方程式の導出 (2)

この節では，ラグランジュの運動方程式を用いて図3に示す回転型倒立振子モデルの運動方程式を導出する。モータ動特性を考盧してアームに与えられるトルクrとモータ端子電圧yとの関係を含めて考盧した非線形な運動方程式は次式となる。

(4)

〃ノーXy−Ce と置けば

窒董I目+I:￨+眺一閏

￨鰯

となる。

ある。

(5)

EZ脇紬2cM‑t/,""2撫郡｜ ^{〃〃･んCOSα} +L織協｡‑"""圓

ここで，〃'は制御入力，すなわち操作量で

3.2運動方程式の線形化

線形制御理論をコントローラに用いる場合，モデルの線形化が必要となる。そこで，式(1)において，

振子が真上にほぼ倒立し，アームの左右の振れが小さいとするならば，アーム角度e,振子角度αは小さく見積もることができ，次の様な仮定が置ける。

つまり，

s加8=e cose=I 92=0

s加α室α COSα二J α2二0

であるから，非線形運動方程式である式(1)は次式のように線形化できる。

Ic+零㎡

＋

0

^G

^O

￨g+L患い加J掴〃 ⁽¹⁾

α：振子角度 8 :アーム角度

恥：振子の質量

"α ：アームの質量

ら：振子の重心までの長さ ′：アーム長さ

路：アームの重心までの長さ

lb :アーム重心まわりの慣性モーメント

4：振子重心まわりの慣性モーメント

ノル :Ihと取り付け部品を含めた慣性モーメント

て：モータトルク

〃：ギヤ比

Ro ：直流抵抗（アマチュア抵抗）

Kb:モータ誘起電圧脇：トルク定数 F+鰯"蝋I:￨

(3)

表1 振子の物理パラメータ

l"零〃

＋

0

j同+ 州

間

^'

⁽⁶⁾

ﾆニニ

3.3状態方程式の導出

モータの動特性を考盧した線形な運動方程式である式(6)から，状態方程式および出力方程式を求め

ると

{童二哉十恥 ⁽⁷⁾

表2アームの物理パラメータ

となる。ここで

4仏E割 1−凶

E‑F+鰯" 蝋

卜筈』〃闇

4ﾉー表3 DCモータのパラメータ

ﾙｰ￨ l o−l州:

xp=[eq94]7 ｡ﾉも=(Ib+"｡危）

〃＝〃

である。 5．制御系の設計について

4．パラメータの測定 5.1 制御領域の分割

制御目的は図4に示すように初期状態において振子が真下にぶら下がった状態から，振り上げを行い振子角度が0．になる真上で倒立した状態で安定す

ることが目標である。そこで，振子が真下にぶら下

物理パラメータ脇刈, Ihは測定あるいは計算に

よって求められるが, 4,Q,Qは振子の自由振動

実験とアーム動作実験によって推定した。表1には，

振子の自由振動実験より得られた振子のパラメータを示す。また，表2には，アーム動作実験より得られたアーム摩擦係数CM,理論的に導出したアームの慣性モーメントlbを示す。本研究に用いたDCモータは千葉精密製のギヤエンコーダ付きモータである。

表3にDCモータの性能を示す。

今

初期状態最終目標値

図4振子の制御目標

ロ弓己

一三口

物理パラメータ数値

E 減衰率 0.0925

7

周期 1.0067[s]

の〃固有振動数 6.241[Hz]

と減衰比 0.0051

4 振子の重心まわりの

完成モーメント

0.971×10‑3[kg･m2]

q 振子粘性摩擦係数

1.671×10‑4[kg･m2]

ﾉ"〃振子質量

0.067[kg]

↓ 振子の重心までの距離 0.155[m]

ロ言己

一言口

物理パラメータ数値

"a アームのみの質量

0.057[kg]

脇アームの重心までの長さ 0.137[m]

L アーム慣性モーメント

2.67×10‑4[kg｡m2]

C, アーム摩擦係数

2.919×10‑3[Nms/rad]

Jも

回転軸まわりの

アーム慣性モーメント

1.2×10‑3[kg･m2]

千葉精密製LEC‑326402G200

記号物理パラメータ

数値

R

a

直流抵抗(アマチュア抵抗）

5.7[Q]

K『トルク定数

3.038×10 2[Nm/A]

Kb 誘起電圧定数

3.06×10‑2[V･rad/s]

〃ギヤ比 29.47

ﾊモータの慣朧モーメント

2.19×10‑6[kg･m2]

(4)

がった状態から真上に倒立するまでの制御手法と制御則について述べる。

本研究では振子の可動領域を，振り上げ領域と安定化領域の2つに分け，振り上げ領域ではエネルギ

法に基づく振り上げ制御を，安定化領域ではLQG

コントローラを用いた安定化制御を行うことにした。

そこで，図5に示すように振り上げ制御をSTEP1 とし，安定化制御をSTEP2とした。つまり，モデルが線形できない領域では非線形な制御を行い，モ

デルが線形化可能な領域に突入した場合にLQG制

御に切り換える2ステップのコントローラを構築する。 1章の冒頭でも述べたが，現代制御理論は線形化されたモデルに対して適応できるが，本研究の制御対象である回転型倒立振子は非線形なシステムであるため，モデルベースに基づく制御理論を適応できない。そこで，振り上げ領域ではモデルが非線形システムなので，エネルギ法を基にした制御手法で行い，安定化領域においてはモデルを線形することが可能となるので, LQG制御理論を利用することにする。したがって本研究における制御則は振り上げ領域と安定化領域を振子角度αで定義し，それぞれ以下の範囲とする

STEP1振り上げ制御： α＜ ^刀， α＞工のとき

6 6

運動エネルギ法に基づく制御

STEP2安定化制御：一旦<cM<Zのとき

6− −6

LQG制御

ステム全体の力学的エネルギE(運動エネルギェ位置エネルギU) とその時間微分Eを示す。

E(9,q,9,d)=T(8,q,9,a)+U(q) (8)

E(9,q,9,")=T(9,",9,a)+U(d) (9) なお，システムの運動エネルギおよび位置エネルギはラグランジュの運動方程式を用いて導出される。

以下に運動エネルギ7および位置エネルギUを示す。

運動エネルギはT

『{， "=ﾅ[' ]￨淵鯏:1:｜

＝;{""''千""'α+"釧過…"，α』｝（'0）

となり，位置エネルギUは

U(d)=加順g"coscM

⁽¹¹⁾ となる。この制御は現在の持つ力学的エネルギよりも次のステップ°の持つ力学的エネルギが増大するこ

とで，位置エネルギあるいは運動エネルギを増大さ

せ，振子を真上に漸近させるねらいである。この目的を達成させるためには常にエネルギを増大し続け

る必要があり，以下の条件を満たす必要がある。

E(8,q,9,")>0 (12)

つまり式(12)は，エネルギ関数である式(8)が単調増加関数であることを示している。次に全体の力学的エネルギの微分は式(9)に示しているが，具体的に表すと，

安定化領域

釘…"一際罰M儂調M

≠院剖M

‑M『慨麗1W":11‑鰹驍劉

≠[' ｡jW噛鯛;"IM"""'M

1

偽

基準

、、振り上げ領域

振子一

＝"'e−Ga2 (13）

図5振子の制御領域

となる。このときE>0であればのE(8,",8,4)傾きが常に正，つまりエネルギを単調増加させることが可能である。つまり式(13)の右辺第2項は振子の摩擦に関する項で制御人力〃は第2項を常に上回るような操作が必要であることがわかる。エネルギ 5.2 STEP1 エネルギ法に基づく振り上げ制御

振り上げ領域の制御則は，回転型倒立振子のシス

テム全体の力学的エネルギが常に増大するような制

御入力を与え続けるような手法を考える。まず，シ

(5)

を増加させる為の制御入力〃jは左＞0であればよい

から

LQGコントローラを用いる｡LQ理論は，次式に示す評価関数を最小化する制御入力を求めるものである。

J=Jr(x'gx+"")c"

=:(y'c'Qq,+"'R")c"

^（20）

ここで, Qは準正定対称行列, Rは正定対称行列で

ある。式(20)における評価Jの値を小さくするようなコントローラを設計する。式(20)を最小化する制御入力は

〃＝−鰍

（21）

である。Kはフィードバックゲインであり，次式のように与えられる。

K=R 'B'F (22)

ここでPは次の代数リカッチ方程式の正定対称解である。

qa2 （14）

邸'＝丁＞0

が要求される。ここで制御入力を電圧に換算するために式(4)より

〃=響+単

（15）

KK

が得られる。しかし，シミュレーションの結果，式 (14)で満足するだけではうまく振子は振り上げらな

かった。これは，アーム角速度9が小さ過ぎると振

子の角速度αも小さく，その結果，操作量"Iが小さく，振子軸の摩擦に打ち勝てないことが考えられる。これはスタート時点でも同様のことが言える。

このことを考盧して操作量は2通りについて考えた。

振り上げ領域でのエネルギを増加させる操作量"'は

①振子が真下付近でアームの振りが小さい場合

一γ≦9≦γのとき

",=","":sg"(9)

^（16）

ただし，

sg"(9)‑に,卿 α

である。つまりアームの角速度の符号を見て左右にモータの最大パワーで振らせる。〃,",":は操作量の最大値である。

剛+47P‑PBR‑'B'P+C℃C=0

^（23）

一般にすべての状態量を検出することは不可能であ

る。本研究においては，検出可能な物理量はアーム

の角度eと振子の角度αであり，残りのアームの角速度9と振子の角速度αは推定することになる。そ

こで，コントローラは推定機構であるオブザーバにカルマンフィルタを用いたLQGコントローラを用いた。LQGコントローラを用いた状態方程式は次式のようになる。

{童二麓+B"

②十分アームの角速度が得られる場合

9≦−γあるいは9≧γのとき

(24）

ただし，

4K=4‑HC‑BK BK=H Ck=‑K

である。また，上式のHはカルマンフィルタゲインであり，次式で求められる。

H=sCr (25)

ここで, Sは次式のリッカチ方程式の正定対称解である。

M+47S‑SCTCS+92BBr=0

（26）

この9はスカラパラメータであり，この場合の最適制御系はLQG/LTR法と呼ばれ9→COによりLQ

コントローラに漸近させることができる。よって，

安定化領域における制御系の設計は，重み関数Q,

￨割十卿⑨

〃ノー＝

(18）

となる。ここでγは非常に小さい整数値， 6は制御入力変数である。なお第2項は例え十分なアーム角

速度9があっても，摩擦を打ち消すエネルギが必要

であるので，そのための付加分を含んだ式(18)を式 (13)に代入すれば

E=9{6sg77(9)} (19)

であり，付加分の符号の調整をし，倉＞0を保って

いる。

5.3 STEP2安定化制御

式(7)で与えられるシステムに対して，制御則に

(6)

Rおよび9を与え，様々な試行の結果フィードバッ

クKとカルマンフィルタゲインHを得ることであ

るo

能」なので, E>0が成り立っているのかどうか，

そしてエネルギを単調増加させることができているかを検討する。図7にエネルギを時間微分したE の時間応答を示すo図7より, E>0を満たしていることがわかる。そして，システム全体のエネルギ Eはほぼ単調増加を示しているといえる。グラフ中の落ち込んでいる部分は，本研究ではアームスウィ

，ング角度の可動領域を‑90｡<8<90.と制限したためだと考えられる。これは制限したアーム角度を超えないようにするために，強制的に反対側へアームをスウィングさせているため，このとき一瞬力学的エネルギが落ち込むと考えられる。

6．シミュレーション結果

6.1 シミュレーションの設計

5章で設計した制御則の有効性を検証するため，

4章で求めた実機のパラメータを用いてシミュレーションを行う。シミュレーションのプログラムとし

てMatlab/Simulink')'2)を用いた。プログラムの構

成はSTEP1の振り上げ領域においてはMatlabのみで構成し, STEP2の安定化領域では,Matlab とSimulinkをリンクさせて計算を行った。

Simulink上で作成したブロック線図を図6に示す。

５４３２１０１２０００００００ −一

﹇つ﹈ン︑﹄の匡山

F呵叫pl

IngleofArm Scopel

mLE gleofPendulumScope2

F雨司ularvelocityoiAm

｢呵

伝

0 0．5 1 1．5 2 2．5 Time[s]

図7方の時間応答

図6 Simulink上で作成したブロック線図

6.3 STEP1の振り上げ制御の時間応答

ここでは，振子が最終目標値である真上（α＝

0｡）で安定するか，またその時のアーム角度がどのようになっているかを検討する。図8にアーム角度の時間応答，図9には振子角度の時間応答について示す。図8の破線は±90.でアーム角度の制限された領域を表し，図9の点線は±30.で, STEP1と

STEP2の境界線を表している。6.1節で述べたが，

また, STEP2におけるLQG制御における重み関数および振り上げ制御時に使用するパラメータのγ

6は以下の様に設定した。

Q=

R=0.¥

9＝〃00 7=0.1 6=0.02 また，システムの初期状態は

−一一

300

０００００２１１２３００００００

﹇の①﹄函の己Ｅ﹄く半○の遍匡く

U■■−0■■−＝ー■■■0■■ー＝＝■■p■■■■F■U■■q■■ーU■■毒＝ーーｰm■■0

[8cM 8d]r=[O‑I80｡0 0]T

である。またシミュレーションではアームのスウィング範囲を‑90｡<e<90｡として，この領域を出た場合は強制的に反対方向へスウィングさせた。

−‐ ‑‑ ＝＝−ーー＝一一＝ｰ＝＝ｰ＝＝ｰーーー要＝

0 0．5 1 1．5 2

2.5

Time[s]

図8アーム角度eの時間応答 6.2 システム全体の力学的エネルギについて

ここでは, ｢E>0であればE(8,q,8,a)の傾きが

常に正，つまりエネルギを単調増加させることが可

(7)

アーム角度の可動領域を制限したため，ほぼ±900

の範囲でアームが可動しているのがわかる。また図

9よりSTEP1の振り上げ制御領域では，振子は真下の−180。から出発し左右に振られ，安定化領域

へ振り上げようとしているのがわかる。その後，

2.7[s]付近でSTEP2の安定化制御領域に達することになる。

するとき，アームや振子の角速度をゼロ近傍に近づけることにより，運動エネルギを減少させスムーズ

な切り換えが可能になると考えられる。

以上のことからシミュレーション上では，初期状態から最終目標値で安定させることができたので，

今回提案した2ステップ°の制御則は有効であったと考えられる。しかしながら，実機に応用したした場合，制御則の切り換え時が問題になる可能性がある。

０００００００００００００３２１１２３−一一

﹇の⑩﹄即の己Ｅ三コでこのＱ﹂○の面匡く

7．結

一一一目

システムの非線形領域と線形化可能領域に分割し，

それぞれの領域に対して「STEP1:エネルギ法による振り上げ制御」と「STEP2:LQG制御理論を用いた安定化制御」を行うという2ステップ制御を提案した。提案した制御手法が有効であるかどうか検証するためにMatlab/Simulinkを用いてシミュ

レーションを行った。その結果をまとめると次のことが言える。

（1）非線形なシステムであっても，線形化可能領域を設定することで, LQG制御等の現代制御理論が適用でき，安定化制御が可能である。

（2）今回提案した力学的エネルギを増大させるエネルギ法は振子の振り上げ制御に有効である。

今後の課題としては, STEP1からSTEP2に切り換わる瞬間，アーム，振子ともにある程度の大きさの角速度を持っているので, LQG制御による操作量に影響を与える可能性があり，実機に本制御手法を適用して検証を行う予定である。

0 0．5 1 1．5 2 2．5 Time[s]

図9振子角度αの時間応答

6.4 STEP1とSTEP2を合わせた全体の時間応答アーム角度αについての初期状態から倒立した過程までの時間応答を図10に示す。STEP1からSTEP 2へ移行する瞬間に，振子は逆応答をしてスムーズな挙動が行われなかった。これは，制御則が急に切り換わってしまったことが原因と考えられる。この問題点を克服するには, STEP1とSTEP2の間に非線形なシステムに有効であると考えられている制御手法を用いることが考えられる。例えば，スライ

ディングモード制御を利用すれば安定化領域に移行参考文献

０００００００００５５０５０５０５−１１２２３３

−一一一一一

雨の﹄函の己Ｅコ言でこのα半◎の面こぐ 1）青山貴伸・蔵本一峰・森口肇著，吉田郷弘

監修，使える!MATLAB,講談社(2002)

2）川田昌克・西岡勝弘著，井上和夫監修，

MATLAB/Simulinkによるわかりやすい制御工学，森北出版(2001)

3）美多勉，非線形制御入門一劣駆動ロボットの

技能制御理論，昭晃社(2000)

4)Spong,M, "TheSwingupControlProblemfor

theAcrobot",IEEEControlSystemsMaga‑

zine,Vol.15,No.1,pp.49‑55,Feb. 1995.

0 1 2 3 4