JP2018089736A - マスタスレーブシステム - Google Patents

Abstract

【課題】未熟な操作者でも、スレーブロボットを正確に操作すること。【解決手段】マスタスレーブシステムは、操作者が操作を行うマスタ操作手段と、マスタ操作手段と離れた位置に配置されたスレーブロボットと、マスタ操作手段の動作に応じてスレーブロボットを動作させるバイラテラル制御を行う制御手段と、スレーブロボットが動作する際の目標となる目標軌道を記憶する記憶手段を備える。制御手段は、スレーブロボットが記憶手段に記憶された目標軌道から逸脱すると、マスタ操作手段の操作に対し抵抗力が生じるように、マスタ操作手段の動作を制御する。【選択図】図１

Description

本発明は、バイラテラル制御を行うマスタスレーブシステムに関する。

操作者が操作を行うマスタ操作手段と、マスタ操作手段と離れた位置に配置されたスレーブロボットと、マスタ操作手段の動作に応じてスレーブロボットを動作させるバイラテラル制御を行う制御手段と、を備えるマスタスレーブシステムが知られている（例えば、特許文献１参照）。

特開２０１６−０２２５５９号公報

ところで、上述したバイラテラル制御では、操作者は、高い操作自由度で直感的にスレーブロボットを操作できる。しかし、未熟な操作者は、そのバイラテラル制御の高い操作自由度によって、スレーブロボットを誤操作する虞がある。

本発明は、かかる課題を解決するためになされたものであり、未熟な操作者でも、スレーブロボットを正確に操作することができるマスタスレーブシステムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、
操作者が操作を行うマスタ操作手段と、
前記マスタ操作手段と離れた位置に配置されたスレーブロボットと、
前記マスタ操作手段の動作に応じて前記スレーブロボットを動作させるバイラテラル制御を行う制御手段と、を備えるマスタスレーブシステムであって、
前記スレーブロボットが動作する際の目標となる目標軌道を記憶する記憶手段を備え、
前記制御手段は、前記スレーブロボットが前記記憶手段に記憶された目標軌道から逸脱すると、前記マスタ操作手段の操作に対し抵抗力が生じるように、該マスタ操作手段の動作を制御する、
ことを特徴とするマスタスレーブシステム
である。

本発明によれば、未熟な操作者でも、スレーブロボットを正確に操作することができるマスタスレーブシステムを提供することができる。

本発明の一実施形態に係るマスタスレーブシステムの概略的なシステム構成を示すブロック図である。 スレーブロボットの一例を示す図である。 スレーブロボットの把持部が行う動作の一例を示す図である。 目標軌道のボクセルの重み量を球の大きさで模擬的に表した図である。 ボクセルを球状体で表現した図である。 把持部の形状をカプセル形状あるいは角丸直方体で表現した図である。 カプセル型形状の把持部と球状体のボクセルとの距離を示す図である。 （ａ）カプセル状把持部及び球状ボクセルの一例を示す図である。（ｂ）図８（ａ）に示すカプセル状把持部及び球状ボクセルに対して算出した、各球状ボクセルの重み量の一例を示す図である。 本発明の一実施形態に係るマスタスレーブ制御処理のフローの一例を示すフローチャートである。 ベースが移動した場合のダンピング処理を説明するための図である。 肘部近傍に障害物が存在する環境下でのダンピング処理を示す図である。 （ａ）ダンピング処理により目標軌道を生成する方法を示す図である。（ｂ）ダンピング処理により目標軌道を生成する方法を示す図である。（ｃ）ダンピング処理により目標軌道を生成する方法を示す図である。

以下、図面を参照して本発明の実施形態について説明する。図１は、本発明の一実施形態に係るマスタスレーブシステムの概略的なシステム構成を示すブロック図である。本実施形態に係るマスタスレーブシステム１は、操作者が操作を行うマスタ操作部２と、マスタ操作部２と離れた位置に配置されたスレーブロボット３と、マスタ操作部２の動作に応じてスレーブロボット３を動作させるバイラテラル制御を行うバイラテラル制御部４と、記憶部５と、を備えている。

スレーブロボット３は、例えば、図２に示す如く、６つ以上の関節部（手首関節、肘関節、肩関節など）を有し、先端位置姿勢（手先位置姿勢など）を６自由度以上操作可能な多関節型のアームロボットとして構成されている。スレーブロボット３は、例えば、ベース部３１と、複数のリンク３２と、各リンク３２を回転可能に連結する関節部３３と、リンク３２に先端に設けられ対象物を把持操作する把持部３４と、を有している。

各関節部３３には、各関節部３３を駆動させるアクチュエータが設けられている。アクチュエータは、例えば、モータ駆動、油圧駆動、空気圧駆動などである。各関節部３３には、各関節部３３の回転角（以下、関節角）を検出する角度センサが設けられている。各関節部３３には、各関節部３３のトルク（以下、関節トルク）を検出するトルクセンサが設けられている。なお、スレーブロボット３は、複数の関節部（股関節、膝関節、足首関節など）を有する脚ロボットであってもよい。

スレーブロボット３には、ステレオカメラ、デプスカメラなどの環境認識センサ３５が設けられている。環境認識センサ３５は、例えば、スレーブロボット３のタスク遂行状態などのスレーブロボット３の周囲の環境情報を取得する。

マスタ操作部２は、マスタ操作手段の一具体例である。マスタ操作部２は、操作者がアームなどを介して操作情報を入力する操作部を有している。スレーブロボット３は、マスタ操作部２に入力された操作情報に応じて動作する。マスタ操作部２は、スレーブロボット３とは離れた位置に配置されている。マスタ操作部２は、スレーブロボット３の先端位置での力やモーメント、あるいは、各関節トルクなどの力情報を、マスタ操作部２の操作部を操作する操作者に対し、フィードバックする。

マスタ操作部２は、例えば、操作者がスレーブロボット３の手先位置を指示する手先指示装置、あるいは、操作者が装着する多関節型のマスタロボットである。マスタ操作部２は、例えば、複数のリンクと、各リンクを回転可能に連結する関節部と、リンクに先端に設けられ、操作者が把持操作する操作部と、を有する。各関節部には、各関節部を駆動させるアクチュエータが設けられている。アクチュエータは、例えば、モータ駆動、油圧駆動、空気圧駆動などである。各関節部には、各関節角を検出する角度センサが設けられている。各関節部には、各関節トルクを検出するトルクセンサが設けられている。

バイラテラル制御部４は、制御手段の一具体例である。バイラテラル制御部４は、マスタ操作部２の動作に倣ってスレーブロボット３を動作させるバイラテラル制御を実行する。バイラテラル制御部４は、バイラテラル制御において、操作者がマスタ操作部２を操作した際のマスタ操作部２の動作と同様に、スレーブロボット３を動作させる制御を行う。このとき、バイラテラル制御部４は、スレーブロボット３にかかる力情報を操作者にフィードバックする。すなわち、バイラテラル制御部４は、スレーブロボット３にかかる力と同様の力がマスタ操作部２に発生するように、マスタ操作部２を制御する。

例えば、バイラテラル制御部４は、マスタ操作部２の角度センサにより検出された各関節角に基づき、順運動学を用いて、目標とするスレーブロボット３の先端位置姿勢を算出する。バイラテラル制御部４は、算出した先端位置姿勢に基づき、逆運動学を用いて、スレーブロボット３の各関節部３３の目標関節角を算出する。バイラテラル制御部４は、算出した目標関節角に応じた指令信号をスレーブロボット３の各関節部３３のアクチュエータに出力してＰＩＤ制御などを行う。

同時に、バイラテラル制御部４は、スレーブロボット３の各関節部３３のトルクセンサにより検出された関節トルクに基づいて、スレーブロボット３の先端（手先など）にかかる先端力（荷重、モーメントなど）を算出する。バイラテラル制御部４は、算出した先端力に基づいて、マスタ操作部２の操作部にスレーブロボット３の先端力と同一の力が発生するように、マスタ操作部２の各関節部のアクチュエータを制御してＰＩＤ制御などを行う。

このようにして、バイラテラル制御部４は、バイラテラル制御において、操作者がマスタ操作部２を操作した際のマスタ操作部２の動作と同様に、スレーブロボット３を動作させつつ、スレーブロボット３にかかる力情報を、マスタ操作部２の操作部を介して操作者にフィードバックすることができる。なお、上記バイラテラル制御方法は一例であり、これに限定されない。

例えば、マスタ操作部２が、スレーブロボット３と類似の構成（関節部、リンクなどの構成）を有するマスタロボットとして構成されていてもよい。この場合、バイラテラル制御部４は、マスタロボットの各関節部と、対応するスレーブロボット３の各関節部３３と、を同期させるようなバイラテラル制御を行ってもよい。具体的には、バイラテラル制御部４は、マスタロボットの各関節部の角度センサにより検出された関節角及びトルクセンサにより検出された関節トルクを、スレーブロボット３の各関節部３３の目標関節角及び目標関節トルクに設定する。バイラテラル制御部４は、設定した目標関節角度及び目標関節トルクに応じた指令信号をスレーブロボット３の各関節部３３のアクチュエータに出力する。

同時に、バイラテラル制御部４は、スレーブロボット３の各関節部３３の角度センサにより検出された関節角及びトルクセンサにより検出された関節トルクを、マスタロボットの各関節部の目標関節角及び目標関節トルクに設定する。バイラテラル制御部４は、設定した目標関節角度及び目標関節トルクに応じた指令信号をマスタロボットの各関節部のアクチュエータに出力する。

以上により、操作者は、例えば、認識センサにより取得された環境情報（スレーブロボット３周囲の画像情報など）を参照し、マスタ操作部２を操作することで、スレーブロボット３をあたかも自らが直接操作しているかのような臨場感で、遠隔操作できる。同時に、操作者は、実際のスレーブロボット３に生じている反力を自ら感知することができる。

なお、バイラテラル制御部４は、は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）４１、ＣＰＵ４１によって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ４２、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）４３、などからなるマイクロコンピュータを中心にして、それぞれ、ハードウェア構成されている。ＣＰＵ４１、メモリ４２、及びインターフェイス部４３は、データバスなどを介して相互に接続されている。

ところで、上述したバイラテラル制御では、操作者は、高い操作自由度で直感的にスレーブロボットを操作できるというメリットを有している。しかしながら、スレーブロボット自体が高い自由度が動作できても、対象物が一定方向にしか動作しない場合も多く存在する。例えば、ドア、冷蔵庫、棚などの扉、タンスやロッカーなどの引出し、窓やシャッター、ペットボトルや瓶などの蓋、などの対象物は、一定方向にしか動作しない。

スレーブロボットが上記のような動作範囲に制約のある対象物を操作する場合、熟練した操作者は、バイラテラル制御の高い操作自由度を利用してスレーブロボットを動作範囲の制約内でより繊細に操作できる。しかし、未熟な操作者は、そのバイラテラル制御の高い操作自由度によって、その動作範囲の制約から外れてスレーブロボットを誤操作する虞がある。

これに対し、本実施形態に係るマスタスレーブシステム１は、スレーブロボット３が動作する際の目標となる目標軌道を記憶する記憶部５を備える。バイラテラル制御部４は、スレーブロボット３が記憶部５に記憶された目標軌道から逸脱すると、マスタ操作部２の操作に抵抗力が生じるようにマスタ操作部２の動作を制御する。

これにより、未熟な操作者がマスタ操作部２の操作を行い、スレーブロボット３が目標軌道から逸脱してもマスタ操作部２の操作に抵抗力が生じるため、スレーブロボット３が自ずと目標軌道へ戻るように、マスタ操作部の操作は修正される。したがって、未熟な操作者でも、目標軌道に従ってスレーブロボット３を正確に操作することができる。

本実施形態に係るマスタスレーブシステム１において、まず、スレーブロボット３が動作する際の目標となる目標軌道を記憶部５に記憶させる。記憶部５は、記憶手段の一具体例である。

例えば、熟練した操作者がマスタ操作部２を操作して、スレーブロボット３により対象物を移動させる。熟練した操作者がマスタ操作部２を操作することで、スレーブロボット３の理想的な軌道が生成される。このときのスレーブロボット３の把持部３４の軌道（以下、手先軌道）を目標軌道とする。

より具体的には、図３に示す如く、環境認識センサ３５は、スレーブロボット３の把持部３４がドアの取手（対象物）を把持し、ドアを開けるまでの把持部３４の動作を撮影する。バイラテラル制御部４は、環境認識センサ３５からの撮影画像に基づいて、把持部３４の特徴量を算出する。バイラテラル制御部４は、該算出した特徴量に基づいて、手先軌道を算出し、算出した手先軌道を目標軌道として記憶部５に記憶させる。

次に、スレーブロボット３が記憶部５に記憶された目標軌道から逸脱すると、マスタ操作部２に抵抗力が生じるように、マスタ操作部２の動作を制御する方法について説明する。

まず、記憶部５に記憶される目標軌道の設定方法について説明する。
上述のように記憶部５に記憶された目標軌道は、例えば、把持部３４が３次元的に通過する位置をボクセル形式で表現している。各ボクセルは、マスタ操作部２の操作に対する抵抗力を示す重み情報（重み量）を有している。その重み量は、把持部３４が各ボクセルを通過したときの接触度合に応じた値（例えば、０〜１）で正規化されている。ボクセルの粒度は、目標軌道を精度よく再現するために把持部形状に対して十分密に設定されている。また、以降の処理では時間情報を用いないため、ボクセルに対する時間情報は無視できる。図４は、目標軌道のボクセルの重み量を球の大きさで模擬的に表した図である。ボクセルの重み量は、スレーブロボット３の動作のし易さ、若しくは、動作制限（上述の抵抗力）を表している。

続いて、ボクセルの重み量の設定方法について詳細に説明する。
バイラテラル制御部４は、例えば、ボクセルを敷き詰めたマップ上を目標軌道に沿って把持部３４を移動させ、把持部３４と各ボクセルとの接触度合に応じて各ボクセルの重み量を算出する。

ここで、把持部３４と各ボクセルとの接触度合の計算を簡略化するため、例えば、図５に示す如く、ボクセルを立方体ではなく、該立方体を内包する球状体で表現してもよい。同様に、把持部３４の形状を、例えば、図６に示す如く、実際の形状を内包するような、カプセル形状あるいは角丸直方体（直方体の四方をカプセル形状で囲った形状）などの簡易形状で表現してもよい。

例えば、図７に示す如く、把持部３４の形状をカプセル形状とすれば、カプセル形状の把持部３４と球状体のボクセルとの距離は、線分と点との距離で算出でき、この距離を用いて把持部３４と各ボクセルとの接触度合を容易に表示できる。さらに、把持部３４の形状を角丸直方体とすれば、カプセル形状とＯＢＢ（Orient Bounding Box）という接触判定が容易な形状の組合せで、実際の把持部３４の形状に近い形でより正確に接触判定を行うことができる。

バイラテラル制御部４は、上記のように設定した簡易形状の把持部３４とその周囲の球状体のボクセルとの接触度合に応じて、各ボクセルの重み量ｗを、以下（１）−（４）のように算出する。本説明では、例えば、ボクセルと簡易形状をカプセル形状とする。

（１）バイラテラル制御部４は、カプセル形状の把持部３４（以下、カプセル状把持部３４）の２つの端点を結ぶ線分と、対象とする球状体のボクセル（以下、球状ボクセル）の中心との最近傍距離ｌを算出する。

（２）バイラテラル制御部４は、下記接触判定式（Ａ）−（Ｃ）を用いて、各球状ボクセルの重み量ｗを算出する。但し、球状ボクセルの半径をｒ_ｂ、カプセル状把持部３４の両端の球の半径をｒ_ｃとする。
（Ａ）ｌ＞ｒ_ｂ＋ｒ_ｃ（カプセル状把持部３４と球状ボクセルが接触しない場合）
：ｗ＝０
（Ｂ）ｒ_ｃ＞ｌ＋ｒ_ｃ（カプセル状把持部３４内に球状ボクセルが含まれる場合）
：ｗ＝１
（Ｃ）上記（Ａ）及び（Ｂ）以外（例えば、図７のように、カプセル状把持部３４と球状ボクセルが交わる場合）
：ｗ＝（ｒ_ｂ＋ｒ_ｃ−ｌ）／２ｒ_ｂ

図８（ａ）は、カプセル状把持部３４及び球状ボクセルの一例を示す図である。図８（ｂ）は、図８（ａ）に示すカプセル状把持部３４及び球状ボクセルに対して算出した、各球状ボクセルの重み量の一例を示す図である。なお、上記各球状ボクセルの重み量ｗの算出方法は一例であり、これに限定されない。例えば、カプセル状把持部３４と各球状ボクセルとが接触する体積比などの他の指標に応じて、各球状ボクセルの重み量ｗを設定してもよい。

（３）バイラテラル制御部４は、カプセル状把持部３４を目標軌道に沿って移動させつつ、上記（１）及び（２）を繰返し、目標軌道及びその周辺の各球状ボクセルの重み量ｗを設定する。

（４）バイラテラル制御部４は、各球状ボクセル（ｘ、ｙ、ｚ）において、最大の重み量ｗをその球状ボクセルの重み量して、ｗ（ｘ、ｙ、ｚ）とし、記憶部５に記憶させる。これにより、全球状ボクセルに、０〜１に正規化された重み量ｗ（ｘ、ｙ、ｚ）が設定される。なお、バイラテラル制御部４は、後段処理での不連続を抑制するために、近傍の球状ボクセル間で平均値を取ってフィルタをかける処理を行ってもよい。

以上のようにして、図４に示すような目標軌道のボクセルに対して重み量ｗ（ｘ、ｙ、ｚ）を設定することができる。

次に、上記のように球状ボクセルに重み量ｗ（ｘ、ｙ、ｚ）を設定した目標軌道を用いて、スレーブロボット３が目標軌道から逸脱すると、マスタ操作部２に抵抗力が生じるように、マスタ操作部２の動作を制御する方法について説明する。

例えば、１制御周期の微小時間において、操作者が、マスタ操作部２の操作部に対して、位置ｐ_ｎ−１（ｘ_ｎ−１、ｙ_ｎ−１、ｚ_ｎ−１）、姿勢（クォータニオン：Quaternion）ｑ_ｎ−１（ｓ_ｎ−１、ｕ_ｎ−１、ｖ_ｎ−１、ｗ_ｎ−１）から、位置ｐ_ｒ（ｘ_ｒ、ｙ_ｒ、ｚ_ｒ）、姿勢ｑ_ｒ（ｓ_ｒ、ｕ_ｒ、ｖ_ｒ、ｗ_ｒ）となるような操作入力を行った場合の処理を以下に説明する。

バイラテラル制御部４は、マスタ操作部２の各関節部の角度センサにより検出された関節角に基づいて、位置ｐ_ｒ及び姿勢ｑ_ｒに対応するスレーブロボット３の把持部３４の候補位置を算出する。

バイラテラル制御部４は、記憶部５の球状ボクセルに重み量ｗ（ｘ、ｙ、ｚ）を設定した目標軌道と、算出したスレーブロボット３の把持部３４の候補位置と、に基づいて、上記接触判定式（Ａ）−（Ｃ）を用いて、把持部３４と球状ボクセルとの接触判定を行う。バイラテラル制御部４は、上記接触判定により、把持部３４と接触する球状ボクセル群を抽出する。

バイラテラル制御部４は、抽出した球状ボクセル群の重み量ｗ（ｘ、ｙ、ｚ）の中から、最小の重み量をｗ_ｎとして算出する。

バイラテラル制御部４は、把持部３４が重み量の小さい球状ボクセルに接触するとダンピングが強く作用しマスタ操作部２の動作が抑制されるダンピング処理を行う。例えば、目標軌道上の球状ボクセルには、最大の重み量（ｗ＝１）が設定され、球状ボクセルが目標軌道から外れに従って、その球状ボクセルには徐々に小さな重み量（０＜ｗ＜１）が設定され、球状ボクセルが完全に目標軌道から外れると、球状ボクセルには、最小の重み量（ｗ＝０）が設定される。これにより、操作者は、把持部３４が目標軌道上にあるときは、マスタ操作部２の操作部に抵抗力をほとんど感じないが、把持部３４が目標軌道上から外れるに従がって、マスタ操作部２の操作部により強い抵抗力を感じ、把持部３４が目標軌道上から完全に外れると、マスタ操作部２の操作部に最大の抵抗力を感じる。

ここで、操作者が未熟であっても、あるい程度経験を積むと、操作ミスが減少する。このため、バイラテラル制御部４は、その経験度合に応じて重み量ｗ_ｎを増加させることで、上記ダンピング処理の抵抗力を減少させる処理を行ってもよい。これにより、操作者の現状のスキルにより適合した調整を行うことができる。例えば、抵抗力を半分にする場合、重み量ｗ_ｎのレンジが[０〜１]ではなく[０．５〜１]となるように、バイラテラル制御部４は、式（ｗ_ｎ＝０．５ｗ_ｎ＋０．５）を用いて重み量ｗ_ｎを補正してよい。
バイラテラル制御部４は、上述のように算出した重み量をｗ_ｎに基づいて、下記式１及び式２を用いて、スレーブロボット３の把持部３４の目標位置ｐ_ｎ及び目標姿勢ｑ_ｎを算出する。

なお、上記式１及び式２において、クォータニオンが用いられているが、これに限定されず、例えば、オイラー角などを用いてもよい。

バイラテラル制御部４は、算出した目標位置ｐ_ｎ及び目標姿勢ｑ_ｎとなるように、スレーブロボット３の各関節部３３のアクチュエータを制御しつつ、スレーブロボット３の手先位置及び姿勢に対応させて、マスタ操作部２の操作部の位置及び姿勢をバイラテラル制御する。

なお、重み量ｗ_ｎからｗ_ｎ−１に急激に変化すると操作者が感じる抵抗力も不連続に変化することとなり、操作感が悪い。このため、バイラテラル制御部４は、重み量ｗ_ｎに時間的なローパスフィルタを掛ける処理や、近傍の球状ボクセル間の重み量との空間的なフィルタを掛ける処理、などを行い、この急激な変化を抑制してもよい。

バイラテラル制御部４は、上記ダンピング処理において、把持部３４が目標軌道上にある場合、操作者がマスタ操作部２の操作部を操作し易くし（抵抗力を低くし）、把持部３４が目標軌道上から外れるに従がって、操作者がマスタ操作部２の操作部を操作し難くなる（抵抗力が高くなる）ような制御を行っている。

さらに、バイラテラル制御部４は、把持部３４が目標軌道上から外れた場合に、把持部３４が目標軌道により戻り易くする制御を行ってもよい。これにより、操作者は、把持部３４が目標軌道上から外れた場合でも、その軌道を容易に修正し目標軌道へ戻すことができる。

例えば、バイラテラル制御部４は、ｗ′_ｎ＞０の場合（ｗ_ｎがｗ_ｎ−１より増加している場合）、下記式を用いて、ｗ_ｄを算出する。ｗ′_ｎは、ｗ′_ｎの微分値である。下記式において、βは、β＞０の定数とし、予めメモリなどに設定されている。
ｗ_ｄ＝ｗ_ｎ（１＋βｗ′_ｎ）

バイラテラル制御部４は、算出したｗ_ｄを上記式１及び式２のｗ_ｎに代入して、スレーブロボット３の把持部３４の目標位置ｐ_ｎ及び目標姿勢ｑ_ｎを算出する。

なお、バイラテラル制御部４は、慣性項を導入した上記式１及び式２を用いて、目標軌道との偏差量に仮想バネ定数を乗じることで目標軌道に引き戻すように、スレーブロボット３の把持部３４の目標位置ｐ_ｎ及び目標姿勢ｑ_ｎを算出してもよい。

バイラテラル制御部４は、上記把持部３４と球状ボクセルとの接触判定において接触すると判定された記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）を増加させて更新を行ってもよい。これにより、単に熟練者などの目標軌道から逸脱し難くなるだけでなく、逸脱方向に繰返し操作すると次第にダンピング（抵抗力）が減少し、意図的に、熟練者などの目標軌道とは違う軌道を生成することができる。

例えば、バイラテラル制御部４は、上記把持部３４と球状ボクセルとの接触判定において接触すると判定された記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）に、所定係数を加算あるいは乗算することで、その重み量ｗ（ｘ、ｙ、ｚ）を増加させてもよい。

さらに、バイラテラル制御部４は、記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）に、その接触度合に応じた所定係数を加算あるいは乗算することで、その重み量ｗ（ｘ、ｙ、ｚ）を増加させてもよい。例えば、接触度合が大きくなるに従がって、所定係数を減少させてもよい。なお、増加後の重み量ｗ（ｘ、ｙ、ｚ）は１以下とする。所定係数は、メモリなどに予め設定され、操作者などが適宜設定変更できる。

上述したように、ダンピングを可変させることで、熟練者などによる目標軌道をある程度踏襲して安全性を確保しつつ、現在の操作者による臨機応変な操作も反映でき、マスタスレーブ操作法が持つ優位性を生かしたより柔軟な操作が実現できる。

例えば、熟練者などによる目標軌道の生成時におけるドアの大きさと、操作者による現在の操作時におけるドアの大きさとが異なる場合などの、目標軌道の生成時の環境と現在の環境とが異なる場合が想定される。このような場合でも、上記ダンピングを可変させ、目標軌道からの逸脱を効果的に緩和することで、操作者は操作遂行できる。

さらに、上記ダンピングの可変処理を繰り返すと、各球状ボクセルの重み量が熟練者などにより設定された初期値から徐々に大きく外れていく。したがって、バイラテラル制御部４は、記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）に、例えば、時間係数を乗算するなどを行うことで、該重み量を時間経過と共に元の初期値に戻す処理を行ってもよい。これにより、上記ダンピングの可変処理を繰り返しても、各球状ボクセルの重み量が熟練者などにより設定された初期値から大きく外れるのを抑制できる。

図９は、本実施形態に係るマスタスレーブ制御処理のフローの一例を示すフローチャートである。

例えば、マスタスレーブシステム１に対し、次の初期設定が行われる（ステップＳ１０１）。スレーブロボット３が動作する際の目標となる目標軌道を記憶部５に記憶させる。バイラテラル制御部４は、記憶部５に記憶された目標軌道の各球状ボクセルの重み量ｗを設定する。バイラテラル制御部４には、上記ダンピング処理の開始及び終了のトリガが設定される。より具体的には、バイラテラル制御部４は、操作者の視線入力、ジェスチャ、音声コマンドなどの所定の物理入力を受けると、その物理入力を上記ダンピング処理の開始のトリガ（以下、開始トリガ）、あるいは、上記ダンピング処理の終了のトリガ（以下、終了トリガ）として、該ダンピング処理を開始あるいは終了する。さらに、バイラテラル制御部４は、ドアを検出及びスレーブロボット３の把持部３４がドアノブを把持したことを検出すると、その検出を上記ダンピング処理の開始トリガとして、該ダンピング処理を自動的に開始してもよい。バイラテラル制御部４は、ドアの開状態を検出及びスレーブロボット３の把持部３４がドアノブを離したことを検出すると、その検出を上記ダンピング処理の終了トリガとして、該ダンピング処理を自動的に終了してもよい。

バイラテラル制御部４は、操作者によるマスタ操作部２の操作部の操作に応じて、次の暫定のスレーブロボット３の把持部３４の目標位置ｐ_ｒ及び目標姿勢ｑ_ｒを算出する（ステップＳ１０２）。

バイラテラル制御部４は、ダンピング処理中であるか否かを判定する（ステップＳ１０３）。

バイラテラル制御部４は、ダンピング処理中であると判定したとき、所定の物理入力などに基づいて、本ダンピング処理を解除（終了）するか否かを判定する（ステップＳ１０４）。

バイラテラル制御部４は、本ダンピング処理を解除しないと判定すると（ステップＳ１０４のＮＯ）、ダンピング処理を継続する。バイラテラル制御部４は、記憶部５の目標軌道と、算出したスレーブロボット３の把持部３４の候補位置と、に基づいて、把持部３４と球状ボクセルとの接触判定を行い、把持部３４と接触する球状ボクセル群を抽出する（ステップＳ１０５）。一方、バイラテラル制御部４は、本ダンピング処理を解除すると判定すると（ステップＳ１０４のＹＥＳ）、下記の（ステップＳ１１１）に移行する。

バイラテラル制御部４は、抽出した球状ボクセル群の重み量ｗ（ｘ、ｙ、ｚ）の中から、最小の重み量をｗ_ｎとして算出する（ステップＳ１０６）。

バイラテラル制御部４は、算出した重み量をｗ_ｎに基づいて、上記式１及び式２を用いて、スレーブロボット３の把持部３４の目標位置ｐ_ｎ及び目標姿勢ｑ_ｎを算出する（ステップＳ１０７）。

バイラテラル制御部４は、上記把持部３４と球状ボクセルとの接触判定において接触すると判定された記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）の更新を行う（ステップＳ１０８）。

バイラテラル制御部４は、記憶部５の球状ボクセルの重み量ｗ（ｘ、ｙ、ｚ）を時間経過と共に元の初期値に戻す処理を行い（ステップＳ１０８）、後述の処理（ステップＳ１１２）に移行する。

バイラテラル制御部４は、ダンピング処理中でないと判定したとき（ステップＳ１０３のＮＯ）、上記ダンピング処理の開始トリガに基づいて、本ダンピング処理を開始するか否かを判定する（ステップＳ１１０）。バイラテラル制御部４は、上記ダンピング処理の開始トリガに基づいて、本ダンピング処理を開始すると判定すると（ステップＳ１１０のＹＥＳ）、上記（ステップＳ１０５）に移行する。一方、バイラテラル制御部４は、上記ダンピング処理の開始トリガに基づいて、本ダンピング処理を開始しないと判定すると（ステップＳ１１０のＮＯ）、目標位置ｐ_ｎ＝ｐ_ｒ及び目標姿勢ｑ_ｎ＝ｑ_ｒに設定する（ステップＳ１１１）。

バイラテラル制御部４は、算出した目標位置ｐ_ｎ及び目標姿勢ｑ_ｎとなるように、スレーブロボット３の各関節部３３のアクチュエータを制御しつつ、スレーブロボット３の手先位置及び姿勢に対応させて、マスタ操作部２の操作部の位置及び姿勢をバイラテラル制御する（ステップＳ１１２）。

バイラテラル制御部４は、ｎをインクリメントし（ステップＳ１１３）、上記（ステップＳ１０２）に移行する。

以上、本実施形態に係るマスタスレーブシステム１は、スレーブロボット３が動作する際の目標となる目標軌道を記憶する記憶部５を備える。バイラテラル制御部４は、スレーブロボット３が記憶部５に記憶された目標軌道から逸脱すると、マスタ操作部２の操作に抵抗力が生じるようにマスタ操作部２の動作を制御する。これにより、スレーブロボット３が目標軌道から逸脱してもマスタ操作部２の操作に抵抗力が生じるため、スレーブロボット３が自ずと目標軌道へ戻るように、マスタ操作部の操作は修正される。したがって、未熟な操作者でも、目標軌道に従ってスレーブロボット３を正確に操作することができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
上記実施形態において、スレーブロボット３は、ベースなどに固定されているが、これに限定されない。例えば、スレーブロボット３は車輪や脚部を有し、移動する構成であってもよい。図１０に示す如く、ベースが移動しても、ベースと対象物との相対位置関係から、上記ベースが固定された場合と同様に、ダンピング処理を行い、同様の効果を得ることができる。

上記実施形態において、バイラテラル制御部４は、スレーブロボット３の把持部３４（手先）と球状ボクセルとの接触判定を行い重み量ｗ_ｎを算出して、把持部３４の目標位置ｐ_ｎ及び目標姿勢ｑ_ｎを算出しているが、これに限定されない。バイラテラル制御部４は、スレーブロット把持部３４と同様に、肘部と球状ボクセルとの接触判定も行い重み量を算出して、肘部の目標位置ｐ_ｎ及び目標姿勢ｑ_ｎも算出してもよい。これにより、例えば、図１１に示すような肘部近傍に障害物が存在する環境下においても、予め熟練者がバイラテラル制御の冗長自由度を利用してその障害物を回避するような操作を行った目標軌道に沿って、未熟者も安全かつ正確に複雑環境下でのスレーブロボット３の遠隔操作を実現できる。

上記実施形態において、バイラテラル制御部４は、上記ダンピング処理を行って、目標軌道を生成してもよい。
ここで、熟練者にとっても、未経験の環境下でスレーブロボット３を遠隔操作するのが困難となる場合がある。そこで、図１２（ａ）に示す如く、初めに、バイラテラル制御部４は、僅かな重み量の球状ボクセルが設定された目標軌道に基づいて、把持部３４と球状ボクセルとの接触判定を行い、把持部３４の目標位置ｐ_ｎ及び目標姿勢ｑ_ｎを算出して、バイラテラル制御を行う。このとき、ダンピング処理効果によって、マスタ操作部２の操作は低速度となるが、誤操作が減少し正確な動作が期待できる。そして、バイラテラル制御部４は、ダンピング処理を繰り返すことで、各球状ボクセルの重み量を増加させ更新する。これにより、図１２（ｂ）に示す如く、把持部３４の軌道上及び周辺の各球状ボクセルの重み量が増加し、最終的に、図１２（ｃ）に示す如く、目標軌道が生成される。このように、バイラテラル制御部４は、僅かな重み量の球状ボクセルの目標軌道から、上記ダンピング処理及び各球状ボクセルの重み量の更新を繰返し、目標軌道を生成する。

本発明は、例えば、図９に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１ マスタスレーブシステム、２ マスタ操作部、３ スレーブロボット、４ バイラテラル制御部、５ 記憶部、３１ ベース部、３２ リンク、３３ 関節部、３４ 把持部、３５ 環境認識センサ

Claims (1)

1. 操作者が操作を行うマスタ操作手段と、
   前記マスタ操作手段と離れた位置に配置されたスレーブロボットと、
   前記マスタ操作手段の動作に応じて前記スレーブロボットを動作させるバイラテラル制御を行う制御手段と、を備えるマスタスレーブシステムであって、
   前記スレーブロボットが動作する際の目標となる目標軌道を記憶する記憶手段を備え、
   前記制御手段は、前記スレーブロボットが前記記憶手段に記憶された目標軌道から逸脱すると、前記マスタ操作手段の操作に対し抵抗力が生じるように、該マスタ操作手段の動作を制御する、
   ことを特徴とするマスタスレーブシステム。

Images