倒立振子ロボットを現代制御で動かしたい (3)遠隔操縦

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

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

はじめに

前回の記事では、最適制御理論の一つであるLQR (Linear Quadratic Regulator)を使って倒立振子ロボットを直立させる制御則を構築し、実際にロボットを立たせることができました。
しかしこれではただその場で立っているだけですのであまり面白くありません。
今回はこのロボットをラジコンのように遠隔操縦できるようにすることを目指します。

回路図やプログラム、計算に使用したJupyter NotebookなどはGitHubで公開しています。

前後進の制御

下記の記事・動画を参考にしました。
本節前半（偏差を使った状態方程式を導出する部分）はこれらの参考文献ですでに解説されていることですが、納得のために自分で書き直しています。

• Compute and Simulate Linear Quadratic Regulator (LQR) in MATLAB for Nonzero Set Points | Fusion of Engineering, Control, Coding, Machine Learning, and Science https://aleksandarhaber.com/compute-and-simulate-linear-quadratic-regulator-lqr-in-matlab-for-set-point-tracking/
• [制御] 状態方程式 part 8 (状態フィードバックにおける偏差系の構成)【工学】✅ https://www.youtube.com/watch?v=-RPNpDAIkG0

今回の目標である「ロボットを自由に前進・後退させる」ということは、「車輪の角速度$ϕ˙\dot{\phi}$を指定した値に制御する」と言い換えることができます。
しかし、前回使用したLQRは最適レギュレータと呼ばれ、「状態変数$x=[θ θ˙ ϕ˙]T\mathbf{x} = [\theta \ \dot{\theta} \ \dot{\phi}]^T$（車体の角度、車体の角速度、車輪の角速度）を0にする」、すなわち「車体の直立を保つと同時にロボットがその場から動かないようにする」という動作にしか対応できませんでした。
そこで今回は、目標とする状態$xr=[0 0 ϕ˙r]T\mathbf{x}_r=[0 \ 0 \ \dot{\phi}_r]^T$に対する偏差$xe=x−xr\mathbf{x}_e = \mathbf{x}-\mathbf{x}_r$を考え、それを0にする最適レギュレータを求めることで車輪の角速度を操作できるようにします。

まず、状態が目標値$xr\mathbf{x}_r$に達したときは定常状態（状態が変化しない）になり、状態の変化を表す$x˙\dot{\mathbf{x}}$は0になると考え、その時に必要な入力$ur\mathbf{u}_r$を求めます。式で表すと

$$0=Axr+Bur \mathbf{0} = A \mathbf{x}_r + B \mathbf{u}_r$$

なので、

$$ur=−B+Axr \mathbf{u}_r = -B^+ A \mathbf{x}_r$$

です。ただし$BB$は正方行列とは限らないので擬似逆行列$B+=(BTB)−1BTB^+ = (B^T B)^{-1} B^T$を使って解いています。

次に、状態および入力を定常状態からの差で考えて$x=xe+xr\mathbf{x}=\mathbf{x}_e+\mathbf{x}_r$、$u=u~+ur\mathbf{u}=\tilde{\mathbf{u}}+\mathbf{u}_r$と表すと、状態方程式$x˙=Ax+Bu\dot{\mathbf{x}}=A\mathbf{x}+B\mathbf{u}$は

$$∂∂t(xe+xr)=A(xe+xr)+B(u~+ur) \frac{\partial}{\partial t} (\mathbf{x}_e+\mathbf{x}_r) = A (\mathbf{x}_e+\mathbf{x}_r) + B (\tilde{\mathbf{u}}+\mathbf{u}_r)$$

となります。
さらに、目標値$xr\mathbf{x}_r$は一定である、つまり$xr˙=0\dot{\mathbf{x}_r}=\mathbf{0}$と仮定すると（実際にはユーザの操作によって逐次変化しますが、今は一定とみなします）

$$xe˙=A(xe+xr)+B(u~+ur)=(Axe+Bu~)+(Axr+Bur) \begin{split} \dot{\mathbf{x}_e} &= A (\mathbf{x}_e+\mathbf{x}_r) + B (\tilde{\mathbf{u}}+\mathbf{u}_r) \\ &= (A \mathbf{x}_e + B \tilde{\mathbf{u}}) + (A \mathbf{x}_r + B \mathbf{u}_r) \end{split}$$

と書けます。
ここで先ほど$0=Axr+Bur\mathbf{0} = A \mathbf{x}_r + B \mathbf{u}_r$と定義したことを利用すると、

$$xe˙=Axe+Bu~ \dot{\mathbf{x}_e} = A \mathbf{x}_e + B \tilde{\mathbf{u}}$$

となります。
元の状態の代わりに目標値に対する偏差で考えても、入力を$ur\mathbf{u}_r$からの差分$u~\tilde{\mathbf{u}}$に置き換えることで、状態方程式の係数$A,BA, B$は以前のものと同じになることがわかりました。

ここで評価関数

$$J=∫0∞(xeTQxe+u~TRu~)dt J = \int_0^\infty \left( {\mathbf{x}_e}^T Q \mathbf{x}_e + \tilde{\mathbf{u}}^T R \tilde{\mathbf{u}} \right) dt$$

を最小化するような入力

$$u~=−Kxe \tilde{u} = -K \mathbf{x}_e$$

のゲイン$KK$を求めれば良いのですが、$A, BA,\ B$が同じなので、重み行列$Q, RQ,\ R$が同じであれば$KK$も同じになります。
ですので今回は以前と全く同じゲイン

$$K=[−280.74261026−89.66049857−10] K = \begin{bmatrix} -280.74261026 & -89.66049857 & -10 \end{bmatrix}$$

を使用しました。

旋回の制御

旋回は左右の車輪の回転数に差をつけることで行いますが、旋回を制御理論で扱おうとするとモデルを2次元から3次元にする必要が出てきてしまいます。
ですがそれでは難しくなりすぎるので、今回は制御理論の枠組みの外で旋回を実装しました。

制御則に基づいて決定される車輪の角加速度$ϕ¨\ddot{\phi}$は、ステッピングモータを駆動するために積分して角速度$ϕ˙\dot{\phi}$に変換されます。
それに回転のためのオフセット（片方は正、もう片方は負）を加えれば、前後方向の速度（左右の平均角速度）は元の角速度$ϕ˙\dot{\phi}$を保ったまま同時に旋回できるはずです。
しかし、モータが出せる角速度には限界があります。
そこで、まず最大角速度$ωmax\omega_\mathrm{max}$と元の角速度$ϕ˙\dot{\phi}$の差を求め、回転のためのオフセット$ωturn\omega_\mathrm{turn}$はその範囲に収まるように制限するようにしました。
式にすると

$$ωheadroom=ωmax−ϕ˙ωturn={−2ωheadroom(ωturn,original<−2ωheadroom)ωturn,original2ωheadroom(ωturn,original>2ωheadroom)(ωleft,ωright)=(ϕ˙+12ωturn,ϕ˙−12ωturn) \begin{split} \omega_\mathrm{headroom} &= \omega_\mathrm{max} - \dot{\phi} \\ \omega_\mathrm{turn} &= \begin{cases} -2 \omega_\mathrm{headroom} && (\omega_\mathrm{turn,original} \lt -2 \omega_\mathrm{headroom}) \\ \omega_\mathrm{turn,original} \\ 2 \omega_\mathrm{headroom} && (\omega_\mathrm{turn,original} \gt 2 \omega_\mathrm{headroom}) \\ \end{cases} \\ (\omega_\mathrm{left}, \omega_\mathrm{right}) &= \left(\dot{\phi} + \frac{1}{2} \omega_\mathrm{turn}, \dot{\phi} - \frac{1}{2} \omega_\mathrm{turn}\right) \end{split}$$

ですが、実際のコードの方がわかりやすいと思います。

fn apply_turn(speed: f32, turn: f32, max_speed: f32) -> (f32, f32) {
    let headroom = max_speed - speed.abs();
    let turn = turn.clamp(-2.0 * headroom, 2.0 * headroom);
    (speed + 0.5 * turn, speed - 0.5 * turn)
}

こうすることで「直立・前後進の制御に影響しない範囲で可能な限り指示通りに旋回する（逆に言えば、制御に影響するなら旋回をあきらめる）」という動作が実現できます。

実際には旋回速度を上げすぎると倒れます。理屈通りにはいかないようです。

Wi-Fiによる遠隔操作

本ロボットで使用しているESP32はもともと無線通信機能が売りのマイコンですので、遠隔操作はWi-Fiを使って実装します。
今回はソフトウェアを簡略化するため、数値や文字列などを簡単に送受信できるOpen Sound Control (OSC)というプロトコルを使用しました。
OSCは音楽やメディアアート関係で使われることが多いプロトコルですが、下位層にUDPを使っていて低遅延な上に様々な言語でライブラリが公開されているので、ロボットを含めた様々な用途に活用できます。

RustでOSCを扱うにはroscというCrate（ライブラリ）が便利です。
ただしroscではUDPの送受信処理を自分で実装する必要ががありますが、ESP32ではRustのstd（標準ライブラリ）が使えるため、PCと同じAPIでソケットが扱えます。

Rust+ESP-IDFでのOSCに関しては、Hideaki Taiさんの「Rust (std) on ESP32-C3 で OSC からシリアル LED (WS2812 / SK6812) を動かす」という記事が参考になります。

OSCパケットを送信するPC側のコントローラにはTouchOSCというソフトを使いました。
これを使えば下図のようなUIをコードを書かずに作成できます（しかもスマートフォンを含めた様々な環境に対応しています）。
このように既存のソフトウェアを使って簡単にプロトタイピングができるのもOSCの長所です。

動作

実際に動かしてみるとこのように動きます。倒れないように移動・旋回の最高速度はかなり遅めに設定しています。
ゆらゆらと揺れながらの動作になってしまい、最後には突然倒れてしまいました。まだ改良の余地はありそうです。
https://www.youtube.com/watch?v=C4cZRSNXc94