特許 7582448 動作計画装置、動作計画方法、及び、プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-11-05

(45)【発行日】2024-11-13

(54)【発明の名称】動作計画装置、動作計画方法、及び、プログラム

(51)【国際特許分類】

   B25J 13/00 20060101AFI20241106BHJP

【ＦＩ】

B25J13/00 Z

【請求項の数】 11

(21)【出願番号】P 2023508264

(86)(22)【出願日】2021-03-24

(86)【国際出願番号】 JP2021012275

(87)【国際公開番号】W WO2022201365

(87)【国際公開日】2022-09-29

【審査請求日】2023-09-13

(73)【特許権者】

【識別番号】000004237

【氏名又は名称】日本電気株式会社

(74)【代理人】

【識別番号】100107331

【弁理士】

【氏名又は名称】中村 聡延

(74)【代理人】

【識別番号】100104765

【弁理士】

【氏名又は名称】江上 達夫

(74)【代理人】

【識別番号】100131015

【弁理士】

【氏名又は名称】三輪 浩誉

(72)【発明者】

【氏名】向後 卓磨

【審査官】杉山 悟史

(56)【参考文献】

【文献】特表２０１８－５０５７８８（ＪＰ，Ａ）

【文献】特開２０１３－１８４２３３（ＪＰ，Ａ）

【文献】特許第６７０７４８５（ＪＰ，Ｂ２）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｂ２５Ｊ １／００ ～ ２１／０２

(57)【特許請求の範囲】

【請求項1】

１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび前記搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、前記ロボットが、前記搬送物体を把持して搬送しているときに、前記ロボットの前記被衝突回避物体周りの前記移動可能領域と搬送されている前記搬送物体の前記被衝突回避物体周りの前記移動可能領域とが一致するという第２条件とを設定する条件設定手段と、
前記第１条件と、前記第２条件とに基づき、前記ロボットの動作計画を算出する動作計画手段と、を有し
前記第１条件は、前記搬送物体が前記被衝突回避物体周りの前記移動可能領域の内部に位置するという第３条件を含み、
前記動作計画手段は、前記動作計画を算出する場合、前記搬送物体が前記ロボットにより搬送されていないときには、前記第３条件についての評価をスキップする動作計画装置。

【請求項2】

１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび前記搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、前記ロボットが、前記搬送物体を把持して搬送しているときに、前記ロボットの前記被衝突回避物体周りの前記移動可能領域と搬送されている前記搬送物体の前記被衝突回避物体周りの前記移動可能領域とが一致するという第２条件とを設定する条件設定手段と、
前記第１条件及び前記第２条件を含む制約条件と、前記ピックアンドプレースが完了するまでのタイムステップ数又は前記ロボットの移動距離の少なくとも一方に関する目的関数とに基づく最適化モデルの解を算出し、当該最適化モデルの解を前記ロボットの動作計画として出力する動作計画手段と、
を有する動作計画装置。

【請求項3】

前記搬送物体の初期位置と目標位置とに基づき、前記ロボットの予測通過確率が閾値よりも低い前記搬送物体周りの前記移動可能領域を、前記ロボットの経路とすることにペナルティがある制約経路として設定する、制約経路設定手段をさらに有し、
前記動作計画手段は、前記条件設定手段が設定した条件と、前記制約経路とに基づき、前記動作計画を算出する、請求項１または２に記載の動作計画装置。

【請求項4】

前記制約経路設定手段は、前記搬送物体の下方に位置する移動可能領域を、前記制約経路に含める、請求項３に記載の動作計画装置。

【請求項5】

前記制約経路設定手段は、水平面に対応する２つの座標軸において前記初期位置と前記目標位置との距離が短い座標軸の軸方向において前記搬送物体の両側に位置する移動可能領域を、前記制約経路に含める、請求項３または４に記載の動作計画装置。

【請求項6】

前記動作計画手段は、前記動作計画として、前記ロボットに関する軌道情報と、前記ロボットの操作に関する操作情報とを生成する、請求項１～５のいずれか一項に記載の動作計画装置。

【請求項7】

前記ロボットは、エンドエフェクタを有し、
前記動作計画手段は、前記エンドエフェクタを前記ロボットとみなして前記第１条件及び前記第２条件の判定を行う、請求項１～６のいずれか一項に記載の動作計画装置。

【請求項8】

コンピュータが、
１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび前記搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、前記ロボットが、前記搬送物体を把持して搬送しているときに、前記ロボットの前記被衝突回避物体周りの前記移動可能領域と搬送されている前記搬送物体の前記被衝突回避物体周りの前記移動可能領域とが一致するという第２条件とを設定し、
前記第１条件と、前記第２条件とに基づき、前記ロボットの動作計画を算出し、
前記第１条件は、前記搬送物体が前記被衝突回避物体周りの前記移動可能領域の内部に位置するという第３条件を含み、
前記動作計画を算出する場合、前記搬送物体が前記ロボットにより搬送されていないときには、前記第３条件についての評価をスキップする動作計画方法。

【請求項9】

１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび前記搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、前記ロボットが、前記搬送物体を把持して搬送しているときに、前記ロボットの前記被衝突回避物体周りの前記移動可能領域と搬送されている前記搬送物体の前記被衝突回避物体周りの前記移動可能領域とが一致するという第２条件とを設定し、
前記第１条件と、前記第２条件とに基づき、前記ロボットの動作計画を算出し、
前記第１条件は、前記搬送物体が前記被衝突回避物体周りの前記移動可能領域の内部に位置するという第３条件を含み、
前記動作計画を算出する場合、前記搬送物体が前記ロボットにより搬送されていないときには、前記第３条件についての評価をスキップする処理をコンピュータに実行させるプログラム。

【請求項10】

コンピュータが、
１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび前記搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、前記ロボットが、前記搬送物体を把持して搬送しているときに、前記ロボットの前記被衝突回避物体周りの前記移動可能領域と搬送されている前記搬送物体の前記被衝突回避物体周りの前記移動可能領域とが一致するという第２条件とを設定し、
前記第１条件及び前記第２条件を含む制約条件と、前記ピックアンドプレースが完了するまでのタイムステップ数又は前記ロボットの移動距離の少なくとも一方に関する目的関数とに基づく最適化モデルの解を算出し、当該最適化モデルの解を前記ロボットの動作計画として出力する動作計画方法。

【請求項11】

１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび前記搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、前記ロボットが、前記搬送物体を把持して搬送しているときに、前記ロボットの前記被衝突回避物体周りの前記移動可能領域と搬送されている前記搬送物体の前記被衝突回避物体周りの前記移動可能領域とが一致するという第２条件とを設定し、
前記第１条件及び前記第２条件を含む制約条件と、前記ピックアンドプレースが完了するまでのタイムステップ数又は前記ロボットの移動距離の少なくとも一方に関する目的関数とに基づく最適化モデルの解を算出し、当該最適化モデルの解を前記ロボットの動作計画として出力する処理をコンピュータに実行させるプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、ロボットの動作計画を算出する動作計画装置、動作計画方法及び記憶媒体に関する。

【背景技術】

【0002】

製品や部品などの物体（以降、搬送物体と記載する）のピックアンドプレースを、ロボットを用いて、自動化している工場や倉庫がある。ピックアンドプレースは、所定の位置にある搬送物体を取り上げ、搬送し、別の所定の位置に置く、という一連の動作である。

【0003】

非特許文献１に開示されているシステムは、双腕ロボットの２本のアームそれぞれが複数の搬送物体に対して同時にピックアンドプレースを実行する動作計画を算出する。より具体的には、非特許文献１に開示されているシステムは、ロボット及び搬送物体のダイナミクスと、各搬送物体についてピックアンドプレースが完了したか否かを判定する論理を、混合整数線形計画問題として定式化した最適化モデルを有し、最適化モデルに対して求解計算を実行することで得られる解を動作計画として出力する。

【0004】

非特許文献２に開示されているシステムは、UAV（Unmanned Aerial Vehicles）といった移動ロボットが障害物を回避しながら目的地まで移動する経路計画を算出する。より具体的には、非特許文献２に開示されているシステムは、障害物の周囲の領域群から移動ロボットが当該障害物と衝突しないものを移動経路として選択する制約条件を含む、混合整数線形計画問題として定式化した最適化モデルを有し、最適化モデルに対して求解計算を実行することで得られる解を経路計画として出力する。

【先行技術文献】

【非特許文献】

【0005】

【文献】M． Katayama、 et al、 “Fast LTL-Based Flexible Planning for Dual-Arm Manipulation”、IEEE/RS International Conference on Intelligent Robots and System、pp.6605-6612、2020．

【文献】M． Arantes、 et al、 “Collision-Free Encoding for Chance-Constrained Nonconvex Path Planning”、 IEEE Transactions on Robotics、vol.35，No.2、April、2019．

【発明の概要】

【発明が解決しようとする課題】

【0006】

仮に非特許文献１及び非特許文献２に基づき衝突回避を考慮したピックアンドプレースの動作計画を算出した場合、搬送物体の個数が増加するとその計算量が爆発的に増加してしまうため実用化が困難となる。

【0007】

本発明の目的の一つは、上述した課題を解決するために、衝突回避を考慮したピックアンドプレースの動作計画を好適に算出可能な動作計画装置、動作計画方法及び記憶媒体を提供することである。

【課題を解決するための手段】

【0008】

動作計画装置の一の態様は、
１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび前記搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、前記ロボットが、前記搬送物体を把持して搬送しているときに、前記ロボットの前記被衝突回避物体周りの前記移動可能領域と搬送されている前記搬送物体の前記被衝突回避物体周りの前記移動可能領域とが一致するという第２条件とを設定する条件設定手段と、
前記第１条件と、前記第２条件とに基づき、前記ロボットの動作計画を算出する動作計画手段と、を有し
前記第１条件は、前記搬送物体が前記被衝突回避物体周りの前記移動可能領域の内部に位置するという第３条件を含み、
前記動作計画手段は、前記動作計画を算出する場合、前記搬送物体が前記ロボットにより搬送されていないときには、前記第３条件についての評価をスキップする動作計画装置である。

【0009】

動作計画方法の一の態様は、
コンピュータが、
１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび前記搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、前記ロボットが、前記搬送物体を把持して搬送しているときに、前記ロボットの前記被衝突回避物体周りの前記移動可能領域と搬送されている前記搬送物体の前記被衝突回避物体周りの前記移動可能領域とが一致するという第２条件とを設定し、
前記第１条件と、前記第２条件とに基づき、前記ロボットの動作計画を算出し、
前記第１条件は、前記搬送物体が前記被衝突回避物体周りの前記移動可能領域の内部に位置するという第３条件を含み、
前記動作計画を算出する場合、前記搬送物体が前記ロボットにより搬送されていないときには、前記第３条件についての評価をスキップする動作計画方法である。なお、コンピュータは、複数の装置から構成されてもよい。

【0010】

プログラムの一の態様は、
１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび前記搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、前記ロボットが、前記搬送物体を把持して搬送しているときに、前記ロボットの前記被衝突回避物体周りの前記移動可能領域と搬送されている前記搬送物体の前記被衝突回避物体周りの前記移動可能領域とが一致するという第２条件とを設定し、
前記第１条件と、前記第２条件とに基づき、前記ロボットの動作計画を算出し、
前記第１条件は、前記搬送物体が前記被衝突回避物体周りの前記移動可能領域の内部に位置するという第３条件を含み、
前記動作計画を算出する場合、前記搬送物体が前記ロボットにより搬送されていないときには、前記第３条件についての評価をスキップする処理をコンピュータに実行させるプログラムである。

【発明の効果】

【0011】

衝突回避を考慮したピックアンドプレースの動作計画を好適に算出することができる。

【図面の簡単な説明】

【0012】

【図1】第１実施形態に係るロボット制御システム及び動作計画装置の第１構成例を示す概略ブロック図である。

【図2】第１実施形態に係るロボット制御システム及び動作計画装置の第２構成例を示す概略ブロック図である。

【図3】第１実施形態に係るロボット制御システム及び動作計画装置の第３構成例を示す概略ブロック図である。

【図4】第１実施形態に係る動作計画装置のハードウェア構成の一例である。

【図5】第１実施形態に係るロボット制御システムのハードウェア構成の一例である。

【図6】（Ａ）障害物周りの移動可能領域を示すｘ－ｙ平面図である。（Ｂ）移動可能領域の第１の例を示す。（Ｃ）移動可能領域の第２の例を示す。第１実施形態に係る被衝突回避物体周りの移動可能領域を示す図である。

【図7】（Ａ）エンドエフェクタが障害物の周りを移動する際の移動可能領域の切り替わりを表した第１の図である。（Ｂ）エンドエフェクタが障害物の周りを移動する際の移動可能領域の切り替わりを表した第２の図である。（Ｃ）エンドエフェクタが障害物の周りを移動する際の移動可能領域の切り替わりを表した第３の図である。

【図8】（Ａ）エンドエフェクタが搬送物体を搬送している状態での障害物周辺のｘ－ｚ平面図である。（Ｂ）エンドエフェクタが搬送物体を搬送している状態での障害物周辺のｘ－ｚ平面図である。

【図9】第１実施形態に係る動作計画の処理工程の例を示すフローチャートである。

【図10】（Ａ）第２実施形態に係るソフト経路制約の対象となる移動可能領域の一例を示すｘ－ｚ平面図である。（Ｂ）第２実施形態に係るソフト経路制約の対象となる移動可能領域の一例を示すｘ－ｙ平面図である。

【図11】第２実施形態に係る動作計画の処理工程の例を示すフローチャートである。

【図12】第３実施形態に係る動作計画装置の概略構成図を示す。

【図13】第３実施形態に係る動作計画の処理工程の例を示すフローチャートである。

【発明を実施するための形態】

【0013】

＜課題の説明＞
本願の発明者は、仮に非特許文献１及び非特許文献２に基づき衝突回避を考慮したピックアンドプレースの動作計画を算出した場合、搬送物体の個数が増加するとその計算量が爆発的に増加してしまい、実用化ができないという課題を見出した。

【0014】

この課題は、より詳細には、混合整数線形計画問題を解くことで動作計画を算出するが、その求解計算を困難にするバイナリ変数とbig-M法による制約条件のそれぞれの個数が搬送物体の個数の増加に応じて増加してしまうという課題である。一般的な混合整数線形計画問題において、バイナリ変数は、判定や論理をモデル化するために用いられる決定変数である。また一般的な混合整数線形計画問題において、big-M法は、連続値をバイナリ値に変換する際に用いられる制約条件の定式化技法である。特に、バイナリ変数およびbig-M法は、障害物との衝突回避において、移動体の位置座標（連続値）と、障害物と衝突しない領域にあるか否かの判定（バイナリ値）とを関係付けるために用いられる。すなわち、障害物との衝突回避においてバイナリ変数及びbig-M法による制約条件は必要不可欠であり、搬送物体の障害物との衝突回避にバイナリ変数およびbig-M法による制約条件を必要とする。

【0015】

この課題は、さらに詳細には、ロボットに把持されていないときの搬送物体は、障害物及びエンドエフェクタだけでなく他の搬送物体にも衝突しないように扱う必要があるため、搬送物体の個数が増えると取り扱う衝突回避に関するバイナリ変数とbig-M法による制約条件それぞれの個数が二乗のオーダーで増加してしまうという課題である。その結果として、「搬送物体の個数が増加すると、動作計画の求解において計算時間の爆発が生じる」という課題を発明者は見出した。

【0016】

発明者は、係る課題を見出すとともに、係る課題を解決する手段を導出するに至った。発明者は、搬送物体がエンドエフェクタに搬送されているときに搬送物体とそれを搬送するエンドエフェクタの移動可能領域を同一にすることで、衝突回避に係るバイナリ変数の個数を削減でき、その結果として、時間効率が高い衝突回避を考慮したピックアンドプレースの動作計画の算出を実現できることを見出した。ここで、「時間効率が高い」とは、動作計画を、その同程度の品質に対してより短い時間で算出することを表す。さらに、発明者は、ピックアンドプレースにおける移動経路の特性を利用して搬送物体の移動経路についてのソフト制約を追加することで、衝突回避に係るbig-M法による制約条件の個数を実質的に削減でき、その結果として、時間効率が高い衝突回避を考慮したピックアンドプレースの動作計画の算出を実現できることを見出した。

【0017】

以下、図面を参照しながら、動作計画装置、動作計画方法及び記憶媒体の実施形態について説明する。なお、以下の実施形態は請求の範囲に係る発明を限定するものではない。また、実施形態の中で説明されている特徴の組み合わせの全てが発明の解決手段に必須であるとは限らない。

【0018】

＜第１実施形態＞
図１を参照しながら、第１実施形態の動作計画装置１０を含むロボット制御システム１が有する構成の一例を詳細に説明する。図１は、第１実施形態におけるロボット制御システム１の第１構成例を示す概略ブロック図である。

【0019】

ロボット制御システム１は、大別して、動作計画装置１０と、制御装置２０と、設定記憶装置３０と、計測装置４０と、ロボット５０とを備える。動作計画装置１０は動作計画部１００を有する。制御装置２０は制御部２００を有する。設定記憶装置３０は設定記憶部３００を有する。

【0020】

動作計画装置１０は、制御装置２０、設定記憶装置３０、及び、計測装置４０と、通信回線を介して通信可能に接続されている。さらに制御装置２０は、設定記憶装置３０、計測装置４０、及び、ロボット５０と、通信回線を介して通信可能に接続されている。通信回線は、例えば、専用線、インターネット、ＶＰＮ（Virtual Private Network ）、ＬＡＮ（Local Area Network）、ＵＳＢ（Universal Serial Bus）、Ｗｉ－Ｆｉ（登録商標）、Ｂｌｕｅ Ｔｏｏｔｈ（登録商標）などが挙げれる。なお、通信回線の占有形態及び有線回線、無線回線などの通信回線の物理形態などの通信回線の形態は問わず、任意の形態により構成されていてもよい。

【0021】

動作計画装置１０は、所望のピックアンドプレースを実現する動作計画情報を算出する。より具体的には、動作計画部１００は、設定記憶部３００から計画設定情報を読み込み、計測装置４０から計測情報を読み込み、読み込んだこれらを用いて動作計画情報を算出する。動作計画部１００は、算出した動作計画情報を制御部２００に送信する。動作計画情報は、どの時刻にロボット５０の代表点がどこに位置するかを表す軌道情報と、どの時刻にロボット５０がどの操作を行うかを表す操作情報とによって構成される。ここで、ロボット５０の操作とは、具体的には、搬送物体のピック動作やプレース動作であり、単なる移動動作だけでなくエンドエフェクタの制御を含むロボット５０の動作である。動作計画情報の算出方法、軌道情報、操作情報、計画設定情報、計測情報の詳細については後述する。動作計画装置１０は、例えば、パーソナルコンピュータ、ワークステーション等のコンピュータで実現される。

【0022】

制御装置２０は、動作計画部１００が算出した動作計画を実現するロボット５０の制御を行うための制御指令情報を算出し、算出した制御指令情報をロボット５０に送信する。より具体的には、制御部２００は、設定記憶部３００から制御設定情報を読み込み、計測装置４０から計測情報を読み込み、ロボット５０からロボット状態情報を読み込み、動作計画部１００から動作計画情報を読み込む。そして、制御部２００は、読み込んだこれらの情報を用いてロボット５０の制御指令情報を算出し、算出した制御指令情報をロボット５０に送信する。制御部２００は、制御指令情報を算出する場合、軌道情報と操作情報をロボット５０の各駆動部の出力に対応付けるよう変換しつつ、同時に時間的に内挿して動作計画情報よりも時間解像度が高くなるようにアップサンプリング変換も行う。ロボット状態情報は、例えば、ロボットの各駆動部の状態（位置、角度等）やロボットの各駆動部に対する制御シーケンスの進捗状態などがある。制御設定情報は、例えば、ロボットの各駆動部の出力の上限値及び下限値を示す情報を含む。制御装置２０は、例えば、パーソナルコンピュータ、ワークステーション等のコンピュータにより実現される。

【0023】

設定記憶装置３０の設定記憶部３００は、計画設定情報及び制御設定情報を記憶する。計画設定情報及び制御設定情報は、ユーザインターフェースを介してユーザが入力することで生成された情報であってもよい。ユーザインターフェースは、パーソナルコンピュータ、ワークステーション、タブレット、スマートフォン等のコンピュータ、または、キーボード、マウス、タッチパネルディスプレイ、音声入力装置等の入力デバイスである。ただし、ユーザインターフェースは、これらの例に限定されない。設定記憶装置３０は、例えば、パーソナルコンピュータ、ワークステーション等のコンピュータで実現される。

【0024】

計測装置４０は、搬送物体の位置と姿勢をセンシング及び推定し、当該センシング及び推定結果を計測情報として出力する。計測装置４０は、例えば、ステレオカメラ、パターンプロジェクションカメラ、ＬＩＤＡＲ（Laser Imaging Detection and Ranging）等のセンシングデバイス、または、これらセンシングデバイスと対象物体の位置と姿勢を推定する後処理装置とが一体化したセンシングシステムである。ただし、計測装置４０はこれらの例に限定されず、種々の外界センサを含む構成であってもよい。

【0025】

ロボット５０は、制御指令情報に従って駆動することで搬送物体を把持して所定の搬送先に移動させることができるロボットであり、例えば、多関節ロボット、双腕ロボット、スカラロボット、パラレルロボット等である。ロボット５０の先端部には搬送物体を把持するデバイスであるエンドエフェクタが備えられており、エンドエフェクタは、例えば、２爪グリッパ、３爪グリッパ、真空吸着ハンド、磁力吸着ハンド、汎用ハンド等である。ロボット５０及びエンドエフェクタはこれらの例に限定されず、種々の構成を有してもよい。

【0026】

ロボット制御システム１は図１に示されている構成に限定されず、種々の変更が行われてもよい。図２は、第１実施形態の第２構成例に係るロボット制御システム１ａの概略ブロック図である。図２に示す第２構成例では、動作計画装置１０ａは、制御部２００を有し、ロボット５０に直接的に制御指令情報を送信する。図３は、第１実施形態の第３構成例に係るロボット制御システム１ｂの概略ブロック図である。図３に示す第３構成例では、動作計画装置１０ｂは、制御部２００と設定記憶部３００を有し、ロボット５０に直接的に制御指令情報を送信し、かつ、計画設定情報と制御設定情報を記憶する。

【0027】

図４は、動作計画装置１０のハードウェア構成の一例を示す。動作計画装置１０は、ハードウェアとして、プロセッサ１１と、メモリ１２と、インターフェース１３とを含む。プロセッサ１１、メモリ１２、及びインターフェース１３は、データバス１９を介して接続されている。

【0028】

プロセッサ１１は、メモリ１２に記憶されているプログラムを実行することにより、動作計画装置１０の全体の制御を行うコントローラ（演算装置）として機能する。プロセッサ１１は、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＴＰＵ（Ｔｅｎｓｏｒ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などのプロセッサである。プロセッサ１１は、複数のプロセッサから構成されてもよい。プロセッサ１１は、コンピュータの一例である。プロセッサ１１は、例えば、図１に示す動作計画装置１０における動作計画部１００、又は、図２若しくは図３に示す動作計画装置１０ａ、１０ｂにおける動作計画部１００及び制御部２００として機能する。

【0029】

メモリ１２は、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フラッシュメモリなどの各種の揮発性メモリ及び不揮発性メモリにより構成される。また、メモリ１２には、動作計画装置１０が実行する処理を実行するためのプログラムが記憶される。なお、メモリ１２が記憶する情報の一部は、動作計画装置１０と通信可能な１又は複数の外部記憶装置により記憶されてもよく、動作計画装置１０に対して着脱自在な記憶媒体により記憶されてもよい。メモリ１２は、例えば、図３に示す動作計画装置１０ｂにおける設定記憶部３００として機能する。

【0030】

インターフェース１３は、動作計画装置１０と他の装置（制御装置２０、設定記憶装置３０、計測装置４０、ロボット５０）とを電気的に接続するためのインターフェースである。これらのインターフェースは、他の装置とデータの送受信を無線により行うためのネットワークアダプタなどのワイアレスインタフェースであってもよく、他の装置とケーブル等により接続するためのハードウェアインターフェースであってもよい。なお、図２に示す動作計画装置１０ａ及び図３に示す動作計画装置１０ｂについても図４に示すハードウェア構成を有する。

【0031】

図５は、ロボット制御システム１のハードウェア構成の一例である。作業台７１には、多関節ロボット５１が取り付けられている。多関節ロボット５１の先端部には、エンドエフェクタとして２爪グリッパ５０１が装着されている。位置姿勢推定カメラシステム４１は、作業台の側の支柱７４に取り付けられている。位置姿勢推定カメラシステム４１は、作業台７１上に置かれた搬送物体６１の位置と姿勢を計測及び推定する。搬送物体６１は、作業台７１上に置かれた状態から、作業台７１の横に設置された棚板７２、棚板８１、棚柱７３で構成される棚の棚板７２上または棚板８１上に置かれた状態となるように、多関節ロボット５１によってピックアンドプレースされる。仮想障害物８２は、物理的な実体を伴わない障害物であり、エンドエフェクタ５０１が侵入してはならない領域として設定される。ここでは、仮想障害物８２は、多関節ロボット５１の自己接触を防ぐために設けられている。図５には、動作計画装置１０、制御装置２０、設定記憶装置３０が明示されていないが、これらは作業台７１の横側、内部、または離れた場所に設置される。ワールド座標系９１は、図５中のロボット制御システム１のワールド座標の軸方向を表している。ここでは、一例として、水平方向に沿ってｘ軸及びｙ軸が設けられ、高さ方向に沿ってｚ軸が設けられている。

【0032】

次に、動作計画部１００が使用する計画設定情報、動作計画情報及び計測情報の算出（生成）方法について、引き続き図５を参照しながら説明する。はじめに動作計画情報の算出方法を説明し、その後計画設定情報、計測情報の順にその算出方法を説明する。

【0033】

動作計画部１００は、一連のピックアンドプレースを混合整数線形計画問題に定式化した最適化モデルを用いて動作計画情報を算出する。より詳細には、動作計画部１００は、最適化モデルを生成した後に、最適化ソルバーを用いて最適化モデルの求解計算を行うことで最適化モデルの解を算出し、最後に、得られた解に対して後処理を行い動作計画情報に変換する、という一連の処理を行う。

【0034】

最適化ソルバーは、混合整数線形計画問題を求解可能なものであればいずれでもよい。例えば、連続緩和と線形計画法を用いる分岐限定法、または、切除平面法、局所探索法、メタヒューリスティクス等と組み合わせた分岐限定法が好適であるが、これらの例に限定されない。

【0035】

最適化モデルは、全ての対象とする搬送物体についてピックアンドプレースが完了する時間ステップ数を最小にしつつ、その中でエンドエフェクタの移動距離を最小にする目的関数を有する。目的関数はこれの例に限定されず、望ましいピックアンドプレースとなるようなものであればよい。例えば、目的関数は、上述の時間ステップ数、又は、エンドエフェクタの移動距離の一方のみを最小化する目的関数であってもよい。

【0036】

また、最適化モデルは、エンドエフェクタ及び搬送物体それぞれの移動についてのダイナミクスをハイブリッドシステムとしてモデル化した制約条件を有する。この制約条件は、エンドエフェクタに把持されて搬送されているときには搬送物体がエンドエフェクタと一体となって移動を行い、エンドエフェクタに把持されていないときには搬送物体は移動せずに同じ位置に居続けるということをモデル化したものである。

【0037】

さらに、最適化モデルは、エンドエフェクタと障害物間、搬送物体と障害物間、エンドエフェクタと搬送物体間、搬送物体と他の搬送物体間の衝突回避についてモデル化した制約条件を有する。この制約条件は、エンドエフェクタなどの衝突回避物体と障害物などの被衝突回避物体（衝突回避物体が衝突を回避すべき物体）が満たすべき条件であり、予め定義した被衝突回避物体の周囲の移動可能領域を衝突回避物体が被衝突回避物体と衝突することなく移動する条件をモデル化したものである。

【0038】

他にも、最適化モデルは、上述の目的関数及び制約条件の定式化に補助的に必要な制約条件を有する。

【0039】

上述の最適化モデルについて、より詳細な一例を示す。この最適化モデルでは、多関節ロボットについては、エンドエフェクタの部分のみをモデル化の対象とする。また、衝突回避すべき障害物として定める物体を必要最小限にするように、エンドエフェクタ５０１の可動領域をユーザが設定することができるものとする。例えば、図５のハードウェア構成では、作業台７１、棚板７２、棚柱７３、支柱７４、位置姿勢推定カメラシステム４１を、エンドエフェクタ５０１の可動領域の内部に位置しないように設定することで、衝突回避すべき障害物を、棚板８１と仮想障害物８２に限定することができる。さらに、モデル化の対象となるエンドエフェクタ、搬送物体、障害物、エンドエフェクタの可動領域、被衝突回避物体周りの移動可能領域は、軸平行な直方体（AABB：Axis Aligned Bounding Box）としてモデル化されるものとする。エンドエフェクタ、搬送物体、障害物の位置はそれぞれそれらに対応する直方体の重心とする。エンドエフェクタは搬送物体の上方からアプローチして把持を行い、搬送状態においてはエンドエフェクタの下面と搬送物体の上面が接触した状態になるものとする。

【0040】

また、この最適化モデルの例では、以下の（ａ）～（ｋ）に示される定数、係数、変数、記号、表記を用いる。バイナリ変数については、値０と１それぞれが対応する状態を括弧内に記している。なお、以後において、これらの記号等を文中で使用する場合には、表示の簡潔化のため、適宜下付きのインデックスを省略する。

【0041】

（ａ）補助変数：インデックス番号

【0042】

【数1】

【0043】

（ｂ）定数・係数：個数

【0044】

【数2】

【0045】

（ｃ）定数・係数：初期値・目標値

【0046】

【数3】

【0047】

（ｄ）定数・係数：上限値・下限値

【0048】

【数4】

【0049】

（ｅ）定数・係数：形状

【0050】

【数5】

（ｆ）定数・係数：距離

【0051】

【数6】

【0052】

（ｇ）定数・係数：その他

【0053】

【数7】

【0054】

（ｈ）決定変数：バイナリ値

【0055】

【数8】

【0056】

（ｉ）決定変数：連続値

【0057】

【数9】

【0058】

【数10】

【0059】

【数11】

【0060】

（ｊ）記号

【0061】

【数12】

【0062】

（ｋ）表記

【0063】

【数13】

【0064】

各記号の混合整数線形計画問題における同値となる制約条件への定式化は、以下の式（１）～（３）で表される。

【0065】

【数14】

【0066】

各インデックス番号を表す補助変数の取りうる値は、式（４）～（８）で表される。

【0067】

【数15】

【0068】

各連続変数の上下限範囲に関する制約条件は、式（９）～（１８）で表される。式（９）は、エンドエフェクタの可動領域を表している。

【0069】

【数16】

【0070】

【数17】

【0071】

エンドエフェクタの位置、速度および搬送物体の位置の初期値に関する制約条件は、式（１９）～（２１）で表される。

【0072】

【数18】

【0073】

「同時に２個以上の搬送物体が把持状態になってはいけない」という制約条件は、式（２２）で表される。

【0074】

【数19】

【0075】

「搬送物体のエンドエフェクタによる把持開始状態は非搬送状態から搬送状態へ遷移したときと一致する」という制約条件は、式（２３）で表される。把持開始状態は、エンドエフェクタがアプローチ点から搬送物体に対してピック動作を行い、ピック動作後にアプローチ点に戻る一連の動作状態である。

【0076】

【数20】

【0077】

「搬送物体のエンドエフェクタによる把持終了状態は搬送状態から非搬送状態へ遷移したときと一致する」という制約条件は、式（２４）で表される。把持終了状態は、エンドエフェクタがアプローチ点から搬送物体に対してプレース動作を行い、プレース動作後にアプローチ点に戻る一連の動作状態である。

【0078】

【数21】

【0079】

「搬送物体のエンドエフェクタによる搬送状態は把持開始状態を除いた把持状態である」という制約条件は、式（２５）で表される。つまり、把持状態は、把持開始状態または搬送状態である。

【0080】

【数22】

【0081】

「搬送物体の把持開始状態、把持終了状態、搬送状態それぞれの初期値は０である」という制約条件は、式（２６）で表される。

【0082】

【数23】

【0083】

「エンドエフェクタの位置は、時間ステップが１つ前のエンドエフェクタの位置と速度によって更新される」という制約条件は、式（２７）で表される。

【0084】

【数24】

【0085】

「非把持状態のときの搬送物体の位置は、時間ステップが１つ前の搬送物体の位置と一致する」という制約条件は、式（２８）及び式（２９）で表される。

【0086】

【数25】

【0087】

「搬送状態のときの搬送物体の位置は、エンドエフェクタの位置から一定距離だけ離れた位置と一致する」という制約条件は、式（３０）及び式（３１）で表される。

【0088】

【数26】

ここで、一定距離だけ離れた位置は、エンドエフェクタと搬送物体が把持により密着する位置となる。ｂ^ｏｎは、搬送状態のときのエンドエフェクタと搬送物体との距離であり、後述する式（５４）により定められる。

【0089】

「把持開始状態および把持終了状態のときの搬送物体の位置は、一定距離だけ離れた位置と一致する」という制約条件は、式（３２）及び式（３３）で表される。

【0090】

【数27】

【0091】

ここで、一定距離だけ離れた位置は、把持開始状態のときはエンドエフェクタが搬送物体をピックする前のアプローチ位置となり、把持終了状態のときはエンドエフェクタが搬送物体をプレースした後の戻り位置となる。ｂ^ｏｆｆは、把持開始状態および把持終了状態のときのエンドエフェクタと搬送物体との距離であり、後述する式（５５）により定められる。

【0092】

「搬送物体の目標位置到達状態は、搬送物体の位置がその目標値と一致する」という制約条件は、式（３４）及び式（３５）で表される。以降では、「目標位置」は位置の目標値（即ち、目標となる位置の座標）を指すものとする。

【0093】

【数28】

【0094】

「搬送物体が非把持状態のときのみに目標位置到達状態となりうる」という制約条件は、式（３６）で表される。

【0095】

【数29】

【0096】

「ピックアンドプレースの完了状態は、現在から将来の全時間ステップに渡って全ての搬送物体が目標位置到達状態となっている」という制約条件は、式（３７）で表される。

【0097】

【数30】

【0098】

「最終的にはピックアンドプレースの完了状態となる」という制約条件は、式（３８）で表される。

【0099】

【数31】

【0100】

続いて、移動可能領域に関する制約条件について説明を行う前に、図６（Ａ）～図６（Ｃ）と図７（Ａ）～図７（Ｃ）を用いて移動可能領域について説明する。

【0101】

図６（Ａ）は、障害物８０周りの移動可能領域を示すｘ－ｙ平面図である。図６（Ａ）では、直方体によりモデル化された障害物８０の各面に沿った仮想面８００を明示している。移動可能領域は、軸平行な直方体（AABB）としてモデル化される。図６（Ｂ）は、移動可能領域８１０の第１の例を示し、図６（Ｃ）は、移動可能領域８１０の第２の例を示すｘ－ｙ平面図である。図６（Ｂ）及び図６（Ｃ）では、移動可能領域８１０がハッチングされている。図６（Ｂ）及び図６（Ｃ）に示すように、移動可能領域８１０は、仮想面８００により区切られた領域であり、具体的には、仮想面８００の外側の半空間に相当する。３次元空間の場合、被衝突回避物体（ここでは障害物８０）周りの移動可能領域は、６個の仮想面８００に対応する６個の半空間の領域となる。なお、移動可能領域８１０は、実際には、衝突回避物体（エンドエフェクタまたは搬送物体）の可動領域との積集合で表される有限な領域となる。

【0102】

図７（Ａ）～図７（Ｃ）は、エンドエフェクタ５００が障害物の周りを移動する際の移動可能領域の切り替わりを表したｘ－ｙ平面図の一例である。図７（Ａ）に示すように、エンドエフェクタ５００は１つの移動可能領域８１０内部に位置している。図７（Ａ）において、エンドエフェクタ５００はこの移動可能領域８１０内部を矢印８３Ａの方向に移動すると、エンドエフェクタ５００は２つの移動可能領域８１０内部に位置するようになり、図７（Ｂ）に示す状態に至る。その後、図７（Ｂ）の状態において、エンドエフェクタ５００は新たに位置するようになった移動可能領域８１０内部を矢印８３Ｂの方向に移動すると、エンドエフェクタ５００は移動前とは異なる２つの移動可能領域８１０内部に位置するようになる。その後、エンドエフェクタ５００は、図７（Ｃ）に示す状態に至る。そして、エンドエフェクタ５００は、さらに矢印８３Ｃの方向に移動する。

【0103】

このように衝突回避物体（図７（Ａ）～図７（Ｃ）の例ではエンドエフェクタ５００）は移動可能領域を切り替えながら移動することで、被衝突回避物体（図７（Ａ）～図７（Ｃ）の例では障害物５０）との衝突を回避しながら移動することができる。ただし、移動可能領域の切り替え前と切り替え後の連続する２つの時間ステップにおいて少なくも１個以上の移動可能領域に衝突回避物体が位置している必要がある。

【0104】

次に、移動可能領域に関する制約条件について説明する。「エンドエフェクタが、障害物周りの移動可能領域に位置している」という制約条件は、式（３９）及び式（４０）で表される。

【0105】

【数32】

【0106】

「エンドエフェクタが、搬送物体周りの移動可能領域に位置している」という制約条件は、式（４１）及び式（４２）で表される。

【0107】

【数33】

【0108】

「搬送物体が、障害物周りの移動可能領域に位置している」という制約条件は、式（４３）及び式（４４）で表される。

【0109】

【数34】

【0110】

【数35】

【0111】

「搬送物体が、別の搬送物体周りの移動可能領域に位置している」という制約条件は、式（４５）及び式（４６）で表される。

【0112】

【数36】

【0113】

【数37】

【0114】

「エンドエフェクタは、連続する２つの時間ステップにおいて、少なくとも１以上の障害物周りの１個以上の移動可能領域の内部に位置する」という制約条件は、式（４７）で表される。

【数38】

【0115】

「エンドエフェクタは、連続する２つの時間ステップにおいて、少なくとも搬送物体周りの１個以上の移動可能領域の内部に位置する」という制約条件は、式（４８）で表される。

【0116】

【数39】

なお、上述した式（４７）及び式（４８）は、１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという制約条件に相当し、本開示における「第１条件」の一例である。

【0117】

また、本実施形態では、搬送物体が搬送状態のときに、搬送物体とそれを搬送するエンドエフェクタの移動可能領域を同一にする。これにより、動作計画部１００は、決定変数φ^{ｄｌｖ，ｏｂｓ}、φ^{ｄｌｖ，ｄｌｖ}をバイナリ変数ではなく連続変数にし、かつ、「搬送物体が被衝突回避物体周りの移動可能領域の内部に位置する」という制約条件を満たすように動作計画を算出する。「搬送物体が被衝突回避物体周りの移動可能領域の内部に位置する」という制約条件は、本開示における第３条件の一例である。

【0118】

搬送物体が搬送状態のときに、搬送物体とそれを搬送するエンドエフェクタの移動可能領域を同一にすることの作用効果について補足説明する。比較例として、搬送物体と障害物周りの移動可能領域、及び、ある搬送物体とその他の搬送物体の移動可能領域周りの移動可能領域について、式（４７）及び式（４８）と同様の制約条件（「比較例に係る制約条件」とも呼ぶ。）を追加し、それらの衝突回避をモデル化する例も考えられる。一方、この場合、決定変数φ^{ｄｌｖ，ｏｂｓ}、φ^{ｄｌｖ，ｄｌｖ}はバイナリ変数であるから、搬送物体の個数の増加によってバイナリ変数φ^{ｄｌｖ，ｏｂｓ}、φ^{ｄｌｖ，ｄｌｖ}の個数が増加することになる。これに対し、本実施形態では、搬送物体が搬送状態のときに搬送物体とそれを搬送するエンドエフェクタの移動可能領域を同一にするという工夫を行うことで、決定変数φ^{ｄｌｖ，ｏｂｓ}、φ^{ｄｌｖ，ｄｌｖ}をバイナリ変数ではなく、連続変数にする。これにより本来であれば搬送物体の個数の増加によって増加するバイナリ変数φ^{ｄｌｖ，ｏｂｓ}、φ^{ｄｌｖ，ｄｌｖ}の個数を０にすることができる。決定変数φ^{ｄｌｖ，ｏｂｓ}、φ^{ｄｌｖ，ｄｌｖ}を連続変数にできるのは、後述の制約条件である式（４９）及び式（５０）によってバイナリ変数φ^{ｅｅ，ｏｂｓ}、φ^{ｅｅ，ｄｌｖ}のバイナリ値を決定変数φ^{ｄｌｖ，ｏｂｓ}、φ^{ｄｌｖ，ｄｌｖ}に選択的に伝搬させることができるためである。

【0119】

図８（Ａ）及び図８（Ｂ）を用いて、搬送物体が搬送状態のときに搬送物体とそれを搬送するエンドエフェクタの移動可能領域を同一にすることについて、さらに詳細に説明する。図８（Ａ）は、エンドエフェクタ５００が搬送物体６０を搬送している状態での障害物８０周辺のｘ－ｚ平面図であり、図８（Ｂ）は、エンドエフェクタ５０１が搬送物体６０を搬送していない状態での障害物８０周辺のｘ－ｚ平面図である。

【0120】

図８（Ａ）に示すように、搬送物体６０の障害物８０周りの移動可能領域８１０は、搬送状態のときに限り、エンドエフェクタ５００の障害物８０周りの移動可能領域８１０と同一となる。一方、図８（Ｂ）に示すように、搬送物体６０が搬送状態でないときは、障害物８０周りの移動可能領域についての判定は考慮する必要がない。なぜならば、搬送物体６０について衝突が発生しうるのは搬送状態のときのみであり、搬送物体６０はピック前およびプレース後には他の被衝突回避物体と衝突しない位置に存在するためである。

【0121】

以上を勘案し、動作計画部１００は、「搬送物体６０の障害物８０周りの移動可能領域８１０は、搬送状態のときに限り、エンドエフェクタ５００の障害物８０周りの移動可能領域８１０と同一となる」に対応する条件として、後述の式（４９）及び式（５０）で表される制約条件を追加する。一方、動作計画部１００は、「搬送物体６０が搬送状態でないときは、障害物８０周りの移動可能領域についての判定は考慮する必要がない」という条件を反映するため、式（４７）及び式（４８）に相当する、比較例に係る制約条件（即ち、搬送物体が被衝突回避物体周りの移動可能領域の内部に位置するという制約条件）を不要とする。言い換えると、動作計画部１００は、動作計画を算出する場合、搬送物体がロボット（ここではエンドエフェクタ）により搬送されていないときには、搬送物体が被衝突回避物体周りの移動可能領域の内部に位置するという制約条件についての評価をスキップする。この態様により、動作計画部１００は、「搬送物体が被衝突回避物体周りの移動可能領域の内部に位置する」という制約条件を満たすように動作計画を算出することが可能である。

【0122】

再び最適化モデルにおける制約条件について説明する。「搬送物体の障害物周りの移動可能領域は、搬送状態のときにエンドエフェクタの障害物周りの移動可能領域と一致する」という制約条件は、式（４９）で表される。

【0123】

【数40】

【0124】

「搬送物体の他の搬送物体周りの移動可能領域は、搬送状態のときにエンドエフェクタの他の搬送物体周りの移動可能領域と一致する」という制約条件は、式（５０）で表される。

【0125】

【数41】

式（４９）及び式（５０）は、ロボットが、搬送物体を把持して搬送しているときに、ロボットの被衝突回避物体周りの移動可能領域と搬送されている搬送物体の被衝突回避物体周りの移動可能領域とが一致するという制約条件に相当し、本開示における「第２条件」の一例である。

【0126】

「搬送物体は、搬送物体自身周りの移動可能領域には位置できない」という制約条件は、式（５１）で表される。

【0127】

【数42】

【0128】

エンドエフェクタの速度をその絶対値に変換するための制約条件は、式（５２）及び（５３）で表される。より正確には、目的関数におけるｖ^ｅｅの絶対値（｜ｖ^ｅｅ｜）の最小化を、υ^ｅｅの最小化と式（５２）及び式（５３）で表される制約条件の組合せにより実現する。

【0129】

【数43】

【0130】

把持操作におけるエンドエフェクタと搬送物体の距離ｂ^ｏｎ、ｂ^ｏｆｆの値は、それぞれ式（５４）及び式（５５）で定めることができる。

【0131】

【数44】

ここで、「ｉ＝Ｎ_ｄｉｍ」はｚ成分と対応し、「ｉ＜Ｎ_ｄｉｍ」はｘ、ｙ成分と対応する。すなわち、把持操作におけるエンドエフェクタと搬送物体のそれぞれの重心位置の距離は、ｘ、ｙ成分では０となり、ｚ成分では、エンドエフェクタと搬送物体がぴったり密着する大きさ、または、エンドエフェクタと搬送物体がぴったり密着する大きさからさらにマージンを加えた大きさとなる。

【0132】

目的関数は、式（５６）のように表される。

【0133】

【数45】

ここで、式（５６）の第１項は、ピックアンドプレースが完了状態になるのに要する時間ステップ数（所要時間長）を０～１の値域となるように正規化した値に対応しており、０に近いほど早く完了状態となることに対応し、１に近いほど遅く完了状態になることに対応する。第２項はエンドエフェクタのＬ１ノルムの意味での総移動距離に対応している。第２項の重み係数εを十分に小さい正の実数とすることで、目的関数の第１項、第２項の順に最小化の優先順位付けをすることができる。

【0134】

以上が、最適化モデルの定義の一例である。動作計画部１００は、上述の最適化モデルを生成し、最適化モデルに対して求解計算を行うことで、全ての決定変数を要素に持つベクトルＸに対する解Ｘ^＊（例えば最適解）を得る。以後では、解Ｘ^＊の各要素となる決定変数について”^＊”を付して表記する。以上が、動作計画部１００による動作計画情報の算出方法の一例である。

【0135】

次に、動作計画情報について説明する。動作計画部１００は、解Ｘ^＊から動作計画情報を抽出する。具体的には、解Ｘ^＊のうち、ｐ^ｅｅ＊を軌道情報として、θ^ｐｃｋ＊、θ^ｐｌｃ＊、θ^ｃｒｙ＊を操作情報として抽出し、これらを合わせて動作計画情報として出力する。「∨θ^ｐｃｋ＊」が１であることは、当該時間ステップにおいてエンドエフェクタが把持操作を開始して搬送物体を取り上げることに対応する。「∨θ^ｐｌｃ＊」が１であることは、当該時間ステップにおいてエンドエフェクタが把持操作を終了して搬送物体を置くことに対応する。「∨θ^ｃｒｙ＊」が１であることは、当該時間ステップにおいてエンドエフェクタが搬送物体を把持しながら移動すること、つまり搬送することに対応する。

【0136】

次に、計画設定情報について説明する。設定記憶部３００は、計画設定情報を記憶する。計画設定情報は、具体的には、搬送物体数Ｎ_ｄｌｖとｐ^{ｄｌｖ，ｉｎｉｔ}を除いた上述の定数及び係数である。これら定数及び係数は、ユーザが設定するものと設定記憶部３００が自動で算出するものに分かれていてもよい。例えば、次元数Ｎ_ｄｉｍやｖ^{ｅｅ，ｉｎｉｔ}などの自明なデフォルト値（それぞれ３と０）が存在する定数及び係数については、ユーザが設定することをスキップできるものとしてもよい。他にも、距離ｂ^ｏｎ及び距離ｂ^ｏｆｆはそれぞれ式（５４）及び式（５５）により算出することができるものとしてもよいし、これら以外のＭ、εなど定数・係数についても、他の定数・係数を用いて設定記憶部３００が適切な値を算出できるものとしてもよい。

【0137】

計測情報について補足説明する。動作計画部１００は、計測装置４０が出力する搬送物体の位置情報と姿勢情報を合わせた計測情報を取得する。動作計画部１００は、取得した計測情報から搬送物体の個数を特定して次元数Ｎ_ｄｉｍを決定しつつ、計測情報とインデックス番号ｍ、ｋとの対応関係を特定する。また動作計画部１００は、取得した計測情報から位置情報をｐ^{ｄｌｖ，ｉｎｉｔ}として読み込む。さらに、動作計画部１００は、取得した計測情報のうち姿勢情報を用い、設定記憶部３００から読み込んだｓ^ｄｌｖの値を更新してもよい。

【0138】

動作計画部１００は、図９に示す手順で計算を実行する。図９は、動作計画部１００の処理工程の例を示すフローチャートである。

【0139】

動作計画部１００は、設定記憶部３００から計画設定情報を読み込む（ステップＳ１０１）。これにより、動作計画部１００は、例えば、「（ｂ）定数・係数：個数」、「（ｃ）定数・係数：初期値・目標値」、「（ｄ）定数・係数：上限値・下限値」、「（ｅ）定数・係数：形状」、「（ｆ）定数・係数：距離」、「（ｇ）定数・係数：その他」で説明した各定数及び係数（Ｎ_ｄｌｖ及びｐ^{ｄｌｖ，ｉｎｉｔ}を除く）を取得する。

【0140】

次に、動作計画部１００は、計測装置４０から搬送物体の位置座標を取得する（ステップＳ１０２）。これにより、動作計画部１００は、例えば、搬送物体数Ｎ_ｄｌｖ及び搬送物体の位置の初期値ｐ^{ｄｌｖ，ｉｎｉｔ}を取得する。

【0141】

次に、動作計画部１００は、読み込んだ計画設定情報および取得した搬送物体の位置座標を用いて最適化モデルを生成する（ステップＳ１０３）。例えば、動作計画部１００は、式（１）～式（５３）に示される制約条件を用い、式（５６）に示される決定変数のベクトルＸに関する最適化モデルを生成する。

【0142】

そして、動作計画部１００は、生成した最適化モデルに対して求解計算を実行し、得られた解から動作計画情報を算出する（ステップＳ１０４）。この場合、動作計画部１００は、例えば、式（５６）に示される最適化モデルの解となる決定変数のベクトルＸの解Ｘ^＊を求め、求めた解Ｘ^＊から、軌道情報に相当するｐ^ｅｅ＊と、操作情報に相当するθ^ｐｃｋ＊、θ^ｐｌｃ＊、θ^ｃｒｙ＊を動作計画情報として抽出する。

【0143】

動作計画部１００は、算出した動作計画情報を出力する（ステップＳ１０５）。そして、動作計画部１００は、ステップＳ１０５の処理が実行された後、図９の処理を終了する。

【0144】

以上に説明したように、本実施形態の動作計画装置は、１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、ロボットが、搬送物体を把持して搬送しているときに、ロボットの被衝突回避物体周りの移動可能領域と搬送されている搬送物体の被衝突回避物体周りの移動可能領域とが一致するという第２条件とを設定する。そして、動作計画装置は、第１条件と、第２条件とに基づき、ロボットの動作計画を算出する。その結果、本実施形態の動作計画装置は、時間効率が高い衝突回避を考慮したピックアンドプレースの動作計画の算出が可能である。

【0145】

＜第２実施形態＞
第１実施形態では、バイナリ変数の個数を削減することを主な目的としていたが、第２実施形態では、big-M法による制約条件の個数を実質的に削減することを主な目的とする。具体的には、式（４３）及び式（４４）で表される制約条件（即ち、搬送物体が障害物周りの移動可能領域に位置しているか否かを判定しうる制約条件）の個数を実質的に削減することを狙いとする。

【0146】

第２実施形態における動作計画装置及び動作計画装置を含むロボット制御システムのシステム構成は、第１実施形態において図１～図３を参照して説明したロボット制御システム１、１ａ、１ｂと同様である。また、第２実施形態におけるロボット制御システムのハードウェア構成についても、第１実施形態において図５を参照して説明した構成と同様である。また、第２実施形態における動作計画装置のハードウェア構成についても、第１実施形態において図４を参照して説明した構成と同様である。さらに第２実施形態において用いる最適化モデルについても第１実施形態と同様である。以後では、第１実施形態と同様の要素については同一の符号を付し、適宜その説明を省略する。

【0147】

第２実施形態では、式（４３）及び式（４４）で表される制約条件の個数を実質的に削減とすることを、決定変数φ^{ｅｅ，ｄｌｖ}の取りうる値をソフト的に制約することで実現する。これは、衝突回避の観点で支障がない範囲で式（４３）及び式（４４）を取り除くことに相当する。具体的には、第２実施形態では、以下の式（５７）に示される最適化モデルを設定する。

【0148】

【数46】

式（５７）に示される目的関数は、第３項を有する点において、第１実施形態において説明した式（５６）に示される目的関数と異なる。

【0149】

式（５７）に示される目的関数の第３項を「ソフト経路制約」と呼ぶこととする。また、「Ｒｃ」はロボットの経路とすることにペナルティがある移動可能領域のインデックス番号の集合であり、「制約経路」と呼ぶこととする。ソフト経路制約は、目的関数を最小化するために、ロボットが位置する移動可能領域により生じるペナルティをできるだけ小さくする、即ち、できるだけ多くのφ^{ｅｅ，ｄｌｖ}を０にするように働く。つまり、制約経路Ｒｃに属する移動可能領域については、φ^{ｅｅ，ｄｌｖ}の値がソフト経路制約によって定まるようなときは、式（４３）及び式（４４）の評価を実質的にスキップすることができる。これは、big-M法による制約条件の個数を実質的に削減することに対応する。

【0150】

ソフト経路制約は、制約経路Ｒｃに属する移動可能領域をできるだけ通過しないような経路を優先して選択することに対応する。従って、通過する可能性が低いと予測される移動可能領域を制約経路Ｒｃの要素とすることで、効率的に上述の制約条件の削減効果を得ることができる。

【0151】

なお、制約条件「φ^{ｅｅ，ｄｌｖ}＝０」を設けることにより強制的に制約経路Ｒｃに属する移動可能領域を通過しないようにすることも考えられる。しかし、一般的にはエンドエフェクタが障害物および搬送物体周りを移動する経路は多数存在して、それらは複雑な依存関係にあるため、通過する可能性が全くない移動可能領域を特定することは困難か、または、保守的に少数の移動可能領域についてのみに制約条件を与えることになる。このような理由により、制約条件「φ^{ｅｅ，ｄｌｖ}＝０」を用いる方法は、効率良くφ^{ｅｅ，ｄｌｖ}の値を定める効果を発揮しにくい。一方で、ソフト経路制約による方法は、事前に通過する可能性が全くない移動可能領域を特定することを必要としないため、実用的にφ^{ｅｅ，ｄｌｖ}の値を定める効果を発揮する。

【0152】

制約経路Ｒｃの算出方法について、図５と図１０（Ａ）及び図１０（Ｂ）を参照しながら説明する。図１０（Ａ）は、ソフト経路制約の対象となる移動可能領域（即ち制約経路Ｒｃ）のｘ－ｚ平面図の一例であり、図１０（Ｂ）は、図１０（Ａ）に対応するｘ－ｙ平面図である。図１０（Ａ）及び図１０（Ｂ）では、制約経路Ｒｃに該当する移動可能領域をハッチングしている。

【0153】

ピックアンドプレースという動作の特性として、エンドエフェクタは搬送物体の上方の移動可能領域を移動する確率（予測通過確率）が高く、反対に、搬送物体の下方の移動可能領域を移動する確率（予測通過確率）が低い。従って、図１０（Ａ）に示す例では、搬送物体６０の下方（即ちｚ方向において搬送物体６０より下）の移動可能領域が制約経路Ｒｃとなっている。

【0154】

他にもピックアンドプレースという動作の特性として、搬送物体の初期位置（搬送元）から目標位置（搬送先）までの経路をエンドエフェクタは移動するが、ｘ方向またはｙ方向のいずれかの移動距離が他方の軸方向の移動距離よりも長い場合が多い。図５の例については、ｘ方向の移動距離の方がｙ方向の移動距離よりも大きい場合である。エンドエフェクタは、移動距離が短い軸方向（図５の例ではｙ方向）の両側の移動可能領域を経由しなくとも、搬送物体の上方の移動可能領域と移動距離が長い軸方向（図５の例ではｘ方向）の両側の移動可能領域を経由することで、一連のピックアンドプレースを完了させることができる場合が多い。

【0155】

これらのピックアンドプレースの特性は制約経路Ｒｃの算出に活用できる。具体的には、動作計画部１００は、制約経路Ｒｃに、ｚ方向において搬送物体６０よりも下側に位置する移動可能領域を追加する（図１０（Ａ）参照）。続いて、動作計画部１００は、移動距離が短い軸方向（図１０（Ａ）、（Ｂ）の例ではｙ方向とする）において搬送物体６０の両側に位置する移動可能領域を追加する（図１０（Ｂ）参照）。動作計画部１００は、移動距離が短い軸を決定する場合、例えば、複数の搬送物体の初期位置と目標位置の重心をそれぞれ算出し、それらの差分のｘ成分の絶対値とｙ成分の絶対値の大小関係を比較して小さい方の軸を移動距離が短い軸とすることができる。

【0156】

なお、制約経路Ｒｃの算出方法は、図１０（Ａ）及び図１０（Ｂ）を参照して説明した例に限定されず、計画設定情報および計測情報を用いて通過する可能性が低い移動可能領域を予測に基づいて算出方法であればよい。具体的には、動作計画部１００は、搬送物体の初期位置と目標位置とに基づき、ロボットの予測通過確率が閾値よりも低いと予測される移動可能領域を、制約経路Ｒｃとして設定する。

【0157】

また、図１０（Ａ）及び図１０（Ｂ）を参照して説明した例では、制約経路Ｒｃは、全搬送物体および全時間ステップにおいて共通のものとしたが、搬送物体毎、時間ステップ毎に異なるようにしてもよい。この場合、制約経路Ｒｃは、制約経路Ｒｃ（ｍ，ｔ）のようにｍ及びｔを変数に持つものとなり、よりきめ細やかに制約経路を決めることができるようになり、その結果、ソフト経路制約により多くのφ^{ｅｅ，ｄｌｖ}の値を定めることができるという効果を発揮する。

【0158】

好適な例では、さらに、ソフト経路制約におけるφ^{ｅｅ，ｄｌｖ}に重み係数を乗ずることで、より効率良くφ^{ｅｅ，ｄｌｖ}の値を定めるようにしてもよい。重み係数は、当該移動可能領域を通過する確率などに基づいて算出するようにしてもよい。

【0159】

図１１は、第２実施形態における動作計画部１００の処理工程の例を示すフローチャートである。

【0160】

動作計画部１００は、設定記憶部３００から計画設定情報を読み込む（ステップＳ２０１）。動作計画部１００は、計測装置４０から搬送物体の位置座標を取得する（ステップＳ２０２）。ステップＳ２０１及びステップＳ２０２の処理は、図９のフローチャートのステップＳ１０１及びステップＳ１０２と同一である。

【0161】

動作計画部１００は、読み込んだ計画設定情報および取得した搬送物体の位置座標を用いて経路制約Ｒｃを算出する（ステップＳ２０３）。例えば、動作計画部１００は、ｚ軸方向で下側の移動可能領域と、移動距離が短い軸方向で両側の移動可能領域とを、経路制約Ｒｃに追加する。

【0162】

次に、動作計画部１００は、読み込んだ計画設定情報、取得した搬送物体の位置座標、および算出した制約経路Ｒｃを用いてソフト経路制約付き最適化モデルを生成する（ステップＳ２０４）。具体的には、動作計画部１００は、式（１）～式（５３）に示される制約条件を用い、式（５７）に示される決定変数のベクトルＸに関する最適化モデルを生成する。

【0163】

動作計画部１００は、生成した最適化モデルに対して求解計算を実行し、得られた解から動作計画情報を算出する（ステップＳ２０５）。この場合、動作計画部１００は、例えば、式（５７）に示される最適化モデルの解となる決定変数のベクトルＸの解Ｘ^＊を求め、求めた解Ｘ^＊から、軌道情報に相当するｐ^ｅｅ＊と、操作情報に相当するθ^ｐｃｋ＊、θ^ｐｌｃ＊、θ^ｃｒｙ＊を動作計画情報として抽出する。

【0164】

動作計画部１００は、算出した動作計画情報を出力する（ステップＳ２０６）。動作計画部１００は、ステップＳ２０６の処理が実行された後、図１０の処理を終了する。

【0165】

以上に説明したように、第２実施形態に係る動作計画装置１０は、搬送物体の初期位置と目標位置とに基づき、ロボットの予測通過確率が閾値よりも低い搬送物体周りの移動可能領域を、ロボットの経路とすることにペナルティがある制約経路として設定する。そして、動作計画装置１０は、設定した制約条件と、制約経路とに基づき、ロボットの動作計画を算出する。その結果、本実施形態の動作計画装置は、時間効率が高い衝突回避を考慮したピックアンドプレースの動作計画の算出が可能である。

【0166】

＜第３実施形態＞
図１２は、第３実施形態における動作計画装置１０Ｘの概略構成図を示す。動作計画装置１０Ｘは、主に、条件設定手段９０Ｘと、動作計画手段１００Ｘとを有する。なお、動作計画装置１０Ｘは、複数の装置から構成されてもよい。動作計画装置１０Ｘは、例えば、第１実施形態及び第２実施形態における動作計画装置１０、１０ａ、１０ｂのいずれかとすることができる。

【0167】

条件設定手段９０Ｘは、１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび前記搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、ロボットが、搬送物体を把持して搬送しているときに、ロボットの被衝突回避物体周りの移動可能領域と搬送されている搬送物体の被衝突回避物体周りの移動可能領域とが一致するという第２条件とを設定する。動作計画手段１００Ｘは、第１条件と第２条件とに基づき、ロボットの動作計画を算出する。条件設定手段９０Ｘと動作計画手段１００Ｘとは、第１実施形態及び第２実施形態における動作計画部１００の一例である。

【0168】

図１３は、第３実施形態において動作計画装置１０Ｘが実行するフローチャートの一例である。条件設定手段９０Ｘは、１以上の搬送物体に対してピックアンドプレースを実行するロボットおよび前記搬送物体は、被衝突回避物体に衝突することがない領域である移動可能領域の内部に位置するという第１条件と、ロボットが、搬送物体を把持して搬送しているときに、ロボットの被衝突回避物体周りの移動可能領域と搬送されている搬送物体の被衝突回避物体周りの移動可能領域とが一致するという第２条件とを設定する（ステップＳ３０１）。そして、動作計画手段１００Ｘは、第１条件と第２条件とに基づき、ロボットの動作計画を算出する（ステップＳ３０２）。

【0169】

第３実施形態によれば、動作計画装置１０Ｘは、衝突回避を考慮したピックアンドプレースの時間効率が高い動作計画を好適に算出することができる。

【0170】

なお、本開示において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium ）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記憶媒体（tangible storage medium ）を含む。非一時的なコンピュータ可読媒体の例は、磁気記憶媒体（たとえばフレキシブルディスク、磁気テープ、ハードディスク）、光磁気記憶媒体（たとえば光磁気ディスク）、ＣＤ－ＲＯＭ（Read Only Memory）、ＣＤ－Ｒ、ＣＤ－Ｒ／Ｗ、ＤＶＤ（Digital Versatile Disc）、ＢＤ(Blu-ray(登録商標) Disc)、半導体メモリ（たとえば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM ）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory）を含む。また、本開示における動作計画装置の構成は、コンピュータを用いた構成に限定されない。たとえば、動作計画装置が、ＡＳＩＣ（Application Specific Integrated Circuit ）やＦＰＧＡ（Field Programmable Gate Array）を用いて構成されるなど、専用のハードウェアを用いて構成されていてもよい。

【0171】

以上、実施形態を参照して本願発明を説明したが、本願発明は上記の実施形態に限定されない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。また、引用した上記の非特許文献等の各開示は、本書に引用をもって繰り込むものとする。

【符号の説明】

【0172】

１、１ａ、１ｂ ロボット制御システム
１０、１０ａ、１０ｂ 動作計画装置
２０ 制御装置
３０ 設定記憶装置
４０ 計測装置
５０ ロボット

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】