Master operation input device, and master-slave manipulator |
|||||||
申请号 | JP2011038796 | 申请日 | 2011-02-24 | 公开(公告)号 | JP2012171088A | 公开(公告)日 | 2012-09-10 |
申请人 | Olympus Corp; オリンパス株式会社; | 发明人 | OGAWA RYOHEI; | ||||
摘要 | PROBLEM TO BE SOLVED: To provide a master operation input device that can detect the position/attitude of an operation part with a structure stronger against failure, and a master-slave manipulator including the same.SOLUTION: A command value of the position/attitude of a holding part arranged in an input part of this master operation input device is calculated based on an output signal of an encoder for detecting a drive amount of a joint mounted to the holding part, and calculated based on an image obtained by an imaging sensor. Error processing is performed when there is a difference equal to or larger than a predetermined value between the command value calculated in accordance with the output signal of the encoder and the command value calculated in accordance with the image obtained by the imaging sensor. | ||||||
权利要求 | 複数の自由度に対応した関節を有するスレーブマニピュレータを操作するためのマスタ操作入力装置であって、 操作者による操作によって位置及び姿勢を変化自在であって、該位置及び姿勢の変化によって前記スレーブマニピュレータの位置及び姿勢の指令値を与えるための操作部と、 前記操作部の位置及び姿勢を検出するために、前記操作部に係る異なる物理量をそれぞれ検出する2系統以上の検出部と、 を具備することを特徴とするマスタ操作入力装置。 前記操作部は、複数の自由度に対応した関節に取り付けられており、 前記検出部は、 前記物理量として複数の前記関節の回転量又は変位量をそれぞれ検出する複数の第1のセンサと、 複数の前記関節の回転量又は変位量と異なる物理量を検出する第2のセンサと、 を有することを特徴とする請求項1に記載のマスタ操作入力装置。 前記第1のセンサは、前記複数の関節のそれぞれに配置され、 前記第2のセンサは、前記複数の関節に対して遠隔位置に配置されていることを特徴とする請求項2に記載のマスタ操作入力装置。 前記第2のセンサは、前記操作部の位置及び姿勢を検出するために前記操作部の画像を取得する撮像センサを含むことを特徴とする請求項2又は3に記載のマスタ操作入力装置。 前記第2のセンサは、 前記操作部の位置を検出するために前記操作部の画像を取得する撮像センサと、 前記操作部に発生した角速度を前記操作部の姿勢として検出する角速度センサと、 を含むことを特徴とする請求項2又は3に記載のマスタ操作入力装置。 前記第2のセンサは、 前記操作部の位置及び姿勢を検出するために、前記操作部の加速度を取得する加速度センサと、前記操作部の角速度を取得する角速度センサと、 を含むことを特徴とする請求項2又は3に記載のマスタ操作入力装置。 前記第2のセンサは、前記操作部の位置及び姿勢を検出する超音波センサ又は磁気センサであることを特徴とする請求項2又は3に記載のマスタ操作入力装置。 請求項1乃至7の何れか1項に記載のマスタ操作入力装置と、 前記2系統以上の検出部のそれぞれで検出された物理量に従って前記操作部の位置及び姿勢を複数算出する第1の制御部と、 前記第1の制御部によって算出された前記操作部の位置及び姿勢から前記スレーブマニピュレータの位置及び姿勢の指令値を算出し、該算出したスレーブマニピュレータの位置及び姿勢の指令値に従って前記スレーブマニピュレータを駆動制御する第2の制御部と、 を有することを特徴とするマスタスレーブマニピュレータ。 前記第1の制御部は、前記2系統以上の検出部のそれぞれで検出された物理量に従って複数算出された前記操作部の位置及び姿勢の差が所定の範囲内であるか否かを判定し、前記所定の範囲内である場合には前記スレーブマニピュレータの位置及び姿勢の指令値を算出し、前記所定の範囲内でない場合にはエラー処理をすることを特徴とする請求項8に記載のマスタスレーブマニピュレータ。 |
||||||
说明书全文 | 本発明は、スレーブマニピュレータを遠隔操作するためのマスタ操作入力装置及びそのようなマスタ操作入力装置を有するマスタスレーブマニピュレータに関する。 近年、医療施設の省人化を図るため、ロボットによる医療処置の研究が行われている。 特に、外科分野では、多自由度(多関節)アームを有するマニピュレータによって患者の処置をするマニピュレータシステムについての各種の提案がなされている。 このようなマニピュレータシステムにおいて、患者の体腔に直接接触するマニピュレータ(スレーブマニピュレータ)を、スレーブマニピュレータに対して遠隔に配置されたマスタ操作入力装置によって遠隔操作できるようにしたマニピュレータシステム(マスタスレーブマニピュレータ)が知られている。 通常、マスタスレーブマニピュレータにおいては、マスタ操作入力装置の操作部の位置・姿勢が指令値として制御装置に入力される。 制御装置においては操作部の位置・姿勢の指令値に従ってスレーブマニピュレータの先端部の逆運動学を解くことによってスレーブマニピュレータの各関節の駆動量が算出され、この駆動量に従って各関節が駆動制御される。 このような制御を行うためには、マスタ操作入力装置の操作部の位置・姿勢を検出することが必要である。 従来、マスタ操作入力装置の位置・姿勢は、マスタ操作入力装置の操作部に取り付けられたセンサにより検出されている。 ここで、操作部の位置・姿勢を検出するためのセンサを1系統のみとすると、この1系統のセンサが故障した場合には位置・姿勢が検出できなくなってしまう。 これを回避するための手法として、センサを二重化する手法が知られている(例えば、特許文献1参照)。 特許文献1の技術をマスタ操作入力装置に対して適用することにより、1つの系統のセンサが故障しても別の系統のセンサを用いてマスタ操作入力装置の操作部の位置・姿勢を検出することが可能となる。 ここで、特許文献1の技術の場合、二重化したセンサはともに同じ物理量(例えば特許文献1の場合にはピッカの変位量)を検出している。 この場合、一方の系統のセンサが故障する際に、もう一方の系統のセンサも同時に故障してしまう可能性が高い。 上記の目的を達成するために、本発明の第1の態様のマスタ操作入力装置は、複数の自由度に対応した関節を有するスレーブマニピュレータを操作するためのマスタ操作入力装置であって、操作者による操作によって位置及び姿勢を変化自在であって、該位置及び姿勢の変化によって前記スレーブマニピュレータの位置及び姿勢の指令値を与えるための操作部と、前記操作部の位置及び姿勢を検出するために、前記操作部に係る異なる物理量をそれぞれ検出する2系統以上の検出部と、を具備することを特徴とする。 また、上記の目的を達成するために、本発明の第2の態様のマスタスレーブマニピュレータは、第1の態様に記載のマスタ操作入力装置と、前記2系統以上の検出部のそれぞれで検出された物理量に従って前記操作部の位置及び姿勢を複数算出する第1の制御部と、前記第1の制御部によって算出された前記操作部の位置及び姿勢から前記スレーブマニピュレータの位置及び姿勢の指令値を算出し、該算出したスレーブマニピュレータの位置及び姿勢の指令値に従って前記スレーブマニピュレータを駆動制御する第2の制御部と、を有することを特徴とする。 本発明によれば、より故障に強い構成で操作部の位置・姿勢を検出することが可能なマスタ操作入力装置及びそのようなマスタ操作入力装置を備えるマスタスレーブマニピュレータを提供することが可能である。 以下、図面を参照して本発明の実施形態を説明する。 マスタ操作入力装置10は、本マスタスレーブマニピュレータにおけるマスタとして機能するものであって、入力部11と、表示部12と、撮像センサ13とを有している。 表示部12は、例えば液晶ディスプレイから構成され、制御装置20から入力された画像信号に基づいて画像を表示する。 後述するが、制御装置20から入力される画像信号は、スレーブマニピュレータに取り付けられた電子カメラ(電子内視鏡)を介して得られた画像信号を、制御装置20において処理したものである。 このような画像信号に基づく画像を、表示部12に表示させることにより、マスタ操作入力装置10の操作者1は、マスタ操作入力装置10から離れた場所に配置されたスレーブマニピュレータ30の先端の画像を確認することが可能である。 撮像センサ13は、例えば所定の間隔を空けて配置された2眼の撮像センサである。 この撮像センサ13は、撮像により、所定の視差を有する2枚の入力部11の画像を生成する。 撮像センサ13の詳細については後述する。 第2の制御部の一例として機能するマニピュレータ制御部22は、マスタ制御部21からの位置・姿勢の指令値を受けて、スレーブマニピュレータ30の先端の位置・姿勢を指令値に一致させるために必要なスレーブマニピュレータの各関節の駆動量を、例えば逆運動学計算によって算出する。 そして、マニピュレータ制御部22は、算出した駆動量に従ってスレーブマニピュレータの各関節を駆動させる。 画像処理部23は、スレーブマニピュレータの先端に設けられた電子カメラ(電子内視鏡等)から得られた画像信号を処理し、表示部12の表示用の画像信号を生成して表示部12に出力する。 図2は、本実施形態におけるマスタ操作入力装置の入力部11の位置・姿勢を検出するためのセンサの二重化について示した図である。 また、それぞれの関節の近傍には、関節の回転量又は直動量を検出するためのセンサ(例えばエンコーダ)200、202、204、206、208、210が設けられている。 各エンコーダの出力信号は、マスタ制御部21に入力される。 図2では、エンコーダ200は、把持部100から見て最も遠い位置に設けられた関節の回転量θ 1に応じた信号を出力するセンサである。 エンコーダ202は、把持部100から見て次に遠い位置に設けられた関節の直動量d 1に応じた信号を出力するセンサである。 以下同様に、エンコーダ204は関節の直動量d 2に応じた信号を出力するセンサであり、エンコーダ206は関節の回転量θ 2に応じた信号を出力するセンサであり、エンコーダ208は関節の回転量θ 3に応じた信号を出力するセンサである。 そして、エンコーダ210は、把持部100から見て最も近い位置に設けられた関節の回転量θ 4に応じた信号を出力するセンサである。 さらに、把持部100には、3つの反射マーカ(以下、マーカという)450、452、454が設けられている。 各マーカは、撮像センサ13で得られた画像中で高輝度となるよう、高反射率の材料から形成されている。 ここで、マーカ450とマーカ452との間隔、マーカ452とマーカ454との間隔、及びマーカ454とマーカ450との間隔がそれぞれ異なるように、反射マーカ450、452、454を把持部100上に配置しておくことが望ましい。 また、3つのマーカ450、452、454が一直線上に並ばないような配置で、さらに、把持部100即ち操作部の姿勢を変えてもマーカが撮像センサ13に認識できるような配置しておくことが望ましい。 これは、画像上でマーカ450、452、454を抽出する際に、それぞれのマーカを正しく識別できるようにするためである。 撮像センサ13で取得する操作部の特徴点は反射マーカに限らない。 例えば、LED等の発光体であっても良い。 撮像センサ13は、2眼の撮像系402、404を有している。 撮像系402、404は、それぞれが、撮像光学系と、撮像素子とを有しており、入力部11に対して所定距離だけ離れて配置されている。 また、撮像系402と撮像系404も予め定められた距離だけ離間するように配置されている。 撮像光学系は、入射した光束を撮像素子の受光面上に結像させるための光学系である。 撮像素子は、撮像光学系を介して入射した光束を電気信号に変換して把持部100の位置・姿勢を得るための画像を生成するセンサである。 撮像素子で生成された画像は、マスタ制御部21に入力される。 以下、本実施形態のマスタスレーブマニピュレータの動作について説明する。 図3は、本発明の第1の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。 図3は、特にマスタ制御部21の動作について示している。 マスタ制御部21は、撮像系402、404のそれぞれを介して得られた画像から特徴点としてのマーカ450、452、454を抽出する。 画像内のマーカ450、452、454を抽出する手法としては例えば周知のパターンマッチング法を用いることが可能である。 画像内のマーカ450、452、454の抽出後、マスタ制御部21は、取得した画像内に3次元の座標系(カメラ座標系)Cを設定し、このカメラ座標系Cに対応して初期設定時の把持部100の位置・姿勢を示す姿勢行列Pを算出する。 例えば、カメラ座標系Cとして、画像の水平方向をX軸、垂直方向をY軸、奥行き方向をZ軸とし、マーカ454の座標を原点に設定する。 このとき、姿勢行列Pは、カメラ座標系C内に設定したX軸の方向を示す単位ベクトルと、Y軸の方向を示す単位ベクトルと、Z軸の方向を示す単位ベクトルを用いた3×3の行列、もしくは、位置ベクトルも含めた4×4の行列で表わすことができる。 姿勢行列Pを算出した後、マスタ制御部21は、カメラ座標系Cの姿勢行列Pをワールド座標系(例えば、地表を基準とした座標系)Wで見た姿勢行列とするための変換行列Qを算出する。 変換行列Qの算出により、初期設定が終了する。 また、マスタ制御部21は、撮像センサ13で得られた画像から把持部100の位置・姿勢の指令値Bを算出する(ステップS3)。 また、画像内の把持部100の姿勢変化(回転量)は、位置・姿勢の変化前に撮像センサ13で得られた把持部100の画像と位置・姿勢の変化後に撮像センサ13で得られた把持部100の画像との間での原点位置のマーカ(例えばマーカ454)に対する残りのマーカの位置変化から算出することが可能である。 カメラ座標系Cで見た把持部100の位置・姿勢の指令値を算出した後、マスタ制御部21は、把持部100の位置の変化量によって表わされる平行移動を示す行列と、把持部100の回転量によって表わされる回転行列とを、姿勢行列Pに左から乗じることにより、位置・姿勢の変化後のカメラ座標系Cで見た姿勢行列P'を算出する。 その後、マスタ制御部21は、姿勢行列P'に変換行列Qを乗じることにより、ワールド座標系で見た把持部100の位置・姿勢の指令値Bを算出する。 位置・姿勢の指令値A、Bの算出後、マスタ制御部21は、指令値Aと指令値Bとの差の絶対値が所定値Th以下であるか否かを判定する(ステップS4)。 指令値Aと指令値Bは、ともに操作者1による操作後の把持部100の位置・姿勢を表わすものであり、本来は略一致するものである。 このため、指令値Aと指令値Bとの差の絶対値が所定値Th以下であるか否かを判定することにより、エンコーダ200〜210と撮像センサ13の何れかが故障しているか否かを判定することが可能である。 ステップS4の判定において、指令値Aと指令値Bとの差の絶対値が所定値Th以下である場合に、マスタ制御部21は、指令値Aと指令値Bの何れか(又は両者の平均値)をマニピュレータ制御部22に出力する(ステップS5)。 これを受けて、マニピュレータ制御部22は、スレーブマニピュレータ30の先端部の位置・姿勢をマスタ制御部21から入力された位置・姿勢の指令値とするために必要な各関節の駆動量を、逆運動学計算を解くことにより算出する。 その後、マニピュレータ制御部22は、算出した駆動量に従ってスレーブマニピュレータ30の各関節を駆動制御する。 このようにしてマスタスレーブマニピュレータの一連の動作が終了する。 また、ステップS4の判定において、指令値Aと指令値Bとの差の絶対値が所定値Thを越えている場合に、マスタ制御部21は、エラー処理を行う(ステップS6)。 エラー処理とは、例えば算出した指令値をマニピュレータ制御部22に送らないようにする処理や、エンコーダ200〜210と撮像センサ13の何れかが故障していることを操作者1に警告する処理、マスタスレーブマニピュレータのシステム自体を遮断してしまう処理等が考えられる。 このエラー処理の場合にも、マスタスレーブマニピュレータの一連の動作が終了する。 以上説明したように、本実施形態によれば、入力部11に設けられた関節の変位量及び回転量を検出する第1のセンサとしてのエンコーダと、撮像によって画像を得る第2のセンサとしての撮像センサと、いう異なる物理量をそれぞれ検出する2系統のセンサから把持部100の位置・姿勢の指令値を算出している。 これにより、例えば故障の検出が容易である。 また、図3では指令値Aと指令値Bとに所定以上の差がある場合にはエラー処理を行うようにしているが、その後の検査によりエンコーダの出力信号と撮像センサの何れが故障しているかが判別された場合には、その後は故障していない側を用いてスレーブマニピュレータ30の駆動制御を行うこともできる。 このため、故障にも強い。 先行技術のように、センサの二重化として同一関節上に、同じ物理量を取得するために2種類のセンサ(例えばエンコーダとリミットスイッチやエンコーダとポテンショメータの組み合せ)を配置することがあったが、この場合、熱や水没で同時に2つのセンサが故障することがあった。 本実施形態のように、撮像センサで得られた画像から把持部100の位置・姿勢の指令値を算出する場合、入力部11の関節上に撮像センサを配置する必要がない。 このため、エンコーダと撮像センサが同時に故障するような事態も回避することが可能である。 また、上述の例では、画像内でマーカ450、452、454を識別するために、マーカ間の距離を異ならせるようにしているが、例えばマーカの反射率や直径、色を異ならせるようにしても良い。 マーカの代わりにLEDのような発光体を撮像センサに認識させる場合は、発光パターン(発光間隔)を変化させても良い。 このようなマーカは、前述の反射マーカなどのパッシブマーカに対して、アクティブマーカとして、その認識方法には様々な技術が知られている。 図5は、本実施形態におけるマスタ操作入力装置の入力部11の位置・姿勢を検出するためのセンサの二重化について示した図である。 なお、図5において図2と同一の部分については説明を省略する。 図5においては、把持部100に設けられたマーカの数が1個であり、また、把持部100にジャイロセンサ600が設けられている点が異なっている。 ジャイロセンサ600は、ワールド座標系WのXYZ軸周りの把持部100に発生した角速度に応じた信号を出力する3軸のセンサである。 この角速度信号を積分することにより、把持部100の姿勢変化量としての回転量を検出する。 まず、第1の実施形態と同様にマスタスレーブマニピュレータの初期設定が行われる(ステップS11)。 ここで、第2の実施形態の初期設定においては、姿勢の変換行列を求める必要はなく、位置の変換行列のみを求めれば良い。 位置の変換行列は、カメラ座標系C内に設定したXYZ軸に対する平行移動を示す行列をワールド座標系(例えば、地表を基準とした座標系)Wで見た平行移動を示す行列とするための変換行列である。 初期設定の後、操作者1が把持部100を操作すると、その操作に応じて把持部100の位置・姿勢が変化し、この位置・姿勢の変化に応じてエンコーダ200〜210からの出力信号が変化する。 マスタ制御部21は、エンコーダ200〜210からの出力信号によって示される直動量、回転量を用いて運動学計算を解くことにより、把持部100の位置・姿勢の指令値Aを算出する(ステップS12)。 また、マスタ制御部21は、撮像センサ13で得られた画像から把持部100の位置を算出するとともに、ジャイロセンサ600の出力信号から把持部100の姿勢を算出し、これらを位置・姿勢の指令値Bとする(ステップS13)。 ステップS14以後の処理は図3のステップS4〜S6の処理と同様であるので説明を省略する。 また、画像から把持部100の姿勢の指令値を算出するのではなく、ジャイロセンサの出力から把持部100の指令値を算出することにより、より姿勢の指令値の算出精度を高めることが可能である。 [第3の実施形態] 加速度センサ650は、ワールド座標系WのXYZ軸のそれぞれに平行な加速度に応じた信号を出力する3軸のセンサである。 この加速度信号を2回積分することにより、把持部100の位置変化量としての変位量を検出する。 操作者1が把持部100を操作すると、その操作に応じて把持部100の位置・姿勢が変化し、この位置・姿勢の変化に応じてエンコーダ200〜210からの出力信号が変化する。 マスタ制御部21は、エンコーダ200〜210からの出力信号によって示される直動量、回転量を用いて運動学計算を解くことにより、把持部100の位置・姿勢の指令値Aを算出する(ステップS21)。 また、マスタ制御部21は、ジャイロセンサ600の出力信号から把持部100の姿勢の指令値を算出するとともに、加速度センサ650の出力信号から把持部100の位置の指令値を算出する。 その後、マスタ制御部21は、加速度センサ650を介して得られた把持部100の位置の指令値とジャイロセンサ600を介して得られた把持部100の姿勢の指令値とを指令値Bとする(ステップS22)。 ステップS23以後の処理は図3のステップS4〜S6の処理と同様であるので説明を省略する。 [第4の実施形態] 超音波センサ700は、超音波発生器750、752、754から発生された超音波信号を検出する。 超音波発生器750、752、754はそれぞれ周波数の異なる超音波を発生させる。 超音波発生器750、752、754から発生された超音波信号が超音波センサ700で受信されるまでの時間を検出することにより、超音波センサ700と超音波発生器750との距離、超音波センサ700と超音波発生器752との距離、超音波センサ700と超音波発生器754との距離をそれぞれ算出することが可能である。 これらの距離から把持部100の3次元的な位置・姿勢を算出することが可能である。 以下、本実施形態のマスタスレーブマニピュレータの動作について説明する。 図11は、本発明の第4の実施形態に係るマスタスレーブマニピュレータの動作について示すフローチャートである。 なお、図11において図9と同様の部分については説明を省略する。 操作者1が把持部100を操作すると、その操作に応じて把持部100の位置・姿勢が変化し、この位置・姿勢の変化に応じてエンコーダ200〜210からの出力信号が変化する。 マスタ制御部21は、エンコーダ200〜210からの出力信号によって示される直動量、回転量を用いて運動学計算を解くことにより、把持部100の位置・姿勢の指令値Aを算出する(ステップS31)。 また、マスタ制御部21は、超音波センサ700の出力信号から把持部100の位置・姿勢の指令値Bを算出する(ステップS32)。 ここで、第4の実施形態では、超音波センサを用いて把持部100の位置・姿勢を検出しているが、例えば磁気センサを用いて把持部100の位置・姿勢を検出しても良い。 さらに、上記した実施形態には種々の段階の発明が含まれており、開示される複数の構成要件の適当な組合せにより種々の発明が抽出され得る。 例えば、実施形態に示される全構成要件からいくつかの構成要件が削除されても、上述したような課題を解決でき、上述したような効果が得られる場合には、この構成要件が削除された構成も発明として抽出され得る。 10…マスタ操作入力装置、11…入力部、12…表示部、13…撮像センサ、20…制御装置、21…マスタ制御部、22…マニピュレータ制御部、23…画像処理部、30…スレーブマニピュレータ、100…把持部、200〜210…エンコーダ、402,404…撮像系、410…撮像センサ、450〜454…反射マーカ、600…ジャイロセンサ、650…加速度センサ、700…超音波センサ、750〜754…超音波発生器 |