振り子の振動をエアリアクションホイールによって制御する

概要

倒立振子を作りたかったのですが、なかなかうまくいかないので、まずは振り子の自由振動を減衰させる事を目標に制御系を組んで実験してみようと思います。

こんな感じで、エンコーダー＋加速度・ジャイロセンサの構成です。

エアリアクションホイール：ERH

リアクションホイールとは、ホイールの慣性力を制御量に使用した方法です。J(dw/dt)を使用します。
駒を回した後、回っている駒の軸をつかむと指にトルクが来るのと同じ方法です。
この方法は定常トルクを出すのが難しいです。なぜなら、ホイールの回転速度が比例的に増加する必要があるからです。回転速度が上がるほど、モーターの逆起電力が上がるため、印加電圧の上限にすぐぶつかってしまいます。
ここで、エアリアクションホイールとは空気抵抗を利用してトルクを生みます。「のび太君のタケコプターで、のび太君が回るのでは？」というのと同じ考えです。この方式の良さは、c*w　の項を使うことになるので、回転速度が上がりすぎず逆起電圧が小さく、モーターの方は電流制御をすればそのまま出力トルクとでき、損失が小さいというメリットが有ります。
CMG：コントロールモーメントジャイロ　というジャイロ効果を利用してトルクを発生させる方法もあります。これを使う場合、たとえ一方向のトルクを出したい場合でも3つ以上機体が必要になるのと（有る方向に固定されていてその方向にしか回転できない場合は1機体でも良いかも）、うまく指定しないと一時的にある方向にはトルクを発生させれなくなったりするそうで、私の能力ではまだ手を出せていません。しかし、発生トルクが上2つの方法とレベチなのでこちらを使えるようになりたいです。youtubeにある立方体が立ち上がるロボットはこの方式を利用しています。

システムモデル

この振り子システムで支点にエアリアクションホイールが付いています。
支配方程式は

\begin{eqnarray}
J \frac{dW}{dt} +cW + mgsin\theta &=& T    :振り子  \\
V&=& Ri + KW_m            :モーター電気　インダクタンスムシ\\
Jm \frac{dW_m}{dt} + c_mW_m + c_{air} W_m &=& Ki:モーター機械\\
T&=& Ki - c_mW_m
\end{eqnarray}

J:振り子慣性モーメント
c:振り子粘性
W:振り子角速度
W_m:モーター角速度
Jm:モーター慣性モーメント
cm:モータ粘性係数
c_air:プロペラ粘性係数

この方程式の最後の発生トルクの式はかなり悩みました。
モーターに電圧を掛けると一瞬びくっとなるとおもいます。これは、無回転時の方が電流が流れるのとdw/dt が一瞬発生するからです。一定回転速度になるとトルクはほぼ発生しません。
合っているかわかりませんが、私の考えでは、
1:シャフトとハウジングの間にKi*iの磁気反発トルクが発生
2:シャフトとハウジングの間にそれぞれ反対方向に粘性摩擦が発生
3:シャフトの先に付いているプロペラにプロペラ抵抗トルクが発生
ハウジングを振り子に付けているので、振り子には1.2.が発生していると考えました。

\begin{eqnarray}
T&=& K*i - c_m*W_m\\
&=&Jm*(dW_m/dt)  + c_air *W_m
\end{eqnarray}

となります。リアクションホイールの場合はc_airがないので

\begin{eqnarray}
T&=& K*i - c_m*W_m\\
&=&Jm*(dW_m/dt) 
\end{eqnarray}

となり、発生トルクが$(dW_m/dt) $で決まり
エアリアクションホイールなら発生トルクが$c_{air} *W_m$で決定できます！！

システム同定

T= + c_air *W_m

と近似し、モーター側は電流I制御でシステムを分けました。
振り子は電流をステップ状に印かすることで同定すると

結構ノイズが乗ってますね。

dw/dt = -8.52467*w -98.6764*sin\theta +2.5969*I

と同定しました。電流上限より角度はかなり微小変位ですが。
自由振動させた場合

dw/dt = -0.55889*w -73.8621*sin\theta 

mg/Jは約70％落ち、cも同じぐらい低下しました。
オブザーバーを立てる際はこの係数が違うと違うところに収束する可能性があります。ただ、初期角を変えたりシミュレーションをRK4にしたりすると係数が変わるのである程度は諦めました。特にｃとKiは違う同定で2倍はずれがあったりします・・・

効果

現在角度をフィードバッグするのには、得られる角度に定常偏差が載っていたり、分解能が足りなかったり利用しても参考にならにないので、角速度をフィードバッグしてみる。これは振り子の支点の粘性が強まることと同意である。
I = -50 * 角速度　とすると

明らかに、収束が早まっている。縦軸は角度[rad]です。

GZ:角速度
Now:電流

電流は飽和しているので、フィードバッグゲインを下げて
I = -1 * 角速度
実験すると

こんな感じで、ここから自由振動時のパラメータ
c/J , mg/J を用い　Ki/Jの推定を行う。

倒立振子

問題は安定角度を知る方法です。どう頑張っても角度をフィードバッグすることは必ず必要です。2つ方法があります

• 振り子だということを利用してaxaygzより求める
• 同定したシステムを使用し、gz,近似微分器由来のdw/dtより求める。

最初の方法は定常偏差が載る
2つ目の方法は入力トルクの誤差の影響、dw/dtの信頼性、手で振り子を持ち上げていたり、外部干渉がある際は待ったこことなった値となる、　に問題がある。

しかし、定常偏差の乗った角度をフィードバッグしても安定させることはできても、入力が0に収束することができません。
そこで、入力についてサーボ系を立てることで0に収束させます。

オブザーバを作って、角度を算出する

dw/dt = -8.52467*w -98.6764*sin\theta +2.5969*I

この式を使い、オブザーバーを作って$\theta$を推測するシステムを立てます。wは計測できますが、同一次元オブザーバを立てます。

A = [-8.52 -98.6
      1     0 ];
      
B=[-2.596
   0];

V=[B, A*B ];
rank(V)

C=[1 0];

VO=[C
    C*A];
rank(VO)

D=[0];

x0=[0
    0.1]
sl = syslin('c',A,B,C,D,x0);
sl2= dscr(sl,0.01);              //離散化

t=0:0.01:5;
for    i = 1:501  do
    u(i)=0
    ,end


//フィードバッグゲイン計算
Q=eye(2,2);
R=1000;
X=riccati(sl2(2),sl2(3)*inv(R)*sl2(3)',Q,'d','eigen');
K=inv(R)*sl2(3)'*X;
F=sl2(2)-sl2(3)*K;


B4=[0 0]';
x2=ltitr(F,B4,u',x0);    //離散シミュレーション
y2=sl2(4)*x2;
plot(t,y2);
scf();

ure=-1*K*x2;
//plot(t,ure);
//scf();
//plot(t,x2);
//scf();

poles = [0.5         
         0.5];

H=(ppol(sl2(2)',-1*sl2(4)',poles))';  //オブザーバゲインH決定

AD=sl2(2);
BD=sl2(3);
CD=sl2(4);

uu=K*x0;
if uu> 1 then
      uu=1;
elseif uu<-1 then
      uu=-1;
end        
xx=x0;
//uに上限がある場合のシミュレーション
xxx=x0;
uuu=uu;
for    i = 1:500  do
    yy=CD*xx;
    uu=K*xx;
    if uu> 1 then
        uu=1;
    elseif uu<-1 then
        uu=-1;
    end        
    xx=AD*xx-BD*uu;
    xxx=cat(2,xxx,xx);  //どんどん列方向に　xxx　に　xx　を繋げていく
    uuu=cat(2,uuu,uu);
,end

plot(t,xxx);
scf();
plot(t,uuu);

これでシステムを離散化し、オブザーバーの極を変えながらオブザーバーを設計し、実際のシステムに導入すると
mbedのプログラムは

x1_n1= 0.9136737 * x1_n  - 0.9436121*x2_n - 0.024844*NowCurrent  + 0.90888*(GZ[0]-x1_n);
x2_n1= 0.0095701 * x1_n  + 0.995211 *x2_n - 0.0001261*NowCurrent - 0.250318*(GZ[0]-x1_n);

こんな感じで修正していくと

角速度はかなり一致する。
角度は最初逆応答していますが、これは最初に手で揺らしているためです。
結構良い感じでやれてます。
倒立振子でやる場合はオブザーバシステムも不安定となるので、上限下限を設定しておかないと発散し戻ってこないことになるので注意です。今は振り子でやってるので、対象システムは安定です。

極を0.3に下げると


あまり変わりませんが、手による逆応答時の変化が増加してますね。
あと、オブザ－バーは誤差をフィードバックして近づける方式上、真値と遅れが生じてしまいますが、角度にその影響が現れてます。

入力飽和

入力電圧電池4本では+-1.2A,8本では2Aまでしか出せないため
安定化できる初期角・初期角速度が決まってします。その境界はどのように求まるのでしょうか？
A+BKを安定行列にできるKのうち、Kxが上限にぶつかるxがその境界になるのでしょうか？

しかし、入力が飽和していても収束する可能性があります。入力が1で飽和している
初期状態が最悪の状態のとき、その後の入力は必ず良い方向へするために働くと考えられるので初期状態から発散方向に向かった場合、もう安定状態戻ってこないでしょう。

• 初期状態で飽和していないなら、収束する
• 飽和によって発散する場合は、初期状態から飽和している－＞入力は一定と仮定して良い
• そして、一度非飽和域に入って飽和することはない。ずっと飽和状態となる

(d/dt)x = Ax + Bu_{saturation}

という、$Bu_{saturation}$定数が乗った方程式になります。
これが安定と不安定の境界はx=constとなることです。よって、初期状態の境界は

x_0 = -A^{-1}Bu_{saturation}

となると予想します。どちらの方向に行けば安定化は不明です。
今回の倒立振子の場合、飽和入力を±1Aとしているので
w = 0,θ = ±0.0263268rad = ±1.5deg
となります。実際、Scilabのシミュレーションでもw=0のときθが0.26で発散安定の境になります。

正確に、0.26を入れていないので収束していますが、おそらく、正確に入れれば初期状態から変動しない結果になるでしょう。
先ほどの結果と比べると飽和している時間が延びているのがわかります。
ですが、θ=0でも初期wが大きければ復帰する事がかなわない事があります。
上の条件は

• 収束：const
• 入力飽和

が同時に起きる条件です。初期角速度があると、初期状態から変動するので計算できないのでしょう。
微分方程式から考えると

dw/dt + a*w + b\theta = 0

の安定条件は

a>0,b>0

となります。

I = \alpha *\theta + \beta*w トスルなら\\
8.52-Ki*\beta>0\\ -98.67 -Ki*\alpha >0\\

となります。

\beta < 3.3 \\
\alpha < -38.0\\

が安定条件です。入力飽和を考えない場合ですが・・・
難しかったので今度考えます。

倒立振子

とりあえず、先ほどのシステムの重力項にマイナスを付け、システム同定からの角度推定システム
最適制御で係数決定して

NeedCurrent = (theta)*(-78.982504)+GZ[0]*(-5.9103623);

としたところ倒立できました。
しかし、システム同定からの角度推定システムの出力と重力加速度・角加速度から導出した角度があまり一致していないので何故うまくいったかちょっと不明です。

thetaはシステム同定からの角度推定システム出力です。縦軸はdegです。
Rgは重力加速度・角加速度から導出した角度です。安定角偏差が存在します
Rはエンコーダ出力です。1deg単位でしか値を取れません。

            float SIN = 0.010134137*W2[0] + 0.0863901098 * GZ[0] - 0.026317869 * NowCurrent;
            //pc.printf("%8f ",SIN);
            if(SIN >=1) {
                SIN = 1;
            } else if(SIN<=-1) {
                SIN = -1;
            }
            float theta = asin(SIN);

でthetaを出しています。
システム同定からの角度推定システムの良いところは、重心を基準にシステムを組むので出力角度は重心基準の角度のため安定角偏差のようなエンコーダーや重力加速度・角加速度から導出した角度に付属するような定常偏差が存在しないという事です。

thetaとRgは定常偏差はあるが、大きく変動するところを除けばかなり形状（大まかな曲率）をしているように見える。

システム同定からの角度推定システムの入力項を除いた場合。あまり安定しなかった。手で最上点付近を往復させている。最上点で微振動した後落下してしまう

            float SIN = 0.010134137*W2[0] + 0.0863901098 * GZ[0];// - 0.026317869 * NowCurrent;
            //pc.printf("%8f ",SIN);
            if(SIN >=1) {
                SIN = 1;
            } else if(SIN<=-1) {
                SIN = -1;
            }
            float theta = asin(SIN);

こうすると、Rgとthetaは定常偏差があるがかなり一致している。

オブザーバを使用すると

x1_n1= 0.9229912 * x1_n  + 0.9467181*x2_n + 0.0249258*NowCurrent  + 0.927788 *(GZ[0]-x1_n);
x2_n1= 0.0096016 * x1_n  + 1.0047968*x2_n + 0.0001263*NowCurrent  + 0.2787629*(GZ[0]-x1_n);
NeedCurrent = (x2_n1)*(-78.982504)+GZ[0]*(-5.9103623);

一番安定していた気がする。

追記：本体改

角度センサを無くし、本体を変えてエアリアクションホイール型倒立振子を作りました。
https://www.youtube.com/watch?v=4Dyvws3sB8Q