(57)【要約】【課題】冗長自由度を有するマニピュレータを適切に制御すること。【解決手段】角度計算部２１ａが、目標位置に基づいて各アクチュエータ１１ａ〜１７ａの角度を計算し、位置修正部２１ｂが、角度計算部２１ａによって計算された各アクチュエータ１１ａ〜１７ａの角度から特定されるマニピュレータ１の各部位の位置のうち何れかが禁止領域に含まれる場合に、禁止領域に含まれる部位の位置を禁止領域外の位置へ修正し、再計算部２１ｃが、位置修正部２１ｂによって修正された部位の位置に基づいて各アクチュエータ１１ａ〜１７ａの角度を再計算する。【選択図】図２
2012240144.tif 000002
【特許請求の範囲】
【請求項１】
複数の関節軸を有するマニピュレータを制御するロボット制御装置であって、
目標位置に基づいて前記関節軸の角度を計算する角度計算部と、
前記角度計算部によって計算された前記関節軸の角度から特定される前記マニピュレータの各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、前記禁止領域に含まれる前記部位の位置を前記禁止領域外の位置へ修正する位置修正部と、
前記位置修正部によって修正された前記部位の位置に基づいて前記関節軸の角度を再計算する再計算部と
を備えることを特徴とするロボット制御装置。
【請求項２】
前記位置修正部は、
前記禁止領域に含まれることとなる前記部位と前記禁止領域の境界面との間に仮想的なバネ・ダンパモデルを設定し、前記部位が前記バネ・ダンパモデルへ作用させる力と前記バネ・ダンパモデルが前記部位へ作用させる力とがつりあう位置を前記部位の修正後の位置とすることを特徴とする請求項１に記載のロボット制御装置。
【請求項３】
前記位置修正部は、
前記禁止領域に含まれることとなる前記部位と前記禁止領域との交点を算出し、算出した交点に基づいて前記バネ・ダンパモデルの設定位置を決定することを特徴とする請求項２に記載のロボット制御装置。
【請求項４】
所定の入力装置からの指示に従って前記禁止領域を設定する設定処理部をさらに備えることを特徴とする請求項１、２または３に記載のロボット制御装置。
【請求項５】
前記設定処理部は、
作業対象物に関する情報を取得するとともに、取得した前記作業対象物に関する情報に基づいて前記禁止領域を変更することを特徴とする請求項４に記載のロボット制御装置。
【請求項６】
複数の関節軸を有するマニピュレータおよび当該マニピュレータを制御するロボット制御装置を備えるロボットシステムであって、
前記ロボット制御装置は、
目標位置に基づいて前記関節軸の角度を計算する角度計算部と、
前記角度計算部によって計算された前記関節軸の角度から特定される前記マニピュレータの各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、前記禁止領域に含まれる前記部位の位置を前記禁止領域外の位置へ修正する位置修正部と、
前記位置修正部によって修正された前記部位の位置に基づいて前記関節軸の角度を再計算する再計算部と
を備えることを特徴とするロボットシステム。
【請求項７】
入力操作を受け付ける入力装置
をさらに備え、
前記ロボット制御装置は、
前記入力装置からの指示に従って前記禁止領域または前記バネ・ダンパモデルの設定変更を行うことを特徴とする請求項６に記載のロボットシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置およびロボットシステムに関する。
【背景技術】
【０００２】
近年、産業用ロボットの一つとして、要求される位置姿勢の自由度よりも多くの自由度（冗長自由度）を有する多関節ロボットが開発されている。
【０００３】
この種の多関節ロボットは、冗長自由度を有することで、対象物に対して求められた位置姿勢を取った場合であっても、マニピュレータの姿勢を種々変更することができる。これにより、たとえば、比較的狭隘な場所であっても障害物を回避しながら作業を行うことが可能となる。なお、特許文献１には、冗長自由度を有するマニピュレータを制御するための一手法が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００７−２０３３８０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、多くのマニピュレータは、予め教示された動作経路に沿って動作するが、適用用途によっては、作業対象物が動いている等、位置が不確かな作業対象物に対して作業を行う場合もある。このような場合には、作業対象物の位置を検知するセンサ等の情報に基づいてマニピュレータの目標位置姿勢を修正することとなる。
【０００６】
しかしながら、マニピュレータが冗長自由度を有する場合、特許文献１のように予め冗長軸を含む各駆動軸全ての動作量を教示により決定しておく手法では、たとえば作業対象物の位置が移動した場合に冗長軸の位置が必ずしも適切な位置とならず、マニピュレータが作業対象物や周囲の障害物と接触する可能性がある。
【０００７】
開示の技術は、上記に鑑みてなされたものであって、冗長自由度を有するマニピュレータを適切に制御することができるロボット制御装置およびロボットシステムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
本願の開示するロボット制御装置は、複数の関節軸を有するマニピュレータを制御するロボット制御装置であって、目標位置に基づいて前記関節軸の角度を計算する角度計算部と、前記角度計算部によって計算された前記関節軸の角度から特定される前記マニピュレータの各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、前記禁止領域に含まれる前記部位の位置を前記禁止領域外の位置へ修正する位置修正部と、前記位置修正部によって修正された前記部位の位置に基づいて前記関節軸の角度を再計算する再計算部とを備える。
【０００９】
また、本願の開示するロボットシステムは、複数の関節軸を有するマニピュレータおよび当該マニピュレータを制御するロボット制御装置を備えるロボットシステムであって、前記ロボット制御装置は、目標位置に基づいて前記関節軸の角度を計算する角度計算部と、前記角度計算部によって計算された前記関節軸の角度から特定される前記マニピュレータの各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、前記禁止領域に含まれる前記部位の位置を前記禁止領域外の位置へ修正する位置修正部と、前記位置修正部によって修正された前記部位の位置に基づいて前記関節軸の角度を再計算する再計算部とを備える。
【発明の効果】
【００１０】
本願の開示するロボット制御装置およびロボットシステムの一つの態様によれば、冗長自由度を有するマニピュレータを適切に制御することができる。
【図面の簡単な説明】
【００１１】
【図１】図１は、本実施例に係るロボットシステムの構成を示す図である。
【図２】図２は、ロボット制御装置の構成を示すブロック図である。
【図３】図３は、マニピュレータの構成を示す図である。
【図４】図４は、禁止領域の一例を示す図である。
【図５−１】図５−１は、目標位置へ移動する前のマニピュレータを示す図である。
【図５−２】図５−２は、移動後のマニピュレータの位置が位置修正処理により修正される様子を示す図である。
【図６】図６は、位置修正処理の動作例を示す図である。
【図７】図７は、ロボット制御装置が実行する処理手順を示すフローチャートである。
【図８】図８は、バネ・ダンパモデルの設定位置の他の例を示す図である。
【発明を実施するための形態】
【００１２】
以下に添付図面を参照して、本願の開示するロボット制御装置およびロボットシステムのいくつかの実施例を詳細に説明する。ただし、これらの実施例における例示で本発明が限定されるものではない。
【実施例】
【００１３】
まず、本実施例に係るロボットシステムについて図１を用いて説明する。図１は、本実施例に係るロボットシステムの構成を示す図である。
【００１４】
なお、以下では、本願の開示するロボットシステムを、乳牛に対して搾乳作業を行う自動搾乳システムへ適用した場合の例について説明するが、本願の開示するロボットシステムは、これに限ったものではなく、たとえば家畜に対して検査や消毒等を自動的に行う場合にも適用することができる。また、本願の開示するロボットシステムは、家畜以外の対象物について所定の作業を行う場合にも適用することができる。
【００１５】
図１に示すように、本実施例に係るロボットシステムは、マニピュレータ１と、ロボット制御装置２と、入力装置３とを含んで構成される。
【００１６】
マニピュレータ１は、複数の関節（後述するアクチュエータ）を備える多関節ロボットである。マニピュレータ１は、エンドエフェクタとして搾乳部１００を備える。そして、マニピュレータ１は、ロボット制御装置２からの指示に従って各関節を個別に任意の角度だけ回転させることによって、搾乳部１００をターゲット部Ｔ（乳牛Ｃの乳頭）へ向けて移動させて搾乳を行う。
【００１７】
なお、マニピュレータ１の先端部（たとえば、搾乳部１００の近傍）には、ターゲット部Ｔを検知するための物体検知センサ１０１が設けられている。
【００１８】
ロボット制御装置２は、マニピュレータ１の動作制御を行うコントローラである。具体的には、ロボット制御装置２は、マニピュレータ１と通信ネットワークを介して相互に接続されており、かかる通信ネットワークを介し、マニピュレータ１に対して各関節の駆動を指示することによってマニピュレータ１を目標位置へ移動させる。
【００１９】
ここで、マニピュレータ１による搾乳作業は、図示しないゲージに乳牛Ｃを収容させた状態で行われる。このように乳牛Ｃをゲージに収容することで乳牛Ｃの動きが規制されるため、ターゲット部Ｔの位置を大まかに固定することができる。ただし、乳牛Ｃがゲージで動くことがあるため、ターゲット部Ｔの位置は細かく変化することとなる。
【００２０】
そこで、ロボット制御装置２は、物体検知センサ１０１の検知結果に基づいてターゲット部Ｔの位置を認識し、認識したターゲット部Ｔの位置を目標位置として搾乳部１００を移動させることとしている。これにより、図示しないゲージ内において不規則に動くターゲット部Ｔに追従してマニピュレータ１を移動させることが可能となる。
【００２１】
なお、ロボット制御装置２としては、たとえば、記憶装置や電子演算器あるいは表示装置等を備えるコンピュータを用いることができる。
【００２２】
入力装置３は、作業者等からの操作を受け付ける装置である。かかる入力装置３は、たとえば有線ＬＡＮ（Local  Area  Network）や無線ＬＡＮといった通信ネットワークで相互に接続されている。入力装置３は、作業者等からの操作に応じて、マニピュレータ１による搾乳作業の開始や終了あるいは各種の設定変更等をロボット制御装置２に対して指示する。
【００２３】
次に、ロボット制御装置２の構成について図２を用いて説明する。図２は、ロボット制御装置２の構成を示すブロック図である。なお、図２では、ロボット制御装置２の特徴を説明するために必要な構成要素のみを示しており、一般的な構成要素についての記載を省略している。
【００２４】
図２に示すように、ロボット制御装置２は、制御部２１と、記憶部２２とを備える。また、制御部２１は、角度計算部２１ａと、位置修正部２１ｂと、再計算部２１ｃと、動作指示部２１ｄと、設定処理部２１ｅとを備える。また、記憶部２２は、禁止領域データ２２ａと、バネ・ダンパモデルデータ２２ｂとを記憶する。
【００２５】
角度計算部２１ａは、物体検知センサ１０１の検知結果に基づいてマニピュレータ１の各関節の角度を計算する処理部である。具体的には、角度計算部２１ａは、物体検知センサ１０１の検知結果を用いて搾乳部１００の移動先となる目標位置を決定し、搾乳部１００の位置を目標位置と一致させるように、マニピュレータ１の各関節部に設けられたアクチュエータの回転角度を計算する。
【００２６】
ここで、マニピュレータ１の構成について図３を用いて説明する。図３は、マニピュレータ１の構成を示す図である。図３に示すように、マニピュレータ１は、基台１０と、第１リンク１１〜第６リンク１６と、フランジ１７と、搾乳部１００と、物体検知センサ１０１とを備える。
【００２７】
基台１０は、壁面や床面等の設置面Ｗに基端部が固定され、先端部において第１アクチュエータ１１ａ（第１関節軸）を介して第１リンク１１を回転可能に支持する構造材である。
【００２８】
第１リンク１１は、第１アクチュエータ１１ａが駆動することによって基台１０の先端部を中心として回転する構造材である。かかる第１リンク１１は、先端部において第２アクチュエータ１２ａ（第２関節軸）を介して第２リンク１２を回転可能に支持する。第２リンク１２は、第２アクチュエータ１２ａが駆動することによって第１リンク１１の先端部を中心として回転する構造材である。かかる第２リンク１２は、先端部において第３アクチュエータ１３ａ（第３関節軸）を介して第３リンク１３を回転可能に支持する。
【００２９】
第３リンク１３は、第３アクチュエータ１３ａが駆動することによって第２リンク１２の先端部を中心として回転する構造材である。かかる第３リンク１３は、先端部において第４アクチュエータ１４ａ（第４関節軸）を介して第４リンク１４を回転可能に支持する。第４リンク１４は、第４アクチュエータ１４ａが駆動することによって第３リンク１３の先端部を中心として回転する構造材である。かかる第４リンク１４は、先端部において第５アクチュエータ１５ａ（第５関節軸）を介して第５リンク１５を回転可能に支持する。
【００３０】
第５リンク１５は、第５アクチュエータ１５ａが駆動することによって第４リンク１４の先端部を中心として回転する構造材である。かかる第５リンク１５は、先端部において第６アクチュエータ１６ａ（第６関節軸）を介して第６リンク１６を回転可能に支持する。第６リンク１６は、第６アクチュエータ１６ａが駆動することによって第５リンク１５の先端部を中心として回転する構造材である。かかる第６リンク１６は、先端部において第７アクチュエータ１７ａ（第７関節軸）を介してフランジ１７と、フランジ１７に取り付けられた搾乳部１００および物体検知センサ１０１とを回転可能に支持する。
【００３１】
なお、本実施例では、第３アクチュエータ１３ａが冗長軸としてあらかじめ設定されているものとするが、冗長軸として設定されるアクチュエータは、第３アクチュエータ１３ａ以外であってもよい。このように、マニピュレータ１が冗長軸を備えることで、ターゲット部Ｔに対して求められた位置姿勢を取った場合であってもマニピュレータ１の姿勢を種々変更することができる。
【００３２】
また、第１アクチュエータ１１ａ〜第７アクチュエータ１７ａは、たとえば、減速機一体型のサーボモータによって構成される。また、第１アクチュエータ１１ａ〜第７アクチュエータ１７ａには、各アクチュエータ１１ａ〜１７ａの回転位置を検出するエンコーダがそれぞれ内蔵される。かかるエンコーダによって検出された回転位置は、通信ネットワークを介してロボット制御装置２へ入力されることとなる。
【００３３】
物体検知センサ１０１は、ターゲット部Ｔを検知するためのセンサであり、ターゲット部Ｔの動作範囲を十分にカバーする検知領域を備える。ここでは、物体検知センサ１０１がカメラであるものとするが、これに限ったものではなく、３次元計測が可能なレーザセンサ等を物体検知センサ１０１として用いてもよい。
【００３４】
物体検知センサ１０１によって撮像された画像の画像情報は、通信ネットワークを介してロボット制御装置２の角度計算部２１ａへリアルタイムに入力される。
【００３５】
図２へ戻り、角度計算部２１ａについての説明を続ける。角度計算部２１ａは、物体検知センサ１０１から入力される画像情報に基づいてターゲット部Ｔの位置を検出する。これにより、ロボット制御装置２は、ターゲット部Ｔの位置をリアルタイムに認識することができる。なお、画像情報からターゲット部Ｔの位置を検出する手法としては、いずれの公知技術を用いてもかまわない。
【００３６】
つづいて、角度計算部２１ａは、検出したターゲット部Ｔの位置を目標位置として決定し、かかる目標位置と搾乳部１００の位置とを一致させるように第１アクチュエータ１１ａ〜第７アクチュエータ１７ａの角度をそれぞれ計算する。これにより、マニピュレータ１の次の位置姿勢が仮決定される。
【００３７】
なお、本実施例では、第４アクチュエータ１４ａ〜第７アクチュエータ１７ａの駆動により直線移動させることが可能な点を制御点としている。制御点とは、マニピュレータ１の動作位置を制御する位置である。具体的には、第６アクチュエータ１６ａの軸心付近に制御点が位置する。角度計算部２１ａは、目標位置に基づいて制御点の位置を決定した後、決定した制御点の位置に基づいて第１アクチュエータ１１ａ〜第７アクチュエータ１７ａの角度を計算する。なお、第１アクチュエータ１１ａ〜第７アクチュエータ１７ａの角度の計算には、逆キネマティクス演算等の一般的な手法を用いることができる。
【００３８】
角度計算部２１ａは、算出した各アクチュエータ１１ａ〜１７ａの角度データを位置修正部２１ｂへ渡す。
【００３９】
位置修正部２１ｂは、角度計算部２１ａによって計算された各アクチュエータ１１ａ〜１７ａの角度から特定されるマニピュレータ１の各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、禁止領域に含まれる部位の位置を禁止領域外の位置へ修正する処理部である。
【００４０】
具体的には、位置修正部２１ｂは、角度計算部２１ａから各アクチュエータ１１ａ〜１７ａの角度データを受け取ると、まず、受け取った角度データから各部位の移動後の位置座標を特定する。ここでは、第１リンク１１〜第６リンク１６、フランジ１７および第１アクチュエータ１１ａ〜第７アクチュエータ１７ａ等の位置座標を特定することとなる。
【００４１】
つづいて、位置修正部２１ｂは、特定した各部位の位置座標および記憶部２２に記憶された禁止領域データ２２ａを用い、移動によりマニピュレータ１が禁止領域に侵入することとなるか否かを判定する。禁止領域とは、障害物等との接触を避けるためにマニピュレータ１の侵入を禁止した領域である。禁止領域データ２２ａは、かかる禁止領域の位置座標を含んだデータである。
【００４２】
ここで、本実施例において設定される禁止領域について図４を用いて説明する。図４は、禁止領域の一例を示す図である。
【００４３】
図４に示すように、本実施例では、図示しないゲージに収容された乳牛Ｃの左後脚ＣＬおよび右後脚ＣＲが位置する領域として統計的に割り出された領域を覆うように、禁止領域Ａ１および禁止領域Ａ２がそれぞれ設定される。
【００４４】
なお、本実施例では、マニピュレータ１が、乳牛Ｃの背後から乳牛Ｃの両後脚ＣＬ，ＣＲの間を通ってターゲット部Ｔへ向かうこととしている。このため、上記のように左後脚ＣＬおよび右後脚ＣＲの周囲を覆うような禁止領域Ａ１，Ａ２を設定することとしたが、禁止領域は、マニピュレータがどの位置からターゲット部へ向かうかによって適宜変更可能である。たとえば、マニピュレータが乳牛の左側面からターゲット部へ向かう仕様である場合には、左後脚の周囲を覆う領域のみを禁止領域として設定してもよい。
【００４５】
また、ここでは、禁止領域Ａ１，Ａ２を搾乳対象となる乳牛Ｃに依らず一様に適用する場合の例について説明するが、搾乳対象となる乳牛Ｃの体格に応じて禁止領域Ａ１，Ａ２を動的に変化させてもよい。
【００４６】
たとえば、乳牛Ｃに設けられたＩＤタグから乳牛Ｃの体格に関する情報（サイズや重量など）を取得し、取得した情報に基づいて禁止領域の大きさを変化させてもよい。このようにすれば、搾乳対象となる乳牛Ｃの大きさに合わせて禁止領域Ａ１，Ａ２を適切に設定することができる。
【００４７】
また、禁止領域Ａ１，Ａ２は、作業者が入力装置３を用いて手動で変更することもできる。禁止領域Ａ１，Ａ２の設定変更処理は、後述する設定処理部２１ｅによって行われる。
【００４８】
図２へ戻り、位置修正部２１ｂの説明を続ける。位置修正部２１ｂは、移動後におけるマニピュレータ１の何れかの位置座標が、禁止領域Ａ１（または禁止領域Ａ２）に含まれる場合に、移動後のマニピュレータ１が禁止領域Ａ１（または禁止領域Ａ２）に侵入すると判定する。
【００４９】
移動後のマニピュレータ１が禁止領域Ａ１（または禁止領域Ａ２）に侵入すると判定すると、位置修正部２１ｂは、位置修正処理を行う。位置修正処理とは、移動によって禁止領域Ａ１（または禁止領域Ａ２）へ侵入することとなるマニピュレータ１の部位の位置を禁止領域Ａ１および禁止領域Ａ２外の位置へ修正する処理である。
【００５０】
ここで、位置修正部２１ｂにより実行される位置修正処理について図５−１および図５−２を用いて説明する。図５−１は、目標位置へ移動する前のマニピュレータ１を示す図であり、図５−２は、移動後のマニピュレータ１の位置が位置修正処理により修正される様子を示す図である。なお、図５−２では、目標位置への移動によってマニピュレータ１の第４リンク１４が禁止領域Ａ１へ侵入することとなる場合の例について示している。
【００５１】
図５−１に示すように、ロボット制御装置２では、角度計算部２１ａが、物体検知センサ１０１から入力される画像情報に基づいて目標位置Ｓを決定するとともに、決定した目標位置Ｓに基づいて制御点の位置を決定する。
【００５２】
そして、角度計算部２１ａは、現在の制御点の位置が決定した制御点の位置と一致するように各アクチュエータ１１ａ〜１７ａの角度を計算する。これにより、目標位置Ｓへ移動後のマニピュレータ１の位置姿勢（各部位の位置座標）が特定される。目標位置Ｓへ移動後のマニピュレータ１を図５−２において破線で示す。
【００５３】
ここで、図５−２に示すように、目標位置Ｓへの移動によってマニピュレータ１の第４リンク１４が禁止領域Ａ１に侵入するとする。かかる場合、位置修正部２１ｂは、第４リンク１４の位置を禁止領域Ａ１外の位置へ修正する。
【００５４】
具体的には、位置修正部２１ｂは、禁止領域Ａ１の境界面と第４リンク１４との間に仮想的なバネ・ダンパモデルを設定する。そして、位置修正部２１ｂは、第４リンク１４の移動によってバネ・ダンパモデルへ加わる力と、仮想バネの弾性力および仮想ダンパの粘性力とがつりあう位置（平衡点）を算出し、この平衡点の位置を第４リンク１４の移動後の位置として修正する。
【００５５】
以下、位置修正処理の内容について図６を用いてさらに詳細に説明する。図６は、位置修正処理の動作例を示す図である。図６では、移動前のマニピュレータ１を実線で、移動後のマニピュレータ１を破線でそれぞれ示している。なお、位置修正部２１ｂは、各アクチュエータ１１ａ〜１７ａから出力されるエンコーダ値（各アクチュエータ１１ａ〜１７ａの回転位置）からマニピュレータ１の移動前の位置姿勢を特定することが可能である。
【００５６】
図６に示すように、位置修正部２１ｂは、まず、移動後の第４リンク１４と禁止領域Ａ１との交点Ｐ１の座標を算出する。つづいて、位置修正部２１ｂは、交点Ｐ１と、交点Ｐ１に位置することとなる第４リンク１４の移動前の位置Ｐ１’との間に、バネ・ダンパモデルＭを仮想的に設定する。かかるバネ・ダンパモデルＭは、記憶部２２に記憶されたバネ・ダンパモデルデータ２２ｂを用いて設定される。
【００５７】
バネ・ダンパモデルデータ２２ｂは、仮想バネのバネ定数および自然長、仮想ダンパのダンパ定数等の数値を含んだデータである。なお、かかるバネ・ダンパモデルデータ２２ｂは、作業者等が入力装置３を操作することによって任意に設定変更することができる。
【００５８】
つづいて、位置修正部２１ｂは、第４リンク１４の移動によってバネ・ダンパモデルＭに加わる力を算出する。そして、位置修正部２１ｂは、算出した力とバネ・ダンパモデルデータ２２ｂとして記憶されたバネ定数等の数値を用いて平衡点の位置座標を算出する。
【００５９】
具体的には、第４リンク１４の移動によってバネ・ダンパモデルＭに加わる力をＦ、仮想ダンパのダンパ定数をＤ、バネ定数をｋ、仮想バネの自然長をｘ０とすると、位置修正部２１ｂは、
【数１】
2012240144.tif 000003
式（１）を用いて、未知数ｘを求める。未知数ｘは、第４リンク１４がバネ・ダンパモデルＭへ作用させる力とバネ・ダンパモデルＭが第４リンク１４へ作用させる力とがつりあった状態における仮想バネの長さである。
【００６０】
位置修正部２１ｂは、かかる未知数ｘを算出することで、平衡点の位置座標を得ることができる。そして、位置修正部２１ｂは、かかる平衡点の位置座標を位置Ｐ１’の移動後の位置座標として再計算部２１ｃへ渡す。
【００６１】
図２へ戻り、再計算部２１ｃについて説明する。再計算部２１ｃは、位置修正部２１ｂから受け取った第４リンク１４の修正後の位置座標に基づいて、各アクチュエータ１１ａ〜１７ａの角度を再計算する処理部である。
【００６２】
具体的には、再計算部２１ｃは、搾乳部１００が目標位置Ｓに位置した状態を維持しつつ、第４リンク１４の位置Ｐ１’が平衡点に位置するように、各アクチュエータ１１ａ〜１７ａの角度を再計算する。
【００６３】
なお、マニピュレータ１は、冗長軸を備えるため、搾乳部１００が目標位置Ｓに位置した状態を維持しつつ、その他の部位の位置姿勢を変更することができる。再計算部２１ｃは、再計算後の各アクチュエータ１１ａ〜１７ａの角度を動作指示部２１ｄへ渡す。
【００６４】
動作指示部２１ｄは、再計算部２１ｃから受け取った各アクチュエータ１１ａ〜１７ａの角度に基づいて各アクチュエータ１１ａ〜１７ａを回転駆動させる処理部である。これにより、マニピュレータ１は、図５−２において実線で示したように禁止領域Ａ１へ侵入することなく目標位置Ｓへ移動することとなる。
【００６５】
設定処理部２１ｅは、入力装置３からの指示に従って禁止領域データ２２ａまたはバネ・ダンパモデルデータ２２ｂの内容を変更する処理部である。具体的には、設定処理部２１ｅは、入力装置３からの指示に従って、禁止領域Ａ１，Ａ２の大きさや位置を変更したり、仮想バネのバネ定数や自然長、仮想ダンパのダンパ定数等を変更したりする。
【００６６】
なお、設定処理部２１ｅは、上述したように、外部から乳牛Ｃの体格に関する情報を取得し、取得した情報に基づいて禁止領域データ２２ａの内容を変更するようにしてもよい。
【００６７】
記憶部２２は、不揮発性メモリやハードディスクドライブといった記憶デバイスで構成され、禁止領域データ２２ａと、バネ・ダンパモデルデータ２２ｂとを記憶する。
【００６８】
なお、本実施例では、ロボット制御装置２と入力装置３とを別体として説明したが、これに限ったものではなく、ロボット制御装置２と入力装置３とを１つの装置として構成してもよい。また、本実施例では、物体検知センサ１０１からの画像情報に基づいて目標位置を決定する処理をロボット制御装置２で行うこととしたが、かかる処理を他の装置で行うようにしてもよい。
【００６９】
次に、ロボット制御装置２の具体的動作について図７を用いて説明する。図７は、ロボット制御装置２が実行する処理手順を示すフローチャートである。
【００７０】
図７に示すように、ロボット制御装置２では、角度計算部２１ａが、物体検知センサ１０１からの画像情報を取得し（ステップＳ１０１）、取得した画像情報に基づいて目標位置を決定する（ステップＳ１０２）。また、角度計算部２１ａは、決定した目標位置に基づいて各アクチュエータ１１ａ〜１７ａの角度を計算する（ステップＳ１０３）。
【００７１】
つづいて、ロボット制御装置２では、位置修正部２１ｂが、マニピュレータ１の各部位の移動後の位置を特定し（ステップＳ１０４）、特定した各部位の位置が禁止領域Ａ１（または禁止領域Ａ２）に含まれるか否かを判定する（ステップＳ１０５）。
【００７２】
かかる処理において、マニピュレータ１の各部位の移動後の位置が禁止領域Ａ１（または禁止領域Ａ２）に含まれない場合（ステップＳ１０５，Ｎｏ）、動作指示部２１ｄは、ステップＳ１０３において算出された各アクチュエータ１１ａ〜１７ａの角度に基づいてマニピュレータ１に対して動作指示を行って（ステップＳ１０８）、処理を終了する。
【００７３】
一方、マニピュレータ１の各部位の移動後の位置が禁止領域Ａ１（または禁止領域Ａ２）に含まれると判定した場合（ステップＳ１０５，Ｙｅｓ）、位置修正部２１ｂは、バネ・ダンパモデルデータ２２ｂを用いて位置修正処理を行う（ステップＳ１０６）。
【００７４】
そして、ロボット制御装置２では、再計算部２１ｃが、修正後の位置に基づいて各アクチュエータ１１ａ〜１７ａの角度を再計算し（ステップＳ１０７）、動作指示部２１ｄが、各アクチュエータ１１ａ〜１７ａの再計算後の角度に基づいてマニピュレータ１に対して動作指示を行って（ステップＳ１０８）、処理を終了する。
【００７５】
なお、ここでは、ステップＳ１０３において第１アクチュエータ１１ａ〜第７アクチュエータ１７ａの角度を全て計算することとしたが、これに限ったものではない。たとえば、ステップＳ１０３では基本軸（たとえば、第１アクチュエータ１１ａ〜第４アクチュエータ１４ａ）の角度のみを計算し、ステップＳ１０７において基本軸の角度の再計算を行った後、残りの軸（たとえば、第５アクチュエータ１５ａ〜第７アクチュエータ１７ａ）の角度を計算することとしてもよい。
【００７６】
上述してきたように、本実施例では、角度計算部が、目標位置に基づいて各アクチュエータの角度を計算し、位置修正部が、角度計算部によって計算された各アクチュエータの角度から特定されるマニピュレータの各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、禁止領域に含まれる部位の位置を禁止領域外の位置へ修正し、再計算部が、位置修正部によって修正された部位の位置に基づいて各アクチュエータの角度を再計算することとした。したがって、冗長自由度を有するマニピュレータを適切に制御することができる。
【００７７】
また、本実施例では、位置修正部が、禁止領域に含まれることとなる部位と禁止領域の境界面との間に仮想的なバネ・ダンパモデルを設定し、当該部位がバネ・ダンパモデルへ作用させる力とバネ・ダンパモデルが当該部位へ作用させる力とがつりあう位置を当該部位の修正後の位置とすることとした。
【００７８】
これにより、マニピュレータの修正後の位置姿勢が一意に決定されるため、たとえば、マニピュレータの修正後の位置姿勢をシミュレーション等により探索的に決定する場合と比較して演算量を削減することができる。
【００７９】
したがって、本実施例のように作業対象物の位置が変化する場合であっても、障害物等との接触を適切に回避しつつ、マニピュレータを作業対象物に対してリアルタイムに追従させることが可能となる。
【００８０】
また、本実施例では、位置修正部が、禁止領域に含まれることとなる部位と禁止領域との交点を算出し、算出した交点に基づいてバネ・ダンパモデルの設定位置を決定することとした。したがって、バネ・ダンパモデルの設定位置を少ない演算量で決定することができる。
【００８１】
ところで、上述してきた実施例では、移動により禁止領域へ侵入することとなるリンクと禁止領域との交点をバネ・ダンパモデルの設定位置として決定することとしたが、バネ・ダンパモデルの設定位置は、これに限ったものではない。
【００８２】
以下では、バネ・ダンパモデルの設定位置の他の例について図８を用いて説明する。図８は、バネ・ダンパモデルの設定位置の他の例を示す図である。なお、図８では、移動の途中で禁止領域Ａ１に接した状態となったマニピュレータ１を一点鎖線で、移動後のマニピュレータ１を破線でそれぞれ示している。
【００８３】
図８に示すように、禁止領域Ａ１に侵入することとなる第４リンク１４と禁止領域Ａ１との交点をそれぞれＰ１，Ｐ２とすると、バネ・ダンパモデルＭの第４リンク１４側への設定位置は、交点Ｐ１，Ｐ２に限らず、第４リンク１４の線分Ｐ１，Ｐ２上の何れの位置であってもよい。
【００８４】
たとえば、交点Ｐ１および交点Ｐ２の中点Ｐ３を、バネ・ダンパモデルＭの第４リンク１４側への設定位置としてもよいし、第４リンク１４が禁止領域Ａ１と最初に接する点Ｐ４をバネ・ダンパモデルＭの第４リンク１４側への設定位置としてもよい。
【００８５】
また、バネ・ダンパモデルＭの禁止領域Ａ１側への設定位置についても、禁止領域Ａ１の線分Ｐ１，Ｐ２上の何れの位置であってもよい。
【００８６】
また、上述してきた実施例では、搾乳対象となる乳牛に依らず禁止領域を固定とする（もしくは、乳牛の体格に応じて動的に変更する）こととしたが、これに限ったものではない。
【００８７】
たとえば、作業対象物である乳牛の乳頭と障害物である両後脚とは連繋しているため、乳頭の位置がずれれば両後脚の位置もずれる可能性がある。そこで、物体検知センサからの画像情報に基づいて決定されるターゲット部の位置に応じて、禁止領域の位置または大きさを動的に変更するようにしてもよい。
【００８８】
このようにすれば、マニピュレータ１と乳牛の後脚との接触をより確実に回避することができる。
【００８９】
また、上述してきた実施例では、マニピュレータの一例として、７個のアクチュエータを有する７自由度のマニピュレータを用いて説明したが、マニピュレータの自由度は、これに限ったものではなく、８自由度以上の自由度を有するマニピュレータであってもよい。
【００９０】
また、６自由度以下のマニピュレータであっても、作業対象物へのアプローチに１以上の自由度が許容される場合には、相対的にマニピュレータが冗長自由度を有することになるが、そのような場合であっても本発明を適用することができる。
【００９１】
さらなる効果や変形例は、当業者によって容易に導き出すことができる。このため、本発明のより広範な態様は、以上のように表しかつ記述した特定の詳細および代表的な実施の形態に限定されるものではない。したがって、添付の特許請求の範囲およびその均等物によって定義される総括的な発明の概念の精神または範囲から逸脱することなく、様々な変更が可能である。
【符号の説明】
【００９２】
Ｃ  乳牛
Ｔ  ターゲット部
Ｗ  設置面
ＣＬ  左後脚
ＣＲ  右後脚
Ａ１  禁止領域（左後脚側）
Ａ２  禁止領域（右後脚側）
Ｍ  バネ・ダンパモデル
１  マニピュレータ
１１  第１リンク
１２  第２リンク
１３  第３リンク
１４  第４リンク
１５  第５リンク
１６  第６リンク
１７  フランジ
１１ａ  第１アクチュエータ
１２ａ  第２アクチュエータ
１３ａ  第３アクチュエータ
１４ａ  第４アクチュエータ
１５ａ  第５アクチュエータ
１６ａ  第６アクチュエータ
１７ａ  第７アクチュエータ
１００  搾乳部
１０１  物体検知センサ
２  ロボット制御装置
２１  制御部
２１ａ  角度計算部
２１ｂ  位置修正部
２１ｃ  再計算部
２１ｄ  動作指示部
２１ｅ  設定処理部
２２  記憶部
２２ａ  禁止領域データ
２２ｂ  バネ・ダンパモデルデータ
３  入力装置
【図１】
2012240144.tif 000004
【図２】
2012240144.tif 000005
【図３】
2012240144.tif 000006
【図４】
2012240144.tif 000007
【図５−１】
2012240144.tif 000008
【図５−２】
2012240144.tif 000009
【図６】
2012240144.tif 000010
【図７】
2012240144.tif 000011
【図８】
2012240144.tif 000012