JP2007515301A

JP2007515301A - 多軸ロボットの可動部分の変位を制御するための方法及び装置

Info

Publication number: JP2007515301A
Application number: JP2006544503A
Authority: JP
Inventors: ジョリィ・リィク
Original assignee: シュトイブリー・ファベルゲ
Priority date: 2003-12-19
Filing date: 2004-12-17
Publication date: 2007-06-14
Also published as: ATE422254T1; DE602004019345D1; US7853358B2; US20080147238A1; FR2864266A1; CN100476654C; EP1695160A1; FR2864266B1; CN1894640A; WO2005062143A1; EP1695160B1

Abstract

この発明による方法は、少なくともパスの幾何形状（３２０）に関する情報と負荷の指令（３１０）を含む動き指令（３００）をパス生成器（４００）に供給する措置と、加える負荷の信号（８００）を計算する措置と、この加える負荷の信号（８００）をパス生成器（４００）に伝える措置と、加える負荷の当該のパスの接線への射影と当該の指令のこの接線への射影との間の偏差を最小化する手法で、パスに沿った動き指令（５００）を計算する措置と、ロボット（６００）を操作するための手段に当該の動き指令（５００）を伝える措置とで構成される。この制御を実施するための手段（２００，４００，７００）を備えた装置も開示する。

Description

この発明は、多軸ロボットの可動部分の変位をパスに沿って制御するための方法及び装置に関する。

多軸ロボットの制御分野では、位置、速度及び電流に作用するＰＩＤ（比例積分微分）型サーボ制御ループを用いて、多軸ロボットを制御して、工具又はグリップ部を取り付けることが可能なロボットの端末部分を所定の形状と速度プロフィルのパスに沿って動かすことが知られている。ロボットの可動部とそれらの周囲環境との間で互いに作用し合う力の関数として、アームの位置を変更することを可能とするために、各軸でのサーボ制御の剛性を低減する形で、そのようなロボットを制御することも知られている。例えば、特許文献１により、特に、直交座標系において、多軸ロボットのアームの剛性を設定するパラメータを使用し、位置の誤差を測定して、力を計算し、ロボットの可動部分の位置を修正して、端末部材における或る程度の柔軟性を得ることが知られている。接触力が、ロボット用にプログラミングされた動きに対抗する可能性がある、研磨、面取り、プレスからの引き抜きなどを目的として、取扱い部品の操作を行うために、そのような柔軟性を利用することができる。

例えば、面取り又は研磨のために、パスに沿って力のレベルを制御する形で操作を行うために、直交座標系で力目標を与えることも知られている。

周知のシステムでは、パスに沿って可動部分の速度又は加速度などのパラメータを規定することができるパスプログラミング・インタフェースが配備されている場合があり、これらのパラメータは、そのパスに沿ったロボットアームの端末部分の位置を時間の関数として決定するものである。

特許文献２により、パルス幅変調を使用して、ロボットの関節に関するブラシレスモータを数値制御することも知られている。

これらの周知のシステムでは、多軸ロボットをプログラミングして、一方において、速度と加速度の両方に関して、端末部分の移動パスを制御可能とし、他方において、可動部分とその周囲環境との間の接点におけるパスの接線方向の力も制御すると同時に、ロボットの機械的な構造に見合った速度と加速度の値に適合させることは容易ではない。

また、特許文献３は、通過したパスからロボットを逸らせて、処理する部品の実際の形状の関数としての力目標に適合させることを開示している。この例では、パスに従うことよりも、力の制御を優先しており、それは、精確にパスに従うことが必須である一定の用途には適合しない。
米国特許公開明細書第５７４２１３８−Ａ号米国特許公開明細書第４８７４９９７−Ａ号欧州特許公開明細書第０３４９２９１−Ａ号

この発明の課題は、詳しくは、周囲環境とロボットの可動部、特に、それによって保持された部品又は工具との間の相互作用を精確に制御する方法を提案することによって、前述した欠点を解消することである。

この観点において、この発明は、多軸ロボットの可動部の動きをパスに沿って制御する方法に関し、その方法では、以下のことで構成される措置を規定するものである。
・動き指令が、少なくともパスの形状と力目標に関する情報を含み、これらの動き指令をパス生成器に提供すること、
・可動部分がその周囲環境に及ぼす力の少なくとも一つの成分を表す外力信号を計算すること、
・所定のサンプリング周波数で動作させて、外力信号をパス生成器に提供すること、
・外力のパスの接線への射影と目標のこの接線への射影との間の差分を最小化する手法で、パス生成器を用いて、並びに所定のサンプリング周波数で、パスに沿った動き目標を計算すること、
・ロボットの少なくとも一つの軸を動き目標に合致して動かすことができるサーボ制御手段に、動き目標を伝えること。

このようにして、接線力のサーボ制御により、パスの形状に沿うようにする一方、そのパスに可動部分の速度を適合させることが可能となる。

有利な形態では、多軸ロボットの可動部分の変位を制御する方法は、以下の特徴の中の一つ以上を組み入れることができる。
・ロボットの少なくとも一つのアクチュエータに流れる電流を表す情報から、外力信号を計算すること、
・当該の外力信号を計算する際に、当該のロボットの動的モデルを使用することで構成される措置を備えること、
・当該の動き目標を計算する際に、当該の目標が当該の限界値に準拠することを考慮するために、少なくとも一つの速度限界値と少なくとも一つの加速度限界値の両方又は一方を当該のパス生成器に供給することで構成される措置を備えること。

また、この発明は、前述した方法を実施可能するための装置、詳しくは、以下のものを備えた装置を規定する。
・少なくともパスの形状と力目標に関する情報を含む動き指令の関数として、動き目標を計算するのに適したパス生成器、
・当該のパス生成器は、外力のパスの接線への射影と力目標のこの接線への射影との間の差分を最小化する手法により、所定のサンプリング周波数で当該のパスに沿った当該の動き目標を計算するのに適しており、当該のロボットの少なくとも一つの軸を動かすことができるサーボ制御手段に当該の動き目標を伝える場合に、当該の可動部分がその周囲環境に及ぼす力の少なくとも一つの成分を表す外力信号を生成するとともに、この信号を所定のサンプリング周波数で当該のパス生成器に伝えるのに適した力の推定器。

有利には、この装置は、少なくともパスの形状と力目標を規定することができる動き指令を含むプログラムを実行するのに適したプログラムインタープリター手段を有する。

この発明の原理にもとづく方法の実施に関して、単に例を挙げるだけであり、添付の図面と関連して記載した、以下における記述を参照すると、この発明をより良く理解することができるとともに、その他の利点がより明確となる。

図１に図示したロボットＲは、六関節型多軸ロボットである。このロボットは、六つの自由度に関して動くことができる。ロボットＲは、デカルト座標モードで制御することができ、そのモードでは、自由度を、三つの軸Ｘ，Ｙ，Ｚの方向に沿って変換される三つの自由度と、それらの軸の周りの回転Ｒ_x，Ｒ_y，Ｒ_zの三つの自由度とすることができる。このロボットは、関節モードでも制御することでき、そのモードでは、自由度を、関節の六つの蝶番運動軸Ｘ₁，Ｘ₂，Ｘ₃，Ｘ₄，Ｘ₅，Ｘ₆の周りの六つの回転Ｒ₁，Ｒ₂，Ｒ₃，Ｒ₄，Ｒ₅，Ｒ₆とすることができる。

このロボットは、訓練と実践段階の間、その操作を制御する制御ユニットＵと繋がっている。

訓練段階の間、手動制御コンソールＢを使用することも可能である。

図示した例では、ロボットＲは、パスＴに沿って移動させることができるフライスなどの工具Ｏを保持している。

図２でより詳しく見ることができる通り、ロボットＲによって、切削する部品のエッジＢ₁に沿って工具Ｏを移動させることを考察する。パスＴは、当該のエッジに対して、ほぼ平行である。工具ＯがエッジＢ₁に対して及ぼす力を符号Ｆで表示している。この力のパスＴの接線に沿った成分を、符号Ｆ_Tで表示しており、この力のパスＴに垂直な成分を、符号Ｆ_Nで表示している。

この発明では、接線成分Ｆ_Tの値を、工具Ｏの変位を制御する方法で使用する。

以下の記述において、符号Ｆを広く使用しており、ロボットアームの関節におけるトルクのベクトル、又はデカルト座標系による力Ｆ_x，Ｆ_y，Ｆ_zとデカルト座標系によるトルクＭ_x，Ｍ_y，Ｍ_zを含む外力のトルソールを表すものとする。

図３に図示した通り、プログラミング環境１００により、システムの利用者は、ロボットが所望の操作を行うように、ロボットをプログラミングすることが可能である。これらの操作は、プログラムによって記述される。このプログラミング環境により、ロボットが到達する連続的な位置を訓練することが可能であり、動きのシーケンスを記述するためにプログラムを書くことが可能であり、プログラムをデバッグすることが可能であり、プログラムを起動し、停止することなどが可能である。

プログラムを実行するために、プログラミング環境は、プログラムインタープリター２００を使用している。このインタープリターは、ロボットにタスクを行わせるためのプログラムを実行する。特に、実行するプログラムは、動き指令として知られている命令３００を含む。これらの命令は、ロボットが実行すべき動きの形式（直線、円等）とそれらを特徴付ける様々なパラメータ（目標点、速度、力等）を記述するものである。

動き指令３００は、パス生成器４００に転送される。パス生成器は、それらの指令を実行する役割を果たす、即ち、動きの形式と動きのパラメータの関数として、定期的に（典型的には、４ミリ秒（ｍｓ）毎に一回）動き目標５００を生成する役割を果たす。これらの動き目標５００は、各瞬間にロボットが従うべきパスを表す。サーボ制御による工業ロボット６００は、サーボ制御システムと共に、六つの軸を持つ工業用アームを有する。サーボ制御システムは、ロボットによって実際に追従されるパスが、動き目標５００で定義されるパスに出来る限り近い形で従うように、ロボットの（各軸に一つの）六つのモータを制御する形態に構成される。

一定の用途では、工業ロボット６００の端末部材は、その周囲環境、例えば、切削する部品と互いに機械的に作用し合う。外力推定器７００は、ロボットがその周囲環境に及ぼす力の値と方向を示す、外力８００として表される信号を得るための手段を構成するものである。この信号は、リアルタイムで（典型的には、４ミリ秒（ｍｓ）毎に一回）パス生成器４００に転送される。このパス生成器は、この戻りの情報を使用して、動き目標を修正し、互いに作用する力を調節する。

プログラミング環境は、ロボットが採るべき様々な位置を訓練することを可能とするために、ロボットを相互作用モードで移動させることができる制御コンソールを有する。このコンソールは、プログラムを記述し、デバッグするためのプログラミングインタフェースとしての役割も果たす。そのようなプログラムを記述するのに使用する言語は、テキスト言語であるが、この発明の文脈においては、その他の形式の言語（例えば、グラフィックス言語）を使用することができる。

ここで使用する言語は、ロボット用プログラミング言語で一般的な種類の命令を含み、これらの命令は、異なる形式の動きを規定し、動きの速度及び加速度を制御するとともに、どのように動きを配列するかも制御する役割を果たす。

例として、この発明に関係する二つの異なる命令の記述を以下に示す。

命令：ＭＯＶＥＪＦ（位置、工具、パラメータ、力）
この命令は、ロボットが、それが保持し、パラメータ「工具」によって定義された工具を、先行する動きの最終的な位置からパラメータ「位置」によって定義される位置に移動するように規定する。この移動は、関節モードで行われる、即ち、移動している時間において、各軸の変位の間の比率は一定である。パラメータ「パラメータ」は、移動の間に超えてはならない速度及び加速度を規定する。そのパラメータは、移動が、次の移動に続くか否かも規定する。パラメータ「力」は、移動の間にパスの接線方向の力に関する所望の値を規定する。「パラメータ」で定義された速度及び加速度の限界に到達しない限りにおいて、ロボットは、パラメータ「力」によって規定される値に出来る限り近い値に接線力を維持して、その変位をパスに沿って適合させるべきである。

命令：ＭＯＶＥＬＦ（位置、工具、パラメータ、力）
この命令も同様である。唯一の相違点は、移動を直線で実行する必要があることである。

円形であるか、或いはスプライン等で規定されるパスを定義するのに、同様の命令を容易に想像することができる。

この発明では、これらの命令が、第一に、ロボットが従うべきパスの形状を定義し、第二に、ロボットが、その移動の間に及ぼすべき接線力の値を定義するものであることが重要である。

プログラムインタープリター２００は、開発環境を用いて生成されたプログラムを実行する役割を果たす。動き指令を実行するために、利用者が規定するパラメータの有効性を検証し、次に動き指令３００をパス生成器４００に送る。動き指令３００は、プログラム内で規定される値から得られる情報を含み、即ち、それらは、以下の通りである。
・３１０−パスの形状の定義：フラグは、パスのタイプ（関節又は直線）を示す。このタイプが「関節」であれば、始点ｑ０と目標点ｑ１の関節座標が与えられる。このタイプが「直線」であれば、始点ｘ０と目標点ｘ１のデカルト座標が与えられる。
・３２０−接線力目標：これは、ロボットの端部がその周囲環境に及ぼすデカルト座標系による力のノルムに関する所望の値ｄｅｓである。
・３３０−速度限界：ロボットの端部でのデカルト座標系による速度のノルムに関する限界値ｖｍａｘである。
・３４０−加速度限界：ロボットの端部でのデカルト座標系による加速度のノルムに関する限界値ａｍａｘである。

ここに述べる実施形態では、移動コマンド５００は、六つのモータの角度位置ｑｄｅｓによって構成される。これらは、パス生成器によって、４ｍｓ毎に一回更新される。六つの位置目標ｑｄｅｓ１，ｑｄｅｓ２，．．．，ｑｄｅｓ６の各々は、図４に図示された通り、モータ６０１，６０２，．．．，６０６の各々に対応するサーボ制御システムに送られる。

各モータは、位置及び速度センサーを備えている。各モータのサーボ制御は、比例積分微分（ＰＩＤ）型である。図５において、ｑｄｅｓ，ｊは、軸ｊ上での位置目標であり、ｑｍｅｓ，ｊは、測定した位置、Ｋｐ，ｊ，Ｋｉ，ｊ，Ｋｄ，ｊは、ＰＩＤの各ゲインであり、Ｉｄｅｓ，ｊは電流目標、Ｉｍｅｓ，ｊは測定した電流、ｐは、ラプラス変数である。

この発明の文脈において、その他の位置サーボ制御技術を使用することもできる。例えば、デカップリング及び線形化技術によって、サーボ制御を行うことができる。このような環境下では、もはや軸毎にサーボ制御を実行することはできない。この例では、動き目標５００は、各モータに関する所望の角度位置である。デカルト座標で表した、端末部材に関する所望の位置及び速度を使用することも可能である。連続的な位置増分又は各瞬間における各モータに関する所望の速度を動き目標とすることもできる。この発明で重要なポイントは、パス生成器４００が、ロボットによって追従されるべきパスを計算するために、リアルタイムで動作することと、ロボットがそのパスに出来る限り近い形で追従することをサーボ制御システムが保証することである。

この発明の文脈において、ロボットが外部に及ぼす力を推定するためには、多くの手段が使用可能である。最も簡単なものは、ロボットの端部に力センサーを配置することで構成される。そのような環境下では、外力８００は、デカルト座標で表される。ロボットの様々な位置で力センサー又はトルクセンサーを使用することも可能である。

図６に図示した提案する実施形態では、位置サーボ制御変数を用いて、外力を推定することが可能である解決法が望ましい。

使用する方法は、各モータに流れる電流Ｉを表す値７０１を必要とする。この例では、それは、サーボ制御システムで使用される測定した電流Ｉｍｅｓに等しい。

Ｉ＝Ｉｍｅｓ＝［Ｉｍｅｓ，１Ｉｍｅｓ，２．．．Ｉｍｅｓ，６］
同様に、以下の電流制御信号又はその他の測定した、或いは計算した、モータに流れる電流を示す変数を使用することが可能である。

Ｉ＝Ｉｄｅｓ＝［Ｉｄｅｓ，１Ｉｄｅｓ，２．．．Ｉｄｅｓ，６］
使用するモータは、ブラシレスモータである。このタイプのモータに関する最も簡単なモデル７０２は、以下の通り、伝えるトルクＣｍ７０３が、モータを流れる電流に比例することを考慮することで構成される。

Ｃｍ＝Ｋｔ．Ｉ
この表現では、ＩとＣｍは、各軸に関係する情報を含むベクトルであり、Ｋｔは、対角行列である。より複雑なモデルを７０２として使用することができる。

更に、使用する方法は、ロボット７１１の動き、例えば、それぞれベクトルｑ，ｄｑ／ｄｔ，ｄ²ｑ／ｄｔ²で表される、各モータの位置、速度、加速度を示す信号を必要としている。この例では、これらの値は、サーボ制御システムで使用されるセンサーの測定値又はその微分値から導き出される。

ｑ＝ｑｍｅｓ
ｄｑ／ｄｔ＝ｄ（ｑｍｅｓ）／ｄｔ
ｄ²ｑ／ｄｔ²＝ｄ²（ｑｍｅｓ）／ｄｔ²
位置目標及びそれらの微分値を使用することも可能である：ｑｄｅ，ｄ（ｑｄｅｓ）／ｄｔ，ｄ²（ｑｍｅｓ）／ｄｔ²。

この情報と、例えば、ニュートン・オイラー方程式を用いて構築される、ロボットアームの動的モデル７１２にもとづき、ロボットが、外力７１３を及ぼさないものと仮定して、ロボットの各モータが理論的に生成すべきトルクを表すベクトルＣｄを計算する。

符号７２０において、差分を取って、ロボットがその周囲環境８００に及ぼす力に対応するモータトルクの推定値を構成するベクトルＣｅｘｔ８００を導き出す。

Ｃｅｘｔ＝Ｃｍ−Ｃｄ
図７は、どのようにして、パス生成器が使用するアルゴリズムを四つの部分に分割することができるのかを図示している。

接線力目標３１０、外力８００及び曲線の形状３２０を出発点として、符号４１０でスカラー誤差変数４２０を計算する。速度及び加速度限界３３０と３４０は、符号４３０において、曲線３２０上に射影される。射影された速度及び加速度限界４７０にもとづき、並びに誤差変数４２０にももとづき、調節器４３０は、曲線４４０に沿った所望の動きを計算する。次に、符号４５０において、定義された曲線の形状３２０と定義された曲線に沿った動き４４０から、ロボットの動き目標５００を計算する。

これらの四つの部分の各々は、以下の通り記述される。
Ａ）誤差変数の計算（４１０〜４２０）：直線での動き
直線での動きに関する誤差変数４２０の計算は、以下の通り記述される。以下に示す計算は、力目標３１０と外力８００の間の差分の曲線３２０上への射影を計算することで構成される。

直線での動きの間において、デカルト座標系によるロボットの位置ｘ（ベクトル）は、以下の通り、パラメータ表示形式で記述することができる。

ｘ＝ｘ０＋ｓ．（ｘ１−ｘ０）
ここで、ｘ０は、動きが始まるデカルト座標系による位置で、ｘ１は、デカルト座標系による目標位置であり、ｓは、０〜１の範囲に有るスカラーパラメータである。上記の式は、以下の形式に書き換えることができる。

（式１）ｘ＝ｆ（ｓ）
この式を微分すると、以下の式が得られる。

（式２）ｖ＝ｄｘ／ｄｔ＝ｄｆ／ｄｓ．ｄｓ／ｄｔ
この式では、ｄｆ／ｄｓは、曲線の接線方向のデカルト座標系によるベクトルである。このデカルト座標系によるベクトルは、以下の通り、ユニタリーであり、曲線の接線方向を向いている。

ｕ＝（ｄｆ／ｄｓ）／‖ｄｆ／ｄｓ‖
この例では、力目標３００は、デカルト座標系による接線力のノルムである。このようにして、デカルト座標系による所望の力ベクトルが、以下の通り与えられる。

（式３）Ｆｄｅｓ＝ｆｄｅｓ．ｕ
外力８００は、関節トルクのベクトルＣｅｘｔで与えられる。この値は、以下の式を用いて、デカルト座標に変換することができる。

（式４）Ｆｅｘｔ＝Ｊ^-t．Ｃｅｘｔ
ここで、Ｊ^-tは、ロボットの運動行列の転置逆行列である。

符号ｅｐｓで記述される選択したコマンド変数４２０は、以下の通り、パスの接線上に射影された、所望の接線力と外力間の差分である。

ｅｐｓ＝ｕ^t．（Ｆｄｅｓ−Ｆｅｘｔ）
この計算は、すべての点において接線を定義することができることを意味する、微分可能である場合、使用する関数ｆ（円、スプライン等）に関係無く有効である。

この計算は、力目標３００が、デカルト座標系による力ベクトルのノルムであり、外力８００が、関節の座標で与えられる特殊な場合に対して実行される。相応の座標変換公式を使用すること、例えば、式３と式４を置換することによって、如何なる座標系で与えられる状況も取り扱うことが可能である。
Ｂ）誤差変数の計算（４１０〜４２０）：関節の動き
関節の動きを取り扱う場合、以下の通り、ロボットの関節の位置ｑをパラメータ形式で記述することができる。

ｑ＝ｑ０＋ｓ．（ｑ１−ｑ０）
ここで、ｑ０は、動きが開始する関節位置で、ｑ１は、目標とする関節位置であり、ｓは、０〜１の範囲に有るスカラーパラメータである。上記の式は、以下の形式に書き換えることができる。

ｑ＝ｇ（ｓ）
それに対応するデカルト座標系による位置は、以下の通り与えられる。

ｘ＝ｋ（ｑ（ｓ））
ここで、ｋは、ロボットの直接的な幾何学モデルである。

このようにして、関数ｆを選択することによって、デカルト座標系による動きと同様に、以下の通り、関節の動きを扱うことができる。

ｆ（ｓ）＝ｋ（ｑ（ｓ））
Ｃ）曲線に沿った速度及び加速度限界の射影（４６０）
この例では、速度限界は、デカルト座標で与えられる。言い換えると、ロボットの速度ｖのノルムが所定の速度ｖａｍｘよりも小さく、デカルト座標系による加速度ｄｖ／ｄｔのノルムが、所定の加速度ａｍａｘよりも小さいことが望ましい。

以下の値が定義される。

（式５）（ｄｓ／ｄｔ）_max＝ｍｉｎ（ｖｍａｘ／‖ｄｆ／ｄｓ‖）；．ｓｑｒｔ（ａｍａｘ／‖ｄｆ／ｄｓ‖‖）
（式６）（ｄ²ｓ／ｄｔ²）_max＝（ａｍａｘ−‖ｄｆ／ｄｓ‖．（ｄｓ／ｄｔ）_max）／‖ｄ²ｆ／ｄｓ²‖）
ここで、ｃは、定数であり、０＜ｃ＜１、ｓｑｒｔ（）は、平方根関数を示す。

以下の仮定を適用する。

｜ｄｓ／ｄｔ｜≦（ｄｓ／ｄｔ）_max
｜ｄ²ｓ／ｄｔ²｜≦（ｄ²ｓ／ｄｔ²）_max
次に、式２から、以下の通りとなる。

‖ｖ‖＝‖ｄｆ／ｄｓ．ｄｓ／ｄｔ‖
＝‖ｄｆ／ｄｓ‖．‖ｄｓ／ｄｔ‖
≦‖ｄｆ／ｄｓ‖．（ｄｓ／ｄｔ）_max
従って、式５から、以下の通りとなる。

‖ｖ‖≦ｖｍａｘ
更に、式２を微分して、以下の式が得られる。

ａ＝ｄｆ／ｄｓ．ｄ²／ｄｔ²＋ｄ²ｆ／ｄｓ²．（ｄｓ／ｄｔ）²
そのため、以下の通りとなる。

‖ａ‖＝‖ｄｆ／ｄｓ．ｄ²ｓ／ｄｔ²＋ｄ²ｆ／ｄｓ²．（ｄｓ／ｄｔ）²‖
≦‖ｄｆ／ｄｓ‖．‖ｄ²ｓ／ｄｔ²‖＋‖ｄ²ｆ／ｄｓ²‖．｜ｄｓ／ｄｔ｜²
≦‖ｄｆ／ｄｓ‖．（ｄ²ｓ／ｄｔ²）_max＋‖ｄ²ｆ／ｄｓ²‖．（ｄｓ／ｄｔ）_max ²
式５と式６の定義を用いて、以下の式が得られる。

‖ａ‖≦ａｍａｘ
このようにして、（ｄｓ／ｄｔ）_maxと（ｄ²ｓ／ｄｔ²）_maxの値は、ｓの微分の限界を定義し、このことは、デカルト座標系による速度及び加速度の限界を満足することを保証するものである。従って、デカルト座標系による速度及び加速度の限界のｓの座標への射影が存在すると考えることができる。

同様に、前の計算で関数ｆの代わりに、関数ｈ（ｓ）＝ｋ^-1（ｆ（ｓ））を使用することによって、関節の速度と加速度の両方又は一方の限界を得ることが可能である。
Ｄ）調節器（４３０）
調節器４３０の関数は、誤差変数を出来る限り零４２０に近い形で維持するように、曲線４４０に沿った動きを生成する。

使用する調節器は、積分型である。「従来の」積分型調節器は、以下の式で記述することができる。

ここで、ｓｄｅｓは、式１のパラメータｓの単位で記述した、ロボットの曲線に沿った所望の位置であり、Ｋは、定数である。これは、以下のブロック接続図で表される。

ここで、ｐは、ラプラス変数を示す。

この接続図は、以下のものと同等である。

速度及び加速度の限界を含めるために、この接続図は、以下の通り修正される。

「ｃａｌｃ（ｄｓ／ｄｔ）_max」と「ｃａｌｃ（ｄ²ｓ／ｄｔ²）_max」のブロックは、式５と式６で与えられるｓｄｅｓの関数として射影された速度及び加速度の限界を計算する（ｄｆ／ｄｓ及びｄ²ｆ／ｄｓ²は、ｓ＝ｓｄｅｓに関して評価される）。ブロックｓａｔＶｅｌは、（ｄｓ／ｄｔ）_maxの関数として、入力速度ｚを飽和させる。言い換えると、以下の通りとなる。

同様に、ｓａｔＡｃｃｅｌは、以下の通り、（ｄ²ｓ／ｄｔ²）_maxの関数として、加速度ｕを飽和させる。

すべての環境下において、この枠組みは、以下の式を保証する。

｜ｄ（ｓｄｅｓ）／ｄｔ｜＜（ｄｓ／ｄｔ）_max
これは、パスに沿ったデカルト座標系による所望の速度のノルムが、ｖｍａｘより小さいことを保証するものである。

最大速度に達しない場合、
ｚ＝ｄ（ｓｄｅｓ）／ｄｔ
このため、
ｗ＝ｄ²（ｓｄｅｓ）／ｄｔ²
並びに、
｜ｄ²（ｓｄｅｓ）／ｄｔ²｜＜（ｄ²ｓ／ｄｔ²）_max
これは、デカルト座標系による加速度のノルムが、ａｍａｘより小さいことを保証するものである。

一般的な意味において、ここに記述したアルゴリズムは、デカルト座標系による速度の限界が、常に満たされることと、デカルト座標系による最大速度に達しない限り、加速度の限界が満たされることを保証する。直線に関して、速度の限界に達した場合、加速度は、零となり、そのため加速度の限界が満たされる。
Ｅ）動き目標（５００）の計算（４３０）
この例では、動き目標５００は、ロボットの関節の座標ｑｄｅｓである。曲線に沿った所望の位置ｓｄｅｓを出発点として、式１で与えられる曲線に関する助変数方程式を用いて、対応するデカルト座標系による位置目標を計算することが可能である。

ｘｄｅｓ＝ｆ（ｓｄｅｓ）
これにより、関節の座標は、ｋ^-1で記述されるロボット逆幾何モデルを使用して得られる。

ｑｄｅｓ＝ｋ^-1（ｘｄｅｓ）
動き目標が速度であれば、対応する速度への座標変換を使用することが可能である（式２とロボット運動行列の逆行列）。

この発明による方法を実施し、パスに沿って切削工具を動かす多軸ロボットの原理を示す図図１のロボットの可動部分において力を分解する原理を示す図この発明の一般的な操作を示すブロック接続図位置目標の各モータに対応するサーボ制御システムへの転送を示すブロック接続図各モータ用サーボ制御を示すブロック接続図外力を推定するために使用する方法を示す図動き目標を生成するためのパス生成器が使用するアルゴリズムを示す図

Claims

多軸ロボットの可動部分の変位をパスに沿って制御する方法において、
・動き指令（３００）が、少なくともパス（３２０）の形状と力目標（３１０）に関する情報を含み、これらの動き指令をパス生成器（４００）に提供することと、
・可動部分（Ｏ）がその周囲環境に及ぼす力（Ｆ）の少なくとも一つの成分を表す外力信号（８００）を計算することと、
・所定のサンプリング周波数で動作させて、当該の外力信号（８００）を当該のパス生成器（４００）に提供することと、
・当該の外力の当該のパスの接線（Ｔ）への射影（Ｆ_T）と当該の目標のこの接線への射影との間の差分を最小化する手法で、当該のパス生成器（４００）を用いて、並びに所定のサンプリング周波数で、当該のパス（３２０）に沿った動き目標（５００）を計算することと、
・当該のロボット（６００）の少なくとも一つの軸を当該の動き目標（５００）に合致して動かすことができるサーボ制御手段（６０１〜６０６）に、当該の動き目標（５００）を伝えることと、
で構成される措置を有することを特徴とする方法。
当該のロボット（６００）の少なくとも一つのアクチュエータ（６０１〜６０６）に流れる電流を表す情報から、当該の外力信号（８００）を計算することを特徴とする請求項１に記載の方法。
当該の外力信号（８００）を計算する際に、当該のロボット（６００）の動的モデル（７１２）を使用することで構成される措置を含むことを特徴とする請求項１又は２に記載の方法。
当該の動き目標（５００）を計算する際に、この目標が、少なくとも一つの速度限界値（３３０）と少なくとも一つの加速度限界値（３４０）の両方又は一方に準拠するものであることを考慮するために、これらの限界値を当該のパス生成器（４００）に供給することで構成される措置を含むことを特徴とする請求項１から３までのいずれか一つに記載の方法。
多軸ロボットの可動部分の変位をパスに沿って制御するための装置において、
・少なくともパス（３２０）の形状と力目標（３１０）に関する情報を含む動き指令（３００）の関数として、動き目標（５００）を計算するのに適したパス生成器（４００）と、
・当該のパス生成器は、外力のパスの接線（Ｔ）への射影（Ｆ_T）と当該の力目標のこの接線への射影との間の差分を最小化する手法により、所定のサンプリング周波数でパス（３２０）に沿った当該の動き目標（５００）を計算するのに適しており、当該のロボット（６００）の少なくとも一つの軸を動かすことができるサーボ制御手段（６０１〜６０６）に当該の動き目標（５００）を伝える場合に、当該の可動部分（Ｏ）がその周囲環境に及ぼす力（Ｆ）の少なくとも一つの成分を表す外力信号（８００）を生成するとともに、この信号を所定のサンプリング周波数で当該のパス生成器に伝えるのに適した力の推定器（７００）と、
を有することを特徴とする装置。
少なくともパス（３２０）の形状と力目標（３１０）を規定することが可能な動き指令（３００）を含むプログラムを実行するのに適したプログラムインタープリター手段（２００）を有することを特徴とする請求項５に記載の装置。