JP2006068890A - 軌道決定システムおよび軌道決定方法 - Google Patents

Abstract

【課題】多関節ロボットの連続軌道追従動作時における手先の位置および姿勢の適正な軌道を容易かつ安定的に決定することができる軌道決定システムおよび軌道決定方法を提供する。
【解決手段】軌道決定システム１００は、多関節ロボット１０の連続軌道追従動作時における手先の位置および姿勢の軌道を決定するシステムであって、動作開始時と動作完了時とにおける手先の位置および姿勢の情報に基づき、手先の位置および姿勢の軌道を複数生成する軌道生成手段１１０と、先端関節の現回転角度を推定する軌道推定手段１１１と、動作開始時と動作完了時との間の複数の箇所において前記現回転角度の検証を行って、前記手先の位置および姿勢の複数の軌道のうち、所定判定基準により適合する最適軌道を特定する軌道検証手段１１２とを備える。
【選択図】図３

Description

本発明は、多関節ロボットの連続軌道追従動作時における手先の位置および姿勢の軌道を決定する軌道決定システムおよび軌道決定方法に関するものである。

多関節ロボットの連続軌道追従動作における、動作開始時から動作完了時に至る手先の位置および姿勢の制御方法に関して、手先の位置および姿勢の変化を手先座標系（ツール座標系）のＺ軸周りの回転とそれに垂直な軸（例：ｘ軸、ｙ軸）周りの回転とに分け、それぞれを時間関数により補間して位置・姿勢制御を行う方法が一般的に用いられている。
図１は、多関節ロボット１０が直線の軌道１１上において手先部１２の手先１２１の位置および姿勢（手先位置・姿勢）を変化させながら動作する様子を表す簡略図である。多関節ロボット１０としては、正逆両方向に回転可能な６つの関節（関節部）を有する垂直６軸型ロボットが想定されている。

図１に示すように多関節ロボット１０の手先部１２の手先１２１が直線の軌道１１上を移動する場合、その多関節ロボット１０の先端関節１４（手先座標系のＺ軸周り）の初めの回転方向１３の取り方によって、前記多関節ロボット１０の手先１２１は、２通りの軌道をとることができる。
一方の軌道（回転方向１３：図中の例ではａに対応）では、動作完了後の手先１２１の姿勢が目標通り実現されると共に、先端関節１４の回転角度は実際に目標位置１５をティーチングしたときと同じ回転角度に達することとなる。

しかし、他方の軌道（回転方向１３：図中の例ではｂに対応）では、動作完了後の手先１２１の姿勢が目標通り実現されるものの、先端関節１４の回転角度は実際に目標位置１５をティーチングしたときとは異なり、１回転分ずれた回転角度に達してしまうこととなる。
なお、ここでは、手先座標系は、手先座標系のＺ軸と先端関節１４の回転軸とが平行、すなわち、手先座標系のＺ軸周りの回転方向と、先端関節１４の回転方向とが一致するように想定されているが、手先座標系のＺ軸と先端関節１４の回転軸とが平行にならないように想定してもよい。

このような、前記多関節ロボット１０の手先１２１が当初の予定とは異なる回転角度で回転する動作を避けるためには、先端関節１４の初めの回転方向１３が正しく選択された軌道を生成する必要がある。その手法の一つとして、前記軌道１１の途中の所定の一点（以下、「中間点」と言う）での先端関節１４の回転角度が多関節ロボット１０の動作開始時の先端関節１４の回転角度と動作完了時の先端関節１４の回転角度との間にあることをもって正しい軌道であると判断する方法が提案されている（例えば、特許文献１参照）。

しかしながら、多関節ロボット１０に与えられた現在位置と目標位置１５との関係によっては、いずれの回転方向１３を選択しても、軌道１１の中間点では先端関節１４の回転角度が動作開始時と動作完了時との間の値をとらないこともある。そのため、前記従来の方法では、正しい軌道を選択することができない場合があるという問題がある。
なぜなら、多関節ロボットの連続軌道追従動作においては、軌道１１に追従するために動作途中で先端関節１４の回転方向１３を反転させなければならないケースがごく普通に存在するからである。このような場合、前記中間点では先端関節１４の回転角度が動作開始時の先端関節１４の回転角度と動作完了時の先端関節１４の回転角度との間にあっても、その後の回転方向１３の反転により、結果として目標位置１５に到達した時点で先端関節１４が１回転分ずれてしまうことがあり、また、逆に、前記中間点では先端関節１４の回転角度が動作開始時の先端関節１４の回転角度と動作完了時の先端関節１４の回転角度との間になくても、その後の回転方向１３の反転により正しい回転角度に到達することもある。

図２は、多関節ロボット１０の先端関節１４の回転角度の時間推移を回転方向別に示した図であり、横軸を多関節ロボット１０の動作開始から動作完了までの時間、縦軸を先端関節１４の回転角度としている。
この図における正しい軌道２０では、多関節ロボット１０の先端関節１４が、一旦、目標回転角度とは逆の回転方向２１に向けて回転する共に、その後、反対の（反転した）回転方向２２に向けて回転して目標回転角度に到達している。

これに対し、この図における誤った軌道２３では、多関節ロボット１０の先端関節１４が、最初から目標回転角度と同じ回転方向２２に向けて回転するが、そのまま目標回転角度を通り過ぎ、１回転分ずれた回転角度で動作を完了している。
こうした現象は、多関節ロボット１０における姿勢変化が大きくなる動作で発生しやすいため、前記目標位置１５を軌道１１の経路上で複数設けて細分化することで、各目標位置１５の間における多関節ロボット１０の姿勢変化を少なくすることにより回避できる。

しかしながら、前記目標位置１５を軌道１１の経路上で複数設けて細分化する方法では、前記複数の目標位置１５について逐次ティーチングする必要が生じ、多大な手間と時間を要してしまうという問題がある。
以上のように、従来では、多関節ロボット１０の連続軌道追従動作時の挙動をシミュレータ等で視覚化することなく動作前に検証することは困難となっている。このため、実際に多関節ロボット１０を動作させて初めて先端関節１４が誤った方向に回転することがわかったり、また、許容値を超えるような関節速度を制御システム側が検出してエラーを生じたるといったケースが少なくない。

特開平１−６１８０４号公報

本発明の目的は、多関節ロボットの連続軌道追従動作時における手先の位置および姿勢の適正な軌道を容易かつ安定的に決定することができる軌道決定システムおよび軌道決定方法を提供することにある。

このような目的は、下記の本発明により達成される。
本発明の軌道決定システムは、多関節ロボットの連続軌道追従動作時における手先の位置および姿勢の軌道を決定する軌道決定システムであって、
連続軌道追従動作の動作開始時と動作完了時とにおける、前記多関節ロボットにおける手先の位置および姿勢の情報に基づき、前記多関節ロボットの手先の位置および姿勢の軌道を複数生成する軌道生成手段と、
前記手先の位置および姿勢の軌道に則した連続軌道追従動作時の前記多関節ロボットにおける、先端関節の現回転角度を推定する軌道推定手段と、
前記連続軌道追従動作を行う多関節ロボットの動作開始時と動作完了時との間の複数の箇所において前記現回転角度の検証を行って、前記手先の位置および姿勢の複数の軌道のうち、所定判定基準により適合する最適軌道を特定する軌道検証手段とを備えることを特徴とする。
これにより、多関節ロボットの連続軌道追従動作時における手先の位置および姿勢の適正な軌道（最適な軌道）を、多関節ロボット動作前に、容易かつ安定的に決定することができる。これによって、多関節ロボットを、容易に、最適かつ確実に作動させることができる。

本発明の軌道決定システムでは、前記軌道検証手段は、前記連続軌道追従動作の動作開始時の前記先端関節の回転角度と前記連続軌道追従動作の動作完了時の前記先端関節の回転角度との間の回転角度範囲に、前記検証対象となった前記現回転角度が含まれる手先の位置および姿勢の軌道を特定し、これを前記最適軌道とすることが好ましい。
これにより、軌道決定の処理効率および処理精度が向上する。

本発明の軌道決定システムでは、前記軌道検証手段は、前記多関節ロボットの先端関節の回転開始角度と回転完了角度との間を線形補間する補間角度に、前記検証対象となった前記現回転角度がより近い手先の位置および姿勢の軌道を特定し、これを前記最適軌道とすることが好ましい。
これにより、軌道決定の処理効率および処理精度が向上する。

本発明の軌道決定システムでは、前記軌道検証手段は、前記連続軌道追従動作の動作開始時の前記先端関節の回転角度と前記連続軌道追従動作の動作完了時の前記先端関節の回転角度との間の回転角度範囲に、前記検証対象となった前記現回転角度が含まれる手先の位置および姿勢の軌道を特定し、これを前記最適軌道とし、前記回転角度範囲に前記検証対象となった前記現回転角度が含まれる手先の位置および姿勢の軌道が無い場合は、前記多関節ロボットの先端関節の回転開始角度と回転完了角度との間を線形補間する補間角度に、前記検証対象となった前記現回転角度がより近い手先の位置および姿勢の軌道を特定し、これを前記最適軌道とすることが好ましい。
これにより、軌道決定の処理効率および処理精度が向上する。

本発明の軌道決定システムでは、前記軌道検証手段は、前記現回転角度の検証の結果、各検証箇所での前記手先の位置および姿勢の軌道の特定結果が一致した場合にのみ、前記最適軌道を特定することが好ましい。
これにより、軌道決定の処理効率および処理精度が向上する。
本発明の軌道決定システムでは、前記現回転角度の検証の結果、各検証箇所での前記手先の位置および姿勢の軌道の特定結果が一致しない場合、エラー通知を出力する、エラー通知手段を備えることが好ましい。
これにより、軌道決定の処理効率および処理精度が向上する。

本発明の軌道決定システムでは、前記エラー通知に基づいて、エラーを報知する報知手段を有することが好ましい。
これにより、作業者（使用者）は、手先の位置および姿勢の最適軌道が特定されないことを把握することができる。
本発明の軌道決定システムでは、前記手先の位置および姿勢の複数の軌道は、前記多関節ロボットの先端関節の初めの回転方向の異なる２つの軌道であることが好ましい。
これにより、軌道決定の処理効率が向上する。

本発明の軌道決定システムでは、前記多関節ロボットは、正逆両方向に回転可能な６つの関節を有する垂直６軸型ロボットであることが好ましい。
これにより、垂直６軸型ロボットを、容易に、最適かつ確実に作動させることができる。
本発明の軌道決定方法は、多関節ロボットの連続軌道追従動作時における手先の位置および姿勢の軌道を決定する軌道決定方法であって、
連続軌道追従動作の動作開始時と動作完了時とにおける、前記多関節ロボットにおける手先の位置および姿勢の情報に基づき、前記多関節ロボットの手先の位置および姿勢の軌道を複数生成し、
前記手先の位置および姿勢の軌道に則した連続軌道追従動作時の前記多関節ロボットにおける、先端関節の現回転角度を推定し、
前記連続軌道追従動作を行う多関節ロボットの動作開始時と動作完了時との間の複数の箇所において前記現回転角度の検証を行って、前記手先の位置および姿勢の複数の軌道のうち、所定判定基準により適合する最適軌道を特定することを特徴とする。
これにより、多関節ロボットの連続軌道追従動作時における手先の位置および姿勢の適正な軌道（最適な軌道）を、多関節ロボット動作前に、容易かつ安定的に決定することができる。これによって、多関節ロボットを、容易に、最適かつ確実に作動させることができる。

以下、本発明の軌道決定システムおよび軌道決定方法を添付図面に示す好適実施形態に基づいて詳細に説明する。
図３は、本発明の軌道決定システムの実施形態を含む全体構成図である。
本実施形態における多関節ロボット１０としては、正逆両方向に回転可能な６つの関節（関節部）を有する垂直６軸型ロボットを想定する（図１参照）。すなわち、多関節ロボット１０は、関節を有する６つのロボットアーム３０と、各ロボットアーム３０（関節）を駆動するための６つのモータ３１と、各ロボットアーム３０の位置を検出する６つのエンコーダ３２とを備えている。

この多関節ロボット１０の駆動は、軌道決定システム１００におけるコントロール部（制御手段）１０１からの指令によって前記モータ３１に駆動パワーを供給するドライブ部３３を介して制御される。また、前記エンコーダ３２によって検出、生成された前記モータ３１の回転等を表すパルスの数（パルス数）は、前記ドライブ部３３のカウンタ３４によってカウントされ、多関節ロボット１０の各ロボットアーム３０の位置情報として、前記コントロール部１０１で管理されることとなる。

軌道決定システム１００の主要部は、例えばパーソナルコンピュータやサーバコンピュータ等のコンピュータで構成されている。この軌道決定システム１００は、軌道決定方法を実行する機能を実現すべく、例えば不揮発性メモリ等の記憶手段で構成されるプログラムデータベース１０２に格納されたプログラム１０３を、演算装置たるＣＰＵ等を有する前記コントロール部１０１が読み出して実行する。

また、軌道決定システム１００は、多関節ロボット１０と通信してデータの授受等を実行するＩ／Ｏ部１０４と、軌道決定システム１００における各種の情報を表示（報知）する表示手段（報知手段）および各操作を行なう操作手段を兼ねる操作表示部１０５とを備える。なお、この操作表示部１０５により、後述するエラー通知部（エラー通知手段）１１３から出力されたエラー通知に基づいて、エラー（エラーである旨）を報知する報知手段が構成される。

また、前記プログラムデータベース１０２には、多関節ロボット１０のポーズ、すなわち姿勢や位置の情報を含むポーズデータ１０６と、多関節ロボット１０が実行する一連の動作を記述したロボットプログラム１０７と、前記コントロール部１０１での処理を規定する制御プログラム１０８とが格納されている。前記制御プログラム１０８は、多関節ロボット１０の軌道追従動作を規定する軌道追従プログラム１０９を含むものとする。

以下に、前記軌道決定システム１００が、例えばプログラム１０３のうち、前記軌道追従プログラム１０９に基づき構成・保持する各機能部について説明を行う。
軌道決定システム１００は、連続軌道追従動作の動作開始時と動作完了時とにおける、前記多関節ロボット１０における手先部１２の手先１２１の位置および姿勢の情報に基づき、多関節ロボット１０の手先１２１の位置および姿勢の軌道を複数（本実施形態では、先端関節１４の初めの回転方向の異なる２つ）生成する軌道生成部（軌道生成手段）１１０を有する。

また、軌道決定システム１００は、前記手先１２１の位置および姿勢の軌道に則した連続軌道追従動作時の多関節ロボット１０における、先端関節１４の現回転角度を推定する軌道推定部（軌道推定手段）１１１を有する。
また、軌道決定システム１００は、前記連続軌道追従動作を行う多関節ロボット１０の動作開始時と動作完了時との間の複数の箇所において、前記現回転角度の検証を行って、前記手先１２１の位置および姿勢の複数の軌道のうち、所定判定基準により適合する最適軌道を特定する軌道検証部（軌道検証手段）１１２を有する。

前記軌道検証部１１２は、前記連続軌道追従動作の動作開始時の前記先端関節１４の回転角度と前記連続軌道追従動作の動作完了時の前記先端関節１４の回転角度との間の回転角度範囲に、前記検証対象となった前記現回転角度が含まれる手先１２１の位置および姿勢の軌道を特定し、これを前記最適軌道とする処理（第１の処理）を行う。
また、前記軌道検証部１１２は、前記多関節ロボット１０の先端関節１４の回転開始角度と回転完了角度との間を線形補間する補間角度に、前記検証対象となった前記現回転角度がより近い手先１２１の位置および姿勢の軌道を特定し、これを前記最適軌道とする処理（第２の処理）を行う。

この場合、前記軌道検証部１１２は、前記第１の処理を前記第２の処理より優先して行うのが好ましい。すなわち、前記軌道検証部１１２は、前記連続軌道追従動作の動作開始時の前記先端関節１４の回転角度と前記連続軌道追従動作の動作完了時の前記先端関節１４の回転角度との間の回転角度範囲に、前記検証対象となった前記現回転角度が含まれる手先１２１の位置および姿勢の軌道を特定し、これを前記最適軌道とし、前記回転角度範囲に前記検証対象となった前記現回転角度が含まれる手先１２１の位置および姿勢の軌道が無い場合は、前記多関節ロボット１０の先端関節１４の回転開始角度と回転完了角度との間を線形補間する補間角度に、前記検証対象となった前記現回転角度がより近い手先１２１の位置および姿勢の軌道を特定し、これを前記最適軌道とする処理を行うのが好ましい。

また、前記軌道検証部１１２は、前記現回転角度の検証（複数回の検証）の結果、各検証箇所での前記手先１２１の位置および姿勢の軌道の特定結果が一致した場合にのみ、前記最適軌道を特定する処理を行うのが好ましい。
また、軌道決定システム１００は、前記現回転角度の検証（複数回の検証）の結果、各検証箇所での前記手先１２１の位置および姿勢の軌道の特定結果が一致しない場合、エラー通知を出力する、エラー通知部（エラー通知手段）１１３を備える。
また、軌道決定システム１００は、軌道に則した多関節ロボット１０の連続軌道追従動作を実行する動作実行部（動作実行手段）１１４を有する。

なお、これまで示した軌道決定システム１００における、ロボットプログラム１０７および制御プログラム１０８、例えば軌道追従プログラム１０９における各機能部である軌道生成部１１０、軌道推定部１１１、軌道検証部１１２、エラー通知部１１３、動作実行部１１４の主機能は、ハードウェアとして実現してもよいし、メモリなどの記憶手段に格納したプログラム（ソフトウェア）として実現してもよいし、これらを併用してもよい。プログラムの場合、前記コントロール部１０１がプログラムの実行に合わせて記憶手段より該当するプログラムを読み出して実行することとなる。

次に、本実施形態の軌道決定方法における処理手順について説明する。
図４は、軌道追従プログラム１０９に基づく処理手順を示すフローチャートである。なお、以下で説明する軌道決定方法に対応する各動作は、軌道決定システム１００における前述したプログラムによって実現される。
軌道決定システム１００は、まず、例えばティーチングなどにより記憶されている、多関節ロボット１０の手先１２１の動作開始位置とそのときの手先１２１の姿勢、動作完了位置となる手先１２１の目標位置１５とそのときの手先１２１の姿勢（目標姿勢）の情報を取得する（ｓ１０００）。

このように、手先１２１の動作開始位置および姿勢、目標位置１５および姿勢等の情報が与えられると、前記軌道決定システム１００は、前記軌道生成部１１０において、前記目標位置１５での先端関節１４（手先１２１）の目標回転角度を計算すると共に（ｓ１００１）、前記取得している手先１２１の位置および姿勢の情報に基づき、多関節ロボット１０の手先１２１の位置および姿勢の軌道を２つ生成する（ｓ１００２）。すなわち、手先１２１の位置および姿勢の軌道に関する、先端関節１４（手先座標系のＺ軸回り）の初めの回転方向１３は未決定であり、前記手先１２１の位置および姿勢の軌道は、この回転方向別（図１で示した回転方向ａおよびｂ）に２通りある状態であるが、その他の必要なパラメータが求められている状態となる。なお、こうした軌道生成の手法については、従来手法を用いることとできる（参考書籍の例：ロボット制御基礎論、1988/11/25、コロナ社）。

次に、軌道決定システム１００の軌道検証部１１２は、実際に多関節ロボット１０の連続軌道追従動作を開始するに先立ち、第１中間点（第１検証箇所）、例えば動作開始から目標位置１５（動作完了時）まで５０％（時間、距離、角度のいずれかに関するもの）移動する位置おいて、先端関節１４の初めの回転方向１３を決定するために手先１２１の位置および姿勢の軌道の検証を行う（ｓ１００３）。更に軌道検証部１１２は、第２中間点（第２検証箇所）、例えば前記目標位置１５まで７５％移動する位置において、同様に、手先１２１の位置および姿勢の軌道の検証を行う（ｓ１００４）。

なお、前記検証を実施する位置およびその数は、前記の例に限定されず、軌道途中の任意の複数点で行うことができるものとする。また、前記検証にあたっては、手先１２１の位置および姿勢の軌道に則した連続軌道追従動作時の多関節ロボット１０における、先端関節１４の現回転角度を軌道推定部１１１が推定しているものとする。
また、前記軌道検証部１１２は、前記第１中間点および第２中間点の２箇所において先端関節１４の現回転角度の検証を行って前記手先１２１の位置および姿勢の軌道のうち、所定判定基準により適合する最適軌道を特定するものとする。この特定処理については、後述する前記ステップｓ１００３、ｓ１００４におけるサブルーチンとともに説明する。

次に、多関節ロボット１０の実際の動作前に、前記第１中間点および第２中間点における検証の結果（全ての検証箇所での検証の結果）、第１中間点と第２中間点での前記手先１２１の位置および姿勢の軌道の特定結果（軌道の候補）が同じか否か（一致するか否か）を判定する（ｓ１００５）。
前記第１中間点では、回転方向ａの軌道が特定されたにもかかわらず、第２中間点では、回転方向ｂの軌道が特定された場合や、逆に、前記第１中間点では、回転方向ｂの軌道が特定されたにもかかわらず、第２中間点では、回転方向ａの軌道が特定された場合（ｓ１００５：ＮＯ）、中間点毎に異なる軌道を採用することはできない訳であるから、軌道決定システム１００は、多関節ロボット１０の動作不能と判断し、最適軌道を特定せずに、エラー通知部１１３を通じてエラー通知を出力し（ｓ１００６）、処理を終了する。これにより、操作表示部１０５において、エラーが表示され、この場合は、多関節ロボット１０の実際の動作は実行されない。前記エラー表示により、作業者（使用者）は、手先の位置および姿勢の最適軌道が特定されず、多関節ロボット１０が作動しないことを把握することができる。

一方、第１中間点と第２中間点での前記手先１２１の位置および姿勢の軌道の特定結果（軌道の候補）が同じ（一致する）場合（ｓ１００５：ＹＥＳ）、前記動作実行部１１４を通じて多関節ロボット１０の動作を実行し（ｓ１００７）、処理を終了する。
次に、手先軌道の特定処理について、図５のフローチャートに基づいて説明する。
図５は、本実施形態における軌道特定の処理手順等を示すフローチャートであって、図４のフローチャート中のステップｓ１００３、ｓ１００４に対応するサブルーチンである。また、図６は、本実施形態における軌道特定の思想を示す説明図であり、横軸を多関節ロボット１０の動作開始から動作完了までの時間、縦軸を先端関節１４の回転角度としている。

軌道決定システム１００は、前記ステップｓ１００３、ｓ１００４のそれぞれにおいて、上述の通り生成した２通りの軌道（先端関節１４の初めの回転方向１３が回転方向ａに対応したものを第１軌道６０、回転方向ｂに対応したものを第２軌道６１とする）のうち、最適な軌道を特定する処理（軌道検証処理）を行う。なお、ステップｓ１００３の第１中間点における処理と、ステップｓ１００４の第２中間点における処理とは、同じであるので、以下の説明では、ステップｓ１００３の第１中間点における処理を代表的に説明する。

まず、前記軌道推定部１１１は、第１中間点での第１軌道６０における先端関節１４の回転角度を予測計算し、その値を前記現回転角度として推定し（ｓ１１００）、第１中間点での第２軌道６１における先端関節１４の回転角度を予測計算し、その値を前記現回転角度として推定する（ｓ１１０１）。
図６に示す例では、前記軌道推定部１１１により、第１軌道６０における第１中間点での先端関節１４の現回転角度６２、第２中間点での先端関節１４の現回転角度６６、第２軌道６１における第１中間点での先端関節１４の現回転角度６３、第２中間点での先端関節１４の現回転角度６７の４つの現回転角度が推定される。

次に、前記軌道検証部１１２は、連続軌道追従動作の動作開始時の先端関節１４の回転角度と連続軌道追従動作の動作完了時の先端関節１４の回転角度との間の回転角度範囲８０に、第１軌道６０における第１中間点での先端関節１４の現回転角度６２が含まれるか否かを判定する（ｓ１１０２）。
回転角度範囲８０に、第１軌道６０における第１中間点での現回転角度６２が含まれる場合（ｓ１１０２：ＹＥＳ）、回転角度範囲８０に、第２軌道６１における第１中間点での先端関節１４の現回転角度６３が含まれるか否かを判定する（ｓ１１０３）。

回転角度範囲８０に、第２軌道６１における第１中間点での現回転角度６３が含まれない場合（ｓ１１０３：ＮＯ）、第１軌道６０を最適軌道として特定し、これを記憶手段に記憶して（ｓ１１０４）、処理を終了する。
一方、前記ステップｓ１１０２において、回転角度範囲８０に、第１軌道６０における第１中間点での現回転角度６２が含まれない場合（ｓ１１０２：ＮＯ）、回転角度範囲８０に、第２軌道６１における第１中間点での現回転角度６３が含まれるか否かを判定する（ｓ１１０５）。

ここで回転角度範囲８０に、第２軌道６１における第１中間点での現回転角度６３が含まれる場合（ｓ１１０５：ＹＥＳ）、第２軌道６１を最適軌道として特定し、これを記憶手段に記憶して（ｓ１１０６）、処理を終了する。
また、回転角度範囲８０に、第１軌道６０における第１中間点での現回転角度６２と、第２軌道６１における第１中間点での現回転角度６３とがどちらも含まれない場合（ｓ１１０５：ＮＯ）、または、現回転角度６２および現回転角度６３がどちらも含まれる場合（ｓ１１０３：ＹＥＳ）がある。

この場合、前記軌道検証部１１２は、前記多関節ロボット１０の先端関節１４の回転開始角度（図６の例では０°）と目標回転角度（回転完了角度）との間を線形補間する補間角度７０に、前記現回転角度６２、６３がより近い手先１２１の位置および姿勢の軌道を特定することとなる。
すなわち、前記軌道検証部１１２は、第１軌道６０における第１中間点での現回転角度６２が第２軌道６０における第１中間点での現回転角度６３より第１中間点での補間角度７０に近いか否かを判定する（ｓ１１０７）。そして、現回転角度６２が現回転角度６３より第１中間点での補間角度７０に近い場合（ｓ１１０７：ＹＥＳ）、第１軌道６０を最適軌道として特定し、これを記憶手段に記憶して（ｓ１１０４）、処理を終了する。一方、現回転角度６３が現回転角度６２より第１中間点での補間角度７０に近い場合（ｓ１１０７：ＮＯ）、第２軌道６１を最適軌道として特定し、これを記憶手段に記憶して（ｓ１１０６）、処理を終了する。

図６の例では、第１軌道６０における第１中間点での現回転角度６２および第２軌道６１における第１中間点での現回転角度６３の両方が前記回転角度範囲８０から外れている。しかしながら、現回転角度６２と第１中間点での補間角度７０との差異６４は、現回転角度６３と第１中間点での補間角度７０との差異６５より小さい。従って、第１中間点においては第１軌道６０が最適軌道として特定される。

また、第１軌道６０における第２中間点での現回転角度６６は、前記回転角度範囲８０に含まれていると共に、第２中間点での補間角度７０との差異がない。一方、第２軌道６１における第２中間点での現回転角度６７は、前記回転角度範囲８０から大きく外れている。さらに、現回転角度６６と第２中間点での補間角度７０との差異は、現回転角度６７と第２中間点での補間角度７０との差異６８より小さい。従って、第２中間点においても第１軌道６０が最適軌道として特定される。
すなわち、図６の例では、全ての検証箇所（第１中間点および第２中間点）において、第１軌道が最適軌道として特定される。

以上説明したように、この軌道決定システム１００および軌道決定方法によれば、多関節ロボット１０の連続軌道追従動作時における手先１２１の位置および姿勢の適正な軌道（先端関節１４の初めの回転方向）を、多関節ロボット１０の動作前に、容易かつ安定的に決定することができる。これによって、多関節ロボット１０を、容易に、最適かつ確実に作動させることができる。

なお、本発明では、例えば、図５のフローチャートにおいて、ステップｓ１１０７の判定を省略し、ステップｓ１１０２、ｓ１１０３、ｓ１１０５の判定で、各中間点（各検証箇所）において最適軌道を特定するよう構成してもよい。
また、図５のフローチャートにおいて、ステップｓ１１０２、ｓ１１０３、ｓ１１０５の判定を省略し、ステップｓ１１０７の判定で、各中間点（各検証箇所）において最適軌道を特定するよう構成してもよい。

以上、本発明の軌道決定システムおよび軌道決定方法を、図示の実施形態に基づいて説明したが、本発明はこれに限定されるものではなく、各部の構成は、同様の機能を有する任意の構成のものに置換することができる。また、本発明に、他の任意の構成物や、工程が付加されていてもよい。
なお、本発明では、多関節ロボットの手先の位置の軌道の形状（パターン）は、特に限定されず、前述した直線（直線の軌道）の他、例えば、曲線（曲線の軌道）等が挙げられる。

多関節ロボットが直線の軌道上において手先部の手先の位置および姿勢を変化させながら動作する様子を表す簡略図。 先端関節の回転角度の時間推移を回転方向別に示した図。 本発明の軌道決定システムの実施形態を含む全体構成図。 本実施形態における軌道追従プログラムに基づく処理手順を示すフローチャート。 本実施形態における軌道特定の処理手順等を示すフローチャート。 本実施形態における軌道特定の思想を示す説明図。

符号の説明

１０……多関節ロボット １１……軌道 １２……手先部 １２１……手先 １３……回転方向 １４……先端関節 １５……目標位置 ２０……正しい軌道 ２１……回転方向 ２２……回転方向 ２３……誤った軌道 ３０……ロボットアーム ３１……モータ ３２……エンコーダ ３３……ドライブ部 ３４……カウンタ ６０……第１軌道 ６１……第２軌道 ６２、６３、６６、６７……現回転角度 ６４、６５……差異 ７０……補間角度 ８０……回転角度範囲 １００……軌道決定システム １０１……コントロール部 １０２……プログラムデータベース １０３……プログラム １０４……Ｉ／Ｏ部 １０５……操作表示部 １０６……ポーズデータ １０７……ロボットプログラム １０８……制御プログラム １０９……軌道追従プログラム １１０……軌道生成部 １１１……軌道推定部 １１２……軌道検証部 １１３……エラー通知部 １１４……動作実行部 ｓ１０００〜ｓ１００７……ステップ ｓ１１００〜ｓ１１０７……ステップ

Claims (10)

1. 多関節ロボットの連続軌道追従動作時における手先の位置および姿勢の軌道を決定する軌道決定システムであって、
   連続軌道追従動作の動作開始時と動作完了時とにおける、前記多関節ロボットにおける手先の位置および姿勢の情報に基づき、前記多関節ロボットの手先の位置および姿勢の軌道を複数生成する軌道生成手段と、
   前記手先の位置および姿勢の軌道に則した連続軌道追従動作時の前記多関節ロボットにおける、先端関節の現回転角度を推定する軌道推定手段と、
   前記連続軌道追従動作を行う多関節ロボットの動作開始時と動作完了時との間の複数の箇所において前記現回転角度の検証を行って、前記手先の位置および姿勢の複数の軌道のうち、所定判定基準により適合する最適軌道を特定する軌道検証手段とを備えることを特徴とする軌道決定システム。
2. 前記軌道検証手段は、前記連続軌道追従動作の動作開始時の前記先端関節の回転角度と前記連続軌道追従動作の動作完了時の前記先端関節の回転角度との間の回転角度範囲に、前記検証対象となった前記現回転角度が含まれる手先の位置および姿勢の軌道を特定し、これを前記最適軌道とする請求項１に記載の軌道決定システム。
3. 前記軌道検証手段は、前記多関節ロボットの先端関節の回転開始角度と回転完了角度との間を線形補間する補間角度に、前記検証対象となった前記現回転角度がより近い手先の位置および姿勢の軌道を特定し、これを前記最適軌道とする請求項１に記載の軌道決定システム。
4. 前記軌道検証手段は、前記連続軌道追従動作の動作開始時の前記先端関節の回転角度と前記連続軌道追従動作の動作完了時の前記先端関節の回転角度との間の回転角度範囲に、前記検証対象となった前記現回転角度が含まれる手先の位置および姿勢の軌道を特定し、これを前記最適軌道とし、前記回転角度範囲に前記検証対象となった前記現回転角度が含まれる手先の位置および姿勢の軌道が無い場合は、前記多関節ロボットの先端関節の回転開始角度と回転完了角度との間を線形補間する補間角度に、前記検証対象となった前記現回転角度がより近い手先の位置および姿勢の軌道を特定し、これを前記最適軌道とする請求項１に記載の軌道決定システム。
5. 前記軌道検証手段は、前記現回転角度の検証の結果、各検証箇所での前記手先の位置および姿勢の軌道の特定結果が一致した場合にのみ、前記最適軌道を特定する請求項１ないし４のいずれかに記載の軌道決定システム。
6. 前記現回転角度の検証の結果、各検証箇所での前記手先の位置および姿勢の軌道の特定結果が一致しない場合、エラー通知を出力する、エラー通知手段を備える請求項１ないし５のいずれかに記載の軌道決定システム。
7. 前記エラー通知に基づいて、エラーを報知する報知手段を有する請求項６に記載の軌道決定システム。
8. 前記手先の位置および姿勢の複数の軌道は、前記多関節ロボットの先端関節の初めの回転方向の異なる２つの軌道である請求項１ないし７のいずれかに記載の軌道決定システム。
9. 前記多関節ロボットは、正逆両方向に回転可能な６つの関節を有する垂直６軸型ロボットである請求項１ないし８のいずれかに記載の軌道決定システム。
10. 多関節ロボットの連続軌道追従動作時における手先の位置および姿勢の軌道を決定する軌道決定方法であって、
    連続軌道追従動作の動作開始時と動作完了時とにおける、前記多関節ロボットにおける手先の位置および姿勢の情報に基づき、前記多関節ロボットの手先の位置および姿勢の軌道を複数生成し、
    前記手先の位置および姿勢の軌道に則した連続軌道追従動作時の前記多関節ロボットにおける、先端関節の現回転角度を推定し、
    前記連続軌道追従動作を行う多関節ロボットの動作開始時と動作完了時との間の複数の箇所において前記現回転角度の検証を行って、前記手先の位置および姿勢の複数の軌道のうち、所定判定基準により適合する最適軌道を特定することを特徴とする軌道決定方法。
    

Images