JP2002091572A - ロボットシステムの制御方法及び制御装置 - Google Patents

Abstract

(57)【要約】 【課題】 ロボット機構部の各関節の許容ピークトルク
（許容値）を越えずかつ最短時間で動作が終了できる最
適な各関節の加減速度を求めることができ、動作時間を
短縮できる。 【解決手段】 開始位置及び目標位置と速度予測値とロ
ボットダイナミクスベースパラメータに基づいて、慣性
行列と粘性行列と重力行列とを含む開始位置及び目標位
置におけるロボットダイナミクスモデルを計算し、計算
されたロボットダイナミクスモデルに基づいて、加速度
予測値の各関節間の比例関係と各関節の許容ピークトル
クの条件を満足する最適な加速度及び減速度を演算した
後、すべての関節で動作時間を一致させる同期処理を実
行する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、ロボットシステム
の制御方法及び制御装置に関する。

【０００２】

【従来の技術】従来、複数の関節を有するロボットシス
テムの制御装置では、動作速度指令に加減速度（以下、
加速度と減速度を意味する。また、加速と減速とを含
み、加減速という。）を設け、なめらかな動作を実現し
ている。ロボットの関節を、同じ速度パターンで動作さ
せても、関節に発生するトルクは、ロボットの姿勢に依
存して異なる。重力や他の関節から受ける干渉力などの
負荷トルクが、ロボットの関節位置すなわちロボット姿
勢により変わってしまうためである。負荷トルクが最大
となるロボット姿勢で、速度パターンを固定すると、負
荷トルクの影響が小さい姿勢での動作時間が必要以上に
延びてしまう。一方、最小の姿勢で固定すると、負荷ト
ルクの大きな姿勢で、モータが許容できる以上のトルク
が必要となり、モータ制御に異常が発生して問題とな
る。

【０００３】図１０は、第１の従来例の位置決め動作処
理を示すフローチャートである。図１０において、ま
ず、ステップＳ１０１で関節番号ｉを０にリセットした
後、関節番号ｉが最終の関節ｉ_maxとなるまで、各関節
番号ｉ毎にステップＳ１０２からステップＳ１０４まで
の処理を繰り返す。ここで、ステップＳ１０２では、開
始位置、目標位置及び目標速度をセットし、ステップＳ
１０３で移動量の計算処理を実行した後、ステップＳ１
０４で加速度及び減速度のプリセット情報に基づいて速
度パターンの計算を行う。さらに、ステップＳ１０７で
は、最長動作時間の関節を抽出し、ステップＳ１０８で
関節番号ｉを０にリセットした後、関節番号ｉが最終の
関節ｉ_maxとなるまで、各関節番号ｉ毎にステップＳ１
０９における速度パターンの再計算処理である終了時間
の同期処理を繰り返す。

【０００４】図１１は、図１０の終了時間の同期処理の
一例を説明するための各関節の速度パターンを示すグラ
フである。図１０のステップＳ１０７では、第２の関節
が最長動作時間ｔ_maxを有するとして抽出され、第１の
関節と第３の関節の速度パターンは、それらの動作時間
が第２の関節が最長動作時間ｔ_maxとなり、（ａ）面積
Ｓａが面積Ｓａｐに等しくなり、（ｂ）面積Ｓｂが面積
Ｓｂｐに等しくなり、（ｃ）面積Ｓｃが面積Ｓｃｐに等
しくなり、（ｄ）面積Ｓｄが面積Ｓｄｐに等しくなるよ
うに決定される。なお、図１０のグラフの積分値である
面積は距離を表す。

【０００５】この第１の従来例において、ステップＳ１
０４での速度パターンの生成に使われる、加速度及び減
速度の情報は、関節毎にプリセットされており、ロボッ
トの姿勢に応じて変動する負荷トルクの影響度は考慮さ
れていない。変動する負荷トルクとは、重力及び負荷慣
性モーメント並びに他の関節から受ける干渉力等であ
る。そのため、プリセットされる加速度及び減速度情報
は、負荷トルクが最大となるロボット姿勢において、モ
ータ制御に異常が発生しない値とするのが一般的であ
る。

【０００６】

【発明が解決しようとする課題】しかしながら、第１の
従来例の方法では以下の問題がある。 （ａ）負荷トルクが最大でないロボット姿勢における位
置決め動作では、モータの能力が最大限に使用されてい
ない。 （ｂ）ロボット設計の際、負荷トルクが最大のロボット
姿勢において、モータ制御に異常が発生しないようにモ
ータを選定しなければならず、必然的にオーバースペッ
クとなる。

【０００７】この問題を解決するために、特開平７−２
６１８２２号公報（以下、第２の従来例という。）で
は、重力モーメント、加速度による慣性、他の関節の動
作による干渉トルクを受ける駆動関節に対して、各関節
の質量並びに重心位置から発生する負荷トルク成分を計
算し、加速度の影響を受ける負荷トルク成分が、駆動関
節の許容ピークトルクから加減速度の影響を受けない負
荷トルク成分の値を差し引いた値となるように、加減速
時間を求める方法がとられていた。しかしながら、この
方法にも、以下の問題がある。

【０００８】（ａ）ロボットのすべての関節の加減速時
間が同じという前提条件のもとで、加減速時間が演算さ
れており、関節毎の移動量に大きな差があるような位置
決め動作を行う場合、駆動関節の許容トルクに余裕があ
るにもかかわらず、動作時間を短縮できない。以下、第
１の課題という。 （ｂ）ロボットのリンク自体の慣性テンソルによる負荷
トルク成分を考慮していないため、関節の回転中心から
重心位置までの距離が短いロボットでは厳密な負荷トル
ク成分の計算ができない。以下、第２の課題という。

【０００９】本発明の目的は以上の問題点を解決し、ロ
ボット機構部の各関節の許容ピークトルク（許容値）を
越えず、かつ最短時間で動作が終了できる最適な各関節
の加減速度を求めることができ、しかも、動作時間を短
縮できるロボットシステムの制御方法及び制御装置を提
供することにある。

【００１０】

【課題を解決するための手段】本発明に係るロボットシ
ステムの制御方法は、複数の関節を有するロボット機構
部の動作を制御するロボットシステムの制御方法であっ
て、上記ロボット機構部を動作させるときのロボット機
構部の各関節の開始位置と、目標位置と、目標速度とを
教示データとして第１の記憶手段に格納する第１の記憶
ステップと、上記ロボット機構部の各関節の速度パター
ンを予測演算するときに用いる各関節の加速度と減速度
の情報とを第２の記憶手段に格納する第２の記憶ステッ
プと、上記第１の記憶手段に格納された教示データと、
上記第２の記憶手段に格納された各関節の加速度及び減
速度とに基づいて、所定の第１の速度パターン計算方法
を用いて、上記ロボット機構部の各関節の仮速度予測値
と、動作時間予測値を演算し、上記動作時間予測値の中
から、その最大値を最長動作時間として演算する第１の
演算ステップと、所定の速度パターン再計算方法を用い
て、上記ロボット機構部の各関節について、上記第１の
演算ステップによって演算された動作時間予測値が、上
記第１の演算ステップによって演算された最長動作時間
と同じになるように、速度並びに加速度及び減速度とし
て再計算した値を、速度予測値、加速度予測値及び減速
度予測値とする第２の演算ステップとを含むロボットシ
ステムの制御方法において、上記ロボット機構部の各関
節に連結される各リンクの慣性テンソル、質量及び重心
位置とを含むロボットダイナミクスベースパラメータを
第３の記憶手段に格納する第３の記憶ステップと、上記
第１の記憶手段に格納された各関節の開始位置及び目標
位置と、上記第２の演算ステップによって演算された速
度予測値と、上記第３の記憶手段に格納されたロボット
ダイナミクスベースパラメータとに基づいて、（ａ）上
記ロボット機構部の各関節についての、加速度により関
節自身に発生するトルク、並びに他関節の加速度により
発生する干渉トルクを示す慣性行列と、（ｂ）上記ロボ
ット機構部の各関節についての、遠心力及びコリオリ力
により発生するトルクを示す粘性行列と、（ｃ）上記ロ
ボット機構部の各関節の重力モーメントにより発生する
トルクを示す重力行列とを含む上記開始位置及び目標位
置におけるロボットダイナミクスモデルを計算する第３
の演算ステップと、上記第３の演算ステップによって演
算されたロボットダイナミクスモデルと、上記第２の演
算ステップによって演算された各関節の加速度予測値及
び減速度予測値とに基づいて、各関節間の加速度予測値
の比例関係である第１の条件を満足し、かつ関節の許容
ピークトルクから、加速度及び減速度に関係の無い重力
行列及び粘性行列を減算した減算結果値が、各関節が上
記加速度予測値及び減速度予測値で動作した場合に各関
節で発生するトルク以上となるという第２の条件を満足
するような最適な加速度及び最適な減速度を演算する第
４の演算ステップと、上記第１の演算ステップにおいて
最長動作時間となった関節を検索し、検索した関節であ
る第ｋの関節について、上記第４の演算ステップによっ
て演算された最適な加速度及び最適な減速度と、上記第
１の記憶手段に格納された第ｋの関節の開始位置、目標
位置及び目標速度を含む教示データとから、上記第１の
速度パターン計算方法を用いて第ｋ関節の速度パターン
を演算し、その動作時間を基準動作時間とする第５の演
算ステップと、上記第ｋの関節以外のすべての関節につ
いて、上記第５の演算ステップによって演算された基準
動作時間と、上記第４の演算ステップによって演算され
た最適な加速度及び最適な減速度と、上記第１の記憶手
段に格納された開始位置及び目標位置を含む教示データ
とに基づいて、所定の第２の速度パターン計算方法を用
いて、動作時間が上記基準動作時間となり、上記最適な
加速度及び最適な減速度で動作するような速度パターン
を生成する第６の演算ステップとを含むことを特徴とす
る。

【００１１】また、上記ロボットシステムの制御方法に
おいて、好ましくは、上記加速度に関係しないトルク成
分を、上記粘性行列及び重力行列に加算することによ
り、上記各関節で発生する摩擦抵抗を考慮することを特
徴とする。

【００１２】さらに、上記ロボットシステムの制御方法
において、好ましくは、上記慣性行列の対角成分に、上
記各関節の慣性モーメントを加算することにより、上記
各関節自身の加減速で発生するトルク演算値を補正する
ステップとをさらに含むことを特徴とする。

【００１３】本発明に係るロボットシステムの制御装置
は、複数の関節を有するロボット機構部の動作を制御す
るロボットシステムの制御装置であって、上記ロボット
機構部を動作させるときのロボット機構部の各関節の開
始位置と、目標位置と、目標速度とを教示データとして
格納する第１の記憶手段と、上記ロボット機構部の各関
節の速度パターンを予測演算するときに用いる各関節の
加速度と減速度の情報とを格納する第２の記憶手段と、
上記第１の記憶手段に格納された教示データと、上記第
２の記憶手段に格納された各関節の加速度及び減速度と
に基づいて、所定の第１の速度パターン計算装置を用い
て、上記ロボット機構部の各関節の仮速度予測値と、動
作時間予測値を演算し、上記動作時間予測値の中から、
その最大値を最長動作時間として演算する第１の演算手
段と、所定の速度パターン再計算装置を用いて、上記ロ
ボット機構部の各関節について、上記第１の演算手段に
よって演算された動作時間予測値が、上記第１の演算手
段によって演算された最長動作時間と同じになるよう
に、速度並びに加速度及び減速度として再計算した値
を、速度予測値、加速度予測値及び減速度予測値とする
第２の演算手段とを備えたロボットシステムの制御装置
において、上記ロボット機構部の各関節に連結される各
リンクの慣性テンソル、質量及び重心位置とを含むロボ
ットダイナミクスベースパラメータを格納する第３の記
憶手段と、上記第１の記憶手段に格納された各関節の開
始位置及び目標位置と、上記第２の演算手段によって演
算された速度予測値と、上記第３の記憶手段に格納され
たロボットダイナミクスベースパラメータとに基づい
て、（ａ）上記ロボット機構部の各関節についての、加
速度により関節自身に発生するトルク、並びに他関節の
加速度により発生する干渉トルクを示す慣性行列と、
（ｂ）上記ロボット機構部の各関節についての、遠心力
及びコリオリ力により発生するトルクを示す粘性行列
と、（ｃ）上記ロボット機構部の各関節の重力モーメン
トにより発生するトルクを示す重力行列とを含む上記開
始位置及び目標位置におけるロボットダイナミクスモデ
ルを計算する第３の演算手段と、上記第３の演算手段に
よって演算されたロボットダイナミクスモデルと、上記
第２の演算手段によって演算された各関節の加速度予測
値及び減速度予測値とに基づいて、各関節間の加速度予
測値の比例関係である第１の条件を満足し、かつ関節の
許容ピークトルクから、加速度及び減速度に関係の無い
重力行列及び粘性行列を減算した減算結果値が、各関節
が上記加速度予測値及び減速度予測値で動作した場合に
各関節で発生するトルク以上となるという第２の条件を
満足するような最適な加速度及び最適な減速度を演算す
る第４の演算手段と、上記第１の演算手段において最長
動作時間となった関節を検索し、検索した関節である第
ｋの関節について、上記第４の演算手段によって演算さ
れた最適な加速度及び最適な減速度と、上記第１の記憶
手段に格納された第ｋの関節の開始位置、目標位置及び
目標速度を含む教示データとから、上記第１の速度パタ
ーン計算装置を用いて第ｋ関節の速度パターンを演算
し、その動作時間を基準動作時間とする第５の演算手段
と、上記第ｋの関節以外のすべての関節について、上記
第５の演算手段によって演算された基準動作時間と、上
記第４の演算手段によって演算された最適な加速度及び
最適な減速度と、上記第１の記憶手段に格納された開始
位置及び目標位置を含む教示データとに基づいて、所定
の第２の速度パターン計算装置を用いて、動作時間が上
記基準動作時間となり、上記最適な加速度及び最適な減
速度で動作するような速度パターンを生成する第６の演
算手段とを備えたことを特徴とする。

【００１４】また、上記ロボットシステムの制御装置に
おいて、好ましくは、上記加速度に関係しないトルク成
分を、上記粘性行列及び重力行列に加算することによ
り、上記各関節で発生する摩擦抵抗を考慮することを特
徴とする。

【００１５】さらに、上記ロボットシステムの制御装置
において、好ましくは、上記慣性行列の対角成分に、上
記各関節の慣性モーメントを加算することにより、上記
各関節自身の加減速で発生するトルク演算値を補正する
手段とをさらに備えたことを特徴とする。

【００１６】

【発明の実施の形態】以下、図面を参照して本発明に係
る実施形態について説明する。

【００１７】図１は、本発明に係る一実施形態であるロ
ボットシステムの構成を示すブロック図であり、図２
は、図１のロボット機構部１の構成を示すスケルトン図
である。図１において、この実施形態に係るロボットシ
ステムは、例えば３個である複数の関節ＲＪ１乃至ＲＪ
３を有するロボット機構部１と、ロボット機構部１の動
作を制御するための制御装置１０と、教示データなどの
データを入力するための入力手段であるティーチペンダ
ント２０とを備えて構成される。ここで、ロボット機構
部１は、図２に示すように、ベースＢＡ１からリンクＬ
１を介して関節ＲＪ１に連結され、次いで、関節ＲＪ１
はリンクＬ２を介して関節ＲＪ２に連結され、そして、
関節ＲＪ２はリンクＬ３を介して関節ＲＪ３に連結さ
れ、さらに、関節ＲＪ３にはリンクＬ４が連結され、そ
のリンクＬ４の端部は例えば溶接作業などの所定の操作
を行う操作端ＬＥとなっている。ここで、各関節ＲＪ１
乃至ＲＪ３は回転可能に構成されている。

【００１８】この実施形態に係るロボットシステムの制
御装置１０は、概略説明すれば、ロボット機構部１の各
関節ＲＪ１乃至ＲＪ３を駆動するモータの性能を効率よ
く使用することで、タクトタイム（工程時間）の短縮、
並びにマニピュレータ開発時のモータ選定裕度の向上を
行い、具体的には、関節ＲＪ１乃至ＲＪ３に発生させる
ことができる許容ピークトルクを指定しておき、ロボッ
ト機構部１の各関節ＲＪ１乃至ＲＪ３の開始位置、目標
位置及び目標速度と、各関節に連結されるロボットリン
ク（リンクアームともいうが、本実施形態では、リンク
Ｌ１乃至Ｌ４をいう。）の重心位置、質量及び慣性テン
ソルから成るダイナミクスベースパラメータから、各関
節の許容ピークトルクを越えず、かつ最短時間で動作が
終了できる最適な各関節の加減速度を求める。この場
合、ロボット機構部１の関節ＲＪ１乃至ＲＪ３毎の移動
量に大きな差がある位置決め動作を行う場合でも、駆動
関節の許容ピークトルクを最大限有効に使用した加減速
度を演算し、動作時間の短縮を可能にするとともに、関
節に連結されるリンク自体の慣性テンソルによる負荷ト
ルク成分を考慮し、関節の回転中心からリンクの重心位
置までの距離が短いロボットや、ロボットが特異な姿勢
となっても、厳密な負荷トルク成分の計算を実現するこ
とで、許容値を越えるトルクを必要とするような加減速
度の演算を防止する。また、この実施形態に係るロボッ
トシステムの制御装置１０は、モータＭＯ１乃至ＭＯ３
の軸受けや減速機の摩擦抵抗を考慮し、厳密な負荷トル
ク成分の計算を実現することで、許容値を越えるトルク
を必要とするような加減速度の演算を防止する。さら
に、この実施形態に係るロボットシステムの制御装置１
０は、減速機を含むモータＭＯ１乃至ＭＯ３のロータの
慣性モーメントにより発生するトルクを考慮し、厳密な
負荷トルク成分の計算を実現することで、許容値を越え
るトルクを必要とするような加減速度の演算を防止す
る。

【００１９】まず、この実施形態に係る制御方法の動作
原理について以下に詳述する。ロボット機構部１の各関
節に連結されるリンクの重心位置、質量、リンク長さ、
慣性テンソル並びに、各関節の位置（角度）、速度、加
速度から、各関節に発生するトルクＴが、次式により演
算できることが知られており、ロボットダイナミクスモ
デルという。

【００２０】

【数１】 Ｔ＝Ｈ（θ）・θｄｄ＋Ｃ（θ，θｄ）＋Ｇ（θ）・ｇ

【００２１】この数１は、例えば、従来技術文献１「広
瀬茂男，”ロボット工学−機械システムのベクトル解析
−”，裳華房，２版，１９８９年２月１０日」の１１．
３．［４］項に、剛体の運動解析手法の１つであるニュ
ートンオイラー法を用いた逆動力学演算の手順が詳しく
説明されており、最終的に逆動力学の演算式は、当該従
来技術文献１の式（１１．２８）となる。ここで、式
（１１．２８）中のＪ^TＫは、ロボット先端に加えるべ
き外力とモーメントで、本発明に係る実施形態では考慮
する必要の無い項となり、上記数１を得ることができ
る。ここで、上記ロボットダイナミクスモデルは、慣性
行列Ｈ、粘性行列Ｃ及び重力行列Ｇから構成され、慣性
行列Ｈはロボット機構部の各関節の加速度により関節自
身に発生するトルク並びに他関節の加速度により発生す
る干渉トルクを示し、粘性行列Ｃはロボット機構部の各
関節の遠心力及びコリオリ力により発生するトルクを示
し、重力行列Ｇは、ロボット機構部の各関節の重力モー
メントにより発生するトルクを示す。

【００２２】慣性行列Ｈ、粘性行列Ｃ及び重力行列Ｇ
は、ロボット機構部１の各関節に連結されるリンクの重
心位置、質量、慣性テンソル、及び各関節の位置、速
度、加速度から計算できる。ここで、ロボット各関節に
連結されるリンクの重心位置、質量、慣性テンソル、リ
ンク長さは、ロボットの機構やリンク形状及び材質から
予め求めておくことが可能な既知の固定値であるから、
上記数１を使用すると、各関節の位置、速度、加速度の
３つから、各関節に発生するトルクが演算できる。上記
数１の各項目は以下の意味を持つ。なお、実施例では、
各関節の数ｎ＝３である。

【００２３】

【表１】 ――――――――――――――――――――――――――――――――――― Ｔ：関節に発生するトルクｎ×１行列 θ：関節位置ｎ×１行列 θｄ：関節速度ｎ×１行列 θｄｄ：関節加速度ｎ×１行列 Ｈ：慣性行列ｎ×ｎ行列 Ｃ：粘性行列ｎ×１行列 Ｇ：重力行列ｎ×３行列 ｎ：ロボットの制御関節数 ｇ：重力加速度ベクトル３×１行列 ―――――――――――――――――――――――――――――――――――

【００２４】次いで、この実施形態で用いる各項目の単
位及び個数を以下に示す。

【００２５】

【表２】 ――――――――――――――――――――――――――――――――――― 記号 項目 単位 個数 ――――――――――――――――――――――――――――――――――― Ｔ 関節に発生するトルク Ｎ・ｍ ｎ×１行列 θ 関節位置（角度） ｒａｄ ｎ×１行列 θｄ 関節速度 ｒａｄ／ｓｅｃ ｎ×１行列 θｄｄ 関節加速度 ｒａｄ／ｓｅｃ² ｎ×１行列 Ｈ 慣性行列 ｋｇ・ｍ² ｎ×ｎ行列 Ｃ 粘性行列 Ｎ・ｍ ｎ×１行列 Ｇ 重力行列 ｋｇ・ｍ ｎ×３行列 ｇ 重力加速度ベクトル ｍ／ｓ² ３×１行列 Ｔｐｅａｋ 関節の許容ピークトルク Ｎ・ｍ ｎ×１行列 θｓ 開始位置 ｒａｄ ｎ×１ θｅ 目標位置 ｒａｄ ｎ×１ Ｖ 目標速度 ｒａｄ／ｓｅｃ ｎ×１ Ａｓｍｐ 仮の加速度 ｒａｄ／ｓｅｃ² ｎ×１ Ｄｓｍｐ 仮の減速度 ｒａｄ／ｓｅｃ² ｎ×１ Ｖｅｓｔ 仮速度予測値 ｒａｄ／ｓｅｃ ｎ×１ Ｔｅｓｔ 動作時間予測値 ｓｅｃ ｎ×１ Ｖｒｅａ 速度予測値 ｒａｄ／ｓｅｃ ｎ×１ Ａｒｅａ 加速度予測値 ｒａｄ／ｓｅｃ² ｎ×１ Ｄｒｅａ 減速度予測値 ｒａｄ／ｓｅｃ² ｎ×１ Ｔｍａｘ 最長動作時間 ｓｅｃ １ ――――――――――――――――――――――――――――――――――― Ｉｘｉ，Ｉｘｙｉ，Ｉｘｚｉ リンクの慣性テンソル ｋｇ・ｍ² ３×３ Ｉｘｙｉ，Ｉｙｉ，Ｉｙｚｉ Ｉｘｚｉ，Ｉｙｚｉ，Ｉｚｉ ――――――――――――――――――――――――――――――――――― ｍｉ リンク質量 ｋｇ １×ｎ ｍｘｉ，ｍｙｉ，ｍｚｉ リンク重心位置ベクトル ｍ ３×ｎ 記号なし 重力加速度 ｍ／ｓｅｃ² １ Ｃｍ 動粘性係数 Ｎ・ｍ・ｓｅｃ ｎ×１行列 θｄｍ モータ速度 ｒａｄ／ｓｅｃ ｎ×１行列 Ｆｃｏｕ クーロン摩擦トルク Ｎ・ｍ ｎ×１行列 ――――――――――――――――――――――――――――――――――― Ｉｍ 減速機を含むモータロータの ｋｇ・ｍ² １×ｎ 慣性モーメント ――――――――――――――――――――――――――――――――――― Ｔａｄ 加減速で使用できるトルク Ｎ・ｍ ｎ×１行列 Ａｏｐｔ 最適な加速度 ｒａｄ／ｓｅｃ² ｎ×１行列 Ｄｏｐｔ 最適な減速度 ｒａｄ／ｓｅｃ² ｎ×１行列 Ｔｂａｓｅ 基準動作時間 ｓｅｃ １ Ｓａｉ 第ｉの関節の移動量 ｒａｄ １ Ｖｏｐｔｉ 第ｉの関節の最適な速度 ｒａｄ／ｓｅｃ １ Ｔａｉ 第ｉの関節の加速時間 ｓｅｃ １ Ｔｃｉ 第ｉの関節の定速時間 ｓｅｃ １ Ｔｄｉ 第ｉの関節の減速時間 ｓｅｃ １ ξｉ 第ｉの関節の減速比 無次元 １ ―――――――――――――――――――――――――――――――――――

【００２６】次いで、実施例（ｎ＝３）におけるロボッ
トダイナミクスのベースパラメータについて説明する。
質量ｍｉは、第ｉの関節（ここで、ｉは自然数であり、
１から関節の最大個数までとる。）の動作により移動し
かつ第ｉ＋１の関節の動作によっては移動しない第ｉの
関節に連結されるリンクの質量を示す。また、重心位置
ｍｘｉ、ｍｙｉ、ｍｚｉは、第ｉの関節の動作により移
動し、かつ第ｉ＋１の関節の動作によっては移動しない
第ｉの関節に連結されるリンクの重心位置を示す。ま
た、第ｉの関節の慣性テンソルＩｘｉ乃至Ｉｚｉは、第
ｉの関節の動作により移動しかつ第ｉ＋１の関節の動作
によっては移動しない第ｉの関節に連結されるリンクの
慣性テンソルを示し、例えば、従来技術文献１の節３．
８に、演算手順が詳しく説明されており、最終的に慣性
テンソルの演算式は、当該従来技術文献１の式（３．３
８）となる。

【００２７】ここで、重心位置及び慣性テンソルを演算
する上での基準をリンク座標系とする。リンク座標系
は、例えば、従来技術文献２「吉川恒夫訳，”ロボット
・マニピュレータ”，株式会社コロナ社，初版，１９８
９年１２月１５日」の節２．９に各関節毎に割り当てる
リンク座標系の定義手順が詳細に説明されているが、こ
こでは、基本的なリンク座標系の定義について説明す
る。

【００２８】まず、慣性テンソルＩは、物体の回転し難
さを示すのに一般的に使用され、公知の通り次式の３×
３の対称行列で表され、その対角成分はロボット機構部
の各関節の慣性モーメントを、その他の成分は慣性乗積
の負の値を表す。

【００２９】

【数２】

【００３０】また、慣性モーメントは次式より求めら
れ、物体（ロボットリンク）の回り難さ及び止め難さを
表す。

【００３１】

【数３】Ｉｘ＝∫（ｙ²＋ｚ²）ｄｍ

【数４】Ｉｙ＝∫（ｚ²＋ｘ²）ｄｍ

【数５】Ｉｚ＝∫（ｘ²＋ｙ²）ｄｍ

【００３２】次いで、慣性乗積は、次式より求められ、
物体（ロボットリンク）が回転運動を行っているとき、
その運動を乱すモーメントを表す。

【００３３】

【数６】Ｉｘｙ＝∫ｘｙｄｍ

【数７】Ｉｙｚ＝∫ｙｚｄｍ

【数８】Ｉｚｘ＝∫ｚｘｄｍ

【００３４】ここで、式中のｘ，ｙ，ｚは、所定の慣性
テンソルの基準座標で表され、ｄｍは微小体積の質量を
示す。ここで、基準座標をリンク座標系とする。例え
ば、第２の関節から第３の関節に伸張するリンクＬ３に
おいて、第２の関節の回転中心を原点として、第２の関
節に割り当てられたリンク座標が定められ、第２の関節
のリンク座標におけるｘ方向をリンク長さの方向とし、
ｚ方向を回転軸方向とし、ｙ方向を右手座標系の方向と
する。ここで、第２の関節に連結されたリンクＬ３の慣
性テンソルの基準座標は、第２の関節に割り当てられた
リンク座標系をリンクＬ３の重心位置に移動したときの
座標系とする。他の関節についても同様に定義する。

【００３５】次いで、慣性、粘性及び重力の行列値の演
算方法について説明する。慣性行列Ｈ、粘性行列Ｃ、及
び重力行列Ｇは、次式を用いて数値計算して求める。な
お、第１の関節の位置をθ₁とし、第２の関節の位置を
θ₂とし、第３の関節の位置をθ₃とする。また、第１の
関節の速度をθｄ₁とし、第２の関節の速度をθｄ₂と
し、第３の関節の速度をθｄ₃とする。

【００３６】

【数９】

【００３７】ここで、

【数１０】 H11 =mx1²・m1+[2・a1・mx2・cos(θ₂)+(1/2)・mx2²{1+cos(2・θ₂)}+a1²+mz2²]m2 +(a1²+2{cos(θ₂)・a2-my3・sin(θ₂+θ₃)+mx3・cos(θ₂+θ₃)}a1 +[-my3{sin(2・θ₂+θ₃)+sin(θ₃)}+mx3{cos(2・θ₂+θ₃)+cos(θ₃)}]a2 -mx3・my3・sin(2・θ₃+2・θ₂)+(1/2)a2²{1+cos(2・θ₂)} -(1/2)my3²{cos(2・θ₃+2・θ₂)-1} +(1/2)mx3²{cos(2・θ₃+2・θ₂)+1)}m3+(1/2)Ix2{1-cos(2・θ₂)} +(1/2)Iy2{1+cos(2・θ₂)} +(1/2)Ix3{1-cos(2・θ₃+2・θ₂)}+(1/2)Iy3{cos(2・θ₃+2・θ₂)+1} -Ixy3・sin(2・θ₃+2・θ₂)+Iz1

【数１１】H12=-m2・mz2・mx2・sin(θ₂)

【数１２】H13=0

【００３８】

【数１３】H21=H12

【数１４】H22=m2・mx2²+[a2²+2{mx3・cos(θ₃)-my3・sin
(θ₃)}a2+my3²+mx3²]m3+Iz3+Iz2

【数１５】H23=[{mx3・cos(θ₃)-my3・sin(θ₃)}a2+my3²+
mx3²]m3+Iz3

【００３９】

【数１６】H31=H13

【数１７】H32=H23

【数１８】H33=(my3²+mx3²)m3+Iz3

【００４０】

【数１９】Ｃ＝［Ｃ１ Ｃ２ Ｃ３］^T

【００４１】ここで、

【数２０】 C1 =[{-mx2・m2(mx2・sin(2・θ₂)+2・a1・sin(θ₂)) +(-2・a1(mx3・sin(θ₂+θ₃)+my3・cos(θ₂+θ₃)+a2・sin(θ₂)) -2・a2(mx3・sin(2・θ₂+θ₃)+my3・cos(2・θ₂+θ₃))-2・my3・mx3・cos(2・θ₃+2・θ₂) +(my3²-mx3²)・sin(2・θ₃+2・θ₂)-a2²・sin(2・θ₂))m3 +(Ix3-Iy3)sin(2・θ₃+2・θ₂) +(Ix2-Iy2)sin(2・θ₂)-2・Ixy3・cos(2・θ₃+2・θ₂)}θd₂ +{m3(-2・a1(my3・cos(θ₂+θ₃)+mx3・sin(θ₂+θ₃)) -2・my3・mx3・cos(2・θ₃+2・θ₂) +(my3²-mx3²)sin(2・θ₃+2・θ₂) -a2(mx3(sin(2・θ₂+θ₃)+sin(θ₃))+my3(cos(2・θ₂+θ₃)+cos(θ₃))}] -2・Ixy3・cos(2・θ₃+2・θ₂)-(Iy3-Ix3)sin(2・θ₃+2・θ₂))θd₃)θd₁ -m2・mz2・mx2・cos(θ₂)・θd₂ ²

【数２１】 C2 =[(1/2)mx2・m2(mx2・sin(2・θ₂)+2・a1・sin(θ₂)) +(1/2)(Iy3-Ix3)sin(2・θ₃+2・θ₂) +(1/2)sin(2・θ₂)(Iy2-Ix2)+Ixy3・cos(2・θ₃+2・θ₂) +{a1(my3・cos(θ₂+θ₃)+mx3・sin(θ₂+θ₃)) +a2(mx3・sin(2・θ₂+θ₃)+my3・cos(2・θ₂+θ₃)) +(1/2)a2²・sin(2・θ₂)+(1/2)(mx3²-my3²)sin(2・θ₃+2・θ₂) +my3・mx3・cos(2・θ₃+2・θ₂) +a1・a2・sin(θ₂)}m3]θd₁ ²-2・m3・a2(mx3・sin(θ₃)+my3・cos(θ₃))θd₂・θd₃ -m3・a2(mx3・sin(θ₃)+my3・cos(θ₃))θd₃ ²

【数２２】 C3=[{a1(my3・cos(θ₂+θ₃)+mx3・sin(θ₂+θ₃)) +(1/2)a2{mx3(sin(2・θ₂+θ₃)+sin(θ₃))+my3(cos(2・θ₂+θ₃)+cos(θ₃))} +(1/2)(mx3²-my3²)sin(2・θ₃+2・θ₂)+my3・mx3・cos(2・θ₃+2・θ₂)}m3 +(1/2)(Iy3-Ix3)sin(2・θ₃+2・θ₂)+Ixy3・cos(2・θ₃+2・θ₂)]θd₁ ² +m3・a2(mx3・sin(θ₃)+my3・cos(θ₃))θd₂ ²

【００４２】

【数２３】

【００４３】ここで、

【数２４】 G11 =-sin(θ₁)・mx1・m1 +{cos(θ₁)・mz2-sin(θ₁)・a1-(1/2)mx2(sin(θ₁+θ₂)-sin(-θ₁+θ₂))}m2 -sin(θ₁)・a1・m3-(1/2)m3((sin(θ₁+θ₂)-sin(-θ₁+θ₂))a2 +mx3(-sin(-θ₁+θ₂+θ₃)+sin(θ₁+θ₂+θ₃)) +my3(cos(θ₁+θ₂+θ₃)-cos(-θ₁+θ₂+θ₃)))

【数２５】 G12 =cos(θ₁)・mx1・m1 +{sin(θ₁)・mz2+cos(θ₁)・a1+(1/2)mx2(cos(-θ₁+θ₂)+cos(θ₁+θ₂))}m2 +cos(θ₁)・a1・m3+(1/2)m3{(cos(-θ₁+θ₂)+cos(θ₁+θ₂))a2 +mx3(cos(-θ₁+θ₂+θ₃)+cos(θ₁+θ₂+θ₃)) +my3(-sin(-θ₁+θ₂+θ₃)-sin(θ₁+θ₂+θ₃))}

【数２６】G13=0

【００４４】

【数２７】G31=(1/2)m3・{mx3(-sin(-θ₁+θ₂+θ₃)-sin
(θ₁+θ₂+θ₃))-my3(cos(-θ₁+θ₂+θ₃)+cos(θ₁+θ₂+
θ₃))}

【数２８】G32=(1/2)m3・{mx3(cos(θ₁+θ₂+θ₃)-cos(-
θ₁+θ₂+θ₃))-my3(-sin(-θ₁+θ₂+θ₃)+sin(θ₁+θ₂+
θ₃))}

【数２９】G33=m3(-my3・sin(θ₂+θ₃)+mx3・cos(θ₂+
θ₃))

【００４５】

【数３０】 G21 =(1/2)mx2・m2(-sin(θ₁+θ₂)-sin(-θ₁+θ₂)) +(1/2)m3・a2(-sin(θ₁+θ₂)-sin(-θ₁+θ₂)) +(1/2)m3{mx3(-sin(-θ₁+θ₂+θ₃)-sin(θ₁+θ₂+θ₃))-my3(cos(-θ₁+θ₂+θ₃) +cos(θ₁+θ₂+θ₃))}

【数３１】 G22 =(1/2)mx2・m2(-cos(-θ₁+θ₂)+cos(θ₁+θ₂)) +(1/2)m3・a2(-cos(-θ₁+θ₂)+cos(θ₁+θ₂)) +(1/2)m3(mx3(cos(θ₁+θ₂+θ₃)-cos(-θ₁+θ₂+θ₃)) -my3(-sin(-θ₁+θ₂+θ₃)+sin(θ₁+θ₂+θ₃)))

【数３２】G23=m2・mx2・cos(θ₂)+cos(θ₂)m3・a2+m3(-my
3・sin(θ₂+θ₃)+mx3・cos(θ₂+θ₃))

【００４６】上記数９乃至数３２は、表２に示すパラメ
ータ、図２に示すリンク長さａ１、ａ２、並びに関節位
置θ、関節速度θｄのみから構成され、ここで、θ、θ
ｄ以外はすべて既知の値である。なお、ａ１はリンクＬ
２のリンク長さであり、ａ２はリンクＬ３のリンク長さ
である。従って、開始位置θｓと速度予測値Ｖｒｅａを
入力すれば、開始位置θｓにおけるロボットダイナミク
スモデルを計算することができ、同様に、目標位置θｅ
と速度予測値Ｖｒｅａを入力すれば、目標位置における
ロボットダイナミクスモデルが計算できる。具体的に
は、以下の式に示す通りである。速度予測値Ｖｒｅａの
演算方法については、後述するのでここでの説明は省略
する。

【００４７】

【数３３】開始位置の慣性行列：Ｈｓ＝Ｈ（θｓ）

【数３４】 開始位置の粘性行列：Ｃｓ＝Ｃ（θｓ，Ｖｒｅａ）

【数３５】開始位置の重力行列：Ｇｓ＝Ｇ（θｓ）

【数３６】目標位置の慣性行列：Ｈｅ＝Ｈ（θｅ）

【数３７】 目標位置の粘性行列：Ｃｅ＝Ｃ（θｅ，Ｖｒｅａ）

【数３８】目標位置の重力行列：Ｇｅ＝Ｇ（θｅ）

【００４８】ところで、ロボットの各関節に発生させる
ことができるトルクは、モータや機械の設計仕様から制
限される既知の値であり、これをＴｐｅａｋとすると、
上述の数１の左辺を次式のように置き換えられる。

【００４９】

【数３９】Ｔｐｅａｋ≧Ｈ（θ）・θｄｄ＋Ｃ（θ，θ
ｄ）＋Ｇ（θ）・ｇ

【００５０】ここで、Ｔｐｅａｋは、関節の許容ピーク
トルクｎ×１行列である。上記数３９を変形して次式の
数４０の形式に変換する。数４０の左辺は、関節の許容
ピークトルクＴｐｅａｋから、加減速度に関係の無い重
力モーメントや遠心力、コリオリ力等の外乱トルクを差
し引いた値であり、加減速に使用できるトルクを表す。
数４０の右辺は、ロボット各関節が加減速度θｄｄで動
作した場合に各関節で発生するトルクを表す。つまり、
数４０は、第１の課題を満足させる加減速度を演算する
上での条件式の１つとなる。

【００５１】

【数４０】Ｔｐｅａｋ−（Ｃ(θ,θｄ)＋Ｇ(θ)・ｇ）
≧Ｈ(θ)・θｄｄ

【００５２】数４０において、Ｔｐｅａｋ及びｇは既知
の固定値であり、また、Ｈ、Ｃ、Ｇは、ロボット各関節
に連結されるリンクの重心位置、質量、慣性テンソル、
リンク長さ、及び関節位置θ、関節速度θｄから演算さ
れるロボットのダイナミクスモデルである。従って、上
記数４０において、関節加速度θｄｄを除くと、未知の
値は関節速度θｄのみとなる。なぜなら、ロボット各関
節に連結されるリンクの重心位置、質量、慣性テンソ
ル、リンク長さは、ロボットの機構やリンク形状及び材
質等から予め求めておくことが可能な既知の固定値で、
また関節位置θは、予め教示された開始位置あるいは目
標位置を使用できるからである。

【００５３】そのため、本実施形態では、数４０におけ
る関節速度θｄを予測する手段を持つ。ここでことわっ
ておくが、θｄとして教示された目標速度を使用するこ
とはできない。すべての関節に対して最高速度での位置
決め動作を要求しても、各関節ごとに移動量が違うこと
が一般的であり、目標位置へ到達する時間すなわち動作
時間に差が発生する。そのため、すべての関節で動作時
間を同じにすると、移動量の少ない関節は、目標速度よ
り低い速度で動作する。同様の理由で、移動量の少ない
関節は加減速度も抑えられるので、この考慮をしなけれ
ば、上述の第１の課題を解決する最適な値を演算するこ
とはできない。加減速度が抑えられる関節は、他の関節
へ与える干渉力の影響を抑えることができる。そのた
め、本発明に係る実施形態では、関節毎の加減速度につ
いても予測する手段を持つ。

【００５４】関節毎の速度、加減速度の予測は、以下の
手順で行う。まず、ロボットのいかなる姿勢でも正常に
動作が可能な加速度及び減速度（それぞれ以下、仮の加
速度、仮の減速度という。）を予め記憶しておく。次
に、予め教示される開始位置θs、目標位置θe、及び目
標速度Ｖから、上記した仮の加減速度が保持されるよう
に、すべての関節の速度パターンを計算する。本発明で
は、この速度パターンの計算方法を、第１の速度パター
ン計算方法という。

【００５５】この第１の速度パターン計算方法は、後述
する図４の処理フローにおいて、ステップＳ２、Ｓ６及
びＳ９において用いるものであり、その方法について以
下に説明する。

【００５６】ロボットの第ｉの関節における、第１の速
度パターン計算方法は、第ｉの関節の教示される教示デ
ータの開始位置θｓｉ、目標位置θeｉ、目標速度Ｖｉ
から、目標加速度Ａｉ、目標減速度Ｄｉで動作するよう
に、各関節の速度パターンを生成する公知の計算方法で
ある。ここで、各関節の速度パターンとは、図７に示す
台形状の速度パターン、又は図８に示す三角形状の速度
パターンのことであり、第ｉの関節の加速時間Ｔａｉ、
定速時間Ｔｃｉ、減速時間Ｔｄｉ、動作速度Ｖｒｉがこ
の第１の速度パターン計算方法により演算される。ま
た、第ｉの関節の加速時間Ｔａｉ、定速時間Ｔｃｉ、減
速時間Ｔｄｉを加算した値が速度パターンにおける動作
時間Ｔｒｉとなる。第ｉの関節の場合、以下の式を使用
することで、第ｉの関節の開始位置θsｉ、目標位置θe
ｉ、目標速度Ｖｉ、目標加速度Ａｉ、目標減速度Ｄｉか
ら、加速時間Ｔａｉ、定速時間Ｔｃｉ、減速時間Ｔｄ
ｉ、動作時間Ｔｒｉ、動作速度Ｖｒｉを計算できる。

【００５７】

【数４１】Ｔａｉ＝Ｖｉ／Ａｉ

【数４２】Ｔｄｉ＝Ｖｉ／Ｄｉ

【数４３】Ｔｃｉ＝｛θｅｉ−θｓｉ−Ｖｉ（Ｔａｉ＋
Ｔｄｉ）／２｝／Ｖｉ

【数４４】Ｔｒｉ＝Ｔａｉ＋Ｔｃｉ＋Ｔｄｉ

【数４５】Ｖｒｉ＝Ｖｉ

【００５８】ここで、上記数４３により求められた定速
時間Ｔｃｉが０未満となったときは、図８に示す三角形
状の速度パターンとなるケースで、この場合は、以下の
式により、加速時間Ｔａｉ、定速時間Ｔｃｉ、減速時間
Ｔｄｉ、動作時間Ｔｒｉ、及び動作速度Ｖｒｉを計算で
きる。

【００５９】

【数４６】

【数４７】Ｔａｉ＝Ｖｉ／Ａｉ・ｓｆ

【数４８】Ｔｄｉ＝Ｖｉ／Ｄｉ・ｓｆ

【数４９】Ｔｃｉ＝０

【数５０】Ｔｒｉ＝Ｔａｉ＋Ｔｃｉ＋Ｔｄｉ

【数５１】Ｖｒｉ＝Ｖｉ・ｓｆ

【００６０】次に、すべての関節の速度パターンの中か
ら、動作時間が最大となる関節（以下、最大動作関節と
いう。）を抽出し、すべての関節において速度パターン
の動作時間が、最大動作関節の動作時間と同じになるよ
うに速度パターンの再生成を行う。この処理を、終了時
間の同期処理という。終了時間の同期処理については、
公知の技術であるので説明は省略する。以上の処理の一
例を図１０及び図１１に示す。図１０及び図１１からも
判るように、第１の速度パターン計算を行い、終了時間
の同期処理を行うことで、速度予測値Ｖｒｅａ、加速度
予測値Ａｒｅａ、減速度予測値Ｄｒｅａを演算できる。

【００６１】速度予測値Ｖｒｅａを上記数４０の関節速
度θｄに代入することにより、上記数４０は関節の加速
度又は減速度であるθｄｄ以外はすべて既知の値とな
る。また、他の関節の動作により干渉トルクを受ける駆
動関節の許容ピークトルクを最適に配分するために、演
算された加速度予測値Ａｒｅａ及び減速度予測値Ｄｒｅ
ａの各関節間の比例関係を求め、この比例関係と上記数
４０とを同時に満足する加減速度を求める。つまり、加
速時においては、予測された加速度の各関節間の比例関
係が保たれ、かつ上記数４０を満足させる加速度が、前
述の第１の課題を解決する最適な加速度Ａｏｐｔとな
り、減速時においては、予測された減速度の各関節間の
比例関係が保たれ、かつ上記数４０を満足させる減速度
が、前述の第１の課題を解決する最適な減速度Ｄｏｐｔ
となる。

【００６２】以上の結果、最適な加速度及び減速度が求
まるがこれだけでは最適な速度パターンが計算できたと
は言えない。なぜなら、最適な加速度Ａｏｐｔと最適な
減速度Ｄｏｐｔを使用しても、速度パターンの生成手段
が従来と同じであれば、関節毎に動作時間が変わり、図
１０及び図１１に示すような従来と同様の終了時間の同
期処理を施さねばならず、これにより最適な加減速度が
保持されないためである。そのため、本発明に係る実施
形態では、計算の結果求められた最適な加減速度が保持
されるような、速度パターン生成処理を含む。以下に、
その手順を説明する。

【００６３】前述した終了時間の同期処理において、最
長動作時間となった関節についてのみ、既に求めた最適
な加速度Ａｏｐｔ及び最適な減速度Ｄｏｐｔを用いて、
前述した第１の速度パターン計算方法により基準動作時
間Ｔｂａｓｅを求める（図４のステップＳ６）。その他
の関節については、最適な加速度Ａｏｐｔと最適な減速
度Ｄｏｐｔを保持し、かつ動作時間が上記基準動作時間
Ｔｂａｓｅとなるように、第２の速度パターン計算方法
を用いた速度パターン生成処理を実行する（図４のステ
ップＳ７）。これにより、すべての関節の動作時間が最
長動作関節と同じになるので、終了時間の同期処理を施
す必要が無くなり、先に計算された最適な加減速度が保
持される。

【００６４】先に求められた最適な加速度Ａｏｐｔ及び
最適な減速度Ｄｏｐｔを保持し、かつ動作時間が上記基
準動作時間Ｔｂａｓｅとなるような速度パターンが実現
できない関節が存在した場合は（図４のステップＳ８で
ＮＯ）、その関節のみ第１の速度パターン計算を行い、
すべての関節の速度パターン計算が終了した（図４のス
テップＳ９）後に、前述の終了時間の同期処理（図４の
ステップＳ１０）を施せばよい。このようなことが起こ
るのは、加減速度を最適化したことで、最適化前とは最
大動作関節が変わってしまうケースである。終了時間の
同期処理を施すことで、加減速度の最適度合いが若干低
下するが、加減速に使用できるトルクを越えることは無
い。

【００６５】以上のように演算される速度パターンで各
関節を動作させることにより、第１の課題を解決でき
る。第２の課題については、上記数４０の慣性行列Ｈ及
び粘性行列Ｃが関節に連結されるリンクの慣性テンソル
を含んだものになるため、必然的に達成可能となる。

【００６６】また、別の実施形態において、各関節内の
モータの軸受けや減速機の摩擦抵抗を考慮した場合につ
いて説明する。上記数１で表されるロボットダイナミク
スモデルを、次式のように変更することにより、各関節
内のモータの軸受けや減速機の摩擦抵抗を考慮できる。

【００６７】

【数５２】

【００６８】ここで、Ｃｍはモータの軸受けや減速機の
動粘性係数ｎ×１行列であり、動粘性係数とは、速度と
動摩擦トルクとの比例関係を表す定数である。また、動
摩擦トルクとは、速度に比例した大きさを持つトルク成
分である。さらに、θｄｍはモータの速度ｎ×１行列で
あり、Ｆｃｏｕはクーロン摩擦トルクである。クーロン
摩擦トルクとは、速度に無関係な大きさを持ち、常に運
動と逆の方向に働くトルク成分である。上記数５２にお
いて、ｓｇｎ（θｄｍ）はモータの速度ｎ×１行列に基
づいて次式のように決定される符号関数である。

【００６９】

【数５３】ｓｇｎ（θｄｍ） ＝１；θｄｍ＞０のとき ＝０；θｄｍ＝０のとき ＝−１；θｄｍ＜０のとき

【００７０】また、第ｉの関節の速度とモータの回転速
度の関係はその減速比ξｉから次式のように表される。

【００７１】

【数５４】θｄｍｉ＝θｄｉ・ξｉ

【００７２】さらに、上記数５２のθｄｍは、次式で表
される。

【００７３】

【数５５】 θｄｍ＝［θｄ₁・ξ₁，θｄ₂・ξ₂，θｄ₃・ξ₃］^T

【００７４】ここで、ξｉは、第ｉの関節内の減速機の
減速比である。ここで、パラメータＣｍ、Ｆｃｏｕ、ξ
も、予め設定可能な既知の値であるから、上記数５２で
あっても、上述と同様に最適な加減速度の演算が可能で
あるので、詳細については省略する。

【００７５】さらに、そのほかの別の実施形態において
は、上述の数９に示す慣性行列Ｈを以下のように補正す
ることにより、各関節内における減速機を含むモータロ
ータの慣性モーメントにより発生するトルクの考慮が可
能になる。

【００７６】

【数５６】

【００７７】ここで、Ｉｍｉは減速機を含むモータロー
タの慣性モーメントである。連動して動く回転体の慣性
モーメントは、その減速比の２乗分の１倍されて入力軸
の慣性モーメントに影響を与える。従って、入力軸の慣
性モーメントが出力軸に与える影響はその逆であるた
め、減速比の２乗倍されなければならない。減速機を含
むモータロータの慣性モーメントが減速比の２乗倍され
て加算されているのはそのためである。

【００７８】慣性行列Ｈの対角要素は自分自身に与える
トルクの影響を示し、他の要素は他の関節に与える干渉
トルクの影響を示している。減速機を含むモータロータ
の慣性モーメントが他の関節に干渉することは無いの
で、対角要素のみ補正すればよい。従って、Ｉｍｉ、ξ
ｉも、予め設定可能な既知の値であるから、上記数５６
であっても、上述と同様に最適な加減速度の演算が可能
であるので、詳細については省略する。

【００７９】次いで、図１を参照して、本実施形態に係
るロボットシステムの構成について説明する。図１に示
すように、制御装置１０は、（ａ）クロック発生器１１
０によって発生されるクロック信号と当該クロック信号
を所定の分周比で分周する分周器１１１によって発生さ
れる同期信号ＳＹＮＣに基づいてＲＯＭ１０１に格納さ
れたシステムプログラムに従って、ロボット機構部１の
動作を制御するＣＰＵ１００と、（ｂ）ロボット機構部
１の動作を制御するための位置決め動作処理を含むプロ
グラムと、当該プログラムを実行するために必要なデー
タを格納するＲＯＭ１０１と、（ｃ）ＣＰＵ１００のワ
ーキングエリアとして用いられ、計算途中のデータを一
時的に格納するＲＡＭ１０２と、（ｄ）上記位置決め動
作処理を実行するために必要なデータを予め格納する記
憶装置であって、教示データメモリ１０６ａと、仮の加
減速度情報メモリ１０６ｂと、ロボットダイナミクスベ
ースパラメータメモリ１０６ｃとを含むハードディスク
メモリ１０６と、（ｅ）ティーチペンダント２０が接続
されたインターフェース１０３と、（ｆ）各モータＭＯ
１乃至ＭＯ３に対する駆動制御値であるモータ指令位置
のデータを格納するデュアルポートＲＡＭ１０４とを備
える。これらの各回路１００乃至１０３及び１０６とデ
ュアルポートＲＡＭ１０４の第１のポートがバス１０９
を介して接続される。なお、ティーチペンダント２０に
ついての処理はＣＰＵ１００の割り込み処理によって実
行される。

【００８０】ハードディスクメモリ１０６内の教示デー
タメモリ１０６ａは、ロボット機構部１を動作させると
きのロボット機構部１の各関節の開始位置と、目標位置
と、目標速度とを、教示データとして予めティーチペン
ダント２０を用いて入力して格納する。また、仮の加減
速度情報メモリ１０６ｂは、速度パターンを予測演算す
るときに用いる各関節の仮の加速度と仮の減速度の情報
を予めティーチペンダント２０を用いて入力して格納す
る。さらに、ロボットダイナミクスベースパラメータメ
モリ１０６ｃは、ロボット機構部１の各関節に連結され
るリンクの慣性テンソルと、質量と、重心位置とを含む
ロボットダイナミクスベースパラメータをティーチペン
ダント２０を用いて入力して格納する。

【００８１】デュアルポートＲＡＭ１０４の第２のポー
トはラッチ回路１０５に接続され、ラッチ回路１０５は
同期信号ＳＹＮＣに同期して、デュアルポートＲＡＭ１
０４に格納された各モータＭＯ１乃至ＭＯ３に対する３
個のモータ指令位置のデータを同時にラッチした後、ロ
ボット機構部１の関節制御に関するモータ指令位置のデ
ータをサーボ制御回路１０７を介してモータＭＯ１乃至
ＭＯ３に出力して駆動する。

【００８２】次いで、ロボット機構部１の各関節ＲＪｋ
（実施例では、ｋ＝１，２，３）の構造とサーボ制御回
路１０７の構成について図３を参照して説明する。図３
に示すように、モータＭＯｋの回転軸は減速機ＲＥｋの
入力軸に連結され、減速機ＲＥｋはモータＭＯｋの回転
速度を所定の減速比λｋだけ減速することによって、モ
ータＭＯｋによって発生されるトルクを上記減速比の逆
数倍だけ増大させて、減速機ＲＥｋの出力軸に連結され
たリンクＬｋに出力する。ここで、モータＭＯｋの回転
軸にはセンサＳＮｋの軸が連結され、当該センサＳＮｋ
からの出力信号がサーボ制御回路１０７内の加算器１２
０の減算入力端子に入力される。一方、ラッチ回路１０
５からのモータ指令位置ＤＰｋのデータが加算器１２０
の加算入力端子に入力される。加算器１２０から出力さ
れるデータは、Ｄ／Ａ変換器（図示せず。）によってア
ナログ電圧信号に変換された後、所定の増幅度を有する
増幅器１２１を介してモータＭＯｋの駆動端子に印加さ
れる。以上のように構成されたサーボ制御のフィードバ
ック系においては、回転角度がモータ指令位置ＤＰｋと
なるように制御され、これによって、モータと減速機を
介して連結されるリンクＬｋが、ＤＰｋを減速比で除し
た分である関節変数θｋだけ回転される。

【００８３】図４は、図１のＣＰＵ１００によって実行
される位置決め動作処理を示すフローチャートである。

【００８４】図４において、まず、ステップＳ１におい
て、教示データメモリ１０６ａ内の教示データに基づい
て、ロボット機構部１の各関節の開始位置、目標位置及
び目標速度を設定し、ステップＳ２において、上記ステ
ップＳ１で設定されたデータと、仮の加減速度情報メモ
リ１０６ｂ内の仮の加減速度情報とに基づいて、第１の
速度パターン計算を行い、ロボット機構部１の各関節の
動作時間と仮速度予測値とを演算する。この処理は、図
１０の従来例のステップＳ１０３及びＳ１０４と同様の
処理である。具体的には、第１の速度パターン計算方法
における目標加速度Ａとして仮の加速度Ａｓｍｐを、目
標減速度Ｄとして仮の減速度Ｄｓｍｐを使用する。ま
た、第１の速度パターン計算方法により演算される動作
速度Ｖｒを仮速度予測値Ｖｅｓｔとし、動作時間Ｔｒを
動作時間予測値Ｔｅｓｔとする。

【００８５】さらに、ステップＳ３において、ステップ
Ｓ２で演算された全ての関節の速度パターンに対して終
了時間の同期処理を実行する。この終了時間の同期処理
は、図１０の従来例のステップＳ１０９乃至Ｓ１１１の
処理（以下、従来例の速度パターン再計算処理とい
う。）と同様の処理であり、上記動作時間予測値の中か
ら、動作時間が最大となる関節を最大動作関節とし、そ
の最大動作関節の動作時間を最長動作時間Ｔｍａｘとす
る。そして、各関節について、上記動作時間予測値が上
記最長動作時間と同じになるように、動作速度及び加減
速度を再計算した値をそれぞれ、速度予測値、加速度予
測値及び減速度予測値としてＲＡＭ１０２に一時的に格
納する。

【００８６】そして、ステップＳ４においてロボットダ
イナミクスベースパラメータメモリ１０６ｃ内のロボッ
トダイナミクスベースパラメータに基づいて、開始位置
及び目標位置のロボットダイナミクスモデルの演算処理
を実行する。この処理では、上記開始位置及び目標位置
と上記速度予測値と上記ロボットダイナミクスベースパ
ラメータから、数９乃至数３８を用いて、開始位置及び
目標位置における慣性行列Ｈ、粘性行列Ｃ及び重力行列
Ｇを含むロボットダイナミクスモデルを演算してロボッ
トダイナミクスベースパラメータメモリ１０６ｃに格納
する。さらに、ステップＳ５において最適な加減速度の
演算を実行する。この処理では、上記演算されたロボッ
トダイナミクスモデルに基づいて、上記数４０と、上記
加速度予測及び減速度予測における各関節間の比例関係
を同時に満足する加減速度を、最適な加減速度として求
める。つまり、加速時においては、加速度予測値Ａｒｅ
ａの各関節間の比例関係（後述する数７６）が保たれ、
かつ数４０を満足するような最適な加速度Ａｏｐｔを演
算する一方、減速時においては、減速度予測値Ｄｒｅａ
の各関節間の比例関係（後述する数８９）が保たれ、か
つ数４０を満足するような最適な減速度Ｄｏｐｔを演算
し、演算された最適な加速度Ａｏｐｔ及び最適な減速度
ＤｏｐｔをＲＡＭ１０２内に格納する。

【００８７】次いで、ステップＳ６では、ステップＳ３
の処理結果に基づいて、最長動作時間となった関節を検
索し、検索した関節である第ｋの関節について、ステッ
プＳ５で演算された最適な加速度Ａｏｐｔｋと、最適な
減速度Ｄｏｐｔｋと、第ｋ関節の教示データ（開始位
置、目標位置、目標速度）とから、上述の第１の速度パ
ターン計算方法を用いて第ｋ関節の速度パターンを演算
し、その動作時間を基準動作時間Ｔｂａｓｅとし、演算
結果をＲＡＭ１０２に格納する。さらに、ステップＳ７
では、上記の第ｋの関節以外のすべての関節について、
第２の速度パターン計算方法を用いて、基準動作時間Ｔ
ｂａｓｅと、最適な加速度Ａｏｐｔと、最適な減速度Ｄ
ｏｐｔと、教示データ（開始位置、目標位置）とから、
動作時間が基準動作時間Ｔｂａｓｅとなり、最適な加速
度Ａｏｐｔで動作し、最適な減速度Ｄｏｐｔで動作する
速度パターンを生成してＲＡＭ１０２に格納する。そし
て、ステップＳ８では、動作時間が基準動作時間Ｔｂａ
ｓｅとなり、最適な加速度Ａｏｐｔで動作し、最適な減
速度Ｄｏｐｔで動作する速度パターンがすべての関節で
存在したか否かが判断され、ＹＥＳのときは当該位置決
め動作処理を終了するが、ＮＯのときはステップＳ９に
進む。

【００８８】ステップＳ９では、動作時間が基準動作時
間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔで動作し、
最適な減速度Ｄｏｐｔで動作する速度パターンが存在し
なかったすべての関節について、第１の速度パターン計
算方法を用いて、最適な加速度Ａｏｐｔと、最適な減速
度Ｄｏｐｔと、教示データ（開始位置、目標位置、目標
速度）から速度パターンを演算してＲＡＭ１０２に格納
する。最後に、ステップＳ１０では、ステップＳ６、Ｓ
７及びＳ９で演算された速度パターンに基づいて、すべ
ての関節の速度パターンの中から最長動作時間を抽出
し、従来例の速度パターン再計算処理と同様に、すべて
の関節について、最長動作時間と一致するように、速度
パターンを再計算して計算結果をＲＡＭ１０２に格納す
る。

【００８９】この位置決め動作処理の後、生成された速
度パターンに基づいて、公知の通り、モータ指令位置Ｄ
Ｐｋのデータを計算してデュアルポートＲＡＭ１０４、
ラッチ回路１０５及びサーボ制御回路１０７を介してロ
ボット機構部１に出力することにより、最適に位置決め
動作処理がなされたデータに基づいて、ロボット機構部
１の動作を制御することができる。

【００９０】なお、以上の実施形態において、上記数５
２に示すように、加速度に関係しないトルク成分とし
て、粘性行列Ｃ及び重力行列Ｇに加え、モータＭＯ１乃
至ＭＯ３の軸受けや減速機ＲＥ１乃至ＲＥ３から発生す
る摩擦抵抗を考慮することで、加速に使用することので
きるトルクの演算精度を向上させてもよい。また、上記
数５６に示すように、慣性行列Ｈの対角成分に、減速機
ＲＥ１乃至ＲＥ３を含むモータＭＯ１乃至ＭＯ３のロー
タの慣性モーメントを加算することで、自分自身の加減
速で発生するトルク演算値を補正してもよい。

【００９１】図４のステップＳ８において、動作時間が
基準動作時間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔ
で動作し、最適な減速度Ｄｏｐｔで動作する速度パター
ン存在しない関節があったとき（ステップＳ８でＮ
Ｏ）、その関節を最長動作時間の関節の番号をｋとし、
ステップＳ６からの処理を再度行ってもよい。

【００９２】

【実施例】図２に示すロボット機構部１（ｎ＝３）にお
ける、位置決め動作のための最適な加減速度の演算例を
図４のフローチャートを参照して説明する。

【００９３】ステップＳ１では、ロボット機構部１の各
関節について、位置決め動作の開始位置、目標位置及び
目標速度を、教示データメモリ１０６ａ内の教示データ
から読み出して以下のようにセットする。

【００９４】

【数５７】 開始位置：θｓ＝［θｓ１，θｓ２，θｓ３］^T

【数５８】 目標位置：θｅ＝［θｅ１，θｅ２，θｅ３］^T

【数５９】目標速度：Ｖ＝［Ｖ１，Ｖ２，Ｖ３］^T

【００９５】一方、仮の加減速度情報メモリ１０６ｂに
は、ロボットのすべての姿勢で動作が可能な仮の加減速
度情報の値が以下のように予めセットされる。

【００９６】

【数６０】仮の加速度：Ａｓｍｐ＝［Ａｓｍｐ１，Ａｓ
ｍｐ２，Ａｓｍｐ３］^T

【数６１】仮の減速度：Ｄｓｍｐ＝［Ｄｓｍｐ１，Ｄｓ
ｍｐ２，Ｄｓｍｐ３］^T

【００９７】そして、ステップＳ２では、上記セットさ
れた開始位置、目標位置、目標速度、仮の加速度及び仮
の減速度に基づいて、各関節毎に前述の第１の速度パタ
ーン計算を行う。第１の速度パターンの計算は、従来例
と同様である。第１の速度パターン計算により、以下の
値が予測される。その概念を図５に示す。図５の例で
は、第１の関節及び第２の関節は、目標速度に達するに
必要な移動量があったが、第３の関節は移動量が少な
く、目標速度に達していない。

【００９８】

【数６２】仮速度予測値：Ｖｅｓｔ＝［Ｖｅｓｔ１，Ｖ
ｅｓｔ２，Ｖｅｓｔ３］^T

【数６３】動作時間予測値：Ｔｅｓｔ＝［Ｔｅｓｔ１，
Ｔｅｓｔ２，Ｔｅｓｔ３］^T

【００９９】さらに、ステップＳ３においては、前述の
終了時間の同期処理後におけるロボット各関節の速度及
び加減速度の値を以下のように再予測する。

【０１００】

【数６４】速度予測値：Ｖｒｅａ＝［Ｖｒｅａ１，Ｖｒ
ｅａ２，Ｖｒｅａ３］^T

【数６５】加速度予測値：Ａｒｅａ＝［Ａｒｅａ１，Ａ
ｒｅａ２，Ａｒｅａ３］^T

【数６６】減速度予測値：Ｄｒｅａ＝［Ｄｒｅａ１，Ｄ
ｒｅａ２，Ｄｒｅａ３］^T

【０１０１】上記動作時間予測値における最長動作時間
をＴｍａｘとおくと、上記の値は、以下の式から演算で
きる。終了時間の同期処理の概念を図６に示す。図６の
予測例では、移動量の少ない第２の関節及び第３の関節
の速度及び加減速度が低下しているのがわかる。

【０１０２】

【数６７】 Ｖｒｅａｉ＝Ｖｅｓｔｉ・Ｔｅｓｔｉ／Ｔｍａｘ

【数６８】 Ａｒｅａｉ＝Ａｓｍｐｉ・（Ｔｅｓｔｉ／Ｔｍａｘ）²

【数６９】 Ｄｒｅａｉ＝Ｄｓｍｐｉ・（Ｔｅｓｔｉ／Ｔｍａｘ）²

【０１０３】なお、ロボットダイナミクスベースパラメ
ータメモリ１０６ｃは、上記数１で表されるロボットダ
イナミクスモデルを生成するための、ロボットダイナミ
クスベースパラメータを予め格納する。ロボットダイナ
ミクスベースパラメータの一覧を以下の表に示す。

【０１０４】

【表３】 第ｉの関節に連結されるリンクのロボットダイナミクスベースパラメータ ――――――――――――――――――――――――――――――――――― 項目 式中の記号 個数 ――――――――――――――――――――――――――――――――――― 慣性テンソル Ｉｘｉ，Ｉｘｙｉ，Ｉｘｚｉ ３×３行列 Ｉｘｙｉ，Ｉｙｉ，Ｉｙｚｉ Ｉｘｚｉ，Ｉｙｚｉ，Ｉｚｉ ――――――――――――――――――――――――――――――――――― 質量 ｍｉ １ ――――――――――――――――――――――――――――――――――― 重心位置 ｍｘｉ，ｍｙｉ，ｍｚｉ ３×１行列 ―――――――――――――――――――――――――――――――――――

【０１０５】次いで、ステップＳ４では、上記ロボット
ダイナミクスベースパラメータと、ロボット機構部の各
関節の位置及び速度から、上記数１で表現されるロボッ
トダイナミクスモデルを生成する。ロボットダイナミク
スモデルは、慣性行列Ｈと、粘性行列Ｃと、重力行列Ｇ
から構成される。図２の機構を持つロボット機構部１に
おけるダイナミクスモデルを、ニュートン及びオイラー
法に基づき計算したものが数９乃至数３８に示されてい
る。これら数９乃至数３８に示されたモデル式におい
て、未知の値は、関節の位置及び速度のみであり、これ
に開始位置θｓと、速度予測値Ｖｒｅａとを入力すれ
ば、開始位置におけるロボットダイナミクスモデルを計
算できる。同様に、目標位置θｅと、速度予測値Ｖｒｅ
ａを入力すれば、目標位置におけるロボットダイナミク
スモデルを計算できる。従って、ステップＳ４では、以
下のロボットダイナミクスモデルが計算される。

【０１０６】

【表４】 ―――――――――――――――― Ｈｓ：開始位置の慣性行列 Ｃｓ：開始位置の粘性行列 Ｇｓ：開始位置の重力行列 Ｈｅ：目標位置の慣性行列 Ｃｅ：目標位置の粘性行列 Ｇｅ：目標位置の重力行列 ――――――――――――――――

【０１０７】次いで、ステップＳ５では、最適な加速度
Ａｏｐｔ及び最適な減速度Ｄｏｐｔを、以下の手順で演
算する。最初は、最適な加速度Ａｏｐｔを求める。そし
て、Ｈｓ、Ｇｓ、Ｃｓをそれぞれ数４０のＨ、Ｇ、Ｃに
代入し、最適な加速度Ａｏｐｔ（又は最適な減速度Ｄｏ
ｐｔ）を数４０のθｄｄとする。また、簡略化のため数
４０の左辺を１つにまとめると次式のように表すことが
できる。

【０１０８】

【数７０】Ｔａｄ≧Ｈｓ（θｓ)・Ａｏｐｔ

【０１０９】数７０の左辺のＴａｄは、次式で表され、
加速時においては加速に使用できるトルクであり、減速
時においては減速に使用できるトルクとなる。ここで
は、加速度を求めようとしているので、Ｔａｄは加速に
使用できるトルクを表す。加速時と減速時とでは、使用
するトルクの符号が異なるので、前述した終了時間の同
期処理後における各関節加速度の予測値Ａｒｅａの符号
を考慮する。

【０１１０】

【数７１】Ｔａｄ＝ｓｇｎ２（Ａｒｅａ）・Ｔｐｅａｋ
−(Ｃｓ(θｓ，Ｖｒｅａ)＋Ｇ(θｓ)・ｇ）

【０１１１】ここで、符号関数ｓｇｎ２は次式で表され
る。

【０１１２】

【数７２】ｓｇｎ２（Ａｒｅａ） ＝１；Ａｒｅａ≧０のとき ＝−１；Ａｒｅａ＜０のとき

【０１１３】上記数７０を要素別に展開すると、次式の
ように表される。Ｔａｄは、加速に使用できるトルクを
表しているから、次式が加速に使用できるトルクを最大
限利用するための加速度を求める条件式となる。

【０１１４】

【数７３】Ｔａｄ１≧Ｈｓ１１・Ａｏｐｔ１＋Ｈｓ１２
・Ａｏｐｔ２＋Ｈｓ１３・Ａｏｐｔ３

【数７４】Ｔａｄ２≧Ｈｓ２１・Ａｏｐｔ１＋Ｈｓ２２
・Ａｏｐｔ２＋Ｈｓ２３・Ａｏｐｔ３

【数７５】Ｔａｄ３≧Ｈｓ３１・Ａｏｐｔ１＋Ｈｓ３２
・Ａｏｐｔ２＋Ｈｓ３３・Ａｏｐｔ３

【０１１５】前述した終了時間の同期処理後における各
関節加速度の予測値はＡｒｅａであった。各関節の最適
な加速度Ａｏｐｔも、加速度予測値Ａｒｅａと同じ比例
関係を保つものとすると、次式の関係を満たさなければ
ならない。

【０１１６】

【数７６】Ａｏｐｔ１：Ａｏｐｔ２：Ａｏｐｔ３＝Ａｒ
ｅａ１：Ａｒｅａ２：Ａｒｅａ３

【０１１７】上記数７３乃至７５及び数７６の関係を満
たす加速度Ａｏｐｔが、加速度予測値Ａｒｅａの各関節
間の比例関係を保ち、かつ加速に使用できるトルクを最
大限利用できる加速度であり、本実施形態において、最
適となる値となる。基準関節は、動作する関節であれば
どれでも良いが、本実施例では、Ａｒｅａの中で絶対値
が最大となる関節を求め、これを基準関節ｊとする。基
準関節ｊをベースに上記数７６を次式のように変形す
る。

【０１１８】

【数７７】 Ａｏｐｔ１＝（Ａｒｅａ１／Ａｒｅａｊ）・Ａｏｐｔｊ

【数７８】 Ａｏｐｔ２＝（Ａｒｅａ２／Ａｒｅａｊ）・Ａｏｐｔｊ

【数７９】 Ａｏｐｔ３＝（Ａｒｅａ３／Ａｒｅａｊ）・Ａｏｐｔｊ

【０１１９】ここで、Ａｒｅａｊは基準関節ｊの予測加
速度であり、Ａｏｐｔｊは基準関節ｊの最適な加速度で
ある。次に、上記数７７乃至７９を上記数７３乃至７５
に代入して展開すると、次の３個の不等式になる。

【０１２０】

【数８０】Tad1≧H11・(Area1/Areaj)・Aoptj+H12・(Area2
/Areaj)・Aoptj+H13・(Area3/Areaj)・Aoptj

【数８１】Tad2≧H21・(Area1/Areaj)・Aoptj+H22・(Area2
/Areaj)・Aoptj+H23・(Area3/Areaj)・Aoptj

【数８２】Tad3≧H31・(Area1/Areaj)・Aoptj+H32・(Area2
/Areaj)・Aoptj+Hn3・(Area3/Areaj)・Aoptj

【０１２１】上記数８０乃至８２の３個の不等式におい
て、基準関節ｊにおける最適な加速度Ａｏｐｔｊ以外は
すべて既知の値であるから、上記数８０乃至８２の３個
の不等式から、基準関節ｊにおける最適な加速度が３通
り計算できる。このときに計算される３個の加速度をそ
れぞれを、Ａｏｐｔｊ＿１、Ａｏｐｔｊ＿２、Ａｏｐｔ
ｊ＿３とすると、次式を得る。

【０１２２】

【数８３】 Aoptj_1=Tad1・Areaj/(H11・Area1+H12・Area2+H13・Area3)

【数８４】 Aoptj_2=Tad2・Areaj/(H21・Area1+H22・Area2+H23・Area3)

【数８５】 Aoptj_3=Tad3・Areaj/(H31・Area1+H32・Area2+H33・Area3)

【０１２３】上記数８３乃至８５に示す加速度Ａｏｐｔ
ｊ＿１、Ａｏｐｔｊ＿２、Ａｏｐｔｊ＿３から、絶対値
が最小の値を基準関節ｊの最適な加速度としなければな
らない。なぜなら、絶対値が最小の値のみが、上記数８
０乃至数８２の３個の不等式を同時に満たすことができ
るからである。加速度Ａｏｐｔｊ＿１、Ａｏｐｔｊ＿
２、Ａｏｐｔｊ＿３の中で最小の値を最小の加速度Ａｏ
ｐｔｊ＿ｍｉｎとし、これを上記数７７乃至数７９に代
入すると次式となり、これによりすべての関節の最適な
加速度を求めることができる。

【０１２４】

【数８６】Ａｏｐｔ１＝（Ａｒｅａ１／Ａｒｅａｊ）・
Ａｏｐｔｊ＿ｍｉｎ

【数８７】Ａｏｐｔ２＝（Ａｒｅａ２／Ａｒｅａｊ）・
Ａｏｐｔｊ＿ｍｉｎ

【数８８】Ａｏｐｔ３＝（Ａｒｅａ３／Ａｒｅａｊ）・
Ａｏｐｔｊ＿ｍｉｎ

【０１２５】このようにして求められた加速度が、第１
の課題を満たす最適な加速度となる。最適な減速度Ｄｏ
ｐｔについても、全く同様の手順で求めることができ
る。具体的には、上記数７０乃至数８８において、パラ
メータθｓ、Ｈｓ、Ｃｓ、Ｇｓ、Ａｏｐｔ、Ａｒｅａの
代わりにそれぞれ、パラメータθｅ、Ｈｅ、Ｃｅ、Ｇ
ｅ、Ｄｏｐｔ、Ｄｒｅａを使用すればよい。ここで、加
速度に関する数７６に対応する減速度に関する式は次式
となる。

【０１２６】

【数８９】Ｄｏｐｔ１：Ｄｏｐｔ２：Ｄｏｐｔ３＝Ｄｒ
ｅａ１：Ｄｒｅａ２：Ｄｒｅａ３

【０１２７】次いで、ステップＳ６では、以上の計算の
結果求められた最適な加減速度が保持されるような、速
度パターンの演算処理を行う。以下に、その手順を説明
する。ステップＳ３の終了時間の同期処理において、最
長動作時間となった第ｋ関節についてのみ、最適な加速
度Ａｏｐｔｋ、最適な減速度Ｄｏｐｔｋを用いて、前述
した第１の速度パターン計算により基準動作時間Ｔｂａ
ｓｅを求める。次いで、ステップＳ７では、第ｋ関節以
外の関節について、加速度が最適な加速度Ａｏｐｔとな
り、減速度が最適な減速度Ｄｏｐｔとなり、かつ動作時
間が基準動作時間Ｔｂａｓｅとなるように、第２の速度
パターン計算を行う。ステップＳ６、Ｓ７により、すべ
ての関節の速度パターンにおける動作時間が基準動作時
間Ｔｂａｓｅとなるので、終了時間の同期処理を施す必
要が無くなり、先に計算された最適な加減速度が保持さ
れる。

【０１２８】ロボットの第ｉの関節における、第２の速
度パターン計算方法について図９を交えて具体的に説明
する。図９から判るように、既知の値である基準動作時
間Ｔｂａｓｅ、第ｉの関節の最適な加速度Ａｏｐｔｉ、
第ｉの関節の最適な減速度Ｄｏｐｔｉ、第ｉの関節の移
動量Ｓａｉは、第ｉの関節の最適な速度Ｖｏｐｔｉ、第
ｉの関節の加速時間Ｔａｉ、第ｉの関節の定速度時間Ｔ
ｃｉ、第ｉの関節の減速時間Ｔｄｉにより以下の式で表
される。

【０１２９】

【数９０】Ｔｂａｓｅ＝Ｔａｉ＋Ｔｃｉ＋Ｔｄｉ

【数９１】Ｓａｉ＝Ｖｏｐｔｉ・（Ｔｃｉ＋（Ｔａｉ＋
Ｔｄｉ）／２）

【数９２】Ｖｏｐｔｉ＝Ｔａｉ・Ａｏｐｔｉ

【数９３】Ｖｏｐｔｉ＝Ｔｄｉ・Ｄｏｐｔｉ

【０１３０】上記数９０乃至９３の４つの式からなる連
立方程式を解くと、時間Ｔｃｉ、Ｖｏｐｔｉ、Ｔａｉ、
Ｔｄｉを、以下の式から演算できる。

【０１３１】

【数９４】

【０１３２】

【数９５】Ｖｏｐｔｉ＝(Ｔｂａｓｅ−Ｔｃｉ)・Ａｏｐ
ｔｉ・Ｄｏｐｔｉ／(Ｄｏｐｔｉ＋Ａｏｐｔｉ)

【数９６】Ｔａｉ＝Ｖｏｐｔｉ／Ａｏｐｔｉ

【数９７】Ｔｄｉ＝Ｖｏｐｔｉ／Ｄｏｐｔｉ

【０１３３】ただし、加速度が最適な加速度Ａｏｐｔｉ
となり、減速度が最適な減速度Ｄｏｐｔｉとなり、かつ
動作時間が基準動作時間Ｔｂａｓｅとなるような、速度
パターン計算が求まらないケースがある。それは、次の
２つのケースである。（ａ）上記数９４において、定速
時間Ｔｃｉが虚数として求まる場合、及び（ｂ）上記数
９４において、最適な速度Ｖｏｐｔｉが教示速度Ｖｉを
越える値として求まる場合。

【０１３４】ステップＳ８では、上記の２つのケースに
該当する関節が存在した場合をＮＯと判定する。ステッ
プＳ９では、ステップＳ８でＮＯと判定された全ての関
節について、従来と同様の第１の速度パターン計算を実
施する。次いでステップＳ１０では、すべての関節の速
度パターンについて、前述の終了時間の同期処理を施
す。ステップＳ８でＮＯと判定されるのは、加減速度を
最適化したことで、最適化前とは最大動作関節が変わっ
てしまうケースであるあるが、ステップＳ９、Ｓ１０の
処理を実施することで、加減速度の最適度合いが若干低
下するが、加減速に使用できるトルクを越えることは無
い。

【０１３５】最後に、上記数７１におけるｇは重力加速
度ベクトルであり、重力の働く方向を定義するものであ
る。一般的に、ロボット座標において重力が−ｚ方向に
働くよう座標を定義するので、ｇは次式のようになる。
なお、重力加速度は、９．８ｍ／ｓｅｃ²である。

【０１３６】

【数９８】ｇ＝［０，０，−１×（重力加速度）］^T

【０１３７】以上説明したように、本発明に係る実施形
態によれば、開始位置及び目標位置と速度予測値とロボ
ットダイナミクスベースパラメータから、上記数９乃至
数３８を用いて、開始位置及び目標位置における慣性行
列Ｈ、粘性行列Ｃ及び重力行列Ｇを含むロボットダイナ
ミクスモデルを計算し（ステップＳ４）、計算されたロ
ボットダイナミクスモデルに基づいて、加速度予測値の
各関節間の比例関係と各関節の許容ピークトルクの条件
を満足する最適な加速度及び減速度を演算した（ステッ
プＳ５）後、すべての関節で動作時間を一致させる同期
処理を実行する（ステップＳ６、Ｓ７）。従って、ロボ
ット機構部１の各関節毎の移動量に大きな差がある位置
決め動作を行う場合でも、駆動関節の許容トルクを最大
限有効に使用した加減速度を演算し、動作時間の短縮を
可能にし、関節に連結されるリンク自体の慣性による負
荷トルク成分を考慮し、関節回転中心から関節に連結さ
れるリンクの重心位置までの距離が短いロボットや、ロ
ボットが特異な姿勢となっても、厳密な負荷トルク成分
の計算を実現することで、許容値を越えるトルクを必要
とするような加減速度の演算を防止する。これにより、
ロボット機構部１の各関節の許容ピークトルクを越え
ず、かつ最短時間で動作が終了できる最適な各関節の加
減速度を求めることができる。

【０１３８】また、動作時間が基準動作時間Ｔｂａｓｅ
となり、最適な加速度Ａｏｐｔで動作し、最適な減速度
Ｄｏｐｔで動作する速度パターンがすべての関節で存在
しないときは、ステップＳ９で第１の速度パターン計算
方法を用いて速度パターンを演算した後、ステップＳ１
０で従来例の速度パターン再計算処理で同期処理を実行
しているので、すべての場合について、最適な速度パタ
ーンを演算できる。

【０１３９】さらに、本実施形態によれば、モータの軸
受けや減速機の摩擦抵抗を考慮し、厳密な負荷トルク成
分の計算を実現するので、許容値を越えるトルクを必要
とするような加減速度の演算を防止することができる。

【０１４０】またさらに、本実施形態によれば、減速機
を含むモータロータの慣性モーメントにより発生するト
ルクを考慮し、厳密な負荷トルク成分の計算を実現する
ので、許容値を越えるトルクを必要とするような加減速
度の演算を防止することができる。

【０１４１】＜変形例＞以上の実施形態において、ロボ
ット機構部１は、３個の関節ＲＪ１乃至ＲＪ３を備えて
いるが、本発明はこれに限らず、少なくとも２個の関節
を備えていればよい。

【０１４２】以上の実施形態においては、各関節ＲＪ１
乃至ＲＪ３は回転可能に構成されているが、本発明はこ
れに限らず、２つの関節以上では少なくとも１つは、並
進運動を行う直動であってもよい。

【０１４３】

【発明の効果】以上詳述したように本発明に係るロボッ
トシステムの制御方法又は制御装置によれば、計算され
たロボットダイナミクスモデルに基づいて、加速度予測
値の各関節間の比例関係と各関節の許容ピークトルクの
条件を満足する最適な加速度及び減速度を演算した後、
すべての関節で動作時間を一致させる同期処理を実行す
る。従って、ロボット機構部の各関節毎の移動量に大き
な差がある位置決め動作を行う場合でも、駆動関節の許
容トルクを最大限有効に使用した加減速度を演算し、動
作時間の短縮を可能にし、関節自体の慣性による負荷ト
ルク成分を考慮し、関節回転中心から重心位置までの距
離が短いロボットや、ロボットが特異な姿勢となって
も、厳密な負荷トルク成分の計算を実現することで、許
容値を越えるトルクを必要とするような加減速度の演算
を防止する。これにより、ロボット機構部の各関節の許
容ピークトルクを越えず、かつ最短時間で動作が終了で
きる最適な各関節の加減速度を求めることができる。

【０１４４】また、上記ロボットシステムの制御方法又
は制御装置において、好ましくは、上記加速度に関係し
ないトルク成分を、上記粘性行列及び重力行列に加算す
ることにより、上記各関節で発生する摩擦抵抗を考慮す
る。従って、上記各関節の摩擦抵抗を考慮し、厳密な負
荷トルク成分の計算を実現するので、許容値を越えるト
ルクを必要とするような加減速度の演算を防止すること
ができる。

【０１４５】さらに、上記ロボットシステムの制御方法
又は制御装置において、好ましくは、上記慣性行列の対
角成分に、上記各関節の慣性モーメントを加算すること
により、上記各関節自身の加減速で発生するトルク演算
値を補正する。従って、上記各関節の慣性モーメントに
より発生するトルクを考慮し、厳密な負荷トルク成分の
計算を実現するので、許容値を越えるトルクを必要とす
るような加減速度の演算を防止することができる。

【図面の簡単な説明】

【図１】 本発明に係る一実施形態であるロボットシス
テムの構成を示すブロック図である。

【図２】 図１のロボット機構部１の構成を示すスケル
トン図である。

【図３】 図１のロボット機構部１の各関節ＲＪｋ（ｋ
＝１，２，３）の構成を示す斜視図、並びに図１の制御
装置１０内のサーボ制御回路１０７の構成を示すブロッ
ク図である。

【図４】 図１のＣＰＵ１００によって実行される位置
決め動作処理を示すフローチャートである。

【図５】 図４の速度パターンの予測演算処理（ステッ
プＳ２）の概念を示すグラフであって、（ａ）は第１の
関節の予想速度パターンを示す時間対第１の関節の速度
特性を示すグラフであり、（ｂ）は第２の関節の予想速
度パターンを示す時間対第２の関節の速度特性を示すグ
ラフであり、（ｃ）は第３の関節の予想速度パターンを
示す時間対第３の関節の速度特性を示すグラフである。

【図６】 図４の速度パターンの終了時間の同期処理
（ステップＳ３）の概念を示すグラフであって、（ａ）
は第１の関節の予想速度パターンを示す時間対第１の関
節の速度特性を示すグラフであり、（ｂ）は第２の関節
の予想速度パターンを示す時間対第２の関節の速度特性
を示すグラフであり、（ｃ）は第３の関節の予想速度パ
ターンを示す時間対第３の関節の速度特性を示すグラフ
である。

【図７】 台形状の速度パターンのときの本実施形態で
用いる第１の速度パターン計算方法を示す、時間対速度
特性を示すグラフである。

【図８】 三角形状の速度パターンのときの本実施形態
で用いる第１の速度パターン計算方法を示す、時間対速
度特性を示すグラフである。

【図９】 本実施形態で用いる第２の速度パターン計算
方法を示す、時間対速度特性を示すグラフである。

【図１０】 第１の従来例の位置決め動作処理を示すフ
ローチャートである。

【図１１】 図１０の終了時間の同期処理の一例を説明
するための各関節の速度パターンを示すグラフであっ
て、（ａ）は第１の関節の速度パターンを示す時間対第
１の関節の速度特性を示すグラフであり、（ｂ）は第２
の関節の速度パターンを示す時間対第２の関節の速度特
性を示すグラフであり、（ｃ）は第３の関節の速度パタ
ーンを示す時間対第３の関節の速度特性を示すグラフで
ある。

【符号の説明】

１…ロボット機構部、 １０…制御装置、 ２０…ティーチペンダント、 １００…ＣＰＵ、 １０１…ＲＯＭ、 １０２…ＲＡＭ、 １０３…インターフェース、 １０４…デュアルポートＲＡＭ、 １０５…ラッチ回路、 １０６…ハードディスクメモリ、 １０６ａ…教示データメモリ、 １０６ｂ…仮の加減速度情報メモリ、 １０６ｃ…ロボットダイナミクスベースパラメータメモ
リ、 １０７…サーボ制御回路、 １１０…クロック発生器、 １１１…分周器、 ＲＪ１乃至ＲＪ３……関節、 Ｌ１乃至Ｌ４…リンク、 ＭＯ１乃至Ｍ３…モータ、 ＲＥ１乃至ＲＥ３…減速機、 ＳＮ１乃至ＳＮ３…センサ。

Claims (6)

【特許請求の範囲】
1. 【請求項１】 複数の関節を有するロボット機構部の動
   作を制御するロボットシステムの制御方法であって、 上記ロボット機構部を動作させるときのロボット機構部
   の各関節の開始位置と、目標位置と、目標速度とを教示
   データとして第１の記憶手段に格納する第１の記憶ステ
   ップと、 上記ロボット機構部の各関節の速度パターンを予測演算
   するときに用いる各関節の加速度と減速度の情報とを第
   ２の記憶手段に格納する第２の記憶ステップと、 上記第１の記憶手段に格納された教示データと、上記第
   ２の記憶手段に格納された各関節の加速度及び減速度と
   に基づいて、所定の第１の速度パターン計算方法を用い
   て、上記ロボット機構部の各関節の仮速度予測値と、動
   作時間予測値を演算し、上記動作時間予測値の中から、
   その最大値を最長動作時間として演算する第１の演算ス
   テップと、 所定の速度パターン再計算方法を用いて、上記ロボット
   機構部の各関節について、上記第１の演算ステップによ
   って演算された動作時間予測値が、上記第１の演算ステ
   ップによって演算された最長動作時間と同じになるよう
   に、速度並びに加速度及び減速度として再計算した値
   を、速度予測値、加速度予測値及び減速度予測値とする
   第２の演算ステップとを含むロボットシステムの制御方
   法において、 上記ロボット機構部の各関節に連結される各リンクの慣
   性テンソル、質量及び重心位置とを含むロボットダイナ
   ミクスベースパラメータを第３の記憶手段に格納する第
   ３の記憶ステップと、 上記第１の記憶手段に格納された各関節の開始位置及び
   目標位置と、上記第２の演算ステップによって演算され
   た速度予測値と、上記第３の記憶手段に格納されたロボ
   ットダイナミクスベースパラメータとに基づいて、
   （ａ）上記ロボット機構部の各関節についての、加速度
   により関節自身に発生するトルク、並びに他関節の加速
   度により発生する干渉トルクを示す慣性行列と、（ｂ）
   上記ロボット機構部の各関節についての、遠心力及びコ
   リオリ力により発生するトルクを示す粘性行列と、
   （ｃ）上記ロボット機構部の各関節の重力モーメントに
   より発生するトルクを示す重力行列とを含む上記開始位
   置及び目標位置におけるロボットダイナミクスモデルを
   計算する第３の演算ステップと、 上記第３の演算ステップによって演算されたロボットダ
   イナミクスモデルと、上記第２の演算ステップによって
   演算された各関節の加速度予測値及び減速度予測値とに
   基づいて、各関節間の加速度予測値の比例関係である第
   １の条件を満足し、かつ関節の許容ピークトルクから、
   加速度及び減速度に関係の無い重力行列及び粘性行列を
   減算した減算結果値が、各関節が上記加速度予測値及び
   減速度予測値で動作した場合に各関節で発生するトルク
   以上となるという第２の条件を満足するような最適な加
   速度及び最適な減速度を演算する第４の演算ステップ
   と、 上記第１の演算ステップにおいて最長動作時間となった
   関節を検索し、検索した関節である第ｋの関節につい
   て、上記第４の演算ステップによって演算された最適な
   加速度及び最適な減速度と、上記第１の記憶手段に格納
   された第ｋの関節の開始位置、目標位置及び目標速度を
   含む教示データとから、上記第１の速度パターン計算方
   法を用いて第ｋ関節の速度パターンを演算し、その動作
   時間を基準動作時間とする第５の演算ステップと、 上記第ｋの関節以外のすべての関節について、上記第５
   の演算ステップによって演算された基準動作時間と、上
   記第４の演算ステップによって演算された最適な加速度
   及び最適な減速度と、上記第１の記憶手段に格納された
   開始位置及び目標位置を含む教示データとに基づいて、
   所定の第２の速度パターン計算方法を用いて、動作時間
   が上記基準動作時間となり、上記最適な加速度及び最適
   な減速度で動作するような速度パターンを生成する第６
   の演算ステップとを含むことを特徴とするロボットシス
   テムの制御方法。
2. 【請求項２】 上記加速度に関係しないトルク成分を、
   上記粘性行列及び重力行列に加算することにより、上記
   各関節で発生する摩擦抵抗を考慮することを特徴とする
   請求項１記載のロボットシステムの制御方法。
3. 【請求項３】 上記慣性行列の対角成分に、上記各関節
   の慣性モーメントを加算することにより、上記各関節自
   身の加減速で発生するトルク演算値を補正するステップ
   とをさらに含むことを特徴とする請求項１又は２記載の
   ロボットシステムの制御方法。
4. 【請求項４】 複数の関節を有するロボット機構部の動
   作を制御するロボットシステムの制御装置であって、 上記ロボット機構部を動作させるときのロボット機構部
   の各関節の開始位置と、目標位置と、目標速度とを教示
   データとして格納する第１の記憶手段と、 上記ロボット機構部の各関節の速度パターンを予測演算
   するときに用いる各関節の加速度と減速度の情報とを格
   納する第２の記憶手段と、 上記第１の記憶手段に格納された教示データと、上記第
   ２の記憶手段に格納された各関節の加速度及び減速度と
   に基づいて、所定の第１の速度パターン計算装置を用い
   て、上記ロボット機構部の各関節の仮速度予測値と、動
   作時間予測値を演算し、上記動作時間予測値の中から、
   その最大値を最長動作時間として演算する第１の演算手
   段と、 所定の速度パターン再計算装置を用いて、上記ロボット
   機構部の各関節について、上記第１の演算手段によって
   演算された動作時間予測値が、上記第１の演算手段によ
   って演算された最長動作時間と同じになるように、速度
   並びに加速度及び減速度として再計算した値を、速度予
   測値、加速度予測値及び減速度予測値とする第２の演算
   手段とを備えたロボットシステムの制御装置において、 上記ロボット機構部の各関節に連結される各リンクの慣
   性テンソル、質量及び重心位置とを含むロボットダイナ
   ミクスベースパラメータを格納する第３の記憶手段と、 上記第１の記憶手段に格納された各関節の開始位置及び
   目標位置と、上記第２の演算手段によって演算された速
   度予測値と、上記第３の記憶手段に格納されたロボット
   ダイナミクスベースパラメータとに基づいて、（ａ）上
   記ロボット機構部の各関節についての、加速度により関
   節自身に発生するトルク、並びに他関節の加速度により
   発生する干渉トルクを示す慣性行列と、（ｂ）上記ロボ
   ット機構部の各関節についての、遠心力及びコリオリ力
   により発生するトルクを示す粘性行列と、（ｃ）上記ロ
   ボット機構部の各関節の重力モーメントにより発生する
   トルクを示す重力行列とを含む上記開始位置及び目標位
   置におけるロボットダイナミクスモデルを計算する第３
   の演算手段と、 上記第３の演算手段によって演算されたロボットダイナ
   ミクスモデルと、上記第２の演算手段によって演算され
   た各関節の加速度予測値及び減速度予測値とに基づい
   て、各関節間の加速度予測値の比例関係である第１の条
   件を満足し、かつ関節の許容ピークトルクから、加速度
   及び減速度に関係の無い重力行列及び粘性行列を減算し
   た減算結果値が、各関節が上記加速度予測値及び減速度
   予測値で動作した場合に各関節で発生するトルク以上と
   なるという第２の条件を満足するような最適な加速度及
   び最適な減速度を演算する第４の演算手段と、 上記第１の演算手段において最長動作時間となった関節
   を検索し、検索した関節である第ｋの関節について、上
   記第４の演算手段によって演算された最適な加速度及び
   最適な減速度と、上記第１の記憶手段に格納された第ｋ
   の関節の開始位置、目標位置及び目標速度を含む教示デ
   ータとから、上記第１の速度パターン計算装置を用いて
   第ｋ関節の速度パターンを演算し、その動作時間を基準
   動作時間とする第５の演算手段と、 上記第ｋの関節以外のすべての関節について、上記第５
   の演算手段によって演算された基準動作時間と、上記第
   ４の演算手段によって演算された最適な加速度及び最適
   な減速度と、上記第１の記憶手段に格納された開始位置
   及び目標位置を含む教示データとに基づいて、所定の第
   ２の速度パターン計算装置を用いて、動作時間が上記基
   準動作時間となり、上記最適な加速度及び最適な減速度
   で動作するような速度パターンを生成する第６の演算手
   段とを備えたことを特徴とするロボットシステムの制御
   装置。
5. 【請求項５】 上記加速度に関係しないトルク成分を、
   上記粘性行列及び重力行列に加算することにより、上記
   各関節で発生する摩擦抵抗を考慮することを特徴とする
   請求項４記載のロボットシステムの制御装置。
6. 【請求項６】 上記慣性行列の対角成分に、上記各関節
   の慣性モーメントを加算することにより、上記各関節自
   身の加減速で発生するトルク演算値を補正する手段とを
   さらに備えたことを特徴とする請求項４又は５記載のロ
   ボットシステムの制御装置。