JPS63174894A - デジタルロボット制御装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 １弧へ１１ この発明は、ロボットに関し、更に詳しくはロボット制
御装置、並びにそのような制御装置に対するエネルギ及
び速度のモニタ及び保護システムに関するものである。

産業ロボットのアームには、リンクによって連結された
複数のジヨイントが設けられている。各ジヨイントはロ
ボット制御装置の方向に基づくモータによって駆動され
る。ロボットツールのチップは、命令された経路を越え
て及び又はジヨイントの整合された動作を介して命令さ
れた位置に動かされる。

通常、ロボット制御装置は、各ジヨイントモータに対す
る速度制御装置を含む制御ループ構成を持っている。速
度制御ループは、典型的にはタコメータ（こよって発生
されるか又は位置エンコーダの出力信号から得られるジ
ヨイントモータの速度フィードバックを用いる。

従って、ロボットアームの運転は、速度フィードバック
の有効性に大きく依存する。もし、タコメータ又はエン
コーダに欠陥が生じるが、又は、そうではなく速度フィ
ードバックが誤差を含むが又は得られなくなると、ロボ
ットアーム制御装置は本質的に喪失され、合成されたア
ームの運動は人員損傷や財産損害を与える結果になり得
る。

従って、安全保護及び財産保護の理由のため、ロボット
制御装置内の速度フィードバックシステムをモニタし、
故障を検出したときに保護動作を提供するためのバック
アップ機構を設けることが望ましい。

ロボットの運転において、いかなる状態の違いによって
も、１つ又はそれ以上のロボットジヨイントモータを過
負荷になし得る。過負荷が生じなときは、人員の安全及
び財産の保護の理由のため、ロボットを停止することが
望ましい、過負荷は、例えば、ロボットプログラムのエ
ラー、ロボットアームと障害物との衝突、ジヨイントモ
ータの欠陥、その他の結果として起こり得る。

従来、モータモニタシステムは、モータ過負荷が生じて
保護動作が開始される必要があるときに、温度センナか
らの測定を作動する。過負荷、即ちジヨイントモータ又
は駆動電子回路の超過加圧は、モータの過熱が存在する
ときに存在すると推測される。

従来のモータモニタシステムがモータ過負荷保護の第ル
ベルを提供する間、それらはいくつかの欠点と共に作動
を行う。例えば、過負荷状態が起きた時間と、温度セン
サに基づくシステムが応答して過負荷を警報する時間と
の間にいくらかの時間遅れがある。更に、システムは、
過負荷閾値の検出において制限された精度で機能する。

結局、典型的に、どのジョイントモータニ対しても、セ
ンサシステムの故障におけるバックアップモニタ手段は
ない。

従って、ロボット内の全てのジヨイントモータに、保護
に基づく第１温度センサと同等のバックアップモニタ保
護を設け、そのようなバックアップ保護が迅速且つ正確
なエネルギモニタを提供することが望ましい。

この莞明は、迅速且つ正確なバックアップエネルギモニ
タ及びバックアップ速度モニタを提供する保護システム
を有するロボット制御装置を意向する。

ｉ朋！１１」 デジタル制御装置は、複数のアームジョイントを有する
ロボットに対して設けられる。電気モータは各ロボット
アームジヨイントを駆動し、電力増幅器は各モータに駆
動電流を供給するように作動する。

各ジヨイントモータは、所定のサンプリングレートで作
動するデジタルフィードバック制御ループ手段と、デジ
タル位置命令に従ってデジタルトルク制御ループを駆動
し、関連する電力増幅器を制御するためのデジタルモー
タ電圧命令を発生する速度制御ループとを有する。

各ジヨイントモータの動作を検知するため、又、位置制
御ループ及び速度制御ループにおける制御計算用の対応
するデジタル位置及び速度フィードバック信号を発生す
るための手段が設けられている。

フィードバックシステムにおいて、トルク制御ループに
おける制御計算用の各ジヨイントモータ駆動電流を表わ
すデジタル信号を発生するための手段が設けられている
。又、モータ温度を検知して所定のモータ温度が超過さ
れたときに超過モータ電流を示す超過温度信号を発生ず
るための手段も設けられている。

バックアップエネルギモニタ手段は、モータ電流から各
ジヨイントモータに印加されるエネルギを独立的に測定
する。各ジヨイントモータに対するバックアップ計算エ
ネルギは、そのモータに対する所定のエネルギ限界と比
較され、その限界が超過されたときに超過限界信号が発
生される。ロボットモータは、超過温度又は超過限界信
号が発生したときに電気が切られる。

更に、バックアップ速度モニタ手段は、モータの電流、
端子電圧、インダクタンス及び抵抗から各ジヨイントモ
ータの速度を独立的に測定する。

各ジヨイントモータに対して、第１速度信号とバックア
ップ速度信号との間で比較がなされる。

−殻に、ロボットの能力は、複合製造システムの一部と
してコンピュータ制御され且つシーケンスを持たされた
単純な点から点への繰り返し動作から複雑な動作までの
範囲に亘っている。工場の適用において、ロボットは、
ダイカスト、スポット溶接、アーク溶接、インベストメ
ント鋳造、鋳造、プレス加工、スプレー塗装、プラスチ
ックモールド、機械ツール装荷、熱処理、鏡面研摩、パ
レット運搬、れんが製造、ガラス製造、その他を含む、
種々の製造用途内の幅広い多種の仕事を実行することが
できる。

その勢力範囲内で仕事をするため、ロボットは、典型的
に、アーム、ピストンサブアセンブリ及び終端作動体を
備えている。ロボットアームに対して用いられる同等の
システムは、典型的に、デカルト円筒状極線又は巻線で
ある。一般に、勢力範囲内でピストンサブアセンブリを
提供するために３つの動作軸が用いられ、又、終端作動
体を全てに方向付けるために３つの付加動作軸が用いら
れる。駆動システムは各動作軸に対して用いられ、それ
らは電気、油圧又は気圧で駆動されてもよい。

ピユーマロボット 更に詳しくは、第１図に示すような６軸の産業用電気駆
動式ロボット２０があり、これは、この発明の原理に従
って運転可能な幅広い種々のロボットを示している。こ
のロボット２０は、この発明の譲受人のカンパニーであ
るユニメーションカンパニー（Ｕｎｉｍａｔｉｏｎ　　
Ｃｏｍｐａｎｙ）から市販されており、商品名を「ユニ
メートピューマシリーズ（ＵＮＩＭ＾ＴＥＰ［ＩＭＡ　
５ＥＲＩＥＳ）フ００」いう比較的強力な電気駆動式ロ
ボットである。モデル７６１ピユーマは、約１０に、（
２２ボンド）のペイロード容量と、約１．５ｍ（５９，
１インチ）の到達範囲とを有する。モデル７６２ピユー
マは、約２０に、（４４ボンド）のペイロード容量と、
約１．２糟（４９，２インチ）の到達範囲とを有する。

ピユーマフ００シリーズのロボットは、過酷で最も要求
される製造環境に置かれた場合でさえ、長寿命及び最適
性能を保オする融通性及び耐久性をもって設計されてい
る。高いペイロード又は広い到達範囲に対する明確な客
の要求が、特定の仕事に対してどちらのモデルが適して
いるかを決定する。

ピユーマフ６１は、その長い到達範囲により、アーク溶
接及び密閉調剤などの正確に繰り返す仕事に理論上述し
ている。ピユーマフ６２は、普通以上の重量の用途にお
いて、高精度の材料処理、機械装荷、検査、試験、結合
及び組み立てを実行する。ピユーマロボットは、最小の
床面積しか占めないにもかかわらず、大きな加工範囲は
、多数の機械及び加工面にロボットを就役させる。

各軸の運動はブラシ形直流モータにより発生し、軸位置
フィー−ドパツクはインクリメント式エンコーダにより
発生する。図示したように、ロボットのリストには３個
の関節が設けられ、即ち、矢印２１で示した上下回転、
矢印２２で示した左右回転、及び矢印２３で示した第３
運動が提供される。上下方向におけるエルボ及びショル
ダの回転は、それぞれ矢印２４及び２５で示される。Ｍ
後に、ベース２７上での左右のアーム回転は矢印２６で
示される。

旦」−ムト胴ＪＬｉ疫スー この発明は、第１図のロボット２０と、軸駆動用ブラシ
レス直流モータ及び絶対位置フィードバックを用いる大
形の８６０ロボツトを含む、他のユニメーションロボッ
トとを運転できるロボット制御装置３０（第４図）に向
けられる。しかし、−ｍに、ロボット制御装置３０は、
単独又はロボット群運転で、異なる種類及びサイズのロ
ボットに普遍的且つ融通的に適用できる。

その普遍性の結果として、ユニパル（ＵＮＩＶＡＬ）制
御装置３０は、完全なファミリーロボットを、運転する
ように構成され得る。従って、ウエスチングカンパニー
即ちこの発明の譲受人であるユニメーションで製造され
且つ油圧及び電気で駆動される全てのロボットアームは
、ユニパル制御装置３０によって運転され得る。ファミ
リー使用の鍵、又は更に一般的にはユニパルの普遍性は
、モジュール化と、アーム依存ハードウェアの使用の最
小化とにあり、又、モジュール制御構成のアーム依存ハ
ードウェアの使用をできるだけ避けることにある。ロボ
ット制御装置３０は、頭文字ユニパルで表わされ、完全
にデジタル化されたサーボ制御装置と共に運転し、ロー
コストで更に優れた性能を提供する。

１１記−乙 第２図において、ユニパルロボット制御装置に使用可能
な、−最北された制御ループ構成１００の一実施例が示
されている。このように、各ロボットアームジヨイント
モータ１０２は、トルク制御ループ１０４により運転さ
れる。外側の位置制御ルー１１０６は、速度制御ループ
１０８にタンデム接続され、速度制御ルー１１０８はト
ルク制御ループ１０４を駆動する。

フィードフォアワード加速度制御ループ１１０は、加速
度命令１１２に応答し、アーム及び負荷の慣性１１４も
又、トルク制御ループ１０４の入力端子に直接結合され
る。ロボットアームは、位置制御ループに印加され、一
連のプログラム位置命令１１６を介したロボットプログ
ラムに従って、制御ループ１００により運転される。

第３図は、ユニパルロボット制御装置において現在用い
られている、−最北された制御ループ構成１１８を示す
、これは、完全にデジタル化された制御装置として実施
されることが好ましい、ここで又は上述した他の特許出
願で記載されたような階層的アーキテクチャ及びマルチ
プロセッサアーキテクチャ並びに浮遊点ハードウェアを
設けることにより、軌道サイクルは、使用モジュール構
成に依存した３２ｓ秒から８輸秒の範囲内のサイクル時
間で特徴づけられ得る。

好ましい制御ループ構成１１８において、位置制御ルー
プ１２０及び速度制御ループ１２２は、トルク制御ルー
プ１２４の入力端子に平行に供給される。速度命令は、
ブロック１２８により受信される位置命令からブロック
１２６により発生される。続いて、フィードフォアワー
ド加速度命令は、速度命令からブロック１３０により発
生される。計算された慣性（負荷及びアーム）１３２は
、フィードフォアワード加速度制御ルー１１３６内の符
号文字１３４で示されるように、加速度命令に対して乗
算される。

速度ループ１２０において、この発明の実施例における
速度命令は、ロボット制御装置のモジュール構成に依存
した８ｍ秒〜３２ｍ秒毎に１回発生される。

後述する基本的ロボット制御装置は、３２ａ１秒の軌道
サイクル時間を有するが、増大された接点は８ｍ秒の軌
道サイクルを有する。

どの場合においても、速度命令発生器１３８は、速度フ
ィードバック通路１４０内の速度フィードバックサンプ
リング率に相当する、毎秒１の割合で速度命令を補間す
る９図示したように、ユニメーション８６０に対する速
度フィードバックは、変換器１４２によりアナログから
デジタル変換されたタコメータ信号によって生成される
。スケーラ１４４及びフィルタ１４６は、速度フィード
バック回路装置を完成する。

同様に、位置制御ループ１２２において、補間器１４８
は５位置フィードバック通路１５０内の位置フィードバ
ックサンプリング率に従って、ＩＬ６秒毎に位置命令を
発生する。ユニメーション８６０速度制御装置において
、位置フィードバックは絶対値であり、速度フィードバ
ック通路１４０及び位置フィードバック通路１５０はち
ょうど上述したように動作する（この場合、スイッチ１
５１が図示した状態にある）、ユニメーションのピユー
マロボットでは、タコメータを使用せず、ブロック１５
２で示すインクリメント式位置フィードバックから速度
フィードバックが計算される（この場合、スイッチ１５
１は図示と反対位置に切換えられる）。

速度誤差は加算器１５４により発生され、これに対しブ
ロック１５６により利得が印加される。同様に、位置誤
差は加算器１５８により発生され、これに対しブロック
１６０により利得が印加される。

速度誤差及び位置誤差並びにフィードフォアワード加速
度命令は加算器１６２で加算される。利得はブロック１
６６に印加され、１１１秒毎にトルク制御ループ１６４
の入力端子に印加されるトルク命令を発生する。トルク
誤差は、トルク命令（モータ電流命令）とフィードバッ
ク通路１７０からの電流フィードバックとを加算するこ
とにより、加算器１６８内で発生される。ブロック１７
２は、トルク制御ループ利得をトルク誤差に印加し、出
力命令（モータ電圧命令）は、ロボットジヨイント動作
用モータ駆動電流を供給する電力増幅器１７４に印加さ
れる。抵抗器１７５からの電流フィードバックは、２５
０μ秒毎に発生され、ブロック１７Ｂによりデジタル信
号に変換され且つブロック１７８によりスケーリングが
印加される。

概にづ」五１− ロボット制御袋ｆｉ３０に対する制御ループの実行は、
複数の電子盤上に配設されたデジタル制御回路装置を使
用することにより達成される。盤上の回路装置の機構及
び種々のプロセッサの間へのプログラミングの振り分け
は、１）ロボット制御性能を高め、製造コストの節減に
より特徴づけられるモジュール制御構成と共に達成する
ことができ、２）普遍的使用を可能にする構成の可変性
と、制御性能のレベル選択における融通性とを容易にす
る。

第４図に示すように、制御盤構成は、アームインタフェ
イス盤８００を含み、このアームインクフェイス盤８０
０は、好ましくは制御中のロボットアームの形式に依存
する′全ての回路装置を収容する０例えば、位置フィー
ドバック回路装置は、制御されるべきロボットアームに
より使用されるのが、絶対位置フィードバック又はイン
クリメント式位置フィードバックかによって異なる。従
って、２つ又はできれば３つ以上の異なるームインタフ
ェイス盤８００は１種々の異なるサイズ又は形式のロボ
ットアームのどれに対しても、デジタル制御システムを
提供するために用いられ得る。特定のロボットアームは
、そのロボットアームと共に働くように構成されたアー
ムインクフェイス盤の使用を必要とする。

アームインタフェイス（＾ＩＦ）盤８００は、−ｉに２
つ又はそれ゛以上の盤に関係し且つ特にどの１つの盤に
も関係しない、ｖＭＥバス制御回路装置のような一般的
な回路装置を収容する。

（パルス幅変調された）制御信号は、＾ＩＦ盤８００か
ら発生され、ロボットジヨイントモータにモータ電流を
供給する電力ブロック１５０を制御する。　ＡＩＦ盤８
００は、符号文字１５２で示すワークセル内の他のロボ
ット制御装置、領域ネットワーク内のプログラマブルコ
ントローラ及び他の入出力袋ｆｆ１５３、並びに管理制
御用の高レベルコンピュータ１５４、に対するロボット
制御装置３０の外部結合用チャネルとして動作する。

トルクプロセッサ（Ｔ　Ｐ）盤６００及びサーボ制御盤
４００は、全ての形式のロボットに対する全てのロボッ
ト制御システムにおけるへＴＦ盤８００及び電力増幅器
ブロック１５０と共に用いられる一般的回路盤である。

これら３つの回路盤４００．６００及び８００は、ロボ
・ントアームに対して完全な６軸制御装置を提供し、従
って、ユニパルファミリーのロボット制御装置及び他の
ロボット制御装置に対して基本的制御構成を形成する。

トルクプロセッサ＠６００は、サーボ制御盤４００から
の命令に応答してモータトルク制御を行なう。

続いて、サーボ制御盤４００は、ロボット制御プログラ
ムに従ってアームの分離並びに位置及び速度制御を行な
う。

拡張された制御能力及び又はシステム機能化は、付加さ
れた電子盤又は電子装置と基本的制御装置４００．６０
０及び８００とを相互接続することにより達成される０
例えば、システム制御盤５００を付加し、アーム分離を
含む所定のプログラム機能を、サーボ制御盤４００から
システム制御盤５００に振り分けることにより、ユニパ
ル制御装置は、ロボット２０及び他のロボットを、かな
り速い制御作用即ち３２＋ｍ秒から８１１１秒に短縮さ
れた軌道サイクルで運転することができる。

制御及び他の目的のための盤間データ通信は、ＶＭＥバ
ス１５５内の多数の信号通路を通して行なわれる。更に
、ＶＭＸバス１５６は、トルクプロセッサ盤６００と＾
ＩＦ盤８００とを接続するために設けられている。

多ビン相互コネクタ（第４図には図示せず）は、＾ＩＦ
盤、ＴＰ盤及びＳＣＭｆｆｉ並びに他の接続可能なユニ
ット上に設けられ、ロボット制御装置３０に対して、盤
間のＶＭＣバス及びＶＭＸバスの接続モジュール並びに
盤アセンブリを容易にしている。他のコネクタは、外部
入出力接続のためにＡＩＦ盤８００上に設けられている
。

ジヨイントモータの過負荷に対してロボットのモニタ　
び−ラを−う　テシスーム ロボットの保護システムには、第５八図に示すように、
この発明に従ってジヨイントモータの過負荷又は過熱に
対してロボットをモニタ及び保護するためのシステム６
００＾が含まれている。トルクプロセッサ（ＴＰ）盤６
００は、モータトルク（電流）命令に応答してアームイ
ンタフェイス（＾ＩＦ）盤８００上のパルス幅変調器（
ＰＨＭ）６０１　Ａに対し電圧命令を発生するためのト
ルク制御ループを、各ロボット軸に対して提供する。

ＰＷＭは、種々のロボット軸に対するジヨイントモータ
巻線（この場合、直流ブラシ形又は直流ブラシレス形の
モータ巻線）の両端電圧の適用を制御するデジタル信号
を、運転電力スイッチ６０２八に対して発生する０種々
のジヨイントモータの合成加圧は、通常、更に高いレベ
ルのロボットプログラムから供給される入力トルク命令
を満足させる。

しかし、既に指摘したように、種々の状態は、１つ又は
それ以上のジヨイントモータの巻線に流れる実際の電流
を、モータ加圧限界より超過させ得る。例えば、ロボッ
トアームが障害物に衝突して過負荷状態を引き起こすか
も知れない。

ジヨイントモータ巻線に超過電流が流れた場合、従来の
モータ温度センサ６０４＾は、保護信号を発生する点ま
で短時間で加熱されるだろう、この保護信号は、へＩＦ
盤８００に印加されて処理され、２１４Ｍの出力を零に
駆動することにより、ロボットを停止する。

バックアップ保護及び又はジヨイントモータ加熱の更に
迅速且つ正確な検知を提供するため、モータモニタ及び
保護ソフトウェア６０６＾は、ＴＰ盤６００上のマイク
ロプロセッサ内で実行される。モニタソフトウェア６０
６＾は、実際のモータ電流フィードバック信号６０５＾
を用いて、所定の計算サイクル時間間園を越えて各ジヨ
イントモータに印加されるエネルギを計算する。

次に、ソフトウェア６０６＾は、計算結果を、格納され
たモータ加圧限界６０８＾と比較する。もし、どれかの
限界が超過されると＝ＴＰＯ１６００上のマイクロプロ
セッサは、保護命令を発生して＾ＩＦ盤８００上のＰｌ
ｌｌＮに印加し、モータ温度センサ６０４＾の１つ又は
それ以上により検出されたか検出されないかの動作とは
無関係にロボットを停止する。

モニタ及び保護システムの運転と関連するロジックは第
６＾図に示される。ブロック６１０＾は、物理センサが
ジヨイントモータのどれかの過熱を示しているか否かを
検出する。もしそうであれば、プロツり６１２＾はＰＷ
Ｍを零にセットし、ブロック６１３＾に示すようにロボ
ットは停止される。

過熱が物理的に検出されない場合、又は、過熱が存在し
ても物理的に信号化されない場合は、バックアップロジ
ック通路は、各連続する２ｍ秒の間隔に亘って各ジヨイ
ントモータに対して印加される平均エネルギを計算する
ブロック６１４＾を含む、ブロック６１６＾は、計算さ
れた各モータに対する印加エネルギを、ブロック６０８
＾で示したように呼び出し用に格納された、そのモータ
に対する（例えば製造により特定されるような）モータ
能力に基づく既知のエネルギ限界と比較する。

もし、どれかのジヨイントモータエネルギ限界が超過さ
れたならば、ブロック６１２＾はロボットを停止する。

もし、どのモータエネルギ限界も超過されなければ、ブ
ロック６１８＾は、次に、特別低位モータエネルギ限界
のどれかが超過されたか否か、即ち、ロボット運転の教
示モードに印加可能な低位モータエネルギ限界６２０＾
が超過されたか否かを測定する。もしそうなら、前述の
ようにブロック６１２＾を介してロボットは停止される
。もしそうでなければ、モニタソフトウェアの実行は、
次のサイクルにおいて繰り返されるまで終了する。

モータエネルギ計算を行う場合、各モータに対する実際
のフィードバック電流サンプル６０５＾は二乗されて、
印加された電力に比例したスカラを生成する。計測され
る所定時間間隔に亘って各モータに対するフィードバッ
ク電流を平均するため、電力スカラは、計算される個々
のモータに対応する時定数６１１Ａを有するデジタルフ
ィルタを介して処理される。フィルタ出力は、格納され
たモータエネルギ限界と比較された量である。

以下のアルゴリズムは、記述されたように、モータエネ
ルギチェックを提供するために用いられる： ；ネル　　ニックアルゴリズム エネルギチェックは、モータエネルギを計算し、特定の
時間間隔に亘ってエネルギが特定限界を超過したときに
ＳＣＭを警報する。

１）入カニ 電流　　　　　ＣＵＲＬ　　　　１−秒の平均電流エネ
ルギ限界　ＥＬＩＭＩＴ　　　３０Ｍ盤がら（定数）時
定数　　　　ＴＡＩＩ　　　　ＳＣＮ盤がら（定数）出
カニ 警報　　　　　ＥＣ０ＤＥ　　　ＳＣＮ盤へ２）原理： ；　＊　＋　　　　　　Ｅ　ｎ　ｅ　ｒｇ　Ｆｌ−−−
−−ｘ−−−−−−１／（１＋ａｓ）−−−一−−Ｔｈ
ｒｅｓｈｏｌｄ−−−＾１ａｒｓ＋１　１　　−次フィ
ルタ １＝電流［＾］ ３）アルゴリズム： ｔｅｍｐ　　＝　ＣＵＲＬ”２−　ＥＮＥＲＧＹ２０Ｅ
ＮＧＹ＝　ＺＯＥＮＧＹ＋　ｔｅｍｐＥＮＥＲＣ：Ｙ＝
　ＺＱＥＮＧＹ／２”１６”ＴＡｔｌｉｆ　　ＩＥＮＥ
ＲＧＹＩ＞ＥＬＩＭＩＴｔｈｅｎ　　ＥＣ０ＤＥ＝　０
ＶＥＮＧＹ４）スケーリング： １）警報出力の状態 ｉ１″２＝ｋ”１１ｍ１ｔのステップ入力が印加された
場合、ステップが印加された後、警報状態をｔｘ秒出出
力る。

ｉｉ）等式 ％式％ −４ｄｅｌｔに対して約２Ｏ−４０＝ｂ速庁モニタ　び
　己シスーム 第５Ｂ図に示すように、ロボット保護システムは、更に
、速度制御運転にバックアップチェックを提供する速度
モニタ及び保護システム６０．ＯＢを含み、安全及び財
産保護の理由のため、故障が生じたときに保護動作を開
始する。バックアップ速度システム６００Ｂは、トルク
プロセッサ（ＴＰ）磐６００の回りの中央に位置してい
る。

次に高い制御レベルにおいて、ＳＣＭ盤４００は、シス
テム動作ソフトウェアを用いて、各ロボット軸に対する
位置命令を引き出す。これら２つのコア盤及びＡＩＦ盤
の使用により、ロボット制御装置は、その基本制御構成
にある。

ＳＣＭＱ１４００上の位置及び速度制御回路装置は、位
置命令に基づいて動作し、ＴＰｆｆｉ６００上のトルク
制御装置に対しトルク命令を発生する。続いて、ＴＰａ
６００は、各ジヨイントモータに対し電圧命令を発生す
る。

電圧命令は、＾ＩＦ盤８００上のパルス幅変調器（ＰＩ
ＩＩＭ）に印加される。　ＰＷＭは、続いて、種々のジ
ヨイントモータに対する電力スイッチを動作し、ジヨイ
ントを同等に動かして、ロボットツールチップが命令位
置に行くようにする。

位置／速度フィードバック６０２Ｂは、ロボットからＡ
ＩＦＷ８００に結合される。υＮＩＭ＾Ｔｌ０Ｎ８６０
ロボットの場合、速度フィードバックはタコメータ信号
の形態である。　ＵＮＩＭ＾Ｔｌ０Ｎ　　ＰＵＮ＾ロボ
ットに対し、フィードバックは、＾ＩＦＩ１８００上の
回路装置で処理されて速度フィードバック信号を生成す
るための位置エンコーダ信号の形態である。

モータ電流フィードバックは、電力スイッチ回路装置内
で検知され、参照文字６０４Ｂで示すように＾ＩＦ盤８
００に結合される。

バックアップ速度モニタソフトウェア６０６Ｂは、各ジ
ヨイントモータの実際の速度の概数を、モータ逆起電力
、即ちモータ端子電圧、既知のモータ抵抗及びインダク
タンス並びに検知されたモータ電流に基づいて正確に計
算するために用いられる。

゛合成された計算速度は、こうして、タコメータ又はエ
ンコーダで検知された速度とは無間係に測定され、バッ
クアップ速度チェックとして作用する。

この場合、速度モニタソフトウェア６０６Ｂは、ＳＣＭ
ｆｆｉ　４００上のマイクロプロセッサとＴｐＨ６００
とに分離されている。従って、逆起電力速度計算はＴＰ
！！６００上でなされ、チェック及び保護動作のための
速度計算の処理は、ＳＣＭ盤６００上で実行される。

図示したように、実際のモータ電流フィードバックは＾
ＩＦ盤８００から引き出される。ＴＰ盤６００からの電
圧命令は、ソフトウェア計算においてモータ端子電圧と
して用いられる。

バッファ・・プ　庁モニ　　び　−ソフトウェア速度モ
ニタ及び保護プログラムは、第６Ｂ図内のフローチャー
トに示される。このように、速度計算ルーチンは、参照
文字６０８Ｂで示すように２１秒毎に実行される。逆起
電力速度計算に必要な入力は、ブロック６１０Ｂ内で得
られ、その速度は、ブロック６１２Ｂ内で各ジヨイント
モータに対して計算される。

計算に用いられるフィードバックモータ電流及び電圧命
令の各位は、先行する２ｍ秒に対する平均値である。

以下の等式及びアルゴリズムは計算に用いられる。

等式： ％式％） 但し、Ｒ＝モータ抵抗　　　　　［ｏｈ論］Ｌ　＝モー
タインダクタンス［）〕 Ｖｉｎ＝モータ端子電圧　　　［Ｖ］ Ｋｅ　＝モータ逆起電力定数　［Ｖ／ｒａｄ／秒］ｗ＝
モータ速度　　　　　［ｒａｄ／秒］ｉ　＝モータ電流
　　　　　［＾］ アルゴリズム： ＶＥＬ（ｎ）　＝　［Ｐ１４Ｍｏｕｔ”２”９−　ＭＯ
ＴＬ’１（ｎ）　−１（ｎ−１）　−１（ｎ）”ＭＯＴ
Ｒ］／　２”１８ 但し、１（ｎ）は右に６ビツトシフトされてｌｌ１１秒
／Ｔｓの時間だけ蓄積された＾／Ｄ値（１ｍ秒間の平均
動作）。

ＴｓはＰＷＮサンプルレート＝　２５０ｍ秒り、Ｒのス
ケーリング： ｉ）等式： ％式％（） ）： ＡＩ　　　Ｋｐ＋＋ｕａ＝　Ｖ＋ｉａｘ／２”１５−−
−Ｋａｄ−−２”２−６一−ＭＯＴＲ−Ｉ　　　　　Ｋ
ａｄ　　＝２”本１５／１＋ｗａｘ−ＭＯＴＬ−１ この点において、計算された速度はマイクロプロセッサ
に転送され、そこでサーボ処理部と呼ばれるブロック６
１４Ｂを実行し、逆起電力計算速度をフィードバック速
度６０２Ｂと比較する。もし、差が存在し、その差が所
定の許容値を超過した場合、ブロック６１６Ｂは故障を
登録し、プロ・７り６１８Ｂは保護動作を開始し、即ち
ロボットを停止する。

セレクタ６２０Ｂ（第５Ｂ図）を介してオペレータが選
択した結果として、ロボットが教示モードで運転されて
いるという特殊な使用状態において、臨時安全を提供す
るために、特別な低速度限界が速度制御装置に印加され
る。その場合には、ソフトウェアブロック６２２Ｂが教
示モードを検出し、ブロック６２４Ｂが逆起電力計算速
度を教示モード速度限界と比較する。そして、もし、そ
の限界を超過したときには、ブロック６１６Ｂが故障を
登録し、プロ・ツク６１８Ｂが保護的停止動作を開始す
る。もし、計算速度が教示モード限界内であれば、プロ
グラムの実行は、前述のように速度フィードバックモニ
タ用のブロック６１４Ｂに進む。

サーボ制御モジュール（ＳＣＭ）即ち盤４００（第）＾
−１図及び第７＾−２図）は、ロボット制御システムの
モジュールアーキテクチャに従って構成され、完全な基
本的ロボット制御装置に対するコア盤として動作し、格
納されたロボットプログラム命令からアームの分離を発
生するか、又は、拡張したロボット制御装置の一部とし
て動作し、更に高いレベルのシステム制御盤５００によ
る、ロボットプログラム命令から生成されたアーム分離
を実行するために受信する。アーム分離の発生は、ロボ
ットプログラム言語の解読、通路確立、軌道計算（中間
位置命令及び座標軸）及びデカルト座標系とロボットジ
ヨイント及びロボットツール座標系との間の位置情報の
変換を含むロボット制御機能の実行を伴う。

ＳＣＭ盤４００は、更に、関連周辺制御装置及びホスト
コントローラ（もしあれば）との通信インクフェイスに
もなる。

５ＣＮＩ３１４００は、ロボット制御システムに対して
アーム動作制御ループを実行するためのプログラム制御
されたデジタル回路装置を備えている。運動制御は、好
ましくはトルクプロセッサモジュール６００による実行
用のトルク命令を生成するための、相関位置、速度、及
び加速度制御ループを含む制御ループ構成を介して各軸
筋に行なわれる。デジタルサーボ制御装置は、座標マル
チプロセッササーボ制御装置であり、（１）アーム分離
により各軸筋に供給された位置命令及び速度命令、並び
に（２）アームインタフェイスモジュール８００を介し
て、位置エンコーダ及びタコメータから得られた位置フ
ィードバック信号及び速度フィードバック信号から出力
トルク命令を発生する。

ＳＣＭ制御ループ動作においては、印加された軸位置命
令及び軸位置フィードバックから、各軸筋に位置誤差が
計算される。速度誤差は、連続的位置命令から引き出さ
れた速度命令から且つ軸速度フィードバックから、各軸
筋に計算される。好ましくは、位置制御ループ及び速度
制御ループは並列に運転され、即ち、位置誤差及び速度
誤差が加算され、トルク制御モジュール６００上のトル
ク制御ループのためのトルク命令を生成する。更に、好
ましくは、加速度命令は、連続的速度命令から引き出さ
れ、フィードフォアワード加速度制御ループに印加され
る。このフィードフォアワード加速度制御ループは、８
０Ｍ出力トルク命令を発生しているときに、位置誤差及
び速度誤差の総和に対する加速度に基づくトルク命令を
発生する。

ループ計算がなされる周波数は、速く、正確で、スムー
ズ且つ安定なロボットアーム動作を生成するために選択
される０例えば、用いられる周波数は、この場合のよう
に３２ｍ秒の軌道サイクルを提供するようなものであり
得る。もし望むならば、更に速い、即ち、８ｍ秒のよう
に短い軌道サイクルを達成することができる。

ＳＣＭ−ジ　ルロ 第７八−１図及び第７＾−２図で示すように、ＳＣＭ盤
４００は、一般に２つの部分、即ちローカルプロセッサ
部分４０１と、システムリソース部分４０３とを備えて
いる。システムリソース部分４０３は、バス４０５を用
いて、全てのロボット制御システムに関係し、且つ位置
制御ループ及び速度制御ループの実行に特に関係しない
機能を提供する。

これらの機能は、ロボットアーム分離の格納用ＥＩ’Ｒ
ＯＭ　、不揮発性データ格納用のバッテリバックアップ
ＲＡＭ、スタティックＲＡＭ、リアルタイムクロック、
ＯＭ＾コントローラ、並びに、２つの複数プロトコル及
び２重チャネル通信コントローラを含む。

システムリソース領域は、２重ボートメモリとして実行
される。このように、システムリソース部分に対する等
しいアクセスが、ローカルプロセッサ又はＶＭＥバスか
ら提供される。システムリソース機能は、バススレーブ
としてＶＭＥバスに現れる。このことは、これら関係機
能に対して、ＳＣＭローカルプロセッサから、又はシス
テムバスに接続された任意のプロセッサから制御される
べき能力を提供する。

ローカルプロセッサ部分４０１においては、ＳＣＭデジ
タル回路装置は、特定性能に必要な座標デジタルコプロ
セッサ（ｃｏｐｒｏｃｅｓｓｏｒｓ）並びにインタフェ
イス及びリソース回路装置を含み、即ち、全ての軸の正
確且つ有効な制御に必要な制御計算及び制御データ管理
を提供し、又、インタフェイス通信にホストコントロー
ラ、周辺装置及び他のロボットコントローラを提供する
。好ましくは、サーボ制御管理器４０２は、主にスレー
ブプロセッサとして機能して位置制御ループ計算及び速
度制御ループ計Ｘ（即ち、フィードバックフィルタ、ル
ープ利得、位置誤差及び速度誤差、その他）を行なうサ
ーボ計算器４０４と共に動作する。

サーボ制御管理器４０２は、制御装置、状態データ及び
プログラムデータを、５ＣＮｐＡ４００へ（から）及び
サーボ位置速度制御計算器４０４へ（から）向ける。サ
ーボ制御管理器４０２は、高いデータ処理能力を持つモ
トローラ（Ｍ　ｏｔｏｒｏｌｍ）６８００Ｇであり得る
。プロセッサ４０２及び４０４の各能力に従ってデータ
管理及び制御計算タスクを隔離することにより、基本的
回路機構は、実質的に改善された制御性能を達成するた
めの基本として、製造上の及びユーザの経済性が提供さ
れる。

図示された実施例において、ＳＣＭ盤４００のローカル
プロセッサ部分の実行は、サーボ制御管理器４０２とし
ての６８０００プロセツサ及び２つのコプロセッサの用
法に基づいている０両方のコプロセッサは、８８０００
に対する周辺装置として就役する。１つのコプロセッサ
４０６（好ましくは、ナショナルセミコンダクタ［Ｎａ
ｔｉｏｎａｌ　Ｓｅｍ１ｃｏｎｄｕｃｔｏｒ］３２０８
１）は、アーム分離がＳＣＭ盤４００によって提供され
るべきときに浮遊点計算能力を提供する。他のコプロセ
ッサ即ちスレーブプロセッサは、位置速度サーボ計算器
４０４であ　リ　、　テキサスインスツルメンツ（Ｔｅ
ｘａｓ　　Ｉｎ５ｔｒｕ＋＊ｅｎｔｓ）のＴｌ４Ｓ−３
２０１０デジタル信号プロセッサと共に実行される。こ
の位置速度プロセッサは、高速固定小数点計算能力を提
供する。

ローカルプロセッサ部分の一部の残りの機能は、ローカ
ルメモリ、ＥＰＲＯＭ及びｒｌＡＭの両方、周辺タイマ
カウンタ装置、インタラブド制御装置、及びシステムエ
ラーモニタ装置を含む、ローカルプロセッサ４０２は、
ＴＰＭ又は他の関係する形式の機能にアクセスするため
の、ＶＭＥバスに対するマスクとして就役し得る。しか
し、ＳＣＭ盤４００は、通常はシステムリセットの発生
、バスに対するアクセス用のバス調停、及びシステムバ
スクロックの発生を含むＶＭＥバスシステムコントロー
ラ形式の機能を提供しない、なぜなら、これらの機能は
アームインタフェイス盤８００上で実行されるからであ
る。

サーボ制御管理器４０２及びサーボ計算器４０４間のイ
ンクフェイスにおいて、［ビンボン（ｐｉｎｇ−ｐｏｎ
ｇ）」メモリ又は［バンク切換（ｂａｎｋ　　５ｗ１ｔ
ｃｈｅｄ）」メモリと呼ばれる特別な２重ボートメモリ
機構は、どちらのプロセッサも、各プロセッサの処理性
能に影響を与えることなく、他方のプロセッサと通信で
きるようにする。

−ハ４御盤のプログラム 第７Ｂ図〜第７Ｅ図に示したように、サーボ制御データ
管理器４０２用のプログラムシステムは、ＭＡＩＮと呼
ばれるバックグランドプログラム４５０と、５ＥＲＶＯ
と呼ばれサイクリックに運転されるフォアグランド割込
みルーチン４５２とを備えている。　ＲＥＳＥＴで示す
ようにシステムが開始されたとき、初期設定ルーチン４
５３は、それに続（ＭＡＩＮプログラム４５０の実行に
先立って実行される。サイクリックに実行される５ＥＲ
ＶＯ割込ミ４５２ニ加えて、Ｃ＆ＵＮＥＸと呼ばれる割
込ルーチン４５４は、プロセスに対する臨時の又は予期
しない割込みの要求により、フォアグランド内で動作す
る。更に、ウォッチドッグ（ｗａｔｃｈｄｏｇ）タイマ
と呼ばれる特殊な最優先ルーチン４５フは、外部のウォ
ッチドッグハードウェアの動作に応答してｌｌ！ｌ能を
遮断する。

ロボット制御システムが更に高い計算容量による高性能
用のシステム制御盤５００を含む場合、Ｈ＾！Ｎプログ
ラムは、システム制御盤５００からの位置命令を受信且
つ分配するために提供される。最小又は基本的なロボッ
ト制御システム構成において、システム制御盤５００は
含まれず、Ｈ＾ＩＮプログラム４５０は、更にアーム分
離を実行し、サーボ制御盤４００上にローカルに位置命
令を発生する。

５ＥＲＶＯルーチン４５２をサイクリックに実行するた
めにＮ＾ＩＮプログラム４５０が遮断されるレートは、
アームインタフェイス盤８００から、各鋼秒毎にＶＭＥ
バス１５５上に１回発生される信号ＶＴＩＣににより制
御される。５ＥＲＶＯルーチン４５２により提供される
基本的機能は、 １）制御データをサーボ計算器４０４に又はサーボ計算
器４０４から転送すること、 ２）制御データをトルクプロセッサ盤６００に又はトル
クプロセッサ盤６００から転送すること、３）アームイ
ンタフェイス盤８００からＶＭＥバス１５５を介したセ
ンサフィードバックデータを受信すること、 ４）バックグランドタスクＲＤＭ＾ＳＣの維持のための
インタフェイス、 ５）同期的データの記録を実行すること、６）ワンショ
ットデータのロギング（ｆｏｇｇｉｎｇ）を実行するこ
と、 ７）ブラックボード格納領域内に放送データを位置させ
ること、 ８）厳格なエラー状態が起きたときにシステムを停止さ
せること、 である。

初期設定ルーチンは、第７Ｃ図に更に詳細に示されてい
る。プロセスは、連続的ブロックで示すように、本質的
に直列タスクの性能を備えている。

以下のリストは、これらタスクの本来の姿に基づく更に
詳細な情報を提供する。

タスク　　初」■え笈− ＩＮＩＳＲＣハンドシェイク用のピットバスエミュレー
タ ｌＮＳＣＲＯＭ　　フィードバックインタフェイスＩＮ
ＩＦＩＸ　　広域可変性 ｌＮＣ０ＮＦ　　ロボット故障状態 ＩＮＴＭＳ　　　サーボ計算器ダウンロード（ｄｏｗｎ
ｌｏａｄ）プログラム記憶装置（ｓｔｏｒｅ） ■旧＾ＩＢ　　）ルクプロセッサ盤６００ＩＮＲＯＢ　
　　ロボットソフトウェア構成ＩＮＪＴＭＳ　　バンク
切換メモリ ＩＮＪＯＮＴ　　サーボ計算器プログラムメモリ及びト
ルクプロセッサ介したジヨイント データ構成 ＩＮＢＵＰ　　　内部バッファ管理領域ＩＮＩＤＬＣワ
ンショット及び同期的データロギングインクフェイス ＣＲＴＩＮｌ　　入出カバードウェアサーボ制御！　４
００８ＢＩＮＩＴ　　ビットバスエミュレータインタフ
ェイス及びシステム制御盤５００とのハンドシェイク 一旦、システムが初期設定されると、Ｈ＾ＩＮプログラ
ム４５０（第７Ｄ図）は、それに続くサイクリックなバ
ックグランドの実行を開始する。第７Ｅ図に示すように
、ＲＤＮＡＳＣと呼ばれるルーチン４５６が実行され、
システム制御ｆｆｉ　５００から受信されたときにシス
テム命令が蓄積される、分配されたＲＡＭ内の指示メモ
リ領域であるピットバスエミュレータからのシステム位
置命令の読出しを継続させる。

全体として見ると、バックグランドルーチン及び割込ル
ーチンは、以下の最終サーボ状態を持つことができる。

０・・・起動状態 １・・・初期設定状態 ２・・・較正状態 ３・・・サーボ適性状態 ４・・・サーボ不可能状態 ５・・・サーボ可能、保持状態 ６・・・サーボ可能、動作状態 バックグランドタスクは、有効状態の変位が起きたとき
の保証に対して、又、割込タスクを新しい′状態に順序
立てることに対して責任がある。従って、割込タスクは
、「命令」状態を受は入れ、且つ「現在の（ｃｕｒｒｅ
ｎｔ）」状態を供給するために書込まれる。更に、バッ
クグランドタスクは、ピットバスエミュレータからの命
令を介して順序立てられる。以下の状態変位表はこの動
作を定義する。

現在の　　　　とットバスからの命令ＩＤｆｌ　３８　
１７　４５　１　　現バーυ−ユ　追促２　　−１　３
　−１−１　−１”　　２　−１　−１　−１−１但し
、「−１」は違法状態変位を示す、ビットバス命令は以
下のように定義される。

３６−、どこの（ｗｈｅｒｅ）命令 １７・・・較正命令 ４５・・・設置（ｉｎｓｔａｌｌ）サーボパラメータ命
令１・・・可能不可能サーボ命令 １６・・・保持命令 ３４・・・初期設定命令 ２０・・・通常運動命令 ２・・・制動（Ｂｒａｋｅ）命令 １８・・・柔軟な（Ｌｉｍｐ）命令 ４６・・・アップロード（Ｕｐ　１ｏａｄ）電流サーボ
パラメータ命令 一般に、位置命令のセットは、サーボループのためにパ
ル（Ｖａｌ）サイクルに１回発生され、この場合、３２
ｍ秒毎に１回発生される。続いて、各パルサイクルは、
この場合、ＶＭＥバス上に各ＶＡＬＣＹＣＬＥ毎に１回
発生するＶ線（Ｖｔｉｃｋｓ）により測定される。この
場合、各ＶＡＬＣＹＣＬＥ４：３２個のｖ線がある。

ＴＣＫＣＮＴＲと名付けられたサイクルカウンタは、各
ＶＡＬＣＹＣＬＥ内のＶ線を計数すル、ＵＮＩＣＮＴｌ
：呼ばれる動作中のカウンタは、Ｖ線の総和が２３１個
に達するまで計数し続ける。

ブロック４５フ内のＵＮＩＣＮＴＲによって示されるよ
うに、次に続＜　ＶＡＬＣＹＣＬＥが終了したとき、ブ
ロック４５８ハ、ＴＣＫＣＮＴＲ＝　１　ノドきの新し
イｖＡＬＣＹＣＬＥノ開始に対し、新しいＶＡＬＣＹＣ
ＬＥに対する位置命令の新しいセットの受は入れを初期
設定するように動作する。こうして、ブロック４５９は
、ピットバスエミュレータからの新しい命令を読み取る
。

命令が緊急命令（不可能サーボのような）を含む場合、
ブロック４６０は、その緊急命令を緊急命令バッファに
向ける。ブロック４６１は、運動（位置）命令を運動命
令バッファに送る。同様に、ブロック４６２は非運動命
令を非運動命令バッファに送る。

ブロック４６Ｇ、４６１及び４６２は基本的機能のよう
に実行する。第１に、命令は、分配されたＲＡＭ内のピ
ットバスエミュレータから読み取られる０次に、命令を
有効にするため、規定されたチェックが行なわれる。最
後に、命令は、ローカルの分配されないＲＡＭ内の指定
されたバッファに発送される。

もし、システム命令を内部のＲＡＭバッファに転送する
ための方向が全く見つからない場合は、ブロック４６３
は、命令がないエラーを記録して、代わりの命令（最後
の命令と同一、又は、これが命令がないエラーの２番目
の発生の場合は停止）を用意する。

次に、ブロック４６４及び４６５は、線（ｔｉｃｋ）カ
ウンタの値が現在のＶＡＬＣＹＣＬＥの終わり近づいた
とき、即ち、それがＶＡＬＣＹＣＬＥがら３を差し引い
た値と等しくなったときに、最新の命令が受信されたと
いう応答をシステム制御盤５００に送る。ブロック４６
６は、次に、ＲＤＭＡＳＣが再び実行されて次のシステ
ム命令を受信するまで、ＵＮＩＣＮＴＲのポーリング（
ｐｏ　ｌ　Ｉ　）を繰り返し行なう。

ＶＴＩＣＫが発生（即ち、１檜秒毎に１回）したとき、
ＲＯＭＡＳＣ４１，５ＥＲＶＯルー　ｆ　ン４５２を実
行すルタメニ一時的に遮断される。フローチャートに示
したように、５ＥＲＶＯルーチン４５２は、ブロック４
７０内に示すように、記録（ｂｏｏｋｋｅｅｐｉｎｇ）
を必要とするタスク及び上位（ｏｖｅｒｈｅａｄ）タス
クを第１に実行する０例えば、ウォッチドッグタイマは
、２ｍ＋秒の割込の間リセットされる。線が長い線、即
ち線カウンタが１に等しく新しいＶＡＬＣＹＣＬＥの開
始を示す場合、ブロック４７１は、成る付加的上位タス
クを実行する。実行される基本的機能は、線カウンタを
１にリセットすること、新しい位置命令をシステム制御
盤からサーボ計算器に発送すること、及び、５ＥＲＶＯ
ルーチン内の状態変化を処理することである。

サーボ計算器に対する切換のための 一一夕のローディング 線が短い線、即ち３２個の線サイクル内の中間の線、又
は長い線の上位が実行された後の線の場合、ブロック４
７２は、サーボ計算器があるべき状態に従つてバック切
換メモリにデータをロードすることにより、データをサ
ーボ計算器に転送するために用意する。

第７Ｅ図内のフローチャートは、サーボ計算器の企画を
更に詳細に示す、計算器が起動状態にあるべきであるこ
とをブロック４７３が見つけ場合、ブロック４７４は、
サーボ計算器４０４に対するダウンローディング（ｄｏ
ｗｎｌｏａｃｌｉｎｇ）用ロジックを起動するのに必要
なデータを取ってくる。初期設定状態のため、ブロック
４７５及び４７６は、サーボ計算器４０４に対するダウ
ンローディング用初期設定命令データを取ってくる。同
様に、較正命令データは、計算器較正状態のためのブロ
ック４７７及び４７８により、サーボ計算器４０４に対
して用意される。

計算器が不可能であるべき場合、ブロック４７９及び４
８０は、不可能命令をバンク切換メモリ内にロードする
。結局、計算器の状態が保持又は動作されるべき場合（
これはほとんどの動作ラインを覆う）、ブロック４８１
及び４８２は、運動命令データをバンク切換メモリ内に
ロードする。リストされた状態が１つも検出されない場
合、ブロック４８３はエラーを記録する。

一ボー　　６に　　る−一夕の切 企画サブルーチン４７２の完了後、ブロック４８４は、
サーボ制御データ管理器４０２内のフォアグランドバッ
クグランドインクフェイスのために転送サービスが必要
などんなデータも実行する。

サーボ計算器内において、２つの基本的機能が実行され
る。第１に、ダウンローディング位置命令データが、Ｖ
ＡＬＣＹＣＬＥ内の長い線間の各３１個の線のために補
間され、各線に対する位置命令データから速度命令デー
タ及び加速度命令データが計算される０次に、現在適用
可能な位置命令、速度命令及び加速度命令に対する各線
の後に各軸についてのサーボ計算がなされ、又、同時に
受信された位置フィードバック及び速度フィードバック
について計算がなされる。その結果、トルクプロセッサ
磐による実行のための全ての線の後に、各軸に対するト
ルク命令が計算される。

トルクプロセラ　盤の」（ トルクプロセッサ（ＴＰ）５３６００は、ロボットジヨ
イント駆動モータに対する機能的なインタフェイスを提
供する０機能的に、ＴＰ盤６００は、６個のロボットの
ための軸閉ループサーボトルク制御を提供する段階的制
御システム内の最低レベルの制御を実行する。物理的に
、ＴＩ’盤６００は、ロボット通路計画制御システム及
びサーボ制御（ＳＣＭ）盤を、アームインタフェイス（
八ＩＦ）盤８００に電気的にインタフェイスし、続いて
、ロボットジヨイント駆動モータにインタフェイスする
。ＴＰ盤６００の第１の機能は、ＡＩＦ盤上のパルス幅
変調計画を用いて実行されるモータ巻線電圧命令を発生
することにより、ロボットジヨイントモータ電流を命令
された値に調整することである。

ＴＰ盤６００は、１つのレベルでＳＣＭ盤にインタフェ
イスし、５ＣＮｉからのトルク命令及び６軸に対するサ
ーボパラメータを受は入れ、状態データを復帰（ｒｅｔ
ｕｒｎ）する、　ＴＰ盤６００は、６個のロボット軸の
ためのサーボ電圧命令を提供する＾ＩＦ盤８００に、第
２の低レベルでインタフェイスする。＾ＩＦＱ１８００
は、ＳＣＭ盤及びＴＰ盤上の閉ループ制御のために、駆
動モータ電流、位置フィードバック及び速度フィードバ
ックを受信する。

ＴＰ盤６００は、以下を含む多数の特徴を提供するため
にベアのマイクロプロセッサを用いる。

１、ブラシモータ又はブラシレスモータに対する６軸（
６軸について２５０μ秒）のためのトルクループ制御、 ２、電流オフセットを調整可能なソフトウェア・・・ポ
テンショメータを除去する、３、ダウンローディング可
能なゲイン・・・アームに依存するパラメータが、Ｓｅ
ｔ４ｍからダウンロードされ得る、 ４、ＰＷＭ補償、 ５、転換補償、 ６、データのロギング及び他の目的のための電流平均化
、 ７、安全のための電流限界チェック、 ８、安全チェック用の速度モニタ（逆起電力）９、停止
（ｓｔａｌｌ）状態を試験するためのエネルギチェック
（ＩＩＴ）、 １０、電力上昇自己診断、及び １１、ダウンローディング可能な診断システムトル　プ
ロセラ　盤 更に進んだロボット性能は、命令軌道に従ってアーム動
作点位置を制御するためにアームが運動中のときに、ア
ーム動作点に印加されるトルクをデジタル制御すること
により生成される。軸駆動力は、更に大きい速度、正確
さ及び効率で位置命令及び軌道命令を満足させるために
、実際に経験されるワークピースのローディングに従っ
て調整される。ｆＩＬ終的に制御される変数としてのト
ルク制御に向けられた関連発明については、ウェスチン
グハウス・エレクトリック・カンパニーの出願ＷＥ５３
，３７６を参照することができる。

トルク制御は、トルクプロセッサ（ＴＰ）０１、即ち異
なるロード容量、異なる形式の駆動、異なる軸数、その
他を有する広い種々のロボットに対してトルク制御を提
供するために使用可能な電子盤と呼ばれる一般的制御回
路盤６００（第４図、第８＾−１図及び第８＾−２図）
上で実施される。

トルクプロセッサ盤６００は、更に高い制御レベル（Ｓ
ＣＭ盤）から得られるトルク命令と、軸駆動装置からア
ームインタフェイス（＾ＩＦ＞盤８００を介して得られ
るフィードバック電流とに基づいて、各ジヨイントモー
タ即ち軸駆動装置に対して電圧命令を発生するため、デ
ジタル回路装置を用いる。従って、全てのジヨイントモ
ータに対するトルク制御ループは、ＴＰ盤回路装置を介
して閉成される。

電気的装置の場合、フィードバック電流は、実際のモー
タトルクに比例するモータ巻線電流であ　□る。油圧駆
動装置に対しては、フィードバック電流も又、実際のモ
ータトルクに比例する。

好ましくは、デジタルトルク制御回路装置は、多数のデ
ジタルプロセッサで構成され、必要な制御計算及び制御
支持機能が、全ての軸についてサンプリング周波数規定
内で正確に且つ効率的に達成され得る。

特に、トルク制御管理器６０２は、格納されたトルク制
御データを交換するための２重ボートのＳＣＭインタフ
ェイスメモリ６０４に対して、ＳＣＭ（サーボ制御モジ
ュール）制御レベルと、ＴＰ（）ルクプロセッサ）制御
レベルとの間でインタフェイスする。軸トルク命令及び
制御ループパラメータは、好ましくはＶＭＥ形式のデー
タバス６０６を介して、ＳＣＭからＴＰインタフェイス
メモリ６０４にダウンロードされる。一方、状態データ
は、サーボ制御レベル（ＳＣＭ）にアップロードされる
。　ＴＰ盤及びＳＣＭ盤間のメモリインタフェイス６０
４は、ＶＮＥバス６０６に対するスレーブとして就役す
るメモリ計画が分配された２重ボートである。

他の盤メモリは、ピンポンメモリ６０８、プログラムＥ
ＦＲＯＭ、ローカルＲＡＭ及びＴＰ計算器メモリを含む
。

トルク制御管理器６０２は又、電流フィードバックの流
れを、次の低制御レベルにある＾ＩＦ盤８００上の回路
装置から、トルク制御ループ動作用のトルクプロセッサ
盤６００に向ける。トルク制御計算の結果としての駆動
電圧命令は、トルク制御管理器６０２によりアームイン
タフェイス（＾ＩＦ）盤８００に向けられる。ピンポン
（バンク切換）メモリ６０８は、ハンドシェイクフラグ
の制御下で動作し、命令データ、フィ−ドパツクデータ
、及び状態データを格納しているので、トルク制御計算
、又は更に高い制御レベルの報告要求、又は軸駆動制御
が必要なときに役立つ。

デジタル信号プロセッサの形態で提供されたコプロセッ
サ６１０は、トルク制御管理器６０２からのトルク命令
及びフィードバック電流をピンポンメモリ６０８を介し
て受信するトルクループ計算器として動作し、トルク命
令及びフィードバック電流がら計算されたトルク誤差に
基づいて種々のロボット軸に対する駆動電圧命令を計算
し、そして、この駆動電圧命令を、トルク制御管理器６
０２がらの命令の際に、ピンポンメモリ６０８を介して
アームインタフェイス回路装置に転送する。

記載されたデジタル回路構成により、全ての必要とされ
るトルク制御機能は、迅速に（２５０μ秒のサンプリン
グレート又はそれ以上で）、且つ周波数帯域要求内で正
確に実行可能である。とりわけ、デジタル信号プロセッ
サ６１０の迅速な計算能力は、データの組織化としての
トルク制御計算のために用いられ、又、トルク制御管理
器６０２の方向付は能力は、はとんど他の機能のために
用いられることにより、能率的に且つ経済的に達成され
るべき非常に改善された制御性能を可能にする。

トルク制御管理器６０２は、データ管理のためのタスク
に適したアーキテクチャウェルを持つが、あまりに制限
された計算速度（即ち、１６×１６ビツトの乗算に対し
て４μ秒以上）のため、トルク制御帯域要求を満足する
ことができない、デジタル信号プロセッサ６１０は、Ｚ
変換計算（即ち、１６ｘ１６ビツトの乗算に対して２０
０ｎ秒の計算速度）のためのアーキテクチャセットを持
つが、もし持たなければ、一般に、データ管理器プロセ
ッサ６０２に指定された種類のタスクに適していない、
これら２つのマイクロプロセッサは、ユニットとして、
又は言いかえればサーボエンジンとして一緒に機能する
。

トルク、　プロブーミング トルクプロセッサ盤６００は、盤上プロセッサ６０２及
び６１０内で実行されるプログラムの制御下で動作され
、高いＳＣＭ制御レベルからトルク命令信号を実行する
。

一般に、トルクプロセッサソフトウェアは、以下に示し
たように分割されたタスクを実行する。

トルク　　　　　６０２ ＳＣＭとの通信 命令操作 電流サンプリング、交信及びオフセット調整通信切換フ
ラグ（状態の読取り） ピンポンメモリ管理 ＰＷＭチップ管理 診断 エラー報告 トル　ループ　　６１０ （２５０μ秒の割込に基づくプログラムサイクリング）
過電流チェック・・・絶対値及び平均値トルクループ計
算 電流平均化 ＰＷＮ補償 転換（ｃｏｍｍｕｔａｔｉｏｎ）補償 逆起電力チェック・・・安全速度のモニタエネルギチェ
ック・・・機能停止状態の試験二二ｍ凱赳１− 前述したように、更に高レベル制御ループ化することは
、アーム軸のために＾ＩＦ盤８００を介して実行される
べき電圧命令信号を発生するので、アーム作動体は、ユ
ーザのロボット又は適用プログラム従って、制御された
速度、加速度及びトルクの下で、命令された位置に動か
される。パルス幅変調回路装置８０１及び駆動回路装置
８０２は、軸駆動信号を引き出すためにへＩＦ盤８００
上に設けられている。

この場合、軸駆動信号は電力増幅器に印加するためのも
のであり、この電力増幅器は、アーム運動の６軸と共同
して、直流（ＤＣ）ブラシレス電気モータにそれぞれ駆
動電流を提供する。

＾ＩＦ盤回路装置は、電圧命令データを処理してデジタ
ルＴＴＬ論理レベル信号を引き出し、軸モータにモータ
駆動電流を供給する電力増幅器のベース又はゲート駆動
回路装置を制御する。前述したように、モータ電流並び
に軸位置データ及び速度データは、閏ループの位置制御
、速度制御及びトルク制御のために、＾ＩＦ盤８００を
介して更に高レベルの制御ループにフィードバックされ
る。

＾ＩＦ　　−パルス−・舌 ＾ＩＦ盤８００上のパルス幅変調回路装置８０１は、軸
モータ駆動回路装置を介してトルク制御ループ又は電流
制御ループを閉成するためのデジタルインタフェイスを
提供する。パルス幅変調の概念は、ジヨイントモータ電
力スイッチに対する誘導時間幅を制御するために適用さ
れ、これにより、モータ電圧及びトルク命令を満足させ
る。

第９図の一般的ブロック図に示すように、デジタルＰＩ
ＩＩＭ発生器８２５＾は、トルクマイクロプロセッサの
Ｐ２バスを介して９個のビットデータ命令及び３個のの
レジスタアドレスビットを受信する。更に、装置選択論
理、リードライト、リセット（初期設定）及びデータス
トロボ信号は、Ｐ２バスから受信される。ＤＴＡＣＫ　
（承認）信号は、トルクプロセッサ盤上のトルク計算器
から各受信が行なわれた後に、ＰＩＩＭ発生器８２５＾
によりそのバスに戻される。

デジタルＰＷＭ発生器８２５＾は、好ましくは３軸に就
役するように配列され、ここで、例えば、ブラシレス形
又はブラシ形の直流モータの一方が軸駆動装置として用
いられる。従って、１組のデジタル信号（この場合、＾
！、＾２，８１．８２のような４つの信号）は、モータ
がブラシレス形又は直流ブラシモータの場合に、各軸モ
ータと共同して増幅器のベース又はゲート駆動回路装置
を制御するために発生される。

４つのデジタルＰｗＮ制御信号は、オンオフ電力スイッ
チ制御を介してモータ巻線に流れる電流の方向及び大き
さを制御するなめに用いられる。ブラシレス直流モータ
の実施例において、ブラシシス直流モータの三相巻線は
、モータ駆動トルクを生成するために、モータ駆動電流
が常に一対の巻線を介して方向付けられ且っモータ伝導
通路が連続的な巻線対を介して回転又はコンミュテート
されるように、ブリッジ回路（第１０Ａ図及び第１０Ｂ
図）内で相互接続される。この構成において＝ＰＷＭパ
ルスはモータ電流が流れる時間間隔を測定し、ＰＷＭパ
ルス及びホール（■ａｌｌ）効果センサフィードバック
信号に基づくコンミュテート切換論理は、駆動電流が流
れる巻線対及びこの巻線対内の方向を測定する。

Ｈ形式の電力増幅器ブリッジ回路が用いられる直流ブラ
シ形の実施例において、直流ブラシ形モータ８２６＾（
第１１図）は、ＰＷＭ出力出力信号及１Ｂ２の制御の下
で電力増幅器スイッチ８２７＾及び８２８＾が開放され
たときに１つの方向に運転され、ＰｗＭ出力信号Ｂ１及
び＾２の制御の下で電力増幅器スイッチ８２９＾及び８
３０＾が開放されたときに反対方向に運転される。

好ましくは、パルス幅変調回路装置は、一対の大スケー
ル複合パルス幅変調（ＰＩＩＩＭ）チップ内で実施され
る。−最に、各ＰＨＨチップは、直流ブラシ形モータ駆
動装置を有する３軸制御に対してデジタルパルス幅変調
信号を発生するために、マイクロプロセッサ周辺袋？ｌ
ｆ（即ち、制御ループ構成内で更に上位のマイクロプロ
セッサの制御下で）として動作する。

【図面の簡単な説明】

第１図はこの発明の装置により制御されるときに更に進
んで正確な性能を発揮して運転されるロボットを示す斜
視図、第２図はこの発明が実施され得る位置、速度及び
トルク制御ループを用いる制御ループ構成を示す一般的
なブロック図、第３図はこの発明が実施される位置、速
度及びトルク制御ループを用いる制御ループ構成を示す
更に詳細なブロック図、第４図は回路装置がこの発明に
従ってロボット制御及びそのエネルギモニタシステムを
実行するために構成された電子盤を示すブロック図、第
５Δ図はこの発明に従ってジヨイントモータエネルギの
モニタを実施するロボット保護システムを示すブロック
図、第５Ｂ図はこの発明に従ってバックアップ速度のモ
ニタを実施するロボット保護システムを示すブロック図
、第６＾図はエネルギモニタ及び保護システムの動作状
態を示すフローチャート図、第６Ｂ図は速度モニタ及び
保護システムのプログラム動作を示すフローチャート図
、第７八−１図及び第７八−２図はこの発明の実行に用
いられるサーボ制御盤を示すブロック図、第７Ｂ図〜第
７Ｅ図はサーボ制御盤のプログラムを示すフローチャ−
ト図、第８＾−１図及び第８＾−２図はこの発明の実行
に用いられるトルクプロセッサ盤を示すブロック図、第
９図はこの発明の実行に用いられるアームインタフェイ
ス盤を示すブロック図、第１０図はジヨイントモータ制
御信号を発生するためにＡＩＦ盤上で用いられるＰＷＭ
回路を示すブロック図、第１０Ａ図及び第１０Ｂ図は直
流ブラシレス形のジヨイントモータ巻線の構成を示す回
路図、第１１世は直流ブラシ形ジヨイントモータ巻線の
構成を示す回路図である。 ２０・・・ロボット ２１〜２６・・・アームジヨイント回転方向３０・・・
ロボット制御装置 １００．１１８・・・制御ループ構成 １０２・・・アームジヨイントモータ １０４．１２４．１８４・・・トルク制御ループ１０６
．１２０・・・位置制御ループ １０８．１２２・・・速度制御ループ １１６・・・プログラム位置命令 １２６・・・速度命令発生器　１２８・・・位置命令１
４０・・・速度フィードバック通路 １５０・・・位置フィードバック通路 １７４・・・電力増幅器　　　１７８・・・スケーラ４
００・・・サーボ制御（ＳＣＭ）盤 ４０１・・・プロセッサ部分 ４０２・・・サーボ制御管理器（ローカルプロセッサ）
４５０・・・メインプログラム ６００・・・トルクプロセッサ（Ｔ　Ｐ　）Ｐ！Ｍ６０
０Ｂ・・・バックアップ速度システム６０２・・・トル
ク制御管理器（データ管理プロセッサ）６０５＾・・・
フィードバック電流サンプル６０６＾・・・保護ソフト
ウェア ６１０・・・デジタル信号プロセッサ ６２０＾・・・低位モータエネルギ限界８００・・・ア
ームインタフェイス（ＡＩＦ）！８０２・・・駆動回路
装置 ＦＩＧ、４ ＦＩＧ、５Ａ ＦＩＧ、５ｆＳ

Claims (14)

【特許請求の範囲】
1. （１）複数のアームジョイントと、各ロボットアームジ
   ョイントを駆動するための電気モータと、各モータに駆
   動電流を供給するために動作可能な電力増幅器と、所定
   のサンプリングレートで動作可能なデジタルフィードバ
   ック制御ループ手段を有すると共に、デジタル位置命令
   に従ってデジタルトルク制御ループを駆動し、関連する
   電力増幅器を制御するためのデジタルモータ電圧命令を
   発生するデジタル位置及び速度制御ループを含む各ジョ
   イントモータと、各ジョイントモータの動作を検知して
   、前記位置及び速度制御ループ内で制御計算するために
   、対応したデジタル位置信号及び速度フィードバック信
   号を発生する手段と、前記トルク制御ループ内で制御計
   算するために、各ジョイントモータの駆動電流を表わす
   デジタル信号を発生する手段と、モータ温度を検知して
   所定のモータ温度が超過されたときに超過モータ電流を
   示す超過温度信号を発生する手段と、を有するロボット
   のためのデジタル制御装置において、 各ジョイントモータに印加されるエネルギをモータ電流
   から独立的に測定するバックアップエネルギモニタ手段
   と、 バックアップ計算された各ジョイントモータに対するエ
   ネルギをそのモータに対する所定のエネルギ限界と比較
   して、その限界が超過されたときに超過限界信号を発生
   するための手段と、 前記超過温度信号又は超過限界信号が発生されたときに
   ロボットモータの加圧を停止するための手段と、 を備えたことを特徴とするデジタルロボット制御装置。
2. （２）モータエネルギ限界はモータ能力に基づく特許請
   求の範囲第１項記載のデジタルロボット制御装置。
3. （３）モータエネルギ限界はモータ能力限界より小さく
   、所定の使用状態に基づく特許請求の範囲第１項記載の
   デジタルロボット制御装置。
4. （４）使用状態は、教示モード運転に対して特定される
   運転限界である特許請求の範囲第３項記載のデジタルロ
   ボット制御装置。
5. （５）前記バックアップエネルギモニタ手段は、各電流
   フィードバックサンプルの二乗から電力に比例したスカ
   ラを発生し、所定の時間間隔に計算された所定の時間間
   隔平均値に亘って電力スカラ値を平均し、更に平均され
   たスカラをろ過し、出力として各モータに対する前記バ
   ックアップ計算エネルギを発生する特許請求の範囲第１
   項記載のデジタルロボット制御装置。
6. （６）バックアップエネルギ計算において、以下のアル
   ゴリズム、 ｔｅｍｐ＝ＣＵＲ１＾＊＾＊２−ＥＮＥＲＧＹＺＯＥＮ
   ＧＹ＝ＺＯＥＮＧＹ＋ｔｅｍｐ ＥＮＥＲＧＹ＝ＺＯＥＮＧＹ／２＾＊＾＊１６＾＊ＴＡ
   Ｕｉｆ｜ＥＮＥＲＧＹ｜＞ＥＬＩＭＩＴ ｔｈｅｎ　ＥＣＯＤＥ＝ＯＶＥＮＧＹ が用いられる特許請求の範囲第１項記載のデジタルロボ
   ット制御装置。
7. （７）前記各モータトルク制御ループは、 トルク命令及びモータ電流フィードバック信号からモー
   タ電圧命令を発生するためのマイクロプロセッサ手段を
   有するトルクサーボ制御手段と、前記各モータ速度制御
   ループ内に含まれた他のマイクロプロセッサ手段を有し
   、位置及び速度命令、並びに位置及び速度フィードバッ
   ク信号に応答してトルク命令を発生する他のサーボ制御
   手段とを含み、 前記トルクマイクロプロセッサ手段は、前記バックアッ
   プエネルギモニタ手段及び前記比較手段を含む特許請求
   の範囲第１項記載のデジタルロボット制御装置。
8. （８）更に、 各モータに駆動電流を供給するために動作可能な電力増
   幅器と、 所定のサンプリングレートで動作可能なデジタルフィー
   ドバック制御ループ手段を有すると共に、デジタル位置
   命令に従ってデジタルトルク制御ループを駆動し、関連
   する電力増幅器を制御するためのデジタルモータ電圧命
   令を発生するデジタル位置及び速度制御ループを含む各
   ジョイントモータとを備え、 前記動作検知及び信号発生手段は、前記位置及び速度制
   御ループ内の制御計算のために位置及び第１速度フィー
   ドバック信号を発生し、 バックアップ速度モニタ手段は、モータ電流及び端子電
   圧、並びにモータインダクタンス及び抵抗から、各ジョ
   イントモータの速度を独立的に測定し、 各ジョイントモータに対して前記第１及びバックアップ
   速度信号を比較するための手段が設けられたことを特徴
   とする特許請求の範囲第１項記載のデジタルロボット制
   御装置。
9. （９）ジョイントモータに対する第１電圧とバックアッ
   プ電圧との差が所定の限界を超過したときにロボット運
   転を停止するための手段が設けられた特許請求の範囲第
   １項記載のデジタルロボット制御装置。
10. （１０）モータ電流値及びモータ電圧値は、電流制御サ
    イクルの次に先行する所定の時間に計算された平均値で
    ある特許請求の範囲第８項記載のデジタルロボット制御
    装置。
11. （１１）バックアップ速度計算において、以下の式、Ｖ
    ＥＬ＝ｗ＾＊Ｋｅ＝Ｖｉｎ−（Ｌ＾＊ｄｉ／ｄｔ＋ｉＲ
    ）但し、Ｒ＝モータ抵抗　［ｏｈｍ］ Ｌ＝モータインダクタンス　［Ｈ］ Ｖｉｎ＝モータ端子電圧　［Ｖ］ Ｋｅ＝モータ逆起電力定数　［Ｖ／ｒａｄ／秒］ｗ＝モ
    ータ速度　［ｒａｄ／秒］ ｉ＝モータ電流　［Ａ］ が用いられる特許請求の範囲第８項記載のデジタルロボ
    ット制御装置。
12. （１２）各モータトルク制御ループは、 トルク命令及びモータ電流フィードバック信号からモー
    タ電圧命令を発生するためのマイクロプロセッサ手段を
    有するトルクサーボ制御手段と、前記各モータ速度制御
    ループに含まれ、位置命令及び速度命令、並びに、位置
    フィードバック信号及び速度フィードバック信号に応答
    してトルク命令を発生する他のマイクロプロセッサ手段
    を有する他のサーボ制御手段と、 を含む特許請求の範囲第８項記載のデジタルロボット制
    御装置。
13. （１３）前記トルクマイクロプロセッサ手段は前記バッ
    クアップ速度モニタ手段を含み、前記他のマイクロプロ
    セッサ手段は前記比較手段を含む特許請求の範囲第１２
    項記載のデジタルロボット制御装置。
14. （１４）前記他のマイクロプロセッサ手段は、各ジョイ
    ントモータに対する第１電圧とバックアップ電圧との差
    が所定の限界を超過したときに、ロボット運転を停止す
    るための手段を更に含む特許請求の範囲第１３項記載の
    デジタルロボット制御装置。