(57)【特許請求の範囲】
【請求項１】
制御対象物のＣＰ動作を制御するパルスモータ駆動系の軌道制御装置において、
前記制御対象物のＣＰ動作曲線に基づいて、所定のＣＰ速度に対応する曲線長データを生成する曲線長データ生成手段と、
前記曲線長データをｎ次元（ｎ≧２）の座標データに変換する座標変換手段と、
前記座標変換手段によって得られるｎ次元の各座標データを、それぞれローパスフィルタに通してスムージングする座標データスムージング手段と、
前記座標データスムージング手段より出力される座標データを速度補正用メモリに格納するメモリ格納手段と、
前記速度補正用メモリに格納した座標データを所定のタイミングで順次読み出す読み出し手段と、
前記読み出し手段によって読み出された座標データを駆動信号に変換する逆変換手段と、
を有することを特徴とするパルスモータ駆動系の軌道制御装置。
【請求項２】
前記曲線長データ生成手段は、前記ＣＰ動作曲線を所定の間隔でサンプリングするサンプリング手段と、前記サンプリング手段によって得られた線分長データに基づいてランプ信号を生成するランプ信号生成手段とを有し、ランプ信号を前記曲線長データとして前記座標変換手段に入力することを特徴とする請求項１記載のパルスモータ駆動系の軌道制御装置。
【請求項３】
前記曲線長データ生成手段と前記座標変換手段との間に、前記曲線長データ生成手段からの曲線長データをローパスフィルタを通して前記座標変換手段に入力する曲線長データスムージング手段を有することを特徴とする請求項１記載のパルスモータ駆動系の軌道制御装置。
【請求項４】
前記座標データスムージング手段の出力データより曲線長を算出する曲線長データ算出手段と、前記曲線長データ算出手段からの曲線長データをローパスフィルタを通してスムージングする曲線長データスムージング手段とを有し、
前記メモリ格納手段は、前記曲線長データ算出手段からの曲線長データと座標データスムージング手段より出力される座標データとを対応付けて前記速度補正用メモリに格納し、
前記読み出し手段は、前記曲線長データスムージング手段の出力と前記速度補正用メモリに格納した座標データとを比較し、この比較結果に基づいて前記座標データを読み出すことを特徴とする請求項１記載のパルスモータ駆動系の軌道制御装置。
【請求項５】
前記読み出し手段によって読み出された座標データに対し、スムージングによる駆動速度の低下を補正する処理を施して前記逆変換手段に入力する補正手段を有することを特徴とする請求項１記載のパルスモータ駆動系の軌道制御装置。
【請求項６】
前記パルスモータ駆動系は、少なくとも３軸関節ロボットの駆動系であり、前記ｎ次元の座標データは、少なくとも３次元空間座標データであることを特徴とする請求項１記載のパルスモータ駆動系の軌道制御装置。
【請求項７】
前記駆動信号は、ロボットの関節角信号であることを特徴とする請求項６記載のパルスモータ駆動系の軌道制御装置。
【請求項８】
前記ローパスフィルタによるフィルタ特性を変更する変更手段を有することを特徴とする請求項１記載のパルスモータ駆動系の軌道制御装置。
【請求項９】
前記フィルタ特性の変更手段は、ローパスフィルタ出力の時定数を変更する手段であることを特徴とする請求項１記載のパルスモータ駆動系の軌道制御装置。
【請求項１０】
制御対象物のＣＰ動作を制御するパルスモータ駆動系の軌道制御方法において、
前記制御対象物のＣＰ動作曲線に基づいて、所定のＣＰ速度に対応する曲線長データを生成する曲線長データ生成工程と、
前記曲線長データをｎ次元（ｎ≧２）の座標データに変換する座標変換工程と、
前記座標変換工程によって得られるｎ次元の各座標データを、それぞれローパスフィルタに通してスムージングする座標データスムージング工程と、
前記座標データスムージング工程より出力される座標データを速度補正用メモリに格納するメモリ格納工程と、
前記速度補正用メモリに格納した座標データを所定のタイミングで順次読み出す読み出し工程と、
前記読み出し工程によって読み出された座標データを駆動信号に変換する逆変換工程と、
を有することを特徴とするパルスモータ駆動系の軌道制御方法。
【請求項１１】
制御対象物のＣＰ動作曲線に基づいて、所定のＣＰ速度に対応する曲線長データを生成する機能と、
前記曲線長データをｎ次元（ｎ≧２）の座標データに変換する機能と、
前記ｎ次元の各座標データを、それぞれローパスフィルタに通してスムージングする機能と、
前記ローパスフィルタを通した座標データを速度補正用メモリに格納する機能と、
前記速度補正用メモリに格納した座標データを所定のタイミングで順次読み出す機能と、
前記速度補正用メモリより読み出された座標データを駆動信号に変換する機能と、
を実現させるためのプログラムを記録したことを特徴とするコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、パルスモータ駆動系の軌道制御装置及び方法及び記録媒体に関する。
【０００２】
【従来の技術】
従来より、サーボループを有するサーボモータ（ＤＣモータ、ＡＣモータ等）を採用したロボット等においては、滑らかなＣＰ（Continuous Path ）曲線を得るための各種のアルゴリズム（以下、スムージングアルゴリズムという）が提案されている。
図１３は、サーボモータロボットのスムージングアルゴリズムの従来例の構成を示す説明図である。
【０００３】
この種のスムージングアルゴリズムとしては、大きく分けてＣＰ動作に先立って軌道計算を行う動作前計算方式とリアルタイムに軌道計算を行うリアルタイム計算方式の２種類がある。
動作前計算方式は、軌道計算専用の高速ＣＰＵを用いて、動作前に曲線のスムージング（スプライン補間）、速度、加速度等を全て計算し、モータ、メカ能力限界に応じて修正し、最終的な位置・速度・加速度結果をメモリに展開するとともに、動作時には、このメモリを参照しながらＣＰ動作が進行する方式であり、軌道計算専用の高速ＣＰＵが必要である。
これに対して、リアルタイム計算方式は、上述のような軌道計算専用の高速ＣＰＵをもたない普通のロボットにおいて、ロボットの動作とスムージングとを同時に処理する方式であり、この従来例で説明するスムージングアルゴリズムは、リアルタイム計算方式によるものである。
【０００４】
次に、図１３に示す従来のスムージングアルゴリズムの構成について説明する。このスムージングアルゴリズムは、ＣＰ曲線のサンプリング部１０と、ランプ信号発生部２０と、３次ローパスフィルタ（ｌ−ソフトモータ）部３０と、座標変換部４０と、インバース（逆変換）部５０とから構成される。
曲線サンプリング部１０では、所定のＣＰ定速度Ｖｃｐに応じて、曲線を一定のエレメント長△ｌに切り、サンプリングする。ここで、サンプリング時間をＴｓとすると、△ｌは以下のようになる。
△ｌ＝Ｖｃｐ・Ｔｓ                                ……（従１）
【０００５】
次に、ランプ信号発生部２０は、ＣＰ定速度Ｖｃｐを発生するものであり、式（従１）に従ってサンプリング時間Ｔｓ毎に△ｌずつ移動長を増していくことにより、図１３（Ｂ）に示すようなランプ信号を生成する。また、ＣＰの全長をＬとすれば、Ｔｆ時間後にランプは水平に折れることとなる。つまり、以下のようになる。
Ｖｃｐ＝Ｌ／Ｔｆ、Ｔｆ＝Ｎ・Ｔｓ（Ｎ＝整数）      ……（従２）
なお、Ｎが整数となるように、ＴｆつまりＶｃｐを多少トリミングするが、Ｔｓは、一般に十分小さいので問題ない。
このような方式により、例えば図１４に示すように、ＣＰ曲線上で、領域毎に速度を変更することや、途中停止等を行うことも容易となる。
【０００６】
次に、３次ローパスフィルタ部３０（ｌ−ソフトモータ）は、図１５（Ａ）に示すようなランプ信号より、図１５（Ｂ）に示すような滑らかな線加減速曲線ｌを生成するものである。この３次のローパスフィルタは、伝達関数Ｇ（ｓ）＝ａ／ｓ＋ｂ／ｓ２＋ｃ／ｓ３のとき、Ｇ（ｓ）／１＋Ｇ（ｓ）で安定となる構造を有するローパスフィルタである。
そして、このフィルタ部３０による滑らかさは、フィルタ部３０の時定数を変更することにより、自由に可変できる。
【０００７】
次に、座標変換部４０は、３次ローパスフィルタ部３０の出力である曲線長ｌを、ＣＰ曲線上の空間座標Ψ（ｘ、ｙ、ｚ）に変換するものである。空間座標Ψ（ｘ、ｙ、ｚ）の各座標をｌを用いてパラメータ表示すると、任意の連続ＣＰ曲線は以下のように表現できる。
ｘ＝ｘ（ｌ）、ｙ＝ｙ（ｌ）、ｚ＝ｚ（ｌ）          ……（従３）
これは曲線が折れ線である場合も同様である。また、極座標（Ｒ軸）成分に対しても指定したい場合には、上式（従３）に、ｒ＝ｒ（ｌ）を加えればよい。
ここで、ｌ→Ψ（ｘ、ｙ、ｚ）座標変換とは、上式（従３）よりΨ（ｘ、ｙ、ｚ）を求めることとなる。したがって、この変換では、Ψ（ｘ、ｙ、ｚ）に位置誤差は発生しない。
【０００８】
次に、インバース（逆変換）部５０は、上述した空間座標Ψ（ｘ、ｙ、ｚ）をロボットの関節角Θ（θｘ、θｙ、θｚ）に変換するものである。ここでΨ＝Ｆ（Θ）とした時、Θ＝Ｆ-1・Ψを求めることとなる。
【０００９】
【発明が解決しようとする課題】
ところで、上述のようなスムージングアルゴリズムは、サーボモータロボット用のものであるが、従来は、パルスモータを採用したロボットにおいても、上述のようなスムージングアルゴリズムをそのまま採用していた。
しかしながら、制御系自体にスムージング機能（すなわち、サーボループのローパス機能）を有するサーボモータロボットに比較し、パルスモータロボットは、パルス列による軌道指令通りに駆動するものである。
したがって、このようなパルスモータロボットにサーボモータロボット用のアルゴリズムを採用した従来技術では、動作速度が低いときやＣＰ曲線自体が滑らかであれば支障がないが、動作速度が高かったり、ＣＰ曲線が折れ線であるときには、適正なＣＰ動作精度を得ることは困難であり、また機構部の振動や脱調等の点で大きな支障を生じる。
【００１０】
例えば図１６（Ａ）に示すように、直交ロボットにおける直交コーナをもったＣＰ曲線では、図１６（Ｂ）（Ｃ）に示すように、ｘ、ｙ軸関節速度Ｖｘ、Ｖｙが大きくステップジャンプする。
これに対してサーボループを有するサーボモータロボットでは、このステップジャンプの影響は、サーボのローパスフィルタ特性により吸収され、ほとんどの場合、モータやメカ動作の障害とならない。
一方、サーボループをもたないパルスモータを採用しているロボットでは、関節速度ジャンプ量が大きくなる、つまりＣＰ速度が大きくなると、メカ衝撃となり、滑らかなＣＰ曲線動作が困難となる。
【００１１】
そこで本発明の目的は、低廉なパルスモータを用いた駆動系において、高速で高精度の円滑なＣＰ動作を得ることができるパルスモータ駆動系の軌道制御装置及び方法及び記録媒体を提供することにある。
【００１２】
【課題を解決するための手段】
本発明は前記目的を達成するため、制御対象物のＣＰ動作を制御するパルスモータ駆動系の軌道制御装置において、前記制御対象物のＣＰ動作曲線に基づいて、所定のＣＰ速度に対応する曲線長データを生成する曲線長データ生成手段と、前記曲線長データをｎ次元（ｎ≧２）の座標データに変換する座標変換手段と、前記座標変換手段によって得られるｎ次元の各座標データを、それぞれローパスフィルタに通してスムージングする座標データスムージング手段と、前記座標データスムージング手段より出力される座標データを速度補正用メモリに格納するメモリ格納手段と、前記速度補正用メモリに格納した座標データを所定のタイミングで順次読み出す読み出し手段と、前記読み出し手段によって読み出された座標データを駆動信号に変換する逆変換手段とを有することを特徴とする。
【００１３】
また本発明は、制御対象物のＣＰ動作を制御するパルスモータ駆動系の軌道制御方法において、前記制御対象物のＣＰ動作曲線に基づいて、所定のＣＰ速度に対応する曲線長データを生成する曲線長データ生成工程と、前記曲線長データをｎ次元（ｎ≧２）の座標データに変換する座標変換工程と、前記座標変換工程によって得られるｎ次元の各座標データを、それぞれローパスフィルタに通してスムージングする座標データスムージング工程と、前記座標データスムージング工程より出力される座標データを速度補正用メモリに格納するメモリ格納工程と、前記速度補正用メモリに格納した座標データを所定のタイミングで順次読み出す読み出し工程と、前記読み出し工程によって読み出された座標データを駆動信号に変換する逆変換工程とを有することを特徴とする。
【００１４】
また本発明は、コンピュータ読み取り可能な記録媒体に、制御対象物のＣＰ動作曲線に基づいて、所定のＣＰ速度に対応する曲線長データを生成する機能と、前記曲線長データをｎ次元（ｎ≧２）の座標データに変換する機能と、前記ｎ次元の各座標データを、それぞれローパスフィルタに通してスムージングする機能と、前記ローパスフィルタを通した座標データを速度補正用メモリに格納する機能と、前記速度補正用メモリに格納した座標データを所定のタイミングで順次読み出す機能と、前記速度補正用メモリより読み出された座標データを駆動信号に変換する機能とを実現させるためのプログラムを記録したことを特徴とする。
【００１５】
以上のような本発明では、曲線長データより得られるｎ次元の各座標データを、それぞれローパスフィルタに通してスムージングする。そして、スムージングされた座標データを速度補正用メモリに一時格納し、この格納した座標データを順次読み出して、駆動信号に変換してパルスモータ駆動系に出力していく。
ここで、ｎ次元の各座標データをローパスフィルタに通してスムージングすることにより、パルスモータ駆動系を駆動する場合の滑らかな動作を得ることができ、メカ振動や脱調を防止する。また、速度補正用メモリを介して伝送することにより、安定した速度によって駆動することができる。
【００１６】
【発明の実施の形態】
以下、本発明によるパルスモータ駆動系の軌道制御装置の実施の形態例について説明する。
本例のパルスモータ駆動系は、例えば４軸小型卓上ロボットとして構成されたものであり、このパルスモータロボットのＣＰ動作を制御する軌道制御装置は例えばホストコンピュータ内のアルゴリズムとして構成されている。
なお、本例のスムージングアルゴリスムは上述のようにリアルタイム計算方式を採用したものである。
【００１７】
図１は、本発明によるパルスモータロボットの軌道制御装置によるスムージングアルゴリスムの第１の例を示すブロック図である。
このスムージングアルゴリスムは、関節速度のジャンプに対応したメカ振動や脱調を回避可能なパルスモータロボット専用のスムージングアルゴリズムとして構成されており、ＣＰ曲線のサンプリング部１１０と、ランプ信号発生部１２０と、３次ローパスフィルタ（ｌ−ソフトモータ）部１３０と、座標変換部１４０と、３次ローパスフィルタ（Ψ−ソフトモータ）部１５０と、速度補正用メモリ部１６０と、速度補正／線形補間部１７０と、インバース（逆変換）部１８０とを有する。
【００１８】
曲線サンプリング部１１０では、所定のＣＰ定速度Ｖｃｐに応じて、曲線を一定のエレメント長△ｌに切り、サンプリングする。ここで、サンプリング時間をＴｓとすると、△ｌは以下のようになる。
△ｌ＝Ｖｃｐ・Ｔｓ                                ……（１）
次に、ランプ信号発生部１２０は、ＣＰ定速度Ｖｃｐを発生するものであり、式（１）に従ってサンプリング時間Ｔｓ毎に△ｌずつ移動長を増していくことにより、図１（Ｂ）に示すようなランプ信号を生成する。
【００１９】
また、ＣＰの全長をＬとすれば、Ｔｆ時間後にランプは水平に折れることとなる。つまり、以下のようになる。
Ｖｃｐ＝Ｌ／Ｔｆ、Ｔｆ＝Ｎ・Ｔｓ（Ｎ＝整数）      ……（２）
なお、Ｎが整数となるように、ＴｆつまりＶｃｐを多少トリミングするが、Ｔｓは、一般に十分小さいので問題ない。
このような方式により、例えば図１４に示すように、ＣＰ曲線上で、領域毎に速度を変更することや、途中停止等を行うことも容易となる。
【００２０】
次に、３次ローパスフィルタ部１３０（ｌ−ソフトモータ）は、図１５（Ａ）に示すようなランプ信号より、図１５（Ｂ）に示すような滑らかな線加減速曲線ｌを生成するものである。この３次ローパスフィルタは、伝達関数Ｇ（ｓ）＝ａ／ｓ＋ｂ／ｓ２＋ｃ／ｓ３のとき、Ｇ（ｓ）／１＋Ｇ（ｓ）で安定となる構造を有するローパスフィルタである。
そして、このフィルタ部１３０による滑らかさは、フィルタ部１３０の時定数を変更することにより、自由に可変できる。
【００２１】
次に、座標変換部１４０は、３次ローパスフィルタ部１３０の出力である曲線長ｌを、ＣＰ曲線上の空間座標Ψ（ｘ、ｙ、ｚ）に変換するものである。空間座標Ψ（ｘ、ｙ、ｚ）の各座標をｌを用いてパラメータ表示すると、任意の連続ＣＰ曲線は以下のように表現できる。
ｘ＝ｘ（ｌ）、ｙ＝ｙ（ｌ）、ｚ＝ｚ（ｌ）          ……（３）
これは曲線が折れ線である場合も同様である。また、極座標（Ｒ軸）成分に対しても指定したい場合には、上式（３）に、ｒ＝ｒ（ｌ）を加えればよい。
ここで、ｌ→Ψ（ｘ、ｙ、ｚ）座標変換とは、上式（３）よりΨ（ｘ、ｙ、ｚ）を求めることとなる。したがって、この変換では、Ψ（ｘ、ｙ、ｚ）に位置誤差は発生しない。
【００２２】
次に、３次ローパスフィルタ（Ψ−ソフトモータ）部１５０は、座標変換部１４０によるｘ、ｙ、ｚのデータを、それぞれ３次ローパスフィルタに通してスムージングするものである。
図１６で説明したように、座標変換部１４０が生成するｘ、ｙ、ｚの各々はＣＰ曲線によっては例えば直交方向に折れて、それぞれの速度ｘ’、ｙ’、ｚ’がジャンプする。
【００２３】
なお、以下の説明において、各座標データｘ、ｙ、ｚに対し、速度をｄｘ／ｄｔ＝ｘ’、ｄｙ／ｄｔ＝ｙ’、ｄｚ／ｄｔ＝ｚ’、加速度をｄ2 ｘ／ｄｔ2 ＝ｘ”、ｄ2 ｙ／ｄｔ2 ＝ｙ”、ｄ2 ｚ／ｄｔ2 ＝ｚ”と表すものとする。また、１度フィルタを通したデータをｘ＊、ｙ＊、ｚ＊、２度フィルタを通したデータをｘ＊＊、ｙ＊＊、ｚ＊＊と表すものとする。なお、ｘ、ｙ、ｚ以外の記号についても同様である。
ただし、図面中の記載においては、速度は各記号の上に１つのドットを付して表し、加速度は各記号の上に２つのドットを付して表すものとする。また、１度フィルタを通したデータは各記号の上に１本の波線を付して表し、２度フィルタを通したデータは各記号の上に２本の波線を付して表すものとする。
【００２４】
上述したジャンプが直接パルスモータに伝わってしまうと脱調したり、メカを大きく揺す恐れがあるので、この（ｘ、ｙ、ｚ）各々もソフトモータに入力し、ローパスする。
ここでパルスモータの脱調、メカ振動対策だけなら、（ｘ、ｙ、ｚ）をローパスするのでなくて、インバース部１８０で逆変換をかけた関節角（Θx 、Θy 、Θz ）の各々をローパスすれば良いと思われるが、この手法をとると、ローパス効果のため、ＣＰ曲線に大きな位置誤差（精度エラー）が発生し、しかもこの位置エラーは、（ｘ、ｙ、ｚ）のＣＰ軌跡エラーとなり、予測できないものとなる。なお、サーボモータでは、この問題を抱えている。
【００２５】
一方、（ｘ、ｙ、ｚ）にスムージングをかける場合でも、位置エラー（精度エラー）は発生するが、これは３次ローパスフィルタ（Ψ−ソフトモータ）部１５０の時定数で制御可能なエラーである。
そこで本例では、（ｘ、ｙ、ｚ）をローパスすることにより、ＣＰ位置精度、速度精度を制御して関節角加速度を滑らかにし、パルスモータの脱調、メカ振動を防止する。
なお、３次ローパスフィルタ（Ψ−ソフトモータ）部１５０におけるｘ、ｙ、ｚの３つのローパスフィルタは、上述した３次ローパスフィルタ部１３０と同様に、伝達関数Ｇ（ｓ）＝ａ／ｓ＋ｂ／ｓ２＋ｃ／ｓ３のとき、Ｇ（ｓ）／１＋Ｇ（ｓ）で安定となる構造を有するローパスフィルタである。
【００２６】
また、３次ローパスフィルタ（Ψ−ソフトモータ）部１５０の各フィルタは、互いに同一の特性を有し、また、各フィルタの特性（極配置）は必要に応じて、自由にチューニング可能であるものとする。
特に、図２に示すように、ステップ信号を入力した時、速度がピークに達する時間Ｔｐを自由にチューニング可能であるものとする。
【００２７】
上述のようにスムージングとは、ＣＰ位置精度、速度精度をコントロールして、加速度を柔らげる機能である。図３（Ａ）〜（Ｉ）は、この状況を示す説明図である。なお、ここでは説明を簡単にするため、図３（Ａ）に示すように、ｘ−ｙの２次元平面上で直交コーナを有するＣＰ曲線の例について説明する。
この場合、ｘ、ｙの曲線長データは、図３（Ｂ）（Ｃ）に示すように、共にＣＰ速度一定のためランプ波形となり、ｘ、ｙの速度ｘ’、ｙ’は、図３（Ｄ）（Ｅ）に示すように、共にステップ関数となる。そして、これらをローパスすることにより、ｘ’＊、ｙ’＊は、図３（Ｆ）（Ｇ）に示すように、一定に時定数で滑らかな曲線となる。
【００２８】
そして、図３（Ｈ）に示すように、これらを合成したＣＰ速度ｌ’において、ソフトモータのピーク速度到達時間はＴｐとなる。ここで、ソフトモータ出力のＣＰ速度ｌ’は、ｌ’＊＝｛（ｘ’＊）2 ＋（ｙ’＊）2 ｝1/2 なるベクトル量であるため、ｌ’は一定とならず、くさび状に落ちてしまう。
また、図３（Ｉ）に示すように、スムージングされたＣＰ曲線（ｘ＊、ｙ＊）は、コーナ部の内側を指定軌跡からズレて走ることとなる。
【００２９】
ロボットのＣＰ動作では、位置精度も速度精度も重要ではあるが、両者を理想的に走らすことは、力学上不可能である。このため、本アルゴリズムでは、スムージングのチューニング機構として、図４に示すようなソフトモータのチューニング（極配置）を用いる。
一方、速度のチューニング機構としては、速度補正メモリ部１６０及び速度補正／線形補間部１７０を用いる。以下、図１に沿って、速度補正メモリ部１６０及び速度補正／線形補間部１７０の動作について説明する。なお、サンプリング時間Ｔｓ、ＣＰ速度Ｖｃｐ及び各種ソフトモータの時定数は既に決定されているものとする。
【００３０】
本例のＣＰ速度補正アルゴリズムでは、上述した座標変換部１４０より出力する空間座標Ψ（ｘ、ｙ、ｚ）を３次ローパスフィルタ（Ψ−ソフトモータ）部１５０に入力するのと同時に、３次ローパスフィルタ部１５０の出力（ｘ＊、ｙ＊、ｚ＊）を速度補正メモリ部１６０にサンプリングタイミング毎にストアしていく。
したがって、速度補正メモリ部１６０には、Ｐ1 （ｘ＊、ｙ＊、ｚ＊）→Ｐ2 （ｘ＊、ｙ＊、ｚ＊）→Ｐ3 （ｘ＊、ｙ＊、ｚ＊）、………、ＰN （ｘ＊、ｙ＊、ｚ＊）の形で格納される。
なお、速度補正メモリ部１６０上のポインタ数Ｎは、時間Ｔｐ分用意する。つまり、Ｔｐ／Ｔｓ＝Ｎとなる。
【００３１】
次に、ＣＰ速度補正について説明する。
一般にロボットのＣＰ動作のアプリケーションでは、ＣＰ速度Ｖｃｐを一定にして走らせたいケース（例えばディスペンサ作業、半田付け、塗布作業等）が多い。
しかし、図５に示すように、３次ローパスフィルタ（Ψ−ソフトモータ）部１５０の出力によるＣＰ速度Ｖｃｐ＊は、Ｖｃｐ＊＝｛（ｘ’＊）2 ＋（ｙ’＊）2 ｝1/2 で一定とならず、くさび状に変化してしまう。
すなわち、（ｘ’＊2 ＋ｙ’＊）は一定となるが、｛（ｘ’＊）2 ＋（ｙ’＊）2 ｝1/2 は一定とならない。
そこで、本例では減速開始時点ｔ＝ｔ1 以後において、以下のような処理を行い、速度Ｖｃｐ＊を補正する。
【００３２】
まず、ｔ＝ｔ1 におけるΨ−ソフトモータの出力がＰ1 （ｘ1 ＊、ｙ1 ＊、ｚ1 ＊）にストアされているとする。そして、以後、サンプリング時間Ｔｓ毎にＰ2 、Ｐ3 、Ｐ4 、…とストアされているとする。
ここで、ｔ＝ｔ1 ＋Ｔｓでの位置データＰ2 （ｘ2 ＊、ｙ2 ＊、ｚ2 ＊）を次式より速度補正メモリ上でサーチする。
△ｌ＝｛Ｐ2 2 −Ｐ1 2 ｝1/2             ……（４）
ただし、Ｐ2 2 −Ｐ1 2 ≒（ｘ2 ＊−ｘ1 ＊）＋（ｙ2 ＊−ｙ1 ＊）＋（ｚ2 ＊−ｚ1 ＊）とする。                                ……（５）
【００３３】
上式（４）を厳格に満たすＰ2 は、メモリ上には一般には存在しない。そこで、まず△ｌ以上となる点Ｐi を探す。つまり、
｛Ｐi 2 −Ｐ1 2 ｝1/2 ≧△ｌ            ……（６）
ここで、Ｐi とＰi-1 の間を線形補間して、上式（４）を満たすＰ2 を線形補間計算にて決定する。
【００３４】
具体的には、例えば図６において、
線分長〈Ｐi-1 〜Ｐi 〉＝△ｌi               ……（７）
で、Ｐi-1 からδｌi の距離にある点Ｐi ´においてちょうど△ｌになったとする。δｌi は知ることのできる値であるので、δｌi ＝Ｋ・△ｌi より、Ｋ＝δｌi ／△ｌi なるＫを決定する。これにより、
0003736029.tif 000002
により、Ｐi ´（ｘi ＊´、ｙi ＊´、ｚi ＊´）が決定できる。
【００３５】
以後、このＰi ´を起点として、ここからの距離が△ｌとなる点を同様に決定していく。このアルゴリズムでは、メモリ上の位置データは既にスムージングされており、この位置を基にＣＰ速度Ｖｃｐ一定となるように線形補間がなされるから、図７に示すように、ＣＰ位置、ＣＰ速度は、各々期待通りとなる。
しかしながら、以上のようなアルゴリズムにおいて、ＣＰＵ能力等の関係で、サンプリング時間Ｔｓが大きいと、関節各速度がジャンプし、このままでは、メカ振動、脱調等の問題が残る。
【００３６】
そこで、本例では、ＣＰ速度補正を１００％行うのでなく、所定のＰ％（＜１００％）のみ実行する方法を採用する。なお、サンプリング時間Ｔｓが充分小さい場合には、このような処理は不要である。
１００％補正では、Ｔｓ間の移動距離が△ｌ＝Ｖｃｐ・Ｔｓとなるようなポイントをメモリ上のポイントデータに基づいて線形補間でもとめた。
ここで、△ｌ（ｎ）を、ｔ＝ｎＴｓとｔ＝（ｎ−１）Ｔｓとの間の距離とする。つまり、
△ｌ（ｎ）＝｛Ｐn 2 −Ｐn-1 2 ｝1/2 ≦△ｌ      ……（９）
【００３７】
次に、△ｌ´（ｎ）を、Ｐ％補正する場合のｔ＝ｎＴｓとｔ＝（ｎ−１）Ｔｓとの間の距離とする。このとき、
0003736029.tif 000003
これにより、△ｌ´（ｎ）を決定する。これ以後、メモリサーチ及び線形補間は、１００％補正時と同様に処理することにより、図８に示すように、ＣＰ速度、Ｐ％補正ができ、関節各速度、ジャンプ量を小さくでき、メカ振動を小さく抑制できる。なお、このＰ％の値は、ロボット言語やティーチングペンダント等によりユーザが適宜入力できるようにする。
【００３８】
次に、以上のようにして得られた補正データ（ｘ、ｙ、ｚ）に基づいて、インバース（逆変換）部１８０において、ロボットの関節角Θ（θｘ、θｙ、θｚ）に変換し、後段のパルスボードに送出する。
【００３９】
図９は、本発明によるパルスモータロボットの軌道制御装置によるスムージングアルゴリスムの第２の例を示すブロック図である。
上述した第１の例による速度補正方法では、図８からも分かるように、補正された速度において、ｔ＝Ｔｐ／２にて速度が滑らかでなくなる。つまり、ＣＰ加速度が、わずかかも知れないが、ジャンプすることとなる。そして、このジャンプ（不連続性）によって、関節各速度のジャンプとなり、メカ振動、脱調上好ましくない。そこで本発明の第２の例では、このような問題に対応する構成ついて説明する。なお、このスムージングアルゴリスムは、上述した第１の例によるスムージングアルゴリスムと併用できるものである。
【００４０】
このスムージングアルゴリスムは、ＣＰ曲線のサンプリング部２１０と、ランプ信号発生部２２０と、座標変換部２３０と、３次ローパスフィルタ（Ψ−ソフトモータ）部２４０と、曲線長計算部２５０と、３次ローパスフィルタ（ｌ−ソフトモータ）部２６０と、速度補正用メモリ部２７０と、インバース（逆変換）部２８０とを有する。
このうち、ＣＰ曲線のサンプリング部２１０とランプ信号発生部２２０は、図１に示すサンプリング部１１０とランプ信号発生１２０と共通である。
【００４１】
また、座標変換部２３０は、図１に示す座標変換部１４０と同様の機能を有しており、ランプ信号発生部２２０より出力される曲線長ｌを空間座標ψ（ｘ、ｙ、ｚ）に変換するものである。
また、３次ローパスフィルタ（Ψ−ソフトモータ）部２４０は、図１に示す３次ローパスフィルタ部１５０と同様であり、座標変換部２３０からの座標データｘ、ｙ、ｚをローパスフィルタを通してスムージングするものである。なお、各軸ｘ、ｙ、ｚのソフトモータにおける時定数は同じである。
【００４２】
また、曲線長計算部２５０は、３次ローパスフィルタ部１５０の出力データｘ＊、ｙ＊、ｚ＊に基づいて、ローパスがかけられたＣＰ曲線長ｌ＊を計算するものである。つまり、ｌ＊＝｛ｘ＊2 ＋ｙ＊2 ＋ｚ＊2 ｝1/2 を計算する。
また、速度補正用メモリ部２７０では、以上のようなｌ＊及び（ｘ＊、ｙ＊、ｚ＊）をサンプリング時間毎に順番にＰn （ｌ＊、ｘ＊、ｙ＊、ｚ＊）の形で格納するものである。
【００４３】
３次ローパスフィルタ（ｌ−ソフトモータ）部２６０は、曲線長計算部２５０から出力されるＣＰ曲線長ｌ＊を３次ローパスフィルタを通してスムージングするものである。この３次ローパスフィルタ部２６０の出力をｌ＊＊とする。
図１０に示すように、ｌ＊’は、くさび形に速度が鋭く落ち込み、ディスペンサ作業等において不都合も予想される。そこで、ｌ＊をさらに３次ローパスフィルタ部２６０でスムージングすることにより、速度もスムージングされる。
図１０から分かるように、ｌ＊’の変化が滑らかにスムージングされたＣＰ加速度、連続のｌ＊＊’が得られる。なお、ｌ−ソフトモータのＦ特性を小さくした方が図１０に示すように補正が強く働くものである。
【００４４】
速度補正用メモリ部２７０は、上述したＰn （ｌ＊、ｘ＊、ｙ＊、ｚ＊）を順次格納するとともに、サンプリング時間に対応し、３次ローパスフィルタ部２６０からのｌ＊＊に基づいて（ｘ＊、ｙ＊、ｚ＊）を順次サーチして出力する機能を有している。以下、この手順について説明する。
まず、ｌ＊＊は、ｌ−ソフトモータによりスムージングを受けた曲線長であるが、このｌ＊＊に対応するＣＰ曲線上の点（ｘ＊、ｙ＊、ｚ＊）をメモリよりサーチして求める。
メモリ内には、Ｐn （ｌ＊、ｘ＊、ｙ＊、ｚ＊）の形で格納されているので、ｌ＊＝ｌ＊＊となるＰn をサーチして、このＰn （ｘ＊、ｙ＊、ｚ＊）を次段のインバース部２８０に送出する。
【００４５】
これにより、ｌ−ソフトモータでスムージングされたｌ＊＊と、Ψ−ソフトモータでスムージングされたｌ＊との間に、ＣＰ曲線として位置エラーが入り込まないようにしている。
なお、上述のようにｌ＊＝ｌ＊＊となるＰn をサーチして見つからない場合には、図１で利用した線形補間法を適用する。
また、インバース部２８０は、速度補正用メモリ部２７０にて得られた補正データ（ｘ＊、ｙ＊、ｚ＊）よりロボットの関節角Θ（θｘ、θｙ、θｚ）を算出し、後段のパルスボードに送出する。
【００４６】
次に、以上のような各例における速度補正用メモリ部１６０、２７０の使用方法の詳細について順を追って説明する。
まず、ＣＰ曲線計算スタートで、ソフトモータ出力をメモリに順次ストアしていく。この段階ではロボットはまだ動作していない。
次に、メモリは、Ｔｐ／Ｔｓｓの記憶領域を用意してあるので、このメモリがｆｕｌｌになったら、ロボットのＣＰ動作を実際にスタートさせる。つまり、メモリの先頭データを変換してパルスボードに送出開始する。
これ以後も、メモリにソフトモータ出力をストアしていくが、メモリ容量はＴｐ／Ｔｓｓしか用意していないので、１つのデータをストアする毎に、Ｔｐ前のメモリデータは順番に捨てていく。したがって、このメモリはリングバッファとするのが有効である。
【００４７】
次に、このメモリを用いて、速度補正をかけるトリがとなるのは、各軸の速度のジャンプ量、すなわち指令加速度（ｘ”、ｙ”、ｚ”）のいずれかの値が、前もって与えられる加速度閾値（ｘS ”、ｙS ”、ｚS ”）をオーバーした場合とする。従って、閾値をオーバーしないようなＣＰ曲線であれば、この速度補正は実行されない。
なお、加速度閾値（ｘS ”、ｙS ”、ｚS ”）は、メカ構造に左右される量であるので、事前の実験等で決定しておく必要がある。また、加速時（０≦ｔ≦Ｔｐ）と減速時（Ｔｆ≦ｔ≦Ｔｆ＋Ｔｐ）にも補正はかけない。
【００４８】
加速度ジャンプがＴｐ時間内にコーナが続いた場合、Ｔｐ間の速度は補正される。しかし、Ｔｐ以降の部分の速度補正はかからない。こういったケースは少ないので対応する必要は必ずしもないが、もし対応する場合は、メモリをリングバッファ構造にして、常に補正をかけてしまう手法も可能である。特に、ＣＰＵの計算負担が重くない場合に、この方法が有効である。
【００４９】
次に、以上のようなパルスモータ専用ＣＰ曲線のスムージングアルゴリズムに関する応用例について説明する。
まず、パルスモータ脱調防止アルゴリズムについて説明する。
ロボットのユーザがどんな形のＣＰ曲線でロボットを走らせるか、ロボット設計者には分からない。これに対し、どんな形のＣＰ曲線に対しても、パルスモータが脱調せぬように、ＣＰ最高速度Ｖｃｐmax をかなり低めに設計することは可能であるが、これでは保守的な設計となり、ロボットの能力を抑えてしまう。
そこでこのような場合、ユーザが欲するＣＰ速度で走らせて見て、脱調した場合、３つのψ−ソフトモータの時定数を適当に大きくすることで脱調を防止することができる。もちろんこの場合、ＣＰ軌跡のズレは大きくなるが、脱調を防止するための選択肢として有効となる。なお、このように３つのψーソフトモータ時定数をユーザがロボット言語やティーチングペンダントから変更できるようにしておく。
【００５０】
また、上記のようにユーザにソフトモータ時定数を変更させるシステムがどうしても好ましくない場合、前計算方式とすると自動説調防止が可能となる。以下に手順概要を示す。
まず、図１に示す処理をＣＰ曲線最後までロボットを動作させずに行う（前計算）。
次に、前計算時における各軸の（Θｘ’、Θｙ’、Θｚ’）及び（Θｘ”、Θｙ”、Θｚ”）を各サンプリング時間毎にメモリに格納する。
このメモリデータを利用して、図１１に示すように、各軸毎にＴＮ特性を描いて見て、ＴＮ特性をはみ出している軸を見つける。なお、複数の軸が共にはみ出している場合、はみ出している割合が大きい軸のＴＮ軌跡を描いて見る。
【００５１】
次に、ＴＮ軌跡の中で加速度がピークとなる点（Θｐ”、Θｐ’）を見つける。ここで、速度Θｐ’におけるＴＮ特性上の加速度をΘ1 ”とする場合に、α＝（Θｐ”／Θ1 ”）1/2 となるαを計算し、ψーソフトモータの時定数をα倍以上に設定すれば、脱調を防止できる。
何故なら、（ｘ、ｙ、ｚ）ソフトモータの時定数をα倍すれば、加速度は１／α2 となり、Θｐ”がΘ1 ”となりＴＮ特性以内に納まるからである。
【００５２】
次に、上述したスムージングアルゴリズムを用いた場合、ＣＰ軌跡がＣＰ速度Ｖｃｐでどのように変化するかを説明する。なお、ＣＰ曲線は無数にあるので、上述した直交ロボットによる直角コーナの場合で説明する。
ψーソフトモータの時定数の１つである速度ピーク到達時間をＴｐとする。すると、Ｔｐ間に走るＣＰ曲線上の距離△ｌ（Ｔｐ）は、△ｌ（Ｔｐ）≒Ｖｃｐ／Ｔｐで、ＣＰ速度Ｖｃｐにほぼ比例して図１２に示すようになる。
つまり、Ｖｃｐが大きい程、コーナからのズレが大きくなるが、Ｖｃｐが小さい程、ズレは小さくなる。そして、ユーザがＶｃｐをチューニングすることで、ズレを調整できる。
【００５３】
また、上述したスムージングアルゴリズムは、ホストＣＰＵにとっては、かなりな処理負担となる。このうち、ｌ−ソフトモータ１３０はＣＰ速度の加減速を滑らかに生成するローパスフィルタであるが、後段のψーソフトモータ１５０も、同様の役割を持っているので、ＣＰＵへの負担が大きい場合、ｌ−ソフトモータ１３０は省略しても良い。
さらに、上述した各例では、制御対象物としてパルスモータロボットを制御する構成について説明したが、本発明はロボット以外の駆動系を制御する構成として広く応用し得るものである。
【００５４】
【発明の効果】
以上説明したように本発明では、曲線長データより得られるｎ次元の各座標データを、それぞれローパスフィルタに通してスムージングし、これらのスムージングされた座標データを速度補正用メモリに一時格納し、この格納した座標データを順次読み出して駆動信号に変換し、パルスモータ駆動系に出力するようにした。
したがって本発明によれば、安価なパルスモータ駆動系において、高速ＣＰ動作や折れ線状のＣＰ軌道に対し、サーボモータ駆動系と同等ないしそれ以上の滑らかな軌道を得ることができ、また安定した動作速度を容易に得ることができる効果がある。
【図面の簡単な説明】
【図１】本発明によるパルスモータロボットの軌道制御装置によるスムージングアルゴリスムの第１の例を示すブロック図である。
【図２】図１に示すアルゴリスムにおけるψソフトモータの作用を説明する説明図である。
【図３】図１に示すアルゴリスムにおけるＣＰ曲線の直線コーナ部に対するスムージング曲線の生成工程を示す説明図である。
【図４】図１に示すアルゴリスムにおけるψソフトモータの特性とＣＰ精度との関係を説明する説明図である。
【図５】図１に示すアルゴリスムにおける速度スムージングによる作用を説明する説明図である。
【図６】図１に示すアルゴリスムにおける速度スムージングデータの補間方法を説明する説明図である。
【図７】図１に示すアルゴリスムにおける速度スムージングデータの補間による作用を説明する説明図である。
【図８】図１に示すアルゴリスムにおける速度スムージングデータの速度補正量を制御した場合の作用を説明する説明図である。
【図９】本発明によるパルスモータロボットの軌道制御装置によるスムージングアルゴリスムの第２の例を示すブロック図である。
【図１０】図９に示すアルゴリスムにおけるψソフトモータの周波数特性と速度スムージングデータとの関係を示す説明図である。
【図１１】図１、図９に示すアルゴリスムにおける脱調防止のためのＴＮ特性と加速度の一例を示す説明図である。
【図１２】図１、図９に示すアルゴリスムにおけるＣＰ速度とＣＰ軌跡の関係を示す説明図である。
【図１３】サーボモータロボットのスムージングアルゴリズムの従来例の構成を示す説明図である。
【図１４】図１、図９、図１３に示すアルゴリズムにおいてＣＰ速度を変更した場合のランプ信号を示す説明図である。
【図１５】ランプ信号をローパスフィルタによってスムージングした場合の波形例を示す説明図である。
【図１６】直角コーナを有するＣＰ曲線におけるＣＰ速度の変動を示す説明図である。
【符号の説明】
１１０、２１０……サンプリング部、１２０、２２０……ランプ信号発生部、１３０、２６０……３次ローパスフィルタ（ｌ−ソフトモータ）部、１４０、２３０……座標変換部、１５０、２４０……３次ローパスフィルタ（Ψ−ソフトモータ）部、１６０、２７０……速度補正用メモリ部、１７０……速度補正／線形補間部、１８０、２８０……インバース（逆変換）部、２５０……曲線長計算部。
【図１】
0003736029.tif 000004
【図２】
0003736029.tif 000005
【図３】
0003736029.tif 000006
【図４】
0003736029.tif 000007
【図５】
0003736029.tif 000008
【図６】
0003736029.tif 000009
【図７】
0003736029.tif 000010
【図８】
0003736029.tif 000011
【図９】
0003736029.tif 000012
【図１０】
0003736029.tif 000013
【図１１】
0003736029.tif 000014
【図１２】
0003736029.tif 000015
【図１３】
0003736029.tif 000016
【図１４】
0003736029.tif 000017
【図１５】
0003736029.tif 000018
【図１６】
0003736029.tif 000019