倒立制御解説（物理バランス方式基礎）

基本的な考え方

ロボットの車軸周りに発生する角速度や力を車輪の速度と加速度を使って打ち消します。
これで状態が安定するので後は運動エネルギー等で移動を制限します。

プログラム構造は
メイン関数{
　初期設定
　定期実行{
　　センシング処理
　　状態変数計算
　　倒立モード判定
　　if(倒立モード){
　　　倒立出力の計算
　　}else{
　　　水平時処理
　　}
　　モーターへの出力
　　通信など
    }
}
となります

---

変数の宣言

前提としてセンシング準備ができており、
プログラムがsample_time[ms]またはsample_sime_s [s]の固定周期で実行できているものとします。

変数を用意します。
基本的にfloat型を使う事とします。

下準備
PIが無いときはPIを宣言
#ifndef PI
#define PI 3.1415f
#endif

ついでに換算定義
radをdegに換算する定義
#define rad2deg (180.0f/PI)

degをradに換算する定義
#define deg2rad (PI/180.0f)

重力加速度
#define gravity 9.086f

角度関連

現在の角度宣言
    float ang_rad= 0; //水平 0、直立時 π/2 ぐらい 単位[rad]　センシング回参照
定期実行で取得(計算)する

直立時角度宣言
    float ang_ref= 89.5f/180.0f*PI; //前回測定およそ π/2 単位[rad]　パラメータ回参照
基本的には固定値

直立時角度オフセット
    float ang_ofs= 0; //直立角度微調整用 単位[rad]
走り続ける場合に補正する

直立時角度と現在の角度の差を求めます
宣言
    float ang_sub = 0; //直立時 0、前転でプラス 単位[rad/s]
定期実行時
    ang_sub = ang_ref + ang_ofs - ang_rad;

角速度関連
現在の角速度宣言
    float gyr_rad= 0; //水平 0、直立時 π/2 ぐらい 単位[rad/s]　センシング回参照
定期実行で取得する

静止時角速度
    float gyr_ref= 0; //静止時は0 単位[rad/s]
基本的には固定値

静止時角速度と現在の角速度の差を求めます
宣言
    float gyr_sub = 0; //静止時 0、前転でプラス 単位[rad/s]
定期実行時
    gyr_sub = gyr_ref - gyr_rad;

速度関連
現在の速度宣言
    float spd_now =0; //前進でプラス 単位[m/s]
定期実行で取得する

目標速度宣言
    float spd_ref =0; //前進でプラス 単位[m/s]
静止時は0、今後走行時に指定する

目標速度と現在の速度の差を求めます
宣言
    float spd_sub = 0; //静止時 0、速度超過でマイナス 単位[m/s]
定期実行時
    spd_sub = spd_ref - spd_now;

出力用変数
出力レート
    float out_rate=0; //±1 センシング回参照
出力pwm
    int out_pwm=0; //±1024とか センシング回参照
AB出力pwm
    int out_A=0; //±1024とか センシング回参照
    int out_B=0; //±1024とか センシング回参照

制御用変数
現状速度
    float spd_base = 0; //前進でプラス 単位[m/s]
バランス用速度
    float spd_counter = 0; //前進でプラス 単位[m/s]
バランス用加速度
    float acc_counter = 0; //前進でプラス 単位[m/s^2]

その他パラメータ
車軸-頂点距離
    float len_top=0.15f; //タイヤから頂点まで 単位[m]
車軸-重心距離
    float len_gra=0.055f; //タイヤから重心まで 単位[m]
タイヤ半径
    float radi_tire=0.027f; //タイヤ半径 単位[m]

最大速度
    float spd_max=0.42f; //最大速度 単位[m/s]パラメータ回参照
出力不感帯
    float out_min=0.08f; //出力不感帯 パラメータ回参照
出力最大値
    float out_max=1; //出力最大値
最大加速度
    float acc_max=2.4f; //最大加速度 単位[m/s^2]パラメータ回参照

目標速度から出力を算出
float spd2rate(float spd){
     return spd/spd_max;
}

目標加速度から出力を算出
float acc2rate(float acc){
     return acc/acc_max;
}

不感帯を抜ける
float revision_rate(float rate){
    if(abs(rate)<0.001) return 0;
    return rate*(out_max-out_min)+sign(rate)*out_min;
}

とりあえず倒立制御に最低限必要な変数はこれぐらい
慣性モーメントは今回放置

---

制御モード

常に制御が働いていると寝かせた状態でも動き続けるので、
直立付近で倒立制御を開始し、ある程度倒れたら諦めて倒立制御を停止します。

モード管理用変数
    int inverted_f = 0;

頂点付近で制御を開始します
以下、定期実行する内容

if(abs(ang_sub)*rad2deg<1){    //倒立制御開始
    inverted_f =1;
}else if(abs(ang_sub)*rad2deg>45){    //倒立制御停止
    inverted_f =0;
}

if(inverted_f ==1){
    //ここに倒立制御の内容を書く
}else{
    //ここに水平時の内容を書く
    out_rate=0;
}

---

倒立制御の基本原理

※2023/02/16追記

倒立制御の根本的な話
倒立動作を行う際は少なくとも２つのパターンが存在します。

第１の動作：支点の移動力による倒立（台車の位置、速度、加速度による倒立）
手の上で傘を立てる際に手の位置を動かしてバランスをとるのはこちらです。
台車等の移動による倒立制御では基本的にはこの作用を使用して制御します。
物理バランス方式の基礎もこちらに相当します。

第２の動作：支点の回転力による倒立（軸のモーメントによる倒立）
回転軸にモーメントを加えて倒立させる方式はこの作用で制御します。
車軸が直接台車を支えている場合は副次的な作用として発生しています。
ほぼ垂直な状態での倒立動作に影響し、タイヤが大きい方が立ちやすいのはこの効果によるものです。

一端、第２の動作の事は忘れて、第１の動作だけで立たせることにします。

---

角速度を打ち消す

やっと本題の制御理論の内容です。
何がどれぐらい効果があるのか確認するため、ひとつづつ要素を増やしていきます。

幾何学の時間です。
ロボットが転倒するとき、先端には
　距離×角速度
の速度が発生します。

これと同じ速度でロボットが移動すれば状態が安定します。
より正確には
 
    spd_counter=len_top*gyr_sub /cos(ang_sub);
となります。
一旦これだけで動かしてみます。前転時に前進で打ち消すので
    out_rate=spd2rate(spd_counter);

まだ、これだけでは立ちません。

現状速度維持（たぶん最重要）

先ほどのは静止状態の話で、ロボットが移動している場合を考えてみます。
ロボットが前進しながら前転するとき、現在の速度に加えて角速度を打ち消す速度が必要です。
とりあえず現在の速度をベース速度とし
    spd_base=spd_now;
これを出力に加えます。
    out_rate=spd2rate(spd_counter+spd_base);
更に不感帯を考慮する場合は
    out_rate=revision_rate(spd2rate(spd_counter+spd_base)); //速度カウンター

さて、では動かしてみます
流石に立ちませんが、かなり長い距離立って走ると思います。
直接パラメータを調整されている方で出力を積算しているのはここに相当すると思います。

---

モーメントを打ち消す

力学の時間です。
速度を打ち消しても傾斜した状態だと自重により転倒しようとするモーメントが発生しているので倒れてしまいます。
そこで、反モーメントを発生させて転倒モーメントを打ち消します。

まず転倒モーメントは
　転倒モーメント=回転方向力×重心までの距離
　　　　　　　　=質量×重力加速度×重心までの距離×sin(角度)

これに対して車両移動で発生するモーメントは
　反モーメント=車体移動力×重心までの距離×cos(角度)
モーメントのつり合いを考えると
　質量×重力加速度×重心までの距離×sin(角度)=車体移動力×重心までの距離×cos(角度)
これを整理して
　車体移動力=質量×重力加速度×sin(角度)/cos(角度)
ちょっと整理して
　車体移動力=質量×重力加速度×tan(角度)
これで車両に加える力がわかりました。

で

どうやって力を発生させるのか

これが問題です。

いよいよ物理学の時間です。
ニュートンの運動方程式
　力=質量×加速度　※F=m*a
があります。
力を発生させるには加速度を発生させればいいのです。

ということで
　加速度 = 力/質量 = 車体移動力/質量
　　　　= 質量×重力加速度×tan(角度)/質量
　　　　= 重力加速度×tan(角度)
    acc_counter = gravity * tan(ang_sub);

では加速度はどうやって発生させるのか。

停止状態から電圧をかけると以下のグラフになりました。

電圧を変えると分かりますが、
停止状態であれば
　加速度=(最大加速度/最大出力)*出力
となります。

ある出力で動いている場合
　加速度=(最大加速度/最大出力)*出力の上乗せ分
となります。

従って望みの加速度を出すには
　出力=現在の出力+最大出力/最大加速度*加速度
と求まります。
ここで現在の出力とは定常速度のことです。
ということで
    out_rate=revision_rate(spd2rate(spd_counter+spd_base)); //速度カウンター
    out_rate+=acc2rate(acc_counter); //モーメントカウンター
さて、では動かしてみます

立ったままずっと走ってます。

何だ簡単じゃないか

慣性モーメント出てきてないし

最終式に質量も無いし

倒立ミニ四駆(高速)がこの状態です。


次回予告、真面目に慣性モーメントを使います。
次回：構造の差について