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

いきなり気温が下がって、肌寒くなってきたなー。すっかり秋だ(・△・)

今回は、実際にロボットアームのシミュレーションを実行してみよう。
もう、すっかり忘れてしまったので、思い出すのが大変。

運動方程式は微分方程式なので、数値積分をして、角度・角速度を求める必要があるなー。
数値積分は実現するツールは色々あるみたいなんだけど、一般的にはルンゲクッタ法を利用するし、これしか使ったことない。
ということでルンゲクッタ法を利用してみた

ルンゲクッタ法のソース作成に参考にしたページ

http://pc-physics.com/rk1.html

シミュレーションの説明は次回にして、今回は、ソースを公開します。（長々と）
ちなみにこのソースをコンパイルして、実行するとわかるけど、目標値に追従しない・・・。
つまり定常偏差が発生してます。詳しくは次回説明できたらいいんだけどなー。

ﾌﾟﾛｸﾞﾗﾑ構成

• ManipulatorFunc.h
• Main.c
• outdata.csv(同じフォルダ内に用意してください)

ManipulatorFunc.h

#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#define pi 3.1416
#define POW2(x) x*x

//リンクパラメータ
struct LinkParam{
	double m1, m2;    //Mass
	double L1, L2;    //Link length
	double J1, J2;    //Inertia
	double g;         //Gravity
};
//PDコントローラパラメータ
struct PdParam{
	double cp1, cp2;
	double cd1, cd2;
	double dv1, dv2;  //desired value
};


//ロボットアーム微分方程式
void Dif_eq(struct LinkParam model, struct PdParam Controller, double x[], double dotx[])
{
	double m1,m2,L1,L2,Lc1,Lc2,J1,J2,g;
	double cd1,cd2,cp1,cp2;
	double dv1,dv2;
	//
	double M[2][2];
	double det;
	double C1,C2;
	double K1,K2;
	double T1,T2;
	//パラメータ設定
	m1 = model.m1;   m2 = model.m2;
	L1 = model.L1;   L2 = model.L2; 
	Lc1 = L1/2.0;    Lc2 = L2/2.0;
	J1 = model.J1;   J2 = model.J2;
	g  = model.g;
	cp1 = Controller.cp1; cp2 = Controller.cp2;
	cd1 = Controller.cd1; cd2 = Controller.cd2;
	dv1 = Controller.dv1; dv2 = Controller.dv2;
	//慣性項
	M[0][0] = J1+J2+m1*POW2(Lc1)+m2*POW2(L1)+m2*POW2(Lc2)+2*m2*L1*Lc2*cos(x[1]);
	M[0][1] = J2+m2*POW2(Lc2)+m2*L1*Lc2*cos(x[1]); 
	M[1][0] = J2+m2*POW2(Lc2)+m2*L1*Lc2*cos(x[1]); 
	M[1][1] = J2+m2*POW2(Lc2);
	det     = M[0][0]*M[1][1];
	det    -= M[0][1]*M[1][0];
	//コリオリ項
	C1      = -m2*L1*Lc2*sin(x[1])*(2*x[2]*x[3]+POW2(x[3]));
	C2      = m2*POW2(x[2])*L1*Lc2*sin(x[1]);
	//重力項
	K1      = -m1*g*Lc1*sin(x[0])-m2*g*(L1*sin(x[0])+Lc2*sin(x[0]+x[1]));
	K2      = -m2*g*Lc2*sin(x[0]+x[1]);
	//コントローラ
	T1      = cp1*(dv1 - x[0]) - cd1;
	T2      = cp2*(dv2 - x[1]) - cd2;

	//運動方程式
	dotx[0] = x[2];
	dotx[1] = x[3];
	dotx[2] = ((M[1][1])*(-C1-K1+T1) + (-M[0][1]*(-C2-K2+T2))) / det;
	dotx[3] = ((-M[1][0])*(-C1-K1+T1) + (M[0][0]*(-C2-K2+T2))) / det;
}

//ルンゲクッタ法
void Runge_Kutta(struct LinkParam model, struct PdParam Controller, double x[], double t, double TSTEP)
{
	//パラメータ設定
	double k1[4], k2[4], k3[4], k4[4];
	double dotx[4];
	double Rx[4], REt, REx[4];
	int i;
	// 初期化
	for (i=0;i<4;i++){
		k1[i] = 0.0; k2[i] = 0.0; k3[i] = 0.0; k4[i] = 0.0;
		dotx[i] = 0.0; Rx[i] = 0.0; REt = 0.0; REx[i] = 0.0;
	}

	// 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;

	// k2[]
	Dif_eq(model, Controller, REx, dotx); 
	for(i=0;i<4;i++){ 
		k2[i] = TSTEP*dotx[i];
		REx[i] = x[i] + k2[i]/2.0;
	}
	REt = t + TSTEP/2.0;

	// k3[]
	Dif_eq(model, Controller, REx, dotx); 
	for(i=0;i<4;i++){ 
		k3[i] = TSTEP*dotx[i];
		REx[i] = x[i] + k3[i];
	}
	REt = t + TSTEP;

	// k4[] 
	Dif_eq(model, Controller, REx, dotx); 
	for(i=0;i<4;i++){ 
		k4[i] = TSTEP*dotx[i];
	}

	// 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;
	}
}

Main.c

// 2 Link Manipulator Control

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "ManipulatorFunc.h"

void main(void)
{
	FILE *fo;
	struct LinkParam Link;
	struct PdParam Cont;
	double x[4];
	// x[0] : theta 1            //
	// x[1] : theta 2            //
	// x[2] : theta velocity 1   //
	// x[3] : theta velocity 2   //
	int i;
	double t,TSTEP,LTIME;
	//
	//パラメータ初期化・設定
	Link.m1 = 1.0;  Link.m2 = 1.0;
	Link.L1 = 0.1;  Link.L2 = 0.1;
	Link.J1 = 0.01; Link.J2 = 0.01;
	Link.g  = 9.8; 
	//
	Cont.cp1 = 360.0;  Cont.cp2 = 360.0;
	Cont.cd1 = 15.0;  Cont.cd2 = 13.0;
	Cont.dv1 = 30*pi/180.0;
	Cont.dv2 = 30*pi/180.0;
	//
	for (i=0;i<4;i++){
		x[i] = 0.0;
	}
	//
	LTIME = 50;
	TSTEP = 0.01;
	//出力ファイル設定
	fo = fopen("outdata.csv","w");
	//ファイル出力
	for (i=0;i<4;i++){
		fprintf(fo,"%f,",x[i]);
	}fprintf(fo,"\n");

	//シミュレーション
	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");
	}
	fclose(fo);

}

参考文献

ロボティクス―機構・力学・制御

JJ Craig著のロボティクス。ロボティクス専門の方は結構お世話になっている本ではないでしょうか。
今回のソース作成でもとてもお世話になりました。