JP2021016924A - ロボット制御システム、装置、方法及びプログラム - Google Patents

Abstract

【課題】自己衝突の危険を低減しつつも、運用コストが抑制されたロボット制御システム等を提供すること。【解決手段】一のリンクを包含する第１の空間領域が他のいずれかのリンク又はリンクに関する第１の空間領域に干渉するか否かを判定する第１干渉判定部と、干渉有りと判定される場合にはロボットの動作制御部へとフェールセーフ信号を提供するフェールセーフ信号提供部と、一のリンクを包含しかつ対応する第１の空間領域と一致するか又は包含する第２の空間領域が他のいずれかのリンク、リンクに関する第１の空間領域又は第２の空間領域に干渉するか否かを判定する第２干渉判定部と、干渉無しと判定される場合にはユーザ姿勢に対応するスレーブロボットの各関節における目標関節角度を動作制御部へと提供し干渉有りと判定される場合には目標関節角度を動作制御部へと提供しない目標関節角度提供部とを備えたロボット制御システムが提供される。【選択図】図１３

Description

この発明は、例えば、ロボットの制御システム等に関する。

近年、人間の姿勢や動作をセンサシステムにより検出し、その検出結果に基づいてロボットを遠隔地等から制御するマスタ・スレーブ制御システムについて研究開発がなされている（例えば、特許文献１）。

ところで、この種のシステムに利用されるロボットには、ロボット自身を構成する部材間での衝突や干渉が生じてロボットが破損や故障等することを防止する目的で、自己衝突防止機構が備えられている。

従来知られている自己衝突防止機構は、ロボットを構成する一の部材が、他の部材を包含する所定の空間領域へと干渉するか否かを監視し、それが干渉すると判定される場合には、ロボットの保護のため、フェールセーフ処理、すなわち、モータ電源をオフにしたり物理的にブレーキを駆動させる等してロボットの動作を停止させる処理を行っていた。

図１８は、干渉判定に用いられる空間領域、すなわち従前の干渉禁止領域の概念図である。同図においては、ロボットの頭部、胴部、腕部等をおよそ包含するように干渉判定に用いられる空間領域が設定されている。これらの領域に他の部材が干渉するとロボットはフェールセーフ処理により停止する。

特許第６４７６３５８号公報

しかしながら、従来の自己衝突防止機構においては、追従対象となる動作が多様である場合や干渉判定に用いられる空間領域の設定が適切でない場合等には、自己衝突防止機構によるフェールセーフ処理が比較的容易に作動し、その処理発生頻度が高くなってしまうことがあった。

一旦フェールセーフ処理が行われると、所定の再起動処理を行わなければならず、復旧作業には工数を要し、特に、マスタ側のロボットのユーザが遠隔地に存在している場合等にはその工数は看過できないものであった。すなわち、ロボットの自己衝突防止機構により、マスタ・スレーブ制御システムの運用コストの増大がもたらされることがあった。

本発明は、上述の技術的課題を解決するためになされたものであり、その目的とするところは、マスタ・スレーブ制御システムにおけるロボットの自己衝突の危険を低減しつつも、運用コストが抑制されたロボット制御システム等を提供することにある。

上述の技術的課題は、以下の構成を有するロボット制御システム等により解決することができる。

すなわち、本発明に係るロボット制御システムは、マスタ装置を介して取得されたユーザ姿勢に追従し、かつ、１又は複数の関節と複数のリンクを備えたスレーブロボットを制御するロボット制御システムであって、前記スレーブロボットの前記リンクのうちの一のリンクを包含するように構成された第１の空間領域が、他のいずれかのリンク又は当該リンクに関する第１の空間領域に干渉するか否かを判定する、第１干渉判定部と、前記第１干渉判定部において干渉有りと判定される場合には、前記スレーブロボットの動作制御部へとフェールセーフ信号を提供する、フェールセーフ信号提供部と、前記リンクのうちの一のリンクを包含しかつ対応する第１の空間領域と一致するか又は包含するように構成された第２の空間領域が、他のいずれかのリンク、当該リンクに関する第１の空間領域又は第２の空間領域に干渉するか否かを判定する、第２干渉判定部と、前記第２干渉判定部において干渉無しと判定される場合には、前記ユーザ姿勢に対応する前記スレーブロボットの各前記関節における目標関節角度を前記スレーブロボットの前記動作制御部へと提供し、前記第２干渉判定部において干渉有りと判定される場合には、前記目標関節角度を前記スレーブロボットの前記動作制御部へと提供しない、目標関節角度提供部と、を備えている。

このような構成によれば、フェールセーフ処理が行われる基準となる第１の空間領域と一致するか又は包含する第２の空間領域を用いて判定を行うので、スレーブロボットにおける自己衝突の危険を低減させることができると共にフェールセーフ処理が行われる頻度を低減させることができる。すなわち、マスタ・スレーブシステムにおけるロボットの自己衝突の危険を低減しつつも、運用コストが抑制されたロボット制御システムを提供することができる。

なお、本ロボット制御システムの第１干渉判定部、フェールセーフ信号提供部等の各構成は、同一の装置内に配置されることを要しない。

前記第２干渉判定部において干渉判定される、前記一のリンクに係る前記第２の空間領域、又は、前記他のいずれかのリンク、当該リンクに関する第１の空間領域若しくは第２の空間領域のいずれか一方は、目標関節角度に基づいて算出され、他方は、前記スレーブロボットの各前記関節における現在関節角度に基づいて算出される、ものであってもよい。

このような構成によれば、目標関節角度を用いるので、未然にスレーブロボットにおける自己衝突や意図しないフェールセーフ処理の適用を防止することができる。また、現在関節角度と目標関節角度とを併用することから、ユーザの動きにスレーブロボットが十分に追従できない場合であっても、違和感なく自己衝突の危険を低減させることができると共に、フェールセーフ処理が行われる頻度を低減させることができる。

前記第２干渉判定部において干渉判定される、前記一のリンクに係る前記第２の空間領域、又は、前記他のいずれかのリンク、当該リンクに関する第１の空間領域若しくは第２の空間領域のいずれも、前記目標関節角度に基づいて算出される、ものであってもよい。

このような構成によれば、目標関節角度を用いるので、未然にスレーブロボットにおける自己衝突や意図しないフェールセーフ処理の適用を防止することができる。また、目標関節角度のみを利用することから比較的に高速な判定処理を行うことができる。

前記ロボット制御システムは、さらに、現在の前記関節角度から前記目標関節角度へと至る過程の一又は複数の中間姿勢における中間姿勢関節角度を生成する、中間関節角度生成部を備え、前記第２干渉判定部は、さらに、前記中間姿勢関節角度のそれぞれに基づいて、前記リンクのうちの一のリンクに関する第２の空間領域が、他のいずれかのリンク、当該リンクに関する第１の空間領域又は第２の空間領域に干渉するか否かを判定する、中間姿勢判定部と、を備えるものであってもよい。

このような構成によれば、目標姿勢へと至る中間姿勢においても自己干渉の有無を判定するので、より確実に、スレーブロボットにおける自己衝突の危険を低減させることができると共にフェールセーフ処理が行われる頻度を低減させることができる。

前記第１干渉判定部における前記判定は、前記リンク又は前記リンクに関する第１の空間領域に対応する３次元モデル同士の干渉判定を行うことにより行われる、ものであってもよい。

前記第１干渉判定部は、さらに、判定対象となる前記リンク又は前記リンクに関する第１の空間領域に対応する境界ボリューム同士の干渉を判定する、第１境界ボリューム判定部と、前記第１境界ボリューム判定部において干渉有りと判断された場合に、前記リンク又は前記リンクに関する第１の空間領域の表面間の干渉を判定する、第１表面干渉判定部と、を備えるものであってもよい。

このような構成によれば、境界ボリュームを利用した大まかな判定処理により干渉しないと判定された場合には、それ以上の干渉判定を行わないので、処理の高速化や処理負荷の低減等を行うことができる。

前記第２干渉判定部における前記判定は、前記リンク、前記リンクに関する第１の空間領域又は第２の空間領域に対応する３次元モデル同士の干渉判定を行うことにより行われる、ものであってもよい。

前記第２干渉判定部は、判定対象となる前記リンク、前記リンクに関する第１の空間領域又は第２の空間領域に対応する境界ボリューム同士の干渉を判定する、第２境界ボリューム判定部と、前記第２境界ボリューム判定部において干渉有りと判断された場合に、前記リンク、前記リンクに関する第１の空間領域又は前記第２の空間領域の表面間の干渉を判定する、第２表面干渉判定部と、を備えるものであってもよい。

このような構成によれば、境界ボリュームを利用した大まかな判定処理により干渉しないと判定された場合には、それ以上の干渉判定を行わないので、処理の高速化や処理負荷の低減等を行うことができる。

前記第１境界ボリューム判定部又は前記第２境界ボリューム判定部における判定は、ＡＡＢＢ法（Ａｘｉｓ−Ａｌｉｇｎｅｄ Ｂｏｕｎｄｉｎｇ Ｂｏｘ法）により行われる、ものであってもよい。

前記ロボット制御システムは、さらに、前記ユーザ姿勢に関するセンサ情報を前記スレーブロボットの関節角度へと変換するための変換情報を記憶する変換情報記憶部と、前記変換情報と逆運動学に基づいて、前記ユーザ姿勢を、対応する前記スレーブロボットの各前記関節における目標関節角度へと変換する、変換部と、を備えるものであってもよい。

前記第１干渉判定部及び前記第２干渉判定部は、隣り合うリンクの間では干渉判定を行わない、ものであってもよい。

このような構成によれば、隣り合うリンクの間では干渉判定を行わないため、過度な干渉判定の防止や、処理の高速化、処理負荷の低減等を行うことができる。

本発明は、装置として観念することもできる。すなわち、本発明に係るロボット制御装置は、マスタ装置を介して取得されたユーザ姿勢に追従し、かつ、１又は複数の関節と複数のリンクを備えたスレーブロボットを制御するロボット制御装置であって、前記スレーブロボットの前記リンクのうちの一のリンクを包含するように構成された第１の空間領域が、他のいずれかのリンク又は当該リンクに関する第１の空間領域に干渉するか否かを判定する、第１干渉判定部と、前記第１干渉判定部において干渉有りと判定される場合には、前記スレーブロボットの動作制御部へとフェールセーフ信号を提供する、フェールセーフ信号提供部と、前記リンクのうちの一のリンクを包含しかつ対応する第１の空間領域と一致するか又は包含するように構成された第２の空間領域が、他のいずれかのリンク、当該リンクに関する第１の空間領域又は第２の空間領域に干渉するか否かを判定する、第２干渉判定部と、前記第２干渉判定部において干渉無しと判定される場合には、前記ユーザ姿勢に対応する前記スレーブロボットの各前記関節における目標関節角度を前記スレーブロボットの前記動作制御部へと提供し、前記第２干渉判定部において干渉有りと判定される場合には、前記目標関節角度を前記スレーブロボットの前記動作制御部へと提供しない、目標関節角度提供部と、を備えている。

本発明は、方法として観念することもできる。すなわち、本発明に係るロボット制御方法は、マスタ装置を介して取得されたユーザ姿勢に追従し、かつ、１又は複数の関節と複数のリンクを備えたスレーブロボットを制御するロボット制御方法であって、前記スレーブロボットの前記リンクのうちの一のリンクを包含するように構成された第１の空間領域が、他のいずれかのリンク又は当該リンクに関する第１の空間領域に干渉するか否かを判定する、第１干渉判定ステップと、前記第１干渉判定部において干渉有りと判定される場合には、前記スレーブロボットの動作制御部へとフェールセーフ信号を提供する、フェールセーフ信号提供ステップと、前記リンクのうちの一のリンクを包含しかつ対応する第１の空間領域と一致するか又は包含するように構成された第２の空間領域が、他のいずれかのリンク、当該リンクに関する第１の空間領域又は第２の空間領域に干渉するか否かを判定する、第２干渉判定ステップと、前記第２干渉判定部において干渉無しと判定される場合には、前記ユーザ姿勢に対応する前記スレーブロボットの各前記関節における目標関節角度を前記スレーブロボットの前記動作制御部へと提供し、前記第２干渉判定部において干渉有りと判定される場合には、前記目標関節角度を前記スレーブロボットの前記動作制御部へと提供しない、目標関節角度提供ステップと、を備えている。

本発明は、コンピュータプログラムとして観念することもできる。すなわち、本発明に係るロボット制御プログラムは、マスタ装置を介して取得されたユーザ姿勢に追従し、かつ、１又は複数の関節と複数のリンクを備えたスレーブロボットを制御するロボット制御プログラムであって、前記スレーブロボットの前記リンクのうちの一のリンクを包含するように構成された第１の空間領域が、他のいずれかのリンク又は当該リンクに関する第１の空間領域に干渉するか否かを判定する、第１干渉判定ステップと、前記第１干渉判定部において干渉有りと判定される場合には、前記スレーブロボットの動作制御部へとフェールセーフ信号を提供する、フェールセーフ信号提供ステップと、前記リンクのうちの一のリンクを包含しかつ対応する第１の空間領域と一致するか又は包含するように構成された第２の空間領域が、他のいずれかのリンク、当該リンクに関する第１の空間領域又は第２の空間領域に干渉するか否かを判定する、第２干渉判定ステップと、前記第２干渉判定部において干渉無しと判定される場合には、前記ユーザ姿勢に対応する前記スレーブロボットの各前記関節における目標関節角度を前記スレーブロボットの前記動作制御部へと提供し、前記第２干渉判定部において干渉有りと判定される場合には、前記目標関節角度を前記スレーブロボットの前記動作制御部へと提供しない、目標関節角度提供ステップと、を備えている。

本発明によれば、マスタ・スレーブ制御システムにおけるロボットの自己衝突の危険を低減しつつも、運用コストが抑制されたロボット制御システムを提供することができる。

図１は、マスタ・スレーブ制御システムの全体構成図である。 図２は、モーションセンサに関する説明図である。 図３は、ロボットに関する説明図である。 図４は、マスタ・スレーブ制御システムの機能ブロック図である。 図５は、マスタ側情報処理装置の動作に関するフローチャートである。 図６は、関節角度監視部の動作に関するフローチャートである。 図７は、自己干渉判定処理の詳細フローチャート（その１）である。 図８は、第１の干渉禁止領域の算出処理に関する詳細フローチャートである。 図９は、リンク間の干渉判定処理の詳細フローチャート（その１）である。 図１０は、スレーブ側情報処理装置の動作に関するフローチャートである。 図１１は、マッピング処理の詳細フローチャートである。 図１２は、自己干渉判定処理の詳細フローチャート（その２）である。 図１３は、干渉禁止領域に関する説明図である。 図１４は、第２の干渉禁止領域の算出処理に関する詳細フローチャートである。 図１５は、リンク間の干渉判定処理の詳細フローチャート（その２）である。 図１６は、スレーブ側情報処理装置の動作に関するフローチャートである（第２の実施形態）。 図１７は、自己干渉判定処理の詳細フローチャートである（第２の実施形態）。 図１８は、従前の干渉禁止領域の概念図である。 図１９は、マスタ・スレーブ制御システムの機能ブロック図（第３の実施形態）である。

以下、本発明の好適な実施の形態について添付の図１〜図１９を参照しつつ詳細に説明する。

（１．第１の実施形態）
（１．１ マスタ・スレーブ制御システムの構成）
図１〜図４を参照しつつ、本実施形態に係るマスタ・スレーブ制御システム１００の構成について説明する。

図１は、マスタシステム１と、ロボット８を含むスレーブシステム４とをインターネット等のネットワークを介して接続して成るマスタ・スレーブ制御システム１００の全体構成図である。

同図から明らかな通り、マスタシステム１は、モーションセンサ２と、マスタ側情報処理装置３とから構成される。モーションセンサ２は、図示しないユーザの身体の姿勢や動きを検出する。マスタ側情報処理装置３は、モーションセンサ２から得られるセンサ情報を処理してユーザの姿勢等を算出したり、スレーブシステム４との間の情報の授受等を行う。

スレーブシステム４は、スレーブ側情報処理装置５と、ロボットのコントローラユニット７と、ロボット８とから構成される。本実施形態において、ロボット８は、後述するように、頭部８１、胴部８２及び一対の腕部８５（図３参照）とから成り、モータ等が内部に配置される関節と、関節間をつなぐリンクとから構成される。スレーブ側情報処理装置５は、ネットワークを介して受信したユーザ姿勢情報に基づいてユーザ姿勢をロボットの関節角度へとマッピングしたり、後述の干渉判定処理等を行う。コントローラユニット７は、スレーブ側情報処理装置からの目標関節角度等に基づいてロボット８の関節に備えられたモータへの電流を制御しロボット８の位置制御を行う。

図示しないものの、マスタ側情報処理装置３及びスレーブ側情報処理装置５は、いずれも、制御部、記憶部、通信部、Ｉ／Ｏ部、入力部、表示部等がバスを介して接続された構成を有する情報処理装置である。制御部は、例えば、ＣＰＵ及びＧＰＵであり、後述の各種のプログラムの実行、制御を行う。記憶部は、ＲＯＭ、ＲＡＭ、フラッシュメモリー、ハードディスク、ストレージ等で構成され、適宜、データやプログラムを記憶する。通信部は、無線通信ユニット又は有線通信ユニット等で構成され、所定の規格の下に外部機器との通信処理を行う。入力部は、例えば、キーボード等を介して入力された情報を検出及び処理する。表示部は、ディスプレイ等への表示制御を行う。

なお、システム又はシステムを構成する装置の構成は本実施形態のものに限定されない。従って、例えば、スレーブ側情報処理装置５とコントローラユニット７とを統合する等、所定の機能や装置について統合したり、或いは、分割したりしてもよい。また、マスタシステム１とスレーブシステム４との間は、インターネット等を介さずにローカルエリアネットワーク等により接続してもよい。さらに、マスタシステム１とスレーブシステム４との間をネットワークにより接続せずに単に有線接続してもよい。加えて、ユーザは、ロボット８を遠隔地から操作してもよいし、同一の空間内に配置してもよい。

図２は、モーションセンサ２に関する説明図である。本実施形態に係るモーションセンサ２は、加速度センサ、角速度センサ及び地磁気センサを組み合わせて３軸の加速度値、角速度値及び地磁気値を検出可能なＩＭＵセンサ（慣性計測センサ）として構成し、ＩＭＵセンサをベルトにより身体の各部に装着するベルト装着型センサである。なお、ＩＭＵセンサの構成は本実施形態に係るものに限定されない。

図２（ａ）は、ユーザによるモーションセンサ２の装着例について示している。同図から明らかな通り、本実施形態においては、ユーザの額に頭部用ＩＭＵセンサ２１、左右の上腕にそれぞれ右上腕用ＩＭＵセンサ２３Ｒ、左上腕用ＩＭＵセンサ２３Ｌ、左右の前腕にそれぞれ右前腕用ＩＭＵセンサ２４Ｒ、左前腕用ＩＭＵセンサ２４Ｌ、手の甲にそれぞれ右甲用ＩＭＵセンサ２５Ｒ、左甲用ＩＭＵセンサ２５Ｌが設けられている。また、ユーザの背中側の中央には、背面用ＩＭＵセンサ２２（不図示）が設けられている。

図２（ｂ）は、各センサの配置に関する正面図である。同図において、ユーザの身体は直線で描かれ簡略表示されている。同図から明らかな通り、ユーザには、頭部に１つのＩＭＵセンサ２１、胴部（背面側）に１つのＩＭＵセンサ２２、左右対称にそれぞれ３つずつのＩＭＵセンサ２３Ｒ〜２５Ｒ、２３Ｌ〜２５Ｌが設けられることとなる。

なお、本実施形態においては、ユーザ姿勢の検出手段としてベルト装着型のＩＭＵセンサを採用したが、本発明はこのような構成に限定されない。従って、他の装着型センサを用いてもよいし、装着を行わずにカメラ等を用いてユーザ姿勢を検出してもよい。

図３は、ロボット８に関する説明図である。図３（ａ）は、ロボット８に備えられたアクチュエータであるサーボモータ８１１〜８５７の配置斜視図であり回転駆動箇所が矢印で示されている。図３（ｂ）は、アクチュエータ配置の正面図であり、ロボット８の身体は直線で描かれ簡略表示されている。

図３（ａ）及び図３（ｂ）から明らかな通り、ロボット８は、略人型の形状を有しており、概して、頭部８１と、頭部８１から下方へと延びる胴部８２と、胴部８２の左右側面から延びる腕部８５Ｌ、８５Ｒとから構成されている。各関節にはそれぞれ回転駆動されるサーボモータが配置され、サーボモータ間は骨格を形成するリンクが設けられている。

頭部８１は、胴部８２と平行な水平回転軸を有し上下を仰ぎ視る動作を可能とする頭部第１関節駆動モータ８１１、鉛直方向の回転軸を有し首の左右への回転を可能とする頭部第２関節駆動モータ８１２とを備えている。

胴部８２は、胴部８２と平行な水平回転軸を有しロボット８が屈む動作を可能とする胴部第１関節駆動モータ８２１と、鉛直方向の回転軸を有し胴部８２を捻るような回転を可能とする胴部第２関節駆動モータ８２２とを備えている。

腕部８５Ｌ、８５Ｒは、胴部８２と平行な水平回転軸を有し腕部８５Ｌ、８５Ｒを前後に振る動作を可能とする腕部第１関節駆動モータ８５１Ｌ、８５１Ｒと、胴部８２と垂直な水平回転軸を有し腕部８５Ｌ、８５Ｒを左右に開閉する動作を可能とする腕部第２関節駆動モータ８５２Ｌ、８５２Ｒとを備えている。

また、腕部８５Ｌ、８５Ｒは、鉛直方向の回転軸を有し上腕部において腕を捻るような動作を可能とする腕部第３関節駆動モータ８５３Ｌ、８５３Ｒと、胴部８２と垂直な水平回転軸を有し肘の曲げ動作を可能とする腕部第４関節駆動モータ８５４Ｌ、８５４Ｒと、鉛直方向の回転軸を有し前腕部を捻るような動作を可能とする腕部第５関節駆動モータ８５５Ｌ、８５５Ｒとを備えている。

さらに、腕部８５Ｌ、８５Ｒは、胴部８２と垂直な水平回転軸を有し手首の曲げ動作を可能とする腕部第６関節駆動モータ８５６Ｌ、８５６Ｒと、鉛直方向の回転軸を有し手先部を捻るような動作を可能とする腕部第７関節駆動モータ８５７Ｌ、８５７Ｒとを備えている。

なお、同図においては、モータの配置のみを例示したが、ロボット８には、他に、モータ制御のための各種基板やバッテリ等が別途備えられていてもよいことは勿論である。また、頭部８１にカメラを付加する等、さらなる機能の付加のための構成を追加してもよい。

図４は、本実施形態に係るマスタ・スレーブ制御システム１００の機能ブロック図である。同図から明らかな通り、同図においては、マスタ・スレーブ制御システム１００のうち、特に、マスタ側情報処理装置３と、スレーブ側情報処理装置５と、ロボットコントローラ７の機能ブロックについて示している。

同図から明らかな通り、マスタ側情報処理装置３は、モーションセンサ２からのセンサ信号を受信してユーザ姿勢を検出するユーザ姿勢検出部３１と、検出されたユーザ姿勢３１をスレーブ側情報処理装置５へとネットワーク等を介して送信するユーザ姿勢送信部３３を備えている。また、ロボット８のセンサで検出される各種の情報、例えば、ロボット８の現在関節角度等をロボットの状態信号として受信するロボット状態信号受信部３７と、ロボットの状態信号を表示部や外部表示装置等に表示する制御を行う状態表示制御部３５を備えている。

また、スレーブ側情報処理装置５は、ユーザ姿勢を受信しマッピング処理部５２へと提供するユーザ姿勢受信部５１を備えている。マッピング処理部５２は、後述する手法にてユーザ姿勢をロボット８の関節角度へとマッピングして目標関節角度を生成し、指令値提供判定処理部５３へと提供する処理を行う。目標関節角度を受信した指令値提供判定処理部５３は、後述するように、第２の空間領域に基づく自己干渉の有無に関する所定の条件判定処理を行う。その結果、自己干渉が無いと判定される場合には、コントローラユニット７との間の通信処理を行う通信部５４を介してコントローラユニット７へと目標関節角度の提供を行い、他方、自己干渉が有ると判定される場合には、コントローラユニット７へと目標関節角度を提供しない処理を行う。

また、スレーブ側情報処理装置５は、ロボット８の状態を監視してマスタ側情報処理装置３へと報告する機能を有している。スレーブ側情報処理装置５は、コントローラユニット７を介してロボット８のセンサから取得された現在の関節角度等の状態信号を取得するコントローラユニット通信部５４を有する。コントローラユニット通信部５４にて取得されたロボット８の現在状態に関する状態信号は、ロボット状態信号送信部５７へと提供される。ロボット状態信号送信部５７は、受信した状態信号をマスタ側情報処理装置３へと送信する。なお、この状態信号により、マスタ側での各種表示処理が行われる。

さらに、スレーブ側情報処理装置５は、ロボット８の関節角度を監視して所定の条件を満たす場合に、フェールセーフ指令を行う。スレーブ側情報処理装置５は、コントローラユニット７を介してロボット８のセンサから取得された現在の関節角度等の状態信号をコントローラユニット通信部５４を介して取得する関節角度監視部５６を有する。関節角度監視部５６は、後述する処理により、ロボット８の現在関節角度を監視し、第１の空間領域に基づく自己干渉の有無について所定の条件判定を行う。その結果、関節角度監視部５６は、自己干渉がないと判定される場合には、何らの処理も行うことなく再度監視処理を行い、自己干渉が有ると判定される場合には、サーボモータへの電流供給を停止したり物理的なブレーキをかけるための信号を生成する等して、フェールセーフ指令を行う。

ロボットコントローラ７は、通信部７１を介して、スレーブ側情報処理装置５から目標関節角度を取得し、ロボット制御部７２にて電流値等の指令値を生成し、ロボット８の関節に備えられた各モータへと提供する。また、ロボットコントローラ７は、ロボット８に備えられた関節角度等の各種センサ情報をロボット制御部７２を用いて取り込み、スレーブ側情報処理装置５へと送信する。

（１．２ マスタ・スレーブ制御システムの動作）
次に、図５〜図１８を参照しつつ、本実施形態に係るマスタ・スレーブ制御システム１００の動作について、以下、詳細に説明する。

（１．２．１ マスタ側情報処理装置３の動作）
図５は、マスタ側情報処理装置３の動作に関するフローチャートである。同図から明らかな通り、処理が開始すると、ユーザ姿勢検出部３１は、モーションセンサ２であるＩＭＵセンサ（２１〜２５Ｌ、２５Ｒ）において検出されたセンサ情報の取得処理を行う（Ｓ１１）。ここで、センサ情報は、上述のＩＭＵセンサのセンサ値であり、具体的には、加速度センサ、角速度センサ及び地磁気センサの３軸センサ値である。

センサ情報の取得処理が完了すると、ユーザ姿勢検出部３１は、センサ情報に基づいてユーザ姿勢情報を生成する処理、すなわちユーザ姿勢の検出処理を行う（Ｓ１２）。このユーザ姿勢情報を生成する手法として、公知の種々の手法を採用し得るが、本実施形態においては、加速度センサと角速度センサのデータに基づいてカルマンフィルタを用いて自己位置推定を行いユーザ姿勢データとする手法が採用される。なお、このとき、地磁気センサの値は、加速度センサと角速度センサのドリフトを防止するために用いられる。

その後、ユーザ姿勢送信部３３は、生成されたユーザ姿勢情報をスレーブ側情報処理装置５へと送信する処理を行う（Ｓ１３）。その後、これら一連の処理（Ｓ１１〜Ｓ１３）が繰り返される。

（１．２．２ スレーブ側情報処理装置の動作（第１の干渉禁止領域））
図６は、スレーブ側情報処理装置５の主に関節角度監視部５６の動作に関するフローチャートである。同図から明らかな通り、処理が開始すると、関節角度監視部５６は、コントローラユニット７からの送信情報、特に、ロボット８に備えられたセンサを介して得られたロボット８の現在の関節角度情報を取得する（Ｓ９１）。なお、関節角度情報の取得周期は、本実施形態においては、１００Ｈｚである。関節角度情報を取得した後、当該関節角度情報が第１の干渉禁止領域と干渉するか否かの判定処理が行われる（Ｓ９２）。

図７は、自己干渉判定処理（Ｓ９２）の詳細フローチャートである。同図から明らかな通り、処理が開始すると、自己干渉判定に必要なロボット機構情報の読出処理が行われる（Ｓ９２１）。ロボット機構情報とは、ロボット８の関節を一連に又は分岐を伴ってリンクでつないで構成されるグラフ構造や、各関節やリンクのハードウェア構造に関する情報（各リンクの寸法、各関節の回転中心の方向ベクトル、等）を含むものである。

次に、第１の干渉禁止領域の算出処理が行われる（Ｓ９２３）。ここで、第１の干渉禁止領域は、図１８に示したものと同様であり、後述するように、第２の干渉禁止領域に包含されるものである。

図８は、第１の干渉禁止領域の算出処理（Ｓ５３）に関する詳細フローチャートである。同図から明らかな通り、処理が開始すると、関節角度監視部５６は、ロボット機構情報に基づいて最上位リンク（ルートリンクと呼ぶ）（Ｍ＝０）を特定し、当該ルートリンクの一つ下の子リンク（Ｍ＝１）を参照リンクとして設定する処理を行う（Ｓ９２３２）。

設定処理が完了すると、関節角度監視部５６は、親リンク（Ｍ＝０）の座標系原点に対して同次変換行列を掛け合わせ、参照リンク（Ｍ＝１）の座標系原点の位置・姿勢を算出する処理を行う（Ｓ９２３３）。その後、当該参照リンクの回転中心の方向ベクトルと、目標関節角度から算出された回転角度に基づいて、参照リンクの座標系原点における姿勢を更新する（Ｓ９２３４）。

参照リンクにおける座標系原点での姿勢の更新が完了すると、参照リンクに対して、３次元的に第１の干渉禁止領域を付与する処理、すなわち、リンク表面から所定距離離れた位置に配置されるポリゴンメッシュを構成する三角形群の頂点座標を更新する処理が行われる（Ｓ９２３５）。なお、ポリゴンメッシュの形状は三角形に限定されず、例えば、四角形や凹凸のある形状であってもよい。また、３次元構造を表現する他の公知の手法を採用してもよい。

その後、Ｍが所定値であるか否かを判定する処理が行われる。Ｍが所定値に満たない場合（Ｓ９２３６ＮＯ）、Ｍを１だけインクリメント（Ｓ９２３８）して次のリンクを参照し、再度一連の処理（Ｓ９２３３〜Ｓ９２３６）を繰り返す。一方、Ｍが所定となった場合（Ｓ９２３６ＹＥＳ）、処理は終了する。すなわち、現在関節角度における第１の干渉禁止領域に相当するポリゴンメッシュが設定されることとなる。

図７に戻り、第１の干渉禁止領域の算出処理（Ｓ９２３）が行われると、算出された第１の干渉禁止領域に基づいて、リンク間の干渉判定処理が行われる（Ｓ９２５）。

図９は、リンク間の干渉判定処理の詳細フローチャートである。同図から明らかな通り、処理が開始すると、参照リンクに干渉があるか否かを示すすべての干渉フラグを、干渉がないことを示すＯＦＦ状態へとすべて初期化する処理が行われる（Ｓ９２５１）。その後、ルートリンクを参照リンクとして設定する処理が行われる（Ｓ９２５２）。

参照リンクが設定されると、当該参照リンクと他のリンクとの間において、境界ボリュームに基づく干渉判定処理が行われる（Ｓ９２５３）。境界ボリュームとは、対象３次元体表面を構成するポリゴンの各頂点をすべて含むように構成される所定の幾何学的形状を有する３次元オブジェクトを言う。所定形状には、例えば、球（Ｓｐｅｈｅｒｅ）形状、軸に平行な箱型（Ａｘｉｓ−ａｌｉｇｈｅｄ ｂｏｕｎｄｉｎｇ ｂｏｘ：ＡＡＢＢ）形状、所定の方向性を有する箱型（Ｏｒｉｅｎｔｅｄ ｂｏｕｎｄｉｎｇ ｂｏｘ：ＯＢＢ）形状、及び凸包（Ｃｏｎｖｅｘ ｈｕｌｌ）形状等が含まれるが、本実施形態においては、軸に平行な箱型の境界ボリュームを利用して干渉判定を行う（ＡＡＢＢ法）。

すなわち、参照リンクに関する第１の干渉禁止領域を含み所定軸と平行な箱型形状と、他のリンクに関する第１の干渉禁止領域を含み所定軸と平行な箱型形状との間における干渉判定処理が行われる。境界ボリュームを利用することで、対象３次元体を含むシンプルな形状同士の比較が行われるので、大まかな干渉判定を行うことができる。

この干渉判定処理（Ｓ９２５３）の結果、両者の間に干渉無しと判定された場合には（Ｓ９２５４ＮＯ）、未だ干渉判定を行っていない他のリンクが存在するかの判定処理が行われる（Ｓ９２５８）。一方、干渉判定処理（Ｓ９２５３）の結果、両者の間に干渉有りと判定された場合には（Ｓ９２５４ＹＥＳ）、第１の干渉禁止領域表面を構成するポリゴンメッシュに基づく干渉判定処理が行われる（Ｓ９２５５）。すなわち、実際のリンク形状に即して設定された干渉禁止領域の表面間において詳細に干渉判定処理が行われる。

この干渉判定処理（Ｓ９２５５）の結果、ポリゴンメッシュ間において干渉無しと判定された場合には（Ｓ９２５６ＮＯ）、未だ干渉判定を行っていない他のリンクが存在するかの判定処理が行われる（Ｓ９２５８）。一方、干渉判定処理（Ｓ９２５５）の結果、両者の間に干渉有りと判定された場合には（Ｓ９２５６ＹＥＳ）、当該参照リンク及び当該参照リンクと干渉するリンクについて干渉フラグをＯＮにする（Ｓ９２５７）。

以上の処理（Ｓ９２５３〜Ｓ９２５７）が終了すると、未だ干渉判定を行っていない他のリンクが存在するか否かについて判定処理が行われ（Ｓ９２５８）、未だすべての干渉判定を行っていないと判定された場合には、参照リンクと比較されるリンクを変更する処理（Ｓ９２６０）が行われ、再度、一連の処理（Ｓ９２５３〜Ｓ９２５７）が行われる。

一方、参照リンクが他のすべてのリンクと干渉判定を行ったものと判定されると（Ｓ９２５８ＹＥＳ）、Ｍが所定値となったか、すなわち、すべてのリンクを参照リンクとしたか否かが判定される（Ｓ９２５９）。未だすべてのリンクを参照リンクとしていない場合（Ｓ９２５９ＮＯ）、Ｍを１だけインクリメントして（Ｓ９２６１）、再度一連の処理（Ｓ９２５３〜Ｓ９２５７）を行う。すべてのリンクを参照リンクとしたと判定された場合（Ｓ９２５９ＹＥＳ）、処理は終了する。

このような構成によれば、境界ボリュームを利用した大まかな判定処理により干渉しないと判定された場合には、それ以上の干渉判定を行わないので、処理の高速化や処理負荷の低減等を行うことができる。

なお、本実施形態においては、すべてのリンク（又はその干渉禁止領域含む）間で干渉判定を行う構成について説明を行ったが、本発明はこのような構成に限定されない。従って、例えば、一部のリンクについては干渉判定の対象から除外してもよい。例えば、隣り合うリンク間での判定処理を行わないように構成してもよい。このような構成によれば、元々干渉が生じやすい隣り合うリンク間で発生した干渉に基づく無用なフェールセーフ処理の発生等を防止することができる。

図６に戻り、現在関節角度の第１の自己干渉禁止領域との干渉判定処理（Ｓ９２）が完了すると、干渉フラグがＯＮとなっているリンクがあるか否かの判定、すなわち、干渉の有無に関する判定処理が行われ（Ｓ９３）、干渉無しと判定される場合（Ｓ９３ＮＯ）、再度一連の処理が行われる（Ｓ９１〜Ｓ９３）。一方、干渉有りと判定される場合、フェールセーフ指令が生成されてコントローラユニット７へと提供され（Ｓ９４）、その後、処理は終了する。

コントローラユニット７は、提供されたフェールセーフ指令に基づいてロボット８に対してフェールセーフ処理を行う。フェールセーフ処理とは、ロボット８を停止させる処理であり、例えば、モータのサーボ電源をＯＦＦ（電流の供給を停止）にしたり、物理的にブレーキをかけたりする等する処理である。なお、本実施形態においては、サーボ電源をオフにすると共に物理的にブレーキをかける処理を採用している。

（１．２．３ スレーブ側情報処理装置の動作（第２の干渉禁止領域））
図１０は、スレーブ側情報処理装置５の動作に関するフローチャートである。同図から明らかな通り、処理が開始すると、ユーザ姿勢受信部５１は、ユーザ姿勢情報を取得する処理を行う（Ｓ３）。その後、マッピング処理部５２は、ユーザ姿勢情報に基づいてロボット８の目標関節角度を算出すべく所定のマッピング処理を行う（Ｓ４）。

図１１は、マッピング処理の詳細フローチャートである。同図から明らかな通り、処理が開始すると、マッピング処理部５２は、逆運動学を解いてロボットにおける目標関節角度を算出するため、記憶部より、センサ構成とロボットの各関節との対応関係、当該対応関係にそれぞれ対応する変換数式のパラメータ等を含む変換テーブルを読み出す（Ｓ４１）。その後、所定の整数値Ｎの初期化を行う（例えば、Ｎ＝１）。

初期化処理の後、マッピング処理部５２は、ユーザ姿勢情報と変換テーブルに基づいて、逆運動学を解くことにより、ユーザ姿勢情報に含まれるＮ個のセンサ情報の各々に基づいて、対応するロボット関節における目標関節角度を算出する。

本実施形態においては、例えば、上腕部に取り付けられ３自由度を有する右上腕用ＩＭＵセンサ２３Ｒの値を用いて、逆運動学を解くことにより、右腕部第１関節駆動モータ８５１Ｒと、右腕部第２関節駆動モータ８５２Ｒと、右腕部第３関節駆動モータ８５３Ｒへと目標関節角度をマッピングする。また、前腕部に取り付けられ１自由度を有する右前腕用ＩＭＵセンサ２４Ｒの値を用いて、逆運動学を解くことにより、右腕部第４関節駆動モータ８５４Ｒへと目標関節角度をマッピングする。さらに、手の甲に取り付けられ３自由度を有する右甲用ＩＭＵセンサ２５Ｒの値を用いて、逆運動学を解くことにより、右腕部第５関節駆動モータ８５５Ｒ、右腕部第６関節駆動モータ８５６Ｒ、及び、右腕部第７関節駆動モータ８５７Ｒへと目標関節角度をマッピングする。他の関節においても同様の手法により目標関節角度を算出することができる。

第Ｎ番目のセンサ情報に基づく処理が終了すると、Ｎが所定の値（例えば、最大センサ数）となったかを判定する処理が行われる（Ｓ４５）。その結果、Ｎが所定値未満である場合には（Ｓ４５ＮＯ）、Ｎを１だけインクリメントして（Ｓ４７）、再度、上述の処理が行われる（Ｓ４３、Ｓ４５）。一方、Ｎが所定値となった場合には（Ｓ４５ＹＥＳ）、処理は終了する。

図１０に戻り、マッピング処理（Ｓ４）が終了すると、自己干渉判定処理が行われる（Ｓ５）。

図１２は、自己干渉判定処理（Ｓ５）の詳細フローチャートである。同図から明らかな通り、処理が開始すると、自己干渉判定に必要なロボット機構情報の読出処理が行われる（Ｓ５１）。ロボット機構情報とは、ロボット８の関節を一連に又は分岐を伴ってリンクでつないで構成されるグラフ構造や、各関節やリンクのハードウェア構造に関する情報（各リンクの寸法、各関節の回転中心の方向ベクトル、等）を含むものである。

次に、第２の干渉禁止領域の算出処理が行われる（Ｓ５３）。ここで、第２の干渉禁止領域は、上述の第１の干渉禁止領域と一致するか又はそれを包含するものである。すなわち、干渉によりフェールセーフ処理が行われる第１の干渉禁止領域と同一か又はさらに空間的余裕を以て設定された干渉禁止領域である。

図１３は、第１の干渉禁止領域３００と第２の干渉禁止領域２００の関係について示す説明図である。同図から明らかな通り、ロボット８の表面を覆うように所定間隔を空けて第１の干渉禁止領域３００（破線）が設定されており、そのさらに外側に、第１の干渉禁止領域を包含するように第２の干渉禁止領域２００（一点鎖線）が設定されている。

図１４は、第２の干渉禁止領域の算出処理（Ｓ５３）に関する詳細フローチャートである。同図から明らかな通り、処理が開始すると、指令値提供判定処理部５３は、ロボット機構情報に基づいて最上位リンク（ルートリンクと呼ぶ）（Ｍ＝０）を特定し、当該ルートリンクの一つ下の子リンク（Ｍ＝１）を参照リンクとして設定する処理を行う（Ｓ５３２）。

設定処理が完了すると、指令値提供判定処理部５３は、親リンク（Ｍ＝０）の座標系原点に対して同次変換行列を掛け合わせ、参照リンク（Ｍ＝１）の座標系原点の位置・姿勢を算出する処理を行う（Ｓ５３３）。その後、当該参照リンクの回転中心の方向ベクトルと、目標関節角度から算出された回転角度に基づいて、参照リンクの座標系原点における姿勢を更新する（Ｓ５３４）。

参照リンクにおける座標系原点での姿勢の更新が完了すると、参照リンクに対して、３次元的に第２の干渉禁止領域を付与する処理、すなわち、リンク表面から所定距離離れた位置に配置されるポリゴンメッシュを構成する三角形群の頂点座標を更新する処理が行われる（Ｓ５３５）。なお、ポリゴンメッシュの形状は三角形に限定されず、例えば、四角形や凹凸のある形状であってもよい。また、３次元構造を表現する他の公知の手法を採用してもよい。

その後、Ｍが所定値であるか否かを判定する処理が行われる。Ｍが所定値に満たない場合（Ｓ５３６ＮＯ）、Ｍを１だけインクリメント（Ｓ５３８）して次のリンクを参照し、再度一連の処理（Ｓ５３３〜Ｓ５３６）を繰り返す。一方、Ｍが所定となった場合（Ｓ５３６ＹＥＳ）、処理は終了する。すなわち、目標関節角度に基づいて、すべてのリンクに対して関節角度更新後の第２の干渉禁止領域に相当するポリゴンメッシュが設定されることとなる。

図１２に戻り、第２の干渉禁止領域の算出処理（Ｓ５３）が行われると、算出された第２の干渉禁止領域に基づいて、リンク間の干渉判定処理が行われる（Ｓ５５）。

図１５は、リンク間の干渉判定処理の詳細フローチャートである。同図から明らかな通り、処理が開始すると、参照リンクに干渉があるか否かを示すすべての干渉フラグを、干渉がないことを示すＯＦＦ状態へとすべて初期化する処理が行われる（Ｓ５５１）。その後、ルートリンクを参照リンクとして設定する処理が行われる（Ｓ５５２）。

参照リンクが設定されると、当該参照リンクと他のリンクとの間において、境界ボリュームに基づく干渉判定処理が行われる（Ｓ５５３）。本実施形態においては、軸に平行な箱型の境界ボリュームを利用して干渉判定を行う（ＡＡＢＢ法）。

すなわち、参照リンクに関する第２の干渉禁止領域を含み所定軸と平行な箱型形状と、他のリンクに関する第２の干渉禁止領域を含み所定軸と平行な箱型形状との間における干渉判定処理が行われる。境界ボリュームを利用することで、対象３次元体を含むシンプルな形状同士の比較が行われるので、大まかな干渉判定を行うことができる。

この干渉判定処理（Ｓ５５３）の結果、両者の間に干渉無しと判定された場合には（Ｓ５５４ＮＯ）、未だ干渉判定を行っていない他のリンクが存在するかの判定処理が行われる（Ｓ５５８）。一方、干渉判定処理（Ｓ５５３）の結果、両者の間に干渉有りと判定された場合には（Ｓ５５４ＹＥＳ）、第２の干渉禁止領域表面を構成するポリゴンメッシュに基づく干渉判定処理が行われる（Ｓ５５５）。すなわち、実際のリンク形状に即して設定された干渉禁止領域の表面間において詳細に干渉判定処理が行われる。

この干渉判定処理（Ｓ５５５）の結果、ポリゴンメッシュ間において干渉無しと判定された場合には（Ｓ５５６ＮＯ）、未だ干渉判定を行っていない他のリンクが存在するかの判定処理が行われる（Ｓ５５８）。一方、干渉判定処理（Ｓ５５５）の結果、両者の間に干渉有りと判定された場合には（Ｓ５５６ＹＥＳ）、参照リンク及び当該参照リンクと干渉するリンクについて干渉フラグをＯＮにする（Ｓ５５７）。

以上の処理（Ｓ５５３〜Ｓ５５７）が終了すると、未だ干渉判定を行っていない他のリンクが存在するか否かについて判定処理が行われ（Ｓ５５８）、未だすべての干渉判定を行っていないと判定された場合には、参照リンクと比較されるリンクを変更する処理（Ｓ５６０）が行われ、再度、一連の処理（Ｓ５５３〜Ｓ５５７）が行われる。

一方、参照リンクが他のすべてのリンクと干渉判定を行ったものと判定されると（Ｓ５５８ＹＥＳ）、Ｍが所定値となったか、すなわち、すべてのリンクを参照リンクとしたか否かが判定される（Ｓ５５９）。未だすべてのリンクを参照リンクとしていない場合（Ｓ５５９ＮＯ）、Ｍを１だけインクリメントして（Ｓ５６１）、再度一連の処理（Ｓ５５３〜Ｓ５５７）を行う。すべてのリンクを参照リンクとしたと判定された場合（Ｓ５５９ＹＥＳ）、処理は終了する。

このような構成によれば、境界ボリュームを利用した大まかな判定処理により干渉しないと判定された場合には、それ以上の干渉判定を行わないので、処理の高速化や処理負荷の低減等を行うことができる。

なお、本実施形態においては、すべてのリンク（又はその干渉禁止領域含む）間で干渉判定を行う構成について説明を行ったが、本発明はこのような構成に限定されない。従って、例えば、一部のリンクについては干渉判定の対象から除外してもよい。例えば、隣り合うリンク間での判定処理を行わないように構成してもよい。このような構成によれば、元々干渉が生じやすい隣り合うリンク間で発生した干渉に基づく無用なフェールセーフ処理の発生等を防止することができる。

このような構成によれば、フェールセーフ処理が行われる基準となる第１の干渉禁止領域と一致するか又はそれを包含する第２の干渉禁止領域への干渉を目標関節角度に基づいて判定するので、スレーブロボットにおける自己衝突の危険を低減させることができると共にフェールセーフ処理が行われる頻度を低減させることができる。すなわち、マスタ・スレーブ制御システムにおけるロボットの自己衝突の危険を低減しつつも、運用コストが抑制されたロボット制御システムを提供することができる。

また、従来のマスタ・スレーブ制御システム１００においては、現在関節角度を例えば１００Ｈｚ程度の周期で監視していたため、ロボット８に対して高速の追従動作等を要求した場合には、フェールセーフ処理が間に合わず、自己衝突が発生してしまうおそれがあった。しかしながら、本実施形態によるマスタ・スレーブ制御システム１００によれば、目標関節角度を基準として、コントローラユニット７への目標関節角度の提供の要否が決定されるので、より自己衝突が発生しにくくなる。

（２．第２の実施形態）
第１の実施形態によれば、ロボット８における自己衝突の危険を低減させることができると共にフェールセーフ処理が行われる頻度を低減させることができる。しかしながら、同構成を以てしても現在関節角度から目標関節角度へと移行する中間姿勢において自己干渉が発生する可能性がある。本実施形態によれば、より確実にロボット８における自己衝突の危険を低減させ、フェールセーフ処理が行われる頻度を低減させることができる。

なお、第２の実施形態において、第１の実施形態と略同一のハードウェア構成を有しており、スレーブ側情報処理装置５においては、第１の実施形態と同様に、第１の干渉禁止領域に関して干渉判定を行うことにより、フェールセーフ処理が行われている。

図１６は、本実施形態に係るスレーブ側情報処理装置５の動作に関するフローチャートである。同図において、処理が開始すると、ユーザ姿勢情報の取得処理（Ｓ３）とマッピング（Ｓ４）が行われる。なお、これらの処理は第１の実施形態の図６と同様であるので説明は省略する。

指令値提供判定処理部５３は、マッピング処理（Ｓ４）終了後、現在関節角度と目標関節角度とに基づいて、中間姿勢を設定する処理を行う（Ｓ６１）。本実施形態においては、ロボット８の関節空間において、ロボット８の現在姿勢から目標姿勢までを結んだ直線上の一又は複数の点を採用している。

なお、中間姿勢の設定方法は、本実施形態の手法に限定されず他の種々の手法を採用してもよい。例えば、ロボット８の関節空間において、ロボット８の現在姿勢から目標姿勢までを各関節の位置・速度・加速度の時系列変化が滑らかになるようにスプライン計算したときの当該軌道上の一又は複数の点であってもよい。また、ロボット８の関節空間ではなく物理的な作業空間において、ロボット８の現在姿勢から目標姿勢までを結ぶ直線上又はスプライン計算した軌道上の一又は複数の点であってもよい。

中間処理の設定処理の終了後、指令値提供判定処理部５３は、自己干渉判定処理を行う（Ｓ６２）。

図１７は、自己干渉判定処理（Ｓ６２）の詳細フローチャートである。同図から明らかな通り、処理が開始すると、ロボット機構情報の読出処理が行われる（Ｓ６２１）。その後、整数Ｌの初期化処理が行われる（Ｓ６２２）。なお、Ｌは中間姿勢について便宜上付与される番号であり、例えば、中間姿勢から最終姿勢まで各姿勢に対して順に付与される。

初期化処理の後、第Ｌ番目の中間姿勢における第２の干渉禁止領域の算出処理が行われる（Ｓ６２３）。この算出処理は、図１２の第２の干渉禁止領域の算出処理（Ｓ５３）と略同一であるので説明は省略する。

その後、第Ｌ番目の中間姿勢における第２の干渉禁止領域に基づくリンク間の干渉判定処理が行われる（Ｓ６２４）。この干渉判定処理は、図１２の干渉判定処理（Ｓ５５）と略同一であるので説明は省略する。

第Ｌ番目の中間姿勢につき、干渉判定処理（Ｓ６２４）が行われると、Ｌが所定値であるか否かを判定する処理が行われる（Ｓ６２５）。判定処理の結果、Ｌが所定値未満であると判定される場合には（Ｓ６２５ＮＯ）、Ｌを１だけインクリメントし、次の中間姿勢について一連の処理（Ｓ６２３〜Ｓ６２５）が行われる。一方、判定処理の結果、Ｌが所定値である場合（Ｓ６２５ＹＥＳ）、処理は終了する。

図１６に戻り、自己干渉判定処理（Ｓ６２）が終了すると、干渉フラグに基づき、いずれかの中間姿勢又は最終姿勢において干渉があるか否かの判定処理が行われる（Ｓ６３）。いずれの中間姿勢又は最終姿勢においても干渉が無いと判定される場合（Ｓ６３ＮＯ）、コントローラユニット７への目標関節角度の送信処理が行われる（Ｓ８）。一方、いずれかの中間姿勢又は最終姿勢において干渉が有ると判定される場合（Ｓ６３ＹＥＳ）、コントローラユニット７への目標関節角度の送信処理は行われず処理は終了する。

本実施形態によれば、現在姿勢から目標姿勢へと移行する中間姿勢においても自己干渉の有無を判定するので、より確実に、ロボット８における自己衝突の危険を低減させることができると共にフェールセーフ処理が行われる頻度を低減させることができる。

（３．第３の実施形態）
第１の実施形態においては、各関節の目標関節角度に基づいて、干渉判定の対象となる第２の干渉禁止領域を算出したが、本実施形態では、干渉判定の対象となる第２の干渉禁止領域の一方を現在角度に基づいて算出する。

図１９は、本実施形態に係るマスタ・スレーブ制御システム１００の機能ブロック図である。なお、本実施形態に係るマスタ・スレーブ制御システム１００の構成は、図１９を除いては、第１の実施形態と略同一であるのでここでは説明を省略する（図１〜図４参照）。また、同一又は略同一の機能を果たすブロックには同一の符号が付されていることに留意されたい。

同図から明らかな通り、本実施形態は、第１の実施形態と略同一の構成を有するものの、コントローラユニット通信部５４からの現在関節角度が、関節角度監視部５６のみならず指令値提供判定処理部５３１へと提供されている点において相違する。

スレーブ側情報処理装置５の動作について説明する。本実施形態においても、図５〜図１５に示したのと同様に、スレーブ側情報処理装置５において、第１の干渉禁止領域に基づく干渉判定、フェールセーフ処理、第２の干渉禁止領域に基づく干渉判定及び目標関節角度の送信処理が行われる。しかしながら、本実施形態においては、目標関節角度に基づく第２の干渉禁止領域の算出（Ｓ５３）と合わせて、提供された現在関節角度に基づいて、第２の干渉禁止領域の算出が行われる。そして、目標関節角度に基づく第２の干渉禁止領域と現在関節角度に基づく第２の干渉禁止領域との間で干渉の有無が判定される。その結果、干渉無しと判定される場合には、目標関節角度のロボット８への送信処理が行われ、干渉有りと判定される場合には、目標関節角度のロボット８への送信処理は行われない。

このような構成によれば、目標関節角度を用いるので、未然にスレーブロボットにおける自己衝突や意図しないフェールセーフ処理の適用を防止することができる。また、現在関節角度と目標関節角度とを併用することから、素早いユーザの動きなどによりスレーブロボットが十分に追従できない場合であっても、違和感なく自己衝突の危険を低減させることができると共に、フェールセーフ処理が行われる頻度を低減させることができる。

（４．変形例）
上述の実施形態においては、第２の干渉禁止領域同士の干渉判定結果に基づいて、目標関節角度を送信するか否かを判定していたが、本発明はこのような構成に限定されない。従って、例えば、いずれかを一方を第２の干渉禁止領域とし、他方は、ロボットの筐体領域又は第１の干渉禁止領域としてもよい。

本発明は、少なくともマスタ・スレーブ制御システム等を製造する産業において利用可能である。

１ マスタシステム１
２ モーションセンサ
３ マスタ側情報処理装置
４ スレーブシステム
５ スレーブ側情報処理装置
７ コントローラユニット
８ ロボット
８９ ロボット（従来図）
２１ 頭部用ＩＭＵセンサ
２２ 背面用ＩＭＵセンサ
２３ 上腕用ＩＭＵセンサ
２４ 前腕用ＩＭＵセンサ
２５ 甲用ＩＭＵセンサ
８１ 頭部
８１１ 頭部第１関節駆動モータ
８１２ 頭部第２関節駆動モータ
８２ 胴部
８２１ 胴部第１関節駆動モータ
８２２ 胴部第２関節駆動モータ
８５ 腕部
８５１ 腕部第１関節駆動モータ
８５２ 腕部第２関節駆動モータ
８５３ 腕部第３関節駆動モータ
８５４ 腕部第４関節駆動モータ
８５５ 腕部第５関節駆動モータ
８５６ 腕部第６関節駆動モータ
８５７ 腕部第７関節駆動モータ
１００ マスタ・スレーブ制御システム
２００ 第２の干渉禁止領域
３００ 第１の干渉禁止領域

Claims (14)

1. マスタ装置を介して取得されたユーザ姿勢に追従し、かつ、１又は複数の関節と複数のリンクを備えたスレーブロボットを制御するロボット制御システムであって、
   前記スレーブロボットの前記リンクのうちの一のリンクを包含するように構成された第１の空間領域が、他のいずれかのリンク又は当該リンクに関する第１の空間領域に干渉するか否かを判定する、第１干渉判定部と、
   前記第１干渉判定部において干渉有りと判定される場合には、前記スレーブロボットの動作制御部へとフェールセーフ信号を提供する、フェールセーフ信号提供部と、
   前記リンクのうちの一のリンクを包含しかつ対応する第１の空間領域と一致するか又は包含するように構成された第２の空間領域が、他のいずれかのリンク、当該リンクに関する第１の空間領域又は第２の空間領域に干渉するか否かを判定する、第２干渉判定部と、
   前記第２干渉判定部において干渉無しと判定される場合には、前記ユーザ姿勢に対応する前記スレーブロボットの各前記関節における目標関節角度を前記スレーブロボットの前記動作制御部へと提供し、前記第２干渉判定部において干渉有りと判定される場合には、前記目標関節角度を前記スレーブロボットの前記動作制御部へと提供しない、目標関節角度提供部と、
   を備えたロボット制御システム。
2. 前記第２干渉判定部において干渉判定される、前記一のリンクに係る前記第２の空間領域、又は、前記他のいずれかのリンク、当該リンクに関する第１の空間領域若しくは第２の空間領域のいずれか一方は、目標関節角度に基づいて算出され、他方は、前記スレーブロボットの各前記関節における現在関節角度に基づいて算出される、請求項１に記載のロボット制御システム。
3. 前記第２干渉判定部において干渉判定される、前記一のリンクに係る前記第２の空間領域、又は、前記他のいずれかのリンク、当該リンクに関する第１の空間領域若しくは第２の空間領域のいずれも、前記目標関節角度に基づいて算出される、請求項１に記載のロボット制御システム。
4. 前記ロボット制御システムは、さらに、
   現在の前記関節角度から前記目標関節角度へと至る過程の一又は複数の中間姿勢における中間姿勢関節角度を生成する、中間関節角度生成部を備え、
   前記第２干渉判定部は、さらに、
   前記中間姿勢関節角度のそれぞれに基づいて、前記リンクのうちの一のリンクに関する第２の空間領域が、他のいずれかのリンク、当該リンクに関する第１の空間領域又は第２の空間領域に干渉するか否かを判定する、中間姿勢判定部と、を備える、請求項１に記載のロボット制御システム。
5. 前記第１干渉判定部における前記判定は、前記リンク又は前記リンクに関する第１の空間領域に対応する３次元モデル同士の干渉判定を行うことにより行われる、請求項１に記載のロボット制御システム。
6. 前記第１干渉判定部は、さらに、
   判定対象となる前記リンク又は前記リンクに関する第１の空間領域に対応する境界ボリューム同士の干渉を判定する、第１境界ボリューム判定部と、
   前記第１境界ボリューム判定部において干渉有りと判断された場合に、前記リンク又は前記リンクに関する第１の空間領域の表面間の干渉を判定する、第１表面干渉判定部と、
   を備える、請求項５に記載のロボット制御システム。
7. 前記第２干渉判定部における前記判定は、前記リンク、前記リンクに関する第１の空間領域又は第２の空間領域に対応する３次元モデル同士の干渉判定を行うことにより行われる、請求項１に記載のロボット制御システム。
8. 前記第２干渉判定部は、
   判定対象となる前記リンク、前記リンクに関する第１の空間領域又は第２の空間領域に対応する境界ボリューム同士の干渉を判定する、第２境界ボリューム判定部と、
   前記第２境界ボリューム判定部において干渉有りと判断された場合に、前記リンク、前記リンクに関する第１の空間領域又は前記第２の空間領域の表面間の干渉を判定する、第２表面干渉判定部と、を備える、請求項７に記載のロボット制御システム。
9. 前記第１境界ボリューム判定部又は前記第２境界ボリューム判定部における判定は、ＡＡＢＢ法（Ａｘｉｓ−Ａｌｉｇｎｅｄ Ｂｏｕｎｄｉｎｇ Ｂｏｘ法）により行われる、請求項６又は８に記載のロボット制御システム。
10. 前記ロボット制御システムは、さらに、
    前記ユーザ姿勢に関するセンサ情報を前記スレーブロボットの関節角度へと変換するための変換情報を記憶する変換情報記憶部と、
    前記変換情報と逆運動学に基づいて、前記ユーザ姿勢を、対応する前記スレーブロボットの各前記関節における目標関節角度へと変換する、変換部と、を備える請求項１に記載のロボット制御システム。
11. 前記第１干渉判定部及び前記第２干渉判定部は、隣り合うリンクの間では干渉判定を行わない、請求項１に記載のロボット制御システム。
12. マスタ装置を介して取得されたユーザ姿勢に追従し、かつ、１又は複数の関節と複数のリンクを備えたスレーブロボットを制御するロボット制御装置であって、
    前記スレーブロボットの前記リンクのうちの一のリンクを包含するように構成された第１の空間領域が、他のいずれかのリンク又は当該リンクに関する第１の空間領域に干渉するか否かを判定する、第１干渉判定部と、
    前記第１干渉判定部において干渉有りと判定される場合には、前記スレーブロボットの動作制御部へとフェールセーフ信号を提供する、フェールセーフ信号提供部と、
    前記リンクのうちの一のリンクを包含しかつ対応する第１の空間領域と一致するか又は包含するように構成された第２の空間領域が、他のいずれかのリンク、当該リンクに関する第１の空間領域又は第２の空間領域に干渉するか否かを判定する、第２干渉判定部と、
    前記第２干渉判定部において干渉無しと判定される場合には、前記ユーザ姿勢に対応する前記スレーブロボットの各前記関節における目標関節角度を前記スレーブロボットの前記動作制御部へと提供し、前記第２干渉判定部において干渉有りと判定される場合には、前記目標関節角度を前記スレーブロボットの前記動作制御部へと提供しない、目標関節角度提供部と、
    を備えたロボット制御装置。
13. マスタ装置を介して取得されたユーザ姿勢に追従し、かつ、１又は複数の関節と複数のリンクを備えたスレーブロボットを制御するロボット制御方法であって、
    前記スレーブロボットの前記リンクのうちの一のリンクを包含するように構成された第１の空間領域が、他のいずれかのリンク又は当該リンクに関する第１の空間領域に干渉するか否かを判定する、第１干渉判定ステップと、
    前記第１干渉判定部において干渉有りと判定される場合には、前記スレーブロボットの動作制御部へとフェールセーフ信号を提供する、フェールセーフ信号提供ステップと、
    前記リンクのうちの一のリンクを包含しかつ対応する第１の空間領域と一致するか又は包含するように構成された第２の空間領域が、他のいずれかのリンク、当該リンクに関する第１の空間領域又は第２の空間領域に干渉するか否かを判定する、第２干渉判定ステップと、
    前記第２干渉判定部において干渉無しと判定される場合には、前記ユーザ姿勢に対応する前記スレーブロボットの各前記関節における目標関節角度を前記スレーブロボットの前記動作制御部へと提供し、前記第２干渉判定部において干渉有りと判定される場合には、前記目標関節角度を前記スレーブロボットの前記動作制御部へと提供しない、目標関節角度提供ステップと、
    を備えたロボット制御方法。
14. マスタ装置を介して取得されたユーザ姿勢に追従し、かつ、１又は複数の関節と複数のリンクを備えたスレーブロボットを制御するロボット制御プログラムであって、
    前記スレーブロボットの前記リンクのうちの一のリンクを包含するように構成された第１の空間領域が、他のいずれかのリンク又は当該リンクに関する第１の空間領域に干渉するか否かを判定する、第１干渉判定ステップと、
    前記第１干渉判定部において干渉有りと判定される場合には、前記スレーブロボットの動作制御部へとフェールセーフ信号を提供する、フェールセーフ信号提供ステップと、
    前記リンクのうちの一のリンクを包含しかつ対応する第１の空間領域と一致するか又は包含するように構成された第２の空間領域が、他のいずれかのリンク、当該リンクに関する第１の空間領域又は第２の空間領域に干渉するか否かを判定する、第２干渉判定ステップと、
    前記第２干渉判定部において干渉無しと判定される場合には、前記ユーザ姿勢に対応する前記スレーブロボットの各前記関節における目標関節角度を前記スレーブロボットの前記動作制御部へと提供し、前記第２干渉判定部において干渉有りと判定される場合には、前記目標関節角度を前記スレーブロボットの前記動作制御部へと提供しない、目標関節角度提供ステップと、
    を備えたロボット制御プログラム。

Images