JPH06190750A - ロボットの制御装置 - Google Patents

Abstract

(57)【要約】 【目的】動作時において教示データの補間周期を長くす
ることがなくハンドから先の重量物体が変化しても位置
及び姿勢制御の性能を低下させないこと。 【構成】ロボットが物体を握持したり開放する時に、LO
AD命令語により重量物の重心座標とその質量とを設定す
る。重心座標と質量とを用いて、各軸の回りの慣性モー
メント及び重力トルクが教示データの補間演算の空き時
間に演算され、演算が完了したものから順に、それらの
最新値が更新される。この慣性モーメント及び重力トル
クを用いて、速度ループのゲインや各種のフィードフォ
ワード値が補正される。

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 ^'とトルク定数Ｋ_tnと定数
Ｋ_a を用いて、次式により、電流指令値がフィードフォ
ワード補正される。

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

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

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

【００３７】以上の処理が微小時間間隔で繰り返し実行
されることにより、図３に示す回路機能と等価な処理が
実行される。尚、上記実施例では、慣性行列Ｄ_i の対角
成分だけで慣性モーメントの補償を行ったが、他の軸と
の相互作用を表した非対角成分を含めた補償を行うよう
にしても良い。

【図面の簡単な説明】

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

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

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

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

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

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

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

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

【符号の説明】

１０…ロボット １８…スイベルリスト １８ａ…フランジ １９…ハンド ２０…ＣＰＵ（データ設定手段、補間演算手段、モーメ
ント／トルク演算手段） ２２ａ〜２２ｆ…サーボＣＰＵ（角度制御手段、第１補
正手段、第２補正手段） ２５…ＲＡＭ（データ設定手段、教示データ記憶手段） Ｅ１〜Ｅ６…エンコーダ（角度検出手段） ステップ１０６…データ設定手段 ステップ２００〜２２２…補間演算手段 ステップ３００〜３１２…モーメント／トルク演算手段

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁵ 識別記号 庁内整理番号 ＦＩ 技術表示箇所 Ｇ０５Ｄ 3/12 ３０５ Ｌ 9179−3Ｈ

Claims (1)

【特許請求の範囲】
1. 【請求項１】 位置フィードバックループ、速度フィー
   ドバックループ及び電流フィードバックループを有し、
   ロボットの工具の位置及び姿勢を指令された位置及び姿
   勢に追随させる制御装置において、 ロボットの動作時における手首部先端より先の重量物体
   の重心座標及び質量を設定するデータ設定手段と、 複数の教示点における位置及び姿勢を規定した教示デー
   タを記憶する教示データ記憶手段と、 ２つの教示点間を結ぶ経路上の補間点における位置及び
   姿勢を規定した補間データを、２つの教示データから補
   間演算により求める補間演算手段と、 前記補間演算手段により演算された補間データに従っ
   て、ロボットの各軸の回転角の制御を行う角度制御手段
   と、 ロボットの各軸の回転角を検出する角度検出手段と、 前記補間演算手段による補間演算終了後から次の補間演
   算開始までの間に、検出された各軸の回転角とロボット
   の各リンクの長さ及び質量と前記データ設定手段により
   その時に設定されている重心座標及び質量とから、順
   次、各軸の回りの慣性モーメント及び重力トルクを繰り
   返し演算することで、各軸の回りの慣性モーメント及び
   重力トルクを順次更新するモーメント／トルク演算手段
   と、 前記モーメント／トルク演算手段により演算され更新さ
   れているその時の慣性モーメントに基づいて前記速度フ
   ィードバックループのゲインを変化させる第１補正手段
   と、 前記モーメント／トルク演算手段により演算され更新さ
   れているその時の重力トルクに基づいて前記電流フィー
   ドバックループへの指令値を加算補正する第２補正手段
   とから成るロボットの制御装置。