3軸姿勢制御モジュールの姿勢制御方法
3軸姿勢制御モジュールの自作です
目次- 概要
- 1辺倒立（1軸制御）
- ソース抜粋
- 点倒立（2軸制御）
- ソース
- 回転制御（3軸制御）
- レシピ頒布
概要
2号機の点倒立がようやく成功しました。
1辺倒立（1軸制御）からプログラムのみを修正して点倒立に拡張。よくよく考えると、点倒立は2軸制御（xy軸）で実現できるんですよね。実際、初号機も2軸制御で点倒立をしてました。なので今回は回転制御（z軸）も加えて、その名の通り3軸姿勢制御（xyz）を行ってます。点倒立（xy軸）を3つのモーターで制御するから3軸制御でいいのかな？
順に制御方法を解説していきます。
1辺倒立（1軸制御）
一辺で倒立する場合は、1軸の傾き制御で倒立させます。リアクションホイールの制御も前面のモーター1つのみ。リアクションホールの動きですが
倒立振子でイメージしてます。倒れる方向と同じ方向にモーターを回転、発生する反トルクで姿勢を起こします。傾き、倒れる速度などに応じて回転速度/方向を操作して倒立を維持します。
制御式/方法は検索するとちらほら参考となる情報が見つかります。だいたいが以下ような感じでモーターの操作量を決めているようでした。
\begin{align} &u = k_1・\theta + k_2・\dot\theta + k_3・\omega\\\\ &\theta = 筐体角度 \hspace{12pt} \omega=モーター回転速度 \hspace{12pt} k=係数 \end{align}当初はこれに基づいて検討してたのですが、なぜかモーター回転数をうまくフィードバックに機能させることができずに断念。なので単純に角度（角速度）PID制御でモーター回転数を制御してます。
ソース抜粋PID制御部分、モーター速度を決めている所のソース抜粋です。
targetAngle[x] += period * 1e-6 * Kt * (targetAngle[x] - angle[x]);targetOmega[x] = Ka * (targetAngle[x] - angle[x]);P[x] = targetOmega[x] - gyro[x];I[x] = angle[x];D[x] = (gyro[x] - preP[x]) / (period * 1e-6);preP[x] = gyro[x];u[x] = Kp * P[x] + Ki * I[x] - Kd * D[x];x軸周りの角度（角速度）についてPID制御を行います。最初に目標となる角度を都度補正します。目標角度に応じて目標角速度を設定。あとは角速度のPIDです。積分項については角速度の積分なので実質角度ということで計測される角度をそのまま使用。微分項も直接計測値を使用してます。で最後に操作量を算出。
この計算を100回/秒（10ms）で更新します。演算の余力はまだまだあるのですが一度4msで回したところ、何故かうまく調整できずに10msに戻しました。
ここで計算した操作量を、そのままPWMデューティー比としてモーター回転数を制御します。あとはひたすら係数（ゲイン）を現物合わせで調整。
係数（ゲイン）のストライクゾーンは割と広く、ただ倒立するだけなら雑にゲイン調整しても割とOKでした。ただ動画のように安定倒立となると調整はだいぶ苦労してます。次に点倒立についてです。
スポンサーリンク点倒立（2軸制御）
CUBE頂点での倒立動作です。2軸制御としてますが、モーターは3つ使うので3軸制御？。考え方は、1軸を２軸に拡張するだけです。
筐体を水平に保つためにX軸とY軸の回転角度をそれぞれで水平角に保つように姿勢制御します。
リアクションホイールの配置によっては以下のようにモーター2つで点倒立も可能。ここの動画チャンネルですが、非常に参考にさせてもらってます。（音注意）
水平軸とホイール軸を合わせて配置して2個のホイールで水平制御しているようです。
キューブ型でも左のようにモーターを配置すれば2個のホイールで点倒立は可能だと思います。ただ今回は右のようにホイール３個を配置してます。
点倒立中を真上から見てます。モーター①でx軸廻り。モーター②③については軸に対して斜めに配置されているため、xy軸それぞれに影響します。
ソース制御部分のコード抜粋です。
motor1_controller(u[x]); motor2_controller(0.866 * u[y] + 0.5 * u[x]); motor3_controller(0.866 * u[y] - 0.5 * u[x]);x軸廻り同様に、y軸廻りについても角度PIDで操作量u[y]を算出します。モーター①はx軸廻りの操作量でそのまま制御。モーター②、③についてはそれぞれの操作量を単純に三角比で分配して回転制御してます。
係数（ゲイン）は再度調整してます。センサーキャリブレーションも結構シビアに調整。ソース見直していることもありますが、初号機の時よりだいぶ安定してます。続いて回転制御（z軸）についてです。
スポンサーリンク回転制御（3軸制御）
最後にz軸廻りの回転制御。
倒立制御（xy平面）に加えて、z軸廻りの回転制御を組込みます。単純にz軸廻りの角速度について、比例制御でモーター回転数にフィードバックします。
P[z] = targetOmega[z] - gyro[z]; u[z] = Kz * P[z]; motor1_controller(u[x] + u[z]); motor2_controller(0.866 * u[y] + 0.5 * u[x] - u[z]); motor3_controller(0.866 * u[y] - 0.5 * u[x] + u[z]);目標となる角速度はパソコンから無線送信するようにしてます。あとは単純に角速度の比例制御で操作量を算出。各モーターの操作量をxyz軸でミキシングしてます。
回転方向はパソコンから無線で指示。これでxyzの３軸姿勢制御の完了です。
3軸姿勢制御についての検討はいったん一区切り。起き上がり動作の検討をしていないのが少し心残りですが、改造が大きいのでちょっと様子見。途中、中だるみして進捗が無くなりそうでしたがここまでやれてよかったです。
▼参考サイト▼[電子工作]リアクションホイールへの道-HomeMadeGarbageremcr-GitHubBalancing cube-willem-pennings GitHub
▼製作記録▼リアクションホイールで3軸姿勢制御モジュールの自作｜製作記録2号機がようやく完成。 1号機に比べてひと廻り小型化（□150mm⇒□136mm）。制御プログラムも見直してずいぶん安定した気がします。点倒立（平面xy）に、回転制御（z軸）も加えて、３軸姿勢制御の完成です。２号機もレシピも頒布開始しました...garchiving.com2024.01.05レシピ頒布
3Dモデル、回路図（ガーバーデータ）、ソースコードを頒布してます。
CUBE型3軸姿勢制御モジュールの自作｜imoリアクションホイールを使用した3軸姿勢制御ロボットの自作です。 2号機のデータもアップロードしました。1号機からの大きな変化点はマイコンボードがESP32⇒Pico、筐体サイズが150mm角⇒136mm角、ソースコードも見直しているので安...note.com CUBE型3軸姿勢制御モジュール - imo Lab. - BOOTH【商品説明】 リアクションホイールを使用した3軸姿勢制御ロボットです。 ・3Dプリンタ造形用の3Dモデル（.stl） ・回路図＆基板ガーバーデータ ・ソースコード（ESP32/Pico（Arduino）、Processing） がダウンロー...garchiving.booth.pm