ロボットシステム及びロボット制御装置
【課題】ロボットアームと周囲の対象物との接触等で生じる外力を低減することができるようにしたロボットシステム及びロボット制御装置を提供する。
【解決手段】ロボットアーム２と、ロボットアーム２に設けられロボットアーム２を駆動させる１以上のアクチュエータ４１Ａ〜４７Ａと、ロボットアーム２とアクチュエータ４１Ａ〜４７Ａとの少なくともいずれかにかかる外力を検出するセンサ部４と、アクチュエータ４１Ａ〜４７Ａの動作を制御するとともに、センサ部４の検出結果に基づいてアクチュエータ４１Ａ〜４７Ａへのトルク指令値を制限するコントローラ３と、を有して構成する。
【解決手段】ロボットアーム２と、ロボットアーム２に設けられロボットアーム２を駆動させる１以上のアクチュエータ４１Ａ〜４７Ａと、ロボットアーム２とアクチュエータ４１Ａ〜４７Ａとの少なくともいずれかにかかる外力を検出するセンサ部４と、アクチュエータ４１Ａ〜４７Ａの動作を制御するとともに、センサ部４の検出結果に基づいてアクチュエータ４１Ａ〜４７Ａへのトルク指令値を制限するコントローラ３と、を有して構成する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットアームを有するロボットシステム及びロボット制御装置に関する。
【背景技術】
【０００２】
ロボットの分野では、ロボット自身あるいはロボットの周囲に存在する物体に対して過剰な負荷（外力）が生じることを回避することが求められている。
例えば、特許文献１にはロボットアームの基端に接触力（外力）を検知する力検出器を取り付け、力検出器の検出結果に基づいてロボットアームの動作を停止させたり、過度な外力がかかった場合に外力が低減する方向にロボットアームを動作させる等の技術が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００６−２１２８７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところが、引用文献１のように、外力を検出してからロボットアームの動作を停止すると、接触した対象が動いている場合にはロボットアームが停止していても大きな衝撃力が生じてしまう。また、外力を低減する方向にロボットを動作させる場合であっても、外力を検出してからロボットアームに新たな動作指令を生成して動作させるため、接触（衝突）が生じてからロボットアームが動作するまでの応答時間が長くなり、衝突時の衝撃を緩和することが困難であるという課題がある。
本発明はこのような課題に鑑みてなされたもので、ロボットアームと周囲の対象物との接触等で生じる外力を低減することができるようにしたロボットシステム及びロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記課題を解決するため、本発明のロボットシステムは、ロボットアームと、ロボットアームに設けられロボットアームを駆動させる１以上のアクチュエータと、ロボットアームとアクチュエータとの少なくともいずれかにかかる外力を検出するセンサ部と、アクチュエータの動作を制御するとともに、センサ部の検出結果に基づいてアクチュエータへのトルク指令値を制限するコントローラと、を有していることを特徴としている。
【０００６】
また、ロボットアーム又はアクチュエータの内、コントローラにより位置及び姿勢が制御される点である制御点の位置及び姿勢を検知する位置センサを有し、コントローラは、制御点のとるべき位置及び姿勢に対応する各アクチュエータの位置を目標位置として設定し、アクチュエータそれぞれの動作指令を設定する動作指令設定部と、少なくとも動作指令と位置センサの検出結果とに基づいて、所定の演算周期毎に、アクチュエータのそれぞれに対してトルク指令を生成するサーボ部とセンサ部の検出結果に基づいて外力が生じたときに外力の方向を検出する外力検出部と、外力検出部により検出された外力の方向に基づいて、１以上のアクチュエータのうち回避動作を行なうべきアクチュエータを選択する回避軸選択部と、サーボ部により生成されるトルク指令に自重分の重力補償トルクを付加する重力補償トルク付加部と、回避軸選択部により選択されたアクチュエータのそれぞれに対してサーボ部により生成されるトルク指令に、アクチュエータ自身の摩擦力に抗して外力の方向に動作するための摩擦補償トルクを付加する摩擦補償トルク付加部と、を有していることが好ましい。
【０００７】
また、コントローラは、外力検出部により外力が検出されると、サーボ部に与える動作指令を遮断する動作指令遮断部を有していることが好ましい。
また、コントローラは、動作指令遮断部により動作指令が遮断されてから予め設定された期間以上外力検出部により外力が検出されないときは、動作指令遮断部による動作指令の遮断を解除する動作指令復帰部を有していることが好ましい。
また、コントローラは、アクチュエータへのトルク指令値を制限するトルク指令制限部を有し、トルク指令制限部は、重力補償トルクと摩擦補償トルクとの和に基づいてトルク指令値の上限値及び下限値を設定することが好ましい。
【０００８】
また、センサ部は、圧電体として水晶を用いた水晶振動子を複数含んで形成されていることが好ましい。
また、センサ部は、ロボットアームの内最も基端側のアクチュエータの基部に設けられた円板状のセンサ固定治具と、センサ固定冶具に同一円弧に沿って埋設された複数のセンサとを有していることが好ましい。
【０００９】
外力検出部は、複数のセンサからの信号の高周波振動成分を抽出し、抽出された高周波振動成分に基づいて、外力の有無を検出することが好ましい。
外力検出部は、抽出された高周波振動成分のピーク値を各センサ毎に求め、各センサ毎のピーク値の正負組み合わせに基づいて外力の方向を検出することが好ましい。
回避軸選択部は、外力検出部により求められた外力の方向ベクトルにヤコビ行列の転置行列を掛け算したベクトルの各成分を閾値処理して回避対象関節軸と方向を算出することが好ましい。
【００１０】
摩擦補償トルク付加部は、回避軸選択部により選択されたアクチュエータが静止しているときは当該アクチュエータの最大静止摩擦トルクを摩擦補償トルクとして付加し、当該アクチュエータが回転しているときは当該アクチュエータの動摩擦トルクを摩擦補償トルクとして付加することが好ましい。
外力検出部は、抽出された各センサ毎の高周波振動成分をそれぞれ移動平均化処理して得た移動平均値に基づいて外力の有無を検出することが好ましい。
外力検出部は、外力が有ることを検出した時点以降における各センサ毎の高周波振動成分の積算値をそれぞれ求め、積算値の正負組み合わせに基づいて外力の方向を検出することが好ましい。
【００１１】
コントローラは、各センサ毎の積算値の正負の組み合わせ、または、各センサ毎の移動平均値の正負の組み合わせ、と外力の方向とを対応付けたデータテーブルを有し、外力検出部は、外力が有ることを検出した時点以降の複数の時点において、それぞれ、各センサ毎の積算値の正負の組み合わせ、または、各センサ毎の移動平均値の正負の組み合わせに基づいて外力の方向を検出することが好ましい。
【００１２】
また、本発明のロボット制御装置は、１以上のアクチュエータを有するロボットアームと、ロボットアームとアクチュエータとの少なくともいずれかにかかる外力を検出するセンサ部とを有するロボットの制御装置であって、アクチュエータに対してトルク指令を送出するアクチュエータ制御部と、センサ部からの入力結果に基づいてアクチュエータへ送出されるトルク指令値を制限するトルク指令制限部と、を有していることを特徴としている。
【発明の効果】
【００１３】
本発明によれば、ロボットアームに内蔵されたセンサに、例えば、アクチュエータに含まれる減速機のリップル等に起因する外乱が作用しても、外力の有無を誤検出することなくより正確に外力の有無と方向を検出することができる。そして、外力が検出されるとアクチュエータに対するトルク指令値を制限するので、外力が各アクチュエータへのトルク指令値を超えるとアクチュエータが外力に倣って変位するため急激な外力の負荷による衝撃を緩和することができる。
また、ロボットアームの周囲の対象物がロボットアームに向かって運動を継続する場合等、継続してロボットアームに外力がかかる場合であってもアクチュエータが外力に倣って変位することでロボットアームの姿勢を変えることで、ロボットアームの周囲の対象物を回避することができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施形態に係るロボットシステムの全体構成を説明するための模式的な構成図
【図２】本発明の実施形態にかかるロボットの構成を示す側面図である。
【図３】センサ部の構成を示す模式的な構造図
【図４】接触検出部の機能構成を説明するためのブロック図
【図５】外力方向検出部の機能構成を説明するためのブロック図
【図６】テーブルデータを示す模式図
【図７】センサの配置と外力方向ベクトルの関係を表す図
【図８】回避軸選択部の機能構成を説明するためのブロック図
【図９】回避補償部の機能構成を説明するためのブロック図
【図１０】トルク制限部の機能を説明するためのチャート
【図１１】本発明の第２実施形態にかかる接触検出部のブロック図
【図１２】本発明の第２実施形態にかかる外力方向検出部のブロック図
【図１３】本発明の第２実施形態にかかる信号積算処理結果の一例を示す図
【図１４】本発明の第２実施形態にかかる外力方向絞込条件テーブルの一例を示す図
【図１５】本発明の第２実施形態にかかる外力方向絞込処理の一例を示す図
【図１６】本発明の第３実施形態にかかるロボットシステムの全体構成を説明するための模式的な構成図
【発明を実施するための形態】
【００１５】
＜第１実施形態＞
以下、本発明の実施の形態について図を参照して説明する。
本実施形態では、ロボットアーム２を用いて家畜に対して検査、消毒、搾乳などの作業を行なうロボットシステムを例に本発明の一実施形態について説明する。
【００１６】
図１に示すように、本実施形態にかかるロボットシステム１００は、家畜１が収納されたフロアＦ，ロボットアーム２，コントローラ３，センサ部４からなっている。
ロボットアーム２は、図２に示すように、設置面（壁面や床等）１０１に固定された基台４０と、基台４０からロボットアーム２の先端にかけて順々に第１構造材４１，第２構造材４２，第３構造材４３，第４構造材４４，第５構造材４５，第６構造材４６，フランジ４７がそれぞれ回転駆動するアクチュエータ（回転関節）を介して連結されている。
【００１７】
基台４０と第１構造材４１とは、第１アクチュエータ（第１関節）４１Ａを介して連結されており、第１アクチュエータ４１Ａの駆動により、第１構造材４１が回転するようになっている。第１構造材４１と第４構造材４２とは、第２アクチュエータ（第２関節）４２Ａを介して連結されており、第２アクチュエータ４２Ａの駆動により、第２構造材４２が旋回するようになっている。
【００１８】
第２構造材４２と第３構造材４３とは、第３アクチュエータ（第３関節）４３Ａを介して連結されており、第３アクチュエータ４３Ａの駆動により、第３構造材４３が回転するようになっている。第３構造材４３と第４構造材４４とは、第４アクチュエータ（第４関節）４４Ａを介して連結されており、第４アクチュエータ４４Ａの駆動により、第４構造材４４が旋回するようになっている。
【００１９】
第４構造材４４と第５構造材４５とは、第５アクチュエータ（第５関節）４５Ａを介して連結されており、第５アクチュエータ４５Ａの駆動により、第５構造材４５が回転するようになっている。第５構造材４５と第６構造材４６とは、第６アクチュエータ（第６関節）４６Ａを介して連結されており、第６アクチュエータ４６Ａの駆動により、第６構造材４６が旋回するようになっている。
第６構造材４６とフランジ４７とは、第７アクチュエータ（第７関節）４７Ａを介して連結されており、第７アクチュエータ４７Ａの駆動により、フランジ４７及びフランジ４７に取り付けられるハンド等のエンドエフェクタ２Ａが回転するようになっている。
【００２０】
エンドエフェクタには、検査器，搾乳器，消毒器等の種々のツール（図示は省略）が取り付けられ、家畜１のターゲット部１Ａに対して、検査、消毒、搾乳などの作業を行なうようになっている。
また、エンドエフェクタ（換言するとロボットアーム２の先端部分）２Ａには、物体検知センサ（本実施形態では、カメラであるがその他の種々のセンサを適用可能である）２Ｂが取り付けられている。
【００２１】
物体検知センサ２Ｂは、四方が仕切られたフロアＦ内で不規則に動く家畜１のターゲット部１Ａを含む十分な広さの検知領域に対して配向されている。
物体検知センサ２Ｂにより取得した画像はコントローラ３の目標認識部２Ｃに入力され、目標認識部２Ｃでは家畜１のターゲット部１Ａの位置を検出し、検出結果を後述する動作指令部５に目標位置としてリアルタイムに入力するようになっている。
なお、本実施形態では、説明を容易とするため、コントローラ３は一体の制御装置として示しているが実際にはコントローラ３は複数のコンピュータ等で構成し、目標認識部２Ｃや外力検出部（後述）については、ロボットアーム２の動作を制御するロボットコントローラとは別体の制御装置により構成する等してもよい。
【００２２】
また、第１〜第７アクチュエータ４１Ａ〜４７Ａは、それぞれ、ケーブル（図示省略）を挿通可能な中空部を有する減速機一体型のサーボモータによって構成されており、各アクチュエータの回転位置は、アクチュエータに内蔵のエンコーダからの信号としてコントローラ３にケーブルを介して入力されるようになっている。
【００２３】
センサ部４は図３に示すように、センサ固定治具１５と４個のセンサＳ１〜Ｓ４により構成されており、円盤状のセンサ固定治具１５はロボットアーム２の第１アクチュエータ４１Ａの固定子の基部に取り付けられている。
【００２４】
各センサＳ１〜Ｓ４（１６ａ〜１６ｄ）は、円盤状のセンサ固定治具１５に埋設されており、各センサＳ１〜Ｓ４は同一円弧に沿って（仮想円）に等間隔に配置されている。そして、各センサＳ１〜Ｓ４には圧電体として水晶が用いられており、各センサＳ１〜Ｓ４はそれぞれセンサ固定治具１５のラジアル方向の歪量を電圧として検出し、得られた電圧はアンプ部１７を介して増幅され、コントローラ３に入力されるようになっている。
各センサＳ１〜Ｓ４に水晶振動子（水晶圧電素子）を用いることで歪みゲージや他の圧電素子を用いた場合よりも応答時間が小さく後述するサーボ部７の演算周期と比較して良好な応答性を得ることができ、ロボットアーム周辺の対象物の接触等による衝撃を十分に緩和することができる。
【００２５】
なお、本実施形態では上述のように応答時間の早さ等の理由から水晶振動子を用いているが、良好な応答性が得られるものであれば各センサＳ１〜Ｓ４にどのような形式のものでも適用可能である。また、水晶圧電センサは、歪みゲージや通常の衝突センサと比較して耐久性に優れ、ロボットアーム２の自重による負荷が最も大きい基端部にセンサ部４設けた場合でも十分に高精度な検出ができるという利点もある。
【００２６】
ロボットアーム２の各アクチュエータ４１Ａ〜４７Ａを含む各駆動部位の動作はコントローラ３で制御される。
コントローラ３は、記憶装置，電子演算器及び入力装置（いずれも詳細には図示省略）を有するコンピュータにより構成されており、ロボットアーム２の各駆動部位と相互通信可能に接続されている。
図１に示すように、コントローラ３は機能構成として、目標認識部２Ｃ，動作指令部５，平滑化処理部６，サーボ部７，接触検出部（外力検出部）８，位置指令遮断部（動作指令遮断部）９，外力方向検出部１０，回避軸選択部１１，回避補償部（摩擦補償トルク付加部）１２，重力トルク補償部（重力補償トルク付加部）１３，トルク制限部１４を有している。
【００２７】
上述したとおり目標認識部２Ｃは物体検知センサ２Ｂの検知結果に基づいて、家畜１のターゲット部１Ａの位置を求め、ターゲット部１Ａの位置が目標位置として動作指令部５に受渡される。
なお、目標認識部２Ｃによるターゲット部１Ａの位置の認識処理手法は種々の方法を適用可能である。
【００２８】
動作指令部５では、目標認識部２Ｃからの目標位置の入力に基づいて、各アクチュエータ４１Ａ〜４７Ａに対しての位置指令（動作指令）をそれぞれ算出し、平滑化処理部６にプールする。
平滑化処理部６ではプールされた位置指令が演算周期Ｔ（mm/sec）毎にサーボ部７側に順次払い出されるようになっている。
サーボ部７は、各アクチュエータ４１Ａ〜４７Ａ毎に、各アクチュエータ４１Ａ〜４７Ａのエンコーダの検出値による関節角度フィードバック回路Ｆｐと、各アクチュエータ４１Ａ〜４７Ａのエンコーダの検出値から得られる角速度検出値による関節角度フィードバック回路Ｆｖを有しており演算周期Ｔ毎にトルク指令Ｔｒｅｆを出力するようになっている。
【００２９】
接触検出部８はセンサ部４の出力をもとに、家畜１とロボットアーム２との接触の有無を検出する。
接触検出部８の構成についてより詳細に説明すると、図４に示すように、接触検出部８はハイパスフィルタ部１８と閾値判断部１９とを有している。
ハイパスフィルタ部１８は、各センサ部４のアンプ部１７からの各センサ信号それぞれに対して、高周波成分のみそれぞれを抽出するハイパスフィルタ（あるいはバンドパスフィルタ）Ｆ１〜Ｆ４を有し、外力に起因する信号成分を分離して外力方向検出部１０に受け渡すようになっている。
【００３０】
各ハイパスフィルタＦ１〜Ｆ４の調整については、予め接触実験（例えば、ロボットアームをハンマー等で叩く）を行い接触により外力が生じる場合の各センサＳ１〜Ｓ４の検出信号の周波数を計測しておき、外力に起因する周波数以外の周波数成分を除去できるように各フィルタのカットオフ周波数を決定しておく。
このようにすることで、各アクチュエータ４１Ａ〜４７Ａに内蔵される減速機のリップル等、ロボットアーム２の内部で発生する外乱要因により各センサＳ１〜Ｓ４の検出信号に含まれる信号成分と周辺物体またはロボットアーム自身との接触による外力に起因する信号成分を分離することができる。
【００３１】
閾値判断部１９は、各センサＳ１〜Ｓ４の検出信号（ハイパスフィルタＦ１〜Ｆ４通過後）の絶対値が予め設定した閾値（衝突実験により調整する）を超えたかどうか判断し、各センサＳ１〜Ｓ４の検出信号のうちいずれか１つでも閾値を超えた場合には、接触が生じた（外力がある）と判断するようになっている。
閾値判断部１９により接触が生じたことが検知されると、位置指令遮断部９（図１参照）が作動して動作指令部５から平滑化処理部６への位置指令の受渡しが遮断され、サーボ部７に伝達される位置指令が平滑化処理部６により逓減しサーボ部７への位置指令は遮断される。
サーボ部７への位置指令が遮断されると、当然ながらフィードバックにより出力されるトルク指令Ｔｒｅｆの値が減少し、ロボットアーム２は速やかに停止動作に入ることとなる。
【００３２】
外力方向検出部１０は、図５に示すように、機能構成としてピーク算出部２０，正負判断部２１，テーブル参照部２２を有している。
ピーク算出部２０は、ハイパスフィルタ部１８から出力された各センサＳ１〜Ｓ４毎の検出信号それぞれに対して、衝撃波のピーク値を算出する。ピーク値は、検出信号の絶対値が極大（即ち、予め設定された時間内での最大値）である。
正負判断部２１は、算出された各ピーク値の正負（１または−１）またはゼロを出力する。
即ち、ピーク値の絶対値が予め設定した閾値を下まわっている場合は、ゼロ（判定不能）を出力するように構成されている。
【００３３】
テーブル参照部２２は、各センサ信号の衝撃波ピークの正負の組み合わせに対して、予め記憶装置に設定したデータテーブルを参照することで、家畜１等が接触することにより生じる外力の作用する方向ベクトル（外力方向ベクトル）を算出する。
データテーブルは、例えば、図６，図７に示すように各センサＳ１〜Ｓ４毎の検出信号の正負の組み合わせと外力の方向とを対応付けたマトリックスとして構成すればよい。
このように、データテーブルと各センサＳ１〜Ｓ４毎の検出信号の正負の組み合わせとを参照することで複数センサ信号を用いて衝突時の衝撃波ピークの正負から、衝突の方向を大まかに（本実施例では８つの方向）に算出することができる。なお、センサ部４はロボットアーム２の基台４０側に取り付けられているので、家畜１等の接触対象がロボットアーム２の先端付近だけでなく、より基端側の位置に接触した場合でも衝突の方向を算出することができる。
【００３４】
回転軸選択部１１は、図８に示すように、機能構成としてベクトル変換部２３及び関節ベクトル正負判断部２４を有している。
ベクトル変換部２３は、ロボットアーム２の作業座標で記述された外力方向ベクトルにロボットアーム２のヤコビ行列の転置行列をかけて、各アクチュエータ４１Ａ〜４７Ａ毎のベクトルに変換する。
なお、ここでは、ロボットアーム２は冗長マニピュレータであるのでアクチュエータ４３Ａ（冗長軸）のベクトルをゼロとすることができる。
そして、各アクチュエータ４１Ａ，４２Ａをそれぞれ第１軸，第２軸とし、各アクチュエータ４４Ａ〜４７Ａをそれぞれ第３軸〜第６軸として各軸ベクトルに変換する。なお、第１軸〜第６軸を特定しない場合は単に軸ともいう。
【００３５】
ヤコビ行列としては、家畜１とロボットアーム２がロボットアーム２の手首付近で接触しやすいので、ロボットアーム２の手首点を対象としたヤコビ行列を算出すれば良い。
また、転置行列の代わりに逆行列を用いることもできるが、転置行列の方が逆行列よりも計算負荷が小さくてすむ。
関節ベクトル正負判断部２４は、関節空間に変換された外力方向ベクトルの各成分（各関節軸の成分）それぞれについて、正負（１または−１）を出力する。なお、絶対値が予め設定した閾値を下まわっている場合はゼロを出力する。
【００３６】
回避補償部１２は図９に示すように、機能構成として摩擦補償ベクトル算出部２５及び摩擦補償値記憶部２６を有している。
摩擦補償ベクトル算出部２５は、回避軸選択部１１から出力される各関節空間での外力方向ベクトルの各成分（±１またはゼロ）に各軸の摩擦補償値を掛け算して出力する。
各軸の摩擦補償値は予め調整して決定した値を摩擦補償値記憶部に保存しておく。
なお、摩擦補償値は、対応する軸が完全に静止している場合は、軸に該当するアクチュエータの最大静止摩擦トルク、対応する軸が回っている場合は軸に該当するアクチュエータの動摩擦トルクに基づいて摩擦補償値を算出するようになっている。
【００３７】
回避補償部１２から出力された摩擦補償ベクトル（トルク）は、図１に示すようにサーボ部７内部のトルク指令Ｔｒｅｆに重力補償トルクとともに加算される。
なお、重力補償トルクは重力トルク補償部１３で計算される。以降、摩擦補償トルクと重力補償トルクの和を回避補償トルクＴａｖｏという。
【００３８】
そして、サーボ部７のフィードバック回路から出力される元々のトルク指令Ｔｒｅｆと回避補償トルクＴａｖｏの合計がトルク制限部１４に入力される。
また、回転軸選択部１１の出力は、回避補償部１２だけでなく、トルク制限部１４にも入力される。トルク制限部１４では、回転軸選択部１１の出力が＋１または−１の軸を対象に該当するアクチュエータへのトルクの指令値を制限する。
【００３９】
トルク制限部１４は、図１０に示すように、該当するアクチュエータへのトルク指令値を回避補償トルク２６（Ｔａｖｏ）を中心値として、予め実験等により求めておいたトルク制限幅２７（Ｗｔｒｑ）以内に制限するように設定されている。したがって、モータトルクは、トルク上限２８（ＴＵ）とトルク下限２９（ＴＬ）との間に制限（リミット）され、制限されたトルクが最終的なトルク指令値として各アクチュエータ４１Ａ〜４７Ａに送出される。
【００４０】
上述のように回避補償トルクＴａｖｏはロボットアーム２が自重によるモーメント荷重に対して各アクチュエータを支持するととともに（重力補償トルク）、アクチュエータの静止摩擦力あるいは動摩擦力にこうして外力が作用する方向にロボットアーム２を動作させる（摩擦補償トルク）ためのトルクであるので、トルク上限２８（ＴＵ）の値が大きすぎると、過剰なトルクの発生を許容することとなり、却ってロボットアーム２に対する負荷となってしまう。
一方、トルク下限２９（ＴＬ）の値が小さすぎると自重を支えられずに関節が重力によって意図せず回動してしまう。
【００４１】
したがって、トルク制限幅Ｗｔｒｑを十分小さくすることによって、不要な力を家畜１に作用させることなく、衝突による衝撃や外力を緩和する方向にロボットアーム２の各関節を倣わせることができる。トルク制限幅Ｗｔｒｑは、できるだけ小さい（トルク上限ＴＵとトルク下限ＴＬとの差が小さい）方が好ましい。しかしながら、予め設定した重力補償トルクや摩擦補償トルクは常時必ずしも最適なとはならず、誤差を含むものであるため、予め設定した重力補償トルクや摩擦補償トルクの誤差を許容するためには予め実験等で得られた程度のトルク制限幅Ｗｔｒｑを設けることでより安定してロボットアーム２を制御することができる。
なお、トルク制限は、回避軸が選択され、回避補償トルクが確定した直後から実施すればよい。
【００４２】
本発明の第１実施形態にかかるロボットシステムはこのように構成されているので、作業開始時には、物体検知センサ２Ｂからの検出画像に基づいて家畜１のターゲット部１Ａの位置を随時検出して、各演算周期毎の目標位置としてサーボ部７に付与することでフィードバック制御により、ターゲット部１Ａの位置が不規則に変化した場合でもターゲット部１Ａに追従してターゲット部１Ａにエンドエフェクタのツールを寄り付かせて家畜１のターゲット部１Ａに対して、検査、消毒、搾乳などの作業を行なうことができる。
【００４３】
そして、かかる作業時に家畜１が大きく動く等してロボットアーム２と接触するなどして接触検出部８で接触（外力）が検知された場合は、位置指令遮断部９が作動して、動作指令部５からの位置指令を遮断して、サーボ部７に位置指令が伝わらないようになるので、ロボットアーム２は停止動作に入る。
上述したように、このとき、平滑化処理部８に保持されている指令データはサーボ部７に順次払い出されるため、ロボットアーム２が急停止することにより衝撃が生じることが緩和される。
【００４４】
一方、接触検出部８からセンサ出力が外力方向検出部１０に渡され、そこで、家畜１とロボットアーム２との衝突の方向を検出する。回避軸選択部１１は、検出された外力方向ベクトルをもとに、ロボットアーム２のどの関節軸を回避させるかを選択する。回避補償部１２は、選択された関節軸に対して動作補償を施すことにより衝突を緩和させる。また、回避軸選択部１１は、トルク制限部１４に対しても働きかけ、回避軸のトルクを制限する。
【００４５】
これにより、家畜１とロボットアーム２とが接触した場合、応答性良く、外力の方向にロボットアーム２が一連のサーボ部の制御の中で能動的に回避するため接触時の衝撃を緩和することができる。また、接触が生じた位置から外力の方向にロボットアーム２が回避した後に停止するため、家畜１とロボットアーム２が続けて衝突する可能性を低減することができる。
また、本実施形態に係る家畜用ロボット制御装置のトルク制限部１４は、回避対象軸に対して、回避補償トルクを中心としたトルク制限幅Ｗｔｒｑ内に制限されるので、家畜１がさらにロボットアーム２側に動いた場合でもロボットアーム２から家畜１に不要な力を作用させずに衝撃力を受け流す方向に倣うことができ、家畜１及びロボットアーム２に過度な外力が生じることなく安全性が向上するという効果がある。
【００４６】
また、本実施形態にかかる衝突検出部８は、センサ部４の出力信号からハイパスフィルタ部１８によって衝突時の高周波振動のみ抽出するので、センサに減速機リップルなど外乱が作用した場合でも高速かつ高感度に家畜１とロボットアーム２との接触を検知できるという効果がある。
また、本実施形態に係る家畜用ロボット制御装置の回避補償部１２は、トルク指令に回避補償トルクを加えるので、位置指令で回避補償するよりも高速な応答ができるという効果もある。
【００４７】
［第２実施形態］
次に、第２実施形態について説明する。本実施形態にかかるロボットシステムは、コントローラ３の機能構成のみが第１実施形態のものを異なっており、第１実施形態と同様なものについては、重複説明を適宜省略し、同符号を用いて説明する。
【００４８】
図１１は、本実施形態における接触検出部８の詳細ブロック図を示している。
本実施形態では接触検出部８は、ハイパスフィルタ部１８と閾値判断部１９との間に移動平均フィルタ部３０を有している。
移動平均フィルタ部３０は、ハイパスフィルタ部１８から出力された各センサ信号それぞれに対して、現在から遡って予め定めた時間におけるセンサ信号の平均値を出力する移動平均フィルタ１〜４を有している。
このことによって、衝撃波の信号成分と同程度あるいは高い周波数成分をもつ、サーボノイズ等の外乱信号がハイパスフィルタを通過した場合に外乱信号のみが移動平均フィルタによって除去される。これによって、衝突時以外に閾値判断部１９が誤って接触を検知することを回避することができる。前述のとおりに、閾値判断部１９が接触を検知すると、位置指令遮断部９が作動すると同時に、センサ信号通過部３１が作動し、ハイパスフィルタ部１８から出力されたセンサ信号を外力方向検出部１０に出力する。
【００４９】
図１２は、本実施形態における外力方向検出部１０の詳細ブロック図を示している。
信号積算部３２は、図１１のセンサ信号通過部３１から出力された各センサ信号それぞれに対して、現在までに入力されたセンサ信号の積算値を算出する。信号の積算値を用いることで、信号の正負の変化がなめらかになり、各時刻における正負の識別の確実性が向上する。
信号積算処理結果の一例は、図１３に示すように、積算前の信号３４を信号積算部３２に入力して積算後の信号３５が得られる。積算後の信号３５は、積算前の信号３４と比較して正負の変化の頻度が少なくなっている。
【００５０】
絞込条件参照部３３は、正負判断部２１が出力した各信号積算値それぞれの正負に対して、予め設定した外力方向絞込条件テーブル（データテーブル）を参照して衝突の方向（外力方向ベクトル）を算出する。
絞込条件テーブルは図１４に示すように、予め定めた複数の時刻における各センサ信号積算値の正負に対する外力方向ベクトルの候補とを対応付けたデータでありコントローラ３の記憶装置に記憶されている。
【００５１】
図１４の例では、Ｘ１〜Ｘ４の４回の時刻におけるテーブルで構成される（但し、Ｘ１＜Ｘ２＜Ｘ３＜Ｘ４である）。
また、センサＳ１〜Ｓ４と外力方向ベクトルＶ１〜Ｖ８の配置関係は図７の通りである。絞込条件参照部３３は、各時刻毎に、各センサ信号積算値の正負に対する条件にすべて当てはまる外力方向ベクトル候補を抽出する。例えば図１５に示すように、時刻Ｘ１におけるＳ１〜Ｓ４の積算値の正負の組み合わせが（負、負、正、正）であった場合について説明する。
【００５２】
Ｓ１の積算値が負のとき、外力方向ベクトル候補はＶ１，Ｖ３，Ｖ７のいずれかとなる。Ｓ２の積算値が負のとき、外力方向ベクトル候補はＶ１，Ｖ５，Ｖ７のいずれかとなる。このとき、双方の外力方向ベクトル候補いずれにも該当するのはＶ１，Ｖ７のいずれかとなる。同様に、Ｓ３およびＳ４の積算値の正負に対する外力方向ベクトル候補にも該当するものはＶ１，Ｖ７のいずれかとなる。
抽出された外力方向ベクトルの数が１ならば、抽出結果（外力方向ベクトル）を回避軸選択部１１に入力する。候補数が２以上ならば、次の時刻における外力方向ベクトル候補の抽出結果と現時刻までの抽出結果の両方を用いて外力方向ベクトル候補を算出する。
【００５３】
この例の場合、次の時刻Ｘ２における積算値の正負の組み合わせと絞込条件テーブルを参照して外力方向ベクトルの抽出を続ける。このようにして、候補の数が１以下になるまで絞込を繰り返す。複数の時刻におけるセンサ信号を用いることで、外力方向の検出率を向上させることが可能である。また、絞込によって候補数が０になった場合、もしくは予め定めた絞込終了時刻までのすべてのテーブルを参照したとき候補の数が２以上の場合は、外力方向不明と判断して、外力方向ベクトルの全要素を零にして回避軸選択部１１に入力する。
【００５４】
本実施形態にかかるロボットシステムこのように構成されているので、衝突検出部８は、ハイパスフィルタ部１８を通過した、ごくわずかな時間に発生する外乱を移動平均フィルタ部３０によって抑制するので、家畜などの対象物とロボットアーム２とが接触していない場合に接触が生じたと誤検出することがより抑制されるという効果がある。
また、外力方向検出部１０は、信号積算部３２によって信号の正負変化の頻度を少なくして、各時刻における正負識別の確実性を向上させる効果がある。また、絞込条件参照部３３において複数時刻のセンサ信号を参照するために外力方向の検出率を向上させる効果がある。
【００５５】
［第３実施形態］
次に、第３実施形態について説明する。本実施形態にかかるロボットシステムは、コントローラ３の機能構成のみが第１実施形態のものを異なっており、第１実施形態と同様なものについては、重複説明を適宜省略し、同符号を用いて説明する。
図１６に示すように、本実施形態ではコントローラ３の機能構成として、動作指令復帰部３８が設けられている点が第１実施形態と異なっている。
【００５６】
動作指令復帰部３８は、接触検出部８が接触を検出して動作指令部５からの位置指令を遮断した状態で、所定の期間（例えば、衝突回避動作後、予め定めた時間)内に接触検出部８が新たに外力を検出しない場合には、位置指令遮断部９による位置指令の遮断を解除し、
動作指令部５からの位置指令を平滑処理部６を介してサーボ部７側に送るように設定されている。
【００５７】
本発明の第３実施形態にかかるロボットシステムはこのように構成されているので、所定の期間を実験等により予め最適値に設定しておくことで、ロボットアーム２の動作中に例えば、家畜１とロボットアーム２とが接触した場合、所定の期間だけロボットアームが外力の作用方向に回避動作を行なって家畜１とロボットアーム２との過度な接触を防止することができるとともに、所定の期間経過後は物体検知センサ２Ｂの検出結果に基づいてターゲット部１Ａへの寄り付き動作が再開されるので、接触が生じてもロボットアーム２が停止することなく、あるいは、短い時間の停止で家畜１に対して検査、消毒、搾乳などの作業を継続することができる。
【００５８】
つまり、ロボットシステム１００の適用用途によっては、ロボットアーム２と周囲の物体との軽度の接触については許容しながらも、作業を停止することなく作業対象（あるいはロボットの位置目標）に対して安定して作業を行なうことが求められる場合がある。
本実施形態の場合、例えば、家畜１が高頻度で動いてロボットアーム２と頻繁に接触するような場合、その都度、ロボットアーム２が停止すると作業効率が著しく低下する虞があるが、上述の所定の期間を設定することで、家畜１とロボットアーム２とが接触した場合にはロボットアーム２を外力の方向に回避動作させて過度な外力（あるいは衝撃）を抑制しながらも、ロボットアーム２により作業を継続することで、接触による作業効率の低下を少なくすることができるという利点がある。
【００５９】
以上、本発明の実施形態について説明したが、本発明は上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲内で、上記実施形態から適宜変更が可能である。また、上記の各実施形態の手法を適宜組み合わせて利用することも可能である。すなわち、このような変更等が施された技術であっても、本発明の技術的範囲に含まれることは言うまでもない
【００６０】
例えば、上記実施形態では、センサ部４として、ロボットベース部の固定治具に水晶圧電センサを４個埋め込んだ例を説明したが、センサの数を３個以下もしくは５個以上にして使用することも可能である。また、センサを治具内に埋め込んだ例を説明したが、センサをロボット表面に貼り付けることも可能である。さらには、水晶圧電センサ以外のセンサ素子を利用することも可能である。
また、外力方向絞込回数を最大４回とした例を示したが、３回以下もしくは５回以上にしても実施可能である。
なお、本発明は家畜に対して作業を施す用途に限定するものではなく、種々の用途に適用可能である。例えば、人間とロボットが共同あるいは協調作業する場合の制御装置などにも適用できる。
【符号の説明】
【００６１】
１ 家畜
２ ロボット
３ コントローラ
４ センサ部
５ 動作指令部
６ 平滑化処理部
７ サーボ部
８ 接触検出部
９ 位置指令遮断部
１０ 外力方向検出部
１１ 回避軸選択部
１２ 回避補償部
１３ 重力トルク補償部
１４ トルク制限部
４０ 基台
４１〜４６ アーム構造材
４１Ａ〜４７Ａ アクチュエータ
４７ フランジ
【技術分野】
【０００１】
本発明は、ロボットアームを有するロボットシステム及びロボット制御装置に関する。
【背景技術】
【０００２】
ロボットの分野では、ロボット自身あるいはロボットの周囲に存在する物体に対して過剰な負荷（外力）が生じることを回避することが求められている。
例えば、特許文献１にはロボットアームの基端に接触力（外力）を検知する力検出器を取り付け、力検出器の検出結果に基づいてロボットアームの動作を停止させたり、過度な外力がかかった場合に外力が低減する方向にロボットアームを動作させる等の技術が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００６−２１２８７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところが、引用文献１のように、外力を検出してからロボットアームの動作を停止すると、接触した対象が動いている場合にはロボットアームが停止していても大きな衝撃力が生じてしまう。また、外力を低減する方向にロボットを動作させる場合であっても、外力を検出してからロボットアームに新たな動作指令を生成して動作させるため、接触（衝突）が生じてからロボットアームが動作するまでの応答時間が長くなり、衝突時の衝撃を緩和することが困難であるという課題がある。
本発明はこのような課題に鑑みてなされたもので、ロボットアームと周囲の対象物との接触等で生じる外力を低減することができるようにしたロボットシステム及びロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記課題を解決するため、本発明のロボットシステムは、ロボットアームと、ロボットアームに設けられロボットアームを駆動させる１以上のアクチュエータと、ロボットアームとアクチュエータとの少なくともいずれかにかかる外力を検出するセンサ部と、アクチュエータの動作を制御するとともに、センサ部の検出結果に基づいてアクチュエータへのトルク指令値を制限するコントローラと、を有していることを特徴としている。
【０００６】
また、ロボットアーム又はアクチュエータの内、コントローラにより位置及び姿勢が制御される点である制御点の位置及び姿勢を検知する位置センサを有し、コントローラは、制御点のとるべき位置及び姿勢に対応する各アクチュエータの位置を目標位置として設定し、アクチュエータそれぞれの動作指令を設定する動作指令設定部と、少なくとも動作指令と位置センサの検出結果とに基づいて、所定の演算周期毎に、アクチュエータのそれぞれに対してトルク指令を生成するサーボ部とセンサ部の検出結果に基づいて外力が生じたときに外力の方向を検出する外力検出部と、外力検出部により検出された外力の方向に基づいて、１以上のアクチュエータのうち回避動作を行なうべきアクチュエータを選択する回避軸選択部と、サーボ部により生成されるトルク指令に自重分の重力補償トルクを付加する重力補償トルク付加部と、回避軸選択部により選択されたアクチュエータのそれぞれに対してサーボ部により生成されるトルク指令に、アクチュエータ自身の摩擦力に抗して外力の方向に動作するための摩擦補償トルクを付加する摩擦補償トルク付加部と、を有していることが好ましい。
【０００７】
また、コントローラは、外力検出部により外力が検出されると、サーボ部に与える動作指令を遮断する動作指令遮断部を有していることが好ましい。
また、コントローラは、動作指令遮断部により動作指令が遮断されてから予め設定された期間以上外力検出部により外力が検出されないときは、動作指令遮断部による動作指令の遮断を解除する動作指令復帰部を有していることが好ましい。
また、コントローラは、アクチュエータへのトルク指令値を制限するトルク指令制限部を有し、トルク指令制限部は、重力補償トルクと摩擦補償トルクとの和に基づいてトルク指令値の上限値及び下限値を設定することが好ましい。
【０００８】
また、センサ部は、圧電体として水晶を用いた水晶振動子を複数含んで形成されていることが好ましい。
また、センサ部は、ロボットアームの内最も基端側のアクチュエータの基部に設けられた円板状のセンサ固定治具と、センサ固定冶具に同一円弧に沿って埋設された複数のセンサとを有していることが好ましい。
【０００９】
外力検出部は、複数のセンサからの信号の高周波振動成分を抽出し、抽出された高周波振動成分に基づいて、外力の有無を検出することが好ましい。
外力検出部は、抽出された高周波振動成分のピーク値を各センサ毎に求め、各センサ毎のピーク値の正負組み合わせに基づいて外力の方向を検出することが好ましい。
回避軸選択部は、外力検出部により求められた外力の方向ベクトルにヤコビ行列の転置行列を掛け算したベクトルの各成分を閾値処理して回避対象関節軸と方向を算出することが好ましい。
【００１０】
摩擦補償トルク付加部は、回避軸選択部により選択されたアクチュエータが静止しているときは当該アクチュエータの最大静止摩擦トルクを摩擦補償トルクとして付加し、当該アクチュエータが回転しているときは当該アクチュエータの動摩擦トルクを摩擦補償トルクとして付加することが好ましい。
外力検出部は、抽出された各センサ毎の高周波振動成分をそれぞれ移動平均化処理して得た移動平均値に基づいて外力の有無を検出することが好ましい。
外力検出部は、外力が有ることを検出した時点以降における各センサ毎の高周波振動成分の積算値をそれぞれ求め、積算値の正負組み合わせに基づいて外力の方向を検出することが好ましい。
【００１１】
コントローラは、各センサ毎の積算値の正負の組み合わせ、または、各センサ毎の移動平均値の正負の組み合わせ、と外力の方向とを対応付けたデータテーブルを有し、外力検出部は、外力が有ることを検出した時点以降の複数の時点において、それぞれ、各センサ毎の積算値の正負の組み合わせ、または、各センサ毎の移動平均値の正負の組み合わせに基づいて外力の方向を検出することが好ましい。
【００１２】
また、本発明のロボット制御装置は、１以上のアクチュエータを有するロボットアームと、ロボットアームとアクチュエータとの少なくともいずれかにかかる外力を検出するセンサ部とを有するロボットの制御装置であって、アクチュエータに対してトルク指令を送出するアクチュエータ制御部と、センサ部からの入力結果に基づいてアクチュエータへ送出されるトルク指令値を制限するトルク指令制限部と、を有していることを特徴としている。
【発明の効果】
【００１３】
本発明によれば、ロボットアームに内蔵されたセンサに、例えば、アクチュエータに含まれる減速機のリップル等に起因する外乱が作用しても、外力の有無を誤検出することなくより正確に外力の有無と方向を検出することができる。そして、外力が検出されるとアクチュエータに対するトルク指令値を制限するので、外力が各アクチュエータへのトルク指令値を超えるとアクチュエータが外力に倣って変位するため急激な外力の負荷による衝撃を緩和することができる。
また、ロボットアームの周囲の対象物がロボットアームに向かって運動を継続する場合等、継続してロボットアームに外力がかかる場合であってもアクチュエータが外力に倣って変位することでロボットアームの姿勢を変えることで、ロボットアームの周囲の対象物を回避することができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施形態に係るロボットシステムの全体構成を説明するための模式的な構成図
【図２】本発明の実施形態にかかるロボットの構成を示す側面図である。
【図３】センサ部の構成を示す模式的な構造図
【図４】接触検出部の機能構成を説明するためのブロック図
【図５】外力方向検出部の機能構成を説明するためのブロック図
【図６】テーブルデータを示す模式図
【図７】センサの配置と外力方向ベクトルの関係を表す図
【図８】回避軸選択部の機能構成を説明するためのブロック図
【図９】回避補償部の機能構成を説明するためのブロック図
【図１０】トルク制限部の機能を説明するためのチャート
【図１１】本発明の第２実施形態にかかる接触検出部のブロック図
【図１２】本発明の第２実施形態にかかる外力方向検出部のブロック図
【図１３】本発明の第２実施形態にかかる信号積算処理結果の一例を示す図
【図１４】本発明の第２実施形態にかかる外力方向絞込条件テーブルの一例を示す図
【図１５】本発明の第２実施形態にかかる外力方向絞込処理の一例を示す図
【図１６】本発明の第３実施形態にかかるロボットシステムの全体構成を説明するための模式的な構成図
【発明を実施するための形態】
【００１５】
＜第１実施形態＞
以下、本発明の実施の形態について図を参照して説明する。
本実施形態では、ロボットアーム２を用いて家畜に対して検査、消毒、搾乳などの作業を行なうロボットシステムを例に本発明の一実施形態について説明する。
【００１６】
図１に示すように、本実施形態にかかるロボットシステム１００は、家畜１が収納されたフロアＦ，ロボットアーム２，コントローラ３，センサ部４からなっている。
ロボットアーム２は、図２に示すように、設置面（壁面や床等）１０１に固定された基台４０と、基台４０からロボットアーム２の先端にかけて順々に第１構造材４１，第２構造材４２，第３構造材４３，第４構造材４４，第５構造材４５，第６構造材４６，フランジ４７がそれぞれ回転駆動するアクチュエータ（回転関節）を介して連結されている。
【００１７】
基台４０と第１構造材４１とは、第１アクチュエータ（第１関節）４１Ａを介して連結されており、第１アクチュエータ４１Ａの駆動により、第１構造材４１が回転するようになっている。第１構造材４１と第４構造材４２とは、第２アクチュエータ（第２関節）４２Ａを介して連結されており、第２アクチュエータ４２Ａの駆動により、第２構造材４２が旋回するようになっている。
【００１８】
第２構造材４２と第３構造材４３とは、第３アクチュエータ（第３関節）４３Ａを介して連結されており、第３アクチュエータ４３Ａの駆動により、第３構造材４３が回転するようになっている。第３構造材４３と第４構造材４４とは、第４アクチュエータ（第４関節）４４Ａを介して連結されており、第４アクチュエータ４４Ａの駆動により、第４構造材４４が旋回するようになっている。
【００１９】
第４構造材４４と第５構造材４５とは、第５アクチュエータ（第５関節）４５Ａを介して連結されており、第５アクチュエータ４５Ａの駆動により、第５構造材４５が回転するようになっている。第５構造材４５と第６構造材４６とは、第６アクチュエータ（第６関節）４６Ａを介して連結されており、第６アクチュエータ４６Ａの駆動により、第６構造材４６が旋回するようになっている。
第６構造材４６とフランジ４７とは、第７アクチュエータ（第７関節）４７Ａを介して連結されており、第７アクチュエータ４７Ａの駆動により、フランジ４７及びフランジ４７に取り付けられるハンド等のエンドエフェクタ２Ａが回転するようになっている。
【００２０】
エンドエフェクタには、検査器，搾乳器，消毒器等の種々のツール（図示は省略）が取り付けられ、家畜１のターゲット部１Ａに対して、検査、消毒、搾乳などの作業を行なうようになっている。
また、エンドエフェクタ（換言するとロボットアーム２の先端部分）２Ａには、物体検知センサ（本実施形態では、カメラであるがその他の種々のセンサを適用可能である）２Ｂが取り付けられている。
【００２１】
物体検知センサ２Ｂは、四方が仕切られたフロアＦ内で不規則に動く家畜１のターゲット部１Ａを含む十分な広さの検知領域に対して配向されている。
物体検知センサ２Ｂにより取得した画像はコントローラ３の目標認識部２Ｃに入力され、目標認識部２Ｃでは家畜１のターゲット部１Ａの位置を検出し、検出結果を後述する動作指令部５に目標位置としてリアルタイムに入力するようになっている。
なお、本実施形態では、説明を容易とするため、コントローラ３は一体の制御装置として示しているが実際にはコントローラ３は複数のコンピュータ等で構成し、目標認識部２Ｃや外力検出部（後述）については、ロボットアーム２の動作を制御するロボットコントローラとは別体の制御装置により構成する等してもよい。
【００２２】
また、第１〜第７アクチュエータ４１Ａ〜４７Ａは、それぞれ、ケーブル（図示省略）を挿通可能な中空部を有する減速機一体型のサーボモータによって構成されており、各アクチュエータの回転位置は、アクチュエータに内蔵のエンコーダからの信号としてコントローラ３にケーブルを介して入力されるようになっている。
【００２３】
センサ部４は図３に示すように、センサ固定治具１５と４個のセンサＳ１〜Ｓ４により構成されており、円盤状のセンサ固定治具１５はロボットアーム２の第１アクチュエータ４１Ａの固定子の基部に取り付けられている。
【００２４】
各センサＳ１〜Ｓ４（１６ａ〜１６ｄ）は、円盤状のセンサ固定治具１５に埋設されており、各センサＳ１〜Ｓ４は同一円弧に沿って（仮想円）に等間隔に配置されている。そして、各センサＳ１〜Ｓ４には圧電体として水晶が用いられており、各センサＳ１〜Ｓ４はそれぞれセンサ固定治具１５のラジアル方向の歪量を電圧として検出し、得られた電圧はアンプ部１７を介して増幅され、コントローラ３に入力されるようになっている。
各センサＳ１〜Ｓ４に水晶振動子（水晶圧電素子）を用いることで歪みゲージや他の圧電素子を用いた場合よりも応答時間が小さく後述するサーボ部７の演算周期と比較して良好な応答性を得ることができ、ロボットアーム周辺の対象物の接触等による衝撃を十分に緩和することができる。
【００２５】
なお、本実施形態では上述のように応答時間の早さ等の理由から水晶振動子を用いているが、良好な応答性が得られるものであれば各センサＳ１〜Ｓ４にどのような形式のものでも適用可能である。また、水晶圧電センサは、歪みゲージや通常の衝突センサと比較して耐久性に優れ、ロボットアーム２の自重による負荷が最も大きい基端部にセンサ部４設けた場合でも十分に高精度な検出ができるという利点もある。
【００２６】
ロボットアーム２の各アクチュエータ４１Ａ〜４７Ａを含む各駆動部位の動作はコントローラ３で制御される。
コントローラ３は、記憶装置，電子演算器及び入力装置（いずれも詳細には図示省略）を有するコンピュータにより構成されており、ロボットアーム２の各駆動部位と相互通信可能に接続されている。
図１に示すように、コントローラ３は機能構成として、目標認識部２Ｃ，動作指令部５，平滑化処理部６，サーボ部７，接触検出部（外力検出部）８，位置指令遮断部（動作指令遮断部）９，外力方向検出部１０，回避軸選択部１１，回避補償部（摩擦補償トルク付加部）１２，重力トルク補償部（重力補償トルク付加部）１３，トルク制限部１４を有している。
【００２７】
上述したとおり目標認識部２Ｃは物体検知センサ２Ｂの検知結果に基づいて、家畜１のターゲット部１Ａの位置を求め、ターゲット部１Ａの位置が目標位置として動作指令部５に受渡される。
なお、目標認識部２Ｃによるターゲット部１Ａの位置の認識処理手法は種々の方法を適用可能である。
【００２８】
動作指令部５では、目標認識部２Ｃからの目標位置の入力に基づいて、各アクチュエータ４１Ａ〜４７Ａに対しての位置指令（動作指令）をそれぞれ算出し、平滑化処理部６にプールする。
平滑化処理部６ではプールされた位置指令が演算周期Ｔ（mm/sec）毎にサーボ部７側に順次払い出されるようになっている。
サーボ部７は、各アクチュエータ４１Ａ〜４７Ａ毎に、各アクチュエータ４１Ａ〜４７Ａのエンコーダの検出値による関節角度フィードバック回路Ｆｐと、各アクチュエータ４１Ａ〜４７Ａのエンコーダの検出値から得られる角速度検出値による関節角度フィードバック回路Ｆｖを有しており演算周期Ｔ毎にトルク指令Ｔｒｅｆを出力するようになっている。
【００２９】
接触検出部８はセンサ部４の出力をもとに、家畜１とロボットアーム２との接触の有無を検出する。
接触検出部８の構成についてより詳細に説明すると、図４に示すように、接触検出部８はハイパスフィルタ部１８と閾値判断部１９とを有している。
ハイパスフィルタ部１８は、各センサ部４のアンプ部１７からの各センサ信号それぞれに対して、高周波成分のみそれぞれを抽出するハイパスフィルタ（あるいはバンドパスフィルタ）Ｆ１〜Ｆ４を有し、外力に起因する信号成分を分離して外力方向検出部１０に受け渡すようになっている。
【００３０】
各ハイパスフィルタＦ１〜Ｆ４の調整については、予め接触実験（例えば、ロボットアームをハンマー等で叩く）を行い接触により外力が生じる場合の各センサＳ１〜Ｓ４の検出信号の周波数を計測しておき、外力に起因する周波数以外の周波数成分を除去できるように各フィルタのカットオフ周波数を決定しておく。
このようにすることで、各アクチュエータ４１Ａ〜４７Ａに内蔵される減速機のリップル等、ロボットアーム２の内部で発生する外乱要因により各センサＳ１〜Ｓ４の検出信号に含まれる信号成分と周辺物体またはロボットアーム自身との接触による外力に起因する信号成分を分離することができる。
【００３１】
閾値判断部１９は、各センサＳ１〜Ｓ４の検出信号（ハイパスフィルタＦ１〜Ｆ４通過後）の絶対値が予め設定した閾値（衝突実験により調整する）を超えたかどうか判断し、各センサＳ１〜Ｓ４の検出信号のうちいずれか１つでも閾値を超えた場合には、接触が生じた（外力がある）と判断するようになっている。
閾値判断部１９により接触が生じたことが検知されると、位置指令遮断部９（図１参照）が作動して動作指令部５から平滑化処理部６への位置指令の受渡しが遮断され、サーボ部７に伝達される位置指令が平滑化処理部６により逓減しサーボ部７への位置指令は遮断される。
サーボ部７への位置指令が遮断されると、当然ながらフィードバックにより出力されるトルク指令Ｔｒｅｆの値が減少し、ロボットアーム２は速やかに停止動作に入ることとなる。
【００３２】
外力方向検出部１０は、図５に示すように、機能構成としてピーク算出部２０，正負判断部２１，テーブル参照部２２を有している。
ピーク算出部２０は、ハイパスフィルタ部１８から出力された各センサＳ１〜Ｓ４毎の検出信号それぞれに対して、衝撃波のピーク値を算出する。ピーク値は、検出信号の絶対値が極大（即ち、予め設定された時間内での最大値）である。
正負判断部２１は、算出された各ピーク値の正負（１または−１）またはゼロを出力する。
即ち、ピーク値の絶対値が予め設定した閾値を下まわっている場合は、ゼロ（判定不能）を出力するように構成されている。
【００３３】
テーブル参照部２２は、各センサ信号の衝撃波ピークの正負の組み合わせに対して、予め記憶装置に設定したデータテーブルを参照することで、家畜１等が接触することにより生じる外力の作用する方向ベクトル（外力方向ベクトル）を算出する。
データテーブルは、例えば、図６，図７に示すように各センサＳ１〜Ｓ４毎の検出信号の正負の組み合わせと外力の方向とを対応付けたマトリックスとして構成すればよい。
このように、データテーブルと各センサＳ１〜Ｓ４毎の検出信号の正負の組み合わせとを参照することで複数センサ信号を用いて衝突時の衝撃波ピークの正負から、衝突の方向を大まかに（本実施例では８つの方向）に算出することができる。なお、センサ部４はロボットアーム２の基台４０側に取り付けられているので、家畜１等の接触対象がロボットアーム２の先端付近だけでなく、より基端側の位置に接触した場合でも衝突の方向を算出することができる。
【００３４】
回転軸選択部１１は、図８に示すように、機能構成としてベクトル変換部２３及び関節ベクトル正負判断部２４を有している。
ベクトル変換部２３は、ロボットアーム２の作業座標で記述された外力方向ベクトルにロボットアーム２のヤコビ行列の転置行列をかけて、各アクチュエータ４１Ａ〜４７Ａ毎のベクトルに変換する。
なお、ここでは、ロボットアーム２は冗長マニピュレータであるのでアクチュエータ４３Ａ（冗長軸）のベクトルをゼロとすることができる。
そして、各アクチュエータ４１Ａ，４２Ａをそれぞれ第１軸，第２軸とし、各アクチュエータ４４Ａ〜４７Ａをそれぞれ第３軸〜第６軸として各軸ベクトルに変換する。なお、第１軸〜第６軸を特定しない場合は単に軸ともいう。
【００３５】
ヤコビ行列としては、家畜１とロボットアーム２がロボットアーム２の手首付近で接触しやすいので、ロボットアーム２の手首点を対象としたヤコビ行列を算出すれば良い。
また、転置行列の代わりに逆行列を用いることもできるが、転置行列の方が逆行列よりも計算負荷が小さくてすむ。
関節ベクトル正負判断部２４は、関節空間に変換された外力方向ベクトルの各成分（各関節軸の成分）それぞれについて、正負（１または−１）を出力する。なお、絶対値が予め設定した閾値を下まわっている場合はゼロを出力する。
【００３６】
回避補償部１２は図９に示すように、機能構成として摩擦補償ベクトル算出部２５及び摩擦補償値記憶部２６を有している。
摩擦補償ベクトル算出部２５は、回避軸選択部１１から出力される各関節空間での外力方向ベクトルの各成分（±１またはゼロ）に各軸の摩擦補償値を掛け算して出力する。
各軸の摩擦補償値は予め調整して決定した値を摩擦補償値記憶部に保存しておく。
なお、摩擦補償値は、対応する軸が完全に静止している場合は、軸に該当するアクチュエータの最大静止摩擦トルク、対応する軸が回っている場合は軸に該当するアクチュエータの動摩擦トルクに基づいて摩擦補償値を算出するようになっている。
【００３７】
回避補償部１２から出力された摩擦補償ベクトル（トルク）は、図１に示すようにサーボ部７内部のトルク指令Ｔｒｅｆに重力補償トルクとともに加算される。
なお、重力補償トルクは重力トルク補償部１３で計算される。以降、摩擦補償トルクと重力補償トルクの和を回避補償トルクＴａｖｏという。
【００３８】
そして、サーボ部７のフィードバック回路から出力される元々のトルク指令Ｔｒｅｆと回避補償トルクＴａｖｏの合計がトルク制限部１４に入力される。
また、回転軸選択部１１の出力は、回避補償部１２だけでなく、トルク制限部１４にも入力される。トルク制限部１４では、回転軸選択部１１の出力が＋１または−１の軸を対象に該当するアクチュエータへのトルクの指令値を制限する。
【００３９】
トルク制限部１４は、図１０に示すように、該当するアクチュエータへのトルク指令値を回避補償トルク２６（Ｔａｖｏ）を中心値として、予め実験等により求めておいたトルク制限幅２７（Ｗｔｒｑ）以内に制限するように設定されている。したがって、モータトルクは、トルク上限２８（ＴＵ）とトルク下限２９（ＴＬ）との間に制限（リミット）され、制限されたトルクが最終的なトルク指令値として各アクチュエータ４１Ａ〜４７Ａに送出される。
【００４０】
上述のように回避補償トルクＴａｖｏはロボットアーム２が自重によるモーメント荷重に対して各アクチュエータを支持するととともに（重力補償トルク）、アクチュエータの静止摩擦力あるいは動摩擦力にこうして外力が作用する方向にロボットアーム２を動作させる（摩擦補償トルク）ためのトルクであるので、トルク上限２８（ＴＵ）の値が大きすぎると、過剰なトルクの発生を許容することとなり、却ってロボットアーム２に対する負荷となってしまう。
一方、トルク下限２９（ＴＬ）の値が小さすぎると自重を支えられずに関節が重力によって意図せず回動してしまう。
【００４１】
したがって、トルク制限幅Ｗｔｒｑを十分小さくすることによって、不要な力を家畜１に作用させることなく、衝突による衝撃や外力を緩和する方向にロボットアーム２の各関節を倣わせることができる。トルク制限幅Ｗｔｒｑは、できるだけ小さい（トルク上限ＴＵとトルク下限ＴＬとの差が小さい）方が好ましい。しかしながら、予め設定した重力補償トルクや摩擦補償トルクは常時必ずしも最適なとはならず、誤差を含むものであるため、予め設定した重力補償トルクや摩擦補償トルクの誤差を許容するためには予め実験等で得られた程度のトルク制限幅Ｗｔｒｑを設けることでより安定してロボットアーム２を制御することができる。
なお、トルク制限は、回避軸が選択され、回避補償トルクが確定した直後から実施すればよい。
【００４２】
本発明の第１実施形態にかかるロボットシステムはこのように構成されているので、作業開始時には、物体検知センサ２Ｂからの検出画像に基づいて家畜１のターゲット部１Ａの位置を随時検出して、各演算周期毎の目標位置としてサーボ部７に付与することでフィードバック制御により、ターゲット部１Ａの位置が不規則に変化した場合でもターゲット部１Ａに追従してターゲット部１Ａにエンドエフェクタのツールを寄り付かせて家畜１のターゲット部１Ａに対して、検査、消毒、搾乳などの作業を行なうことができる。
【００４３】
そして、かかる作業時に家畜１が大きく動く等してロボットアーム２と接触するなどして接触検出部８で接触（外力）が検知された場合は、位置指令遮断部９が作動して、動作指令部５からの位置指令を遮断して、サーボ部７に位置指令が伝わらないようになるので、ロボットアーム２は停止動作に入る。
上述したように、このとき、平滑化処理部８に保持されている指令データはサーボ部７に順次払い出されるため、ロボットアーム２が急停止することにより衝撃が生じることが緩和される。
【００４４】
一方、接触検出部８からセンサ出力が外力方向検出部１０に渡され、そこで、家畜１とロボットアーム２との衝突の方向を検出する。回避軸選択部１１は、検出された外力方向ベクトルをもとに、ロボットアーム２のどの関節軸を回避させるかを選択する。回避補償部１２は、選択された関節軸に対して動作補償を施すことにより衝突を緩和させる。また、回避軸選択部１１は、トルク制限部１４に対しても働きかけ、回避軸のトルクを制限する。
【００４５】
これにより、家畜１とロボットアーム２とが接触した場合、応答性良く、外力の方向にロボットアーム２が一連のサーボ部の制御の中で能動的に回避するため接触時の衝撃を緩和することができる。また、接触が生じた位置から外力の方向にロボットアーム２が回避した後に停止するため、家畜１とロボットアーム２が続けて衝突する可能性を低減することができる。
また、本実施形態に係る家畜用ロボット制御装置のトルク制限部１４は、回避対象軸に対して、回避補償トルクを中心としたトルク制限幅Ｗｔｒｑ内に制限されるので、家畜１がさらにロボットアーム２側に動いた場合でもロボットアーム２から家畜１に不要な力を作用させずに衝撃力を受け流す方向に倣うことができ、家畜１及びロボットアーム２に過度な外力が生じることなく安全性が向上するという効果がある。
【００４６】
また、本実施形態にかかる衝突検出部８は、センサ部４の出力信号からハイパスフィルタ部１８によって衝突時の高周波振動のみ抽出するので、センサに減速機リップルなど外乱が作用した場合でも高速かつ高感度に家畜１とロボットアーム２との接触を検知できるという効果がある。
また、本実施形態に係る家畜用ロボット制御装置の回避補償部１２は、トルク指令に回避補償トルクを加えるので、位置指令で回避補償するよりも高速な応答ができるという効果もある。
【００４７】
［第２実施形態］
次に、第２実施形態について説明する。本実施形態にかかるロボットシステムは、コントローラ３の機能構成のみが第１実施形態のものを異なっており、第１実施形態と同様なものについては、重複説明を適宜省略し、同符号を用いて説明する。
【００４８】
図１１は、本実施形態における接触検出部８の詳細ブロック図を示している。
本実施形態では接触検出部８は、ハイパスフィルタ部１８と閾値判断部１９との間に移動平均フィルタ部３０を有している。
移動平均フィルタ部３０は、ハイパスフィルタ部１８から出力された各センサ信号それぞれに対して、現在から遡って予め定めた時間におけるセンサ信号の平均値を出力する移動平均フィルタ１〜４を有している。
このことによって、衝撃波の信号成分と同程度あるいは高い周波数成分をもつ、サーボノイズ等の外乱信号がハイパスフィルタを通過した場合に外乱信号のみが移動平均フィルタによって除去される。これによって、衝突時以外に閾値判断部１９が誤って接触を検知することを回避することができる。前述のとおりに、閾値判断部１９が接触を検知すると、位置指令遮断部９が作動すると同時に、センサ信号通過部３１が作動し、ハイパスフィルタ部１８から出力されたセンサ信号を外力方向検出部１０に出力する。
【００４９】
図１２は、本実施形態における外力方向検出部１０の詳細ブロック図を示している。
信号積算部３２は、図１１のセンサ信号通過部３１から出力された各センサ信号それぞれに対して、現在までに入力されたセンサ信号の積算値を算出する。信号の積算値を用いることで、信号の正負の変化がなめらかになり、各時刻における正負の識別の確実性が向上する。
信号積算処理結果の一例は、図１３に示すように、積算前の信号３４を信号積算部３２に入力して積算後の信号３５が得られる。積算後の信号３５は、積算前の信号３４と比較して正負の変化の頻度が少なくなっている。
【００５０】
絞込条件参照部３３は、正負判断部２１が出力した各信号積算値それぞれの正負に対して、予め設定した外力方向絞込条件テーブル（データテーブル）を参照して衝突の方向（外力方向ベクトル）を算出する。
絞込条件テーブルは図１４に示すように、予め定めた複数の時刻における各センサ信号積算値の正負に対する外力方向ベクトルの候補とを対応付けたデータでありコントローラ３の記憶装置に記憶されている。
【００５１】
図１４の例では、Ｘ１〜Ｘ４の４回の時刻におけるテーブルで構成される（但し、Ｘ１＜Ｘ２＜Ｘ３＜Ｘ４である）。
また、センサＳ１〜Ｓ４と外力方向ベクトルＶ１〜Ｖ８の配置関係は図７の通りである。絞込条件参照部３３は、各時刻毎に、各センサ信号積算値の正負に対する条件にすべて当てはまる外力方向ベクトル候補を抽出する。例えば図１５に示すように、時刻Ｘ１におけるＳ１〜Ｓ４の積算値の正負の組み合わせが（負、負、正、正）であった場合について説明する。
【００５２】
Ｓ１の積算値が負のとき、外力方向ベクトル候補はＶ１，Ｖ３，Ｖ７のいずれかとなる。Ｓ２の積算値が負のとき、外力方向ベクトル候補はＶ１，Ｖ５，Ｖ７のいずれかとなる。このとき、双方の外力方向ベクトル候補いずれにも該当するのはＶ１，Ｖ７のいずれかとなる。同様に、Ｓ３およびＳ４の積算値の正負に対する外力方向ベクトル候補にも該当するものはＶ１，Ｖ７のいずれかとなる。
抽出された外力方向ベクトルの数が１ならば、抽出結果（外力方向ベクトル）を回避軸選択部１１に入力する。候補数が２以上ならば、次の時刻における外力方向ベクトル候補の抽出結果と現時刻までの抽出結果の両方を用いて外力方向ベクトル候補を算出する。
【００５３】
この例の場合、次の時刻Ｘ２における積算値の正負の組み合わせと絞込条件テーブルを参照して外力方向ベクトルの抽出を続ける。このようにして、候補の数が１以下になるまで絞込を繰り返す。複数の時刻におけるセンサ信号を用いることで、外力方向の検出率を向上させることが可能である。また、絞込によって候補数が０になった場合、もしくは予め定めた絞込終了時刻までのすべてのテーブルを参照したとき候補の数が２以上の場合は、外力方向不明と判断して、外力方向ベクトルの全要素を零にして回避軸選択部１１に入力する。
【００５４】
本実施形態にかかるロボットシステムこのように構成されているので、衝突検出部８は、ハイパスフィルタ部１８を通過した、ごくわずかな時間に発生する外乱を移動平均フィルタ部３０によって抑制するので、家畜などの対象物とロボットアーム２とが接触していない場合に接触が生じたと誤検出することがより抑制されるという効果がある。
また、外力方向検出部１０は、信号積算部３２によって信号の正負変化の頻度を少なくして、各時刻における正負識別の確実性を向上させる効果がある。また、絞込条件参照部３３において複数時刻のセンサ信号を参照するために外力方向の検出率を向上させる効果がある。
【００５５】
［第３実施形態］
次に、第３実施形態について説明する。本実施形態にかかるロボットシステムは、コントローラ３の機能構成のみが第１実施形態のものを異なっており、第１実施形態と同様なものについては、重複説明を適宜省略し、同符号を用いて説明する。
図１６に示すように、本実施形態ではコントローラ３の機能構成として、動作指令復帰部３８が設けられている点が第１実施形態と異なっている。
【００５６】
動作指令復帰部３８は、接触検出部８が接触を検出して動作指令部５からの位置指令を遮断した状態で、所定の期間（例えば、衝突回避動作後、予め定めた時間)内に接触検出部８が新たに外力を検出しない場合には、位置指令遮断部９による位置指令の遮断を解除し、
動作指令部５からの位置指令を平滑処理部６を介してサーボ部７側に送るように設定されている。
【００５７】
本発明の第３実施形態にかかるロボットシステムはこのように構成されているので、所定の期間を実験等により予め最適値に設定しておくことで、ロボットアーム２の動作中に例えば、家畜１とロボットアーム２とが接触した場合、所定の期間だけロボットアームが外力の作用方向に回避動作を行なって家畜１とロボットアーム２との過度な接触を防止することができるとともに、所定の期間経過後は物体検知センサ２Ｂの検出結果に基づいてターゲット部１Ａへの寄り付き動作が再開されるので、接触が生じてもロボットアーム２が停止することなく、あるいは、短い時間の停止で家畜１に対して検査、消毒、搾乳などの作業を継続することができる。
【００５８】
つまり、ロボットシステム１００の適用用途によっては、ロボットアーム２と周囲の物体との軽度の接触については許容しながらも、作業を停止することなく作業対象（あるいはロボットの位置目標）に対して安定して作業を行なうことが求められる場合がある。
本実施形態の場合、例えば、家畜１が高頻度で動いてロボットアーム２と頻繁に接触するような場合、その都度、ロボットアーム２が停止すると作業効率が著しく低下する虞があるが、上述の所定の期間を設定することで、家畜１とロボットアーム２とが接触した場合にはロボットアーム２を外力の方向に回避動作させて過度な外力（あるいは衝撃）を抑制しながらも、ロボットアーム２により作業を継続することで、接触による作業効率の低下を少なくすることができるという利点がある。
【００５９】
以上、本発明の実施形態について説明したが、本発明は上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲内で、上記実施形態から適宜変更が可能である。また、上記の各実施形態の手法を適宜組み合わせて利用することも可能である。すなわち、このような変更等が施された技術であっても、本発明の技術的範囲に含まれることは言うまでもない
【００６０】
例えば、上記実施形態では、センサ部４として、ロボットベース部の固定治具に水晶圧電センサを４個埋め込んだ例を説明したが、センサの数を３個以下もしくは５個以上にして使用することも可能である。また、センサを治具内に埋め込んだ例を説明したが、センサをロボット表面に貼り付けることも可能である。さらには、水晶圧電センサ以外のセンサ素子を利用することも可能である。
また、外力方向絞込回数を最大４回とした例を示したが、３回以下もしくは５回以上にしても実施可能である。
なお、本発明は家畜に対して作業を施す用途に限定するものではなく、種々の用途に適用可能である。例えば、人間とロボットが共同あるいは協調作業する場合の制御装置などにも適用できる。
【符号の説明】
【００６１】
１ 家畜
２ ロボット
３ コントローラ
４ センサ部
５ 動作指令部
６ 平滑化処理部
７ サーボ部
８ 接触検出部
９ 位置指令遮断部
１０ 外力方向検出部
１１ 回避軸選択部
１２ 回避補償部
１３ 重力トルク補償部
１４ トルク制限部
４０ 基台
４１〜４６ アーム構造材
４１Ａ〜４７Ａ アクチュエータ
４７ フランジ
【特許請求の範囲】
【請求項１】
ロボットアームと、
前記ロボットアームに設けられ前記ロボットアームを駆動させる１以上のアクチュエータと、
前記ロボットアームと前記アクチュエータとの少なくともいずれかにかかる外力を検出するセンサ部と、
前記アクチュエータの動作を制御するとともに、前記センサ部の検出結果に基づいて前記アクチュエータへのトルク指令値を制限するコントローラと、を有している
ことを特徴とする、ロボットシステム。
【請求項２】
前記ロボットアーム又はアクチュエータの内、前記コントローラにより位置及び姿勢が制御される点である制御点の位置及び姿勢を検知する位置センサを有し、
前記コントローラは、
前記制御点のとるべき位置及び姿勢に対応する各アクチュエータの位置を目標位置として設定し、前記アクチュエータそれぞれの動作指令を設定する動作指令設定部と、
少なくとも前記動作指令と前記位置センサの検出結果とに基づいて、所定の演算周期毎に、前記アクチュエータのそれぞれに対してトルク指令を生成するサーボ部と
前記センサ部の検出結果に基づいて前記外力が生じたときに前記外力の方向を検出する外力検出部と、
前記外力検出部により検出された前記外力の方向に基づいて、前記１以上のアクチュエータのうち回避動作を行なうべきアクチュエータを選択する回避軸選択部と、
前記サーボ部により生成される前記トルク指令に自重分の重力補償トルクを付加する重力補償トルク付加部と、
前記回避軸選択部により選択された前記アクチュエータのそれぞれに対して前記サーボ部により生成される前記トルク指令に、前記アクチュエータ自身の摩擦力に抗して前記外力の方向に動作するための摩擦補償トルクを付加する摩擦補償トルク付加部と、を有している
ことを特徴とする、請求項１記載のロボットシステム。
【請求項３】
前記コントローラは、
前記外力検出部により外力が検出されると、前記サーボ部に与える前記動作指令を遮断する動作指令遮断部を有している
ことを特徴とする、請求項２記載のロボットシステム。
【請求項４】
前記コントローラは、
前記動作指令遮断部により前記動作指令が遮断されてから予め設定された期間以上前記外力検出部により外力が検出されないときは、前記動作指令遮断部による前記動作指令の遮断を解除する動作指令復帰部を有している
ことを特徴とする、請求項３記載のロボットシステム。
【請求項５】
前記コントローラは、前記アクチュエータへのトルク指令値を制限するトルク指令制限部を有し、
前記トルク指令制限部は、前記重力補償トルクと前記摩擦補償トルクとの和に基づいて前記トルク指令値の上限値及び下限値を設定する
ことを特徴とする、請求項２〜４のいずれか１項に記載のロボットシステム。
【請求項６】
前記センサ部は、
圧電体として水晶が用いられたセンサを複数含んで形成されている
ことを特徴とする、請求項２〜５のいずれか１項に記載のロボットシステム。
【請求項７】
前記センサ部は、
前記ロボットアームの内最も基端側のアクチュエータの基部に設けられた円板状のセンサ固定治具と、
前記センサ固定冶具に同一円弧に沿って埋設された複数の前記センサとを有している
ことを特徴とする、請求項６記載のロボットシステム。
【請求項８】
前記外力検出部は、
前記複数の前記センサからの信号の高周波振動成分を抽出し、抽出された前記高周波振動成分に基づいて、前記外力の有無を検出する
ことを特徴とする請求項７記載のロボットシステム。
【請求項９】
前記外力検出部は、
抽出された前記高周波振動成分のピーク値を各センサ毎に求め、前記各センサ毎の前記ピーク値の正負組み合わせに基づいて前記外力の方向を検出する
ことを特徴とする請求項８記載のロボットシステム。
【請求項１０】
前記回避軸選択部は、
前記外力検出部により求められた前記外力の方向ベクトルにヤコビ行列の転置行列を掛け算したベクトルの各成分を閾値処理して回避対象関節軸と方向を算出する
ことを特徴とする請求項９記載のロボットシステム。
【請求項１１】
前記摩擦補償トルク付加部は、
前記回避軸選択部により選択された前記アクチュエータが静止しているときは当該アクチュエータの最大静止摩擦トルクを前記摩擦補償トルクとして付加し、
当該アクチュエータが回転しているときは当該アクチュエータの動摩擦トルクを前記摩擦補償トルクとして付加する
ことを特徴とする請求項１０記載のロボットシステム。
【請求項１２】
前記外力検出部は、
抽出された各センサ毎の前記高周波振動成分をそれぞれ移動平均化処理して得た移動平均値に基づいて前記外力の有無を検出する
ことを特徴とする請求項８記載のロボットシステム。
【請求項１３】
前記外力検出部は、
前記外力が有ることを検出した時点以降における各センサ毎の前記高周波振動成分の積算値をそれぞれ求め、前記積算値の正負組み合わせに基づいて前記外力の方向を検出する
ことを特徴とする請求項８記載のロボットシステム。
【請求項１４】
前記コントローラは、
各センサ毎の前記積算値の正負の組み合わせ、または、各センサ毎の前記移動平均値の正負の組み合わせ、と前記外力の方向とを対応付けたデータテーブルを有し、
前記外力検出部は、
前記外力が有ることを検出した時点以降の複数の時点において、それぞれ、各センサ毎の前記積算値の正負の組み合わせ、または、各センサ毎の前記移動平均値の正負の組み合わせに基づいて前記外力の方向を検出する
ことを特徴とする、請求項１２又は１３記載のロボットシステム。
【請求項１５】
１以上のアクチュエータを有するロボットアームと、前記ロボットアームと前記アクチュエータとの少なくともいずれかにかかる外力を検出するセンサ部とを有するロボットの制御装置であって、
前記アクチュエータに対してトルク指令を送出するアクチュエータ制御部と、
前記センサ部からの入力結果に基づいて前記アクチュエータへ送出される前記トルク指令値を制限するトルク指令制限部と、を有している
ことを特徴とする、ロボット制御装置。
【請求項１】
ロボットアームと、
前記ロボットアームに設けられ前記ロボットアームを駆動させる１以上のアクチュエータと、
前記ロボットアームと前記アクチュエータとの少なくともいずれかにかかる外力を検出するセンサ部と、
前記アクチュエータの動作を制御するとともに、前記センサ部の検出結果に基づいて前記アクチュエータへのトルク指令値を制限するコントローラと、を有している
ことを特徴とする、ロボットシステム。
【請求項２】
前記ロボットアーム又はアクチュエータの内、前記コントローラにより位置及び姿勢が制御される点である制御点の位置及び姿勢を検知する位置センサを有し、
前記コントローラは、
前記制御点のとるべき位置及び姿勢に対応する各アクチュエータの位置を目標位置として設定し、前記アクチュエータそれぞれの動作指令を設定する動作指令設定部と、
少なくとも前記動作指令と前記位置センサの検出結果とに基づいて、所定の演算周期毎に、前記アクチュエータのそれぞれに対してトルク指令を生成するサーボ部と
前記センサ部の検出結果に基づいて前記外力が生じたときに前記外力の方向を検出する外力検出部と、
前記外力検出部により検出された前記外力の方向に基づいて、前記１以上のアクチュエータのうち回避動作を行なうべきアクチュエータを選択する回避軸選択部と、
前記サーボ部により生成される前記トルク指令に自重分の重力補償トルクを付加する重力補償トルク付加部と、
前記回避軸選択部により選択された前記アクチュエータのそれぞれに対して前記サーボ部により生成される前記トルク指令に、前記アクチュエータ自身の摩擦力に抗して前記外力の方向に動作するための摩擦補償トルクを付加する摩擦補償トルク付加部と、を有している
ことを特徴とする、請求項１記載のロボットシステム。
【請求項３】
前記コントローラは、
前記外力検出部により外力が検出されると、前記サーボ部に与える前記動作指令を遮断する動作指令遮断部を有している
ことを特徴とする、請求項２記載のロボットシステム。
【請求項４】
前記コントローラは、
前記動作指令遮断部により前記動作指令が遮断されてから予め設定された期間以上前記外力検出部により外力が検出されないときは、前記動作指令遮断部による前記動作指令の遮断を解除する動作指令復帰部を有している
ことを特徴とする、請求項３記載のロボットシステム。
【請求項５】
前記コントローラは、前記アクチュエータへのトルク指令値を制限するトルク指令制限部を有し、
前記トルク指令制限部は、前記重力補償トルクと前記摩擦補償トルクとの和に基づいて前記トルク指令値の上限値及び下限値を設定する
ことを特徴とする、請求項２〜４のいずれか１項に記載のロボットシステム。
【請求項６】
前記センサ部は、
圧電体として水晶が用いられたセンサを複数含んで形成されている
ことを特徴とする、請求項２〜５のいずれか１項に記載のロボットシステム。
【請求項７】
前記センサ部は、
前記ロボットアームの内最も基端側のアクチュエータの基部に設けられた円板状のセンサ固定治具と、
前記センサ固定冶具に同一円弧に沿って埋設された複数の前記センサとを有している
ことを特徴とする、請求項６記載のロボットシステム。
【請求項８】
前記外力検出部は、
前記複数の前記センサからの信号の高周波振動成分を抽出し、抽出された前記高周波振動成分に基づいて、前記外力の有無を検出する
ことを特徴とする請求項７記載のロボットシステム。
【請求項９】
前記外力検出部は、
抽出された前記高周波振動成分のピーク値を各センサ毎に求め、前記各センサ毎の前記ピーク値の正負組み合わせに基づいて前記外力の方向を検出する
ことを特徴とする請求項８記載のロボットシステム。
【請求項１０】
前記回避軸選択部は、
前記外力検出部により求められた前記外力の方向ベクトルにヤコビ行列の転置行列を掛け算したベクトルの各成分を閾値処理して回避対象関節軸と方向を算出する
ことを特徴とする請求項９記載のロボットシステム。
【請求項１１】
前記摩擦補償トルク付加部は、
前記回避軸選択部により選択された前記アクチュエータが静止しているときは当該アクチュエータの最大静止摩擦トルクを前記摩擦補償トルクとして付加し、
当該アクチュエータが回転しているときは当該アクチュエータの動摩擦トルクを前記摩擦補償トルクとして付加する
ことを特徴とする請求項１０記載のロボットシステム。
【請求項１２】
前記外力検出部は、
抽出された各センサ毎の前記高周波振動成分をそれぞれ移動平均化処理して得た移動平均値に基づいて前記外力の有無を検出する
ことを特徴とする請求項８記載のロボットシステム。
【請求項１３】
前記外力検出部は、
前記外力が有ることを検出した時点以降における各センサ毎の前記高周波振動成分の積算値をそれぞれ求め、前記積算値の正負組み合わせに基づいて前記外力の方向を検出する
ことを特徴とする請求項８記載のロボットシステム。
【請求項１４】
前記コントローラは、
各センサ毎の前記積算値の正負の組み合わせ、または、各センサ毎の前記移動平均値の正負の組み合わせ、と前記外力の方向とを対応付けたデータテーブルを有し、
前記外力検出部は、
前記外力が有ることを検出した時点以降の複数の時点において、それぞれ、各センサ毎の前記積算値の正負の組み合わせ、または、各センサ毎の前記移動平均値の正負の組み合わせに基づいて前記外力の方向を検出する
ことを特徴とする、請求項１２又は１３記載のロボットシステム。
【請求項１５】
１以上のアクチュエータを有するロボットアームと、前記ロボットアームと前記アクチュエータとの少なくともいずれかにかかる外力を検出するセンサ部とを有するロボットの制御装置であって、
前記アクチュエータに対してトルク指令を送出するアクチュエータ制御部と、
前記センサ部からの入力結果に基づいて前記アクチュエータへ送出される前記トルク指令値を制限するトルク指令制限部と、を有している
ことを特徴とする、ロボット制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【公開番号】特開２０１２−５１０４２（Ｐ２０１２−５１０４２Ａ）
【公開日】平成２４年３月１５日（２０１２．３．１５）
【国際特許分類】
【出願番号】特願２０１０−１９３７１７（Ｐ２０１０−１９３７１７）
【出願日】平成２２年８月３１日（２０１０．８．３１）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
【公開日】平成２４年３月１５日（２０１２．３．１５）
【国際特許分類】
【出願日】平成２２年８月３１日（２０１０．８．３１）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
[ Back to top ]