JP3316967B2 - ロボットの制御装置 - Google Patents
ロボットの制御装置Info
- Publication number
- JP3316967B2 JP3316967B2 JP25921993A JP25921993A JP3316967B2 JP 3316967 B2 JP3316967 B2 JP 3316967B2 JP 25921993 A JP25921993 A JP 25921993A JP 25921993 A JP25921993 A JP 25921993A JP 3316967 B2 JP3316967 B2 JP 3316967B2
- Authority
- JP
- Japan
- Prior art keywords
- torque
- moment
- axis
- robot
- inertia
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Landscapes
- Control Of Position Or Direction (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【0001】
【産業上の利用分野】本発明は、軸の回りの重力トルク
を補償するためのバランサを有したロボットにおいて、
各軸の回りの慣性モーメント及び重力トルクがロボット
の姿勢の変化に伴って変化することによって生じる軌跡
制御性能の低下を防止した装置に関する。
を補償するためのバランサを有したロボットにおいて、
各軸の回りの慣性モーメント及び重力トルクがロボット
の姿勢の変化に伴って変化することによって生じる軌跡
制御性能の低下を防止した装置に関する。
【0002】
【従来の技術】多関節ロボットにおいては、軸の回りの
重力トルクを補償するためにバランサを取り付けたもの
が存在する。このバランサを取り付けたロボットにおい
ても、姿勢変化に伴って、各軸の回りの慣性モーメント
及び重力トルクが変化する。このため、位置及び速度フ
ィードバックループのゲインを一定とすると、ロボット
の全作動領域において位置決め誤差や位置決め速度を一
定にすることは困難である。例えば、位置や速度のフィ
ードバックループのゲインを慣性モーメントや重力トル
クが最大となる姿勢の時に所定の位置決め誤差及び位置
決め速度が得られるように設定した場合には、慣性モー
メントや重力トルクが小さくなる姿勢では、フィードバ
ックループのゲインが高くなり過ぎ、振動が発生し、位
置決め時間が長くなるという問題がある。逆に、フィー
ドバックループのゲインを慣性モーメントや重力トルク
が最小となる姿勢の時に所定の位置決め誤差及び位置決
め速度が得られるように設定した場合には、慣性モーメ
ントや重力トルクが大きくなる姿勢では、フィードバッ
クループのゲインが不足し、位置決め誤差が増大した
り、追随遅れが増大し、位置決め時間が長くなるという
問題がある。
重力トルクを補償するためにバランサを取り付けたもの
が存在する。このバランサを取り付けたロボットにおい
ても、姿勢変化に伴って、各軸の回りの慣性モーメント
及び重力トルクが変化する。このため、位置及び速度フ
ィードバックループのゲインを一定とすると、ロボット
の全作動領域において位置決め誤差や位置決め速度を一
定にすることは困難である。例えば、位置や速度のフィ
ードバックループのゲインを慣性モーメントや重力トル
クが最大となる姿勢の時に所定の位置決め誤差及び位置
決め速度が得られるように設定した場合には、慣性モー
メントや重力トルクが小さくなる姿勢では、フィードバ
ックループのゲインが高くなり過ぎ、振動が発生し、位
置決め時間が長くなるという問題がある。逆に、フィー
ドバックループのゲインを慣性モーメントや重力トルク
が最小となる姿勢の時に所定の位置決め誤差及び位置決
め速度が得られるように設定した場合には、慣性モーメ
ントや重力トルクが大きくなる姿勢では、フィードバッ
クループのゲインが不足し、位置決め誤差が増大した
り、追随遅れが増大し、位置決め時間が長くなるという
問題がある。
【0003】この課題を解決するために、教示データ間
を補間して各軸の指令角度を演算すると共にその指令角
度における各軸の回りの慣性モーメント及び重力トルク
を演算して、それらの値に基づいて位置及び速度フィー
ドバックループのゲインを変化させることが考えられ
る。
を補間して各軸の指令角度を演算すると共にその指令角
度における各軸の回りの慣性モーメント及び重力トルク
を演算して、それらの値に基づいて位置及び速度フィー
ドバックループのゲインを変化させることが考えられ
る。
【0004】
【発明が解決しようとする課題】しかしながら、軸の回
りの重力トルクを補償するためのバランサを用いたロボ
ットでは、このバランサによる重力トルクの補償量がロ
ボットの姿勢によって変化する。従って、軸の回りの重
力トルクを演算する場合にも、補償量を考慮に入れる必
要があり、しかも、姿勢によって補償量が変化すること
も考慮に入れなけ れば、サーボ制御性能が向上しない。
そこで、本発明の第1の目的は、バランサによる重力ト
ルクの補償を考慮に入れて、そのトルクに応じた制御を
行うことで、制御性能を向上させることにある。さら
に、教示データの補間には多大な時間がかかる上に、各
補間点における慣性モーメント及び重力トルクを演算す
ると、CPUの演算時間が長くなり、教示データの補間
周期が長くなる。この結果、ロボットの姿勢変化速度を
大きくすることができないという問題が発生する。
りの重力トルクを補償するためのバランサを用いたロボ
ットでは、このバランサによる重力トルクの補償量がロ
ボットの姿勢によって変化する。従って、軸の回りの重
力トルクを演算する場合にも、補償量を考慮に入れる必
要があり、しかも、姿勢によって補償量が変化すること
も考慮に入れなけ れば、サーボ制御性能が向上しない。
そこで、本発明の第1の目的は、バランサによる重力ト
ルクの補償を考慮に入れて、そのトルクに応じた制御を
行うことで、制御性能を向上させることにある。さら
に、教示データの補間には多大な時間がかかる上に、各
補間点における慣性モーメント及び重力トルクを演算す
ると、CPUの演算時間が長くなり、教示データの補間
周期が長くなる。この結果、ロボットの姿勢変化速度を
大きくすることができないという問題が発生する。
【0005】又、慣性モーメント及び重力トルクの演算
量を減少させる方法として、特開昭59−220806
号公報に記載のものが存在する。この方法は、位置及び
姿勢の教示時に、教示点での慣性モーメント及び重力ト
ルクを演算して、教示データと共に記憶しておき、ロボ
ットの動作時には、教示点間の補間演算と共に慣性モー
メント及び重力トルクも補間演算するという方法であ
る。この方法によれば、教示点での慣性モーメント及び
重力トルクを各軸の角度から演算する必要がないことか
ら、CPUの演算時間は短縮され、教示データの補間周
期が長くならないという利点がある。
量を減少させる方法として、特開昭59−220806
号公報に記載のものが存在する。この方法は、位置及び
姿勢の教示時に、教示点での慣性モーメント及び重力ト
ルクを演算して、教示データと共に記憶しておき、ロボ
ットの動作時には、教示点間の補間演算と共に慣性モー
メント及び重力トルクも補間演算するという方法であ
る。この方法によれば、教示点での慣性モーメント及び
重力トルクを各軸の角度から演算する必要がないことか
ら、CPUの演算時間は短縮され、教示データの補間周
期が長くならないという利点がある。
【0006】しかしながら、上記のように、教示時に教
示点における慣性モーメント及び重力トルクを演算する
という方法は、教示時において、ロボットの手首部先端
に取り付けられるハンドやこのハンドが把持する工具の
重心や質量が既知であることが必要となるため、工具を
動作時と同一工具にしておかなければならないという問
題がある。又、別の工具を用いて同一教示データで加工
する場合には、各教示点における慣性モーメント及び重
力トルクが異なるため精度の高い制御を行うことが困難
である。さらに、異なる重量の物体をロボットにより把
持して同一経路で移動させる様な場合には、重量が異な
れば、同一姿勢でも慣性モーメント及び重力トルクが異
なるので、それぞれの教示データを作成する必要があ
る。又、重量物の把持前と把持後では、同一姿勢でも慣
性モーメント及び重力トルクが異なるため、単一の教示
データでは対応できないという問題がある。そこで、本
発明の第2の目的は、動作時において教示データの補間
周期を長くすることがなく、各軸の重力トルクを演算し
た上で、そのトルクに応じた制御を行うことである。
示点における慣性モーメント及び重力トルクを演算する
という方法は、教示時において、ロボットの手首部先端
に取り付けられるハンドやこのハンドが把持する工具の
重心や質量が既知であることが必要となるため、工具を
動作時と同一工具にしておかなければならないという問
題がある。又、別の工具を用いて同一教示データで加工
する場合には、各教示点における慣性モーメント及び重
力トルクが異なるため精度の高い制御を行うことが困難
である。さらに、異なる重量の物体をロボットにより把
持して同一経路で移動させる様な場合には、重量が異な
れば、同一姿勢でも慣性モーメント及び重力トルクが異
なるので、それぞれの教示データを作成する必要があ
る。又、重量物の把持前と把持後では、同一姿勢でも慣
性モーメント及び重力トルクが異なるため、単一の教示
データでは対応できないという問題がある。そこで、本
発明の第2の目的は、動作時において教示データの補間
周期を長くすることがなく、各軸の重力トルクを演算し
た上で、そのトルクに応じた制御を行うことである。
【0008】
【課題を解決するための手段】上記第1の目的を達成す
るための発明の構成は、軸にかかる重力トルクを減少さ
せるためのバランサを有したロボットの制御装置であっ
て、位置フィードバックループ、速度フィードバックル
ープ及び電流フィードバックループを有し、前記ロボッ
トの工具を指令された位置及び姿勢に追随させる制御装
置において、ロボットの各軸の回転角を検出する角度検
出手段と、前記角度検出手段により検出された各軸の回
転角とロボットの各リンクの長さ及び質量とから慣性モ
ーメント及び重力トルクを演算するモーメント/トルク
演算手段と、前記角度検出手段により検出された各軸の
回転角、及び、バランサの構造に基づいた定数から前記
バランサの取り付けられている軸の回りの前記バランサ
による補償トルクを演算する補償トルク演算手段と、前
記モーメント/トルク演算手段により演算された重力ト
ルクと前記補償トルク演算手段により演算された補償ト
ルクとを合成するトルク合成手段と、前記モーメント/
トルク演算手段により演算された慣性モーメントに基づ
いて前記速度フィードバックループのゲインを変化させ
る第1補正手段と、前記トルク合成手段により演算され
た合成トルクに基づいて前記電流フィードバックループ
への指令値を加算補正する第2補正手段とから構成した
ことである。又、第1の目的に加え、第2の目的を達成す
る発明の構成は、前記モーメント/トルク演算手段及び
補償トルク演算手段を、前記工具が指令された位置への
移動経路を決定するために周期的に実行される補間点演
算の間の空時間を用いて実行するようにしたことであ
る。
るための発明の構成は、軸にかかる重力トルクを減少さ
せるためのバランサを有したロボットの制御装置であっ
て、位置フィードバックループ、速度フィードバックル
ープ及び電流フィードバックループを有し、前記ロボッ
トの工具を指令された位置及び姿勢に追随させる制御装
置において、ロボットの各軸の回転角を検出する角度検
出手段と、前記角度検出手段により検出された各軸の回
転角とロボットの各リンクの長さ及び質量とから慣性モ
ーメント及び重力トルクを演算するモーメント/トルク
演算手段と、前記角度検出手段により検出された各軸の
回転角、及び、バランサの構造に基づいた定数から前記
バランサの取り付けられている軸の回りの前記バランサ
による補償トルクを演算する補償トルク演算手段と、前
記モーメント/トルク演算手段により演算された重力ト
ルクと前記補償トルク演算手段により演算された補償ト
ルクとを合成するトルク合成手段と、前記モーメント/
トルク演算手段により演算された慣性モーメントに基づ
いて前記速度フィードバックループのゲインを変化させ
る第1補正手段と、前記トルク合成手段により演算され
た合成トルクに基づいて前記電流フィードバックループ
への指令値を加算補正する第2補正手段とから構成した
ことである。又、第1の目的に加え、第2の目的を達成す
る発明の構成は、前記モーメント/トルク演算手段及び
補償トルク演算手段を、前記工具が指令された位置への
移動経路を決定するために周期的に実行される補間点演
算の間の空時間を用いて実行するようにしたことであ
る。
【0009】
【作用】第1の目的を達成する構成により、角度検出手
段により検出された各軸の回転角とロボットの各リンク
の長さ及び質量とから慣性モーメント及び重力トルクを
演算し、角度検出手段により検出された各軸の回転角、
及び、バランサの 構造に基づいた定数から前記バランサ
の取り付けられている軸の回りの前記バランサによる補
償トルクを演算する。これら重力トルクと補償トルクか
ら合成トルクが演算され、慣性モーメントに基づいて前
記速度フィードバックループのゲインを変化させ、合成
トルクに基づいて前記電流フィードバックループへの指
令値を加算補正する。 又、ロボットの動作時には、動作
プログラムに従って指定の教示点への指定された移動が
行われる。この時、教示点間における各軸の角度の指令
値は、教示データの補間によってある補間周期毎に演算
される。
段により検出された各軸の回転角とロボットの各リンク
の長さ及び質量とから慣性モーメント及び重力トルクを
演算し、角度検出手段により検出された各軸の回転角、
及び、バランサの 構造に基づいた定数から前記バランサ
の取り付けられている軸の回りの前記バランサによる補
償トルクを演算する。これら重力トルクと補償トルクか
ら合成トルクが演算され、慣性モーメントに基づいて前
記速度フィードバックループのゲインを変化させ、合成
トルクに基づいて前記電流フィードバックループへの指
令値を加算補正する。 又、ロボットの動作時には、動作
プログラムに従って指定の教示点への指定された移動が
行われる。この時、教示点間における各軸の角度の指令
値は、教示データの補間によってある補間周期毎に演算
される。
【0010】この補間周期から補間演算手段による補間
演算に使用される時間を引いた残りの空時間を利用し
て、検出された各軸の回転角とロボットの各リンクの長
さ及び質量とから、順次、各軸の回りの慣性モーメント
及び重力トルクが繰り返し演算される。この順次繰り返
される演算により、その時の各軸の回りの慣性モーメン
ト及び重力トルクが順次更新される。そして、順次、更
新されているその時の各軸の慣性モーメントに基づいて
速度フィードバックループのゲインが変化される。又、
順次、更新されているその時の合成重力トルクに基づい
て電流フィードバックループへの指令値が加算補正され
る。
演算に使用される時間を引いた残りの空時間を利用し
て、検出された各軸の回転角とロボットの各リンクの長
さ及び質量とから、順次、各軸の回りの慣性モーメント
及び重力トルクが繰り返し演算される。この順次繰り返
される演算により、その時の各軸の回りの慣性モーメン
ト及び重力トルクが順次更新される。そして、順次、更
新されているその時の各軸の慣性モーメントに基づいて
速度フィードバックループのゲインが変化される。又、
順次、更新されているその時の合成重力トルクに基づい
て電流フィードバックループへの指令値が加算補正され
る。
【0011】
【発明の効果】このように、バランサによる軸回りの重
力トルクの補償量も考慮した合成重力トルクを演算して
いるので、姿勢に係わらず制御性能が向上する。 又、モ
ーメント/トルク演算手段による演算は、補間周期から
補間演算手段による補間演算に使用される時間を引いた
残りの空時間に実行され、数回の補間周期を使って演算
が完了されるので、慣性モーメント及び重力トルクの演
算のために補間演算の周期が長くなるということが防止
される。つまり、慣性モーメント及び重力トルクの演算
は上記の残り時間を利用して分割して演算される。この
ような演算を行うことによって、各補間点毎の慣性モー
メント及び重力トルクを演算することはできないが、リ
アルタイムによる演算が可能となるため、ロボット等が
各種のワークを把持する場合でも容易に対応できる。
力トルクの補償量も考慮した合成重力トルクを演算して
いるので、姿勢に係わらず制御性能が向上する。 又、モ
ーメント/トルク演算手段による演算は、補間周期から
補間演算手段による補間演算に使用される時間を引いた
残りの空時間に実行され、数回の補間周期を使って演算
が完了されるので、慣性モーメント及び重力トルクの演
算のために補間演算の周期が長くなるということが防止
される。つまり、慣性モーメント及び重力トルクの演算
は上記の残り時間を利用して分割して演算される。この
ような演算を行うことによって、各補間点毎の慣性モー
メント及び重力トルクを演算することはできないが、リ
アルタイムによる演算が可能となるため、ロボット等が
各種のワークを把持する場合でも容易に対応できる。
【0012】
【実施例】以下本発明を具体的な実施例に基づいて説明
する。図1は6軸多関節ロボットの機構を示した機構図
である。10がロボット本体であり、フロアに本体10
を固定するベース13が配設され、ベース13上にはコ
ラム12が固設されており、コラム12はボディ14を
回転自在に配設している。ボディ14はアッパーアーム
15を回動自在に軸支し、アッパーアーム15は、フォ
アアーム16を回動自在に軸支している。ボディ14、
アッパーアーム15、フォアアーム16は、それぞれ、
サーボモータSm1,Sm2,Sm3(図2参照)によって、
軸a,b,cの回りに回転駆動される。この回転角はエ
ンコーダE1,E2,E3によって検出される。フォア
アーム16の先端部にはツイストリスト17がd軸の周
りに回転可能に軸支され、ツイストリスト17にはベン
ドリスト9がe軸の周りに回動自在に軸支されている。
ベンドリスト9には先端にフランジ18aを有するスイ
ベルリスト18がf軸の回りに回転可能に軸支されてい
る。また、フランジ18aにはハンド19が取り付けら
れている。d軸、e軸、f軸はサーボモータSm4、Sm
5、Sm6によって駆動され、その回転角はエンコーダE
4,E5,E6によって検出される。ハンド19の開閉
動作は工具駆動回路23により制御される。
する。図1は6軸多関節ロボットの機構を示した機構図
である。10がロボット本体であり、フロアに本体10
を固定するベース13が配設され、ベース13上にはコ
ラム12が固設されており、コラム12はボディ14を
回転自在に配設している。ボディ14はアッパーアーム
15を回動自在に軸支し、アッパーアーム15は、フォ
アアーム16を回動自在に軸支している。ボディ14、
アッパーアーム15、フォアアーム16は、それぞれ、
サーボモータSm1,Sm2,Sm3(図2参照)によって、
軸a,b,cの回りに回転駆動される。この回転角はエ
ンコーダE1,E2,E3によって検出される。フォア
アーム16の先端部にはツイストリスト17がd軸の周
りに回転可能に軸支され、ツイストリスト17にはベン
ドリスト9がe軸の周りに回動自在に軸支されている。
ベンドリスト9には先端にフランジ18aを有するスイ
ベルリスト18がf軸の回りに回転可能に軸支されてい
る。また、フランジ18aにはハンド19が取り付けら
れている。d軸、e軸、f軸はサーボモータSm4、Sm
5、Sm6によって駆動され、その回転角はエンコーダE
4,E5,E6によって検出される。ハンド19の開閉
動作は工具駆動回路23により制御される。
【0013】軸bの回りの重力トルクはロボットの姿勢
によって大きく変化する。この重力トルクの姿勢による
変動を補償するためにコイルスプリングによるバランサ
11が設けられている。このバラサン11の一点111
はボディ14において軸bを通る鉛直線上に固定されて
いる。又、他点112はアッパーアーム15においてb
軸とc軸を含む平面上であってb軸とc軸とを結ぶ線分
g上に固定さている。そして、アッパーアーム15が鉛
直線上方向から鉛直線下方向に向かって回転するに連れ
て、このバランサ11は伸びる。従って、アッパーアー
ム15は軸bの回りにバランサ11の引張力により鉛直
上向きに回転する方向の補償トルクを受ける。この補償
トルクが軸bの回りに姿勢によって変化する重力トルク
をある程度補償する。
によって大きく変化する。この重力トルクの姿勢による
変動を補償するためにコイルスプリングによるバランサ
11が設けられている。このバラサン11の一点111
はボディ14において軸bを通る鉛直線上に固定されて
いる。又、他点112はアッパーアーム15においてb
軸とc軸を含む平面上であってb軸とc軸とを結ぶ線分
g上に固定さている。そして、アッパーアーム15が鉛
直線上方向から鉛直線下方向に向かって回転するに連れ
て、このバランサ11は伸びる。従って、アッパーアー
ム15は軸bの回りにバランサ11の引張力により鉛直
上向きに回転する方向の補償トルクを受ける。この補償
トルクが軸bの回りに姿勢によって変化する重力トルク
をある程度補償する。
【0014】図2は、本発明のロボットの制御装置の電
気的構成を示したブロックダイヤグラムである。CPU
20には、メモリ25、サーボモータを駆動するための
サーボCPU22a〜22f、動作開始指令、ジョグ運
転の指令、教示点の指示等を行う操作盤26が接続され
ている。ロボットに取付けられた各軸a〜f駆動用のサ
ーボモータSm1〜Sm6は、それぞれサーボCPU22a
〜22fによって駆動される。
気的構成を示したブロックダイヤグラムである。CPU
20には、メモリ25、サーボモータを駆動するための
サーボCPU22a〜22f、動作開始指令、ジョグ運
転の指令、教示点の指示等を行う操作盤26が接続され
ている。ロボットに取付けられた各軸a〜f駆動用のサ
ーボモータSm1〜Sm6は、それぞれサーボCPU22a
〜22fによって駆動される。
【0015】サーボCPU22a〜22fのそれぞれ
は、CPU20から出力される各軸の角度指令値θ1 〜
θ6 、慣性モーメントDi 、重力トルクTi に基づい
て、サーボモータSm1〜Sm6の出力トルクを制御する。
各駆動軸に連結されたエンコーダE1〜E6の出力する
検出角度α1 〜α6 はCPU20及びサーボCPU22
a〜22fに入力しており、CPU20による各軸の慣
性モーメント及び重力トルクの演算及びサーボCPU2
2a〜22fによる位置フィードバック制御、速度フィ
ードバック制御、電流フィードバック制御に用いられ
る。
は、CPU20から出力される各軸の角度指令値θ1 〜
θ6 、慣性モーメントDi 、重力トルクTi に基づい
て、サーボモータSm1〜Sm6の出力トルクを制御する。
各駆動軸に連結されたエンコーダE1〜E6の出力する
検出角度α1 〜α6 はCPU20及びサーボCPU22
a〜22fに入力しており、CPU20による各軸の慣
性モーメント及び重力トルクの演算及びサーボCPU2
2a〜22fによる位置フィードバック制御、速度フィ
ードバック制御、電流フィードバック制御に用いられ
る。
【0016】メモリ25にはロボットを教示点データに
従って動作させるためのプログラムが記憶されたPA領
域とハンド19の位置と姿勢を表す教示点データを記憶
するPDA領域とフランジ18aから先の重量物、即
ち、ハンド19、又は、ハンド19とそれにより握持さ
れた物体を総合した物体の重心座標(a,b,c)とそ
の質量mL 、ロボットの各アームの長さ重量、バランサ
11の幾何学的位置関係及びバネ定数等の慣性モーメン
ト、質量を記憶するSDA領域とサーボパラメータを記
憶するITA領域と補間演算により求められた補間点に
おける各軸の角度指令値θ1 〜θ6 を記憶するINA領
域とエンコーダE1〜E6から出力された検出角度α1
〜α6 を記憶するANG領域とが形成されている。
従って動作させるためのプログラムが記憶されたPA領
域とハンド19の位置と姿勢を表す教示点データを記憶
するPDA領域とフランジ18aから先の重量物、即
ち、ハンド19、又は、ハンド19とそれにより握持さ
れた物体を総合した物体の重心座標(a,b,c)とそ
の質量mL 、ロボットの各アームの長さ重量、バランサ
11の幾何学的位置関係及びバネ定数等の慣性モーメン
ト、質量を記憶するSDA領域とサーボパラメータを記
憶するITA領域と補間演算により求められた補間点に
おける各軸の角度指令値θ1 〜θ6 を記憶するINA領
域とエンコーダE1〜E6から出力された検出角度α1
〜α6 を記憶するANG領域とが形成されている。
【0017】各軸のサーボCPU22a〜22fは、図
3に示す回路の機能をシーケンシャルなディジタル制御
により達成するものである。即ち、各回転角の位置のフ
ィードバックループと速度のフィードバックループとで
電流のフィードバックループと速度のフィードフォワー
ドループと加速度のフィードフォワードループとを有し
ている。
3に示す回路の機能をシーケンシャルなディジタル制御
により達成するものである。即ち、各回転角の位置のフ
ィードバックループと速度のフィードバックループとで
電流のフィードバックループと速度のフィードフォワー
ドループと加速度のフィードフォワードループとを有し
ている。
【0018】次に、本装置の作動について説明する。図
5はRAM25のPA領域に記憶されている動作プログ
ラムである。行番号10、50、90が重量物の重心座
標r(a,b,c)とその質量mL とを設定するための
命令語である。この命令語の実行により重心座標r
(a,b,c)と質量mL のデータがRAM25のSD
A領域に記憶される。命令語の領域Aが重心座標rを表
し、領域Bが質量mL を表す。重心座標(a,b,c)
はスイベルリスト18のフランジ18aの中心に固定さ
れたリンク座標系から見た座標、即ち、ノーマル、オリ
エント、アプローチの各座標(mm単位) で与えられる。
又、質量はkg単位で表記される。行番号10、90は、
フランジ18aから先の重量物がハンド19だけの場合
のデータ設定に関する命令語であり、行番号50はハン
ド19により物体が握持された場合のデータ設定に関す
る命令語である。又、行番号20及び80のHAND
OFF命令語は、ハンド19を開く命令語であり、行番
号40のHAND ON命令語はハンド19を閉じる命
令語である。さらに、行番号30、60、70、10
0、110のMOVE命令語は指定された教示点Pnに
移動させるための命令語である。
5はRAM25のPA領域に記憶されている動作プログ
ラムである。行番号10、50、90が重量物の重心座
標r(a,b,c)とその質量mL とを設定するための
命令語である。この命令語の実行により重心座標r
(a,b,c)と質量mL のデータがRAM25のSD
A領域に記憶される。命令語の領域Aが重心座標rを表
し、領域Bが質量mL を表す。重心座標(a,b,c)
はスイベルリスト18のフランジ18aの中心に固定さ
れたリンク座標系から見た座標、即ち、ノーマル、オリ
エント、アプローチの各座標(mm単位) で与えられる。
又、質量はkg単位で表記される。行番号10、90は、
フランジ18aから先の重量物がハンド19だけの場合
のデータ設定に関する命令語であり、行番号50はハン
ド19により物体が握持された場合のデータ設定に関す
る命令語である。又、行番号20及び80のHAND
OFF命令語は、ハンド19を開く命令語であり、行番
号40のHAND ON命令語はハンド19を閉じる命
令語である。さらに、行番号30、60、70、10
0、110のMOVE命令語は指定された教示点Pnに
移動させるための命令語である。
【0019】図5の動作プログラムにより、ロボット
は、ハンド19を開いて、P1点に移動し、その後、ハ
ンド19を閉じて物体を握持し、P2点を経由してP3
点に移動して、ハンド19を開いて、物体を放置し、P
4点を経由してP5点に移動することが可能である。
は、ハンド19を開いて、P1点に移動し、その後、ハ
ンド19を閉じて物体を握持し、P2点を経由してP3
点に移動して、ハンド19を開いて、物体を放置し、P
4点を経由してP5点に移動することが可能である。
【0020】図4は、CPU20による動作プログラム
を解読するための主プログラムのフローチャートであ
る。ステップ100において、MOVE命令語が解読さ
れると、ステップ102において、現在位置から指定さ
れた教示点までハンド19を移動させるための補間演算
が実行される。そして、補間演算により求められた各軸
の角度指令値θ1 〜θ6 はサーボCPU22a〜22f
に出力される。又、ステップ104において、LOAD
命令語が解読されると、ステップ106において、その
命令語に表記されているハンドから先の重量物の重心座
標と質量のデータがRAM25のSDA領域に記憶され
る。又、ステップ108でHAND OFF命令語が解
読されると、ステップ110において、工具駆動回路2
3にハンド19を開く指令を与える。又、ステップ11
2でHAND ON命令語が解読されると、ステップ1
14において、工具駆動回路23にハンド19を閉じる
指令を与える。
を解読するための主プログラムのフローチャートであ
る。ステップ100において、MOVE命令語が解読さ
れると、ステップ102において、現在位置から指定さ
れた教示点までハンド19を移動させるための補間演算
が実行される。そして、補間演算により求められた各軸
の角度指令値θ1 〜θ6 はサーボCPU22a〜22f
に出力される。又、ステップ104において、LOAD
命令語が解読されると、ステップ106において、その
命令語に表記されているハンドから先の重量物の重心座
標と質量のデータがRAM25のSDA領域に記憶され
る。又、ステップ108でHAND OFF命令語が解
読されると、ステップ110において、工具駆動回路2
3にハンド19を開く指令を与える。又、ステップ11
2でHAND ON命令語が解読されると、ステップ1
14において、工具駆動回路23にハンド19を閉じる
指令を与える。
【0021】ステップ102における教示点間の補間演
算は、図6に示すフローチャートに従って実行される。
この補間演算は、例えば、8mS毎の補間周期で実行され
る。補間演算は、良く知られたように、回転主軸法等を
用いて行うことができる(例えば、特開昭62−154
006号)。ステップ200において、ワールド座標系
で表記された教示データとして与えられた開始点の位置
及び姿勢と次の位置決め目標点の位置及び姿勢から回転
主軸の方向ベクトルが演算され、ステップ202におい
て、その回転主軸回りの回転角Θが演算される。次に、
ステップ204において、開始点の位置及び姿勢を基準
とした補間角ΔΘが演算され、次のステップ206にお
いて、その補間角ΔΘを用いて開始点の位置及び姿勢を
補間点における位置及び姿勢に変換するための姿勢変換
行列Rが演算される。そして、ステップ208におい
て、開始点の位置及び姿勢を表す同次座標行列に姿勢変
換行列を作用させて、補間点における位置及び姿勢を表
す同次座標行列が演算される。次に、ステップ210に
おいて、その補間点におけるワールド座標系で表記され
た位置及び姿勢の同次座標行列からジョイント座標系で
の値、即ち、各軸の回転角が演算され、この値はRAM
25のINA領域に記憶される。
算は、図6に示すフローチャートに従って実行される。
この補間演算は、例えば、8mS毎の補間周期で実行され
る。補間演算は、良く知られたように、回転主軸法等を
用いて行うことができる(例えば、特開昭62−154
006号)。ステップ200において、ワールド座標系
で表記された教示データとして与えられた開始点の位置
及び姿勢と次の位置決め目標点の位置及び姿勢から回転
主軸の方向ベクトルが演算され、ステップ202におい
て、その回転主軸回りの回転角Θが演算される。次に、
ステップ204において、開始点の位置及び姿勢を基準
とした補間角ΔΘが演算され、次のステップ206にお
いて、その補間角ΔΘを用いて開始点の位置及び姿勢を
補間点における位置及び姿勢に変換するための姿勢変換
行列Rが演算される。そして、ステップ208におい
て、開始点の位置及び姿勢を表す同次座標行列に姿勢変
換行列を作用させて、補間点における位置及び姿勢を表
す同次座標行列が演算される。次に、ステップ210に
おいて、その補間点におけるワールド座標系で表記され
た位置及び姿勢の同次座標行列からジョイント座標系で
の値、即ち、各軸の回転角が演算され、この値はRAM
25のINA領域に記憶される。
【0022】この状態で、本補間プログラムは終了し、
ステップ212で、後述する優先度が低いレベルに設定
された慣性モーメント及び重力トルク演算プログラムの
再開処理(中断した命令語の記憶アドレスへの分岐命令
及び状態レジスタ等の復元処理)が行われた後、その慣
性モーメント及び重力トルク演算プログラムが継続起動
される。
ステップ212で、後述する優先度が低いレベルに設定
された慣性モーメント及び重力トルク演算プログラムの
再開処理(中断した命令語の記憶アドレスへの分岐命令
及び状態レジスタ等の復元処理)が行われた後、その慣
性モーメント及び重力トルク演算プログラムが継続起動
される。
【0023】一方、8msec毎のタイマ割り込みが発生す
ると(ステップ214)、ステップ216において、優
先度の低いレベルに設定された慣性モーメント及び重力
トルク演算プログラムへの割込処理(中断した命令語が
記憶されているアドレスの記憶、状態レジスタ等のスタ
ック処理等)が行われ、優先度の高い本補間処理のプロ
グラムが実行さる。そして、ステップ212、214に
おいて、既に、演算完了してRAM25のINA領域に
記憶された補間点の角度指令値θ1 〜θ6 、後述する慣
性モーメント及び重力トルク演算プログラムにより演算
されRAM25のITA領域に記憶されているサーボパ
ラメータDi,Ti がサーボCPU22a〜22fに出力
される。この結果、各軸はその時の位置及び姿勢に対応
した慣性モーメント及び重力トルクを補償したフィード
フォワード制御によりそれらの角度指令値θ1 〜θ6 に
位置決め制御される。そして、ステップ216におい
て、補間点が教示データとして与えられた目標位置に達
したか否かが判定され、補間が終了していない場合に、
次の補間点の位置及び姿勢の演算のために、ステップ2
06以下が実行される。一方、補間が終了した場合に
は、次の命令語を解読するために図4のメインプログラ
ムに戻る。
ると(ステップ214)、ステップ216において、優
先度の低いレベルに設定された慣性モーメント及び重力
トルク演算プログラムへの割込処理(中断した命令語が
記憶されているアドレスの記憶、状態レジスタ等のスタ
ック処理等)が行われ、優先度の高い本補間処理のプロ
グラムが実行さる。そして、ステップ212、214に
おいて、既に、演算完了してRAM25のINA領域に
記憶された補間点の角度指令値θ1 〜θ6 、後述する慣
性モーメント及び重力トルク演算プログラムにより演算
されRAM25のITA領域に記憶されているサーボパ
ラメータDi,Ti がサーボCPU22a〜22fに出力
される。この結果、各軸はその時の位置及び姿勢に対応
した慣性モーメント及び重力トルクを補償したフィード
フォワード制御によりそれらの角度指令値θ1 〜θ6 に
位置決め制御される。そして、ステップ216におい
て、補間点が教示データとして与えられた目標位置に達
したか否かが判定され、補間が終了していない場合に、
次の補間点の位置及び姿勢の演算のために、ステップ2
06以下が実行される。一方、補間が終了した場合に
は、次の命令語を解読するために図4のメインプログラ
ムに戻る。
【0024】上記の処理でステップ200〜204は、
メインプログラムから移行した補間演算の最初に実行さ
れ、ステップ214〜212は、8mS毎のタイマ割り込
みに同期して実行される。よって、次の出力タンミング
のために演算されている角度指令値θ1 〜θ6 が、8mS
毎にステップ218において出力される。そして、ステ
ップ214〜212は、およそ6mSかかり、残りの2mS
が慣性モーメント/重力トルクの演算に使用される。こ
の2mSでは慣性モーメント/重力トルクの演算が完了し
ないので、1補間周期の空時間2mSを断続的に使用し
て、即ち、数回の補間周期を使用して、約6〜12mSか
けて、一回の演算が完了する。
メインプログラムから移行した補間演算の最初に実行さ
れ、ステップ214〜212は、8mS毎のタイマ割り込
みに同期して実行される。よって、次の出力タンミング
のために演算されている角度指令値θ1 〜θ6 が、8mS
毎にステップ218において出力される。そして、ステ
ップ214〜212は、およそ6mSかかり、残りの2mS
が慣性モーメント/重力トルクの演算に使用される。こ
の2mSでは慣性モーメント/重力トルクの演算が完了し
ないので、1補間周期の空時間2mSを断続的に使用し
て、即ち、数回の補間周期を使用して、約6〜12mSか
けて、一回の演算が完了する。
【0025】次に、この優先度の低いレベルに設定され
ている慣性モーメント及び重力トルクの演算手順につい
て、図7に示すフローチャートを参照して説明する。ス
テップ300において、ロボットの軸を特定する軸変数
iが1に初期設定され、ステップ302においてエンコ
ーダE1〜E6から各軸の現在の回転角α1〜α6 が検
出され、その値はRAM25のANG領域に記憶され
る。次に、ステップ304において、i軸の回りの慣性
行列Di ( α1,…, α6)( 慣性モーメントの行列表現)
が演算され、ステップ306において、i軸の回りの重
力トルクTi ( α1,…, α6)が演算される。尚、このス
テップでの重力トルクの演算では、b軸の回りの重力ト
ルクには、バランサ12による補償トルクが加味されて
いない。又、i軸の回りのトルクτi は、次式で良く知
られたように、ロボットアームに関するラグランジュ関
数の微分から求めることができる。
ている慣性モーメント及び重力トルクの演算手順につい
て、図7に示すフローチャートを参照して説明する。ス
テップ300において、ロボットの軸を特定する軸変数
iが1に初期設定され、ステップ302においてエンコ
ーダE1〜E6から各軸の現在の回転角α1〜α6 が検
出され、その値はRAM25のANG領域に記憶され
る。次に、ステップ304において、i軸の回りの慣性
行列Di ( α1,…, α6)( 慣性モーメントの行列表現)
が演算され、ステップ306において、i軸の回りの重
力トルクTi ( α1,…, α6)が演算される。尚、このス
テップでの重力トルクの演算では、b軸の回りの重力ト
ルクには、バランサ12による補償トルクが加味されて
いない。又、i軸の回りのトルクτi は、次式で良く知
られたように、ロボットアームに関するラグランジュ関
数の微分から求めることができる。
【0026】
【数1】τi =Σj=inΣk=ijTr(UjkJjUjiT)D2θk -
ΣmjgUjijrj 但し、コリオリ力と遠心力の成分は小さいとして上式で
は除外した。Jj はj軸を構成する部材だけによるリン
ク慣性行列、Trは行列式の対角成分の和、mj はj軸リ
ンクの質量、gは重力加速度、 jrj はj軸リンク座標
系で表されたj軸リンクの重心のベクトルであり、D2θ
k は角度の時間に関する2次微分、即ち、角加速度であ
る。又、上式の第1項の展開式において、D2θi の係数
が慣性行列Di となり、第2項がi軸の回りの重力トル
クTi となる。又、Ujkは、
ΣmjgUjijrj 但し、コリオリ力と遠心力の成分は小さいとして上式で
は除外した。Jj はj軸を構成する部材だけによるリン
ク慣性行列、Trは行列式の対角成分の和、mj はj軸リ
ンクの質量、gは重力加速度、 jrj はj軸リンク座標
系で表されたj軸リンクの重心のベクトルであり、D2θ
k は角度の時間に関する2次微分、即ち、角加速度であ
る。又、上式の第1項の展開式において、D2θi の係数
が慣性行列Di となり、第2項がi軸の回りの重力トル
クTi となる。又、Ujkは、
【0027】
【数2】0 (j<iの時)
【数3】δ(0A11A2…j-1Aj )/δθi =Πs=1j s-1A s
・δi-1Ai / δθi (j≧iの時) 但し、s≠iである。又、j-1Aj はj軸リンク座標をj
−1軸リンク座標へ変換する同次座標変換行列であり、
0A1 は1軸リンク座標をワールド座標に変換する同次座
標変換行列である。
・δi-1Ai / δθi (j≧iの時) 但し、s≠iである。又、j-1Aj はj軸リンク座標をj
−1軸リンク座標へ変換する同次座標変換行列であり、
0A1 は1軸リンク座標をワールド座標に変換する同次座
標変換行列である。
【0028】Jj は慣性テンソルIijを用いて次の様に
表現することができる。
表現することができる。
【数4】
【0029】但し、rix、riy、rizはj軸リンク座標
から見たj軸リンクの重心の座標であり、miはその質
量である。又、
から見たj軸リンクの重心の座標であり、miはその質
量である。又、
【数5】Iij=∫〔δij〔Σxk2 〕−xiyi 〕dm である。
【0030】尚、LOAD命令語によって設定される重
心座標(a,b,c)及び質量mLにより、J6 が変化
する。J6 は、数4において、(rix,riy,riz)=
(a,b,c)、mi =mL とおくことにより求めるこ
とができる。
心座標(a,b,c)及び質量mLにより、J6 が変化
する。J6 は、数4において、(rix,riy,riz)=
(a,b,c)、mi =mL とおくことにより求めるこ
とができる。
【0031】数1から、現在の検出角度(α1 ,…,α
6 )における各軸の回りの慣性行列Di と重力トルクT
i とが演算される。本実施例では、演算時間を短縮する
ために、他軸との相互作用は小さいとして、(i,i) 対角
成分のみを考慮している。請求の範囲の慣性モーメント
の用語は、対角成分だけでなく慣性行列を含む意味で用
いているが、以下の記載では、便宜上、慣性行列Di の
(i,i) 対角成分を、慣性モーメントDi と記す。
6 )における各軸の回りの慣性行列Di と重力トルクT
i とが演算される。本実施例では、演算時間を短縮する
ために、他軸との相互作用は小さいとして、(i,i) 対角
成分のみを考慮している。請求の範囲の慣性モーメント
の用語は、対角成分だけでなく慣性行列を含む意味で用
いているが、以下の記載では、便宜上、慣性行列Di の
(i,i) 対角成分を、慣性モーメントDi と記す。
【0032】次に、ステップ307において、i=2、
即ち、b軸であるならば、ステップ308においてにお
いて、バランサ11によるb軸の回りの補償トルクが演
算される。補償トルクの演算方法を次に述べる。バラン
サ11の幾何学的配置関係は図9に示すとおりである。
即ち、b軸であるならば、ステップ308においてにお
いて、バランサ11によるb軸の回りの補償トルクが演
算される。補償トルクの演算方法を次に述べる。バラン
サ11の幾何学的配置関係は図9に示すとおりである。
【0033】
【数6】l=(r2+h2−2r×h×cosα)1/2
【数7】δ=l−(r−h)
【数8】β=tan-1{h sinα/(r−h cosα) }
【数9】f=δ×k+f0
【数10】t2=r×f×sinβ×n
【0034】但し、 r:アッパーアーム15の長さ(m) l:バランサ11の長さ(m) h:バランサ11の取付け高さ(m) k:バネ定数(N/m) f0 :バネ初期力(N) n:バランサー数 α:ジョイント角θ2 (rad) β:アッパーアーム15とバランサ11とのなす角 (ra
d) γ:バランサ11のたわみ (m) f:バランサ1本当たりの補償力 (N) t2 :b軸の回りのバランサ11による補償トルク (N
・m)
d) γ:バランサ11のたわみ (m) f:バランサ1本当たりの補償力 (N) t2 :b軸の回りのバランサ11による補償トルク (N
・m)
【0035】次に、ステップ310において、i軸がb
軸(i=2)の場合には、b軸に関してステップ306
で得られた重力トルクT2 にステップ308で得られた
補償トルクt2 を加算して、この値を新らしい合成重力
トルクT2 とする。
軸(i=2)の場合には、b軸に関してステップ306
で得られた重力トルクT2 にステップ308で得られた
補償トルクt2 を加算して、この値を新らしい合成重力
トルクT2 とする。
【0036】ステップ312において、慣性モーメント
Di と重力トルクTi がサーボパラメータとしてRAM
25のITA領域に更新記憶される。この値が図6のス
テップ220においてサーボCPU22a〜22fに出
力される。次に、ステップ310において、軸変数iが
最大値6に達したか否かが判定され、その値に達してい
ない場合には、ステップ312において、軸変数iが1
だけ加算されて、ステップ302に戻り、次の軸に関す
る慣性モーメント及び重力トルクの演算が繰り返し実行
される。又、軸変数iが最大値6になれば、ステップ3
00に戻り、軸変数iを1に初期設定して、1軸から演
算が繰り返し実行される。
Di と重力トルクTi がサーボパラメータとしてRAM
25のITA領域に更新記憶される。この値が図6のス
テップ220においてサーボCPU22a〜22fに出
力される。次に、ステップ310において、軸変数iが
最大値6に達したか否かが判定され、その値に達してい
ない場合には、ステップ312において、軸変数iが1
だけ加算されて、ステップ302に戻り、次の軸に関す
る慣性モーメント及び重力トルクの演算が繰り返し実行
される。又、軸変数iが最大値6になれば、ステップ3
00に戻り、軸変数iを1に初期設定して、1軸から演
算が繰り返し実行される。
【0037】この図7に示すプログラムは、上述したよ
うに、図4、図6に示すプログラムよりも優先度が1ラ
ンク低く設定されているので、図4、図6に示すプログ
ラムが実行されていない時に実行される。このようにし
て、補間演算手段(図6に示す手順を実行する手段)の
空き時間(2mS) において、図7に示す慣性モーメント
及び重力トルクの演算が実行される。従って、図7のプ
ログラムは、連続されて実行されるのではなく、8msec
の補間演算毎に中断され、中断された次の命令語から再
開される。このため、補間周期とは別の遅い周期で各軸
毎に慣性モーメント及び重力トルクが順次、最新の値に
更新されていくことになる。
うに、図4、図6に示すプログラムよりも優先度が1ラ
ンク低く設定されているので、図4、図6に示すプログ
ラムが実行されていない時に実行される。このようにし
て、補間演算手段(図6に示す手順を実行する手段)の
空き時間(2mS) において、図7に示す慣性モーメント
及び重力トルクの演算が実行される。従って、図7のプ
ログラムは、連続されて実行されるのではなく、8msec
の補間演算毎に中断され、中断された次の命令語から再
開される。このため、補間周期とは別の遅い周期で各軸
毎に慣性モーメント及び重力トルクが順次、最新の値に
更新されていくことになる。
【0038】次に、i軸のサーボCPUの動作手順を図
8のフローチャートに従って説明する。ステップ400
において、エンコーダEiから現在の検出角度αi が読
み取られ、ステップ402において検出角度αi の時間
変化率から角速度βi 、角速度βi の時間変化率から角
加速度γi が演算される。次、ステップ404におい
て、RAM25の領域ITAに記憶されている最新の重
力トクルTi と慣性モーメントDi がCPU20を介し
て受信され、ステップ406において、RAM25の領
域INAに記憶されている角度指令値θi がCPU20
を介して受信される。次に、ステップ408において、
角加速度γi と、慣性モーメントDi とバネ定数Kc と
定数K2 に基づいて、次式により、角度指令値θi のフ
ィードフォワード補正が行われる。
8のフローチャートに従って説明する。ステップ400
において、エンコーダEiから現在の検出角度αi が読
み取られ、ステップ402において検出角度αi の時間
変化率から角速度βi 、角速度βi の時間変化率から角
加速度γi が演算される。次、ステップ404におい
て、RAM25の領域ITAに記憶されている最新の重
力トクルTi と慣性モーメントDi がCPU20を介し
て受信され、ステップ406において、RAM25の領
域INAに記憶されている角度指令値θi がCPU20
を介して受信される。次に、ステップ408において、
角加速度γi と、慣性モーメントDi とバネ定数Kc と
定数K2 に基づいて、次式により、角度指令値θi のフ
ィードフォワード補正が行われる。
【0039】
【数11】θinew=θi+γiDiK2/Kc
【0040】次に、ステップ410において、補正され
た角度指令値θinewの検出角度αiに対する角度偏差Δ
θi (=θinew−αi )が演算される。次に、ステップ
412において、角度偏差Δθi 、位置ループゲインK
p を用いて、次式により、角速度指令値ωi が演算され
る。
た角度指令値θinewの検出角度αiに対する角度偏差Δ
θi (=θinew−αi )が演算される。次に、ステップ
412において、角度偏差Δθi 、位置ループゲインK
p を用いて、次式により、角速度指令値ωi が演算され
る。
【0041】
【数12】ωi =KpΔθi
【0042】次に、ステップ414において、検出され
た角速度βi 、ゲインKf を用いて、角速度のフィード
フォワード補正が次式により行われる。
た角速度βi 、ゲインKf を用いて、角速度のフィード
フォワード補正が次式により行われる。
【数13】ωinew=ωi+βiKf 次に、ステップ416において、補正された角速度指令
値ωinewの検出された角速度βi に対する角速度偏差Δ
ωi (=ωinew−βi )が演算される。
値ωinewの検出された角速度βi に対する角速度偏差Δ
ωi (=ωinew−βi )が演算される。
【0043】次に、ステップ418において、電流指令
値Ii が、定数K、慣性モーメントDi を用いて、次式
により、演算される。
値Ii が、定数K、慣性モーメントDi を用いて、次式
により、演算される。
【数14】Ii =KDi'Δωi 但し、Di' は上記の慣性モーメントDi にi軸のサー
ボモータのロータの慣性モーメントを加算した全慣性モ
ーメントである。このように電流指令値を求めるための
速度フィードバックループのゲインが全慣性モーメント
Di'により補償されることになる。即ち、慣性モーメン
トが大きな姿勢では、追随性を向上させるために速度フ
ィードバックループのゲインが大きくなるように制御さ
れる。
ボモータのロータの慣性モーメントを加算した全慣性モ
ーメントである。このように電流指令値を求めるための
速度フィードバックループのゲインが全慣性モーメント
Di'により補償されることになる。即ち、慣性モーメン
トが大きな姿勢では、追随性を向上させるために速度フ
ィードバックループのゲインが大きくなるように制御さ
れる。
【0044】次に、ステップ420において、角加速度
γi と、全慣性モーメントDi'とトルク定数Ktnと定数
Ka を用いて、次式により、電流指令値がフィードフォ
ワード補正される。
γi と、全慣性モーメントDi'とトルク定数Ktnと定数
Ka を用いて、次式により、電流指令値がフィードフォ
ワード補正される。
【数15】Iinew=Ii+γiKaDi'/Ktn 次に、ステップ422において、最新の重力トルクTi
と、ゲインKT とを用いて、次式により、電流指令値の
重力トルクによる補正値が演算される。
と、ゲインKT とを用いて、次式により、電流指令値の
重力トルクによる補正値が演算される。
【数16】Iinnew =Iinew+TiKT
【0045】次に、ステップ424において、最終的に
演算された電流指令値Iinnew に基づいて、i軸のサー
ボモータに対する電流制御が実行され、このサーボモー
タは慣性モーメント及び重力トルクの補償がされた状態
で角度指令値θi に向けて位置決めされる。
演算された電流指令値Iinnew に基づいて、i軸のサー
ボモータに対する電流制御が実行され、このサーボモー
タは慣性モーメント及び重力トルクの補償がされた状態
で角度指令値θi に向けて位置決めされる。
【0046】以上の処理が微小時間間隔で繰り返し実行
されることにより、図3に示す回路機能と等価な処理が
実行される。尚、上記実施例では、慣性行列Di の対角
成分だけで慣性モーメントの補償を行ったが、他の軸と
の相互作用を表した非対角成分を含めた補償を行うよう
にしても良い。
されることにより、図3に示す回路機能と等価な処理が
実行される。尚、上記実施例では、慣性行列Di の対角
成分だけで慣性モーメントの補償を行ったが、他の軸と
の相互作用を表した非対角成分を含めた補償を行うよう
にしても良い。
【0047】尚、上記実施例では、床置きタイプのロボ
ットの場合について述べたが、天吊りタイプのロボット
の場合には、図9の配置関係のバランサにおいて、数
9、数10式に代えて、次の数17、数18式となる。
ットの場合について述べたが、天吊りタイプのロボット
の場合には、図9の配置関係のバランサにおいて、数
9、数10式に代えて、次の数17、数18式となる。
【数17】f=δ×k+f0
【数18】t2=r×f×sinβ×n
【0048】又、上記実施例では、コイルスプリングの
バランサを用いているが、図10に示すようなエアーバ
ランサを用いても良い。その場合には、補償トルクの演
算式は次のようになる。 (1) エアーバランサーの床置きタイプの場合
バランサを用いているが、図10に示すようなエアーバ
ランサを用いても良い。その場合には、補償トルクの演
算式は次のようになる。 (1) エアーバランサーの床置きタイプの場合
【数19】l=(r2+h2−2r×h×cosα) 1/2
【数20】δ=l−(r−h)
【数21】β=tan-1{h sinα/(r−h cosα) }
【数22】A=(π/4) (D2−d2)
【数23】P2×{V2+n×A×(s−δ)}=P1×
(V2+n×A×s)
(V2+n×A×s)
【数24】P2={P1×(V2+n×A×s)}/{V2
+n×A×(s−δ)}
+n×A×(s−δ)}
【数25】f=A×P2
【数26】t2=r×f×sinβ×n
【0049】ただし、 s:シリンダーの全ストローク (m) D:シリンダーの直径 (m) d:ピストンロッドの直径 (m) A:シリンダーの有効断面積 (m2) V2 :サージタンクの体積 (m3) P1 :α=0の時の圧力 (Pa) P2 :α=ジョイント角θ2の時の圧力 (Pa) n:バランサ数
【0050】(2) エアーバランサーの点吊りタイプの場
合
合
【数27】l=(r2+h2−2r×h×cosα)1/2
【数28】δ=l−(r−h)
【数29】β=tan-1{h sinα/(r−h cosα) }
【数30】A=(π/4)D2
【数31】lL=(r2+h2−2r×h×cosαL)1/2
【数32】δL=lL−(r−h)
【数33】P2×(V2+n×A×δ)=P1×(V2+n
×A×δL)
×A×δL)
【数34】P2={P1×(V2+n×A×δL)}/(V
2+n×A×δ)
2+n×A×δ)
【数35】f=−A×P2
【数36】t2=r×f×sinβ×n
【0051】ただし、 lL:α=αLの時のバランサーの長さ (m) αL:ジョイント2のリミット角 (rad) δL:α=αLの時のバランサーのたわみ (m) P1:α=αLの時の圧力 (Pa)
【図1】本発明の具体的な実施例装置で使用されるロボ
ットを示した構成図。
ットを示した構成図。
【図2】ロボットの制御装置の構成を示したブロック
図。
図。
【図3】図2におけるサーボCPUの処理を示したブロ
ック図。
ック図。
【図4】ロボットの位置及び姿勢制御のための主プログ
ラムを示したフローチャート。
ラムを示したフローチャート。
【図5】ロボットの位置及び姿勢制御のための動作プロ
グラムを示した説明図。
グラムを示した説明図。
【図6】補間演算の手順を示したフローチャート。
【図7】慣性モーメント/重力トルクの演算手順を示し
たフローチャート。
たフローチャート。
【図8】サーボCPUによる角度制御の処理手順を示し
たフローチャート。
たフローチャート。
【図9】スプリングバランサの取付け状態における幾何
学的構成を示した説明図。
学的構成を示した説明図。
【図10】エアーバランサの取付け状態における幾何学
的構成を示した説明図。
的構成を示した説明図。
10…ロボット 11…バランサ 15…アッパーアーム 120…バランサ 18…スイベルリスト 18a…フランジ 19…ハンド 20…CPU(補間演算手段、モーメント/トルク演算
手段、補償トルク演算手段、トルク合成手段) 22a〜22f…サーボCPU(角度制御手段、第1補
正手段、第2補正手段) 25…RAM(教示データ記憶手段) E1〜E6…エンコーダ(角度検出手段) ステップ200〜222…補間演算手段 ステップ300〜306…モーメント/トルク演算手段 ステップ308…補償トルク演算手段 ステップ310…トルク合成手段 ステップ400〜424…角度制御手段
手段、補償トルク演算手段、トルク合成手段) 22a〜22f…サーボCPU(角度制御手段、第1補
正手段、第2補正手段) 25…RAM(教示データ記憶手段) E1〜E6…エンコーダ(角度検出手段) ステップ200〜222…補間演算手段 ステップ300〜306…モーメント/トルク演算手段 ステップ308…補償トルク演算手段 ステップ310…トルク合成手段 ステップ400〜424…角度制御手段
───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 昭60−3714(JP,A) 特開 昭61−173885(JP,A) (58)調査した分野(Int.Cl.7,DB名) G05B 19/18 - 19/46 B25J 3/00 - 3/04 B25J 9/10 - 9/22 B25J 13/00 - 13/08 B25J 19/02 - 19/06
Claims (2)
- 【請求項1】軸にかかる重力トルクを減少させるための
バランサを有したロボットの制御装置であって、位置フ
ィードバックループ、速度フィードバックループ及び電
流フィードバックループを有し、前記ロボットの工具を
指令された位置及び姿勢に追随させる制御装置におい
て、ロ ボットの各軸の回転角を検出する角度検出手段と、 前記角度検出手段により検出された各軸の回転角とロボ
ットの各リンクの長さ及び質量とから慣性モーメント及
び重力トルクを演算するモーメント/トルク演算手段
と、 前記角度検出手段により検出された各軸の回転角、及
び、バランサの構造に基づいた定数から前記バランサの
取り付けられている軸の回りの前記バランサによる補償
トルクを演算する補償トルク演算手段と、 前記モーメント/トルク演算手段により演算された重力
トルクと前記補償トルク演算手段により演算された補償
トルクとを合成するトルク合成手段と、 前記モーメント/トルク演算手段により演算された慣性
モーメントに基づいて前記速度フィードバックループの
ゲインを変化させる第1補正手段と、 前記トルク合成手段により演算された合成トルクに基づ
いて前記電流フィードバックループへの指令値を加算補
正する第2補正手段とから成るロボットの制御装置。 - 【請求項2】請求項1のロボットの制御装置において、 前記モーメント/トルク演算手段及び補償トルク演算手
段を、前記工具が指令された位置への移動経路を決定す
るために周期的に実行される補間点演算の間の空時間を
用いて実行するロボットの制御装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP25921993A JP3316967B2 (ja) | 1993-09-22 | 1993-09-22 | ロボットの制御装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP25921993A JP3316967B2 (ja) | 1993-09-22 | 1993-09-22 | ロボットの制御装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JPH0793010A JPH0793010A (ja) | 1995-04-07 |
JP3316967B2 true JP3316967B2 (ja) | 2002-08-19 |
Family
ID=17331062
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP25921993A Expired - Fee Related JP3316967B2 (ja) | 1993-09-22 | 1993-09-22 | ロボットの制御装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP3316967B2 (ja) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109928201A (zh) * | 2019-02-02 | 2019-06-25 | 衢州职业技术学院 | 具有工件搬运机构的智能自动化生产线 |
WO2023201381A1 (de) * | 2022-04-19 | 2023-10-26 | IIES - Inspired Innovation Engineering Services e.U. | Drehantrieb für einen roboterarm |
Families Citing this family (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP1550924A4 (en) * | 2002-10-08 | 2010-04-07 | Yaskawa Denki Seisakusho Kk | DEVICE FOR CREATING OPTIMAL INSTRUCTIONS |
JP5458769B2 (ja) * | 2009-09-18 | 2014-04-02 | 株式会社デンソーウェーブ | ロボットの制御装置 |
GB2477575B (en) * | 2010-02-09 | 2012-02-08 | Servomex Group Ltd | Apparatus and methods for imbalance compensation |
JP5698519B2 (ja) * | 2010-12-22 | 2015-04-08 | 川崎重工業株式会社 | 制御装置およびロボットアームの制御方法 |
JP6255901B2 (ja) | 2013-10-30 | 2018-01-10 | セイコーエプソン株式会社 | ロボット制御装置、ロボットおよびロボットシステム |
JP6977729B2 (ja) * | 2016-09-13 | 2021-12-08 | ソニーグループ株式会社 | 医療用支持アーム装置、医療用システム、及び外科手術用顕微鏡システム |
CN108081278A (zh) * | 2018-01-25 | 2018-05-29 | 巨轮中德机器人智能制造有限公司 | 一种喷涂用机器人快速示教装置 |
CN112847327B (zh) * | 2019-11-12 | 2022-02-11 | 中国科学院沈阳自动化研究所 | 一种用于协作机器人关节的柔顺控制方法 |
-
1993
- 1993-09-22 JP JP25921993A patent/JP3316967B2/ja not_active Expired - Fee Related
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109928201A (zh) * | 2019-02-02 | 2019-06-25 | 衢州职业技术学院 | 具有工件搬运机构的智能自动化生产线 |
CN109928201B (zh) * | 2019-02-02 | 2020-11-06 | 衢州职业技术学院 | 具有工件搬运机构的智能自动化生产线 |
WO2023201381A1 (de) * | 2022-04-19 | 2023-10-26 | IIES - Inspired Innovation Engineering Services e.U. | Drehantrieb für einen roboterarm |
Also Published As
Publication number | Publication date |
---|---|
JPH0793010A (ja) | 1995-04-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6268819B2 (ja) | 多軸型ロボットの軌道生成方法 | |
JP6167770B2 (ja) | 多軸型ロボットの軌道生成方法及び多軸型ロボットの制御装置 | |
JP3207728B2 (ja) | 冗長マニピュレータの制御方法 | |
JP3808321B2 (ja) | ロボット制御装置 | |
US5390288A (en) | Control apparatus for a space robot | |
JPH079606B2 (ja) | ロボット制御装置 | |
JP3316967B2 (ja) | ロボットの制御装置 | |
CN109623812B (zh) | 考虑航天器本体姿态运动的机械臂轨迹规划方法 | |
JP5921248B2 (ja) | ロボットの制御装置、ロボット、そのプログラム及びその制御方法 | |
JPH09136279A (ja) | 力制御ロボットを用いた位相合わせ嵌合方法 | |
JP6123595B2 (ja) | 2軸ロボットの速度制御方法 | |
JPH06190750A (ja) | ロボットの制御装置 | |
JPH07200018A (ja) | ロボットの制御装置 | |
US8670869B2 (en) | Robot controller | |
JPH06289915A (ja) | ディジタルサーボ制御装置 | |
JPH08190433A (ja) | 負荷重量推定方法 | |
JPH08118270A (ja) | 産業用ロボットの制御方法とその装置 | |
CN116901063B (zh) | 一种基于二次规划的冗余空间机械臂轨迹规划方法 | |
JPH0675009U (ja) | サーボ制御装置 | |
JP3194829B2 (ja) | ロボットの動作プログラム生成装置 | |
JPS60118478A (ja) | 関節形ロボツトの位置制御装置 | |
JPH11198072A (ja) | ロボットの最短時間速度制御装置 | |
JP3101134B2 (ja) | ロボット制御装置 | |
CN114378833B (zh) | 一种基于鲁棒约束控制的机械臂轨迹规划方法 | |
JP2594546B2 (ja) | ロボツトの仮想内部モデルに基づく制御方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
LAPS | Cancellation because of no payment of annual fees |