JP3246367B2 - ロボットのパレタイジング姿勢算出方法 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、パレタイジング位
置での姿勢を計算により求めるロボットのパレタイジン
グ姿勢算出方法に係り、特にロボットアームの歪みなど
の影響を極力排除して、実際に必要な姿勢を取ることが
できるようにしたものに関する。

【０００２】

【従来の技術】ロボットは、産業用として組み立て、検
査、搬送などの種々の作業分野に使用される。それらの
作業分野は何らかの形でパレタイジング作業を含んでお
り、そのパレタイジング作業をロボットに実行させるた
めには、多数の位置情報の入力が必要となる。それらの
位置情報を全てのロボットに教示しようとすると、位置
情報の入力だけで相当の時間を費やすこととなり、非能
率である。

【０００３】そこで、従来では、多数のパレタイジング
位置から任意に選択した少数の基準点だけをロボットに
教示し、他のパレタイジング位置はその基準点の位置情
報を基にして数値計算で算出する方法が採用されてい
た。

【０００４】例えば、縦横に多数並ぶパレタイジング位
置を計算により求める場合、任意の一つの隅角部に存す
るパレタイジング位置を第１の基準点、この第１の基準
点から縦および横方向に隔たった他の隅角部に存するパ
レタイジング位置を第２および第３の基準点としてその
位置をロボットに教示する。そして、ロボットの制御装
置は、教示された位置情報と基準点間のパレタイジング
位置の数とからパレタイジング位置の縦方向ピッチと横
方向ピッチとを算出し、第１の基準点の位置と縦方向ピ
ッチおよび横方向ピッチとから各パレタイジング位置を
計算して求める、というものである。

【０００５】

【発明が解決しようとする課題】しかしながら、ロボッ
トでは関節が多く、その機械的剛性は比較的低い。この
ため、ロボットのアームは自重、或いはワークの重量で
歪みを生じ易く、ロボットのベースに近いパレタイジン
グ位置とベースから遠く隔たったパレタイジング位置と
では、ロボットアームの歪み量が異なる。このため、上
述のようにしてパレタイジング位置を計算で求める場
合、図９（ａ）に示すように、求められたパレタイジン
グ位置（破線）と実際のロボットの移動位置（実線）と
の間にはロボットアームの歪み量に対応した誤差が生ず
ることとなり、パレットとの間でのワークの授受ができ
なくなったりする。この問題については、後述の「発明
の実施の形態」の項において説明する計算方法によって
解消することができる。

【０００６】一方、ロボットの姿勢、すなわちロボット
アームの先端部分である手首の姿勢は、後述する図７に
符号９で示すフランジの端面の中心から垂直方向のアプ
ローチベクトルとこのアプローチベクトルと直交する方
向のオリエントベクトルとを設定し、ロボット座標をそ
れら両ベクトルの交点に平行移動させたとき、アプロー
チベクトルとオリエントベクトルがそれぞれロボット座
標上でどの方向を向いているかによって示される。

【０００７】上述のようにしてパレタイジング位置を数
値計算によって求める場合、ロボットの姿勢に関して
は、従来、制御装置は、第１の基準位置における手首の
姿勢を記憶し、その記憶した第１の基準位置の姿勢を、
他のどのパレタイジング位置においても取るように制御
する。

【０００８】しかしながら、上述のようにロボットアー
ムは歪み易く、アームが歪むと、制御装置はロボット座
標を平行移動させたと認識しても、実際にはロボット座
標はロボットアームの歪み量に相応するだけ傾いている
こととなる。このため、図９（ｂ）に示すように、二点
鎖線のパレタイジング位置と実線のパレタイジング位置
とでは、ロボット座標の傾きが異なることとなり、その
結果、手首の姿勢も異なることとなって、例えば軸状の
ワークをパレットの穴に挿入することが必要なパレタイ
ジング作業を行なう場合、これができなくなったりする
という問題を生ずる。

【０００９】本発明は上記の事情に鑑みてなされたもの
で、その目的は、パレタイジング位置での姿勢を計算に
よって求める場合、実際に必要な姿勢との誤差を極力少
なくすることができるロボットのパレタイジング姿勢算
出方法を提供するにある。

【００１０】

【００１１】

【００１２】

【課題を解決するための手段】本発明は、一方向にＭ
列、他方向にＮ行並ぶパレタイジング位置でのロボット
の姿勢を計算によって求めるロボットのパレタイジング
姿勢算出方法において、前記（Ｍ×Ｎ）個のパレタイジ
ング位置のうち、４隅に存するパレタイジング位置を教
示し、いずれかの教示位置を第１の基準点、この第１の
基準点から前記一方向に離れた教示位置を第２の基準
点、前記第１の基準点から他方向に離れた教示位置を第
３の基準点、前記第１の基準点と対角関係にある教示位
置を第４の基準点として以下により各パレタイジング位
置での姿勢を算出することを特徴とする。 （１）前記第２の基準点のアプローチベクトルから前記
第１の基準点のアプローチベクトルを差し引いて得たベ
クトルを（Ｍ−１）等分して一方向単位アプローチベク
トルＡａを求めると共に、前記第３の基準点のアプロー
チベクトルから前記第１の基準点のアプローチベクトル
を差し引いて得たベクトルを（Ｎ−１）等分して他方向
単位アプローチベクトルＡｂを求める。 （２）前記第２の基準点のオリエントベクトルから前記
第１の基準点のオリエントベクトルを差し引いて得たベ
クトルを（Ｍ−１）等分して一方向単位オリエントベク
トルＯａを求めると共に、前記第３の基準点のオリエン
トベクトルから前記第１の基準点のオリエントベクトル
を差し引いて得たベクトルを（Ｎ−１）等分して他方向
単位オリエントベクトルＯｂを求める。 （３）前記第２の基準点のアプローチベクトルから前記
第１の基準点のアプローチベクトルを差し引いて得たベ
クトルと前記第３の基準点のアプローチベクトルから前
記第１の基準点のアプローチベクトルを差し引いて得た
ベクトルとの和から、前記第４の基準点のアプローチベ
クトルから前記第１の基準点のアプローチベクトルを差
し引いて得たベクトルを差し引いて補正アプローチベク
トルを求め、この補正アプローチベクトルを｛（Ｍ−
１）×（Ｎ−１）｝等分して単位補正アプローチベクト
ルＡｃを求める。 （４）前記第２の基準点のオリエントベクトルから前記
第１の基準点のオリエントベクトルを差し引いて得たベ
クトルと前記第３の基準点のオリエントベクトルから前
記第１の基準点のオリエントベクトルを差し引いて得た
ベクトルとの和から、前記第４の基準点のオリエントベ
クトルから前記第１の基準点のオリエントベクトルを差
し引いて得たベクトルを差し引いて補正オリエントベク
トルを求め、この補正オリエントベクトルを｛（Ｍ−
１）×（Ｎ−１）｝等分して単位補正オリエントベクト
ルＯｃを求める。 （５）前記第１の基準点として教示されたパレタイジン
グ位置から第ｍ列目の第ｎ行目に存在するパレタイジン
グ位置Ｐ（ｍ，ｎ）のアプローチベクトルＡ（ｍ，ｎ）
とオリエントベクトルＯ（ｍ，ｎ）を次式から算出す
る。

【数２】Ａ（ｍ，ｎ）＝Ａ１＋（ｍ−１）×Ａａ＋（ｎ
−１）×Ａｂ＋｛（ｍ−１）×（ｎ−１）｝×Ａｃ 但し、Ａ１は第１の基準点のアプローチベクトル Ｏ（ｍ，ｎ）＝Ｏ１＋（ｍ−１）×Ｏａ＋（ｎ−１）×
Ｏｂ＋｛（ｍ−１）×（ｎ−１）｝×Ｏｃ 但し、Ｏ１は第１の基準点のオリエントベクトル

【００１３】この手段において、補正アプローチベクト
ルと補正オリエントベクトルは、第４の基準点での姿勢
を、単純に第１の基準点との位置関係、姿勢に関する一
方向ピッチと他方向ピッチを基に計算して得た姿勢と、
実際にロボットを第４の基準点に動かして取らせた姿勢
との間の誤差に相当する。そこで、この差を｛（Ｍ−
１）×（Ｎ−１）｝等分して単位補正アプローチベクト
ルＡｃと単位補正オリエントベクトルＯｃを求め、上記
各数式の右辺に第４項を導入したことは、各パレタイジ
ング位置でのロボット本体の歪み量を考慮し、計算によ
り求めた或るパレタイジング位置での姿勢と、そのパレ
タイジング位置にロボットを動かしたときの実際の姿勢
との間に差が生じないことを意味する。

【００１４】

【発明の実施の形態】以下、本発明の一実施例を図１〜
図８を参照しながら説明する。ロボットは、図７に示す
ように、ロボット本体１と、このロボット本体１を制御
する制御装置２と、この制御装置２に接続された教示装
置としてのティーチペンダント３とから構成されてい
る。

【００１５】上記ロボット本体１は、多関節型のものと
して構成され、ベース４と、このベース４に水平方向に
旋回可能に支持されたショルダ部５と、このショルダ部
５に上下方向に旋回可能に支持された下アーム６と、こ
の下アーム６に上下方向に旋回可能に支持された上アー
ム７と、この上アーム７に上下方向に旋回可能に支持さ
れた手首８とから構成されており、手首８は先端部に回
転（捻り動作）可能なフランジ９を備えている。なお、
図示はしないが、ワークを把持するハンドはフランジ９
に取り付けられるようになっている。

【００１６】一方、前記制御装置２は、図６に示すよう
に、制御部としてのＣＰＵ１０、駆動回路１１、位置検
出回路１２を備えている。そして、上記ＣＰＵ１０に
は、記憶手段として、ロボット全体のシステムプログラ
ムを記憶したＲＯＭ１３、ロボット本体１の動作プログ
ラムを記憶したＲＡＭ１４が接続されていると共に、教
示作業を行なう際に使用する教示手段としてのティーチ
ペンダント３が接続されている。

【００１７】上記位置検出回路１２は、ショルダ部５、
各アーム６，７、手首８およびフランジ９の位置を検出
するためのもので、この位置検出回路１２には、ショル
ダ部５、各アーム６，７、手首８およびフランジ９の駆
動モータ１５に設けられた位置検出センサとしてのロー
タリエンコーダ１６が接続されている。上記位置検出回
路１２は、ロータリエンコーダ１６の検出信号によって
ショルダ部５、各アーム６，７、手首８およびフランジ
９の位置を検出し、その位置検出情報はＣＰＵ１０に与
えられる。そして、ＣＰＵ１０は、動作プログラムに基
づいてショルダ部５、各アーム６，７、手首８およびフ
ランジ９を動作させる際、位置検出回路１２からの入力
信号をフィードバック信号としてそれらの動作を制御す
るようになっている。なお、図６では、駆動モータ１５
およびロータリエンコーダ１６は１個のみ示すが、実際
には、各部の動作に対して一対一の関係で複数設けられ
ているものである。

【００１８】さて、上記構成において、パレタイジング
作業を行なう場合において、パレタイジング位置を計算
によって求めるための手順を説明する。なお、この実施
例では、図４に示すように、パレット１７は、横方向に
Ｍ個（列）、縦方向Ｎ個（行）並ぶ計（Ｍ×Ｎ）個のパ
レタイジング位置に順次ワークを配置していくパレタイ
ジング作業を行なう場合を例にして説明する。

【００１９】パレタイジング位置を計算によって求める
には、まず、縦横に並ぶパレタイジング位置のうち４隅
のパレタイジング位置Ｐ（１，１）、Ｐ（Ｍ，１）、Ｐ
（１，Ｎ）、Ｐ（Ｍ，Ｎ）を教示する。なお、以下の説
明では、上記４隅のパレタイジング位置Ｐ（１，１）、
Ｐ（Ｍ，１）、Ｐ（１，Ｎ）、Ｐ（Ｍ，Ｎ）はそれぞれ
Ｐ１、Ｐ２、Ｐ３、Ｐ４で示すこととする。

【００２０】ここで、ロボットに動作を教示する場合、
その教示される位置は手首８のフランジ９の先端面の中
心Ｐｏが占める座標で示される。一方、手首８の姿勢
は、図８に示すように、フランジ９の先端面の中心Ｐｏ
から直角前向きに突出する長さ「１」のアプローチベク
トルＡと、同じくフランジ９の先端面の中心Ｐｏからフ
ランジ９に形成された孔９ａの中心を通って図示左方に
突出する長さ「１」のオリエントベクトルＯを設定し、
ロボット座標をその原点がフランジ９の中心Ｐｏに合致
するように平行移動させたとき、その平行移動されたロ
ボット座標上でのアプローチベクトルＡとオリエントベ
クトルＯで教示される。

【００２１】さて、４隅のパレタイジング位置Ｐ１〜Ｐ
４の教示は、ティーチペンダント３を使用して実際にロ
ボット本体１を動かし、そして、図示しないハンドに把
持したワークをパレタイジング位置Ｐ１〜Ｐ４で停止さ
せる。すると、その停止した時のショルダ５、各アーム
６，７、手首８、フランジ９の位置がそれらの駆動モー
タ１５のロータリエンコーダ１６により検出され、ＣＰ
Ｕ１０はその位置検出信号から各パレタイジング位置Ｐ
１〜Ｐ４でのフランジ９の中心Ｐｏの座標、それら各パ
レタイジング位置Ｐ１〜Ｐ４での手首８の姿勢がＲＡＭ
１４に記憶される。

【００２２】この場合、ロボット本体１の組み立て誤差
や歪みなどの影響で、４隅のパレタイジング位置Ｐ１〜
Ｐ４は実際には図１（ａ）に示すように教示される。ま
た、手首８の姿勢を示すアプローチベクトルＡおよびオ
リエントベクトルＯは、各パレタイジング位置Ｐ１〜Ｐ
４で同じ姿勢をとるように動作させても、ロボット本体
１の組み立て誤差や歪みなどの影響で、図２（ａ）およ
び図３（ａ）にＡ１〜Ａ４、Ｏ１〜Ｏ４で示すように各
パレタイジング位置で異なる傾きを持った姿勢で教示さ
れる。

【００２３】上述のようにして４隅のパレタイジング位
置Ｐ１〜Ｐ４およびその位置での姿勢が教示されると、
ＣＰＵ１０は、パレタイジング位置Ｐ１〜Ｐ４のうち、
任意の位置、例えばベース４に最も近いパレタイジング
位置Ｐ１を第１の基準点Ｐ１とし、この第１の基準点Ｐ
１から一方向に離れたパレタイジング位置を第２の基準
点、第１の基準点Ｐ１から他方向に離れたパレタイジン
グ位置Ｐ３を第３の基準点、第１の基準点Ｐ１と対角関
係にあるパレタイジング位置Ｐ４を第４の基準点とし、
第１の基準点Ｐ１から第２の基準点Ｐ２に至るベクトル
Ｄを（Ｍ−１）等分して一方向たる列方向単位ベクトル
ａを演算すると共に、第１の基準点Ｐ１から第３の基準
点Ｐ３に至るベクトルＥを（Ｎ−１）等分して他方向た
る行方向単位ベクトルｂを演算する。

【００２４】また、ＣＰＵ１０は、第１の基準点Ｐ１か
ら第２の基準点Ｐ２に至るベクトルＤと第１の基準点Ｐ
１から第３の基準点Ｐ３に至るベクトルＥの和を、第１
の基準点Ｐ１から第４の基準点Ｐ４に至るベクトルＦ、
換言すれば第２の基準位置Ｐ２から第４の基準位置Ｐ４
に至るベクトルＧから差し引いて差を求め、この差のベ
クトルである補正ベクトルＨを｛（Ｍ−１）×（Ｎ−
１）｝等分して単位補正ベクトルｃを求める。そして、
ＣＰＵ１０は、上述のようにして求めた前記列方向単位
ベクトルａおよび行方向単位ベクトルｂおよび単位補正
ベクトルｃをＲＡＭ１４に記憶する。

【００２５】一方、ＣＰＵ１０は、図２（ｂ）に示すよ
うに、第２の基準点Ｐ２のアプローチベクトルＡ２から
第１の基準点Ｐ１のアプローチベクトルＡ１を差し引い
て得たベクトルＪを（Ｍ−１）等分して一方向たる列方
向単位アプローチベクトルＡａを求めると共に、第３の
基準点Ｐ３のアプローチベクトルＡ３から第１の基準点
Ｐ１のアプローチベクトルＡ１を差し引いて得たベクト
ルＫを（Ｎ−１）等分して他方向たる行方向単位アプロ
ーチベクトルＡｂを求める。

【００２６】また、ＣＰＵ１０は、図３（ｂ）に示すよ
うに、第２の基準点Ｐ２のオリエントベクトルＯ２から
第１の基準点Ｐ１のオリエントベクトルＯ１を差し引い
て得たベクトルＱを（Ｍ−１）等分して一方向たる列方
向単位オリエントベクトルＯａを求めると共に、第３の
基準点Ｐ３のオリエントベクトルＯ３から第１の基準点
Ｐ１のオリエントベクトルＯ１を差し引いて得たベクト
ルＲを（Ｎ−１）等分して他方向たる行方向単位オリエ
ントベクトルＯｂを求める。

【００２７】更に、ＣＰＵ１０は、図２（ｂ）に示すよ
うに、第４の基準点Ｐ４のアプローチベクトルＡ４から
第１の基準点Ｐ３のアプローチベクトルＡ３を差し引い
てベクトルＳを得る。そして、第２の基準点Ｐ２のアプ
ローチベクトルＡ２から第１の基準点Ｐ１のアプローチ
ベクトルＡ１を差し引いて得たベクトルＪと第３の基準
点Ｐ３のアプローチベクトルＡ３から第１の基準点Ｐ１
のアプローチベクトルＡ１を差し引いて得たベクトルＫ
との和から、上記ベクトルＳを差し引いて補正アプロー
チベクトルＴを求め、この補正アプローチベクトルＴを
｛（Ｍ−１）×（Ｎ−１）｝等分して単位補正アプロー
チベクトルＡｃを得る。

【００２８】また、ＣＰＵ１０は、図３（ｂ）に示すよ
うに、第４の基準点Ｐ４のオリエントベクトルＯ４から
第１の基準点Ｐ３のオリエントベクトルＯ３を差し引い
てベクトルＵを得る。そして、第２の基準点Ｐ２のオリ
エントベクトルＯ２から第１の基準点Ｐ１のオリエント
ベクトルＯ１を差し引いて得たベクトルＱと第３の基準
点Ｐ３のオリエントベクトルＯ３から第１の基準点Ｐ１
のオリエントベクトルＯ１を差し引いて得たベクトルＲ
との和から、上記ベクトルＵを差し引いて補正オリエン
トベクトルＶを求め、この補正オリエントベクトルＶを
｛（Ｍ−１）×（Ｎ−１）｝等分して単位補正オリエン
トベクトルＯｃを得る。

【００２９】そして、ＣＰＵ１０は、上述のようにして
求めた前記列方向単位アプローチベクトルＡａ、行方向
単位アプローチベクトルＡｂ、補正単位アプローチベク
トルＡｃ、列方向単位オリエントベクトルＡａ、行方向
単位オリエントベクトルＡｂ、補正単位オリエントベク
トルＡｃをＲＡＭ１４に記憶する。

【００３０】以上のような演算が終了すると、ＣＰＵ１
０は、第１の基準点Ｐ１の位置を示す座標を残して他の
基準点Ｐ２〜Ｐ４の座標をＲＡＭ１４から消去すると共
に、第１の基準点Ｐ１のアプローチベクトルＡ１および
オリエントベクトルＯ１を残して他の基準点Ｐ２〜Ｐ４
のアプローチベクトルＡ２〜Ａ４およびオリエントベク
トルＯ２〜Ｏ４をＲＡＭ１４から消去する。これによ
り、記憶手段としてのＲＡＭ１４の記憶容量の減少化を
図っている。

【００３１】この結果、ＲＡＭ１４には、第１の基準点
Ｐ１、列方向単位ベクトルａ、行方向単位ベクトルｂ、
補正単位ベクトルｃ、第１の基準点Ｐ１でのアプローチ
ベクトルＡ１およびオリエントベクトルＯ１、補正単位
アプローチベクトルＡｃおよび補正単位オリエントベク
トルＯｃが残されることとなる。

【００３２】そして、ＣＰＵ１０は、実際のパレタイジ
ング作業を実行する場合、次の式（１）〜（３）によっ
て第１の基準点Ｐ１から第ｍ列目の第ｎ行目に存在する
パレタイジング位置Ｐ（ｍ，ｎ）、アプローチベクトル
Ａ（ｍ，ｎ）、オリエントベクトルＯ（ｍ，ｎ）を算出
し、その算出値に基づいてロボット本体１を制御する。

【００３３】

【数３】 Ｐ（ｍ，ｎ）＝Ｐ１＋（ｍ−１）×ａ＋（ｎ−１）×ｂ＋｛（ｍ−１）× （ｎ−１）｝×ｃ……（１） Ａ（ｍ，ｎ）＝Ａ１＋（ｍ−１）×Ａａ＋（ｎ−１）×Ａｂ＋｛（ｍ−１） ×（ｎ−１）｝×Ａｃ……（２） Ｏ（ｍ，ｎ）＝Ｏ１＋（ｍ−１）×Ｏａ＋（ｎ−１）×Ｏｂ＋｛（ｍ−１） ×（ｎ−１）｝×Ｏｃ……（３） ここで、上記（１）式に関して、右辺第３項まで［Ｐ１
＋（ｍ−１）×ａ＋（ｎ−１）×ｂ］は、第１の基準点
Ｐ１とパレタイジング位置の関係、列方向ピッチａと行
方向ピッチｂを基にして計算によって求めたパレタイジ
ング位置Ｐ（ｍ，ｎ）である。

【００３４】これに対し、補正ベクトルＨは、第１の基
準点Ｐ１から第２の基準点Ｐ２に至るベクトルＤと第１
の基準点Ｐ１から第３の基準点Ｐ３に至るベクトルＥと
の和を、第１の基準点Ｐ１から第４の基準点Ｐ４に至る
ベクトルＦから差し引いて得たものである。従って、こ
の補正ベクトルＨは、第４の基準点Ｐ４、すなわちパレ
タイジング位置Ｐ（Ｍ，Ｎ）の座標を、（１）式の右辺
から第４項を除いた式で求めた計算値と実際にロボット
をパレタイジング位置Ｐ（Ｍ，Ｎ）に動かして得た値と
の差に相当する。そして、この差（補正ベクトルＨ）
は、ロボット本体１を第１の基準位置Ｐ１から第４の基
準位置Ｐ４に動かした時のロボット本体１の歪み量の差
として現出したものと考えられる。

【００３５】そこで、補正ベクトルＨを｛（Ｍ−１）×
（Ｎ−１）｝等分して単位補正ベクトルｃを求め、上記
（１）式の右辺第４項に単位補正ベクトルｃを含む補正
項として導入したことは、パレタイジング位置に応じた
ロボット本体の歪み量を考慮し、計算により求めたパレ
タイジング位置と、その計算によって求めたパレタイジ
ング位置にロボット本体を動かしたときの位置とに差が
生じないことを意味する。

【００３６】また、上記（２）式、（３）式において、
右辺第３項まで［Ａ１＋（ｍ−１）×Ａａ＋（ｎ−１）
×Ａｂ］、［Ｏ１＋（ｍ−１）×Ｏａ＋（ｎ−１）×Ｏ
ｂ］は、第１の基準点Ｐ１とパレタイジング位置の関
係、姿勢に関する列方向ピッチＡａ，Ａｂ、Ｏａ，Ｏｂ
を基に計算によって求めた姿勢である。

【００３７】これに対し、第４の基準点Ｐ４、すなわち
パレタイジング位置Ｐ（Ｍ，Ｎ）の補正アプローチベク
トルＡｃと補正オリエントベクトルＯｃを上記（２）
式、（３）式の右辺第３項までで計算した値と、実際に
ロボット本体１をパレタイジング位置Ｐ（Ｍ，Ｎ）に動
かして得るべく姿勢を取らせたときの値との誤差に相当
する。

【００３８】そこで、この誤差である補正アプローチベ
クトルＡｃ、補正オリエントベクトルＯｃを｛（Ｍ−
１）×（Ｎ−１）｝等分して単位補正アプローチベクト
ルＡｃと単位補正オリエントベクトルＯｃを求め、上記
（２）式、（３）式の右辺に第４項に単位補正アプロー
チベクトルＡｃ、単位補正オリエントベクトルＯｃを含
む補正項を導入したことは、パレタイジング位置に応じ
たロボット本体１の歪み量を考慮し、計算により求めた
ロボットの姿勢と、その計算によって求めた姿勢にロボ
ット本体を動かしたときの姿勢とに差が生じないことを
意味する。

【００３９】従って、本実施例によれば、ロボット本体
１が歪みを生じたりしても、より正確に実際のパレタイ
ジング位置に移動させ得るものであり、また、その時の
姿勢も得るべき姿勢により正確に制御できるものであ
り、軸状のワークをパレットの穴に対して挿脱する作業
も支承なく行なうことができるものである。

【００４０】実際のパレタイジング作業の実行時、ＣＰ
Ｕ１０は図５に示すフローチャートに従って順次パレタ
イジング作業を実行して行く。この図５のフローチャー
トは図４のパレタイジング位置に対して１行ずつ順番に
パレタイジング作業を行なうためのものである。

【００４１】すなわち、パレタイジング作業時には、
ｍ，ｎの値は予め「１」にセットされている。そして、
パレタイジング作業を開始すると、ＣＰＵ１０は、まず
ステップＳ１でＲＡＭ１４に記憶されている第１の基準
点Ｐ１、アプローチベクトルＡ１、オリエントベクトル
Ｏ１を読み出してパレタイジング位置Ｐ（１，１）にパ
レタイジング作業を行なう。

【００４２】その後、ＣＰＵ１０は、ステップＳ２で
「ｍ＝Ｍ」か否かを判断する。ここではｍは「１」に設
定されているので、「ＮＯ」と判断し、次の「ｍ＝ｍ＋
１」を実行するステップＳ３でｍを「２」にセットす
る。そして、このとき、ｎは未だ「１」のままであるか
ら、パレタイジング位置を計算するステップＳ４では、
ＣＰＵ１０は、第２列目の第１行目にあるパレタイジン
グ位置Ｐ（２，１）の座標、アプローチベクトルＡ
（２，１）およびオリエントベクトルＯ（２，１）を演
算し、その演算結果をＲＡＭ１４に一時記憶し、ステッ
プＳ１に戻る。

【００４３】そして、ステップＳ１でＲＡＭ１４に一時
記憶したパレタイジング位置Ｐ（２，１）にロボット本
体１を動作させる。以後、ｍがＭになるまで第１行目の
パレタイジング位置に対して順にパレタイジング作業を
行ない、第１行目のパレタイジング位置Ｐ（１，１）〜
Ｐ（Ｍ，１）の全てに対してパレタイジング作業を終了
すると、ＣＰＵ１０はステップＳ２で「ＹＥＳ」となっ
てステップＳ６でｍを「１」にセットし、次のステップ
Ｓ７でｎがＮであるか否かを判断する。ここでは、未だ
ｎは「１」であるから、ステップＳ７で「ＮＯ」と判断
し、次の「ｎ＝ｎ＋１」を実行するステップＳ８でｎを
「２」にセットする。この結果、ステップＳ４では、第
１列目第２行目のパレタイジング位置（１，２）に関し
て前記式（１）〜（３）の演算を行ない、その結果をＲ
ＡＭ１４に一時記憶する。

【００４４】この後、ＣＰＵ１０は、ステップＳ１に戻
り、第１列目第２行目のパレタイジング位置Ｐ（１，
２）にパレタイジング作業を行なう。そして、以後、Ｃ
ＰＵ１０は、上述したと同様にして、第２行目のパレタ
イジング位置に対して順にパレタイジング作業を行な
い、第２行目のパレタイジング位置Ｐ（１，２）〜Ｐ
（Ｍ，２）の全てに対してパレタイジング作業を終了す
ると、次に第３行目第１列目のパレタイジング位置Ｐ
（１，３）に対してパレタイジング作業を行なう、とい
うように第１列目第１行目のパレタイジング位置Ｐ
（１，１）から最後のパレタイジング位置Ｐ（Ｍ，Ｎ）
まで順にパレタイジング作業を実行するものである。

【００４５】なお、第１の基準点Ｐ１の位置や計算結果
などを一時的に記憶する記憶手段としてはＲＡＭ１４と
は別のＲＡＭを設け、これに記憶させるようにしても良
い。

【図面の簡単な説明】

【図１】本発明の一実施例を示すもので、パレタイジン
グ位置の計算式を導くためのベクトル図

【図２】アプローチ方向の姿勢の計算式を導くためのベ
クトル図

【図３】オリエント方向の姿勢の計算式を導くためのベ
クトル図

【図４】パレタイジング位置を示す平面図

【図５】パレタイジング作業を順に行なうためのフロー
チャート

【図６】ロボットの電気的ブロック図

【図７】ロボットの斜視図

【図８】ロボットの手首の斜視図

【図９】従来の問題点の説明図

【符号の説明】

図中、１はロボット本体、２は制御装置、３はティーチ
ペンダント、８は手首、９はフランジ、１０はＣＰＵで
ある。

───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.⁷，ＤＢ名) G05B 19/18 - 19/46 B25J 3/00 - 3/04 B25J 9/10 - 9/22 B25J 13/00 - 13/08 B25J 19/02 - 19/06

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】 一方向にＭ列、他方向にＮ行並ぶパレタ
   イジング位置でのロボットの姿勢を計算によって求める
   ロボットのパレタイジング姿勢算出方法において、 前記（Ｍ×Ｎ）個のパレタイジング位置のうち、４隅に
   存するパレタイジング位置を教示し、いずれかの教示位
   置を第１の基準点、この第１の基準点から前記一方向に
   離れた教示位置を第２の基準点、前記第１の基準点から
   他方向に離れた教示位置を第３の基準点、前記第１の基
   準点と対角関係にある教示位置を第４の基準点として以
   下により各パレタイジング位置での姿勢を算出すること
   を特徴とするロボットのパレタイジング姿勢算出方法。 （１）前記第２の基準点のアプローチベクトルから前記
   第１の基準点のアプローチベクトルを差し引いて得たベ
   クトルを（Ｍ−１）等分して一方向単位アプローチベク
   トルＡａを求めると共に、前記第３の基準点のアプロー
   チベクトルから前記第１の基準点のアプローチベクトル
   を差し引いて得たベクトルを（Ｎ−１）等分して他方向
   単位アプローチベクトルＡｂを求める。 （２）前記第２の基準点のオリエントベクトルから前記
   第１の基準点のオリエントベクトルを差し引いて得たベ
   クトルを（Ｍ−１）等分して一方向単位オリエントベク
   トルＯａを求めると共に、前記第３の基準点のオリエン
   トベクトルから前記第１の基準点のオリエントベクトル
   を差し引いて得たベクトルを（Ｎ−１）等分して他方向
   単位オリエントベクトルＯｂを求める。 （３）前記第２の基準点のアプローチベクトルから前記
   第１の基準点のアプローチベクトルを差し引いて得たベ
   クトルと前記第３の基準点のアプローチベクトルから前
   記第１の基準点のアプローチベクトルを差し引いて得た
   ベクトルとの和から、前記第４の基準点のアプローチベ
   クトルから前記第１の基準点のアプローチベクトルを差
   し引いて得たベクトルを差し引いて補正アプローチベク
   トルを求め、この補正アプローチベクトルを｛（Ｍ−
   １）×（Ｎ−１）｝等分して単位補正アプローチベクト
   ルＡｃを求める。（４）前記第２の基準点のオリエントベクトルから前記
   第１の基準点のオリエントベクトルを差し引いて得たベ
   クトルと前記第３の基準点のオリエントベクト ルから前
   記第１の基準点のオリエントベクトルを差し引いて得た
   ベクトルとの和から、前記第４の基準点のオリエントベ
   クトルから前記第１の基準点のオリエントベクトルを差
   し引いて得たベクトルを差し引いて補正オリエントベク
   トルを求め、この補正オリエントベクトルを｛（Ｍ−
   １）×（Ｎ−１）｝等分して単位補正オリエントベクト
   ルＯｃを求める。 （５）前記第１の基準点として教示されたパレタイジン
   グ位置から第ｍ列目の第ｎ行目に存在するパレタイジン
   グ位置Ｐ（ｍ，ｎ）のアプローチベクトルＡ（ｍ，ｎ）
   とオリエントベクトルＯ（ｍ，ｎ）を次式から算出す
   る。 【数１】Ａ（ｍ，ｎ）＝Ａ１＋（ｍ−１）×Ａａ＋（ｎ
   −１）×Ａｂ＋｛（ｍ−１）×（ｎ−１）｝×Ａｃ 但し、Ａ１は第１の基準点のアプローチベクトル Ｏ（ｍ，ｎ）＝Ｏ１＋（ｍ−１）×Ｏａ＋（ｎ−１）×
   Ｏｂ＋｛（ｍ−１）×（ｎ−１）｝×Ｏｃ 但し、Ｏ１は第１の基準点のオリエントベクトル