JP5555838B2 - 軌道生成方法および軌道生成装置 - Google Patents

Description

本発明は軌道生成方法および軌道生成装置に関する。さらに詳しくは、所要エネルギーが低減されてなる軌道生成方法および軌道生成装置に関する。

従来より、ロボットにおいては各種の補間法により軌道生成がなされてロボットの移動がなされている。

この補間法には、任意の点列を滑らかに移動するよう軌道生成をなすスプライン補間法が知られている。

しかしながら、スプライン補間法は、移動速度や加速度を一定とする制御には不向きであるという欠点を有している。

かかるスプライン補間法の欠点を解消すべく特許文献１には、接線方向のピッチ角およびヨー角のそれぞれが曲線長軌跡またはワークの輪郭形状を表現し、この３次元曲線によって工具の運動を制御する数値制御方法および装置が提案されている。

しかしながら、特許文献１におる制御方法は、曲率および接線方向を同時に指定なし得ないという欠点を有しているので、所要エネルギーの低減を図るのが困難であるという問題を有している。

特開２００５−２７６１５５号公報

本発明はかかる従来技術の課題に鑑みなされたものであって、所要エネルギーの低減を図ることができる軌道生成方法および軌道生成装置を提供することを目的としている。

本発明の軌道生成方法は、点列間をクロソイド曲線により補間して軌道生成をなす軌道生成方法であって、前記クロソイド曲線が三連クロソイド曲線とされ、それにより通過点での接線方向の連続性および曲率の連続性が担保されてなることを特徴とする。

本発明の軌道生成方法においては、通過点での接線方向が弦ベクトル転向角の二分法により算出されるのが好ましい。

また、本発明の軌道生成方法においては、ローカル座標系におけるクロソイド曲線を生成し、ついで生成されたクロソイド曲線をグローバル座標系に変換し、座標変換して得られたクロソイド曲線により補間をなすのが好ましい。

さらに、本発明の軌道生成方法においては、端点での接線方向を接続させる曲線と一致させるようにしてクロソイド曲線を生成するのが好ましい。

一方、本発明の軌道生成装置の第１形態は、点列間をクロソイド曲線により補間して軌道生成をなす軌道生成装置であって、入力手段と、演算処理装置と、出力手段とを備え、前記演算処理装置が、クロソイド曲線生成手段と、座標変換手段と、補間指令値生成手段とを有し、前記クロソイド曲線生成手段が、接線方向算出部と、単一の２次元クロソイド曲線生成部と、三連２次元クロソイド曲線生成部とを有することを特徴とする。

また、本発明の軌道生成装置の第２形態は、点列間をクロソイド曲線により補間して軌道生成をなす軌道生成装置であって、入力手段と、演算処理装置と、出力手段とを備え、前記演算処理装置が、クロソイド曲線生成手段と、座標変換手段と、補間指令値生成手段とを有し、前記クロソイド曲線生成手段が、接線方向算出部と、単一の３次元クロソイド曲線生成部と、三連３次元クロソイド曲線生成部とを有するものとされる。

しかして、本発明の軌道生成方法により生成された軌道はロボットなどの移動体の移動に適用される。

また、本発明の軌道生成装置は、ロボットなどの移動体に備えられる。

本発明は前記の如く構成されているので、所要エネルギーが低減される軌道の生成がなし得るという優れた効果が得られる。

クロソイド曲線の一例を示すグラフ図である。 ローカル座標系における二点の座標および相対角を示す説明図である。 ２次元クロソイド曲線による補間曲線の例を示す例示図である。 弦ベクトル転向角の二分法の説明図である。 単一の２次元クロソイド曲線による補間例の例示図である。 三連２次元クロソイド曲線による補間例の例示図である。 三連２次元クロソイド曲線による直線との補間例の例示図である。 ３次元クロソイド曲線の接線ベクトルの説明図である。 ３次元クロソイド曲線の曲率の変化の例を示す例示図である。 ３次元クロソイド曲線による補間曲線例を示す例示図である。 三連３次元クロソイド曲線による補間曲線例を示す例示図である。 三連３次元クロソイド曲線による直線との補間例を示す例示図である。 本発明の一実施形態に係る軌道生成装置のブロック図である。 本発明の他の実施形態に係る軌道生成装置のブロック図である。

以下、添付図面を参照しながら本発明を実施形態に基づいて説明するが、本発明はかかる実施形態のみに限定されるものではない。

実施形態１
本発明は、クロソイド曲線を利用して軌道の生成をなすものである。

本発明の理解を容易にするため、まず２次元クロソイド曲線について説明する。

一般的に、任意の連続曲線セグメントは以下の式（１）により表わされる。

ここで、ベクトルＰ_０は曲線の始点（ベクトル）を示し、ベクトルｕは曲線上の任意の単位接線ベクトルを示し、ｓは始点からの曲線長を示し、ｈは曲線の全長を示し、Ｓ（０≦Ｓ≦１）は無次元の曲線長を示す。

いま、ベクトルｕをＳの関数として定義すれば、それに対応する曲線が求められる。

複素平面において、２次元クロソイド曲線の単位接線ベクトル（ベクトルｕ）は以下の式（２）により与えられる。

ここで、φは曲線上の任意点の接線方向を示し、φ_０は始点における接線方向を示し、φ_ｖは接線方向の１次増分係数を示し、φ_ｕは接線方向の２次増分係数を示す。

式（１）と式（２）による積分は一般に初等関数で表わせないことが知られており、数値積分などの数値法で演算する必要がある。

図１に、数値積分により得られた２次元クロソイド曲線の一例を示す。

φをｓで微分すると２次元クロソイド曲線（単一の２次元クロソイド曲線）の曲率が得られる。すなわち、下記式（３）が得られる。

式（３）により、得られた曲率が符号を持つことが理解される。つまり、曲線が時計方向に曲がるならば負、反時計方向に曲がるならば正となる。また、式（３）により、得られた曲率は曲線長のリニア関数であることが理解される。

次に、図２を参照しながら平面上の任意の２点Ｐ_０と点Ｐ_１との間の補間について説明する。

点Ｐ_０の接線角Ψ_０とし、点Ｐ_１での接線角Ψ_１とする。

演算処理を簡素化するため、点Ｐ_０を原点とし、Ｘ軸を点Ｐ_０から点Ｐ_１に向かう方向に取ったローカル座標系を採用する。

このローカル座標系においては、曲線は原点（０，０）から出発し、点（ｒ，０）に終着するので、曲線は以下の式（４）により得られる。

式（４）と式（２）の二番目の式から各パラメータφ_０、φ_ｖ、φ_ｕおよびｈが求められる。

このようにして得られた各パラメータをグローバル座標系の値に変換するには、φ_０にθを加算すればよい。それ以外のパラメータは、ローカル座標系の値がそのまま使用できる。

図３に、θを３０度とし、φ_１を−３０度として、φ_０を−１８０度〜１８０度において１０度ずつ変化させた場合における軌跡の生成例を示す。

ここで、前記補間法を「曲線端点の接線方向指定法」と定義することにする。

この曲線端点の接線方向指定法は、本願発明の基礎となる。

ちなみに、ロボットの移動経路を生成する補間法においては、加速度が不連続になるとロボットの動作が不安定となるため、加速度の連続性が要求される。一方、曲率が連続していない軌道上における移動は加速度が不連続となる。そのため、ロボットなどの移動経路のための補間法においては、曲率の連続性が要求される。

しかるに、前記曲線端点の接線方向指定法においては、通過点での接線角を指定することで、２次元クロソイド曲線の全パラメータφ_０、φ_ｖ、φ_ｕおよびｈを決定するため、通過点での曲率を指定することができない。通過点での接線角および曲率の両者を同時に指定するには、クロソイド曲線のパラメータを増やす必要がある。その場合、リニアな曲率を維持したところから、Ｓの３次項，Ｓの４次項などの高次項が含まれないようにしてパラメータを設ける必要がある。

この高次項が含まれないようにしてパラメータを決定する手法を以下に説明する。この手法をここでは三連２次元クロソイド曲線による補間法と定義することにする。

三連２次元クロソイド曲線の単位接線ベクトルｕを式（５），式（６）および式（７）により定義する。

ここで、無次元の定数Ｓ_１およびＳ_２（０<Ｓ_１<Ｓ_２<１）は三連２次元クロソイド曲線のつなぎ目の位置を示す。
なお、無次元の定数Ｓ_１およびＳ_２は、任意に指定できるが、対称性を保ちたい場合には、Ｓ_１＝γ（０<γ<０．５）と指定し、Ｓ_２＝１−γと指定すればよい。本発明者等は、γ＝０．２５を推奨値、つまりＳ_１＝０．２５およびＳ_２＝０．７５を推奨値としている。

また、ａ_１０，ａ_１１，ａ_１２，ａ_２０，ａ_２１，ａ_２２，ａ_３０，ａ_３１，ａ_３２およびｈは三連２次元クロソイド曲線のパラメータである。これらのパラメータは全部で１０個あるが、式（７）により、そのうち４個は他のパラメータにより決定されるため、独立のパラメータは６個である。

また、式（７）の第１式および第２式は、Ｓ_１およびＳ_２での接線方向の連続条件を示し、式（７）の第３式および第４式は、Ｓ_１およびＳ_２での曲率の連続条件を示す。

式（５）〜式（７）および式（１）より、三連２次元クロソイド曲線の曲率は下記式（８）により与えられる。

この式（８）により得られた曲率をＳに対して描くと３つの連続した線分になる。

また、三連２次元クロソイド曲線の特性は、単一の２次元クロソイド曲線の特性に似ていて直線および円の表現も可能であるが、それに加えてゼロの曲率の位置を３つまで持つことができる。なお、単一の２次元クロソイド曲線はゼロの曲率の位置を１つしか持てない。

次に、この三連２次元クロソイド曲線による点列の補間について説明する。

補間したい自由点列（点数:Ｎ＋１）を下記（９）とする。

Ｐ_０，Ｐ_１，Ｐ_２，・・・，Ｐ_Ｎ （Ｐはベクトルとする） （９）

ここで、これらの各点での接線方向を、図４に示すように、弦ベクトル転向角の二分法により決定する。

図４において、ベクトルＡＢは、Ｐ_０ＣとＰ_１Ｐ_２との間の角を二等分し、∠ＡＰ_０Ｐ_１＝∠ＡＰ_１Ｐ_０とする。Ｐ_０での接線方向をＰ_０Ａとし、Ｐ_１での接線方向をＰ_１Ｂとする。ついで、点Ｐ_２での接線方向Ｐ_２Ｅを図４（ｂ）のように定める。同様にして他の点Ｐ_３，Ｐ_４，・・・，Ｐ_Ｎでの接線方向も定める。

自由点列Ｐ_０，Ｐ_１，Ｐ_２，・・・，Ｐ_Ｎに対して前述したように、弦ベクトル転向角の二分法により接線方向を指定し、単一の２次元クロソイド曲線により補間すると、得られた補間曲線は各通過点での接線方向は連続になるが、一般的には、各通過点での曲率は連続とはならない。

図５、このようにして補間された通過点Ｐ_ｋ前後の軌道を点線で示す。

そこで、本発明では、三連２次元クロソイド曲線による点列の補間を行う。

例として、点Ｐ_ｋと点Ｐ_ｋ＋１との区間を三連２次元クロソイド曲線により補間し、ついで点Ｐ_ｋおよび点Ｐ_ｋ＋１の接線方向を変えずに曲率を連続させる場合について説明する（図５参照）。なお、図５中、実線は目標曲線を示す。

軌道の生成においては各点における曲率を同一とする場合、低い方の曲率に他方の曲率を合わせるのが望ましいので、図５に示す例においては、点Ｐｋでの曲率を点Ｐ_ｋの左側の曲率に合わせ、それをκ_０とする。同様にして、点Ｐ_ｋ＋１での曲率を右側に合わせ、それをκ_１とする。

次に、点Ｐ_ｋと点Ｐ_ｋ＋１との区間において、始点接線角（φ［ｋ］）、曲率（κ０）、終点接線角（φ［ｋ＋１］）、曲率（κ_１）の条件を満足する三連２次元クロソイド曲線を求める。この処理においては、演算処理の簡素化の観点から、点Ｐ_ｋを原点にし、Ｘ軸を点Ｐ_ｋから点Ｐ_ｋ＋１に向かうローカル座標系を採用する。

このローカル座標系において、曲線は原点（０，０）から出発し、点（ｒ，０）に終着する。そのため、三連２次元クロソイド曲線の算出は下記式（１０）を演算処理することに帰結する。

ここで、ｒは点Ｐ_ｋと点Ｐ_ｋ＋１との距離を示し、θ［ｋ］はｋ番目のローカル座標系のＸ軸がグローバル座標系のＸ軸に対してなす角を示す。

式（１０）および式（６）から、曲線の全パラメータａ_１０，ａ_１１，ａ_１２，ａ_２０，ａ_２１，ａ_２２，ａ_３０，ａ_３１，ａ_３２およびｈが得られる。

前記演算処理で得られた三連２次元クロソイド曲線のパラメータをグローバル座標系へ変換するには、ａ_１０，ａ_２０，ａ_３０のそれぞれにθ［ｋ］を加算すればよい。それ以外のパラメータは、ローカル座標系の値がそのまま使用できる。

図６に、指定された点列に対する三連２次元クロソイド曲線による補間例を単一の２次元クロソイド曲線による補間例と合わせて示す。

図示例より、単一の２次元クロソイド曲線による補間例では、通過点での曲率が連続していないが、三連２次元クロソイド曲線による補間例では通過点での曲率が連続しているのが理解される。

実施形態２
実施形態１では、弦ベクトル転向角の二分法で接線方向を指定するようにされているが、実施形態２では、端点での接線方向を接続したい直線の方向に一致させるものとされる。

端点での接線方向を接続したい直線の方向に一致させた上で、κ_０およびκ_１をゼロとして三連２次元クロソイド曲線による補間をなせば、直線との接続もなし得る。

図７に、このようにして補間された補間例を示す。

このように、実施形態２によれば、障害物を避けるような軌道設定が可能となる。

実施形態３
実施形態３は、実施形態１を改変してなるものであって、クロソイド曲線による補間法を３次元まで拡張してなるものとされる。

３次元クロソイド曲線による補間法については特許文献１に提案されているが、前述したように、特許文献１の提案に係る補間法では、各通過点において接線方向と曲率とを同時に指定することができない。

そのため、実施形態３では三連３次元クロソイド曲線による補間をなすようにしてなるものである。

なお、３次元クロソイド曲線は、研究段階のものであるため、種々の定義が存在するが、実施形態３では下記式（１１），（１２）による定義、いわゆる牧野定義を採用している。

ここで、αはピッチ角を示し、βはヨー角を示す。
式（１１）から、接線ベクトルｕは、単位ベクトルｉ（ｉ＝［１，０，０］^Ｔ）をまずｊ軸（Ｙ軸）回りに角αだけ回転させ、ついでｋ軸（Ｚ軸）回りに角βだけ回転させることにより得られることが理解される（図８参照）。

また、式（１１）から、３次元クロソイド曲線の曲率は下記式（１３）により与えられることが理解される。

式（１３）から理解されるように、ｄα／ｄＳおよびｄβ／ｄＳは曲線長のリニア関数であるが、一般的には、３次元クロソイド曲線の曲率は曲線長のリニア関数ではない。また、式（１３）の曲率は符号を持たない。曲線の曲がる方向は、ｄα／ｄＳおよびｄβ／ｄＳの符号により決定される。

図９に、ａ_０＝ｂ_０＝０，ａ_１＝ｂ_１＝ａ_２＝ｂ_２＝１，ｈ＝１とした場合の３次元クロソイド曲線の曲率と曲線長との関係を示す。
図９より、曲率は必ずしも曲線長のリニア関数とはならないが、曲率の変化は緩やかであるのが理解される。

２次元の座標系を任意に回転させると、曲線上の任意点の接線方向角はその回転角を加算するだけで求められる。これに対し、３次元の座標系を任意に回転させる場合、曲線上の任意点の接線方向のピッチ角およびヨー角は、２次元の場合のように簡単に求めることはできない。

例えば、単位接線ベクトルを座標系Ａのピッチ角およびヨー角で表わして式（１２）を満足するような曲線が存在すると仮定する。しかし、この同じ曲線を座標系Ｂのピッチ角およびヨー角で表わすと、一般的には、式（１２）のようなＳの二次関数では表せない。すなわち、ピッチ角およびヨー角を用いて補間する場合、得られる曲線は補間する座標系の選び方に依存する。

点列の各区間を同一のグローバル座標系のピッチ角およびヨー角により補間すると、ピッチ角およびヨー角の変動が大きくなるため、補間曲線には曲率の高い個所が生ずる場合がある。また、グローバル座標系の選び方によっても曲線は変化する。

かかる不具合を避けるため、つまりピッチ角およびヨー角の変動を小さくし、曲線の形が各点の相対位置のみに依存するようし、しかも曲率の小さな曲線となし得るようにするため、実施形態３では弦ベクトルをＸ軸とするローカル座標系が採用されている。

以下、かかるローカル座標系による３次元点列の補間について説明する。

より具体的には、点列を以下のように設定し、隣接する二点間Ｐ_ｋ，Ｐ_ｋ＋１間の補間について説明する。

Ｐ_０，Ｐ_１，Ｐ_２，・・・，Ｐ_Ｎ （Ｐはベクトルとする）
ローカル座標系は以下のように設定する。

原点:点Ｐ_ｋ
Ｘ軸：弦ベクトルＰ_ｋＰ_ｋ＋１の方向とする。
Ｙ軸：ベクトルＰ_ｋＰ_ｋ＋１とベクトルＰ_ｋ＋１Ｐ_ｋ＋２との外積ベクトルの方向とする。
Ｚ軸：Ｘ軸およびＹ軸がともに右手系となるように設定する。

なお、ベクトルＰ_ｋＰ_ｋ＋１とベクトルＰ_ｋ＋１Ｐ_ｋ＋２が同一直線上にある場合、ベクトルＰ_ｋＰ_ｋ＋１とベクトルＰ_ｋ＋１Ｐ_ｋ＋２の外積がゼロとなるので、Ｙ軸方向を決めるには、ベクトルＰ_ｋＰ_ｋ＋１とベクトルＰ_ｋ＋１Ｐ_ｋ＋２の外積の代わりにベクトルＰ_ｋＰ_ｋ＋１とベクトルＰ_ｋ＋１Ｐ_ｉの外積ベクトル方向を用いる。ここで、ｉは以下の条件を満たす最も若い数である。
（１）ｉ＝ｋ＋３，ｋ＋４，・・・，Ｎ
（２）ベクトルＰ_ｋ＋１Ｐ_ｉとベクトルＰ_ｋＰ_ｋ＋１は同一直線上にない

また、（１）のすべてのｉに対して、ベクトルＰ_ｋ＋１Ｐ_ｉとベクトルＰ_ｋＰ_ｋ＋１が同一直線上にある場合に、その前段（前区間）のローカル座標系のＹ軸方向を現段（現区間）のローカル座標系のＹ軸方向とする。同様に、ｋ＝Ｎ−１（最終区間）の場合は、前段（前区間ｋ＝Ｎ−２）のローカル座標系のＹ軸方向を現段（現区間）のローカル座標系のＹ軸方向とする。極端な例として、点列の全ての点が同一直線上にある場合に、補間結果はベクトルＰ_０Ｐ_Ｎ（線分Ｐ_０Ｐ_Ｎ）となり、ローカル座標系もそれ以後の演算も不要である。

補間曲線が通過する各点における接線方向は、前述した弦ベクトル転向角の二分法により決定する。

しかして、このようにして設定されたローカル座標系においては、補間曲線は原点（０，０，０）から出発し、点（ｒ，０，０）に終着するので、補間曲線は下記式（１４）を演算処理することにより算出される。

ここで、ｒは点Ｐ_ｋと点Ｐ_ｋ＋１との距離を示す。

なお、点Ｐ_ｋと点Ｐ_ｋ＋１とにおける接線姿勢角、すなわちα_ｋ，β_ｋ，α_ｋ＋１，β_ｋ＋１はローカル座標系の値を採用するものとする。

しかして、式（１４）および式（１２）から７つのパラメータ、すなわちａ_０，ａ_１，ａ_２，ｂ_０，ｂ_１，ｂ_２およびｈが得られる。ついで、これらのパラメータを用いてローカル座標系での補間曲線が得られる。しかる後、このローカル座標系での補間曲線をグローバル座標系に座標変換すれば、３次元クロソイド曲線による補間曲線が得られる。

図１０に、３次元クロソイド曲線による補間曲線の例を示す。

図１０のうち、同（ａ）における点列は立方体の頂点を示し、同（ｂ）における上方および下方のそれぞれの点列は正六角形の頂点を示す。

図１０より、得られた補間曲線は、各通過点においてその接線方向が連続であり、各区間の曲がり方も緩やかであるのが理解される。また、立方体のサイズが変化してもパラメータｈだけを調整すればよく、他のパラメータは調整する必要がないのが理解される。

実施形態５
実施形態５は実施形態２を３次元クロソイド曲線に拡張してなるものであって、三連３次元クロソイド曲線により補間をなすものである。

実施形態４では、つまり単一の３次元クロソイド曲線による補間では、各通過点での接触角は連続となるが、一般的には、各通過点での曲率は連続とはならない。そのため、実施形態５では、三連３次元クロソイド曲線により補間をなし、各通過点での曲率も常に連続させるものである。

実施形態５では、式（１２）が下記式（１５），（１６）に置き換えられる。

実施形態５においては、パラメータは曲線長ｈを含めて１９個あるが、つなぎ目の接触角と曲率の連続条件により、そのうちの８個は他のパラメータにより決定されるので、独立変数は１１個となり、実施形態４の場合より４個増加しているだけである。

実施形態５における各通過点における曲率の決定は、曲線の曲がる方向を考慮してなさなれる。実施形態２では、曲率の大きさと曲線の曲がる方向は、符号を有する曲率により決定することができたが、３次元クロソイド曲線の場合、曲がり方向が無限であるため、一つのスカラー変数のみで曲がり方向を決定することができない。そのため、実施形態５においては、各通過点の曲率はピッチ角（α）およびヨー角（β）の微分（ｄα／ｄｓ；ｄβ／ｄｓ）により間接的に決定するものとされている。つまり、下記式（１７）により決定するものとされている。

式（１７）から、曲率の絶対値のみならず、曲線の曲がる方向も決定されるのが理解される。

式（１７）を式（１３）に代入して下記式（１８）が得られる。

式（１８）から、κ_αとκ_βとが連続であれば、曲率κも連続となるのが理解される。このことから、各通過点においてκ_αとκ_βとが連続するような曲線を求めればよいのが理解される。

しかして、各通過点のκ_αとκ_βを得るには、まず単一の３次元クロソイド曲線で補間をしてその通過点両側のκ_αとκ_βを得、ついで絶対値の低い側のκ_αとκ_βをその通過点の目標値とする。その場合、κ_αとκ_βとは、補間する区間のローカル座標系に変換された値が用いられる。

次に、点Ｐ_ｋから点Ｐ_ｋ＋１までの区間について説明する。なお、座標系は前記と同様とする。

点Ｐｋの（κ_α，κ_β）の目標値を（κ_０α，κ_０β）とし、点Ｐ_ｋ＋１の（κ_α，κ_β）の目標値を（κ_１α，κ_１β）とする。このローカル座標系において、補間曲線は原点（０，０，０）から出発し、点（ｒ，０，０）に終着するので、補間曲線は下記式（１９）を演算処理することにより得られる。

ここで、ｒは点Ｐ_ｋから点Ｐ_ｋ＋１までの距離を示す。

なお、点Ｐ_ｋと点Ｐ_ｋ＋１とおける接線姿勢角α_ｋ，β_ｋ，α_ｋ＋１，β_ｋ＋１は、ローカル座標系での値を用いる。また、式（１９）の４番目以降の各式はピッチ角αおよびヨー角βの連続条件および曲率の連続条件を示す。

式（１９）、式（１５）および式（１６）から、三連３次元クロソイド曲線のパラメータａ_１０，ａ_１１，ａ_１２，ａ_２０，ａ_２１，ａ_２２，ａ_３０，ａ_３１，ａ_３２，ｂ_１０，ｂ_１１，ｂ_１２，ｂ_２０，ｂ_２１，ｂ_２２，ｂ_３０，ｂ_３１，ｂ_３２，およびｈを得る。

しかして、これらのパラメータを用いてローカル座標系での補間曲線を得、ついで得られた補間曲線の座標系をローカル座標系からグローバル座標系に変換することにより補間曲線が得られる。

式（１９）より明らかなように、三連３次元クロソイド曲線における独立変数は、単一の３次元クロソイド曲線における独立変数に比して４個増加しているので、各区間において接線方向だけでなく、（κ_０α，κ_０β）および（κ_１α，κ_１β）も指定できる。換言すれば、各通過点において曲率が連続な補間が可能となる。

図１１に、三連３次元クロソイド曲線による曲率が連続とされた補間例を、単一の３次元クロソイド曲線による補間例と併せて示す。なお、点列と図１０におけるものと同一とされている。また、Ｓ１は０．２５とされ、Ｓ２は０．７５とされている。

ここで、（κ_０α，κ_０β）＝（κ_１α，κ_１β）＝（０，０）として、三連３次元クロソイド曲線により補間すれば、異なる平面にある直線との３次元接続も容易になし得る。その場合、端点での接線方向は、接続する直線の方向に一致させればよい。

図１２に、三連３次元クロソイド曲線による直線との補間例を示す。

実施形態６
図１３に、本発明の実施形態６をブロック図で示す。

図１３に示すように、実施形態６は実施形態１を軌道生成装置Ａに改変してなるものとされる。

軌道生成装置Ａは、図１３に示すように、入力手段１と、演算処理装置２と、出力手段３とを主要構成要素として備えてなるものとされる。

入力手段１は、例えばキーボードとされるが、これに限定されるものではなく、コンピュータの入力装置として利用されている各種の入力装置を好適に用いることができる。

演算処理装置２は、クロソイド曲線生成手段１０と、座標変換手段２０と、補間指令値生成手段３０とを有するものとされる。なお、かかる手段１０，２０，３０を有する演算処理装置２は、具体的には、コンピュータに後述する機能を実施するためのプログラムを格納することにより実現される。

クロソイド曲線生成手段１０は、接線方向算出部１１と、単一の２次元クロソイド曲線生成部１２と、三連２次元クロソイド曲線生成手段部１３とを含むものとされ、入力手段１から入力された点列や接続する直線の方向に基づいて、式（１）〜式（８）および式（１０）を演算処理してローカル座標系における三連クロソイド曲線を生成するものとされる。

接線方向算出部１１は、弦ベクトル転向角の二分法により接線方向を算出するものとされる。なお、接線方向算出部１１を省略して入力手段１から接線方向が入力されるようにされてもよい。

座標変換手段２０は、クロソイド曲線生成手段１０により生成されたローカル座標系における２次元クロソイド曲線を、グローバル座標系における２次元クロソイド曲線に変換するものとされる。

補間指令値生成手段３０は、グローバル座標系に座標変換された三連２次元クロソイド曲線に基づいてロボットなどの移動体の補間指令値を生成するものとされる。なお、補間指令値生成手段３０としては、従来よりロボットに用いられているものを好適に用いることができる。

出力手段３は、例えば通信装置とされるが、これに限定されるものではなく、各種の出力手段を好適に用いることができ、例えばＣＤライタとすることもできる。

このように、実施形態６によれば、各通過点における接線方向および曲率を連続させることができるので、所要エネルギーが低減されてなる軌道を生成することができる。

実施形態７
図１４に、本発明の実施形態７をブロック図で示す。

図１４に示すように、実施形態７の軌道生成装置Ａ１は実施形態６の軌道生成装置Ａを改変してなるものとされる。すなわち、実施形態７はクロソイド曲線による補間を３次元まで拡張してなるものとされる。

以下、実施形態７の実施形態６と異なる点を主として説明する。

クロソイド曲線生成手段４０は、接線方向算出部４１と、単一の３次元クロソイド曲線生成部４２と、三連３次元クロソイド曲線生成手段部４３とを含むものとされ、入力手段１から入力された点列や接続する直線の方向に基づいて、式（１１）〜式（１９）を演算処理してローカル座標系における三連クロソイド曲線を生成するものとされる。

接線方向算出部４１は、弦ベクトル転向角の二分法により接線方向を算出するものとされる。なお、接線方向算出部４１を省略して入力手段１から接線方向が入力されるようにされてもよい。

座標変換手段２０は、クロソイド曲線生成手段４０により生成されたローカル座標系における３次元クロソイド曲線を、グローバル座標系における３次元クロソイド曲線に変換するものとされる。

補間指令値生成手段３０は、グローバル座標系に座標変換された三連３次元クロソイド曲線に基づいてロボットなどの移動体の補間指令値を生成するものとされる。なお、補間指令値生成手段３０としては、従来よりロボットに用いられているものを好適に用いることができる。

このように、実施形態７によれば、各通過点における接線方向および曲率を連続させることができるので、所要エネルギーが低減されてなる３次元軌道を生成することができる。

本発明は、ロボットなどの移動体の軌道生成に適用できる。

Ａ 軌道生成装置
１ 入力手段
２ 演算処理装置
３ 出力手段
１０ クロソイド曲線生成手段
１１ 接線方向算出部
１２ 単一の２次元クロソイド曲線生成部
１３ 三連２次元クロソイド曲線生成部
２０ 座標変換手段
３０ 補間指令値生成手段
４０ クロソイド曲線生成手段
４１ 接線方向算出部
４２ 単一の３次元クロソイド曲線生成部
４３ 三連３次元クロソイド曲線生成部

Claims (9)

1. 点列間をクロソイド曲線により補間して軌道生成をなす軌道生成方法であって、前記クロソイド曲線が三連クロソイド曲線とされ、それにより通過点での接線方向の連続性および曲率の連続性が担保されてなることを特徴とする軌道生成方法。
2. 通過点での接線方向が弦ベクトル転向角の二分法により算出されることを特徴とする請求項１記載の軌道生成方法。
3. ローカル座標系におけるクロソイド曲線を生成し、ついで生成されたクロソイド曲線をグローバル座標系に変換し、座標変換して得られたクロソイド曲線により補間をなすことを特徴とする請求項１記載の軌道生成方法。
4. 端点での接線方向を接続させる曲線と一致させるようにしてクロソイド曲線を生成することを特徴とする請求項１記載の軌道生成方法。
5. 点列間をクロソイド曲線により補間して軌道生成をなす軌道生成装置であって、
   入力手段と、演算処理装置と、出力手段とを備え、
   前記演算処理装置が、クロソイド曲線生成手段と、座標変換手段と、補間指令値生成手段とを有し、
   前記クロソイド曲線生成手段が、接線方向算出部と、単一の２次元クロソイド曲線生成部と、三連２次元クロソイド曲線生成部とを有することを特徴とする軌道生成装置。
6. 点列間をクロソイド曲線により補間して軌道生成をなす軌道生成装置であって、
   入力手段と、演算処理装置と、出力手段とを備え、
   前記演算処理装置が、クロソイド曲線生成手段と、座標変換手段と、補間指令値生成手段とを有し、
   前記クロソイド曲線生成手段が、接線方向算出部と、単一の３次元クロソイド曲線生成部と、三連３次元クロソイド曲線生成部とを有することを特徴とする軌道生成装置。
7. 請求項１ないし請求項４のいずれか一項に記載の軌道生成方法により生成された軌道により移動されることを特徴とする移動体。
8. 請求項５または請求項６に記載の軌道生成装置を備えてなることを特徴とする移動体。
9. 移動体がロボットとされてなることを特徴とする請求項７または８記載の移動体。