２リンクアームの動力学シミュレーション(シミュレーション編)②
台風がまた日本に向かってるね。
先週、福岡旅行行ったときも台風が通り過ぎた後だったからなー。
東京は小雨程度だったけど、どこにも行く気しないなー。せっかく仕事休めたのに・・。
前回は、投げやり的にシミュレーションのソースを載せたんだけど、まったく説明なかったので、今回は簡単な説明。
内容は
- システム全体について
- ソースコードについて
- コントローラについて
システム全体について
システムは、基本的に入力があり、制御システムで処理を行い、出力する。
今回はこんな感じ
システム系入力 :
、
アームモデル入力 : コントローラ出力
システム系出力 :、
コントローラはPD制御を採用。理由は、・・・簡単だから(・△・;)
ソースコードについて
前回にも書いたけど、プログラム構成は、こんな感じ
Main.c
ManipulatorFunc.h
Main.c
はっきり言ってこっちは、あんまり話すことない。
出力データをcsvファイルに書き込んでいるんだけど、出力内容は以下の通り
1列目は、
2列目は、
3列目は、
4列目は、
実際にシミュレーションを行っている箇所は、以下の通り。
//シミュレーション
for (t=0;t < LTIME; t += TSTEP){
//数値積分
Runge_Kutta(Link,Cont,x,t,TSTEP);
//ファイル出力
for (i=0;i<4;i++){
fprintf(fo,"%f,",x[i]);
}fprintf(fo,"\n");
}ここで、LTIME : シミュレーション時間、TSTEP : ステップ時間（つまり、シミュレーションは離散系なので、どれくらいの間隔でシミュレーションを行うかを設定）
ManipulatorFunc.h
void Runge_Kutta()について
// k1[]
Dif_eq(model, Controller, x, dotx);
for(i=0;i<4;i++){
k1[i] = TSTEP*dotx[i];
REx[i] = x[i] + k1[i]/2.0;
}
REt = t + TSTEP/2.0;Dif_eq(model, Controller, x, dotx)を呼びだして最新のxの値を与え、結果としてdotxを受け取ります。ここで、dotxはポインタを渡していると同じで、返り値なしで最新のdotx値を受け取ることができる。
// change x[]
for(i=0;i<4;i++){
x[i] = x[i] + (k1[i] + 2.0*k2[i] + 2.0*k3[i] + k4[i])/6.0;
}この部分で、最新のxを導き出している。一番重要なシステム系出力値を算出していることに注目してください。
void Dif_eq(struct LinkParam model, struct PdParam Controller, double x, double dotx)について
ここの処理系は、入力をx、それを微分方程式に代入して、dotxを出力としている。
重要なのは、dotxを設定する方法。この部分を自由に設定できれば、自分で数値積分できるﾌﾟﾛｸﾞﾗﾑを書くことができる。
dotx[0] = x[2]; //(1) dotx[1] = x[3]; //(2) dotx[2] = ((M[1][1])*(-C1-K1+T1) + (-M[0][1]*(-C2-K2+T2))) / det; //(3) dotx[3] = ((-M[1][0])*(-C1-K1+T1) + (M[0][0]*(-C2-K2+T2))) / det; //(4)
dotxは数値積分すると新しいxになるということを念頭に考えます。
求めたいのは、角度と角速度。
角度は角速度を数値積分すればよいので、(1), (2)のように設定すればよいです。
同様に考えると、角速度は角加速度を数値積分すればよいですね。実は、微分方程式を変形すれば角加速度を求める方程式にすることができます。
微分方程式は、。
それを以下のように変形させます。
これをソースで記述すると(3), (4)のようになります。
