(57)【要約】【課題】理想とする動作軌道に沿ってロボットを正確に動作させるロボット制御装置を提供する。【解決手段】作業指令に基づいて運動指令を生成する運動指令生成部２と、運動指令をロボットの各関節座標系での運動指令に変換するための逆運動学演算部３と、各関節座標系での運動指令に基づいて電動機へのトルク指令を生成する位置・速度フィードバック制御部４と、を有するロボット制御装置において、位置・速度フィードバック制御のシミュレーションを行う制御シミュレーション部５ａと、制御シミュレーション部５ａと逆運動学演算部３の出力に基づいて各関節座標系での運動指令を修正する修正量を出力する修正量演算部６ａと、修正量が所定の範囲内にあるかの判定を行う修正量判定部７ａと、修正量から各関節座標系での運動指令を修正する指令修正部８ａと、逆運動学演算部３または指令修正部８ａにて生成された指令を格納する指令格納部９ａと、を有することを特徴とするロボット制御装置。【選択図】図１
2004348250.tif 000002
【特許請求の範囲】
【請求項１】
作業指令に基づいて、直交座標系での位置または速度指令等の運動指令を生成する運動指令生成部と、前記直交座標系での運動指令をロボットの各関節座標系での運動指令に変換するための逆運動学演算部と、前記各関節座標系での運動指令に基づいて電動機へのトルク指令を生成する位置・速度フィードバック制御部と、を有するロボット制御装置において、
位置・速度フィードバック制御のシミュレーションを行う制御シミュレーション部と、
前記制御シミュレーション部と前記逆運動学演算部の出力に基づいて各関節座標系での運動指令を修正する修正量を出力する修正量演算部と、
前記修正量が所定の範囲内にあるかの判定を行う修正量判定部と、
前記修正量から各関節座標系での運動指令を修正する指令修正部と、
前記逆運動学演算部または前記指令修正部にて生成された指令を格納する指令格納部と、
を有することを特徴とするロボット制御装置。
【請求項２】
作業指令に基づいて、直交座標系での位置または速度指令等の運動指令を生成する運動指令生成部と、前記直交座標系での運動指令をロボットの各関節座標系での運動指令に変換するための逆運動学演算部と、前記各関節座標での運動指令に基づいて電動機へのトルク指令を生成する位置・速度フィードバック制御部と、を有するロボット制御装置において、
位置・速度フィードバック制御のシミュレーションを行う制御シミュレーション部と、
前記制御シミュレーション部と前記運動指令生成部の出力に基づいて直交座標系での運動指令を修正する修正量を出力する修正量演算部と、
前記修正量が所定の範囲内にあるかの判定を行う修正量判定部と、
前記修正量から直交座標系での運動指令を修正する指令修正部と、
前記運動指令生成部または前記指令修正部にて生成された指令を格納する指令格納部と、
を有することを特徴とするロボット制御装置。
【請求項３】
前記制御シミュレーション部は、直交座標系での運動指令をロボットの各関節座標系での運動指令に変換する逆運動学演算部と、
各関節座標系での運動指令を直交座標系での運動指令に変換する順運動学演算部とを有することを特徴とする請求項２記載のロボット制御装置。
【請求項４】
作業対象物に対して理想的な軌道を生成する理想軌道生成部を有し、前記修正量演算部で理想軌道のデータと前記運動指令生成部または前記逆運動学演算部の出力の軌道とに基づいて運動指令の修正量を演算することを特徴とする請求項１乃至３記載のロボット制御装置。
【請求項５】
ロボットの各関節に配置された電動機の角度検出手段で検出されたフィードバック角度または外界センサで検出されたセンサ信号を元に補正量を出力する補正量演算部と、
前記補正量を格納する補正量格納部と、
を有することを特徴とする請求項１乃至４記載のロボット制御装置。
【請求項６】
前記指令格納部から出力される修正角度指令または修正位置指令を元に速度と加速度、または速度とトルクを出力する第２制御シミュレーション部と、
前記速度と前記加速度、または前記速度と前記トルクが所定の範囲を超えるかの判定を行う関節速度・加速度判定部と、
前記速度と前記加速度、または前記速度と前記トルクが所定の範囲をこえる場合には、前記運動指令生成部にて使用する直交座標系の速度パラメータあるいは加減速パラメータを再計算する直交速度・加減速パラメータ演算部と、
を有することを特徴とする請求項１乃至５記載のロボット制御装置。
【請求項７】
前記関節速度・加速度判定部は、前記第２制御シミュレーション部の出力データを所定の最大値で除して倍率データを演算し、
前記直交速度・加速度パラメータ演算部は、前記倍率データに基づいて再計算を行うことを特徴する請求項６記載のロボット制御装置。
【請求項８】
前記関節速度・加速度判定部は、前記第２制御シミュレーション部から出力された速度および加速度、または、速度およびトルクを画面表示する表示部を有し、
直交速度・加減速パラメータ演算部は、速度・加減速パラメータを手動で調節する調整手段を設けたことを特徴とする請求項６乃至７記載のロボット制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する分野】
この発明はロボットの制御装置に関し、特に、運動指令発生部分に関するものである。
【０００２】
【従来の技術】
産業用ロボット等で搬送や溶接などの作業を行わせる場合、いくつかのポイントを教示し、その教示点までの補間方法を指定して、ロボットの各関節のモータへの指令を発生させることで、動作させている。例えば図１８のように、ポイントデータとロボット言語で構成されるジョブファイル１を読み込み、逐次実行していく。ジョブファイル１から読み出されたロボット言語（コマンド）は運動指令生成部２によって、直線や円弧など指定された補間方法で、現在位置から目標位置までを補間しながら一定周期毎に指令を生成していく。生成された位置指令は逆運動学演算部３によって、各関節角度指令に変換される。また、その各関節角度指令に応じて、位置・速度フィードバック制御部４によって、モータへのトルク指令を生成する。このように生成されたトルク指令によって、各関節軸のモータが駆動され、ロボットが教示位置まで動作し、作業に必要な動作を行う。この指令演算について、制御系の出力から位置指令への関係を表す逆伝達関数に基づいて付加軸先端の参照位置関数を生成する指令生成方法がある（特許文献１参照）。
【０００３】
【特許文献１】
特開平１０−１４９２１０号公報（第３頁右上第４行〜第１９行、図１）
【０００４】
【発明が解決しようとする課題】
ロボットは必ずしも指令通りに運動するわけではない。なぜならば、ジョブファイル１で指定するのは、ポイント、速度、補間方法であり、サーボ系の遅れ、機構の特性などを含んだロボットの運動特性を反映していないからである。つまり、サーボ系の応答遅れによって、ロボット先端の運動指令からの追従遅れが生じたり、多自由度アームの動力学特性による運動指令からのずれが生じたりする。
また、特許文献１のように、サーボ系と機構を考慮した逆伝達関数による指令生成方法があるが、軌道が予め決められており、柔軟性に乏しいという問題があった。
本発明は、理想とする動作軌道に沿ってロボットを正確に動作させるロボット制御装置を提供するものである。
【０００５】
【課題を解決するための手段】
上記問題を解決するため、本発明は、次のように構成したものである。
本発明の請求項１記載のロボット制御装置は、作業指令に基づいて、直交座標系での位置または速度指令等の運動指令を生成する運動指令生成部と、前記直交座標系での運動指令をロボットの各関節座標系での運動指令に変換するための逆運動学演算部と、前記各関節座標系での運動指令に基づいて電動機へのトルク指令を生成する位置・速度フィードバック制御部と、を有するロボット制御装置において、位置・速度フィードバック制御のシミュレーションを行う制御シミュレーション部と、前記制御シミュレーション部と前記逆運動学演算部の出力に基づいて各関節座標系での運動指令を修正する修正量を出力する修正量演算部と、前記修正量が所定の範囲内にあるかの判定を行う修正量判定部と、前記修正量から各関節座標系での運動指令を修正する指令修正部と、前記逆運動学演算部または前記指令修正部にて生成された指令を格納する指令格納部と、を有することを特徴とするものである。
【０００６】
本発明の請求項２記載のロボット制御装置は、作業指令に基づいて、直交座標系での位置または速度指令等の運動指令を生成する運動指令生成部と、前記直交座標系での運動指令をロボットの各関節座標系での運動指令に変換するための逆運動学演算部と、前記各関節座標での運動指令に基づいて電動機へのトルク指令を生成する位置・速度フィードバック制御部と、を有するロボット制御装置において、位置・速度フィードバック制御のシミュレーションを行う制御シミュレーション部と、前記制御シミュレーション部と前記運動指令生成部の出力に基づいて直交座標系での運動指令を修正する修正量を出力する修正量演算部と、前記修正量が所定の範囲内にあるかの判定を行う修正量判定部と、前記修正量から直交座標系での運動指令を修正する指令修正部と、前記運動指令生成部または前記指令修正部にて生成された指令を格納する指令格納部と、を有することを特徴とするものである。
【０００７】
本発明の請求項３記載のロボット制御装置は、前記制御シミュレーション部は、直交座標系での運動指令をロボットの各関節座標系での運動指令に変換する逆運動学演算部と、各関節座標系での運動指令を直交座標系での運動指令に変換する順運動学演算部とを有することを特徴とするものである。
本発明の請求項４記載のロボット制御装置は、作業対象物に対して理想的な軌道を生成する理想軌道生成部を有し、前記修正量演算部で理想軌道のデータと前記運動指令生成部または前記逆運動学演算部の出力の軌道とに基づいて運動指令の修正量を演算することを特徴とするものである。
【０００８】
本発明の請求項５記載のロボット制御装置は、ロボットの各関節に配置された電動機の角度検出手段で検出されたフィードバック角度または外界センサで検出されたセンサ信号を元に補正量を出力する補正量演算部と、前記補正量を格納する補正量格納部と、を有することを特徴とするものである。
本発明の請求項６記載のロボット制御装置は、前記指令格納部から出力される修正角度指令または修正位置指令を元に速度と加速度、または速度とトルクを出力する第２制御シミュレーション部と、前記速度と前記加速度、または前記速度と前記トルクが所定の範囲を超えるかの判定を行う関節速度・加速度判定部と、前記速度と前記加速度、または前記速度と前記トルクが所定の範囲をこえる場合には、前記運動指令生成部にて使用する直交座標系の速度パラメータあるいは加減速パラメータを再計算する直交速度・加減速パラメータ演算部と、を有することを特徴とするものである。
【０００９】
本発明の請求項７記載のロボット制御装置は、前記関節速度・加速度判定部は、前記第２制御シミュレーション部の出力データを所定の最大値で除して倍率データを演算し、前記直交速度・加速度パラメータ演算部は、前記倍率データに基づいて再計算を行うことを特徴するものである。
本発明の請求項８記載のロボット制御装置は、前記関節速度・加速度判定部は、前記第２制御シミュレーション部から出力された速度および加速度、または、速度およびトルクを画面表示する表示部を有し、直交速度・加減速パラメータ演算部は、速度・加減速パラメータを手動で調節する調整手段を設けたことを特徴とするものである。
【００１０】
【発明の実施の形態】
以下、本発明の具体的実施例を図に基づいて説明する。
（第１実施例）
この発明の第１の実施例を図１から図３で説明する。図１は本発明のロボット制御装置の構成概略図である。ＪＯＢファイル１は作業を教示する時に作成するもので、教示位置データや動作速度、補間方法等がロボット言語で記述されている。このＪＯＢファイル１は教示時に装置に付随するＲＯＭやハードディスク等のストレージに格納される。実際にロボットが作業動作する時には、このＪＯＢファイル１を内部メモリに読み込み、コマンドとして、運動指令生成部２に送られる。運動指令生成部２では位置データや速度、補間方法、加速度等の内部パラメータに応じて、制御周期毎のロボット先端の位置指令を生成する。逆運動学演算部３ではロボット先端の位置指令をロボットの各関節角度指令に変換するための演算を行う。通常は、この各関節角度指令に応じて、位置・速度フィードバック制御部４での処理が行われ、角度指令に応じたトルク指令を生成し、モータを駆動するためのアンプへ信号を送り、モータを駆動する。位置・速度フィードバック制御部４では、角度指令および各関節軸モータの角度検出器からの角度フィードバックに応じて、位置・速度制御演算が行われてトルク指令を生成する。
【００１１】
しかしながら、本発明では、指令系にシミュレーション部を内蔵することによって、生成軌道を保つように運動指令を修正していくところに特徴がある。これを実現するために、まず、逆運道学演算部３で生成され、指令格納部９ａに格納された各関節の角度指令を制御シミュレーション部５ａに入力する。制御シミュレーション部５ａでは位置速度フィードバック制御部４とロボット機構のモデルが構成されており、サーボ系の遅れと機構の特性を考慮したシミュレーション角度が出力される。
【００１２】
修正量演算部６ａでは制御シミュレーション部５ａで計算された軌道と逆運動学演算部３からの各関節角度指令とから、修正量を演算する。例えば、６自由度のロボットで、制御周期毎の角度指令時系列をθｒｅｆ（ｋ）＝（θｒｅｆ１（ｋ），θｒｅｆ２（ｋ），θｒｅｆ３（ｋ），θｒｅｆ４（ｋ），θｒｅｆ５（ｋ），θｒｅｆ６（ｋ）），（ｋ＝０，１，２，．．．）、シミュレーション角度時系列をθｓｉｍ（ｋ）＝（θｓｉｍ１（ｋ），θｓｉｍ２（ｋ），θｓｉｍ３（ｋ），θｓｉｍ４（ｋ），θｓｉｍ５（ｋ），θｓｉｍ６（ｋ）），（ｋ＝０，１，２，．．．）とすると、修正量Δθ（ｋ）は以下のように計算される。
【００１３】
Δθ（ｋ）＝θｒｅｆ（ｋ）−θｓｉｍ（ｋ）  ・・・（１）
【００１４】
修正量判定部７ａにて、この修正量の絶対値の平均を取り、これが例えば、精度等で表わされる所定の値以下かどうか判定する。所定の値以下の場合は判定結果を０とし、そうでなければ判定結果を１とする。その後、前記判定結果および前記修正量を出力する。指令修正部８ａにおいて、修正量判定部７ａから入力される前記判定結果が１の場合には、修正量判定部７ａから入力される前記修正量と指令格納部９ａから入力される各関節の角度指令値とから、次式のように修正角度指令θｒｅｆ‘（ｋ）を生成する。
【００１５】
θｒｅｆ‘（ｋ）＝θｒｅｆ（ｋ）＋Δθ（ｋ）  ・・・（２）
【００１６】
この修正角度指令θｒｅｆ‘（ｋ）を角度指令θｒｅｆ（ｋ）の代わりに指令格納部９ａに格納し修正角度指令を更新する。更新された修正角度指令を制御シミュレーション部５ａへ渡して上記の処理を繰り返す。前記判定結果が０の場合には、（２）式の修正を行った後、更新された修正角度指令を指令格納部９ａに格納して、位置・速度フィードバック制御部４に出力して実制御を行う。
このようにすることで、サーボ系の遅れと機構の特性による振動や軌道ずれ、あるいは追従遅れを補正する方向に各関節角度の運動指令を修正するため、生成軌道に近い応答が得られる。
【００１７】
図２は制御シミュレーション部５ａの構成例である。位置・速度制御を行うフィードバック演算部５ａ１、ロボットの機構をモデル化したメカモデル部５ａ２から構成されている。フィードバック演算部５ａ１は位置・速度フィードバック演算部４と同等の演算処理を行う。このとき、ゲインなどのパラメータも位置・速度フィードバック演算部４と同じ値にしておく。フィードバック演算部５ａ１で演算されたトルク指令をメカモデル部５ａ２に送り、メカモデル部５ａ２からの応答をフィードバック演算部５ａ１にフィードバックしてシミュレーションを行う。
【００１８】
図３は図２の制御シミュレーション部５ａの構成例をより詳細に示した図である。フィードバック演算部５ａ１では、位置比例、速度比例積分の制御を行っている。ここで、Ｋｐは位置ループゲイン、Ｋｖは速度ループゲイン、Ｋｉは速度積分ゲイン、Ｊｍはモータの慣性モーメント、Ｎは減速比である。メカモデル部５ａ２はロボットの機構を２慣性系で近似した例を示している。ここで、Ｊｍはモータの慣性モーメント、Ｄｍはモータの粘性係数、Ｎは減速比、Ｋｃは減速器のバネ定数、Ｄｃは減速器の粘性係数、Ｊｌは負荷側の慣性モーメント、Ｄｌは負荷側の粘性係数である。このようにシミュレーション部を構成することで、フィルタやサーボ系の遅れと機構の特性による振動や軌道ずれ等がシミュレーションできる。
【００１９】
ここで示した制御シミュレーション部は５ａは一例である。例えばフィードバック演算部５ａ１は位置・速度フィードバック制御部４をシミュレーションするものであるが、すべてを忠実にシミュレーションしてもよいし、簡単に１次遅れで近似してもよい。またメカモデル部５ａ２はロボットの機構を２慣性系機構としてモデル化したものであるが、さらにベルト駆動等がある場合は３慣性機構として詳細にモデル化するなど、必要に応じてモデルを決定する。どこまで詳細にシミュレーションするかは、実際にロボットに作業をさせる際に要求される精度やＣＰＵの演算負荷などに応じて決定すればよい。
【００２０】
（第２実施例）
第２の実施例を図４と図５で説明する。第２の実施例は第１の実施例と比較して、修正量を各関節角度で算出するのでなく、直交位置で算出するところが異なる。図４は第２の実施例のロボット制御装置の構成概略図である。ＪＯＢファイル１を内部メモリに読み込み、コマンドとして、運動指令生成部２に送られる。運動指令生成部２では位置データや速度、補間方法、加速度等の内部パラメータに応じて、制御周期毎のロボット先端の位置指令を生成する。逆運動学演算部３ではロボット先端の位置指令をロボットの各関節角度指令に変換するための演算を行う。通常は、この各関節角度指令に応じて、位置・速度フィードバック制御部４での処理が行われ、角度指令に応じたトルク指令を生成し、モータを駆動するためのアンプへ信号を送り、モータを駆動する。位置・速度フィードバック制御部４では、角度指令および各関節軸モータの角度検出器からの角度フィードバックに応じて、位置・速度制御演算が行われてトルク指令を生成する。
【００２１】
しかしながら、本発明では、指令系にシミュレーション部を内蔵することによって、生成軌道を保つように運動指令を修正していくところに特徴がある。これを実現するために、まず、運動指令生成部２で生成され、指令格納部９ａに格納された直交座標系での位置指令を制御シミュレーション部５ｂに入力する。制御シミュレーション部５ｂでは逆運動学演算部３と、位置速度フィードバック制御部４、ロボット機構のモデルが構成されており、サーボ系の遅れと機構の特性を考慮したシミュレーション角度が出力される。このシミュレーション角度を順運動学演算によって直交座標系でのシミュレーション位置に変換する。
【００２２】
修正量演算部６ｂでは制御シミュレーション部５ｂで計算された軌道と運動指令生成部２で生成された直交座標系での位置指令とから、修正量を演算する。例えば、制御周期毎の直交座標系での位置指令時系列をＰｒｅｆ（ｋ）＝（Ｐｒｅｆｘ（ｋ），Ｐｒｅｆｙ（ｋ），Ｐｒｅｆｚ（ｋ），Ｐｒｅｆα（ｋ），Ｐｒｅｆβ（ｋ），Ｐｒｅｆγ（ｋ）），（ｋ＝０，１，２，．．．）、直交座標系でのシミュレーション位置時系列Ｐｓｉｍ（ｋ）＝（Ｐｓｉｍｘ（ｋ），Ｐｓｉｍｙ（ｋ），Ｐｓｉｍｚ（ｋ），Ｐｓｉｍα（ｋ），Ｐｓｉｍβ（ｋ），Ｐｓｉｍγ（ｋ）），（ｋ＝０，１，２，．．．）とすると、修正量ΔＰ（ｋ）は以下のように計算される。
【００２３】
ΔＰ（ｋ）＝Ｐｒｅｆ（ｋ）−Ｐｓｉｍ（ｋ）  ・・・（３）
【００２４】
修正量判定部７ｂにて、この修正量の絶対値の平均を取り、これが例えば、精度等で表わされる所定の値以下かどうか判定する。所定の値以下の場合は判定結果を０とし、そうでなければ判定結果を１とする。その後、前記判定結果および前記修正量を出力する。指令修正部８ｂにおいて、修正量判定部７ｂから入力される前記判定結果が１の場合には、修正量判定部７ｂから入力される前記修正量と指令格納部９ｂから入力される直交座標系での位置指令値とから、次式のように修正位置指令Ｐｒｅｆ‘（ｋ）を生成する。
【００２５】
Ｐｒｅｆ‘（ｋ）＝Ｐｒｅｆ（ｋ）＋ΔＰ（ｋ）  ・・・（４）
【００２６】
この修正位置指令Ｐｒｅｆ‘（ｋ）を位置指令Ｐｒｅｆ（ｋ）の代わりに指令格納部９ｂに格納し修正位置指令を更新する。更新された修正位置指令を制御シミュレーション部５ｂへ渡して上記の処理を繰り返す。前記判定結果が０の場合には、（４）式の修正を行った後、更新された修正位置指令を指令格納部９ｂに格納して、逆運動学演算部３に出力して実制御を行う。
このようにすることで、サーボ系の遅れと機構の特性による振動や軌道ずれ、あるいは追従遅れを補正する方向に直交座標系での運動指令を修正するため、生成軌道に近い応答が得られる。
【００２７】
図５は制御シミュレーション部５ｂの構成例である。図２で示したように、位置・速度制御を行うフィードバック演算部５ａ１、ロボットの機構をモデル化したメカモデル部５ａ２から構成される制御シミュレーション部５ａがあり、この前段に、位置指令値から各関節角度指令値へ変換するためにの逆運動学演算部５ｂ１と、後段に、各関節角度指令値から位置指令値に変換するための順運動学演算部５ｂ２を設けている。フィードバック演算部、メカモデル部の詳細は図３と同様である。
このようにシミュレーション部を構成することで、サーボ系の遅れと機構の特性による振動や軌道ずれ等が直交座標系でシミュレーションできる。
【００２８】
（第３実施例）
第３の実施例を図６で説明する。第３の実施例は第２の実施例（図４）に理想軌道生成部１１を追加したものである。運動指令生成部２による運動指令から生成された生成軌道はあくまで、目標到達位置と動作速度により生成されたもので、これをつなぎあわせて１つの作業動作を行うことになる。例えばこのつなぎあわせのところで軌道にひずみが生じることがあり、教示者が所望する軌道とずれる可能性がある。ここで、作業対象物のＣＡＤデータなどがあればこのＣＡＤデータ上で軌道を指定し、これを運動指令から生成される軌道と重ねあわせることによって、修正量を算出できればより理想軌道に近づけることが可能となる。
【００２９】
理想軌道生成部１１では作業対象物のＣＡＤなどのデータ１０からロボット先端が通過するべき理想軌道を生成する。ここでロボットと作業対象物との位置関係からロボット座標系での理想軌道が算出できる。また、ここでは理想軌道生成部１１にＣＡＤデータを直接入力しているが、ＣＡＤデータから３次元の通過点位置データを別のパソコン等で作成し、そのデータをこの理想軌道生成部１１に入力してもよい。理想軌道生成部１１では、入力された３次元位置データがＣＡＤ座標系である場合、これをロボット座標系での位置データに変換する。
ここで生成された理想軌道位置データと、運動指令生成部２で生成された位置指令とを比較することで、修正量を算出する。ただし、ロボットと作業対象物の位置関係はＣＡＤデータと実際とは誤差があるため、絶対量で比較を行うのではなく、相対的な位置関係で比較を行う。例えば、図７のように両者の始点と進行ベクトルを一致させるようにすることで、理想軌道とのずれが相対的に算出できる。
【００３０】
これに制御シミュレーション部５ｂで演算されたシミュレーション位置による修正量を加える。具体的には、第２の実施例と同様に直交座標系での位置指令Ｐｒｅｆ（ｋ）、直交座標系でのシミュレーション位置Ｐｓｉｍ（ｋ）、理想軌道とのずれΔＰｉｄｅａｌ（ｋ）＝（Ｐｉｄｅａｌｘ（ｋ），Ｐｉｄｅａｌｙ（ｋ），Ｐｉｄｅａｌｚ（ｋ），Ｐｉｄｅａｌα（ｋ），Ｐｉｄｅａｌβ（ｋ），Ｐｉｄｅａｌγ（ｋ）），（ｋ＝０，１，２，．．．）とすると、修正量ΔＰ（ｋ）は以下の式によって算出される。
【００３１】
ΔＰ（ｋ）＝Ｐｒｅｆ（ｋ）−Ｐｓｉｍ（ｋ）＋ΔＰｉｄｅａｌ（ｋ）  ・・・（５）
【００３２】
あとは第２の実施例と同じように、修正量判定部７ｂにて、この修正量が所定の範囲内にあるかの判定を行い、指令修正部８ｂにおいて、修正量判定部７ｂから入力される判定結果が１の場合には、修正量判定部７ｂから入力される前記修正量と指令格納部９ｂから入力される直交座標系での位置指令値とから、次式のように修正位置指令Ｐｒｅｆ‘（ｋ）を生成する。
【００３３】
Ｐｒｅｆ‘（ｋ）＝Ｐｒｅｆ（ｋ）＋ΔＰ（ｋ）  ・・・（６）
【００３４】
この修正位置指令Ｐｒｅｆ‘（ｋ）を位置指令Ｐｒｅｆ（ｋ）の代わりに指令格納部９ｂに格納し修正位置指令を更新する。更新された修正位置指令を制御シミュレーション部５ｂへ渡して上記の処理を繰り返す。前記判定結果が０の場合には、（６）式の修正を行った後、更新された修正位置指令を指令格納部９ｂに格納して、逆運動学演算部３に出力して実制御を行う。
このようにすることで、サーボ系の遅れと機構の特性による振動や軌道ずれ、あるいは追従遅れを補正する方向に直交座標系での運動指令を修正するため、生成軌道に近い応答が得られるとともに、さらに、ＣＡＤデータなどから生成された理想軌道とのずれも修正できより正確な軌道が実現できる。
【００３５】
（第４実施例）
第４の実施例を図８で説明する。第４の実施例は第１の実施例（図１）にモータの角度検出器からのフィードバック値および指令から補正量を演算する補正量演算部１２および前記補正量を格納する補正量格納部１３を追加したものである。まず第１の実施例のように制御シミュレーションに応じて運動指令を修正し、実際にロボットを動作させる。この時、ロボットの各関節のモータに付随するエンコーダ信号を補正量演算部１２に取り込み、関節角度に換算して逆運動学演算部３からの出力である角度指令との比較を行い、補正量を演算する。例えば、制御周期毎の角度指令時系列をθｒｅｆ（ｋ）＝（θｒｅｆ１（ｋ），θｒｅｆ２（ｋ），θｒｅｆ３（ｋ），θｒｅｆ４（ｋ），θｒｅｆ５（ｋ），θｒｅｆ６（ｋ）），（ｋ＝０，１，２，．．．）、補正量演算部１２に取り込まれたフィードバック角度時系列をθｆｂ（ｋ）＝（θｆｂ１（ｋ），θｆｂ２（ｋ），θｆｂ３（ｋ），θｆｂ４（ｋ），θｆｂ５（ｋ），θｆｂ６（ｋ）），（ｋ＝０，１，２，．．．）とすると、補正量Δθｒ（ｋ）は以下のように計算される。
【００３６】
Δθｒ（ｋ）＝θｒｅｆ（ｋ）−θｆｂ（ｋ）  …（７）
【００３７】
この補正量Δθｒ（ｋ）を補正量格納部１３に格納しておく。そして次回動作時に、補正量格納部１３に格納された補正量Δθｒ（ｋ）を指令修正部８ａに読み込んで、指令格納部９ａに格納された修正角度指令θｒｅｆ‘（ｋ）から、次式のように最終的な修正角度指令θｒｅｆ“（ｋ）を生成する。
【００３８】
θｒｅｆ“（ｋ）＝θｒｅｆ‘（ｋ）＋Δθｒ（ｋ）  ・・・（８）
【００３９】
この修正角度指令を指令格納部９ａに格納し、位置・速度フィードバック制御部４へ出力して実制御を行う。
このようにすることで、サーボ系の遅れと機構の特性による振動や軌道ずれ、あるいは追従遅れを補正する方向に各関節角度の運動指令を修正するため、生成軌道に近い応答が得られるとともに、実際のロボットの動作結果に応じて指令を補正するので、より生成軌道に近い軌道が得られる。
本実施例ではモータのエンコーダからのフィードバック値を実際の動作データとして扱っているが、これに留まらず、ロボット先端に取り付けた距離センサ等の外界センサの信号で指令補正を行ってもよい。
【００４０】
（第５実施例）
第１から４の実施例の位置・速度フィードバック制御部４において、入力された角度指令に対して、移動平均やローパスフィルタ等といった加減速をなめらかにするためのフィルタ処理を行った場合、フィードバック演算部５ａ１の前段に同じフィルタ処理を入れることによって、フィルタの遅れを考慮した修正が可能となる。
例えば、第１の実施例の場合、各関節の角度指令をフィルタ処理を含んだ制御シミュレーション部５ａに入力すると、フィルタやサーボ系の遅れと機構の特性を考慮したシミュレーション角度が出力される。
その後、修正量演算部６ａにて制御シミュレーション部５ａで計算された軌道と逆運動学演算部３からの各関節角度指令とから、修正量Δθ（ｋ）を次式のように演算する。
【００４１】
Δθ（ｋ）＝θｒｅｆ（ｋ）−θｓｉｍ（ｋ＋ｋｆ）  ・・・（９）
【００４２】
ここでｋｆはフィルタ処理による遅れを調整する調整量で予め与えられた値である。
あとは、第１の実施例と同様に、修正量判定部７ａにて、この修正量が所定の範囲内にあるかの判定を行い、指令修正部８ａにて、前記修正量を用いて次式のように修正角度指令θｒｅｆ‘（ｋ）を生成し、
【００４３】
θｒｅｆ‘（ｋ）＝θｒｅｆ（ｋ）＋Δθ（ｋ）  ・・・（１０）
【００４４】
修正角度指令θｒｅｆ‘（ｋ）を指令格納部９ａに格納し、修正量判定部７ａでの判定結果が０になるまで上記の処理を繰り返す。判定結果が０になったら、位置・速度フィードバック制御部４に出力して実制御を行う。
このようにすることで、加減速のフィルタが入った場合でも、フィルタによる軌道ずれを補正する方向に運動指令を修正するため、生成軌道に近い応答が得られる。
【００４５】
（第６実施例）
第６の実施例を図９から図１２で説明する。図９は第６の実施例のロボット制御装置の構成概略図である。修正角度指令時系列θｒｅｆ‘（ｋ）を生成するまでは第１の実施例と同様であるので説明を省略する。
第６の実施例では、さらに、第２制御シミュレーション部５ａにて、各関節の速度および加速度のシミュレーションを行う。第２制御シミュレーション部５ａは制御シミュレーション部５ａと同じものである。また、フィードバック演算部５ａ１およびメカモデル部５ａ２は図１０に示すように第１の実施例と同等な形態のモデルを使用することができる。ただし、メカモデル部５ａ２において、関節角度だけでなく、関節速度や関節加速度も出力できるようになっている。第２制御シミュレーション部５ａにより前記修正角度指令時系列θｒｅｆ‘（ｋ）からサーボ系の遅れと機構の特性を考慮した関節速度時系列ωｓｉｍ‘（ｋ）＝（ωｓｉｍ‘１（ｋ），ωｓｉｍ‘２（ｋ），ωｓｉｍ‘３  （ｋ），ωｓｉｍ‘４（ｋ），ωｓｉｍ‘５（ｋ），ωｓｉｍ‘６（ｋ））、関節加速度時系列αｓｉｍ‘（ｋ）＝（αｓｉｍ‘１（ｋ），αｓｉｍ‘２（ｋ），αｓｉｍ‘３  （ｋ），αｓｉｍ‘４（ｋ），αｓｉｍ‘５（ｋ），αｓｉｍ‘６（ｋ））を求める。
【００４６】
関節速度・加速度判定部１０１においては、前記関節速度時系列ωｓｉｍ‘（ｋ）および前記関節加速度時系列αｓｉｍ‘（ｋ）が所定の範囲を超えているかの判定を次のようにして行う。まず、関節速度時系列ωｓｉｍ‘ｉ（ｋ）、関節加速度時系列αｓｉｍ‘ｉ（ｋ），（ｉ＝１，２，．．．，６）の各々においてその絶対値｜ωｓｉｍ‘ｉ（ｋ）｜、｜αｓｉｍ‘ｉ（ｋ）｜と予め設定されたロボットの特性などから決定される各関節の最大速度をωｍａｘｉ、最大加速度をαｍａｘｉ，（ｉ＝１，２，．．．，６）とから倍率データｂωｉ（ｋ）、ｂαｉ（ｋ），（ｉ＝１，２，．．．，６）を以下のようにして求める。
【００４７】
ｂωｉ（ｋ）＝｜ωｓｉｍ‘ｉ（ｋ）｜／ωｍａｘｉ  ・・・（１１）
ｂαｉ（ｋ）＝｜αｓｉｍ‘ｉ（ｋ）｜／αｍａｘｉ  ・・・（１２）
【００４８】
全倍率データｂωｉ（ｋ）、ｂαｉ（ｋ）のうち最大のものをｂｍａｘとすると、ｂｍａｘ＞１となった場合は倍率パラメータｂｍをｂｍａｘとして各関節の速度および加速度が所定の範囲を超えたと判定し、直交速度・加減速パラメータ演算部１０２に前記倍率パラメータｂｍを出力する。ｂｍａｘ≦１の場合は倍率パラメータｂｍを１として各関節の速度および加速度が所定の範囲を超えていないと判定し、その判定結果を指令格納部９ａに送って前記修正角度指令時系列θｒｅｆ‘（ｋ）を用いて位置・速度フィードバック制御部４による実制御を行う。
このように各関節の速度や加速度を確認することで、減速機等の機械要素に過大な負荷をかけないようにして、ロボットに衝撃を与えないようにすることができる。
【００４９】
また、上記の例では、第２制御シミュレーション部５ａから各関節の速度や加速度を出力し、関節速度・加速度判定部１０１にて前記速度や加速度が所定の範囲にあるかの判定を行っていたが、図１１のように第２制御シミュレーション部５ａから各関節のモータへの速度指令やトルク指令を出力して、関節速度・加速度判定部１０１にて前記速度指令やトルク指令が所定の範囲にあるかの判定を行ってもよい。ただし、このときの判定は、モータの特性から決定される速度やトルクの最大値による。
このようにモータへの指令を確認することで、モータに過大な負荷をかけないようにして、ロボットに衝撃を与えないようにすることができる。
直交速度・加減速パラメータ演算部１０２においては、ｂｍ＞１となった場合に直交座標系の速度Ｖｒｅｆあるいは加減速時間ｔａを再計算する。再計算は再計算後の直交座標系の速度をＶｒｅｆ’、加減速時間をｔａ’とすると（１３）式、（１４）式のどちらかを行うことになる。
【００５０】
ｔａ’＝ｂｍ・ｔａ  ・・・（１３）
Ｖｒｅｆ’＝Ｖｒｅｆ／ｂｍ  ・・・（１４）
【００５１】
（１３）、（１４）のどちらを行うかの判定は、再計算後の移動時間により次のように行う。
今回、速度が加速度一定の台形パターンであるものとすると直交座標系の速度Ｖｒｅｆ、加減速時間ｔａのときの目標位置までの移動距離Ｓは以下のようになる。
【００５２】
Ｓ＝Ｖｒｅｆ（ｔａ＋ｔｂ）  ・・・（１５）
【００５３】
ただし、ｔｂは上記の条件での定速時間である。
また、（１３）式により直交座標系の加減速時間ｔａを再計算した場合は、目標位置までの移動距離Ｓは以下のようになる。
【００５４】
Ｓ＝Ｖｒｅｆ（ｂｍ・ｔａ＋ｔｂ’）  ・・・（１６）
【００５５】
ただし、ｔｂ’は上記の条件での定速時間である。
このときの移動時間Ｔ１は、移動距離Ｓは変わらないので、（１５）式、（１６）式より以下のようになる。
【００５６】
Ｔ１＝（１＋ｂｍ）ｔａ＋ｔｂ  ・・・（１７）
【００５７】
一方、（１４）式により直交座標系の速度Ｖｒｅｆを再計算した場合、再計算前と同じ加速度で速度パターンを生成したとすると、目標位置までの移動距離Ｓは以下のようになる。
【００５８】
Ｓ＝（Ｖｒｅｆ／ｂｍ）（ｔａ’’＋ｔｂ’’）  ・・・（１８）
【００５９】
ただし、ｔａ’’は再計算前と加速度が同じという条件のもとで、以下の式より求められ、ｔｂ’’は上記の条件での速度パターンにおける定速時間である。
【００６０】
Ｖｒｅｆ／（ｂｍ・ｔａ’’）＝Ｖｒｅｆ／ｔａ  ・・・（１９）
【００６１】
このときの移動時間Ｔ２は、移動距離Ｓは変わらないので、（１５）式、（１８）式、（１９）式より以下のようになる。
【００６２】
Ｔ２＝（１／ｂｍ＋ｂｍ）ｔａ＋ｂｍ・ｔｂ  ・・・（２０）
【００６３】
以上より、Ｔ１が小さくなる、すなわち、直交座標系の加減速時間ｔａを再計算する条件は（１７）式、（２０）式から以下のようになる。
【００６４】
ｔａ≦ｂｍ・ｔｂ  ・・・（２１）
【００６５】
すなわち、再計算前の加減速時間ｔａおよび定速時間ｔｂと、倍率パラメータｂｍと、により（２１）式を満たすような場合は、（１３）式を実行して直交座標系の加減速時間ｔａを再計算すれば良く、（２１）式を満たさない場合は、（１４）式を実行して直交座標系の速度Ｖｒｅｆを再計算すれば良いことになる。
その後、再計算されたパラメータを用いて前記運動指令生成部２から処理をやり直すようにし、前記倍率パラメータｂｍが１以下になるまで繰り返す。
【００６６】
また、通常は、ｎ個の速度指令があり、速度パターンが例えば、図１２（ａ）、（ｂ）のようになっている。なお、図１２（ａ）、（ｂ）はｎ＝３のときの速度パターンである。このような場合は、前記速度指令が与えられた移動命令毎に倍率パラメータｂｍｉ（ｉ＝０，１，２，．．．，ｎ−１）を求め、図１２（ａ）のように加減速時間を再計算した場合と図１２（ｂ）のように速度を再計算した場合についての移動時間を計算し、移動時間が小さくなった方についてのパラメータを再計算すればよい。
このようにすることで、生成軌道に近い応答が得られるように修正された関節座標系の運動指令に対して、ロボットに衝撃を与えない、かつ、作業時間の増加を抑えた指令を生成することができる。
【００６７】
この実施例において、関節速度・加速度判定部１０１であらかじめ設定された条件データから自動的に判定を行い、また、その結果を用いて直交速度・加減速パラメータ演算部１０２で自動的にパラメータを演算している。ここで別の方法として、関節速度・加速度判定部１０１で第２制御シミュレーション部５ａから出力された関節速度・加速度の時系列データを人間が判断しやすいようにディスプレイに表示する。ここで人間が可否の判定をおこない、可であれば指令格納部９ａに指令を格納するようにする。また否であれば直交速度・加減速パラメータ演算部１０２に設けたキーボードやダイヤル等による手動調整手段によって、人間がパラメータを調整し、運動指令生成部２へ送る。このように人間が介在する形にすることで、より細かな調整が可能となる。
【００６８】
（第７実施例）
第７の実施例は、第６の実施例と第２シミュレーション部５ｃが異なる。図１３に示すように関節速度・加速度演算部５ｃ１から構成され、修正角度指令時系列θｒｅｆ‘（ｋ）から差分法により１階および２階微分値を求め、各関節の速度および加速度を生成する。
このようにすることで、シミュレーションの計算量を少なくすることができる。
【００６９】
（第８実施例）
第８の実施例を図１４から図１５で説明する。図１４は第８の実施例のロボット制御装置の構成概略図である。修正位置指令時系列Ｐｒｅｆ‘（ｋ）を生成するまでは第２の実施例と同様であるので説明を省略する。
第８の実施例では、さらに、第２制御シミュレーション部５ｄにて、各関節の速度および加速度のシミュレーションを行う。第２制御シミュレーション部５ｄは図１５に示すように逆運動学演算部５ｄ１を有し、直交座標系の修正位置指令時系列Ｐｒｅｆ‘（ｋ）から、第６の実施例と同様にサーボ系の遅れと機構の特性を考慮した関節速度時系列ωｓｉｍ‘（ｋ）および関節加速度時系列αｓｉｍ‘（ｋ）を求めることができる。
その後は、第６の実施例と同様に、関節速度・加速度判定部１０１、直交速度・加減速パラメータ演算部１０２の処理を行い、各関節の速度および加速度が所定の範囲内にあるような修正位置指令を生成して、逆運動学演算部３および位置・速度フィードバック制御部４による実制御を行う。
このようにすることで、生成軌道に近い応答が得られるように修正された直交座標系の運動指令に対して、ロボットに衝撃を与えない、かつ、作業時間の増加を抑えた指令を生成することができる。
【００７０】
（第９実施例）
第９の実施例は、第８の実施例と第２シミュレーション部５ｅが異なる。図１６に示すように逆運動学演算部５ｅ１および関節速度・加速度演算部５ｅ２から構成され、修正位置指令時系列Ｐｒｅｆ‘（ｋ）を修正角度指令時系列θｒｅｆ‘（ｋ）に変換し、差分法により１階および２階微分値を求め、各関節の速度および加速度を生成する。
このようにすることで、シミュレーションの計算量を少なくすることができる。
【００７１】
（第１０実施例）
第１０の実施例を図１７で説明する。図１７は第１０の実施例のロボット制御装置の構成概略図である。第１０の実施例は第３の実施例と第８の実施例の組み合わせである。修正位置指令時系列Ｐｒｅｆ‘（ｋ）を第３の実施例のように生成し、前記修正位置指令時系列Ｐｒｅｆ‘（ｋ）から、第８の実施例のように第２制御シミュレーション部５ｄ、関節速度・加速度判定部１０１、直交速度・加減速パラメータ演算部１０２の処理を行い、各関節の速度および加速度が所定の範囲内にあるような修正位置指令を生成して、逆運動学演算部３および位置・速度フィードバック制御部４による実制御を行う。
また、第９の実施例のように第２制御シミュレーション部５ｄは５ｅを使ってもよい。
このようにすることで、サーボ系や機構の特性による軌道ずれだけでなく、ＣＡＤデータなどから生成された理想軌道とのずれも修正した直交座標系の運動指令に対して、ロボットに衝撃を与えない、かつ、作業時間の増加を抑えた指令を生成することができる。
【００７２】
【発明の効果】
本発明の請求項１及び２によれば、サーボ系の遅れと機構の特性による振動や軌道ずれ、あるいは追従遅れを補正する方向に直交座標系、あるいは関節座標系での運動指令を修正するため、生成軌道に近い応答が得られる。
請求項３によれば、直交座標系でのシミュレーションが可能になる。
請求項４によれば、運動指令生成部で生成された軌道自体にひずみが生じていても、理想軌道を保つことが可能となる。
請求項５によれば、実際のロボットの動作結果に応じて指令を補正するので、より生成軌道に近い軌道を生成させることが可能となる。
請求項６から８記載のロボット制御装置によれば、サーボ系の遅れと機構の特性による振動や軌道ずれ、あるいは追従遅れを補正する方向に直交座標系、あるいは関節座標系での運動指令を修正し、修正した運動指令に対し各関節の速度、加速度等を確認し、それらが所定の範囲を超えた場合は移動時間の増加が小さくなるように直交座標系の速度パラメータあるいは加減速パラメータを再計算して各関節の速度、加速度等が所定の範囲におさまるような指令を生成することにより、ロボット先端の軌跡精度を向上することができるとともに、ロボットに衝撃を与えない、かつ、作業時間の増加を抑えて作業時間短縮を図った指令を生成することができる。
【図面の簡単な説明】
【図１】本発明の第１の実施例の構成図
【図２】本発明の第１の実施例の制御シミュレーション部の構成図
【図３】本発明の第１の実施例の制御シミュレーション部の詳細図
【図４】本発明の第２の実施例の構成図
【図５】本発明の第２の実施例の制御シミュレーション部の構成図
【図６】本発明の第３の実施例の構成図
【図７】本発明の第３の実施例での理想軌道と運動指令の重ねあわせの概念図
【図８】本発明の第４の実施例の構成図
【図９】本発明の第６の実施例の構成図
【図１０】本発明の第６の実施例の制御シミュレーション部、第２制御シミュレーション部の詳細図
【図１１】本発明の第６の実施例の制御シミュレーション部、第２制御シミュレーション部の出力データを替えた場合の詳細図
【図１２】本発明の第６の実施例での複数の移動命令がある場合の速度パターン
【図１３】本発明の第７の実施例の第２制御シミュレーション部の構成図
【図１４】本発明の第８の実施例の構成図
【図１５】本発明の第８の実施例の第２制御シミュレーション部の構成図
【図１６】本発明の第９の実施例の第２制御シミュレーション部の構成図
【図１７】本発明の第１０の実施例の構成図
【図１８】従来のロボット制御装置の構成図
【符号の説明】
１：ＪＯＢファイル
２：運動指令生成部
３：逆運動学演算部
４：位置・速度フィードバック制御部
５ａ：制御シミュレーション部、第２制御シミュレーション部
５ａ１：フィードバック演算部
５ａ２：メカモデル部
５ｂ：制御シミュレーション部
５ｂ１：逆運動学演算部
５ｂ２：順運動学演算部
５ｃ：第２制御シミュレーション部
５ｃ１：関節速度・加速度演算部
５ｄ：第２制御シミュレーション部
５ｄ１：逆運動学演算部
５ｅ：第２制御シミュレーション部
５ｅ１：逆運動学演算部
５ｅ２：関節速度・加速度演算部
６ａ、６ｂ：修正量演算部
７ａ、７ｂ：修正量判定部
８ａ、８ｂ：指令修正部
９ａ、９ｂ：指令格納部
１０：ＣＡＤデータファイル
１１：理想軌道生成部
１２：補正量演算部
１３：補正量格納部
１４：角度検出器
１０１：関節速度・加速度判定部
１０２：直交速度・加減速パラメータ演算部
【図１】
2004348250.tif 000003
【図２】
2004348250.tif 000004
【図３】
2004348250.tif 000005
【図４】
2004348250.tif 000006
【図５】
2004348250.tif 000007
【図６】
2004348250.tif 000008
【図７】
2004348250.tif 000009
【図８】
2004348250.tif 000010
【図９】
2004348250.tif 000011
【図１０】
2004348250.tif 000012
【図１１】
2004348250.tif 000013
【図１２】
2004348250.tif 000014
【図１３】
2004348250.tif 000015
【図１４】
2004348250.tif 000016
【図１５】
2004348250.tif 000017
【図１６】
2004348250.tif 000018
【図１７】
2004348250.tif 000019
【図１８】
2004348250.tif 000020