JPH05204460A - マスタ・スレーブマニピュレータシステムの制御装置 - Google Patents
マスタ・スレーブマニピュレータシステムの制御装置Info
- Publication number
- JPH05204460A JPH05204460A JP1211192A JP1211192A JPH05204460A JP H05204460 A JPH05204460 A JP H05204460A JP 1211192 A JP1211192 A JP 1211192A JP 1211192 A JP1211192 A JP 1211192A JP H05204460 A JPH05204460 A JP H05204460A
- Authority
- JP
- Japan
- Prior art keywords
- tip
- manipulator
- master
- acceleration
- slave
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Manipulator (AREA)
- Control Of Position Or Direction (AREA)
Abstract
(57)【要約】
【目的】 加速度センサを用いることなく、マニピュレ
ータのモデル化誤差や未知のダイナミクスによる影響を
極力抑制して、オペレータに高度な臨場感を与えること
のできるマスタ・スレーブマニピュレータシステム制御
装置を提供する。 【構成】 外乱オブザーバ３５（３６）はアーム先端位
置計算装置３１（３２）とアーム先端速度計算装置３３
（３４）で求められたマスタマニピュレータ１０（スレ
ーブマニピュレータ２０）の先端位置と先端速度の情報
を利用してマスタマニピュレータ１０（スレーブマニピ
ュレータ２０）先端に発生する外乱加速度を推定する。
先端加速度推定装置３７（３８）は外乱オブザーバ３５
（３６）の外乱推定値と３軸力センサ１１（２１）から
出力された力センサ情報を利用してマスタマニピュレー
タ１０（スレーブマニピュレータ２０）先端の加速度を
計算する。
ータのモデル化誤差や未知のダイナミクスによる影響を
極力抑制して、オペレータに高度な臨場感を与えること
のできるマスタ・スレーブマニピュレータシステム制御
装置を提供する。 【構成】 外乱オブザーバ３５（３６）はアーム先端位
置計算装置３１（３２）とアーム先端速度計算装置３３
（３４）で求められたマスタマニピュレータ１０（スレ
ーブマニピュレータ２０）の先端位置と先端速度の情報
を利用してマスタマニピュレータ１０（スレーブマニピ
ュレータ２０）先端に発生する外乱加速度を推定する。
先端加速度推定装置３７（３８）は外乱オブザーバ３５
（３６）の外乱推定値と３軸力センサ１１（２１）から
出力された力センサ情報を利用してマスタマニピュレー
タ１０（スレーブマニピュレータ２０）先端の加速度を
計算する。
Description
【０００１】
【産業上の利用分野】本発明は、先端に力センサを有
し、関節に関節角度センサまたは関節角度センサと関節
角速度センサを有するマスタマニピュレータとスレーブ
マニピュレータとからなるマスタ・スレーブマニピュレ
ータシステムの制御装置に関する。
し、関節に関節角度センサまたは関節角度センサと関節
角速度センサを有するマスタマニピュレータとスレーブ
マニピュレータとからなるマスタ・スレーブマニピュレ
ータシステムの制御装置に関する。
【０００２】
【従来の技術】マスタ・スレーブマニピュレータシステ
ムにおける望ましい応答特性は、オペレータがあたかも
対象環境中で実際に作業しているかのような高度な臨場
感の実現であると考えられる。マスタ・スレーブマニピ
ュレータの制御法として、従来から対称型、力逆送型、
力帰還型の制御アルゴリズムが提案されているが、これ
らの制御法はマスタマニピュレータやスレーブマニピュ
レータのダイナミクスがオペレータに影響して必ずしも
理想的な制御性能を示すものではなかった。これに対し
て日本ロボット学会誌vol.8,no.3,1990 の舘らの文献”
インピーダンス制御型マスタ・スレーブ・システム
（１）”や第２７回ＳＩＣＥ学術講演予稿集,1990 の吉
川らの文献”マスタ・スレーブマニピュレータの動的制
御の提案”に示されているようなオペレータに高度の臨
場感を与えることのできる制御法が提案されている。こ
れらの制御法を実システムへ適用する際には、マニピュ
レータの正確な数学モデルとともにマニピュレータ先端
加速度センサが高度な臨場感を実現するためには必要で
あった。
ムにおける望ましい応答特性は、オペレータがあたかも
対象環境中で実際に作業しているかのような高度な臨場
感の実現であると考えられる。マスタ・スレーブマニピ
ュレータの制御法として、従来から対称型、力逆送型、
力帰還型の制御アルゴリズムが提案されているが、これ
らの制御法はマスタマニピュレータやスレーブマニピュ
レータのダイナミクスがオペレータに影響して必ずしも
理想的な制御性能を示すものではなかった。これに対し
て日本ロボット学会誌vol.8,no.3,1990 の舘らの文献”
インピーダンス制御型マスタ・スレーブ・システム
（１）”や第２７回ＳＩＣＥ学術講演予稿集,1990 の吉
川らの文献”マスタ・スレーブマニピュレータの動的制
御の提案”に示されているようなオペレータに高度の臨
場感を与えることのできる制御法が提案されている。こ
れらの制御法を実システムへ適用する際には、マニピュ
レータの正確な数学モデルとともにマニピュレータ先端
加速度センサが高度な臨場感を実現するためには必要で
あった。
【０００３】
【発明が解決しようとする課題】一方、実際のマスタ及
びスレーブマニピュレータの数学モデルを求める際には
誤差を含むことは避けられないし、各関節軸回りの粘性
摩擦といったマニピュレータに含まれる未知のダイナミ
クスの影響により制御性能の劣化を招いたり、あるいは
システム全体の安定性にまで影響を及ぼす可能性も考え
られる。また、両マニピュレータ先端の力制御に必要な
力センサに加えて加速度センサも装備することになれば
装置全体が複雑化する。
びスレーブマニピュレータの数学モデルを求める際には
誤差を含むことは避けられないし、各関節軸回りの粘性
摩擦といったマニピュレータに含まれる未知のダイナミ
クスの影響により制御性能の劣化を招いたり、あるいは
システム全体の安定性にまで影響を及ぼす可能性も考え
られる。また、両マニピュレータ先端の力制御に必要な
力センサに加えて加速度センサも装備することになれば
装置全体が複雑化する。
【０００４】本発明の目的は、加速度センサを用いるこ
となく、マニピュレータのモデル化誤差や未知のダイナ
ミクスによる影響を極力抑制して、オペレータに高度な
臨場感を与えることのできるマスタ・スレーブマニピュ
レータ制御装置を提供することである。
となく、マニピュレータのモデル化誤差や未知のダイナ
ミクスによる影響を極力抑制して、オペレータに高度な
臨場感を与えることのできるマスタ・スレーブマニピュ
レータ制御装置を提供することである。
【０００５】
【課題を解決するための手段】本発明の第１のマスタ・
スレーブマニピュレータシステムの制御装置は、マスタ
マニピュレータとスレーブマニピュレータのそれぞれの
先端に力センサを有し、関節に関節角度センサと関節角
速度センサを有するマスタマニピュレータとスレーブマ
ニピュレータからなるマスタ・スレーブマニピュレータ
システムの制御装置であって、マスタマニピュレータと
スレーブマニピュレータのそれぞれの先端位置を前記関
節角度センサの出力から計算するアーム先端位置計算装
置と、マスタマニピュレータとスレーブマニピュレータ
のそれぞれの先端速度を前記関節角速度センサの出力か
ら計算するアーム先端速度計算装置と、前記アーム先端
位置計算装置と前記アーム先端速度計算装置で求められ
た、マスタマニピュレータの先端位置と先端速度の情報
を利用して両マニピュレータが含む未知のダイナミクス
に起因してマスタマニピュレータ先端に発生する外乱加
速度を推定する第１の外乱オブザーバと、前記アーム先
端位置計算装置と前記アーム先端速度計算装置で求めら
れた、スレーブマニピュレータの先端位置と先端速度の
情報を利用して両マニピュレータが含む未知のダイナミ
クスに起因してスレーブマニピュレータ先端に発生する
外乱加速度を推定する第２のオブザーバと、第１の外乱
オブザーバからの外乱加速度の推定値と前記力センサか
ら出力された力センサ情報を利用してマスタマニピュレ
ータ先端の加速度を計算する第１の先端加速度推定装置
と、第２の外乱オブザーバからの外乱加速度の推定値と
前記力センサから出力された力センサ情報を利用してス
レーブマニピュレータ先端の加速度を計算する第２の先
端加速度推定装置と、前記アーム先端位置計算装置、前
記アーム先端速度計算装置、第１，第２の先端加速度推
定装置で求められた両マニピュレータの先端位置、先端
速度、加速度推定値および前記力センサから出力された
力センサ情報を用いて両マニピュレータの位置と力を一
致させるマスタ・スレーブ制御装置とを有している。
スレーブマニピュレータシステムの制御装置は、マスタ
マニピュレータとスレーブマニピュレータのそれぞれの
先端に力センサを有し、関節に関節角度センサと関節角
速度センサを有するマスタマニピュレータとスレーブマ
ニピュレータからなるマスタ・スレーブマニピュレータ
システムの制御装置であって、マスタマニピュレータと
スレーブマニピュレータのそれぞれの先端位置を前記関
節角度センサの出力から計算するアーム先端位置計算装
置と、マスタマニピュレータとスレーブマニピュレータ
のそれぞれの先端速度を前記関節角速度センサの出力か
ら計算するアーム先端速度計算装置と、前記アーム先端
位置計算装置と前記アーム先端速度計算装置で求められ
た、マスタマニピュレータの先端位置と先端速度の情報
を利用して両マニピュレータが含む未知のダイナミクス
に起因してマスタマニピュレータ先端に発生する外乱加
速度を推定する第１の外乱オブザーバと、前記アーム先
端位置計算装置と前記アーム先端速度計算装置で求めら
れた、スレーブマニピュレータの先端位置と先端速度の
情報を利用して両マニピュレータが含む未知のダイナミ
クスに起因してスレーブマニピュレータ先端に発生する
外乱加速度を推定する第２のオブザーバと、第１の外乱
オブザーバからの外乱加速度の推定値と前記力センサか
ら出力された力センサ情報を利用してマスタマニピュレ
ータ先端の加速度を計算する第１の先端加速度推定装置
と、第２の外乱オブザーバからの外乱加速度の推定値と
前記力センサから出力された力センサ情報を利用してス
レーブマニピュレータ先端の加速度を計算する第２の先
端加速度推定装置と、前記アーム先端位置計算装置、前
記アーム先端速度計算装置、第１，第２の先端加速度推
定装置で求められた両マニピュレータの先端位置、先端
速度、加速度推定値および前記力センサから出力された
力センサ情報を用いて両マニピュレータの位置と力を一
致させるマスタ・スレーブ制御装置とを有している。
【０００６】本発明の第２のマスタ・スレーブマニピュ
レータシステムの制御装置は、マスタマニピュレータと
スレーブマニピュレータのそれぞれの先端に力センサを
有し、関節に関節角度センサを有するマスタマニピュレ
ータとスレーブマニピュレータからなるマスタ・スレー
ブマニピュレータシステムの制御装置であって、マスタ
マニピュレータとスレーブマニピュレータのそれぞれの
先端位置を前記関節角度センサの出力から計算するアー
ム先端位置計算装置と、マスタマニピュレータの先端速
度を前記アーム先端位置計算装置から出力されたマスタ
マニピュレータの先端位置情報を用いて推定する第１の
速度オブザーバと、スレーブマニピュレータの先端速度
を前記アーム先端位置計算装置から出力されたスレーブ
マニピュレータの先端位置情報を用いて推定する第２の
速度オブザーバと、前記アーム先端位置計算装置と第１
の速度オブザーバで求められた、マスタマニピュレータ
の先端位置と先端速度の推定情報を利用して両マニピュ
レータが含む未知のダイナミクスに起因してマスタマニ
ピュレータ先端に発生する外乱加速度を推定する第１の
外乱オブザーバと、前記アーム先端位置計算装置と第２
の速度オブザーバで求められた、スレーブマニピュレー
タの先端位置と先端速度の推定情報を利用して両マニピ
ュレータが含む未知のダイナミクスに起因してスレーブ
マニピュレータ先端に発生する外乱加速度を推定する第
２のオブザーバと、第１の外乱オブザーバからの外乱加
速度の推定値と前記力センサから出力された力センサ情
報を利用してマスタマニピュレータ先端の加速度を計算
する第１の先端加速度推定装置と、第２の外乱オブザー
バからの外乱加速度の推定値と前記力センサから出力さ
れた力センサ情報を利用してスレーブマニピュレータ先
端の加速度を計算する第２の先端加速度推定装置と、前
記アーム先端位置計算装置、第１，第２の速度オブザー
バ、第１，第２の先端加速度推定装置で求められた両マ
ニピュレータの先端位置、先端速度、推定値、加速度推
定値および前記力センサから出力された力センサ情報を
用いて両マニピュレータの位置と力を一致させるマスタ
・スレーブ制御装置とを有している。
レータシステムの制御装置は、マスタマニピュレータと
スレーブマニピュレータのそれぞれの先端に力センサを
有し、関節に関節角度センサを有するマスタマニピュレ
ータとスレーブマニピュレータからなるマスタ・スレー
ブマニピュレータシステムの制御装置であって、マスタ
マニピュレータとスレーブマニピュレータのそれぞれの
先端位置を前記関節角度センサの出力から計算するアー
ム先端位置計算装置と、マスタマニピュレータの先端速
度を前記アーム先端位置計算装置から出力されたマスタ
マニピュレータの先端位置情報を用いて推定する第１の
速度オブザーバと、スレーブマニピュレータの先端速度
を前記アーム先端位置計算装置から出力されたスレーブ
マニピュレータの先端位置情報を用いて推定する第２の
速度オブザーバと、前記アーム先端位置計算装置と第１
の速度オブザーバで求められた、マスタマニピュレータ
の先端位置と先端速度の推定情報を利用して両マニピュ
レータが含む未知のダイナミクスに起因してマスタマニ
ピュレータ先端に発生する外乱加速度を推定する第１の
外乱オブザーバと、前記アーム先端位置計算装置と第２
の速度オブザーバで求められた、スレーブマニピュレー
タの先端位置と先端速度の推定情報を利用して両マニピ
ュレータが含む未知のダイナミクスに起因してスレーブ
マニピュレータ先端に発生する外乱加速度を推定する第
２のオブザーバと、第１の外乱オブザーバからの外乱加
速度の推定値と前記力センサから出力された力センサ情
報を利用してマスタマニピュレータ先端の加速度を計算
する第１の先端加速度推定装置と、第２の外乱オブザー
バからの外乱加速度の推定値と前記力センサから出力さ
れた力センサ情報を利用してスレーブマニピュレータ先
端の加速度を計算する第２の先端加速度推定装置と、前
記アーム先端位置計算装置、第１，第２の速度オブザー
バ、第１，第２の先端加速度推定装置で求められた両マ
ニピュレータの先端位置、先端速度、推定値、加速度推
定値および前記力センサから出力された力センサ情報を
用いて両マニピュレータの位置と力を一致させるマスタ
・スレーブ制御装置とを有している。
【作用】外乱オブザーバと先端加速度推定装置を用いて
マスタマニピュレータ，スレーブマニピュレータの先端
に発生する加速度を推定するので、加速度センサを用い
ることなく、オペレータに高度の臨場感を与えることの
できるマスタ・スレーブシステムを実現できる。また、
摩擦力のようなマニピュレータに含まれる未知のダイナ
ミクスを補償することができる。
マスタマニピュレータ，スレーブマニピュレータの先端
に発生する加速度を推定するので、加速度センサを用い
ることなく、オペレータに高度の臨場感を与えることの
できるマスタ・スレーブシステムを実現できる。また、
摩擦力のようなマニピュレータに含まれる未知のダイナ
ミクスを補償することができる。
【０００７】なお、マスタマニピュレータ、スレーブマ
ニピュレータが関節角速度センサを備えていない場合に
は、アーム先端速度計算装置の代りに速度オブザーバを
備え、アーム先端位置計算装置から出力されたアーム先
端位置情報を用いてマスタマニピュレータ、スレーブマ
ニピュレータの先端速度を推定する。
ニピュレータが関節角速度センサを備えていない場合に
は、アーム先端速度計算装置の代りに速度オブザーバを
備え、アーム先端位置計算装置から出力されたアーム先
端位置情報を用いてマスタマニピュレータ、スレーブマ
ニピュレータの先端速度を推定する。
【０００８】
【実施例】次に、本発明の実施例について図面を参照し
て説明する。
て説明する。
【０００９】図１は本発明の一実施例のマスタ・マニピ
ュレータシステムの制御装置のブロック図である。多自
由度のマスタおよびスレーブマニピュレータの運動方程
式は次のように表すことができる。
ュレータシステムの制御装置のブロック図である。多自
由度のマスタおよびスレーブマニピュレータの運動方程
式は次のように表すことができる。
【００１０】
【数１】
【００１１】
【数２】 ただし、ｑｍ，ｑｓはマスタおよびスレーブマニピュレ
ータ１０、２０の関節角ベクトル、τｍ，τｓは各関節
駆動トルクベクトル、Ｍｍ、Ｍｓは慣性行列、ｈｍ，ｈ
ｓは求心力、コリオリ力、重力、摩擦力等を表すベクト
ルである。Ｆｍはオペレータがマスタマニピュレータ１
０先端に加える力、Ｆｓはスレーブマニピュレータ２０
先端が操作対象物体に作用する力を示している。力Ｆ
ｍ，Ｆｓはそれぞれ３軸力センサ１１，２１で検出され
る。また、Ｘｍ，Ｘｓを各マニピュレータ１０，２０の
先端の位置ベクトル，Ｊｍ，Ｊｓを各ヤコビアン行列と
する。Ｘｍ，Ｘｓはそれぞれアーム先端位置計算装置３
１，３２により関節角度ベクトルｑｍ，ｑｓより計算さ
れる。さらに、各マニピュレータ１０，２０の先端速度
ータ１０、２０の関節角ベクトル、τｍ，τｓは各関節
駆動トルクベクトル、Ｍｍ、Ｍｓは慣性行列、ｈｍ，ｈ
ｓは求心力、コリオリ力、重力、摩擦力等を表すベクト
ルである。Ｆｍはオペレータがマスタマニピュレータ１
０先端に加える力、Ｆｓはスレーブマニピュレータ２０
先端が操作対象物体に作用する力を示している。力Ｆ
ｍ，Ｆｓはそれぞれ３軸力センサ１１，２１で検出され
る。また、Ｘｍ，Ｘｓを各マニピュレータ１０，２０の
先端の位置ベクトル，Ｊｍ，Ｊｓを各ヤコビアン行列と
する。Ｘｍ，Ｘｓはそれぞれアーム先端位置計算装置３
１，３２により関節角度ベクトルｑｍ，ｑｓより計算さ
れる。さらに、各マニピュレータ１０，２０の先端速度
【００１２】
【数３】 はそれぞれアーム先端速度計算装置３３，３４により関
節角速度ベクトルとヤコビアン行列Ｊｍ，Ｊｓの積によ
り求められる。ここで、少なくとも両マニピュレータ１
０，２０の慣性行列は既知であると仮定して次の形の関
節駆動トルクベクトルを考える。
節角速度ベクトルとヤコビアン行列Ｊｍ，Ｊｓの積によ
り求められる。ここで、少なくとも両マニピュレータ１
０，２０の慣性行列は既知であると仮定して次の形の関
節駆動トルクベクトルを考える。
【００１３】
【数４】
【００１４】
【数５】 ただし、Ｕｍ，Ｕｓは加速度の次元を有する新しい制御
入力ベクトルである。（３），（４）式をそれぞれ
（１），（２）式に代入すれば次式を得る。
入力ベクトルである。（３），（４）式をそれぞれ
（１），（２）式に代入すれば次式を得る。
【００１５】
【数６】
【００１６】
【数７】 ただし、Γｍ、Γｓはそれぞれマスタおよびスレーブマ
ニピュレータ１０、２０先端に関する慣性テンソル、ｄ
ｍ，ｄｓはｈｍ，ｈｓ等の補償していないダイナミクス
や摩擦力等の未知のダイナミクスに起因する外乱ベクト
ルを示している。（５），（６）式右辺が少なくともサ
ンプリング時間Ｔｓの間零次ホールドされるものして離
散値システムに変換すれば次式を得る。
ニピュレータ１０、２０先端に関する慣性テンソル、ｄ
ｍ，ｄｓはｈｍ，ｈｓ等の補償していないダイナミクス
や摩擦力等の未知のダイナミクスに起因する外乱ベクト
ルを示している。（５），（６）式右辺が少なくともサ
ンプリング時間Ｔｓの間零次ホールドされるものして離
散値システムに変換すれば次式を得る。
【００１７】
【数８】
【００１８】
【数９】 アーム先端位置計算装置３１，３２は、１サンプリング
時間の演算時間遅れを考慮し、入力ベクトルＵｍ［ｋ＋
１］，Ｕｓ［ｋ＋１］を、時刻ｔ＝ｋＴｓ以前のサンプ
リングデータから計算する（図２）。
時間の演算時間遅れを考慮し、入力ベクトルＵｍ［ｋ＋
１］，Ｕｓ［ｋ＋１］を、時刻ｔ＝ｋＴｓ以前のサンプ
リングデータから計算する（図２）。
【００１９】外乱オブザーバ３５、３６は、速度信号Ｘ
ｍ［ｋ］，Ｘｓ［ｋ］を用いて時刻ｔ＝ｋＴｓにおける
外乱推定値ｄ’ｍ［ｋ−１］，ｄ’ｓ［ｋ］を次のよう
に求める。
ｍ［ｋ］，Ｘｓ［ｋ］を用いて時刻ｔ＝ｋＴｓにおける
外乱推定値ｄ’ｍ［ｋ−１］，ｄ’ｓ［ｋ］を次のよう
に求める。
【００２０】
【数１０】
【００２１】
【数１１】 先端力の速度推定装置３７，３８はこれと力Ｆｍ［ｋ＋
１］，Ｆｓ［ｋ＋１］を用いて加速度ベクトルの推定値
１］，Ｆｓ［ｋ＋１］を用いて加速度ベクトルの推定値
【００２２】
【数１２】 を次式で計算する。
【００２３】
【数１３】
【００２４】
【数１４】 両マニピュレータ１０，２０の先端位置ベクトルＸｍ，
Ｘｓと力Ｆｍ，Ｆｓを一致させるバイラテラル制御アル
ゴリズムは、例えばProceedings of IEEE/RSJ I-nterna
tional Workshop on Intelligent Robots and Systems,
pp.929-936,1991の吉田らの文献“Digital Control Sta
bility Improvement of Master-Slave M-anipulator Sy
stem”に示されているように、Ｋｐ，Ｋｄをそれぞれ位
置ベクトルＸｍ，Ｘｓ、速度ベクトルに関するフィード
バックゲインとして次のように与えられる。
Ｘｓと力Ｆｍ，Ｆｓを一致させるバイラテラル制御アル
ゴリズムは、例えばProceedings of IEEE/RSJ I-nterna
tional Workshop on Intelligent Robots and Systems,
pp.929-936,1991の吉田らの文献“Digital Control Sta
bility Improvement of Master-Slave M-anipulator Sy
stem”に示されているように、Ｋｐ，Ｋｄをそれぞれ位
置ベクトルＸｍ，Ｘｓ、速度ベクトルに関するフィード
バックゲインとして次のように与えられる。
【００２５】
【数１５】
【００２６】
【数１６】 マスタ・スレーブ制御装置３９は、上式の制御アルゴリ
ズム中の加速度信号として先端加速度推定装置３７，３
８で求められた推定値をもちいると共に、未知のダイナ
ミクスの補償のために推定された外乱情報を逆向きにし
てフィールドフォワードし、最終的に（３），（４）式
と組合わせて先端位置ベクトルＸｍとＸｓ、力ＦｍとＦ
ｓを一致させる。
ズム中の加速度信号として先端加速度推定装置３７，３
８で求められた推定値をもちいると共に、未知のダイナ
ミクスの補償のために推定された外乱情報を逆向きにし
てフィールドフォワードし、最終的に（３），（４）式
と組合わせて先端位置ベクトルＸｍとＸｓ、力ＦｍとＦ
ｓを一致させる。
【００２７】本発明の有効性を確認するためにスレーブ
マニピュレータ２０が自由に動ける場合と完全に拘束を
受ける場合の特徴的な２種類の条件のもとで数値シミュ
レーションを行う。マスタマヌピュレータ１０として図
３に示すような２リンクマヌピュレータを用いる。ま
た、各パラメータを表１に示す。スレーブマニピュレー
タ２０側はそのダイナミクスが理想的に補償され、マニ
ピュレータ先端が質量１［ｋｇ］の質点とみなすことが
できるものとする。オペレータのダイナミクスも質量３
［ｋｇ］の質点で表し、これにｔ＝０［ｓｅｃ］でＦｏ
ｐ＝３１．２［Ｎ］のステップ状の筋力が垂直上向きに
作用する。重力加速度は９．８［ｍ／ｓ2］とする。フ
ィードバックゲイン行列は、 Ｋｐ＝ｄｉａｇ［５０，５０］，Ｋｄ＝ｄｉａｇ［５，５］ のように選び、サンプリング時間Ｔｓは１０［ｍｓｅ
ｃ］とした。また、制御を始める直前までマスタマニピ
ュレータ１０には自重を支えるための初期トルクが与え
られているものとする。
マニピュレータ２０が自由に動ける場合と完全に拘束を
受ける場合の特徴的な２種類の条件のもとで数値シミュ
レーションを行う。マスタマヌピュレータ１０として図
３に示すような２リンクマヌピュレータを用いる。ま
た、各パラメータを表１に示す。スレーブマニピュレー
タ２０側はそのダイナミクスが理想的に補償され、マニ
ピュレータ先端が質量１［ｋｇ］の質点とみなすことが
できるものとする。オペレータのダイナミクスも質量３
［ｋｇ］の質点で表し、これにｔ＝０［ｓｅｃ］でＦｏ
ｐ＝３１．２［Ｎ］のステップ状の筋力が垂直上向きに
作用する。重力加速度は９．８［ｍ／ｓ2］とする。フ
ィードバックゲイン行列は、 Ｋｐ＝ｄｉａｇ［５０，５０］，Ｋｄ＝ｄｉａｇ［５，５］ のように選び、サンプリング時間Ｔｓは１０［ｍｓｅ
ｃ］とした。また、制御を始める直前までマスタマニピ
ュレータ１０には自重を支えるための初期トルクが与え
られているものとする。
【００２８】
【表１】 スレーブマニピュレータ１０側に操作対象物体がなく、
外力の作用もない場合（Ｆｓ＝０）、理想的なマスタ・
スレーブ制御が実現されていればオペレータはマスタマ
ニピュレータ１０の反力（−Ｆｍ）を受けることなく垂
直上向きに０．６［ｍ／ｓ2］の等加速度運動をするこ
とになる。図４は提案したアルゴリズムで制御した場合
のマスタおよびスレーブマニピュレータ１０，２０の先
端位置Ｘｍ、Ｘｓと力Ｆｍのｘ軸、ｚ軸それぞれの時間
応答を示したものである。。先端速度の増加に伴う外乱
の影響を受けるものの、同図よりほぼ理想的なマスタ・
スレーブマニピュレータのバイラテラル制御が実現され
ていることが分かる。また、同図ではサンプル点間の応
答も示しており、力ベクトルＦｍがサンプル点の前後で
微小に変動していることが分かる。
外力の作用もない場合（Ｆｓ＝０）、理想的なマスタ・
スレーブ制御が実現されていればオペレータはマスタマ
ニピュレータ１０の反力（−Ｆｍ）を受けることなく垂
直上向きに０．６［ｍ／ｓ2］の等加速度運動をするこ
とになる。図４は提案したアルゴリズムで制御した場合
のマスタおよびスレーブマニピュレータ１０，２０の先
端位置Ｘｍ、Ｘｓと力Ｆｍのｘ軸、ｚ軸それぞれの時間
応答を示したものである。。先端速度の増加に伴う外乱
の影響を受けるものの、同図よりほぼ理想的なマスタ・
スレーブマニピュレータのバイラテラル制御が実現され
ていることが分かる。また、同図ではサンプル点間の応
答も示しており、力ベクトルＦｍがサンプル点の前後で
微小に変動していることが分かる。
【００２９】スレーブマニピュレータ２０が固定され動
かない状態になった場合、理想的なマスタ・スレーブ制
御が実現されていればオペレータの位置も固定される。
また、オペレータがマスタマニピュレータ１０に及ぼす
力（３１．２［Ｎ］−３×９．８［Ｎ］＝１．８
［Ｎ］）がそのままマスタマニピュレータ１０からの反
力となる。図５はマスタおよびスレーブマニピュレータ
１０、２０の先端位置Ｘｍ，Ｘｓと力Ｆｍ，Ｆｓのｘ
軸、ｚ軸それぞれの時間応答を示したものである。同図
よりマスタマニピュレータ線谷値のオーバーシュートは
極力低く抑えられ、力ベクトルＦｍ，Ｆｓも目標値に速
やかに収束している様子が分かる。
かない状態になった場合、理想的なマスタ・スレーブ制
御が実現されていればオペレータの位置も固定される。
また、オペレータがマスタマニピュレータ１０に及ぼす
力（３１．２［Ｎ］−３×９．８［Ｎ］＝１．８
［Ｎ］）がそのままマスタマニピュレータ１０からの反
力となる。図５はマスタおよびスレーブマニピュレータ
１０、２０の先端位置Ｘｍ，Ｘｓと力Ｆｍ，Ｆｓのｘ
軸、ｚ軸それぞれの時間応答を示したものである。同図
よりマスタマニピュレータ線谷値のオーバーシュートは
極力低く抑えられ、力ベクトルＦｍ，Ｆｓも目標値に速
やかに収束している様子が分かる。
【００３０】なお、マスタマニピュレータ１０、スレー
ブマニピュレータ２０が関節角速度センサを備えていな
い場合には、速度オブザーバによりアーム先端位置計算
装置３１，３２から出力されたアーム先端位置情報を用
いて、マスタマニピュレータ１０、スレーブマニピュレ
ータ２０の先端速度を推定する。
ブマニピュレータ２０が関節角速度センサを備えていな
い場合には、速度オブザーバによりアーム先端位置計算
装置３１，３２から出力されたアーム先端位置情報を用
いて、マスタマニピュレータ１０、スレーブマニピュレ
ータ２０の先端速度を推定する。
【００３１】
【発明の効果】以上説明したように、本発明は、マスタ
マニピュレータ、スレーブマニピュレータ先端に発生す
る外乱加速度を推定する外乱オブザーバと先端加速度推
定装置を有することにより、オペレータに高度の臨場感
を与えることのできるマスタ・スレーブシステムを加速
度センサを用いることなく実現でき、摩擦力のようなマ
ニピュレータに含まれる未知のダイナミクスを補償可能
であるという効果を有する。
マニピュレータ、スレーブマニピュレータ先端に発生す
る外乱加速度を推定する外乱オブザーバと先端加速度推
定装置を有することにより、オペレータに高度の臨場感
を与えることのできるマスタ・スレーブシステムを加速
度センサを用いることなく実現でき、摩擦力のようなマ
ニピュレータに含まれる未知のダイナミクスを補償可能
であるという効果を有する。
【図１】本発明の一実施例のマスタ・スレーブマニピュ
レータシステムの制御装置のブロック図である。
レータシステムの制御装置のブロック図である。
【図２】ディジタル制御を行う際の、システムの状態変
数と制御変数の時間経過を示すグラフである。
数と制御変数の時間経過を示すグラフである。
【図３】シミュレーションに用いたマスタマニピュレー
タ１０のモデルを示す図である。
タ１０のモデルを示す図である。
【図４】スレーブマニピュレータ２０が自由に動ける場
合の本実施例によるシミュレーション結果を示す図であ
る。
合の本実施例によるシミュレーション結果を示す図であ
る。
【図５】スレーブマニピュレータ２０が拘束を受ける場
合の本実施例によるシミュレーション結果を示す図であ
る。
合の本実施例によるシミュレーション結果を示す図であ
る。
１０ マスタマニピュレータ １１ ３軸力センサ ２０ スレーブマニピュレータ ２１ ３軸センサ ３０ マスタ・スレーブマニピュレータシステム制御装
置 ３１，３２ アーム先端位置計算装置 ３３，３４ アーム先端速度計算装置 ３５，３６ 外乱オブザーバ ３７，３８ 先端加速度推定装置 ３９ マスタ・スレーブ制御装置
置 ３１，３２ アーム先端位置計算装置 ３３，３４ アーム先端速度計算装置 ３５，３６ 外乱オブザーバ ３７，３８ 先端加速度推定装置 ３９ マスタ・スレーブ制御装置
Claims (2)
- 【請求項１】 先端に力センサを有し、関節に関節角度
センサと関節角速度センサを有するマスタマニピュレー
タとスレーブマニピュレータからなるマスタ・スレーブ
マニピュレータシステムの制御装置であって、 マスタマニピュレータとスレーブマニピュレータのそれ
ぞれの先端位置を前記関節角度センサの出力から計算す
るアーム先端位置計算装置と、 マスタマニピュレータとスレーブマニピュレータのそれ
ぞれの先端速度を前記関節角速度センサの出力から計算
するアーム先端速度計算装置と、 前記アーム先端位置計算装置と前記アーム先端速度計算
装置で求められた、マスタマニピュレータの先端位置と
先端速度の情報を利用して両マニピュレータが含む未知
のダイナミクスに起因してマスタマニピュレータ先端に
発生する外乱加速度を推定する第１の外乱オブザーバ
と、 前記アーム先端位置計算装置と前記アーム先端速度計算
装置で求められた、スレーブマニピュレータの先端位置
と先端速度の情報を利用して両マニピュレータが含む未
知のダイナミクスに起因してスレーブマニピュレータ先
端に発生する外乱加速度を推定する第２のオブザーバ
と、 第１の外乱オブザーバからの外乱加速度の推定値と前記
力センサから出力された力センサ情報を利用してマスタ
マニピュレータ先端の加速度を計算する第１の先端加速
度推定装置と、 第２の外乱オブザーバからの外乱加速度の推定値と前記
力センサから出力された力センサ情報を利用してスレー
ブマニピュレータ先端の加速度を計算する第２の先端加
速度推定装置と、 前記アーム先端位置計算装置、前記アーム先端速度計算
装置、第１，第２の先端加速度推定装置で求められた両
マニピュレータの先端位置、先端速度、加速度推定値お
よび前記力センサから出力された力センサ情報を用いて
両マニピュレータの位置と力を一致させるマスタ・スレ
ーブ制御装置とを有するマスタ・スレーブマニピュレー
タシステムの制御装置。 - 【請求項２】 先端に力センサを有し、関節に関節角度
センサを有するマスタマニピュレータとスレーブマニピ
ュレータからなるマスタ・スレーブマニピュレータシス
テムの制御装置であって、 マスタマニピュレータとスレーブマニピュレータのそれ
ぞれの先端位置を前記関節角度センサの出力から計算す
るアーム先端位置計算装置と、 マスタマニピュレータの先端速度を前記アーム先端位置
計算装置から出力されたマスタマニピュレータの先端位
置情報を用いて推定する第１の速度オブザーバと、 スレーブマニピュレータの先端速度を前記アーム先端位
置計算装置から出力されたスレーブマニピュレータの先
端位置情報を用いて推定する第２の速度オブザーバと、 前記アーム先端位置計算装置と第１の速度オブザーバで
求められた、マスタマニピュレータの先端位置と先端速
度の推定情報を利用して両マニピュレータが含む未知の
ダイナミクスに起因してマスタマニピュレータ先端に発
生する外乱加速度を推定する第１の外乱オブザーバと、 前記アーム先端位置計算装置と第２の速度オブザーバで
求められた、スレーブマニピュレータの先端位置と先端
速度の推定情報を利用して両マニピュレータが含む未知
のダイナミクスに起因してスレーブマニピュレータ先端
に発生する外乱加速度を推定する第２のオブザーバと、 第１の外乱オブザーバからの外乱加速度の推定値と前記
力センサから出力された力センサ情報を利用してマスタ
マニピュレータ先端の加速度を計算する第１の先端加速
度推定装置と、 第２の外乱オブザーバからの外乱加速度の推定値と前記
力センサから出力された力センサ情報を利用してスレー
ブマニピュレータ先端の加速度を計算する第２の先端加
速度推定装置と、 前記アーム先端位置計算装置、第１，第２の速度オブザ
ーバ、第１，第２の先端加速度推定装置で求められた両
マニピュレータの先端位置、先端速度推定値、加速度推
定値および前記力センサから出力された力センサ情報を
用いて両マニピュレータの位置と力を一致させるマスタ
・スレーブ制御装置とを有するマスタ・スレーブマニピ
ュレータシステムの制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP1211192A JPH05204460A (ja) | 1992-01-27 | 1992-01-27 | マスタ・スレーブマニピュレータシステムの制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP1211192A JPH05204460A (ja) | 1992-01-27 | 1992-01-27 | マスタ・スレーブマニピュレータシステムの制御装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH05204460A true JPH05204460A (ja) | 1993-08-13 |
Family
ID=11796458
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP1211192A Pending JPH05204460A (ja) | 1992-01-27 | 1992-01-27 | マスタ・スレーブマニピュレータシステムの制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH05204460A (ja) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2014004656A (ja) * | 2012-06-25 | 2014-01-16 | Univ Of Tsukuba | マニピュレーションシステム |
| JP2014004655A (ja) * | 2012-06-25 | 2014-01-16 | Univ Of Tsukuba | マニピュレーションシステム |
-
1992
- 1992-01-27 JP JP1211192A patent/JPH05204460A/ja active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2014004656A (ja) * | 2012-06-25 | 2014-01-16 | Univ Of Tsukuba | マニピュレーションシステム |
| JP2014004655A (ja) * | 2012-06-25 | 2014-01-16 | Univ Of Tsukuba | マニピュレーションシステム |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP3651942B1 (en) | Vibration control of systems with configuration dependent dynamics | |
| Swevers et al. | Experimental robot identification using optimised periodic trajectories | |
| US5767648A (en) | Base force/torque sensor apparatus for the precise control of manipulators with joint friction and a method of use thereof | |
| CN106483964B (zh) | 一种基于接触力观测器的机器人柔顺控制方法 | |
| Khosla et al. | Parameter identification of robot dynamics | |
| RU2487796C2 (ru) | Управление перемещением упругих структур робота | |
| An et al. | Experimental determination of the effect of feedforward control on trajectory tracking errors | |
| JPH0740204B2 (ja) | 多自由度非線形機械システムの制御装置 | |
| Han et al. | Toward sensorless interaction force estimation for industrial robots using high-order finite-time observers | |
| WO2019087357A1 (ja) | キャリブレーション装置、キャリブレーション方法および制御装置 | |
| JPS63314606A (ja) | 多関節ロボットの制御装置 | |
| JPH10133703A (ja) | 適応的ロバスト制御装置 | |
| JP2010524714A (ja) | 関節を制御する方法及びトルク速度変換装置 | |
| Chan et al. | Position and force tracking for non-linear haptic telemanipulator under varying delays with an improved extended active observer | |
| Lu et al. | Experimental determination of dynamic parameters of robotic arms | |
| Su et al. | Hybrid adaptive/robust motion control of rigid-link electrically-driven robot manipulators | |
| Ramadurai et al. | Application of unscented kalman filter to a cable driven surgical robot: A simulation study | |
| Huang et al. | Position/force control of uncertain constrained flexible joint robots | |
| JPH10128688A (ja) | ロボットの非干渉化制御方法 | |
| US5091683A (en) | Servo control apparatus | |
| JP3698770B2 (ja) | 負荷重量の推定方法 | |
| WO2006046500A1 (ja) | 通信遅延を有する通信路を介して信号を送受信する遠隔制御システム | |
| JPH05204460A (ja) | マスタ・スレーブマニピュレータシステムの制御装置 | |
| Angel et al. | Parametric identification of a delta type parallel robot | |
| JPH0852675A (ja) | マニピュレータの選択的外乱補償型ハイブリッド制御装置および制御方法 |