拡張状態オブザーバによる能動的外乱除去制御について(副題：爆速でマニピュレータの位置制御系を構築する)

はじめに

拡張状態オブザーバによる能動的外乱除去という制御手法は、PID制御に次いで「制御対象のモデルを詳しく知らなくても制御できる」手法として(一部界隈で？？)注目を集めています。
以下、拡張状態オブザーバ(Extended State Observer)をESO、それを用いた能動的外乱除去制御(Active Disturbance Rejection Control)をADRCと表記します。

1990年代に提案された手法ですが、国内の主だった文献はこれ¹とこれ²ぐらいです。個人的にはかなり独創的に感じる制御手法なのですが、ふだん議論に上がることがほとんどない印象だったので記事にしました。

( (2023/01/22 追記) 他にも「サイクロイド減速機の角度伝達誤差に起因した速度振動の拡張状態オブザーバによる抑制法」、電学論D、2014などもあるみたいですね。会員じゃないので内容見れてないですが、FA財団論文賞を受賞しているようです)

ADRCはもちろん線形系にも適用できますが、個人的にこの手法のインパクトは非線形系への適用にあると考えています。

ADRCでは、あるクラスの制御対象であれば非線形系であっても「制御対象のモデリングがほぼ不要」かつ「3つ程度のパラメータチューニング」である程度制御ができてしまいます。多関節マニピュレータなどはまさにそのような系とみなすことができ、PID制御と比べてもかなり簡単に位置制御系が構築できます。本記事では、そのあたりをMATLAB/Simulinkによるシミュレーションで確認していきます。

最終的に、下記のようなシミュレーション結果をお見せします。

拡張状態オブザーバとは

まず初めに、拡張カルマンフィルタとは特に関係ありません。
拡張状態オブザーバという用語は「状態オブザーバ」の拡張のような印象を受けます。そうではなく、「拡張状態」という状態を推定する、ある意味構造自体は普通のオブザーバです。

ESOは外乱オブザーバと少し似ています。状態方程式を対象に外乱オブザーバを構成するとき、外乱を状態に含めて拡大系を作ることを考えます。同様に、「拡張状態」という状態を含めて拡大系を作ってオブザーバを構成するのがESOです。

以下、詳細を述べていきます。

制御対象

下記であらわされる1入出力の非線形の連続時間系を考えます。

\begin{align} 
\left\{
\begin{array}{l}
\dot{x}_1 &=& x_2 \\
\dot{x}_2 &=& x_3 \\
&\vdots& \\
\dot{x}_{n-1} &=& x_n \\
\dot{x}_n &=& f(x_1, \cdots, x_n) + b u  \\
y &=& x_1
\end{array}
\right.
\end{align}

ここで、状態$x \in \mathbb{R}^n$、入力$u \in \mathbb{R}^1$、出力$y \in \mathbb{R}^1$で、$b$は定数です。

早速かなり限定的な制御対象では？と思ってしまうかもですが、意外とこれに当てはまる系はそこそこあります。例えば$x_1$を位置、$x_2$を速度、$u$を力とし、位置が観測できる典型的なバネマスダンパ系は下記のようなかたちであらわされます。$a_1$はバネ定数、$a_2$は減衰定数に相当します。

\begin{align} 
\left\{
\begin{array}{l}
\dot{x}_1 &=& x_2 \\
\dot{x}_2 &=& a_1 x_1 + a_2 x_2 + u  \\
y &=& x_1
\end{array}
\right.
\end{align}

つまりこの場合、$f:=a_1 x_1 + a_2 x_2$、$b:=1$ということです。

同様に、下記の1関節マニピュレータもこの形で表現することができます。以下、$n=2$と限定して考えていきます。
ほぼ図を見てのとおりですが、$\theta$を角度、$\omega$を角速度、$c$を角度に対する減衰係数、$M$を質量、$I$を支点周りの慣性モーメントとします。また、角度$\theta$が計測でき、トルク$\tau$が印可できるものとします。

上図のような1関節マニピュレータ(というより、トルクが印可できる剛体振り子)の運動方程式は下式で表されます。

\begin{align} 
I \dot{\omega} &= -Mgl \sin(\theta) - c \omega + \tau \\
\dot{\omega} &= -\frac{Mgl}{I} \ \sin(\theta) - \frac{c}{I} \ \omega + \frac{1}{I} \ \tau 
\end{align}

この式も、$f:= -(Mgl/I) \sin(\theta) - (c/I) \omega$、$b:=1/I$と置くことで、最初に示した非線形系のフォーマットに合わせて記述することができます。つまり、下式となります。

\begin{align} 
\dot{\omega} &= f(\theta,\omega) + b \tau 
\end{align}

これをふまえ、状態方程式ふうの表現で書くと下式となります。入力以外の項をまとめて右端に押し込めた形です。

\begin{align} 
 \left(
\begin{array}{c}
\dot{\theta} \\
\dot{\omega}
\end{array}
\right) &=  \left(
\begin{array}{c}
0 & 1 \\
0 & 0 
\end{array}
\right) \left(
\begin{array}{c}
\theta \\
\omega
\end{array}
\right) + \left(
\begin{array}{c}
0 \\
b
\end{array}
\right) 
\tau + \left(
\begin{array}{c}
0 \\
1
\end{array}
\right) f
\end{align}

拡張状態オブザーバの構成

以下が特徴的な発想ですが、上記の状態方程式に対し、$f$を状態変数とみなして拡大系を作ることを考えます。この$f$が拡張状態と呼ばれる要素です。
外乱オブザーバは「外乱」を状態として追加して拡大系を作りますが、それと似ているようで少し違います。ESOでは、「入力項を除いたすべてのダイナミクス(外乱も含む)」を新たな状態として拡大系を作るのが特徴です。

\begin{align} 
 \left(
\begin{array}{c}
\dot{\theta} \\
\dot{\omega} \\
\dot{f}
\end{array}
\right) &=  \left(
\begin{array}{c}
0 & 1 & 0 \\
0 & 0 & 1 \\
0 & 0 & 0 \\
\end{array}
\right) \left(
\begin{array}{c}
\theta \\
\omega \\
f
\end{array}
\right) + \left(
\begin{array}{c}
0 \\
b \\
0
\end{array}
\right) 
\tau + \left(
\begin{array}{c}
0 \\
0 \\
1
\end{array}
\right) \dot{f}
\end{align}

ここで少し乱暴ですが、$\dot{f}$を無視します。微小時間であれば$\dot{f} \approx 0$だろうという考えで、このあたりが議論のあるところかと思いますがいったん受け入れてください。なお、この$\dot{f}$を残して議論を進めるESOもありますが、本記事では一切無視することにします。

\begin{align} 
 \left(
\begin{array}{c}
\dot{\theta} \\
\dot{\omega} \\
\dot{f}
\end{array}
\right) &=  \left(
\begin{array}{c}
0 & 1 & 0 \\
0 & 0 & 1 \\
0 & 0 & 0 \\
\end{array}
\right) \left(
\begin{array}{c}
\theta \\
\omega \\
f
\end{array}
\right) + \left(
\begin{array}{c}
0 \\
b \\
0
\end{array}
\right) 
\tau
\end{align}

上記の状態方程式は$y=\theta$であることをふまえると、少し意外ですが可観測と言えます。
ちなみに、この状態方程式は可観測である一方、可制御ではありません。このあたりも外乱オブザーバの「外乱を観測できるけど、外乱を制御はできない」という様子とよく似ています。

したがって、拡張状態$f$を定義し$\dot{f}=0$さえ受け入れれば、もともとの非線形系は線形かつ可観測な系とみなすことができます。そこで、ふつうの同一次元状態オブザーバを下記のように構成します。

\begin{align} 
 \left(
\begin{array}{c}
\dot{\hat{\theta}} \\
\dot{\hat{\omega}} \\
\dot{\hat{f}}
\end{array}
\right) &=  \left(
\begin{array}{c}
0 & 1 & 0 \\
0 & 0 & 1 \\
0 & 0 & 0 \\
\end{array}
\right) \left(
\begin{array}{c}
\hat{\theta} \\
\hat{\omega} \\
\hat{f}
\end{array}
\right) + \left(
\begin{array}{c}
0 \\
b \\
0
\end{array}
\right) 
\tau + L \left( \theta - \hat{\theta}
\right)
\end{align}

ここで、$L$は3次元のオブザーバゲインベクトルです。ハット記号は推定変数であることを表しています。

上記は一番簡単なESOの構成となっています。オブザーバゲイン設計は重極に極配置を行うのが一般的なようです。ただし、対象を線形とみなした後のオブザーバの構成はいろいろ派生があるようで、文献¹では非線形オブザーバの適用を考えるなどしています。

ちょっと信じがたいですが、これで$f$も$\omega$も推定できるようになります。察しの良い方はすでにお気づきと思いますが、この$f$が推定できることが重要です。

能動的外乱除去制御とは

ADRCは、上述したESOによる推定値$\hat{f}$でダイナミクス$f$を相殺して除去する制御です。
角度指令$\theta_{\rm ref}$、角速度指令$\omega_{\rm ref}$を与えて制御することを考え、具体的に制御側を記述すると下式となります。

\begin{align} 
\tau &= \frac{-\hat{f} - k_d (\hat{\omega} - \omega_{\rm ref}) - k_p (\hat{\theta} - \theta_{\rm ref}) } {b}
\end{align}

$k_d$は角度に対する微分ゲイン、$k_p$は角度に対する比例ゲインです。ステップ状の角度指令を与えるのであれば$\omega_{\rm ref}$は0でOKです。なんでPDゲインだけなの？は気になるところですが、$f$の推定と相殺によって一定値の定常外乱も相殺されるためです。このあたりの様子も後述のシミュレーションで確認します。

ちなみに、$\hat{\theta}$は直接観測できるので$\theta$と置き換えてもいいと思われます。$f$を相殺するところ以外は結構自由で良いはずですが、おおむねどの文献もこの制御則をとることが多いようです。 この点、$f$が打ち消された後の挙動について少し補足します。

もう一度思い出すと、制御対象は$\dot{\omega} = f(\theta,\omega)+ b \tau $であらわされました。
上述の制御則をこの制御対象に印可して、定常状態で$\hat{\omega} \approx \omega$、$\hat{f} \approx f$となったとします。代入していくと下式になります。

\begin{align} 
\dot{\omega} &= -k_d (\omega- \omega_{\rm ref}) - k_p (\theta - \theta_{\rm ref}) \\
\ddot{\theta} &= -k_d (\dot{\theta} - \dot{\theta}_{\rm ref}) - k_p (\theta - \theta_{\rm ref})
\end{align}

途中で$\omega$の表記をやめ、$\theta$のみの式にしました。上記は$f$が消去され、いわゆる加速度制御系と言われる扱いやすい形になりました。この系は、明らかに$\theta_{\rm ref}$から$\theta$までの特性を$k_p$、$k_d$によって簡単に操作することができます。

念のためもう少しだけ掘り下げます。各変数の初期値0として両辺をラプラス変換して整理すると、理想状態での$\theta_{\rm ref}$から$\theta$までの応答特性は下式となります。

\begin{align} 
(s^2 + k_d s+ k_p) \ \theta(s)&= (k_d s + k_p) \ \theta_{\rm ref}(s) \\
\frac{\theta(s)}{\theta_{\rm ref}(s)} &= \frac{k_d s + k_p}{s^2 + k_d s+ k_p}
\end{align}

上記の2次系は、$k_p$と$k_d$で応答特性を簡単に操れます。特に、$\omega_{\rm ref}=0$なら上記の分子の$k_d s$の項が0になり、教科書で見るような典型的な2次遅れ系となります。よく出てくる固有角周波数$\omega_n$、減衰比$\zeta$の表記に合わせると、$k_p=\omega^2_n$、$k_d = 2\zeta \omega_n$と対応づいています。したがって、$f$を消去したあとの特性を2次遅れ系のモデルに照らし合わせて応答性を操作しよう、という意図が上述の制御則です。

まとめると、ADRCによる制御系の設計手順は下記のとおりです。

1. メカ設計値やパラメータ同定実験などで、$b=1/I$を求める。
2. オブザーバゲイン$L$を極配置などで選ぶ。通常は同一極への配置で良い。
3. 比例ゲイン$k_p$、微分ゲイン$k_d$を選ぶ。
4. 上述のESOの構成式、ADRCの制御則を実装する。

たったこれだけで、制御対象としてはそこそこ厄介なはずの非線形系が制御できます。
(なお、文献²では定数$b$もチューニングパラメータとみなしています。このあたりは、制御対象をどう表現するかが文献によって微妙に異なるためです)

したがって、冒頭で述べた項目は以下を指していました。

• 制御対象のモデリングがほぼ不要 → 慣性モーメントだけわかればOKなので。
• 3つ程度のパラメータチューニング → ESOの重極の位置、比例ゲインと微分ゲインだけ調節する。

以上で手法についての概要は終わりです。
いろいろ突っ込みどころがあり、ホントにそんな上手くいくのか？とは思いますが以下で検証していきます。

1関節マニピュレータのシミュレーション

Simulinkモデルは下記です。これを動かすMATLABコードは末尾に載せました。

$t=0.5$で$\theta_{\rm ref}=\pi/2$を与え、$t=1.5$で外乱トルクが加わるものとしました。また、$\theta$の初期値は$\pi/4$とし、最初はレギュレータとしての動作をさせてみます。ESO内での推定角度初期値は0とし、実値と相違を与えることで推定過渡期の様子を確認します。

比較対象がないとさみしいので、積分型最適サーボと比較することにしました。導出方法はこちら³や、末尾のコード内を参照ください。運動方程式を$\theta=0$の近傍で線形化して得られた状態方程式を用いて各ゲインを導出しています。
最適サーボ系は下記の構成となります。本来はオブザーバで速度推定する必要があるかと思いますが、省略しました。

1関節マニピュレータのシミュレーション結果

結果は下記です。上の図はステップ状の角度指令$\theta_{\rm ref}$に、ADRCで制御した場合の角度、積分型最適サーボで制御した場合の角度を載せています。

下の図は、ADRCで用いたESOの推定の様子です。運動方程式の$f$の項(それに外乱トルクも足した)と、ESOによって推定された$\hat{f}$を載せています。

シミュレーション結果の解釈

結果は甲乙つけがたいかたちとなりました。
$\theta=\pi/2$は$\sin(\theta)\approx \theta$と線形近似できない領域なので少し意外でしたが、最適サーボ系もかなり頑張っているようです。どちらも初期値から0に向かった後、角度指令に滑らかに追従している様子がわかります。また、外乱の影響も抑制されています。

ESOはステップ状の外乱までまとめて$f$として推定する様子がわかります。また、ESOは$t=0.05$あたりまで、$f$の推定値が枠外で非常に大きくなっています。最初の0に向かうのがやや早いのはこれによって過大なトルクが印可されただけに見えます。この点は、文献⁴などでは$f$の推定が落ち着くまでは制御入力を印可しないなどの対策を挙げています。

甲乙つけがたい結果となりましたが、ここでの優劣にあまり意味はありません。最適サーボ系の評価関数の重みを変更すると結果は変わりますし、ADRCでもESOの極やPDゲインを変更すると結果が変わります。ここで強調したいのは、明らかにADRCの方が簡単に設計できる点です。

最適サーボ系は制御対象の運動方程式を求めた後、線形近似して得た状態方程式の係数行列から初めて各種ゲインが求まります。一方ADRCの場合は慣性モーメントだけわかっていれば良いです。設計パラメータの選定に関しても、最適サーボ系は評価関数の重みを、ADRCではESOの極位置と比例・微分ゲインを選定します。このシミュレーションの限りでは、その選定の試行錯誤の手間はほぼ変わりません。

したがって、モデリングの手間が少ない分設計が容易なADRCが最適サーボ系と戦えている点はある意味すごいと言えます。
唯一求めなければいけない$b=1/I$に関しても、適当に倍にしたり半分にしても即座に制御系が破綻することはなく、定常値にずれが生じるぐらいでそこそこ動きます。このあたりの様子は割愛しますが、興味がある方は試してみてください。

2関節マニピュレータへの適用

上述のとおり、1関節マニピュレータの場合はある程度上手くいきました。しかし、ADRCの本領は2関節以上でも上記内容を「そのまま」適用しようとするところにあります。

どういうことかというと、各関節を独立した1入出力系だとみなし、複数関節のマニピュレータが互いに動くことで生じる$f$を相殺させようとします。つまり、やることはESOを独立に並べて各関節ごとの$f$を推定し相殺させるだけです。

この点も、外乱オブザーバを各関節に配置して互いの外乱を相殺する・・・という様子と似てないこともないです。ただし、外乱オブザーバを使った場合には各々の関節のモデリングぐらいは必要かと思いますが、ADRCの場合にはそれすら不要です。

以下、もう少しだけ補足します。
2関節マニピュレータは下図であらわされるものを対象とします。1関節のものと上下反転してますが、特に深い意味はないです。
ほぼ図を見てのとおりですが、重心と回転軸の間の距離を$l$、関節の長さを$L$と分けています。また、重心周りの慣性モーメントを$I$、各関節の減衰係数を$c$としています。

ちなみに、この2関節マニピュレータの動作モデルをMATLAB Symbolic Math Toolboxを使って求める方法を別記事に書きました。今回の検証ではそちらを使っています。詳細は下記をご確認ください。

この2関節マニピュレータの運動方程式は下式となります。

\left(
\begin{array}{cc}
I_1 + I_2 + M_1 l_1^2 + M_2 L_1^2 + M_2 l_2^2 + 2 M_2 L_1 l_2 \cos(\theta_2)  & I_2 + M_2 l_2^2 + M_2 L_1 l_2 \cos(\theta_2) \\
I_2 M_2 l_2^2 + M_2 L_1 l_2 \cos(\theta_2) & I_2 + M_2 l_2^2 
\end{array}
\right) \left(
\begin{array}{cc}
\ddot{\theta}_1 \\
\ddot{\theta}_2 
\end{array}
\right) 
= \left(
\begin{array}{cc}
M_2 L_1 l_2 (2 \dot{\theta_1} \dot{\theta_2} + \dot{\theta_2}^2)   \sin(\theta_2) \\
-M_2 L_1 l_2   \dot{\theta_1}^2   \cos(\theta_2)
\end{array}
\right) -
\left(
\begin{array}{cc}
(M_1 l_1 + M_2 L_1) g \sin(\theta_1) - M_2 g l_2 \sin(\theta_1 + \theta_2) \\
M_2 g l_2 \sin(\theta_1 + \theta_2)
\end{array}
\right) - 
\left(
\begin{array}{cc}
c_1 \dot{\theta}_1 \\
c_2 \dot{\theta}_2 
\end{array}
\right) + 
\left(
\begin{array}{cc}
\tau_1 \\
\tau_2
\end{array}
\right) 

見る方も書く方もうんざりする式ですが、記号を定義してもう少し簡略化することができます。慣性行列の(1,1)要素だけ$\theta_2$に依存する項、そうでない定数の項で分けました。

\begin{align} 
\left(
\begin{array}{cc}
M_{11} + M(\theta_2) & M_{12}(\theta_2) \\
M_{12}(\theta_2)  & M_{22} 
\end{array}
\right) \left(
\begin{array}{cc}
\ddot{\theta}_1 \\
\ddot{\theta}_2 
\end{array}
\right) 
= 
\left(
\begin{array}{cc}
\tilde{f}_1 (\theta_1, \dot{\theta}_1, \theta_2, \dot{\theta}_2) \\
\tilde{f}_2 (\theta_1, \dot{\theta}_1, \theta_2, \dot{\theta}_2)
\end{array}
\right) + 
\left(
\begin{array}{cc}
\tau_1 \\
\tau_2
\end{array}
\right) 
\end{align} 

さらに、非対角の項、対角だけど$\theta_2$に依存している項も右側に押し込めます。角加速度まで$f$に含めてしまうので、一番当初にかかげた制御対象のフォーマットすら逸脱している気がしますがとにかくやってみます。$f_1 := \tilde{f}_1-M(\theta_2) \ddot{\theta}_1 -M_{12}(\theta_2) \ddot{\theta}_2 $、$f_2 := \tilde{f}_2-M_{12}(\theta_2) \ddot{\theta}_1 $と置いた結果、下式となります。

\begin{align} 
\left(
\begin{array}{cc}
M_{11} & 0 \\
0 & M_{22} 
\end{array}
\right) \left(
\begin{array}{cc}
\ddot{\theta}_1 \\
\ddot{\theta}_2 
\end{array}
\right) 
&= 
\left(
\begin{array}{cc}
f_1 (\cdot) \\
f_2 (\cdot)
\end{array}
\right) + 
\left(
\begin{array}{cc}
\tau_1 \\
\tau_2
\end{array}
\right) \\
\left(
\begin{array}{cc}
\ddot{\theta}_1 \\
\ddot{\theta}_2 
\end{array}
\right) 
&= 
\left(
\begin{array}{cc}
\frac{1}{M_{11}} f_1 (\cdot) \\
\frac{1}{M_{22}} f_2 (\cdot)
\end{array}
\right) + 
\left(
\begin{array}{cc}
\frac{1}{M_{11}} \tau_1 \\
\frac{1}{M_{22}} \tau_2
\end{array}
\right) \\
\end{align} 

$f$のかっこ内はもはや意味をなしていないので省略しています。いくら何でも強引な気がしますが、これで見かけ上1入出力の独立した系が2つ得られたことになります。

2関節マニピュレータのシミュレーション

上述の内容をふまえ、作成したSimulinkモデルは下記です。結局やってることはESOを2つ並べ、独立に$f$を抑制させつつPD制御しているだけです。$b_1$の逆数、$b_2$の逆数は上述の$M_{11}$と$M_{22}$から求めていますが、それ以外に制御対象の物理パラメータを使っていません。

ちなみに、一般的なマニピュレータの位置制御には計算トルク法(もしくは線形化補償器とも呼ばれる)が使われているかと思います。そちらも参考までに載せておきます。(あまりちゃんとした比較になってないので隠して載せておきました。)

通常、マニピュレータの位置指令には時間多項式や三角関数をベースとした式で滑らかな波形を与えることが多いと思いますが、今回はステップ状の位置指令に対する応答を見てみます。
$t=2$で$\theta_{1 {\rm ref}}=\pi/4$を与え、$t=4$で$\theta_{2 {\rm ref}}=\pi/2$を与え、$t=7$で両関節に外乱トルクが加わるものとしました。

パラメータ選定について

ここまで述べてきた通り、設計にあたってはESOの極配置と比例・微分ゲインの選定が必要です。線形システムではいろいろと選定の目安が提案されつつある⁴ようですが、今回は完全に試行錯誤としました。

選定の難易度ですが、今回の2関節マニピュレータはまったく難しくありません。適当にESOの極を選んで特に発振することもなかったので、簡易的に第1関節も第2関節もまったく同じパラメータとしました。ESOの極をだいたい決めた後、比例・微分ゲインで振動しやすさ・減衰などを調節します。

なお、極を負側に配置しすぎるとオブザーバゲインの3要素目が非常に大きくなります。重極に配置する都合上、3要素目は極位置の3乗の値となるためです。今回は応答性を見つつ適当なところでやめにしましたが、それでも8,000,000という数値になりました。実機適用時には、このあたりに注意が必要そうです。

2関節マニピュレータのシミュレーション結果

結果は下記のとおりです。アニメーション描画もしてみました。赤丸が指令角度をもとにした手先の指令位置です。

アニメーション描画は下記のページを参考にさせていただきました。

シミュレーションの解釈

両関節とも、ステップ状の指令に滑らかに追従しています。$t=7$でのステップ状の外乱トルクに対しても速やかに応答しています。

事前にわかる制御対象の物理パラメータは慣性行列の一部だけ、という縛りにしてはちょっと気味が悪いくらい上手くいきました。外乱にも速やかに応答しているので、把持物で手先の重量が変わる、などの状況にも強そうです。ただやはり2関節を完全に独立で動かしているためか、お互いが動いた時に他方が影響を受けているところは若干気になります。

ちなみに、オブザーバゲインがハイゲインになるのが気になったので、両関節角度に0.1[deg]程度の一様乱数を印可してみたりもしました。するとやはり$\hat{f}$と印可トルクにスパイク上波形がのる様子が確認されましたが、高周波の印可トルクは制御対象のローパス特性であまり関節角度にあらわれてこない様子でした。こちらも詳細は割愛しますので、興味がある方は試してみてください。

計算トルク法の場合のシミュレーション

参考までに、計算トルク法を使った場合はこんな感じです。

計算トルク法(Computed Torque Method)の構成は↓です。各種非線形項を除去して、慣性行列を経由することで線形化補償をしています。角度に対するPIDゲインは試行錯誤で適当に決めました(プロの方のチューニングアドバイスをお待ちします・・・)。

結果はこんな感じです。非線形項を除去してるつもりですが意外と各関節の駆動時に他方が影響を受けていて、あとはやはり外乱の影響は取り除けないです(Iゲイン多少いじってもだめ)。おそらくは、ここにさらに線形近似モデルから外乱オブザーバを付加したりするのかなという感じですが。

円弧軌跡への追従シミュレーション

最後に、多関節マニピュレータと言えば・・・で手先で円を描かせてみます。手先が円を描くために必要な関節角度を逆運動学で求め、各関節の角度指令に印可しました。また、角速度指令としてそれらの数値微分を印可しました。あまり大した内容じゃないので、詳細はシミュレーションモデルを直接ご確認ください。

ADRCの設計パラメータは上述のステップ指令のものと同一としました。
結果は下記の通りです。

適当に選んだパラメータでも円ぐらいは簡単に描けるようです。
パラメータチューニングを頑張ればもっと追従性はよくなるかと思いますが、とりあえず今回はこんなもんだということで。

記事に使ったMATLABコード、モデル

今回使ったMATLABコード、モデルはこちら
1関節マニピュレータの積分型最適サーボの設計にControl Sytem Toolboxを使ってます。それ以外はMATLAB/Simulinkだけです。

1関節マニピュレータのMATLABコード

ADRC_1link_arm.m

clear

%% ADRCの1リンクマニピュレータの適用 積分型最適サーボとの比較

%% 物理パラメータ
M = 1;%[kg]
l = 1;%[m]
g = 9.8;
c_theta = 0.2;%減衰係数
I = M*l^2/12 + (l/2)^2*M; %剛体振り子のイナーシャ

theta_ini = pi/4; dtheta_ini = 0; %初期状態

%% 線形近似した状態空間モデル
A=[0 1; -M*g*l/I -c_theta/I]; B=[0;1/I]; C=[1 0]; D=0; 
x0 = [theta_ini; dtheta_ini];
[n,~] = size(A); [~,m] = size(B); [p,~] = size(C);

%% 拡張状態方程式の定義
A_ex = [0 1 0; 0 0 1; 0 0 0 ];
B_ex = [0; B(2); 0];
C_ex = [1 0 0]; % 角度のみ観測
b_inv = 1/B(2); % 拡張状態を相殺させるにあたっての係数

%% 拡張状態オブザーバの極配置
% pole_exobs = [-5 -6 -7]*10;
% L_ex = place(A_ex',C_ex',pole_exobs)'; 

%% 拡張状態オブザーバの極配置 同一極に配置する場合
exobs_omega = 200; % ESOの極
c2 = 3*exobs_omega;
c1 = 3*exobs_omega^2;
c0 = exobs_omega^3;
L_ex = [c2;c1;c0]; 

kp = 600;% 適当なPゲイン
kd = 40;% 適当なDゲイン 


%% 積分型最適サーボ系設計
A_tilde=[A zeros(n,1); -C 0]; B_tilde=[B; 0];
Q_ctrl = diag([1e2, 1e2, 1e8]);
R_ctrl = 1;
F_ctrl = lqr(A_tilde, B_tilde, Q_ctrl, R_ctrl);
Kp = F_ctrl(1);
Kd = F_ctrl(2);
Ki = -F_ctrl(n+1);

%% シミュレーション実行
Tsim = 0.001;%シミュレーション時間間隔
out = sim('sim_ADRC_1link_arm');

%% 結果の描画
lw = 1;
FontSize = 18;
fig1 = figure;
subplot(2,1,1),plot(out.theta_ref.Time, out.theta_ref.Data,...
    out.theta.Time, out.theta.Data,...
    out.theta_opt.Time, out.theta_opt.Data,...
    'LineWidth',lw); grid on
graph_x1_1 = legend('\theta_{ref} ', '\theta ADRC', '\theta OptServo');
graph_x1_1.FontSize = FontSize;
set(graph_x1_1,'Location','NorthEast');
title('Position tracking','FontSize',FontSize)
ylabel('Angle[rad]','FontSize',FontSize)
xlabel('Time[s]','FontSize',FontSize)
subplot(2,1,2),plot(...
    out.f.Time, out.f.Data,...
    out.f_hat.Time, out.f_hat.Data,...
    'LineWidth',lw); grid on
graph_x1_2 = legend('f', 'estimated f');
graph_x1_2.FontSize = FontSize;
set(graph_x1_2,'Location','NorthEast');
title('Dynamics estimation','FontSize',FontSize)
ylabel('[rad/s^2]','FontSize',FontSize)
xlabel('Time[s]','FontSize',FontSize)
ylim([-200 200]);

2関節マニピュレータのMATLABコード

ADRC_2link_arm.m

clear
close all

%% 物理パラメータ
M1 = 1; % [kg]
M2 = 1; % [kg]
l1 = 0.5; % [m]
l2 = 0.5; % [m]
L1 = 1; % [m]
L2 = 1; % [m] 運動方程式には使わないが、アニメーションで使う

g = 9.8;
c1 = 0.5; % 減衰係数
c2 = 0.5; % 減衰係数

I1 = M1*l1^2;
I2 = M2*l2^2;

M_0 = [I1+I2+M1*l1^2+M2*L1^2+M2*l2^2 I2+M2*l2^2; I2+M2*l2^2 I2+M2*l2^2]; % cos(theta2)によらない部分
M_cth2 = [2*M2*L1*l2 M2*L1*l2; M2*L1*l2 0]; % cos(theta2)がかかる部分

theta1_ini = 0; dtheta1_ini = 0; %初期状態
theta2_ini = 0; dtheta2_ini = 0; %初期状態
theta1_ini = -0.5; theta2_ini = 2; % 円を描かせる場合はこっちの初期値

%% 拡張状態方程式の定義
A1_ex = [0 1 0; 0 0 1; 0 0 0 ];
B1_ex = [0; 1/M_0(1,1); 0];
% B1_ex = [0; 1; 0];
C1_ex = [1 0 0]; % 角度のみ観測
b1_inv = M_0(1,1); % 拡張状態を相殺させるにあたっての係数

A2_ex = [0 1 0; 0 0 1; 0 0 0 ];
B2_ex = [0; 1/M_0(2,2); 0];
% B2_ex = [0; 1; 0];
C2_ex = [1 0 0]; % 角度のみ観測
b2_inv = M_0(2,2); % 拡張状態を相殺させるにあたっての係数

%% 拡張状態オブザーバの極配置
% 同一極に配置する
exobs_omega1 = 200; % ESOの重極 第1関節
L1_ex = [3*exobs_omega1;3*exobs_omega1^2;exobs_omega1^3]; 
exobs_omega2 = 200;  % ESOの重極 第2関節
L2_ex = [3*exobs_omega2;3*exobs_omega2^2;exobs_omega2^3]; 

kp1 = 30; % Pゲイン 第1関節
kd1 = 10; % Dゲイン 第1関節
kp2 = 30; % Pゲイン 第2関節
kd2 = 10; % Dゲイン 第2関節 

%% シミュレーション実行
Tsim = 0.001;%シミュレーション時間間隔
Tend = 10;
% out = sim('sim_ADRC_2link_arm');  % ステップ角度指令はこっち
out = sim('sim_ADRC_2link_arm_circle');  % 円を描かせる場合はこっち

%% 結果の描画
lw = 1;
width = 600;
hight = 400;
fig1 = figure('Position',[100 100 width hight]);
plot(out.theta1_ref.Time, out.theta1_ref.Data,...
    out.theta1.Time, out.theta1.Data,...
    out.theta2_ref.Time, out.theta2_ref.Data,...
    out.theta2.Time, out.theta2.Data,...
    'LineWidth',lw); grid on
graph_x1_1 = legend('\theta_{1 ref}', '\theta_1', '\theta_{2 ref}', '\theta_2');
set(graph_x1_1,'Location','NorthEast');
title('ADRC Angle Tracking')
ylabel('Angle[rad]')
xlabel('Time[s]')

アニメーション描画に使ったMATLABコード

animation_2link.m

close all
% あらかじめADRC_2link_arm.mを実行しておく。
% ワークスペースに展開されたtheta1、theta2等々からアニメーションを描画
% ↓を参考にしました。
% https://fumikirinobouken.hatenablog.com/entry/2018/10/16/174553

hight = 300; % プロットするfigureの縦幅
width = 300; % プロットするfigureの横幅

sizen= 512; % rgb2ind()の関数でRGBイメージをインデックス付きイメージに変換する…らしい。
            % ただ、gifにアニメーションとして保存するサイズに対してこのsizeが小さいと"権限がない"
            % とかのエラーがでる。そうなったらこのsizeを大きくする。

delaytime = 0.1; % 画像の更新間隔[s]         


% プロット＆保存 %%%%%%%%%%%%%%%

h = figure('Position',[100 100 width hight]);
axis tight manual % this ensures that getframe() returns a consistent size
filename = '2link_Animation.gif'; % 保存する名前
theta1 = out.theta1.Data;
theta2 = out.theta2.Data;
theta1_ref = out.theta1_ref.Data;
theta2_ref = out.theta2_ref.Data;
dec_size = uint32((length(out.theta1.Data)-1)/100); % アニメーションのデータレート
x1 = L1*sin(theta1);   
y1 = L1*cos(theta1); 
x2 = L2*sin(theta1+theta2);
y2 = L2*cos(theta1+theta2); 
x_ref = L1*sin(theta1_ref) + L2*sin(theta1_ref+theta2_ref);
y_ref = L1*cos(theta1_ref) + L2*cos(theta1_ref+theta2_ref); 
t = 0:Tsim:Tend;

for n = 1:dec_size
    % plot
    i = n*dec_size; % アニメーション用に間引いたデータの添え字
    plot([0,x1(i)],[0,y1(i)],'b-', 'LineWidth', 3) % 第1関節の描画
    hold on
    plot([x1(i),x1(i)+x2(i)],[y1(i),y1(i)+y2(i)],'g-', 'LineWidth', 3) % 第2関節の描画
    hold on
    plot(x_ref(i),y_ref(i),'ro', 'LineWidth', 2) % 手先指令位置の描画
    hold off
    txt = text(-1, -1, '', 'FontSize', 16); 
    txt.String = sprintf('t =  %0.1f [s]', t(i)); % 時刻を表示
    title('ADRC 2-Link Manipulator Animation')
%     if t(i)>=7
%         dist_txt = text(-1, -1.5, '', 'FontSize', 16); 
%         dist_txt.String = sprintf('disturbance'); % 時刻を表示
%     end
    ylabel('Position[m]')
    xlabel('Position[m]')
    ylim([-2 2])
    xlim([-2 2])
    drawnow % figureを更新する
    
    % Capture the plot as an image
    frame = getframe(h);
    im = frame2im(frame);
    [imind,cm] = rgb2ind(im,sizen);
    % Write to the GIF File
    if n == 1
        imwrite(imind,cm,filename,'gif', 'Loopcount',inf,'DelayTime',delaytime);
    else
        imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',delaytime);
    end
end

おわりに

ADRCという手法について述べました。お見せした通り、ADRCはあるクラスの制御対象であれば非常に簡単に制御系が構築できます。特に2関節マニピュレータについて、過渡的な挙動をあまり気にしなければ、ある程度の精度で簡単に位置制御が行える様子を確認しました。

ただし、制御系設計にあたってはふつう、制御対象の数理モデルをもとに理想の制御性能を追求することが行われます。ADRCは、与えられた制御対象の数理モデルに対し、どう設計パラメータを選ぶのが最適か？という点についてはまだまだ研究途上な気がします(今回扱ったのは非線形系なのでそもそも難しいですが)。そうはいっても制御系設計に試行錯誤はつきもので、試行錯誤の手間は残しつつもモデリングの手間が減らせるのなら非常に良い手法だと感じました。

執筆者自身も、イマイチ他の手法との比較やなぜこれで動くのか？が腹落ちしてない感があるため、議論が活発になって他の方が補っていただければと思います。

ご覧いただきありがとうございました。

ちなみに、この手法の名前について思うところを書きました。

「拡張状態オブザーバによる能動的外乱除去制御」、はあんまりいい名前じゃない気がしています。特に、拡張という言葉は微妙です。

個人的には拡張状態というより、ダイナミクスを推定しているので「ダイナミクスオブザーバ？動特性オブザーバ？」あたりがまだましな気がしますが、もう一声欲しいところです。

また「オブザーバで推定して打ち消す」という手法なので、「外乱オブザーバによる外乱補償制御」と似た感じで意味が重複してますし、長いですね。能動的外乱除去制御：ADRCとだけ言って通じるようになるといいんですが・・・

1. 「拡張状態オブザーバによるロボットの高速・高精度運動制御」, マハワン バグス,羅 正華,韓 京清,中嶋 新一, 日本ロボット学会誌, 2000, https://www.jstage.jst.go.jp/article/jrsj1983/18/2/18_2_244/_pdf ↩ ↩²

2. 「能動的外乱除去制御器を用いた場合の閉ループ系の安定性解析とその応用」, 杉山 開路, 丸田 一郎, 杉江 俊治, 2015, 計測自動制御学会論文集, https://www.jstage.jst.go.jp/article/sicetr/51/7/51_494/_pdf ↩ ↩²

3. 「積分型最適サーボ系の構成」, 池田 雅夫, 須田 信英, 1988 , 計測自動制御学会論文集, https://www.jstage.jst.go.jp/article/sicetr1965/24/1/24_1_40/_article/-char/ja/ ↩

4. 「Performance analysis of active disturbance rejection tracking control for a class of uncertain LTI systems」, Wenchao Xue, Yi Huang, 2015, ISA Transactions, https://www.sciencedirect.com/science/article/abs/pii/S0019057815001159 ↩ ↩²