軌道生成方法および軌道生成装置
【課題】所要エネルギーの低減を図ることができる軌道生成方法および軌道生成装置を提供する。
【解決手段】点列間をクロソイド曲線により補間して軌道生成をなす軌道生成方法および軌道生成装置Ａであって、軌道生成装置Ａはクロソイド曲線生成手段１０を有する演算処理装置２を備え、前記クロソイド曲線が三連クロソイド曲線とされ、それにより通過点での接線方向の連続性および曲率の連続性が担保されてなるものである。なお、端点が直線と接続される場合、その接線方向は直線方向に一致させられる。
【解決手段】点列間をクロソイド曲線により補間して軌道生成をなす軌道生成方法および軌道生成装置Ａであって、軌道生成装置Ａはクロソイド曲線生成手段１０を有する演算処理装置２を備え、前記クロソイド曲線が三連クロソイド曲線とされ、それにより通過点での接線方向の連続性および曲率の連続性が担保されてなるものである。なお、端点が直線と接続される場合、その接線方向は直線方向に一致させられる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は軌道生成方法および軌道生成装置に関する。さらに詳しくは、所要エネルギーが低減されてなる軌道生成方法および軌道生成装置に関する。
【背景技術】
【０００２】
従来より、ロボットにおいては各種の補間法により軌道生成がなされてロボットの移動がなされている。
【０００３】
この補間法には、任意の点列を滑らかに移動するよう軌道生成をなすスプライン補間法が知られている。
【０００４】
しかしながら、スプライン補間法は、移動速度や加速度を一定とする制御には不向きであるという欠点を有している。
【０００５】
かかるスプライン補間法の欠点を解消すべく特許文献１には、接線方向のピッチ角およびヨー角のそれぞれが曲線長軌跡またはワークの輪郭形状を表現し、この３次元曲線によって工具の運動を制御する数値制御方法および装置が提案されている。
【０００６】
しかしながら、特許文献１におる制御方法は、曲率および接線方向を同時に指定なし得ないという欠点を有しているので、所要エネルギーの低減を図るのが困難であるという問題を有している。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００５−２７６１５５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
本発明はかかる従来技術の課題に鑑みなされたものであって、所要エネルギーの低減を図ることができる軌道生成方法および軌道生成装置を提供することを目的としている。
【課題を解決するための手段】
【０００９】
本発明の軌道生成方法は、点列間をクロソイド曲線により補間して軌道生成をなす軌道生成方法であって、前記クロソイド曲線が三連クロソイド曲線とされ、それにより通過点での接線方向の連続性および曲率の連続性が担保されてなることを特徴とする。
【００１０】
本発明の軌道生成方法においては、通過点での接線方向が弦ベクトル転向角の二分法により算出されるのが好ましい。
【００１１】
また、本発明の軌道生成方法においては、ローカル座標系におけるクロソイド曲線を生成し、ついで生成されたクロソイド曲線をグローバル座標系に変換し、座標変換して得られたクロソイド曲線により補間をなすのが好ましい。
【００１２】
さらに、本発明の軌道生成方法においては、端点での接線方向を接続させる曲線と一致させるようにしてクロソイド曲線を生成するのが好ましい。
【００１３】
一方、本発明の軌道生成装置は、点列間をクロソイド曲線により補間して軌道生成をなす軌道生成装置であって、入力手段と、演算処理装置と、出力手段とを備え、前記演算処理装置が、クロソイド曲線生成手段と、座標変換手段と、補間指令値生成手段とを有してなることを特徴とする。
【００１４】
本発明の軌道生成装置の第１形態においては、クロソイド曲線生成手段が、接線方向算出部と、単一の２次元クロソイド曲線生成部と、三連２次元クロソイド曲線生成部とを有するものとされる。
【００１５】
また、本発明の軌道生成装置の第２形態においては、クロソイド曲線生成手段が、接線方向算出部と、単一の３次元クロソイド曲線生成部と、三連３次元クロソイド曲線生成部とを有するものとされる。
【００１６】
しかして、本発明の軌道生成方法により生成された軌道はロボットなどの移動体の移動に適用される。
【００１７】
また、本発明の軌道生成装置は、ロボットなどの移動体に備えられる。
【発明の効果】
【００１８】
本発明は前記の如く構成されているので、所要エネルギーが低減される軌道の生成がなし得るという優れた効果が得られる。
【図面の簡単な説明】
【００１９】
【図１】クロソイド曲線の一例を示すグラフ図である。
【図２】ローカル座標系における二点の座標および相対角を示す説明図である。
【図３】２次元クロソイド曲線による補間曲線の例を示す例示図である。
【図４】弦ベクトル転向角の二分法の説明図である。
【図５】単一の２次元クロソイド曲線による補間例の例示図である。
【図６】三連２次元クロソイド曲線による補間例の例示図である。
【図７】三連２次元クロソイド曲線による直線との補間例の例示図である。
【図８】３次元クロソイド曲線の接線ベクトルの説明図である。
【図９】３次元クロソイド曲線の曲率の変化の例を示す例示図である。
【図１０】３次元クロソイド曲線による補間曲線例を示す例示図である。
【図１１】三連３次元クロソイド曲線による補間曲線例を示す例示図である。
【図１２】三連３次元クロソイド曲線による直線との補間例を示す例示図である。
【図１３】本発明の一実施形態に係る軌道生成装置のブロック図である。
【図１４】本発明の他の実施形態に係る軌道生成装置のブロック図である。
【発明を実施するための形態】
【００２０】
以下、添付図面を参照しながら本発明を実施形態に基づいて説明するが、本発明はかかる実施形態のみに限定されるものではない。
【００２１】
実施形態１
本発明は、クロソイド曲線を利用して軌道の生成をなすものである。
【００２２】
本発明の理解を容易にするため、まず２次元クロソイド曲線について説明する。
【００２３】
一般的に、任意の連続曲線セグメントは以下の式（１）により表わされる。
【００２４】
【数１】
【００２５】
ここで、ベクトルＰ０は曲線の始点（ベクトル）を示し、ベクトルｕは曲線上の任意の単位接線ベクトルを示し、ｓは始点からの曲線長を示し、ｈは曲線の全長を示し、Ｓ（０≦Ｓ≦１）は無次元の曲線長を示す。
【００２６】
いま、ベクトルｕをＳの関数として定義すれば、それに対応する曲線が求められる。
【００２７】
複素平面において、２次元クロソイド曲線の単位接線ベクトル（ベクトルｕ）は以下の式（２）により与えられる。
【００２８】
【数２】
【００２９】
ここで、φは曲線上の任意点の接線方向を示し、φ０は始点における接線方向を示し、φｖは接線方向の１次増分係数を示し、φｕは接線方向の２次増分係数を示す。
【００３０】
式（１）と式（２）による積分は一般に初等関数で表わせないことが知られており、数値積分などの数値法で演算する必要がある。
【００３１】
図１に、数値積分により得られた２次元クロソイド曲線の一例を示す。
【００３２】
φをｓで微分すると２次元クロソイド曲線（単一の２次元クロソイド曲線）の曲率が得られる。すなわち、下記式（３）が得られる。
【００３３】
【数３】
【００３４】
式（３）により、得られた曲率が符号を持つことが理解される。つまり、曲線が時計方向に曲がるならば負、反時計方向に曲がるならば正となる。また、式（３）により、得られた曲率は曲線長のリニア関数であることが理解される。
【００３５】
次に、図２を参照しながら平面上の任意の２点Ｐ０と点Ｐ１との間の補間について説明する。
【００３６】
点Ｐ０の接線角Ψ０とし、点Ｐ１での接線角Ψ１とする。
【００３７】
演算処理を簡素化するため、点Ｐ０を原点とし、Ｘ軸を点Ｐ０から点Ｐ１に向かう方向に取ったローカル座標系を採用する。
【００３８】
このローカル座標系においては、曲線は原点（０，０）から出発し、点（ｒ，０）に終着するので、曲線は以下の式（４）により得られる。
【００３９】
【数４】
【００４０】
式（４）と式（２）の二番目の式から各パラメータφ０、φｖ、φｕおよびｈが求められる。
【００４１】
このようにして得られた各パラメータをグローバル座標系の値に変換するには、φ０にθを加算すればよい。それ以外のパラメータは、ローカル座標系の値がそのまま使用できる。
【００４２】
図３に、θを３０度とし、φ１を−３０度として、φ０を−１８０度〜１８０度において１０度ずつ変化させた場合における軌跡の生成例を示す。
【００４３】
ここで、前記補間法を「曲線端点の接線方向指定法」と定義することにする。
【００４４】
この曲線端点の接線方向指定法は、本願発明の基礎となる。
【００４５】
ちなみに、ロボットの移動経路を生成する補間法においては、加速度が不連続になるとロボットの動作が不安定となるため、加速度の連続性が要求される。一方、曲率が連続していない軌道上における移動は加速度が不連続となる。そのため、ロボットなどの移動経路のための補間法においては、曲率の連続性が要求される。
【００４６】
しかるに、前記曲線端点の接線方向指定法においては、通過点での接線角を指定することで、２次元クロソイド曲線の全パラメータφ０、φｖ、φｕおよびｈを決定するため、通過点での曲率を指定することができない。通過点での接線角および曲率の両者を同時に指定するには、クロソイド曲線のパラメータを増やす必要がある。その場合、リニアな曲率を維持したところから、Ｓの３次項，Ｓの４次項などの高次項が含まれないようにしてパラメータを設ける必要がある。
【００４７】
この高次項が含まれないようにしてパラメータを決定する手法を以下に説明する。この手法をここでは三連２次元クロソイド曲線による補間法と定義することにする。
【００４８】
三連２次元クロソイド曲線の単位接線ベクトルｕを式（５），式（６）および式（７）により定義する。
【００４９】
【数５】
【００５０】
【数６】
【００５１】
【数７】
【００５２】
ここで、無次元の定数Ｓ１およびＳ２（０<Ｓ１<Ｓ２<１）は三連２次元クロソイド曲線のつなぎ目の位置を示す。
なお、無次元の定数Ｓ１およびＳ２は、任意に指定できるが、対称性を保ちたい場合には、Ｓ１＝γ（０<γ<０．５）と指定し、Ｓ２＝１−γと指定すればよい。本発明者等は、γ＝０．２５を推奨値、つまりＳ１＝０．２５およびＳ２＝０．７５を推奨値としている。
【００５３】
また、ａ１０，ａ１１，ａ１２，ａ２０，ａ２１，ａ２２，ａ３０，ａ３１，ａ３２およびｈは三連２次元クロソイド曲線のパラメータである。これらのパラメータは全部で１０個あるが、式（７）により、そのうち４個は他のパラメータにより決定されるため、独立のパラメータは６個である。
【００５４】
また、式（７）の第１式および第２式は、Ｓ１およびＳ２での接線方向の連続条件を示し、式（７）の第３式および第４式は、Ｓ１およびＳ２での曲率の連続条件を示す。
【００５５】
式（５）〜式（７）および式（１）より、三連２次元クロソイド曲線の曲率は下記式（８）により与えられる。
【００５６】
【数８】
【００５７】
この式（８）により得られた曲率をＳに対して描くと３つの連続した線分になる。
【００５８】
また、三連２次元クロソイド曲線の特性は、単一の２次元クロソイド曲線の特性に似ていて直線および円の表現も可能であるが、それに加えてゼロの曲率の位置を３つまで持つことができる。なお、単一の２次元クロソイド曲線はゼロの曲率の位置を１つしか持てない。
【００５９】
次に、この三連２次元クロソイド曲線による点列の補間について説明する。
【００６０】
補間したい自由点列（点数:Ｎ＋１）を下記（９）とする。
【００６１】
Ｐ０，Ｐ１，Ｐ２，・・・，ＰＮ （Ｐはベクトルとする） （９）
【００６２】
ここで、これらの各点での接線方向を、図４に示すように、弦ベクトル転向角の二分法により決定する。
【００６３】
図４において、ベクトルＡＢは、Ｐ０ＣとＰ１Ｐ２との間の角を二等分し、∠ＡＰ０Ｐ１＝∠ＡＰ１Ｐ０とする。Ｐ０での接線方向をＰ０Ａとし、Ｐ１での接線方向をＰ１Ｂとする。ついで、点Ｐ２での接線方向Ｐ２Ｅを図４（ｂ）のように定める。同様にして他の点Ｐ３，Ｐ４，・・・，ＰＮでの接線方向も定める。
【００６４】
自由点列Ｐ０，Ｐ１，Ｐ２，・・・，ＰＮに対して前述したように、弦ベクトル転向角の二分法により接線方向を指定し、単一の２次元クロソイド曲線により補間すると、得られた補間曲線は各通過点での接線方向は連続になるが、一般的には、各通過点での曲率は連続とはならない。
【００６５】
図５、このようにして補間された通過点Ｐｋ前後の軌道を点線で示す。
【００６６】
そこで、本発明では、三連２次元クロソイド曲線による点列の補間を行う。
【００６７】
例として、点Ｐｋと点Ｐｋ＋１との区間を三連２次元クロソイド曲線により補間し、ついで点Ｐｋおよび点Ｐｋ＋１の接線方向を変えずに曲率を連続させる場合について説明する（図５参照）。なお、図５中、実線は目標曲線を示す。
【００６８】
軌道の生成においては各点における曲率を同一とする場合、低い方の曲率に他方の曲率を合わせるのが望ましいので、図５に示す例においては、点Ｐｋでの曲率を点Ｐｋの左側の曲率に合わせ、それをκ０とする。同様にして、点Ｐｋ＋１での曲率を右側に合わせ、それをκ１とする。
【００６９】
次に、点Ｐｋと点Ｐｋ＋１との区間において、始点接線角（φ［ｋ］）、曲率（κ０）、終点接線角（φ［ｋ＋１］）、曲率（κ１）の条件を満足する三連２次元クロソイド曲線を求める。この処理においては、演算処理の簡素化の観点から、点Ｐｋを原点にし、Ｘ軸を点Ｐｋから点Ｐｋ＋１に向かうローカル座標系を採用する。
【００７０】
このローカル座標系において、曲線は原点（０，０）から出発し、点（ｒ，０）に終着する。そのため、三連２次元クロソイド曲線の算出は下記式（１０）を演算処理することに帰結する。
【００７１】
【数１０】
【００７２】
ここで、ｒは点Ｐｋと点Ｐｋ＋１との距離を示し、θ［ｋ］はｋ番目のローカル座標系のＸ軸がグローバル座標系のＸ軸に対してなす角を示す。
【００７３】
式（１０）および式（６）から、曲線の全パラメータａ１０，ａ１１，ａ１２，ａ２０，ａ２１，ａ２２，ａ３０，ａ３１，ａ３２およびｈが得られる。
【００７４】
前記演算処理で得られた三連２次元クロソイド曲線のパラメータをグローバル座標系へ変換するには、ａ１０，ａ２０，ａ３０のそれぞれにθ［ｋ］を加算すればよい。それ以外のパラメータは、ローカル座標系の値がそのまま使用できる。
【００７５】
図６に、指定された点列に対する三連２次元クロソイド曲線による補間例を単一の２次元クロソイド曲線による補間例と合わせて示す。
【００７６】
図示例より、単一の２次元クロソイド曲線による補間例では、通過点での曲率が連続していないが、三連２次元クロソイド曲線による補間例では通過点での曲率が連続しているのが理解される。
【００７７】
実施形態２
実施形態１では、弦ベクトル転向角の二分法で接線方向を指定するようにされているが、実施形態２では、端点での接線方向を接続したい直線の方向に一致させるものとされる。
【００７８】
端点での接線方向を接続したい直線の方向に一致させた上で、κ０およびκ１をゼロとして三連２次元クロソイド曲線による補間をなせば、直線との接続もなし得る。
【００７９】
図７に、このようにして補間された補間例を示す。
【００８０】
このように、実施形態２によれば、障害物を避けるような軌道設定が可能となる。
【００８１】
実施形態３
実施形態３は、実施形態１を改変してなるものであって、クロソイド曲線による補間法を３次元まで拡張してなるものとされる。
【００８２】
３次元クロソイド曲線による補間法については特許文献１に提案されているが、前述したように、特許文献１の提案に係る補間法では、各通過点において接線方向と曲率とを同時に指定することができない。
【００８３】
そのため、実施形態３では三連３次元クロソイド曲線による補間をなすようにしてなるものである。
【００８４】
なお、３次元クロソイド曲線は、研究段階のものであるため、種々の定義が存在するが、実施形態３では下記式（１１），（１２）による定義、いわゆる牧野定義を採用している。
【００８５】
【数１１】
【００８６】
【数１２】
【００８７】
ここで、αはピッチ角を示し、βはヨー角を示す。
式（１１）から、接線ベクトルｕは、単位ベクトルｉ（ｉ＝［１，０，０］Ｔ）をまずｊ軸（Ｙ軸）回りに角αだけ回転させ、ついでｋ軸（Ｚ軸）回りに角βだけ回転させることにより得られることが理解される（図８参照）。
【００８８】
また、式（１１）から、３次元クロソイド曲線の曲率は下記式（１３）により与えられることが理解される。
【００８９】
【数１３】
【００９０】
式（１３）から理解されるように、ｄα／ｄＳおよびｄβ／ｄＳは曲線長のリニア関数であるが、一般的には、３次元クロソイド曲線の曲率は曲線長のリニア関数ではない。また、式（１３）の曲率は符号を持たない。曲線の曲がる方向は、ｄα／ｄＳおよびｄβ／ｄＳの符号により決定される。
【００９１】
図９に、ａ０＝ｂ０＝０，ａ１＝ｂ１＝ａ２＝ｂ２＝１，ｈ＝１とした場合の３次元クロソイド曲線の曲率と曲線長との関係を示す。
図９より、曲率は必ずしも曲線長のリニア関数とはならないが、曲率の変化は緩やかであるのが理解される。
【００９２】
２次元の座標系を任意に回転させると、曲線上の任意点の接線方向角はその回転角を加算するだけで求められる。これに対し、３次元の座標系を任意に回転させる場合、曲線上の任意点の接線方向のピッチ角およびヨー角は、２次元の場合のように簡単に求めることはできない。
【００９３】
例えば、単位接線ベクトルを座標系Ａのピッチ角およびヨー角で表わして式（１２）を満足するような曲線が存在すると仮定する。しかし、この同じ曲線を座標系Ｂのピッチ角およびヨー角で表わすと、一般的には、式（１２）のようなＳの二次関数では表せない。すなわち、ピッチ角およびヨー角を用いて補間する場合、得られる曲線は補間する座標系の選び方に依存する。
【００９４】
点列の各区間を同一のグローバル座標系のピッチ角およびヨー角により補間すると、ピッチ角およびヨー角の変動が大きくなるため、補間曲線には曲率の高い個所が生ずる場合がある。また、グローバル座標系の選び方によっても曲線は変化する。
【００９５】
かかる不具合を避けるため、つまりピッチ角およびヨー角の変動を小さくし、曲線の形が各点の相対位置のみに依存するようし、しかも曲率の小さな曲線となし得るようにするため、実施形態３では弦ベクトルをＸ軸とするローカル座標系が採用されている。
【００９６】
以下、かかるローカル座標系による３次元点列の補間について説明する。
【００９７】
より具体的には、点列を以下のように設定し、隣接する二点間Ｐｋ，Ｐｋ＋１間の補間について説明する。
【００９８】
Ｐ０，Ｐ１，Ｐ２，・・・，ＰＮ （Ｐはベクトルとする）
ローカル座標系は以下のように設定する。
【００９９】
原点:点Ｐｋ
Ｘ軸：弦ベクトルＰｋＰｋ＋１の方向とする。
Ｙ軸：ベクトルＰｋＰｋ＋１とベクトルＰｋ＋１Ｐｋ＋２との外積ベクトルの方向とする。
Ｚ軸：Ｘ軸およびＹ軸がともに右手系となるように設定する。
【０１００】
なお、ベクトルＰｋＰｋ＋１とベクトルＰｋ＋１Ｐｋ＋２が同一直線上にある場合、ベクトルＰｋＰｋ＋１とベクトルＰｋ＋１Ｐｋ＋２の外積がゼロとなるので、Ｙ軸方向を決めるには、ベクトルＰｋＰｋ＋１とベクトルＰｋ＋１Ｐｋ＋２の外積の代わりにベクトルＰｋＰｋ＋１とベクトルＰｋ＋１Ｐｉの外積ベクトル方向を用いる。ここで、ｉは以下の条件を満たす最も若い数である。
（１）ｉ＝ｋ＋３，ｋ＋４，・・・，Ｎ
（２）ベクトルＰｋ＋１ＰｉとベクトルＰｋＰｋ＋１は同一直線上にない
【０１０１】
また、（１）のすべてのｉに対して、ベクトルＰｋ＋１ＰｉとベクトルＰｋＰｋ＋１が同一直線上にある場合に、その前段（前区間）のローカル座標系のＹ軸方向を現段（現区間）のローカル座標系のＹ軸方向とする。同様に、ｋ＝Ｎ−１（最終区間）の場合は、前段（前区間ｋ＝Ｎ−２）のローカル座標系のＹ軸方向を現段（現区間）のローカル座標系のＹ軸方向とする。極端な例として、点列の全ての点が同一直線上にある場合に、補間結果はベクトルＰ０ＰＮ（線分Ｐ０ＰＮ）となり、ローカル座標系もそれ以後の演算も不要である。
【０１０２】
補間曲線が通過する各点における接線方向は、前述した弦ベクトル転向角の二分法により決定する。
【０１０３】
しかして、このようにして設定されたローカル座標系においては、補間曲線は原点（０，０，０）から出発し、点（ｒ，０，０）に終着するので、補間曲線は下記式（１４）を演算処理することにより算出される。
【０１０４】
【数１４】
【０１０５】
ここで、ｒは点Ｐｋと点Ｐｋ＋１との距離を示す。
【０１０６】
なお、点Ｐｋと点Ｐｋ＋１とにおける接線姿勢角、すなわちαｋ，βｋ，αｋ＋１，βｋ＋１はローカル座標系の値を採用するものとする。
【０１０７】
しかして、式（１４）および式（１２）から７つのパラメータ、すなわちａ０，ａ１，ａ２，ｂ０，ｂ１，ｂ２およびｈが得られる。ついで、これらのパラメータを用いてローカル座標系での補間曲線が得られる。しかる後、このローカル座標系での補間曲線をグローバル座標系に座標変換すれば、３次元クロソイド曲線による補間曲線が得られる。
【０１０８】
図１０に、３次元クロソイド曲線による補間曲線の例を示す。
【０１０９】
図１０のうち、同（ａ）における点列は立方体の頂点を示し、同（ｂ）における上方および下方のそれぞれの点列は正六角形の頂点を示す。
【０１１０】
図１０より、得られた補間曲線は、各通過点においてその接線方向が連続であり、各区間の曲がり方も緩やかであるのが理解される。また、立方体のサイズが変化してもパラメータｈだけを調整すればよく、他のパラメータは調整する必要がないのが理解される。
【０１１１】
実施形態５
実施形態５は実施形態２を３次元クロソイド曲線に拡張してなるものであって、三連３次元クロソイド曲線により補間をなすものである。
【０１１２】
実施形態４では、つまり単一の３次元クロソイド曲線による補間では、各通過点での接触角は連続となるが、一般的には、各通過点での曲率は連続とはならない。そのため、実施形態５では、三連３次元クロソイド曲線により補間をなし、各通過点での曲率も常に連続させるものである。
【０１１３】
実施形態５では、式（１２）が下記式（１５），（１６）に置き換えられる。
【０１１４】
【数１５】
【０１１５】
【数１６】
【０１１６】
実施形態５においては、パラメータは曲線長ｈを含めて１９個あるが、つなぎ目の接触角と曲率の連続条件により、そのうちの８個は他のパラメータにより決定されるので、独立変数は１１個となり、実施形態４の場合より４個増加しているだけである。
【０１１７】
実施形態５における各通過点における曲率の決定は、曲線の曲がる方向を考慮してなさなれる。実施形態２では、曲率の大きさと曲線の曲がる方向は、符号を有する曲率により決定することができたが、３次元クロソイド曲線の場合、曲がり方向が無限であるため、一つのスカラー変数のみで曲がり方向を決定することができない。そのため、実施形態５においては、各通過点の曲率はピッチ角（α）およびヨー角（β）の微分（ｄα／ｄｓ；ｄβ／ｄｓ）により間接的に決定するものとされている。つまり、下記式（１７）により決定するものとされている。
【０１１８】
【数１７】
【０１１９】
式（１７）から、曲率の絶対値のみならず、曲線の曲がる方向も決定されるのが理解される。
【０１２０】
式（１７）を式（１３）に代入して下記式（１８）が得られる。
【０１２１】
【数１８】
【０１２２】
式（１８）から、καとκβとが連続であれば、曲率κも連続となるのが理解される。このことから、各通過点においてκαとκβとが連続するような曲線を求めればよいのが理解される。
【０１２３】
しかして、各通過点のκαとκβを得るには、まず単一の３次元クロソイド曲線で補間をしてその通過点両側のκαとκβを得、ついで絶対値の低い側のκαとκβをその通過点の目標値とする。その場合、καとκβとは、補間する区間のローカル座標系に変換された値が用いられる。
【０１２４】
次に、点Ｐｋから点Ｐｋ＋１までの区間について説明する。なお、座標系は前記と同様とする。
【０１２５】
点Ｐｋの（κα，κβ）の目標値を（κ０α，κ０β）とし、点Ｐｋ＋１の（κα，κβ）の目標値を（κ１α，κ１β）とする。このローカル座標系において、補間曲線は原点（０，０，０）から出発し、点（ｒ，０，０）に終着するので、補間曲線は下記式（１９）を演算処理することにより得られる。
【０１２６】
【数１９】
【０１２７】
ここで、ｒは点Ｐｋから点Ｐｋ＋１までの距離を示す。
【０１２８】
なお、点Ｐｋと点Ｐｋ＋１とおける接線姿勢角αｋ，βｋ，αｋ＋１，βｋ＋１は、ローカル座標系での値を用いる。また、式（１９）の４番目以降の各式はピッチ角αおよびヨー角βの連続条件および曲率の連続条件を示す。
【０１２９】
式（１９）、式（１５）および式（１６）から、三連３次元クロソイド曲線のパラメータａ１０，ａ１１，ａ１２，ａ２０，ａ２１，ａ２２，ａ３０，ａ３１，ａ３２，ｂ１０，ｂ１１，ｂ１２，ｂ２０，ｂ２１，ｂ２２，ｂ３０，ｂ３１，ｂ３２，およびｈを得る。
【０１３０】
しかして、これらのパラメータを用いてローカル座標系での補間曲線を得、ついで得られた補間曲線の座標系をローカル座標系からグローバル座標系に変換することにより補間曲線が得られる。
【０１３１】
式（１９）より明らかなように、三連３次元クロソイド曲線における独立変数は、単一の３次元クロソイド曲線における独立変数に比して４個増加しているので、各区間において接線方向だけでなく、（κ０α，κ０β）および（κ１α，κ１β）も指定できる。換言すれば、各通過点において曲率が連続な補間が可能となる。
【０１３２】
図１１に、三連３次元クロソイド曲線による曲率が連続とされた補間例を、単一の３次元クロソイド曲線による補間例と併せて示す。なお、点列と図１０におけるものと同一とされている。また、Ｓ１は０．２５とされ、Ｓ２は０．７５とされている。
【０１３３】
ここで、（κ０α，κ０β）＝（κ１α，κ１β）＝（０，０）として、三連３次元クロソイド曲線により補間すれば、異なる平面にある直線との３次元接続も容易になし得る。その場合、端点での接線方向は、接続する直線の方向に一致させればよい。
【０１３４】
図１２に、三連３次元クロソイド曲線による直線との補間例を示す。
【０１３５】
実施形態６
図１３に、本発明の実施形態６をブロック図で示す。
【０１３６】
図１３に示すように、実施形態６は実施形態１を軌道生成装置Ａに改変してなるものとされる。
【０１３７】
軌道生成装置Ａは、図１３に示すように、入力手段１と、演算処理装置２と、出力手段３とを主要構成要素として備えてなるものとされる。
【０１３８】
入力手段１は、例えばキーボードとされるが、これに限定されるものではなく、コンピュータの入力装置として利用されている各種の入力装置を好適に用いることができる。
【０１３９】
演算処理装置２は、クロソイド曲線生成手段１０と、座標変換手段２０と、補間指令値生成手段３０とを有するものとされる。なお、かかる手段１０，２０，３０を有する演算処理装置２は、具体的には、コンピュータに後述する機能を実施するためのプログラムを格納することにより実現される。
【０１４０】
クロソイド曲線生成手段１０は、接線方向算出部１１と、単一の２次元クロソイド曲線生成部１２と、三連２次元クロソイド曲線生成手段部１３とを含むものとされ、入力手段１から入力された点列や接続する直線の方向に基づいて、式（１）〜式（８）および式（１０）を演算処理してローカル座標系における三連クロソイド曲線を生成するものとされる。
【０１４１】
接線方向算出部１１は、弦ベクトル転向角の二分法により接線方向を算出するものとされる。なお、接線方向算出部１１を省略して入力手段１から接線方向が入力されるようにされてもよい。
【０１４２】
座標変換手段２０は、クロソイド曲線生成手段１０により生成されたローカル座標系における２次元クロソイド曲線を、グローバル座標系における２次元クロソイド曲線に変換するものとされる。
【０１４３】
補間指令値生成手段３０は、グローバル座標系に座標変換された三連２次元クロソイド曲線に基づいてロボットなどの移動体の補間指令値を生成するものとされる。なお、補間指令値生成手段３０としては、従来よりロボットに用いられているものを好適に用いることができる。
【０１４４】
出力手段３は、例えば通信装置とされるが、これに限定されるものではなく、各種の出力手段を好適に用いることができ、例えばＣＤライタとすることもできる。
【０１４５】
このように、実施形態６によれば、各通過点における接線方向および曲率を連続させることができるので、所要エネルギーが低減されてなる軌道を生成することができる。
【０１４６】
実施形態７
図１４に、本発明の実施形態７をブロック図で示す。
【０１４７】
図１４に示すように、実施形態７の軌道生成装置Ａ１は実施形態６の軌道生成装置Ａを改変してなるものとされる。すなわち、実施形態７はクロソイド曲線による補間を３次元まで拡張してなるものとされる。
【０１４８】
以下、実施形態７の実施形態６と異なる点を主として説明する。
【０１４９】
クロソイド曲線生成手段４０は、接線方向算出部４１と、単一の３次元クロソイド曲線生成部４２と、三連３次元クロソイド曲線生成手段部４３とを含むものとされ、入力手段１から入力された点列や接続する直線の方向に基づいて、式（１１）〜式（１９）を演算処理してローカル座標系における三連クロソイド曲線を生成するものとされる。
【０１５０】
接線方向算出部４１は、弦ベクトル転向角の二分法により接線方向を算出するものとされる。なお、接線方向算出部４１を省略して入力手段１から接線方向が入力されるようにされてもよい。
【０１５１】
座標変換手段２０は、クロソイド曲線生成手段４０により生成されたローカル座標系における３次元クロソイド曲線を、グローバル座標系における３次元クロソイド曲線に変換するものとされる。
【０１５２】
補間指令値生成手段３０は、グローバル座標系に座標変換された三連３次元クロソイド曲線に基づいてロボットなどの移動体の補間指令値を生成するものとされる。なお、補間指令値生成手段３０としては、従来よりロボットに用いられているものを好適に用いることができる。
【０１５３】
このように、実施形態７によれば、各通過点における接線方向および曲率を連続させることができるので、所要エネルギーが低減されてなる３次元軌道を生成することができる。
【産業上の利用可能性】
【０１５４】
本発明は、ロボットなどの移動体の軌道生成に適用できる。
【符号の説明】
【０１５５】
Ａ 軌道生成装置
１ 入力手段
２ 演算処理装置
３ 出力手段
１０ クロソイド曲線生成手段
１１ 接線方向算出部
１２ 単一の２次元クロソイド曲線生成部
１３ 三連２次元クロソイド曲線生成部
２０ 座標変換手段
３０ 補間指令値生成手段
４０ クロソイド曲線生成手段
４１ 接線方向算出部
４２ 単一の３次元クロソイド曲線生成部
４３ 三連３次元クロソイド曲線生成部
【技術分野】
【０００１】
本発明は軌道生成方法および軌道生成装置に関する。さらに詳しくは、所要エネルギーが低減されてなる軌道生成方法および軌道生成装置に関する。
【背景技術】
【０００２】
従来より、ロボットにおいては各種の補間法により軌道生成がなされてロボットの移動がなされている。
【０００３】
この補間法には、任意の点列を滑らかに移動するよう軌道生成をなすスプライン補間法が知られている。
【０００４】
しかしながら、スプライン補間法は、移動速度や加速度を一定とする制御には不向きであるという欠点を有している。
【０００５】
かかるスプライン補間法の欠点を解消すべく特許文献１には、接線方向のピッチ角およびヨー角のそれぞれが曲線長軌跡またはワークの輪郭形状を表現し、この３次元曲線によって工具の運動を制御する数値制御方法および装置が提案されている。
【０００６】
しかしながら、特許文献１におる制御方法は、曲率および接線方向を同時に指定なし得ないという欠点を有しているので、所要エネルギーの低減を図るのが困難であるという問題を有している。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００５−２７６１５５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
本発明はかかる従来技術の課題に鑑みなされたものであって、所要エネルギーの低減を図ることができる軌道生成方法および軌道生成装置を提供することを目的としている。
【課題を解決するための手段】
【０００９】
本発明の軌道生成方法は、点列間をクロソイド曲線により補間して軌道生成をなす軌道生成方法であって、前記クロソイド曲線が三連クロソイド曲線とされ、それにより通過点での接線方向の連続性および曲率の連続性が担保されてなることを特徴とする。
【００１０】
本発明の軌道生成方法においては、通過点での接線方向が弦ベクトル転向角の二分法により算出されるのが好ましい。
【００１１】
また、本発明の軌道生成方法においては、ローカル座標系におけるクロソイド曲線を生成し、ついで生成されたクロソイド曲線をグローバル座標系に変換し、座標変換して得られたクロソイド曲線により補間をなすのが好ましい。
【００１２】
さらに、本発明の軌道生成方法においては、端点での接線方向を接続させる曲線と一致させるようにしてクロソイド曲線を生成するのが好ましい。
【００１３】
一方、本発明の軌道生成装置は、点列間をクロソイド曲線により補間して軌道生成をなす軌道生成装置であって、入力手段と、演算処理装置と、出力手段とを備え、前記演算処理装置が、クロソイド曲線生成手段と、座標変換手段と、補間指令値生成手段とを有してなることを特徴とする。
【００１４】
本発明の軌道生成装置の第１形態においては、クロソイド曲線生成手段が、接線方向算出部と、単一の２次元クロソイド曲線生成部と、三連２次元クロソイド曲線生成部とを有するものとされる。
【００１５】
また、本発明の軌道生成装置の第２形態においては、クロソイド曲線生成手段が、接線方向算出部と、単一の３次元クロソイド曲線生成部と、三連３次元クロソイド曲線生成部とを有するものとされる。
【００１６】
しかして、本発明の軌道生成方法により生成された軌道はロボットなどの移動体の移動に適用される。
【００１７】
また、本発明の軌道生成装置は、ロボットなどの移動体に備えられる。
【発明の効果】
【００１８】
本発明は前記の如く構成されているので、所要エネルギーが低減される軌道の生成がなし得るという優れた効果が得られる。
【図面の簡単な説明】
【００１９】
【図１】クロソイド曲線の一例を示すグラフ図である。
【図２】ローカル座標系における二点の座標および相対角を示す説明図である。
【図３】２次元クロソイド曲線による補間曲線の例を示す例示図である。
【図４】弦ベクトル転向角の二分法の説明図である。
【図５】単一の２次元クロソイド曲線による補間例の例示図である。
【図６】三連２次元クロソイド曲線による補間例の例示図である。
【図７】三連２次元クロソイド曲線による直線との補間例の例示図である。
【図８】３次元クロソイド曲線の接線ベクトルの説明図である。
【図９】３次元クロソイド曲線の曲率の変化の例を示す例示図である。
【図１０】３次元クロソイド曲線による補間曲線例を示す例示図である。
【図１１】三連３次元クロソイド曲線による補間曲線例を示す例示図である。
【図１２】三連３次元クロソイド曲線による直線との補間例を示す例示図である。
【図１３】本発明の一実施形態に係る軌道生成装置のブロック図である。
【図１４】本発明の他の実施形態に係る軌道生成装置のブロック図である。
【発明を実施するための形態】
【００２０】
以下、添付図面を参照しながら本発明を実施形態に基づいて説明するが、本発明はかかる実施形態のみに限定されるものではない。
【００２１】
実施形態１
本発明は、クロソイド曲線を利用して軌道の生成をなすものである。
【００２２】
本発明の理解を容易にするため、まず２次元クロソイド曲線について説明する。
【００２３】
一般的に、任意の連続曲線セグメントは以下の式（１）により表わされる。
【００２４】
【数１】
【００２５】
ここで、ベクトルＰ０は曲線の始点（ベクトル）を示し、ベクトルｕは曲線上の任意の単位接線ベクトルを示し、ｓは始点からの曲線長を示し、ｈは曲線の全長を示し、Ｓ（０≦Ｓ≦１）は無次元の曲線長を示す。
【００２６】
いま、ベクトルｕをＳの関数として定義すれば、それに対応する曲線が求められる。
【００２７】
複素平面において、２次元クロソイド曲線の単位接線ベクトル（ベクトルｕ）は以下の式（２）により与えられる。
【００２８】
【数２】
【００２９】
ここで、φは曲線上の任意点の接線方向を示し、φ０は始点における接線方向を示し、φｖは接線方向の１次増分係数を示し、φｕは接線方向の２次増分係数を示す。
【００３０】
式（１）と式（２）による積分は一般に初等関数で表わせないことが知られており、数値積分などの数値法で演算する必要がある。
【００３１】
図１に、数値積分により得られた２次元クロソイド曲線の一例を示す。
【００３２】
φをｓで微分すると２次元クロソイド曲線（単一の２次元クロソイド曲線）の曲率が得られる。すなわち、下記式（３）が得られる。
【００３３】
【数３】
【００３４】
式（３）により、得られた曲率が符号を持つことが理解される。つまり、曲線が時計方向に曲がるならば負、反時計方向に曲がるならば正となる。また、式（３）により、得られた曲率は曲線長のリニア関数であることが理解される。
【００３５】
次に、図２を参照しながら平面上の任意の２点Ｐ０と点Ｐ１との間の補間について説明する。
【００３６】
点Ｐ０の接線角Ψ０とし、点Ｐ１での接線角Ψ１とする。
【００３７】
演算処理を簡素化するため、点Ｐ０を原点とし、Ｘ軸を点Ｐ０から点Ｐ１に向かう方向に取ったローカル座標系を採用する。
【００３８】
このローカル座標系においては、曲線は原点（０，０）から出発し、点（ｒ，０）に終着するので、曲線は以下の式（４）により得られる。
【００３９】
【数４】
【００４０】
式（４）と式（２）の二番目の式から各パラメータφ０、φｖ、φｕおよびｈが求められる。
【００４１】
このようにして得られた各パラメータをグローバル座標系の値に変換するには、φ０にθを加算すればよい。それ以外のパラメータは、ローカル座標系の値がそのまま使用できる。
【００４２】
図３に、θを３０度とし、φ１を−３０度として、φ０を−１８０度〜１８０度において１０度ずつ変化させた場合における軌跡の生成例を示す。
【００４３】
ここで、前記補間法を「曲線端点の接線方向指定法」と定義することにする。
【００４４】
この曲線端点の接線方向指定法は、本願発明の基礎となる。
【００４５】
ちなみに、ロボットの移動経路を生成する補間法においては、加速度が不連続になるとロボットの動作が不安定となるため、加速度の連続性が要求される。一方、曲率が連続していない軌道上における移動は加速度が不連続となる。そのため、ロボットなどの移動経路のための補間法においては、曲率の連続性が要求される。
【００４６】
しかるに、前記曲線端点の接線方向指定法においては、通過点での接線角を指定することで、２次元クロソイド曲線の全パラメータφ０、φｖ、φｕおよびｈを決定するため、通過点での曲率を指定することができない。通過点での接線角および曲率の両者を同時に指定するには、クロソイド曲線のパラメータを増やす必要がある。その場合、リニアな曲率を維持したところから、Ｓの３次項，Ｓの４次項などの高次項が含まれないようにしてパラメータを設ける必要がある。
【００４７】
この高次項が含まれないようにしてパラメータを決定する手法を以下に説明する。この手法をここでは三連２次元クロソイド曲線による補間法と定義することにする。
【００４８】
三連２次元クロソイド曲線の単位接線ベクトルｕを式（５），式（６）および式（７）により定義する。
【００４９】
【数５】
【００５０】
【数６】
【００５１】
【数７】
【００５２】
ここで、無次元の定数Ｓ１およびＳ２（０<Ｓ１<Ｓ２<１）は三連２次元クロソイド曲線のつなぎ目の位置を示す。
なお、無次元の定数Ｓ１およびＳ２は、任意に指定できるが、対称性を保ちたい場合には、Ｓ１＝γ（０<γ<０．５）と指定し、Ｓ２＝１−γと指定すればよい。本発明者等は、γ＝０．２５を推奨値、つまりＳ１＝０．２５およびＳ２＝０．７５を推奨値としている。
【００５３】
また、ａ１０，ａ１１，ａ１２，ａ２０，ａ２１，ａ２２，ａ３０，ａ３１，ａ３２およびｈは三連２次元クロソイド曲線のパラメータである。これらのパラメータは全部で１０個あるが、式（７）により、そのうち４個は他のパラメータにより決定されるため、独立のパラメータは６個である。
【００５４】
また、式（７）の第１式および第２式は、Ｓ１およびＳ２での接線方向の連続条件を示し、式（７）の第３式および第４式は、Ｓ１およびＳ２での曲率の連続条件を示す。
【００５５】
式（５）〜式（７）および式（１）より、三連２次元クロソイド曲線の曲率は下記式（８）により与えられる。
【００５６】
【数８】
【００５７】
この式（８）により得られた曲率をＳに対して描くと３つの連続した線分になる。
【００５８】
また、三連２次元クロソイド曲線の特性は、単一の２次元クロソイド曲線の特性に似ていて直線および円の表現も可能であるが、それに加えてゼロの曲率の位置を３つまで持つことができる。なお、単一の２次元クロソイド曲線はゼロの曲率の位置を１つしか持てない。
【００５９】
次に、この三連２次元クロソイド曲線による点列の補間について説明する。
【００６０】
補間したい自由点列（点数:Ｎ＋１）を下記（９）とする。
【００６１】
Ｐ０，Ｐ１，Ｐ２，・・・，ＰＮ （Ｐはベクトルとする） （９）
【００６２】
ここで、これらの各点での接線方向を、図４に示すように、弦ベクトル転向角の二分法により決定する。
【００６３】
図４において、ベクトルＡＢは、Ｐ０ＣとＰ１Ｐ２との間の角を二等分し、∠ＡＰ０Ｐ１＝∠ＡＰ１Ｐ０とする。Ｐ０での接線方向をＰ０Ａとし、Ｐ１での接線方向をＰ１Ｂとする。ついで、点Ｐ２での接線方向Ｐ２Ｅを図４（ｂ）のように定める。同様にして他の点Ｐ３，Ｐ４，・・・，ＰＮでの接線方向も定める。
【００６４】
自由点列Ｐ０，Ｐ１，Ｐ２，・・・，ＰＮに対して前述したように、弦ベクトル転向角の二分法により接線方向を指定し、単一の２次元クロソイド曲線により補間すると、得られた補間曲線は各通過点での接線方向は連続になるが、一般的には、各通過点での曲率は連続とはならない。
【００６５】
図５、このようにして補間された通過点Ｐｋ前後の軌道を点線で示す。
【００６６】
そこで、本発明では、三連２次元クロソイド曲線による点列の補間を行う。
【００６７】
例として、点Ｐｋと点Ｐｋ＋１との区間を三連２次元クロソイド曲線により補間し、ついで点Ｐｋおよび点Ｐｋ＋１の接線方向を変えずに曲率を連続させる場合について説明する（図５参照）。なお、図５中、実線は目標曲線を示す。
【００６８】
軌道の生成においては各点における曲率を同一とする場合、低い方の曲率に他方の曲率を合わせるのが望ましいので、図５に示す例においては、点Ｐｋでの曲率を点Ｐｋの左側の曲率に合わせ、それをκ０とする。同様にして、点Ｐｋ＋１での曲率を右側に合わせ、それをκ１とする。
【００６９】
次に、点Ｐｋと点Ｐｋ＋１との区間において、始点接線角（φ［ｋ］）、曲率（κ０）、終点接線角（φ［ｋ＋１］）、曲率（κ１）の条件を満足する三連２次元クロソイド曲線を求める。この処理においては、演算処理の簡素化の観点から、点Ｐｋを原点にし、Ｘ軸を点Ｐｋから点Ｐｋ＋１に向かうローカル座標系を採用する。
【００７０】
このローカル座標系において、曲線は原点（０，０）から出発し、点（ｒ，０）に終着する。そのため、三連２次元クロソイド曲線の算出は下記式（１０）を演算処理することに帰結する。
【００７１】
【数１０】
【００７２】
ここで、ｒは点Ｐｋと点Ｐｋ＋１との距離を示し、θ［ｋ］はｋ番目のローカル座標系のＸ軸がグローバル座標系のＸ軸に対してなす角を示す。
【００７３】
式（１０）および式（６）から、曲線の全パラメータａ１０，ａ１１，ａ１２，ａ２０，ａ２１，ａ２２，ａ３０，ａ３１，ａ３２およびｈが得られる。
【００７４】
前記演算処理で得られた三連２次元クロソイド曲線のパラメータをグローバル座標系へ変換するには、ａ１０，ａ２０，ａ３０のそれぞれにθ［ｋ］を加算すればよい。それ以外のパラメータは、ローカル座標系の値がそのまま使用できる。
【００７５】
図６に、指定された点列に対する三連２次元クロソイド曲線による補間例を単一の２次元クロソイド曲線による補間例と合わせて示す。
【００７６】
図示例より、単一の２次元クロソイド曲線による補間例では、通過点での曲率が連続していないが、三連２次元クロソイド曲線による補間例では通過点での曲率が連続しているのが理解される。
【００７７】
実施形態２
実施形態１では、弦ベクトル転向角の二分法で接線方向を指定するようにされているが、実施形態２では、端点での接線方向を接続したい直線の方向に一致させるものとされる。
【００７８】
端点での接線方向を接続したい直線の方向に一致させた上で、κ０およびκ１をゼロとして三連２次元クロソイド曲線による補間をなせば、直線との接続もなし得る。
【００７９】
図７に、このようにして補間された補間例を示す。
【００８０】
このように、実施形態２によれば、障害物を避けるような軌道設定が可能となる。
【００８１】
実施形態３
実施形態３は、実施形態１を改変してなるものであって、クロソイド曲線による補間法を３次元まで拡張してなるものとされる。
【００８２】
３次元クロソイド曲線による補間法については特許文献１に提案されているが、前述したように、特許文献１の提案に係る補間法では、各通過点において接線方向と曲率とを同時に指定することができない。
【００８３】
そのため、実施形態３では三連３次元クロソイド曲線による補間をなすようにしてなるものである。
【００８４】
なお、３次元クロソイド曲線は、研究段階のものであるため、種々の定義が存在するが、実施形態３では下記式（１１），（１２）による定義、いわゆる牧野定義を採用している。
【００８５】
【数１１】
【００８６】
【数１２】
【００８７】
ここで、αはピッチ角を示し、βはヨー角を示す。
式（１１）から、接線ベクトルｕは、単位ベクトルｉ（ｉ＝［１，０，０］Ｔ）をまずｊ軸（Ｙ軸）回りに角αだけ回転させ、ついでｋ軸（Ｚ軸）回りに角βだけ回転させることにより得られることが理解される（図８参照）。
【００８８】
また、式（１１）から、３次元クロソイド曲線の曲率は下記式（１３）により与えられることが理解される。
【００８９】
【数１３】
【００９０】
式（１３）から理解されるように、ｄα／ｄＳおよびｄβ／ｄＳは曲線長のリニア関数であるが、一般的には、３次元クロソイド曲線の曲率は曲線長のリニア関数ではない。また、式（１３）の曲率は符号を持たない。曲線の曲がる方向は、ｄα／ｄＳおよびｄβ／ｄＳの符号により決定される。
【００９１】
図９に、ａ０＝ｂ０＝０，ａ１＝ｂ１＝ａ２＝ｂ２＝１，ｈ＝１とした場合の３次元クロソイド曲線の曲率と曲線長との関係を示す。
図９より、曲率は必ずしも曲線長のリニア関数とはならないが、曲率の変化は緩やかであるのが理解される。
【００９２】
２次元の座標系を任意に回転させると、曲線上の任意点の接線方向角はその回転角を加算するだけで求められる。これに対し、３次元の座標系を任意に回転させる場合、曲線上の任意点の接線方向のピッチ角およびヨー角は、２次元の場合のように簡単に求めることはできない。
【００９３】
例えば、単位接線ベクトルを座標系Ａのピッチ角およびヨー角で表わして式（１２）を満足するような曲線が存在すると仮定する。しかし、この同じ曲線を座標系Ｂのピッチ角およびヨー角で表わすと、一般的には、式（１２）のようなＳの二次関数では表せない。すなわち、ピッチ角およびヨー角を用いて補間する場合、得られる曲線は補間する座標系の選び方に依存する。
【００９４】
点列の各区間を同一のグローバル座標系のピッチ角およびヨー角により補間すると、ピッチ角およびヨー角の変動が大きくなるため、補間曲線には曲率の高い個所が生ずる場合がある。また、グローバル座標系の選び方によっても曲線は変化する。
【００９５】
かかる不具合を避けるため、つまりピッチ角およびヨー角の変動を小さくし、曲線の形が各点の相対位置のみに依存するようし、しかも曲率の小さな曲線となし得るようにするため、実施形態３では弦ベクトルをＸ軸とするローカル座標系が採用されている。
【００９６】
以下、かかるローカル座標系による３次元点列の補間について説明する。
【００９７】
より具体的には、点列を以下のように設定し、隣接する二点間Ｐｋ，Ｐｋ＋１間の補間について説明する。
【００９８】
Ｐ０，Ｐ１，Ｐ２，・・・，ＰＮ （Ｐはベクトルとする）
ローカル座標系は以下のように設定する。
【００９９】
原点:点Ｐｋ
Ｘ軸：弦ベクトルＰｋＰｋ＋１の方向とする。
Ｙ軸：ベクトルＰｋＰｋ＋１とベクトルＰｋ＋１Ｐｋ＋２との外積ベクトルの方向とする。
Ｚ軸：Ｘ軸およびＹ軸がともに右手系となるように設定する。
【０１００】
なお、ベクトルＰｋＰｋ＋１とベクトルＰｋ＋１Ｐｋ＋２が同一直線上にある場合、ベクトルＰｋＰｋ＋１とベクトルＰｋ＋１Ｐｋ＋２の外積がゼロとなるので、Ｙ軸方向を決めるには、ベクトルＰｋＰｋ＋１とベクトルＰｋ＋１Ｐｋ＋２の外積の代わりにベクトルＰｋＰｋ＋１とベクトルＰｋ＋１Ｐｉの外積ベクトル方向を用いる。ここで、ｉは以下の条件を満たす最も若い数である。
（１）ｉ＝ｋ＋３，ｋ＋４，・・・，Ｎ
（２）ベクトルＰｋ＋１ＰｉとベクトルＰｋＰｋ＋１は同一直線上にない
【０１０１】
また、（１）のすべてのｉに対して、ベクトルＰｋ＋１ＰｉとベクトルＰｋＰｋ＋１が同一直線上にある場合に、その前段（前区間）のローカル座標系のＹ軸方向を現段（現区間）のローカル座標系のＹ軸方向とする。同様に、ｋ＝Ｎ−１（最終区間）の場合は、前段（前区間ｋ＝Ｎ−２）のローカル座標系のＹ軸方向を現段（現区間）のローカル座標系のＹ軸方向とする。極端な例として、点列の全ての点が同一直線上にある場合に、補間結果はベクトルＰ０ＰＮ（線分Ｐ０ＰＮ）となり、ローカル座標系もそれ以後の演算も不要である。
【０１０２】
補間曲線が通過する各点における接線方向は、前述した弦ベクトル転向角の二分法により決定する。
【０１０３】
しかして、このようにして設定されたローカル座標系においては、補間曲線は原点（０，０，０）から出発し、点（ｒ，０，０）に終着するので、補間曲線は下記式（１４）を演算処理することにより算出される。
【０１０４】
【数１４】
【０１０５】
ここで、ｒは点Ｐｋと点Ｐｋ＋１との距離を示す。
【０１０６】
なお、点Ｐｋと点Ｐｋ＋１とにおける接線姿勢角、すなわちαｋ，βｋ，αｋ＋１，βｋ＋１はローカル座標系の値を採用するものとする。
【０１０７】
しかして、式（１４）および式（１２）から７つのパラメータ、すなわちａ０，ａ１，ａ２，ｂ０，ｂ１，ｂ２およびｈが得られる。ついで、これらのパラメータを用いてローカル座標系での補間曲線が得られる。しかる後、このローカル座標系での補間曲線をグローバル座標系に座標変換すれば、３次元クロソイド曲線による補間曲線が得られる。
【０１０８】
図１０に、３次元クロソイド曲線による補間曲線の例を示す。
【０１０９】
図１０のうち、同（ａ）における点列は立方体の頂点を示し、同（ｂ）における上方および下方のそれぞれの点列は正六角形の頂点を示す。
【０１１０】
図１０より、得られた補間曲線は、各通過点においてその接線方向が連続であり、各区間の曲がり方も緩やかであるのが理解される。また、立方体のサイズが変化してもパラメータｈだけを調整すればよく、他のパラメータは調整する必要がないのが理解される。
【０１１１】
実施形態５
実施形態５は実施形態２を３次元クロソイド曲線に拡張してなるものであって、三連３次元クロソイド曲線により補間をなすものである。
【０１１２】
実施形態４では、つまり単一の３次元クロソイド曲線による補間では、各通過点での接触角は連続となるが、一般的には、各通過点での曲率は連続とはならない。そのため、実施形態５では、三連３次元クロソイド曲線により補間をなし、各通過点での曲率も常に連続させるものである。
【０１１３】
実施形態５では、式（１２）が下記式（１５），（１６）に置き換えられる。
【０１１４】
【数１５】
【０１１５】
【数１６】
【０１１６】
実施形態５においては、パラメータは曲線長ｈを含めて１９個あるが、つなぎ目の接触角と曲率の連続条件により、そのうちの８個は他のパラメータにより決定されるので、独立変数は１１個となり、実施形態４の場合より４個増加しているだけである。
【０１１７】
実施形態５における各通過点における曲率の決定は、曲線の曲がる方向を考慮してなさなれる。実施形態２では、曲率の大きさと曲線の曲がる方向は、符号を有する曲率により決定することができたが、３次元クロソイド曲線の場合、曲がり方向が無限であるため、一つのスカラー変数のみで曲がり方向を決定することができない。そのため、実施形態５においては、各通過点の曲率はピッチ角（α）およびヨー角（β）の微分（ｄα／ｄｓ；ｄβ／ｄｓ）により間接的に決定するものとされている。つまり、下記式（１７）により決定するものとされている。
【０１１８】
【数１７】
【０１１９】
式（１７）から、曲率の絶対値のみならず、曲線の曲がる方向も決定されるのが理解される。
【０１２０】
式（１７）を式（１３）に代入して下記式（１８）が得られる。
【０１２１】
【数１８】
【０１２２】
式（１８）から、καとκβとが連続であれば、曲率κも連続となるのが理解される。このことから、各通過点においてκαとκβとが連続するような曲線を求めればよいのが理解される。
【０１２３】
しかして、各通過点のκαとκβを得るには、まず単一の３次元クロソイド曲線で補間をしてその通過点両側のκαとκβを得、ついで絶対値の低い側のκαとκβをその通過点の目標値とする。その場合、καとκβとは、補間する区間のローカル座標系に変換された値が用いられる。
【０１２４】
次に、点Ｐｋから点Ｐｋ＋１までの区間について説明する。なお、座標系は前記と同様とする。
【０１２５】
点Ｐｋの（κα，κβ）の目標値を（κ０α，κ０β）とし、点Ｐｋ＋１の（κα，κβ）の目標値を（κ１α，κ１β）とする。このローカル座標系において、補間曲線は原点（０，０，０）から出発し、点（ｒ，０，０）に終着するので、補間曲線は下記式（１９）を演算処理することにより得られる。
【０１２６】
【数１９】
【０１２７】
ここで、ｒは点Ｐｋから点Ｐｋ＋１までの距離を示す。
【０１２８】
なお、点Ｐｋと点Ｐｋ＋１とおける接線姿勢角αｋ，βｋ，αｋ＋１，βｋ＋１は、ローカル座標系での値を用いる。また、式（１９）の４番目以降の各式はピッチ角αおよびヨー角βの連続条件および曲率の連続条件を示す。
【０１２９】
式（１９）、式（１５）および式（１６）から、三連３次元クロソイド曲線のパラメータａ１０，ａ１１，ａ１２，ａ２０，ａ２１，ａ２２，ａ３０，ａ３１，ａ３２，ｂ１０，ｂ１１，ｂ１２，ｂ２０，ｂ２１，ｂ２２，ｂ３０，ｂ３１，ｂ３２，およびｈを得る。
【０１３０】
しかして、これらのパラメータを用いてローカル座標系での補間曲線を得、ついで得られた補間曲線の座標系をローカル座標系からグローバル座標系に変換することにより補間曲線が得られる。
【０１３１】
式（１９）より明らかなように、三連３次元クロソイド曲線における独立変数は、単一の３次元クロソイド曲線における独立変数に比して４個増加しているので、各区間において接線方向だけでなく、（κ０α，κ０β）および（κ１α，κ１β）も指定できる。換言すれば、各通過点において曲率が連続な補間が可能となる。
【０１３２】
図１１に、三連３次元クロソイド曲線による曲率が連続とされた補間例を、単一の３次元クロソイド曲線による補間例と併せて示す。なお、点列と図１０におけるものと同一とされている。また、Ｓ１は０．２５とされ、Ｓ２は０．７５とされている。
【０１３３】
ここで、（κ０α，κ０β）＝（κ１α，κ１β）＝（０，０）として、三連３次元クロソイド曲線により補間すれば、異なる平面にある直線との３次元接続も容易になし得る。その場合、端点での接線方向は、接続する直線の方向に一致させればよい。
【０１３４】
図１２に、三連３次元クロソイド曲線による直線との補間例を示す。
【０１３５】
実施形態６
図１３に、本発明の実施形態６をブロック図で示す。
【０１３６】
図１３に示すように、実施形態６は実施形態１を軌道生成装置Ａに改変してなるものとされる。
【０１３７】
軌道生成装置Ａは、図１３に示すように、入力手段１と、演算処理装置２と、出力手段３とを主要構成要素として備えてなるものとされる。
【０１３８】
入力手段１は、例えばキーボードとされるが、これに限定されるものではなく、コンピュータの入力装置として利用されている各種の入力装置を好適に用いることができる。
【０１３９】
演算処理装置２は、クロソイド曲線生成手段１０と、座標変換手段２０と、補間指令値生成手段３０とを有するものとされる。なお、かかる手段１０，２０，３０を有する演算処理装置２は、具体的には、コンピュータに後述する機能を実施するためのプログラムを格納することにより実現される。
【０１４０】
クロソイド曲線生成手段１０は、接線方向算出部１１と、単一の２次元クロソイド曲線生成部１２と、三連２次元クロソイド曲線生成手段部１３とを含むものとされ、入力手段１から入力された点列や接続する直線の方向に基づいて、式（１）〜式（８）および式（１０）を演算処理してローカル座標系における三連クロソイド曲線を生成するものとされる。
【０１４１】
接線方向算出部１１は、弦ベクトル転向角の二分法により接線方向を算出するものとされる。なお、接線方向算出部１１を省略して入力手段１から接線方向が入力されるようにされてもよい。
【０１４２】
座標変換手段２０は、クロソイド曲線生成手段１０により生成されたローカル座標系における２次元クロソイド曲線を、グローバル座標系における２次元クロソイド曲線に変換するものとされる。
【０１４３】
補間指令値生成手段３０は、グローバル座標系に座標変換された三連２次元クロソイド曲線に基づいてロボットなどの移動体の補間指令値を生成するものとされる。なお、補間指令値生成手段３０としては、従来よりロボットに用いられているものを好適に用いることができる。
【０１４４】
出力手段３は、例えば通信装置とされるが、これに限定されるものではなく、各種の出力手段を好適に用いることができ、例えばＣＤライタとすることもできる。
【０１４５】
このように、実施形態６によれば、各通過点における接線方向および曲率を連続させることができるので、所要エネルギーが低減されてなる軌道を生成することができる。
【０１４６】
実施形態７
図１４に、本発明の実施形態７をブロック図で示す。
【０１４７】
図１４に示すように、実施形態７の軌道生成装置Ａ１は実施形態６の軌道生成装置Ａを改変してなるものとされる。すなわち、実施形態７はクロソイド曲線による補間を３次元まで拡張してなるものとされる。
【０１４８】
以下、実施形態７の実施形態６と異なる点を主として説明する。
【０１４９】
クロソイド曲線生成手段４０は、接線方向算出部４１と、単一の３次元クロソイド曲線生成部４２と、三連３次元クロソイド曲線生成手段部４３とを含むものとされ、入力手段１から入力された点列や接続する直線の方向に基づいて、式（１１）〜式（１９）を演算処理してローカル座標系における三連クロソイド曲線を生成するものとされる。
【０１５０】
接線方向算出部４１は、弦ベクトル転向角の二分法により接線方向を算出するものとされる。なお、接線方向算出部４１を省略して入力手段１から接線方向が入力されるようにされてもよい。
【０１５１】
座標変換手段２０は、クロソイド曲線生成手段４０により生成されたローカル座標系における３次元クロソイド曲線を、グローバル座標系における３次元クロソイド曲線に変換するものとされる。
【０１５２】
補間指令値生成手段３０は、グローバル座標系に座標変換された三連３次元クロソイド曲線に基づいてロボットなどの移動体の補間指令値を生成するものとされる。なお、補間指令値生成手段３０としては、従来よりロボットに用いられているものを好適に用いることができる。
【０１５３】
このように、実施形態７によれば、各通過点における接線方向および曲率を連続させることができるので、所要エネルギーが低減されてなる３次元軌道を生成することができる。
【産業上の利用可能性】
【０１５４】
本発明は、ロボットなどの移動体の軌道生成に適用できる。
【符号の説明】
【０１５５】
Ａ 軌道生成装置
１ 入力手段
２ 演算処理装置
３ 出力手段
１０ クロソイド曲線生成手段
１１ 接線方向算出部
１２ 単一の２次元クロソイド曲線生成部
１３ 三連２次元クロソイド曲線生成部
２０ 座標変換手段
３０ 補間指令値生成手段
４０ クロソイド曲線生成手段
４１ 接線方向算出部
４２ 単一の３次元クロソイド曲線生成部
４３ 三連３次元クロソイド曲線生成部
【特許請求の範囲】
【請求項１】
点列間をクロソイド曲線により補間して軌道生成をなす軌道生成方法であって、前記クロソイド曲線が三連クロソイド曲線とされ、それにより通過点での接線方向の連続性および曲率の連続性が担保されてなることを特徴とする軌道生成方法。
【請求項２】
通過点での接線方向が弦ベクトル転向角の二分法により算出されることを特徴とする請求項１記載の軌道生成方法。
【請求項３】
ローカル座標系におけるクロソイド曲線を生成し、ついで生成されたクロソイド曲線をグローバル座標系に変換し、座標変換して得られたクロソイド曲線により補間をなすことを特徴とする請求項１記載の軌道生成方法。
【請求項４】
端点での接線方向を接続させる曲線と一致させるようにしてクロソイド曲線を生成することを特徴とする請求項１記載の軌道生成方法。
【請求項５】
点列間をクロソイド曲線により補間して軌道生成をなす軌道生成装置であって、
入力手段と、演算処理装置と、出力手段とを備え、
前記演算処理装置が、クロソイド曲線生成手段と、座標変換手段と、補間指令値生成手段とを有してなることを特徴とする軌道生成装置。
【請求項６】
クロソイド曲線生成手段が、接線方向算出部と、単一の２次元クロソイド曲線生成部と、三連２次元クロソイド曲線生成部とを有することを特徴とする請求項５記載の軌道生成装置。
【請求項７】
クロソイド曲線生成手段が、接線方向算出部と、単一の３次元クロソイド曲線生成部と、三連３次元クロソイド曲線生成部とを有することを特徴とする請求項５記載の軌道生成装置。
【請求項８】
請求項１ないし請求項４のいずれか一項に記載の軌道生成方法により生成された軌道により移動されることを特徴とする移動体。
【請求項９】
請求項５ないし請求項７のいずれか一項に記載の軌道生成装置を備えてなることを特徴とする移動体。
【請求項１０】
移動体がロボットとされてなることを特徴とする請求項８または９記載の移動体。
【請求項１】
点列間をクロソイド曲線により補間して軌道生成をなす軌道生成方法であって、前記クロソイド曲線が三連クロソイド曲線とされ、それにより通過点での接線方向の連続性および曲率の連続性が担保されてなることを特徴とする軌道生成方法。
【請求項２】
通過点での接線方向が弦ベクトル転向角の二分法により算出されることを特徴とする請求項１記載の軌道生成方法。
【請求項３】
ローカル座標系におけるクロソイド曲線を生成し、ついで生成されたクロソイド曲線をグローバル座標系に変換し、座標変換して得られたクロソイド曲線により補間をなすことを特徴とする請求項１記載の軌道生成方法。
【請求項４】
端点での接線方向を接続させる曲線と一致させるようにしてクロソイド曲線を生成することを特徴とする請求項１記載の軌道生成方法。
【請求項５】
点列間をクロソイド曲線により補間して軌道生成をなす軌道生成装置であって、
入力手段と、演算処理装置と、出力手段とを備え、
前記演算処理装置が、クロソイド曲線生成手段と、座標変換手段と、補間指令値生成手段とを有してなることを特徴とする軌道生成装置。
【請求項６】
クロソイド曲線生成手段が、接線方向算出部と、単一の２次元クロソイド曲線生成部と、三連２次元クロソイド曲線生成部とを有することを特徴とする請求項５記載の軌道生成装置。
【請求項７】
クロソイド曲線生成手段が、接線方向算出部と、単一の３次元クロソイド曲線生成部と、三連３次元クロソイド曲線生成部とを有することを特徴とする請求項５記載の軌道生成装置。
【請求項８】
請求項１ないし請求項４のいずれか一項に記載の軌道生成方法により生成された軌道により移動されることを特徴とする移動体。
【請求項９】
請求項５ないし請求項７のいずれか一項に記載の軌道生成装置を備えてなることを特徴とする移動体。
【請求項１０】
移動体がロボットとされてなることを特徴とする請求項８または９記載の移動体。
【図１】
【図２】
【図５】
【図６】
【図７】
【図９】
【図１３】
【図１４】
【図３】
【図４】
【図８】
【図１０】
【図１１】
【図１２】
【図２】
【図５】
【図６】
【図７】
【図９】
【図１３】
【図１４】
【図３】
【図４】
【図８】
【図１０】
【図１１】
【図１２】
【公開番号】特開２０１１−１４５７９７（Ｐ２０１１−１４５７９７Ａ）
【公開日】平成２３年７月２８日（２０１１．７．２８）
【国際特許分類】
【出願番号】特願２０１０−４６８２（Ｐ２０１０−４６８２）
【出願日】平成２２年１月１３日（２０１０．１．１３）
【出願人】（３９５０１８２５１）マッスル株式会社 (15)
【Ｆターム（参考）】
【公開日】平成２３年７月２８日（２０１１．７．２８）
【国際特許分類】
【出願日】平成２２年１月１３日（２０１０．１．１３）
【出願人】（３９５０１８２５１）マッスル株式会社 (15)
【Ｆターム（参考）】
[ Back to top ]