3軸 姿勢制御モジュール SHISEIGYO-3 マイコン削減 ーリアクションホイールへの道19ー

Home > 電子工作 > 3軸 姿勢制御モジュール SHISEIGYO-3 マイコン削減 ーリアクションホイールへの道19ー
すき 1
うんこ 0

前回はSHISEIGYO-3の筐体を構築いたしました。
引き続き点倒立を目指して課題を解決していきます。

3軸 姿勢制御モジュール SHISEIGYO-3 筐体検討 ーリアクションホイールへの道18ー

 

 

点倒立 調整

安定した点倒立を目指してパラメータ調整を進めてまいりました。

なかなか難しくどうしても倒立後に回転してしまいます。

点倒立時に各ホイールの回転がおさまれば良いのですが、傾きの補正や他のホイールの回転による影響で回転が止まることはありませんでした。
そこで倒立時の他のホイールの回転数もフィードバックしながら倒立制御する必要があるのではないかという結論に至りました。

現状ではモータごとにマイコン(M5Stack Core2ひとつとATOM Matrix 2個) を接続して、全く独立に制御しております。
各モータの回転数を1元で管理するために、ここではM5Stack Core2ひとつでモータ3つの制御を目指します。

マイコンひとつでの制御出来れば各モータの状況を即座に把握しフィードバックも容易にできます。
そして何より経済的です。
 

座標変換

SHISEIGYOはモータをモジュールの傾きと角速度とモータ回転速度でフィードバックかけて制御し倒立動作します。

これまでは3つのモータそれぞれの回転軸上にマイコンに内蔵されたIMUが配置されておりましたので、モータ軸の傾きや角速度はIMUの測定値を使用して制御することができました。

ここではM5Stack Core2のみで3つのモータを制御します。

真中のモータはM5Stack Core2のIMUのY軸の角度と角速度がそのまま使用できます。

左右のモータの回転軸はM5Stack Core2のIMUのY軸に対して±45°回転させたX軸上に配置されていますので、
傾きや角速度はM5Stack Core2のIMUの測定値を座標変換して使用する必要があります。

傾きの算出

傾きはIMUセンサの加速度から算出します。
IMUの加速度を$(a_x, a_y, a_z)$とするとY軸に対してθ回転させた座標の加速度$(a’_x, a’_y, a’_z)$は以下で算出されます。

$$
\begin{pmatrix}
a’_x \\
a’_y \\
a’_z
\end{pmatrix}  =
\begin{pmatrix}
\cos θ & 0& \sin θ \\
0 & 1& 0 \\
-\sin θ & 0& \cos θ
\end{pmatrix}
\begin{pmatrix}
a_x \\
a_y \\
a_z
\end{pmatrix}$$

以上より傾き$φ’$は

$$φ’ = \arctan \displaystyle \frac{a’_y}{a’_z} = \displaystyle \frac{a_y}{-a_x \sin θ + a_z \cos θ}$$

となり左右のモータの軸上の傾きは θ = ±45° としてIMUの加速度から算出できます。

角速度の算出

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}$$

以上より座標変換後のX’軸の角速度$ω’_x$は

$$ω’_x = ω_x + (ω_y \sin φ + ω_z \cos φ) \tan θ$$

となり左右のモータの軸上の角速度は$φ$ = 0°、θ = ±45° としてそれぞれ

$$ω’_x = ω_x + (ω_y \sin 0° + ω_z \cos 0°) \tan 45° = ω_x + ω_z$$

$$ω”_x = ω_x + (ω_y \sin 0° + ω_z \cos 0°) \tan (-45°) = ω_x – ω_z$$

で得られます。

動作

試しに上記変換で得られた傾き、角速度にカルマン・フィルタをかけてM5Stack Core2で左モータを制御して辺倒立させてみました。

問題なく倒立動作が実現できました。

参考

  • 実践 ロボット制御: 基礎から動力学まで

     

  • Interface 2020年 03 月号

     
  • 6軸センサの状態方程式

 

構成

座標変換によってIMUひとつで左右のモータの傾き、角速度が導出できるようになりましたので、
M5Stack Core2 にブラシレスモータを接続するべくピン接続を検討します。

SHISEIGYO-3に使用したブラシレスモータID-529XWの配線は以下の通りです。

モータ用電源 24V
GND
ブレーキ LOWで回転停止
PWM入力 20kHz、ON Duty 0%で最高速度、100%で停止
回転制御 オープンで時計回り、プルダウンで反時計回り
ロジック用電源 5V
エンコーダ出力 B相
エンコーダ出力 A相

これを3個接続ですのでいくらM5Stack Core2といえどもピンが足りません。
そこで M5Stack用 I/O拡張ユニットを使用してみました。

I2C制御で8個のIOを制御することができます。4ピンコネクタでM5Stack Core2 (GPIO32, 33) と接続しました。
こちらでブラシレスモータのブレーキ線と回転制御線をつないで制御します。

ピン接続は以下の通りです。ギリギリで何とか接続できました。

  ブレーキ PWM 回転 エンコーダB エンコーダA
左モータ EXT 2 GPIO25 EXT 3 GPIO13 GPIO14
中央モータ EXT 4 GPIO26 EXT 5 GPIO36 GPIO35
右モータ EXT 6 GPIO27 EXT 7 GPIO19 GPIO34

 
GPIO34はマイクの入力が接続されているので泣く泣くマイクとIMUの載ったボードの34ピンを切り取りました。

 

M5Stack用BUSモジュール を用いて配線します。

 

M5Stack Core2のみになってスッキリしました。

参考

 

動作

M5Stack Core2でモータ3つを制御して点倒立動作を確認することができました。
しかしやはりまだ倒立後回転し長時間の点倒立はできておりません。

 

作業中誤ってM5Stack Core2に過電圧を印可してしまい壊してしまい
動画を撮ることができませんでした。。。 😥 

購入後、動画撮影実施します。

おわりに

ここではM5Stack Core2 でモータ3つの制御を実現し点倒立動作を確認しました。

以上より倒立時の各モータの回転速度を一元管理でき安定倒立が目指せそうです。

また M5Stack用 I/O拡張ユニットを使用したことにより若干制御ループ時間が伸びてしまいました。出力切り替えに時間がかかっているようです。
以下のライブラリを使用しているのですが、高速化の余地があるのか検討したいです。

それでは次の道で!

次の記事

3軸 姿勢制御モジュール SHISEIGYO-3 物理ブレーキ検討 ーリアクションホイールへの道20ー

コメントはこちらから

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

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