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

Description

本発明は、ロボットの制御装置および制御方法に関するものであり、特に、特異点および特異点近傍の通過制御を行うロボットの制御装置および制御方法に関するものである。

ロボットの制御点の軌跡が直線を描くように動作させる直線補間動作は、まず、動作開始位置と目標位置にかかる位置データおよび姿勢データ（以下「直交データ」と呼称）をロボットの制御装置に予め記憶しておき、つぎに、それら２つの位置の間にロボットの制御周期ごとに補間される制御点が直線を描くような直交データを算出するとともに、その直交データから座標変換によって各関節軸の角度（以下「関節データ」と呼称）を算出し、さらに、その関節データに一致するように各軸のモータを回転制御することにより実現している。

ところで、多関節型のロボットの場合、一つの直交データに対して複数の関節データを算出することができる。このことは、一つの直交データに対してとり得る関節データが複数あることを意味しており、実際の制御の場面では、それらの中から所望の関節データを一意に定める必要がある。なお、関節データを一意に定めるための手法の一つとして、例えばフラグを用いる手法がある。この手法では、例えば関節データを一意に定めるためのフラグを直交データの中に設けておき、直交データから関節データを算出する際に、当該直交データに設けられたフラグに基づいて関節データを一意に定めるようにしている。

一方、多関節型のロボットにおいて、直交データから関節データに座標変換する際に一意に関節データを求めることのできない特異点と呼ばれる位置が存在する。このような特異点が存在する場合、多関節型のロボットを特異点あるいは特異点の近傍を通過させるために先のフラグに基づいて関節データを算出しようとすると、ある関節軸の回転速度が非常に大きな値をとることが知られている。

ところが、実際にはサーボモータやその他ロボットを構成する部品等の制限により、そのような大きな速度で回転することはできないので、この種の問題を回避するための特異点および特異点近傍での制御処理が必須となってくる。

そこで、従来は、特異点やその近傍を通過する際に発生するこの種の問題を回避するために、関節軸の動作速度を、補間動作中において常時算出するとともに、算出された動作速度の増加量に基づいてロボットが特異点に近付いたか否かを判定し、近付いたと判断した場合に特異点近傍を通過できるように指令速度を補正するといった方法が提案されている（例えば、特許文献１）。

特開平６−３２４７３０号公報

しかしながら、上記特許文献１に示される従来技術のような関節軸の動作速度に基づいて特異点に近づいたか否かを判定するような方法では、通常の動作において誤判定することがないように、判定基準となる速度しきい値を各関節軸の最高速度以上に設定しておく必要があった。そのため、特異点近傍において、指令速度が低速度であっても一部の関節軸が最高速度付近で回転しているような状態が生起することになる。このような状態を許容することは、意図しない動作を生じさせる可能性を増大させることにつながり、ロボットを制御する上で危険な状態にあるといった問題点があった。

本発明は、上記に鑑みてなされたものであって、ロボットに対する指令速度が低速度であっても一部の関節軸の回転速度が最高速度付近まで上昇するといった問題点を解消するとともに、ロボットが速度過大となることなく、あるいは停止することなく特異点および特異点近傍を通過させることができるロボットの制御装置および制御方法を提供することを目的とする。

上述した課題を解決し、目的を達成するため、本発明にかかるロボットの制御装置は、特異点近傍を含む特異点領域に進入するロボットを制御するための特異点処理機能を有する補間処理部を備えたロボットの制御装置において、前記補間処理部は、速度指令に基づいて位置指令を生成出力する補間位置生成部と、前記位置指令に基づく関節データを生成出力する座標変換部と、前記位置指令と前記関節データと前記特異点領域情報とに基づいてロボットが特異点領域に進入したか否かを判定する進入判断部と、前記進入判断部の判定結果に基づいて起動され、特異点領域に進入したロボットが該特異点領域を通過する際の位置指令を生成出力する特異点通過位置指令生成部を具備する特異点通過位置生成部と、を備えたことを特徴とする。

この発明によれば、位置指令、この位置指令にかかる関節データおよび特異点領域情報に基づいてロボットが特異点領域に進入したか否かが判定されるものであり、特異点または特異点近傍にあることが、速度の変化ではなく指令位置に基づいているので、指令速度が低速度であっても、ある関節軸が高速に動作することはない。

本発明にかかるロボットの制御装置および制御方法によれば、位置指令や、位置指令にかかる関節データおよび特異点領域情報に基づいてロボットが特異点領域に進入したか否かを判定するようにしているので、関節角度の変動が低減され、特異点領域を滑らかに通過することが可能となる。また、速度変化を用いずに特異点付近であるかどうかを判定しているので、指令速度が低速度であっても、ある関節軸が高速に動作することなく特異点を通過させることができる。

以下に、本発明にかかるロボットの制御装置および制御方法の実施の形態を図面に基づいて詳細に説明する。なお、この実施の形態により本発明が限定されるものではない。

図１は、本発明にかかるロボットの制御装置の機能構成を示すブロック図である。同図に示すロボットの制御装置は、ロボットを制御するための制御情報（例えば直交データ、関節データ）を生成出力する補間処理部１と、補間処理部１から出力された制御情報に基づいて各関節に具備される駆動部（例えばサーボモータ）を制御するための制御情報を生成出力するサーボ制御部３０と、を備えている。

また、補間処理部１は、速度指令生成部１１、補間位置生成部１２、座標変換部１３、進入判断部１４、処理切り替え手段１５、特異点通過位置生成部２０などの各機能部を備えている。速度指令生成部１１は、予め記憶されている動作開始位置、動作目標位置、補間種類、指令速度などの指令情報である補間指令１０に基づいて速度指令を生成出力する。補間位置生成部１２は、速度指令生成部１１から出力された速度指令に基づいて補間処理のための位置指令を生成出力する。座標変換部１３は、補間位置生成部１２から出力された位置指令に対して座標変換処理を行う。進入判断部１４は、これらの直交データおよび関節データと、予め記憶されている特異点の領域情報である特異点領域１４ａに基づいて、ロボットが特異点に進入したか否かを判定する。特異点通過位置生成部２０は、ロボットが特異点に進入した場合に起動され、特異点および特異点近傍を通過する際の位置指令を生成出力する。処理切り替え手段１５は、進入判断部１４の判定結果に基づいて速度指令生成部１１の出力を補間位置生成部１２または特異点通過位置生成部２０のいずれに出力するように自身を切替制御する。なお、上述の処理の中で、補間指令１０や特異点領域１４ａの情報は、予め記憶されているものを用いるようにしているが、このような記憶情報に限定されるものではなく、例えば外部から入力された情報に基づいて制御するようにしてもよい。

つぎに、本発明にかかるロボットの制御装置において実行される補間処理について図１および図２を用いて説明する。なお、図２は、本発明にかかるロボットの制御装置において実行される補間処理の流れを示すフローチャートである。

まず、速度指令生成部１１は、補間指令１０を読み込み（ステップＳ１０１）、速度指令を生成する（ステップＳ１０２）。ロボットが特異点領域に進入していないときには、処理切り替え手段１５がａ側に接続され、ステップＳ１０２で生成された速度指令が処理切り替え手段１５を介して補間位置生成部１２に出力される。補間位置生成部１２は、速度指令生成部１１から出力された速度指令に基づいて位置指令を生成する（ステップＳ１０３）。座標変換部１３は、補間位置生成部１２から出力された位置指令に対して座標変換処理を行う（ステップＳ１０４）。いま、補間処理部１で実行される処理が直線補間の場合を想定すると、補間位置生成部１２から出力される位置指令が直交データであり、座標変換部１３で生成されるデータは、直交データに対応する関節データとなる。なお、以後の説明では、特にことわらない限り、直線補間処理の場合を例にとり説明する。

進入判断部１４は、座標変換部１３から出力された直交データおよび関節データに基づいてロボットが特異点領域に進入したか否かを判定する（ステップＳ１０５）。ロボットが特異点領域に進入していない場合（ステップＳ１０５、Ｎｏ）、これらの直交データおよび関節データがサーボ制御部３０に出力される（ステップＳ１０６）。一方、ロボットが特異点領域に進入している場合（ステップＳ１０５、Ｙｅｓ）、処理切り替え手段１５がｂ側に接続され、ステップＳ１０２で生成された速度指令が処理切り替え手段１５を介して特異点通過位置生成部２０に出力され、特異点通過位置生成処理が実行される（ステップＳ１０７）。両者のいずれかの処理が行われた後、全体の補間処理が終了しているか否かが判定され、終了していない場合（ステップＳ１０８、Ｎｏ）にはステップＳ１０２の処理に移行し、終了している場合（ステップＳ１０８、Ｙｅｓ）には全体の補間処理を終了する。

つぎに図２のステップＳ１０７の処理である特異点通過位置生成処理について説明する。図３は、特異点通過位置生成処理の処理フローを示すフローチャートである。なお、この処理は図１に示した機能ブロック図上の、特異点通過位置生成部２０によって実行される。

図３において、補間回数算出部２０ａは、特異点領域に進入する際の関節速度と特異点領域情報とに基づいて特異点領域内における補間回数を算出するとともに、退出位置算出部２０ｂは、補間回数算出部２０ａが算出した補間回数に基づいて特異点領域を退出する際の退出位置を算出する（ステップＳ２０１）。参照位置算出部２０ｃは、進入位置および退出位置における各関節データに基づいて特異点領域を通過する位置指令を生成する際に用いられる参照位置を算出する（ステップＳ２０２）。そして特異点通過位置指令生成部２０ｄは、参照位置の情報を用いずに生成した位置指令と参照位置算出部２０ｃが算出した参照位置とに基づいて特異点領域を通過させるための位置指令（直交データ）および関節データが生成され（ステップＳ２０３）、これらの位置指令（直交データ）および関節データがサーボ制御部３０に出力される（ステップＳ２０４）。このとき、ステップＳ２０２〜Ｓ２０４の処理が補間回数だけ繰り返して実行されているか否かが判定され（ステップＳ２０５）、補間回数分実行されていない場合（ステップＳ２０５、Ｎｏ）には、ステップＳ２０２〜Ｓ２０４の処理が補間回数だけ繰り返して実行され、補間回数分実行されている場合（ステップＳ２０５、Ｙｅｓ）には、処理切り替えスイッチ１５がｂ側からａ側に切り替えられ、通常の補間処理に戻る。なお、以上の処理が実行されることにより、ロボットは速度過大を生ずることなく、あるいは停止することなく特異点領域を通過することができる。

ところで、上述の背景技術の項において、多関節型のロボットの場合には、一つの直交データに対して複数の関節データが存在するため、ロボットが特異点あるいは特異点近傍を通過する場合に、ある関節軸の回転速度が非常に大きな値を有する場合があることについて触れたが、そのことの詳細について図４〜図８を用いて説明する。

図４に示すような多関節ロボットでは、ある直交データに対し複数の関節データを得ることができる。例えば、図５のように一つの直交データに対して２つの関節データ（関節データ１、関節データ２）が存在する。その中から所望の関節データを算出するため、例えば関節データを一意に定めるためのフラグを直交データの中に設け、直交データから関節データを算出する際に、当該直交データに設けられたフラグに基づいて関節データを一意に定めるような手法がよく用いられる。

ところが、ロボットが特異点やその近傍を通過する場合、先のフラグに基づいて関節データを算出しようとすると、上述のようにある関節軸の回転速度が非常に大きな値を有するようになる。例えば、図６に示すような、関節Ｊ１〜Ｊ６を有する多関節ロボットにおいて、図７のように関節Ｊ５の回転軸（以下、単に「Ｊ５軸」と呼称）の関節角度が０度以上の場合（＋側）を“ＮｏｎＦｌｉｐ”とし、０度未満の場合（−側）を“Ｆｌｉｐ”というフラグとすると、図８の状態１からフラグをＮｏｎＦｌｉｐに保持したままで状態３へ推移させる場合、状態２において、Ｊ４軸とＪ６軸とを瞬時に１８０度回転させなければならない。したがって、Ｊ４軸とＪ６軸とは、状態２において、非常に大きな値の回転速度を持たなければならないが、実際にはサーボモータなどの制限により、そのような速度で回転することができず、背景技術のところで述べたように、指令速度が低速度であっても一部の関節軸が最高速度付近で回転しているような状態が生起することになり、ロボットを制御する上で危険な状態にあるといった問題点を生じさせていた。

一方、本発明は、図１のように構成されるロボットの制御装置において、前述の図２および図３の処理フローに基づいた補間処理を行うことで、ロボットに対する指令速度が低速度であっても一部の関節軸の回転速度が最高速度付近まで上昇するといった問題点を解消するとともに、ロボットが速度過大となることなく、あるいは停止することなく特異点および特異点近傍を通過させることを可能としている。

つぎに、図２および図３の処理フローの一部について、さらに詳細な説明を行う。まず、ロボットが特異点領域に進入したか否かの判定処理（図２のステップＳ１０５の処理）の詳細について図６、図８および図９を用いて説明する。

まず、説明を簡単化するため、図６に示す多関節ロボットでの手先姿勢にかかるＪ４軸からＪ６軸について考える。例えば、先に説明したように、図８の状態２に示すようなＪ５軸が０度の場合が特異点の位置であり、この状態時にＪ４軸とＪ６軸とが一意に定まらない。一方、特異点の位置は直交データに含まれるフラグの値が切り替わる位置と一致しているので、特異点領域Ｓは、図９に示すように、Ｊ５軸の関節角度θ５で定義することができる。
つまり、特異点領域Ｓを次式のように定義することができる。

−θｓ≦θ５≦＋θｓ ・・・（１）

ここで、ある位置指令に対するＪ５軸の関節角度が式（１）を満たせば、特異点領域へ進入していると判断することができる。

つぎに、特異点領域内における補間回数および特異点領域からの退出位置の算出処理（図３のステップＳ２０１の処理）の詳細について図９および図１０を用いて説明する。

例えば、前回の補間処理で生成された位置指令の関節データをｊ（ｋ−１）とし、今回の補間処理で生成された関節データをｊ（ｋ）とすると、関節速度ｊｖ（ｋ）は、次式で表すことができる。

ｊｖ（ｋ）＝ｊ（ｋ）−ｊ（ｋ−１） ・・・（２）

また、この関節速度ｊｖ（ｋ）と図９に示した特異点領域Ｓとを用いて、図１０に示すような特異点領域Ｓを通過するのに必要な補間回数Ｃｎを、次式に基づいて算出するようにする。

Ｃｎ＝ｉｎｔ(Ｓ／ｊｖ(ｋ))＋１＝ｉｎｔ(２＊θｓ／ｊｖ(ｋ))＋１・・・（３）
なお、式（３）における、ｉｎｔ（α）は、“α”の小数点以下を切り捨てることを意味する。

したがって、特異点領域からの退出位置をｐ（ｎ）とすると、退出位置ｐ（ｎ）は指令速度ＶｃでＣｎ回動作した位置となるので、今回の補間処理で生成された直交データｐ（ｋ）を特異点領域への進入位置とするとき、退出位置ｐ（ｎ）は、図１１にも示すように、次式で表すことができる。

ｐ（ｎ）＝ｐ（ｋ＋Ｃｎ）＝ｐ（ｋ）＋Ｖｃ＊Ｃｎ ・・・（４）

また、退出位置ｐ（ｎ）は特異点を通過した後の直交データであるので、退出位置ｐ（ｎ）のフラグは特異点の通過によりｐ（ｋ）のフラグから切り替わった値となっている可能性がある。例えば、図９のようにフラグを定めると、ｐ（ｋ）のフラグが“ＮｏｎＦｌｉｐ”であった場合、ｐ（ｎ）のフラグは“Ｆｌｉｐ”である可能性がある。そこで、退出位置ｐ（ｎ）から関節データｊ（ｎ）を算出するときは、つぎの２つの関節データｊａ（ｎ）およびｊｂ（ｎ）を算出する。

（ア）ｊａ（ｎ）：ｐ（ｋ）と同じフラグを使って求めた関節データ（第１の関節データ）
（イ）ｊｂ（ｎ）：ｐ（ｋ）から切り替わったフラグを使って求めた関節データ（第２の関節データ）

一の関節角度から他の関節角度へ同一の補間回数で補間するのであれば、その関節角度の差分が小さい方が無理な動作とならないのは明らかである。したがって、上記のｊａ（ｎ）、ｊｂ（ｎ）のうち、進入位置の関節データｊ（ｋ）との差が小さい方を退出側の関節データｊ（ｎ）とし、それに対応したフラグを退出位置ｐ（ｎ）のフラグとする。すなわち、次式に示すような処理を行って、ｊ（ｎ）を算出する。

ｉｆ(ａｂｓ(ｊｂ(ｎ)−ｊ(ｋ))≦ａｂｓ(ｊａ(ｎ)−ｊ(ｋ))
ｔｈｅｎ ｊ(ｎ)＝ｊｂ(ｎ)
ｅｌｓｅ ｊ(ｎ)＝ｊａ(ｎ) ・・・（５）

つぎに、特異点領域を通過する際の参照位置の算出処理（図３のステップＳ２０２の処理）および位置指令生成処理（図３のステップＳ２０３の処理）の詳細について説明する。

図３のステップＳ２０１の処理の直後では、特異点領域の進入位置ｐ（ｋ）にかかる関節データｊ（ｋ）と退出位置ｐ（ｎ）にかかる関節データｊ（ｎ）とは既に定められているので、これらの間をそれぞれの関節データに基づいてＣｎ回の関節補間処理によって補間すれば、特異点領域を滑らかに通過させることが可能となる。ただし、関節補間ではその間の制御点の軌跡が直線から外れる可能性がある。そこで、特異点領域通過の最中は、補間周期ごとに関節補間による参照位置ｊｒ（ｉ）を算出し、その位置と指令速度Ｖｃとから算出された位置指令とを比較してサーボ制御部へ出力する関節データを決定すればよい。具体的には次のステップを踏むようにすればよい。

（ア）特異点領域の進入側の関節データｊ（ｋ）、退出側の関節データｊ（ｎ）および補間周期ｉ（ｉ＝１，２，・・・，Ｃｎ）から、次式に基づいて参照位置ｊｒ（ｉ）を算出する。
ｊｒ(ｉ)＝ｊ(ｋ)＋(ｊ(ｎ)−ｊ(ｋ))＊(ｉ／Ｃｎ) ・・・（６）

（イ）指令速度Ｖｃから位置指令ｐ（ｋ＋ｉ）を算出し、それに対する関節データｊ（ｋ＋ｉ）を算出する。なお、その際、特異点領域内でフラグが切り替わる可能性があるので、つぎの２つの関節データｊａ（ｋ＋ｉ）およびｊｂ（ｋ＋ｉ）を算出するようにする。
（ａ） ｊａ（ｋ＋ｉ）：ｐ（ｋ）と同じフラグを使って求めた関節データ（第１の関節データ群）
（ｂ） ｊｂ（ｋ＋ｉ）：ｐ（ｋ）から切り替わったフラグを使って求めた関節データ（第２の関節データ群）

（ウ）これらの第１の関節データ群ｊａ（ｋ＋ｉ）および第２の関節データ群ｊｂ（ｋ＋ｉ）のそれぞれと参照位置ｊｒ（ｉ）との差を計算し、その差が小さい方をサーボ制御部へ出力する関節データｊ（ｋ＋ｉ）とする。
ｉｆ(ａｂｓ(ｊｂ(ｋ＋ｉ)−ｊｒ(ｉ))≦ａｂｓ(ｊａ(ｋ＋ｉ)−ｊｒ(ｉ)))
ｔｈｅｎ ｊ(ｋ＋ｉ)＝ｊｂ(ｋ＋ｉ)
ｅｌｓｅ ｊ(ｋ＋ｉ)＝ｊａ(ｋ＋ｉ) ・・・（７）

例えば、図１２の軌跡Ａに示すように、ロボットが特異点上を通過する場合であっても、上記（ア）〜（ウ）に基づく直線補間処理を行うことで、速度過大となることなく、あるいは停止することなく特異点領域を通過することが可能となる。

ただし、図１２の軌跡Ｂに示すように直線補間処理の軌跡がわずかでも特異点から離れていると、上記（ウ）の処理において、サーボ制御部へ出力する関節データｊ（ｋ＋ｉ）がｊａ（ｋ＋ｉ）からｊｂ（ｋ＋ｉ）に変更される際に、関節角度が大きく変化してしまう場合がある。なお、図１３は、特異点近傍を通過する際にＪ４軸が大きく変化する状況を示す図である。

図１３において、曲線ｊｓは後述する修正関節データｊｃ（ｉ）を用いずに算出した軌跡におけるＪ４軸角度を示すものである。同図に示すように、Ｊ４軸が大きく変化する理由は、特異点から離れた位置を移動する場合には、Ｊ４軸、Ｊ５軸、Ｊ６軸の各軸が一つの軸上に並ばず、姿勢を保持して動作させる上でＪ４軸およびＪ６軸を捻る必要がある一方で、特異点に近付いたときに、その捻りの影響でｊａ（ｋ＋ｉ）の関節データとｊｂ（ｋ＋ｉ）の関節データとが近接していないことに起因している。

この現象を防止するため、例えば、関節データｊａ（ｋ＋ｉ）およびｊｂ（ｋ＋ｉ）のそれぞれと参照位置ｊｒ（ｉ）との差に許容値ｄｆを設定し、参照位置ｊｒ（ｉ）との差が許容値ｄｆを越える場合には、許容値ｄｆを越えずに推移する修正関節データｊｃ（ｉ）を算出し、この修正関節データｊｃ（ｉ）と参照位置ｊｒ（ｉ）との差と、ｊａ（ｋ＋ｉ）あるいはｊｂ（ｋ＋ｉ）と参照位置ｊｒ（ｉ）との差とを計算し、両者の差が小さい方をサーボ制御部へ出力する関節データｊ（ｋ＋ｉ）とするような処理を行えばよい。

例えば、ｊａ（ｋ＋ｉ）がｍ回目の補間周期で許容値ｄｆを越えた場合、修正関節データｊｃ（ｉ）は、次式で定義することができる。

ｊｃ(ｉ)＝ｊａ(ｋ＋ｍ−１)＋(−２＊ｄｆ／(Ｃｎ−２＊ｍ))＊(ｉ−ｍ−１)
・・・（８）
（ｉ＝ｍ＋１，ｍ＋２，…，Ｃｎ）

一方、サーボ制御へ出力する関節データｊ（ｋ＋ｉ）は、次式に基づいて処理される。
ｉｆ(ａｂｓ(ｊｃ(ｋ＋ｉ)−ｊｒ(ｉ))≦ａｂｓ(ｊｂ(ｋ＋ｉ)−ｊｒ(ｉ)))
ｔｈｅｎ ｊ(ｋ＋ｉ)＝ｊｃ(ｉ)
ｅｌｓｅ ｊ(ｋ＋ｉ)＝ｊｂ(ｋ＋ｉ) ・・・（９）

なお、修正関節データｊｃ（ｉ）を採用している間は、軌跡が直線から外れる可能性があるが、関節角度が大きく変化することないので、特異点領域を円滑に通過させることができる。

例えば、図１４は、本発明を適用した直線補間処理によって特異点上を通過する際のＪ４軸の角度変化の一例を示す図であり、図１５は、本発明を適用した直線補間処理によって特異点近傍を通過する際のＪ４軸の角度変化の一例を示す図である。図１４および図１５に示されるｊｓは上述の修正関節データｊｃ（ｉ）を用いて算出した軌跡におけるＪ４軸角度を示すものである。これらの図に示されるように、特異点近傍を含む特異点領域を通過する場合であっても、関節角度が大きく変化することなく、円滑に特異点領域を通過していることが明らかである。

以上説明したように、この実施の形態のロボットの制御装置および制御方法によれば、位置指令や、位置指令にかかる関節データおよび特異点領域情報に基づいてロボットが特異点領域に進入したか否かを判定するようにしているので、ロボットに対する指令速度が低速度であっても一部の関節軸の回転速度が最高速度付近まで上昇するといった問題点が解消されるとともに、ロボットが速度過大となることなく、あるいは停止することなく特異点および特異点近傍を通過させることができるので、ロボットの動作上の制約が減少し、ロボットの利便性が向上する。

なお、この実施の形態では、ロボットの手先の部分であるＪ４軸からＪ６軸に対して本発明を適用した例について説明したが、これに限定されるものではなく、他の軸の特異点に対しても、本発明を適用することができる。

また、この実施の形態では、図６に示すような垂直多関節ロボットを一例にとり説明したが、垂直多関節ロボットに限定される要素はなく、例えば水平多関節ロボットに対しても適用できることは勿論である。

以上のように、本発明は、特異点および特異点近傍を速度過大とならず、円滑に通過させることができるロボットの制御装置および制御方法として有用である。

本発明にかかるロボットの制御装置の機能構成を示すブロック図である。 本発明にかかるロボットの制御装置において実行される補間処理の流れを示すフローチャートである。 特異点通過位置生成処理の処理フローを示すフローチャートである。 多関節ロボットの一例を示す図である。 ある直交データに対して、複数の関節データが存在する状態を示す図である。 ６自由度垂直多関節ロボットの簡略構造を示す図である。 図６に示した６自由度垂直多関節ロボットのＪ５軸のフラグの定義を示す図である。 フラグを保持したまま特異点を通過させる際に、ある関節の回転速度が過大な速度で回転しなければならないことを示す図である。 特異点領域Ｓの定義を示す図である。 特異点領域進入時の速度と補間回数の関係を示す図である。 特異点領域進入時の指令速度と補間回数、特異点領域退出側の位置の関係を示す図である。 特異点上の通過と特異点からわずかに離れた位置を通過する直線軌跡を示す図である。 特異点近傍を通過する際にＪ４軸角度が大きく変化する状況を示す図である。 本発明を適用した補間処理によって特異点上を通過する際のＪ４軸の角度変化を示す図である。 本発明を適用した補間処理によって特異点近傍を通過する際のＪ４軸の角度変化を示す図である。

符号の説明

１ 補間処理部
１０ 補間指令
１１ 速度指令生成部
１２ 補間位置生成部
１３ 座標変換部
１４ａ 特異点領域
１４ 進入判断部
１５ 処理切替手段
２０ 特異点通過位置生成部
２０ａ 補間回数算出部
２０ｂ 退出位置算出部
２０ｃ 参照位置算出部
２０ｄ 特異点通過位置指令生成部
３０ サーボ制御部

Claims (6)

1. 特異点近傍を含む特異点領域に進入するロボットを制御するための特異点処理機能を有する補間処理部を備えたロボットの制御装置において、
   前記補間処理部は、
   速度指令に基づいて位置指令を生成出力する補間位置生成部と、
   前記位置指令に基づく関節データを生成出力する座標変換部と、
   前記位置指令と前記関節データと前記特異点領域情報とに基づいてロボットが特異点領域に進入したか否かを判定する進入判断部と、
   前記進入判断部の判定結果に基づいて起動され、特異点領域に進入したロボットが該特異点領域を通過する際の位置指令を生成出力する特異点通過位置指令生成部を具備する特異点通過位置生成部と、
   を備え、
   前記特異点通過位置生成部は、
   特異点領域に進入する際の関節速度と特異点領域情報とに基づいて該特異点領域内における補間回数を算出する補間回数算出部と、
   前記指令速度と前記特異点領域への進入位置と前記補間回数とに基づいて該特異点領域からの退出位置を算出する退出位置算出部と、
   を備えたことを特徴とするロボットの制御装置。
2. 前記退出位置算出部は、関節データを一意に定めるための識別符号として前記進入位置にかかる直交データの中に設けられたフラグと同一フラグを用いて算出された第１の関節データおよび前記進入位置にかかるフラグから切り替わったフラグを用いて算出された第２の関節データのそれぞれと、進入位置における関節データとを比較し、その差分が小さい方を退出位置における関節データとして算出することを特徴とする請求項１に記載のロボットの制御装置。
3. 前記特異点通過位置生成部は、前記特異点領域の進入側の関節データと退出側の関節データとの間を前記補間回数で分割し、該分割された関節データのそれぞれを補間周期ごとの参照位置として算出する参照位置算出部をさらに備えたことを特徴とする請求項１または２に記載のロボットの制御装置。
4. 前記特異点通過位置指令生成部は、前記進入位置にかかるフラグと同一フラグを用いて算出された補間周期ごとの関節データである第１の関節データ群および該進入位置にかかるフラグから切り替わったフラグを用いて算出された補間周期ごとの関節データである第２の関節データ群のそれぞれと前記補間周期ごとの参照位置のそれぞれとを比較し、その差分が小さい方の関節データを用いることを特徴とする請求項３に記載のロボットの制御装置。
5. 前記特異点通過位置指令生成部は、前記第１の関節データ群または第２の関節データ群から選択された一方の関節データ群と前記補間周期ごとの参照位置のそれぞれとの差分を計算し、該差分が所定の許容値を越える場合に該許容値を越えずに推移する修正関節データ群を算出するとともに、該修正関節データ群と前記参照位置との間の第１の差分情報と、該修正関節データの算出に用いられなかった他方の関節データ群と参照位置との間の第２の差分情報と、に基づいて算出される関節データを用いることを特徴とする請求項３に記載のロボットの制御装置。
6. 特異点近傍を含む特異点領域に進入するロボットを制御するための特異点処理ステップを有するロボットの制御方法において、
   速度指令に基づいて位置指令を生成出力する補間位置生成ステップと、
   前記位置指令を座標変換によって生成した関節データを生成出力する座標変換ステップと、
   前記位置指令と前記関節データと前記特異点領域情報とに基づいてロボットが特異点領域に進入したか否かを判定する進入判断ステップと、
   前記進入判断ステップの判定結果に基づいて起動され、特異点領域に進入したロボットが該特異点領域を通過する際の位置指令を生成出力する特異点通過位置指令生成ステップと、
   を含み、
   前記特異点通過位置生成ステップは、
   特異点領域に進入する際の関節速度と特異点領域情報とに基づいて該特異点領域内における補間回数を算出する補間回数算出ステップと、
   前記指令速度と前記特異点領域への進入位置と前記補間回数とに基づいて該特異点領域からの退出位置を算出する退出位置算出ステップと、
   を含むことを特徴とするロボットの制御方法。

Images