Acrobat

9.倒立振子の制御

9.1　本体角度θと角速度ωの取得

姿勢センサとして一般に知られているものに角速度センサがあり，自動車の姿勢検出やカメラの手振れ補正などに利用されています。本倒立振子には，MEMS技術を用いた小型な角速度センサを利用しました。


図9.1　角速度センサ　MLX90609	図9.2　加速度センサ　KXM52-1050

角速度センサからは角速度に比例した電圧が出力されるので，この値をマイコンでAD変換すれば本体角速度ωを得ることができます。また，これを時間積分すればθを得ることができます。ここで注意しなければならない事は，θが積分値であるため，温度変化による出力変化やノイズ等により得られるθが徐々にドリフトしてしまう点です。そこで，角度そのものを検出できる加速度センサを併用する手法を用いました。加速度センサを用いて重力加速度を検出すれば，本体角度θを直接検出できます。しかし，加速度センサには自身の並進運動による加速度も検知してしまう欠点があります。そこで，以下の方法を用いました。

相対的に見て，角速度センサは周波数の高い領域で，加速度センサは周波数の低い領域で優れていると言えます。よって，図9.3のように，各々の出力からフィルタで欲しい領域だけ取り出して角度同士で加えることにより，適切な車体角度θを得る事にしました。


図9.3　角速度センサと加速度センサの併用(a)	図9.4　角速度センサと加速度センサの併用(b)

ここで，LPFとHPFに同じ時定数τを持たせると，両者の伝達関数の和は1となります。すると，必要なHPFをLPFで実現することができます。

上記の表現が正しいかどうかは良く分かりませんが・・・。とにかく，2系統でLPFを共有できるようにすると，図9.4のようにフィルタ部を1個で済ませることができるので，計算資源を削減することができる！。詳しい原理はよくわかんないので制御工学の教科書をよんでください。

9.1.1　センサ回路

角速度センサはMLX90609を選択しました。このセンサは角速度ω[deg/s]に比例した電圧が出力されます。フルスケール（FS）は±300[deg/s]あります。 AD変換の分解能向上のため，CMOSオペアンプLMC6482で出力信号を-3.3倍で反転増幅しています。

加速度センサはKXM52-1050を選択しました。このセンサからは加速度に比例した電圧を出力し，FSは±2[g]です。こちらの信号出力はボルテージフォロワを介してSH/7125のAD変換器へ接続しました。

図9.5　センサ回路


図9.6　回路の位置	表9.1　センサ仕様

センサ回路は最も低い位置に設置しました。その理由は，加速度センサの並進運動をできるだけ抑えるため，センサ回路の回転中心と思われる車軸に出来る限りセンサ回路を近づけるべきだと考えたからです。

9.1.2　LPF

マイコン上でLPFを実現するため，オイラー法を用いました。プログラムで実装した計算式を以下に示します。

T_A：倒立制御周期　　k：1,2,3・・・　　τ：時定数　　y_k：フィルタ出力　　x_in：フィルタ入力

時定数τの設定は，実際にセンサの出力を観察しながら合わせ込むことにしました。用いた手順は以下の通りです。本当はもっとまともな方法がありそうですが・・・

角速度センサ，加速度センサ，及びフィルタ出力結果の3値を倒立制御周期T_Aで取得し、一定時間間隔（40ms）でPCに送信できるようにプログラムを作成。データはターミナルソフトを用いて受信し，ログファイルに残す。
上のプログラムを実行しつつ倒立振子を左右に振り，データを取得する。
ログデータをエクセルでグラフ化し，フィルタの出力を観察する。LPFが機能していることを確認する。
LPFの出力波形が適当になるように，時定数τを増加させつつ2と3を繰り返す。

加速度センサの出力は，鉛直が分かり角速度センサのオフセットが除去できる程度に反映できればよいと考え，時定数τを100[ms]から徐々に大きくしていきました。すると，τ = 3[s]で適当な出力を得られたので，この値をLPFの時定数に決定しました。・・・なんというか，手動で周波数特性を取ったという感じです

図9.7　フィルタ出力　τ = 100[ms]

τ = 100[ms]において①を見ると，LPFの機能が働いて，加速度センサのノイズを除外できているのが分かります。また，②を見ると角速度センサのオフセットが除去できています。しかし，③を見ると，フィルタ出力が角速度センサから求めた角度値の振幅を大きく上回っており，加速度センサ成分を過剰に反映していることが分かります。よって，τ = 100[ms]ではLPFによる帯域制限が不十分であると判断しました。

図9.8　フィルタ出力　τ = 3[s]

τ = 3[s]において，①や②の挙動はτ = 100[ms]と同様に良好です。さらに，時定数を増加させた効果が③に現れ，フィルタ出力の振幅と角速度センサ出力の振幅はほぼ一致するようになりました。与えている振動数は最大1[Hz]（周期1[s]）程度なので，3[s]の時定数はうまく機能したみたいです。ここで，角速度センサ積分値のドリフトを見てみると，動作開始からわずか16[s]で約4[deg]のドリフトが生じています。しかし，フィルタ出力はドリフトを除去できているので当面の倒立制御には問題ないと考えました。このドリフト値に改善の余地があるかどうかは分かりません。

9.2　車輪並進速度vの取得

vを得るために，モータはロータリエンコーダが付属するものを選択しました。ロータリエンコーダから出力される2相のパルス波の数から車輪傾斜角度φが分かるので，変換係数を乗じて倒立制御周期T_Aで割れば車輪並進速度vが分かります。また，パルス波の位相進みと遅れから回転方向を知ることができます。

図9.9　位相計数モード（2逓倍）

SH/7125には，このようなパルス波を計測するパルスカウンタ機能（位相計数機能）が用意されています。この機能をもつ端子に図9.9のような波形を入力すれば，内部のカウンタが自動でパルス波の個数を計測してくれます。vを求める計算は，以下の通りです。

位相計数を2逓倍で行うと，モータ一周あたり200[count]である。ギヤ比はn = 22.4なので，タイヤ一周あたりのカウント数は

ここで，1[count]あたりの走行距離分解能Δxは，タイヤの直径が80[mm]なので

倒立制御周期T_A あたりのカウント値の変化をΔCNTとすると

図9.10　駆動部

9.3　倒立制御ゲインの決定

5.2式のゲインF_1～4は以下の手順で決定しました。この決定方法は何らかの理論に基づいているものではなく，私が場当たり的に適用した方法です(；´Д`A ```

F₁だけ調整するため，F_2～4 = 0にしておく。また，速度目標値v_dもゼロにしておく。倒立振子を手で支えて直立させながら，F₁を徐々に増加させる。すると，倒立振子が自ら立ちはじめ，本体傾斜角度θのフィードバックが機能する様子が分かる。ある値で手の支えは指一本で十分となり，その指を左右に振ると倒立振子は追従するが，1テンポ遅れた反応を示す。
F₂を徐々に増加させる。ある値で指の支えを離すと，倒立振子は移動と静止を繰り返しながら直立するようになる。しかし，一定箇所に留まる事はない。この時，指1本で支えながら倒立振子を左右に振ると，1のような1テンポ遅れた反応は改善して滑らかに追従し，本体角速度ωのフィードバックが機能する様子が分かる。
F₄を徐々に増加させる。今まで一箇所に留まらなかった倒立振子の移動が止まり，位置（速度vの積算値）のフィードバックが機能する様子が分かる。しかし，指で押すなどして外乱を加えると，大きな振幅で発振して走行が止まらない状態になる。
F₃を徐々に増加させる。すると，指で倒立振子を左右に振った際に抵抗を感じるようになる。上記の発振が収束し，車輪並進速度vのフィードバックが機能する様子が分かる。