JP2009166164A - 工業用ロボット - Google Patents

Abstract

【課題】 補助装置の位置を微修正することにより、マニピュレータの特異点を回避する。
【解決手段】 補正移動装置目標値演算部２７は、特異点判別部２６が特異点近傍を判別すると、次時刻のツール先端のワークに対する位置及び姿勢を維持しつつ、次時刻のマニピュレータの姿勢が特異点近傍範囲外となるように、補助装置目標値Ｊｓｔａ（ｔ）を補正して補正補助装置目標値Ｊｓｔａ（ｔ）'を算出する。第２座標変換演算部２８は、補正補助装置目標値Ｊｓｔａ（ｔ）'を使用して、ツール目標値worldＰｔａ（ｔ）をマニピュレータ座標系Σbaseに変換する。補正目標関節角度演算部２９は、マニピュレータ座標系Σbaseに変換されたツール目標値worldＰｔａ（ｔ）''の逆キネマティクス演算で補正目標関節角度Ｊｍｔａ（ｔ）''を算出する。補正目標関節角度Ｊｍｔａ（ｔ）''と補正補助装置目標値Ｊｓｔａ（ｔ）'がマニピュレータ１１と移動装置１２に出力される。
【選択図】図２

Description

本発明は工業用ロボットに関する。特に、本発明は、ツールを備えるマニピュレータと、マニピュレータを移動させる移動装置やワークを位置決めするポジショナのような補助装置とを備える工業用ロボットに関する。

一般に工業用ロボットのマニピュレータは複数の自由度を有し、この自由度により手首先端に取り付けた溶接トーチ、塗装ガン等のツール（加工具）を任意の位置及び姿勢に位置決めしてワーク（加工対象物）を加工する。このような多関節型のマニピュレータの先端位置を直交座標系にて動作させる際に、特異点と呼ばれる動作不能点が存在する。この特異点近傍では、直交座標系におけるマニピュレータ先端の位置及び姿勢から、マニピュレータの関節角度を求める逆キネマティクス計算の解が不定となる。その結果、各関節モータへの指令値が求まらず、エラー発生としてマニピュレータは動作停止となる。

一方、工業用ロボットには、マニピュレータに加え、マニピュレータを移動させる移動装置や、ワークを移動・回転させるポジショナ等の補助装置を備えるものがある。移動装置によって作業領域を拡大し、ポジショナによってワークをマニピュレータの作業領域内で適切に位置決めすることで、より多様なワークの加工が可能となる。一般に作業時間の短縮と作業品質の向上のために、マニピュレータと補助装置を同期して同時に動作させる。例えば、アーク溶接の場合、移動装置とポジショナをマニピュレータと同期して動作させることにより、マニピュレータ単独では作業領域が不足するような大型のワークの連続溶接が可能となり、溶接作業時間の短縮と溶接品質の向上を達成できる。

例えば特許文献１〜３に前述の特異点の回避方法が開示されている。これらの特許文献に記載の特異点回避方法は、いずれも垂直多関節型のマニピュレータの手首軸における特異点近傍で速度と軌跡を変更することで、特異点近傍を滑らかに通過することを意図している。しかし、特許文献１〜３に開示されているのは、いずれもマニピュレータのみを用いる特異点回避方法であり、特異点回避に関しては補助装置を考慮していない。そのため、特許文献１〜３の特異点回避方法は、位置精度や速度精度の確保と両立させることが困難であり、かつ複雑な演算を要する。

特開２００３−３００１８３号公報 特開平１０−１３８１８０号公報 特開平１０−３２９０６６号公報

本発明は、マニピュレータと補助装置を同期させて動作させる工業用ロボットにおいて、補助装置の位置を微修正することにより、位置精度及び速度精度を確保しつつ、比較的簡易な演算でマニピュレータの特異点を回避することを課題とする。

本発明は、先端にツールを備える多関節型のマニピュレータと、前記ツールによって加工されるワーク及び／又は前記マニピュレータを移動させる補助装置と、前記マニピュレータと前記補助装置を同期して動作させる制御装置とを備える工業用ロボットであって、前記制御装置は、予め定められた固定直交座標系における次時刻のツール先端の位置及び姿勢の目標値であるツール目標値を計算するツール目標値演算手段と、前記次時刻の前記補助装置の位置及び姿勢の目標値である補助装置目標値を計算する補助装置目標値演算手段と、前記補助装置目標値を使用して、前記ツール目標値をマニピュレータ座標系に変換する第１の座標変換演算手段と、前記マニピュレータ座標系に変換された前記ツール目標値の逆キネマティクス演算により、前記次時刻における前記マニピュレータの各関節の関節角度の目標値である目標関節角度を計算する目標関節角度演算手段と、前記目標関節角度が特異点近傍であるか否かを判別する特異点判別手段と、前記特異点判別手段が特異点近傍を判別すると、前記次時刻の前記ツール先端の前記ワークに対する位置及び姿勢を維持しつつ、前記次時刻のマニピュレータの姿勢が特異点近傍範囲外となるように、前記補助装置目標値を補正して補正補助装置目標値を計算する補正補助装置目標値演算手段と、前記補正補助装置目標値を使用して、前記次時刻の前記固定直交座標系における前記ツール目標値をマニピュレータ座標系に変換する第２の座標変換演算手段と、前記第２の座標変換手段により前記マニピュレータ座標系に変換されたツール目標値の逆キネマティクス演算により、前記次時刻における前記マニピュレータの関節角度の目標値である補正目標関節角度を計算する補正目標関節角度演算手段と、前記特異点判別手段が特異点近傍を判別していなければ前記目標関節角度と前記補助装置目標値により前記マニピュレータと前記補助装置を駆動する一方、前記特異点判別手段が特異点近傍を判別していれば前記補正目標関節角度と前記補正補助装置目標値とにより前記マニピュレータと前記補助装置を駆動する駆動手段とを備える、工業用ロボットを提供する。

具体的には、前記補正補助装置目標値演算手段は、前記目標関節角度を予め定められた特異点近傍範囲外となるように補正して得られる予備補正目標関節角度を計算し、前記予備補正目標関節角度の順キネマティクス演算によりマニピュレータ座標系における予備補正ツール目標値を計算し、前記予備補正ツール目標値とマニピュレータ座標系における前記ツール目標値との差である補正値を計算し、前記補正値を使用して、前記補助装置目標値から前記補正補助装置目標値を計算する。

例えば、前記制御装置は、前記固定直交座標系における前記ツール先端の位置及び姿勢の時間変化を規定するツール経路を生成するツール経路生成手段と、前記補助装置の位置及び姿勢の時間変化を規定する補助装置経路を生成する補助装置経路生成手段とをさらに備え、前記ツール目標値演算手段は、前記ツール経路に基づいて前記次時刻の前記ツール目標値を計算し、かつ前記補助装置目標値演算手段は、前記補助装置経路に基づいて前記次時刻の前記補助装置目標値を計算する。

補助装置は、移動装置とポジショナのうちのいずれか一方であってもよいし、移動装置とポジショナを両方備えていてもよい。

また、補助装置は少なくとも一つの直動関節又は回転関節を備えるものであればよい。

マニピュレータは、垂直多関節型かつ６軸で、第４軸がロール、第５軸がベント、かつ第６軸がロールである手首を有し、前記補助装置として回転関節のみを有するポジショナのみを備える場合には、ポジショナの回転関節の少なくとも一つがマニピュレータの第５軸と同方向に回転するものである必要がある。

マニピュレータの次時刻の目標関節角度（次時刻のマニピュレータの姿勢）が特異点近傍となった場合、次時刻のマニピュレータの姿勢が特異点近傍範囲外となるように補助装置目標値を補正した補正補助装置目標値を計算し、この補正補助装置目標値から補正目標関節角度を計算し、これら補正目標関節角度と補正補助装置目標値をマニピュレータと補助装置に出力する。言い換えれば、マニピュレータが特異点近傍となった場合、補助装置の位置を自動的に修正することで、マニピュレータの姿勢を特異点近傍範囲外としている。そのため、ワークに対するツール先端の位置及び姿勢を変換させることなく、位置精度及び速度精度を維持しつつ、特異点回避が可能であり、特にアーク溶接等、加工具の位置及び姿勢の変動が加工品質に大きく影響する作業における特異点回避において有効である。また、補助装置の位置の微修正による特異点回避であるので、比較的簡易な演算でマニピュレータの特異点を回避できる。

次に、添付図面を参照して本発明の実施形態を詳細に説明する。

（第１実施形態）
図１及び図２に示す第１実施形態は、マニピュレータ１１と移動装置（スライダ）１２を備えるＰＴＰ型の工業用ロボットに本発明を適用した例である。

図１を参照すると、工業用ロボットは、６軸垂直多関節型のマニピュレータ１１、移動装置１２、制御装置１３、及び教示装置１４を備える。本実施形態では、ワーク１７自体の位置及び姿勢は固定されている。

マニピュレータ１１は先端にツール１６を備え、かつツール１６の位置及び姿勢を三次元空間内で変化させる。マニピュレータ１１は６個の回転関節ＲＪ_ｍ１，ＲＪ_ｍ２，ＲＪ_ｍ３，ＲＪ_ｍ４，ＲＪ_ｍ５，ＲＪ_ｍ６を有する。回転関節ＲＪ_ｍ１〜ＲＪ_ｍ６間はリンクで連結され、最も基端側の回転関節ＲＪ_ｍ１は台座１１ａに取り付けられている。各回転関節ＲＪ_ｍ１〜ＲＪ_ｍ６は、回転駆動のためのモータＭＯ_ｍ１〜ＭＯ_ｍ６と、関節角度Ｊｍ（Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６）を検出するための角度センサＳＥ_ｍ１〜ＳＥ_ｍ６を備える。また、マニピュレータ１１は手首３軸（回転関節ＲＪ_ｍ４〜ＲＪ_ｍ６）が１点で交わるロール・ベンド・ロール型（第４軸がロール、第５軸がベント、第６軸がロール）の手首を有する。

移動装置１２は、互いに直交する方向に直進移動する３個の直動関節ＳＪ_１，ＳＪ_２，ＳＪ_３を備え、直動関節ＳＪ_１にマニピュレータ１１の台座１１ａが保持されている。各直動関節ＳＪ_１〜ＳＪ_３は、それぞれ駆動用のモータＭＯ_ｓ１〜ＭＯ_ｓ３と、後述するワールド座標系Σworldにおける関節位置Ｊｓ（ＳＸ，ＳＹ，ＳＺ）を検出するための位置センサＳＥ_ｓ１〜ＳＥ_ｓ３を備える。

次に、マニピュレータ１１及び移動装置１２の制御に使用する座標を説明する。

マニピュレータ１１に関し、原点がマニピュレータ１１の台座１１ａに設定され、かつ三次元空間に対して固定された直交座標系（マニピュレータ座標系Σbase）を設定する。マニピュレータ座標系Σbaseにおけるマニピュレータ１１の位置及び姿勢（ツール先端１６ａの位置及び姿勢）、すなわちマニピュレータ座標系Σbaseでの座標と座標軸周りの回転角をbaseＰ（Ｘ_ｂ，Ｙ_ｂ，Ｚ_ｂ，α_ｂ，β_ｂ，γ_ｂ）で表記する。αはロール角、βはピッチ角、γはヨー角をそれぞれ示す。

マニピュレータ座標系Σbaseとは別に、三次元空間に対して固定された直交座標系（ワールド座標系Σworld）を設定する。ワールド座標系Σworldにおけるマニピュレータ１１の位置及び姿勢（ツール先端１６ａの位置及び姿勢）、すなわちワールド座標系Σworldでの座標と座標軸周り回転角をworldＰ（Ｘ_ｗ，Ｙ_ｗ，Ｚ_ｗ，α_ｗ，β_ｗ，γ_ｗ）で表記する。ワールド座標系Σworldとマニピュレータ座標系Σbaseの三次元空間内での姿勢（ＸＹＺ軸の向き）は同一である。移動装置１２の各直動関節ＳＪ_１〜ＳＪ_３の向きはワールド座標系ΣworldのＸＹＺ軸の向きと一致しており、ワールド座標系Σworldの原点は、関節位置Ｊｓが（０，０，０）の位置に設定している。

図２を参照すると、制御装置１３は、経路生成部２１、ツール目標値演算部２２、移動装置目標値演算部２３、第１座標変換演算部２４、目標関節角度演算部２５、特異点判別部２６、補正移動装置目標値演算部２７、第２座標演算部２８、補正目標関節角度演算部２９、及び駆動部３０を備える。制御装置１３は、プロセッシングユニット、ＲＡＭ、ＲＯＭ等の各種記憶装置、入出力ポート等のハードウェアと、それに実装されるソフトウェアとにより構築されている。

次に、図３Ａ及び図３Ｂのフローチャートを参照して、制御装置１３により実行されるマニピュレータ１１及び移動装置１２の制御を説明する。

まず、ステップＳ３−１において、経路生成部２１がワールド座標系Σworldにおける現在位置から教示位置までのツール先端１６ａの位置及び姿勢の時系列変化を規定する方程式（ツール経路方程式worldＰ（ｔ），符号ｔは時刻を示す）を計算する。また、ステップＳ３−２において、経路生成部２１はツール経路方程式worldＰ（ｔ）で規定されるワールド座標系Σworldにおけるツール先端１６ａの位置及び姿勢の時系列変化と同期するように、現在位置から教示位置までの移動装置１２の位置及び姿勢の時系列変化を規定する方程式（移動装置経路方程式Ｊｓ（ｔ））を計算する。ツール経路方程式worldＰ（ｔ）及び移動装置経路方程式Ｊｓ（ｔ）の計算のために、経路生成部２１には教示位置Ｐｔｅ（Ｊｍｔｅ＋Ｊｓｔｅ）が教示装置１４から入力され、現在の位置Ｐ０（Ｊｍ０＋Ｊｓ０）が角度センサＳＥ_ｍ１〜ＳＥ_ｍ６と位置センサＳＥ_ｓ１〜ＳＥ_ｓ３から入力される。また、経路生成部２１にはツール経路方程式worldＰ（ｔ）及び移動装置経路方程式Ｊｓ（ｔ）の計算のために必要な制御条件（ツール先端１６ａの軌跡の形状、移動速度等）が入力される。

ツール経路方程式worldＰ（ｔ）及び移動装置経路方程式Ｊｓ（ｔ）を求めた後、ステップＳ３−３において時刻ｔを初期化する（ｔ＝０）。

ステップＳ３−４からステップＳ３−１７の処理が一定の制御周期Ｔｃ毎に繰り返される。まず、ステップＳ３−４において時刻ｔを時刻ｔ＋Ｔｃ（次時刻）に更新する。

次に、ステップＳ３−５において、ツール目標値演算部２２がツール経路方程式worldＰ（ｔ）を使用して、ワールド座標系Σworldにおけるツール先端１６ａの時刻ｔでの位置及び姿勢の目標値であるツール目標値worldＰｔａ（ｔ）（Ｘ_ｗｔａ，Ｙ_ｗｔａ，Ｚ_ｗｔａ，α_ｗｔａ，β_ｗｔａ，γ_ｗｔａ）を計算する。また、ステップＳ３−６において、移動装置目標値演算部２３が、移動装置経路方程式Ｊｓ（ｔ）を使用して、ワールド座標系Σworldにおける時刻ｔでの移動装置１２の位置及び姿勢の目標値である移動装置目標値Ｊｓｔａ（ｔ）（ＳＸｔａ，ＳＹｔａ，ＳＺｔａ）を計算する。

ステップＳ３−７では、第１座標変換部２４が移動装置目標値Ｊｓｔａ（ｔ）を使用してワールド座標系Σworldにおけるツール目標値worldＰｔａ（ｔ）を、マニピュレータ座標系Σbaseにおける時刻ｔでのツール目標値baseＰｔａ（ｔ）（Ｘｂｔａ，Ｙｂｔａ，Ｚｂｔｚ，αｂｔａ，βｂｔａ，γｂｔａ）に座標変換する。この種の座標変換のための座標変換手段は種々知られているが、本実施形態ではマニピュレータ座標系Σbaseの直交３軸の方向と、移動装置１２の移動方向が一致しているので、ツール目標値worldＰｔａ（ｔ）と移動装置目標値Ｊｓｔａ（ｔ）の単純な減算によりマニピュレータ座標系Σbaseにおけるツール目標値baseＰｔａ（ｔ）が得られる。

次に、ステップＳ３−８において、目標関節角度演算部２５がマニピュレータ座標系Σbaseにおけるツール目標値baseＰｔａ（ｔ）の逆キネマティクスを計算し、マニピュレータ１１の各回転関節ＲＪ_ｍ１〜ＲＪ_ｍ６の時刻ｔでの関節角度の目標値である目標関節角度Ｊｍｔａ（ｔ）（Ｊ１ｔａ，Ｊ２ｔａ，Ｊ３ｔａ，Ｊ４ｔａ，Ｊ５ｔａ，Ｊ６ｔａ）に変換する。

続いて、ステップＳ３−９において、特異点判別部２６が、目標関節角度演算部２５により計算された目標関節角度Ｊｍｔａ（ｔ）が特異点近傍にあるか否かを判別する。本実施形態のような手首３軸の回転中心が１点で交わるロール・ベンド・ロール型の手首を備えるマニピュレータ１１では、第５軸である回転関節ＲＪ_ｍ５の関節角度Ｊ５が０度付近で、マニピュレータ座標系Σbaseにおけるトーチ先端１６ａの位置及び姿勢baseＰｔａ（ｔ）から各回転関節ＲＪ_ｍ１〜ＲＪ_ｍ６の関節角度Ｊ１〜Ｊ６が一義的に求まらない「特異点」となる。マニピュレータ１１の動作中に特異点近傍に接近した場合、手首軸である回転関節ＲＪ_ｍ４〜ＲＪ_ｍ６が急激に変化することになる。従来、特異点付近に近付いた場合、エラーとしてマニピュレータ１１の動作を停止するか、手首軸の回転数が最高回転数を超えないようにツール先端１６ａの速度を減速することで特異点を通過していた。

本実施形態では、特異点判別部２６は、以下の式（１）が成立する場合、時刻ｔでの目標関節角度Ｊｍｔａ（ｔ）（マニピュレータ１１の姿勢）が特異点近傍にあると判別する。逆に、式（１）が成立しない場合、特異点判別部２６は時刻ｔでの目標関節角度Ｊｍｔａ（ｔ）は特異点近傍ではないと判別する。

式（１）において、Ｊ５ｔａは時刻ｔにおける第５軸の回転関節ＲＪ_ｍ５の目標関節角度であり、ΔＪｓは判別閾値である。この判別閾値ΔＪｓは、手首軸である回転関節ＲＪ_ｍ４〜ＲＪ_ｍ６の許容最大速度とマニピュレータリンク長で決まる。式（１）の範囲外となるように第５軸の回転関節ＲＪ_ｍ５を動作させれば、特異点を回避し、滑らかにマニピュレータ１１を駆動させることができる。

ステップＳ３−９において目標関節角度Ｊｍｔａ（ｔ）が特異点近傍でないと判別されれば、ステップＳ３−１０において駆動部３０が目標関節角度Ｊｍｔａ（ｔ）をマニピュレータ１１に出力すると共に、移動装置目標値Ｊｓｔａ（ｔ）を移動装置１２に出力する。これらの指令によりマニピュレータ１１のモータＭＯ_ｍ１〜ＭＯ_ｍ６と移動装置１２のモータＭＯ_ｓ１〜ＭＯ_ｓ３が作動し、マニピュレータ１１と移動装置１２が同期して同時に作動する。

一方、ステップＳ３−９において、目標関節角度Ｊｍｔａ（ｔ）が特異点近傍であると判別された場合、目標関節角度Ｊｍｔａ（ｔ）と移動装置目標値Ｊｓｔａ（ｔ）をそのままマニピュレータ１１と移動装置１２に出力するのではなく、ステップＳ３−１１〜３−１７の処理を実行して特異点回避のための補正移動装置目標値Ｊｓｔａ（ｔ）'及び補正目標関節角度Ｊｍｔａ（ｔ）''を算出及び出力する。

ステップＳ３−１１〜Ｓ３−１４の処理は、補正移動装置目標値演算部２７が実行する。補正移動装置目標値演算部２７は、時刻ｔでのワールド座標系Σworldにおけるツール先端１６ａの位置及び姿勢（ツール先端１６ａのワーク１７に対する位置及び姿勢）を維持しつつ、時刻ｔにおけるマニピュレレータ１１の姿勢が特異点近傍範囲外となるように、移動装置１２の時刻ｔにおける位置及び姿勢を微修正するものである。

まず、ステップＳ３−１１において、予備補正目標関節角度Ｊｍｔａ（ｔ）’を計算する。前述のように、式（１）の範囲外となるように第５軸の回転関節ＲＪ_ｍ５を動作させれば、特異点を回避し、滑らかにマニピュレータ１１を駆動させることができる。予備補正目標値関節角度Ｊｍｔａ（ｔ）’は、目標関節角度Ｊｍｔａ（ｔ）のうち、回転関節ＲＪ_ｍ５の目標関節角度Ｊ５ｔａを式（１）の範囲外に変更し、残りの５軸の回転関節角度ＲＪ_ｍ１〜ＲＪ_ｍ４，ＲＪ_ｍ６の目標関節角度Ｊ１ｔａ〜Ｊ４ｔａ，Ｊ６ｔａは変更することなく維持したものである。図４に目標関節角度Ｊｍｔａと予備補正目標関節角度Ｊｍｔａ（ｔ）の関係を概念的に示す。Ｊ５＞＋ΔＪｓの状態で経路移動中、時刻ｔ（次時刻）の目標関節角度Ｊｍｔａ（ｔ）においてＪ５ｔａ＜＋ΔＪｓとなった場合、特異点近傍範囲外に第５軸の回転関節ＲＪ_ｍ５の関節角度Ｊ５を制限するために、Ｊ５ｔａをＪ５ｔａ’＝＋ΔＪｓに設定する。また、Ｊ５＜−ΔＪｓの状態で経路移動中、時刻ｔの目標関節角度Ｊｍｔａ（ｔ）においてＪ５ｔａ＞−ΔＪｓとなった場合には、Ｊ５ｔａをＪ５ｔａ’＝−ΔＪｓに設定する。

次に、ステップＳ３−１２において、予備補正目標関節角度Ｊｍｔａ（ｔ）’の順キネマティクスを計算し、マニピュレータ座標系Σbaseにおける予備補正ツール目標値baseＰｔａ（ｔ）’を計算する。

ステップＳ３−１２では、ツール目標値baseＰｔａ（ｔ）と予備補正ツール目標値baseＰｔａ（ｔ）’から、ツール先端１６ａを特異点近傍範囲外とするためのベース座標系Σbaseにおける必要な補正量ΔＰを計算する。具体的には、以下の式（２）で示すように、補正量ΔＰは予備補正ツール目標値baseＰｔａ（ｔ）’の直交成分とツール目標値baseＰｔａ（ｔ）の直交成分の差として与えられる。

次に、移動装置１２の位置及び姿勢の微修正で補正量ΔＰを実現するために、補正量ΔＰを使用して時刻ｔにおける移動装置目標値Ｊｓｔａ（ｔ）を補正し、移動装置１２の時刻ｔにおける新たな目標位置（補正移動装置目標値Ｊｓｔａ（ｔ）’）を計算する。以下の式（３）で示すように、補正移動装置目標値Ｊｓｔａ（ｔ）’（ＳＸ’，ＳＹ’，ＳＺ’）は、移動装置目標値Ｊｓｔａ（ｔ）（ＳＸ，ＳＹ，ＳＺ）と補正量ΔＰ（ΔＸ，ΔＹ，ΔＺ）の和として与えられる。

次に、ステップＳ３−１５において、第２座標変換演算部２８が、補正移動装置目標値演算部２７によって計算された補正移動装置目標値Ｊｓｔａ（ｔ）’を使用してワールド座標系Σworldにおけるツール目標値worldＰｔａ（ｔ）をマニピュレータ座標系Σbaseにおけるツール目標値である補正ツール目標値baseＰｔａ（ｔ）''（Ｘｔａ''，Ｙｔａ''，Ｚｔａ''，αｔａ''，βｔａ''，γｔａ''）に座標変換する。本実施形態ではマニピュレータ座標系のＸＹＺ軸と移動装置１２の移動方向が一致しているので、以下の式（４）に示すように、補正ツール目標値baseＰｔａ（ｔ）''のＸＹＺ成分はツール目標値worldＰｔａ（ｔ）と補正移動装置目標値Ｊｓｔａ（ｔ）’の単純な減算により算出され、補正ツール目標値baseＰｔａ（ｔ）''のαβγ成分はツール目標値worldＰｔａ（ｔ）と同一である。

次に、ステップＳ３−１６において、補正目標関節角度演算部２９が、マニピュレータ座標系Σbaseにおける補正ツール目標値baseＰｔ（ｔ）''の逆キネマティクスを計算し、マニピュレータ１１の各回転関節ＲＪｍ１〜ＲＪｍ６の時刻ｔで関節角度の目標値の補正値である補正目標関節角度Ｊｍｔａ（ｔ）''（Ｊ１ｔａ''，Ｊ２ｔａ''，Ｊ３ｔａ''，Ｊ４ｔａ''，Ｊ５ｔａ''，Ｊ６ｔａ''）に変換する。

その後、ステップＳ３−１７において、駆動部３０が補正目標関節角度Ｊｍｔａ（ｔ）''をマニピュレータ１１に出力すると共に、補正移動装置目標値Ｊｓｔａ（ｔ）'を移動装置１２に出力する。これらの指令によりマニピュレータ１１のモータＭＯ_ｍ１〜ＭＯ_ｍ６と移動装置１２のモータＭＯ_ｓ１〜ＭＯ_ｓ３が作動し、マニピュレータ１１と移動装置１２が同期して移動する。図５Ａに示すように、仮に特異点回避のための動作を実行しなければ時刻ｔにマニピュレータ１１の姿勢が特異点となる場合でも、本実施形態では図５Ｂにおいて矢印Ａで示すように移動装置１２によってマニピュレータ１１全体を移動することでトーチ先端１６ａのワーク１７に対する位置及び姿勢を維持しつつ、マニピュレータ１１の姿勢が特異点近傍となるのを回避している。

ステップＳ３−１８においてツール先端１６ａが教示位置Ｐｔｅに到達するまで、ステップＳ３−４からステップＳ３−１７の処理が繰り返される。

本実施形態では、マニピュレータ１１の次時刻の目標関節角度Ｊｍｔａ（ｔ）、すなわち次時刻のマニピュレータの姿勢が特異点近傍となった場合、次時刻のマニピュレータ１１の姿勢が特異点近傍範囲外となるように移動装置目標値Ｊｓｔａ（ｔ）を補正した補正補助装置目標値Ｊｓｔａ（ｔ）'を計算し、この補正移動装置目標値Ｊｓｔａ（ｔ）'から補正目標関節角度Ｊｍｔａ（ｔ）''を計算し、これら補正目標関節角度Ｊｍｔａ（ｔ）''と補助移動装置目標値Ｊｓｔａ（ｔ）'をマニピュレータ１１と移動装置１２に出力する。言い換えれば、図５Ｂに示すように、マニピュレータ１１が特異点近傍となった場合、移動装置１２の位置を自動的に修正することで、マニピュレータ１１の姿勢を特異点近傍範囲外としている。そのため、ワーク１７に対するツール先端１６ａの位置及び姿勢を変換させることなく、位置精度及び速度精度を維持しつつ、特異点回避が可能であり、特にアーク溶接等、加工具の位置及び姿勢の変動が加工品質に大きく影響する作業における特異点回避において有効である。また、移動装置１２の位置の微修正による特異点回避であるので、比較的簡易な演算でマニピュレータ１１の特異点を回避できる。

（第２実施形態）
図６及び図７に示す第２実施形態は、マニピュレータ１１とポジショナ３２を備えるＰＴＰ型の工業用ロボットに本発明を適用した例である。ワーク１７はポジショナ３２により移動するが、マニピュレータ１７の位置及び姿勢は固定されている。

本実施形態におけるポジショナ３２は、互いに直交方向に直線移動する３個の直動関節ＰＪ_１，ＰＪ_２，ＰＪ_３と１個の回転関節ＰＪ_４を備える。各直動関節ＰＪ_１〜ＰＪ_３は、それぞれ駆動用のモータＭＯ_Ｐ１〜ＭＯ_Ｐ３と、ワールド座標系Σworldにおける関節位置Ｊｐを検出するための位置センサＳＥ_ｐ１〜ＳＥ_ｐ３を備える。各直動関節ＰＪ_１〜ＰＪ_３の向きはワールド座標系ΣworldのＸＹＺ軸の向きと一致しており、ワールド座標系の原点は関節位置Ｊｐが（０，０，０）の位置に設定している。また、ポジショナ３２の回転関節ＰＪ_４は、ワールド座標系ΣworldのＹ_ｗ軸周りに回転する。ポジショナ３２のワールド座標系Σworldにおける関節位置Ｊｐを（ＰＸ，ＰＹ，ＰＺ，βｐ）と表記する。

本実施形態における制御装置１３が備えるポジショナ目標値演算部３３と補正ポジショナ目標値演算部３７は、それぞれ第１実施形態における制御装置１３が備える移動位置目標値演算部２３と補正移動装置目標値演算部２７と同様の機能を有する。

図８Ａ及び図８Ｂと図３Ａ及び図３Ｂを比較すれば明らかなように、本実施形態における制御装置１３は第１実施形態と同様の手法でマニピュレータ１１及びポジショナ３２を制御する。以下、図８Ａ及び図８Ｂの制御の概要を説明する。

経路生成部２１がツール経路方程式worldＰ（ｔ）及びポジショナ経路方程式Ｊｐ（ｔ）を算出した後（ステップＳ８−１，Ｓ８−２）、時刻ｔが初期化される（ステップＳ８−３）。ステップＳ８−１８で教示位置Ｐｔｅへの到達が検出されるまで、一定の制御周期Ｔｃ毎にステップＳ８−４からステップＳ８−１７の処理が繰り返される。

ツール目標値演算部２２が、ツール経路方程式worldＰ（ｔ）より時刻ｔ（次時刻）でのワールド座標系Σworldにおけるツール目標値worldＰｔａ（ｔ）を計算し（ステップＳ８−５）、ポジショナ目標値演算部３３がポジショナ経路方程式worldＪｐ（ｔ）より時刻ｔでのポジショナ目標値Ｊｐｔａ（ｔ）（ＰＸｔａ，ＰＹｔａ，ＰＺｔａ，βｐｔａ）を計算する（ステップＳ８−６）。

次に、第１座標変換部２４がポジショナ目標値Ｊｐｔａ（ｔ）を使用してワールド座標系Σworldにおけるツール目標値worldＰｔａ（ｔ）を、マニピュレータ座標系Σbaseにおける時刻ｔでのツール目標値baseＰｔａ（ｔ）に座標変換する（ステップ８−７）。また、目標関節角度演算部２５が逆キネマティクス演算によりマニピュレータ座標系Σbaseにおけるツール目標値baseＰｔａ（ｔ）をマニピュレータ１１の各回転関節ＲＪ_ｍ１〜ＲＪ_ｍ６の時刻ｔでの目標関節角度Ｊｍｔａ（ｔ）に変換する（ステップＳ８−８）。

特異点判別部２６が、マニピュレータ１１の第５軸の回転関節ＲＪ_ｍ５の目標関節角度Ｊ５ｔａから時刻ｔのマニピュレータ１１の姿勢が特異点でないと判別すれば（式（１）参照）、駆動部３０が目標関節角度Ｊｍｔａ（ｔ）をマニピュレータ１１に出力すると共に、ポジショナ目標値Ｊｐｔａ（ｔ）をポジショナ３２に出力し、マニピュレータ１１とポジショナ３２を同期して同時に駆動する（ステップ８−９，Ｓ８−１０）。

一方、特異点判別部２６が時刻ｔのマニピュレータ１１の姿勢が特異点であると判断すれば、特異点回避のための補正ポジショナ目標値Ｊｐｔａ（ｔ）’及び補正目標関節角度Ｊｍｔａ（ｔ）''を算出してポジショナ３２とマニピュレータ１１に出力する（ステップＳ８−９，Ｓ８−１１〜Ｓ８１７）。

具体的には、補正ポジショナ目標値演算部３７は、予備補正目標関節角度Ｊｍｔａ（ｔ）'を計算し、この予備補正目標関節角度Ｊｍｔａ（ｔ）'の順キネマティクス演算により予備補正ツール目標値baseＰｔａ（ｔ）’を算出し、さらにツール目標値baseＰｔａ（ｔ）と予備補正ツール目標値baseＰｔａ（ｔ）’から、マニピュレータ１１を特異点近傍範囲外とするためのベース座標系Σbaseにおける必要な補正量ΔＰ（式（２）参照）を算出する（ステップＳ８−１１〜Ｓ８−１３）。また、補正ポジショナ目標値演算部３７は補正量ΔＰを使用して、時刻ｔにおけるポジショナ目標値位置Ｊｐｔａ（ｔ）からポジショナ３２の時刻ｔにおける新たな目標位置である補正ポジショナ目標値Ｊｐｔａ（ｔ）'を計算する。具体的には、補正ポジショナ目標値Ｊｐｔａ（ｔ）'（ＰＸｔａ'，ＰＹｔａ'，ＰＺｔａ'，βｐｔａ'）は、以下の式（５）に示す通りである。

次に、第２座標変換演算部２８が、補正ポジショナ目標値Ｊｐｔａ（ｔ）’を使用してワールド座標系Σworldにおけるツール目標値worldＰｔａ（ｔ）をマニピュレータ座標系Σbaseにおける補正ツール目標値baseＰｔａ（ｔ）''に座標変換する（ステップＳ８−１５）。マニピュレータ座標系のＸＹＺ軸とポジショナ３２の直進移動の方向が一致しているので、以下の式（６）に示すように、補正ツール目標値baseＰｔａ（ｔ）''のＸＹＺ成分はツール目標値worldＰｔａ（ｔ）と補正ポジショナ目標値Ｊｐｔａ（ｔ）’の単純な減算により算出され、補正ツール目標値baseＰｔａ（ｔ）''のαβγ成分はツール目標値worldＰｔａ（ｔ）と同一である。

さらに、補正目標関節角度演算部２９が、マニピュレータ座標系Σbaseにおける補正ツール目標値baseＰｔ（ｔ）''の逆キネマティクスを計算し、マニピュレータ１１の各回転関節ＲＪ_ｍ１〜ＲＪ_ｍ６についての補正目標関節角度Ｊｍｔａ（ｔ）''に変換する。

本実施形態の工業用ロボットも、マニピュレータ１１が特異点近傍となった場合、ポジショナ３２の位置を自動的に修正することで、マニピュレータ１１の姿勢を特異点近傍範囲外としている。そのため、ワーク１７に対するツール先端１６ａの位置及び姿勢を変換させることなく、位置精度及び速度精度を維持しつつ、特異点回避が可能であり、かつ特異点回避のための演算も比較的簡易である。

第２実施形態のその他構成及び作用は第１実施形態と同様である。

（第３実施形態）
第１及び第２実施形態では補助装置（移動装置１２とポジショナ３２）は３個の直動関節を備えているが、本発明は補助装置として１軸又は２軸の回転関節のみを備えるポジショナのみを備える工業用ロボットにも適用できる。本実施形態は、補助装置として１軸の回転関節のみを備えるポジショナ４２のみを備える工業用ロボットに本発明を適用した例である。

図９を参照すると、ポジショナ４２は水平方向の軸線（ワールド座標系ΣworldのＹｗ軸）周りに回転する一つの回転関節ＰＪのみを備える。回転関節ＰＪは、駆動用のモータＭＯと位置センサＳＥを備える。図９においてΣposiはポジショナ４２のワーク１７の取付面に固定されたポジショナ座標である。本実施形態の他の機械的な構成は、第２実施形態と同一である。また、制御装置１３の構成も第２実施形態の制御装置１３（図７参照）と同様である。

図１０Ａ及び図１０Ｂに示すマニピュレータ１１及びポジショナ４２の制御は、第２実施形態における制御（図８Ａ及び図８Ｂ）と同様である。具体的には、図１０Ａ及び図１０ＢのステップＳ１０−１〜Ｓ１０−１０，Ｓ１０−１３〜Ｓ１０−１６は、それぞれ図８Ａ及び図８ＢのＳ８−１〜Ｓ８−１０，Ｓ８−１４〜Ｓ８−１８と同様である。

本実施形態では、補正ポジショナ目標値演算部３７により実行される特異点回避のための補正ポジショナ目標値Ｊｐｔａ（ｔ）’の求め方が第２実施形態とは異なる。第３実施形態では、予備補正目標関節角度Ｊｍｔａ（ｔ）’を計算し、順キネマティクスにより予備補正ツール目標位置base Ｐｔａ（ｔ）’求め、この予備補正ツール目標位置baseＰｔａ（ｔ）’とツール目標値base Ｐｔａ（ｔ）から式（２）より補正量ΔＰを求める（図８ＢのステップＳ８−１１〜Ｓ８−１３）。これに対して、本実施形態では特異点近傍であるか否かの判別閾値ΔＪｓを補正量ΔＰに設定する（図１０ＢのステップＳ１０−１１）。具体的には、Ｊ５＞＋ΔＪｓの状態で経路移動中、時刻ｔ（次時刻）の目標関節角度Ｊｍｔａ（ｔ）においてＪ５ｔａ＜＋ΔＪｓとなった場合、＋ΔＪｓを補正量ΔＰに設定する。また、また、Ｊ５＜−ΔＪｓの状態で経路移動中、時刻ｔの目標関節角度Ｊｍｔａ（ｔ）においてＪ５ｔａ＞−ΔＪｓとなった場合、−ΔＪｓを補正量ΔＰに設定する。以下の式（７）に本実施形態における補正量ΔＰを示す。

この補正量Δにより、ポジショナ目標値Ｊｐｔａ（ｔ）（＝βｐ）を補正することにより時刻ｔにおけるツール先端１６ａが特異点近傍範囲外となる。以下の式（８）に補正ポジショナ目標値Ｊｐｔａ（ｔ）’を示す。

補正ポジショナ目標値Ｊｐｔａ（ｔ）’算出後の演算は、前述のように第２実施形態と同様である（ステップＳ１０−１２〜Ｓ１０−１５）。

図１１Ａに示すように、仮に特異点回避のための動作を実行しなければ時刻ｔにマニピュレータ１１の姿勢が特異点となる場合でも、本実施形態では図１１Ｂにおいて矢印Ｂで示すようにポジショナ４２によってワーク１７を回転することでマニピュレータ１１の姿勢が特異点近傍となるのを回避している。このポジショナ４２の回転補正により重力方向（ワールド座標系worldのＺｗ方向）に対するトーチ先端１６ａの姿勢角は、ΔＪｓだけ変化するが、この角度は数度以下であり加工品質上実質的に問題とならない程度の微少量である。

本実施形態のように、手首３軸が１点で交わるロール・ベンド・ロール型の手首を有するマニピュレータの場合に、回転関節のみを有するポジショナにより本発明を実施する場合、ポジショナの回転関節の少なくとも１つの回転成分がマニピュレータの第５軸（ＲＪ_ｍ５）の回転成分とほぼ一致する配置関係である必要がある。例えば、マニピュレータが前述の手首を有する場合、重量方向（ワールド座標系ΣworldのＺｗ軸）周りに回転する１つの回転関節のみを備えるターンテーブル型のポジショナでは本発明を実施できない。

本発明は前記実施形態に限定されず、種々の変形が可能である。例えば、マニピュレータに移動装置とポジショナの両方を組み合わせた場合でも本発明を適用できる。また、特異点近傍であるか否かの判別は式（１）に限定されず、マニピュレータの構成等に応じて他の手法で特異点近傍を判別してもよい。さらに、本発明は工業用ロボット動作中の実時間制御における特異点回避だけでなく、ＰＴＰ型の工業用ロボットのオフライン教示における経路計画において、経路途中で特異点が発生しないようにマニピュレータと補助装置の位置データを作成する際にも応用できる。

本発明の第１実施形態に係る工業用ロボットを示す概略構成図。 第１実施形態における制御装置のブロック図。 本発明に第１実施形態に係る工業用ロボットの動作を説明するためのフローチャート。 本発明の第１実施形態に係る工業用ロボットの動作を説明するためのフローチャート。 目標関節角度Ｊｍｔａ（ｔ）と予備補正目標関節角度Ｊｍｔａ（ｔ）’の関係を示す概念図。 特異点回避動作を実行しない場合のマニピュレータの挙動を示す模式図。 本発明の特異点回避動作を実行する場合のマニピュレータの挙動を示す模式図。 本発明の第２実施形態に係る工業用ロボットを示す概略構成図。 第２実施形態における制御装置のブロック図。 本発明の第２実施形態に係る工業用ロボットの動作を説明するためのフローチャート。 本発明の第２実施形態に係る工業用ロボットの動作を説明するためのフローチャート。 本発明の第３実施形態に係る工業用ロボットを示す概略構成図。 第３実施形態における制御装置のブロック図。 第３実施形態における制御装置のブロック図。 本発明の第３実施形態に係る工業用ロボットの動作を説明するためのフローチャート。 本発明の第３実施形態に係る工業用ロボットの動作を説明するためのフローチャート。

符号の説明

１１ マニピュレータ
１１ａ 台座
１２ 移動装置
１３ 制御装置
１４ 教示装置
１６ ツール
１７ ワーク
２１ 経路生成部
２２ ツール目標値演算部
２３ 移動装置目標値演算部
２４ 第１座標変換部
２５ 目標関節角度演算部
２６ 特異点判別部
２７ 補正移動装置目標値演算部
２８ 第２座標変換部
２９ 補正目標関節角度演算部
３０ 駆動部
３２，４２ ポジショナ
３３ ポジショナ目標値演算部
３７ 補正ポジショナ目標値演算部

Claims (7)

1. 先端にツールを備える多関節型のマニピュレータと、前記ツールによって加工されるワーク及び／又は前記マニピュレータを移動させる補助装置と、前記マニピュレータと前記補助装置を同期して動作させる制御装置とを備える工業用ロボットであって、
   前記制御装置は、
   予め定められた固定直交座標系における次時刻のツール先端の位置及び姿勢の目標値であるツール目標値を計算するツール目標値演算手段と、
   前記次時刻の前記補助装置の位置及び姿勢の目標値である補助装置目標値を計算する補助装置目標値演算手段と、
   前記補助装置目標値を使用して、前記ツール目標値をマニピュレータ座標系に変換する第１の座標変換演算手段と、
   前記マニピュレータ座標系に変換された前記ツール目標値の逆キネマティクス演算により、前記次時刻における前記マニピュレータの各関節の関節角度の目標値である目標関節角度を計算する目標関節角度演算手段と、
   前記目標関節角度が特異点近傍であるか否かを判別する特異点判別手段と、
   前記特異点判別手段が特異点近傍を判別すると、前記次時刻の前記ツール先端の前記ワークに対する位置及び姿勢を維持しつつ、前記次時刻のマニピュレータの姿勢が特異点近傍範囲外となるように、前記補助装置目標値を補正して補正補助装置目標値を計算する補正補助装置目標値演算手段と、
   前記補正補助装置目標値を使用して、前記次時刻の前記固定直交座標系における前記ツール目標値をマニピュレータ座標系に変換する第２の座標変換演算手段と、
   前記第２の座標変換手段により前記マニピュレータ座標系に変換されたツール目標値の逆キネマティクス演算により、前記次時刻における前記マニピュレータの関節角度の目標値である補正目標関節角度を計算する補正目標関節角度演算手段と、
   前記特異点判別手段が特異点近傍を判別していなければ前記目標関節角度と前記補助装置目標値により前記マニピュレータと前記補助装置を駆動する一方、前記特異点判別手段が特異点近傍を判別していれば前記補正目標関節角度と前記補正補助装置目標値とにより前記マニピュレータと前記補助装置を駆動する駆動手段と
   を備える、工業用ロボット。
2. 前記補正補助装置目標値演算手段は、
   前記目標関節角度を予め定められた特異点近傍範囲外となるように補正して得られる予備補正目標関節角度を計算し、
   前記予備補正目標関節角度の順キネマティクス演算によりマニピュレータ座標系における予備補正ツール目標値を計算し、
   前記予備補正ツール目標値とマニピュレータ座標系における前記ツール目標値との差である補正値を計算し、
   前記補正値を使用して、前記補助装置目標値から前記補正補助装置目標値を計算する、請求項１に記載の工業用ロボット。
3. 前記制御装置は、
   前記固定直交座標系における前記ツール先端の位置及び姿勢の時間変化を規定するツール経路を生成するツール経路生成手段と、
   前記補助装置の位置及び姿勢の時間変化を規定する補助装置経路を生成する補助装置経路生成手段と
   をさらに備え、
   前記ツール目標値演算手段は、前記ツール経路に基づいて前記次時刻の前記ツール目標値を計算し、かつ
   前記補助装置目標値演算手段は、前記補助装置経路に基づいて前記次時刻の前記補助装置目標値を計算する、請求項１又は請求項２に記載の工業用ロボット。
4. 前記補助装置として移動装置とポジショナのうちの少なくとも一方を備える、請求項１から請求項３のいずれか１項に記載の工業用ロボット。
5. 前記補助装置は少なくとも一つ直動関節を備える、請求項１から請求項３のいずれか１項に記載の工業用ロボット。
6. 前記補助装置は少なくとも一つの回転関節を備える、請求項１から請求項３のいずれか１項に記載の工業用ロボット。
7. 前記マニピュレータは、垂直多関節型かつ６軸で、第４軸がロール、第５軸がベント、かつ第６軸がロールである手首を有し、
   前記補助装置として回転関節のみを有するポジショナのみを備え、
   前記ポジショナの回転関節の少なくとも一つが前記マニピュレータの前記第５軸と同方向に回転するものである、請求項１から請求項３のいずれか１項に記載の工業用ロボット。

Images