JPH06289915A - ディジタルサーボ制御装置 - Google Patents
ディジタルサーボ制御装置Info
- Publication number
- JPH06289915A JPH06289915A JP9858493A JP9858493A JPH06289915A JP H06289915 A JPH06289915 A JP H06289915A JP 9858493 A JP9858493 A JP 9858493A JP 9858493 A JP9858493 A JP 9858493A JP H06289915 A JPH06289915 A JP H06289915A
- Authority
- JP
- Japan
- Prior art keywords
- current
- target
- angle
- angular velocity
- moment
- 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.)
- Pending
Links
Landscapes
- Numerical Control (AREA)
- Feedback Control In General (AREA)
- Control Of Position Or Direction (AREA)
Abstract
(57)【要約】
【目的】加速時、減速時における追随遅れを防止するこ
と 【構成】 位置フィードバックループ、速度フィードバ
ックループを有したサーボ制御装置において、制御軸の
回りの慣性モーメントを制御軸の角度に対応して演算
し、角加速度とその時の慣性モーメントに基づいて演算
される補正角だけ位置フィードバックループに対して加
算補正する第１補正手段（４０８）、角加速度とその時
の慣性モーメントに基づいて目標電流を加算補正する第
２補正手段（４２０）を設けた。
と 【構成】 位置フィードバックループ、速度フィードバ
ックループを有したサーボ制御装置において、制御軸の
回りの慣性モーメントを制御軸の角度に対応して演算
し、角加速度とその時の慣性モーメントに基づいて演算
される補正角だけ位置フィードバックループに対して加
算補正する第１補正手段（４０８）、角加速度とその時
の慣性モーメントに基づいて目標電流を加算補正する第
２補正手段（４２０）を設けた。
Description
【０００１】
【産業上の利用分野】本発明は、加速時又は減速時にお
いて、サーボモータ軸と負荷軸との間に発生するねじれ
に起因する角度の追随遅れや、慣性モーメントによる角
度の追随遅れを防止するようにしたディジタルサーボ制
御装置に関する。
いて、サーボモータ軸と負荷軸との間に発生するねじれ
に起因する角度の追随遅れや、慣性モーメントによる角
度の追随遅れを防止するようにしたディジタルサーボ制
御装置に関する。
【０００２】
【従来の技術】多関節ロボットにおいては、ロボットの
姿勢変化に伴って、各軸の回りの慣性モーメント及び重
力トルクが変化する。このため、位置及び速度フィード
バックループのゲインを一定とすると、ロボットの全作
動領域において位置決め誤差や位置決め速度を一定にす
ることは困難である。例えば、位置や速度のフィードバ
ックループのゲインを慣性モーメントや重力トルクが最
大となる姿勢の時に所定の位置決め誤差及び位置決め速
度が得られるように設定した場合には、慣性モーメント
や重力トルクが小さくなる姿勢では、フィードバックル
ープのゲインが高くなり過ぎ、振動が発生し、位置決め
時間が長くなるという問題がある。逆に、フィードバッ
クループのゲインを慣性モーメントや重力トルクが最小
となる姿勢の時に所定の位置決め誤差及び位置決め速度
が得られるように設定した場合には、慣性モーメントや
重力トルクが大きくなる姿勢では、フィードバックルー
プのゲインが不足し、位置決め誤差が増大したり、追随
遅れが増大し、位置決め時間が長くなるという問題があ
る。
姿勢変化に伴って、各軸の回りの慣性モーメント及び重
力トルクが変化する。このため、位置及び速度フィード
バックループのゲインを一定とすると、ロボットの全作
動領域において位置決め誤差や位置決め速度を一定にす
ることは困難である。例えば、位置や速度のフィードバ
ックループのゲインを慣性モーメントや重力トルクが最
大となる姿勢の時に所定の位置決め誤差及び位置決め速
度が得られるように設定した場合には、慣性モーメント
や重力トルクが小さくなる姿勢では、フィードバックル
ープのゲインが高くなり過ぎ、振動が発生し、位置決め
時間が長くなるという問題がある。逆に、フィードバッ
クループのゲインを慣性モーメントや重力トルクが最小
となる姿勢の時に所定の位置決め誤差及び位置決め速度
が得られるように設定した場合には、慣性モーメントや
重力トルクが大きくなる姿勢では、フィードバックルー
プのゲインが不足し、位置決め誤差が増大したり、追随
遅れが増大し、位置決め時間が長くなるという問題があ
る。
【０００３】
【発明が解決しようとする課題】この課題を解決するた
めに、目標角度を微分して角速度を演算し、その角速度
を位置のフィードバックループにフィードフォワードさ
せることが行われている。しかしながら、このような制
御の場合には、慣性モーメントが大きくなると、アーム
の先端が大きく揺れて、長い残留振動が発生するという
問題がある。又、サーボモータ軸と負荷軸との間は弾性
的に結合しているので、加速時及び減速時に、ねじれに
よる追随遅れが発生する。又、慣性モーメントの存在に
より、加速時及び減速時に追随遅れが発生するという問
題がある。
めに、目標角度を微分して角速度を演算し、その角速度
を位置のフィードバックループにフィードフォワードさ
せることが行われている。しかしながら、このような制
御の場合には、慣性モーメントが大きくなると、アーム
の先端が大きく揺れて、長い残留振動が発生するという
問題がある。又、サーボモータ軸と負荷軸との間は弾性
的に結合しているので、加速時及び減速時に、ねじれに
よる追随遅れが発生する。又、慣性モーメントの存在に
より、加速時及び減速時に追随遅れが発生するという問
題がある。
【０００４】本発明は上記の課題を解決するために成さ
れたものであり、その目的は、加速時、減速時における
追随遅れを防止することである。
れたものであり、その目的は、加速時、減速時における
追随遅れを防止することである。
【０００５】
【課題を解決するための手段】上記課題を解決するため
の請求項１に記載の発明の構成は、位置フィードバック
ループ、速度フィードバックループを有し、電流の制御
を行うサーボ制御装置において、制御軸の回りの慣性モ
ーメントを前記制御軸の角度に対応して演算し、角加速
度とその時の慣性モーメントに基づいて演算される補正
角だけ位置フィードバックループに対して加算補正する
第１補正手段を設けたことである。また、請求項２に記
載の発明は、角加速度とその時の慣性モーメントに基づ
いて目標電流を加算補正する第２補正手段を設けたこと
である。
の請求項１に記載の発明の構成は、位置フィードバック
ループ、速度フィードバックループを有し、電流の制御
を行うサーボ制御装置において、制御軸の回りの慣性モ
ーメントを前記制御軸の角度に対応して演算し、角加速
度とその時の慣性モーメントに基づいて演算される補正
角だけ位置フィードバックループに対して加算補正する
第１補正手段を設けたことである。また、請求項２に記
載の発明は、角加速度とその時の慣性モーメントに基づ
いて目標電流を加算補正する第２補正手段を設けたこと
である。
【０００６】
【作用及び発明の効果】目標角度出力手段からは制御軸
が時々刻々とるべき目標角度が出力され、現在角度検出
手段により制御軸の時々刻々の現在角度が検出される。
又、角加速度演算手段により目標角度の時間変動から角
加速度が演算され、モーメント演算手段により目標角度
に対応して各軸の回りの慣性モーメントが演算される。
次に、請求項１の発明では、第１補正手段により、角加
速度とその時の慣性モーメントに基づいて補正角が演算
され、その角度だけ位置フィードバックループに対する
指令値が加算補正される。即ち、角加速度及び慣性モー
メントが大きい程、目標角度は前進補正される。これに
より、加速時及び減速時における、ねじれ及び慣性モー
メントによる追随遅れが防止される。
が時々刻々とるべき目標角度が出力され、現在角度検出
手段により制御軸の時々刻々の現在角度が検出される。
又、角加速度演算手段により目標角度の時間変動から角
加速度が演算され、モーメント演算手段により目標角度
に対応して各軸の回りの慣性モーメントが演算される。
次に、請求項１の発明では、第１補正手段により、角加
速度とその時の慣性モーメントに基づいて補正角が演算
され、その角度だけ位置フィードバックループに対する
指令値が加算補正される。即ち、角加速度及び慣性モー
メントが大きい程、目標角度は前進補正される。これに
より、加速時及び減速時における、ねじれ及び慣性モー
メントによる追随遅れが防止される。
【０００７】次に、請求項１の発明では、目標角度及び
第１補正手段の出力する補正角と、検出された現在角度
との角度偏差に応じて、速度フィードバックループに対
する目標角速度が演算される。一方、請求項２の発明で
は、目標角度及び現在角度との角度偏差に応じて目標角
速度が演算される。そして、その目標角速度と検出され
た現在角速度との角速度偏差に対する比例・積分演算に
より目標電流が演算される。また、請求項２の発明で
は、第２補正手段により、角加速度とその時の慣性モー
メントに基づいて目標電流が加算補正される。即ち、角
加速度及び慣性モーメントが大きい程、目標電流は前進
補正される。これにより、加速時及び減速時における、
ねじれ及び慣性モーメントによる追随遅れが防止され
る。
第１補正手段の出力する補正角と、検出された現在角度
との角度偏差に応じて、速度フィードバックループに対
する目標角速度が演算される。一方、請求項２の発明で
は、目標角度及び現在角度との角度偏差に応じて目標角
速度が演算される。そして、その目標角速度と検出され
た現在角速度との角速度偏差に対する比例・積分演算に
より目標電流が演算される。また、請求項２の発明で
は、第２補正手段により、角加速度とその時の慣性モー
メントに基づいて目標電流が加算補正される。即ち、角
加速度及び慣性モーメントが大きい程、目標電流は前進
補正される。これにより、加速時及び減速時における、
ねじれ及び慣性モーメントによる追随遅れが防止され
る。
【０００８】
【実施例】以下本発明を具体的な実施例に基づいて説明
する。本実施例は、本発明のサーボ制御装置を多関節ロ
ボットの各軸の制御装置に応用した例である。図１は６
軸多関節ロボットの機構を示した機構図である。１０が
ロボット本体であり、フロアに本体１０を固定するベー
ス１３が配設され、ベース１３上にはコラム１２が固設
されており、コラム１２はボディ１４を回転自在に配設
している。ボディ１４はアッパーアーム１５を回動自在
に軸支し、アッパーアーム１５は、フォアアーム１６を
回動自在に軸支している。ボディ１４、アッパーアーム
１５、フォアアーム１６は、それぞれ、サーボモータSm
１，Sm２，Sm３（図２参照）によって、軸ａ，ｂ，ｃの
回りに回転駆動される。この回転角はエンコーダＥ１，
Ｅ２，Ｅ３によって検出される。フォアアーム１６の先
端部にはツイストリスト１７がｄ軸の周りに回転可能に
軸支され、ツイストリスト１７にはベンドリスト９がｅ
軸の周りに回動自在に軸支されている。ベンドリスト９
には先端にフランジ１８ａを有するスイベルリスト１８
がｆ軸の回りに回転可能に軸支されている。また、フラ
ンジ１８ａにはハンド１９が取り付けられている。ｄ
軸、ｅ軸、ｆ軸はサーボモータSm４、Sm５、Sm６によっ
て駆動され、その回転角はエンコーダＥ４，Ｅ５，Ｅ６
によって検出される。ハンド１９の開閉動作は工具駆動
回路２３により制御される。
する。本実施例は、本発明のサーボ制御装置を多関節ロ
ボットの各軸の制御装置に応用した例である。図１は６
軸多関節ロボットの機構を示した機構図である。１０が
ロボット本体であり、フロアに本体１０を固定するベー
ス１３が配設され、ベース１３上にはコラム１２が固設
されており、コラム１２はボディ１４を回転自在に配設
している。ボディ１４はアッパーアーム１５を回動自在
に軸支し、アッパーアーム１５は、フォアアーム１６を
回動自在に軸支している。ボディ１４、アッパーアーム
１５、フォアアーム１６は、それぞれ、サーボモータSm
１，Sm２，Sm３（図２参照）によって、軸ａ，ｂ，ｃの
回りに回転駆動される。この回転角はエンコーダＥ１，
Ｅ２，Ｅ３によって検出される。フォアアーム１６の先
端部にはツイストリスト１７がｄ軸の周りに回転可能に
軸支され、ツイストリスト１７にはベンドリスト９がｅ
軸の周りに回動自在に軸支されている。ベンドリスト９
には先端にフランジ１８ａを有するスイベルリスト１８
がｆ軸の回りに回転可能に軸支されている。また、フラ
ンジ１８ａにはハンド１９が取り付けられている。ｄ
軸、ｅ軸、ｆ軸はサーボモータSm４、Sm５、Sm６によっ
て駆動され、その回転角はエンコーダＥ４，Ｅ５，Ｅ６
によって検出される。ハンド１９の開閉動作は工具駆動
回路２３により制御される。
【０００９】図２は、本発明のロボットの制御装置の電
気的構成を示したブロックダイヤグラムである。ＣＰＵ
２０には、メモリ２５、サーボモータを駆動するための
サーボＣＰＵ２２ａ〜２２ｆ、動作開始指令、ジョグ運
転の指令、教示点の指示等を行う操作盤２６が接続され
ている。ロボットに取付けられた各軸ａ〜ｆ駆動用のサ
ーボモータSm１〜Sm６は、それぞれサーボＣＰＵ２２ａ
〜２２ｆによって駆動される。
気的構成を示したブロックダイヤグラムである。ＣＰＵ
２０には、メモリ２５、サーボモータを駆動するための
サーボＣＰＵ２２ａ〜２２ｆ、動作開始指令、ジョグ運
転の指令、教示点の指示等を行う操作盤２６が接続され
ている。ロボットに取付けられた各軸ａ〜ｆ駆動用のサ
ーボモータSm１〜Sm６は、それぞれサーボＣＰＵ２２ａ
〜２２ｆによって駆動される。
【００１０】サーボＣＰＵ２２ａ〜２２ｆのそれぞれ
は、ＣＰＵ２０から出力される各軸の角度指令値θ1 〜
θ6 、慣性モーメントＤi 、重力トルクＴi に基づい
て、サーボモータSm１〜Sm６の出力トルクを制御する。
各駆動軸に連結されたエンコーダＥ１〜Ｅ６の出力する
検出角度α1 〜α6 はＣＰＵ２０及びサーボＣＰＵ２２
ａ〜２２ｆに入力しており、ＣＰＵ２０による各軸の慣
性モーメント及び重力トルクの演算及びサーボＣＰＵ２
２ａ〜２２ｆによる位置フィードバック制御、速度フィ
ードバック制御、電流フィードバック制御に用いられ
る。
は、ＣＰＵ２０から出力される各軸の角度指令値θ1 〜
θ6 、慣性モーメントＤi 、重力トルクＴi に基づい
て、サーボモータSm１〜Sm６の出力トルクを制御する。
各駆動軸に連結されたエンコーダＥ１〜Ｅ６の出力する
検出角度α1 〜α6 はＣＰＵ２０及びサーボＣＰＵ２２
ａ〜２２ｆに入力しており、ＣＰＵ２０による各軸の慣
性モーメント及び重力トルクの演算及びサーボＣＰＵ２
２ａ〜２２ｆによる位置フィードバック制御、速度フィ
ードバック制御、電流フィードバック制御に用いられ
る。
【００１１】メモリ２５にはロボットを教示点データに
従って動作させるためのプログラムが記憶されたＰＡ領
域とハンド１９の位置と姿勢を表す教示点データを記憶
するＰＤＡ領域とフランジ１８ａから先の重量物、即
ち、ハンド１９、又は、ハンド１９とそれにより握持さ
れた物体を総合した物体の重心座標（ａ，ｂ，ｃ）とそ
の質量ｍL を記憶するＳＤＡ領域とサーボパラメータを
記憶するＩＴＡ領域と補間演算により求められた補間点
における各軸の角度指令値θ1 〜θ6 を記憶するＩＮＡ
領域とエンコーダＥ１〜Ｅ６から出力された検出角度α
1 〜α6 を記憶するＡＮＧ領域とが形成されている。
従って動作させるためのプログラムが記憶されたＰＡ領
域とハンド１９の位置と姿勢を表す教示点データを記憶
するＰＤＡ領域とフランジ１８ａから先の重量物、即
ち、ハンド１９、又は、ハンド１９とそれにより握持さ
れた物体を総合した物体の重心座標（ａ，ｂ，ｃ）とそ
の質量ｍL を記憶するＳＤＡ領域とサーボパラメータを
記憶するＩＴＡ領域と補間演算により求められた補間点
における各軸の角度指令値θ1 〜θ6 を記憶するＩＮＡ
領域とエンコーダＥ１〜Ｅ６から出力された検出角度α
1 〜α6 を記憶するＡＮＧ領域とが形成されている。
【００１２】各軸のサーボＣＰＵ２２ａ〜２２ｆは、図
３に示す回路の機能をシーケンシャルなディジタル制御
により達成するものである。即ち、各回転軸の位置のフ
ィードバックループと速度のフィードバックループとで
アンプの指令電流を制御するための制御ループが構成さ
れている。そして、この制御ループは角速度のフィード
フォワードループ、角加速度のフィードフォワードルー
プ及び重力トルクのフィードフォワードループを有して
いる。そして、角加速度γi と慣性モーメントＤi に基
づいて目標位置が加算補正されており、角加速度γi 及
び慣性モーメントＤi'、重力トルクＴi に基づいてアン
プに対する目標電流が加算補正されている。さらに、角
速度βi に基づいて速度フィードバックループに対する
目標速度が加算補正されている。
３に示す回路の機能をシーケンシャルなディジタル制御
により達成するものである。即ち、各回転軸の位置のフ
ィードバックループと速度のフィードバックループとで
アンプの指令電流を制御するための制御ループが構成さ
れている。そして、この制御ループは角速度のフィード
フォワードループ、角加速度のフィードフォワードルー
プ及び重力トルクのフィードフォワードループを有して
いる。そして、角加速度γi と慣性モーメントＤi に基
づいて目標位置が加算補正されており、角加速度γi 及
び慣性モーメントＤi'、重力トルクＴi に基づいてアン
プに対する目標電流が加算補正されている。さらに、角
速度βi に基づいて速度フィードバックループに対する
目標速度が加算補正されている。
【００１３】次に、本装置の作動について説明する。図
５はＲＡＭ２５のＰＡ領域に記憶されている動作プログ
ラムである。行番号１０、５０、９０がデータ設定手段
を構成する重量物の重心座標（ａ，ｂ，ｃ）とその質量
ｍL とを設定するための命令語である。この命令語の実
行により重心座標（ａ，ｂ，ｃ）と質量ｍL のデータが
ＲＡＭ２５のＳＤＡ領域に記憶される。命令語の領域Ａ
が重心座標ｒを表し、領域Ｂが質量ｍL を表す。重心座
標（ａ，ｂ，ｃ）はスイベルリスト１８のフランジ１８
ａの中心に固定されたリンク座標系から見た座標、即
ち、ノーマル、オリエント、アプローチの各座標（mm単
位) で与えられる。又、質量はkg単位で表記される。行
番号１０、９０は、フランジ１８ａから先の重量物がハ
ンド１９だけの場合のデータ設定に関する命令語であ
り、行番号５０はハンド１９により物体が握持された場
合のデータ設定に関する命令語である。又、行番号２０
及び８０のＨＡＮＤ ＯＦＦ命令語は、ハンド１９を開
く命令語であり、行番号４０のＨＡＮＤ ＯＮ命令語は
ハンド１９を閉じる命令語である。さらに、行番号３
０、６０、７０、１００、１１０のＭＯＶＥ命令語は指
定された教示点Ｐｎに移動させるための命令語である。
５はＲＡＭ２５のＰＡ領域に記憶されている動作プログ
ラムである。行番号１０、５０、９０がデータ設定手段
を構成する重量物の重心座標（ａ，ｂ，ｃ）とその質量
ｍL とを設定するための命令語である。この命令語の実
行により重心座標（ａ，ｂ，ｃ）と質量ｍL のデータが
ＲＡＭ２５のＳＤＡ領域に記憶される。命令語の領域Ａ
が重心座標ｒを表し、領域Ｂが質量ｍL を表す。重心座
標（ａ，ｂ，ｃ）はスイベルリスト１８のフランジ１８
ａの中心に固定されたリンク座標系から見た座標、即
ち、ノーマル、オリエント、アプローチの各座標（mm単
位) で与えられる。又、質量はkg単位で表記される。行
番号１０、９０は、フランジ１８ａから先の重量物がハ
ンド１９だけの場合のデータ設定に関する命令語であ
り、行番号５０はハンド１９により物体が握持された場
合のデータ設定に関する命令語である。又、行番号２０
及び８０のＨＡＮＤ ＯＦＦ命令語は、ハンド１９を開
く命令語であり、行番号４０のＨＡＮＤ ＯＮ命令語は
ハンド１９を閉じる命令語である。さらに、行番号３
０、６０、７０、１００、１１０のＭＯＶＥ命令語は指
定された教示点Ｐｎに移動させるための命令語である。
【００１４】図５の動作プログラムにより、ロボット
は、ハンド１９を開いて、Ｐ１点に移動し、その後、ハ
ンド１９を閉じて物体を握持し、Ｐ２点を経由してＰ３
点に移動して、ハンド１９を開いて、物体を放置し、Ｐ
４点を経由してＰ５点に移動することが可能である。
は、ハンド１９を開いて、Ｐ１点に移動し、その後、ハ
ンド１９を閉じて物体を握持し、Ｐ２点を経由してＰ３
点に移動して、ハンド１９を開いて、物体を放置し、Ｐ
４点を経由してＰ５点に移動することが可能である。
【００１５】図４は、ＣＰＵ２０による動作プログラム
を解読するための主プログラムのフローチャートであ
る。ステップ１００において、ＭＯＶＥ命令語が解読さ
れると、ステップ１０２において、現在位置から指定さ
れた教示点までハンド１９を移動させるための補間演算
が実行される。そして、補間演算により求められた各軸
の角度指令値θ1 〜θ6 はサーボＣＰＵ２２ａ〜２２ｆ
に出力される。又、ステップ１０４において、ＬＯＡＤ
命令語が解読されると、ステップ１０６において、その
命令語に表記されているハンドから先の重量物の重心座
標と質量のデータがＲＡＭ２５のＳＤＡ領域に記憶され
る。又、ステップ１０８でＨＡＮＤ ＯＦＦ命令語が解
読されると、ステップ１１０において、工具駆動回路２
３にハンド１９を開く指令を与える。又、ステップ１１
２でＨＡＮＤ ＯＮ命令語が解読されると、ステップ１
１４において、工具駆動回路２３にハンド１９を閉じる
指令を与える。
を解読するための主プログラムのフローチャートであ
る。ステップ１００において、ＭＯＶＥ命令語が解読さ
れると、ステップ１０２において、現在位置から指定さ
れた教示点までハンド１９を移動させるための補間演算
が実行される。そして、補間演算により求められた各軸
の角度指令値θ1 〜θ6 はサーボＣＰＵ２２ａ〜２２ｆ
に出力される。又、ステップ１０４において、ＬＯＡＤ
命令語が解読されると、ステップ１０６において、その
命令語に表記されているハンドから先の重量物の重心座
標と質量のデータがＲＡＭ２５のＳＤＡ領域に記憶され
る。又、ステップ１０８でＨＡＮＤ ＯＦＦ命令語が解
読されると、ステップ１１０において、工具駆動回路２
３にハンド１９を開く指令を与える。又、ステップ１１
２でＨＡＮＤ ＯＮ命令語が解読されると、ステップ１
１４において、工具駆動回路２３にハンド１９を閉じる
指令を与える。
【００１６】ステップ１０２における教示点間の補間演
算は、図６に示すフローチャートに従って実行される。
補間演算は、良く知られたように、回転主軸法等を用い
て行うことができる（例えば、特開昭６２−１５４００
６号）。ステップ２００において、ワールド座標系で表
記された教示データとして与えられた開始点の位置及び
姿勢と次の位置決め目標点の位置及び姿勢から回転主軸
の方向ベクトルが演算され、ステップ２０２において、
その回転主軸回りの回転角Θが演算される。次に、ステ
ップ２０４において、開始点の位置及び姿勢を基準とし
た補間角ΔΘが演算され、次のステップ２０６におい
て、その補間角ΔΘを用いて開始点の位置及び姿勢を補
間点における位置及び姿勢に変換するための姿勢変換行
列Ｒが演算される。そして、ステップ２０８において、
開始点の位置及び姿勢を表す同次座標行列に姿勢変換行
列を作用させて、補間点における位置及び姿勢を表す同
次座標行列が演算される。次に、ステップ２１０におい
て、その補間点におけるワールド座標系で表記された位
置及び姿勢の同次座標行列からジョイント座標系での
値、即ち、各軸の回転角が演算され、この値はＲＡＭ２
５のＩＮＡ領域に記憶される。この状態で、本補間プロ
グラムは終了し、ステップ２１２で、後述する優先度が
低いレベルに設定された慣性モーメント及び重力トルク
演算プログラムの再開処理（中断した命令語の記憶アド
レスへの分岐命令及び状態レジスタ等の復元処理）が行
われた後、そのプログラムが継続起動される。
算は、図６に示すフローチャートに従って実行される。
補間演算は、良く知られたように、回転主軸法等を用い
て行うことができる（例えば、特開昭６２−１５４００
６号）。ステップ２００において、ワールド座標系で表
記された教示データとして与えられた開始点の位置及び
姿勢と次の位置決め目標点の位置及び姿勢から回転主軸
の方向ベクトルが演算され、ステップ２０２において、
その回転主軸回りの回転角Θが演算される。次に、ステ
ップ２０４において、開始点の位置及び姿勢を基準とし
た補間角ΔΘが演算され、次のステップ２０６におい
て、その補間角ΔΘを用いて開始点の位置及び姿勢を補
間点における位置及び姿勢に変換するための姿勢変換行
列Ｒが演算される。そして、ステップ２０８において、
開始点の位置及び姿勢を表す同次座標行列に姿勢変換行
列を作用させて、補間点における位置及び姿勢を表す同
次座標行列が演算される。次に、ステップ２１０におい
て、その補間点におけるワールド座標系で表記された位
置及び姿勢の同次座標行列からジョイント座標系での
値、即ち、各軸の回転角が演算され、この値はＲＡＭ２
５のＩＮＡ領域に記憶される。この状態で、本補間プロ
グラムは終了し、ステップ２１２で、後述する優先度が
低いレベルに設定された慣性モーメント及び重力トルク
演算プログラムの再開処理（中断した命令語の記憶アド
レスへの分岐命令及び状態レジスタ等の復元処理）が行
われた後、そのプログラムが継続起動される。
【００１７】一方、８msec毎のタイマ割り込みが発生す
ると（ステップ２１４）、ステップ２１６において、優
先度の低いレベルに設定された慣性モーメント及び重力
トルク演算プログラムへの割込処理（中断した命令語が
記憶されているアドレスの記憶、状態レジスタ等のスタ
ック処理等）が行われる。そして、ステップ２１８、２
２０において、ＲＡＭ２５のＩＮＡ領域に記憶された補
間点の角度指令値θ1〜θ6 、慣性モーメント及び重力
トルク演算プログラムにより演算されＲＡＭ２５のＩＴ
Ａ領域に記憶されているサーボパラメータＤi,Ｔi がサ
ーボＣＰＵ２２ａ〜２２ｆに出力される。この結果、各
軸はその時の位置及び姿勢に対応した慣性モーメント及
び重力トルクを補償したフィードフォワード制御により
それらの角度指令値θ1 〜θ6 に位置決め制御される。
そして、ステップ２２２において、補間点が教示データ
として与えられた目標位置に達したか否かが判定され、
補間が終了していない場合に、次の補間点の位置及び姿
勢の演算のために、ステップ２０６以下が実行される。
一方、補間が終了した場合には、次の命令語を解読する
ために図４のメインプログラムに戻る。
ると（ステップ２１４）、ステップ２１６において、優
先度の低いレベルに設定された慣性モーメント及び重力
トルク演算プログラムへの割込処理（中断した命令語が
記憶されているアドレスの記憶、状態レジスタ等のスタ
ック処理等）が行われる。そして、ステップ２１８、２
２０において、ＲＡＭ２５のＩＮＡ領域に記憶された補
間点の角度指令値θ1〜θ6 、慣性モーメント及び重力
トルク演算プログラムにより演算されＲＡＭ２５のＩＴ
Ａ領域に記憶されているサーボパラメータＤi,Ｔi がサ
ーボＣＰＵ２２ａ〜２２ｆに出力される。この結果、各
軸はその時の位置及び姿勢に対応した慣性モーメント及
び重力トルクを補償したフィードフォワード制御により
それらの角度指令値θ1 〜θ6 に位置決め制御される。
そして、ステップ２２２において、補間点が教示データ
として与えられた目標位置に達したか否かが判定され、
補間が終了していない場合に、次の補間点の位置及び姿
勢の演算のために、ステップ２０６以下が実行される。
一方、補間が終了した場合には、次の命令語を解読する
ために図４のメインプログラムに戻る。
【００１８】次に、優先度の低いレベルに設定されてい
る慣性モーメント及び重力トルクの演算手順について、
図７に示すフローチャートを参照して説明する。ステッ
プ３００において、ロボットの軸を特定する軸変数ｉが
１に初期設定され、ステップ３０２においてエンコーダ
Ｅ１〜Ｅ６から各軸の現在の回転角α1〜α6 が検出さ
れ、その値はＲＡＭ２５のＡＮＧ領域に記憶される。次
に、ステップ３０４において、ｉ軸の回りの慣性行列Ｄ
i ( α1,…, α6)( 慣性モーメントの行列表現）が演算
され、ステップ３０６において、ｉ軸の回りの重力トル
クＴi ( α1,…, α6)が演算される。尚、ｉ軸の回りの
トルクτi は、次式で良く知られたように、ロボットア
ームに関するラグランジュ関数の微分から求めることが
できる。
る慣性モーメント及び重力トルクの演算手順について、
図７に示すフローチャートを参照して説明する。ステッ
プ３００において、ロボットの軸を特定する軸変数ｉが
１に初期設定され、ステップ３０２においてエンコーダ
Ｅ１〜Ｅ６から各軸の現在の回転角α1〜α6 が検出さ
れ、その値はＲＡＭ２５のＡＮＧ領域に記憶される。次
に、ステップ３０４において、ｉ軸の回りの慣性行列Ｄ
i ( α1,…, α6)( 慣性モーメントの行列表現）が演算
され、ステップ３０６において、ｉ軸の回りの重力トル
クＴi ( α1,…, α6)が演算される。尚、ｉ軸の回りの
トルクτi は、次式で良く知られたように、ロボットア
ームに関するラグランジュ関数の微分から求めることが
できる。
【００１９】
【数１】τi ＝Σj=i nΣk=i jTr(ＵjkＪjＵji T)D2θk -
ΣｍjｇＵji jｒj 但し、コリオリ力と遠心力の成分は小さいとして上式で
は除外した。Ｊj はｊ軸を構成する部材だけによるリン
ク慣性行列、Trは行列式の対角成分の和、ｍj はｊ軸リ
ンクの質量、ｇは重力加速度、 jｒj はｊ軸リンク座標
系で表されたｊ軸リンクの重心のベクトルであり、D2θ
k は角度の時間に関する２次微分、即ち、角加速度であ
る。又、上式の第１項の展開式において、D2θi の係数
が慣性行列Ｄi となり、第２項がｉ軸の回りの重力トル
クＴi となる。又、Ｕjkは、
ΣｍjｇＵji jｒj 但し、コリオリ力と遠心力の成分は小さいとして上式で
は除外した。Ｊj はｊ軸を構成する部材だけによるリン
ク慣性行列、Trは行列式の対角成分の和、ｍj はｊ軸リ
ンクの質量、ｇは重力加速度、 jｒj はｊ軸リンク座標
系で表されたｊ軸リンクの重心のベクトルであり、D2θ
k は角度の時間に関する２次微分、即ち、角加速度であ
る。又、上式の第１項の展開式において、D2θi の係数
が慣性行列Ｄi となり、第２項がｉ軸の回りの重力トル
クＴi となる。又、Ｕjkは、
【００２０】
【数２】０ （ｊ＜ｉの時）
【数３】δ（0A1 1A2…j-1Aj )/δθi ＝Πs=1 j s-1A s
・δi-1Ai / δθi （ｊ≧ｉの時） 但し、ｓ≠ｉである。又、j-1Aj はｊ軸リンク座標をｊ
−１軸リンク座標へ変換する同次座標変換行列であり、
0A1 は１軸リンク座標をワールド座標に変換する同次座
標変換行列である。
・δi-1Ai / δθi （ｊ≧ｉの時） 但し、ｓ≠ｉである。又、j-1Aj はｊ軸リンク座標をｊ
−１軸リンク座標へ変換する同次座標変換行列であり、
0A1 は１軸リンク座標をワールド座標に変換する同次座
標変換行列である。
【００２１】Ｊj は慣性テンソルＩijを用いて次の様に
表現することができる。
表現することができる。
【数４】
【００２２】但し、ｒix、ｒiy、ｒizはｊ軸リンク座標
から見たｊ軸リンクの重心の座標であり、ｍiはその質
量である。又、
から見たｊ軸リンクの重心の座標であり、ｍiはその質
量である。又、
【数５】 Ｉij＝∫〔δij〔Σｘk 2 〕−ｘiｙi 〕dｍ である。
【００２３】尚、ＬＯＡＤ命令語によって設定される重
心座標（ａ，ｂ，ｃ）及び質量ｍLにより、Ｊ6 が変化
する。Ｊ6 は、数４において、（ｒix，ｒiy，ｒiz）＝
（ａ，ｂ，ｃ）、ｍi ＝ｍL とおくことにより求めるこ
とができる。
心座標（ａ，ｂ，ｃ）及び質量ｍLにより、Ｊ6 が変化
する。Ｊ6 は、数４において、（ｒix，ｒiy，ｒiz）＝
（ａ，ｂ，ｃ）、ｍi ＝ｍL とおくことにより求めるこ
とができる。
【００２４】数１から、現在の検出角度（α1 ，…，α
6 ）における各軸の回りの慣性行列Ｄi と重力トルクＴ
i とが演算される。本実施例では、演算時間を短縮する
ために、他軸との相互作用は小さいとして、(i,i) 対角
成分のみを考慮している。請求の範囲の慣性モーメント
の用語は、対角成分だけでなく慣性行列を含む意味で用
いているが、以下の記載では、便宜上、慣性行列Ｄi の
(i,i) 対角成分を、慣性モーメントＤi と記す。次に、
ステップ３０８において、慣性モーメントＤiと重力ト
ルクＴi がサーボパラメータとしてＲＡＭ２５のＩＴＡ
領域に更新記憶される。この値が図６のステップ２２０
においてサーボＣＰＵ２２ａ〜２２ｆに出力される。次
に、ステップ３１０において、軸変数ｉが最大値６に達
したか否かが判定され、その値に達していない場合に
は、ステップ３１２において、軸変数ｉが１だけ加算さ
れて、ステップ３０２に戻り、次の軸に関する慣性モー
メント及び重力トルクの演算が繰り返し実行される。
又、軸変数ｉが最大値６になれば、ステップ３００に戻
り、軸変数ｉを１に初期設定して、１軸から演算が繰り
返し実行される。
6 ）における各軸の回りの慣性行列Ｄi と重力トルクＴ
i とが演算される。本実施例では、演算時間を短縮する
ために、他軸との相互作用は小さいとして、(i,i) 対角
成分のみを考慮している。請求の範囲の慣性モーメント
の用語は、対角成分だけでなく慣性行列を含む意味で用
いているが、以下の記載では、便宜上、慣性行列Ｄi の
(i,i) 対角成分を、慣性モーメントＤi と記す。次に、
ステップ３０８において、慣性モーメントＤiと重力ト
ルクＴi がサーボパラメータとしてＲＡＭ２５のＩＴＡ
領域に更新記憶される。この値が図６のステップ２２０
においてサーボＣＰＵ２２ａ〜２２ｆに出力される。次
に、ステップ３１０において、軸変数ｉが最大値６に達
したか否かが判定され、その値に達していない場合に
は、ステップ３１２において、軸変数ｉが１だけ加算さ
れて、ステップ３０２に戻り、次の軸に関する慣性モー
メント及び重力トルクの演算が繰り返し実行される。
又、軸変数ｉが最大値６になれば、ステップ３００に戻
り、軸変数ｉを１に初期設定して、１軸から演算が繰り
返し実行される。
【００２５】この図７に示すプログラムは、図４、図６
に示すプログラムよりも優先度が１ランク低く設定され
ているので、図４、図６に示すプログラムが実行されて
いない時に実行される。このようにして、補間演算手段
（図６に示す手順を実行する手段）の空き時間におい
て、図７に示す慣性モーメント及び重力トルクの演算が
実行される。従って、図７のプログラムは、連続されて
実行されるのではなく、８msecの補間演算毎に中断さ
れ、中断された次の命令語から再開される。このため、
補間周期とは別の遅い周期で各軸毎に慣性モーメント及
び重力トルクが順次、最新の値に更新されていくことに
なる。
に示すプログラムよりも優先度が１ランク低く設定され
ているので、図４、図６に示すプログラムが実行されて
いない時に実行される。このようにして、補間演算手段
（図６に示す手順を実行する手段）の空き時間におい
て、図７に示す慣性モーメント及び重力トルクの演算が
実行される。従って、図７のプログラムは、連続されて
実行されるのではなく、８msecの補間演算毎に中断さ
れ、中断された次の命令語から再開される。このため、
補間周期とは別の遅い周期で各軸毎に慣性モーメント及
び重力トルクが順次、最新の値に更新されていくことに
なる。
【００２６】次に、ｉ軸のサーボＣＰＵの動作手順を図
８、図９のフローチャートに従って説明する。ステップ
４００において、エンコーダＥｉから現在の検出角度α
i が読み取られ、ステップ４０２において検出角度αi
の時間変化率から角速度βi 、角速度βi の時間変化率
から角加速度γi が演算される。次に、ステップ４０４
において、ＲＡＭ２５の領域ＩＴＡに記憶されている最
新の重力トクルＴi と慣性モーメントＤi がＣＰＵ２０
を介して受信され、ステップ４０６において、ＲＡＭ２
５の領域ＩＮＡに記憶されている角度指令値θi がＣＰ
Ｕ２０を介して受信される。次に、ステップ４０８にお
いて、角加速度γi と、慣性モーメントＤi とバネ定数
Ｋc と定数Ｋ2 に基づいて、次式により、角度指令値θ
i のフィードフォワード補正が行われる。
８、図９のフローチャートに従って説明する。ステップ
４００において、エンコーダＥｉから現在の検出角度α
i が読み取られ、ステップ４０２において検出角度αi
の時間変化率から角速度βi 、角速度βi の時間変化率
から角加速度γi が演算される。次に、ステップ４０４
において、ＲＡＭ２５の領域ＩＴＡに記憶されている最
新の重力トクルＴi と慣性モーメントＤi がＣＰＵ２０
を介して受信され、ステップ４０６において、ＲＡＭ２
５の領域ＩＮＡに記憶されている角度指令値θi がＣＰ
Ｕ２０を介して受信される。次に、ステップ４０８にお
いて、角加速度γi と、慣性モーメントＤi とバネ定数
Ｋc と定数Ｋ2 に基づいて、次式により、角度指令値θ
i のフィードフォワード補正が行われる。
【００２７】
【数６】θinew＝θi＋γiＤiＫ2／Ｋc
【００２８】次に、ステップ４１０において、補正され
た角度指令値θinewの検出角度αiに対する角度偏差Δ
θi （＝θinew−αi ）が演算される。次に、ステップ
４１２において、角度偏差Δθi 、位置ループゲインＫ
p を用いて、次式により、角速度指令値ωi が演算され
る。
た角度指令値θinewの検出角度αiに対する角度偏差Δ
θi （＝θinew−αi ）が演算される。次に、ステップ
４１２において、角度偏差Δθi 、位置ループゲインＫ
p を用いて、次式により、角速度指令値ωi が演算され
る。
【００２９】
【数７】ωi ＝ＫpΔθi
【００３０】次に、ステップ４１４において、検出され
た角速度βi 、ゲインＫf を用いて、角速度のフィード
フォワード補正が次式により行われる。
た角速度βi 、ゲインＫf を用いて、角速度のフィード
フォワード補正が次式により行われる。
【数８】ωinew＝ωi＋βiＫf 次に、ステップ４１６において、補正された角速度指令
値ωinewの検出された角速度βi に対する角速度偏差Δ
ωi （＝ωinew−βi ）が演算される。
値ωinewの検出された角速度βi に対する角速度偏差Δ
ωi （＝ωinew−βi ）が演算される。
【００３１】次に、ステップ４１７において、慣性モー
メントＤi に定数であるｉ軸のサーボモータのロータの
慣性モーメントを加算した全慣性モーメントＤi 'を用い
て、ＲＯＭ２４の特性記憶領域に記憶されている図１０
に示す特性のマップを検索して、対応するゲインＫ（Ｄ
i ' ）が求められる。この特性は、図１０に示すよう
に、全慣性モーメントＤi 'の１０倍の最大変動範囲に対
してゲインは４倍だけ変動するように圧縮されている。
メントＤi に定数であるｉ軸のサーボモータのロータの
慣性モーメントを加算した全慣性モーメントＤi 'を用い
て、ＲＯＭ２４の特性記憶領域に記憶されている図１０
に示す特性のマップを検索して、対応するゲインＫ（Ｄ
i ' ）が求められる。この特性は、図１０に示すよう
に、全慣性モーメントＤi 'の１０倍の最大変動範囲に対
してゲインは４倍だけ変動するように圧縮されている。
【００３２】次に、ステップ４１８において、電流指令
値Ｉi が、演算されたゲインＫ（Ｄi ' ）を用いて、次
式により、演算される。
値Ｉi が、演算されたゲインＫ（Ｄi ' ）を用いて、次
式により、演算される。
【数９】Ｉi ＝Ｋ（Ｄi '）〔Δωi ＋（ΣΔωi ・Δ
ｔ）／Ｔi 〕 但し、Ｔi は積分時定数、Δｔは、速度制御周期、即
ち、電流指令値Ｉi を求める制御周期である。このよう
に電流指令値を求めるための速度フィードバックループ
のゲインが全慣性モーメントＤi 'の変動範囲に対して圧
縮された変動範囲を有するゲインＫ（Ｄi '）により補償
されることになる。即ち、慣性モーメントが大きな姿勢
では、追随性を向上させるために速度フィードバックル
ープのゲインが大きくなるように制御される。この時、
慣性モーメントの変動範囲に対して、ゲインの変動範囲
は圧縮されているので、系の状態量の発振、電流波形の
ハネが抑制され、モータの過負荷が防止される。従っ
て、サーボ制御の安定性が向上する。
ｔ）／Ｔi 〕 但し、Ｔi は積分時定数、Δｔは、速度制御周期、即
ち、電流指令値Ｉi を求める制御周期である。このよう
に電流指令値を求めるための速度フィードバックループ
のゲインが全慣性モーメントＤi 'の変動範囲に対して圧
縮された変動範囲を有するゲインＫ（Ｄi '）により補償
されることになる。即ち、慣性モーメントが大きな姿勢
では、追随性を向上させるために速度フィードバックル
ープのゲインが大きくなるように制御される。この時、
慣性モーメントの変動範囲に対して、ゲインの変動範囲
は圧縮されているので、系の状態量の発振、電流波形の
ハネが抑制され、モータの過負荷が防止される。従っ
て、サーボ制御の安定性が向上する。
【００３３】次に、ステップ４２０において、角加速度
γi と、全慣性モーメントＤi 'とトルク定数Ｋtnと定数
Ｋa を用いて、次式により、電流指令値に対するフィー
ドフォワード値Ｉf が演算される。
γi と、全慣性モーメントＤi 'とトルク定数Ｋtnと定数
Ｋa を用いて、次式により、電流指令値に対するフィー
ドフォワード値Ｉf が演算される。
【数１０】Ｉf ＝γiＫaＤi '／Ｋtn
【００３４】次に、ステップ４２１において、フィード
フォワード値Ｉf の過去一定期間の値、Ｉf (n),Ｉf (n
-1),…, Ｉf (n-j) を用いて、ローパスフィルタによる
ディジタルフィルタ処理により、現時刻のフィルタリン
クされたフィードフォワード値Ｉf (n)`が演算される。
このフィルタリング処理には、高速フーリエ変換の手法
を用いても良い。即ち、上記の過去一定期間のフィード
フォワード値Ｉf (n),Ｉf (n-1),…, Ｉf (n-j) に対し
てフーリエ変換を行う。次に、その周波数成分のうち、
遮断周波数fc以上の周波数成分を除去する。その後、高
速フーリエ逆変換を行って、実時間の波形を求め、その
波形から現在のフィルタリングされたフィードフォワー
ド値Ｉf (n)`を求めても良い。
フォワード値Ｉf の過去一定期間の値、Ｉf (n),Ｉf (n
-1),…, Ｉf (n-j) を用いて、ローパスフィルタによる
ディジタルフィルタ処理により、現時刻のフィルタリン
クされたフィードフォワード値Ｉf (n)`が演算される。
このフィルタリング処理には、高速フーリエ変換の手法
を用いても良い。即ち、上記の過去一定期間のフィード
フォワード値Ｉf (n),Ｉf (n-1),…, Ｉf (n-j) に対し
てフーリエ変換を行う。次に、その周波数成分のうち、
遮断周波数fc以上の周波数成分を除去する。その後、高
速フーリエ逆変換を行って、実時間の波形を求め、その
波形から現在のフィルタリングされたフィードフォワー
ド値Ｉf (n)`を求めても良い。
【００３５】次に、ステップ４２２において、ステップ
４１８で演算された電流指令値Ｉiに、ステップ４２０
で演算されたフィードフォワード値Ｉf ` が加算（Ｉ
inew＝Ｉi ＋Ｉf ` ）されて、電流値Ｉinewが電流フィ
ードバックループに対する補正された目標電流となる。
４１８で演算された電流指令値Ｉiに、ステップ４２０
で演算されたフィードフォワード値Ｉf ` が加算（Ｉ
inew＝Ｉi ＋Ｉf ` ）されて、電流値Ｉinewが電流フィ
ードバックループに対する補正された目標電流となる。
【００３６】次に、ステップ４２３において、最新の重
力トルクＴi と、ゲインＫT とを用いて、次式により、
電流指令値の重力トルクによる補正値が演算される。
力トルクＴi と、ゲインＫT とを用いて、次式により、
電流指令値の重力トルクによる補正値が演算される。
【数１１】Ｉinnew ＝Ｉinew＋ＴiＫT
【００３７】次に、ステップ４２４において、最終的に
演算された電流指令値Ｉinnew に基づいて、ｉ軸のサー
ボモータに対する電流制御が実行され、このサーボモー
タは慣性モーメント及び重力トルクの補償がされた状態
で角度指令値θi に向けて位置決めされる。
演算された電流指令値Ｉinnew に基づいて、ｉ軸のサー
ボモータに対する電流制御が実行され、このサーボモー
タは慣性モーメント及び重力トルクの補償がされた状態
で角度指令値θi に向けて位置決めされる。
【００３８】以上の処理が微小時間間隔で繰り返し実行
されることにより、図３に示す回路機能と等価な処理が
実行される。尚、上記実施例では、慣性行列Ｄi の対角
成分だけで慣性モーメントの補償を行ったが、他の軸と
の相互作用を表した非対角成分を含めた補償を行うよう
にしても良い。又、慣性モーメントとゲインとの対応関
係を示す特性は、図１０に示す特性を用いたが、図１１
おいて、曲線Ａ、Ｂ、Ｃで示す特性を用いても良い。
されることにより、図３に示す回路機能と等価な処理が
実行される。尚、上記実施例では、慣性行列Ｄi の対角
成分だけで慣性モーメントの補償を行ったが、他の軸と
の相互作用を表した非対角成分を含めた補償を行うよう
にしても良い。又、慣性モーメントとゲインとの対応関
係を示す特性は、図１０に示す特性を用いたが、図１１
おいて、曲線Ａ、Ｂ、Ｃで示す特性を用いても良い。
【図１】本発明の具体的な実施例装置で使用されるロボ
ットを示した構成図。
ットを示した構成図。
【図２】ロボットの制御装置の構成を示したブロック
図。
図。
【図３】図２におけるサーボＣＰＵの処理を示したブロ
ック図。
ック図。
【図４】ロボットの位置及び姿勢制御のための主プログ
ラムを示したフローチャート。
ラムを示したフローチャート。
【図５】ロボットの位置及び姿勢制御のための動作プロ
グラムを示した説明図。
グラムを示した説明図。
【図６】補間演算の手順を示したフローチャート。
【図７】慣性モーメントの演算手順を示したフローチャ
ート。
ート。
【図８】サーボＣＰＵによる角度制御の処理手順を示し
たフローチャート。
たフローチャート。
【図９】サーボＣＰＵによる角度制御の処理手順を示し
た図８に続くフローチャート。
た図８に続くフローチャート。
【図１０】慣性モーメントとゲインとの関係を示した特
性図。
性図。
【図１１】慣性モーメントとゲインとの他の関係を示し
た特性図。
た特性図。
１０…ロボット １８…スイベルリスト １８ａ…フランジ １９…ハンド ２０…ＣＰＵ（目標角度出力手段、モーメント演算手
段） ２２ａ〜２２ｆ…サーボＣＰＵ（角加速度演算手段、第
１補正手段、第２補正手段、目標電流演算手段） ２５…ＲＡＭ Ｅ１〜Ｅ６…エンコーダ（角度検出手段） ステップ２００〜２２２…目標角度出力手段 ステップ３００〜３１２…モーメント演算手段 ステップ４０２…角加速度演算手段 ステップ４０８…第１補正手段 ステップ４１０〜４１３…目標電流演算手段 ステップ４２０…第２補正手段 ステップ４２１…ローパスフィルタ
段） ２２ａ〜２２ｆ…サーボＣＰＵ（角加速度演算手段、第
１補正手段、第２補正手段、目標電流演算手段） ２５…ＲＡＭ Ｅ１〜Ｅ６…エンコーダ（角度検出手段） ステップ２００〜２２２…目標角度出力手段 ステップ３００〜３１２…モーメント演算手段 ステップ４０２…角加速度演算手段 ステップ４０８…第１補正手段 ステップ４１０〜４１３…目標電流演算手段 ステップ４２０…第２補正手段 ステップ４２１…ローパスフィルタ
───────────────────────────────────────────────────── フロントページの続き (72)発明者 下越 昭 愛知県刈谷市朝日町１丁目１番地 豊田工 機株式会社内 (72)発明者 神谷 新吾 愛知県刈谷市朝日町１丁目１番地 豊田工 機株式会社内 (72)発明者 杉浦 功久 愛知県刈谷市朝日町１丁目１番地 豊田工 機株式会社内
Claims (2)
- 【請求項１】 位置フィードバックループ、速度フィー
ドバックループを有して、電流を制御し、指令値及び帰
還値をディジタル量で与えて、制御軸をサーボ制御する
ディジタルサーボ制御装置において、 前記制御軸が時々刻々とるべき目標角度を出力する目標
角度出力手段と、 前記目標角度の時間微分から求められる角加速度を演算
する角加速度演算手段と、 前記制御軸の回りの慣性モーメントを前記制御軸の角度
に対応して演算するモーメント演算手段と、 前記角加速度演算手段により演算された角加速度と前記
モーメント演算手段により演算され更新されているその
時の慣性モーメントに基づいて演算される補正角だけ前
記位置フィードバックループに対して加算補正する第１
補正手段と、 前記制御軸の現在角度を検出する現在角度検出手段と、 前記目標角度出力手段の出力する前記目標角度及び前記
第１補正手段の出力する補正角と、前記現在角度検出手
段の出力する現在角度との角度偏差に応じて、前記速度
フィードバックループに対する目標角速度を演算する目
標角速度演算手段と、 前記制御軸の現在角速度を検出する現在角速度検出手段
と、 前記目標角速度演算手段により演算された前記目標角速
度と前記現在角速度検出手段により検出された現在角速
度との角速度偏差に対する比例・積分演算により目標電
流を演算する目標電流演算手段と、 を有することを特徴とするディジタルサーボ制御装置。 - 【請求項２】 位置フィードバックループ、速度フィー
ドバックループを有して、電流を制御し、指令値及び帰
還値をディジタル量で与えて、制御軸をサーボ制御する
ディジタルサーボ制御装置において、 前記制御軸が時々刻々とるべき目標角度を出力する目標
角度出力手段と、 前記目標角度の時間微分から求められる角加速度を演算
する角加速度演算手段と、 前記制御軸の回りの慣性モーメントを前記制御軸の角度
に対応して演算するモーメント演算手段と、 前記制御軸の現在角度を検出する現在角度検出手段と、 前記目標角度出力手段の出力する前記目標角度と、前記
現在角度検出手段の出力する現在角度との角度偏差に応
じて、前記速度フィードバックループに対する目標角速
度を演算する目標角速度演算手段と、 前記制御軸の現在角速度を検出する現在角速度検出手段
と、 前記目標角速度演算手段により演算された前記目標角速
度と前記現在角速度検出手段により検出された現在角速
度との角速度偏差に対する比例・積分演算により目標電
流を演算する目標電流演算手段と、 前記角加速度演算手段により演算された角加速度と前記
モーメント演算手段により演算され更新されているその
時の慣性モーメントに基づいて前記電流フィードバック
ループへの目標電流を加算補正する第２補正手段とを有
することを特徴とするディジタルサーボ制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP9858493A JPH06289915A (ja) | 1993-03-31 | 1993-03-31 | ディジタルサーボ制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP9858493A JPH06289915A (ja) | 1993-03-31 | 1993-03-31 | ディジタルサーボ制御装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH06289915A true JPH06289915A (ja) | 1994-10-18 |
Family
ID=14223707
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP9858493A Pending JPH06289915A (ja) | 1993-03-31 | 1993-03-31 | ディジタルサーボ制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH06289915A (ja) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4820014A (en) * | 1987-02-25 | 1989-04-11 | Sumitomo Electric Industries, Ltd. | Optical cable |
| JP2009101444A (ja) * | 2007-10-22 | 2009-05-14 | Okuma Corp | 回転構造物の位置制御方法 |
| JP2020041970A (ja) * | 2018-09-13 | 2020-03-19 | 株式会社ミツトヨ | レーザ追尾装置およびレーザ追尾装置の制御方法 |
| CN113267995A (zh) * | 2021-04-27 | 2021-08-17 | 长春同泽科技有限公司 | 一种驱动控制装置、控制方法及矿井运输车 |
-
1993
- 1993-03-31 JP JP9858493A patent/JPH06289915A/ja active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4820014A (en) * | 1987-02-25 | 1989-04-11 | Sumitomo Electric Industries, Ltd. | Optical cable |
| JP2009101444A (ja) * | 2007-10-22 | 2009-05-14 | Okuma Corp | 回転構造物の位置制御方法 |
| JP2020041970A (ja) * | 2018-09-13 | 2020-03-19 | 株式会社ミツトヨ | レーザ追尾装置およびレーザ追尾装置の制御方法 |
| CN113267995A (zh) * | 2021-04-27 | 2021-08-17 | 长春同泽科技有限公司 | 一种驱动控制装置、控制方法及矿井运输车 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111660306B (zh) | 一种基于操作者舒适度的机器人可变导纳控制方法及系统 | |
| JP4896276B2 (ja) | ロボット、ロボットの制御装置、制御方法、及び制御プログラム | |
| CN100396452C (zh) | 加减速控制方法及装置 | |
| JPH079606B2 (ja) | ロボット制御装置 | |
| JPH05104469A (ja) | 宇宙ロボツトの制御装置 | |
| WO2014061681A1 (ja) | 多関節ロボットのウィービング制御装置 | |
| JP2014136260A (ja) | 制御装置 | |
| JP3981773B2 (ja) | ロボット制御装置 | |
| JP2017209762A (ja) | ロボット装置、ロボット制御方法、プログラム、記録媒体及び物品の製造方法 | |
| WO2013122018A1 (en) | Control apparatus of robot, robot, and program thereof | |
| JP3316967B2 (ja) | ロボットの制御装置 | |
| JP4269150B2 (ja) | ロボット制御装置 | |
| WO2005063455A1 (ja) | ロボットの制御装置 | |
| JP4498061B2 (ja) | 溶接ロボット制御装置 | |
| JPH06289915A (ja) | ディジタルサーボ制御装置 | |
| JP2020015124A (ja) | ロボット制御方法、物品の製造方法、ロボット制御装置、ロボット、プログラム及び記録媒体 | |
| JPH05111889A (ja) | 制御方式可変型ロボツト制御方式 | |
| JPH06190750A (ja) | ロボットの制御装置 | |
| JPH07261844A (ja) | モータ制御装置 | |
| JP3599849B2 (ja) | サーボ制御における移動指令の分配方法 | |
| JPH0760667A (ja) | ロボットのウィービング制御装置 | |
| JPH06222817A (ja) | ロボットのウィービング制御装置 | |
| JPH08190433A (ja) | 負荷重量推定方法 | |
| JP2003084804A (ja) | 最適指令作成装置 | |
| JP3355420B2 (ja) | 産業用ロボットの制御方法 |