倒立振子その1

また現れましたmisosoupsiteです。
皆さんは倒立振子というのはご存知でしょうか、倒立振子というのは簡単に言えば、いわゆる「逆立ちした振り子」です。よくあるものだと台車の上に棒が乗っていて下の台車を動かすことでバランスをとるなんてものがあります。たとえばセグウェイなんてものがありますがあれなんかは完全に人が乗るタイプの倒立振子です。
じつは以前にも倒立振子作ったのですが適当につくりすぎてあまりうまく立たなかったので、今度は運動方程式の導出からはじめてます。
今回作ろうとしているのは次のようなモデルです

%e5%80%92%e7%ab%8b%ef%bc%92

下の車輪が動くことで上の棒が倒れないようにします。

物理モデル まず以下のようなモデルを考えました。

タイヤ半径 r

倒立振子のy軸方向に対する傾きθ

モータによるトルクr(t)

振子の重心位置xg yg

タイヤの回転角 Φ

倒立振子の慣性モーメントJ

タイヤの中心から重心までの距離L

なおタイヤの慣性モーメント,重量は計算を簡単にするため無視しました(無視していいのかわかりませんが)。ここでラグランジュ法をもちいて運動方程式をたててみたいと思います。

まず並進運動エネルギーから


Emov = \frac{1}{2}m(\dot{xg}^2+\dot{yg}^2)

次に回転運動エネルギー

Ecyc = \frac{1}{2}J(\dot{\theta}^2)

ここでxg ygについては次が成り立ちます

xg = r(\theta+\phi)+L\sin(\theta)\\ yg=r+L\cos(\theta)

よって全体の運動エネルギーは次のようになります

E=\frac{1}{2}m((r(\dot{\theta}+\dot{\phi}))^2+2rL(\dot{\theta}+\dot{\phi})\dot{\theta}\cos(\theta)+L^2\dot{\theta}^2\cos^2(\theta)+L^2\dot{\theta}^2\sin^2(\theta))+\frac{1}{2}J\dot{\theta}^2

ラグラジアンLは

L=T-U=\\ \frac{1}{2}m((r(\dot{\theta}+\dot{\phi}))^2+2rL(\dot{\theta}+\dot{\phi})\dot{\theta}\cos(\theta)+L^2\dot{\theta}^2)+\frac{1}{2}J\dot{\theta}^2-mg(L\cos(\theta)+r)

これを用いてθ、Φについてラグランジュの運動方程式をたててみます
まずはθについて

\dfrac{d}{dt}(\dfrac{\partial L}{\partial \dot{\theta}})-\dfrac{\partial L}{\partial \theta}=\\ mr^2(\ddot{\theta}+\ddot{\phi})+mrL(-\sin(\theta))(2\dot{\theta}+\dot{\phi})\dot{\theta}+mrL\cos\theta(2\ddot{\theta}+\ddot{\phi})+mL^2\ddot{\theta}+J\ddot{\theta}+mgL\sin\theta\\ =0

次はΦについて

\dfrac{d}{dt}(\dfrac{\partial L}{\partial \dot{\phi}})-\dfrac{\partial L}{\partial \phi}=\\ mr^2(\ddot{\theta}+\ddot{\phi})+mrL(-\sin(\theta))(\dot{\theta}^2)+mrL\cos\theta(\ddot{\theta})\\ =r(t)

次にこれを線形化するためにθを十分に小さいとし以下のような近次を行います

\sin\theta=\theta\ \ \ \cos\theta=1

なお2乗以上の微分項は0としました

近似を行った結果が次です

\dfrac{d}{dt}(\dfrac{\partial L}{\partial \dot{\theta}})-\dfrac{\partial L}{\partial \theta}=\\ mr^2(\ddot{\theta}+\ddot{\phi})+mrL(2\ddot{\theta}+\ddot{\phi})+mL^2\ddot{\theta}+J\ddot{\theta}+mgL\theta\\ =0


\dfrac{d}{dt}(\dfrac{\partial L}{\partial \dot{\phi}})-\dfrac{\partial L}{\partial \phi}=\\ mr^2(\ddot{\theta}+\ddot{\phi})+mrL(\ddot{\theta})\\ =r(t)

ここで今回はステッピングモーターを用いるのでΦに関する変数を入力と仮定します。すると下のΦに関するラグランジュの運動方程式は無視することができ、上のみを考えます。ここで

mr^2+2mrL+mL^2+J=a

とおきます すると運動方程式は
\left( \begin{array}{ccc} \ddot{\theta} \\ \dot{\theta} \end{array} \right)= \left( \begin{array}{ccc} \ 0& -\frac{mgL}{a} \\ 1 & 0 \end{array} \right) \left( \begin{array}{ccc} \dot{\theta} \\ \theta \end{array} \right) + \left( \begin{array}{ccc} \frac{mr^2+mrL}{a}\ddot{\phi} \\ 0 \end{array} \right)

とりあえず長くなってしまったのでここまでにします。
次回は制御にいきたいのですが入力がΦの2階微分だとステッピングモーターだと厳しそうですね‥。どうすればいいか考えてるところです。
またいくら近似を多くしたとはいえほかで見かける運動方程式とくらべてずいぶんすっきりしてしまいました。本当にあっているのか正直間違っている気しかしないです。
誤りなどを見つけた方がいらっしゃったらコメントをいただければ幸いです.

広告

倒立振子その1」への2件のフィードバック

  1. ピンバック: 自分の作りたいロボット | 慶應義塾大学ロボット技術研究会

  2. ピンバック: PythonでTwitterAPIをいじろう [Advent2016] | 慶應義塾大学ロボット技術研究会

コメントを残す

以下に詳細を記入するか、アイコンをクリックしてログインしてください。

WordPress.com ロゴ

WordPress.com アカウントを使ってコメントしています。 ログアウト / 変更 )

Twitter 画像

Twitter アカウントを使ってコメントしています。 ログアウト / 変更 )

Facebook の写真

Facebook アカウントを使ってコメントしています。 ログアウト / 変更 )

Google+ フォト

Google+ アカウントを使ってコメントしています。 ログアウト / 変更 )

%s と連携中