ロボットシステムまたはロボット制御装置
【課題】冗長な自由度を有するマニピュレータをより最適に制御することができるようにしたロボットシステム及びロボット制御装置を提供する。
【解決手段】１以上のアクチュエータを有するマニピュレータ２とこれを制御するコントローラ３とを有し、コントローラ３は、アクチュエータの内の一部を冗長軸と設定し、マニピュレータの目標位置姿勢を設定し、マニピュレータが現在の姿勢から目標位置姿勢に達するまでの動作軌跡を生成し、マニピュレータの到達可能範囲内の領域を分割して設定した小領域と各小領域に対応する冗長軸角度用パラメータとを対応付けた冗長角度定義テーブル７を有し、冗長角度定義テーブル中の該当する小領域を選択し、選択結果に基づいて冗長軸角度を設定し、動作軌跡と冗長軸角度とに基づいて動作指令を生成する。
【解決手段】１以上のアクチュエータを有するマニピュレータ２とこれを制御するコントローラ３とを有し、コントローラ３は、アクチュエータの内の一部を冗長軸と設定し、マニピュレータの目標位置姿勢を設定し、マニピュレータが現在の姿勢から目標位置姿勢に達するまでの動作軌跡を生成し、マニピュレータの到達可能範囲内の領域を分割して設定した小領域と各小領域に対応する冗長軸角度用パラメータとを対応付けた冗長角度定義テーブル７を有し、冗長角度定義テーブル中の該当する小領域を選択し、選択結果に基づいて冗長軸角度を設定し、動作軌跡と冗長軸角度とに基づいて動作指令を生成する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、マニピュレータを用いたロボットシステム及びロボット制御装置に関する。
【背景技術】
【０００２】
多関節ロボットでは、求められる位置姿勢の自由度よりも多くの自由度（冗長自由度）を有するロボットを制御することが求められる場合がある。
即ち、ロボットが冗長自由度を有することで、対象物に対して求められた位置姿勢を取った際であってもマニピュレータの姿勢を種々変更することができるため、冗長自由度を適切に制御することで比較的狭隘な場所であっても作業時にマニピュレータ自身や周辺の物体との干渉を回避しながら作業を行えるという利点がある。
特許文献１には、冗長自由度を有するマニピュレータを制御するための一手法が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−２０３３８０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、多くのマニピュレータは予め教示された動作経路に沿って動作するが、適用用途によっては、作業対象物が動いている等、位置が不確かな作業対象物に対して作業を行なう場合もある。このような場合には作業対象物の位置を検知するセンサ等の情報に基づいてマニピュレータの目標位置姿勢を修正する。これにより、マニピュレータを作業対象物に追従させながら作業を行なうことができる。
しかしながら、マニピュレータが冗長自由度を有する場合、特許文献１のように、予め冗長自由度を含む各駆動軸全ての動作量を教示により決定しておく手法では、上述のように作業対象物の位置が移動する場合、冗長自由度の位置が必ずしも適切な位置とならず、作業対象物やマニピュレータ自身と干渉（接触）等が生じることが考えられる。
また、教示された動作経路に対して、作業対象物に追従して動作経路が修正されることにより、意図せずにマニピュレータが特異点姿勢となりマニピュレータの制御上、支障が生じることも考えられる。
このような不都合はセンサ等に基づいて目標位置姿勢を修正する場合に限らず、マニピュレータの動作範囲に存在する障害物が移動する場合やマニピュレータと周囲物との意図しない接触が生じた場合にマニピュレータの動作軌道を修正する場合等にも同様の不都合が生じうる。
【０００５】
本発明はこのような課題に鑑みてなされたもので、冗長自由度を有するマニピュレータをより最適に制御することができるようにしたロボットシステム及びロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記課題を解決するため、本発明のロボットシステムは、１以上のアクチュエータを有するマニピュレータと、アクチュエータそれぞれの動作を制御するコントローラと、を有し、コントローラは、アクチュエータの内の一部を冗長軸と設定する冗長軸設定部と、マニピュレータの目標位置姿勢を設定する目標位置姿勢生成部と、目標位置姿勢に基づいてマニピュレータが現在の姿勢から目標位置姿勢に達するまでの動作軌跡を生成するシミュレーション部と、マニピュレータの到達可能範囲内の領域を分割して設定した小領域と各小領域に対応する冗長軸角度用パラメータとを対応付けた冗長角度定義テーブルと、目標位置姿勢生成部の出力結果に基づいて冗長角度定義テーブル中の該当する小領域を選択する領域判定手段と、領域判定手段の選択結果と冗長角度定義テーブルとに基づいて、冗長軸角度を設定する冗長軸角度設定部と動作軌跡と冗長軸角度とに基づいて、アクチュエータのそれぞれに対して動作指令を生成する補間演算部と、を有していることを特徴としている。
【０００７】
また、作業対象物の位置を検知する物体検出センサを有し、目標位置姿勢生成部は、物体検出センサの検知結果に基づいて目標位置姿勢を修正することが好ましい。
【０００８】
また、マニピュレータにかかる外力を検出する外力センサと、を有し、コントローラは、アクチュエータの動作を制御するとともに、センサ部の検出結果に基づいてアクチュエータが外力に対して回避する方向に回避動作を行なわせる回避動作実行部を有していることが好ましい。
また、コントローラは、冗長角度定義テーブルを表示するとともに、冗長角度定義テーブルの内容の修正等を受け付けるテーブル表示・入力部を有していることが好ましい。
また、マニピュレータは、基台と、基台に対して第１構造材を動作させる第１関節と、第1構造材に対して第２構造材を動作させる第２関節と、第２構造材に対して第３構造材を動作させる第３関節と、第３構造材に対して第４構造材を動作させる第４関節と、第４構造材に対して第５構造材を動作させる第５関節と、第５構造材に対して第６構造材を動作させる第６関節と、第６構造材に対してフランジを旋回させる第７関節と、を有していることが好ましい。
【０００９】
また、冗長角度定義テーブルに規定される小領域はそれぞれその形状を直方体とすることが好ましい。
また、冗長角度定義テーブルに規定される小領域は、作業対象物の位置のバラつきを考慮して予め設定されることが好ましい。
【００１０】
領域判定手段は、目標位置姿勢生成部の出力結果から冗長角度定義テーブル中の該当する小領域が存在しないと判定した場合には異常信号を物体検出センサに出力することが好ましい。
また、本発明のロボット制御装置は、１以上のアクチュエータを有するマニピュレータの動作を制御するロボット制御装置であって、アクチュエータの内の一部を冗長軸と設定する冗長軸設定部と、マニピュレータの目標位置姿勢を設定する目標位置姿勢生成部と、目標位置姿勢に基づいてマニピュレータが現在の姿勢から目標位置姿勢に達するまでの動作軌跡を生成するシミュレーション部と、マニピュレータの到達可能範囲内の領域を分割して設定した小領域と各小領域に対応する冗長軸角度用パラメータとを対応付けた冗長角度定義テーブルと、目標位置姿勢生成部の出力結果に基づいて冗長角度定義テーブル中の該当する小領域を選択する領域判定手段と、領域判定手段の選択結果と冗長角度定義テーブルとに基づいて、冗長軸角度を設定する冗長軸角度設定部と動作軌跡と冗長軸角度とに基づいて、アクチュエータのそれぞれに対して動作指令を生成する補間演算部と、を有していることを特徴としている。
【発明の効果】
【００１１】
本発明によれば、目標となる位置姿勢が変更される場合でもより少ない演算量でマニピュレータの冗長軸を適切に設定することができ、冗長な自由度を有するマニピュレータをより最適に制御することができる。
【図面の簡単な説明】
【００１２】
【図１】本発明の実施形態に係るロボットシステムの全体構成を説明するための模式的な構成図
【図２】本発明の実施形態にかかるロボットの構成を示す側面図である。
【図３】センサ部の構成を示す模式的な構造図
【図４】冗長角度定義テーブルに規定される３次元空間の領域を説明するための模式図
【図５】エンドエフェクタとターゲット部（作業対象物）座標関係を模式的に示す図
【図６】冗長角度定義テーブルの一例を示す図
【発明を実施するための形態】
【００１３】
以下、本発明の実施の形態について図を参照して説明する。
本実施形態では、マニピュレータ２を用いて家畜に対して検査、消毒、搾乳などの作業を行なうロボットシステムを例に本発明の一実施形態について説明する。
【００１４】
図１に示すように、本実施形態にかかるロボットシステム１００は、家畜１が収納されたフロアＦ，マニピュレータ２，コントローラ３，センサ部４からなっている。
マニピュレータ２は、図２に示すように、設置面（壁面や床等）１０１に固定された基台４０と、基台４０からマニピュレータ２の先端にかけて順々に第１構造材４１，第２構造材４２，第３構造材４３，第４構造材４４，第５構造材４５，第６構造材４６，フランジ４７がそれぞれ回転駆動するアクチュエータ（回転関節）を介して連結されている。
【００１５】
基台４０と第１構造材４１とは、第１アクチュエータ（第１関節）４１Ａを介して連結されており、第１アクチュエータ４１Ａの駆動により、第１構造材４１が回転するようになっている。第１構造材４１と第４構造材４２とは、第２アクチュエータ（第２関節）４２Ａを介して連結されており、第２アクチュエータ４２Ａの駆動により、第２構造材４２が旋回するようになっている。
【００１６】
第２構造材４２と第３構造材４３とは、第３アクチュエータ（第３関節）４３Ａを介して連結されており、第３アクチュエータ４３Ａの駆動により、第３構造材４３が回転するようになっている。
第３構造材４３と第４構造材４４とは、第４アクチュエータ（第４関節）４４Ａを介して連結されており、第４アクチュエータ４４Ａの駆動により、第４構造材４４が旋回するようになっている。
【００１７】
第４構造材４４と第５構造材４５とは、第５アクチュエータ（第５関節）４５Ａを介して連結されており、第５アクチュエータ４５Ａの駆動により、第５構造材４５が回転するようになっている。第５構造材４５と第６構造材４６とは、第６アクチュエータ（第６関節）４６Ａを介して連結されており、第６アクチュエータ４６Ａの駆動により、第６構造材４６が旋回するようになっている。
第６構造材４６とフランジ４７とは、第７アクチュエータ（第７関節）４７Ａを介して連結されており、第７アクチュエータ４７Ａの駆動により、フランジ４７及びフランジ４７に取り付けられるハンド等のエンドエフェクタ２Ａが回転するようになっている。
【００１８】
なお、本実施形態では、第３アクチュエータ（第３関節）４３Ａが冗長軸として予め設定されており（冗長軸設定部）、後述するコントローラ３の機能により第３アクチュエータ４３Ａの回転位置が最適に制御されることとなる。また、アクチュエータ４３Ａを冗長軸、各アクチュエータ４１Ａ，４２Ａをそれぞれ第１軸，第２軸とし、各アクチュエータ４４Ａ〜４７Ａをそれぞれ第３軸〜第６軸という。第１軸〜第６軸を特定しない場合は単に軸ともいう。
【００１９】
エンドエフェクタ２Ａには、検査器，搾乳器，消毒器等の種々のツール（図示は省略）が取り付けられ、家畜１のターゲット部１Ａに対して、検査、消毒、搾乳などの作業を行なうようになっている。なお、マニピュレータ２の動作位置を制御する位置である制御点は本実施形態のマニピュレータ２の場合、制御点は各アクチュエータ４４Ａ〜４７Ａの駆動により直線移動させることが可能な点としており、第６アクチュエータ４６Ａの軸心付近に位置している。
そして、制御点とフランジ４７Ａとエンドエフェクタ２Ａ及び各ツールとの位置関係は予めコントローラ３に入力されており、制御点の位置姿勢を制御することで各ツールの位置を制御できるようになっている。
また、エンドエフェクタ（換言するとマニピュレータ２の先端部分）２Ａには、物体検知センサ（本実施形態では、カメラであるがその他の種々のセンサを適用可能である）２Ｂが取り付けられている。
【００２０】
物体検知センサ２Ｂは、四方が仕切られたフロアＦ内で不規則に動く家畜１のターゲット部１Ａを含む十分な広さの検知領域に対して配向されている。
物体検知センサ２Ｂにより取得した画像はコントローラ３の目標認識部２Ｃに入力され、画像認識部２Ｃでは家畜１のターゲット部１Ａの位置を検出し、検出結果を後述する動作指令部５に目標位置としてリアルタイムに入力するようになっている。
なお、本実施形態では、説明を容易とするため、コントローラ３は一体の制御装置として示しているが実際にはコントローラ３は複数のコンピュータ等で構成し、目標認識部２Ｃや外力検出部（後述）については、マニピュレータ２の動作を制御するロボットコントローラとは別体の制御装置により構成する等してもよい。
【００２１】
また、第１〜第７アクチュエータ４１Ａ〜４７Ａは、それぞれ、ケーブル（図示省略）を挿通可能な中空部を有する減速機一体型のサーボモータによって構成されており、各アクチュエータの回転位置は、アクチュエータに内蔵のエンコーダからの信号としてコントローラ３にケーブルを介して入力されるようになっている。
【００２２】
センサ部４は図３に示すように、センサ固定治具２５と４個のセンサＳ１〜Ｓ４により構成されており、円盤状のセンサ固定治具２５はマニピュレータ２の第１アクチュエータ４１Ａの固定子の基部に取り付けられている。
【００２３】
各センサＳ１〜Ｓ４は、円盤状のセンサ固定治具２５に埋設されており、各センサＳ１〜Ｓ４は同一円弧（仮想円）に沿って等間隔に配置されている。そして、各センサＳ１〜Ｓ４には圧電体として水晶が用いられており、各センサＳ１〜Ｓ４はそれぞれセンサ固定治具１５のラジアル方向の歪量を電圧として検出し、得られた電圧はアンプ部（図示省略）を介して増幅され、コントローラ３に入力されるようになっている。
各センサＳ１〜Ｓ４に水晶振動子（水晶圧電素子）を用いることで歪みゲージや他の圧電素子を用いた場合よりも応答時間が小さく後述するサーボ部１４の演算周期と比較して良好な応答性を得ることができ、マニピュレータ周辺の対象物の接触等による衝撃を十分に緩和することができる。
【００２４】
なお、本実施形態では上述のように応答時間の早さ等の理由から水晶振動子を用いているが、良好な応答性が得られるものであれば各センサＳ１〜Ｓ４にどのような形式のものでも適用可能である。また、水晶圧電センサは、歪みゲージや通常の衝突センサと比較して耐久性に優れ、マニピュレータ２の自重による負荷が最も大きい基端部にセンサ部４設けた場合でも十分に高精度な検出ができるという利点もある。
マニピュレータ２の各アクチュエータ４１Ａ〜４７Ａを含む各駆動部位の動作はコントローラ３で制御される。
【００２５】
コントローラ３は、記憶装置，電子演算器，入力装置及び表示装置（いずれも詳細には図示省略）を有するコンピュータにより構成されており、マニピュレータ２の各駆動部位と相互通信可能に接続されている。
図１に示すように、コントローラ３は機能構成としてシミュレーション部５，冗長角度定義テーブル生成部６，冗長角度定義テーブル７，テーブル表示・入力部８，目標位置姿勢生成部１０，領域判定部１１，冗長軸角度設定部１２，補間演算部１３，サーボ制御部１４，アンプ部１５，外力検出部（回避動作実行部）１６から構成されている。
【００２６】
シミュレーション部５は、予め入力されたマニピュレータ２の動作可能領域内の障害物の位置等の情報と目標位置姿勢生成部１０からの制御点の目標位置姿勢と各アクチュエータ４１Ａ〜４７Ａのエンコーダの検出信号から得られる現在のマニピュレータ２の位置姿勢とに基づいて、現在の位置姿勢から目標位置姿勢に至る動作軌跡を生成するように構成されている。なお、この動作軌跡の生成アルゴリズムは、種々のアルゴリズムを適用することができる。
また、シミュレーション部５による動作軌跡の生成は、マニピュレータ２の教示運転時に行えばよい。そして自動運転時には、冗長軸角度定義テーブル７を参照して、冗長角度を決定すればよく、ＣＰＵの負荷を軽減し実用的な時間で、解（各アクチュエータ４１Ａ〜４７Ａに対する目標位置指令）を決定することが可能である。
【００２７】
冗長角度定義テーブル生成部６は、テーブル表示・入力部８からの入力情報に基づいて図４に示すように、マニピュレータ２の動作可能領域内で予め設定された３次元空間の領域１０１について、マニピュレータ２とその周辺の対象物あるいは家畜１の動作等を考慮して予め行なった実験に従って領域１０１を分割して離散化した小領域を設定するようになっている。
【００２８】
領域１０１及び各小領域（ｍ，ｎ，ｌ）は、それぞれ基準座標系Σｃに対してマニピュレータ２の基準座標系Σｗと同時変換行列で関係付けられている。領域１０１は、上述のとおりの実験結果に基づいて予めＺ方向にＬ個、Ｙ方向にＮ個、Ｘ方向にＭ個に分割されている。
より詳しくは、冗長角度定義テーブル生成部６は、家畜１の固体のバラツキ（例えば、ターゲット部１Ａの位置のバラつきや家畜１の脚部などの障害物の位置のバラつき）に合わせて、領域１０１の大きさを決定する。つまり、バラツキが大きい場合は領域１０１の大きさがきく設定され、バラツキが小さい場合は小さく設定される。
また、ここではそれぞれの分割された小領域Ｓｃ（ｍ，ｎ，ｌ）は、直方体となるように設定されている。なお、小領域Ｓｃ（ｍ，ｎ，ｌ）の形状等については演算量を抑制できる点で方形であることが好ましいが、必要に応じて種々の形状に設定することが可能である。
【００２９】
冗長角度定義テーブル７は、分割された領域１０１の各領域に対する冗長軸の位置を設定するためのパラメータが対応付けられている。
かかる領域１０１の情報、即ち、小領域Ｓｃ（ｍ，ｎ，ｌ）の位置・寸法・形状等の情報とこれに対応するパラメータの情報とはテーブル表示・入力部８の処理により表示装置に表示され、これらの情報は入力装置を介して作業者により入力・修正可能となっている。
【００３０】
目標位置姿勢生成部１０は、物体検出センサ４からの入力情報および後述の外力検出部１６からの入力に基づいてマニピュレータ２のエンドエフェクタ２Ａに把持された各種ツールの目標となる位置姿勢を生成するように構成されている。
【００３１】
一方、目標位置姿勢生成部１０は、外力検出部１６が外力を検出したときは、物体検出センサ４の検出結果に基づく演算一旦停止し、外力検出部１６から入力される目標位置姿勢の接触回避用の補正値を最新の目標位置姿勢に反映させたものを目標位置姿勢とする。
【００３２】
領域判定部１１は、目標位置姿勢生成部１０の出力と冗長角度定義テーブル７の出力とから小領域Ｓｃ（ｍ，ｎ，ｌ）の中から該当する冗長角度定義テーブルの小領域Ｓを選択するようになっている。
また、領域判定部１１は、小領域Ｓｃ（ｍ，ｎ，ｌ）の中に目標位置姿勢生成部の出力に該当する小領域が存在しないと判定した場合には異常信号を物体検出センサ４に出力するようになっており、物体検出センサ４側で例えば焦点の調整等の回避作業が実行される。
あるいは、小領域Ｓｃ（ｍ，ｎ，ｌ）の中に目標位置姿勢生成部１０の出力に対応する小領域が存在しないと判定した場合には、コントローラ３の表示装置を介してエラー警告を表示するように構成されてもよい。
冗長軸角度設定部１２は、領域判定部１１により選択された領域に対応する冗長角度定義テーブル７のパラメータに基づいて、冗長軸の角度（目標位置）を設定するようになっている。
【００３３】
補間演算部１３は、冗長軸角度設定部１２により設定された冗長軸４３Ａの角度位置と、シミュレーション部５で求められた動作軌跡とに基づいて、逆キネマティクス演算を行ない現在位置から目標位置姿勢までの各アクチュエータ４１Ａ〜４７Ａの動作態様（あるいは動作速度を含む）を補間演算し、各アクチュエータ４１Ａ〜４７Ａそれぞれの目標位置指令を所定の演算周期毎に出力するようになっている。
【００３４】
サーボ制御部１４は、補間演算部１３から所定の演算周期毎に出力される各アクチュエータ４１Ａ〜４７Ａそれぞれの目標位置指令と、各アクチュエータ４１Ａ〜４７Ａのエンコーダからの入力信号とに基づいて位置速度フィードバック制御を行うようになっている。
【００３５】
アンプ部１５は、サーボ制御部１４から出力される動作指令に基づいて各アクチュエータ４１Ａ〜４７Ａの動作制御を行うようになっている。
外力検出部１６は、各センサＳ１〜Ｓ４からの入力信号に基づいて、マニピュレータ２に対して外力が生じたか否かを検出し、マニピュレータ２が外力に対して回避するための目標位置の接触回避用の補正値を求め、目標位置姿勢生成部１０側に入力するようになっている。
【００３６】
本発明の一実施形態にかかるロボットシステム１００はこのように構成されているので、作業開始時には、物体検出センサ４が家畜１のターゲット部１Ａの位置を計測し、計測結果がコントローラ３に入力され、目標位置生成部１０で制御点の目標位置姿勢が算出される。
なお、物体検出センサ４から得られる計測情報はＸ、Ｙ、Ｚの並進３自由度の位置情報の３個であり、マニピュレータ２の動作自由度７に対して、冗長性が４自由度あることになる。
【００３７】
このため、本実施形態の物体検出センサ４の検出情報に基づいて並進３自由度に加えて回転３自由度の６自由度の目標位置姿勢を生成するために、ターゲット部１Ａは家畜の乳頭部を想定しており、乳頭部は円筒形状であり図５に示すように、Ｚ”軸周りには一様の形状とみなしており、また、ターゲット部１Ａは下向きにあるので、Ｙ”とＸ”周りの回転角度は一定であるとみなすことで、目標位置生成部１０，領域判定部１１，冗長軸角度設定部１２でＲｚ（Ｚ”軸まわりの回転位置）と肘角度Ｅｘを冗長軸角度テーブル７で決定するようになっている。
【００３８】
即ち、物体検出センサ４により求められた物体検出センサ座標系に基づいたターゲット部１Ａの位置と姿勢（Ｘｓ，Ｙｓ，Ｚｓ、）は、領域判定部１１に入力され、図６に例示するような冗長軸角度定義テーブル７を参照して、小領域Ｓｃ（m，n，l）の中から該当する小領域の番号を決定する。
【００３９】
例えば、図６で領域Ｎｏ２が選択されたとすると、ＲｚとＥｘの値は、それぞれ、４７と７５が、冗長軸角度設定部１２で設定される。設定された位置と姿勢角度は、補間演算部１３に入力され、逆運動学に基づいて各アクチュエータ４１Ａ〜４７Ａのそれぞれに対する目標位置指令が生成され、サーボ制御部１４とアンプ部１５に入力され、マニピュレータ２は、ターゲット部１Ａを、ターゲット部１Ａに位置決めする（図５参照）。以上の動作が、物体検出センサ４の計測周波数（例えば、１００Ｈｚ以上）のサンプリング周期毎に繰り返し実行される。
なお、サーボ制御部１４での位置速度フィードバック制御の演算周期は上記のサンプリング周期よりも短い。
【００４０】
このように本実施形態にかかるロボットシステムによれば、物体センサ４から計測周波数毎に入力される検出結果に基づいて演算周期毎に目標位置姿勢を算出し、目標位置姿勢に基づいて冗長角度定義テーブル７との照合結果から冗長軸の角度位置を設定するので、より少ない演算量でマニピュレータの冗長軸を適切に設定することができ、冗長な自由度を有するマニピュレータをより最適に制御することができる。
これにより、家畜１の脚部などの位置が移動する障害物などが存在する狭隘な作業空間内においてマニピュレータ２を動作させる場合であっても、ターゲット部１Ａの位置に応じて冗長軸の姿勢を変更することができ、マニピュレータ２と障害物との接触確率を低減させ、作業効率を高く維持することができる。
【００４１】
また、マニピュレータ２と障害物とが接触した場合であっても外力検出部（回避動作実行部）１６により外力に対して回避する目標位置姿勢が修正されるので、マニピュレータ２が障害物と過度に接触することなく障害物を回避することができる。さらに、接触回避動作時にも修正された目標位置姿勢に基づいて冗長角度定義テーブル７との照合結果から冗長軸の角度位置を設定するので、障害物の回避動作中に新たに別の障害物との接触確率を低減するようにマニピュレータ２の姿勢を適切に制御することができる。
【００４２】
以上、本発明の実施形態について説明したが、本発明は上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲内で、上記実施形態から適宜変更が可能である。また、上記の各実施形態の手法を適宜組み合わせて利用することも可能である。すなわち、このような変更等が施された技術であっても、本発明の技術的範囲に含まれることは言うまでもない
なお、本発明は実施形態のごとく家畜に対して作業を施す用途に限定するものではなく、種々の用途に適用可能である。例えば、マニピュレータの動作範囲内の障害物の位置が変動する場合などにも適用できる。
また、上述の実施形態では７個のアクチュエータを有する７自由度のマニピュレータを例に説明したが、マニピュレータの自由度はこれに限定されることなく、冗長角度定義テーブルのデータを適宜設定することで８自由度以上の自由度を有するものにも本発明を適用することができる。
また、６自由度以下のマニピュレータであっても、作業対象物へのアプローチに１以上の自由度が許容される場合には、相対的にマニピュレータが冗長自由度を有することになるが、そのような場合であっても本発明を適用することができる。
【符号の説明】
【００４３】
１ 家畜
２ マニピュレータ
２Ａ エンドエフェクタ
２Ｂ 物体検出センサ
３ コントローラ
４ 視覚センサ
５ シミュレーション部
６ 冗長角度定義テーブル生成部
７ 冗長角度定義テーブル
８ 冗長角度テーブル表示部
１０ 目標位置姿勢生成部
１１ 領域判定部
１２ 冗長軸角度設定部
１３ 補間演算部
１４ サーボ制御部
１５ アンプ部
４０ 基台
４１〜４６ アーム構造材
４１Ａ〜４７Ａ アクチュエータ
４７ フランジ
【技術分野】
【０００１】
本発明は、マニピュレータを用いたロボットシステム及びロボット制御装置に関する。
【背景技術】
【０００２】
多関節ロボットでは、求められる位置姿勢の自由度よりも多くの自由度（冗長自由度）を有するロボットを制御することが求められる場合がある。
即ち、ロボットが冗長自由度を有することで、対象物に対して求められた位置姿勢を取った際であってもマニピュレータの姿勢を種々変更することができるため、冗長自由度を適切に制御することで比較的狭隘な場所であっても作業時にマニピュレータ自身や周辺の物体との干渉を回避しながら作業を行えるという利点がある。
特許文献１には、冗長自由度を有するマニピュレータを制御するための一手法が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−２０３３８０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、多くのマニピュレータは予め教示された動作経路に沿って動作するが、適用用途によっては、作業対象物が動いている等、位置が不確かな作業対象物に対して作業を行なう場合もある。このような場合には作業対象物の位置を検知するセンサ等の情報に基づいてマニピュレータの目標位置姿勢を修正する。これにより、マニピュレータを作業対象物に追従させながら作業を行なうことができる。
しかしながら、マニピュレータが冗長自由度を有する場合、特許文献１のように、予め冗長自由度を含む各駆動軸全ての動作量を教示により決定しておく手法では、上述のように作業対象物の位置が移動する場合、冗長自由度の位置が必ずしも適切な位置とならず、作業対象物やマニピュレータ自身と干渉（接触）等が生じることが考えられる。
また、教示された動作経路に対して、作業対象物に追従して動作経路が修正されることにより、意図せずにマニピュレータが特異点姿勢となりマニピュレータの制御上、支障が生じることも考えられる。
このような不都合はセンサ等に基づいて目標位置姿勢を修正する場合に限らず、マニピュレータの動作範囲に存在する障害物が移動する場合やマニピュレータと周囲物との意図しない接触が生じた場合にマニピュレータの動作軌道を修正する場合等にも同様の不都合が生じうる。
【０００５】
本発明はこのような課題に鑑みてなされたもので、冗長自由度を有するマニピュレータをより最適に制御することができるようにしたロボットシステム及びロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記課題を解決するため、本発明のロボットシステムは、１以上のアクチュエータを有するマニピュレータと、アクチュエータそれぞれの動作を制御するコントローラと、を有し、コントローラは、アクチュエータの内の一部を冗長軸と設定する冗長軸設定部と、マニピュレータの目標位置姿勢を設定する目標位置姿勢生成部と、目標位置姿勢に基づいてマニピュレータが現在の姿勢から目標位置姿勢に達するまでの動作軌跡を生成するシミュレーション部と、マニピュレータの到達可能範囲内の領域を分割して設定した小領域と各小領域に対応する冗長軸角度用パラメータとを対応付けた冗長角度定義テーブルと、目標位置姿勢生成部の出力結果に基づいて冗長角度定義テーブル中の該当する小領域を選択する領域判定手段と、領域判定手段の選択結果と冗長角度定義テーブルとに基づいて、冗長軸角度を設定する冗長軸角度設定部と動作軌跡と冗長軸角度とに基づいて、アクチュエータのそれぞれに対して動作指令を生成する補間演算部と、を有していることを特徴としている。
【０００７】
また、作業対象物の位置を検知する物体検出センサを有し、目標位置姿勢生成部は、物体検出センサの検知結果に基づいて目標位置姿勢を修正することが好ましい。
【０００８】
また、マニピュレータにかかる外力を検出する外力センサと、を有し、コントローラは、アクチュエータの動作を制御するとともに、センサ部の検出結果に基づいてアクチュエータが外力に対して回避する方向に回避動作を行なわせる回避動作実行部を有していることが好ましい。
また、コントローラは、冗長角度定義テーブルを表示するとともに、冗長角度定義テーブルの内容の修正等を受け付けるテーブル表示・入力部を有していることが好ましい。
また、マニピュレータは、基台と、基台に対して第１構造材を動作させる第１関節と、第1構造材に対して第２構造材を動作させる第２関節と、第２構造材に対して第３構造材を動作させる第３関節と、第３構造材に対して第４構造材を動作させる第４関節と、第４構造材に対して第５構造材を動作させる第５関節と、第５構造材に対して第６構造材を動作させる第６関節と、第６構造材に対してフランジを旋回させる第７関節と、を有していることが好ましい。
【０００９】
また、冗長角度定義テーブルに規定される小領域はそれぞれその形状を直方体とすることが好ましい。
また、冗長角度定義テーブルに規定される小領域は、作業対象物の位置のバラつきを考慮して予め設定されることが好ましい。
【００１０】
領域判定手段は、目標位置姿勢生成部の出力結果から冗長角度定義テーブル中の該当する小領域が存在しないと判定した場合には異常信号を物体検出センサに出力することが好ましい。
また、本発明のロボット制御装置は、１以上のアクチュエータを有するマニピュレータの動作を制御するロボット制御装置であって、アクチュエータの内の一部を冗長軸と設定する冗長軸設定部と、マニピュレータの目標位置姿勢を設定する目標位置姿勢生成部と、目標位置姿勢に基づいてマニピュレータが現在の姿勢から目標位置姿勢に達するまでの動作軌跡を生成するシミュレーション部と、マニピュレータの到達可能範囲内の領域を分割して設定した小領域と各小領域に対応する冗長軸角度用パラメータとを対応付けた冗長角度定義テーブルと、目標位置姿勢生成部の出力結果に基づいて冗長角度定義テーブル中の該当する小領域を選択する領域判定手段と、領域判定手段の選択結果と冗長角度定義テーブルとに基づいて、冗長軸角度を設定する冗長軸角度設定部と動作軌跡と冗長軸角度とに基づいて、アクチュエータのそれぞれに対して動作指令を生成する補間演算部と、を有していることを特徴としている。
【発明の効果】
【００１１】
本発明によれば、目標となる位置姿勢が変更される場合でもより少ない演算量でマニピュレータの冗長軸を適切に設定することができ、冗長な自由度を有するマニピュレータをより最適に制御することができる。
【図面の簡単な説明】
【００１２】
【図１】本発明の実施形態に係るロボットシステムの全体構成を説明するための模式的な構成図
【図２】本発明の実施形態にかかるロボットの構成を示す側面図である。
【図３】センサ部の構成を示す模式的な構造図
【図４】冗長角度定義テーブルに規定される３次元空間の領域を説明するための模式図
【図５】エンドエフェクタとターゲット部（作業対象物）座標関係を模式的に示す図
【図６】冗長角度定義テーブルの一例を示す図
【発明を実施するための形態】
【００１３】
以下、本発明の実施の形態について図を参照して説明する。
本実施形態では、マニピュレータ２を用いて家畜に対して検査、消毒、搾乳などの作業を行なうロボットシステムを例に本発明の一実施形態について説明する。
【００１４】
図１に示すように、本実施形態にかかるロボットシステム１００は、家畜１が収納されたフロアＦ，マニピュレータ２，コントローラ３，センサ部４からなっている。
マニピュレータ２は、図２に示すように、設置面（壁面や床等）１０１に固定された基台４０と、基台４０からマニピュレータ２の先端にかけて順々に第１構造材４１，第２構造材４２，第３構造材４３，第４構造材４４，第５構造材４５，第６構造材４６，フランジ４７がそれぞれ回転駆動するアクチュエータ（回転関節）を介して連結されている。
【００１５】
基台４０と第１構造材４１とは、第１アクチュエータ（第１関節）４１Ａを介して連結されており、第１アクチュエータ４１Ａの駆動により、第１構造材４１が回転するようになっている。第１構造材４１と第４構造材４２とは、第２アクチュエータ（第２関節）４２Ａを介して連結されており、第２アクチュエータ４２Ａの駆動により、第２構造材４２が旋回するようになっている。
【００１６】
第２構造材４２と第３構造材４３とは、第３アクチュエータ（第３関節）４３Ａを介して連結されており、第３アクチュエータ４３Ａの駆動により、第３構造材４３が回転するようになっている。
第３構造材４３と第４構造材４４とは、第４アクチュエータ（第４関節）４４Ａを介して連結されており、第４アクチュエータ４４Ａの駆動により、第４構造材４４が旋回するようになっている。
【００１７】
第４構造材４４と第５構造材４５とは、第５アクチュエータ（第５関節）４５Ａを介して連結されており、第５アクチュエータ４５Ａの駆動により、第５構造材４５が回転するようになっている。第５構造材４５と第６構造材４６とは、第６アクチュエータ（第６関節）４６Ａを介して連結されており、第６アクチュエータ４６Ａの駆動により、第６構造材４６が旋回するようになっている。
第６構造材４６とフランジ４７とは、第７アクチュエータ（第７関節）４７Ａを介して連結されており、第７アクチュエータ４７Ａの駆動により、フランジ４７及びフランジ４７に取り付けられるハンド等のエンドエフェクタ２Ａが回転するようになっている。
【００１８】
なお、本実施形態では、第３アクチュエータ（第３関節）４３Ａが冗長軸として予め設定されており（冗長軸設定部）、後述するコントローラ３の機能により第３アクチュエータ４３Ａの回転位置が最適に制御されることとなる。また、アクチュエータ４３Ａを冗長軸、各アクチュエータ４１Ａ，４２Ａをそれぞれ第１軸，第２軸とし、各アクチュエータ４４Ａ〜４７Ａをそれぞれ第３軸〜第６軸という。第１軸〜第６軸を特定しない場合は単に軸ともいう。
【００１９】
エンドエフェクタ２Ａには、検査器，搾乳器，消毒器等の種々のツール（図示は省略）が取り付けられ、家畜１のターゲット部１Ａに対して、検査、消毒、搾乳などの作業を行なうようになっている。なお、マニピュレータ２の動作位置を制御する位置である制御点は本実施形態のマニピュレータ２の場合、制御点は各アクチュエータ４４Ａ〜４７Ａの駆動により直線移動させることが可能な点としており、第６アクチュエータ４６Ａの軸心付近に位置している。
そして、制御点とフランジ４７Ａとエンドエフェクタ２Ａ及び各ツールとの位置関係は予めコントローラ３に入力されており、制御点の位置姿勢を制御することで各ツールの位置を制御できるようになっている。
また、エンドエフェクタ（換言するとマニピュレータ２の先端部分）２Ａには、物体検知センサ（本実施形態では、カメラであるがその他の種々のセンサを適用可能である）２Ｂが取り付けられている。
【００２０】
物体検知センサ２Ｂは、四方が仕切られたフロアＦ内で不規則に動く家畜１のターゲット部１Ａを含む十分な広さの検知領域に対して配向されている。
物体検知センサ２Ｂにより取得した画像はコントローラ３の目標認識部２Ｃに入力され、画像認識部２Ｃでは家畜１のターゲット部１Ａの位置を検出し、検出結果を後述する動作指令部５に目標位置としてリアルタイムに入力するようになっている。
なお、本実施形態では、説明を容易とするため、コントローラ３は一体の制御装置として示しているが実際にはコントローラ３は複数のコンピュータ等で構成し、目標認識部２Ｃや外力検出部（後述）については、マニピュレータ２の動作を制御するロボットコントローラとは別体の制御装置により構成する等してもよい。
【００２１】
また、第１〜第７アクチュエータ４１Ａ〜４７Ａは、それぞれ、ケーブル（図示省略）を挿通可能な中空部を有する減速機一体型のサーボモータによって構成されており、各アクチュエータの回転位置は、アクチュエータに内蔵のエンコーダからの信号としてコントローラ３にケーブルを介して入力されるようになっている。
【００２２】
センサ部４は図３に示すように、センサ固定治具２５と４個のセンサＳ１〜Ｓ４により構成されており、円盤状のセンサ固定治具２５はマニピュレータ２の第１アクチュエータ４１Ａの固定子の基部に取り付けられている。
【００２３】
各センサＳ１〜Ｓ４は、円盤状のセンサ固定治具２５に埋設されており、各センサＳ１〜Ｓ４は同一円弧（仮想円）に沿って等間隔に配置されている。そして、各センサＳ１〜Ｓ４には圧電体として水晶が用いられており、各センサＳ１〜Ｓ４はそれぞれセンサ固定治具１５のラジアル方向の歪量を電圧として検出し、得られた電圧はアンプ部（図示省略）を介して増幅され、コントローラ３に入力されるようになっている。
各センサＳ１〜Ｓ４に水晶振動子（水晶圧電素子）を用いることで歪みゲージや他の圧電素子を用いた場合よりも応答時間が小さく後述するサーボ部１４の演算周期と比較して良好な応答性を得ることができ、マニピュレータ周辺の対象物の接触等による衝撃を十分に緩和することができる。
【００２４】
なお、本実施形態では上述のように応答時間の早さ等の理由から水晶振動子を用いているが、良好な応答性が得られるものであれば各センサＳ１〜Ｓ４にどのような形式のものでも適用可能である。また、水晶圧電センサは、歪みゲージや通常の衝突センサと比較して耐久性に優れ、マニピュレータ２の自重による負荷が最も大きい基端部にセンサ部４設けた場合でも十分に高精度な検出ができるという利点もある。
マニピュレータ２の各アクチュエータ４１Ａ〜４７Ａを含む各駆動部位の動作はコントローラ３で制御される。
【００２５】
コントローラ３は、記憶装置，電子演算器，入力装置及び表示装置（いずれも詳細には図示省略）を有するコンピュータにより構成されており、マニピュレータ２の各駆動部位と相互通信可能に接続されている。
図１に示すように、コントローラ３は機能構成としてシミュレーション部５，冗長角度定義テーブル生成部６，冗長角度定義テーブル７，テーブル表示・入力部８，目標位置姿勢生成部１０，領域判定部１１，冗長軸角度設定部１２，補間演算部１３，サーボ制御部１４，アンプ部１５，外力検出部（回避動作実行部）１６から構成されている。
【００２６】
シミュレーション部５は、予め入力されたマニピュレータ２の動作可能領域内の障害物の位置等の情報と目標位置姿勢生成部１０からの制御点の目標位置姿勢と各アクチュエータ４１Ａ〜４７Ａのエンコーダの検出信号から得られる現在のマニピュレータ２の位置姿勢とに基づいて、現在の位置姿勢から目標位置姿勢に至る動作軌跡を生成するように構成されている。なお、この動作軌跡の生成アルゴリズムは、種々のアルゴリズムを適用することができる。
また、シミュレーション部５による動作軌跡の生成は、マニピュレータ２の教示運転時に行えばよい。そして自動運転時には、冗長軸角度定義テーブル７を参照して、冗長角度を決定すればよく、ＣＰＵの負荷を軽減し実用的な時間で、解（各アクチュエータ４１Ａ〜４７Ａに対する目標位置指令）を決定することが可能である。
【００２７】
冗長角度定義テーブル生成部６は、テーブル表示・入力部８からの入力情報に基づいて図４に示すように、マニピュレータ２の動作可能領域内で予め設定された３次元空間の領域１０１について、マニピュレータ２とその周辺の対象物あるいは家畜１の動作等を考慮して予め行なった実験に従って領域１０１を分割して離散化した小領域を設定するようになっている。
【００２８】
領域１０１及び各小領域（ｍ，ｎ，ｌ）は、それぞれ基準座標系Σｃに対してマニピュレータ２の基準座標系Σｗと同時変換行列で関係付けられている。領域１０１は、上述のとおりの実験結果に基づいて予めＺ方向にＬ個、Ｙ方向にＮ個、Ｘ方向にＭ個に分割されている。
より詳しくは、冗長角度定義テーブル生成部６は、家畜１の固体のバラツキ（例えば、ターゲット部１Ａの位置のバラつきや家畜１の脚部などの障害物の位置のバラつき）に合わせて、領域１０１の大きさを決定する。つまり、バラツキが大きい場合は領域１０１の大きさがきく設定され、バラツキが小さい場合は小さく設定される。
また、ここではそれぞれの分割された小領域Ｓｃ（ｍ，ｎ，ｌ）は、直方体となるように設定されている。なお、小領域Ｓｃ（ｍ，ｎ，ｌ）の形状等については演算量を抑制できる点で方形であることが好ましいが、必要に応じて種々の形状に設定することが可能である。
【００２９】
冗長角度定義テーブル７は、分割された領域１０１の各領域に対する冗長軸の位置を設定するためのパラメータが対応付けられている。
かかる領域１０１の情報、即ち、小領域Ｓｃ（ｍ，ｎ，ｌ）の位置・寸法・形状等の情報とこれに対応するパラメータの情報とはテーブル表示・入力部８の処理により表示装置に表示され、これらの情報は入力装置を介して作業者により入力・修正可能となっている。
【００３０】
目標位置姿勢生成部１０は、物体検出センサ４からの入力情報および後述の外力検出部１６からの入力に基づいてマニピュレータ２のエンドエフェクタ２Ａに把持された各種ツールの目標となる位置姿勢を生成するように構成されている。
【００３１】
一方、目標位置姿勢生成部１０は、外力検出部１６が外力を検出したときは、物体検出センサ４の検出結果に基づく演算一旦停止し、外力検出部１６から入力される目標位置姿勢の接触回避用の補正値を最新の目標位置姿勢に反映させたものを目標位置姿勢とする。
【００３２】
領域判定部１１は、目標位置姿勢生成部１０の出力と冗長角度定義テーブル７の出力とから小領域Ｓｃ（ｍ，ｎ，ｌ）の中から該当する冗長角度定義テーブルの小領域Ｓを選択するようになっている。
また、領域判定部１１は、小領域Ｓｃ（ｍ，ｎ，ｌ）の中に目標位置姿勢生成部の出力に該当する小領域が存在しないと判定した場合には異常信号を物体検出センサ４に出力するようになっており、物体検出センサ４側で例えば焦点の調整等の回避作業が実行される。
あるいは、小領域Ｓｃ（ｍ，ｎ，ｌ）の中に目標位置姿勢生成部１０の出力に対応する小領域が存在しないと判定した場合には、コントローラ３の表示装置を介してエラー警告を表示するように構成されてもよい。
冗長軸角度設定部１２は、領域判定部１１により選択された領域に対応する冗長角度定義テーブル７のパラメータに基づいて、冗長軸の角度（目標位置）を設定するようになっている。
【００３３】
補間演算部１３は、冗長軸角度設定部１２により設定された冗長軸４３Ａの角度位置と、シミュレーション部５で求められた動作軌跡とに基づいて、逆キネマティクス演算を行ない現在位置から目標位置姿勢までの各アクチュエータ４１Ａ〜４７Ａの動作態様（あるいは動作速度を含む）を補間演算し、各アクチュエータ４１Ａ〜４７Ａそれぞれの目標位置指令を所定の演算周期毎に出力するようになっている。
【００３４】
サーボ制御部１４は、補間演算部１３から所定の演算周期毎に出力される各アクチュエータ４１Ａ〜４７Ａそれぞれの目標位置指令と、各アクチュエータ４１Ａ〜４７Ａのエンコーダからの入力信号とに基づいて位置速度フィードバック制御を行うようになっている。
【００３５】
アンプ部１５は、サーボ制御部１４から出力される動作指令に基づいて各アクチュエータ４１Ａ〜４７Ａの動作制御を行うようになっている。
外力検出部１６は、各センサＳ１〜Ｓ４からの入力信号に基づいて、マニピュレータ２に対して外力が生じたか否かを検出し、マニピュレータ２が外力に対して回避するための目標位置の接触回避用の補正値を求め、目標位置姿勢生成部１０側に入力するようになっている。
【００３６】
本発明の一実施形態にかかるロボットシステム１００はこのように構成されているので、作業開始時には、物体検出センサ４が家畜１のターゲット部１Ａの位置を計測し、計測結果がコントローラ３に入力され、目標位置生成部１０で制御点の目標位置姿勢が算出される。
なお、物体検出センサ４から得られる計測情報はＸ、Ｙ、Ｚの並進３自由度の位置情報の３個であり、マニピュレータ２の動作自由度７に対して、冗長性が４自由度あることになる。
【００３７】
このため、本実施形態の物体検出センサ４の検出情報に基づいて並進３自由度に加えて回転３自由度の６自由度の目標位置姿勢を生成するために、ターゲット部１Ａは家畜の乳頭部を想定しており、乳頭部は円筒形状であり図５に示すように、Ｚ”軸周りには一様の形状とみなしており、また、ターゲット部１Ａは下向きにあるので、Ｙ”とＸ”周りの回転角度は一定であるとみなすことで、目標位置生成部１０，領域判定部１１，冗長軸角度設定部１２でＲｚ（Ｚ”軸まわりの回転位置）と肘角度Ｅｘを冗長軸角度テーブル７で決定するようになっている。
【００３８】
即ち、物体検出センサ４により求められた物体検出センサ座標系に基づいたターゲット部１Ａの位置と姿勢（Ｘｓ，Ｙｓ，Ｚｓ、）は、領域判定部１１に入力され、図６に例示するような冗長軸角度定義テーブル７を参照して、小領域Ｓｃ（m，n，l）の中から該当する小領域の番号を決定する。
【００３９】
例えば、図６で領域Ｎｏ２が選択されたとすると、ＲｚとＥｘの値は、それぞれ、４７と７５が、冗長軸角度設定部１２で設定される。設定された位置と姿勢角度は、補間演算部１３に入力され、逆運動学に基づいて各アクチュエータ４１Ａ〜４７Ａのそれぞれに対する目標位置指令が生成され、サーボ制御部１４とアンプ部１５に入力され、マニピュレータ２は、ターゲット部１Ａを、ターゲット部１Ａに位置決めする（図５参照）。以上の動作が、物体検出センサ４の計測周波数（例えば、１００Ｈｚ以上）のサンプリング周期毎に繰り返し実行される。
なお、サーボ制御部１４での位置速度フィードバック制御の演算周期は上記のサンプリング周期よりも短い。
【００４０】
このように本実施形態にかかるロボットシステムによれば、物体センサ４から計測周波数毎に入力される検出結果に基づいて演算周期毎に目標位置姿勢を算出し、目標位置姿勢に基づいて冗長角度定義テーブル７との照合結果から冗長軸の角度位置を設定するので、より少ない演算量でマニピュレータの冗長軸を適切に設定することができ、冗長な自由度を有するマニピュレータをより最適に制御することができる。
これにより、家畜１の脚部などの位置が移動する障害物などが存在する狭隘な作業空間内においてマニピュレータ２を動作させる場合であっても、ターゲット部１Ａの位置に応じて冗長軸の姿勢を変更することができ、マニピュレータ２と障害物との接触確率を低減させ、作業効率を高く維持することができる。
【００４１】
また、マニピュレータ２と障害物とが接触した場合であっても外力検出部（回避動作実行部）１６により外力に対して回避する目標位置姿勢が修正されるので、マニピュレータ２が障害物と過度に接触することなく障害物を回避することができる。さらに、接触回避動作時にも修正された目標位置姿勢に基づいて冗長角度定義テーブル７との照合結果から冗長軸の角度位置を設定するので、障害物の回避動作中に新たに別の障害物との接触確率を低減するようにマニピュレータ２の姿勢を適切に制御することができる。
【００４２】
以上、本発明の実施形態について説明したが、本発明は上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲内で、上記実施形態から適宜変更が可能である。また、上記の各実施形態の手法を適宜組み合わせて利用することも可能である。すなわち、このような変更等が施された技術であっても、本発明の技術的範囲に含まれることは言うまでもない
なお、本発明は実施形態のごとく家畜に対して作業を施す用途に限定するものではなく、種々の用途に適用可能である。例えば、マニピュレータの動作範囲内の障害物の位置が変動する場合などにも適用できる。
また、上述の実施形態では７個のアクチュエータを有する７自由度のマニピュレータを例に説明したが、マニピュレータの自由度はこれに限定されることなく、冗長角度定義テーブルのデータを適宜設定することで８自由度以上の自由度を有するものにも本発明を適用することができる。
また、６自由度以下のマニピュレータであっても、作業対象物へのアプローチに１以上の自由度が許容される場合には、相対的にマニピュレータが冗長自由度を有することになるが、そのような場合であっても本発明を適用することができる。
【符号の説明】
【００４３】
１ 家畜
２ マニピュレータ
２Ａ エンドエフェクタ
２Ｂ 物体検出センサ
３ コントローラ
４ 視覚センサ
５ シミュレーション部
６ 冗長角度定義テーブル生成部
７ 冗長角度定義テーブル
８ 冗長角度テーブル表示部
１０ 目標位置姿勢生成部
１１ 領域判定部
１２ 冗長軸角度設定部
１３ 補間演算部
１４ サーボ制御部
１５ アンプ部
４０ 基台
４１〜４６ アーム構造材
４１Ａ〜４７Ａ アクチュエータ
４７ フランジ
【特許請求の範囲】
【請求項１】
１以上のアクチュエータを有するマニピュレータと、
前記アクチュエータそれぞれの動作を制御するコントローラと、を有し、
前記コントローラは、
前記アクチュエータの内の一部を冗長軸と設定する冗長軸設定部と、
前記マニピュレータの目標位置姿勢を設定する目標位置姿勢生成部と、
前記目標位置姿勢に基づいて前記マニピュレータが現在の姿勢から前記目標位置姿勢に達するまでの動作軌跡を生成するシミュレーション部と、
前記マニピュレータの到達可能範囲内の領域を分割して設定した小領域と各小領域に対応する冗長軸角度用パラメータとを対応付けた冗長角度定義テーブルと、
前記目標位置姿勢生成部の出力結果に基づいて前記冗長角度定義テーブル中の該当する前記小領域を選択する領域判定手段と、
前記領域判定手段の選択結果と前記冗長角度定義テーブルとに基づいて、前記冗長軸角度を設定する冗長軸角度設定部と
前記動作軌跡と前記冗長軸角度とに基づいて、前記アクチュエータのそれぞれに対して動作指令を生成する補間演算部と、を有している
ことを特徴とする、ロボットシステム。
【請求項２】
作業対象物の位置を検知する物体検出センサを有し、
前記目標位置姿勢生成部は、前記物体検出センサの検知結果に基づいて前記目標位置姿勢を修正する
ことを特徴とする、請求項１記載のロボットシステム。
【請求項３】
前記マニピュレータにかかる外力を検出する外力センサを有し、
前記コントローラは、
前記アクチュエータの動作を制御するとともに、前記センサ部の検出結果に基づいて前記アクチュエータが前記外力に対して回避する方向に回避動作を行なわせる回避動作実行部を有している
ことを特徴とする、請求項１または２記載のロボットシステム。
【請求項４】
前記コントローラは、
前記冗長角度定義テーブルを表示するとともに、前記冗長角度定義テーブルの内容の修正等を受け付けるテーブル表示・入力部を有している
ことを特徴とする、請求項１〜３のいずれか１項に記載のロボットシステム。
【請求項５】
前記マニピュレータは、
基台と、
前記基台に対して第１構造材を動作させる第１関節と、
前記第1構造材に対して第２構造材を動作させる第２関節と、
前記第２構造材に対して第３構造材を動作させる第３関節と、
前記第３構造材に対して第４構造材を動作させる第４関節と、
前記第４構造材に対して第５構造材を動作させる第５関節と、
前記第５構造材に対して第６構造材を動作させる第６関節と、
前記第６構造材に対してフランジを旋回させる第７関節と、を有している
ことを特徴とする、請求項１〜４のいずれか１項に記載のロボットシステム。
【請求項６】
前記冗長角度定義テーブルに規定される前記小領域はそれぞれその形状を直方体とすることを特徴とする請求項１〜５のいずれか１項に記載のロボットシステム。
【請求項７】
前記冗長角度定義テーブルに規定される前記小領域は、前記作業対象物の位置のバラつきを考慮して予め設定される
ことを特徴とする請求項２〜６のいずれか１項に記載のロボットシステム。
【請求項８】
前記領域判定手段は、前記目標位置姿勢生成部の出力結果から前記冗長角度定義テーブル中の該当する前記小領域が存在しないと判定した場合には異常信号を前記物体検出センサに出力する
ることを特徴とする請求項２〜７のいずれか１項に記載のロボットシステム。
【請求項９】
１以上のアクチュエータを有するマニピュレータの動作を制御するロボット制御装置であって、
前記アクチュエータの内の一部を冗長軸と設定する冗長軸設定部と、
前記マニピュレータの目標位置姿勢を設定する目標位置姿勢生成部と、
前記目標位置姿勢に基づいて前記マニピュレータが現在の姿勢から前記目標位置姿勢に達するまでの動作軌跡を生成するシミュレーション部と、
前記マニピュレータの到達可能範囲内の領域を分割して設定した小領域と各小領域に対応する冗長軸角度用パラメータとを対応付けた冗長角度定義テーブルと、
前記目標位置姿勢生成部の出力結果に基づいて前記冗長角度定義テーブル中の該当する前記小領域を選択する領域判定手段と、
前記領域判定手段の選択結果と前記冗長角度定義テーブルとに基づいて、前記冗長軸角度を設定する冗長軸角度設定部と
前記動作軌跡と前記冗長軸角度とに基づいて、前記アクチュエータのそれぞれに対して動作指令を生成する補間演算部と、を有している
ことを特徴とする、ロボット制御装置。
【請求項１】
１以上のアクチュエータを有するマニピュレータと、
前記アクチュエータそれぞれの動作を制御するコントローラと、を有し、
前記コントローラは、
前記アクチュエータの内の一部を冗長軸と設定する冗長軸設定部と、
前記マニピュレータの目標位置姿勢を設定する目標位置姿勢生成部と、
前記目標位置姿勢に基づいて前記マニピュレータが現在の姿勢から前記目標位置姿勢に達するまでの動作軌跡を生成するシミュレーション部と、
前記マニピュレータの到達可能範囲内の領域を分割して設定した小領域と各小領域に対応する冗長軸角度用パラメータとを対応付けた冗長角度定義テーブルと、
前記目標位置姿勢生成部の出力結果に基づいて前記冗長角度定義テーブル中の該当する前記小領域を選択する領域判定手段と、
前記領域判定手段の選択結果と前記冗長角度定義テーブルとに基づいて、前記冗長軸角度を設定する冗長軸角度設定部と
前記動作軌跡と前記冗長軸角度とに基づいて、前記アクチュエータのそれぞれに対して動作指令を生成する補間演算部と、を有している
ことを特徴とする、ロボットシステム。
【請求項２】
作業対象物の位置を検知する物体検出センサを有し、
前記目標位置姿勢生成部は、前記物体検出センサの検知結果に基づいて前記目標位置姿勢を修正する
ことを特徴とする、請求項１記載のロボットシステム。
【請求項３】
前記マニピュレータにかかる外力を検出する外力センサを有し、
前記コントローラは、
前記アクチュエータの動作を制御するとともに、前記センサ部の検出結果に基づいて前記アクチュエータが前記外力に対して回避する方向に回避動作を行なわせる回避動作実行部を有している
ことを特徴とする、請求項１または２記載のロボットシステム。
【請求項４】
前記コントローラは、
前記冗長角度定義テーブルを表示するとともに、前記冗長角度定義テーブルの内容の修正等を受け付けるテーブル表示・入力部を有している
ことを特徴とする、請求項１〜３のいずれか１項に記載のロボットシステム。
【請求項５】
前記マニピュレータは、
基台と、
前記基台に対して第１構造材を動作させる第１関節と、
前記第1構造材に対して第２構造材を動作させる第２関節と、
前記第２構造材に対して第３構造材を動作させる第３関節と、
前記第３構造材に対して第４構造材を動作させる第４関節と、
前記第４構造材に対して第５構造材を動作させる第５関節と、
前記第５構造材に対して第６構造材を動作させる第６関節と、
前記第６構造材に対してフランジを旋回させる第７関節と、を有している
ことを特徴とする、請求項１〜４のいずれか１項に記載のロボットシステム。
【請求項６】
前記冗長角度定義テーブルに規定される前記小領域はそれぞれその形状を直方体とすることを特徴とする請求項１〜５のいずれか１項に記載のロボットシステム。
【請求項７】
前記冗長角度定義テーブルに規定される前記小領域は、前記作業対象物の位置のバラつきを考慮して予め設定される
ことを特徴とする請求項２〜６のいずれか１項に記載のロボットシステム。
【請求項８】
前記領域判定手段は、前記目標位置姿勢生成部の出力結果から前記冗長角度定義テーブル中の該当する前記小領域が存在しないと判定した場合には異常信号を前記物体検出センサに出力する
ることを特徴とする請求項２〜７のいずれか１項に記載のロボットシステム。
【請求項９】
１以上のアクチュエータを有するマニピュレータの動作を制御するロボット制御装置であって、
前記アクチュエータの内の一部を冗長軸と設定する冗長軸設定部と、
前記マニピュレータの目標位置姿勢を設定する目標位置姿勢生成部と、
前記目標位置姿勢に基づいて前記マニピュレータが現在の姿勢から前記目標位置姿勢に達するまでの動作軌跡を生成するシミュレーション部と、
前記マニピュレータの到達可能範囲内の領域を分割して設定した小領域と各小領域に対応する冗長軸角度用パラメータとを対応付けた冗長角度定義テーブルと、
前記目標位置姿勢生成部の出力結果に基づいて前記冗長角度定義テーブル中の該当する前記小領域を選択する領域判定手段と、
前記領域判定手段の選択結果と前記冗長角度定義テーブルとに基づいて、前記冗長軸角度を設定する冗長軸角度設定部と
前記動作軌跡と前記冗長軸角度とに基づいて、前記アクチュエータのそれぞれに対して動作指令を生成する補間演算部と、を有している
ことを特徴とする、ロボット制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１２−５１０４３（Ｐ２０１２−５１０４３Ａ）
【公開日】平成２４年３月１５日（２０１２．３．１５）
【国際特許分類】
【出願番号】特願２０１０−１９３７１８（Ｐ２０１０−１９３７１８）
【出願日】平成２２年８月３１日（２０１０．８．３１）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
【公開日】平成２４年３月１５日（２０１２．３．１５）
【国際特許分類】
【出願日】平成２２年８月３１日（２０１０．８．３１）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
[ Back to top ]