THINK GLOBALLY ACT LOCALLY AND RESPOND PERSONALLY|人と地球にやさしいエレクトロニクスづくりで社会に貢献します

   

ユタカ電子社員 個人的趣味活動

倒立制御を利用したロボットの製作

倒立振り子の制御は、ロボットのASIMOの足やセグウェイなどに利用されています。
常に制御ロジックが動いていないと倒れてしまう事から逆に床面の凸凹などの変化が有っても安定に移動できる特徴を持っています。
適度の制御の難しさから組込みソフトウェアの題材として面白さが有りますので、いざ挑戦!

倒立制御を使ったロボットとして、テレビなどでおなじみのムラタセイサク君とかムラタセイコちゃんなどをよく見かけます。
このロボットは、自転車型や一輪車型のロボットで胸の位置にあるフライホイルを回転させて、バランスを取るやり方を行っているようです。

初挑戦では少し難易度がたかいので今回は、車輪型の棒の倒立振り子のロボットを作る事にしました。
棒の倒立振り子は、ほうきを指の上に乗せて倒れない様に指を前後左右に動かして立たせた記憶があるかと思います。 あの要領で棒の下端を適当な方向と速さで動かす事で棒を立たせる事が出来るおもちゃを作りました。

使ってないラジコンのプロポを分解してステックを取り出し機構を使いました。 ステックには位置が判る様にポテンション(VR)が付いていましたので これを角度センサーとして利用します。 ほうきの代わりにラジコンヘリのロータ・スタビライザーバーを利用しました。駆動モータはRE-280RAです。 制御方法は、微分先行型PI-D方式をソフトウエアにて作りました。

棒の倒立制御が出来るならここで少し欲をだして二輪車倒立制御に初挑戦!

いざ物つくり開始!
CPUボードはPICのPIC18F26K22でクロックは64MHz
傾斜角を知る為にKionix社の三軸加速度センサ(測定レンジ:±2g)と田村製圧電振動ジャイロモジュール(測定レンジ:±300deg/sec)

CPU側面の部品位置は、左図のように配置しデバッグ途中でBATT位置を変更。

PWMモータ駆動はFull Bridge ICを実装。
駆動部は、タミヤ模型のミニ四駆モータとツインモータギアBOXを使用しました。

進行方向の傾斜角
初めは、加速度センサから傾斜角を取り出していましたが、動作中の加速度が加わり不安定で失敗しました。

今の進行方向の傾斜角は、加速度センサ+ジャイロセンサから角度を求めています。
角度は、ロボットが移動中に加速度により正しい角度が求まりませんので、ジャイロセンサからの角速度を積分した角度を ハイパスフィルタ(HPF)、加速度センサからの角度をローパスフィルタ(LPF)に通して合成することで、 角度がオフセットすることなく、ロボットが移動中も正しい角度が求まるように計算しています。
下図の様な方式をソフトで組み込みました。

ジャイロセンサの角速度:θg  加速度センサの角度:θα
本体の進行方向の傾斜角:θ、前回の傾斜角:θ(n-1)、
角速度(倒れる速度):ω、カットオフ周波数:fc、サンプリング時間:Δtω=2πfc
本体の進行方向の傾斜角 θ=θ(n-1) +θg×Δt -ω×θg(n-1) +ω×θα(n-1)

運動方程式等は単純な方法でPD制御式で行いました。
本体の進行方向の傾斜角:θ 角速度(倒れる速度):ω 位置(原点からの距離):d 前回の速度:v(n-1) 速度:v 加速度:α
角度ゲイン:Gθ、角速度ゲイン:Gω、距離ゲイン:Gd、速度ゲイン:Gv 二輪車倒立制御のモータの加速度をα
α=Gθ×θ+Gω×ω+Gd×d +Gv×v(n-1)
v=v(n-1)+α

今回の倒立制御の運動方程式は上記の式を使っています。
この式の前の2項は、車体を鉛直に保つためのPD制御(P:傾斜角、D:角速度)、後ろの2項が車体を最初の位置に保つためのPD制御(P:原点からの距離、D:回転速度)
この式で計算した結果の加速度で、二輪車を現在の速度からどれくらい加速、減速したら良いかを知ることができます。

ソフトデバッグも終えた時の写真です。ぶれていますがほぼ倒立しています。

最後には、子供のおもちゃになりモータから煙が出まして壊れました。

menu_top