(57)【特許請求の範囲】
【請求項１】
予め与えられた点列を、制御手段により算出したクロソイドセグメントを用いて補間することにより得られる軌跡に基づいて、被制御部材の制御を行う軌跡制御方法であって、
補間すべき複数の点の座標を入力する第一行程と、
前記第一行程により入力された各点における仮の接線方向角を求める第二行程と、
前記第一行程により入力された複数の点により構成される各区間について区間助変数である弧長、曲角、縮角をそれぞれ求める第三行程と、
各点における両側の曲率が等しくなるように前記第二行程により求められた接線方向角を修正する第四行程と、
前記第三行程により求められた区間助変数を分割助変数に変換し、これよって順次補間位置を求める第五行程と、
を備え、
前記区間助変数は、始点と終点との位置の差から、弦の長さと方向角とを求める第一演算処理と、始点と終点との接線方向角の差から縮角多項式の係数を求める第二演算処理と、ｙの縮角多項式を使って縮角をニュートン法によって逆算する第三演算処理と、上記縮角とｘの縮角多項式とを使って弧長を計算する第四演算処理と、接線方向角の差と縮角とから曲角を計算する第五演算処理と、の各演算処理によって算出し、
前記第四行程が、前記各点のうちの端点を除く中間点での曲率差評価値を求める第六演算処理と、各点での曲率差評価値を比較し最大点をマークする第七演算処理と、最大点での曲率差評価値が許容範囲内にあるか否かを判断し、該曲率差評価値がこの許容範囲内にあれば該第四行程を終了する第八演算処理と、上記第八演算処理において上記曲率差評価値が上記許容範囲内にない場合に、最大点の接線方向角を修正する第九演算処理と、上記最大点の前後２区間の区間助変数を再計算する第十演算処理と、最大点及び前後点の３点での曲率差評価値を再計算する第十一演算処理と、から構成されたものである
ことを特徴とする、軌跡制御方法。
【請求項２】
前記第三演算処理におけるニュートン法の初期値を決定する処理としてクロソイドの関節近似式を採用したことを特徴とする、請求項１に記載の軌跡制御方法。
【請求項３】
予め与えられた点列を、制御手段により算出したクロソイドセグメントを用いて補間することにより得られる軌跡に基づいて、被制御部材の制御を行う軌跡制御方法を記録した記録媒体であって、
補間すべき複数の点の座標を入力する第一行程と、
前記第一行程により入力された各点における仮の接線方向角を求める第二行程と、
各区間について区間助変数である弧長、曲角、縮角をそれぞれ求める第三行程と、
各点における両側の曲率が等しくなるように前記第二行程により求められた接線方向角を修正する第四行程と、
前記第三行程により求められた区間助変数を分割助変数に変換し、これよって順次補間位置を求める第五行程と、
を備え、
前記区間助変数は、始点と終点との位置の差から、弦の長さと方向角とを求める第一演算処理と、始点と終点との接線方向角の差から縮角多項式の係数を求める第二演算処理と、ｙの縮角多項式を使って縮角をニュートン法によって逆算する第三演算処理と、上記縮角とｘの縮角多項式とを使って弧長を計算する第四演算処理と、接線方向角の差と縮角とから曲角を計算する第五演算処理と、の各演算処理によって算出し、
前記第四行程が、前記各点のうちの端点を除く中間点での曲率差評価値を求める第六演算処理と、各点での曲率差評価値を比較し最大点をマークする第七演算処理と、最大点での曲率差評価値が許容範囲内にあるか否かを判断し、該曲率差評価値がこの許容範囲内にあれば該第四行程を終了する第八演算処理と、上記第八演算処理において上記曲率差評価値が上記許容範囲内にない場合に、最大点の接線方向角を修正する第九演算処理と、上記最大点の前後２区間の区間助変数を再計算する第十演算処理と、最大点及び前後点の３点での曲率差評価値を再計算する第十一演算処理と、から構成されたものである
軌跡制御方法を記録したことを特徴とする、軌跡制御方法を記録した記録媒体。
【請求項４】
前記第三演算処理におけるニュートン法の初期値を決定する処理としてクロソイドの関節近似式を採用した軌跡制御方法を記録したことを特徴とする、請求項３に記載の軌跡制御方法を記録した記録媒体。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、数値情報によって図形を表現するグラフィックプロセッサ、或いは数値制御工作機械及びロボット等の分野において、離散的な点列を滑らかな曲線を用いて補間することにより得られる軌跡に基づいて各種制御を行う軌跡制御方法の改良及びこのような軌跡制御方法を記録した記録媒体に関する。
【０００２】
【従来の技術】
例えば、自由曲線の作図、文字描き、工作物や金型の加工、アーク溶接、接着剤やシール剤の塗布等を行う上で、予め与えられた点列から滑らかな曲線を計算し、これによって得られた曲線を用いて補間することにより得られる軌跡に基づいて、連続的に位置制御を行う軌跡制御方法（ＣＰ(Continuous Path) 制御）が知られている。このようなＣＰ制御の場合、上記点列は離散的なデータとして入力されるため、補間点軌跡をマイクロコンピュータ（ＭＰＵ）等の制御手段により算出し制御を行うようにしている。
【０００３】
図４は、上述したような制御手段の１例を示している。この図４における軌跡制御部１が、上記軌跡制御方法に基づいて補間制御を行う。上記軌跡制御部１による従来から知られた軌跡制御方法としては、線分を用いて補間する直線補間法、円弧を用いて補間する円弧補間法、更にはＢスプライン補間法やベジエ補間法と呼ばれる３次式を用いて補間する三次式系スプライン補間法が広く知られている。
【０００４】
上述した各補間法のうち、直線補間法は、任意に与えられた点列を容易に接続することができる。又、いかなる補間法よりも距離が短くなるとともに補間区間内では曲率が零となる。ところが、この直線補間法の場合、一般に、区間のつなぎ目では接線方向角が不連続になり、曲率が無限大になってしまう。その結果、例えばロボットアーム等の被制御部材において水平方向から垂直方向に移動する軌跡を得たい場合に上記直線補間方法を用いた際には、装置（ロボットや数値制御工作機械等）の持つ慣性により直角に折れ曲がることはできず、軌跡誤差を生じてしまう。これを防止するためには、当該コーナーで一旦作動を停止させる必要があるが、このように作動を一旦停止させると、装置の作動速度を高速化できないばかりか、例えば接着剤塗布作業にあっては塗布すべき接着剤の量が不均一になってしまう。
【０００５】
尚、上記接線方向角とは、任意の点Ｐにおける曲線の接線と基準線とのなす角φを指す。また、上記曲率は、接線方向角φを、弧長ｓで微分した値φ' ＝ｄφ／ｄｓで定義されるものである。本明細書においては、この接線方向角φと曲率φ' をいずれも−∞から∞の範囲で定義している。
【０００６】
上述した直線補間法に対して上記円弧補間法においては、与えられた点Ｐにおける接線方向角φが連続となるように接続（補間）できる。この場合、当該補間に用いた円弧は区間内で曲率が一定で、他のいかなる補間方法に比較しても、最大曲率が最小（最小曲率半径が最大）となる。しかしながら、上記円弧補間法においては、一般に、区間のつなぎ目で曲率が不連続となってしまう。従って、円弧補間法を用いた場合には、接線方向の速度、加速度を連続にしたとしても、法線方向の加速度が不連続になって慣性力が不連続になり、上記被制御部材を経路どおり滑らかに移動させることができない。このため、上述した直線補間ほどではないにせよ、不具合を生じることがある。また自由点列の始点で接線方向角を与え、以下順次円弧で接続することは、理論的には可能であるが実用的でない。
【０００７】
例えば楕円上に上下左右の４点をとり、下から右向きに出発する円弧補間はすぐに逆曲率になってしまい、うまくいかない。結果的に円弧補間は直線補間より悪いことも多いので、特定の用途にしか使われていない。
【０００８】
次に、上記Ｂスプライン補間法やベジエ曲線による補間法は、与えられた点列の近傍を結ぶ滑らかな曲線を容易に得ることができるが、上記点列を厳密に通るようにすることはできない。このため、例えばグラフィックプロセッサ等のように、その場で結果を見て修正できる場合を除き、実用に値しない。次に、上記３次式スプライン補間法においては、部分的にそれぞれの式を与え、接線方向及び曲率が連続となるように上記与えられた点列間をつないでいくことにより、滑らかな自由曲線を得ることかできる。しかしながら、この３次式スプライン補間法の場合、互いに直交するｘ軸方向及びｙ軸方向に、それぞれ別々に３次式を与えるため、点列のｘ軸、ｙ軸に対する角度如何によっては不規則なうねりを生じることがある。このため、上記アーム等の被制御部材の円滑で無駄のない正確な動作を確実に行うことは困難であった。また、「アフィン空間での不変性」は保証されているが、ｘ軸、ｙ軸に別々の式を与えるため、「ユークリッド空間での不変性」がなく、接線方向角、弧長、曲率の計算が困難である。
【０００９】
尚、図５（Ａ）に示すように、上記直線補間法と円弧補間法と三次式系スプライン補間法との３つの補間法は、独立した存在であり、条件によって適宜選択して補間制御を行っている。このように各種補間制御法を適宜選択して補間制御を行う場合、円弧を３次式で近似することもその逆も簡単でない。図６に、これら各補間法における表現形式を、図７に、各概形を、それぞれ示す。ここからみると直線−放物線−三次式系スプラインがひとつの系列、直線―円弧―クロソイドがもうひとつの系列であることがわかる。定規で描く直線、コンパスで描く円の次の第三の曲線はクロソイドである。
【００１０】
上記従来の補間制御に対し、上述した直線補間法、円弧補間法や三次式系スプライン補間法等の有する不具合を解消すべく本出願人は、次述するようにクロソイドセグメントを用いて補間を行う方法を発明した（例えば、特開平１−５０１８１号公報、特開昭６４−１００８号公報、特開平６−１６８０２２号公報、特開平６−２５９５６７号公報等）。このようなクロソイドセグメントを利用した補間法は、図５（Ｂ）に示すように、上記直線補間法や円弧補間法を包含するものである。
【００１１】
一般に補間を実現するには
１．補間式を決定する。
２．助変数を決定する。
３．きざみを決めて順次座標を計算する。
の３段階があり、２．で後述する逆解が、３．で後述する順解が必要とされる。直線・円弧を包含した補間法の採用は、１．の補間式の決定を自動化したことになる。
【００１２】
以下に、クロソイド曲線とクロソイドセグメントに関する基本的な理論について簡単に説明しておく。先ず、上記クロソイド曲線をはじめ、関連する用語の定義を示す。
0004146549.tif 000002
図８は基本クロソイド曲線を示しており、同図の実線は、φ0 ＝φ'0＝０、
φ" ＝π／２とした場合における曲線を示すものである。この曲線はコルニューの螺旋と呼ばれる。同図の破線はφ" ＝−π／２とした場合の曲線を示している。Ｃｓ，Ｓｎはフレネル積分として知られている。
【００１３】
尚、数式の表現として、本明細書においては以下のような記載方法を採用する。
/     除算記号
^     べき乗記号
ｉ＝［ｖ］  小数部切り捨て  例［０．５］＝０  −［−０．５］＝１
ａ．．ｂ    ａからｂまでの積分区間または累計区間
ａ・・・    ａから無限大までの累計区間
ｅｊ（φ）＝ｅ^(ｊ・φ) ＝cos φ＋ｊ・sin φ    ２次元単位ベクトル
【００１４】
クロソイドセグメントは、直線から線分を、円から円弧を切り出すのと同様、クロソイド曲線の一部を切り出したものである。上記基本式で始点Ｐ0 、終点Ｐ1 を確定し、弧長を０からｈとして定積分する。又、区間を（Ｓ＝０..１）と無次元化し、角度変化の円弧成分としての曲角φv ＝φ'0・ｈと、同じくクロソイド成分としての縮角φ u＝φ" ／２・ｈ^2と、を定義する。クロソイドセグメントの基本式は（１）より
0004146549.tif 000003
【００１５】
クロソイドセグメントの形は、曲角φv と縮角φu とのみで決まり、大きさはｈ、位置はＰ0 、方向はφ0 で決まる。弧長ｈと曲角φv と縮角φu とをあわせて区間助変数と称する。
直線と円とクロソイドとは別々の図形である。直線は無限で方向があり、円は有限で大きさがあり、クロソイドは長さは無限、存在範囲は有限で方向も大きさもある。先の定義によって線分は円弧の部分集合、円弧はクロソイドセグメントの部分集合となる。尚、上述したように、始点と接線方向角と区間助変数とを与えて、終点と接線方向角とを求める方法を順解と呼ぶ。これに対し、始点と終点の位置と接線方向角とを与えて、区間助変数を求める方法を逆解と呼ぶ。
【００１６】
前述したような先発明に係る方法においては、図９に示すような、クロソイド創生装置を用いて、
ｘ1 ＝ｘ0       ＋ｄｘ0
ｙ1 ＝ｙ0       ＋ｄｙ0
ｄｘ1 ＝ｄｘ0 * ｖｘ0 −ｄｙ0 * ｖｙ0
ｄｙ1 ＝ｄｘ0 * ｖｙ0 ＋ｄｙ0 * ｖｘ0
ｖｘ1 ＝ｖｘ0 * ｕｘ  −ｖｙ0 * ｕｙ
ｖｙ1 ＝ｖｘ0 * ｕｙ  ＋ｖｙ0 * ｕｘ          （３）
を繰り返すことにより、順次クロソイドセグメントの位置を算出する順解のみを提供しており、逆解は「はさみうち」法などの汎用手段で実現してきた。そのため、処理速度や精度に問題があった。高速な制御を行い得る短時間での算出処理に限界があった。そこで逆解まではオフラインで計算し、区間助変数を記憶しておいて、実行段階で分割助変数（上記ｄｘ，ｄｙ，ｖｘ，ｖｙ，ｕｘ，ｕｙ) を求めて順解を実行する手法であった。この方式では、１点あたり４データ（φ0 ，φv ，φu ，ｈ）が記憶される。しかし高速逆解がリアルタイムで可能になれば、１点あたり２データ（ｘ，ｙ）のみを記憶するだけで済む。
【００１７】
ところで、近年、マイクロコンピュータに係る開発が急速に進み、その処理能力が著しく進歩してきた。このため、多数の点から成る点列が与えられた場合であっても、クロソイドセグメントを用いた補間をロボットアームに組み込んだ制御器で行えるよう、そのアルゴリズムの開発が熱望されていた。この発明に係る軌跡制御方法及び軌跡制御方法を記録した記録媒体は、上述のような事情に鑑みて創案されたもので、その目的とするところは、多数の点から成る点列が与えられた場合に、この点列をクロソイドセグメントを用いて高速に、且つ曲率を連続として補間可能として、この補間によって得られる軌跡に基づいてロボットアーム等の被制御部材の制御を行えるようにすることにある。
【００１８】
【課題を解決するための手段】
この発明に係る軌跡制御方法及び軌跡制御方法を記録した記録媒体のうち、請求項１に記載した軌跡制御方法は、予め与えられた点列を、制御手段により算出したクロソイドセグメントを用いて補間することにより得られる軌跡に基づいて、被制御部材の制御を行う軌跡制御方法に関する。このような請求項１に記載した軌跡制御方法においては、補間すべき複数の点の座標を入力する第一行程と、第一行程により入力された各点における仮の接線方向角を求める第二行程と、第一行程により入力された複数の点により構成される各区間について区間助変数（弧長、曲角、縮角）をそれぞれ求める第三行程と、各点における両側の曲率が等しくなるように第二行程により求められた接線方向角を修正する第四行程と、第三行程により求められた区間助変数を分割助変数に変換し、これによって順次補間位置を求める第五行程と、を備えたことを特徴としている。
【００１９】
上述のように構成される、この発明に係る軌跡制御方法を用いることにより、入力された点列を最適なクロソイドセグメントで迅速に補間することができる。尚、上記各行程は、マイクロコンピュータ等の制御手段が演算処理を施すことによって行う。このような制御手段を備えることにより、自動組立装置や描画装置等における補間制御を容易且つ円滑に、しかも高速に行えるようになり、補間制御を行う種々の分野で有効な効果を奏する。ちなみに、コンピュータの演算速度は１８ヶ月で２倍になってきた。過去１５年で１０００倍早くなったことになる。本発明のアルゴリズムは、同じハードウエアで従来の逆解の速度より約１０００倍早くなっている。ハードウエアとあわせて１００万倍の高速化が実現できた。クロソイドが高速道路の設計に使われ１０００秒（約１７分）かかって計算していた問題を、サーボコントローラのサンプルタイム１ｍｓで計算できるようになったのである。
【００２０】
尚、前記区間助変数の決定は、始点と終点との位置の差から、弦の長さと方向角とを求める第一演算処理と、始点と終点との接線方向角の差から縮角多項式の係数を求める第二演算処理と、ｙの縮角多項式を使って縮角をニュートン法によって逆算する第三演算処理と、上記縮角とｘの縮角多項式とを使って弧長を計算する第四演算処理と、接線方向角の差と縮角とから曲角を計算する第五演算処理と、から構成されたものとすることができる。
【００２１】
また、上記第三演算処理におけるニュートン法の初期値を決定する処理としては、クロソイドの関節近似式を採用することができる。
【００２２】
さらに、前記第四行程が、前記各点のうちの端点を除く中間点での曲率差評価値を求める第六演算処理と、各点での曲率差評価値を比較し最大点をマークする第七演算処理と、最大点での曲率差評価値が許容範囲内にあるか否かを判断し、該曲率差評価値がこの許容範囲内にあれば該第四行程を終了する第八演算処理と、上記第八演算処理において上記曲率差評価値が上記許容範囲内にない場合に、最大点の接線方向角を修正する第九演算処理と、上記最大点の前後２区間の区間助変数を再計算する第十演算処理と、最大点及び前後点の３点での曲率差評価値を再計算する第十一演算処理と、から構成されたものとすることができる。
【００２３】
上述のように構成することにより、各点を補間するための最適なクロソイドセグメントを容易且つ円滑に、しかも、より一層高速に算出することが可能になり、実際のロボット等に備えた制御装置によっても当該クロソイドセグメントを算出可能となる。従って、実際の装置にリアルタイムで応用することが可能となり、高品質の補間制御を実施できるようになる。
【００２４】
更に、上記軌跡制御方法は、上記制御手段に直接記憶させる他、フレキシブルディスク（いわゆるＦＤ）等の従来知られた種々の記録媒体に記憶させておき、必要に応じて上記制御手段に読み込ませて使用することができる。このような請求項５に記載した発明は、予め与えられた点列を、制御手段により算出したクロソイドセグメントを用いて補間することにより得られる軌跡に基づいて、被制御部材の制御を行う軌跡制御方法を記録した記録媒体に関する。
【００２５】
このような記録媒体に関する発明においては、補間すべき複数の点の座標を入力する第一行程と、第一行程により入力された各点における仮の接線方向角を求める第二行程と、第一行程により入力された複数の点により構成される各区間について区間助変数である弧長、曲角、縮角をそれぞれ求める第三行程と、各点における両側の曲率が等しくなるように第二行程により求められた接線方向角を修正する第四行程と、第三行程により求められた区間助変数を分割助変数に変換し、これよって順次補間位置を求める第五行程と、を備えた軌跡制御方法を記録したものである。
【００２６】
又、前記区間助変数の決定を、始点と終点との位置の差から、弦の長さと方向角とを求める第一演算処理と、始点と終点との接線方向角の差から縮角多項式の係数を求める第二演算処理と、ｙの縮角多項式を使って縮角をニュートン法によって逆算する第三演算処理と、上記縮角とｘの縮角多項式とを使って弧長を計算する第四演算処理と、接線方向角の差と縮角とから曲角を計算する第五演算処理と、から構成した軌跡制御方法を記録したものとすることができる。
【００２７】
又、上記第三演算処理におけるニュートン法の初期値を決定する処理として、クロソイドの関節近似式を採用した軌跡制御方法を記録したものとすることができる。
【００２８】
さらに、前記第四行程が、前記各点のうちの端点を除く中間点での曲率差評価値を求める第六演算処理と、各点での曲率差評価値を比較し最大点をマークする第七演算処理と、最大点での曲率差評価値が許容範囲内にあるか否かを判断し、該曲率差評価値がこの許容範囲内にあれば該第四行程を終了する第八演算処理と、上記第八演算処理において上記曲率差評価値が上記許容範囲内にない場合に、最大点の接線方向角を修正する第九演算処理と、上記最大点の前後２区間の区間助変数を再計算する第十演算処理と、最大点及び前後点の３点での曲率差評価値を再計算する第十一演算処理と、から構成された軌跡制御方法を記録したものとすることができる。
【００２９】
このような構成とすることにより、当該記録媒体によって所望の装置に上記方法を読み込ませ、補間制御を実施することが可能になる。記録媒体に記録させる際の言語については特に問わない。使用する制御手段等によって、種々のものを採用できる。
【００３０】
【発明の実施の形態】
次に、本発明の実施の一形態例について、図１乃至図３を参照しつつ説明する。尚、最初に本形態例の構成の概略について説明し、次いで、図面を参照しつつ詳細に説明する。
【００３１】
本形態例に係る軌跡制御方法は、予め与えられた多数（例えば、１００点）の点から成る点列を、マイクロコンピュータ（ＭＰＵ）等の制御手段により算出したクロソイドセグメントを用いて補間することにより得られる軌跡に基づいて、溶接ロボットのアーム等の被制御部材の制御を行うものである。このために本形態例に係る軌跡制御方法においては、上記点列の各座標Ｐｉ（ｘｉ，ｙｉ）を入力する。この点列の入力を行う行程が、特許請求の範囲に記載した第一行程である。
【００３２】
次いで、図２に示すように、各点における接線方向角φを求める（第二行程）。尚、上記接線方向角φとは、前述したように上記各点におけるそれぞれ接線の方向を指し、基準線に対する接線のなす角φで表す。この第二行程で求める接線方向角φは、端点以外は仮のものである。
【００３３】
次いで、全ての区間における区間助変数を求める（第三行程）。区間助変数は、弧長ｈ、曲角φｖ、縮角φｕによって構成される。区間助変数は、「クロソイドの縮角多項式」の逆解を次の第一演算処理乃至第五演算処理からなる手順で解くことで高速に求めることができる。すなわち、始点と終点との位置の差から、弦の長さと方向角とを算出し（第一演算処理）、始点と終点とのそれぞれ接線方向角の差から縮角多項式の係数を算出し（第二演算処理）、ｙの縮角多項式をニュートン法によって解いて縮角を算出し（第三演算処理）、上記縮角とｘの縮角多項式とを使って弧長を算出し（第四演算処理）、接線方向角の差と縮角とから曲角を算出（第五演算処理）する。尚、上記第三演算処理については、ニュートン法逆解のために関節近似式を使うこともできる。
【００３４】
次いで、第四行程に進み、各点のうちの両端を除く中間点での曲率差評価値を求め（第六演算処理）、これら曲率差評価値の最大の点をマ一クし（第七演算処理）、最大点の曲率差評価値が許容範囲内にあるか否かを判断し、該曲率差評価値がこの許容範囲内にあれば第四行程を終了し（第八演算処理）、そうでなければ、上記最大点の接線方向角を修正し（第九演算処理）、最大点の前後２区間の区間助変数を再計算し（第十演算処理）、最大点及び前後点の３点での曲率差評価値を再計算し（第十一演算処理）た後、第二演算処理に戻って繰り返す。
【００３５】
これにより、最終的にすべての点での曲率差評価値を予め与えた許容差以下にすることができる。
【００３６】
上述のような処理は、制御手段であるＭＰＵとして一般に使用されているインテル社製のＭＰＵ（ペンティアム）なら、与えられた点列がｌ００点であっても１秒以内で終了する。したがって、ロボットやＮＣの運動と同時に（リアルタイムで）演算可能である。入力する点列によっては、一箇所の接線方向角を修正すると両側の曲率差が大きくなり、収束するのに多くの時間を要したり、著しい場合には収束しない場合もあると考えられる。このような場合、新たに、点を補うことにより対処する。又、２点の接線方向角差がｌ回転を超えたり、進行方向と目標点との方向差がｌ回転を超えたりすると、当初の接線方向角から区間助変数を算出する逆解が解けなくなってしまう。このような場合も点を補うことにより対処する。
【００３７】
続く第五行程においては、上述したような第四行程で得られた区間助変数を分割することによって積和演算に適した分割助変数を算出する。そして、これら分割助変数に基づいて順次位置を求める。これにより、上記点列間を補間するのに最適な位置指令を得ることができる。本形態例に係る軌跡制御方法は、上述したようなクロソイドセグメントを求める理論を、マイクロコンピュータ等の制御手段を用いて実施し、最適なクロソイドセグメントを算出して補間する。
【００３８】
具体的な構成としては、図１のフローチャートに示すとおり、先ず、補間すべき点列Ｐｉ（ｘｉ、ｙｉ）（但し、ｉは０、１、２、…、ｎ）を入力する（ステップ１）。尚、このステップ１は、特許請求の範囲に記載した第一行程に対応する。また、上記点列の入力は、図示しないキーボード、ディジタイザ等の入力手段を介して行うことも、ファイルや通信から読み込むこともできる。
【００３９】
次いで、特許請求の範囲に記載した第二行程に対応するステップ２により、上記各点Ｐｉにおける接線方向角φｉの初期値を求める。本ステップ２における解法手段の一例を以下に示す。図３に示すように、連続した３点を選択し、これら３点（図３のＡ，Ｂ，Ｃ）を通る円弧の各点における接線方向角φA 、φB 、φC を求める。三角形ａ＋ｂ＝ｃの各辺の角度をθa,θb,θc とすると、頂点の角度αは、
α＝θc −θa
β＝π−θb ＋θa
γ＝θb −θc
３点を通る円弧の各点での接線方向は、円周角と弦弧角が等しいので、
φA ＝θa −γ＝θa −θb ＋θc
φB ＝θb −α＝θb −θc ＋θa
φC ＝θc −β＋π＝θc −θa ＋θb
で与えられる。
【００４０】
上述のような理論により、各点での接線方向角φが順次求められる。尚、本明細書においては上述した解法を「３点円弧」と称する。ｉ＝１からｎ−１までの（ｎ−１）点に対し、φB が計算できる。ｉ＝０，ｉ＝ｎの端点については、別途入力することもありうるが、簡単には、ｉ＝１でのφA をｉ＝０に、ｉ＝ｎ−１でのφC をｉ＝ｎに使ってもよい。
【００４１】
次いで、特許請求の範囲に記載した第三行程に対応するステップ３に進み、各区間の区間助変数を求める。区間助変数は、２点間を結ぶ曲線の弧長ｈ、曲角φv 、縮角φu によって構成される。区間助変数を高速に求めるためには、以下の「クロソイドの縮角多項式表現」が使われる。
【００４２】
Ｐ1 ＝Ｐ0 ＋ｈ・Σｃn[ｎ] ・ｅｊ（φn[ｎ])・φu^n   ｎ＝０・・・ （４）
係数の大きさ    ｃn[ｎ] ＝Σｃnm[ ｍ]     ｍ＝０…
ここで、ｃnm[ ｍ] ＝ｗ^m／（２ｍ＋１）！／Π（４ｍ＋４ｋ＋２）
ｋ＝１..ｎ
ｗ＝−ｖ^2
ｖ＝（φ1 −φ0 ）／２
係数の方向      φn[ｎ] ＝（φ0 ＋φ1 −ｎ・π）／２
この式の証明は詳述しないが、クロソイドセグメントの基本式（２）で変数をＳ＝０..１ではなく、Ｔ＝−１..１と置き換えて両振り無次元化し、マクローリン展開、二項展開したのちに積分して得られる。逆解のために弦の長さｒ、弦の方向角θを使って変形し、スカラ分解すると
ｒ／ｈ＝Σｘn[ｎ] ・φu^n     ｎ＝０...
０＝Σｙn[ｎ] ・φu^n     ｎ＝０...
ｘn[ｎ] ＝ｃn[ｎ] ・cos(ψn[ｎ])    ｎ＝０...
ｙn[ｎ] ＝ｃn[ｎ] ・sin(ψn[ｎ])    ｎ＝０...
ψn ＝（φ0 ＋φ1 −ｎ・π）／２−θ
となる。これらの式を使って次の手順で逆解を解く。
【００４３】
最初に、出発位置（Ｐ0 ＝ｘ0 ＋ｊ・ｙ0 ）と到着位置（Ｐ1 ＝ｘ1 ＋ｊ・ｙ1 ）との差から、弦の角度θと長さｒとを求める（第一演算処理）。
θ＝ａ・tan （（ｙ1 −ｙ0 ）／（ｘ1 −ｘ0 ））
ｒ＝ｘ・cos θ＋ｙ・sin θ
【００４４】
次いで、出発接線方向角φ0 と到着接線方向角φ1 と弦の角度θとから、初期計算４式
ψn ＝（φ0 ＋φ1 ）／２−θ
ｗ＝−（φ1 −φ0 ）^2／４
ｃnm [０] ＝１
ｃnm [ｍ] ＝ｃnm [ｍ−１] ＊ｗ／２ｍ／（２ｍ＋１）
ｍ＝１..ｍmax   ｃnm [ｍmax]＜δ／ｒ
を計算する。次いで以下５式をｎ＝０から始めて、ｃn[ｎmax]＜δ／ｒになるまで繰り返す。
ｃn[ｎ] ＝Σｃnm      ｍ＝０..ｍmax
ｘn[ｎ] ＝ｃn[ｎ] ＊cos ψn
ｙn[ｎ] ＝ｃn[ｎ] ＊sin ψn
ｃnm [ｍ] ＝ｃnm[ ｍ−１] ／（４ｍ＋４ｎ＋２）    ｍ＝０..ｍmax
ψn ＝ψn −π／２
これが第二演算処理である。
【００４５】
次いで、ｙの縮率多項式
Σｙn[ｎ] ・φu^n ＝０        ｎ＝０..ｎmax
をニュートン法で解いてφu を求める（第三演算処理）。すなわち適当なφu を初期値として、
Ｅr ＝Σｙn[ｎ] ・φu^n       ｎ＝０..ｎmax
を計算し、許容誤差δ＞ |Ｅr|であれば第三演算処理完了する。そうでなければ、
φu ＝φu −Ｅr ／Σ｛ｎ・ｙn[ｎ] ・φu^（ｎ−１）｝
ｎ＝１..ｎmax
として、再度Ｅr を計算する。
【００４６】
次いで、ｘの縮率多項式を使って
ｈ＝ｒ／Σ｛ｘn[ｎ] ・φu^n ｝    ｎ＝０..ｎmax
でｈを求める（第四演算処理）。
【００４７】
最後に、曲角を計算する（第五演算処理）。
φv ＝φ1 −φ0 −φu
ニュートン法は２次の収束をするので大変効率がよい。係数の性質がよいので発散することもない。求めるφv 、φu の領域を挟く指定することにより、より一層、高速になる。現在使用されている代表的なＭＰＵであるペンティアム（例えば１６６ＭＨｚ）であれば、１００μｓのオーダーである。尚、図１のステップ３に係る解法を、「回旋逆解」と称する。上記「回旋」とはクロソイド曲線の意である。回旋逆解は、次のステップ４４でも使われる。
【００４８】
ニュートン法の効率を左右するのは、適切な初期値の選択である。次の「クロソイドの関節近似式」によれば、高精度の初期値が得られる。この式の証明も詳述しないが、この式をマクローリン展開して、縮率の多項式にして（４）と比較すると０次から２次まで完全に一致し３次項の係数の差がｈ／１２６００より小さいことがわかる。
【００４９】
0004146549.tif 000004
ｋ＝２＊ｃn[２] ／ｃn[１]
ｂ＝ｃn[１] ／ｋ
ａ＝ｃn[０] −ｂ
誤差は、 h・φu^3 ／１２６００で評価される。例えば、ｈが１０００ｍｍ、φu が１ｒａｄとして、誤差は８μｍ以下である。弦の長さｒと角度θとを利用して変形し、スカラ分解すると、ψ＝（φ0 ＋φ1 ）／２−θとして
ｒ／ｈ≒ａ・cos ψ＋ｂ・cos(ψ−ｋ・φu ）
０≒ａ・sin ψ＋ｂ・sin （ψ−ｋ・φu ）
となるから、
φu ≒｛ａ・sin （ａ・sin ψ／ｂ）＋ψ｝／ｋ
がきわめてよい近似を与えることになる。
【００５０】
更に、特許請求の範囲に記載した第四行程に対応するステップ４に進む。このステップ４を構成するステップ４１（特許請求の範囲に記載した第六演算処理に対応する。）により、各中間点の曲率差評価値を次の式で求める。中間点１での区間０の曲率をφ'10 、区間１の曲率をφ'11 とすると、
（φ'10 −φ'11 ）・ｈ0 ・ｈ1 ／２
これは、それぞれに反対側の曲率を採用したときの位置誤差の相乗平均になっている。位置の次元であるから精度の判断がしやすい。
【００５１】
そして、このステップ４１で、最大点をマークする（特許請求の範囲に記載した第七演算処理に対応する) 。次の第八演算処理に対応するステップ４２は、上記最大点でのこの値の絶対値を与えられた許容値と比較するもので、最大点が許容値以下なら第四行程完了である。このステップ４２で上記最大点が上記許容値より大きければ、次のステップ４３に進み、上記最大点での接線方向角を修正する（特許請求の範囲における第九演算処理）。修正角度は、曲率差評価値を両側の弧長の相乗平均と４とで割った値で、
(φ'10 −φ'11 ）・sqrt（ｈ0 ・ｈ1 ）／８
となる。これによって、最大点の曲率差はほぼ０になるが、前後点の曲率にはあまり響かないことがわかっている。
【００５２】
更に、特許請求の範囲に記載した第十演算処理に対応するステップ４４で、前記ステップ３と同様の解法（回旋逆解）を２回だけ実行し、最大点の前後区間の区間助変数を求める。
【００５３】
更に、特許請求の範囲に記載した第十一演算処理に対応する演算処理を行う。すなわち、上記ステップ４１に戻り、このステップ４１の処理と同じ式を３回だけ実行し、最大点と前後点の曲率差評価値を計算したのち、再びステップ４２を行う。こうして、ステップ４３の判定で評価値が許容範囲内に収まるまで、ステップ４１からステップ４４の各ステップから成るステップ４を繰り返す。
【００５４】
次に、特許請求の範囲に記載した第五行程に対応するステップ５を構成するステップ５１に進み、始点と接線方向角（ｘ0 ，ｙ0 ，φ0 ）、区間助変数（ｈ，φv ，φu ）から、歩進助変数（ｄｕ，ｄｖ，ｄｘ，ｄｙ，ｖｘ，ｖｙ，ｕｘ，ｕｙ）を求める。
はじめに分割数ｎを計算する。ここでは、後段が直線機能を持っているケースの例をとりあげる。当然点機能しかないときは、もっと多く分割し、円弧機能があればもっと少なく分割する。クロソイド機能があれば、ｎ＝１で分割不要である。分割数は、近似した直線（弦）と元の曲線（弧）との差が所要誤差δ以内になるように決める。曲率の大きいところは短く、小さいところは長く可変長で分割すれば、分割数が最小になるが、計算の単純化のため、「等弧長分割」を採用する。
そこで、φ'0またはφ'1の絶対値のうちの大きい方のφ'maxを曲率とし、弧長ｈの円弧を想定し、これをｎ分割したときの誤差を評価する。
【００５５】
ここで、
φ'0＝（φ1 −φ0 −φu ）／ｈ
φ'1＝（φ1 −φ0 ＋φu ）／ｈ
を利用すれば
φ'max＝（ |φ1 −φ0|＋ |φu|）／ｈ
となる。
δ＝｛１−cos （φ'max＊ｈ／ｎ／２）｝／φ'max
cos θ＝１−θ^2／２！＋θ^4／４！... であるから
δ＜＝φ'max＊（ｈ／ｎ）^2／８
整数切りあげ記号として−［−ａ］を使えば、
ｎ＝−［−ｈ＊sqrt（φ'max／δ／８）］
である。
【００５６】
細分化された区間の助変数（分割助変数）は、ｄｈ＝ｈ／ｎによって次のように計算すればよい。
ｄｕ，ｕｘ，ｕｙは定数である。
ｄｕ＝φu ／ｎ^2
ｕｘ＝cos （ｄｕ）
ｕｙ＝sin （ｄｕ）
ｄｖ，ｖｘ，ｖｙ，ｄｘ，ｄｙは変数の初期値である。
ｄｖ＝φ'0／２＊ｄｈ＋ｄｕ／２
ｖｘ＝cos （ｄｖ）
ｖｙ＝sin （ｄｖ）
ｄｘ＝ｄｈ＊cos （φ0 ＋ｄｖ）
ｄｙ＝ｄｈ＊sin （φ0 ＋ｄｖ）
【００５７】
最後に、ステップ５２で上記分割助変数を歩進し、順次位置を得る。
ｘ＝ｘ        ＋ｄｘ＊ｖｙ／ｄｖ      （７）
ｙ＝ｙ        ＋ｄｘ＊ｖｙ／ｄｖ
ｗ＝ｄｘ＊ｖｘ−ｄｙ＊ｖｙ
ｄｙ＝ｄｘ＊ｖｙ＋ｄｙ＊ｖｘ
ｄｘ＝ｗ
ｄｖ＝ｄｖ      ＋ｄｕ
ｗ＝ｖｘ＊ｕｘ−ｖｙ＊ｕｙ
ｖｙ＝ｖｘ＊ｕｙ＋ｖｙ＊ｕｘ
ｖｘ＝ｗw
ｗ＝ｄｘ＊ｖｘ−ｄｙ＊ｖｙ
ｄｙ＝ｄｘ＊ｖｙ＋ｄｙ＊ｖｘ
ｄｘ＝ｗ
を繰り返す。
【００５８】
オリジナルの（３）が６個の和（差）と８個の積によって歩進させたのに対し、９個の和（差）と１４個の積（商）で歩進させる。演算量は、ほぼ倍に近いが、精度は桁ちがいに向上している。区間の弧の長さと弦の長さの比率を考慮に入れたこと、区間を半分にして曲率を端で切り替え、接線方向を区間中央で切り替えるようにしたことが精度向上にあずかっている。
【００５９】
このようにして、順次すべての区間についてクロソイドセグメントで補間する。上述のように構成される本形態例に係る軌跡制御方法を用いれば、最適のクロソイド曲線を容易に且つ高速に得ることができ、要求水準に見合う補間制御を行える。尚、上記各行程（演算処理を含む）はマイクロコンピュータ等の制御手段が演算処理を施すことによって行う。このような制御手段を備えることにより、自動組付け装置や描画装置等における補間制御等の制御を容易且つ円滑に行えるようになり、補間制御を行う種々の分野で有効な効果を奏する。
【００６０】
尚、上記軌跡制御方法は、上記制御器に直接記憶させる他、フレキシブルディスク（いわゆるＦＤ）や光磁気ディスク（いわゆるＭＯ）或いはＣＤ−ＲＯＭ等の従来知られた種々の記録媒体に記憶させておくことができる。このようにしておくと、本形態例に係る軌跡制御方法を、当該軌跡制御方法を実施すべき装置に付設の制御器に読み込ませ、該装置に実施させることが容易になる。又、販売も容易に行うことが可能になる。これら記録媒体に記録する際の言語に就いては、Ｃ言語、ＢＡＩＳＩＣ言語、アセンブラ言語等、特には問わない。
【００６１】
本形態例に係る軌跡制御方法は、上述したクロソイドセグメントの作成を、予め与えられた点列（予め制御手段に入力された多数の点から成る点列）に基づいて制御手段によって行わせ、上記点列間を高速に補間する。従って、ＣＰ制御に係る被制御部材を円滑且つ高速に制御することを可能としている。
【００６２】
さらに付言するならば、理論式から計算で決まった点列を補間するときは、同時に各点での接線方向角も計算しておき、第二行程、第四行程をパスすることができる。また、曲率連続を要求しないときは、第四行程をパスすることができる。さらに、演算精度が低くてよいときは、第三行程に代えてはじめから関節近似式を使うことができる。このとき、ｃn[ｎ] は（４）の級数式によらず、次のように三角関数から演算することができる。
−ｗ／６＜＝δのとき
ｃn[０] ＝１
それ以外の場合、
ｃn[０] ＝sin （ｖ）／ｖ
ｗ^2／８４０＜＝δのとき
ｃn[１] ＝（１＋ｗ／１０）／６
それ以外の場合、
ｃn[１] ＝（cos （ｖ）−ｃn[０] ）／ｗ／２
−ｗ^3／４９８９６０＜＝δのとき
ｃn[２] ＝（１＋（１＋ｗ／３６）＊ｗ／１４）／６０
それ以外の場合、
ｃn[２] ＝（ｃn[０] −６＊ｃn[１] ）／ｗ／４
【００６３】
【発明の効果】
この発明に係る軌跡制御方法は、上述のように構成され作用するため、補正すべき多数の点をすべて厳正点として与えることが可能であり、又、もし厳正点として与える必要のない場合には、適当な誤差を設定することによりフェアリングを行うことができる。
【００６４】
しかも、得られたクロソイド曲線は、曲率の小さい滑らかな曲線であり、予期しないうねり（あばれ）の出ない補間が得られる。更に、曲率の計算が容易であり、曲率、従って求心加遠度の値を制御することができるとともに、直線補間、円弧補間を接続して用いることができる。
【００６５】
又、曲線の長さの計算が容易であり、等速制御が可能である。等速制御を行う場合、最大曲率から最大加速度を求めることができる。更に、曲線に沿って加減速を与えることが容易になる。これらの結果、アーク溶接、接着剤塗布、シール材塗布、文字描画等の分野に利用される、高速なＣＰと称される途中経過点の軌跡を要求するロボットや数値制御機械等を、端末条件ではなく任意の点列を与えることで、円滑で正確、且つ高速に制御できる。
【００６６】
以上のような効果があるため、この発明に係る軌跡制御方法を用いて軌跡を制御することによって、溶接、塗装、加工、作図等の仕事を容易且つ効果的に行うことができる。又、複雑な曲線を少ないデータによって記述することが可能となるので、曲線の登録（ドキュメンテーション）及び正確な再現を容易にする効果がある。更に、曲率φ' の二乗をｓに関して積分することで表される歪エネルギが小さいために、前述した先発明に比較して被制御部材の作動が滑らかである。
【図面の簡単な説明】
【図１】この発明に係る軌跡制御方法を示すフローチャートである。
【図２】接線方向角の初期値を得る際の方法について説明するための略図である。
【図３】同じく３点円弧法について説明するための略図である。
【図４】従来から知られた軌跡制御補間を行うための制御部の構成の一例を示すブロック図である。
【図５】従来から知られた軌跡制御並びに先発明に係る軌跡制御を説明するための略図である。
【図６】各種補間法における表現形式を示している。
【図７】各種補間法における概形を示している。
【図８】基本クロソイドを示す線図である。
【図９】クロソイド創生装置を説明するための略図である。
【符号の説明】
１  制御手段
【図１】
0004146549.tif 000005
【図２】
0004146549.tif 000006
【図３】
0004146549.tif 000007
【図４】
0004146549.tif 000008
【図５】
0004146549.tif 000009
【図６】
0004146549.tif 000010
【図７】
0004146549.tif 000011
【図８】
0004146549.tif 000012
【図９】
0004146549.tif 000013