ロボットの姿勢判定方法
【課題】簡易な演算により、ロボットの姿勢を判定するロボットの姿勢判定方法を提供する。
【解決手段】ワークに想定される誤差Δｐにともなうエンドエフェクタの位置の変化を位置変化量Δｒ、誤差Δｐにともなうマニピュレータにおける各軸の変化量を姿勢変化量Δｑ、誤差Δｐが存在しないときのマニピュレータの各軸の値をｑｉとし、このｑｉにおけるヤコビアンをＪｖ（ｑｉ）とすると、姿勢変化量Δｑを、Δｑｉ＝Ｊｖ（ｑｉ）-1Δｒにより算出する。そして、誤差Δｐが最大となるときのマニピュレータの各軸の変化量を変化量Δｑｉｍａｘとし、関節の回転角度限界をｑｍａｘとすると、ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘを満たすマニピュレータの各軸の値ｑｉを誤差Δｐを有するワークに対し当該作業点ｉにおける作業が可能な姿勢であると判定する。
【解決手段】ワークに想定される誤差Δｐにともなうエンドエフェクタの位置の変化を位置変化量Δｒ、誤差Δｐにともなうマニピュレータにおける各軸の変化量を姿勢変化量Δｑ、誤差Δｐが存在しないときのマニピュレータの各軸の値をｑｉとし、このｑｉにおけるヤコビアンをＪｖ（ｑｉ）とすると、姿勢変化量Δｑを、Δｑｉ＝Ｊｖ（ｑｉ）-1Δｒにより算出する。そして、誤差Δｐが最大となるときのマニピュレータの各軸の変化量を変化量Δｑｉｍａｘとし、関節の回転角度限界をｑｍａｘとすると、ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘを満たすマニピュレータの各軸の値ｑｉを誤差Δｐを有するワークに対し当該作業点ｉにおける作業が可能な姿勢であると判定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの姿勢判定方法に関する。
【背景技術】
【０００２】
ライン作業などで部品の組み立てや検査などに用いられるロボットは、ラインを流れる作業対象となるワークとの位置関係を高い精度で適切に設定する必要がある。この位置関係が不適切であると、作業点すなわちロボットがワークに作用する点がロボットの動作不可能な領域になったり、動作中のロボットと周囲の機器との干渉を招く。そこで、ロボットの設置位置とワークの載置位置との関係は、種々の計算を用いたシミュレーションにより、最適な値を導き出している。
【０００３】
特許文献１に開示されているシミュレーション装置は、ロボットの複数の関節軸のそれぞれについて、関節軸ごとに動作範囲に相関する関数を設定している。そして、設定した関数に基づいて、各関節軸の動作余裕値、すなわち各関節軸に許容される動作範囲の余裕値を算出している。これにより、各関節軸のぎりぎりの動作を回避し、ロボットの作業点が動作不可能な領域にならないように考慮し、種々のワークに対応可能な位置を求めている。
【０００４】
しかしながら、関数によって導き出される余裕度は、各関節軸の作動角度ごと、つまり姿勢ごとに変化する。これに対し、特許文献１の場合、各関節軸の作動の組み合わせによって決定されるロボットの最終的な姿勢は考慮されていない。すなわち、特許文献１の場合、各関節軸ごとの余裕度の設定にとどまり、ロボットの作業空間座標での最終的な姿勢は考慮されていない。また、特許文献１の場合、予め設定された作業範囲についてのみ、ロボットとワークとの最適な位置関係が算出される。そのため、特許文献１の場合、ロボットを用いて希望する作業が限定されている場合にのみ適用可能である。
【０００５】
ところで、特許文献１のように、シミュレーションを用いてロボットとワークとの適切な位置関係を導き出したとしても、ロボットシステムが構築される工場などでは、導き出した位置関係とロボットの設置位置およびラインの設置位置との間に誤差が生じる。また、適切な位置関係を導き出した場合でも、ラインやテーブルに載置されるワークの搭載位置、ワークの大きさなどに僅かな変化が生じると、ロボットとワークとの位置関係にはシミュレーションでは考慮されない誤差が生じる。さらに、ワークの仕様を変更した場合でも、ロボットとワークとの位置関係には相違が生じる。このようにロボットとワークとの位置関係に僅かな誤差や相違が生じると、ロボットの設置時におけるシミュレーションの精度を如何に高めても、作業点がロボットの動作不可能な領域に侵入し、ロボットの動作不良を招くという問題がある。
【０００６】
さらに、シミュレーションを用いてロボットの設置位置とワークの載置位置との関係を導き出す場合、通常、ロボットの各軸の変化量を入力することにより、ロボットのエンドエフェクタの変化量を算出する。そのため、ワーク側で誤差が想定される場合、各軸の変化量を逐次入力し、その結果として得られるエンドエフェクタの変化量がその想定される誤差に適合するか否かを検証する必要がある。その結果、想定される誤差に適合させるためには各軸の変化量を広範囲に入力する必要が生じることから、極めて多くの演算が必要となる。したがって、ロボットとワークとの間に誤差が生じることを想定する場合、ロボットとワークとの最適な位置関係を導き出すためには多くの演算と長期間を必要とするという問題がある。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特許第３７９７９８６号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００８】
そこで、本発明の目的は、簡易な演算により、ロボットの姿勢を判定するロボットの姿勢判定方法を提供することにある。
【課題を解決するための手段】
【０００９】
請求項１または２記載の発明では、式（１）から姿勢変化量Δｑｉを算出する。
Δｑｉ＝Ｊｖ（ｑｉ）-1Δｒ （１）
ここで、位置変化量Δｒは、ワークに誤差Δｐが生じたとき、エンドエフェクタに生じる位置の変化である。また、姿勢変化量Δｑｉは、誤差Δｐにともなうマニピュレータの各軸の変化量である。さらに、ｑｉは、誤差Δｐが存在しないときのマニピュレータの各軸の値である。そして、Ｊｖ（ｑｉ）は、このｑｉにおけるヤコビアンをである。ここで、添字ｉは、作業点の番号を示す。エンドエフェクタがワークに作用する作業点は、通常、複数設定される。この作業点を任意にｉ＝１、２、３・・・として番号を付している。したがって、ｉ≧１となる。
上記の式（１）から算出した姿勢変化量Δｑｉは、式（２）におけるロボットの姿勢判定に用いられる。
ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘ （２）
ここで、最大変化量Δｑｉｍａｘは、マニピュレータの各軸の変化量が最大となる値として式（１）から求められる。また、ｑｍａｘは、関節の回転角度限界である。式（１）で算出した姿勢変化量Δｑｉを用いて、式（２）にあてはめることにより、ワークとの間に誤差Δｐが生じたときのロボットの姿勢が判定される。
【００１０】
上記のような式（１）は、線形近似されている。そのため、コンピュータなどの演算装置にとって処理が容易な線形式を利用してロボットが作業可能な範囲であるか否かが判定される。ところで、ロボットの姿勢変化量が小さいときは、ロボットの姿勢の大きな変化や、作業時間の大幅な変化を招くものではない。すなわち、ロボットの各軸について、＋側で＋側の範囲内または−側で−側の範囲内に僅かな移動が生じる場合、ロボットの姿勢の変化は僅かであり、作業時間の変化も小さい。そのため、ロボットとワークとの位置関係を検証するためには、誤差Δｐが生じていないときを基準とすればよく、線形近似された式（１）により簡単な演算で至適解の候補が算出される。
【００１１】
一方、従来は、ロバスト解探索と称される解の探索手法により、最悪の条件設定の下で最良の解（ロボットの場合、最短の動作時間）を求めている。これをロボットに適用する場合、ワークの設置には自由度ｎの誤差が存在する。この自由度の数ｎは、最大で６（ｘ，ｙ，ｚの各軸方向の並進のずれ３と、各軸の回転のずれ３とで合計が６となる。）である。そのため、最良の解を探索するためには、作業点の数をｉとすると、全自由度の方向へ対応する必要があることから、２6×ｉ×Ｍ×２通りの解の探索が必要となる。ここで、Ｍは、ロボットの軸数である。解の探索は、このロボットの各軸を中心としたプラス方向およびマイナス方向についても判定が必要なため、さらに２倍の演算が必要となる。仮に６軸型ロボットで３０点の作業点における作業を実行する場合、従来の解の探索手法では、２6×３０×６×２＝２３０４０通りの解の探索が必要となり、解の探索に長時間を必要とする。
【００１２】
本願の発明では、式（１）を用いることにより、ワークの各作業点において一通りのヤコビアンで演算できる。そのため、本願の発明により、上述のように６軸型のロボットで３０点の作業点における作業を実行する場合、ロボットの６つの関節軸においてプラス方向およびマイナス方向のそれぞれについて解を探索すれば足り、その演算は６×２＝１２通りとなる。このように、本願の発明では、作業点が多い場合でも、演算が容易となり、解の探索に必要な演算数は低減される。したがって、ロボットとワークとの位置関係に誤差Δｐが生じる場合でも、ロボットが動作可能な姿勢を簡単な処理で判定することができる。
【図面の簡単な説明】
【００１３】
【図１】一実施形態によるロボットシミュレータの構成を示すブロック図
【図２】一実施形態によるロボットシミュレータのシミュレーションの対象となるロボットシステムの一例を示す模式図
【図３】一実施形態によるロボットシミュレータのシミュレーションの対象となるワークおよび作業点を示す模式図
【図４】一実施形態によるロボットシミュレータのシミュレーションの対象となるワークおよび作業点の誤差を示す模式図
【図５】一実施形態によるロボットシミュレータのシミュレーションの流れを示す概略図
【図６】一実施形態によるロボットシミュレータで用いる式（１）を行列式で示す説明図
【図７】一実施形態によるロボットシミュレータで用いる式（２）を行列式で示す説明図
【発明を実施するための形態】
【００１４】
以下、本発明の一実施形態によるロボットシミュレータを図面に基づいて説明する。
（ロボットシステム）
まず、ロボットシミュレータによるシミレーションの対象となるロボットシステムについて図２を用いて説明する。ロボットシステム１０は、ロボット１１およびワーク搭載部１２を備えている。本実施形態のロボット１１は、６軸型である。なお、本実施形態で説明する６軸型のロボット１１は一例であり、ロボット１１の軸の数すなわち関節の数は任意に設定することができる。また、本実施形態では、図２に示すように、設備の床面１３をワーク搭載部１２とし、この床面１３の座標軸をそれぞれＸ軸およびＹ軸と定義し、この床面１３に垂直な座標軸をＺ軸と定義する。さらに、Ｘ軸を中心軸とした回転座標をＲｘ、Ｙ軸を中心とした回転座標をＲｙ、Ｚ軸を中心とした回転座標をＲｚと定義する。
【００１５】
ロボットシステム１０は、上述の通りロボット１１およびワーク搭載部１２を備えている。また、ロボットシステム１０は、図示しないコントローラを備えている。ロボット１１は、ロボットコントローラからの指示により駆動される。ロボット１１は、アームおよび関節から構成されるマニピュレータ１４、ならびにエンドエフェクタ１５を有している。図２に示す本実施形態のロボット１１は、第一関節２１、第二関節２２、第三関節２３、第四関節２４、第五関節２５および第六関節２６を有している。また、ロボット１１は、第一アーム３１、第二アーム３２、第三アーム３３、第四アーム３４および第五アーム３５を有している。
【００１６】
ロボット１１は、設備の床面１３に設置されている。床面１３には、床面１３から垂直上方すなわちＺ軸と平行に立ち上がる土台部４１が固定されている。第一関節２１は、この土台部４１と第一アーム３１とを接続している。第一アーム３１は、第一関節２１において床面１３に垂直な軸を中心に回転可能である。第二関節２２は、第一アーム３１と第二アーム３２とを接続している。第一アーム３１と第二アーム３２とは、第二関節２２を中心に回転する。これにより、第二アーム３２は、Ｚ軸に対する傾斜角度が変化する。第三関節２３は、第二アーム３２と第三アーム３３とを接続している。第二アーム３２と第三アーム３３とは、第三関節２３を中心に回転可能である。すなわち、第二アーム３２と第三アーム３３とは、第三関節２３を支点として開閉する。
【００１７】
第四関節２４は、第三アーム３３と第四アーム３４とを接続している。第三アーム３３と第四アーム３４とは、第四関節２４を中心に相対的に回転する。第三アーム３３と第四アーム３４とは、同一の軸上に位置している。第四アーム３４は、第四関節２４においてこの軸を中心に第三アーム３３に対し回転する。第五関節２５は、第四アーム３４と第五アーム３５とを接続している。第五アーム３５は、第五関節２５を中心に回転する。すなわち、第五アーム３５は、第四アーム３４の軸に垂直な第五関節２５の軸を中心に回転する。第六関節２６は、第五アーム３５とエンドエフェクタ１５とを接続している。第五アーム３５とエンドエフェクタ１５とは、同一の軸上に位置している。エンドエフェクタ１５は、第六関節２６においてこの軸を中心に第五アーム３５に対し回転する。すなわち、エンドエフェクタ１５は、第五関節２５の軸と垂直な軸を中心に回転する。
【００１８】
エンドエフェクタ１５は、作業の対象となるワーク４２に作用する。具体的には、ロボット１１を用いてワーク４２を加工する場合、エンドエフェクタ１５はワーク４２を把持するハンドやワーク４２に加工を加えるツールなどであり、ロボット１１を用いてワーク４２を検査する場合、エンドエフェクタ１５はワーク４２を撮影するカメラなどである。このように、エンドエフェクタ１５は、作業の対象となるワーク４２に対し、接触または非接触により作用する。
【００１９】
ワーク搭載部１２は、ロボット１１による作業の対象となるワーク４２が搭載される。本実施形態の場合、ワーク４２は、床面１３に搭載される。そのため、ワーク搭載部１２は設備の床であり、この床の上端面すなわち床面１３はワーク４２が搭載される搭載面に一致する。なお、ワーク搭載部１２は、例えば床面１３から上方へ立ち上がったテーブル上やコンベヤなどの搬送手段上に形成してもよい。この場合、ワーク４２が搭載されるテーブルやコンベヤがワーク搭載部１２を構成する。
【００２０】
（ロボットシミュレータ）
次に、図１に基づいてロボットシミュレータ５０について説明する。ロボットシミュレータ５０は、例えばパーソナルコンピュータなどの演算装置において、コンピュータソフトウェアであるシミレーションプログラムを実行することにより、ソフトウェア的に実現される。ロボットシミュレータ５０を実現するパーソナルコンピュータは、入力部５１、演算部５２および出力部５３を備えている。入力部５１は、例えばマウス５４およびキーボード５５などの一般的な入力機器を有している。演算部５２は、例えばＣＰＵ５６、ＲＯＭ５７およびＲＡＭ５８を有するコンピュータで構成されており、上述のシミュレーションプログラムを実行することにより、ロボットシミュレータ５０の全体を制御する。出力部５３は、例えば液晶ディスプレイなどの表示部６１やプリンタ６２などを備えており、入力部５１から入力されたデータや演算部５２でシミュレーションプログラムにしたがって算出されたシミュレーションデータなどを出力する。
【００２１】
以下、具体的にロボットシミュレータ５０の構成について説明する。ロボットシミュレータ５０は、パーソナルコンピュータなどの演算装置においてシミュレーションプログラムを実行することにより、演算部５２において、作業順序設定部６５、最短位置抽出部６６および設置位置判定部６７をソフトウェアによって仮想的に実現する。また、ロボットシミュレータ５０の入力部５１は、条件入力部６８およびワーク誤差入力部６９を構成する。
【００２２】
条件入力部６８は、ロボット１１の初期位置、作業順序および作業点の座標などが入力される。ロボット１１の初期位置とは、ロボットシミュレータ５０によるシミュレーションの実行に際して、ワーク搭載部１２に搭載されたワーク４２に対するロボット１１の初期位置である。すなわち、ロボット１１の初期位置は、シミュレーションの開始時における、ロボット１１とワーク４２との初期的な位置関係である。ロボット１１の初期位置となるロボット１１の原点ΣＲ０は、ワーク４２の中心を原点ΣＷとしたとき、ΣＷ上の座標WＰR0とするワークの座標系として表わされる。
このロボット１１の初期位置は、入力部５１から任意の座標として入力することができる。作業順序とは、ロボットシステム１０を用いて実行する複数の工程を時系列で設定したものである。ロボット１１は、各作業点への移動と移動した作業点における作業とを繰り返す。作業順序は、この作業点への移動の順序および作業点における作業を設定したものである。作業点の座標とは、ロボット１１のエンドエフェクタ１５がワーク４２に対して作用を及ぼす位置の座標である。例えばロボットシステム１０を用いてワーク４２の加工を行う場合、作業点とはその加工を行うためにエンドエフェクタ１５とワーク４２とが接する点である。また、例えばロボットシステム１０を用いてワーク４２の検査を行う場合、作業点とはその検査を行うためにエンドエフェクタ１５が移動する位置である。作業点の座標とは、図３に示すようにワーク４２の中心を原点ΣＷ０としたとき、これらの作業点７１、７２、７３における各座標軸の座標WＰ(71)、WＰ(72)、WＰ(73)である。
【００２３】
作業順序設定部６５は、作業所要時間が最小となる最短作業順序を求める。作業順序設定部６５は、条件入力部６８から入力されたロボット１１とワーク４２との位置関係に基づいて、条件入力部６８から入力された作業順序に含まれる複数の工程の作業所要時間を算出する。作業所要時間は、ロボット１１とワーク４２との位置関係および作業点ごとの工程の順序が変更されると、これに基づいて変化する場合がある。そこで、作業順序設定部６５は、ロボット１１とワーク４２との位置関係を設定した状態で必要に応じて工程の順序を入れ替えながら作業所要時間が最短となる最短作業順序を求める。
【００２４】
最短位置抽出部６６は、所要時間が最短となるロボット１１とワーク４２との位置関係を抽出する。最短位置抽出部６６は、条件入力部６８から入力されたワーク４２に対するロボット１１の初期位置における原点ΣＲ０を含む床面１３すなわちＸ−Ｙ平面上に任意の設置範囲Ａを設定する。つまり、最短位置抽出部６６は、まず床面１３の任意の位置に、ロボット１１の設置位置の候補となる範囲の設置範囲Ａを設定する。この設置範囲Ａには、ロボット１１の初期位置における原点ΣＲ０が含まれている。そして、最短位置抽出部６６は、この設定した任意の設置範囲Ａ内において、ロボット１１とワーク４２との位置関係について至適解の候補となる複数の位置関係を抽出する。すなわち、最短位置抽出部６６は、設置範囲Ａ内において、ロボット１１とワーク４２とが好適な位置関係になると予想される複数の位置を抽出し、これを至適解の候補とする。ここで、最適解とせずに至適解の候補としているのは、ロボット１１とワーク４２との最適な位置関係が存在することが予想される場合でも、その最適解が設置範囲Ａ内に含まれるとは限らない。ロボット１１およびワーク４２は、無限の空間に設置されるのではなく、工場など有限の設備に設置される。そのため、算出した解は、最適解とは限らない。したがって、本実施形態の場合、最短位置抽出部６６は、設置範囲Ａ内において条件を満たす位置関係を至適解とし、この至適解となることが予想される複数の位置関係を至適解の候補として抽出している。
【００２５】
最短位置抽出部６６は、抽出した至適解の候補となるロボット１１とワーク４２との位置関係ごとに最短作業順序でロボット１１の複数の工程を仮想的に実行する。すなわち、最短位置抽出部６６は、抽出した至適解の候補となるロボット１１とワーク４２との複数の位置関係について、それぞれ作業順序設定部６５で求めた最短作業順序にしたがって複数の工程を仮想的に実行する。この場合、最短位置抽出部６６は、複数の工程をシミュレーションプログラムにしたがって仮想的に実行する。そして、最短位置抽出部６６は、至適解の候補のうち最も所要時間の短いロボット１１とワーク４２との位置関係を抽出する。すなわち、最短位置抽出部６６は、至適解の候補となる複数の位置関係において、仮想的に複数の工程を実行する。そして、複数の位置関係の中から複数の工程の所要時間が最短となるロボット１１とワーク４２との位置関係を抽出する。これにより、ワーク４２に対する位置関係が設置範囲Ａ内において最も好適となるロボット１１の設置位置が抽出される。この場合、好適となるロボット１１の設置位置は、必ずしも一つに限らず、検討候補として複数抽出してもよい。
【００２６】
ワーク誤差入力部６９は、ワーク搭載部１２に搭載されるワーク４２の位置として想定される誤差量、または個体差によって想定されるワーク４２の誤差量が誤差Δｐとして入力される。最短位置抽出部６６において、複数の工程の所要時間が最短となるロボット１１とワーク４２との至適な位置関係が抽出されても、工場などの設備にロボットシステム１０を設置する際に、ロボット１１とワーク搭載部１２との位置関係が変化したり、ワーク４２のロットの変更やワーク４２の寸法的な個体差などによってロボット１１とワーク４２との位置関係が僅かに変化することがある。このような場合、最短位置抽出部６６におけるロボット１１とワーク４２との位置関係の抽出精度を向上させても、実際の設備では僅かな誤差によってロボット１１の動作不良を招くことになる。そこで、ワーク誤差入力部６９からは、予め想定されるワーク４２の誤差量が誤差Δｐとして入力される。この誤差Δｐは、図４に示すようにワーク４２に対し初期位置として設定されている初期作業点ｐに対し、ワーク搭載部１２に搭載されるワーク４２に想定される誤差量である。ここで初期作業点ｐは、ロボット１１とワーク４２との位置関係が最短位置抽出部６６で抽出された至適な位置関係にあるとき、ワーク４２の原点ΣＷ０からの座標として表わされる。つまり、初期作業点ｐは、空間の位置座標（ｐｘ，ｐｙ，ｐｚ）として示される。これに対し、誤差Δｐが生じた場合の作業点ｐ’は、Ｐ’＝Ｔ（Δｐ）・Ｐとして表わされる。ここで、Ｐ’、Ｐは、それぞれ位置座標（ｐｘ，ｐｙ，ｐｚ，１）、（ｐｘ’，ｐｙ’，ｐｚ’，１）で示されるベクトルであり、Ｔ（Δｐ）は、Δｐ＝（Δｐｘ，Δｐｙ，Δｐｚ，Δｐｒｘ，Δｐｒｙ，Δｐｒｚ）より求められる同時変換行列である。
【００２７】
設置位置判定部６７は、ワーク誤差入力部６９から入力された誤差Δｐに基づいて、ロボット１１でワーク４２に対する作業が可能か否かを判定する。具体的には、設置位置判定部６７は、ワーク誤差入力部６９から入力された誤差Δｐに基づいて、この誤差Δｐが生じたときのエンドエフェクタ１５の位置の変化をベクトルである位置変化量Δｒとして設定する。また、設置位置判定部６７は、入力された誤差Δｐにともなうマニピュレータ１４における各軸の変化量を姿勢変化量Δｑｉとして設定する。位置変化量Δｒは、入力された誤差Δｐに基づいて算出してもよく、誤差Δｐに対応するテーブルとして予め設定しておいてもよい。ここで、誤差Δｐが存在しないときのマニピュレータ１４の各軸の値をｑｉとする。このｑｉは、最短位置抽出部６６で抽出した至適解に相当するロボット１１とワーク４２との位置関係におけるマニピュレータ１４の各軸の値である。そして、設置位置判定部６７は、このｑｉにおけるヤコビアンをＪｖ（ｑｉ）として、以下の式（１）から姿勢変化量Δｑを算出する。なお、ヤコビアンＪｖ（ｑｉ）は、ｑｉが抽出されるごとに逐次設定される構成としてもよく、想定されるｑｉについて予め設定してＲＯＭ５７やＲＡＭ５８に記録しておく構成としてもよい。
Δｑｉ＝Ｊｖ（ｑｉ）-1Δｒ （１）
ここで、Ｊｖ（ｑｉ）は、誤差Δｐが存在しないときのマニピュレータ１４の各軸におけるヤコビアン、すなわちヤコビ行列である。
【００２８】
この式（１）は、姿勢変化量Δｑｉに基づいて位置変化量Δｒを算出する既知の式（１Ａ）を逆変換することにより導き出される。
Δｒ＝Ｊｖ（ｑｉ）Δｑｉ （１Ａ）
上記の式（１）は、姿勢変化量Δｑｉと位置変化量Δｒとが線形の関係に近似されているため、姿勢変化量Δｑｉは位置変化量Δｒに基づいて線形式によって算出される。そのため、式（１Ａ）に比較して姿勢変化量Δｑｉの算出に必要な演算が簡略化され、複数の関節を有するマニピュレータ１４の姿勢変化量Δｑｉの演算が容易になる。
【００２９】
設置位置判定部６７は、上記の式（１）に基づいて姿勢変化量Δｑｉが算出されると、式（２）に基づいてマニピュレータ１４の各軸の値ｑｉを判定する。すなわち、設置位置判定部６７は、式（１）による姿勢変化量Δｑｉの算出の対象としたロボット１１とワーク４２との位置関係の至適解における各軸の値ｑｉが、式（２）の関係を満たすか否かを判定する。
ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘ （２）
ここで、最大変化量Δｑｉｍａｘは、姿勢変化量Δｑｉが最大となるときのマニピュレータ１４の各軸の変化量の最大値である。この最大変化量Δｑｉｍａｘは、誤差Δｐが最大となるときの姿勢変化量として式（１）で算出することにより、既知の値として得られる。また、ｑｍａｘは、ロボット１１のマニピュレータ１４を構成する複数の関節ごとの回転角度限界である。すなわち、設置位置判定部６７は、式（１）で算出した誤差Δｐが最大となるときの姿勢変化量Δｑｉである最大変化量Δｑｉｍａｘから、式（１）における演算で用いたｑｉが、式（２）で示される関係において回転角度限界ｑｍａｘとの大小関係を満たすか否かを判定する。設置位置判定部６７は、この判定した結果に基づいて、ロボット１１でワーク４２に対する作業が可能か否かについて判定する。つまり、設置位置判定部６７は、式（１）で算出された姿勢変化量Δｑｉに含まれる要素が式（２）の大小関係を満たすとき、ワーク４２の位置に変化が生じたときでも、ロボット１１とワーク４２との位置関係が至適であると判定する。設置位置判定部６７は、以上の演算により、式（１）および式（２）に基づいて、ワーク４２が初期位置から誤差Δｐだけ移動したとしても、ロボット１１がワーク４２に対して作業可能であるか否かを判定する。この判定についての詳細は、以下のシミュレーションの流れにおいて詳細に説明する。
【００３０】
（シミュレーションの流れ）
次に、上記の構成によるロボットシミュレータ５０によるシミュレーションの流れについて図５に基づいて説明する。
図５に示すように、ロボットシミュレータ５０は、大きくＳ１０１およびＳ１０２における入力処理、Ｓ１０３からＳ１０６までにおける最適化処理、およびＳ１０７における出力処理に大別される。以下、各処理について説明する。
【００３１】
ロボットシミュレータ５０は、条件入力部６８から初期状態の入力を受け付ける（Ｓ１０１）。Ｓ１０１では、初期状態としてロボット１１の初期位置、作業順序および作業点の座標が入力部５１から入力される。ロボット１１の初期位置は、上述の通り、シミュレーションの実行に際して、ワーク搭載部１２に搭載されたワーク４２に対するロボット１１の初期位置である。ロボット１１の初期位置における原点ΣＲ０は、ワーク４２の中心を原点ΣＷとしたとき、各軸の座標としてΣＲ０＝（ＸＲ０，ＹＲ０，ＺＲ０）で表わされる。このロボット１１の初期位置は、入力部５１から任意の座標として入力される。
作業手順は、ロボットシステム１０を用いて実行する複数の工程を時系列で設定したものである。すなわち、作業手順は、ロボット１１の作業点における作業順序の初期的な情報である。なお、対象となるワーク４２の外観を検査する外観検査などの場合、検査順序は拘束されない。そのため、検査の場合、すべての検査に必要な所要時間が最小となるように作業点における検査の順序が変更される。一方、対象となるワーク４２の加工を行う場合、加工順序が拘束される。そのため、ワーク４２の加工を行う場合、作業手順に含まれる各工程の順序は予め設定される。
【００３２】
作業点は、ロボット１１のエンドエフェクタ１５がワーク４２に対して作用を及ぼす位置の座標である。上述の通り、ロボットシステム１０でワーク４２の加工を行う場合、作業点はその加工を行うためにエンドエフェクタ１５とワーク４２とが接する点であり、ロボットシステム１０でワーク４２の検査を行う場合、作業点はエンドエフェクタ１５が検査のために移動する点である。作業点は、各工程に応じて複数設定される。これら作業点Ｐ［ｉ］の座標は、各軸における位置および回転角度から、Ｐ［ｉ］＝（Ｘｉ，Ｙｉ，Ｚｉ，Ｒｘｉ，Ｒｙｉ，Ｒｚｉ）で表わされる。
【００３３】
また、これら初期状態の入力に加えて、入力部５１を構成するワーク誤差入力部６９からは、ワーク４２に想定される誤差量が誤差Δｐとして入力される（Ｓ１０２）。上述のようにロボット１１とワーク４２との至適な位置関係を抽出したとしても、工場などへのロボットシステム１０の設置の際、およびワーク４２の形状の変化などによってロボット１１とワーク４２との位置関係が僅かに変化する。そのため、ワーク誤差入力部６９から予め予想されるワーク４２の誤差量を既知の誤差Δｐとして入力し、この誤差Δｐが許容されるか否かを後述するＳ１０５において判定する。そのため、Ｓ１０２において、Ｓ１０５における判定で用いる誤差Δｐが入力される。なお、この誤差Δｐの入力は、Ｓ１０２に限らず、Ｓ１０５における判定を実行する以前であれば、任意の時期に行ってもよい。
【００３４】
以上のように、Ｓ１０１において入力部５１から初期状態が入力、およびＳ１０２において誤差Δｐが入力されると、ロボットシミュレータ５０の作業順序設定部６５は、作業所要時間が最小となる最短作業順序の評価を行う（Ｓ１０３）。具体的には、作業順序設定部６５は、ワーク４２に対してロボット１１が条件入力部６８から入力された初期位置における原点ΣＲ０にあるとき、任意の作業順序で作業所要時間を算出する。上述のように作業所要時間は、ロボット１１とワーク４２との位置関係や作業点ごとの工程の順序によって変化する。そのため、作業順序設定部６５は、ワーク４２に対しロボット１１が初期位置における原点ΣＲ０にある状態で複数の工程の順序を入れ替えながら作業所要時間が最短となる最短作業順序を求め、工程の順序ごとに作業所要時間を評価する。
【００３５】
Ｓ１０３において最短作業順序の評価が終了すると、ロボットシミュレータ５０の最短位置抽出部６６は、所要時間が最短となるロボット１１とワーク４２との位置関係を抽出または変更する（Ｓ１０４）。最短位置抽出部６６は、上述のように条件入力部６８から入力されたワーク４２に対するロボット１１の初期位置における原点ΣＲ０を含む床面１３であるＸ−Ｙ平面上に任意の設置範囲Ａを設定する。ロボット１１の初期位置における原点ΣＲ０は、この設置範囲Ａに含まれている。最短位置抽出部６６は、この任意の設置範囲Ａ内において、ロボット１１とワーク４２との位置関係について至適解の候補となる複数の位置関係を抽出する。すなわち、最短位置抽出部６６は、設置範囲Ａ内において、ロボット１１とワーク４２とが好適な位置関係になると予想される複数の位置を抽出し、これを至適解の候補とする。そして、最短位置抽出部６６は、抽出した至適解の候補となるロボット１１とワーク４２の位置関係ごとに作業順序設定部６５で評価した最短作業順序にしたがって複数の工程を仮想的に実行する。最短位置抽出部６６は、至適解の候補となっている位置関係ごとに最短作業順序にしたがって複数の工程を仮想的に実行し、至適解の候補から所要時間が最短となるロボット１１とワーク４２との位置関係を抽出する。このように、最短位置抽出部６６は、候補として抽出した至適解に基づいてロボット１１とワーク４２との位置関係を変化させながら、所要時間が最短となる至適解に相当するロボット１１とワーク４２との位置関係を抽出する。この複数の至適解の候補から、所要時間が最短となるロボット１１とワーク４２との位置関係に対応する至適解を抽出する処理は、例えばＰＳＯ法など既知の手法によって実行される。後述するＳ１０５の処理によってＳ１０４へリターンし、Ｓ１０４を再び実行する場合、最短位置抽出部６６はロボット１１とワーク４２との位置関係を変更、すなわち至適解の候補をさらに変更し、上述の処理を繰り返す。
【００３６】
Ｓ１０４において、所要時間が最短となるロボット１１とワーク４２との位置関係が抽出されると、ロボットシミュレータ５０の設置位置判定部６７は、誤差Δｐに基づいて、その誤差Δｐを設定してもロボット１１でワーク４２に対して作業可能であるか、すなわち誤差Δｐがロボット１１の作業に許容できるものであるか否かを判定する（Ｓ１０５）。具体的には、設置位置判定部６７は、上述の通り、Ｓ１０２においてワーク誤差入力部６９から入力された誤差Δｐに基づいて、この誤差Δｐが生じたときのエンドエフェクタ１５の位置の変化を位置変化量Δｒとして設定する。この位置変化量Δｒは、誤差Δｐによって生じるワーク４２の初期位置となる原点ΣＷに対する各軸方向の位置および回転方向のずれであり、方向と大きさとからなるベクトルとして、Δｒ＝（ΔＸｗ，ΔＹｗ，ΔＺｗ，ΔＲｘｗ，ΔＲｙｗ，ΔＲｚｗ）で示される。添字ｗは、位置変化量Δｒがワーク４２の位置変化量であることを示す。また、設置位置判定部６７は、入力された誤差Δｐにともなうマニピュレータ１４における各軸の値の変化を姿勢変化量Δｑとして算出する。この姿勢変化量Δｑｉは、マニピュレータ１４の各関節ごとの回転角度として、Δｑｉ＝（Δθ１，Δθ２，Δθ３，Δθ４，Δθ５，Δθ６）で表わされる。
【００３７】
より具体的には、設置位置判定部６７は、以下の式（１）に基づいて、位置変化量Δｒから姿勢変化量Δｑｉを算出する。以下の式（１）において、ｑｉは、誤差Δｐが存在しないときのマニピュレータ１４の各軸の値である。このｑｉは、上述の通り最短位置抽出部６６においてＳ１０４で抽出した至適解に相当するロボット１１とワーク４２との位置関係における各軸の値である。Ｊｖ（ｑｉ）は、このｑｉにおけるヤコビアンである。
Δｑ＝Ｊｖ（ｑｉ）-1Δｒ （１）
この式（１）は、姿勢変化量Δｑｉと位置変化量Δｒとが線形の関係に近似されているため、姿勢変化量Δｑｉは誤差Δｐに相関する位置変化量Δｒに基づいて線形的に算出される。そのため、式（１）を用いることにより、姿勢変化量Δｑｉの算出に必要な演算は簡略化され、複数の関節を有するマニピュレータ１４の姿勢変化量Δｑｉはより容易な演算で算出される。式（１）を行列式で表わした場合、図６に示す式（１）’となる。この式（１）’では、誤差Δｐに相関するワーク４２の位置変化量Δｒを、各軸方向の変化量および回転角度の要素として、Δｒ＝（ΔＸ，ΔＹ，ΔＺ，ΔＲｘ，ΔＲｙ，ΔＲｚ）と表わしている。また、この位置変化量Δｒを、ｑｉのヤコビアンＪｖ（ｑｉ）の逆行列であるＪｖ（ｑｉ）-1で逆変換した結果として得られる姿勢変化量Δｑは、ロボット１１の各軸の回転角度として、Δｑｊ＝（Δθ１，Δθ２，Δθ３，Δθ４，Δθ５，Δθ６）で表わされる。ここで添字のｊ＝１〜６は、ロボット１１の第一関節２１〜第六関節２６にそれぞれ対応している。
【００３８】
設置位置判定部６７は、上記の式（１）に基づいて姿勢変化量Δｑｉが算出されると、式（２）に基づいてマニピュレータ１４の各軸の値ｑｉを判定する。すなわち、設置位置判定部６７は、式（１）による姿勢変化量Δｑｉの算出の対象としたロボット１１とワーク４２との位置関係の至適解における各軸の値ｑｉが式（２）の関係を満たすか否かを判定する。
ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘ （２）
式（２）において、最大変化量Δｑｉｍａｘは、ワーク４２の誤差Δｐが最大となるときのマニピュレータ１４の各軸の変化量である。この最大変化量Δｑｉｍａｘは、誤差Δｐが最大となるときの姿勢変化量Δｑｉを式（１）で算出することにより、既知の値として得られる。また、ｑｍａｘは、ロボット１１のマニピュレータ１４を構成する複数の関節ごとの回転角度限界である。
【００３９】
具体的に説明すると、式２は、図７の式（２）’に示すような行列式として表わされる。式（２）における最大変化量Δｑｍａｘは、誤差Δｐが最大となるときに式（１）に基づいて算出される既知の値である。姿勢変化量Δｑｉを構成する要素は、Δθｊ（ｊは、ロボット１１の軸の番号であり、六軸型の本実施形態のロボット１１の場合、ｊ＝１〜６となる。）で示される。また、式（２）におけるｑｍａｘは、ロボット１１のマニピュレータ１４を構成する複数の関節ごとに許容される回転角度、すなわち回転角度限界である。この回転角度限界ｑｍａｘは、図７の式（２）’に示すように各関節ごとにθｊｍａｘ（ｊ＝１〜６）としてそれぞれ予め設定されている。
【００４０】
設置位置判定部６７は、Ｓ１０４で抽出して式（１）における計算の基礎とした至適解における初期的な各軸の値ｑｉと式（１）を用いて算出した誤差Δｐが最大となるときの姿勢変化量Δｑｉｍａｘとの和が、回転角度限界ｑｍａｘ以下となるように、要素ごとに大小関係を判定する。つまり、設置位置判定部６７は、式（１）の算出の基礎としたｑｉを構成する要素である第一関節２１の回転角度θ１と、誤差Δｐが最大となるとして式（１）で算出した姿勢変化量Δｑｉｍａｘを構成する要素である第一関節２１の角度変化量Δθ１ｍａｘとの和が、予め設定されている第一関節２１の回転角度限界θ１ｍａｘ以下であるかを判定する。同様に、設置位置判定部６７は、第二関節２２についてθ２＋Δθ２ｍａｘ≦θ２ｍａｘであるか、第三関節２３についてθ３＋Δθ３ｍａｘ≦θ３ｍａｘであるか、第四関節２４についてθ４＋Δθ４ｍａｘ≦θ４ｍａｘであるか、第五関節２５についてθ５＋Δθ５ｍａｘ≦θ５ｍａｘであるか、第六関節２６についてθ６＋Δθ６ｍａｘ≦θ６ｍａｘであるか否かをそれぞれ判定する。
【００４１】
そして、設置位置判定部６７は、上記の式（２）すなわち式（２）’を用いた判定の結果、要素となる複数の関節のすべてが式（２）すなわち式（２）’を満たす場合、式（１）における演算の基礎とした各軸の値ｑｉが成立するとして、このｑｉに対応するロボット１１とワーク４２との位置関係が適切であると判定する。一方、設置位置判定部６７は、要素となる複数の関節のうちいずれか一つが式（２）すなわち式（２）’の関係を満たさない場合、式（１）における演算の基礎とした各軸の値ｑｉが成立しないとして、このｑｉに対応するロボット１１とワーク４２との位置関係が不適切であると判定する。上記のように、設置位置判定部６７は、以上の演算により式（１）おおび式（２）に基づいて、ワーク４２が初期位置から誤差Δｐだけ移動、特に誤差Δｐが際となるまで移動したとしても、ロボット１１がワーク４２に対して作業可能であるか否かをすべての作業点について判定する。
【００４２】
設置位置判定部６７は、式（１）および式（２）に基づく演算結果から、ロボット１１がワーク４２に対して作業可能である、すなわち誤差Δｐが許容可能であると判定すると（Ｓ１０５：Ｙｅｓ）、計算時間制約を判定するために計算所要時間に到達したか否かを判断する（Ｓ１０６）。設置位置判定部６７は、式（１）および式（２）によりロボット１１がワーク４２に対して作業可能であると判定したとき、計算時間が予め設定した計算所要時間内にあるか否かを判断する。ロボットシステム１０では、複数の工程からなる作業の所要時間が最短となる具体的な時間を予め知ることはできない。そのため、設置位置判定部６７では、各演算の無限の繰り返しを招くおそれがある。そこで、設置位置判定部６７は、Ｓ１０６において予め設定した計算所要時間に到達したか否かを判断する。この計算所要時間は、確保したい精度に応じて任意に設定される。設置位置判定部６７は、予め設定した計算所要時間に到達するまでＳ１０３からＳ１０６までの最適化処理を繰り返し、予め設定した計算所要時間を経過すると、Ｓ１０７へ移行する。すなわち、設置位置判定部６７は、Ｓ１０３の最初の実行からの経過時間が計算所要時間に到達したか否かを判断し、計算所要時間に到達していないとき（Ｓ１０６：Ｎｏ）、Ｓ１０３へリターンし、作業順序の変更以降の最適化処理を繰り返す。一方、設置位置判定部６７は、Ｓ１０３の最初の実行からの経過時間が計算所要時間に到達したと判断すると（Ｓ１０６：Ｙｅｓ）、Ｓ１０７の出力処理へ移行し、最適化によって得られた条件の範囲内における最適な作業順序、ロボット１１とワーク４２との位置関係を表示部６１へ可視的に出力する。
【００４３】
ところで、設置位置判定部６７は、式（１）および式（２）に基づく算出結果から、ロボット１１がワーク４２に対して作業可能でない、すなわち誤差Δｐが許容できないと判定すると（Ｓ１０５：Ｎｏ）、Ｓ１０４へリターンし、ロボット１１とワーク４２との位置関係を、複数の至適解の候補のうち他の候補に変更する。すなわち、Ｓ１０４では複数の至適解の候補から所要時間が最短となるロボット１１とワーク４２との位置関係が抽出され、この位置関係についてＳ１０５で誤差が許容されるか否かが判定される。ここで、所要時間が最短となるロボット１１とワーク４２との位置関係ではＳ１０５で誤差が許容できないと判定されると、先のＳ１０４で所要時間が最短と判断された位置関係の次に所要時間の短い至適解の候補を抽出し、Ｓ１０５の処理を繰り返す。そして、設置位置判定部６７は、変更した至適解の候補に対応するロボット１１とワーク４２との位置関係に基づいて、Ｓ１０５における判定処理を実行する。
【００４４】
以上の手順により、ロボットシミュレータ５０は、Ｓ１０４で設置範囲Ａに抽出された至適解の候補について、ロボット１１とワーク４２と位置関係、および誤差Δｐによるロボット１１の作動の可否を計算所要時間に到達するまで繰り返し評価する。そして、計算所要時間に到達すると、ロボット１１における作業順序および至適解に相当するロボット１１とワーク４２との位置関係を、シミュレーションの範囲内における最適値として表示部６１に出力する。なお、至適解または至適解に近似するロボット１１とワーク４２との位置関係が複数ある場合、これら複数の位置関係を好ましい順に順位を付して表示部６１に表示する構成としてもよい。
【００４５】
以上説明した一実施形態の場合、上記の式（１）は、線形近似されている。そのため、パーソナルコンピュータなどの演算部５２にとって処理が容易な線形式を利用してロボット１１が作業可能な範囲であるか否かが判定される。ところで、ワーク４２に生じるとして設定している誤差Δｐは、ロボット１１の姿勢の大きな変化や、作業時間の大幅な増加を招くものではない。そのため、ロボット１１とワーク４２との位置関係を検証するためには、誤差Δｐが生じていないときを基準とすればよく、線形近似された式（１）により簡単な演算で至適解の候補を算出することができる。一方、ロボット１１の各軸の姿勢に大きな変化が生じる場合でも、上記の式（１）および式（２）を用いてそのまま計算することにより、作業が不可能な姿勢は式（２）によって不適切な解と判定される。そのため、ロボットとワークとの位置関係を検証するためには、誤差Δｐが生じていないときを基準とすればよく、誤差Δｐは無視してもよい。
【００４６】
また、一実施形態では、式（１）を用いることにより、ワーク４２の各作業点において一通りのヤコビアンで演算できる。したがって、ロボット１１とワーク４２との位置関係に誤差Δｐが生じる場合でも、ロボット１１が動作可能となるロボット１１とワーク４２との最適な位置関係を簡単な処理で導き出すことができる。
【００４７】
以上説明した本発明は、上記実施形態に限定されるものではなく、その要旨を逸脱しない範囲で種々の実施形態に適用可能である。
【符号の説明】
【００４８】
図面中、１０はロボットシステム、１１はロボット、１２はワーク搭載部、１４はマニピュレータ、１５はエンドエフェクタ、２１は第一関節、２２は第二関節、２３は第三関節、２４は第四関節、２５は第五関節、２６は第六関節、３１は第一アーム、３２は第二アーム、３３は第三アーム、３４は第四アーム、３５は第五アーム、４２はワーク、５０はロボットシミュレータ、５１は入力部、５２は演算部、５３は出力部、６５は作業順序設定部、６６は最短位置抽出部、６７は設置位置判定部、６８は条件入力部、６９はワーク誤差入力部を示す。
【技術分野】
【０００１】
本発明は、ロボットの姿勢判定方法に関する。
【背景技術】
【０００２】
ライン作業などで部品の組み立てや検査などに用いられるロボットは、ラインを流れる作業対象となるワークとの位置関係を高い精度で適切に設定する必要がある。この位置関係が不適切であると、作業点すなわちロボットがワークに作用する点がロボットの動作不可能な領域になったり、動作中のロボットと周囲の機器との干渉を招く。そこで、ロボットの設置位置とワークの載置位置との関係は、種々の計算を用いたシミュレーションにより、最適な値を導き出している。
【０００３】
特許文献１に開示されているシミュレーション装置は、ロボットの複数の関節軸のそれぞれについて、関節軸ごとに動作範囲に相関する関数を設定している。そして、設定した関数に基づいて、各関節軸の動作余裕値、すなわち各関節軸に許容される動作範囲の余裕値を算出している。これにより、各関節軸のぎりぎりの動作を回避し、ロボットの作業点が動作不可能な領域にならないように考慮し、種々のワークに対応可能な位置を求めている。
【０００４】
しかしながら、関数によって導き出される余裕度は、各関節軸の作動角度ごと、つまり姿勢ごとに変化する。これに対し、特許文献１の場合、各関節軸の作動の組み合わせによって決定されるロボットの最終的な姿勢は考慮されていない。すなわち、特許文献１の場合、各関節軸ごとの余裕度の設定にとどまり、ロボットの作業空間座標での最終的な姿勢は考慮されていない。また、特許文献１の場合、予め設定された作業範囲についてのみ、ロボットとワークとの最適な位置関係が算出される。そのため、特許文献１の場合、ロボットを用いて希望する作業が限定されている場合にのみ適用可能である。
【０００５】
ところで、特許文献１のように、シミュレーションを用いてロボットとワークとの適切な位置関係を導き出したとしても、ロボットシステムが構築される工場などでは、導き出した位置関係とロボットの設置位置およびラインの設置位置との間に誤差が生じる。また、適切な位置関係を導き出した場合でも、ラインやテーブルに載置されるワークの搭載位置、ワークの大きさなどに僅かな変化が生じると、ロボットとワークとの位置関係にはシミュレーションでは考慮されない誤差が生じる。さらに、ワークの仕様を変更した場合でも、ロボットとワークとの位置関係には相違が生じる。このようにロボットとワークとの位置関係に僅かな誤差や相違が生じると、ロボットの設置時におけるシミュレーションの精度を如何に高めても、作業点がロボットの動作不可能な領域に侵入し、ロボットの動作不良を招くという問題がある。
【０００６】
さらに、シミュレーションを用いてロボットの設置位置とワークの載置位置との関係を導き出す場合、通常、ロボットの各軸の変化量を入力することにより、ロボットのエンドエフェクタの変化量を算出する。そのため、ワーク側で誤差が想定される場合、各軸の変化量を逐次入力し、その結果として得られるエンドエフェクタの変化量がその想定される誤差に適合するか否かを検証する必要がある。その結果、想定される誤差に適合させるためには各軸の変化量を広範囲に入力する必要が生じることから、極めて多くの演算が必要となる。したがって、ロボットとワークとの間に誤差が生じることを想定する場合、ロボットとワークとの最適な位置関係を導き出すためには多くの演算と長期間を必要とするという問題がある。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特許第３７９７９８６号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００８】
そこで、本発明の目的は、簡易な演算により、ロボットの姿勢を判定するロボットの姿勢判定方法を提供することにある。
【課題を解決するための手段】
【０００９】
請求項１または２記載の発明では、式（１）から姿勢変化量Δｑｉを算出する。
Δｑｉ＝Ｊｖ（ｑｉ）-1Δｒ （１）
ここで、位置変化量Δｒは、ワークに誤差Δｐが生じたとき、エンドエフェクタに生じる位置の変化である。また、姿勢変化量Δｑｉは、誤差Δｐにともなうマニピュレータの各軸の変化量である。さらに、ｑｉは、誤差Δｐが存在しないときのマニピュレータの各軸の値である。そして、Ｊｖ（ｑｉ）は、このｑｉにおけるヤコビアンをである。ここで、添字ｉは、作業点の番号を示す。エンドエフェクタがワークに作用する作業点は、通常、複数設定される。この作業点を任意にｉ＝１、２、３・・・として番号を付している。したがって、ｉ≧１となる。
上記の式（１）から算出した姿勢変化量Δｑｉは、式（２）におけるロボットの姿勢判定に用いられる。
ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘ （２）
ここで、最大変化量Δｑｉｍａｘは、マニピュレータの各軸の変化量が最大となる値として式（１）から求められる。また、ｑｍａｘは、関節の回転角度限界である。式（１）で算出した姿勢変化量Δｑｉを用いて、式（２）にあてはめることにより、ワークとの間に誤差Δｐが生じたときのロボットの姿勢が判定される。
【００１０】
上記のような式（１）は、線形近似されている。そのため、コンピュータなどの演算装置にとって処理が容易な線形式を利用してロボットが作業可能な範囲であるか否かが判定される。ところで、ロボットの姿勢変化量が小さいときは、ロボットの姿勢の大きな変化や、作業時間の大幅な変化を招くものではない。すなわち、ロボットの各軸について、＋側で＋側の範囲内または−側で−側の範囲内に僅かな移動が生じる場合、ロボットの姿勢の変化は僅かであり、作業時間の変化も小さい。そのため、ロボットとワークとの位置関係を検証するためには、誤差Δｐが生じていないときを基準とすればよく、線形近似された式（１）により簡単な演算で至適解の候補が算出される。
【００１１】
一方、従来は、ロバスト解探索と称される解の探索手法により、最悪の条件設定の下で最良の解（ロボットの場合、最短の動作時間）を求めている。これをロボットに適用する場合、ワークの設置には自由度ｎの誤差が存在する。この自由度の数ｎは、最大で６（ｘ，ｙ，ｚの各軸方向の並進のずれ３と、各軸の回転のずれ３とで合計が６となる。）である。そのため、最良の解を探索するためには、作業点の数をｉとすると、全自由度の方向へ対応する必要があることから、２6×ｉ×Ｍ×２通りの解の探索が必要となる。ここで、Ｍは、ロボットの軸数である。解の探索は、このロボットの各軸を中心としたプラス方向およびマイナス方向についても判定が必要なため、さらに２倍の演算が必要となる。仮に６軸型ロボットで３０点の作業点における作業を実行する場合、従来の解の探索手法では、２6×３０×６×２＝２３０４０通りの解の探索が必要となり、解の探索に長時間を必要とする。
【００１２】
本願の発明では、式（１）を用いることにより、ワークの各作業点において一通りのヤコビアンで演算できる。そのため、本願の発明により、上述のように６軸型のロボットで３０点の作業点における作業を実行する場合、ロボットの６つの関節軸においてプラス方向およびマイナス方向のそれぞれについて解を探索すれば足り、その演算は６×２＝１２通りとなる。このように、本願の発明では、作業点が多い場合でも、演算が容易となり、解の探索に必要な演算数は低減される。したがって、ロボットとワークとの位置関係に誤差Δｐが生じる場合でも、ロボットが動作可能な姿勢を簡単な処理で判定することができる。
【図面の簡単な説明】
【００１３】
【図１】一実施形態によるロボットシミュレータの構成を示すブロック図
【図２】一実施形態によるロボットシミュレータのシミュレーションの対象となるロボットシステムの一例を示す模式図
【図３】一実施形態によるロボットシミュレータのシミュレーションの対象となるワークおよび作業点を示す模式図
【図４】一実施形態によるロボットシミュレータのシミュレーションの対象となるワークおよび作業点の誤差を示す模式図
【図５】一実施形態によるロボットシミュレータのシミュレーションの流れを示す概略図
【図６】一実施形態によるロボットシミュレータで用いる式（１）を行列式で示す説明図
【図７】一実施形態によるロボットシミュレータで用いる式（２）を行列式で示す説明図
【発明を実施するための形態】
【００１４】
以下、本発明の一実施形態によるロボットシミュレータを図面に基づいて説明する。
（ロボットシステム）
まず、ロボットシミュレータによるシミレーションの対象となるロボットシステムについて図２を用いて説明する。ロボットシステム１０は、ロボット１１およびワーク搭載部１２を備えている。本実施形態のロボット１１は、６軸型である。なお、本実施形態で説明する６軸型のロボット１１は一例であり、ロボット１１の軸の数すなわち関節の数は任意に設定することができる。また、本実施形態では、図２に示すように、設備の床面１３をワーク搭載部１２とし、この床面１３の座標軸をそれぞれＸ軸およびＹ軸と定義し、この床面１３に垂直な座標軸をＺ軸と定義する。さらに、Ｘ軸を中心軸とした回転座標をＲｘ、Ｙ軸を中心とした回転座標をＲｙ、Ｚ軸を中心とした回転座標をＲｚと定義する。
【００１５】
ロボットシステム１０は、上述の通りロボット１１およびワーク搭載部１２を備えている。また、ロボットシステム１０は、図示しないコントローラを備えている。ロボット１１は、ロボットコントローラからの指示により駆動される。ロボット１１は、アームおよび関節から構成されるマニピュレータ１４、ならびにエンドエフェクタ１５を有している。図２に示す本実施形態のロボット１１は、第一関節２１、第二関節２２、第三関節２３、第四関節２４、第五関節２５および第六関節２６を有している。また、ロボット１１は、第一アーム３１、第二アーム３２、第三アーム３３、第四アーム３４および第五アーム３５を有している。
【００１６】
ロボット１１は、設備の床面１３に設置されている。床面１３には、床面１３から垂直上方すなわちＺ軸と平行に立ち上がる土台部４１が固定されている。第一関節２１は、この土台部４１と第一アーム３１とを接続している。第一アーム３１は、第一関節２１において床面１３に垂直な軸を中心に回転可能である。第二関節２２は、第一アーム３１と第二アーム３２とを接続している。第一アーム３１と第二アーム３２とは、第二関節２２を中心に回転する。これにより、第二アーム３２は、Ｚ軸に対する傾斜角度が変化する。第三関節２３は、第二アーム３２と第三アーム３３とを接続している。第二アーム３２と第三アーム３３とは、第三関節２３を中心に回転可能である。すなわち、第二アーム３２と第三アーム３３とは、第三関節２３を支点として開閉する。
【００１７】
第四関節２４は、第三アーム３３と第四アーム３４とを接続している。第三アーム３３と第四アーム３４とは、第四関節２４を中心に相対的に回転する。第三アーム３３と第四アーム３４とは、同一の軸上に位置している。第四アーム３４は、第四関節２４においてこの軸を中心に第三アーム３３に対し回転する。第五関節２５は、第四アーム３４と第五アーム３５とを接続している。第五アーム３５は、第五関節２５を中心に回転する。すなわち、第五アーム３５は、第四アーム３４の軸に垂直な第五関節２５の軸を中心に回転する。第六関節２６は、第五アーム３５とエンドエフェクタ１５とを接続している。第五アーム３５とエンドエフェクタ１５とは、同一の軸上に位置している。エンドエフェクタ１５は、第六関節２６においてこの軸を中心に第五アーム３５に対し回転する。すなわち、エンドエフェクタ１５は、第五関節２５の軸と垂直な軸を中心に回転する。
【００１８】
エンドエフェクタ１５は、作業の対象となるワーク４２に作用する。具体的には、ロボット１１を用いてワーク４２を加工する場合、エンドエフェクタ１５はワーク４２を把持するハンドやワーク４２に加工を加えるツールなどであり、ロボット１１を用いてワーク４２を検査する場合、エンドエフェクタ１５はワーク４２を撮影するカメラなどである。このように、エンドエフェクタ１５は、作業の対象となるワーク４２に対し、接触または非接触により作用する。
【００１９】
ワーク搭載部１２は、ロボット１１による作業の対象となるワーク４２が搭載される。本実施形態の場合、ワーク４２は、床面１３に搭載される。そのため、ワーク搭載部１２は設備の床であり、この床の上端面すなわち床面１３はワーク４２が搭載される搭載面に一致する。なお、ワーク搭載部１２は、例えば床面１３から上方へ立ち上がったテーブル上やコンベヤなどの搬送手段上に形成してもよい。この場合、ワーク４２が搭載されるテーブルやコンベヤがワーク搭載部１２を構成する。
【００２０】
（ロボットシミュレータ）
次に、図１に基づいてロボットシミュレータ５０について説明する。ロボットシミュレータ５０は、例えばパーソナルコンピュータなどの演算装置において、コンピュータソフトウェアであるシミレーションプログラムを実行することにより、ソフトウェア的に実現される。ロボットシミュレータ５０を実現するパーソナルコンピュータは、入力部５１、演算部５２および出力部５３を備えている。入力部５１は、例えばマウス５４およびキーボード５５などの一般的な入力機器を有している。演算部５２は、例えばＣＰＵ５６、ＲＯＭ５７およびＲＡＭ５８を有するコンピュータで構成されており、上述のシミュレーションプログラムを実行することにより、ロボットシミュレータ５０の全体を制御する。出力部５３は、例えば液晶ディスプレイなどの表示部６１やプリンタ６２などを備えており、入力部５１から入力されたデータや演算部５２でシミュレーションプログラムにしたがって算出されたシミュレーションデータなどを出力する。
【００２１】
以下、具体的にロボットシミュレータ５０の構成について説明する。ロボットシミュレータ５０は、パーソナルコンピュータなどの演算装置においてシミュレーションプログラムを実行することにより、演算部５２において、作業順序設定部６５、最短位置抽出部６６および設置位置判定部６７をソフトウェアによって仮想的に実現する。また、ロボットシミュレータ５０の入力部５１は、条件入力部６８およびワーク誤差入力部６９を構成する。
【００２２】
条件入力部６８は、ロボット１１の初期位置、作業順序および作業点の座標などが入力される。ロボット１１の初期位置とは、ロボットシミュレータ５０によるシミュレーションの実行に際して、ワーク搭載部１２に搭載されたワーク４２に対するロボット１１の初期位置である。すなわち、ロボット１１の初期位置は、シミュレーションの開始時における、ロボット１１とワーク４２との初期的な位置関係である。ロボット１１の初期位置となるロボット１１の原点ΣＲ０は、ワーク４２の中心を原点ΣＷとしたとき、ΣＷ上の座標WＰR0とするワークの座標系として表わされる。
このロボット１１の初期位置は、入力部５１から任意の座標として入力することができる。作業順序とは、ロボットシステム１０を用いて実行する複数の工程を時系列で設定したものである。ロボット１１は、各作業点への移動と移動した作業点における作業とを繰り返す。作業順序は、この作業点への移動の順序および作業点における作業を設定したものである。作業点の座標とは、ロボット１１のエンドエフェクタ１５がワーク４２に対して作用を及ぼす位置の座標である。例えばロボットシステム１０を用いてワーク４２の加工を行う場合、作業点とはその加工を行うためにエンドエフェクタ１５とワーク４２とが接する点である。また、例えばロボットシステム１０を用いてワーク４２の検査を行う場合、作業点とはその検査を行うためにエンドエフェクタ１５が移動する位置である。作業点の座標とは、図３に示すようにワーク４２の中心を原点ΣＷ０としたとき、これらの作業点７１、７２、７３における各座標軸の座標WＰ(71)、WＰ(72)、WＰ(73)である。
【００２３】
作業順序設定部６５は、作業所要時間が最小となる最短作業順序を求める。作業順序設定部６５は、条件入力部６８から入力されたロボット１１とワーク４２との位置関係に基づいて、条件入力部６８から入力された作業順序に含まれる複数の工程の作業所要時間を算出する。作業所要時間は、ロボット１１とワーク４２との位置関係および作業点ごとの工程の順序が変更されると、これに基づいて変化する場合がある。そこで、作業順序設定部６５は、ロボット１１とワーク４２との位置関係を設定した状態で必要に応じて工程の順序を入れ替えながら作業所要時間が最短となる最短作業順序を求める。
【００２４】
最短位置抽出部６６は、所要時間が最短となるロボット１１とワーク４２との位置関係を抽出する。最短位置抽出部６６は、条件入力部６８から入力されたワーク４２に対するロボット１１の初期位置における原点ΣＲ０を含む床面１３すなわちＸ−Ｙ平面上に任意の設置範囲Ａを設定する。つまり、最短位置抽出部６６は、まず床面１３の任意の位置に、ロボット１１の設置位置の候補となる範囲の設置範囲Ａを設定する。この設置範囲Ａには、ロボット１１の初期位置における原点ΣＲ０が含まれている。そして、最短位置抽出部６６は、この設定した任意の設置範囲Ａ内において、ロボット１１とワーク４２との位置関係について至適解の候補となる複数の位置関係を抽出する。すなわち、最短位置抽出部６６は、設置範囲Ａ内において、ロボット１１とワーク４２とが好適な位置関係になると予想される複数の位置を抽出し、これを至適解の候補とする。ここで、最適解とせずに至適解の候補としているのは、ロボット１１とワーク４２との最適な位置関係が存在することが予想される場合でも、その最適解が設置範囲Ａ内に含まれるとは限らない。ロボット１１およびワーク４２は、無限の空間に設置されるのではなく、工場など有限の設備に設置される。そのため、算出した解は、最適解とは限らない。したがって、本実施形態の場合、最短位置抽出部６６は、設置範囲Ａ内において条件を満たす位置関係を至適解とし、この至適解となることが予想される複数の位置関係を至適解の候補として抽出している。
【００２５】
最短位置抽出部６６は、抽出した至適解の候補となるロボット１１とワーク４２との位置関係ごとに最短作業順序でロボット１１の複数の工程を仮想的に実行する。すなわち、最短位置抽出部６６は、抽出した至適解の候補となるロボット１１とワーク４２との複数の位置関係について、それぞれ作業順序設定部６５で求めた最短作業順序にしたがって複数の工程を仮想的に実行する。この場合、最短位置抽出部６６は、複数の工程をシミュレーションプログラムにしたがって仮想的に実行する。そして、最短位置抽出部６６は、至適解の候補のうち最も所要時間の短いロボット１１とワーク４２との位置関係を抽出する。すなわち、最短位置抽出部６６は、至適解の候補となる複数の位置関係において、仮想的に複数の工程を実行する。そして、複数の位置関係の中から複数の工程の所要時間が最短となるロボット１１とワーク４２との位置関係を抽出する。これにより、ワーク４２に対する位置関係が設置範囲Ａ内において最も好適となるロボット１１の設置位置が抽出される。この場合、好適となるロボット１１の設置位置は、必ずしも一つに限らず、検討候補として複数抽出してもよい。
【００２６】
ワーク誤差入力部６９は、ワーク搭載部１２に搭載されるワーク４２の位置として想定される誤差量、または個体差によって想定されるワーク４２の誤差量が誤差Δｐとして入力される。最短位置抽出部６６において、複数の工程の所要時間が最短となるロボット１１とワーク４２との至適な位置関係が抽出されても、工場などの設備にロボットシステム１０を設置する際に、ロボット１１とワーク搭載部１２との位置関係が変化したり、ワーク４２のロットの変更やワーク４２の寸法的な個体差などによってロボット１１とワーク４２との位置関係が僅かに変化することがある。このような場合、最短位置抽出部６６におけるロボット１１とワーク４２との位置関係の抽出精度を向上させても、実際の設備では僅かな誤差によってロボット１１の動作不良を招くことになる。そこで、ワーク誤差入力部６９からは、予め想定されるワーク４２の誤差量が誤差Δｐとして入力される。この誤差Δｐは、図４に示すようにワーク４２に対し初期位置として設定されている初期作業点ｐに対し、ワーク搭載部１２に搭載されるワーク４２に想定される誤差量である。ここで初期作業点ｐは、ロボット１１とワーク４２との位置関係が最短位置抽出部６６で抽出された至適な位置関係にあるとき、ワーク４２の原点ΣＷ０からの座標として表わされる。つまり、初期作業点ｐは、空間の位置座標（ｐｘ，ｐｙ，ｐｚ）として示される。これに対し、誤差Δｐが生じた場合の作業点ｐ’は、Ｐ’＝Ｔ（Δｐ）・Ｐとして表わされる。ここで、Ｐ’、Ｐは、それぞれ位置座標（ｐｘ，ｐｙ，ｐｚ，１）、（ｐｘ’，ｐｙ’，ｐｚ’，１）で示されるベクトルであり、Ｔ（Δｐ）は、Δｐ＝（Δｐｘ，Δｐｙ，Δｐｚ，Δｐｒｘ，Δｐｒｙ，Δｐｒｚ）より求められる同時変換行列である。
【００２７】
設置位置判定部６７は、ワーク誤差入力部６９から入力された誤差Δｐに基づいて、ロボット１１でワーク４２に対する作業が可能か否かを判定する。具体的には、設置位置判定部６７は、ワーク誤差入力部６９から入力された誤差Δｐに基づいて、この誤差Δｐが生じたときのエンドエフェクタ１５の位置の変化をベクトルである位置変化量Δｒとして設定する。また、設置位置判定部６７は、入力された誤差Δｐにともなうマニピュレータ１４における各軸の変化量を姿勢変化量Δｑｉとして設定する。位置変化量Δｒは、入力された誤差Δｐに基づいて算出してもよく、誤差Δｐに対応するテーブルとして予め設定しておいてもよい。ここで、誤差Δｐが存在しないときのマニピュレータ１４の各軸の値をｑｉとする。このｑｉは、最短位置抽出部６６で抽出した至適解に相当するロボット１１とワーク４２との位置関係におけるマニピュレータ１４の各軸の値である。そして、設置位置判定部６７は、このｑｉにおけるヤコビアンをＪｖ（ｑｉ）として、以下の式（１）から姿勢変化量Δｑを算出する。なお、ヤコビアンＪｖ（ｑｉ）は、ｑｉが抽出されるごとに逐次設定される構成としてもよく、想定されるｑｉについて予め設定してＲＯＭ５７やＲＡＭ５８に記録しておく構成としてもよい。
Δｑｉ＝Ｊｖ（ｑｉ）-1Δｒ （１）
ここで、Ｊｖ（ｑｉ）は、誤差Δｐが存在しないときのマニピュレータ１４の各軸におけるヤコビアン、すなわちヤコビ行列である。
【００２８】
この式（１）は、姿勢変化量Δｑｉに基づいて位置変化量Δｒを算出する既知の式（１Ａ）を逆変換することにより導き出される。
Δｒ＝Ｊｖ（ｑｉ）Δｑｉ （１Ａ）
上記の式（１）は、姿勢変化量Δｑｉと位置変化量Δｒとが線形の関係に近似されているため、姿勢変化量Δｑｉは位置変化量Δｒに基づいて線形式によって算出される。そのため、式（１Ａ）に比較して姿勢変化量Δｑｉの算出に必要な演算が簡略化され、複数の関節を有するマニピュレータ１４の姿勢変化量Δｑｉの演算が容易になる。
【００２９】
設置位置判定部６７は、上記の式（１）に基づいて姿勢変化量Δｑｉが算出されると、式（２）に基づいてマニピュレータ１４の各軸の値ｑｉを判定する。すなわち、設置位置判定部６７は、式（１）による姿勢変化量Δｑｉの算出の対象としたロボット１１とワーク４２との位置関係の至適解における各軸の値ｑｉが、式（２）の関係を満たすか否かを判定する。
ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘ （２）
ここで、最大変化量Δｑｉｍａｘは、姿勢変化量Δｑｉが最大となるときのマニピュレータ１４の各軸の変化量の最大値である。この最大変化量Δｑｉｍａｘは、誤差Δｐが最大となるときの姿勢変化量として式（１）で算出することにより、既知の値として得られる。また、ｑｍａｘは、ロボット１１のマニピュレータ１４を構成する複数の関節ごとの回転角度限界である。すなわち、設置位置判定部６７は、式（１）で算出した誤差Δｐが最大となるときの姿勢変化量Δｑｉである最大変化量Δｑｉｍａｘから、式（１）における演算で用いたｑｉが、式（２）で示される関係において回転角度限界ｑｍａｘとの大小関係を満たすか否かを判定する。設置位置判定部６７は、この判定した結果に基づいて、ロボット１１でワーク４２に対する作業が可能か否かについて判定する。つまり、設置位置判定部６７は、式（１）で算出された姿勢変化量Δｑｉに含まれる要素が式（２）の大小関係を満たすとき、ワーク４２の位置に変化が生じたときでも、ロボット１１とワーク４２との位置関係が至適であると判定する。設置位置判定部６７は、以上の演算により、式（１）および式（２）に基づいて、ワーク４２が初期位置から誤差Δｐだけ移動したとしても、ロボット１１がワーク４２に対して作業可能であるか否かを判定する。この判定についての詳細は、以下のシミュレーションの流れにおいて詳細に説明する。
【００３０】
（シミュレーションの流れ）
次に、上記の構成によるロボットシミュレータ５０によるシミュレーションの流れについて図５に基づいて説明する。
図５に示すように、ロボットシミュレータ５０は、大きくＳ１０１およびＳ１０２における入力処理、Ｓ１０３からＳ１０６までにおける最適化処理、およびＳ１０７における出力処理に大別される。以下、各処理について説明する。
【００３１】
ロボットシミュレータ５０は、条件入力部６８から初期状態の入力を受け付ける（Ｓ１０１）。Ｓ１０１では、初期状態としてロボット１１の初期位置、作業順序および作業点の座標が入力部５１から入力される。ロボット１１の初期位置は、上述の通り、シミュレーションの実行に際して、ワーク搭載部１２に搭載されたワーク４２に対するロボット１１の初期位置である。ロボット１１の初期位置における原点ΣＲ０は、ワーク４２の中心を原点ΣＷとしたとき、各軸の座標としてΣＲ０＝（ＸＲ０，ＹＲ０，ＺＲ０）で表わされる。このロボット１１の初期位置は、入力部５１から任意の座標として入力される。
作業手順は、ロボットシステム１０を用いて実行する複数の工程を時系列で設定したものである。すなわち、作業手順は、ロボット１１の作業点における作業順序の初期的な情報である。なお、対象となるワーク４２の外観を検査する外観検査などの場合、検査順序は拘束されない。そのため、検査の場合、すべての検査に必要な所要時間が最小となるように作業点における検査の順序が変更される。一方、対象となるワーク４２の加工を行う場合、加工順序が拘束される。そのため、ワーク４２の加工を行う場合、作業手順に含まれる各工程の順序は予め設定される。
【００３２】
作業点は、ロボット１１のエンドエフェクタ１５がワーク４２に対して作用を及ぼす位置の座標である。上述の通り、ロボットシステム１０でワーク４２の加工を行う場合、作業点はその加工を行うためにエンドエフェクタ１５とワーク４２とが接する点であり、ロボットシステム１０でワーク４２の検査を行う場合、作業点はエンドエフェクタ１５が検査のために移動する点である。作業点は、各工程に応じて複数設定される。これら作業点Ｐ［ｉ］の座標は、各軸における位置および回転角度から、Ｐ［ｉ］＝（Ｘｉ，Ｙｉ，Ｚｉ，Ｒｘｉ，Ｒｙｉ，Ｒｚｉ）で表わされる。
【００３３】
また、これら初期状態の入力に加えて、入力部５１を構成するワーク誤差入力部６９からは、ワーク４２に想定される誤差量が誤差Δｐとして入力される（Ｓ１０２）。上述のようにロボット１１とワーク４２との至適な位置関係を抽出したとしても、工場などへのロボットシステム１０の設置の際、およびワーク４２の形状の変化などによってロボット１１とワーク４２との位置関係が僅かに変化する。そのため、ワーク誤差入力部６９から予め予想されるワーク４２の誤差量を既知の誤差Δｐとして入力し、この誤差Δｐが許容されるか否かを後述するＳ１０５において判定する。そのため、Ｓ１０２において、Ｓ１０５における判定で用いる誤差Δｐが入力される。なお、この誤差Δｐの入力は、Ｓ１０２に限らず、Ｓ１０５における判定を実行する以前であれば、任意の時期に行ってもよい。
【００３４】
以上のように、Ｓ１０１において入力部５１から初期状態が入力、およびＳ１０２において誤差Δｐが入力されると、ロボットシミュレータ５０の作業順序設定部６５は、作業所要時間が最小となる最短作業順序の評価を行う（Ｓ１０３）。具体的には、作業順序設定部６５は、ワーク４２に対してロボット１１が条件入力部６８から入力された初期位置における原点ΣＲ０にあるとき、任意の作業順序で作業所要時間を算出する。上述のように作業所要時間は、ロボット１１とワーク４２との位置関係や作業点ごとの工程の順序によって変化する。そのため、作業順序設定部６５は、ワーク４２に対しロボット１１が初期位置における原点ΣＲ０にある状態で複数の工程の順序を入れ替えながら作業所要時間が最短となる最短作業順序を求め、工程の順序ごとに作業所要時間を評価する。
【００３５】
Ｓ１０３において最短作業順序の評価が終了すると、ロボットシミュレータ５０の最短位置抽出部６６は、所要時間が最短となるロボット１１とワーク４２との位置関係を抽出または変更する（Ｓ１０４）。最短位置抽出部６６は、上述のように条件入力部６８から入力されたワーク４２に対するロボット１１の初期位置における原点ΣＲ０を含む床面１３であるＸ−Ｙ平面上に任意の設置範囲Ａを設定する。ロボット１１の初期位置における原点ΣＲ０は、この設置範囲Ａに含まれている。最短位置抽出部６６は、この任意の設置範囲Ａ内において、ロボット１１とワーク４２との位置関係について至適解の候補となる複数の位置関係を抽出する。すなわち、最短位置抽出部６６は、設置範囲Ａ内において、ロボット１１とワーク４２とが好適な位置関係になると予想される複数の位置を抽出し、これを至適解の候補とする。そして、最短位置抽出部６６は、抽出した至適解の候補となるロボット１１とワーク４２の位置関係ごとに作業順序設定部６５で評価した最短作業順序にしたがって複数の工程を仮想的に実行する。最短位置抽出部６６は、至適解の候補となっている位置関係ごとに最短作業順序にしたがって複数の工程を仮想的に実行し、至適解の候補から所要時間が最短となるロボット１１とワーク４２との位置関係を抽出する。このように、最短位置抽出部６６は、候補として抽出した至適解に基づいてロボット１１とワーク４２との位置関係を変化させながら、所要時間が最短となる至適解に相当するロボット１１とワーク４２との位置関係を抽出する。この複数の至適解の候補から、所要時間が最短となるロボット１１とワーク４２との位置関係に対応する至適解を抽出する処理は、例えばＰＳＯ法など既知の手法によって実行される。後述するＳ１０５の処理によってＳ１０４へリターンし、Ｓ１０４を再び実行する場合、最短位置抽出部６６はロボット１１とワーク４２との位置関係を変更、すなわち至適解の候補をさらに変更し、上述の処理を繰り返す。
【００３６】
Ｓ１０４において、所要時間が最短となるロボット１１とワーク４２との位置関係が抽出されると、ロボットシミュレータ５０の設置位置判定部６７は、誤差Δｐに基づいて、その誤差Δｐを設定してもロボット１１でワーク４２に対して作業可能であるか、すなわち誤差Δｐがロボット１１の作業に許容できるものであるか否かを判定する（Ｓ１０５）。具体的には、設置位置判定部６７は、上述の通り、Ｓ１０２においてワーク誤差入力部６９から入力された誤差Δｐに基づいて、この誤差Δｐが生じたときのエンドエフェクタ１５の位置の変化を位置変化量Δｒとして設定する。この位置変化量Δｒは、誤差Δｐによって生じるワーク４２の初期位置となる原点ΣＷに対する各軸方向の位置および回転方向のずれであり、方向と大きさとからなるベクトルとして、Δｒ＝（ΔＸｗ，ΔＹｗ，ΔＺｗ，ΔＲｘｗ，ΔＲｙｗ，ΔＲｚｗ）で示される。添字ｗは、位置変化量Δｒがワーク４２の位置変化量であることを示す。また、設置位置判定部６７は、入力された誤差Δｐにともなうマニピュレータ１４における各軸の値の変化を姿勢変化量Δｑとして算出する。この姿勢変化量Δｑｉは、マニピュレータ１４の各関節ごとの回転角度として、Δｑｉ＝（Δθ１，Δθ２，Δθ３，Δθ４，Δθ５，Δθ６）で表わされる。
【００３７】
より具体的には、設置位置判定部６７は、以下の式（１）に基づいて、位置変化量Δｒから姿勢変化量Δｑｉを算出する。以下の式（１）において、ｑｉは、誤差Δｐが存在しないときのマニピュレータ１４の各軸の値である。このｑｉは、上述の通り最短位置抽出部６６においてＳ１０４で抽出した至適解に相当するロボット１１とワーク４２との位置関係における各軸の値である。Ｊｖ（ｑｉ）は、このｑｉにおけるヤコビアンである。
Δｑ＝Ｊｖ（ｑｉ）-1Δｒ （１）
この式（１）は、姿勢変化量Δｑｉと位置変化量Δｒとが線形の関係に近似されているため、姿勢変化量Δｑｉは誤差Δｐに相関する位置変化量Δｒに基づいて線形的に算出される。そのため、式（１）を用いることにより、姿勢変化量Δｑｉの算出に必要な演算は簡略化され、複数の関節を有するマニピュレータ１４の姿勢変化量Δｑｉはより容易な演算で算出される。式（１）を行列式で表わした場合、図６に示す式（１）’となる。この式（１）’では、誤差Δｐに相関するワーク４２の位置変化量Δｒを、各軸方向の変化量および回転角度の要素として、Δｒ＝（ΔＸ，ΔＹ，ΔＺ，ΔＲｘ，ΔＲｙ，ΔＲｚ）と表わしている。また、この位置変化量Δｒを、ｑｉのヤコビアンＪｖ（ｑｉ）の逆行列であるＪｖ（ｑｉ）-1で逆変換した結果として得られる姿勢変化量Δｑは、ロボット１１の各軸の回転角度として、Δｑｊ＝（Δθ１，Δθ２，Δθ３，Δθ４，Δθ５，Δθ６）で表わされる。ここで添字のｊ＝１〜６は、ロボット１１の第一関節２１〜第六関節２６にそれぞれ対応している。
【００３８】
設置位置判定部６７は、上記の式（１）に基づいて姿勢変化量Δｑｉが算出されると、式（２）に基づいてマニピュレータ１４の各軸の値ｑｉを判定する。すなわち、設置位置判定部６７は、式（１）による姿勢変化量Δｑｉの算出の対象としたロボット１１とワーク４２との位置関係の至適解における各軸の値ｑｉが式（２）の関係を満たすか否かを判定する。
ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘ （２）
式（２）において、最大変化量Δｑｉｍａｘは、ワーク４２の誤差Δｐが最大となるときのマニピュレータ１４の各軸の変化量である。この最大変化量Δｑｉｍａｘは、誤差Δｐが最大となるときの姿勢変化量Δｑｉを式（１）で算出することにより、既知の値として得られる。また、ｑｍａｘは、ロボット１１のマニピュレータ１４を構成する複数の関節ごとの回転角度限界である。
【００３９】
具体的に説明すると、式２は、図７の式（２）’に示すような行列式として表わされる。式（２）における最大変化量Δｑｍａｘは、誤差Δｐが最大となるときに式（１）に基づいて算出される既知の値である。姿勢変化量Δｑｉを構成する要素は、Δθｊ（ｊは、ロボット１１の軸の番号であり、六軸型の本実施形態のロボット１１の場合、ｊ＝１〜６となる。）で示される。また、式（２）におけるｑｍａｘは、ロボット１１のマニピュレータ１４を構成する複数の関節ごとに許容される回転角度、すなわち回転角度限界である。この回転角度限界ｑｍａｘは、図７の式（２）’に示すように各関節ごとにθｊｍａｘ（ｊ＝１〜６）としてそれぞれ予め設定されている。
【００４０】
設置位置判定部６７は、Ｓ１０４で抽出して式（１）における計算の基礎とした至適解における初期的な各軸の値ｑｉと式（１）を用いて算出した誤差Δｐが最大となるときの姿勢変化量Δｑｉｍａｘとの和が、回転角度限界ｑｍａｘ以下となるように、要素ごとに大小関係を判定する。つまり、設置位置判定部６７は、式（１）の算出の基礎としたｑｉを構成する要素である第一関節２１の回転角度θ１と、誤差Δｐが最大となるとして式（１）で算出した姿勢変化量Δｑｉｍａｘを構成する要素である第一関節２１の角度変化量Δθ１ｍａｘとの和が、予め設定されている第一関節２１の回転角度限界θ１ｍａｘ以下であるかを判定する。同様に、設置位置判定部６７は、第二関節２２についてθ２＋Δθ２ｍａｘ≦θ２ｍａｘであるか、第三関節２３についてθ３＋Δθ３ｍａｘ≦θ３ｍａｘであるか、第四関節２４についてθ４＋Δθ４ｍａｘ≦θ４ｍａｘであるか、第五関節２５についてθ５＋Δθ５ｍａｘ≦θ５ｍａｘであるか、第六関節２６についてθ６＋Δθ６ｍａｘ≦θ６ｍａｘであるか否かをそれぞれ判定する。
【００４１】
そして、設置位置判定部６７は、上記の式（２）すなわち式（２）’を用いた判定の結果、要素となる複数の関節のすべてが式（２）すなわち式（２）’を満たす場合、式（１）における演算の基礎とした各軸の値ｑｉが成立するとして、このｑｉに対応するロボット１１とワーク４２との位置関係が適切であると判定する。一方、設置位置判定部６７は、要素となる複数の関節のうちいずれか一つが式（２）すなわち式（２）’の関係を満たさない場合、式（１）における演算の基礎とした各軸の値ｑｉが成立しないとして、このｑｉに対応するロボット１１とワーク４２との位置関係が不適切であると判定する。上記のように、設置位置判定部６７は、以上の演算により式（１）おおび式（２）に基づいて、ワーク４２が初期位置から誤差Δｐだけ移動、特に誤差Δｐが際となるまで移動したとしても、ロボット１１がワーク４２に対して作業可能であるか否かをすべての作業点について判定する。
【００４２】
設置位置判定部６７は、式（１）および式（２）に基づく演算結果から、ロボット１１がワーク４２に対して作業可能である、すなわち誤差Δｐが許容可能であると判定すると（Ｓ１０５：Ｙｅｓ）、計算時間制約を判定するために計算所要時間に到達したか否かを判断する（Ｓ１０６）。設置位置判定部６７は、式（１）および式（２）によりロボット１１がワーク４２に対して作業可能であると判定したとき、計算時間が予め設定した計算所要時間内にあるか否かを判断する。ロボットシステム１０では、複数の工程からなる作業の所要時間が最短となる具体的な時間を予め知ることはできない。そのため、設置位置判定部６７では、各演算の無限の繰り返しを招くおそれがある。そこで、設置位置判定部６７は、Ｓ１０６において予め設定した計算所要時間に到達したか否かを判断する。この計算所要時間は、確保したい精度に応じて任意に設定される。設置位置判定部６７は、予め設定した計算所要時間に到達するまでＳ１０３からＳ１０６までの最適化処理を繰り返し、予め設定した計算所要時間を経過すると、Ｓ１０７へ移行する。すなわち、設置位置判定部６７は、Ｓ１０３の最初の実行からの経過時間が計算所要時間に到達したか否かを判断し、計算所要時間に到達していないとき（Ｓ１０６：Ｎｏ）、Ｓ１０３へリターンし、作業順序の変更以降の最適化処理を繰り返す。一方、設置位置判定部６７は、Ｓ１０３の最初の実行からの経過時間が計算所要時間に到達したと判断すると（Ｓ１０６：Ｙｅｓ）、Ｓ１０７の出力処理へ移行し、最適化によって得られた条件の範囲内における最適な作業順序、ロボット１１とワーク４２との位置関係を表示部６１へ可視的に出力する。
【００４３】
ところで、設置位置判定部６７は、式（１）および式（２）に基づく算出結果から、ロボット１１がワーク４２に対して作業可能でない、すなわち誤差Δｐが許容できないと判定すると（Ｓ１０５：Ｎｏ）、Ｓ１０４へリターンし、ロボット１１とワーク４２との位置関係を、複数の至適解の候補のうち他の候補に変更する。すなわち、Ｓ１０４では複数の至適解の候補から所要時間が最短となるロボット１１とワーク４２との位置関係が抽出され、この位置関係についてＳ１０５で誤差が許容されるか否かが判定される。ここで、所要時間が最短となるロボット１１とワーク４２との位置関係ではＳ１０５で誤差が許容できないと判定されると、先のＳ１０４で所要時間が最短と判断された位置関係の次に所要時間の短い至適解の候補を抽出し、Ｓ１０５の処理を繰り返す。そして、設置位置判定部６７は、変更した至適解の候補に対応するロボット１１とワーク４２との位置関係に基づいて、Ｓ１０５における判定処理を実行する。
【００４４】
以上の手順により、ロボットシミュレータ５０は、Ｓ１０４で設置範囲Ａに抽出された至適解の候補について、ロボット１１とワーク４２と位置関係、および誤差Δｐによるロボット１１の作動の可否を計算所要時間に到達するまで繰り返し評価する。そして、計算所要時間に到達すると、ロボット１１における作業順序および至適解に相当するロボット１１とワーク４２との位置関係を、シミュレーションの範囲内における最適値として表示部６１に出力する。なお、至適解または至適解に近似するロボット１１とワーク４２との位置関係が複数ある場合、これら複数の位置関係を好ましい順に順位を付して表示部６１に表示する構成としてもよい。
【００４５】
以上説明した一実施形態の場合、上記の式（１）は、線形近似されている。そのため、パーソナルコンピュータなどの演算部５２にとって処理が容易な線形式を利用してロボット１１が作業可能な範囲であるか否かが判定される。ところで、ワーク４２に生じるとして設定している誤差Δｐは、ロボット１１の姿勢の大きな変化や、作業時間の大幅な増加を招くものではない。そのため、ロボット１１とワーク４２との位置関係を検証するためには、誤差Δｐが生じていないときを基準とすればよく、線形近似された式（１）により簡単な演算で至適解の候補を算出することができる。一方、ロボット１１の各軸の姿勢に大きな変化が生じる場合でも、上記の式（１）および式（２）を用いてそのまま計算することにより、作業が不可能な姿勢は式（２）によって不適切な解と判定される。そのため、ロボットとワークとの位置関係を検証するためには、誤差Δｐが生じていないときを基準とすればよく、誤差Δｐは無視してもよい。
【００４６】
また、一実施形態では、式（１）を用いることにより、ワーク４２の各作業点において一通りのヤコビアンで演算できる。したがって、ロボット１１とワーク４２との位置関係に誤差Δｐが生じる場合でも、ロボット１１が動作可能となるロボット１１とワーク４２との最適な位置関係を簡単な処理で導き出すことができる。
【００４７】
以上説明した本発明は、上記実施形態に限定されるものではなく、その要旨を逸脱しない範囲で種々の実施形態に適用可能である。
【符号の説明】
【００４８】
図面中、１０はロボットシステム、１１はロボット、１２はワーク搭載部、１４はマニピュレータ、１５はエンドエフェクタ、２１は第一関節、２２は第二関節、２３は第三関節、２４は第四関節、２５は第五関節、２６は第六関節、３１は第一アーム、３２は第二アーム、３３は第三アーム、３４は第四アーム、３５は第五アーム、４２はワーク、５０はロボットシミュレータ、５１は入力部、５２は演算部、５３は出力部、６５は作業順序設定部、６６は最短位置抽出部、６７は設置位置判定部、６８は条件入力部、６９はワーク誤差入力部を示す。
【特許請求の範囲】
【請求項１】
マニピュレータの先端にワークを取り扱うエンドエフェクタを有するロボットの設置位置と前記ワークの設置位置との関係を求めるロボットの姿勢判定方法であって、
前記ワークに想定される誤差量を誤差Δｐとし、前記誤差Δｐにともなう前記エンドエフェクタの位置の変化を位置変化量Δｒとし、前記誤差Δｐにともなう前記マニピュレータにおける各軸の変化量を姿勢変化量Δｑｉとし、前記誤差Δｐが存在しないときの前記マニピュレータの各軸の値をｑｉ（ｉは、作業点の番号であり、ｉ≧１）とし、このｑｉにおけるヤコビアンをＪｖ（ｑｉ）として、前記姿勢変化量Δｑｉを、
Δｑｉ＝Ｊｖ（ｑｉ）-1Δｒ （１）
により算出するとともに、
前記式（１）で算出される前記姿勢変化量Δｑｉが最大となるときの前記マニピュレータの各軸の変化量を最大変化量Δｑｉｍａｘとし、前記マニピュレータを構成する関節の回転角度限界をｑｍａｘとすると、
ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘ （２）
を満たす前記マニピュレータの各軸の値ｑｉを誤差Δｐを有するワークに対し当該作業点ｉにおける作業が可能な姿勢であると判定することを特徴とするロボットの姿勢判定方法。
【請求項２】
複数のアームおよび関節から構成されるマニピュレータおよび前記マニピュレータの先端に設けられワークを取り扱うエンドエフェクタを有するロボットの設置位置と前記ワークの設置位置との関係を求めるロボットの姿勢判定方法であって、
前記エンドエフェクタが前記ワークに作用する作業点のうち初期位置として予め設定されている初期作業点に対し、前記ワークに想定される誤差量を誤差Δｐとして設定する手順と、
設定された前記誤差Δｐに基づいて、前記誤差Δｐにともなう前記エンドエフェクタの位置の変化を位置変化量Δｒとし、前記誤差Δｐにともなう前記マニピュレータにおける各軸の値の変化量を姿勢変化量Δｑｉ（ｉは、作業点の番号であり、ｉ≧１）とし、前記誤差Δｐが存在しないときの前記マニピュレータの各軸の値をｑｉとし、このｑｉにおけるヤコビアンをＪｖ（ｑｉ）として、前記姿勢変化量Δｑｉを、
Δｑｉ＝Ｊｖ（ｑｉ）-1Δｒ （１）
により算出するとともに、
前記式（１）で算出される前記姿勢変化量Δｑｉが最大となるときの前記マニピュレータの各軸の変化量を最大変化量Δｑｉｍａｘとし、前記関節の回転角度限界をｑｍａｘとすると、
ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘ （２）
を満たす前記マニピュレータの各軸の値ｑｉを誤差Δｐを有するワークに対し当該作業点ｉにおける作業が可能な姿勢であると判定する手順と、
を含むことを特徴とするロボットの姿勢判定方法。
【請求項１】
マニピュレータの先端にワークを取り扱うエンドエフェクタを有するロボットの設置位置と前記ワークの設置位置との関係を求めるロボットの姿勢判定方法であって、
前記ワークに想定される誤差量を誤差Δｐとし、前記誤差Δｐにともなう前記エンドエフェクタの位置の変化を位置変化量Δｒとし、前記誤差Δｐにともなう前記マニピュレータにおける各軸の変化量を姿勢変化量Δｑｉとし、前記誤差Δｐが存在しないときの前記マニピュレータの各軸の値をｑｉ（ｉは、作業点の番号であり、ｉ≧１）とし、このｑｉにおけるヤコビアンをＪｖ（ｑｉ）として、前記姿勢変化量Δｑｉを、
Δｑｉ＝Ｊｖ（ｑｉ）-1Δｒ （１）
により算出するとともに、
前記式（１）で算出される前記姿勢変化量Δｑｉが最大となるときの前記マニピュレータの各軸の変化量を最大変化量Δｑｉｍａｘとし、前記マニピュレータを構成する関節の回転角度限界をｑｍａｘとすると、
ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘ （２）
を満たす前記マニピュレータの各軸の値ｑｉを誤差Δｐを有するワークに対し当該作業点ｉにおける作業が可能な姿勢であると判定することを特徴とするロボットの姿勢判定方法。
【請求項２】
複数のアームおよび関節から構成されるマニピュレータおよび前記マニピュレータの先端に設けられワークを取り扱うエンドエフェクタを有するロボットの設置位置と前記ワークの設置位置との関係を求めるロボットの姿勢判定方法であって、
前記エンドエフェクタが前記ワークに作用する作業点のうち初期位置として予め設定されている初期作業点に対し、前記ワークに想定される誤差量を誤差Δｐとして設定する手順と、
設定された前記誤差Δｐに基づいて、前記誤差Δｐにともなう前記エンドエフェクタの位置の変化を位置変化量Δｒとし、前記誤差Δｐにともなう前記マニピュレータにおける各軸の値の変化量を姿勢変化量Δｑｉ（ｉは、作業点の番号であり、ｉ≧１）とし、前記誤差Δｐが存在しないときの前記マニピュレータの各軸の値をｑｉとし、このｑｉにおけるヤコビアンをＪｖ（ｑｉ）として、前記姿勢変化量Δｑｉを、
Δｑｉ＝Ｊｖ（ｑｉ）-1Δｒ （１）
により算出するとともに、
前記式（１）で算出される前記姿勢変化量Δｑｉが最大となるときの前記マニピュレータの各軸の変化量を最大変化量Δｑｉｍａｘとし、前記関節の回転角度限界をｑｍａｘとすると、
ｑｉ＋Δｑｉｍａｘ≦ｑｍａｘ （２）
を満たす前記マニピュレータの各軸の値ｑｉを誤差Δｐを有するワークに対し当該作業点ｉにおける作業が可能な姿勢であると判定する手順と、
を含むことを特徴とするロボットの姿勢判定方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２０１１−１５６６０４（Ｐ２０１１−１５６６０４Ａ）
【公開日】平成２３年８月１８日（２０１１．８．１８）
【国際特許分類】
【出願番号】特願２０１０−１８５７９（Ｐ２０１０−１８５７９）
【出願日】平成２２年１月２９日（２０１０．１．２９）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【出願人】（５０４１３７９１２）国立大学法人 東京大学 (1,942)
【Ｆターム（参考）】
【公開日】平成２３年８月１８日（２０１１．８．１８）
【国際特許分類】
【出願日】平成２２年１月２９日（２０１０．１．２９）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【出願人】（５０４１３７９１２）国立大学法人 東京大学 (1,942)
【Ｆターム（参考）】
[ Back to top ]