JPH0473164B2 - - Google Patents

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明は、多関節形のロボツトの制御装置に
関する。

〔従来の技術〕 従来、例えばロボツト各軸の関節角の現在値を
内界センサ（位置センサ）から入力して、その入
力した関節角の現在値に応じてロボツト先端の位
置及び姿勢データを算出し、その算出したデータ
のうちの位置データに移動増分を加算して、その
加算値と先の姿勢データとに基づいてロボツト各
軸の目標関節角を求めるようにしたロボツトの制
御装置が知られている。

なお、ここで云うロボツト先端の位置及び姿勢
データとは、例えば第４図に示すように位置デー
タが絶対座標系（カルテシアン座標系）における
Ｐ（ｘ、ｙ、ｚ）であり、姿勢データがロボツト
先端に固定したハンド座標系のx′、y′、z′各軸の
絶対座標系のＸ、Ｙ、Ｚ各軸に対する成分のデー
タ（オイラー角データ）である。

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

しかしながら、このような従来のロボツトの制
御装置にあつては、動作を行なう度に関節角の現
在値から位置データと共に、姿勢データをも算出
し直す構成となつていたため、テイ−チングを行
なう時などでロボツト先端姿勢を一定にして動作
を繰り返したい場合、姿勢データ算出の際の誤差
及び目標関節算出の際の誤差が、次の動作時の姿
勢データ算出に影響を及ぼして誤差が蓄積され、
ロボツト先端の姿勢が変化してしまうという問題
点があつた。

この発明は、このような問題点の解決を図ろう
とするものである。

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

そこで、この発明においては、第１図に示すよ
うに、ロボツト各軸の関節角の現在値に応じて算
出したロボツト先端の位置及び姿勢データに基づ
いて、ロボツト各軸の目標関節角を求めるように
したロボツトの制御装置Ａにおいて、ロボツトの
動作モードがロボツト先端の姿勢を変化させない
定姿勢動作モードに入つたか否かを判定する判定
手段Ｂと、この判定手段Ｂによつてロボツトの動
作モードが定姿勢動作モードに入つたと判定され
た時にロボツト各軸の関節角の現在値に応じて算
出したロボツト先端の姿勢データを記憶する記憶
手段Ｃと、この記憶手段Ｃに記憶した姿勢データ
とロボツト各軸の関節角の現在値に応じて算出し
たロボツト先端の位置データとに基づいて、定姿
勢動作モード下でのロボツト各軸の目標関節角を
算出する目標値算出手段Ｄとを設けて構成してい
る。

〔作用〕

このような構成では、ロボツト先端の姿勢が変
化しない動作が繰り返される場合、最初に算出さ
れた姿勢データが目標関節角の算出に供せられる
ようになる。

〔実施例〕

以下、この発明の実施例を図面の第２図及び第
３図を参照して説明する。

第２図は、この発明の一実施例を示すブロツク
図である。

同図において、１，２，……，３は図示しない
多関節形のロボツト各軸を夫々駆動するモータで
あり、各々モータ制御部４，５，……，６によつ
て駆動制御され、そのロボツト各軸の関節角の現
在値θ₁〜θnは、各モータ１，２，……，３の出力
軸に取り付けた図示しない各エンコーダからの信
号に基づいて現在値検出器７，８，……，９が検
出するようになつている。

１０はマイクロコンピユータ等によつてシステ
ム構成したロボツトの制御装置であり、機能ブロ
ツク化して示すと図示のように関節角現在値入力
部１１、第１の変換部１２、判定／計算部１３、
姿勢データ記憶部１４、第２の変換部１５、及び
目標関節角出力部１６等によつて構成されてい
る。

先ず、関節角現在値入力部１１は、各現在値検
出器７，８，……，９からロボツト各軸の関節角
の現在値θ₁〜θnを読み込む。

第１の変換部１２は、関節角現在値入力部１１
が読み込んだロボツト各軸の関節角の現在値θ₁〜
θnに所要の座標変換ｆ、ｇを施すことによつて、
ロボツト先端の位置データ（ｘ、ｙ、ｚ）＝ｆ
（θ₁、θ₂、……θn）及び姿勢データ（α、β、γ）
＝ｇ（θ₁、θ₂、……、θn）を算出する。

判定／計算部１３は、このロボツトの制御装置
１０に接続した図示しないテイ−チングボツクス
や制御盤で設定したロボツトの動作モードがロボ
ツト先端の姿勢を変化させない定姿勢動作モード
（例えばCP動作モード）に入つたか否かを判定し
て、動作モードが定姿勢動作モードでない時に
は、第１の変換部１２で算出している位置データ
（ｘ、ｙ、ｚ）及び姿勢データ（α、β、γ）に
おける位置データ（ｘ、ｙ、ｚ）にテイ−チング
ボツクスなどからの動作指示に基づく移動指令
（Δx、Δy、Δz）を加算して、その加算値を目標
位置データ（cx、cy、cz）＝（ｘ、ｙ、ｚ）＋
（Δx、Δy、Δz）として姿勢データ（α、β、γ）
と共に第２の変換部１５に出力する。

また、動作モードが定姿勢動作モードに入つた
時には、先ずその時第１の変換部１２が算出して
いる姿勢データ（α、β、γ）を固定の姿勢デー
タ（α₀、β₀、γ₀）として姿勢データ記憶部１４に
記憶（ラツチ）すると共に、目標位置データ
（cx、cy、cz）の方は上記と同様に計算して、そ
の計算結果（cx、cy、cz）を姿勢データ記憶部
１４に記憶した固定の姿勢データ（α₀、β₀、γ₀）
と共に第２の変換部１５に出力する。

なお、定姿勢動作モードに入つたか否かの判定
に供するデータとしては、テイ−チングボツクス
などからの例えばCP動作モードの選択指令の他
に、専用のマニユアルスイツチを設けて、そのマ
ニユアルスイツチの操作入力を用いるようにする
こともできる。

第２の変換部１５は、判定／計算部１３からの
データ（cx、cy、cz、α、β、γ）又は（cx、
cy、cz、α₀、β₀、γ₀）に所要の座標変換Ｆを施す
ことによつて、ロボツト各軸の目標関節角（cθ₁、
cθ₂、……、cθn）＝Ｆ（cx、cy、cz、α、β、
γ）、又は＝Ｆ（cx、cy、cz、α₀、β₀、γ₀）を算
出する。

そして、目標関節角出力部１６は、第２の変換
部１５で算出したロボツト各軸の目標関節角cθ₁、
cθ₂、……、cθnを各々モータ制御部４，５，…
…，６に出力する。

次に、この実施例の作用を第３図のフロー図を
参照しながら説明する。

なお、以下に述べる説明はロボツトをテイ−チ
ングする際の動作である。

先ず、第２図のロボツトの制御装置１０を構成
する図示しないマイクロコンピユータのCPUは、
やはり図示しないテイ−チングボツクスから起動
がかかるのを待ち、起動がかかると各現在値検出
器７，８，……，９からロボツト各軸の関節角の
現在値θ₁〜θnを読み込む（関節角現在入力部１１
に対応する処理）。

そして、その読み込んだロボツト各軸の関節角
の現在値θ₁〜θnに座標変換ｆ、ｇを施すことによ
つてロボツト先端の位置データ（ｘ、ｙ、ｚ）＝
ｆ（θ₁、θ₂、……、θn）及び姿勢データ（α、β、
γ）＝ｇ（θ₁、θ₂、……、θn）を算出する（第１の
変換部１２に対応する処理）。

次に、ロボツトの動作モードが定姿勢動作モー
ドか否かをテイ−チングボツクスなどからの選択
指令によつてセツトされるモードフラツグをチエ
ツクすることによつて判定（この判定処理は図示
していない）した後、動作モードがCP動作モー
ドなどの定姿勢動作モードであれば、ロボツトの
動作がその定姿勢動作モードに入つて最初の動作
か否かをチエツクし、最初（一回目）の動作であ
れば上記の処理で算出した姿勢データ（α、β、
γ）を（α₀、β₀、γ₀）としてメモリ（姿勢データ
記憶部１４）格納し、二回目以降であれば一回目
で記憶格納した固定の姿勢データ（α₀、β₀、γ₀）
を読み出す。

このようにして、動作モードが定姿勢動作モー
ドに入つた時点と、入つた後との処理の振り分け
を行なつている。なお、図示を省略しているが、
動作モードが定姿勢動作モードでない時には直接
後述する増分加算処理に進む。

そして、定姿勢動作モードに入つた一回目の動
作であれば、記憶格納したばかりの姿勢データ
（α₀、β₀、γ₀）を、二回目以降であれば一回目で
記憶格納した姿勢データ（α₀、β₀、γ₀）を、定姿
勢動作モードでなければ上記処理で算出した姿勢
データ（α、β、γ）を夫々変換用データとする
と共に、上記処理によつて算出した位置データ
（ｘ、ｙ、ｚ）にテイ−チングボツクスなどから
の動作指示に基づく移動増分（Δx、Δy、Δz）を
増加して、その加算結果を変換用の目標位置デー
タ（cx、cy、cz）とする。

なお、上記定姿勢動作モードの判定処理から増
分加算処理までが判定／計算部１３に対応する処
理である。

次に、上記処理によつて得た目標位置データ
（cx、cy、cz）と姿勢データ（α、β、γ）又は
（α₀、β₀、γ₀）とに座標変換Ｆを施すことによつ
てロボツト各軸の目標関節角（cθ₁、cθ₂、……、
cθn）を算出する（第２の変換部１５に対応する
処理）。

そして、そのようにして求めた目標関節角
（cθ₁、cθ₂、……、cθn）を第２図の各モータ制御
部４，５，……，６に出力（目標関節角出力部１
６に対応する処理）した後、テイ−チングボツク
スからの起動が解除されたか否かをチエツクし、
解除されていなければ増分加算処理に戻つて該処
理から目標関節角の出力処理までの処理を繰り返
し、解除されていれば一旦本処理群の処理を終了
する。

したがつて、動作モードが定姿勢動作モードと
なつていれば、テイ−チングボツクスから起動が
かかつている間は最初に求めた位置データ（ｘ、
ｙ、ｚ）に（Δx、Δy、Δz）が繰り返し加算され
て目標位置データ（cx、cy、cz）が算出される
と共に、姿勢データは最初にメモリに記憶格納さ
れた（α₀、β₀、γ₀）が利用されるようになるばか
りか、一度起動解除がなされても定姿勢動作モー
ドが解除されなければ再度起動がかかることによ
つてその再起動がかかつている間も繰り返し
（Δx、Δy、Δz）が加算されて得られる目標位置
データ（cx、cy、cz）と姿勢データ（α₀、β₀、
γ₀）が利用されるため、それ等のデータに基づい
て作動する各モータ制御部４，５，……，６の作
用により、ロボツト各軸は先端姿勢が一定のまま
所要方向に移動するようになる。

そして、動作モードが定姿勢動作モードでなけ
れば、起動がかかつて動作が行なわれる度に算出
される姿勢データ（α、β、γ）が利用される。

このように、本実施例によれば定姿勢動作モー
ド下では、該モードに入つた時に最初に求めた姿
勢データ（α₀、β₀、γ₀）が繰り返し利用されるた
め、動作を繰り返す毎に演算誤差が蓄積されてロ
ボツト先端の姿勢が変化してしまうことがなくな
る。

〔発明の効果〕

以上説明してきたように、この発明によれば、
テイ−チングを行なう時などでロボツト先端姿勢
を一定にして動作を繰り返したい場合、最初に求
めた姿勢データを繰り返し使用するようにしたた
め、姿勢データや目標関節角の算出誤差が蓄積さ
れることがなくなり、それによつてテイ−チング
時等の繰り返し動作時にロボツト先端の姿勢が変
化しなくなる。

【図面の簡単な説明】

第１図はこの発明の構成を示すブロツク図、第
２図はこの発明の一実施例を示すブロツク図、第
３図は第２図の作用説明に供するフロー図、第４
図はロボツト先端の位置及び姿勢データの説明に
供する図である。 １，２，……，３……モータ、４，５，……，
６……モータ制御部、７，８，……，９……現在
値検出器、１０……ロボツトの制御装置、１１…
…関節角現在値入力部、１２……第１の変換部、
１３……判定／計算部、１４……姿勢データ記憶
部、１５……第２の変換部、１６……目標関節角
出力部。

Claims (1)

1. 【特許請求の範囲】 １ ロボツト各軸の関節角の現在値に応じて算出
   したロボツト先端の位置及び姿勢データに基づい
   て、前記ロボツト各軸の目標関節角を求めるよう
   にしたロボツトの制御装置において、 ロボツトの動作モードがロボツト先端の姿勢を
   変化させない定姿勢動作モードに入つたか否かを
   判定する判定手段と、 この判定手段によつてロボツトの動作モードが
   前記定姿勢動作モードに入つたと判定された時に
   前記ロボツト各軸の関節角の現在値に応じて算出
   したロボツト先端の姿勢データを記憶する記憶手
   段と、 この記憶手段に記憶した姿勢データと前記ロボ
   ツト各軸の関節角の現在値に応じて算出したロボ
   ツト先端の位置データとに基づいて、前記定姿勢
   動作モード下での前記ロボツト各軸の目標関節角
   を算出する目標値算出手段とを設けたことを特徴
   とするロボツトの制御装置。