JPH0276010A - ディジタル制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔概要〕 多関節ロボットや自動機のアーム先端に取付けた力セン
サーにより検出した力をフィードバックし、位置と力の
制御を同時に行うディジタル制御装置に関し、 この種のディジタル制御装置において、サーボ系の動特
性およびロバスト性を向上でき、結果として力制御系全
体の特性をも向上できるようにすることを目的とし、 ディジタル制御装置を２個のマイクロプロセッサと１個
のホスト計算機で以て構成し、一方のマイクロプロセッ
サにサーボ制御を行わせ、他方のマイクロプロセッサに
力制御を行わせるものである。

〔産業上の利用分野〕

本発明は、多関節ロボットや自動機のアーム先端に取付
けた力センサーにより検出した力をフィードバックし、
位置と力の制御を同時に行うディジクル制御装置に関す
る。組立て作業における嵌め合い、ならい、押し付は動
作などアームと対象物体とが接触するような作業では位
置と同時に力を制御Ｉｌする必要が生じる。

〔従来の技術〕

第１５図は６関節ロボットの例を示す図である。

同図において、ＪｌないしＪ、は関節、トＩはハンド（
手首）、Σえは基準座標系、Σ、はハンド座標系をそれ
ぞれ示す。

多関節ロボットを制御ＩＩする場合、機構の複雑さから
、力センサに対してハンド座標系Σ□、関節角やモータ
駆動トルクに対して関節座標系、制御演算に対して基準
座標系Σ８を設け、それぞれの機構特性に合わせて座標
変換を決定している。

第１３図は多関節ロボットの力制御系の例を示す図であ
る。同図において、１は力の補償部、２と３は座標変換
部、４は関数発生器、５はサーボ補償部、６は機構部、
７は力センサ−，１１’、Ｒは力の設定値、ＦＲは力の
現在値、Ｆ、ＴＩは力の偏差、Ｆ’は力の観測値、Ｖ４
は指示速度、Ｖも指示速度、ｒは目標位置、θは現在位
置、ｅは偏差、Ｕは操作量を示す。なお、添字Ｒは基準
座標系で表現される変数を示し、添字Ｈはハンド座標系
で表現される変数を示し、添字なしは関節座標系で表現
される変数を示す。

〔発明が解決しようとする課題〕

第１４図は従来の力制御系のハードウェア構成例を示す
図である。同図において、８はマイクロプロセッサを示
している。

第１３図の力制御系をディジタル制御で実現しようとし
た場合、第１４図のようなハードウェア構成が考えられ
るが、第１３図で示す全ての処理をマイクロプロセッサ
８でシーケンシャルに処理すると、演算時間が増大して
しまう。その結果、サンプリング周期が大きくなり、内
側のループの位置決めサーボ系の動特性およびロバスト
性が劣化し、延いては力制御系の特性も悪くなる。

本発明は、この点に鑑みて創作されたものであって、サ
ーボ系の動特性およびロバスト性を向上でき、結果とし
て力制御系全体の特性をも向上できるディジタル制御装
置を提供することを目的としている。

〔課題を解決するための手段〕

本発明のディジタル制御装置は、アームの先端にハンド
が取り付けられると共にハンドに作用する力を検出する
力センサーを持つ多関節ロボットまたは自動機を制御す
るものである。

第１図は本発明のディジタル制御装置のハードウェア構
成を示すものである。同図において、１１と１２はマイ
クロプロセッサ、１３と１４は外部メモリ、１５と１６
はＩ１０装置、１７と１８はローカル・バス、１９と２
０はバス、２１は同期用クロック源、２２はＤＭＡシー
ケンス制御部、２３はホスト・インタフェース制御部、
２４はホスト計算機をそれぞれ示している。

マイクロプロセッサ１１．１２として、ディジタル・シ
グナル・プロセッサ（例えば富士通製ＭＢ８７６４　）
を使用することが出来る。このディジタル・シグナル・
プロセッサは、内部にメモリを有すると共にＤＭＡＩ能
を有するものである。マイクロプロセッサ１１．１２は
、同期用クロック源２１の同期クロック（サンプリング
・クロック５ＣＫ）を取り込み、サンプリング同期を取
る。また、マイクロプロセッサ１１．１２は、ローカル
・バス１７．１８を通して外部メモリ１３．１４及びＩ
１０装置１５．１６を互いに干渉することなく自由にア
クセスすることが出来る。Ｉ１０装置１５はエンコーダ
＆カウンタを含み、Ｉ１０装置１６は力センサーを含ん
でいる。マイクロプロセッサ１１．１２間の通信は、サ
ンプリング毎に高速に行う必要があり、ＤＭＡ転送によ
り、マイクロプロセッサ１１．１２の内部動作とは非同
期にバス１９．２０を通して行う。ホスト計算機２４か
らは、各マイクロプロセッサ１１．１２に対してマイク
ロプロセッサ１１．１２の処理とは非同期にデータ転送
する必要がある。そこで、ホスト・インタフェース制御
部２３により、ホスト計算機側のバスとマイクロプロセ
ッサ側のＤＭＡインタフェースを結合し、ホスト計算機
２３からＤＭＡでマイクロプロセッサ１１．１２をアク
セス可能にする。更に、マイクロプロセッサ１１と１２
のＤＭＡ転送、ホスト計算機とマイクロプロセッサ１１
間のＤＭＡ転送、ホスト計算機とマイクロプロセッサ１
２間のＤＭＡ転送のタイミングが重なった場合に、競合
を回避する機能をＤＭＡシーケン制御部２２に持たせる
。

第２図は本発明におけるマイクロプロセッサの処理手順
を示す図である。マイクロプロセンサ１１はサーボ演算
を行い、マイクロプロセッサ１２は力制御演算を行う。

マイクロプロセッサ１１とマイクロプロセッサ１２は、
同一のサンプリング・クロックＳＣＫで同期して動作す
るものとする。

先ず、マイクロプロセッサ１１の処理手順について説明
する。

■　サンプリング・クロックＳＣＫのトリガ発生をチェ
ックし、サンプリング・タイミングならば、［２］の処
理に進む。

■　ＤＭＡ転送により１ステップ前の関節指示速度Ｖを
入力する。

■　関節角θを人力する。

■　ｅ＝ｒ−０に応じてサーボ演算を実行し、モーフ駆
動力Ｕを更新する。

■　■で入力した１スツプ前の関節指示速度Ｖに応じて
目標位置ｒを計算する。

■　■で入力した関節角θを次のサンプリング時の座標
変換のためにマイクロプロセッサ１２に送る。

■　ホスト計算機からのコマンドを解析する。

■　必要に応じて各コマンドを実行する。

次に、マイクロプロセッサ１２の処理手順を説明する。

■　サンプリング・クロックＳＣＫのトリガ発生をチェ
ックし、サンプリング・タイミングならば、［２］の処
理に進む。

■　センサー信号ＦＨを入力する。

■　センサー信号Ｆ１を１ステップ前の姿勢θをもとに
基準座標系の値ＦＲに変換する。

■　ＰＲをホスト計算機から設定したＦｉＲと比較し、
ＦｉＲ＝ＦｉＲ−Ｆｉに応じて適当な補償演算を行い、
この結果を指示速度ｖＲとする。

■　指示速度ｖｌｌを関節速度Ｖに変換し、次のサンプ
リング時にマイクロプロセッサ１１側に渡す。

■、■　マイクロプロセッサ１１と同様にホスト・イン
タフェース処理を行う。

なお、■、■の座標変換は機構の構造により決定される
ものであり、演算方法は例えばＰ、Ｐａｕｌ著ロボフロ
ボットプレータ等に記載されている。

第３図はホスト・インタフェース・バッファの設計例を
示す図である。ロボットを動かしながら、力制御やサー
ボ制御のパラメータ（フィードバック・ゲインなど）を
変更する場合、１サンプリング周期内で関連するパラメ
ータを全て切り換えなければならない。そのために、−
度に換える最大のパラメータ数だけのバッファ容量とパ
ラメータ変更のための手順（ソフトウェア）が必要であ
る。

ホスト計算機とＤＳＰの速度が何十倍も異なるために特
に問題になる。これを実現するために、転送コマンド受
付時に同一サンプリング内で全てのパラメータを転送す
る必要がある。そこで、ホスト・インタフェース・バッ
ファ領域は、ホスト←→マイクロプロセッサ間で一度に
転送すべき最大データ量分だけ確保し、第３図のような
形にする必要がある。第３図（ａ）は−度に転送するデ
ータを連続アドレスに限定するもので、ホスト計算機か
ら指定したアドレスから指定データ数分だけデータ転送
する。また、これとは別にコマンドを指定してマイクロ
プロセッサに所定の処理を実行させる（サーボ・オン／
オフなど）。この場合、マイクロプロセッサのメモリ・
マツプは、同一コマント処理に関連したデータを全て連
続したアドレスに配置しておく必要がある。第３図（ト
））は転送データのアドレスを一つずつ指定してマイク
ロプロセッサに転送先を示す方法であり、この場合はコ
マンドは成るアドレスに固定で割付けておけば良い。

第３図（Ｃ）は、マイクロプロセッサがリアルタイムで
用いるメモリ領域と全く同一サイズのパンファを用意す
るものである。これらは、マイクロプロセッサのメモリ
空間のサイズや処理スピード等によって適当なものを選
択すれば良い。

〔作用〕

第４図は本発明と従来例の演算処理の比較を示す図であ
る。本発明の演算処理と従来の演算処理を比較すると、
第４図のようになる。力信号ＦＨを入力してから操作量
ｕを更新するまでの時間については本発明も従来例も略
ぼ同じであるが、本発明によればサーボ系（マイクロプ
ロセンサ＃１）のサンプリング周期を半減でき、また、
演算時間遅れを著しく減少できる。この結果、サーボ系
のみならず力制御系全体の特性を向上させることが出来
る。なお、力制御の座標変換で用いるθは１ステップ前
の値であるが、この程度の誤差は力制御演算において大
きな影響を与えない（サンプリング周期はＩＫＨｚ程度
）。

〔実施例］ 第５図は位置と力のハイブリッド制御の実施例を示すブ
ロック図である。同図において、３０はロボット・マユ
ブレーク、３１はモータ、３３はエンコーダ＆カウンタ
、３４はパワーアンプ、３５はＤ／Ａ変換器、３６はサ
ーボ演算・関数発生部、３７は力センサ−，３８は座標
変換部、３９も座標変換部、４０は逆ヤコビ行列演算部
、４１は力制御部、４２は位置制御部、４３は速度サー
ボ系をそれぞれ示している。

第１３図は単なる力制御系であ′るのに対して、第５図
は力制御と位置制御を組み合わせたハイプリント制御と
呼ばれるものである。ハイブリッド制御については、例
えばＭ、ｌ＋、Ｒａ１ｂｅｒｔ、Ｈｙｂｒｉｄ　Ｐｏ５
ｉｔｉｏｎ／Ｆｏｒｃｅ　Ｃｏｎｔｒｏｌ　ｏｆ　Ｍａ
ｎｉｐｕｌａｔｏｒｓ、ＡＳＭＥ、１９８１に記載され
ている。第５図の中において、右半分（４３，３０，３
７，３８，４Ｌ　　４０）が力制御部で第１３図に対応
し、それに対して第５図の左半分は位置制御系を表して
いる。ハイブリッド制御は、位置制御と力制御の方向を
直交する方向にとることにより、拘束条件下で運動を実
現しようとするものである。第５図は多関節のロボット
・マニプレータを使用して対象物表面を倣い加工する場
合の制御系を示すものである。速度サーボ系４３は、モ
ータ３１、エンコーダ＆カウンタ３３、パーアンプ３４
、Ｄ／Ａ変換器３５及びサーボ演算・関数発生部３６を
有している。速度サーボ系４３は、自由度の数だけ存在
する。ロボット・マニプレータ１０の先端にはハンド（
図示せず）が取り付けられ、ハンドの成る１点に作用す
る力は、マユブレークの手首部分に装着された力センサ
−３７によって検出される。ハンド座標系（Ｘゎ、Ｙｈ
、Ｚｂ）で検出した力Ｆ　ｈは、座標変換部３８におい
てロボット基準座標系（Ｘ、。

Ｙ、、Ｚ、）の値Ｆ、に変換され、力制御部４１に入力
される。エンコーダ＆カウンタ３３で検出された各関節
角θ、は座標変換部３９によってハンドの位置姿勢Ｘｒ
に変換される。マニプレータは、速度指令ｖ０と、位置
制御部４２で生成された速度指令ｖｐと、力制御パラメ
ータをもとに力制御部４１で生成された速度指令■、と
の和ｖ＝ｖｐ＋ｖｐ＋ｖｒ に従って動作する。基準座標系における速度■を生成す
るためのマニプレータの各関節速度ｉは逆ヤコビ行列演
算部４０で算出され、 θ−Ｊ−１・■ で与えられる。Ｊ−１は逆ヤコビ行列である。速度サー
ボ系４３がδを実現することにより、マニプレータが動
作する。な゛お、第５図の詳細は特願昭６２−３０４６
０６号に記載されている。

第６図は第５図の制御系を２個のマイクロプロセッサ（
ＤＳＰ）を使用して実現した場合におけるマイクロプロ
セッサの処理手順の例を示す図である。第５図の制御系
の場合、位置制御パラメータや力制′４１「パラメータ
等を状況に応じてリアルタイムで変更する必要があり、
これらのパラメータを一括転送可能なバッファ・エリア
をそれぞれＤＳＰのメモリ空間に第３図（ａ）の形で設
けている。

このタイプを選んだのはＤＳＰのメモリ空間が小さいた
め（内部ＲＡＭ２５６ワード）である。ＤＳＰの場合、
三角関数のライブラリが用意されていないので、近似計
算を先ず行い、これを用いて位置制御部および力制御部
などの演算を行う。

第６図に示すように、１側のマイクロプロセッサ１１で
は下記のような処理が行われる。

■　サンプリング・クロックＳＣＫのトリガ発生をチェ
ックし、サンプリング・タイミングならば、［２］の処
理に進む。

■　ＤＭＡで関節指示速度δを２側のマイクロプロセッ
サからリードする。

■　エンコーダ・カウンタから角度θ、を入力する。

■　サーボ演算を行い、操作量Ｕを出力する。

■　関数発生を行う。

■　角度θ、をＤＭＡで２側のマイクロプロセッサに書
き込む。

■　コマンド解析を行う。

■　コマンドを実行する。

２例のマイクロプロセッサ１２では下記のような処理が
行われる。

■　サンプリング・クロックＳＣＫのトリガ発生をチェ
ックし、サンプリング・タイミングならば、［２］の処
理に進む。

■　三角関数の演算を行う。

■　座標変換３９より現在位置Ｘ、、を求める。

■　位置制御部４２の演算により位置制御方向の速度ベ
クトルｖ、を算出する。

■　力センサーからセンサー信号Ｆ５を入力する。

■　Ｆ、をＦ、に座標変換する。

■　力制御部４１で力の偏差ベクトルＦ、−Ｆ。

に基づき力制御方向の速度ベクトルＶｒを求める。

■　速度指令ｖ０とＶ、とＶ、の和にＪ−１を施し、関
節指示速度ｉを求める。

■°コマンドを解析する。

［相］　コマンドを実行する。

上記のようなハイブリッド制御の場合も、本発明の原理
を適用することにより、通常のシーケンシャルな処理を
行う方法に比べ、特性の良い制御系が実現できる。

次に、本発明のディジタル制御装置のハードウェア構成
を詳細に説明する。第７図はマルチパスとＤＳＰ間及び
ＤＳＰ相互間のインタフェースを示す図である。同図に
おいて、５１と５２はＤＭＡコントローラ、５３ないし
６０はバス・バッファ、６１と６２はボード、＊ＥＣＥ
は拡張メモリ・セレクト、本へ〇ＥＮはアドレス入力イ
ネーフ゛ル、＊ＤＴＥＮはデータ人力イネーブル、＊Ｏ
Ｃはデータ出力イネーブルをそれぞれ示している。

ボード６１には、マイクロプロセッサ１１や外部メモリ
１３、ＤＭＡコントローラ５１、バス・バッファ５３、
バス・バッファ５５、バス・バッファ５７、バス・バッ
ファ５９等が搭載されている。ボード６２は、ボード６
１と同じ構成を有している。ＤＭＡコントローラ５１は
、ＤＳＰコントロール・バスを介してマイクロプロセッ
サ１１と接続されると共に、ＤＳＰ間コントロール・バ
スを介してＤＭＡコントローラ５２と接続されている。

ＤＭＡコントローラ５１．５２は、プログラマブル論理
アレイで構成される。バス・バッファ５３は、ＤＳＰ間
データバスを介してバス・バッファ５４と接続されてい
る。バス・バッファ５５はマルチパス上のアドレスをボ
ード６１の外部データバス上に取り込むものであり、バ
ス・バッファ５７はマルチパス上のデータをボード６１
の外部データバスに取り込むものであり、バス・バッフ
ァ５９はボード６１の外部データバスのデータをマルチ
パスに出力するものである。マルチパスは、例えばＩｎ
ｔｅ１社のマルチパスである。＊ＥＣＥは、ＤＭＡ転送
時と外部メモリ使用時を区別するための信号であり、こ
れを用いて外部メモリをチップ・セレクトする。ホスト
計算機から各マイクロプロセッサ１１．１２に対して書
込み命令が出されると、上位のアドレスがデコードされ
、Ａモード・シーケンスが実行される。各マイクロプロ
セッサ１１．１２のメモリ・アドレスは＊ＡＤＥＮのタ
イミングで取り込まれ、データは＊ＤＴＥＮのタイミン
グで取り込まれる。逆に、ホスト計算機から各マイクロ
プロセッサ１１．１２に対して読込み命令が出されると
、Ａモード・シーケンスに続いてＩモード・シーケンス
が実行され、＊ＯＣのタイミングでマルチパスにデータ
が出力される。ＤＳＰ相互間のデータ転送は、＊Ａ叶倍
信号出力要求）が出力されることにより開始される。

第８図はＤＭＡコントローラの入出力信号を示す図であ
る。本へ〇ＥＮはアドレス人力イネーブル、＊ＤＴＥＮ
はデータ人力イネーブル、＊ＯＣはデータ出力イネーフ
゛ル、本ＲＤはマルチパス・リード・コマンド、＊ＷＴ
はマルチパス・ライト・コマンＦ、ｍＡＯＦは出力要求
、＊ＢＣＴは出力許可、＊ＷＣＫは出力同期クロック、
本Ａｔｐは入力要求、＊ＡＣＴは入力許可、本ＲＣには
入力クロックをそれぞれ示している。また、＊ｏＡｏｐ
のようにＤが付加された信号は、ＤＳＰ間専用の信号で
あり、意味は上記のものに対応する。

第９図はＤＭＡコントローラのＡモード時の状態遷移を
示す図である。状態ＳＯにおいて＊ＲＤ＝１．ｍＷＴ・
１及び＊Ｄ＾ＩＦ＝１になっても、状態はＳＯのままで
ある。状態ＳＯにおいて＊ＲＤ・０または傘−Ｔ・０で
あり且つ＊ＤＡ　Ｉ　Ｆ・１になると、状態はＳＩＯに
遷移する。

状態Ｓ１０において様ＣＴ＝　１になっても状態はＳＬ
Ｏのままであり、＊ＡＣＴ＝０になると状態はＳｌｌに
遷移する。Ｓｌｌに遷移した後、クロックＣＫＩに同期
してＳ１２．Ｓ１３．Ｓ１４．Ｓ１５．Ｓ１６と遷移す
る。状態Ｓ工６においては＊ＡＣＴ・０になっても状態
は変化せず、＊ＡＣＴ・１になると３１７に遷移する。

状態Ｓ１７において寧ＲＤ＝０または＊ＷＴ＝Ｏになっ
ても状態は変化せず、本ＲＤ。

１で且つ本−Ｔ・１になると、状態はＳＯに遷移する。

状態ＳＯにおいて＊ＤＡＩＦ・０になると、状態はＳ２
０に遷移する。状Ｂ５２０において＊ＡＣＴ・１になっ
ても状態は変化せず、峠ＣＴ・０になると、状態はＳ２
１に遷移する。状態Ｓ２１において＊ＡＣＴ＝Ｏになっ
ても状態は変化せず、本ＡＣＴ・１になると状態はＳＯ
に遷移する。

Δモードの各状態におけるＤＭＡコントローラからの信
号出力は、下記のようになる。

Ｓｏ　　：　　＊＾ＩＦ＝１．＊ＲＣＫ＝１．＊ＡＤＥ
Ｎ＝１．＊ＤＴＥＮ＝１．＊ＡＣＫ＝１．＊ＤＡＣＴ＝
１ Ｓ１０：傘Ａ　ＩＦ＝０．　＊ＲＣＫ＝１．　＊ＡＤＥ
Ｎ＝＊ＡＣＴ、　ＤＴＥＮ＝１−＾ｃｇ＝ｔ。

＊ＤＡＣＴ＝１ Ｓｌｌ：　　＊ＡＩＦ＝帆＊ＲＣＫ＝Ｏ，＊ＡＤＥＮ＝
Ｏ，＊ＤＴＥＮ＝１　、＊ＡＣＫ＝１．本ＤＡＣＴ＝１ Ｓ１２：　傘ＡＩＦ・ｏ、＊ｐｃＫ・ｌ、＊ＡＤＥＮ＝
Ｏ，本ＤＴＩＥＮ＝１．＊ＡＣＫ＝１．＊Ｄ八ＣＴ・ｌ ５１３：　ネへＩＰ＝１．＊ＲＣＫ＝１．傘ＡＤＥＮ＝
１．ネ［］ＴＩＥＮ＝０．傘八ＣＫ＝１．ネＤ八ＣＴ＝
１ Ｓ１４：　　＊＾ＩＦ・１１本ＲＣＫ＝Ｏ，＊ＡＤＥＮ
＝１．＊ＤＴＥＮ＝Ｏ，＊ＡＣＫ＝１．本Ｄ＾ＣＴ＝１ Ｓ１５：　　＊ＡりＦ＝１．ネＲＣＫ＝１．＊ＡＤＥＮ
＝１．宰ＤＴＥＮ＝Ｏ，本ＡＣＫ＝１　、＊［］ＡＣＴ
＝１ Ｓ１６二　本ＡＩＦ＝１．本Ｒ（Ｊ＝１．＊ＡＤＥＮ＝
１．本ＤＴＥＮ＝１．車八ＣＫ＝１．本り八ＣＴ、ｌ ５１７二　本ＡＩＦ＝１．本ＲＣＫ＝１．＊ＡＤＥＮ工
１．＊ＤＴＥＮ＝１．ｍＡＣＫ＝＊讐Ｔ。

＊ＤＡＣＴ＝１ Ｓ２０：　　＊ＡＩＦ−本ＤＡＩＦ、本ＤＡＣＴ＝１．
＊ＲＣＫ＝１．＊＾ＤＥＮ＝Ｌ、＊ＤＴＥＮ＝１１本Ａ
ＣＫ＝１ Ｓ２１：　本ＡＩＦ＝ネＤＡＩＦ、＊ＤＡＣＴ＝＊ＡＣ
Ｔ、＊ＲＣＫ−＊ＤＲＣＫ、車ＡＤＥＮ＝１、＊ＤＴＥ
Ｎ＝１．＊ＤＴＥＮ・ＩＩ＊八Ｃへ、１第１０図はＡモ
ードのタイミング・チャートである。先ず、マルチパス
からＤＳＰＩ（又はＤＳＰ２）にデータを転送する場合
について説明する。

状態ＳＯにおいて＊ＷＴ＝Ｏになると、状態はＳＩＯに
遷移する。なお、状態遷移はクロックＣＫ　１　（１６
ＫＨｚ）に同期して行われる。状態ＳＩＯにおいては、
本ＡＩＦ＝Ｏ。

傘ＲＣＫ＝１．＊ＡＤＥＮニネＡＣＴ、ＤＴＩ’Ｎ＝１
．＊ＡＩＪ＝１．本ＤＡＣＴ＝１になる。なお、ｍＡ（
Ｊはコマンドに対する応答（アクノリッジ）信号である
。状態ＳＩＯにおいて率ＡＣＴ＝０になると、状態はＳ
ｌｌに遷移する。状ＢＳＩＩにおいては、　＊ＡＩＦ＝
Ｏ，＊ＲＣＫ＝Ｏ，＊八ＤＥＮ＝Ｏ，＊ＤＴＥＮ＝１へ
、本ＡＣＫ＝１．＊ＤＡＣＴ＝１である。状態Ｓ１１に
おいてクロック（、Ｋｌが立ち上がると、状態はＳｌｌ
からＳ１２に遷移する。

状態Ｓ１２においては、本ＡＩＦ・０１本ＲＣＫ＝　１
　、本ＡＤＥＮ・０１本ＤＴＥＮ＝　１　、５ＡＣＫ・
１．傘ＤＡＣＴ・１である。状態Ｓ１２においてクロッ
クＣＫＩが立ち上がると、状態はＳ１２から５１３に遷
移する。状態３１３′においては、＊ＡＩＦ＝１．＊Ｒ
ＣＫ２１１本ＡＤＥＮ＝１．　＊ＤＴＥＮ＝Ｏ，＊ＡＣ
Ｋ＝ｌ　、　＊ＤＡＣＴ＝１である。状態Ｓ１３におい
てクロックＣＫＩが立ち上がると、状態はＳ１３からＳ
１４に遷移する。状態Ｓ１４においては、寧ＡＩＦ＝１
．本ＲＣＫ＝Ｏ，＊＾ＤＥＮ＝１．＊ＤＴＥＮ＝Ｏ，＊
＾ＣＫ＝１　、　＊ＤＡＣＴ・１である。状ＪＩＱＳ１
４においてクロックＣＫＩが立ち上がると、状態はＳ１
４から３１５に遷移する。状態Ｓ１５においては、＊Ａ
ｔｐ＝ｔ、傘ＲＣＫ＝１．＊ＡＤＥＮ・１　、　＊ＤＴ
ＥＮ・Ｑ、　＊ＡＣＫ＝１．本ＤＡＣＴ＝１である。状
Ｂ５１５においてクロックＣＫＩが立ち上がると、状態
は３１６に遷移する。状態Ｓ１６においては、＊ＡＩＦ
＝１．＊ＲＣＫ＝１．＊ＡＤＥＮ＝１、車ＤＴＥＮ＝１
．本ＡＣに＝１．’＊ＤＡＣＴ・１である。状ｊｌＪ　
Ｓ　１６において様ＣＴ＝　１になると、状態はＳ１６
から５１７に遷移する。状ＭＳ１７におし１ては、＊Ａ
ＩＦ＝１．＊ＲＣＫ・１１本八へＥＮ＝１．本ＤＴＥＮ
＝１．本＾ＣＫ＝本−Ｔ１本ＤＡＣＴ＝１である。状態
Ｓ１７において本−丁・１になると、状態はＳ１７から
ＳＯに遷移する。

次に、ＤＳＰＩ（又はＤＳＰ２）からＤＳＰ２（ＤＳＰ
Ｉ）にデータを転送する場合について説明する。状態Ｓ
Ｏにおいて＊ＤＡ　ＩＦ、Ｏになると、状態はＳＯから
３２０に遷移する。状Ｂ５２０においては、本ＡＩＦ＝
傘ＤＡＩＦ、本ＤＡＣＴ＝１　、＊ＲＣＫ＝１．＊ＡＤ
ＥＮ工ｌ１本口ＴＥＮ＝１．本ＡＣＫ・１である。状態
Ｓ２０において＊ＡＣＴ＝Ｏになると、状態はＳ２０か
ら３２１に遷移する。状態Ｓ２１においては、　率＾Ｉ
Ｆ＝＊ＤＡＩＦ、＊ＤＡＣＴ＝＊＾ＣＴ、宰ＲＣＫ＝＊
ＤＲＣＫ、＊ＡＤＥＮ・ｌ　、　＊ＤＴＩｌ！Ｎ・１９
本口ＴＥＮ・１５本ＡＣＫ・１である。状態Ｓ２１にお
いて本ＡＣ？＝１になると、状態はＳ２１からＳＯに遷
移する。

第１１図はＤＭＡコントローラの■モード時の状Ｂ遷移
を示す図である。状ＢＳＯにおいて＊Ａｏｐ・１又は様
ＣＴ＝Ｏ又は＊ＤＢＣＴ＝１　になっても、状態はＳＯ
のままである。状態ＳＯにおいて本Ａｏｐ・Ｑ、　車Ａ
ＣＴ＝１及び傘ＤＢＣＴ＝Ｏになると、状態はＳＯから
Ｓｌに遷移する。

状態Ｓ１において、＊ＡＯＦ＝１又は本ＷＣＫ＝１にな
っても状態は変化ぜず、＊ＡＯＦ・０．＊騙ＣＫ＝０及
び＊Ｕ・７であると、状態はＳｌからＳ２に遷移する。

なお、Ｕ−（０１４，Ｄｉ３．０１２　）であり、機蕃
を表す。なお、ＤＳＰＩに対しては機番１、ＤＳＰ２に
対しては機番２、マルチパス・マスクに対しては仮想的
に機番７が設定されている。状態Ｓ２において、峠ｏｐ
＝ｏ又は本ＷＣＫ・０になっても状態は変化せず、４叶
・１で且つ峠ＣＫ・０であると、状態はＳ２から８３に
遷移する。状態Ｓ３においてクロックＣＫ２が立ち上が
ると、状態はＳ４に遷移する。状態Ｓ４において、宰Ｒ
Ｄ・０になっても状態は変化せず、＊Ｒｏ＝ｉであると
、状態はＳ４からＳＯに遷移する。状態Ｓ１において本
ＡＯＦ＝０．　＊ＷＣＫ＝Ｏ及び＊Ｕ≠７になると、状
態はＳｌから３１１に遷移する。

状態５１１において、＊ＤＢＣＴ＝Ｏになっても状態は
変化せず、本ＤＢＣＴ・１であると、状態はＳｌｌから
Ｓｏに遷移する。

■モードの各状態におけるＤＭＡコントローラからの信
号出力は下記のようになる。

ＳＯ：　　本ＢＣＴ＝１．傘０Ｃ＝１．＊ＡＣＫ＝１．
本０ＡＯＦ−本ＡＯＦｏｒＮＯＴ本ＡＣＴ。

＊ＤＷＣＫ＝本−ＣＭ Ｓｌ：　　傘ＢＣＴ＝０．本０Ｃ＝１．＊ＡＣＫ＝１．
車ＤＡＯＦ＝＊ＡＯＦｏｒＮＯＴ＊ＡＣＴ、＊ＤＷＣＫ
＝本−ＣＫ Ｓ２：　　本ＢＣＴ＝Ｏ，本０Ｃ＝１．本ＡＣＫ＝１．
傘ＤＡＯＦ−本ＡＯＦｏｒＮＯＴ＊ＡＣＴ。

＊ＤＷＣＫ＝＊ＷＣＫ Ｓ３：　　ＪＣＴ＝Ｏ，本０Ｃ＝Ｏ，＊ＡＣＫ＝１．＊
ＤＡＯＦ＝＊八０ＦｏｒＮＯＴ＊ＡＣＴ。

＊ＤＷＣＫ＝＊ＷＣＫ Ｓ４：　　ＪＣＴ＝本ＤＢＣＴ、＊ＯＣ＝本ＲＤ、傘Ａ
ＣＫ＝本ＲＤ、＊０ＡＯＦ＝＊ＡＯＦｏｒＮＯＴ＊八Ｃ
Ｔ、＊ＤＷＣＫ−本ＷＣＫＳｌｌ：　　＊［１ＣＴ＝本
ＤＢＣＴ、＊０Ｃ＝ｌ、＊＾ＣＫ＝１．＊ＤＡＯＦ＝＊
ＡＯＦｏｒＮＯＴ＊ＡＣＴ、本ＤＷＣＫ、車−ＣＫ 第１２図は■モードのタイミング・チャートを示す図で
ある。先ず、ＤＳＰＩ（又はＤＳＰ２）からマルチパス
へのデータ転送について説明する。

状ＢＳＯにおいては、＊ＢＣＴ＝１．＊０Ｃ＝１．本Ａ
ＣＫ＝１．本ＤＡＯＦ＝＊ＡＯＦｏｒＮＯＴ本＾ＣＴ、
　＊ＤＷＣＫ＝本ＷＣＫである。状態ＳＯにおいて＊Ａ
ＯＦ・０．＊ＡＣＴ・１及び本ＤＢＣＴ・０になると、
状態はＳＯから３１に遷移する。なお、状態遷移はクロ
ックＣＫ２に同期して行われる。状ＢＳ１においては＊
ＢＣＴ＝０．＊０Ｃ＝１　、＊へＣＫ＝１．＊ＤＡＯＦ
−＊ＡＯＦｏｒＮＯＴ＊ＡＣＴ　　、本ＤＷＣＫ＝＊Ｗ
ＣＫである。状ＢＳ１において、＊ＡＯＦ＝Ｏ，＊ＷＣ
Ｋ＝０及びＵ＝７であると、状態はＳｌから３２に遷移
する。状態Ｓ２においては、＊ＢＣＴ＝Ｏ，＊０Ｃ＝１
　、　＊ＡＣＫ＝１．　＊ＤＡＯＦ＝＊ＡＯＦｏｒＮＯ
Ｔ＊ＡＣＴ、　＊ＤＷＣＫ＝＊ＷＣＫである。状態Ｓ２
において様ＯＦ・１で且つｄｃＫ・０であると、状態は
Ｓ２から８３に遷移する。状ＵＳ３においては＊ＢＣＴ
・０．＊０Ｃ＝Ｏ，＊＾ＣＫ＝１、本０＾ＯＦ＝本ＡＯ
ＦｏｒＮＯＴ＊ＡＣＴ、　傘ＤＷＣＫ＝＊ＷＣＫである
。　状態Ｓ３においてクロックＣＫ２が立ち上がると、
状態はＳ３から８４に遷移する。状態Ｓ４においては、
＊ＢＣＴ・本ＤＢＣＴ、　＊ＯＣ＝＊ＲＤ、　＊ＡＣＫ
＝＊ＲＤ、　＊Ｄ＾ＯＦ＝＊＾０ＦｏｒＮＯＴ＊ＡＣＴ
、　＊ＤＷＣＫ−傘−ＣＫである。状態Ｓ４において＊
ＲＤ＝１になると、状態はＳ４からＳＯに遷移する。

次に、ＤＳＰＩ（又はＤＳＰ２）からＤＳＰ２（ＤＳＰ
Ｉ）にデータを転送する場合について説明する。状態Ｓ
１において、車ＡＯＦ＝０−匈ＣＫ、Ｏ及びＵ≠７であ
ると、状態はＳｌからＳｌｌに遷移する。状態Ｓｌｌに
おいては、本ＢＣＴ＝本ＤＢＣＴ、＊０Ｃ＝１．＊ＡＣ
Ｋ＝１．＊［１ＡＯＦ−寧Ａ叶ｏｒＮＯＴ本ＡｃＴ、　
本ＤＷＣＫ＝＊ＷＣＫである。状態ＳｌｌにおいてネＤ
ＢＣＴ・１になると、状態はＳｌｌからＳＯに遷移する
。

〔発明の効果〕

以上の説明から明らかなように、本発明によれば、力と
位置を同時に制御する力制御系において、サンプリング
周期を短くできると共に、サーボの演算時間遅れを著し
く減少することが出来るので、制御系全体の特性を向上
させることが出来、また、それぞれのマイクロプロセッ
サの内部処理とは独立に、ホスト計算機から非同期に所
定のコマンドやパラメータを送信することが出来る。

【図面の簡単な説明】

第１図は本発明のディジタル制御装置のハードウェア構
成を示す図、第２図は本発明におけるマイクロプロセッ
サの処理手順を示す図、第３図は本発明におけるホスト
・インタフェース・バッファの例を示す図、第４図は本
発明と従来例の演算処理の比較を示す図、第５図は位置
と力のハイブリッド制御の実施例を示す図、第６図は第
５図の制御系を２個のマイクロプロセッサを使用して実
現した場合におけるマイクロプロセッサの処理手順の例
を示す図、第７図はマルチパスとＤＳＰ間およびＤＳＰ
相互間のインタフェースを示す図、第８図はＤＭＡコン
トローラの入出力信号を示す図、第９図はＤＭＡコント
ローラのＡモード時の状態遷移を示す図、第１０図はＤ
ＭＡコントローラのＡモード時のタイミング・チャート
を示す図、第１１図はＤＭＡコントローラのＩモード時
の状態遷移を示す図、第１２図はＤＭＡコントローラの
Ｉモード時のタイミング・チャートを示す図、第１３図
は多関節ロボットの力制御系の例を示す図、第１４図は
従来の力制御系のハードウェア構成例を示す図、第１５
図は６関節ロボットの例を示す図である。 １１と１２・・・マイクロプロセッサ、１３と１４・・
・外部メモリ、１５と１６・・弓１０装置、１７と１８
・・・ローカル・バス、１９と２０・・・バス、２１・
・・同期用クロック源、２２・・・ＤＭＡシーケンス制
御部、２３・・・ホスト・インクフェース制御部、２４
・・・ホスト計算機、３０・・・ロボット・マユブレー
ク、３１・・・モータ、３３・・・エンコーダ＆カウン
タ、３４・・・パワーアンプ、３５・・・Ｄ／Ａ変ｔＡ
　２Ｎ、３６・・・サーボ演算・関数発生部、３７・・
・力センサ−，３８・・・座標変換部、３つ・・・座標
変換部、４０・・・逆ヤコビ行列演算部、４１・・・力
制御部、４２・・・位置制御部、４３・・・速度サーボ
系、５１と５２・・・ＤＭＡコントローラ、５３ないし
６０・・・ハス・バッファ、６１と６２・・・ボード。 特許出願人　　　富士通株式会社 代理人弁理士　　京　谷　四　部 （ａ）　　　　　　　　　（ｂ） （ｃ） ホスト・イングツニー久・バヅファの杖を十分Ａ第３図 イｆＬＬと力のハイフ゛リッド惨Ｉ？ＪＰの＊方匪分」
第５図 マルケノ（人ヒＤＳＰＭ及ドＤＳＰ湘亙間乃イングフェ
ース尭７図 ＤＭＡコントローラの入出カイ芭号 浦８図 ＤＭＡコントローラのＡモード時の状態、遷木多Ｍ９図 ■＠■Ｏ■Ｏ＠　■［相］ ■■■（２）Ｏ■■　− 京ＡＯＦ＝１　ｏｒ　＊ＡＣＴ＝Ｏ ｏｒ　＊　ＤＢＣＴ　＝　ｌ ＤＭＡコンドローラリＩモード吟の伏胱ｌ神Ｍ１１図 ■■＠■［有］■［相］　　［相］

Claims (2)

【特許請求の範囲】
1. （１）アームの先端にハンドが取り付けられると共にハ
   ンドに作用する力を検出する力センサーを持つ多関節ロ
   ボットまたは、自動機を制御するディジタル制御装置で
   あって、 内部にメモリを有すると共にＤＭＡ機能を持つ第１のマ
   イクロプロセッサ（１１）と、 内部にメモリを有すると共にＤＭＡ機能を持つ第２のマ
   イクロプロセッサ（１２）と、 第１のマイクロプロセッサ（１１）にローカル・バス（
   １７）を介して接続された外部メモリ（１３）および入
   出力装置（１５）と、 第２のマイクロプロセッサ（１２）にローカル・バス（
   １８）を介して接続された外部メモリ（１４）および入
   出力装置（１６）と、 ＤＭＡシーケンス制御部（２２）と、 ＤＭＡシーケンス制御部（２２）と第１のマイクロプロ
   セッサ（１１）とを接続するバス（１９）と、ＤＭＡシ
   ーケンス制御部（２２）と第２のマイクロプロセッサ（
   １２）とを接続するバス（２０）と、第１のマイクロプ
   ロセッサ（１１）および第２のマイクロプロセッサ（１
   ２）に対してサンプリング・クロックＳＣＫを供給する
   同期クロック源（２１）と、ホスト計算機（２４）と、 ホスト計算機（２４）のバスとマイクロプロセッサ側の
   ＤＭＡインタフェースとを接続するホスト・インタフェ
   ース制御部（２３）と、 を具備し、且つ ＤＭＡシーケン制御部（２２）は、第１のマイロクプロ
   セッサ（１１）と第２のマイクロプロセッサ（１２）間
   のＤＭＡ転送、ホスト計算機（２４）と第１のマイクロ
   プロセッサ（１１）間のＤＭＡ転送、ホスト計算機（２
   ４）と第２のマイクロプロセッサ（１２）間のＤＭＡ転
   送のタイミングが重なった場合に、競合を回避する機能
   を持ち、 第１のマイクロプロセッサ（１１）は、 ［１］サンプリング・クロックＳＣＫのトリガ発生をチ
   ェックし、サンプリング・タイミングならば、［２］の
   処理に進む。 ［２］ＤＭＡ転送により１ステップ前の関節指示速度ｖ
   を入力する。 ［３］関節角θを入力する。 ［４］ｅ＝ｒ−θに応じてサーボ演算を実行し、モータ
   駆動力ｕを更新する。 ［５］［２］で入力した１ステップ前の関節指示速度ｖ
   に応じて目標位置ｒを計算する。［６］［３］で入力し
   た関節角θを次のサンプリング時の座標変換のためにマ
   イクロプロセッサ（１２）に送る。 ［７］ホスト計算機からのコマンドを解析する。 ［８］必要に応じて各コマンドを実行する。 等の処理を行うように構成され、 第２のマイクロプロセッサ（１２）は、 ［１］サンプリング・クロックＳＣＫのトリガ発生をチ
   ェックし、サンプリング・タイミングならば、［２］の
   処理に進む。 ［２］力センサー信号Ｆ＾Ｈを入力する。 ［３］力センサー信号Ｆ＾Ｈを１ステップ前の姿勢θを
   もとに基準座標系の値Ｆ＾Ｒに変換する。 ［４］Ｆ＾Ｒをホスト計算機から設定したＦ＿ｉ＾Ｒと
   比較し、Ｆ＿■＾Ｒ＝Ｆ＿ｉ＾Ｒ−Ｆ＾Ｒに応じて適当
   な補償演算を行い、この結果を指示速度ｖ＾Ｒとする。 ［５］指示速度ｖ＾Ｒを関節速度ｖに変換し、次のサン
   プリング時にマイクロプロセッサ（１１）側に渡す。 ［６］ホスト計算機からのコマンドを解析する。 ［７］必要に応じて各コマンドを実行する。 等の処理を行うように構成されている ことを特徴とするディジタル制御装置、 ただし、Ｆ＿ｉ＾Ｒは基準座標系での力の設定値、Ｆ＾
   Ｒは基準座標系での力の現在値、Ｆ＿■＾Ｒは基準座標
   系での力の偏差、Ｆ＾Ｈはハンド座標系での力の観測値
   、ｖ＾Ｒは基準座標系での指示速度、ｖは関節座標系で
   の指示速度、ｒは関節座標系での目標位置、θは関節座
   標系での現在位置、ｅは関節座標系での偏差、ｕは操作
   量をそれぞれ示す。
2. （２）第１のマイクロプロセッサ（１１）および第２の
   マイクロプロセッサ（１２）は、それぞれホスト計算機
   （２４）から送られてくるデータを格納するホスト・イ
   ンタフェース・バッファを有することを特徴とする請求
   項（１）記載のディジタル制御装置。