JPH0756606B2 - 移動体の経路教示方法 - Google Patents

Description

【発明の詳細な説明】 産業上の利用分野 本発明は移動体を動かし、この移動体が移動した経路上
の点を順次記憶し、次にはこの記憶を順次読み出して、
この記憶に基づいて移動体を動かすようにした移動体の
経路教示方法に関するものである。

本発明は移動体として工業用ロボットやある種の工作機
械などが考えられるが、以下の説明では移動体は工業用
ロボットとして説明する。

従来の技術 従来用いられていた工業用ロボットの教示方法は、工業
用ロボットの経路制御対象部が一定距離以上動く毎に、
その点を記憶するか、あるいは一定時間毎に経路制御対
象部の位置を記憶し、再生時はそれらの記憶された点の
間を直線で再生する。

このため、教示点間の間隔は相当細かくしないと再生精
度が劣化する。したがって、再生精度の劣化を防ぐため
には多数の経路位置データを必要とし、大容量の記憶装
置が必要となる。

また、教示経路が急激に屈曲する場合、教示後のプレイ
バック時にこの屈曲部付近でロボットアームに振動が生
じる。

一方、記憶容量の問題を解消する試みとして、特開昭58
−176705号公報のものがある。この開示技術では、仮記
憶手段に記憶した現在の最も新しい点P_n-1から工業用ロ
ボット現在位置Ｐまでの距離ｌを演算し、該距離ｌが予
め定めたピッチｌに達したとき、その点P_nを前記仮記憶
手段に記憶するとともに、前記点P_n-1の１つ前に記憶し
た点P_n-2と点P_n-1を結んだ線と点P_n-1と前記点P_nとを結
んだ線とのなす角a_nを前記点P_nと対をなすデータとして
前記仮記憶手段に記憶し、順次仮記憶された前記角a_nの
うち最も古い角a₁と前記角a_nとの差が予め定めた許容差
Δａよりも大きくなったときに、最も古い点P₁と最新の
点P_nとこの２点の中間の他の１点P_mとの合計３点の位置
データを本記憶手段に転送し、再生時に円弧補間する位
置データとして記憶する一方、前記仮記憶手段には前記
最新の点P_nおよび角a_nのデータを初期値P₁,a₁として残
し、他のデータは全て消去するようにして教示を進めて
ゆくもので、経路をある範囲内の曲率をもった部分に分
割し、この分割したそれぞれの経路を円弧補間すること
によって、曲線部の多い複雑な形状でも少ない記憶容量
を実現しようとするものである。

しかしながら、この開示技術では、仮記憶手段に記憶し
た最も古い点P₁から現在の最も新しい点P_n-1さらには前
記P_n点までのｎ点とそれらと対をなす角a₁からa_nを仮記
憶手段に記憶しておく必要があり、経路中に相当距離の
円弧あるいは曲率の変化の少ない経路部分が存在した場
合には、この仮記憶手段自体が大きな記憶容量を持つ必
要が生じ、総記憶容量を著しく減少させるには到らない
と考えられる。また、この開示技術では経路をいくつか
の円弧で近似していくことになるが、隣り合った円弧同
士が滑らかにつながる可能性はないと言ってよく、この
経路の方向の不連続点でロボットの動特性上、経路制御
対象部に振動等が生じる可能性が出てくる。

発明が解決しようとする問題点 上記のごとく従来の技術では、実質的に記憶容量の節約
を飛躍的に向上させるには到らない。また、経路をいく
つかの部分に一定の方法で分割し、それらの部分を円弧
補間するが、隣り合った補間円弧同士は接続点で方向が
不連続であり、ロボットの動特性上、振動等の発生する
要因となる。

本発明は上記問題点に鑑み、教示経路データの記憶容量
の減少を図り、再生経路の方向が滑らかである移動体の
経路教示方法を提供するものである。

問題点を解決するための手段 上記問題点を解決するために本発明の移動体の経路教示
方法は、移動体を動かし、該移動体が移動した経路上の
点を順次記憶する方法において、仮記憶手段に順次記憶
した教示経路上の３点P₁,P_n-1,P_nについて、∠P₁P_n-1P_n
初期位置P₁点から現在位置P_n点までの経路の長さＳとP₁
点からP_n点までの直線距離ｌを演算し、 ａ ∠P₁P_n-1P_nが予め定めた判定角αより大きい場合に
は、P₁点からP_n点までの前記経路の長さＳとこの２点間
の前記直線距離ｌの差を求め、 ａ−１ 経路の長さＳと直線距離ｌの差が予め定めた判
定長さｄより大きい場合には、P₁点を本記憶手段に記憶
し、仮記憶手段には、P_n-1点のデータを新たな初期位置
P₁点のデータとして残し、また現在位置P_n点のデータを
新たなP_n-1点のデータとして残し、 ａ−２ 前記経路の長さＳと前記直線距離ｌの差が予め
定めた判定長さｄより小さい場合には、仮記憶手段にお
いて現在位置P_n点のデータを新たなP_n-1点のデータとし
てP₁点のデータとともに残す一方、P₂,P₃,………,P_n-1
各点のデータを残さず、 ｂ ∠P₁P_n-1P_nが予め定めた判定角αより小さい場合に
は、P₁点のデータを本記憶手段に記憶し、仮記憶手段に
おいてP₂,P₃,………,P_n-2点のデータは残さず、屈曲部
となるP_n-1点のデータを新たな初期位置P₁点のデータと
して残し、また現在位置P_n点のデータを新たなP_n-1点の
データとして残し、前記新たな初期位置P₁点のデータと
して残されたP_n-1点のデータは、本記憶手段に記憶せ
ず、 次に新しく経路データP_n点をサンプリングし、新たに設
定された３点P₁,P_n-1,P_nについて上記の判定を繰り返し
て経路データを本記憶手段に記憶していき、記憶した経
路データを高次の補間法で補間する位置データとするも
のである。

作 用 本発明は上記した方法により、教示経路に屈曲部がなく
∠P₁P_n-1P_n＞αとなる場合に、P₁点からスタートして
P₂,P₃………,P_n点と順次サンプリングを進める過程で、
P₁点とP_n点間の経路に沿った弧の長さＳと、P₁点,P_n点
を直線で結ぶ弦の長さｌとの差が判定長さｄより大き
く、すなわちＳ−ｌ＞ｄとなって経路の曲率が比較的大
きくなるとP₁点からP_n-1点の直前にサンプリングしたP
_n-2点までの（ｎ−２）個の点の本記憶手段29への記憶
がP₁点の１点のみとなる。

すなわち、Ｓ−ｌ＜ｄとなっている間は経路の曲率も比
較的小さく、直線に近いため、サンプリング段階におけ
る各角∠P₁P₂P₃,∠P₁P₃P₄,∠P₁P₄P₅,………，∠P₁P_n-2P
_n-1の頂部に位置するP₂,P₃,P₄,………,P_n-2の各点のデ
ータもＳ−ｌとｄとの大小を比較完了するまでは順次仮
記憶手段30に記憶する必要があるが、Ｓ−ｌとｄの両者
を比較した結果、Ｓ−ｌ＜ｄの状態が続く間は、この状
態がサンプリングの結果として確認されるたびごとに前
記P₂,P₃,P₄,………,P_n-2各点のデータは仮記憶手段30に
残す必要もなくなり、仮記憶手段30から順次消されてい
くことになる。

上記した状態から、経路の曲率が比較的大きな状態、す
なわちＳ−ｌ＞ｄの状態にひとたび転ずると、P₁点だけ
は再生すべき点として本記憶手段29へ記憶され、現在位
置P_n点のデータを新たなP_n-1点のデータとして、またP
_n-1点のデータを新たなP₁点のデータとして仮記憶手段3
0に残せばよい。

また、教示経路に屈曲部があり∠P₁P_n-1P_n≦αとなる場
合はP₂点からP_n-2点までの（ｎ−３）個の点の記憶を全
く行わないので、経路教示で最大の欠点である記憶容量
の問題が解消できる。

さらに、教示経路が急激に屈曲する場合には、∠P₁P_n-1
P_nの判定によりその屈曲部付近の経路データを本記憶手
段に記憶せず、記憶点を高次の関数曲線により、経路が
滑らかにつながるよう補間する高次の補間法を用いるた
め、ロボットのプレイバック時に屈曲点においても振動
が発生せず、滑らかな動作が行える。

実施例 以下本発明の一実施例を添付図面に基づいて説明する。

第２図において、Ｒは工業用ロボットの本体であり、１
はベース、２は回転テーブルである。回転テーブル２の
囲りに歯３が設けられる。一方、ベース１に電動機４を
固定し、これにピニオンギヤ５を直結する。そして、ピ
ニオンギヤ５と歯３とを噛合させ、回転テーブル２が垂
直旋回軸を中心として矢印で示すθ方向へ旋回可能とす
る。回転テーブル２に第１アーム６をピン（図示せず）
で枢着する。油圧アクチュエータ７のヘッド８をピン９
で回転テーブル２に枢着し、ロッド10をピン11で第１ア
ーム６に枢着する。そして、油圧アクチュエータ７で第
１アーム６を矢印で示すφ方向へ旋回できるようにす
る。第１アーム６に第２アーム12をピン（図示せず）で
枢着する。油圧アクチュエータ13のヘッド14をピン15に
より第１アーム６に枢着し、ロッド16をピン17により第
２アーム12に枢着する。そして、油圧アクチュエータ13
で第２アーム12を矢印ψ方向へ旋回可能とする。第２ア
ーム12の先端に水平旋回軸を有し矢印γ方向に旋回可能
な第１揺動モータ18を設ける。第１揺動モータ18に第２
揺動モータ19を取りつける。この第２揺動モータ19は第
１揺動モータ18の揺動軸に対して直角をなす揺動軸を有
し、矢印β方向に揺動できるようにする。第２揺動モー
タ19に一対のフィンガ20を設け、このフィンガ20でシー
ル剤の塗布ガン21を保持する。

第３図にも示す、位置検出手段22,23,24,25および26を
設け、ベース１に対する回転テーブル２の姿勢θ、回転
テーブル２に対する第１アーム６の姿勢φ、第１アーム
６に対する第２アーム12の姿勢ψ、第２アーム12に対す
る第１揺動モータ18の姿勢γ、第１揺動モータ18に対す
る第２揺動モータ19の姿勢βをそれぞれ検出するように
する。

第３図において、27は判定角度値設定手段、28は判定長
さ値設定手段、29は本記憶手段、30は仮記憶手段、31は
マイクロコンピュータ等の演算処理手段である。本記憶
手段29は、再生時に本実施例ではアキマの方法で補間す
るための位置データとして、後述する経路上の特定の点
を記憶する。

ここで、アキマの方法とは高次の補間法の一つで、教示
点間を３次曲線ｙ＝ｆ（ｘ）＝ax³＋bx²＋cx＋ｄでつな
ぎ、つなぎ目における接線の傾きが等しくなるようにし
て滑らかにつなげる補間方法のことである（槙書店発行
の長嶋秀世著「数値計算法」参照）。

演算処理手段31はその処理内容を第１図に示すが、位置
検出手段22〜26の出力を受けて塗布ガン21の現在位置P_n
を演算する。また、仮記憶手段30から該記憶手段が記憶
している初期位置P₁の位置データを読み出し、第５図に
示すようにこの点P₁と現在位置P_n点との間の直線距離ｌ
を演算する。ここで現在位置P_n点は点P₁から数えて第ｎ
番目のサンプリング点であり、経路データのサンプリン
グは塗布ガン21が一定距離以上動く毎に行う。現在位置
P_n点の１つ前にサンプリングしたP_n-1点の位置データを
仮記憶手段30から読み出し、この点P_n-1点と現在位置P_n
点との間の経路の長さＳ′を演算し、仮記憶手段30が記
憶しているP₁点とP_n-1点の間の経路の長さＳを読み出
し、この経路の長さＳにP_n-1点とP_n点との経路の長さ
Ｓ′を加えたものをP₁点とP_n点との間の経路の長さとし
て仮記憶手段30に書き込んで経路長さＳを更新する。ま
た、仮記憶手段30に記憶されているP₁,P_n-1,P_n点の位置
データから∠P₁P_n-1P_nを演算し、∠P₁P_n-1P_nが判定角度
値設定手段27から得た判定角度αの値より大きい場合に
は、次の（Ｉ）の処理を行う。

（Ｉ） 先に求めたP₁点からPn点までの経路の長さＳと
直線距離ｌの差を求め、判定長さ値設定手段28から得ら
れる判定長さｄの値と比較する。経路の長さＳと直線距
離ｌの差が判定長さｄより大きく、すなわちＳ−ｌ＞ｄ
となり、経路の曲率が比較的大きくなって、この点P_n点
をそれまでのサンプリング点のように記憶しないという
扱いはできなくなった場合には、仮記憶手段30中に記憶
されており、サンプリング点が屈曲点であるかどうかを
表示する判定フラグを読み出し、判定フラグがP₁点が屈
曲点でないことを示す０であればP₁点の経路データを本
記憶手段29に書き込み、判定フラグが１でP₁点が屈曲点
であることを表示すれば、P₁点の経路データを記憶しな
い。そして、仮記憶手段30の記憶している経路の長さＳ
をP_n-1点とP_n点の距離Ｓ′に書き換え、P_n-1点のデータ
を初期位置P₁点のデータとして残し、現在位置P_n点のデ
ータをP_n-1点のデータとして残して、仮記憶手段30の判
定フラグを新たなP₁点が屈曲点でないことを示す０の状
態にリセットする。

次に第６図に示すように、経路の曲率が比較的小さく、
経路の長さＳと直線距離ｌの差が判定長さｄより小さい
Ｓ−ｌ＜ｄの関係となっている間は、∠P₁P₂P₃,∠P₁P₃P
₄,∠P₁P₄P₅,………，∠P₁P_n-2P_n-1の頂部に位置するP₂,
P₃,………,P_n-2の（ｎ−３）個の各点のデータを仮記憶
手段30から順次消していきながら、現在位置P_n点のデー
タをP_n-1点のデータとして残し、さらにサンプリングを
続けＳ−ｌ＞ｄとなって、経路の曲率が比較的大きくな
った時点で前述のようにP₁点のデータを本記憶手段29に
書き込む。

第７図に示すように、∠P₁P_n-1P_nが判定角度αの値より
小さい場合には、次の（II）の処理を行う。

（II） 仮記憶手段30に記憶している経路の長さＳをP
_n-1点とP_n点との距離Ｓ′に書き換え、P_n-1点のデータ
を初期位置P₁点のデータとして残し、現在位置P_n点のデ
ータをP_n-1点のデータとして残し、もとのP_n-1点におけ
る頂角がαより小さく屈曲点となっていることを仮記憶
手段30の判定フラグを１にして記憶する。

ここでは、屈曲点となるP₂点からP_n-2点までの（ｎ−
３）個の点の記憶は行わない。

さらに、演算処理手段31は位置検出手段22〜26の出力を
受けて新たに塗布ガン21の現在位置P_nを演算する。以下
同様に本記憶手段29への経路データの格納を繰り返して
いく。

さて、位置検出手段22〜26の出力θ，φ，ψ，γ，βの
値から、塗布ガン21の先端の直交座標上での値X_n,Y_n,Z_n
（なお、P_n（X_n,Y_n,Z_n）である）は、 で求めることができる。逆に、直交座標上での値X_n,Y_n,
Z_nからθ，φ，ψ，γ，βの値は、 で求めることができる。なお、Ａは工業用ロボットの構
成で決まる行列であり、A^-1Aの逆行列である。また、点
P₁と点P_n-1を結ぶ直線と点P_n-1点P_nを結ぶ直線のなす角
θは、３点P₁,P_n-1,P_nの各X,Y,Zの値から求まる。

なお、点P₁の記憶には（X,Y,Z）、または（θ，φ，
ψ，γ，β）の値で記憶する。本記憶手段に記憶した経
路データは再生時アキマの方法で補間されるが、アキマ
の方法では３次曲線における未知数がa,b,c,dと４つあ
るため、補間節点が少なくとも４点必要であり、教示さ
れた点が始点と終点の２点のときは直線補間、教示され
た点が３点のときは円弧補間を用いる。

次に、この方法で行った教示の具体例を示す。

第４図（ａ），（ｂ）は実線で示した直角部をもつ教示
経路において本発明の経路教示方法を適用したものであ
る。

第４図（ａ）は教示経路のコーナー部近傍における各点
のサンプリングの様子を示す図であり、教示経路を実線
で示す。

第４図（ａ）において、P₂,………,P₆の各点のデータ
は、これらをサンプリングし、判定が行われるまでは仮
記憶手段30に一時的に記憶されるが、前記各点間では曲
率も小さく、経路の大きな変化もないため、判定が完了
するたびごとに前記各点のデータは仮記憶手段30から消
去されていく。

そして、サンプリング点P₈において、∠P₁P₇P₈＞α、か
つＳ−ｌ＝ と判定され、このP₁点が採用点として本記憶手段29に記
憶されることとなる。

次に、第４図（ｂ）に教示が進んだ場合を示す。このと
き、第４図（ａ）におけるP₇点を新たなP₁点、またP₈点
をP_n-1点として教示は進められる。

第４図（ｂ）において、P₂,………,P₈の各点は、サンプ
リングされるごとに判定が行われるまでは仮記憶手段30
に一時的に記憶されるが、前記各点間では曲率も小さ
く、経路の大きな変化もないため、判定が完了するたび
ごとに前記各点のデータは仮記憶手段30から消去されて
いく。

そして、サンプリング点P₁₀において、∠P₁P₉P₁₀＜αと
判定される。このとき、第４図（ｂ）におけるP₁点のデ
ータを本記憶手段29に記憶し、P_n-1点、すなわちこの場
合はP₉点のデータを初期位置P₁点のデータとして残し、
現在位置P_n点、すなわちP₁₀点のデータをP_n-1点のデー
タとして残す。また、この新しいP₁点となったもとのP₉
点は屈曲点であると判定されているため、以後経路教示
が進んでもこの新しいP₁点のデータは本記憶手段29に記
憶されない。

このようにして、経路教示を進めていき、最終的に本記
憶手段29に残った点を第４図（ａ），（ｂ）において採
用点として示す。そして、これらの点をアキマの方法で
補間した結果は図中の破線で示すようになり、滑らかに
再生することが可能となる。また、サンプリングした多
くの点を採用せず、仮記憶手段から消去することにより
記憶容量が少なくてすむ。

また、判定角αを用いた判定によって急激に曲がった直
角部付近ではサンプリングした多くの点も採用されな
い。したがって、サンプリング点の中から本記憶手段に
残された採用点をアキマの方法で補間すれば、破線で示
すように滑らかに再生することが可能となり、サンプリ
ングした多くの点を採用せず、仮記憶手段から消去する
ことにより記憶容量が少なくてすむ。

発明の効果 本発明は以上により、P₁点からP_n-2までの（ｎ−２）個
の点の記憶がP₁点の１点となるか、あるいはP₁点からP
_n-1点までの（ｎ−１）個の点の記憶を全く行わないの
で、経路教示で最大の欠点である記憶容量の問題が解消
できるとともに、教示経路が急激に屈曲する場合、その
屈曲部付近の経路データを本記憶手段に記憶しないの
で、再生時に高次の補間法で補間された再生経路は滑ら
かになる。

【図面の簡単な説明】

第１図は本発明の一実施例における経路教示方法を示す
フローチャート、第２図は本発明の一実施例を適用する
工業用ロボットの斜視図、第３図は本発明の一実施例に
おける教示装置を示すブロック図、第４図（ａ）は本発
明の教示方法により再生経路を得る際の教示経路上の各
点のサンプリングについて説明する図、第４図（ｂ）は
同図（ａ）のコーナー部近傍における各点のサンプリン
グについて説明し、本発明の教示方法により得られた再
生経路を教示経路と比較して示す線図、第５図は教示経
路に屈曲部がなく曲率が比較的大きい場合を示す図、第
６図は教示経路に屈曲部がなく曲率が比較的小さい場合
を示す図、第７図は教示経路に屈曲部がある場合を示す
図である。 22,23,24,25,26……位置検出手段、27……判定角度値設
定手段、28……判定長さ値設定手段、29……本記憶手
段、30……仮記憶手段、31……演算処理手段。

フロントページの続き (72)発明者 久保田 均 大阪府門真市大字門真1006番地 松下電器 産業株式会社内 (56)参考文献 特開 昭58−176705（ＪＰ，Ａ) 特開 昭56−114685（ＪＰ，Ａ) 特開 昭51−151646（ＪＰ，Ａ)

Claims (1)

【特許請求の範囲】
1. 【請求項１】移動体を動かし、該移動体が移動した経路
   上の点を順次記憶する方法において、仮記憶手段に順次
   記憶した教示経路上の３点P₁,P_n-1,P_nについて、∠P₁P
   _n-1P_nと初期位置P₁点から現在位置P_n点までの経路の長
   さＳとP₁点からP_n点までの直線距離ｌを演算し、 ａ ∠P₁P_n-1P_nが予め定めた判定角αより大きい場合に
   は、P₁点からP_n点までの前記経路の長さＳとこの２点間
   の前記直線距離ｌの差を求め、 ａ−１ 経路の長さＳと直線距離ｌの差が予め定めた判
   定長さｄより大きい場合には、P₁点を本記憶手段に記憶
   し、仮記憶手段には、P_n-1点のデータを新たな初期位置
   P₁点のデータとして残し、また現在位置P_n点のデータを
   新たなP_n-1点のデータとして残し、 ａ−２ 前記経路の長さＳと前記直線距離ｌの差が予め
   定めた判定長さｄより小さい場合には、仮記憶手段にお
   いて現在位置P_n点のデータを新たなP_n-1点のデータとし
   てP₁点のデータとともに残す一方、P₂,P₃,………,P_n-1
   各点のデータを残さず、 ｂ ∠P₁P_n-1P_nが予め定めた判定角αより小さい場合に
   は、P₁点のデータを本記憶手段に記憶し、仮記憶手段に
   おいてP₂,P₃,………,P_n-2点のデータは残さず、屈曲部
   となるP_n-1点のデータを新たな初期位置P₁点のデータと
   して残し、また現在位置P_n点のデータを新たなP_n-1点の
   データとして残し、前記新たな初期位置P₁点のデータと
   して残されたP_n-1点のデータは、本記憶手段に記憶せ
   ず、 次に新しく経路データP_n点をサンプリングし、新たに設
   定された３点P₁,P_n-1,P_nについて上記の判定を繰り返し
   て経路データを本記憶手段に記憶していき、記憶した経
   路データを高次の補間法で補間する位置データとするこ
   とを特徴とする移動体の経路教示方法。