JPH0764621A

JPH0764621A - ロボットの軌道補間装置

Info

Publication number: JPH0764621A
Application number: JP5209818A
Authority: JP
Inventors: Masahiro Ooto; 雅裕大音; Hisashi Kinoshita; 久木下
Original assignee: Matsushita Electric Industrial Co Ltd
Current assignee: Panasonic Holdings Corp
Priority date: 1993-08-25
Filing date: 1993-08-25
Publication date: 1995-03-10

Abstract

(57)【要約】【目的】作業点である先端部が移動するときに発生す
る加速度が、所定の上限値を越えないように軌道を設定
して補間できるロボットの軌道補間装置を提供する。【構成】先端部が移動する軌道の基準を与える複数個
の補間点と、前記教示点に基づく前記軌道の形状を指定
する移動命令と、教示点と軌道との間の最大経路誤差と
を備えた教示点列データＰを入力し、補間方法選択手段
が教示点列データに基づいて直線補間演算手段、円弧補
間演算手段および放物線補間演算手段の１つを選択して
教示点列データＰを出力し、教示点列データＰを入力し
た補間演算手段がその補間方法にしたがって補間演算す
る。このとき、ロボットの先端部に発生する加速度が所
定の上限値を越えないように補間点を演算する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は入力された教示点から目
標軌道を生成するロボットの軌道補間装置に関する。

【０００２】

【従来の技術】近年、ロボットがいろいろな作業現場で
活用されているが、その動作の精度と安定性が課題であ
る。

【０００３】ロボットが作業のために描く軌道は複数の
教示点によりあらかじめ与えられ、とくに作業を行う先
端部が描く軌道が重要である。１つの教示点からつぎの
教示点までの間を単純に直線運動させる場合には、教示
点を結ぶ直線軌道を教示点間の補間演算により求める。
このような折れ線軌道でロボットを動作させた場合、折
れ線の節点で先端部の速度が大きく変化して、振動する
という問題がある。

【０００４】このような問題を回避するため、折れ線の
各節点で一旦停止させる手段や、特開平１−２７４４３
号公報や特開平４−１１１００６号公報が開示している
ように、折れ線の節点を経由せず、折れ線を構成する２
つの線分を滑らかな曲線で接続して節点を内まわりする
軌道を生成する手段などがある。

【０００５】

【発明が解決しようとする課題】このような従来のロボ
ットの軌道補間装置において、折れ線の各節点で一端停
止するように動作させる上記の手段では、一定の速度で
連続的に動作させる場合のおよそ２倍の動作時間を必要
とする。

【０００６】また、折れ線の節点を内まわりする軌道で
動作させる上記の手段では、教示点とロボットの先端部
の軌道との経路誤差を設定できない手段であるため、ロ
ボットの先端部と作業対象や治具との間の距離を必要以
上に大きく設定するなど、先端部で接触や衝突が発生し
ないように勘案しながら教示点を設定しなければなら
ず、教示作業の簡易性が損なわれるとともに、動作時間
も増大する問題があった。

【０００７】本発明は上記の課題を解決するもので、大
きい速度変化がなく、動作時間が短くて、教示作業が簡
単なロボットの軌道補間装置を提供することを目的とす
る。

【０００８】

【課題を解決するための手段】本発明は上記の目的を達
成するために、請求項１に係わる本発明のロボットの軌
道演算装置は、ロボットの先端部が移動する軌道の基準
を与える複数個の教示点と、前記教示点に基づく前記軌
道の形状を指定する移動命令と、教示点と軌道との間の
最大誤差を指定する許容経路誤差とを備えた教示点列デ
ータを入力し、前記移動命令と前記許容経路誤差とに基
づいて直線補間演算手段、円弧補間演算手段と放物線補
間演算手段のうちの１つを選んで前記教示点列データを
出力する補間演算方法選択手段と、前記教示点列データ
を入力して教示点間を直線軌道で補間演算する直線補間
演算手段と、前記教示点列データを入力して連続する３
つの教示点を通る円弧軌道で補間演算する円弧補間演算
手段と、前記教示点列データを入力して連続する３つの
教示点間を放物線軌道で補間演算する放物線補間演算手
段と、補間演算された補間点のデータを入力して単位補
間時間ごとの関節角の変化量を演算して出力する関節角
演算手段とを備え、前記補間点に従って移動するロボッ
トの先端部の加速度が所定上限値以下となるようにした
ロボットの軌道補間装置であり、また、請求項２に係わ
る本発明は、ロボットの先端部が移動する軌道の基準を
与える複数個の教示点と、前記教示点に基づく前記軌道
の形状を指定する移動命令と、教示点と軌道との間の最
大誤差を指定する許容経路誤差とを備えた教示点列デー
タを入力し、前記移動命令と前記許容経路誤差とに基づ
いて直線補間演算手段と円弧補間演算手段と二重放物線
補間演算手段のうちの１つを選んで前記教示点列データ
を出力する補間演算方法選択手段と、前記教示点列デー
タを入力して教示点間を直線軌道で補間演算する直線補
間演算手段と、前記教示点列データを入力して連続する
３つの教示点を通る円弧軌道で補間演算する円弧補間演
算手段と、前記教示点列データを入力して連続する３つ
の教示点間を２つの放物線軌道を連結して補間演算する
放物線補間演算手段と、補間演算された補間点のデータ
を入力して単位補間時間ごとの関節角の変化量を演算し
て出力する関節角演算手段とを備え、前記補間点に従っ
て移動するロボットの先端部の加速度が所定上限値以下
となるようにしたロボットの軌道補間装置であり、ま
た、請求項３に係わる本発明は、ロボットの先端部が移
動する軌道の基準を与える複数個の教示点と、前記教示
点に基づく前記軌道の形状を指定する移動命令と、教示
点と軌道との間の最大誤差を指定する許容経路誤差とを
備えた教示点列データを入力し、前記移動命令と前記許
容経路誤差とに基づいて直線補間演算手段と円弧補間演
算手段と放物線切換補間演算手段のうちの１つを選んで
前記教示点列データを出力する補間演算方法選択手段
と、前記教示点列データを入力して教示点間を直線軌道
で補間演算する直線補間演算手段と、前記教示点列デー
タを入力して連続する３つの教示点を通る円弧軌道で補
間演算する円弧補間演算手段と、前記教示点列データを
入力して連続する３つの教示点で構成される折れ線軌道
の内角の大きさにより放物線補間演算手段と二重放物線
補間演算手段のうちの１つを選択して補間演算する放物
線切換補間演算手段と、補間演算された補間点のデータ
を入力して単位補間時間ごとの関節角の変化量を演算し
て出力する関節角演算手段とを備え、前記補間点に従っ
て移動するロボットの先端部の加速度が所定上限値以下
となるようにしたロボットの軌道補間装置であり、ま
た、請求項４に係わる本発明は、ロボットの先端部が移
動する軌道の基準を与える複数個の教示点と、前記教示
点に基づく前記軌道の形状を指定する移動命令と、教示
点と軌道との間の最大誤差を指定する許容経路誤差とを
備えた教示点列データを入力し、前記移動命令と前記許
容経路誤差とに基づいて直線補間演算手段と円弧補間演
算手段と円弧放物線補間演算手段のうちの１つを選んで
前記教示点列データを出力する補間演算方法選択手段
と、前記教示点列データを入力して教示点間を直線軌道
で補間演算する直線補間演算手段と、前記教示点列デー
タを入力して連続する３つの教示点を通る円弧軌道で補
間演算する円弧補間演算手段と、前記教示点列データを
入力して連続する３つの教示点間を円弧軌道と放物線軌
道を連結して補間演算する放物線補間演算手段と、補間
演算された補間点のデータを入力して単位補間時間ごと
の関節角の変化量を演算して出力する関節角演算手段と
を備え、前記補間点に従って移動するロボットの先端部
の加速度が所定上限値以下となるようにしたロボットの
軌道補間装置である。

【０００９】

【作用】本発明は、請求項１に係わる本発明において、
補間演算方法選択手段が入力した教示点列データに基づ
いて補間演算の方法が直線補間、円弧補間および放物線
補間のいづれであるかを選択して教示点列データを直線
補間演算手段、円弧補間演算手段および放物線補間演算
手段のうちの１つに出力する。直線補間演算手段は教示
点を直線で結んで補間点を演算し、円弧補間演算手段は
連続する３つの教示点を円弧で結んで補間演算し、放物
線補間演算手段は連続する３点で構成される折れ線を放
物線で滑らかに接続し、放物線軌道を移動するときの加
速度が所定の上限値以下となるように補間点を演算す
る。関節角演算手段は演算された補間点のデータに基づ
いて関節角の変化量を演算して出力する。

【００１０】また、請求項２に係わる本発明において、
補間演算方法選択手段が入力した教示点列データに基づ
いて補間演算の方法が直線補間、円弧補間および二重放
物線補間のいづれであるかを選択して教示点列データを
直線補間演算手段、円弧補間演算手段および放物線補間
演算手段のうちの１つに出力する。直線補間演算手段は
教示点を直線で結んで補間点を演算し、円弧補間演算手
段は連続する３つの教示点を円弧で結んで補間演算し、
二重放物線補間演算手段は連続する３点で構成される折
れ線を２つの放物線の連結で滑らかに接続し、放物線軌
道を移動するときの加速度が所定の上限値以下となるよ
うに補間点を演算する。関節角演算手段は演算された補
間点のデータに基づいて関節角の変化量を演算して出力
する。

【００１１】また、請求項３に係わる本発明において、
補間演算方法選択手段が入力した教示点列データに基づ
いて補間演算の方法が直線補間、円弧補間および放物線
切換放物線補間のいづれであるかを選択して教示点列デ
ータを直線補間演算手段、円弧補間演算手段および放物
線切換補間演算手段のうちの１つに出力する。直線補間
演算手段は教示点を直線で結んで補間点を演算し、円弧
補間演算手段は連続する３つの教示点を円弧で結んで補
間演算し、放物線切換補間演算手段は連続する３点で構
成される折れ線を、折れ線の内角の大きさに応じて放物
線補間または二重放物線補間のいづれかで補間演算し、
放物線軌道を移動するときの加速度が所定の上限値以下
となるように補間点を演算する。関節角演算手段は演算
された補間点のデータに基づいて関節角の変化量を演算
して出力する。

【００１２】また、請求項４に係わる発明において、補
間演算方法選択手段が入力した教示点列データに基づい
て補間演算の方法が直線補間、円弧補間および円弧放物
線補間のいづれであるかを選択して教示点列データを直
線補間演算手段、円弧補間演算手段および円弧放物線補
間演算手段のうちの１つに出力する。直線補間演算手段
は教示点を直線で結んで補間点を演算し、円弧補間演算
手段は連続する３つの教示点を円弧で結んで補間演算
し、円弧放物線補間演算手段は連続する３点で構成され
る折れ線を、円弧と放物線との連結で補間演算し、円弧
軌道および放物線軌道を移動するときの加速度が所定の
上限値以下となるように補間点を演算する。関節角演算
手段は演算された補間点のデータに基づいて関節角の変
化量を演算して出力する。

【００１３】

【実施例】

（実施例１）以下、請求項１に係わる本発明のロボット
の軌道補間装置の一実施例について図面を参照しながら
説明する。図１は本実施例のロボットの軌道補間装置の
主要部である軌道演算手段の構成を示すブロック図であ
る。図において、１は軌道演算手段であって、補間方法
選択手段１１と直線補間演算手段１２と放物線補間演算
手段１３と関節角演算手段１５とを備えている。上記構
成要素の相互関係と動作について説明すると、補間方法
選択手段１１は、ロボットの先端部が移動する軌道の基
準を与える複数個の教示点と、前記教示点に基づく軌道
の形状を指定する移動命令と、教示点と軌道との間の最
大誤差を指定する許容経路誤差とを備えた教示点列デー
タＰを入力し、教示点列データＰに記録されている移動
命令と許容経路誤差の値とに従って教示点間の補間演算
の方法を選択し、選択した結果に従って教示点列データ
Ｐを直線補間演算手段１２、放物線補間演算手段１３と
円弧補間演算手段１４の１つに出力する。教示点列デー
タＰを入力した上記の１つの補間演算手段は、教示点列
データＰに記録されている教示点の位置座標データから
教示点間をそれぞれの補間方法で補間演算し、補間点デ
ータＰn を関節角演算手段１５に出力する。関節角演算
手段１５は補間点データＰn から補間時間ごとの関節角
の変化量を計算し、関節角データＪを外部に出力する。

【００１４】以下、補間方法選択手段１１の動作につい
て詳細に説明する。図２は補間方法選択手段１１の選択
動作を示すフローチャートである。処理２１において、
入力された教示点列データＰに記録された移動命令が円
弧移動の命令であるか否かを判断する。教示点列データ
Ｐに記録された１点目の教示点の移動命令が円弧移動の
命令であれば、処理２２に移行して教示点列データＰを
円弧補間演算手段１４に出力する。教示点列データＰに
記録された１点目の教示点の移動命令が円弧移動の命令
でなければ処理２３に移行し、２点目の教示点に付加さ
れた許容経路誤差が０か否かを判断する。許容経路誤差
が０であれば処理２４に移行して教示点列データＰを直
線補間演算手段１２に出力する。処理２３において、許
容経路誤差が０でなければ処理２５に移行して教示点列
データＰを放物線補間演算手段１３に出力する。以上の
処理により円弧補間演算手段、直線補間演算手段または
放物線補間演算手段のうちの１つの補間演算手段が選択
される。

【００１５】以下、放物線補間演算手段１３の放物線補
間演算処理について詳細に説明する。図３は本実施例の
放物線補間演算手段１３の動作を示すフローチャートで
ある。また、図４は教示点Ａ、Ｂ、Ｃで規定される折れ
線軌道を本実施例の放物線補間演算により放物線軌道で
補間する方法を示す説明図である。

【００１６】本実施例の放物線補間演算は、連続する３
つの教示点で規定される折れ線の節点から許容経路誤差
以内の点を通って折れ線に接する放物線軌道を決定し、
ロボットの先端部がこの放物線軌道を移動するときの加
速度が所定値以下になるように補間する演算である。こ
の場合、一定加速度αを受ける一定速度Ｖの運動が放物
線軌道となり、逆に、一定速度Ｖに加わる加速度を放物
線軌道から求めると一定の加速度αとなることに着目
し、本実施例では、その加速度αが所定値以下となる速
度Ｖで直線上を補間した点から放物線上の補間点を求め
る手段について説明する。このような放物線上の補間点
は、折れ線上の一定速度Ｖの補間点を加速度αの方向で
放物線上に投影した点となる。

【００１７】以下、上記の方法による本実施例の放物線
補間演算について説明する。まず、折れ線を滑らかに接
続する放物線軌道を決定する。処理３１において、放物
線を決定するパラメータを求める。いま、図３におい
て、連続する３つの教示点Ａ、Ｂ、Ｃで規定される折れ
線ＡＢＣを許容経路誤差がＤa となる放物線で滑らかに
接続する場合、点Ｂから∠ＡＢＣの二等分線の方向に距
離Ｄa だけ離れた点Ｂ’を通過して折れ線ＡＢＣに接す
る放物線を採用する。この放物線が線分ＡＢに接する点
Ｍから点Ｂまでの距離、および線分ＢＣに接する点Ｎか
ら点Ｂまでの距離Ｌを（１）式の条件により（２）式か
ら演算する。

【００１８】Ｄ＝Ｄa ・・・（１）Ｌ＝２Ｄ／｛COS（θ／2）｝・・・（２）ここでθは線分ＡＢと線分ＢＣとによって構成される内
角である。なお、この放物線は点Ｍと点Ｎで折れ線ＡＢ
Ｃと交わるのではなく、同一勾配で滑らかに接続するこ
とが証明されるが詳細な説明を省略する。つぎに、処理
３２に移行して線分ＡＢの長さと線分ＢＣの長さの小さ
い方の大きさを（３）式により求める。

【００１９】Ｌa＝ｍｉｎ（｜ＡＢ｜,｜ＢＣ｜）・・・（３）ここで、ｍｉｎ（ｘ，ｙ）はｘとｙのうちで大きさが小
さい方を選択する関数である。

【００２０】つぎに、処理３３に移行して、放物線が折
れ線ＡＢＣに接する点Ｍと点Ｎがそれぞれ線分ＡＢと線
分ＢＣの上に存在できるのか、または延長線上にはみ出
すのかを判定するために、ＬをＬa と比較し、ＬがＬa
より小さいか否かを判断する。ＬがＬa よりも小さけれ
ば処理３５に移行して点Ｍ、Ｎの位置を点Ｂから（２）
式で求めた距離Ｌだけ離れた点とする。

【００２１】ＬがＬa よりも大きければ、点Ａまたは点
Ｃで接する放物線を採用することとし、この放物線が通
る点の点Ｂからの距離、すなわち経路誤差Ｄを処理３４
に移行して（４）式の条件により（５）式から求め、つ
ぎに処理３５に移行してこの放物線が折れ線に接する点
を求めるが、この場合は点Ａまたは点Ｃが接する点の１
つとなる。

【００２２】Ｌ＝Ｌa ・・・（４）Ｄ＝｛Ｌa・COS（θ／2）｝／２・・・（５）つぎに、処理３６に移行して、ロボットの先端部が放物
線軌道に沿って動作するための加速度を求める。前述の
ように本実施例においては、加速度αを一定速度Ｖと先
に求めた放物線とから求める。この一定速度Ｖを線分Ａ
Ｂ上の速度とし、加速度αの方向を線分Ｂ’Ｂに平行な
方向とする。この場合、前述のように加速度αの大きさ
は一定であり、速度Ｖによる点Ｍから仮目標点Ｎ’に至
る移動ＭＮ’と加速度αによる移動Ｎ’Ｎとをベクトル
加算した点が点Ｎであるので、加速度αの大きさは
（６）式で与えられる。

【００２３】 α＝｛Ｖ・COS（θ／2）｝²／２Ｄ・・・（６）つぎに、処理３７に移行して、処理３６で求めた加速度
αの大きさが所定の加速度上限値αmax より小さいか否
かを比較により判断する。加速度αの大きさが上限値α
max より小さければαをそのまま採用して処理４０に移
行する。加速度αの大きさが上限値αmax より小さくな
ければ処理３８に移行して、α＝αmaxを採用すること
とし、加速度αの大きさががαmax となる速度Ｖを
（７）式の条件により（８）式から求め、処理４０に移
行する。

【００２４】 α＝αmax ・・・（７）Ｖ＝（２Ｄ・α）^1/2／COS（θ／2）・・・（８）この速度Ｖの大きさは（６）式で用いた速度Ｖの大きさ
よりも小さい値となる。

【００２５】処理４０では、線分ＡＭ上の補間点を速度
Ｖのもとに直線補間演算によって求める。つぎに、処理
４１に移行して、仮目標点Ｎ’を点Ｎを通ってＢＢ’に
平行な直線と線分ＡＢとの交点として設定し、処理４２
に移行する。

【００２６】処理４２では、線分ＭＮ’間の補間点Ｑk
を速度Ｖのもとに直線補間で求める。ただしＱk におけ
るｋは線分ＭＮ’間の補間点の配列順序を示す番号であ
る。

【００２７】つぎに処理３４に移行して、加速度αによ
る線分ＢＢ’に平行な位置の変化量ΔＰk を、処理４２
で求めた線分ＭＮ’上の補間点Ｑk にベクトル加算して
放物線軌道上の補間点座標Ｐk を求める。この位置変化
量ΔＰk は（９）式で与えられる。

【００２８】 ΔＰk＝｛α（ｋ・ｔ）²／２｝・ｎ・・・（９）ここで、ｔは線分ＭＮ’間の単位補間時間であり、ｎは
線分ＢＢ’方向の単位ベクトルである。なお、別の見方
をすれば、上記の演算により求めた放物線上の補間点Ｐ
k は線分ＭＮ’上の速度Ｖにおける補間点Ｑk を線分Ｂ
Ｂ’に平行に放物線に投影した点に相当し、また、線分
ＢＮ’上の補間点を投影した点は線分ＢＮ上の速度Ｖに
よる補間点を投影した点と一致し、結局、折れ線ＡＢＣ
上の速度Ｖにおける補間点を放物線に投影した点が放物
線上の補間点となる。

【００２９】つぎに、処理４４に移行して、線分ＮＣ間
の補間点座標を速度Ｖのもとに直線補間演算によって求
め、最後に処理４５に移行して点Ａから点Ｃに至る補間
点の座標を順次、補間点データとして関節角演算手段１
５に出力して放物線補間演算手段１３の補間演算処理を
終了する。

【００３０】図５は本実施例のロボットの補間演算装置
により折れ線ＡＢＣを放物線補間演算した軌道における
ロボット先端部の位置と、その位置における移動速度と
加速度との関係の１例を示す特性図である。図におい
て、線分ＡＢのうちの点Ａから点Ｍまでと、線分ＢＣの
うちの点Ｎから点Ｃまでとは一定速度Ｖで直線補間され
た直線軌道であって、当然ロボットの先端部の速度の大
きさはＶであり、加速度はゼロである。また、点Ｍから
点Ｎまでは放物線補間された放物線軌道であって、点Ｍ
と点Ｎとにおいて直線軌道上の速度Ｖで放物線に連続的
に接続され、放物線軌道の区間ではＢＢ’方向の一定の
大きさの加速度αを受けるために、そこでの速度の大き
さは直線軌道における速度Ｖ以下となり、たとえば、放
物線軌道の中央である点Ｂ’における速度の大きさはＶ
sin（θ／2）となる。

【００３１】なお、説明を省略したが一定速度における
直線移動では加速度が発生しないし、一定速度における
円弧移動では円弧の中心に向かう加速度の大きさが一定
であって、それ以上の大きい加速度が発生しないことは
言うまでもない。

【００３２】以上のように本実施例によれば、ロボット
の先端部が移動する軌道の基準を与える複数個の教示点
と、前記教示点に基づく前記軌道の形状を指定する移動
命令と、教示点と軌道との間の最大誤差を指定する許容
経路誤差とを備えた教示点列データを入力し、前記移動
命令と前記許容経路誤差とに基づいて直線補間演算手
段、円弧補間演算手段と放物線補間演算手段のうちの１
つを選んで前記教示点列データを出力する補間演算方法
選択手段と、前記教示点列データを入力して教示点間を
直線軌道で補間演算する直線補間演算手段と、前記教示
点列データを入力して連続する３つの教示点を通る円弧
軌道で補間演算する円弧補間演算手段と、前記教示点列
データを入力して連続する３つの教示点間を放物線軌道
で補間演算する放物線補間演算手段と、補間演算された
補間点のデータを入力して単位補間時間ごとの関節角の
変化量を演算して出力する関節角演算手段とを備え、放
物線補間演算手段は、連続する３つの教示点が構成する
折れ線の節点の内角の二等分線上にあって前記節点から
許容経路誤差以内の点を通過して前記折れ線に接する放
物線軌道を生成し、折れ線上を一定速度で移動させた補
間点を前記放物線軌道上に前記節点の内角の二等分線方
向に平行に投影した点を放物線軌道上の補間点として補
間演算し、このときに発生する前記投影方向の加速度の
大きさは一定であって、その大きさが所定値以下となる
ように前記折れ線上の移動速度を設定して補間演算する
ようにしたことにより、ロボットの先端部に加わる加速
度の大きさを所定上限値以下にでき、大きい加速度が加
わらないので振動などが発生しない効果がある。また、
軌道は許容経路誤差の範囲で自動的に生成されるので、
衝突などを発生しない教示点を容易に設定できる。

【００３３】なお、実施例では直線移動部分の速度を一
定に設定したが、加速度が所定上限値以内となる範囲で
変化させてもよく、また、放物線上に投影する直線補間
点の速度についても、投影した放物線上の加速度が上限
値を越えない範囲で変化させてもよいことは言うまでも
ない。

【００３４】（実施例２）以下、請求項２に係わる本発
明のロボットの軌道補間装置の一実施例について、図面
を参照しながら説明する。図６は本実施例のロボットの
軌道補間装置の主要部である軌道演算手段の構成を示し
たブロック図である。なお、図１に示した実施例１と同
じ構成要素には同一番号を付与している。本実施例が実
施例１と異なる点は、放物線補間演算手段の代わりに二
重放物線補間演算手段６３を備えたことにある。図６に
おいて、補間方法選択手段１１は、ロボットの先端部が
移動する軌道の基準を与える複数個の教示点と、前記教
示点に基づく軌道の形状を指定する移動命令と、教示点
と軌道との間の最大誤差を指定する許容経路誤差とを備
えた教示点列データＰを入力し、教示点列データＰに記
録されている移動命令と許容経路誤差の値とに従って教
示点間の補間演算の方法を選択し、選択した結果に従っ
て教示点列データＰを直線補間演算手段１２と円弧補間
演算手段１４と二重放物線補間演算手段６３のうちの１
つに出力する。教示点列データＰを入力した上記の１つ
の補間演算手段は、教示点列データＰに記録されている
教示点の位置座標データから教示点間をそれぞれの補間
方法で補間演算し、補間点データＰn を関節角演算手段
１５に出力する。関節角演算手段１５は補間点データＰ
nから補間時間ごとの関節角の変化量を計算し、関節角
データＪを外部に出力する。

【００３５】以下、補間方法選択手段１１の動作につい
て詳細に説明する。図７は補間方法選択手段１１の動作
を示すフローチャートである。処理２１において、入力
された教示点列データＰに記録された移動命令により、
移動命令が円弧移動の命令か否かを判断する。教示点列
データＰに記録された１点目の教示点の移動命令が円弧
移動の命令であれば、処理２２に移行して教示点列デー
タＰを円弧補間演算手段１４に出力する。処理２１にお
いて、教示点列データＰに記録された１点目の教示点の
移動命令が円弧移動の命令でなければ処理２３に移行
し、２点目の教示点に付加された許容経路誤差が０か否
かを判断する。許容経路誤差が０であれば処理２４に移
行して教示点列データＰを直線補間演算手段１２へ出力
し、許容経路誤差が０でなければ処理７５に移行して教
示点列データＰを二重放物線補間演算手段６３に出力す
る。

【００３６】以下、二重放物線補間演算手段６３の二重
放物線補間演算について詳細に説明する。図８は二重放
物線補間演算手段６３の動作を示すフローチャートであ
る。また、図９は教示点Ａ、Ｂ、Ｃで規定される折れ線
軌道を本実施例の二重放物線補間により２つの放物線軌
道により補間する方法を示す説明図である。

【００３７】実施例１に記載した放物線補間演算手段は
折れ線軌道を１つの放物線により滑らかに接続する手段
であるが、本実施例の二重放物線補間演算手段は折れ線
軌道を２つの折れ線軌道に展開し、それぞれの折れ線軌
道を放物線補間により滑らかに接続する手段であって、
２つの放物線を用いた補間手段である。ただし、折れ線
軌道を放物線軌道により補間演算する原理は実施例１に
記載した内容と同一であり、放物線補間演算についての
詳細な説明は省略する。

【００３８】本実施例においては、まず、３つの教示点
で規定される折れ線軌道ＡＢＣを２つの折れ線軌道に展
開する。図９において、節点Ｂから∠ＡＢＣの二等分線
方向に許容経路誤差Ｄa だけ離れた点Ｂ’を通り、線分
ＢＢ’と直交する直線を設け、線分ＡＢとの交点を点
Ｅ、線分ＢＣとの交点をＦとする。この線分ＥＢ’Ｆを
経路誤差が許容経路誤差Ｄa 以下となる補助目標軌道と
して設定して、折れ線ＡＢＣを折れ線ＡＥＢ’と折れ線
Ｂ’ＦＮとに展開し、それぞれを放物線軌道により滑ら
かに接続する。

【００３９】処理８１において、補助目標軌道の始点Ｅ
から点Ｂに至る距離ｌを（１０）式の条件により（１
１）式から求める。この距離は点Ｂから点Ｆに至る距離
でもある。

【００４０】Ｄ＝Ｄa ・・・（１０）ｌ＝Ｄ／COS（θ／2）・・・（１１）ここでθは線分ＢＡと線分ＢＣとが構成する内角であ
る。折れ線ＡＥＢ’と折れ線Ｂ’ＦＮを滑らかに接続す
る放物線は多数存在し、いづれも許容経路誤差を満足す
る軌道を生成できるが、本実施例では点Ｂ’で接する放
物線を設定する。この設定は放物線軌道が小回りになら
ない配慮にもなる。このように設定した放物線の線分Ａ
Ｂに対する接続点Ｍと、線分ＢＣに対する接続点Ｎは
（１２）式の関係から自動的に決まる。

【００４１】処理８２に移行して、上記の放物線軌道を
設定したときの、線分ＭＥ、線分ＥＢ’、線分Ｂ’Ｆお
よび線分ＦＮの大きさと、点Ｂから接続点Ｍに至る距離
Ｌとをそれぞれ（１２）式と（１３）式により求める。

【００４２】｜ＭＥ｜＝｜ＥＢ’｜＝｜Ｂ’Ｅ｜＝｜ＥＮ｜＝Ｄtan（θ／2）・・・（１２）Ｌ＝ｌ＋Ｄtan（θ／2）・・・（１３）処理８３に移行して、線分ＡＢと線分ＢＣの小さい方の
長さＬa を（１４）式により求める。

【００４３】Ｌa＝ｍｉｎ（｜ＡＢ｜，｜ＢＣ｜）として求める。・・・（１４）ただし、ｍｉｎ（ｘ，ｙ）はｘとｙの値の小さい方の値
を選択する関数である。

【００４４】つぎに、処理８４に移行して、設定した放
物線の接続点が折れ線ＡＢＣの上に存在できるのか、ま
たはその延長線上にはみ出すのかを判断するために、Ｌ
とＬa とを比較する。ＬがＬa よりも小さければ処理８
６に移行して接続点ＭとＮの位置を求める。ＬがＬa よ
りも大きければ、先に設定した放物線が折れ線の外で接
続されることになる。この場合は、処理８５に移行し
て、点Ｂからの距離が小さい方の点Ａまたは点Ｃのいづ
れかで接し、新たな補助目標経路にも接する放物線を設
定する。このときの経路誤差Ｄを（１５）式の条件によ
り（１６）式から求める。

【００４５】Ｌ＝Ｌa ・・・（１５）Ｄ＝｛Ｌcos（θ／2）｝／｛１＋sin（θ／2）｝・・・（１６）この経路誤差Ｄは許容経路誤差Ｄa よりも小さいことは
言うまでもない。また、この場合に処理８６で求める接
続点のいづれかの位置は教示点に一致している。

【００４６】つぎに、処理８７に移行して、ロボットの
先端部が放物線軌道に沿って動作するときの加速度を求
める。いま、点Ａから点Ｂに向かう速度をＶとし、接続
点Ｍを速度Ｖで通過したのち放物線軌道に沿って移動
し、点Ｂ’に速度Ｖで到達するときの加速度αは、実施
例１で説明したように、方向が∠ＭＥＢ’の二等分線の
方向に平行であって、大きさは（１７）式と（１８）式
により求められる。

【００４７】 α＝Ｖ²sin（λ）／｛Ｄ・tan（θ／2）｝・・・（１７） λ＝（∠ＭＥＢ’）／２＝（π−θ）／４・・・（１８）また、ロボットの先端部が点Ｂ’を速度Ｖで通過したの
ち、つぎの放物線軌道に沿って移動し、点Ｎに速度Ｖで
到達するときの加速度の大きさも線分ＢＢ’に関する対
称性から上記のαとなる。ただし、この場合の加速度の
方向は∠Ｂ’ＦＮの二等分線の方向に平行となる。

【００４８】つぎに、処理８８に移行して、処理８７で
求めた角速度αの大きさが所定の上限値αmax 以下であ
るか否かを比較により判断する。αがαmax 以下であれ
ば処理８７で求めたαをそのまま採用して処理９０に移
行する。αがαmax 以下でなければ処理８９に移行し
て、α＝αmax となるような速度に変更する。この速度
は（１９）式の条件により（２０）式から求める。

【００４９】 α＝αmax ・・・（１９）Ｖ＝｛（Ｄ・α・tan（θ／2）／sinλ｝^1/2 ・・・（２０）この速度は、当然、（１７）式で用いた速度よりも小さ
いことは言うまでもない。

【００５０】つぎに、処理９０に移行して、線分ＡＭ間
の補間点を速度Ｖで直線補間演算により求める。つぎ
に、折れ線軌道ＭＥＢ’を放物線軌道で補間するため、
処理９１に移行して、線分ＭＥを同じ長さだけ延長した
位置に仮目標点Ｂ”を設定する。この手法は実施例１で
説明した内容と同じであることは言うまでもない。つぎ
に処理９２に移行して線分ＭＢ”間の補間点Ｑk を速度
Ｖで直線補間により求める。ここでＱk におけるｋはＭ
Ｂ”間の補間点の順序を示す番号である。つぎに、処理
９３に移行して、加速度αによる∠ＭＥＢ’の二等分線
方向に平行な位置変化量ΔＰk を処理９２で求めた補間
点座標Ｑk にベクトル加算して放物線軌道上の補間点座
標Ｐk を求める。このときの位置変化量ΔＰk を（２
１）式により求める。

【００５１】 ΔＰk＝｛α（ｋ・ｔ）²｝・ｎｄ・・・（２１）ここで、ｔは点ＭＮ’間の単位補間時間であり、ｎｄは
∠ＭＥＢ’の二等分線の方向の単位ベクトルを表してい
る。このようにして得た放物線軌道上の補間点は、実施
例１で説明したように、折れ線ＭＥＢ’上を速度Ｖで直
線補間した補間点を、∠ＭＥＢ’の二等分線の方向に平
行に投影した点に等しい。

【００５２】放物線軌道Ｂ’Ｎにおける補間点について
も同様であって、処理９４で仮目標点Ｎ’を設定し、処
理９５で線分Ｂ’Ｎ’間を速度Ｖで直線補間した補間点
Ｑkを求め、処理９６で放物線上の補間点ＰK を求め
る。このときに用いる加速度αによる位置変化量ΔＰK
は（２２）式により求められる。

【００５３】 ΔＰk＝｛α（ｋ・ｔ）²｝・ｎｅ・・・（２２）ここで、ｔは点Ｂ’Ｎ’間の単位補間時間であり、ｎｅ
は∠Ｂ’ＦＮの二等分線の方向の単位ベクトルを表して
いる。つぎに、処理９７において、線分ＮＣ間の補間点
座標を速度Ｖで直線補間演算によって求め、最後に処理
９８において、点Ａから点Ｃまでの補間点ＰK に基づく
補間点データＰn を関節角演算手段１５に出力し、二重
放物線補間演算手段６３の補間演算処理を終了する。

【００５４】図１０は本実施例のロボットの軌道補間装
置により折れ線軌道ＡＢＣを二重放物線補間演算した軌
道におけるロボット先端部の位置と、その位置における
移動速度および加速度の大きさとの関係の１例を示す特
性図である。図において、線分ＡＢのうちの点Ａから点
Ｍまでと、線分ＢＣのうちの点Ｎから点Ｃまでは一定速
度Ｖで直線補間された直線軌道であって、ロボットの先
端部の速度の大きさはＶであり、加速度はゼロである。
また、点Ｍから点Ｂ’までと、点Ｂ’から点Ｎまでと
は、それぞれ放物線補間された放物線軌道であって、点
Ｍと点Ｎとにおいて直線上の速度Ｖで放物線に連続的に
接続され、これら放物線軌道の区間では一定の大きさの
加速度αを受けるために、そこでの速度の大きさは直線
部における速度Ｖ以下となる。また、２つの放物線を連
結し、その連結点における速度はＶとなるため、速度の
大きさの変化は実施例１における放物線補間演算手段の
場合よりも小さくできる。放物線軌道の中央における速
度の大きさはＶcos（λ／2）であり、λ＞θであるた
め、実施例１の場合よりも小さい。

【００５５】以上のように、本実施例のロボットの軌道
補間装置によれば、ロボットの先端部が移動する軌道の
基準を与える複数個の教示点と、前記教示点に基づく前
記軌道の形状を指定する移動命令と、教示点と軌道との
間の最大誤差を指定する許容経路誤差とを備えた教示点
列データを入力し、前記移動命令と前記許容経路誤差と
に基づいて直線補間演算手段と円弧補間演算手段と二重
放物線補間演算手段のうちの１つを選んで前記教示点列
データを出力する補間演算方法選択手段と、前記教示点
列データを入力して教示点間を直線軌道で補間演算する
直線補間演算手段と、前記教示点列データを入力して連
続する３つの教示点を通る円弧軌道で補間演算する円弧
補間演算手段と、前記教示点列データを入力して連続す
る３つの教示点間を２つの放物線軌道を連結して補間演
算する放物線補間演算手段と、補間演算された補間点の
データを入力して単位補間時間ごとの関節角の変化量を
演算して出力する関節角演算手段とを備え、前記二重放
物線補間演算手段は、連続する３つの教示点が構成する
折れ線の節点の内角の二等分線上にあって前記節点から
許容経路誤差以内の点を通過して前記二等線に直交する
直線の補助軌道を設け、前記折れ線の一部と前記補助軌
道とで構成される２つの折れ線軌道をそれぞれの放物線
軌道で補間し、各放物線軌道において、折れ線上を一定
速度で移動させた補間点をその放物線が対応する内角の
二等分線方向に平行に放物線に投影した点を放物線軌道
上の補間点として補間演算し、このときに発生する前記
投影方向の加速度の大きさは一定であって、その大きさ
が所定値以下となるように前記折れ線上の移動速度を設
定して補間演算するようにしたことにより、ロボットの
先端部に加わる加速度の大きさを実施例１の場合よりも
さらに小さい範囲で所定値以下にでき、大きい加速度が
加わらないので振動などが発生しない効果がある。ま
た、軌道は許容経路誤差の範囲で自動的に生成されるの
で、衝突などを発生しない教示点を容易に設定できる。

【００５６】なお、実施例では直線移動部分の速度を一
定に設定したが、加速度が所定上限値以内となる範囲で
変化させてもよく、また、放物線上に投影する直線補間
点の速度についても、投影した放物線上の加速度が上限
値を越えない範囲で変化させてもよいことは言うまでも
ない。

【００５７】（実施例３）以下、請求項３に係わる本発
明のロボットの軌道補間装置の一実施例について図面を
参照しながら説明する。図１１は本実施例のロボットの
軌道補間装置の主要部である軌道演算手段の構成を示す
ブロック図である。なお、実施例１および実施例２と同
じ構成要素には同一番号を付与して詳細な説明を省略す
る。本実施例が実施例１と異なる点は、放物線補間演算
手段の代わりに放物線切換補間演算手段１１３を備えた
ことにある。上記構成要素の相互関係と動作について説
明すると、補間方法選択手段１１は、ロボットの先端部
が移動する軌道の基準を与える複数個の教示点と、前記
教示点に基づく軌道の形状を指定する移動命令と、教示
点と軌道との間の最大誤差を指定する許容経路誤差とを
備えた教示点列データＰを入力し、教示点列データＰに
記録されている移動命令と許容経路誤差の値とに従って
教示点間の補間演算の方法を選択し、選択した結果に従
って教示点列データＰを直線補間演算手段１２と円弧補
間演算手段１４と放物線切換補間演算手段のうちの１つ
に出力する。教示点列データＰを入力した上記の１つの
補間演算手段は、教示点列データＰに記録されている教
示点の位置座標データから教示点間をそれぞれの補間方
法で補間演算し、補間点データＰn を関節角演算手段１
５に出力する。関節角演算手段１５は補間点データＰn
から補間時間ごとの関節角の変化量を計算し、関節角デ
ータＪを外部に出力する。

【００５８】この場合、放物線切換補間演算手段１１３
において、放物線補間方法選択手段１１４は教示点列デ
ータＰを入力し、連続する３つの教示点で規定される折
れ線軌道の内角を求め、その角度の大きさにより放物線
補間演算手段１３または二重放物線補間演算手段６３の
何れかに教示点列データＰを出力する。

【００５９】以下、補間方法選択手段１１の動作につい
て詳細に説明する。図１２は本実施例における補間方法
選択手段１１の動作を示すフローチャートである。処理
２１において、入力された教示点列データＰに記録され
た移動命令が円弧移動の命令であるか否かを判断する。
教示点列データＰに記録された１点目の教示点の移動命
令が円弧移動の命令であれば、処理２２に移行して教示
点列データＰを円弧補間演算手段１４に出力する。ま
た、教示点列データＰ記録された教示点の１点目の移動
命令が円弧移動の命令でなければ処理２３に移行して、
２点目の教示点に付加された許容経路誤差が０か否かを
判断する。許容経路誤差が０であれば処理２４に移行し
て教示点列データＰを直線補間演算手段１２へ出力す
る。また、許容経路誤差が０でなければ処理１２５に移
行して、教示点列データＰを放物線切換補間演算手段１
１３に出力する。

【００６０】以下、放物線補間方法選択手段１１４の動
作について説明する。図１３は放物線補間方法選択手段
１１４の動作を示すフローチャートである。処理１３１
において、３つの教示点Ａ、Ｂ、Ｃによって規定される
折れ線軌道の線分ＢＡと線分ＢＣとがなす内角θの大き
さを所定の設定値θs と比較して判断する。θがθs以
下であれば処理１３２に移行して教示点列データＰを放
物線補間演算手段１３に出力し、θがθs 以下でなけれ
ば処理１３３に移行して教示点列データＰを二重放物線
補間演算手段６３に出力する。なお、放物線補間演算手
段１３は実施例１で説明した放物線補間演算処理を行
い、二重放物線補間演算手段６３は実施例２１で説明し
た二重放物線補間演算処理を行う手段であり、詳細な説
明を省略する。

【００６１】本実施例において、折れ線の内角が小さい
場合は単純な放物線補間を用い、内角が大きい場合には
二重放物線補間を用いて一層滑らかに補間することがで
きる。

【００６２】以上のように、本実施例のロボットの軌道
補間装置によれば、ロボットの先端部が移動する軌道の
基準を与える複数個の教示点と、前記教示点に基づく前
記軌道の形状を指定する移動命令と、教示点と軌道との
間の最大誤差を指定する許容経路誤差とを備えた教示点
列データを入力し、前記移動命令と前記許容経路誤差と
に基づいて直線補間演算手段と円弧補間演算手段と放物
線切換補間演算手段のうちの１つを選んで前記教示点列
データを出力する補間演算方法選択手段と、前記教示点
列データを入力して教示点間を直線軌道で補間演算する
直線補間演算手段と、前記教示点列データを入力して連
続する３つの教示点を通る円弧軌道で補間演算する円弧
補間演算手段と、前記教示点列データを入力して連続す
る３つの教示点で構成される折れ線軌道の内角の大きさ
により放物線補間演算手段と二重放物線補間演算手段の
うちの１つを選択して補間演算する放物線切換補間演算
手段と、補間演算された補間点のデータを入力して単位
補間時間ごとの関節角の変化量を演算して出力する関節
角演算手段とを備え、前記放物線切換補間演算手段は、
折れ線のなす内角が所定値θs 以下であれば放物線補間
演算手段を用い、θs 以下でなければ二重放物線補間演
算手段を用いるようにしたことにより、折れ線の内角の
大小に応じて放物線補間演算手段と二重放物線補間演算
手段とを使い分けでき、また、軌道上を移動するときの
加速度が所定の上限値を越えることがない。

【００６３】（実施例４）以下、請求項４に係わる本発
明のロボットの軌道補間装置の一実施例について説明す
る。図１４は本実施例のロボットの軌道補間装置におけ
る主要部である軌道演算手段の構成を示すブロック図で
ある。なお、図１に示した実施例１と同じ構成要素には
同一番号を付与して詳細な説明を省略する。本実施例が
実施例１と異なる点は放物線補間演算手段の代わりに円
弧放物線補間演算手段１４３を備えたことにある。図１
４において、軌道演算手段１４１は、ロボットの先端部
が移動する軌道の形状を指定する移動命令と、教示点と
軌道との間の最大誤差を指定する許容経路誤差とを備え
た教示点列データＰを入力し、教示点列データＰに記録
されている移動命令と許容経路誤差の値とに従って教示
点間の補間方法を選択し、選択した結果に従って教示点
列データＰを円弧補間演算手段１４と直線補間演算手段
１２と円弧放物線補間演算手段１４３のうちの１つに出
力する。教示点列データＰを入力した補間演算手段は、
教示点列データＰに記録されている教示点の位置座標デ
ータから教示点間をそれぞれの補間方法で補間演算し、
補間点データＰn を間接角演算手段１５に出力する。関
節角演算手段１５は補間点データＰn から補間時間ごと
の関節角の変化量を計算し、関節角データＪを外部に出
力する。

【００６４】以下、本実施例における補間方法選択手段
１１の動作について詳細に説明する。図１５は本実施例
における補間方法選択手段１１の動作を示すフローチャ
ートである。処理２１において、入力された教示点列デ
ータＰに記録された移動命令により、移動命令が円弧移
動の命令か否かを判断する。教示点列データＰに記録さ
れた教示点の１点目の移動命令が円弧移動の命令であれ
ば、処理２２に移行して教示点列データＰを円弧補間演
算手段１４に出力する。処理２１において、教示点列デ
ータＰに記録された教示点の１点目の移動命令が円弧移
動の命令でなければ処理２３に移行して、２点目の教示
点に付加された許容経路誤差が０か否かを判断する。許
容経路誤差が０であれば処理２４に移行して教示点列デ
ータＰを直線補間演算手段１２へ出力する。許容誤差が
０でなければ処理１５５に移行して教示点列データＰを
円弧放物線補間演算手段１４３に出力する。

【００６５】以下、本実施例における円弧放物線補間演
算手段１４３の円弧放物線補間演算動作について詳細に
説明する。図１６は円弧放物線補間演算手段１４３の動
作を示すフローチャートである。また、図１７は教示点
Ａ、Ｂ、Ｃで規定される折れ線軌道を円弧と放物線とに
より補間する方法を示す説明図である。

【００６６】本実施例の円弧放物線補間は、円弧軌道と
放物線軌道とを連結して折れ線軌道を滑らかに接続する
ものである。速度Ｖによる直線軌道と速度Ｖによる円軌
道とを単純に接続すると、円弧軌道における加速度Ｖ²
／ｒ（ただし、ｒは円弧の半径）が加速度の所定限度
値を越える場合がある。この場合、円弧軌道上の速度を
直線軌道の速度Ｖより小さくし、その速度差を放物線軌
道が有する速度変換機能により吸収することができる。
すなわち、実施例１における図５からもわかるように、
放物線軌道の中間点における速度の大きさは、放物線軌
道への進入速度Ｖより小さいＶsin（θ／2）になってお
り、速度の大きさが変換されていることが分かる。

【００６７】図１７において、３つの教示点Ａ、Ｂ、Ｃ
で規定される折れ線軌道に対して円弧放物線軌道を下記
のように設定する。いま、許容経路誤差をＤa とし、折
れ線の接点Ｂから∠ＡＢＣの二等分線の方向に距離Ｄa
だけ離れた点Ｂ’を通過し、線分ＡＢと線分ＢＣとに接
する中心点Ｇ、半径ｒの円弧を描き、その円弧と線分Ａ
Ｂの接点を点Ｅ、線分ＢＣとの接点を点Ｆとする。この
円弧ＥＢ’Ｆを折れ線軌道ＡＢＣから経路誤差Ｄa 以下
となる補助目標経路として設定し、そのうちの円弧Ｈ
Ｂ’と線分ＡＢを放物線ＭＨにより滑らかに接続し、ま
た、円弧Ｂ’Ｉと線分ＦＣとを放物線ＩＮにより滑らか
に接続する。ただし、円弧放物線軌道における放物線
は、点Ｍで線分ＡＭに接し、頂点で円弧ＨＢ’と接する
放物線ＭＨとし、また、点Ｎで線分ＮＣに接し、頂点で
円弧Ｂ’Ｉと接する放物線ＩＮとする。他の実施例にお
ける放物線軌道は頂点で接続していなかった点が異な
る。

【００６８】この場合、円弧ＨＢ’の開き角度と、円弧
Ｂ’Ｉの開き角度は一意的には決まらないので、あらか
じめ指定しておくか、または、装置に自動的に決定させ
る必要がある。たとえば、装置が∠ＥＧＢ／２に自動的
に設定することは可能である。本実施例では上記の開き
角度に対応する点Ｈおよび点Ｉを（２３）式を満足する
点として設定する例について説明する。

【００６９】 ∠ＨＧＢ’＝∠ＩＧＢ’＝λ ・・・（２３）このように円弧と放物線を設定した場合、線分ＧＨを延
長して線分ＡＢと交わる点を点Ｈ’とし、線分ＧＩを延
長して線分ＢＣと交わる点をＩ’とする。補助目標軌道
の始点Ｅと点Ｂ、終点Ｆと点Ｂの距離Ｌ0 はＤ＝Ｄa ・・・（２４）ｒ＝｛Ｄ・sin（θ／2）｝／｛１−sin（θ／2）｝・・・（２５）なる条件を用いて式（２６）により与えられる。

【００７０】Ｌ0＝｜ＤＢ｜＝｜ＢＥ｜＝（Ｄ＋ｒ）／cos（θ／2）・・・（２６）ここでθは線分ＢＡと線分ＢＣによって構成される内角
である。

【００７１】また、円弧ＨＢ’に接続する放物線が線分
ＡＢと接する点をＭ、円弧Ｂ’Ｉに接続する放物線が線
分ＢＣと接する点をＮとすると、Ｍから点Ｂまでの距
離、および点Ｎから点Ｂまでの距離Ｌはｄ＝｜ＨＨ’｜＝｜ＩＩ’｜＝ｒ｛１−sin（θ／2＋λ）｝／sin（θ／2＋λ）・・・（２７）｜Ｈ’Ｂ｜＝｜ＢＩ’｜＝｜ＤＢ｜−｜Ｈ’Ｄ｜＝（Ｄ＋ｒ）／cos（θ／2）−（ｄ＋ｒ）／cos（θ／2＋λ）・・・（２８）および｜ＭＨ’｜＝｜Ｉ’Ｎ｜＝２ｄ／cos（θ／2＋λ）・・・（２９）により、Ｌ＝｜ＭＨ’｜＋｜Ｈ’Ｂ｜＝｛Ｄ／cos（θ／2）｝・｛１＋ｆ1（θ）・ｆ2（θ,λ）｝＝Ｆ（Ｄ）・・・（３０）ただし、ｆ1（θ）＝｛sin（θ／2）｝／（１−sin（θ／2））ｆ2（θ,λ）＝｛sin（θ＋2λ）−２cos（θ／2）−２sin（θ＋λ）｝／sin（θ＋2λ）として与えられる。ここでＦ（Ｄ）は上記Ｌを与える式
の第２行目の式をＤの関数として表したものである。

【００７２】以下、図１６に示したフローチャートを参
照しながら説明する。教示点列データＰが円弧放物線補
間演算手段１４３に入力されると、処理１６１におい
て、点Ｍから点Ｂまでの距離、および点Ｎから点Ｂまで
の距離Ｌを（３０）式により求め、つぎに、処理１６２
に移行して、円弧放物線補間に使用できる最大距離Ｌa
を（３１）式により求める。

【００７３】Ｌa＝min（｜ＡＢ｜，｜ＢＣ｜）・・・（３１）ここでmin（ｘ，ｙ）はｘとｙの値のうちの小さい方の
値を与える関数である。つぎに、処理１６３に移行して
ＬとＬa を比較し、点Ｍが線分ＡＢの上にあるか否か、
また、点Ｎが線分ＢＣの上にあるか否かを判断する。Ｌ
がＬa 以下であればそのまま処理１６５に移行する。Ｌ
がＬa 以下でなければ処理１６４に移行して、Ｌ＝Ｌa
とする円弧放物線軌道に設定し直し、そのときの経路誤
差を（３２）式の条件により（３０）式を逆に解いて求
める。すなわち、（３３）から経路誤差を求め、つぎに
処理１６５に移行する。

【００７４】Ｌ＝Ｌa ・・・（３２）Ｄ＝Ｆ^-1（Ｌ）・・・（３３）ここでＦ^-1はＦの逆関数を表す。

【００７５】処理１６５では点Ｍおよび点Ｎの位置を点
Ｂから距離Ｌだけ離れた点として求める。つぎに、処理
１６６に移行して、ロボットの先端部が受ける加速度の
上限値をαmax として、その加速度のもとで円弧軌道Ｈ
Ｂ’Ｉに沿って移動するときの速度Ｖ0 を（３４）式に
より求める。

【００７６】Ｖ0＝（αmax・ｒ）^1/2 ・・・（３４）つぎに、処理１６７に移行して、ロボットの先端部が放
物線軌道ＭＨと放物線軌道ＩＮに沿って移動するときの
先端部の加速度を求める。円弧ＨＩ間の移動速度をＶ0
としたので、上記のように設定した円弧放物線軌道にお
ける放物線軌道では、点Ｍを（３５）式が与える速度Ｖ
で通過し、放物線状の軌道を経由して点Ｈに速度Ｖ0 で
到達し、また、点Ｉを速度ＶO で通過したのち放物線状
の軌道を経由して点Ｎを（３５）式が与える速度Ｖで通
過する。この関係は前述のように実施例１における図５
から明らかである。

【００７７】Ｖ＝Ｖ0／sin（θ／2＋λ）・・・（３５）この場合の加速度の方向は、放物線軌道ＭＨ上では線分
ＨＧの方向に平行であり、放物線軌道ＩＮ上では線分Ｉ
Ｇの方向に平行であり、加速度の大きさは（３６）式に
より与えらる。

【００７８】 α＝｛Ｖ・cos（θ／2＋λ）｝²／２ｄ・・・（３６）つぎに、処理１６８において、処理１６７で求めた加速
度αの大きさが所定の上限値αmax 以下であるか否かを
判断する。αがαmax 以下であれば処理１６７で求めた
αをそのまま採用して処理１７０に移行する。また、α
がαmax 以下でなければ処理１６９に移行して、α＝α
max とし、この条件を満たす速度Ｖと速度Ｖ0 とを（３
７）式の条件により（３８）式および（３９）式の与え
る値に変更する。すなわち、移動速度を小さく設定し直
す。

【００７９】 α＝αmax ・・・（３７）Ｖ＝（２ｄ・α）^1/2／cos（θ／２＋λ）・・・（３８）Ｖ0＝Ｖ・Sin（θ／２＋λ）・・・（３９）以上の処理により準備されたパラメータを用いて軌道上
を通過する補間点を求める。まず、処理１７０におい
て、線分ＡＭ間の補間点を速度Ｖにおける直線補間演算
によって求める。つぎに、処理１７１に移行して、線分
ＭＨ’上の補間点ＱK を速度Ｖにおける直線補間で求め
る。ここで、Ｑk におけるｋは軌道ＭＢ”間の補間順序
の番号を表している。つぎに、処理１７２に移行して加
速度αによる位置の変化量ΔＰk を処理１７１で求めた
補間点ＰK にベクトル加算して放物線軌道上の補間点座
標Ｐk を求める。このときの加速度による位置変化量Δ
Ｐkは（４０）式で与えられる。

【００８０】 ΔＰk＝｛α（ｋ・ｔ）²／２｝・ｎｈ・・・（４０）ここで、ｔは点ＭＨ’間の単位補間時間であり、ｎｈは
線分ＨＧ方向の単位ベクトルを表している。なお、この
ようにして演算した放物線上の補間点ＰK は、線分Ｍ
Ｈ’上の速度Ｖにおける補間点を、加速度の方向、すな
わち、線分ＨＨ’の方向に平行に投影した補間点に同じ
であることは他の実施例と同様である。

【００８１】つぎに、処理１７３に移行して点Ｈから点
Ｉまでの円弧軌道を速度Ｖ0 で補間し、処理１７４に移
行して、線分Ｉ’Ｎ間の補間点Ｑk を速度Ｖにおける直
線補間で求める。ここで、Ｑk におけるｋはＩ’Ｎ間の
補間順序の番号を表している。つぎに、処理１７５に移
行して、加速度αによる位置の変化量ΔＰk を処理９５
で求めた補間点Ｑk にベクトル加算して放物線上の補間
点Ｐk を求める。このとき位置変化量ΔＰk は（４１）
式で与えられる。

【００８２】 ΔＰk＝｛α（Ｔ−Ｋ・ｔ）²／２｝・ｎｉ・・・（４１）ここでｔは点Ｉ’Ｎ間の動作時間であり、ｎｉは線分Ｉ
Ｇ方向の単位ベクトルを表している。この放物線Ｉ’Ｎ
上の補間点についても、線分Ｉ’Ｎ上の速度Ｖにおける
直線補間点を、加速度の方向、すなわち、線分Ｉ’Ｉの
方向に平行に放物線上に投影した補間点であることは言
うまでもない。つぎに、処理１７６に移行して、線分Ｎ
Ｃ上の補間点を速度Ｖにおける直線補間演算によって求
め、最後に処理１７７において点Ａから点Ｃまでの補間
点Ｐk を順次補間点データＰn として関節角演算手段１
５に出力し、円弧放物線補間演算手段１４３の補間演算
処理を終了する。

【００８３】図１８は本実施例の補間演算処理によって
求めたロボット先端部の位置と、その位置における速度
および加速度との関係を示す特性図である。図からわか
るように、直線軌道ＡＭおよびＮＣでは速度Ｖで移動し
て加速度はゼロであり、円弧軌道ＨＩでは加速度の上限
値αmax で決まる速度Ｖ0 で移動して加速度は当然αma
x である。放物線軌道ＭＨでは速度ＶからＶ0 に、放物
線軌道ＩＮでは速度Ｖ0 からＶに変化して加速度はα≦
αmax であり、ロボットの先端部の加速度の大きさはい
づれの軌道においてもαmax 以下であり、急激な速度変
化を速度変化を伴うことなく線分ＡＢと線分ＢＣとが滑
らかに接続されていることがわかる。

【００８４】なお、図１８では円弧軌道をαmax で決ま
る速度で通過させた場合を示したが、それ以下の速度で
通過させる場合、すべての軌道において加速度の大きさ
が上限値以下であることは言うまでもない。

【００８５】以上のように本実施例のロボットの軌道補
間装置によれば、ロボットの先端部が移動する軌道の基
準を与える複数個の教示点と、前記教示点に基づく前記
軌道の形状を指定する移動命令と、教示点と軌道との間
の最大誤差を指定する許容経路誤差とを備えた教示点列
データを入力し、前記移動命令と前記許容経路誤差とに
基づいて直線補間演算手段と円弧補間演算手段と円弧放
物線補間演算手段のうちの１つを選んで前記教示点列デ
ータを出力する補間演算方法選択手段と、前記教示点列
データを入力して教示点間を直線軌道で補間演算する直
線補間演算手段と、前記教示点列データを入力して連続
する３つの教示点を通る円弧軌道で補間演算する円弧補
間演算手段と、前記教示点列データを入力して連続する
３つの教示点間を円弧軌道と放物線軌道を連結して補間
演算する放物線補間演算手段と、補間演算された補間点
のデータを入力して単位補間時間ごとの関節角の変化量
を演算して出力する関節角演算手段とを備え、前記円弧
放物線補間演算手段は、連続する３つの教示点が構成す
る折れ線の節点の内角の二等分線上にあって前記節点か
ら許容経路誤差以内の点を通過して前記折れ線に接する
円弧を設定し、前記円弧のうち前記節点に対向する部分
円弧を円弧軌道とし、前記円弧軌道の両端をそれぞれ対
向する前記折れ線の線分と放物線により滑らかに接続し
て円弧放物線軌道を構成し、前記放物線はそれぞれ円弧
軌道の端を頂点とする放物線であって、折れ線上を一定
の速度で補間した点を、円弧と放物線との交点と円弧の
中心とを結ぶ方向に平行な方向に投影した点を放物線上
の補間点とするとともに、円弧との接続点における速度
が円弧上の速度と一致するように前記一定速度を設定
し、放物線軌道上の加速度の大きさおよび円弧軌道上の
加速度の大きさが所定の上限値を越えないように補間演
算するようにしたことにより、ロボットの先端部に加わ
る加速度の大きさを所定上限値以下にでき、大きい加速
度が加わらないので振動などが発生しない効果がある。
また、軌道は許容経路誤差の範囲で自動的に生成される
ので、衝突などを発生しない教示点を容易に設定でき
る。

【００８６】なお、実施例では直線移動部分の速度を一
定に設定したが、加速度が所定上限値以内となる範囲で
変化させてもよく、また、放物線上に投影する直線補間
点の速度についても、投影した放物線上の加速度が上限
値を越えない範囲で変化させてもよく、また、円弧上の
速度についても加速度が上限値を越えない範囲で変化さ
せてもよいことは言うまでもない。

【００８７】実施例１ないし実施例４では本発明の最も
好ましい実施様態の一例について説明したが、それらの
組み合せによる他の構成も可能であることは言うまでも
ない。

【００８８】

【発明の効果】以上の説明から明らかなように、本発明
のロボットの軌道補間装置によれば、連続する３点以上
の教示点で規定される折れ線軌道を、教示点列データの
移動命令と許容経路誤差とに基づいて放物線軌道、二重
放物線軌道および円弧放物線軌道で補間演算し、それら
の軌道上を移動するときの加速度が所定の上限値を越え
ないように補間点を求めるようにしたことにより、ロボ
ットの先端部に大きい加速度が発生して振動が発生する
課題が解決されるとともに、許容経路誤差を教示点列デ
ータに設定して軌道を自動的に設定するようにしたこと
により、ロボットの先端部における衝突などを配慮しな
がら教示点を設定するような従来の教示作業を簡単化で
きる。

【図面の簡単な説明】

【図１】請求項１に係わる本発明のロボットの軌道補間
装置の一実施例の主要部である軌道演算手段の構成を示
すブロック図

【図２】請求項１に係わる本発明のロボットの軌道補間
装置の一実施例における補間方法選択手段の動作を示す
フローチャート

【図３】請求項１に係わる本発明のロボットの軌道補間
装置の一実施例における放物線補間演算手段の動作を示
すフローチャート

【図４】請求項１に係わる本発明のロボットの軌道補間
装置の一実施例における放物線補間演算の処理を示す説
明図

【図５】請求項１に係わる本発明のロボットの軌道補間
装置の一実施例におけるロボットの先端部の位置と、そ
の位置における速度および加速度との関係を示す特性図

【図６】請求項２に係わる本発明のロボットの軌道補間
装置の一実施例の主要部である軌道演算手段の構成を示
すブロック図

【図７】請求項２に係わる本発明のロボットの軌道補間
装置の一実施例における補間方法選択手段の動作を示す
フローチャート

【図８】請求項２に係わる本発明のロボットの軌道補間
装置の一実施例における二重放物線補間演算手段の動作
を示すフローチャート

【図９】請求項２に係わる本発明のロボットの軌道補間
装置の一実施例における二重放物線補間演算を示す説明
図

【図１０】請求項２に係わる本発明のロボットの軌道補
間装置の一実施例におけるロボット先端部の位置と、そ
の位置における速度および加速度との関係を示す特性図

【図１１】請求項３に係わる本発明のロボットの軌道補
間装置の一実施例の主要部である軌道演算手段の構成を
示すブロック図

【図１２】請求項３に係わる本発明のロボットの軌道補
間装置の一実施例における補間方法選択手段の動作を示
すフローチャート

【図１３】請求項３に係わる本発明のロボットの軌道補
間装置の一実施例における放物線補間方法選択手段の動
作を示すフローチャート

【図１４】請求項４に係わる本発明のロボットの軌道補
間装置の一実施例の主要部である軌道演算手段の構成を
示すブロック図

【図１５】請求項４に係わる本発明のロボットの軌道補
間装置の一実施例における補間方法選択手段の動作を示
すフローチャート

【図１６】請求項４に係わる本発明のロボットの軌道補
間装置の一実施例における円弧補間演算手段の動作を示
すフローチャート

【図１７】請求項４に係わる本発明のロボットの軌道補
間装置の一実施例における円弧放物線補間演算を示す説
明図

【図１８】請求項４に係わる本発明のロボットの軌道補
間装置の一実施例におけるロボット先端部の位置と、そ
の位置における速度および加速度との関係を示す特性図

【符号の説明】

１演算処理手段１１補間方法選択手段１２直線補間演算手段１３放物線補間演算手段１５関節角演算手段Ｐ教示点列データＪ間接角データ

Claims

【特許請求の範囲】

【請求項１】ロボットの先端部が移動する軌道の基準
を与える複数個の教示点と、前記教示点に基づく前記軌
道の形状を指定する移動命令と、教示点と軌道との間の
最大誤差を指定する許容経路誤差とを備えた教示点列デ
ータを入力し、前記移動命令と前記許容経路誤差とに基
づいて直線補間演算手段、円弧補間演算手段と放物線補
間演算手段のうちの１つを選んで前記教示点列データを
出力する補間演算方法選択手段と、前記教示点列データ
を入力して教示点間を直線軌道で補間演算する直線補間
演算手段と、前記教示点列データを入力して連続する３
つの教示点を通る円弧軌道で補間演算する円弧補間演算
手段と、前記教示点列データを入力して連続する３つの
教示点間を放物線軌道で補間演算する放物線補間演算手
段と、補間演算された補間点のデータを入力して単位補
間時間ごとの関節角の変化量を演算して出力する関節角
演算手段とを備え、前記補間点に従って移動するロボッ
トの先端部の加速度が所定上限値以下となるようにした
ロボットの軌道補間装置。
【請求項２】ロボットの先端部が移動する軌道の基準
を与える複数個の教示点と、前記教示点に基づく前記軌
道の形状を指定する移動命令と、教示点と軌道との間の
最大誤差を指定する許容経路誤差とを備えた教示点列デ
ータを入力し、前記移動命令と前記許容経路誤差とに基
づいて直線補間演算手段と円弧補間演算手段と二重放物
線補間演算手段のうちの１つを選んで前記教示点列デー
タを出力する補間演算方法選択手段と、前記教示点列デ
ータを入力して教示点間を直線軌道で補間演算する直線
補間演算手段と、前記教示点列データを入力して連続す
る３つの教示点を通る円弧軌道で補間演算する円弧補間
演算手段と、前記教示点列データを入力して連続する３
つの教示点間を２つの放物線軌道を連結して補間演算す
る放物線補間演算手段と、補間演算された補間点のデー
タを入力して単位補間時間ごとの関節角の変化量を演算
して出力する関節角演算手段とを備え、前記補間点に従
って移動するロボットの先端部の加速度が所定上限値以
下となるようにしたロボットの軌道補間装置。
【請求項３】ロボットの先端部が移動する軌道の基準
を与える複数個の教示点と、前記教示点に基づく前記軌
道の形状を指定する移動命令と、教示点と軌道との間の
最大誤差を指定する許容経路誤差とを備えた教示点列デ
ータを入力し、前記移動命令と前記許容経路誤差とに基
づいて直線補間演算手段と円弧補間演算手段と放物線切
換補間演算手段のうちの１つを選んで前記教示点列デー
タを出力する補間演算方法選択手段と、前記教示点列デ
ータを入力して教示点間を直線軌道で補間演算する直線
補間演算手段と、前記教示点列データを入力して連続す
る３つの教示点を通る円弧軌道で補間演算する円弧補間
演算手段と、前記教示点列データを入力して連続する３
つの教示点で構成される折れ線軌道の内角の大きさによ
り放物線補間演算手段と二重放物線補間演算手段のうち
の１つを選択して補間演算する放物線切換補間演算手段
と、補間演算された補間点のデータを入力して単位補間
時間ごとの関節角の変化量を演算して出力する関節角演
算手段とを備え、前記補間点に従って移動するロボット
の先端部の加速度が所定上限値以下となるようにしたロ
ボットの軌道補間装置。
【請求項４】ロボットの先端部が移動する軌道の基準
を与える複数個の教示点と、前記教示点に基づく前記軌
道の形状を指定する移動命令と、教示点と軌道との間の
最大誤差を指定する許容経路誤差とを備えた教示点列デ
ータを入力し、前記移動命令と前記許容経路誤差とに基
づいて直線補間演算手段と円弧補間演算手段と円弧放物
線補間演算手段のうちの１つを選んで前記教示点列デー
タを出力する補間演算方法選択手段と、前記教示点列デ
ータを入力して教示点間を直線軌道で補間演算する直線
補間演算手段と、前記教示点列データを入力して連続す
る３つの教示点を通る円弧軌道で補間演算する円弧補間
演算手段と、前記教示点列データを入力して連続する３
つの教示点間を円弧軌道と放物線軌道を連結して補間演
算する放物線補間演算手段と、補間演算された補間点の
データを入力して単位補間時間ごとの関節角の変化量を
演算して出力する関節角演算手段とを備え、前記補間点
に従って移動するロボットの先端部の加速度が所定上限
値以下となるようにしたロボットの軌道補間装置。
【請求項５】連続する３つの教示点が構成する折れ線
の節点の内角の二等分線上にあって前記節点から許容経
路誤差以内の点を通過して前記折れ線に接する放物線軌
道を生成し、折れ線上を一定速度で移動させた補間点を
前記放物線軌道上に前記節点の内角の二等分線方向に平
行に投影した点を放物線軌道上の補間点として補間演算
し、このときに発生する前記投影方向の加速度の大きさ
は一定であって、その大きさが所定値以下となるように
前記折れ線上の移動速度を設定して補間演算する放物線
補間演算手段を備えた請求項１または請求項３記載のロ
ボットの軌道補間装置。
【請求項６】連続する３つの教示点が構成する折れ線
の節点の内角の二等分線上にあって前記節点から許容経
路誤差以内の点を通過して前記二等分線に直交する直線
の補助軌道を設け、前記折れ線の一部と前記補助軌道と
で構成される２つの折れ線軌道をそれぞれを放物線軌道
で補間し、各放物線軌道において、折れ線上を一定速度
で移動させた補間点をその放物線が対応する内角の二等
分線方向に平行に放物線に投影した点を放物線軌道上の
補間点として補間演算し、このときに発生する前記投影
方向の加速度の大きさは一定であって、その大きさが所
定値以下となるように前記折れ線上の移動速度を設定し
て補間演算する二重放物線補間演算手段を備えた請求項
２または請求項３記載のロボットの軌道補間装置。
【請求項７】連続する３つの教示点が構成する折れ線
の節点の内角の二等分線上にあって前記節点から許容経
路誤差以内の点を通過して前記折れ線に接する円弧を設
定し、前記円弧のうち前記節点に対向する部分円弧を円
弧軌道とし、前記円弧軌道の両端をそれぞれ対向する前
記折れ線の線分と放物線により滑らかに接続して円弧放
物線軌道を構成し、前記放物線はそれぞれ円弧軌道の端
を頂点とする放物線であって、折れ線上を一定の速度で
補間した点を、円弧と放物線との交点と円弧の中心とを
結ぶ方向に平行な方向に投影した点を放物線上の補間点
とするとともに、円弧との接続点における速度が円弧上
の速度と一致するように前記一定速度を設定し、放物線
軌道上の加速度の大きさおよび円弧軌道上の加速度の大
きさが所定の上限値を越えないように補間演算する円弧
放物線補間演算手段を備えた請求項４記載のロボットの
軌道補間装置。
【請求項８】教示点列データの移動命令が円弧移動の
命令であるときは前記教示点列データを円弧補間演算手
段に出力し、教示点列データの移動命令が円弧補間の命
令でなくて許容誤差が０であるときには前記教示点列デ
ータを直線補間演算手段に出力し、移動命令が円弧移動
の命令でなくて許容誤差が０でないときには前記教示点
列データを他の補間演算手段に出力するようにした補間
方法選択手段を備えた請求項１ないし請求項７記載のい
づれかに記載のロボットの軌道補間装置。