(57)【要約】【課題】未熟な操作者でも、スレーブロボットを正確に操作すること。【解決手段】マスタスレーブシステムは、操作者が操作を行うマスタ操作手段と、マスタ操作手段と離れた位置に配置されたスレーブロボットと、マスタ操作手段の動作に応じてスレーブロボットを動作させるバイラテラル制御を行う制御手段と、スレーブロボットが動作する際の目標となる目標軌道を記憶する記憶手段を備える。制御手段は、スレーブロボットが記憶手段に記憶された目標軌道から逸脱すると、マスタ操作手段の操作に対し抵抗力が生じるように、マスタ操作手段の動作を制御する。【選択図】図１
2018089736.tif 000002
【特許請求の範囲】
【請求項１】
操作者が操作を行うマスタ操作手段と、
前記マスタ操作手段と離れた位置に配置されたスレーブロボットと、
前記マスタ操作手段の動作に応じて前記スレーブロボットを動作させるバイラテラル制御を行う制御手段と、を備えるマスタスレーブシステムであって、
前記スレーブロボットが動作する際の目標となる目標軌道を記憶する記憶手段を備え、
前記制御手段は、前記スレーブロボットが前記記憶手段に記憶された目標軌道から逸脱すると、前記マスタ操作手段の操作に対し抵抗力が生じるように、該マスタ操作手段の動作を制御する、
ことを特徴とするマスタスレーブシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、バイラテラル制御を行うマスタスレーブシステムに関する。
【背景技術】
【０００２】
操作者が操作を行うマスタ操作手段と、マスタ操作手段と離れた位置に配置されたスレーブロボットと、マスタ操作手段の動作に応じてスレーブロボットを動作させるバイラテラル制御を行う制御手段と、を備えるマスタスレーブシステムが知られている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１６−０２２５５９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、上述したバイラテラル制御では、操作者は、高い操作自由度で直感的にスレーブロボットを操作できる。しかし、未熟な操作者は、そのバイラテラル制御の高い操作自由度によって、スレーブロボットを誤操作する虞がある。
【０００５】
本発明は、かかる課題を解決するためになされたものであり、未熟な操作者でも、スレーブロボットを正確に操作することができるマスタスレーブシステムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、
操作者が操作を行うマスタ操作手段と、
前記マスタ操作手段と離れた位置に配置されたスレーブロボットと、
前記マスタ操作手段の動作に応じて前記スレーブロボットを動作させるバイラテラル制御を行う制御手段と、を備えるマスタスレーブシステムであって、
前記スレーブロボットが動作する際の目標となる目標軌道を記憶する記憶手段を備え、
前記制御手段は、前記スレーブロボットが前記記憶手段に記憶された目標軌道から逸脱すると、前記マスタ操作手段の操作に対し抵抗力が生じるように、該マスタ操作手段の動作を制御する、
ことを特徴とするマスタスレーブシステム
である。
【発明の効果】
【０００７】
本発明によれば、未熟な操作者でも、スレーブロボットを正確に操作することができるマスタスレーブシステムを提供することができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の一実施形態に係るマスタスレーブシステムの概略的なシステム構成を示すブロック図である。
【図２】スレーブロボットの一例を示す図である。
【図３】スレーブロボットの把持部が行う動作の一例を示す図である。
【図４】目標軌道のボクセルの重み量を球の大きさで模擬的に表した図である。
【図５】ボクセルを球状体で表現した図である。
【図６】把持部の形状をカプセル形状あるいは角丸直方体で表現した図である。
【図７】カプセル型形状の把持部と球状体のボクセルとの距離を示す図である。
【図８】（ａ）カプセル状把持部及び球状ボクセルの一例を示す図である。（ｂ）図８（ａ）に示すカプセル状把持部及び球状ボクセルに対して算出した、各球状ボクセルの重み量の一例を示す図である。
【図９】本発明の一実施形態に係るマスタスレーブ制御処理のフローの一例を示すフローチャートである。
【図１０】ベースが移動した場合のダンピング処理を説明するための図である。
【図１１】肘部近傍に障害物が存在する環境下でのダンピング処理を示す図である。
【図１２】（ａ）ダンピング処理により目標軌道を生成する方法を示す図である。（ｂ）ダンピング処理により目標軌道を生成する方法を示す図である。（ｃ）ダンピング処理により目標軌道を生成する方法を示す図である。
【発明を実施するための形態】
【０００９】
以下、図面を参照して本発明の実施形態について説明する。図１は、本発明の一実施形態に係るマスタスレーブシステムの概略的なシステム構成を示すブロック図である。本実施形態に係るマスタスレーブシステム１は、操作者が操作を行うマスタ操作部２と、マスタ操作部２と離れた位置に配置されたスレーブロボット３と、マスタ操作部２の動作に応じてスレーブロボット３を動作させるバイラテラル制御を行うバイラテラル制御部４と、記憶部５と、を備えている。
【００１０】
スレーブロボット３は、例えば、図２に示す如く、６つ以上の関節部（手首関節、肘関節、肩関節など）を有し、先端位置姿勢（手先位置姿勢など）を６自由度以上操作可能な多関節型のアームロボットとして構成されている。スレーブロボット３は、例えば、ベース部３１と、複数のリンク３２と、各リンク３２を回転可能に連結する関節部３３と、リンク３２に先端に設けられ対象物を把持操作する把持部３４と、を有している。
【００１１】
各関節部３３には、各関節部３３を駆動させるアクチュエータが設けられている。アクチュエータは、例えば、モータ駆動、油圧駆動、空気圧駆動などである。各関節部３３には、各関節部３３の回転角（以下、関節角）を検出する角度センサが設けられている。各関節部３３には、各関節部３３のトルク（以下、関節トルク）を検出するトルクセンサが設けられている。なお、スレーブロボット３は、複数の関節部（股関節、膝関節、足首関節など）を有する脚ロボットであってもよい。
【００１２】
スレーブロボット３には、ステレオカメラ、デプスカメラなどの環境認識センサ３５が設けられている。環境認識センサ３５は、例えば、スレーブロボット３のタスク遂行状態などのスレーブロボット３の周囲の環境情報を取得する。
【００１３】
マスタ操作部２は、マスタ操作手段の一具体例である。マスタ操作部２は、操作者がアームなどを介して操作情報を入力する操作部を有している。スレーブロボット３は、マスタ操作部２に入力された操作情報に応じて動作する。マスタ操作部２は、スレーブロボット３とは離れた位置に配置されている。マスタ操作部２は、スレーブロボット３の先端位置での力やモーメント、あるいは、各関節トルクなどの力情報を、マスタ操作部２の操作部を操作する操作者に対し、フィードバックする。
【００１４】
マスタ操作部２は、例えば、操作者がスレーブロボット３の手先位置を指示する手先指示装置、あるいは、操作者が装着する多関節型のマスタロボットである。マスタ操作部２は、例えば、複数のリンクと、各リンクを回転可能に連結する関節部と、リンクに先端に設けられ、操作者が把持操作する操作部と、を有する。各関節部には、各関節部を駆動させるアクチュエータが設けられている。アクチュエータは、例えば、モータ駆動、油圧駆動、空気圧駆動などである。各関節部には、各関節角を検出する角度センサが設けられている。各関節部には、各関節トルクを検出するトルクセンサが設けられている。
【００１５】
バイラテラル制御部４は、制御手段の一具体例である。バイラテラル制御部４は、マスタ操作部２の動作に倣ってスレーブロボット３を動作させるバイラテラル制御を実行する。バイラテラル制御部４は、バイラテラル制御において、操作者がマスタ操作部２を操作した際のマスタ操作部２の動作と同様に、スレーブロボット３を動作させる制御を行う。このとき、バイラテラル制御部４は、スレーブロボット３にかかる力情報を操作者にフィードバックする。すなわち、バイラテラル制御部４は、スレーブロボット３にかかる力と同様の力がマスタ操作部２に発生するように、マスタ操作部２を制御する。
【００１６】
例えば、バイラテラル制御部４は、マスタ操作部２の角度センサにより検出された各関節角に基づき、順運動学を用いて、目標とするスレーブロボット３の先端位置姿勢を算出する。バイラテラル制御部４は、算出した先端位置姿勢に基づき、逆運動学を用いて、スレーブロボット３の各関節部３３の目標関節角を算出する。バイラテラル制御部４は、算出した目標関節角に応じた指令信号をスレーブロボット３の各関節部３３のアクチュエータに出力してＰＩＤ制御などを行う。
【００１７】
同時に、バイラテラル制御部４は、スレーブロボット３の各関節部３３のトルクセンサにより検出された関節トルクに基づいて、スレーブロボット３の先端（手先など）にかかる先端力（荷重、モーメントなど）を算出する。バイラテラル制御部４は、算出した先端力に基づいて、マスタ操作部２の操作部にスレーブロボット３の先端力と同一の力が発生するように、マスタ操作部２の各関節部のアクチュエータを制御してＰＩＤ制御などを行う。
【００１８】
このようにして、バイラテラル制御部４は、バイラテラル制御において、操作者がマスタ操作部２を操作した際のマスタ操作部２の動作と同様に、スレーブロボット３を動作させつつ、スレーブロボット３にかかる力情報を、マスタ操作部２の操作部を介して操作者にフィードバックすることができる。なお、上記バイラテラル制御方法は一例であり、これに限定されない。
【００１９】
例えば、マスタ操作部２が、スレーブロボット３と類似の構成（関節部、リンクなどの構成）を有するマスタロボットとして構成されていてもよい。この場合、バイラテラル制御部４は、マスタロボットの各関節部と、対応するスレーブロボット３の各関節部３３と、を同期させるようなバイラテラル制御を行ってもよい。具体的には、バイラテラル制御部４は、マスタロボットの各関節部の角度センサにより検出された関節角及びトルクセンサにより検出された関節トルクを、スレーブロボット３の各関節部３３の目標関節角及び目標関節トルクに設定する。バイラテラル制御部４は、設定した目標関節角度及び目標関節トルクに応じた指令信号をスレーブロボット３の各関節部３３のアクチュエータに出力する。
【００２０】
同時に、バイラテラル制御部４は、スレーブロボット３の各関節部３３の角度センサにより検出された関節角及びトルクセンサにより検出された関節トルクを、マスタロボットの各関節部の目標関節角及び目標関節トルクに設定する。バイラテラル制御部４は、設定した目標関節角度及び目標関節トルクに応じた指令信号をマスタロボットの各関節部のアクチュエータに出力する。
【００２１】
以上により、操作者は、例えば、認識センサにより取得された環境情報（スレーブロボット３周囲の画像情報など）を参照し、マスタ操作部２を操作することで、スレーブロボット３をあたかも自らが直接操作しているかのような臨場感で、遠隔操作できる。同時に、操作者は、実際のスレーブロボット３に生じている反力を自ら感知することができる。
【００２２】
なお、バイラテラル制御部４は、は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）４１、ＣＰＵ４１によって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ４２、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）４３、などからなるマイクロコンピュータを中心にして、それぞれ、ハードウェア構成されている。ＣＰＵ４１、メモリ４２、及びインターフェイス部４３は、データバスなどを介して相互に接続されている。
【００２３】
ところで、上述したバイラテラル制御では、操作者は、高い操作自由度で直感的にスレーブロボットを操作できるというメリットを有している。しかしながら、スレーブロボット自体が高い自由度が動作できても、対象物が一定方向にしか動作しない場合も多く存在する。例えば、ドア、冷蔵庫、棚などの扉、タンスやロッカーなどの引出し、窓やシャッター、ペットボトルや瓶などの蓋、などの対象物は、一定方向にしか動作しない。
【００２４】
スレーブロボットが上記のような動作範囲に制約のある対象物を操作する場合、熟練した操作者は、バイラテラル制御の高い操作自由度を利用してスレーブロボットを動作範囲の制約内でより繊細に操作できる。しかし、未熟な操作者は、そのバイラテラル制御の高い操作自由度によって、その動作範囲の制約から外れてスレーブロボットを誤操作する虞がある。
【００２５】
これに対し、本実施形態に係るマスタスレーブシステム１は、スレーブロボット３が動作する際の目標となる目標軌道を記憶する記憶部５を備える。バイラテラル制御部４は、スレーブロボット３が記憶部５に記憶された目標軌道から逸脱すると、マスタ操作部２の操作に抵抗力が生じるようにマスタ操作部２の動作を制御する。
【００２６】
これにより、未熟な操作者がマスタ操作部２の操作を行い、スレーブロボット３が目標軌道から逸脱してもマスタ操作部２の操作に抵抗力が生じるため、スレーブロボット３が自ずと目標軌道へ戻るように、マスタ操作部の操作は修正される。したがって、未熟な操作者でも、目標軌道に従ってスレーブロボット３を正確に操作することができる。
【００２７】
本実施形態に係るマスタスレーブシステム１において、まず、スレーブロボット３が動作する際の目標となる目標軌道を記憶部５に記憶させる。記憶部５は、記憶手段の一具体例である。
【００２８】
例えば、熟練した操作者がマスタ操作部２を操作して、スレーブロボット３により対象物を移動させる。熟練した操作者がマスタ操作部２を操作することで、スレーブロボット３の理想的な軌道が生成される。このときのスレーブロボット３の把持部３４の軌道（以下、手先軌道）を目標軌道とする。
【００２９】
より具体的には、図３に示す如く、環境認識センサ３５は、スレーブロボット３の把持部３４がドアの取手（対象物）を把持し、ドアを開けるまでの把持部３４の動作を撮影する。バイラテラル制御部４は、環境認識センサ３５からの撮影画像に基づいて、把持部３４の特徴量を算出する。バイラテラル制御部４は、該算出した特徴量に基づいて、手先軌道を算出し、算出した手先軌道を目標軌道として記憶部５に記憶させる。
【００３０】
次に、スレーブロボット３が記憶部５に記憶された目標軌道から逸脱すると、マスタ操作部２に抵抗力が生じるように、マスタ操作部２の動作を制御する方法について説明する。
【００３１】
まず、記憶部５に記憶される目標軌道の設定方法について説明する。
上述のように記憶部５に記憶された目標軌道は、例えば、把持部３４が３次元的に通過する位置をボクセル形式で表現している。各ボクセルは、マスタ操作部２の操作に対する抵抗力を示す重み情報（重み量）を有している。その重み量は、把持部３４が各ボクセルを通過したときの接触度合に応じた値（例えば、０〜１）で正規化されている。ボクセルの粒度は、目標軌道を精度よく再現するために把持部形状に対して十分密に設定されている。また、以降の処理では時間情報を用いないため、ボクセルに対する時間情報は無視できる。図４は、目標軌道のボクセルの重み量を球の大きさで模擬的に表した図である。ボクセルの重み量は、スレーブロボット３の動作のし易さ、若しくは、動作制限（上述の抵抗力）を表している。
【００３２】
続いて、ボクセルの重み量の設定方法について詳細に説明する。
バイラテラル制御部４は、例えば、ボクセルを敷き詰めたマップ上を目標軌道に沿って把持部３４を移動させ、把持部３４と各ボクセルとの接触度合に応じて各ボクセルの重み量を算出する。
【００３３】
ここで、把持部３４と各ボクセルとの接触度合の計算を簡略化するため、例えば、図５に示す如く、ボクセルを立方体ではなく、該立方体を内包する球状体で表現してもよい。同様に、把持部３４の形状を、例えば、図６に示す如く、実際の形状を内包するような、カプセル形状あるいは角丸直方体（直方体の四方をカプセル形状で囲った形状）などの簡易形状で表現してもよい。
【００３４】
例えば、図７に示す如く、把持部３４の形状をカプセル形状とすれば、カプセル形状の把持部３４と球状体のボクセルとの距離は、線分と点との距離で算出でき、この距離を用いて把持部３４と各ボクセルとの接触度合を容易に表示できる。さらに、把持部３４の形状を角丸直方体とすれば、カプセル形状とＯＢＢ（Orient  Bounding  Box）という接触判定が容易な形状の組合せで、実際の把持部３４の形状に近い形でより正確に接触判定を行うことができる。
【００３５】
バイラテラル制御部４は、上記のように設定した簡易形状の把持部３４とその周囲の球状体のボクセルとの接触度合に応じて、各ボクセルの重み量ｗを、以下（１）−（４）のように算出する。本説明では、例えば、ボクセルと簡易形状をカプセル形状とする。
【００３６】
（１）バイラテラル制御部４は、カプセル形状の把持部３４（以下、カプセル状把持部３４）の２つの端点を結ぶ線分と、対象とする球状体のボクセル（以下、球状ボクセル）の中心との最近傍距離ｌを算出する。
【００３７】
（２）バイラテラル制御部４は、下記接触判定式（Ａ）−（Ｃ）を用いて、各球状ボクセルの重み量ｗを算出する。但し、球状ボクセルの半径をｒｂ、カプセル状把持部３４の両端の球の半径をｒｃとする。
（Ａ）ｌ＞ｒｂ＋ｒｃ（カプセル状把持部３４と球状ボクセルが接触しない場合）
：ｗ＝０
（Ｂ）ｒｃ＞ｌ＋ｒｃ（カプセル状把持部３４内に球状ボクセルが含まれる場合）
：ｗ＝１
（Ｃ）上記（Ａ）及び（Ｂ）以外（例えば、図７のように、カプセル状把持部３４と球状ボクセルが交わる場合）
：ｗ＝（ｒｂ＋ｒｃ−ｌ）／２ｒｂ
【００３８】
図８（ａ）は、カプセル状把持部３４及び球状ボクセルの一例を示す図である。図８（ｂ）は、図８（ａ）に示すカプセル状把持部３４及び球状ボクセルに対して算出した、各球状ボクセルの重み量の一例を示す図である。なお、上記各球状ボクセルの重み量ｗの算出方法は一例であり、これに限定されない。例えば、カプセル状把持部３４と各球状ボクセルとが接触する体積比などの他の指標に応じて、各球状ボクセルの重み量ｗを設定してもよい。
【００３９】
（３）バイラテラル制御部４は、カプセル状把持部３４を目標軌道に沿って移動させつつ、上記（１）及び（２）を繰返し、目標軌道及びその周辺の各球状ボクセルの重み量ｗを設定する。
【００４０】
（４）バイラテラル制御部４は、各球状ボクセル（ｘ、ｙ、ｚ）において、最大の重み量ｗをその球状ボクセルの重み量して、ｗ（ｘ、ｙ、ｚ）とし、記憶部５に記憶させる。これにより、全球状ボクセルに、０〜１に正規化された重み量ｗ（ｘ、ｙ、ｚ）が設定される。なお、バイラテラル制御部４は、後段処理での不連続を抑制するために、近傍の球状ボクセル間で平均値を取ってフィルタをかける処理を行ってもよい。
【００４１】
以上のようにして、図４に示すような目標軌道のボクセルに対して重み量ｗ（ｘ、ｙ、ｚ）を設定することができる。
【００４２】
次に、上記のように球状ボクセルに重み量ｗ（ｘ、ｙ、ｚ）を設定した目標軌道を用いて、スレーブロボット３が目標軌道から逸脱すると、マスタ操作部２に抵抗力が生じるように、マスタ操作部２の動作を制御する方法について説明する。
【００４３】
例えば、１制御周期の微小時間において、操作者が、マスタ操作部２の操作部に対して、位置ｐｎ−１（ｘｎ−１、ｙｎ−１、ｚｎ−１）、姿勢（クォータニオン：Quaternion）ｑｎ−１（ｓｎ−１、ｕｎ−１、ｖｎ−１、ｗｎ−１）から、位置ｐｒ（ｘｒ、ｙｒ、ｚｒ）、姿勢ｑｒ（ｓｒ、ｕｒ、ｖｒ、ｗｒ）となるような操作入力を行った場合の処理を以下に説明する。
【００４４】
バイラテラル制御部４は、マスタ操作部２の各関節部の角度センサにより検出された関節角に基づいて、位置ｐｒ及び姿勢ｑｒに対応するスレーブロボット３の把持部３４の候補位置を算出する。
【００４５】
バイラテラル制御部４は、記憶部５の球状ボクセルに重み量ｗ（ｘ、ｙ、ｚ）を設定した目標軌道と、算出したスレーブロボット３の把持部３４の候補位置と、に基づいて、上記接触判定式（Ａ）−（Ｃ）を用いて、把持部３４と球状ボクセルとの接触判定を行う。バイラテラル制御部４は、上記接触判定により、把持部３４と接触する球状ボクセル群を抽出する。
【００４６】
バイラテラル制御部４は、抽出した球状ボクセル群の重み量ｗ（ｘ、ｙ、ｚ）の中から、最小の重み量をｗｎとして算出する。
【００４７】
バイラテラル制御部４は、把持部３４が重み量の小さい球状ボクセルに接触するとダンピングが強く作用しマスタ操作部２の動作が抑制されるダンピング処理を行う。例えば、目標軌道上の球状ボクセルには、最大の重み量（ｗ＝１）が設定され、球状ボクセルが目標軌道から外れに従って、その球状ボクセルには徐々に小さな重み量（０＜ｗ＜１）が設定され、球状ボクセルが完全に目標軌道から外れると、球状ボクセルには、最小の重み量（ｗ＝０）が設定される。これにより、操作者は、把持部３４が目標軌道上にあるときは、マスタ操作部２の操作部に抵抗力をほとんど感じないが、把持部３４が目標軌道上から外れるに従がって、マスタ操作部２の操作部により強い抵抗力を感じ、把持部３４が目標軌道上から完全に外れると、マスタ操作部２の操作部に最大の抵抗力を感じる。
【００４８】
ここで、操作者が未熟であっても、あるい程度経験を積むと、操作ミスが減少する。このため、バイラテラル制御部４は、その経験度合に応じて重み量ｗｎを増加させることで、上記ダンピング処理の抵抗力を減少させる処理を行ってもよい。これにより、操作者の現状のスキルにより適合した調整を行うことができる。例えば、抵抗力を半分にする場合、重み量ｗｎのレンジが[０〜１]ではなく[０．５〜１]となるように、バイラテラル制御部４は、式（ｗｎ＝０．５ｗｎ＋０．５）を用いて重み量ｗｎを補正してよい。
バイラテラル制御部４は、上述のように算出した重み量をｗｎに基づいて、下記式１及び式２を用いて、スレーブロボット３の把持部３４の目標位置ｐｎ及び目標姿勢ｑｎを算出する。
【００４９】
【数１】
2018089736.tif 000003
なお、上記式１及び式２において、クォータニオンが用いられているが、これに限定されず、例えば、オイラー角などを用いてもよい。
【００５０】
バイラテラル制御部４は、算出した目標位置ｐｎ及び目標姿勢ｑｎとなるように、スレーブロボット３の各関節部３３のアクチュエータを制御しつつ、スレーブロボット３の手先位置及び姿勢に対応させて、マスタ操作部２の操作部の位置及び姿勢をバイラテラル制御する。
【００５１】
なお、重み量ｗｎからｗｎ−１に急激に変化すると操作者が感じる抵抗力も不連続に変化することとなり、操作感が悪い。このため、バイラテラル制御部４は、重み量ｗｎに時間的なローパスフィルタを掛ける処理や、近傍の球状ボクセル間の重み量との空間的なフィルタを掛ける処理、などを行い、この急激な変化を抑制してもよい。
【００５２】
バイラテラル制御部４は、上記ダンピング処理において、把持部３４が目標軌道上にある場合、操作者がマスタ操作部２の操作部を操作し易くし（抵抗力を低くし）、把持部３４が目標軌道上から外れるに従がって、操作者がマスタ操作部２の操作部を操作し難くなる（抵抗力が高くなる）ような制御を行っている。
【００５３】
さらに、バイラテラル制御部４は、把持部３４が目標軌道上から外れた場合に、把持部３４が目標軌道により戻り易くする制御を行ってもよい。これにより、操作者は、把持部３４が目標軌道上から外れた場合でも、その軌道を容易に修正し目標軌道へ戻すことができる。
【００５４】
例えば、バイラテラル制御部４は、ｗ′ｎ＞０の場合（ｗｎがｗｎ−１より増加している場合）、下記式を用いて、ｗｄを算出する。ｗ′ｎは、ｗ′ｎの微分値である。下記式において、βは、β＞０の定数とし、予めメモリなどに設定されている。
ｗｄ＝ｗｎ（１＋βｗ′ｎ）
【００５５】
バイラテラル制御部４は、算出したｗｄを上記式１及び式２のｗｎに代入して、スレーブロボット３の把持部３４の目標位置ｐｎ及び目標姿勢ｑｎを算出する。
【００５６】
なお、バイラテラル制御部４は、慣性項を導入した上記式１及び式２を用いて、目標軌道との偏差量に仮想バネ定数を乗じることで目標軌道に引き戻すように、スレーブロボット３の把持部３４の目標位置ｐｎ及び目標姿勢ｑｎを算出してもよい。
【００５７】
バイラテラル制御部４は、上記把持部３４と球状ボクセルとの接触判定において接触すると判定された記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）を増加させて更新を行ってもよい。これにより、単に熟練者などの目標軌道から逸脱し難くなるだけでなく、逸脱方向に繰返し操作すると次第にダンピング（抵抗力）が減少し、意図的に、熟練者などの目標軌道とは違う軌道を生成することができる。
【００５８】
例えば、バイラテラル制御部４は、上記把持部３４と球状ボクセルとの接触判定において接触すると判定された記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）に、所定係数を加算あるいは乗算することで、その重み量ｗ（ｘ、ｙ、ｚ）を増加させてもよい。
【００５９】
さらに、バイラテラル制御部４は、記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）に、その接触度合に応じた所定係数を加算あるいは乗算することで、その重み量ｗ（ｘ、ｙ、ｚ）を増加させてもよい。例えば、接触度合が大きくなるに従がって、所定係数を減少させてもよい。なお、増加後の重み量ｗ（ｘ、ｙ、ｚ）は１以下とする。所定係数は、メモリなどに予め設定され、操作者などが適宜設定変更できる。
【００６０】
上述したように、ダンピングを可変させることで、熟練者などによる目標軌道をある程度踏襲して安全性を確保しつつ、現在の操作者による臨機応変な操作も反映でき、マスタスレーブ操作法が持つ優位性を生かしたより柔軟な操作が実現できる。
【００６１】
例えば、熟練者などによる目標軌道の生成時におけるドアの大きさと、操作者による現在の操作時におけるドアの大きさとが異なる場合などの、目標軌道の生成時の環境と現在の環境とが異なる場合が想定される。このような場合でも、上記ダンピングを可変させ、目標軌道からの逸脱を効果的に緩和することで、操作者は操作遂行できる。
【００６２】
さらに、上記ダンピングの可変処理を繰り返すと、各球状ボクセルの重み量が熟練者などにより設定された初期値から徐々に大きく外れていく。したがって、バイラテラル制御部４は、記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）に、例えば、時間係数を乗算するなどを行うことで、該重み量を時間経過と共に元の初期値に戻す処理を行ってもよい。これにより、上記ダンピングの可変処理を繰り返しても、各球状ボクセルの重み量が熟練者などにより設定された初期値から大きく外れるのを抑制できる。
【００６３】
図９は、本実施形態に係るマスタスレーブ制御処理のフローの一例を示すフローチャートである。
【００６４】
例えば、マスタスレーブシステム１に対し、次の初期設定が行われる（ステップＳ１０１）。スレーブロボット３が動作する際の目標となる目標軌道を記憶部５に記憶させる。バイラテラル制御部４は、記憶部５に記憶された目標軌道の各球状ボクセルの重み量ｗを設定する。バイラテラル制御部４には、上記ダンピング処理の開始及び終了のトリガが設定される。より具体的には、バイラテラル制御部４は、操作者の視線入力、ジェスチャ、音声コマンドなどの所定の物理入力を受けると、その物理入力を上記ダンピング処理の開始のトリガ（以下、開始トリガ）、あるいは、上記ダンピング処理の終了のトリガ（以下、終了トリガ）として、該ダンピング処理を開始あるいは終了する。さらに、バイラテラル制御部４は、ドアを検出及びスレーブロボット３の把持部３４がドアノブを把持したことを検出すると、その検出を上記ダンピング処理の開始トリガとして、該ダンピング処理を自動的に開始してもよい。バイラテラル制御部４は、ドアの開状態を検出及びスレーブロボット３の把持部３４がドアノブを離したことを検出すると、その検出を上記ダンピング処理の終了トリガとして、該ダンピング処理を自動的に終了してもよい。
【００６５】
バイラテラル制御部４は、操作者によるマスタ操作部２の操作部の操作に応じて、次の暫定のスレーブロボット３の把持部３４の目標位置ｐｒ及び目標姿勢ｑｒを算出する（ステップＳ１０２）。
【００６６】
バイラテラル制御部４は、ダンピング処理中であるか否かを判定する（ステップＳ１０３）。
【００６７】
バイラテラル制御部４は、ダンピング処理中であると判定したとき、所定の物理入力などに基づいて、本ダンピング処理を解除（終了）するか否かを判定する（ステップＳ１０４）。
【００６８】
バイラテラル制御部４は、本ダンピング処理を解除しないと判定すると（ステップＳ１０４のＮＯ）、ダンピング処理を継続する。バイラテラル制御部４は、記憶部５の目標軌道と、算出したスレーブロボット３の把持部３４の候補位置と、に基づいて、把持部３４と球状ボクセルとの接触判定を行い、把持部３４と接触する球状ボクセル群を抽出する（ステップＳ１０５）。一方、バイラテラル制御部４は、本ダンピング処理を解除すると判定すると（ステップＳ１０４のＹＥＳ）、下記の（ステップＳ１１１）に移行する。
【００６９】
バイラテラル制御部４は、抽出した球状ボクセル群の重み量ｗ（ｘ、ｙ、ｚ）の中から、最小の重み量をｗｎとして算出する（ステップＳ１０６）。
【００７０】
バイラテラル制御部４は、算出した重み量をｗｎに基づいて、上記式１及び式２を用いて、スレーブロボット３の把持部３４の目標位置ｐｎ及び目標姿勢ｑｎを算出する（ステップＳ１０７）。
【００７１】
バイラテラル制御部４は、上記把持部３４と球状ボクセルとの接触判定において接触すると判定された記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）の更新を行う（ステップＳ１０８）。
【００７２】
バイラテラル制御部４は、記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）を時間経過と共に元の初期値に戻す処理を行い（ステップＳ１０８）、後述の処理（ステップＳ１１２）に移行する。
【００７３】
バイラテラル制御部４は、ダンピング処理中でないと判定したとき（ステップＳ１０３のＮＯ）、上記ダンピング処理の開始トリガに基づいて、本ダンピング処理を開始するか否かを判定する（ステップＳ１１０）。バイラテラル制御部４は、上記ダンピング処理の開始トリガに基づいて、本ダンピング処理を開始すると判定すると（ステップＳ１１０のＹＥＳ）、上記（ステップＳ１０５）に移行する。一方、バイラテラル制御部４は、上記ダンピング処理の開始トリガに基づいて、本ダンピング処理を開始しないと判定すると（ステップＳ１１０のＮＯ）、目標位置ｐｎ＝ｐｒ及び目標姿勢ｑｎ＝ｑｒに設定する（ステップＳ１１１）。
【００７４】
バイラテラル制御部４は、算出した目標位置ｐｎ及び目標姿勢ｑｎとなるように、スレーブロボット３の各関節部３３のアクチュエータを制御しつつ、スレーブロボット３の手先位置及び姿勢に対応させて、マスタ操作部２の操作部の位置及び姿勢をバイラテラル制御する（ステップＳ１１２）。
【００７５】
バイラテラル制御部４は、ｎをインクリメントし（ステップＳ１１３）、上記（ステップＳ１０２）に移行する。
【００７６】
以上、本実施形態に係るマスタスレーブシステム１は、スレーブロボット３が動作する際の目標となる目標軌道を記憶する記憶部５を備える。バイラテラル制御部４は、スレーブロボット３が記憶部５に記憶された目標軌道から逸脱すると、マスタ操作部２の操作に抵抗力が生じるようにマスタ操作部２の動作を制御する。これにより、スレーブロボット３が目標軌道から逸脱してもマスタ操作部２の操作に抵抗力が生じるため、スレーブロボット３が自ずと目標軌道へ戻るように、マスタ操作部の操作は修正される。したがって、未熟な操作者でも、目標軌道に従ってスレーブロボット３を正確に操作することができる。
【００７７】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
上記実施形態において、スレーブロボット３は、ベースなどに固定されているが、これに限定されない。例えば、スレーブロボット３は車輪や脚部を有し、移動する構成であってもよい。図１０に示す如く、ベースが移動しても、ベースと対象物との相対位置関係から、上記ベースが固定された場合と同様に、ダンピング処理を行い、同様の効果を得ることができる。
【００７８】
上記実施形態において、バイラテラル制御部４は、スレーブロボット３の把持部３４（手先）と球状ボクセルとの接触判定を行い重み量ｗｎを算出して、把持部３４の目標位置ｐｎ及び目標姿勢ｑｎを算出しているが、これに限定されない。バイラテラル制御部４は、スレーブロット把持部３４と同様に、肘部と球状ボクセルとの接触判定も行い重み量を算出して、肘部の目標位置ｐｎ及び目標姿勢ｑｎも算出してもよい。これにより、例えば、図１１に示すような肘部近傍に障害物が存在する環境下においても、予め熟練者がバイラテラル制御の冗長自由度を利用してその障害物を回避するような操作を行った目標軌道に沿って、未熟者も安全かつ正確に複雑環境下でのスレーブロボット３の遠隔操作を実現できる。
【００７９】
上記実施形態において、バイラテラル制御部４は、上記ダンピング処理を行って、目標軌道を生成してもよい。
ここで、熟練者にとっても、未経験の環境下でスレーブロボット３を遠隔操作するのが困難となる場合がある。そこで、図１２（ａ）に示す如く、初めに、バイラテラル制御部４は、僅かな重み量の球状ボクセルが設定された目標軌道に基づいて、把持部３４と球状ボクセルとの接触判定を行い、把持部３４の目標位置ｐｎ及び目標姿勢ｑｎを算出して、バイラテラル制御を行う。このとき、ダンピング処理効果によって、マスタ操作部２の操作は低速度となるが、誤操作が減少し正確な動作が期待できる。そして、バイラテラル制御部４は、ダンピング処理を繰り返すことで、各球状ボクセルの重み量を増加させ更新する。これにより、図１２（ｂ）に示す如く、把持部３４の軌道上及び周辺の各球状ボクセルの重み量が増加し、最終的に、図１２（ｃ）に示す如く、目標軌道が生成される。このように、バイラテラル制御部４は、僅かな重み量の球状ボクセルの目標軌道から、上記ダンピング処理及び各球状ボクセルの重み量の更新を繰返し、目標軌道を生成する。
【００８０】
本発明は、例えば、図９に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【００８１】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【００８２】
プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００８３】
１  マスタスレーブシステム、２  マスタ操作部、３  スレーブロボット、４  バイラテラル制御部、５  記憶部、３１  ベース部、３２  リンク、３３  関節部、３４  把持部、３５  環境認識センサ
【図１】
2018089736.tif 000004
【図２】
2018089736.tif 000005
【図３】
2018089736.tif 000006
【図４】
2018089736.tif 000007
【図５】
2018089736.tif 000008
【図６】
2018089736.tif 000009
【図７】
2018089736.tif 000010
【図８】
2018089736.tif 000011
【図９】
2018089736.tif 000012
【図１０】
2018089736.tif 000013
【図１１】
2018089736.tif 000014
【図１２】
2018089736.tif 000015