JPH01274980A - ロボット制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 タル・ロボット制御に係わる。

ロボット・アームの運動を制御する際には、カスケード
接続されている位置、速度及びトルク制御ループの制御
下に指令パスが実行される。各ジヨイントに対応するト
ルク制御ループからの出カイ３号がモーター駆動指令と
して作用し、ジョイント・モーターをして位置、速度及
び加速指令を満足させるのに必要なトルクを発生させ、
指令されたジヨイント・パスに亘ってジヨイントを移動
させる。。

速度変化の過程で、即ち、停止状態から回転速度への加
速、回転速度から停止状態への減速、及び回転速度変化
の過程で、制御ループからトルク指令信号が発生し、こ
れによってモーター駆動指令が形成されるようにしなけ
ればならない。位置及び速度エラーのみに基づいてトル
ク指令を形成すると、特に始動や停止などのように速度
変化が大きい過程では実際の位置エラーが比較的大きく
なる。即ち、ロボットの動作が不正確になり、間違った
パスを辿ることになる。停止エラーも大きくなり、オー
バーシュートが起こる可能性がある− 従来はそれぞれのジヨイント制御に指令加速に基づくフ
ィードフォワード・トルク信号及び連携のリンクに関す
る慣性定数を利用することによつてトルク発生に伴なう
ラグに起因する位げの不正確さをある程度軽減している
。このようなフィードフォワード制御は本願と同じ譲受
人に譲渡されている米国特許第４，７６３，０５５号及
び第４．７８６．８４７号が開示したようなディジタル
・ロボット制御システムなとでも採用された。

し・かし、フィードフォワード加速調節をもってしても
充分な制御精度は得られなかった。その主な原因は公知
技術における同調定数、即ち慣性が制御システムによっ
て操作される特定ロボットに関して求められた測定値に
基づく固定パラメーターにほかならないからである。実
際には、リンク慣性は負荷やアーム形状に応じて異なる
。公知技術にあっては、アーム形状が最悪の場合、フ４
−ドパツク・ゲインと加速指令を折衷しなければならな
いのが普通である。このように、公知技術はパス精度を
ある程度まで改善したが充分とはいえず、その理由はバ
ス・エラー原因の多様性と取り組むための適応性を欠い
ていたことにある。

し 本発明の目的は適応性に冨む好ま警くはディジタル方式
のフィードフォワード・トルク制御でロボット制御精度
を高めることにより多軸ディジタル・ロボット制御技術
を進歩させることにある。

発明の１刃 この目的にかんがみ、本発明はロボット・アーム・ジョ
イントのそれぞれを駆動するモーターと； 各モーターに駆動電流を供給するための電力増幅器と； 各ジョイント・モーターに設けられ、位置指令（ＰＣＯ
Ｍ）に従ってトルク制御ループを駆動することにより連
携の電力増幅器を制御するためのモーター指令（ＴＱＣ
ＯＭ）を発生させる位置制御ループ（ＰＶＢ）及び速度
制御ループ（ＶＥＢ）を含むフィードバック制御ループ
と；前記ジョイント・モーターの動作を感知し、位置及
び速度指令とそれぞれ組み合わせることにより、対応の
位置及び速度制御ループから前記トルク制御ループのそ
れぞれに対するトルク指令としてエラー信号を形成する
ための位置及び速度フィードバック信号を発生させる手
段 前記ロボッｉ・・ジヨイントの少なくとも所定の１つに
加わる負荷力を感知する手段と：前記負荷力感知手段か
らの出力に基づいてそれぞれのロボット・ジヨイントご
とに実負荷モーメントを計算する手段と； ロボット・アームに関して記憶されているダイナミック
／キネマチック・データに基づいてそれぞれのロボット
・ジヨイントごとにダイナミック／キネマチック・デー
タ・モーメントを計算する手段と； データ・モーメント及び負荷モーメントを複合し、前記
トルク制御ループのそわぞれに対する前記トルク指令を
形成する際に前記位置及び速度エラーと組み合わせるべ
くフィードフすワ・−ド・トルク（ＣＯＭＰＴ）として
前記複合モーメントを供給する手段と； トルク・エラー信号を形成するため各トルク制御ループ
においてトルク指令と組み合わされるジョイント・モー
ター駆動電流を表わすフィードパロボット制御システム
は各ロボット・ジョイント・モーターに駆動電流を供給
する電力増幅器、ロボット・ジヨイントの少なくとも所
定の１つに加わる負荷力を感知する手段、及び負荷力感
知手段からの出力に基づいて各ロボット・ジヨイントご
とに実際の負荷モーメントを計算し、ロボット・アーム
に関して記憶されているダイナミック及びキネマチック
・データ・モーメントを計算する手段を含む。ジヨイン
トごとに前記データ及び負荷モーメントを複合してその
フィードフォワード・トルクを形成する。各ジョイント
・モーターのためのフィードバック制御ループ手段は位
置指令及びフィードフォワード・トルクに従って連携の
電力増幅器に対するモーター指令を形成するトルク制御
ループを含む。

第１図に好ましい実施態様として示すように、ロボット
制御システム１０は本発明の改良式ロボット操作を可能
にする適応フィードフォワード・１−　ｋ−＋り制御装
置を含む。図示のような制御装置をそれぞれのロボット
軸に使用し、すべての軸の動作プランナー１２は指令さ
れたジヨイント動作を決定する逐次位置指令を出力する
０位置制御装置１４はそれぞれの位置指令とセンサー１
５からの位置フィードバック信号から位置エラーを求め
る。

速度指令は位置指令から形成されて速度制御装置１６に
供給される。速度制御装置１６はそれぞれの速度指令と
、位置フィードバックから得られる速度フィードバック
から速度エラーを求める。

フィードフォワード・トルク指令システム１８は本発明
の特徴である位置、速度及び加速指令に対する迅速かつ
正確な対応を可能にするトルク指令を適応的に出力する
。トルク指令は位置指令や、好ましくはＲ端（リス）−
）ｌ：ｆｆポット・ジヨイントに配置されるゆンサー１
９からのフィードバック力などのような人力から計算さ
れる。トルク指令計算の過程において実際のリスト力に
応答することにより、フィードフォワード・トルク指令
システムはアーム慣性の相違、即ち、ロボット負荷及び
ロボット・アーム形状の相違に応じたトルク指令を出力
する。これによってロボットの適応を縮小し、ｍ動作の
精度を高めることができる。

位置及び速度エラーとフィードフォワード・トルク指令
が加算されてトルク制御装置８に供給される。ジョイン
ト・モーター駆動指令は参照番号２１で示すようにトル
ク指令及び電流フィードバックに基づいて形成され、ジ
ョイント・モーターの電源ブロック（増幅器）に供給さ
れる。こうしてモーター電流を制御することによってよ
り高いレベルの指令に対応するのに必要なジヨイント・
トルクを発生させる。

参照番号１９で示すように、計算された実際のモーター
・トルクを動作プランナー１１２に供給することでさら
に好ましい結果が得られる。この動作プランナー１２は
実トルクと既知の利用可能トルクとの関係に基づいて次
のサイクルの速度／加速度を増減する。

・モーターに使用することが好ましいディジタル制御ル
ープ構成１００を示した。この制御ループ構成１００は
完全にディジタル方式の制御装置として実施することが
好ましい。階層構造、マルチプロセッサー構造、及び浮
動小数点ハードウェアについては後述するが、上記引用
米国特許及び関連の米国出願のほか、これらに引用され
ているいくつかの特許にも記載されている。トラジエク
トリー・サイクルは使用されるモジュール構成にもよる
が３２乃至８ミリセコンドのサイクル・タイムとなるよ
うに設定すればよい。

好ましい制御ループ構成１００では位置制御ループ１２
２及び速度制御ループ１２０がトルク制御ループ１２４
の入力に並列関係である。ブロック１２８が受イ３１ノ
た位置指令に基づいてブロック１２６が速度指令を出力
する。

この実施例では、ロボット制御システムのモジュール構
成に応じて８乃至３２ミリセコンドに一度ずつ速度ルー
プ１２０において速度指令が形成される。以下に述べる
基本的なロボット制御システムのトラジェクトリー・サ
イクル・タイムは３２ミリセコンドであるが、さらに性
能を高めた制御システムではトラジェクトリー・サイク
ルが８ミリセコンドとなる。

いずれにしても、速度指令発生ブロック１２６は速度フ
ィードバック・バス１４０における速度フィードバック
・サンプリング速度に対応する毎ミリセコンドに１個の
割合で速度指令を補間する。図示のように、Ｕｎｉｍａ
ｔｊｏｎ　８６０　ロボットの場合、速度フィードバッ
クはコンバーター１４２によってＡＤ変換されるタコメ
ーター信号によって形成される、スケーラ−１４４及び
フィルター１４６が速度ノイードバック回路を補足する
。

同様に、位置制御ループ１２２においては補間回路１４
８がフィードバック・バス１５０における位置フィ・−
ドパツク・サンプリング速度に対応して毎ミリ秒ごとに
位置指令を出力する。ｔｌｎＬｍａｔｉｏｎ８６０ロボ
ットの制御においでは、位置フィードバックは絶対であ
り、速度及び位置フィードバック・バス１４０．１５０
は（図示のスイッチ１５１を介して）図示のように作用
する。ＬＩｎｉｍａｔｌｏｎ　ＰＵＭＡロボットの場合
、タコメーターは利用されず、詳しくは引用の出願Ｗ、
Ｅ、５３，２２５号及びＷ、Ｅ、５３，３６８号に記載
されているように、速度フィードバックは（スイッチ１
５１が他の位置へ移動することで）ブ吋ｌり１５２で示
すように増分位置フィードバックから計算される。

フィードフォワード・トルク指令はブロック１３０にお
いて計算され、制御ループによって実行され、その結果
、ロボットが指令されたバスに従ってより正確に動作す
る。フィードフォワード・トルク指令発生ブロック１３
０におけるアルゴリズムはダイナミック及びキネマチッ
ク・データを利用し、ブロックｉ２９が位置指令１２８
から得た人力と、センサー１３２からのりストカフィー
ドバックを演算することによりフィードフォワード・ト
ルク指令を出力する。

加算器１５４はボックス１５６から供給されるゲインで
速度エラーを形成し／、同様に、加算器１５８はボック
ス１６０から供給されるゲインで位置エラーを形成する
。

速度及び位置エラーとフィードフォワード・トルク指令
が加算器１６２において加算される。以下の説明でさら
に明らかにするが、ロボット位置指令を満たすようにジ
ョイント・モーター駆！ａ電流が供給されるから、極め
て正確なトルク・フィードフォワードを実行することで
速度及び位置エラーを縮小することかできる。こうｌノ
で性能が著しく改善される。

ボックス１６８から供給さｔＩだゲインがトルク指令を
形成し、これが１ミリ秒ごとにトルク制御ループ１６４
の人力に供給される。加算器１６８はトルク指令（モー
ター電流指令）をフィードバック・バス１７０からの電
流フィードバックと加算することによりトルク・エラー
を形成する。ボックス１７２はトルク・エラー（モータ
ー電圧指令）にトルク・ループ・ゲインを加え、パルス
幅変調（ＰＷＭ）出力信号がロボット・ジヨイント操作
のためのモーター駆動電流を供給する電力増幅器１７４
に供給される。抵抗器１７５からの電流フィードバック
が２５０マイクロセコンドごとにサンプリングされ（引
用特許出願Ｗ、Ｅ、５３，３２４号に対応する米国特許
第４，７６３，０５５号参照）、ボックス１７８による
スケーリングでボックス１７６によってディジタル信号
に変換される。

１五豆！ｌ亘■Ｉ 第３図に示すロボット制御システム３０のための制御ル
ーピングは複数の電子制御盤上に配列されたディジタル
制御回路を利用することによって実施することができる
。制御盤上の回路を組織化し、種々のマイクロプロセッ
サ−間のプログラミングを区分することにより、１）製
造コストの低いモジュール制御構成でロボット制御性能
を高め、２）汎用性と制御性能レベル選択の融通性を可
能にする構成の可変性を容易にすることができる。

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

アーム・インターフェース（ＡＩｒ）ＩＩＪｍｆｆｔ１
８００は一般に特定の制御盤とではなく２つ以上の制御
盤と関連する例えばＶＭＥバス制御回路のような包括的
回路をも収容する。

ロボット・ジョイント・モーターにモーター電流を供給
する電力増幅器ブロック１５０を制御するタメ、Ａ　ｘ
　ｐＨｄＪ御盤ａｏｏ　カラＩＪｍｆｆ１号（パルス副
変調）が出力される。ＡＩＦ制御盤８００は参照番号１
５２で示す作業セル内の他のロボット制御装置、区域ネ
ットワーク内のプログラマブル・コントローラー及びそ
の他の入／出力装置１５３、及び監視制御用高レベル・
コンビ３１−ター１５４にロボット制御システム３０を
外部結合するチャンネルとしても作用する。

基本トルク・プロセッサー（ＴＰ）盤６００及びサーボ
ｉｉｌＪｍ盤４ｔｌＯはあらゆるタイプのロボットに対
するあらゆるロボット制御システムにおいてＡＩＦ制御
盤ａＯＯ及び電力増幅器ブロック１５Ｇと併用される包
括的回路盤である。３つの回路盤４００、制御システム
・シリーズだけでなくその他のロボット制御システムに
とっても基本制御構成を形成する。

基本トルク・プロセッサー盤６００はサーボ制御盤４０
０からの指令に呼応してモーター・トルクを巧、！］御
し、サーボ制御盤４００はロボット制御プログラムに従
ってアームの解を求め、その位置及び速度を制御する。

本発明の実施に際しては、サーボ制御盤４００に補足フ
ィードフォワード・トルク・プロセッサーＣＦＦＴＰ）
盤６００Ａをリンクする。ＦＦＴＰ盤６０盤上００であ
るが、異なるソフトウェア、即ち、ＴＰ盤６００Ａをフ
ィードフォワード・トルク指令発生器として作用させる
ソフトウェアを具えている。−船釣には、ＦＦＴＰ盤６
００Ａはロボット・ジヨイントに対する動作指令を満た
すのに必要な一連のトルクを計算する。計算されたトル
クはサーボ制御盤４００に伝送され、それぞれのジヨイ
ント制御装置へのトルク指令チャンネルにおける゛フィ
ードフォワード・トルク指令として実施される。

補足的な電子制御盤または電子制御デバイスをきる０例
えば、システム制御盤５００を追加し、アームの解を求
めるステップを含めてサーボ制御盤・１０（ｌからシス
テム制御盤５００までの所定のプログラム機能を区分す
れば、ＵＮＩＶＡＬ制御システムはロボット２０及びそ
の他のロボットを、はるかに迅速な制御作用で、即ち、
トラジェクトリー・サイクルを３２ミリセコンドから８
ミリセコンドに短縮して操作することができる。

制御などを目的とする盤間のデータ通信はＶＭＥバス１
５５中の多重信号バスを介して行なわれる。ほかに、ト
ルク・プロセッサー盤６００とＡＩＦ制御盤８００とを
結ぶＶＭＸバス１５６をも設ける。

ＡＩＦ％ＴＰ及びＳＣＭ盤のほか、接続可能なその他の
ユニットに（第３図には示してないが）多重ビン・イン
ターコネクターを設けることにより、ＶＭ３及びＶＭＸ
盤間バスの接続を、従つて、ロボット制御システム３０
のためのモジュール式制御盤集合体の形成を容易にする
。ＡＩＦ制御盤８００には外部的な入／出力接続のため
のコネクターも設ける。

制御盤回路構造に関しては、さらに詳しく後述するが、
米国特許第４゜７６：ｌ、０５５号及び第４．７８６，
８４７号、及びこれらと関連の米国出願やこれらに引用
されている特許をも参照されたい。ロボット制御システ
ムｌＯは詳しくは上記参考文献ロボティック・ネットワ
ークとして作用させることができる。

基本トルク・プロセッサー（ＴＰ）盤６００はロボット
・ジョイント・モーターに対する機能インターフェース
を構成する。機能的には、基本ＴＰ盤６００は階層制御
システムにおける最低レベルの制御を実施し、６つのロ
ボット軸を閉ループ・す−ボ・トルク制御する。物理的
には、この基本ＴＰｉ６００はロボット・バス・ブラン
−；ング制御システム及びサーボ制御（ＳＣＭ）盤をア
ーム・インターフェース（ＡＩＦ）制御盤８００と電気
的にインターフェース接続し、ＡＩＦｌＪ１８盤ａＯＯ
はロボット・ジヨイント駆動モーターとインターフとに
ある。

基本ＴＰ盤６００は１つのレベルにおいて３０Ｍ盤とイ
ンターフェースし、３０Ｍ盤から６軸に対するトルク指
令及びサーボ・パラメーターを受信し、状態データーを
返送する。基本ＴＰ盤６００はい 前記レベルよりも低孕第２レベルにおいてＡＩＦ盤８０
０とインターフェースし、６つのロボット軸に関ｌノで
サーボ電圧指令を出力する。ＡＩＦ盤８００はＳＣＭ及
びＴＰ盤を閉ループ制御するためのモーター駆動電流、
位置及び速度フィードバックを受信する。

基本ＴＰ盤６００は対マイクロプロセッサ−を利用して
下記のような多様な特徴と提供する。

１、ブラシ及びブラシレス・モーターに対する６軸トル
ク・ループ制御（２５０マイクロセコンド／６軸）； ２、ソフトウェアで調節可能な電流オフセット−電位差
計不要； ３、ダウンロード可能ゲイン−アーム従属パラメータ・
−を３０Ｍ盤からダウンロードできる；４、ＰＷＭ補償
： ５、計算補償； ６、データ・ロギングなどのための電流平均化；７、安
全のための電流リミット・チエツク；８、安全チエツク
のための速度モニタリング（逆起電力）； ９、停動条件をテストするためのエネルギー・チエツク
（ＷＩＴ）； １０、パワーアップ自己診断：及び １１、ダウンロード可能な診断システム。

基本トルク・プロセッサー盤 運動中のアームの作業点位置を指令軌道に従って制御す
るため、アームの作業点に加わるトルクをディジタル制
御することによりロボットの性能を高める。実際に経験
ずみのワークピースに基づいて軸駆動力を調節すること
により、位置及び軌道指令をより迅速に、正確に、能率
的に満たす。

基本トルク・プロセッサー盤６００は荷搬重量、駆動方
式、軸数などの異なる多様なロボットをトルク制御する
のに利用できる電子制御盤としての包括的制御回路盤６
００（第３図、第６Ａ−１図、第６Ａ−２図）である。

基本ＴＰ盤６００はディジタル回路を利用してより高い
制御レベル（３０Ｍ盤）から得られるトルク指令、及び
アーム・インターフェース（ＡＩＦ）盤８００を介して
軸駆動手段から得られるフィードバック電流に基づいて
各ジョイント・モーターまたは軸駆動手段に対する電圧
指令を出力する。

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

ディジタル・トルク制御回路はサンプリング速度条件の
範囲内ですべての軸に必要な制御計算及び制御支援機能
を正確かつ能率的に達成できるように複数のディジタル
・プロセッサーで構成するのが好ましい。

具体的には、ＳＣＭ　（サーボ制御モジュール）及びＴ
Ｐ　（）ルク・プロセッサー）ＩＪ御リレベル間記憶ト
ルク制御データ交換を可能にするデュアル・ボート５０
Ｍインターフェース・メモリー６０４とトルク制御マネ
ジャー６０２がインターフェース関係にある。好ましく
はＶＭＥタイプのデータ・バス６０８を介してＳＣＭか
らＴＰエンターフェース・メモリー６０４へ軸トルク指
令及び制御ループ・パラメーターがダウンロードされる
。帰路に、状態データがサーボ制御レベル（ＳＣＭ）ヘ
アツブロードされる。ＴＰ及びＳＣＭ盤間のインターフ
ェース・メモリー６０４はＶＭＥバス［ｉ０６に従属す
るデュアル・ポート共用メモリーである。

ほかに、ピンポン・メモリー６０８、プログラムＥＰＲ
ＯＭ６１４．局部ＲＡＭ６１６、第６Ａ−２図のＴＲカ
ルキニレ−ター・メモリ６１１などがある。

トルク制御マネジャー６０２はまた、トルク制御ル・−
ブ動作のため１階層低い制御レベルのＡＩＦ盤８００の
回路からトルク・プロセッサー盤６０１１へ電流をフィ
ードバックさせる。トルク制御側算の結果形成された駆
動電圧指令がト・ルク制御マネジャー６０２によってア
ーム・インターフェース（ＡＩＦ）盤８００へ送られる
。ピンポン（バンク・スイッチド）メモリー６０８はハ
ンドシェーク・フラグの制御下に指令、フィードバック
及び状態データを記憶し、トルク制御計算、高制御レベ
ル・リポート要求または軸駆動制御に際して必要となフ
た時、これらを利用で咎るようにする。

ディジタル信号プロセッサーの形で設けたコプロセッサ
−６１０はトルク・ループ・カルキニレ−ターとして作
用し、ピンポン・メモリー６０８を介してトルク制御マ
ネジャー６０２からトルク指令及びフィードバック電流
を受け、トルク指令及びフィードバック電流から計算さ
れたトルク・エラーに基づいて各ロボット軸に対する駆
動電圧指令を計算し、トルク制御マネジャー６０２から
の指令に基づ咎、ピンポン・メモリー６０８を介して駆
動電圧指部をアーム・インターフェース回路へ転送する
。

このようなデジタル回路構成では、必要なすべてのトル
ク制御機能を、周波数帯域幅条件の範囲内で迅速に（２
５０マイクロセコンド以上のサンプリング速度で）かつ
正確に行なうことができる。

具体的には、ディジタル信号プロセッサー６１０の迅速
な計算能力をトルク制御計算に利用し、トルク制御マネ
ジャー６０２のデータ整理及び伝送能力をその他の大部
分の機能に利用することにより、著しく高められた制御
性能を能率的かつ経済的に達成することができる。

トルク制御マネジャー６０２はデータ管理に関連する作
業に好適な構造を具えているが、その計算速度（即ち、
１６Ｘ１６ビツト乗算に４マイクロセコンド以上）はト
ルク制御帯域幅条件を満たすには低過ぎる。ディジタル
信号プロセッサー６１０はＺ変換計算（即ち、１６Ｘ　
１６ビツト乗算に２００ナノセコンドの計算速度）には
好適であるが、データ・マネジャー・プロセッサー［ｉ
０２に割り当てられているような作業には概して不適当
である。これら２つのマイクロプロセッサ−は１つのユ
ニット、換言すればサーボ・エンジンとして機能する。

基本ＴＰ盤回路構成の詳細については、米国出願第９３
２．９７７号（Ｗ、Ｅ、第５３．４２３号）または米国
出願第９３２，９９２号（１１１，Ｅ、第５３，２２６
号）の１９８６年１１月り２０日付記出願及びｉａ続出
願を参照されたい。

フィードフォワード・トルク・プロセッサー盤フィード
フォワード・トルク・プロセッサ（ＦＦＴＰ）盤６００
Ａを第７Ａ−１図及び第７Ａ−２図に示した。ハードウ
ェアに関しては基本ＴＰ盤６００と全く同じであり、従
って、ハードウェア素子には同じ参照番号を付しである
。

しかし、ソフトウェアの相違でＦＦＴＰ盤６００Ａは基
本ＴＰ盤６００と異なる態様で機能する。従つて、プロ
セッサー及びデュアル・ボート・インターフェース・メ
モリーには、ソフトウェアの違いから機能が異なること
を示唆するため基本ＴＰ盤６００とは異なる参照番号を
イ」シた。

トルク・エステイメーター６００Ａは記憶されているキ
ネマチック及びダイナミック・データを実ジヨイント・
トルクに基づいてすべてのロボット・ジヨイントについ
て実トルクを計算するコプロセッサ−である。

フォルク・フィードフォワード（ＴＦＦ）６０２Ａはデ
ュアル・ボート・トルク・フィードフォワード制御デー
タ・メモリー６０４Ａとインターフェースすることによ
り、ＳＣＭ盤４００及びＦＦＴＰ盤６００Ａ間のインタ
ーフェース関係を可能にす口 る。ピンポン・メモリーへのダウンＷ−ド・データは位
置指令及び実ジヨイント力信号などであり、アップロー
ド・データは計算トルク値信号などである。

Ｔ　Ｆ　Ｆ’マネジャー６０２Ａはまた、トルク・エス
テイメーター　６１０Ａからデュアル・ボート・メモリ
ー６０４Ａ及びＳＣＭ盤４００へ５１算トルク値な転送
し、ＳＣＭ盤４００及びデュアル・ボート・メモリー６
０４Ａからトルク・エステイメーター６１ＯＡへ実ジヨ
イント力を転送する。

基本トルク制御プロ灼ユゑ 基本トルク・プロセッサー盤６００はトルク制御マネジ
ャー６０２及びトルク・ループ・カルキュレータ−６１
０において実行されるプログラムの制御下に操作されて
、上位のＳＣＭ制御レベルからのトルク指令４３号を実
施する。

基本トルク・プロセッサー・ソフトウェアは下記のよう
Ｃ区分された作業を行なう。

トルク制御マネジャー６０２ ＳＣＭ及びフィードフォワード・トルク制御盤６００Ａ
との交信 指令処理 電流のザンブリング、変換及びオフセット調整転流スイ
ッチ・フラグ（状態読み取り）ビンボン・メモリー管理 Ｐ　Ｗ　Ｍグーツブ管理 診断 エラー報告 トルク・ループ・カルキニレ−ター６１０（２５０マイ
クロセコンド割り込みに基づくプログラム・サイクリン
グ） 過電流チエツク−絶対値及び平均値 トルク・ループ計算 電流平均化 ＰＷＭ補償 転流補償 逆起電力チ覧ツクー安全のため速度をモニターエネルギ
ー・チエツク−停動条件に関するテスト ＴＰソフトウェアの構造及び作用については、米国特許
第４，７６３，０５５号（Ｗ、Ｅ、第５３，２２７号）
に詳述されている。

トルり・フィードフォワード・マネジャ・−’６０２　
Ａが使用するプログラミング６５０Ｆのフローチャート
を第５図に示した。

ジーア 始動ブロシーシ’Ｆ６５２Ｆはロボット・モーターを実
際に始動させる前にプログラム実行条件を初期設定する
。初期設定が完了すると、ブロック６５４Ｆがトルク・
エステイメーター６１０Ａに対して、トルク計算が可能
であることを通告する。

次いでブロック　６５６Ｆはカレンｌ−４ｍ５チツクが
完了したかどうか（各３２ｍｓサイクルに８つの４ｍｓ
チックが含まれる）を検知する。４ｍｓが経過するごと
に６５８Ｆは１組のフィードフォワード・トルクが計算
されたかどうかを検知する。常態では、トルク計算には
２．９ｍｓを要し、従って、各チック終りまでにトルク
計算が完了ｌノていることになる。チックの終りまでに
トルク計算が完了していなければ、ブロック　６８０Ｆ
が点滅信号でエラーを指示する。

トルク計算の完了が検知されると、ブロック６６２Ｆは
ＴＦマネジャーの制御下にピンポン・メモリーを介して
トルク計算値をＳＣＭ盤４００に伝送するように命令し
、実ジヨイント力をＳＣＭ盤３００からトルク・エステ
イメータ・−６１０Ａへコピーするように命令し、（ト
ルク・エステイメーター　６１０Ａにおいて）サブルー
チンＣＭＤＴ　Ｉ　Ｃを実行して、線形補間により位１
、速度及び加速指令を更新するように命令し、ブロック
　６５６Ｆにおける次のチックに対してトルク・エステ
イメーター　６１０ＡなＧＯにセットする。

各サイクルの終了をブロック　６６４Ｆを検知すると、
即ち、３２ｒｎｓの最後の４ｍｓチックが終わると（こ
の時点において新しいサイクルに対応の新しいＳＣＭ位
置指令が現われる）、ブロック６６６Ｆ及び６６８Ｆが
ブロック６５８Ｆ及び６６２Ｆと同様に機能する。ただ
し、ブロック　６６８Ｆは新しいサイクルにおける新し
い補間を開始し、新しいＳＣＭ位置指令から補間するこ
とにより新しいサイクルの第１チツクのための位置、速
度及び加速指令を形成する（トルク・エステイメーター
６１ＯＡにおいて実行される）サブルーチンＣＭＤＥＸ
Ｔを命令する。

ｌ・ルク・エステイメーター６１ＯＡが使用するプログ
ラミング６７０　Ｆのフローチャ・−トを第４図に示し
た。

ブロック　６７２Ｆにおいて定数、メモリー・スペース
などが所期されたら、既に述べたようにＴ　Ｆマネジャ
−６０２Ａによって命令されたの結、　ＣＭＤＥ×Ｔ＊
たはＣＭＤＴＩＣが実行される。

次に、ブロック　８７４Ｆはトルク・フィ・−ドフォワ
ード・アルゴリズムを実行するため、記憶さねているキ
ネマチック／ダイナミック・データを得る。キネマチッ
ク・データはリンクの長さ及び隣接リンク間のねじり角
度に基づくジョイント・オフセットを含む、ダイナミッ
ク・データはジヨイント慣性及び各リンクの質量中心を
含む。

ブロック６７６Ｆにおいて、線形補間により位置（ｑ）
、速度（Ｉ１１輯亭ヰ）及び加速度（Ｑ）＆挫才庫→が
計算される。

次いでブロック　６７８Ｆにおいて、計算すべきジヨイ
ントがプリズマチック・ジヨイントか１ノボリユート・
ジヨイントかが判定され、対応のジヨイント変数が使用
されるようにする。

ブロック　８８０Ｆにおいて下記のアルゴリズム・ルー
チンが実行される。

＾ＭＡＴＲＸ−キネマヂック・データから変換マトリッ
クスを計算。

（：ＯＭＡＮＤ−：ｑμ中→及びｑｃｉ−ｆ＝ｓ→に基
づきワールド座標で表わされる加速度を計算。

（：ＯＭＰＡＷ−ワ・・−ルド座漂で表される各リンク
の速度を計算。

ＣＯＭ［’ＡＦ−−ダ、イナミック・データに基づぎジ
ヨイント力を計算。

ＣＯＭＰＡトーダイナミック・データに基づきジヨイン
ト力からジヨイント・モーメントを計算。

ジヨイント１からリスト・ジヨイント６への順方向帰納
プロセスによって力及びモーメントの計算が完了したら
、ブロック　６８２Ｆがジヨイント６からジヨイント１
への逆方向帰納でジヨイント・負荷トルク及びジヨイン
ト・ダイナミック・データ・トルクの和に相当するそれ
ぞれの実ジヨイント出力トルクを計算する。このため、
下記ルーチンが実行される。

ＣＯＭＡＳＦ−ダイナミック・データ及びリスト力セン
サーからの力／モーメント・フィードバックに基づきジ
ヨイント力を計算。

ＣＯＭＭＡＳＮ−ダイナミック・データ及びリスト・力
センサーからの力／モーメント・フィードバックに基づ
きジヨイント・モーメントを計算。

ＣＯＭＰＴ−指令された位置、速度、加速度及び力／モ
ーメント・フィードバックの条件下に最終フィードフォ
ワード・トルク指令を計算。

各ジヨイントについて計算されたトルクはラオボット・
ジヨイントの位置、速度及び加速度を求める計算の時点
において加わりたトルクを表わす、一連のトルク計算が
完了したが、次のチックでの反復実行のためプログラム
がルー・ブ・バックする。

ロボット・コントローラーにおいてダイナミック・フィ
ードフォワード・トルクを計算するため、ニュートン・
オイラー運動方程式を解く。マニピュレーターの各リン
クに関するキネマチック・データ及びダイナミック・デ
ータに基づき、特定の位置、速度及び加速度指令下でニ
ュートン・オイラー運動方程式を利用して各ジジイン１
−を駆動するのに必要なトルクを計算する。マニピュレ
ーターのタイプとは無関係のこのアプローチではを変換
して各ジヨイントにおけるト）レフが得られる。リアル
タイム計算を達成するため、高速信号プロセッサーを使
用する（この場合にはＴＩＴＭＳ３２０）、ニュートン
・オイラー・ソフトウェアの平均実行時間は２．９ｍｓ
である。

ニュートン・オイラーの゛　　程 下記方程式はフィードフォワード・トルクの計算に使用
されるニュートン・オイラーの運動方程式である。方程
式１乃至３から明らかなように、計算はマニピュレータ
ーのジヨイント・タイプに応じて異なる。方程式には各
変数のＱフォーマットも示しである。

（以　　下　　余　　白） 四　　　　　　　　　　　　　　　　　　　梶ｒｍ 与の定数の内容は下記の通りである。

ｎ−ジヨイント数（即ち、ｎ＋１リンク）、ａｔ０＝ｗ
、＝Ｏ２ 及びＶ。；０ １八。：独自座標で表わされる質量中心周りのリンクｉ
の慣性マトリックスのモーメント。

＋Ｓ＋　：独自座標で表わされるリンクｉの質量中心。

１１：　リンクｉの総質量、スカラー。

ｖｌ：　リンクｉの質量中心の線形速度。

Ｆドリンクｉに作用する総外部ベクトルカ。

Ｎム：リンクｉに作用する総外部ベクトル・モーメント
。

ｆＩ：　リンクｉ−１によってリンクｉに加えられるベ
クトル力。

ｎＩ：　リンクｉ−ｔによってリンクｉに加えられるベ
クトル・モーメント。

ｎ個のジ）インド・及びｎ＋１個のリンクを有するマニ
ピュレーターの場合、ハンド（例えばリンクｎ）が外部
物体に加える力及びモーメントはそれぞれｆ　ｎ＋１及
びｎｎ＊１である。

上記方程式中の各変数のＱ７オーマツトは７ＭＳニュー
トン・オイラー・ソフトウェアの開発に利用されるフォ
ーマットである。ＴＭＳ３２０の計算は固定小数点方式
で行なわれるから、Ｑフォーマットはニュートン・オイ
ラー運動方程式における各変数の２進小数点位置を決定
する。変数Ａ、Ｔ、、＾ＩＮ＋、Ａｔｆ＋、Ａｌｏｉ及
び？、はすべて２種類のフォーマットを有する。小ジヨ
イント（ジヨイント４．５．６）を駆動するのに必要な
トルクは小さいから、Ｑ８フォーマットが使用され、大
ジヨイント（ジヨイント１．２．３）を駆動するのに必
要な１−ルクは大きいから、ＱＯフす−マットが使用さ
える。トルク計算においてユニ・−トン・オイラーの運
動方程式は順方向及び逆方向帰納を必要とするから、帰
納プロセス中に大ジヨイントと小ジ（インドの間で適当
なフォーマット切り換えが行なわれる。

先に述べた第４図のフローチャートは二ニート・ン・オ
イラー運動方程式の展開を示す。フローチャート中、大
文字の語はニュートン・オイラー運動方程式における該
当の中間データを計算するためのサブルーチンを表わす
。これらのサブルーチ：）の内容は下記の通りである。

ＩＮＩＴ：　Ｔ　Ｍ　Ｓ　３２０実行時間環境を設定し
、次のダ・イナミック・フィードフォワード計算サイク
ルに必要なデータ・メモリー場所をクリアするや ＣＭＤＥＸＴ：マニピュレーターの先行及び現時点位Δ
指令を利用することにより新しい３２ｍ５サイクル１に
おける６つのジヨイントすべての速度及び加速度指令を
計算する。

（：ＭＤＴＩＣ：現３２ｍｓサイクルにおける６つのジ
ヨイントすべての位置、速度及び加速度指令を維持及び
更新する。

ＩＮＱ：ニュートン・オイラー・ソフトウェアに供給で
きるようにジヨイント指令を適当な単位に換算する。

ＡＭＡＴＲＸ　：　リンクｉ−１からリンクｉへ、変換
マトリックスＡｔ−１、ｉ＝を乃至６を計算する。

″（ＶＡＬコントローラーの軌道プランナーにおいて、
マニピュレーターの各ジヨイントの位置指令だけを３２
ｍｓごとに更新する。） サブルー　チンＣＯＭＡＷＤ、　ＣＯＭＰＡＪ　ＣＯＭ
ＰＡＦ、ＣＯＭＰＡＮ％Ｃ０ＭＭＡＳＮ、　ＣＯＭＡＳ
Ｆ及びＣＯＭＰＴはそれぞれＡ目、Ａ目、＾＋Ｆ＋ｖＡ
＋Ｎ＋、Ａ＋ｆ＋、Ａｌｏｉ、１を計算する。ただし、
ｉ＝１乃至６．１−ルク値はＣＯＭＰＴサブルーチンに
おいて出力される。

（以　　下　　余　　白） プログラムの実行時間は約２．９ｍｓであるから、ロボ
ットの各ジヨイントの位置指令は３２ｍ５ごとに形成さ
れ、ＶＡＬサーボは１ｍｓ周期で行われ、ＶＡＬコント
ローラーへのダイナミック・フィールドフォワード組み
込みは４ｍｓごとにフィードフォワード・トルクを出力
することによって達成される。

トルク・エステイメーター・フローチャートにおいて、
ボックス６７３Ｆはサイクル終了をチエツクする。７Ｍ
Ｓソフトウェアはメモリー場所゛ＣＨＫ３２′をチエツ
クすることによりカレント３２ｍ５サイクルが終了した
かどうかを確認する。

もし終了しておれば、ＶＡＬコントローラーから一連の
新しい位置指令が得られ、サブルーチンＣＭＤＥＸＴに
より次の３２ｍｓサイクルのための一連の新しい速度指
令及び加速度指令が計算される。もし現３２ｍｓサイク
ルが終りてなければ、サブルーチンＣＭＤＴＩＣは現サ
イクルの速度及び加速度指令を保持し、サイクルタイム
の進行に従ってすべてのジヨイントに関する位置及び速
度を計算する。

ジヨイントの加速度指令は３２　ｍ、　ｓサイクル以内
では変化しない。

変数”　ＮＵＭＪＮＴ　’はマニピュレーターのジヨイ
ント総数を特定する。ｎ個のジヨイントを有するマニピ
ュレータ・−の場合、順方向機能も逆方向機能もｎ個ル
ープすることにより一連のジヨイント・トルクを計算す
る。プログラム６７０　Ｆのスタート後、ボックス６７
３Ｆにおいてプロセスが停止し、ＴＦマネージャ・−６
０２Ａがトルク・エステイメーター６１０Ａを作動させ
るのを待機する。ＴＭＳ　　３２０プロセツサーのＢＴ
Ｏビンを６８０００プロセツザーと正しく接続すること
により、　６８０００プロセツサーがＢＩＯビンをアク
ティブ・ステート（アクティブ・ロー）にすると、プロ
グラム　６７０Ｆにおけるニュートン・オイラー・アル
ゴリズムが開始されるようにする。ダイナミック・フィ
ールドワードの計算が完了すると、１Ｍ　Ｓ　３２０が
６８０００に対して、演算が終り、ＢＩＯピンが６８０
００によってハイに保持され、その結果、Ｔ　Ｍ　Ｓ　
３２０がプロセッシングを停止する旨通告する。従って
、６８０００がＴ　Ｍ　Ｓ　３２０を再び作動させ、次
のトルク計算サイクルを開始さゼるまでＴ　Ｍ　Ｓ　３
２０は待機状態に置かれる。従って、２・つのプロセッ
サーの動作が同期化される。

入力指令抽出 マニピュレーターの各ジヨイントについてダイデミツク
・フィールドワードを計算するには、ジヨイントごとの
位置、速度及び加速度指令が既知でなければならない。

軌道プランニングにおいては、位置指令だけが３２ｍｓ
サイクルで更新される。従って、先行及び現時点位置指
令とサイクルタイムを利用することによって速度及び加
速度指令が求められる。速度及び加速度プロフィルから
速度及び加速度を計算する最善の方法として、線形補間
を利用する。

即ち、 速　度；（新しい位置−古い位１） ／サイクルタイム 加速度＝（新しい速度−古い速度） ／サイクルタイム それぞれの３２ｔｎｓサイクル中、各リンクの加速度は
変化せず、速度プロフィルは時間と線形関係にある。正
確を期して、速度（位置増分または減分）をビット／　
ｍ　ｓ単位で計算することにより、３２ｍ５ザイクル中
、速度を、線形補間によって計算される所期の値まで増
分または減分できる。線形補間を行うことの利点は３２
ｍｓごとに速度が大無くジャンプするのではなく、３２
ｍｓサイクル中に３２回に亘って速度を小さいジャンプ
で増分または減分することによって、はぼ線形の速度プ
ロフィルが得られることにある。

このことは３２　ｍ　Ｓサイクルで各ジヨイントの中間
位置指令を計算する場合についても同様である。

ＶＡＬコントローラによって供給される位置指令の単位
は゛ビット′であり、マニピュレーターの各ジヨイント
に設けた光学的エンコーダーから直接読み取られる。従
って、速度及び加速度の単位はそれぞれ゛ビット／ｍｓ
’及び゛ビット／　ｍ　ｓ２　′である。ただし、ニュ
ートン・オイラー運動に換算しなければならない。従っ
て、ニュートン・オイラー・ソフトフェアを実行する前
に位置、速度及び加速度について異なる単位を換算する
適当な換算プロシーシアを行わねばならない。

坦ｌＬＬ土ｊコ【換 ニュートン・オイラー運動方程式によって計算されたト
ルクは特定の位置、速度及び加速度指令下に、定められ
た軌道に沿ってアームを移動させるのに必要なトルクで
ある。アームの各ジヨイントにおけるモータを駆動する
には、ニュートン・オイラー運動方程式で計算されたト
ルクを各ジヨイントにおけるギア列を介して再び変換し
て然るべきモーター・トルク値を得なければならない。

ギア列における人力トルクと出力トルクの関係は ｌ ただし、 １゛ｌ：人力軸に作用するトルク。

Ｔ２　＝出力軸に伝達されるトルク。

ｎ　Ｉ　％　ｎ、３　　：それぞれギア１及びギア２の
歯数。

ｎ２／ｎｌ　：ギア比。

ニュートン・オイラー運動方程式で計算されＲた各ジヨ
イントのトルクは上記式のＴ２％即ち、マニビュレ・−
ターを駆動する各ジヨイントの出力軸におけるトルクに
相当する。ニュートン・オイラー運動方程式で計算され
た出力トルクを各ジヨイントの全体的なギア比で割るこ
とにより、マニピュレータ・−の各ジヨイントにおける
モーターを駆動するのに必要な一連の正しいトルク値が
得うえる。

側二匣二乙Δユニノと」ユニ二二」二・　トルータコ直
Ｊと、Ｕ里Ｔ　Ｍ　Ｓ　３２０のデータ記憶単位はワー
ド（１６ビツト）であり、演算は固定小数点方式で行な
われるから、比較的正確な結果を得るには、ユニ、−ト
ン・オイラー運動方程式中の各変数について適切なフォ
ーマット選択が必要である。Ｃ言語で書かれたニュート
ン・オイラー運動方程式の高レベル・シュミ［ノーシコ
ン・プログラムを利用することににす、特定の位置、速
度及び加速度指令下にマニピュレーター各ジヨイントの
理想的なトルク値を計算することができる。次にＴ　Ｍ
　Ｓ　３２０コードを利用することによって計算したト
ルクな理想値どして比較することにより、エラーを理想
値の数％、例えば５％以内に抑えることができる。こね
は適正なＱフォーマットを選択することによって達成さ
れる。

ダイナミック・７４−ルドワードの１へ商業ロボットに
最も広く使用されているコントロールはＰＩＤコントロ
ーラである。もｌ／システムが迅速な応答を必要とする
なら、システム・ゲインを増大させればよい。しかし、
オーバーシュー１−が大きくなったり、不安定なシステ
ムとなるおそれがある。制約のある環境で作業するマニ
ピュレーターの場合、オーバーシュートが組み立て部品
を損傷したり、ロボットアーム自体を損傷するおそれさ
えある。オーバーシュートを避けるには、システム・ゲ
インの低いＰＩＤコントローラーとダイナミック・フィ
ールドワードを併用すればよい。コントローラーのフィ
ールドバック・ゲインを小さくすることでマニピュレー
ターは環境に順応し易くなる。しかし、ゲインが小さく
なると、大きい軌道エラーが発生する可能性がある。

マニピュレータの動力学的特性を知り、正確なフィール
ドフォワード環を計算すれば、フィードバック・エラー
を小さくし、マニと５レータ−の軌道追跡性能を高める
ことができる。従って、位置エラーは著しく小さくなる
。ハイブリッド・コントローラーにあっては、マニピュ
レーターが自由空間内で運動し、純粋な位置制御を利用
できる。

もしマニピュレーターが制約のある環境で作業する場合
には選択マトリックス［Ｓ］が位置サーボの適正自由度
を選択し、もつと順応性に富んだ動作を必要とするカサ
ーボについてはスイッチ自由度を選択する。

第１３Ｂ図及び第１４Ｂ図は第１３Ａ図及び第ｉＪＡ図
に示した公知例に比較ｊノで本発明の採用で達成される
よりすぐれた応答特性を示す。

立ｊユニ九二 既知の力感知装置はマニピュレーターに対する配置場所
に応じて３種類に大別できる。即ち、力感知装置はｌ）
マニピュレーターのジヨイントに取り付けられるか、２
）マニピュレーター・ハンｊ：またはその近傍に取り付
けられるか、３）扱うべき物体の支持プラットホームに
取り付けるのが普通である。また、同じ種類に属してい
ても、個々の機械的な構成は千差万別である。

Ｊ　Ｒ’　Ｕｎｉｖｅｒｓａｌ　Ｆｏｒｃｅ　５ｅｎｓ
ｏｒもロボット用の力センサーであり、第２の種類に属
し、マニピュレーターのリストに取り付けられる。この
ようなり程度の小さい力及びモーメントを感知すること
ができる。また、センサーの謹みからハンド力及びモー
メントへの変換・態様を決定する要因は感知装置の構成
及び幾何学形状だけであるから、この柿のセンサーの多
くは変換ルーチンを１つだけ利用すればよいように設計
されている。

リスト力センサ・−の多くはハンドに作用する力及びモ
ーメントをリストにおりる測定可能な変位量に変換する
トランジューサーとして機能するから、力センサーによ
って生ずるリストの動きがマニピュレーターの位置精度
を低下させてはならない。従りて、典型的なリスト力セ
ンサーは下記特性を具えねばならない。

１）高度のスティフネス：攪乱要素となる力が迅速に減
衰するようなリストの周波数応答が得られること。

２）コンパクトな設計、できるだけ大きいハンド力を測
定したから、ハンドからリスト力センサーまでの距離を
極力短縮することにより、ハンドに力を作用させるレバ
ー・ア・−ムを小さくする。リストの全体的な寸法を小
さく抑えることにより、制約のあるスベ・−ス内でのマ
ニピュレーターの動作を制約しないようにする。

３）すぐれた線形性：力感知装置の応答が作用する力及
びモーメントに対して線形なら、簡単な７１〜リククス
演算を利用することによってセンサーの読みを解像する
ことができる。

４）低ヒステリシス及び内部摩擦：内部摩擦は測定可能
な変位が起こり得るためには先ず力がこの摩擦を克服し
なければならないから、力感知装置の感知を低下させる
。内部摩擦はヒステリンス効果を生み、従って、同じ力
を加え、続いてこの力を除くと、位置測定装置が元の読
み「戻らなくなる。

Ｊ　Ｒ’　Ｌｌｎｉｖｅｒｓａｌ　Ｆｏｒｃｅ　５ｅｎ
ｓｏｒ（ＵＦＳ）システムＪＲ’　リスト力センサ−・
システムは主として下記の２成分から成る。

１）負荷成分の６自由度をすべて測定できるコンパクト
なモノリシック・センサー。

ＪＲ’ＵＰＳシステムの全負荷定格を次表に示す。

（以　　下　　余　　白） 九皮、ｔ　　　　　　　　　　魚盈Ｉ工格Ｆ８　　　　
　　　２５ポンド Ｆ２　　　　　　２５ボンド Ｆｌ　　　　　　　　５０ポンド Ｍ８７５インチ−ボンド Ｍ、　　　　　　　　　フ５インチーポンドＭエ　　　
　　　　　７５インチ−ボンド表１−・Ｆ８、Ｆ□Ｆ□
がそれぞれＸ％ｙ１Ｚ軸に沿った力の負荷定格、Ｍ　、
、　Ｍ　、、　Ｍ　、がそれぞれＸ、ｙ％Ｚ軸を中心と
するモーメントの負荷定格を表わす場合のＪＲ’ＵＰＳ
シスデムの全負荷定格２０！＋コンデイジｊニング盤、
Ａ／Ｄコンバーター盤、及びセンサー・データ解読し、
ホストコンビュートーを相互作用するＶ！ＳＩプロセッ
サー盤（Ｉｎｔｅｌ　８０１８８盤）を含むインテリジ
ェント支持システム。

センサ一応答は２梯類のデータに分類される。

即ち、一方は生データ、他方は処理ずみデータである。

生データはディジタル・フィルタリング、クロス感度除
去、ツールフレーム座標変換、オフセット除去などの処
理を全く施してないＡ／Ｄ変換からの直接的なセンサー
読み取りデータである。データ数字は（１６進、１０進
、２遭いずれのフォーマットで表示されるにしても）フ
ル・レンジ上２０４８カウント（１２ビツト、表１に示
す各チャンネルのフル・スケール・ローディングに相当
）からのカウント数であるい特定チャンネルにおけるセ
ンサー読み取り値が＋２０４８または−２０４８ならば
、このチャンネルにおいて１２ピッｌ−Ａ　／　Ｄが飽
和状態にあり、データが実ローディングを反映しないこ
とを示唆する。

処理ずみデータはディジタル・フィルタリング、クロス
感度除去、ツールフレーム座標変換、及びオフセット除
去の処理が施されたあとのセンサー・データである。こ
のデータは１６進、１０進、２進カウント；またはエン
ジニアリング単位′ボ゛ンド′または゛インチ・ボンド
′で表示することができるつ生データの表示及び処理ず
みデータの表示にはそれヂオ゛ｔセンづ−＝〜指令’　
［）　Ｒ′（Ｄｉｓｐｌａｙ　Ｒａｗ　ｄａｔａ）及び
＋Ｄ　Ｒ’　（Ｄｉｓｐｌａｙ　Ｐｒｏｃｅｓｓｅｒｌ
　ｄａｔａ）を使用すればよい。

センサーのユーザーズ・マニュアルに記載されているよ
うに、センサー生データはノイズをできるだけ小さくす
るためチャンネルごとに設けた４番Ｂｕｔｔｅｒ胃ｏｒ
ｔｈ低域フィルターによってフィルターされる。センサ
ー生データをＵＰＳシステム・メモリーに保持されてい
る６Ｘ６校正マトリックス（クロス感度マトリックスと
も呼ばれる）によって校正することにより、６チヤンネ
ル間の交差結合効果を排除する。なお、センサー・シス
テムのローディング・データ出力は原点がセンサー頂面
中心に位置し、ｌ軸がセン・トー面に垂直な（ツールフ
レーム座標と呼ばれる）座標系で測定される。ツー・−
ルフレーム座標のχ及びｙ軸を決定するには簡単な実験
を行なうだけでよい、即ち、センサーに負荷を作用させ
、ｌ軸が基底と平行になるように保持する。この時点で
、センサーのｌ軸方向力成分の負荷読み取り値はほぼ０
となる。ここで２軸を中心にセンサーをゆっくり回転さ
せ、χ及びｙ軸の力成分をチエツクする。ｙ軸沿いの力
成分の読み取り値がＯに達し、χ軸沿いの力成分の読み
取り値が正の値であれば、ｌ軸はツールフレーム座標の
原点から基底を指すベクトルで出る。ｙ軸も同様にして
求められる。

ＪＲ３ＵＰＳシステムのアナログ出力ボートはＳＣＭ盤
４００とインターフェース関係にあるアナログ・データ
・モジュールＡＤＭ２３　（第３図）と接続している。

センサ一応答はユーザーが相互に規定した周波数でＡＤ
Ｍによってサンプリングされる。ＵＰＳにおいて、セン
サー信号がディジタル化され、交差結合が除かれ、デー
タが再びアナログ変換されたのち、アナログ・ボートか
ら出力される。ＡＤＭによってサンプリングされたデー
タはこうして減結合されたが、センサーの実ローディン
グ（処理ずみデータ）を得るためには、オフセットを正
しく補償しなければならない。

−負荷読み取り値にオフセットを加える。オフセットを
利用することによって種々のドリフトを除くと共に、セ
ンサー出力データを“零点規定“することができる、１
０進方式でオフセットを表示するにはセンサー指令゛Ｄ
Ｏ′を利用すればよい。外部負荷が存在する場合、この
外部負荷による実ローディングが得られるように生セン
サー負荷読み取り値からオフセットを減算する。

例えば、マニピュレーターのジヨイント６にセンサー及
びエンドエフェクタが取り付けである場合には、センサ
ー指令”ＲＯ’（現時点センサー生ｆ−タ読み取り値に
対してするｊｑｅｓｅｔ　ｃｕｒｒｅｎｔＱｆｆｓｅｔ
ｓ　）を利用すればよい、この指令に従ってセンサー・
システムは（指令に入る時点における）生センサー負荷
読み取り値を取り、この読み取り値を新しいオフセット
として使用する。現時点生センサー負荷読み取り値から
オフセットを差し引くことにより、センサー・システム
・データはり値を含んでいる。物体がエンドエフェクタ
によってピックアップされるか、または外部的な力及び
モーメントが検知されたあとの実ローディング（処理ず
みセンサー・データ）は生センサー負荷読み取り値から
各チャンネルのオフセットを差し引いた値に相当する。

この場合、処理ずみデータは外部的な力及びモーメント
がマニピュレーターに加えた負荷成分を表わす。

力Ｚヱ艷二ｌユニＬ皮退 センサー負荷読み取り値を種々の座Ｐ１４系に変換セン
サーのツールフレーム座標はセンサー頂面中心に位置し
、この座標に基づいてセンサー負荷読み取り値が測定さ
れる。ニー、−トン・オイラー・アルゴリズムに外部負
荷データを正しく供給するためにはいくつかのステップ
を踏まねばならない。ニュートン・オイラー・アルゴリ
ズムでは６ジヨイント・マニピュレーターの場合、ｆ７
及びｎ７はジヨイント６の質量中心に作用する外部的な
力及びモーメントに相当する。従って、センサーのツー
ルフレーム座標からジヨイント６座標への力／モーメン
ト変換は適切に行なわれる。ここでは両座標間の力／モ
ーメント変換をかなり単純化するロボットについて述べ
る。

１）両座標のχ、ｙ軸が同一直線」−にあり、両２軸が
一致するようにＪＲ’ＬＩＦＳシスデムをＰＵＭ　Ａ　
５８０のジヨイント６に取り付ける。

２）　Ｐ　Ｕ　Ｍ　Ａ　５８０をＲＥＡＤＹ　’位置に
し、セ瓜 ンサー指令’ＲＯ’を呼び礪−シて新しいオフセット群
を得る。

３）センサーをジヨイント６の一部と考え、質量中心、
慣性マトリックスのモ・−メント及びジヨイント６の質
量を再計算する。これらのデーターをニュートン・オイ
ラー・アルゴリズムに使用されるジヨイント６の新しい
ダイナミック・データとして使用する。力センサーはか
なり対称的な幾何形状を有するから、ジヨイント６の新
しいダイナミック・データの計算は難しくない。

４）ステップ１）と同様にエンドエフェクタを取り付け
る。センサー・のツー・ルフ１ノー八座標で表わさｈる
センサー負荷読み取り値はエンドエフェクタのローディ
ングだけでなくエンドエフェクタによってピックアップ
される外部ロードをも反映する。

５）下記のような簡単な力及びモーメント変換式が利用
される。

センサーのツールフレ−ム座標からジヨイント６座標へ
の変換マトリックスＴは ただし Ｄはセンサーのツールフレーム座標の原点からジヨイン
ト６座標の原点までの距離。

ｔｆＦ及びｔｆＭはツールフレーム座標でセンサーによ
っＣ測定される力及び千−メントのベクトル　ｊ８　Ｆ
及び３６Ｍはジヨイント６座標で計算される力及びモー
メントのベクトル。

上記プロシー♂４を利用することにより、外部的な力（
ｊ’　Ｆ）及びモーメント（ｊ’　Ｍ）をニュートン・
オイラー・アルゴリズムに導入し、ジヨイント・トルク
を計算することができる。

上記プロシーシアにおいて、センサー負荷読み取り値は
外部負荷によるローディング効果だけでなく、エンドエ
フェクタによるロー・ディング効果をも反映する。これ
を改善するためには、上記プロシー♂りにおいてステッ
プ２とステップ４とを逆にすればよい。センサー指令”
ＲＯ′を呼び出したあと、新しいオフセット群は力セン
サー及びエンドエフェクタの双方によるローディングを
含んでいる。従って、処理ずみセンサー負荷読み取り値
は外部負荷を反映するだけである。しかし、この方法で
はエンドエフェクタも力センサーと共にジヨイント６の
一部と見做される。エンドエフェクタの形状は動作に伴
なって、例えば、グリッパ−を閉じたり開いたりするの
に伴なって変化するから、ジヨイント６のダイナミック
・データも変化し、その計算は困難である。しかし、も
しエンドエフェクタの形状がその動作に伴なって変化し
ないなら、この方法は、マニピュレーターが制約のある
環境内で作業する場合に特に重要なことであるが、外部
負荷によるローディングだけを反映するという点で有利
である。

ジヨイント・トルム又」 カセンサー負荷読み取り値からジヨイント・トルクを計
算することの基本的な目的は外部的な力及びモーメント
有無に応じてニュートン・オイラー・アルゴリズムによ
って計算される各ジヨイントのトルクに現われる差がセ
ンサー負荷読み取り値を各ジヨイントに変換することに
よって直接計算されるトルク値に等しくなければならな
いことを立証することにある。換言すると、外部負荷が
存在しなければ、センサーの読みはＯとなり、−連のジ
ヨイント・トルクをニュートン・オイラー・アルゴリズ
ムを利用することで計算できる。もし外部負荷が存在す
れば、同じ位置、速度及び加よ 速度指伶下にｆ、及びｎｔ　　（センサーに会って測定
された外部的な力及びモーメント）をニュートン・オイ
ラ・アルゴリズムに導入することにより、新しいジ９イ
ンド・トルク群が得られる。従って、これら２組のジヨ
イント・トルクの差は各ジヨイントが外部ローディング
を扱うのに必要な−・組′エクストラ′トルクと考える
ことができる。

この゛エクストラ′トルクはマニピュレーターのヤコビ
転置行列でセンサー読み取り値を除算した値に等しくな
けわばならない。

簡単な変換関係は下記式で表わされる。

ＦＩＩｌｌＪＴ＝ｊ６Ｆ ただし、 Ｆは６×１ジヨイント・トルク・ネクトル。

Ｊ　６　Ｆはジヨイント６座標で表わされる外部力／モ
ーメント。

ＪＴはマニピュレーターのヤコビ転置行列。

）Ｖ デカルト空間への　仇 所要の力及びモーメント軌道がデカルト空間に規定され
るいカセンサー負荷読み取り値はこのセンサー独自のツ
ールフレーム座標で与えられるから、センサー情報を正
しくフィードバックするにはツールフレーム座標からデ
カルト座標への力変換が必要である。上述した式と同様
の式を利用してこの変換を行うことができるが、ここで
は変換マトリックスＴは下記の通りである。

Ｔ　−（ＢＡＳＥ’　Ｔａ　”　５ＥＮＳＯＲ）−’Ｔ
　ａ−Ａ　Ｉ”Ａ　ｘ’Ａ　３’Ａ　４”Ａ　Ｓ”Ａ　
６ただし、 ＢＡＳＥはマニピュレーターのベース（即ち、デカルト
座標）からジヨイント１座標への変換マトリックス。

５ＥＮＳＯＲはジヨイント６座標から力センサーのツー
ルフレーム座標への変換マトリックスＡＩはリンクｉ−
１からリンクｉへの変換マトリックス。

従って、Ｔｅｌはジヨイント１座標からジヨイント６座
標への変換マトリックス。

変換マトリックスＴは座標系を力センサーのツールフレ
ーム座標からマニピュレーターのベースに変換する。従
って、カセンザーの力及びモーメント読み取り値が変換
されたのち、センサー情報をハイブリッド・コントロー
ラへ正しく供給することができる。

基本ＴＰ盤のプロセッサーと同様、ＦＦＴＰ盤に使用し
たプロセッサーはＭｏｔｏｒｏｌａ　８８０００（１０
，０Ｍｈｚシステム・クロック周波数で動作する６８に
、１６ビツトマイクロプロセツサー）であり、ＴＩ　　
ＴＭ５３２０信号プロセッサーは高速（２００ナノセコ
ンド／命令サイクル）固定小数点演算を行なうコプロセ
ッサ−とし・て作用する。

サーボ・システムのパワーアップと同時に、ＦＦＴＰモ
ジュールはＳＣＭからパラメーター設定指令を受信する
。ＦＦＴＰモジュールはサーボが使用可能状態にあるか
どうかをチエツクする。ＦＦＴＰモジュールは主として
フィードフォワード・トルクの計算に使用されるから、
ＳＣＭから３２ｍ５ごとに位置指令が供給されるのを待
機する。逆に４ｍｓごとにＳＣＭにむかってフィードフ
ォワード・トルクを返送し、このフィードフォワード・
トルクが位置及び速度エラーに基づくトルク指令と加算
され、基本トルク制御装置に利用される。ＴＰモジュー
ルが使用可能状態になければ０出力を送信する。

ムユＪユニ１基 トルク・プロセッサー・モジュールでは、メモリーがい
くつかの部分、即ち、局部プロセッサーＥＰＲＯＭ、局
部ＲＡＭ　（ＬＲ）　、共用ＲＡＭ（ＳＲ）、ＴＭＳビ
ンボンＲＡＭ、及びＴＭＳプログラムＲＡＭに分けられ
ている。アドレスは下記の通り。

＞０００００　＞７ＦＦＦ　　局部プロセッサー（６８
にと呼称）ＥＦＲＯＭ０６８にシェル ・コード及びＴＳＭニュートン ・オイラー・（−ドから成る。

＞３４０００　＞３７ＦＦＦ　　共用ＲＡＭ、ＴＲモジ
ュールがアクセスでき、ＳＣＭもＶＭＥ バスを介してこのメモリーにア クセスできる。

＞３８０００　＞３８ＦＦＦ　　局部ＲＡＭ、ＴＰモジ
ュールの６８にのための専用ＲＡＭ。

ン３ＣＯＯＯ＞３Ｃ３ＦＦ　　ビンボンＲＡＭ　（２バ
ンク）。

人 ６８にもＴ　Ｍ　Ｓ　３２０もアクセ却できる。

＞４００００　＞４１ＦＦＦ　　Ｔ　Ｍ　Ｓプログラム
・メモリー。

Ｔ　Ｍ　Ｓ　３２０が７ＭＳソース・コードを保持する
ために専用する ＲＡＭブロック。

Ｔ　Ｍ　３３２０プログラム・メモリーの内容、４に×
１６ビツトはＴＭＳがプログラム実行を開始する前に６
８Ｋによってダウンロードしなければならない、６８Ｋ
がＴ　Ｍ　Ｓ　３２０プログラム・メモリーにアクセス
できるのはＴ　Ｍ　Ｓ　３２０がリセット状態に保持さ
れている時だけである。場所＞７Ｅ＾０１に０を書き込
むことにより６８にはＴ　Ｍ　Ｓ　３２０を強制リセッ
トする。＞７Ｅ＾Ｏ１に１が書き込まれると、Ｔ　Ｍ　
Ｓ　３２０がリセット状態を解かれる。

インターフェルスは下記のように構成される。

ＦＦＴＰ−七ユ〉２ヱ」」−９間のインターフェースト
ルク・プロセッサーはＶＭＥ八スへスレーブ・モジュー
ルであり、ディジタル・トルク・ループ計算を行なう。

高レベル・フントローラ（即ち、５ｅｒｖｏ　Ｃｏｒｅ
　Ｍｏｄｕｌｅ）との−ンターフエー・スはＴＰモジュ
ール内に含まわる共用ＲＡＭを介して実現される。トル
ク・プロセッサーは位置指令を受信し、共用ＲＡＭを介
Ｉ、・で出力トルク及び状態情報をＳＣＭに送る。共用
ＲＡＭｋ：おいてフラグをＳＣＭと結合するためのメモ
リー・マツピングについて以下に説明する。

ビンボン・メモリ・− Ｔ　Ｍ　Ｓ　３２０とＴＰ：Ｃ：ジュールの６８０００
　（６８Ｋ）との間で情報交換するためのＭ要なインタ
ーフェースはバンク・スイッチ式メモリー（ビンボン・
メモリーとも呼ばれる）によって形成される。

バンク・スイッチ式メモリーは物理的には別々の２つの
全く同じＩＫＸ１６ビツトＲ，ＡＭアレイから成る。こ
れらのアレイをバンクと呼ぶ。両バンクともに同じアド
レス・スペース内に位置する。

所与の時点において、一方のバンクの所有物は６８にと
連携し、他方のバンクの所有物はＴ　Ｍ　Ｓ　３２０と
連携する。メモリーは単一のＩＫＸ１６ビツトＲＡＭの
形態と取るから、いずれか一方のプロセッサーによるこ
のメモリーへのアクセスは他のプロセッサーによる動作
にとっては完全にトランスペアレントである。

６８に／ＴＭ３３２０インターフェースに関連して４つ
の状態がある。即ち、遊休、始動、処理及び終了の４状
態である。ＴＭＳ３２Ｑ　リセット信号はインターフェ
ースを強制して遊休状態にする。

６８Ｋが始動状態をリクエストするまでインターフェー
スは遊休状態のままである。インターフェースが遊休状
態にある間だけバンク・スイッチングが可能となる。

６８には指令及びデータから成る情報をバンク・メモリ
ーに書き込み、バンク・スイッチをリクエストし、始動
状態をセットする。バンクのスイッチングによりこの情
報がＴ　Ｍ　Ｓ　３２０に供給される。Ｔ　Ｍ　Ｓ　３
２０は始動状態を検知し、処理状態をセットし、データ
を演算し、その結果を再びバンク・メモリーに書き込む
。これと同時に、８８にはＴ　Ｍ　Ｓ　３２０の動作に
とってトランスペアレントな次の指令及びデーター・セ
ットを他のバンク・メモリーに書き込むことができる。

Ｔ　Ｍ　Ｓ　３２０は必要な処理が完了すると終了状態
をセットする。

６８にはこの終了状態を検知し、遊休状態を強制し、バ
ンクを切り換えることにより、Ｔ　Ｍ　３３２０計算の
結果にアクセスする。

７Ｍ３３２０ＢＩＯビン及びＩ１０ボー）−ＰＡ３は６
８にとのハンドシェーク・インターフェースと連携する
。６８にはｘｘｘｌをアドレス＞７ＥＡＯ７へ出力する
ことによって始動状態をリクエストする。これによって
Ｔ　Ｍ　Ｓ　３２０が作動する。７ＭＳ３２０はこれに
応答して処理状態を設定し、ＸＸＸ１のデータ値をＩ１
０ボート３へ出力する。必要な処理が完了すると、Ｔ　
Ｍ　Ｓ　３２０はｘｘｘ２をアト゛レス＞７Ｅ＾０７へ
出力することにより終了状態をセットする。

６８Ｋから見たビンボン・メモリー・アドレスは場所＞
３ＣＯＯＯに始まって場所＞３Ｃ３ＦＦで終り、ＴＭＳ
　３２０から見たビンボン・メモリー・アドレスは場所
Ｏに始まって場所＞ＩＦＦ　（ワードで）に終る。

Ｔ　Ｍ　Ｓ　３２０がビンボン・メモリーにアクセスす
るためには、先ずＴ　Ｍ　３３２０のＩ１０ボートＰＡ
Ｑから最初のアドレスを出力し、次いで読み取りか書き
込みかの別なく、次のデータをＩ１０ボートＦＡＩまた
はＰＡ２を介して入力する。もしＩ１０動作がポートＰ
ＡＩを介して行なわれると、アドレスがＴＰ盤内のアド
レス・カウンターによって自動的に増分される。Ｉ１０
動作がボートＰＡ２を介して行なわれると、アドレスは
自動的に減分される。

共用ＲＡＭのメモリー・マツピングは下記の通りである
。

アドレス　信号　サイズ　　　　　内容＞３４０００　
　ＦＦＢＲＤ　　ワード　Ｆ　Ｆ　（Ｉｉｌ　Ｉ　Ｄ　
（−＞４６４６）。

＞３４００２　　ＢＤＧＯバイト　ＴＴ盤始動状態：１
：始動、　０：Ｊ体 ＞：１４００３　　ＢＤＩＮＩＴバイト　盤初期設定識
別。

１：初期設定完了： ０：初期設定未完了 ＞３４００３　　ＢＤＭＯＤＥワード　盤モード。

１：ＲＶＮモード； ０：ＤＩＡＧＮＯ５ＴＩＣモード ＞３４００６　ＦＦ、ＥＲＲＯＲワード　盤エラー報告
。

エラーがなければＯの まま。

＞３４００８　ＦＦ、ＥＲＰＴＲＯンク・ワードエラー
発生場所を指すアドレス・ポインター ＞３４０１０　５ＥＭＡＩ　　バイト　新しい位置指令
セットが準備できたことを指 示するセマフォ１゜ ＞３４０１１　　ＳＥＡＭ２　　バイト　外部的な力及
びモーメントの新しいセットが 準備できたことを指示 するセマフォ２゜ ＞３４０１２　　ＳＥＭ＾３　バイト　ニュートン・オ
イラー運動方程式で計算され たトルクが準備できた ことを指示するセマフ ォ３゜ ＞３４０１３　　ＳＥＭＡ４　　バイト　Ｃ０ＭＭＡＮ
Ｄ　′が準備できたことを指示するセ マフォ４゜ ＞３４０１４　　ＣＯＭＭＡＮＤ”ワード゛　指令ＩＤ
コード。

ビット１〜１５は使用 されない。

ビット０：１→初期位置 指令準備完了。

〇−初期位置指令準備 未完了。

＞３４０２０　　ＳＲ，ＩＴ　　　　　　新しい位置指
令の開始アドレス。

＞３４０７０　　ＳＲＦＭ　　　　　　外部的な力及び
モーメントの開始アドレス。

＞３４０９０　５ＲＴＱ　　　　　　　）−ルク出力の
開始アドレス。

＞３４０ＢＯ５ＲＰＶＡ　　　　　　古い位置、速度及
び加速度の開始アドレス。

＞３４１７０　　ＳＲＤ＾　　　　　診断データの開始
アドレス。

局部ＲＡＭのメモリー・マツピングを以下に列記する。

アドレス　信号　サイズ　　　　　内容＞３８０００　
　ＢＡＮに　バイト　ビンボンＲＡＭバンクＯまたは１
を選択する スイッチ。

＞３８００Ｉ　　ＣＬＯＧに　バイト　４ｍｓを記録す
るクロック。

＞３８００２　　ＶＣＬＯＣにバイト　３２ｍｓを記録
するクロック。

＞３８００４　　ＬＣｌｌＮＥＸ　７−ド　予期しない
プロセス・コード報告領域。

＞３Ｈ１）６　　　ＵＮＣＤ　　ワード　エラー・発生
時のプロセス・コード。

＞３８００８　　１ＪＮＳＲワード　予期せぬプロセス
のための状態レジスタ・−０ ＞３１１００＾　　υＮＰＣロング・ワード　予期せぬ
プロセスのためのプログラム・カラ ンター。

＞３８０１０　　ＵＮＡＣＡ　　ロング・ワード予期せ
ぬプロセスのためのアクセス・アドレ ス。

＞３８０１４　　　［ＪＮＩＲワード　予期せぬプロセ
スのための命令レジスター。

ビンボンＲＡＭのメモリー・マツピングは下記の通り。

アドレス　信号　　　　　　　内容 ＞３ＣＯＯＯＣ）ｌに３２　３２ｍ５サイクルが終了し
たかどうかを判断するチエツク ・ポイント。

＞３ＣＯＩＯＴＭＳＪＴ　　新しい位置指令の開始アド
レス。

＞３ＣＯ６０ＴＭＳＦＭ　　外部的な力及びモーメント
の開始アドレス。

＞３ＣＯ８０ＴＭＳＴＱ　　トルク出力の開始アドレス
。

＞３ＣＯＡＯＴＭＳＰＶＡ　　古い位置、速度及び加速
度の開始アドレス。

＞３Ｃ１６０ＴＭＳＤＡ　　診断データの開始アドレス
。

制御線ニ アドレス　信号　　　　　　　内容 ＞７Ｅ６０１　　　ＦＦＧＯＧ　Ｏ−Ｐ　Ａ　Ｌとも呼
ばれる。

１→レベル５　ＶＴＩＣに割り込み を可能化、ＦＦ盤はＲＵ Ｎモード。

０→レベル６　ＶＴＩＣＫ割り込み を不能化、ＦＦ盤は［＋ＮＡＧ ＮＯ５ＴＪＧモード。

＞７ＥＡＯＩ　　ＴＭＳＲ５Ｔ　　Ｔ　Ｍ　Ｓ　３２０
リセツト・ライン１→ＴＭＳをリセット状態に にする。

０→ＴＭＳをリセット状態か ら開放する。

＞７ＥΔ０３　　ＢＮＫＳＥＬ　　ビンボン・メモリー
・バンク選択線。

１→バンク１．０→バンク０゜ ＞７ＥＡＯ５５ＴＩＣに　　ソフト・チック。

１→八−ドウエアを使用して バンク選択を制御する。

０→ソフトウエアを使用して バンク選択を制御する。

＞７ＥＡＯ７ＴＭＳＧＯＴ　Ｍ　Ｓゴー・ライン。

６８にシェル・サービス・ルーチンは６８にマイクロプ
ロセッサ−及びＴ　Ｍ　Ｓ　３２０信号ブロセッをセッ
トアツプすることによってＴＰ及びＳＣＭ間の交信を処
理する。また、１ｍｓごとに８８Ｋに割り込むＶＴＩＣ
Ｋ割り込みとＴＰ盤との４ｍｓ同期化も行なわれる。こ
れが必要である理由はＴ　Ｍ　Ｓ　３２０によるフィー
ドフォワード・トルクの計算が約２．９ｍｓを必要とす
ることにある。トルク・データをＶＡＬコントローラー
に記録するためには同期化を行なわねばならない、レベ
ル６ＶＴＩＣＫ！ｌＩＪり込み、バス／アドレス・エラ
ー及び予期しないエラーを扱うための割り込みサービス
・ルーチンが構成される。以下に詳細を説明する。

スタート・アップ・ルーチン 以下に述べる手順から、ＴＰモジュールの初期状態、及
びシステムのパワーアップまたはリセットに伴なりて６
８Ｋが行う動作が明らかになるであろう。

１、システム・スタックをセットアツプし、割り込みを
不能化し、６８Ｋをスーパーバイザー・モードにする。

２．７　Ｍ　Ｓ　３２０をリセット・モードにしく＞７
ＥＡＱ１←０）、ソフト・チックを選択しく＞７ＥＡＯ
５−Ｏ）、ビンボン・メモリー・バンクＯを選択する（
　＞７ＥＡＯ３−０）。

３、共用ＲＡＭ（ＳＲ）、ビンボン・メモリー及び島部
ＲＡＭ（Ｌ［？、）中のハンドシェーキング・バッファ
を払う。

４、フィードフォワード盤（ＦＦ）ＩＤコード（−＞４
６４６）をＳＲの場所ＦＦＢＲＤ　（・＞３４０００）
に書き込む。

５、ＥＰＲＯＭの場所＞４０００からＴＭＳプログラム
・メモリーの場所＞４０００にＴＭＳコー・ドにュート
ン・オイラー運動方程式）をコピーするや ６、ＦＦ盤初期設定終了コード（−１）をＳＲの場所Ｂ
　Ｄ　Ｉ　Ｎ　Ｘ　Ｔ　（−＞３４００３）に書き込む
。

７、ＦＦ盤始動状態をボールする（ＢＤＧＯ１場所＞３
４００２）。

もしＢＤＧＯ状態が生きている（＝１）なら、ステップ
８に進む、さもなければ、ステップ７に戻り、ＢＤＧＯ
のポーリングを続ける。

８、ＦＦ盤モードをチエツクする（ＢＤＭＯＤＥ、場所
＞３４００４）　。

もしＢＤＭＯＤＥ＝１なら、ＦＦ盤はＲＵＮモードにあ
る。

もしＢＤＭＯＤＥ＝Ｏなら、ＦＦ盤はＤＩＡＧＯＮＯＳ
ＴＩＣモードにある。

ＢＤＭＯＤＥ　（１または０）を場所＞７Ｅ６０１に位
置するＦＦＧＯに書き込む。

９、ＳＥＭＡ４をボールすることにより新しいＣＯＭＭ
ＡＮＤの有無をチエツクする。

もしＳＥＭＡ４≠〉５５なら、ステップ９に戻ってＳＥ
ＭＡ４のポーリングを続ける。

もしＳＥＭＡ４−＞５５なら、ＣＯＭＭＡＮＤビットを
チエツクする： ビット０＝０、即ち、初期位置指令が 未だ準備されていなければ、ステップ ９に戻り、ＳＥＭＡ４のポーリングを 続ける。

ビット０＝１、即ち、初期位置指令が 準備されているなら、ステップ１０に 進む。

１０、初期位置指令をＳＲからビンボン・メモリーにコ
ピーし、＞ＡＡをＳＥＭＡ４に書き込んでコピーがなさ
れたことを確認する。

１１、場所＞７Ｅ＾０１に１を書き込むことによフてＴ
Ｍ　Ｓ　３２０をリセット状態から解ぎ、割り込みを可
能化し、１ｍ５ＶＴＩＣＫ割り込みを待つ。

スタート・アップ・プロシージｌのステップ１１におい
て、６８には無限ループに入り、レベル轟の動作は純粋
に割り込みによって駆動される。

割り込みサービス・ルーチンがＴ　Ｍ　Ｓ　３２０ニユ
ートン・オイラー・ソフトウェアを作用させる態様を以
下に説明する。

五−υ込邊」Ｌ１旦２！」Ｌ二１２ レベル６ＶＴＩＣＫオート・ベクトル割り込みはＶＭＥ
バスを介してＳ　ＣＭによって提供され、１ｍ５（チッ
ク）ごとにＴＰの６８Ｋに割り込む。

予期しない割り込みが起こると、サービス・ルーチンは
現状態レジスターの内容、及び命令レジスター及びプロ
グラム・カウンター中のエラー・コードをセーブし、将
来のデバッギングに備えて共用ＲＡＭ及び局部ＲＡＭの
エラー報告領域に記録する。また、この動作はシステム
が再びリセットされるまで一時停止される。

共用ＲＡＭ中に位置指令及び外部的な力及びモーメント
が準備されているかどうかをＴＰ盤に指示するセマフォ
”ＳＥＭＡＩ’及びＳＥＭＡ２′を有するレベル６ＶＴ
ＩＣＫ割り込みサービス・ルーチンを使用する。セマフ
ォがセットされているなら、データをＳＲからビンボン
・メモリーにコピーすることができ、このビンボン・メ
モリーにおいてＴＭＳがデータにアクセスすることがで
きる。変数’ＣＬＯＣＫ’及びＶＣＬＯＣＫ′を利用し
て３２ｍｓサイクルを維持する。ＶＴＩＣＫ割り込みは
ｕｒｎｓごとに６８Ｋを呼び出し、ＴＭＳは１組のフィ
ードフォワード・トルクの計算を完了するのに合計４ｍ
ｓを必要とするから、３２ｍ５サイクルにトルクを８回
出力することができる。システムがパワー・アップする
と、スタートアップ・ルーチンはＣＬＯＣに′及び゛Ｖ
ＣＬＯＣＫ”をＯに（７−１トし、ＴＰ盤はＳＣＭから
初期ジヨイント位置を得る。もしＶＡＬがサーボ開始可
能な状態にあれば、ＳＣＭは次のＶＴＩＣＫがＴＰ盤に
割り込むとＴＰの６８Ｋが新しい位置指令をコピーし、
もし存在するなら外部的な力及びモーメントをビンボン
・メモリーにコピーするようにＳＥＭＡＩ　’をセット
する。ＣＬＯＣＫ’及び’ＶＣＬＯＣＫ’はソＪ−Ｌ４
ｆｉ３及び８にセットされる。また、ＴＭＳはニュート
ン・オイラー・ソフトウェア中のサブルーチン゛ＣＭＤ
ＥＸＴ’を呼び出すように通告される。

（以　　下　　余　　白） 最後にＴＭＳＧＯをセットする。なお、’ＣＬＯＣＫ’
及びＶＣＬＯＣＫ’は主として２つカウンターとして作
用し、’ＣＬＯＣＫ’は３からＯまでカウントし、”Ｖ
ＣＬＯＧに′は８から１までカウントする。”ＶＣＬＯ
ＣＫ’−０なら、サーボが停止し・たことを意味し、レ
ベル６ＶＴＩＣＫ割り込みサービス・ルーチンが呼び出
されると、いかなる動作も行なわれない。”ＣＬＯＣに
′ｔ＝０なら、４ｍｓが経過し、ＴＭＳがフィードフォ
ワード・トルク計算を完了したことを意味する。（場所
＞Ａ７ＥＡＯ１の内容が２であるかどうかをチエツクし
た結果）ＴＭＳが準備未完了の状態であれば、エラーの
発生を意味し、システムがリセットされるまでＴＰ盤の
動作が一時停止される。ＴＭＳが準備完了状態なら、ビ
ンボン・メモリーからＳＲへトルクがコピーされ、これ
らのトルクをＶＡＬコントローラーに導入することがで
きる。

アーム１１皿１ 高レベル制御ルーピングの結果、電圧指令信号が形成さ
れ、アーム軸に関してこの電圧指令信号をＡＩＦ盤８０
０（第８図）によって実行され、ユロ ーザーのロボットまたは応用ブシオグラムに従い、制御
された速度、加速度及びトルクでエンドエフェクタが指
令位置まで移動する。ＡＩＦ盤扁 ８００にパルス暴変調回路８０１及び駆動回路８０２を
設け％６つのアーノー運動軸とそれぞれ連携するブラシ
レスＤ、Ｃ，モーターに駆動電流を供給する電力増幅器
に印加さ゛れる軸駆動信号を発生させる。

ＡＩＦ盤の回路は電圧指令データを処理することにより
、運動軸モーターにモーター駆動電流を供給する電力増
幅器のベースまたはゲート駆動回路を制御するディジタ
ルＴＴＬ論理レベル信号を形成する。既に述べたように
、閉ループ位置、速度及びトルク制御のため、そ−ター
電流と軸位置及び速度データがＡＩＦ盤８０Ｇを介して
高レベル制御ループへフィードバックされる。

ＡＩＦ盤−パルス幅　調　式 ＡＩＦ盤８００のパルス幅変調回路８０１は運動軸モー
ター駆動回路を通るトルクまた電流制御ループを閉じる
ディジタル・インターフェースとして作用する。パルス
幅変調の原理をジョイント・モーター・パワー・スイッ
チの導通時間幅制御に利用して、モーター電圧及びトル
ク指令を満足させる。

第９図の概略的なブロックダイヤグラムに示すように、
ディジタルＰＷＭ発生器８２５Ａはアトルク・マイクロ
プロセッサ−Ｐ２バスを介して９ビツト・データ指令及
び３つのレジスター・アドレス・ビットを受信する。さ
らに、デバイス選択論理、読み取り／書き込み、リセッ
ト（初期設定）及びデータ・ストロ・−ブ信号がＰ２バ
スから受信される。トルク・プロセッサー盤のトルク・
カルキュレータ−からの受信ごとにＰＷＭ発生器８２５
ＡからバスへＤＴＡＣＫ　（確認）を返信する。

ディジタルＰＷＭ発生器８２５Ａは運動軸駆動手段とし
てブラシレス・タイプまたはＤＣブラシ・タイプ・モー
ターが使用される３軸に対して作用できるように構成す
ることが好ましい。即ち、モ−ターがブラシレス・タイ
プかＤＣブラシ・タイプかに関係なく、各軸モーターと
連携する増幅器ベースまたはゲート駆動回路を制御する
ため一連のディジタル信号（ここでは４つの信号Ａ１、
Ａ２．８１％　Ｂ２）が形成される。

４つのディジタルＰＷＭ制御信号を利用することにより
、オン／＄オフ・パワーへ・スイッチ制御１を介してモ
ーター巻線を流れるｉｌＥ流の方向及び大きさを制御す
る。ブラシレスＤＣモーターの場合、ブラシレスＤＣモ
ーターの３相巻線をブリッジ回路（第１．　ＯＡ　−Ｂ
図）の形に接続し、モーター駆動電流が常時１対の巻線
を流れるようにし、モーター導電バスを順次隣接の巻線
対へ回転または転流させることによりモーター駆動トル
クを発生させる。この構成では、ＰＷＭパルスがモータ
ーを流れる電流の時間長を決定し、ＰＷＭパルス及びホ
ール効果センサー・フィードバック信号に基づく転流ス
イッチング論理が駆動電流を流すべき巻線対及び方向を
決定する。

Ｈ形電力増幅器ブリッジ回路が利用されるＤＣブラシ・
タイプ実施例の場合、ＰＷＭ出力侶号Ａ１及びＢ２の制
御下に電力増幅器スイッチ８２７Ａ及び８２８Ａが開放
されるとＤＣブラシ・タイプ・モーター８２６Ａ　（第
１１図）が１つの方向に回転し、ＰＷＭ出力信号Ｂ１及
びＡ２の制御下に電力増幅器スイッチ８２９Ａ及び８３
０Ａが開放すると反対方向に回転するゆ パルス幅変調回路は１対の大規模集積パルス幅変調（Ｐ
ＷＭ）チップの形で実施するのが好ましい。一般に、そ
れぞれのＰＷＭチップはマイクロプロセッサ−周辺デバ
イスとして（即ち、制御ループ構造中の上位マイクロプ
ロセッサ−の制御下に）作用してＤＣブラシ・タイプ・
モーター駆動手段を有する３軸を制御するためのディジ
タル・パルス幅変調信号を出力する。

モーター電流、位置及び速度フィードバックや、その他
のＡＩＦ盤回路の詳細については、１υ ９８６年１１月２０９督付米国出願第９３２，９８２号
（Ｗ　、　Ｅ　、第５３．３６９号）を参照されたい。

ＰＷＭ方式の詳細については、同じ日付の親米国特許出
願第９３２，８４１号（Ｗ、Ｅ、第５３．２２５号）の
継続出願を参照されたい。

【図面の簡単な説明】

第１図は本発明の実施例としてのロボット制御ループを
略示するブロックダイヤグラムである。 第２図は本発明の実施例としてのディジタル・ロボット
制御ループの詳細なブロックダイヤグラムである。 第３図は第２図のディジタル制御ループを実施するため
の好ましい回路盤構成を示すブロックダイヤグラムであ
る。 第４図は好ましい実施例に利用されるトルク計算アルゴ
リズムのフローチャートである。 第５図はフィードフォワード・トルク制御と関連するデ
ータ処理のフローチャートである。 第６Ａ−１図及び第６Ａ−２図はロボット制御システム
用の基本トルク・サーボ制御として利用される第１電子
トルク・プロセッサー　（ＴＰ）盤口 のブロックダイヤグラムである。 第７Ａ−１図及び第フＡ−２図はハードウェアについて
は第６Ａ−１図及び第６Ａ−２図の第１ＴＰ盤と同じで
あるが第１ＴＰ盤における基本トルク・サーボ制御用フ
ィードフォワード・トルク・コントローラーとして作用
する第２電子ＴＰ盤のブロックダイヤグラムである。 第８図は第３図のシステムに利用されるアーム・インタ
ーフェース盤のブロックダイヤグラムである。 第９図はＡＩＦ盤に使用されてジョイント・モーター制
御信号を形成するＰＷＭ回路のブロックダイヤグラムで
ある。 第１０Ａ図、１０８図及び１１図はブラシレス・タイプ
及びブラシ・タイプＤＣジョイント・モーターのブリッ
ジ構成を示す回路図である。 第１２図はロボット制御テストに使用される速度指令の
タイム・プロットである。 第１３Ａ図は第１２図の速度指令を実行する際の典型的
な公知ロボット制御の経路精度を示す公知の制御で発生
する位置エラーのタイム・プロツトである。 第１３Ｂ図は第１２図の速度指令を実行する際に本発明
のロボット制御で達成できる改善された精度を示すタイ
ム・プロットである。 第１４Ａ図及び第１４Ｂ図は第１３Ａ図及び第１３Ｂ図
の位置エラー信号にそれぞれ対、応するロボット駆動出
力信号図である。 （以　下　余　白） 〔上質な参照番号の説明〕 ６・・・Ｉ・ルク制？Ｊ装置、１２・・・運動プランナ
ー、１４・・・位；η制御ヰ装置、１６・・・速度制御
装置、１８・・・フィードフォワードトルク指令システ
ム、１９・・・リスト力センサ−，２０・・・ロボット
、２２・・・電源ブロンク、１２８・・・位置指令、１
２９・・・入力プロセッサー、１３２・・・リスト力セ
ンサ−，１３８・・・速度指令補間装置、１４２・・・
Ａ／Ｄ、１５２・・・微分回路、１７Ｂ・・・スケーラ
−１４００・・・サーボ制御盤、５００・・・システム
制御盤、６００Ａ・・・フィードフォワード１−ルクプ
ロセッサー盤、６（ン２・・・トルク制御マネジャー。 特許出願人：ウェスチングハウス・エレクトリンク・コ
ーポレーション 代　理　人：加藤　紘一部（外１名） ＰＯ５ＩＴＩＯＮＥＲＲＯＲ 自重よ上４　〜桑のω６ ００００　ｃｓ　ｂ　００ロロ０ ＰＯ５ＪＴＩＯＮ　ＥＲＲＯＲ Ｄ／Ａ　　０１ＪＴＰｔ１丁

Claims (1)

1. 【特許請求の範囲】 （１）ロボット・アーム・ジョイントのそれぞれを駆動
   するモーターと； 各モーターに駆動電流を供給するための電力増幅器と； 各ジョイント・モーターに設けられ、位置指令（ＰＣＯ
   Ｍ）に従ってトルク制御ループを駆動することにより連
   携の電力増幅器を制御するためのモーター指令（ＴＱＣ
   ＯＭ）を発生させる位置制御ループ（ＰＶＢ）及び速度
   制御ループ（ＶＥＢ）を含むフィードバック制御ループ
   と； 前記ジョイント・モーターの動作を感知し、位置及び速
   度指令とそれぞれ組み合わせることにより、対応の位置
   及び速度制御ループから前記トルク制御ループのそれぞ
   れに対するトルク指令としてエラー信号を形成するため
   の位置及び速度フィードバック信号を発生させる手段 を含む複数のアーム・ジョイントを有するロボットのた
   めの制御装置であって、 前記ロボット・ジョイントの少なくとも所定の１つに加
   わる負荷力を感知する手段と； 前記負荷力感知手段からの出力に基づいてそれぞれのロ
   ボット・ジョイントごとに実負荷モーメントを計算する
   手段と； ロボット・アームに関して記憶されているダイナミック
   ／キネマチック・データに基づいてそれぞれのロボット
   ・ジョイントごとにダイナミック／キネマチック・デー
   タ・モーメントを計算する手段と； データ・モーメント及び負荷モーメントを複合し、前記
   トルク制御ループのそれぞれに対する前記トルク指令を
   形成する際に前記位置及び速度エラーと組み合わせるべ
   くフィードフォワード・トルク（ＣＯＭＰＴ）として前
   記複合モーメントを供給する手段と； トルク・エラー信号を形成するため各トルク制御ループ
   においてトルク指令と組み合わされるジョイント・モー
   ター駆動電流を表わすフィードバック信号を発生させる
   手段とを有すること を特徴とするロボット制御装置。 （２）前記データ・モーメント計算手段がニュートン・
   オイラーの運動方程式を利用してそれぞれのジョイント
   ・データ・モーメントを計算することを特徴とする請求
   項第（１）項に記載のロボット制御装置。 （３）前記負荷力感知手段がエンドエフェクタに連接さ
   れた最端ジョイントに配設され、前記負荷モーメント計
   算手段がそれぞれのジョイント負荷モーメントを逆方向
   帰納法で計算し、前記データ・モーメント計算手段がニ
   ュートン・オイラーの運動方程式を利用してそれぞれの
   ジョイント・データ・モーメントを順方向帰納法で計算
   することを特徴とする請求項第（１）項または第２項に
   記載のロボット制御装置。 （４）ロボット制御装置がほぼ完全にディ ジタル式であり、それぞれのフィードバック制御ループ
   手段が所定のサンプリング速度でディジタル動作して、
   ディジタル位置指令に基づいてディジタル・モーター指
   令を形成し、それぞれのモーター動作感知手段が位置及
   び速度制御ループにおける制御計算のための位置及び速
   度フィードバック信号を出力し、それぞれの電流フィー
   ドバック発生手段がトルク制御ループにおける制御計算
   のためのディジタル信号を出力することを特徴とする請
   求項第（２）項に記載のロボット制御装置。 （５）運動プランニング手段が前記ディジタル位置指令
   を形成し、各ジョイントの計算トルク値が前記運動プラ
   ンニング手段に供給されて利用可能なトルクと比較され
   、比較結果に従って計画速度が増減されることを特徴と
   する請求項第（４）項に記載のロボット制御装置。 （６）前記キネマチック・データが各リンクの長さ及び
   隣接リンク間のねじり角に基づくジョイント・オフセッ
   トを含み、前記ダイナミック・データが各ジョイントの
   慣性及び各リンクの質量中心を含むことを特徴とする請
   求項第（４）項に記載のロボット制御装置。（７）前記
   キネマチック・データが各リンクの長さ及び隣接リンク
   間のねじり角に基づくジョイント・オフセットを含み、
   前記ダイナミック・データが各ジョイントの慣性及び各
   リンクの質量中心を含むことを特徴とする請求項第（１
   ）項または第（６）項に記載のロボット制御装置。 （８）添付の図面に図示し、かつこれに沿つて後述する
   ようなロボット制御装置。