姿勢制御

これまで作った倒立振子ロボットはICHROさんの倒立振子ロボットのクローンだったので、下記の状態方程式を元に姿勢制御をしていた。

モータの加速度をα、傾斜角θ、角速度ω、位置x、速度vとして

状態方程式

状態方程式

今回も、この状態方程式を利用してもよいのだが、カルマンフィルターでかなり正確な筐体の傾斜角度が得られているという前提のもと、PID制御で姿勢制御してみた。

PID制御

PID制御とは
P…比例(Proportional)
I…積分(Integral)
D…微分(Derivative)
の頭文字をとった制御方式で、現在の偏差eに比例した修正量を出す比例動作(Proportional Action:P)と、過去の偏差の累積値を元に修正量を出す積分動作(Integral Action:I)と、偏差eの変化の速度を元に修正量を出す微分動作(Derivative)で制御をする方式で、古典的と称される制御方式である。

PID制御の方程式は以下のとおり、

PID制御方程式

PID制御方程式

今回は鉛直に立ってる姿勢からの傾斜角度を偏差eとし操作量はステッピングモータの回転速度としてプログラムを組んでみた。

一応立った

何とか立った。それがこの動画である。
You need to a flashplayer enabled browser to view this YouTube video

電源コードとUSBケーブルを刺した状態では余裕で小一時間は立つようなのだが、ケーブルがないと倒れてしまう。

モータのトルク不足のようなのだ。筐体が載った状態でステッピングモータを低速で回すときには十分なトルクが必要なのだが、モータシールドとそのライブラリィではトルク制御が出来ないのだ。

色々と工夫が必要だ

倒立振子ロボット

倒立振子ロボット

カルマンフィルタ&PID制御は、倒立振子ロボットの姿勢制御には適してるようだ。
現状は一応は立ってるという状態なので、外力などによって姿勢が乱れても修正できるように、モータ周りとプログラムを修正する必要がありそうだ。