JP4496483B2 - ロボット制御方法および制御装置 - Google Patents

Description

本発明は、旋回軸３軸で構成される関節部を備えたロボットのソフトウェア処理を伴う制御装置において、特異領域に生じる軸の過大速度動作の回避方法である。

多くの場合、旋回軸３軸で構成される関節部は、例えば図２の構成をしている。ここで、逆運動学によって導かれるＪ１軸角度の式は、以下の式１のようになる。
ｓｉｎθＪ１×ｓｉｎθＪ２＝Ａ
ｃｏｓθＪ１×ｓｉｎθＪ２＝Ｂ
∴ ｓｉｎθＪ２＝Ａ／ｓｉｎθＪ１＝Ｂ／ｃｏｓθＪ１
θＪ１＝Ａ／Ｂ ・・・（式１）
ここで、ＡとＢはそれぞれ定数を表し、θＪ１はＪ１軸角度、θＪ２はＪ２軸角度を表しているが、式からも分かるようにｓｉｎθＪ２≒０となる場合に特異点となる。式１はあくまで一般的な旋回３軸で構成される手首軸を備えた多関節ロボットの逆運動学によって導かれる式を示しているが、関節部が他の構成をしたロボット、例えば図３においても、同様に特異点が現れる。
このように特異点は特定の姿勢軸の角度によって生じるので、所定の各軸角度が特異領域を示しているかの判定は、式１を例に言えば、Ｊ２軸角度が０度であることを基準にすれば良い。さらに言えば、特異点の条件に近い角度であるほどＪ１軸角度が大きくなり、軸の過大速度動作となるので、Ｊ２軸角度が０度近傍であることを基準にしていれば良い。また、補間動作における始点と終点のＪ２軸角度に着目すると、同符号であれば特異点を通過せず、異符号であれば特異点を通過するという判定基準になる。

以上を踏まえて、旋回軸３軸で構成される関節部を備えたロボットのソフトウェア処理を伴う制御装置において、従来の特異領域に生じる軸の過大速度回避は、補間動作開始前に終点のＪ２軸角度が特異領域である条件を満たすか、始点のＪ２軸角度が特異領域である条件を満たすか、始点と終点間のＪ２軸角度が異符号か、を判定し、姿勢制御を禁止する処理によって、補間動作中の間、Ｊ１軸、Ｊ２軸、Ｊ３軸の角度変化量を移動量に対して均等に微小変化させて動作することで、逆運動学演算により求まってしまうであろう過大なＪ１軸目標角度を避けて、制御点の位置は補間動作を実現している（例えば特許文献１、特許文献２参照）。図４は、以上で述べた過大速度回避のための従来の制御方法を示している。図４が示すフローは、補間動作区間の始点から終点に特異領域が存在すれば、無条件に、その補間動作区間は姿勢制御を禁止していることを示している。
特開平８−２４１１０８号公報（第９頁、図６） 特開平１１−１４９３０４号公報（第３−４頁、図３）

ところが、従来の方法では、図４でも明らかであるが、補間動作開始の始点から終点までに特異領域がわずかでも存在する場合、姿勢制御を禁止する為に、補間動作開始から終点に到達する間、例え特異領域とならない位置であろうと任意の姿勢が実現できない。
また、従来の過大速度回避の方法では、例えば、特許文献１では本来の補間動作軌跡の逸脱が見られ、また、特許文献２では指定された補間速度の低下が見られている。
この為、アーク溶接用途やシーリング用途の位置と姿勢が要求される用途ではきれいな仕上げをすることが困難となっていた。
このような問題を解決する手段として、各関節速度をアクチュエータの実現出来る最大速度に抑え込む手段があるが、これではアーク溶接用途やシーリング用途の作業時間の遅延が生じる。また、プレス間のワークのハンドリング用途やパレタイズ用途の短い作業時間が要求される場面では、これは非常に大きな問題となる。
よって、本発明の目的は、補間動作中に特異領域が存在し得る場合であろうと速度を低下させることなく、また軸の過大速度動作を生じることなく、なめらかに補間動作を実現することが出来るロボットの制御方法およびその制御装置を提供することである。

上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、Ｊ３軸の先端に制御点をもち、前記Ｊ３軸から順にＪ２軸、Ｊ１軸の３軸の旋回軸からなる関節部を有するロボットの制御装置において、前記関節部の補間動作区間に特異領域が存在するとき、前記特異領域を脱出する脱出位置を探索する脱出位置探索を行い、前記特異領域のみの、或いは前記特異領域から前記脱出位置までの区間のみの、姿勢制御を禁止し、前記補間動作区間の終点における前記Ｊ２軸の角度が前記特異領域にあるときは、前記脱出位置が補間動作区間に存在しないと判定して前記脱出位置探索を行わず、前記特異領域の開始点から補間動作の終点までの区間のみ姿勢制御を禁止することを特徴としたロボット制御装置とするものである。
請求項２に記載の発明は、前記特異領域は、前記関節部の特異点と、前記特異点を含み、かつ予め定めた前記特異点近傍のＪ２軸の角度とから決定される領域である請求項１記載のロボット制御装置とするものである。
請求項３に記載の発明は、前記補間動作区間の始点と終点におけるＪ２軸の角度が、それぞれ前記特異領域になく、かつ前記補間動作区間の始点と終点におけるＪ２軸の角度符号が同符号であるときは、前記特異領域は前記補間動作区間に存在しないと判定し、前記特異領域のみ姿勢制御を禁止することを特徴とした請求項１記載のロボット制御装置とするものである。
請求項４に記載の発明は、前記補間動作区間の始点におけるＪ２軸の角度が特異領域にあるときで、かつ前記補間動作区間の始点と終点におけるＪ２軸の角度符号が異符号であるときは、前記補間動作区間を予め設定されたサンプリング数で分割した探索点において、Ｊ２軸の角度符号が、前記補間動作区間の始点での符号から異符号に転じる探索点を二分探索法によって探索し、異符号に転じる探索点が特異領域外ならば、該探索点を脱出位置とし、異符号に転じる探索点が特異領域内であれば、該探索点に最も近接した特異領域外の探索点を探索して脱出位置とすることを特徴とした請求項１記載のロボット制御装置とするものである。
請求項５に記載の発明は、前記補間動作区間の始点におけるＪ２軸の角度が特異領域にあるときで、かつ前記補間動作区間の始点と終点におけるＪ２軸の角度符号が同符号であるときは、前記補間動作区間を予め設定されたサンプリング数で分割した探索点において、前記探索点でのＪ２軸の角度が特異領域を脱出する探索点を、前記補間動作区間の始点から終点方向まで逐次探索法によって探索し、特異領域を脱出した探索点を脱出位置とすることを特徴とした請求項１記載のロボット制御装置とするものである。
請求項６に記載の発明は、前記補間動作区間の始点と終点におけるＪ２軸の角度が、それぞれ前記特異領域になく、かつ前記補間動作区間の始点と終点におけるＪ２軸の角度符号が異符号であるときは、前記特異領域があると判定し、前記補間動作区間を予め設定されたサンプリング数で分割した探索点において、Ｊ２軸の角度符号が、前記補間動作区間の始点での符号から異符号に転じる探索点を二分探索法によって探索し、異符号に転じる探索点が特異領域外ならば、該探索点を脱出位置とし、異符号に転じる探索点が特異領域内であれば、該探索点に最も近接した特異領域外の探索点を探索して脱出位置とすることを特徴とした請求項１記載のロボット制御装置とするものである。
請求項７に記載の発明は、Ｊ３軸の先端に制御点をもち、前記Ｊ３軸から順にＪ２軸、Ｊ１軸の３軸の旋回軸からなる関節部を有するロボットの制御方法において、前記関節部の補間動作前に、前記補間動作の始点と終点と軌跡上とに特異領域が存在するかを判定し、前記関節部の補間動作区間に特異領域が存在するとき、前記特異領域を脱出する脱出位置を探索する脱出位置探索を行い、前記特異領域のみの、或いは前記特異領域から前記脱出位置までの区間のみの、姿勢制御を禁止し、前記補間動作区間の終点における前記Ｊ２軸の角度が前記特異領域にあるときは、前記脱出位置が補間動作区間に存在しないと判定して前記脱出位置探索を行わず、前記特異領域の開始点から補間動作の終点までの区間のみ姿勢制御を禁止することを特徴としたロボットの制御方法とするものである。

本発明は、今まで姿勢制御が出来なかった特異領域を含む補間動作を、的確な必要最低限の姿勢制御の抑制によって、特異領域内であろうと確実に位置を補間しながら、且つ補間速度を実現し動作出来る。
また、特異領域とその脱出位置を補間動作前に計算し、Ｊ２軸の角度が特異領域となることを監視する事と、脱出位置へ到達するまでの間、姿勢制御を禁止する事が出来るので、一般に処理の負荷が大きいロボットの動作中において中央演算処理装置やＲＡＭの資源を奪うことなく補間動作が出来る。よって、本来の補間動作軌跡の逸脱も見られず、指定された補間速度の低下も見られない制御装置を提供できる。
これらの効果により、位置と姿勢の精度が要求されるアーク溶接用途やシーリング用途、速度が要求されるプレス間のワークのハンドリング用途やパレタイズ用途にも広く対応することが出来る。

以下、本発明の実施の形態について図を参照して説明する。

まず、本発明の過大速度回避のためのフローを説明する。
本発明は、旋回軸３軸で構成される関節部を備えたロボットのソフトウェア処理を伴う制御装置において、補間動作開始時に始点から終点までの総移動量を算出し、前記補間動作開始時に始点と終点のＪ２軸角度が異符号か、もしくは始点のＪ２軸角度が特異領域となる角度か、もしくは終点のＪ２軸角度が特異領域となる角度かを判定し、補間動作の始点と終点と軌跡上に特異領域が存在することを判定する処理（以下、特異領域判定処理）によって図１にあるように処理が分けられる。

まず、図１の１１において、補間動作の終点におけるＪ２軸の角度が、予め定めておいた特異領域内の角度であるかを判定する。ここでは、例えばその角度を±０．２°とする。ここで、終点が特異領域内であれば、補間動作中に脱出位置は存在しないことが明らかとなる為に、脱出位置探索処理を実行せず、特異領域の開始点から補間動作の終点までの区間のみ姿勢制御を禁止する。

次に、図１の１２において、補間動作の終点が特異領域になく、始点が特異領域内にあれば、この時点では特異点が始点と終点の間に存在するのか、しないのかが判別が出来ない為に、更に、始点と終点におけるＪ２軸の角度符号が異符号であるか判定をし、異符号であるならば特異点が始点と終点の間に存在し、同符号であるならば特異点が始点と終点の間に存在しないと判定する。このように判定した後、本発明では、特異領域の脱出位置を探索する。
図１の１２ａにおいて、始点と終点におけるＪ２軸の角度符号が異符号である場合の脱出位置の探索は、以下で説明する二分探索法によって行う。二分探索法は、予め設定されたサンプリング数で各探索点を求め、前記各探索点を逆変換処理によってＪ２軸角度の算出をし、Ｊ２軸角度の符号が異符号、または同符号の判定を二分探索法によって繰り返し、最終的に異符号となる境界になる探索点が特異領域外であるならばその位置を脱出位置とし、特異領域内であるならば終点方向へ隣接した探索点が特異領域内か判定し、特異領域内であるならば更に次の探索点へと逐次探索をしていき、最終的に始点と異符号で且つ、特異領域内の探索点と隣接する特異領域外の探索点を脱出位置と定める。
また、図１の１２ｂにおいて、始点と終点におけるＪ２軸の角度符号が同符号である場合の脱出位置の探索は、以下で説明する逐次探索法によって行う。逐次探索法は、予め設定されたサンプリング数で各探索点を求め、前記各探索点を逆変換処理によってＪ２軸角度の算出をし、始点に隣接した探索点から終点方向へ隣接した探索点が特異領域外であるか判定し、特異領域内であるならば、更に終点方向へ隣接した探索点へと逐次探索をしていき、最終的に始点方向の特異領域内にある探索点に隣接した終点方向の探索点を脱出位置と定める。
以上、補間動作の始点が特異領域にある場合は、この始点から上記で特定した脱出位置までの区間のみで姿勢制御を禁止する。

次に、図１の１３において、補間動作の終点も始点も特異領域になく、かつ始点と終点におけるＪ２軸の角度符号が異符号であれば、特異領域が補間動作区間に存在すると判定し、前述の二分探索法と同様に脱出位置を定める。この場合は、特異領域の開始点から特定された脱出位置までの区間のみで姿勢制御を禁止する。

以上のように本発明では、補間動作中に、現在Ｊ２軸の角度が特異領域となる角度であることを判断し、姿勢制御を禁止する処理が常にＪ２軸の角度を監視しているため、特異領域となった位置から姿勢制御を禁止することで、姿勢軸に対して逆変換処理を実行せず、脱出位置へ補間動作するまでの間、Ｊ１軸、Ｊ２軸、Ｊ３軸の角度変化量を特異領域内の移動量に対して均等に微小変化させていき、脱出位置へ到達した時点で姿勢制御を有効にし、残りの移動量を補間動作している。

次に、以上で説明した過大速度回避のための方法を使用した関節部の具体例について述べる。
今、図５のような教示された始点５１と終点５２との間を、制御点の進行方向５３の矢印方向へ直線補間動作しながら、理想的な直線補間の制御点の軌跡５４を描き、また、特異領域となる区間５５を通過する場合を考える。この時の姿勢制御は、基準となる座標系から見た始点のアプローチベクトル５６と、終点のアプローチベクトル５７の回転ベクトルの変化量を直線移動量に対して均等に変化させることで、図６のアプローチベクトルの推移６８に示すように制御するのが理想的であるが、補間動作軌跡上に特異領域となる区間６９が存在する為、前述のように、Ｊ１軸角度が過大になる理由から、姿勢制御を実現することができない。
そこで、特異領域に生じる軸の過大速度動作の従来の回避方法によれば、図７に示すように、特異領域となる区間７０が補間動作の軌跡上に存在するので、姿勢制御を禁止する処理によって、補間動作中の間、Ｊ１軸、Ｊ２軸、Ｊ３軸の角度変化量を移動量に対して均等に微小変化させて動作することにより、アプローチベクトルの推移７１に示すような姿勢の崩れが生じる。このような場合にロボットやワークの設置の変更や、それにともなう教示点の変更によって特異領域を通過しない位置で作業させることで前述の姿勢制御を禁止する処理によって生じる姿勢の崩れを避けることが出来る可能性があるが、周囲の作業区域が充分に確保出来ない為、ロボットやワークの設置の変更や、それにともなう教示点の変更が出来ない場合を想定される。
ここで、本発明を適用すれば、補間動作開始前に、図８における特異領域となる区間８４が、補間動作軌跡上に存在することを判断するとともに、その脱出位置を探索した後で補間動作を開始し、Ｊ２軸の角度が特異領域となることを監視する事で、特異領域内に侵入した時点から脱出位置へ到達するまでの間だけを姿勢制御を禁止するので、図８の本発明の直線補間の制御点の軌跡８２において、補間速度を変化させることなく動作し、アプローチベクトルの推移８３が示しているように、特異領域外は姿勢制御を行い、特異領域内は姿勢制御を禁止することで、補間速度と軌跡を実現することが出来ている。

本発明の特異領域判定処理と脱出位置探索処理のフロー図 一般的な旋回軸３軸で構成される関節部のモデル図 旋回軸３軸で構成される関節部のその他のモデル図 従来の特異領域判定処理のフロー図 関節部の直線補間動作軌跡図 理想的な直線補間動作時の姿勢制御図 従来の特異領域を含む直線補間動作時の姿勢制御図 本発明の特異領域を含む直線補間動作時の姿勢制御図

符号の説明

１１、１２、１３ 特異領域有無判定
１２ａ 二分探索法
１２ｂ 逐次探索法

Claims (7)

1. Ｊ３軸の先端に制御点をもち、前記Ｊ３軸から順にＪ２軸、Ｊ１軸の３軸の旋回軸からなる関節部を有するロボットの制御装置において、
   前記関節部の補間動作区間に特異領域が存在するとき、前記特異領域を脱出する脱出位置を探索する脱出位置探索を行い、前記特異領域のみの、或いは前記特異領域から前記脱出位置までの区間のみの、姿勢制御を禁止し、
   前記補間動作区間の終点における前記Ｊ２軸の角度が前記特異領域にあるときは、前記脱出位置が補間動作区間に存在しないと判定して前記脱出位置探索を行わず、前記特異領域の開始点から補間動作の終点までの区間のみ姿勢制御を禁止することを特徴としたロボット制御装置。
2. 前記特異領域は、前記関節部の特異点と、前記特異点を含み、かつ予め定めた前記特異点近傍のＪ２軸の角度とから決定される領域であることを特徴とする請求項１記載のロボット制御装置。
3. 前記補間動作区間の始点と終点におけるＪ２軸の角度が、それぞれ前記特異領域になく、かつ前記補間動作区間の始点と終点におけるＪ２軸の角度符号が同符号であるときは、
   前記特異領域は前記補間動作区間に存在しないと判定し、前記特異領域のみ姿勢制御を禁止することを特徴とした請求項１記載のロボット制御装置。
4. 前記補間動作区間の始点におけるＪ２軸の角度が特異領域にあるときで、かつ前記補間動作区間の始点と終点におけるＪ２軸の角度符号が異符号であるときは、
   前記補間動作区間を予め設定されたサンプリング数で分割した探索点において、Ｊ２軸の角度符号が、前記補間動作区間の始点での符号から異符号に転じる探索点を二分探索法によって探索し、
   異符号に転じる探索点が特異領域外ならば、該探索点を脱出位置とし、
   異符号に転じる探索点が特異領域内であれば、該探索点に最も近接した特異領域外の探索点を探索して脱出位置とすることを特徴とした請求項１記載のロボット制御装置。
5. 前記補間動作区間の始点におけるＪ２軸の角度が特異領域にあるときで、かつ前記補間動作区間の始点と終点におけるＪ２軸の角度符号が同符号であるときは、
   前記補間動作区間を予め設定されたサンプリング数で分割した探索点において、前記探索点でのＪ２軸の角度が特異領域を脱出する探索点を、前記補間動作区間の始点から終点方向まで逐次探索法によって探索し、
   特異領域を脱出した探索点を脱出位置とすることを特徴とした請求項１記載のロボット制御装置。
6. 前記補間動作区間の始点と終点におけるＪ２軸の角度が、それぞれ前記特異領域になく、かつ前記補間動作区間の始点と終点におけるＪ２軸の角度符号が異符号であるときは、前記特異領域があると判定し、
   前記補間動作区間を予め設定されたサンプリング数で分割した探索点において、Ｊ２軸の角度符号が、前記補間動作区間の始点での符号から異符号に転じる探索点を二分探索法によって探索し、
   異符号に転じる探索点が特異領域外ならば、該探索点を脱出位置とし、
   異符号に転じる探索点が特異領域内であれば、該探索点に最も近接した特異領域外の探索点を探索して脱出位置とすることを特徴とした請求項１記載のロボット制御装置。
7. Ｊ３軸の先端に制御点をもち、前記Ｊ３軸から順にＪ２軸、Ｊ１軸の３軸の旋回軸からなる関節部を有するロボットの制御方法において、
   前記関節部の補間動作前に、前記補間動作の始点と終点と軌跡上とに特異領域が存在するかを判定し、
   前記関節部の補間動作区間に特異領域が存在するとき、前記特異領域を脱出する脱出位置を探索する脱出位置探索を行い、前記特異領域のみの、或いは前記特異領域から前記脱出位置までの区間のみの、姿勢制御を禁止し、
   前記補間動作区間の終点における前記Ｊ２軸の角度が前記特異領域にあるときは、前記脱出位置が補間動作区間に存在しないと判定して前記脱出位置探索を行わず、前記特異領域の開始点から補間動作の終点までの区間のみ姿勢制御を禁止することを特徴としたロボットの制御方法。
   

Images