マシンビジョン技術を用いた倒立振子の安定化

高知工科大学大学院
工学研究科基盤工学専攻
知能機械コース
田上浩士

目次
第一章 緒言1-1 はじめに1-2 倒立振子について
1-3

本研究の目的第二章 マシンビジョン技術
2-1 概要2-2 応用分野

第三章 実験装置
3-1 実験装置の構成
3-2 実験装置について

第四章 カメラでのセンシング方法
4-1 概要
4-2 2 値化画像処理による対象物の抽出
4-3 センシング試行実験結果

第五章 制御器設計
5-1 概要
5-2 システムのモデル化
5-3 振子系の運動方程式
5-4 アーム系の運動方程式
5-5 最適レギュレータ法による制御器設計

第六章 シミュレーション
6-1

概要

6-2

速度制御系

6-3

トルク制御系

第七章 周波数応答実験(速度制御システム)
7-1 目的7-2 システム概要
7-3 結果

第八章 安定化実験(速度制御システム)
8-1 概要8-2 実験
8-3 結果第九章 結言9-1 考察
9-2

今後の展望

参考文献

第一章

緒言

1-1 はじめに近年，ロボット工学の急速な進歩により 2 足歩行ロボットや犬型ロボットの登場が相次いだ．ロボットは人間にとってより身近なものになってくるだろう．そんな時代に制御工学を学ぶ事は非常に意義があるといえる．

1-2 倒立振子の研究について
倒立振子は制御工学のテーマの中でも最もポピュラーなものの一つであろう．誰もが子供のころに，ほうきを手の平の上に乗せて倒れないようにする遊びを，掃除の時間にさぼってやった記憶があると思う．倒立振子とは読んで字のごとく逆立ちした振子の事である.
想像していただきたい,童謡｢大きな古時計｣で歌われている『チックタック，チックタック』
している振子を上下逆にしたものを．
このように身近な存在であるため様々な振子系について研究が為されている. Fig.1 は最も基本的な型の倒立振子系である．振子が倒れると振子の上端の真下に台車が動くように制御して振子を安定化させるものである.Fig.2 は Fig.1 の応用で振子の上に振子が乗っているようなタイプである．Fig.3 は台車の替わりにシーソーを用いて倒立振子の安定化を図るタイプである．

Fig.1 Pendulum system on a cart1

Fig.2 Pendulum system on a cart2

Fig.3 Pendulum system on a seesaw

振子の動きを感知するためのセンサにはエンコーダやポテンショメータなどがよく用いられる．
Fig.4[1]は光学式のエンコーダの構成を示したものである．光学式エンコーダはディジ
タル式の検出器で発光ダイオードなどの光源からの光をコリメータレンズで鉛直軸に平行な光束にした後，回転ディスクにあけたスリットを通過させる．それをフォトダイオードなどの受光素子で受ける構造となっている．入力軸が回転するとスリットを通過した光は正弦波となるが，一定電圧レベルで切ってパルス波形に変換する．そのパルス出力を検出することで変位量が計測される[2]．
Fig.5 は回転変位形のポテンショメータを示したものである．ポテンショメータはアナログ形の変位センサである．ブラシが抵抗体上を 慴動すると，電気抵抗が変化することにより，回転角を検出する．抵抗体の変位量当りの抵抗値は一定なので，入力端に一定電圧 (Vi ) を加えると，変位に比例した出力電圧 (Vo ) はθを回転角度とすると次式で求められる．

Vo = kViθ

(k は比例定数)(1.1)

よって(1.1)式で Vo を検出することで変位量が計測できる，

Fig.4 Optical encoder
1-3 本研究の目的

Fig.5 Potentiometer振子の動きを計測するためにエンコーダ，ポテンショメータのようなリアルタイム性が高いセンサを用いるのではなく，性能が向上したとはいえリアルタイム性で劣る視覚セン
サを用い，より人間に近いシステムで安定化をめざす．よって，本研究では CCD カメラ
で振子の位置をセンシングしながらアーム上に乗せただけの形で２自由度運動する倒立振子を立たせることが目的である．
またＰＤ速度制御の場合でシミュレーションを行い，カメラからの 2 自由度振子の座標値から算出された振子と鉛直軸との角度をアームにフィードバックしてＰＤ制御を行った実験結果を報告する．

第二章 マシンビジョン技術

2-1 概要マシンビジョン技術は基本的にコンピュータ，画像処理ソフト，カメラで構成されており，人の目の替わりをするために発達した技術である，近年 CPU の高速化，カメラの小型化やサンプリング周波数の向上でセンサとしてのリアルタイム性が向上してきた．
Fig.6 はマシンビジョン技術を用いた検査システムのモデル図である．ラインを流れる対
象物をカメラで撮影し，その画像をパソコンに送る．画像処理ソフトを用いて画像から対象物の特徴(位置，形状，大きさ等)を計測する．

Fig.6 Inspection system with Machine Vision

2-2 応用分野[3]
具体的なマシンビジョン技術の応用分野を挙げてみる．
①食品，医薬品分野
まず身近な商品を取り上げてみる。最近いろいろと問題になって注目が集まった食品分
野においては，野菜果物や魚介類の大きさ選別や鮮度検査を行って商品を分類する必要がある．医薬品の製造工程では，錠剤，顆粒剤，カプセルの形状の検査が必要とされる．ビールびんはリサイクルされて使われるので，傷のあるびんが混入していると，これを検出しなければならない．またビールびんに限らず，びん製品の中に異物があると大きな問題になるのでその検査も必要である．これらの検査を目視で行おうとすると，先のFig.6でカメラを人に,対象物をビールびんに置き換えて考えてみると高速で流れる製品を作業者が注視する作業になる．人が長時間集中して検査する作業には無理があるので，これらの作業を画像処理により自動的に行うことが要望されており，
一部では既に実用化されている.

②文字認識および検査
郵便番号の読み取りに文字認識が使われていることは有名である．最近では、パソコン
用画像スキャナーに画像文書をテキスト化するソフトウエアが付属している．また商品や工業製品に貼られたラベル印刷文字にかすれや誤り等があると無用の混乱が起こるので検査が必要とされる．

③交通，自動車のナンバー読み取り
近年交通分野での画像応用が盛んである．高速道路での自動車番号読み取りシステムは良く知られている．また，道路の混雑度を計測して情報の提供や車の流れを制御することも試みられている．その他，踏み切り監視や事故が多発する道路で交通事故を自動監視して，異常がある場合に後続車に警告を出すシステムも試みられている．

④医療分野
医用分野では，画像処理の目的は自動化ではなく人による認識や理解を助ける手段として用いられることが多い．X 線画像や人体断層撮像装置CT(Computer Tomography )画像のコントラストを強調し，画像を見やすくするための処理が研究されている．最近では，
人体内の血管などを3 次元的に画像化して組織の把握を助ける試みが行われている．

⑤監視モニター
化学プラント工場を常に監視して，蒸気もれや装置の過熱などの異常を検知することが試みられている．
画像処理は装置異常検知の他にも，侵入者を検知する用途に有効である．
また，室内の人の有無や位置を検知して空調機を適切に制御する研究が行われている．これらの場合，赤外線画像が使われることが多い．

このようにマシンビジョン技術は第1次，第2次，第3次産業とあらゆる分野で応用されており，今後ますます我々の生活に様々な影響を与えるのは必至である．

第三章 実験装置

3‑1 実験装置の構成
Fig.7は実験装置の構成を示す．カメラは鉛直方向から,上から吊るして配置する．そうすれば振子上端を真上から見るようにして撮影すれば縦と横の2次元情報が得られ，
センシ
ングがしやすい．よって本研究ではこの方法でセンシングする.実験装置は2軸水平駆動多関節ロボットアーム, サンプリング周波数120Hzである単眼モノクロCCDカメラ，パソコ
ン,棒から構成されている.
パソコンは速度制御電圧またはトルク制御電圧をロボットアームに入力する．ロボット
アームは指令された速度または指令されたトルクで駆動する．同時にアームに内蔵されているエンコーダが初期位置からの変位量である角度を計測しパソコンにフィードバックする．アームは倒立振子との間に発生した摩擦力を振子に伝えて制御する．振子を撮影しているカメラはその画像をパソコンに入力する．パソコンは入力画像から画像処理ソフトによって機械的に扱いやすい位置情報を検出し，内部で制御に必要な移動量を算出する．最後に必要移動量の電気信号がPD制御器を通過して，パソコンは速度制御電圧またはトルク制御電圧を出力する．

Fig.7 Control system

3-2

実験装置について倒立振子：市販されている棒状製品
ロボットアーム：東京エレクトロニクツシステムズ社製 SR-402DD(S)(Fig.8)CCDカメラ：PULNiX社製 TM-6710 (Fig.9)
画像入力ボード：BitFlow社製 PCIバス対応画像入力ボードRoad Runner/Raven
画像処理ソフト：MVTec社製 HALCON5.2
DSPボード，D/Aコンバータ：dSPACE社製 DSP Controller Board DS1102
制御器設計：MATHWORKS社製 MATLAB5.3(R11.1)
PC：PentiumⅢプロセッサ(866MHz)搭載AT互換機
実行プログラム作成：マイクロソフト社製 Visual C++6.0

Fig.8 Photograph of robot arm

Fig.9 Photograph of CCD camera

第四章 CCDカメラを用いたセンシング実験

4‑1 概要本実験では静止した対象物の撮影画像を一枚の静止画と見なした時における対象物の位置検出が可能かどうかを確認した。検出可能ならば静止画の連続である動画の時も検出可能といえ，動く対象物の場合もセンシング可能である。

4‑2 2値化画像処理による対象物の抽出
Fig.11はカメラからの入力画像を示す．入力画像からだけでは対象物の位置情報は得られない．そのためしきいち中央の白い円を対象物としてその位置情報を検出してみる．ただし本研究で用いる画像処理プログラムは複数の円形領域を個別に認識するように作成していないため，対象物と同じような白色の円形部分が入力画像内に入らないようにしておく．なお原点は入力画像の左上隅である.

Fig.11 Input image 1

カメラからの入力画像を一定の輝度を境として，それよりも輝度の低い領域を0に，それ以上の輝度を255とする2値化処理を行う．Fig.12はFig.11を輝度100で区切り，その値より小さい部分は黒く,明るい部分は白くしたものである．この段階では対象物のFig.12 Input image 2

Fig.13はFig.12の白色領域内で真円度0.6以上の円形部分のみを抽出したものである．真円度を0.6以上と幅を持たせたのは，実際に実験を行う際に振子の上端はその傾きによって円形部分が楕円なってしまうためである．そして抽出された円の中心座標(x,y)が振子の位
置情報となる．

Fig.13 Input image 3

4‑3 センシング試行実験結果
いくらカメラのサンプリング周波数が120Hzあっても,画像処理プログラムの実行速度
でセンシング性能は変わってくる．そこでしきい値を用いたセンシング方法で実際にセンシングを試してみた．Fig.14は周波数1Hz，振幅0.1733[rad](=10°)で駆動させたリンク2
上に載せた対象物（Fig.11参照）を5秒間センシングした結果を示す．縦軸がy座標である．
多少波形が荒くプログラムの実行速度がカメラのサンプリング周波数に達していないことがわかる．事実，プログラムの実行速度は25Hzほどであった．この条件下でアームが振子に追従して駆動するかどうか実験(周波数応答実験)して確かめる必要があるがそれは第七章で述べる事にする．

Fig.14

Sensing result

第五章 制御器設計

5-1 概要実験を行う前に倒立振子の安定化が可能かどうか，理論的検証であるシミュレーションをしなければならない．そのために必要な最適ゲインを求める．その手順を以下に示す。
①倒立振子系とアーム系をモデル化し，それぞれの運動方程式をたてる．
②運動方程式から線形制御理論で利用できる状態空間モデルを求める．
③状態空間モデルからMATLABで最適ゲインを求める．

5-2 システムのモデル化
Fig.15，Fig.16は倒立振子系，アーム系をそれぞれモデル化したものである．

Fig.15 System of inverted pendulum

Fig.16 System of arm

各パラメータは次のとおりである.

m :振子の質量,M1 :1軸アーム質量,M 2 :2軸アーム質量,l :振子の回転軸から重心までの距離,
I :振子の慣性モーメントI 1 :1 軸の慣性モーメントI 2 :2 軸の慣性モーメントl1 :1 軸のリンク長さ
l2 :2 軸のリンク長さ

φ 1 :1 軸の x 軸との角度
φ 2 :2 軸の 1 軸のリンクとの角度
g ：重力加速度Fig.15において倒立振子系のモデルがZ-X平面の場合しか示してないのは，振子の運動
はZ-X，Z-Y平面に分ける事ができ、それぞれの平面内の振子は同様の運動をするためである．つまりZ-X平面とZ-Y平面のモデルはパラメータの添え字が違うだけである．
またFig.16においてリンク角を φ 1 = 90 , φ 2 = −90 としてアームの基本姿勢としoo

Fig.17に示す．

Fig.17 Arm’s initial position

ここでジョイントの角度変位が微小であると仮定すると，ジョイント１はｘ方向、y 方向両方の成分の動きをジョイント 2 は y 方向の動きのみを実現することになる．ただし最適ゲインを求めるときはジョイント１はｘ方向の動きのみをジョイント 2 は y 方向の動きのみを実現しているとみなして求める．

5-3 倒立振子系の運動方程式
ラグランジュの運動方程式から倒立振子系の運動方程式は次式になる，

mL cosθ x &&x + ( I + mL2 )θ&& x − mgL sin θ x = 0(5.1)

5-4 アーム系の運動方程式
アーム系の運動方程式は速度制御かトルク制御の場合で大きく異なる．速度制御系の場合は次式とする．

x&& + ζx& = ξu(5.2)

( u は入力電圧)トルク制御系の場合は、ラグランジュの運動方程式より，( J1 + J 2 + 2 R cos φ 2 )φ&& 1 + ( J 2 + R cos φ 2 )φ&& 2 − 2 R sin φ 2 ⋅ φ& 1φ& 2 − R sin φ 2 ⋅ φ& 2 2 = τ 1(5.3)( J 2 + R cos φ 2 )φ&& 1 + J 2φ&& 2 + R sin φ 2 ⋅ φ& 12 = τ 2

(5.4)

ただし，

1
1
1
J1 = ( M 1 + M 2 ) L12 + I 1 , J 2 = M 2 L2 2 + I 2 , R = M 2 L1 L2442

5-1 最適レギュレータ法による制御器設計
線形制御理論を利用するためには運動方程式を線形化する必要がある．
倒立振子は θ x = 0 の近傍で安定化するとして，式(5.1)を線形化すると,

mLx&& + ( I + mL2 )θ&& x − mgLθ x = 0

式(5.3)，(5.4)を φ 1 = 90 , φ 2 = −90
o

o

(5.5)

を代入し，1軸と2軸は互いに独立しているので&& 以外の項を消去すると，
式(5.3)の φ
1( J1 + J 2 )φ&& 1 = τ 1&& 以外の項を消去すると，
式(5.4)の φ
2(5.6)

J 2φ&& 2 = τ 2(5.6)

まず速度制御系の最適ゲインを求める．式(5.2)，式(5.5)よりそれぞれ，

x&& = −ζx& + ξu

θ&& x =

(5.7)

mgL
mL
θx +
( −ζx& + ξu)2( I + mL )( I + mL2 )(5.8)となる．ここで上式を状態空間モデル，いわゆる状態方程式に変換するために次のように状態量を定める．

Fx I
Gθ J
X =G J
GG x& JJ
Hθ& K
x

(5.9)

x

これを時間で微分すると，

F x& I
GGθ& JJ&X=
GG x&& JJ
Hθ&& Kxx

式(5.7)，式(5.8)を式(5.9)，式(5.10)を用いてあらわすと，(5.10)

X& = AX + Bu

(5.11)F 0I
F 0 0 1 0I
G 0 JJG 0 0 0 1JJ
A=G
B=G
GG ξ JJ
GG 0 0 −ζ 0JJ
H 0 pg pζ 0K , H − pζ K
p=mL
I + mL2上式により，倒立振子システムの運動特性を表現するモデルを得ることができた．ところ
で，フィードバック制御を行う場合には，制御対象のどのような情報が得ることができるのかを明確にしておく必要がある．それはその情報に基づいて制御を行うためである．本
研究での倒立振子システムにおいてはロボットアームと振子の位置がエンコーダとCCDカメラにより計測可能である．これらの量は，次式で表現することができる．

FG x IJ = CX
Hθ K
F1 0 0 0IJ
C=G
H 0 1 0 0K

Y=x(5.12)

この方程式を観測方程式と呼び，式(5.11) ，(5.12)をあわせて状態空間モデルと呼ぶ．これからシミュレーションや実験に必要な最適ゲインを求める．
ところで，式(5.2)はこちらがアームの運動方程式を勝手に定義したものであるから、

ξ ,ζ は未知数である．よってこれらを求めなければならない．そのために式(5.2)から伝達関数 G ( s) を求めると，

G ( s) =ξ(5.13)s( s + ζ )となる．この伝達関数中の物理パラメータを同定するために周波数応答法を用いた結果,
リンク1のパラメータは，RSζ = 28
Tξ = 42.5
1(5.14)

1

リンク2のパラメータはRSζ = 28
Tξ = 852(5.15)

2となり，これらより式(5.11)でリンク1のパラメータは，

1
0I
0
F0
F0
I
JJ
GG 0
GG 0
0
1J
0
J
A =
B =
-28
0J
0
JJ
GG 0
GG 42.5
J
H 0 11.9512195122 34.1463414634 0K , H -51.82926829K
1

リンク2は、

1

1
0I
0
F0
F0
I
JJ
GG 0
GG 0
0
1J
0
J
A =
B =
-28
0J
0
JJ
GG 0
GG 85
J
H 0 11.9512195122 34.1463414634 0K , H -103.6585366K
22となる．これらを用いてMATLABで最適ゲインを求める．ここで

k11 :リンク1の比例ゲイン

k 21 :リンク2の比例ゲイン

k12 :Z‑X平面内の振子の比例ゲイン

k 22 : Z‑Y平面内の振子の比例ゲイン

k13 :リンク1の微分ゲイン

k 23 :リンク2の微分ゲイン

k14 : Z‑X平面内の振子の微分ゲイン

k 24 : Z‑Y平面内の振子の微分ゲインとすると、リンク１の最適ゲインは( k11 , k12 , k13 , k14 ) = (-10, - 26.4613, - 8.069, - 7.71)(5.16)

リンク2は，( k 21 , k 22 , k 23 , k 24 ) = (-10, - 25.3145, - 7.5739, - 7.3812)(5.17)となり、速度制御系における最適ゲインが求まった．
次にトルク制御系における最適ゲインを求める．式(5.5)，(5.6)よりリンク１はφ&& 1 =

τ1
( J1 + J 2 )(5.18)

θ&& x =

τ1
mgL
mL
⋅
θx +
2
2
( I + mL )( I + mL ) J1 + J 2(5.19)となり，上式から状態方程式を求めるために状態量を，

Fφ I
GGθ JJX = &GGφ JJ
Hθ& K
1

x

1

(5.20)

1xとすると，状態方程式はX& 1 = At 1 X 1 + Bt 1τ 1

(5.21)F 0 I
GG 0 JJ
F 0 0 1 0I
GG 0 0 0 1JJ
G 1 JJ
A =
B =G
J +J
GG 0 0 0 0JJ
G
H 0 pg 0 0K , GG p JJJ
HJ +J K
t1

p=

t1

mL
I + mL2

1

2

1

2

FG φ IJ = CX
Hθ K
F1 0 0 0IJ
C=G
H 0 1 0 0K

Y=

1

t1

x

(5.22)となる．式(5.19)のパラメータをMATLABを用いて求めると，

1 0I
0
F0
F0
I
JJ
GG 0
GG 0
0 1J
0
J
A =
B =
0
0 0J
GG 0
GG1.376034434JJJ
H 0 11.9512195122 0 0K , H1.678090773K
t1

t1となった．次にリンク2の状態方程式を求めるために状態量をFφ I
GGθ JJY= &GGφ JJ
Hθ& K
2

x

2

(5.23)xとすると，リンク2の状態方程式は，Y& = At 2 Y + Bt 2 τ 2(5.24)

F0I
GG 0 JJ
F 0 0 1 0I
GG 0 0 0 1JJ
G 1 JJA =B =GJ
GG 0 0 0 0JJ
G
H 0 pg 0 0K , GG p JJJ
H JK
t2

t2

2

2

p=

mL
I + mL2

FG φ IJ = CY
Hθ K
F1 0 0 0IJ
C=G
H 0 1 0 0K
Z=

2

x

(5.25)となる．式(5.22)のパラメータをMATLABを用いて求めると，

1 0I
0
F0
F0
I
JJ
GG 0
GG 0
0 1J
0
J
A =
B =
0
0 0J
GG 0
GG 9.335978527JJJ
H 0 0.026902013 0 0K , H11.38533967K
t2

t2

となった．
これらからリンク1，2のトルク制御系における最適ゲインをMATLABを用いて求める．
ここで

kt11 :リンク1の比例ゲイン

kt 21 :リンク2の比例ゲイン

kt12 :Z‑X平面内の振子の比例ゲイン

kt 22 : Z‑Y平面内の振子の比例ゲイン

kt13 :リンク1の微分ゲイン

kt 23 :リンク2の微分ゲイン

kt14 : Z‑X平面内の振子の微分ゲイン

kt 24 : Z‑Y平面内の振子の微分ゲインとすると，リンク1のトルク制御系における最適ゲインは，( kt11 , kt12 , kt13 , kt14 ) = ( -31.6228, 111.0853, - 26.0808, 32.2589)(5.26)

またリンク2のトルク制御系における最適ゲインは，( kt 21 , kt 22 , kt 23 , kt 24 ) = (-31.6228, 83.0687, - 23.2115, 24.2065)

これでシミュレーションや安定化実験に必要な最適ゲインが求められた．

(5.27)

第六章 シミュレーション

6‑1 概要第五章では最適ゲインを求めるための運動方程式をたてた．トルク制御系のシミュレー
ションは第五章の運動方程式を用いればよいが速度制御系シミュレーションの場合はアーム系モデルにおいてリンクの運動を互いに独立しているものと見なしていたので，第五章のとは別の方程式を立てる必要がある．その後，倒立振子システムの安定性を理論的に検証するために速度制御系とトルク制御系それぞれのシミュレーションを行った．

6‑2 速度制御系シミュレーション
倒立振子系の運動方程式は，

mL cosθ x &&x + ( I + mL2 )θ&& x − mgL sin θ x = 0(6.1)

mL cosθ y &&y + ( I + mL2 )θ&& y − mgL sin θ y = 0(6.2)となり，アームの運動方程式は，

φ&& 1 + ζ 1φ& 1 = ξ 1U φ& 1

( U φ& 1 は角速度 φ& 1 のための入力電圧)(6.3)

φ&& 2 + ζ 2φ& 2 = ξ 2U φ& 2

( U φ& 2 は角速度 φ& 2 のための入力電圧)(6.4)となる．またアーム手先の速度とリンクの角速度との関係は，

FG x& IJ = J Fφ& I J = FG −l sin φ − l sin(φ + φ ) −l sin(φ + φ )IJ
H y&K GHφ& JK H l cosφ + l cos(φ + φ ) l cos(φ + φ ) K
1

2

1

1

1

1

2

2

1

1

2

2

2

2

1

1

2

2

(6.5)

となる．ちなみにJはヤコビアンと呼ばれる行列である．これらの方程式に基づいたシミュ
レーションのダイアグラムをFig.17に示す．

Fig.18 Block diagram for velocity control system

x が入力され，
Z-X 平面内における振子の運動方程式のブロックは手先の加速度である &&振子の角度 θ x を出力する．Z-Y 平面の方も同様である． U Vx , U Vy はアーム手先が速度

FU I
GHU JK は逆ヤコビアンをかけてジョ
FU I&&
イント 1，2 の角速度 φ , φ を出力するのに必要な入力電圧 G U J に変換される．それ
H K
V x ,V y を出力するのに必要な入力電圧のことである．

Vx
Vy

φ& 1

1

2

φ& 2

ら入力電圧をリンク１，リンク 2 の運動方程式のブロックに入力すると，ジョイント 1，2の角速度 φ& , φ& がそれぞれ出力される．
1

2

F φ& IGH φ& JK はヤコビアンをかけてアーム手先の速度
12

V x ,V y に変換される． V x ,V y をそれぞれ時間微分するとアーム手先の加速度 x&&, &&y がでる．
Fig.19，Fig.20はZ-X平面内の倒立振子とリンク1の，Fig.21，Fig.22はZ-Y平面内の倒立
振子とリンク2の，Fig.23，Fig.24は振子上端とアーム手先のxy平面内におけるシミュレー
ション結果を示す．ただし振子の初期値は鉛直軸との角度を0.034[rad]とし、手先の座標
を x = 0.2075[m], y = 0.199[ m] とした．結果は振子の目標値を0[rad]，アームの目標値

を x = 0.2075[m], y = 0.199[ m] としたとき各偏差が1.5秒前後で0に収束していることがわかる.
またFig.25，Fig.26は振子の初期値を θ x = −0.087[rad], θ y = 0.087[ rad ] としたときの振子上端とアーム手先のxy平面内の軌跡である．初期角度が大きい分振子とアームの動きも大きくなっている．

Fig.19 Simulation result1

Fig.20 Simulation result2

Fig.21 Simulation result3

Fig.22 Simulation result4

Fig.23 Simulation result5

Fig.24 Simulation result6

Fig.25 Simulation result7

Fig.26 Simulation result8
6‑3 トルク制御系
Fig.20は式(5.1)，(5.3)，(5.4)に基づいて作成したトルク制御系におけるダイアグラムである．Z-X平面内の振子を表す P1( s) は鉛直軸との角度 θ x を出力する．その角度は振子のＰＤゲインのブロックを通過した信号とリンク１の角度 φ 1 がリンク1のＰＤゲインを通過した信号を足し合わせた信号がリンク１にかかるトルク τ 1 である．そのトルクがリンク1&& に変換される．角加速度は積分器を表すブロック G1( s) に入力されリンク1の角加速度 φ1を通過するごとに速度 φ& 1 ，制御量 φ 1 に変換され， φ& 1 は P1( s) と P2( s) に φ 1 はリンク1のＰＤゲインに入力される．また，Z‑Y平面内の振子を表す P2( s) は鉛直軸との角度 θ y を出力する．その角度は振子のＰＤゲインのブロックを通過した信号とリンク2の角度 φ 2 がリ
ンク2のＰＤゲインを通過した信号を足し合わせた信号がリンク2にかかるトルク τ 2 であ&& に変換る．そのトルクがリンク2を表すブロック G 2( s) に入力されリンク2の角加速度 φ2される．角加速度は P1( s) に入力され，積分器を通過するごとに速度 φ& 2 ，制御量 φ 2 に変換される， φ& 2 は P1( s) に P2(s) ， φ 2 はリンク2のＰＤゲインに入力される．

Fig.27 Block diagram for torque control system

Fig.26，Fig.27はZ-X平面内の倒立振子とリンク1の，Fig.28，Fig.29はZ-Y平面内の倒立
振子とリンク2の，Fig.30，Fig.31は振子上端とアーム手先のxy座標(単位は[m])におけるシミュレーション結果を示す．ただし振子の初期値は鉛直軸との角度を0.034[rad]とし、各リンクの角度を φ 1 = 0, φ 2 = 0 とした．また目標値は振子の角度を0[rad]とした．
結果は振子，リンクとも2秒前後で収束していることがわかる.
また振子の初期値を θ x = −0.087[rad], θ y = 0.087[ rad ] とした時の振子上端とアーム手
先のxy平面内におけるシミュレーション結果をFig.35，Fig.36に示す．

Fig.28 Simulation result9

Fig.29 Simulation result10

Fig.30 Simulation result11

Fig.31 Simulation result12

Fig.33 Simulation result13

Fig.34 Simulation result14

Fig.35 Simulation result15

Fig.36 Simulation result16

第七章 周波数応答実験(速度制御システム)

7‑1 目的
この章の周波数応答実験はマシンビジョンでフィードバックして倒立振子の実験を行うための基礎データとして振子の動きにアームがどれだけ追従できるか確かめるのが目的である．

7‑2 システム概要
Fig.32に示すような装置を用いて次のような実験を行った．まず直線運動をするボイス
コイルモータ(以下VCM)にシャフトつきの球体を取り付けたその球体を振子上端に見立てて，様々な周波数で振動させた動きをカメラでセンシングした．このカメラから得られた情報に基づいてアームを追従させる実験を行った．VCMの動きは1自由度なのでリンク１のみを動作させる場合，リンク2のみの場合それぞれ個別に行った．カメラで見る対象物の動きを入力としアームの動きを出力としたシステムの特性を調べた．

7‑3 結果
具体的には，Fig.33の運動を目標値として，球体をカメラでセンシングし，そのx座標を入力とした時の各リンクの応答波形を記録した．Fig.34はリンク1の出力であり，Fig.35はリ
ンク2の出力である．両者ともにこれ以上周波数を高くすると追従できなくなることがわかった．そこで振子の長さを調節して，振れる速さが７Hzより小さく抑えられれば，安定化可能であるといえる．

Fig.37 Frequency response experimentFig.38 Ball movement

Fig.39 Frequency response result1

Fig.40 Frequency response result2

第八章 安定化実験

8‑1 概要第六章のシミュレーションのダイアグラムに基づいた制御器に第五章で求めた最適ゲインを入れてコントローラを作成した後，ＣＣＤカメラでセンシングしてアームを速度制御
して実際に倒立振子の安定化実験を行った．

8‑2 実験
実際に振子を立たせるための実験を行った．初期位置を決めるために，振子を立たせる
位置にマーカーを置き，その位置をカメラで取り込み，振子の安定化のための目標位置とした．振子の角度は入力画像より振子上端とアーム位置の座標との差分によって求めた．
振子の角度とアームの手先位置に対してPD制御を行った．

8‑3 結果シミュレーションで用いた最適ゲインで安定化実験を行った結果，倒立振子の倒れる速度にアームが追従しきれていないので安定化できなかった.

第九章 結言

シミュレーションで用いた最適ゲインで安定化実験を行った結果，倒立振子の倒れる速
度にアームが追従しきれていない．
第一に，制御器に対して外乱であるリンク間の動摩擦力の影響が大きすぎるためだと思われる．
第二に，カメラのサンプリング周波数120Hzに比べて画像処理ソフトの実行速度が25Hzと遅いためということも考えられる．実行速度が遅いと非常に粗いセンシングになり，振子上端の動きを正確に捉えきれなくなる．
第三に，振子上端がカメラの視界内からすぐにでてしまうことも問題である．ただカメ
ラを吊るしている天井の高さには限界があり，自ずとアームからカメラまでの高さも限定されてしまう．カメラの視界の広さは距離に比例するので，振子が長ければ長いほど倒れる速度は小さくなるが，カメラの視界が狭くなり振子上端は外に出やすくなる．振子が短ければ倒れる速度が大きくなり，視界が広くても外に出やすくなってしまう．このあたりの兼ね合いの難しさが不安定の要因とも考えられる．
最後に，振子の初期位置の決定が非常に微妙である事も考えられる．第八章の安定化実験において振子を立てるのは人の手であり，振子の初期位置を判断するのも人の目であるために振子の鉛直軸に対する角度が5°以上あると安定が困難である．

参考文献[1] 多摩川精機(株)：エンコーダとその応用[2] 土谷武士，深谷健一:メカトロニクス入門[3] FEST Project編集委員会：実践画像処理