姿勢制御入門(その1)

前に紹介した「カッコイイおもちゃ買ってみたら凄かった」では制御工学の力に感動しました。おもちゃを買った理由の一つに、Arduinoのソースコードが公開されている点をあげました。解説資料も付属しています。ここでは解説資料を和訳しつつ、補足説明を加えながら姿勢制御プログラムの背景にある技術を紹介していきます。カッコイイおもちゃに付属の英語のしかも専門的な解説を読むのがしんどい・おっくうだという方には、本文を先に読むことでハードルが下がるのではと思います。(解説書の原文やコードまるごとは私の著作ではないので掲載しておりません。Tumbllerを購入して確認くださいね。)

制御工学って何?
制御工学を習得している方はこの辺の説明は飛ばして、「Tumbllerの姿勢制御の原理」以降をお読みください。

棒(ほうき)を立てる遊びを思い出す
小学生の頃に、手のひらにほうきを立てて倒れないようにバランスを取ったことはあるでしょう。ほうきがまっすぐ立った状態から倒れかかろうとすると、その倒れ具合(角度)を見て、それがキャンセルされるように、素早く手の位置を変えていました。(棒が倒れる方向に手を差し出す)現状を把握し、手を動かす。これを繰り返す事でほうきの傾斜角度を一定(ゼロ度)に保とうとしていました。手の位置を動かす量はほうきの傾く角度に依りましたようね。反射神経が鈍ければ、手の位置を素早く移動させられずにほうきを落とし、仲間に「へっぼー、へぼいのー、おまえ」と嘲笑されたものです。あれは現況を操作量に反映する制御の例です。

制御が何の役に立つか
制御工学で扱う制御とは、温度を一定に保つとか姿勢を一定に保つとか、変動要因に負けずに制御対象を一定の状態に維持するために操作量を自動的に適量にするこを指します。
制御工学が何の役に立つかを言うと、ざっくり2つあります。
・不安定なものを安定にしたり、安定なものをもっと安定にする(性能を良くする)
・何が制御できて何が制御できないかを明らかにすることで、「(理論上無理があるので)時間やお金かけても出来ないことを示す」あるいは「(理論上制御可能なので)現時点の弱点を分析して改良案を提示できる」。

制御対象のモデリングと制御設計
制御対象の振る舞いは常微分方程式や伝達関数、状態方程式など数学を用いて表現され、その表現はモデリングと呼ばれます。そのモデルには外から制御可能な要素があり、そこをどう制御するか設計していくことになります。(制御系の設計)

フィードバック制御の式が示すこと
以下のようなフィードバックシステムがあるとします。
負帰還であっても正帰還であってもKの大きさが、制御対象システムの制御可能性、その性能にとても重要な影響を与えることが分かります。 上記の例では単純化のためにフィードバックゲインはx1としていますが、ほうきの例を思い浮かべていただくと、このフィードバックの仕方(量とかタイミングとか)が制御の安定性に重要だろうなということは想像がつくのではないでしょうか。

さて、Tumbblerの話に戻りましょう。 ここからは「ELEGOO TumbllerV1.1 Self-Balancing Car Tutorial」(ELEGOO社のネットで公開されています)にある解説資料や理解のためのサンプルコードを解説していきます。

Tumbllerの姿勢制御の原理
車両のバランスを取る核となる考え方は「動的安定性」です。動的安定性とは、方向転換動作や前進・行進などで生じる不安定な環境において、重心を移動させながら、重力などの外力に抗い、意図した状態となるよう姿勢を制御することです。
姿勢データはMPU6050によって計測されArduino NanoはI2Cプロトコルによって姿勢データを取り込み、制御アルゴリズムを使用してモーターの回転数を制御して車輪を動かし車両の姿勢のバランスとを取ります。 下図は車両が姿勢のバランスを取る原理を示したものです。
車体が車輪に対して垂直に立つと車体は静止します。
車体が左に傾くと、車輪は左に向かって加速し、左に向かう車体の速度よりも速くなると車体は車輪に対して垂直な状態に戻ります。
車体が右に傾くと、車輪は右に向かって加速し、右に向かう車体の速度よりも速くなると車体は車輪に対して垂直な状態に戻ります。

Tumbllerの姿勢制御のタスク
Tumbllerの姿勢制御のタスクは、大きく3つのタスクで成り立っています。
1.バランス制御:Tumbllerの前後の動きを制御し、直立のバランスを保つこと。
2.スピード制御:モーターの回転数を制御して、車輪のスピードを制御すること。
3.方向制御:2つのモーターの回転数の差を制御することで、車両の方向(Z軸に対し右回転・左回転)を制御すること。
これらの制御は、最終的には、モーターの回転制御のための出力電圧の制御量として重畳されることになりますが、直感的に分かるとおりバランス制御が最も重要です。車両モデルの直立制御(バランス制御)の観点からは他の2つの制御が干渉となります。従って、バランス制御周期に対して、スピード制御と方向制御の周期は遅くなるよう設計されます。
具体的には、バランス制御は5ms周期のタイマー割り込みで実行されるのに対し、スピード制御と方向制御は40ms周期のタイマー割り込みで実行されます。
Balanced.cppの Timer2::interrupt()内で実行タイミングが制御されています。Timer2は5ms毎に呼ばれ、
・車両速度の取得(Balanced.Get_EncoderSpeed())
・姿勢データの取得(Mpu6050.DataProcessing())
・バランス制御の実行(Balanced.PD_VerticalRing())
が行われます。Timer2が呼ばれたのが8回目ならスピード制御(Balanced.PI_SpeedRing)や方向制御(Balanced.PI_SteeringRing())も実行されます。

3つのタスクはPID制御により実現される
Tumbllerの姿勢制御の3つのタスクは、PID制御プロセスによって実現されています。
PID制御は、目標値と現在値の差の情報を使って制御対象への制御量(入力)を決める際に、誤差に対する比例(P)制御量と積分(I)制御量と微分(D)制御量を活用する方法で、制御の世界では最も有名な方法で広く活用されています。
目標値をr(t)、出力値(現在値)をy(t)、目標値と現在値の差をe(t)、制御対象への制御量(入力)をu(t)とすると、入力u(t)は次式のように表現できます。KPは比例制御のゲイン、KDは微分制御のゲイン、KIは積分制御のゲインです。

比例制御は、目標値と現在値の差が多いときは入力量を大きくし、誤差が小さいときは入力量を小さくします。誤差のある無しに従い単純にある量の入力をON/OFFするのでは、目標値に対して現在値は過ぎたり不足したりを行き来し(大きな振動)収束させることは困難ですが、この比例制御により目標値に近づけることが可能になります。しかし、それでも振動的挙動が残るため微分制御を活用します。微分制御は現在の速度が目標速度をブレーキをかけ、下回るときは加速するよう入力量を調整します。ブレーキまたは加速の大きさは速度誤差の大きさに比例させます。この制御によって振動的挙動を軽減することができます。積分制御は誤差を取り除くために役立ちます。外乱があるような場合、例えば車両の制御のケースでは風力が常に働いているような場合、PD制御だけである目標位置に制御させようとしても目標値から少しずれた位置でバランスされてしまいます。この外乱に抗うために制御開始から現在までの誤差蓄積量に比例した力を加えることで誤差を解消する役割を担います。

バランス制御
バランス制御は、Balanced.cpp内のPD_VerticalRing()で実行されます。

void Balanced::PD_VerticalRing()
{
  balance_control_output= kp_balance * (kalmanfilter.angle - 0) + kd_balance * (kalmanfilter.Gyro_x - 0);
}

コードを見ると、積分項がありません。PID制御では無くPD制御のみが記述されています。誤差に対して反応して後一押しの車輪加速が却ってバランスを崩してしまうのを回避するためにI制御は外しているようです。I制御があった場合の挙動を見るためにKIの値を小さくしておいて実装して試してみても良いかもしれません。

バランス制御の制御対象モデル
先ず、単純な振り子のモデルを考えます。
視点からまっすぐ下にぶら下がっている状態から角度θ上がっている状態を考えます。質量mの物体には真下にmgの力が働いています。物体は最下部方向に向かって動作しますが、接線のに沿って働く力は、θが増大する方向を正とすると、-mgSinθで表現できます。ここで、θが非常に小さい場合はSinθ≓θとすることができます。つまり、Ft=-mgSinθ≅ーmgθ です。

次に、Tumbllerを模した車両モデルをものすごく単純化して考えます。倒れかかろうとする(θが増大する)車体頂上には倒れる方向にmgSinθの力が働きます。これに抗うべく車輪に加速度aを加えて車体頂上にmgSinθの力の向きの逆方向にmaの力を加えバランスしようとしているイメージを考えます。これを制御するシステムは、負のフィードバック制御システムであり車輪の加速度aは傾斜角θに正比例し、そのゲインをK1とすると、車体頂上にかかる力は次式で表現できます。
F=mgSinθ-ma ここで、a=K1θのとき、F=mgSinθ-mK1θ≅mgθ-mK1θ
より早くθをゼロにして安定させるためには減衰力を大きくする必要があるため、傾斜の角速度も使ってaを変更し、Fの式は以下のように変更できます。

従って、本システムは傾斜角と傾斜角速度のデータが必要となります。これらのデータは上述のとおりMPU6050モジュールが計測を行いそのデータを受け取りますので、Arduino Nanoは、MPU6050モジュールとやり取りをする必要があります。

MPU6050モジュールを制御する
MPU6050モジュールを扱うにあたって、初期化処理を行います。その前にI2Cの初期化も行う必要があります。
Balanced.cpp内のMpu6050::init()です。
この中で、Wire.begin()とMPU6050.initialize()があって、これによりMPU6050に計測をさせてデータを受け取れるようになります。 ただし、MPU6050のセンサーの計測結果にはたくさんのノイズと累積誤差が含まれるためそのままでは使えません。カルマンフィルターのパラメータ設定を行う必要があります。下記ががそれに該当します。

Mpu6050::Mpu6050()
{
    dt = 0.005, Q_angle = 0.001, Q_gyro = 0.005, R_angle = 0.5, C_0 = 1, K1 = 0.05;
}

そしてカルマンフィルターを通して計測データを取得します。

void Mpu6050::DataProcessing()
{  
  MPU6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  // Data acquisition of MPU6050 gyroscope and accelerometer
  kalmanfilter.Angletest(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);
  // Obtaining Angle by Kalman Filter
}

スピード制御
計測には誤差があり、実際に測定した角度は常に車両モデルの角度とズレが常に存在します。そのため、車両モデルは実際には地面に対して垂直ではなく、重力の影響で傾斜角があります。車体モデルは地面に対して垂直ではなく、重力の影響で傾斜角があります。 そのため傾斜方向へ加速していきます。そこで、速度を安定させるために速度ループを導入する必要があります。
Tumbllerが静止し、安定して移動できるように、速度安定性を維持するための速度ループを導入する必要があります。
スピード制御は、Balanced.cpp内のPI_SpeedRing()で実行されます。
スピード制御もPID制御プロセスが使われています。(ただし、D制御は使われていません。)

void Balanced::PI_SpeedRing()
{
   double car_speed=(encoder_left_pulse_num_speed + encoder_right_pulse_num_speed) * 0.5;
   encoder_left_pulse_num_speed = 0;
   encoder_right_pulse_num_speed = 0;
   speed_filter = speed_filter_old * 0.7 + car_speed * 0.3;
   speed_filter_old = speed_filter;
   car_speed_integeral += speed_filter;
   car_speed_integeral += -setting_car_speed; 
   car_speed_integeral = constrain(car_speed_integeral, -3000, 3000);
   speed_control_output = -kp_speed * speed_filter - ki_speed * car_speed_integeral;
}

パラメータについて補足説明します。
KP * 速度:
速度制御ですから、当然、速度を比例パラメータとして掛け合わせ、速度を安定した状態に維持します。速度が安定した状態になるように比例ゲインとして掛けます。
Ki * 変位:
Kiは積分ゲインで速度積分である変位に対して掛けます。
積分値を制限して、速度の上限を決めます。
car_speed_integeral = constrain(car_speed_integeral, -3000, 3000);

方向制御
方向制御(ステアリングリング)の実現は、主にZ軸ジャイロのデータをP制御のための操舵速度偏差として取得することです。その目的は、ステアリングスピードを設定値どおりにすることです。
方向制御は、Balanced.cpp内のPI_SteeringRing()で実行されます。

void Balanced::PI_SteeringRing()
{  
   rotation_control_output = setting_turn_speed + kd_turn * kalmanfilter.Gyro_z;
}

最終的な制御出力(操作量)の決定
姿勢制御、スピード制御、方向制御の関数で決定した結果を足し合わせて、モーター制御のための出力電圧を決定します。
それは、Balanced.cpp内のTotal_Contorol()で実行されます。

void Balanced::Total_Control()
{
  pwm_left = balance_control_output - speed_control_output - rotation_control_output;
  pwm_right = balance_control_output - speed_control_output + rotation_control_output;
  pwm_left = constrain(pwm_left, -255, 255);
  pwm_right = constrain(pwm_right, -255, 255);
   while(EXCESSIVE_ANGLE_TILT || PICKED_UP)
  { 
    Mpu6050.DataProcessing();
    Motor.Stop();
  }
  
  (pwm_left < 0) ?  (Motor.Control(AIN1,1,PWMA_LEFT,-pwm_left)):
                    (Motor.Control(AIN1,0,PWMA_LEFT,pwm_left));
  
  (pwm_right < 0) ? (Motor.Control(BIN1,1,PWMB_RIGHT,-pwm_right)): 
                    (Motor.Control(BIN1,0,PWMB_RIGHT,pwm_right));
}

左車輪、右車輪に与える速度を計算します。(上下限処理も入れます。)
続いてWhile文がありますが、これは異常時の処理です。
速度情報から、PWM制御信号に変換してモーターへ信号を与えます。

いくつか制御のためのゲインが登場しましたが、これらはBalancedクラスを初期化するときに定数が与えられています。

Balanced::Balanced()
{
  kp_balance = 55, kd_balance = 0.75;
  kp_speed = 10, ki_speed = 0.26;
  kp_turn = 2.5, kd_turn = 0.5;
}

制御対象の出力である、角度と角速度を計測してフィードバックし、目的の角度との差から左右のモーター制御量を算出してモーターを駆動する。 それを繰り返す。そのような制御演算が割り込み処理により定期定期に実行されている、プログラムということがわかりました。

アップロードと動作確認
Tumbllerにインストールされているプログラムは、姿勢制御プログラムの他にBluetoothによりリモートコントロールするためのプログラムや目標物を追従するプログラム、障害を回避するプログラムなどが盛り込まれた巨大なプログラムになっています。ここで紹介したのはcとその解説書です。その姿勢制御プログラムをアップロードして動作確認してみましょう。
Arduino開発環境にて、すでにアップロードされいてる実行ファイルをバックアップし、上記実験後リストアすれば良いのですが、手元にArduino Nanoがいくつかあるので、そちらに書き込んで基板を交換して試したいと思います。
デモ用の姿勢制御プログラムは、前進して後進して右ターン、左ターンしてストップを延々繰り返します。 割り込み処理でバランスを取りながらです。
Tumbllerにインストールされているプログラムは障害物を回避できますが、上記の姿勢制御プルグラムだけの場合、狭い部屋で動かすと障害物にぶつかってしまいます。そのため前進と後進はしないようにして狭い範囲でウイウニ動くようにしてアップロードしました。


結果

さて、なんとなく概要はつかめたかと思いますが、どんな制御システムになったのか、各種ゲインはどうやって決定したのか設計計算がありませんよね。 闇雲に各種ゲインを設定するとは思えません。いったいいくつにすれば良いのかある程度見通しを得てから実験で微調整するというこうとは想像付くかと思います。 最初にP制御ゲインを決めて他のゲインを決めて、実験してチューニングしていくのでしょうね。決め方にも理論があります。今後はこのあたりを探って行きたいと思います。