�U．　ロボットアームの速度加速度解析 　2004.10.05 作成

ソースファイル kineSource 実行ファイル kineexe

　　　　　２−１．順運動学　　　　 　analys_pose( )

　　　　　２−２．速度加速度解析ｋ 　analys_vel_acck( )

　　　　　２−３．速度加速度解析A　 analys_vel_accA( )

　　　　　２−４．ヤコビアン Jacobian( )

の解析を行う．

１ ．ロボット構造の設定

（１）関節・アームをベースから

B-J0-L0-J1-L1-……Jn-1-Ln-1-Jn-Ln(Hand/Work)

と名付ける．(n=NFREE-1)

（２）初期状態（関節変位＝０の状態）を 　　　

基本とする．

（３）関節位置を各リンク座標の原点とする．

（４）回転関節軸をＺ軸とする．

（５）直動関節の場合はその方向をＸ軸とする．

（６）逆運動学解析のためのロボット構造の入力は

以下のようにする．初期状態で 　　　　　　

（６−１）静止座標系から見たロボットのベース

位置：B

（６−２）静止座標系から見た関節位置：Jk　　　　　　　

（k=0,1,…..,n=NFREE-1）

（６−３）静止座標系から見た各アーム座標系：Σk 　　　

（k=0,1,….., n=NFREE-1）

（６−４）静止座標系から見たワークの位置：rw

（６−５）静止座標系から見たワークの姿勢：Σw　　　

（６−６）関節種別（回転Ｒか直動Ｓか）

２．前準備

　

　ロボット構造・動的パラメータを kinema.c へ受け渡す．　　　　　　　　　　　　　　　　　　　　　　　　　　　ロボットの初期ポーズ

　　　　（RRSRRRの例）

［註］座標変換行列は静変換行列と動変換行列の積で表す．その配列順序に注意

　　　，　

()

　　　⇒

　　　：Σk-1から見たΣskの姿勢

　　　：初期ポーズのアームベクトルのΣk-1表示

　　　　()　（）

　　　　()　⇒

（２−１）初期形態から座標変換行列を計算するとき　

図の初期状態を与えて座標変換行列を作る．

EtoA( NFREE, JTYPE, BASP0, BASE0, rJ0, E0, As ) (2002-6-28, 2004-9-25 改訂)

　　　　　　　　　　　　　　

　●この関数に受け渡す変数

　　int NFREE：ロボットの自由度

　　char JTYPE[k-th]：Jkの関節タイプ　(‘R’or ‘S’）（‘RRRRRR’，’RRSRSR’など）

　　double BASP0[3]：ロボットベースの位置（ΣA表示）

　　double BASE0[3][3]：ロボットベースの姿勢（ΣA表示）

　　double rJ0[k-th][3]：関節点Jkの初期ポーズの位置（ΣA表示）

　　double E0[k-th][3][3]：アームLkの初期ポーズの姿勢（ΣA表示）

　●戻し値

　double As[k-th][4]3[]：Σk→Σk-1への静座標変換行列

●kinema.cに作成する変数

NFREE：自由度

JTYPE[k-th]：Jkの関節タイプ(‘R’or ‘S’)

AB[4][3]：ΣB→ΣAの座標変換行列（一定）

AS[k-th][4][3]：静座標変換行列

WRKP0[3]：ワークのΣn表示の位置(AS[NFREE][3-th)[3]と同じ）

　WRKXN[3]，WRKYN[3],WRKZN[3]：ワークのΣn表示の姿勢（AS[NFREE][0,1,2][3]と同じ）

（２−２）既に座標変換行列が分かっているとき

setrobot( NFREE, JTYPE, AB, As )

●この関数に受け渡す変数

　　int NFREE：ロボットの自由度(=6)

　　char JTYPE[k-th]：関節Jkのタイプ　‘R’または‘S’(‘RRRRRR’,’RRSRSR’など）

　　double AB[4][3]：ΣB→ΣAの座標変換行列

　　double As[k-th][4][3]：Σk →Σk-1への静座標変換行列(k=0,1,2,…,NFREE-1)

　　double As[NFREE][4][3]：Σn表示のワークの位置と姿勢

　●戻し値　なし

●kinema.cに作成する変数

NFREE：自由度

JTYPE[k-th]：関節タイプ

AB[4][3]：ΣB→ΣAの座標変換行列（一定）

AS[k-th][4][3]：静座標変換行列（As[k-th][4][3]と同じ）

WRKP0[3]：ワークのΣn表示の位置(As[NFREE][3-th)[3]と同じ）

　　WRKXN[3]，WRKYN[3],WRKZN[3]：ワークのΣn表示の姿勢（As[NFREE][0,1,2][3]と同じ）

３．解析項目

（１）順運動学

　　　関節変位Φを与えて各関節位置rJk・アーム姿勢Ek，ワーク位置rw姿勢Ewを求め

る．

analys_pose( PHI, RJ, EARM, RW, EW )

●計算のために必要な既知変数（kinema.cへ既に受け渡している変数．前準備による）

NFREE：自由度

As[k-th][4][3]：静座標変換行列

As[NFREE-th][4][3]：ワークの位置姿勢のΣn表示（n=NFREE-1）

●関数への入力

double PHI[k]：関節変位( rad or m )

●関数の出力

　　　double RJ[k-th][3]：関節点Jkの位置（ΣA表示）(m)

　　　double EARM[k-th][3][3]：アームLkの姿勢（ΣA表示）

　　　double RW[3]：ワークの位置（ΣA表示）(m)

　　　double EW[3][3]：ワークの姿勢（ΣA表示）

　　　

（２）速度加速度解析k

関節変位・速度・加速度，各アーム重心位置rGkを与えて，各関節点の速度vJk，加速度aJk，各アームの角速度ωk，角加速度αk，ワークの速度vw加速度aw，角速度ωw角加速度αw，および各アーム重心の速度vGk，加速度aGk，ワーク重心の速度vGw加速度aGwを，Σk座標系表示で求める．

　　　analys_vel_acck( PHI, DPHI, DDPHI, rg, OMEGA, ALPHA, VJ, AJ, VW, AW,

VG , AG, VGW, AGW )

●計算のために必要な既知変数（kinema.cへ既に受け渡している変数．前準備による）

NFREE：自由度

JTYPE[k-th]：Jkの関節タイプ（’R’ or ‘S’ ）

As[k-th][4][3]：静座標変換行列

As[NFREE-th][4][3]：ワークの位置姿勢のΣn表示（n=NFREE-1）

●関数への入力

double PHI[k]：関節変位（rad or m）

double DPHI[k]：関節速度(rad/s or m/s)

double DDPHI[k]：関節加速度( or )

double rg[k-th][3]：アームLkの重心位置（Σk表示）(m)

double rg[n-th][3]：ハンド／ワークの重心位置（Σn表示）(m)(n=NFREE-1)

　　●関数の出力　（VW[], AW[]以外は全てΣk座標系表示）

　　　double OMEGA[k-th][3]：アームLkの角速度(rad/s)

　　　double ALPHA[k-th][3]：アームLkの角加速度()

　　　double OMEGA[n-th][3]：ハンド／ワークの角速度(rad/s) (n=NFREE-1)

　　　double ALPHA[n-th][3]：ハンド／ワークの角加速度()

　　　double VJ[k-th][3]：関節点Jkの速度(m/s)

double AJ[k-th][3]：関節点Jkの加速度()

　　　double VW[3]：ワークWの速度(m/s)

double AW[3]：ワークWの加速度()

　　　double VG[k-th][3]：アームLk重心の速度(m/s)

double AG[k-th][3]：アームLk重心の加速度)

　　　double VGW[3]：ハンド／ワークの重心の速度(m/s)　（ΣA表示）

　　　double AGW[3]：ハンド／ワークの重心の加速度()（ΣA表示）

（３）速度加速度解析A

　　　（２）と同じ計算であるが，結果をΣA表示で出力する．

関節変位・速度・加速度を与えて，各関節点の速度vJk，加速度aJk，各アームの角速度ωk，角加速度αk，ワークの速度vw加速度aw，角速度ωw角加速度αwを，ΣA座標系表示で求める．

　　　analys_vel_accA( PHI, DPHI, DDPHI, OMEGA, ALPHA, VJ, AJ, VW, AW )

●計算のために必要な既知変数（kinema.cへ既に受け渡している変数．前準備による）

NFREE：自由度

JTYPE[k-th]：Jkの関節タイプ（’R’ or ‘S’ ）

As[k-th][4][3]：静座標変換行列

As[NFREE-th][4][3]：ワークの位置姿勢のΣn表示（n=NFREE-1）

●関数への入力

　　　double OMEGA[k-th][3]：アームLkの角速度(rad/s)

　　　double ALPHA[k-th][3]：アームLkの角加速度()

　　　double OMEGA[n-th][3]：ハンド／ワークの角速度(rad/s) (n=NFREE-1)

　　　double ALPHA[n-th][3]：ハンド／ワークの角加速度()

　　　double VJ[k-th][3]：関節点Jkの速度(m/s)

double AJ[k-th][3]：関節点Jkの加速度()

　　　double VW[3]：ワークWの速度(m/s)

double AW[3]：ワークWの加速度()

　　　double VG[k-th][3]：アームLk重心の速度(m/s)

double AG[k-th][3]：アームLk重心の加速度)

　　　double VGW[3]：ハンド／ワークの重心の速度(m/s)　（ΣA表示）

　　　double AGW[3]：ハンド／ワークの重心の加速度()（ΣA表示）

（３）速度加速度解析A

　　　（２）と同じ計算であるが，結果をΣA表示で出力する．

関節変位・速度・加速度を与えて，各関節点の速度vJk，加速度aJk，各アームの角速度ωk，角加速度αk，ワークの速度vw加速度aw，角速度ωw角加速度αwを，ΣA座標系表示で求める．

　　　analys_vel_accA( PHI, DPHI, DDPHI, OMEGA, ALPHA, VJ, AJ, VW, AW )

●計算のために必要な既知変数（kinema.cへ既に受け渡している変数．前準備による）

NFREE：自由度

JTYPE[k-th]：Jkの関節タイプ（’R’ or ‘S’ ）

As[k-th][4][3]：静座標変換行列

As[NFREE-th][4][3]：ワークの位置姿勢のΣn表示（n=NFREE-1）

●関数への入力

double PHI[k]：関節変位（rad or m）

double DPHI[k]：関節速度(rad/s or m/s)

double DDPHI[k]：関節加速度( or )

double rg[k-th][3]：アームLkの重心位置（Σk表示）(m)

double rg[n-th][3]：ハンド／ワークの重心位置（Σn表示）(m)(n=NFREE-1)

　　●関数の出力　（VW[], AW[]以外は全てΣk座標系表示）

　　　double OMEGA[k-th][3]：アームLkの角速度(rad/s)

　　　double ALPHA[k-th][3]：アームLkの角加速度()

　　　double OMEGA[n-th][3]：ハンド／ワークの角速度(rad/s) (n=NFREE-1)

　　　double ALPHA[n-th][3]：ハンド／ワークの角加速度()

　　　double VJ[k-th][3]：関節点Jkの速度(m/s)

double AJ[k-th][3]：関節点Jkの加速度()

　　　double VW[3]：ワークWの速度(m/s)

double AW[3]：ワークWの加速度()

　　　double VG[k-th][3]：アームLk重心の速度(m/s)

double AG[k-th][3]：アームLk重心の加速度)

　　　double VGW[3]：ハンド／ワークの重心の速度(m/s)　（ΣA表示）

　　　double AGW[3]：ハンド／ワークの重心の加速度()（ΣA表示）

（３）速度加速度解析A

　　　（２）と同じ計算であるが，結果をΣA表示で出力する．

関節変位・速度・加速度を与えて，各関節点の速度vJk，加速度aJk，各アームの角速度ωk，角加速度αk，ワークの速度vw加速度aw，角速度ωw角加速度αwを，ΣA座標系表示で求める．

　　　analys_vel_accA( PHI, DPHI, DDPHI, OMEGA, ALPHA, VJ, AJ, VW, AW )

●計算のために必要な既知変数（kinema.cへ既に受け渡している変数．前準備による）

NFREE：自由度

JTYPE[k-th]：Jkの関節タイプ（’R’ or ‘S’ ）

As[k-th][4][3]：静座標変換行列

As[NFREE-th][4][3]：ワークの位置姿勢のΣn表示（n=NFREE-1）

●関数への入力

double PHI[k]：関節変位（rad or m）

double DPHI[k]：関節速度(rad/s or m/s)

double DDPHI[k]：関節加速度( or )

　　●関数の出力　（全てΣk座標系表示）

　　　double OMEGA[k-th][3]：アームLkの角速度(rad/s)

　　　double ALPHA[k-th][3]：アームLkの角加速度()

　　　double OMEGA[n-th][3]：ハンド／ワークの角速度(rad/s) (n=NFREE-1)

　　　double ALPHA[n-th][3]：ハンド／ワークの角加速度()

　　　double VJ[k-th][3]：関節点Jkの速度(m/s)

double AJ[k-th][3]：関節点Jkの加速度()

　　　double VW[3]：ワークWの速度(m/s)

double AW[3]：ワークWの加速度()

（４）ヤコビアン

　

（４）ヤコビアン

　　　関節速度とワーク上の点の速度角速度比を与える．

Jacobian( PHI, rp, JCBNV, JCBNO )

●計算のために必要な既知変数（kinema.cへ既に受け渡している変数．前準備による）

NFREE：自由度

JTYPE[k-th]：Jkの関節タイプ（’R’ or ‘S’ ）

As[k-th][4][3]：静座標変換行列

As[NFREE-th][4][3]：ワークの位置姿勢のΣn表示（n=NFREE-1）

●関数への入力

double PHI[k-th]：関節変位 ( rad or m )

　　　double rp[3]：ワーク上の点のΣn表示　(m)

　　●関数の出力

　　　double JCBNV[3][NFREE]：=

　　　double JCBNO[3][NFREE]：=

４ ．使用の注意

（１）運動学の関数を使う前にEtoA( )またはsetrobot( )の予備計算を行う（座標変換行列を決めるため）．

１度これを計算すれば，ロボット構造・ワークの位置姿勢が変わらない限り，その

後運動学の関数を何回でも使える．

（２）extern int analys_pose( );

extern int analys_vel_acck( );

extern int analys_vel_accA( );

extern int EtoA( ); (または　setrobot( ); )

を定義する．

（３）ベクトルの配列は[0],[1],[2]，…，マトリクス[3][3]の成分は第１行 ([0][0] [1][0]

[2][0])，第２行([0][1] [1][1] [2][1])，第３行([0][2] [1][2] [2][2]) の順で

（１）運動学の関数を使う前にEtoA( )またはsetrobot( )の予備計算を行う（座標変換行列を決めるため）．

１度これを計算すれば，ロボット構造・ワークの位置姿勢が変わらない限り，その

後運動学の関数を何回でも使える．

（２）extern int analys_pose( );

extern int analys_vel_acck( );

extern int analys_vel_accA( );

extern int EtoA( ); (または　setrobot( ); )

を定義する．

（３）ベクトルの配列は[0],[1],[2]，…，マトリクス[3][3]の成分は第１行 ([0][0] [1][0]

[2][0])，第２行([0][1] [1][1] [2][1])，第３行([0][2] [1][2] [2][2]) の順で

ある．

（４） 関節J0 の変位はPHI[0]，J1 の関節変位はPHI[1]，J2 の変位はPHI[2]，．．．で

ある．

（５）E[3][3] = (ex, ey, ez)は，E[0-th][3] = ex, E[1st][3] = ey, E[2nd][3] = ezである．

［参考］

　使用例はktest.cを参照のこと．

［参考文献］

「詳説ロボットの運動学」，高野政晴，オーム社，2004．