TOP > 国内特許検索 > 3次元経路制御方法 > 明細書

明細書 :3次元経路制御方法

発行国 日本国特許庁(JP)
公報種別 特許公報(B2)
特許番号 特許第5278850号 (P5278850)
公開番号 特開2010-026613 (P2010-026613A)
登録日 平成25年5月31日(2013.5.31)
発行日 平成25年9月4日(2013.9.4)
公開日 平成22年2月4日(2010.2.4)
発明の名称または考案の名称 3次元経路制御方法
国際特許分類 G05B  19/42        (2006.01)
G05B  19/4103      (2006.01)
B25J   9/10        (2006.01)
G05B  19/404       (2006.01)
FI G05B 19/42 W
G05B 19/4103 D
B25J 9/10 A
G05B 19/404 Z
請求項の数または発明の数 4
全頁数 11
出願番号 特願2008-184348 (P2008-184348)
出願日 平成20年7月16日(2008.7.16)
新規性喪失の例外の表示 特許法第30条第1項適用 平成20年1月17日 社団法人計測自動制御学会北海道支部主催の「第40回計測自動制御学会北海道支部学術講演会」において文書をもって発表
審査請求日 平成23年6月28日(2011.6.28)
特許権者または実用新案権者 【識別番号】592218300
【氏名又は名称】学校法人神奈川大学
発明者または考案者 【氏名】江上 正
個別代理人の代理人 【識別番号】110000545、【氏名又は名称】特許業務法人大貫小竹国際特許事務所
審査官 【審査官】柿崎 拓
参考文献・文献 特開平07-005908(JP,A)
田坂泰久,江上正,探索を用いた経路制御手法の提案とその応用,日本機械学会論文集(C編),日本,2007年 8月,73巻732号,64-71
調査した分野 G05B 19/18-19/416,19/42-19/46
特許請求の範囲 【請求項1】
3次元空間上の位置目標値の軌跡である目標経路に実際の位置の軌跡である経路を追従させる経路制御方法であって、
前記3次元空間上の位置目標値の軌跡である目標経路を、ディジタル制御に対応するように離散化した目標点の集合として捉えて、現在位置から最も近い最短目標点を探索によって求め、前記最短目標点を含み、その隣接する目標点を結んだ目標点列ベクトルに対して垂直となる平面上で、前記現在位置と前記最短目標点間の距離を経路誤差として評価し、この経路誤差を小さくする制御を行うことを特徴とする3次元経路制御方法。
【請求項2】
現在位置から最も近い最短目標点の探索は、前回の最短目標点を基準点として、その前後数点に対して現在位置からの距離を算出して探索するものである請求項1記載の3次元経路制御方法。
【請求項3】
前記現在位置に最も近い前記最短目標点を含み、目標点列ベクトルに対して垂直となる平面は、前記最短目標点とこの目標点に隣接する目標点とを用いて回転移動座標変換を行って得られるものであることを特徴とする請求項1記載の3次元経路制御方法。
【請求項4】
各目標点間距離の総和を成分とする軸を定義し、この軸上で進行方向誤差を評価し、この進行方向誤差を小さくする制御を前記経路誤差を小さくする制御とは独立に行うことを特徴とする請求項1乃至3のいずれかに記載の3次元経路制御方法。
発明の詳細な説明 【技術分野】
【0001】
本発明は、3次元空間上の位置目標値の軌跡である目標経路に実際の位置の軌跡である経路を追従させ、経路誤差を最小限に抑える経路制御の新規な方法に関する。
【背景技術】
【0002】
産業用ロボットや工作機械においては位置制御と同等に経路制御が重要となることが多い。経路制御とは、空間上の位置目標値の軌跡である目標経路に実際の位置の軌跡である経路を追従させ、経路に対する誤差を最小限に抑えることを目的とした制御である。一般に優れた経路応答を必要とする場合には、各軸独立にできるだけ制御特性を一致させたサーボ系を構成し、位置制御を行うことによって経路応答を得ることが多い。
【0003】
しかし、各軸独立制御は、現在の目標値と現在の位置の差である誤差を小さくしようとする制御であって、経路誤差を小さくしようとしているわけではない。このため、各軸に異なった外乱や負荷変動などが生じた場合、あるいはパラメータ変動などが生じた場合などには経路応答が劣化してしまう問題があり、望ましい経路応答が得られないことが多い。
【0004】
このような中で、本発明者は、3次元空間における経路制御手法として3次元ベクトル分解経路制御を提案し、その有効性を検証してきている(非特許文献1)。経路制御を考えた場合、工作機械やロボットマニピュレータなど一般のメカニカルシステムにおいては、2次元よりも3次元の方がより有効性が高いと考えられる。
【0005】
しかし、3次元で経路誤差をどう評価するかは容易ではなく、ベクトル分解経路制御においては、経路の形状によって3種類に分類して扱いを変えて制御を行うようにしていたので、非常に煩雑であり、また、直線経路を除いた経路に対して正確な誤差評価ができていないという不都合がみられた。
【0006】
これに対して本発明者は、2次元平面に対して、目標経路によらずに同一の扱いを可能とし、ベクトル分解経路制御の問題点(経路の形状によって制御則を変える不都合)を解決することで、ほぼ正確な経路誤差の評価を可能とする最短点探索を用いた経路制御手法を提案している(非特許文献2)。この手法は、ディジタル制御においては一般的に行われているように、目標経路を離散化した点集合として考える。そして現在位置と目標点とを比較し、最も近い目標点を探索によって求め、この距離を経路誤差として座標変換を用いて最小化を図る方法である。

【非特許文献1】佐藤,西川,江上:"3次元ベクトル分解経路制御とその応用",第46回システム制御情報学会研究発表講演会,pp45-46(2002)
【非特許文献2】田坂泰久,江上正:"探索を用いた経路制御手法の提案とその応用", 日本機械学会論文集(C編),vol.73,No.732(2007)
【発明の開示】
【発明が解決しようとする課題】
【0007】
しかしながら、この手法は、ベクトル分解経路制御の最大の問題点である経路目標値によって制御則を切る替える不都合はないものの、この手法をそのまま3次元に拡張することは容易ではない。産業界では、3軸以上の3次元システムの経路制御の必要性が高いが、3次元システムの経路制御は、2次元システムに比べて、経路誤差の評価が格段に難しく、3次元へ拡張するためには新たな制御手法の構築が必要となる。
【0008】
本発明は、係る事情に鑑みてなされたものであり、3次元空間における経路制御においても、ほぼ正確に経路誤差を評価でき、目標経路によらずに同じ扱いが可能となる新たな3次元経路制御方法を確立することを主たる課題としている。
【課題を解決するための手段】
【0009】
しかして、本発明は、3次元空間への拡張を図るために、最短点探索を用いた経路制御手法の最短目標点探索の考え方と、3次元ベクトル分解経路制御の経路誤差の評価法とを基に、新たな3次元経路制御方法を提案する。
【0010】
すなわち、本発明に係る3次元経路制御方法は、3次元空間上の位置目標値の軌跡である目標経路に実際の位置の軌跡である経路を追従させる経路制御方法であって、前記3次元空間上の位置目標値の軌跡である目標経路を離散化した目標点の集合として捉えて、現在位置から最も近い最短目標点を探索によって求め、前記最短目標点を含み、前記最短目標点と隣接した目標点から定まる方向をもつ目標点列ベクトルに対して垂直となる平面上で、前記現在位置と前記最短目標点間の距離を経路誤差として評価し、この経路誤差を小さくする制御を行うことを特徴としている。
【0011】
したがって、3次元空間上の目標経路に追従させるために、現在位置から最も近い最短目標点を求め、この最短目標点を通り、その隣接する目標点を結んだ接線に相当する目標点列ベクトルに対して垂直な平面上で経路誤差を評価するようにしたので、3次元空間上においても目標経路によらずに同一の扱いが可能となり、ほぼ正確な経路誤差の評価が可能となる。
【0012】
ここで、現在位置から最も近い最短目標点を探索によって求めるためには、探索数が少なく、且つ、短時間で行えることが望ましいため、単位サンプリング周期での現在位置の移動距離は前回の位置周辺にとどまると考え、前回の最短目標点を基準点として、その前後数点に対して現在位置からの距離を算出して探索するようにしてもよい。
【0013】
また、前記現在位置に最も近い前記最短目標点を含み、目標点列ベクトルに対して垂直となる平面は、前記最短目標点とこの目標点に隣接する目標点とを用いて回転移動座標変換を行って得るようにするとよい。
【0014】
さらに、各目標点間距離の総和(目標点の総移動距離)を成分とする軸を定義し、この軸上で進行方向誤差を評価し、この進行方向誤差を小さくする制御を前記経路誤差を小さくする制御とは独立に行う。このように進行方向誤差を独立に制御することで、進行方向での追従が遅延しても目標経路に精度よく沿わせる制御が可能となる。このような制御を行うために、前記現在位置に最も近い前記最短目標点とこの目標点に隣接する目標点とを用いて回転移動座標変換を行い、経路誤差方向と進行方向とに分解し、進行方向をさらに目標点の総移動距離に変換して制御するとよい。
【発明の効果】
【0015】
以上述べたように、この発明に係る3次元経路制御方法によれば、3次元空間における経路制御においても、ほぼ正確に経路誤差を評価でき、目標経路によらずに同じ扱いが可能となり、産業ロボットや工作機械等など一般的なメカニカルシステムにおいてすぐれた経路応答が可能となる。
【発明を実施するための最良の形態】
【0016】
以下、この発明の最良の実施形態を説明する。
1. 最短目標点探索
本発明に係る経路制御を行うにあたり、まず、現在位置から最も近い目標経路上の最短目標点を探索する方法を考える。ディジタル制御に対応するように、目標経路に対して離散的な時間目標値を設定すればよいが、サンプリング周期によって各目標点間の距離が遠くなる場合には、各点間をさらに分割して目標増加点を設定するとより正確な経路誤差が得られる。そして、現在位置と最も近い最短目標点を探索によって求める。
【0017】
しかし、探索を用いる場合にはできるだけ探索数が少なく、短時間で行えることが望ましい。そこで図1に示すように、1サンプリング周期での現在位置p(k)の移動距離は、前回の位置周辺にとどまると考え、前回の最短目標点R(i)を基準点とし、その前後数点に対して次式(1)を用いて現在位置からの距離E(j)を算出し、最短点を探索する。
【数1】
JP0005278850B2_000002t.gif

【0018】
2. 座標変換
次に、前述した探索によって求めた最短目標点を用いて、座標変換を行う。図2のように、3次元経路上の現在位置p(k)との最短目標点R(i)を探索により求め、前記最短目標点R(i)における経路との接線を求める。前記接線は前記最短目標点R(i)と隣接する前後の目標点を用いて求める。一例としてはR(i-1)とR(i+1)と結ぶ直線の傾きを持ち、R(i)を通る直線とする。前記接線をx-y平面に投影し、その直線とx軸とのなす角をθとする。次式(2)によりx-y座標系をx-y座標系に変換する。
【数2】
JP0005278850B2_000003t.gif

【0019】
さらに、前記接線をx-z座標系に投影し、その直線とx軸とのなす角ψを求め、次式(3)によりd-q座標系に変換する。
【数3】
JP0005278850B2_000004t.gif

【0020】
次に、次式(4)、(5)の移動座標変換により、d軸をd軸に平行で始点から探索によって求めたi番目の目標点である前記最短目標点R(i)までの目標点間距離の総和R(i)を成分とするl軸に変換する。またy軸に関しては、前記最短目標点R(i)を原点として、y軸に平行でy軸の誤差を成分とするm軸に、q軸に関しては前記最短目標点R(i)を原点としてq軸に平行でq軸の誤差を成分とするn軸にそれぞれ変換する。すなわちd-y-q座標系をl-m-n座標系に変換する。
【数4】
JP0005278850B2_000005t.gif

【0021】
3.誤差の評価
前述したような座標変換を行うことで、l軸が進行方向となり、前記最短目標点R(i)を通る接線とR(i)を原点とするm-n平面が垂直となる。そして、l軸成分とm軸成分、n軸成分はそれぞれ独立で非干渉となる特徴を有する。このときl軸成分は現時刻のl軸位置pl(k)をあらかじめ与えるl軸目標値R(k)に式(6)のように追従させることにより進行方向を制御する。
【数5】
JP0005278850B2_000006t.gif
またm、n軸成分は
【数6】
JP0005278850B2_000007t.gif
とすることによって、現在位置p(k)がm-n平面上にあるときには経路誤差を完全に0とすることができる。現在位置p(k)が、最短点目標値R(i)の法線上からずれる場合にはその分だけm-n平面上からもずれ、経路誤差の評価が完全に正確ではなくなるが、目標点間の分割を適切に行うことによりその影響はあまり大きくないことは確認している。またl軸とm-n平面の制御は独立に扱えるため、外乱などの影響も経路誤差とは直接関係のないl軸で吸収し、m-n平面の応答への影響を少なくすることも可能である。
【0022】
4.座標変換の適用
制御対象は一般の3次元空間上で運動するメカニカルシステムである。ここでは一例として一般のXYZテーブルに適用する。
【0023】
XYZテーブルはリニアモータ駆動のものでも、回転モータ駆動にボールねじを用いたものでも以下のような運動方程式で表される。ロボットマニピュレータの場合も非線形補償などを行えばこの形で表すことができる。
【数7】
JP0005278850B2_000008t.gif
ここで、i=x,y,zで pi:位置、vi:速度、uci:入力、dci:負荷推力
なお、リニアモータ駆動のXYZテーブルの場合、Mi:負荷を含めた質量、Di:粘性係数、KFi:推力定数として、
【数8】
JP0005278850B2_000009t.gif
となり、回転モータ駆動にボールネジを用いた場合、例えばJi:モータ、ボールネジ、駆動部を含めた慣性モーメント、mi:ナット、台車、負荷を含めた質量、Ki:トルク定数、pti:ボールネジピッチとして、
【数9】
JP0005278850B2_000010t.gif
などとなる。
【0024】
パラメータは各軸で異なるため、特性を一致させるように共通のパラメータ変数a、bを用いると式(10)は、
【数10】
JP0005278850B2_000011t.gif
となる。
【0025】
式(9)、(13)に対して式(2)から(5)の変換を行うと、式(9)、(13)式と同形の微分方程式がl軸、m軸、n軸に対して式(14)から(19)で得られる。
【数11】
JP0005278850B2_000012t.gif

【0026】
5.シミュレーション
本手法の有効性を確認するためにシミュレーション結果を示す。制御系は1型のものであればどのような制御系を用いてもよいが、ここでは式(14)~(19)を離散時間系に変換し、最適ディジタルサーボ系を構成して制御する。比較のための各軸独立制御の場合も同様に、特性一致させた式(9)、(13)に対して最適デジタルサーボ系を構成している。
【0027】
今回、直線目標経路と曲線目標経路に対してシミュレーションを行っており、それぞれ目標値は、[x(t),y(t),z(t)]=[0.04t,0.04t,0.04t]および [x(t),y(t),z(t)]=[0.1sin(πt/10),0.04t,0.03t]としている。始点はz軸方向に10mmずれたところとし、制御時間は10s、サンプリング周期は1msとしている。また探索に用いる目標増加点はサンプリング周期間を100分割したものとし、基準点の前後それぞれ250点を探索した。開始3秒後にy軸に20N、z軸に30Nの外乱を印加している。その結果を図4~図7に示す。図4、5は直線経路目標値、図6、7は曲線経路目標値の応答結果を示している。図4、6は最短目標点探索を用いた本手法の経路制御を行った場合で、図5、7は比較のために示した各軸独立制御を行った場合である。それぞれ(a)は3次元応答、(b)は経路誤差、(c)はx軸応答、(d)はy軸応答、(e)はz軸応答を示している。図8(a)、(b)はそれぞれ図4と図5の外乱のない部分の経路誤差を比較したものである。
【0028】
この結果より、本手法によると直線経路でも曲線経路でもすべての軸が協調することによって外乱の影響を大きく抑制できていることが確認できる。また図8から外乱の影響がない場合でも各軸独立制御では経路誤差が発生しているのに対し、本手法を用いた場合では経路誤差をほとんど0とできていることが確認できる。
【図面の簡単な説明】
【0029】
【図1】図1は、現在位置p(k)から最も近い目標経路上の最短目標点R(i)を探索する手法を説明する図である。
【図2】図2は、座標変換の手法を説明する図である。
【図3】図3は、変換した座標上で経路誤差と進行方向を説明する図である。
【図4】図4は、目標経路を直線とした場合の最短目標点探索を用いた本手法の経路制御を行った場合の応答結果を示す図である。
【図5】図5は、目標経路を直線とした場合の各軸独立制御を行った場合の応答結果を示す図である。
【図6】図6は、目標経路を曲線とした場合の最短目標点探索を用いた本手法の経路制御を行った場合の応答結果を示す図である。
【図7】図7は、目標経路を曲線とした場合の各軸独立制御を行った場合の応答結果を示す図である。
【図8】図8は、目標経路を直線とした場合の図4の最短目標点探索を用いた本手法の経路制御を行った場合と図5の各軸独立制御を行った場合の外乱のない場合の経路誤差を比較した図である。
図面
【図1】
0
【図2】
1
【図3】
2
【図4】
3
【図5】
4
【図6】
5
【図7】
6
【図8】
7