位置制御装置、位置制御方法及び位置制御プログラム
【目的】物体の位置を、物体を保持する移動機構によって目標位置に速やかに追従させる技術を提供する。
【構成】本発明は、物体の位置を検出する位置検出器と、物体を保持しており物体に加える力の指令値が入力され入力された指令値を実現するように動力によって動作する移動機構と、物体に加える力の指令値を移動機構へ出力する指令装置を備える。指令装置内に構築された制御ロジックは、スライディングモード制御ロジックに対してｓ／（Ｌ＋Ｋ・ｓ＋Ｂ・ｓ２）の局所的なフィードバックループを付加したものである。この局所的なフィードバックループにより、物体の位置を目標位置に追従させる際にオーバーシュートを生じさせることなく、さらに指令装置が出力する指令値がチャタリングすることを防止できる。
【構成】本発明は、物体の位置を検出する位置検出器と、物体を保持しており物体に加える力の指令値が入力され入力された指令値を実現するように動力によって動作する移動機構と、物体に加える力の指令値を移動機構へ出力する指令装置を備える。指令装置内に構築された制御ロジックは、スライディングモード制御ロジックに対してｓ／（Ｌ＋Ｋ・ｓ＋Ｂ・ｓ２）の局所的なフィードバックループを付加したものである。この局所的なフィードバックループにより、物体の位置を目標位置に追従させる際にオーバーシュートを生じさせることなく、さらに指令装置が出力する指令値がチャタリングすることを防止できる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体の位置を目標位置に追従させる位置制御装置、位置制御方法および位置制御プログラムに関する。
【背景技術】
【０００２】
物体を保持し、物体に加える力の指令値が入力され、入力された指令値を発生する移動機構を有し、物体の位置を目標位置に追従させるように力の指令値を出力することによって移動機構を制御し、物体の位置を目標位置に追従させる位置制御装置が知られている。本明細書では、物体の位置という場合にはその物体の位置及び／又はその物体の姿勢を意味するものとし、速度という場合には並進速度及び／又は角速度を意味するものとし、力という場合には並進力及び／又はトルクを意味するものとする。
位置制御装置の典型例である産業用ロボットは、一般に、関節摩擦などのモデル化しにくい機械要素を多く含む。このため、力制御やインピーダンス制御を行う際にも、高ゲインのＰＤ（比例・微分）またはＰＩＤ（比例・積分・微分）制御方式の位置制御器を最下層のフィードバックループとして用いることが多い。高ゲインのＰＤ制御またはＰＩＤ制御方式の位置制御器を用いることによって、ロボットの動特性や非線形要素の影響を低減し、ロボットの手先の現在位置を目標位置に精度よく追従させることができることがよく知られている。
また、力制御やインピーダンス制御の他に、スライディングモード制御という制御法が知られている（非特許文献１）。スライディングモード制御は、状態空間に設計した切換面上に状態を拘束することによって希望の動特性を達成する制御法である。これはいわゆるバンバン制御の１つの一般化であり、不連続な制御出力を発生する。外乱に対してロバストであるということがスライディングモード制御の利点である。
【０００３】
【非特許文献１】野波 健蔵、田 宏奇、スライディングモード制御−非線形ロバスト制御の設計理論、コロナ社、１９９４年
【発明の開示】
【発明が解決しようとする課題】
【０００４】
高ゲインのＰＤまたはＰＩＤ位置制御は、次のような課題がある。たとえば、環境との接触や、ロボットが有するアクチュエータの一時的なパワー低下などの異常によって、位置制御器の目標位置とロボットの手先位置との偏差が大きくなった場合、大きな復元力によって環境が損傷を受けたり、復帰動作の際に過大な速度が発生したりすることがある。即ち、何らかの要因による異常時にロボットが予想外の動作をする可能性がある。
位置制御されているロボットで、正常時の追従性と異常時の予想外の動作の防止とを両立する問題は単純ではない。まず、アクチュエータの出力トルクに上限を設けることは必要条件である。そして、異常状態から復帰する際にオーバーシュートを起こさずに目標軌道へゆるやかに収束する必要がある。これは、正常時の即応性・追従性と相反する要求であるといえる。ＰＤ制御またはＰＩＤ制御において収束性を確保するためには、Ｄ（微分）ゲインを大きくする必要があるが、これは正常運転時の即応性を低下させ、また、特に低速運動時に高周波振動の原因となる。また、Ｉ（積分）ゲインを大きくするとオーバーシュートが生じやすくなる。このように、正常動作時の追従性と異常時の予想外の動作の防止との両立は、ＰＤ制御またはＰＩＤ制御のゲイン設計とトルク制限では対処できない問題を含んでいる。
本発明は、上記の課題を解決する。本発明は、物体を保持した移動機構に対して、正常動作時の追従性と異常時の予想外の動作の防止とを両立する単純な位置制御装置および位置制御方法を提供する。本発明は、物体の位置を目標位置に追従させるための汎用性の高い技術を提供する。
【課題を解決するための手段】
【０００５】
本発明は、一般によく知られているスライディングモード制御という制御法を着想の原点としている。本発明は、スライディングモード制御が本来持っている欠点を取り除き、その利点を応用した新たな制御方法である。
【０００６】
まず、スライディングモード制御について概説する。スライディングモード制御は、状態空間に設計した切換面上に状態を拘束することによって希望の動特性を達成する制御法である。これはいわゆるバンバン制御の１つの一般化であり、不連続な制御出力を発生する。外乱に対してロバストであるということがこれの本来の利点であるが、アクチュエータ出力（これは、ロボットが保持した物体に加える力と等価である）に明示的に上限を設できるという点と、状態を希望の動特性に拘束できるという点は、異常時の予想外の動作の防止と追従性の両立という目的にもかなっている。すなわち、トルク制限、正常動作時の追従性、および復帰動作時の収束性をすべて実現できる可能性がある。しかし、スライディングモード制御の理論は、無限に高速に応答する連続時間系を前提としているので、通常の離散時間系に実装された場合には、切換の繰り返しによって高周波振動（チャタリング）が発生する。
ここでは簡単のため、１自由度の位置制御系を考える。制御対象（ロボットが保持する物体）は質量Ｍの質点とし、その現在位置および現在速度をそれぞれｐおよびｖとする。制御器はｐおよびｖを測定し、アクチュエータから力ｇを発生する。ここで、「アクチュエータが発生する力ｇ」は、ロボットが保持している制御対象に加わる力ｇと等価であるとする。制御器が設定する目標位置と目標速度をそれぞれｐｄおよびｖｄとする。質点はアクチュエータからの力ｇと外力−ｆを受けて運動する。すなわち質点の運動方程式は下記のようになる。
Ｍ・ｄｖ／ｄｔ＝ｇ−ｆ ・・・（１）
最も単純なスライディングモード制御器は、たとえば図１のような構造を持つ。ここでアクチュエータからの出力ｇは、
ｇ＝Ｆ・ｓｇｎ［ｐｄ−ｐ＋Ｈ・（ｖｄ−ｖ）］ ・・・（２）
と決定される。ただし、ここでｓｇｎ［・］は符号関数であり、
ｓｇｎ［ａ］＝１；（ａ＞０の場合）、−１；（ａ＜０の場合） ・・・（３）
である。また、ＦおよびＨは正の実数である。関数φ（ｐ、ｖ）＝ｐｄ−ｐ＋Ｈ・（ｖｄ−ｖ）を定義すると、状態空間（ｐ−ｖ平面）内において、φ（ｐ、ｖ）＝０は超平面（直線）を構成する。この超平面は切替平面と呼ばれ、これを境界として、式（２）に示すように、制御器の出力が不連続に切り替えられる。この切換が無限に高速であれば、この系は切替平面φ（ｐ、ｖ）＝０に有限時間内に到達する。そして、切替平面に到達した後は、外力ｆと質点の慣性力の合力の大きさがＦを超えない限り、系は切替平面内に拘束される。切替平面内に拘束されている間、制御対象はφ（ｐ、ｖ）＝０、すなわち
ｄｐ／ｄｔ＝ｖ＝（ｐｄ−ｐ）／Ｈ＋ｖｄ ・・・（４）
という微分方程式に従って推移する。この状態を滑り状態（ｓｌｉｄｉｎｇ ｍｏｄｅ）と呼ぶ。
この方式で、アクチュエータの出力ｇの上限は定数Ｆとして明示的に与えることができる。また、復帰運動時の挙動は定数Ｈにより決定できる。そして、切替が無限に高速であれば、完全な追従性が得られる。しかし、離散時間系では切替に時間遅れが生じるため、切換面交差の繰り返しによって高周波振動（チャタリング）が発生する。したがって、図１の形の制御系は、現実の制御対象には用いることが難しい。
【０００７】
そこで本発明では、図２の系を考える。ここでは、スライディングモード制御器は、制御対象に直接力を加えるわけではなく、計算器内に仮想的に想定された「プロキシ（代理点）」に力を加えるものとする。スライディングモード制御器に設定された目標位置および目標速度をそれぞれｐｄおよびｖｄとし、プロキシの位置および速度をそれぞれｐｓおよびｖｓとする。前節と同じ制御則を用いると、スライディングモード制御器の発生力ｇは、
ｇ＝Ｆ・ｓｇｎ［ｐｄ−ｐｓ＋Ｈ・（ｖｄ−ｖｓ）］ ・・・（５）
となる。プロキシと実際の制御対象は、仮想的な保持要素（ｖｉｒｔｕａｌ ｃｏｕｐｌｉｎｇ）で結合されており、この仮想保持要素の発生する力は
ｆ＝Ｌ・∫（ｐｓ−ｐ）ｄｔ＋Ｋ・（ｐｓ−ｐ）＋Ｂ・（ｖｓ−ｖ） ・・・（６）
となる。ただし、位置および速度をそれぞれｐおよびｖとしている。力ｆはアクチュエータから出力されて実制御対象に加えられるが、その反力はプロキシにも加えられるものとする。そうすると、プロキシの質量をＭとすると、プロキシの運動方程式は下記のようになる。
Ｍ・ｄ＾２ｐｓ／ｄｔ＾２＝ｇ−ｆ ・・・（７）
ここで、式（５）（６）（７）にプロキシの質量Ｍ＝０を代入すると、
ｆ＝Ｆ・ｓｇｎ［ｐｄ−ｐ−（ｄａ／ｄｔ）
＋Ｈ・（ｖｄ−（ｄ＾２ａ／ｄｔ＾２））］ ・・・（８）
ｆ＝Ｌ・ａ＋Ｋ（ｄａ／ｄｔ）＋Ｂ・（ｄ＾２ａ／ｄｔ＾２） ・・・（９）
ｇ＝ｆ ・・・（１０）
が得られる。ただし、ａ＝∫（ｐｓ−ｐ）ｄｔである。この系のブロック線図は図３のようになる。以後、簡単のため、
ｓ＝（ｐｄ−ｐ）＋Ｈ・（ｖｄ−ｖ） ・・・（１１ａ）
ｆ＝Ｆ・ｓｇｎ［ｓ−（ｄａ／ｄｔ）−Ｈ・（ｄ＾２ａ／ｄｔ＾２）］
・・・（１１ｂ）
ｆ＝Ｌ・ａ＋Ｋ・（ｄａ／ｄｔ）＋Ｂ・（ｄ＾２ａ／ｄｔ＾２） ・・・（１１ｃ）
と表す。これは、ｐｄ、ｖｄ、ｐおよびｖを入力とし、ｆを出力とし、ａを内部状態変数とする制御器と解釈できる。
式（１１）の離散時間表現は、下記のようになる。
ｓ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））＋Ｈ・（ｖｄ（ｋ）−ｖ（ｋ））
・・・（１２ａ）
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｓ（ｋ）−（ａ（ｋ）−ａ（ｋ−１））／Ｔ−Ｈ・（ａ（ｋ）−２・ａ（ｋ−１）＋ａ（ｋ−２））／Ｔ／Ｔ］ ・・・（１２ｂ）
ｆ（ｋ）＝Ｌ・ａ（ｋ）＋Ｋ・（ａ（ｋ）−ａ（ｋ−１））／Ｔ＋Ｂ・（ａ（ｋ）−２・ａ（ｋ−１）＋ａ（ｋ−２））／Ｔ／Ｔ ・・・（１２ｃ）
ここで、括弧内の記号は整数であり、時刻をあらわす。また、時間微分（ｄａ／ｄｔ）はオイラー差分（ａ（ｋ）−ａ（ｋ−１））／Ｔに置き換えている。
式（１２）に出現する変数のうち、未知数はｆ（ｋ）とａ（ｋ）である。これらを求めるためには、式（１２ｂ）と式（１２ｃ）を代数的に解く必要がある。まず、式（１２ｃ）を変形すると
ａ（ｋ）＝（（２・Ｂ＋Ｋ・Ｔ）・ａ（ｋ−１）−Ｂ・ａ（ｋ−２）
＋Ｔ・Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＝ａ（ｋ−１）・（Ｂ＋Ｋ・Ｔ）／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋（ａ（ｋ−１）−ａ（ｋ−２））・Ｂ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋ｆ（ｋ）・Ｔ・Ｔ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ） ・・・（１３）
が得られる。これを式（１２ｂ）に代入すると、ａ（ｋ）を消去した式が出現する。
ｆ（ｋ）＝Ｆ・ｓｇｎ［（ｕ（ｋ）−ｆ（ｋ））・（Ｈ＋Ｔ）／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）］ ・・・（１４）
ただし、
ｕ（ｋ）＝ｓ（ｋ）・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）＋ａ（ｋ−１）・Ｌ
＋（ａ（ｋ−１）−ａ（ｋ−２））・（Ｈ・（Ｋ＋Ｌ・Ｔ）−Ｂ）／（Ｈ＋Ｔ）／Ｔ ・・・（１５）
である。式（１４）はすなわち、
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）−ｆ（ｋ）］ ・・・（１６）
である。
簡単な考察により、代数方程式（１６）は、
ｆ（ｋ）＝ｕ（ｋ）；（｜ｕ（ｋ）｜≦Ｆの場合）
または
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）］；（｜ｕ（ｋ）｜＞Ｆの場合） ・・・（１７）
と書き直すことができるということが分かる。ただしここでの符号関数ｓｇｎ［ｘ］はｘ＝０のときに閉区間［−１、１］の任意の値を取りうると定義している。
式（１７）によりｆ（ｋ）が求められれば、式（１３）によりａ（ｋ）が求められる。結局、式（１２）に基づいて値を更新するためには、下記の手順で演算を行えばよい。
ｓ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））＋Ｈ（ｖｄ（ｋ）−ｖ（ｋ））・・・（１８ａ）
ｕ（ｋ）＝ｓ（ｋ）・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）＋ａ（ｋ−１）・Ｌ
＋（ａ（ｋ−１）−ａ（ｋ−２））・（Ｈ・（Ｋ＋Ｌ・Ｔ）−Ｂ）／（Ｈ＋Ｔ）／Ｔ
・・・（１８ｂ）
ｆ（ｋ）＝ｕ（ｋ）；（｜ｕ（ｋ）｜≦Ｆの場合）
または
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）］；（｜ｕ（ｋ）｜＞Ｆの場合） ・・・（１８ｃ）
ａ（ｋ）＝（（２・Ｂ＋Ｋ・Ｔ）・ａ（ｋ−１）−Ｂ・ａ（ｋ−２）
＋Ｔ・Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ） ・・・（１８ｄ）
なお、式（１８ａ）を式（１８ｂ）に代入すると次式が得られる。
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋ａ（ｋ−１）・Ｌ
＋（ａ（ｋ−１）−ａ（ｋ−２））・（Ｈ・（Ｋ＋Ｌ・Ｔ）−Ｂ）／（Ｈ＋Ｔ）／Ｔ ・・・（１８ｅ）
この系において制御器は、
φ（ｐｓ（ｋ）、ｖｓ（ｋ））＝（ｐｄ（ｋ）−ｐｓ（ｋ））
＋Ｈ・（ｖ（ｋ）−ｖｓ（ｋ）） ・・（１９）
を有限時間で零に到達させる役割を持つ。式（１９）で
ｐｓ（ｋ）＝ｐ（ｋ）＋（ａ（ｋ）−ａ（ｋ−１））／Ｔ
ｖｓ（ｋ）＝ｖ（ｋ）＋（ａ（ｋ）−２・ａ（ｋ−１）＋ａ（ｋ−２））／Ｔ／Ｔ
を考慮に入れると、
φ（ｐｓ（ｋ）、ｖｓ（ｋ））＝（ｕ（ｋ）−ｆ（ｋ））・（Ｈ＋Ｔ）／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ） ・・・（２０）
が成立する。式（１８ｃ）より、ｆ（ｋ）とｕ（ｋ）の符号は常に一致し、ｆ（ｋ）の絶対値はｕ（ｋ）の絶対値を超えることがない。したがって、ｆ（ｋ）は常にφ（ｐｓ（ｋ）、ｖｓ（ｋ））を零に引き付ける方向に働き、φ（ｐｓ（ｋ）、ｖｓ（ｋ））の符号を逆転させることがない。これは、ｆ（ｋ）の存在によってｐｓ（ｋ）が振動的にならないことを意味する。これにより、プロキシの状態が有限時間で滑り状態（φ（ｐｓ、ｖｓ）＝０）に到達する。厳密にいうと、実制御対象の状態量（ｐ、ｖ）は滑り平面に到達しない。しかし、実制御対象はプロキシに常に拘束されているので、プロキシが滑り状態にあるとき、系全体として滑り状態にあると言ってよい。式（１８）において｜ｕ（ｋ）｜≦Ｆであるとき、この系は滑り状態にある。
【０００８】
即ち、本発明では、動力によって動作する移動機構（上記説明の「アクチュエータ」に相当する）に対して保持した物体（上記説明の「実制御対象」に相当する）に加える力の指令値を出力する指令装置（上記説明の「制御器」に相当する）が力の指令値を計算する際に、質量ゼロの仮想質点（上記説明の「プロキシ」に相当する）と、一端が物体の検出された位置に連結されており、他端が仮想質点に連結されている仮想的な連結要素を仮想空間内に設定する。そしてスライディングモード制御則に基づいて、仮想質点の位置を目標位置に追従させる制御を仮想空間内で実行した際に、仮想的な保持要素から物体に加えられる力を算出する。算出された力を移動機構の動力源への指令値とする。
【０００９】
指令装置は、時刻ｋにおける力の指令値ｆ（ｋ）を算出する過程で、指令値ｆ（ｋ）を算出するための暫定制御力ｕ（ｋ）を算出する。暫定制御力ｕ（ｋ）は、例えば、式（１８ｅ）で求めることができる。時刻ｋ＋１において式（１８ｅ）によって暫定制御力ｕ（ｋ＋１）を求める場合には、式（１８ｅ）の中に未知数ａ（ｋ）が表れる。このａ（ｋ）は、式（１３）によって求めることができる。
【００１０】
上記の説明ではＰＩＤ制御方式の仮想保持部材を用いた手法を導いたが、Ｌ＝０とすることによってＰＤ制御方式の仮想保持部材を用いた手法を導くことも可能である。これは下記のようになる。
ｓ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））＋Ｈ（ｖｄ（ｋ）−ｖ（ｋ））・・・（２２ａ）
ｕ（ｋ）＝（ｓ（ｋ）・（Ｂ＋Ｋ・Ｔ）＋ｅ（ｋ−１）・（Ｈ・Ｋ−Ｂ））／（Ｈ＋Ｔ） ・・・（２２ｂ）
ｆ（ｋ）＝ｕ（ｋ）；（｜ｕ（ｋ）｜≦Ｆの場合）
または
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）］；（｜ｕ（ｋ）｜＞Ｆの場合）・・ ・（２２ｃ）
ｅ（ｋ）＝（Ｂ・ｅ（ｋ−１）＋Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ） ・・・（２２ｄ）
ただしここで、ｅ（ｋ）＝（ａ（ｋ）−ａ（ｋ−１））／Ｔに相当する。さらに、ａ（ｋ）はａ（ｋ）＝∫（ｐｓ（ｋ）−ｐ（ｋ））ｄｔであるのでｅ（ｋ）は、
ｅ（ｋ）＝ｐｓ（ｋ）−ｐ（ｋ） ・・・（２２ｅ）
とも表すことができる。
また、式（２２ａ）を式（２２ｂ）に代入すると、
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋ｅ（ｋ−１）・（Ｈ・Ｋ−Ｂ）／（Ｈ＋Ｔ） ・・・（２２ｆ）
となる。即ち、暫定制御力ｕ（ｋ）を、式（１８ｅ）の代わりに式（２２ｆ）によって求めてもよい。このとき時刻ｋ＋１において式（２２ｆ）によって暫定制御力ｕ（ｋ＋１）を求める場合には、式（２２ｆ）の中に未知数ｅ（ｋ）が表れる。このｅ（ｋ）は、式（２２ｄ）によって求めることができる。
【００１１】
指令装置は、上記のように算出された暫定制御力ｕ（ｋ）を用いて移動機構へ出力する力の指令値ｆ（ｋ）を算出する。指令装置は、移動機構の動力が出力することを許容されている力の集合をＸとしたときに、暫定制御力ｕ（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｕ（ｋ）とする。暫定制御力ｕ（ｋ）∈Ｘが満たされないときには指令値ｆ（ｋ）を、暫定制御力ｕ（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘが満たされるｆ（ｋ）を指令値ｆ（ｋ）とする。
ここで、集合Ｘは、例えば設定されたスカラ値Ｆよりも絶対値が小さい全ての値の集合である。「移動機構の動力が出力することを許容されている力」を集合Ｘとして表現したのは次の理由による。即ち、「移動機構の動力が出力することを許容されている力」が移動機構先端の座標系で表される並進力やトルクのベクトル量で表されることもあれば、移動機構が複数の回転関節を有する機構である場合に、各関節に備えられたモータなどのトルクのベクトル量で表されることもあるので、それらを包含する意味で集合Ｘという表現を用いた。
前記指令装置は、移動機構の動力が出力することを許容されている力の大きさをＦとしたときに、｜ｕ（ｋ）｜＜Ｆが満たされる場合は指令値ｆ（ｋ）＝ｕ（ｋ）とし、｜ｕ（ｋ）｜＜Ｆが満たされない場合は式（１７）、換言すれば、指令値ｆ（ｋ）＝（Ｆ／｜ｕ（ｋ）｜）・ｕ（ｋ）とする。即ち、ここでは（Ｆ／｜ｕ（ｋ）｜）が、設定されたスカラ値Ｆよりも絶対値が小さい値の例となっている。
【００１２】
指令装置によって算出された力の指令値ｆ（ｋ）は、移動機構へと出力される。移動機構は指令された力の指令値に基づいて物体に力を加える。その結果、物体は目標位置に近づくように移動する。
【００１３】
以上の動作によって、上記説明したように、プロキシの位置ｐｓ（ｋ）を振動させることがなく、プロキシの状態（ｐｓ、ｖｓ）が有限時間で滑り状態（φ（ｐｓ、ｖｓ）＝０）に到達する。厳密にいうと、物体（即ち実制御対象）の状態量（ｐ、ｖ）は滑り平面に到達しない。しかし、物体の状態量（ｐ、ｖ）はプロキシの状態量（ｐｓ、ｖｓ）に常に拘束されているので、プロキシが滑り状態にあるとき、系全体として滑り状態にあると言ってよい。従って物体の状態量（ｐ、ｖ）は振動的にならずに目標位置と目標速度（ｐｄ、ｖｄ）に追従する。
【００１４】
本発明の位置制御装置は、上記説明したように、物体の位置を検出する位置検出器と、物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、物体に加える力の指令値を移動機構へ出力する指令装置を備える。
力の指令値を算出する際に物体の速度を知る必要がある場合には、物体の速度を取得する速度取得手段を備えてもよい。物体の速度ｖを取得する速度取得手段は、位置制御装置が別途備える速度検出器によって検出してもよいし、位置検出器によって検出される物体の位置ｐの経時的な変化から計算によって求めてもよい。離散時間系の場合は位置検出器によって検出した物体の位置ｐ（ｋ）と、その単位時間Ｔ前に検出した物体の位置ｐ（ｋ−１）の差分を単位時間Ｔで除した値、即ち、ｖ（ｋ）＝（ｐ（ｋ）−ｐ（ｋ−１））／Ｔの式により求めてもよい。
【００１５】
指令装置が移動機構へ出力する力の指令値を算出する場合、例えば上記説明したように、暫定制御力ｕ（ｋ）を式（１８ｅ）や式（２２ｆ）によって求め、暫定制御力ｕ（ｋ）と動力が出力することを許容されている力の集合Ｘとの関係から求めることができる。
なお、物体の位置ｐは、位置検出器によって検出される。
【００１６】
式（１８）は各変数がベクトルであっても成立するので、容易に多次元に拡張できる。ただしここで、絶対値演算子｜・｜はベクトルのノルム‖ｘ‖＝√（ｘ＾２）として、符号関数ｓｇｎ［・］はベクトルの正則化関数ｓｇｎ［ｘ］＝ｘ／‖ｘ‖として、それぞれ読み替える必要がある。ノルムとしては、Ｌ２ノルムやＬ無限大ノルムを用いることができる。
【００１７】
さらに発明者らは、本発明を応用することにより、作業者が、物体を広域的に目標位置へ追従するように移動機構を動かすと、局所的には物体の位置を目標位置に追従させる位置制御装置を実現できること発案した。
この位置制御装置は、前述した位置制御装置に対して、さらに、移動機構が動力によって物体に加える力の大きさの上限値Ｆが、移動機構の機械的な静止摩擦力と略同等以下の大きさに設定されており、移動機構は、作業者が移動機構に操作力を加えるための操作子を備える。
上記構成によれば、移動機構が物体に加える力の大きさの上限値Ｆが、移動機構の機械的な静止摩擦力と略同等以下の大きさに設定される。これによって、移動機構が出力できる最大の大きさの力Ｆを物体に加えるように動作しても、力Ｆが静止摩擦力に打ち勝つことができなければ物体は移動しない。また力Ｆが静止摩擦力に打ち勝つとしても物体には僅かしか移動しないことになる。
指令装置から入力された力指令値によって、移動機構は物体の位置を目標位置へ動かす方向に物体に力を発生させている。ここで、作業者が物体を広域的に目標位置へ追従するように移動機構を動かすと、物体には、移動機構が発生する力と操作力がほぼ同じ方向に作用する。移動機構が発生する力と操作力の合力によって、移動機構は保持した物体を目標位置へ移動させることができるようになる。これによって、作業者が、物体を広域的に目標位置へ追従するように移動機構を動かすと、局所的には物体の位置を目標位置に追従させる位置制御装置を実現できる。
一方、移動機構が物体に加える力の大きさの上限値Ｆが、静止摩擦力と略同等以下の大きさに設定されることによって、物体の位置と目標位置が大きく離れている場合に作業者が操作子を操作して物体を移動させることを妨げるほどには移動機構が発生する力が過大となることを防止することができる。
上記の構成によれば、移動機構に保持された物体を作業者が広域的に移動させる作業を妨げることなく、物体の位置が目標位置に近づいた際には物体の位置を目標位置に一致させることのできる位置制御装置を実現することができる。
【発明の効果】
【００１８】
本発明によれば、物体を保持した移動機構に対して、正常動作時の追従性と異常時の予想外の動作の防止とを両立する単純な位置制御装置および位置制御方法を提供することができる。この位置制御装置及び方法は、物体を保持する移動機構の正常動作時の小さな位置決め誤差からのすばやい復帰と、異常動作時の大きな位置誤差からのゆるやかな復帰を保つことができる。その際に移動機構に入力される物体に加える力の指令値が発振することを防止できる。
さらに、移動機構が発生できる力の上限値を、移動機構が保持した物体と移動機構の機械的な静止摩擦力と略同等以下の大きさに設定することによって、作業者が物体の位置を目標位置へ広域的に移動させる操作力を移動機構へ加えるだけで、移動機構は物体の位置を目標位置へ迅速に追従するように位置制御することができる。
【実施例】
【００１９】
＜実施例１＞
図４に示す２自由度平行リンクマニピュレータを用いて実験を行った。各関節のアクチュエータはハーモニックドライブ（登録商標）ギアによって減速されたＡＣサーボモータである。各アクチュエータには光学エンコーダが取り付けられており、手先効果器の位置を計測することができる。各モータには、出力軸側換算で１０Ｎ・ｍ程度の摩擦が存在する。
以下に示す３種類の制御器Ａ、Ｂ、およびＣを用いて実験を行った。各制御器は式（１８）の構造を持つ。
｜パラメータ ｜ 制御器Ａ ｜制御器Ｂ ｜制御器Ｃ ｜
｜Ｆ（Ｎ） ｜ ６０｜ ６０｜ ６０｜
｜Ｌ（Ｎ／ｍ／秒）｜ ０｜ ０｜ ０｜
｜Ｋ（Ｎ／ｍ） ｜ ６００００｜６００００｜６００００｜
｜Ｂ（Ｎ秒／ｍ） ｜ ２００｜１２０００｜ ２００｜
｜Ｈ（秒） ｜０．００３３｜ ０．２｜ ０．２｜
ただし、ｐ（ｋ）、ｆ（ｋ）などの各変数は２次元ベクトルである。制御器への入力ｐ（ｋ）は、手先効果器の現在位置であり、エンコーダの計測値である。制御器からの出力ｆ（ｋ）は、手先効果器で発生するべき力ベクトルであり、静力学的にそれと等価なトルクがアクチュエータから出力される。各制御器のパラメータ値は表１に示すとおりである。ただし、制御器ＡおよびＢは、Ｈ＝Ｂ／Ｋを満たすので、上限付のＰＤ制御（式（２１））に等価である。制御器Ｃは本発明の最良の形態の一つであり、Ｈ≪Ｂ／Ｋをみたす。なお、サンプリング周期はＴ＝０．００１秒である。
制御器に入力される目標軌道としては、８の字運動のリサジュー曲線
ｐｄ（ｔ）＝［Ａｘ・ｓｉｎ［Ωｘ・ｔ］、Ａｙ・ｓｉｎ［Ωｙ・ｔ］］
を与えた。ただし、Ａｘ＝０．２５ｍ、Ａｙ＝０．１５ｍ、Ωｘ＝２π／３ｒａｄ／秒、Ωｙ＝４π／３ｒａｄ／秒である。
異常からの復帰動作を観察するために、時刻ｔ＝２．０秒から２．５秒の間、アクチュエータへの指令トルクを強制的に零とした。
図５から図８に結果を示す。図５から図８において、灰色の太線は目標位置ｐｄのｙ成分、黒の細い曲線は手先位置の実測値ｐのｙ成分を表す。また、各グラフの下部の折れ線は、系が滑り状態にあるか否かを表す。図８は制御器ＢおよびＣを用いたときの、目標位置ｐｄと実位置ｐの間の偏差である。制御器Ａは追従性を重視したゲインを持つトルク制限つきＰＤ制御であるので、図５において、復帰運動時に大きなオーバーシュートを生じ、その後の振動の減衰にも時間がかかっている。制御器Ｂは収束性を重視したゲインを持つトルク制限つきＰＤ制御であるので、図８によく表れているとおり、正常運動時に偏差が大きくなっている。またこのとき、指令トルクｆが振動的になり（図示せず）、高周波の騒音がアクチュエータから発生することが確認できた。制御器Ｃは本発明の最良の形態の一つであり、正常動作時の追従性と、復帰動作時の収束性との両立ができているのが図７および図８から分かる。
【００２０】
本実施例の２自由度平行リンクマニピュレータが請求項の「移動機構」の一態様に相当する。各アクチュエータに取り付けられた光学エンコーダが請求項の「位置検出器」の一態様に相当する。図３に破線で囲んだ制御器が請求項「指令装置」の一態様に相当する。
【００２１】
ところで、式（１８）の手順は、Ｈ＝Ｂ／ＫおよびＬ＝０を代入すると、
ｕ（ｋ）＝Ｋ・（ｐｄ（ｋ）−ｐ（ｋ））
＋Ｂ・（ｖｄ（ｋ）−ｖ（ｋ）） ・・・（２１ａ）
ｆ（ｋ）＝ｕ（ｋ）；（｜ｕ（ｋ）｜≦Ｆの場合）
または
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）］；（｜ｕ（ｋ）｜＞Ｆの場合） ・・・（２１ｂ）
となる。これは、出力トルクの上限をＦとしたＰＤ制御と等価である。スライディングモード制御のチャタリングを防ぐために、切替面周囲の不連続性を滑らかにする方法も考えられているが、これは本質的にトルク制限付ＰＤ制御（式（２１））と等価である。
正常動作時の局所的な追従性を保ちつつ、異常動作時の広域的な収束性を保つためには、Ｈ≫Ｂ／Ｋと設定するのが望ましい。比例ゲインＫは、正常運転時の追従性を保つために、できるだけ大きく設定するのが望ましい。微分ゲインＢは、発振を防ぎつつ、追従性と即応性を損なわない程度の大きさにとどめるべきである。そして、定数Ｈが大きいほど異常から復帰する動作が緩やかであり、これが小さすぎると目標軌道近傍でオーバーシュートが発生する。発生トルクの上限値Ｆは、関節摩擦とリンク慣性に打ち勝って必要な速度を出すために必要な程度で、かつ、人間や環境に危害を与えない程度の力の大きさの値を設定すればよい。
【００２２】
本実施例において、Ｈ≫Ｂ／Ｋと設定されれば、正常動作時の小さな位置決め誤差からのすばやい復帰と、異常動作時の大きな位置誤差からのゆるやかな復帰を保つことができる。比例ゲインＫは、正常運転時の追従性を保つために、できるだけ大きく設定するのが望ましい。微分ゲインＢは、発振を防ぎつつ、追従性と即応性を損なわない程度の大きさにとどめるべきである。そして、定数Ｈが大きいほど異常から復帰する動作が緩やかであり、これが小さすぎると目標軌道近傍でオーバーシュートが発生する。発生トルクの上限値Ｆは、関節摩擦とリンク慣性に打ち勝って必要な速度を出すために必要な程度で、かつ、人間や環境に危害を与えない程度の力の大きさの値を設定すればよい。
なお、従来のＰＤ制御器はＨ＝Ｂ／Ｋとした場合と等価である。すなわち、正常動作時の追従性を決定するＫおよびＢと、異常動作時の収束性を決定するＨを、独立して設定できるのが、本実施例の効果であるといえる。
また、アクチュエータの発生力の大きさＦを明示的に設定できるという点も、本発明の効果である。
【００２３】
＜実施例２＞
次に実施例２について説明する。この実施例における位置制御装置は、作業者が器具を用いてワークに対して行う作業について、作業者の負担を軽減するものである。作業の例としては、目標軌道（目標位置）に沿って、切断器具によりワークを目標軌道に沿って切断する作業や、接着剤塗布器具によりワーク上の目標軌道に沿って接着剤を塗布する作業などがある。切断器具や接着剤塗布器具（以下、器具と称する）は、移動機構が保持する。
【００２４】
まず、そのような作業の課題を説明する。そのような作業は、作業者が直接に器具を保持し、自らの力だけで器具を目標位置に追従させることは作業者の負担が大きい。作業者にとって、器具の位置を大まかに（広域的に）目標位置へ合わせる作業は容易であるが、器具の位置を目標位置に正確に（局所的に）に追従させる作業は負担となる。
【００２５】
そこで、作業者が行う器具の局所的な移動を補助する位置制御装置を考える。器具は位置制御装置の移動機構に保持させる。移動機構は操作子を備える。器具を広域的に移動させる場合には、作業者が操作子を操作して移動機構に保持された器具を移動させる。一方、位置制御装置は、移動機構が保持した器具の位置を目標位置に正確に追従するように移動機構を制御する。換言すれば、この位置制御装置は、器具の位置を目標位置に追従させる作業者の操作を誘導する装置ということができる。
この場合、器具を移動させるための広域的な動作が支配的な作業フェーズにおいては、作業者の随意的な操作を阻害せずに良好な作業感覚を与えるために、位置制御装置が制御する移動機構には、十分滑らかに動くことが要求される。「滑らか」とは、移動機構の動作に高周波成分が少ないことを意味する。
器具の位置が目標位置に近づいて、器具を目標位置に正確に追従させる際の局所的な（小振幅な）動作が支配的な作業フェーズにおいては、位置制御装置は、十分に高剛性で速応性の高い（例えば、器具の位置が目標位置に一致したときに「カチッ」とラッチするような）動作を移動機構に行わせる必要がある。そうすることによって、高周波かつ小振幅
である作業については作業者の負担を低減することができる。このことを模式的に表したのが図９である。図９の縦軸は器具の移動の振幅（器具を移動させる動作の大きさ）である。横軸は器具を移動させる際の動作の周波数である。振幅が大きくかつ低周波である領域Ａに相当する作業フェーズでは、人間が随意的に器具を移動させる動作を可能にし、振幅が小さくかつ高周波である領域Ｂに相当する作業フェーズでは、目標位置に追従させるための器具の移動を位置制御装置がカバーする。そのためには、位置制御装置が移動機構の保持する器具の位置を制御する際には、広域的な「滑らかさ」と局所的な「高い剛性と速応性」を両立する必要がある。
【００２６】
移動機構を制御する際、従来のＰＤあるいはＰＩＤ制御方式では「滑らかさ」と「速応性」は相反するため、上記の両立は困難であった。
また、「高い剛性」と「滑らかさ」を両立するためには、大きい速度フィードバックゲイン（器具に作用する粘性係数に相当する）を設定する必要がある。これは、「速応性」を損なうだけでなく、速度信号のノイズを増幅してしまうという課題も発生する。
【００２７】
そこで本実施例では、実施例１で説明した位置制御装置を活用する。位置制御装置が備えており、移動機構が物体に加える力の指令値を出力する指令装置内には図３のブロック図に示す制御ロジックが構築されている。その制御ロジックの原理を再度説明するために制御ロジックを構築する出発点となる式を以下に再記する。
ｆ＝Ｋ・（ｐｓ−ｐ）＋Ｂ・（ｖｓ−ｖ） ・・・（６ｂ）
ｆ＝Ｆ・ｓｇｎ［ｐｄ−ｐ−（ｄａ／ｄｔ）
＋Ｈ・（ｖｄ−（ｄ＾２ａ／ｄｔ＾２））］ ・・・（８）
式（８）はａ＝∫（ｐｓ−ｐ）ｄｔであることを考慮すると、
ｆ＝Ｆ・ｓｇｎ［ｐｄ−ｐｓ＋Ｈ・（ｖｄ−ｖｓ）］ ・・・（８ｂ）
と書き直すことができる。また、上記の式（６ｂ）は、前述した式（６）において積分ゲインＬをゼロとしたものである。
上記の式（６ｂ）と式（８ｂ）の夫々の記号の意味は次の通りである。ｐおよびｖは連続時間系で表現された器具の位置および速度である。ｐｓおよびｖｓは連続時間系で表現された仮想質点（プロキシ）の位置および速度である。ｐｄおよびｖｄは連続時間系で表現された目標位置および目標速度である。Ｋは第１の係数である。係数Ｋの物理的な意味は、器具とプロキシを仮想的に連結する連結要素の弾性係数である。Ｂは第２の係数である。係数Ｂの物理的な意味は、連結要素の粘性係数である。即ち、式（６ｂ）は、器具の位置ｐとプロキシの位置ｐｓとが仮想的な連結要素で連結されているときに器具の位置ｐとプロキシの位置ｐｓの間に作用する仮想的な粘弾性力ｆを表している。Ｈは第３の係数を表す。
器具は移動機構に保持されている。式（６ｂ）と式（８ｂ）の左辺ｆは、仮想的な空間内で仮想連結要素が器具へ加える力を表す。この力を移動機構によって器具へ加える。また、プロキシは仮想空間内で目標位置に近づくようにスライディングモード制御されており、式（８ｂ）はその制御則を表す。プロキシの質量はゼロに設定されており、プロキシに仮想的な連結要素から加えられる力（式(６ｂ)のｆ）とスライディングモード制御器から加えられる力（式（８ｂ）のｆ）は常に等しい．なお式（８ｂ）において、変数Ｆは移動機構が発生する力の上限値を表す。この上限値Ｆは、移動機構の機械的な静止摩擦力と同等かそれ以下に設定される。
式（８ｂ）の制御則は、プロキシの状態（ｐｓ、ｖｓ）を、
ｐｄ−ｐｓ＋Ｈ・（ｖｄ−ｖｓ）＝０ ・・・（２３）
という超平面（滑り平面）に引き付けるように働く。式（２３）は、プロキシの位置ｐｓが目標位置ｐｄへ漸近的に時定数Ｈで収束するような動特性を表す。プロキシが式（２３）に従って運動する限り、プロキシの位置ｐｓにはオーバーシュートは発生しない。移動機構に保持された器具は仮想的な連結要素によってプロキシと連結されているため、器具の位置ｐと速度ｖもほぼ式（２３）の動特性に従うことになる。
【００２８】
この制御則の利点は、局所的な応答特性と広域的な応答特性とを独立して設定できる点である。局所的な応答特性とは、比較的小さな位置誤差に対する応答特性であり、これは仮想的な連結要素の粘性係数Ｂと弾性係数Ｋとの比によって決定される。このため、正確な追従のためにＫはなるべく大きく、局所的な速応性のためにＢは振動を抑えられる程度でなるべく小さく設定する必要がある。
一方、広域的な応答特性とは、比較的大きな位置誤差に対する応答特性であり、これは第３の係数Ｈによって決定される。ここでは位置制御装置の目的は作業者の動作を誘導することにある。従って位置制御装置が備えており、器具を保持する移動機構は作業者が操作子を操作する際の随意的な動作と同程度に滑らかな動特性を実現する必要がある。人間の随意的な動作の周波数帯域はおよそ６Hz程度が限界値であり、通常はおよそ２Hz程度といわれている。折点周波数が２Hzの一次遅れ系の時定数は、１／（２π×２)＝０．０８秒である。したがって、望ましい第３の係数Ｈの値はおよそ０．１秒程度と考えることができる。通常の場合、移動機構の局所的な応答特性は、作業者の応答特性よりも高い速応性を必要とするので、Ｈ＞＞Ｂ／Ｋとなるように設定されることが望ましい。
【００２９】
次に実施例２の位置制御装置について説明する。図１０は実施例２の位置制御装置１００の概略図である。この実施例は、塗布器具１１２の先端から吐出する接着剤をワーク２００に設定された目標軌道Ｌに沿って吹き付ける作業である。
この位置制御装置１００は、移動機構１０２と指令装置１２０を備える。
移動機構１０２は、基部１０４と第１リンク１０６ａの一端が第１モータ１０８ａによって回転自在に連結されている。第１リンク１０６ａの他端と第２リンク１０６ｂの一端が第１モータ１０８ｂによって回転自在に連結されている。第２リンク１０６ｂの他端の下側には塗布器具１１２が保持されている。第２リンク１０６ｂの他端の上側には操作子１１４が取り付けられている。操作子１１４は、作業者３００によって把持されて塗布器具１１２の位置を移動させるための操作力を移動機構１０２に伝えるものである。
移動機構１０２は全体として２リンクマニピュレータを形成しており、第２リンク１０６ｂの他端に保持された塗布器具１１２の位置ｐ（ｋ）のワーク２００の平面内での移動を幾何学的に拘束しない。従って、移動機構１０２が発生する力が大きくなければ、移動機構が発生する力は、作業者３００が操作子１１４を操作して塗布器具１１２の位置ｐ（ｋ）を移動させる作業を妨げない。なお、位置ｐ（ｋ）のｋは離散時間系における時刻を表す記号である。
第１モータ１０８ａには第１エンコーダ１１０ａが備えられている。第２モータ１０８ｂには第２エンコーダ１１０ｂが備えられている。第１エンコーダ１１０ａは、基部１０４と第１リンク１０６ａの相対回転角を検出する。第２エンコーダ１１０ｂは、第１リンク１０６ａと第２リンク１０６ｂの相対回転角を検出する。第１エンコーダ１１０ａと第２エンコーダｂの検出値と、移動機構１０２の幾何学的な構造と寸法から、塗布器具１１２の先端の位置ｐ（ｋ）が求められる。
【００３０】
指令装置１２０は移動機構１０２と電気的に接続されている。指令装置１２０には第１エンコーダ１１０ａ及び第２エンコーダ１１０ｂの検出値が入力される。指令装置１２０からは移動機構１０２が塗布器具１１２に加える力の指令値ｆ（ｋ）が出力される。ここで「移動機構１０２が塗布器具１１２に加える力の指令値ｆ（ｋ）」は、実際には、移動機構１０２が塗布器具１１２に加える力を発生するために、第１モータ１０８ａと第２モータ１０８ｂが出力するトルク指令値として出力される。移動機構１０２が塗布器具１１２に加える力ｆ（ｋ）と、第１モータ１０８ａ及び第２モータ１０８ｂが出力するトルクとは良く知られるヤコビ行列によって相互に変換可能である。従って、移動機構１０２が塗布器具１１２に加える力と、第１モータ１０８ａ及び第２モータ１０８ｂが出力するトルクは等価である。
【００３１】
目標軌道Ｌは、目標位置ｐｄ（ｋ）の経時的なデータとして表される。そのデータは、指令装置１２０内の記憶装置（不図示）に記憶されている。塗布器具１１２の目標速度ｖｄ（ｋ）が設定されている場合には、目標速度ｖｄ（ｋ）の経時的なデータも記憶装置に記憶されていてもよい。また、目標速度ｖｄ（ｋ）の経時的なデータを記憶する代わりに、目標位置ｐｄ（ｋ）の経時的なデータのうち、幾つかの経時的に連続した目標位置ｐｄ（ｋ）のデータからそれらの位置の差分によって目標速度ｖｄ（ｋ）を求めてもよい。
この実施例の作業では、各時刻ｋにおいて、塗布器具１１２の先端の位置ｐ（ｋ）を目標軌道Ｌ上の目標位置ｐｄ（ｋ）に追従させる必要がある。そこで指令装置１２０内には、図３に示すブロック図の制御ロジックが構築されている。
【００３２】
指令装置１２０内で実際に実行される制御ロジックを再記する。
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋ｅ（ｋ−１）・（Ｈ・Ｋ−Ｂ）／（Ｈ＋Ｔ） ・・・（２２ｆ）
ｆ（ｋ）＝ｕ（ｋ）；（｜ｕ（ｋ）｜≦Ｆの場合） または、
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）］；（｜ｕ（ｋ）｜＞Ｆの場合） ・・・（２２ｃ）
ｅ（ｋ）＝（Ｂ・ｅ（ｋ−１）＋Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ） ・・・（２２ｄ）
ｅ（ｋ）＝ｐｓ（ｋ）−ｐ（ｋ） ・・・（２２ｅ）
式（２２ｆ）、式（２２ｃ）、式（２２ｄ）に表れる記号の意味は以下の通りである。ｕ（ｋ）は、時刻ｋにおける暫定制御力を表す。ｆ（ｋ）は、時刻ｋにおいて指令装置１２０が移動機構１０２へ指令する力指令値を表す。力指令値ｆ（ｋ）は、移動機構１０２が塗布器具１１２へ加えるべき力である。
ｐｄ（ｋ）とｖｄ（ｋ）は時刻ｋにおける目標位置と目標速度を表す。ｐ（ｋ）とｖ（ｋ）は時刻ｋにおける塗布器具１１２の位置と速度を表す。Ｔは制御周期を表す。ＫとＢは式（６ｂ）で説明した通り仮想的な連結要素の弾性係数と粘性係数を表す。Ｈは第３の係数を表す。
Ｆは移動機構１０２が塗布器具１１２に加える力の上限値を表す。この上限値Ｆは、指令装置１２０が出力する力指令値のリミッタとして設定される。符号関数ｓｇｎ［ｘ］は、ｘ＝０のときに閉区間［−１、１］の任意の値を取りうると定義している。
ｅ（ｋ）は制御ロジック内の内部状態変数である。その物理的な意味は、式（２２ｅ）に示されるように、時刻ｋにおける仮想質点（プロキシ）の位置ｐｓ（ｋ）と塗布器具１１２の位置ｐ（ｋ）の偏差である。但し、ｅ（ｋ）は式（２２ｅ）で計算されることはなく、式（２２ｄ）の漸化式によって計算できる。ｅ（ｋ）が式（２２ｄ）の漸化式で計算されるのでｅ（ｋ）を内部状態変数とみることができる。
なお、本実施例では、塗布器具１１２の速度ｖ（ｋ）は、（ｐ（ｋ）−ｐ（ｋ−１））／Ｔで算出される。ｐ（ｋ）は第１エンコーダ１１０ａ、第２エンコーダ１１０ｂの出力値と移動機構１０２の幾何学的構造と寸法から算出される。従って本実施例では塗布器具１１２の速度ｖ（ｋ）を検出する検出器は必要ない。なお、塗布器具１１２の速度ｖ（ｋ）を位置ｐ（ｋ）の経時的なデータから算出する代わりに、速度センサなど、物体の速度を検出するセンサを配置してそのセンサから物体の速度を直接検出してもよい。
【００３３】
次に位置制御装置１００が実行する処理を図１１のフローチャート図を参照して説明する。
ステップＳ１００では、第１エンコーダ１１０ａ、第２エンコーダ１１０ｂによって塗布器具１１２の位置ｐ（ｋ）を検出する。ステップＳ１０２では前述したように、塗布器具１１２の時刻ｋでの位置ｐ（ｋ）と時刻ｋ−１での位置ｐ（ｋ−１）から塗布器具１１２の速度ｖ（ｋ）を算出する。ステップＳ１０４では時刻ｋにおける目標位置ｐｄ（ｋ）と目標速度ｖｄ（ｋ）を取得する。目標位置と目標速度の経時的なデータは指令装置１２０内の記憶装置に予め記憶されている。
次にステップＳ１０６では、式（２２ｆ）に従って暫定制御力ｕ（ｋ）を算出する。
次にステップＳ１０８では、暫定制御力ｕ（ｋ）の大きさと上限値Ｆを比較する。本実施例では、塗布器具１１２はワーク２００の平面内で移動可能であるので暫定制御力ｕ（ｋ）は２次元ベクトルとなる。２次元ベクトルの場合の大きさはそのベクトルのノルムで定義される。
比較の結果、暫定制御力ｕ（ｋ）の大きさが上限値Ｆ以下の場合には（ステップＳ１０８：ＹＥＳ）、ステップＳ１１０によって、移動機構１０２へ出力する力の指令値ｆ（ｋ）を暫定制御力ｕ（ｋ）にセットする。比較の結果、暫定制御力ｕ（ｋ）の大きさが上限値Ｆを超える場合には（ステップＳ１０８：ＮＯ）、ステップＳ１１２によって移動機構１０２へ出力する力の指令値ｆ（ｋ）は、方向が暫定制御力ｕ（ｋ）ベクトルと同じであり、大きさがＦの値にセットする。
ステップＳ１１０又はステップＳ１１２でセットされた力の指令値ｆ（ｋ）は指令装置１２０から移動機構１０２へ出力される。最後にステップＳ１１６では、次の制御周期の際に式（２２ｆ）の計算に必要なｅ（ｋ）の値を式（２２ｄ）により計算する。以上の処理を制御周期Ｔ毎に繰り返す。
【００３４】
本実施例では上限値Ｆの大きさは、移動機構１０４の機械的な静止摩擦力より小さい値に設定される。これによって、塗布器具１１２が静止している状態で移動機構１０４が出力できる最大の大きさの力Ｆを塗布器具１１２に加えるように動作しても、力Ｆが静止摩擦力に打ち勝つことができず塗布器具１１２は移動しない。塗布器具１１２は、作業者３００が操作子１１４を操作して、塗布器具１１２の位置ｐ（ｋ）が目標位置ｐｄ（ｋ）に近づく方向に操作力を加えたときに始めて移動する。
指令装置１２０が移動機構１０２に出力する力の指令値ｆ（ｋ）は、図１０に示すように、塗布器具１１２の位置ｐ（ｋ）を目標位置ｐｄ（ｋ）に一致させる向きに作用する。従って作業者３００が操作子１１４に対して塗布器具１１２の位置ｐ（ｋ）を目標位置ｐｄ（ｋ）に近づける方向に操作力Ｆｓを加えると、操作力Ｆｓと移動機構１０２が出力する力指令値ｆ（ｋ）の合力によって、塗布器具１１２の位置ｐ（ｋ）は滑らかに目標位置ｐｄ（ｋ）へ移動する。力指令値ｆ（ｋ）の大きさの上限値Ｆは、塗布器具１１２と移動機構１０４の機械的な静止摩擦力より小さい値に設定されている。一般に静止摩擦力は動摩擦力よりも大きい。従って、塗布器具１１２がひとたび目標位置ｐｄ（ｋ）の方向へ向って動き出すと、移動機構１０２が発生する力ｆ（ｋ）のみによって、あるいはｆ（ｋ）とＦｓの合力によって、塗布器具１１２の位置ｐ（ｋ）は目標位置ｐｄ（ｋ）に追従するようになる。作業者３００は、移動機構１０２が発生する力ｆ（ｋ）によって、塗布器具１１２の位置ｐ（ｋ）を目標位置ｐｄ（ｋ）に追従させるという局所的な動作の負担を軽減される。さらに前述したように、指令値１２０が出力する力指令値ｆ（ｋ）は塗布器具１１２の位置ｐ（ｋ）の目標位置ｐｄ（ｋ）への追従に対してオーバーシュートを生じることがない。その上、指令値１２０が出力する力指令値ｆ（ｋ）がチャタリングを発生することもない。
また仮に作業者３００が操作子１１４に加える操作力Ｆｓの方向が目標軌道ｐｄ（ｋ）に向かう方向と異なる場合には、移動機構１０２が発生する力ｆ（ｋ）の方向と操作力Ｆｓの方向が異なることになる。従って作業者３００は、目標位置ｐｄ（ｋ）へ向う方向以外の方向へは操作子１１４を動かし難くなる。このことは換言すれば、作業者の行う動作は、位置制御装置１００によって、塗布器具１１２の位置ｐ（ｋ）を目標位置ｐｄ（ｋ）に追従させるように誘導されるということもできる。
一方、塗布器具１１２の位置ｐ（ｋ）と目標位置ｐｄ（ｋ）が大きく離れている場合には式（２２ｆ）からわかる通りｕ（ｋ）は大きな値となる。しかし指令装置１２０が出力する力指令値ｆ（ｋ）の大きさは、塗布器具１１２と移動機構１０４の機械的な静止摩擦力より小さい値である上限値Ｆを超えることはない。塗布器具１１２の位置ｐ（ｋ）と目標位置ｐｄ（ｋ）が大きく離れている場合に、移動機構が出力する力ｆ（ｋ）は、作業者３００が操作子１１４に操作力を加えて塗布器具１１２を広域的に移動させる作業を妨げない。
なお、図１１のフローチャートの処理は、実施例１の位置制御装置についても同様に実行される。実施例１の位置制御装置と実施例２の位置制御装置は同じ処理を実行する。実施例１の位置制御装置と実施例２の位置制御装置で異なるのは、実施例２の位置制御装置１００の移動機構１０２が操作子１１４を有することと、実施例２の位置制御装置では、力の上限値Ｆが移動機構１０４の機械的な静止摩擦力より小さい値に設定されることである。
また、本実施例では、力の上限値Ｆを移動機構１０４の機械的な静止摩擦力より小さい値に設定したが、力の上限値Ｆは、移動機構１０４の機械的な静止摩擦力と同等かそれ以下の値であればよい。
また、図１１のフローチャートに示した処理は、コンピュータによって実行されるプログラムの形式で提供されるものであってもよい。その場合には、より厳密には、図１１のステップＳ１００は、第１エンコーダ１１０ａ、第２エンコーダ１１０ｂによって検出された物体の位置を取得するステップとなる。
【００３５】
実施例２の位置制御装置１００の効果を調べるための実験を行った。移動機構には、図４に示した水平面内で稼動する２自由度平行リンクマニピュレータを用いた。マニピュレータの稼動領域の直下には液晶モニタを水平に設置した。被験者はマニピュレータの手先効果器に取り付けられたグリップ（操作子）を握り、液晶モニタに表示された目標位置に追従するように、マニピュレータの手先を動かす。即ち、マニピュレータの手先位置が物体の位置ｐに相当する。
被験者は一回の試行において、幅０．５ｍ、奥行き０．３ｍの「∞」形の目標軌道を１周期あたり４秒で２周期（８秒）の追従動作を行った。なお、この運動は縦横の周波数比が１：２のリサジュー運動である。マニピュレータの手先位置の直交座標に対して、指令装置は式（２２ｆ）、式（２２ｃ）で算出される力指令値ｆ（ｋ）をマニピュレータに対して出力する。ただしマニピュレータの手先に発生させる力の上限値は、手先の並進力ではなく、各軸の関節トルクに対して設けた。制御則のパラメータ（Fτ、Ｈ、Ｋ、Ｂ）は、図１２に示すＣ０からＣ５の６種類の組み合わせ（条件）を用いた。なお、Ｆτは関節トルクの大きさの上限値である。条件Ｃ０においてはＦτ＝０であるので、マニピュレータはトルクを発生しない。条件Ｃ１からＣ５においてはＦτ＝７Ｎｍであるが、各関節の減速器におよそ１０Ｎｍの摩擦力がはたらくので、被験者から力が加えられなければマニピュレータはほとんど動かない。一人の被験者は各条件の下で１回ずつ、合計６回の試行を行った。被験者は８人の健康な男性とした。
一回の試行の結果は、メートルを単位とした追従誤差の大きさの自然対数の時間平均（以下、Ｌ値と呼ぶ）を用いて評価した。ただし、被験者間の身体能力の差異の影響を除くために、各試行のＬ値から各被験者内の平均Ｌ値を減じた値を、各試行の成績（以下、Ｎ値と呼ぶ）とした。図１３に、条件Ｃ０からＣ５の下での、Ｎ値の平均および標準偏差を示す。大きいＮ値は大きい追従誤差を意味する。「*」および「**」はそれぞれ、ｐ＜０．０５およびＰ＜０．０１の有意差を表す。図１３から、条件Ｃ０におけるＮ値が他の条件におけるＮ値より有意に大きいことが分かる。これは、弱い力の運動誘導によっても軌道追従の誤差を低減する効果があるということを示している。
条件Ｃ１、Ｃ２およびＣ３はＫ値が同じでＨ値が異なるが、前述したように、Ｈ値が中間（０．１秒）である条件Ｃ２が最も良い結果となっている。Ｈ値が小さい（０．０１秒）と誘導動作の速応性が高くなり、被験者が誘導動作に従えなくなる。これによってオーバーシュートが発生し、追従誤差が増加する。一方で、大きすぎるＨ値（０．５秒）は速応性の低下を招くため、これも追従誤差の増加の原因となる。条件Ｃ２、Ｃ４およびＣ５はＨ値が同じでＫ値が異なるが、Ｋ値が最も大きい条件Ｃ２が最も良い結果となっている。これは、大きいＫ値、すなわち高い剛性は、正確な追従に必要であるということを表している。なおこの制御則は、Ｈ＝Ｂ／Ｋを満たすとき、マニピュレータの発生力に上限があるＰＤ制御と等価になる。ＨとＫの値を条件Ｃ２と同じにし、Ｈ＝Ｂ／Ｋを満たすようにＢ値を設定すると、Ｂ値が大きくなりすぎたため、マニピュレータは速度信号のノイズに起因する騒音を発生した。これは、条件Ｃ２と同様の動特性を実現するためには、本実施例で提案した、式（２２ｆ）、式（２２ｃ）で算出される力指令値ｆ（ｋ）を算出する位置制御装置が有効であるということを示している。
【００３６】
以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。本発明は一般的な位置制御装置や位置制御方法などに適用可能である。また、実施例２のように作業者の作業を誘導する位置制御装置は、例えば、人間への技能・運動教示や、リハビリテーションなどへの適用も可能である。
【００３７】
本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。
【図面の簡単な説明】
【００３８】
【図１】最も簡単なスライディングモード制御を示す模式図である。
【図２】本発明の位置制御装置の物理的解釈を示す模式図である。
【図３】本発明の位置制御装置を示すブロック図である。
【図４】実験に用いたマニピュレータの模式図である。
【図５】制御器Ａ（従来技術の一例）を用いた場合の実験結果を示す図である。
【図６】制御器Ｂ（従来技術の一例）を用いた場合の実験結果を示す図である。
【図７】制御器Ｃ（本発明の最良の形態の一つ）を用いた場合の実験結果を示す図である。
【図８】制御器ＢおよびＣを用いた場合の位置決め誤差を示す図である。
【図９】人間が随意的に動作可能な領域と位置制御装置がカバーすべき動作の領域を示す模式図である。
【図１０】実施例２の位置制御装置の模式図である。
【図１１】実施例２の位置制御装置が実行する処理のフローチャート図である。
【図１２】実施例２の位置制御装置の効果を検証する実験における変数の値を示す図である。
【図１３】実施例２の位置制御装置の効果を検証する実験の結果を表す図である。
【符号の説明】
【００３９】
１００：位置制御装置
１０２：移動機構
１０４：基部
１０６ａ：第１リンク
１０６ｂ：第２リンク
１０８ａ：第１モータ
１０８ｂ：第２モータ
１１０ａ：第１エンコーダ
１１０ｂ：第２エンコーダ
１１２：塗布器具
１１４：操作子
１２０：指令装置
２００：ワーク
３００：作業者
Ｌ：目標軌道
【技術分野】
【０００１】
本発明は、物体の位置を目標位置に追従させる位置制御装置、位置制御方法および位置制御プログラムに関する。
【背景技術】
【０００２】
物体を保持し、物体に加える力の指令値が入力され、入力された指令値を発生する移動機構を有し、物体の位置を目標位置に追従させるように力の指令値を出力することによって移動機構を制御し、物体の位置を目標位置に追従させる位置制御装置が知られている。本明細書では、物体の位置という場合にはその物体の位置及び／又はその物体の姿勢を意味するものとし、速度という場合には並進速度及び／又は角速度を意味するものとし、力という場合には並進力及び／又はトルクを意味するものとする。
位置制御装置の典型例である産業用ロボットは、一般に、関節摩擦などのモデル化しにくい機械要素を多く含む。このため、力制御やインピーダンス制御を行う際にも、高ゲインのＰＤ（比例・微分）またはＰＩＤ（比例・積分・微分）制御方式の位置制御器を最下層のフィードバックループとして用いることが多い。高ゲインのＰＤ制御またはＰＩＤ制御方式の位置制御器を用いることによって、ロボットの動特性や非線形要素の影響を低減し、ロボットの手先の現在位置を目標位置に精度よく追従させることができることがよく知られている。
また、力制御やインピーダンス制御の他に、スライディングモード制御という制御法が知られている（非特許文献１）。スライディングモード制御は、状態空間に設計した切換面上に状態を拘束することによって希望の動特性を達成する制御法である。これはいわゆるバンバン制御の１つの一般化であり、不連続な制御出力を発生する。外乱に対してロバストであるということがスライディングモード制御の利点である。
【０００３】
【非特許文献１】野波 健蔵、田 宏奇、スライディングモード制御−非線形ロバスト制御の設計理論、コロナ社、１９９４年
【発明の開示】
【発明が解決しようとする課題】
【０００４】
高ゲインのＰＤまたはＰＩＤ位置制御は、次のような課題がある。たとえば、環境との接触や、ロボットが有するアクチュエータの一時的なパワー低下などの異常によって、位置制御器の目標位置とロボットの手先位置との偏差が大きくなった場合、大きな復元力によって環境が損傷を受けたり、復帰動作の際に過大な速度が発生したりすることがある。即ち、何らかの要因による異常時にロボットが予想外の動作をする可能性がある。
位置制御されているロボットで、正常時の追従性と異常時の予想外の動作の防止とを両立する問題は単純ではない。まず、アクチュエータの出力トルクに上限を設けることは必要条件である。そして、異常状態から復帰する際にオーバーシュートを起こさずに目標軌道へゆるやかに収束する必要がある。これは、正常時の即応性・追従性と相反する要求であるといえる。ＰＤ制御またはＰＩＤ制御において収束性を確保するためには、Ｄ（微分）ゲインを大きくする必要があるが、これは正常運転時の即応性を低下させ、また、特に低速運動時に高周波振動の原因となる。また、Ｉ（積分）ゲインを大きくするとオーバーシュートが生じやすくなる。このように、正常動作時の追従性と異常時の予想外の動作の防止との両立は、ＰＤ制御またはＰＩＤ制御のゲイン設計とトルク制限では対処できない問題を含んでいる。
本発明は、上記の課題を解決する。本発明は、物体を保持した移動機構に対して、正常動作時の追従性と異常時の予想外の動作の防止とを両立する単純な位置制御装置および位置制御方法を提供する。本発明は、物体の位置を目標位置に追従させるための汎用性の高い技術を提供する。
【課題を解決するための手段】
【０００５】
本発明は、一般によく知られているスライディングモード制御という制御法を着想の原点としている。本発明は、スライディングモード制御が本来持っている欠点を取り除き、その利点を応用した新たな制御方法である。
【０００６】
まず、スライディングモード制御について概説する。スライディングモード制御は、状態空間に設計した切換面上に状態を拘束することによって希望の動特性を達成する制御法である。これはいわゆるバンバン制御の１つの一般化であり、不連続な制御出力を発生する。外乱に対してロバストであるということがこれの本来の利点であるが、アクチュエータ出力（これは、ロボットが保持した物体に加える力と等価である）に明示的に上限を設できるという点と、状態を希望の動特性に拘束できるという点は、異常時の予想外の動作の防止と追従性の両立という目的にもかなっている。すなわち、トルク制限、正常動作時の追従性、および復帰動作時の収束性をすべて実現できる可能性がある。しかし、スライディングモード制御の理論は、無限に高速に応答する連続時間系を前提としているので、通常の離散時間系に実装された場合には、切換の繰り返しによって高周波振動（チャタリング）が発生する。
ここでは簡単のため、１自由度の位置制御系を考える。制御対象（ロボットが保持する物体）は質量Ｍの質点とし、その現在位置および現在速度をそれぞれｐおよびｖとする。制御器はｐおよびｖを測定し、アクチュエータから力ｇを発生する。ここで、「アクチュエータが発生する力ｇ」は、ロボットが保持している制御対象に加わる力ｇと等価であるとする。制御器が設定する目標位置と目標速度をそれぞれｐｄおよびｖｄとする。質点はアクチュエータからの力ｇと外力−ｆを受けて運動する。すなわち質点の運動方程式は下記のようになる。
Ｍ・ｄｖ／ｄｔ＝ｇ−ｆ ・・・（１）
最も単純なスライディングモード制御器は、たとえば図１のような構造を持つ。ここでアクチュエータからの出力ｇは、
ｇ＝Ｆ・ｓｇｎ［ｐｄ−ｐ＋Ｈ・（ｖｄ−ｖ）］ ・・・（２）
と決定される。ただし、ここでｓｇｎ［・］は符号関数であり、
ｓｇｎ［ａ］＝１；（ａ＞０の場合）、−１；（ａ＜０の場合） ・・・（３）
である。また、ＦおよびＨは正の実数である。関数φ（ｐ、ｖ）＝ｐｄ−ｐ＋Ｈ・（ｖｄ−ｖ）を定義すると、状態空間（ｐ−ｖ平面）内において、φ（ｐ、ｖ）＝０は超平面（直線）を構成する。この超平面は切替平面と呼ばれ、これを境界として、式（２）に示すように、制御器の出力が不連続に切り替えられる。この切換が無限に高速であれば、この系は切替平面φ（ｐ、ｖ）＝０に有限時間内に到達する。そして、切替平面に到達した後は、外力ｆと質点の慣性力の合力の大きさがＦを超えない限り、系は切替平面内に拘束される。切替平面内に拘束されている間、制御対象はφ（ｐ、ｖ）＝０、すなわち
ｄｐ／ｄｔ＝ｖ＝（ｐｄ−ｐ）／Ｈ＋ｖｄ ・・・（４）
という微分方程式に従って推移する。この状態を滑り状態（ｓｌｉｄｉｎｇ ｍｏｄｅ）と呼ぶ。
この方式で、アクチュエータの出力ｇの上限は定数Ｆとして明示的に与えることができる。また、復帰運動時の挙動は定数Ｈにより決定できる。そして、切替が無限に高速であれば、完全な追従性が得られる。しかし、離散時間系では切替に時間遅れが生じるため、切換面交差の繰り返しによって高周波振動（チャタリング）が発生する。したがって、図１の形の制御系は、現実の制御対象には用いることが難しい。
【０００７】
そこで本発明では、図２の系を考える。ここでは、スライディングモード制御器は、制御対象に直接力を加えるわけではなく、計算器内に仮想的に想定された「プロキシ（代理点）」に力を加えるものとする。スライディングモード制御器に設定された目標位置および目標速度をそれぞれｐｄおよびｖｄとし、プロキシの位置および速度をそれぞれｐｓおよびｖｓとする。前節と同じ制御則を用いると、スライディングモード制御器の発生力ｇは、
ｇ＝Ｆ・ｓｇｎ［ｐｄ−ｐｓ＋Ｈ・（ｖｄ−ｖｓ）］ ・・・（５）
となる。プロキシと実際の制御対象は、仮想的な保持要素（ｖｉｒｔｕａｌ ｃｏｕｐｌｉｎｇ）で結合されており、この仮想保持要素の発生する力は
ｆ＝Ｌ・∫（ｐｓ−ｐ）ｄｔ＋Ｋ・（ｐｓ−ｐ）＋Ｂ・（ｖｓ−ｖ） ・・・（６）
となる。ただし、位置および速度をそれぞれｐおよびｖとしている。力ｆはアクチュエータから出力されて実制御対象に加えられるが、その反力はプロキシにも加えられるものとする。そうすると、プロキシの質量をＭとすると、プロキシの運動方程式は下記のようになる。
Ｍ・ｄ＾２ｐｓ／ｄｔ＾２＝ｇ−ｆ ・・・（７）
ここで、式（５）（６）（７）にプロキシの質量Ｍ＝０を代入すると、
ｆ＝Ｆ・ｓｇｎ［ｐｄ−ｐ−（ｄａ／ｄｔ）
＋Ｈ・（ｖｄ−（ｄ＾２ａ／ｄｔ＾２））］ ・・・（８）
ｆ＝Ｌ・ａ＋Ｋ（ｄａ／ｄｔ）＋Ｂ・（ｄ＾２ａ／ｄｔ＾２） ・・・（９）
ｇ＝ｆ ・・・（１０）
が得られる。ただし、ａ＝∫（ｐｓ−ｐ）ｄｔである。この系のブロック線図は図３のようになる。以後、簡単のため、
ｓ＝（ｐｄ−ｐ）＋Ｈ・（ｖｄ−ｖ） ・・・（１１ａ）
ｆ＝Ｆ・ｓｇｎ［ｓ−（ｄａ／ｄｔ）−Ｈ・（ｄ＾２ａ／ｄｔ＾２）］
・・・（１１ｂ）
ｆ＝Ｌ・ａ＋Ｋ・（ｄａ／ｄｔ）＋Ｂ・（ｄ＾２ａ／ｄｔ＾２） ・・・（１１ｃ）
と表す。これは、ｐｄ、ｖｄ、ｐおよびｖを入力とし、ｆを出力とし、ａを内部状態変数とする制御器と解釈できる。
式（１１）の離散時間表現は、下記のようになる。
ｓ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））＋Ｈ・（ｖｄ（ｋ）−ｖ（ｋ））
・・・（１２ａ）
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｓ（ｋ）−（ａ（ｋ）−ａ（ｋ−１））／Ｔ−Ｈ・（ａ（ｋ）−２・ａ（ｋ−１）＋ａ（ｋ−２））／Ｔ／Ｔ］ ・・・（１２ｂ）
ｆ（ｋ）＝Ｌ・ａ（ｋ）＋Ｋ・（ａ（ｋ）−ａ（ｋ−１））／Ｔ＋Ｂ・（ａ（ｋ）−２・ａ（ｋ−１）＋ａ（ｋ−２））／Ｔ／Ｔ ・・・（１２ｃ）
ここで、括弧内の記号は整数であり、時刻をあらわす。また、時間微分（ｄａ／ｄｔ）はオイラー差分（ａ（ｋ）−ａ（ｋ−１））／Ｔに置き換えている。
式（１２）に出現する変数のうち、未知数はｆ（ｋ）とａ（ｋ）である。これらを求めるためには、式（１２ｂ）と式（１２ｃ）を代数的に解く必要がある。まず、式（１２ｃ）を変形すると
ａ（ｋ）＝（（２・Ｂ＋Ｋ・Ｔ）・ａ（ｋ−１）−Ｂ・ａ（ｋ−２）
＋Ｔ・Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＝ａ（ｋ−１）・（Ｂ＋Ｋ・Ｔ）／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋（ａ（ｋ−１）−ａ（ｋ−２））・Ｂ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋ｆ（ｋ）・Ｔ・Ｔ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ） ・・・（１３）
が得られる。これを式（１２ｂ）に代入すると、ａ（ｋ）を消去した式が出現する。
ｆ（ｋ）＝Ｆ・ｓｇｎ［（ｕ（ｋ）−ｆ（ｋ））・（Ｈ＋Ｔ）／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）］ ・・・（１４）
ただし、
ｕ（ｋ）＝ｓ（ｋ）・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）＋ａ（ｋ−１）・Ｌ
＋（ａ（ｋ−１）−ａ（ｋ−２））・（Ｈ・（Ｋ＋Ｌ・Ｔ）−Ｂ）／（Ｈ＋Ｔ）／Ｔ ・・・（１５）
である。式（１４）はすなわち、
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）−ｆ（ｋ）］ ・・・（１６）
である。
簡単な考察により、代数方程式（１６）は、
ｆ（ｋ）＝ｕ（ｋ）；（｜ｕ（ｋ）｜≦Ｆの場合）
または
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）］；（｜ｕ（ｋ）｜＞Ｆの場合） ・・・（１７）
と書き直すことができるということが分かる。ただしここでの符号関数ｓｇｎ［ｘ］はｘ＝０のときに閉区間［−１、１］の任意の値を取りうると定義している。
式（１７）によりｆ（ｋ）が求められれば、式（１３）によりａ（ｋ）が求められる。結局、式（１２）に基づいて値を更新するためには、下記の手順で演算を行えばよい。
ｓ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））＋Ｈ（ｖｄ（ｋ）−ｖ（ｋ））・・・（１８ａ）
ｕ（ｋ）＝ｓ（ｋ）・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）＋ａ（ｋ−１）・Ｌ
＋（ａ（ｋ−１）−ａ（ｋ−２））・（Ｈ・（Ｋ＋Ｌ・Ｔ）−Ｂ）／（Ｈ＋Ｔ）／Ｔ
・・・（１８ｂ）
ｆ（ｋ）＝ｕ（ｋ）；（｜ｕ（ｋ）｜≦Ｆの場合）
または
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）］；（｜ｕ（ｋ）｜＞Ｆの場合） ・・・（１８ｃ）
ａ（ｋ）＝（（２・Ｂ＋Ｋ・Ｔ）・ａ（ｋ−１）−Ｂ・ａ（ｋ−２）
＋Ｔ・Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ） ・・・（１８ｄ）
なお、式（１８ａ）を式（１８ｂ）に代入すると次式が得られる。
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋ａ（ｋ−１）・Ｌ
＋（ａ（ｋ−１）−ａ（ｋ−２））・（Ｈ・（Ｋ＋Ｌ・Ｔ）−Ｂ）／（Ｈ＋Ｔ）／Ｔ ・・・（１８ｅ）
この系において制御器は、
φ（ｐｓ（ｋ）、ｖｓ（ｋ））＝（ｐｄ（ｋ）−ｐｓ（ｋ））
＋Ｈ・（ｖ（ｋ）−ｖｓ（ｋ）） ・・（１９）
を有限時間で零に到達させる役割を持つ。式（１９）で
ｐｓ（ｋ）＝ｐ（ｋ）＋（ａ（ｋ）−ａ（ｋ−１））／Ｔ
ｖｓ（ｋ）＝ｖ（ｋ）＋（ａ（ｋ）−２・ａ（ｋ−１）＋ａ（ｋ−２））／Ｔ／Ｔ
を考慮に入れると、
φ（ｐｓ（ｋ）、ｖｓ（ｋ））＝（ｕ（ｋ）−ｆ（ｋ））・（Ｈ＋Ｔ）／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ） ・・・（２０）
が成立する。式（１８ｃ）より、ｆ（ｋ）とｕ（ｋ）の符号は常に一致し、ｆ（ｋ）の絶対値はｕ（ｋ）の絶対値を超えることがない。したがって、ｆ（ｋ）は常にφ（ｐｓ（ｋ）、ｖｓ（ｋ））を零に引き付ける方向に働き、φ（ｐｓ（ｋ）、ｖｓ（ｋ））の符号を逆転させることがない。これは、ｆ（ｋ）の存在によってｐｓ（ｋ）が振動的にならないことを意味する。これにより、プロキシの状態が有限時間で滑り状態（φ（ｐｓ、ｖｓ）＝０）に到達する。厳密にいうと、実制御対象の状態量（ｐ、ｖ）は滑り平面に到達しない。しかし、実制御対象はプロキシに常に拘束されているので、プロキシが滑り状態にあるとき、系全体として滑り状態にあると言ってよい。式（１８）において｜ｕ（ｋ）｜≦Ｆであるとき、この系は滑り状態にある。
【０００８】
即ち、本発明では、動力によって動作する移動機構（上記説明の「アクチュエータ」に相当する）に対して保持した物体（上記説明の「実制御対象」に相当する）に加える力の指令値を出力する指令装置（上記説明の「制御器」に相当する）が力の指令値を計算する際に、質量ゼロの仮想質点（上記説明の「プロキシ」に相当する）と、一端が物体の検出された位置に連結されており、他端が仮想質点に連結されている仮想的な連結要素を仮想空間内に設定する。そしてスライディングモード制御則に基づいて、仮想質点の位置を目標位置に追従させる制御を仮想空間内で実行した際に、仮想的な保持要素から物体に加えられる力を算出する。算出された力を移動機構の動力源への指令値とする。
【０００９】
指令装置は、時刻ｋにおける力の指令値ｆ（ｋ）を算出する過程で、指令値ｆ（ｋ）を算出するための暫定制御力ｕ（ｋ）を算出する。暫定制御力ｕ（ｋ）は、例えば、式（１８ｅ）で求めることができる。時刻ｋ＋１において式（１８ｅ）によって暫定制御力ｕ（ｋ＋１）を求める場合には、式（１８ｅ）の中に未知数ａ（ｋ）が表れる。このａ（ｋ）は、式（１３）によって求めることができる。
【００１０】
上記の説明ではＰＩＤ制御方式の仮想保持部材を用いた手法を導いたが、Ｌ＝０とすることによってＰＤ制御方式の仮想保持部材を用いた手法を導くことも可能である。これは下記のようになる。
ｓ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））＋Ｈ（ｖｄ（ｋ）−ｖ（ｋ））・・・（２２ａ）
ｕ（ｋ）＝（ｓ（ｋ）・（Ｂ＋Ｋ・Ｔ）＋ｅ（ｋ−１）・（Ｈ・Ｋ−Ｂ））／（Ｈ＋Ｔ） ・・・（２２ｂ）
ｆ（ｋ）＝ｕ（ｋ）；（｜ｕ（ｋ）｜≦Ｆの場合）
または
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）］；（｜ｕ（ｋ）｜＞Ｆの場合）・・ ・（２２ｃ）
ｅ（ｋ）＝（Ｂ・ｅ（ｋ−１）＋Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ） ・・・（２２ｄ）
ただしここで、ｅ（ｋ）＝（ａ（ｋ）−ａ（ｋ−１））／Ｔに相当する。さらに、ａ（ｋ）はａ（ｋ）＝∫（ｐｓ（ｋ）−ｐ（ｋ））ｄｔであるのでｅ（ｋ）は、
ｅ（ｋ）＝ｐｓ（ｋ）−ｐ（ｋ） ・・・（２２ｅ）
とも表すことができる。
また、式（２２ａ）を式（２２ｂ）に代入すると、
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋ｅ（ｋ−１）・（Ｈ・Ｋ−Ｂ）／（Ｈ＋Ｔ） ・・・（２２ｆ）
となる。即ち、暫定制御力ｕ（ｋ）を、式（１８ｅ）の代わりに式（２２ｆ）によって求めてもよい。このとき時刻ｋ＋１において式（２２ｆ）によって暫定制御力ｕ（ｋ＋１）を求める場合には、式（２２ｆ）の中に未知数ｅ（ｋ）が表れる。このｅ（ｋ）は、式（２２ｄ）によって求めることができる。
【００１１】
指令装置は、上記のように算出された暫定制御力ｕ（ｋ）を用いて移動機構へ出力する力の指令値ｆ（ｋ）を算出する。指令装置は、移動機構の動力が出力することを許容されている力の集合をＸとしたときに、暫定制御力ｕ（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｕ（ｋ）とする。暫定制御力ｕ（ｋ）∈Ｘが満たされないときには指令値ｆ（ｋ）を、暫定制御力ｕ（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘが満たされるｆ（ｋ）を指令値ｆ（ｋ）とする。
ここで、集合Ｘは、例えば設定されたスカラ値Ｆよりも絶対値が小さい全ての値の集合である。「移動機構の動力が出力することを許容されている力」を集合Ｘとして表現したのは次の理由による。即ち、「移動機構の動力が出力することを許容されている力」が移動機構先端の座標系で表される並進力やトルクのベクトル量で表されることもあれば、移動機構が複数の回転関節を有する機構である場合に、各関節に備えられたモータなどのトルクのベクトル量で表されることもあるので、それらを包含する意味で集合Ｘという表現を用いた。
前記指令装置は、移動機構の動力が出力することを許容されている力の大きさをＦとしたときに、｜ｕ（ｋ）｜＜Ｆが満たされる場合は指令値ｆ（ｋ）＝ｕ（ｋ）とし、｜ｕ（ｋ）｜＜Ｆが満たされない場合は式（１７）、換言すれば、指令値ｆ（ｋ）＝（Ｆ／｜ｕ（ｋ）｜）・ｕ（ｋ）とする。即ち、ここでは（Ｆ／｜ｕ（ｋ）｜）が、設定されたスカラ値Ｆよりも絶対値が小さい値の例となっている。
【００１２】
指令装置によって算出された力の指令値ｆ（ｋ）は、移動機構へと出力される。移動機構は指令された力の指令値に基づいて物体に力を加える。その結果、物体は目標位置に近づくように移動する。
【００１３】
以上の動作によって、上記説明したように、プロキシの位置ｐｓ（ｋ）を振動させることがなく、プロキシの状態（ｐｓ、ｖｓ）が有限時間で滑り状態（φ（ｐｓ、ｖｓ）＝０）に到達する。厳密にいうと、物体（即ち実制御対象）の状態量（ｐ、ｖ）は滑り平面に到達しない。しかし、物体の状態量（ｐ、ｖ）はプロキシの状態量（ｐｓ、ｖｓ）に常に拘束されているので、プロキシが滑り状態にあるとき、系全体として滑り状態にあると言ってよい。従って物体の状態量（ｐ、ｖ）は振動的にならずに目標位置と目標速度（ｐｄ、ｖｄ）に追従する。
【００１４】
本発明の位置制御装置は、上記説明したように、物体の位置を検出する位置検出器と、物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、物体に加える力の指令値を移動機構へ出力する指令装置を備える。
力の指令値を算出する際に物体の速度を知る必要がある場合には、物体の速度を取得する速度取得手段を備えてもよい。物体の速度ｖを取得する速度取得手段は、位置制御装置が別途備える速度検出器によって検出してもよいし、位置検出器によって検出される物体の位置ｐの経時的な変化から計算によって求めてもよい。離散時間系の場合は位置検出器によって検出した物体の位置ｐ（ｋ）と、その単位時間Ｔ前に検出した物体の位置ｐ（ｋ−１）の差分を単位時間Ｔで除した値、即ち、ｖ（ｋ）＝（ｐ（ｋ）−ｐ（ｋ−１））／Ｔの式により求めてもよい。
【００１５】
指令装置が移動機構へ出力する力の指令値を算出する場合、例えば上記説明したように、暫定制御力ｕ（ｋ）を式（１８ｅ）や式（２２ｆ）によって求め、暫定制御力ｕ（ｋ）と動力が出力することを許容されている力の集合Ｘとの関係から求めることができる。
なお、物体の位置ｐは、位置検出器によって検出される。
【００１６】
式（１８）は各変数がベクトルであっても成立するので、容易に多次元に拡張できる。ただしここで、絶対値演算子｜・｜はベクトルのノルム‖ｘ‖＝√（ｘ＾２）として、符号関数ｓｇｎ［・］はベクトルの正則化関数ｓｇｎ［ｘ］＝ｘ／‖ｘ‖として、それぞれ読み替える必要がある。ノルムとしては、Ｌ２ノルムやＬ無限大ノルムを用いることができる。
【００１７】
さらに発明者らは、本発明を応用することにより、作業者が、物体を広域的に目標位置へ追従するように移動機構を動かすと、局所的には物体の位置を目標位置に追従させる位置制御装置を実現できること発案した。
この位置制御装置は、前述した位置制御装置に対して、さらに、移動機構が動力によって物体に加える力の大きさの上限値Ｆが、移動機構の機械的な静止摩擦力と略同等以下の大きさに設定されており、移動機構は、作業者が移動機構に操作力を加えるための操作子を備える。
上記構成によれば、移動機構が物体に加える力の大きさの上限値Ｆが、移動機構の機械的な静止摩擦力と略同等以下の大きさに設定される。これによって、移動機構が出力できる最大の大きさの力Ｆを物体に加えるように動作しても、力Ｆが静止摩擦力に打ち勝つことができなければ物体は移動しない。また力Ｆが静止摩擦力に打ち勝つとしても物体には僅かしか移動しないことになる。
指令装置から入力された力指令値によって、移動機構は物体の位置を目標位置へ動かす方向に物体に力を発生させている。ここで、作業者が物体を広域的に目標位置へ追従するように移動機構を動かすと、物体には、移動機構が発生する力と操作力がほぼ同じ方向に作用する。移動機構が発生する力と操作力の合力によって、移動機構は保持した物体を目標位置へ移動させることができるようになる。これによって、作業者が、物体を広域的に目標位置へ追従するように移動機構を動かすと、局所的には物体の位置を目標位置に追従させる位置制御装置を実現できる。
一方、移動機構が物体に加える力の大きさの上限値Ｆが、静止摩擦力と略同等以下の大きさに設定されることによって、物体の位置と目標位置が大きく離れている場合に作業者が操作子を操作して物体を移動させることを妨げるほどには移動機構が発生する力が過大となることを防止することができる。
上記の構成によれば、移動機構に保持された物体を作業者が広域的に移動させる作業を妨げることなく、物体の位置が目標位置に近づいた際には物体の位置を目標位置に一致させることのできる位置制御装置を実現することができる。
【発明の効果】
【００１８】
本発明によれば、物体を保持した移動機構に対して、正常動作時の追従性と異常時の予想外の動作の防止とを両立する単純な位置制御装置および位置制御方法を提供することができる。この位置制御装置及び方法は、物体を保持する移動機構の正常動作時の小さな位置決め誤差からのすばやい復帰と、異常動作時の大きな位置誤差からのゆるやかな復帰を保つことができる。その際に移動機構に入力される物体に加える力の指令値が発振することを防止できる。
さらに、移動機構が発生できる力の上限値を、移動機構が保持した物体と移動機構の機械的な静止摩擦力と略同等以下の大きさに設定することによって、作業者が物体の位置を目標位置へ広域的に移動させる操作力を移動機構へ加えるだけで、移動機構は物体の位置を目標位置へ迅速に追従するように位置制御することができる。
【実施例】
【００１９】
＜実施例１＞
図４に示す２自由度平行リンクマニピュレータを用いて実験を行った。各関節のアクチュエータはハーモニックドライブ（登録商標）ギアによって減速されたＡＣサーボモータである。各アクチュエータには光学エンコーダが取り付けられており、手先効果器の位置を計測することができる。各モータには、出力軸側換算で１０Ｎ・ｍ程度の摩擦が存在する。
以下に示す３種類の制御器Ａ、Ｂ、およびＣを用いて実験を行った。各制御器は式（１８）の構造を持つ。
｜パラメータ ｜ 制御器Ａ ｜制御器Ｂ ｜制御器Ｃ ｜
｜Ｆ（Ｎ） ｜ ６０｜ ６０｜ ６０｜
｜Ｌ（Ｎ／ｍ／秒）｜ ０｜ ０｜ ０｜
｜Ｋ（Ｎ／ｍ） ｜ ６００００｜６００００｜６００００｜
｜Ｂ（Ｎ秒／ｍ） ｜ ２００｜１２０００｜ ２００｜
｜Ｈ（秒） ｜０．００３３｜ ０．２｜ ０．２｜
ただし、ｐ（ｋ）、ｆ（ｋ）などの各変数は２次元ベクトルである。制御器への入力ｐ（ｋ）は、手先効果器の現在位置であり、エンコーダの計測値である。制御器からの出力ｆ（ｋ）は、手先効果器で発生するべき力ベクトルであり、静力学的にそれと等価なトルクがアクチュエータから出力される。各制御器のパラメータ値は表１に示すとおりである。ただし、制御器ＡおよびＢは、Ｈ＝Ｂ／Ｋを満たすので、上限付のＰＤ制御（式（２１））に等価である。制御器Ｃは本発明の最良の形態の一つであり、Ｈ≪Ｂ／Ｋをみたす。なお、サンプリング周期はＴ＝０．００１秒である。
制御器に入力される目標軌道としては、８の字運動のリサジュー曲線
ｐｄ（ｔ）＝［Ａｘ・ｓｉｎ［Ωｘ・ｔ］、Ａｙ・ｓｉｎ［Ωｙ・ｔ］］
を与えた。ただし、Ａｘ＝０．２５ｍ、Ａｙ＝０．１５ｍ、Ωｘ＝２π／３ｒａｄ／秒、Ωｙ＝４π／３ｒａｄ／秒である。
異常からの復帰動作を観察するために、時刻ｔ＝２．０秒から２．５秒の間、アクチュエータへの指令トルクを強制的に零とした。
図５から図８に結果を示す。図５から図８において、灰色の太線は目標位置ｐｄのｙ成分、黒の細い曲線は手先位置の実測値ｐのｙ成分を表す。また、各グラフの下部の折れ線は、系が滑り状態にあるか否かを表す。図８は制御器ＢおよびＣを用いたときの、目標位置ｐｄと実位置ｐの間の偏差である。制御器Ａは追従性を重視したゲインを持つトルク制限つきＰＤ制御であるので、図５において、復帰運動時に大きなオーバーシュートを生じ、その後の振動の減衰にも時間がかかっている。制御器Ｂは収束性を重視したゲインを持つトルク制限つきＰＤ制御であるので、図８によく表れているとおり、正常運動時に偏差が大きくなっている。またこのとき、指令トルクｆが振動的になり（図示せず）、高周波の騒音がアクチュエータから発生することが確認できた。制御器Ｃは本発明の最良の形態の一つであり、正常動作時の追従性と、復帰動作時の収束性との両立ができているのが図７および図８から分かる。
【００２０】
本実施例の２自由度平行リンクマニピュレータが請求項の「移動機構」の一態様に相当する。各アクチュエータに取り付けられた光学エンコーダが請求項の「位置検出器」の一態様に相当する。図３に破線で囲んだ制御器が請求項「指令装置」の一態様に相当する。
【００２１】
ところで、式（１８）の手順は、Ｈ＝Ｂ／ＫおよびＬ＝０を代入すると、
ｕ（ｋ）＝Ｋ・（ｐｄ（ｋ）−ｐ（ｋ））
＋Ｂ・（ｖｄ（ｋ）−ｖ（ｋ）） ・・・（２１ａ）
ｆ（ｋ）＝ｕ（ｋ）；（｜ｕ（ｋ）｜≦Ｆの場合）
または
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）］；（｜ｕ（ｋ）｜＞Ｆの場合） ・・・（２１ｂ）
となる。これは、出力トルクの上限をＦとしたＰＤ制御と等価である。スライディングモード制御のチャタリングを防ぐために、切替面周囲の不連続性を滑らかにする方法も考えられているが、これは本質的にトルク制限付ＰＤ制御（式（２１））と等価である。
正常動作時の局所的な追従性を保ちつつ、異常動作時の広域的な収束性を保つためには、Ｈ≫Ｂ／Ｋと設定するのが望ましい。比例ゲインＫは、正常運転時の追従性を保つために、できるだけ大きく設定するのが望ましい。微分ゲインＢは、発振を防ぎつつ、追従性と即応性を損なわない程度の大きさにとどめるべきである。そして、定数Ｈが大きいほど異常から復帰する動作が緩やかであり、これが小さすぎると目標軌道近傍でオーバーシュートが発生する。発生トルクの上限値Ｆは、関節摩擦とリンク慣性に打ち勝って必要な速度を出すために必要な程度で、かつ、人間や環境に危害を与えない程度の力の大きさの値を設定すればよい。
【００２２】
本実施例において、Ｈ≫Ｂ／Ｋと設定されれば、正常動作時の小さな位置決め誤差からのすばやい復帰と、異常動作時の大きな位置誤差からのゆるやかな復帰を保つことができる。比例ゲインＫは、正常運転時の追従性を保つために、できるだけ大きく設定するのが望ましい。微分ゲインＢは、発振を防ぎつつ、追従性と即応性を損なわない程度の大きさにとどめるべきである。そして、定数Ｈが大きいほど異常から復帰する動作が緩やかであり、これが小さすぎると目標軌道近傍でオーバーシュートが発生する。発生トルクの上限値Ｆは、関節摩擦とリンク慣性に打ち勝って必要な速度を出すために必要な程度で、かつ、人間や環境に危害を与えない程度の力の大きさの値を設定すればよい。
なお、従来のＰＤ制御器はＨ＝Ｂ／Ｋとした場合と等価である。すなわち、正常動作時の追従性を決定するＫおよびＢと、異常動作時の収束性を決定するＨを、独立して設定できるのが、本実施例の効果であるといえる。
また、アクチュエータの発生力の大きさＦを明示的に設定できるという点も、本発明の効果である。
【００２３】
＜実施例２＞
次に実施例２について説明する。この実施例における位置制御装置は、作業者が器具を用いてワークに対して行う作業について、作業者の負担を軽減するものである。作業の例としては、目標軌道（目標位置）に沿って、切断器具によりワークを目標軌道に沿って切断する作業や、接着剤塗布器具によりワーク上の目標軌道に沿って接着剤を塗布する作業などがある。切断器具や接着剤塗布器具（以下、器具と称する）は、移動機構が保持する。
【００２４】
まず、そのような作業の課題を説明する。そのような作業は、作業者が直接に器具を保持し、自らの力だけで器具を目標位置に追従させることは作業者の負担が大きい。作業者にとって、器具の位置を大まかに（広域的に）目標位置へ合わせる作業は容易であるが、器具の位置を目標位置に正確に（局所的に）に追従させる作業は負担となる。
【００２５】
そこで、作業者が行う器具の局所的な移動を補助する位置制御装置を考える。器具は位置制御装置の移動機構に保持させる。移動機構は操作子を備える。器具を広域的に移動させる場合には、作業者が操作子を操作して移動機構に保持された器具を移動させる。一方、位置制御装置は、移動機構が保持した器具の位置を目標位置に正確に追従するように移動機構を制御する。換言すれば、この位置制御装置は、器具の位置を目標位置に追従させる作業者の操作を誘導する装置ということができる。
この場合、器具を移動させるための広域的な動作が支配的な作業フェーズにおいては、作業者の随意的な操作を阻害せずに良好な作業感覚を与えるために、位置制御装置が制御する移動機構には、十分滑らかに動くことが要求される。「滑らか」とは、移動機構の動作に高周波成分が少ないことを意味する。
器具の位置が目標位置に近づいて、器具を目標位置に正確に追従させる際の局所的な（小振幅な）動作が支配的な作業フェーズにおいては、位置制御装置は、十分に高剛性で速応性の高い（例えば、器具の位置が目標位置に一致したときに「カチッ」とラッチするような）動作を移動機構に行わせる必要がある。そうすることによって、高周波かつ小振幅
である作業については作業者の負担を低減することができる。このことを模式的に表したのが図９である。図９の縦軸は器具の移動の振幅（器具を移動させる動作の大きさ）である。横軸は器具を移動させる際の動作の周波数である。振幅が大きくかつ低周波である領域Ａに相当する作業フェーズでは、人間が随意的に器具を移動させる動作を可能にし、振幅が小さくかつ高周波である領域Ｂに相当する作業フェーズでは、目標位置に追従させるための器具の移動を位置制御装置がカバーする。そのためには、位置制御装置が移動機構の保持する器具の位置を制御する際には、広域的な「滑らかさ」と局所的な「高い剛性と速応性」を両立する必要がある。
【００２６】
移動機構を制御する際、従来のＰＤあるいはＰＩＤ制御方式では「滑らかさ」と「速応性」は相反するため、上記の両立は困難であった。
また、「高い剛性」と「滑らかさ」を両立するためには、大きい速度フィードバックゲイン（器具に作用する粘性係数に相当する）を設定する必要がある。これは、「速応性」を損なうだけでなく、速度信号のノイズを増幅してしまうという課題も発生する。
【００２７】
そこで本実施例では、実施例１で説明した位置制御装置を活用する。位置制御装置が備えており、移動機構が物体に加える力の指令値を出力する指令装置内には図３のブロック図に示す制御ロジックが構築されている。その制御ロジックの原理を再度説明するために制御ロジックを構築する出発点となる式を以下に再記する。
ｆ＝Ｋ・（ｐｓ−ｐ）＋Ｂ・（ｖｓ−ｖ） ・・・（６ｂ）
ｆ＝Ｆ・ｓｇｎ［ｐｄ−ｐ−（ｄａ／ｄｔ）
＋Ｈ・（ｖｄ−（ｄ＾２ａ／ｄｔ＾２））］ ・・・（８）
式（８）はａ＝∫（ｐｓ−ｐ）ｄｔであることを考慮すると、
ｆ＝Ｆ・ｓｇｎ［ｐｄ−ｐｓ＋Ｈ・（ｖｄ−ｖｓ）］ ・・・（８ｂ）
と書き直すことができる。また、上記の式（６ｂ）は、前述した式（６）において積分ゲインＬをゼロとしたものである。
上記の式（６ｂ）と式（８ｂ）の夫々の記号の意味は次の通りである。ｐおよびｖは連続時間系で表現された器具の位置および速度である。ｐｓおよびｖｓは連続時間系で表現された仮想質点（プロキシ）の位置および速度である。ｐｄおよびｖｄは連続時間系で表現された目標位置および目標速度である。Ｋは第１の係数である。係数Ｋの物理的な意味は、器具とプロキシを仮想的に連結する連結要素の弾性係数である。Ｂは第２の係数である。係数Ｂの物理的な意味は、連結要素の粘性係数である。即ち、式（６ｂ）は、器具の位置ｐとプロキシの位置ｐｓとが仮想的な連結要素で連結されているときに器具の位置ｐとプロキシの位置ｐｓの間に作用する仮想的な粘弾性力ｆを表している。Ｈは第３の係数を表す。
器具は移動機構に保持されている。式（６ｂ）と式（８ｂ）の左辺ｆは、仮想的な空間内で仮想連結要素が器具へ加える力を表す。この力を移動機構によって器具へ加える。また、プロキシは仮想空間内で目標位置に近づくようにスライディングモード制御されており、式（８ｂ）はその制御則を表す。プロキシの質量はゼロに設定されており、プロキシに仮想的な連結要素から加えられる力（式(６ｂ)のｆ）とスライディングモード制御器から加えられる力（式（８ｂ）のｆ）は常に等しい．なお式（８ｂ）において、変数Ｆは移動機構が発生する力の上限値を表す。この上限値Ｆは、移動機構の機械的な静止摩擦力と同等かそれ以下に設定される。
式（８ｂ）の制御則は、プロキシの状態（ｐｓ、ｖｓ）を、
ｐｄ−ｐｓ＋Ｈ・（ｖｄ−ｖｓ）＝０ ・・・（２３）
という超平面（滑り平面）に引き付けるように働く。式（２３）は、プロキシの位置ｐｓが目標位置ｐｄへ漸近的に時定数Ｈで収束するような動特性を表す。プロキシが式（２３）に従って運動する限り、プロキシの位置ｐｓにはオーバーシュートは発生しない。移動機構に保持された器具は仮想的な連結要素によってプロキシと連結されているため、器具の位置ｐと速度ｖもほぼ式（２３）の動特性に従うことになる。
【００２８】
この制御則の利点は、局所的な応答特性と広域的な応答特性とを独立して設定できる点である。局所的な応答特性とは、比較的小さな位置誤差に対する応答特性であり、これは仮想的な連結要素の粘性係数Ｂと弾性係数Ｋとの比によって決定される。このため、正確な追従のためにＫはなるべく大きく、局所的な速応性のためにＢは振動を抑えられる程度でなるべく小さく設定する必要がある。
一方、広域的な応答特性とは、比較的大きな位置誤差に対する応答特性であり、これは第３の係数Ｈによって決定される。ここでは位置制御装置の目的は作業者の動作を誘導することにある。従って位置制御装置が備えており、器具を保持する移動機構は作業者が操作子を操作する際の随意的な動作と同程度に滑らかな動特性を実現する必要がある。人間の随意的な動作の周波数帯域はおよそ６Hz程度が限界値であり、通常はおよそ２Hz程度といわれている。折点周波数が２Hzの一次遅れ系の時定数は、１／（２π×２)＝０．０８秒である。したがって、望ましい第３の係数Ｈの値はおよそ０．１秒程度と考えることができる。通常の場合、移動機構の局所的な応答特性は、作業者の応答特性よりも高い速応性を必要とするので、Ｈ＞＞Ｂ／Ｋとなるように設定されることが望ましい。
【００２９】
次に実施例２の位置制御装置について説明する。図１０は実施例２の位置制御装置１００の概略図である。この実施例は、塗布器具１１２の先端から吐出する接着剤をワーク２００に設定された目標軌道Ｌに沿って吹き付ける作業である。
この位置制御装置１００は、移動機構１０２と指令装置１２０を備える。
移動機構１０２は、基部１０４と第１リンク１０６ａの一端が第１モータ１０８ａによって回転自在に連結されている。第１リンク１０６ａの他端と第２リンク１０６ｂの一端が第１モータ１０８ｂによって回転自在に連結されている。第２リンク１０６ｂの他端の下側には塗布器具１１２が保持されている。第２リンク１０６ｂの他端の上側には操作子１１４が取り付けられている。操作子１１４は、作業者３００によって把持されて塗布器具１１２の位置を移動させるための操作力を移動機構１０２に伝えるものである。
移動機構１０２は全体として２リンクマニピュレータを形成しており、第２リンク１０６ｂの他端に保持された塗布器具１１２の位置ｐ（ｋ）のワーク２００の平面内での移動を幾何学的に拘束しない。従って、移動機構１０２が発生する力が大きくなければ、移動機構が発生する力は、作業者３００が操作子１１４を操作して塗布器具１１２の位置ｐ（ｋ）を移動させる作業を妨げない。なお、位置ｐ（ｋ）のｋは離散時間系における時刻を表す記号である。
第１モータ１０８ａには第１エンコーダ１１０ａが備えられている。第２モータ１０８ｂには第２エンコーダ１１０ｂが備えられている。第１エンコーダ１１０ａは、基部１０４と第１リンク１０６ａの相対回転角を検出する。第２エンコーダ１１０ｂは、第１リンク１０６ａと第２リンク１０６ｂの相対回転角を検出する。第１エンコーダ１１０ａと第２エンコーダｂの検出値と、移動機構１０２の幾何学的な構造と寸法から、塗布器具１１２の先端の位置ｐ（ｋ）が求められる。
【００３０】
指令装置１２０は移動機構１０２と電気的に接続されている。指令装置１２０には第１エンコーダ１１０ａ及び第２エンコーダ１１０ｂの検出値が入力される。指令装置１２０からは移動機構１０２が塗布器具１１２に加える力の指令値ｆ（ｋ）が出力される。ここで「移動機構１０２が塗布器具１１２に加える力の指令値ｆ（ｋ）」は、実際には、移動機構１０２が塗布器具１１２に加える力を発生するために、第１モータ１０８ａと第２モータ１０８ｂが出力するトルク指令値として出力される。移動機構１０２が塗布器具１１２に加える力ｆ（ｋ）と、第１モータ１０８ａ及び第２モータ１０８ｂが出力するトルクとは良く知られるヤコビ行列によって相互に変換可能である。従って、移動機構１０２が塗布器具１１２に加える力と、第１モータ１０８ａ及び第２モータ１０８ｂが出力するトルクは等価である。
【００３１】
目標軌道Ｌは、目標位置ｐｄ（ｋ）の経時的なデータとして表される。そのデータは、指令装置１２０内の記憶装置（不図示）に記憶されている。塗布器具１１２の目標速度ｖｄ（ｋ）が設定されている場合には、目標速度ｖｄ（ｋ）の経時的なデータも記憶装置に記憶されていてもよい。また、目標速度ｖｄ（ｋ）の経時的なデータを記憶する代わりに、目標位置ｐｄ（ｋ）の経時的なデータのうち、幾つかの経時的に連続した目標位置ｐｄ（ｋ）のデータからそれらの位置の差分によって目標速度ｖｄ（ｋ）を求めてもよい。
この実施例の作業では、各時刻ｋにおいて、塗布器具１１２の先端の位置ｐ（ｋ）を目標軌道Ｌ上の目標位置ｐｄ（ｋ）に追従させる必要がある。そこで指令装置１２０内には、図３に示すブロック図の制御ロジックが構築されている。
【００３２】
指令装置１２０内で実際に実行される制御ロジックを再記する。
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋ｅ（ｋ−１）・（Ｈ・Ｋ−Ｂ）／（Ｈ＋Ｔ） ・・・（２２ｆ）
ｆ（ｋ）＝ｕ（ｋ）；（｜ｕ（ｋ）｜≦Ｆの場合） または、
ｆ（ｋ）＝Ｆ・ｓｇｎ［ｕ（ｋ）］；（｜ｕ（ｋ）｜＞Ｆの場合） ・・・（２２ｃ）
ｅ（ｋ）＝（Ｂ・ｅ（ｋ−１）＋Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ） ・・・（２２ｄ）
ｅ（ｋ）＝ｐｓ（ｋ）−ｐ（ｋ） ・・・（２２ｅ）
式（２２ｆ）、式（２２ｃ）、式（２２ｄ）に表れる記号の意味は以下の通りである。ｕ（ｋ）は、時刻ｋにおける暫定制御力を表す。ｆ（ｋ）は、時刻ｋにおいて指令装置１２０が移動機構１０２へ指令する力指令値を表す。力指令値ｆ（ｋ）は、移動機構１０２が塗布器具１１２へ加えるべき力である。
ｐｄ（ｋ）とｖｄ（ｋ）は時刻ｋにおける目標位置と目標速度を表す。ｐ（ｋ）とｖ（ｋ）は時刻ｋにおける塗布器具１１２の位置と速度を表す。Ｔは制御周期を表す。ＫとＢは式（６ｂ）で説明した通り仮想的な連結要素の弾性係数と粘性係数を表す。Ｈは第３の係数を表す。
Ｆは移動機構１０２が塗布器具１１２に加える力の上限値を表す。この上限値Ｆは、指令装置１２０が出力する力指令値のリミッタとして設定される。符号関数ｓｇｎ［ｘ］は、ｘ＝０のときに閉区間［−１、１］の任意の値を取りうると定義している。
ｅ（ｋ）は制御ロジック内の内部状態変数である。その物理的な意味は、式（２２ｅ）に示されるように、時刻ｋにおける仮想質点（プロキシ）の位置ｐｓ（ｋ）と塗布器具１１２の位置ｐ（ｋ）の偏差である。但し、ｅ（ｋ）は式（２２ｅ）で計算されることはなく、式（２２ｄ）の漸化式によって計算できる。ｅ（ｋ）が式（２２ｄ）の漸化式で計算されるのでｅ（ｋ）を内部状態変数とみることができる。
なお、本実施例では、塗布器具１１２の速度ｖ（ｋ）は、（ｐ（ｋ）−ｐ（ｋ−１））／Ｔで算出される。ｐ（ｋ）は第１エンコーダ１１０ａ、第２エンコーダ１１０ｂの出力値と移動機構１０２の幾何学的構造と寸法から算出される。従って本実施例では塗布器具１１２の速度ｖ（ｋ）を検出する検出器は必要ない。なお、塗布器具１１２の速度ｖ（ｋ）を位置ｐ（ｋ）の経時的なデータから算出する代わりに、速度センサなど、物体の速度を検出するセンサを配置してそのセンサから物体の速度を直接検出してもよい。
【００３３】
次に位置制御装置１００が実行する処理を図１１のフローチャート図を参照して説明する。
ステップＳ１００では、第１エンコーダ１１０ａ、第２エンコーダ１１０ｂによって塗布器具１１２の位置ｐ（ｋ）を検出する。ステップＳ１０２では前述したように、塗布器具１１２の時刻ｋでの位置ｐ（ｋ）と時刻ｋ−１での位置ｐ（ｋ−１）から塗布器具１１２の速度ｖ（ｋ）を算出する。ステップＳ１０４では時刻ｋにおける目標位置ｐｄ（ｋ）と目標速度ｖｄ（ｋ）を取得する。目標位置と目標速度の経時的なデータは指令装置１２０内の記憶装置に予め記憶されている。
次にステップＳ１０６では、式（２２ｆ）に従って暫定制御力ｕ（ｋ）を算出する。
次にステップＳ１０８では、暫定制御力ｕ（ｋ）の大きさと上限値Ｆを比較する。本実施例では、塗布器具１１２はワーク２００の平面内で移動可能であるので暫定制御力ｕ（ｋ）は２次元ベクトルとなる。２次元ベクトルの場合の大きさはそのベクトルのノルムで定義される。
比較の結果、暫定制御力ｕ（ｋ）の大きさが上限値Ｆ以下の場合には（ステップＳ１０８：ＹＥＳ）、ステップＳ１１０によって、移動機構１０２へ出力する力の指令値ｆ（ｋ）を暫定制御力ｕ（ｋ）にセットする。比較の結果、暫定制御力ｕ（ｋ）の大きさが上限値Ｆを超える場合には（ステップＳ１０８：ＮＯ）、ステップＳ１１２によって移動機構１０２へ出力する力の指令値ｆ（ｋ）は、方向が暫定制御力ｕ（ｋ）ベクトルと同じであり、大きさがＦの値にセットする。
ステップＳ１１０又はステップＳ１１２でセットされた力の指令値ｆ（ｋ）は指令装置１２０から移動機構１０２へ出力される。最後にステップＳ１１６では、次の制御周期の際に式（２２ｆ）の計算に必要なｅ（ｋ）の値を式（２２ｄ）により計算する。以上の処理を制御周期Ｔ毎に繰り返す。
【００３４】
本実施例では上限値Ｆの大きさは、移動機構１０４の機械的な静止摩擦力より小さい値に設定される。これによって、塗布器具１１２が静止している状態で移動機構１０４が出力できる最大の大きさの力Ｆを塗布器具１１２に加えるように動作しても、力Ｆが静止摩擦力に打ち勝つことができず塗布器具１１２は移動しない。塗布器具１１２は、作業者３００が操作子１１４を操作して、塗布器具１１２の位置ｐ（ｋ）が目標位置ｐｄ（ｋ）に近づく方向に操作力を加えたときに始めて移動する。
指令装置１２０が移動機構１０２に出力する力の指令値ｆ（ｋ）は、図１０に示すように、塗布器具１１２の位置ｐ（ｋ）を目標位置ｐｄ（ｋ）に一致させる向きに作用する。従って作業者３００が操作子１１４に対して塗布器具１１２の位置ｐ（ｋ）を目標位置ｐｄ（ｋ）に近づける方向に操作力Ｆｓを加えると、操作力Ｆｓと移動機構１０２が出力する力指令値ｆ（ｋ）の合力によって、塗布器具１１２の位置ｐ（ｋ）は滑らかに目標位置ｐｄ（ｋ）へ移動する。力指令値ｆ（ｋ）の大きさの上限値Ｆは、塗布器具１１２と移動機構１０４の機械的な静止摩擦力より小さい値に設定されている。一般に静止摩擦力は動摩擦力よりも大きい。従って、塗布器具１１２がひとたび目標位置ｐｄ（ｋ）の方向へ向って動き出すと、移動機構１０２が発生する力ｆ（ｋ）のみによって、あるいはｆ（ｋ）とＦｓの合力によって、塗布器具１１２の位置ｐ（ｋ）は目標位置ｐｄ（ｋ）に追従するようになる。作業者３００は、移動機構１０２が発生する力ｆ（ｋ）によって、塗布器具１１２の位置ｐ（ｋ）を目標位置ｐｄ（ｋ）に追従させるという局所的な動作の負担を軽減される。さらに前述したように、指令値１２０が出力する力指令値ｆ（ｋ）は塗布器具１１２の位置ｐ（ｋ）の目標位置ｐｄ（ｋ）への追従に対してオーバーシュートを生じることがない。その上、指令値１２０が出力する力指令値ｆ（ｋ）がチャタリングを発生することもない。
また仮に作業者３００が操作子１１４に加える操作力Ｆｓの方向が目標軌道ｐｄ（ｋ）に向かう方向と異なる場合には、移動機構１０２が発生する力ｆ（ｋ）の方向と操作力Ｆｓの方向が異なることになる。従って作業者３００は、目標位置ｐｄ（ｋ）へ向う方向以外の方向へは操作子１１４を動かし難くなる。このことは換言すれば、作業者の行う動作は、位置制御装置１００によって、塗布器具１１２の位置ｐ（ｋ）を目標位置ｐｄ（ｋ）に追従させるように誘導されるということもできる。
一方、塗布器具１１２の位置ｐ（ｋ）と目標位置ｐｄ（ｋ）が大きく離れている場合には式（２２ｆ）からわかる通りｕ（ｋ）は大きな値となる。しかし指令装置１２０が出力する力指令値ｆ（ｋ）の大きさは、塗布器具１１２と移動機構１０４の機械的な静止摩擦力より小さい値である上限値Ｆを超えることはない。塗布器具１１２の位置ｐ（ｋ）と目標位置ｐｄ（ｋ）が大きく離れている場合に、移動機構が出力する力ｆ（ｋ）は、作業者３００が操作子１１４に操作力を加えて塗布器具１１２を広域的に移動させる作業を妨げない。
なお、図１１のフローチャートの処理は、実施例１の位置制御装置についても同様に実行される。実施例１の位置制御装置と実施例２の位置制御装置は同じ処理を実行する。実施例１の位置制御装置と実施例２の位置制御装置で異なるのは、実施例２の位置制御装置１００の移動機構１０２が操作子１１４を有することと、実施例２の位置制御装置では、力の上限値Ｆが移動機構１０４の機械的な静止摩擦力より小さい値に設定されることである。
また、本実施例では、力の上限値Ｆを移動機構１０４の機械的な静止摩擦力より小さい値に設定したが、力の上限値Ｆは、移動機構１０４の機械的な静止摩擦力と同等かそれ以下の値であればよい。
また、図１１のフローチャートに示した処理は、コンピュータによって実行されるプログラムの形式で提供されるものであってもよい。その場合には、より厳密には、図１１のステップＳ１００は、第１エンコーダ１１０ａ、第２エンコーダ１１０ｂによって検出された物体の位置を取得するステップとなる。
【００３５】
実施例２の位置制御装置１００の効果を調べるための実験を行った。移動機構には、図４に示した水平面内で稼動する２自由度平行リンクマニピュレータを用いた。マニピュレータの稼動領域の直下には液晶モニタを水平に設置した。被験者はマニピュレータの手先効果器に取り付けられたグリップ（操作子）を握り、液晶モニタに表示された目標位置に追従するように、マニピュレータの手先を動かす。即ち、マニピュレータの手先位置が物体の位置ｐに相当する。
被験者は一回の試行において、幅０．５ｍ、奥行き０．３ｍの「∞」形の目標軌道を１周期あたり４秒で２周期（８秒）の追従動作を行った。なお、この運動は縦横の周波数比が１：２のリサジュー運動である。マニピュレータの手先位置の直交座標に対して、指令装置は式（２２ｆ）、式（２２ｃ）で算出される力指令値ｆ（ｋ）をマニピュレータに対して出力する。ただしマニピュレータの手先に発生させる力の上限値は、手先の並進力ではなく、各軸の関節トルクに対して設けた。制御則のパラメータ（Fτ、Ｈ、Ｋ、Ｂ）は、図１２に示すＣ０からＣ５の６種類の組み合わせ（条件）を用いた。なお、Ｆτは関節トルクの大きさの上限値である。条件Ｃ０においてはＦτ＝０であるので、マニピュレータはトルクを発生しない。条件Ｃ１からＣ５においてはＦτ＝７Ｎｍであるが、各関節の減速器におよそ１０Ｎｍの摩擦力がはたらくので、被験者から力が加えられなければマニピュレータはほとんど動かない。一人の被験者は各条件の下で１回ずつ、合計６回の試行を行った。被験者は８人の健康な男性とした。
一回の試行の結果は、メートルを単位とした追従誤差の大きさの自然対数の時間平均（以下、Ｌ値と呼ぶ）を用いて評価した。ただし、被験者間の身体能力の差異の影響を除くために、各試行のＬ値から各被験者内の平均Ｌ値を減じた値を、各試行の成績（以下、Ｎ値と呼ぶ）とした。図１３に、条件Ｃ０からＣ５の下での、Ｎ値の平均および標準偏差を示す。大きいＮ値は大きい追従誤差を意味する。「*」および「**」はそれぞれ、ｐ＜０．０５およびＰ＜０．０１の有意差を表す。図１３から、条件Ｃ０におけるＮ値が他の条件におけるＮ値より有意に大きいことが分かる。これは、弱い力の運動誘導によっても軌道追従の誤差を低減する効果があるということを示している。
条件Ｃ１、Ｃ２およびＣ３はＫ値が同じでＨ値が異なるが、前述したように、Ｈ値が中間（０．１秒）である条件Ｃ２が最も良い結果となっている。Ｈ値が小さい（０．０１秒）と誘導動作の速応性が高くなり、被験者が誘導動作に従えなくなる。これによってオーバーシュートが発生し、追従誤差が増加する。一方で、大きすぎるＨ値（０．５秒）は速応性の低下を招くため、これも追従誤差の増加の原因となる。条件Ｃ２、Ｃ４およびＣ５はＨ値が同じでＫ値が異なるが、Ｋ値が最も大きい条件Ｃ２が最も良い結果となっている。これは、大きいＫ値、すなわち高い剛性は、正確な追従に必要であるということを表している。なおこの制御則は、Ｈ＝Ｂ／Ｋを満たすとき、マニピュレータの発生力に上限があるＰＤ制御と等価になる。ＨとＫの値を条件Ｃ２と同じにし、Ｈ＝Ｂ／Ｋを満たすようにＢ値を設定すると、Ｂ値が大きくなりすぎたため、マニピュレータは速度信号のノイズに起因する騒音を発生した。これは、条件Ｃ２と同様の動特性を実現するためには、本実施例で提案した、式（２２ｆ）、式（２２ｃ）で算出される力指令値ｆ（ｋ）を算出する位置制御装置が有効であるということを示している。
【００３６】
以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。本発明は一般的な位置制御装置や位置制御方法などに適用可能である。また、実施例２のように作業者の作業を誘導する位置制御装置は、例えば、人間への技能・運動教示や、リハビリテーションなどへの適用も可能である。
【００３７】
本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。
【図面の簡単な説明】
【００３８】
【図１】最も簡単なスライディングモード制御を示す模式図である。
【図２】本発明の位置制御装置の物理的解釈を示す模式図である。
【図３】本発明の位置制御装置を示すブロック図である。
【図４】実験に用いたマニピュレータの模式図である。
【図５】制御器Ａ（従来技術の一例）を用いた場合の実験結果を示す図である。
【図６】制御器Ｂ（従来技術の一例）を用いた場合の実験結果を示す図である。
【図７】制御器Ｃ（本発明の最良の形態の一つ）を用いた場合の実験結果を示す図である。
【図８】制御器ＢおよびＣを用いた場合の位置決め誤差を示す図である。
【図９】人間が随意的に動作可能な領域と位置制御装置がカバーすべき動作の領域を示す模式図である。
【図１０】実施例２の位置制御装置の模式図である。
【図１１】実施例２の位置制御装置が実行する処理のフローチャート図である。
【図１２】実施例２の位置制御装置の効果を検証する実験における変数の値を示す図である。
【図１３】実施例２の位置制御装置の効果を検証する実験の結果を表す図である。
【符号の説明】
【００３９】
１００：位置制御装置
１０２：移動機構
１０４：基部
１０６ａ：第１リンク
１０６ｂ：第２リンク
１０８ａ：第１モータ
１０８ｂ：第２モータ
１１０ａ：第１エンコーダ
１１０ｂ：第２エンコーダ
１１２：塗布器具
１１４：操作子
１２０：指令装置
２００：ワーク
３００：作業者
Ｌ：目標軌道
【特許請求の範囲】
【請求項１】
物体の位置を目標位置に追従させる位置制御装置であり、
物体の位置を検出する位置検出器と、
物体の速度を取得する速度取得手段と、
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、
物体に加える力の指令値を移動機構へ出力する指令装置と、を備え、
指令装置は、
離散時間系における時刻をｋ、単位時間をＴ、時刻ｋにおける物体の目標位置をｐｄ（ｋ）、時刻ｋにおける物体の目標速度をｖｄ（ｋ）、時刻ｋにおいて位置検出器によって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度取得手段によって取得される物体の速度をｖ（ｋ）、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、第４の係数をＬ、時刻ｋにおいて指令装置内で演算される内部状態変数をａ（ｋ）としたときに、時刻ｋにおいて指令装置が出力する力の指令値ｆ（ｋ）を算出するための暫定制御力ｕ（ｋ）を次の第１式、即ち、
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋ａ（ｋ−１）・Ｌ
＋（ａ（ｋ−１）−ａ（ｋ−２））・（Ｈ・（Ｋ＋Ｌ・Ｔ）−Ｂ）／（Ｈ＋Ｔ）／Ｔ・・・（第１式）
によって算出し、
時刻ｋにおいて指令装置が算出する指令値ｆ（ｋ）は、移動機構の動力が出力することを許容されている力の集合をＸとしたときに、暫定制御力ｕ（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｕ（ｋ）とし、暫定制御力ｕ（ｋ）∈Ｘが満たされないときには指令値ｆ（ｋ）を、暫定制御力ｕ（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘが満たされるｆ（ｋ）を指令値ｆ（ｋ）とし、
時刻ｋ＋１において暫定制御力ｕ（ｋ＋１）を算出する場合に第１式に表れるａ（ｋ）は次の第２式、即ち、
ａ（ｋ）＝ａ（ｋ−１）・（Ｂ＋Ｋ・Ｔ）／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋（ａ（ｋ−１）−ａ（ｋ−２））・Ｂ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋ｆ（ｋ）・Ｔ・Ｔ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）・・・（第２式）
によって算出することを特徴とする位置制御装置。
【請求項２】
物体の位置を目標位置に追従させる位置制御装置であり、
物体の位置を検出する位置検出器と、
物体の速度を取得する速度取得手段と、
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、
物体に加える力の指令値を移動機構へ出力する指令装置と、を備え、
指令装置は、
離散時間系における時刻をｋ、単位時間をＴ、時刻ｋにおける物体の目標位置をｐｄ（ｋ）、時刻ｋにおける物体の目標速度をｖｄ（ｋ）、時刻ｋにおいて位置検出器によって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度検出手段によって取得される物体の速度をｖ（ｋ）、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、時刻ｋにおいて指令装置内で演算される内部状態変数をｅ（ｋ）としたときに、時刻ｋにおいて指令装置が算出する指令値ｆ（ｋ）を算出するための暫定制御力ｕ（ｋ）を次の第３式、即ち、
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋ｅ（ｋ−１）・（Ｈ・Ｋ−Ｂ）／（Ｈ＋Ｔ）・・・（第３式）
によって算出し、
時刻ｋにおいて指令装置が算出する指令値ｆ（ｋ）は、移動機構の動力が出力することを許容されている力の集合をＸとしたときに、暫定制御力ｕ（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｕ（ｋ）とし、暫定制御力ｕ（ｋ）∈Ｘが満たされないときには指令値ｆ（ｋ）を、暫定制御力ｕ（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘが満たされるｆ（ｋ）を指令値ｆ（ｋ）とし、
時刻ｋ＋１において暫定制御力ｕ（ｋ＋１）を算出する場合に第３式に表れるｅ（ｋ）は次の第４式、即ち、
ｅ（ｋ）＝（Ｂ・ｅ（ｋ−１）＋Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ）・・・（第４式）
によって算出されることを特徴とする位置制御装置。
【請求項３】
前記集合Ｘは、設定されたスカラ値Ｆよりも絶対値が小さい全ての値の集合であり、前記指令装置は、移動機構の動力が出力することを許容されている力の大きさをＦとしたときに、｜ｕ（ｋ）｜＜Ｆが満たされる場合は指令値ｆ（ｋ）＝ｕ（ｋ）とし、｜ｕ（ｋ）｜＜Ｆが満たされない場合は指令値ｆ（ｋ）＝（Ｆ／｜ｕ（ｋ）｜）・ｕ（ｋ）とすることを特徴とする請求項１又は２に記載の位置制御装置。
【請求項４】
前記集合Ｘは、設定されたスカラ値ＦよりもＬ２ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項１又は２に記載の位置制御装置。
【請求項５】
前記集合Ｘは、設定されたスカラ値ＦよりもＬ無限大ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項１又は２に記載の位置制御装置。
【請求項６】
物体の位置を目標位置に追従させる位置制御装置であり、
物体の位置を検出する位置検出器と、
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、
物体に加える力の指令値を移動機構へ出力する指令装置と、を備え、
指令装置は、
質量ゼロの仮想質点と、一端が物体の検出された位置に連結されており、他端が仮想質点に連結されている仮想的な保持要素と、を仮想空間内に設定し、
スライディングモード制御則に基づいて、仮想質点の位置を目標位置に追従させる制御を仮想空間内で行った際に仮想保持要素から物体に加えられる力を算出し、算出された力を指令値として移動機構へ出力することを特徴とする位置制御装置。
【請求項７】
移動機構が動力によって物体に加える力の大きさの上限値が、移動機構の機械的な静止摩擦力と略同等以下の大きさに設定されており、
前記移動機構は、作業者が移動機構に操作力を加えるための操作子を備えることを特徴とする請求項１から６のいずれか１項に記載の位置制御装置。
【請求項８】
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を利用して、物体の位置を目標位置に追従させる位置制御方法であり、
物体の位置を検出する位置検出ステップと、
物体の速度を検出する速度検出ステップと、
物体に加える力の指令値を算出する指令値算出ステップと、
指令値算出ステップによって算出された力の指令値を移動機構へ出力するステップと、を含み、
前記指令値算出ステップは、
離散時間系における時刻をｋ、単位時間をＴ、時刻ｋにおける物体の目標位置をｐｄ（ｋ）、時刻ｋにおける物体の目標速度をｖｄ（ｋ）、時刻ｋにおいて位置検出ステップによって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度取得ステップによって取得される物体の速度をｖ（ｋ）、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、第４の係数をＬ、時刻ｋにおける内部状態変数をａ（ｋ）としたときに、時刻ｋにおいて力の指令値ｆ（ｋ）を算出するための暫定制御力ｕ（ｋ）を次の第１式、即ち、
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋ａ（ｋ−１）・Ｌ
＋（ａ（ｋ−１）−ａ（ｋ−２））・（Ｈ・（Ｋ＋Ｌ・Ｔ）−Ｂ）／（Ｈ＋Ｔ）／Ｔ・・・（第１式）
によって算出し、
時刻ｋにおける指令値ｆ（ｋ）は、移動機構の動力が出力することを許容されている力の集合をＸとしたときに、暫定制御力ｕ（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｕ（ｋ）とし、暫定制御力ｕ（ｋ）∈Ｘが満たされないときには指令値ｆ（ｋ）を、暫定制御力ｕ（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘが満たされるｆ（ｋ）を指令値ｆ（ｋ）とし、
時刻ｋ＋１において暫定制御力ｕ（ｋ＋１）を算出する場合に第１式に表れるａ（ｋ）は次の第２式、即ち、
ａ（ｋ）＝ａ（ｋ−１）・（Ｂ＋Ｋ・Ｔ）／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋（ａ（ｋ−１）−ａ（ｋ−２））・Ｂ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋ｆ（ｋ）・Ｔ・Ｔ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）・・・（第２式）
によって算出することを特徴とする位置制御方法。
【請求項９】
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を利用して、物体の位置を目標位置に追従させる位置制御方法であり、
物体の位置を検出する位置検出ステップと、
物体の速度を検出する速度検出ステップと、
物体に加える力の指令値を算出する指令値算出ステップと、
指令値算出ステップによって算出された力の指令値を移動機構へ出力するステップと、を含み、
前記指令値算出ステップは、
離散時間系における時刻をｋ、単位時間をＴ、時刻ｋにおける物体の目標位置をｐｄ（ｋ）、時刻ｋにおける物体の目標速度をｖｄ（ｋ）、時刻ｋにおいて位置検出ステップによって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度取得ステップによって取得される物体の速度をｖ（ｋ）、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、時刻ｋにおける内部状態変数をｅ（ｋ）としたときに、時刻ｋにおいて力の指令値ｆ（ｋ）を算出するための暫定制御力ｕ（ｋ）を次の第１式、即ち、
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋ｅ（ｋ−１）・（Ｈ・Ｋ−Ｂ）／（Ｈ＋Ｔ）・・・（第３式）
によって算出し、
時刻ｋにおいて算出する指令値ｆ（ｋ）は、移動機構の動力が出力することを許容されている力の集合をＸとしたときに、暫定制御力ｕ（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｕ（ｋ）とし、暫定制御力ｕ（ｋ）∈Ｘが満たされないときには指令値ｆ（ｋ）を、暫定制御力ｕ（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘが満たされるｆ（ｋ）を指令値ｆ（ｋ）とし、
時刻ｋ＋１において暫定制御力ｕ（ｋ＋１）を算出する場合に第３式に表れるｅ（ｋ）は次の第４式、即ち、
ｅ（ｋ）＝（Ｂ・ｅ（ｋ−１）＋Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ）・・・（第４式）
によって算出することを特徴とする位置制御方法。
【請求項１０】
前記集合Ｘは、設定されたスカラ値Ｆよりも絶対値が小さい全ての値の集合であり、前記指令装置は、移動機構の動力が出力することを許容されている力の大きさをＦとしたときに、｜ｕ（ｋ）｜＜Ｆが満たされる場合は指令値ｆ（ｋ）＝ｕ（ｋ）とし、｜ｕ（ｋ）｜＜Ｆが満たされない場合は指令値ｆ（ｋ）＝（Ｆ／｜ｕ（ｋ）｜）・ｕ（ｋ）とすることを特徴とする請求項８又は９に記載の位置制御方法。
【請求項１１】
前記集合Ｘは、設定されたスカラ値ＦよりもＬ２ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項８又は９に記載の位置制御方法。
【請求項１２】
前記集合Ｘは、設定されたスカラ値ＦよりもＬ無限大ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項８又は９に記載の位置制御方法。
【請求項１３】
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を利用して、物体の位置を目標位置に追従させる位置制御方法であり、
物体の位置を検出する位置検出ステップと、
物体に加える力の指令値を算出する指令値算出ステップと、
指令値算出ステップによって算出された力の指令値を移動機構へ出力するステップと、を含み、
前記指令値算出ステップは、さらに、
質量ゼロの仮想質点と、一端が物体の検出された位置に連結されており、他端が仮想質点に連結されている仮想的な保持要素と、を仮想空間内に設定するステップと、
スライディングモード制御則に基づいて、仮想質点の位置を目標位置に追従させる制御を仮想空間内で行った際に仮想保持要素から物体に加えられる力を移動機構へ出力する力の指令値として算出するステップと、
を含むことを特徴とする位置制御方法。
【請求項１４】
移動機構が動力によって物体に加える力の大きさの上限値が、移動機構の機械的な静止摩擦力と略同等以下の大きさに設定されており、
前記移動機構は、作業者が移動機構に操作力を加えるための操作子を備えることを特徴とする請求項８から１３のいずれか１項に記載の位置制御方法。
【請求項１５】
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、少なくとも物体の位置を検出する検出器を利用して、物体の位置を目標位置に追従させるプログラムであり、コンピュータによって、請求項８から１４のいずれか１項に記載の位置制御方法を実行させるための位置制御プログラム。
【請求項１】
物体の位置を目標位置に追従させる位置制御装置であり、
物体の位置を検出する位置検出器と、
物体の速度を取得する速度取得手段と、
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、
物体に加える力の指令値を移動機構へ出力する指令装置と、を備え、
指令装置は、
離散時間系における時刻をｋ、単位時間をＴ、時刻ｋにおける物体の目標位置をｐｄ（ｋ）、時刻ｋにおける物体の目標速度をｖｄ（ｋ）、時刻ｋにおいて位置検出器によって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度取得手段によって取得される物体の速度をｖ（ｋ）、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、第４の係数をＬ、時刻ｋにおいて指令装置内で演算される内部状態変数をａ（ｋ）としたときに、時刻ｋにおいて指令装置が出力する力の指令値ｆ（ｋ）を算出するための暫定制御力ｕ（ｋ）を次の第１式、即ち、
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋ａ（ｋ−１）・Ｌ
＋（ａ（ｋ−１）−ａ（ｋ−２））・（Ｈ・（Ｋ＋Ｌ・Ｔ）−Ｂ）／（Ｈ＋Ｔ）／Ｔ・・・（第１式）
によって算出し、
時刻ｋにおいて指令装置が算出する指令値ｆ（ｋ）は、移動機構の動力が出力することを許容されている力の集合をＸとしたときに、暫定制御力ｕ（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｕ（ｋ）とし、暫定制御力ｕ（ｋ）∈Ｘが満たされないときには指令値ｆ（ｋ）を、暫定制御力ｕ（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘが満たされるｆ（ｋ）を指令値ｆ（ｋ）とし、
時刻ｋ＋１において暫定制御力ｕ（ｋ＋１）を算出する場合に第１式に表れるａ（ｋ）は次の第２式、即ち、
ａ（ｋ）＝ａ（ｋ−１）・（Ｂ＋Ｋ・Ｔ）／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋（ａ（ｋ−１）−ａ（ｋ−２））・Ｂ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋ｆ（ｋ）・Ｔ・Ｔ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）・・・（第２式）
によって算出することを特徴とする位置制御装置。
【請求項２】
物体の位置を目標位置に追従させる位置制御装置であり、
物体の位置を検出する位置検出器と、
物体の速度を取得する速度取得手段と、
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、
物体に加える力の指令値を移動機構へ出力する指令装置と、を備え、
指令装置は、
離散時間系における時刻をｋ、単位時間をＴ、時刻ｋにおける物体の目標位置をｐｄ（ｋ）、時刻ｋにおける物体の目標速度をｖｄ（ｋ）、時刻ｋにおいて位置検出器によって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度検出手段によって取得される物体の速度をｖ（ｋ）、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、時刻ｋにおいて指令装置内で演算される内部状態変数をｅ（ｋ）としたときに、時刻ｋにおいて指令装置が算出する指令値ｆ（ｋ）を算出するための暫定制御力ｕ（ｋ）を次の第３式、即ち、
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋ｅ（ｋ−１）・（Ｈ・Ｋ−Ｂ）／（Ｈ＋Ｔ）・・・（第３式）
によって算出し、
時刻ｋにおいて指令装置が算出する指令値ｆ（ｋ）は、移動機構の動力が出力することを許容されている力の集合をＸとしたときに、暫定制御力ｕ（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｕ（ｋ）とし、暫定制御力ｕ（ｋ）∈Ｘが満たされないときには指令値ｆ（ｋ）を、暫定制御力ｕ（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘが満たされるｆ（ｋ）を指令値ｆ（ｋ）とし、
時刻ｋ＋１において暫定制御力ｕ（ｋ＋１）を算出する場合に第３式に表れるｅ（ｋ）は次の第４式、即ち、
ｅ（ｋ）＝（Ｂ・ｅ（ｋ−１）＋Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ）・・・（第４式）
によって算出されることを特徴とする位置制御装置。
【請求項３】
前記集合Ｘは、設定されたスカラ値Ｆよりも絶対値が小さい全ての値の集合であり、前記指令装置は、移動機構の動力が出力することを許容されている力の大きさをＦとしたときに、｜ｕ（ｋ）｜＜Ｆが満たされる場合は指令値ｆ（ｋ）＝ｕ（ｋ）とし、｜ｕ（ｋ）｜＜Ｆが満たされない場合は指令値ｆ（ｋ）＝（Ｆ／｜ｕ（ｋ）｜）・ｕ（ｋ）とすることを特徴とする請求項１又は２に記載の位置制御装置。
【請求項４】
前記集合Ｘは、設定されたスカラ値ＦよりもＬ２ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項１又は２に記載の位置制御装置。
【請求項５】
前記集合Ｘは、設定されたスカラ値ＦよりもＬ無限大ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項１又は２に記載の位置制御装置。
【請求項６】
物体の位置を目標位置に追従させる位置制御装置であり、
物体の位置を検出する位置検出器と、
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、
物体に加える力の指令値を移動機構へ出力する指令装置と、を備え、
指令装置は、
質量ゼロの仮想質点と、一端が物体の検出された位置に連結されており、他端が仮想質点に連結されている仮想的な保持要素と、を仮想空間内に設定し、
スライディングモード制御則に基づいて、仮想質点の位置を目標位置に追従させる制御を仮想空間内で行った際に仮想保持要素から物体に加えられる力を算出し、算出された力を指令値として移動機構へ出力することを特徴とする位置制御装置。
【請求項７】
移動機構が動力によって物体に加える力の大きさの上限値が、移動機構の機械的な静止摩擦力と略同等以下の大きさに設定されており、
前記移動機構は、作業者が移動機構に操作力を加えるための操作子を備えることを特徴とする請求項１から６のいずれか１項に記載の位置制御装置。
【請求項８】
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を利用して、物体の位置を目標位置に追従させる位置制御方法であり、
物体の位置を検出する位置検出ステップと、
物体の速度を検出する速度検出ステップと、
物体に加える力の指令値を算出する指令値算出ステップと、
指令値算出ステップによって算出された力の指令値を移動機構へ出力するステップと、を含み、
前記指令値算出ステップは、
離散時間系における時刻をｋ、単位時間をＴ、時刻ｋにおける物体の目標位置をｐｄ（ｋ）、時刻ｋにおける物体の目標速度をｖｄ（ｋ）、時刻ｋにおいて位置検出ステップによって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度取得ステップによって取得される物体の速度をｖ（ｋ）、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、第４の係数をＬ、時刻ｋにおける内部状態変数をａ（ｋ）としたときに、時刻ｋにおいて力の指令値ｆ（ｋ）を算出するための暫定制御力ｕ（ｋ）を次の第１式、即ち、
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）／（Ｈ＋Ｔ）
＋ａ（ｋ−１）・Ｌ
＋（ａ（ｋ−１）−ａ（ｋ−２））・（Ｈ・（Ｋ＋Ｌ・Ｔ）−Ｂ）／（Ｈ＋Ｔ）／Ｔ・・・（第１式）
によって算出し、
時刻ｋにおける指令値ｆ（ｋ）は、移動機構の動力が出力することを許容されている力の集合をＸとしたときに、暫定制御力ｕ（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｕ（ｋ）とし、暫定制御力ｕ（ｋ）∈Ｘが満たされないときには指令値ｆ（ｋ）を、暫定制御力ｕ（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘが満たされるｆ（ｋ）を指令値ｆ（ｋ）とし、
時刻ｋ＋１において暫定制御力ｕ（ｋ＋１）を算出する場合に第１式に表れるａ（ｋ）は次の第２式、即ち、
ａ（ｋ）＝ａ（ｋ−１）・（Ｂ＋Ｋ・Ｔ）／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋（ａ（ｋ−１）−ａ（ｋ−２））・Ｂ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）
＋ｆ（ｋ）・Ｔ・Ｔ／（Ｂ＋Ｋ・Ｔ＋Ｌ・Ｔ・Ｔ）・・・（第２式）
によって算出することを特徴とする位置制御方法。
【請求項９】
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を利用して、物体の位置を目標位置に追従させる位置制御方法であり、
物体の位置を検出する位置検出ステップと、
物体の速度を検出する速度検出ステップと、
物体に加える力の指令値を算出する指令値算出ステップと、
指令値算出ステップによって算出された力の指令値を移動機構へ出力するステップと、を含み、
前記指令値算出ステップは、
離散時間系における時刻をｋ、単位時間をＴ、時刻ｋにおける物体の目標位置をｐｄ（ｋ）、時刻ｋにおける物体の目標速度をｖｄ（ｋ）、時刻ｋにおいて位置検出ステップによって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度取得ステップによって取得される物体の速度をｖ（ｋ）、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、時刻ｋにおける内部状態変数をｅ（ｋ）としたときに、時刻ｋにおいて力の指令値ｆ（ｋ）を算出するための暫定制御力ｕ（ｋ）を次の第１式、即ち、
ｕ（ｋ）＝（ｐｄ（ｋ）−ｐ（ｋ））・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋（ｖｄ（ｋ）−ｖ（ｋ））・Ｈ・（Ｂ＋Ｋ・Ｔ）／（Ｈ＋Ｔ）
＋ｅ（ｋ−１）・（Ｈ・Ｋ−Ｂ）／（Ｈ＋Ｔ）・・・（第３式）
によって算出し、
時刻ｋにおいて算出する指令値ｆ（ｋ）は、移動機構の動力が出力することを許容されている力の集合をＸとしたときに、暫定制御力ｕ（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｕ（ｋ）とし、暫定制御力ｕ（ｋ）∈Ｘが満たされないときには指令値ｆ（ｋ）を、暫定制御力ｕ（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘが満たされるｆ（ｋ）を指令値ｆ（ｋ）とし、
時刻ｋ＋１において暫定制御力ｕ（ｋ＋１）を算出する場合に第３式に表れるｅ（ｋ）は次の第４式、即ち、
ｅ（ｋ）＝（Ｂ・ｅ（ｋ−１）＋Ｔ・ｆ（ｋ））／（Ｂ＋Ｋ・Ｔ）・・・（第４式）
によって算出することを特徴とする位置制御方法。
【請求項１０】
前記集合Ｘは、設定されたスカラ値Ｆよりも絶対値が小さい全ての値の集合であり、前記指令装置は、移動機構の動力が出力することを許容されている力の大きさをＦとしたときに、｜ｕ（ｋ）｜＜Ｆが満たされる場合は指令値ｆ（ｋ）＝ｕ（ｋ）とし、｜ｕ（ｋ）｜＜Ｆが満たされない場合は指令値ｆ（ｋ）＝（Ｆ／｜ｕ（ｋ）｜）・ｕ（ｋ）とすることを特徴とする請求項８又は９に記載の位置制御方法。
【請求項１１】
前記集合Ｘは、設定されたスカラ値ＦよりもＬ２ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項８又は９に記載の位置制御方法。
【請求項１２】
前記集合Ｘは、設定されたスカラ値ＦよりもＬ無限大ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項８又は９に記載の位置制御方法。
【請求項１３】
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を利用して、物体の位置を目標位置に追従させる位置制御方法であり、
物体の位置を検出する位置検出ステップと、
物体に加える力の指令値を算出する指令値算出ステップと、
指令値算出ステップによって算出された力の指令値を移動機構へ出力するステップと、を含み、
前記指令値算出ステップは、さらに、
質量ゼロの仮想質点と、一端が物体の検出された位置に連結されており、他端が仮想質点に連結されている仮想的な保持要素と、を仮想空間内に設定するステップと、
スライディングモード制御則に基づいて、仮想質点の位置を目標位置に追従させる制御を仮想空間内で行った際に仮想保持要素から物体に加えられる力を移動機構へ出力する力の指令値として算出するステップと、
を含むことを特徴とする位置制御方法。
【請求項１４】
移動機構が動力によって物体に加える力の大きさの上限値が、移動機構の機械的な静止摩擦力と略同等以下の大きさに設定されており、
前記移動機構は、作業者が移動機構に操作力を加えるための操作子を備えることを特徴とする請求項８から１３のいずれか１項に記載の位置制御方法。
【請求項１５】
物体を保持しており、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、少なくとも物体の位置を検出する検出器を利用して、物体の位置を目標位置に追従させるプログラムであり、コンピュータによって、請求項８から１４のいずれか１項に記載の位置制御方法を実行させるための位置制御プログラム。
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図１０】
【図１１】
【図１２】
【図１３】
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２００７−１０２７４８（Ｐ２００７−１０２７４８Ａ）
【公開日】平成１９年４月１９日（２００７．４．１９）
【国際特許分類】
【出願番号】特願２００５−３５８８２４（Ｐ２００５−３５８８２４）
【出願日】平成１７年１２月１３日（２００５．１２．１３）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り ２００５年９月１５日 社団法人日本ロボット学会発行の「第２３回日本ロボット学会学術講演会予稿集」に発表
【出願人】（３０４０２１２７７）国立大学法人 名古屋工業大学 (784)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成１９年４月１９日（２００７．４．１９）
【国際特許分類】
【出願日】平成１７年１２月１３日（２００５．１２．１３）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り ２００５年９月１５日 社団法人日本ロボット学会発行の「第２３回日本ロボット学会学術講演会予稿集」に発表
【出願人】（３０４０２１２７７）国立大学法人 名古屋工業大学 (784)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]