Home Made Garbage

1軸 姿勢制御モジュール がやっと立った ーリアクションホイールへの道7ー

ホーム
電子工作
Raspberry Pi
IOT
WEB

さて、前回の1軸 姿勢制御モジュール SHISEIGYO-1 (シセーギョーワン) 倒立に向けた下準備から随分日が経ちました。

実はずーーーーっと倒立させるべく試行錯誤しておりました。
この度無事になんとか倒立させることができましたのでご報告いたします。

 

 

倒立した!

まずは SHISEIGYO-1 が倒立した勇ましい姿をご覧ください。

マジで何度も心が折れそうになりましたが遂に倒立を確認することができました。
ちなみにモータ制御なしで倒立することはありません。

以下で今回実施した制御方法を記載いたします。

 

制御方法

姿勢制御モジュールは作用反作用の法則を用いて倒立を維持しています。
倒立地点 (傾き0°) から傾いた方向にフライホイールを回して反作用で起き上がります。

傾きを検出し、その状況に応じたモータ回転に制御して倒立を維持します。

 

2020年6月号のトラ技の3軸姿勢制御モジュールの記事にモータのトルク$T_m$を以下の式に基づいて制御していると記載がありました。

$$T_m = -K_{d1}・θ_b-K_{d2}・\dotθ_b-K_{d3}・\dotθ_w   (1)$$
$$θ_bはモジュールの傾き、\dotθ_wはホイールの角速度、K_{dx}はそれぞれの係数$$

こちらを参考にSHISEIGYO-1も制御することにいたしました。

モータ電流特性

SHISEIGYO-1はリアクションホイールとしてフライホイール付きのブラシレスモータ ID-549XWを使用しています。

モータのトルクはモータに流れる電流に比例します。
SHISEIGYO-1のモータはPWM入力によって回転速度を制御しますので、PWMのOFFデューティーとモータに流れる電流の関係を測定しました。

モータに入力するPWM信号のOFFデューティーを上げていくとブラシレスモータ ID-549XWの回転速度も上昇し負荷電流も増えます。

上の測定結果のグラフからデューティーとモータ負荷電流Iはほぼリニアに比例していることがわかりました。

したがって上式(1)より

$$T_m ∝ I ∝Duty ∝-K_{d1}・θ_b-K_{d2}・\dotθ_b-K_{d3}・\dotθ_w$$

となり、モジュールの姿勢角($θ_b$) とその角速度($\dotθ_b$) とモータの回転速度($\dotθ_w$) からモータに入力するべき信号のデューティー比が導き出せると言えそうです。

また、SHISEIGYO-1がこの理論式だけで立つと感覚的に確証が持てなかったので更に以下のパラメータも追加して、手動で各パラメータを調整して倒立を目指しました。

  • 傾きオフセット補正
     SHISEIGYO-1の物理的偏りやセンサのオフセットを数値で補正します。
  • モータ初速
     モータの電気的抵抗や物理的摩擦による抵抗を打ち消すためのオフセット値
  • ブレーキ範囲
     倒立する姿勢角でモータがきっちり止まるためにブレーキをかけるデューティー値の範囲を指定します。

 

構成

前回と変更はありません。
PWM入力線とエンコーダ出力、ブレーキ端子をATOM Matrixに接続し制御します。

 

部品

 

Arduinoコード

 

  • L. 135~139
    M5 ATOM Matrix内蔵の6軸慣性センサ MPU6886による加速度センサとジャイロからカルマン・フィルタライブラリを用いてモジュールの姿勢角 (kalAngleY )とその角速度 (kalAngleDotY )を算出します。

  • L. 132
    モータの回転速度 (theta_dotWheel ) はエンコーダのステップ数を割り込みでカウントして算出します。
  • L. 218~221
    モータに入力するPWM信号のデューティを算出します。
    PWMの分解能は9ビット(0~511)としました(L. 111)。

    モジュールの姿勢角と角速度、モータの回転速度に関する係数 (Kp, Kd, Kw) に加えて
    傾きオフセット補正値 (D0) とモータ初速 (DutyIni) も絡めて算出しています。

  • L. 230~240
    算出されたデューティ値の正負からモータ回転方向を判定しています。
    またブレーキ範囲 (brakeRange) を設けてバランスが取れた姿勢角付近でモータが止まるようにします。

  • L. 157~201
    M5 ATOM MatrixにUSBケーブルをつなぎながらシリアル入力で各パラメータを手動で調整しました。

 

動作

以下のパラメータ値で取りあえず倒立が実現できました。

  • 姿勢角係数 Kp = 4.2
  • 姿勢角速度係数 Kd = 0.19
  • モータ回転速度係数 Kw = 0.1
  • 姿勢角オフセット補正 D0 = 0.31
  • ブレーキ範囲 brakeRange = 0
  • モータ初速デューティ DutyIni = 490

上記パラメータの妥当性や物理的理解はまだできておらず、実際外乱には弱い状況です。

しかし、パラメータの大体のオーダーを把握することができ、なにより実際に倒立することが確認できたことは非常に有意義であったと言えます。

 

おわりに

やっと立ちました!

もうこの道を驀進するしかないですね。

次回はM5 ATOM MatrixにUSBケーブルを除去して無線でパラメータ調整できるようにする予定です。

またパラメータを物理的意味のあるものに絞って外乱に強い倒立を目指したいです!!

Related Posts

コメントはこちらから

メールアドレスが公開されることはありません。コメントのみでもOKです。

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください