3軸姿勢制御モジュールの自作です
概要
2号機の点倒立がようやく成功しました。
1辺倒立（1軸制御）からプログラムのみを修正して点倒立に拡張。よくよく考えると、点倒立は2軸制御（xy軸）で実現できるんですよね。実際、初号機も2軸制御で点倒立をしてました。なので今回は回転制御（z軸）も加えて、その名の通り3軸姿勢制御（xyz）を行ってます。点倒立（xy軸）を3つのモーターで制御するから3軸制御でいいのかな？
順に制御方法を解説していきます。
1辺倒立（1軸制御）
一辺で倒立する場合は、1軸（x軸）の傾き制御で倒立させます。リアクションホイール（モーター）の制御は前面のモーター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軸の姿勢制御）
2軸制御としてますが、モーターは3つ使うので3軸制御？。考え方は、1軸を２軸に拡張するだけです。
筐体を水平に保つためにX軸とY軸の回転角度をそれぞれ0°（水平）に保つようにモーターを制御します。
リアクションホイールの配置によっては以下のようにモーター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軸）についてです。
回転制御（xyz軸の３軸姿勢制御）
最後にz軸廻りの回転制御。
倒立制御（xy平面）に加えて、z軸廻りの回転制御を組込みます。単純にz軸廻りの角速度について、P制御（比例制御）でモーター回転数にフィードバック。
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軸姿勢制御についての検討はいったん一区切り。起き上がり動作の検討をしていないのが少し心残りですが、改造が大きいのでちょっと様子見。途中、中だるみして進捗が無くなりそうでしたがここまでやれてよかったです。2号機の内容もそのうちレシピを纏めて頒布する予定です。
コメント