JP6238628B2 - ロボット装置、ロボット制御方法、ロボット制御プログラム及びロボット装置を用いた部品の製造方法 - Google Patents

Description

本発明は、多関節ロボットを有するロボット装置及び多関節ロボットの制御方法に関する。

近年、多関節ロボットにワークの組み立て等の作業を行わせるロボット装置の開発事例が増加している。このようなロボット装置の究極の目的は、多関節ロボットを人間の手のように動かして、複雑なワークの組立て等の作業を高速で行わせることにある。

しかしながら、多関節ロボットには、３次元空間で指定した１つの位置姿勢が関節空間では１つに定まらない、いわゆる特異点と呼ばれる姿勢が複数存在し、特異点が多関節ロボットを複雑かつ高速に動かす妨げとなっている。そのため、ロボット装置の開発においては、特異点を適当に処理することが重要となっている。

ここで、多関節ロボットを制御する一般的な制御装置のブロック図を図１３に示す。図１３において、現在位置Ｃ_ｉは、３次元空間で表したマニピュレータの手先の位置姿勢を示し、偏差指令Ｖ_ｉは、予め決められた制御周期（例えば、２ｍｓ）毎の手先の位置姿勢の偏差を示している。制御周期が一定なので、偏差指令Ｖ_ｉは速度を表しており、ティーチングペンダントなどにより制御周期毎に生成される。

このような構成において、位置指令計算部１５０で現在位置Ｃ_ｉに偏差指令Ｖ_ｉを加算して次の位置Ｃ_ｉ＋１を求め、逆機構計算部１５５で次の位置Ｃ_ｉ＋１を逆機構計算して次の関節角指令ｑ_ｉ＋１を求める。そして、これを多関節ロボットの関節を制御するサーボ制御装置１３０に送信することで多関節ロボットが駆動制御される。しかし、多関節ロボットが特異点近傍にあると、関節角指令ｑ_ｉ＋１が前回の関節角指令ｑ_ｉに対して大きく変化し、急激な動作をするおそれがあるという問題がある。

これに対しては、関節角指令の速度を制限する方法が考えられるが、関節角指令の速度を制限すると、当初の手先の目標位置から大きく外れる場合があり、一度外れると元に戻すことが困難になるという問題がある。これに対しては、関節の角速度が過大であれば減速させる速度制限をすることで、多関節ロボットが特異点近傍で急激に動作することを防止する技術が開示されている（特許文献１参照）。

特開２００３−３００１８３号公報

しかしながら、特許文献１に開示の技術は、関節の角速度を常に制限しているため、特異点以外の位置姿勢でも多関節ロボットの動作が遅くなるという問題がある。

そこで、本発明は、特異点を適当に処理して多関節ロボットの動作速度を向上可能なロボット装置及びロボット制御方法を提供することを目的とする。

本発明は、複数の回転関節を有する多関節ロボットと、前記多関節ロボットを操作可能な操作手段から入力された動作指令に基づいて、前記多関節ロボットを駆動制御する制御装置と、を備えたロボット装置において、前記制御装置は、前記動作指令に基づいて前記多関節ロボットを駆動するための、前記複数の回転関節のそれぞれの関節角指令を演算する関節角演算部と、前記関節角演算部が演算した関節角指令に基づいて前記複数の回転関節のそれぞれを回転駆動させて、前記多関節ロボットを動作させる関節駆動制御部と、前記多関節ロボットの複数種類の特異点の各々に対する前記回転関節の特異点角度又は特異点距離を計算する特異点計算部と、前記複数種類の特異点の中に、前記特異点計算部が計算した特異点角度又は特異点距離が所定の値よりも小さくなる特異点が存在する場合には、特異点角度又は特異点距離が所定の値よりも小さくなる当該特異点の種類に基づいて予め特定された回転関節の回転速度を制限する関節速度制限部と、を備え、前記関節角演算部は、前記動作指令に基づいて位置指令を計算する位置指令計算部と、前記特異点計算部が計算した特異点角度又は特異点距離に応じた補正の割合を計算する補正強度調整部と、前回の補正量に前記補正強度調整部で計算した補正割合を乗じたものを位置指令に加えて、位置指令を補正する位置指令補正部と、補正した位置指令を、仮の関節角指令に変換する逆機構計算部と、仮の関節角指令と前回の関節角指令とから関節角偏差を計算し、前記関節角偏差が前記回転速度を超えないように仮の関節角指令を減じて次回の関節角指令を計算する関節角補正部と、修正した関節角指令から次回の位置を計算する順機構計算部と、次回の位置と位置指令との差に補正量を加えて、次回の補正量を計算する補正量計算部と、を有する、ことを特徴とする。

また、本発明は、複数の回転関節を有する多関節ロボットを操作可能な操作手段から入力された動作指令に基づいて、制御部が前記多関節ロボットを駆動制御するロボット制御方法において、前記制御部が、前記動作指令に基づいて前記多関節ロボットを駆動するための、前記複数の回転関節のそれぞれの関節角指令を演算する関節角演算工程と、前記制御部が、前記関節角演算工程が演算した関節角指令に基づいて前記複数の回転関節のそれぞれを回転駆動させて、前記多関節ロボットを動作させる関節制御工程と、前記制御部が、前記多関節ロボットの複数種類の特異点の各々に対する前記回転関節の特異点角度又は特異点距離を計算する特異点計算工程と、前記制御部が、前記複数種類の特異点の中に、前記特異点計算工程が計算した特異点角度又は特異点距離が所定の値よりも小さくなる特異点が存在する場合には、特異点角度又は特異点距離が所定の値よりも小さくなる当該特異点の種類に基づいて予め特定された回転関節の回転速度を制限する関節速度制限工程と、を備え、前記関節角演算工程は、前記動作指令に基づいて位置指令を計算する位置指令計算工程と、前記特異点計算工程で計算された前記特異点角度又は前記特異点距離に応じた補正の割合を計算する補正強度調整工程と、前回の補正量に前記補正強度調整工程で計算した補正割合を乗じたものを位置指令に加えて、位置指令を補正する位置指令補正工程と、補正した位置指令を、仮の関節角指令に変換する逆機構計算工程と、仮の関節角指令と前回の関節角指令とから関節角偏差を計算し、前記関節角偏差が前記回転速度を超えないように仮の関節角指令を減じて次回の関節角指令を計算する関節角補正工程と、計算した次回の関節角指令から次回の位置を計算する順機構計算工程と、次回の位置と位置指令との差に補正量を加えて、次回の補正量を計算する補正量計算工程と、を有する、ことを特徴とする。

本発明によれば、特異点を適当に処理して多関節ロボットの動作速度を向上可能なロボット装置及びロボット制御方法を提供することができる。

本発明の第１実施形態に係るロボット装置の全体構造を模式的に示す斜視図である。 第１実施形態に係るロボットアームの第１の特異点を説明する説明図である。 第１実施形態に係るロボットアームの第２の特異点を説明する説明図である。 第１実施形態に係るロボットアームの第３の特異点を説明する説明図である。 第１実施形態に係る関節制御装置の構成を示すブロック図である。 最大関節角偏差を計算する関数を説明する説明図である。 特異点の種類に応じて速度制限する関節軸を説明する説明図である。 第２実施形態に係る関節制御装置の構成を示すブロック図である。 第３実施形態に係る関節制御装置の構成を示すブロック図である。 第４実施形態に係る関節制御装置の構成を示すブロック図である。 第５実施形態に係る関節制御装置の構成を示すブロック図である。 第６実施形態に係る関節制御装置の構成を示すブロック図である。 従来技術に係る多関節ロボットを制御する制御装置のブロック図である。

＜第１実施形態＞
以下、本発明の第１実施形態に係るロボット装置について、図１から図７を参照しながら説明する。まず、第１実施形態に係るロボット装置１全体の概略構成について、図１を参照しながら説明する。図１は、本発明の第１実施形態に係るロボット装置１の全体構造を模式的に示す図である。

図１に示すように、ロボット装置１は、ワークの組立て等を行う６軸の垂直多関節ロボット（以下、「多関節ロボット」という）２と、多関節ロボット２を制御するコントローラ３と、多関節ロボット２を操作可能なティーチングペンダント４と、を備えている。

多関節ロボット（多関節マニピュレータ）２は、６軸多関節のロボットアーム２０と、ロボットアーム２０の先端に接続されるエンドエフェクタ２１と、を備えている。ロボットアーム２０は、６つの回転関節（Ｊ１〜Ｊ６）と、関節軸（Ｊ１軸〜Ｊ６軸）を中心に回転駆動する６つのアクチュエータ（本実施形態においては、サーボ）と、を備えている。ロボットアーム２０は、各関節のサーボを選択的に駆動することでエンドエフェクタ２１を任意の３次元位置に移動させる。エンドエフェクタ２１は、ロボットアーム２０の先端に着脱自在に取り付けられており、作業内容に基づいて交換可能になっている。本実施形態においては、３本のフィンガでワークを把持可能な、所謂ハンドと呼ばれるエンドエフェクタが取り付けられている。

コントローラ（制御装置）３は、ロボットアーム２０の各関節のサーボを駆動制御するサーボ制御装置（後述の図５参照）３０と、ロボットアーム２０の関節を制御する関節制御装置（後述の図５参照）３１と、を備えている。また、コントローラ３は、エンドエフェクタ２１を制御するエンドエフェクタ制御装置と、記憶部と、記録メディア読取装置と、通信装置と、を備えている。

関節制御装置３１は、ロボットアーム２０が特異点に近づいた際に、特定の回転関節の急激な動作を回避しながらロボットアーム２０が動作するように各回転関節を制御する。なお、関節制御装置３１については、後に詳しく説明する。サーボ制御装置（関節駆動制御部）３０は、関節制御装置３１により計算された後述する関節角指令に基づいて、各回転関節のサーボを回転駆動させてロボットアーム２０を動作させる。

記憶部は、後述するロボット制御方法を実行するためのプログラム（ロボット制御プログラム）やロボットアーム２０の特異点、予めユーザによって教示された初期教示点等のデータが格納されている。記録メディア読取装置は、ロボット制御プログラム等の各種プログラムを記録したコンピュータ読み取り可能な記録媒体を読み込み、記録媒体に記録されたプログラムやデータ等を記憶装置に格納させるため等に用いられる。通信装置は、例えば、上述したような記録媒体を使用せずに、通信装置を介してインターネット等から配信される更新プログラム等をダウンロードする際に用いられる。

ティーチングペンダント（操作手段）４は、ユーザとのマンマシンインターフェイスであり、ユーザが多関節ロボット２を操作するために用いられる。また、ティーチングペンダント４は、ジョグ運転と呼ばれる機能を有しており、ジョグ運転は、例えば、ユーザがボタンを押している間だけ、３次元座標で多関節ロボット２の手先位置を直線上に動かすことができるようになっている。そのため、多関節ロボット２は、ジョグ運転中に特異点に近づくおそれがあり、特異点に近づくと特定の関節が急激に動作するおそれがある。このとき、本実施形態においては、関節制御装置３１で、特異点に近づいた場合でも特定の関節の急激な動作を回避しつつ、ロボットアーム２０が駆動するようになっている。

次に、上述した関節制御装置３１について、図２から図７を参照しながら具体的に説明する。まず、ロボットアーム２０の特異点及び特異点距離について、図２から図４を参照しながら説明する。図２は、第１実施形態に係るロボットアーム２０の第１の特異点を説明する説明図である。図３は、第１実施形態に係るロボットアーム２０の第２の特異点を説明する説明図である。図４は、第１実施形態に係るロボットアーム２０の第３の特異点を説明する説明図である。

一般に、多関節ロボット（多関節マニピュレータ）では、ある特定の手先位置に対して関節角度が計算できない特異点が多数存在する。例えば、６軸の多関節ロボット２においては、３つの特異点（第１の特異点〜第３の特異点）が存在することが知られており、多関節ロボット２が３つの特異点に同時に近づく場合もある。以下、多関節ロボット２が有する３つの特異点について説明する。

なお、以下においては、説明のため、Ｊ１軸の方向を垂直方向と呼び、Ｊ１軸と直交する方向を水平方向と呼ぶものとする。また、一般的な産業用ロボットにおいては、Ｊ１軸の方向とＪ２軸の方向とは直交し、Ｊ２軸の方向とＪ３軸の方向とは平行になるように設計されるため、この条件に基づいて説明する。Ｊ２軸がＪ１軸に対して水平方向にオフセットされた構成を用いることもあるが、同様に議論可能であるため、ここではその説明を省略する。更に、一般的には、Ｊ４軸、Ｊ５軸及びＪ６軸は、逆機構計算が計算し易いように、１点（図２から図４に示すＰ点）で交わるように設計される場合が多いため、この条件に基づいて説明する。

図２に示すように、第１の特異点は、Ｊ４軸、Ｊ５軸及びＪ６軸が交わる点、即ち、Ｐ点がＪ１軸上に有る場合である。第１の特異点上では、Ｊ１軸、Ｊ４軸、Ｊ５軸及びＪ６軸がＰ点において、１点で交わるので、４つの回転関節Ｊ１、Ｊ４〜Ｊ６の回転角度が定まらない。３次元空間では、回転角度は３種類の方向（変数）により定まるのに対して、第１の特異点は、変数が１つ多い４つあるためである。第１の特異点の近傍では、手先の僅かな位置姿勢の変化が回転関節Ｊ１、Ｊ４〜Ｊ６の大きな角度変化を引き起こす。

また、Ｐ点をＪ１軸上に位置させるために必要な回転関節Ｊ２の回転角度をδ１とすると、回転角度δ１は、第１の特異点までの距離を示す代表的な指標となる。そのため、本実施形態においては、特異点距離として、特定の回転関節の回転角度（以下、「特異点角度」という）を用いるものとする。経験的に、回転関節Ｊ２の回転角度（特異点角度）δ１が１５度を下回ると、手先の僅かな位置姿勢の変化で回転関節Ｊ１、Ｊ４〜Ｊ６の回転角度が大きく変化するので注意が必要となる。

図３に示すように、第２の特異点は、Ｊ４軸とＪ６軸とが同軸上に有る場合である。第２の特異点では、Ｊ４軸とＪ６軸とが一致するので、回転関節Ｊ４、Ｊ６の回転角度が１つに定まらない。第２の特異点の近傍では、手先の僅かな位置姿勢の変化が回転関節Ｊ４、Ｊ６の大きな角度変化を引き起こす。また、Ｊ４軸とＪ６軸とを同軸上に位置させるのに必要な回転関節Ｊ５の回転角度をδ２とすると、経験的に回転角度（特異点角度）δ２が１５度を下回ると、手先の僅かな位置姿勢の変化で回転関節Ｊ４、Ｊ６の回転角度が大きく変化するので注意が必要となる。

図４に示すように、第３の特異点は、Ｊ２軸とＪ３軸とがＪ４軸上に有る場合である。第３の特異点では、Ｊ２軸とＪ３軸とがＪ４軸上に位置するので回転関節Ｊ２、Ｊ３の回転角度が定まらない。第３の特異点の近傍では、手先の僅かな位置姿勢の変化が回転関節Ｊ２、Ｊ３の大きな角度変化を引き起こす。また、Ｊ２軸とＪ３軸とをＪ４軸上に位置させるのに必要な回転関節Ｊ３の回転角度をδ３とすると、経験的に回転角度（特異点角度）δ３が１５度を下回ると手先の僅かな位置姿勢の変化で回転関節Ｊ２、Ｊ３の回転角度が大きく変化するので注意が必要となる。

次に、上述した３つの特異点に近づいた際に、ロボットアーム２０の急激な動作を抑制しつつ駆動制御可能な関節制御装置３１について、図５を参照しながら説明する。図５は、第１実施形態に係る関節制御装置３１の構成を示すブロック図である。

まず、関節制御装置３１の構成について説明する。図５に示すように、関節制御装置３１は、動作指令に基づいてロボットアーム２０を駆動するための各回転関節の関節角指令を演算する関節角演算部３２と、特異点に対する回転関節の特異点角度を計算する特異点計算部５１と、を備えている。また、関節制御装置３１は、特異点角度に応じて、回転関節毎に予め与えられている最大関節角偏差（最大回転速度）の制限値を小さくする処理を行う最大関節角偏差調整部５２を備えている。

関節角演算部３２は、位置指令計算部５０と、補正強度調整部５３と、位置指令補正部５４と、逆機構計算部５５と、関節角補正部５６と、順機構計算部５７と、補正量計算部５８と、を備えている。

位置指令計算部５０は、３次元座標で表現したロボットアーム２０の手先の現在位置に、同手先の偏差指令を加えて位置指令を計算する。補正強度調整部５３は、特異点角度の大きさに応じた補正強度（補正の割合であり、以下、「補正割合」という）を計算する処理を行う。具体的には、特異点角度が小さいほど補正割合を小さくする処理を行う。位置指令補正部５４は、位置指令計算部５０で計算した位置指令に前回の補正量と補正割合を掛けたものを加えて、補正した位置指令を計算する。逆機構計算部５５は、位置指令補正部５４で補正した位置指令を、仮の関節角指令に変換する。関節角補正部５６は、位置指令補正部５４で計算した仮の関節角指令と前回の関節角指令とから、関節角偏差を計算し、最大関節角偏差調整部５２で計算した最大関節角偏差を超えないように仮の関節角を小さくして次の関節角指令を計算する。順機構計算部５７は、修正した関節角指令から次の関節角指令と、次の位置とを計算する。補正量計算部５８は、順機構計算部５７で計算した次回の位置と位置指令との差に、補正量を加えて次の補正量を計算する。

最大関節角偏差調整部（関節速度制限部）５２は、特異点の種類に基づいて予め特定された回転関節の特異点角度が所定の値より小さくなると、最大関節角偏差（最大回転速度）の制限値を予め設定された最大関節角偏差の制限値より小さくする。

次に、上述のように構成された関節制御装置３１によるロボットアーム２０の駆動制御（ロボット制御方法）について、図５に加え、図６及び図７を参照しながら説明する。なお、以下に示す記号の添え字ｉは、計算回数を表し、一連の計算が終わる毎に１つ増やすようになっている。また、この計算は、例えば、２ｍｓといった制御周期毎に実施される。図６は、最大関節角偏差を計算する関数を説明する説明図である。図７は、特異点の種類に応じて速度制限する関節軸を説明する説明図である。

ティーチングペンダント４に動作指令が入力されると、入力された動作指令に基づいて、ティーチングペンダント４から偏差指令Ｖ_ｉ、現在位置Ｃ_ｉ、前回の補正量Ｅ_ｉ及び前回の関節角指令ｑ_ｉが関節制御装置３１に入力される。すると、関節制御装置３１は、入力されたデータに基づいて、次の関節角指令ｑ_ｉ＋１、Ｃ_ｉ＋１及びＥ_ｉ＋１を計算し、サーボ制御装置３０に出力する。サーボ制御装置３０は、入力された次の関節角指令に基づいて回転関節を回転駆動させて、ロボットアーム２０を動作させる。以下、関節制御装置３１による、次の関節角指令ｑ_ｉ＋１、Ｃ_ｉ＋１及びＥ_ｉ＋１の計算方法について、具体的に説明する。

ティーチングペンダント４から偏差指令Ｖ_ｉ、現在位置Ｃ_ｉ、前回の補正量Ｅ_ｉ及び前回の関節角指令ｑ_ｉが入力されると、まず、手先の偏差指令Ｖ_ｉと手先の現在位置Ｃ_ｉを加えて位置指令Ｄ_ｉを計算する（位置指令計算工程）。ここで、現在位置データや偏差指令データは、ＸＹＺ方向の移動と回転とで６つの成分を持つベクトルであり、位置姿勢を表している。このような位置姿勢を表す別の方法として、４行４列の同次座標変換行列を用いる方法があるが、この場合、上記ベクトルの和は、行列の掛け算で表現される。しかし、これは表現の違いだけで本発明の主旨とは無関係なので、本実施形態においては、位置姿勢の表現として、直感的に理解し易いベクトル表現を用いて説明する。

次に、特異点角度を計算し、特異点近傍か否かを判定する（特異点計算工程）。本実施形態においては、３種類の特異点に応じて、対応する回転関節の特異点角度を計算する。なお、対応する回転関節の特異点角度は、現在の関節角度を代用することができる。本実施形態に係る多関節ロボット２の制御周期は、２ｍｓといった十分に短い時間なので関節角度の変化も小さいからである。

特異点角度が計算されると、次に、特異点角度に応じて、回転関節毎に予め与えられている最大関節角偏差（最大回転速度）の制限値を小さくする処理を行う（関節速度調整工程）。ここで、モータが発生するトルクやエンコーダが対応できる最大回転速度などの制約により、最大回転速度、即ち、最大関節角偏差の制限値が回転関節毎に決まっている。ここでは、特異点角度に応じて、この制限値をさらに小さくする。

例えば、図６に示すように、回転関節毎に与えられた最大回転角偏差（最大回転速度）をＶ_ｍａｘとし、特異点角度がゼロの場合の回転角偏差をＶ_ｍｉｎとし、特異点近傍を判定する角度をθｓとする。特異点角度θに対する減速した最大回転角偏差Ｖ_ｌｉｍを次式で計算する。

特異点近傍を判定するθｓを、例えば１５度に設定した場合、特異点角度が１５度を下回ると特定の回転関節の回転速度を制限する。また、制限値を小さくする処理は、特異点の種類に基づいて予め特定される回転関節毎に行われる。例えば、図７に示すように、第１の特異点の場合は、回転関節Ｊ１、Ｊ４、Ｊ５、Ｊ６の最大関節角偏差（最大回転速度）の制限値を小さくする。なお、このときの制限値の最小値はゼロとする。また、第２特異点の場合は、回転関節Ｊ４、Ｊ６の最大関節角偏差（最大回転速度）の制限値を小さくする。なお、このときの制限値の最小値はゼロとする。更に、第３の特異点の場合は、回転関節Ｊ２、Ｊ３の最大関節角偏差（最大回転速度）の制限値を小さくする。なお、このときの制限値の最小値は、ゼロではなく、例えば、秒速１度といった小さな速度に設定する。この速度をゼロにすると、この特異点から脱出できないからである。

また、ロボットアーム２０は、第１の特異点から第３の特異点に同時に近づくこともある。例えば、第１の特異点と第２の特異点とに同時に近づいた場合は、それぞれの回転関節の最大関節角偏差（最大回転速度）を計算し、最も小さいものを制限値とする。

次に、特異点角度に応じた補正割合を計算する（補正強度調整工程）。上述したように特異点は３つあり、それぞれに対応した特異点角度があるが、ここではその最小値を特異点角度として考える。また、補正割合は、０から１までの数字であり、特異点近傍ではゼロに、特異点から離れれば１となる。例えば、特異点近傍を判定する特異点角度をθｓとし、３種類の特異点までの特異点角度のうち最少のものをθとすると、補正割合ｒは、

で表すことができる。なお、この関数の形は図６において、をＶ_ｍａｘ＝１、Ｖ_ｍｉｎ＝０としたのと同じであるため説明を省略する。また、θｓは、例えば、１５度に設定する。

次に、位置指令計算工程で計算した位置指令Ｄ_ｉに、前回の補正量Ｅ_ｉと補正割合ｒとを掛けたものを加えて、補正した位置指令Ｆ_ｉを計算する（位置指令補正工程）。

補正強度調整工程により特異点近傍だと補正割合が小さくなるので、前回の補正量に補正割合を掛けることで、特異点近傍では補正量が小さくなる。そのため、従来技術では特異点近傍の補正量が大きくなることで関節の動作が不安定になるおそれがあったが、本実施形態においては特異点近傍で補正量が小さくなるため、関節の動作を安定させることができる。

次に、位置指令補正工程で補正した位置指令Ｆ_ｉを、仮の関節角指令

に変換する（逆機構計算工程）。次に、仮の関節角指令

と、前回の関節角指令ｑ_ｉから関節角偏差を計算し、これが最大関節角偏差調整工程で計算した最大回転角偏差Ｖ_ｌｉｍを超えないように仮の関節角偏差を減じて次の関節角指令ｑ_ｉ＋１を計算する（関節角補正工程）。ここでは、関節角偏差が上限を超えない条件から、次式が成り立たなければならない。

これを満たすように、次の関節角指令ｑ_ｉ＋１を次式のように計算する。

次の関節角指令ｑ_ｉ＋１を計算すると、次に、次の関節角指令ｑ_ｉ＋１から次の位置Ｃ_ｉ＋１を計算する（順機構計算工程）。次に、順機構計算工程で計算した次の位置Ｃ_ｉ＋１と、位置指令Ｄ_ｉ＋１との差に、補正量Ｅ_ｉを加えて、次の補正量Ｅ_ｉ＋１を計算する（補正量計算工程）。

ここで、もし特異点近傍でなければ補正割合は１となるので、補正した位置指令は、

となる。また、関節角補正工程で減速もされないため、逆機構計算工程と順機構計算工程により元に戻るので、Ｃ_ｉ＋１＝Ｄ_ｉ＋Ｅ_ｉとなる。このＣ_ｉ＋１を上式に代入すると、Ｅ_ｉ＋１は、ゼロになる。つまり、特異点から離れれば補正量はゼロになる。

以上説明したように、第１実施形態に係るロボット装置１は、特異点角度（距離）を計算し、特異点角度に応じて最大関節角偏差を調整し、その最大値以内になるように関節角偏差を補正する。そのため、例えば、特異点近傍では最大関節角偏差を小さく、それ以外では大きく設定すれば、特異点以外で動作が遅くなるという問題を解決することができる。これにより、多関節ロボットの動作速度を向上させることができる。また、特異点近傍では特異点の種類に基づいて特定される回転関節ごとに最大関節角偏差、すなわち関節角速度が制限されるのでロボットアームが急激に動くことを防止することができる。

また、関節角偏差を修正することで、手先の位置が当初の目標から外れてしまい、手先の位置を補正する必要がある。従来技術では特異点近傍でも手先位置を補正したために、関節速度の大きな変化を引き起こしていた。それに対し、本実施形態では、特異点角度を計算し、それに応じて補正割合を調整し、補正割合を考慮して位置指令を補正する。例えば、特異点近傍では補正割合を小さく、それ以外では大きく設定する。そのため、特異点近傍では補正量が小さくなるので、関節速度が大きくなることを防止することができる。また、関節速度が大きくなることを避けるために停止させることを防止することができるため、生産性を向上させることができる。また、特異点から離れれば補正強度が大きくなるので、手先の位置は当初の目標位置に追従させることができる。

また、第１実施形態に係るロボット装置１は、特異点の種類ごとに、特異点角度を計算するので、特異点の種類ごとに、異なる関節角偏差、すなわち回転関節の回転速度を制限することができる。そのため、特異点近傍でも減速されない回転関節を使って特異点から速やかに脱出できる。

また、第１実施形態に係るロボット装置１は、複数ある特異点角度が最小の場合に対応して補正量を小さくできるので、特異点近傍でも計算が安定し、手先速度を減速する必要も無いので動作速度を向上できる。

＜第２実施形態＞
次に、第２実施形態に係るロボット装置について、図８を参照しながら説明する。第２実施形態では、関節制御装置３１の順機構計算部５７をヤコビ行列計算部５９及びヤコビ行列掛け算部６０に置き換えたことが第１実施形態と相違する。そのため、第２実施形態では、ヤコビ行列計算部５９及びヤコビ行列掛け算部６０について説明する。図８は、第２実施形態に係る関節制御装置３１Ａの構成を示すブロック図である。

順機構計算は、根元の関節から位置姿勢を順次計算して手先の位置姿勢を計算するが、ヤコビ行列との掛け算でこれを近似計算できることが知られている。ヤコビ行列計算部５９は、ヤコビ行列を計算する。６軸多関節のロボットアーム２０の場合、ヤコビ行列は６行６列で、関節角の偏差ベクトル（関節の速度）をかけると、手先位置の偏差ベクトル（手先の速度）となる。

ヤコビ行列掛け算部６０は、ヤコビ行列と、関節角補正工程で計算した関節角指令と前回の関節角指令との差、すなわち関節角の偏差ベクトルとヤコビ行列を掛け、これに現在位置を加えることによって、次回の位置を計算する。例えば、

を用いて計算することができる。第２実施形態によれば、順機構計算は三角関数を含む複雑な座標変換なので、順機構計算部５７をヤコビ行列計算部５９及びヤコビ行列掛け算部６０に代用することで計算量を削減することができる。

＜第３実施形態＞
次に、第３実施形態に係るロボット装置について、図９を参照しながら説明する。第３実施形態では、補正量修正部６１を追加して補正量を小さくしたことが第１実施形態と相違する。そのため、第３実施形態では、補正量修正部６１について説明する。図９は、第３実施形態に係る関節制御装置３１Ｂの構成を示すブロック図である。

ここでは、作業者がティーチングペンダント４のボタンを押して多関節ロボット２の手先位置（エンドエフェクタ２１）を動かしている場面を想定する。偏差指令は、ティーチングペンダント４のボタンが押されている間中、常にゼロではない数字を出力し続ける。ロボットアーム２０が動き続けて特異点に近づくと、回転関節の速度制限がかかるため手先の速度は落ちるが、このとき動けなかった分は補正量に蓄積される。そして、特異点から離れると、こんどは補正量を解消するので手先速度が増加する。例えば、補正量がゼロに戻ると、手先速度も元の速度に戻る。

この一連の動作を考えると、特異点近傍から離れる瞬間、ロボットアーム２０の予期しない速度の増加が起こるため、作業者が意図している動作の方向には、補正量を蓄積させないほうが好ましい。第３実施形態では、偏差指令から方向の成分を除去する補正量修正部６１を設け、補正量修正部６１では、次のようなベクトル計算を行う。まず、偏差指令Ｖ_ｉの方向ベクトルｖ_ｉを計算する。

次に、修正前の補正量

と、方向ベクトルｖ_ｉから修正後の補正量Ｅ_ｉ＋１を計算する。

この式の意味は、補正量から偏差指令の方向の成分を除去することである。第３実施形態によれば、補正量を小さくできるので、特異点近傍から離れる瞬間の不要な速度増加を抑えることができる。特に、ティーチングペンダントの操作者の視点で考えると、操作者が意図している方向、すなわち偏差指令の方向の増速を抑えられるので好ましい。また、補正量が小さくなるので、特異点近傍の補正計算がより安定になる。さらに、補正量が小さくなるので、本来の位置を回復する時間も短くできる。

＜第４実施形態＞
次に、第４実施形態に係るロボット装置について、図１０を参照しながら説明する。第４実施形態では、最大関節角偏差の制限に加えて偏差変化も制限する最大関節角偏差変化調整部６２を設けた点が第１実施形態と相違する。そのため、第４実施形態では、最大関節角偏差変化調整部６２について説明する。図１０は、第４実施形態に係る関節制御装置３１Ｃの構成を示すブロック図である。

最大関節角偏差変化調整部６２は、特異点角度に応じて回転関節毎に予め与えられている最大関節角偏差変化（最大回転速度変化）を小さくする。制御周期が一定なので関節角偏差変化は加速度であり、回転関節のモータが発生できるトルクなどの条件から関節ごとに最大値が決まっている。ここでは、特異点角度に応じて、この制限値をさらに小さくして減速する。具体的に減速する方法は偏差（回転速度）の場合に図６を用いて説明した方法と同じである。

回転関節ごとに与えられた最大回転角偏差変化をＡ_ｍａｘとし、横軸の特異点角度θがゼロの場合の関節角偏差変化をＡ_ｍｉｎとし、特異点近傍を判定する特異点角度をθｓとする。特異点角度θに対する減速した最大回転角偏差変化Ａ_ｌｉｍは、

で計算される。例えば、θｓを１５度に設定すると、特異点角度が１５度を下回ると回転関節の回転速度が急激になる。第４実施形態では、更に、最大関節角偏差の制限に加えて、最大関節角偏差変化も制限する。偏差変化は、次の関節角指令ｑ_ｉ＋１と、前回の関節角指令ｑ及び前々回の関節角指令ｑ_ｉ−１から次の差分で計算する。

これが、最大関節角偏差変化調整部６２で計算した制限値を超えないという条件から、

この式を満たすようにｑ_ｉ＋１を次式のように計算する。

さらに最大関節角偏差の制限をかける。これは、第１実施形態の関節角補正工程と同じ説明なので省略する。なお、上述した２種類の制限の優先順位だが、第４実施形態では最大関節角偏差の制限の計算を、偏差変化の制限のあとに行っているので、最大関節角偏差の制限が優先される。第４実施形態によれば、最大関節角偏差の制限に加えて最大関節角偏差変化も制限するので、より滑らかな速度制御が可能となる。

＜第５実施形態＞
次に、第５実施形態に係るロボット装置について、図１１を参照しながら説明する。第５実施形態では、位置指令補正工程及び逆機構計算工程を特異点計算工程の前に行う点が第１実施形態と相違する。そのため、第５実施形態では、位置指令補正工程及び逆機構計算工程を特異点計算工程の前に行った場合について説明する。図１１は、第５実施形態に係る関節制御装置３１Ｄの構成を示すブロック図である。

特異点角度は、逆機構計算でも得られるので、この計算の後に特異点角度を計算すれば、最新の関節角度で特異点角度を計算することができる。第５実施形態においては、位置指令補正工程は、前回の補正割合ｒ_ｉを用いて補正した位置指令Ｆ_ｉを計算する。

なお、前回の補正割合を用いた場合においても、制御周期が２ｍｓといった十分に短い時間であるため問題ない。２ｍｓ間の関節角度の変化は小さいため、特異点角度の変化も小さく、補正割合の変化も小さいといえるためである。第５実施形態によれば、第１実施形態と同様の効果を得ることができる。

＜第６実施形態＞
次に、第６実施形態に係るロボット装置について、図１２を参照しながら説明する。第６実施形態では、補正量計算工程の後に減衰工程を設けた点が第１実施形態と相違する。そのため、第６実施形態では、減衰計算工程を実施する減衰計算部６３について説明する。図１２は、第６実施形態に係る関節制御装置３１Ｅの構成を示すブロック図である。

第１実施形態においては、補正割合は、特異点近傍では小さく、特異点から離れると大きくなるようになっている。ここで、特異点近傍に長い間停留してから離れる場合を考える。この場合、長時間の前の補正量に対しても補正が働き、ロボットアーム２０が動くことになる。これは、例えば、多関節ロボット２の操作者が偏差指令を指示する場合、長時間経過した後にロボットアーム２０が動くと、操作者に対して好ましくない。この場合、時間に応じて補正量を減衰させた方が自然である。

そこで、第６実施形態では、減衰計算部６３に、予め定めた倍率で補正量を減衰計算させる。具体的には、修正後の補正量Ｅ_ｉ＋１にｎＥ_ｉ＋１を代入する。倍率は、ｎ回で半分に減衰する場合、ｎ＝０．５＾（１／ｎ）である。例えば、制御周期２ｍｓ、４秒で半分に減衰させる場合、回数ｎは、４／０．００２＝２０００回。倍率は、ｎ＝０．９９９６５３である。

以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限定されるものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。

例えば、第１実施形態においては、現在位置を前回の制御周期で計算したＣ_ｉで説明したが、サーボ制御装置で実際の回転角度を計算し、順機構計算により現在位置を求めてもよい。この場合でも本発明の主旨は変わらず、効果も同じである。

１ ロボット装置
２ 多関節ロボット
３ コントローラ（制御装置）
４ ティーチングペンダント（操作手段）
３０ サーボ制御装置（関節駆動制御部）
３２ 関節角演算部
５１ 特異点計算部
５２ 最大関節角偏差調整部（関節速度制限部）

Claims (11)

1. 複数の回転関節を有する多関節ロボットと、前記多関節ロボットを操作可能な操作手段から入力された動作指令に基づいて、前記多関節ロボットを駆動制御する制御装置と、を備えたロボット装置において、
   前記制御装置は、
   前記動作指令に基づいて前記多関節ロボットを駆動するための、前記複数の回転関節のそれぞれの関節角指令を演算する関節角演算部と、
   前記関節角演算部が演算した関節角指令に基づいて前記複数の回転関節のそれぞれを回転駆動させて、前記多関節ロボットを動作させる関節駆動制御部と、
   前記多関節ロボットの複数種類の特異点の各々に対する前記回転関節の特異点角度又は特異点距離を計算する特異点計算部と、
   前記複数種類の特異点の中に、前記特異点計算部が計算した特異点角度又は特異点距離が所定の値よりも小さくなる特異点が存在する場合には、特異点角度又は特異点距離が所定の値よりも小さくなる当該特異点の種類に基づいて予め特定された回転関節の回転速度を制限する関節速度制限部と、を備え、
   前記関節角演算部は、
   前記動作指令に基づいて位置指令を計算する位置指令計算部と、
   前記特異点計算部が計算した特異点角度又は特異点距離に応じた補正の割合を計算する補正強度調整部と、
   前回の補正量に前記補正強度調整部で計算した補正割合を乗じたものを位置指令に加えて、位置指令を補正する位置指令補正部と、
   補正した位置指令を、仮の関節角指令に変換する逆機構計算部と、
   仮の関節角指令と前回の関節角指令とから関節角偏差を計算し、前記関節角偏差が前記回転速度を超えないように仮の関節角指令を減じて次回の関節角指令を計算する関節角補正部と、
   修正した関節角指令から次回の位置を計算する順機構計算部と、
   次回の位置と位置指令との差に補正量を加えて、次回の補正量を計算する補正量計算部と、を有する、
   ことを特徴とするロボット装置。
2. 前記関節速度制限部は、前記特異点角度又は前記特異点距離が所定の値よりも小さくなると、前記特異点角度又は前記特異点距離に応じて、前記特定された回転関節の回転速度を制限するための制限値を小さくするが、当該特異点の種類に基づいて前記制限値の最小値が設定されている、
   ことを特徴とする請求項１に記載のロボット装置。
3. 複数の回転関節を有する多関節ロボットを操作可能な操作手段から入力された動作指令に基づいて、制御部が前記多関節ロボットを駆動制御するロボット制御方法において、
   前記制御部が、前記動作指令に基づいて前記多関節ロボットを駆動するための、前記複数の回転関節のそれぞれの関節角指令を演算する関節角演算工程と、
   前記制御部が、前記関節角演算工程が演算した関節角指令に基づいて前記複数の回転関節のそれぞれを回転駆動させて、前記多関節ロボットを動作させる関節制御工程と、
   前記制御部が、前記多関節ロボットの複数種類の特異点の各々に対する前記回転関節の特異点角度又は特異点距離を計算する特異点計算工程と、
   前記制御部が、前記複数種類の特異点の中に、前記特異点計算工程が計算した特異点角度又は特異点距離が所定の値よりも小さくなる特異点が存在する場合には、特異点角度又は特異点距離が所定の値よりも小さくなる当該特異点の種類に基づいて予め特定された回転関節の回転速度を制限する関節速度制限工程と、を備え、
   前記関節角演算工程は、
   前記動作指令に基づいて位置指令を計算する位置指令計算工程と、
   前記特異点計算工程で計算された前記特異点角度又は前記特異点距離に応じた補正の割合を計算する補正強度調整工程と、
   前回の補正量に前記補正強度調整工程で計算した補正割合を乗じたものを位置指令に加えて、位置指令を補正する位置指令補正工程と、
   補正した位置指令を、仮の関節角指令に変換する逆機構計算工程と、
   仮の関節角指令と前回の関節角指令とから関節角偏差を計算し、前記関節角偏差が前記回転速度を超えないように仮の関節角指令を減じて次回の関節角指令を計算する関節角補正工程と、
   計算した次回の関節角指令から次回の位置を計算する順機構計算工程と、
   次回の位置と位置指令との差に補正量を加えて、次回の補正量を計算する補正量計算工程と、を有する、
   ことを特徴とするロボット制御方法。
4. 前記関節速度制限工程は、前記特異点角度又は前記特異点距離が所定の値よりも小さくなると、前記特異点角度又は前記特異点距離に応じて、前記特定された回転関節の回転速度を制限するための制限値を小さくするが、当該特異点の種類に基づいて前記制限値の最小値が設定されている、
   ことを特徴とする請求項３に記載のロボット制御方法。
5. 前記順機構計算工程は、ヤコビ行列を求め、前記関節角補正工程で計算した次回の関節角指令と前回の関節角指令との差と、前記ヤコビ行列とを掛け、これに現在位置を加えて次回の位置を計算する、
   ことを特徴とする請求項３又は４に記載のロボット制御方法。
6. 前記補正量計算工程は、補正量から偏差指令の方向の成分を除去する、
   ことを特徴とする請求項３から５のいずれか１項に記載のロボット制御方法。
7. 前記関節速度制限工程において、前記回転速度に加えて最大回転速度変化も計算し、
   前記関節角補正工程において、仮の関節角指令と、前回の関節角指令と、前々回の関節角指令とから関節角偏差変化を計算し、これが先の最大関節角偏差変化を超えないように、仮の関節角指令を減じ、さらに、仮の関節角指令と、前回の関節角指令とから関節角偏差を計算し、これが先の最大関節角偏差を超えないように、仮の関節角指令を減じて次の関節角指令を計算する、
   ことを特徴とする請求項３から６のいずれか１項に記載のロボット制御方法。
8. 前記補正量計算工程で計算された補正量を予め定めた倍率で減衰計算する減衰計算工程を備えた、
   ことを特徴とする請求項３から７のいずれか１項に記載のロボット制御方法。
9. 請求項３から８のいずれか１項に記載のロボット制御方法を前記制御部に実行させるためのロボット制御プログラム。
10. 請求項９に記載のロボット制御プログラムを記録したコンピュータ読み取り可能な記録媒体。
11. 請求項１に記載のロボット装置を用いて部品を製造することを特徴とする部品の製造方法。

Images