「pendulum」タグアーカイブ

M5StickC で倒立振子 Blynk でコントロール ー倒立振子への道 4ー

前回、実現できたM5StickCによる倒立振子を前進/後進/旋回動作できるようにいたしました。

M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー

倒立振子自体の制御は前回と同様にPID制御です。タイヤの回転などを加味したより安定した制御方法は現在勉強中です。いつまでかかるかな。。。?

 

動作

早速動作を見てください。

スマホアプリBlynkを使用してBluetoothでコントロールしています。なかなかかわいいです♪

Blynk設定

前回のアプリ設定にジョイスティックウィジットを追加しただけです。

 

ジョイスティックの設定はx軸とy軸の出力をMERGEしてヴァーチャルピンV4に出力させ、それぞれ値は-5~5としました。y軸の値で前進/後進をx軸で左右旋回動作します。値が大きくなるほど動作速度が上がります。

Arduino IDEプログラム

前回とほぼ同様のコードです。カルマン・フィルタによって傾斜角と角速度の推定値を導出してPID制御によってモータをフィードバック制御しております。

前回コードとの主な差異

  • L421 – 435
    旋回動作を実現させるためにモータドライバの制御を左右のモータで関数を分離しました。これにより左右のモータを異なる動作の指示ができます。
     
  • L459-476
    ジョイスティックの値をヴァーチャルピンV4で受けます。
    y軸の値で前進/後進、x軸の値で旋回動作させます。
     
  • L520, 521
    推定された傾斜角(theta_data[0][0])にジョイスティックy軸の値を足すことで倒立振子の前進(+y)と後進(-y)を実現させています。
     
  • L526, 527
    PIDを係数かけてモータ出力を導出する前に比例項にジョイスティックx軸の値を足すことで倒立振子の旋回動作を実現します。左右それぞれx軸の値を反転させて(L469, 470)足して旋回動作させています。

 

おわりに

PID制御でも動作コントロールできることを確かめました。しかしまだ外乱には弱いですしもっと安定した振子にしたいです。でもロータリエンコーダは使用しないカジュアルな倒立振子にしたいものです。

折角M5StickCが小さくて半端ない集積でもってカワイイのですから。粛々と勉強を進めます。

追記

久々に動かしてみた (2020/3/19)

M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー

さて前回まででカルマン・フィルタを使用したM5StickC傾斜計を作りました。
いよいよM5StickCによる倒立振子を作っていきます。

この道シリーズではトランジスタ技術 2019年 7月号を参考に倒立振子の実現を目指しています。

トラ技の倒立振子はカルマンでの傾斜推測に加えてロータリエンコーダでタイヤの角度、角速度検出して倒立振子自体のモデルで更にカルマン・フィルタかまして安定度の高い倒立振子を実現しています。モータへの電圧供給フィードバックもなんか難しい関数で算出されています。

しかし、今回はM5SickCを用いたカジュアルな倒立振子の実現を目指します。ロータリエンコーダは使わず、さらにモータへの供給電圧は難しい関数は使用せずに馴染みのあるPID制御で算出しました。

 

 

まずは作ってみた

とりあえず手を動かさないことには始まらないということで、倒立振子を作ってみた。

 

やっぱ何も考えずに適当につくった倒立振子はどうあがいても全然倒立しませんでした。。。

頑張って改良した

いろいろ工夫してついに立ちました!!見てください!

 

立った!立った!立ったー!やりぃー!キャッホー!!うれCY!!

 

さていかにして、この倒立振子が実現されたかを以下に記します。

 

構成

I2CモータドライバDRV8830をM5StickCで制御してモータを駆動します。

部品

マシン

以下が自慢のマシン詳細です。

 
いろいろ試して気づいたのですがマシン自体のバランスが非常に重要です(あたりまえか)。底面に鉄板貼り付けてバランス調整しています。

PID制御

今回作成した倒立振子ではM5StickCの傾きθと角速度ω ($\dot{θ}$)をカルマン・フィルタをかまして推定値を算出して、その値を元にモータ供給電圧を決定して姿勢を床に対して垂直に保つようにします。

ここではPID制御を用いて供給電圧を決定します。

PはProportional(比例)、IはIntegral(積分)、DはDifferential(微分)です。

$$操作量 = Kp × 誤差 + Ki × 誤差の積算値 + Kd × (前回の誤差と今回の誤差の差)$$

上式でモータ供給電圧を決定します。誤差は目標値と現在の傾きの差で今回は垂直(0°)を目指しますので誤差は傾きθ(θ – 0°)となりますので

$$モータ供給電圧 = Kp × θ + Ki × \sum{θ} + Kd × ω$$

係数Kp, Ki, Kdを調整して、過度な供給や供給不足のない適切な電圧の供給を実現します。今回はBluetoothでM5StickCと通信してスマホで係数を送れるようして地道に手動で調整しました。

参考

6軸センサ SH200Q調整

比較的 高速で繊細な観測が要求されますのでM5StickCに搭載されている6軸センサ SH200Qの設定値を調整しました。

SH200Q用のクラスを修正して以下を変更しました。

  • 加速度センサ出力周期:256Hz → 1024Hz 
  • ジャイロセンサ出力周期:500Hz → 1000Hz 
  • ジャイロセンサLPF遮断周波数:50Hz → 250Hz
  • 加速度センサレンジ:±8G → ±4G
  • ジャイロセンサレンジ:±2000°/sec → ±250°/sec
     

また出力レンジの変更に伴いヘッダファイルも修正しました。

 

Blynk設定

PID制御用の係数Kp, Ki, Kdとモータ供給電圧の最小値Vminをスマホから送れるようにします。

スマホとM5StickCはスマホアプリのBlynkを用いてBluetooth通信させます。Blynkアプリのバージョンは2.27.6です。

新規プロジェクトを作成します。HARDWRE MODELはESP32 Dev Boardを選択。CONNECTION TYPEはBluetoothを選択。AUTH TOKENはArduinoコード生成時に使用します(アカウント登録したメールに送信されます)。

 

ウィジェットとしてBluetoothとNumeric Inputウェジットを4つ配置します。Numeric Inputウェジットは±ボタンを押して値を増減させて送信できます。
 

Numeric Inputウェジットの設定は最小値と最大値、ステップ数を指定してヴァーチャルピンに出力させます。以下のように設定しました。

  • Kp:-1000~1000、0.1ステップ、出力ヴァーチャルピンV0
  • Ki:-1000~1000、0.001ステップ、出力ヴァーチャルピンV1
  • Kd:-1000~1000、0.01ステップ、出力ヴァーチャルピンV2
  • Vmin:0~255、1ステップ、出力ヴァーチャルピンV3

I2Cモータードライバ DRV8830

DRV8830はI2C入力によって、モータ供給電圧(スピード)と供給電圧方向(回転方向)を制御するものです。つまり正負の電圧を生成できるということです。

I2Cアドレス設定

本モジュールは基板上のジャンパA1, A0のステートによって以下の表のようにアドレスを指定することができます。

表のアドレス末尾xは読み込み時には1、書き込み時は0をしてします。ここでは書き込みのみ使用します。両方Openのアドレス:0x64としました。

書き込みI2Cデータ

アドレス0x00に8bitの情報を書き込みます。
各ビットの設定は以下の通り。上位6ビットで電圧を設定し、下位2ビットで正負を指定できます。

Arduino IDEプログラム

以下のBlynkのArduino用ライブラリを使用してプログラムしました。バージョンは0.6.1。
 https://github.com/blynkkk/blynk-library

ここではBluetoothを使用してM5StickCと通信します。ライブラリのコード例のESP32_BT.ino を参考にプログラムしました。

カルマン・フィルタによる傾斜角と角速度の誤差の推定値を元にPID制御によってモータに電圧を供給して姿勢を保ちます。

タイマ割り込み(2.5msec毎)でカルマン・フィルタ介して傾斜角と角速度の誤差を算出します。角速度に角速度の誤差を足して角速度の推定値としています。

10msec毎に供給電圧を計算してモータ駆動します。

初期設定時にセンサの測定値のオフセットを測って差し引くようにしてます。

 

参考

倒立振子によるPID制御のコードは以下を参考にさせていただきました。

動作

調整中の様子。

 

起動すると初期設定(LED点灯中)がはじまり終了すると、倒立動作が開始します。BluetoothでBlynkとつながるとディスプレイが赤から緑にかわり各種設定値を送信できます。

動きをみながら地道に調整を進めます。。。

上記コードでVMIN = 1、Kp = 0.4、Ki = 0.1、Kd = 0.2でだいたい安定しました。外乱には非常に弱いですが。。。

とりあえず倒立するマシンができたので次はカット&エラーではなくトラ技の制御法を取り入れて
もっと安定性の高い倒立振子を目指します!

傾斜計 カルマン・フィルタ アルゴリズムの解きほぐし ー倒立振子への道 2ー

前回トラ技を参考に作ったM5stickCを用いた傾斜計ではカルマン・フィルタの効能を堪能しただけで、そのカルマン・フィルタが何をしているかはすっ飛ばして見てみぬフリをしておりました。

カルマン・フィルタで M5stickC 傾斜計 ー倒立振子への道 1ー

ここでは前回用いたコードのカルマン・フィルタ アルゴリズムを少し解きほぐし何をしているのかをみてみたいと思います。

しかし内容がかなり難しくここでも様々な点をすっ飛ばさなくてはなりません。カルマン・フィルタが制御工学や統計学など様々な基礎学問の上に成り立っているからです。

でも気にしてはいられません。この道の目的はM5stickCを用いたオリジナルの倒立振子の実現です。今から統計や制御理論を1から学ぶのは得策ではありません。挫折するに決まっています(お父ちゃんの場合)。

漫画家見習いが鉛筆の素材の木を植えるところから始めるわけにはいかないのですから。

 

 

傾斜計コード

前回のコードを改めて記載いたします。

トラ技のソースコードInclinometer.cppをほぼ丸々参考にしています(p. 49 – 52)。

ここではカルマン・フィルタ アルゴリズム処理 update_theta()を解きほぐします。

カルマン・フィルタの概要

カルマン・フィルタは”センサの測定値”と”モデルから算出した予測値”を元に真値を推定するフィルタです。下は傾斜計の場合のブロック図です。

実際どういった処理を施して傾斜角の推定値を得ているのかコードより解きほぐしてみましょう。

まずはフィルタに導入する測定値と予測値についてみていきましょう。

センサによる測定値

M5stickCに内蔵されている6軸の慣性センサ(加速度、ジャイロ) SH200Q を使用して傾斜角と角速度を測ります。

傾斜角θは加速度センサから重力加速度の傾きを検出して導出します。

角速度はジャイロセンサの値(x軸)をそのまま使用します。

数理モデルによる予測値

予測値を得るために物理現象を記述する式(数理モデル)を立てます。

傾斜角の関係式は以下の通りです。

$$θ_{k+1} = (ω_k – ω_{offset})・Δt + θ_k$$

$θ_k $は時刻kのときの傾斜角で$ω_k $は時刻kのときの角速度です。$w_{offset}$は角速度のオフセット誤差です。
時間kのときの角速度に微小時間$Δt $(コードでは2.5msec)をかけて$θ_k $を足すと$Δt $後(時間 k+1)の傾斜角$θ_{k+1}$が得られるということを示しています。

またここで簡単のため角速度(ジャイロセンサ)のオフセットは時刻によらず一定と仮定します。

$$ω_{offset}(k+1) = ω_{offset}(k)=ω_{offset}$$

ここですっ飛ばしブチかましますが、現代制御理論では数理モデルを行列で表現する習わしがあるそうです。

状態方程式

物理状態の移り変わりを示すのが状態方程式です。上記2式より以下で表せます。

$$\left( \begin{array}{c}  θ_{k+1}\\ ω_{offset} \end{array} \right)=\left( \begin{array}{cc}  1&-Δt\\0&1 \end{array} \right)\left( \begin{array}{c}  θ_{k}\\ ω_{offset} \end{array} \right)+\left( \begin{array}{c} Δt\\ 0 \end{array} \right)ω_k$$

一般式では以下のように書きます。

$$x_{k+1}=A・x_k+B・u_k$$

ここでの物理状態$x$ ($θ_k$と$ω_{offset}$)は行列Aをかけて2項目の入力$ω_k$(角速度)に関する値を足すと次の状態があられることを示しています。

出力方程式

物理状態で実際に観測できる状態(値)を示す行列式です。

$$θ_k=\left( \begin{array}{cc} 1&0\end{array} \right)\left( \begin{array}{c}  θ_{k}\\ ω_{offset} \end{array} \right)$$

$θ_{k}$だけがセンサから直接得られることを示しています。$ω_{offset}$だけを得ることはできませんのでね。

一般式は以下で書きます。

$$y_k=C・x_k$$

以上で数理モデルが用意できました。また行列A, B, Cは時間依存のない定数でした。

 

カルマン・フィルタ アルゴリズム

やっとコードのカルマン・フィルタ アルゴリズムupdate_theta()が何をやっているかをみてみます。かなりすっ飛ばします。とにかくここではコード上でどんな処理をしているかをみるのが目的です。よく理解できないし。。ここでは丸飲みします。

update_theta()は以下の5つの処理を実行しています。

  1. カルマン・ゲイン$G_k$の算出
  2. 現時点kの推定値$\hat x_k$の算出
  3. 現時点kの共分散行列$P_k$の算出
  4. 次時刻k+1の予測値$\tilde x_{k+1}$の算出
  5. 次時刻k+1の共分散行列$P’_{k+1} $の算出

1. カルマン・ゲイン算出

カルマン・ゲイン$G_k$とはモデルによる予測値$\tilde x_k$とセンサによる測定値$y_k$を混ぜ合わせて推定値$\hat x_k$を求める際の重みづけ配分比を意味します。

そして詳細は分かりませんが$G_k$は以下の式で得られます。

$$G_k=P’_k C^T(W + C P’_k C^T)^{-1}$$

   $P’_k$:前時刻に算出された共分散行列(5. で算出)
   $C^T$:出力方程式の係数行列C = $\left( \begin{array}{cc} 1&0\end{array} \right)$の転置。つまり$\left( \begin{array}{c} 1\\0\end{array} \right)$
   $W$:観測雑音の程度を表す量。つまり加速度センサによる角度の分散(theta_variance)

2. 推定値算出

前時刻に算出された予測値(4. で算出)$\tilde x_{k}$と測定値$y_k = θ_{k}$からカルマン・ゲイン$G_k$を絡めて推定値$\hat x_k$を求めます。

$$\hat x_k=\tilde x_k+G_k(y_k – C \tilde x_k)$$

3. 共分散行列算出

共分散行列とはデータをxとyとすると$\left( \begin{array}{cc}  {σ_x}^2 & {σ_{xy}}^2\\ {σ_{xy}}^2 & {σ_y}^2\end{array} \right)$で表されます。

要するにデータをxとyの変動の相関を表します。
データxとyに相関がない場合は分散$ {σ_{xy}}^2 = 0$となります。

物理状態$θ_k$の共分散行列$P_k$は以下で表されます。例によってなんでかは知りません。

$$P_k = (I_2 – G_k C) P’_k$$

   $I_2$:2行2列の単位行列$\left( \begin{array}{cc} 1&0\\0&1\end{array} \right)$
   $G_k$:カルマン・ゲイン
   $C$:出力方程式の係数行列C = $\left( \begin{array}{cc} 1&0\end{array} \right)$
   $P’_k$:前時刻に算出された共分散行列(5. で算出)

4. 次時刻の予測値算出

モデルを用いて次時刻k+1の傾斜角予測値$\tilde x_{k+1}$を算出します。

$$\tilde x_{k+1} = A \hat x_k + B \bar u_k$$

   $A$:状態方程式の係数行列$\left( \begin{array}{cc}  1&-Δt\\0&1 \end{array} \right)$
   $B$:状態方程式の係数行列$\left( \begin{array}{c}  Δt\\0\end{array} \right)$
   $ \hat x_k$:現時刻の傾斜角推定値(2. で算出)
   $ \bar u_k$:入力(角速度)の平均。ここでは現時刻の角速度測定値$ω_k$で近似。

5. 次時刻の共分散行列算出

これもよくわかりませんが次時刻k+1の共分散行列は以下で得られます。

$$P’_{k+1} = A P_k A^T + B U B^T$$

   $P_k$:現時刻に算出された共分散行列(3. で算出)
   $U$:入力雑音の程度を表す量。つまり角速度の分散(theta_dot_variance)

カルマン・フィルタの初期設定

カルマン・フィルタの処理は前回算出した予測値と共分散行列を用いて推定を行います。常に前のデータを必要とするので最初に起動(k=0)する際には手動で初期値を指定する必要があります。

アルゴリズムの4. と5. で算出する次時刻の予測値が時刻0の時点ではないのであらかじめそれらしい値を決める必要があるということです。

コードでは初期の傾斜は直立であると条件付けして初期値を与えています。

状態予測値の初期設定

$\tilde x_0 = \left( \begin{array}{c}  θ\\ ω_{offset} \end{array} \right)  = \left( \begin{array}{c}  0\\ \bar ω \end{array} \right)$

直立を仮定しているため $θ = 0$、 動かしていないと仮定して$ω_{offset}$をジャイロの平均値としています。

共分散行列の初期設定

$P’_{0} =\left( \begin{array}{cc}  {σ_θ}^2&0\\0&{σ_{ω_{offset}}}^2 \end{array} \right) =\left( \begin{array}{cc} 1&0\\0&{σ_ω}^2 \end{array} \right)$

角度のばらつきは1°程度と想定し、$ω_{offset}$のばらつきはジャイロの測定値のばらつきとしています。傾斜角と$ω_{offset}$に相関はないので対角の値は0です。

初期値の値は多少ずれがあっても時刻が進み測定データが増えると推定値は真値付近に収束するようです。

 

おわりに

傾斜計コードで使用したカルマン・フィルタ アルゴリズムの処理内容を追いかけました。

数式などは分からないことが多々ありますが、確かにその処理をしていい感じの結果を得ているのです。

とにかくカルマン・フィルタ アルゴリズムを飲み込んで数理モデル方程式を立てて初期値の設定をそれらしく実施すればよいとのことなので応用が利くことでしょう。

さて次は倒立振子に向けて動きたいと思います。

カルマン・フィルタで M5stickC 傾斜計 ー倒立振子への道 1ー

今月のトランジスタ技術が非常に興味深い内容でした。

 

確率統計コンピューティング特集ということでカルマン・フィルタの歴史とカルマン倒立振子の作り方を非常に丁寧に紹介しています。

必要な数学や力学の基礎も記載されておりロボット制御の入門として最適なのではと思います。DVDでのセミナーも易しくありがたい限りです。

はっきり言ってお父ちゃんはカルマン・フィルタとは何ぞやというレベルで全く知識のない状況です。そこで倒立振子への道と題してM5StickCを用いた倒立振子の実現を目指して一歩ずつお勉強していこうと思います。

ここではトラ技のコードを参考にカルマン・フィルタを用いた傾斜計を作ってみたいと思います。カルマン・フィルタの素晴らしさをまずは体験してみようと思います。

 

 

M5stickC

M5stickCには6軸の慣性センサ(加速度、ジャイロ) SH200Q が内蔵されていますので、このセンサ値を用いてM5stickC自身の傾きを算出してみます。

6軸センサ SH200Q

SH200Q データシート
 https://github.com/m5stack/M5-Schematic/blob/master/Core/SH200Q.pdf

SH200QのArduino IDEサンプルコードは以下にございます。
 https://github.com/m5stack/M5StickC/blob/master/examples/Basics/SH200I/SH200I.ino 

M5StickCのXYZ軸方向は以下の通りです。

参考:144Labグループ開発者ブログ

センサのデフォルトの設定は以下の通りです。

  • 加速度:16bit ±8 g
  • ジャイロ(角速度):16bit ±2000 deg/sec

 

上の写真のようにM5stickCを置いてSH200Q のサンプルコードを動かすとSH200Qから出力されるデータは以下のようになるはずです。

  • 加速度:AccX = 0,  AccY = 0, AccZ = 4096(+1g)
  • 角速度 : GyroX = 0, GyroY = 0, GyroZ = 0

しかしセンサのオフセットのため実際に出力される値にはずれがありましたので、今回はあらかじめオフセットを測って引いて使用します。

カルマン・フィルタを用いた傾斜計に関してはセンサのデフォルトオフセットの影響はないかもしれませんが今回は生データとの比較も行いたいのでオフセット引いております。

カルマン・フィルタによる傾斜導出の概要

ここではカルマン・フィルタの詳細理解はすっ飛ばして とにかく使ってみてご利益を実感するのを目的としています。ですので詳細はトラ技や他に譲るとしてざっくりとした概要です。

カルマン・フィルタは測定値と数理モデルによる予測値をうまいこと利用してもっともらしい傾斜角を導出します。

加速度センサによる角度

重力加速度のY軸、Z軸の成分よりM5stickCの傾斜角θは以下の式で求められます。

$$θ = tan^{-1}\left(  \dfrac{g_z}{-g_y} \right)$$

しかし加速度センサは振動などの重力加速度以外の加速度も検出してしまう問題があります。

ジャイロセンサによる角度

ジャイロセンサのx軸の角速度$ω_x$を時間積分することでM5stickCの角度を求めることができます。

しかしジャイロは初期姿勢の角度は検出できません。さらにセンサ測定値にはオフセット誤差が含まれるので誤差も積分されて増幅していきます。

モデルによる予測

上記のそれぞれ短所をもった測定値とそのバラつき誤差を加味した数理モデルをつくって傾斜角度の予測値を算出します。

測定バラつきとして予めセンサを複数回測定(ここでは100回)して分散を求めます。

モデルの詳細はまだ理解できてないので割愛します。いつか理解できるその日まで。。。

カルマン・フィルタのアルゴリズム

上記の測定値とその分散そして数理モデルによる予測値をうまいこと用いて角度の推定値を得ます。

内容は全く分からないので後に回します。トラ技紙面に譲ります。。今は分からない。。。今は。。。

傾斜計コード

Arduino IDEをもちいでカルマン・フィルタM5stickC 傾斜計を作製しました。比較のために加速度センサによる傾斜角とカルマン・フィルタによる推定値を出力します。

トラ技のソースコードInclinometer.cppをほぼ丸々参考にしています(p. 49 – 52)。