JP6860863B2 - 経路出力方法 - Google Patents

Description

本発明は、経路出力方法、経路出力システム及び経路出力プログラムに関する。

今日の産業界では、多数のロボットが使用されている。ロボットは、実際に作業を行う前に、その動作をプログラミングや教示により学習する必要がある。例えば、特許文献１には、バラ積みされた物品を、ロボットハンドが取り付けられたロボットアームを用いて容器から取り出す方法が開示されている。特許文献１に記載の方法は、作業者が、産業用ロボットが物品を取り出す動作経路上の点であって、ロボットアームがとり得る姿勢を定義する複数の教示点を予め指定し、複数の教示点からなる教示点列を示す基準動作プログラムを作成するものである。ここで、ロボットの動作経路上の複数の教示点を、予め設定する作業等、ロボットに動作を学習させる作業は、大きな労力を必要とすることがある。このため、ロボットに移動開始位置及び目的位置を与え、移動開始位置から目的位置までの経路を自動的に生成するモーションプランニング技術が開発されている。例えば、特許文献２には、ＲＲＴ（Rapidly-Exploring Random Tree）とロードマップ法を組み合わせて使用することにより移動経路を探索する手法が開示されている。

特開２０１３−１２６７０６号公報 特開２０１４−７３５５０号公報 特開２００６−１５５５５９号公報

ロボットの動作経路を示す教示点列を指定し、指定された教示点列に基づいて生成される動作プログラムによってロボットを動作させる場合には、作業者は、教示点を指定するときに、ロボットアームの姿勢をその都度考慮する。そのため、ロボットが保持する対象物の姿勢は、作業者が考慮した教示点におけるロボットアームの姿勢により定めることができる。しかしながら、移動開始位置（始点）から目的位置（終点）までの動作経路を自動的に生成するモーションプランニングでは、ロボットが移動開始位置（初期姿勢）から目的位置（目標姿勢）まで動作する間にロボットが把持する対象物の姿勢は任意の向きを取りうる。そのため、モーションプランニングで自動生成された経路を用いると、例えば、液体の入った容器を搬送する間に液体をこぼしてしまう等の悪影響を及ぼすことがあり、実際には、自動生成された経路をそのまま用いることができない。この点で、上記特許文献１及び特許文献２のいずれも、ロボットの動作経路を自動的に生成するときの対象物の姿勢については考慮されていない。

一方、特許文献３は、ロボットの姿勢を考慮に入れた上で、ロボットの動作経路を自動的に生成する方法について言及している。この方法は、ロボットを移動開始位置から目的位置まで移動させる動作経路を生成するにあたり、移動開始位置と目的位置との間に中間点を確率的に生成している。そして、この方法は、ロボットが存在する実空間におけるロボットの取り得る姿勢を制約するための拘束条件を、ロボットの自由度に基づいて定義されるコンフィギュレーション空間の中に設定し、拘束条件に基づいて拘束中間点を生成し、拘束中間点を用いて移動開始位置から目的位置までの間の経路を生成している。

このとき、拘束条件を満たす中間点を確率的に生成することは困難であるため、特許文献３に記載の方法は、拘束条件を満たす拘束部分空間をコンフィギュレーション空間の中に設定し、確率的に生成した中間点を拘束部分空間に射影して拘束中間点を生成している。しかし、このような射影計算を用いたとしても、拘束条件を満たす中間点を確率的に生成することが困難であることに変わりはない。このため、射影計算が高頻度に行われることとなり、効率の良い計算方法とは言い難い。更に、コンフィギュレーション空間内に確率的に生成された中間点ｑ１，ｑ２をその拘束部分空間ｖ１，ｖ２へと写像した場合、ｖ１とｖ２との論理積が空集合となる場合がある。このような現象は、特に、冗長軸を有しないロボットにおいて生じ易い。このような場合、中間点ｑ１，ｑ２の間を接続する経路は、拘束条件を満たさないことから、中間点の再生成や修正を行う必要が生じることとなり、効率の良い計算方法とは言い難い。

また、特許文献３に記載の方法は、ロボットの自由度に基づいて定義されるコンフィギュレーション空間において、拘束条件を満たす任意の位置とその近傍とを満たす拘束楕円体を設定することにより、経路計算の簡略化を図っている。この計算方法は、射影計算に関わる上述の問題を解決する方法となり得るが、拘束楕円体は、ロボットの姿勢に関する制約しか与えていない。仮に、拘束楕円体上の点のなす空間と、コンフィギュレーション空間上の障害物空間との論理積が空集合でない場合、拘束楕円体に基づいて生成された中間点は、障害物と干渉することがある。ロボットが障害物に囲まれている場合には、このような干渉が生じる確率が高いため、中間点の再生成や修正を行う必要が生じることとなり、効率の良い計算方法とは言い難い。

一般的に、多関節ロボットにおいて、障害物空間をコンフィギュレーション空間上の拘束空間として表現しようとすると、計算量が多くなるため、コンフィギュレーション空間上で拘束楕円体と障害物空間の双方の空間を定義して効率よく経路計算を行うのは困難である。

そこで、本発明は、上述の問題を解決し、ロボットが対象物を搬送する間、対象物の姿勢を保持することができる経路出力方法、経路出力システム及び経路出力プログラムを提供することを目的とする。

本発明の一態様に係る経路出力方法は、取得部、出力部、記憶部及び演算部を備える演算装置を用いて、複数の可動軸を有するロボットが、対象物を第１の位置から第２の位置に搬送する際の経路を出力する方法であって、演算部が、取得部を用いて第１の位置及び第２の位置を取得するステップと、ロボットが対象物を第１の位置から第２の位置に搬送するときの搬送条件を取得するステップと、ロボットが対象物を第１の位置から第２の位置に搬送する経路であって、当該経路の全体にわたり、対象物の姿勢が搬送条件を満たす経路を生成するステップと、出力部を用いて生成された経路を出力するステップと、を実行することを含む。ここで、搬送条件は対象物の姿勢に関する第１の条件を含む。

この態様によれば、ロボットが対象物を第１の位置から第２の位置に搬送する経路の全体にわたり、対象物の姿勢に関する条件を満たす経路を生成する。このため、ロボットが対象物を搬送する間、対象物の姿勢を一定の範囲内で保持する経路出力方法を提供することができる。すなわち、ロボットが第１の位置から第２の位置まで動作する経路を自動的に生成するモーションプランニングでは、対象物が任意の姿勢を取り得る動作経路が生成される場合があるが、生成される動作経路の全体にわたり、対象物の姿勢に関する条件を満たす経路が生成されるので、ロボットが対象物を搬送する間、対象物の姿勢を一定の範囲内で保持することができる。なお、「搬送する」とは、複数の可動軸を有するロボットが、対象物を第１の位置から第２の位置まで移動させることをいう。

上記態様において、記憶部には第１の条件の初期値が格納されていて、演算部は、搬送条件を取得するステップにおいて、記憶部に格納された初期値を第１の条件として取得してもよい。この態様によれば、対象物の姿勢に関する第１の条件を都度入力しなくても、予め設定された条件を満たす経路が生成される。

上記態様において、演算部は、搬送条件を取得するステップにおいて、取得部を介してユーザが任意に設定した第１の条件を取得してもよい。この態様によれば、ユーザの要望に応じた経路が生成される。

上記態様において、演算部は、搬送条件を取得するステップにおいて、取得部を介して取得された、ユーザが任意に設定した条件を、初期値又は上書値として記憶部に格納してもよい。この態様によれば、条件を現状のニーズに即した形に逐次変更して記憶しておくことで、現在の作業環境に適した条件により経路を生成することができる。

上記態様において、演算部は、経路を生成するステップにおいて、第１の条件を満たす第３の位置を探索し、対象物を第１の位置から第３の位置を経由して第２の位置に搬送する経路を生成してもよい。この態様によれば、対象物の姿勢が所定の条件を満たす経路を生成することができる。また、点を対象として条件を満たすか否かの判定をすることになるので、経路が生成できるか否かの探索処理を、所定の範囲に限ることができ、経路の生成における演算負荷を削減することができる。さらに、ロボットが経由すべき点をユーザが設定する場合には、当該点を経由する経路の生成を容易に行うことができる。

上記態様において、記憶部には搬送条件を満たすか否かを判定する所定の関数が格納されており、演算部は、経路を生成するステップにおいて、所定の関数に基づいて、搬送条件を満たす経路を生成してもよい。この態様によれば、対象物の姿勢が所定の条件を満たさない経路でロボットを動作させることを防ぐことができる。

上記態様において、演算部は、第１の位置と第２の位置とを取得するステップにおいて、取得部を用いて、所定のセンサにより取得される情報に基づいて算出された、第１の位置と第２の位置との少なくともいずれかを取得してもよい。この態様によれば、対象物の実際の姿勢を第１の位置又は第２の位置とし、第１の位置から第２の位置までのより正確な経路を生成することができる。

上記態様において、演算部は、第１の位置と第２の位置とを取得するステップにおいて、取得部を用いて、所定のセンサにより取得される情報を取得し、取得した情報に基づいて、第１の位置と第２の位置との少なくともいずれかを算出してもよい。この態様によれば、対象物の実際の姿勢に基づいて、第１の位置から第２の位置までのより正確な経路を生成することができる。

上記態様において、所定のセンサにより取得される情報は、撮像装置により取得される対象物の画像情報、又は、ロボットが有する複数の可動軸の角度の情報であってもよい。この態様によれば、撮像装置やロボットが有するセンサ等から取得された情報を用いて、第１の位置から第２の位置までのより正確な経路を生成することができる。

上記態様において、搬送条件は、ロボットの周辺環境情報関する第２の条件をさらに含み、演算部は、経路を生成するステップにおいて、第１の条件と第２の条件とを満たす経路を生成してもよい。この態様によれば、ロボットの周辺環境情報を考慮した経路を生成することができる。

本発明の一態様に係る経路出力方法は、出力部及び演算部を備える演算装置を用いて、複数の可動軸を有するロボットが対象物を搬送する際の経路を出力する方法であって、演算部が、対象物が存在する実空間内における対象物の取り得る姿勢を制約するための第１の条件を満たす第１の位置及び第２の位置を計算するステップと、第１の位置と第２の位置との間を接続する一つ以上の中間点であって、第１の条件に加えて、ロボットの周辺環境情報に関する第２の条件を満たす一つ以上の中間点を実空間内に計算するステップと、第１の位置から一つ以上の中間点を経由して第２の位置に至る経路を生成するステップと、出力部を用いて、生成された経路を出力するステップと、を実行することを含む。

コンフィギュレーション空間内に中間点を確率的に生成すると、実空間の第１の条件を満たさない中間点が生成される可能性が高いという不都合が生じるが、この態様によれば、第１の条件を満たす中間点を実空間内で生成することにより、そのような不都合を解消できる。

上記態様において、演算部は、第１の位置及び第２の位置を計算するステップにおいて、第１の位置と第２の位置との間に第１の条件を満たす経路が存在する第１の位置及び第２の位置を計算してもよい。この態様によれば、第１の位置及び第２の位置が第１の条件を満たしていても、第１の位置と第２の位置との間の如何なる経路も第１の条件を満たさないことがあるという不都合を解消できる。

上記態様において、中間点は、第１の位置から第２の位置へ至る経路上の対象物又はロボットの手先の中間位置及び中間姿勢に関わる情報を含んでもよい。演算部は、一つ以上の中間点を実空間内に計算するステップにおいて、ある一つの中間点の中間姿勢に関わる情報を変更することなく、その中間位置に関わる情報を変更することにより、第１の条件を満たすもう一つの中間点を実空間内に計算してもよい。これにより、第１の条件を満たす中間点を簡易な計算により求めることができる。

上記態様において、演算部は、一つ以上の中間点を実空間内に計算するステップにおいて、もう一つの中間点の中間位置が第２の位置へ向かうように、ある一つの中間点の中間位置に関わる情報を変更してもよい。これにより、ロボットの効率的な経路計画を実現することができる。

本発明の一態様に係る経路出力システムは、複数の可動軸を有するロボットが、対象物を第１の位置から第２の位置に搬送する際の経路を出力するシステムであって、第１の位置及び第２の位置と、ロボットが対象物を第１の位置から第２の位置に搬送するときの搬送条件を取得する取得部と、ロボットが対象物を第１の位置から第２の位置に搬送する経路であって、当該経路の全体にわたり、対象物の姿勢が搬送条件を満たす経路を生成する演算部と、生成された経路を外部に出力する出力部と、を含む。ここで、搬送条件は対象物の姿勢に関する第１の条件を含む。

この態様によれば、ロボットが対象物を第１の位置から第２の位置に搬送する経路の全体にわたり、対象物の姿勢に関する条件を満たす経路を生成する。このため、ロボットが対象物を搬送する間、対象物の姿勢を一定の範囲内で保持する経路出力システムを提供することができる。

本発明の一態様に係る経路出力プログラムは、複数の可動軸を有するロボットが、対象物を第１の位置から第２の位置に搬送する際の経路を生成するためのプログラムであって、取得部及び演算部を備える演算装置に、第１の位置及び前記第２の位置を取得するステップと、ロボットが対象物を第１の位置から第２の位置に搬送するときの搬送条件を取得するステップと、ロボットが対象物を第１の位置から第２の位置に搬送する経路であって、当該経路の全体にわたり、対象物の姿勢が搬送条件を満たす経路を生成するステップと、生成された経路を出力するステップと、を実行させることを含む。ここで、搬送条件は対象物の姿勢に関する第１の条件を含む。

この態様によれば、ロボットが対象物を第１の位置から第２の位置に搬送する経路の全体にわたり、対象物の姿勢に関する条件を満たす経路を生成する。このため、ロボットが対象物を搬送する間、対象物の姿勢を一定の範囲内で保持する経路出力プログラムを提供することができる。

本発明によれば、ロボットが対象物を搬送する間、対象物の姿勢を保持することができる経路出力方法、経路出力システム及び経路出力プログラムを提供することができる。

図１は、第１実施形態に係るロボットシステム１の構成の一例を示す図である。 図２は、演算装置１０のハードウェア構成の一例を示すブロック図である。 図３は、演算装置１０における、経路出力処理のフローチャートの一例である。 図４は、ＲＲＴに基づく経路生成処理のフローチャートの一例である。 図５は、ＲＲＴに基づく経路生成処理におけるコンフィギュレーション空間の一例を示す図である。 図６は、ＰＲＭに基づく経路生成処理のフローチャートの一例である。 図７は、ＰＲＭに基づく経路生成処理におけるコンフィギュレーション空間の一例を示す図である。 図８は、ポテンシャル法に基づく経路生成処理のフローチャートの一例である。 図９は、ポテンシャル法に基づく経路生成処理におけるコンフィギュレーション空間の一例を示す図である。 図１０は、軌道最適化に基づく経路生成処理のフローチャートである。 図１１は、軌道最適化に基づく経路生成処理におけるコンフィギュレーション空間の一例を示す図である。 図１２は、搬送条件に基づくコンフィギュレーション空間における、経路生成処理の説明図である。 図１３は、搬送条件に基づく実空間における、経路生成処理の説明図である。 図１４は、中間点の生成処理のフローチャートである。 図１５は、ポテンシャル法を用いたバイアス処理の説明図である。 図１６は、空間分割を用いたバイアス処理の説明図である。

添付図面を参照して、本発明の好適な実施形態について説明する。なお、各図において、同一の符号を付したものは、同一又は同様の構成を有する。また、以下の実施の形態は、本発明を説明するための例示であり、本発明をその実施の形態のみに限定する趣旨ではない。さらに、本発明は、その要旨を逸脱しない限り、さまざまな変形が可能である。

［第１実施形態］
まず、図１及び図２を参照しながら、本発明の第１実施形態に係るロボットシステムについて説明する。図１は、本実施形態に係るロボットシステム１の構成の一例を示す図である。同図に示すように、ロボットシステム１は、演算装置１０とロボット２０とを備える。また、ロボットシステム１は、図１や図２に示すように、撮像装置３０、角度検出装置４０、入力装置５０、出力装置６０等を備えてもよい。

演算装置１０は、初期姿勢と目標姿勢に基づいて、ロボット２０が初期姿勢から目標姿勢に移動する経路を演算する装置である。本実施形態において、演算された経路は、ロボット２０に対して出力される。ここでは、演算装置１０をロボット２０と別体として構成する例を示すが、これに限られず、演算装置１０とロボット２０が一体化された構成としてもよい。なお、演算結果としての経路は、必ずしも実在のロボット２０に対して出力されなくてもよく、例えば、シミュレータに対して出力されるものとしてもよい。一例として、後述する出力装置６０に対して、ロボット２０の仮想的なモデルと、演算結果としての経路とを出力し、当該出力装置６０でロボット２０の動作を再現するようにしてもよい。

なお、演算装置１０は、後述する、経路の生成及び出力に関する演算処理を実行する演算部、当該演算処理を実行させるための情報（プログラム、条件等を含む）を記憶する揮発性及び／又は不揮発性の記憶装置（記憶部、メモリ等を含む）、当該情報を取得するとともに演算処理の結果を出力するためのインタフェース、を少なくとも備えていればよい。すなわち、演算装置１０は、上述の各演算処理を実行する専用又は汎用の情報処理装置であれば足りる。また、ロボットシステム１は、演算装置１０に相当する構成を含むものであるから、上述の各演算処理を実行する構成を備える専用又は汎用の情報処理システムであれば足りる。

ロボット２０は、例えば、六軸垂直多関節ロボットである。ロボット２０は、図１の例に示すように、台座２１と、第１リンク２２と、第２リンク２３と、第３リンク２４と、第４リンク２５と、第５リンク２６と、第６リンク２７と、エンドエフェクタ２８とを備える。ロボット２０は、それぞれのリンクを回転運動させる可動軸（駆動用モータ）を備えている。これにより、ロボット２０は、台座２１及び複数のリンクが、駆動用モータを介して互いに接続されて構成されている。

台座２１は、ロボット２０が設置される構造物に取り付けられる。台座２１は、上面と垂直な可動軸により第１リンク２２と接続されている。第１リンク２２は、台座２１に対し、この可動軸を中心として回転することができる。

第１リンク２２は、台座２１の側の一端が互いに接続されている二枚の板状部材である。第１リンク２２の二枚の板状部材は、互いに接続されている。第１リンク２２の二枚の板状部材は、台座２１と反対の方向へ延びており、互いに対向している。第１リンク２２は、二枚の板状部材により第２リンク２３の一端を挟んでいる。第１リンク２２は、二枚の板状部材の互いに対向している面と垂直な可動軸により第２リンク２３と接続されている。第２リンク２３は、第１リンク２２に対し、この可動軸を中心として回転することができる。

第２リンク２３は、第１リンク２２の側の一端が互いに接続されている二枚の板状部材である。第２リンク２３の二枚の板状部材は、互いに対向している。第２リンク２３は、二枚の板状部材により第３リンク２４を挟んでいる。第２リンク２３は、二枚の板状部材の互いに対向している面と垂直な可動軸により第３リンク２４と接続されている。第３リンク２４は、第２リンク２３に対し、この可動軸を中心として回転することができる。

第３リンク２４は、第２リンク２３の二枚の板状部材の互いに対向している面と平行な面内に位置する可動軸により第４リンク２５と接続されている部材である。第４リンク２５は、第３リンク２４に対し、この可動軸を中心として回転することができる。

第４リンク２５は、第３リンク２４の側の一端が互いに接続されている二枚の板状部材である。第４リンク２５の二枚の板状部材は、互いに対向している。第４リンク２５は、二枚の板状部材により第５リンク２６を挟んでいる。第４リンク２５は、二枚の板状部材の互いに対向している面と垂直な可動軸により第５リンク２６と接続されている。第５リンク２６は、第４リンク２５に対し、この可動軸を中心として回転することができる。

第５リンク２６は、第４リンク２５の二枚の板状部材の互いに対向している面と垂直な可動軸により第６リンク２７と接続される部材である。第６リンク２７は、可動軸を介して第５リンク２６と接続される部材である。本実施形態では、第５リンク２６と第６リンク２７は、円筒状に構成されている。また、第５リンク２６と第６リンク２７とは、共通する中心軸を有し、第６リンク２７は第５リンク２６に対し、可動軸を中心として回転する。第６リンク２７は、第５リンク２６が接続される端部とは反対側の端部に、エンドエフェクタ２８を直接的、または、接続部材（アタッチメント）を介して間接的に取りつけるための接続構造を有し、当該接続構造を介してエンドエフェクタ２８と接続される部材である。

図１の例に示すように、エンドエフェクタ２８は、対象物Ｗを把持する機構である。この機構は、対象物Ｗを把持することができればよく、特に限定されない。また、エンドエフェクタ２８は、対象物Ｗを把持する代わりに、対象物Ｗを吸着する機構でもよい。エンドエフェクタ２８は、第４リンク２５の二枚の板状部材の互いに対向している面と平行な面内に位置する可動軸により第５リンク２６と接続されている。エンドエフェクタ２８は、第５リンク２６に対し、この可動軸を中心として回転することができる。なお、エンドエフェクタ２８は、対象物を把持する機構に限定されない。例えば、エンドエフェクタ２８として、対象物Ｗを吸着して保持する吸着パッドを用いてもよい。また、エンドエフェクタ２８として、対象物Ｗを載置する平面を有する平板状部材を用いてもよい。

エンドエフェクタ２８は、対象物Ｗを吊り下げた状態で把持してもよい。これにより、ロボット２０は、対象物Ｗを初期位置から目標位置に搬送する間、対象物Ｗの姿勢を保持することができる。

なお、エンドエフェクタ２８は、後述する記憶部１２に含まれる初期条件記憶部１２１が記憶している搬送条件を満たすように対象物Ｗを把持する機構を有していてもよい。また、エンドエフェクタ２８は、初期条件記憶部１２１が記憶している搬送条件をユーザ又は演算装置１０が任意に変更した条件を満たすように対象物Ｗを把持する機構を有していてもよい。

ここで、搬送条件とは、ロボット２０が対象物Ｗを第１の位置から第２の位置に搬送するときの条件であり、対象物Ｗの姿勢に関する第１の条件を含む。本実施形態において、演算装置１０は、搬送条件に基づいてロボット２０の経路を生成又は修正する。なお、搬送条件には、ロボット２０が配置される環境に関する第２の条件を含んでいてもよい。第２の条件とは、例えば、ロボット２０の周辺物（障害物）が存在する領域に関する条件であって、ロボット２０の進入が禁止される領域に関する条件である。なお、対象物Ｗの姿勢に関する条件は、対象物Ｗの姿勢を、水平方向に対して、所定の角度範囲未満に保持するための条件であることが好ましい。これによれば、対象物Ｗが第１の位置から第２の位置に搬送されるときに、天地逆転することを抑制できる。また、搬送条件は、第１の条件及び第２の条件に限定されず、例えば、ロボット２０が対象物Ｗを第１の位置から第２の位置に搬送するときの速度に関する条件を含んでいてもよい。なお、第１の位置、及び、第２の位置は、それぞれ、対象物Ｗの保持を開始する所定の位置、及び、保持した対象物を移動させた後に対象物の保持を終了する所定の位置、に限定されない。例えば、対象物Ｗの保持を開始してから、対象物Ｗの保持を終了するまでの間に含まれる任意のいずれかの位置であってよい。

上述の例に示したとおり、台座２１、第１リンク２２、第２リンク２３、第３リンク２４、第４リンク２５、第５リンク２６、第６リンク２７、及びエンドエフェクタ２８は、可動軸により一列に接続され、六軸垂直多関節ロボットを構成する。なお、ロボット２０は、六軸垂直多関節ロボットに限定されるものではなく、二つ以上の可動軸を有していればよい。ロボット２０は、例えば、五軸垂直多関節ロボット又は七軸垂直多関節ロボットでもよい。また、上述したとおり、ロボット２０はハードウェアで構成される実在のロボットに限らず、シミュレータなどの、仮想的に構成されるロボットでもよい。

撮像装置３０は、エンドエフェクタ２８に把持される対象物Ｗの画像を撮像するための装置である。撮像装置３０は、例えば、カメラである。撮像装置３０は、エンドエフェクタ２８に把持される対象物Ｗの画像を演算装置１０へ送信し、演算装置１０において対象物Ｗの初期位置や目標位置を算出する。或いは、撮像装置３０は、自身が有するプロセッサを使用してエンドエフェクタ２８に把持される対象物Ｗの画像に画像処理を施すことにより対象物Ｗの初期位置や目標位置を算出し、演算装置１０へ送信してもよい。初期位置は、ロボット２０により搬送される始点における対象物Ｗの位置である。目標位置は、ロボット２０により搬送される終点における対象物Ｗの位置である。

なお、初期位置及び目標位置は、それぞれ対象物Ｗの姿勢に関する情報を含んでいてよい。また、初期位置及び目標位置は、それぞれ、対象物Ｗを把持するエンドエフェクタ２８の位置と姿勢との少なくともいずれかの情報を含んでいてよい。

なお、ロボットシステム１は、角度検出装置４０を、撮像装置３０に換えて、又は、撮像装置３０に加えて有していてもよい。角度検出装置４０は、エンドエフェクタ２８に把持されている対象物Ｗの角度を検出する。角度検出装置４０は、例えば、加速度センサ、ジャイロセンサである。これらは、例えば、エンドエフェクタ２８に取り付けられる。エンドエフェクタ２８は、対象物Ｗを堅く把持している。このため、角度検出装置４０は、エンドエフェクタ２８の角度を検出することにより、対象物Ｗの角度を検出することができる。角度検出装置４０は、計測した対象物Ｗの角度を演算装置１０へ送信する。なお、角度検出装置４０が検出する対象物Ｗの角度の基準は、特に限定されない。角度検出装置４０は、例えば、鉛直方向を基準にする場合、鉛直方向に対するエンドエフェクタ２８の角度に基づいて、対象物Ｗの角度を検出する。

なお、角度検出装置４０は、対象物Ｗに取り付けられていてもよい。また、角度検出装置４０は、第１リンク２２、第２リンク２３、第３リンク２４、第４リンク２５、第５リンク２６、第６リンク２７、及びエンドエフェクタ２８の少なくとも一つに取り付けられていてもよい。具体的には、角度検出装置４０は、第４リンク２５、第５リンク２６及びエンドエフェクタ２８各々に取り付けられていてもよい。この場合、角度検出装置４０は、例えば、第４リンク２５の第３リンク２４に対する回転角度、第５リンク２６の第４リンク２５に対する回転角度及びエンドエフェクタ２８の第５リンク２６に対する回転角度を計測し、所定の演算処理（例えば、順運動学による演算）を行うことにより、エンドエフェクタ２８に把持されている対象物Ｗの角度を検出する。

図２は、本実施形態における演算装置１０のハードウェア構成の一例を示すブロック図である。演算装置１０は、ロボット２０の経路を演算する装置であり、同図に示すとおり、本実施形態では、外部インタフェース１１と、記憶部１２と、演算部１５とを備える。また、演算装置１０は、ドライブ１３を備えてもよい。

外部インタフェース１１は、外部機器を演算装置１０に接続するためのインタフェースである。図２に示す例において、外部インタフェース１１は、ロボット２０、撮像装置３０、角度検出装置４０、入力装置５０及び出力装置６０と、演算装置１０とを電気通信かつ情報通信可能に接続する。外部インタフェース１１は、対象物Ｗの初期位置及び目標位置を取得するための取得部としての機能を有する。外部インタフェース１１は、例えば、ユーザが入力装置５０を使用して入力した対象物Ｗの初期位置及び目標位置を取得する。或いは、外部インタフェース１１は、撮像装置３０からエンドエフェクタ２８に把持された対象物Ｗの画像を取得する。演算部１５は、記憶部１２に格納された画像処理プログラムに基づいて、取得した画像から、対象物Ｗの初期位置及び目標位置を算出する。或いは、外部インタフェース１１は、撮像装置３０が算出した初期位置及び目標位置を取得する。

外部インタフェース１１は、初期位置とともに、又は、初期位置に加えて、初期位置に置かれている対象物Ｗを掴む際のロボット２０の姿勢を取得してもよい。以下の説明では、この姿勢を初期姿勢と呼ぶ。また、外部インタフェース１１は、目標位置とともに、又は、目標位置に加えて、対象物Ｗを目標位置に置く際のロボット２０の姿勢を取得してもよい。以下の説明では、この姿勢を目標姿勢と呼ぶ。また、外部インタフェース１１は、角度検出装置４０から計測した対象物Ｗの角度を取得し、記憶部１２に格納してもよい。また、外部インタフェース１１は、さらに、ロボット２０の現在の姿勢（現在姿勢）を取得するように構成されていてもよい。なお、外部インタフェース１１が取得するロボット２０の姿勢とは、ロボット２０の複数の可動軸に対応して設けられ、ロボット２０の可動軸の回転角度に関する情報を検出するエンコーダ値であってよい。

一般に、ロボットの姿勢は、コンフィギュレーション空間（Configuration Space）内の点として表現することができる。コンフィギュレーション空間とは、経路計画を行う際に使用されるベクトル空間である。コンフィギュレーション空間に点を置くことをサンプリングということもある。経路計画とは、障害物の位置、スタート及びゴールが与えられたコンフィギュレーション空間内において、スタートからゴールまでを結ぶ経路を生成することである。コンフィギュレーション空間は、ロボットが有する可動軸の数と同数の次元を持つ。コンフィギュレーション空間のある一点は、ロボットが有する可動軸の個数と同じ個数の値により定義され、各可動軸の値は、ロボットの各可動軸周りの回転角度を表す。なお、コンフィギュレーション空間は、ロボットの経路計画の分野においては、関節空間と呼ばれることがある。なお、経路計画を行う空間として実空間に基づく絶対座標空間を用いてもよい。

本実施形態のように、ロボット２０として六軸垂直多関節ロボットを用いる場合、コンフィギュレーション空間は、六次元ベクトル空間となる。ロボット２０のコンフィギュレーション空間の各可動軸の値は、それぞれ第１リンク２２の台座２１に対する回転角度、第２リンク２３の第１リンク２２に対する回転角度、第３リンク２４の第２リンク２３に対する回転角度、第４リンク２５の第３リンク２４に対する回転角度、第５リンク２６の第４リンク２５に対する回転角度及び第６リンク２７の第５リンク２６に対する回転角度を表す。演算部１５は、経路を生成する演算処理において、各可動軸の回転角度に関するデータ（ロボット２０に設けられたエンコーダの検出値）を、外部インタフェースを用いて取得し、順運動学に基づいて、ロボット２０又はその仮想モデルの位置、姿勢を演算する。また、演算部１５は、コンフィギュレーション空間を用いて、ロボット２０の初期姿勢を決定する点の各座標の値及びロボット２０の目標姿勢を決定する点の各座標の値を演算してよい。なお、演算部１５は、外部インタフェース１１の取得部としての機能を用いて、コンフィギュレーション空間におけるロボット２０の初期姿勢及び／又は目標姿勢を決定するための点の各座標の値を取得してもよい。

また、外部インタフェース１１は、後述する経路生成手順記憶部１２２及び経路修正手順記憶部１２３によって生成された経路をロボット２０又は出力装置６０へ出力するための出力部としての機能を有する。

記憶部１２は、例えば、揮発性メモリ、不揮発性メモリ、ハードディスクドライブ（Hard Disc Drive：ＨＤＤ）によって構成される。本実施形態において、記憶部１２は、初期条件記憶部１２１と、経路生成手順記憶部１２２と、経路修正手順記憶部１２３と、経路記憶部１２４と、経路出力手順記憶部１２５とを備える。

初期条件記憶部１２１は、搬送条件の初期値を格納するものであり、例えば、記憶部１２の内部に設けられた所定の記憶領域により構成される。ここで、搬送条件とは、上述の第２の条件、すなわち、ロボット２０が配置される周辺の環境に関する条件を含んでいてよい。ここで、第２の条件の初期値は、例えば、障害物領域Ｃｏである。障害物領域Ｃｏとは、コンフィギュレーション空間内において、ロボット２０及び対象物Ｗの少なくとも一方が周囲の障害物に衝突するようなロボット２０の姿勢を与える点が存在する領域である。なお、以下の説明では、「ロボット２０が障害物に衝突する」とは、ロボット２０が把持する対象物Ｗが障害物に衝突する場合も含むものとする。また、ここでは、ロボット２０の周辺環境に関する情報を「周辺環境情報」といい、障害物領域Ｃｏは、周辺環境情報の一例である。また、搬送条件は、ロボットが、対象物Ｗを第１の位置から第２の位置に搬送するときの、対象物Ｗの姿勢に関する第１の条件を含んでいてよい。例えば、対象物Ｗの姿勢に関する第１の条件の初期値は、角度不適合領域Ｃｐを含んでもよい。角度不適合領域Ｃｐとは、コンフィギュレーション空間内において、対象物Ｗの姿勢が所定の条件に適合しないロボット２０の姿勢を与える点が存在する領域である。対象物Ｗはエンドエフェクタ２８によって把持されて、初期姿勢から目標姿勢まで搬送されるので、角度不適合領域Ｃｐは、コンフィギュレーション空間内における所定の領域として表すことができる。角度不適合領域Ｃｐは、対象物Ｗの姿勢に関する条件に基づいて決定されるものであり、その条件としては、例えば、搬送中に対象物Ｗが天地逆転しないとか、搬送中の対象物Ｗの角度を鉛直方向に対して所定の範囲内に保つ等である。なお、障害物領域と角度不適合領域とは、重複することもある。これらの搬送条件は、ユーザが任意に設定又は変更することができる。例えば、搬送条件は、ユーザが入力装置５０を用いて入力してもよい。このとき、演算装置１０は、外部インタフェース１１を介して、入力された条件を取得する。取得された条件は、初期条件記憶部１２１の初期値又は上書値として、記憶部１２に格納されてもよい。また、搬送条件は、例えば、ロボット２０の周辺環境を取得する不図示のセンサやＣＡＤのデータ、予め定められた値等に基づいて、演算装置１０が任意に設定又は変更するものとしてもよい。例えば、演算装置１０は、ロボット２０の周辺環境を取得するセンサから取得された値に基づいて、障害物領域Ｃｏを更新し、かつ、障害物領域Ｃｏの更新に伴って、増減する角度不適合領域Ｃｐを更新する演算処理を実行する。

なお、初期条件記憶部１２１は、経路を使用するロボット２０に関する条件を記憶していてもよい。すなわち、ロボット２０は、前述のように、台座及び複数のリンクのそれぞれを可動軸により互いに接続して構成される。したがって、初期条件記憶部１２１は、台座及び各リンクの形状に関する情報、可動軸に関する情報（例えば、回転可能な角度、速度、加速度に関する情報）を、ロボット２０に関する条件として記憶してもよい。これによれば、例えば、演算装置１０でロボット２０の経路を演算することができ、また、仮想空間上に、当該経路を使用した、ロボット２０の動作を再現させることができる。なお、初期条件記憶部１２１は、ロボット２０の動作の再現を容易にするため、ロボット２０の近似モデルを、ロボット２０に関する条件として記憶していてもよい。また、初期条件記憶部１２１は、エンドエフェクタ２８及び／又は対象物Ｗの初期位置及び目標位置を記憶しておいてもよい。なお、初期条件記憶部１２１に記憶される条件は、様々な形式で表現されてよく、例えば、条件を満たすか否かを判別するための所定の関数によって表現されてもよい。

経路生成手順記憶部１２２は、例えば、記憶部１２の内部に設けられた所定の記憶領域により構成される。経路生成手順記憶部１２２は、ロボット２０が対象物Ｗを初期位置から目標位置に搬送するときの経路を生成するための演算処理を、演算部１５に実行させるためのプログラムを格納する。例えば、記憶部１２の経路生成手順記憶部１２２に保存されたソフトウェアプログラムが演算部１５に読み出されて実行されることによって、経路の生成に関する演算処理が実行される。演算部１５は、経路生成手順記憶部１２２に格納されたプログラムに基づき、搬送条件を、演算装置１０の外部から取得、又は、初期条件記憶部１２１から呼び出す。演算部１５は、取得又は呼び出した条件に基づいて、ロボット２０が、対象物Ｗを初期位置から目標位置に搬送する経路の全体にわたり、当該搬送条件を満たす経路を生成する。例えば、搬送条件が、対象物Ｗを搬送する際の角度を所定の範囲内に収めるというものである場合、演算部１５は、ロボット２０が対象物Ｗを初期位置から目標位置まで搬送する間、対象物Ｗの角度が所定の範囲内に収まるような経路を生成する。

本実施形態において、経路生成手順記憶部１２２は、ロボット２０が周辺環境に設置された障害物と干渉することなく、かつ、搬送中の対象物Ｗの姿勢に関する条件を満たすような経路を生成するための、プログラムを格納している。なお、生成される経路は、コンフィギュレーション空間上で初期姿勢を示す点Ｓから目標姿勢を示す点Ｇまでを結ぶ複数の点Ｎｉの列Ｔとして表すことができる。演算部１５により実行される経路生成処理の手法としては、ポテンシャル法、ロードマップ法、最適化法などを含む任意の手法を用いることができる。それぞれの手法による経路生成処理については後述する。

経路修正手順記憶部１２３は、例えば、記憶部１２の内部に設けられた所定の記憶領域により構成される。経路修正手順記憶部１２３は、経路生成手順記憶部１２２に格納されたプログラムに基づいて生成された経路を修正するための演算処理を、演算部１５に実行させるためのプログラムを格納する。本実施形態において、経路修正手順記憶部１２３に格納されるプログラムは、ロボット２０が周辺環境に設置された障害物と干渉することなく、かつ、搬送中の対象物Ｗの姿勢に関する条件を満たすように、経路を修正する処理を実行するためのプログラムである。経路修正手順記憶部１２３に格納されるプログラムは、経路生成手順記憶部１２２が生成した経路を平滑化する処理を実行するためのものでもよい。また、経路生成手順記憶部１２２に格納されたプログラムに基づいて生成された経路のショートカットを探索する処理を実行するためのものでもよい。また、ゴールまでの経路長、角速度、計算時間、移動予測時間の少なくともいずれかに基づく評価関数を定め、評価関数の値が最大化するように経路を修正する処理を実行するためのものでもよい。なお、経路生成手順記憶部１２２に格納されたプログラムに基づく演算処理によって複数の経路が生成される場合には、所定の評価関数により各経路を評価して、複数の経路の中から最適な経路が選択されるようにしてもよい。なお、ロボット２０と周辺環境に設置された障害物との干渉を回避するように経路を修正するためのプログラムと、搬送中の対象物Ｗの姿勢に関する条件を満たすように経路を修正するプログラムとは、別々に格納されていてよい。

経路記憶部１２４は、経路生成手順記憶部１２２や経路修正手順記憶部１２３に格納されたプログラムを実行することによって生成又は修正された経路（以下、単に「生成された経路」ともいう。）を記憶するものであり、例えば、記憶部１２の内部に設けられた記憶領域により構成される。経路生成手順記憶部１２２に格納されたプログラムに基づく演算処理は、経路を生成又は修正するときの条件に変わりがない場合に、経路記憶部１２４が記憶している経路を読みだすことによって、行われてもよい。例えば、演算部１５は、すでに生成された経路における対象物Ｗの初期位置と目標位置とに変わりがない場合や、ロボット２０の周囲にある障害物等の環境、すなわち周辺環境情報に変わりがない場合には、経路記憶部１２４が記憶している経路を読み出すことによって、経路を生成又は修正してもよい。これにより、演算部１５は、迅速に適切な経路を生成することができる。なお、経路記憶部１２４は、経路生成手順記憶部１２２や経路修正手順記憶部１２３に格納されたプログラムを実行することによって生成された経路を、経路を生成又は修正したときに考慮した条件と紐づけて、記憶しておくことが好ましい。

経路出力手順記憶部１２５は、例えば、記憶部１２の内部に設けられた所定の記憶領域により構成される。経路出力手順記憶部１２５は、経路生成手順記憶部１２２や経路修正手順記憶部１２３に格納されたプログラムを実行することによって生成された経路、又は、経路記憶部１２４に記憶された経路を、外部インタフェース１１を介してロボット２０に出力するためのプログラムを格納する。経路出力手順記憶部１２５に保存されたソフトウェアプログラムが演算部１５に読み出されて実行されることによって、経路が外部インタフェース１１を介して外部に出力される。

なお、経路生成手順記憶部１２２、経路修正手順記憶部１２３及び経路出力手順記憶部１２５は、それぞれ区別されなくてもよい。例えば、経路生成手順記憶部１２２に格納されたプログラムにより実行される処理と、経路修正手順記憶部１２３に格納されたプログラムにより実行される処理と、経路出力手順記憶部１２５に格納されたプログラムにより実行される処理とは、一体的に行われてもよい。

ドライブ１３は、演算装置１０に挿入された記憶媒体１４を駆動する。記憶媒体１４に対象物Ｗの初期位置及び目標位置が記憶されている場合、演算装置１０は、記憶媒体１４から対象物Ｗの初期位置及び目標位置を取得してもよい。このとき、ドライブ１３は、対象物Ｗの初期位置及び目標位置を取得するための取得部として機能する。また、演算装置１０は、経路生成手順記憶部１２２及び経路修正手順記憶部１２３によって生成された経路を記憶媒体１４に出力してもよい。このとき、ドライブ１３は、生成された経路を記憶媒体１４へ出力するための出力部として機能する。

記憶媒体１４は、記憶部１２の代わりに、初期条件記憶部１２１及び経路記憶部１２４に格納されたデータを記憶する媒体として用いることができる。また、記憶媒体１４は、記憶部１２の代わりに、経路生成手順記憶部１２２及び経路修正手順記憶部１２３の少なくともいずれか一つに格納されたソフトウェアプログラムを記憶することができる。記憶媒体１４は、例えば、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−ＲＷ、ＤＶＤ−ＲＯＭ、ＤＶＤ−Ｒ、ＤＶＤ−ＲＷ、ＤＶＤ−ＲＡＭ、ＬＤ、光磁気ディスク、メモリーカードである。

演算部１５は、ハードウェアプロセッサとしてのＣＰＵ（Central Processing Unit）１５１と、ＲＡＭ（Random Access Memory）１５２と、ＲＯＭ（Read Only Memory）１５３とを備える。

ＣＰＵ１５１は、記憶部１２の経路生成手順記憶部１２２、経路修正手順記憶部１２３、及び経路出力手順記憶部１２５の少なくともいずれか一つに格納されたソフトウェアプログラムを読み出して実行するプロセッサである。ＣＰＵ１５１は、例えば、記憶部１２の経路生成手順記憶部１２２、経路修正手順記憶部１２３、及び経路出力手順記憶部１２５の少なくともいずれか一つに格納されたプログラムを読み出し、これらのプログラムをＲＡＭ（Random Access Memory）１５２に格納し、これらのプログラムを実行する。

入力装置５０は、ロボットシステム１へ指示や設定を入力するための装置であり、例えば、マウスやキーボードによって構成される。ユーザが入力装置５０に対して入力した指示や設定は、演算装置１０の演算部１５へ送信される。

出力装置６０は、例えば、ディスプレイであり、経路生成手順記憶部１２２及び／又は経路修正手順記憶部１２３が生成した経路をユーザに対して出力する。

なお、演算装置１０は、演算処理の結果に基づいて、搬送条件を満たす経路を生成するように構成されていればよい。したがって、演算装置１０は、搬送条件を満たす経路を生成する処理を実行するハードウェアプロセッサと、ハードウェアプロセッサに対して、搬送条件を満たす経路を生成する処理を実行させるためのソフトウェアプログラムを格納する揮発性メモリ及び不揮発性メモリの少なくともいずれか一方と、搬送条件を外部から取得するための外部インタフェースと、を少なくとも備えてればよい。

次に、図３を参照しながら、本実施形態における経路出力処理の流れについて説明する。図３は、演算装置１０における経路出力処理のフローチャートの一例である。

まず、演算部１５は、外部インタフェース１１を用いて、ロボット２０の初期姿勢及び目標姿勢を取得する（ステップＳ３１）。具体的には、外部インタフェース１１は、ユーザが入力装置５０から入力した初期姿勢と目標姿勢を取得する。また、外部インタフェース１１は、撮像装置３０や角度検出装置４０から得られた情報に基づいて算出された初期姿勢と目標姿勢との少なくともいずれかを取得してもよく、撮像装置３０や角度検出装置４０から得られる情報を取得して初期姿勢と目標姿勢との少なくともいずれかを算出してもよい。さらに、外部インタフェース１１は、ロボット２０が有する可動軸の角度を検出するセンサ（エンコーダ）の検出値に基づいて算出された、ロボット２０の現在の姿勢を初期姿勢として取得してもよい。また、演算部１５は、外部インタフェース１１を用いて、ロボット２０が有する可動軸の角度を検出するセンサ（エンコーダ）の検出値を取得して、ロボット２０の現在の姿勢を算出し、これを初期姿勢としてもよい。

また、演算部１５は、Ｓ３１の処理と並行して、又は、Ｓ３１の処理の前後で、ロボット２０が対象物Ｗを搬送するときの搬送条件を取得する（ステップＳ３２）。演算部１５は、ユーザが入力装置５０から入力した搬送条件を、外部インタフェース１１を介して取得してもよいし、或いは、予め初期条件記憶部１２１に記憶された搬送条件を取得してもよい。演算部１５が取得する搬送条件は任意であるが、例えば、搬送中の対象物Ｗの姿勢に関する条件やロボット２０が配置される周辺の環境に関する条件などが取得される。なお、図３においては、ステップＳ３１の後にステップＳ３２が実行される形で図示されているが、これは一例にすぎない。上述のとおり、ステップＳ３１とＳ３２は並行して実行されてもよく、また、ステップＳ３２の後にステップＳ３１が実行されてもよい。演算部１５は、外部インタフェース１１を用いて取得した情報、及び、算出した情報を、初期条件記憶部１２１に格納、又は、上書きして、保存する。

なお、ステップＳ３１とステップＳ３２との少なくともいずれかの処理は、後述する経路を生成する処理（ステップＳ３３）、および、経路を修正する処理（ステップＳ３４）の事前処理として実行することができる。この事前処理は、例えば、ロボット２０を用いた作業工程の設置や立ち上げを行うときに実行することができる。

このとき、演算部１５は、ステップＳ３１において取得する初期位置と目標位置とが、ステップＳ３２において取得する搬送条件を満足するかを判定する処理と、取得した初期位置と目標位置との少なくともいずれか一方が搬送条件を満たさなかった場合に、その旨をユーザに通知するための情報を、外部インタフェース１１を介して外部（例えば、出力装置６０）に出力する処理と、を実行してもよい。これによれば、ロボット２０を用いた作業工程の設置や立ち上げを行うときに、ユーザが誤って初期位置や目標位置を入力した場合に、その誤りを認識することができる。その結果、ユーザが意図せず設定した搬送条件に基づいてロボット２０の経路が生成されることを防止できる。

なお、この事前処理は、対象物Ｗを搬送する始点（初期位置）と終点（目標位置）とが、対象物Ｗに応じて変動しない、または、変動が少ない場合に好適である。例えば、パーツフィーダ等により、予め定められた位置及び姿勢に保持された対象物Ｗを、所定のボックスに収納する工程に好適である。これによれば、事前処理のステップで設定した搬送条件を用いて繰り返し経路を生成および修正できるので、経路を算出して出力するまでの時間を短縮することができる。

次に、演算部１５は、経路生成手順記憶部１２２に格納されたプログラムに基づいて、経路生成処理を実行する（ステップＳ３３）。演算部１５は、経路生成手順記憶部１２２に格納されたプログラムに基づいて、コンフィギュレーション空間内に初期姿勢を表す点Ｓと目標姿勢を表す点Ｇを配置し、コンフィギュレーション空間において、障害物領域Ｃｏと角度不適合領域Ｃｐを除く空間内で経路を生成する。具体的な経路生成処理の手法については後述する。

その後、演算部１５は、経路生成手順記憶部１２２に格納されたプログラムに基づいて生成された経路に対して、経路修正手順記憶部１２３が格納するプログラムを呼び出して、平滑化やショートカット等の、経路を修正する処理を実行する（ステップＳ３４）。演算部１５は、経路生成手順記憶部１２２が格納するプログラムに基づいて生成した経路に対して、コンフィギュレーション空間において、障害物領域Ｃｏと角度不適合領域Ｃｐを除く空間内で、経路を修正する。なお、経路を修正する処理は必須ではなく、必要に応じて任意に実行すればよい。

演算部１５は、経路出力手順記憶部１２５に格納されたプログラムに基づいて、生成された経路（修正された経路を含む）を、外部インタフェース１１を介してロボット２０に出力する（ステップＳ３５）。ロボット２０は、演算装置１０から受信した経路に従って動作し、初期位置から目標位置まで対象物Ｗを搬送する。なお、経路は、必ずしもロボット２０に対して出力されなくてもよい。例えば、演算部１５は、ディスプレイ等の出力装置６０に対して、ロボット２０の近似モデルと経路とを出力し、出力装置６０上で、ロボット２０の動作を再現するようにしてもよい。

なお、ここでは、経路生成手順記憶部１２２に格納されたプログラムに基づく経路生成処理（ステップＳ３３）と経路修正手順記憶部１２３に格納されたプログラムに基づく経路修正処理（ステップＳ３４）を分けて説明したが、経路修正処理を経路生成処理に含めてもよい。

以下、経路生成処理について具体的に説明する。これらの処理は、対象物Ｗを搬送する始点（初期位置）と終点（目標位置）との少なくともいずれかが、対象物Ｗ毎に異なる場合により好適である。例えば、ベルトコンベアに不規則に配置された複数の対象物Ｗを、対象物Ｗの種別ごとに設定された複数のボックスに仕分ける工程に好適である。なお、以下の各実施例においては、経路生成手順記憶部１２２に格納されたソフトウェアプログラムが演算部１５に読み出されて実行されることによって、演算部１５が、経路の生成に関する各種の演算処理を実行するものとして説明する。

＜実施例１＞
実施例１では、ランダムサンプリング法の一つであるＲＲＴ（Rapidly-Exploring Random Tree）に基づく経路生成処理について説明する。本実施例において、演算部１５は、コンフィギュレーション空間内に点を一つ配置し、当該点と、既にコンフィギュレーション空間内に配置されている点とを結ぶ処理を繰り返すことにより経路を生成する。

図４は、ＲＲＴに基づく経路生成処理のフローチャートの一例である。図５は、ＲＲＴに基づく経路生成処理におけるコンフィギュレーション空間の一例を示す図である。本実施例では、ロボット２０が配置される周辺の環境に関する搬送条件（第２の条件）を満足するか否かの判定に障害物領域Ｃｏを利用し、ロボット２０が、対象物Ｗを第１の位置から第２の位置に搬送するときの、対象物の姿勢に関する搬送条件（第１の条件）を満足するか否かの判定に角度不適合領域Ｃｐを利用する。なお、第１の条件および第２の条件を満足するか否かの判定には、必ずしも、領域を利用しなくてもよい。例えば、ロボット２０の姿勢を示すコンフィギュレーション空間の点を、順運動学に基づく演算によって所定の形式に変換し、この変換値とユーザが指定した搬送条件とを比較して、搬送条件を満たすか否かを判定してもよい。

演算部１５は、コンフィギュレーション空間上にロボット２０の初期姿勢を表す点Ｓと目標姿勢を表す点Ｇを配置し（ステップＳ１０１）、点Ｓと点Ｇとを結ぶことができるか否かを判定する（ステップＳ１０２）。具体的には、演算部１５は、点Ｓと点Ｇとを結ぶ線分が障害物領域Ｃｏ及び角度不適合領域Ｃｐと重複していない場合、これら二つの点を結ぶことができるものと判定する。演算部１５は、点Ｓと点Ｇとを結ぶ線分が障害物領域Ｃｏ及び角度不適合領域Ｃｐの少なくとも一方と重複する部分がある場合、これら二つの点を結ぶことができないものと判定する。そのような場合、ロボット２０が障害物に衝突したり、対象物Ｗの姿勢に関する条件が満たされないからである。

演算部１５は、点Ｓと点Ｇとを結ぶことができるものと判定した場合（ステップＳ１０２：Ｙｅｓ）、処理をステップＳ１０３へ進める。他方、演算部１５は、点Ｓと点Ｇとを結ぶことができないものと判定した場合（ステップＳ１０２：Ｎｏ）、処理をステップＳ１０４へ進める。

演算部１５は、点Ｓと点Ｇとを結び、経路を生成し、処理を終了させる（ステップＳ１０３）。

演算部１５は、コンフィギュレーション空間内に点Ｎｉを一つ配置（サンプリング）する（ステップＳ１０４）。この点は、点Ｓ及び点Ｇとは異なる点であり、ロボット２０の姿勢、すなわち、複数の可動軸を有する多関節ロボットであるロボット２０における、所定の姿勢の一つを表している。この姿勢は、初期位置（姿勢）と目標位置（姿勢）との間に設けられる中継位置（姿勢）の候補である。

演算部１５は、ステップＳ１０４で配置した点により表されるロボット２０の姿勢により、ロボット２０が周囲の障害物と衝突するか否かを判定する（ステップＳ１０５）。具体的には、演算部１５は、ステップＳ１０４で配置した点が障害物領域Ｃｏ内に存在する場合（例えば、図５の点Ｎｂ，Ｎｄ）、ロボット２０が周囲の障害物と衝突するものと判定する。他方、演算部１５は、ステップＳ１０４で配置した点が障害物領域Ｃｏ内に存在しない場合（例えば、図５の点Ｎ１〜Ｎ５，Ｎａ，Ｎｃ）、ロボット２０が周囲の障害物と衝突しないものと判定する。

演算部１５は、ステップＳ１０５でロボット２０が周囲の障害物と衝突すると判定した場合（ステップＳ１０５：Ｙｅｓ）、ステップＳ１０４で配置した点Ｎｉをコンフィギュレーション空間内から削除し（ステップＳ１１０）、処理をステップＳ１０４へ戻す。他方、演算部１５は、ステップＳ１０５でロボット２０が周囲の障害物と衝突しないものと判定した場合（ステップＳ１０５：Ｎｏ）、処理をステップＳ１０７へ進める。

演算部１５は、ロボット２０が把持する対象物Ｗが姿勢に関する条件を満たすか否かを判定する（ステップＳ１０７）。

具体的には、演算部１５は、ステップＳ１０４で配置した点が、対象物Ｗの姿勢に関する条件に基づいて求められる角度不適合領域Ｃｐ内に存在する場合（例えば、図５の点Ｎａ，Ｎｃ）、当該点によって表されるロボット２０の姿勢により把持される対象物Ｗの姿勢が条件を満たさないものと判定する。また、演算部１５は、ステップＳ１０４で配置した点が、角度不適合領域Ｃｐ内に存在しない場合（例えば、図５の点Ｎ１〜Ｎ５）、当該点によって表されるロボット２０の姿勢により把持される対象物Ｗの姿勢が条件を満たすものと判定する。

演算部１５は、ステップＳ１０４で配置した点が角度不適合領域Ｃｐ内に存在する、すなわち、対象物Ｗの姿勢が当該姿勢に関する条件を満たさないものと判定した場合（ステップＳ１０７：Ｙｅｓ）、ステップＳ１０４で配置した点Ｎｉをコンフィギュレーション空間内から削除し（ステップＳ１１０）、処理をステップＳ１０４へ戻す。他方、演算部１５は、対象物Ｗの姿勢が条件を満たすものと判定した場合（ステップＳ１０７：Ｎｏ）、処理をステップＳ１０９へ進める。

演算部１５は、ステップＳ１０４で配置した点Ｎｉと、初期姿勢を表す点Ｓ及び既に配置された点の中で、点Ｎｉの最近傍に配置された点Ｎｉ−ｎｅａｒｅｓｔとを結ぶことができるか否かを判定する（ステップＳ１０９）。具体的には、演算部１５は、ステップＳ１０４で配置した点Ｎｉと、これまでに配置された点Ｎ０，・・・，Ｎｉ−１（点Ｓは点Ｎ０とする）の中で、点Ｎｉの最近傍に配置されている点Ｎｉ−ｎｅａｒｅｓｔとを結ぶ線分が、障害物領域Ｃｏ及び角度不適合領域Ｃｐのいずれとも重複していない場合、これら二つの点を結ぶことができるものと判定する。

演算部１５は、ステップＳ１０４で配置した点Ｎｉと、点Ｎｉ−ｎｅａｒｅｓｔとを結ぶことができないものと判定した場合（ステップＳ１０９：Ｎｏ）、ステップＳ１０４で配置した点Ｎｉをコンフィギュレーション空間内から削除し（ステップＳ１１０）、処理をステップＳ１０４へ戻す。他方、演算部１５は、ステップＳ１０４で配置した点Ｎｉと、点Ｎｉ−ｎｅａｒｅｓｔとを結ぶことができるものと判定した場合（ステップＳ１０９：Ｙｅｓ）、処理をステップＳ１１１へ進める。

演算部１５は、ステップＳ１０４で配置した点Ｎｉと、前回配置された点Ｎｉ−ｎｅａｒｅｓｔとを結ぶ（ステップＳ１１１）。

演算部１５は、点Ｎｉと、目標姿勢を表す点Ｇとを結ぶことができるか否かを判定する（ステップＳ１１２）。具体的には、演算部１５は、点Ｎｉと点Ｇとを結ぶ線分が、障害物領域Ｃｏ及び角度不適合領域Ｃｐのいずれとも重複していない場合、これら二つの点を結ぶことができるものと判定する。

演算部１５は、点Ｎｉと点Ｇとを結ぶことができないものと判定した場合（ステップＳ１１２：Ｎｏ）、ｉの値に１を加えた上で処理をステップＳ１０４へ戻し、新たな点Ｎｉ＋１をサンプリングする。他方、演算部１５は、点Ｎｉと点Ｇとを結ぶことができるものと判定した場合（ステップＳ１１２：Ｙｅｓ）、処理をステップＳ１１３へ進める。このように、ステップＳ１０４からステップＳ１１２までの処理を、点Ｇまで結ぶことができるまで繰り返し実行する。

演算部１５は、点Ｎｉと点Ｇとを結び、経路を生成する（ステップＳ１１３）。

このように、演算部１５は、対象物Ｗを初期位置（点Ｓ）から中継位置（点Ｎ１，Ｎ２，・・・）を経由して目標位置（点Ｇ）に搬送する経路（点列Ｔ＝（Ｓ，Ｎ１，Ｎ２，・・・,Ｇ））を生成する。図５では、点列Ｔ＝（Ｓ，Ｎ１，Ｎ２，Ｎ３，Ｎ５，Ｎ６，Ｇ）が経路として生成される。

なお、ステップＳ１１０の処理は、点Ｎｉをコンフィギュレーション空間内から単純に削除する処理に限られない。例えば、点Ｎｉを基準とした所定の領域を設定し、Ｓ１０４において、設定した当該領域の中で点Ｎｉをサンプリングすることで、当該領域内で点Ｎｉを探索することができる。また、例えば、点Ｎｉを基準とした所定の領域を設定し、Ｓ１０４において、設定した当該領域の外で点Ｎｉをサンプリングすることで、当該領域外で点Ｎｉを探索することができる。

なお、上記の経路生成処理では、初期位置（点Ｓ）から目標位置（点Ｇ）へ、一方通行で点を配置して接続していく処理を記載したが、初期位置（点Ｓ）と目標位置（点Ｇ）との双方から点Ｎｉを配置してもよい。すなわち、初期位置（点Ｓ）から順に配置する点をＮｓｉとし、目標位置（点Ｇ）から順に配置する点をＮｇｉとして、点Ｓと点Ｇとの双方から順に点を配置し、経路計画を行う所定の空間（コンフィギュレーション空間）で、点Ｎｓｉと点Ｎｇｉとを接続することで、点Ｓと点Ｇとを接続する経路を設計してもよい。

＜実施例２＞
実施例２では、ロードマップ法の一つである確率的ロードマップ（Probabilistic Road Map：ＰＲＭ）に基づく経路生成処理について説明する。本実施例において、演算部１５は、複数の点をコンフィギュレーション空間内にランダムに配置し、これらの点の中から障害物領域Ｃｏ及び角度不適合領域Ｃｐの外にある点を抽出し、これらの点を結ぶことにより初期姿勢から目標姿勢までの経路を生成する。

図６は、ＰＲＭに基づく経路生成処理のフローチャートの一例である。図７は、ＰＲＭに基づく経路生成処理におけるコンフィギュレーション空間の一例を示す図である。本実施例では、ロボット２０が配置される周辺の環境に関する搬送条件（第２の条件）を満足するか否かの判定に障害物領域Ｃｏを利用し、ロボット２０が、対象物Ｗを第１の位置から第２の位置に搬送するときの、対象物の姿勢に関する搬送条件（第１の条件）を満足するか否かの判定に角度不適合領域Ｃｐを利用する。なお、第１の条件および第２の条件を満足するか否かの判定には、必ずしも、領域を利用しなくてもよい。例えば、ロボット２０の姿勢を示すコンフィギュレーション空間の点を、順運動学に基づく演算によって所定の形式に変換し、この変換値とユーザが指定した搬送条件とを比較して、搬送条件を満たすか否かを判定してもよい。

演算部１５は、コンフィギュレーション空間上にロボット２０の初期姿勢を表す点Ｓと目標姿勢を表す点Ｇを配置する（ステップＳ２０１）。

演算部１５は、コンフィギュレーション空間内に複数の点Ｎｉをランダムに配置する（ステップＳ２０２）。

演算部１５は、ステップＳ２０２で配置した点Ｎｉの中から、障害物領域Ｃｏの中に含まれる点を棄却し、障害物領域Ｃｏの外にある点を抽出する（ステップＳ２０３）。図７では、障害物領域Ｃｏの中に含まれる点ＮｂとＮｄが棄却される。

続いて、演算部１５は、角度不適合領域Ｃｐの中に含まれる点を棄却し、角度不適合領域Ｃｐの外にある点を抽出する（ステップＳ２０４）。図７では、角度不適合領域Ｃｐの中に含まれる点ＮａとＮｃが棄却され、残りの点Ｎ１〜Ｎ８が抽出される。

演算部１５は、ステップＳ２０４で抽出した点をもとに、初期姿勢を表す点Ｓから目標姿勢を表す点Ｇを結ぶ経路を生成する（ステップＳ２０５）。具体的には、演算部１５は、これらの点を障害物領域及び角度不適合領域のいずれとも重複しない線分で互いに結ぶことにより、経路を生成する。また、ある点から他の点に結ぶ際、点間の距離が所定の範囲内にある点を結ぶものとしてもよい。例えば、図７において、点Ｎ１は、点Ｎ１の近傍にある点Ｓ，Ｎ２，Ｎ３，Ｎ７と結ばれる。図７では、点間が互いに結ばれることを、実線又は破線で表している。

抽出された点から複数の経路が生成されるとき、演算部１５は、所定の基準に基づく評価関数を用いて最適な経路を選択する（ステップＳ２０６）。例えば、評価関数は、点Ｓから点Ｇまでの経路長が最も短くなる経路を選択するための評価関数としてもよい。図７では、例えば、実線で結ばれる点列Ｔ＝（Ｓ，Ｎ１，Ｎ３，Ｎ４，Ｎ６，Ｇ）が経路として選択される。なお、評価関数は、経路長に限られず、他の基準に基づいて経路を選択するものとしてもよい。

なお、ステップＳ２０３、Ｓ２０４の処理は、それぞれの領域に含まれる点を棄却する処理に限られない。例えば、点Ｎｉを基準とした所定の領域を設定し、Ｓ２０２に戻り、当該領域の中で、点Ｎｉをさらに複数配置することができる。また、例えば、点Ｎｉを基準とした所定の領域を設定し、Ｓ２０２に戻り、設定した当該領域の外で点Ｎｉをさらに複数配置することができる。さらに、例えば、点Ｎｉを棄却した後、Ｓ２０２に戻り、コンフィギュレーション空間内に新たに別の点Ｎｉを再付与するようにしてもよい。これによれば、点Ｎｉが棄却された場合でも、経路生成の候補となる点Ｎｉの数を担保することができ、障害物などが多く、経路生成の条件が厳しい場合でも、経路生成処理が不能となることを抑制できる。

＜実施例３＞
実施例３では、ポテンシャル法（Artificial Potential Method）に基づく経路生成処理について説明する。本実施例において、演算部１５は、ロボット２０及び対象物Ｗが位置し得る領域内のポテンシャルを算出し、ポテンシャルに基づいて経路を生成する。演算部１５は、例えば、評価関数を使用することにより、ロボット２０及び対象物Ｗが位置し得る領域内のポテンシャルを算出する。

評価関数とは、コンフィギュレーション空間において、ロボット２０の初期姿勢を表す点Ｓ、障害物領域Ｃｏ及び角度不適合領域Ｃｐに斥力が設定され、ロボット２０の目標姿勢を表す点Ｇに引力が設定されるように、ロボット２０及び対象物Ｗが位置し得る領域内を評価する関数である。すなわち、ロボット２０の周囲に存在する障害物や対象物Ｗの姿勢に関する条件は、評価関数に基づいて定義される。また、演算部１５は、例えば、第４リンク２５の第３リンク２４に対する回転角度、第５リンク２６の第４リンク２５に対する回転角度及びエンドエフェクタ２８の第５リンク２６に対する回転角度それぞれを所定の範囲内に制限した評価関数を使用してもよい。

図８は、ポテンシャル法に基づく経路生成処理のフローチャートの一例である。図９は、ポテンシャル法に基づく経路生成処理におけるコンフィギュレーション空間の一例を示す図である。

まず、演算部１５は、障害物との距離に応じてコストが変化する評価関数により、コンフィギュレーション空間内のフィールドを重み付けする（ステップＳ３０１）。例えば、コンフィギュレーション空間において、障害物領域Ｃｏに斥力が設定されるように、障害物領域Ｃｏからの距離が離れるにつれてポテンシャル値が低くなる評価関数によりフィールドが評価される。

次に、演算部１５は、対象物Ｗの姿勢に応じてコストが変化する評価関数により、コンフィギュレーション空間内のフィールドを重み付する（ステップＳ３０２）。例えば、コンフィギュレーション空間において、角度不適合領域Ｃｐに斥力が設定されるように、角度不適合領域Ｃｐからの距離が離れるにつれてポテンシャル値が低くなる評価関数によりフィールドが評価される。

このように、演算部１５は、障害物領域Ｃｏと角度不適合領域Ｃｐとに基づいて、コンフィギュレーション空間内のフィールドが評価される。この処理は、事前に行われることが好ましいが、この限りではない。なお、障害物との距離に応じてコストが変化する評価関数と、対象物Ｗの姿勢に応じてコストが変化する評価関数と、は単一の関数として表現されていてもよい。

演算部１５は、外部インタフェース１１を介して入力された、又は、初期条件記憶部１２１に記憶された、ロボット２０の初期姿勢と目標姿勢とに基づいて、初期姿勢を表す点Ｓ及び目標姿勢を表す点Ｇとの距離に応じてコストが変化する評価関数により、コンフィギュレーション空間内のフィールドを重み付けする（ステップＳ３０３）。例えば、コンフィギュレーション空間において、点Ｓに斥力が設定され、点Ｇに引力が設定されるように、点Ｓからの距離が近いほどポテンシャル値が高く、かつ、点Ｇからの距離が近いほどポテンシャル値が低くなる評価関数により、フィールドが評価される。

演算部１５は、障害物との距離、対象物Ｗの姿勢、点Ｓと点Ｇとの距離に基づいて、コンフィギュレーション空間内の各位置におけるポテンシャル値を算出し、算出されたポテンシャル値に基づいて経路を生成する（ステップＳ３０４）。例えば、ステップＳ３０１〜Ｓ３０３の各ステップにおいて評価した値を足し合わせたり掛け合わせたりして、フィールドのポテンシャルを評価する。図９に図示された矢印は、評価されたポテンシャルのベクトルを示すものである。演算部１５は、算出したポテンシャルに基づいて、点Ｓから点Ｇまでの経路のコストが最小となる経路を生成する。この経路は、実施例１や実施例２で生成される経路と異なり、連続的なものとなることが多い。

＜実施例４＞
実施例４では、軌道最適化（Trajectory Optimization）に基づく経路生成処理について説明する。本実施例において、演算部１５は、コンフィギュレーション空間内における点の配置に関するコスト関数の最適化問題を解くことにより、ロボット２０の経路を生成する。コスト関数とは、例えば、ロボット２０又は対象物Ｗと障害物との距離、コンフィギュレーション空間内における経路の滑らかさ、ロボット２０のトルクの最大値、第１リンク２２、第２リンク２３、第３リンク２４、第４リンク２５、第５リンク２６又はエンドエフェクタ２８の回転角度等に基づいて設定される関数である。また、ロボット２０又は対象物Ｗと障害物との距離に関するコストは、両者の距離に応じて変化させてもよい。軌道最適化の具体的な手法としては、例えば、ＳＴＯＭＰ（Stochastic Trajectory Optimization for Motion Planning）、ＣＨＯＭＰ（Covariant Hamiltonian Optimization for Motion Planning）等が挙げられる。

図１０は、軌道最適化に基づく経路生成処理のフローチャートである。図１１は、軌道最適化に基づく経路生成処理におけるコンフィギュレーション空間の一例を示す図である。経路最適化においては、コンフィギュレーション空間内に配置された複数の点において、初期姿勢を表す点Ｓから、複数の点Ｎ１，Ｎ２，・・・を経由して目標姿勢を表す点Ｇまで結ばれているとき、これらの点及び隣接する点を結んでいる線分が障害物領域Ｃｏ及び角度不適合領域Ｃｐのいずれとも重複しないように点Ｎｉを移動させることにより経路を生成する。

まず、演算部１５は、コンフィギュレーション空間内において、初期姿勢を表す点Ｓと目標姿勢を表す点Ｇを配置し、これらを結ぶ線分上に複数の点Ｎｉを配置する（ステップＳ４０１）。例えば、点Ｓと点Ｇを結ぶ線分を等間隔にＮ分割し、Ｎ−１個の点Ｎｉ（ｉ＝１，２，・・・，Ｎ−１）を配置する。図１１では、点Ｓと点Ｇを結ぶ線分上に、４つの点Ｎ１〜Ｎ４が配置されている。

次に、演算部１５は、点Ｓと点Ｇを結ぶ線分上に配置された複数の点Ｎｉのうち、障害物領域Ｃｏの中にある点Ｎｉを抽出し、障害物領域Ｃｏの外に移動させる（ステップＳ４０２）。このとき、コスト関数が最小化する位置に点Ｎｉを移動させることが好ましい。図１１では、点Ｎ４が障害物領域Ｃｏに含まれているので、障害物領域Ｃｏの外の点Ｎ４’に移動される。なお、コスト関数は、その出力値が大きいほど最適であるように設計をしてもよい。

さらに、演算部１５は、複数の点Ｎｉのうち、角度不適合領域Ｃｐの中にある点Ｎｉを抽出し、角度不適合領域Ｃｐの外に移動させる（ステップＳ４０３）。このとき、コスト関数が最小化する位置に点Ｎｉを移動させることが好ましい。図１１では、点Ｎ１とＮ２が角度不適合領域Ｃｐに含まれているので、角度不適合領域Ｃｐの外の点Ｎ１’とＮ２’に移動される。

演算部１５は、移動後の点Ｎｉ間を障害物領域Ｃｏ及び角度不適合領域Ｃｐと重複することなく結ぶことができるか否かを判定する（ステップＳ４０４）。重複する部分なくすべての点を結ぶことができれば（ステップＳ４０４：Ｙｅｓ）、移動後の点列Ｔを経路として生成する（ステップＳ４０５）。図１１では、点列Ｔ＝（Ｓ，Ｎ１’，Ｎ２’，Ｎ３，Ｎ４’，Ｇ）が経路として生成される。

他方、ステップＳ４０４において、少なくとも一つの点間を結ぶ線分が障害物領域Ｃｏ又は角度不適合領域Ｃｐと重複する部分がある場合（ステップＳ４０４：Ｎｏ）、ステップＳ４０１に戻り、演算部１５は、点Ｎｉを別の位置に移動させる等して、再度経路探索を行う。また、点Ｎｉを別の位置に移動させても経路を生成できない場合、演算部１５は、点Ｓと点Ｇを結ぶ線分上に配置する点の数を変えて、経路探索してもよい。なお、ステップＳ４０２の演算処理とステップＳ４０３と演算処理とは、順序を入れ替えて実行されてもよい。また、ステップＳ４０２の演算処理とステップＳ４０３と演算処理とは、所定のコスト関数に基づいて、同時に行われてもよい。

なお、上記各実施例において、演算部１５は、初期位置（初期姿勢）として与えられる点Ｓと目標位置（目標姿勢）として与えられる点Ｇとの少なくともいずれかが、ステップＳ３２において取得される搬送条件を満足するかを判定するステップ（図示せず）を実行してもよい。このとき、演算部１５は、さらに、点Ｓと点Ｇとの少なくともいずれかが搬送条件を満たさない場合には、搬送条件を満たす点Ｓまたは点Ｇを再設定するステップ（図示せず）を実行してもよい。これらのステップは、点Ｓと点Ｇとを接続する経路を生成または修正する処理の前に行われることが好ましい。

なお、これらのステップを含む経路生成処理は、例えば、バラ積みピッキングに適用すると好適である。すなわち、バラ積みピッキングにおいては、複数の対象物Ｗが、位置と姿勢とが不規則な状態でボックスに格納されているから、このステップを実行することにより、条件を満たす対象物Ｗを自動的に判別してピッキングすることができる。

＜実施例５＞
実施例５では、搬送条件に基づく経路生成処理について説明する。搬送条件は、例えば、対象物Ｗの取り得る姿勢を制約するための条件と、対象物Ｗの周辺環境情報に関する条件とを含む。対象物Ｗの取り得る姿勢を制約するための条件の一例として、ワーク座標系における軸回りの回転角度（例えば、ロール角、ピッチ角、ヨー角）の制限に関わる条件を挙げることができる。対象物Ｗの周辺環境情報に関する条件の一例として、対象物Ｗがその周辺の障害物と干渉しないための条件を挙げることができる。

なお、本実施例において、「コンフィギュレーション空間」とは、ロボット２０の動作の自由度によって定まる空間を意味し、対象物Ｗが存在する「実空間」とは区別する。また、本実施例では、説明の便宜上、対象物Ｗが存在する実空間内における対象物Ｗの取り得る姿勢を制約するための条件を「第１の条件」と呼び、ロボットの周辺環境情報に関する条件を「第２の条件」と呼ぶ。また、「第１の位置」として、点Ｓ（初期位置）を例示し、「第２の位置」として、点Ｇ（目標位置）を例示するが、これに限られるものではない。

図１２は、第１の位置（点Ｓ）及び第２の位置（点Ｇ）の計算を説明する説明図である。演算部１５は、コンフィギュレーション空間中の第１の位置（点Ｓ）と第２の位置（点Ｇ）との間に第１の条件を満たす経路Ｒが存在する可能性を高くするように、コンフィギュレーション空間中の第１の位置（点Ｓ）及び第２の位置（点Ｇ）を実空間の上の第１の位置、第２の位置から計算する。演算部１５は、例えば、コンフィギュレーション空間中の第１の位置（点Ｓ）及び第２の位置（点Ｇ）の距離が短くなる逆運動学の解の組み合わせを選択する。このような計算により、例えば、第１の位置（点Ｓ）及び第２の位置（点Ｇ）が第１の条件を満たしていても、第１の位置（点Ｓ）と第２の位置（点Ｇ）との間の如何なる経路も第１の条件を満たさないことがあるという不都合を解消できる。このような不都合が生じる原因は、実空間上の第１の位置（点Ｓ）及び第２の位置（点Ｇ）をコンフィギュレーション空間へ写像するための逆運動学の解が複数存在し得ることによる。このような場合、第１の位置（点Ｓ）と第２の位置（点Ｇ）との間に第１の条件を満たす経路を探しても解を求めることができない。なお、図１２に示すコンフィギュレーション空間は、Ｊ１〜Ｊ６の６自由度を有する場合を示している。

図１３は、一つ以上の中間点ｑ_ｎ，ｑ_ｎ＋１の計算を説明する説明図である。第１の位置（点Ｓ）から第２の位置（点Ｇ）へ至る経路上のｎ番目の中間点ｑ_ｎは、実空間の基準座標系に対するワーク座標系の相対座標として、または実空間の基準座標系に対するツール座標系の相対座標として、中間位置に関わる情報（ｘ，ｙ，ｚ）と、中間姿勢に関わる情報（α，β，γ）とを有する。例えば、ワーク座標系においては、中間点は、第１の位置（点Ｓ）から第２の位置（点Ｇ）へ至る経路上の対象物Ｗの中間位置及び中間姿勢に関わる情報を有する。ここで、ワーク座標系における中間位置とは、中間点ｑ_ｎにおける対象物Ｗの座標位置を示し、中間姿勢とは、中間点ｑ_ｎにおける対象物Ｗの傾き（例えば、ロール角、ピッチ角、ヨー角）を示す。また、例えば、ツール座標系においては、中間点は、第１の位置（点Ｓ）から第２の位置（点Ｇ）へ至る経路上のロボット２０の手先（例えば、エンドエフェクタ２８）の中間位置及び中間姿勢に関わる情報を有する。ここで、ツール座標系における中間位置とは、中間点ｑ_ｎにおけるロボット２０の手先（例えば、エンドエフェクタ２８）の座標位置を示し、中間姿勢とは、中間点ｑ_ｎにおけるロボット２０の手先（例えば、エンドエフェクタ２８）の傾き（例えば、ロール角、ピッチ角、ヨー角）を示す。ワーク座標系またはツール座標系の中間位置と中間姿勢の情報は、変換行列を用いることによって、基準座標系の中間位置と中間姿勢として表現する事も可能である。また、ロボット２０の手先（例えば、エンドエフェクタ２８）の中間位置及び中間姿勢は、対象物Ｗの中間位置及び中間姿勢から計算により求めることができる。演算部１５は、第１の位置（点Ｓ）と第２の位置（点Ｇ）との間を接続する一つ以上の中間点（ｑ_ｎ，ｑ_ｎ＋１）であって、第１の条件に加えて、第２の条件を満たす一つ以上の中間点（ｑ_ｎ，ｑ_ｎ＋１）を実空間内に計算する。コンフィギュレーション空間内に中間点を確率的に生成すると、実空間の第１の条件を満たさない中間点が生成される可能性が高い。このため、第１の条件を満たす中間点を実空間内で生成し、その生成された中間点が第２の条件をも満たしている場合に、これをコンフィギュレーション空間内の中間点として用いることにより、第１の条件及び第２の条件を満たす中間点を効率よく探索することができる。

演算部１５は、例えば、第１の条件を満たすある一つの中間点ｑ_ｎの中間姿勢に関わる情報（α，β，γ）を変更することなく、中間位置に関わる情報（ｘ，ｙ，ｚ）を変更することにより、第１の条件を満たすもう一つの中間点ｑ_ｎ＋１を実空間内に計算してもよい。このとき、中間点ｑ_ｎ＋１は、実空間の座標系を基準にした中間位置に関わる情報（ｘ＋ａ，ｙ+ｂ，ｚ＋ｃ）と、中間姿勢に関わる情報（α，β，γ）とを有する。このような計算方法によれば、第１の条件を満たすある一つの中間点ｑ_ｎから、第１の条件を満たすもう一つの中間点ｑ_ｎ＋１を簡単な計算で求めることができる。演算部１５は、例えば、ｘ，ｙ，ｚのそれぞれに乱数ａ，ｂ，ｃを加算することにより、ある一つの中間点の中間位置に関わる情報（ｘ，ｙ，ｚ）から、もう一つの中間点ｑ_ｎ＋１の中間位置に関わる情報（ｘ＋ａ，ｙ+ｂ，ｚ＋ｃ）を計算してもよい。演算部１５は、加算に替えて減算、除算、乗算、或いは関数演算を用いてもよい。なお、このようにして求めたもう一つの中間点ｑ_ｎ＋１が第２の条件を満たさない場合には、これを、第２の条件を満たす拘束空間へ射影してもよい。

演算部１５は、第１の位置（点Ｓ）から一つ以上の中間点（ｑ_ｎ，ｑ_ｎ＋１）を経由して第２の位置（点Ｇ）に至る経路を生成する。演算部１５は、このようにして生成された経路を、外部インタフェース１１を通じてロボット２０に出力する。

図１４は、中間点の生成処理のフローチャートの一例である。
まず、演算部１５は、第１の位置（点Ｓ）の位置情報（例えば、初期位置の情報）及び姿勢情報（例えば、初期姿勢の情報）を経路情報に登録する（ステップＳ５０１）。ここで、経路情報は、第１の位置（点Ｓ）から一つ以上の中間点（ｑ_ｎ，ｑ_ｎ＋１）を経由して第２の位置（点Ｇ）に至る経路の情報を意味する。

演算部１５は、ステップＳ５０１において経路情報に登録された位置情報及び姿勢情報を有する点を注視点として設定する（ステップＳ５０２）。「注視点」とは、後述のステップＳ５０３で生成される中間点の一つ前の中間点と同義である。ステップＳ５０３で生成される中間点が、例えば、ｑ_n+1である場合には、中間点ｑ_nが注視点として設定される。また、ステップＳ５０３で生成される中間点が、例えば、ｑ₁である場合には、第１の位置（点Ｓ）が注視点として設定される。

演算部１５は、注視点の位置情報（ｘ，ｙ，ｚ）のｘ，ｙ，ｚのそれぞれにａ，ｂ，ｃを加算し、位置情報（ｘ＋ａ，ｙ+ｂ，ｚ＋ｃ）と姿勢情報（α，β，γ）とを有する中間点を生成する（ステップＳ５０３）。この姿勢情報（α，β，γ）は、注視点の姿勢情報（α，β，γ）と同じであるものとする。

演算部１５は、ステップＳ５０３で生成された中間点が第１の条件に加えて第２の条件をも満たすようにその位置情報又は姿勢情報を補正する（ステップＳ５０４）。

演算部１５は、ステップＳ５０４で補正された中間点について逆運動学計算を行い、中間点の姿勢を計算する（ステップＳ５０５）。

演算部１５は、ステップＳ５０５における逆運動学計算が解を有するか否かを判定する（ステップＳ５０６）。

ステップＳ５０６の判定結果が「肯定判定」である場合には、演算部１５は、ステップＳ５０２で設定される注視点とステップＳ５０３で生成される中間点との間の経路が第１の条件及び第２の条件を満たすか否かを判定する（ステップＳ５０７）。

ステップＳ５０７の判定結果が「肯定判定」である場合には、演算部１５は、ステップＳ５０３で生成される中間点の位置情報及び姿勢情報を経路情報に登録する（ステップＳ５０８）。

ステップＳ５０８の登録処理の後に、演算部１５は、ステップＳ５０３で生成される中間点の位置情報及び姿勢情報が、第２の位置（点Ｇ）の位置情報（例えば、目標位置の情報）及び姿勢情報（例えば、目標姿勢の情報）に一致するか否かを判定する（ステップＳ５０９）。ステップＳ５０９の判定結果が「否定判定」である場合には、第１の位置（点Ｓ）から第２の位置（点Ｇ）に至る経路上の中間点の生成が不足しているため、演算部１５は、ステップＳ５０２の処理に戻る。

なお、ステップＳ５０６又はＳ５０７の判定結果が「否定判定」である場合には、演算部１５は、ステップＳ５０３の処理に戻る。

ロボット２０の効率的な経路計画を実現するために、演算部１５は、中間点（ｑ_ｎ，ｑ_ｎ＋１）を、第１の位置（点Ｓ）から第２の位置（点Ｇ）へ向かう方向に沿って計算してもよい。例えば、ｘ，ｙ，ｚのそれぞれにａ，ｂ，ｃを加算することにより、ある一つの中間点ｑ_ｎの中間位置に関わる情報（ｘ，ｙ，ｚ）から、もう一つの中間点ｑ_ｎ＋１の中間位置に関わる情報（ｘ＋ａ，ｙ+ｂ，ｚ＋ｃ）を計算する場合には、中間点ｑ_ｎ＋１が第２の位置（点Ｇ）へ向かうように、ａ，ｂ，ｃの値を調整すればよい。このように、もう一つの中間点ｑ_ｎ＋１の中間位置が第２の位置（点Ｇ）へ向かうように、ある一つの中間点ｑ_ｎの中間位置に関わる情報を変更する演算処理を、本明細書では、「バイアス処理」と呼ぶ。

図１５は、バイアス処理の一例を示す説明図である。ここでは、実施例３のポテンシャル法と同様に評価関数を用いる。この評価関数は、中間点からの第１の位置（点Ｓ）、第２の位置（点Ｇ）、及び障害物７１，７２までのそれぞれの距離に応じてポテンシャル値が変化する。例えば、第１の位置（点Ｓ）及び障害物７１，７２に斥力を設定し、第２の位置（点Ｇ）に引力を設定する。これにより、第１の位置（点Ｓ）又は障害物７１，７２からの距離が近いほど、評価関数のポテンシャル値は高くなり、第２の位置（点Ｇ）からの距離が近いほど評価関数のポテンシャル値は低くなる。演算部１５は、このような評価関数を用いて、評価関数のポテンシャル値が低くなるように、バイアス処理を行う。これにより、障害物７１，７２から離れて第２の位置（点Ｇ）へ向かうように、バイアス処理を行うことができる。

図１６は、バイアス処理の他の例を示す説明図である。この例では、障害物７１，７２の間の空間を任意の立体形状（例えば、立方体、直方体、球体など）に分割し、分割された立体形状を、第２の位置（点Ｇ）に向かう有向グラフ８０で連結する。演算部１５は、有向グラフ８０の向きに沿って中間点ｑ_ｎ＋１が生成されるようにバイアス処理を行う。これにより、障害物７１，７２に干渉せずに第２の位置（点Ｇ）へ向かうように、バイアス処理を行うことができる。なお、立体形状の種類やサイズは必ずしも同一である必要はなく、障害物７１，７２の形状に適合するように様々な種類又はサイズの立体形状を組み合わせてもよい。

なお、説明の便宜上、図面上は、中間点の個数が二つの場合を図示しているが、中間点の個数は三つ以上でもよい。

以上のとおり、本発明の所定の実施形態において、演算装置１０は、コンフィギュレーション空間全域から障害物領域Ｃｏだけでなく角度不適合領域Ｃｐを除いた領域内でロボット２０の経路を生成する。これにより、演算装置１０は、ロボット２０が対象物Ｗを初期姿勢から目標姿勢まで搬送するとき、障害物との干渉を回避しつつ、対象物Ｗの姿勢に関する条件を満たす経路でロボット２０を動作させることができる。したがって、演算装置１０は、ロボット２０が初期位置から目標位置まで搬送した対象物Ｗを次の工程で円滑に使用させることができる。実施例１、２及び４において、演算装置１０は、対象物Ｗの姿勢に関する条件を満たす中継位置を探索し、対象物Ｗを初期位置から中継位置を経由して目標位置に搬送する経路を生成する。実施例３及び４において、演算装置１０は、ロボット２０の周辺に存在する障害物に関する条件と対象物Ｗの姿勢に関する条件とを満たす所定の関数（評価関数やコスト関数等）を用いて、対象物Ｗを初期位置から目標位置に搬送する経路を生成する。

対象物Ｗの姿勢に関する条件は、ユーザが任意に設定又は変更可能である。これにより、演算装置１０は、ユーザの要望に応じた経路を生成することができる。

記憶部１２は、対象物Ｗの姿勢に関する条件の初期値を記憶しており、演算装置１０は、この初期値を対象物Ｗの姿勢に関する条件として取得してもよい。これにより、演算装置１０は、ロボット２０及び対象物Ｗが位置し得る領域内の環境及びロボット２０により初期位置から目標位置まで搬送される対象物Ｗの姿勢が満たすべき条件を満たす経路を生成することができる。

以上説明した実施形態は、本発明の理解を容易にするためのものであり、本発明を限定して解釈するためのものではない。実施形態が備える各要素並びにその配置、材料、条件、形状及びサイズ等は、例示したものに限定されるわけではなく適宜変更することができる。また、異なる実施形態で示した構成同士を部分的に置換し又は組み合わせることが可能である。上記実施形態はあらゆる点で単なる例示にすぎず、限定的に解釈されるものではない。例えば、上述の各処理ステップは処理内容に矛盾を生じない範囲で任意に順番を変更して又は並列に実行することができる。

また、上記の実施形態の一部又は全部は、以下の付記のようにも記載され得るが、以下には限られない。
（付記１）
少なくとも一つのハードウェアプロセッサによって、
複数の可動軸を有するロボットが、対象物を第１の位置から第２の位置に搬送する際の前記第１の位置及び前記第２の位置を取得するステップと、
前記ロボットが前記対象物を前記第１の位置から前記第２の位置に搬送するときの、前記対象物の姿勢に関する第１の条件を取得するステップと、
前記ロボットが前記対象物を前記第１の位置から前記第２の位置に搬送する経路を生成するステップであって、当該経路の全体にわたり、前記対象物の姿勢が前記取得された第１の条件を満たす経路を生成するステップと、
前記生成された経路を出力するステップと、
を実行する経路出力方法。

（付記２）
少なくとも一つのハードウェアプロセッサを備え、
複数の可動軸を有するロボットが、対象物を第１の位置から第２の位置に搬送する際の経路を出力するシステムであって、
前記ハードウェアプロセッサは、
前記第１の位置及び前記第２の位置と、前記ロボットが前記対象物を前記第１の位置から前記第２の位置に搬送するときの、前記対象物の姿勢に関する条件と、を取得し、
前記ロボットが前記対象物を前記第１の位置から前記第２の位置に搬送する経路であって、当該経路の全体にわたり、前記対象物の姿勢が前記取得された条件を満たす経路を生成し、
前記生成された経路を外部へ出力する、経路出力システム。

１…ロボットシステム、１０…演算装置、１１…外部インタフェース（取得部、出力部）、１２…記憶部、１３…ドライブ（取得部、出力部）、１４…記憶媒体、１５…演算部、１２１…初期条件記憶部、１２２…経路生成手順記憶部、１２３…経路修正手順記憶部、１２４…経路記憶部、１２５…経路出力手順記憶部、２０…ロボット、３０…撮像装置、４０…角度検出装置、５０…入力装置、６０…出力装置、Ｗ…対象物

Claims (5)

1. 出力部及び演算部を備える演算装置を用いて、複数の可動軸を有するロボットが対象物を搬送する際の経路を出力する方法であって、
   前記演算部が、
   前記対象物が存在する実空間内における前記対象物の取り得る姿勢を制約するための第１の条件を満たす第１の位置及び第２の位置を計算するステップと、
   前記第１の位置と前記第２の位置との間を接続する一つ以上の中間点であって、前記第１の条件に加えて、前記ロボットの周辺環境情報に関する第２の条件を満たす一つ以上の中間点を前記実空間内に計算するステップと、
   前記第１の位置から前記一つ以上の中間点を経由して前記第２の位置に至る経路を生成するステップと、
   前記出力部を用いて、前記生成された経路を出力するステップと、
   を実行し、
   前記中間点は、前記第１の位置から前記第２の位置へ至る経路上の前記対象物又は前記ロボットの手先の中間位置及び中間姿勢に関わる情報を含み、
   前記演算部は、前記一つ以上の中間点を前記実空間内に計算するステップにおいて、ある一つの中間点の前記中間姿勢に関わる情報を変更することなく、前記中間位置に関わる情報を変更することにより、前記第１の条件を満たすもう一つの中間点を前記実空間内に計算する、経路出力方法。
2. 前記演算部は、前記第１の位置及び前記第２の位置を計算するステップにおいて、前記第１の位置と前記第２の位置との間に前記第１の条件を満たす経路が存在する前記第１の位置及び前記第２の位置を計算する、請求項１に記載の経路出力方法。
3. 前記演算部は、前記一つ以上の中間点を前記実空間内に計算するステップにおいて、前記もう一つの中間点の前記中間位置が前記第２の位置へ向かうように、前記ある一つの中間点の前記中間位置に関わる情報を変更する、請求項１に記載の経路出力方法。
4. 前記演算部は、評価関数のポテンシャル値が低くなるように、前記ある一つの中間点の前記中間位置に関わる情報を変更し、
   前記評価関数は、前記第１の位置からの距離が近い程、前記評価関数のポテンシャル値は高くなり、前記第２の位置からの距離が近い程、前記評価関数のポテンシャル値は低くなる、請求項３に記載の経路出力方法。
5. 前記演算部は、前記もう一つの中間点の前記中間位置が、前記第２の位置に向かう有向グラフの向きに沿うように、前記ある一つの中間点の前記中間位置に関わる情報を変更する、請求項３に記載の経路出力方法。

Images