TOP > 国内特許検索 > 位置・力制御装置 > 明細書

明細書 :位置・力制御装置

発行国 日本国特許庁(JP)
公報種別 特許公報(B2)
特許番号 特許第4696307号 (P4696307)
登録日 平成23年3月11日(2011.3.11)
発行日 平成23年6月8日(2011.6.8)
発明の名称または考案の名称 位置・力制御装置
国際特許分類 B25J   3/00        (2006.01)
G05D   3/00        (2006.01)
G05D   3/12        (2006.01)
B25J  13/08        (2006.01)
FI B25J 3/00 A
G05D 3/00 Q
G05D 3/12 N
B25J 13/08 Z
請求項の数または発明の数 3
全頁数 17
出願番号 特願2006-519226 (P2006-519226)
出願日 平成16年7月21日(2004.7.21)
国際出願番号 PCT/JP2004/010335
国際公開番号 WO2005/109139
国際公開日 平成17年11月17日(2005.11.17)
優先権出願番号 2003278919
優先日 平成15年7月24日(2003.7.24)
優先権主張国 日本国(JP)
審査請求日 平成19年6月22日(2007.6.22)
特許権者または実用新案権者 【識別番号】899000079
【氏名又は名称】学校法人慶應義塾
発明者または考案者 【氏名】大西 公平
【氏名】北島 政樹
【氏名】森川 康英
【氏名】小澤 壮治
【氏名】古川 俊治
【氏名】村上 俊之
【氏名】中澤 和夫
【氏名】飯田 亘
【氏名】矢野 智子
個別代理人の代理人 【識別番号】100100930、【弁理士】、【氏名又は名称】長澤 俊一郎
審査官 【審査官】松浦 陽
参考文献・文献 特開2002-307336(JP,A)
特開2001-198870(JP,A)
大石 潔 Kiyoshi OHISHI,カセンサを用いない位置と力のハイブリッド制御 Hybrid Position and Force Control without Force Sensor,日本ロボット学会誌 第11巻 第3号 Journal of the Robotics Society of Japan,日本,社団法人日本ロボット学会 Robotics Society of Japan,第11巻
調査した分野 B25J 1/00 - 21/02
G05D 3/00
G05D 3/12
IEEE Xplore
CiNii
JSTPlus(JDreamII)
特許請求の範囲 【請求項1】
対象物の位置を検出する位置検出手段と、
上記対象物を駆動する駆動手段と、
上記位置検出手段が出力する位置信号と上記駆動手段への加速度指令信号である駆動信号に基づき上記対象物が受ける反力を求める反力検出手段と、
上記対象物が受ける反力と目標となる力信号とから第1の加速度信号を求めるとともに、上記位置信号と目標位置とから第2の加速度信号を求め、上記第1、第2の加速度信号とを加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する制御手段を備えた
ことを特徴とする位置・力制御装置。
【請求項2】
対象物の位置及び該対象物に作用する力を位置指令信号、力指令信号に応じて制御する位置・力制御装置であって、
上記対象物を駆動する駆動手段と、
上記対象物の位置を検出する位置検出手段と、
上記位置検出手段が出力する位置信号と、上記駆動手段への加速度指令信号である駆動信号から、該対象物が受ける反力を求める反力検出手段と、
位置指令信号と上記位置検出手段が出力する位置信号との偏差を求め、該偏差信号を第1の加速度信号に変換する第1の演算手段と、
上記反力検出手段により検出された上記反力と、力指令信号とのを求め、該を第2の加速度信号に変換する第2の演算手段と、
上記第1、第2の加速度信号を加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する制御手段を備えた
ことを特徴とする位置・力制御装置。
【請求項3】
マスタ側の操作部とスレーブ側の対象物の位置偏差に応じて、スレーブ側の対象物とマスタ側の操作部の位置を制御し、マスタ側の操作力に応じた駆動力で対象物を駆動するとともに、対象物が受ける力をマスタ側に伝える位置・力制御装置であって、
マスタ側の操作部を駆動する第1の駆動手段と、
マスタ側の操作部の位置を検出する第1の位置検出手段と、
上記第1の位置検出手段が出力する位置信号と、上記第1の駆動手段への加速度指令信号である駆動信号から、上記操作部に作用する反力を求める第1の反力検出手段と、
スレーブ側の対象物を駆動する第2の駆動手段と、
スレーブ側の対象物の位置を検出する第2の位置検出手段と、
上記第2の位置検出手段が出力する位置信号と、上記第2の駆動手段への加速度指令信号である駆動信号から、上記対象物が受ける反力を求める第2の反力検出手段と、
上記第1の位置検出手段が出力する位置信号と、第2の位置検出手段が出力する位置信号との偏差を求め、該偏差をマスタ側およびスレーブ側を制御するための第1、第2の加速度信号に変換する第1の演算手段と、
上記第1の反力検出手段と、上記第2の反力検出手段の出力の和を求め、該和をマスタ側およびスレーブ側を制御するための第3、第4の加速度信号に変換する第2の演算手段と、
上記第1,第3の加速度信号を加算する第1の加算手段と、
上記第2,第4の加速度信号を加算する第2の加算手段と、
上記第1の加算手段の出力に基づき、上記マスタ側の操作部への駆動信号となる加速度指令信号を出力する第1の制御手段と、
上記第2の加算手段の出力に基づき、上記スレーブ側の対象物への駆動信号となる加速度指令信号を出力する第2の制御手段とを備えた
ことを特徴とする位置・力制御装置。
発明の詳細な説明 【技術分野】
【0001】
本発明は、力センサを用いることなく、対象物の位置と対象物に作用する力を応答性よく制御することができる位置・力制御装置に関する。
【背景技術】
【0002】
人と機械が接触するヒューマンインタラクションの分野において、人間の操作に応じて遠隔に配置された対象物の位置や対象物に作用する力を応答性よく制御したいという要望がある。
例えば、人間が立ち入ることができない作業現場で用いられる遠隔操作装置や、遠隔地の患者に対する遠隔医療等の分野では、操作者によるマスタ側の操作に応じて遠隔地に配置されたスレーブ側の装置を作動させるようにしたマスタ・スレーブ制御装置が用いられている。この種のマスタ・スレーブ制御装置では、対象物の位置と対象物に作用する力を応答性よく制御し、繊細な作業を実現したいという要望がある。
上記マスタ・スレーブ装置としては、操作者から、マスタ側装置を介して、スレーブ側装置に指令を送ることができるが、スレーブ側が作業対象物等から受ける作業反力を操作者にフィードバックすることができないユニラテラル制御方式や、操作者からマスタ側装置を介してスレーブ側装置に指令を送ることができ、かつ、上記スレーブ側が作業対象物等から受ける作業反力を操作者にフィードバックすることができるバイラテラル制御方式が知られている。
【0003】
バイラテラル制御方式はスレーブ側が作業対象物等から受ける作業反力を操作者にフィードバックしているので、この制御方式によれば、遠隔地における触覚を、現実と同様にリアルタイムで得ることができる。
バイラテラル制御方式の制御装置は、マスタ・スレーブ型ロボットで構成されており、遠隔地のスレーブにかかる力を、マスタを通じて人間に返す。また人間はマスタを動かすことにより、スレーブにマスタと同一の動きをさせることができる。こうすることで遠隔地の物体を、あたかも手元で触っている感覚を得ることができる。
上記マスタ・スレーブ制御装置としては、例えば特許文献1に記載のものが提案されている。
上記特許文献1に記載のものは、操作者によるマスタの操作に応じてスレーブが動作するマスタ・スレーブ装置において、スレーブが物体と接触している状態では、操作者の操作によりマスタに加わる力f1を求め、スレーブにかかる力f3が、マスタに加わる力fに応じた力f2に追従するようにスレーブを制御するように構成したものであり、引用文献1に記載のものによれば、バイラテラル制御方式による長所を維持したまま、制御系の複雑化や高コスト化等を避けることができる。
【0004】
上記バイラテラル制御方式としては、上記特許文献1にも記載されるように図14に示すものや、図15に示すものが知られている。
図14に示すものは、マスタ201の位置とスレーブ202の位置との偏差を求め、この偏差に応じて位置制御部203によりスレーブ側の位置を制御するとともに、マスタ201とスレーブ202に作用する力を力検出器で検出し、その偏差をマスタ側にフィードバックして、力制御部204によりマスタ側の力を制御するようにしたものである。
また、図15に示すものは、マスタ201の位置とスレーブ202の位置との偏差を求め、この偏差に応じて位置制御部203によりスレーブ側の位置を制御するとともに、位置制御部203’によりマスタ側の位置を制御し、さらに、マスタ201とスレーブ202に作用する力を力検出器で検出し、その偏差をマスタ側にフィードバックして、力制御部204によりマスタ側の力を制御するとともに、力制御部204’によりスレーブ側を制御するようにしたものである。
さらに、上記のような力検出器と力制御部を備え、固い制御対象物でも安定して高精度な力制御を実現できるすると同時に、アームのどの部分に接触しても柔軟な特性を持つことができるロボットの制御装置が提案されている(特許文献2参照)

【特許文献1】特開2002-307336号公報
【特許文献2】特開2001-198870号公報
【発明の開示】
【発明が解決しようとする課題】
【0005】
上記従来の制御装置において、力または加速度情報の検出をする場合、歪みゲージなどの力センサあるいは加速度センサを用いて直接的に測定するのが一般的であった。
しかし、上記力センサや加速度センサを用いた直接的な力または加速度情報の検出では、以下のような問題点があった。
(i)信号ノイズの問題
力センサや加速度センサによる力の検出では微小なアナログ信号を増幅して検出しているが、信号に含まれるノイズも増幅してしまう。高周波ノイズに関しては低周波域通過フィルタを通すことによりその影響を除去することができるが、その場合には必要としている信号の周波数帯域までも狭めてしまう。
(ii)センサの固有周波数の問題
力センサを用いた力検出では、環境を直接力センサに接触させて測定する。力センサには物理的な固有周波数が存在し、それ以上高い周波数帯域での力測定は不可能である。
(iii)システムモデルの変化
力センサはシステムに直接設置するため、センサ慣性がそのままシステムモデルに加わってくる。また力センサでは加わった力に比例した歪みを利用して力を測定するが、このため力センサには物理的に剛性の低い材料が用いられている。しかし環境との接触を伴うシステムでは、この低剛性の力センサによりシステムモデルが変化してしまう。モデルの変化はシステムの制御においては致命的な問題である。
(iv)センサ価格の問題
一般的に力センサや加速度センサは高価であり、このため、装置全体も高価なものとなってしまう。
【0006】
また、位置および力等のように複数の変量を制御する場合、例えば前記図14、図15に示したように、位置検出器と力検出器を設け、位置制御系と力制御系により位置と力を制御するのが一般的である。
しかし、上記構成の制御系では、位置制御系と力制御系のそれぞれにより位置と力を制御しているので、位置制御系と力制御系が互いに干渉して、ゲインを上げることができず、誤差が残ってしまうことがある。
さらに、上記制御系において、歪みゲージなどの力センサを用いて力検出を行うと、前記したように力センサのノイズや固有周波数等の影響により、測定可能周波数帯域が狭い範囲に限定されてしまうとともに、力センサの慣性や低い剛性が、システムモデルに影響を与えてしまい、高い周波数帯域での応答性能を向上させることができない。
このため、従来のバイラテラル制御系においては、硬い環境に触ると不安定になったり、瞬間的な衝撃力が得られない、また、操作感が重いといった問題があり、繊細な感覚情報を伝えることができず、例えば遠隔医療等の分野などにおいて、繊細な作業を実現することは困難であった。
また、従来の制御系では、位置センサに加え、比較的高価な力センサを使用しており、装置価格が高くなるといった問題もあった。
本発明は上記従来技術の問題点を解決するためになされたものであって、本発明の目的は、高い周波数帯域での応答性能を向上させることができ、繊細な作業を実現することが可能な位置・力制御装置を提供することである。
【課題を解決するための手段】
【0007】
従来の歪みゲージを用いた力センサや、加速度センサのアナログ情報には信号ノイズが不可避であるが、位置情報はデジタル情報として検出可能なのでノイズの影響を受けにくい。
そこで、本発明においては、力センサに代えて反力検出手段を設け、位置情報より対象物に作用する反力を推定する。
これにより、ノイズを含まない反力を推定することができる。また、位置検出器はモータ軸とは非接触に位置情報を得ることができるので、力センサのような固有周波数による測定可能周波数帯域の制限がなく、非接触なのでセンサ設置による慣性および剛性などのシステムモデルの変化は起こらない。また、位置センサのみを用いているので、システムを安価に構築することができる。
また、従来のように位置制御系と力制御系をそれぞれ設けて位置および力を制御すると、位置制御系と力制御系が互いに干渉して、ゲインを上げることができない。
そこで、本発明においては、位置および力を加速度信号に変換し、両者を合成して、対象物を駆動する加速度指令信号である駆動信号を得るようにした。
これにより、位置制御と力制御が干渉することなく、それぞれのゲインを独立して設定することができる。
以上に基づき、本発明においては、次のようにして前記課題を解決する。
(1)対象物の位置を検出する位置検出手段を設け、位置情報と対象物を駆動する駆動手段への加速度指令信号である駆動信号に基づき、応力検出手段により対象物が受ける反力を推定する。
そして、上記対象物が受ける反力と目標となる力信号とから第1の加速度信号を求め、また、上記位置信号と目標位置とから第2の加速度信号を求め、上記第1、第2の加速度信号とを加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する。
(2)上記(1)の制御を、位置指令信号、力指令信号に応じて対象物の位置、対象物が受ける力を制御する制御装置に適用する。
すなわち、対象物の位置を検出する位置検出手段を設け、上記のように、反力検出手段により対象物が受ける反力を推定する。
そして、位置指令信号と上記位置検出手段が出力する位置信号との偏差を求め、該偏差信号を第1の加速度信号に変換し、また、上記反力検出手段により検出された上記反力と、力指令信号とのを求め、該を第2の加速度信号に変換し、第1、第2の加速度信号を加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する。
(3)上記(1)の制御を、マスタ側の操作部とスレーブ側の対象物の位置偏差に応じて、スレーブ側の対象物とマスタ側の操作部の位置を制御し、マスタ側の操作力に応じた駆動力で対象物を駆動するとともに、対象物が受ける力をマスタ側に伝えるバイラテラル制御方式に適用する。
すなわち、マスタ側の操作部とスレーブ側の対象物の位置を検出する第1、第2の位置検出器をそれぞれ設け、また、上記のように第1、第2の反力検出手段によりマスタ側の操作部が受ける反力および、対象物が受ける反力を推定する。
そして、上記第1の位置検出手段が出力する位置信号と、第2の位置検出手段が出力する位置信号との偏差を求め、該偏差をマスタ側およびスレーブ側を制御するための第1、第2の加速度信号に変換する。また、上記第1の反力検出手段と、第2の反力検出手段の出力の和を求め、該和を、マスタ側およびスレーブ側を制御するための第3、第4の加速度信号に変換する。
上記第1,第3の加速度制御信号を加算するとともに、第2,第4の加速度制御信号を加算し、上記第1,第3の加速度制御信号の加算結果に基づき、上記マスタ側の操作部への駆動信号となる加速度指令信号を出力するとともに、上記第第2,第4の加速度制御信号の加算結果に基づき、上記スレーブ側の対象物への駆動信号となる加速度指令信号を出力する。
【発明の効果】
【0008】
本発明においては、以下の効果を得ることができる。
(1)反力検出手段を設け、位置検出手段の出力に基づき対象物に加わる力を推定しているので、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、対象物の加わる力を求めることができる。
したがって、位置検出手段として高精度のセンサを用い、位置検出結果を短い周期でサンプリングすれば、対象物の位置、対象物に加わる力を応答性よく制御することができる。
また、センサとしては位置検出手段を設けるだけでよく、比較的高価な力センサを使用しないので、システムを安価に構築することができる。
(2)対象物が受ける反力と目標となる力信号とから第1の加速度信号を求め、また、上記位置検出手段により検出された位置信号と目標位置とから第2の加速度信号を求め、上記第1、第2の加速度信号を合成し、対象物を制御する駆動手段への駆動信号を出力するようにしたので、従来の制御装置のように位置、力制御が互いに干渉するといった問題が生ずることがなく、位置制御のゲインと力制御のゲインを独立に設定することが可能となる。このため、位置制御のゲインを大きくすることにより、位置誤差を生じさせることなく、リアルタイムで制御することができる。
(3)本発明をバイラテラル制御に適用し、マスタ側とスレーブ側に設けた位置検出手段の出力から位置偏差を求めて、加速度信号に変換し、また、マスタ側とスレーブ側に設けた反力検出手段により推定されたマスタ側とスレーブ側の反力を加算して加速度信号に変換し、上記位置偏差から求めた加速度信号と、反力の加算結果から求めた加速度信号を合成してマスタ側とスレーブ側を制御するようにしたので、上記のように位置制御のゲインと力制御のゲインを独立に設定することが可能となり、例えば、マスタ側とスレーブ側の位置誤差が零になるように、かつ、力については両者の和に倣うようにマスタ側とスレーブ側を制御することができる。
このため、マスタ側とスレーブ側の位置誤差を生じさせることなく、マスタ側の操作力をスレーブ側に応答性よく伝えることができるとともに、スレーブ側に加わる力をマスタ側に応答性よく伝えることができ、繊細な作業を実現することが可能となる。
【図面の簡単な説明】
【0009】
[図1]本発明の第1の実施例の位置・力制御装置の概略構成を示す図である。
[図2]外科医が手術等に使用される鉗子の構成例を示す図である。
[図3]本発明の第1の実施例におけるマスタ側の装置構成を示す図である。
[図4]本発明の第1の実施例におけるスレーブ側の装置構成を示す図である。
[図5]本発明の第1の実施例の制御系のブロック図である。
[図6]本発明で使用される反力オブザーバのブロック図である。
[図7]従来の力センサと反力推定オブザーバの周波数特性を示す図である。
[図8]指令値を正弦波状に変化させた場合における従来の力センサと反力オブザーバの出力を示す図である。
[図9]反力オブザーバの他の構成例を示す図である。
[図10]本実施例の制御装置の応答特性(位置)を示す図である。
[図11]本実施例の制御装置の応答特性(力)を示す図である。
[図12]本発明の第2の実施例の位置・力制御装置の概略構成を示す図である。
[図13]本発明の第2の実施例の制御系のブロック図である。
[図14]従来のバイラテラル制御方式の構成例(1)を示す図である。
[図15]従来のバイラテラル制御方式の構成例(2)を示す図である。
【符号の説明】
【0010】
1 マスタ
1a リニアモータ
1b 位置検出器
1c 操作部
2,4 反力オブザーバ
3 スレーブ
3a リニアモータ
3b 位置検出器
3f 把持部
5 位置制御部
6 操作力制御部
7 加速度合成部
11 位置/力指令発生部
13 スレーブ
14 反力オブザーバ
15 位置制御部
16 操作力制御部
17 加速度合成部
【発明を実施するための最良の形態】
【0011】
図1は本発明の第1の実施例の位置・力制御装置の概略構成を示す図であり、同図は本発明をバイラテラル制御に適用した場合の概略構成を示している。
同図において、1は作業者により操作されるマスタであり、例えば作業者により操作される操作部、該操作部を駆動するマスタ側モータ、該モータの位置を検出するマスタ側位置検出器から構成される。2は反力オブザーバであり、後述するように上記マスタ側位置検出器の出力と、上記マスタ側への駆動信号から上記操作部に加わる力を求める。
3はマスタ1の操作に応じて動作するスレーブであり、対象物を駆動するスレーブ側モータと、該モータの位置を検出するスレーブ側位置検出器から構成される。上記対象物とは、例えば、各種作業を行うロボットのハンドや、後述する鉗子であればその把持部等、遠隔地において実際に作業を行う操作部である(ここではこれらを含めて対象物と呼ぶこととする)。
4は反力オブザーバであり、上記マスタ側反力オブザーバと同様な構成を備え、後述するように上記スレーブ側位置検出器の出力と、上記スレーブ側への駆動信号から上記対象物に加わる力を求める。
上記マスタ側位置検出器により検出された操作部の位置、スレーブ側位置検出器により検出された対象物の位置信号x,xは、それぞれ位置制御部5に送られ、位置制御部5は上記位置信号を加速度参照値apm,apsに変換する。
また、マスタ側の反力オブザーバ2により検出されたマスタ側の力信号F、スレーブ側の反力オブザーバ2により検出されたスレーブ側の力信号Fは操作力制御部6に送られ、操作力制御部6は、上記力信号F,Fを加速度参照値afm,afsに変換する。
加速度合成部7は、上記加速度参照値apm,aps、加速度参照値afm,afsを合成し、マスタ側モータへの駆動信号となる加速度指令信号a、スレーブ側モータへの駆動信号となる加速度指令信号aを出力する。
本実施例の制御装置の反力オブザーバ2,4、位置制御部5、操作力制御部6、加速度合成部7は、コンピュータにより実現することができ、本実施例の制御装置をコンピュータで構成する場合には、上記位置検出器の出力を所定のサンプリング周期でコンピュータに取り込み、ソフトウェアにより上記機能を実現するための演算処理を行って、マスタ側、スレーブ側のモータを制御する。
【0012】
次に、医療用の鉗子を遠隔制御する場合を例として、バイラテラル制御方式に適用した本発明の実施例の位置・力制御装置について説明する。
鉗子は、外科医が手術等に使用する医療器具であり、図2(a)に示すように、ハンドル部101(以下操作部101という)と鉗子部102(以下把持部102という)から構成される。
鉗子の軸部103内を操作部材(図示せず)が貫通しており、操作部101のハンドル1eを手で開閉することにより、上記操作部材が同図の左右方向に移動する。
把持部102は図2(b)に示すように、把持部102がリンク機構104に連結され、リンク機構104の他端側に上記操作部材105が連結されており、上記操作部101のハンドル1eを操作して、上記操作部材を同図の矢印方向に動かすことにより、把持部101を開閉させることができる。
本実施例では、上記鉗子を上記操作部101と把持部102の2つに分けて、操作部101をマスタ側、把持部102をスレーブ側とし、それぞれにリニアモータを連結し、該リニアモータを前記図1に示した制御系を用いて制御することにより、操作部101の操作に応じて、遠隔地に設置された把持部102を制御するようにした。
なお、以下では、操作部101の操作に応じて把持部を開閉させる1軸制御について説明するが、本実施例で説明する制御系を複数設けることにより操作部101の操作に応じて把持部全体を回転させたり、把持部全体を揺動させる等の多軸制御を行うこともできる。
【0013】
図3、図4は本実施例におけるマスタ側とスレーブ側の装置構成を示す図である。
図3(a)はマスタ側装置の上面図、(b)は側面図、(c)は同図(a)をA方向から見た図である。
同図において、1aはリニアモータであり、リニアモータ1aの可動軸1dに操作部1cを構成するハンドル1eの一方が連結され、ハンドル1eの他方はリニアモータ1aのケースに固定されている。従って、手で操作部1cのハンドル1eを操作することにより、リニアモータ1aの可動軸1dが同図(a)(b)の左右方向に動く。1bはマスタ側位置検出器でありリニアモータ1aの可動軸1dの位置を検出する。
図4(a)はスレーブ側装置の上面図、(b)は側面図、(c)は同図(a)をA方向から見た図である。
同図において、3aはリニアモータであり、リニアモータ3aの可動軸3dには、取付け金具3cを介して鉗子の軸部3d内を貫通する前記操作部材3eが連結され、操作部材3eの先端側には前記したようにリンク機構を介して把持部3fが取付けられている。
従って、リニアモータ3aの可動軸3dが同図(a)(b)の左右方向に動くことにより、操作部材3eが左右方向に動き把持部3fが開閉する。3bはスレーブ側位置検出器であり、リニアモータ3aの可動軸3dの位置を検出する。
なお、上記モータとしては、摩擦力の少ないモータを用いるのが望ましく、上記のようなリニアモータを使用する外、回転運動の場合にはダイレクトドライブモータ等を使用することができる。
また、上記位置検出器としては、高精度の検出器を用いるのが望ましく、また、応答性を向上させるには、位置検出器の検出結果を取り込むサンプリング周期を十分短くすることが必要である。
【0014】
図5は本実施例の制御系のブロック図である。
同図において、前記図1の示したものと同一のものには同一の符号が付されており、1はマスタであり、マスタ1は図3に示したように、リニアモータ1aと位置検出器1bと操作部1cから構成される。マスタ側の反力オブザーバ2は、上記マスタ1のリニアモータ1aに供給される電流信号Iamrefと、位置検出器1bにより検出されるリニアモータ1aの可動軸1dの位置(即ち操作部1cの位置)に応じた位置検出信号xからマスタ側に加わる力の推定値Fを求める。
3はスレーブであり、スレーブ3は図4に示したように、リニアモータ3aと位置検出器3bと前記機構により動作する把持部3fから構成される。
スレーブ側の反力オブザーバ4は、上記スレーブ2のリニアモータ3aに供給される電流信号Iasrefと、位置検出器3bにより検出されるリニアモータ3aの可動軸3dの位置(即ち把持部1fの位置)に応じた位置検出信号xからスレーブ側に加わる力の推定値Fを求める。
【0015】
5は位置制御部であり、上記マスタ側の位置検出器1bにより検出されたマスタ側の操作部1cの位置信号xと、スレーブ側の位置検出器3bにより検出されたスレーブ側の把持部1fの位置信号xとの差を求める減算部5aと、減算部5aの出力に(Ks+K)の演算を施す制御部5bと、制御部5bの出力に、〔M/(M+M)〕,〔-M/(M+M)〕を乗じて加速度参照値apm,apsを生成する変換部5c,5dから構成される。なお、ここで、Kは位置ゲイン、Kは速度ゲイン、Mはマスタ側の慣性、Mはスレーブ側の慣性である。
6は操作力制御部であり、マスタ側の反力オブザーバ2が出力するマスタ側の操作部1cに加わる力の推定値Fとスレーブ側の反力オブザーバ4が出力するスレーブ側の把持部3fに加わる力の推定値Fとの和を出力する加算部6aと、加算部6aの出力に力ゲインKfを乗算し、加速度参照値afm,afsを生成する変換部6bとから構成される。上記力ゲインKは仮想慣性の逆数であり任意の値に設定することができ、Kを適切に設定することにより、等価質量を見かけ上小さくすることができる。
7は加速度合成部であり、上記加速度参照値apm,afmを加算してマスタ側の加速度参照値(xrefを出力する加算部7aと、加算部7aの出力に〔M/Ktnm〕を乗じて、マスタ側のリニアモータ1aを駆動する電流参照値Iamrefを生成する変換部7bと、上記加速度参照値aps,afsを加算してスレーブ側の加速度参照値(xrefを出力する加算部7cと、加算部7cの出力に〔M/Ktns〕を乗じて、スレーブ側のリニアモータ3aを駆動する電流参照値Iasrefを生成する変換部7dとから構成される。ここで、M,Mは前記したマスタ側の慣性、スレーブ側の慣性であり、Ktnm,Ktnsはそれぞれマスタトルク定数、スレーブトルク定数である。
なお、図面上では、一次微分、2次微分を符号の上にドットを1または2付けて示すが、明細書中では一次微分を「’」を付して示し、2次微分を上記(xrefのように「”」を付けて示す。
【0016】
図6は、反力オブザーバ2,4のブロック図である。
同図において、1,3はマスタ側、スレーブ側のブロック図であり、Irefはリニアモータ1a,3aに供給される電流参照値、Ktは推力定数であり、Iref×Kはリニアモータの駆動力に相当する。Fはリニアモータの負荷、Mはマスタ側、スレーブ側の慣性であり、リニアモータ1a,3aに電流Irefが供給され、負荷がFのとき、マスタ側、スレーブ側は、同図に示す速度x’で動き、その位置xは速度x’を積分した値となる。
反力オブザーバ2,4には上記電流参照値Iref、位置xが入力され、反力オブザーバ2,4は以下の(1)式によりマスタ側、スレーブ側の推定反力F,F(=F)を求める。
F=〔g/(s+g)〕×〔Ireftn+xMsg-Finit〕-Msg…(1)ここで、Finit=Fdis+Dx’+(M-M)x’である。また、g/(s+g)は一次の周波数選択フィルタである。
上記式および図6中の各記号は以下の値を表している。
M:慣性、M:慣性公称値、Ktn:推力定数公称値、x:位置、x’:速度、Fdis:クーロン摩擦、D:粘性摩擦係数。
なお、Fdis,D,M,M等は予備実験により求めた既知の値であるとする。
上記反力オブザーバを用いて反力を推定することにより、力センサを用いる場合に比べ、前記したように、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、かつ安価にマスタ側、スレーブ側の反力を推定することができる。またクーロン摩擦、粘性摩擦係数による影響を最小限にすることもできる。
【0017】
間接的センシングの有効性を確認するため、従来の歪みゲージを用いた力センサを用いて直接力を測定した場合と、反力推定オブザーバを用いて間接的に力を測定した場合を比較した。
図7に実験結果を示す。同図の横軸はω(rad/sec)、縦軸はgain(dB)であり、同図(a)は従来の力センサの周波数特性、(b)は反力オブザーバの周波数特性を示す。
同図から明らかなように、従来の力センサでは固有周波数の存在により、測定可能周波数帯域が約500rad/sec以下に限定されてしまうが、反力推定オブザーバでは設定したゲイン1500rad/secまでの広帯域で測定できる。
図8は指令値(印加する力)を正弦波状に変化させて、従来の歪みゲージを用いた力センサと上記反力オブザーバの出力を比較した図である。同図の横軸は時間(秒)、縦軸は力(ニュートン)であり、同図の点線は指令値(与えた力)、破線は上記反力オブザーバによる推定結果、実線は従来の力センサによる検出結果を示す(指令値と反力オブザーバによる推定結果はほぼ重なっている)。
同図の示すように、反力オブザーバの出力は、指令値とほぼ一致しているが、従来の力センサの出力は、上記指令値よりやや遅れており、また、固有周波数や慣性の影響により検出結果が一部振動的に変化している。
【0018】
図7、図8から明らかなように、反力オブザーバを用いれば、従来の力センサを用いた場合に比べ、広帯域での測定が可能であり、また、殆ど遅れなくマスタ側、スレーブ側に加わる力を推定することができ、これによりマスタ側、スレーブ側を応答よく制御することができる。
上記反力オブザーバは、例えば、「大西,”外乱オブザーバによるロバスト・モーションコントロール”日本ロボット学会誌vol.11,no.4,pp.486~493,1993」等に開示されている。
図9は、反力オブザーバの他の構成例を示す図であり、同図は、周波数選択フィルタとして、図6に示した〔g/(s+g)〕に替え、〔k/(s+ks+k)〕の伝達関数で表される2次フィルタを用いた場合を示している。
上記〔k/(s+ks+k)〕の極は、s+ks+k=0から得られ、この極をα,βとすると、α+β=-k、αβ=kとなる。
なお、上記図9に示した反力オブザーバについては、例えば「k.Ohnishi,M.Shibata,T.Murakami,″Motion Control for Advanced Mechatoronics″,IEEE/ASME Transactions on Mechatronics」等を参照されたい。
【0019】
次に、図3、図4、図5に示す本実施例の制御装置の動作について説明する。
マスタ側の操作部1cを操作すると、リニアモータ1aの可動軸1dが移動し、位置検出器1aにより可動軸1dの位置が検出される。
この位置検出器1aの出力と、スレーブ側の位置検出器3aの出力が位置制御部5に送られ、位置制御部5はその偏差に基づき加速度参照値apm,apsを生成し出力する。
一方、マスタ側の反力オブザーバ2はマスタ側に加わる力の推定値Fを出力する。
操作力制御部6は、上記マスタ側に加わる力の推定値Fと、スレーブ側の反力オブザーバ4が出力するスレーブ側に加わる力Fの和を求め、この和から加速度参照値afm,afsを生成し出力する。
上記加速度参照値apsと加速度参照値afsは、加速度合成部7に送られ、加速度合成部7は上記加速度参照値apsと加速度参照値afsの和から、電流参照値Iasrefを生成する。また、上記加速度参照値apmと加速度参照値afmの和から、電流参照値Iamrefを生成する。
上記電流生成値Iasrefはスレーブ側のリニアモータ3aに与えられ、リニアモータ3aが駆動される。同様に、上記電流生成値Iamrefはマスタ側のリニアモータ1aに与えられ、リニアモータ1aが駆動される。
【0020】
マスタ側の操作部1cを操作することにより、スレーブ側のリニアモータ3aの可動軸とマスタ側のリニアモータ1aの可動軸に位置誤差が生ずると、上記加速度合成部7から、マスタ側のリニアモータ1aとスレーブ側のリニアモータ3aに上記位置誤差を修正するような駆動信号が与えられ、マスタ側のリニアモータ1a、スレーブ側のリニアモータ3aの可動軸が駆動される。
これにより、マスタ側のリニアモータ1aとスレーブ側のリニアモータ3aの可動軸の位置が一致するように制御される。すなわち、マスタ側の操作部1cのハンドル1eの位置に応じて、スレーブ側の把持部3fの開度が制御される。
また、これと同時に、把持部3fに加わる力Fが反力オブザーバ4で推定されるとともに、操作部1cに加わる力Fが反力オブザーバ2で推定され、FとFとの和に応じた加速度参照値afm、afsが加速度合成部7に与えられる。
加速度合成部7は、把持部3fに加わる力Fと操作部1cに加わる力Fの和に応じて、マスタ側のリニアモータ1aとスレーブ側のリニアモータ3aを駆動する。
これにより、マスタ側の操作部1cに加わる力とスレーブ側の把持部3fに加わる力との和に倣うように、マスタ側の操作部1cとスレーブ側の把持部3fが制御される。
すなわち、マスタ側の操作部1cを操作して、スレーブ側の把持部3fで物体を把持すると、把持部3fに加わる反力に応じた力が操作部1cに伝わり、これにより、作業者は把持部3fで把持した物体からの反力等を操作部1cで感じ取ることができる。
以上の説明では、マスタ側を操作した場合について説明したが、スレーブ側を操作した場合でも上記と同様にマスタ側が制御され、スレーブ側にマスタ側の力が伝えられる。
【0021】
本実施例のバイラテラル制御装置は、上記のように、マスタ側の操作部、スレーブ側の対象物の位置を検出する位置検出手段を設け、位置検出手段の出力に基づき、反力オブザーバにより、マスタ側の操作部、スレーブ側の対象物が受ける反力を推定しているので、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、かつ安価にスレーブ側の対象物に加わる反力を推定することができる。またクーロン摩擦、粘性摩擦係数による影響を最小限にすることもできる。
このため、位置検出手段として高精度のセンサを用い、位置検出結果を比較的短い周期でサンプリングし、位置制御部の制御ゲインを大きく設定すれば、マスタ側とスレーブ側の位置誤差を生じさせることなく、位置および操作力を応答性よく制御することができる。
また、位置制御部5と操作力制御部6を設けて、加速度合成部で位置制御部5と操作力制御部6で生成した加速度参照値を合成して、リニアモータを駆動しているので、位置制御のゲインと操作力制御のゲインを独立に設定することが可能となる。
このため、マスタ側とスレーブ側の位置誤差が無くなるように位置制御部5を設定することができ、また、マスタ側とスレーブ側の反力の和に倣うように両者を制御することができる。さらに、操作力制御部6の力ゲインKfの設定により、マスタ側の操作力を自由に設定することができる。
【0022】
本実施例のバイラテラル制御装置により前記鉗子を遠隔制御して、本発明の有効性を確認した。
実験では、制御装置をコンピュータで構成し、位置検出器として、分解能が1μm程度のリニアエンコーダを用い、100μs程度のサンプリング周期でコンピュータで構成された制御装置に取り込んで、鉗子をバイラテラル制御した。これにより、160Hz程度の応答性を得ることができた。
図10、図11に上記実験結果を示す。
なお、図10、図11の横軸は時間(sec)、図10の縦軸は位置、図11の縦軸は力(ニュートン)であり、位置、力をステップ状に変化させたときの応答特性を示す。
前記した従来のバイラテラル制御系では環境接触時において位置偏差が生じてしまい位置再現性が悪かったが、本発明においては、計算機のサンプリング時間を十分に短して周波数帯域を十分広く確保し、位置制御部5の制御ゲインを上げることにより、図10に示すように位置偏差を生じることなくリアルタイムに環境を再現できた。
【0023】
以上説明した実施例では、本発明をバイラテラル制御に適用する場合について説明したが、本発明は、上記バイラテラル制御に限らず、位置指令値、力指令値に応じてスレーブ側を制御する制御装置に適用することもできる。
図12に上記のように位置指令値、力指令値に応じてスレーブ側を制御する本発明の第2の実施例の位置・力制御装置の概略構成を示す。
同図において、11は位置指令値と力指令値を発生する位置/力指令発生部であり、位置/力指令発生部11が出力する位置指令値xrefは位置制御部15に与えられ、力指令値frefは、操作力制御部16に与えられる。
13は上記位置指令、力指令に応じて動作するスレーブであり、前記したように、対象物を駆動するスレーブ側モータと、該モータの位置を検出するスレーブ側位置検出器から構成される。上記対象物とは、例えば、各種作業を行うロボットのハンド等、遠隔地において実際に作業を行う操作部である。
14は反力オブザーバであり、前記したように、スレーブ側位置検出器の出力と、上記スレーブ側への駆動信号から上記対象物に加わる力を求める。
上記位置指令値xrefとスレーブ側に設けられた位置検出器により検出された対象物の位置信号xは、それぞれ位置制御部15に送られ、位置制御部15は上記位置信号を加速度参照値apsに変換する。
また、力指令値frefとスレーブ側の反力オブザーバ12により検出されたスレーブ側の力信号fは操作力制御部16に送られ、操作力制御部16は、上記力信号fref,fを加速度参照値afsに変換する。
加速度合成部17は、上記加速度参照値aps、加速度参照値afsを合成し、スレーブ側モータへの駆動信号となる加速度指令信号aを出力する。
【0024】
上記位置/力指令発生部11が発生する位置指令値、力指令値は、予めプログラミングされた値であり、例えば、熟練した作業者等による操作等を記憶することにより得ることができる。
例えば、前記実施例に示したバイラテラル制御装置を用いて熟練した作業者により所望の作業をしてもらい、マスタ側(もしくはスレーブ側)の位置x(またはx)、応力オブザーバの出力f(またはf)を、それぞれ位置指令値、力指令値として記憶しておく。そして、この記憶された位置指令値、力指令値を上記位置/力指令発生部11から出力し、スレーブ側を制御することにより、熟練した作業者による作業を再現することができる。
【0025】
図13は、本実施例の制御系のブロック図である。
同図において、11は上記位置/力指令発生部、13はスレーブであり、スレーブ13は前記したように、リニアモータ等のモータと、位置検出器を備える。
14は反力オブザーバであり、前記図6、図9に示した構成を備え、スレーブ13のモータに供給される電流信号Iasref と、位置検出器により検出される位置に応じた位置検出信号xs からスレーブ側に加わる力の推定値Fs を求める。
15は位置制御部であり、上記位置/力指令発生部11が出力する位置指令値xref と、とスレーブ側の位置検出器により検出されたスレーブ側の位置信号xs との差を求める減算部15aと、減算部15aの出力に(Kv s+Kp )の演算を施す制御部15bと、加速度参照値apsを生成する変換部15cから構成される。なお、ここで、Kp は位置ゲイン、Kv は速度ゲインである。
16は操作力制御部であり、力指令値fref と反力オブザーバ14が出力するスレーブ側の対象物に加わる力の推定値Fs とのを出力する加算部16aと、加算部16aの出力に力ゲインKf を乗算し、加速度参照値afsを生成する変換部16bとから構成される。上記力ゲインKf は前記したように仮想慣性の逆数であり任意の値に設定することができ、Kf を適切に設定することにより等価質量を見かけ上小さくすることができる。
7は加速度合成部であり、上記加速度参照値aps,afs を加算してスレーブ側の加速度参照値(xs ref" を出力する加算部17aと、加算部17aの出力に〔Ms /Ktns 〕を乗じて、スレーブ側のモータを駆動する電流参照値Iasref を生成する変換部17bとから構成される。ここで、Ms はスレーブ側の慣性であり、Ktns はスレーブトルク定数である。
【0026】
次に、図13に示す本実施例の制御装置の動作について説明する。
位置/力指令発生部11が出力する位置指令値xref とスレーブ側の位置検出器3aの出力が位置制御部15に送られ、位置制御部15はその偏差に基づき加速度参照値apsを生成し出力する。
一方、操作力制御部16は、上記位置/力指令発生部11が出力する力指令値fref と反力オブザーバ4が出力するスレーブ側に加わる力Fs を求め、このから加速度参照値afsを生成し出力する。
上記加速度参照値apsと加速度参照値afSは、加速度合成部17に送られ、加速度合成部17は上記加速度参照値apsと加速度参照値afsの和から、電流参照値Iasref を生成する。上記電流生成値Iasref はスレーブ側のモータに与えられ、モータが駆動される。
位置指令値xref が変化すると、スレーブ側のモータの位置との位置誤差が生じ、加速度合成部17から、スレーブ側のモータに上記位置誤差を修正するような駆動信号が与えられ、スレーブ側のモータが駆動される。
これにより、上記位置指令値にスレーブ側の位置が一致するように制御される。
また、これと同時に、スレーブ側に加わる力Fs が反力オブザーバ4で推定され、力指令値fref とFs との偏差に応じた加速度参照値afsが加速度合成部17に与えられる。
加速度合成部17は、力指令値fref とスレーブ側の力Fs の大きさが異なると、この大きさが一致するような駆動信号をスレーブ側のモータに与える。これにより、スレーブ側に加わる力が力指令値fref 一致するように制御される。
【0027】
本実施例においては、上記のように、スレーブ側の対象物の位置を検出する位置検出手段を設け、位置検出手段の出力に基づき、反力オブザーバにより、対象物が受ける反力を推定しているので、第1の実施例と同様、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、かつ安価にスレーブ側の対象物に加わる反力を推定することができる。またクーロン摩擦、粘性摩擦係数による影響を最小限にすることもできる。
このため、位置検出手段として高精度のセンサを用い、位置検出結果を比較的短い周期でサンプリングし、位置制御部の制御ゲインを大きく設定すれば、位置誤差を生じさせることなく、スレーブ側の位置、操作力を応答性よく制御するこができる。
また、位置制御部15と操作力制御部16を設けて、加速度合成部17で位置制御部15と操作力制御部16で生成した加速度参照値を合成して、スレーブ側の対象物を駆動しているので、第1の実施例と同様、位置制御のゲインと操作力制御のゲインを独立に設定することが可能となる。
【産業上の利用可能性】
【0028】
本発明は、医療分野、建築用作業ロボット、食品加工や食品のハンドリング、アッセッブル、楽器の操作等の各種分野に適用することができ、バイラテラル制御系に適用すれば、マスタ側の操作力をスレーブ側に応答性よく伝えることができるとともに、スレーブ側に加わる力をマスタ側に応答性よく伝えることができ、繊細な作業を実現することが可能となる。
例えば、本発明を医療分野で使用される鉗子等の遠隔制御に適用すれば、従来の遠隔手術では実現することができなかった繊細な感覚情報を伝えることができ、鉗子を直接手で操作する場合と同様な感覚で使用することが可能となる、また、繊細な感覚情報を伝えることが必要な触診等を遠隔から行うことも可能となる。
また、建築用作業ロボットに適用することにより、セメント塗り等の作業を熟練した作業者が直接手で塗ったのと同様に行うことができ、さらに、食品のハンドリング作業やアッセンブル、楽器の操作等においても、人手による作業や操作と同様に行うことが可能となる。
図面
【図1】
0
【図2】
1
【図3】
2
【図4】
3
【図5】
4
【図6】
5
【図7】
6
【図8】
7
【図9】
8
【図10】
9
【図11】
10
【図12】
11
【図13】
12
【図14】
13
【図15】
14