(57)【要約】      （修正有）【課題】マスタマニピュレータとスレーブマニピュレータの両方の先端に工具を装着したマスタスレーブマニピュレータにおいて、マスターマニピュレータとスレーブマニピュレータとで異なる動きをさせることが可能なマスタスレーブマニピュレータを提供する。【解決手段】マスタマニピュレータ１に力センサＳ２を介して配置した操作ハンドル３と、マスタマニピュレータに操作ハンドルと並列になるように配置したジョイスティック６と、ジョイスティックでの操作対象をマスターマニピュレータ１かスレーブマニピュレータ２かに切り換える切り換えスイッチ手段９ｂと制御装置８とを備えた。これによって、操作ハンドルによるマスタスレーブ操作に加え、切り換えスイッチで指定されたマニピュレータはジョイスティックによる操作も可能となる。【選択図】図１
2006102866.tif 000002
【特許請求の範囲】
【請求項１】
操作者によるマスタマニピュレータの操作に基づいてスレーブマニピュレータを制御するマスタスレーブマニピュレータの制御装置において、
前記マスタマニピュレータに力センサを介して配置された操作ハンドルと、
前記マスタマニピュレータに前記操作ハンドルと並列になるように配置され、前記マスタマニピュレータまたは前記スレーブマニピュレータを操作するジョイスティックと、
前記ジョイスティックの操作対象とするマニピュレータを切り換える切り換え手段と、
前記ジョイスティックに対する操作入力を有効または無効に設定するスイッチと、
前記力センサで検出した前記操作ハンドルに作用する前記操作者の操作力と前記切り換え手段の出力と前記スイッチの出力とに従い前記マスタマニピュレータと前記スレーブマニピュレータを協調制御する協調制御手段とを備えたことを特徴とするマスタスレーブマニピュレータの制御装置。
【請求項２】
前記協調制御手段は、前記入力有効スイッチが有効の場合に、予め定められた関数に基づき前記力センサで検出した前記操作ハンドルに作用する操作力に応じて前記ジョイスティックに対する操作入力を調整することを特徴とする請求項１記載のマスタスレーブマニピュレータの制御装置。
【請求項３】
前記協調制御手段は、前記スイッチが有効となった時点をゼロ点として前記ジョイスティックに対する操作入力を補正する補正手段を備えたことを特徴とする請求項１または２記載のマスタスレーブマニピュレータの制御装置。
【請求項４】
前記ジョイスティックは、前記操作ハンドルを回転中心として回転自在に前記マスタマニピュレータに設置されていることを特徴とする請求項１乃至３記載のマスタスレーブマニピュレータの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、マスタスレーブ型のマニピュレータの制御装置に関する。
【背景技術】
【０００２】
操作者の指令に従い、物体を把持して所定の位置に移動して位置決めするマニピュレータとしては、入力レバーを操作してこの入力レバーの位置に対応した速度で従動側のスレーブマニピュレータを移動させるジョイスティック型（操縦型）のマニピュレータと、入力装置であるマスタマニピュレータを操作することによりスレーブマニピュレータに同じ動きをさせるマスタスレーブ型のマニピュレータとがある。
この種のマニピュレータとして、特許文献１には物体を把持するグリッパなどのエンドエフェクタの３自由度操作に関しては、マスタスレーブ方式を用い、エンドエフェクタの位置決めを行なうアームの位置の３自由度操作に関してはジョイスティック方式を用いるマニピュレータが記載されている。
上記２つの方式のうち、ジョイスティック型マニピュレータは、入力レバーの位置とスレーブマニピュレータの速度とが対応するので、操作者は小さい動作でスレーブマニピュレータを大きく移動することが可能であるが、スレーブマニピュレータの正確な位置決めや、精細な動作を行なうことができないという問題があった。
一方、マスタスレーブ型マニピュレータは、入力装置であるマスタマニピュレータの位置と出力装置であるスレーブマニピュレータの位置とが対応するので、スレーブマニピュレータの正確な位置決めや精細な動作を行なうことはできるが、スレーブマニピュレータを大きく移動させる場合は操作者が大きい動作を行なう必要があるという問題があつた。
このような問題を解決するため、スレーブマニピュレータの正確な位置決めや精細な動作を可能とし、かつマスタマニピュレータの小さい動作でスレーブマニピュレータを大きく移動させることのできる、操作性のよいマスタスレーブマニピュレータの操作装置が提案されている（例えば、特許文献２参照）。図６は、従来のマスタスレーブマニピュレータの操作装置の概略構成を示す図である。
図において、１０４はマスタマニピュレータであり、関節部にはアームを駆動するアクチュエータと、各関節の回動角、回動角速度をそれぞれ検出する角度検出器及び角速度検出器などが設けられている。１０７はスレーブマニピュレータであり、マスタマニピュレータ１０４の先端には、操作者が握る入力レバー１０６とこの操作者が加えた力及びトルクを検出する力トルク検出器１０５とが取り付けられている。またスレーブマニピュレータ１０７の先端には被作業物を把持するグリッパなどのエンドエフェクタ１０９と、このエンドエフェクタ１０９に加えられた力及びトルクを検出する力トルク検出器１０８が取り付けられている。１０１は制御装置であり、スイッチ１１０と、力トルク検出器１０５、１０８からそれぞれ出力される信号が伝達される信号号線１１１、１１２とが接続されている。この出力信号はＸ、Ｙ、Ｚ軸方向の力信号と各軸回りのトルク信号との６個の信号である。また矢印１１４、１１３はそれぞれマスタマニピュレータ１０４及びスレーブマニピュレータ１０７の各関節の角度検出器及び角速度検出器から出力され、制御装置１０１に入力される信号である。さらに１０２、１０３は制御装置１０１から出力されて、それぞれマスタマニピュレータ及びスレーブマニピュレータの各関節に設けられたアクチエータを駆動する駆動信号を増幅するサーボ増幅器である。
制御装置１０１は、マスタマニピュレータ１０４の位置に対応してスレーブマニピュレータ１０７の位置が移動するマスタスレーブモードとマスタマニピュレータ１０４の位置に対応した速度でスレーブマニピュレータ１０７が移動するジョイスティックモードとを切り換える切換手段と、ジョイスティックモードの開始位置からのマスタマニピュレータ１０４の移動量に前記スレーブマニピュレータ１０７の速度を対応させる制御手段と、マスタマニピュレータ１０４の負荷が零になったとき、マスタマニピュレータ１０４をジョイスティックモードの開始位置へ戻す制御手段とを備える。そして切換手段によりマスタスレーブモードを選択すると、マスタマニピュレータ１０４とスレーブマニピュレータ１０７との位置が対応するので、スレーブマニピュレータ１０７の正確な位置決めや精細な動作を行なうことができる。また切換手段によりジョイスティックモードを選択すると、ジョイスティックモードの開始位置からのマスタマニピュレータ１０４の移動量とスレーブマニピュレータ１０７の速度が対応するので、操作者はマスタマニピュレータ１０４を中立点に戻す操作を必要とせず、ジョイスティックモードの開始位置を中立点として、マスタマニピュレータ１０４の小さい動作でスレーブマニピュレータを大きく移動させることができる。
【０００３】
【特許文献１】特開昭６１−９５８８６号公報
【特許文献２】特許第２６５６５８４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
従来のマスタスレーブマニピュレータの制御装置は、マスタマニピュレータを単に操作者の操作入力の手段として使用する場合の操作性向上を目指したものであり、マスタマニピュレータ側は、操作者の手以外の拘束力は作用しないことを前提としている。
ところがマスタマニピュレータの先端にもスレーブアームと同様に工具を装着して作業可能なマニピュレータとし、作業対象物に対してマスタマニピュレータとスレーブマニピュレータの両方で作業するような場合（例えば、マスタマニピュレータ側で作業対象物を把持したままスレーブマニピュレータ側にて作業対象物に対して微細な位置姿勢が必要となる場合）、従来の制御装置ではマスタマニピュレータが把持した作業対象物そのものが動いたり、作業対象部からの反力によりスレーブアームの所望の位置姿勢を得ることが困難になったりするという問題があった。
本発明はこのような問題点に鑑みてなされたものであり、マスタマニピュレータとスレーブマニピュレータの両方の先端に工具を装着したマスタスレーブマニピュレータの操作性と安全性を向上させるマスタスレーブマニピュレータの制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、操作者によるマスタマニピュレータの操作に基づいてスレーブマニピュレータを制御するマスタスレーブマニピュレータの制御装置において、前記マスタマニピュレータに力センサを介して配置された操作ハンドルと、前記マスタマニピュレータに前記操作ハンドルと並列になるように配置され、前記マスタマニピュレータまたは前記スレーブマニピュレータを操作するジョイスティックと、前記ジョイスティックの操作対象とするマニピュレータを切り換える切り換え手段と、前記ジョイスティックに対する操作入力を有効または無効に設定するスイッチと、前記力センサで検出した前記操作ハンドルに作用する前記操作者の操作力と前記切り換え手段の出力と前記スイッチの出力とに従い前記マスタマニピュレータと前記スレーブマニピュレータを協調制御する協調制御手段とを備えたことを特徴とする。
請求項２に記載の発明は、前記協調制御手段は、前記入力有効スイッチが有効の場合に、予め定められた関数に基づき前記力センサで検出した前記操作ハンドルに作用する操作力に応じて前記ジョイスティックに対する操作入力を調整することを特徴とする。
請求項３に記載の発明は、前記協調制御手段は、前記スイッチが有効となった時点をゼロ点として前記ジョイスティックに対する操作入力を補正する補正手段を備えたことを特徴とする。
請求項４に記載の発明は、前記ジョイスティックは、前記操作ハンドルを回転中心として回転自在に前記マスタマニピュレータに設置されていることを特徴とする。
【発明の効果】
【０００６】
請求項１に記載の発明によると、マスタマニピュレータを直接操作しながら、スレーブマニピュレータの操作も行え、スレーブマニピュレータはマスタマニピュレータと協調動作するため安全で操作性が向上する。また請求項２に記載の発明によると、マスタマニピュレータの動作量に応じてスレーブマニピュレータの操作量が調整されるため、操作者の意図に沿った操作感覚を提供でき操作性が向上する。また請求項３に記載の発明によると、操作者のジョイスティックの把持位置にかかわらず一定の操作量を提供できるため操作性が向上する。また、請求項４に記載の発明によると、ジョイスティックを操作者の操作姿勢に合わせることが容易となるため操作性が向上する。
【発明を実施するための最良の形態】
【０００７】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【０００８】
図１は、本発明の請求項１乃至４に係るマスタスレーブマニピュレータの実施例を示す構成図である。図において、１はマスタマニピュレータである。複数の関節部で構成され、各関節部にはアームを駆動するアクチュエータと、各関節の回動角、回動角速度をそれぞれ検出する角度検出器及び角速度検出器が設けられている（図示せず）。
２はスレーブマニピュレータであり、マスタマニピュレータ１と同様に複数の関節部で構成され、各関節部にはアームを駆動するアクチュエータと、各関節の回転角、回転角速度をそれぞれ検出する角度検出器及び角速度検出器が設けられている（図示せず）。
マスタマニピュレータ１の先端部にはマスタマニピュレータ１の根元側から順に、操作者１９が握る操作ハンドル３の固定部４、第１工具５に加えられた力及びトルクを検出する力センサＳ１、第１工具５が設置されている。
第１工具５は、具体的には作業台１８上の被作業物１７を把持するグリッパなどであり、固定部４と操作ハンドル３の間には、操作者１９が加えた力及びトルクを検出する力センサＳ２が取り付けられている。固定部４からは更に、操作ハンドル３と並列するようにジョイスティック６が取り付けられている。
【０００９】
またスレーブマニピュレータ２の先端部には被作業物１７を把持するグリッパなどの第２工具７と、この第２工具７に加えられた力及びトルクを検出する力センサＳ３が取り付けられている。８は本実施例の特徴である協調制御装置であり、力センサＳ１、Ｓ２、Ｓ３からそれぞれ出力される信号が伝達される信号線１０、１１、１２と、操作ハンドル３に付設された切り換えスイッチ９ｂ及びジョイスティック６に付設された入力有効スイッチ９ａの出力信号が伝達される信号線が接続されている。力センサの出力信号はＸ、Ｙ、Ｚ軸方向の力信号と各軸回りのトルク信号との６個の信号である。
ジョイスティック６に対する操作は入力有効スイッチ９ａが押下されている間のみ有効となり、切り換えスイッチ９ｂはジョイスティック６によって操作する対象をマスタマニピュレータ１またはスレーブマニピュレータ２に設定する際に用いる。
また図中の１３、１４はそれぞれマスタマニピュレータ１、スレーブマニピュレータ２の各関節の角度検出器及び角速度検出器から出力され、協調制御装置８に入力される信号である。さらに１５及び１６はサーボ増幅器であり、協調制御装置８から出力されてそれぞれマスタマニピュレータ１及びスレーブマニピュレータ２の各関節のアクチュエータを駆動する駆動信号を増幅する。
【００１０】
次に協調制御装置８の構成を図２にて説明する。マスタマニピュレータ１の固定部４に設けられた力センサＳ２の出力は信号線１１を介して座標変換器４０に接続されている。力センサＳ２から出力される力トルク信号は力センサ座標系から共通座標系へ変換されｆ２として出力される。
同様にスレーブマニピュレータ２に設けられた力センサＳ３の出力は信号線１２を介して座標変換器４５に接続されており、力センサＳ３から出力される力トクル信号は力センサ座標系から共通座標系へ変換されｆ３として出力される。
また、マスタマニピュレータ１に設けられた力センサＳ１の出力は信号線１０を介して座標変換器４２に接続されており、力センサＳ１から出力される力トルク信号は力センサ座標系から共通座標系へ変換されｆ１として出力される。
２０は、入力有効スイッチ９ａが押下された時点をゼロ点としてジョイスティック６の操作入力を補正し出力するゼロ点補正部であり、２１は、入力有効スイッチ９ａが押下されている場合に力センサＳ２で検出した操作ハンドル３に作用する操作者１９の操作力に応じ、予め定められた関数に基づいてゼロ点補正部２０の出力を補正する補正関数部である。
補正関数部２１から出力されたジョイスティック６の速度指令は、積分器２２にて位置指令に変換され、切り換え器２３に導かれる。切り換え器２３は切り換えスイッチ９ｂの信号によってマスタ側接点２３ｂ、スレーブ側接点２３ａのどちらに位置指令を送るかを切り換える。
マスタ側接点２３ｂには力制御部４１が接続され、力制御部４１の出力は反力生成部４３の出力と加算される。さらにヤコビ変換器４４にて、共通座標系でのトルク指令がマスタマニピュレータ１の各関節のトルク指令に変換されサーボ増幅器１５に出力される。
同様にスレーブ側接点２３ａには力制御部４６が接続され、ヤコビ変換器４７にて共通座標系でのトルク指令がスレーブマニピュレータ２の各関節のトルク指令に変換されサーボ増幅器１６に出力される。
【００１１】
ここで力制御部４１、４６と反力生成部４３についてそれぞれ図３と図４を用いて説明する。力制御部４１、４６は制御構造が同じであるため、その内部構成については図３にてまとめて説明する。
図３に示す力制御部４１、４６の制御方式は、公知技術である力トルクベースのコンプライアンス制御方式（文献「ロボット工学ハンドブック p.２５４、日本ロボット学会、１９９０、コロナ社」にみられる方式）であり、図におけるＤｖ、 Ｋｄはそれぞれ、仮想粘性、仮想剛性を表し、Ｋｆは力フィードバックゲインを表す。力フィードバックゲインＫｆを大きくし定常偏差を小さくすることで仮想粘性Ｄｖと仮想剛性Ｋｄを所望の値に任意に調整できる。
【００１２】
図４に示す反力生成部４３は、力センサＳ１の出力ｆ１を比例ゲインＨＫにより増幅した出力と微分器４１１を介して比例ゲインＨＤにより増幅した出力とを加算した値τ３を出力する。
【００１３】
続いて図１を例として実際の動作を説明する。操作者１９が操作ハンドル３を左手で押すことにより力制御部４１への操作入力f２が発生するため、マスタマニピュレータ１の第１工具５は、作業台１８上の被作業物１７に接近する。接近に伴い操作者１９が左手で操作ハンドル３の切り換えスイッチ９ｂを使ってスレーブ側接点２３ａへ切り換えてから右手でジョイスティック６を倒すと、操作指令ｒｅｆ２が力制御部４６に入力されスレーブマニピュレータ２が動作する。
このとき補正関数部２１では、図５に示すように予め定められた関数Ｆにより、操作力f２から速度指令の傾きＡＮＧを算出しジョイスティック６の出力を調整する。つまり、操作ハンドル３に作用する操作力が低下したら自動的にジョイスティック６の動作速度が実際よりも低速に調整され、逆に操作力が大きくなれば自動的にジョイスティック６の動作速度が実際よりも高速に調整され、操作者１９の意図を自動的に解してマスタマニピュレータ１とスレーブマニピュレータ２が協調的に動作することが可能となる。
次に、第１工具５が被作業物１７に接触すると、接触力を力センサＳ１が検出し、反力τ３が反力生成部４３で生成された後、加算器２４で加算され、マスタマニピュレータ１に抵抗力が発生し減速するため、操作者１９は操作ハンドル３を介して接触を感知する。
また第２工具７が被作業物１７に接触すると、力制御部４６により、一定の接触力で被作業物１７を保持する。
【００１４】
マスタマニピュレータ１を大きく動作させたい場合には、操作者１９が左手で操作ハンドル３の切り換えスイッチ９bを使ってマスタ側接点２３ｂへ切り換えてから右手でジョイスティック６を倒すと、操作ハンドル３に作用している操作力に応じた操作指令ｒｅｆ１が力制御部４１に入力され、操作ハンドル３にて単独で操作するときよりも大きな動作をマスタマニピュレータ１に発生させることが可能となる。
以上の実施例において、請求項４記載のように固定部４から操作ハンドル３と並列するようジョイスティック６を回転自在に取り付けると、操作者１９は作業姿勢に応じてジョィスティック６を右手の操作し易い位置に配置することが可能となり、操作性が向上する。
【産業上の利用可能性】
【００１５】
本発明は、マスタマニピュレータとスレーブマニピュレータの両方の先端に工具を装着したマスタスレーブマニピュレータに広く適用でき、その操作性と安全性を向上させる。
【図面の簡単な説明】
【００１６】
【図１】本発明の全体構成を示す図
【図２】本発明の制御装置の制御ブロック図
【図３】本発明の力制御部の制御ブロック図
【図４】本発明の反力生成部の制御ブロック図
【図５】本発明の補正関数部を説明する図
【図６】従来のマスタスレーブマニピュレータの制御装置を示す図
【符号の説明】
【００１７】
１  マスタマニピュレータ
２  スレーブマニピュレータ
３  操作ハンドル
４  固定部
５  第１工具
６  ジョイスティック
７  第２工具
８  制御装置
９ａ  入力有効スイッチ
９ｂ  切り換えスイッチ
１０〜１４  信号線
１５、１６  サーボ増幅器
１７  被作業物
１８  作業台
１９  操作者
２０  ゼロ点補正部
２１  補正関数部
２２  積分器
２３  切り換え器
２３ａ  スレーブ側接点
２３ｂ  マスタ側接点
２４  加算器
４０  座標変換器
４１  力制御部
４２  座標変換器
４３  反力生成部
４４  ヤコビ変換器
４５  座標変換器
４６  力制御部
４７  ヤコビ変換器
Ｓ１  力センサ
Ｓ２  力センサ
Ｓ３  力センサ
１０１  制御装置
１０２  サーボ増幅器
１０３  サーボ増幅器
１０４  マスタマニピュレータ
１０５  力トルク検出器
１０６  入力レバ−
１０７  スレーブマニピュレータ
１０８  力トルク検出器
１０９  エンドエフェクタ
１１０  スイッチ
１１１〜１１４  信号線
４０１  順変換器
４０２  微分器
４０３  仮想粘性
４０４  仮想剛性
４０５  力フィードバックゲイン
４０６〜４０９  加算器
４１０  比例ゲイン
４１１  微分器
４１２  比例ゲイン
４１３  加算器
【図１】
2006102866.tif 000003
【図２】
2006102866.tif 000004
【図３】
2006102866.tif 000005
【図４】
2006102866.tif 000006
【図５】
2006102866.tif 000007
【図６】
2006102866.tif 000008