JP2020171989A - ロボット教示システム - Google Patents

Abstract

【課題】多関節ロボットが教示中に特異姿勢に近づいても、特異点及びその近傍でロボットが停止することを回避する。【解決手段】ロボット関節Ｒ５位置が肩特異点及びその近傍にある場合、ロボット手先の進行ベクトルをＸ−Ｙ平面に射影したロボット手先のＸＹ進行ベクトルを計算し、進行方向に対する回避ベクトルを求めることにより、ロボット手先目標位置を補正し、その後、逆運動学を用いて、関節角度を計算して肩特異点回避を行う。Ｒ５関節角度が、手首特異点及びその近傍にある場合、Ｒ５関節角度の符号に依存して、単調増加あるいは単調減少を行い、手首特異点回避後のＲ５関節角度を計算する。その後、ロボット手先目標位置のＲ４関節角度、Ｒ５関節角度、Ｒ６関節角度に近づけるなまし動作を行い、得られた関節角度で多関節ロボットを駆動する。【選択図】 図１４

Description

本発明は、複数の回転関節を持つ多関節ロボットに対して、複雑な動きを教示する際に、相対位置姿勢測定システムを搭載した教示デバイスを備えた多関節ロボットが特異姿勢になり、追従動作が停止する事を回避するためのロボット姿勢制御が可能なロボット教示システムに関する。

従来、ロボットを用いて作業を行わせるためには、あらかじめロボットの行うべき動作、作業の内容をロボットに対して指示することが必要である。このことをティーチング（教示）という。かかる教示の方法としては、教示ペンダントを用いて入力された動作指令に基づいて、ロボットを駆動制御することにより、ロボットアームの各関節の角度、あるいはアーム先端の基準位置、姿勢などを制御する方法や、ロボットを移動させたい方向に力を作用させることにより、ロボットを直接的に誘導し、それにより、ロボットを直交座標系上における所望の位置、姿勢に移動させることができる直接教示が知られている。

しかしながら、教示ペンダントを用いた教示方法では、複雑な動作を教示する際に、操作が煩雑になることで、教示にかかる時間と労力が膨大になり、教示が困難になる問題がある。また、力を作用させた直接教示による教示方法の場合、人の動きに対するロボットの応答遅れがあるために教示中に違和感を与え、意図した軌道と異なる動作をロボットが行うという問題や、人間がロボットに与える力とエンドエフェクタが作業対象物に接触した時に受ける反力との区別がつかず、意図しない動作となる問題がある。

そこで、前述の教示方法の問題を解決する方法として、非特許文献１には、教示者が与える教示点とロボットとを６本のワイヤを介して接続し、そのワイヤの引き出された長さから教示点の相対位置と相対姿勢を計測するパラレルワイヤ機構を用いた教示方法が提案されている。

一方、複数の関節を持つ、いわゆる多関節ロボットには、特異点という機構上の制約を受ける問題がある。かかる特異点とは、関節から延びるアームの機構的な制約により、ある特定の方向の自由度の機能が失われる姿勢を表している。即ち、直交座標系における経路速度を維持しようとすると、関節空間における関節速度が数学的に無限大となり、ロボットの各軸の角速度が急激に上昇し、その結果、ロボットの動作が停止することになる。この問題を回避する方法としては、特異点を考慮した協調運動回避方法や、特異点低感度運動分解回避方法などがある。

非特許文献２には、特異点を考慮した協調運動回避方法が提案されている。前記協調運動回避方法とは、運動の始端、終端条件が与えられたとき、それを満足する運動軌道は無限に存在し、この解の中から作業内容により課せられる制限条件を満たす運動軌道を選ぶために、ロボットの鉛直軸と一致する直交座標系と、作業内容により課せられる制限条件だけでなく特異点の分布を考慮に入れて協調運動を決定するために運動軌道を記述する座標系として円筒座標系と、手先の位置、姿勢が与えられたときのロボットの関節角度の解は、制限しても８通りあるので、運動軌道を記述する座標系として関節角度の各成分を座標値とする固有の座標系の３種類の座標系を適宜使い分けることにより、作業内容により課せられる複雑な制限条件を満たす運動軌道を可動範囲内の全域にわたって計算することができる方法である。

非特許文献３には、特異点低感度運動分解回避方法が提案されている。前記特異点低感度運動分解回避方法とは、ロボットの手先の位置、姿勢と関節角度の関係は、一般的に非線型方程式で表され、これは、位置制御系を構成して制御する場合の運動分解であり、速度制御、加速度制御、トルク制御の際の運動分解もヤコビ行列を係数行列とする線型方程式で表され、これを解くことにより関節角度が計算できる。特異点に近づくとき、関節角度空間により現状関節角度に最も近く、現実可能な動作を生じる関節角度を求めることで特異点を通過する手先の軌道を修正し、特異点を通る軌道を修正する際、手先の位置、姿勢のうち一方はできるだけ正しく保ちたいということがあり、このような場合、優先される変数に対応する成分の値を大きくとって、重み付けをし、優先度の高い変数と低い変数の両方にそれぞれ特異点低感度分解を適用して、低い変数のスケールファクタを大きくすることが有効であり、これによって、動きやすい方向の成分はほぼ実現し、動きにくい方向の成分を修正することで、修正パラメータを特異点と手先の距離から自動調整することで、修正より生じる特異点における不連続な動作をなめらかに補完し、多少軌道からずれているが、特異点に留まることなく通過する方法である。

酒井 昌夫、佐藤 徳孝、森田 良文、"ロボット教示用パラレルワイヤ機構の開発"、[online]、2013年7月23日、日本機械学会、[2018年8月30日]、インターネット、〈URL:https://www.jstage.jst.go.jp/article/kikaic/79/807/79_4321/_pdf/-char/ja〉 内山 勝、"人口の手の運動制御に関する研究"、[online]、昭和51年10月17日、第54期全国大会講演会、[2018年10月10日]、インターネット、〈URL:https://www.jstage.jst.go.jp/article/kikaic1979/45/391/45_391_314/_pdf〉 中村 仁彦、花房 秀朗、"関節系ロボットアームの特異点低感度運動分解"、[online]、昭和59年5月、計測自動制御学会論文集、[2018年10月10日]、インターネット、〈URL:https://www.jstage.jst.go.jp/article/sicetr1965/20/5/20_5_453/_pdf〉 John J.Craig著、三浦 宏文、下山 勲訳 「ロボティクス−機構・力学・制御−」、共立出版株式会社 １９９１年１月初版1刷発行、２０１０年９月初版１２刷発行

しかしながら、非特許文献２に記載の特異点回避方法では、ロボットの運動の始点と終点が確定している場合でなくては使用できず、ロボットが教示された軌道を再生する場合に、移動開始前に特異点及びその近傍でロボットが停止することを回避するように軌道を修正する方法であり、教示の段階での特異点及びその近傍でロボットが停止することを回避することはできないという問題がある。また、非特許文献３は処理に求められる計算量が膨大なため、教示作業中の特異点をリアルタイムで回避するような処理には適さない。

そこで、本発明は、多関節ロボットが教示中に特異姿勢に近づいても、特異点及びその近傍でロボットが停止することを回避することができるロボット姿勢制御が可能なロボット教示システムを提供することを目的とする。

上記目的を達成するための本発明の第１の態様は、複数の回転関節を有する多関節ロボットと、前記多関節ロボットと移動可能な教示点との間の相対位置姿勢を計測する教示デバイスを備えたロボット装置とを用いて、前記教示点を人間が動かすことで前記教示デバイスにより、前記多関節ロボットが追従しながら、前記多関節ロボットに人間が動かした前記教示点の軌道を教示できるロボット教示システムにおいて、教示作業時に前記多関節ロボットが特異点及びその近傍を移動する場合、前記特異点及びその近傍で前記多関節ロボットが停止することを回避しながら教示作業を行うことができる制御手段を有することを特徴とするロボット教示システムを提供する。

また、本発明の第２の態様は、前記第１の態様のロボット教示システムにおいて、前記制御手段は、前記多関節ロボットが前記特異点及びその近傍にあることを判定する特異点判定手段と、前記特異点判定手段により前記多関節ロボットが前記特異点及びその近傍にあると判定されるときの前記複数の回転関節の現在位置に応じて、前記特異点及びその近傍で前記多関節ロボットが停止することを回避するように前記多関節ロボットを駆動する特異点回避制御手段とを有することを特徴とするロボット教示システムを提供する。

また、本発明の第３の態様は、前記第２の態様のロボット教示システムにおいて、前記特異点回避制御手段は、前記多関節ロボットの肩特異点及びその近傍を避けて移動するように前記多関節ロボットを駆動する肩特異点回避駆動制御手段と、前記多関節ロボットの手首特異点及びその近傍で止まらずに通過するように前記多関節ロボットを駆動する手首特異点回避駆動制御手段とを有することを特徴とするロボット教示システムを提供する。

また本発明の第４の態様は、前記第３の態様のロボット教示システムにおいて、前記肩特異点制御手段は、前記特異点判定手段により前記多関節ロボットが前記肩特異点及びその近傍にあると判定されるときの前記複数の回転関節の現在位置に応じて、前記肩特異点及びその近傍を避けて移動するように前記多関節ロボットが教示動作結果を基にロボットが特異点から遠ざかるように追従する軌道の補正を行う肩特異点回避補正手段と、前記肩特異点回避補正手段の補正に基づいて、前記複数の回転関節のそれぞれを回転駆動させて、前記多関節ロボットを駆動する肩特異点回避駆動制御手段を有することを特徴とするロボット教示システムを提供する。

本発明の第５の態様は、前記第３の態様のロボット教示システムにおいて、前記手首特異点制御手段は、前記特異点判定手段により前記多関節ロボットが前記手首特異点及びその近傍にあると判定されるとき、前記多関節ロボットが前記手首特異点及びその近傍で止まらずに通過するように補正を加える手首特異点回避補正手段と、前記手首特異点回避補正手段の補正に基づいて、前記複数の回転関節のそれぞれを回転駆動させて、前記多関節ロボットを駆動する手首特異点回避駆動制御手段を有することを特徴とするロボット教示システムを提供する。

本発明の第６の態様は、前記第１の態様乃至前記第５の態様のロボット教示システムであって、前記教示デバイスが、前記多関節ロボットと移動可能な教示点との間の相対位置姿勢の計測をすることのできる複数のワイヤにより連結されたパラレルワイヤ機構を有するロボット装置であることを特徴とするロボット教示システムを提供する。

本発明の第７の態様は、前記第１の態様乃至前記第５の態様のロボット教示システムであって、前記教示デバイスが、前記多関節ロボットと移動可能な教示点との間の相対位置姿勢の計測をすることのできる複数のリンクにより連結されたパラレルリンク機構を有するロボット装置であることを特徴とするロボット教示システムを提供する。

本発明の第８の態様は、前記第１の態様乃至前記第５の態様のロボット教示システムであって、前記教示デバイスが、前記多関節ロボットと移動可能な教示点との間の相対位置姿勢の計測をすることのできる非接触センサー機構を有するロボット装置であることを特徴とするロボット教示システムを提供する。

本発明の第１の態様によれば、複数の回転関節を有する多関節ロボットが、教示作業中に特異姿勢に近づいたとしても、多関節ロボットの姿勢を制御することにより、特異点及びその近傍で多関節ロボットが停止することを回避することができ、また、複雑な動作を教示する際に、教示ペンダントを用いた教示方法のような煩雑な操作を行う必要が無く、また、従来の直接教示のような人の動きに対するロボットの応答遅れに起因する教示中の違和感をなくすことができる。更に連続する大量の位置情報を与えることで、その軌道を再現する際になめらかな動作ができる。

本発明の第２の態様によれば、複数の回転関節を有する多関節ロボットが、教示中に特異点及びその近傍に近づく際に、特異点近傍だと判定する範囲を設定することで、その範囲内においてロボットが停止しないように補正を加える。その結果特異点近傍においても多関節ロボットの駆動制御を行うことができる。

本発明の第３の態様によれば、複数の回転関節を有する多関節ロボットの肩特異点及びその近傍に近づく際に、肩特異点及びその近傍を避けて移動するように多関節ロボットの駆動制御をすることができ、且つ、多関節ロボットの手首特異点及びその近傍に近づく際に、手首特異点及びその近傍で止まらずに通過するように多関節ロボットの駆動制御を行うことができる。

本発明の第４の態様によれば、複数の回転関節を有する多関節ロボットが肩特異点及びその近傍に近づいたと判定される際に、複数の回転関節の現在の位置に応じて、肩特異点及びその近傍を避けて移動するように多関節ロボットの教示結果を基に追従する軌道の補正を行い、各回転関節のそれぞれの回転駆動の制御を行うことができる。

本発明の第５の態様によれば、複数の回転関節を有する多関節ロボットが手首特異点及びその近傍に近づいたと判定される際に、手首特異点及びその近傍で止まらずに通過するように補正を行い、各回転関節のそれぞれの回転駆動の制御を行うことができる。

本発明の第６の態様によれば、多関節ロボットと教示点との間を複数のワイヤでつなぐことで教示点の移動を可能にし、多関節ロボットと移動可能な教示点との間の相対位置姿勢を計測することができる。

本発明の第７の態様によれば、多関節ロボットと教示点の間を複数のリンクでつなぐことで教示点の移動を可能にし、多関節ロボットと移動可能な教示点との間の相対位置姿勢を計測することができる。

本発明の第８の態様によれば、多関節ロボットと移動可能な教示点との間の相対位置姿勢を、非接触センサーを用いることで計測することができる。

本発明の第１の実施形態に係るロボット教示システムの全体構造を模式的に示す斜視図。 本発明の第１の実施形態に係る多関節ロボットの軸と特異点を説明する説明図。 本発明の第１の実施形態に係る多関節ロボットの追従アルゴリズムを説明する説明図。 本発明の第４の実施形態に係る肩特異点を説明するロボット装置の側面図。 本発明の第４の実施形態に係る肩特異点を説明するロボット装置の上面図。 本発明の第４の実施形態に係る肩特異点回避手段を説明するロボット装置の上面図。 本発明の第４の実施形態に係る肩特異点回避方向を説明する説明図１。 本発明の第４の実施形態に係る肩特異点回避方向を説明する説明図２。 本発明の第４の実施形態に係る肩特異点回避方向を説明する説明図３。 本発明の第４の実施形態に係る肩特異点回避に関する目標位置と移動量の関係を示す推移図。 本発明の第４の実施形態に係る肩特異点回避方向を説明する説明図４。 本発明の第４の実施形態に係る肩特異点回避方向を説明する説明図５。 本発明の第５の実施形態に係る手首特異点回避を説明する説明図。 本発明の第３の実施形態に係る特異点回避を行う教示サイクルのフローチャートを説明する制御ブロック図。 本発明の第３の実施形態に係る肩特異点回避動作判定領域通過時の肩特異点回避駆動手段を適用しないロボット手先の軌道を説明する実験図。 本発明の第３の実施形態に係る肩特異点回避動作判定領域通過時の肩特異点回避駆動手段を適用するロボット手先の軌道を説明する実験図。 本発明の第３の実施形態に係る肩特異点回避駆動制御手段の適用の有無を示す教示中におけるロボット関節Ｒ１角度θ₁の時間変化を説明する実験図。 本発明の第３の実施形態に係る肩特異点回避駆動制御手段の適用の有無を示す教示中におけるロボット関節Ｒ４角度θ₄の時間変化を説明する実験図。 本発明の第３の実施形態に係る肩特異点回避駆動制御手段の適用の有無を示す教示中におけるロボット関節Ｒ６角度θ₆の時間変化を説明する実験図。 本発明の第３の実施形態に係る手首特異点回避動作判定領域通過時の手首特異点回避駆動手段を適用しないロボット手先の軌道を説明する実験図。 本発明の第３の実施形態に係る手首特異点回避動作判定領域通過時の手首特異点回避駆動手段を適用するロボット手先の軌道を説明する実験図。 本発明の第３の実施形態に係る手首特異点回避駆動制御手段の適用の有無を示す教示中におけるロボット関節Ｒ５角度θ₅の時間変化を説明する実験図。 前述の図２２の実験図の手首特異点回避動作判定領域を通過する部分の拡大図。 本発明の第３の実施形態に係る手首特異点回避駆動制御手段の適用の有無を示す教示中におけるロボット関節Ｒ４角度θ₄の時間変化を説明する実験図。 本発明の第３の実施形態に係る手首特異点回避駆動制御手段の適用の有無を示す教示中におけるロボット関節Ｒ６角度θ₆の時間変化を説明する実験図。

以下、本発明のロボット教示システムの実施形態について、図１から図２５を参照しながら説明する。

図１は、本発明の実施形態のロボット教示システムの全体構造を模式的に示す斜視図である。ロボット装置１は、複数の関節を供えた垂直多関節ロボット（以下、「多関節ロボット」という）２と、多関節ロボット２の先端に取り付けられるエンドエフェクタ３とを備える。これに加えて多関節ロボット２を駆動する制御装置４を備える。かかる構造において、エンドエフェクタ３は、多関節ロボット２の先端に着脱自在に取り付けられており、作業に基づいて交換可能であり、例えば、エアシリンダを使ったハンドや、バリ取り作業用のブラシなどが用いられる。

多関節ロボット２の特異点について以下に説明する。尚、一般の特異点の説明として、非特許文献４、１４４頁〜１４５頁には、普通、産業用ロボットには逆ヤコビ行列が存在しない関節角度が存在し、その状態が特異点と記載されている。

図２は、本発明の実施形態の多関節ロボット２の軸と特異点を説明する説明図である。図２に示すように、本実施形態の多関節ロボット２は、６つの回転関節（Ｒ１〜Ｒ６）と、６つの関節軸（Ｒ１軸〜Ｒ６軸）を中心に回転駆動する駆動部を備えている。

例えば、本発明の実施形態の６軸の多関節ロボット２には、肩特異点、肘特異点、手首特異点の３つの特異点が存在する。このため、多関節ロボット２が３つの特異点に同時に近づく場合もある。以下に、多関節ロボット２が有する３つの特異点について説明する。一般的に産業用ロボットにおいて、Ｒ１軸の方向とＲ２軸の方向とは直交し、Ｒ２軸の方向とＲ３軸の方向とは平行になり、Ｒ３軸の方向とＲ４軸の方向は直交し、Ｒ４軸の方向とＲ５軸の方向は直交し、Ｒ５軸とＲ６軸の方向は直交し、Ｒ４軸、Ｒ５軸及びＲ６軸はＲ５軸上で交わるように設計されるため、この条件に基づいて説明する。

多関節ロボット２において、肩特異点は、Ｒ４軸、Ｒ５軸及びＲ６軸が交わる点がＲ１軸上にある場合である。肘特異点は、Ｒ２軸とＲ３軸とがＲ４軸上にある場合である。手首特異点は、Ｒ４軸とＲ６軸とが同軸上にあり、Ｒ５の角度が０になる場合である。

前記各特異点上において、ロボットの角速度が無限大の値が必要になるが、ロボットの関節は、無限大の速度で回転することができないため、動作が停止してしまう。また、前記各特異点の近傍では、エンドエフェクタ３を一定速度で動かそうとすると、ロボットの関節が高速で動かなければならないので、ロボットの動作が不安定になり、特異点に近いほど不安定な動作を起こすようになる。

多関節ロボット２に教示ペンダントなどを用いて動作を教示する際には、上述した３つの特異点に近づかないようにしなければならない。なお、肘特異点の特異姿勢は、教示システムにおいて多関節ロボット２の可動範囲を超える教示をしていることになるので、本発明の実施形態では、教示に関係しないことから説明の対象としない。

ロボットが指定した位置、姿勢になるために各関節角度を求める必要がある。例えば、非特許文献４、１１０頁〜１１４頁には、逆運動学の解法によって、多関節ロボット２の各関節角度を求めることが記載されている。

次に、ロボットの追従アルゴリズムについて説明する。図３は、本発明の実施形態の多関節ロボット２の追従アルゴリズムを説明する説明図である。多関節ロボット２の先端部にエンドエフェクタ３として、教示デバイス１０を取り付け、教示デバイス１０の先端にある教示点１１を移動させることによって、教示動作を行うと教示デバイス１０にロボットが追従しながら教示が行われる。ここで、Ｒ１軸と同じ軸をＺ軸とし、Ｚ軸に垂直な軸をそれぞれＸ軸、Ｙ軸としている。

本発明の実施形態で用いている教示デバイス１０は、非特許文献１に記載されているパラレルワイヤ機構を用いた教示装置であり、パラレルワイヤ機構のベース１３と可動プラットフォーム１２を６本のワイヤでつなげ、６本のワイヤの既知の位置、姿勢を初期値として、移動後の位置、姿勢と引き出されたワイヤの長さの関係より教示点を求める方法を利用しているが、別の方法でもよい。例えば、ワイヤを用いずに、ベース１３と可動プラットフォーム１２を複数のリンクでつなぐパラレルリンク機構や、ベース１３と可動プラットフォーム１２間の位置、姿勢の関係を非接触センサーなどによって測定することで教示点を求める方法でもよい。非接触センサーは、例えば、光センサー、磁気センサー、カメラなどがある。

可動プラットフォーム１２とベース１３とは、６本のワイヤによって連結されているために、機構が剛体として連結していないので、教示時にベース１３は、人の動きに対するロボットの応答遅れを吸収して追従するように駆動することができる。

図１の制御装置４がロボット装置１から得られた現状のロボット関節角度を下記の式とする。

制御装置４においてロボット手先目標位置１７を計算する。上記の式より順運動学を用いて計算した現在のロボット手先位置１４(X_R)と、ベース１３から移動させた可動プラットフォーム１２までの相対位置１５(X_P)と、可動プラットフォーム１２に対し、ベース１３の姿勢が平行で一定距離離れるための追従オフセット１６(X_d)からロボット手先目標位置１７(X_bT)の計算を行うと、下記の式になる。ここで、X_Hは、教示点である。

上記の式で求めた、ロボット手先目標位置１７から逆運動学を用いて多関節ロボット２の各関節角度を求め、ロボット装置１に送ることでロボットが駆動する。

教示することにより得られたデータをもとに多関節ロボット２を再生する時には、可動プラットフォーム１２とベース１３をホルダで固定させ、その時のスキマ量を追従オフセット１６の値とすることでロボット再生目標位置を得ることができ、多関節ロボット２は教示したとおりの軌道で駆動することができる。

次に、肩特異点とその回避方法について説明する。図４は、本発明の実施形態の肩特異点を説明するロボット装置１の側面図であり、図５は、本発明の実施形態の肩特異点を説明するロボット装置１の上面図である。図４及び図５は、多関節ロボット２が肩特異点回避駆動制御手段を適用せず、肩特異点の影響で多関節ロボット２が停止する時の側面図及び上面図である。Ｒ１軸をＺ軸とし、Ｘ−Ｙ平面上の（０，０）座標において、Ｒ４軸、Ｒ５軸及びＲ６軸が交わる点がＺ軸上に移動した時、つまり、Ｒ５の位置ＸＹ成分がｘ＝０、ｙ＝０となる時が肩特異点となり、その近傍では多関節ロボット２の動作が停止する。

図６は本発明の実施形態の肩特異点回避手段を説明するロボット装置１の上面図である。図６は、図４及び図５と同じ軌道で教示動作を行い、多関節ロボット２が肩特異点回避を行う場合の動作を表す上面図である。ＸＹＺ軸の配置はそれぞれ前述の図４、図５と同じとする。多関節ロボット２の先端部に取り付けられた教示デバイス１０の先端部にある教示点１１を移動させることで教示動作を行う。Ｒ５が肩特異点回避動作判定領域１８に近づくと、Ｒ５が肩特異点回避動作判定領域１８の中心に近づかないように、つまり特異点及びその近傍を避けて移動するように制御装置４が多関節ロボット２を駆動する。そのため、図６の移動後の図ではＲ５がＺ軸上から離れて位置するので、ロボット手先位置１４が図５のロボット手先位置１４と異なる。この位置の違いを、教示デバイス１０が可動プラットフォーム１２とベース１３の位置の違いとして吸収する。なお、図６に記載の肩特異点回避動作判定領域１８は、肩特異点の近傍を表しており、Ｘ−Ｙ平面上の（０，０）座標を中心に円柱状になっているが、別の形になるように設定してもよい。例えば、楕円柱状になってもよい。

図７、図８及び図９を説明する。図７は、本発明の実施形態の肩特異点回避方向を説明する説明図１であり、図８は、本発明の実施形態の肩特異点回避方向を説明する説明図２であり、図９は、本発明の実施形態の肩特異点回避方向を説明する説明図３である。図７、図８及び図９において、Ｘ−Ｙ平面上の（０，０）座標を肩特異点２０とする。

図７の説明図１は、ロボット手先位置１４からロボット手先目標位置１７に向かうロボット手先の進行ベクトルをＸ−Ｙ平面に射影したロボット手先のＸＹ進行ベクトル２１に対して、ベース１３が９０度の方向へ肩特異点回避をした場合の説明図である。可動プラットフォーム１２を次の可動プラットフォーム１２’の位置に移動させた場合、ベース１３は可動プラットフォーム１２’に追従するようにベース１３’に移動しようとするが、Ｒ５の目標位置２４が肩特異点回避動作判定領域１８に進入する場合は、ロボット手先のＸＹ進行ベクトル２１に対して、９０度の方向に移動するような補正が加わることで、ベース１３’’の位置に移動する。

図８の説明図２は、ロボット手先のＸＹ進行ベクトル２１に対して、ベース１３が０度の方向へ肩特異点回避をした場合の説明図である。９０度の方向へ特異点回避動作をした場合と同様に、可動プラットフォーム１２を次の可動プラットフォーム１２’の位置に移動させた場合、ベース１３は可動プラットフォーム１２’に追従するようにベース１３’に移動しようとするが、Ｒ５の目標位置２４が肩特異点回避動作判定領域１８に進入する場合は、ロボット手先のＸＹ進行ベクトル２１に対して、０度の方向に移動するような補正が加わることで、ベース１３’’の位置に移動する。しかし、この場合は、可動プラットフォーム１２’とベース１３’’が接近しすぎてしまい、衝突してしまう可能性があるので使用することができない。

図９の説明図３は、ロボット手先のＸＹ進行ベクトル２１に対して、ベース１３が１８０度の方向へ肩特異点回避をした場合の説明図である。９０度の方向へ肩特異点回避をした場合と同様に、可動プラットフォーム１２を次の可動プラットフォーム１２’の位置に移動させた場合、ベース１３は可動プラットフォーム１２’に追従するようにベース１３’に移動しようとするが、Ｒ５の目標位置２４が肩特異点回避動作判定領域１８に進入する場合は、ロボット手先のＸＹ進行ベクトル２１に対して、１８０度の方向に移動するような補正が加わることで、ベース１３’’の位置に移動する。しかし、この場合は、可動プラットフォーム１２’とベース１３’’が離れすぎてしまい、動作遅れが増大してしまう為に使用することができない。

以上、図７、図８及び図９の説明により、本発明の実施形態では、図７のロボット手先のＸＹ進行ベクトル２１に対して、９０度の方向へ肩特異点回避動作を行うように補正を行う。

ロボット手先の進行ベクトル(V_T)は、以下の式で求められる。

ロボット手先の進行ベクトル(V_T)をＸ−Ｙ平面に射影したロボット手先のＸＹ進行ベクトル２１(V_Txy)を計算し、これに対して、９０度の方向へ肩特異点回避を行う回避ベクトル２２(ΔX_d)を求めることにより、ロボット手先回避目標位置２３（X_hT1）を下記の式で表せる。

上記の式より、図７のロボット手先回避目標位置２３を求めた後に、逆運動学を用いて、多関節ロボット２の各関節角度を求め、ロボット手先回避目標位置２３にロボットの手先が移動することで、多関節ロボット２が肩特異点回避動作を行うことができる。

図１０は、本発明の実施形態の肩特異点回避に関するＲ５の目標位置２４と移動量との関係を示す推移図である。図１０は、図７における肩特異点２０とＲ５の目標位置２４間の距離rと、肩特異点回避による移動量との関係を示したグラフ図である。横軸をＸ−Ｙ平面上の肩特異点２０とＲ５の目標位置２４との距離r、縦軸を肩特異点回避による移動量f₀、c₀は、肩特異点回避を行う時の最大移動量であり、l₀は、肩特異点回避動作判定領域１８の円半径である。肩特異点２０とＲ５の距離rと、肩特異点回避による移動量f₀との関係は、図１０のようなグラフになる。

本発明の実施形態では、ベース１３と可動プラットフォーム１２間の距離は、教示動作中は通常５０mmに保たれており、肩特異点回避を行う時の最大移動量c₀が３０mmより大きくなると、ロボット手先の姿勢によってはベース１３上にある可動プラットフォーム１２を再生時に固定するホルダと可動プラットフォーム１２が衝突する事があるので、衝突しないようにc₀=２５mmとした。また、肩特異点回避動作判定領域１８は、ロボットの原点座標から半径l₀=４０mmの円柱状にした。ここで、今回は、最大移動距離c₀=２５mmとしたが、教示デバイス１０の形状によって変化する物である。また、肩特異点回避動作判定領域１８を円柱状で表しているが、別の形にしてもよい。例えば、楕円柱状などでもよい。その場合は、肩特異点２０とＲ５の目標位置２４の距離と、肩特異点回避による移動量との関係は別のグラフに表される。

肩特異点回避を行う場合、多関節ロボット２の肩特異点回避による移動量f₀は、最大移動量c₀、肩特異点回避動作判定領域１８の半径l₀、Ｘ−Ｙ平面上の肩特異点２０とＲ５の目標位置２４との距離より、下記の式に表せる。

図１１を説明する。図１１は、本発明の実施形態の肩特異点回避方向を説明する説明図４である。図７の説明において、ロボット手先のＸＹ進行ベクトル２１に対して、９０度の方向へ肩特異点回避動作を行うように補正を行う際に、図１１の説明図４は、Ｚ軸回り＋９０°方向へ肩特異点回避動作を行った時の説明図である。移動前のＲ５位置３０がロボット手先のＸＹ進行ベクトル２１により、移動後のＲ５位置３１に移動した時に、移動後のＲ５位置３１が肩特異点回避動作判定領域１８内にある場合、Ｚ軸回り＋９０°方向の回避ベクトルＬ３２(ΔX_dL)は、肩特異点回避による移動量f₀、Ｚ軸回りの＋９０°回転行列(R_zL)、ロボット手先のＸＹ進行ベクトル２１(V_Txy)より、以下の式で求められる。

次に、Ｚ軸回り＋９０°方向に移動した場合のロボット手先回避目標位置Ｌ３３（X_hTL）は以下の式で表せる。

図１２を説明する。図１２は、本発明の実施形態の肩特異点回避方向を説明する説明図５である。図７の説明において、ロボット手先のＸＹ進行ベクトル２１に対して、９０度の方向へ肩特異点回避動作を行うように補正を行う際に、図１２の説明図５は、Ｚ軸回り−９０°方向へ肩特異点回避動作を行った時の説明図である。移動前のＲ５位置３０がロボット手先のＸＹ進行ベクトル２１により、移動後のＲ５位置３１に移動した時に、移動後のＲ５位置３１が肩特異点回避動作判定領域１８内にある場合、Ｚ軸回り−９０°方向の回避ベクトルＲ３２’(ΔX_dR)は、肩特異点回避による移動量f₀、Ｚ軸回りの−９０°回転行列(R_zR)、ロボット手先のＸＹ進行ベクトル２１(V_Txy)より、以下の式で求められる。

次に、Ｚ軸回り−９０°方向に移動した場合のロボット手先回避目標位置Ｒ３３’（X_hTR）は以下の式で表せる。

ロボット手先回避目標位置Ｌ３３に移動した時のＲ５回避目標位置Ｌ３４と、ロボット手先回避目標位置Ｒ３３’に移動した時のＲ５回避目標位置Ｒ３４’の二つを比べて、特異点２０から離れている方の回避ベクトルＬ３２(ΔX_dL)もしくは回避ベクトルＲ３２’(ΔX_dR)を回避ベクトル２２(ΔX_d)とする事で、図７のロボット手先回避目標位置２３(X_hT1)を求める。ロボット手先回避目標位置２３より逆運動学を用いて、多関節ロボット２の各関節角度を求め、ロボット手先回避目標位置２３にロボットの手先が移動することで、多関節ロボット２が肩特異点回避動作を行うことができる。以上により、多関節ロボット２を駆動する肩特異点回避駆動制御手段を説明した。

肩特異点回避駆動制御手段を適用した各目標関節角度の成分は、下記のように表される。

次に、手首特異点とその回避方法について説明する。手首特異点回避は肩特異点回避動作判定を行った後に行う。図１３は、本発明の実施形態の多関節ロボット２が手首特異点回避を行う場合を説明する説明図である。前述したように、手首特異点とは、Ｒ４軸とＲ６軸とが同軸上にある場合、つまり、回転関節Ｒ５の関節角度θ₅＝０になる時である。多関節ロボット２が手首特異点の条件を満たす時、回転関節Ｒ４の関節角度θ₄及びＲ６の関節角度θ₆の値は求めることができず、手首特異点近傍では、関節角度θ₄及び関節角度θ₆の値が急激に上昇し、関節速度の制限以上の速さで動作しようとして、多関節ロボット２は安全性のため停止する。図１３に記載の手首特異点回避動作判定領域４０のＲ５関節角度の制限値θ_limitの範囲内が、手首特異点の近傍を表している。ロボット追従動作になるべく支障にならないようにθ_limit＝２．０degとした。

手首特異点回避動作判定後における手先目標位置の各関節角度の成分は、下記のように表す。Ｒ１からＲ３は修正しないのでそのまま肩特異点回避動作判定後の角度を使用する。

目標関節角度θ_ht15が手首特異点回避動作判定領域４０に入ったら出るまでＲ５の逆運動学計算をやめる。

Ｒ４の目標関節角度は逆運動学計算より求め、それから求めた角速度が上限値もしくは下限値を超える場合、上限値もしくは下限値となるように補正する。Ｒ６の目標関節角度も逆運動学計算より求め、それから求めた角速度が上限値もしくは下限値を超える場合、上限値もしくは下限値となるように補正する。Ｒ５の目標関節角度は、手首特異点回避動作判定領域４０を通過するように計算する。手首特異点回避動作判定領域４０に入る前のＲ５の現在角度の符号に依存し、負の場合は、手首特異点回避動作判定領域４０を通過するまで目標関節角度を増加させ、正の場合は、手首特異点回避動作判定領域４０を通過するまで減少させる。例えば、単調増加あるいは単調減少がある。

手首特異点回避動作判定時の目標関節角度を計算するときのステップ数をnステップとすると、この時点での現在ロボットＲ５関節角度はθ_R5(n-1)である。Ｒ５の目標関節角度θ_hT15(n）が手首特異点回避動作判定領域４０に入った時、角度の変化量Δθ_T5>0(一定)とし、手首特異点回避動作判定後のＲ５目標関節角度θ_hT25(n)は、現在のＲ５関節角度θ_R5(n-1)の符号により決まり、以下のような式になる。Δθ_T5はθ_limitの値より小さい値とする。

Ｒ５の目標関節角度が手首特異点回避動作判定領域４０から出た後、手先目標位置が不連続になることを避けるために、ロボット手先位置１４を図３のロボット手先目標位置１７に徐々に近づける。ロボット手先回避目標位置は、手首特異点回避動作判定領域４０から出た時のロボット手先位置１４と、ロボット手先目標位置１７に基づいて、ロボット手先位置がロボット手先目標位置１７に遅れながら近づくものである。例えば、なまし処理がある。ロボット手先目標位置１７に近づくときの時定数をtcと設定する。継続してロボットの追従動作ができるようにtc＝０．５secとした。

上記の工程を行うことにより、多関節ロボット２が手首特異点回避動作を行うことができる。以上で多関節ロボット２を駆動する手首特異点回避駆動制御手段を説明した。

手首特異点回避動作時に肩特異点に近づく場合は、肩特異点回避動作は、移動後のＲ５位置３１の位置成分のみを修正し、姿勢を修正せず、手首特異点回避動作は、ロボット手先の姿勢のみを修正し、位置を修正しない為、お互いの回避動作に影響を及ぼさずに同時に回避動作を行うことができる。
肩特異点回避動作時に手首特異点に近づく場合は、６軸多関節ロボットのリンク構造より、手首特異点回避動作が肩特異点回避動作に影響を及ぼし動作が不安定になってしまうので肩特異点回避動作時には手首特異点回避動作をしないようにする必要がある。

図１４を説明する。図１４は、本発明の実施形態の特異点回避を行う教示サイクルのフローチャートを示すブロック図である。教示開始前に、回避パラメータである肩特異点回避動作判定領域１８の円半径l₀、肩特異点回避を行う時の最大移動量c₀、手首特異点回避動作判定領域４０の境界角度θ_limit、なまし処理の時定数tcの設定を行う。次に、現在のロボット手先位置１４(X_R)と、教示デバイス１０からの情報を利用して、ロボット手先目標位置１７(X_bt)の位置計算を行う。このとき、移動後のＲ５位置３１が肩特異点回避動作判定領域１８内にあるなしの判定を行い、範囲内にない場合は、ロボット手先目標位置１７より、逆運動学を用いて、関節角度θ_hT1を計算する。このとき、Ｒ５の目標関節角度θ_hT1が、手首特異点回避動作判定領域４０内にあるなしの判定を行い、範囲内にない場合は、関節角度θ_hT1で制御装置４が多関節ロボット２を駆動する。

移動後のＲ５位置３１が肩特異点回避動作判定領域１８内にある場合は、ロボット手先位置１４からロボット手先目標位置１７までとするロボット手先の進行ベクトル(V_T)を計算し、次に、この進行ベクトルをＸ−Ｙ平面に射影したロボット手先のＸＹ進行ベクトル２１(V_Txy）を計算し、ロボット手先ＸＹ進行ベクトル２１に対して、Ｒ５の目標位置２４が特異点２０から離れる９０度の方向へ肩特異点回避を行う回避ベクトル２２(ΔX_d）を求めることにより、ロボット手先回避目標位置２３（X_hT1）を求める。その後、逆運動学を用いて、目標関節角度θ_hT1を計算することで、肩特異点回避を行う。

Ｒ５の関節角度θ_hT15が、手首特異点回避動作判定領域４０内にある場合は、Ｒ５の逆運動学計算をやめ、Ｒ４及びＲ６の速度を制限し、特異点回避動作判定領域４０に入る前のＲ５現在関節角度θ_R5の符号に依存して、単調増加あるいは単調減少を行い、手首特異点を通過するＲ５関節角度θ_hT25を計算する。手首特異点回避動作判定領域４０を出た後、Ｒ４、Ｒ５及びＲ６のなまし処理を行うことで、ロボット手先目標位置１７のＲ４関節角度θ_hT14、Ｒ５関節角度θ_hT15、Ｒ６関節角度θ_hT16に近づけるなまし動作を行う。そして、得られた関節角度θ_hT2で制御装置４が多関節ロボット２を駆動することで、手首特異点回避を行う。以上により、多関節ロボット２の特異点回避を行う教示サイクルを説明した。

図１５及び図１６を説明する。これらの図は本発明の実施形態のロボット制御システムで肩特異点を通過する動作を試行した実験の結果であり、これらは図５のＹ軸方向にＲ５が肩特異点を通過するよう教示点１１を人が動かした例である。ロボット手先の進行方向に対して肩特異点回避方向を９０°で実施したが、この肩特異点回避を行う方向は８０°から１００°程度に収めることが望ましい。教示点１１に追従するＲ５の軌道として図５のＸ−Ｙ平面上の位置で表したものである。図１５は肩特異点回避駆動制御手段を適用しない場合であり、教示点１１の移動にＲ５が追従しているが、肩特異点付近で動作停止している。図１６は肩特異点回避駆動制御手段を適用した場合であり、移動後のＲ５位置３１が肩特異点動作判定領域１８に入ると＋Ｘ方向に補正しながら教示点１１に追従し、さらに肩特異点回避動作判定領域内１８であっても特異点から離れるにつれて、本来の軌道に戻って追従を継続することができる。

動作中のロボットのうち、肩特異点で関節角度の変化が過大になるロボットの関節Ｒ１、Ｒ４及びＲ５について説明する。図１７乃至図１９を説明する。図１５及び図１６の教示を行ったときの関節角度の時間変化を示す。縦軸は関節角度を示し、横軸は教示開始からの時間経過を示す。肩特異点回避駆動制御手段を適用しない場合は特異点近傍で、Ｒ１、Ｒ４及びＲ６の関節角度θ₁、θ₄、θ₆が急激に増加してロボットが停止するのに対し、肩特異点回避駆動制御手段を適用した場合は、肩特異点回避動作判定領域通過時の関節角度θ₁、θ₄、θ₆の変化量が抑制され、動作停止することなく教示点の追従動作を継続している。

図２０及び図２１を説明する。これらの図は本発明の実施形態のロボット制御システムで手首特異点(θ₅＝０)を通過する動作を試行した結果であり、これらはＺ軸方向にＲ５が手首特異点を通過するよう教示点１１を人が動かし、ロボット手先が追従した例である。図２０は手首特異点回避駆動制御手段を適用しない場合であり、特異点近傍であるＺ＝６００付近で追従動作が停止してしまうのに対し、図２１に示す様に手首特異点回避機能を適用することで手首特異点を通過後もロボット手先が教示点１１に追従しており、手首特異点回避駆動制御手段により教示点を追従する機能は維持している。

教示中のロボットのうち、手首特異点で関節角度の変化が過大になるロボットの関節Ｒ４、Ｒ５及びＲ６について説明する。図２２乃至図２５を説明する。図２０及び図２１の教示を行ったときの関節角度の時間変化を示す。縦軸は関節角度を示し、横軸は教示開始からの時間経過を示す。図２２はＲ５の角度を示し、図２３は前述の図２２の手首特異点回避動作判定領域４０を通過する付近の拡大図である。手首特異点回避駆動制御手段を適用しない場合は手首特異点回避動作判定領域４０を通過出来ずロボットが停止しているが、手首特異点回避駆動制御手段を適用する場合は手首特異点(θ₅＝０)を通過している。
また、図２０、図２１におけるＲ４、Ｒ６の関節角度の変化を図２４、図２５に示す。手首特異点回避駆動制御手段を適用しない場合は特異点近傍で、Ｒ４、Ｒ６の関節角度θ₄、θ₆が急激に増加してロボットが停止するのに対し、手首特異点回避駆動制御手段を適用した場合は、手首特異点(θ₅＝０)通過時の関節角度θ₄、θ₆の変化量が抑制され、動作停止することなく教示動作を継続できる。

肩特異点回避駆動制御手段及び手首特異点回避駆動制御手段により、肩及び手首特異点へ近づいても教示点１１へロボット手先が追従するため、特異点とその近傍でも教示が可能になり、肩特異点回避駆動制御手段及び手首特異点回避駆動制御手段は有効である。

１ ロボット装置
２ 多関節ロボット
３ エンドエフェクタ
４ 制御装置
１０ 教示デバイス
１１ 教示点
１２ 可動プラットフォーム
１３ ベース
１４ ロボット手先位置
１５ １２と１３の相対位置
１６ 追従オフセット
１７ ロボット手先目標位置
１８ 肩特異点回避動作判定領域
２０ 肩特異点
２１ ロボット手先のＸＹ進行ベクトル
２２ 回避ベクトル
２３ ロボット手先回避目標位置
２４ Ｒ５の目標位置
３０ 移動前のＲ５位置
３１ 移動後のＲ５位置
３２ 回避ベクトルＬ
３２’ 回避ベクトルＲ
３３ ロボット手先回避目標位置Ｌ
３３’ ロボット手先回避目標位置Ｒ
３４ Ｒ５回避目標位置Ｌ
３４’ Ｒ５回避目標位置Ｒ
４０ 手首特異点回避動作判定領域

Claims (8)

1. 複数の回転関節を有する多関節ロボットと、前記多関節ロボットと移動可能な教示点との間の相対位置姿勢を計測する教示デバイスを備えたロボット装置とを用いて、前記教示点を人間が動かすことで前記教示デバイスにより、前記多関節ロボットが追従しながら、前記多関節ロボットに人間が動かした前記教示点の軌道を教示できるロボット教示システムにおいて、教示作業時に前記多関節ロボットが特異点及びその近傍を移動する場合、前記特異点及びその近傍での前記多関節ロボットの停止を回避しながら教示作業を行うことができる制御手段を有することを特徴とするロボット教示システム。
2. 前記制御手段は、前記多関節ロボットが前記特異点及びその近傍にあることを判定する特異点判定手段と、前記特異点判定手段により前記多関節ロボットが前記特異点及びその近傍にあると判定されるときの前記多関節ロボットの複数の回転関節の現在位置に応じて、前記特異点及びその近傍を前記多関節ロボットが停止するのを回避するように前記多関節ロボットを駆動する特異点回避制御手段とを有することを特徴とする請求項１記載のロボット教示システム。
3. 前記特異点回避制御手段は、前記多関節ロボットの肩特異点及びその近傍を避けて移動するように前記多関節ロボットを駆動する肩特異点回避制御手段と、前記多関節ロボットの手首特異点及びその近傍で止まらずに通過するように前記多関節ロボットを駆動する手首特異点回避制御手段とを有することを特徴とする請求項２記載のロボット教示システム。
4. 前記肩特異点制御手段は、前記特異点判定手段により前記多関節ロボットが前記肩特異点及びその近傍にあると判定されるときの前記多関節ロボットの複数の回転関節の現在位置に応じて、前記肩特異点及びその近傍を避けて移動するように前記多関節ロボットが教示動作の結果を基に追従するロボットが特異点から遠ざかる様に軌道の補正を行う肩特異点回避補正手段と、前記肩特異点回避補正手段の補正に基づいて、前記多関節ロボットの複数の回転関節のそれぞれを回転駆動させて、前記多関節ロボットを駆動する肩特異点回避駆動制御手段を有することを特徴とする請求項３記載のロボット教示システム。
5. 前記手首特異点制御手段は、前記特異点判定手段により前記多関節ロボットが前記手首特異点及びその近傍にあると判定されるとき、前記多関節ロボットが前記手首特異点及びその近傍で止まらずに通過するように補正を加える手首特異点回避補正手段と、前記手首特異点回避補正手段の補正に基づいて、前記多関節ロボットの複数の回転関節のそれぞれを回転駆動させて、前記多関節ロボットを駆動する手首特異点回避駆動制御手段とを有することを特徴とする請求項３記載のロボット教示システム。
6. 請求項１乃至請求項５に記載のいずれか一つのロボット教示システムであって、前記教示デバイスが、前記多関節ロボットと移動可能な教示点との間の相対位置姿勢の計測をすることのできる複数のワイヤにより連結されたパラレルワイヤ機構を有するロボット装置であることを特徴とするロボット教示システム。
7. 請求項１乃至請求項５に記載のいずれか一つのロボット教示システムであって、前記教示デバイスが、前記多関節ロボットと移動可能な教示点との間の相対位置姿勢の計測をすることのできる複数のリンクにより連結されたパラレルリンク機構を有するロボット装置であることを特徴とするロボット教示システム。
8. 請求項１乃至請求項５に記載のいずれか一つのロボット教示システムであって、前記教示デバイスが、前記多関節ロボットと移動可能な教示点との間の相対位置姿勢の計測をすることのできる非接触センサーを持つ非接触センサー機構を有するロボット装置であることを特徴とするロボット教示システム。