JP2020113214A

JP2020113214A - 制御装置

Info

Publication number: JP2020113214A
Application number: JP2019005583A
Authority: JP
Inventors: 亮菊植; Ryo Kikuue
Original assignee: Hiroshima University NUC
Current assignee: Hiroshima University NUC
Priority date: 2019-01-16
Filing date: 2019-01-16
Publication date: 2020-07-27
Anticipated expiration: 2039-01-16
Also published as: JP7216405B2

Abstract

【課題】外力によってロボットが動かされてモータの発生力が飽和する場合であっても、適切にロボットを制御することができる位置制御またはアドミッタンス制御を用いた制御装置を提供する。【解決手段】制御装置１は、仮想物体の動特性と制御則とから、次時刻ステップの飽和前位置ベクトルと飽和前速度ベクトルとを算出し、位置センサ１９の測定値と飽和前位置ベクトルと制御則とから操作量を設定する。操作量が所定の範囲内の場合、操作量を駆動装置２０ａに出力し、飽和前速度ベクトルを仮想物体の次時刻ステップの位置ベクトルとし、飽和前速度ベクトルを仮想物体の次時刻ステップの速度ベクトルとする。操作量が所定の範囲外の場合、修正した操作量を駆動装置に出力し、修正した飽和前位置ベクトルを仮想物体の次時刻ステップの位置ベクトルとし、飽和前速度ベクトルとゼロベクトルとの中間の値を仮想物体の次時刻ステップの速度ベクトルとする。【選択図】図１

Description

本発明は、制御装置に関する。

主に人と協調して作業するロボットの制御に関して、ロボットの各関節に取り付けられた関節角度センサ情報を用いた位置制御による制御方法、ロボットの手先（エンドエフェクタ）に取り付けられた力センサの情報を用いた機械インピーダンス制御による制御方法等が開発されている。

機械インピーダンス制御のうち位置制御をベースとするアドミッタンス制御は、力センサの情報に基づいて運動目標値を求め、その目標に対して位置又は速度制御を行うものであり、産業用ロボット等の位置制御ベースのロボットに適用しやすいという利点を有する。このことから、アドミッタンス制御を応用した種々の制御装置及び制御方法が開発されている。アドミッタンス制御を行う制御器は、実際のロボットに比べて単純化された動特性を有する仮想物体（proxy又はプロクシともいう）を制御器の中で想定し、ロボットに取り付けられた力センサで測定された力の情報によって仮想物体の運動をシミュレートする。そして、ロボットは、仮想物体の運動を追跡するように位置制御される（例えば、非特許文献１）。

また、位置制御においても、所定の動特性に基づいて制御対象を目標位置へ収束させるように制御を行うスライディングモード制御とよばれる手法がある。この場合の所定の動特性は、位置または速度を変数とする単純な微分方程式で記述される（例えば、特許文献１〜３）。

特開２００７−１０２７４８号公報特開２００８−９７１５２号公報特開２０１４−１５７５４８号公報

Ryo Kikuuwe, "Torque-Bounded Admittance Control", 日本ロボット学会学術講演会講演論文集, 2G2-03, 2017年9月

特許文献３および非特許文献１の制御装置では、ロボットを駆動するモータの発生力が予め定められた限界値に達することにより飽和した場合、仮想物体のモータの発生力を限界値にセットして、仮想物体の計算上の位置を算出する。そして、ロボットに仮想物体の運動を追跡させる。これにより、モータの発生力が飽和しない場合には、位置制御の性能を担保するとともに、モータの発生力が飽和する場合であっても、ロボットの想定外の動作を抑制している。

しかしながら、特許文献３および非特許文献１の制御装置では、外力によってロボットが動かされてモータの発生力が飽和すると、仮想物体が本来の速度（飽和前速度）よりも大きい速度を持つ場合がある。この場合、仮想物体が飽和前速度よりも大きい速度を持った後、モータの発生力が非飽和状態になると、仮想物体がその速度を慣性によって保持することとなる。すなわち、モータの発生力が飽和することによって、ロボットの運動エネルギーが不要に増大し、ロボットが意図しない動作をする可能性がある。

本発明は、上述の事情に鑑みてなされたものであり、外力によってロボットが動かされてモータの発生力が飽和する場合であっても、適切にロボットを制御することができる位置制御またはアドミッタンス制御を用いた制御装置を提供することを目的とする。

上記目的を達成するために、この発明に係る制御装置は、
制御対象の位置を検出する位置センサと、
前記制御対象を動作させる駆動装置の操作量を決定する演算部と、を備え、
前記演算部は、
所定の動特性を有する仮想物体と制御則とを保持しており、
前記仮想物体の運動をシミュレートして次時刻ステップの飽和前位置ベクトルと飽和前速度ベクトルとを算出し、
前記位置センサの測定値と、算出された前記飽和前位置ベクトルと、前記制御則とに基づいて、前記操作量を設定するとともに、
前記操作量が予め設定された所定の範囲内にある場合には、
前記操作量を前記駆動装置に出力し、
前記飽和前位置ベクトルを、前記仮想物体の次時刻ステップの位置ベクトルとして設定し、
前記飽和前速度ベクトルを、前記仮想物体の次時刻ステップの速度ベクトルとして設定し、
前記操作量が予め設定された所定の範囲外にある場合には、
前記操作量が所定の範囲内の値となるように、前記飽和前位置ベクトル及び前記操作量を修正し、
修正した前記操作量を前記駆動装置に出力し、
修正した前記飽和前位置ベクトルを、前記仮想物体の次時刻ステップの位置ベクトルとして設定し、
前記飽和前速度ベクトルとゼロベクトルとの中間の値を、前記仮想物体の次時刻ステップの速度ベクトルとして設定する。

また、前記仮想物体の前記次時刻ステップの速度ベクトルｖ_ｎは、以下の式
ｖ_ｎ＝proj_｛co（０，ｖ_ａ）｝（ｑ_ｎ−ｑ）／Ｔ
ただし、projは射影、coは凸包、ｖ_ａは前記飽和前速度ベクトル、ｑ_ｎは前記仮想物体の次時刻ステップの位置ベクトル、ｑ_ｐは前記仮想物体の位置ベクトル，Ｔはステップ時間
を用いて算出される、
こととしてもよい。

また、前記制御則は、前記仮想物体の加速度フィードフォワードを含み、
前記仮想物体の加速度は、前記仮想物体の飽和前速度と前時刻ステップの速度との差をステップ時間で除したものであり、
前記加速度フィードフォワードは前記操作量が飽和しない範囲に制限される、
こととしてもよい。

また、前記仮想物体は、目標位置へ収束するために設定された所定の動特性を有する、
こととしてもよい。

また、前記仮想物体は、目標速度へ収束するために設定された所定の動特性を有する、
こととしてもよい。

また、前記制御対象は、制御対象と外部環境である物体とが接触する力を検出する力センサを備え、
前記演算部に保持される前記仮想物体の動特性は、力を受けて運動する質点または剛体の動特性であり、
前記演算部は、
前記制御対象に加える目標力と前記力センサの測定値とを前記仮想物体に与えた場合の運動をシミュレートして、次時刻ステップの飽和前位置ベクトルと飽和前速度ベクトルとを算出する、
こととしてもよい。

また、前記動特性は、クーロン摩擦特性を含む、
こととしてもよい。

また、前記位置センサは複数の角度センサであり、
前記駆動装置は複数のモータを備え、
前記仮想物体の位置ベクトルと速度ベクトルとは、多次元のベクトル値である、
こととしてもよい。

また、前記仮想物体の位置ベクトルは、
３次元の並進位置ベクトルと、前記仮想物体の姿勢を表現する３個から１６個の数値の集合とから構成され、
前記仮想物体の速度ベクトルは、
３次元の並進速度ベクトルと、３次元の角速度ベクトルとから構成される、
こととしてもよい。

本発明の制御装置によれば、次時刻ステップに持ち越される仮想物体の速度を、仮想物体の飽和前速度ベクトルとゼロベクトルの中間のベクトルとすることにより、外力によって行われた仕事が仮想物体の運動エネルギーとして保持されることを抑制できるので、より安全にロボットを制御することが可能である。

本発明の実施の形態に係る制御装置の構成を示す機能ブロック図である。実施の形態に係る制御部の制御処理の概念を示すブロック図である。（ａ）が実施の形態に係る射影を示すグラフ、（ｂ）が実施の形態に係る不感帯関数を示すグラフ、（ｃ）が実施の形態に係る法錐を示すグラフである。実施の形態に係るロボットの例を示す図である。実施の形態１に係る制御装置による制御結果の例を示すグラフであり、（ａ）が従来の制御器、（ｂ）が式（２８）に示される修正を加えた制御器、（ｃ）が（ｂ）においてＭ＝０とした制御器、（ｄ）が（ｂ）に加速度フィードフォワードの修正を追加した制御器、（ｅ）が（ｄ）にクーロン摩擦力を追加した制御器の場合である。ロボットのエンドエフェクタを物体に押しつけた状態を示す図である。（ａ）がロボットのエンドエフェクタを物体に押しつけた場合の力センサの測定値を示すグラフであり、（ｂ）が（ａ）においてＭ＝０とした場合のグラフである。（ａ）が実施の形態１に係る制御ステップを示す概念図であり、（ｂ）が多自由度マニピュレータに実装した実施の形態２に係る制御ステップを示す概念図である。

以下、図を参照しつつ、本発明の実施の形態に係る制御装置について説明する。

（実施の形態１）
本実施の形態では、外部環境と接触する状況で動作するロボットについて、ロボットと外部環境との間に生じる力に基づいて、制御部内の仮想物体を動作させるとともに、仮想物体の位置にロボットの位置を追従させる制御装置を例として説明する。

本実施の形態に係る制御装置１は、図１に示すように、ロボット２０を制御するものであり、制御ユニット１０、制御ユニット１０に接続された力センサ１８及び位置センサ１９を備える。

ロボット２０は、ロボット２０を動作させる駆動装置として駆動部２０ａを備える。ロボット２０の種類は特に限定されないが、例えば、重量物を搬送する産業用ロボット、リハビリテーション支援ロボット等のモータで駆動される多関節ロボットである。ロボット２０は、制御ユニット１０に接続されている。

制御ユニット１０は、図１のブロック図に示すように、制御部１１、記憶部１２、表示部１３、入力部１４を備える。

制御部１１は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、水晶発振器等から構成されるコンピュータ装置であり、ロボット２０の動作を制御する。制御部１１は、制御部１１のＲＯＭ、記憶部１２等に記憶されている各種動作プログラム及びデータをＲＡＭに読み込んでＣＰＵを動作させることにより、図１に示される制御部１１、すなわちアドミッタンス制御器としての各機能を実現させる。これにより、制御部１１は、演算部１１１及び動作制御部１１２として動作する。

演算部１１１は、力センサ１８の測定値と目標力とに基づいて、仮想物体の位置をシミュレートし、これに基づいて、ロボット２０を動作させるための駆動部２０ａの操作量を算出するアドミッタンス制御器である。

動作制御部１１２は、演算部１１１で算出された操作量に基づいて、ロボット２０の駆動部２０ａを制御し、ロボット２０を動作させる。

記憶部１２は、ハードディスク、フラッシュメモリ等の不揮発性メモリであり、仮想物体の動特性、アドミッタンス制御器の演算アルゴリズム等を記憶する。本実施の形態に係る仮想物体の動特性は、ロボット２０の望ましい動特性を単純化したものであり、力を受けて運動する質点又は剛体の動特性として表される。

表示部１３は、液晶パネル、有機ＥＬ（Electroluminescence）等の表示用デバイスであり、各種設定パラメータ、力センサ１８及び位置センサ１９の測定値等を表示する。本実施の形態に係る表示部１３は、制御ユニット１０に搭載された液晶パネルである。

入力部１４は、ロボット２０を動作させる各種設定パラメータを入力するための入力デバイスである。入力部１４は、キーボード、タッチパネル、マウス等である。

力センサ１８は、ロボット２０のエンドエフェクタが外部環境である物体に接触した際、エンドエフェクタが物体に及ぼす力を検出し、制御ユニット１０の制御部１１へ出力する。

位置センサ１９は、ロボット２０のエンドエフェクタの位置を検出する計測器であり、計測されたエンドエフェクタの位置を制御ユニット１０の制御部１１へ出力する。

以下、本実施の形態に係る制御部１１の制御処理、すなわちロボット２０の駆動部２０ａへ出力する操作量の算出アルゴリズムについて具体的に説明する。本例では、図２に示すように、モータであるの駆動部２０ａのトルク値を算出する制御アルゴリズムを例として説明する。

（１）数学的準備
本実施の形態では、Ａを実数上の閉区間とし、以下の式（１）〜（３）に示す関数を用いる。
ここで、proj_Ａは、集合Ａへの射影、dzn_Ａは集合Ａに対する不感帯関数、Ｎ_Ａ（ｘ）は点ｘにおける集合Ａの法錐である。

集合ＡをＡ＝［Ａ，Ｂ］と書くと、上記式（１）〜（３）は、以下の式（４）〜（６）と書ける。
これらの式（４）〜（６）の関数のグラフを図３（ａ）〜（ｃ）に示す。

また、射影と法錐とは、以下の式（７）の関係を有する。

また、集合Ｂと単一値ｘとの加減算は、以下の式（８）で表される。

式（８）は、集合Ｂが区間［ａ，ｂ］で表されるとき、集合Ｂ＋ｘが区間［ａ＋ｘ，ｂ＋ｘ］となることを示している。

また、本実施の形態では、以下の式（９）に示す集合値符号関数を用いる。

非負のスカラーＦについて、対称閉区間［−Ｆ，Ｆ］の法錐は、以下の関係式（１０）に示すように、符号関数の逆写像となる。

また、符号関数と飽和関数とは、以下の関係式（１１）で表される。
この式（１１）は、式（７）の特殊な場合である。

また、本実施の形態では、集合Ｘの凸包をco（Ｘ）と表記する。２つのスカラーＡ、Ｂについて、co（｛Ａ，Ｂ｝）＝co（｛Ｂ，Ａ｝）＝［min（Ａ，Ｂ），max（Ａ，Ｂ）］が成り立つ。集合Ａ、Ｂの加減算は、以下の式（１２）で表される。

ただし、以下の式（１３）に注意する必要がある。

さらに、２つの集合値関数Φ：Ｒ→２^Ｒ、Ψ：Ｒ→２^Ｒを用いた入れ子式Ψ（Φ（ｘ）＋ｙ）は、以下の式（１４）のように解される。

（２）制御則の連続時間表現
続いて、本実施の形態に係る制御則の一般表現について説明する。制御対象であるロボット２０のエンドエフェクタの計測位置をｑ_ｓ∈Ｒ、仮想物体の位置をｑ_ｘ、駆動部２０ａであるモータの駆動力をτ∈Ｒ、力センサ１８の測定値をｆ、アドミッタンス制御器に与える目標値をｆ_ｄとし、以下の式（１５ａ）〜（１５ｃ）で表されるアドミッタンス制御器を考える。

ここで、ｑ_ｘ∈Ｒは仮想物体のエンドエフェクタの位置であり、式（１５ａ）で表される動特性を持つ。また、Ｍ_ｘは仮想物体の慣性、Ｂ_ｘは仮想物体の粘性であり、制御対象の特性に応じて設定されるパラメータである。式（１５ｂ）、（１５ｃ）は、所望の加速度フィードフォワードを有するＰＩＤ制御器を表している。Ｋ、Ｂ、Ｌはそれぞれ比例ゲイン、微分ゲイン、積分ゲインであり、閉ループシステムの安定性が保たれる範囲で高く設定されることが好ましい。係数Ｍは正の定数であり、制御対象の慣性に近づくように設定される。

本実施の形態に係る駆動部２０ａは、予め定められた所定の範囲である出力限界、すなわち飽和条件を有するので、式（１５）にτ∈Ｆとの制約を課す。ここで、Ｆはゼロを含む実数の閉区間である。

本実施の形態では、上記の制約を加えたアドミッタンス制御器として、以下の式（１６ａ）〜（１６ｃ）を考える。

上記の式（１６ａ）〜（１６ｃ）は、微分代数的包含（differential-algebraic inclusion，ＤＡＩ）と見ることができる。この法錐関数は、集合Ｆの外のτを許容しないので、τ∈Ｆを満たすように仮想物体の加速度（ｑ_ｓの二階微分）が決定される。また、Ｎ_Ｆ（τ）＝０であるので、τがＦの内部にある限り、式（１６ａ）〜（１６ｃ）で表される制御器は、式（１５ａ）〜（１５ｃ）で表される通常のアドミッタンス制御器に相当する。式（１６ａ）のＮ_Ｆ（τ）の項は、図２の代数フィードバックとして表される。

ここで、集合Ｆ＝［−Ｆ，Ｆ］（Ｆは正の定数）の場合、式（１６ａ）〜（１６ｃ）は、以下の式（１７ａ）〜（１７ｃ）と書き換えることができる。

（３）離散時間表現
Ｔをステップ時間（タイムステップの大きさ）、ｋを離散時間インデックスを表す整数として、陰的オイラー法を用いることにより、式（１６ａ）〜（１６ｃ）の制御器は、以下に示す式（１８ａ）〜（１８ｃ）及び式（１９）〜（２４）のように離散化される。

関係式（７）を用いて式（１８ｂ）、（１８ｃ）からｑ_ｘ（ｋ）を消去すると、以下の式（２５）が得られる。

したがって、式（１６ａ）〜（１６ｃ）の制御器は、以下の式（２６ａ）〜（２６ｊ）で表されるアルゴリズムとして離散時間領域で実現することができる。

式（２６ａ）〜（２６ｊ）で表される制御器では、駆動部２０ａの発生力が飽和していない場合、仮想物体は式（１５ａ）で既定されるように動く。このとき、算出された飽和前位置ベクトルｑ^＊ _ｘ（ｋ）、飽和前速度ベクトルｕ^＊ _ｘ（ｋ）は、それぞれ次時刻ステップの位置ｑ_ｘ（ｋ）、速度ｕ_ｘ（ｋ）となる。一方、ロボット２０が外力を受けて運動し、駆動部２０ａの発生力が飽和した場合、仮想物体はロボット２０の動きに追従して速度ｕ_ｘ（ｋ）を得る。いずれの場合も、この速度ｕ_ｘ（ｋ）を使って、次時刻ステップの仮想物体位置ｑ_ｘ（ｋ＋１）が算出される。このことは、駆動部２０ａの発生力が飽和した場合に、外力によって行われた仕事が仮想物体の運動エネルギーとして保持されることを意味している。

このように、外力による仕事が仮想物体の運動エネルギーとして保持されると、ロボット２０が意図されていない動作をする可能性がある。そこで、本実施の形態では、駆動部２０ａの発生力が飽和した場合の仮想物体の動作を改善するため、次時刻ステップに持ち越される仮想物体の速度を調整する。

具体的には、次時刻ステップにおける仮想物体の速度は、飽和前速度ベクトルとゼロベクトルとの中間のベクトルとして設定される。本実施の形態では、次時刻ステップにおける仮想物体の速度は、仮想物体の飽和前速度ベクトルとゼロベクトルとの中間のベクトルのうち飽和後速度ベクトルに最も近いベクトルに決定する。以下、より具体的に説明する。

式（２６ａ）に示されるように、ｕ^＊ _ｘ（ｋ）は、仮想物体の所定の動特性のみによって決定される仮想物体の速度であり、飽和前速度ベクトルと呼ぶ。また、ｕ_ｘ（ｋ）は、飽和前速度ベクトルｕ^＊ _ｘ（ｋ）と飽和効果とによって決定される飽和後の仮想物体の速度であり、飽和後速度ベクトルと呼ぶ。本実施の形態に係る飽和後速度ベクトルｕ_ｘ（ｋ）は、以下の式（２７）を満たすように決定される。

式（２７）により、仮想物体の運動エネルギーの影響は除去されつつ、ｕ^＊ _ｘ（ｋ）の方向はｕ_ｘ（ｋ）で保持される。アルゴリズムとしては、式（２６ｉ）を以下の式（２８）で置き換えることによって実現することができる。

上記の式（２８）に示される修正は、現在の仮想物体の位置ｑ_ｘ（ｋ）には影響しないが、次時刻ステップにおける仮想物体位置ｑ_ｘ（ｋ＋１）に影響を及ぼす仮想物体の速度ｕ_ｘ（ｋ）のみを変更している。

以上説明したように、本実施の形態に係る制御装置１では、次時刻ステップにおける仮想物体の速度ベクトルは、飽和前速度ベクトルとゼロベクトルとの中間のベクトルとして設定するため、外力による仕事が仮想物体に運動エネルギーとして保存されない。

さらに、本実施の形態では、Ｍがゼロでない場合、すなわち、加速度フィードフォワード項が存在する場合の挙動を改善するための修正を加える。以下、具体的に説明する。

式（１６ｃ）の右辺第１項で表される加速度フィードフォワード項は、アドミッタンス制御器の安定性を高めることが知られている。本実施の形態では、駆動部２０ａの発生力に飽和が生じた場合の加速度フィードフォワードを改善する。

具体的には、式（１６ｃ）の右辺第１項を駆動部２０ａの発生力の飽和の影響を受けない量に置き換える。言い換えると、操作量である駆動部２０ａのトルク値が、飽和しない範囲に制限される。すなわち、式（１６ａ）〜（１６ｃ）の制御器を以下の式（２９ａ）〜（２９ｄ）に変形する。

上記の式（２９ｄ）のｑ^＊ _ｘの二階微分は、駆動部２０ａの発生力が飽和しなかった場合の加速度（飽和前加速度）と見ることができる。また、式（２９ｄ）の右辺第１項の投影演算子は、力センサ１８に大きな衝撃力が加えられた場合でも、この項の値が過度に大きくならないようにするためのものである。

式（２６ａ）〜（２６ｊ）のアルゴリズムとその変形において、ｕ^＊ _ｘ（ｋ）は、飽和が生じなかった場合の仮想物体の速度（飽和前速度ベクトル）を表している。したがって、仮想物体の飽和前加速度は、以下の式（３０）のように、仮想物体の飽和前速度と前時刻ステップの速度との差をステップ時間で除することによって得られる。

式（３０）を用いて、式（２９ａ）〜（２９ｄ）は、以下の式（３１ａ）〜（３１ｃ）及び式（３２）〜（３７）のように離散化できる。

また、上式は式（７）を用いて、以下の式（３８ａ）〜（３８ｅ）と書き換えることができる。
したがって、式（２６ａ）〜（２６ｊ）のアルゴリズムにおいて、式（２６ｄ）〜（２６ｈ）を上記の式（３８ａ）〜（３８ｅ）で置き換えることにより、加速度フィードフォワードが改善される。

また、ロボット２０が、力センサ１８に加わる小さな力によってドリフトすることを抑制するため、仮想物体の動特性にクーロン摩擦特性を表す項を含めることができる。これにより、式（１６ａ）〜（１６ｃ）は、以下の式（３９ａ）〜（３９ｃ）に変形される。
ここで、Ｆ_ｘ＞０は、仮想物体が受けるクーロン摩擦力の大きさである。

また、式（３９ａ）〜（３９ｃ）の制御器の離散時間アルゴリズムは、式（２６ａ）を以下の式（４０）、（４１）で置き換えることによって得られる。

また、上述のアルゴリズムは、次時刻ステップに持ち越される仮想物体の速度の修正及び加速度フィードフォワードの修正と組み合わせて用いることとしてもよい。具体的には、以下の式（４２ａ）〜（４２ｊ）で表されるアルゴリズムとなる。

続いて、上記本実施の形態に係る制御装置１によるロボット２０の制御実験の結果について説明する。本実験では、図４に示す６軸垂直多関節ロボット（（株）安川電機製、MOTOMAN-HP3J）を用いて、ロボットに外力を加えたときのロボット２０の位置を計測した。具体的には、期間Ａにおいて、実験者がロボット２０の力センサ１８を上方向に押し、期間Ｂにおいて、実験者がロボット２０のリンクを下方向へ押した場合のロボット２０及び仮想物体の位置を確認した。

図５（ａ）〜（ｅ）に実験結果を示す。図５（ａ）は従来の制御アルゴリズム、図５（ｂ）は式（２８）に示される修正を加えた制御アルゴリズム（式（２６ａ）〜（２６ｊ）、（２８））、図５（ｃ）は式（２８）に示される修正を加えた制御アルゴリズムにおいて定数Ｍ＝０とおいた場合、図５（ｄ）は加速度フィードフォワードの修正を追加した場合（式（４２ａ）〜（４２ｊ）でＦ_ｘ＝０とした場合）、図５（ｅ）は本実施の形態に係る制御アルゴリズム（式（４２ａ）〜（４２ｊ））による実験結果を表す。

図５（ａ）に示すように、従来の制御アルゴリズムでは、期間Ａにおいて力が加えられると、その後もロボット２０は上方向へ移動し続けた。また、期間Ｂにおいてリンクに下方向の力が加えられると、その後もロボット２０は下方向へ移動し続けた。

図５（ｂ）に示すように、式（２８）に示される修正を加えた場合、期間Ｂにおいて実験者がロボット２０に加えた力により、仮想物体位置とロボット位置の乖離が生じ、スナップバック、振動等の現象が見られる。

一方、図５（ｃ）に示すように、定数Ｍ＝０と設定することにより、期間Ｂの後のスナップバック、振動等の動作は発生していない。すなわち、式（２８）に示される修正により、加速度フィードフォワードが従来技術と同じであっても、Ｍ＝０であれば、期間Ｂの後、ロボット２０は速やかに停止され、好ましい動作をすることがわかる。

図５（ｄ）に示すように、式（２８）に示される修正に加えて、加速度フィードフォワードの修正（式（３８ａ）〜（３８ｅ）の修正）を行うことにより、図５（ｃ）と同様に、期間Ｂの後、ロボット２０は速やかに停止され、好ましい動作をしていることがわかる。

図５（ｅ）に示すように、各修正を組み合わせることにより、期間Ａの後のロボット２０の動作が、仮想物体の動特性として設定したクーロン摩擦力によって減衰されている。さらに、期間Ｂの後のロボット２０の動作が抑制されており、適切な動作をしていることがわかる。

また、図６に示すように、ロボット２０のエンドエフェクタを、外部環境である物体としてのテーブルに繰り返し押しつけた場合の、力センサ１８の測定値を図７（ａ）に示す。また、図７（ｂ）は、定数Ｍ＝０と設定した場合の力センサ１８の測定値を示している。図７（ａ）、（ｂ）に示すように、本実施の形態に係る制御装置１により、ロボット２０が安定して動作していることがわかる。特に、修正された加速度フィードフォワード及び仮想物体のクーロン摩擦が安定性に寄与することにより、ロボット２０は好ましい動作をしている。

以上詳細に説明したように、本発明に係る制御装置１では、次時刻ステップに持ち越される仮想物体の速度を、仮想物体の飽和前速度ベクトルとゼロベクトルの中間のベクトルとすることにより、外力によって行われた仕事が仮想物体の運動エネルギーとして保持されることを防止できるので、より安全にロボットを制御することが可能である。

特に、仮想物体の飽和前速度ベクトルとゼロベクトルとの中間のベクトルのうち飽和後速度ベクトルに最も近いベクトルを次時刻ステップに持ち越される速度として用いることにより、高い制御性能を維持しつつ、安全にロボットを動作させることができる。

また、制御アルゴリズムに、加速度フィードフォワードの修正、クーロン摩擦力の影響を加えることにより、ロボット２０をより適切に動作させることが可能である。

また、仮想物体は、目標位置へ収束するために設定された所定の動特性を有することとしてもよい。例えば、仮想物体の位置をｐ_ｘ、仮想物体の速度をｖ_ｘ、仮想物体の加速度をａ_ｘ、目標位置をｐ_ｄ、目標速度をｖ_ｄ、目標加速度をａ_ｘとした場合、仮想物体の動特性は、ある正の定数Ｈ１及びＨ２を用いて、ｐ_ｘ−ｐ_ｄ＋Ｈ１（ｖ_ｘ−ｖ_ｄ）＋Ｈ２（ａ_ｘ−ａ_ｄ）＝０、で表されることしてもよい。これにより、仮想物体の位置ｐ_ｘ、速度ｖ_ｘを、目標位置ｐ_ｄ、目標速度ｖ_ｄに指数関数的に収束させることができる。

また、仮想物体は、目標速度へ収束するために設定された所定の動特性を有することとしてもよい。例えば、仮想物体の速度をｖ_ｘ、仮想物体の加速度をａ_ｘ、目標速度をｖ_ｄとした場合、仮想物体の動特性は、ある正の定数Ｈを用いて、ｖ_ｘ−ｖ_ｄ＋Ｈａ_ｘ＝０、で表されることとしてもよい。これにより、仮想物体の速度ｖ_ｘを目標速度ｖ_ｄに指数関数的に収束させることができる。

（実施の形態２）
上記実施の形態１に係る制御装置１では、ロボット２０が備える１つの駆動軸を制御することとしたが、これに限られない。例えば、位置センサ１９は角度センサであり、駆動部２０ａとして複数のモータを備える多自由度マニピュレータを制御することとしてもよい。この場合、仮想物体の挙動は作業空間で定義され、制御対象の位置制御とトルク飽和とは関節角度空間で行われる。

より具体的には、図８（ａ）に示される上記実施の形態１における離散時間アルゴリズムに対して、図８（ｂ）に示すように、作業空間（手先空間）と関節角度空間（関節空間）との間で、順運動学と逆運動学とを用いた演算処理を加える。このような多自由度マニピュレータを制御対象とする場合、仮想物体の位置ベクトルは、３次元の並進位置ベクトルと、仮想物体の姿勢を表現する３個から１６個の数値の集合とからなる多次元のベクトル値として構成すればよい。ここで仮想物体の姿勢の表現としては、例えば、オイラー角、ロール・ピッチ・ヨー角、四元数、回転行列等を用いることができる。また、仮想物体の速度ベクトルは、３次元の並進速度ベクトルと、３次元の角速度ベクトルとからなる多次元のベクトル値として構成すればよい。

これにより、制御対象が、複数の駆動軸を備える多自由度マニピュレータであっても、射影を用いて飽和時の速度ベクトル等を適切に設定することができるので、制御対象の意図しない動作を抑制し、安全に制御対象を動作させることができる。

本発明は、外力によって駆動部の発生力が飽和する可能性があるロボットの制御を行う制御装置に好適である。特に、作業者とロボットが協調して作業を行う、重量物の組み立て工程で用いられるロボット、リハビリテーション支援ロボット等の制御装置に好適である。

１制御装置、１０制御ユニット、１１制御部、１１１演算部、１１２動作制御部、１２記憶部、１３表示部、１４入力部、１８力センサ、１９位置センサ、２０ロボット、２０ａ駆動部

Claims

制御対象の位置を検出する位置センサと、
前記制御対象を動作させる駆動装置の操作量を決定する演算部と、を備え、
前記演算部は、
所定の動特性を有する仮想物体と制御則とを保持しており、
前記仮想物体の運動をシミュレートして次時刻ステップの飽和前位置ベクトルと飽和前速度ベクトルとを算出し、
前記位置センサの測定値と、算出された前記飽和前位置ベクトルと、前記制御則とに基づいて、前記操作量を設定するとともに、
前記操作量が予め設定された所定の範囲内にある場合には、
前記操作量を前記駆動装置に出力し、
前記飽和前位置ベクトルを、前記仮想物体の次時刻ステップの位置ベクトルとして設定し、
前記飽和前速度ベクトルを、前記仮想物体の次時刻ステップの速度ベクトルとして設定し、
前記操作量が予め設定された所定の範囲外にある場合には、
前記操作量が所定の範囲内の値となるように、前記飽和前位置ベクトル及び前記操作量を修正し、
修正した前記操作量を前記駆動装置に出力し、
修正した前記飽和前位置ベクトルを、前記仮想物体の次時刻ステップの位置ベクトルとして設定し、
前記飽和前速度ベクトルとゼロベクトルとの中間の値を、前記仮想物体の次時刻ステップの速度ベクトルとして設定する、
ことを特徴とする制御装置。
前記仮想物体の前記次時刻ステップの速度ベクトルｖ_ｎは、以下の式
ｖ_ｎ＝proj_｛co（０，ｖ_ａ）｝（ｑ_ｎ−ｑ）／Ｔ
ただし、projは射影、coは凸包、ｖ_ａは前記飽和前速度ベクトル、ｑ_ｎは前記仮想物体の次時刻ステップの位置ベクトル、ｑ_ｐは前記仮想物体の位置ベクトル，Ｔはステップ時間
を用いて算出される、
ことを特徴とする請求項１に記載の制御装置。
前記制御則は、前記仮想物体の加速度フィードフォワードを含み、
前記仮想物体の加速度は、前記仮想物体の飽和前速度と前時刻ステップの速度との差をステップ時間で除したものであり、
前記加速度フィードフォワードは前記操作量が飽和しない範囲に制限される、
ことを特徴とする請求項１から２のいずれか一項に記載の制御装置。
前記仮想物体は、目標位置へ収束するために設定された所定の動特性を有する、
ことを特徴とする請求項１から３のいずれか一項に記載の制御装置。
前記仮想物体は、目標速度へ収束するために設定された所定の動特性を有する、
ことを特徴とする請求項１から３のいずれか一項に記載の制御装置。
前記制御対象は、制御対象と外部環境である物体とが接触する力を検出する力センサを備え、
前記演算部に保持される前記仮想物体の動特性は、力を受けて運動する質点又は剛体の動特性であり、
前記演算部は、
前記制御対象に加える目標力と前記力センサの測定値とを前記仮想物体に与えた場合の運動をシミュレートして、次時刻ステップの飽和前位置ベクトルと飽和前速度ベクトルとを算出する、
ことを特徴とする請求項１から３のいずれか一項に記載の制御装置。
前記動特性は、クーロン摩擦特性を含む、
ことを特徴とする請求項６に記載の制御装置。
前記位置センサは複数の角度センサであり、
前記駆動装置は複数のモータを備え、
前記仮想物体の位置ベクトルと速度ベクトルとは、多次元のベクトル値である、
ことを特徴とする請求項１から７のいずれか一項に記載の制御装置。
前記仮想物体の位置ベクトルは、
３次元の並進位置ベクトルと、前記仮想物体の姿勢を表現する３個から１６個の数値の集合とから構成され、
前記仮想物体の速度ベクトルは、
３次元の並進速度ベクトルと、３次元の角速度ベクトルとから構成される、
ことを特徴とする請求項８に記載の制御装置。