HomeMadeGarbage Loading

3軸 姿勢制御モジュール SHISEIGYO-3 ver 1.0 完成 ーリアクションホイールへの道29ー

ホーム
電子工作
Raspberry Pi
IOT
音楽

前回は搭載モータをすべて同一のモノにして安定点倒立動作を確認しました。

3軸 姿勢制御モジュール SHISEIGYO-3 点倒立 安定性検討 ーリアクションホイールへの道28ー

ここではSHISEIGYO-3がひとまず完成いたしましたので報告させていただきます。

 

 

安定点倒立

安定点倒立のために実施したことを記載します。

点倒立時にリアクションホイールが回ってしまうと以下のようにモジュール自体が回転してしまいます。

 
そこで、各ホイールは倒立時に極力停止できるように以前開発した IDRS (Inverted angle Dynamic Readjustment System:倒立角動的再調整システム )を採用しました。

それでも、完全に点倒立後の回転を止めることができなかったので、真ん中のモータには更にモジュール自体の回転をフィードバックして回転を止めるようにしました。

モジュール自体の回転角速度は以下のようにIMUセンサ (MPU6050)の角速度を重力方向に座標変換して得ました。

IMUの角速度を$(ω_x, ω_y, ω_z)$とするとX軸に対して$φ$、Y軸に対してθ回傾いた座標の角速度$(ω’_x, ω’_y, ω’_z)$は以下で算出されます。

$$
\begin{pmatrix}
ω’_x \\
ω’_y \\
ω’_z
\end{pmatrix}  =
\begin{pmatrix}
1 & \sin φ \tan θ & \cos φ \tan θ \\
0 & \cos φ& -sin φ \\
0 & \sin φ \sec θ& \cos φ \sec θ
\end{pmatrix}
\begin{pmatrix}
ω_x \\
ω_y \\
ω_z
\end{pmatrix}$$

以上より重力方向の周りの角速度$ω’_z$は

$$ω’_z = ω_y \sin φ \secθ + ω_z \cosφ \secθ$$

で算出されます。これを真ん中のモータ制御にフィードバックしました。

 

モータは以下の式に基づいてトルクつまりモータの回転数を決定しています。

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

IDRSは上式の第1項にフィードバックさせ、$ω’_z$は第2項にフィードバックさせました。

 

起き上がり調整

安定点倒立が確立できましたので、起き上がり点倒立のために調整実施しました。

以下を調整しました。

  1. 起き上がり直前のモータ最大回転速度
  2. バンドブレーキを引くサーボの角度
  3. モータ停止からモータ倒立制御開始までの遅延時間

SHISEIGYO-3はIMUセンサを1つしか搭載していないので起き上がり時の遠心力の影響をもろにうけるので、3.で起き上がり中は制御を停止して遠心力低下してから倒立動作に入るようにしています。

真中のモータでまず辺倒立させて、両サイドのモータを同じ速度で起き上がり方向に回転させて同じタイミングと力でブレーキをかけて起き上げて点倒立させます。

 

モジュール回転制御

点倒立後にモジュールを回転させたり所望の角度に向きを変えたりしたいと思い回転制御の検討しました。

まずは回転の仕組みを知るべくSHISEIGYO-3を吊るすことにしました。

 

吊るし実験によってホイールの回転方向でモジュール自体の回転を制御できることがわかりました。

 

回転のメカニズムは何となく理解できたのですが、点倒立との両立方法が思いつかず。。
単純なフィードバック制御では回転制御は難しいのかもしれません。

ということで今回は断念して引き続き勉強します。

 

おわりに

ここで SHISEIGYO-3 の完成を宣言させていただきます。
回転制御ができていないのでver 1.0とします。

SHISEIGYO-1と同様に、近いうちにレシピとしてまとめて出版したいと思います。
回転制御も皆さんに作ってもらううちに誰かが実現してくれるかもしれませんし(他力本願)。

それではレシピの執筆にとりかかりたいと思います。

Related Posts

「3軸 姿勢制御モジュール SHISEIGYO-3 ver 1.0 完成 ーリアクションホイールへの道29ー」への1件のフィードバック

コメントはこちらから

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

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