Arduinoで３軸姿勢制御モジュールの自作です。だいぶ間が空いてしまいましたがようやく点倒立での制御が実現できました。今回はここまでの検討内容を紹介です。
▼これまでの製作記録はこちら▼
概要
前回までで、一軸（一辺）での倒立振子が完成しました。その後、いったん２軸（２モータ）での倒立振子を検討をしてます。
▼2軸制御用の筐体▼
2軸（2個のモータ）でカカシのような水平倒立を検討。ただ無理やり筐体を改造したのが原因？？なのか安定倒立が中々できず、、。動画撮っておけばよかったのですが、何とか倒立はするもののモータ回転で筐体が共振？するような激しい振動が発生。なので2軸はあきらめて予定の3軸制御（点倒立）の検討に入ることに、、、余計な寄り道をしたため、ずいぶん時間をとられました。。
3軸制御（点倒立）トライ1
とりあえずで制御プログラムを組んで試してみました。
▼点倒立中の真上から見たところ▼
単純にROLL軸廻りをMotorCTRで、PITCH軸廻りをMotorLH/RHで制御する考え。それぞれ軸廻りのPID制御で各モーターの回転制御を行います。（2軸倒立を検討していた時のソースをほぼそのまま流用してます）
トライ１結果
これでもかなりゲイン調整した結果でいい感じの時の動画です。持って数秒、、。今までの傾向から制御モデル（プログラム）がある程度嵌まっているとゲインのストライクゾーンが割と広い感じで、雑に調整してもある程度制御ができる感じ。そこまで行き着かないのでプログラムから見直します。
プログラム見直し
PID制御を角度⇒角速度に変更。
TargetOmega = Kp_angle * (TargetAngle - angle);
P = TargetOmega - (gyroRawLPF.B16.X * 0.0152587890625);
I += P * 0.01;
D = (gyroRawLPF.B16.X * 0.0152587890625) - preP;
preP = (gyroRawLPF.B16.X * 0.0152587890625);
PID = Kp * P + Ki * I - Kd * D;
1軸制御のときに角度より角速度制御の方が調整しやすかったので同じように変更。現在角度から目標角速度を逐次更新。微分項は実角速度を直接入力。微分先行にしてますがこれの効果はよくわかりません。
TargetAngle += 0.01 * Ka * (TargetAngle - angle);
倒立状態（目標角度）は傾き角度0°となるのですが、筐体バランスやセンサー精度など、、実際のセンサー値の０°が現実のバランスポイントと一致しているとは限らないので目標角度を逐次補正する制御を加えてます。これが安定倒立にかなり効いてる気がします。
PID値をミキシング
先ほどはMotorRH/LHはPITCH廻りの制御だけを考えていたのですが、モーターが上図のように軸に対して斜めに配置されているため、ホイールの反トルクがROLL/PITCH両方へ影響。
motorControl_CTR = PID[ROLL]; motorControl_RH = 0.866 * PID[PITCH] + 0.5 * PID[ROLL]; //cos(30)=0.866 cos(60)=0.5 motorControl_LH = 0.866 * PID[PITCH] - 0.5 * PID[ROLL];
それぞれ軸廻りのPID制御値をミキシングしてモーター制御するようにします。単純に三角比で分配。
ほか、ちょこちょこプログラムを修正して再トライしてみました。
点倒立に再トライ
ゲインは現物合わせで調整。ようやく点倒立が実現できました。結局今回もフィードバックには角度情報（角速度）しか使えてません。モータ回転数や電流値など、他にもセンサーから取得している情報があるのですが活用しきれてません。
起き上がり動作やスタンドアローン化、もう少し小型にしてみたいとかほかにもやりたいことがあるのでもうしばらくは3軸姿勢制御の検討を続けていくつもりです。
▼制作記録▼
コメント