JP3341305B2 - 加減速パターン生成装置及び加減速パターン生成方法、並びにこれに用いる逆運動学問題の解法と時間軸補正方法 - Google Patents
加減速パターン生成装置及び加減速パターン生成方法、並びにこれに用いる逆運動学問題の解法と時間軸補正方法Info
- Publication number
- JP3341305B2 JP3341305B2 JP19470192A JP19470192A JP3341305B2 JP 3341305 B2 JP3341305 B2 JP 3341305B2 JP 19470192 A JP19470192 A JP 19470192A JP 19470192 A JP19470192 A JP 19470192A JP 3341305 B2 JP3341305 B2 JP 3341305B2
- Authority
- JP
- Japan
- Prior art keywords
- acceleration
- curve
- time
- deceleration
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【0001】
【産業上の利用分野】本発明は、ロボット構造に依存す
ることのない汎用的なアルゴリズムにより動作制御の統
一化を図ることができる新規な加減速パターン生成装置
及び加減速パターン生成方法と、演算処理上の負担を軽
減するための逆運動学問題の解法及び時間軸補正方法を
提供しようとするものである。
ることのない汎用的なアルゴリズムにより動作制御の統
一化を図ることができる新規な加減速パターン生成装置
及び加減速パターン生成方法と、演算処理上の負担を軽
減するための逆運動学問題の解法及び時間軸補正方法を
提供しようとするものである。
【0002】
【従来の技術】ロボットに対する作業の教示に際して、
経路の端点だけを指定するか、あるいは所望の経路を指
定するかによってロボットの動作を分けたときに、PT
P(Poit To Poit)動作(又は制御)とC
P(Continuous Path)動作(又は制
御)とに分類することができる。
経路の端点だけを指定するか、あるいは所望の経路を指
定するかによってロボットの動作を分けたときに、PT
P(Poit To Poit)動作(又は制御)とC
P(Continuous Path)動作(又は制
御)とに分類することができる。
【0003】前者のPTP動作にあっては、ロボットの
移動経路上の重要な点のみを指定し、その点間の経路が
不問とされるのに対し、後者のCP動作ではロボットの
移動経路(以下、「CP経路」という。)を指定すると
ともに、この経路に沿ってロボットの手先位置を如何に
正確にトレースさせるかが問題になる。
移動経路上の重要な点のみを指定し、その点間の経路が
不問とされるのに対し、後者のCP動作ではロボットの
移動経路(以下、「CP経路」という。)を指定すると
ともに、この経路に沿ってロボットの手先位置を如何に
正確にトレースさせるかが問題になる。
【0004】また、点と点との間を直線や円弧で補間す
る機能をPTP動作に付加することによってロボットの
連続的な運動の再生が可能となっている。
る機能をPTP動作に付加することによってロボットの
連続的な運動の再生が可能となっている。
【0005】
【発明が解決しようとする課題】ところで、これまでの
CP制御のアルゴリズムは、ロボットの構造に関する依
存度が高いため、CP制御に係るソフトウェア資産の汎
用化や、標準化を図る上で支障を来すという問題があ
る。
CP制御のアルゴリズムは、ロボットの構造に関する依
存度が高いため、CP制御に係るソフトウェア資産の汎
用化や、標準化を図る上で支障を来すという問題があ
る。
【0006】例えば、直交型ロボットのCP制御に係る
ソフトウェアを、そのままの形、或いは、一部少数の変
更を加えるだけで垂直多関節型ロボット等の、他の構造
を有するロボットに適用することが困難であり、CP制
御のソフトウェアに係るシステム設計をロボット毎に個
別的に行わなければならない。
ソフトウェアを、そのままの形、或いは、一部少数の変
更を加えるだけで垂直多関節型ロボット等の、他の構造
を有するロボットに適用することが困難であり、CP制
御のソフトウェアに係るシステム設計をロボット毎に個
別的に行わなければならない。
【0007】よって、ロボットの性能が設計者の経験や
能力等によって左右され易いとか、また、仮に良好な成
果が得られたとしても、これを種類の異なるロボットに
適用する場合にそれが最良の方法であると確信すること
ができないといった不都合がある。
能力等によって左右され易いとか、また、仮に良好な成
果が得られたとしても、これを種類の異なるロボットに
適用する場合にそれが最良の方法であると確信すること
ができないといった不都合がある。
【0008】また、CP制御において従来から直線補間
時によく用いられている台形波状の加減速曲線(つま
り、その立ち上がり時に速度ゼロから所定の勾配をもっ
て直線的な速度の上昇をみせた後、一定速の定速区間を
経て減速時に所定の勾配をもって直線的に速度が減少し
てゼロになるような加減速パターンに対応する。)を採
用する場合において、加速度が不連続となる時点、例え
ば、加速の開始時点や減速の終了時点、加速期間から定
速期間への移行や定速期間から減速期間への移行の際の
速度変化点において衝撃が発生しやすいという問題があ
る。
時によく用いられている台形波状の加減速曲線(つま
り、その立ち上がり時に速度ゼロから所定の勾配をもっ
て直線的な速度の上昇をみせた後、一定速の定速区間を
経て減速時に所定の勾配をもって直線的に速度が減少し
てゼロになるような加減速パターンに対応する。)を採
用する場合において、加速度が不連続となる時点、例え
ば、加速の開始時点や減速の終了時点、加速期間から定
速期間への移行や定速期間から減速期間への移行の際の
速度変化点において衝撃が発生しやすいという問題があ
る。
【0009】この問題は加減速曲線に係る単なる選択の
問題ではなく、与えられたCP経路に対してロボットの
能力を如何にして最大限に引出して、タクトタイムの短
縮化を図ることができるかという問題にも関わってい
る。
問題ではなく、与えられたCP経路に対してロボットの
能力を如何にして最大限に引出して、タクトタイムの短
縮化を図ることができるかという問題にも関わってい
る。
【0010】つまり、CP制御中に不要な衝撃が生じて
動作がぎこちなくならないようにするためには、加減速
曲線をできるだけ滑らかな曲線にする必要があるという
認識を持っていても、これをロボットの個別的な要因に
依存することなく満足させるための加減速パターンを如
何にして生成するかといった方法論が確立されていない
ために、適正な加減速制御を行うために数々の試行錯誤
的要因が徘徊することになる。
動作がぎこちなくならないようにするためには、加減速
曲線をできるだけ滑らかな曲線にする必要があるという
認識を持っていても、これをロボットの個別的な要因に
依存することなく満足させるための加減速パターンを如
何にして生成するかといった方法論が確立されていない
ために、適正な加減速制御を行うために数々の試行錯誤
的要因が徘徊することになる。
【0011】このことはまたCP経路の接続にあたって
も問題となり、例えば、ある区間での直線補間から後続
区間での円弧補間へと移行させる際等において、衝撃を
伴うことなく移行を円滑に行う必要がある。
も問題となり、例えば、ある区間での直線補間から後続
区間での円弧補間へと移行させる際等において、衝撃を
伴うことなく移行を円滑に行う必要がある。
【0012】そして、CP制御は、一般に相当の計算量
を要する(例えば、ロボットの手先位置から関節状態を
求める逆運動学問題を解く場合)ため、計算時間の短縮
化を図り、実用上の支障を来さないようにすること、か
といって、求められている精度をないがしろにすること
なくこれを保証することも重要な課題となる。
を要する(例えば、ロボットの手先位置から関節状態を
求める逆運動学問題を解く場合)ため、計算時間の短縮
化を図り、実用上の支障を来さないようにすること、か
といって、求められている精度をないがしろにすること
なくこれを保証することも重要な課題となる。
【0013】さらに、従来の制御システムでは、PTP
制御とCP制御とを統一的に取り扱うアルゴリズムが確
立されていないため、各制御に係る別個の処理ルーチン
を必要とするといった問題がある。
制御とCP制御とを統一的に取り扱うアルゴリズムが確
立されていないため、各制御に係る別個の処理ルーチン
を必要とするといった問題がある。
【0014】
【課題を解決するための手段】そこで、上記した課題を
解決するために、本発明加減速パターン生成装置は、作
用点に対する制御の開始点と終了点及び/又は制御曲線
に関する指令を発する指令手段と、指令手段からの信号
に応じて制御曲線上での速度ピーク値の仮値を決定する
速度ピーク仮値決定手段と、速度ピーク値の仮値に基づ
いて加減速パターンを生成するとともに制御曲線を多数
の微小な長さのセグメントに分割して線素列を得る加減
速パターン生成/線素列計算手段と、順運動学問題を解
く際に座標系間の変換に使用される同次変換行列に対応
した演算要素を含むネガティブフィードバックのフィル
ター構成とされ、作用点に対する指令位置を与えてこれ
に対応する関節軸の状態を導き出す逆運動学問題解析手
段と、速度ピーク値の仮値に基づいて逆運動学問題解析
手段により得られる関節状態についての解をソフトウェ
ア処理によって構成される仮想サーボ系、つまり、現実
のサーボ系に対応する模擬的なサーボ系に与えることに
よってこれを計算手段として用いて制御精度を求めると
ともに、この制御精度が規定又は指定の精度に一致する
ように速度ピーク値の最適値を決定する最適速度ピーク
値決定手段と、速度ピーク値の最適値に基づいて関節状
態に係る再計算又は修正計算を行う補正手段とを備えた
ものである。
解決するために、本発明加減速パターン生成装置は、作
用点に対する制御の開始点と終了点及び/又は制御曲線
に関する指令を発する指令手段と、指令手段からの信号
に応じて制御曲線上での速度ピーク値の仮値を決定する
速度ピーク仮値決定手段と、速度ピーク値の仮値に基づ
いて加減速パターンを生成するとともに制御曲線を多数
の微小な長さのセグメントに分割して線素列を得る加減
速パターン生成/線素列計算手段と、順運動学問題を解
く際に座標系間の変換に使用される同次変換行列に対応
した演算要素を含むネガティブフィードバックのフィル
ター構成とされ、作用点に対する指令位置を与えてこれ
に対応する関節軸の状態を導き出す逆運動学問題解析手
段と、速度ピーク値の仮値に基づいて逆運動学問題解析
手段により得られる関節状態についての解をソフトウェ
ア処理によって構成される仮想サーボ系、つまり、現実
のサーボ系に対応する模擬的なサーボ系に与えることに
よってこれを計算手段として用いて制御精度を求めると
ともに、この制御精度が規定又は指定の精度に一致する
ように速度ピーク値の最適値を決定する最適速度ピーク
値決定手段と、速度ピーク値の最適値に基づいて関節状
態に係る再計算又は修正計算を行う補正手段とを備えた
ものである。
【0015】そして、この加減速パターン生成装置にお
いて、作用点位置から関節状態を求める逆運動学問題解
析手段は、関節状態から作用点の位置を求める順運動学
問題を解く際に使用される同次変換行列に対応した演算
要素と、動的なゲイン調整のための乗数要素と、ループ
安定化のための積分要素と、ループの収束速度を調整す
るのための定数要素とを含むネガティブフィードバック
のフィルター構成を有し、逆運動学問題解析手段への位
置情報の入力に対して、演算要素の前段から関節状態の
解を得ることができるように構成される。
いて、作用点位置から関節状態を求める逆運動学問題解
析手段は、関節状態から作用点の位置を求める順運動学
問題を解く際に使用される同次変換行列に対応した演算
要素と、動的なゲイン調整のための乗数要素と、ループ
安定化のための積分要素と、ループの収束速度を調整す
るのための定数要素とを含むネガティブフィードバック
のフィルター構成を有し、逆運動学問題解析手段への位
置情報の入力に対して、演算要素の前段から関節状態の
解を得ることができるように構成される。
【0016】また、加減速パターン生成装置において、
加減速パターン生成/線素列計算手段は、制御曲線の曲
線長を計算する曲線長計算手段と、加減速曲線における
速度のピーク時点を曲線長に応じて決定する速度ピーク
時点決定手段と、速度のピーク時点に関する指令を受け
て関節軸に係る加速パターンを生成するとともに、減速
パターンの生成に際しては加速パターンに対して時間的
な対称操作を施すことによって時間的に折り返されたパ
ターンとして減速パターンを生成するパターン生成手段
と、加減速曲線の形状に応じて制御曲線を所定の時間刻
みに分割することによって線素列を計算する線素列計算
手段とを具備する。
加減速パターン生成/線素列計算手段は、制御曲線の曲
線長を計算する曲線長計算手段と、加減速曲線における
速度のピーク時点を曲線長に応じて決定する速度ピーク
時点決定手段と、速度のピーク時点に関する指令を受け
て関節軸に係る加速パターンを生成するとともに、減速
パターンの生成に際しては加速パターンに対して時間的
な対称操作を施すことによって時間的に折り返されたパ
ターンとして減速パターンを生成するパターン生成手段
と、加減速曲線の形状に応じて制御曲線を所定の時間刻
みに分割することによって線素列を計算する線素列計算
手段とを具備する。
【0017】このような加減速パターン生成装置におい
て、作用点を指定された制御曲線に沿って移動制御させ
るべく加減速パターンを生成するに際しては次のような
加減速パターン生成方法を採る。
て、作用点を指定された制御曲線に沿って移動制御させ
るべく加減速パターンを生成するに際しては次のような
加減速パターン生成方法を採る。
【0018】先ず、制御曲線に係る計算時間の最小単位
を決めた後、制御曲線の曲線長を求めて、該曲線長に対
応した速度のピーク時点を決定して加速パターンと減速
パターンとが時間的な対称性を有する加減速パターンを
生成するとともに、速度ピーク値の仮値をメモリー容量
の許容値に基づいて決定して制御曲線を計算時間の単位
時間間隔でもって分割することによって線素列を導出す
る。
を決めた後、制御曲線の曲線長を求めて、該曲線長に対
応した速度のピーク時点を決定して加速パターンと減速
パターンとが時間的な対称性を有する加減速パターンを
生成するとともに、速度ピーク値の仮値をメモリー容量
の許容値に基づいて決定して制御曲線を計算時間の単位
時間間隔でもって分割することによって線素列を導出す
る。
【0019】そして、この線素列と作用点の位置指令に
係る情報に基づいて、順運動学問題を解く際に座標系間
の変換に使用される同次変換行列に対応した演算要素を
含むネガティブフィードバックのフィルター計算を行う
ことにより、作用点の位置を与えてこれに対応した関節
軸の状態を求める逆運動学問題を解く。その解をソフト
ウェア処理によって現実のサーボ系を模して構成される
仮想サーボ系に与えることによって制御精度を求めると
ともにこれと規定(デフォルト)又は指定の精度との大
小関係に基づいて速度ピーク値の最適値を決定する。
係る情報に基づいて、順運動学問題を解く際に座標系間
の変換に使用される同次変換行列に対応した演算要素を
含むネガティブフィードバックのフィルター計算を行う
ことにより、作用点の位置を与えてこれに対応した関節
軸の状態を求める逆運動学問題を解く。その解をソフト
ウェア処理によって現実のサーボ系を模して構成される
仮想サーボ系に与えることによって制御精度を求めると
ともにこれと規定(デフォルト)又は指定の精度との大
小関係に基づいて速度ピーク値の最適値を決定する。
【0020】それから、速度ピーク値の最適値に基づい
て関節状態の解に係る再計算又はこれに相当する修正計
算を行った結果をサーボ系への制御指令として送出す
る。
て関節状態の解に係る再計算又はこれに相当する修正計
算を行った結果をサーボ系への制御指令として送出す
る。
【0021】さらに逆運動学問題の解法としては、関節
軸の状態に対応した作用点の位置を求める順運動学問題
の解法にあたって使用される同次変換行列に対応した演
算要素と、動的なゲイン調整のための乗数要素と、ルー
プ安定化のための積分要素と、ループの収束速度を調整
するための定数要素とを含むネガティブフィードバック
のフィルターを構成することが前提となる。
軸の状態に対応した作用点の位置を求める順運動学問題
の解法にあたって使用される同次変換行列に対応した演
算要素と、動的なゲイン調整のための乗数要素と、ルー
プ安定化のための積分要素と、ループの収束速度を調整
するための定数要素とを含むネガティブフィードバック
のフィルターを構成することが前提となる。
【0022】そして、制御曲線を最小計算時間間隔をも
って分割することによって得られる線素列と作用点の位
置情報を上記フィルターへの入力情報として与えてフィ
ルターに起動をかけ、ループでの収束を待って演算要素
の前段から関節軸の状態を求める。
って分割することによって得られる線素列と作用点の位
置情報を上記フィルターへの入力情報として与えてフィ
ルターに起動をかけ、ループでの収束を待って演算要素
の前段から関節軸の状態を求める。
【0023】このような手順を各関節軸に対して繰り返
すことによって最終的な逆運動学問題の解としての関節
状態を求める。
すことによって最終的な逆運動学問題の解としての関節
状態を求める。
【0024】ところで、計算時間が最小単位をもって稼
動される制御システムにおいては、制御曲線を連続直線
として取り扱うことができず、実際の計算では有限差分
近似による離散化を伴うことになるが、作用点の軌跡と
指令された制御曲線との間で高い近似精度を保証するた
めに次のような時間軸補正方法を採る。
動される制御システムにおいては、制御曲線を連続直線
として取り扱うことができず、実際の計算では有限差分
近似による離散化を伴うことになるが、作用点の軌跡と
指令された制御曲線との間で高い近似精度を保証するた
めに次のような時間軸補正方法を採る。
【0025】先ず、加減速曲線において速度のピーク時
点や総移動時間が、計算時間の最小単位の整数倍の値と
なるように速度ピーク値や加速及び減速期間での移動距
離に修正を施して計算時間の最小単位をもって関節状態
を示すデータ列を標本化した後最小単位時間の間隔をも
って該データを保持する。
点や総移動時間が、計算時間の最小単位の整数倍の値と
なるように速度ピーク値や加速及び減速期間での移動距
離に修正を施して計算時間の最小単位をもって関節状態
を示すデータ列を標本化した後最小単位時間の間隔をも
って該データを保持する。
【0026】それから制御曲線を計算時間の最小単位の
時間間隔でもって分割して得られる線素列の長さと、加
減速曲線の形状との間に成立する距離の関係に基づく計
算によって関節状態を示すデータ列に対応した新たな時
間列を計算時間の最小単位にとらわれることなく生成す
る。
時間間隔でもって分割して得られる線素列の長さと、加
減速曲線の形状との間に成立する距離の関係に基づく計
算によって関節状態を示すデータ列に対応した新たな時
間列を計算時間の最小単位にとらわれることなく生成す
る。
【0027】そして、新たな時間列と関節状態を示すデ
ータ列の組に対して、計算時間の最小単位による再標本
化を施すことによって時間的に等間隔に配置された関節
状態の情報を得る。
ータ列の組に対して、計算時間の最小単位による再標本
化を施すことによって時間的に等間隔に配置された関節
状態の情報を得る。
【0028】
【作用】本発明によれば、CP制御において基本的に制
御曲線の長さに応じた速度ピーク時点が規定される加減
速パターンを生成するとともに、制御曲線に係る線素列
を算出してこれと制御曲線上の位置指令に基づいて逆運
動学問題の解としての関節状態を知ることができるの
で、ロボット系を構成するアーム等の組み合わせが如何
なるものになろうと各関節軸を同等に取り扱うことがで
き、ロボットの構造上の違いに起因して加減速パターン
生成に係るアルゴリズムに大幅な変更を余儀なくされる
といった不都合が解消される。
御曲線の長さに応じた速度ピーク時点が規定される加減
速パターンを生成するとともに、制御曲線に係る線素列
を算出してこれと制御曲線上の位置指令に基づいて逆運
動学問題の解としての関節状態を知ることができるの
で、ロボット系を構成するアーム等の組み合わせが如何
なるものになろうと各関節軸を同等に取り扱うことがで
き、ロボットの構造上の違いに起因して加減速パターン
生成に係るアルゴリズムに大幅な変更を余儀なくされる
といった不都合が解消される。
【0029】そして、本発明では逆運動学問題の解析に
あたって、順運動学問題の解法に係る演算要素を利用す
ることでこれにロボット構造を反映させ、該演算要素を
含むネガティブフィードバックフィルターの計算処理を
通して逆運動学問題解を求めることができるので、アル
ゴリズムが理論的に明確であり、この点について設計者
の能力や経験等が割り込む余地(例えば、逆運動学問題
を事前に数学的に解く方法として幾何学的な方法を採る
かあるいは代数学的な方法を採るかの選択基準等)が非
常に少なくなるので加減速曲線の生成に係るアルゴリム
の汎用性が高く、また、計算処理を四則演算及び三角関
数計算程度で行うことができるため計算時間の短縮化や
CPUの処理負担の軽減を図ることができる。
あたって、順運動学問題の解法に係る演算要素を利用す
ることでこれにロボット構造を反映させ、該演算要素を
含むネガティブフィードバックフィルターの計算処理を
通して逆運動学問題解を求めることができるので、アル
ゴリズムが理論的に明確であり、この点について設計者
の能力や経験等が割り込む余地(例えば、逆運動学問題
を事前に数学的に解く方法として幾何学的な方法を採る
かあるいは代数学的な方法を採るかの選択基準等)が非
常に少なくなるので加減速曲線の生成に係るアルゴリム
の汎用性が高く、また、計算処理を四則演算及び三角関
数計算程度で行うことができるため計算時間の短縮化や
CPUの処理負担の軽減を図ることができる。
【0030】また、本発明では、加減速パターンの生成
方法において、作用点を指定された制御曲線に沿って精
度良く移動させるために、標本化に際して時間軸補正を
行っており、これによって加減速曲線の生成や速度制御
等に要する計算処理上の負担を減らして処理の高速化に
答えることができる他、この時間軸補正を介在させるこ
とによってCP制御のアルゴリズムの基本部分には何等
変更を加えることなく単にCP制御の曲線長に係る変数
から軸移動量に係る変数への類推的な置換を行うことに
よって容易にPTP制御を実現することができ、両制御
に係る統一化された処理システムを構築することができ
る。
方法において、作用点を指定された制御曲線に沿って精
度良く移動させるために、標本化に際して時間軸補正を
行っており、これによって加減速曲線の生成や速度制御
等に要する計算処理上の負担を減らして処理の高速化に
答えることができる他、この時間軸補正を介在させるこ
とによってCP制御のアルゴリズムの基本部分には何等
変更を加えることなく単にCP制御の曲線長に係る変数
から軸移動量に係る変数への類推的な置換を行うことに
よって容易にPTP制御を実現することができ、両制御
に係る統一化された処理システムを構築することができ
る。
【0031】
【実施例】以下に、本発明加減速パターン生成装置及び
加減速パターン生成方法、並びにこれに用いる逆運動学
問題の解法と時間軸補正方法について詳細に説明する。
加減速パターン生成方法、並びにこれに用いる逆運動学
問題の解法と時間軸補正方法について詳細に説明する。
【0032】本発明の基本的な考え方には、加減速曲線
の生成に係るアルゴリズムと、ロボットの手先位置から
アーム系の状態に関する解を求めるアルゴリズムとが密
接に関連しており、これらを基礎としてさらに進んだ議
論が展開されることになるので、以下では、装置の構成
を説明する前にこれらの基本事項について順次説明して
いくことにする。
の生成に係るアルゴリズムと、ロボットの手先位置から
アーム系の状態に関する解を求めるアルゴリズムとが密
接に関連しており、これらを基礎としてさらに進んだ議
論が展開されることになるので、以下では、装置の構成
を説明する前にこれらの基本事項について順次説明して
いくことにする。
【0033】先ず、加減速パターンの生成に係るアルゴ
リズムを図3乃至図11に従って説明する。
リズムを図3乃至図11に従って説明する。
【0034】図3に示す制御曲線a(以下、「CP曲
線」という。)はロボットの作用点が始点Sから終点E
にかけて描く軌跡を示すものである。
線」という。)はロボットの作用点が始点Sから終点E
にかけて描く軌跡を示すものである。
【0035】CP曲線a上の任意の点をPiとし、ロボ
ットの基軸又は地面に固定した直交座標系での表現をP
i(xi,yi,zi)(i=0、1、2・・・、N
(自然数)であり、i=0の時に始点Sに一致し、i=
Nの時に終点Eに一致する)と定義する。
ットの基軸又は地面に固定した直交座標系での表現をP
i(xi,yi,zi)(i=0、1、2・・・、N
(自然数)であり、i=0の時に始点Sに一致し、i=
Nの時に終点Eに一致する)と定義する。
【0036】また、点Piと始点Sとの間のCP曲線a
の長さを「l」とし、CP曲線aの全長を「L」とす
る。
の長さを「l」とし、CP曲線aの全長を「L」とす
る。
【0037】そして、CP曲線aをN個の線素に分割し
た場合のn番目の線素の長さを「Δln」とすると、C
P曲線aを線素列{Δl1,Δl2,Δl3,・・・,
ΔlN}(以下、「{Δln}」と略記す。)に対応付
けることができる。
た場合のn番目の線素の長さを「Δln」とすると、C
P曲線aを線素列{Δl1,Δl2,Δl3,・・・,
ΔlN}(以下、「{Δln}」と略記す。)に対応付
けることができる。
【0038】尚、この写像において、線素列が分割セグ
メントの長さだけを情報とすることから明らかなよう
に、線素列がCP曲線aの形状的特徴までをも代表する
ものではないことには注意を要する(つまり、曲線から
線素列への写像が上への1対1写像になっていないこと
は、始点Sと終点Eとを通る直線に関してCP曲線aに
線対象操作を施した曲線についても同様の線素分解によ
り同じ系列の線素列が得られることから明かであ
る。)。
メントの長さだけを情報とすることから明らかなよう
に、線素列がCP曲線aの形状的特徴までをも代表する
ものではないことには注意を要する(つまり、曲線から
線素列への写像が上への1対1写像になっていないこと
は、始点Sと終点Eとを通る直線に関してCP曲線aに
線対象操作を施した曲線についても同様の線素分解によ
り同じ系列の線素列が得られることから明かであ
る。)。
【0039】また、時間軸を表すパラメーターを「t」
とし、サンプリング(標本化)時間を「Ts」、加減速
曲線において、加速開始時点から速度がピーク値に達す
るまでに要する時間(以下、「速度ピーク到達時間」と
いう。)を「Tp」とする。以上の諸量の定義を下記の
[表1]に示す。
とし、サンプリング(標本化)時間を「Ts」、加減速
曲線において、加速開始時点から速度がピーク値に達す
るまでに要する時間(以下、「速度ピーク到達時間」と
いう。)を「Tp」とする。以上の諸量の定義を下記の
[表1]に示す。
【0040】
【表1】
【0041】尚、距離lは関数L(xi,yi,zi)
を用いて表現することができるものとする。
を用いて表現することができるものとする。
【0042】図4及び図5は加減速曲線を概略的に示す
グラフ図(横軸に時間軸をとり、縦軸に速度(V=dl
/dt)をとっている。)であり、その形状的特徴は、
加速期間における曲線形状と、減速期間における曲線形
状との間に時間的な対称性が認められる点にある。
グラフ図(横軸に時間軸をとり、縦軸に速度(V=dl
/dt)をとっている。)であり、その形状的特徴は、
加速期間における曲線形状と、減速期間における曲線形
状との間に時間的な対称性が認められる点にある。
【0043】つまり、図4に示す加減速曲線1は、速度
がそのピーク値(これを「Vp」と記す。)に到達する
までの加速期間における加速パターン部2、定速期間に
おける定速パターン部3、速度Vpから速度がゼロにな
るまでの減速パターン部4からなり、加速期間=減速期
間=Tpという関係が成り立つとともに、加速パターン
部2に時間的な折り返し操作を施すことによってこれを
減速パターン部4に重ね合わせることができる。
がそのピーク値(これを「Vp」と記す。)に到達する
までの加速期間における加速パターン部2、定速期間に
おける定速パターン部3、速度Vpから速度がゼロにな
るまでの減速パターン部4からなり、加速期間=減速期
間=Tpという関係が成り立つとともに、加速パターン
部2に時間的な折り返し操作を施すことによってこれを
減速パターン部4に重ね合わせることができる。
【0044】より詳しくは、加速パターン部2をt=T
pに関して線対称操作によって折り返した後で時間軸方
向に平行移動を施すことによって減速パターン部4に一
致させることができるということである。
pに関して線対称操作によって折り返した後で時間軸方
向に平行移動を施すことによって減速パターン部4に一
致させることができるということである。
【0045】また、図5に示す加減速曲線5は、定速期
間を含まず加速パターン部6と減速パターン部7とから
なり、t=Tpに関する明瞭な線対称性を有する。
間を含まず加速パターン部6と減速パターン部7とから
なり、t=Tpに関する明瞭な線対称性を有する。
【0046】加減速曲線1、5と時間軸とによって囲ま
れる面積が上記した曲線の全長Lに相当することから、
図4の加減速曲線1は曲線長Lがある程度長い場合にお
いて生成される加減速曲線であり、図5の加減速曲線5
は曲線長Lが短い場合において生成される加減速曲線で
あることが分かる。
れる面積が上記した曲線の全長Lに相当することから、
図4の加減速曲線1は曲線長Lがある程度長い場合にお
いて生成される加減速曲線であり、図5の加減速曲線5
は曲線長Lが短い場合において生成される加減速曲線で
あることが分かる。
【0047】図6は、このような加減速曲線を得るにあ
たって速度ピーク到達時間Tpと線素列を得る際の手順
を示すフローチャート図である。
たって速度ピーク到達時間Tpと線素列を得る際の手順
を示すフローチャート図である。
【0048】先ず、手順1)において曲線の全長Lを求
める。
める。
【0049】この場合、CP曲線は、ロボットシステム
側で予め用意されるシステム固有の曲線と、ユーザーが
指定するユーザー指定曲線とに分けられる。
側で予め用意されるシステム固有の曲線と、ユーザーが
指定するユーザー指定曲線とに分けられる。
【0050】前者は、直線補間や円弧補間等のために必
要とされ、これは、3次元空間内の直線や円弧等の基本
的な形状である場合が多いため、その全長Lを容易に求
めることができる。 後者は、ユーザーがティーチング
ペンダント等の教示手段によって曲線を代表する複数の
点の座標から隣接点間を直線とみなすことで近似的に求
めることができる。
要とされ、これは、3次元空間内の直線や円弧等の基本
的な形状である場合が多いため、その全長Lを容易に求
めることができる。 後者は、ユーザーがティーチング
ペンダント等の教示手段によって曲線を代表する複数の
点の座標から隣接点間を直線とみなすことで近似的に求
めることができる。
【0051】即ち、ユーザーが指定するn−1番目の点
とn番目の点の座標を、それぞれ(Xn−1,Yn−
1,Zn−1)、(Xn,Yn,Zn)とすると、全長
Lは線素列の長さの和として下式に示すようして求めら
れる。
とn番目の点の座標を、それぞれ(Xn−1,Yn−
1,Zn−1)、(Xn,Yn,Zn)とすると、全長
Lは線素列の長さの和として下式に示すようして求めら
れる。
【0052】
【数1】
【0053】尚、このようなボリゴン的な近似は、説明
を簡単にするために用いたものであり、要求される精度
に応じて点間の補間にスプライン近似法を適宜に用いる
ことができる。
を簡単にするために用いたものであり、要求される精度
に応じて点間の補間にスプライン近似法を適宜に用いる
ことができる。
【0054】次に手順2)に示すように、全長Lに対応
する速度ピーク到達時間Tpを決定する。
する速度ピーク到達時間Tpを決定する。
【0055】このためには、図7に示すように曲線長L
と速度ピーク到達時間Tpとの関係を予め規定しておく
必要がある。
と速度ピーク到達時間Tpとの関係を予め規定しておく
必要がある。
【0056】図7(a)は横軸に曲線長Lをとり、縦軸
に速度ピーク到達時間をとって両者の関係を表したもの
である。
に速度ピーク到達時間をとって両者の関係を表したもの
である。
【0057】図示するようにTpの値には下限Tpmi
n、上限Tpmaxが存在する。つまり、その下限はサ
ーボ制御の応答が有限であることに起因して存在し、サ
ーボ系の時定数等によって規定され、また、その上限は
サーボモータの最高速度が有限であることに起因して存
在する。
n、上限Tpmaxが存在する。つまり、その下限はサ
ーボ制御の応答が有限であることに起因して存在し、サ
ーボ系の時定数等によって規定され、また、その上限は
サーボモータの最高速度が有限であることに起因して存
在する。
【0058】また、図7(b)は、横軸に曲線長Lをと
り、縦軸に移動時間(これを、「Tm」と記す。)をと
って、両者の関係を表したものであり、折れ線グラフ8
上の点Pmin(Lmin,2・Tpmin)と点Pm
ax(Lmax,2・Tpmax)で一次微分が不連続
となることから分かるように、加減速パターンに関して
3つのモード分けを行うことができる。
り、縦軸に移動時間(これを、「Tm」と記す。)をと
って、両者の関係を表したものであり、折れ線グラフ8
上の点Pmin(Lmin,2・Tpmin)と点Pm
ax(Lmax,2・Tpmax)で一次微分が不連続
となることから分かるように、加減速パターンに関して
3つのモード分けを行うことができる。
【0059】図8乃至図10は各モードにおける加減速
曲線の挙動をグラフ化して示すものである。
曲線の挙動をグラフ化して示すものである。
【0060】0≦L≦Lminの範囲に相当する第1モ
ードでは、移動時間がLに依らずTm=2・Tpmin
と一定であるため、加減速曲線は、図8に9、9、・・
・に示すように幅が一定でLが大きくなるにつれて山の
高さが高くなる。
ードでは、移動時間がLに依らずTm=2・Tpmin
と一定であるため、加減速曲線は、図8に9、9、・・
・に示すように幅が一定でLが大きくなるにつれて山の
高さが高くなる。
【0061】そして、Lmin≦L≦Lmaxの範囲に
相当する第2モードでは、移動時間がLの増加とともに
移動時間Tm(=2・Tp)が増加するため、加減速曲
線10、10、・・・は、図9に示すように、山の高さ
と幅がLの増加に従って増加していく。
相当する第2モードでは、移動時間がLの増加とともに
移動時間Tm(=2・Tp)が増加するため、加減速曲
線10、10、・・・は、図9に示すように、山の高さ
と幅がLの増加に従って増加していく。
【0062】Lmax≦Lの範囲に相当する第3モード
では、Tpの値が頭打ちとなるため、移動時間Tmは定
速期間の長さに応じて増加することになり、加減速曲線
は、図10の11、11、・・・に示すように、加速期
間と減速期間は不変のままで定速期間がLの増加に従っ
て増加する。尚、図7(b)においてLに対するTmの
変化率(つまり、Lmax≦Lの範囲におけるグラフの
傾斜)は、サーボモータの最大速度(これを「Vpma
x」とする。)に逆比例する。
では、Tpの値が頭打ちとなるため、移動時間Tmは定
速期間の長さに応じて増加することになり、加減速曲線
は、図10の11、11、・・・に示すように、加速期
間と減速期間は不変のままで定速期間がLの増加に従っ
て増加する。尚、図7(b)においてLに対するTmの
変化率(つまり、Lmax≦Lの範囲におけるグラフの
傾斜)は、サーボモータの最大速度(これを「Vpma
x」とする。)に逆比例する。
【0063】このようにL−Tm特性において、Tmが
Tpと密接な関係を持っていることから、前の手順1)
によって得られた曲線長Lに対応したTp値をモード毎
に算出することができる。
Tpと密接な関係を持っていることから、前の手順1)
によって得られた曲線長Lに対応したTp値をモード毎
に算出することができる。
【0064】手順3)は各モードに対応した加減速曲線
の生成に係る処理を示しており、これには本願出願人が
既に特開平3−147103号あるいは特願平2−28
3867号において示した方法を踏襲することができ
る。
の生成に係る処理を示しており、これには本願出願人が
既に特開平3−147103号あるいは特願平2−28
3867号において示した方法を踏襲することができ
る。
【0065】ここでは、その要点のみを説明すると、加
減速曲線の形状に関する標準形関数を予め用意してお
き、これに時間的なスケーリング操作や上記した時間的
対称操作を施すことによって、各モードに対応した加減
速曲線を生成するという方法を用いるものである。
減速曲線の形状に関する標準形関数を予め用意してお
き、これに時間的なスケーリング操作や上記した時間的
対称操作を施すことによって、各モードに対応した加減
速曲線を生成するという方法を用いるものである。
【0066】この標準形関数は、サーボ系の伝達関数と
して3次の関数を考え、ステップ状の入力信号を与えた
ときの応答出力を周波数領域で表現した関係式に対して
逆ラプラス変換を施すことにより時間領域で表現し直す
ことで得られるものであり、下式に示すように指数関数
の和として表される。
して3次の関数を考え、ステップ状の入力信号を与えた
ときの応答出力を周波数領域で表現した関係式に対して
逆ラプラス変換を施すことにより時間領域で表現し直す
ことで得られるものであり、下式に示すように指数関数
の和として表される。
【0067】尚、式の導出についての説明に深入りする
ことにはそれ程益がないと考えられるので、その詳細に
ついては説明を省略する(特願平2−137615号を
参照)。
ことにはそれ程益がないと考えられるので、その詳細に
ついては説明を省略する(特願平2−137615号を
参照)。
【0068】L=1かつTp=Tpminにおける規格
化された距離関数をln(t:Tpmin)とすると、
これはサーボ系の時定数T1min、T2min、T3
minを用いて下式のように表される。
化された距離関数をln(t:Tpmin)とすると、
これはサーボ系の時定数T1min、T2min、T3
minを用いて下式のように表される。
【0069】
【数2】
【0070】前記の手順2)において曲線長Lに対応す
るTpが求められたならば、パラメーターα(=Tp/
Tpmin)の値を求め、次式で定義するTi(iは軸
の指標に対応しi=1、2、3である。)やδ(Tp)
を決定する。
るTpが求められたならば、パラメーターα(=Tp/
Tpmin)の値を求め、次式で定義するTi(iは軸
の指標に対応しi=1、2、3である。)やδ(Tp)
を決定する。
【0071】
【数3】
【0072】上式は、パラメーターαによるスケール変
換操作を表している。
換操作を表している。
【0073】尚、上式中のパラメーターβ、δ(Tp)
(以下、「基本パラメーター」という。)のうちβは定
数であり、また、δ(Tp)は図11に示したような加
減速曲線において該曲線と時間軸とで囲まれた面積に相
当する高さVpの四角形の横幅に等しい。
(以下、「基本パラメーター」という。)のうちβは定
数であり、また、δ(Tp)は図11に示したような加
減速曲線において該曲線と時間軸とで囲まれた面積に相
当する高さVpの四角形の横幅に等しい。
【0074】Tpに対応する関数をl(t:Tp)(0
≦t≦Tp)とすると、これは次式で表される。
≦t≦Tp)とすると、これは次式で表される。
【0075】
【数4】
【0076】尚、上式におけるln(t:Tp)は、
[数2]式においてTpminをTpで置換した規格化
関数である。
[数2]式においてTpminをTpで置換した規格化
関数である。
【0077】また、lpは加減速時における移動量であ
り、lp=Vp・δ(Tp)を満たす。
り、lp=Vp・δ(Tp)を満たす。
【0078】この関係式は、減速パターンが加速パター
ンとの間で時間的な対称性を有することから減速時にお
ける移動量が加速時における移動量に等しいことに注意
してlp=2・β・lp・lnからβが求められるの
で、[数4]式を時間で微分した式にt=Tpを代入す
れば、[数3]で示したδの定義式から求めることがで
きる。
ンとの間で時間的な対称性を有することから減速時にお
ける移動量が加速時における移動量に等しいことに注意
してlp=2・β・lp・lnからβが求められるの
で、[数4]式を時間で微分した式にt=Tpを代入す
れば、[数3]で示したδの定義式から求めることがで
きる。
【0079】加減速曲線は、[数4]式を時間で微分し
た式に基づいて生成することができる。即ち、上記の関
数ln(t:Tp)の時間微分は加速期間0≦t≦Tp
における加減速曲線の形状を規定するものであるから、
手順2)において図8乃至図10で説明したように、減
速期間における時間的な折り返し操作や、定速期間にお
ける一定値のパターンの付加操作をTp値に応じて施す
ことによって加減速パターンの全体を得ることができ
る。
た式に基づいて生成することができる。即ち、上記の関
数ln(t:Tp)の時間微分は加速期間0≦t≦Tp
における加減速曲線の形状を規定するものであるから、
手順2)において図8乃至図10で説明したように、減
速期間における時間的な折り返し操作や、定速期間にお
ける一定値のパターンの付加操作をTp値に応じて施す
ことによって加減速パターンの全体を得ることができ
る。
【0080】このようなパターンを、所定の時間間隔T
s(例えば、10m秒)で標本化することによって算出
されるデータの組をメモリー(RAM)に時系列データ
として記憶しておく。
s(例えば、10m秒)で標本化することによって算出
されるデータの組をメモリー(RAM)に時系列データ
として記憶しておく。
【0081】そして、次の手順4)において、線素列
{Δli}(i=1,2,3・・・)を求める。
{Δli}(i=1,2,3・・・)を求める。
【0082】つまり、[数4]式は、加速期間0≦t≦
Tpにおいて、サンプリング時間Ts毎のCP曲線a上
の作用点の移動を表現しているので、この式を用いて線
素の長さΔlnを下式のように求めることができる。
Tpにおいて、サンプリング時間Ts毎のCP曲線a上
の作用点の移動を表現しているので、この式を用いて線
素の長さΔlnを下式のように求めることができる。
【0083】
【数5】
【0084】減速期間における線素の長さについては、
前述したように加減速パターンもつの対称性に着目する
と、加速期間において算出された線素長Δlnを時間的
に逆向きに遡って取り出すこと容易に求めることができ
る。
前述したように加減速パターンもつの対称性に着目する
と、加速期間において算出された線素長Δlnを時間的
に逆向きに遡って取り出すこと容易に求めることができ
る。
【0085】また、加減速パターンが定速パターンを含
む場合には、線素長Δlnは下式のようにサンプリング
時間Tsと速度Vpとの積に等しい。
む場合には、線素長Δlnは下式のようにサンプリング
時間Tsと速度Vpとの積に等しい。
【0086】
【数6】
【0087】以上の手順を経て曲線長Lに対応する加減
速曲線と、CP曲線aをサンプリング時間Tsで刻んだ
線素列{Δli}(i=1,2,3・・・)が得られる
ことになる。
速曲線と、CP曲線aをサンプリング時間Tsで刻んだ
線素列{Δli}(i=1,2,3・・・)が得られる
ことになる。
【0088】次に、CP制御にとって必須となる逆運動
学問題の解法について説明する。
学問題の解法について説明する。
【0089】ロボットの運動学を手先位置と関節変位や
姿勢との関係という視点から捉えた場合、各関節の変位
や姿勢が与えらたときにこの状態に対応したロボットの
手先位置を求める問題が順運動学問題であり、これとは
反対に手先位置が与えられたときにロボットの関節変位
や姿勢を求める問題が逆運動学問題である。
姿勢との関係という視点から捉えた場合、各関節の変位
や姿勢が与えらたときにこの状態に対応したロボットの
手先位置を求める問題が順運動学問題であり、これとは
反対に手先位置が与えられたときにロボットの関節変位
や姿勢を求める問題が逆運動学問題である。
【0090】一般に、順運動学問題を解くことは容易で
あり、例えば、スカラ型ロボットの場合には、ロボット
アームの各関節に固定したリンク座標系で記述した同次
変換行列の積を計算することに帰着させることができ
る。
あり、例えば、スカラ型ロボットの場合には、ロボット
アームの各関節に固定したリンク座標系で記述した同次
変換行列の積を計算することに帰着させることができ
る。
【0091】これに対してCP制御においては、所望の
経路に沿ったロボットの軌跡制御、つまり、この軌跡を
描かせるためにロボットの各関節をどのように制御する
必要があるかという逆運動学問題に関する解法に関心が
向けられる。
経路に沿ったロボットの軌跡制御、つまり、この軌跡を
描かせるためにロボットの各関節をどのように制御する
必要があるかという逆運動学問題に関する解法に関心が
向けられる。
【0092】逆運動学問題を解くことは一般に難しく、
また計算量が多いことも相まって処理に要する時間が長
いといった問題がある。
また計算量が多いことも相まって処理に要する時間が長
いといった問題がある。
【0093】本発明では、順運動学問題を解く際に得ら
れる同次変換行列を演算要素として含むソフトウェアフ
ィルター(以下、「インバートフィルター」といい、逆
運動学問題解析手段に相当する。具体例については、図
13を参照。)を構成し、このフィルターを通して順運
動学問題と逆運動学問題の解を得る方法(以下、「イン
バートフィルター法」という。)を採用する。
れる同次変換行列を演算要素として含むソフトウェアフ
ィルター(以下、「インバートフィルター」といい、逆
運動学問題解析手段に相当する。具体例については、図
13を参照。)を構成し、このフィルターを通して順運
動学問題と逆運動学問題の解を得る方法(以下、「イン
バートフィルター法」という。)を採用する。
【0094】この方法は、数学的解法の形式化が比較的
容易であり、ロボット構造の違い(例えば、直交型、ス
カラ型、垂直型等)に依らずに処理の統一化を図ること
ができるという利点を有し、各ロボットへの適用にあた
っては、解法の一部を変更するだけで容易に対応するこ
とができる。
容易であり、ロボット構造の違い(例えば、直交型、ス
カラ型、垂直型等)に依らずに処理の統一化を図ること
ができるという利点を有し、各ロボットへの適用にあた
っては、解法の一部を変更するだけで容易に対応するこ
とができる。
【0095】以下では、インバートフィルターの構成上
同次変換行列が必要となるので、順運動学問題の解法を
簡単に説明した後インバートフィルターの構成について
説明を行うことにする。
同次変換行列が必要となるので、順運動学問題の解法を
簡単に説明した後インバートフィルターの構成について
説明を行うことにする。
【0096】先ず、ロボットの関節軸に固定したリンク
座標系と、以下に使用する記号の定義について説明す
る。
座標系と、以下に使用する記号の定義について説明す
る。
【0097】3次元空間におけるロボットの運動状態を
数学的に表現するにあたっては、基準となる座標系(以
下、「基準座標系」という。)を設定し、この基準座標
系から、物体に固定した座標系(以下、「物体座標系」
という。)の原点位置や各座標軸を見た時に両者間の相
対的な関係がどのようになっているかを記述する必要が
ある。
数学的に表現するにあたっては、基準となる座標系(以
下、「基準座標系」という。)を設定し、この基準座標
系から、物体に固定した座標系(以下、「物体座標系」
という。)の原点位置や各座標軸を見た時に両者間の相
対的な関係がどのようになっているかを記述する必要が
ある。
【0098】ロボットの各関節に対して指標を付け、各
関節に物体座標系を設定して関節の変位や姿勢を状態ベ
クトルによって表すとともにロボットの手先位置を基準
座標系での位置ベクトルによって表すことにすると、関
節の状態ベクトルを独立変数とし位置ベクトルを従属変
数とする写像によって両者を関係付けることができる。
関節に物体座標系を設定して関節の変位や姿勢を状態ベ
クトルによって表すとともにロボットの手先位置を基準
座標系での位置ベクトルによって表すことにすると、関
節の状態ベクトルを独立変数とし位置ベクトルを従属変
数とする写像によって両者を関係付けることができる。
【0099】リンク座標系は、各関節に設定された物体
座標系の集合系であり、隣接する座標系間の変換則が知
られている場合には、回転操作や並進操作を含む同次変
換の内容を明らかにすることによってリンク座標系と基
準座標系とを関係付けることができ、これが順運動学問
題を解くことに他ならない。
座標系の集合系であり、隣接する座標系間の変換則が知
られている場合には、回転操作や並進操作を含む同次変
換の内容を明らかにすることによってリンク座標系と基
準座標系とを関係付けることができ、これが順運動学問
題を解くことに他ならない。
【0100】隣接する物体座標の相対的な位置関係は、
関節間の距離や角度等のパラメーター(以下、「リンク
パラメーター」という。)を用いて記述することがで
き、「Denavit−Hartenbergの記法」
によれば、4変数によってリンク機構を記述することが
できることが知られている。
関節間の距離や角度等のパラメーター(以下、「リンク
パラメーター」という。)を用いて記述することがで
き、「Denavit−Hartenbergの記法」
によれば、4変数によってリンク機構を記述することが
できることが知られている。
【0101】以下では基準座標系や物体座標系を整数値
nによって指標付けられた「Σn(On;Xn,Yn,
Zn)」で表現し、その際点Onを原点とし、Xn,Yn,
Znを直交座標軸とする。
nによって指標付けられた「Σn(On;Xn,Yn,
Zn)」で表現し、その際点Onを原点とし、Xn,Yn,
Znを直交座標軸とする。
【0102】また、座標系Σn(On;Xn,Yn,Zn)
の基本ベクトルの組を<in,jn,kn>とし、ベク
トルrに関する座標系Σnでの表現を「nr」と記すこと
にする。
の基本ベクトルの組を<in,jn,kn>とし、ベク
トルrに関する座標系Σnでの表現を「nr」と記すこと
にする。
【0103】そして、Σn系の基本ベクトルをΣm系で表
現したものの組を<min,mjn,mkn>とし、Σn系か
らΣm系を見た場合の同次変換行列を「nTm」と記す。
現したものの組を<min,mjn,mkn>とし、Σn系か
らΣm系を見た場合の同次変換行列を「nTm」と記す。
【0104】尚、座標系間の相対的な回転角θの三角関
数について「S(θ)(=SINθ)」、「C(θ)
(=COSθ)」という簡略記法を用いることにする。
数について「S(θ)(=SINθ)」、「C(θ)
(=COSθ)」という簡略記法を用いることにする。
【0105】上述の定義を下表2にまとめて示す。
【0106】
【表2】
【0107】次に、図12に示すようなスカラ型ロボッ
トを例にして、各関節の回動状態と手先位置との関係に
ついて説明する。
トを例にして、各関節の回動状態と手先位置との関係に
ついて説明する。
【0108】この例では、基軸部に関して第1アームが
回動可能な状態で設けられ、該第1アームに対して回動
可能な状態で取り付けられた第2アームの先端に、並進
移動が可能なツール搭載軸が設けられ、その先端に回動
可能なツールが取り付けられた構造を有している。つま
り、ロボットは、3つの回動関節と1つの直動関節が直
列的に結合されたリンク構造を有するものとする。
回動可能な状態で設けられ、該第1アームに対して回動
可能な状態で取り付けられた第2アームの先端に、並進
移動が可能なツール搭載軸が設けられ、その先端に回動
可能なツールが取り付けられた構造を有している。つま
り、ロボットは、3つの回動関節と1つの直動関節が直
列的に結合されたリンク構造を有するものとする。
【0109】そして、座標系については、上記した定義
に従って各関節軸に固定された4つの直交座標系Σ
0(O0;X0,Y0,Z0)、Σ1(O1;X1,Y1,
Z1)、Σ2(O2;X2,Y2,Z2)、Σ3(O3;X3,
Y3,Z3)、Σ4(O4;X4,Y4,Z4)を設定する。
に従って各関節軸に固定された4つの直交座標系Σ
0(O0;X0,Y0,Z0)、Σ1(O1;X1,Y1,
Z1)、Σ2(O2;X2,Y2,Z2)、Σ3(O3;X3,
Y3,Z3)、Σ4(O4;X4,Y4,Z4)を設定する。
【0110】尚、ここで座標系Σ0(O0;X0,Y0,Z
0)の原点O0は、ロボットの基軸部の中心であって、そ
の高さは原点位置出し終了後におけるツール搭載軸の先
端位置に等しいものとし、下方をZ0軸の正の向きとす
る。
0)の原点O0は、ロボットの基軸部の中心であって、そ
の高さは原点位置出し終了後におけるツール搭載軸の先
端位置に等しいものとし、下方をZ0軸の正の向きとす
る。
【0111】また、座標系Σ1(O1;X1,Y1,Z1)
はその原点O1が座標系Σ0(O0;X0,Y0,Z0)の原
点O0に一致し、X1軸が第1ア−ムの中心軸に選ばれ、
これに直交する水平面内の軸がY1軸とされ、Z1軸は座
標系Σ0(O0;X0,Y0,Z0)のZ0軸に一致するよう
に選ばれた物体座標系である。
はその原点O1が座標系Σ0(O0;X0,Y0,Z0)の原
点O0に一致し、X1軸が第1ア−ムの中心軸に選ばれ、
これに直交する水平面内の軸がY1軸とされ、Z1軸は座
標系Σ0(O0;X0,Y0,Z0)のZ0軸に一致するよう
に選ばれた物体座標系である。
【0112】尚、X1軸とX0軸との間になす角をθ1と
する。
する。
【0113】座標系Σ2(O2;X2,Y2,Z2)はその
原点O2が第1アームと第2アームとを連結する回動関
節中心に置かれるとともにその高さは座標系Σ0(O0;
X0,Y0,Z0)の原点O0の高さと同じとされ、X2軸
が第2ア−ムの中心軸に選ばれ、これに直交する水平面
内の軸がY2軸とされ、Z2軸が座標系Σ0(O0;X0,
Y0,Z0)のZ0軸に対して平行な軸に選ばれた物体座
標系である。尚、X2軸とX1軸との間になす角をθ2と
する。
原点O2が第1アームと第2アームとを連結する回動関
節中心に置かれるとともにその高さは座標系Σ0(O0;
X0,Y0,Z0)の原点O0の高さと同じとされ、X2軸
が第2ア−ムの中心軸に選ばれ、これに直交する水平面
内の軸がY2軸とされ、Z2軸が座標系Σ0(O0;X0,
Y0,Z0)のZ0軸に対して平行な軸に選ばれた物体座
標系である。尚、X2軸とX1軸との間になす角をθ2と
する。
【0114】座標系Σ3(O3;X3,Y3,Z3)は、そ
の原点O3がツール搭載軸の中心であって原点位置出し
終了時の高さが座標系Σ2(O2;X2,Y2,Z2)の原
点O2の高さと同じとされ、Z3軸が鉛直軸に選ばれ、こ
れに直交する水平面内の2軸X3軸、Y3軸がΣ2(O2;
X2,Y2,Z2)の2軸X2軸、Y2軸にそれぞれ平行に
選ばれた物体座標系である。
の原点O3がツール搭載軸の中心であって原点位置出し
終了時の高さが座標系Σ2(O2;X2,Y2,Z2)の原
点O2の高さと同じとされ、Z3軸が鉛直軸に選ばれ、こ
れに直交する水平面内の2軸X3軸、Y3軸がΣ2(O2;
X2,Y2,Z2)の2軸X2軸、Y2軸にそれぞれ平行に
選ばれた物体座標系である。
【0115】尚、ツール搭載軸の長さを「P3」とし、
その変位を「P3・θ3」とする。座標系Σ4(O4;X
4,Y4,Z4)はその原点O4が座標系Σ3(O3;X3,
Y3,Z3)の原点O3に一致し、そのZ4軸がZ3軸に一
致するとともにX3軸、Y3軸を水平面内においてθ4の
角度をもって回転させた軸をそれぞれX4軸、Y4軸とす
る物体座標系である。
その変位を「P3・θ3」とする。座標系Σ4(O4;X
4,Y4,Z4)はその原点O4が座標系Σ3(O3;X3,
Y3,Z3)の原点O3に一致し、そのZ4軸がZ3軸に一
致するとともにX3軸、Y3軸を水平面内においてθ4の
角度をもって回転させた軸をそれぞれX4軸、Y4軸とす
る物体座標系である。
【0116】尚、第1アームの長さを「L1」、第2ア
ームの長さを「L2」とする。
ームの長さを「L2」とする。
【0117】回転操作や並進操作等についての行列表現
に係る知識を用いると、座標系Σi+ 1から座標系Σiをみ
たときの同次変換行列を求めることができる。
に係る知識を用いると、座標系Σi+ 1から座標系Σiをみ
たときの同次変換行列を求めることができる。
【0118】例えば、同次変換行列0T1は下式[数7]
に示すようになり、また、鉛直軸回りの回動操作にアー
ム長分の並進を加味した同次変換行列に一般化すること
によって同次変換行列1T2を求めたり、角度パラメータ
ーの単なる置き換えによってや同次変換行列3T4を容易
に求めることができる。
に示すようになり、また、鉛直軸回りの回動操作にアー
ム長分の並進を加味した同次変換行列に一般化すること
によって同次変換行列1T2を求めたり、角度パラメータ
ーの単なる置き換えによってや同次変換行列3T4を容易
に求めることができる。
【0119】また、X2軸やZ2軸に関する並進操作のみ
を取り入れた同次変換行列として2T3が下式にように求
められる。
を取り入れた同次変換行列として2T3が下式にように求
められる。
【0120】
【数7】
【0121】これらの同次変換行列に積0T1 1T2 2T3 3
T4を計算することによって0T4が求められ、座標系Σ0
とΣ4との間に関係付けがなされることになる。
T4を計算することによって0T4が求められ、座標系Σ0
とΣ4との間に関係付けがなされることになる。
【0122】次に、逆運動学問題の解法についての説明
に移るが、これは手先位置の位置ベクトルを独立変数と
し関節の状態ベクトルを従属変数とする写像によって両
者を関係付けること、つまり形式的には前述した写像の
逆写像を求めることに相当する。
に移るが、これは手先位置の位置ベクトルを独立変数と
し関節の状態ベクトルを従属変数とする写像によって両
者を関係付けること、つまり形式的には前述した写像の
逆写像を求めることに相当する。
【0123】図13は、インバートフィルターの基本構
成を示すブロック線図である。
成を示すブロック線図である。
【0124】図中の記号「Xr」、「Yr」はそれぞれ
X座標、Y座標に関する指令値、「R2r」は、作用端
位置と基準座標系の原点との間の距離についての指令値
であり、これらは選択的にインバートフィルターに与え
られるようになっている。
X座標、Y座標に関する指令値、「R2r」は、作用端
位置と基準座標系の原点との間の距離についての指令値
であり、これらは選択的にインバートフィルターに与え
られるようになっている。
【0125】尚、この例では説明の簡単にするため、図
14に示すように2リンク系の構造を取り上げ、その平
面内での動作に限定して話を進めることにする。
14に示すように2リンク系の構造を取り上げ、その平
面内での動作に限定して話を進めることにする。
【0126】図14はX−Y座標系において2つの回動
型関節をもつアーム系を線状モデル化(アームの中心軸
のみを示す。)して示すものであり、原点O寄りの第1
アームがX軸に対してなす角度をθ1とし、原点から遠
い方の第2アームが第1アームの延長線に対してなす角
度をθ2とする。
型関節をもつアーム系を線状モデル化(アームの中心軸
のみを示す。)して示すものであり、原点O寄りの第1
アームがX軸に対してなす角度をθ1とし、原点から遠
い方の第2アームが第1アームの延長線に対してなす角
度をθ2とする。
【0127】また、第2アームの先端位置を点P(X,
Y)、X−Y平面内の任意の点を点P′(X′,Y′)
をとし、原点O−点P間の距離をRとする。尚、説明の
簡単化のために各アーム長を1とする。
Y)、X−Y平面内の任意の点を点P′(X′,Y′)
をとし、原点O−点P間の距離をRとする。尚、説明の
簡単化のために各アーム長を1とする。
【0128】インバートフィルターは、図13に示すよ
うに順運動学問題を解くときに用いるパス(以下、「フ
ォワードパス」という。)に係る伝達関数GF(s)を
含んでおり、上記の指令値がゲイン1/|GF(s)
|、積分演算(積分記号で示す。)、そして定数ゲイン
1/Gaを経てフォーワードパスGF(s)に送られる
ように構成されている。
うに順運動学問題を解くときに用いるパス(以下、「フ
ォワードパス」という。)に係る伝達関数GF(s)を
含んでおり、上記の指令値がゲイン1/|GF(s)
|、積分演算(積分記号で示す。)、そして定数ゲイン
1/Gaを経てフォーワードパスGF(s)に送られる
ように構成されている。
【0129】そして、フォワードパスの出力X、Y、R
2はネガティブフィードバックループを通してこれらの
指令値の入力ノードへと選択的に戻される(「ε」はエ
ラーを意味する。)。
2はネガティブフィードバックループを通してこれらの
指令値の入力ノードへと選択的に戻される(「ε」はエ
ラーを意味する。)。
【0130】GF(s)は、この例では[数7]式の3T
4に対応すものであり、ノードΘ、Φの和やΦそのもの
を受けてC、Sで示す三角関数要素(「C」がCOS関
数、「S」がSIN関数を示す)を通して、加算や自乗
演算によって各出力値X、Y、R2が得られるように構
成される。
4に対応すものであり、ノードΘ、Φの和やΦそのもの
を受けてC、Sで示す三角関数要素(「C」がCOS関
数、「S」がSIN関数を示す)を通して、加算や自乗
演算によって各出力値X、Y、R2が得られるように構
成される。
【0131】尚、Θはフィルター内の位相でありループ
内で制御されるのに対し、Φは各関節軸について固定し
た位相である。
内で制御されるのに対し、Φは各関節軸について固定し
た位相である。
【0132】位相Θが角度θ2に対応し、位相Φが角度
θ1に対応しており、位相和Θ+ΦがCを通ってノード
Xに送られるとともに位相ΦがCを通ってノードXで合
成され、また、位相和Θ+ΦがSを通ってノードXに送
られるとともに位相ΦがSを通ってノードYで合成され
る。そして、Xの自乗とYの自乗との和がノードR2に
取り出される。
θ1に対応しており、位相和Θ+ΦがCを通ってノード
Xに送られるとともに位相ΦがCを通ってノードXで合
成され、また、位相和Θ+ΦがSを通ってノードXに送
られるとともに位相ΦがSを通ってノードYで合成され
る。そして、Xの自乗とYの自乗との和がノードR2に
取り出される。
【0133】これは、図14のアーム状態を想像したと
き、リンクパラメーターθ1、θ2を用いてX、Y座標
値を表した下式に対応していると考えれば直感的な理解
を得ることができる。
き、リンクパラメーターθ1、θ2を用いてX、Y座標
値を表した下式に対応していると考えれば直感的な理解
を得ることができる。
【0134】
【数8】
【0135】インバートフィルターにおいてフォワード
パスの前に設けられる要素のうち1/|GF(s)|は
動的なゲイン調整のために設けられる。
パスの前に設けられる要素のうち1/|GF(s)|は
動的なゲイン調整のために設けられる。
【0136】積分要素はループの安定化を図るために設
けられるものであり、図では一段とされているが一般に
は複数段の構成をとることによってフィルター特性を変
られることはフィルター理論から明らかである。
けられるものであり、図では一段とされているが一般に
は複数段の構成をとることによってフィルター特性を変
られることはフィルター理論から明らかである。
【0137】この例では、[数8]式の角度パラメータ
ーを位相に置き換えた式からRの自乗を求めると[数
9]式に示すようにCOSΘに依存することが分かり、
これをΘで微分した式から1/|GF(s)|が求めら
れる。
ーを位相に置き換えた式からRの自乗を求めると[数
9]式に示すようにCOSΘに依存することが分かり、
これをΘで微分した式から1/|GF(s)|が求めら
れる。
【0138】
【数9】
【0139】定数ゲイン1/Gaはループの収束の速さ
を制御するために入れたものである。
を制御するために入れたものである。
【0140】尚、指令値Xr、Yr、R2rの入力段で
の切換と、フォワードパスの出力値X、Y、R2の切換
とはそれぞれ対応するように連動した切換制御が行われ
る。
の切換と、フォワードパスの出力値X、Y、R2の切換
とはそれぞれ対応するように連動した切換制御が行われ
る。
【0141】次に、このような構成のインバートフィル
ターによって順運動学問題や逆運動学問題の解を得る方
法について説明する。
ターによって順運動学問題や逆運動学問題の解を得る方
法について説明する。
【0142】順運動学問題については上記インバートフ
ィルターの一部をなすフォワードパスを用いれば良いこ
とがこれまでの説明から容易に理解される。
ィルターの一部をなすフォワードパスを用いれば良いこ
とがこれまでの説明から容易に理解される。
【0143】即ち、インバートフィルターを開ループに
したままで、図13のノードΘやΦに値を与えると出力
値X、Yを得ることができ、[数8]に対応した結果が
得られる。
したままで、図13のノードΘやΦに値を与えると出力
値X、Yを得ることができ、[数8]に対応した結果が
得られる。
【0144】また、逆運動学問題を、インバートフィル
ターを用いて解くには次のような手順を踏む(図13、
図14参照。)。
ターを用いて解くには次のような手順を踏む(図13、
図14参照。)。
【0145】(A1)各アームのある時点における姿勢
から次の姿勢へとアームの状態を変化させたい場合、現
時点での角度パラメーターθ1、θ2(図14参照。)
の値は知られている。これはポジションカウンター値か
ら読み出すことができるからである。
から次の姿勢へとアームの状態を変化させたい場合、現
時点での角度パラメーターθ1、θ2(図14参照。)
の値は知られている。これはポジションカウンター値か
ら読み出すことができるからである。
【0146】(A2)そこで、その時のθ1の値に対応
した位相を、図13のノードΦにセットする。 (A3)次にO−P′間(図14参照。)の距離Rrを
計算する。点P′は移動指令の目標位置として与えられ
るものであり、事前に分かっている。そして、Rrの自
乗値をインバートフィルター(図13参照。)への入力
値としてセットする。つまり、インバートフィルターに
おいて、指令値R2rとフォワードパスの出力値R2が選
択されるように切換制御を行い、距離の自乗値に関して
閉ループが形成されるようにする。
した位相を、図13のノードΦにセットする。 (A3)次にO−P′間(図14参照。)の距離Rrを
計算する。点P′は移動指令の目標位置として与えられ
るものであり、事前に分かっている。そして、Rrの自
乗値をインバートフィルター(図13参照。)への入力
値としてセットする。つまり、インバートフィルターに
おいて、指令値R2rとフォワードパスの出力値R2が選
択されるように切換制御を行い、距離の自乗値に関して
閉ループが形成されるようにする。
【0147】(A4)インバートフィルターを起動し
て、図13に示すエラーεがゼロに収束するのを待ち、
ε=0となったらそのときのノードΘの値(これを
「Θ′」とする。)が第2アームに関する逆運動学問題
の解として求まる。
て、図13に示すエラーεがゼロに収束するのを待ち、
ε=0となったらそのときのノードΘの値(これを
「Θ′」とする。)が第2アームに関する逆運動学問題
の解として求まる。
【0148】(A5)それから、目標位置P′(図14
参照。)のX−Y座標X′又はY′に対応する指令値X
r又はYrをインバートフィルターへの入力値としてセ
ットして、フォワードパスの出力値X又はYがフィード
バックされるように閉ループを形成する(図13の「−
1」を付して示す経路を参照。)。
参照。)のX−Y座標X′又はY′に対応する指令値X
r又はYrをインバートフィルターへの入力値としてセ
ットして、フォワードパスの出力値X又はYがフィード
バックされるように閉ループを形成する(図13の「−
1」を付して示す経路を参照。)。
【0149】(A6)このとき同時に(A4)で求めた
Θ′の値を、図13のノードΦにセットしてインバート
フィルターを起動し、エラーεがゼロになったときのノ
ードΘ値が第1アームに関する逆運動学問題の解として
求まる(尚、実際上は精度以上の計算を行う必要がない
ので、エラー|ε|が精度以下になったらフィルター計
算を停止して計算時間の短縮化を図るようにすれば良
い。)。
Θ′の値を、図13のノードΦにセットしてインバート
フィルターを起動し、エラーεがゼロになったときのノ
ードΘ値が第1アームに関する逆運動学問題の解として
求まる(尚、実際上は精度以上の計算を行う必要がない
ので、エラー|ε|が精度以下になったらフィルター計
算を停止して計算時間の短縮化を図るようにすれば良
い。)。
【0150】以上の手順によって目標位置と距離の指令
値からアームの状態を示す角度パラメーター値が求めら
れることになるが、このような手順について図15を用
いて幾何学的に解釈すると各手順の意味内容を把握し易
い。
値からアームの状態を示す角度パラメーター値が求めら
れることになるが、このような手順について図15を用
いて幾何学的に解釈すると各手順の意味内容を把握し易
い。
【0151】図15は図14と同様にロボットアームの
動きをモデル化して示すものであり、上記した手順に対
するアームの回動状態を、時間の経過を追って(a)乃
至(c)に示したものである。
動きをモデル化して示すものであり、上記した手順に対
するアームの回動状態を、時間の経過を追って(a)乃
至(c)に示したものである。
【0152】先ず、図15(a)に示すようにX−Y座
標系の原点Oを中心としてOP′(=Rr)を半径とす
る円弧CIRを描く。
標系の原点Oを中心としてOP′(=Rr)を半径とす
る円弧CIRを描く。
【0153】次に、図15(b)に示すように第1アー
ムの角度θ1の値を固定したまま、第2アームをその関
節中心O′の反時計回りに回転させ、その先端位置Pが
円弧CIRと交わる位置P1になったときに回転を停止
する。
ムの角度θ1の値を固定したまま、第2アームをその関
節中心O′の反時計回りに回転させ、その先端位置Pが
円弧CIRと交わる位置P1になったときに回転を停止
する。
【0154】このときの第2アームの角度θ2′が当該
アームに関する逆運動学問題の解である。
アームに関する逆運動学問題の解である。
【0155】尚、インバートフィルターがネガティブフ
ィードバックとして構成されていることから、第2アー
ムの回転方向については、必ず点Pが目標点P′に近づ
く方向となり、その逆方向に回転されることはない。
ィードバックとして構成されていることから、第2アー
ムの回転方向については、必ず点Pが目標点P′に近づ
く方向となり、その逆方向に回転されることはない。
【0156】これまでの動きは、インバートフィルター
についての手順(A2)乃至(A4)に対応する。
についての手順(A2)乃至(A4)に対応する。
【0157】次に、図15(c)に示すように第2アー
ムの角度θ2をθ2′に固定した状態で第1アームを回
転させ、それまで点P1に一致していた第2アームの端
点Pが目標点P′に一致したときに回転を止める。
ムの角度θ2をθ2′に固定した状態で第1アームを回
転させ、それまで点P1に一致していた第2アームの端
点Pが目標点P′に一致したときに回転を止める。
【0158】このときの第1アームの角度θ1′が当該
アームに関する逆運動学問題の解である。尚、この動き
がインバートフィルターについての手順(A5)、(A
6)に相当する。
アームに関する逆運動学問題の解である。尚、この動き
がインバートフィルターについての手順(A5)、(A
6)に相当する。
【0159】インバートフィルターを用いてこのような
移動制御を実現するシーケンスが上記した手順(A1)
乃至(A6)である。
移動制御を実現するシーケンスが上記した手順(A1)
乃至(A6)である。
【0160】尚、図14及び図15ではアームの数を2
に限定した例を示したが、N個のアームを有する系にこ
のインバートフィルター法を一般化することができるこ
とは勿論である。
に限定した例を示したが、N個のアームを有する系にこ
のインバートフィルター法を一般化することができるこ
とは勿論である。
【0161】その場合にはインバートフィルターの起動
回数をアーム数に応じて増やすようにすれば、全く同一
のアルゴリズムを用いて逆運動学問題を解くことができ
る。例えば、アーム数がNの場合には、N−1番目のア
ームに係る関節中心が図14の原点Oに対応し、N番目
のアームの関節中心が点O′にそれぞれ対応するものと
考えれば良く、それぞれの角度パラメーターθN、θN
−1を決定してから、Nの値を減らして同様のフィルタ
ー計算を行ってロボットの基準座標の原点に近いアーム
に関する角度パラメーター列θJ(J=N−2、N−
3、・・・、1)を順番に決定していけば最終的な解を
得ることができる。
回数をアーム数に応じて増やすようにすれば、全く同一
のアルゴリズムを用いて逆運動学問題を解くことができ
る。例えば、アーム数がNの場合には、N−1番目のア
ームに係る関節中心が図14の原点Oに対応し、N番目
のアームの関節中心が点O′にそれぞれ対応するものと
考えれば良く、それぞれの角度パラメーターθN、θN
−1を決定してから、Nの値を減らして同様のフィルタ
ー計算を行ってロボットの基準座標の原点に近いアーム
に関する角度パラメーター列θJ(J=N−2、N−
3、・・・、1)を順番に決定していけば最終的な解を
得ることができる。
【0162】また、上述の例では回動型関節を有するア
ーム構造だけを示したが、これに限らず、直動型関節だ
けから構成させるアーム系や、回動型関節と直動型関節
とを適宜に組み合わせたアーム系にインバートフィルタ
ー法を適用することができる。尚、その際、直動型関節
に対して解くべきパラメーターは変位となる。
ーム構造だけを示したが、これに限らず、直動型関節だ
けから構成させるアーム系や、回動型関節と直動型関節
とを適宜に組み合わせたアーム系にインバートフィルタ
ー法を適用することができる。尚、その際、直動型関節
に対して解くべきパラメーターは変位となる。
【0163】このように、インバートフィルター法に係
るアルゴリズムは基本的にアーム構造に左右されないの
で汎用性が高く、よって、直交型、スカラ型、垂直多関
節型等の各種のマニュピレーターに適用することができ
る。
るアルゴリズムは基本的にアーム構造に左右されないの
で汎用性が高く、よって、直交型、スカラ型、垂直多関
節型等の各種のマニュピレーターに適用することができ
る。
【0164】ところで、上記の例では説明を簡単にする
ために、ロボットの手先位置に対するCP制御だけを説
明してきたが、上記インバートフィルター法によれば、
ワーク上の任意の位置に対するCP制御を実現すること
も可能である(但し、一般にはワーク上の点がワーク座
標系で与えられていれば、手先位置を基準としたオフセ
ットを加味することでワーク上の位置を容易に求めるこ
とができる。)。
ために、ロボットの手先位置に対するCP制御だけを説
明してきたが、上記インバートフィルター法によれば、
ワーク上の任意の位置に対するCP制御を実現すること
も可能である(但し、一般にはワーク上の点がワーク座
標系で与えられていれば、手先位置を基準としたオフセ
ットを加味することでワーク上の位置を容易に求めるこ
とができる。)。
【0165】例えば、図16に示すように、第2アーム
の先端部に点O′′を中心として回動可能な状態で円板
が設けられており、円板上の点PSを点PEに移動させ
る場合を想定する。尚、円板の点O′′に関する回動角
をθ3とする。
の先端部に点O′′を中心として回動可能な状態で円板
が設けられており、円板上の点PSを点PEに移動させ
る場合を想定する。尚、円板の点O′′に関する回動角
をθ3とする。
【0166】この例について逆運動学問題を解く手順は
以下にようになる。
以下にようになる。
【0167】(B1)先ず、第2アームの関節中心O′
を中心とし、この点と目標点PEとの間の距離を半径と
する円弧CIR1を描く。この操作は、点O−点PE間
距離の2乗をインバートフィルターへの入力としてセッ
トするとともに、第2アームの角度パラメーターθ2の
値を固定してノードΦにセットする処理に対応する。 (B2)インバートフィルターの起動をかける前に、点
O′−点O′′間の距離と点O′′−点PS間の距離と
の差を求め、その2乗値をリミッター値とする。これは
点O′−点PS間の距離がリミッター値より大きくなる
ことは理論上ありえないからであり、従って、インバー
トフィルターを起動してεが0になる前に点O′−点P
S間の距離がリミッター値に達した場合には、フィルタ
ーの動作をそこで停止し、そのときの角度θ3を解とす
る必要がある。
を中心とし、この点と目標点PEとの間の距離を半径と
する円弧CIR1を描く。この操作は、点O−点PE間
距離の2乗をインバートフィルターへの入力としてセッ
トするとともに、第2アームの角度パラメーターθ2の
値を固定してノードΦにセットする処理に対応する。 (B2)インバートフィルターの起動をかける前に、点
O′−点O′′間の距離と点O′′−点PS間の距離と
の差を求め、その2乗値をリミッター値とする。これは
点O′−点PS間の距離がリミッター値より大きくなる
ことは理論上ありえないからであり、従って、インバー
トフィルターを起動してεが0になる前に点O′−点P
S間の距離がリミッター値に達した場合には、フィルタ
ーの動作をそこで停止し、そのときの角度θ3を解とす
る必要がある。
【0168】(B3)インバートフィルターを起動す
る。つまり、これは図16ではθ3軸回りに円板を回転
させることに対応する。
る。つまり、これは図16ではθ3軸回りに円板を回転
させることに対応する。
【0169】(B4)そして、ε=0になったときには
点O′−点PS間の距離が点O′−点PE間の距離に等
しくなっており、よってこの時点でフィルターの動作を
止める。この時のノードΘの値がθ3に関する解であ
る。また、ε=0にならず点O′−点PS間距離が上記
(B2)のリミッター値になった時には、この時点でフ
ィルターの動作を止めればノードΘにθ3の解が得られ
る。
点O′−点PS間の距離が点O′−点PE間の距離に等
しくなっており、よってこの時点でフィルターの動作を
止める。この時のノードΘの値がθ3に関する解であ
る。また、ε=0にならず点O′−点PS間距離が上記
(B2)のリミッター値になった時には、この時点でフ
ィルターの動作を止めればノードΘにθ3の解が得られ
る。
【0170】図16は前者の場合に相当する例を示して
おり、点O′−点PE間の距離に等しい半径をもち点
O′を中心とする円弧CIR1上の点P′Sが解に対応
した点である。
おり、点O′−点PE間の距離に等しい半径をもち点
O′を中心とする円弧CIR1上の点P′Sが解に対応
した点である。
【0171】(B5)θ3を固定して第2アームを回転
させることによって、点P′Sが円弧CIR1上を移動
して最終的に点PEに一致し、このときの角度θ2が解
となる。この動きをインバートフィルターの動作に対応
させると、指令値Rrを点O′−PE間距離に等しい値
にセット、Φをθ1に対応した位相値にセットしてフィ
ルターを起動して、ε=0になったときのノードΘにθ
2の解が得られることに他ならない。
させることによって、点P′Sが円弧CIR1上を移動
して最終的に点PEに一致し、このときの角度θ2が解
となる。この動きをインバートフィルターの動作に対応
させると、指令値Rrを点O′−PE間距離に等しい値
にセット、Φをθ1に対応した位相値にセットしてフィ
ルターを起動して、ε=0になったときのノードΘにθ
2の解が得られることに他ならない。
【0172】尚、この例では、点PSに軌跡と円弧CI
R1とが交点P′Sで交わるため、θ3とθ2に関する
解を求めるだけで良かったが、点O′−点PS間距離が
小さいために交点を持たない場合には、3つの角度θ
3、θ2、θ1のついての解くことになる(尚、その手
順はこれまで説明してきたアルゴリズムから明かであろ
う。)。
R1とが交点P′Sで交わるため、θ3とθ2に関する
解を求めるだけで良かったが、点O′−点PS間距離が
小さいために交点を持たない場合には、3つの角度θ
3、θ2、θ1のついての解くことになる(尚、その手
順はこれまで説明してきたアルゴリズムから明かであろ
う。)。
【0173】上記したインバートフィルター法の特徴を
箇条書きにまとめると以下のように要約することができ
る。
箇条書きにまとめると以下のように要約することができ
る。
【0174】(C1)選択的に切り換えられるインバー
トフィルターの入力値やフィードバック量を関節構造に
合ったパラメーターとし、フォワードパスをリンク構造
に応じて変更するだけで各種のロボットに適用すること
ができるので、CP制御の標準化を図ることができる。
トフィルターの入力値やフィードバック量を関節構造に
合ったパラメーターとし、フォワードパスをリンク構造
に応じて変更するだけで各種のロボットに適用すること
ができるので、CP制御の標準化を図ることができる。
【0175】(C2)インバートフィルター内にはフォ
ワードパスが設けられ、インバートフィルターの開ルー
プから順運動学問題を解くことができるので、逆運動学
問題の解法ルーチンと順運動学問題の解法ルーチンとを
全く独立したものとして用意する必要がなく、処理を部
分的に兼用することができる。
ワードパスが設けられ、インバートフィルターの開ルー
プから順運動学問題を解くことができるので、逆運動学
問題の解法ルーチンと順運動学問題の解法ルーチンとを
全く独立したものとして用意する必要がなく、処理を部
分的に兼用することができる。
【0176】(C3)解を効率良く得ることができる。
つまり、逆運動学問題を解くことははロボットの手先位
置からアーム状態を求めることに他ならないが、上述し
たように慣性の小さい手先寄りの関節軸程その動きが大
きく、慣性の大きい基軸部寄りの関節軸程その動きが小
さくなるような一意解を得ることができる(例えば、図
16では第1アームの回動量がゼロであった。)。
つまり、逆運動学問題を解くことははロボットの手先位
置からアーム状態を求めることに他ならないが、上述し
たように慣性の小さい手先寄りの関節軸程その動きが大
きく、慣性の大きい基軸部寄りの関節軸程その動きが小
さくなるような一意解を得ることができる(例えば、図
16では第1アームの回動量がゼロであった。)。
【0177】(C4)ワーク上の任意の一点に関する解
を求めることができる。つまり、ツール先端に固定され
た座標系の原点位置に対するCP制御に限らず、該座標
系での任意の点について解を得ることができる。これに
ついては図16で説明した通りである。また、例えば、
円弧補間時における作用端の座標系の原点や任意の点に
対して円弧補間をかけることができる。
を求めることができる。つまり、ツール先端に固定され
た座標系の原点位置に対するCP制御に限らず、該座標
系での任意の点について解を得ることができる。これに
ついては図16で説明した通りである。また、例えば、
円弧補間時における作用端の座標系の原点や任意の点に
対して円弧補間をかけることができる。
【0178】(C5)逆関数を用いずにフォワードパス
に含まれる順関数を用いて解くことができる。逆運動学
問題の解法には、幾何学的な解法や代数的な解法が知ら
れているがこれにはかなりの計算力を要するために処理
に時間がかかってしまうことが知られている。
に含まれる順関数を用いて解くことができる。逆運動学
問題の解法には、幾何学的な解法や代数的な解法が知ら
れているがこれにはかなりの計算力を要するために処理
に時間がかかってしまうことが知られている。
【0179】例えば、図14のようなアーム系について
の解を幾何学的な解法に従って求めてみると、角度θ
1、θ2が座標値X、Yを用いて表されるが、逆三角関
数や2乗根の逆数等が現れる。また、同じ問題を代数的
な方法で解いた場合にはATAN2(a、b)≡ARG
(b+j*a)(但し、jは虚数単位)で定義される関
数や2乗根等が現れるために解を公式化する上での複雑
さは解消されない。
の解を幾何学的な解法に従って求めてみると、角度θ
1、θ2が座標値X、Yを用いて表されるが、逆三角関
数や2乗根の逆数等が現れる。また、同じ問題を代数的
な方法で解いた場合にはATAN2(a、b)≡ARG
(b+j*a)(但し、jは虚数単位)で定義される関
数や2乗根等が現れるために解を公式化する上での複雑
さは解消されない。
【0180】これらの解法は基本的には解の公式を数学
的に手計算で求めてからソフトウェア処理によって解を
求め、その解を評価することによって最終的な解を得る
ようにプログラムを作成する必要がある。
的に手計算で求めてからソフトウェア処理によって解を
求め、その解を評価することによって最終的な解を得る
ようにプログラムを作成する必要がある。
【0181】従って、公式を見いだすのに時間がかかっ
てしまい、また、公式内に現れる関数を用意しなければ
ならない等の煩雑さがあり、このような解析作業をロボ
ットの種類毎に行なわなければならない。
てしまい、また、公式内に現れる関数を用意しなければ
ならない等の煩雑さがあり、このような解析作業をロボ
ットの種類毎に行なわなければならない。
【0182】これに対して本インバートフィルター法に
あっては、解を求めるに際してロボットの構造に依らな
い共通化されたアルゴリズムに則った処理をインバート
フィルターを用いて行い、このような処理をアーム系に
関して複数回に亘って行うことによって最終的な解に至
る。
あっては、解を求めるに際してロボットの構造に依らな
い共通化されたアルゴリズムに則った処理をインバート
フィルターを用いて行い、このような処理をアーム系に
関して複数回に亘って行うことによって最終的な解に至
る。
【0183】つまり、中心的な要素となるリンク系の部
分系を取り出して解を求めるという手続きを順次に踏む
ことによって、解を積み上げて最終的な解を得ることが
できる。
分系を取り出して解を求めるという手続きを順次に踏む
ことによって、解を積み上げて最終的な解を得ることが
できる。
【0184】本法はその意味でリンク座標系での自由度
を削減しながら解を求めていく次元削減法であるとも言
える。
を削減しながら解を求めていく次元削減法であるとも言
える。
【0185】例えば、図17に示すように垂直多関節型
ロボットを線状モデル化した図において、ある子午線上
の点PS(XS,YS,ZS)から別の子午線上の点P
E(XE,YE,ZE)への移動を想定する。
ロボットを線状モデル化した図において、ある子午線上
の点PS(XS,YS,ZS)から別の子午線上の点P
E(XE,YE,ZE)への移動を想定する。
【0186】尚、図17において直交座標系X−Y−Z
の原点O寄りのアームを第1アームと呼びその先のアー
ムを第2アームと呼ぶことにし、第1アームのX−Y平
面への写影がX軸に対してなす角度をθ1とし、第1ア
ームがZ軸に対してなす角度をθ2、そして、第1アー
ムの延長軸と第2アームとの間になす角度をθ3とす
る。
の原点O寄りのアームを第1アームと呼びその先のアー
ムを第2アームと呼ぶことにし、第1アームのX−Y平
面への写影がX軸に対してなす角度をθ1とし、第1ア
ームがZ軸に対してなす角度をθ2、そして、第1アー
ムの延長軸と第2アームとの間になす角度をθ3とす
る。
【0187】この場合、点PSと点PEとが同一平面な
いことは、座標値の比YS/XSとYE/XEとを比較
すれば容易に分かり、両点の関係が3次元的であるた
め、先ずこれを2次元での問題となるように次元を落と
して、つまり、子午線を含む面内でθ3、θ2の解を求
めてからθ1の解を求めるようにインバートフィルター
を起動すれば良い。
いことは、座標値の比YS/XSとYE/XEとを比較
すれば容易に分かり、両点の関係が3次元的であるた
め、先ずこれを2次元での問題となるように次元を落と
して、つまり、子午線を含む面内でθ3、θ2の解を求
めてからθ1の解を求めるようにインバートフィルター
を起動すれば良い。
【0188】このように問題の次数を下げて縮退化した
解を求めてから縮退をといて最終的な解へと解き進んで
いくことができる。
解を求めてから縮退をといて最終的な解へと解き進んで
いくことができる。
【0189】次に、前述したCP制御の加減速曲線の生
成手順とインバートフィルター法に係る手順を総合した
アルゴリズムについて図18に従って説明する。
成手順とインバートフィルター法に係る手順を総合した
アルゴリズムについて図18に従って説明する。
【0190】(D1)先ず、CP曲線の計算時間間隔、
つまり、サンプリング時間Tsを決定する。
つまり、サンプリング時間Tsを決定する。
【0191】この時間Tsはロボットの制御に用いられ
るCPUの処理速度の高速化とともに短縮化することが
できるが、現状ではTs=10ミリ秒が基本となってお
り、この時間間隔でCP曲線を刻んだ情報がサーボ系に
送られる。
るCPUの処理速度の高速化とともに短縮化することが
できるが、現状ではTs=10ミリ秒が基本となってお
り、この時間間隔でCP曲線を刻んだ情報がサーボ系に
送られる。
【0192】サーボ系ではサーボループのサンプリング
時間(これを「Tss」とする。)の時間間隔毎にルー
プ計算を行っており、Tss<Tsとされている。
時間(これを「Tss」とする。)の時間間隔毎にルー
プ計算を行っており、Tss<Tsとされている。
【0193】よって、Tsを小さくしてTs=Tssと
なることが理想的であるが(CP曲線をより滑らかにす
ることができる。)、それだけメモリーの記憶容量が大
きくなることを勘案してTsを決定すべきである。
なることが理想的であるが(CP曲線をより滑らかにす
ることができる。)、それだけメモリーの記憶容量が大
きくなることを勘案してTsを決定すべきである。
【0194】(D2)CP曲線の曲線長に応じて加減速
曲線を生成するととともに、そのときの線素列を計算す
る。
曲線を生成するととともに、そのときの線素列を計算す
る。
【0195】加減速曲線の発生に係るアルゴリズムにつ
いては既に説明したので、その詳細は省略し、ここで
は、加減速曲線が定速パターン部を有する第3モードの
ピーク速度Vpの決定について説明する。
いては既に説明したので、その詳細は省略し、ここで
は、加減速曲線が定速パターン部を有する第3モードの
ピーク速度Vpの決定について説明する。
【0196】関節を回動型関節とすると、Vpについて
の理想的な値は、各関節の角速度がその最大値を越え
ず、しかも各関節の位相がサーボ系及びメカ系での許容
範囲内に収まるという条件下において可能な限り大きい
値である。
の理想的な値は、各関節の角速度がその最大値を越え
ず、しかも各関節の位相がサーボ系及びメカ系での許容
範囲内に収まるという条件下において可能な限り大きい
値である。
【0197】換言すれば、ある精度内でCP経路の軌跡
及び速度が指令値通りになる最大速度が理想的である。
及び速度が指令値通りになる最大速度が理想的である。
【0198】しかしながら、この時点でVpの値を知る
よしもないため、Vpの仮値Vp0を設定し、この仮値
Vp0を使って線素列{Δln}を計算してその結果を
メモリーに記憶する。
よしもないため、Vpの仮値Vp0を設定し、この仮値
Vp0を使って線素列{Δln}を計算してその結果を
メモリーに記憶する。
【0199】尚、仮値Vp0は、CP曲線の形状に応じ
て可変することが望ましく、また、CP制御における最
高速度値の半分程度の値が当面の目安となるが、その具
体的な決定方法はメモリー容量に関連するため後に詳述
する。
て可変することが望ましく、また、CP制御における最
高速度値の半分程度の値が当面の目安となるが、その具
体的な決定方法はメモリー容量に関連するため後に詳述
する。
【0200】ここでは、なんらかの方法によって仮値V
p0が決定されたものとして説明を続ける。
p0が決定されたものとして説明を続ける。
【0201】(D3)線素列{Δln}を基にしてi番
目の関節軸に係る関節角列{θin}をインバートフィ
ルターを用いて求める。
目の関節軸に係る関節角列{θin}をインバートフィ
ルターを用いて求める。
【0202】CP曲線上における線素列{Δln}が
(D2)で求まっており、また、与えられたCP曲線を
サンプリング時間Tsで刻んだときの分割点の座標値
(Xn,Yn,Zn)の組、そして基準座標系の原点か
ら各分割点までの距離Rnは容易に計算することができ
るので、これらをインバートフィルターへの指令値とし
て入力すれば前述したように関節角列{θin}を求め
ることができ、複数回に亘るインバートフィルターの起
動によってこのような関節角列を各関節軸に関して得る
ことができる。
(D2)で求まっており、また、与えられたCP曲線を
サンプリング時間Tsで刻んだときの分割点の座標値
(Xn,Yn,Zn)の組、そして基準座標系の原点か
ら各分割点までの距離Rnは容易に計算することができ
るので、これらをインバートフィルターへの指令値とし
て入力すれば前述したように関節角列{θin}を求め
ることができ、複数回に亘るインバートフィルターの起
動によってこのような関節角列を各関節軸に関して得る
ことができる。
【0203】つまり、既述のインバートフィルター法を
CP曲線の分割点毎に小刻みに適用していけば良い。
CP曲線の分割点毎に小刻みに適用していけば良い。
【0204】そして、このときついでに角速度列{dθ
in/dt}や角加速度列{d2θin/dt2}を求め
るとともに、関節角列をサーボループ系に入力したとき
の位相エラーの列(これを{Δθin}と記す。)を計
算してメモリーに記憶しておく。尚、これらの列につい
ては列中のそれぞれの最大値だけをメモリーに記憶する
ようにしても良い。
in/dt}や角加速度列{d2θin/dt2}を求め
るとともに、関節角列をサーボループ系に入力したとき
の位相エラーの列(これを{Δθin}と記す。)を計
算してメモリーに記憶しておく。尚、これらの列につい
ては列中のそれぞれの最大値だけをメモリーに記憶する
ようにしても良い。
【0205】以上の処理によって得られる結果を視覚化
して例示したものが図19及び図20である。
して例示したものが図19及び図20である。
【0206】これらの図では簡単化のために図14に示
したようなアーム系に関する2次元的な動作を示してお
り、図19はアームの作用端位置rの時間的変化と速度
dr/dtの時間的変化とを対比して示すものである。
したようなアーム系に関する2次元的な動作を示してお
り、図19はアームの作用端位置rの時間的変化と速度
dr/dtの時間的変化とを対比して示すものである。
【0207】また、図20は関節角θ1、θ2の時間的
な変化についての一例を示すものである。
な変化についての一例を示すものである。
【0208】尚、両図における時刻teは、動作の終了
時点を示している。
時点を示している。
【0209】このようにして得られる関節角列{θi
n}がVpの仮値Vp0に基づいて算出されたものであ
ることは前述した通りであり、よってこのままの関節角
を用いることは、ロボットの運動能力を十分に引き出し
得ないので、Vp値について何らかの評価を行った後V
pの仮値に修正を施して再度関節角列を算出するといっ
た補正処理が必要となる。
n}がVpの仮値Vp0に基づいて算出されたものであ
ることは前述した通りであり、よってこのままの関節角
を用いることは、ロボットの運動能力を十分に引き出し
得ないので、Vp値について何らかの評価を行った後V
pの仮値に修正を施して再度関節角列を算出するといっ
た補正処理が必要となる。
【0210】(D4)そこで、CP制御の精度を評価量
としてVpの最適値を決定する方法について説明する。
としてVpの最適値を決定する方法について説明する。
【0211】関節角列は最終的にサーボ系への指令値と
して与えられるが、関節角θi(t)に対応する位相Θ
i(s)とエラー出力εi(t)に対応する位相エラー
ΔΘi(s)との間を結びつける関数をG(s)として
両者の関係が下式[数10]のように表されるものと定
義すると、これに逆ラプラス変換を施すことによって
[数11]式が得られる。
して与えられるが、関節角θi(t)に対応する位相Θ
i(s)とエラー出力εi(t)に対応する位相エラー
ΔΘi(s)との間を結びつける関数をG(s)として
両者の関係が下式[数10]のように表されるものと定
義すると、これに逆ラプラス変換を施すことによって
[数11]式が得られる。
【0212】
【数10】
【0213】
【数11】
【0214】[数11]式中の「L-1」は逆ラプラス演
算子であり、g(τ)はG(s)の逆ラプラス変換をと
ったものであるが、これはサーボループの構成が知られ
ていることから既知であり、よってΔθi(t)を[数
11]式を用いて計算することができる。
算子であり、g(τ)はG(s)の逆ラプラス変換をと
ったものであるが、これはサーボループの構成が知られ
ていることから既知であり、よってΔθi(t)を[数
11]式を用いて計算することができる。
【0215】尚、[数10]式では関節角を連続量とし
て扱ってきたが、データはサンプリングにより実際には
離散値となっており、離散系での数式表現で記述され
る。
て扱ってきたが、データはサンプリングにより実際には
離散値となっており、離散系での数式表現で記述され
る。
【0216】また、これらの値はサーボ系の応答として
実際に得られる結果を利用するものではなく、サーボ系
のシミュレーションとしてソフトウェア処理によって組
まれた仮想上のモータ(以下、「シミュレーションモー
ター」という。)を用いて計算されるか、もしくは[数
11]式を離散化した式を用いて計算される。
実際に得られる結果を利用するものではなく、サーボ系
のシミュレーションとしてソフトウェア処理によって組
まれた仮想上のモータ(以下、「シミュレーションモー
ター」という。)を用いて計算されるか、もしくは[数
11]式を離散化した式を用いて計算される。
【0217】θi(t)がある時刻tで決められる度に
Δθi(t)を求めると同時にこれを用いてCP曲線に
関する位置エラーε(t)を計算する。
Δθi(t)を求めると同時にこれを用いてCP曲線に
関する位置エラーε(t)を計算する。
【0218】これには、CP曲線の線速度と関節角速度
との間の関係式を用いる。
との間の関係式を用いる。
【0219】今、ロボットの作用端の位置ベクトルをベ
クトル「r」、関節角ベクトルをベクトル「θ」とする
と、両者の関係は関数Fを用いてr=F(θ)と関係付
けることができるので、これを時間tで微分することに
よって[数12]が得ることができる。
クトル「r」、関節角ベクトルをベクトル「θ」とする
と、両者の関係は関数Fを用いてr=F(θ)と関係付
けることができるので、これを時間tで微分することに
よって[数12]が得ることができる。
【0220】
【数12】
【0221】尚、上式中の∂F/∂θは所謂ヤコビアン
である。
である。
【0222】ベクトルdr/dtに係るパターン形状が
加減速曲線で与えられ、その最高値がVpである。
加減速曲線で与えられ、その最高値がVpである。
【0223】尚、[数12]式の上段式の両辺をパラメ
ーターαで割ることによって時間軸に関するスケーリン
グ操作を施したときに得られる下段の式を見ると、ベク
トルdr/dtとdθ/dtとが同じ比率(α)で伸縮
することに注意を要する(この事実は後に利用され
る。) Δθ=[Δθ1 Δθ2 ・・・]T(「T]は転値を
意味する。)のように列ベクトルで表現して、[数1
2]式を用いて位置エラーε(t)を下式のように定義
するとともに、ε(t)の最大値をεmaxとする。
ーターαで割ることによって時間軸に関するスケーリン
グ操作を施したときに得られる下段の式を見ると、ベク
トルdr/dtとdθ/dtとが同じ比率(α)で伸縮
することに注意を要する(この事実は後に利用され
る。) Δθ=[Δθ1 Δθ2 ・・・]T(「T]は転値を
意味する。)のように列ベクトルで表現して、[数1
2]式を用いて位置エラーε(t)を下式のように定義
するとともに、ε(t)の最大値をεmaxとする。
【0224】
【数13】
【0225】他方、ロボットにはCP制御の精度が仕様
によって決まっており(デフォルトト値)、これをεM
AXとすると、εmaxとεMAXとの大小関係によっ
てVpの評価を3つの場合に分けて考えることができ
る。
によって決まっており(デフォルトト値)、これをεM
AXとすると、εmaxとεMAXとの大小関係によっ
てVpの評価を3つの場合に分けて考えることができ
る。
【0226】i)εmax<εMAXの場合 仕様の精度以上のCP制御精度が実現されており、よっ
て、Vpの仮値をそのまま採用してロボットを動作させ
ても構わないが、CP制御の速度の面からはこの仮値で
は遅すぎる可能性がある。
て、Vpの仮値をそのまま採用してロボットを動作させ
ても構わないが、CP制御の速度の面からはこの仮値で
は遅すぎる可能性がある。
【0227】ii)εmax>εMAXの場合。
【0228】CP制御精度が仕様の精度を満足しておら
ず、Vpの仮値では速すぎるので、これより値を小さく
する必要がある。
ず、Vpの仮値では速すぎるので、これより値を小さく
する必要がある。
【0229】iii)εmax=εMAXの場合。
【0230】CP制御精度が仕様の精度を丁度満たして
おり、仮値も適度な速さである。
おり、仮値も適度な速さである。
【0231】以上からCP曲線の如何なる形状に対して
もロボットの能力を最大限に引き出すためにはiii)
の条件が必要十分条件となることが分かる。
もロボットの能力を最大限に引き出すためにはiii)
の条件が必要十分条件となることが分かる。
【0232】そして、Vp値の最適値を決定するには次
にような方法で行う。
にような方法で行う。
【0233】先ず、パラメーターαを用いた時間的なス
ケール変換t→α・tによってθi(s)がどのような
変換を受けるかについて調べることにする。
ケール変換t→α・tによってθi(s)がどのような
変換を受けるかについて調べることにする。
【0234】ラプラス変換の定義式からθi(t)を時
間軸に関してα倍したθi(α・t)にラプラス変換を
施したものを計算すると[数14]式のようになる。
間軸に関してα倍したθi(α・t)にラプラス変換を
施したものを計算すると[数14]式のようになる。
【0235】
【数14】
【0236】尚、計算の途中でα・t=τという置き換
えを行っている。
えを行っている。
【0237】[数14]式は、θi(t)を時間軸に関
してα倍にする操作が、θi(t)のラプラス表現Θi
(s)の大きさをα分の1にするとともに、周波数をα
分の1にする操作(ラプラス変数がs/αとなる)に対
応することを示している。
してα倍にする操作が、θi(t)のラプラス表現Θi
(s)の大きさをα分の1にするとともに、周波数をα
分の1にする操作(ラプラス変数がs/αとなる)に対
応することを示している。
【0238】図21はこの変換則を視覚的に表したもの
であり、Θi(s)、Θi(s/α)、Θi(s/α)
/αとの対比からゲインと周波数とがとともに圧縮され
ている様子が分かる。
であり、Θi(s)、Θi(s/α)、Θi(s/α)
/αとの対比からゲインと周波数とがとともに圧縮され
ている様子が分かる。
【0239】つまり、エラーΔΘi(t)を1/αに伸
縮したい場合には、上記スケール変換を施せば良く、こ
の変換によって下式[数15]に示すように関節角速度
をα分の1にすることができる。尚、このときG(s)
の変換性が問題にされない理由は、G(s)がサーボル
ープの特性そのものを表すものであり、従って、その周
波数特性を確保できるように設計される関係上特性を容
易に変化させることができないことに依っており、それ
故にG(s)の入力信号の時間軸の伸張操作によって出
力のゲイン調整を行うことが可能となる。
縮したい場合には、上記スケール変換を施せば良く、こ
の変換によって下式[数15]に示すように関節角速度
をα分の1にすることができる。尚、このときG(s)
の変換性が問題にされない理由は、G(s)がサーボル
ープの特性そのものを表すものであり、従って、その周
波数特性を確保できるように設計される関係上特性を容
易に変化させることができないことに依っており、それ
故にG(s)の入力信号の時間軸の伸張操作によって出
力のゲイン調整を行うことが可能となる。
【0240】
【数15】
【0241】[数12]式から関節角速度をα分の1に
することがCP曲線速度をα分の1にすることであるか
ら、αの値を適当に選ぶことで位置エラーε(t)の最
大値εmaxが仕様精度εMAXに等しくなるようにす
るVp値(これを「Vpmax」とする。)を決定する
ことができる。
することがCP曲線速度をα分の1にすることであるか
ら、αの値を適当に選ぶことで位置エラーε(t)の最
大値εmaxが仕様精度εMAXに等しくなるようにす
るVp値(これを「Vpmax」とする。)を決定する
ことができる。
【0242】つまり、下式[数16]に示すように、V
pの仮値に精度比εMAX/εmaxを掛けたものをV
pmaxとし、前述した(2)乃至(4)の処理を再度
行って関節角列を求めるようにすれば良い。
pの仮値に精度比εMAX/εmaxを掛けたものをV
pmaxとし、前述した(2)乃至(4)の処理を再度
行って関節角列を求めるようにすれば良い。
【0243】
【数16】
【0244】尚、上記したVp値に決定法について要約
すると次のようになる。
すると次のようになる。
【0245】位相エラーについての周波数軸での表現式
は[数10]式であり、また時間軸での表現は[数1
1]式で表されるが、CP曲線の位置エラーε(t)が
[数13]式に示すように位相エラーΔΘに関連してお
り両者の関係が線形的であることに注意すると、ε
(t)をα分の1にしたい場合には、位相エラーΔθi
をα分の1にすれば良いことが分かる。
は[数10]式であり、また時間軸での表現は[数1
1]式で表されるが、CP曲線の位置エラーε(t)が
[数13]式に示すように位相エラーΔΘに関連してお
り両者の関係が線形的であることに注意すると、ε
(t)をα分の1にしたい場合には、位相エラーΔθi
をα分の1にすれば良いことが分かる。
【0246】[数14]式から位相の周波数軸での表現
において該位相が時間的なスケール変換操作によってど
のような変化を見せるかが分かるので、[数10]によ
って位相と結びつけられる位相エラーのスケール変換則
が明かとなる。
において該位相が時間的なスケール変換操作によってど
のような変化を見せるかが分かるので、[数10]によ
って位相と結びつけられる位相エラーのスケール変換則
が明かとなる。
【0247】即ち、関節角度θi(t)を時間軸に関し
てα倍すると(つまり、θi(α・t))、位相エラー
Δθ(t)、位置エラーε(t)及びその最大値εma
xをα分の1にすることができる。
てα倍すると(つまり、θi(α・t))、位相エラー
Δθ(t)、位置エラーε(t)及びその最大値εma
xをα分の1にすることができる。
【0248】[数12]式によれば、このようなスケー
ル変換操作によって関節角度やCP曲線速度もα分の1
になることが分かるので、[数16]式に示すようにV
pの仮値とその適正値とを精度比に係るパラメーターα
によって結び付けることができる。
ル変換操作によって関節角度やCP曲線速度もα分の1
になることが分かるので、[数16]式に示すようにV
pの仮値とその適正値とを精度比に係るパラメーターα
によって結び付けることができる。
【0249】従って、与えられたCP曲線に対してロボ
ットのCP制御精度の仕様を丁度満たすようなCP曲線
速度を求めてロボットの能力を最大限に発揮させる制御
を実現することができる。
ットのCP制御精度の仕様を丁度満たすようなCP曲線
速度を求めてロボットの能力を最大限に発揮させる制御
を実現することができる。
【0250】尚、このときCP曲線に応じて加減速曲線
における最高速度が異なること、そして、制御精度が変
われば最高速度も変化すること(あるいは、制御精度を
人為的に操作すれば最高速度を故意に可変することがで
きる)は特筆すべき事項である。
における最高速度が異なること、そして、制御精度が変
われば最高速度も変化すること(あるいは、制御精度を
人為的に操作すれば最高速度を故意に可変することがで
きる)は特筆すべき事項である。
【0251】例えば、ユーザーがVp値及び精度εMA
Xを指定することができるようにシステムを構築したと
すると、このVp値に基づいた仮の加減速曲線を生成し
て、シミュレーションモーターを用いた計算処理によっ
てεmaxを求め、εmaxとεMAXとの大小関係に
応じてVpに修正を施すといったことが可能となる。
Xを指定することができるようにシステムを構築したと
すると、このVp値に基づいた仮の加減速曲線を生成し
て、シミュレーションモーターを用いた計算処理によっ
てεmaxを求め、εmaxとεMAXとの大小関係に
応じてVpに修正を施すといったことが可能となる。
【0252】(D5)Vpの仮値を決めて試行的に関節
角列を求め、精度を評価量として最適なVp値を決定し
たので、再び関節角列の算出に係る計算処理を行う段階
に入ることになるが、これは考え方としてこのような修
正処理を要するという意味であって、全く同じような計
算を実際に2回行わなければならないことを意味するも
のではない。
角列を求め、精度を評価量として最適なVp値を決定し
たので、再び関節角列の算出に係る計算処理を行う段階
に入ることになるが、これは考え方としてこのような修
正処理を要するという意味であって、全く同じような計
算を実際に2回行わなければならないことを意味するも
のではない。
【0253】例えば、仮値Vp0を用いて得られた関節
角列を利用し、これに時間軸補正を施すことによって、
Vpが最適値である場合に得られるであろう関節角列を
導出することが可能である。
角列を利用し、これに時間軸補正を施すことによって、
Vpが最適値である場合に得られるであろう関節角列を
導出することが可能である。
【0254】これには次のような方法を用いる。
【0255】関節角度列{θin}はサンプリング時間
Ts毎にサーボ系に送られるが、このTsはCPUやサ
ーボ系にとって基本的な時間であり、これを自由に可変
することができない。
Ts毎にサーボ系に送られるが、このTsはCPUやサ
ーボ系にとって基本的な時間であり、これを自由に可変
することができない。
【0256】そこで、下式のように定義される新たなサ
ンプリング時間tsをソフトウェア上で用意する。
ンプリング時間tsをソフトウェア上で用意する。
【0257】
【数17】
【0258】[数17]式の両辺をn倍すると、n・t
s=α・n・Tsとなり、Ts間隔のサンプリング毎に
tsずつのインクリメントがなされる。
s=α・n・Tsとなり、Ts間隔のサンプリング毎に
tsずつのインクリメントがなされる。
【0259】よって、時刻n・Tsのときにθi(n・
Ts)をサーボ系に送らずにθi(n・ts)=θi
(α・n・Ts)をサーボ系に送るようにすれば、時間
軸に関するα倍に伸縮操作が可能となる。
Ts)をサーボ系に送らずにθi(n・ts)=θi
(α・n・Ts)をサーボ系に送るようにすれば、時間
軸に関するα倍に伸縮操作が可能となる。
【0260】図22にθi(n・Ts)に関してα=2
とα=1/2の時間的な変倍操作を施したときの様子を
示す。
とα=1/2の時間的な変倍操作を施したときの様子を
示す。
【0261】尚、このような操作によって、求める関節
角列を得ることができるのは、前述したように加減速曲
線の生成に関しても時間的なスケール変換を利用してい
ることに密接な関係がある。
角列を得ることができるのは、前述したように加減速曲
線の生成に関しても時間的なスケール変換を利用してい
ることに密接な関係がある。
【0262】また、この方法は、CP制御速度を所望の
レベル、例えば、最高速を基準として1%刻みに速度を
制御するオーバーライド法に適用することも可能である
(尚、この場合同じデータがTs毎に数回に亘ってサー
ボ系に送られたり、逆にデータが間引かれたりするで
の、線形補間等を必要に応じて施こすことが望まし
い。)。
レベル、例えば、最高速を基準として1%刻みに速度を
制御するオーバーライド法に適用することも可能である
(尚、この場合同じデータがTs毎に数回に亘ってサー
ボ系に送られたり、逆にデータが間引かれたりするで
の、線形補間等を必要に応じて施こすことが望まし
い。)。
【0263】以上で関節角列{θin}を得ることがで
きたことになるが、加減速曲線が図10に示したような
パターンとなる場合において、関節角速度が、関節軸の
駆動モーターの最高速度を越えてしまうという虞れがあ
り、これに対処しないとモーターの破損等の事態を招く
ことになる。
きたことになるが、加減速曲線が図10に示したような
パターンとなる場合において、関節角速度が、関節軸の
駆動モーターの最高速度を越えてしまうという虞れがあ
り、これに対処しないとモーターの破損等の事態を招く
ことになる。
【0264】例えば、図23(a)に示すような階段状
の位相指令において、あるサンプリング時刻での値θi
n−1と次のサンプリング時刻での値θinとの差Δθ
iをサンプリング時間Tsで割ったものが角速度に相当
するが、このΔθiがモーターの最高速度に相当するΔ
θimaxより大きくなってしまう場合があり得る。
の位相指令において、あるサンプリング時刻での値θi
n−1と次のサンプリング時刻での値θinとの差Δθ
iをサンプリング時間Tsで割ったものが角速度に相当
するが、このΔθiがモーターの最高速度に相当するΔ
θimaxより大きくなってしまう場合があり得る。
【0265】図23(b)は、この状況をCP曲線上に
おいて示したものであり、n番目の点Pnとn+1番目
の点Pn+1との間の距離の差が許容値を越えている
(尚、図は問題点を理解し易いようにCP曲線と軌跡と
の間の差を強調して示している。)。
おいて示したものであり、n番目の点Pnとn+1番目
の点Pn+1との間の距離の差が許容値を越えている
(尚、図は問題点を理解し易いようにCP曲線と軌跡と
の間の差を強調して示している。)。
【0266】そこで、関節角速度の最高値を越えないよ
うに規制する方法とし以下に示す方法を採用する。
うに規制する方法とし以下に示す方法を採用する。
【0267】先ず、図24(a)に示すようにCP曲線
の全長LをN等分したときの座標値をメモリーに記憶す
る。図では、4軸に係る座標値(X,Y,Z,R)をそ
れぞれ記憶するN個分の座標値記憶用メモリー(つま
り、N×4の容量)が用意される例を示している。
の全長LをN等分したときの座標値をメモリーに記憶す
る。図では、4軸に係る座標値(X,Y,Z,R)をそ
れぞれ記憶するN個分の座標値記憶用メモリー(つま
り、N×4の容量)が用意される例を示している。
【0268】これら座標値記憶用メモリー内のデータに
ついて順番に前記したインバートフィルター部を通した
結果を関節角記憶用メモリーにストアする。尚、このと
きメモリー量が十分確保されている場合には座標値記憶
用メモリーと関節角記憶用メモリーとを別々に設けるこ
とができるが、メモリー量に制約がある場合には両メモ
リーを兼用とすれば良い。
ついて順番に前記したインバートフィルター部を通した
結果を関節角記憶用メモリーにストアする。尚、このと
きメモリー量が十分確保されている場合には座標値記憶
用メモリーと関節角記憶用メモリーとを別々に設けるこ
とができるが、メモリー量に制約がある場合には両メモ
リーを兼用とすれば良い。
【0269】次に、図 24(b)に示すように関節角
記憶用メモリーのデータについて仮速度Vp0の下で時
間軸補正処理(後に詳述する)を施した後、シミュレー
ションモーターに入力する。この処理は、各関節軸につ
いて同時に行われる。
記憶用メモリーのデータについて仮速度Vp0の下で時
間軸補正処理(後に詳述する)を施した後、シミュレー
ションモーターに入力する。この処理は、各関節軸につ
いて同時に行われる。
【0270】図24(b)にはシミュレーションモータ
ーの構成の一例をシグナルフローグラフで示しており、
ノードεi、ωiは指標iで特定される関節軸について
の位相エラーと関節角速度をそれぞれ示している。
ーの構成の一例をシグナルフローグラフで示しており、
ノードεi、ωiは指標iで特定される関節軸について
の位相エラーと関節角速度をそれぞれ示している。
【0271】このようなシミュレーションモーターをソ
フトウェア処理として組んでおけば、前記の畳み込み計
算や、位相エラー、速度、加速度等を必要に応じて得る
ことができるとともにノード値を容易に知ることができ
るので、ノードεi、ωiの値を常時監視していれば、
空間曲線精度εやωiの各最大値(これらを「εma
x」、「ωimax」とする。)を求めることができ
る。
フトウェア処理として組んでおけば、前記の畳み込み計
算や、位相エラー、速度、加速度等を必要に応じて得る
ことができるとともにノード値を容易に知ることができ
るので、ノードεi、ωiの値を常時監視していれば、
空間曲線精度εやωiの各最大値(これらを「εma
x」、「ωimax」とする。)を求めることができ
る。
【0272】ユーザーが指定する精度、角速度、Vpを
それぞれ「εMAX」、「ωiMAX」、「VpMA
X」とすると、下式[18]によって定義される「α
(ε)」、「α(ωi)」(i=1〜4)、「α(V
p)」のうちの最大値として[数19]式に示す「αm
ax」をもって時間軸変倍パラメーターの最終値とする
ことができ(これは[数12]式、[数13]式等によ
り保証される。)、これによって図23(b)で説明し
たような問題は生じなくなる。
それぞれ「εMAX」、「ωiMAX」、「VpMA
X」とすると、下式[18]によって定義される「α
(ε)」、「α(ωi)」(i=1〜4)、「α(V
p)」のうちの最大値として[数19]式に示す「αm
ax」をもって時間軸変倍パラメーターの最終値とする
ことができ(これは[数12]式、[数13]式等によ
り保証される。)、これによって図23(b)で説明し
たような問題は生じなくなる。
【0273】
【数18】
【0274】
【数19】
【0275】尚、[数18]式には関節角加速度、つま
りモーター電流についての制限を含めるようにしてもよ
い。
りモーター電流についての制限を含めるようにしてもよ
い。
【0276】図1及び図2は本発明に係る加減速パター
ン生成装置12の概要を示すものである。
ン生成装置12の概要を示すものである。
【0277】指令部13から発せられる指令は先ず、V
pの仮値Vp0を決定するVp0決定部14に送られる。
pの仮値Vp0を決定するVp0決定部14に送られる。
【0278】後段の加減速パターン生成/線素列計算部
15は、この仮値Vp0に基づいて線素列{Δln}を
求める部分であり、前述したアルゴリズムの各過程に対
応した曲線長計算部15a、Tp値決定部15b、パタ
ーン生成部15c、線素列計算部15dから構成されて
いる(図2参照)。
15は、この仮値Vp0に基づいて線素列{Δln}を
求める部分であり、前述したアルゴリズムの各過程に対
応した曲線長計算部15a、Tp値決定部15b、パタ
ーン生成部15c、線素列計算部15dから構成されて
いる(図2参照)。
【0279】つまり、線素列は前述したように曲線長L
に対応したTp値に基づいて生成される加減速曲線の形
状関数に従ってCP曲線をサンプリング時間刻みのセグ
メントに分割してそれぞれの長さを計算することによっ
て得られる(但し、後述するする時間軸補正処理によれ
ば、CP曲線の線素列はメモリー量に応じて等分割され
る。)。
に対応したTp値に基づいて生成される加減速曲線の形
状関数に従ってCP曲線をサンプリング時間刻みのセグ
メントに分割してそれぞれの長さを計算することによっ
て得られる(但し、後述するする時間軸補正処理によれ
ば、CP曲線の線素列はメモリー量に応じて等分割され
る。)。
【0280】そして、この線素列に基づいてインバート
フィルター部16での計算処理によって関節角列{θi
n}を求めてから、最適Vp値決定部17においてシミ
ュレーションモーターを使った計算に基づいてVpの適
正値を決定して線素列に対する補正計算を行った後、最
終的に加減速パターンに係る制御指令をサーボ系及びメ
カニズム系18に送出するようになっている。
フィルター部16での計算処理によって関節角列{θi
n}を求めてから、最適Vp値決定部17においてシミ
ュレーションモーターを使った計算に基づいてVpの適
正値を決定して線素列に対する補正計算を行った後、最
終的に加減速パターンに係る制御指令をサーボ系及びメ
カニズム系18に送出するようになっている。
【0281】尚、Vpの適正値に基づく補正計算は原理
的には加減速パターン生成/線素列計算部15とインバ
ートフィルター部16と通した再計算によって得ること
ができるが、前述したようにこれと同等の修正計算を時
間軸の伸張操作によって行うようにすれば、図1に2点
鎖線で示すように補正計算部19の計算結果をそのまま
サーボ系及びメカニズム系18に送出することができ
る。
的には加減速パターン生成/線素列計算部15とインバ
ートフィルター部16と通した再計算によって得ること
ができるが、前述したようにこれと同等の修正計算を時
間軸の伸張操作によって行うようにすれば、図1に2点
鎖線で示すように補正計算部19の計算結果をそのまま
サーボ系及びメカニズム系18に送出することができ
る。
【0282】また、インバートフィルター部16につい
ての構成例は図13に示した通りである。
ての構成例は図13に示した通りである。
【0283】次に、CP制御の速度に関しては以下のよ
うな可能性を考慮しておく必要がある。
うな可能性を考慮しておく必要がある。
【0284】これまでに説明した加減速曲線では、定速
期間での速度値が一定とされた制御を取り上げてきた
が、定速制御の途中で必要に応じて減速して停止へと移
行させたいような場合、あるいはCP曲線のあるセグメ
ントの曲率がゼロに近くて直線状をなしているときには
高速な制御を行い、曲率の大きいセグメントでは速度を
落とすようにした減り張りのある制御を行いたい場合が
ある。
期間での速度値が一定とされた制御を取り上げてきた
が、定速制御の途中で必要に応じて減速して停止へと移
行させたいような場合、あるいはCP曲線のあるセグメ
ントの曲率がゼロに近くて直線状をなしているときには
高速な制御を行い、曲率の大きいセグメントでは速度を
落とすようにした減り張りのある制御を行いたい場合が
ある。
【0285】以下では、この要請と上記した関節角速度
の制限に対する解決法として、サンプリング時間に変調
をかける方法(以下、「サンプリング時間変調法」とい
う。)について説明する。
の制限に対する解決法として、サンプリング時間に変調
をかける方法(以下、「サンプリング時間変調法」とい
う。)について説明する。
【0286】先ず、図25の加減速曲線に示すように、
加速後の定速期間において何らかの必要が生じて途中
(この時点を「T0」とし、その後の減速曲線を図では
破線で示す。)から減速させたい場合の減速制御につい
て説明する。
加速後の定速期間において何らかの必要が生じて途中
(この時点を「T0」とし、その後の減速曲線を図では
破線で示す。)から減速させたい場合の減速制御につい
て説明する。
【0287】例えば、非常停止がかかった場合や、閉曲
線上に沿ってロボットの作用端を周期的に多数回に亘っ
て回転させる制御の途中において閉曲線の任意に点で動
作を停止させたいような場合等を挙げることができる。
線上に沿ってロボットの作用端を周期的に多数回に亘っ
て回転させる制御の途中において閉曲線の任意に点で動
作を停止させたいような場合等を挙げることができる。
【0288】前述したように、逆運動学問題の解として
得られる関節角列{θin}はメモリーに記憶されてい
るため、このデータをそのままサーボ系に送出したので
は定速期間の途中から減速に移行させることができな
い。
得られる関節角列{θin}はメモリーに記憶されてい
るため、このデータをそのままサーボ系に送出したので
は定速期間の途中から減速に移行させることができな
い。
【0289】そこで、t=T0の時点以後においてサン
プリング時間間隔で図25に破線で示すような減速パタ
ーンを得るために関節角列{θin}を新たに計算しな
ければならない。
プリング時間間隔で図25に破線で示すような減速パタ
ーンを得るために関節角列{θin}を新たに計算しな
ければならない。
【0290】しかしながら、サンプリング時間Tsは固
定した値であるといった制約や、再計算時間に対する短
縮化の要求がある。
定した値であるといった制約や、再計算時間に対する短
縮化の要求がある。
【0291】そこで、CP制御の開始前に既に計算済の
関節角列{θin}をそのまま再利用することによって
減速パターンを生成することを可能にするアルゴリズム
を用いることにする。
関節角列{θin}をそのまま再利用することによって
減速パターンを生成することを可能にするアルゴリズム
を用いることにする。
【0292】定速期間を含む加減速曲線において定速期
間での速度をVp、Ts間の移動量をΔlとし、t=T
0以後における減速パターンについて時刻t=n・Ts
での速度をVEXP(n・Ts)とするとき、修正された
時間列{tsn}を下式のように定義する。
間での速度をVp、Ts間の移動量をΔlとし、t=T
0以後における減速パターンについて時刻t=n・Ts
での速度をVEXP(n・Ts)とするとき、修正された
時間列{tsn}を下式のように定義する。
【0293】
【数20】
【0294】尚、ここでVEXP(n・Ts)やVpは既
知である。
知である。
【0295】この時間列{tsn}の意味を図示したも
のが図26であり、時間例{tsn}は減速の過程で描
かれる速度VEXP(n・Ts)×時間tsnの面積がΔ
l(Ts間の移動量)に等しい面積となる(図では、傾
斜方向の異なる斜線部分の面積として表示している)よ
うに規定される時間列である。
のが図26であり、時間例{tsn}は減速の過程で描
かれる速度VEXP(n・Ts)×時間tsnの面積がΔ
l(Ts間の移動量)に等しい面積となる(図では、傾
斜方向の異なる斜線部分の面積として表示している)よ
うに規定される時間列である。
【0296】そこで、サンプリング時間を一定値ではな
く、この時間列{tsn}の間隔でもって関節角列をサ
ーボ系に送出すれば、正確な減速パターンを描かせるこ
とができるが、Tsは前述したように固定した値である
ため、図27に示すような方法を採ることにする。
く、この時間列{tsn}の間隔でもって関節角列をサ
ーボ系に送出すれば、正確な減速パターンを描かせるこ
とができるが、Tsは前述したように固定した値である
ため、図27に示すような方法を採ることにする。
【0297】図27は、t=T0での関節角を起点とし
てこれ以後に定速移動する関節列に軌跡とサンプリング
時間変調法による減速パターンの軌跡とを併せて示すも
のであり、横軸の時間軸tはTs刻みに区切られてお
り、縦軸に示す関節角の位相はTs毎にサンプリングさ
れている。
てこれ以後に定速移動する関節列に軌跡とサンプリング
時間変調法による減速パターンの軌跡とを併せて示すも
のであり、横軸の時間軸tはTs刻みに区切られてお
り、縦軸に示す関節角の位相はTs毎にサンプリングさ
れている。
【0298】そして、黒丸の印で示す位置が定速パター
ンの軌跡を示し、各区間で位相値が保持されて階段状に
なってる。
ンの軌跡を示し、各区間で位相値が保持されて階段状に
なってる。
【0299】また、図中三角印で示す位置は時間列{t
sn}を時間軸t上にとったときのt=tsnとΘ=Θ
nとの交点を示しており、上記[数20]式に対応する
減速パターンの軌跡を示している。この軌跡は時間列
{tsn}の各区間で位相値が保持されて階段状をした
パターンになっている。
sn}を時間軸t上にとったときのt=tsnとΘ=Θ
nとの交点を示しており、上記[数20]式に対応する
減速パターンの軌跡を示している。この軌跡は時間列
{tsn}の各区間で位相値が保持されて階段状をした
パターンになっている。
【0300】この軌跡をサンプリング時間Tsの間隔で
再度サンプリングすることによって近似したデータ列を
白丸印で示しており、サンプリング時間TsごとにΘn
の値の何回かに亘って保持しつつ、時間列{tsn}の
位相列に近い軌跡を得ることができる。
再度サンプリングすることによって近似したデータ列を
白丸印で示しており、サンプリング時間TsごとにΘn
の値の何回かに亘って保持しつつ、時間列{tsn}の
位相列に近い軌跡を得ることができる。
【0301】図27を見ても明かなように両者の差は最
大でTsと位相との積程度であるため比較的良い近似で
ある。
大でTsと位相との積程度であるため比較的良い近似で
ある。
【0302】図28はサンプリング時間変調法に関する
ブロック構成を概念的に示したものであり、逆運動学問
題の解がメモリー20に貯えられ、これらがTs間隔の
タイミングスイッチ21及び後段の0次ホールド部22
を経て、tsnのタイミングスイッチ23によって変調
がかけられた後(つまり、速度による変調)、さらに0
次ホールド部24、Ts間隔のタイミングスイッチ25
を介してサーボ系へと送られる。
ブロック構成を概念的に示したものであり、逆運動学問
題の解がメモリー20に貯えられ、これらがTs間隔の
タイミングスイッチ21及び後段の0次ホールド部22
を経て、tsnのタイミングスイッチ23によって変調
がかけられた後(つまり、速度による変調)、さらに0
次ホールド部24、Ts間隔のタイミングスイッチ25
を介してサーボ系へと送られる。
【0303】尚、0次ホールド部22、24を用いた理
由は処理の簡単化や高速化を考慮して現実の要請に答え
たためであり、一般には高次のホールドを用いることも
可能である。
由は処理の簡単化や高速化を考慮して現実の要請に答え
たためであり、一般には高次のホールドを用いることも
可能である。
【0304】関節角列θinはTs刻みにサンプリング
されて既にメモリーに格納されているので、加減速曲線
が決められると時間列{tsn}及びこれに対応した位
相列が図27に示すように決定され、tsnのタイミン
グでサンプルホールドされた階段状波形が得られる(図
27の三角印参照)。
されて既にメモリーに格納されているので、加減速曲線
が決められると時間列{tsn}及びこれに対応した位
相列が図27に示すように決定され、tsnのタイミン
グでサンプルホールドされた階段状波形が得られる(図
27の三角印参照)。
【0305】この波形を再度Ts刻みにサンプリングす
ることによって図27に白抜きの丸印で示した位相列が
得られることになる。
ることによって図27に白抜きの丸印で示した位相列が
得られることになる。
【0306】尚、時間列{tsn}の位相列と再サンプ
リングされた位相列との間の誤差をより小さくするには
線形補間を用いる(つまり、図28で後段側の0次ホー
ルド部24に補間機能をもたせる)とさらに近似が良く
なる。
リングされた位相列との間の誤差をより小さくするには
線形補間を用いる(つまり、図28で後段側の0次ホー
ルド部24に補間機能をもたせる)とさらに近似が良く
なる。
【0307】今、移動量をΔθとし移動時間Δtしたと
きの速度v=Δθ/Δtの変分δv(このδは変分を意
味し、前述した基本パラメーターのδとは異なる。)を
求めると、下式のようになる。
きの速度v=Δθ/Δtの変分δv(このδは変分を意
味し、前述した基本パラメーターのδとは異なる。)を
求めると、下式のようになる。
【0308】
【数21】
【0309】つまり、最大で(Δθ/(Δt)2)・T
sの変化が発生し、Δθに比例することが分かる。
sの変化が発生し、Δθに比例することが分かる。
【0310】この変化があまりに大きいと円滑な動作が
保証されないことになるので、これをなるべく小さくす
るために図29に示すような線形補間を施すことにす
る。
保証されないことになるので、これをなるべく小さくす
るために図29に示すような線形補間を施すことにす
る。
【0311】つまり、時刻n・Ts、(n+1)・Ts
での位相値がともにΘnで、時刻(n+2)・Tsでの
位相値がΘn+1である場合(図27に黒丸印で示
す。)、時刻(n+1)・Tsでの位相値をΘnとΘn
+1との中間に位置する値(図29に□印で示す。)に
なるようにすれば、CP制御の精度上問題なく円滑な動
作を実現することができる。
での位相値がともにΘnで、時刻(n+2)・Tsでの
位相値がΘn+1である場合(図27に黒丸印で示
す。)、時刻(n+1)・Tsでの位相値をΘnとΘn
+1との中間に位置する値(図29に□印で示す。)に
なるようにすれば、CP制御の精度上問題なく円滑な動
作を実現することができる。
【0312】要するにこの補間法は、[数21]式中の
Δθを小さくすることで速度の変動δvを抑えて動作に
滑らかにする方法であると言える。
Δθを小さくすることで速度の変動δvを抑えて動作に
滑らかにする方法であると言える。
【0313】尚、図27では縦軸に位相をとっている
が、精度の観点からは縦軸をCP曲線上の座標(X、
Y、Z)をとって上記したサンプリング時間変調法を適
用すればCP曲線に関する線形補間が可能となり、精度
の向上を期待することができる。勿論、メモリー容量を
増やすことが許されるのであれば、必要十分な精度を得
ることができる。
が、精度の観点からは縦軸をCP曲線上の座標(X、
Y、Z)をとって上記したサンプリング時間変調法を適
用すればCP曲線に関する線形補間が可能となり、精度
の向上を期待することができる。勿論、メモリー容量を
増やすことが許されるのであれば、必要十分な精度を得
ることができる。
【0314】さて、上記したサンプリング時間変調法を
用いて下記に示すような機能を実現することが可能とな
る。
用いて下記に示すような機能を実現することが可能とな
る。
【0315】(E1)閉曲線に沿う多回転制御後の円滑
な減速制御。
な減速制御。
【0316】(E2)CP制御速度のオーバーライド法
(速度可変法)。
(速度可変法)。
【0317】(E3)CP制御精度が一定となるように
CP制御速度を自由に可変すること。
CP制御速度を自由に可変すること。
【0318】(E4)加減速期間での移動距離に関する
ユーザー指定。
ユーザー指定。
【0319】以下では、この順番に説明を行うことにす
る。
る。
【0320】(E1)先ず、周期的なCP制御、つまり
閉曲線に沿って多数回の回転制御を行う場合、例えば、
2回目の回転からペーストを行うような場合には、その
回転の終了時点で塗布を止めれば、加減速パターンでの
ロボット動作に影響されることなく作業を行うことがで
きる。
閉曲線に沿って多数回の回転制御を行う場合、例えば、
2回目の回転からペーストを行うような場合には、その
回転の終了時点で塗布を止めれば、加減速パターンでの
ロボット動作に影響されることなく作業を行うことがで
きる。
【0321】図30は、CP曲線長L、加速期間Tpと
減速期間Tpでの移動量の和をlTpとした時のピーク速
度Vpの定速パターンを含む加減速曲線を示すものであ
る。
減速期間Tpでの移動量の和をlTpとした時のピーク速
度Vpの定速パターンを含む加減速曲線を示すものであ
る。
【0322】このような加減速曲線を何かに亘って接続
したのでは、作業に十分な定速期間を確保することがで
きないため、例えば塗装作業の場合には塗りムラが生じ
る等の不都合が生じることになる。
したのでは、作業に十分な定速期間を確保することがで
きないため、例えば塗装作業の場合には塗りムラが生じ
る等の不都合が生じることになる。
【0323】そこで、多回転制御では複数回に亘る回転
軌跡を一のCP曲線とみなすとともに、定速パターンで
の関節角列だけを用意しておき、加減速曲線の立ち上が
り時において上述したサンプリング時間変調法を使用す
れば、加減速時において同一のアルゴリズムを用いるこ
とができる。
軌跡を一のCP曲線とみなすとともに、定速パターンで
の関節角列だけを用意しておき、加減速曲線の立ち上が
り時において上述したサンプリング時間変調法を使用す
れば、加減速時において同一のアルゴリズムを用いるこ
とができる。
【0324】尚、加速時や減速時における時間列{ts
n}は、TpやlTpの値が予め知られていることからC
P制御の開始前に計算して求めるようにしても良いし、
また、Ts間にリアルタイム処理で行うようにしても良
い。
n}は、TpやlTpの値が予め知られていることからC
P制御の開始前に計算して求めるようにしても良いし、
また、Ts間にリアルタイム処理で行うようにしても良
い。
【0325】(E2)次にCP制御に関するオーバーラ
イド法について説明する。
イド法について説明する。
【0326】図30に示すような加減速パターンに従っ
てCP制御を行う場合、制御速度を、最高速に関して自
由に可変するには下記にような2種類の制御方法を用い
ることができる。
てCP制御を行う場合、制御速度を、最高速に関して自
由に可変するには下記にような2種類の制御方法を用い
ることができる。
【0327】その一は、総移動時間をTとしたときに、
これをp%だけ増加したい場合には時間軸を100/p
倍に操作する方法である。
これをp%だけ増加したい場合には時間軸を100/p
倍に操作する方法である。
【0328】つまり、新しいサンプリング時間を「Ts
p」とするとき、下式のように計算すれば良い。
p」とするとき、下式のように計算すれば良い。
【0329】
【数22】
【0330】但し、何度も繰り返すようにサンプリング
時間Tsは固定値であるため、この場合にもサンプリン
グ時間変調法を適用する。
時間Tsは固定値であるため、この場合にもサンプリン
グ時間変調法を適用する。
【0331】尚、この方法では、総移動時間を所定の比
率、例えば1%刻みで可変させることができ、これに伴
って速度も減少させることができるが、その速度が最高
速に関してどれくらいの速度まで落ちているかを容易に
知ることができないという難点がある。
率、例えば1%刻みで可変させることができ、これに伴
って速度も減少させることができるが、その速度が最高
速に関してどれくらいの速度まで落ちているかを容易に
知ることができないという難点がある。
【0332】これに対してCP制御速度を直接に可変す
ることの方が、実用面では多いと考えられ、もう一の方
法はこのような状況に対応することができるものであ
る。今、オーバーライドをかける前の時刻t=n・Ts
での速度をv(n・Ts)とし、その時の位相値をΘ
(n・Ts)とすると、p%のオーバーライドをかけた
ときの速度はV(n・Ts)・p/100となる。
ることの方が、実用面では多いと考えられ、もう一の方
法はこのような状況に対応することができるものであ
る。今、オーバーライドをかける前の時刻t=n・Ts
での速度をv(n・Ts)とし、その時の位相値をΘ
(n・Ts)とすると、p%のオーバーライドをかけた
ときの速度はV(n・Ts)・p/100となる。
【0333】よって、軌跡が与えられたCP曲線に沿う
ようにオーバーライドをかけるための修正されたサンプ
リング時間tsnは下式に示すようにして求めることが
できる。
ようにオーバーライドをかけるための修正されたサンプ
リング時間tsnは下式に示すようにして求めることが
できる。
【0334】
【数23】
【0335】このようにして時間列{tsn}が求めら
れたならば、後はサンプリング変調法のアルゴリズムを
そのまま適用すれば良い。
れたならば、後はサンプリング変調法のアルゴリズムを
そのまま適用すれば良い。
【0336】(E3)CP制御では、一般に軌跡や速度
の精度が重要な評価要素とされ、ロボットの作業時間帯
は加減速曲線の定速パターン部とされることが多い。
の精度が重要な評価要素とされ、ロボットの作業時間帯
は加減速曲線の定速パターン部とされることが多い。
【0337】しかし、ユーザーはCP曲線上を制御開始
点から終了点に向かってある精度内で可能な限り速い速
度でロボットを動作させたい状況に遭遇することがある
と考えられる。
点から終了点に向かってある精度内で可能な限り速い速
度でロボットを動作させたい状況に遭遇することがある
と考えられる。
【0338】つまり、CP曲線上の直線状のセグメント
では速度を上げて高速にトレースし、曲がりの程度が大
きいセグメントでは速度を落として低速でトレースする
ような制御を可能にすることが望まれる。
では速度を上げて高速にトレースし、曲がりの程度が大
きいセグメントでは速度を落として低速でトレースする
ような制御を可能にすることが望まれる。
【0339】図31はCP曲線の指令軌跡と実際の軌跡
との誤差ベクトルや諸量の定義を示すものである。
との誤差ベクトルや諸量の定義を示すものである。
【0340】図中の点SがCP曲線の始点を示し、点E
が終点を示している。
が終点を示している。
【0341】ベクトルε(t)はCP曲線と軌跡との間
の誤差ベクトルを時間の関数として表したものであり、
その大きさは|ε(t)|≧0である。
の誤差ベクトルを時間の関数として表したものであり、
その大きさは|ε(t)|≧0である。
【0342】また、原点OからCP曲線上の任意の点に
向けて引いたベクトルr(t)が、その点の位置ベクト
ルを示している。
向けて引いたベクトルr(t)が、その点の位置ベクト
ルを示している。
【0343】尚、モータの位相エラーをΔΘ(t)=
[ΔΘ1 ΔΘ2 ・・・]Tとし、サーボ系について位
相入力−出力間の関係を規定する伝達関数をG(s)と
する。
[ΔΘ1 ΔΘ2 ・・・]Tとし、サーボ系について位
相入力−出力間の関係を規定する伝達関数をG(s)と
する。
【0344】また、εMAXをCP制御の位置精度と
し、ユーザーがティーチングペンダント等を使って指定
することができる精度をεuとする。
し、ユーザーがティーチングペンダント等を使って指定
することができる精度をεuとする。
【0345】[数11]式及び[数13]式から分かる
ように、|ε(t)|の値は畳み込み計算結果Δθとヤ
コビアンとの積の絶対値として下式のように表される。
ように、|ε(t)|の値は畳み込み計算結果Δθとヤ
コビアンとの積の絶対値として下式のように表される。
【0346】
【数24】
【0347】尚、図32に|ε(t)|の時間的な変化
の一例をグラフ化して示す。
の一例をグラフ化して示す。
【0348】時間軸に関する変倍用パラメーターをα
(t)としこれが時間に依存するものと考えると、[数
24]式の両辺をこのα(t)で割った式をユーザー指
定の精度εuに等しいとして下式を得ることができる。
(t)としこれが時間に依存するものと考えると、[数
24]式の両辺をこのα(t)で割った式をユーザー指
定の精度εuに等しいとして下式を得ることができる。
【0349】
【数25】
【0350】つまり、時間軸に関して時間に依存するパ
ラメーターα(t)の変倍操作を施すことによってεu
を一定化させることができる。
ラメーターα(t)の変倍操作を施すことによってεu
を一定化させることができる。
【0351】よって、[数12]式の上段式の両辺をα
(t)で割ることによって得られる下式[数26]か
ら、CP制御速度がεu=一定という条件下に変化する
ことになる。
(t)で割ることによって得られる下式[数26]か
ら、CP制御速度がεu=一定という条件下に変化する
ことになる。
【0352】
【数26】
【0353】例えば、位置エラーε(t)が大きいとき
にはα(t)が大きな値となるので低速になる。
にはα(t)が大きな値となるので低速になる。
【0354】このような時間軸に関する操作によって変
調をかけることによってサンプリング時間Tsに対して
新たな時間tnが下式のように得られる。
調をかけることによってサンプリング時間Tsに対して
新たな時間tnが下式のように得られる。
【0355】
【数27】
【0356】この新たな時間列{tn}に基づいてサン
プリング時間変調法を適用すれば、所期の目的を達成す
るこができる。
プリング時間変調法を適用すれば、所期の目的を達成す
るこができる。
【0357】尚、その際、tnには限界値を設けておか
ないと|ε(t)|=0のときにα(t)=0となるの
で動作に支障を来たすことになるので注意を要する。
ないと|ε(t)|=0のときにα(t)=0となるの
で動作に支障を来たすことになるので注意を要する。
【0358】次に、これまで留保してきたピーク速度V
pの仮値の決定方法及びこれとサンプリング時間変調法
との関係について説明する。
pの仮値の決定方法及びこれとサンプリング時間変調法
との関係について説明する。
【0359】Vpの決定がメモリー容量と密接な関係を
もっていることは、Vpが小さいときのメモリー消費量
が多いことから伺い知ることができる。
もっていることは、Vpが小さいときのメモリー消費量
が多いことから伺い知ることができる。
【0360】以下では、Vpの仮値Vp0、最適速度V
pOPTとメモリー容量の関係について説明する。
pOPTとメモリー容量の関係について説明する。
【0361】尚、下表に記号とその意味を定義する。
【0362】
【表3】
【0363】今、図33に示すように始点S、終点Eの
CP曲線に沿って同一の速度Vp0で移動制御する場合
(勿論これはあくまでも仮想上のものである。)を仮定
すると、総移動時間Tall 0は曲線長LをVpの仮値Vp
0で割ったものであり、下式のようになる。
CP曲線に沿って同一の速度Vp0で移動制御する場合
(勿論これはあくまでも仮想上のものである。)を仮定
すると、総移動時間Tall 0は曲線長LをVpの仮値Vp
0で割ったものであり、下式のようになる。
【0364】
【数28】
【0365】よって、CP曲線はサンプリング時間Ts
の間隔で下式に示すようにNn個に分割されることにな
る。
の間隔で下式に示すようにNn個に分割されることにな
る。
【0366】
【数29】
【0367】そして、このNnが1軸当たりのメモリー
容量Nmに丁度等しいならば、メモリーを最大限に使っ
て最も精度良く曲線を分割したことになる。
容量Nmに丁度等しいならば、メモリーを最大限に使っ
て最も精度良く曲線を分割したことになる。
【0368】つまり、下式のように仮値Vp0を決定す
れば良い。
れば良い。
【0369】
【数30】
【0370】尚、曲線長Lが小さい場合には、上式[数
30]から得られる値をそのまま用いると、確かに分割
精度は高いが計算時間がかかることになるため、Lの最
大値Lmaxを予め決めておき、この値Lmaxのとき
にメモリー容量Nmが消費されるようにし、下式に示す
ように単位長さ当たりのメモリー容量nmを決定するよ
うにしても良い。
30]から得られる値をそのまま用いると、確かに分割
精度は高いが計算時間がかかることになるため、Lの最
大値Lmaxを予め決めておき、この値Lmaxのとき
にメモリー容量Nmが消費されるようにし、下式に示す
ように単位長さ当たりのメモリー容量nmを決定するよ
うにしても良い。
【0371】
【数31】
【0372】このように仮値Vp0を決定すれば、CP
曲線に対して単位長さ当たりの分割数を一定化すること
ができ、曲線長の短いCP曲線についての計算時間を短
縮することができる。
曲線に対して単位長さ当たりの分割数を一定化すること
ができ、曲線長の短いCP曲線についての計算時間を短
縮することができる。
【0373】次にサンプリング時間変調法による加減速
パターンの発生について説明する。上記のようにNm個
に分割されたCP曲線上の各分割点について得られる関
節角列{θin}はインバートフィルター部での処理後
にメモリーに記憶される。
パターンの発生について説明する。上記のようにNm個
に分割されたCP曲線上の各分割点について得られる関
節角列{θin}はインバートフィルター部での処理後
にメモリーに記憶される。
【0374】関節角列{θin}の添字iが各関節軸を
区別し、nが分割点を特定していることから明らかなよ
うに、この列はNm×i個の要素からなる集合である。
区別し、nが分割点を特定していることから明らかなよ
うに、この列はNm×i個の要素からなる集合である。
【0375】よって、この集合の中からnが同一となる
要素、例えば、4軸のリンク構成の場合には(θ1n、
θ2n、θ3n、θ4n)を取り出してサーボ系に送れ
ば、図33に示したCP曲線におけるn番目の分割点に
作用端を移動させることができる。
要素、例えば、4軸のリンク構成の場合には(θ1n、
θ2n、θ3n、θ4n)を取り出してサーボ系に送れ
ば、図33に示したCP曲線におけるn番目の分割点に
作用端を移動させることができる。
【0376】また、Ts間隔で関節角列のデータをサー
ボ系に送れば、作用点が一定速度Vp0でCP曲線上を
移動することになる。
ボ系に送れば、作用点が一定速度Vp0でCP曲線上を
移動することになる。
【0377】そして、集合{θin}からある時間間隔
でデータを取り出してサーボ系に送ればこの時間間隔に
応じた速度で作用点をCP曲線に沿って移動させること
ができる。
でデータを取り出してサーボ系に送ればこの時間間隔に
応じた速度で作用点をCP曲線に沿って移動させること
ができる。
【0378】例えば、n=1から順に2・Tsの間隔で
データをサーボ系に送れば図33のCP曲線上をVp/
2の速度で作用点を移動させることができる。
データをサーボ系に送れば図33のCP曲線上をVp/
2の速度で作用点を移動させることができる。
【0379】そこで、加減速曲線が[数4]式で示した
ような形状をなす場合について加減速期間における時間
間隔tsnを求めてみる。
ような形状をなす場合について加減速期間における時間
間隔tsnを求めてみる。
【0380】図34に示す加減速曲線において、Tpm
inはTp値の最小値であり、下式のようにサンプリン
グ時間TsのK(整数)倍となるように選んでおく。
inはTp値の最小値であり、下式のようにサンプリン
グ時間TsのK(整数)倍となるように選んでおく。
【0381】
【数32】
【0382】Tpmin間における移動量をlminと
するとこれは次式のようになる。
するとこれは次式のようになる。
【0383】
【数33】
【0384】尚、この式は前述した式lp=Vp・δ
(デルタ)(Tp)を使って得られるものである。
(デルタ)(Tp)を使って得られるものである。
【0385】よって、0≦t≦Tpmin間の移動距離
l(t)と、速度dl(t)/dtは下式に示すように
なる。
l(t)と、速度dl(t)/dtは下式に示すように
なる。
【0386】
【数34】
【0387】尚、lmin(t)は加減速曲線の標準関
数であり、例えば[数2]式のような形をしている。
数であり、例えば[数2]式のような形をしている。
【0388】時間列は、[数34]式の速度に関する関
係式においてt=n・Tsのときに時間tsn内の移動
距離が仮値Vp0のTs間にける移動距離に等しいとお
いて下式のように得ることができる。
係式においてt=n・Tsのときに時間tsn内の移動
距離が仮値Vp0のTs間にける移動距離に等しいとお
いて下式のように得ることができる。
【0389】
【数35】
【0390】従って、時間列tsnに従って集合{θi
n}からn番目のデータをサーボ系に送れば、作用点が
指定されたCP曲線上をTpminの加速期間をもって
立ち上がることになる。
n}からn番目のデータをサーボ系に送れば、作用点が
指定されたCP曲線上をTpminの加速期間をもって
立ち上がることになる。
【0391】t=tsnにおけるθ値をθin(ts
n)と記すことにすると、これが直ちにサーボ系に送ら
れる訳ではなく、Vpの最適値の決定に係る前述した処
理を経た後時間的なスケール変換(t→α・t)を受け
て、さらにTsによる再サンプリング処理を施された上
で最終的にサーボ系に送出される。
n)と記すことにすると、これが直ちにサーボ系に送ら
れる訳ではなく、Vpの最適値の決定に係る前述した処
理を経た後時間的なスケール変換(t→α・t)を受け
て、さらにTsによる再サンプリング処理を施された上
で最終的にサーボ系に送出される。
【0392】尚、Tsによる再サンプリング処理を受け
る前にθinについての補間処理を必要に応じて行うこ
とは何等構わない。
る前にθinについての補間処理を必要に応じて行うこ
とは何等構わない。
【0393】この一連の処理において注目すべき点は、
仮値Vp0に基づいて集合{θin}を計算した後は、
サンプリング時間変調法により時間軸に関する操作を行
うだけでθinの最終結果を得ることができるという点
である。
仮値Vp0に基づいて集合{θin}を計算した後は、
サンプリング時間変調法により時間軸に関する操作を行
うだけでθinの最終結果を得ることができるという点
である。
【0394】[数35]式以後の処理を具体的に説明す
ると、[数11]式の畳み込み式に基づいてエラーΔθ
i(t)を各関節軸毎に算出する(又はシミュレーショ
ンモーターに{θin}の列を速度Vp0に従って入力
することによってΔθi(t)を算出する。)。
ると、[数11]式の畳み込み式に基づいてエラーΔθ
i(t)を各関節軸毎に算出する(又はシミュレーショ
ンモーターに{θin}の列を速度Vp0に従って入力
することによってΔθi(t)を算出する。)。
【0395】Δθ=[Δθ1 Δθ2 ・・・]Tのときの
作用端のエラーε(t)、つまりCP制御精度は、例え
ば、[数13]式の上段の式のように定義され、これを
α分の1にした場合には[数13]式の下段の式を用い
れば良い。
作用端のエラーε(t)、つまりCP制御精度は、例え
ば、[数13]式の上段の式のように定義され、これを
α分の1にした場合には[数13]式の下段の式を用い
れば良い。
【0396】処理は時間的なスケール変換によって実現
されるが、具体的にはサンプリング変調法のアルゴリズ
ムを適用する。
されるが、具体的にはサンプリング変調法のアルゴリズ
ムを適用する。
【0397】そして、Vpの最適値を[数16]式に従
って(Vp→Vp0に置き換えて考えれば良い。)決定
した後は関節角列に係る時間軸の変倍操作をやはりサン
プリング時間変調法を用いて行うことができる。
って(Vp→Vp0に置き換えて考えれば良い。)決定
した後は関節角列に係る時間軸の変倍操作をやはりサン
プリング時間変調法を用いて行うことができる。
【0398】こうして、図34のような加減速曲線が得
られることになる。
られることになる。
【0399】尚、この例ではTp=Tpminの場合に
ついて説明したが、一般のTp値への適用にあたっては
加減速曲線の標準関数に対する時間的なスケール変換を
考慮すれば容易であり、また減速パターンについては時
間的な折り返しによる対称操作を適用すれば良いことは
前述した通りであるので説明を割愛する。
ついて説明したが、一般のTp値への適用にあたっては
加減速曲線の標準関数に対する時間的なスケール変換を
考慮すれば容易であり、また減速パターンについては時
間的な折り返しによる対称操作を適用すれば良いことは
前述した通りであるので説明を割愛する。
【0400】(E4)ユーザーは一般に加減速曲線の定
速期間においてロボットに作業をさせようとする場合が
ほとんどであり、加速期間や減速期間は過渡的な期間と
考えられる。
速期間においてロボットに作業をさせようとする場合が
ほとんどであり、加速期間や減速期間は過渡的な期間と
考えられる。
【0401】このため、加速期間や減速期間での移動距
離Tp・Vp/2を計算して定速期間における移動距離
を把握したいと要求に答えるようなユーザーインターフ
ェースを用意しておくことが好ましい。
離Tp・Vp/2を計算して定速期間における移動距離
を把握したいと要求に答えるようなユーザーインターフ
ェースを用意しておくことが好ましい。
【0402】そこで、ユーザーが加速期間や減速期間で
の移動距離lTpを指定できるものとし、Vpの適正値を
このlTpやCP曲線の形状に応じて自動的に計算する方
法を以下に説明する。このとき必要に応じて総移動時間
やVpをユーザーに知らせることとする。
の移動距離lTpを指定できるものとし、Vpの適正値を
このlTpやCP曲線の形状に応じて自動的に計算する方
法を以下に説明する。このとき必要に応じて総移動時間
やVpをユーザーに知らせることとする。
【0403】尚、本発明に係る加減速曲線の形状に特別
な制限は課せられないので説明の簡単化を図るために、
台形波状の加減速曲線を例にして説明する。
な制限は課せられないので説明の簡単化を図るために、
台形波状の加減速曲線を例にして説明する。
【0404】先ず、新たに導入するパラメーターの意味
について説明すると、「N」が各関節軸に対するメモリ
ー容量(ワード単位)を意味し、このN値が大きい程滑
らかなCP曲線を描かせることができる。
について説明すると、「N」が各関節軸に対するメモリ
ー容量(ワード単位)を意味し、このN値が大きい程滑
らかなCP曲線を描かせることができる。
【0405】Vpの仮値Vp0は下式を満たすように決
定する。
定する。
【0406】
【数36】
【0407】図35に示す台形波状の加減速曲線におい
てユーザーが指定する移動距離lTpは定速期間(これを
「T□」とする。)を除く期間での移動距離に当たるの
で、これが、同図に破線で示すように加速パターンと減
速パターンをそれぞれ示す斜辺と時間軸との間に形成さ
れる2等辺三角形の面積に等しいことから下式のように
表すことができる。
てユーザーが指定する移動距離lTpは定速期間(これを
「T□」とする。)を除く期間での移動距離に当たるの
で、これが、同図に破線で示すように加速パターンと減
速パターンをそれぞれ示す斜辺と時間軸との間に形成さ
れる2等辺三角形の面積に等しいことから下式のように
表すことができる。
【0408】
【数37】
【0409】尚、図35における「Tcp」はCP制御
にかかる総移動時間である。
にかかる総移動時間である。
【0410】ユーザーが作業内容に応じてCP制御の精
度εMAXを指定することができ、これとシミュレーシ
ョンモーターを使った計算からVpの適正値を得る事は
前述した通りである。
度εMAXを指定することができ、これとシミュレーシ
ョンモーターを使った計算からVpの適正値を得る事は
前述した通りである。
【0411】ところで、移動量lTpや、総移動時間Tc
pは一般にサンプリング時間Tsの整数倍とはなってい
ないため、ここにもサンプリング時間変調法が必要とな
る。今、lTpやVpを僅かに変化させることで、Tpや
TcpをTsの整数倍にすることを考える。
pは一般にサンプリング時間Tsの整数倍とはなってい
ないため、ここにもサンプリング時間変調法が必要とな
る。今、lTpやVpを僅かに変化させることで、Tpや
TcpをTsの整数倍にすることを考える。
【0412】つまり、修正後のピーク速度をVp′と
し、lTpをlTp′として下式のように定義する。
し、lTpをlTp′として下式のように定義する。
【0413】
【数38】
【0414】上式中の「np」や「n□」は正の整数で
あり、次式に示すように決定する。
あり、次式に示すように決定する。
【0415】
【数39】
【0416】但し、[x]はxを越えない整数で定義さ
れる所謂ガウスの記号である。
れる所謂ガウスの記号である。
【0417】[数38]式からVp′、lTp′を求める
と下式のようになる。
と下式のようになる。
【0418】
【数40】
【0419】尚、この式はn□=0の場合でも成立し、
このようにしてVp′、lTp′を決定すればTsに同期
した加減速パターンを得ることができる(図36参
照。)。
このようにしてVp′、lTp′を決定すればTsに同期
した加減速パターンを得ることができる(図36参
照。)。
【0420】lTp′はユーザーの指定したlTp値と異な
るが、両者の差が小さいためユーザーの意図から大きく
外れることはなく、よってユーザーの量的な感覚を大幅
に狂わせるといった弊害にそれほど神経を尖らせる必要
はない。
るが、両者の差が小さいためユーザーの意図から大きく
外れることはなく、よってユーザーの量的な感覚を大幅
に狂わせるといった弊害にそれほど神経を尖らせる必要
はない。
【0421】以上で台形波状の加減速パターンがTsと
の同期関係をもって得られたので次に時間列tsnの求
め方を、加速期間、定速期間、減速期間の3通りに場合
分けして説明する。
の同期関係をもって得られたので次に時間列tsnの求
め方を、加速期間、定速期間、減速期間の3通りに場合
分けして説明する。
【0422】i)加速期間(0≦t≦np・Ts)。
【0423】この期間における速度をVac(t)とす
ると、図36の加速パターンの傾斜が一定であることか
ら時間tに比例した下式で表される。
ると、図36の加速パターンの傾斜が一定であることか
ら時間tに比例した下式で表される。
【0424】
【数41】
【0425】よって、図36の加速パターンのある時点
で移動量が、この時点までの加減速曲線と時間軸との間
に囲まれた面積に相当することから、CP曲線に係るn
番目の線素長Δlnが今の例では一定(これを「Δl」
とする。)であることに注意するとtsnが下式のよう
に求められる。
で移動量が、この時点までの加減速曲線と時間軸との間
に囲まれた面積に相当することから、CP曲線に係るn
番目の線素長Δlnが今の例では一定(これを「Δl」
とする。)であることに注意するとtsnが下式のよう
に求められる。
【0426】
【数42】
【0427】ii)定速期間(np・Ts≦t≦(np
+n□)・Ts) Vp′=一定であり、加減速曲線に関する面積の関係か
ら下式に示すようにtsnが求められる。
+n□)・Ts) Vp′=一定であり、加減速曲線に関する面積の関係か
ら下式に示すようにtsnが求められる。
【0428】
【数43】
【0429】iii)減速期間((np+n□)・Ts
≦t≦(2・np+n□)・Ts) この期間に速度をVdc(t)とすると、下式のように
表すことができる。
≦t≦(2・np+n□)・Ts) この期間に速度をVdc(t)とすると、下式のように
表すことができる。
【0430】
【数44】
【0431】よって、これまでの議論と同様にして加減
速曲線に係る面積の関係からtsnを求めることができ
るが、加減速パターンがTsで同期していることと加速
パターンと減速パターンとの間の時間的な対称性に着目
すれば、[数42]式を利用してして容易にtsnを求
めることができる。
速曲線に係る面積の関係からtsnを求めることができ
るが、加減速パターンがTsで同期していることと加速
パターンと減速パターンとの間の時間的な対称性に着目
すれば、[数42]式を利用してして容易にtsnを求
めることができる。
【0432】
【数45】
【0433】以上でサンプリング時間変調法における時
間列{tsn}が求められ、再サンプリングの後に関節
角度列を得てサーボ系に送れば良いことになる。
間列{tsn}が求められ、再サンプリングの後に関節
角度列を得てサーボ系に送れば良いことになる。
【0434】図37に示すグラフ図は上段に加減速曲線
を示すt−V図を配置すると共に、下段には、距離ln
をΔl単位で表し(縦軸)、時間軸(横軸)をTsを単
位として表したものである。尚、説明の簡単化を図るた
めにN=18としている。
を示すt−V図を配置すると共に、下段には、距離ln
をΔl単位で表し(縦軸)、時間軸(横軸)をTsを単
位として表したものである。尚、説明の簡単化を図るた
めにN=18としている。
【0435】図中の原点Oと点P0を結ぶ線分OP0は仮
速度値Vp0でロボットの作用点を移動させたときの軌
跡を示すもので仮想上の線である。
速度値Vp0でロボットの作用点を移動させたときの軌
跡を示すもので仮想上の線である。
【0436】また、原点Oと点P0を結ぶ曲線OPは台
形波状の加減速曲線に従ってロボットの作用点を移動さ
せたときの軌跡を示すものである。
形波状の加減速曲線に従ってロボットの作用点を移動さ
せたときの軌跡を示すものである。
【0437】曲線OP上の白抜きの丸印は[数42]式
乃至[数45]式を用いて算出した時刻tsnでの位置
を示している。
乃至[数45]式を用いて算出した時刻tsnでの位置
を示している。
【0438】図38は図37に白抜き丸印で示すデータ
をt=n・Ts、つまりTs刻みで再度サンプリングし
て得られる修正された位置(三角印で示す)をプロット
したものである。
をt=n・Ts、つまりTs刻みで再度サンプリングし
て得られる修正された位置(三角印で示す)をプロット
したものである。
【0439】この三角印で示す位置データをサーボ系に
渡せばCP曲線に近似した軌跡を描かせることができる
が、揺らぎの度合を低減するためには前述したように線
形補間を用いる。
渡せばCP曲線に近似した軌跡を描かせることができる
が、揺らぎの度合を低減するためには前述したように線
形補間を用いる。
【0440】図38の黒丸印で示す位置が線形補間を施
された後の位置を示している。
された後の位置を示している。
【0441】つまり、時刻t=n・Tsに関して時間的
に前にくるtsnの時点をtsn−とし、後にくるts
nの時点をtsn+と定義すると、下式に示すように前
後の位置の間を比例分割した位置である。
に前にくるtsnの時点をtsn−とし、後にくるts
nの時点をtsn+と定義すると、下式に示すように前
後の位置の間を比例分割した位置である。
【0442】
【数46】
【0443】こうして得られる位置データをサーボ系に
送出ことで、より正確にCP曲線を描くことが可能とな
る。
送出ことで、より正確にCP曲線を描くことが可能とな
る。
【0444】次に、あるCP曲線と別のCP曲線との接
続に関する問題を説明する。
続に関する問題を説明する。
【0445】これまでは、一のCP曲線に関する説明を
主として行ってきたが、実際の場面では複数のCP曲線
セグメントを繋いだ曲線に沿ってロボットの作用端の移
動させる場合が多い。
主として行ってきたが、実際の場面では複数のCP曲線
セグメントを繋いだ曲線に沿ってロボットの作用端の移
動させる場合が多い。
【0446】図39は、曲率が異なる複数の円弧を接続
することによって一の制御曲線が形成される例を示して
おり、始点Sから終点Eにかけて5つの円弧C1、C
2、C3、C4、C5が連結されている。
することによって一の制御曲線が形成される例を示して
おり、始点Sから終点Eにかけて5つの円弧C1、C
2、C3、C4、C5が連結されている。
【0447】本発明では、始点Sから終点Eにかけて位
置する各曲線について各別に加減速パターンを生成し、
これらを連結したパターンとしてCP制御を行うのでは
なく、これらを一のCP曲線として取り扱うことでこれ
に対する一の加減速パターンを生成するようにしてい
る。
置する各曲線について各別に加減速パターンを生成し、
これらを連結したパターンとしてCP制御を行うのでは
なく、これらを一のCP曲線として取り扱うことでこれ
に対する一の加減速パターンを生成するようにしてい
る。
【0448】これによって、各CP曲線の接続部での衝
撃の緩和や動作の円滑化を図ることができる。
撃の緩和や動作の円滑化を図ることができる。
【0449】勿論、直線状のセグメントと円弧状をした
セグメントとの接続についても同様の方法を用いること
ができる。
セグメントとの接続についても同様の方法を用いること
ができる。
【0450】最後に上記したサンプリング時間変調法と
PTP動作との関係について付言する。
PTP動作との関係について付言する。
【0451】前述したようにCP制御に係る各種のアル
ゴリズムはPTP動作のそれと全く無関係ではないこと
について触れたが、両動作がロボットの作用端をある軌
跡に沿って、かつある加減速パターンに従いながら移動
させる制御であるという観点からするとアルゴリズムに
共通性を見出すことができる。
ゴリズムはPTP動作のそれと全く無関係ではないこと
について触れたが、両動作がロボットの作用端をある軌
跡に沿って、かつある加減速パターンに従いながら移動
させる制御であるという観点からするとアルゴリズムに
共通性を見出すことができる。
【0452】即ち、サンプリング時間変調法を介するこ
とによって両動作を統一化されたアルゴリズムで取り扱
うことが可能となる。
とによって両動作を統一化されたアルゴリズムで取り扱
うことが可能となる。
【0453】以下にPTP制御をサンプリング時間変調
法のアルゴリズムに従って実現させる場合の手順を説明
する。
法のアルゴリズムに従って実現させる場合の手順を説明
する。
【0454】(1)先ず、ロボットの手先位置について
現在位置と目標位置が制御装置に与えられる。つまり、
現在位置はロボットの状態を常時監視していることから
既知であり、目標位置は教示操作等によって指示され
る。
現在位置と目標位置が制御装置に与えられる。つまり、
現在位置はロボットの状態を常時監視していることから
既知であり、目標位置は教示操作等によって指示され
る。
【0455】(2)目標位置の座標情報をインバートフ
ィルターにセットして関節角の変位量を求める(X軸、
Y軸、Z軸の各成分をΔΘx、ΔΘy、ΔΘzとし、ツ
ール先端の回動軸成分をΔΘrとする。)。
ィルターにセットして関節角の変位量を求める(X軸、
Y軸、Z軸の各成分をΔΘx、ΔΘy、ΔΘzとし、ツ
ール先端の回動軸成分をΔΘrとする。)。
【0456】(3)移動量(ΔΘx,ΔΘy,ΔΘz,
ΔΘr)をそれぞれN等分した値をN個のメモリー番地
(ワード単位)にストアする。
ΔΘr)をそれぞれN等分した値をN個のメモリー番地
(ワード単位)にストアする。
【0457】換言すれば、図40に示すように各成分に
関して一定の角速度の組み(ωx0,ωy0,ωz0,ω
r0)でもってロボットの作用端を仮想的に移動させる
ように位置データ列をN個のメモリー番地に格納するこ
とである(つまり、4・Nワードを使用する。)。
関して一定の角速度の組み(ωx0,ωy0,ωz0,ω
r0)でもってロボットの作用端を仮想的に移動させる
ように位置データ列をN個のメモリー番地に格納するこ
とである(つまり、4・Nワードを使用する。)。
【0458】尚、このN等分計算は短時間で行えるので
メモリーへの格納は必須ではないが、CP制御との共通
性を重視する意味でこの方法を採用する。
メモリーへの格納は必須ではないが、CP制御との共通
性を重視する意味でこの方法を採用する。
【0459】(4)各軸の移動量を考慮して加減速パタ
ーンやTp等を決定する。
ーンやTp等を決定する。
【0460】尚、この手続きとサンプリング時間変調法
とは全く独立したものであることは、これまで説明した
ように後者が加減速パターンの確定以後に係るアルゴリ
ズムであることから明らかである。
とは全く独立したものであることは、これまで説明した
ように後者が加減速パターンの確定以後に係るアルゴリ
ズムであることから明らかである。
【0461】(5)得られた加減速パターンに関してT
p値や総移動時間がTsの整数倍には必ずしもなってい
ないことから、角速度のピーク値に若干の修正を施して
Tsと同期関係を築き加減速パターンの修正を行う。
p値や総移動時間がTsの整数倍には必ずしもなってい
ないことから、角速度のピーク値に若干の修正を施して
Tsと同期関係を築き加減速パターンの修正を行う。
【0462】これには[数40]式でV→ω、L→Δθ
への置換を適宜に行えば良い。
への置換を適宜に行えば良い。
【0463】(6)4軸中の1軸についての位置デー
タ、例えば、X軸成分ΔΘxに注目し、これについてサ
ンプリング時間変調法により[数42]式乃至[数4
3]式相当の計算を行う。
タ、例えば、X軸成分ΔΘxに注目し、これについてサ
ンプリング時間変調法により[数42]式乃至[数4
3]式相当の計算を行う。
【0464】つまり、各式中ではΔl→Δθx(=ΔΘ
x/N)、L→ΔΘx等の置換を行えば良い。
x/N)、L→ΔΘx等の置換を行えば良い。
【0465】(7)tsnに基づいてTsでの再サンプ
リング処理を施せば、最終的にTsとの同期関係をもっ
たデータ列を得ることができ、4軸ともに同一のTp
で、かつ同時スタート、同時ストップのPTP動作を実
現することができる。
リング処理を施せば、最終的にTsとの同期関係をもっ
たデータ列を得ることができ、4軸ともに同一のTp
で、かつ同時スタート、同時ストップのPTP動作を実
現することができる。
【0466】尚、これまでサンプリング時間変調法の適
用例として図28で説明したように速度パターンによる
変調を行ったが、CP曲線の曲率に基づいて変調をかけ
ることによって直線路では速く、カーブでは遅くすると
いった制御を行ったり、あるいは、双腕型ロボットのよ
うに協調作業を行う場合には他のロボットやアームの速
度又は距離等に基づいた変調をかけることによってアー
ム同士の干渉を避けることができる。
用例として図28で説明したように速度パターンによる
変調を行ったが、CP曲線の曲率に基づいて変調をかけ
ることによって直線路では速く、カーブでは遅くすると
いった制御を行ったり、あるいは、双腕型ロボットのよ
うに協調作業を行う場合には他のロボットやアームの速
度又は距離等に基づいた変調をかけることによってアー
ム同士の干渉を避けることができる。
【0467】また、各種のセンサー出力(温度、圧力、
力等)、モーターの電流値等に応じて変調をかけること
によってロボットの状態を考慮した制御を実現すること
が可能である。
力等)、モーターの電流値等に応じて変調をかけること
によってロボットの状態を考慮した制御を実現すること
が可能である。
【0468】最後に付言すると、本実施例によればCP
制御、PTP制御のいずれの場合にも手先位置がメモリ
ーによって管理されているので、手先の移動範囲に対し
て許容範囲を設定することでセイフティゾーンを設け、
手先位置が不用意に許容範囲から逸脱しないように規制
をかけるといったことが可能となり、これによって安全
対策について万全を期すことができる。
制御、PTP制御のいずれの場合にも手先位置がメモリ
ーによって管理されているので、手先の移動範囲に対し
て許容範囲を設定することでセイフティゾーンを設け、
手先位置が不用意に許容範囲から逸脱しないように規制
をかけるといったことが可能となり、これによって安全
対策について万全を期すことができる。
【0469】
【発明の効果】以上に記載したところから明らかなよう
に、本発明によれば、CP制御において制御曲線の長さ
に応じて速度ピーク時点が規定される加減速パターンを
生成するとともに、CP曲線を所定の時間間隔で小刻み
に分割して線素列を算出し、この線素列と制御曲線上の
位置指令に基づいて逆運動学問題の解としての関節状態
を知ることができる。
に、本発明によれば、CP制御において制御曲線の長さ
に応じて速度ピーク時点が規定される加減速パターンを
生成するとともに、CP曲線を所定の時間間隔で小刻み
に分割して線素列を算出し、この線素列と制御曲線上の
位置指令に基づいて逆運動学問題の解としての関節状態
を知ることができる。
【0470】その際、本発明に係る逆運動学問題の解析
にあたっては、順運動学問題の解法に用いる演算要素を
ネガティブフィードバックフィルター内に設けることに
よってロボット構造を反映させ、上記線素列と曲線上の
位置指令をフィルターにセットして関節軸に対応した回
数だけ起動し、該フィルターの収束を待って演算要素の
入力段から逆運動学問題解を求めることができる。
にあたっては、順運動学問題の解法に用いる演算要素を
ネガティブフィードバックフィルター内に設けることに
よってロボット構造を反映させ、上記線素列と曲線上の
位置指令をフィルターにセットして関節軸に対応した回
数だけ起動し、該フィルターの収束を待って演算要素の
入力段から逆運動学問題解を求めることができる。
【0471】従って、ロボット系を構成するアーム等の
組み合わせが如何なるものになろうと各関節軸を同等に
取り扱うことができ、ロボットの構造上の違いに起因し
て加減速パターン生成に係るアルゴリズムに大幅な変更
を余儀なくされるといった不都合が解消される。
組み合わせが如何なるものになろうと各関節軸を同等に
取り扱うことができ、ロボットの構造上の違いに起因し
て加減速パターン生成に係るアルゴリズムに大幅な変更
を余儀なくされるといった不都合が解消される。
【0472】そして、アルゴリズムが理論的に明確であ
り、制御の善し悪しが設計者の能力や経験等に左右され
やすいといったが不都合はなく、加減速パターンの生成
に係るアルゴリムの汎用性が高まり、また、計算処理を
四則演算や三角関数計算程度で行うことができるため計
算時間の短縮化やCPUの処理負担の軽減を図ることが
できる。
り、制御の善し悪しが設計者の能力や経験等に左右され
やすいといったが不都合はなく、加減速パターンの生成
に係るアルゴリムの汎用性が高まり、また、計算処理を
四則演算や三角関数計算程度で行うことができるため計
算時間の短縮化やCPUの処理負担の軽減を図ることが
できる。
【0473】また、本発明では作用点を指定された制御
曲線に沿って精度良く移動させるために標本化に際して
時間軸補正を行うことによって加減速曲線の生成や速度
制御等に要する計算処理上の負担を減らして処理の高速
化を図ることができる他、この時間軸補正を介在させる
ことによってCP制御のアルゴリズムとPTP制御のア
ルゴリズムとの統一化を図ることができる。
曲線に沿って精度良く移動させるために標本化に際して
時間軸補正を行うことによって加減速曲線の生成や速度
制御等に要する計算処理上の負担を減らして処理の高速
化を図ることができる他、この時間軸補正を介在させる
ことによってCP制御のアルゴリズムとPTP制御のア
ルゴリズムとの統一化を図ることができる。
【0474】尚、前期した実施例によって本発明の技術
的範囲が狭く解釈される訳ではなく、本発明は工作機械
や各種の駆動機構等に広く適用し得ることは勿論であ
る。
的範囲が狭く解釈される訳ではなく、本発明は工作機械
や各種の駆動機構等に広く適用し得ることは勿論であ
る。
【図1】本発明に係る加減速パターン生成装置の構成例
を示すブロック図である。
を示すブロック図である。
【図2】加減速パターン生成/線素列計算部の構成を示
すブロック図である。
すブロック図である。
【図3】CP曲線の一例を示す図である。
【図4】定速パターンを含む加減速曲線を示すグラフ図
である。
である。
【図5】定速パターンを含まない加減速曲線を示すグラ
フ図である。
フ図である。
【図6】加減速曲線の生成に係る処理を示すフローチャ
ート図である。
ート図である。
【図7】(a)は曲線長Lと速度ピーク到達時間との関
係を示すグラフ図、(b)は曲線長Lと移動時間Tmと
の関係を示すグラフ図である。
係を示すグラフ図、(b)は曲線長Lと移動時間Tmと
の関係を示すグラフ図である。
【図8】距離(0≦L≦Lmin)に応じた加減速曲線
の変化を示すグラフ図である。
の変化を示すグラフ図である。
【図9】距離(Lmin≦L≦Lmax)に応じた加減
速曲線の変化を示すグラフ図である。
速曲線の変化を示すグラフ図である。
【図10】距離(Lmax≦L)に応じた加減速曲線の
変化を示すグラフ図である。
変化を示すグラフ図である。
【図11】基本パラメーターδ(Tp)の物理的意味に
ついて示すグラフ図である。
ついて示すグラフ図である。
【図12】スカラ型ロボットに関する座標系の設定例を
示す図である。
示す図である。
【図13】インバートフィルターの構成例を示すブロッ
ク線図である。
ク線図である。
【図14】2軸アーム系をモデル化して示す略線図であ
る。
る。
【図15】インバートフィルター処理に対応したロボッ
トアームの動きを(a)から(c)へと順を追って示す
図である。
トアームの動きを(a)から(c)へと順を追って示す
図である。
【図16】ワーク上の任意位置に関するCP制御につい
て説明するためにロボットの動きを示す図である。
て説明するためにロボットの動きを示す図である。
【図17】垂直多関節型ロボットに関する手先位置の移
動について説明するための図である。
動について説明するための図である。
【図18】加減速曲線の生成方法及び逆運動学問題の解
法について総括的に示すフローチャート図である。
法について総括的に示すフローチャート図である。
【図19】アームの作用点位置及び速度の時間的な変化
の一例を示すグラフ図である。
の一例を示すグラフ図である。
【図20】関節角θ1及びθ2の時間的な変化を示すグ
ラフ図である。
ラフ図である。
【図21】時間的なスケール変換に係るΘi(s)の変
換則について示すグラフ図である。
換則について示すグラフ図である。
【図22】θi(n・Ts)に関する時間的なスケール
変換について説明するためのグラフ図である。
変換について説明するためのグラフ図である。
【図23】(a)は階段状をした位相指令の時間的変化
を示すグラフ図、(b)はCP曲線上において点Pnと
点Pn+1との間隔について示す図である。
を示すグラフ図、(b)はCP曲線上において点Pnと
点Pn+1との間隔について示す図である。
【図24】(a)はCP曲線の分割と座標記憶用メモリ
ーとの関係を示す概念図、(b)は関節角記憶用メモリ
ーから時間軸補正処理を経てシミュレーションモーター
に至る処理の流れを示す図である。
ーとの関係を示す概念図、(b)は関節角記憶用メモリ
ーから時間軸補正処理を経てシミュレーションモーター
に至る処理の流れを示す図である。
【図25】加減速曲線において加速後の定速期間の途中
で減速に移行させる必要が生じた様子を示すグラフ図で
ある。
で減速に移行させる必要が生じた様子を示すグラフ図で
ある。
【図26】修正された時間列{tsn}の意味について
説明するためのグラフ図である。
説明するためのグラフ図である。
【図27】サンプリング時間変調法による減速パターン
の軌跡を説明するためのグラフ図である。
の軌跡を説明するためのグラフ図である。
【図28】サンプリング時間変調法について概念的に説
明するためのブロック図である。
明するためのブロック図である。
【図29】線形補間についての説明図である。
【図30】加減速曲線の一例を示す図である。
【図31】CP曲線に関する指令軌跡と実際の軌跡と誤
差等を示す図である。
差等を示す図である。
【図32】エラー|ε(t)|の時間的な変化の一例を
示すグラフ図である。
示すグラフ図である。
【図33】CP曲線上を一定の速度で作用点を移動させ
る仮想上の制御を示す図である。
る仮想上の制御を示す図である。
【図34】サンプリング時間変調法を用いて得られる加
減速曲線の一例を示すグラフ図である。
減速曲線の一例を示すグラフ図である。
【図35】台形波状の加減速パターンを示すグラフ図で
ある。
ある。
【図36】TpやTcpがサンプリング時間の整数倍と
なるように修正された台形状の加減速パターンを示すグ
ラフ図である。
なるように修正された台形状の加減速パターンを示すグ
ラフ図である。
【図37】台形状の加減速パターンと作用点の軌跡とを
示すグラフ図である。
示すグラフ図である。
【図38】図37の作用点に対して修正された軌跡を台
形状の加減速パターンとともに示すグラフ図である。
形状の加減速パターンとともに示すグラフ図である。
【図39】曲率が異なる複数の円弧の接続について説明
するための図である。
するための図である。
【図40】PTP制御とサンプリング時間変調法との関
係において一定化された角速度を示すグラフ図である。
係において一定化された角速度を示すグラフ図である。
a 制御曲線 1、5、9、10、11 加減速パターン 12 加減速パターン生成装置 13 指令手段 14 速度ピーク仮値決定手段 15 加減速パターン生成/線素列計算手段 16 逆運動学問題解析手段 17 最適速度ピーク値決定手段 19 補正手段 15a 曲線長計算手段 15b 速度ピーク時点決定手段 15c パターン生成手段 15d 線素列計算手段
フロントページの続き (58)調査した分野(Int.Cl.7,DB名) G05B 19/416 G05B 19/18 B25J 13/00
Claims (7)
- 【請求項1】 作用点に対する制御の開始点と終了点及
び/又は制御曲線に関する指令を発する指令手段と、指
令手段からの信号に応じて制御曲線上での速度ピーク値
の仮値を決定する速度ピーク仮値決定手段と、速度ピー
ク値の仮値に基づいて加減速パターンを生成するととも
に制御曲線を多数の微小な長さのセグメントに分割して
線素列を得る加減速パターン生成/線素列計算手段と、
順運動学問題を解く際に座標系間の変換に使用される同
次変換行列に対応した演算要素を含むネガティブフィー
ドバックのフィルター構成とされ、作用点に対する指令
位置を与えてこれに対応する関節軸の状態を導き出す逆
運動学問題解析手段と、速度ピーク値の仮値に基づいて
逆運動学問題解析手段により得られる関節状態について
の解をソフトウェア処理によって現実のサーボ系を模し
て構成される仮想サーボ系に与えることによって制御精
度を求めるとともにこれが規定又は指定の精度に一致す
るように速度ピーク値の最適値を決定する最適速度ピー
ク値決定手段と、速度ピーク値の最適値に基づいて関節
状態に係る再計算又は修正計算を行う補正手段とを備え
たことを特徴とする加減速パターン生成装置。 - 【請求項2】 請求項1に記載の加減速パターン生成装
置において、逆運動学問題解析手段が、関節状態から作
用点の位置を求める順運動学問題を解く際に使用される
同次変換行列に対応した演算要素と、動的なゲイン調整
のための乗数要素と、ループ安定化のための積分要素
と、ループの収束速度を調整するのための定数要素とを
含むネガティブフィードバックのフィルター構成を有
し、逆運動学問題解析手段への位置情報の入力に対し
て、演算要素の前段から関節状態の解を得ることができ
るようにしたことを特徴とする加減速パターン生成装
置。 - 【請求項3】 請求項1又は請求項2に記載の加減速パ
ターン生成装置において、加減速パターン生成/線素列
計算手段が、制御曲線の曲線長を計算する曲線長計算手
段と、加減速曲線における速度のピーク時点を曲線長に
応じて決定する速度ピーク時点決定手段と、速度のピー
ク時点に関する指令を受けて関節軸に係る加速パターン
を生成するとともに、減速パターンの生成に際しては加
速パターンに対して時間的な対称操作を施すことによっ
て時間的に折り返されたパターンとして減速パターンを
生成するパターン生成手段と、加減速曲線の形状に応じ
て制御曲線を所定の時間刻みに分割することによって線
素列を計算する線素列計算手段とを備えたことを特徴と
する加減速パターン生成装置。 - 【請求項4】 作用点を指定された制御曲線に沿って移
動制御させるべく加減速パターンを生成する加減速パタ
ーン生成方法であって、 (イ)先ず、制御曲線に係る計算時間の最小単位を決め
た後、 (ロ)制御曲線の曲線長を求めて、該曲線長に対応した
速度のピーク時点を決定して加速パターンと減速パター
ンとが時間的な対称性を有する加減速パターンを生成す
るとともに、速度ピーク値の仮値をメモリー容量の許容
値に基づいて決定して制御曲線を計算時間の単位時間間
隔でもって分割することよって線素列を導出し、 (ハ)(ロ)の線素列と作用点の位置指令に係る情報に
基づいて、順運動学問題を解く際に座標系間の変換に使
用される同次変換行列に対応した演算要素を含むネガテ
ィブフィードバックのフィルター計算を行うことによ
り、作用点の位置を与えてこれに対応した関節軸の状態
を求める逆運動学問題の解を求め、 (ニ)(ハ)で得られる関節状態についての解をソフト
ウェア処理によって現実のサーボ系を模して構成される
仮想サーボ系に与えることによって制御精度を求めると
ともにこれが規定又は指定の精度に一致するように速度
ピーク値の最適値を決定し、 (ホ)速度ピーク値の最適値に基づいて関節状態の解に
係る(ロ)及び(ハ)の再計算又はこれに相当する修正
計算を行った結果をサーボ系への制御指令として送出す
るようにした、ことを特徴とする加減速パターン生成方
法。 - 【請求項5】 指定された制御曲線に沿っての移動が予
定される作用点の位置に対応した関節軸の状態を求める
逆運動学問題の解法であって、(イ)関節軸の状態に対
応した作用点の位置を求める順運動学問題の解法にあた
って使用される同次変換行列に対応した演算要素と、動
的なゲイン調整のための乗数要素と、ループ安定化のた
めの積分要素と、ループの収束速度を調整するための定
数要素とを含むネガティブフィードバックのフィルター
を構成し、(ロ)制御曲線を最小計算時間間隔をもって
分割することによって得られる線素列と作用点の位置情
報を(イ)のフィルターへの入力情報として与えてフィ
ルターに起動をかけ、ループでの収束を待って演算要素
の前段から関節軸の状態を求め、(ハ)(イ)、(ロ)
の手順を各関節軸に対して繰り返すことによって最終的
な逆運動学問題の解としての関節状態を求めるようにし
た、ことを特徴とする逆運動学問題の解法。 - 【請求項6】 請求項4に記載の加減速パターン生成方
法において、(ハ)における逆運動学問題の解法とし
て、請求項5に記載の解法を用いたことを特徴とする加
減速パターン生成方法。 - 【請求項7】 請求項4に記載の加減速パターン生成方
法に使用され、計算時間が最小単位をもって稼動される
制御システムに関して、作用点の軌跡が指令された制御
曲線に所定の精度内で近似した曲線を描くように移動制
御するための時間軸補正方法であって、 (イ)加減速曲線において速度のピーク時点や総移動時
間が、計算時間の最小単位の整数倍の値となるように速
度ピーク値や加速及び減速期間での移動距離に修正を施
して計算時間の最小単位をもって関節状態を示すデータ
列を標本化した後最小単位時間の間隔をもって該データ
を保持し、 (ロ)制御曲線を計算時間の最小単位の時間間隔でもっ
て分割して得られる線素列の長さと、加減速曲線の形状
との間に成立する距離の関係に基づく計算によって関節
状態を示すデータ列に対応した新たな時間列を計算時間
の最小単位にとらわれることなく生成し、 (ハ)(ロ)で得られた新たな時間列と関節状態を示す
データ列の組に対して、計算時間の最小単位による再標
本化を施すことによって時間的に等間隔に配置された関
節状態の情報を得るようにした、ことを特徴とする時間
軸補正方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP19470192A JP3341305B2 (ja) | 1992-06-30 | 1992-06-30 | 加減速パターン生成装置及び加減速パターン生成方法、並びにこれに用いる逆運動学問題の解法と時間軸補正方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP19470192A JP3341305B2 (ja) | 1992-06-30 | 1992-06-30 | 加減速パターン生成装置及び加減速パターン生成方法、並びにこれに用いる逆運動学問題の解法と時間軸補正方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
JPH0619528A JPH0619528A (ja) | 1994-01-28 |
JP3341305B2 true JP3341305B2 (ja) | 2002-11-05 |
Family
ID=16328831
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP19470192A Expired - Fee Related JP3341305B2 (ja) | 1992-06-30 | 1992-06-30 | 加減速パターン生成装置及び加減速パターン生成方法、並びにこれに用いる逆運動学問題の解法と時間軸補正方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP3341305B2 (ja) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106945042A (zh) * | 2017-03-29 | 2017-07-14 | 浙江工业大学 | 一种机械手的离散s型曲线速度控制方法 |
KR20190112644A (ko) * | 2018-03-26 | 2019-10-07 | 니혼 덴산 가부시키가이샤 | 로봇 제어 장치, 로봇 제어 방법, 프로그램 |
Families Citing this family (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2003001576A (ja) | 2001-06-25 | 2003-01-08 | Matsushita Electric Ind Co Ltd | ロボット制御装置およびその制御方法 |
JP5349478B2 (ja) * | 2007-08-28 | 2013-11-20 | ザ ユニバーシティー オブ サリー | 逆運動学 |
DE102009049172B4 (de) * | 2009-10-13 | 2019-07-25 | Kuka Roboter Gmbh | Verfahren und Vorrichtung zur Steuerung eines Manipulators |
JP5417392B2 (ja) * | 2011-07-29 | 2014-02-12 | 新日本工機株式会社 | 数値制御装置 |
JP2013225284A (ja) * | 2012-04-20 | 2013-10-31 | Linestream Technologies | 動作システムのパラメータ推定法、そのパラメータ推定システム、動作プロファイルの生成方法およびその生成システム |
CN106625680B (zh) * | 2017-02-07 | 2019-07-09 | 华侨大学 | 一种冗余度机械臂加速度层容噪控制方法 |
CN108237535A (zh) * | 2018-02-02 | 2018-07-03 | 广东工业大学 | 一种机械臂及其控制***与方法 |
JP7047524B2 (ja) * | 2018-03-26 | 2022-04-05 | 日本電産株式会社 | ロボット制御装置、ロボット制御方法、プログラム |
JP7199952B2 (ja) * | 2018-12-18 | 2023-01-06 | 日本電産サンキョー株式会社 | ロボットの制御装置及び制御方法 |
JPWO2022158428A1 (ja) * | 2021-01-20 | 2022-07-28 | ||
CN113186665B (zh) * | 2021-04-02 | 2022-09-06 | 浙江工业大学 | 一种缝纫伺服电机的平滑速度规划控制方法 |
CN117111549B (zh) * | 2023-07-20 | 2024-05-31 | 江南大学 | 一种食品、医药生产工艺逆向调控方法 |
CN118170030A (zh) * | 2024-04-19 | 2024-06-11 | 英孚康(浙江)工业技术有限公司 | 变换系数自适应的运控***退绕式旋转轴超柔性控制方法 |
-
1992
- 1992-06-30 JP JP19470192A patent/JP3341305B2/ja not_active Expired - Fee Related
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106945042A (zh) * | 2017-03-29 | 2017-07-14 | 浙江工业大学 | 一种机械手的离散s型曲线速度控制方法 |
CN106945042B (zh) * | 2017-03-29 | 2019-05-28 | 浙江工业大学 | 一种机械手的离散s型曲线速度控制方法 |
KR20190112644A (ko) * | 2018-03-26 | 2019-10-07 | 니혼 덴산 가부시키가이샤 | 로봇 제어 장치, 로봇 제어 방법, 프로그램 |
KR102297735B1 (ko) | 2018-03-26 | 2021-09-03 | 니혼 덴산 가부시키가이샤 | 로봇 제어 장치, 로봇 제어 방법, 프로그램 |
Also Published As
Publication number | Publication date |
---|---|
JPH0619528A (ja) | 1994-01-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP3341305B2 (ja) | 加減速パターン生成装置及び加減速パターン生成方法、並びにこれに用いる逆運動学問題の解法と時間軸補正方法 | |
Yuwen et al. | Path, feedrate and trajectory planning for free-form surface machining: A state-of-the-art review | |
Tajima et al. | Accurate interpolation of machining tool-paths based on FIR filtering | |
Beudaert et al. | 5-Axis tool path smoothing based on drive constraints | |
Beudaert et al. | Feedrate interpolation with axis jerk constraints on 5-axis NURBS and G1 tool path | |
US7146242B2 (en) | Method and system for generating multi-dimensional motion profiles | |
US5434489A (en) | Method and system for path planning in cartesian space | |
US7180253B2 (en) | Method and system for generating multi-dimensional motion profiles | |
KR101798652B1 (ko) | 매니퓰레이터를 제어하기 위한 방법 및 장치 | |
CA1202101A (en) | Manipulator with controlled path motion | |
Liu et al. | FIR filter-based continuous interpolation of G01 commands with bounded axial and tangential kinematics in industrial five-axis machine tools | |
JPS61173311A (ja) | マニピユレ−タを制御する方法および装置 | |
Horsch et al. | Cartesian spline interpolation for industrial robots | |
JP2002175105A (ja) | 数値制御方法 | |
JP3476287B2 (ja) | ロボットの接続動作時に速度制御を行なうための曲線補間方法 | |
Yang et al. | A tool path generation and contour error estimation method for four-axis serial machines | |
JP3038972B2 (ja) | 加減速パターン生成装置及びパターン生成方法 | |
Jiang et al. | A decoupled five-axis local smoothing interpolation method to achieve continuous acceleration of tool axis | |
Li et al. | A novel cartesian trajectory planning method by using triple NURBS curves for industrial robots | |
Müller et al. | High accuracy spline interpolation for 5-axis machining | |
Song et al. | Non-isometric dual-spline interpolation for five-axis machine tools by FIR filtering-based feedrate scheduling using pseudo curvature under axial drive constraint | |
Tang et al. | Toolpath interpolation with novel corner smoothing technique | |
He et al. | Real-time time-optimal continuous multi-axis trajectory planning using the trajectory index coordination method | |
US4922431A (en) | Method and apparatus of tool control in arbitrary plane operations | |
JPH0570162B2 (ja) |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
LAPS | Cancellation because of no payment of annual fees |