JP4085418B2

JP4085418B2 - 曲線補間加減速制御方法

Info

Publication number: JP4085418B2
Application number: JP31644698A
Authority: JP
Inventors: 友之関山; 紳一郎柳
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 1998-11-06
Filing date: 1998-11-06
Publication date: 2008-05-14
Anticipated expiration: 2018-11-06
Also published as: JP2000148223A

Description

【０００１】
【発明の属する技術分野】
本発明は、工作機械やロボットなどを制御する数値制御装置において、制御対象を動作させる際に曲線補間して軌道計算する方法に係るものであり、特に、曲線補間加減速制御方法に関する。
【０００２】
【従来の技術】
従来の工作機械などに用いられる数値制御装置において、制御対象の軌道を求める際に用いられる補間計算方法は、ある地点での目標速度を実現するために、ある目標地点までの距離を計算してそれを基に加減速制御をする方法がとられていた。ところがNURBS曲線などの自由曲線では、曲線上の距離の計算が困難であるという問題があった。
この対策として例えば特開平７−６４６２０号公報に開示されているように、残りの距離の評価をする際に、現在位置から曲線の終点までの直線距離を基にして加減速度を決めるという方法が開発されている。
【０００３】
【発明が解決しようとする課題】
ところが、上記の特開平７-６４６２０号公報に開示された方法では、図１２のような場合に曲線上の残りの距離を過小評価してしまい、早い段階で減速を始めて、十分な移動速度が得られないという欠点があった。
そこで、本発明は、残りの距離の評価に、現在補間点から曲線の曲率半径極小点までの直線距離、または前記現在補間点が前記曲線上の最後の曲率半径極小点に到達している場合は前記現在補間点から前記曲線の終点までの直線距離を用いることによって、曲線の形状に適した速い速度で移動するなめらかな加減速度を得る方法を提供することを目的とする。
【０００４】
【課題を解決するための手段】
そこで本発明の曲線補間加減速制御方法は、曲線データを入力する指令入力部と、そのデータから曲率半径と曲率半径極小点を求める曲率評価部と、目標速度設定部と、減速目標点設定部と、加減速設定部とからなる数値制御装置において、以下の手順を踏んで曲線の補間計算をすることを特徴としたのである。（１）送り速度データとNURBS 曲線データと指令加速度を入力し、（２）前記曲線の曲率半径極小点をすべて求め、（３）前記送り速度と、前記曲率半径極小点での曲率半径と、前記指令加速度とを用いてそれぞれの前記曲率半径極小点での曲率限界速度を求め、（４）前記曲率半径極小点すべてと前記曲線上の終点を減速目標点として設定し、（５）前記曲率限界速度を用いて前記減速目標点における目標速度を決定し、（６）前記減速目標点のうち、前記曲線の終点に向かってある時刻における現在補間点に最も近い点を現在減速目標点とし、（７）前記現在補間点と前記現在減速目標点との間の直線距離と、前記現在減速目標点における目標速度と、前記指令加速度を用いて前記時刻における補間速度を決定し、（８）前記NURBS 曲線データと前記補間速度を用いて前記時刻の１制御周期後時刻における補間点を決定し、（９）前記補間点が前記現在減速目標点に到達するまで（７）、（８）を繰り返し、前記補間点が前記現在減速目標点に到達し、（１０）もし前記現在減速目標点が前記曲線の終点でないならば次減速目標点を新たに現在減速目標点に設定し、（１１）前記補間点が前記曲線の終点に到達するまで前記ステップ（７）〜（１０）を繰り返す。
このようにすることにより、従来よりも残り距離の評価が正しく行われるとともに、減速が必要である曲率半径極小点における目標速度が設定でき、曲線の形状に適した速い速度で移動するなめらかな加減速制御を行えるようになる。
また、請求項２に記載の曲線補間加減速制御方法は、前記現在減速目標点で目標速度を決定する際は、前記現在減速目標点での曲率限界速度と、現在減速目標点と前記曲線の終点に向かって現在減速目標点に最も近い次減速目標点と前記指令加速度から決まる前記現在減速目標点における許容最高初速度とを用いて決定することを特徴としたのである。
このようにすることにより、現在減速目標点における目標速度を決定する際に許容最高初速度を評価するため、現在補間点から現在減速目標点を経てその次の減速目標点までスムーズに加減速可能となり、軌跡が滑らかな曲線補間が実現される。
さらに、請求項３に記載の曲線補間加減速制御方法は、前記現在減速目標点で目標速度を決定する際は、以下の手順を踏むことを特徴としたのである。（１）曲線の１次微分と２次微分が連続になるように滑らかに接続される連続した複数の前記曲線補間データを入力し、（２）曲線の１次微分と２次微分が連続になるように滑らかに接続される連続した曲線を生成し、（３）曲線の１次微分と２次微分が連続になるように滑らかに接続された各前記曲線の終点が曲率半径極小点でない場合は、前記終点を始点とする曲線の１次微分と２次微分が連続になるように滑らかに接続された次曲線上の最初の曲率半径極小点を減速目標点として前記終点と置き換える。
このようにすることにより、曲線の１次微分と２次微分が連続になるように滑らかに接続される連続した複数のNURBS 曲線データを逐次入力しながら減速目標点を再評価し、曲率半径極小点ではない前記曲線終点を曲率半径極小点と置き換えることにより、曲率半径極小点ではない曲線接続点で無駄に減速することがなくなり、入力された曲線群全体にわたってより高速な補間が実現される。
【０００５】
【発明の実施形態】
以下、本発明の曲線補間加減速制御方法を図を用いて説明する。図１は本発明の方法を実施するための装置の構成を示すブロック図である。図において、１は指令入力部であり、tをパラメータとして表されたNURBS曲線などの曲線データP（t）と、送り速度ＶF、指令加速度αを読み込む。２は曲率評価部であり、前記曲線上の各点での曲率半径ρ（t）と曲率半径極小点を求め、その曲率半径極小点のすべてと前記曲線の終点を減速目標点とする。３は目標速度設定部であり、前記曲率半径ρ（t）と前記指令加速度αから決まる曲率限界速度Ｖｃを計算し、前記曲率限界速度Ｖｃと前記送り速度ＶFを用いて減速目標点での目標速度を設定する。４は減速目標点設定部であり、前記減速目標点のうち現在補間点から前記終点に向かって前記現在補間点に最も近い減速目標点を現在減速目標点に設定する。５は加減速設定部であり、制御周期毎に前記現在補間点から前記現在減速目標点までの直線距離ｌｒと前記現在減速目標点における目標速度と、前記指令加速度αを用いて補間速度を決定し、前記補間速度を出力し、現在補間点が現在減速目標点に到達したら次の減速目標点を現在減速目標点として終点に到達するまで繰り返す。
【０００６】
次に本発明の方法による処理の流れを図４と図５のフローチャートを用いて説明する。まずtをパラメータとして表された曲線データP（t）と、送り速度ＶF、指令加速度αが指令入力部１を通じて読み込まれ、次の曲線データを読み込む必要があればそれを読み込む（Ｓ１、Ｓ２）。曲率評価部２はパラメータtにおける曲線上各点での曲率半径ρ（t）を計算して曲率半径極小点を求め（Ｓ３）、前記曲率半径極小点すべてと曲線の終点を減速目標点とする（Ｓ４）。目標速度設定部３は前記の各減速目標点での目標速度を設定する。このとき前記の各減速目標点での曲率半径ρ（t）と指令加速度αから式（１）を用いて求められる曲率限界速度Ｖｃ
Ｖｃ（t） =√[α・ρ（t）] …（１）
と（Ｓ５）、送り速度ＶFのうち小さい方（Ｓ６）を減速目標点での目標速度として設定し（Ｓ７）、さらに曲線の終点での指令速度を設定する（Ｓ８）。減速目標点設定部４は、前記現在補間点が前記現在減速目標点に到達していれば（Ｓ９）、次の減速目標点を前記現在減速目標点として設定する。すなわち前記減速目標点に曲線の始点に近い方から順に１, ２, …, i,（i+１）, …と番号をつければ、前記現在補間点がi番目の前記減速目標点に到達していれば（Ｓ１０）、（i+１）番目の前記減速目標点を前記現在減速目標点とする（Ｓ１１）。このようにして、前記減速目標点設定部４は、減速目標点のうち前記曲線の終点に向かって現在補間点に最も近い点を現在減速目標点として設定する。加減速制御部５は現在補間点から現在減速目標点までの直線距離を計算し、残り距離とする（Ｓ１２）。残り距離、現在減速目標点での目標速度、指令加速度から減速が必要であると判断できるならば、前記目標速度設定部が設定した現在減速目標点での目標速度を実現するような減速パターンに沿って残り距離に応じた補間速度を決定し（Ｓ１３）、減速が必要でなければ送り速度ＶFを補間速度として、出力セグメントデータを生成し出力する（Ｓ１４）。そしてセグメントデータ出力後に到達する曲線上の位置を現在補間点とし（Ｓ１５）、以降、以上の手順を繰り返す。
【０００７】
図６は本発明の請求項２における第２の補間方法の概念を説明する図である。図において、６１は補間曲線、６２は曲線始点、６３は曲率半径第１極小点、６４は曲率半径第２極小点、６５は曲線終点、６６は曲率半径極小点間直線距離、６７は曲率半径極小点−終点間直線距離である。この図において、曲率半径極小点間直線距離６６と曲率半径第２極小点６４における目標速度によって、曲率半径第１極小点６３における許容最高初速度は決定される。同様に、曲率半径極小点−終点間直線距離６７と曲線終点６５における目標速度によって、曲率半径第２極小点６４における許容最高初速度は決定される。
図７は請求項２の第２の補間方法における目標速度の設定を説明する概念図である。図において、７１は曲線始点、７２は曲率半径第１極小点、７３は曲率半径第２極小点、７４は曲線終点、７５は曲率半径第１極小点における曲率限界速度、７６は曲率半径第２極小点における曲率限界速度、ＶＦは指令送り速度、７８は曲率半径第２極小点での速度と第１・第２極小点間直線距離によって決まる曲率半径第１極小点における許容最高初速度、７９は曲線終点での速度と第２極小点終点間直線距離によって決まる曲率半径第２極小点における許容最高初速度、７aは曲線各点における曲率限界速度グラフである。通常曲率半径極小点における曲率限界速度のみを考慮した場合、補間速度は例えば曲率半径第２極小点における曲率限界速度７６を実現されるように生成される。ところが、曲線終点での速度と第２極小点終点間直線距離によって決まる曲率半径第２極小点における許容最高初速度７９も考慮した場合、本当に滑らかに補間するには、両者の小さい方、ここでは許容最高初速度７９を実現しなければならないことがわかる。さらに、７８は曲率半径第２極小点での速度と第１・第２極小点間直線距離によって決まる曲率半径第１極小点における許容最高初速度７８も考慮して、第１・第２曲率半径極小点間の速度パターンが生成される。
【０００８】
図８は請求項２の第２の補間方法の手順を示すフローチャートである。請求項１との違いは、曲率限界速度Ｖｃ、送り速度ＶFに加えて許容最高初速度とも比較し、目標速度を決定する点である（Ｓ２７）。
図９は本発明の請求項３における第３の補間方法を説明する概念図である。図において、９１は第一補間曲線、９２は第二補間曲線、９３は第一補間曲線曲率半径極小点、９４は第一補間曲線終点（第二補間曲線始点）、９５は第二補間曲線曲率半径極小点、９６は第二補間曲線終点、９７は第一補間曲線曲率半径極小点９３における減速目標点を第一補間曲線終点にしていたとき、第一補間曲線曲率半径極小点９３における目標速度を決定する際に評価する減速距離、９８は第一補間曲線曲率半径極小点９３における減速目標点を第二補間曲線曲率半径極小点に変更したとき、第一補間曲線曲率半径極小点９３における目標速度を決定する際に評価する減速距離である。この図において、第二補間曲線の入力前は、第一補間曲線曲率半径極小点９３の次減速目標点は第一補間曲線終点９４であったが、第二補間曲線の入力後は第二補間曲線曲率半径極小点９５に変更されていることを示している。
【０００９】
図１０は請求項３の第３の補間方法における目標速度の設定を説明する概念図である。図において、１０１は第一補間曲線始点、１０２は第一補間曲線曲率半径極小点、１０３は第一補間曲線終点（第二補間曲線始点）、１０４は第二補間曲線曲率半径極小点、１０５は第二補間曲線終点、ＶＦは指令送り速度、１０７は第一補間曲線曲率半径極小点における曲率限界速度、１０８は第二補間曲線曲率半径極小点における目標速度、１０９は第一補間曲線終点を減速目標点としていたときの速度グラフである。この図において、第二補間曲線の入力前は、例えば第一補間曲線終点１０４の目標速度が０であった場合、生成される速度パターンは第一補間曲線終点を減速目標点としていたときの速度グラフ１０９のようであるが、第二補間曲線の入力後は、第一補間曲線曲率半径極小点における曲率限界速度１０７をとおる速度パターンが生成される。
図１１は請求項３の第３の補間方法の手順を示すフローチャートである。請求項２との違いは、目標速度設定部において次曲線の始点が曲率半径極小点であるかどうかを評価し、極小点であれば以前の設定速度をそのまま利用し、そうでない場合は終点を減速目標点から削除する点である。
【００１０】
【発明の効果】
以上説明したように、請求項１の発明によれば、従来よりも残り距離の評価が正しく行われるとともに、減速が必要である曲率半径極小点における目標速度が設定でき、曲線の形状に適した速い速度で移動するなめらかな加減速制御を行えるようになる。
請求項２の発明によれば、各減速目標点での目標速度をより高速に設定可能、かつ現在補間点から現在減速目標点を経てその次の減速目標点までスムーズに加減速可能となり、より高速にかつ軌跡が滑らかな曲線補間が実現されるという効果がある。
請求項３の発明によれば、曲率半径極小点ではない曲線接続点で無駄に減速することがなくなり、入力された曲線群全体にわたってより高速な補間が実現されるという効果がある。
【図面の簡単な説明】
【図１】実施例における数値制御装置の構成図
【図２】本発明請求項１の方法の概念図（その１）
【図３】本発明請求項１の方法の概念図（その２）
【図４】本発明の方法の手順を示すフローチャート（その１）
【図５】本発明の方法の手順を示すフローチャート（その２）
【図６】請求項２の方法の概念を説明する図
【図７】請求項２の方法の目標速度の設定を説明する図
【図８】請求項２の方法の手順を示すフローチャート
【図９】請求項３の方法の概念を説明する図
【図１０】請求項３の方法の目標速度の設定を説明する図
【図１１】請求項３の方法の手順を示すフローチャート
【図１２】従来技術の方法の概念を説明する図
【符号の説明】
１指令入力部
２曲率評価部
３目標速度設定部
４減速目標点設定部
５加減速制御部

Claims

NURBS曲線データを入力する指令入力部と、そのデータから曲率半径と曲率半径極小点を求める曲率評価部と、目標速度設定部と、減速目標点設定部と、加減速設定部とからなる数値制御装置において、以下の手順を踏んで曲線の補間計算をすることを特徴とする曲線補間加減速制御方法。
（１）送り速度データとNURBS 曲線データと指令加速度を入力し、
（２）前記曲線の曲率半径極小点をすべて求め、
（３）前記送り速度と、前記曲率半径極小点での曲率半径と、前記指令加速度とを用いてそれぞれの前記曲率半径極小点での曲率限界速度を求め、
（４）前記曲率半径極小点すべてと前記曲線上の終点を減速目標点として設定し、
（５）前記曲率限界速度を用いて前記減速目標点における目標速度を決定し、
（６）前記減速目標点のうち、前記曲線の終点に向かってある時刻における現在補間点に最も近い点を現在減速目標点とし、
（７）前記現在補間点と前記現在減速目標点との間の直線距離と、前記現在減速目標点における目標速度と、前記指令加速度を用いて前記時刻における補間速度を決定し、
（８）前記NURBS 曲線データと前記補間速度を用いて前記時刻の１制御周期後時刻における補間点を決定し、
（９）前記補間点が前記現在減速目標点に到達するまで（７）、（８）を繰り返し、
前記補間点が前記現在減速目標点に到達し、
（１０）もし前記現在減速目標点が前記曲線の終点でないならば次減速目標点を新たに現在減速目標点に設定し、
（１１）前記補間点が前記曲線の終点に到達するまで前記ステップ（７）〜（１０）を繰り返す。
前記現在減速目標点で目標速度を決定する際は、前記現在減速目標点での曲率限界速度と、現在減速目標点と前記曲線の終点に向かって現在減速目標点に最も近い次減速目標点と前記指令加速度から決まる前記現在減速目標点における許容最高初速度とを用いて決定することを特徴とする請求項１記載の曲線補間加減速制御方法。
前記現在減速目標点で目標速度を決定する際は、以下の手順を踏むことを特徴とする請求項１記載の曲線補間加減速制御方法。
（１）曲線の１次微分と２次微分が連続になるように滑らかに接続される連続した複数の前記NURBS 曲線データを入力し、
（２）曲線の１次微分と２次微分が連続になるように滑らかに接続される連続した曲線を生成し、
（３）曲線の１次微分と２次微分が連続になるように滑らかに接続された各前記曲線の終点が曲率半径極小点でない場合は、前記終点を始点とする曲線の１次微分と２次微分が連続になるように滑らかに接続された次曲線上の最初の曲率半径極小点を減速目標点として前記終点と置き換える。