🤖

倒立振子ロボットを現代制御で動かしたい (2)直立制御

2023/01/04に公開

2件

robotics

tech

この記事は「倒立振子ロボットを現代制御で動かしたい」シリーズの一本です。

モデル化
直立制御（この記事）
遠隔操作

はじめに

前回は、ステッピングモータを使った倒立振子ロボットの運動方程式を導出し、状態空間モデルを求めました。
今回の記事では最適制御理論に基づいて制御則を求め、実際のハードウェアに実装します。

ハードウェア

早速最適制御に・・・といきたいところですが、制御対象となるロボットの詳細をまだ書いていなかったので、ここで説明していきます。

製作したハードウェアの写真。プリント基板のフレームに、Atom Matrix、16340リチウム電池、昇圧回路やステッピングモータドライバが搭載されている。車輪は2台のステッピングモータで駆動されている。

制御には以前と同様、ESP32とIMUを搭載したAtom Matrixを使っています。
ステッピングモータは秋月電子で1個350円のSPG27-1101というユニポーラ型モータです。
モータの駆動にはTB67S158NGというICを使っています。これはシフトレジスタと大電流ドライバを組み合わせたような構成で、3本の信号線でユニポーラステッピングモータを2台同時に駆動できます。
こう聞くと最高のドライバのように見えますが、マイクロステッピングには不向きで、その上ピンが1.78mmピッチのため普通の基板には実装できないという微妙なICです。
電源は16340型リチウムイオン電池ですが、モータドライバは最低10V必要なため、LT1172を使って3.7Vから10Vに昇圧しています。熱々になるためヒートシンクをつけていますがおそらく定格オーバーです。
ロボットのフレームはモータマウント部分も含めてほとんどプリント基板です。JLCPCBに発注しました。

最適制御（LQR）

前回の記事では、ロボットの状態変数を $\mathbf{x} = [\theta \ \dot{\theta} \ \dot{\phi}]^T$ （ $\theta$ はロボットの角度、 $\phi$ は車体に対する車輪の角度）、入力を $\mathbf{u} = [\ddot{\phi}]^T$ として、状態方程式 $\dot{\mathbf{x}} = A \mathbf{x} + B \mathbf{u}$ の係数を求めました。
求めた式に適当に見積もった質量や慣性モーメントなどを代入すると、

A = \begin{bmatrix} 0 & 1 & 0 \\ 22.6443895687399 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}, \ B = \begin{bmatrix} 0 \\ -0.774824387837912 \\ 1 \end{bmatrix}

という値が求まりました。

今回はまずロボットを直立させる、つまり $\mathbf{x}$ を $\mathbf{0}$ に近づけるため、最適制御理論の一つ、LQR (Linear-Quadratic Regulator)を使って制御器を求めます。

LQRは、システムに $\mathbf{u}=-K\mathbf{x}$ という入力を与えることを考え、

J = \int_0^\infty \left( \mathbf{x}^T Q \mathbf{x} + \mathbf{u}^T R \mathbf{u} \right) dt

という評価関数を最小化するようなゲイン $K$ を求める方法です。
この式は積分範囲が無限な上に微分方程式である状態方程式まで関わってくるので威圧的ですが、解法は十分に確立されており、簡単に解けるようなソフトウェアも存在します。

今回はpython-controlライブラリのlqr関数を使って計算しました。
重み行列 $Q$ と $R$ は自分で設定する必要がありますが、今回はシミュレーション結果を見つつ、 $\theta$ と $\dot{\phi}$ が0に収束するように試行錯誤して

Q = \begin{bmatrix} 100 & 0 & 0 \\ 0 & 50 & 0 \\ 0 & 0 & 1 \end{bmatrix}, \ R = \begin{bmatrix} 0.01 \end{bmatrix}

としました。

lqr関数を実行した結果、求まったゲインは

K = \begin{bmatrix} -280.74261026 & -89.66049857 & -10 \end{bmatrix}

でした。つまり、

\mathbf{u} = \ddot{\phi} = 280.74261026 \, \theta + 89.66049857 \, \dot{\theta} + 10 \, \dot{\phi}

という加速度でモータを駆動すればロボットを直立させられるはずです。

制御ソフトウェアの実装

プログラムには相変わらずRustを使用しました。
制御は普通に一定周期で上記の式を計算するだけなので、計算コストはPID制御とあまり変わりません。
制御周期は10ms (100Hz)にしました。

ただし今回は加速度を入力変数としたモデルになっているため、実際にステッピングモータを動かすにはまず加速度を積分して角速度（回転数）を求め、さらにそれに応じて一定間隔でコイルの通電を切り替える必要がありました。
単純に考えればタイマ割り込みで通電を切り替え、角速度に合わせて割り込み周期を変えればよい気がするのですが、今回は常に角速度が変化し続けるため、角速度を変えた瞬間に通電の切り替え周期が変化するような工夫をしました。
具体的には、タイマ割り込み自体は一定周期（1ms）で発生させ、その時の角速度に比例した数をカウンタに加算し、カウンタが一定値を超えたタイミングで通電を切り替えるというものです。
この方法は波形生成の手法であるDDS (Direct Digital Synthesizer)にインスパイアされたものです。

動作テスト

制御プログラムをロボットに書き込んで起動すると、次のような動作になりました。うまく直立できています。

細かく振動していますが、これはおそらくモータの角度を1.5度単位でしか変えられないためです。
マイクロステップ駆動も一度実装してみたのですが、あまり効果がなかったため結局単純な駆動方式（1-2相励磁）に戻してしまいました。

実は最初はうまく立たせられなかったのですが、試行錯誤しているうちにうまくいきました。
成功の決め手は重み行列（ $Q,R$ ）の設定と、「制御開始時に角速度の積分をリセットする」といったように実装をしっかりすることでした。

まとめ

今回は、最適制御理論の一つであるLQR制御に基づいて状態方程式から制御則を求め、実際の倒立振子ロボットに実装して直立できることを確認しました。
現代制御理論は正確なモデルが必要というイメージがあったため、大雑把なモデルでうまく制御できるか心配だったのですが、無事にうまく立たせることができました。

次回はラジコンのように自由に前後進や旋回の操作ができるように改良することを目指します。

Discussion

興味深い記事でした。
「具体的には、タイマ割り込み自体は一定周期（1ms）で発生させ、その時の角速度に比例した数をカウンタに加算し、カウンタが一定値を超えたタイミングで通電を切り替えるというものです。」
この記述について，もう少し詳しく教えていただけないでしょうか。

tana

たとえばステッピングモータが1パルスで30°回転すると仮定すれば、処理を擬似コードで表すと次のような流れになります。

// グローバル変数
float angle = 0;  // 現在の角度
float angular_velocity = 0.1;  // 角速度（1msあたりの角度変化）

// タイマ割り込み（1ms間隔で呼ばれる）
void onTimer() {
  angle += angularVelocity;
  if (angle > 30) {
    sendPulse();  // ステッピングモータにパルスを送る（コイルの通電を切り替える）
    angle -= 30;
  }
}

実際には少し変数の取り方が違ったりしますが、イメージとしてはこのように、状態として角度（に相当する値）を持っておき、それが一定以上増えたときにステッピングモータを1ステップ動かすという方法です。