JPH0651838A

JPH0651838A - 倣い制御装置

Info

Publication number: JPH0651838A
Application number: JP20679192A
Authority: JP
Inventors: Yasuyuki Nakada; 康之中田; Mitsuru Shiraishi; 満白石
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 1992-08-03
Filing date: 1992-08-03
Publication date: 1994-02-25

Abstract

(57)【要約】【目的】本発明は、部材と接触を保ちながら移動する
ロボットを制御する倣い制御装置に関し、オペレータ
が、より簡便に制御変数設定のためのコマンドの入力を
行うことができることを目的とする。【構成】倣い制御装置において、外部より入力される
コマンドの制御変数に基づいて、制御手段４が接触倣い
ロボット５を制御する。コマンド解析装置１は、コマン
ド解釈手段２と制御変数生成手段３とよりなる。コマン
ド解釈手段２は、入力されるコマンドの文字列を解釈す
る。制御変数生成手段３は、コマンド解釈手段２による
コマンド解釈の結果と、ロボット５の現在の状態を示す
信号に基づいて制御変数を生成する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は倣い制御装置に係り、特
に、部材と接触を保ちながら移動するロボットを制御す
る倣い制御装置に関する。

【０００２】塗布作業、バリ取り作業、レンチによるネ
ジの締結動作、ドアの開閉動作、ラップトップパソコン
の蓋の開閉動作等、ロボットが部材と接触を保ちながら
移動する作業を行うロボットを、接触倣いロボットとい
う。この接触倣いロボットを制御する倣い制御装置にお
いては、動作に必要な制御変数を簡便に設定できること
が必要とされている。

【０００３】

【従来の技術】図１１は、従来の倣い制御装置の一例の
構成図を示す。図１１に示すように、この従来の倣い制
御装置は、ホストコンピュータ５１と制御コントローラ
５２とからなる。制御コントローラ５２は、制御装置の
動作に必要な制御変数を設定しておく制御コントローラ
メモリ５３と、制御コントローラメモリ５３に設定され
た制御変数に基づいて、ロボットを制御する制御部５４
とからなる。

【０００４】オペレータは、動作開始コマンドを入力す
る前に、動作に必要な制御変数を設定する必要がある。
この制御変数の設定は、ホストコンピュータ５１のモニ
タプログラムのメモリ・ライト命令を用いて、制御コン
トローラメモリ５３の、制御変数に対応する番地に適当
な値を書き込むことで行う。

【０００５】ロボット５５の接触倣い動作では、制御部
５４で使用されるフィードバック・ゲイン等、何種類も
の制御変数を設定する必要があり、オペレータは、全て
の制御変数を設定する。

【０００６】図１２は、従来の倣い制御装置の別の例の
構成図を示す。同図中、図１１と同一構成部分には、同
一符号を付し、適宜説明を省略する。図１２の倣い制御
装置は、図１１の装置に、コマンド列ファイル装置５０
を設けている。

【０００７】コマンド列ファイル装置５０中のコマンド
列ファイルから、コマンド列がホストコンピュータ５１
により読み込まれ、このコマンド列中の、動作に必要な
制御変数が制御コントローラメモリ５３に自動的に設定
される。コマンド列ファイル装置５０には、オペレータ
が、コマンドを予め入力して、倣い制御装置の動作に必
要な制御変数を含むコマンド列ファイルを作成してお
く。

【０００８】図１３は、コマンド列ファイルの一例を示
す。図１３に示すように、各種の制御変数を設定するた
めのコマンドを制御変数ごとに記述する。

【０００９】

【発明が解決しようとする課題】しかるに、図１１の従
来の倣い制御装置では、動作をさせるために、全ての制
御変数を、対応するメモリの番地に書き込まなければな
らず、オペレータは、制御変数を入力するのに、煩雑な
作業をしなければならないという問題があった。

【００１０】また、図１２のコマンド列ファイル装置５
０を設けた倣い制御装置では、新たなコマンド列ファイ
ルの作成時、及び、コマンド列ファイルの保守時に、各
種制御変数を設定するためのコマンド列を、各種制御変
数ごとに入力する必要があり、オペレータは、コマンド
列を入力するのに、煩雑な作業をしなければならないと
いう問題があった。

【００１１】本発明は、上記の点に鑑みてなされたもの
で、オペレータが、簡便に制御変数設定のためのコマン
ドを入力することができる倣い制御装置を提供すること
を目的とする。

【００１２】

【課題を解決するための手段】図１は本発明の原理構成
図を示す。請求項１の発明は、外部より入力されるコマ
ンドの制御変数に基づいて、制御手段４が接触倣いロボ
ット５を制御する倣い制御装置において、入力されるコ
マンドの文字列を解釈するコマンド解釈手段２と、コマ
ンド解釈手段２によるコマンド解釈の結果と、ロボット
５の現在の状態を示す信号に基づいて制御変数を生成す
る制御変数生成手段３とよりなるコマンド解析装置１を
有する構成とする。

【００１３】請求項２の発明では、制御変数生成手段３
は、コマンド中で指定される進行方向と軌道平面とから
制御方向に関する制御変数を生成する構成とする。

【００１４】請求項３の発明では、制御変数生成手段３
は、前記コマンド中で指定される動作時間から、時間に
関する必要な制御変数を生成する構成とする。

【００１５】

【作用】請求項１の発明の、コマンド解析装置１が入力
されるコマンドを解釈して、現在のロボットの状態を示
す信号に基づいて制御変数を生成する構成は、制御変数
を設定するコマンドを、簡潔で平易な形式で記述し、よ
り少ないコマンドの列で、接触倣い動作を記述すること
を可能とする。

【００１６】請求項２の発明の、コマンド中で指定され
る進行方向と軌道平面とから制御方向に関する制御変数
を生成する構成は、オペレータが、制御方向に関する制
御変数を設定するコマンドを入力することを不要とす
る。

【００１７】請求項３の発明の、コマンド中で指定した
動作時間から、時間に関する必要な制御変数を生成する
構成は、オペレータが、時間に関する制御変数を設定す
るコマンドを入力することを不要とする。

【００１８】

【実施例】図２は本発明の一実施例の構成図を示す。図
２に示すように、本実施例の倣い制御装置は、コマンド
列ファイル装置１０、コマンド解析装置１、制御手段４
を構成するホストコンピュータ１４と制御コントローラ
１５とからなる。

【００１９】制御コントローラ１５は、倣い制御装置の
動作に必要な制御変数を設定しておく、制御変数記憶手
段である制御コントローラメモリ１６と、制御コントロ
ーラメモリ１６に設定された制御変数に基づいて、ロボ
ットを制御する制御手段である制御部１７とからなる。

【００２０】オペレータ、或いは、コマンド列ファイル
装置１０から、コマンド解析装置１に、動作コマンドの
文字列が入力される。コマンド解析装置１では、与えら
れた動作コマンドの文字列から、制御方向、動作方向、
制御フラグを解釈し、制御変数をホストコンピュータ１
４へ転送する。

【００２１】ホストコンピュータ１４のモニタプログラ
ムでは、転送された制御変数を、制御コントローラメモ
リ１６に設定する。制御コントローラ１５の制御部１７
は、制御コントローラメモリ１６に設定された制御変数
と、ロボット５の位置情報、力情報を参照して、ロボッ
ト５の制御を行う。

【００２２】先ず、本実施例における基準座標系と作業
座標系の関係について説明する。作業座標系は、倣い動
作の制御方向を表すために用いられ、ロボット５先端と
部材間の接触点を原点とし、軌道の法線をＸ軸、接線方
向をＺ軸、Ｘ軸とＺ軸に直交する方向をＹ軸として定義
される。各軸を制御方向と対応付けると、下記のように
なる。

【００２３】Ｘ軸力制御方向Ｙ軸相対目標位置ゼロの軌道制御方向Ｚ軸ロボットの進行方向作業座標系のＸ、Ｙ、Ｚ軸のそれぞれの単位ベクトルを
ｎ_L、ｏ_L、ａ_Lと置き、単位ベクトルを求めることで
座標系を決定する。作業座標系は、次のようにして算出
される。ロボット５と部材間の接触力ベクトル^OＦ_flw
は、下記のように表せる。

【００２４】

【数１】

【００２５】を用いて法線方向の単位ベクトル^Oｎ_Lを
求めると、下記のように表せる。

【００２６】

【数２】

【００２７】単位ベクトル^Oａ_Lは、制御コントローラ
メモリ１６に設定されている軌道平面を指定する軌道平
面指定ベクトル^Oｏ_OPを基に算出する。^Oｎ_Lと^Oｏ_OP
からベクトル^Oａ_Lを求めると、^Oａ_Lは、下記のよう
に表せる。

【００２８】

【数３】

【００２９】このとき、もう一つの軌道制御方向の単位
ベクトル^Oｏ_Lには、軌道平面指定ベクトル^Oｏ_OPを用
い、下記のように置く。

【００３０】^Oｏ_L＝^Oｏ_OP 作業座標系で記述されているベクトルを基準座標系での
表現へ変換する変換行列^OＲ_Lを、算出した^Oｎ_L、^O
ｏ_L、^Oａ_Lで表すと、下記のように表せる。

【００３１】

【数４】

【００３２】変換行列^OＲ_Lは、作業座標系上で算出さ
れた速度指令の基準座標系への変換、基準座標系で表さ
れた位置、力の作業座標系への変換に用いられる。

【００３３】次に、制御変数について説明する。制御コ
ントローラメモリ１６に設定する制御変数には、軌道平
面指定ベクトル^Oｏ_op、基準座標系の選択行列^OＳ_p、
動作時間ｔ_ind、一定速度移動開始時刻ｔ_st 、一定速
度移動終了時刻ｔ_en、制御モードフラグｃｍｄｆｌｇ、
作業座標系の選択行列^LＳ_f、^LＳ_p、^LＳ_oa、倣い速
度ｖ_flw、加速度ａ_CL等が有る。これらの制御変数は、
制御コントローラ１５の制御部１７において、各種演算
のために用いられる。

【００３４】軌道平面指定ベクトル^Oｏ_opは、前記〔基
準座標系と作業座標系〕の項で説明したように、軌道制
御方向（作業座標系のＹ軸方向）を基準座標系で示した
単位ベクトルである。

【００３５】基準座標系の選択行列^OＳ_pは、制御コン
トローラ１５の制御部１７で、各種演算のときに、基準
座標系の各軸成分の抽出に用いる。本実施例では、選択
行列 ^OＳ_pは、基準座標系で、（１００）、（０１
０）、（００１）のいずれかである。

【００３６】制御モードフラグｃｍｄｆｌｇは、倣い制
御、位置制御、力制御のいずれかを指示するフラグであ
る。作業座標系の選択行列^LＳ_f、^LＳ_p、^LＳ_oa、
は、作業座標系での各軸方向の成分を抽出するのに用い
られる。作業座標系で、^LＳ_f＝（１００）、^LＳ_p＝
（０１０）、^LＳ_oa＝（００１）である。

【００３７】動作時間ｔ_ind、一定速度移動開始時刻ｔ
_st 、一定速度移動終了時刻ｔ_en、最大速度ｖ_flw、加
速度ａ_CLは、進行方向（作業座標系のＺ軸方向）の速度
関数を定める制御変数である。図４は、この速度関数の
説明図を示す。速度関数^Lｖ _oaは、図４に示すような台
形状の関数である。ここで、一定速度動作開始時刻ｔ _st
と、一定速度動作終了時刻ｔ_enは、下記の式で与えられ
る。

【００３８】ｔ_st＝ｖ_flw／ａ_CL ｔ_en＝ｔ_ind−ｖ_flw／ａ_CL 図４の速度関数を式で表すと、下記の式となる。

【００３９】

【数５】

【００４０】次に、コマンド解析装置１、ホストコンピ
ュータ１４、制御コントローラメモリ１６、及び制御部
１７のそれぞれについて、詳細に説明する。〔コマンド解析装置〕コマンド解析装置１は、図４の構
成図に示すように、コマンド解釈手段２を構成するコマ
ンド読み込み部２１と文字列解釈部２２、制御変数生成
手段３を構成する制御変数生成部２３と制御変数転送部
２４とからなる。

【００４１】コマンド読み込み部２１は、オペレータ、
或いは、コマンド列ファイル装置１０から与えられたコ
マンドを読み込み、文字列解釈部２２は、コマンドを構
成する文字列から、コマンド名部分と引き数部分を解釈
する。制御変数生成部２３は、文字列を解釈した結果を
基に、制御変数を生成し、制御変数転送部２４は、ホス
トコンピュータ１４のモニタプログラムへ、制御変数生
成部２３で生成された制御変数を引き渡す。

【００４２】図５は本実施例におけるコマンド列の一例
を示す。コマンド列は、図５に示すように、コマンド名
部分と、引き数部分からなり、一行が一コマンドに対応
している。倣い動作を実行するために必要なコマンド
は、２種類に分けられ、倣い動作開始時に必要な制御変
数を設定するコマンドと、その他の制御変数を設定する
コマンドがある。

【００４３】図５の第１群のコマンドは、倣い動作を行
うためのコマンドであり、動作時に必ず実行しなければ
ならない。第２群のコマンドは、倣い動作を行うときに
必ずしも実行する必要はなく、制御変数を変更する必要
が生じたときに、適宜実行すればよい。

【００４４】図５の第１群の倣い動作のコマンド“ｆｏ
ｌｌｏｗｘ＋ｘｙ１０”は、図１３の従来装置の
コマンド列ファイル例に対応している。ここで、“ｆｏ
ｌｌｏｗ”は、倣い動作を指示するコマンド名、“ｘ
＋”は、進行方向を指定する引き数、“ｘｙ”は、軌道
平面を指定する引き数、“１０”は、動作時間を指定す
る引き数を意味する。

【００４５】従来装置では、多数のコマンドで記述して
いたものを、本実施例では、“ｆｏｌｌｏｗｘ＋ｘ
ｙ１０”の一行で記述できる。これから分かるよう
に、本実施例では、制御変数設定のためのコマンドを従
来装置に比べて、極めて簡潔で平易な形で記述できる。

【００４６】コマンド読み込み部２１では、コマンド列
から一行分のコマンドを読み込み、コマンド文字列を一
時記憶メモリに格納する。コマンドを実行している間
は、次のコマンドの読み込み待ちの状態を保つ。読み込
み待ち状態は、制御コントローラ１５の制御部１７の動
作シーケンス生成部４１でコマンド終了信号が発生する
と解除され、次のコマンドを読み込む。

【００４７】図６は、文字列解釈部の処理の例を示すフ
ローチャートである。文字列解釈部２２では、コマンド
文字列をコマンド名、第１引き数、第２引き数、・・
・、のように分解し、それぞれ、一時記憶変数に格納し
た後、コマンド名に対応した制御変数生成部を呼び出
す。図６は、倣い動作コマンドｆｏｌｌｏｗに対する処
理を示している。

【００４８】ステップ１０１で受信したコマンドの文字
列“ｆｏｌｌｏｗｘ＋ｘｙ１０”に対して解釈を
行う。次のステップ１０２、ステップ１０３、ステップ
１０４では、文字列の先頭から順に、文字列の区切りで
ある空白を探している。次のステップ１０５では、文字
列の先頭から、ステップ１０３で見つけた空白の前の文
字までをコマンド名と判断して、一時記憶変数ｃｍｄに
格納している。この例では、コマンド名“ｆｏｌｌｏ
ｗ”が一時記憶変数ｃｍｄに格納される。

【００４９】次に、ステップ１０６では、コマンドの文
字列の最後まで、解釈を終えたかどうかを判断してい
る。文字列の最後まで解釈を終えたときは、処理を終了
する。

【００５０】次の、ステップ１０７、ステップ１０８、
ステップ１０９では、第１引き数の先頭から順に、文字
列の区切りである空白を探している。次のステップ１１
０では、第１引き数の先頭から、ステップ１０８で見つ
けた空白の前の文字までを第１引き数と判断して、一時
記憶変数ｐｒａｍｌに格納している。この例では、進行
方向を指定する第１引き数“ｘ＋”が一時記憶変数ｐｒ
ａｍｌに格納される。

【００５１】次に、ステップ１０６では、コマンドの文
字列の最後まで、解釈を終えたかどうかを判断してい
る。文字列の最後まで解釈を終えたときは、処理を終了
する。

【００５２】次の、ステップ１１１、ステップ１１２、
ステップ１１３では、第２引き数の先頭から順に、文字
列の区切りである空白を探している。次のステップ１１
５では、第２引き数の先頭から、ステップ１１３で見つ
けた空白の前の文字までを第２引き数と判断して、一時
記憶変数ｐｒａｍ２に格納している。この例では、軌道
平面を指定する第２引き数“ｘｙ”が一時記憶変数ｐｒ
ａｍ２に格納される。

【００５３】次に、ステップ１１６では、コマンドの文
字列の最後まで、解釈を終えたかどうかを判断し、文字
列の最後まで解釈を終えたときは、処理を終了する。

【００５４】次の、ステップ１１７、ステップ１１８、
ステップ１１９では、第３引き数の先頭から順に、文字
列の終わりを探している。次のステップ１２０では、第
３引き数の先頭から、ステップ１１８で見つけた文字列
の終わりまでを第３引き数と判断して、一時記憶変数ｐ
ｒａｍ３に格納している。この例では、動作時間を指定
する第３引き数“１０”が一時記憶変数ｐｒａｍ３に格
納される。

【００５５】図７は制御変数生成部２３の処理の一例を
示すフローチャートである。制御変数生成部２３は、各
コマンド名に対応した制御変数の生成を行う複数の処理
部分からなる。図７は、コマンド“ｆｏｌｌｏｗｘ＋
ｘｙ１０”の場合を示している。

【００５６】この倣い動作のコマンドの例では、コマン
ドの引き数として、進行方向“＋ｘ”、軌道平面“ｘ
ｙ”、動作時間“１０”を指定しているが、この指定し
た引き数から、倣い動作に必要な制御変数を自動的に生
成する。また、制御変数の生成時に、現在のロボット５
の状態を示す法線ベクトルを用いている。

【００５７】ステップ２０１では、一時記憶変数ｐｒａ
ｍｌを調べ、指定された進行方向に応じて、ベクトル変
数ａ_opを設定する。このコマンドの例では、“＋ｘ”が
真なのでベクトル変数ａ_op＝（１００）となる。

【００５８】次にステップ２０２では、ロボットの現在
の状態を示す法線ベクトルをｎ_Cに格納し、ステップ２
０３では、ａ_op×ｎ_Cを計算し、結果をベクトル変数ｖ
_bufに格納する。

【００５９】次に、ステップ２０４では、一時記憶変数
ｐｒａｍ２を調べ、指定された軌道平面から、選択行列
^OＳ_pを求める。このコマンドの例では、 “ｘｙ”が真なので、^OＳ_p＝（００１）と
なる。

【００６０】次に、ステップ２０５では、コマンドで指
定した進行方向が適切かどうかを判断する。指定した進
行方向が適切でない場合は、選択行列^OＳ_pの要素^OＳ
_pi＝１で、かつ、これに対応するｖ_bufの要素の絶対値
｜ｖ_i｜＝０となる。このときは、ステップ２０６でエ
ラー表示を行う。

【００６１】次に、ステップ２０７では、一時記憶変数
ｐｒａｍ２を調べ、指定された軌道平面と、ステップ２
０３で算出したベクトル変数ｖ_bufとから、軌道平面指
定ベクトル^Oｏ_opを求める。このコマンドの例では、 “ｘｙ”が真なので、^Oｏ_op＝（００ｖ₃／｜ｖ₃
｜）となる。

【００６２】次に、ステップ２０８では、コマンドで指
定された動作時間から、動作時間に関する制御変数を求
めている。一時記憶変数ｐｒａｍ３（指定された動作時
間）の値を動作時間の制御変数ｔ_indに設定し、ｖ_flw
／ａ_CLの値を一定速度移動開始時刻ｔ_stに設定し、ｔ
_ind−ｖ_flw／ａ_CLの値を一定速度移動終了時刻ｔ_enに
設定する。

【００６３】次に、ステップ２０６では、コマンドに対
応した制御モードフラグｃｍｄｆｌｇを設定する。この
コマンドの例では、倣い動作なので、倣い動作を示すフ
ラグをｃｍｄｆｌｇに設定する。

【００６４】コマンド解析装置１の、制御変数転送部２
４では、制御変数生成部２３で算出した制御変数をホス
トコンピュータ１４のモニタプログラムへ転送する。モ
ニタプログラムへは、制御変数名、制御変数値、書き込
み信号を送る。倣い動作を行うコマンド（例えば、前記
の“ｆｏｌｌｏｗｘ＋ｘｙ１０”）の場合には、
軌道平面指定ベクトル^Oｏ_op、基準座標系の選択行列^O
Ｓ_p、動作時間ｔ_ind、一定速度移動開始時刻ｔ_st 、
一定速度移動終了時刻ｔ_en、制御モードフラグｃｍｄｆ
ｌｇ、作業座標系の選択行列^LＳ_f、^LＳ_p、^LＳ_oa、
を転送する。

【００６５】上記のように、コマンド解析装置１に入力
するコマンドは、簡潔で平易な形式で記述でき、コマン
ド解析装置１が、倣い制御装置の動作に必要な制御変数
を、入力されたコマンドから生成する。このため、オペ
レータが、簡便に制御変数設定のためのコマンドを入力
することができる。

【００６６】なお、コマンドで指定する進行方向、軌道
平面指定ベクトルは、本実施例では、基準座標系のＸ
軸、Ｙ軸、Ｚ軸方向のいずれかにしているが、他の方向
を指定できる方法も可能である。〔ホストコンピュータ〕ホストコンピュータ１４のモニ
タプログラムでは、コマンド解析装置１が生成した制御
変数を、制御コントローラ１５の制御コントローラメモ
リ１６へ書き込み、設定する。また、制御コントローラ
メモリ１６からの制御変数の読み出しを行う。

【００６７】図８はホストコンピュータ１４のモニタプ
ログラムの構成を示す。モニタプログラムでは、制御変
数名と書き込み信号が指定されると、書き込みルーチン
が起動して、変数テーブルを参照して、制御コントロー
ラメモリ１６の、制御変数に対応する番地に値を書き込
む。

【００６８】また、制御変数名と読み出し信号が指定さ
れると、読み出しルーチンが起動して、変数テーブルを
参照して、制御コントローラメモリ１６の、制御変数に
対応する番地から値を読みだす。

【００６９】コマンド解析装置１で制御変数の値を決定
するときに、以前の制御変数の値が必要な場合、モニタ
プログラムは、読み出した制御変数の値を、コマンド解
析装置１へ転送する。〔制御コントローラメモリ〕制御コントローラメモリは
図９に示すように、制御コントローラメモリ１６の所定
の番地に、各制御変数の値を設定している。ホストコン
ピュータ１４のモニタプログラム、及び制御コントロー
ラ１５の制御部１７から制御変数の読み書きを行うため
に、制御変数名に対応する制御コントローラメモリ１６
の番地をアクセスする。

【００７０】ホストコンピュータ１４のモニタプログラ
ムと制御コントローラの制御部１７には、制御変数名と
この番地を結び付ける同一のテーブルが備えられてお
り、対応する制御コントローラメモリ１６の番地をアク
セスして、制御変数の読み書きを行う。

【００７１】倣い動作を行うための制御変数には、動作
を開始する度に設定される制御変数と、必要なときに適
宜設定される制御変数とがある。

【００７２】動作開始時に設定される制御変数には、下
記のものがある。

【００７３】・選択行列 ^OＳ_p ・軌道平面指定ベクトル ^Oｏ_op ・動作時間ｔ_ind ・一定速度移動開始時刻ｔ_st ・一定速度移動終了時刻ｔ_en ・制御モードフラグｃｍｄｆｌｇまた、必要なときに適宜設定される制御変数には、下記
のものがある。

【００７４】・ローパスフィルタ帯域・フィードバックゲイン・倣い速度ｖ_flw ・加速度ａ_CL 制御変数は、効率、保守性、拡張性を考慮して、制御コ
ントローラメモリ１６上に適切に配置する。〔制御部〕図１０は制御コントローラ１５の制御部１７
の構成図を示す。位置検出部４２は、制御対象であるロ
ボット５の関節部のモータの回転角を検出するエンコー
ダ及びカウンタと、回転角から基準座標系への座標を変
換する座標変換部とからなり、基準座標系でのロボット
５の先端位置Ｘを検出する。

【００７５】力検出部４３は、ロボットの先端に接続さ
れた力覚センサと、力覚センサで検出された信号から基
準座標系における接触力を算出する力信号算出部とから
なり、基準座標系での接触力を検出する。基準座標系で
の接触力の算出方法は、後述する。

【００７６】ロボット５は、操作部４７によって駆動さ
れる。操作部４７は、サーボモータ、パワーアンプ、Ｄ
／Ａコンバータ、補償器から構成され、軌道制御部４
４、力制御部４５で生成された基準座標系での速度指令
Ｖ_p、Ｖ_fに追従するように、ロボット５の関節部を駆
動する。

【００７７】また、軌道制御部４４は、位置検出部４２
で検出されたロボット５の先端位置Ｘと動作シーケンス
生成部４１から与えられた目標位置Ｘ_O、動作時間、及
び軌道制御ゲイン等の軌道制御パラメータに基づいて、
軌道制御方向の速度指令Ｖ _pを生成する。力制御部４
５は、力検出部４３で検出した力信号と、設定力（力指
令Ｆ_O）及び力制御パラメータに基づいて、力制御方向
の速度指令Ｖ_fを生成する。

【００７８】作業座標系算出部４６は、倣い動作の軌道
を決定するために、ロボット５と部材間の接触力、指定
された軌道平面から、作業座標系を算出する。作業座標
系では、軌道の法線方向を力制御方向とし、接線方向を
軌道制御方向とする。作業座標系算出部は、ロボット５
の先端と把時部材間の接触力から、軌道の法線方向と、
接線方向をリアルタイムで算出する。

【００７９】動作シーケンス生成部４１は、力指令Ｆ_O
の生成、力制御パラメータの転送、位置指令Ｘ_Oの生
成、軌道制御パラメータの転送、倣い制御動作の初期動
作において必要な軌道制御と力制御の切り換え、倣い動
作の軌道を決定するベクトルの設定、コマンド終了信号
の生成を行う。

【００８０】なお、倣い動作の軌道を決定するベクトル
の設定は、ベクトル^Oｎ_L、^Oｏ_L、^Oａ_Lから変換行
列^OＲ_Lを算出して、この変換行列^OＲ_Lを軌道制御部
４４と力制御部４５に設定することを意味する。動作シ
ーケンス生成部４１に設定される動作手順は、オペレー
タ、或いは、コマンド列ファイルによって示される。

【００８１】次に、力制御部４５について説明する。力
制御部４５では、力覚センサで検出した力を作業座標系
のＸ軸方向の目標力に一致させる速度指令を生成する。
この速度指令の生成のために、先ず、作業座標系上での
目標力と検出力との差を算出する。

【００８２】基準座標系記述の接触力ベクトル^OＦ
_qrを、作業座標系記述に変換する。作業座標系から基準
座標系への変換行列^OＲ_Lの転置行列^OＲ_L ^Tを用いる
と、作業座標系記述の接触ベクトル^LＦ_qは、下記のよ
うに表せる。

【００８３】^LＦ_q＝^OＲ_L ^T・^OＦ_qr 従って、目標力ベクトル^LＦ_oqと検出ベクトル^LＦ_qと
の偏差^LｄＦは、下記のように表せる。

【００８４】^L ｄＦ＝^LＦ_oq−（−^LＦ_q）＝^LＦ_oq＋^LＦ_q 検出ベクトル^LＦ_qが負となるのは、接触ベクトル^OＦ
_flwと法線ベクトル ^Oｎ_Lの向きが逆であることによ
る。

【００８５】次に力補償器による速度指令の生成につい
て説明する。力制御速度指令ベクトル^Lｖ_f’は、ゲイ
ンＧ_fを用いて、下記のように表せる。

【００８６】^Lｖ_f’＝Ｇ_f・^LｄＦ力制御は、作業座標系のＸ軸で行われ、下記の選択行列
^LＳ_fによって力制御方向の速度指令が抽出される。

【００８７】^LＳ_f＝ｄｉａｇ（１００）従って、最終的な速度指令ベクトル^Lｖ_fは、下記のよ
うに表せる。

【００８８】^Lｖ_f＝^LＳ_f・^Lｖ_f’ なお、力制御速度指令^Lｖ_fを基準座標系へ変換する
と、下記のようになる。

【００８９】^Oｖ_f＝^OＲ_L・^Lｖ_f ゲインＧ_f、選択行列^LＳ_fは、制御コントローラメモ
リ１６に設定されている。

【００９０】次に、軌道制御部４４について説明する。
軌道制御部４４では、作業座標系のＹ軸方向の移動距離
がゼロとなる軌道制御速度指令をＹ軸方向へ生成し、ロ
ボット先端を移動する軌道制御速度指令を作業座標系の
Ｚ軸方向に生成する。Ｙ軸方向の速度指令は、位置検出
部４２で検出された現在位置と目標位置の差から算出さ
れ、Ｚ軸方向の速度指令は、速度の関数発生器を指令源
とする開ループ系から算出される。

【００９１】軌道平面を基準座標系のＸＹ、ＹＺ、ＺＸ
平面に選んだとき、作業座標系のＹ軸は、それぞれ、基
準座標系のＺ軸、Ｘ軸、Ｙ軸に一致する。目標位置がゼ
ロのとき、基準座標系のある軸方向の位置偏差ベクトル
^OｄＰは、動作開始時の基準座標系記述の先端の位置ベ
クトル^OＰ_cr、現在の位置ベクトル^OＰ_ar、制御コント
ローラメモリに設定されている選択行列^OＳ_pを用い
て、下記のように表せる。

【００９２】^OｄＰ＝^OＳ_p・（^OＰ_cr−^OＰ_ar）作業座標系の速度指令は、ゲインＧ_pを用いて下記のよ
うに表せる。

【００９３】^Lｖ_p’＝Ｇ_p・^OＲ_L ^T・^OｄＰ作業座標系のＹ軸方向の速度指令成分を選択行列^L Ｓ_p＝ｄｉａｇ（０１０）によって抽出し、基準座標系へ変換すると、最終的な速
度指令ベクトル^Oｖ_pは、下記のように表せる。

【００９４】^Oｖ_p＝^OＲ_L・^LＳ_p・^Lｖ_p’ ゲインＧ_p、選択行列^LＳ_pは、制御コントローラメモ
リ１６に設定されている。

【００９５】Ｚ軸方向の速度指令は、制御コントローラ
メモリ１６に設定されている、動作時間ｔ_ind、最大速
度ｖ_flw、加速度ａ_CLを基に、関数発生器で算出され
る。関数発生器では、速度関数が図４に示すような台形
状の速度指令^Lｖ_oaを生成する。

【００９６】関数発生器で生成された速度^Lｖ_oaは、作
業座標系のＺ軸方向のみに発せられるため、選択行列^L
Ｓ_oa＝ｄｉａｇ（００１）によって、進行方向の成
分が抽出される。基準座標系記述の速度^Oｖ_oaは、下記
の式で表せる。

【００９７】^Oｖ_oa＝^OＲ_L・^LＳ_oa・^Lｖ_oa 上記のようにして求められた速度^Lｖ_oaに基づいて、操
作部４７が、ロボット５のＺ軸方向の速度を制御する。

【００９８】

【発明の効果】上述の如く、請求項１の発明によれば、
制御変数を設定するコマンドを、簡潔で平易な形式で記
述でき、より少ないコマンドの列で、接触倣い動作を記
述可能なため、オペレータが、簡便に制御変数設定のた
めのコマンドを入力することができる等の特長を有す
る。

【００９９】請求項２の発明によれば、制御方向に関す
る制御変数を設定するコマンドを入力する必要がないた
め、オペレータが、より簡便に制御変数設定のためのコ
マンドを入力することができる。

【０１００】請求項３の発明によれば、時間に関する制
御変数を設定するコマンドを入力する必要がないため、
オペレータが、より簡便に制御変数設定のためのコマン
ドを入力することができる。

【図面の簡単な説明】

【図１】本発明の原理構成図である。

【図２】本発明の一実施例の構成図である。

【図３】速度関数の説明図である。

【図４】コマンド解析装置の構成図である。

【図５】コマンド列の一例を示す図である。

【図６】文字列解釈部の処理の例を示すフローチャート
である。

【図７】制御変数生成部の処理の例を示すフローチャー
トである。

【図８】モニタプログラムの構成図である。

【図９】制御コントローラメモリの構成例である。

【図１０】制御部の構成図である。

【図１１】従来の倣い制御装置の一例の構成図である。

【図１２】従来の倣い制御装置の別の例の構成図であ
る。

【図１３】従来装置のコマンド列ファイルの例を示す図
である。

【符号の説明】

１コマンド解析装置２コマンド解釈手段３制御変数生成手段４制御手段５ロボット１０コマンド列ファイル装置１４ホストコンピュータ１５制御コントローラ１６制御コントローラメモリ１７制御部２１コマンド読み込み部２２文字列解釈部２３制御変数生成部２４制御変数転送部

Claims

【特許請求の範囲】

【請求項１】外部より入力されるコマンドの制御変数
に基づいて、制御手段（４）が接触倣いロボット（５）
を制御する倣い制御装置において、前記入力されるコマンドの文字列を解釈するコマンド解
釈手段（２）と、該コマンド解釈手段（２）によるコマンド解釈の結果
と、前記ロボット（５）の現在の状態を示す信号に基づ
いて前記制御変数を生成する制御変数生成手段（３）と
よりなるコマンド解析装置（１）を有する構成としたこ
とを特徴とする倣い制御装置。
【請求項２】前記制御変数生成手段（３）は、前記コ
マンド中で指定される進行方向と軌道平面とから制御方
向に関する制御変数を生成する構成としたことを特徴と
する請求項１記載の倣い制御装置。
【請求項３】前記制御変数生成手段（３）は、前記コ
マンド中で指定される動作時間から、時間に関する必要
な制御変数を生成する構成としたことを特徴とする請求
項１記載の倣い制御装置。