８．４．直角座標ロボットによるＣＰ運動の最短時間制御　２００７−６−２９　２００８−１１−１０改訂

ソースファイル cptocxySource 実行ファイル cptocxyexe

　目次

（１） 軌道長による軌道表現・その速度加速度関係

（２） 関節変位速度加速度の軌道パラメータ表現

（３） ロボットの動力学式の軌道パラメータ表現

（４）限界トルクによる軌道パラメータの限界加速度

（５）限界加速度　−最大加速度＝最小加速度となる限界の−

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

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

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

（１） 　軌道長による軌道表現・その速度加速度関係

（１−１）直線軌道

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

　　　　　　　　　　　　　 (1-1)

　　　　　　　　 (1-2)

　　　　　　　　 (1-3)

（１−２）円弧軌道

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

　　　，　　　　　 (1-4)

　　　　　　 (1-5)

　　　　　　　 (1-6)

　　　　　 (1-7)

（１−３）Ｕ字軌道

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

　　：，　　 (1-8)

　　　　　　　　，　　 (1-9)

　　　　　　　　，　　 (1-10)

　　：，　　 (1-11)

　　　　　　　　　　　，　 (1-12)

　　　　　　　　　　　 (1-13)

　　　　　　　　　　　 (1-14)

　　：，　　 (1-15)

　　　　　　　　，　　 (1-16)

　　　　　　　　　　　　　　，　　 (1-17)

（１−４）正弦波軌道

・正弦波軌道

　　　 (1-18)

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

・速度・加速度は

　　，　 (1-19)

　　，　 (1-20)

（１−５）　軌道の一般的表現

・これらはいずれも

　　　　，　　　　　　 (1-20)

　　　　，　　　　　　　 (1-21)

　　　　，　　　 (1-22)

の形をしている．

（２） 関節変位速度加速度の軌道パラメータ表現

・空間位置とその速度加速度は関節変位・速度加速度と同じである．

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

　　　　 　　 (2-1)

　　　， (2-2)

（３）ロボットの動力学式の軌道パラメータ表現

・アーム先端の集中質量のみを考慮する．

　　　 　　　　　　　　　 (3-1)

（４）限界トルクによる軌道パラメータの限界加速度

・X,Y方向限界加速度をとする．

・各トルクに対応する正負の限界軌道加速度は

　　　，　　　　　　　　　　　　　　　　　　　　 (4-1)

　のとき：　　，　　　　　 (4-2)

　のとき：　　，　　　　　 (4-3)

・最大限界加速度は両者の小さい方であり，最小限界加速度は両者の大きい方である．

(4-3)

(4-4)

・

・加速度はできるだけ大きい方がよいが，となることはできない．

・軌道上で通常はであるが，速度が上がり過ぎると逆になり，軌道を正しく辿ることができない．

・は軌道位置速度の関数であり，の境界はの関数である．

（５）限界加速度　−最大加速度＝最小加速度となる限界の−

・を表す式を導く．

（１）のとき

(5-1)

(5-2)

と置く．

　（1-1），のとき

　　　　 → → (5-3)

f’’’<0のときのみ値を持つ．

このとき　，で条件は成り立っている．

　（1-2）， のとき：存在しない．

　（1-3），のとき：存在しない．

　（1-4）， のとき

　　　　 → → (5-4)

f’’’>0のときのみ値を持つ．

このとき　，で条件は成り立っている．

（２）のとき

(5-5)

(5-6)

と置く．

　（2-1），のとき

　　　　 → → 　　 (5-7)

この値は正でなければならない．

このとき　，で条件は成り立っている．

　（2-2），のとき：存在しない．

　（2-3），のとき：存在しない．

　（2-4），のとき

　　　　 → → 　　 (5-8)

この値は正でなければならない．

このとき　，で条件は成り立っている．

これらをまとめると

（１）のとき： (5-9)

（２）のとき： (5-10)

まとめて

　　　　 (5-11)

が軌道を追従できる限界の曲線である．

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

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

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

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

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

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

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

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

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

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

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

　　　：，　　　　　　　　　　(7-1)

　　　：　　，　　　　　(7-2)

　　　　　　　　　　　　　　　　　　　　　　 (7-3)

　　　：，　　　　　　　 (7-4)

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

　　　：　　　　　　　　　 (7-5)

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

　　　　　　　　　　　　　　　　 (7-6)

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

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

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

・最短時間軌道探索は対話型で求めるようになっている．

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

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

を設定する．

（２） 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)へ

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

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

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

　ESC：終了

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

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

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

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

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

　ｓ：軌道生成の継続

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

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

　ｃ：加減速切り替え

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

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

　　　最終軌道に乗せた後，終点に向けてsを押し続けると自動的に軌道生成を終了し，ロボットアニメーションに切り替わる．

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

・半円弧運動を最短時間制御する例について軌道探索の手順を示す．

（１） ｓキーによる運動継続

（２）ｔキーによる運動２つ戻し　　（３）ｓキーによる微小軌道生成継続

（４）ｒキーで１つ戻して軌道上の加減速切り替え　　　（５）ｓキーによる軌道生成継続

（６）ｃキーによる軌道上の加減速切り替え　　（７）ｓキーによる軌道生成継続

（８）ｆキーによる最終軌道へ切り替え　　（９）最終軌道に自動的に正確に乗る

（１０）ｓキーによる最終軌道生成

探索終了後直ちに画面が切り替わる．

（１１）ワークの運動とx,y変位・駆動力（駆動加速度）の変化のグラフ

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

・限界領域外は２駆動力（駆動加速度）とも限界を越えていることを意味する．

・が大きいほど速いので，位相平面上の軌道はできるだけが大きい所を通るように調整する．

・従って限界領域の端を通り抜ける軌道が最短時間である．

・限界線の近傍ではわずかの時間ずれでも大きく変化する．

・計算では時間刻みが有限で，限界領域の端を丁度通り抜ける軌道とすることができない．

・上記の例では生成軌道が領域の端からわずかに隔たっており，真の最短時間制御と異なる．

・実際の制御を考えたとき，最大駆動力や外乱などによる軌道のずれがあり，そのために限界領域外に飛び込む危険がある．１度飛び込むとその後の軌道は目標と大きくずれて（必要躯動力が不足するので），どうなるか分からない．

・すなわち，実用的には限界ぎりぎりを通るのはよくない．

・最短時間制御とはフィードバックループを持たないので，中途の誤差は累積されて次第に目標軌道と離れてしまう性質を持っている．

い方かたタ