見出し画像

倒立振子メモ

Ninagawa123

トランジスタ技術2019年7月号にある倒立振子をArduino MICROで作ろうとしたらすでに半年ほど沼にハマり続けているので、最新の状況をメモ。
メモをすぐなくすのでここに書いたり書き直ししながら指差し確認と更新していく考え。


本体の形状

画像1

画像2

トラ技というマニュアルもあるから簡単にできるかなと思ったけどそうでもなかった。
経験上いちばん厄介なモーターからのパルスノイズについては、黄色キャップの低回転モーターを使うことでなんとかなりそうである。
ロータリーエンコーダのところが課題で、結局3Dプリンタを使ったりしている。エンコーダ自体はマウス用の1個50円程度の部品。タミヤのダブルギアボックスの途中のギアからロータリーエンコーダー用の回転を取り出し、これをArduinoの割り込みで監視している。

画像3

モータードライバはTB6612を使用。ArduinoのPWM入力にも対応しているはず。
センサリングについては、GY-521をI2Cで読み取っている。センサの真値測定はMadgwickFilterというライブラリを利用している。
電源はとりあえずモーター用とArduino用で別系統。
ということで本体およびArduio側体制はだいたいメドが立ちつつある。

トラ技のアプローチは「現代制御理論」と「カルマンフィルタ」を使うもの。現代制御理論を使うには実物を正しくモデル化する必要がある。
そのモデル化のところがうまくできていない感じがあったので、これまで作業した分について改めて順を追って検証していく。

倒立振子の状態方程式に必要な定数の値

画像14

トラ技p57の表に準拠し、上記の必要項目を順に埋めていく。

本体の大まかな部品を実測 P.58

画像9

慣性モーメントをモデル化していくために、部品の一つ一つを地道に実測していく。
計算上の慣性モーメントは0.0014470なのだが、そこから「本体を等質的に1つの質点と見做した場合の、回転軸から質点までの距離」を求めると、0.075mとなる。実際の重心までの距離は0.0613m程度なのでここで既に大きくズレている。そもそも実物にはワイヤーなどが張り巡らされていてかなりいい加減にしてある。（ワイヤーの重量は制御基盤に含めている）

各パーツの実測値に合わせて本体側の形状の辻褄を合わせると、0.1207mという実物よりも低い高さの板の形状が算出される。実物が動きさえすればこれでもよいかもしれない。

また、p.59の rp=√(Ip/mp) を使えるのであれば、
Ip = rp^2 * mp で実測から割り出した慣性モーメント0.0011も有効かもしれない。

画像15

以上の実測等により、上記のように欄が埋まる。
次はモーターまわりを調べていく。

モータを実測する p.59-61

黄色キャップのタミヤ低回転型130モーターをモデル化していく。
通常のモーターではノイズの影響でArduinoが誤動作するはず。モーター側にパスコンを追加するなどのノイズ対策が必要になるが、この黄色キャップはノイズ対策済みの製品だ。

画像5

モータ回転子の慣性モーメント(p62)

画像6

分解したモーターのサイズと重量を計測し、回転子の慣性モーメントを求める。

Im = 1/2mrotetor * rrotator^2 …(34)
= 0.5*0.00751 * 0.0075* 0.0075 = 0.00000021

これは公式どおりなので特に問題ない。
大変なのは逆起電力測定のためのトルク実測である。

モータのモデル化 p.59

ブラシ付きDCモーターの要素を、
・回転子(rotor)
・固定子(stator)
・整流子(commutator)
・ブラシ(brush)
回転子の慣性モーメント Im
回転子の角度を θm
とすると、モーター回転子の運動方程式は下記となる。

ブラシ付きモータの運動方程式
     Im * θm.. = τG - τm -τ’m …(15)

回転子の慣性モーメント[Im] * 回転子の角加速度[θ..m]
     = モータが生み出すトルク[τG] - 負荷トルク[τm] - 摩擦トルク[τ’m]

モータが生み出すトルク [τG]　τG = kt*I …(16)

モータが生み出すトルク[τG] = トルク定数[kt] * 電流[I]
→ トルクは電流に比例する。

逆起電力定数 [kb]　Vb = kb * θ.m …(17)

回転子に生じる逆起電力 [Vb] = 逆起電力定数[kb] * 回転子の回転速度 [θ.m]
→ 逆起電力は回転速度に比例する。

ブラシ付きDCモータの等価回路
V = Rm * I + Vb = Rm* I + kb* θ.m …(18)

・Rm : モータにかかる抵抗をまとめたもの（巻線抵抗やブラシ抵抗など）
・V : モータ端子に加える電圧
・I : モータに流れる電流
とすると、
モータ端子に加える電圧[V] = モータにかかる抵抗[Rm] * モータに流れる電流[I] + 回転子に生じる逆起電力 [Vb]
= モータにかかる抵抗[Rm] * モータに流れる電流[I] + 逆起電力定数[kb] * 回転子の回転速度 [θ.m]

DCモータの式等価回路
τG = kt * I = kt(V/Rm)-kt(kb/Rm)θ.m …(19)

機械的な運動モデルと電気的な等価回路モデルを融合する。
モータが発生するトルクτGは、次式になる

τG = kt * I = kt(V/Rm)-kt(kb/Rm)θ.m
モータが生み出すトルク[τG] = トルク定数[kt] * 電流[I]
　　= トルク定数[kt] * (V/Rm -( kb* θ.m)/Rm )
　　　　　　　　　　　　↑ ここにI = V/Rm -( kb* θ.m)/Rm を代入し
　　= トルク定数[kt] * (電圧[V]/抵抗[Rm]) - トルク定数[kt] * (逆起電力定数[kb] * 回転子の回転速度 [θ.m]) / 抵抗[Rm]

この式(19) τG = kt * I = kt(V/Rm)-kt(kb/Rm)θ.m をモーターの運動方程式(15)Im * θm.. = τG - τm -τ’m に代入すると下記になる。

DCモータの式
Im * θ..m = kt(V/Rm) - kt(kb/Rm)θ.m - τm -τ’m …(20)　

Im * θ..m = kt(V/Rm) - kt(kb/Rm)θ.m - τm -τ’m
回転子の慣性モーメント[Im] * 回転子の角速度[θ..m]
= トルク定数[kt] * (電圧[V]/抵抗[Rm]) - トルク定数[kt] * (逆起電力定数[kb] * 回転子の回転速度 [θ.m]) / 抵抗[Rm] - 負荷トルク[τm] - 摩擦トルク[τ’m]

ギアボックスの効果

ギアボックスはギア比[n] によって、回転数が1/n倍、トルクがn 倍になる。
よって、DCモータの式は以下に書き換えできる。

ギアボックスの出力角度をθw、トルクをτwとすると、
θw = θm/n, τw = τm * n
Im * θ..w = kt(V/Rm) - kt(kb/Rm)θ.w - τw -τ’m
ギアボックスありのDCモータの式は上記になる。
ここにギア比nを入れてもどすと

回転子の慣性モーメント[Im] * n*回転子の角速度[θ..w]
= トルク定数[kt] * (電圧[V]/抵抗[Rm]) - トルク定数[kt] * (逆起電力定数[kb] * n*回転子の回転速度 [θ.w]) / 抵抗[Rm] - 負荷トルク[τw]/n - 摩擦トルク[τ’m]

Im * n* θ..w = kt * V / Rm - kt * kb * n * θ.w / Rm - τw/n - τ’m …(21)

両辺にnをかけて整理すると
n^2 * Im * θ..w = n*(kt/Rm)*V - n^2 * (kt*kb)/Rm * θ.w - τw - n*τ’m
　　　… DCモータの式（ギアボックス付き） …(22)

回転子の慣性モーメント[Im] * n*回転子の角加速度[θ..w]
= トルク定数[kt] * (電圧[V]/抵抗[Rm]) - トルク定数[kt] * (逆起電力定数[kb] * n*回転子の回転速度 [θ.w]) / 抵抗[Rm] - 負荷トルク[τw]/n - 摩擦トルク[τ’m]


モータのトルク定数測定 p.60

DCモータの式（ギアボックス付き）に
・θ..w = 0 → 角加速度がゼロ
・θ.w = 0 → 角速度がゼロ
を代入して、ギアボックスの出力軸が静止している場合の運動方程式を得る。

n^2 * Im * θ..w = n*(kt/Rm)*V - n^2 * (kt*kb)/Rm * θ.w - τw - n*τ’m … DCモータの式（ギアボックス付き） …(22)
↓
n^2 * Im * 0 = n*(kt/Rm)*V - n^2 * (kt*kb)/Rm * 0 - τw - n*τ’m
0 = n*(kt/Rm)*V - 0 - τw - n*τ’m …(23)

ここでモータが静止しているので、逆起電力はゼロになる。(17)
よってこの場合 V = I * Rmがなりたつので(23)と組み合わせると、
0 = n*(kt/Rm)*V - 0 - τw - n*τ’m　←V = I * Rm を変形し、Rm = V / I を代入
0 = n*(kt/(V / I))*V - τw - n*τ’m
τw = n * kt * I - n * τ’m …(24)
ギアボックスの出力トルク[τw] = ギア比[n] * トルク定数[kt] * 電流[I] - ギア比[n] * 摩擦トルク[τ’m]

よって、ギアボックスの出力トルク[τw] と* 電流[I] のグラフを書けば、
傾きからギア比[n] * トルク定数[kt] が求まる。
また、切片からギアボックスによる摩擦トルク(ギア比[n] * 摩擦トルク[τ’m])が求まる。

トルク定数kt および摩擦による負荷トルクτ'mの実測 p.61

水を入れたペットボトル（重量mbottle[kg]）をプーリー（半径rpulley[m]）で持ち上げる。
重力加速度 g[m/s^2]とすると、
ギアボックスの出力トルク[τw] = mbottle * g * rpulley …(25)
である。
モータが静止する時の電流値を実測する。

画像7

モータにある電圧を印加した時、水入りのペットボトルを重さを変えながらプーリで持ち上げ、釣り合ったところで流れている電流を計測していく。
通常の電流計では計測時に電流が下がって測定ができないのでクランプ型の電流系を用いる。

画像8

ほどほど線形の結果が得られたので線形近似すると、
ギアボックスの出力トルクτw[N•m]=0.0665 * モータ電流[A] - 0.0054
という式が求まった。
ここから、
トルク定数kt = ギアボックストルクnkt / ギア比n = 0.0665/38.2
= 0.001740 [N•m/A] …(26)
という結果が得られる。
また、
摩擦による負荷トルクτ'm = nτ'm/n=0.0054/38.2 = 0.000141 [N•m] …(27)
と求まった。

モーターの逆起電力定数を求める p.61

モーターの逆起電力定数kbを実験で求める。
負荷を取り付けずに一定速度で回転させている状態を考えると、

n^2 * Im * θ..w = n*(kt/Rm)*V - n^2 * (kt*kb)/Rm * θ.w - τw - n*τ’m … DCモータの式（ギアボックス付き） …(22)

に対して、
・θ..w = 0 → 角加速度がゼロ
・τw = 0 → 負荷トルクがゼロ
を代入すると、

0 = n*(kt/Rm)*V - n^2 * (kt*kb)/Rm * θ.w - 0 - n*τ’m …(28)
変形して、
θ.w = (1 / n *kb) *V - (Rm / (n * kt*kb)) * τ’m …(29)

上式より、モータに加える電圧Vと回転角速度θ.w の傾きから、(1 / n *kb)が求まる。
また、グラフのy切片から、(Rm / (n * kt*kb)) * τ’m より、Rmが求まる。

電圧と回転数の関係を実測 p.61

電圧と回転数の関係をArduinoで作ったカウンタを用いて計測していく。取り付けたロータリーエンコーダはギア比率 38.2:1の時に車輪1.53度毎の分解能がある。このギア比とロータリーエンコーダの関係であまりよい精度が出ていない可能性があり、その場合は117.7:1のギア比を使う。
回転数(rad/s)=8.0833 * モータ電圧[V]-3.5553
という式がもとまってはいるが、違う気がするので後で再計測したほうが良いかもしれない。

画像9

画像10


上記の回転数とモータ電圧の関係を表すグラフの傾きより、
1/nkb = 8.0833と求まった。

逆起電力定数kb は、
kb = 1/(8.0833*38.2) = 0.003238 V•s/rad

またグラフのy切片から
モーターの内部抵抗Rmの値が求まる。
Rm = n*kt*kb / τ'm • 3.5553 = (38.2 *0.00174 * 0.00324) / 0.000141
 = 1.5273 [Ω]

モーターの同定 p.62

使用しているモータが低回転型130モーターであることは確定しているが、
入手できたデータシートの停止時トルクおよび電流地からトルク定数ktを求めてみる。
26gm を単位変換すると 0.002549742N•m

kt = 0.002549742[N•m] / 1.23[A] = 0.002072[N•m / A]
実験で求めたのは0.001740 [N•m/A]なのでだいぶちがう気もするが、どちらも候補数字にできそうである。

続いて逆起電力定数kbをデータシートから求めてみる。
無負荷状態と最高効率状態のデータを、
モータ等価回路の式 V = Rm* I + kb* θ.m に当てはめて連立方程式にする。
無負荷状態　　3.0 = Rm * 0.16 + kb * 1015.78 rad/s
最高効率状態　3.0 = Rm* 0.7 + kb * 507.89 rad/s
kb = 0.002572
Rm = 2.41935

実験で求めた逆起電力定数kb = 0.003238 V•s/rad
実験で求めたモーターの内部抵抗Rmの = 1.5273 [Ω]

まあまあ近い値にはなっているかもしれないが、
そもそもデータシートから逆起電力定数kb、内部抵抗Rmが求められると今知った。

画像16

ここまで取得してきた情報をまとめるとこのようになる。

モータードライバをモデル化する p.62

モータードライバーにはブレイクアウトボード付きのTB6612FNGを使用している。
内部回路について少し調べてみる。

画像17

入力と出力のところにMOSFETが使われているらしい。
MOSFETの働きや回路についてよく知らないので、この際勉強してみる。

画像18

入力側を例にとると、おそらく上記のような働きになっている。
入力されたHI-LOW信号に対して、MOSFETが極を反転しつつ、安定した結果を出力していると思われる。
ただし、ここから先の実測はちょっと難しい感じがする。
マイクロマウス系らしいロボコン日記　モータの特性を調べる(4) ～　動特性(PWMキャリア周期について）によると、TB6612FNGの内部抵抗を0.5Ωとして処理しているようなので、今回必要であればその数値を借用することにする。

大事なのはトータルの実測値であろうということで、下記の測定を行った。

Arduinoの電圧信号指定とモータードライバーからの出力電圧の関係

Arduinoからモータードライバー（TB6612FNGブレイクアウトボード）のPWM入力に0-255の値でanalogWriteを行った際、モーター未接続の状態でモーター側への出力電圧がどうなるかを測定してみる。
TB6612FNGのデータシートはこちら

画像11

モーターへの出力電圧 = analogWrite値 * 0.0201 + 0.0171
となったが、モーターを接続すると同様にはいかないので悩ましい。

たとえば片側に接続、もう一方に接続せずに電圧測定、とした場合に、測定値がモータの動きに応じて変化してしまうという課題。
また、NT値と車輪が回り始める最小電圧値を見ると、左右で最大0.48Vも差がある。
重いのはロータリーエンコーダをつけている方。これもうまく動かない原因の一つである。

画像14

最小電圧値Voffsetについては、仮に上記の平均の0.6553Vとしてみる。

倒立振子の状態方程式における定数の値

概算もあるが現状でわかってきた数値をまとめる。

画像18

いちおう総ての必要項目が埋まった。いちおう…

このあと

低電圧時のギアボックスの挙動がかなり惜しい感じなので、若干心が折れそうになっている。この処理をハードソフト両面からなんとかしたい。
トラ技の事例では、モータが260型ではあるが、タミヤのギアボックスでもちゃんと動いている。
また、制作途中段階で試作した、モデル抜きの倒立振子は、調整努力次第では立ちそうな状態になっている。つまり、ハードウェア的なポテンシャルはあるとも言えそうである。


低回転130モーター＋ダブルギアボックス＋arduinoの先行事例があればぜひ参考にしたい。
コード上の不安もあるので、一旦物理エンジン上で試してみるというのも手かもしれない。


この記事が気に入ったらサポートをしてみませんか？