コンテンツへスキップ

フィラメント菅時計

時計って家にあります? 私,引っ越したので無かったんです。

PCかスマホ見ればいいですし,時計を持ってない人は多いかも。 しかし,朝はギリギリまで寝ているので,スマホをわざわざポケットから取り出す時間すら勿体なかったりします。

時計が欲しい! しかもなるべくカッコイイ時計が欲しい!

そこで,欲求を整理した結果,自分が欲しい時計がamazonでは見つかりませんでした。なんてこった!! そんなわけで時計を作りました。

作ったの半年くらい前なんですけどね。作り方をざっくり紹介します。

ガラスの加工がうまくいかなかったのが残念。

目次

  • 欲求の整理
  • 全体構想と仕様追加
  • 開発環境
  • 回路の部品一覧
  • 回路とソースコード
  • タイマモジュールの使い方
  • GPSの使い方
  • IOエキスパンダ―のつなぎ方
  • フィラメント間の使い方
  • 木枠
  • 回路発注
  • 完成!
  • 雑記

欲求の整理

時計を作るにあたって,求める機能を羅列していきます。

  • 見た目をカッコよくしたい → 木材で枠を作る。フィラメント菅を使用。
  • 電池交換したくない → コンセントから給電
  • 手動で時間を合わせるのが面倒 → 電波等で時間を公正

ニキシー菅を使って作りたかったけど,ニキシー菅はかなり高電圧らしい。5Vで作れた方が安心なので,フィラメント菅に変更しました。敗北 +1。

全体の構想と仕様追加

また,基本的な部品はこれを使用。これにGPSをくっつけます。 6桁表示・フィラメント管時計キット -nixie-tube.com さん

コントローラ(マイコン)部分は私が慣れているRaspberry Pi Picoを使用します。

全体の構成はこんな感じ。

電波で公正ですが,なるべく簡単に作りたいという気持ちがあり,GPSでやります。

GPS受信キット 1PPS出力尽き 「みちびき」2機受信対応

GPSから時刻を取得して時刻表示を更新するタイミングは下記にします。

  • 電源投入時
  • スイッチ(SW)押下時
  • 内部で持っている時刻が12時0分0秒の時

開発環境

  • 開発PC: windows
  • 開発対象: Raspberry Pi Pico
  • 言語: MicroPython
  • 開発環境: Thonny
  • その他: Mini GPS_r1.20 (GPSの設定に使う)
  • その他: Power GPS_r1.00 (GPSの設定に使う)

回路の部品一覧

NAMEQuantitymemoURL
Raspberry Pi Pico1https://akizukidenshi.com/catalog/g/gM-16132/
IV-96フィラメント菅https://nixie-tube.com/shop/1_76.html
5V2A11Aで十分https://akizukidenshi.com/catalog/g/gM-01801/
DCジャック12.1mmhttps://akizukidenshi.com/catalog/g/gC-09408/
タクトスイッチ4色は何でもhttps://akizukidenshi.com/catalog/g/gP-03647/
ボタン電池11.5Vhttps://www.amazon.co.jp/dp/B003X5WJR6?tag=amz-mkt-edg-jp-22
電池ホルダ(LR44用)1https://akizukidenshi.com/catalog/g/gP-08208/
ICソケット18pin1https://akizukidenshi.com/catalog/g/gP-00008/
ICソケット8pin1https://akizukidenshi.com/catalog/g/gP-00017/
ICソケット28pin3https://akizukidenshi.com/catalog/g/gP-00013/
DS1307+1RTC(タイムキーパー)https://akizukidenshi.com/catalog/g/gI-06949/
MCP230173I/Oエキスパンダーhttps://akizukidenshi.com/catalog/g/gI-09486/
水晶発振子32.768kHz1https://akizukidenshi.com/catalog/g/gP-04005/
10kΩ5https://akizukidenshi.com/catalog/g/gR-25103/
1kΩ2I2Cバス用https://akizukidenshi.com/catalog/g/gR-25102/
15Ω6https://akizukidenshi.com/catalog/g/gR-09429/
0.1µF6https://akizukidenshi.com/catalog/g/gP-00090/
M3ねじ適量
スペーサー適量
GOS受信機キット1https://akizukidenshi.com/catalog/g/gK-09991/
ボタン電池1予備https://amzn.to/3oIKaik
基板1PCBwayでプリントしてもらいました。

過不足あるかもしれないけど御愛嬌。

回路とソースコード

私のgithubで公開したはず

回路は watch.pdfです。

タイマモジュール

DS1307+を使います。どうやら内部のメモリに格納された値が時間の初期値で,そこからカウントアップしていく感じらしいですね。

時間を合わせるには,このメモリに時間を上書きしてあげます。

ドライバーですが,Micropythonで簡単に使えるライブラリがgithubに上がってるので使わせていただきます。

書き込みなどは下記のgithubの関数を使うのが簡単です。

micropython-tinyrtc-i2c -github

使い方は簡単,「ds1307.py」をクローンして,picoに保存してあげましょう。それで準備完了。あとは main.py 内でインクルードしたりして,普通に関数をコールすればOK。

下記も参照。

注意点ですが,タイマモジュールに時間を書き込むときは2進化10進法表記の整数にしないといけません。この後出てくるGPSはASCIIで時刻を取得するので,変換してあげましょう。

なお,メモリの値は,電源供給が絶えると消えてしまいます。が,私の設計ではGPSで取得して上書きするので,電池追加等はしていません。

GPS

下記を設定

  • 5Hzで時刻を受信
  • GPZDAモード

GPZDAモードは日付と時刻だけが流れてきます。受信データはASCIIコード。 ホットスタンバイで,ずーっと受信します。消費電力など知るか。

仕様について。下記の3つの条件が一つでも満たされたとき,GPS情報を読み込むように設計しました。

  • 電源投入時
  • タイマモジュール が保持している時刻が0時0分0秒のとき
  • push switch が押下されたとき

GPSの使い方

GPSは色々と受信できます(位置情報とか)が,日付と時刻のみのモードに設定します。ボーレートや更新周波数も設定します。

GPS受信キット 1PPS出力尽き 「みちびき」2機受信対応

秋月のサイトに,GPS設定用ソフトのリンクが張ってあります。

受信するデータの形状はこんな感じの33文字です。ただし,時間は本初子午線基準なので,日本時間にするためには 9 時間足してあげる必要があります。

$GPZDA,131003.400,28,10,2022,,*5B

githubで共有した302行目でGPSを読み込んでいます。 次の行のcorrect_time(buff, rtc_write)の引数ですが,buffは302行目で読み込んだ値,rtc_writeは書き込み先(つまりタイマモジュールに対応するオブジェクト)です。

IOエキスパンダ―

GPIOピンを増やすICです。下記のようにI2Cで結線してあげればOKです。

I2Cの結線
回路一部抜粋

A0, A1, A2 の3ピンでスレーブアドレスを指定できます。Highが1, Lowが 0で,3bitなので最大で8個のMCP23017が繋げるってことですね。 (A0, A1, A2)=(0,0,0)の時,アドレスは0x20になります。

フィラメント菅の使い方

※フィラメント菅の回路図記号が分からなかったので(存在するんですか?)代わりに7セグで作りました。

回路一部抜粋

フィラメント菅には制御ピン8本と電源ピン(コモン)1本の計9ピンあります。

電源は5VでOK。コモンに5Vを供給しておいて,制御ピン(IOエキスパンダ―MCP23017に繋いである)を0Vにすると,電流が流れて対応するセグメントが光ります。

フィラメント菅の実験

木の枠

やっぱり3Dプリンタより木の方がカッコイイよね。

ところで,趣味で CAD 使う時って皆さん何を使ってるんですか?

こんなに単純ならCAD要らんやろって? CADなしで物を作るなどありえん……

自分は oneshapeを使ってみました。 インストール不要ですし,履歴型なので操作感がsolidworksに似ていて使いやすい。長さの測定方法がよく分からなかったのでそこだけイマイチでした。

多分下記リンクで飛べば,自分が作ったデータが見れるはず。

onshape

CAD

これに合わせて切っていきました。切る/穴を空ける/削る。 ドリルとのこぎりと,ガラスカッターが必要です。

水溶性ニス(ダークブラウン) ガラス切る

ガラスはガラスカッターで傷を付けた後,板チョコのように割って加工するんですが,上手くカットできませんでした。当然です,板チョコですら上手く割れないじゃないですか。

自分はガラスカッター以外はホームセンターで全部買いました。

・ガラスカッター & オイル(左端2個): 使いやすかったかと言われると微妙。普通なのでは。

・ニス(真ん中): 二度塗りしました。塗って乾かすだけなので楽でした。色も良い感じ。

・ドリル(右端): ドリルって何が良いんだか分からないのでとりあえず BOSCH にしました。小さい木材を削る分には全く問題無い,使い勝手がいいドリルと言う印象でした。

・ドリルスタンド(右端): ドリルを固定してボール盤みたいにして使ってます。小さい木材を固定するのはちょっと工夫が必要ですが,まぁ大丈夫でしょう。

金が溜まったら卓上フライスが欲しい。

回路発注

PCBway で注文しました。注文時にオペレーターにチャットで繋がって基板チェックが入って,新鮮でした。後でメールでやり取りとかしないんだなぁと。一発OKだったのでほぼ会話なしでしたし,届くまでも1週間しなかったので,かなり良かったです。 ただし,たしか 5枚 $30 くらいだったんですが,ちょうど円安の時期だったので5500円くらいに…。

んで,出来た。

完成!

部品値段 [円]
プリント基板5493
フィラメント菅時計キット11600
GPS受信機キット2200
木材・ハケ・ニス・ガラス1526
合計13419

他にのこぎりとかドリルとか買ったので結構な額が動いた。

夜の姿はこんな感じ。

夜のフィラメント菅の暖かい輝き
夜のフィラメント菅

雑記

利用させてもらった ds1307.py は MITライセンス。こういう無償で公開しているコードってすごく有難いですし有難すぎて投げ銭したい気持ち。github さんスパチャ機能を実装してくれないかな。

追記 (2023/5/13)

みちびき受信キット (GPS) の電池が無くなると動作がおかしくなります。

詳細ですが,GPS はホットスタンバイでずっと正確な時間を受信しており,ラズピコが時々読みに行くという設計です。GPS は受信する信号の種類やボーレートを設定する必要があり,秋月のページに載っている設定ソフトであらかじめ設定します。そして,設定を保持するために,ボタン電池が付いています。この電池が無くなると,設定が初期化されて,ラズピコが読みたいデータ形式ではなくなってしまい,結果,動作がおかしくなってしまいます。(たぶん 11:59:59 から時間が更新されなくなります。)

つまり,「電池交換したくない → コンセントから給電」という要求は達成できていません!!! くやしい!!!!! 

とはいえ,正確な時間 or 停止 なので異常が分かり易くて使い勝手は良いのかなと思います。「いつの間にか時計が30分遅れていた! 遅刻だ!」が一番ヤバいと思いますので。なので,いったんは保留にして,今後もし Ver 2. を設計することになったら修正したいと思います。

本記事は学校の課題をプログラムで楽してこなそうとした際のメモを編集したものです。

2020/04/11
本記事は学校の課題をプログラムで楽してこなそうとした際のメモを編集したものです。レポートの面影はありません。

目次

  • 運動方程式を求める
    • モデル
    • 式変形用コード
    • 運動方程式
  • シミュレーションしてみる
    • 順運動学、逆運動学
    • 順逆動力学計算用コード
    • 特異点
  • 雑記

運動方程式を求める

※参考1-1のままやって一致すれば正しく出来ました、ということにしています。

モデル

2リンクマニピュレータ。x-y軸の原点をアームの根元に一致させた。

上図のような2リンクマニピュレータを考えます。G_0=(x_0,y_0), G_1=(x_0,y_1), m_0, m_1, l_0, l_1をそれぞれ第一リンク、第二リンクの重心、質量、長さとします。また、リンク端からそれぞれの重心までの長さをそれぞれl_g0, l_g1としています。

$$l_{gi}=\frac{1}{2}l_i  (i=0,1)$$

ラグランジュの運動方程式に当てはめて運動方程式を求めます。

ラグランジュの運動方程式:

$$ \frac{d}{dt}(\frac{\partial L}{\partial\dot q_i})-\frac{\partial L}{\partial q_i}= Q_i (i =1,2,..,f)$$


$$L=T-U$$

あるいは、

$$ \frac{d}{dt}(\frac{\partial T}{\partial\dot q})-\frac{\partial T}{\partial q_i}+\frac{\partial U}{\partial q_i}+\frac{\partial D}{\partial \dot{q_i}}= Q_i (i =1,2,..,f)$$

L:ラグランジアン

Q:非保存力による一般化力

f:自由度

q:一般化座標

式変形用コード

ラグランジュ~でやると無限微分地獄で計算ミスのリスクが高いですね。少しの変更でものすごく時間がかかるのは嫌なので、Pythonで式変形する方法をやってみたいと思います。こういうときはmatlabっぽく書けるSimPyが便利 (雑記1.)。

  • 式変形
simplify()
>from sympy import *
>x, y, z = symbols('x y z')
>init_printing(use_unicode=True)
>simplify(sin(x)**2 + cos(x)**2)
1
>simplify((x**3 + x**2 - x - 1)/(x**2 + 2*x + 1))
x - 1
>simplify(gamma(x)/gamma(x - 2))
(x - 2)⋅(x - 1)

式を単純化する関数で、計算量が不必要に大きくなることがあることに注意が必要です。神。

Simplification -SymPy1.5.1

  • 解く
sp.solve (Eq)

引数に=0になる関数を入力します。

Solvers -SymPy 1.5.1

  • 微分
diff(func, t)

第二引数に何で微分するのかを入れます。

Calculus -SymPy 1.5.1

参考=>1-2、1-3、1-4

  • コード

環境:

  • Python 3.7.3
  • numpy 1.16.4
  • sympy 1.4

https://github.com/inomatly/robot_arm/blob/master/solv_lagrange.ipynb

一応.pyも置いてありますが jupyter notebok で見ることを前提にしています。

式を単純化するだけのコードです。計算結果はTEXで出しています。

運動方程式

2リンクマニピュレータ(再)

各リンクのx軸とのなす角τ_0, τ_1は

$$\tau_0=\theta_0 ...(1-1)$$

$$\tau_1=\theta_0+\theta_1 ...(1-2)$$

です。 また、第一リンクの重心位置(x_0, y_0)は

$$\begin{bmatrix}x_0 \cr y_0\end{bmatrix}=\begin{bmatrix}l_{g0}cos\theta_0\cr l_{g0}sin\theta_0\end{bmatrix} ...(1-3)$$

で表せます。また、第二リンクの重心位置(x_1,y_1)は

$$\begin{bmatrix}x_1 \cr y_1\end{bmatrix}=\begin{bmatrix}l_{g0}cos\theta_0+ l_{g1}cos(\theta_0+\theta_1)\cr l_{g0}sin\theta_0+ l_{g1}sin(\theta_0+\theta_1)\end{bmatrix} ...(1-4)$$

で表せる。各リンクの運動エネルギー、位置エネルギー、損失エネルギー、一般化力をT_0, T_1, U_0, U_1, D_0, D_1, Q_0, Q_1、一般化座標q_0, q_1として、ラグランジュの運動方程式

$$ \frac{d}{dt}(\frac{\partial T}{\partial\dot q})-\frac{\partial T}{\partial q_i}+\frac{\partial U}{\partial q_i}+\frac{\partial D}{\partial \dot{q_i}}= Q_i (i =1,2) ...(1-5)$$

を用いて各リンクの運動方程式を求める。

ここで、一般化座標はq_i=θ_i (i=0,1)である。各リンクの慣性モーメントをI_0, I_1,関節の粘性摩擦係数をd_0, d_1としてT_0, T_1, U_0, U_1, D_0, D_1を求める。

$$T_0=\frac{1}{2} \left(I_{0} + l_{g0}^{2} m_{0}\right) \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2} ...(1-6)$$

$$U_0=g l_{g0} m_{0} \sin{\left({q_{0}}{\left(t \right)} \right)} ...(1-7)$$

$$D_0=\frac{d_{0} \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2}}{2} ...(1-8)$$

$$T_1=\frac{1}{2} I_{1} \left(\frac{d}{d t} {q_{0}}{\left(t \right)} + \frac{d}{d t} {q_{1}}{\left(t \right)}\right)^{2} \\+ \frac{1}{2} m_{1} \left(l_{0}^{2} \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2} + 2 l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2} \\+ 2 l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} + l_{g1}^{2} \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2} \\+ 2 l_{g1}^{2} \frac{d}{d t} {q_{0}}{\left(t \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} + l_{g1}^{2} \left(\frac{d}{d t} {q_{1}}{\left(t \right)}\right)^{2}\right) ...(1-9)$$

$$U_1=g m_{1} \left(l_{0} \sin{\left({q_{0}}{\left(t \right)} \right)} + l_{g1} \sin{\left({q_{0}}{\left(t \right)} + {q_{1}}{\left(t \right)} \right)}\right) ...(1-10)$$

$$D_1=\frac{d_{0} \left(\frac{d}{d t} {q_{1}}{\left(t \right)}\right)^{2}}{2} ...(1-11)$$

また、全体の運動エネルギー、位置エネルギー、損失エネルギーT, U, Dは

$$T=T_0+T_1 ...(1-12)$$

$$U=U_0+U_1 ...(1-13)$$

$$T=D_0+D_1 ...(1-14)$$

これより、

$$\frac{\partial T}{\partial \dot{q_0}}=I_{1} \left(\frac{d}{d t} {q_{0}}{\left(t \right)} + \frac{d}{d t} {q_{1}}{\left(t \right)}\right) + m_{1} \left(l_{0}^{2} \frac{d}{d t} {q_{0}}{\left(t \right)} \\+ 2 l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} + l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} + l_{g1}^{2} \frac{d}{d t} {q_{0}}{\left(t \right)} \\+ l_{g1}^{2} \frac{d}{d t} {q_{1}}{\left(t \right)}\right) +  \left(I_{0} + l_{g0}^{2} m_{0}\right) \frac{d}{d t} {q_{0}}{\left(t \right)}  ...(1-15)$$

$$\frac{d}{dt}(\frac{\partial T}{\partial \dot{q_0}})=I_{1} \left(\frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) + m_{1} \left(l_{0}^{2} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} \\- 2 l_{0} l_{g1} \sin{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} - l_{0} l_{g1} \sin{\left({q_{1}}{\left(t \right)} \right)} \left(\frac{d}{d t} {q_{1}}{\left(t \right)}\right)^{2} \\+ 2 l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)} + l_{g1}^{2} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} \\+ l_{g1}^{2} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) +  \left(I_{0} + l_{g0}^{2} m_{0}\right) \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} ...(1-16)$$

$$\frac{\partial T}{\partial q_0}=0 ...(1-17)$$

$$\frac{\partial U}{\partial q_0}=g \left(l_{g0} m_{0} \cos{\left({q_{0}}{\left(t \right)} \right)} + m_{1} \left(l_{0} \cos{\left({q_{0}}{\left(t \right)} \right)} + l_{g1} \cos{\left({q_{0}}{\left(t \right)} + {q_{1}}{\left(t \right)} \right)}\right)\right) ..(1-18)$$

$$\frac{\partial D}{\partial \dot{q_0}}=d_{0} \frac{d}{d t} {q_{0}}{\left(t \right)} ...(1-19)$$

$$∴ Q_0=I_{1} \left(\frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) + d_{0} \frac{d}{d t} {q_{0}}{\left(t \right)} \\+ g \left(l_{g0} m_{0} \cos{\left({q_{0}}{\left(t \right)} \right)} + m_{1} \left(l_{0} \cos{\left({q_{0}}{\left(t \right)} \right)} + l_{g1} \cos{\left({q_{0}}{\left(t \right)} + {q_{1}}{\left(t \right)} \right)}\right)\right) \\+ m_{1} \left(l_{0}^{2} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} - 2 l_{0} l_{g1} \sin{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} - l_{0} l_{g1} \sin{\left({q_{1}}{\left(t \right)} \right)} \left(\frac{d}{d t} {q_{1}}{\left(t \right)}\right)^{2} \\+ 2 l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)} + l_{g1}^{2} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} \\+ l_{g1}^{2} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) +  \left(I_{0} + l_{g0}^{2} m_{0}\right) \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} ...(1-20)$$

$$\frac{\partial T}{\partial \dot{q_1}}=I_{1} \left(\frac{d}{d t} {q_{0}}{\left(t \right)} + \frac{d}{d t} {q_{1}}{\left(t \right)}\right) \\+ l_{g1} m_{1} \left(l_{0} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} + l_{g1} \frac{d}{d t} {q_{0}}{\left(t \right)} + l_{g1} \frac{d}{d t} {q_{1}}{\left(t \right)}\right) ...(1-21)$$

$$\frac{d}{dt}(\frac{\partial T}{\partial \dot{q_1}})=I_{1} \left(\frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) + l_{g1} m_{1} \left(- l_{0} \sin{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} \\+ l_{0} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + l_{g1} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + l_{g1} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) ...(1-22)$$

$$\frac{\partial T}{\partial q_1}=-  l_{0} l_{g1} m_{1} \left(\frac{d}{d t} {q_{0}}{\left(t \right)} + \frac{d}{d t} {q_{1}}{\left(t \right)}\right) \sin{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} ...(1-23)$$

$$\frac{\partial U}{\partial q_1}=g l_{g1} m_{1} \cos{\left({q_{0}}{\left(t \right)} + {q_{1}}{\left(t \right)} \right)} ...(1-24)$$

$$\frac{\partial D}{\partial \dot{q_1}}=d_{0} \frac{d}{d t} {q_{1}}{\left(t \right)} ...(1-25)$$

$$∴ Q_1= I_{1} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + I_{1} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)} + d_{0} \frac{d}{d t} {q_{1}}{\left(t \right)} +\\ g l_{g1} m_{1} \cos{\left({q_{0}}{\left(t \right)} + {q_{1}}{\left(t \right)} \right)} +  l_{0} l_{g1} m_{1} \sin{\left({q_{1}}{\left(t \right)} \right)} \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2} +\\ l_{0} l_{g1} m_{1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} +  l_{g1}^{2} m_{1} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + l_{g1}^{2} m_{1} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)} ...(1-26)$$

以上を行列式の形にすると、

$$\begin{bmatrix}Q_0 \cr Q_1\end{bmatrix}=\\ \begin{bmatrix}I_0+m_0l_{g0}^2+I_1+m_1(l_0^2+l_{g1}^2+2l_0l_{g1}cos\theta_1)& I_1+m_1(l_{g1}^2+l_0l_{g1}cos\theta_1)\cr I_1+m_1(l_{g1}^2+l_0l_{g1}cos\theta_1)& I_1+m_1 l_{g1}^2\end{bmatrix}\begin{bmatrix}\ddot{\theta_0} \cr \ddot{\theta_1}\end{bmatrix}\\+\begin{bmatrix}d_0& 0\cr 0& d_1\end{bmatrix}\begin{bmatrix}\dot{\theta_0} \cr \dot{\theta_1}\end{bmatrix}\\+\begin{bmatrix}-m_1l_0l_{g1}(2\dot{\theta_0}\dot{\theta_1}+\dot{\theta_1}^2)sin\theta_1& m_0gl_{g0}cos(\theta_0)+m_1g(l_{0}cos\theta_0+l_{g1}cos(\theta_0+\theta_1))\cr m_1l_0l_{g1}\dot{\theta_0}^2sin\theta_1& m_1gl_{g1}cos(\theta_0+\theta_1)\end{bmatrix} \\...(1-27)$$

参考資料と一致しました。OK!! (雑記2.)

simplyfy式変形後は順番がめちゃくちゃなので綺麗に行列の形に直すのはかなり面倒です。気をつけてください。(雑記3.)

ここまでの参考

シミュレーション

動く図を作っていきます。

順運動学と逆運動学

第二リンクの先端位置(x_0,y_0)は

$$\begin{bmatrix}x_0 \cr y_0\end{bmatrix}=\begin{bmatrix}l_{0}cos\theta_0\cr l_{0}sin\theta_0\end{bmatrix} ...(2-1)$$

第二リンクの先端位置(x_1,y_1)は

$$\begin{bmatrix}x \cr y\end{bmatrix}=\begin{bmatrix}x_1 \cr y_1\end{bmatrix}=\begin{bmatrix}l_{0}cos\theta_0+ l_{1}cos(\theta_0+\theta_1)\cr l_{0}sin\theta_0+ l_{1}sin(\theta_0+\theta_1)\end{bmatrix}...(2-2)$$

これを解いていきます。移項して,

$$x-l_{0}cos\theta_0= l_{1}cos(\theta_0+\theta_1)...(2-3)$$

$$y-l_{0}sin\theta_0= l_{1}sin(\theta_0+\theta_1) ...(2-4)$$

どちらも両辺2乗し、2式を足し合わせます。

$$x^2+y^2+l^2_0-2l_0(xcos\theta_0+ysin\theta_0)=l^2_1...(2-5)$$

cos,sinに関してまとめると,

$$\sqrt{x^2+y^2}cos(\theta_0+\alpha)=\frac{x^2+y^2+l^2_0-l^2_1}{2l_0}...(2-6)$$

ただし,

$$tan( \alpha)=\frac{y}{x}...(2-7)$$

$$∴ \theta_0=-\alpha\pm cos^{-1}(\frac{x^2+y^2+l^2_0-l^2_1}{2l^2_0\sqrt{x^2+y^2}})...(2-8)$$

次にθ_1を求めます。

(2-3)/(2-4)より,

$$\frac{y-l_{0}sin\theta_0}{x-l_{0}cos\theta_0}=tan(\theta_0+\theta_1)...(2-9)$$

$$∴\theta_1=-\theta_0+tan^{-1}(\frac{y-l_{0}sin\theta_0}{x-l_{0}cos\theta_0})...(2-10)$$

  • 参考

2-1.2リンクマニピュレータの軌道追従制御 -qiita

2-2.【Python】2リンクマニピュレータの逆運動学シミュレーション -西住工房

2-3.大分大学工学部福祉環境工学科メカトロニクスコース松尾研究室ゼミ資料"MATLAB による 2 リンクロボットマニピュレータ制御のシミュレーション"

順逆運動学計算用コード

  • そも
%matplotlib inline 
%matplotlib notebook

って何ですか、ということ。jupyter内で図を見えてくれるinlineは良く使います。使わなくても出るような気がするけど、出ない時にはとりあえず書き加える感じ。

一方notebookは編集可能らしい。アイコンをタッチしてから右クリックでドラッグしたり、いろんな操作ができ図を弄れる、上の電源ボタンみたいなもので画像を固定できる。

=>参考2-1、2-2

  • wedget

jupyter widget

公式のUsing~を見てみます。

%matplotlib inline
import matplotlib.pyplot as plt
import numpy as np

@interact(amp=(0.1, 4.0, 0.1), omega=(0.1, 4.0, 0.1), phase=(-np.pi, np.pi, 0.1), 
          fn = {'sin': np.sin, 'cos': np.cos, 'tan': np.tan})
def h(amp=1.0, omega=1.0, phase=0.0, fn=np.sin):
    domain=[-np.pi, np.pi]
    x = np.linspace(domain[0], domain[1], 100)
    y  = amp * fn(omega * x + phase)
    plt.plot(x, y)
    plt.plot(-phase/omega, 0, 'or')
    plt.xlim(domain)
    plt.ylim([-4, 4])

x=(最小,最大,ステップ)はinteractでも設定でき、初期設定としてinteractを使用したい場合はデコレータとして使用出来る。辞書型で与えると選択肢に出来る。

  • コード 2

https://github.com/inomatly/robot_arm/blob/master/arm_sim.ipynb

一応.pyも置いてありますが jupyter notebok で見ることを前提にしています。

動いている様子:

楽しい。でもめっちゃ重い。(雑記3.)

このとき、(2-9)の±によって解(すなわち姿勢)は2通りあります。たとえば(1,1)は間接が(1,0)にある場合と(0,1)にある場合が考えられます。今回は計算を簡単にする為に1通りに統一しています。

特異点に関して

ヤコビ行列=0になるような姿勢は特異姿勢と呼ばれ、その点は特異点と呼ばれる。

(1)式を微分するとヤコビ行列Jは

$$\begin{bmatrix}\dot{x} \cr \dot{y}\end{bmatrix}=\begin{bmatrix}-l_{0}sin\theta_0 - l_{1}sin(\theta_0+\theta_1) & - l_{1}sin(\theta_0+\theta_1) \cr l_{0}cos\theta_0+ l_{1}cos(\theta_0+\theta_1) & l_{1}cos(\theta_0+\theta_1)\end{bmatrix}\begin{bmatrix}\dot{\theta_0} \cr \dot{\theta_1}\end{bmatrix}...(1)$$

$$∴ J=\begin{bmatrix}-l_{0}sin\theta_0 - l_{1}sin(\theta_0+\theta_1) & - l_{1}sin(\theta_0+\theta_1) \cr l_{0}cos\theta_0+ l_{1}cos(\theta_0+\theta_1) & l_{1}cos(\theta_0+\theta_1)\end{bmatrix}$$

特異点では

$$det J = l_0l_1sin\theta_1=0$$

$$∴ \theta_1=n\pi$$

n=0,1,2。この場合、たとえば(x,y) = (-2/√2,-2/√2)の伸びきった姿勢のような姿勢や、(0,0)のような縮こまった姿勢を示します。それ以上伸びたり縮んだり出来ない出来ませんから、一般に特異点では自由度が下がってることになります。

目標点が特異点を通る場合は問題ありませんが、特異点付近を通るときはアームが急激に動くため注意が必要です。(雑記5.)=>2-3

参考 2

2.1 Jupyter Notebookにおけるmatplotlib -Pythonオンライン学習サービスPyQ(パイキュー)

2-2 jupyter notebookにmatplotlibを使ってグラフを描画する -山本隆の開発日誌

2-3「ロボット工学 ー機械システムのベクトル解析ー, 広瀬茂男著 裳華房 2003年」の10章

雑記

  1. コードに関して、「pythonじゃなくてmatlabとかmathematicaでいいんじゃないの」とも思いますが。新しく覚えるおが面倒なのと、学生のうちは安価ですが将来的には金がかかりますのでなるべくオープンソースを使っていきたいという意思表示です。オープンソース贔屓です。判官贔屓的な。
  2. 「まだ微分で消耗してるの?」正月休みの1/3に手でゴリゴリ計算して、参考にしたのが正しいっぽいことを確認してからコード書いたので二度手間ですが、今後はラグランジュ方程式を使いまわせるのでOKということにしました。TEXコードまで出してくれるの神ですね。禁断の果実という感じがします。
  3. ラグランジュ方程式は使えるようになったかな(わからないけど)と思いますが、お恥ずかしながらベクトル的な解法に苦手意識があります。これからやっていきたいと思います。
  4. シミュレーションは、jupyter上の位置の問題なのか、かくかくしますね。難しい。動かしているときめっちゃチカチカするのは何故なのか。計算が重いんだろうか?だれかプログラミングおしえて。
  5. ひよこの真似やワクワクっ!な動作のように脇を開閉する動きはこの類い...ではないですね。人間の腕は自由度のバケモノですから。ワクワクの時の手首が特異点かと言えばそんなわけない。

画像から、全体の中心点と模様展の半径、角度を取得してファイルで出力する方法です。opencvを使用します。

画像から、全体の中心点と模様(濃い青丸)の半径、角度を取得したので、メモを修正して公開します。最終的に極座標グラフにします。

目次

  • 目的
  • 手順
  • 環境
  • コード
  • 解説

目的

最近、画像から半径と角度をたくさん取得する必要に駆られました。その作業はかなり手間なので自動化してやろうというのが目的です。

私の使用する画像をそのまま使うわけにはいかないので、似たような画像をいらすとやで拾ってきました。輪郭があんまり丸くなかったので加工しています。

螺旋の貝殻のイラスト

この画像から全体の中心点と模様(濃い青丸)の半径、角度を取得します。

手順

  1. グレースケール&位置座標を取得
  2. 円の取得の中心点の座標を取得
  3. 中心点を基準にして (rcosθ,rsinθ)で表現
  4. 各点(0~i)を(r, θ)で表記
  5. csvで出力

環境

  • Python 3.8.0
  • conda 4.8.0
  • opencv 4.1.2
  • matplotlib 3.1.2
  • numpy 1.17.3

コード

import cv2
import matplotlib.pyplot as plt
import numpy as np
import csv
import math
import sys

"""
あらかじめ同じフォルダに「white.png」と「読み込み対象.png」をおいておく必要がある。
芯の中心を原点にして各点(小円)の半径、角度を取得する。
"""
#input
input='rasen'
img = cv2.imread('input'+input+'.png',cv2.IMREAD_COLOR)#ndarray.グレイスケールで読み込みとエラー。
white = cv2.imread('white.png',0)#ndarray

#事前処理
img = cv2.medianBlur(img, 5) #メディアンフィルタを用いて画像を平滑化
cv2.imwrite("output/img.png",img)#debag1。outputフォルダに作成される。
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
cv2.imwrite("output/gray.png",gray)#debag2
edge = cv2.Canny(gray, 20, 40)
cv2.imwrite("output/edge.png",edge)#debag3
#中心円の取得
circles = cv2.HoughCircles(edge, cv2.HOUGH_GRADIENT, 1, 1200, param1=60, param2=20, minRadius = 800, maxRadius =1000)
#print(circles)
circles = np.uint16(np.around(circles))
x_c=np.int64(circles[0][0][0])
y_c=-np.int64(circles[0][0][1])
r_c=np.int64(circles[0][0][2])
center =[x_c,y_c]
norm_c= np.linalg.norm(center, ord=2)
for i in circles[0,:]:
    # 円を描く
    cv2.circle(img,(i[0],i[1]),i[2],(0,0,0),4)
#小円の取得
circles = cv2.HoughCircles(edge, cv2.HOUGH_GRADIENT, 1, 200,param1=60,param2=20, minRadius = 15 ,maxRadius = 50) #param1は色の違い、param2は形の雑さ
circles = np.uint16(np.around(circles))#整数になり、16bitに変換された配列になる。
j=0
for i in circles[0,:]:
    # 小円を描く
    j+=1
    cv2.circle(img,(i[0],i[1]),i[2],(0,0,0),4)
print(str(j)+"points")

#データの整形
#radiusは不要
radius=circles[:,:,2]
radius=np.array(radius[0,:])
x=circles[:,:,0]
x=x[0,:]
y=circles[:,:,1]
y=y[0,:]

#csvファイルの作成
xs=[]
ys=[]
points=[]
with open('output/'+input+'_x-y.csv', 'w') as f:
    writer = csv.writer(f, lineterminator='\n')
    writer.writerow(center)
    for i in range(len(x)):
        xs.append(np.int64(x[i]))
        ys.append(-np.int64(y[i]))
        points.append([xs[i],ys[i]])# points=[x,y]
        writer.writerow([xs[i],ys[i]])

# L2ノルム(距離)の取得とCSVファイルの生成
ds = []
Theta=[]
cos=[]
with open('output/'+input+'_d-Theta.csv', 'w') as f:
    writer = csv.writer(f, lineterminator='\n')
    writer.writerow(['Radius','Deg[deg]'])#ラベル
    for i in range(len(xs)):
        X = np.array([xs[i]-x_c,ys[i]-y_c])#中心からのベクトル
        norm = np.linalg.norm(X, ord=2)#中心からの距離
        ds.append(norm)
        cos.append(X[0]/norm)#cos = X[0]/SQRT(X[0]^2+X[1]^2)
        if ys[i]>y_c:#芯の中心より上側。
            Theta.append(360-math.degrees(math.acos(cos[i])))
        else:        #芯の中心より下側。
            Theta.append(math.degrees(math.acos(cos[i])))
        writer.writerow([ds[i],Theta[i]])

cv2.imwrite('output/'+input+'circle.png',img)# debag4

なお、角度は時計回りに作っています。

メモ:

画像は、色情報の2次元マトリックスになっており、画像処理の基本は左上から右上まで走査したあと一個下の行を走査するイメージです。取得した各点の順番は左上が先だと思います。

解説

1. グレースケール&xy座標で画像を取得

読み込み

cv2.imread(img,flag)
  • - cv2.IMREAD_COLOR : カラー画像として読み込む.画像の透明度は無視される.デフォルト値
  • - cv2.IMREAD_GRAYSCALE : グレースケール画像として読み込む
  • - cv2.IMREAD_UNCHANGED : アルファチャンネルも含めた画像として読み込む 上記のフラグを使う代わりに,単に1, 0, -1 の整数値を与えて指定することもできます.

画像を扱う -OpenCV より

フィルタリング

ここではあまり働いてませんが、やると精度が上がります。下の関数はC++(のはず)の記法ですが、引数の参考まで。

medianBlur(const Mat& src, Mat& dst, int ksize)

画像フィルタリング -openCV

グレイスケール

cv2.cvtColor(img, code)
  • code – 色空間の変換コード.説明を参照してください

色空間の変換 -OpenCV

debag2: グレイスケールまでの画像

エッジ処理

輪郭を取得します。

cv2.Canny

Canny -openCV

debag3 :エッジ検出までの画像

2. ハフ変換で円を検出する

ハフ変換は画像中の直線や円などを検出する操作です。ここでもpython用のサイトより見やすかったのでC++(のはず)へのリンクを張っておきます。

HoughCircles(Mat& image, vector<Vec3f>& circles, int method, double dp, double minDist, double param1=100, double param2=100, int minRadius=0, int maxRadius=0)¶

検出された円の (中心の x 座標, 中心の y 座標, 半径) のタプルを返します。

  • - image – 8ビット,シングルチャンネル,グレースケールの入力画像.
  • - circles – 検出された円を出力するベクトル.各ベクトルは,3要素の浮動小数点型ベクトル (x, y, radius) としてエンコードされます.
  • - method – 現在のところ, CV_HOUGH_GRADIENT メソッドのみが実装されています.基本的には 2段階ハフ変換 で,これについては Yuen90 で述べられています.
  • - dp – 画像分解能に対する投票分解能の比率の逆数.例えば, dp=1 の場合は,投票空間は入力画像と同じ分解能をもちます.また dp=2 の場合は,投票空間の幅と高さは半分になります.
  • - minDist – 検出される円の中心同士の最小距離.このパラメータが小さすぎると,正しい円の周辺に別の円が複数誤って検出されることになります.逆に大きすぎると,検出できない円がでてくる可能性があります.
  • - param1 – 手法依存の 1 番目のパラメータ. CV_HOUGH_GRADIENT の場合は, Canny() エッジ検出器に渡される2つの閾値の内,大きい方の閾値を表します(小さい閾値は,この値の半分になります).
  • - param2 – 手法依存の 2 番目のパラメータ. CV_HOUGH_GRADIENT の場合は,円の中心を検出する際の投票数の閾値を表します.これが小さくなるほど,より多くの誤検出が起こる可能性があります.より多くの投票を獲得した円が,最初に出力されます.
  • - minRadius – 円の半径の最小値.
  • - maxRadius – 円の半径の最大値.

cv::HoughCircles¶ -OpenCV より

出力は中心位置と半径です。単位は画素だと思われます。

メモ:

上記コードでは以下のように使用しています。

circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, 180, param1=60, param2=40, minRadius = 800, maxRadius =1100)

上のコードでは入力画像は2504*2607です.minRadius = 800なので、画像の半分以上を覆うような大きい円だけを検出しています。 param2は経験上、円の雑さに関するパラメータで、小さいほど歪な円を検出します。

円を描画

void circle(Mat& img, Point center, int radius, const Scalar& color, int thickness=1, int lineType=8, int shift=0)¶
  • - img – 円を描画する画像.
  • - center – 円の中心座標.
  • - radius – 円の半径.
  • - color – 円の色.
  • - thickness – 円の枠線の太さ.負の値の場合,円が塗りつぶされます.
  • - lineType – 円の枠線の種類, Line() の説明を参照してください.
  • - shift – 中心点の座標と半径の値において,小数点以下の桁を表すビット数

cv::circle -OpenCV より

debag4: ハフ変換までの画像

3. 円の中心点の座標を取得

各点は(x,y,r)を取得します。芯の位置ベクトル($x_c,y_c$)から、中心点からの各点の位置ベクトル(x_i,y_i)=(x-x_c,y-y_c)を求めます。x軸単位ベクトルを(x_e,y_e)=(1,0)とすると、内積から、

$$cos\theta = \frac{(x-x_c)x_e+(y-y_c)y_e}{\sqrt{x_c^2+y_c^2}\sqrt{(x-x_c)^2+(y-y_c)^2}}$$

$$cos\theta = \frac{x-x_c}{\sqrt{(x-x_c)^2+(y-y_c)^2}}$$

$$\theta = deg(arccos(\frac{x-x_c}{\sqrt{(x-x_c)^2+(y-y_c)^2}}))[deg]$$

【NumPy入門】ベクトルの大きさ(ノルム)を計算するnp.linalg.norm

np.linalg.norm(X, ord=2)

L2ノルム(ユークリッドノルム)は一般的な"長さ"です。

ノルムの意味とL1,L2,L∞ノルム -高校数学の美しい物語

グラフにして確認

結果が正しいかを確認します。アバウトでOKなので、画像とグラフを重ねていきます。

細かいことはかつて書きました=>[python]データのインポートと極座標系のグラフを表示- イノマタの趣味部屋

import matplotlib.pyplot as plt
import numpy as np
import pandas as pd

"""
CSVファイルから極座標系のグラフをプロットする
入力:(CSVファイル名, 出力画像ファイル名,半径,角度)
出力:出力画像ファイル名.png
"""
def polar(in_file,out_file,param_r,param_theta):    
    df = pd.read_csv("output/"+in_file)#ファイル読み込み
    r = df[param_r]
    deg = df[param_theta]
    radian = np.deg2rad(deg)#
    Len=len(r)

    fig = plt.figure(figsize=(10, 10))#新しい図面
    ax = fig.add_subplot(111, projection='polar')#軸の表示
    ax.set_theta_direction(-1)#時計回り
    #ax.plot(radian,r, '-o')
    ax.plot(radian,r, 'or')
    
    i=2
    #while i < Len:
    #    ax.plot([radian[i],radian[i-1]],[r[i], r[i-1]], 'xr-')
    #    i +=2
    plt.savefig(out_file)
    plt.show()

polar('0%_d-Theta.csv','0%.png','Radius','Deg[deg]')
取得した数値からグラフを作成

なお、画像は下記サイトで背景を透明にしました。

画像に透明色を設定する -peko step

はじめの画像と重ね合わせると、うまくグラフにできていることが確認できます。 取得したCSVは正確みたいですね。

グラフと元々の画像を重ねた画像

もともとの画像を修正したりエッジを強調したりすれはもっといい結果が得られるはずです。

雑記

パラメータの変更も自動化できれば良かったですが、できませんでした。

目的は達成したものの、手で測定するのと同じくらい時間がかかってしまったのも悲しいです。追加があれば効果が発揮できるんですが。

参考

画像処理の基本は、古い本ですがこの本が良いと思います。

コンピュータ画像処理 田村 秀行 (著) オーム社

他に、最新のアルゴリズムとか実用的な部分はOpenCVを見るのが早いでしょう。

pythonでグラフを表示するまでの流れを紹介します。グラフは自由振動、減衰振動、強制振動、倒立振り子(倒立振子、縦棒制御)の制御っぽいことをして時間変化をグラフで表示しました。おまけでCR回路の電圧の充電具合をプロットしました。 グラフを表示するのはCとかでも簡単にできますが、機械学習やってみたいのでpythonに触れる機会にします。

目次

  • 目的
  • グラフの様子
  • pythonとは
  • 環境とインストール
  • Jupyter Notebookの使い方
  • 振動の基礎とコード
  • 倒立振り子の運動方程式
  • 雑記
  • 参考

目的

思いのままのグラフを表示します。多項式だけではなく行列やベクトル、微分方程式も扱えるようになりたいと思います。

グラフの様子

今回は2つの似たような数式モデルをプロットしました。

図1:倒立振り子
  • 倒立振り子(縦棒制御)

手の上で箒のバランスを取るやつです。振動の中でも強制振動です。ニュートンの運動方程式を立てると、次のようになります。振り子は2l[m]、棒はm[kg]、台車はM[kg]です。このとき、角度Θを十分に小さいとして 近似してかなり簡単な式にまとめてあります。導出過程はページ最後に書きました。

$$\dot \theta_1(t)=\theta_2 \\ \dot \theta_2(t) = \frac{3}{l(4M+m)}(-L\theta_2+{g(M+m)-K}\theta_1)$$

簡単のためにl=1[m],M=2/3[kg],m=1/3[kg]を選ぶと、

$$\dot \theta_1(t)=\theta_2 \\ \dot \theta_2(t) = -L\theta_2+(g-K)\theta_1$$

この式で常微分方程式を解くと下記のような美しいグラフに!!コードが汚いのは御愛嬌ということで。python初めてだったのでほぼパクリです。減衰比については振動の基礎の項目に軽く書いています。

図2:倒立振り子の角度制御
from sympy import *      # sympyライブラリから全ての機能をimoprt
import numpy as np       #ベクトルや行列を表すのに使うライブラリnumpy
from scipy.integrate import odeint #scipyに含まれる常微分方程式を解く関数
import matplotlib.pyplot as plt #グラフを書くためのライブラリ
%matplotlib inline     #jupyter notebook内で表示させるため

"""
2階微分方程式 倒立振子 
"""
#シンボル定義
x=Symbol('x')                  # 文字'x'を変数xとして定義
t=Symbol('t')                  # 文字 't'を変数tとして定義

g = 9.8     #重力加速度
La = 0.89   #減衰比0.7のグラフのフィードバックゲイン
Ka = 10.2  
Lb = 1.0    #減衰比1.0のグラフのフィードバックゲイン
Kb = 10.05  
Lc = 0.0    #減衰比0.0のグラフのフィードバックゲイン
Kc = 10.0   
Ld = 1.0    #減衰比0.3のグラフのフィードバックゲイン
Kd = 12.57  

def func1(x, t, K):
    x1,x2=x
    dxdt=[x2,(-La*x[1])+((g-Ka)*x[0])]
    return dxdt
#同じことを複数書いているので省略

x0 = [10.0,2.0] # 初期条件 [x(0), dx(0)/dt]を表す

t=np.linspace(0,30,1000) # 時間 0から10までを101等分する
sol1=odeint(func1, x0, t, args=(k,)) # 微分方程式を解く 
#同じことを複数書いているので省略

#グラフ表示
plt.plot(t, sol3[:,0], linewidth=1,label='ζ=0.0') #  角度を図示。
plt.xlabel('time t[s]', fontsize=18)
plt.ylabel('angle θ[rad]', fontsize=18)
plt.legend(loc='upper right')
plt.savefig('huriko1.jpg')
plt.show()
  • LCR回路

そしてもう一つの式がこれです。回路のキャパシタはダンパー、インダクタはばねと同じです。

$$ E = v_R + v_C + v_L \\ E = L\frac{di(t)}{dt}+Ri(t)+\frac{1}{C}\int i(t)dt $$

これをキャパシタ電圧でまとめると

$$ i = C\frac{d}{dt}v_C \\ \frac{d^2}{dt^t} v_c = -\frac{R}{C} \frac{d}{dt}v_c -\frac{1}{LC}v_c + \frac{E}{LC}$$

これは強制振動の式と同じ形なので、同じようなグラフがかけます。

図3:LRC回路のVcの時間応答

python とは

今回のpythonは読みやすく、それでいて効率もよいコードをなるべく簡単に書けるように作られたプログラミング言語です。言語自体も書きやすいように作られていますが、ライブラリやワーススペースが豊富で、だいたい何でも出来るみたいです。最近ではAIや機械学習が人気ですが、そういう事をするためのライブラリが豊そろっているみたいで、最近活躍している言語です。 2系と3系は全然違うので注意が必要です。

ライブラリというのは、

ライブラリとは、複数のプログラムが共通して利用するコードをまとめたファイルである。 ライブラリ自体は単独で実行することはできない。 from I-8-8. ライブラリとプログラムの関係 | 日本OSS推進フォーラム

というものです。必要なときに呼び出すと自分の仕事だけやって帰って行きます。

ちなみに、初学者へ。世の中のホームページやアプリの画面表示は別な言語で書いています。pythonとかCとかは計算処理をやってくれるものなので、これだけやっても画面表示は出来ません。注意してください。勉強しはじめたころの私はこのあたりよく分かって無かったために時間が掛かってしまったので一応書いておきました。

環境とインストール

環境はAnacondaでまとめてインストールしてJupyter Notebookで書きます。 python公式でインストールしても良いんですが、他にもいろいろまとめてインストール出来るらしいDownloads - Anacondaでインストールした方が楽みたいです。デフォルトのままで良いので指示に従うだけです。

Jupyter Notebookの使い方

インストールしたらJupyterNotebookを起動します。するとコマンドコマンドプロンプト(っぽいなにか)が出てきますので、そこに書かれているURLをブラウザで開いてください。

ユーザーのフォルダの中身が表示されますので、好きなのを選んで右上のnewからpython3をクリック。すると、プログラムを書くためのセルが出てきます。

今数字が書いてある色塗りの部分を押すとコマンドモードになり数式やらがかけます。In[]やOut[]と書いている部分はエディットモードで、コピーやらが出来ます。エディットモードでHを押すとヘルプが出ます。

コマンドモードで上のcodeの部分をmorkdownにすると、morkdownで文章を書けます。 morkdownは私がこのブログの下書きやメモに普段使いしているマークアップ言語(HTMLの仲間)で、# ABC と書くとタイトル的な大きな文字、## ABCと書くと1段階小さい文字、なにもつけないと普通の文字、- と書くと箇条書き、というようにかなり使いやすい言語です。

pythonの記法はすっ飛ばします。numpyというライブラリを読みこんでベクトル(1次元配列)や行列(2次元配列)を表せます。 スライシング。bool。

コマンドプロンプトでサーバーをlocalhostで立てて、ブラウザで表示しているんじゃないかと思います(このあたりよく解ってませんが。たぶん。。なので、ブラウザで開いている間にJupyterNotebookを消すとエラーをはきます。

発生したエラーについて

  • 保存した画像が真っ白になっている。jupyterにはグラフが出力されている

Python matplotlibのグラフが保存できない(windows+anaconda) teratail

振動の基礎とコード

図4:線形振動系の力学モデル

機械屋さんにとってのかなり基本的なモデル(らしい)で考えます。ニュートンの運動方程式を立てます。

$$ f(t)= m\frac{d^2x(t)}{dt^2}+c\frac{dx(t)}{dt}+kx(t) $$

これは強制振動を表しています。

振動は、自由振動、減衰振動、強制振動の3つに分けられます。自由振動は正弦波などのように振動し続けます。減衰振動は自由振動が次第に減衰していきます。強制振動は減衰振動に外力を加えている場合です。 日常生活では大概空気抵抗などで減衰するので減衰振動がイメージしやすいかと思います。一方、制御するときはモータが動いているので強制振動です。

外力をf(t)と書いていますから、f(t)=0なら減衰振動、f(t)の項が有れば強制振動になります。

上の式を作為的に以下のように変形します。

$$ f(t)= \frac{d^2x(t)}{dt^2}+2\zeta\omega_n\frac{dx(t)}{dt}+\omega_n^2 \ \\減衰比\zeta = \frac{c}{2\sqrt{km}}, 固有振動数\omega_n = \sqrt{\frac{k}{m}} $$

減衰比ζは減衰具合、固有振動数ω_nは振動時の周波数に対応しています。これが減衰振動の基本的なモデルです。ζ=0のとき全く減衰しないのでsin波のような自由振動をし、ζ=1の時過不足無く(クッションに沈むように)目標値に到達します。実際には誤差や外乱が入るので制御系を作るときには0.7くらいにするそうです。

今回はpythonで解きたいので、ここから変形していきます。本当はいろんな方法が有るみたいなんですが、そのうちやると言うことにして、今回は参考本と同じ方法で解いていきたいと思います。

常微分方程式を解きます。二次方程式は大変なので、以下のように書きかえます。

$$\frac{dx_1(t)}{dt}=x_2 \\ \frac{dx_2(t)}{dt} = -\frac{c}{m}x_2-\frac{k}{m}x_1 +f(t)$$

これを行列を用いたベクトル表現にすると、

$$ \frac{d}{dt} \left[ \begin{array}{rrr} x_1 \\ x_2 \end{array} \right] = \left[ \begin{array}{rrr} 0 \ & 1 \ \\ -\frac{k}{m} & -\frac{c}{m} \\ \end{array} \right] \left[ \begin{array}{rrr} x_1 \\ x_2 \end{array} \right] + \left[ \begin{array}{rrr} 0 \\ 1 \end{array} \right] f(t)$$

この式のように変数の位置x(t)を配列としてpythonで定義すると、以下のようになります。

m = 1.0 #質量
c = 0.5 #粘性減衰
k = 1.0 #ばね定数

def func(x, t, k):
    x1,x2=x
    dxdt=[x2,-c/m*x[1]-k/m*x[0]+sin(t/2)]
    return dxdt

x0 = [0.0,0.0] # 初期条件 [x(0), dx(0)/dt]を表す.今は初速0.

t=np.linspace(0,50,100000) # 時間 0から10までを101等分する
sol=odeint(func, x0, t, args=(k,)) # 数値的に微分方程式を解き, x(t)とx'(t)をsolのリストの[:,0]、[:,1]成分へ格納する。

以上のような感じで、パラメータをいじることで自由振動、減衰振動、強制振動がそれぞれ再現出来ました。

  • 自由振動(図5) 初期x=0.0, 初期x' = 1.0, m = 1.0, c = 0.0, k = 1.0
  • 減衰振動(図6) 初期x=0.0, 初期x' = 1.0, m = 1.0, c = 0.5, k = 1.0
  • 強制振動(図7) 初期x=0.0, 初期x' = 0.0, m = 0.0, c = 0.5, k = 1.0 f(t) = sin(t/2)
  • 強制振動(図8) 初期x=0.0, 初期x' = 0.0, m = 1.0, c = 0.5, k = 1.0 f(t) = 1
図5:自由振動
図6:減衰振動
図7:強制振動
図8:強制振動

倒立振り子の運動方程式

再度掲載

台車と振り子を図に示します。 粘性摩擦係数をB、 軸の粘性摩擦係数をCとおきました。ここから、下の4つの式が立てられます。

振り子について

$$J\ddot \theta = Vlsin\theta - Hlcos \theta - C\dot \theta …(1)\\ (J=\frac{ml^2}{3})\\ m\frac{d^2}{dt^2}(x + lsin\theta)= H…(2) \\ m\frac{d^2}{dt^2}(lcos\theta)=V - mg…(3)$$

台車について

$$M \ddot x = f(t) - B \dot x - H…(4)$$

簡単のためにC = 0, B = 0とします。ここからH,Vを消去します。

$$H = m \ddot x + ml \ddot \theta sin\theta - ml (\dot \theta)^2 sin\theta…(5) \\ V = -ml \ddot \theta sin \theta - ml(\dot \theta) cos \theta +mg…(6)$$

これを(1)(4)に代入してまとめると、

$$ J\ddot \theta= -ml^2 \ddot \theta - lmcos\theta \ddot x + mglsin\theta…(7) \\ (M+m) \ddot x = -ml\ddot \theta cos\theta + ml(\dot \theta)^2 sin\theta+f(t)…(8)$$

この式からxの2階微分を消去して、Θが十分小さいとして線形化すると

$$cos\theta\approx 1,sin\theta\approx\theta, (\theta)^2 \approx 0 \\ \ddot \theta l (4M+m)-3(M+m)g \theta = -3f(t)…(9)$$

f(t)に角度と各速度についてフィードバックさせます。

$$ f(t)= L \dot \theta + K \theta\ $$

簡単のために l = 1[m], M = 2/3[kg], m = 1/3[kg]とおくと、以下のようにかけます。プログラムで解けるように一階化します。

$$ \dot \theta_1 = \theta_2 \\ \dot \theta_2 = -L \theta_2 + (g-K)\theta_1$$

一応全体の全体をラプラス変換して伝達関数の式を求めます。(せっかく導出したから書きたい。)

$$L{\theta(t)}=\Theta(s),L{f(t)}=F(s)として\\ \frac{\Theta(s)}{F(s)}=\frac{-3}{(4M+m)ls^2+3ls-3(M+m)g+K}$$

雑記

プログラミング系のサイトは最近公開されたものを見るように気をつけるようになりました。機械系や電気系の、特に理論はかなり成熟しているのでふるいサイトでも何でも読みやすくわかりやすければ良いんですが、情報系はツールがどんどん更新されていくので最新のを見ないと妙なところで躓いてしまいます。 おかげで私のPCちゃんはパスとかアプリとか訳が分からない感じになっています。情報系の特にweb系のブロガーさんは情報収集がすごいので、最新のでも誰かしら先にやっていますので安心ですね。先人は偉大です。 それと、解析力学が難しいですね。もう少し時間をかけないとわからないです。ラグランジュ難しい。制御はやって行きたいので、のんびり本を読んでいきます。
追記(2019/3/20)参考リンクをリストにしました。Amazonリンクを増やしました。

参考