JP4587052B2 - 位置制御装置、位置制御方法及び位置制御プログラム - Google Patents
位置制御装置、位置制御方法及び位置制御プログラム Download PDFInfo
- Publication number
- JP4587052B2 JP4587052B2 JP2006275763A JP2006275763A JP4587052B2 JP 4587052 B2 JP4587052 B2 JP 4587052B2 JP 2006275763 A JP2006275763 A JP 2006275763A JP 2006275763 A JP2006275763 A JP 2006275763A JP 4587052 B2 JP4587052 B2 JP 4587052B2
- Authority
- JP
- Japan
- Prior art keywords
- time
- value
- command value
- satisfied
- moving mechanism
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Feedback Control In General (AREA)
Description
ロボットなどの位置制御装置では、移動機構が物体を過度な速度で移動させることがないように、コントローラ内に、移動機構の動作が所定の速度を超えないように制限する速度制限ロジックを組み込むことが望ましい。なお、本明細書では、物体の位置という場合にはその物体の位置及び/又はその物体の姿勢を意味するものとし、速度という場合には並進速度及び/又は角速度を意味するものとする。また、本明細書では後に「力」という表現を用いるが本明細書で用いる「力」は並進力及び/又はトルクを意味するものとする。
速度制限ロジックを組み込んだ位置制御技術の例として、たとえば非特許文献1及び2には、受動性に基づくエネルギ成形方法(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)が開示されている。
或いは他の例として、非特許文献3には、時間軸変換を用いた速度制限の方法(Rate constraints using time-scale transformations)が開示されている。
上記の非特許文献1及び2に開示された技術では、速度制限を付加することによって位置追従精度が低下してしまう。上記の非特許文献3に開示された技術では、上位のコントローラ(例えば物体の移動軌道を生成するコントローラ)から過大な目標位置指令値が入力されたときに下位のコントローラ(上位のコントローラからの指令に基づいて移動機構を制御するコントローラ)は速度を制限することができない。
本願発明は上記の課題に鑑みてなされたものであり、その目的は、良好な速度制限ロジックを組み込むことのできる位置制御技術を提供することにある。特に、位置追従性能を損なわずに、物体を所定の速度以下で移動させて目標位置に追従させることのできる位置制御技術を提供することにある。
本発明は、発明者らが既に開発した位置制御技術に速度制限のロジックを組み込むことによって、位置追従性能を損なわずに、物体を所定の速度以下で移動させて目標位置に追従させることのできる位置制御装置、位置制御方法、及び位置制御プログラムを実現するものである。
n次元関数sgn(・)とsat(・)を定義する。
sgn(・)は、n次元空間RnからRnへ写像する正規化関数であり次の(1)式で定義される。
<定理1> 2つのn次元ベクトルxとyについて、次の(4)式を満たす関係が成立する。
関数sgn(・)の定義から、xとyは次の(5)式の関係を満足することは明らかである。
Z>0である全てのスカラ値Z、及びz∈Rnを満たすzに対してsgn(Zz)=sgn(z)が成立する。よって(8)式は次の(9)式に書き直すことができる。
以上で定理1と系1についての説明を終る。
移動機構10には、保持した制御対象物14の位置を検出するセンサが取り付けられている。具体的には移動機構10の各関節にエンコーダ(不図示)が取り付けられており、各エンコーダの値からロボット工学における順キネマティクスを用いて先端に保持した制御対象物14の位置を算出する。またコントローラ20は、制御対象物14の位置を時間微分して速度を算出する。なお、本明細書では、制御対象物14の位置を直接検出するセンサ、間接的に検出するセンサ(例えば上記のエンコーダ)を含めて位置検出器と称する。また、制御対象物14の速度を直接検出するセンサ、間接的に検出するセンサ、或いは位置から速度を推定する推定手段を含めて制御対象物14の速度を検出する速度検出器と称する。速度を間接的に検出するとは例えば移動機構の各関節にジャイロを備え、各ジャイロの出力から制御対象物14の速度を求めることである。また、制御対象物14の位置を時間微分することによって制御対象物14の速度を得ることは換言すれば制御対象物14の速度を推定することであるが、本発明では制御対象物14の速度が直接検出したものであっても推定によって得たものであってもよいため、速度を推定する手段を含めて速度検出器と称する。
コントローラ20からは移動機構10の先端が制御対象物14に加えるべき力が出力される。移動機構10は、コントローラが出力した力の指令を実現するように動作する。例えば、移動機構10の先端で発生する力をf、そのときに移動機構10の各関節が発生するトルクをτ、移動機構10のヤコビ行列をJとすると、f=JTτの関係が成立する。この関係を用いることで、移動機構10の先端で発生すべき力fを実現するための各関節のトルクτを求めることができる。移動機構10の各関節に配置されたアクチュエータがこのトルクτを発生することによって、移動機構10の先端で力fを発生させることができる。即ち、移動機構10が制御対象物14へ力fを加えることができる。
なお、図2に示す位置制御装置100は、本発明の特徴をなす制御則を説明するための例示であり、移動機構10の物理的な構造(例えば関節の数やリンクの接続関係など)を限定するものではない。
コントローラ20は、目標位置pdと位置pを入力として、移動機構10の先端が発生すべき力f(先端が制御対象物14へ加えるべき力f)の指令値を出力する。移動機構10は、コントローラ20が出力する力fの指令値に基づいて、制御対象物14に対して力fを加えるように制御される。コントローラ20が出力する力fの指令値に基づいて移動機構10の先端が制御対象物14へ力fを加えることによって、制御対象物14の位置pを目標位置pdへ移動させることができる。
図2に示すように、PSMCではプロキシ(proxy)と呼ぶ質量ゼロの仮想質点を設定する。仮想質点は、制御ロジック内で想定される点であり、実際には存在しない。仮想質点の位置を記号psで表し、仮想質点の速度を記号vsで表す。
なお、「制御ロジック内で想定する」とは、より具体的には制御ロジックを構築する上で仮に想定するという意味である。請求項の「仮想空間内に設定する」との表現も同様の意味である。
制御ロジック内で、仮想質点に対して加える力fを想定する。仮想質点は質量がゼロであり、かつ制御対象物14と弾性を有する仮想連結要素で連結されているため、仮想質点に加える力fは制御対象物14でも発生する。制御ロジック内では仮想質点と仮想連結要素を想定するが、コントローラ20は実際にはこの力fを移動機構10の指令値として出力する。以下で説明するように、力fは、仮想質点を目標位置へ移動させるための力として算出される。そのように算出された力fを移動機構10によって制御対象物14へ加えることで制御対象物14を移動させる。なお、質量ゼロの仮想質点へ力fを加えるとは、仮想質点のダイナミクスにおいて慣性項が存在しないことを意味する。この制御ロジックは、仮想質点の位置psが、仮想連結要素が発生する力と力fがバランスする位置となることに着目する。
なお、図2に示すhは、システムに加わる外力を表している。
図3に示すL/s+K+Bsのブロックがこの仮想連結要素の特性を示している。ここで、Lは積分ゲインであり、Kは比例ゲインであり、Bは微分ゲインを表す。なお、sはラプラス演算子である。即ち、仮想連結要素が発生する力fは、次の(10)式を満足する。
なお、図3に示すhは、システムに加わる外力を表している。
ss=0を満足する点の集合が仮想質点の2n次元の状態空間{ps、vs}におけるn次元の多様体を表す。図6に、ss=0で表される多様体を模式的に示す。図6に示す灰色の太線が多様体ss=0である。
(12b)式の右辺第1項のノルムは常にVよりも小さいので、(12b)式は、ss=0が‖vs‖≦Vを満足することを示している。このことは、仮想質点の状態がss=0で表される多様体上にある限りにおいて、仮想質点の速度の大きさがV以下となることを意味する。ss=0と‖vs‖≦Vが成立する場合には‖Aσs+vs‖は1より小さくなり、その結果、Aσs+vs−vs=0の関係が満足される。このことは、ss=0と‖vs‖≦Vが成立する場合には、σs=0が満足されることを意味する。それゆえ、‖Aσs+vs‖≦Vが満足される場合は常に(12)式は(11)式と等価となる。なお、パラメータAは多様体ss=0には影響を与えないことに留意されたい。
図5と図6を比較すると理解できるように、(11)式の制御則における多様体σs=0(図5)が、有界でないのに対して、(12)式の制御則における多様体ss=0(図6)は、速度vsの軸に対して有界となる。
図3と図7によって表現される制御則は、次の(13)式を満足する。
(13)式で表される制御則を実装すると、図2に示す位置制御装置100は、制御対象物14の速度の大きさをV以下に抑制しながら、制御対象物14の位置pを目標位置psに追従させることができる。
(13)式をオイラー近似に基づいて離散時間系へ変換すると次の(14)式を得る。
なお、(14)式の表現は、コンピュータによる演算を考慮した表現とはなっていないが、制御対象物14の速度の大きさをV以下に抑制するために満足すべき条件を網羅している。システムへの入力は目標位置pd(k)と制御対象物14の位置p(k)、及びそれらの時間微分値(即ち速度)である。従って制御ロジック内ではこれらの値は既知であるものとして取り扱うことができる。それらの入力値が与えられると、(14)式によって、時刻kにおける位置制御装置100の移動機構10が制御対象物14へ加えるべき力f(k)と内部状態変数a(k)(及びその時間部分∇a(k))を決定することができる。
「A」は任意の正値のスカラ値であればよいので、解析的な導出が容易となるように「A」を選定すると好都合である。そこで、「A」を含め、新たに第1中間値u*(k)を次の(15)式により定義する。
同様に、(28d)式は、(28c)式によって求められる第3中間値f*(k)を関数sat(・)で制限したものを力f(k)とすることを意味している。従って(28d)式は、数学的に次の通り一般化できることが自明である。即ち、「力の指令値f(k)は、移動機構が物体に加えることを許容されている力の集合をXとしたときに、f*(k)∈Xが満たされるときには指令値f(k)=f*(k)で求められ、f*(k)∈Xが満たされないときにはf*(k)に正のスカラ値を乗じた値として求められる。」
(28)式を導出した上記の過程においてL=0と置くと次の(29)式を得る。
集合Yは、設定されたスカラ値VよりもL2ノルムが小さい全てのベクトル値の集合である。或いは、集合Xは、設定されたスカラ値FよりもL2ノルムが小さい全てのベクトル値の集合である。
また、他の代表的なノルムにL無限大ノルム(一般にL∞と表現することがある)がある。そこで上記の集合YやXを次の通り特定することも好適である。
集合Yは、設定されたスカラ値VよりもL無限大ノルムが小さい全てのベクトル値の集合である。或いは、集合Xは、設定されたスカラ値FよりもL無限大ノルムが小さい全てのベクトル値の集合である。
図6に示すように、スライディング多様体σs=0は、少なくとも仮想質点の速度を状態変数に含む仮想質点の状態空間内の部分多様体である。そして、(12)式は、スライディング多様体σs=0上で仮想質点の速度が有界であることを表している。
次に、VB−PSMCをPSMCとの対比において説明する。
(28)式によって表されるVB−PSMCの制御則から速度制限を外すと(即ち、(28)式においてVを無限大とすると)、次の(30)式を得る。
前述した文献1で述べているように、PSMCはPID制御を拡張したものと捉えることができる。従って、VB−PSMCもPID制御を拡張したものと捉えることができる。(30)式においてFを無限大とすると次の(31)式を得る。
以上より、(28)式で表されるVB−PSMCは、‖u*(k)‖≦Vと‖f*(k)‖≦Fが満足される限りにおいて(35)式で表されるPID制御則と等価である。このことは、‖u*(k)‖≦Vと‖f*(k)‖≦Fが満足されている状態においては(即ち、制御対象物14の速度の大きさが制限速度の大きさVに至らない状態においては)、スカラ値F、V及びHは追従性能に影響しないことを意味している。即ち、VB−PSMCは、(35)式で表されるPID制御則に次の特徴を付加した制御則であるということができる。
・移動機構が物体(移動機構が保持した制御対象物)に加えるべき力の指令値に、任意の大きさFの制限を加えることができる。
・移動機構によって実現する物体の移動速度に、任意の大きさVの制限を加えることができる。
・移動機構によって実現する物体の位置の目標位置への追従特性として、任意の時定数Hによる減衰特性を与えることができる。
VB−PSMC制御則は、PID制御ゲインK、L、及びBとは独立に上記のF、V、及びHの3パラメータを設定できる点に、従来のPID制御に優る利点がある。パラメータFとVは、位置制御装置が使用される環境に応じて設定することができる。
(1)移動機構が保持した物体に対して質量ゼロの仮想質点が連結されていると設定し、その仮想質点を目標位置に追従させるようにスライディングモード制御則を構築する。ここで、仮想質点と物体は仮想的な連結要素(少なくとも弾性を有する)で連結されており、仮想質点は、仮想質点と物体の位置偏差に応じて連結要素が発生する力(弾性による復元力)を受ける。スライディングモード制御則は、連結要素から力を受ける仮想質点に制御力として加える力を算出する。この制御力は、仮想質点を目標位置まで移動させる力として算出される。算出された力を物体に加える指令値として移動機構へ出力する。
(2)スライディングモード制御則が用いるスライディング多様体として、次の条件を満たす多様体を採用する。
・スライディング多様体は、仮想質点の速度を含む状態空間(上記の説明では仮想質点の位置psと速度vsが張る状態空間{ps,vs})における部分多様体である。
・スライディング多様体は、その多様体上で仮想質点の速度が有界である。
なお、仮想質点の速度が有界であるとは、例えば物体の運動が所定の一方向に規制されている場合にはその速度の大きさにスカラ値である上限値Vを課すことに相当する。
上記の(1)は、PSMCの特徴でもあるが、「質量ゼロの仮想質点」を新たに導入し、その仮想質点に対して位置制御する制御力を求めるスライディングモード制御である点が従来のPID制御、或いはスライディングモード制御と異なる点である。
仮想質点は仮想的な連結要素を介して実際の物体に連結されている。一方、仮想質点は質量がゼロであるので仮想質点の慣性力は制御ロジック内に現れない。従って、仮想質点の位置(運動)を制御することはそのまま実際の物体を制御することに繋がる。一方、仮想質点の運動は、制御ロジック内で仮想される運動であるので精密に制御することができる。制御ロジック内において、仮想質点をスライディングモード制御の対象とすることによって、従来のスライディングモード制御が有していたチャタリングの生じ易さを抑制することに成功した。
そのようなPSMCに対して、さらに仮想質点の速度に対して上限値を付加した点が上記(2)の特徴である。特に、実際の物体ではなく、仮想質点の運動に対して速度制限を加えている点が特徴である。前述したように、制御ロジック内で仮想される仮想質点の運動は精密に制御することができる。従って、仮想質点に対する速度制限は、確実に仮想質点の運動に反映することができる。仮想質点の運動に速度制限を確実に反映できるということは、実際の物体に対して速度制限を確実に反映できることを意味する。
制御ロジック内で実際の物体に速度制限を付加しても、実際の物体には例えば図2や図3に示すように未知の外力hが作用する。そのため、物体に速度制限を確実に課すことが困難となる。VB−PSMC制御則では、速度制限を含めて仮想質点の運動を制御することで、実際の物体に確実に速度制限を課すことができる。
図8のマニピュレータは、ハーモニックドライブ(登録商標)ギアによって減速されたACサーボモータを含むアクチュエータを備えている。アクチュエータには光学エンコーダが取り付けられており、手先の位置p(即ち、制御対象物の位置p)を計測することができる。手先の速度v(即ち、制御対象物の速度v)は、手先の位置pを時間微分することによって得ることができる。
図8のマニピュレータを制御するコントローラ(不図示)には、(28)式で表されるVB−PSMCの2次元版が実装されている。マニピュレータが手先で発生する力f(制御対象物に加えるべき力f)は、コントローラ内で(28)式を演算することによって決定される。コントローラ内ではまた、算出された力fをマニピュレータの手先で実現するためにアクチュエータが出力すべきトルクが算出される。算出されたトルクがマニピュレータのアクチュエータへの指令トルクとなる。なお、制御周期Tは、T=0.001秒である。
K=80000N/m、B=600Ns/m、L=100000N/(ms)、F=80N、H=0.2s
なお、従来のPID制御に対するPSMCの優位性については前述した文献1で述べている。
最初の実験として、所定の点から他の所定の点へ手先(制御対象物)を移動させる実験を行った。目標位置pd(t)は、次の(数36)で表されるステップ状の関数として与えた。
なお、Vの値を無限大とした場合は、(30)式で表されるPSMCに基づく制御に相当する。
第1の実験の結果を図9に示す。図9(a)は、手先位置p(制御対象物の位置p)のx座標値(pxと称する)の時間変化を示す。図9(a)における灰色の太線は、目標位置のx座標値pdxを示す。図9(b)は、手先のx軸方向の速度vxの時間変化を表す。図9(c)は、手先のx座標位置px対x軸方向速度vxの変化を表す。目標位置pd(t)が時刻t=5sでステップ状に変化しているので、手先位置pxは時刻t=5sで加速を開始している。VB−PSMC(V=0.4m/s及び0.8m/sに設定した場合)では、手先の速度vxは、夫々設定したVの値に到達した後に一定の値(設定したVの値)となっていることがわかる。即ち、パラメータVによって設定される速度制限がよく実現されていることがわかる。一方、Vを無限大に設定した場合(PSMC制御に相当する場合)は、実現される手先速度vxが制限されていないことがわかる。
図9(a)より、時間が経過するにつれて手先の位置が目標位置(x座標0.2mの位置)へ減衰しながら漸近していることがわかる。手先(制御対象物)を減衰させながら目標位置へ漸近させることができる点が従来のPID制御に対するPSMC制御の優位な点である。
第2の実験として、目標位置pd(t)を次の(37)式で与えた実験を行った。
(37)式で与えられる目標位置に対してパラメータVをV=0.4m/sと無限大に設定して実験を行った。
実験結果を図10に示す。図10(a)は、手先位置p(制御対象物の位置p)のx座標値(pxと称する)の時間変化を示す。図10(a)における灰色の太線は、目標位置のx座標値pdxを示す。図10(b)は、手先のx軸方向の速度vxの時間変化を表す。図10(b)における灰色の太線は、x方向の目標速度vdを示す。図10(c)は、手先の位置pと目標位置pdの偏差を表す。図10(a)では、灰色の太線が示す目標位置のx座標値pdxは、Vを無限大としたときの手先位置pのグラフとほぼ重なっている。同様に図10(b)では、灰色の太線が示す目標速度ddは、Vを無限大としたときの手先速度vxのグラフとほぼ重なっている。
図10(c)から、手先の速度が上限値V=0.4m/s以下である期間(時刻t=8s以前、及び時刻t=13s以降)では、VB−PSMCとPSMCはほぼ同等の偏差となっていることがわかる。即ち、手先の速度がパラメータVで設定される上限値以下の場合には、VB−PSMC(V=0.4m/sに設定した場合)の追従性能はPSMC(V=無限大に設定した場合)の追従性能と同等の性能を実現できている。
一方、時刻t=8sから時刻t=13sで目標速度が増加する期間(目標位置の変化の周期が速くなっている期間)では、VB−PSMCでは速度制限がよく実現できていることがわかる。即ち、位置制御則にVB−PSMCを用いることによって、例えば上位のコントローラから与えられる目標位置と実際の手先位置(制御対象物の位置)との偏差が過大となる場合でも、手先(制御対象物)の速度を上限値以下に制限することができる。
VB−PSMC制御を実装することによって、ユーザは速度制限値以下における目標位置追従性能と応答性を損なうことなく、次の2つのパラメータを任意に設定することができる。
・物体(制御対象物)の位置を目標位置へ追従させる際の減衰時定数
・速度の上限値
また、移動機構として、リンクが関節によって回転可能に連結されている多リンクマニピュレータを利用する場合には、次の通り構成することも好適である。この場合の位置制御装置は、多リンクマニピュレータの手先に物体を保持し、保持した物体を目標位置に追従させる。多リンクマニピュレータの場合、手先に保持した物体の位置と各関節の角度はロボット工学における順運動学(或いは逆運動学)によって対応付けることが可能である。従って、物体を目標位置に追従させることは、目標位置に対応する各関節の目標角度に各関節の角度を追従させることと同じとなる。また、多リンクマニピュレータの場合、各関節の目標関節角が決まれば、関節毎に独立して角度制御を行うことも可能である。そこでそのような場合には、上記説明したVB−PSMCの1次元版を多リンクマニピュレータの各関節の角度制御器として用いることができる。そうすることによって、各関節の角速度に制限を加えることができ(制限値は関節毎に独立して与えることができる)、その制限以下の角速度においては角度制御の追従性(即ち物体の位置制御の追従性)を損なうことのない位置制御装置を実現できる。多リンクマニピュレータが直動関節を有する場合も同様である。
14:制御対象物
20:コントローラ
100:位置制御装置
Claims (9)
- 保持した物体の位置を目標位置に追従させる位置制御装置であり、
物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、
物体の位置を検出する位置検出器と、
物体の速度を検出する速度検出器と、
検出した物体の位置と速度に基づいて、物体に加える力の指令値を算出して移動機構へ出力するコントローラと、を備え、
コントローラは、
離散時間系における時刻をk、時刻kにおける物体の目標位置をpd(k)、時刻kにおける物体の目標速度をvd(k)、時刻kにおいて位置検出器によって検出される物体の位置をp(k)、時刻kにおいて速度検出器によって検出される物体の速度をv(k)、単位時間をT、第1の係数をK、第2の係数をB、第3の係数をH、第4の係数をL、後退差分演算子を∇、時刻kにおいてコントローラ内で演算される内部状態変数をa(k)としたときに、
時刻kにおける第1中間値u*(k)を次の第1式、即ち、
u*(k)={pd(k)−p(k)+Hvd(k)+Tv(k)
−∇a(k−1)/T}/(H+T) ・・・(第1式)
を満たすように算出し、
移動機構によって実現される物体の速度として許容されている速度の集合をYとしたときに、u*(k)∈Yが満たされるときにはu(k)=u*(k)を満たし、u*(k)∈Yが満たされないときにはu*(k)に正のスカラ値を乗じた値であり、かつu(k)∈Yを満たすように時刻kにおける第2中間値u(k)を算出し、
時刻kにおける第3中間値f*(k)を次の第2式、即ち、
f*(k)=La(k−1)+(LT+K)∇a(k−1)/T
+(LT2+KT+B)(u(k)−v(k)) ・・・(第2式)
を満たすように算出し、
時刻kにおいてコントローラが算出して移動機構へ出力する指令値f(k)は、移動機構が物体に加えることを許容されている力の集合をXとしたときに、f*(k)∈Xが満たされるときには指令値f(k)=f*(k)とし、f*(k)∈Xが満たされないときにはf*(k)に正のスカラ値を乗じた値であり、かつf(k)∈Xを満たすf(k)を指令値f(k)とし、
時刻k+1における第1中間値u*(k+1)と第3中間値f*(k+1)を算出する場合に第1式と第2式に現れるa(k)を次の第3式、即ち、
a(k)={(KT+B)a(k−1)+B∇a(k−1)
+T2f(k)}/(LT2+KT+B) ・・・(第3式)
を満たすように算出することを特徴とする位置制御装置。 - 保持した物体の位置を目標位置に追従させる位置制御装置であり、
物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構と、
物体の位置を検出する位置検出器と、
物体の速度を検出する速度検出器と、
検出した物体の位置と速度に基づいて、物体に加える力の指令値を算出して移動機構へ出力するコントローラと、を備え、
コントローラは、
離散時間系における時刻をk、時刻kにおける物体の目標位置をpd(k)、時刻kにおける物体の目標速度をvd(k)、時刻kにおいて位置検出器によって検出される物体の位置をp(k)、時刻kにおいて速度検出器によって検出される物体の速度をv(k)、単位時間をT、第1の係数をK、第2の係数をB、第3の係数をH、第4の係数をL、時刻kにおいてコントローラ内で演算される内部状態変数をe(k)としたときに、
時刻kにおける第1中間値u*(k)を次の第4式、即ち、
u*(k)={pd(k)−p(k)+Hvd(k)+Tv(k)
−e(k−1)}/(H+T) ・・・(第4式)
を満たすように算出し、
移動機構によって実現される物体の速度として許容されている速度の集合をYとしたときに、u*(k)∈Yが満たされるときにはu(k)=u*(k)を満たし、u*(k)∈Yが満たされないときにはu*(k)に正のスカラ値を乗じた値であり、かつu(k)∈Yを満たすように時刻kにおける第2中間値u(k)を算出し、
時刻kにおける第3中間値f*(k)を次の第5式、即ち、
f*(k)=Ke(k−1)+(KT+B)(u(k)−v(k))・・・(第5式)
を満たすように算出し、
時刻kにおいてコントローラが算出して移動機構へ出力する指令値f(k)は、移動機が物体に加えることを許容されている力の集合をXとしたときに、f*(k)∈Xが満たされるときには指令値f(k)=f*(k)とし、f*(k)∈Xが満たされないときにはf*(k)に正のスカラ値を乗じた値であり、かつf(k)∈Xを満たすf(k)を指令値f(k)とし、
時刻k+1における第1中間値u*(k+1)と第3中間値f*(k+1)を算出する場合に第4式と第5式に現れるe(k)を次の第6式、即ち、
e(k)={Be(k−1)+Tf(k)}/(KT+B) ・・・(第6式)
を満たすように算出することを特徴とする位置制御装置。 - 前記集合Yは、設定されたスカラ値Vよりも絶対値が小さい全ての値の集合であり、前記コントローラは、移動機構によって実現される物体の速度として許容されている速度の大きさをVとし、ベクトルxのノルムを表す記号を‖x‖としたときに、‖u*(k)‖<Vが満たされる場合は第2中間値u(k)=u*(k)とし、‖u*(k)‖<Vが満たされない場合は第2中間値u(k)=(V/‖u*(k)‖)u*(k)とすることを特徴とする請求項1又は2に記載の位置制御装置。
- 前記集合Xは、設定されたスカラ値Fよりも絶対値が小さい全ての値の集合であり、前記コントローラは、移動機構が物体に加えることを許容されている力の大きさをF、ベクトルxのノルムを表す記号を‖x‖としたときに、‖f*(k)‖<Fが満たされる場合は指令値f(k)=f*(k)とし、‖f*(k)‖<Fが満たされない場合は指令値f(k)=(F/‖f*(k)‖)f*(k)とすることを特徴とする請求項1から3のいずれか1項に記載の位置制御装置。
- 前記集合Yは、設定されたスカラ値VよりもL2ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項1又は2に記載の位置制御装置。
- 前記集合Xは、設定されたスカラ値FよりもL2ノルムが小さい全てのベクトル値の集合であることを特徴とする請求項1、2、又は5のいずれか1項に記載の位置制御装置。
- 物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を利用して、移動機構が保持した物体の位置を目標位置に追従させる位置制御方法であり、
物体の位置を検出する位置検出ステップと、
物体の速度を検出する速度検出ステップと、
検出した物体の位置と速度に基づいて、移動機構が物体に加える力の指令値を算出する指令値算出ステップと、
算出された力の指令値を移動機構へ出力する指令値出力ステップと、を含み、
前記指令値算出ステップは、
離散時間系における時刻をk、時刻kにおける物体の目標位置をpd(k)、時刻kにおける物体の目標速度をvd(k)、時刻kにおいて位置検出器によって検出される物体の位置をp(k)、時刻kにおいて速度検出器によって検出される物体の速度をv(k)、単位時間をT、第1の係数をK、第2の係数をB、第3の係数をH、第4の係数をL、後退差分演算子を∇、時刻kにおいて算出される内部状態変数をa(k)としたときに、
時刻kにおける第1中間値u*(k)を次の第1式、即ち、
u*(k)={pd(k)−p(k)+Hvd(k)+Tv(k)
−∇a(k−1)/T}/(H+T) ・・・(第1式)
を満たすように算出し、
移動機構によって実現される物体の速度として許容されている速度の集合をYとしたときに、u*(k)∈Yが満たされるときにはu(k)=u*(k)を満たし、u*(k)∈Yが満たされないときにはu*(k)に正のスカラ値を乗じた値であり、かつu(k)∈Yを満たすように時刻kにおける第2中間値u(k)を算出し、
時刻kにおける第3中間値f*(k)を次の第2式、即ち、
f*(k)=La(k−1)+(LT+K)∇a(k−1)/T
+(LT2+KT+B)(u(k)−v(k)) ・・・(第2式)
を満たすように算出し、
時刻kにおける力の指令値f(k)は、移動機構が物体に加えることを許容されている力の集合をXとしたときに、f*(k)∈Xが満たされるときには指令値f(k)=f*(k)とし、f*(k)∈Xが満たされないときにはf*(k)に正のスカラ値を乗じた値であり、かつf(k)∈Xを満たすf(k)を指令値f(k)とし、
時刻k+1における第1中間値u*(k+1)と第3中間値f*(k+1)を算出する場合に第1式と第2式に現れるa(k)を次の第3式、即ち、
a(k)={(KT+B)a(k−1)+B∇a(k−1)
+T2f(k)}/(LT2+KT+B) ・・・(第3式)
を満たすように算出することを特徴とする位置制御方法。 - 物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を利用して、移動機構が保持した物体の位置を目標位置に追従させる位置制御方法であり、
物体の位置を検出する位置検出ステップと、
物体の速度を検出する速度検出ステップと、
検出した物体の位置と速度に基づいて、移動機構が物体に加える力の指令値を算出する指令値算出ステップと、
算出された力の指令値を移動機構へ出力する指令値出力ステップと、を含み、
前記指令値算出ステップは、
離散時間系における時刻をk、時刻kにおける物体の目標位置をpd(k)、時刻kにおける物体の目標速度をvd(k)、時刻kにおいて位置検出器によって検出される物体の位置をp(k)、時刻kにおいて速度検出器によって検出される物体の速度をv(k)、単位時間をT、第1の係数をK、第2の係数をB、第3の係数をH、第4の係数をL、時刻kにおいて演算される内部状態変数をe(k)としたときに、
時刻kにおける第1中間値u*(k)を次の第4式、即ち、
u*(k)={pd(k)−p(k)+Hvd(k)+Tv(k)
−e(k−1)}/(H+T) ・・・(第4式)
を満たすように算出し、
移動機構によって実現される物体の速度として許容されている速度の集合をYとしたときに、u*(k)∈Yが満たされるときにはu(k)=u*(k)を満たし、u*(k)∈Yが満たされないときにはu*(k)に正のスカラ値を乗じた値であり、かつu(k)∈Yを満たすように時刻kにおける第2中間値u(k)を算出し、
時刻kにおける第3中間値f*(k)を次の第5式、即ち、
f*(k)=Ke(k−1)+(KT+B)(u(k)−v(k))・・・(第5式)
を満たすように算出し、
時刻kにおける力の指令値f(k)は、移動機構が物体に加えることを許容されている力の集合をXとしたときに、f*(k)∈Xが満たされるときには指令値f(k)=f*(k)とし、f*(k)∈Xが満たされないときにはf*(k)に正のスカラ値を乗じた値であり、かつf(k)∈Xを満たすf(k)を指令値f(k)とし、
時刻k+1における第1中間値u*(k+1)と第3中間値f*(k+1)を算出する場合に第4式と第5式に現れるe(k)を次の第6式、即ち、
e(k)={Be(k−1)+Tf(k)}/(KT+B) ・・・(第6式)
を満たすように算出することを特徴とする位置制御方法。 - 物体を保持可能であり、物体に加える力の指令値が入力され、入力された指令値を実現するように動力によって動作する移動機構を制御するコンピュータによって、請求項7又は8に記載の位置制御方法を実行させるための位置制御プログラム。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2006275763A JP4587052B2 (ja) | 2006-10-06 | 2006-10-06 | 位置制御装置、位置制御方法及び位置制御プログラム |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2006275763A JP4587052B2 (ja) | 2006-10-06 | 2006-10-06 | 位置制御装置、位置制御方法及び位置制御プログラム |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2008097152A JP2008097152A (ja) | 2008-04-24 |
JP4587052B2 true JP4587052B2 (ja) | 2010-11-24 |
Family
ID=39379961
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2006275763A Expired - Fee Related JP4587052B2 (ja) | 2006-10-06 | 2006-10-06 | 位置制御装置、位置制御方法及び位置制御プログラム |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP4587052B2 (ja) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107272677A (zh) * | 2017-06-07 | 2017-10-20 | 东南大学 | 一种移动机器人的变结构自适应轨迹跟踪控制方法 |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106716282B (zh) * | 2016-12-17 | 2019-06-11 | 深圳前海达闼云端智能科技有限公司 | 一种对目标进行控制的方法、控制装置及控制设备 |
CN109828528A (zh) * | 2019-01-21 | 2019-05-31 | 河北工业职业技术学院 | 机器人轨迹跟踪方法及装置 |
CN109927032B (zh) * | 2019-03-28 | 2022-02-11 | 东南大学 | 一种基于高阶滑模观测器的机械臂轨迹跟踪控制方法 |
-
2006
- 2006-10-06 JP JP2006275763A patent/JP4587052B2/ja not_active Expired - Fee Related
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107272677A (zh) * | 2017-06-07 | 2017-10-20 | 东南大学 | 一种移动机器人的变结构自适应轨迹跟踪控制方法 |
Also Published As
Publication number | Publication date |
---|---|
JP2008097152A (ja) | 2008-04-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Nemec et al. | Human robot cooperation with compliance adaptation along the motion trajectory | |
Cheah et al. | Adaptive Jacobian tracking control of robots with uncertainties in kinematic, dynamic and actuator models | |
Duchaine et al. | Stable and intuitive control of an intelligent assist device | |
Singh et al. | Inverse dynamics and robust sliding mode control of a planar parallel (2-PRP and 1-PPR) robot augmented with a nonlinear disturbance observer | |
JP6934173B2 (ja) | 力制御装置、力制御方法及び力制御プログラム | |
Hao et al. | Observer-based robust control of 6-DOF parallel electrical manipulator with fast friction estimation | |
Kumar et al. | Trajectory control of a two DOF rigid–flexible space robot by a virtual space vehicle | |
JP6032811B2 (ja) | アドミッタンス制御を用いた力制御装置及び位置制御装置 | |
Wang et al. | Adaptive neural network-based visual servoing control for manipulator with unknown output nonlinearities | |
CN115890735B (zh) | 机械臂系统、机械臂及其控制方法、控制器和存储介质 | |
JP4587052B2 (ja) | 位置制御装置、位置制御方法及び位置制御プログラム | |
Scherzinger et al. | Inverse kinematics with forward dynamics solvers for sampled motion tracking | |
Cavacanti Santos et al. | Redundancy resolution of kinematically redundant parallel manipulators via differential dynamic programing | |
Li et al. | A nonlinear momentum observer for sensorless robot collision detection under model uncertainties | |
Bai et al. | Sliding mode nonlinear disturbance observer-based adaptive back-stepping control of a humanoid robotic dual manipulator | |
Žlajpah et al. | Unified virtual guides framework for path tracking tasks | |
JP7068133B2 (ja) | 制御システム、制御方法、及び制御プログラム | |
EP3978203A1 (en) | Singularity-free kinematic parameterization of soft robot manipulators | |
Ravi et al. | Redundancy resolution using tractrix—simulations and experiments | |
JP2007102748A (ja) | 位置制御装置、位置制御方法及び位置制御プログラム | |
Han et al. | Visual servoing control of robotics with a neural network estimator based on spectral adaptive law | |
Zhang et al. | Time delay compensation of a robotic arm based on multiple sensors for indirect teaching | |
Kurode et al. | Output feedback control of flexible link manipulator using sliding modes | |
Nemec et al. | Adaptive human robot cooperation scheme for bimanual robots | |
Al-Shuka et al. | Adaptive hybrid regressor and approximation control of robotic manipulators in constrained space |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20090410 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20100331 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20100420 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20100609 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20100810 |
|
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20100825 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 4587052 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130917 Year of fee payment: 3 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
LAPS | Cancellation because of no payment of annual fees |