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

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

点倒立調整

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

【SHISEIGYO-3】
難しい。
今日はここまで#リアクションホイールへの道 #M5stack #姿勢制御モジュール pic.twitter.com/XdlhcOaWXO

— HomeMadeGarbage (@H0meMadeGarbage) November 12, 2020

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

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

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

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

座標変換

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

これまでは３つのモータそれぞれの回転軸上にマイコンに内蔵された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