JP6950638B2 - マニピュレータ制御装置、マニピュレータ制御方法、及びマニピュレータ制御プログラム - Google Patents

Description

本発明は、マニピュレータ制御装置、マニピュレータ制御方法、及びマニピュレータ制御プログラムに関する。

コンビニエンスストア等において商品を陳列棚に載置する作業を自動化する場合、マニピュレータが移動型ロボットに設けられた移動型マニピュレータを用いて商品のピックアンドプレースを行うことが考えられる。この場合、床自体に傾斜があったり、移動型ロボットが低い段差に乗り上げてしまったりしていると、移動型マニピュレータに傾きが生じる。このような状態で、傾きを考慮せずにマニピュレータを目標位置まで移動させると、傾きにより目標位置がずれるため正確に作業できない。例えば、陳列棚等の障害物の位置を予め定めた位置のままにして、傾きを考慮せずにマニピュレータを目標位置まで動作させた場合、陳列棚にマニピュレータが衝突してしまう等の作業ミスが発生する虞がある。

従来、二次元のレーザレンジファインダを用いて周辺環境を計測しながら動作する移動型ロボットの傾斜角を計測し、計測した移動型ロボットの傾斜角を考慮して地図の生成及び自己位置姿勢の推定を行う技術が提案されている（例えば特許文献１参照）。

また、移動型ロボットの傾斜角を計測し、移動型ロボットに設けられた基体の傾斜角が減少するように制御することにより、基体に設けられた把持部が水平となるように制御する技術が提案されている（例えば特許文献２参照）。

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

しかしながら、特許文献１記載の技術は、計測した移動型ロボットの傾斜角は、地図の作成及び移動体の自己位置姿勢の推定に用いられるものであり、傾斜角に応じてマニピュレータの動作を補正するものではないため、商品をピックアンドプレースするような装置に適用するのは困難である。

また、特許文献２記載の技術は、基体の水平を維持するための機構が大掛かりであるため、装置が大型化すると共にコストが高くなる、という問題があった。

本発明は、上記の点に鑑みてなされたものであり、マニピュレータが設けられた基体が基準面に対して傾斜している場合でも作業ミスを抑制することができるマニピュレータ制御装置、マニピュレータ制御方法、及びマニピュレータ制御プログラムを提供することを目的とする。

本発明に係るマニピュレータ制御装置は、マニピュレータが設けられた基体の基準面に対する傾斜角を取得する傾斜角取得部と、前記基体が前記基準面に対して傾斜していない状態での前記マニピュレータの目標位置を取得する目標位置取得部と、前記傾斜角取得部で測定された傾斜角、及び前記目標位置取得部で取得された目標位置に基づいて、前記目標位置を前記傾斜角に応じた補正目標位置に補正する補正部と、前記補正目標位置に基づいて、前記マニピュレータの動作を制御する制御部と、を備える。

なお、障害物の形状及び位置に関する予め定めた障害物情報を取得する障害物情報取得部を備え、前記補正部は、前記障害物の位置を前記傾斜角に応じた補正障害物位置に補正し、前記制御部は、前記補正目標位置及び前記補正障害物位置に基づいて、前記マニピュレータの動作を制御するようにしてもよい。

また、前記基体に設けられ、障害物の形状及び位置に関する障害物情報を検出する検出センサを備え、前記制御部は、前記補正目標位置及び前記障害物情報に基づいて、前記マニピュレータの動作を制御するようにしてもよい。

また、前記制御部は、前記傾斜角が所定値以上の場合に報知する制御を行うようにしてもよい。

本発明に係るマニピュレータ制御方法は、コンピュータが、マニピュレータが設けられた基体の基準面に対する傾斜角を取得する傾斜角取得工程と、前記マニピュレータが前記基準面に対して傾斜していない状態での前記マニピュレータの目標位置を取得する目標位置取得工程と、前記傾斜角取得工程で測定された傾斜角、及び前記目標位置取得工程で取得された目標位置に基づいて、前記目標位置を前記傾斜角に応じた補正目標位置に補正する補正工程と、前記補正目標位置に基づいて、前記マニピュレータの動作を制御する制御工程と、を含む処理を実行する。

本発明に係るマニピュレータ制御プログラムは、コンピュータを、マニピュレータが設けられた基体の基準面に対する傾斜角を取得する傾斜角取得部、前記マニピュレータが前記基準面に対して傾斜していない状態での前記マニピュレータの目標位置を取得する目標位置取得部、前記傾斜角取得部で測定された傾斜角、及び前記目標位置取得部で取得された目標位置に基づいて、前記目標位置を前記傾斜角に応じた補正目標位置に補正する補正部、及び、前記補正目標位置に基づいて、前記マニピュレータの動作を制御する制御部、として機能させるための制御プログラムである。

本発明によれば、マニピュレータが設けられた基体が基準面に対して傾斜している場合でも作業ミスを抑制することができる。

ピックアンドプレース装置の概略構成図である。 垂直多関節ロボットの一例を示す斜視図である。 マニピュレータ制御装置のハードウェア構成の例を示す構成図である。 マニピュレータ制御装置の機能構成の例を示すブロック図である。 ピッチ傾斜角について説明するための図である。 ロール傾斜角について説明するための図である。 ピッチ軸及びロール軸について説明するための図である。 マニピュレータ制御処理のフローチャートである。 目標位置及び障害物位置の補正について説明するための図である。 目標位置及び障害物位置の補正について説明するための図である。

以下、本発明の実施形態の一例を、図面を参照しつつ説明する。なお、各図面において同一又は等価な構成要素及び部分には同一の参照符号を付与している。また、図面の寸法比率は、説明の都合上誇張されている場合があり、実際の比率とは異なる場合がある。

図１は、本実施形態に係るピックアンドプレース装置１の構成図である。図１に示すように、ピックアンドプレース装置１は、移動型マニピュレータＭＰ、画像センサＳ、及びマニピュレータ制御装置１０を備える。

移動型マニピュレータＭＰは、基体の一例としての移動体Ｍに、マニピュレータの一例としてのロボットＲＢが取り付けられた構成である。移動体Ｍは、図示しないモータの駆動力によって回転する４つの車輪Ｒを備え、マニピュレータ制御装置１０の指示によりフロア内を移動する。

ロボットＲＢは、台２０上に設けられた箱２２に収容された図示しないワークを保持して棚２４まで搬送し、棚２４内に載置する。

本実施形態では、一例としてロボットＲＢのロボットアームの先端にはエンドエフェクタとしてロボットハンドＨが取り付けられており、ロボットハンドＨで箱２２内のワークを把持することによってワークを保持した状態となる。そして、ワークを保持した状態で棚２４まで搬送し、ワークの保持を解除してワークを載置する。なお、ワークを保持する部材は、ロボットハンドＨに限らず、ワークを吸着する吸着パッドでもよい。

箱２２の上方には画像センサＳが設置されている。画像センサＳは、箱２２内に収容されたワークの集合であるワーク集合を静止画として撮影する。

マニピュレータ制御装置１０は、ロボットＲＢの任意の初期姿勢から目標姿勢までの経路を生成する。ここで、経路とは、ロボットＲＢを初期姿勢から目標姿勢まで動作させる場合の姿勢のリストである。具体的には、マニピュレータ制御装置１０は、画像センサＳから取得した撮影画像を画像処理し、画像処理の結果に基づいて保持すべきワークの位置及び姿勢を認識する。そして、マニピュレータ制御装置１０は、箱２２内のワークを撮影するためにロボットＲＢが画像センサＳの視野範囲外の退避位置に退避した退避姿勢を初期姿勢とし、ロボットＲＢが箱２２内のワークを保持する保持姿勢を目標姿勢とする第１の経路と、ロボットＲＢが箱２２内のワークを保持した保持姿勢を初期姿勢とし、ロボットＲＢがワークを棚２４に載置する載置姿勢を目標姿勢とする第２の経路と、ロボットＲＢがワークを棚２４に載置した載置姿勢を初期姿勢とし、箱２２内のワークを撮影するためにロボットＲＢが画像センサＳの視野範囲外の退避位置に退避した退避姿勢を目標姿勢とする第３の経路と、を生成する。なお、棚２４を撮影する画像センサを備えた構成とし、画像センサで撮影された撮影画像に基づいてワークを載置すべき位置を認識するようにしてもよい。そして、マニピュレータ制御装置１０は、生成した経路に従ってロボットＲＢが動作するように動作指令値をロボットＲＢに出力する。

次に、ロボットＲＢについて説明する。本実施形態では、一例としてロボットＲＢが垂直多関節ロボットである場合について説明するが、水平多関節ロボット（スカラーロボット）及びパラレルリンクロボット等にも本発明を適用可能である。

図２は、垂直多関節ロボットであるロボットＲＢの構成を示す図である。図２に示すように、ロボットＲＢは、ベースリンクＢＬ、リンクＬ１〜Ｌ６、ジョイントＪ１〜Ｊ６を備えた６自由度の６軸ロボットである。なお、ジョイントとは、リンク同士を接続する関節である。また、以下では、リンクＬ１〜Ｌ６及びリンクＬ６に接続されたロボットハンドＨを含めてロボットアームと称する。

ベースリンクＢＬとリンクＬ１とは、図２において鉛直軸Ｓ１を中心として矢印Ｃ１方向に回転するジョイントＪ１を介して接続されている。従って、リンクＬ１は、ベースリンクＢＬを支点として矢印Ｃ１方向に回転する。

リンクＬ１とリンクＬ２とは、図２において水平軸Ｓ２を中心として矢印Ｃ２方向に回転するジョイントＪ２を介して接続されている。従って、リンクＬ２は、リンクＬ１を支点として矢印Ｃ２方向に回転する。

リンクＬ２とリンクＬ３とは、図２において軸Ｓ３を中心として矢印Ｃ３方向に回転するジョイントＪ３を介して接続されている。従って、リンクＬ３は、リンクＬ２を支点として矢印Ｃ３方向に回転する。

リンクＬ３とリンクＬ４とは、図２において軸Ｓ４を中心として矢印Ｃ４方向に回転するジョイントＪ４を介して接続されている。従って、リンクＬ４は、リンクＬ３を支点として矢印Ｃ４方向に回転する。

リンクＬ４とリンクＬ５とは、図２において軸Ｓ５を中心として矢印Ｃ５方向に回転するジョイントＪ５を介して接続されている。従って、リンクＬ５は、リンクＬ４を支点として矢印Ｃ５方向に回転する。

リンクＬ５とリンクＬ６とは、図２において軸Ｓ６を中心として矢印Ｃ６方向に回転するジョイントＪ６を介して接続されている。従って、リンクＬ６は、リンクＬ５を支点として矢印Ｃ６方向に回転する。なお、図２では図示は省略したが、リンクＬ６にロボットハンドＨが取り付けられる。

ジョイントＪ１〜Ｊ６は、予め定めた回転角度の範囲が可動域として各々設定されている。

ロボットＲＢの姿勢は、ジョイントＪ１〜Ｊ６の各々の回転角度によって定まる。従って、ロボットＲＢを動作させるための動作指令値は、ロボットＲＢが取るべき姿勢に対応したジョイントＪ１〜Ｊ６の各々の回転角度である。

図３は、本実施形態に係るマニピュレータ制御装置１０のハードウェア構成を示すブロック図である。

図３に示すように、マニピュレータ制御装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１１、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１３、ストレージ１４、入力部１５、モニタ１６、光ディスク駆動装置１７、及び通信インタフェース１８を有する。各構成は、バス１９を介して相互に通信可能に接続されている。

本実施形態では、ＲＯＭ１２又はストレージ１４には、マニピュレータ制御処理を実行するマニピュレータ制御プログラムが格納されている。ＣＰＵ１１は、中央演算処理ユニットであり、各種プログラムを実行したり、各構成を制御したりする。すなわち、ＣＰＵ１１は、ＲＯＭ１２又はストレージ１４からプログラムを読み出し、ＲＡＭ１３を作業領域としてプログラムを実行する。ＣＰＵ１１は、ＲＯＭ１２又はストレージ１４に記録されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。

ＲＯＭ１２は、各種プログラム及び各種データを格納する。ＲＡＭ１３は、作業領域として一時的にプログラム又はデータを記憶する。ストレージ１４は、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）又はＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。

入力部１５は、キーボード１５１、及びマウス１５２等のポインティングデバイスを含み、各種の入力を行うために使用される。モニタ１６は、例えば、液晶ディスプレイであり、各種の情報を表示する。モニタ１６は、タッチパネル方式を採用して、入力部１５として機能してもよい。光ディスク駆動装置１７は、各種の記録媒体(ＣＤ−ＲＯＭ又はブルーレイディスクなど)に記憶されたデータの読み込みや、記録媒体に対するデータの書き込み等を行う。

通信インタフェース１８は、ロボットＲＢ及び画像センサＳ等の他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ−Ｆｉ（登録商標）、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）、及びＩＥＥＥ１３９４等の規格が用いられる。

次に、マニピュレータ制御装置１０の機能構成について説明する。

図４は、マニピュレータ制御装置１０の機能構成の例を示すブロック図である。図４に示すように、マニピュレータ制御装置１０は、機能構成として、傾斜角取得部３０、目標位置取得部３２、障害物情報取得部３４、補正部３６、制御部３８、及び記憶部４０を有する。各機能構成は、ＣＰＵ１１がＲＯＭ１２又はストレージ１４に記憶されたマニピュレータ制御プログラムを読み出し、ＲＡＭ１３に展開して実行することにより実現される。

傾斜角取得部３０は、ロボットＲＢが設けられた移動体Ｍの基準面に対する傾斜角を取得する。なお、基準面は、本実施形態では一例として水平面である。

図５、６に示すように、移動体ＭのロボットＲＢが搭載された面には、傾斜センサＫ１、Ｋ２が設けられている。

図５に示すように、傾斜センサＫ１は、ロボットＲＢが設けられた移動体Ｍの水平面ＨＲに対するピッチ傾斜角θ１を検出する。

ピッチ傾斜角θ１は、図７に示すように、Ｘ軸をピッチ軸、Ｘ軸と直交するＹ軸をロール軸とした場合に、ピッチ軸を中心とした回転方向における水平面に対する傾斜角である。

図６に示すように、傾斜センサＫ２は、ロボットＲＢが設けられた移動体Ｍの水平面ＨＲに対するロール傾斜角θ２を検出する。

ピッチ傾斜角θ１は、図７に示すように、Ｘ軸をピッチ軸、Ｘ軸と直交するＹ軸をロール軸とした場合に、ロール軸を中心とした回転方向における水平面に対する傾斜角である。

傾斜角取得部３０は、傾斜センサＫ１からピッチ傾斜角θ１を取得し、傾斜センサＫ２からロール傾斜角θ２を取得する。

目標位置取得部３２は、移動体Ｍが水平面ＨＲに対して傾斜していない状態でのロボットＲＢの目標位置を取得する。

障害物情報取得部３４は、予め定めた障害物の形状及び位置に関する障害物情報を取得する。

補正部３６は、傾斜角取得部３０で測定されたピッチ傾斜角θ１及びロール傾斜角θ２と、目標位置取得部３２で取得された目標位置と、障害物情報取得部３４で取得された障害物情報と、に基づいて、目標位置をピッチ傾斜角θ１及びロール傾斜角θ２に応じた補正目標位置に補正する。

制御部３８は、ロボットＲＢの現在位置、及び補正目標位置に基づいて、補正部３６で補正された補正目標姿勢に基づいて、ロボットＲＢの動作を制御する。

記憶部４０は、マニピュレータ制御プログラム及び障害物情報等の各種情報を記憶する。

障害物情報は、障害物の形状及び位置に関する情報である。障害物は、本実施形態では台２０、箱２２、及び棚２４等である。

次に、マニピュレータ制御装置１０の作用について説明する。なお、以下では、説明を簡単にするため、移動体Ｍは移動せずに同じ場所に留まっているものとして説明する。

図８は、マニピュレータ制御装置１０のＣＰＵ１１により実行されるマニピュレータ制御処理の流れを示すフローチャートである。オペレータによりマニピュレータ制御処理の実行が指示されると、ＣＰＵ１１がＲＯＭ１２又はストレージ１４からマニピュレータ制御プログラムを読み出して、ＲＡＭ１３に展開し実行することにより、マニピュレータ制御処理が実行される。

ＣＰＵ１１は、障害物情報取得部３４として、障害物情報を記憶部４０から読み出すことにより取得する（ステップＳ１００）。

ＣＰＵ１１は、傾斜角取得部３０として、傾斜センサＫ１からピッチ傾斜角θ１を取得し、傾斜センサＫ２からロール傾斜角θ２を取得する（ステップＳ１０２）。

ＣＰＵ１１は、制御部３８として、ピッチ傾斜角θ１及びロール傾斜角θ２のうち少なくとも一方が予め定めた閾値以上であるか否かを判定する（ステップＳ１０４）。ここで、閾値は、移動型マニピュレータＭＰが転倒する可能性がある値に設定される。

そして、ピッチ傾斜角θ１及びロール傾斜角θ２のうち少なくとも一方が予め定めた閾値以上である場合（ステップＳ１０４：ＹＥＳ）、すなわち、移動型マニピュレータＭＰが転倒する虞がある場合はステップＳ１０５へ移行する。

ステップＳ１０５では、ＣＰＵ１１が、制御部３８として、移動型マニピュレータＭＰが転倒する虞があることを報知する制御を行う。例えば、ＣＰＵ１１は、移動型マニピュレータＭＰが転倒する虞があることを示す警告メッセージをモニタ１６に表示させる。なお、図示しないブザーをマニピュレータ制御装置１０に接続して、ブザーを鳴らすことで移動型マニピュレータＭＰが転倒する虞があることを報知するようにしてもよい。また、図示しない警告灯をマニピュレータ制御装置１０に接続して、警告灯を点灯させることで移動型マニピュレータＭＰが転倒する虞があることを報知するようにしてもよい。

一方、ピッチ傾斜角θ１及びロール傾斜角θ２のうち少なくとも一方が予め定めた閾値未満である場合（ステップＳ１０４：ＮＯ）、すなわち、移動型マニピュレータＭＰが転倒する虞がない場合はステップＳ１０６へ移行する。

ＣＰＵ１１は、制御部３８として画像センサＳに撮影を指示し、画像センサＳが撮影した撮影画像を取得する（ステップＳ１０６）。

ＣＰＵ１１は、目標位置取得部３２として、撮影画像に基づいて、保持するワークの位置を選択し、選択された位置を、移動体Ｍが水平面に対して傾斜していない状態でのロボットＲＢの目標位置として取得する（ステップＳ１０８）。

保持するワークの選択基準としては、例えばワーク集合の中で最上部にあるワークを選択する、ワーク集合の中央に存在するワークを選択する、他のワークと重ならないワークを選択する等が挙げられるが、これらに限られるものではない。

ＣＰＵ１１は、補正部３６として、ステップＳ１０２で取得したピッチ傾斜角θ１及びロール傾斜角θ２と、ステップＳ１０８で取得した目標位置と、に基づいて、目標位置をピッチ傾斜角θ１及びロール傾斜角θ２に応じた補正目標位置に補正する（ステップＳ１１０）。

具体的には、ステップＳ１０８で取得した目標位置を（Ｘｇ、Ｙｇ、Ｚｇ）、補正目標位置を（Ｘ’ｇ、Ｙ’ｇ、Ｚ’ｇ）とした場合、次式により補正目標位置を算出する。

・・・（１）

ここで、Ｒは、下記に示すように、ピッチ傾斜角θ１及びロール傾斜角θ２をパラメータとする３次回転行列である。

・・・（２）

ＣＰＵ１１は、補正部３６として、ステップＳ１００で取得した障害物情報に基づいて、障害物の位置をピッチ傾斜角θ１及びロール傾斜角θ２に応じた補正障害物位置に補正する（ステップＳ１１２）。

具体的には、ステップＳ１００で取得した障害物としての箱２２の位置を（Ｘｔ、Ｙｔ、Ｚｔ）、補正目標位置を（Ｘ’ｔ、Ｙ’ｔ、Ｚ’ｔ）とした場合、次式により補正障害物位置を算出する。

・・・（３）

ＣＰＵ１１は、制御部３８として、ステップＳ１１０で算出した補正目標位置及びステップＳ１１２で算出した補正障害物位置に基づいて、ロボットＲＢが退避位置に退避した退避姿勢を初期姿勢とし、ロボットＲＢが箱２２内でワークを保持する保持姿勢を目標姿勢とする第１の経路を生成する（ステップＳ１１４）。第１の経路の生成は、公知の経路計画法を用いることができる。後述する第２の経路及び第３の経路についても同様である。

ＣＰＵ１１は、目標位置取得部３２として、保持したワークを棚２４に載置する際の載置位置を選択し、選択された載置位置を、移動体Ｍが水平面に対して傾斜していない状態でのロボットＲＢの目標位置とする（ステップＳ１１６）。

ロボットＲＢがワークを棚２４に載置する載置位置については、以下のように取得できる。例えば棚２４に設けられた複数の載置場所を予めティーチングしておき、ティーチングした複数の載置場所の位置情報及び複数の載置場所にワークを載置する載置順序を記憶部４０に記憶しておく。そして、載置順序に従って決められた載置位置をロボットＲＢの目標位置とする。なお、保持するワークを選択する場合と同様に、棚２４を撮影する画像センサを設け、棚２４を撮影した撮影画像に基づいてワークを載置する載置位置を選択してもよい。

ＣＰＵ１１は、ステップＳ１１０と同様に、補正部３６として、ステップＳ１０２で取得したピッチ傾斜角θ１及びロール傾斜角θ２と、ステップＳ１１６で取得した目標位置と、に基づいて、目標位置をピッチ傾斜角θ１及びロール傾斜角θ２に応じた補正目標位置に補正する（ステップＳ１１８）。

ＣＰＵ１１は、ステップＳ１１２と同様に、補正部３６として、ステップＳ１００で取得した障害物情報に基づいて、障害物としての棚２４の位置を、ピッチ傾斜角θ１及びロール傾斜角θ２に応じた補正障害物位置に補正する（ステップＳ１２０）。

ＣＰＵ１１は、制御部３８として、ステップＳ１１８で算出した補正目標位置及びステップＳ１２０で算出した補正障害物位置に基づいて、ロボットＲＢが箱２２内のワークを保持した保持姿勢である初期姿勢から、ステップＳ１１８で補正した補正目標位置における目標姿勢である載置姿勢までの第２の経路を生成する（ステップＳ１２２）。

ＣＰＵ１１は、制御部３８として、ロボットＲＢがワークを棚２４に載置した載置姿勢である補正目標姿勢から退避姿勢までの第３の経路を生成する（ステップＳ１２４）。

ＣＰＵ１１は、制御部３８として、ロボットＲＢの第１〜３の経路に基づいてロボットＲＢの動作指令値を生成し、ロボットＲＢに送信する（ステップＳ１２６）。これにより、生成された第１〜第３の経路に従ってロボットＲＢが移動する。すなわち、ロボットＲＢが、退避位置から移動して箱２２の中のワークを保持し、棚２４の所定位置に載置した後、退避位置に戻るように動作する。

ＣＰＵ１１は、制御部３８として、箱２２内から全てのワークを棚２４へ搬送したか否かを判定する（ステップＳ１２８）。具体的には、例えば箱２２内を撮影した撮影画像を取得してパターン認識処理等の公知の画像処理を実行し、撮影画像中にワークが存在するか否かを判定する。また、最初に箱２２内に載置されていたワークの数が予め判っている場合には、ワークを搬送した回数が最初に箱２２内に収容されていたワークの数に達したか否かを判定してもよい。そして、全てのワークを搬送した場合（ステップＳ１２８：ＹＥＳ）、本ルーチンを終了する。一方、全てのワークを搬送していない場合（ステップＳ１２８：ＮＯ）、ステップＳ１０２へ移行し、全てのワークを搬送するまでステップＳ１００〜Ｓ１２０の処理を繰り返す。

このように、本実施形態では、ロボットＲＢの目標位置及び障害物の位置を補正してロボットＲＢの動作を制御する。このため、移動体Ｍが水平面に対して傾斜している場合でも作業ミスを抑制することができる。例えば、図９に示すように、ワークを棚２４に載置する場合において、移動体Ｍが水平面に対して傾斜していない状態におけるロボットＲＢの目標位置を（Ｘｇ、Ｙｇ、Ｚｇ）、障害物としての棚２４の原点位置を障害物位置（Ｘｔ、Ｙｔ、Ｚｔ）とする。この場合、ステップＳ１１８の処理により目標位置を補正すると共にステップＳ１２０の処理により障害物としての棚２４の位置を補正することにより、図１０に示すように、補正目標位置（Ｘ’ｇ、Ｙ’ｇ、Ｚ’ｇ）と補正障害物位置（Ｘ’ｔ、Ｙ’ｔ、Ｚ’ｔ）との相対的な位置関係は、補正前における目標位置（Ｘｇ、Ｙｇ、Ｚｇ）と障害物位置（Ｘｔ、Ｙｔ、Ｚｔ）との相対的な位置関係と同じになる。これにより、ワークを棚２４に載置する際にロボットＲＢのロボットアームが棚２４に衝突する等の作業ミスが抑制される。

マニピュレータ制御装置１０は、上記の実施形態に限定されず、種々の改変が可能である。例えば、障害物情報を検出する検出センサとして例えばカメラを移動体Ｍに設けた構成とし、図８のステップＳ１００の処理に代えて、カメラで撮影された撮影画像に基づいて障害物の形状及び位置を検出する処理を実行する。この場合、移動体Ｍが水平面に対して傾いている場合には、撮影画像における障害物も傾斜した状態となるため、障害物位置の補正は不要である。すなわち、図８のステップＳ１１２、Ｓ１２０の処理を省略することができる。

また、障害物を考慮する必要がない場合には、図８のステップＳ１００、Ｓ１１２、Ｓ１２０の処理を省略してもよい。

また、本実施形態では、ロボットＲＢが移動体Ｍに設けられた場合について説明したが、ロボットＲＢが移動しない基体上に設けられた場合にも本発明を適用可能である。

なお、上記各実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した経路計画を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、経路計画を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

また、上記各実施形態では、マニピュレータ制御プログラムがストレージ１４又はＲＯＭ１２に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ−ＲＯＭ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリ等の記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

１ ピックアンドプレース装置
１０ マニピュレータ制御装置
２２ 箱
２４ 棚
３０ 傾斜角取得部
３２ 目標位置取得部
３４ 障害物情報取得部
３６ 補正部
３８ 制御部
４０ 記憶部

Claims (4)

1. マニピュレータが設けられた基体の基準面に対する傾斜角を取得する傾斜角取得部と、
   前記基体が前記基準面に対して傾斜していない状態での前記マニピュレータの目標位置を取得する目標位置取得部と、
   障害物の形状及び位置に関する予め定めた障害物情報を取得する障害物情報取得部と、
   前記傾斜角取得部で測定された傾斜角、及び前記目標位置取得部で取得された目標位置に基づいて、前記目標位置を前記傾斜角に応じた補正目標位置に補正すると共に、前記障害物の位置を前記傾斜角に応じた補正障害物位置に補正する補正部と、
   前記補正目標位置及び前記補正障害物位置に基づいて、前記マニピュレータの動作を制御する制御部と、
   を備えたマニピュレータ制御装置。
2. 前記制御部は、前記傾斜角が所定値以上の場合に報知する制御を行う
   請求項１記載のマニピュレータ制御装置。
3. コンピュータが、
   マニピュレータが設けられた基体の基準面に対する傾斜角を取得する傾斜角取得工程と、
   前記マニピュレータが前記基準面に対して傾斜していない状態での前記マニピュレータの目標位置を取得する目標位置取得工程と、
   障害物の形状及び位置に関する予め定めた障害物情報を取得する障害物情報取得工程と、
   前記傾斜角取得工程で測定された傾斜角、及び前記目標位置取得工程で取得された目標位置に基づいて、前記目標位置を前記傾斜角に応じた補正目標位置に補正すると共に、前記障害物の位置を前記傾斜角に応じた補正障害物位置に補正する補正工程と、
   前記補正目標位置及び前記補正障害物位置に基づいて、前記マニピュレータの動作を制御する制御工程と、
   を含む処理を実行するマニピュレータ制御方法。
4. コンピュータを、
   マニピュレータが設けられた基体の基準面に対する傾斜角を取得する傾斜角取得部、
   前記マニピュレータが前記基準面に対して傾斜していない状態での前記マニピュレータの目標位置を取得する目標位置取得部、
   障害物の形状及び位置に関する予め定めた障害物情報を取得する障害物情報取得部、
   前記傾斜角取得部で測定された傾斜角、及び前記目標位置取得部で取得された目標位置に基づいて、前記目標位置を前記傾斜角に応じた補正目標位置に補正すると共に、前記障害物の位置を前記傾斜角に応じた補正障害物位置に補正する補正部、及び、
   前記補正目標位置及び前記補正障害物位置に基づいて、前記マニピュレータの動作を制御する制御部、
   として機能させるためのマニピュレータ制御プログラム。

Images