JPH0675009U - サーボ制御装置 - Google Patents

Abstract

(57)【要約】 【目的】制御軸の回りの慣性モーメントが制御軸の角度
と共に大きく変化する場合にも、安定したサーボ制御を
可能とすること。 【構成】制御軸の回りの慣性モーメントを制御軸の角度
に対応して演算するモーメント演算手段と、慣性モーメ
ントを、その最大変動範囲に対して所定範囲に圧縮した
範囲のゲインに対応させる特性を記憶した特性記憶手段
と、特性記憶手段に記憶されている特性を用いて、モー
メント演算手段により演算され更新されているその時の
慣性モーメントに対応したゲインを求めるゲイン演算手
段４１７と、ゲイン演算手段により演算されたゲインに
基づいて、速度フィードバックループのゲインを変化さ
せる補正手段４１８とを設けた。

Description

【考案の詳細な説明】

【０００１】

【産業上の利用分野】

本考案は、制御軸の回りの慣性モーメントが制御軸の回転角と共に変化するサ ーボ制御装置に関する。

【０００２】

【従来の技術】

制御軸の慣性モーメントが制御軸の回転角と共に変化するサーボ制御装置とし て、多関節ロボットの各軸を制御するサーボ制御装置がある。多関節ロボットで は、ロボットの姿勢変化に伴って、各軸の回りの慣性モーメントが変化する。こ のため、位置及び速度フィードバックループのゲインを一定とすると、ロボット の全作動領域において位置決め誤差や位置決め速度を一定にすることは困難であ る。例えば、位置や速度のフィードバックループのゲインを慣性モーメントが最 大となる姿勢の時に所定の位置決め誤差及び位置決め速度が得られるように設定 した場合には、慣性モーメントが小さくなる姿勢では、フィードバックループの ゲインが高くなり過ぎ、振動が発生し、位置決め時間が長くなるという問題があ る。逆に、フィードバックループのゲインを慣性モーメントが最小となる姿勢の 時に所定の位置決め誤差及び位置決め速度が得られるように設定した場合には、 慣性モーメントが大きくなる姿勢では、フィードバックループのゲインが不足し 、位置決め誤差が増大したり、追随遅れが増大し、位置決め時間が長くなるとい う問題がある。

【０００３】

【考案が解決しようとする課題】

この課題を解決するために、教示データ間を補間して各軸の指令角度を演算す ると共にその指令角度における各軸の回りの慣性モーメントを演算して、それら の値に基づいて位置及び速度フィードバックループのゲインを変化させることが 考えられる。しかしながら、この慣性モーメントの変化が、例えば、１０倍もあ るような場合には、速度フィードバックループのゲインが１０倍も変化すること になり、系の状態量の発振が生じる。具体的には、電流波形のハネが大きくなり 、モータが過負荷となるという問題が発生する。

【０００４】 本考案は上記の課題を解決するために成されたものであり、その目的は、制御 軸の回りの慣性モーメントが制御軸の角度と共に大きく変化する場合にも、安定 したサーボ制御を可能とすることである。

【０００５】

【課題を解決するための手段】

上記課題を解決するための考案の構成は、位置フィードバックループ、速度フ ィードバックループを有したサーボ制御装置において、制御軸の回りの慣性モー メントを制御軸の角度に対応して演算するモーメント演算手段と、慣性モーメン トをゲインに対応させる特性を記憶した特性記憶手段と、特性記憶手段に記憶さ れている特性を用いて、モーメント演算手段により演算され更新されているその 時の慣性モーメントに対応したゲインを求めるゲイン演算手段と、ゲイン演算手 段により演算されたゲインに基づいて、速度フィードバックループのゲインを変 化させる補正手段とを設けたことである。

【０００６】

【作用】

各制御軸の回りの慣性モーメントが制御軸の角度変化に伴って演算される。こ の演算された慣性モーメントから、特性記憶手段に記憶されている特性を用いて 、対応するゲインが求められる。そして、この値に基づいて、速度フィードバッ クループのゲインが変化させられる。

【０００７】

【考案の効果】

このように、速度フィードバックループのゲインは、制御軸の回りのその時の 慣性モーメントに対して経験的に適切と考えられる値として与えられる。この結 果、慣性モーメントの変動範囲に対して、ゲインの変動範囲が規制されることに なり、系の状態量の発振、電流波形のハネが抑制され、モータの過負荷が防止さ れる。従って、サーボ制御の安定性が向上する。

【０００８】

【実施例】

以下本考案を具体的な実施例に基づいて説明する。 本実施例は、本考案のサーボ制御装置を多関節ロボットの各軸の制御装置に応 用した例である。 図１は６軸多関節ロボットの機構を示した機構図である。１０がロボット本体 であり、フロアに本体１０を固定するベース１３が配設され、ベース１３上には コラム１２が固設されており、コラム１２はボディ１４を回転自在に配設してい る。ボディ１４はアッパーアーム１５を回動自在に軸支し、アッパーアーム１５ は、フォアアーム１６を回動自在に軸支している。ボディ１４、アッパーアーム １５、フォアアーム１６は、それぞれ、サーボモータSm１，Sm２，Sm３（図２参 照）によって、軸ａ，ｂ，ｃの回りに回転駆動される。この回転角はエンコーダ Ｅ１，Ｅ２，Ｅ３によって検出される。フォアアーム１６の先端部にはツイスト リスト１７がｄ軸の周りに回転可能に軸支され、ツイストリスト１７にはベンド リスト９がｅ軸の周りに回動自在に軸支されている。ベンドリスト９には先端に フランジ１８ａを有するスイベルリスト１８がｆ軸の回りに回転可能に軸支され ている。また、フランジ１８ａにはハンド１９が取り付けられている。ｄ軸、ｅ 軸、ｆ軸はサーボモータSm４、Sm５、Sm６によって駆動され、その回転角はエン コーダＥ４，Ｅ５，Ｅ６によって検出される。ハンド１９の開閉動作は工具駆動 回路２３により制御される。

【０００９】 図２は、本考案のロボットの制御装置の電気的構成を示したブロックダイヤグ ラムである。ＣＰＵ２０には、メモリ２５、サーボモータを駆動するためのサー ボＣＰＵ２２ａ〜２２ｆ、動作開始指令、ジョグ運転の指令、教示点の指示等を 行う操作盤２６が接続されている。ロボットに取付けられた各軸ａ〜ｆ駆動用の サーボモータSm１〜Sm６は、それぞれサーボＣＰＵ２２ａ〜２２ｆによって駆動 される。

【００１０】 サーボＣＰＵ２２ａ〜２２ｆのそれぞれは、ＣＰＵ２０から出力される各軸の 角度指令値θ₁ 〜θ₆ 、慣性モーメントＤ_i 、重力トルクＴ_i に基づいて、サー ボモータSm１〜Sm６の出力トルクを制御する。各駆動軸に連結されたエンコーダ Ｅ１〜Ｅ６の出力する検出角度α₁ 〜α₆ はＣＰＵ２０及びサーボＣＰＵ２２ａ 〜２２ｆに入力しており、ＣＰＵ２０による各軸の慣性モーメント及び重力トル クの演算及びサーボＣＰＵ２２ａ〜２２ｆによる位置フィードバック制御、速度 フィードバック制御、電流フィードバック制御に用いられる。

【００１１】 メモリ２５にはロボットを教示点データに従って動作させるためのプログラム が記憶されたＰＡ領域とハンド１９の位置と姿勢を表す教示点データを記憶する ＰＤＡ領域とフランジ１８ａから先の重量物、即ち、ハンド１９、又は、ハンド １９とそれにより握持された物体を総合した物体の重心座標（ａ，ｂ，ｃ）とそ の質量ｍ_L を記憶するＳＤＡ領域とサーボパラメータを記憶するＩＴＡ領域と補 間演算により求められた補間点における各軸の角度指令値θ₁ 〜θ₆ を記憶する ＩＮＡ領域とエンコーダＥ１〜Ｅ６から出力された検出角度α₁ 〜α₆ を記憶す るＡＮＧ領域とが形成されている。

【００１２】 各軸のサーボＣＰＵ２２ａ〜２２ｆは、図３に示す回路の機能をシーケンシャ ルなディジタル制御により達成するものである。即ち、各回転軸の位置のフィー ドバックループと速度のフィードバックループとでアンプの指令電流を制御する ための制御ループが構成されている。そして、この制御ループは角速度のフィー ドフォワードループ、角加速度のフィードフォワードループおよび動トルクのフ ィードフォワードループを有している。

【００１３】 次に、本装置の作動について説明する。 図５はＲＡＭ２５のＰＡ領域に記憶されている動作プログラムである。行番号 １０、５０、９０がデータ設定手段を構成する重量物の重心座標（ａ，ｂ，ｃ） とその質量ｍ_L とを設定するための命令語である。この命令語の実行により重心 座標（ａ，ｂ，ｃ）と質量ｍ_L のデータがＲＡＭ２５のＳＤＡ領域に記憶される 。命令語の領域Ａが重心座標ｒを表し、領域Ｂが質量ｍ_L を表す。重心座標（ａ ，ｂ，ｃ）はスイベルリスト１８のフランジ１８ａの中心に固定されたリンク座 標系から見た座標、即ち、ノーマル、オリエント、アプローチの各座標（mm単位 ) で与えられる。又、質量はkg単位で表記される。行番号１０、９０は、フラン ジ１８ａから先の重量物がハンド１９だけの場合のデータ設定に関する命令語で あり、行番号５０はハンド１９により物体が握持された場合のデータ設定に関す る命令語である。又、行番号２０及び８０のＨＡＮＤ ＯＦＦ命令語は、ハンド １９を開く命令語であり、行番号４０のＨＡＮＤ ＯＮ命令語はハンド１９を閉 じる命令語である。さらに、行番号３０、６０、７０、１００、１１０のＭＯＶ Ｅ命令語は指定された教示点Ｐｎに移動させるための命令語である。

【００１４】 図５の動作プログラムにより、ロボットは、ハンド１９を開いて、Ｐ１点に移 動し、その後、ハンド１９を閉じて物体を握持し、Ｐ２点を経由してＰ３点に移 動して、ハンド１９を開いて、物体を放置し、Ｐ４点を経由してＰ５点に移動す ることが可能である。

【００１５】 図４は、ＣＰＵ２０による動作プログラムを解読するための主プログラムのフ ローチャートである。ステップ１００において、ＭＯＶＥ命令語が解読されると 、ステップ１０２において、現在位置から指定された教示点までハンド１９を移 動させるための補間演算が実行される。そして、補間演算により求められた各軸 の角度指令値θ₁ 〜θ₆ はサーボＣＰＵ２２ａ〜２２ｆに出力される。又、ステ ップ１０４において、ＬＯＡＤ命令語が解読されると、ステップ１０６において 、その命令語に表記されているハンドから先の重量物の重心座標と質量のデータ がＲＡＭ２５のＳＤＡ領域に記憶される。又、ステップ１０８でＨＡＮＤ ＯＦ Ｆ命令語が解読されると、ステップ１１０において、工具駆動回路２３にハンド １９を開く指令を与える。又、ステップ１１２でＨＡＮＤ ＯＮ命令語が解読さ れると、ステップ１１４において、工具駆動回路２３にハンド１９を閉じる指令 を与える。

【００１６】 ステップ１０２における教示点間の補間演算は、図６に示すフローチャートに 従って実行される。補間演算は、良く知られたように、回転主軸法等を用いて行 うことができる（例えば、特開昭６２−１５４００６号）。ステップ２００にお いて、ワールド座標系で表記された教示データとして与えられた開始点の位置及 び姿勢と次の位置決め目標点の位置及び姿勢から回転主軸の方向ベクトルが演算 され、ステップ２０２において、その回転主軸回りの回転角Θが演算される。次 に、ステップ２０４において、開始点の位置及び姿勢を基準とした補間角ΔΘが 演算され、次のステップ２０６において、その補間角ΔΘを用いて開始点の位置 及び姿勢を補間点における位置及び姿勢に変換するための姿勢変換行列Ｒが演算 される。そして、ステップ２０８において、開始点の位置及び姿勢を表す同次座 標行列に姿勢変換行列を作用させて、補間点における位置及び姿勢を表す同次座 標行列が演算される。次に、ステップ２１０において、その補間点におけるワー ルド座標系で表記された位置及び姿勢の同次座標行列からジョイント座標系での 値、即ち、各軸の回転角が演算され、この値はＲＡＭ２５のＩＮＡ領域に記憶さ れる。この状態で、本補間プログラムは終了し、ステップ２１２で、後述する優 先度が低いレベルに設定された慣性モーメント及び重力トルク演算プログラムの 再開処理（中断した命令語の記憶アドレスへの分岐命令及び状態レジスタ等の復 元処理）が行われた後、そのプログラムが継続起動される。

【００１７】 一方、８msec毎のタイマ割り込みが発生すると（ステップ２１４）、ステップ ２１６において、優先度の低いレベルに設定された慣性モーメント及び重力トル ク演算プログラムへの割込処理（中断した命令語が記憶されているアドレスの記 憶、状態レジスタ等のスタック処理等）が行われる。そして、ステップ２１８、 ２２０において、ＲＡＭ２５のＩＮＡ領域に記憶された補間点の角度指令値θ₁ 〜θ₆ 、慣性モーメント及び重力トルク演算プログラムにより演算されＲＡＭ２ ５のＩＴＡ領域に記憶されているサーボパラメータＤ_i,Ｔ_i がサーボＣＰＵ２２ ａ〜２２ｆに出力される。この結果、各軸はその時の位置及び姿勢に対応した慣 性モーメント及び重力トルクを補償したフィードフォワード制御によりそれらの 角度指令値θ₁ 〜θ₆ に位置決め制御される。そして、ステップ２２２において 、補間点が教示データとして与えられた目標位置に達したか否かが判定され、補 間が終了していない場合に、次の補間点の位置及び姿勢の演算のために、ステッ プ２０６以下が実行される。一方、補間が終了した場合には、次の命令語を解読 するために図４のメインプログラムに戻る。

【００１８】 次に、優先度の低いレベルに設定されている慣性モーメント及び重力トルクの 演算手順について、図７に示すフローチャートを参照して説明する。 ステップ３００において、ロボットの軸を特定する軸変数ｉが１に初期設定さ れ、ステップ３０２においてエンコーダＥ１〜Ｅ６から各軸の現在の回転角α₁ 〜α₆ が検出され、その値はＲＡＭ２５のＡＮＧ領域に記憶される。次に、ステ ップ３０４において、ｉ軸の回りの慣性行列Ｄ_i ( α₁,…, α₆)( 慣性モーメン トの行列表現）が演算され、ステップ３０６において、ｉ軸の回りの重力トルク Ｔ_i ( α₁,…, α₆)が演算される。尚、ｉ軸の回りのトルクτ_i は、次式で良く 知られたように、ロボットアームに関するラグランジュ関数の微分から求めるこ とができる。

【００１９】

【数１】 τ_i ＝Σ_j=i ⁿΣ_k=i ^jTr(Ｕ_jkＪ_jＵ_ji ^T)D²θ_k - Σｍ_jｇＵ_ji ^jｒ_j 但し、コリオリ力と遠心力の成分は小さいとして上式では除外した。 Ｊ_j はｊ軸を構成する部材だけによるリンク慣性行列、Trは行列式の対角成分 の和、ｍ_j はｊ軸リンクの質量、ｇは重力加速度、 ^jｒ_j はｊ軸リンク座標系で 表されたｊ軸リンクの重心のベクトルであり、D²θ_k は角度の時間に関する２次 微分、即ち、角加速度である。又、上式の第１項の展開式において、D²θ_i の係 数が慣性行列Ｄ_i となり、第２項がｉ軸の回りの重力トルクＴ_i となる。 又、Ｕ_jkは、

【００２０】

【数２】 ０ （ｊ＜ｉの時）

【数３】 δ（⁰A₁ ¹A₂…^j-1A_j )/δθ_i ＝Π_s=1 ^{j s-1}A _s・δ^i-1A_i / δθ_i （ｊ≧ｉの 時） 但し、ｓ≠ｉである。 又、^j-1A_j はｊ軸リンク座標をｊ−１軸リンク座標へ変換する同次座標変換行 列であり、⁰A₁ は１軸リンク座標をワールド座標に変換する同次座標変換行列で ある。

【００２１】 Ｊ_j は慣性テンソルＩ_ijを用いて次の様に表現することができる。

【数４】

【００２２】 但し、ｒ_ix、ｒ_iy、ｒ_izはｊ軸リンク座標から見たｊ軸リンクの重心の座標で あり、ｍ_iはその質量である。 又、

【数５】 Ｉ_ij＝∫〔δ_ij〔Σｘ_k ² 〕−ｘ_iｙ_i 〕dｍ である。

【００２３】 尚、ＬＯＡＤ命令語によって設定される重心座標（ａ，ｂ，ｃ）及び質量ｍ_L により、Ｊ₆ が変化する。Ｊ₆ は、数４において、（ｒ_ix，ｒ_iy，ｒ_iz）＝（ａ ，ｂ，ｃ）、ｍ_i ＝ｍ_L とおくことにより求めることができる。

【００２４】 数１から、現在の検出角度（α₁ ，…，α₆ ）における各軸の回りの慣性行列 Ｄ_i と重力トルクＴ_i とが演算される。本実施例では、演算時間を短縮するため に、他軸との相互作用は小さいとして、(i,i) 対角成分のみを考慮している。請 求の範囲の慣性モーメントの用語は、対角成分だけでなく慣性行列を含む意味で 用いているが、以下の記載では、便宜上、慣性行列Ｄ_i の(i,i) 対角成分を、慣 性モーメントＤ_i と記す。次に、ステップ３０８において、慣性モーメントＤ_i と重力トルクＴ_i がサーボパラメータとしてＲＡＭ２５のＩＴＡ領域に更新記憶 される。この値が図６のステップ２２０においてサーボＣＰＵ２２ａ〜２２ｆに 出力される。次に、ステップ３１０において、軸変数ｉが最大値６に達したか否 かが判定され、その値に達していない場合には、ステップ３１２において、軸変 数ｉが１だけ加算されて、ステップ３０２に戻り、次の軸に関する慣性モーメン ト及び重力トルクの演算が繰り返し実行される。又、軸変数ｉが最大値６になれ ば、ステップ３００に戻り、軸変数ｉを１に初期設定して、１軸から演算が繰り 返し実行される。

【００２５】 この図７に示すプログラムは、図４、図６に示すプログラムよりも優先度が１ ランク低く設定されているので、図４、図６に示すプログラムが実行されていな い時に実行される。このようにして、補間演算手段（図６に示す手順を実行する 手段）の空き時間において、図７に示す慣性モーメント及び重力トルクの演算が 実行される。従って、図７のプログラムは、連続されて実行されるのではなく、 ８msecの補間演算毎に中断され、中断された次の命令語から再開される。このた め、補間周期とは別の遅い周期で各軸毎に慣性モーメント及び重力トルクが順次 、最新の値に更新されていくことになる。

【００２６】 次に、ｉ軸のサーボＣＰＵの動作手順を図８、図９のフローチャートに従って 説明する。 ステップ４００において、エンコーダＥｉから現在の検出角度α_i が読み取ら れ、ステップ４０２において検出角度α_i の時間変化率から角速度β_i 、角速度 β_i の時間変化率から角加速度γ_i が演算される。次、ステップ４０４において 、ＲＡＭ２５の領域ＩＴＡに記憶されている最新の重力トクルＴ_i と慣性モーメ ントＤ_i がＣＰＵ２０を介して受信され、ステップ４０６において、ＲＡＭ２５ の領域ＩＮＡに記憶されている角度指令値θ_i がＣＰＵ２０を介して受信される 。次に、ステップ４０８において、角加速度γ_i と、慣性モーメントＤ_i とバネ 定数Ｋ_c と定数Ｋ₂ に基づいて、次式により、角度指令値θ_i のフィードフォワ ード補正が行われる。

【００２７】

【数６】 θ_inew＝θ_i＋γ_iＤ_iＫ₂／Ｋ_c

【００２８】 次に、ステップ４１０において、補正された角度指令値θ_inewの検出角度α_i に対する角度偏差Δθ_i （＝θ_inew−α_i ）が演算される。次に、ステップ４１ ２において、角度偏差Δθ_i 、位置ループゲインＫ_p を用いて、次式により、角 速度指令値ω_i が演算される。

【００２９】

【数７】 ω_i ＝Ｋ_pΔθ_i

【００３０】 次に、ステップ４１４において、検出された角速度β_i 、ゲインＫ_f を用いて 、角速度のフィードフォワード補正が次式により行われる。

【数８】 ω_inew＝ω_i＋β_iＫ_f 次に、ステップ４１６において、補正された角速度指令値ω_inewの検出された 角速度β_i に対する角速度偏差Δω_i （＝ω_inew−β_i ）が演算される。

【００３１】 次に、ステップ４１７において、慣性モーメントＤ_i にｉ軸のサーボモータの ロータの慣性モーメントを加算した全慣性モーメントＤ_i ^'を用いて、ＲＯＭ２４ の特性記憶領域に記憶されている図１０に示す特性のマップを検索して、対応す るゲインＫ（Ｄ_i ^'）が求められる。この特性は、図１０に示すように、全慣性モ ーメントＤ_i ^'の１０倍の最大変動範囲に対してゲインは４倍だけ変動するように 圧縮されている。

【００３２】 次に、ステップ４１８において、電流指令値Ｉ_i が、演算されたゲインＫ（Ｄ _i ^' ）を用いて、次式により、演算される。

【数９】 Ｉ_i ＝Ｋ（Ｄ_i ^'）〔Δω_i ＋（ΣΔω_i ・Δｔ）／Ｔ_i 〕 但し、Ｔ_i は積分時定数、Δｔは、速度制御周期、即ち、電流指令値Ｉ_i を求 める制御周期である。 このように電流指令値を求めるための速度フィードバックループのゲインが全 慣性モーメントＤ_i ^'の変動範囲に対して圧縮された変動範囲を有するゲインＫ（ Ｄ_i ^'）により補償されることになる。即ち、慣性モーメントが大きな姿勢では、 追随性を向上させるために速度フィードバックループのゲインが大きくなるよう に制御される。この時、慣性モーメントの変動範囲に対して、ゲインの変動範囲 は圧縮されているので、系の状態量の発振、電流波形のハネが抑制され、モータ の過負荷が防止される。従って、サーボ制御の安定性が向上する。

【００３３】 次に、ステップ４２０において、角加速度γ_i と、全慣性モーメントＤ_i ^'とト ルク定数Ｋ_tnと定数Ｋ_a を用いて、次式により、電流指令値がフィードフォワー ド補正される。

【数１０】 Ｉ_inew＝Ｉ_i＋γ_iＫ_aＤ_i ^'／Ｋ_tn 次に、ステップ４２２において、最新の重力トルクＴ_i と、ゲインＫ_T とを用 いて、次式により、電流指令値の重力トルクによる補正値が演算される。

【数１１】 Ｉ_innew ＝Ｉ_inew＋Ｔ_iＫ_T

【００３４】 次に、ステップ４２４において、最終的に演算された電流指令値Ｉ_innew に基 づいて、ｉ軸のサーボモータに対する電流制御が実行され、このサーボモータは 慣性モーメント及び重力トルクの補償がされた状態で角度指令値θ_i に向けて位 置決めされる。

【００３５】 以上の処理が微小時間間隔で繰り返し実行されることにより、図３に示す回路 機能と等価な処理が実行される。 尚、上記実施例では、慣性行列Ｄ_i の対角成分だけで慣性モーメントの補償を 行ったが、他の軸との相互作用を表した非対角成分を含めた補償を行うようにし ても良い。 又、慣性モーメントとゲインとの対応関係を示す特性は、図１０に示す特性を 用いたが、図１１おいて、曲線Ａ、Ｂ、Ｃで示す特性を用いても良い。 さらに、図１１のような複数の特性Ａ，Ｂ，Ｃを予め選択できるようにしてお き、所望に応じて特性を変化できるようにしても良い。この場合は、図９に示さ れるフローチャートを図１２に示すフローチャートのように改良する。即ち、図 ９に示すステップ４１７０を予め選択しておいた特性マップを判別するステップ とし、特性マップをＡ，Ｂ，Ｃの内から判別した後、ステップ４１７ａ，４１７ ｂ，４１７ｃにおいて、全慣性モーメントＤ_i'に対応するゲインＫ(Ｄ_i')を選択 された特性マップより求める。 このように特性マップを選択できるようにすることによって、より制御システ ムに適合したゲインＫ(Ｄ_i')を選び出すことができる。

【図面の簡単な説明】

【図１】本考案の具体的な実施例装置で使用されるロボ
ットを示した構成図。

【図２】ロボットの制御装置の構成を示したブロック
図。

【図３】図２におけるサーボＣＰＵの処理を示したブロ
ック図。

【図４】ロボットの位置及び姿勢制御のための主プログ
ラムを示したフローチャート。

【図５】ロボットの位置及び姿勢制御のための動作プロ
グラムを示した説明図。

【図６】補間演算の手順を示したフローチャート。

【図７】慣性モーメントの演算手順を示したフローチャ
ート。

【図８】サーボＣＰＵによる角度制御の処理手順を示し
たフローチャート。

【図９】サーボＣＰＵによる角度制御の処理手順を示し
た図８に続くフローチャート。

【図１０】慣性モーメントとゲインとの関係を示した特
性図。

【図１１】慣性モーメントとゲインとの他の関係を示し
た特性図。

【図１２】他の変形例にかかるサーボＣＰＵによる角度
制御の処理手順を示した図８に続くフローチャート。

【符号の説明】

１０…ロボット １８…スイベルリスト １８ａ…フランジ １９…ハンド ２０…ＣＰＵ（モーメント演算手段） ２２ａ〜２２ｆ…サーボＣＰＵ（ゲイン演算手段、補正
手段） ２４ａ〜２４ｆ…ＲＡＭ（特性記憶手段） ２５…ＲＡＭ Ｅ１〜Ｅ６…エンコーダ ステップ３００〜３１２…モーメント演算手段 ステップ４１７…ゲイン演算手段 ステップ４１８…補正手段

Claims (1)

【実用新案登録請求の範囲】
1. 【請求項１】 位置フィードバックループ、速度フィー
   ドバックループを有したサーボ制御装置において、 前記制御軸の回りの慣性モーメントを前記制御軸の角度
   に対応して演算するモーメント演算手段と、 前記慣性モーメントをゲインに対応させる特性を記憶し
   た特性記憶手段と、 前記特性記憶手段に記憶されている特性を用いて、前記
   モーメント演算手段により演算され更新されているその
   時の慣性モーメントに対応したゲインを求めるゲイン演
   算手段と、 前記ゲイン演算手段により演算されたゲインに基づい
   て、前記速度フィードバックループのゲインを変化させ
   る補正手段とを有するサーボ制御装置。