JP5278850B2 - ３次元経路制御方法 - Google Patents

Description

本発明は、３次元空間上の位置目標値の軌跡である目標経路に実際の位置の軌跡である経路を追従させ、経路誤差を最小限に抑える経路制御の新規な方法に関する。

産業用ロボットや工作機械においては位置制御と同等に経路制御が重要となることが多い。経路制御とは、空間上の位置目標値の軌跡である目標経路に実際の位置の軌跡である経路を追従させ、経路に対する誤差を最小限に抑えることを目的とした制御である。一般に優れた経路応答を必要とする場合には、各軸独立にできるだけ制御特性を一致させたサーボ系を構成し、位置制御を行うことによって経路応答を得ることが多い。

しかし、各軸独立制御は、現在の目標値と現在の位置の差である誤差を小さくしようとする制御であって、経路誤差を小さくしようとしているわけではない。このため、各軸に異なった外乱や負荷変動などが生じた場合、あるいはパラメータ変動などが生じた場合などには経路応答が劣化してしまう問題があり、望ましい経路応答が得られないことが多い。

このような中で、本発明者は、３次元空間における経路制御手法として３次元ベクトル分解経路制御を提案し、その有効性を検証してきている（非特許文献１）。経路制御を考えた場合、工作機械やロボットマニピュレータなど一般のメカニカルシステムにおいては、２次元よりも３次元の方がより有効性が高いと考えられる。

しかし、３次元で経路誤差をどう評価するかは容易ではなく、ベクトル分解経路制御においては、経路の形状によって３種類に分類して扱いを変えて制御を行うようにしていたので、非常に煩雑であり、また、直線経路を除いた経路に対して正確な誤差評価ができていないという不都合がみられた。

これに対して本発明者は、２次元平面に対して、目標経路によらずに同一の扱いを可能とし、ベクトル分解経路制御の問題点（経路の形状によって制御則を変える不都合）を解決することで、ほぼ正確な経路誤差の評価を可能とする最短点探索を用いた経路制御手法を提案している（非特許文献２）。この手法は、ディジタル制御においては一般的に行われているように、目標経路を離散化した点集合として考える。そして現在位置と目標点とを比較し、最も近い目標点を探索によって求め、この距離を経路誤差として座標変換を用いて最小化を図る方法である。
佐藤,西川,江上:"３次元ベクトル分解経路制御とその応用",第46回システム制御情報学会研究発表講演会,pp45-46(2002) 田坂泰久,江上正:"探索を用いた経路制御手法の提案とその応用", 日本機械学会論文集(C編),vol.73,No.732(2007)

しかしながら、この手法は、ベクトル分解経路制御の最大の問題点である経路目標値によって制御則を切る替える不都合はないものの、この手法をそのまま３次元に拡張することは容易ではない。産業界では、３軸以上の３次元システムの経路制御の必要性が高いが、３次元システムの経路制御は、２次元システムに比べて、経路誤差の評価が格段に難しく、３次元へ拡張するためには新たな制御手法の構築が必要となる。

本発明は、係る事情に鑑みてなされたものであり、３次元空間における経路制御においても、ほぼ正確に経路誤差を評価でき、目標経路によらずに同じ扱いが可能となる新たな３次元経路制御方法を確立することを主たる課題としている。

しかして、本発明は、３次元空間への拡張を図るために、最短点探索を用いた経路制御手法の最短目標点探索の考え方と、３次元ベクトル分解経路制御の経路誤差の評価法とを基に、新たな３次元経路制御方法を提案する。

すなわち、本発明に係る３次元経路制御方法は、３次元空間上の位置目標値の軌跡である目標経路に実際の位置の軌跡である経路を追従させる経路制御方法であって、前記３次元空間上の位置目標値の軌跡である目標経路を離散化した目標点の集合として捉えて、現在位置から最も近い最短目標点を探索によって求め、前記最短目標点を含み、前記最短目標点と隣接した目標点から定まる方向をもつ目標点列ベクトルに対して垂直となる平面上で、前記現在位置と前記最短目標点間の距離を経路誤差として評価し、この経路誤差を小さくする制御を行うことを特徴としている。

したがって、３次元空間上の目標経路に追従させるために、現在位置から最も近い最短目標点を求め、この最短目標点を通り、その隣接する目標点を結んだ接線に相当する目標点列ベクトルに対して垂直な平面上で経路誤差を評価するようにしたので、３次元空間上においても目標経路によらずに同一の扱いが可能となり、ほぼ正確な経路誤差の評価が可能となる。

ここで、現在位置から最も近い最短目標点を探索によって求めるためには、探索数が少なく、且つ、短時間で行えることが望ましいため、単位サンプリング周期での現在位置の移動距離は前回の位置周辺にとどまると考え、前回の最短目標点を基準点として、その前後数点に対して現在位置からの距離を算出して探索するようにしてもよい。

また、前記現在位置に最も近い前記最短目標点を含み、目標点列ベクトルに対して垂直となる平面は、前記最短目標点とこの目標点に隣接する目標点とを用いて回転移動座標変換を行って得るようにするとよい。

さらに、各目標点間距離の総和（目標点の総移動距離）を成分とする軸を定義し、この軸上で進行方向誤差を評価し、この進行方向誤差を小さくする制御を前記経路誤差を小さくする制御とは独立に行う。このように進行方向誤差を独立に制御することで、進行方向での追従が遅延しても目標経路に精度よく沿わせる制御が可能となる。このような制御を行うために、前記現在位置に最も近い前記最短目標点とこの目標点に隣接する目標点とを用いて回転移動座標変換を行い、経路誤差方向と進行方向とに分解し、進行方向をさらに目標点の総移動距離に変換して制御するとよい。

以上述べたように、この発明に係る３次元経路制御方法によれば、３次元空間における経路制御においても、ほぼ正確に経路誤差を評価でき、目標経路によらずに同じ扱いが可能となり、産業ロボットや工作機械等など一般的なメカニカルシステムにおいてすぐれた経路応答が可能となる。

以下、この発明の最良の実施形態を説明する。
１． 最短目標点探索
本発明に係る経路制御を行うにあたり、まず、現在位置から最も近い目標経路上の最短目標点を探索する方法を考える。ディジタル制御に対応するように、目標経路に対して離散的な時間目標値を設定すればよいが、サンプリング周期によって各目標点間の距離が遠くなる場合には、各点間をさらに分割して目標増加点を設定するとより正確な経路誤差が得られる。そして、現在位置と最も近い最短目標点を探索によって求める。

しかし、探索を用いる場合にはできるだけ探索数が少なく、短時間で行えることが望ましい。そこで図１に示すように、１サンプリング周期での現在位置ｐ(k)の移動距離は、前回の位置周辺にとどまると考え、前回の最短目標点Ｒ(i)を基準点とし、その前後数点に対して次式（１）を用いて現在位置からの距離Ｅ(j)を算出し、最短点を探索する。

２． 座標変換
次に、前述した探索によって求めた最短目標点を用いて、座標変換を行う。図２のように、３次元経路上の現在位置ｐ(k)との最短目標点Ｒ(i)を探索により求め、前記最短目標点Ｒ(i)における経路との接線を求める。前記接線は前記最短目標点Ｒ(i)と隣接する前後の目標点を用いて求める。一例としてはＲ(i-1)とＲ(i+1)と結ぶ直線の傾きを持ち、Ｒ(i)を通る直線とする。前記接線をｘ−ｙ平面に投影し、その直線とｘ軸とのなす角をθとする。次式（２）によりｘ−ｙ座標系をｘ_１−ｙ_１座標系に変換する。

さらに、前記接線をｘ_１−ｚ座標系に投影し、その直線とｘ_１軸とのなす角ψを求め、次式（３）によりｄ−ｑ座標系に変換する。

次に、次式（４）、（５）の移動座標変換により、ｄ軸をｄ軸に平行で始点から探索によって求めたｉ番目の目標点である前記最短目標点Ｒ(i)までの目標点間距離の総和Ｒ_ｌ(i)を成分とするｌ軸に変換する。またｙ_１軸に関しては、前記最短目標点Ｒ(i)を原点として、ｙ_１軸に平行でｙ_１軸の誤差を成分とするｍ軸に、ｑ軸に関しては前記最短目標点Ｒ(i)を原点としてｑ軸に平行でｑ軸の誤差を成分とするｎ軸にそれぞれ変換する。すなわちｄ−ｙ_１−ｑ座標系をｌ−ｍ−ｎ座標系に変換する。

３．誤差の評価
前述したような座標変換を行うことで、ｌ軸が進行方向となり、前記最短目標点Ｒ(i)を通る接線とＲ(i)を原点とするｍ−ｎ平面が垂直となる。そして、ｌ軸成分とｍ軸成分、ｎ軸成分はそれぞれ独立で非干渉となる特徴を有する。このときｌ軸成分は現時刻のｌ軸位置p_l(k)をあらかじめ与えるｌ軸目標値Ｒ_ｌ(k)に式（６）のように追従させることにより進行方向を制御する。

またｍ、ｎ軸成分は

とすることによって、現在位置ｐ(k)がｍ−ｎ平面上にあるときには経路誤差を完全に０とすることができる。現在位置ｐ(k)が、最短点目標値Ｒ(i)の法線上からずれる場合にはその分だけｍ−ｎ平面上からもずれ、経路誤差の評価が完全に正確ではなくなるが、目標点間の分割を適切に行うことによりその影響はあまり大きくないことは確認している。またｌ軸とｍ−ｎ平面の制御は独立に扱えるため、外乱などの影響も経路誤差とは直接関係のないｌ軸で吸収し、ｍ−ｎ平面の応答への影響を少なくすることも可能である。

４.座標変換の適用
制御対象は一般の３次元空間上で運動するメカニカルシステムである。ここでは一例として一般のＸＹＺテーブルに適用する。

ＸＹＺテーブルはリニアモータ駆動のものでも、回転モータ駆動にボールねじを用いたものでも以下のような運動方程式で表される。ロボットマニピュレータの場合も非線形補償などを行えばこの形で表すことができる。

ここで、ｉ＝ｘ，ｙ，ｚで ｐ_i:位置、ｖ_i:速度、ｕ_ci:入力、d_ci:負荷推力
なお、リニアモータ駆動のＸＹＺテーブルの場合、Ｍ_i：負荷を含めた質量、Ｄ_i：粘性係数、Ｋ_Ｆi:推力定数として、

となり、回転モータ駆動にボールネジを用いた場合、例えばＪ_i：モータ、ボールネジ、駆動部を含めた慣性モーメント、ｍ_i:ナット、台車、負荷を含めた質量、Ｋ_i:トルク定数、ｐ_ti:ボールネジピッチとして、

などとなる。

パラメータは各軸で異なるため、特性を一致させるように共通のパラメータ変数ａ、ｂを用いると式(１０)は、

となる。

式（９）、（１３）に対して式（２）から（５）の変換を行うと、式（９）、（１３）式と同形の微分方程式がｌ軸、ｍ軸、ｎ軸に対して式（１４）から（１９）で得られる。

５．シミュレーション
本手法の有効性を確認するためにシミュレーション結果を示す。制御系は１型のものであればどのような制御系を用いてもよいが、ここでは式（１４）〜（１９）を離散時間系に変換し、最適ディジタルサーボ系を構成して制御する。比較のための各軸独立制御の場合も同様に、特性一致させた式（９）、（１３）に対して最適デジタルサーボ系を構成している。

今回、直線目標経路と曲線目標経路に対してシミュレーションを行っており、それぞれ目標値は、[ｘ(t)，ｙ(t)，ｚ(t)]=[0.04t，0.04t，0.04t]および [ｘ(t)，ｙ(t)，ｚ(t)]=[0.1sin(πt/10)，0.04t，0.03t]としている。始点はｚ軸方向に１０mmずれたところとし、制御時間は１０s、サンプリング周期は１msとしている。また探索に用いる目標増加点はサンプリング周期間を１００分割したものとし、基準点の前後それぞれ２５０点を探索した。開始３秒後にｙ軸に２０N、ｚ軸に３０Nの外乱を印加している。その結果を図４〜図７に示す。図４、５は直線経路目標値、図６、７は曲線経路目標値の応答結果を示している。図４、６は最短目標点探索を用いた本手法の経路制御を行った場合で、図５、７は比較のために示した各軸独立制御を行った場合である。それぞれ(a)は３次元応答、(b)は経路誤差、(c)はｘ軸応答、(d)はｙ軸応答、(e)はｚ軸応答を示している。図８(a)、(b)はそれぞれ図４と図５の外乱のない部分の経路誤差を比較したものである。

この結果より、本手法によると直線経路でも曲線経路でもすべての軸が協調することによって外乱の影響を大きく抑制できていることが確認できる。また図８から外乱の影響がない場合でも各軸独立制御では経路誤差が発生しているのに対し、本手法を用いた場合では経路誤差をほとんど０とできていることが確認できる。

図１は、現在位置ｐ(k)から最も近い目標経路上の最短目標点Ｒ(i)を探索する手法を説明する図である。 図２は、座標変換の手法を説明する図である。 図３は、変換した座標上で経路誤差と進行方向を説明する図である。 図４は、目標経路を直線とした場合の最短目標点探索を用いた本手法の経路制御を行った場合の応答結果を示す図である。 図５は、目標経路を直線とした場合の各軸独立制御を行った場合の応答結果を示す図である。 図６は、目標経路を曲線とした場合の最短目標点探索を用いた本手法の経路制御を行った場合の応答結果を示す図である。 図７は、目標経路を曲線とした場合の各軸独立制御を行った場合の応答結果を示す図である。 図８は、目標経路を直線とした場合の図４の最短目標点探索を用いた本手法の経路制御を行った場合と図５の各軸独立制御を行った場合の外乱のない場合の経路誤差を比較した図である。

Claims (4)

1. ３次元空間上の位置目標値の軌跡である目標経路に実際の位置の軌跡である経路を追従させる経路制御方法であって、
   前記３次元空間上の位置目標値の軌跡である目標経路を、ディジタル制御に対応するように離散化した目標点の集合として捉えて、現在位置から最も近い最短目標点を探索によって求め、前記最短目標点を含み、その隣接する目標点を結んだ目標点列ベクトルに対して垂直となる平面上で、前記現在位置と前記最短目標点間の距離を経路誤差として評価し、この経路誤差を小さくする制御を行うことを特徴とする３次元経路制御方法。
2. 現在位置から最も近い最短目標点の探索は、前回の最短目標点を基準点として、その前後数点に対して現在位置からの距離を算出して探索するものである請求項１記載の３次元経路制御方法。
3. 前記現在位置に最も近い前記最短目標点を含み、目標点列ベクトルに対して垂直となる平面は、前記最短目標点とこの目標点に隣接する目標点とを用いて回転移動座標変換を行って得られるものであることを特徴とする請求項１記載の３次元経路制御方法。
4. 各目標点間距離の総和を成分とする軸を定義し、この軸上で進行方向誤差を評価し、この進行方向誤差を小さくする制御を前記経路誤差を小さくする制御とは独立に行うことを特徴とする請求項１乃至３のいずれかに記載の３次元経路制御方法。

Images