マスタ操作入力装置及びマスタスレーブマニピュレータ
【課題】より故障に強い構成で操作部の位置・姿勢を検出することが可能なマスタ操作入力装置及びそのようなマスタ操作入力装置を備えるマスタスレーブマニピュレータを提供すること。
【解決手段】マスタ操作入力装置の入力部に設けられた把持部の位置・姿勢の指令値を、把持部に取り付けられた関節の駆動量を検出するエンコーダの出力信号に基づいて算出するとともに、撮像センサで得られた画像に基づいて算出する。エンコーダの出力信号に従って算出された指令値と撮像センサで得られた画像に従って算出された指令値との間に所定以上の差がある場合にエラー処理を行う。
【解決手段】マスタ操作入力装置の入力部に設けられた把持部の位置・姿勢の指令値を、把持部に取り付けられた関節の駆動量を検出するエンコーダの出力信号に基づいて算出するとともに、撮像センサで得られた画像に基づいて算出する。エンコーダの出力信号に従って算出された指令値と撮像センサで得られた画像に従って算出された指令値との間に所定以上の差がある場合にエラー処理を行う。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、スレーブマニピュレータを遠隔操作するためのマスタ操作入力装置及びそのようなマスタ操作入力装置を有するマスタスレーブマニピュレータに関する。
【背景技術】
【０００２】
近年、医療施設の省人化を図るため、ロボットによる医療処置の研究が行われている。特に、外科分野では、多自由度（多関節）アームを有するマニピュレータによって患者の処置をするマニピュレータシステムについての各種の提案がなされている。このようなマニピュレータシステムにおいて、患者の体腔に直接接触するマニピュレータ（スレーブマニピュレータ）を、スレーブマニピュレータに対して遠隔に配置されたマスタ操作入力装置によって遠隔操作できるようにしたマニピュレータシステム（マスタスレーブマニピュレータ）が知られている。通常、マスタスレーブマニピュレータにおいては、マスタ操作入力装置の操作部の位置・姿勢が指令値として制御装置に入力される。制御装置においては操作部の位置・姿勢の指令値に従ってスレーブマニピュレータの先端部の逆運動学を解くことによってスレーブマニピュレータの各関節の駆動量が算出され、この駆動量に従って各関節が駆動制御される。このような制御を行うためには、マスタ操作入力装置の操作部の位置・姿勢を検出することが必要である。従来、マスタ操作入力装置の位置・姿勢は、マスタ操作入力装置の操作部に取り付けられたセンサにより検出されている。
【０００３】
ここで、操作部の位置・姿勢を検出するためのセンサを１系統のみとすると、この１系統のセンサが故障した場合には位置・姿勢が検出できなくなってしまう。これを回避するための手法として、センサを二重化する手法が知られている（例えば、特許文献１参照）。特許文献１の技術をマスタ操作入力装置に対して適用することにより、１つの系統のセンサが故障しても別の系統のセンサを用いてマスタ操作入力装置の操作部の位置・姿勢を検出することが可能となる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平６−１６８５３０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ここで、特許文献１の技術の場合、二重化したセンサはともに同じ物理量（例えば特許文献１の場合にはピッカの変位量）を検出している。この場合、一方の系統のセンサが故障する際に、もう一方の系統のセンサも同時に故障してしまう可能性が高い。
本発明は、上記の事情に鑑みてなされたもので、より故障に強い構成で操作部の位置・姿勢を検出することが可能なマスタ操作入力装置及びそのようなマスタ操作入力装置を備えるマスタスレーブマニピュレータを提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記の目的を達成するために、本発明の第１の態様のマスタ操作入力装置は、複数の自由度に対応した関節を有するスレーブマニピュレータを操作するためのマスタ操作入力装置であって、操作者による操作によって位置及び姿勢を変化自在であって、該位置及び姿勢の変化によって前記スレーブマニピュレータの位置及び姿勢の指令値を与えるための操作部と、前記操作部の位置及び姿勢を検出するために、前記操作部に係る異なる物理量をそれぞれ検出する２系統以上の検出部と、を具備することを特徴とする。
【０００７】
また、上記の目的を達成するために、本発明の第２の態様のマスタスレーブマニピュレータは、第１の態様に記載のマスタ操作入力装置と、前記２系統以上の検出部のそれぞれで検出された物理量に従って前記操作部の位置及び姿勢を複数算出する第１の制御部と、前記第１の制御部によって算出された前記操作部の位置及び姿勢から前記スレーブマニピュレータの位置及び姿勢の指令値を算出し、該算出したスレーブマニピュレータの位置及び姿勢の指令値に従って前記スレーブマニピュレータを駆動制御する第２の制御部と、を有することを特徴とする。
【発明の効果】
【０００８】
本発明によれば、より故障に強い構成で操作部の位置・姿勢を検出することが可能なマスタ操作入力装置及びそのようなマスタ操作入力装置を備えるマスタスレーブマニピュレータを提供することが可能である。
【図面の簡単な説明】
【０００９】
【図１】本発明の第１の実施形態に係るマスタスレーブマニピュレータの一例の全体構成を示す図である。
【図２】本発明の第１の実施形態におけるマスタ操作入力装置の操作部の位置・姿勢を検出するためのセンサの二重化について示した図である。
【図３】本発明の第１の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。
【図４】本発明の第１の実施形態においてマーカと撮像センサの配置の関係を逆転させた変形例を示す図である。
【図５】本発明の第２の実施形態におけるマスタ操作入力装置の操作部の位置・姿勢を検出するためのセンサの二重化について示した図である。
【図６】本発明の第２の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。
【図７】本発明の第２の実施形態においてマーカと撮像センサの配置の関係を逆転させた変形例を示す図である。
【図８】本発明の第３の実施形態におけるマスタ操作入力装置の操作部の位置・姿勢を検出するためのセンサの二重化について示した図である。
【図９】本発明の第３の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。
【図１０】本発明の第４の実施形態におけるマスタ操作入力装置の操作部の位置・姿勢を検出するためのセンサの二重化について示した図である。
【図１１】本発明の第４の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。
【発明を実施するための形態】
【００１０】
以下、図面を参照して本発明の実施形態を説明する。
［第１の実施形態］
まず、本発明の第１の実施形態について説明する。図１は、本発明の第１の実施形態に係るマスタスレーブマニピュレータの一例の全体構成を示す図である。図１に示すように、本実施形態に係るマスタスレーブマニピュレータは、マスタ操作入力装置１０と、制御装置２０と、スレーブマニピュレータ３０と、を有している。
【００１１】
マスタ操作入力装置１０は、本マスタスレーブマニピュレータにおけるマスタとして機能するものであって、入力部１１と、表示部１２と、撮像センサ１３とを有している。
入力部１１は、例えばマスタ操作入力装置１０の表示部１２に固定されており、操作者１の操作を受けてスレーブマニピュレータ３０を操作するための信号を出力する。入力部１１の詳細については後述する。
【００１２】
表示部１２は、例えば液晶ディスプレイから構成され、制御装置２０から入力された画像信号に基づいて画像を表示する。後述するが、制御装置２０から入力される画像信号は、スレーブマニピュレータに取り付けられた電子カメラ（電子内視鏡）を介して得られた画像信号を、制御装置２０において処理したものである。このような画像信号に基づく画像を、表示部１２に表示させることにより、マスタ操作入力装置１０の操作者１は、マスタ操作入力装置１０から離れた場所に配置されたスレーブマニピュレータ３０の先端の画像を確認することが可能である。
【００１３】
撮像センサ１３は、例えば所定の間隔を空けて配置された２眼の撮像センサである。この撮像センサ１３は、撮像により、所定の視差を有する２枚の入力部１１の画像を生成する。撮像センサ１３の詳細については後述する。
制御装置２０は、マスタ制御部２１と、マニピュレータ制御部２２と、画像処理部２３と、を有している。
第１の制御部の一例として機能するマスタ制御部２１は、マスタ操作入力装置１０からの信号に従って、マスタ操作入力装置１０の入力部１１の先端の位置・姿勢を算出する。後述するが、本実施形態では、入力部１１の先端の位置・姿勢を算出するために、それぞれが異なる物理量を検出する２系統以上の検出部（センサ）が設けられている。マスタ制御部２１は、これら２系統以上のセンサのそれぞれからの出力に従って入力部１１の先端の位置・姿勢を算出する。そして、マスタ制御部２１は、算出した入力部１１の先端の位置・姿勢をスレーブマニピュレータの先端の位置・姿勢の指令値としてマニピュレータ制御部２２に出力する。
【００１４】
第２の制御部の一例として機能するマニピュレータ制御部２２は、マスタ制御部２１からの位置・姿勢の指令値を受けて、スレーブマニピュレータ３０の先端の位置・姿勢を指令値に一致させるために必要なスレーブマニピュレータの各関節の駆動量を、例えば逆運動学計算によって算出する。そして、マニピュレータ制御部２２は、算出した駆動量に従ってスレーブマニピュレータの各関節を駆動させる。
【００１５】
画像処理部２３は、スレーブマニピュレータの先端に設けられた電子カメラ（電子内視鏡等）から得られた画像信号を処理し、表示部１２の表示用の画像信号を生成して表示部１２に出力する。
スレーブマニピュレータ３０は、複数の自由度に対応した複数の関節を有している。このスレーブマニピュレータ３０は、マニピュレータ制御部２２からの制御信号に従って各関節が駆動される。
【００１６】
図２は、本実施形態におけるマスタ操作入力装置の入力部１１の位置・姿勢を検出するためのセンサの二重化について示した図である。
入力部１１は、その先端に操作部としての把持部１００が設けられている。そして、把持部１００には、複数のリンクがそれぞれ関節を介して連接されている。図２に示す関節は、回転関節と直動関節とにより構成されている。操作者１が、把持部１００を把持して操作することにより、その操作に応じて各関節が回転又は直動する。ここで、図２は、入力部１１が、４つの回転関節と２つの直動関節の６関節を有している例を示しており、これらの６関節によって把持部１００の自由度を６自由度（位置の３自由度＋姿勢の３自由度）とすることが可能である。なお、図２のリンク構造は一例であり、回転関節や直動関節の数が異なる他のリンク構造であっても良い。操作部の自由度も６とは限らなくても良い。また、図２は、右手用の操作部のみを示している。左手用の操作部の構成は、右手用の操作部に対して左右の関係が逆転するだけで実質的な構成は図２に示すものと同様である。
【００１７】
また、それぞれの関節の近傍には、関節の回転量又は直動量を検出するためのセンサ（例えばエンコーダ）２００、２０２、２０４、２０６、２０８、２１０が設けられている。各エンコーダの出力信号は、マスタ制御部２１に入力される。図２では、エンコーダ２００は、把持部１００から見て最も遠い位置に設けられた関節の回転量θ１に応じた信号を出力するセンサである。エンコーダ２０２は、把持部１００から見て次に遠い位置に設けられた関節の直動量ｄ１に応じた信号を出力するセンサである。以下同様に、エンコーダ２０４は関節の直動量ｄ２に応じた信号を出力するセンサであり、エンコーダ２０６は関節の回転量θ２に応じた信号を出力するセンサであり、エンコーダ２０８は関節の回転量θ３に応じた信号を出力するセンサである。そして、エンコーダ２１０は、把持部１００から見て最も近い位置に設けられた関節の回転量θ４に応じた信号を出力するセンサである。
【００１８】
さらに、把持部１００には、３つの反射マーカ（以下、マーカという）４５０、４５２、４５４が設けられている。各マーカは、撮像センサ１３で得られた画像中で高輝度となるよう、高反射率の材料から形成されている。ここで、マーカ４５０とマーカ４５２との間隔、マーカ４５２とマーカ４５４との間隔、及びマーカ４５４とマーカ４５０との間隔がそれぞれ異なるように、反射マーカ４５０、４５２、４５４を把持部１００上に配置しておくことが望ましい。また、３つのマーカ４５０、４５２、４５４が一直線上に並ばないような配置で、さらに、把持部１００即ち操作部の姿勢を変えてもマーカが撮像センサ１３に認識できるような配置しておくことが望ましい。これは、画像上でマーカ４５０、４５２、４５４を抽出する際に、それぞれのマーカを正しく識別できるようにするためである。撮像センサ１３で取得する操作部の特徴点は反射マーカに限らない。例えば、ＬＥＤ等の発光体であっても良い。
【００１９】
撮像センサ１３は、２眼の撮像系４０２、４０４を有している。撮像系４０２、４０４は、それぞれが、撮像光学系と、撮像素子とを有しており、入力部１１に対して所定距離だけ離れて配置されている。また、撮像系４０２と撮像系４０４も予め定められた距離だけ離間するように配置されている。
【００２０】
撮像光学系は、入射した光束を撮像素子の受光面上に結像させるための光学系である。撮像素子は、撮像光学系を介して入射した光束を電気信号に変換して把持部１００の位置・姿勢を得るための画像を生成するセンサである。撮像素子で生成された画像は、マスタ制御部２１に入力される。
【００２１】
以下、本実施形態のマスタスレーブマニピュレータの動作について説明する。図３は、本発明の第１の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。図３は、特にマスタ制御部２１の動作について示している。
まず、マスタスレーブマニピュレータの初期設定が行われる（ステップＳ１）。この初期設定において、例えば操作者１は、把持部１００の位置・姿勢を予め定められた初期の位置・姿勢に設定する。この後、操作者１は、撮像センサ１３の撮像系４０２、４０４を動作させて、マーカ４５０、４５２、４５４を含む把持部１００の画像を取得する。把持部１００の位置・姿勢の設定を自動的に行えるようにしても良い。
【００２２】
マスタ制御部２１は、撮像系４０２、４０４のそれぞれを介して得られた画像から特徴点としてのマーカ４５０、４５２、４５４を抽出する。画像内のマーカ４５０、４５２、４５４を抽出する手法としては例えば周知のパターンマッチング法を用いることが可能である。
【００２３】
画像内のマーカ４５０、４５２、４５４の抽出後、マスタ制御部２１は、取得した画像内に３次元の座標系（カメラ座標系）Ｃを設定し、このカメラ座標系Ｃに対応して初期設定時の把持部１００の位置・姿勢を示す姿勢行列Ｐを算出する。例えば、カメラ座標系Ｃとして、画像の水平方向をＸ軸、垂直方向をＹ軸、奥行き方向をＺ軸とし、マーカ４５４の座標を原点に設定する。このとき、姿勢行列Ｐは、カメラ座標系Ｃ内に設定したＸ軸の方向を示す単位ベクトルと、Ｙ軸の方向を示す単位ベクトルと、Ｚ軸の方向を示す単位ベクトルを用いた３×３の行列、もしくは、位置ベクトルも含めた４×４の行列で表わすことができる。
【００２４】
姿勢行列Ｐを算出した後、マスタ制御部２１は、カメラ座標系Ｃの姿勢行列Ｐをワールド座標系（例えば、地表を基準とした座標系）Ｗで見た姿勢行列とするための変換行列Ｑを算出する。変換行列Ｑの算出により、初期設定が終了する。
初期設定の後、操作者１が把持部１００を操作すると、その操作に応じて把持部１００の位置・姿勢が変化し、この位置・姿勢の変化に応じてエンコーダ２００〜２１０からの出力信号が変化する。マスタ制御部２１は、エンコーダ２００〜２１０からの出力信号によって示される直動量、回転量を用いて運動学計算を解くことにより、把持部１００の位置・姿勢の指令値Ａを算出する（ステップＳ２）。
【００２５】
また、マスタ制御部２１は、撮像センサ１３で得られた画像から把持部１００の位置・姿勢の指令値Ｂを算出する（ステップＳ３）。
画像内の把持部１００の位置変化（変位量）は、位置・姿勢の変化前に撮像センサ１３で得られた把持部１００の画像と位置・姿勢の変化後に撮像センサ１３で得られた把持部１００の画像との間での原点位置のマーカ（例えばマーカ４５４）の位置変化から算出することが可能である。ここで、Ｚ方向の位置変化は、撮像系４０２と撮像系４０４との視差によって発生する、撮像系４０２を介して得られる画像におけるマーカ４５４の位置と撮像系４０４を介して得られる画像におけるマーカ４５４の位置とのずれ量から算出することが可能である。このずれ量を算出する手法としては、例えば画像相関法を用いることが可能である。
【００２６】
また、画像内の把持部１００の姿勢変化（回転量）は、位置・姿勢の変化前に撮像センサ１３で得られた把持部１００の画像と位置・姿勢の変化後に撮像センサ１３で得られた把持部１００の画像との間での原点位置のマーカ（例えばマーカ４５４）に対する残りのマーカの位置変化から算出することが可能である。
上述の例では、２眼の撮像系を用いているが、１眼の撮像系でも良い。この場合、奥行座標は撮像センサ１３に映るマーカの大きさから取得でき、さらに、同一直線状に並ばない３つのマーカを用いれば操作部の位置・姿勢を取得することができる。この技術については公知であるため、詳細は記載しない。
【００２７】
カメラ座標系Ｃで見た把持部１００の位置・姿勢の指令値を算出した後、マスタ制御部２１は、把持部１００の位置の変化量によって表わされる平行移動を示す行列と、把持部１００の回転量によって表わされる回転行列とを、姿勢行列Ｐに左から乗じることにより、位置・姿勢の変化後のカメラ座標系Ｃで見た姿勢行列Ｐ’を算出する。その後、マスタ制御部２１は、姿勢行列Ｐ’に変換行列Ｑを乗じることにより、ワールド座標系で見た把持部１００の位置・姿勢の指令値Ｂを算出する。
【００２８】
位置・姿勢の指令値Ａ、Ｂの算出後、マスタ制御部２１は、指令値Ａと指令値Ｂとの差の絶対値が所定値Ｔｈ以下であるか否かを判定する（ステップＳ４）。指令値Ａと指令値Ｂは、ともに操作者１による操作後の把持部１００の位置・姿勢を表わすものであり、本来は略一致するものである。このため、指令値Ａと指令値Ｂとの差の絶対値が所定値Ｔｈ以下であるか否かを判定することにより、エンコーダ２００〜２１０と撮像センサ１３の何れかが故障しているか否かを判定することが可能である。
【００２９】
ステップＳ４の判定において、指令値Ａと指令値Ｂとの差の絶対値が所定値Ｔｈ以下である場合に、マスタ制御部２１は、指令値Ａと指令値Ｂの何れか（又は両者の平均値）をマニピュレータ制御部２２に出力する（ステップＳ５）。これを受けて、マニピュレータ制御部２２は、スレーブマニピュレータ３０の先端部の位置・姿勢をマスタ制御部２１から入力された位置・姿勢の指令値とするために必要な各関節の駆動量を、逆運動学計算を解くことにより算出する。その後、マニピュレータ制御部２２は、算出した駆動量に従ってスレーブマニピュレータ３０の各関節を駆動制御する。このようにしてマスタスレーブマニピュレータの一連の動作が終了する。
【００３０】
また、ステップＳ４の判定において、指令値Ａと指令値Ｂとの差の絶対値が所定値Ｔｈを越えている場合に、マスタ制御部２１は、エラー処理を行う（ステップＳ６）。エラー処理とは、例えば算出した指令値をマニピュレータ制御部２２に送らないようにする処理や、エンコーダ２００〜２１０と撮像センサ１３の何れかが故障していることを操作者１に警告する処理、マスタスレーブマニピュレータのシステム自体を遮断してしまう処理等が考えられる。このエラー処理の場合にも、マスタスレーブマニピュレータの一連の動作が終了する。
【００３１】
以上説明したように、本実施形態によれば、入力部１１に設けられた関節の変位量及び回転量を検出する第１のセンサとしてのエンコーダと、撮像によって画像を得る第２のセンサとしての撮像センサと、いう異なる物理量をそれぞれ検出する２系統のセンサから把持部１００の位置・姿勢の指令値を算出している。これにより、例えば故障の検出が容易である。
【００３２】
また、図３では指令値Ａと指令値Ｂとに所定以上の差がある場合にはエラー処理を行うようにしているが、その後の検査によりエンコーダの出力信号と撮像センサの何れが故障しているかが判別された場合には、その後は故障していない側を用いてスレーブマニピュレータ３０の駆動制御を行うこともできる。このため、故障にも強い。
【００３３】
先行技術のように、センサの二重化として同一関節上に、同じ物理量を取得するために２種類のセンサ（例えばエンコーダとリミットスイッチやエンコーダとポテンショメータの組み合せ）を配置することがあったが、この場合、熱や水没で同時に２つのセンサが故障することがあった。本実施形態のように、撮像センサで得られた画像から把持部１００の位置・姿勢の指令値を算出する場合、入力部１１の関節上に撮像センサを配置する必要がない。このため、エンコーダと撮像センサが同時に故障するような事態も回避することが可能である。
ここで、上述の例では、入力部１１の把持部１００上に３つのマーカ４５０、４５２、４５４を設け、撮像センサ１３を入力部１１から離して配置している。これに対し、図４に示すように、把持部１００に撮像センサ４１０を設け、マーカ４６０、４６２、４６４を入力部１１から離して配置するようにしても良い。さらに、マーカを省略しても良い。この場合には、画像上の特徴点は、例えば把持部１００の輪郭部分として、操作部の位置・姿勢を取得する。
【００３４】
また、上述の例では、画像内でマーカ４５０、４５２、４５４を識別するために、マーカ間の距離を異ならせるようにしているが、例えばマーカの反射率や直径、色を異ならせるようにしても良い。マーカの代わりにＬＥＤのような発光体を撮像センサに認識させる場合は、発光パターン（発光間隔）を変化させても良い。このようなマーカは、前述の反射マーカなどのパッシブマーカに対して、アクティブマーカとして、その認識方法には様々な技術が知られている。
［第２の実施形態］
次に、本発明の第２の実施形態について説明する。第２の実施形態は、指令値Ｂとしての把持部１００の姿勢の指令値をジャイロセンサの出力から得る例である。
【００３５】
図５は、本実施形態におけるマスタ操作入力装置の入力部１１の位置・姿勢を検出するためのセンサの二重化について示した図である。なお、図５において図２と同一の部分については説明を省略する。図５においては、把持部１００に設けられたマーカの数が１個であり、また、把持部１００にジャイロセンサ６００が設けられている点が異なっている。
【００３６】
ジャイロセンサ６００は、ワールド座標系ＷのＸＹＺ軸周りの把持部１００に発生した角速度に応じた信号を出力する３軸のセンサである。この角速度信号を積分することにより、把持部１００の姿勢変化量としての回転量を検出する。
以下、本実施形態のマスタスレーブマニピュレータの動作について説明する。図６は、本発明の第２の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。なお、図６において図３と同様の部分については説明を省略する。
【００３７】
まず、第１の実施形態と同様にマスタスレーブマニピュレータの初期設定が行われる（ステップＳ１１）。ここで、第２の実施形態の初期設定においては、姿勢の変換行列を求める必要はなく、位置の変換行列のみを求めれば良い。位置の変換行列は、カメラ座標系Ｃ内に設定したＸＹＺ軸に対する平行移動を示す行列をワールド座標系（例えば、地表を基準とした座標系）Ｗで見た平行移動を示す行列とするための変換行列である。
【００３８】
初期設定の後、操作者１が把持部１００を操作すると、その操作に応じて把持部１００の位置・姿勢が変化し、この位置・姿勢の変化に応じてエンコーダ２００〜２１０からの出力信号が変化する。マスタ制御部２１は、エンコーダ２００〜２１０からの出力信号によって示される直動量、回転量を用いて運動学計算を解くことにより、把持部１００の位置・姿勢の指令値Ａを算出する（ステップＳ１２）。
【００３９】
また、マスタ制御部２１は、撮像センサ１３で得られた画像から把持部１００の位置を算出するとともに、ジャイロセンサ６００の出力信号から把持部１００の姿勢を算出し、これらを位置・姿勢の指令値Ｂとする（ステップＳ１３）。
画像から把持部１００の位置を算出する手法は第１の実施形態と同様の手法を用いることが可能である。なお、第２の実施形態では画像から把持部１００の姿勢を算出しないので、マーカは少なくとも１つあれば良いが、撮像センサとマーカ間の遮蔽を考慮して、１つ以上のマーカを用いても良い。
【００４０】
ステップＳ１４以後の処理は図３のステップＳ４〜Ｓ６の処理と同様であるので説明を省略する。
以上説明したように、本実施形態によれば、入力部１１に設けられた関節の変位量及び回転量を検出する第１のセンサとしてのエンコーダと、第２のセンサとしての撮像によって画像を得る撮像センサ及び把持部１００の角速度を検出するジャイロセンサと、いう異なる物理量をそれぞれ検出する２系統のセンサから把持部１００の位置・姿勢の指令値を算出している。これにより、第１の実施形態と同様の効果が得られる。
【００４１】
また、画像から把持部１００の姿勢の指令値を算出するのではなく、ジャイロセンサの出力から把持部１００の指令値を算出することにより、より姿勢の指令値の算出精度を高めることが可能である。
ここで、上述の例では、入力部１１の把持部１００上にマーカ４５４を設け、撮像センサ１３を入力部１１から離して配置している。これに対し、図７に示すように、把持部１００に撮像センサ４１０を設け、マーカ４６０を入力部１１から離して配置するようにしても良い。さらに、マーカを省略しても良い。この場合には、画像上の特徴点は、例えば把持部１００の輪郭部分とする。
【００４２】
［第３の実施形態］
次に、本発明の第３の実施形態について説明する。第３の実施形態は、指令値Ｂとしての把持部１００の位置の指令値を加速度センサから得て、把持部１００の姿勢の指令値をジャイロセンサの出力から得る例である。
図８は、本実施形態におけるマスタ操作入力装置の入力部１１の位置・姿勢を検出するためのセンサの二重化について示した図である。なお、図８において図５と同一の部分については説明を省略する。図８においては、把持部１００にジャイロセンサ６００と加速度センサ６５０とが設けられている点が異なっている。
【００４３】
加速度センサ６５０は、ワールド座標系ＷのＸＹＺ軸のそれぞれに平行な加速度に応じた信号を出力する３軸のセンサである。この加速度信号を２回積分することにより、把持部１００の位置変化量としての変位量を検出する。
以下、本実施形態のマスタスレーブマニピュレータの動作について説明する。図９は、本発明の第３の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。なお、図９において図６と同様の部分については説明を省略する。
【００４４】
操作者１が把持部１００を操作すると、その操作に応じて把持部１００の位置・姿勢が変化し、この位置・姿勢の変化に応じてエンコーダ２００〜２１０からの出力信号が変化する。マスタ制御部２１は、エンコーダ２００〜２１０からの出力信号によって示される直動量、回転量を用いて運動学計算を解くことにより、把持部１００の位置・姿勢の指令値Ａを算出する（ステップＳ２１）。
【００４５】
また、マスタ制御部２１は、ジャイロセンサ６００の出力信号から把持部１００の姿勢の指令値を算出するとともに、加速度センサ６５０の出力信号から把持部１００の位置の指令値を算出する。その後、マスタ制御部２１は、加速度センサ６５０を介して得られた把持部１００の位置の指令値とジャイロセンサ６００を介して得られた把持部１００の姿勢の指令値とを指令値Ｂとする（ステップＳ２２）。
【００４６】
ステップＳ２３以後の処理は図３のステップＳ４〜Ｓ６の処理と同様であるので説明を省略する。
以上説明したように、本実施形態によれば、入力部１１に設けられた関節の変位量及び回転量を検出する第１のセンサとしてのエンコーダと、第２のセンサとしての把持部１００の加速度を検出する加速度センサ及び把持部１００の角速度を検出するジャイロセンサと、いう異なる物理量をそれぞれ検出する２系統のセンサから把持部１００の位置・姿勢の指令値を算出している。これにより、第１及び第２の実施形態と同様の効果が得られる。
なお、位置を算出する際に、加速度センサの出力値から重力加速度の影響を除去しなくてはならないが、この計算方法については例えば特開２０１０−２７３７６５号公報等で知られている。重力加速度と、操作部の変位により生じる加速度を区別するために、公知技術のようにジャイロセンサの出力を用いて補正を行っても良い。
【００４７】
［第４の実施形態］
次に、本発明の第４の実施形態について説明する。第３の実施形態は、指令値Ｂとしての把持部１００の位置・姿勢を超音波センサの出力から得る例である。
図１０は、本実施形態におけるマスタ操作入力装置の入力部１１の位置・姿勢を検出するためのセンサの二重化について示した図である。なお、図１０において図８と同一の部分については説明を省略する。図１０においては、把持部１００に超音波センサ７００が設けられており、入力部１１と離間して３つの超音波発生器７５０、７５２、７５４が設けられている点が異なっている。
【００４８】
超音波センサ７００は、超音波発生器７５０、７５２、７５４から発生された超音波信号を検出する。超音波発生器７５０、７５２、７５４はそれぞれ周波数の異なる超音波を発生させる。超音波発生器７５０、７５２、７５４から発生された超音波信号が超音波センサ７００で受信されるまでの時間を検出することにより、超音波センサ７００と超音波発生器７５０との距離、超音波センサ７００と超音波発生器７５２との距離、超音波センサ７００と超音波発生器７５４との距離をそれぞれ算出することが可能である。これらの距離から把持部１００の３次元的な位置・姿勢を算出することが可能である。
【００４９】
以下、本実施形態のマスタスレーブマニピュレータの動作について説明する。図１１は、本発明の第４の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。なお、図１１において図９と同様の部分については説明を省略する。
【００５０】
操作者１が把持部１００を操作すると、その操作に応じて把持部１００の位置・姿勢が変化し、この位置・姿勢の変化に応じてエンコーダ２００〜２１０からの出力信号が変化する。マスタ制御部２１は、エンコーダ２００〜２１０からの出力信号によって示される直動量、回転量を用いて運動学計算を解くことにより、把持部１００の位置・姿勢の指令値Ａを算出する（ステップＳ３１）。
【００５１】
また、マスタ制御部２１は、超音波センサ７００の出力信号から把持部１００の位置・姿勢の指令値Ｂを算出する（ステップＳ３２）。
ステップＳ３３以後の処理は図３のステップＳ４〜Ｓ６の処理と同様であるので説明を省略する。
以上説明したように、本実施形態によれば、入力部１１に設けられた関節の変位量及び回転量を検出する第１のセンサとしてのエンコーダと、把持部１００の変位量及び回転量を検出する第２のセンサとしての超音波センサと、いう異なる物理量をそれぞれ検出する２系統のセンサから把持部１００の位置・姿勢の指令値を算出している。これにより、第１〜第３の実施形態と同様の効果が得られる。
【００５２】
ここで、第４の実施形態では、超音波センサを用いて把持部１００の位置・姿勢を検出しているが、例えば磁気センサを用いて把持部１００の位置・姿勢を検出しても良い。
以上実施形態に基づいて本発明を説明したが、本発明は上述した実施形態に限定されるものではなく、本発明の要旨の範囲内で種々の変形や応用が可能なことは勿論である。例えば、第１〜第４の実施形態で説明したセンサを組み合わせて３系統以上のセンサで把持部１００の位置・姿勢を検出するようにしても良い。
【００５３】
さらに、上記した実施形態には種々の段階の発明が含まれており、開示される複数の構成要件の適当な組合せにより種々の発明が抽出され得る。例えば、実施形態に示される全構成要件からいくつかの構成要件が削除されても、上述したような課題を解決でき、上述したような効果が得られる場合には、この構成要件が削除された構成も発明として抽出され得る。
【符号の説明】
【００５４】
１０…マスタ操作入力装置、１１…入力部、１２…表示部、１３…撮像センサ、２０…制御装置、２１…マスタ制御部、２２…マニピュレータ制御部、２３…画像処理部、３０…スレーブマニピュレータ、１００…把持部、２００〜２１０…エンコーダ、４０２，４０４…撮像系、４１０…撮像センサ、４５０〜４５４…反射マーカ、６００…ジャイロセンサ、６５０…加速度センサ、７００…超音波センサ、７５０〜７５４…超音波発生器
【技術分野】
【０００１】
本発明は、スレーブマニピュレータを遠隔操作するためのマスタ操作入力装置及びそのようなマスタ操作入力装置を有するマスタスレーブマニピュレータに関する。
【背景技術】
【０００２】
近年、医療施設の省人化を図るため、ロボットによる医療処置の研究が行われている。特に、外科分野では、多自由度（多関節）アームを有するマニピュレータによって患者の処置をするマニピュレータシステムについての各種の提案がなされている。このようなマニピュレータシステムにおいて、患者の体腔に直接接触するマニピュレータ（スレーブマニピュレータ）を、スレーブマニピュレータに対して遠隔に配置されたマスタ操作入力装置によって遠隔操作できるようにしたマニピュレータシステム（マスタスレーブマニピュレータ）が知られている。通常、マスタスレーブマニピュレータにおいては、マスタ操作入力装置の操作部の位置・姿勢が指令値として制御装置に入力される。制御装置においては操作部の位置・姿勢の指令値に従ってスレーブマニピュレータの先端部の逆運動学を解くことによってスレーブマニピュレータの各関節の駆動量が算出され、この駆動量に従って各関節が駆動制御される。このような制御を行うためには、マスタ操作入力装置の操作部の位置・姿勢を検出することが必要である。従来、マスタ操作入力装置の位置・姿勢は、マスタ操作入力装置の操作部に取り付けられたセンサにより検出されている。
【０００３】
ここで、操作部の位置・姿勢を検出するためのセンサを１系統のみとすると、この１系統のセンサが故障した場合には位置・姿勢が検出できなくなってしまう。これを回避するための手法として、センサを二重化する手法が知られている（例えば、特許文献１参照）。特許文献１の技術をマスタ操作入力装置に対して適用することにより、１つの系統のセンサが故障しても別の系統のセンサを用いてマスタ操作入力装置の操作部の位置・姿勢を検出することが可能となる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平６−１６８５３０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ここで、特許文献１の技術の場合、二重化したセンサはともに同じ物理量（例えば特許文献１の場合にはピッカの変位量）を検出している。この場合、一方の系統のセンサが故障する際に、もう一方の系統のセンサも同時に故障してしまう可能性が高い。
本発明は、上記の事情に鑑みてなされたもので、より故障に強い構成で操作部の位置・姿勢を検出することが可能なマスタ操作入力装置及びそのようなマスタ操作入力装置を備えるマスタスレーブマニピュレータを提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記の目的を達成するために、本発明の第１の態様のマスタ操作入力装置は、複数の自由度に対応した関節を有するスレーブマニピュレータを操作するためのマスタ操作入力装置であって、操作者による操作によって位置及び姿勢を変化自在であって、該位置及び姿勢の変化によって前記スレーブマニピュレータの位置及び姿勢の指令値を与えるための操作部と、前記操作部の位置及び姿勢を検出するために、前記操作部に係る異なる物理量をそれぞれ検出する２系統以上の検出部と、を具備することを特徴とする。
【０００７】
また、上記の目的を達成するために、本発明の第２の態様のマスタスレーブマニピュレータは、第１の態様に記載のマスタ操作入力装置と、前記２系統以上の検出部のそれぞれで検出された物理量に従って前記操作部の位置及び姿勢を複数算出する第１の制御部と、前記第１の制御部によって算出された前記操作部の位置及び姿勢から前記スレーブマニピュレータの位置及び姿勢の指令値を算出し、該算出したスレーブマニピュレータの位置及び姿勢の指令値に従って前記スレーブマニピュレータを駆動制御する第２の制御部と、を有することを特徴とする。
【発明の効果】
【０００８】
本発明によれば、より故障に強い構成で操作部の位置・姿勢を検出することが可能なマスタ操作入力装置及びそのようなマスタ操作入力装置を備えるマスタスレーブマニピュレータを提供することが可能である。
【図面の簡単な説明】
【０００９】
【図１】本発明の第１の実施形態に係るマスタスレーブマニピュレータの一例の全体構成を示す図である。
【図２】本発明の第１の実施形態におけるマスタ操作入力装置の操作部の位置・姿勢を検出するためのセンサの二重化について示した図である。
【図３】本発明の第１の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。
【図４】本発明の第１の実施形態においてマーカと撮像センサの配置の関係を逆転させた変形例を示す図である。
【図５】本発明の第２の実施形態におけるマスタ操作入力装置の操作部の位置・姿勢を検出するためのセンサの二重化について示した図である。
【図６】本発明の第２の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。
【図７】本発明の第２の実施形態においてマーカと撮像センサの配置の関係を逆転させた変形例を示す図である。
【図８】本発明の第３の実施形態におけるマスタ操作入力装置の操作部の位置・姿勢を検出するためのセンサの二重化について示した図である。
【図９】本発明の第３の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。
【図１０】本発明の第４の実施形態におけるマスタ操作入力装置の操作部の位置・姿勢を検出するためのセンサの二重化について示した図である。
【図１１】本発明の第４の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。
【発明を実施するための形態】
【００１０】
以下、図面を参照して本発明の実施形態を説明する。
［第１の実施形態］
まず、本発明の第１の実施形態について説明する。図１は、本発明の第１の実施形態に係るマスタスレーブマニピュレータの一例の全体構成を示す図である。図１に示すように、本実施形態に係るマスタスレーブマニピュレータは、マスタ操作入力装置１０と、制御装置２０と、スレーブマニピュレータ３０と、を有している。
【００１１】
マスタ操作入力装置１０は、本マスタスレーブマニピュレータにおけるマスタとして機能するものであって、入力部１１と、表示部１２と、撮像センサ１３とを有している。
入力部１１は、例えばマスタ操作入力装置１０の表示部１２に固定されており、操作者１の操作を受けてスレーブマニピュレータ３０を操作するための信号を出力する。入力部１１の詳細については後述する。
【００１２】
表示部１２は、例えば液晶ディスプレイから構成され、制御装置２０から入力された画像信号に基づいて画像を表示する。後述するが、制御装置２０から入力される画像信号は、スレーブマニピュレータに取り付けられた電子カメラ（電子内視鏡）を介して得られた画像信号を、制御装置２０において処理したものである。このような画像信号に基づく画像を、表示部１２に表示させることにより、マスタ操作入力装置１０の操作者１は、マスタ操作入力装置１０から離れた場所に配置されたスレーブマニピュレータ３０の先端の画像を確認することが可能である。
【００１３】
撮像センサ１３は、例えば所定の間隔を空けて配置された２眼の撮像センサである。この撮像センサ１３は、撮像により、所定の視差を有する２枚の入力部１１の画像を生成する。撮像センサ１３の詳細については後述する。
制御装置２０は、マスタ制御部２１と、マニピュレータ制御部２２と、画像処理部２３と、を有している。
第１の制御部の一例として機能するマスタ制御部２１は、マスタ操作入力装置１０からの信号に従って、マスタ操作入力装置１０の入力部１１の先端の位置・姿勢を算出する。後述するが、本実施形態では、入力部１１の先端の位置・姿勢を算出するために、それぞれが異なる物理量を検出する２系統以上の検出部（センサ）が設けられている。マスタ制御部２１は、これら２系統以上のセンサのそれぞれからの出力に従って入力部１１の先端の位置・姿勢を算出する。そして、マスタ制御部２１は、算出した入力部１１の先端の位置・姿勢をスレーブマニピュレータの先端の位置・姿勢の指令値としてマニピュレータ制御部２２に出力する。
【００１４】
第２の制御部の一例として機能するマニピュレータ制御部２２は、マスタ制御部２１からの位置・姿勢の指令値を受けて、スレーブマニピュレータ３０の先端の位置・姿勢を指令値に一致させるために必要なスレーブマニピュレータの各関節の駆動量を、例えば逆運動学計算によって算出する。そして、マニピュレータ制御部２２は、算出した駆動量に従ってスレーブマニピュレータの各関節を駆動させる。
【００１５】
画像処理部２３は、スレーブマニピュレータの先端に設けられた電子カメラ（電子内視鏡等）から得られた画像信号を処理し、表示部１２の表示用の画像信号を生成して表示部１２に出力する。
スレーブマニピュレータ３０は、複数の自由度に対応した複数の関節を有している。このスレーブマニピュレータ３０は、マニピュレータ制御部２２からの制御信号に従って各関節が駆動される。
【００１６】
図２は、本実施形態におけるマスタ操作入力装置の入力部１１の位置・姿勢を検出するためのセンサの二重化について示した図である。
入力部１１は、その先端に操作部としての把持部１００が設けられている。そして、把持部１００には、複数のリンクがそれぞれ関節を介して連接されている。図２に示す関節は、回転関節と直動関節とにより構成されている。操作者１が、把持部１００を把持して操作することにより、その操作に応じて各関節が回転又は直動する。ここで、図２は、入力部１１が、４つの回転関節と２つの直動関節の６関節を有している例を示しており、これらの６関節によって把持部１００の自由度を６自由度（位置の３自由度＋姿勢の３自由度）とすることが可能である。なお、図２のリンク構造は一例であり、回転関節や直動関節の数が異なる他のリンク構造であっても良い。操作部の自由度も６とは限らなくても良い。また、図２は、右手用の操作部のみを示している。左手用の操作部の構成は、右手用の操作部に対して左右の関係が逆転するだけで実質的な構成は図２に示すものと同様である。
【００１７】
また、それぞれの関節の近傍には、関節の回転量又は直動量を検出するためのセンサ（例えばエンコーダ）２００、２０２、２０４、２０６、２０８、２１０が設けられている。各エンコーダの出力信号は、マスタ制御部２１に入力される。図２では、エンコーダ２００は、把持部１００から見て最も遠い位置に設けられた関節の回転量θ１に応じた信号を出力するセンサである。エンコーダ２０２は、把持部１００から見て次に遠い位置に設けられた関節の直動量ｄ１に応じた信号を出力するセンサである。以下同様に、エンコーダ２０４は関節の直動量ｄ２に応じた信号を出力するセンサであり、エンコーダ２０６は関節の回転量θ２に応じた信号を出力するセンサであり、エンコーダ２０８は関節の回転量θ３に応じた信号を出力するセンサである。そして、エンコーダ２１０は、把持部１００から見て最も近い位置に設けられた関節の回転量θ４に応じた信号を出力するセンサである。
【００１８】
さらに、把持部１００には、３つの反射マーカ（以下、マーカという）４５０、４５２、４５４が設けられている。各マーカは、撮像センサ１３で得られた画像中で高輝度となるよう、高反射率の材料から形成されている。ここで、マーカ４５０とマーカ４５２との間隔、マーカ４５２とマーカ４５４との間隔、及びマーカ４５４とマーカ４５０との間隔がそれぞれ異なるように、反射マーカ４５０、４５２、４５４を把持部１００上に配置しておくことが望ましい。また、３つのマーカ４５０、４５２、４５４が一直線上に並ばないような配置で、さらに、把持部１００即ち操作部の姿勢を変えてもマーカが撮像センサ１３に認識できるような配置しておくことが望ましい。これは、画像上でマーカ４５０、４５２、４５４を抽出する際に、それぞれのマーカを正しく識別できるようにするためである。撮像センサ１３で取得する操作部の特徴点は反射マーカに限らない。例えば、ＬＥＤ等の発光体であっても良い。
【００１９】
撮像センサ１３は、２眼の撮像系４０２、４０４を有している。撮像系４０２、４０４は、それぞれが、撮像光学系と、撮像素子とを有しており、入力部１１に対して所定距離だけ離れて配置されている。また、撮像系４０２と撮像系４０４も予め定められた距離だけ離間するように配置されている。
【００２０】
撮像光学系は、入射した光束を撮像素子の受光面上に結像させるための光学系である。撮像素子は、撮像光学系を介して入射した光束を電気信号に変換して把持部１００の位置・姿勢を得るための画像を生成するセンサである。撮像素子で生成された画像は、マスタ制御部２１に入力される。
【００２１】
以下、本実施形態のマスタスレーブマニピュレータの動作について説明する。図３は、本発明の第１の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。図３は、特にマスタ制御部２１の動作について示している。
まず、マスタスレーブマニピュレータの初期設定が行われる（ステップＳ１）。この初期設定において、例えば操作者１は、把持部１００の位置・姿勢を予め定められた初期の位置・姿勢に設定する。この後、操作者１は、撮像センサ１３の撮像系４０２、４０４を動作させて、マーカ４５０、４５２、４５４を含む把持部１００の画像を取得する。把持部１００の位置・姿勢の設定を自動的に行えるようにしても良い。
【００２２】
マスタ制御部２１は、撮像系４０２、４０４のそれぞれを介して得られた画像から特徴点としてのマーカ４５０、４５２、４５４を抽出する。画像内のマーカ４５０、４５２、４５４を抽出する手法としては例えば周知のパターンマッチング法を用いることが可能である。
【００２３】
画像内のマーカ４５０、４５２、４５４の抽出後、マスタ制御部２１は、取得した画像内に３次元の座標系（カメラ座標系）Ｃを設定し、このカメラ座標系Ｃに対応して初期設定時の把持部１００の位置・姿勢を示す姿勢行列Ｐを算出する。例えば、カメラ座標系Ｃとして、画像の水平方向をＸ軸、垂直方向をＹ軸、奥行き方向をＺ軸とし、マーカ４５４の座標を原点に設定する。このとき、姿勢行列Ｐは、カメラ座標系Ｃ内に設定したＸ軸の方向を示す単位ベクトルと、Ｙ軸の方向を示す単位ベクトルと、Ｚ軸の方向を示す単位ベクトルを用いた３×３の行列、もしくは、位置ベクトルも含めた４×４の行列で表わすことができる。
【００２４】
姿勢行列Ｐを算出した後、マスタ制御部２１は、カメラ座標系Ｃの姿勢行列Ｐをワールド座標系（例えば、地表を基準とした座標系）Ｗで見た姿勢行列とするための変換行列Ｑを算出する。変換行列Ｑの算出により、初期設定が終了する。
初期設定の後、操作者１が把持部１００を操作すると、その操作に応じて把持部１００の位置・姿勢が変化し、この位置・姿勢の変化に応じてエンコーダ２００〜２１０からの出力信号が変化する。マスタ制御部２１は、エンコーダ２００〜２１０からの出力信号によって示される直動量、回転量を用いて運動学計算を解くことにより、把持部１００の位置・姿勢の指令値Ａを算出する（ステップＳ２）。
【００２５】
また、マスタ制御部２１は、撮像センサ１３で得られた画像から把持部１００の位置・姿勢の指令値Ｂを算出する（ステップＳ３）。
画像内の把持部１００の位置変化（変位量）は、位置・姿勢の変化前に撮像センサ１３で得られた把持部１００の画像と位置・姿勢の変化後に撮像センサ１３で得られた把持部１００の画像との間での原点位置のマーカ（例えばマーカ４５４）の位置変化から算出することが可能である。ここで、Ｚ方向の位置変化は、撮像系４０２と撮像系４０４との視差によって発生する、撮像系４０２を介して得られる画像におけるマーカ４５４の位置と撮像系４０４を介して得られる画像におけるマーカ４５４の位置とのずれ量から算出することが可能である。このずれ量を算出する手法としては、例えば画像相関法を用いることが可能である。
【００２６】
また、画像内の把持部１００の姿勢変化（回転量）は、位置・姿勢の変化前に撮像センサ１３で得られた把持部１００の画像と位置・姿勢の変化後に撮像センサ１３で得られた把持部１００の画像との間での原点位置のマーカ（例えばマーカ４５４）に対する残りのマーカの位置変化から算出することが可能である。
上述の例では、２眼の撮像系を用いているが、１眼の撮像系でも良い。この場合、奥行座標は撮像センサ１３に映るマーカの大きさから取得でき、さらに、同一直線状に並ばない３つのマーカを用いれば操作部の位置・姿勢を取得することができる。この技術については公知であるため、詳細は記載しない。
【００２７】
カメラ座標系Ｃで見た把持部１００の位置・姿勢の指令値を算出した後、マスタ制御部２１は、把持部１００の位置の変化量によって表わされる平行移動を示す行列と、把持部１００の回転量によって表わされる回転行列とを、姿勢行列Ｐに左から乗じることにより、位置・姿勢の変化後のカメラ座標系Ｃで見た姿勢行列Ｐ’を算出する。その後、マスタ制御部２１は、姿勢行列Ｐ’に変換行列Ｑを乗じることにより、ワールド座標系で見た把持部１００の位置・姿勢の指令値Ｂを算出する。
【００２８】
位置・姿勢の指令値Ａ、Ｂの算出後、マスタ制御部２１は、指令値Ａと指令値Ｂとの差の絶対値が所定値Ｔｈ以下であるか否かを判定する（ステップＳ４）。指令値Ａと指令値Ｂは、ともに操作者１による操作後の把持部１００の位置・姿勢を表わすものであり、本来は略一致するものである。このため、指令値Ａと指令値Ｂとの差の絶対値が所定値Ｔｈ以下であるか否かを判定することにより、エンコーダ２００〜２１０と撮像センサ１３の何れかが故障しているか否かを判定することが可能である。
【００２９】
ステップＳ４の判定において、指令値Ａと指令値Ｂとの差の絶対値が所定値Ｔｈ以下である場合に、マスタ制御部２１は、指令値Ａと指令値Ｂの何れか（又は両者の平均値）をマニピュレータ制御部２２に出力する（ステップＳ５）。これを受けて、マニピュレータ制御部２２は、スレーブマニピュレータ３０の先端部の位置・姿勢をマスタ制御部２１から入力された位置・姿勢の指令値とするために必要な各関節の駆動量を、逆運動学計算を解くことにより算出する。その後、マニピュレータ制御部２２は、算出した駆動量に従ってスレーブマニピュレータ３０の各関節を駆動制御する。このようにしてマスタスレーブマニピュレータの一連の動作が終了する。
【００３０】
また、ステップＳ４の判定において、指令値Ａと指令値Ｂとの差の絶対値が所定値Ｔｈを越えている場合に、マスタ制御部２１は、エラー処理を行う（ステップＳ６）。エラー処理とは、例えば算出した指令値をマニピュレータ制御部２２に送らないようにする処理や、エンコーダ２００〜２１０と撮像センサ１３の何れかが故障していることを操作者１に警告する処理、マスタスレーブマニピュレータのシステム自体を遮断してしまう処理等が考えられる。このエラー処理の場合にも、マスタスレーブマニピュレータの一連の動作が終了する。
【００３１】
以上説明したように、本実施形態によれば、入力部１１に設けられた関節の変位量及び回転量を検出する第１のセンサとしてのエンコーダと、撮像によって画像を得る第２のセンサとしての撮像センサと、いう異なる物理量をそれぞれ検出する２系統のセンサから把持部１００の位置・姿勢の指令値を算出している。これにより、例えば故障の検出が容易である。
【００３２】
また、図３では指令値Ａと指令値Ｂとに所定以上の差がある場合にはエラー処理を行うようにしているが、その後の検査によりエンコーダの出力信号と撮像センサの何れが故障しているかが判別された場合には、その後は故障していない側を用いてスレーブマニピュレータ３０の駆動制御を行うこともできる。このため、故障にも強い。
【００３３】
先行技術のように、センサの二重化として同一関節上に、同じ物理量を取得するために２種類のセンサ（例えばエンコーダとリミットスイッチやエンコーダとポテンショメータの組み合せ）を配置することがあったが、この場合、熱や水没で同時に２つのセンサが故障することがあった。本実施形態のように、撮像センサで得られた画像から把持部１００の位置・姿勢の指令値を算出する場合、入力部１１の関節上に撮像センサを配置する必要がない。このため、エンコーダと撮像センサが同時に故障するような事態も回避することが可能である。
ここで、上述の例では、入力部１１の把持部１００上に３つのマーカ４５０、４５２、４５４を設け、撮像センサ１３を入力部１１から離して配置している。これに対し、図４に示すように、把持部１００に撮像センサ４１０を設け、マーカ４６０、４６２、４６４を入力部１１から離して配置するようにしても良い。さらに、マーカを省略しても良い。この場合には、画像上の特徴点は、例えば把持部１００の輪郭部分として、操作部の位置・姿勢を取得する。
【００３４】
また、上述の例では、画像内でマーカ４５０、４５２、４５４を識別するために、マーカ間の距離を異ならせるようにしているが、例えばマーカの反射率や直径、色を異ならせるようにしても良い。マーカの代わりにＬＥＤのような発光体を撮像センサに認識させる場合は、発光パターン（発光間隔）を変化させても良い。このようなマーカは、前述の反射マーカなどのパッシブマーカに対して、アクティブマーカとして、その認識方法には様々な技術が知られている。
［第２の実施形態］
次に、本発明の第２の実施形態について説明する。第２の実施形態は、指令値Ｂとしての把持部１００の姿勢の指令値をジャイロセンサの出力から得る例である。
【００３５】
図５は、本実施形態におけるマスタ操作入力装置の入力部１１の位置・姿勢を検出するためのセンサの二重化について示した図である。なお、図５において図２と同一の部分については説明を省略する。図５においては、把持部１００に設けられたマーカの数が１個であり、また、把持部１００にジャイロセンサ６００が設けられている点が異なっている。
【００３６】
ジャイロセンサ６００は、ワールド座標系ＷのＸＹＺ軸周りの把持部１００に発生した角速度に応じた信号を出力する３軸のセンサである。この角速度信号を積分することにより、把持部１００の姿勢変化量としての回転量を検出する。
以下、本実施形態のマスタスレーブマニピュレータの動作について説明する。図６は、本発明の第２の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。なお、図６において図３と同様の部分については説明を省略する。
【００３７】
まず、第１の実施形態と同様にマスタスレーブマニピュレータの初期設定が行われる（ステップＳ１１）。ここで、第２の実施形態の初期設定においては、姿勢の変換行列を求める必要はなく、位置の変換行列のみを求めれば良い。位置の変換行列は、カメラ座標系Ｃ内に設定したＸＹＺ軸に対する平行移動を示す行列をワールド座標系（例えば、地表を基準とした座標系）Ｗで見た平行移動を示す行列とするための変換行列である。
【００３８】
初期設定の後、操作者１が把持部１００を操作すると、その操作に応じて把持部１００の位置・姿勢が変化し、この位置・姿勢の変化に応じてエンコーダ２００〜２１０からの出力信号が変化する。マスタ制御部２１は、エンコーダ２００〜２１０からの出力信号によって示される直動量、回転量を用いて運動学計算を解くことにより、把持部１００の位置・姿勢の指令値Ａを算出する（ステップＳ１２）。
【００３９】
また、マスタ制御部２１は、撮像センサ１３で得られた画像から把持部１００の位置を算出するとともに、ジャイロセンサ６００の出力信号から把持部１００の姿勢を算出し、これらを位置・姿勢の指令値Ｂとする（ステップＳ１３）。
画像から把持部１００の位置を算出する手法は第１の実施形態と同様の手法を用いることが可能である。なお、第２の実施形態では画像から把持部１００の姿勢を算出しないので、マーカは少なくとも１つあれば良いが、撮像センサとマーカ間の遮蔽を考慮して、１つ以上のマーカを用いても良い。
【００４０】
ステップＳ１４以後の処理は図３のステップＳ４〜Ｓ６の処理と同様であるので説明を省略する。
以上説明したように、本実施形態によれば、入力部１１に設けられた関節の変位量及び回転量を検出する第１のセンサとしてのエンコーダと、第２のセンサとしての撮像によって画像を得る撮像センサ及び把持部１００の角速度を検出するジャイロセンサと、いう異なる物理量をそれぞれ検出する２系統のセンサから把持部１００の位置・姿勢の指令値を算出している。これにより、第１の実施形態と同様の効果が得られる。
【００４１】
また、画像から把持部１００の姿勢の指令値を算出するのではなく、ジャイロセンサの出力から把持部１００の指令値を算出することにより、より姿勢の指令値の算出精度を高めることが可能である。
ここで、上述の例では、入力部１１の把持部１００上にマーカ４５４を設け、撮像センサ１３を入力部１１から離して配置している。これに対し、図７に示すように、把持部１００に撮像センサ４１０を設け、マーカ４６０を入力部１１から離して配置するようにしても良い。さらに、マーカを省略しても良い。この場合には、画像上の特徴点は、例えば把持部１００の輪郭部分とする。
【００４２】
［第３の実施形態］
次に、本発明の第３の実施形態について説明する。第３の実施形態は、指令値Ｂとしての把持部１００の位置の指令値を加速度センサから得て、把持部１００の姿勢の指令値をジャイロセンサの出力から得る例である。
図８は、本実施形態におけるマスタ操作入力装置の入力部１１の位置・姿勢を検出するためのセンサの二重化について示した図である。なお、図８において図５と同一の部分については説明を省略する。図８においては、把持部１００にジャイロセンサ６００と加速度センサ６５０とが設けられている点が異なっている。
【００４３】
加速度センサ６５０は、ワールド座標系ＷのＸＹＺ軸のそれぞれに平行な加速度に応じた信号を出力する３軸のセンサである。この加速度信号を２回積分することにより、把持部１００の位置変化量としての変位量を検出する。
以下、本実施形態のマスタスレーブマニピュレータの動作について説明する。図９は、本発明の第３の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。なお、図９において図６と同様の部分については説明を省略する。
【００４４】
操作者１が把持部１００を操作すると、その操作に応じて把持部１００の位置・姿勢が変化し、この位置・姿勢の変化に応じてエンコーダ２００〜２１０からの出力信号が変化する。マスタ制御部２１は、エンコーダ２００〜２１０からの出力信号によって示される直動量、回転量を用いて運動学計算を解くことにより、把持部１００の位置・姿勢の指令値Ａを算出する（ステップＳ２１）。
【００４５】
また、マスタ制御部２１は、ジャイロセンサ６００の出力信号から把持部１００の姿勢の指令値を算出するとともに、加速度センサ６５０の出力信号から把持部１００の位置の指令値を算出する。その後、マスタ制御部２１は、加速度センサ６５０を介して得られた把持部１００の位置の指令値とジャイロセンサ６００を介して得られた把持部１００の姿勢の指令値とを指令値Ｂとする（ステップＳ２２）。
【００４６】
ステップＳ２３以後の処理は図３のステップＳ４〜Ｓ６の処理と同様であるので説明を省略する。
以上説明したように、本実施形態によれば、入力部１１に設けられた関節の変位量及び回転量を検出する第１のセンサとしてのエンコーダと、第２のセンサとしての把持部１００の加速度を検出する加速度センサ及び把持部１００の角速度を検出するジャイロセンサと、いう異なる物理量をそれぞれ検出する２系統のセンサから把持部１００の位置・姿勢の指令値を算出している。これにより、第１及び第２の実施形態と同様の効果が得られる。
なお、位置を算出する際に、加速度センサの出力値から重力加速度の影響を除去しなくてはならないが、この計算方法については例えば特開２０１０−２７３７６５号公報等で知られている。重力加速度と、操作部の変位により生じる加速度を区別するために、公知技術のようにジャイロセンサの出力を用いて補正を行っても良い。
【００４７】
［第４の実施形態］
次に、本発明の第４の実施形態について説明する。第３の実施形態は、指令値Ｂとしての把持部１００の位置・姿勢を超音波センサの出力から得る例である。
図１０は、本実施形態におけるマスタ操作入力装置の入力部１１の位置・姿勢を検出するためのセンサの二重化について示した図である。なお、図１０において図８と同一の部分については説明を省略する。図１０においては、把持部１００に超音波センサ７００が設けられており、入力部１１と離間して３つの超音波発生器７５０、７５２、７５４が設けられている点が異なっている。
【００４８】
超音波センサ７００は、超音波発生器７５０、７５２、７５４から発生された超音波信号を検出する。超音波発生器７５０、７５２、７５４はそれぞれ周波数の異なる超音波を発生させる。超音波発生器７５０、７５２、７５４から発生された超音波信号が超音波センサ７００で受信されるまでの時間を検出することにより、超音波センサ７００と超音波発生器７５０との距離、超音波センサ７００と超音波発生器７５２との距離、超音波センサ７００と超音波発生器７５４との距離をそれぞれ算出することが可能である。これらの距離から把持部１００の３次元的な位置・姿勢を算出することが可能である。
【００４９】
以下、本実施形態のマスタスレーブマニピュレータの動作について説明する。図１１は、本発明の第４の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。なお、図１１において図９と同様の部分については説明を省略する。
【００５０】
操作者１が把持部１００を操作すると、その操作に応じて把持部１００の位置・姿勢が変化し、この位置・姿勢の変化に応じてエンコーダ２００〜２１０からの出力信号が変化する。マスタ制御部２１は、エンコーダ２００〜２１０からの出力信号によって示される直動量、回転量を用いて運動学計算を解くことにより、把持部１００の位置・姿勢の指令値Ａを算出する（ステップＳ３１）。
【００５１】
また、マスタ制御部２１は、超音波センサ７００の出力信号から把持部１００の位置・姿勢の指令値Ｂを算出する（ステップＳ３２）。
ステップＳ３３以後の処理は図３のステップＳ４〜Ｓ６の処理と同様であるので説明を省略する。
以上説明したように、本実施形態によれば、入力部１１に設けられた関節の変位量及び回転量を検出する第１のセンサとしてのエンコーダと、把持部１００の変位量及び回転量を検出する第２のセンサとしての超音波センサと、いう異なる物理量をそれぞれ検出する２系統のセンサから把持部１００の位置・姿勢の指令値を算出している。これにより、第１〜第３の実施形態と同様の効果が得られる。
【００５２】
ここで、第４の実施形態では、超音波センサを用いて把持部１００の位置・姿勢を検出しているが、例えば磁気センサを用いて把持部１００の位置・姿勢を検出しても良い。
以上実施形態に基づいて本発明を説明したが、本発明は上述した実施形態に限定されるものではなく、本発明の要旨の範囲内で種々の変形や応用が可能なことは勿論である。例えば、第１〜第４の実施形態で説明したセンサを組み合わせて３系統以上のセンサで把持部１００の位置・姿勢を検出するようにしても良い。
【００５３】
さらに、上記した実施形態には種々の段階の発明が含まれており、開示される複数の構成要件の適当な組合せにより種々の発明が抽出され得る。例えば、実施形態に示される全構成要件からいくつかの構成要件が削除されても、上述したような課題を解決でき、上述したような効果が得られる場合には、この構成要件が削除された構成も発明として抽出され得る。
【符号の説明】
【００５４】
１０…マスタ操作入力装置、１１…入力部、１２…表示部、１３…撮像センサ、２０…制御装置、２１…マスタ制御部、２２…マニピュレータ制御部、２３…画像処理部、３０…スレーブマニピュレータ、１００…把持部、２００〜２１０…エンコーダ、４０２，４０４…撮像系、４１０…撮像センサ、４５０〜４５４…反射マーカ、６００…ジャイロセンサ、６５０…加速度センサ、７００…超音波センサ、７５０〜７５４…超音波発生器
【特許請求の範囲】
【請求項１】
複数の自由度に対応した関節を有するスレーブマニピュレータを操作するためのマスタ操作入力装置であって、
操作者による操作によって位置及び姿勢を変化自在であって、該位置及び姿勢の変化によって前記スレーブマニピュレータの位置及び姿勢の指令値を与えるための操作部と、
前記操作部の位置及び姿勢を検出するために、前記操作部に係る異なる物理量をそれぞれ検出する２系統以上の検出部と、
を具備することを特徴とするマスタ操作入力装置。
【請求項２】
前記操作部は、複数の自由度に対応した関節に取り付けられており、
前記検出部は、
前記物理量として複数の前記関節の回転量又は変位量をそれぞれ検出する複数の第１のセンサと、
複数の前記関節の回転量又は変位量と異なる物理量を検出する第２のセンサと、
を有することを特徴とする請求項１に記載のマスタ操作入力装置。
【請求項３】
前記第１のセンサは、前記複数の関節のそれぞれに配置され、
前記第２のセンサは、前記複数の関節に対して遠隔位置に配置されていることを特徴とする請求項２に記載のマスタ操作入力装置。
【請求項４】
前記第２のセンサは、前記操作部の位置及び姿勢を検出するために前記操作部の画像を取得する撮像センサを含むことを特徴とする請求項２又は３に記載のマスタ操作入力装置。
【請求項５】
前記第２のセンサは、
前記操作部の位置を検出するために前記操作部の画像を取得する撮像センサと、
前記操作部に発生した角速度を前記操作部の姿勢として検出する角速度センサと、
を含むことを特徴とする請求項２又は３に記載のマスタ操作入力装置。
【請求項６】
前記第２のセンサは、
前記操作部の位置及び姿勢を検出するために、前記操作部の加速度を取得する加速度センサと、前記操作部の角速度を取得する角速度センサと、
を含むことを特徴とする請求項２又は３に記載のマスタ操作入力装置。
【請求項７】
前記第２のセンサは、前記操作部の位置及び姿勢を検出する超音波センサ又は磁気センサであることを特徴とする請求項２又は３に記載のマスタ操作入力装置。
【請求項８】
請求項１乃至７の何れか１項に記載のマスタ操作入力装置と、
前記２系統以上の検出部のそれぞれで検出された物理量に従って前記操作部の位置及び姿勢を複数算出する第１の制御部と、
前記第１の制御部によって算出された前記操作部の位置及び姿勢から前記スレーブマニピュレータの位置及び姿勢の指令値を算出し、該算出したスレーブマニピュレータの位置及び姿勢の指令値に従って前記スレーブマニピュレータを駆動制御する第２の制御部と、
を有することを特徴とするマスタスレーブマニピュレータ。
【請求項９】
前記第１の制御部は、前記２系統以上の検出部のそれぞれで検出された物理量に従って複数算出された前記操作部の位置及び姿勢の差が所定の範囲内であるか否かを判定し、前記所定の範囲内である場合には前記スレーブマニピュレータの位置及び姿勢の指令値を算出し、前記所定の範囲内でない場合にはエラー処理をすることを特徴とする請求項８に記載のマスタスレーブマニピュレータ。
【請求項１】
複数の自由度に対応した関節を有するスレーブマニピュレータを操作するためのマスタ操作入力装置であって、
操作者による操作によって位置及び姿勢を変化自在であって、該位置及び姿勢の変化によって前記スレーブマニピュレータの位置及び姿勢の指令値を与えるための操作部と、
前記操作部の位置及び姿勢を検出するために、前記操作部に係る異なる物理量をそれぞれ検出する２系統以上の検出部と、
を具備することを特徴とするマスタ操作入力装置。
【請求項２】
前記操作部は、複数の自由度に対応した関節に取り付けられており、
前記検出部は、
前記物理量として複数の前記関節の回転量又は変位量をそれぞれ検出する複数の第１のセンサと、
複数の前記関節の回転量又は変位量と異なる物理量を検出する第２のセンサと、
を有することを特徴とする請求項１に記載のマスタ操作入力装置。
【請求項３】
前記第１のセンサは、前記複数の関節のそれぞれに配置され、
前記第２のセンサは、前記複数の関節に対して遠隔位置に配置されていることを特徴とする請求項２に記載のマスタ操作入力装置。
【請求項４】
前記第２のセンサは、前記操作部の位置及び姿勢を検出するために前記操作部の画像を取得する撮像センサを含むことを特徴とする請求項２又は３に記載のマスタ操作入力装置。
【請求項５】
前記第２のセンサは、
前記操作部の位置を検出するために前記操作部の画像を取得する撮像センサと、
前記操作部に発生した角速度を前記操作部の姿勢として検出する角速度センサと、
を含むことを特徴とする請求項２又は３に記載のマスタ操作入力装置。
【請求項６】
前記第２のセンサは、
前記操作部の位置及び姿勢を検出するために、前記操作部の加速度を取得する加速度センサと、前記操作部の角速度を取得する角速度センサと、
を含むことを特徴とする請求項２又は３に記載のマスタ操作入力装置。
【請求項７】
前記第２のセンサは、前記操作部の位置及び姿勢を検出する超音波センサ又は磁気センサであることを特徴とする請求項２又は３に記載のマスタ操作入力装置。
【請求項８】
請求項１乃至７の何れか１項に記載のマスタ操作入力装置と、
前記２系統以上の検出部のそれぞれで検出された物理量に従って前記操作部の位置及び姿勢を複数算出する第１の制御部と、
前記第１の制御部によって算出された前記操作部の位置及び姿勢から前記スレーブマニピュレータの位置及び姿勢の指令値を算出し、該算出したスレーブマニピュレータの位置及び姿勢の指令値に従って前記スレーブマニピュレータを駆動制御する第２の制御部と、
を有することを特徴とするマスタスレーブマニピュレータ。
【請求項９】
前記第１の制御部は、前記２系統以上の検出部のそれぞれで検出された物理量に従って複数算出された前記操作部の位置及び姿勢の差が所定の範囲内であるか否かを判定し、前記所定の範囲内である場合には前記スレーブマニピュレータの位置及び姿勢の指令値を算出し、前記所定の範囲内でない場合にはエラー処理をすることを特徴とする請求項８に記載のマスタスレーブマニピュレータ。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１２−１７１０８８（Ｐ２０１２−１７１０８８Ａ）
【公開日】平成２４年９月１０日（２０１２．９．１０）
【国際特許分類】
【出願番号】特願２０１１−３８７９６（Ｐ２０１１−３８７９６）
【出願日】平成２３年２月２４日（２０１１．２．２４）
【出願人】（００００００３７６）オリンパス株式会社 (11,466)
【Ｆターム（参考）】
【公開日】平成２４年９月１０日（２０１２．９．１０）
【国際特許分類】
【出願日】平成２３年２月２４日（２０１１．２．２４）
【出願人】（００００００３７６）オリンパス株式会社 (11,466)
【Ｆターム（参考）】
[ Back to top ]