(57)【要約】【課題】  マニピュレーターの関節加速度を利用しなくても、マニピュレーターに作用する外乱トルクを正確に推定できるロボットマニピュレーターの制御装置及びその制御方法を提供する。【解決手段】  マニピュレーターの関節位置と関節トルクを感知する感知器と、前記感知された関節位置と関節トルクを入力とする前記マニピュレーターに対する状態空間方程式を用いて外乱トルクを推定する外乱推定器と、前記推定された外乱トルクに基づいて前記マニピュレーターを制御する制御器とを含んでロボットマニピュレーターの制御装置を構成する。【選択図】      図３
2009285824.tif 000002
【特許請求の範囲】
【請求項１】
マニピュレーターの関節位置と関節トルクを感知する感知器と；
前記感知された関節位置と関節トルクを入力とする前記マニピュレーターに対する状態空間方程式を用いて外乱トルクを推定する外乱推定器と；
前記推定された外乱トルクに基づいて前記マニピュレーターを制御する制御器と；を含むロボットマニピュレーターの制御装置。
【請求項２】
前記外乱推定器は、前記マニピュレーターの関節トルク値、重力補償値、コリオリ力補償値及び慣性補償値に基づいて前記外乱トルクを推定することを特徴とする請求項１に記載のロボットマニピュレーターの制御装置。
【請求項３】
前記状態空間方程式は、次のような式
【数１９】
2009285824.tif 000021
を含み、
ｄは、前記外乱トルクの推定値であり、Ｚは、状態変数であり、ｐは、状態変数であり、Ｌは、正の定符号行列であり、τは、駆動トルクであり、Ｃは、コリオリ及び遠心行列であり、
【数２０】
2009285824.tif 000022
は、関節速度であり、ｇは、重力ベクトルであり、Ｍは、慣性行列である、
ことを特徴とする請求項１に記載のロボットマニピュレーターの制御装置。
【請求項４】
前記感知器は、前記マニピュレーターの関節位置を感知する位置センサーと、前記マニピュレーターの関節トルクを感知するトルクセンサーと、を含むことを特徴とする請求項１に記載のロボットマニピュレーターの制御装置。
【請求項５】
前記感知器は、前記マニピュレーターの関節位置を感知する位置センサーと、前記マニピュレーターの関節を駆動するモーターの電流を感知する電流センサーと、を備えており、前記感知されたモーター電流から前記関節トルクを推定することを特徴とする請求項１に記載のロボットマニピュレーターの制御装置。
【請求項６】
前記マニピュレーターは、剛性可変が可能であり、前記制御器は、前記外乱推定器によって推定された外乱トルクが予め設定された値より大きい場合、前記マニピュレーターの剛性を可変することを特徴とする請求項１に記載のロボットマニピュレーターの制御装置。
【請求項７】
マニピュレーターの関節位置と関節トルクを感知する段階と；
前記感知された関節位置と関節トルクに基づいて前記マニピュレーターに対する状態空間方程式を用いて外乱トルクを推定する段階と；を含むロボットマニピュレーターの制御方法。
【請求項８】
前記外乱トルクを推定する段階は、次の状態空間方程式
【数２１】
2009285824.tif 000023
を用いて前記外乱トルクを推定することを含み、
ｄは、前記外乱トルクの推定値であり、Ｚは、状態変数であり、ｐは、状態変数であり、Ｌは、正の定符号行列であり、τは、駆動トルクであり、Ｃは、コリオリ及び遠心行列であり、
【数２２】
2009285824.tif 000024
は、関節速度であり、ｇは、重力ベクトルであり、Ｍは、慣性行列である、
ことを特徴とする請求項７に記載のロボットマニピュレーターの制御方法。
【請求項９】
前記推定された外乱トルクに基づいて前記マニピュレーターの剛性を可変する段階をさらに含むことを特徴とする請求項７に記載のロボットマニピュレーターの制御方法。
【請求項１０】
前記剛性を可変する段階は、前記推定された外乱トルクと予め設定された値を比較する段階と、前記比較結果、前記推定された外乱トルクが前記予め設定された値より大きい場合、前記マニピュレーターが物体と衝突したと判断する段階と、前記判断結果によって前記マニピュレーターの剛性を可変する段階と、を含むことを特徴とする請求項９に記載のロボットマニピュレーターの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットマニピュレーターの制御装置及びその制御方法に関するもので、より詳細には、マニピュレーターに加えられた外乱トルクを推定してマニピュレーターと人間との間の衝突を検出し、人間との衝突時に人間が衝撃を受けないように柔軟性を制御できるロボットマニピュレーターの制御装置及びその制御方法に関するものである。
【背景技術】
【０００２】
一般的な産業用ロボットは、人間による動作や監督なしにも正確な作業を遂行するため、生産ラインなどで広範囲に使用されている。例えば、自動車産業で使用されているロボットは、自動車の本体を運搬または溶接するなどの多様な作業を遂行している。
【０００３】
知能型サービスロボット（以下、ロボットという。）は、一般的な産業用ロボットと異なり、人間の居住・活動空間内で作業を遂行するようになる。したがって、人間とぶつかって人間に衝撃を加える危険性があるので、知能型サービスロボットのうち、人との衝突発生可能性が最も高いマニピュレーターの場合、人に対する安全性の確保程度が重要な仕様となる。マニピュレーターは、人間の手と腕の形態からなり、手と腕の動作を提供する機械的な装置である。現在使用されているほとんどのマニピュレーターは、多数個のリンクが互いに連結されて構成される。各リンクの部位を関節というが、マニピュレーターの運動特性は、このようなリンクと各関節との間の幾何学的な関係によって決定される。
【０００４】
このための一般的な技術的解決方案として、マニピュレーターのソフトウェア的な知能を向上させ、予め周囲の障害物を認識することで、衝突可能性を予め予測して危険を除去する方法論が最も理想的であるが、演算速度及びその他のアルゴリズム／知能具現技術水準が未だに絶対的な安全性を保障できない水準に留まっている。したがって、マニピュレーターを開発する場合には、衝突を前提とした安全保障対策が必須的である。
【０００５】
マニピュレーターが人間と衝突するとき、人間が衝撃を受けないようにマニピュレーターに柔軟性を与える必要がある。このような技術的解決方案をロボットコンプライアンス（ｒｏｂｏｔ  ｃｏｍｐｌｉａｎｃｅ）という。マニピュレーターにロボットコンプライアンスを与える方法としては、スプリング及びダンパーなどの要素を用いて機械的なメカニズムで柔軟性を与える受動的な方法と、マニピュレーターに装着されたセンサーのフィードバック信号を制御器で感知し、外部で作用した力や衝撃に対して適切な柔軟性を与える能動的な方法がある。
【０００６】
マニピュレーターが人間と衝突した場合、これを正確に検出しなければならない。特許文献１及び特許文献２では、外乱推定器によってロボットの腕に加えられた外乱トルクを推定し、この外乱トルクが予め設定された値以上である場合、ロボットの腕が衝突したと判断する。この方法は、力センサーなどの衝突検出用のセンサーを使用せずロボットの腕の衝突発生を検出する。
【０００７】
上記の各特許文献に開示された関連技術において、外乱推定器は、外乱トルクを次のように表現されるマニピュレーターに対する動力学モデルに基づいた状態空間方程式（１）によって求めている。
【０００８】
【数１】
2009285824.tif 000003
ここで、Ｍは、マニピュレーターの慣性行列で、Ｃは、コリオリ及び遠心行列で、ｇは、重力ベクトルで、τは、各関節に作用する駆動トルクで、ｄは、外部の作用力によって発生する関節の外乱トルクで、
【０００９】
【数２】
2009285824.tif 000004
は関節加速度で、
【００１０】
【数３】
2009285824.tif 000005
は関節速度である。
【特許文献１】日本公開特許公報特開平６−１３１０５０号
【特許文献２】日本公開特許公報特開平１１−２５４３８０号
【発明の概要】
【発明が解決しようとする課題】
【００１１】
上記の式（１）から分かるように、既存には、外乱トルクを推定するために必ず関節加速度を知らなければならなかった。したがって、関節加速度を知るために、関節位置を測定して２次微分したり、マニピュレーターの関節に加速度センサーを別途に設置しなければならなかった。
【００１２】
しかしながら、観測された関節位置値に２次の数値微分をして関節加速度を求める場合、測定された関節位置値に含まれたノイズも一緒に増幅されるので、正確な関節加速度を得ることが難しい。また、マニピュレーターの関節に加速度センサーを設置する場合、マニピュレーターの運動に制限要素として作用し、センサーノイズのために正確な関節加速度を得ることが難しい。また、部品の追加によって製造費用が増加し、維持補修が難しいという問題点がある。
【００１３】
したがって、本発明の目的は、マニピュレーターの関節加速度を利用しなくても、マニピュレーターに作用する外乱トルクを正確に推定できるロボットマニピュレーターの制御装置及びその制御方法を提供することにある。
【課題を解決するための手段】
【００１４】
上述した目的を達成するための本発明に係るロボットマニピュレーターの制御装置は、マニピュレーターの関節位置と関節トルクを感知する感知器と、前記感知された関節位置と関節トルクを入力とする前記マニピュレーターに対する状態空間方程式を用いて外乱トルクを推定する外乱推定器と、前記推定された外乱トルクに基づいて前記マニピュレーターを制御する制御器とを含むことを特徴とする。
【００１５】
また、本発明に係るロボットマニピュレーターの制御方法は、マニピュレーターの関節位置と関節トルクを感知する段階と、前記感知された関節位置と関節トルクに基づいて前記マニピュレーターに対する状態空間方程式を用いて外乱トルクを推定する段階とを含むことを特徴とする。
【発明の効果】
【００１６】
本発明によると、マニピュレーターの関節加速度を利用しなくても、マニピュレーターに作用する外乱トルクを正確に推定することで、関節加速度を獲得するために関節位置を２次微分したり、加速度センサーを別途に設置する必要がないので、外乱トルクをより正確に推定することができ、製造費用を低下させることができ、空間活用度を高めることができ、維持補修が簡便であるという効果がある。
【図面の簡単な説明】
【００１７】
【図１】本発明に適用されるロボットマニピュレーターの概略的な構成を示した図である。
【図２】本発明の実施例に係るロボットマニピュレーターの制御装置の制御ブロック図である。
【図３】図２に示した外乱推定器を説明するための図である。
【図４】本発明の実施例に係るロボットマニピュレーターと物体との衝突時におけるマニピュレーターの剛性変化を示した図である。
【発明を実施するための形態】
【００１８】
以下、本発明の実施例を図面に基づいて説明する。
【００１９】
まず、本発明に適用されるロボットマニピュレーターに関して簡単に説明する。図１は、アクチュエーターとリンクが減速器によって結合された１自由度ロボットマニピュレーターの概略図である。
【００２０】
図１に示すように、本発明に適用されるロボットマニピュレーター１は、アクチュエーター２、減速器３、リンク４、エンドエフェクター５、トルクセンサー６及び位置センサー７を備えている。アクチュエーター２は、サーボモーターからなり、減速器３を介在してリンク４に連結され、回転によってリンク４を動かす。エンドエフェクター５は、リンク４の終端に設けられ、直接的な作業を実行する役割をする。トルクセンサー６と位置センサー７は、感知部を構成する。トルクセンサー６は、マニピュレーター１の関節トルクを感知するセンサーで、位置センサー７は、マニピュレーター１の関節位置を感知するセンサーである。参考に、高効率の減速器を採用した場合、トルクセンサーの代わりにアクチュエーターの駆動電流を感知する電流センサーを使用することができ、この場合、アクチュエーターの駆動電流からマニピュレーターの関節トルクを推定する。
【００２１】
図２は、本発明の実施例に係るロボットマニピュレーター制御装置の概略的な制御ブロック図である。図２に示すように、本発明の実施例に係るロボットマニピュレーターの制御装置は、マニピュレーターの関節位置と関節トルクを感知する感知器から感知された関節位置と関節トルクを入力とするマニピュレーターに対する状態空間方程式を用いて外乱トルクを推定する外乱推定器３０と、この外乱推定器３０によって推定された外乱トルクに基づいてマニピュレーターを制御する制御器１０とを備えている。
【００２２】
全体の制御周期内でサンプルごとに作動する制御器は、まず、目標位置値（ｑｄ）、マニピュレーターの関節位置（ｑ）及び関節速度（ｄｑ／ｄｔ）の入力を受ける。制御器は、入力された各情報に基づいてマニピュレーターを目標位置に制御するための基準関節トルク（τｒｅｆ）をマニピュレーターに出力する。マニピュレーターは、制御器１０からマニピュレーターに伝達された基準関節トルク（τｒｅｆ）によって作動するようになる。これと同時に、制御器１０からマニピュレーターに伝達された基準関節トルク（τｒｅｆ）は、外乱推定器３０に入力される。このとき、マニピュレーターの関節摩擦を補償しない場合、外乱推定器３０に入力された関節トルク（τ）が基準関節トルク（τｒｅｆ）であるが、システム性能を高めるために、関節摩擦を補償するために関節摩擦推定器２０によって推定された関節摩擦がある場合、外乱推定器３０に入力された関節トルク（τ）は、基準関節トルク（τｒｅｆ）と推定された関節摩擦値とを合わせた値である。
【００２３】
一方、マニピュレーターの作動中にマニピュレーターが物体と衝突すると、衝突による実際の外乱によってマニピュレーターの出力が変化するようになる。このマニピュレーターの出力（ｑ、ｄｑ／ｄｔ）は、制御器１０及び関節摩擦推定器２０の入力にフィードバックされる。また、このマニピュレーターの出力（ｑ、ｄｑ／ｄｔ）は、外乱推定器の入力にフィードバックされる。
【００２４】
外乱推定器３０は、マニピュレーターの関節位置（ｑ）、関節速度（ｄｑ／ｄｔ）、関節トルク（τ）を入力とするマニピュレーターに対する動力学モデルによる状態空間方程式を用いて外乱トルクを推定する。これによって、外乱推定器は、関節加速度を使用しなくても外乱トルクを推定することができる。
【００２５】
一方、制御器１０は、外乱推定器３０によって推定された外乱トルク値の入力を受け、入力された外乱トルク値が予め設定された値以上である場合、マニピュレーターと物体との間に衝突があると判断する。衝突があると判断される場合、マニピュレーターの剛性を可変し、マニピュレーターの特性を構造的に柔軟にすることで物理的衝撃を最小化する。マニピュレーターの剛性を可変することに対しては、大韓民国公開特許公報第２００８−００１４３４３号に開示されている。この技術には、一般的な動作状況では高い剛性を維持するが、一定大きさ以上の衝撃が加えられる場合、低い剛性を有するように剛性を機械的に変換する関節メカニズムが開示されている。
【００２６】
以下、外乱推定器３０で外乱トルクを推定する方法を図３に基づいて説明する。
【００２７】
上述したように、マニピュレーターに対する動力学モデルに基づいた状態空間方程式は、式（１）のように表すことができる。
【００２８】
式（１）を外乱トルクに対して再び整理すると、次の式（２）のように表すことができる。
【００２９】
【数４】
2009285824.tif 000006
ここで、Ｍは、マニピュレーターの慣性行列で、Ｃは、コリオリ行列で、ｇは、重力ベクトルで、τは、各関節に作用する駆動トルクで、ｄは、外部の作用力によって発生する関節の外乱トルクで、
【００３０】
【数５】
2009285824.tif 000007
は関節加速度で、
【００３１】
【数６】
2009285824.tif 000008
は関節速度である。
【００３２】
結局、外乱トルクの認識問題は、次のｄの値を求める問題に帰着される。
【００３３】
しかしながら、外乱トルクをセンサーから直接的に得るにおいて、信号の雑音及びモデル不確実性のようなデータの信頼性に悪影響を及ぼす要素が多いので、次の式（３）のような推定値を用いるようになる。
【００３４】
【数７】
2009285824.tif 000009
ここで、Ｌは、正の定符号（Ｐｏｓｉｔｉｖｅ  Ｄｅｆｉｎｉｔｅ）行列として、観測器の利得行列である。また、外乱トルクは、比較的低周波領域で発生するので、次の式（４）のように仮定する。
【００３５】
【数８】
2009285824.tif 000010
このような外乱推定器３０の設計問題を容易に解き、各関節の加速度センサーを使用しないために、次の式（５）のような補助状態変数を定義する。一般的に、関節の加速度は、関節角度（関節位置）を２回微分するようになるが、この場合、信号雑音が増幅されて表れる。
【００３６】
【数９】
2009285824.tif 000011
この状態変数ｚに対する状態変移方程式を誘導してみると、次の式（６）のように表すことができる。
【００３７】
【数１０】
2009285824.tif 000012
このとき、関節加速度センサーの影響を除去するために、状態変数であるｐの力学を次の式（７）のように定義する。
【００３８】
【数１１】
2009285824.tif 000013
したがって、これを用いて式（６）を再び表現すると、次の式（８）のようになる。
【００３９】
【数１２】
2009285824.tif 000014
ｐに対する力学式（７）は、ロボットマニピュレーターモデル式（１）を構成する行列間の関係式によって、次の式（９）及び式（１０）のように再び定義することができる。
【００４０】
【数１３】
2009285824.tif 000015
ここで、ＣＴは、Ｃの転置行列である。
【００４１】
式（１０）は、式（７）を部分積分して得ることができる。
【００４２】
以上の過程を整理し、外乱推定器３０の支配方程式を提示すると、次の式（１１）乃至式（１３）のようになる。
【００４３】
【数１４】
2009285824.tif 000016
図３は、上記の各過程を順次的に示している。
【００４４】
このとき、外乱推定器３０の誤差が０に収斂することをリアプノフ安定度理論（Ｌｙａｐｕｎｏｖ  Ｓｔａｂｉｌｉｔｙ  Ｔｈｅｏｒｙ）を用いて証明すると、次の通りである。
【００４５】
すなわち、外乱推定器の誤差項は、
【００４６】
【数１５】
2009285824.tif 000017
に定義され、リアプノフ関数を次の式（１６）のように定義すると、
【００４７】
【数１６】
2009285824.tif 000018
これに対する微分は、次の式（１７）のように定義され、常に負の半定符号（Ｎｅｇａｔｉｖｅ  Ｓｅｍｉｄｅｆｉｎｉｔｅ）であることが分かる。
【００４８】
【数１７】
2009285824.tif 000019
式（１７）は、次に提示する式（１８）の誤差動力学によって誘導される。
【００４９】
【数１８】
2009285824.tif 000020
したがって、外乱推定器の推定誤差は、常に０に収斂し、外乱推定器によって推定された外乱トルク値を信頼可能であることを数学的に証明することができる。
【００５０】
本発明の実施例では、既存の単純な位置制御でない、動力学効果をモデルに基づいてフィードフォワード（Ｆｅｅｄｆｏｒｗａｒｄ）で補償することで、マニピュレーターの如何なる姿勢においても同一の制御性能を表すことができる。以後、外乱推定器の推定結果によって物体との衝突が判断されると、この衝突を回避したり、衝撃を吸収するための適切な対応をしなければならない。本発明の実施例では、認識された衝撃の大きさによって制御利得を変化させることで、マニピュレーターの剛性を変化させる。衝撃力が一定の臨界値を超えると、比例制御及び微分制御利得を低下させることで、システム剛性及び減衰を低下させ、衝突物体に加えられる反作用を最小化する。
【００５１】
図４は、本発明の実施例に係るロボットマニピュレーターと物体との衝突時におけるマニピュレーターの剛性変化を示している。制御利得の衝突後の時間による値は、図４のグラフによって説明される。すなわち、マニピュレーターが物体と衝突すると、制御利得を急激に低下させることで、マニピュレーターの剛性を低下させ、物体との衝突が解消されると、制御利得を徐々に上昇させることで、マニピュレーターの剛性を元通りに高める。
【符号の説明】
【００５２】
１    マニピュレーター
２    アクチュエーター
３    減速器
４    リンク
５    エンドエフェクター
６    トルクセンサー
７    位置センサー
１０  制御器
２０  外乱推定器
３０  慣性摩擦推定器
【図１】
2009285824.tif 000025
【図２】
2009285824.tif 000026
【図３】
2009285824.tif 000027
【図４】
2009285824.tif 000028