２リンクアームの動力学シミュレーション(軌道生成編)

ロボティクス 復習

最近モチベーションあがらず、なかなか更新できなかった・・・。
モチベーションがあがらなかったので久しぶりに小説・漫画を読んでみたよー。

小説は、遠藤周作の「フランスの大学生」。
遠藤周作がフランスに留学したときのお話を書いてるんだけど、当時は戦後間もないころもあって、彼は、「戦争とヨーロッパ」を題材にしている。風景の描写・ストーリー構成は非常に面白いんだけど、昔の宗教的観念や戦争に対する考え方などは現代では理解に苦しむ箇所があり、内容が難しくなっている感じをうけたなー。

フランスの大学生 (ぶんか社文庫 え 4-1)

フランスの大学生 (ぶんか社文庫 え 4-1)


漫画は、よしたにの「理系の人々」。
リクナビNEXT Tech総研で連載しているものをまとめた漫画。「ぼく、オタリーマン。」の作者みたいね。自分のバックグランドは機械工学（学部）と情報科学（修士）なので、半分以上、「あーそうそう、わかるわかるｗ」という場面があって面白かった。しかし、不覚にも化学系のお話になると「へーなるほど。」と関心してしまった。

理系の人々

理系の人々


話がだいぶ違う方向にそれてしまったので、本題に戻しましょう。
前回まで公開しているソースは、アームの目標値をただ入力しているだけなので、コントローラはかなりオーバーシュートしてしまっていた。これでは、かなりみっともないので、今回は、時系列の目標値を作成する関数を公開します。なんか偉そうに書いてますが、非常に簡単な関数を公開するだけなので、たいしたことないです。

sin波を使った目標値生成

double Traj_sin(double t, double tl, double dv)
{// tl : シミュレーション最終時間 
 // tf : 目標値追従時間
 // t  : 現在の時間
 // dv : 目標値
	double tf;
	//パラメータ設定
	tf = tl * 0.8;//最終時間よりちょっと早めに設定
	if (t > tf)t = tf;
	//
	return (dv*(1+sin(3.0/2.0*pi+t/tf*pi))/2.0);
}

説明
PDコントローラは急激な変化があると、オーバーシュートしてしまうので、初めと終わりはゆっくり変化させる目標軌道生成関数を作成する必要がある。こんな軌道を生成するには三角関数を利用すると便利（・▽・）

上の図で軌道生成に使えそうなのは3/2\pi〜5/2\piの間。
ということで初期時間では、3/2\piで目標値に収束させたい時間を5/2\pi(= 3/2\pi + \pi)とすればよいわけです。
つまり、以下の式を使用すれば3/2\pi〜5/2\piの軌道を取得できます。
\sin(3/2\pi+t/t_{f}\pi)
ここで、現在の時間tが変数になっているので、3/2\pi〜5/2\piの値をとることができます。
あとは、振幅について考えなければなりません。
このままだとマイナスからスタートしてしまいます。それは避けなければならないので、
1 + \sin(3/2\pi+t/t_{f}\pi)
とすれば、常にプラスの値を取得できます。しかし、これを使用すると振幅が0〜2になっちゃうので、
(1 + \sin(3/2\pi+t/t_{f}\pi))/2
とすれば良いわけです。あとは、コレに目標値を掛け合わせれば完成。
dv(1 + \sin(3/2\pi+t/t_{f}\pi))/2



今回は非常に簡単な例ですが、生成したい軌道を時間関数にすれば、色々な軌道を生成することができます。
次は、OpenGLを使ってアニメーション表示したいなー。
あと、ソースのバージョン変更したので、アップしたいんですが、ちょっと方法を考え中。