JP2008097152A - 位置制御装置、位置制御方法及び位置制御プログラム - Google Patents

Abstract

【目的】物体の位置を、物体を保持する移動機構によって速度制限を付しながら目標位置に速やかに追従させる技術を提供する。
【構成】本発明に係る位置制御装置は、物体を保持して移動させる移動機構と、物体に加える力の指令値を移動機構へ出力するコントローラを備える。図はコントローラ内の制御ブロック図である。図においてｐ_ｄは物体の位置を表し、ｐ_sは仮想質点の位置を表す。仮想質点は物体と仮想的な連結要素で連結されている。コントローラは、スライディングモード制御に基づいて物体に加えるべき力ｆを算出する。移動機構は、力ｆを物体に加えるように制御され、物体を移動させる。コントローラ内で力ｆを算出する過程において、移動機構によって実現される物体の速度の大きさに上限値Ｖが設定される。これにより、物体の速度の大きさを上限値Ｖ以下に制限しながら物体を目標位置へ追従させる位置制御が実現される。
【選択図】図７

Description

本発明は、移動させる速度を制限しながら物体の位置を目標位置に追従させる位置制御装置、位置制御方法および位置制御プログラムに関する。

産業用ロボットに代表されるように、物体の位置を所定の目標位置へ位置決めする、或いは経時的に変化する目標位置に追従させる位置制御装置が知られている。位置制御装置は、動力（アクチュエータ）を有しており、物体を保持して移動させる移動機構と、移動機構（より正確には移動機構のアクチュエータ）に対して指令値を出力するコントローラを備える。
ロボットなどの位置制御装置では、移動機構が物体を過度な速度で移動させることがないように、コントローラ内に、移動機構の動作が所定の速度を超えないように制限する速度制限ロジックを組み込むことが望ましい。なお、本明細書では、物体の位置という場合にはその物体の位置及び／又はその物体の姿勢を意味するものとし、速度という場合には並進速度及び／又は角速度を意味するものとする。また、本明細書では後に「力」という表現を用いるが本明細書で用いる「力」は並進力及び／又はトルクを意味するものとする。
速度制限ロジックを組み込んだ位置制御技術の例として、たとえば非特許文献１及び２には、受動性に基づくエネルギ成形方法（A passivity-based energy shaping scheme）（例えば M. Takegaki and S. Arimoto, “A new feedback method for dynamic control of manipulator,” Trans. of the ASME: J. of Dynamic Systems,Measurement, and Control, vol. 102, pp. 109-125, 1981.に記載されている）に基づく位置或いは速度の制限を付加した制御方法（Some control schemes for imposing state (position and/or velocity) constraints）が開示されている。
或いは他の例として、非特許文献３には、時間軸変換を用いた速度制限の方法（Rate constraints using time-scale transformations）が開示されている。

K. B. Ngo and R. Mahony, "Passivity-based control of robot manipulators subject to constraints," in Proc. of the 2005 Australian Conf. on Roboticsand Automation, 2005. "Bounded torque control for robot manipulators subject to jointvelocity constraints," in Proc. of the 2006 IEEE Int. Conf. on Robotics and Automation, 2006, pp. 7-12. T. Sugie, K. Fujimoto, and Y. Kito, "Obstacle avoidance of manipulators with rate constraints," IEEE Trans. on Robotics and Automation, vol. 19, no. 1, pp. 168-174, 2003.

位置制御装置において、物体の速度を制限する良好な制御ロジックを実際の位置制御装置に実装するのは困難である場合が多いといわれている。ここでいう「良好」とは、例えば、物体の速度が制限値以下であるときの追従精度を犠牲にすることなく、物体の速度を制限できること、或いは、目標位置と物体の位置の偏差が大きくなったときに確実に物体の速度を制限できること、などである。
上記の非特許文献１及び２に開示された技術では、速度制限を付加することによって位置追従精度が低下してしまう。上記の非特許文献３に開示された技術では、上位のコントローラ（例えば物体の移動軌道を生成するコントローラ）から過大な目標位置指令値が入力されたときに下位のコントローラ（上位のコントローラからの指令に基づいて移動機構を制御するコントローラ）は速度を制限することができない。
本願発明は上記の課題に鑑みてなされたものであり、その目的は、良好な速度制限ロジックを組み込むことのできる位置制御技術を提供することにある。特に、位置追従性能を損なわずに、物体を所定の速度以下で移動させて目標位置に追従させることのできる位置制御技術を提供することにある。

発明者らは既に、従来のスライディングモード制御を発展させた制御ロジックを組み込むことによって優れた特性を有する位置制御装置を提案した。その位置制御装置は、正常動作時における高追従性と異常時における移動機構の予想外の動作の防止とを両立することができる。その技術については文献１（R. Kikuuwe and H. Fujimoto, "Proxy-based sliding mode control for accurate and safe position control", in Proc. of the 2006 IEEE Int. Conf. on Robotics and Automation, 2006. pp26-31）に詳しく記載されている。
本発明は、発明者らが既に開発した位置制御技術に速度制限のロジックを組み込むことによって、位置追従性能を損なわずに、物体を所定の速度以下で移動させて目標位置に追従させることのできる位置制御装置、位置制御方法、及び位置制御プログラムを実現するものである。

以下、本発明の特徴である制御則を導出する過程を説明する。なお、以下の説明で用いる記号は基本的にｎ次元のベクトル又はマトリクスを表し、記号がスカラ値を表す場合にはその都度述べることにする。記号「０」はゼロベクトルを表す。

まず、制御則を導出する際に利用する定理とその系を説明する。以下に説明する定理とその系は、発明者らが見出したものである。
ｎ次元関数ｓｇｎ（・）とｓａｔ（・）を定義する。
ｓｇｎ（・）は、ｎ次元空間Ｒ^ｎからＲ^ｎへ写像する正規化関数であり次の（１）式で定義される。

ここで、ｘは任意のｎ次元ベクトルを表し、‖ｘ‖は、ベクトルｘの大きさ、即ちノルムを表す。なお、本明細書を通して記号‖ｘ‖は、ベクトルｘのノルムを表す。（１）式の関数ｓｇｎ（・）は、ｎ次元に拡張した符号関数（signum function）である。（１）式によるｓｇｎ（・）の定義のもとでは、ｙ＝ｓｇｎ（ｘ）は次の（２）式で表される関係と等価である。

ｓａｔ（・）は、ｎ次元空間Ｒ^ｎからＲ^ｎへ写像する単位飽和関数であり次の（３）式で定義される。

関数ｓｇｎ（・）とｓａｔ（・）は、次の定理１と系１で示される関係を有する。
＜定理１＞ ２つのｎ次元ベクトルｘとｙについて、次の（４）式を満たす関係が成立する。

＜定理１の証明＞
関数ｓｇｎ（・）の定義から、ｘとｙは次の（５）式の関係を満足することは明らかである。

ここで、ｘとｙは、ｙ＝（ｘ−ｙ）／‖ｘ−ｙ‖、及びｘ≠ｙの関係を満足すると仮定する。ｒ＝‖ｘ−ｙ‖＞０と置くと（ｒはスカラ値）、仮定した関係は（１＋ｒ）ｙ＝ｘと記述することができる。このとき、‖ｙ‖＝１であるので‖ｘ‖＝１＋ｒの関係が成立する。このことは、‖ｘ‖＞１であることを意味する。よって、‖ｘ‖＝１＋ｒの関係を（１＋ｒ）ｙ＝ｘの関係に代入すると、ｙ＝ｘ／‖ｘ‖の関係を得る。以上より次の（６）式を得る。

上記の逆が成立することは自明である。また、（ｙ＝ｘ∧‖ｙ‖≦１）が（ｙ＝ｘ∧‖ｘ‖≦１）と等価であることも自明である。よって次の（７）式を得る。

以上で定理１が証明された。

＜系１＞ ２つのｎ次元ベクトルｘ、ｙ、及び２つの正値のスカラ値Ｘ、Ｙについて、次の（８）式を満たす関係が成立する。

＜系１の証明＞
Ｚ＞０である全てのスカラ値Ｚ、及びｚ∈Ｒ^ｎを満たすｚに対してｓｇｎ（Ｚｚ）＝ｓｇｎ（ｚ）が成立する。よって（８）式は次の（９）式に書き直すことができる。

定理１より、上記（９）式の関係が成立する。以上で系１が証明された。

図１に、定理１のブロック図を示す。定理１は、発明者らが以前に導出した定理（前述の文献１に開示されている）の多次元への拡張である。定理１と系１は、「不連続関数ｓｇｎ（・）が時間遅れのない閉ループ系に含まれている場合には、その閉ループ内の不連続関数ｓｇｎ（・）は、連続関数ｓａｔ（・）を用いることにより除去することができる」という事実を示唆する。
以上で定理１と系１についての説明を終る。

次に、図２に模式的に示す位置制御装置１００を例として、本発明に係る位置制御装置の特徴をなす制御則を説明する。位置制御装置１００は、移動機構１０とコントローラ２０を備える。移動機構１０はその基部１２で絶対座標系に対して固定されている。移動機構１０の先端は、位置制御する対象である制御対象物１４（請求項の「物体」に相当する）を保持することができる。制御対象物１４を保持する機構はどのようなものでもよい。移動機構１０は、複数の関節とリンクが連結されている多リンク機構を有しており、各関節には連結されたリンク同士を揺動させるアクチュエータが取り付けられている。移動機構１０はコントローラ２０からの指令に基づいて、制御対象物１４を移動させることができる。
移動機構１０には、保持した制御対象物１４の位置を検出するセンサが取り付けられている。具体的には移動機構１０の各関節にエンコーダ（不図示）が取り付けられており、各エンコーダの値からロボット工学における順キネマティクスを用いて先端に保持した制御対象物１４の位置を算出する。またコントローラ２０は、制御対象物１４の位置を時間微分して速度を算出する。なお、本明細書では、制御対象物１４の位置を直接検出するセンサ、間接的に検出するセンサ（例えば上記のエンコーダ）を含めて位置検出器と称する。また、制御対象物１４の速度を直接検出するセンサ、間接的に検出するセンサ、或いは位置から速度を推定する推定手段を含めて制御対象物１４の速度を検出する速度検出器と称する。速度を間接的に検出するとは例えば移動機構の各関節にジャイロを備え、各ジャイロの出力から制御対象物１４の速度を求めることである。また、制御対象物１４の位置を時間微分することによって制御対象物１４の速度を得ることは換言すれば制御対象物１４の速度を推定することであるが、本発明では制御対象物１４の速度が直接検出したものであっても推定によって得たものであってもよいため、速度を推定する手段を含めて速度検出器と称する。
コントローラ２０からは移動機構１０の先端が制御対象物１４に加えるべき力が出力される。移動機構１０は、コントローラが出力した力の指令を実現するように動作する。例えば、移動機構１０の先端で発生する力をｆ、そのときに移動機構１０の各関節が発生するトルクをτ、移動機構１０のヤコビ行列をＪとすると、ｆ＝Ｊ^Ｔτの関係が成立する。この関係を用いることで、移動機構１０の先端で発生すべき力ｆを実現するための各関節のトルクτを求めることができる。移動機構１０の各関節に配置されたアクチュエータがこのトルクτを発生することによって、移動機構１０の先端で力ｆを発生させることができる。即ち、移動機構１０が制御対象物１４へ力ｆを加えることができる。
なお、図２に示す位置制御装置１００は、本発明の特徴をなす制御則を説明するための例示であり、移動機構１０の物理的な構造（例えば関節の数やリンクの接続関係など）を限定するものではない。

位置制御装置１００によって、ｎ次元直交空間内で制御対象物１４を目標位置に追従させる場合を考察する。目標位置を記号ｐ_ｄで表し、制御対象物１４の位置を記号ｐで表す。目標位置ｐ_ｄは経時的に変化する値であってもよい。目標位置ｐ_ｄの時間微分及び制御対象物１４の位置ｐの時間微分を夫々ｖ_ｄとｖで表す。即ち、ｖ_ｄは目標速度を表し、ｖは制御対象物１４の速度を表す。
コントローラ２０は、目標位置ｐ_ｄと位置ｐを入力として、移動機構１０の先端が発生すべき力ｆ（先端が制御対象物１４へ加えるべき力ｆ）の指令値を出力する。移動機構１０は、コントローラ２０が出力する力ｆの指令値に基づいて、制御対象物１４に対して力ｆを加えるように制御される。コントローラ２０が出力する力ｆの指令値に基づいて移動機構１０の先端が制御対象物１４へ力ｆを加えることによって、制御対象物１４の位置ｐを目標位置ｐ_ｄへ移動させることができる。

まず、発明者らが先に提案した位置制御装置に組み込まれる制御ロジック（以下、ＰＳＭＣ（プロキシベースのスライディングモード制御）と称する）の概念について図２と図３を参照して説明する。ＰＳＭＣについては前述した文献１に詳説されている。図３は、図２に示すコントローラ２０内に実現される制御則を示すブロック図である。
図２に示すように、ＰＳＭＣではプロキシ（proxy）と呼ぶ質量ゼロの仮想質点を設定する。仮想質点は、制御ロジック内で想定される点であり、実際には存在しない。仮想質点の位置を記号ｐ_ｓで表し、仮想質点の速度を記号ｖ_ｓで表す。
なお、「制御ロジック内で想定する」とは、より具体的には制御ロジックを構築する上で仮に想定するという意味である。請求項の「仮想空間内に設定する」との表現も同様の意味である。

仮想質点は制御対象物１４と仮想的な連結要素で連結されている。この仮想的な連結要素も仮想質点と同様に制御ロジック内で想定される点であり、実際には存在しない。仮想連結要素は弾性を有している。
制御ロジック内で、仮想質点に対して加える力ｆを想定する。仮想質点は質量がゼロであり、かつ制御対象物１４と弾性を有する仮想連結要素で連結されているため、仮想質点に加える力ｆは制御対象物１４でも発生する。制御ロジック内では仮想質点と仮想連結要素を想定するが、コントローラ２０は実際にはこの力ｆを移動機構１０の指令値として出力する。以下で説明するように、力ｆは、仮想質点を目標位置へ移動させるための力として算出される。そのように算出された力ｆを移動機構１０によって制御対象物１４へ加えることで制御対象物１４を移動させる。なお、質量ゼロの仮想質点へ力ｆを加えるとは、仮想質点のダイナミクスにおいて慣性項が存在しないことを意味する。この制御ロジックは、仮想質点の位置ｐ_ｓが、仮想連結要素が発生する力と力ｆがバランスする位置となることに着目する。
なお、図２に示すｈは、システムに加わる外力を表している。

仮想連結要素は、その長さを自然長に維持するために、ＰＩＤ制御に基づく力を発生するものと設定する。但し、その自然長はゼロである。即ち、仮想質点の位置ｐ_ｓが目標位置ｐ_ｄに一致し、かつ仮想連結要素が自然長となったとき（仮想連結要素が力を発生しない状態となったとき）に、制御対象物１４の位置ｐは目標位置ｐ_ｄに一致することになる。
図３に示すＬ／ｓ＋Ｋ＋Ｂｓのブロックがこの仮想連結要素の特性を示している。ここで、Ｌは積分ゲインであり、Ｋは比例ゲインであり、Ｂは微分ゲインを表す。なお、ｓはラプラス演算子である。即ち、仮想連結要素が発生する力ｆは、次の（１０）式を満足する。

図３に示すスライディングモード制御部は、スライディングモード制御（ＳＭＣと称する場合がある）に基づいて力ｆを算出する。前述の文献１では、次の（１１）式で示される制御則を採用した。

ここで、記号ＦとＨは正値のスカラ値である。（１１）式で示される制御則のブロック図を図４に示す。σ_ｓ＝０を満たす点の集合は、仮想質点のｎ次元位置ｐ_ｓとｎ次元速度ｖ_ｓによって張られる２ｎ次元の状態空間においてｎ次元の部分空間となる。なお、本明細書で用いる「多様体」との用語は、数学上で定義され用いられるものと同義である。以下ではそのような多様体を多様体σ_ｓ＝０と称する。図５に多様体σ_ｓ＝０の模式図を示す。図５に示す灰色の太線が多様体σ_ｓ＝０である。この多様体σ_ｓ＝０はスライディング多様体（sliding manifold）と呼ばれており、（１１）式で示される制御則は、仮想質点の状態（ｐ_ｓ、ｖ_ｓ）をこのスライディング多様体に引き付けるように機能する。
なお、図３に示すｈは、システムに加わる外力を表している。

（１１）式で表される制御則を用いた場合、仮想質点の状態がスライディング多様体に達する前に、仮想質点が過度の速度となる可能性がある。例えば、制御対象物１４の位置ｐと目標位置ｐ_ｓとの偏差が大きい場合である。偏差が大きいと、目標位置へ素早く追従させるために移動機構１０が制御対象物１４を高速で移動させる場合があるからである。また、コントローラ２０の上位のコントローラが、何らかの不具合で過大な目標速度をコントローラ２０へ与える場合などである。即ち、位置制御装置１００が（１１）式によって表される制御則を用いた場合、制御対象物１４が過度の速度で移動する可能性がある。そこで、制御対象物１４が過度の速度とならないように、次の（１２）式で表わされるＳＭＣ制御則を提案する。

ここで、ＶとＡは、正値のスカラ値である。
ｓ_ｓ＝０を満足する点の集合が仮想質点の２ｎ次元の状態空間｛ｐ_ｓ、ｖ_ｓ｝におけるｎ次元の多様体を表す。図６に、ｓ_ｓ＝０で表される多様体を模式的に示す。図６に示す灰色の太線が多様体ｓ_ｓ＝０である。
（１２ｂ）式の右辺第１項のノルムは常にＶよりも小さいので、（１２ｂ）式は、ｓ_ｓ＝０が‖ｖ_ｓ‖≦Ｖを満足することを示している。このことは、仮想質点の状態がｓ_ｓ＝０で表される多様体上にある限りにおいて、仮想質点の速度の大きさがＶ以下となることを意味する。ｓ_ｓ＝０と‖ｖ_ｓ‖≦Ｖが成立する場合には‖Ａσ_ｓ＋ｖ_ｓ‖は１より小さくなり、その結果、Ａσ_ｓ＋ｖ_ｓ−ｖ_ｓ＝０の関係が満足される。このことは、ｓ_ｓ＝０と‖ｖ_ｓ‖≦Ｖが成立する場合には、σ_ｓ＝０が満足されることを意味する。それゆえ、‖Ａσ_ｓ＋ｖ_ｓ‖≦Ｖが満足される場合は常に（１２）式は（１１）式と等価となる。なお、パラメータＡは多様体ｓ_ｓ＝０には影響を与えないことに留意されたい。
図５と図６を比較すると理解できるように、（１１）式の制御則における多様体σ_ｓ＝０（図５）が、有界でないのに対して、（１２）式の制御則における多様体ｓ_ｓ＝０（図６）は、速度ｖ_ｓの軸に対して有界となる。

図７に、（１２）式で表される制御則のブロック図を示す。図７において灰色の部分が、図４に示したこれまでのＰＳＭＣとの違いである。なお、図７中の記号ｓはラプラス演算子を表す。
図３と図７によって表現される制御則は、次の（１３）式を満足する。

ここで、力ｆは、（１３ａ）式と（１３ｂ）式の双方を満足することに留意されたい。
（１３）式で表される制御則を実装すると、図２に示す位置制御装置１００は、制御対象物１４の速度の大きさをＶ以下に抑制しながら、制御対象物１４の位置ｐを目標位置ｐ_ｓに追従させることができる。

（１３）式の制御則は連続時間系で表現されており、現実の位置制御装置へ実装するには（１３）式を離散時間系で表現し直すことが必要である。そこで次に（１３）式の離散時間系への変換について説明する。
（１３）式をオイラー近似に基づいて離散時間系へ変換すると次の（１４）式を得る。

ここで、ｋは、時間の経過を離散化したときの時刻ｋを表す。記号∇は後退差分演算子を表す。具体的には記号∇は、∇ｘ（ｋ）＝ｘ（ｋ）−ｘ（ｋ−１）と∇^２ｘ（ｋ）＝∇ｘ（ｋ）−∇ｘ（ｋ−１）＝ｘ（ｋ）−２ｘ（ｋ−１）＋ｘ（ｋ−２）の両式を満足する演算子として定義される。以下では、ｖ（ｋ）＝∇ｐ（ｋ）／Ｔ、ｖ_ｓ（ｋ）＝∇ｐ_ｓ（ｋ）／Ｔ、及び、ｖ_ｄ（ｋ）＝∇ｐ_ｄ（ｋ）／Ｔと置く。また、ａ（ｋ）は、（１４）式が表す制御ロジックを簡略化して表現するための内部状態変数である。
なお、（１４）式の表現は、コンピュータによる演算を考慮した表現とはなっていないが、制御対象物１４の速度の大きさをＶ以下に抑制するために満足すべき条件を網羅している。システムへの入力は目標位置ｐ_ｄ（ｋ）と制御対象物１４の位置ｐ（ｋ）、及びそれらの時間微分値（即ち速度）である。従って制御ロジック内ではこれらの値は既知であるものとして取り扱うことができる。それらの入力値が与えられると、（１４）式によって、時刻ｋにおける位置制御装置１００の移動機構１０が制御対象物１４へ加えるべき力ｆ（ｋ）と内部状態変数ａ（ｋ）（及びその時間部分∇ａ（ｋ））を決定することができる。

図３と図７は、制御則において、不連続関ｓｇｎ（・）が時間遅れなしにフィードバックループ内に含まれていることを示している。従って、前述した系１により、（１４ｂ）式内の関数ｓｇｎ（・）は除去することができ、少なくとも（１４ｃ）式の右辺第１項が既知ならば（１４）式の解析的な解を得ることができる。
「Ａ」は任意の正値のスカラ値であればよいので、解析的な導出が容易となるように「Ａ」を選定すると好都合である。そこで、「Ａ」を含め、新たに第１中間値ｕ^＊（ｋ）を次の（１５）式により定義する。

ｖ_ｓ（ｋ）とσ_ｓ（ｋ）は次の（１６）式と（１７）式の通り書き換えることができる。

なおここでは、∇ａ（ｋ）は未知である。「Ａ」を次の（１８）式の通り置くことによって、第１中間値ｕ^＊（ｋ）を（１９）式の通り表現し直すことができる。

（１９）式は、ｕ^＊（ｋ）が∇ａ（ｋ）とは独立であることを示している。このことは、（１８）式の通り設定することによって、ｕ^＊（ｋ）を既知の値として扱うことができることを意味している。

第２中間値ｕ（ｋ）を次の（２０）式の通り設定する。

（２０）式のｕ（ｋ）によって、（１４ｂ）式は（２１）式の通り書き換えることができる。

ここで、ｖ_ｓ（ｋ）は、未知の変数ｆ（ｋ）に依存する。（１４ａ）式から次の（２２）式を得ることができる。

（２２）式を（１６）式へ代入すると次の（２３）式を得る。

第３中間値ｆ^＊（ｋ）を次の（２４）式の通り設定する。

このｆ^＊（ｋ）を用いることによって次の（２５）式を得る。

上記（２５）式を（２１）式に代入することによって次の（２６）式を得る。

ここで、系１を用いることによって、次の（２７）式を得る。

（２７）式によって、時刻ｋにおいて図２の移動機構１０が制御対象物１４へ与えるべき力ｆ（ｋ）を得る。なお、内部状態変数ａ（ｋ）は、（１４ａ）式によって算出することができる。

以上の説明をまとめると次の結論を得る。速度の大きさをＶ以下に抑制しながら制御対象物１４の位置ｐを目標位置ｐ_ｄへ追従させるために時刻ｋにおいて移動機構１０が制御対象物１４へ加えるべき力ｆ（ｋ）は次の（２８）式によって算出することができる。また（２８ｅ）式は、ａ（ｋ）が満たすべき漸化式であり、この式から各時刻ｋにおけるａ（ｋ）を求めることができる。

（２８ｂ）式は、（２８ａ）式によって求められる第１中間値ｕ^＊（ｋ）を関数ｓａｔ（・）で制限したものを第２中間値ｕ（ｋ）とすることを意味している。関数ｓａｔ（・）による（２８ｂ）式の制限は、数学的に次の通り一般化できることが自明である。即ち、「第２中間値ｕ（ｋ）は、移動機構によって実現される物体（制御対象物１４）の速度として許容されている速度の集合をＹとしたときに、ｕ^＊（ｋ）∈Ｙが満たされるときにはｕ（ｋ）＝ｕ^＊（ｋ）を満たす値であり、ｕ^＊（ｋ）∈Ｙが満たされないときにはｕ^＊（ｋ）に正のスカラ値を乗じた値であり、かつｕ（ｋ）∈Ｙを満たす値である。」
同様に、（２８ｄ）式は、（２８ｃ）式によって求められる第３中間値ｆ^＊（ｋ）を関数ｓａｔ（・）で制限したものを力ｆ（ｋ）とすることを意味している。従って（２８ｄ）式は、数学的に次の通り一般化できることが自明である。即ち、「力の指令値ｆ（ｋ）は、移動機構が物体に加えることを許容されている力の集合をＸとしたときに、ｆ^＊（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｆ^＊（ｋ）で求められ、ｆ^＊（ｋ）∈Ｘが満たされないときにはｆ^＊（ｋ）に正のスカラ値を乗じた値として求められる。」

図３に示すように上記の説明においては、仮想連結要素の特性を「Ｌ／ｓ＋Ｋ＋Ｂｓ」と設定した。ここで、積分ゲインＬをゼロとしても上記と同様の効果を得る制御則が得られる。これは仮想連結要素の特性を、その長さを自然長に維持するためにＰＤ制御に基づく力を発生するものと設定することを意味する。
（２８）式を導出した上記の過程においてＬ＝０と置くと次の（２９）式を得る。

（２８ｂ）式、或いは（２９ｂ）式は、前述した集合Ｙを用いると次の通り表現することと等価である。即ち、「前記集合Ｙは、設定されたスカラ値Ｖよりも絶対値が小さい全ての値の集合であり、位置制御装置のコントローラは、移動機構によって実現される物体の速度として許容されている速度の大きさをＶとしたときに、‖ｕ^＊（ｋ）‖＜Ｖが満たされる場合は第２中間値ｕ（ｋ）＝ｕ^＊（ｋ）とし、‖ｕ^＊（ｋ）‖＜Ｖが満たされない場合は第２中間値ｕ（ｋ）＝（Ｖ／‖ｕ^＊（ｋ）‖）・ｕ^＊（ｋ）とする。」

同様に、（２８ｄ）式、或いは（２９ｄ）式は、前述した集合Ｘを用いると次の通り表現することと等価である。即ち、「前記集合Ｘは、設定されたスカラ値Ｆよりも絶対値が小さい全ての値の集合であり、位置制御装置のコントローラは、移動機構が物体に加えることを許容されている力の大きさをＦとしたときに、‖ｆ^＊（ｋ）‖＜Ｆが満たされる場合は指令値ｆ（ｋ）＝ｆ^＊（ｋ）とし、‖ｆ^＊（ｋ）‖＜Ｆが満たされない場合は指令値ｆ（ｋ）＝（Ｆ／‖ｆ^＊（ｋ）‖）ｆ^＊（ｋ）とする。」

（２８ａ）式、（２８ｃ）式、及び（２８ｅ）式がそれぞれ請求項の第１式、第２式、及び第３式である。（２９ａ）式、（２９ｃ）式、及び（２９ｅ）式がそれぞれ請求項の第４式、第５式、及び第６式である。

上記の表現は、さらにベクトルのノルムを用いて表現し直すことができる。代表的なノルムにＬ２ノルムがある。そこで上記の集合ＹやＸを次の通り特定してよい。
集合Ｙは、設定されたスカラ値ＶよりもＬ２ノルムが小さい全てのベクトル値の集合である。或いは、集合Ｘは、設定されたスカラ値ＦよりもＬ２ノルムが小さい全てのベクトル値の集合である。
また、他の代表的なノルムにＬ無限大ノルム（一般にＬ_∞と表現することがある）がある。そこで上記の集合ＹやＸを次の通り特定することも好適である。
集合Ｙは、設定されたスカラ値ＶよりもＬ無限大ノルムが小さい全てのベクトル値の集合である。或いは、集合Ｘは、設定されたスカラ値ＦよりもＬ無限大ノルムが小さい全てのベクトル値の集合である。

一方、図６に示すスライディング多様体σ_ｓ＝０についても、次の通り数学的に一般化することができる。
図６に示すように、スライディング多様体σ_ｓ＝０は、少なくとも仮想質点の速度を状態変数に含む仮想質点の状態空間内の部分多様体である。そして、（１２）式は、スライディング多様体σ_ｓ＝０上で仮想質点の速度が有界であることを表している。

本発明の特徴は、発明者らが提案したＰＳＭＣに速度制限を付加したことにある。その際、発明者らが導出した新たな定理とその系を用いることによって、ＰＳＭＣに適切に速度制限を組み込むことが可能となった。速度制限を付加したＰＳＭＣを以下、ＶＢ−ＰＳＭＣ（Velocity Bounding PSMC）と称する。
次に、ＶＢ−ＰＳＭＣをＰＳＭＣとの対比において説明する。
（２８）式によって表されるＶＢ−ＰＳＭＣの制御則から速度制限を外すと（即ち、（２８）式においてＶを無限大とすると）、次の（３０）式を得る。

（３０）式は、（１１）式によって与えられる線形スライディング多様体に基づくＰＳＭＣの制御則を表す。（１１）式によって与えられる線形スライディング多様体とは、図５において太線で示す多様体であり、図６においてσ_ｓ＝ｐ_ｄ−ｐ_ｓ＋Ｈ（ｖ_ｄ−ｖ_ｓ）＝０を満たす点の集合である。
前述した文献１で述べているように、ＰＳＭＣはＰＩＤ制御を拡張したものと捉えることができる。従って、ＶＢ−ＰＳＭＣもＰＩＤ制御を拡張したものと捉えることができる。（３０）式においてＦを無限大とすると次の（３１）式を得る。

（３１）式は、次の（３２）式と等価であることは自明である。

ここで、（３２）式を見やすくするため次の（３３）式を採用する。

（３３）式を採用することによって、（３２）式は次の（３４）式の通り書き換えることができる。

（３４）式においてｋを無限大とすると次の（３５）式を得る。

（３５）式は、ＰＩＤ制御則と等価である。
以上より、（２８）式で表されるＶＢ−ＰＳＭＣは、‖ｕ^＊（ｋ）‖≦Ｖと‖ｆ^＊（ｋ）‖≦Ｆが満足される限りにおいて（３５）式で表されるＰＩＤ制御則と等価である。このことは、‖ｕ^＊（ｋ）‖≦Ｖと‖ｆ^＊（ｋ）‖≦Ｆが満足されている状態においては（即ち、制御対象物１４の速度の大きさが制限速度の大きさＶに至らない状態においては）、スカラ値Ｆ、Ｖ及びＨは追従性能に影響しないことを意味している。即ち、ＶＢ−ＰＳＭＣは、（３５）式で表されるＰＩＤ制御則に次の特徴を付加した制御則であるということができる。
・移動機構が物体（移動機構が保持した制御対象物）に加えるべき力の指令値に、任意の大きさＦの制限を加えることができる。
・移動機構によって実現する物体の移動速度に、任意の大きさＶの制限を加えることができる。
・移動機構によって実現する物体の位置の目標位置への追従特性として、任意の時定数Ｈによる減衰特性を与えることができる。
ＶＢ−ＰＳＭＣ制御則は、ＰＩＤ制御ゲインＫ、Ｌ、及びＢとは独立に上記のＦ、Ｖ、及びＨの３パラメータを設定できる点に、従来のＰＩＤ制御に優る利点がある。パラメータＦとＶは、位置制御装置が使用される環境に応じて設定することができる。

本願発明に係る位置制御装置に実装されるＶＢ−ＰＳＭＣ制御ロジックの概要をまとめると次の通りである。
（１）移動機構が保持した物体に対して質量ゼロの仮想質点が連結されていると設定し、その仮想質点を目標位置に追従させるようにスライディングモード制御則を構築する。ここで、仮想質点と物体は仮想的な連結要素（少なくとも弾性を有する）で連結されており、仮想質点は、仮想質点と物体の位置偏差に応じて連結要素が発生する力（弾性による復元力）を受ける。スライディングモード制御則は、連結要素から力を受ける仮想質点に制御力として加える力を算出する。この制御力は、仮想質点を目標位置まで移動させる力として算出される。算出された力を物体に加える指令値として移動機構へ出力する。
（２）スライディングモード制御則が用いるスライディング多様体として、次の条件を満たす多様体を採用する。
・スライディング多様体は、仮想質点の速度を含む状態空間（上記の説明では仮想質点の位置ｐ_ｓと速度ｖ_ｓが張る状態空間｛ｐ_ｓ，ｖ_ｓ｝）における部分多様体である。
・スライディング多様体は、その多様体上で仮想質点の速度が有界である。
なお、仮想質点の速度が有界であるとは、例えば物体の運動が所定の一方向に規制されている場合にはその速度の大きさにスカラ値である上限値Ｖを課すことに相当する。
上記の（１）は、ＰＳＭＣの特徴でもあるが、「質量ゼロの仮想質点」を新たに導入し、その仮想質点に対して位置制御する制御力を求めるスライディングモード制御である点が従来のＰＩＤ制御、或いはスライディングモード制御と異なる点である。
仮想質点は仮想的な連結要素を介して実際の物体に連結されている。一方、仮想質点は質量がゼロであるので仮想質点の慣性力は制御ロジック内に現れない。従って、仮想質点の位置（運動）を制御することはそのまま実際の物体を制御することに繋がる。一方、仮想質点の運動は、制御ロジック内で仮想される運動であるので精密に制御することができる。制御ロジック内において、仮想質点をスライディングモード制御の対象とすることによって、従来のスライディングモード制御が有していたチャタリングの生じ易さを抑制することに成功した。
そのようなＰＳＭＣに対して、さらに仮想質点の速度に対して上限値を付加した点が上記（２）の特徴である。特に、実際の物体ではなく、仮想質点の運動に対して速度制限を加えている点が特徴である。前述したように、制御ロジック内で仮想される仮想質点の運動は精密に制御することができる。従って、仮想質点に対する速度制限は、確実に仮想質点の運動に反映することができる。仮想質点の運動に速度制限を確実に反映できるということは、実際の物体に対して速度制限を確実に反映できることを意味する。
制御ロジック内で実際の物体に速度制限を付加しても、実際の物体には例えば図２や図３に示すように未知の外力ｈが作用する。そのため、物体に速度制限を確実に課すことが困難となる。ＶＢ−ＰＳＭＣ制御則では、速度制限を含めて仮想質点の運動を制御することで、実際の物体に確実に速度制限を課すことができる。

本発明は、上記説明したＶＢ−ＰＳＭＣ制御則を実装した位置制御装置として具現化してもよいし、上記説明したＶＢ−ＰＳＭＣ制御則を位置制御装置に実行させるプログラムとして具現化してもよい。また、上記説明したＶＢ−ＰＳＭＣ制御則による制御方法に具現化することもできる。

本発明によれば、物体の位置を目標位置に追従させる位置制御装置であって、位置追従精度を損なうことなく良好な速度制限ロジックを組み込んだ位置制御装置を実現することができる。また、物体の位置を目標位置に追従させる位置制御装置に対して、位置追従精度を損なうことなく良好な速度制限ロジックを組み込んだ位置制御を実現するプログラム及び方法を提供することができる。

図８に示す２自由度平行リンクマニピュレータを用いて、本発明の特徴であるＶＢ−ＰＳＭＣ制御則の効果を確認する実験を行った。図８に示すマニピュレータが、図２に示す移動機構１０に相当する。図８に「手先」と示している点が、図２に示す制御対象物１４に相当する。図８に示すマニピュレータは、手先（制御対象物１４）を図８に示すｘｙ直交座標系のｘ軸とｙ軸の方向に移動させることができる。
図８のマニピュレータは、ハーモニックドライブ（登録商標）ギアによって減速されたＡＣサーボモータを含むアクチュエータを備えている。アクチュエータには光学エンコーダが取り付けられており、手先の位置ｐ（即ち、制御対象物の位置ｐ）を計測することができる。手先の速度ｖ（即ち、制御対象物の速度ｖ）は、手先の位置ｐを時間微分することによって得ることができる。
図８のマニピュレータを制御するコントローラ（不図示）には、（２８）式で表されるＶＢ−ＰＳＭＣの２次元版が実装されている。マニピュレータが手先で発生する力ｆ（制御対象物に加えるべき力ｆ）は、コントローラ内で（２８）式を演算することによって決定される。コントローラ内ではまた、算出された力ｆをマニピュレータの手先で実現するためにアクチュエータが出力すべきトルクが算出される。算出されたトルクがマニピュレータのアクチュエータへの指令トルクとなる。なお、制御周期Ｔは、Ｔ＝０．００１秒である。

ＶＢ−ＰＳＭＣとＰＳＭＣの制御性能の差を確認するために、パラメータＶの影響を調べた。他のパラメータＫ、Ｂ、Ｌ、Ｆ、及びＨは次の通り設定した。
Ｋ＝80000N/m、Ｂ＝600Ns/m、Ｌ＝100000N/(ms)、Ｆ＝80N、Ｈ＝0.2s
なお、従来のＰＩＤ制御に対するＰＳＭＣの優位性については前述した文献１で述べている。

＜第１の実験＞
最初の実験として、所定の点から他の所定の点へ手先（制御対象物）を移動させる実験を行った。目標位置ｐ_ｄ（ｔ）は、次の（数３６）で表されるステップ状の関数として与えた。

Ｖの値として0.4m/s、0.8m/s、及び無限大の３つの値を選定した。即ち、マニピュレータの手先（マニピュレータによって保持された物体）が実現すべき速度として許容されている速度の大きさを0.4m/s、0.8m/sとした。なお、マニピュレータの手先はｘｙ平面内で移動可能であるから手先の速度は２次元のベクトルとなる。従って上記の速度制限は、手先（マニピュレータによって保持された物体）の速度ベクトルのＬ２ノルムを0.4m/s、0.8m/s以下に制限すること、と換言することができる。
なお、Ｖの値を無限大とした場合は、（３０）式で表されるＰＳＭＣに基づく制御に相当する。
第１の実験の結果を図９に示す。図９（ａ）は、手先位置ｐ（制御対象物の位置ｐ）のｘ座標値（ｐ_ｘと称する）の時間変化を示す。図９（ａ）における灰色の太線は、目標位置のｘ座標値ｐ_ｄｘを示す。図９（ｂ）は、手先のｘ軸方向の速度ｖ_ｘの時間変化を表す。図９（ｃ）は、手先のｘ座標位置ｐ_ｘ対ｘ軸方向速度ｖ_ｘの変化を表す。目標位置ｐ_ｄ（ｔ）が時刻ｔ＝５sでステップ状に変化しているので、手先位置ｐ_ｘは時刻ｔ＝５sで加速を開始している。ＶＢ−ＰＳＭＣ（Ｖ＝0.4m/s及び0.8m/sに設定した場合）では、手先の速度ｖ_ｘは、夫々設定したＶの値に到達した後に一定の値（設定したＶの値）となっていることがわかる。即ち、パラメータＶによって設定される速度制限がよく実現されていることがわかる。一方、Ｖを無限大に設定した場合（ＰＳＭＣ制御に相当する場合）は、実現される手先速度ｖ_ｘが制限されていないことがわかる。
図９（ａ）より、時間が経過するにつれて手先の位置が目標位置（ｘ座標0.2mの位置）へ減衰しながら漸近していることがわかる。手先（制御対象物）を減衰させながら目標位置へ漸近させることができる点が従来のＰＩＤ制御に対するＰＳＭＣ制御の優位な点である。

＜第２の実験＞
第２の実験として、目標位置ｐ_ｄ（ｔ）を次の（３７）式で与えた実験を行った。

なお、（３７）式の右辺の上段は目標位置のｘ座標を表し、右辺の下段は目標位置のｙ座標を表す。（３７）式は、目標位置が半径0.1mの円を描く軌道となることを表している。即ち、目標位置のｘ座標とｙ座標は夫々周期的な軌道となる。また、（数３７）は、時刻ｔ＝１０s付近でその周期が最速となる軌道となっている。
（３７）式で与えられる目標位置に対してパラメータＶをＶ＝0.4m/sと無限大に設定して実験を行った。
実験結果を図１０に示す。図１０（ａ）は、手先位置ｐ（制御対象物の位置ｐ）のｘ座標値（ｐ_ｘと称する）の時間変化を示す。図１０（ａ）における灰色の太線は、目標位置のｘ座標値ｐ_ｄｘを示す。図１０（ｂ）は、手先のｘ軸方向の速度ｖ_ｘの時間変化を表す。図１０（ｂ）における灰色の太線は、ｘ方向の目標速度ｖ_ｄを示す。図１０（ｃ）は、手先の位置ｐと目標位置ｐ_ｄの偏差を表す。図１０（ａ）では、灰色の太線が示す目標位置のｘ座標値ｐ_ｄｘは、Ｖを無限大としたときの手先位置ｐのグラフとほぼ重なっている。同様に図１０（ｂ）では、灰色の太線が示す目標速度ｄ_ｄは、Ｖを無限大としたときの手先速度ｖ_ｘのグラフとほぼ重なっている。
図１０（ｃ）から、手先の速度が上限値Ｖ＝0.4m/s以下である期間（時刻ｔ＝８s以前、及び時刻ｔ＝１３ｓ以降）では、ＶＢ−ＰＳＭＣとＰＳＭＣはほぼ同等の偏差となっていることがわかる。即ち、手先の速度がパラメータＶで設定される上限値以下の場合には、ＶＢ−ＰＳＭＣ（Ｖ＝0.4m/sに設定した場合）の追従性能はＰＳＭＣ（Ｖ＝無限大に設定した場合）の追従性能と同等の性能を実現できている。
一方、時刻ｔ＝８sから時刻ｔ＝１３ｓで目標速度が増加する期間（目標位置の変化の周期が速くなっている期間）では、ＶＢ−ＰＳＭＣでは速度制限がよく実現できていることがわかる。即ち、位置制御則にＶＢ−ＰＳＭＣを用いることによって、例えば上位のコントローラから与えられる目標位置と実際の手先位置（制御対象物の位置）との偏差が過大となる場合でも、手先（制御対象物）の速度を上限値以下に制限することができる。

以上の実験により、本発明の特徴であるＶＢ−ＰＳＭＣ制御則が、速度制限を付した位置制御をよく実現できることが立証できた。
ＶＢ−ＰＳＭＣ制御を実装することによって、ユーザは速度制限値以下における目標位置追従性能と応答性を損なうことなく、次の２つのパラメータを任意に設定することができる。
・物体（制御対象物）の位置を目標位置へ追従させる際の減衰時定数
・速度の上限値

ＶＢ−ＰＳＭＣは、物体を目標位置へ移動させる位置制御装置において、制限以下の速度においては追従性能を損なうことなく、物体の移動速度に制限を加えることができる。即ち、ＶＢ−ＰＳＭＣを実装した位置制御装置は、物体を安全に移動させることができる。それゆえ、このＶＢ−ＰＳＭＣは、物体を移動させる産業用ロボットの下位コントローラに実装することが好適である。移動機構は多リンクマニピュレータに限らず、位置制御するための移動機構であれば例えば１自由度の移動機構であってもよい。
また、移動機構として、リンクが関節によって回転可能に連結されている多リンクマニピュレータを利用する場合には、次の通り構成することも好適である。この場合の位置制御装置は、多リンクマニピュレータの手先に物体を保持し、保持した物体を目標位置に追従させる。多リンクマニピュレータの場合、手先に保持した物体の位置と各関節の角度はロボット工学における順運動学（或いは逆運動学）によって対応付けることが可能である。従って、物体を目標位置に追従させることは、目標位置に対応する各関節の目標角度に各関節の角度を追従させることと同じとなる。また、多リンクマニピュレータの場合、各関節の目標関節角が決まれば、関節毎に独立して角度制御を行うことも可能である。そこでそのような場合には、上記説明したＶＢ−ＰＳＭＣの１次元版を多リンクマニピュレータの各関節の角度制御器として用いることができる。そうすることによって、各関節の角速度に制限を加えることができ（制限値は関節毎に独立して与えることができる）、その制限以下の角速度においては角度制御の追従性（即ち物体の位置制御の追従性）を損なうことのない位置制御装置を実現できる。多リンクマニピュレータが直動関節を有する場合も同様である。

以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。

定理１のブロック図である。 位置制御装置の模式図である。 位置制御装置の制御ブロック図である。 ＰＳＭＣ制御則のブロック図である。 ＰＳＭＣに基づくスライディング多様体を示す図である。 ＶＢ−ＰＳＭＣに基づくスライディング多様体を示す図である。 ＶＢ−ＰＳＭＣ制御則のブロック図である。 実験に用いたマニピュレータの模式図である。 第１の実験の結果を示すグラフである。 第２の実験の結果を示すグラフである。

符号の説明

１０：移動機構
１４：制御対象物
２０：コントローラ
１００：位置制御装置

Claims (11)

1. 保持した物体の位置を目標位置に追従させる位置制御装置であり、
   物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、
   物体の位置を検出する位置検出器と、
   物体の速度を検出する速度検出器と、
   検出した物体の位置と速度に基づいて、物体に加える力の指令値を算出して移動機構へ出力するコントローラと、を備え、
   コントローラは、
   離散時間系における時刻をｋ、時刻ｋにおける物体の目標位置をｐ_ｄ（ｋ）、時刻ｋにおける物体の目標速度をｖ_ｄ（ｋ）、時刻ｋにおいて位置検出器によって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度検出器によって検出される物体の速度をｖ（ｋ）、単位時間をＴ、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、第４の係数をＬ、後退差分演算子を∇、時刻ｋにおいてコントローラ内で演算される内部状態変数をａ（ｋ）としたときに、
   時刻ｋにおける第１中間値ｕ^＊（ｋ）を次の第１式、即ち、
   ｕ^＊（ｋ）＝｛ｐ_ｄ（ｋ）−ｐ（ｋ）＋Ｈｖ_ｄ（ｋ）＋Ｔｖ（ｋ）
   −∇ａ（ｋ−１）／Ｔ｝／（Ｈ＋Ｔ） ・・・（第１式）
   を満たすように算出し、
   移動機構によって実現される物体の速度として許容されている速度の集合をＹとしたときに、ｕ^＊（ｋ）∈Ｙが満たされるときにはｕ（ｋ）＝ｕ^＊（ｋ）を満たし、ｕ^＊（ｋ）∈Ｙが満たされないときにはｕ^＊（ｋ）に正のスカラ値を乗じた値であり、かつｕ（ｋ）∈Ｙを満たすように時刻ｋにおける第２中間値ｕ（ｋ）を算出し、
   時刻ｋにおける第３中間値ｆ^＊（ｋ）を次の第２式、即ち、
   ｆ^＊（ｋ）＝Ｌａ（ｋ−１）＋（ＬＴ＋Ｋ）∇ａ（ｋ−１）／Ｔ
   ＋（ＬＴ^２＋ＫＴ＋Ｂ）（ｕ（ｋ）−ｖ（ｋ）） ・・・（第２式）
   を満たすように算出し、
   時刻ｋにおいてコントローラが算出して移動機構へ出力する指令値ｆ（ｋ）は、移動機構が物体に加えることを許容されている力の集合をＸとしたときに、ｆ^＊（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｆ^＊（ｋ）とし、ｆ^＊（ｋ）∈Ｘが満たされないときにはｆ^＊（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘを満たすｆ（ｋ）を指令値ｆ（ｋ）とし、
   時刻ｋ＋１における第１中間値ｕ^＊（ｋ＋１）と第３中間値ｆ^＊（ｋ＋１）を算出する場合に第１式と第２式に現れるａ（ｋ）を次の第３式、即ち、
   ａ（ｋ）＝｛（ＫＴ＋Ｂ）ａ（ｋ−１）＋Ｂ∇ａ（ｋ−１）
   ＋Ｔ^２ｆ（ｋ）｝／（ＬＴ^２＋ＫＴ＋Ｂ） ・・・（第３式）
   を満たすように算出することを特徴とする位置制御装置。
2. 保持した物体の位置を目標位置に追従させる位置制御装置であり、
   物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、
   物体の位置を検出する位置検出器と、
   物体の速度を検出する速度検出器と、
   検出した物体の位置と速度に基づいて、物体に加える力の指令値を算出して移動機構へ出力するコントローラと、を備え、
   コントローラは、
   離散時間系における時刻をｋ、時刻ｋにおける物体の目標位置をｐ_ｄ（ｋ）、時刻ｋにおける物体の目標速度をｖ_ｄ（ｋ）、時刻ｋにおいて位置検出器によって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度検出器によって検出される物体の速度をｖ（ｋ）、単位時間をＴ、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、第４の係数をＬ、時刻ｋにおいてコントローラ内で演算される内部状態変数をｅ（ｋ）としたときに、
   時刻ｋにおける第１中間値ｕ^＊（ｋ）を次の第４式、即ち、
   ｕ^＊（ｋ）＝｛ｐ_ｄ（ｋ）−ｐ（ｋ）＋Ｈｖ_ｄ（ｋ）＋Ｔｖ（ｋ）
   −ｅ（ｋ−１）｝／（Ｈ＋Ｔ） ・・・（第４式）
   を満たすように算出し、
   移動機構によって実現される物体の速度として許容されている速度の集合をＹとしたときに、ｕ^＊（ｋ）∈Ｙが満たされるときにはｕ（ｋ）＝ｕ^＊（ｋ）を満たし、ｕ^＊（ｋ）∈Ｙが満たされないときにはｕ^＊（ｋ）に正のスカラ値を乗じた値であり、かつｕ（ｋ）∈Ｙを満たすように時刻ｋにおける第２中間値ｕ（ｋ）を算出し、
   時刻ｋにおける第３中間値ｆ^＊（ｋ）を次の第５式、即ち、
   ｆ^＊（ｋ）＝Ｋｅ（ｋ−１）＋（ＫＴ＋Ｂ）（ｕ（ｋ）−ｖ（ｋ））・・・（第５式）
   を満たすように算出し、
   時刻ｋにおいてコントローラが算出して移動機構へ出力する指令値ｆ（ｋ）は、移動機が物体に加えることを許容されている力の集合をＸとしたときに、ｆ^＊（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｆ^＊（ｋ）とし、ｆ^＊（ｋ）∈Ｘが満たされないときにはｆ^＊（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘを満たすｆ（ｋ）を指令値ｆ（ｋ）とし、
   時刻ｋ＋１における第１中間値ｕ^＊（ｋ＋１）と第３中間値ｆ^＊（ｋ＋１）を算出する場合に第４式と第５式に現れるｅ（ｋ）を次の第６式、即ち、
   ｅ（ｋ）＝｛Ｂｅ（ｋ−１）＋Ｔｆ（ｋ）｝／（ＫＴ＋Ｂ） ・・・（第６式）
   を満たすように算出することを特徴とする位置制御装置。
3. 前記集合Ｙは、設定されたスカラ値Ｖよりも絶対値が小さい全ての値の集合であり、前記コントローラは、移動機構によって実現される物体の速度として許容されている速度の大きさをＶとし、ベクトルｘのノルムを表す記号を‖ｘ‖としたときに、‖ｕ^＊（ｋ）‖＜Ｖが満たされる場合は第２中間値ｕ（ｋ）＝ｕ^＊（ｋ）とし、‖ｕ^＊（ｋ）‖＜Ｖが満たされない場合は第２中間値ｕ（ｋ）＝（Ｖ／‖ｕ^＊（ｋ）‖）ｕ^＊（ｋ）とすることを特徴とする請求項１又は２に記載の位置制御装置。
4. 前記集合Ｘは、設定されたスカラ値Ｆよりも絶対値が小さい全ての値の集合であり、前記コントローラは、移動機構が物体に加えることを許容されている力の大きさをＦ、ベクトルｘのノルムを表す記号を‖ｘ‖としたときに、‖ｆ^＊（ｋ）‖＜Ｆが満たされる場合は指令値ｆ（ｋ）＝ｆ^＊（ｋ）とし、‖ｆ^＊（ｋ）‖＜Ｆが満たされない場合は指令値ｆ（ｋ）＝（Ｆ／‖ｆ^＊（ｋ）‖）ｆ^＊（ｋ）とすることを特徴とする請求項１から３のいずれか１項に記載の位置制御装置。
5. 前記集合Ｙは、設定されたスカラ値ＶよりもＬ２ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項１又は２に記載の位置制御装置。
6. 前記集合Ｘは、設定されたスカラ値ＦよりもＬ２ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項１、２、５のいずれか１項に記載の位置制御装置。
7. 物体の位置を目標位置に追従させる位置制御装置であり、
   物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、
   物体の位置を検出する位置検出器と、
   物体の速度を検出する速度検出器と、
   物体に加える力の指令値を算出して移動機構へ出力するコントローラと、を備え、
   コントローラは、
   質量ゼロの仮想質点と、一端が検出された物体の位置に連結されており、他端が仮想質点に連結されている仮想的な連結要素と、を仮想空間内に設定し、
   スライディングモード制御則に基づいて、仮想質点の位置を目標位置に追従させる制御を仮想空間内で行った際に仮想連結要素から物体に加えられる力を算出し、算出された力を指令値として移動機構へ出力し、
   前記スライディングモード制御則が用いるスライディング多様体は、少なくとも仮想質点の速度を状態変数に含む仮想質点の状態空間内の部分多様体であり、その多様体上で仮想質点の速度が有界であることを特徴とする位置制御装置。
8. 物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を利用して、移動機構が保持した物体の位置を目標位置に追従させる位置制御方法であり、
   物体の位置を検出する位置検出ステップと、
   物体の速度を検出する速度検出ステップと、
   検出した物体の位置と速度に基づいて、移動機構が物体に加える力の指令値を算出する指令値算出ステップと、
   算出された力の指令値を移動機構へ出力する指令値出力ステップと、を含み、
   前記指令値算出ステップは、
   離散時間系における時刻をｋ、時刻ｋにおける物体の目標位置をｐ_ｄ（ｋ）、時刻ｋにおける物体の目標速度をｖ_ｄ（ｋ）、時刻ｋにおいて位置検出器によって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度検出器によって検出される物体の速度をｖ（ｋ）、単位時間をＴ、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、第４の係数をＬ、後退差分演算子を∇、時刻ｋにおいて算出される内部状態変数をａ（ｋ）としたときに、
   時刻ｋにおける第１中間値ｕ^＊（ｋ）を次の第１式、即ち、
   ｕ^＊（ｋ）＝｛ｐ_ｄ（ｋ）−ｐ（ｋ）＋Ｈｖ_ｄ（ｋ）＋Ｔｖ（ｋ）
   −∇ａ（ｋ−１）／Ｔ｝／（Ｈ＋Ｔ） ・・・（第１式）
   を満たすように算出し、
   移動機構によって実現される物体の速度として許容されている速度の集合をＹとしたときに、ｕ^＊（ｋ）∈Ｙが満たされるときにはｕ（ｋ）＝ｕ^＊（ｋ）を満たし、ｕ^＊（ｋ）∈Ｙが満たされないときにはｕ^＊（ｋ）に正のスカラ値を乗じた値であり、かつｕ（ｋ）∈Ｙを満たすように時刻ｋにおける第２中間値ｕ（ｋ）を算出し、
   時刻ｋにおける第３中間値ｆ^＊（ｋ）を次の第２式、即ち、
   ｆ^＊（ｋ）＝Ｌａ（ｋ−１）＋（ＬＴ＋Ｋ）∇ａ（ｋ−１）／Ｔ
   ＋（ＬＴ^２＋ＫＴ＋Ｂ）（ｕ（ｋ）−ｖ（ｋ）） ・・・（第２式）
   を満たすように算出し、
   時刻ｋにおける力の指令値ｆ（ｋ）は、移動機構が物体に加えることを許容されている力の集合をＸとしたときに、ｆ^＊（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｆ^＊（ｋ）とし、ｆ^＊（ｋ）∈Ｘが満たされないときにはｆ^＊（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘを満たすｆ（ｋ）を指令値ｆ（ｋ）とし、
   時刻ｋ＋１における第１中間値ｕ^＊（ｋ＋１）と第３中間値ｆ^＊（ｋ＋１）を算出する場合に第１式と第２式に現れるａ（ｋ）を次の第３式、即ち、
   ａ（ｋ）＝｛（ＫＴ＋Ｂ）ａ（ｋ−１）＋Ｂ∇ａ（ｋ−１）
   ＋Ｔ^２ｆ（ｋ）｝／（ＬＴ^２＋ＫＴ＋Ｂ） ・・・（第３式）
   を満たすように算出することを特徴とする位置制御方法。
9. 物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を利用して、移動機構が保持した物体の位置を目標位置に追従させる位置制御方法であり、
   物体の位置を検出する位置検出ステップと、
   物体の速度を検出する速度検出ステップと、
   検出した物体の位置と速度に基づいて、移動機構が物体に加える力の指令値を算出する指令値算出ステップと、
   算出された力の指令値を移動機構へ出力する指令値出力ステップと、を含み、
   前記指令値算出ステップは、
   離散時間系における時刻をｋ、時刻ｋにおける物体の目標位置をｐ_ｄ（ｋ）、時刻ｋにおける物体の目標速度をｖ_ｄ（ｋ）、時刻ｋにおいて位置検出器によって検出される物体の位置をｐ（ｋ）、時刻ｋにおいて速度検出器によって検出される物体の速度をｖ（ｋ）、単位時間をＴ、第１の係数をＫ、第２の係数をＢ、第３の係数をＨ、第４の係数をＬ、時刻ｋにおいて演算される内部状態変数をｅ（ｋ）としたときに、
   時刻ｋにおける第１中間値ｕ^＊（ｋ）を次の第４式、即ち、
   ｕ^＊（ｋ）＝｛ｐ_ｄ（ｋ）−ｐ（ｋ）＋Ｈｖ_ｄ（ｋ）＋Ｔｖ（ｋ）
   −ｅ（ｋ−１）｝／（Ｈ＋Ｔ） ・・・（第４式）
   を満たすように算出し、
   移動機構によって実現される物体の速度として許容されている速度の集合をＹとしたときに、ｕ^＊（ｋ）∈Ｙが満たされるときにはｕ（ｋ）＝ｕ^＊（ｋ）を満たし、ｕ^＊（ｋ）∈Ｙが満たされないときにはｕ^＊（ｋ）に正のスカラ値を乗じた値であり、かつｕ（ｋ）∈Ｙを満たすように時刻ｋにおける第２中間値ｕ（ｋ）を算出し、
   時刻ｋにおける第３中間値ｆ^＊（ｋ）を次の第５式、即ち、
   ｆ^＊（ｋ）＝Ｋｅ（ｋ−１）＋（ＫＴ＋Ｂ）（ｕ（ｋ）−ｖ（ｋ））・・・（第５式）
   を満たすように算出し、
   時刻ｋにおける力の指令値ｆ（ｋ）は、移動機構が物体に加えることを許容されている力の集合をＸとしたときに、ｆ^＊（ｋ）∈Ｘが満たされるときには指令値ｆ（ｋ）＝ｆ^＊（ｋ）とし、ｆ^＊（ｋ）∈Ｘが満たされないときにはｆ^＊（ｋ）に正のスカラ値を乗じた値であり、かつｆ（ｋ）∈Ｘを満たすｆ（ｋ）を指令値ｆ（ｋ）とし、
   時刻ｋ＋１における第１中間値ｕ^＊（ｋ＋１）と第３中間値ｆ^＊（ｋ＋１）を算出する場合に第４式と第５式に現れるｅ（ｋ）を次の第６式、即ち、
   ｅ（ｋ）＝｛Ｂｅ（ｋ−１）＋Ｔｆ（ｋ）｝／（ＫＴ＋Ｂ） ・・・（第６式）
   を満たすように算出することを特徴とする位置制御方法。
10. 物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を利用して、移動機構が保持した物体の位置を目標位置に追従させる位置制御方法であり、
    物体の位置を検出する位置検出ステップと、
    物体の速度を検出する速度検出ステップと、
    検出した物体の位置と速度に基づいて、移動機構が物体に加える力の指令値を算出する指令値算出ステップと、
    算出された力の指令値を移動機構へ出力する指令値出力ステップと、を含み、
    前記指令値算出ステップは、
    質量ゼロの仮想質点と、一端が検出された物体の位置に連結されており、他端が仮想質点に連結されている仮想的な連結要素と、を仮想空間内に設定し、
    スライディングモード制御則に基づいて、仮想質点の位置を目標位置に追従させる制御を仮想空間内で行った際に仮想連結要素から物体に加えられる力を算出し、算出された力を指令値として移動機構へ出力し、
    前記スライディングモード制御則が用いるスライディング多様体は、少なくとも仮想質点の速度を状態変数に含む仮想質点の状態空間内の部分多様体であり、その多様体上で仮想質点の速度が有界であることを特徴とする位置制御方法。
11. 物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を制御するコンピュータによって、請求項８から１０のいずれか１項に記載の位置制御方法を実行させるための位置制御プログラム。

Images