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

Abstract

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

Description

【発明の詳細な説明】 ［序論］ 本発明はロボット制御装置、特に、サーボ制御ループ及
び連携のフィードバックシステムの構成にマイクロプロ
セッサが利用される電子ロボット制御装置に係わる。

ロボット制御装置は６つのロボット軸を作動する能力を
有するのが普通であり、かつ各軸に位置及び速度制御ル
ービングを設けるのが普通である。本願が開示するよう
な全デジタル式のロボット制御装置の場合、ロボット制
御装置にトルク電流制御ルービングをも組込むことがで
きる。

マイクロプロセッサ技術によるロボット制御の実施に際
しては、デジタル／アナログ混成システムでも全デジタ
ル・システムでも、サーボループ制御の実行に高度の処
理能力が要求される。必要な処理能力は総合的な制御条
件、データ処理条件及び数学的処理条件に基づいて決定
される。

市販のマイクロプロセッサではこれらの能力の必要条件
をすべて満たすことはできない。そこで、従来の方式で
はロボット制御装置に複数のマイクロプロセッサを使用
し、軸ごとの位置、速度及び関連制御タスクをそれぞれ
のマイクロプロセッサに割当てる。このような設計アプ
ローチに伴なう欠点の１つは各軸間の相互作用を考慮に
入れるのが容易でないことにある。また、複数のマイク
ロプロセッサを使用しても、ロボット制御技術の絶えざ
る進歩に伴なって開発が可能なより複雑なアルゴリズム
に対応できる能力には限界がある。

単一のマイクロプロセッサでは多軸ロボット制御の処理
能力条件を満たすことはできないから、改良されたマイ
クロプロセッサ・ベースのロボット制御装置として機能
し、高性能、低コスト及び製造の経済性をもたらす全デ
ジタル式ロボット制御を可能にする新しい多重マイクロ
プロセッサ・システムの開発が要望されて来た。本発明
は一般的には種々の多軸ロボット制御ループの形で実施
できるマルチプロセッサ式サーボ制御装置に係わる。

また、ロボット制御システムの作動に際しては、個々の
軸の位置及び速度パラメータを閉ループ制御下に置く。

マイクロプロセッサを利用することにより、ロボット制
御に必要な制御ループ演算などの機能を行うことができ
る。いかなる場合にも、制御ループが作用するにはモー
タの速度及び軸（モータ）位置信号が゛必要である。

位置フィードバックは絶対量でも増分量でもよく、本発
明は後者を採用する。速度フィードバックはタコメータ
から発生させるか、本発明のように位置フィードバック
から求めることができ、この両方式を併用してもよい。

最も広く採用されている増分位置フィードバックエンコ
ーダは光学技術に基づいている。光学的エンコーダは透
明スリットを有する回転ディスクを含む光学送受信機を
使用するのが普通である。２つの位相信号と１つの指標
信号がエンコーダから発生する。

位相信号は直角符号化（ｑｕａｄｒａｔｕｒｅ　ｅｎｃ
ｏｄ−ｉｎｇ）方式で互いに９０°位置ずれしており、
４つの異なる状態を取ることができる。状態を切換える
ことにより増分的位置変化及び位置変化の方向を検出す
ることができる。

光学ディスクが１回転するごとに指標信号を発生させる
ことにより、２つの位相信号から累算増分位置変化を検
証するための機構が得られる。

デジタル式ロボット制御の実現に際して、エンコーダ位
相信号をマイクロコンピュータによりてアクセスされる
増分カウントに変換するための従来のアプローチに必要
なデジタル回路の量は厖大である。また、位置変化から
の速度の導出は高速の場合には問題なく達成されるが、
低速の場合、従来の変換回路では充分な速度分解能は得
られない。エンコーダの分解能を高めることで速度分解
能をある程度改善できるが、これによって達成される改
善の程度には限界があり、例外なく製品コストの著しい
増大を伴なう。

［発明の要約］ 本発明はそれぞれがモータ駆動手段を有する複数のジョ
イントを有するアームと；各モータに駆動電流を供給す
る電力増幅器と；前記ジョイント・モータ用の前記電力
増幅器をそれぞれ制御するフィードバック制御ループ手
段とより成り、前記フィードバック制御ループ手段のそ
れぞれが連携の電力増幅器を制御する少なくともデジタ
ル位置及び速度制御ループを含み；さらに、すべてのジ
ョイント・モータのための前記制御ループの少なくとも
１つについて制御補助タスク及び計算タスクを行う少な
くとも第１サーボ制御手段とを含むロボットにおいて、
前記第１サーボ制御手段が各ジョイント・モータのため
の前記１つの制御ループについて記憶されているアルゴ
リズムから出力制御指令を演算するなどの計算を行う第
１マイクロプロセッサを含むことと；前記第１マイクロ
プロセッサが比較的高い演算能力及び比較的低いデータ
処理インターフェース能力を有することと；前記第１サ
ーボ制御手段が前記第１サーボ制御手段の動作を統括し
、かつ各ジョイント・モータのための前記１つの制御ル
ープにおけるサーボ制御補助タスク、例えば、前記第１
マイクロプロセッサとの間の制御指令、状態及び帰還デ
ータのルーティングを行うための第２マイクロプロセッ
サを含むことと；前記マイクロプロセッサが比較的高度
のデータ処理能力を有することと；各ジョイント・モー
タのための前記１つの制御ループを前記第１サーボ制御
手段が作動して前記１つの制御ループの制御変数を制御
できるように前記第１及び第２マイクロプロセッサを相
互にかつ高及び低レベル制御回路とインターフェースさ
せる手段を含むことを特徴とするロボットを提案する。

本発明の詳細は添附図面に示す好ましい実施例に関する
以下の説明から明らかになるであろう。

［好ましい実施例の説明］ 三翌工Σ二皇旦 ロボットの性能は、簡単な点間の反復動作から、コンピ
ュータ制御され、−法的な製造システムの一部としてシ
ーケンスされる複雑な動作まで多様である。工場用の場
合、ロボットはダイカスト、スポット溶接、アーク溶接
、インベストメント鋳造、鍛造、プレス加工、吹付は塗
装、プラスチック成形、工作機械装填、熱処理、金属粗
面削り、パレット積み、れんが製造、ガラス製造など、
種々の製造部門における多様な仕事を行うことができる
。ロボット及びその利用法の詳細については、１９８０
年に出版されたＪｏｓｅｐｈ　Ｆ、　Ｅｎｇｅｌｂｅｒ
ｇｅｒ著”Ｒｏｂｏｔｉｃｓ　Ｉｎ　Ｐｒａｃｔｉｃｅ
”を参照されたい。

作用域内で作業を行うため、ロボットはアーム、リスト
小集合体及びエンドエフェクタを具えているのが普通で
ある。ロボット・アームに使用される座標系は多くの場
合、デカルト、円柱、極または回転座標である。一般的
には３つの運動軸を利用することにより、リスト小集合
体を作用域内の任意の場所へ移動させ、別に３つの運動
軸を使用することによりエンドエフェクタを自在に方向
づける。

各運動軸ごとに駆動系を連携させるが、この駆動系は電
動式でも液圧式でも空気圧式でもよい。

ＰＵＭＡロボット 本発明の原理に従って作動することができる多様なロボ
ットのうちの１例として６軸の産業用電動ロボット２０
を第１図に示した。

このロボット２０は本願出願人であるＵｎｉｍａ−ｔｉ
ｏｎ社から商品名ｔｌＮＩＭＡＴＥ　Ｐｔ１ＭＡシリー
ズ７００で販売されている比較的強力な電動ロボットで
ある。Ｍｏｄｅｌ　７　’６１　ＰＵＭＡは有効荷重が
２２ボンド、リーチが５９．１インチ、Ｍｏｄｅｌ　７
６２　ＰＵＭＡは有効荷重が４４ボンド、リーチが４９
．２インチである。

ＰＵＭＡ７００シリーズロボツトは最も苛酷な、最も条
件のきびしい製造環境にあっても長い耐用寿命と最適の
性能が確保されるように、融通性と堅牢さを重視して設
計されている。特定の仕事にどのモデルが好適であるか
を決定するのは顧客が有効荷重を重視するかリーチを重
視するかにかかっている。

リーチの長いＰｔ１ＭＡ７６１はアーク溶接やシーラン
ト注入などのような正確かつ反復的な仕事に好適であり
、他方、ＰＵＭＡ７６２は高精度の材料取扱い、機械装
填、検査、試験、媒体中の接合及び組立や、重い対象物
を扱う部門に好適である。ＰＵＭＡロボットが占めるフ
ロア・スペースは極めて狭いにも拘らず、作業範囲が広
いからロボットは複数の機械や加工面に作用することが
できる。

各軸の運動は増分エンコーダから発生する軸位置フィー
ドバック信号に従ってブラシ付き直流モータによって行
われる。図示のように、リストは３つの関節を具備する
。即ち、矢印２１によって示す上／下回転、矢印２２に
よって示す左／右回転及び矢印２３によって示す第３の
アーム運動をそれぞれ行う。エルボ及びショルダの上／
下方向回転を矢印２４．２５でそれぞれ示した。最後に
、台座２７上での左／右方向アーム回転を矢印２６で示
した。

ロボット制御装置 本発明は第１図のロボット２０や、ブラシなし直流軸モ
ータ及び絶対位置フィードバックを利用する大型のＵｎ
ｉｍａｔｉｏｎ　８８０ロボツトを含むその他のロボッ
トを作動することができるロボット制御装置３０（第２
図）に係わる。ただし、一般的には、ロボット制御装置
３０は単独の、またはネットワーク動作する種類及びサ
イズの異なるロボットに汎用的に応用することができる
。

汎用性であるから、制御装置３０は１つのロボット・シ
リーズを作動するように構成することができる。即ち、
本願の出願人であるＵｎｉｍａｔｉｏｎ社の製造にかか
るすべての液圧駆動または電動ロボット・アームを制御
装置３０によって作動することができる。制御装置３０
をシリーズのすべてのロボットに使用できるということ
、もっと一般的に言えばその汎用性の決め手はモジュー
ル化すると共にアーム従属ハードウェアの使用を極力少
なくし、モジュール制御構造にアーム従属ハードウェア
が使用されることをできるだけ避けることにある。ロボ
ット制御装置３０は頭字語ｔｌＮＩＶＡＬで表わされる
商標で販売されており、低コストですぐれたロボット性
能を発揮するように全デジタル・サーボ制御方式で動作
する。

制御ループ 第２図にはｔｌＮＩＶＡＬロボット制御装置に利用でき
る普遍的な制御ループ構成１００の実施例を示した。各
ロボット・アーム・ジョイントモータ１０２はトルク制
御ループ１０４によって作動される。外側位置制御ルー
プ１０６が速度制御ループ１０８と直列に接続し、速度
制御ループ１０８はトルク制御ループ１０４を駆動する
。フィードフォワード加速制御ループ１１０は加速指令
１１２に応答し、トルク制御ループ１０４の入力にはア
ーム及び負荷慣性１１４も直接供給される。ロボット・
アームは位置制御ループに供給される一連のプログラム
位置指令１１６を介してロボット・プログラムに従い、
制御ループ１００によって作動される。

第３図はＵＮＩＶＡＬロボット制御装置に使用できる好
ましい普遍的制御ループ構成１１８を示す。この構成は
全デジタル式制御装置として実施するのが好ましい。本
願発明または上記した他の特許出願に開示されているよ
うな階層構造、多重プロセッサ構造及び浮動小数点ハー
ドウェアを組込めば、採用するモジュール構成に応じて
３２乃至８ミリ秒のサイクル時間で起動サイクルを特徴
づけることができる。

好ましい制御ループ構成１１８では、位置及び速度制御
ループ１２０，１２２がトルク制御ループ１２４の入力
と並列に接続する。

速度指令はブロック１°２８が受信する位置指令に基づ
きブロック１２６によって形成される。フィードフォワ
ード加速指令は速度指令に基づきブロック１３０によっ
て形成される。フィードフォワード加速制御ループ１３
６において、参照番号１３４で示すように慣性計算値（
負荷及びアーム）１３２が加速指令に乗算される。

速度ループ１２０においては、この実施例の場合、ロボ
ット制御装置のモジュール構成に応じて８乃至３２ミリ
秒ごとに速度指令が発生する。後述する基本的なロボッ
ト制御装置の軌道サイクル時間は３２ミリ秒であり、改
良型の軌道サイクル時間は８ミリ秒である。

いずれの場合にも、速度指令発生器１３８は速度フィー
ドバックバス１４０における速度フィードバックサンプ
リング速度に対応する１ミリ秒に１回の速度で速度指令
を補間する。図示のように、Ｕｎｉｍａｔｉｏｎ８６０
　ロボットのための速度フィードバックはタコメータ信
号をコンバータ１４２によりＡ／Ｄ変換することによフ
て形成される。スケーラ１４４及びフィルタ１４６が組
込まれて速度フィードバック回路が完全なものになる。

同様に、位置制御ループ１２２においてもフィードバッ
クバス１５０における位置フィードバックサンプリング
速度に対応して１ミリ秒毎に１回ずつ、補間回路１４８
が位置指令を発生する。Ｕｎｉｍａｔｉｏｎ　８６０ロ
ボツト制御装置の場合、位置フィードバックは絶対量で
あり、速度及び位置フィードバックバス１４０．１５０
は（スイッチ１５１が図示位置を占める状態で）動作す
る。Ｕｎｉｍａｔｉｏｎ　ＰＩＩＭＡロボットに対して
は、タコメータを使用せず、（スイッチ１５１が他方の
位置へ６行した状態で）ブロック１５２で示すように増
分位置フィードバック、から速度フィードバックが算出
される。位置及び速度フィードバックシステムについて
は第１３乃至１７図との関連で詳しく後述する。

速度誤差はループ１５６によって供給されるゲインに基
づいて加算器１５４が形成する。同様に、位置誤差はボ
ックス１６０によりて供給されるゲインに基づいて加算
器１５８が形成する。速度及び位置の誤差とフィードフ
ォワード加速指令が加算器１６２において加算される。

ゲインがボックス１６６に供給されてトルク指令を形成
し、このトルク指令は１ミリ秒毎に１回ずつトルク制御
ループ１６４の入力に供給される。加算器１６８はトル
ク指令（モータ電流指令）をフィードバックバス１７０
からの電流フィードバックと加算することによってトル
ク誤差を形成する。ボックス！７２がトルク・ループ・
ゲインをトルク誤差に印加し、パルス幅変調（ＰＷＭ）
出力指令（モ・−夕電圧指令）が電力増幅器１７４に供
給され、電力増幅器１７４がロボット・ジョイントを動
作させるためのモータ駆動電流を供給する。２５０マイ
クロ秒ごとに抵抗１７５から電流フィードバックが発生
しく第７Ｂ１図の詳細なブロックダイヤグラムを参照さ
れたい）、ボックス１７８によって供給されるスケーリ
ングに基づき、ボックス１７６によっ゛てデジタル信号
に変換される。

第７Ａｘ−７Ａ３歯には特殊な速度フィードバック処理
を特徴とする特定の［１１ｉｍａｔｌｏｎ８６０ロボッ
トを作動するように構成した本発明の他の実施例として
制御ループ１１８Ａを示した。制御ループ１１８Ａはそ
の構成、動作が第３図の制御ループ１１８に酷似してい
るが、ここでは第３図よりも詳細なブロックダイヤグラ
ムとして図示しである。下に掲げる表は制御ループ１１
８Ａと制御ループ１１８の対応関係を示す： ループ１１８の構　　素　　　　　ループ１１８Ａの対
応　素速度指令発生器１２６、　１２６Ａ　、　１２６
Ｂ位置指令発生器１４８　　　１４８Ａ；１４８Ｂ加速
指令発生器１３０　　　１３０Ａ；１３０Ｂ、１３０Ｃ
；１３００速度指令補間回路１３８　　　１３８Ａ、１
３８Ｂ、１３８Ｃ速度ループ・ゲイン　１５６　　　　
　　　　１５６Ａ；１５８Ｂ位置ドブ補償回路１６０　
　　１６０Ａ　　乃至１６０Ｅル一プ補償回路１６６　
　　　　１６６Ａ　　乃至１６６Ｂ速度フィードバック
スケーラ　１４４　　　　　　　　　ボックス１４４　
Ａ速度フィードバックスケーラは特定のｔｌｎｉ−ｍａ
ｔｉｏｎ８６０ロボツトにおける実際のジョイント・モ
ータ速度と実際のジョイント・モータシャフト速度との
関係（感知速度パラメータ）に存在する非線形性を補償
するように構成されている。このシステムは駆動機構と
して浮動ピボットポイントを有するポールスクリュウを
使用する。ボールスク゛リュウの移動速度は非線形関数
によってモータ速度と関連づけられる。ＪＴ２及びＪＴ
３０ボットはこの機構を採用するロボットであり、サー
ボシステムに供給されるフィードバック速度信号を線形
化するための探索表が設けられている。

！王！上ヨ２１漿ｌ ロボット制御装置３０の制μルービングは複数の電子ボ
ートに設けたデジタル制御回路を利用することによって
実現する。ボード別に回路を組織し、種々のマイクロプ
ロセッサにプログラミングを分担させることにより、モ
ジュール制御構成を利用してロボット制御能力を高める
と共に、製造コストを軽減し、汎用を可能にする構成の
可変性及び制御性能レベル選択の融通性を高めることが
できる。

第４図に示すように、制御ボード構成は制御されるロボ
ット・アームのタイプに応じて異なるが、好ましくはす
べての回路を収容するアーム・インターフェース・ボー
ド８００を含む。例えば、位置フィードバック回路は制
御されるロボット・アームが絶対位置フィードバックを
採用するか増分位置フィードバックを採用するかに従っ
て異なったものとなる。従って、サイズまたはタイプの
異なる種々のロボット・アームに対応できるデジタル制
御システムを得るには、２種類以上のアーム・インター
フェース・ボード８００を使用すればよい。特定のロボ
ット・アームはこのロボット・アームと協働するように
構成されたアーム・インターフェース・ボードの使用を
必要とする。

アーム・インターフェース（ＡＩＦ）ボード８００はま
た、２つ以上のボードと連携するが特に１つのボードと
は連携しない例えばＶＭＥバス制御回路のような包括的
な回路をも収容する。

（パルス幅変調された）制御信号がＡＩＦボード８００
から発生して、ロボット・ジョイント・モータにモータ
電流を供給する電力増幅器ブロック１５０を制御する。

ＡＩＦボード８００はロボット制御装置３０を作業セル
１５２内の他のロボット制御装置、エリア・ネットワー
ク中のプログラム可能制御装置及びその他の入／出力装
置１５３、及び統括制御用の高レベル・コンピュータ１
５４に外部接続するチャンネルをも兼ねる。

トルク・プロセッサ（ＴＰ）ボード６００及びサーボ制
御ボード４００はあらゆる種類のロボットを対象とする
あらゆるロボット制御システムにおいてＡＩＦボード８
００及び電力増幅器ブロック１５０と併用される包括的
な回路ボードである。３つの回路ボード４ｏｏ、ｇｏｏ
及び８００はロボット・アームに対する完全６軸制御を
可能にするから、ＵＮＩＶＡＬタイプ及びその他のロボ
ット制御装置の基本的な制御装置構造を形成する。

トルク・プロセッサ・ボード６００はサーボ制御ボード
４００からの指令に応答してモータトルクを制御する。

サーボ制御ボード４００はロボット制御プログラムに従
ってアームの解を求めると共に、位置及び速度制御を行
う。

追加の電子ボードまたは装置を基本制御装置４００．６
００．８００と接続することによって制御能力及び／ま
たはシステム機能をさらに充実することができる。例え
ば、システム制御ボード５００゛を追加し、サーボ制御
ボード４００からアームの解を求めるなどの所定プログ
ラム機能をこのシステム制御ボード５００に割当てると
、ＵＮＩＶＡＬ制御装置はロボット２０やその他のロボ
ットを、はるかに迅速な制御作用で、即ち、軌道サイク
ル時間を３２ミリ秒から８ミリ秒に短縮して作動するこ
とができる。

制御などを目的とするボード間データ通信ハＶ　Ｍ　Ｅ
バス１５５の多重信号バスを介して行われる。また、ト
ルク・プロセッサ・ボード６００とＡＩＦボード８００
を接続する７ＭＸバス１５６をも設けた。

ＡＩＦ、ＴＰ及びＳＣＭボード及びその他の接続可能な
ユニットに（第４図には示さない）多重ビン・インター
コネクタを設けることにより、ＶＭＥ及びＶＭＸボード
間接続を容易にすると共に、ロボット制御装置３０を構
成するためモジュール方式でボードを組立てる作業を容
易にする。外部入／出力接続用として別のコネクタをＡ
ＩＦボード８００に設ける。

ボード回路構造の詳細は後述する。

本発明のマルチプロセッサ・サーボ制御システム２００
の基本構造を第５図に示した。

多軸ロボット・サーボ制御は２つのマイクロプロセッサ
の協働によって行われる。具体的には、多軸ロボット制
御管理装置２０２がデータ処理機能を一般を行うのに対
して、多軸デジタル信号プロセッサまたは演算装置２０
４は管理プロセッサ２０２に関しスレーブの役割で演算
機能一般を行う。バンク・スイッチ・メモリまたはビン
ボン・メモリ２０６が２つのマイクロプロセッサをイン
ターフェースさせる。入指令及び出指令を含む入／出力
データ２０８は高レベル制御回路２１０から受信される
か、または低レベル制御回路２１２へ送信される。

一般に、従来のフォノ・ノイマン・コンピュータ構成に
基づくマイクロプロセッサは新しい多軸ロボット制御装
置に関連の制御補助及びデータ処理のタスクを行うのに
必要な処理能力を具えている。しかし、フォノ・ノイマ
ン・マイクロプロセッサの数学的処理能力は６軸ロボツ
ト制御装置として単一マイクロプロセッサでサーボ制御
を行うには一般に不充分である。

他方、デジタル信号処理用マイクロプロセッサとして専
用のコンピュータ構成が開発された。このマイクルプロ
セッサは一般に高速固定小数点演算処理能力を有し、こ
の演算処理能力は多軸ロボット制御装置における新しい
サーボ制御アルゴリズムを実施するのに必要な数値計算
を達成して余りある能力である。しかし、信号プロセッ
サは制御補助及びデータ処理能力に劣るのが普通である
。

そこで、本発明の実８に際しては、２つの基本的なマイ
クロプロセッサ・タイプを１組にしてマルチプロセッサ
を構成する。この１組のマイクロプロセッサは協調され
たユニットまたはサーボ・プロセッサ・“エンジン”と
して機能することにより、制御ユニットの製造コストを
軽減しながら、多軸ロボット制御装置がロボット性能を
著しく高めることを可能にする。

一般に、プロセッサ２０２．２０４はマルチプロセッサ
構造内で密接に連携し、信号プロセッサ２０４は管理プ
ロセッサ２０２のスレーブ、すなわち従属周辺装置とし
て動作する。サーボ・エンジンの協調はインターフェー
ス２０６を介して管理プロセッサ２０２におけるソフト
ウェア制御によって行われる。

従って、回路２１０．２１２への上向き及び下向きデー
タ通信は統括コンピュータの制御下に行われる。

好ましくは２つのマイクロプロセッサ間に２つのインタ
ーフェース・バスを設ける。一方のインターフェース・
バスは管理プロセッサ２０２がスレーブ・プロセッサの
実行機能を特定することを可能にし、他方のインターフ
ェース・バスは指令、状態及びデータに関連して、即ち
、特定のスレーブ機能の実行を指令し、この機能の実行
に利用されるデータを提供し、かつ実行の結果として生
じた状態及びデータを返送するのに使用される。

管理プロセッサ２０２としてはＭｏｔｏｒｏｌａ　６８
０００装置を、信号プロセッサ２０４としてはＴｅｘａ
ｓ　Ｉｎｓｔｒｕｍｅ’ｎｔｓ　ＴＭＳ　３２０をそれ
ぞれ採用することができる。マルチプロセッサ・サーボ
制御装置のトルク制御装置実施例に使用されているＭｏ
ｔｏｒｏｌａ　８８０００マイクロプロセッサは１０．
０　Ｍｈｚのシステム・クロック周波数で動作する１６
ビツト・マイクロプロセッサである。

一以　下　余　白− ＴＭＳ３２０プロセッサは速度及び融通性を高めるため
に改良されたＨａｒｖａｒｄ’構造を使用している。本
来のＨａｒｖａｒｄ構造では、プログラムとデータ・メ
モリが２つの別々のスペースを占め、命令取出しと実行
の完全オーバラップを可能にする。これに対してＴＭＳ
　３２０に採用されている改良型Ｈａｒｖａｒｄ構造は
プログラム・スペース及びデータ・スペース間の切換え
を可能にすることで装置の融通性を広げる。

７ＭＳ３２０は他のプロセッサならソフトウェアまたは
マイクロプログラム・ファームウェアが行うのが普通の
機能を行うのに内部ハードウェアを使用するから、高速
固定小数点演算能力を有する。例えば、７ＭＳ３２０は
１６ｘｌ　６ビツト乗算を２００ナノ秒で遂行するハー
ドウェア乗算器を内蔵する。また、ハードウェア・バレ
ル・シフタがＡＬＵヘデータをシフトするから、データ
整列及び乗算を単一の命令で実行することが℃きる。

７ＭＳ３２０は主として個別ロボット制御用に開発され
たものであるから、外部インターフェース能力に限界が
ある。従って、制御管理プロセッサ２０２が同時アクセ
スできるようにＴＭＳプログラム・メモリまたはＩ１０
動作を一時停止させるのが困難である。この問題を克服
するため、特殊なインターフェース回路が必要であり、
ここに述べる本発明実施例の特殊需要を満たすバンク・
スイッチ・メモリ２０６の形態で設ける。

ボード トルク・プロセッサ（ＴＰ）ボード６００はロボット・
ジョイント駆動モータとの機能的インターフェースを可
能にする。機能的には、ＴＰボード６００は階層制御シ
ステムにおける最も低いレベルの制御を実施してロボッ
トの６軸を閉ループ・サーボ・トルク制御する。物理的
には、ＴＰボード６００はロボット・バス・プラニング
制御システム及びサーボ制御（ＳＣＭ）ボードをアーム
・インターフェース（ＡＩＦ）ボード８００と電気的に
インターフェースさせ、前記アーム・インターフェース
（ＡＩＦ）ボード８００はロボット・ジョイント駆動モ
ータとインターフェースする。ＴＰボード８００の主な
機能はＡＩＦボードにおいてパルス幅変調方式で実行さ
れるモータ巻線電圧指令を形成することにより、ロボッ
ト・ジョイント・モータ電流を指令値に調定することに
ある。

ＴＰボード６００は１つのレベルにおいて３０Ｍボード
とインターフェースし、６軸に関して３０Ｍボードから
トルク指令及びサーボ・パラメータを受信し、状態デー
タを返送する。ＴＰボード６６０は２番目に低いレベル
においてＡＩＦボード８００とインターフェースし、ロ
ボット６軸に関するサーボ電圧指令を供給する。ＡＩＦ
ボード８００はＳＣＭ及びＴＰボードにおける閉ループ
制御のための駆動モータ電流、位置及び速度帰還を受信
する。

ＴＰボード６００は対のマイクロプロセッサを利用する
ことにより、以下に列挙するような多くの機能を果たす
。

１、ブラシ付き及びブラシなしモータに対する６軸トル
ク・ループ制御（６軸ごとに２５０マイクロ秒）； ２、ソフトウェア調節可能な電流オフセット−電位差計
を不要にする； ３、ダウンロード可能なゲイン−３ＣＭボードからアー
ム関連パラメータをダウンロードできる； ４、ＰＷＭ補償； ５、演算補償； ６、データ記録などのための電流平均ニア、限流チェッ
ク ８、安全性チェックのための速度モニタリング（逆起電
力） ９、自己診断の強化；及び １０、ダウンロード可能な診断システム・トルク・プロ
セッサ・ボード アームが運動状態にある時、アームの作用点に加わるト
ルクをデジタル制御して指令軌道に従いアーム作用点の
位置を制御することによってロボットの性能をさらに高
める。実際の工作物ローディングに従って軸駆動力を調
節することによって、一段・と高い速度、精度及び能率
で位置及び軌道指令を満たす。

トルク制御はトルク・プロセッサ（ＴＰ）ボードと呼ば
れる包括的な制御回路ボード６００（第４及び６図）、
即ち、負荷能力、駆動方式、軸数などの異なる多様なロ
ボットのトルク制御に利用できる電子ボードにおいて行
われる。

トルク・プロセッサ・ボード６００は高制御レベル（５
０Ｍボード）から得られるトルク指令及びアーム・イン
ターフェース（ＡＩＦ）ボード８００を介して軸駆動手
段から得られるフィードバック電流に基づいて各ジョイ
ント・モータまたは各軸駆動手段ごとに、デジタル回路
を利用して電圧指令を形成する。従って、すべてのジョ
イント・モータと連携するトルク制御ループはＴＰボー
ド回路を介して閉成される。

電動駆動手段の場合、フィードバック電流は実際のモー
タトルクに比例するモータ巻線電流である。液圧式駆動
手段の場合、フィードバック信号も実際のモータトルク
に比例する。

好ましくは、デジタル・トルク制御回路を多重デジタル
・プロセッサで構成することにより、サンプリング周波
数条件の範囲内で、必要な制御演算及び制御補助機能を
すべての軸について正確かつ能率的に達成できるように
する。

具体的には、トルク制御管理プロセッサ６０２がデュア
ル・ボートＳＣＭインターフェース・メモリ６０４とイ
ンターフェースして、ＳＣＭ（サーボ制御モジュール）
とＴＰ（トルク・プロセッサ）制御レベル間で記憶され
ているトルク制御データの交換が行われるようにする。

軸トルク指令及び制御ループ・パラメータがＳ、ＣＭか
らＴＰゼインーフェース・メモリ６０４へ、好ましくは
ＶＭＥタイプのデータ・バス６０６を介してダウンロー
ドされる。逆に、状態データがサーボ制御レベ（Ｓ　Ｃ
Ｍ）へ゛アップロードされる。ＴＰ及びＳＣＭボード間
のメモリ・インターフェース６０４はＶＭＥバス６０６
（７）スレーブ、すなわち従属手段として作用するデュ
アル・ボート共用メモリ形である。その他のボード・メ
モリとしては、ピンポン・メモリ６０８、プログラムＥ
ＦＲＯＭ　、ローカルＲＡＭ　、及びＴＰ演算メモリな
どがある。

トルク制御管理プロセッサ６０２はまた、次に低い制御
レベルのＡＩＦボード６ｏｏへ電流フィードバックの流
れを向けることによってトルク制御ループを動作させる
。トルク制御演算から得られた駆動電圧指令がトルク制
御管理プロセッサ６０２によってアーム・インターフェ
ース（Ａ　Ｉ　Ｆ）ボード６００に伝送される。ビンボ
ン（パンク・スイッチ）メモリ６０８は初期手順フラッ
グの制御下に指令、フィードバック、及び状態データを
記憶することにより、トルク制御演算、高制御レベル・
リポートまたは軸駆動制御に必要な時、利用できるよう
にする。

デジタル信号プロセッサの形態で設けられるプロセッサ
６１０はビンボン・メモリ６０８を介してトルク制御管
理プロセッサ６０２からトルク指令及びフィードバック
電流を受信し、トルク指令及びフィードバック電流から
算出されるトルク誤差に基づいてそれぞれのロボット軸
に関して駆動電圧指令を演算し、トルク制御管理プロセ
ッサ６０２からの指令に基づきビンボン・メモリ６０８
を介してアーム・インターフェース回路へ駆動電圧指令
を転送するトルク・ループ演算回路として動作する。

以上に述べたデジタル回路構造を採用すれば、必要なす
べてのトルク制御機能を周波数応答条件の範囲内で迅速
に（２５０マイクロ秒以上のサンプリング速度）かつ正
確に行うことができる。具体的には、トルク制御管理プ
ロセッサ６０２のデータ編成及び指示転送能力を他の大
部分の機能に利用しながら、デジタル信号プロセッサ６
１０の迅速な演算能力をトルク制御演算に利用すること
により、著しく改善された制御性能を能率的かつ経済的
に達成すること示セきる。

トルク制御管理プロセッサ６０２はデータ管理プログラ
ムに関する上記タスクには好適な構造を具えているが、
その演算速度（即ち、１６ｘｌ　６ビツト乗算に４マイ
クロ秒以上の時間を要する演算速度）はトルク制御帯域
幅条件を満たすには低過ぎる。デジタル信号プロセッサ
６１０はＺ変換演算に適合させた（即ち、１６ｘｌ　６
ビツト乗算の演算速度が２００ナノ秒の）構造を具えて
いるものの、その他の点ではデータ管理プロセッサ６０
２に割当られた各種タスクには概して不適当である。こ
の２つのマイクロプロセッサが１つのユニットとして、
還元すれば、サーボ・エンジンとして機能する。

トルク制御プログラミング トルク・プロセッサ・ボード６００はボード・プロセッ
サ６０２．６１０において実行されるプログラムの制御
下に動作して高ＳＣＭ制御レベルからのトルク指令を実
行する。

トルク・プロセッサ・ソフトウェアはそれぞれに割当ら
れた下記タスクを行うのが普通である。

トルク匍御管　プロセッサ６０２ ＳＣＭとの通信 指令処理 電流サンプリング、変換及びオフセット調節コミュテー
ション・スイッチ・フラッグ（状態読取） ＰＷＭチップ管理 診断 誤差リポート トルク・ループ演　回路５ｔＯ （プログラム・サイクルは２５０マイクロ秒割込み） 過電流チェック−絶対量及び平均量 トルク・ループ演算 電流平均 ＰＷＭ補償 コミュテーション補償 逆起電力チェック−安全性のための速度モニタリング エネルギー・チェック−停動状態の試験ＴＰ管理プロセ
ッサ 具体的には、トルク制御管理プロセッサ６０２はその名
称が示すように、トルク・プロセッサ・ボード動作の多
くを管理する。トルク・ループ演算回路６１０は演算さ
れた出力がトルク制御管理プロセッサ６０２の管理下に
特定のボード動作が行われることを可能にするという意
味でボード動作に作用する。

トルク制御管理プロセッサ６０２のためのプログラミン
グとしては、本発明の実施に際して使用される応用プロ
グラムの操作を助ける種々の公知システム準備プログラ
ムがある。応用プログラミングはＴＣＭ　（トルク制御
管理）プログラム・メモリと呼ばれるＥＰＲＯＭメモリ
６１４に記憶されている。オペレーティング・データは
ＴＣ（トルク制御）データ・ベース・メモリと呼ばれる
ローカルＲＡＭメモリ６１６に記憶される。

第８Ｂ図に示すように、トルク制御管理プログラムはメ
イン・プログラム６１８及び割込みプログラム６２０を
含む。一般に、メイン・プログラム６１８はＴＰボード
動作を起動させるため、アーム・インターフェース及び
トルク・プロセッサ制御レベルに必要な初期値設定プロ
グラム６２８を提供するリセット・サブプログラム６２
２を含む。リセット・サブプログラム６２２はまた、Ｔ
Ｐボード６００が動作指令に応答可能な状態になるとこ
れを高ＳＣＭ制御レベルに通報する。

次いで、待機指令サブプログラム６２４が高ＳＣＭ制御
レベルからの診断その他の高優先順位ノン・モーション
の指令の実行を管理する。モーション制御中、周期的に
、好ましくは１ミリ秒毎に１回ずつトルク指令が発生し
、トルク管理割込みプログラム６２０（第８Ａ１乃至８
Ａ４図）の制御下に、ビンボン・メモリ６０８を介して
トルク・ループ演算回路６１０（第６図）に転送される
。割込みプログラム６２０は低制御レベルＡＩＦボード
８００に設けられた割込みクロックにより（好ましくは
２５０マイクロ秒に１回の）サンプリング速度で形成さ
れる割込み信号によって周期的に作動される。割込みク
ロックはまた、１ミリ秒毎に１回ずつＳＣＭ）−ルク指
令を発生させるよにタイミング制御する。

割込みプログラム６２０はトルク・ループ演算回路９１
０が各別込みサイクル中にロボット・アームの６軸のそ
れぞれについてトルク演算を行う際にトルク・ループ演
算回路６１０の動作を補助するのに必要な種々の管理機
能を行う。トルク・ループ演算から形成された電圧指令
はビンボン・メモリ６０８からＴＣデータ・ベース・メ
モリ６１６に、次いで、好ましくはＶＭＸタイプのデー
タ・バス６０フ及びバス・インターフェースを介してＡ
ＩＦボード回路に送られ、軸駆動手段によりパルス幅変
調信号に変換される。電流及び状態フィードバックデー
タもＶＭＸバス及びインターフェース６０７からＴＣデ
ータ・ベース・メモリ６１６に転送されたのち、ビンボ
ン・メモリ６０Ｂを介してトルク演算回路６１０に送ら
れる。

一以　下　余　白− バス６０７は特定の実施例の場合、タイミングと信号ビ
ン番号との連携に関するＶＭＸ仕様に従って動作する。

しかし、アービトレーション、多重バス・マスター及び
２４ビツト・アドレス能力に関するＶＭＸバス仕様すべ
てに従うわけではなく、ＶＭＸバス仕様に規定されてい
ない信号も制御、割込み及び誤差状態情報に利用される
。

再び第８Ｂ図から明らかなように、リセット及び待機指
令サブプログラム６２２．６２４及び割込みプログラム
６２０によりて種々のサブルーチン６２６が利用される
。リセット・サブプログラム６２２は初期値設定サブル
ーチン６２８をも利用する。

第８Ｅ図に示すように、ユーティリティ・プログラム６
２６はそれぞれ指示しである機能を有する下記のサブル
ーチンを含む。

Ｃ）ＩＥｃＫＲＡＭ−所定メモリの診断−ボード６００
のすべてのＲＡＭメモリをチェックする。

ＲＡＭＴＥＳＴ−所定め記憶場所にビット・テストを適
用する。

ＧＥＴＡＤ−メモリへのアナログ入力転送を可能にする
。

ＡＤ−５ＥＴ−フィードバックチャンネルのＡ／Ｄコン
バータをセットする。

Ｃ０ＰＹＩＮ−６つのアーム・ジョイントに対応の規定
パラメータをトルク制御データＳ０Ｍインターフェース
・メモリ６０４からＴＣデータ・ベース・メモリ６１６
ヘコピーする。

Ｃ０ＰＹＯＵＴ−６つのアーム・ジョイントに対応の規
定パラメータをＴＣデータ・ベース・メモリ６１６から
トルク制御データＳ０Ｍインターフェース・メモリ６０
４ヘコピーする。

ＣＯＰＹＴＭＳ／Ｓ−（：ＯＰＹＴＭＳ　−Ｔ　Ｃデー
タ・ベース・メモリ６１６及びビンボン・メモリ６０８
との間でコピーを行う。

ＣＯＰＹＧＭＳ　−Ｔ　Ｃデータ・ベース・メモリ６１
６からビンボン・メモリ６０８へ指令をコピーする。

ＣＬＲ５ＥＲＶＯ−Ｔ　Ｃデータ・ベース・メモリ６１
６及びビンボン・メモリ８０ａ中のオンライン・トルク
・ループ演算データをクリアする。

ＰＷＭＳＷ−アーム・インターフェース制御レベルに位
置し、アーム駆動手段のための電力増幅器指令を発生す
るＰＷＭチップのためのスイッチをオン／オフする。

ＰＷＭＣＬＥＡＲ−Ｐ　Ｗ　Ｍチップへの出力指令をゼ
ロにする。

５ＶＥＮＡＢＬＥ−実行可能な最大電圧指令を規定する
。

５ＶＤＩＳＡＢＬＥ　−Ｐ　Ｗ　Ｍをゼロ出力に保持す
る。

ＬＯＡＤＴＭＳ　−）−ルク・ループ演算回路６１０の
ＲＡＭメモリ６１１にプログラムをダウンロードする。

ＡＣＴＴＭＳ　−トルク・ループ演算回路ｓｉｏを作動
／停止させる。

ＧＩＶＰＭ　−Ｐ　Ｗ　Ｍチップに電圧指令を供給する
。

ＣＯＭＭＣＨＥＣＫ−６つの駆動手段すべてについて位
置フィードバックデータを受信し、直ぐ先行のデータと
異なる場合にはそれぞれのフラッグをセットする。

Ｃ０ＭＭ５ＴＯＲＥ　−６つのジョイントモータすべて
についてコミュテーション状態を記憶する。

ＧＯ−トルク・ループ演算回路６１０の制御レジスタ６
３０（第６Ａ図）にＧＯ信号を送信する。

ＢＳＷＩＴＣＨ−ビンボン・メモリ６０８に対する指令
をスイッチする。

ＥＲＲ−高ＳＣＭ制御レベルに誤差をリポートする。

ＤＩ−割込みプログラム６２０の実行を不能化する。

ＥＩ−割込みプログラム６２０の実行を可能化する。

リセット・サブプログラムに利用される初期値設定サブ
ルーチン６２８は第８Ｃ図に示した通りであり、下記の
ものを含む。

ＤＡＴＤＩＲ−Ｔ　Ｃデータ・ベース・メモリ６１６中
にＳＣＭインターフェース・メモリ６０４のためのデー
タ・ディレクトリを作成する。

ＳＴＲＰＷＭ　−Ｔ　Ｃデータ・ベース・メモリ６１６
中にＰＷＭＩ１０アドレスを記憶させる。

ＴＣＡＬＣＤＢ−トルク・ループ演算回路６１０の内部
ＲＡＭメモリ中のトルク・ループ演算回路データ・ベー
スをクリアし、初期値設定する。

ＴＣＯＮＴＤＢ−Ｔ　Ｃデータ・ベース・メモリ６１６
中のデータ・ベース・パラメータをクリアする。

メイン・プログラム６１８の詳細を第８Ｄに示した。ト
ルク・プロセッサ・ボード６００が起動すると、トルク
制御管理プロセッサ６０２がメイン・プログラム６１８
に進み、リセット・サブプログラム６２２を実行するこ
とによって初期値設定を行う。その結果、特定のロボッ
ト用途において一括パッケージすることのできる高制御
レベルとの通信のため、デュアル・ボートＳＣＭインタ
ーフェースを介してＶＭＥデータ・バス６０６にトルク
制御管理プロセッサ６０２の識別コードが書込まれる。

さらに、すべてのセマフォが同じ無効状態にセットされ
、トルク・ループ演算回路６１０がクリアされ、リセッ
トされる。局部入／出力バードウェア・ラッチ６０７は
トルク・ループ演算回路６１０に対するハードウェアま
たはソフトウェア制御セレクタとして動作し、ソフトウ
ェア制御フラッグがセットされるとトルク制御管理プロ
セッサ６０２からの制御が可能になる。

実行の最終段階において、リセット・サブプログラム６
２２がユーティリティ・サブルーチンＰＷＭＥＮをコー
ルし、次に低い制御レベル（アーム・インターフェース
φレベル）のための信号が発生してアーム駆動回路をク
リアする、即ち、ＰＷＭチップと呼ばれる制御デジタル
装置をＯＦＦ状態に切換える。

メイン・プログラム６１８はブロック６３０のユーティ
リティ・サブルーチンＣ）ＩＥｃＫＲＡＭをコールする
ことによ°つて開始手順を継続してＴＰボード６００に
おけるすべてのＲＡＭメモリに対するメモリ診断を行う
。次いで、参照番号６３２で示すように初期値設定サブ
ルーチンＴ（：ＡＬＣＤＢをコールすることにより、Ｔ
Ｃデータ・ベース・メモリ６１６中のトルク・ループ演
算回路データ・ベースをクリア／初期値設定を行う。

次いで、参照番号６３４に示すようにサブルーチン八Ｃ
ＴＴＭＳによりトルク・ループ演算回路６１０をＯＮ状
態にする。次に、ブロック６３６においてユーティリテ
ィ・サブルーチンＣＯＰＹＴＭＳをコールすることによ
り、ＴＣデータ・ベース・メモリ６１６及びビンボン・
メモリ６０８との間のデータ・コピーを不能にする。さ
らに、ユーティリティ・サブルーチンＬＯＡＤＴＭＳを
コールして、参照番号６３８で示すように、７ＭＳプロ
グラミングをメモリ６１１ヘダウンロードする。

ブロック６４０において、再びユーティリティ・サブル
ーチンＣＯＰＹＴＭＳを実行することにより、ＴＣデー
タ・ベース・メモリ６１６及びピンポン・メモリ６０８
との間のコピーを可能にする。次いで、次に高いＳＣＭ
制御レベルに関して、ＴＰボード６００がロボツテ制御
動作をできる状態にあることを示す信号がブロック６４
４によって形成される。

次の能動的ロボット制御動作中に、メイン・プログラム
６１８は高ＳＣＭ制御レベルから発生する特定の高優先
順位指令を待機しながら連続ルービング操作を受ける待
機指令モードに進む。即ち、ブロック６４４は特定の指
令がＴＰボード６００に送信されているかどうかを判定
する。指令が検出されるまでブロック６４４においてル
ービングが続き、検出されると、待機指令サブプログラ
ム６２４がコールされる。実行後、次のＳＣＭ指令が受
信されるまで再び指令検出ループに入る。ブロック６４
４において、高ＳＣＭ制御レベルが特定指令をＴＰボー
ド６００にダウンロードしたことを意味するフラッグが
セットされているかどうか、セマフォ３を観察して判断
することにより、Ｓ０Ｍ指令検出が行われる。　第８Ｆ
図に示すように、待機指令サブプログラム６２４はシス
テムがロボット・アーム動作を制御できる状態にあるか
どうか、即ち、高ＳＣＭ制御レベルがブロック６４６に
示すようにマスター・サーボ可能化信号を発生したかど
うかを先ず判定する。マスター・サーボ可能化モードに
おいて、３０Ｍ制御装置はＴＰボード６００が実行すべ
き一連のトルク指令信号を発生する。好ましくは、ＡＩ
Ｆ制御レベルからのクロック制御下に１ミリ秒毎に１回
ずつトルク指令が発生する。

マスター・サーボ可能化信号が発生して優先モーション
制御を行うと、ブロック６４８は高ＳＣＭ制御レベルに
対して、指令が受信され、応答が得られたことを確認す
るセマフォ３フラツグ・ダウンをセットする。次いで待
機指令サブプログラムはロボット・アーム・モーション
制御が実施されるのに伴なって３０Ｍ指令検出ループに
戻る。

逆に、マスター・サーボが不能化されると、先ずブロッ
ク６５０がＳＣＭ指令準備ビットをチェックし、次いで
ブロック６５２が割込みプログラムの実行を不能化する
。なぜなら、優先順位がモーション指令よりも高い指令
を確認し、実行しなければならないからである。次いで
ブロック６５４がサブルーチンＰＷＭＣＬＥＡＲをコー
ルすることによって出力電圧指令をゼロにセットする。

次に、割込みプログラムが可能化されて再び割込み制御
に基づくアーム・モーション制御が行われる。

ブロック６５８．６６０において、診断指令またはパラ
メータ変更指令がダウンロードされたかどうかの判定が
行われ、サブルーチン（：０ＰＹＩＮにより３０Ｍ指令
がＴＰデータ・ベース・メモリ６１６ヘコピーされ、実
行される。ブロック６６２がユーティリティ・サブルー
チン００１！ＹＣＭＤを実行することにより、指令をピ
ンポン・メモリ６０８のバンク１に転送する。次いで、
ブロック６６４がＢＳＷＩＴＣＨを実行することにより
、ブロック６６６が３０Ｍ指令をビンボン・メモリ６０
８のバンク２へ転送することを可能にする。次に、トル
ク・ループ演算回路ａｔＯが３０Ｍ指令を実行し、ブロ
ック６６８はダウンロードされた３０Ｍ指令が実行され
たことを示すＳ０Ｍ制御レベルのための信号を発生する
。次いで、ブロック６４８がセマフォ３ダウン・フラッ
グをセットし、上述のようにプログラム実行が続く。

割込みプログラム６２０の詳細なフローチャートを第８
Ａ１乃至８Ａ４図に示した。ＡＩＦボード８００からの
割込みクロック信号が待機指令サブプログラム６２４に
おいて可能化されると、第８Ａ１図のブロック６７０に
示すように開始される。次いで、制御ループ・サンプリ
ング速度、即ち、２５０マイクロ秒に１回の割込み速度
で実行される。

第８Ａｉ図において、ブロック６）２は割込みを不能化
し、ブロック６７３はマスター・サーボが可能化されて
ロボット制御が可能である瞭どうかを判定する。マスタ
ー・サーボが可能化されているなら、５Ｖ−ＥＮＡＢＬ
Ｅ、　ＡＤＳＥＴ、　ＣＯＭＭＣＨＥＣＫ及びＣ０ＭＭ
５ＴＯＲＥユーテイリテイがブロック６７４−６７７に
おいて実行される。

マスター・サーボが可能化されていなければ、即ち、ロ
ボット制御が不能なら、ブロック６７８がＰＷＭＳＭサ
ブルーチンを実行することによってＡＩＦボード８００
のＰＷＭモータ制御チップを不能化し、ブロック６７９
．６８０がユーティリティＰＷＭＣＬＲ及びＣＬＲ５Ｖ
をそれぞれ実行する。

次いで、終端割込みブランチ６８１に進み、ブロック６
８２がシャットダウン・エラーをチェックする。シャッ
トダウン・エラーが存在するならブロック６８３がロボ
ット停止を実行する。（２５０マイクロ秒ごとに発生す
る）割込みクロック・チックがカレント・ミリ秒共通サ
イクル時間で３カウントに達すると、ブロック６８５が
カウンタを０にリセットし、プログラムが終結する。も
しカウント３に達しなければ、ブロック６８６がカウン
タを増分し、ブロック６８７がモータコミュテーション
状態を記憶し、プログラムが終結する。

マスター・サーボが可能化されている場合を示す第８Ａ
２図において、ユーティリティ・プログラム６７４−６
７７が実行されたのち、ブロック６８８．６８９がチッ
ク・カウント０及び３をそれぞれチェックする。０も３
も存在しなければ、ブロック６９０が各ロボット軸から
の電流フィードバックを得る。もしチック・カウントが
０なら、ブランチ６９１が新しいトルク指令を得る（ボ
ックス６９２）か、この新しいトルク指令がなければ、
先行のトルク指令を利用する（ボックス６９３）。チッ
ク・カウント３において、逆起電力指令がＴＭＳにセッ
トされ（ボックス６９４）、エネルギー演算出力が得ら
れ（ボックス６９５）、ビンボン・メモリ・スイッチが
セットされる（ボックス６９４Ａ）。

電流フィードバックを取出し、各チック後に行われるそ
の他の機能を実行するためのルーチンＩＦＢは６軸すべ
てが完了するまでループされる。第８Ａ４に示すように
、ビンボン・メモリ・スイッチＢＳＷＩＴＣＨはブロッ
クＦ１２２においてセットされ、ＰＷＭ電圧指令はボッ
クスＦ１３４によって出力され、　ＢＡ（：ＫＥＭＦは
ボックスＦ１２Ｂによって出力される。

リミット・チェックはＦ！３０及びＦ！３８において行
われる。次いで、プログラムはＦ７６を通ってＦ８，０
に分岐させられる。

第８Ａ３図において１．Ｆ　８２でＴＩＣＫ・２なら、
ボックスＦ８４が平均電流をビンボン・メモリヘコピー
し、ボックス７０１がエネルギー・チェック指令をセッ
トする。ＴＭＳ動作がＦ１ａを介して行゛われたのち、
ボックス７０２がビンボン・メモリ・スイッチをセット
し、ポックスフ０３がＰＷＭ電圧指令を出力する。

ブロック７０４が再びチック・カウンタをチェックし、
カウントがそれまでと異なり２であれば、ブロック７０
０Ａはビンボン・メモリへ平均電流をコピーする。゛次
いで、フラッグがセットされ、チック・カウントがブロ
ックＦ１０２において３であると識別されると、ブロッ
ク７０５においてなんらかのデータがコピーアウトされ
る。最後に、終端割込みブランチ６８１に進み、第８Ａ
１図の６８２−６８７に示す命令を利用して割込みプロ
グラム６２０の実行を終結させる。

１１星ｍ ＴＰ演算回路６１０のためのプログラミングはサンプリ
ング速度で周期的に実行されるメイン・プログラムを含
む。メイン・プログラムは演算回路（ＴＭＳ）リセット
、及び演算回路（ＴＭＳ）ＲＡＭチェック、クリア及び
初期値設定、及びこれに続く、トルク管理プロセッサ６
０２からのトルク指令待機を行う。

トルク指令の可用性が指示されると、６つのジョイント
・モータすべてについてトルク・ループ制御アルゴリズ
ムが実行される。

次いで、エネルギー演算がなされ、プログラム実行前に
逆起電力が演算される。

札二二土ヱユ１亘 ロボット制御装置全体のうちのトルク制御ループ部分の
詳細なダイヤグラムを第７Ａ３図に示した。下記の制御
アルゴリズムはロボット制御ループ構成全体のうちのト
ルク制御ループ部分の実施に際してトルク、ループ演算
回路６１０において実行される。

ＰＩ　　　アルゴリズム このアルゴリズムを２５０マイクロ秒サンプル周期の場
合に関連して述べる。１）入カニ ＳＣＭからのトルク指令ＴＣ（１ｍｓ。

更新速度） Ａ／Ｄコンバータからのフィードバッ クトルク Ｔ（２５０マイクロ秒更新速度） Ｔｃ、Ｔは次のようにスケールされ る；符号ビットで左詰めされた１６ビ ツト量で記憶される１０個の有効ビッ ト。この型式を選択するのはＡ／Ｄイ ンターフェーシングを簡略化するため である。

５ＤＤＤ　　ＤＤＤＤ　　ＤＤＸＸ　　ＸＸＸＸ出カニ ＰＷＭ指令レジスタへのＰＷＭ指令Ｐ ＷＭｏｕｔ　（２５０マイクロ秒更新速度） ２）原理： Ｔｅｒｒ　＝　Ｔｃ　−Ｔ にｐ：トルク・ループ比例ゲイ ン ｔａｕ　：積分の時定数（モータの 機械的時定数） ３）アルゴリズム ／＊誤差を得る申／ Ｔ　（ｎ）・（Ａ／Ｄからの入力データ）／牟すイズを
正規化する申／ Ｔｅｒｒ−（Ｔｃ　（ｎ）２”１Ｏ−Ｔ（ｎ）２″″１
０）／２”１Ｂ ／＊次の積分を計算する中７ ｔｅｍｐ　（ｎ）＝ｉｎｔｅｇ＋ＫＩ”Ｔｅｒｒ／申積
分限異積分限界クする＊／ もしくｔｅｍｐ＜・−１ｎｔｌａ＋ｔ’）ならｔｅｍｐ
＝−ｉｎｔｌｍｔ もしくｉｎｔｌｍｔ＜−ｔａｍｐ）ならｔｅｍｐ−ｉｎ
ｔ１ｍｔ／＊次の時間の積分値を記憶する申／ ｉｎｔｅｇ−ｔｅｍｐ ／傘ＰＩの結果を得るＩ／ ｌｅｍｐ−ｉｎｔｅｇ／２°１６＋にＰ’Ｔｅｒｒ／Ｉ
あふれをチェックする＊／ もしくｔｅｍｐ＞７８ＦＦ）ならｔｅｍｐ−７８ＦＦも
しくｔｅｍｐ＞８６８０）ならｔｅ、ｍｐ−８６８０／
＊ＰＷＭを線形化する＊＊／ もしくｔｅｍｐ＞−２００）なら ＰＷＭｏｕｔ＝ｔｅｍｐ＋７００ もしく２００＞ｔｅｍｐ＞ｉ）なら ＰＷＭｏｕｔ＝ｔｅｍｐ”２＋５００ もしく０＞ｔｅａ＋ｐ＞−ＦＤＧＯ）ならＰＷＭｏｕｔ
＝ｔｅｍｐ’２＋ＦＢ８０もしくＦＤＧＯ＞ｔｅｍｐ）
なら ＰＷＭｏｕｔ＝ｔｅｍｐ＋Ｆ９８０ Ｋｌ　−（にＰ／１ａｕ）　”Ｔｓ Ｔｓ−サンプル周期−２５０マイクロセコンド４）ＫＰ
及び’Ｋ　Ｉのスケーリング Ｉ）次元　　にｐ　：　［Ｖ／Ｎ−Ｍｌｔａｕ、　ＴＳ
　：　［ｓｅｃ］ トルク　　　電流　　Ｔｅｒｒ’２”６　　ＰＷＭｏｕ
ｔ　　　モータボルトＮ−Ｍ　　　　　　Ａ　　　　　
　　ビット　　　　　　ビット　　　　　　ボルト＞−
−−Ｋｔ−−−−Ｋａｄ−−−−−にＰ”２”−１６一
−Ｋｐｗｍ−−−−−−＞く曹曽讃−曽−−讃一一一一
一睡−にＩ）ｓｍ−曽−−蒙−−−一口會曽會・−ｍ＠
ｆｉａ）Ｋｔ　　トルク−電流ゲイン　［Ａ／Ｎ−Ｍｌ
ＫａｄＡ／Ｄコンバータ・ゲイン　［ビット／Ａ］Ｋｐ
ｗｍＰＷＭ　ゲイ　ン　　　　　　　　［Ｖ／ピッ目ｉ
ｉ）方程式 ％式％ ： １、Ａ／Ｄコンバータのフル・スケールが”／−Ｉｍ６
ｘなら Ｋａｄ−２°１５／Ｉｍａｘ　［ビット／Ａ１２、ＰＷ
Ｍチップ及びパワー・ブロックのゲインが＋／−Ｖｍａ
ｘならＫｐｗｍ−Ｖｍａｘ／２”１５［Ｖ／ビット］　
、　従っ　て、　にａｄ”Ｋｐｗｍ−Ｖｍａｘ／Ｉｍａ
ｘ　［Ｖ／Ａ］ ゛起電　アルゴリズム 逆起電力ＥＭＦ演算を利用して安全上のモータ速度を算
定する。

１）人カニ Ａ／Ｄコンバータからのモータ電流ｉ ＰＷＭ指令レジスタからのモータ電圧 ｉｎ 出カニ 逆起電力　　ＶＥＬ算定速度 ２）原理： ＶＥＬ−Ｗ”Ｋｅ−Ｖｉｎ−（Ｌ”ｄｉ／ｄｔ＋１Ｒ）
Ｒ−モータ抵抗　　［オーム］ Ｌ＋モータインダクタンス　［Ｈ］ Ｖｉｎ　＝モータ端子電圧　　　［Ｖ］にｅ＝モータ逆
起電力定数［Ｖ／ラド／秒］Ｗ−モータ速度　　［ラド
／秒］ｉ ＝モータ電流　　［Ａ］ ３）アルゴリズム： ＶＥＬ　（ｎ）　・［ＰＷＭｏｕｔ”２°９−ＭｏＴＬ
”　ｉ　（ｎ）　−ｉ　（ｎ−１）−ｓ　（ｎ）　１ｘ
ｏＴｎ］／２１１６ただし、 １　（ｎ）は６ビツト右詰めされ、１ミリ秒／　Ｔ　ｓ
回累積されたＡ／Ｄ値（１ミリ秒累算の平均値）。

ＴｓはＰＷＭサンプル速度；２５ｏマイクロ秒 ４）Ｌ、Ｒのスケーリング： ｉ）方程式： ％式％（） ただし、Ｋｒｐｍ・（ＳＶ／ＫＥ）　傘ＶＥＬｉｉ）例
： Ｖｍａｘｓ３００ｖ、　　Ｉｍａｘ−２０Ａ、　　Ｒ−
０，４９オー入。

Ｌ−５，５ｍｈ ＫＥ−１２０Ｖ／Ｋｒｐｍ、　　ｄｅｌｔ・４ミリセコ
ンドＭＯＴＲ−２，８８，ＭＯＴＬ鍵７５１Ｋｒｐｍ＝
９．７６″１０”−３”ＶＥＬＶｉｎ　　ＰＷＭｏｕｔ
　　　　　ＶＥＬ　　　　　ＶＥＬｅｍｆＶ　　　　　
　ビット　　　　　　　　　ビット　　　　　　　　　
　ビット−−−Ｋｐｗｍ−一−−−−２”−９−＋−−
−−２”−１６”５Ｖ−−１／にＥ−一−１＋１ＯＴＬ
−１ エネルギー・チェック・アルゴリズム エネルギー・チェックはモータエネルギーを演算し、エ
ネルギーが規定時間に互って規定限界を超えるとこれを
ＳＣＦにアラームする。

１）入カニ １ミリ秒平均電流ＣＵＲＩ ＳＣＭからのエネルギー限界ＥＬＩＭＩＴ（一定） ＳＣＭからの時定数　ＴＡｔｌ　（一定）出カニ ＳＣＭへのアラーム　ＥＣ０ＤＥ ２）原理： ｌ＊ｉ　　　　　　　　　エネルギー １−−−−−ｘ−−−−−１／　（１＋ａｓ）−−−−
−一閾イ直−−−一一−アラーム１−−−−−１　　　
　１　次フィルタｉ−電流［Ａ］ ３）アルゴリズム： ｔｅｍｐ　　＝ＣＵＲ１”２−ＥＮＥＲＧＹＺＯＥＮＧ
Ｙ＝ＺＯＥＮＧＹ　　＋　　ｔｅｍｐＥＮＥＲＧＹ−Ｚ
ＯＥＮＧＹ／２″”１６”ＴＡＵＥＮＥＲＧＹ　＞　Ｅ
ＬＩＭＩＴ　　ならＥ（：０ＤＥ−ＯＶＥＮＧＹ ４）スケーリング ｉ）アラーム状態出力 ステップ人力１１１Ｊ・Ｋ”１１ｍ１ｔが印加されると
、ｔｘ秒後にアラーム状態を出力する。

ｉｆ）方程式 ％式％ ＝　ｄｅｌｔＩＩ４ミリ秒なら約２０ 乃至４０ エエ遣」ヨ”ｙ　＞ｂ１立　・ ＴＰ回路のサイクル動作を第９図に示して、種々の制御
事象の選択態様を明らかにした。上方スケールで示すよ
うに、ＳＣＭボード４００は１ミリ秒サイクルで動作す
る。ＴＰボード６００は下方スケールで示すように１７
４ミリ秒のサンプリング・サイクルで動作する。４つの
ＴＰボード・サンプリング・インターバルをチックＯ乃
至３で表わした。

６８にスケール上に６つのジョイントについてそれぞれ
のサンプリング・インターバルに示す順序で最も新しい
トルク指令がトルク管理プロセッサ６００によって受信
される。

トルク指令は６２．５マイクロ秒の遅延を伴なってＴＰ
演算回路へ同じ順序で転送される。すべてのジョイント
について、各サンプリング・インターバルの始めに電流
フィードバックが受信され、ＴＰ演算回路へ転送される
。

トルク・プロセッサ・ソフトウェア・ コメント ＴＰボードでの動作に関するその他の詳細事項を以下に
述べる。

メモリ・マツプ トルク・プロセッサ・ハードウェアに関する文書に記述
されているように、モジュールはプログラムＥＦＲＯＭ
、ローカルＲＡＭ。

共用ＲＡＭ　（ＶＭＥ）、ＴＭＳ？”、’ボン・ラム及
びＴＭＳプログラムＲＡＭを具備する。

アドレスは下記の通り。

０ＱＯＱＱＯＱ（１７ｆｆｆ　　ＥＰＲＯＭ０３４００
０　　　０３７ｆｆｆ　　　　共用ＲＡＭ　　（Ｖ１４
Ｅ７’ドレＸ９０００００−！１０３ｆｆｆ） ０３８０００　　　　Ｑ３ｂｆｆｆ　　　　ローカルＲ
ＡＭ０３ｃ０００　　　０３ｃ３ｆｆｆ　　　ピンポン
ＲＡＭ　　（２バンク）０４００００　　　０４１ｆｆ
ｆ　　　　ＴＭＳブ０グラム・メモリ凱旦Ｊ口ｉ　％ ｅａｏｏｏマイクロプロセッサ６０２は７つのユーザ割
込みレベルを有し、これらはすべてベクトル割込みであ
る。ＴＰハードウェアにおいては３つの割込みレベル、
即ち、レベル７．６及び５を利用できる。レベル７は下
記の２つのソースによりて発生させることのできるノン
・マスカブル割込み（ＮＭＩ）である。

次に、アーム・インターフェースに使用される割込みを
説明する。

レベル７　（ＮＭＩ）：１　）　ＰＷＭチップ故障また
はＰＷＭ故障信号。ＰＷＭ チップの以上または２つ の連続するＰＷＭサイク ルに新しいＰＷＭ指令が 、現われないことを指摘す る。

２）ドッグタイマーを監視− タイムアウト。

レベル６　（１’ＷＭＩＲＥＱ）　：　Ｐ　Ｗ　Ｍチッ
プから発生するＰＷＭ割込みリフニス ト。トルク・ループ演算 の開始に利用される。４ Ｋｈｚ８２５０マイクロ秒） レヘ７１／　５　（ＶＴＩＣＫ）　：　１ミリ秒サーボ
・サイクル・チック。

アドレス・エラーのような八−ドウエア故障タイプのエ
ラーは６８０θ０トラツプ・ベクトルに組込まれる。

ＰＷＭゲート・アレイ 第７８１及び１３図に示すＰＷＭチップは８ビツト・モ
ード動作及び１６ビツト・モード動作を備える。トルク
・プロセッサでは１６ビツト・モードが使用される。

詳細にライてはＧＡ１５００　ＰＲＯＤＵＣＴ　ＤＡＴ
Ａ”を参照されたい。

アドレス　　　　　　チップ１ ８００００　　　　　　　ＰＷＭ　　内部ランプ・カウ
ンタ・レジスタ［００２ＰＷＭ　　制御／状態レジスタ
８００１）８　　　　　　１’ＷＭ　　指令レジスタ　
Ｊｔ１８０００ａ　　　　　　　ＰＩ！ＩＮ　　指令レ
ジスタ　Ｊｔ２８０００ｅ　　　　　　　ＰＷＩＡ　　
指令レジスタ　Ｊｔ３アドレス　　　　　　チップ１ ８００４０　　　　　　　ｆ’ＷＭ　　内１１ランプ・
カウンタ・レジスタ８００４２　　　　　　　［’ＷＭ
　　制御／状態レジスタ８（１（１４６ＰＷＭ　　指令
レジスタ　Ｊｔ４８Ｑ（１４ａ　　　　　　　ｆ’ＷＭ
　　指令レジスタ　Ｊｔ５８０Ｑ４ａ　　　　　　　Ｐ
ＷＭ　　指令レジスタ　Ｊｔ６プロセス・タスク配分 密に詰まったす身クル時間でロボット状態を最適化する
ためには、６８０００と７ＭＳ３２０との間で下記のよ
うにタスクを配分する。

１　　、　６８０００ １．５ＣＭとの通信 ２、指令処理 ３、電流サンプリング及びオフセット記憶 ４゜コミュテーション状態読取り ５、ビンボン・メモリ管理 ６、ＰＷＭチップ管理 ７、診断 ８、エラー・リポート ２．７Ｍ５３２０ １、トルク・ループ演算 ２、電流平均化 ３、過電流チェック（絶対量及び平均 量） ４、ＰＷＭ補償 ５、コミエラーシコン補償 ６、エネルギー・チェック ７、逆起電力チェック プロセス・アルゴリズム ループは８６０シリーズ・ロボット及びＲＸロボットの
ブラシなしモータにもＰＵＭＡシリーズ・ロボットのブ
ラシ付きモータのも対応できるように構成するのが普通
であり、モータだけでなく、トルク・プロセッサは液圧
式ロボットのための圧力ループ・プロセッサとしても機
能する。

ＳＣＭとのインターフェース ソフトウェアのオーバヘッド時間は極力小さく抑えねば
ならないから、共用ＲＡＭのセマフォと共に、第６図に
示すような２つのバッファが必要である。このように構
成すれば、２つのプロセッサが同時に別々の場所に対す
る書込みまたは読取りを行なうことができる。

１、バッファ１１−３Ｃからのデータ トルク指令（１ミリ秒）やパラメータ（〉１ミリ秒）の
ようなＳＣＭからのデータはこのバッファに書込まねば
ならない２、バッファ２−トルク・プロセッサからのデ
ータフィードバックや状態のようなトルク・プロセッサ
からのデータはこのバッファに書込まねばならない。

注： ＩＮＴＦＳＰＥＣＯ４−トルク・プロセッサ・インター
フェース通信化様を参照されたい。

各バッファへのアクセスは連携のセマフォで制御される
。

セマ７ｌ−ＢＵＦＦＥＲＩ　： ＳＣＭ　ＤｏＮ＋Ｌ　Ｓｌｌ；Ｍ書込み完了ＴＰ　０Ｏ
ＮＥ　−ＴＰ読取り完了 セマフォーＢＵＦＦＥＲ２： ＳＣＭ　ＤＯＮＥ−ＳＣＭ読取り完了 ＴＰ　０ＯＮＥ　−ＴＰ書込み完了 （ＳＣＭ　ＤＯＮＥ−５５ｈ、　ＴＰ　ＤＯＮｇ−＾Ａ
ｈ）プロセスは下記の通り： １、セマフォ状態をＲＥＳＥＴ−０に初期値設定。

２、書込みプロセッサがデータを書込む。

３゜セマフォをＷＲＩＴＥ　ＰＲＯＣＥＳＳＯＲＤＯＮ
Ｅにセット。

４．１ミリ秒割込みを待機（ＳＣＨに 関しては１ミリ秒割込み、ＴＰニ 関しては第４トルク・ループ・ チック） ５、読取りプロセッサがデータを読取る。

６、セマフォをＲＥＡＤ　ＰＲＯＣＥＳＳＯＲＤＯＮ、
Ｅにセット。

７、ステップ２に進む。

注： データ記録器はＷＲＩＴＥ　ＰＲＯＩＩ：ＥＳＳＯＲが
なされるとデータを読取るが、読取りが完了すればセマ
フォをセットしない。

ダブル・バッファの動作タイミングは下記の通り： １ミリセコンド・ チック バッファ１ ＳＣＭ　　　　　　　　　　　　ｗ　　　　　　　　　
　　　　　　　　　　ｗ　　　　　　　　　　　　　　
　　　ｗＴＰ　　　　　　　ｒ　　　　　　　　　ｒ　
　　　　　　　　ｒバッファ２ ＴＰ　ｗ　　胃胃 ＳＣＭ　　　　　　　　ｒ　　　　　　　　　　　　　
ｒ　　　　　　　　　　　　ｒＴＰタイミングは極めて
重要であるから、バッフトデータ構造はトルク・プロセ
ッサ・ソフトウェア・デザイナ−の手で設計しなければ
ならない。この設計には６８０００命令セツト及びＴＰ
ハードウェア構造の長所を取入れることになる。

一以　下　余　白− ６８０００と７ＭＳ３２０との間のインターフェーシン
グ ７ＭＳ３２０インターフエーシングはビンボン・メモリ
を介して第６図に示すような連携ハードウェア回路との
間にのみ行なわれる。

ダウンロードロ　な診断 ＴＰに特殊なＥＦＲＯＭを組込んだり、あるいは例えば
ＥＭＵＬＯＧＩＣやチュータ・ボードのような開発シス
テムを挿入しなくても、ＴＰ関連ハードウェア、即ち、
ＰＷＭチップ及びＡ／Ｄを試験できるように、ダウンロ
ード可能な診断機構を設ける。

パワーアップと同時に、ＳＣＭやＭｌｚａｒ　ＣＰＵの
ようなホスト・コンピュータやＴＰ中に配置された予約
共有ＲＡ　Ｍ　（９０２αｏｏｈ乃至９０３ｆｆｆｈ）
にＴＰ診断をダウンロードすることができる。自己診断
または初期値設定のような通常の開始手順が完了すると
、ＳＣＭその他のマスター装置がＴＰに診断指令を供給
することができる（４０００ｈ乃至９００００４ｈに書
込む）。

指令を確認したＴＰは９０２００００ｈに位置するサブ
ルーチンをコールし、バックグラウンド・タスクに戻る
。なお、ＴＰアドレスとＶＭＥアドレスは異なるから、
ダウンロード可能な診断はすべて再配置可能でなければ
ならない。

サーボ制御モジュール（ＳＣＭ）またはボード４００（
第４図）はロボット制御システムのモジュール構成に従
い、基本的ロボット制御全般に対応するコア・ボードと
して動作し、記憶されているロボット・プログラム指令
からアームの解を形成するか、または広範囲のロボット
制御装置の一部として動作し、高レベル・システム制御
ボード３５０によりロボット・プログラム指令から形成
されるアームの解を受信してこれを実行するように構成
する。アームの解の形成には、ロボット・プログラム・
ランゲージの解読、バス・プランニング、軌道演算（中
間位置指令及び軸整合）、デカルト座標系とロボット・
ジョイント及びロボット工具座標系との間の位置情報変
換などのようなロボット制御機能の実行が伴なう。ＳＣ
Ｍボード４００は関連周辺装置及び、もし設置されるな
ら、ホスト・コントローラとインターフェースする通信
をも可能にする。

ＳＣＭボード４００はロボット制御系のアーム・モーシ
ョン制御ループを実行するためのプログラム制御される
デジタル回路を含む。モーション制御は好ましくは相関
位置、速度及び加速制御ループを含む制御ループ構造に
より軸ごとに行なわれ、前記ループからトルク指令が形
成され、トルク・プロセッサ・モジュール６００によっ
て実行される。

デジタル・サーボ制御装置は協調されたマルチプロセッ
サ・サーボ制御装置であり、（１）アームの解によって
各軸について与えられる位置及び速度指令と、（２）ア
ーム・インターフェース・モジュール８００を介して位
置エンコーダ及びタコメータから得られる位置及び速度
フィードバック信号に基づいて出力トルク指令を形成す
る。

ＳＣＭ制御ループ動作において、供給される軸位置指令
及び軸位置フィードバックから各軸について位置誤差が
算出される。また、連続する位置指令、及び軸速度フィ
ードバックから、各軸について速度誤差が算出される。

好ましくは位置及び速度制御ループを並列に操作する。

即ち、位置及び速度誤差を加算することにより、トルク
制御モジュール６００におけるトルク制御ループのため
のトルク指令を形成する。さらにまた、加速指令を好ま
しくは連続する速度指令から求め、これをフィードフォ
ワード加速制御ループに供給し、該制御ループが加速に
基づくトルク指令を発生し、ＳＣＭ出力トルク指令形成
の際に前記加速に基づくトルク指令が位置及び速度誤差
と加算されるようにする。

ループ演算が行な、われる頻度は迅速、正確、円滑かつ
安定なロボット・アーム・モーションが得られるように
選択する。例えば、ここに述べる実施例のように軌道サ
イクルが３２ミリ秒となるような頻度を選択する。必要
なら、もっと速い軌道サイクル、即ち、８ミリ秒という
短いサイクルも可能である。

ＳＣＭデジタル回路 第１０図から明らかなように、３０Ｍボード４００は一
般に２つのセクション、即ち、局部プロセッサ・セクシ
ョン４０１とシステム演算セクション４０３とから成り
、システム資源セクション４０３はバス４０５を使用し
、ロボット制御システム全般に関連し、位置および速度
制御ループの実行には特に関連しない機能を提供する。

システム資源セクション４０３が提供する機能としては
、ロボット・アームの解を記憶するＥＰＲＯＭ４０８、
持久データを記憶するバッテリ・バックアップＲＡＭ４
１０、静的ＲＡＭ４１２、リアルタイム・クロック４１
５、ＤＭＡコントローラ４１４、及び２つのマルチプロ
トコル・デュアル・チャンネル通信コントローラ４１６
．４１８などがある。

システム資源セクションはデュアル・ボート・メモリと
して構成する。従って、局部プロセッサ４０１からでも
ＶＭＥバス４２０からでも同様にシステム資源セクショ
ンにアクセスできる。システム資源機能はＶＭＥバスに
従属するバスの様相を呈する。従って、これらの関連機
能をＳＣＭ局部プロセッサからでも、システム・バスに
接続した任意プロセッサからでも制御することができる
。

局部プロセッサ・セクション４０１において、ＳＣＭデ
ジタル回路は特定の性能を得るのに必要な、即ち、すべ
ての軸を正確かつ能率的に制御するのに必要な制御演算
及び制御データ管理を可能にすると共に、ホスト・コン
トローラ、周辺装置及びその他のロボット・コントロー
ラとのインターフェース通信を可能にする整合コプロセ
ッサ・インターフェース及び資源回路を含む。好ましく
は、サーボ制御管理プロセッサ４０２が、位置及び速度
制御ループ演算を主要目的とする従属プロセッサとして
機能するサーボ演算回路４０４と協働するように構成す
る（即ち、フィードバックフィルタ・ループ・ゲイン、
位置及び速度誤差など）。

サーボ制御管理プロセッサ４０２は３０Ｍボード４００
との間で、また、サーボ位置／速度制御演算回路４０４
との間で制御、状態及びプログラム・データを送受信す
る。サーボ制御管理プロセッサ４０２としては、データ
処理能力の高いＭｏｔｏｒｏｌａ　６８００Ｇと利用で
きる。プロセッサ４０２．４０４のそれぞれの能力に従
ってデータ管理及び制御演算タスクを分離することによ
り、製造及び使用コストを軽減しながら著しく改善され
た制御性能を達成する基礎となる基本回路編成が得られ
る。

図示の実施例では、３０Ｍボード４００の局部プロセッ
サ・セクションを、サーボ制御管理プロセッサ及び２つ
のコプロセッサとして６８０００プロセツサを使用して
実施した。コプロセッサはいずれもａａｏｏＯの周辺装
置として作用する。コプロセッサの４０６の１つ（好ま
しくはＮａｔｉｏｎａｌ　Ｓｅｍ１−ｃｏｎｄｕｃｔｏ
ｒ３２０８１）は３０Ｍボード４００によってアームの
解を供給すべき時に浮動少数点演算能力を提供する。他
方のコプロセッサ、即ち、従属プロセッサは位置／速度
サーボ、演算回路４０４であり、これにはＴｅｘａｓ　
Ｉｎｓｔｒｕｍｅｎｔｓ　ＴＭＳ−３２０１０Ｄｉｇｉ
ｔａｌ　Ｓｉｇｎａｌ　Ｐｒｏｃｅｓｓｏｒと採用した
。位置／速度プロセッサは高速固定少数点演算能力を提
供する。

局部プロセッサ・セクションの一部であるその他の機能
としては、局部メモリ、ＥＰＲＯＭ４２２及びＲＡＭ４
２４、周辺タイマ／カウンタ装置、割込み制御装置４３
０、及びシステム・エラー・モニター装置４２８がある
。局部プロセッサ４０２はＴＰＭまたはその他の関連タ
イプの機能にアクセスするためのＶＭＥバスに対するマ
スターとして作用することができる。ただし、３０Ｍボ
ード４００はシステム・リセット発生、バスにアクセス
するためのバス選択及びシステム・バス・クロック発生
などのよりなＶＭＥバス・システム・コントローラ・タ
イプの機能は提供しない。なぜなら、これらの機能はア
ーム・インターフェース・ボード８００において行なわ
れるからである。

３０Ｍボード４００はできる限り多くのシステムに融通
性を与えると共に、市販の大規模集積（ＬＳＩ）回路か
ら最大限の性能を引出せるように構成する。局部プロセ
ッサ・バスに直接接続せずに、システム資源セクション
にＤＭＡ及び通信手段を組込む理由の１つもここにある
。このように構成すれば、サーボ制御管理プロセッサ４
００が通信データのり動に直接干渉し、なくてすみ、局
部プロセッサ・バス通信に伴なうオーバヘッド無駄時間
がなくなるから、サーボ制御管理プロセッサ４００にお
けるプログラム実行時間に大きい影響を与えることなく
高速逐次通信を行なうことができる。また、゛これらの
機能をシステム資源セクションに組込むことにより、Ｖ
ＭＥバス・マスターとして作用できる他の任意のプロセ
ッサによってこれらの手段を操作することができる。そ
の結果、サーボ制御管理プロセッサ４００を通信に関連
する処理から完全に解放することになる。この構成を採
用すれば、ロボット制御システムに必要なすべての機能
をコストの点で能率的に、かつ最小セットのボードで実
現でき、しかも全体的なシステム設計に影響を及ぼすこ
となく制御装置の性能を高めることができる。

他の重要なセクションはサーボ制御管理プロセッサ４０
２とサーボ演算回路４０４との間のインターフェースで
ある。ここでは“ビンボン”または”バンクスイッチ”
メモリと呼ばれる特殊なデュアルポート・メモリ編成に
より、どちらのプロセサの処理性能にも影響を及ぼすこ
となく、一方のプロセッサが他方のプロセッサと通信す
ることができる。

サーボ匍　ボードのプログラムされた動作第１１Ｂ図に
略示するように、サーボ制御データ管理プロセッサ４０
２のためのプログラム・システムはＭＡＩＮと呼ぶバッ
クグラウンドプログラム４５０及び周期的に作動される
５ＥＲＶＯと呼ぶフォアグラウンド割込みルーチン４５
２から成る。図示のようにシステムがＲＥＳＥＴからス
タートすると、ＭＡＩＮプログラム４５０の連続実行の
前に初期設定ルーチン４５３が実行される。周期的に実
行される５ＥＲＶＯ割込みルーチン４５２のほかに、必
要に応じてフォアグラウンドにおいてＣ＄ｔｌＮＥＸと
呼ぶ割込みルーチン４５４が動作して予定外の割込みを
処理する。さらに、ウォッチ・ドッグ・タイマー割込み
と呼ばれる特殊な最高優先順位ルーチン４５７が外部ウ
ォッチ・ドッグ・ハードウェアの動作に応答して機能す
る。

ロボット制御システムが演算能力を高めることによって
性能を高めるためのシステム制御ボード３５０を含む場
合、ＭＡＩＮプログラムがシステム制御ボード３５０か
ら位置指令を受信し、分配する。最小限の、即ち、基礎
的なロボット制御システム構成にはシステム制御ボード
３５０が含まれず、ＭＡＩＮプログラム４５０はサーボ
制御ボード４００において局部的に位置指令を発生させ
るためアームの解を求める機能をも行なう。基礎ロボッ
ト制御についてはさらに後述する。

５ＥＲＶＯルーチン４５２を周期的に実行するためにＭ
ＡＩＮプログラム４５０が割込まれる頻度はアーム・イ
ンターフェース・ボード８００からのＶＭＥバス１５５
に１ミリ秒毎に１回ずつ発生する信号ＶＴＩＣＨによっ
て制御される。５ＥＲＶＯルーチン４５２の基本的機能
は下記の通り。

１）サーボ演算回路４０４との間で制御データを転送す
る； ２）トルク・プロセッサ・ボード６００との間で制御デ
ータを転送する； ３）アーム・インターフェース・ボード８００からＶＭ
Ｅバス１５５を介してセンサフィードバックデータを受
信する； ４）補助バックグラウンドタスクＲＤＭＡＳＣとインタ
ーフェースする； ５）同期データ記録を行なう； ６）ワン・ショット・データ記録を行なう；７）同報デ
ータをブラックボード記憶部に記憶させる； ８）重大なエラー状態が発生するとシステムを停止させ
る。

初期設定ルーチンを第１１Ｃ図に詳細に示した。このプ
ロセスは第１１Ｃ図の連続するブロックに指示しである
一連のタスクを行うのがその主要機能である。下に掲げ
る表はこれらのタスクの性能に関する情報を示す。

タスク　　　　　　　初期テ ｌＮｌ５ＲＣ初期手順のためのビット・バス・エミュレ
ータ ｌＮＳｃＲＯＭ　　　　　　フィードバックインターフ
ェースｆＮＩＦｒＸ　　　　グローバル変数 ｌＮＣ０ＮＥ　　　　ロボットデフォルト構成ＩＮＴＭ
Ｓ　　　　サーボ演算回路−ダウンロード・プログラム
・メモリ ＩＮＩＡＩＢ　　　　トルク・プロセッサ・ボードＩＮ
ＲＯＢ　　　　ロボット・、ソフトウェア構成ＩＮＪＴ
ＭＳ　　　　バンク・スイッチ・メモリＩＮＪＯＮＴ　
　　　サーボ演算回路プログラム・メモリ及びトルク・
プロセッサ ・ボード共用ＲＡＭ読取りに基 づくジョイント・データ構造 ＩＮＢｔｌＦ　　　　内部バッファ管理セクションｌＮ
ＩＤＩＣワン・ショット及び同期データ記録装置インタ
ーフェース ＣＲＴＩＮＩ　　　　入／出力バードウェア・サーボ制
御ボード４００ ＢＢＩＮＩＴ　　　　システム制御ボード３５０とのビ
ット・バス・エミュレータの インターフェース及び初期手順 システムが初期設定されると、ＩＡＡＩＮプログラム４
５０（第１１Ｄ図）が連続周期的なバックグラウンド実
行を開始する。第１１Ｄ及びＩＩＥ図に示すように、Ｒ
ＤＭＡＳＣと呼ばれるルーチン４５６が実行されて、シ
ステム制御ボード５００（第４図）から受信されたシス
テム指令が記憶される共用ＲＡＭ中の指定記憶場所であ
るビット・バス・エミュレータからシステム位置指令が
継続的に読取られる。

全体的に見て、バックグラウンド及び割込みルーチンは
下記のような有限サーボ状態を取ることができる。

〇　−起動状態 １−　初期設定状態 ２−　較正状態 ３−　サーボ使用禁止状態 ４−　サーボ使用可能、停止状態 ５−　サーボ使用可能、動作状態 有効な状態転移が行われるようにすると共に、新しい状
態に対する割込みタスクをシーケンスするのがバックグ
ラウンドタスクである。従って、割込みタスクが書込ま
れて”指令された”状態を受け、”現時”状態を供給す
る。また、バックグラウンドタスクはビット・バス・エ
ミュレータからの指令によってシーケンス制御される。

下記の状態転移表はこの動作を示す。

ビット・バス　か　らの指令１［＋ 現時状態　３６１７旦　１１６３４旦」昆■″−１”は
違法な状態運転転移を示す。ビット・バス指令の定義は
下記の通り： ３６−　位置指令 １７−　較正指令 ４５−　サーボ・パラメータ設定指令 １−　サーボ可能化／不能化指令 １６−　停止指令 ３４−　初期設定指令 ２〇　−正常動作指令 ２−　ブレーキ指令 １８−　リンブ指令 ４６−　現時サーボ・パラメータ・ アップロード指令 一般に、毎Ｖａｌサイクル、この実施例では３２ミリ秒
毎に１回、サーボ・ループのために１組の位置指令が発
生する。また、各Ｖａｌサイクルは１ミリ秒毎に１回Ｖ
ＭＥバスで発生する■チックによって測定される。この
実施例では各ＶＡＬＣＹＣＬＥ中に３２個のＶチックが
存在する。

サイク７Ｌ／−カウンタＴＣＫ（：ＮＴＲは各ＶＡＬＣ
ＹＣＬＥ中のＶチックをカウントする。ランニング・カ
ウンタＵＮＩＣＮＴＲはＶチックのカウントを２３１チ
ツクまで累算する。

ブロック４５７のｕＮＩｃＮＴＲで指示されるように１
つ前（７）　ＶＡＬＣＹＣＬＥが終結すると、ＴＣＫＣ
ＮＴＲ−１となる新しいＶＡＬＣＹＯＬＥの開始と同時
にブロック４５８が作動して新しいＶＡＬＣＹＣＬＥの
ための新しい位置指令セットの受信を開始させる。その
結果、ブロック４５９がビット・バス・エミュレータか
ら新しい指令を読取る。

指令が（サーボ使用禁止のような）緊急指令を含む場合
には、ブロック４６０が緊急指令バッファへ緊急指令を
送る。ブロック４６１は運動指令バッファへ運動（位置
）指令を送る。同様に、ブロック４６２は非運動指令バ
ッファへ非運動指令を送る。

ブロック４６０．４６１及び４６２は同様の基本的機能
を行う。先ず、共用ＲＡＭのビット・バス・エミュレー
タから指令が読取られ、次いで所定のチェックが加えら
れて指令を有効化し、最終的に、局部非共用内部ＲＡＭ
の指定バッファに指令が送られる。

システム指令を内部ＲＡＭバッファへ転送する指示がな
ければ、ブロック４６３は指令エラーなしを記録し、別
の指令を作成する（この指令エラーなしが２度目なら、
最終指令と同じ指令または停止指令を作成する）。

次いで、ブロック４６４，４６５はシステム制御ボード
５００に対し、チック・カウンタが現時ＶＡＬ（：ＹＣ
ＬＥニ近い、即ち、ＶＡＬＣＹＣ−３に等しければ、最
も新しい指令が受信されたことを回答する。次いでブロ
ック４６６はＲＤＭＡＳＣが再び実行されるまでｌｌＮ
ＩｃＮＴＲを繰返しボールすることにより次のシステム
指令を受信する。

（１ミリ秒毎に１回）　ＶＴＩＣにが発生すると、５Ｅ
ＲＶＯ７１ｚ−チン４５２を実行するためＲＤＭＡＳＣ
が一時的に割込まれる。第１１Ａ図のフローチャートに
示すように、　５ＥＲＶＯルーチン４５２がブロック４
７０に示すように、必要な雑用及び付加タスクを行う。

もしチックが長いチックなら、即ち、チック・カウンタ
が１で、新しいＶＡＬ（：ＹＣＬＥの開始を示すなら、
ブロック４７１はさらにいくつかの付加タスクを行う。

具体的な機能としては、チック・カウンタを１にリセッ
トし；システム制御ボードからサーボ演算回路へ新しい
位置指令をルーチングシ；　５ＥＲＶルーチンの状態変
化を処理する。

チックが短いチックなら、即ち、３２チツク・アイクル
中の中間チックなら、または長いチックのあとに付加タ
スクが行われた場合には、ブロック４７２はサーボ演算
回路が取るべき状態に従ってデータをバックスイッチ・
メモリにロードすることにより、サーボ演算回路へのデ
ータ転送を用意する。

第１１Ｆ図のフローチャートはサーボ演算回路のセット
アツプを詳細に示す。演算回路が起動状態にあることを
ブロック４７３が検出すると、ブロック４７４はサーボ
演算回路４０４にダウンロードするため、始動ロジック
に必要なデータを取出す。初期設定状態では、ブロック
４７５．４７６が初期設定指令データを取出してサーボ
演算回路４０４にダウンロードする。同様に、演算回路
較正状態では、ブロック４７７．４７Ｂがサーボ演算回
路４０４のために較正指令データを用意する。

演算回路を不能化したければ、ブロック４７９．４８０
がバンク・スイッチ・メモリに不能化指令をロードする
。また、演算回路を、演算ラインの大部分を占める停止
または動作状態にしたければ、ブロック４８１．４８２
がバンク・スイッチ・メモリに動作指令データをロード
する。表に挙げた状態が全く検出されなければ、ブロッ
ク４８３がエラーを記録する。

データの流れ　びタイミング 停止／動作状態において、制御データは第１２−２図の
関連、して後述するように流れるのが普通である。毎Ｖ
ＡＬＣＹＣＬＥに１回、即ち、この実施例では３２ミリ
秒に１回ずつ、すべて軸に関する位置指令データがサー
ボ制御ボード・メイン・プログラムにより、システム制
御ボードから受信゛され、バンク・スイッチ・メモリを
介してサーボ演算回路に供給される。１ミリ秒毎に１回
ずつ各軸についてフィードバックデータ（位置及び速度
）が発生し、サーボ割込みルーチンの動作を介してサー
ボ制御データ管理プロセッサにより、１ミリ秒毎に１回
ずつバンク・スイッチ・メモリを通ってサーボ演算回路
へ転送される。

サーボ演算回路は指令及びフィードバックデータから、
各軸について１ミリ秒毎に１回ずつトルク指令を算出す
る。このトルク指令はサーボ制御データ管理プロセッサ
により、バンク・スイッチ・メモリからトルク・プロセ
ッサ・ボードへ１ミリ秒毎に１回ずつ転送される。ＴＰ
ボードでは、トルク指令及び電流フィードバックに基づ
くトルク演算が行われて、２５０マイクロ秒ごとにすべ
ての軸に関するモータ電圧指令を形成する。

−以　下　余　白− サーボ演　回路へのデータ　換え 始動サブルーチン４７２が完了したのち、ブロック４８
４がサーボ制御データ管理プロセッサ４０２内のフォア
グラウンド／バックグラウンドインターフェースに必要
なデータ転送を行う。

フェース ６８Ｋにおいて割込みルーチンを有効に利用するために
は、バックグラウンドタスクにおいて種々の同期的”始
動”操作を行う一方、フォアグラウンドタスクを利用し
て変化を例示化することが必要となった。このアプロー
チにより、フォアグラウンドタスクが従属プロセッサ及
び装置を完全に制御することができる。このアプローチ
はまた、時間的条件がきびしくない始動をいくつかのサ
ーボ・サイクルに互って散在させることができる。

（ここではバックグラウンドタスクは非割込みタスクで
ある。） フォアグラウンドルーチンが行うべき一連のタスクをパ
ックグラウンドルーチンが通信できるように共用ＲＡＭ
メイルボックス・インターフェースを選択した。各タス
ク・リクエストはフォアグラウンドタスクが必要となる
情報を含む”パケット“の形に符号化される。このパケ
ット構造は簡単ではあっても（デコーディング及び実行
のコストという点で）できる限り能率的であるように構
成した。下に掲げる表はインターフェースの形式％式％
） １　　　パケット（リクエスト）数 ２−３　１→指令フイールド ４−５％　　１　　ｌ　　　　アドレス・フィールドか
らアドレス・フィールド６−９　へケ　Ｉ　　　へのメ
ツセージ・サイズａ−ｄ　　ット　１　→　（１６ビツ
ト・ワードの数）ｅ−１５第２　（上記内容のうち第２
〜ｄ項）パケット １６−１ｄ第　３パケツト サーボ割込みルーチンについては、各パケット中の要素
は下記のように解読される（単一パケットが提供される
場合を想定する）。

士令フィールド（ＣＦ　： ＣＦ＞Ｏ：ＴＭＳ３２０サーボ指令 ＊ＴＭＳ指令リストにＣＦをロードする（７ＭＳビンボ
ン・メモリ・マツプを 参照） ＊”ＦＲＯＭ″から“メツセージ・サイズ・ワード”を
コピーし、ＴＭＳ人力 バッファに付記する。

＊セマフォバイトを”Ｅｍｐｔｙ”にセットする。

ＣＦ−０：データ転送指令 ＊”メツセージ・サイズワードを ＦＲ０Ｍ”から″ＴＯ″ヘコビー する。

セマフォバイトを”Ｅｍｐｔｙ”にセットする。

ＣＦ雪−９９９９：７ＭＳリセット指令（７ＭＳプログ
ラム・メモリへの直接 書込みを可能にする）。

＊割込みを不能化する（レベル５−ＶＴＩＣＩ）。

＊ＴＭＳをリセット状態にする。

＊”メツセージ・サイズワードを ＦＲ０Ｍ″からＴｏ”ヘコビー する。

＊ＴＭＳからリセット状態を除く。

＊スティックをバンク０にセットする。

＊割込みを可能化する（レベル５−ＶＴＩＣＫ）。

＊セマフォバイトを”Ｅｍｐｔｙ”にセットする。

ＣＦ＜Ｏ：７ＭＳ３２０サーボ・マルチパス指令 ＊ＣＦ’−−ＣＦにセットする。

＊ＣＦをＴＭ、Ｓ指令リストにロードする。

＊”メツセージ・サイズワードを ＦＲ０Ｍ”からコピーし、７ＭＳ入力 バッファに付記する。

＊セマフォバイトを”Ｅａ＋ｐｔｙ”にセットしない。

注： ＊マルチパス指令に対し、時間のかかるパケット管理を
回避するため、マルチパ ス・タイプのパケットが含まれているなら、すべてのパ
ケットを第２パスで再処理する。

＊ＴＭＳをリセットすると自動的にバンク０（先行のパ
ケットが記憶されていたバンクとは限らない）に切換わ
るから、もＬＴＭＳリセット指令が存在するとＴＭＳバ
ンク・メモリは切換わらない。従ってこれらの指令は別
々に送信すべきである。

＊システム資源ＲＡＭの３００へツクス・バイトをメイ
ルボックス・セクションとして、また、データを記憶さ
せる空きスペースとして確保するのが普通である。

下に掲げる包括記号名リストはこのセクションをさらに
限定する。

包括記号リスト ＦＢ　　ＳＨＲ０ｘ３４９００　　ハフオアグラウンド
／バックグラウンドセマフォ上クシ ョンのスタート点に対す る共用ＲＡＭベース・ア ドレス ＦＢ　　ＳＥＭ　０ｘ００００＋　　／傘ＦＢセマフォ
バイトに対する　　　　ＦＢ　　５）ＩＲＰｔｒ傘／Ｆ
Ｂ　　ＮＯＰ　０ｘＯＯ０１＋　　／＊）＜ケラト数（
バイト）ＦＢ　　ＳＨＲに対するＰｔｒ＊／ ＦＢ　　ＣＦＩ　０ｘ０００２＋　　／拳第１指令フィ
ールドにＦＢ　　ＳＨＲ対するＰｔｒ＊／ ＦＢ　　ＭＡＸ　０ｘ０００４　　／中許容されるパケ
ットの最大数＊／ ＦＢ　　ＤＡＴ　０ｘ００３２÷　／−フォアグラウン
ドタスクへ転送され ＦＢ　　ＳＨＲるデータを記憶するための空きスペース
・セクシ ョンに対するＰｔｒ◆／ ＦＢ　　ＭＸＤ　０ｘ０２ｃｅ　　／＊ＦＢ−ＤＡＴの
最大サイズ（バイト）　Ｉ／ ＦＢ　　ＥＭＰＴＹ　０ｘ０４　　／中指令すスト不在
中／ＦＢ　　ＦＵＬＬ　０ｘ０１　　　／＊指令リスト
が存在＊／サーボ演算回路４０４が作動可能状態にある
ことをブロック４８５が検出する。と、ブロック４８６
はビンボン・メモリを介して演算回路４０４へ、すでに
形成された位置指令及び／またはその他のデータを転送
する。サーボ演算回路４０４が不良状態にあることをブ
ロック４８５またはブロック４８７が検出するとエラー
・シャットダウンが指令される。

下に掲げる指令リストはサーボ演算回路へ転送されるデ
ータに関する詳細を示す。

ビンボン・メモリ・マツプ アドレス ６８Ｋ　　　　ＴＭＳ　　記号　　　　内容３ｃ０００
−３ｃ００７　　０−７　　　ＴＭＳＣＯＭ　　　　７
ＭＳ指令リスト３ｃ０１０−３ｃ０９ｆ　　　８−４ｆ
　　ＴＭＳＩＮ　　　　　ＴＭＳ人カバッフ７３ｃＯａ
Ｏ−３ｃＯｆｆ　　５Ｏ−７ｆ　　ＴＭＳＳＴＡ　　　
　ＴＭＳ状態バッファ３ｃ１００−３ｃｌｌｆ　　８Ｏ
−８ｆ　　ＴＭＳＴＯＲＴＭＳ）ルク　出力バッファ ３ｃ１２０−３ｃ２１ｆ　　９Ｏ−１０ｆ　　ＴＭＳＬ
ＯＧ　　　７ＭＳ記録データ・Ａツフ７ ３ｃ２２Ｇ−３ｃ７ｆｆ　１１０−３ｆｆ　ＴＭＳＯＣ
ＴＭＳ汎用通信七クシりン １にワード・ビンボン・メモリを含む各バッファの詳細
を以下に説明する。表に示す記号は６８Ｋ及びＴＭＳコ
ード間の一致性を高めるため、各バッファの始動に使用
されるようにその内容を示唆した名称である。バッファ
は初めの２つのセクション（ＴＭＳＣＯＭ及びＴＭＳＩ
Ｎ）がＴＭＳ用には読取専用（６８に用には書込専用）
となるように構成する。次の３つのセクションはＴＭＳ
用には書込専用、６８に用には読取専用である。最終セ
クション（ＴＭＳＧＣ）は両プロセッサ共通の読取／書
込セクションである。

７ＭＳ指令リストは所与のサイクルにＴＭＳが行うこと
をリクエストする。ＮＵＬＬ指令で終るタスク・リクエ
スト・リストであり、１つのリストが最大限７つの指令
を含むことができる。各エレメントはＴＭＳの作用を指
示する１６ビツト指令である。好ましい指令は下記の通
り： 指　　令　　　　　　　　　　　イ　　　　用１　　　
　現時点では使用されない ２　　　　現時点では使用されない ３　　　　現時点では使用されない ４　　　　現時点では使用されない ５　　　　現時点では使用されない ６　　　　初期位置及び速度を較正 ７　　　　トルクを演算（サイクル更新なしでサーボを
実行） ８　　　　トルクを演算（サイクル更新伴なってサーボ
を実行） ９　　　　現時点では使用されない ａ　　　　　ＴＭＳサーボ・パラメータ及びバージョン
ＩＤを識別 ＴＭＳ人カバッファは指令ワードの値及び指令リストに
占めるその相対位置にコンディショニングされたＴＭＳ
が必要とする補足的入力データをも含む。各指令が必要
とする入力データは指令リスト中での指令の順序で逐次
記憶される。指令ワード＝８（例えば、サイクルまたは
”長い”チック）なら、このバッファは下記の形式を取
る。ただし、この表の編成は１６ビツト”短”ワード・
カウントに基づいている。

絶対量方式のエンコーダ及びタコメータを使用する場合
（例えば５ｅｏｘ／ｒｘ）、ＴＭＳＩＮは下記の通り二 ワード　　　　　　　　　内　　容 ０　　　タイム・スタンプ １　　　サーボ可能化マスク ２　　　速度倍率 ３　　　ジョイント１に関する位置指令の高ワード ４　　　ジョイント１に関する位置指令の低ワード ５　　　ジョイント１に関する絶対エンコーダ測定 ６　　　ジョイント１に関する絶対速度測定 ７−１Ｏジョイント２に関するワード １１−１４ジョイント３に関するワード３−６　　” １５−１８　　　ジョイント４に関するワード１９−２
２ジョイント５に関するワード２３−２６ジョイント６
に関するワード指令ワード＝７（例えば、ノン・サイク
ルまたは”短”チック）なら、ＴＭＳ人カバカバッファ
記の形を取る。

ワード　　　　　　　　　　　　容 Ｏタイム・スタンプ １　　　サーボ可能化マスク ２　　　ジョイント１に関する絶対エンコーダ測定 ３　　　ジョイント１に関する絶対速度測定 ４−５　　　　ジョイント２に関するワード６−７ジョ
イント３に関するワード ８−９ジョイント４に関するワード １０−１１ジョイント５に関するワード１２−１３ジョ
イント６に関するワード増分エンコーダのみ使用の場合
（例えば、ＰＵＭＡライン）指令＝８に対応するＴＭＳ
ＩＮは下記の通り： ワード　　−　　　　　ｒｑ−二組一一一一一〇　　　
タイム・スタンプ １　　　インデックス割込み／サーボ可能化マスク ２　　　位置フィードバック桁上げ（ラップ） ３　　　速度倍率（１６０８フオー マツト） ４　　　ジョイント１に関する位置指令の高ワード （３２０８フオーマツト） ５　　　ジョイント１に関する位置指令の低ワード ６　　　ジョイント１に関するエンコーダ・カウント／
インデックス ７−９　　　　ジョイント２に関するワード１１−１２
ジョイント３に関するワード１３−１５ジョイント４に
関するワード１６−４８ジョイント５に関するワード１
９−２１ジョイント６に関するワード指令ワード！７（
例えば、ノン・サイクルまたは”短”チック）なら、増
分エンコーダを使用する場合、ＴＭＳ人カバカバッファ
記の形を取る。

ワード　　　　　　　　　　　　容 ０　　　タイム・スタンプ １　　　インデックス割込み／サーボ可能化マスク ２　　　ジョイント１に関するエンコーダカウント／イ
ンデックス ３　　　ジョイント２に関するエンコーダカウント／イ
ンデックス ４　　　ジョイント３に関するエンコーダカウント／イ
ンデックス ５　　　ジョイント４に関するエンコーダカウント／イ
ンデックス ６　　　ジョイント５に関するエンコーダカウント／イ
ンデックス ７　　　ジョイント６に関するエンコーダカウント／イ
ンデックス 絶対レゾルバ−が使用される場合（例えば、１００ロボ
ツト）、 ワード　　　　　　　　　　　　容 ０　　　タイム・スタンプ １　　　サーボ可能化マスク ２　　　速度倍率 ３　　　シミインド１に関する位置指令の高ワード ４　　　ジョイント２に関する位置指令の低ワード ５　　　ジョイント１に関する絶対レゾルパー測定 ６−８　　　　ジョイント２に関するワード９−１１ジ
ョイント３に関するワード １２−１４ジョイント４に関するワード１５−１７ジョ
イント５に関するワード１８−２０ジョイント６に関す
るワード指令ワード＝７（例えばノン・サイクルまたは
”短”チック）なら、ＴＭＳ人カバカバッファ記の形を
取る。

ワード　　　　　　　　　内　　容 Ｏタイム・スタンプ １　　　サーボ可能化マスク ２　　　ジョイント１に関する絶対レゾルバ−測定 ３　　　ジョイント２に関する絶対レゾルバ−測定 ４　　　ジョイント３に関する絶対レゾルバ−測定 ５　　　ジョイント４に関する絶対レゾルバ−測定 ６　　　ジョイント５に関する絶対レゾルバ−測定 ７　　　ジョイント６に関する絶対レゾルバ−測定 指令ワード−６（例えば、較正指令）ならＴＭＳ人カバ
カバッファ記の通り： ワード　　　　　　　　　　　　容 ０　　　タイム・スタンプ １　　　ジョイント１に関するエンコーダ・ビット（３
２０８）で表わされ る絶対位置 ２　　　ジョイント２に関するエンコーダ・ビット（３
２０８）で表わされ る絶対位置 ３　　　ジョイント３に関するエンコーダ・ビット（３
２０８）で表わされ る絶対位置 ４　　　ジョイント４に関するエンコーダ・ビット（３
２０８）で表わされ る絶対位置 ５　　　ジョイント５に関するエンコーダ・ビット（３
２０８）で表わされ る絶対位置 ６　　　ジョイント６に関するエンコーダ・ビット（３
２０８）で表わされ る絶対位置 指令ワード−Ａ（例えば、ＩＤ指令）なら、ＴＭＳ人カ
バッファは入力を必要としない。

ＴＭＳ状態バッファの目的はいかなるブロモ・スにおい
てもアームの現時位置／速度を検出するために探索でき
るセクションを提供することにある。従って、このバッ
ファは下記のように編成される。

ワード　　　　　　　　　　　　容 ０　　　タイム・スタンプ（将来） １　　　可能化／不能化ビット・マスク２　　　リンブ
状態ビット・マスク ３　　　ブレーキ状態ビット・マスク ４　　　エンコーダ・ビット３２０８　（高ワード、）
で表わされるジョイント １の位置、即ち、ＰＦＢ ５　　　エンコーダ・ビット３２０Ｂ（低ワード）で表
わされるジョイント １の位置、即ち、ＰＦＢ ６　　　エンコーダ・、ビット／サーボ・チック１６０
８で表わされるジョ インド１の速度、即ち、ＶＦＩＮＯ ７−９ジョイント２に関する４−６ １０−１２ジョイント３に関する４−６１３−１５ジョ
イント４に関する４−６１６−１ｌｌ　　　ジョイント
５に関する４−６１！１−２１　　　ジョイント６に関
する４−６２２状態（エラー／ノー・エラー／ 指令エラー） ２３　　　　サーボ一致ワード（静／粗）２４−２５　
　　互う−状態（高及び低）２６　　　　人力タスク・
ビット・マスク（高ハイド）／完了状態ビット・ マスク（低バイト） トルク出力バッファは次のサーボ・チックに互って加え
られるトルクを含む。トルクは６８Ｋが変更を加えるこ
となく該当の装置（Ｄ　Ａ　Ｃ／Ｔ　Ｐボード）に直接
コピーできるようにフォーマットを与えられる。このバ
ッファの編成は簡単であり、下記の通りである。

ワード　　　　　　　　　　　　容 ０　　　ジョイント１に関するトルク出力１　　　ジョ
イント２に関するトルク出力２　　　ジョイント３に関
するトルク出力３　　　ジョイント４に関するトルク出
力４　　　ジョイント５に関するトルク出力５　　　ジ
ョイント６に関するトルク出力記録データ・バッファは
サーボ・サンプリング速度で記録される下記項目から成
るのが普通である。

ワード　　　　　　　　内　　容 ０　　　ジョイントＩＰＣＴ、３２０８　（高ワード）
で表わされた位置指令 １　　　ジ目インドＩＰＣＴ、位置指令（低ワード）（
ビット） ２　　　ジョイントｔｖｃ”ｒ、１ａｏｓ形式ＣＢ／チ
ック）で表わされた 速度指令 ３　　　ジョイントＩＶＦＢＦ、１６０８形式（Ｂ／チ
ック）で表わされた フィルタされた速度フィードバラ ク ４　　　ジョイント１に関する１６０５形式（ビット）
で表わされた位置エ ５　　　ジョイント１に関する３２０１６形式（高ワー
ド）で表わされた積 分エラー ６　　　ジョイント１に関する積分エラー（低ワード） ７　　　ジョイント１に関する１６０８形式（Ｂ／チッ
ク）で表わされた 速度エラー ８　　　ジョイント１．ＤＶ）ＩＡＴＣ。

１６０８形式（Ｂ／チック／ チック）で表わされた加速指令 ９　　　ジョイントＩＳＵＭ、ｔｓｏ。

形式で表わされた中間信号和 １０−１８　　　ジョイント２に関する０−８１９−２
７ジョイント３に関する０−８２８−３６ジョイント４
に関する０−８３７−４５ジョイント５に関する０−８
４６−５４ジョイント６に関する０−８包括通信セクシ
ョンはサーボ演算回路との間でデータを転送するための
包括的な読取／書込セクションとして利用される。

次いで、ブロック′４８８はトルク・プロセッサ・ボー
ド６００がデータ転送可能な状態にあるかどうかを判定
し、この状態になければエラー・シャットダウンが指令
される。しかし、常態ではＴＰボード６００は転送可能
状態にあり、ブロック４８９がトルク指令及び／または
その他のデータをＴＰボード６００に転送する。その他
のデータとしては、制御される特定ロボットに対応する
サーボ・ゲインや修正ゲインがあり、いずれも始動時間
中にダウンロードされる。次にブロック４９０．４９１
が同期的及びワンショット・データ記録ルーチンを実行
する。ワンショット・データ記録装置４９１はエラー発
生の場合種々のルーチンによりフラッグされて作動する
。

ＳＣＭデータ記録　オ 第１１Ｇ及び１１）１図にそれぞれ示すデータ記録装置
４９０．４９１はいずれも共用ＲＡＭ″メイルボックス
”インターフェースによって制御される。さらにまた、
いずれもサーボ・コードの実行に続くサーボ割込みルー
チンにおいて呼出される。従って、データを記録できる
最高速度は採用されるサーボ更新速度（現在は１ミリセ
コンド）によって決定される。ワンショット・データ記
録装置の場合、サブルーチン・コール（ＪＳＲｌｏｇｏ
ｎｅ）を介して呼出すこともできる。これは予期しない
エラ一時に存在する状態を（例えば、緊急停止の前に）
直ちに記録できるようにするために設けられる。

同期的データ記録装置のための共用ＲＡＭインターフェ
ースは下記のように構成される（アドレスはすべてベー
ス・アドレスからのオフセットで表わされる）。

１７土！ト　　　　サイズ　　　　　　　　　　　　　
容０　　　　８　ビット　　セマフォ（１−＞データ記
録）１−３　　２４ビツト　使用されない ４−５　　１６ビツト　チック・カウンタ始動６−７　
　１６ビツト　使用されない ８−９　　１６ビツト　チック◆カウンタ停止ａ−ｂ　
　　１６ビツト　使用されないｃ−ｆ　　　３２ビツト
　記録すべきアドレス・リストに対するポインタ（３２ ビット・データ転送） １０−１３　３２ビツト　ＳＣＭが記録データをデポジ
ットすべきメモリ中のセ クションに対するポインタ 注：上記マツプ中の使用されないセクションは将来削除
される。これらのセクションが存在するのは元のインタ
ーフェースが専ら長いワード量を使用したことに起因す
る。

非同期的ワンシミツト・データ記録装置のための共用Ｒ
ＡＭインターフェースは下記のように構成される（アド
レスはすべてベース・アドレスからのオフセットで表わ
される）。

オフセット　　　　サイズ　　　　　　　　　　内　　
容０　　８ビツト　ワンショット・トリガー（１・記録
、Ｏにリセット） １　　８ビツト　使用されない ２−３　　１６ビツト　綜合事象カウンタサーボ演算回
路では２つの基本的機能が行われる。第１に、ＶＡＬＣ
ＹＣＬＥ中の長チック間の３１チツクのそれぞれにおい
て、ダウンロードされた位置指令データが補間され、各
チックごとに位置指令データから速度及び加速指令デー
タが算出される。次に、位置、速度及び加速指令につい
て、さらに適用可能な、同時受信される位置及び速度フ
ィードバックについて、各軸ごとに、各チック後にサー
ボ演算が行われる。その結果、各チック後に、各軸ごと
に、トルク・プロセッサ・ボードによって実行されるト
ルク指令が算出される。サーボ演算回路によって実行さ
れる制御アルゴリズウの詳細を第３図に示した。

一以　　下　　余　　白− ［ｆｉｆ’Ｌ凪−フイードバック制御ループ動イ第１３
図には本発明の実施例である第３図の制御ループ構成を
簡略化したブロックダイヤグラムで示した。要約すれば
、ロボット指令は位置指令プログラム１２８によって各
軸ごとに形成され、サーボ制御（ＳＣＭ）コンピュータ
１１８に供給され、該コンピュータにおいて位置及び速
度制御が行われる。８０Ｍコンピュータ１１８は電流ま
たはトルク指令を形成し、これが設定値としてＴＰコン
ピュータ１６８．１７２（第３図）に供給される。電圧
指令はＴＰコンビエータ１７２からデジタル・パルス幅
変調（ＰＷＭ）回路に送られる。スイッチ制御信号がＰ
ＷＭから電力増幅器１７４に供給されてロボットのＤＣ
幅モータの動作を制御する。閉ループ制御は電流フィー
ドバック機能ブロックによって可能化される。第１３図
の構成におけるソフ、トウエア及び回路の構造及び動作
については既に詳述した通りである。

本発明ではまた、各軸モータから増分位置フィードバッ
クセンサ８１０と位置及び速度プロセッサ８１２を介し
て８０Ｍコンピュータ１１８へ位置及び速度フィードバ
ック信号が供給される。本発明のこの部分を以下に詳述
する。

増　位置フィードバックプロセッサ 位置エンコーダフィードバックを処理し、本発明のロボ
ット制御を可能にする回路８１２を第１４図にブロック
ダイヤグラムで示した。本発明では回路を簡略化するこ
とにより、機能性を高めると共にコストを軽減しながら
エンコーダのインターフェースを達成する。

回路８１２は高レベル・マイクロプロセッサが回路８１
２に対して読取／書込できるようにする指令レジスタ８
１４を含む。図示のように、マイクロプロセッサ・イン
ターフェース信号は下記の通りである。

九０−Ａ３　　　　　アドレス・ラインｃｓｂ　　　　
　チップ選択 ＲＤ　　　　　　読取 りＳｂ　　　　　　データ・ストローブＤＴＡＣＫｂ　
　　　データ確認 サンプル／タイムベース回路８１６はＡＩＦボード信号
ＳＡＭＰ及びＣＬＯ（：にに応答して回路８１２中のレ
ジスタのためのサイクル基準及びタイム基準信号を供給
する。人力及び出力データ・バス・バッファ８１８．８
２０は回路レジスタのための、ＶＭＥデータ・バスとの
データ・インターフェースをマイクロプロセッサの制御
下に提供す。る。割込み状態レジスタ８２２はエンコー
ダ・デコード信号及び指令レジスタ信号に応答して各イ
ンデクス・カウントに割込みを発生させる。　各回路８
１２は６つのロボット軸のうち３つの軸について位置フ
ィードバックエンコーダ信号を処理するように構成され
ている。即ち、位置及び速度関連データをデコード及び
レジスタするために３つのチャンネル８２４，８２６及
び８２８を設ける。図示のように、各チャンネルはデコ
ード回路８３０及びデータ・バス・インターフェース回
路８３２を含む。各チャンネルはまた、位置カウントす
るための増分カウント・レジスタ８３４、エンコーダ・
インデックス信号を記録するインデックス・レジスタ８
３６、及び速度演算に利用される最初及び最後のタイム
・レジスタ８３８．８４０をも含む。出力信号ＩＮＤＸ
Ｆはエンコーダ・インデックス信号と２つのエンコーダ
位相信号の論理ＡＮＤコンビネーショ゛ンである。各チ
ャンネル８３０はさらに、割込み状態レジスタ８２２及
び入出力データ・バス・バッファ８１８，８２０と接続
するデータ・バス・インターフェース８４２をも含む。

チャンネル８２４ｐついて第１５図に詳しく図示しであ
るように、デコード回路８３０は３つのエンコーダ信号
Ｐ）ＩＡ％ＢＨＢ及びＩＮＤＥＸに基づいて、３つの信
号のすべてが共存（第１６Ａ図）すれば信号ＩＮＤＸＦ
を形成し、さらに、方向信号Ｕ、Ｐ／ＤＮ及び位置変化
またはＩＮＣＲＥＭＥＮＴ侶号を増分カウンタ８３４の
ために形成する論理回路である。第１６Ａ図はデコード
回路８３０によってデコードされるエンコーダ信号°と
４つの状態との関係を示す。

エンコーダが１回転するごとにＩＮＤＥＸが発生する。

従フて、ＩＮＤＥＸが発生すると、実際のカウント数が
既知の毎回転カウント数と比較される。カウント数が正
しくなければ、例えばノイズによってエラーが発生した
ことを示唆する。

ＣＡＲＲＹ　ｔｌＰ及びＣＡＲＲＹ　ＤＯＷＮを記憶す
ることでレジスタ８３４の長さ、即ち、累算可能カウン
トのサイズが倍増する。）ＩＯＬＤｆＮＧ　ＲＥ（＋ｌ
５ＴＥＲ８４４は毎回転ＩＮＣＲＥＭＥＮＴカウント数
のチェックを可能にする。

各サンプリグ同期に１つ、２つ程度の位置変化カウント
しか発生しないほどモータ速度が低ければ、従来のフィ
ードバック方式の場合なら、位置変化を経過時間で割算
することによって速度を計算できる精度が著しく損なわ
れる。

本発明では、定速度においても従来よりも高い分解能で
位置変化データから速度を求めることができるように構
成して改良を実現する。一般に、デコードされた位置変
化パルス発生間の時間が測定される。経過時間カウンタ
８４６は信号ＳＡＭＰの制御下にサイクル時間カウント
を発生し、ＩＮＣＲＥＭＥＮＴ信号発生の経過時間を検
出する。

最初のタイム・レジスタ８２８はサンプリング・インタ
ーバル開始から次のＩＮ（：ＲＥＶＥＮＴパルス発生ま
での経過時間を記録する。最終タイム・レジスタ８４０
はサンプリング・インターバルにおける最終ＩＮＣＲＥ
ＭＥＮＴパルスの発生からサンプリング・インターバル
の終りまでの経過時間を記録する。

最初及び最終経過時間信号は実際の低いモータ速度を正
確に反映する速度計算のため、マイクロプロセッサ８１
４に転送される。第１７図の速度フィー、ドパツク演算
フローチャートにブロック８４８で示し、かつ第１６Ｂ
図に詳細に示したように、速度演算は逆時間概念に基づ
いて行われる。即ち、下記アルゴリズムを利用して速度
が算出される。

ＶＥＬＯＣＩＴＹ　−１７ＴＩＭＥ 高速度の場合には、マイクロプロセッサはブロック８５
０で示すように、位置変化ＩＮＣＲＥＭＥＮＴパルス数
）を経過時間（サイクル数）で割算することによって速
度を計算する。マイクロプロセッサはモータ速度の上昇
及び下降に伴なって低速アルゴリズムと高速アルゴリズ
ムの間をクロスする。なぜなら、モータ速度が上昇して
、個々のサンプリング周期における各サンプリング時間
中に発生するＩＮＣＲＥＭＥＮＴパルス数が増加すると
、低速アルゴリズムの精度が低下するからである。選択
ブロツク８５２においてクロスオーバが行われ、例えば
、サンプリング時間ごとのＩＮ（：ＲＥＭＥＮＴカウン
ト数が３以上なら高速アルゴリズムが選択される。

回路８１２は好ましくは破線ボックス８１３で示すよう
にＬＳＩチップの形で実施する。チップ８１２は半導体
ウェハ内にシリコン・マスク・デポジションによって複
数の別々のトランジスタが形成されている標準的な大規
模集積（ＬＳＩ）ゲート・アレイ・チップから構成する
。この場合、回路はＣＭＯ３３ミクロン、単一金属、１
５００ゲート・アレイ・チップの形に実施する。所要の
回路構成はこれを可能にする所定のパターンでトランジ
スタ・ゲートを相互接続するように位置／速度フィード
バック（ＰＶＦ）チップに特定金属マスクを配置するこ
とによって形成される。ロボットの３軸ごとに１つのＰ
ＶＦチップを使用すればよい、この実施例では、Ｕｎｉ
ｍａｔｉｏｎ　７００シリーズ−ｏボット、即ち、増分
位置フィードバックを有し、ＤＣＣブラシ台ジョイント
モータを採用するロボットと協働するように構成された
ＡＩＦボードに２つのＰＶＦチッ、ブを使用する。

【図面の簡単な説明】

第１図は本発明を利用するシステムによる制御下に従来
よりも進歩した、かつ正確な性能を発揮するロボットの
斜視図：第２図は本発明を組込むことのできる位置、速
度及びトルク制御ループを使用する制御ループ構成を略
示するブロックダイヤグラム；第３図は本発明を組込む
ことが好ましい位置、速度及びトルク制御ループを使用
する制御ループ構成の詳細なブロックダイヤグラム；第
４図は本発明のサーボ制御装置を含むロボット制御シス
テムを実施するように回路が構成されている電子ボード
の構成を示す概略図；第５図は本発明の多軸デジタル・
ロボット制御方式で動作するサーボ制御システムのブロ
ックダイヤグラム；第６−１及び第６−２割は第４図に
含まれ、かつ第５図に示したサーボ制御システムを本発
明の第１実施例として実施する電子トルク・プロセッサ
（ＴＰ）ボードの簡略図；第７Ａ　１−７Ａ３図は第２
図に示した制御ループ構成の詳細図；第７８１−７ＢＺ
図は第７Ａ図に含まれるトルク制御ループのダイヤグラ
ム；第８Ａ１−８Ｆ図はＴＰボードの回路に使用される
マイクロプロセッサにおけるプログラム実行を略示する
フローチャート；第９図はトルク・プロセッサ・ボード
動作のタイミング・チャート；第１０−１及び１０−２
図は第４図に含まれ、第５図のサーボ制御システムを第
２実施例として実施する電子サーボ制御（ＳＣＭ）ボー
ドの簡略図；第１１Ａ−１１Ｈ図はＳＣＭボード回路に
使用されるマイクロプロセッサにおけるプログラム実行
を略示するフローチャート；第１２−１及び１２−２図
はＳＣ％ＡＩＦ、ＳＣＭ及びＴＰボードのデータ流れ及
びタイミングを示すダイヤグラム；第１３図は本発明の
原理に従って構成された位置及び速度フィードバックシ
ステムを使用する簡単な制御ループ構成のブロックダイ
ヤ、グラム；第１４図はロボットの３軸に関するエンコ
ーダ信号を処理してこれを増分位置カウントに変換し、
より正確な軸速度を求める基礎とするための、第５図に
採用されてい°るフィードバックシステム回路の綜合的
でロックダイヤグラム；第１５図は３軸のうちの１つに
関連して第１４図をさらに詳細に示したブロックダイヤ
グラム；第１６Ａ図はエンコーダ信号、位置増分信号及
び速度関連時間信号の関係を示す波形図；第１６Ｂ図は
位置変化から速度を求める態様を示す波形図；第１７図
はプログラミングのための速度演算制御に利用できる動
作を示す機能ブロックダイヤグラムである。 ＦＩＧ、８８ ＦＩｏ、８Ｃ ＦＩＧ、８Ｅ ＦＩＧ、８Ｆ ＦＩＧ、１ｌＦ ＦＩＧ、ｌｌＧ　　　　　　　　　　　　　ＦＩＧ、ｌ
ｌＨＦＩＧ、１３ ＦＩＧ、１７ ＦＩＧ、＋４ ＦＩＧ、＋５

Claims (1)

1. 【特許請求の範囲】 １、それぞれがモータ駆動手段を有する複数のジョイン
   トを有するアームと；各モータに駆動電流を供給する電
   力増幅器と；前記ジョイント・モータ用の前記電力増幅
   器をそれぞれ制御するフィードバック制御ループ手段と
   より成り、前記フィードバック制御ループ 手段のそれぞれが連携の電力増幅器を制御する少なくと
   もデジタル位置及び速度制御ループを含み；さらに、す
   べてのジョイント・モータのための前記制御ループの少
   なくとも１つについて制御補助タスク及び計算タスクを
   行う少なくとも第１サーボ制御手段とを含むロボットに
   おいて、 前記第１サーボ制御手段が各ジョイント・ モータのための前記１つの制御ループについて記憶され
   ているアルゴリズムから出力制御指令を演算するなどの
   計算を行う第１マイクロプロセッサを含むことと； 前記第１マイクロプロセッサが比較的高い 演算能力及び比較的低いデータ処理インターフェース能
   力を有することと； 前記第１サーボ制御手段が前記第１サーボ 制御手段の動作を統括し、かつ各ジョイン ト・モータのための前記１つの制御ループにおけるサー
   ボ制御補助タスク、例えば、前記第１マイクロプロセッ
   サとの間の制御指令、状態及び帰還データのルーティン
   グを行うための第２マイクロプロセッサを含むことと；
   前記マイクロプロセッサが比較的高度のデ ータ処理能力を有することと； 各ジョイント・モータのための前記１つの 制御ループを前記第１サーボ制御手段が作動して前記１
   つの制御ループの制御変数を制御できるように前記第１
   及び第２マイクロプロセッサを相互にかつ高及び低レベ
   ル制御回路とインターフェースさせる手段を含むことを
   特徴とするロボット。 ２、前記第１サーボ制御手段が前記位置及び前記速度制
   御ループの双方のタスクを実行 し、前記第１マイクロプロセッサが両ループの演算タス
   クを実行し、前記第２マイクロプロセッサが両ループの
   制御補助タスクを実施することを特徴とする特許請求の
   範囲第１項に記載のロボット。 ３、前記低レベル回路が前記電力増幅器に伝送されるモ
   ータ電圧指令を発生させるための対応のトルク制御回路
   を含むことと； 前記ロボットが 前記第２マイクロプロセッサによって実行 される位置指令を発生する手段と； 各軸ごとにモータの位置及び速度を検出し て対応のデジタルフィードバック信号を発生する手段と
   ； 位置指令を前記第１マイクロプロセッサへ 送信し、前記第１マイクロプロセッサから演算されたト
   ルク指令を受信するように前記インターフェース手段を
   制御するため前記第２マイクロプロセッサを作動する手
   段と； 前記トルク制御回路にトルク指令を送信し てこれを実行させ、前記モータ電流フィードバック信号
   を受信するため前記第２マイクロプロセッサを作動する
   手段と； を含むことを特徴とする特許請求の範囲第１項に記載の
   ロボット。 ４、前記第２マイクロプロセッサ作動手段がサーボプロ
   グラム手段及びメイン・プログラム手段を含み； 前記サーボ・プログラム手段がデジタル制 御サンプリング速度（ＶＴＩＣＫ）で実行されて複数の
   機能、例えば、前記インターフェース手段から前記トル
   ク制御回路へのトルク指令ルーティング方向づけや、前
   記デジタル発生手段から前記インターフェース手段及び
   前記第１マイクロプロセッサへのモータフィードバック
   信号ルーティングの方向づけを実現し；前記メイン・プ
   ログラム手段が起動と同時 に位置／速度サーボ制御マイクロプロセッサ動作を開始
   させ、位置指令を受信してこれを前記第１マイクロプロ
   セッサのための前記インターフェース手段へ伝送する ことを特徴とする特許請求の範囲第１項に記載のロボッ
   ト。 ５、割込みプログラム実行速度が少なくとも２５０マイ
   クロ秒ごとに１回の速度であり、ロボットが少なくとも
   ６軸であることを特徴とする特許請求の範囲第４項に記
   載のロボ ット。 ６、各ジョイント・モータごとの前記フィードバック制
   御ループ手段がトルク制御ループをも含み、前記位置／
   速度制御ループからの順方向経路出力がトルク制御のた
   めのモータ電流設定値を提供することと； 各ジョイント・モータと連携する前記トル ク制御ループのための制御補助タスク及び演算タスクを
   行う第２サーボ制御手段を設けたことと； 前記第２サーボ制御手段が各ジョイント・ モータと連携する前記トルク制御ループのための記憶ア
   ルゴリズムからの出力制御指令演算などのような演算タ
   スクを行う第３マイクロプロセッサを含むことと； 前記第３マイクロプロセッサが比較的高い 演算能力及び比較的低いデータ処理インターフェース能
   力を具えることと； 前記第２サーボ制御手段が前記第２サーボ 制御手段の動作を統括し、かつ各ジョイン ト・モータと連携する前記トルク制御ループにおけるサ
   ーボ制御補助、タスク、例えば、前記第３マイクロプロ
   セッサとの間の制御指 令、状態及びフィードバックデータのルーティングを行
   う第４マイクロプロセッサをも含むことと； 前記第２サーボ制御手段が前記トルク制御 ループを作動して各ジョイント・モータのトルクを制御
   できるように前記第３及び第４マイクロプロセッサを相
   互にかつ高及び低レベル制御回路とインターフェースさ
   せる手段を含むことを特徴とする特許請求の範囲第１項
   に記載のロボット。 ７、前記第１サーボ制御手段が前記位置及び前記速度制
   御ループのためのタスクを実行 し、前記第１マイクロプロセッサが両ループの演算タス
   クを実行し、前記第２マイクロプロセッサが両ループの
   制御補助タスクを実行することを特徴とする特許請求の
   範囲第６項に記載のロボット。 ８、第３及び第４マイクロプロセッサに関連の前記低レ
   ベル回路がそれぞれのロボット軸と連携する前記電力増
   幅器に供給されるジョイント・モータ制御信号を発生す
   るデジタ ル・パルス幅変調手段を含むことと； ロボットが各軸ごとのモータ電流を感知 して対応のデジタル電流フィードバック信号を発生する
   手段と； トルク指令を受信してこれを実行し、状態 データを送信するため前記第４マイクロプロセッサを高
   レベル制御ルービングとインターフェースさせる手段と
   ； トルク指令を前記第３マイクロプロセッサ に送信し、演算された電圧指令を前記第３マイクロプロ
   セッサから受信するように前記対応のインターフェース
   手段を制御するため前記第４マイクロプロセッサを作動
   する手 段と； 電圧指令を前記パルス幅変調手段に供給し て実行させ、前記モータ電流フィードバック信号を受信
   するため前記第４マイクロプロセッサを作動する手段を
   含むこと を特徴とする特許請求の範囲第６項に記載のロボット。 ９、前記第４マイクロプロセッサ作動手段が割込みプロ
   グラム手段及びメイン・プログラム手段を含み； 前記割込みプログラム手段がデジタル制御 サンプリング速度で実行されて複数の機能、例えば、前
   記対応のインターフェース手段から前記パルス幅変調手
   段への電圧指令ルーティングの方向づけや、前記デジタ
   ル発生手段から前記インターフェース手段及び前記第３
   マイクロプロセッサへのモータ電流フィードバック信号
   ルーティングの方向づけを行うことと； 前記メイン・プログラム手段が起動と同時 にトルク・サーボ制御マイクロプロセッサ動作を開始さ
   せ、次いで待機指令モードで動作することにより、次の
   高さの制御レベルから受信された高優先順位指令の実行
   を管理することを特徴とする特許請求の範囲第６項に記
   載のロボット。 １０、前記高優先順位指令は診断及びパラメータ変更指
   令を含むことを特徴とする特許請求の範囲第９項に記載
   のロボット。 １１、割込みプログラム実行速度が少なくとも２５０マ
   イクロ秒ごとに１回の速度であり、ロボットが少なくと
   も６軸を有することを特徴とする特許請求の範囲第９項
   に記載のロボット。 １２、モータの位置変化を表わす複数の信号を発生する
   エンコーダ手段と； 前記エンコーダ手段に応答して、所定のサ ンプリング速度で前記フィードバック制御手段のための
   位置及び速度フィードバック信号を供給するデジタルフ
   ィードバックシステムと を含むと共に； 前記フィードバックシステムが前記エンコ ーダ手段信号に応答して位置及び動作の増分変化を表わ
   す増分パルスのカウントを前記フィードバック制御手段
   のための前記位置フィードバック信号として累算する手
   段を有する回路手段を含むことと； 前記回路手段が連続する増分パルス間の経 過時間を検出し、経過時間から速度を算出 し、前記フィードバック制御手段のための前記速度フィ
   ードバック信号を形成する手段をも有すること を特徴とする特許請求の範囲第１項に記載のロボット。 １３、基準時間に亙って発生する増分パルス数から速度
   を算出する手段を設けると共に、前記速度フィードバッ
   ク信号として使用するため２つの算出された速度の１つ
   を選択する手段を設けたことを特徴とする特許請求の範
   囲第１２項に記載のロボット。 １４、増分パルス発生速度に基づいて速度を選択し、比
   較的低い速度として経過時間速度を選択し、比較的高い
   速度として増分パルス速度を選択したことを特徴とする
   特許請求の範囲第１３項に記載のロボット。 １５、各ジョイント・モータごとの前記フィードバック
   制御ループ手段がトルク制御ループをも含み、前記位置
   及び速度制御ループからの順方向経路出力がトルク制御
   のためのモータ電流設定値を供給することと； 前記第１サーボ制御手段が前記位置及び速 度ループ及びトルク制御ループの少なくとも１つのため
   の制御補助タスク及び演算タスクを行うこと を特徴とする特許請求の範囲第１項に記載のロボット。 １６、前記ロボットが少なくとも６つのモータ作動ジョ
   イントを有することを特徴とする特許請求の範囲第１５
   項に記載のロボット。 １７、連携のモータによってそれぞれ駆動される複数の
   アーム・ジョイントを有するロボットのための、 各モータに駆動電流を供給する電力増幅器 と； 前記ジョイント・モータと連携する前記電 力増幅器をそれぞれ制御する対応のフィードバック制御
   ループ手段とより成り； 前記フィードバック制御ループ手段のそれ ぞれが連携の電力増幅器を制御する、少なくともデジタ
   ル位置及び速度制御ループを 含み； さらに、すべてのジョイント・モータと連 携する前記制御ループの少なくとも１つのための制御補
   助タスク及び演算タスクを行う少なくとも第１サーボ制
   御手段とを含むデジタル制御装置であって、 前記第１サーボ制御手段が各ジョイント・ モータと対応する前記１つの制御ループのため記憶され
   ているアルゴリズムから出力制御指令を算出するなどの
   演算タスクを行う第１マイクロプロセッサを含むことと
   ； 前記第１マイクロプロセッサが比較的高い 演算能力及び比較的低いデータ処理インターフェース能
   力を有することと； 前記第１サーボ制御手段がその動作を統括 すると共に前記第１マイクロプロセッサとの間の制御指
   令、状態及びフィードバックデータのルーティングなど
   、各ジョイント・モータと連携する前記１つの制御ルー
   プにおけるサーボ制御補助タスクをも行う第２マイクロ
   プロセッサをも含むことと； 前記第１サーボ制御手段が各ジョイント・ モータと連携する前記１つの制御ループを作動して前記
   １つの制御ループの制御変数を制御できるようにするた
   め、前記第１及び第２マイクロプロセッサを相互にかつ
   高及び低レベル制御回路とインターフェースさせる手段
   を含むことを特徴とするデジタル・ロボット制御装置。 １８、前記第１サーボ制御手段が前記位置制御ループ及
   び前記速度制御ループのためのタスクを行い、前記第１
   マイクロプロセッサが両ループのための演算タスクを、
   前記第２マイクロプロセッサが両ループのための制御補
   助タスクを行うことを特徴とする特許請求の範囲第１７
   項に記載のデジタル・ロボット制御装置。 １９、前記低レベル回路が前記電力増幅器に伝送される
   モータ電圧指令を発生するそれぞれ対応のトルク制御回
   路を含み； 前記デジタル制御装置が 前記第２マイクロプロセッサによって実行 される位置指令を発生する手段と； それぞれの軸についてモータの位置及び速 度を検出し、これに対応するデジタルフィードバック信
   号を発生する手段と； 前記第１マイクロプロセッサに位置指令を 送信し、前記第１マイクロプロセッサから算出されたト
   ルク指令を受信するように前記第２マイクロプロセッサ
   を作動して前記インターフェース手段を制御させる手段
   と； 前記トルク指令を前記トルク制御回路に送 信してこれを実行させ、前記モータ電流フィードバック
   信号を受信するように前記第２マイクロプロセッサを作
   動する手段と を含むことを特徴とする特許請求の範囲第１７項に記載
   のロボットのデジタル制御装置。 ２０、前記第２マイクロプロセッサ作動手段がサーボ・
   プログラム手段及びメイン・プログラム手段を含む； 前記サーボ・プログラム手段がデジタル制 御サンプリング速度（ＶＴＩＣＫ）で周期的に実行され
   、前記インターフェース手段から前記トルク制御回路へ
   のトルク指令ルーティングの方向づけや、前記デジタル
   発生手段から前記インターフェース手段及び前記第１マ
   イクロプロセッサへのモータフィードバック信号ルーテ
   ィングの方向づけなど、複数の機能を行うこと； 前記メイン・プログラム手段が起動と同時 に位置／速度サーボ制御マイクロプロセッサ動作を開始
   させ、位置指令を受信してこれを前記第１マイクロプロ
   セッサのための前記インターフェース手段に伝送するこ
   と を特徴とする特許請求の範囲第１７項に記載のロボット
   のデジタル制御装置。 ２１、割込みプログラム実行速度が少なくとも２５０マ
   イクロ秒ごとに１回の速度であ り、ロボットが少なくとも６軸を有することを特徴とす
   る特許請求の範囲第２０項に記載のロボットのデジタル
   制御装置。 ２２、各ジョイント・モータと連携するフィードバック
   制御ループ手段がトルク制御ループをも含み、前記位置
   及び速度制御ループからの順方向経路出力がトルク制御
   のためのモータ電流設定値となることと； 各ジョイント・モータと連携する前記トル ク制御ループのための制御補助タスク及び演算タスクを
   行う第２サーボ制御手段を設けたことと； 前記第２サーボ制御手段が各ジョイント・ モータと連携する前記トルク制御ループのため、記憶さ
   れているアルゴリズムからの出力制御指令算出などの演
   算タスクを行う第３マイクロプロセッサを含むことと； 前記第３マイクロプロセッサが比較的高い 演算能力及び比較的低いデータ処理インターフェース能
   力を有することと； 前記第２サーボ制御手段がその動作を統括 すると共に、前記第３マイクロプロセッサとの間の制御
   指令、状態及びフィードバックデータのルーティングな
   ど、各ジョイント・モータと連携する前記トルク制御ル
   ープにおけるサーボ制御補助タスクを行う第４マイクロ
   プロセッサをも含むことと； 前記第２サーボ制御手段が前記トルク制御 ループを作動して各ジョイント・モータのトルクを制御
   できるように前記第３及び第４マイクロプロセッサを相
   互にかつ高及び低レベル制御回路とインターフェースさ
   せる手段とを含むことを特徴とする特許請求の範囲第１
   ７項に記載のロボットのデジタル制御装置。 ２３、前記第１サーボ制御手段が前記位置制御ループ及
   び前記速度制御ループのためのタスクを行い、前記第１
   マイクロプロセッサが両ループの演算タスクを行い、前
   記第２マイクロプロセッサが両ループの制御補助タスク
   を行うことを特徴とする特許請求の範囲第２２項に記載
   のロボットのデジタル制御装置。 ２４、第３及び第４マイクロプロセッサと連携する前記
   低レベル回路が各ロボット軸と連携する前記電力増幅器
   に供給するジョイン ト・モータ制御信号を発生する対応のデジタル・パルス
   幅変調手段を含み、 ロボットのデジタル制御装置が 各軸のモータ電流を感知して対応のデジタ ル電流フィードバック信号を発生する手段 トルク指令を受信して実行し、状態データを送信するよ
   うに前記第４マイクロプロセッサを高レベル制御ループ
   に接続する手段と； 前記第３マイクロプロセッサにトルク指令を送信し、算
   出された電圧指令を前記第３マイクロプロセッサから受
   信するように前記第４マイクロプロセッサを作動して前
   記対応のインターフェース手段を制御させる手段と； 電圧指令を前記パルス幅変調手段に送信して実行させ、
   前記モータ電流フィードバック信号を受信するように前
   記第４マイクロプロセッサを作動する手段と を含むことを特徴とする特許請求の範囲第１７項に記載
   のロボットのデジタル制御装置。 ２５、前記第４マイクロプロセッサ作動手段が割込みプ
   ログラム手段及びメイン・プログラム手段を含み； 前記割込みプログラム手段がデジタル制御 サンプリング速度で周期的に実行されて、前記対応イン
   ターフェース手段から前記パルス幅変調手段への電圧指
   令ルーティングの方向づけや、前記デジタル発生手段か
   ら前記インターフェース手段及び前記第３マイクロプロ
   セッサへのモータ電流フィードバック信号ルーティング
   の方向づけなど、複数の機能を実現することと；前記メ
   イン・プログラム手 段が起動と同時にトルク・サーボ制御マイクロプロセッ
   サ動作を開始させ、しかるのち、待機指令モードで動作
   することにより、次に高い制御レベルから受信される高
   優先順位指令の実行を管理することを特徴とする特許請
   求の範囲第１７項に記載のロボットのデジタル制御装置
   。 ２６、前記高優先順位指令が診断及びパラメータ変更指
   令を含むことを特徴とする特許請求の範囲第２５項に記
   載のロボットのデジタル制御装置。 ２７、割込みプログラム実行速度が少なくとも２５０マ
   イクロ秒ごとに１回の速度であ り、ロボットが少なくとも６軸を有することを特徴とす
   る特許請求の範囲第２５項に記載のロボットのデジタル
   制御装置。 ２８、モータ位置変化を表わす複数の信号を発生するエ
   ンコーダ手段と； 前記エンコーダ手段に応答して所定のサン プリング速度で前記フィードバック制御手段のための位
   置及び速度フィードバック信号を供給するデジタルフィ
   ードバックシステムとを含み； 前記フィードバックシステムが位置及び動 作の増分変化を表わす増分パルスのカウントを前記フィ
   ードバック制御手段のための前記位置フィードバック信
   号として累算する手段を有する回路手段を含むことと； 前記回路手段が連続する増分パルス間の経 過時間を検出し、経過時間から速度を算出 し、前記フィードバック制御手段のための前記速度フィ
   ードバック信号を発生する手段をも有することを特徴と
   する特許請求の範囲第１７項に記載のロボットのデジタ
   ル制御 装置。 ２９、基準時間に亙って発生する増分パルス数から速度
   を算出する手段を設けると共に、前記速度使用するため
   ２つの算出された速度の１つを選択する手段を設けたこ
   とを特徴とする特許請求の範囲第２８項に記載のロボッ
   トのデジタル制御装置。 ３０、速度選択が増分パルス発生速度に基づいて行われ
   、低速として経過時間速度が、高速として増分パルス速
   度が選択されることを特徴とする特許請求の範囲第２９
   項に記載のロボットのデジタル制御装置。 ３１、複数のアーム・ジョイントを有するロボットのデ
   ジタル制御装置のための、 デジタル制御装置の動作に従ってロボット のアーム・ジョイントのそれぞれを駆動するモータと； モータの位置変化を表わす複数の信号を発 生するエンコーダ手段と から成るデジタル装置／速度フィードバックシステムで
   あって、 前記エンコーダ手段の信号に応答して位置 及び動作の増分変化を表わす増分パルスのカウントを位
   置フィードバック信号として累算する回路手段と； 前記回路手段が連続する増分パルス間の経 過時間を検出し、経過時間から速度を算出 し、速度フィードバック信号を発生する手段をも有する
   ことを特徴とするデジタル位置／速度フィードバックシ
   ステム。 ３２、基準時間に亙って発生する増分パルス数から速度
   を算出する手段を設けると共に、前記速度フィードバッ
   ク信号として使用するため２つの算出された速度の１つ
   を選択する手段を設けたことを特徴とする特許請求の範
   囲第３１項に記載のシステム。 ３３、速度選択が増分パルス発生速度に基づいて行われ
   、低速として経過時間速度が選択され、高速として増分
   パルス速度が選択されることを特徴とする特許請求の範
   囲第３２項に記載のシステム。 ３４、前記回路手段が得られるように大規模集積ゲート
   ・アレイ・チップを構成したことを特徴とする特許請求
   の範囲第３１項に記載のシステム。 ３５、前記回路手段が前記位置及び速度制御ループの所
   定サンプリング速度と同期させたサンプル・パルスを周
   期的に発生する手段をも有することと； 各サンプル周期の開始を表わすサンプル・ パルスから前記サンプル周期における第１増分パルス開
   始までの経過時間を検出する手 段と； 各サンプル周期における最終増分パルスの 終了から、前記サンプル周期の終了を表わすサンプル・
   パルスまでの経過時間を検出する手段と； 連続する最終及び第１増分パルス間の経過 時間から速度を算出し、前記フィードバック制御手段の
   ための前記速度フィードバック信号を発生する手段とを
   特徴とする特許請求の範囲第３１項に記載のシステム。 ３６、前記回路手段が得られるように大規模集積ゲート
   ・アレイ・チップを構成したことを特徴とする特許請求
   の範囲第３５項に記載のシステム。