８．５．　円筒座標・SCARAロボットによるＣＰ運動の最短時間制御

２００７−６−２９，　２００８−１１−１０改訂

ソース(ファイル CPtoc2Source　　実行ファイル CPtoc2exe

（１） 空間位置速度の関節変位速度による表現（順運動学）

・円筒座標ロボットの関節変位と座標値の関係は

　　　，　　　　　　　　　　　 (1-1)

・SCARAロボットの関節変位と座標値の関係は

　　　 (1-2)

　 (1-3)

（２）　動力学式

・ワークのみが質量を持つとし，慣性モーメントは無視する．ワークの運動に必要な力は慣性力のみで

　　　 (2-1)

・円筒座標ロボットの動力学式は

　　　 (2-2)

・SCARAロボットの動力学式は

　　　 (2-3)

（３） いくつかのＣＰ軌道のパラメータ表現

（３−１）直線軌道

・基点を，直線の傾きをβとすると

　　　　　　　　　　　　　 (3-1)

　　　　　　　　 (3-2)

　　　　　　　　 (3-3)

（３−２）円弧軌道

・円の中心を，半径をR，軌道長をsとすると

　　　，　　　　　 (3-4)

　　　　　　 (3-5)

　　　　　　　 (3-6)

　　　　　 (3-7)

（３−３）Ｕ字軌道

・逆Ｕ字の基点を，円弧との接点を，，終点を，直線部の長さを，円弧部の長さを，円弧（半円）の半径をとすると

　　：，　　 (3-8)

　　　　　　　　，　　 (3-9)

　　　　　　　　，　　 (3-10)

　　：，　　 (3-11)

　　　　　　　　　　　，　 (3-12)

　　　　　　　　　　　 (3-13)

　　　　　　　　　　　 (3-14)

　　：，　　 (3-15)

　　　　　　　　，　　 (3-16)

　　　　　　　　　　　　　　，　　 (3-17)

（３−４）正弦波軌道

・正弦波軌道

　　　 (3-18)

の軌道パラメータとしてをとる．

・速度・加速度は

　　，　 (3-19)

　　，　 (3-20)

（４）関節トルク力と軌道パラメータの関係の一般表現

・軌道のパラメータ表現は一般に

　　　　，　　　　　　 (4-1)

　　　　，　　　　　　　 (4-2)

　　　　，　　　 (4-3)

の形をしている．

・一方，円筒座標ロボットの動力学式は

　　　　 (4-4)

　であるから，これをｓで表現すると

　　　　 (4-5)

または

　 (4-6)

　ただし

　　　，　　　，　　 (4-7)

・関節トルク力を与えると軌道上の加速度は，各関節について

　　　，　　　 (4-8)

（５） 限界の加速度を与える

・軌道上の加速度は駆動加速度を超えることはできない．最大加速度は上記２関節による加速度の小さい方であり，両者が等しいとき限界加速度となる．限界加速度を与える条件は

　　　　　　　　　　　　　　　　　　　　　　 (5-1)

・8.4の結果を参考にして

　　　 (5-2)

・このの境界を超えない範囲で，できるだけ速く（をできるだけ大きくして）ｓを目標値に持っていく位相平面上の軌道を探索する．

（６）最大加減速による軌道上の運動の求め方

・位相平面上で，どの時点で最大加速から最大減速へ移るか（図のP2点）を見付けるのは，人の関与なしのコンピュータの手続きだけでは困難である．

・本シミュレーションでは，位相平面上のトラジェクトリの推移を見ながら人が遷移点を探す手法をとる．

・予め，加減速限界の領域，および始点からの最大加速によるトラジェクトリ，終点から逆時間の最大減速によるトラジェクトリは図示しておく．

（７）通常のCP運動制御の所要時間

・最短時間制御と比較するために，通常のCP運動制御の所要時間を調べる．

・通常のCP制御の１つの方法として次を仮定する．

（１）軌道パラメータｓの時間関数を台形速度則に従うとする．

（２）始点からの加速・終点に到る減速を関節の最大駆動加減速度（この場合はX,Y方向の加速度）とする．

（３）中間では一定速度であり，その値はとする．これはコリオリ加速度が最大関節加速度を越えない条件である．

・軌道パラメータの全行程（軌道長）をとすると以下の式が成り立つ．

　　　：，　　　　　　　　　　(7-1)

　　　：　　，　　　　　(7-2)

　　　　　　　　　　　　　　　　　　　　　　 (7-3)

　　　：，　　　　　　　 (7-4)

　　　以降は時間に対し対称である．

　　　：　　　　　　　　　 (7-5)

　最後の式から所要時間が求まる．

　　　　　　　　　　　　　　　　 (7-6)

・これらの値は，コリオリ加速度を含めた関節駆動加速度が限界を超えない限りの最小の所要時間を採用する．

・この時間を最短時間制御の所要時間と比較する．

［シミュレーションの使い方］

●　シミュレーションの諸元設定

（１） ロボットタイプの選択

円筒座標ロボット・SCARAロボットの２種

ワーク質量(m[kg])のみを考慮し，ロボットは質量なしとする．

ロボットのアーム長は固定値で変更しない．

（２） 関節駆動最大加速度設定

円筒座標では　

SCARAロボットでは　

を設定する．

（３） CP運動の選択と諸元設定

１．直線：始点位置[m]，傾きβ[deg]，長さl[m]

　　　　　２．円弧：円の中心位置[m]，半径R[m]，円弧角θ[deg]

　　　　　　　始点は右端とする．

　　　　　３．正弦曲線：始点[m]，波長(X方向)L[m]，振幅(Y方向高さ)A[m]

　　　　　　　　　　　　１周期だけとする．

　　　　　４．逆Ｕ字曲線：始点[m]，直線部長さ(Y方向)L[m]，半円部半径R[m]

　　　　５．等加速度カム曲線：始点[m]，X方向行程L[m]，Y方向行程H[m]

　　　　６．サイクロイドカム曲線：始点[m]，X方向行程L[m]，Y方向行程H[m]

　　　　運動種類を選択し，諸元を設定する．

●　実行

・画面は文字画面と描画画面とがある．

・文字画面ではシミュレーションの条件を設定する．

・描画画面では，始めに最短時間軌道を探索する位相平面が描画され，終了後にロボットの運動と関節変位・関節駆動加速度のグラフが描画される．

・なお，同時に位相平面軌道も薄く描画される．またワークの描くCP軌道には重ねて通常のCP運動制御の軌道も白抜きの点列でTOC運動を追いかけるように表示される．

（１）　全体の手順

スタート → 文字画面へ → 諸元設定(1) → 描画画面へ → 手動で位相平面上の最短時間制御軌道を生成(3)

→ 軌道生成終了後，画面が変わってロボットの運動アニメーションと関節変位・関節トルクのグラフ表示(4)

→ 別の条件でシミュレーションしたければ，キー入力によって文字画面へ(2) → (1)へ

（２）　描画画面上でのキー入力による諸元変更

　ａ，ｂ：ロボットタイプの変更とSCARAの場合はアーム長設定

　ｕ：関節最大加速度の変更

　ｋ：CP運動の変更 → 文字画面上で運動の諸元設定

　ESC：終了

（３）　手動（キー操作）による最短時間制御軌道生成

・位相平面上の軌道を探索する．

・最短時間軌道はを限界領域内でできるだけ大きくする道を探索する．

・その手順はXYロボットの場合と同様であるが，改めて述べる．

・操作キーは下記の通りである．計算の時間刻みをdtとする．

　ｓ：軌道生成の継続

　ｔ：時間を2 dt戻してその後の時間刻みをdt1= dt /50と細かくする．

　ｒ：時間をdt（またはdt1）戻して加減速切り替え

　ｃ：加減速切り替え

　ｆ：目標点に到る最終軌道に乗せる．

　　　[註]少し行き過ぎてからfを押す．この際r，cを押してはならない．

　0 ：全てをリセットして始点に戻る（やり直し）

・SCARAロボットの正弦曲線運動を最短時間制御する例について軌道探索の手順を示す．

・探索終了後直ちにロボットの運動と変位・関節駆動トルク（加速度）のグラフ表示に入る．

（４）ロボットの運動アニメーションと関節変位・トルクのグラフ描画

・ｓを押し続けると運動が進む．また0によって始点に戻る．

・薄く位相平面上の軌道も描画される．

［CP運動の最短時間制御の特徴］

・XYロボットによるCPの最短時間制御と基本的に同じである．

・加減速の限界を示す境界線はX,Y方向の力を関節トルクに変換しているので複雑な形状をしている．

・そのために加減速の切り替え回数が頻繁になっている．

・探索軌道は限界線と離れているが，真の解は境界線と接している（図の�A，�Cの部分）．

・時間刻みが有限であり，その切り替え時間の誤差が大きく軌道に影響し，本シミュレーションでは真の解は得られない．

・実際の最短時間制御でも外乱やロボットパラメータの推定誤差などのために切り替え時間の誤差が免れず，それが軌道誤差を生じさせる．

・フィードバックのない最適制御は諸々の誤差に弱い．実際の制御ではPTPの最短時間制御と同様に上多指観測フィードバックや学習制御など他の制御手法を組み合わせる必要がある．