(57)【特許請求の範囲】
【請求項１】
先端に設けられた作業部の姿勢を変化させる３つの駆動軸を有する第１関節駆動系と前記第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を有する第２関節駆動系とを備えてなる多関節型ロボットを制御する多関節型ロボットの制御装置であって，
予め設定される作業開始点及び作業終了点各々の位置及び姿勢を結ぶ教示経路上で前記作業部の位置を予め定められた速度で移動させるための複数の補間点を算出する補間点算出手段と，
前記補間点算出手段により算出された前記補間点に従って前記第１関節駆動系及び前記第２関節駆動系を駆動させる駆動制御手段と，
前記駆動制御手段により前記第１関節駆動系及び前記第２関節駆動系が駆動されているときに，前記第１関節駆動系の特異姿勢の検出条件とは異なる予め設定された例外条件が充足したか否かを判定する例外条件判定手段と，
前記例外条件判定手段により前記例外条件が充足したと判定された後，前記作業部が前記作業終了点に到達するまでの間，前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形に変化させるための該第１関節駆動系の駆動軸各々の角度を算出すると共に，該算出された第１関節駆動系の駆動軸各々の角度と前記補間点算出手段によって算出された前記補間点における前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を算出し，該算出結果に従って前記第１関節駆動系及び前記第２関節駆動系の駆動軸各々を駆動させる例外動作手段と，
を備えてなり，
前記例外動作手段が，
前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形動作させる場合より該目標に近い角度の範囲内において該駆動軸各々の角度の候補を複数算出する角度候補算出手段と，
前記角度候補算出手段により算出された角度の候補から，該候補各々を採用したときの次の前記補間点の前記作業部から見た作業座標系における３つの姿勢角度の成分のうち予め設定された一つ又は二つの特定成分の変動が最も抑制される角度の候補を選択する角度選択手段と，
を備えてなることを特徴とする多関節型ロボットの制御装置。
【請求項２】
前記例外条件が，次の前記補間点からの前記作業部の残りの移動時間又は移動距離の間
に前記第１関節駆動系の駆動軸各々の角度を前記作業終了点における角度に到達させる場合に前記第１関節駆動系の少なくとも一つの駆動軸の速度が予め設定された許容範囲を超えることである請求項１に記載の多関節型ロボットの制御装置。
【請求項３】
前記例外条件が，前記作業部の残りの移動時間又は移動距離が予め設定された所定値以下に達することである請求項１又は２のいずれかに記載の多関節型ロボットの制御装置。
【請求項４】
前記例外条件が，次の前記補間点における前記第１関節駆動系の少なくとも一つの駆動軸の角度が前記作業開始点及び／又は前記作業終了点に基づいて定まる基準角度から予め設定された所定角度以上離間することである請求項１〜３のいずれかに記載の多関節型ロボットの制御装置。
【請求項５】
前記例外条件が，次の前記補間点における前記第１関節駆動系の少なくとも一つの駆動軸の角度が予め設定された作動範囲を超えることである請求項１〜４のいずれかに記載の多関節型ロボットの制御装置。
【請求項６】
前記作業部と加工対象物の加工線とのズレ量を検出するズレ量検出手段と，前記ズレ量検出手段により検出されたズレ量に基づいて前記作業部を前記加工線に倣って移動させる倣い制御手段と，前記ズレ量検出手段により検出されたズレ量を前記作業終了点に加算して該作業終了点を更新する作業終了点更新手段とを更に備えてなり，
前記例外条件判定手段及び／又は前記例外動作手段が，前記作業終了点更新手段による更新後の前記作業終了点に基づいて処理を実行するものである請求項１〜５のいずれかに記載の多関節型ロボットの制御装置。
【請求項７】
先端に設けられた作業部の姿勢を変化させる３つの駆動軸を有する第１関節駆動系と前記第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を有する第２関節駆動系とを備えてなる多関節型ロボットを制御する多関節型ロボットの制御方法であって，
予め設定される作業開始点及び作業終了点各々の位置及び姿勢を結ぶ教示経路上で前記作業部の位置を予め定められた速度で移動させるための複数の補間点を算出する補間点算出工程と，
前記補間点算出工程により算出された前記補間点に従って前記第１関節駆動系及び前記第２関節駆動系を駆動させる駆動制御工程と，
前記駆動制御工程により前記第１関節駆動系及び前記第２関節駆動系が駆動されているときに，前記第１関節駆動系の特異姿勢の検出条件とは異なる予め設定された例外条件が充足したか否かを判定する例外条件判定工程と，
前記例外条件判定工程により前記例外条件が充足したと判定された後，前記作業部が前記作業終了点に到達するまでの間，前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形に変化させるための該第１関節駆動系の駆動軸各々の角度を算出すると共に，該算出された第１関節駆動系の駆動軸各々の角度と前記補間点算出工程によって算出された前記補間点における前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を算出し，該算出結果に従って前記第１関節駆動系及び前記第２関節駆動系の駆動軸各々を駆動させる例外動作工程と，を実行するものであって，
前記例外動作工程が，
前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形動作させる場合より該目標に近い角度の範囲内において該駆動軸各々の角度の候補を複数算出する角度候補算出工程と，
前記角度候補算出手段により算出された角度の候補から，該候補各々を採用したときの次の前記補間点の前記作業部から見た作業座標系における３つの姿勢角度の成分のうち予め設定された一つ又は二つの特定成分の変動が最も抑制される角度の候補を選択する角度選択工程と，  を実行することを特徴とする多関節型ロボットの制御方法。
【請求項８】
先端に設けられた作業部の姿勢を変化させる３つの駆動軸を有する第１関節駆動系と前記第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を有する第２関節駆動系とを備えてなる多関節型ロボットを制御するための多関節型ロボットの制御プログラムであって，
予め設定される作業開始点及び作業終了点各々の位置及び姿勢を結ぶ教示経路上で前記作業部の位置を予め定められた速度で移動させるための複数の補間点を算出する補間点算出工程と，
前記補間点算出工程により算出された前記補間点に従って前記第１関節駆動系及び前記第２関節駆動系を駆動させる駆動制御工程と，
前記駆動制御工程により前記第１関節駆動系及び前記第２関節駆動系が駆動されているときに，前記第１関節駆動系の特異姿勢の検出条件とは異なる予め設定された例外条件が充足したか否かを判定する例外条件判定工程と，
前記例外条件判定工程により前記例外条件が充足したと判定された後，前記作業部が前記作業終了点に到達するまでの間，前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形に変化させるための該第１関節駆動系の駆動軸各々の角度を算出すると共に，該算出された第１関節駆動系の駆動軸各々の角度と前記補間点算出工程によって算出された前記補間点における前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を算出し，該算出結果に従って前記第１関節駆動系及び前記第２関節駆動系の駆動軸各々を駆動させる例外動作工程と，をプロセッサに実行させるものであって，
前記例外動作工程が，
前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形動作させる場合より該目標に近い角度の範囲内において該駆動軸各々の角度の候補を複数算出する角度候補算出工程と，
前記角度候補算出手段により算出された角度の候補から，該候補各々を採用したときの次の前記補間点の前記作業部から見た作業座標系における３つの姿勢角度の成分のうち予め設定された一つ又は二つの特定成分の変動が最も抑制される角度の候補を選択する角度選択工程と，をプロセッサに実行させることを特徴とする多関節型ロボットの制御プログラム。 
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は，先端に設けられた作業部の姿勢を変化させる３つの駆動軸を有する第１関節駆動系と前記第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を有する第２関節駆動系とを備えてなる多関節型ロボットの制御技術に関するものである。
【背景技術】
【０００２】
図２３に示す多関節型ロボットの一種である６軸マニピュレータは，先端に設けられたエンドエフェクタ（作業部）の姿勢を変化させるＪ４軸，Ｊ５軸，Ｊ６軸を有する第１関節駆動系と，その第１関節駆動系の位置を変化させるＪ１軸，Ｊ２軸，Ｊ３軸を有する第２関節駆動系とを備えている。そして，６軸マニピュレータの制御装置は，予め設定された作業開始点及び作業終了点各々の位置及び姿勢に基づいてＪ１軸〜Ｊ６軸を制御することによりエンドエフェクタの位置及び姿勢を変化させる。
【０００３】
より具体的には，作業開始点と作業終了点とを結ぶ教示経路を補間する複数の補間点を算出し，該補間点に従ってＪ１軸〜Ｊ６軸を駆動させる。なお，エンドエフェクタの位置座標Ｘ，Ｙ，Ｚ及び姿勢角α，β，γが与えられたとき，逆運動学問題の解を求めることによってＪ１軸〜Ｊ６軸の各関節角θ1〜θ6を求めることができる。但し，このとき逆運動学問題の解として得られる関節角θ1〜θ6には２通りの解が存在する。具体的に，二つの解におけるＪ４軸の角度θ４，θ’４，Ｊ５軸の角度θ５，θ’５，Ｊ６軸の角度θ６，θ’６は，θ４−θ’４＝±１８０度，θ５＋θ’５＝０度，θ６−θ’６＝±１８０度の関係にある。
【０００４】
ところで，６軸マニピュレータにおいてＪ５軸の角度が０度になると，Ｊ４軸，Ｊ６軸の速度が急激に変化するいわゆる特異姿勢（特異点）をとることになる。そのため，一般には，作業開始点における解と同じ符号のままでＪ５軸の角度を遷移させることにより特異姿勢を回避している。但し，この場合には，作業開始点及び作業終了点におけるＪ５軸の角度の符号が異符号であるとき，そのＪ５軸の角度を作業終了点における角度に到達させることができない。
【０００５】
これに対し，例えば特許文献１では，特異姿勢近傍であることが検出された場合に，逆運動学問題の他方の解に移行させることが開示されている。このような構成によれば，作業開始点及び作業終了点におけるＪ５軸の角度の符号が異符号である場合でも，該Ｊ５軸の角度を作業終了点における角度に到達させることができる。なお，一般に特異姿勢の検出は，Ｊ５軸の角度が０度近傍に達することや，Ｊ４軸及びＪ６軸の角度が急激に変化することを条件に行われる。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開平０２−２１８５６９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら，前記特許文献１に係る構成では，特異姿勢近傍に達したことが逆運動学問題の他方の解への移行条件であるため，特異点姿勢に近づくことなく動作する場合には，駆動軸を作業終了点の正しい角度に到達させることができない。
具体的に，図２４は，作業開始点及び作業終了点におけるＪ５軸の角度の符号が異符号であるが，Ｊ５軸の角度が０度近傍に到達せず，Ｊ４軸やＪ６軸の角度も急激に変化することなく動作する場合の軌跡を示している。この場合，特異姿勢近傍に近づいたことを検出することができない。そのため，Ｊ５軸の角度が作業開始点における符号のまま移動するため，Ｊ４軸〜Ｊ６軸の角度を作業終了点の正しい角度に到達させることができない。
そのため，例えば作業終了点の次の教示点に向けて動作する場合にＪ４軸〜Ｊ６軸の角度が作動範囲外に到達するおそれがある。
【０００８】
従って，本発明は上記事情に鑑みてなされたものであり，その目的とするところは，作業部の位置及び移動速度を維持しつつ，第１関節駆動系の駆動軸各々の角度を確実に作業終了点の角度に到達させることのできる多関節型ロボットの制御装置，制御方法及び制御プログラムを提供することにある。
【課題を解決するための手段】
【０００９】
上記目的を達成するために本発明は，先端に設けられた作業部の姿勢を変化させる３つの駆動軸を有する第１関節駆動系と前記第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を有する第２関節駆動系とを備えてなる多関節型ロボットを制御する多関節型ロボットの制御装置に適用されるものであって，以下の（１）〜（４）を備えてなることを特徴として構成される。
【００１０】
（１）予め設定される作業開始点及び作業終了点各々の位置及び姿勢を結ぶ教示経路上で前記作業部の位置を予め定められた速度で移動させるための複数の補間点を算出する補間点算出手段。
（２）前記補間点算出手段により算出された前記補間点に従って前記第１関節駆動系及び前記第２関節駆動系を駆動させる駆動制御手段。
【００１１】
（３）前記駆動制御手段により前記第１関節駆動系及び前記第２関節駆動系が駆動されているときに，前記第１関節駆動系の特異姿勢の検出条件とは異なる予め設定された例外条件が充足したか否かを判定する例外条件判定手段。
（４）前記例外条件判定手段により前記例外条件が充足したと判定された後，前記作業部が前記作業終了点に到達するまでの間，前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形に変化させるための該第１関節駆動系の駆動軸各々の角度を算出すると共に，該算出された第１関節駆動系の駆動軸各々の角度と前記補間点算出手段によって算出された前記補間点における前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を算出し，該算出結果に従って前記第１関節駆動系及び前記第２関節駆動系の駆動軸各々を駆動させる例外動作手段。
【００１２】
これにより，前記作業開始点及び前記作業終了点における前記第１関節駆動系の中央の駆動軸の符号の異同にかかわらず，前記補間点算出手段で算出された前記補間点における前記作業部の位置及び移動速度を維持しつつ，前記第１関節駆動系の駆動軸各々の角度を前記作業終了点の角度に確実に到達させることができる。
例えば，前記例外条件は，次の前記補間点からの前記作業部の残りの移動時間又は移動距離の間に前記第１関節駆動系の駆動軸各々の角度を前記作業終了点における角度に到達させる場合に前記第１関節駆動系の少なくとも一つの駆動軸の速度が予め設定された許容範囲を超えることである。これにより，前記第１関節駆動系の駆動軸各々の速度を前記許容範囲に抑えつつ該第１関節駆動系の駆動軸各々の角度を前記作業終了点の角度に確実に到達させることができる。また，前記例外条件が，前記作業部の残りの移動時間又は移動距離が予め設定された所定値以下に達することであってもよい。
【００１３】
さらに，前記例外条件は，次の前記補間点における前記第１関節駆動系の少なくとも一つの駆動軸の角度が前記作業開始点や前記作業終了点に基づいて定まる基準角度から予め設定された所定角度以上離間することであってもよい。これにより，例えば前記第１関節駆動系の駆動軸各々の角度が前記基準角度から離れ過ぎて前記作業終了点の角度に到達するまでの速度が急激になることを予防し，該第１関節駆動系の駆動軸各々を前記作業終了点に確実に到着させることができる。
【００１４】
また，前記例外条件は，次の前記補間点における前記第１関節駆動系の少なくとも一つの駆動軸の角度が予め設定された作動範囲を超えることであってもよい。これにより，前記作動範囲を超えない範囲で前記第１関節駆動系の駆動軸各々を駆動させて前記作業終了点の角度に到達させることができる。
ところで，前記作業部と加工対象物の加工線とのズレ量を検出するズレ量検出手段と，前記ズレ量検出手段により検出されたズレ量に基づいて前記作業部を前記加工線に倣って移動させる倣い制御手段とを備える構成では，前記作業終了点の位置が変化することになる。
【００１５】
そこで，前記ズレ量検出手段により検出されたズレ量を前記作業終了点に加算して該作業終了点を更新する作業終了点更新手段を更に備えてなり，前記例外条件判定手段や前記例外動作手段が前記作業終了点更新手段による更新後の前記作業終了点に基づいて処理を実行するものであることが考えられる。これにより，前記例外動作手段は，前記倣い制御手段による倣い制御が実行される場合であっても，前記駆動軸各々を前記作業終了点更新手段による更新後の作業終了点に到達させることができる。前記例外条件判定手段による前記例外条件の判定も前記作業終了点更新手段による更新後の作業終了点を基準として適切に行うことができる。
【００１６】
さらに，前記例外動作手段が，前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形動作させる場合より該目標に近い角度の範囲内において該駆動軸各々の角度の候補を複数算出する角度候補算出手段と，前記角度候補算出手段により算出された角度の候補から，該候補各々を採用したときの次の前記補間点の前記作業部から見た作業座標系における３つの姿勢角度の成分のうち予め設定された一つ又は二つの特定成分の変動が最も抑制される角度の候補を選択する角度選択手段とを備えることも考えられる。
【００１７】
これにより，前記特定成分の変動を抑制しつつ前記第１関節駆動系の駆動軸各々を前記作業終了点における角度に確実に到達させることができる。
また，本発明は，多関節型ロボットの制御方法や制御プログラムの発明として捉えることも可能である。
即ち，予め設定される作業開始点及び作業終了点各々の位置及び姿勢を結ぶ教示経路上で前記作業部の位置を予め定められた速度で移動させるための複数の補間点を算出する補間点算出工程と，前記補間点算出工程により算出された前記補間点に従って前記第１関節駆動系及び前記第２関節駆動系を駆動させる駆動制御工程と，前記駆動制御工程により前記第１関節駆動系及び前記第２関節駆動系が駆動されているときに，前記第１関節駆動系の駆動軸各々が前記作業終了点の角度に到達するまでの間に該第１関節駆動系の少なくとも一つの駆動軸の速度又は角度が予め設定された所定範囲を超えることを，前記第１関節駆動系の特異姿勢の検出条件とは異なる予め設定された例外条件により判定する例外条件判定工程と，前記例外条件判定工程により前記例外条件が充足したと判定された場合に，前記駆動制御工程に代わって，前記第１関節駆動系及び前記第２関節駆動系を，前記第１関節駆動系の駆動軸各々が前記作業終了点の角度を目標として線形に変化し，且つ前記作業部の位置が予め定められた速度で移動するように駆動させる例外動作工程とを実行する
多関節型ロボットの制御方法や，前記各工程をプロセッサに実行させる制御プログラムの発明として捉えてもよい。
なお、本発明にかかる多関節型ロボットの制御装置の最も好ましい形態は、先端に設けられた作業部の姿勢を変化させる３つの駆動軸を有する第１関節駆動系と前記第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を有する第２関節駆動系とを備えてなる多関節型ロボットを制御する多関節型ロボットの制御装置であって，予め設定される作業開始点及び作業終了点各々の位置及び姿勢を結ぶ教示経路上で前記作業部の位置を予め定められた速度で移動させるための複数の補間点を算出する補間点算出手段と，前記補間点算出手段により算出された前記補間点に従って前記第１関節駆動系及び前記第２関節駆動系を駆動させる駆動制御手段と，前記駆動制御手段により前記第１関節駆動系及び前記第２関節駆動系が駆動されているときに，前記第１関節駆動系の特異姿勢の検出条件とは異なる予め設定された例外条件が充足したか否かを判定する例外条件判定手段と，前記例外条件判定手段により前記例外条件が充足したと判定された後，前記作業部が前記作業終了点に到達するまでの間，前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形に変化させるための該第１関節駆動系の駆動軸各々の角度を算出すると共に，該算出された第１関節駆動系の駆動軸各々の角度と前記補間点算出手段によって算出された前記補間点における前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を算出し，該算出結果に従って前記第１関節駆動系及び前記第２関節駆動系の駆動軸各々を駆動させる例外動作手段と，を備えてなり，前記例外動作手段が，前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形動作させる場合より該目標に近い角度の範囲内において該駆動軸各々の角度の候補を複数算出する角度候補算出手段と，前記角度候補算出手段により算出された角度の候補から，該候補各々を採用したときの次の前記補間点の前記作業部から見た作業座標系における３つの姿勢角度の成分のうち予め設定された一つ又は二つの特定成分の変動が最も抑制される角度の候補を選択する角度選択手段と，を備えてなることを特徴とする。
また、本発明にかかる多関節型ロボットの制御方法の最も好ましい形態は、先端に設けられた作業部の姿勢を変化させる３つの駆動軸を有する第１関節駆動系と前記第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を有する第２関節駆動系とを備えてなる多関節型ロボットを制御する多関節型ロボットの制御方法であって，予め設定される作業開始点及び作業終了点各々の位置及び姿勢を結ぶ教示経路上で前記作業部の位置を予め定められた速度で移動させるための複数の補間点を算出する補間点算出工程と，前記補間点算出工程により算出された前記補間点に従って前記第１関節駆動系及び前記第２関節駆動系を駆動させる駆動制御工程と，前記駆動制御工程により前記第１関節駆動系及び前記第２関節駆動系が駆動されているときに，前記第１関節駆動系の特異姿勢の検出条件とは異なる予め設定された例外条件が充足したか否かを判定する例外条件判定工程と，前記例外条件判定工程により前記例外条件が充足したと判定された後，前記作業部が前記作業終了点に到達するまでの間，前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形に変化させるための該第１関節駆動系の駆動軸各々の角度を算出すると共に，該算出された第１関節駆動系の駆動軸各々の角度と前記補間点算出工程によって算出された前記補間点における前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を算出し，該算出結果に従って前記第１関節駆動系及び前記第２関節駆動系の駆動軸各々を駆動させる例外動作工程と，を実行するものであって，前記例外動作工程が，前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形動作させる場合より該目標に近い角度の範囲内において該駆動軸各々の角度の候補を複数算出する角度候補算出工程と，前記角度候補算出手段により算出された角度の候補から，該候補各々を採用したときの次の前記補間点の前記作業部から見た作業座標系における３つの姿勢角度の成分のうち予め設定された一つ又は二つの特定成分の変動が最も抑制される角度の候補を選択する角度選択工程と，を実行することを特徴とする。
また、本発明にかかる多関節型ロボットの制御プログラムの最も好ましい形態は、先端に設けられた作業部の姿勢を変化させる３つの駆動軸を有する第１関節駆動系と前記第１
関節駆動系の位置を変化させる少なくとも３つの駆動軸を有する第２関節駆動系とを備えてなる多関節型ロボットを制御するための多関節型ロボットの制御プログラムであって，予め設定される作業開始点及び作業終了点各々の位置及び姿勢を結ぶ教示経路上で前記作業部の位置を予め定められた速度で移動させるための複数の補間点を算出する補間点算出工程と，前記補間点算出工程により算出された前記補間点に従って前記第１関節駆動系及び前記第２関節駆動系を駆動させる駆動制御工程と，前記駆動制御工程により前記第１関節駆動系及び前記第２関節駆動系が駆動されているときに，前記第１関節駆動系の特異姿勢の検出条件とは異なる予め設定された例外条件が充足したか否かを判定する例外条件判定工程と，前記例外条件判定工程により前記例外条件が充足したと判定された後，前記作業部が前記作業終了点に到達するまでの間，前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形に変化させるための該第１関節駆動系の駆動軸各々の角度を算出すると共に，該算出された第１関節駆動系の駆動軸各々の角度と前記補間点算出工程によって算出された前記補間点における前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を算出し，該算出結果に従って前記第１関節駆動系及び前記第２関節駆動系の駆動軸各々を駆動させる例外動作工程と，をプロセッサに実行させるものであって，前記例外動作工程が，前記第１関節駆動系の駆動軸各々を前記作業終了点の角度を目標として線形動作させる場合より該目標に近い角度の範囲内において該駆動軸各々の角度の候補を複数算出する角度候補算出工程と，前記角度候補算出手段により算出された角度の候補から，該候補各々を採用したときの次の前記補間点の前記作業部から見た作業座標系における３つの姿勢角度の成分のうち予め設定された一つ又は二つの特定成分の変動が最も抑制される角度の候補を選択する角度選択工程と，をプロセッサに実行させることを特徴とする。
【発明の効果】
【００１８】
本発明によれば，前記作業開始点及び前記作業終了点における前記第１関節駆動系の中央の駆動軸の符号の異同にかかわらず，前記補間点算出手段で算出された前記補間点における前記作業部の位置及び移動速度を維持しつつ，前記第１関節駆動系の駆動軸各々の角度を前記作業終了点の角度に確実に到達させることができる。
【図面の簡単な説明】
【００１９】
【図１】本発明の実施形態に係る多関節型ロボットの制御装置１及び多関節型ロボット２の概略構成を表すブロック図。
【図２】多関節型ロボット２の関節駆動系を示すモデル図。
【図３】本発明の実施形態に係る多関節型ロボットの制御装置１が実行するロボット制御処理の手順の一例を示すフローチャート。
【図４】従来の駆動軸３４〜３６の動作例を示す図。
【図５】実施の形態に係るロボット制御処理が実行されたときの駆動軸３４〜３６の動作例を示す図。
【図６】従来の駆動軸３４〜３６の動作例を示す図。
【図７】実施例１に係るロボット制御処理が実行されたときの駆動軸３４〜３６の動作例を示す図。
【図８】ロボット制御処理における線形動作の変形例を説明するための図。
【図９】実施例２に係るロボット制御処理の手順の一例を示すフローチャート。
【図１０】実施例２に係るロボット制御処理が実行されたときの駆動軸３４〜３６の動作例を示す図。
【図１１】倣い制御及びロボット制御処理が共に実行されるときのエンドエフェクタ２の動作例を示す図。
【図１２】特定成分抑制処理の手順の一例を示すフローチャート。
【図１３】溶接線座標系Σｌｉｎｅを説明するための図。
【図１４】溶接線座標系Σｌｉｎｅを説明するための図。
【図１５】溶接線座標系Σｌｉｎｅを説明するための図。
【図１６】溶接線座標系Σｌｉｎｅを説明するための図。
【図１７】特定成分抑制処理の実行結果を説明するための図。
【図１８】特定成分抑制処理の実行結果を説明するための図。
【図１９】特定成分抑制処理の実行結果を説明するための図。
【図２０】特定成分抑制処理の実行結果を説明するための図。
【図２１】特定成分抑制処理の実行結果を説明するための図。
【図２２】特定成分抑制処理の実行結果を説明するための図。
【図２３】一般的な多関節型ロボットの関節駆動系を示すモデル図。
【図２４】特異姿勢に近づかない場合の従来の駆動軸Ｊ４軸〜Ｊ６軸の動作例を示す図。
【図２５】特異姿勢を回避中であることを報知する装置を説明するための図。
【発明を実施するための形態】
【００２０】
以下添付図面を参照しながら，本発明の実施の形態について説明し，本発明の理解に供する。尚，以下の実施の形態は，本発明を具体化した一例であって，本発明の技術的範囲を限定する性格のものではない。
図１に示すように，本発明の実施の形態に係る多関節型ロボットの制御装置１（以下「制御装置１」と略称する）は，先端にエンドエフェクタ２が設けられた多関節型ロボット３の動作を制御するものであって，演算部１１，記憶部１２，操作部１３などを備えている。前記エンドエフェクタ２は，加工対象物に対して，例えば溶接や塗装などの作用を与える溶接器（トーチ），塗装器，工具，捕獲器などである。
【００２１】
前記記憶部１２は，所定の制御プログラムや各種処理で参照されるパラメータ情報などが予め記憶されるＲＯＭやＨＤＤ等と，前記演算部１１で各種処理が実行される際の作業領域（一時記憶領域）として利用されるＥＥＰＲＯＭなどの揮発性メモリとを有している。
前記操作部１３は，シートキーや操作ボタン，操作レバー等を有するユーザインターフェースである。前記操作部１３は，ユーザ操作に対応する操作信号を前記演算部１１に入力する。具体的に，ユーザによって前記操作部１３が操作されることにより，前記多関節型ロボット３の作業開始点，作業終了点，作業時間，移動速度などの各種加工パラメータが設定される。
【００２２】
前記演算部１１は，前記記憶部１２に記憶された前記制御プログラムに従って後述のロボット制御処理（図３参照）などの各種処理を実行するＣＰＵなどのプロセッサである。
なお，本発明は，前記演算部１１に後述のロボット制御処理を実行させるための制御プログラムの発明として捉えてもよい。
ここに，図２は前記多関節型ロボット３の関節駆動系を示すモデル図である。
【００２３】
図２に示すように，前記多関節型ロボット３は，前記エンドエフェクタ２の姿勢を変化させる３つの駆動軸３４〜３６を有する第１関節駆動系と，その第１関節駆動系の後端の駆動軸３４に連結されて該第１関節駆動系の位置を変化させる３つの駆動軸３１〜３３を有する第２関節駆動系とを備えている。前記駆動軸３１〜３６各々は，電気モータ等の駆動手段によって例えば±１８０度の範囲で個別に回転駆動される。なお，前記第２関節駆動系は４軸以上の駆動軸を有する構成であってもよい。
【００２４】
前記多関節型ロボット３における前記エンドエフェクタ２の位置・姿勢は，該多関節型ロボット３のベース座標系Σｂａｓｅにおける位置座標ＸＹＺと姿勢角αβγとによって表される。前記姿勢角αβγは，オイラー角やロール・ピッチ・ヨー角などで表される。
ここで，前記第２関節駆動系は，前記第１関節駆動系の駆動軸３４〜３６の動作にかかわらず，前記多関節型ロボット３のベース座標系Σｂａｓｅにおける前記エンドエフェクタ２の位置座標ＸＹＺを自由に作り出すことができる。
【００２５】
なお，前記多関節型ロボット３における前記駆動軸３１〜３６各々の角度θ１〜θ６は，前記多関節型ロボット３のベース座標系Σｂａｓｅにおける位置座標ＸＹＺ，姿勢角αβγから逆運動学問題の解を求めることにより算出可能である。
以下，図３のフローチャートに従って，前記制御装置１により実行されるロボット制御処理の手順の一例について説明する。ここに，図３に示すＳ１，Ｓ２，…は処理手順（ステップ）番号を表す。なお，本発明は，以下の処理手順の各工程を実行する多関節型ロボットの制御方法の発明として捉えてもよい。
【００２６】
前記制御装置１は，当該ロボット制御処理を実行することにより，前記操作部１３によって予め設定された作業開始点及び作業終了点に基づいて前記多関節型ロボット３を制御する。ここでは，前記操作部１３の操作により作業開始点Ｑ１の位置・姿勢｛Ｘｓ，Ｙｓ，Ｚｓ，αｓ，βｓ，γｓ｝，作業終了点Ｑ２の位置・姿勢｛Ｘｅ，Ｙｅ，Ｚｅ，αｅ，βｅ，γｅ｝が予め設定されているものとする。
（ステップＳ１）
まず，ステップＳ１において，前記制御装置１は，前記作業開始点Ｑ１及び前記作業終了点Ｑ２各々の位置及び姿勢を結ぶ教示経路を補間する複数の補間点を算出するための経路方程式Ｐ（ｔ）＝Ｑ１＋ΔＰ・ｔを求める。ここに，ΔＰは単位時間（１ｍｓｅｃ）あたりの前記エンドエフェクタ２の位置の移動量，ｔは前記作業開始点Ｑ１から計時される制御時間［ｍｓｅｃ］を示している。なお，前記経路方程式Ｐ（ｔ）で算出される補間点は，前記多関節型ロボット３のベース座標系Σｂａｓｅにおける位置・姿勢ＸＹＺαβγ
（ｔ）より表されるものである。
【００２７】
具体的に，前記経路方程式Ｐ（ｔ）は，前記エンドエフェクタ２の位置を前記作業開始点Ｑ１から前記作業終了点Ｑ２まで予め定められた速度で移動させるための複数の補間点を算出するものである。具体的に，前記エンドエフェクタ２の位置が前記作業開始点Ｑ１から前記作業終了点Ｑ２まで移動するときの合計制御時間をＴ［ｍｓｅｃ］，前記エンドエフェクタ２の位置・姿勢ＸＹＺαβγの単位時間（１ｍｓｅｃ）あたりの変化量をΔＸ，ΔＹ，ΔＺ，Δα，Δβ，Δγとすると，ΔＰは，（Ｑ２−Ｑ１）／Ｔ＝｛ΔＸ＝（Ｘｅ−Ｘｓ）／Ｔ，ΔＹ＝（Ｙｅ−Ｙｓ）／Ｔ，ΔＺ＝（Ｚｅ−Ｚｓ）／Ｔ，Δα＝（αｅ−αｓ）／Ｔ，Δβ＝（βｅ−βｓ）／Ｔ，Δγ＝（γｅ−γｓ）／Ｔ｝によって表される。もちろん，前記合計制御時間Ｔが設定されている場合に限らず，前記エンドエフェクタ２の移動速度が予め定められた速度に設定されていてもよい。
（ステップＳ２〜Ｓ３）
続いて，ステップＳ２において，前記制御装置１は，前記作業終了点Ｑ２における前記エンドエフェクタ２の位置・姿勢｛Ｘｅ，Ｙｅ，Ｚｅ，αｅ，βｅ，γｅ｝の逆運動学問題の解を求めることにより，該作業終了点Ｑ２における前記駆動軸３１〜３６の角度｛θ１ｅ，θ２ｅ，θ３ｅ，θ４ｅ，θ５ｅ，θ６ｅ｝を算出する。なお，前記操作部１３による教示時に前記作業終了点Ｑ２における前記駆動軸３１〜３６の角度θ１ｅ〜θ６ｅが直接設定されてもよい。
【００２８】
また，ステップＳ３において，前記制御装置１は，制御時間ｔ及び制御開始フラグＦを０に初期設定する。なお，前記制御開始フラグＦは，後述の例外条件（Ｓ６１）が既に充足されているか否かを示す情報であって，後述のステップＳ６２において１に設定される。
（ステップＳ４〜Ｓ６）
次に，ステップＳ４において，前記制御装置１は前記制御時間ｔを更新する（ｔ＝ｔ＋Δｔ）。ここに，Δｔは，前記制御装置１による前記多関節型ロボット３の制御周期（サンプル周期）であって，例えば１０［ｍｓｅｃ］や１００［ｍｓｅｃ］である。
【００２９】
そして，ステップＳ５において，前記制御装置１は，前記経路方程式Ｐ（ｔ）に基づいて次の補間点における前記駆動軸３１〜３６各々の角度θ１（ｔ）〜θ６（ｔ）を算出する。ここに，係る算出処理を実行するときの前記制御装置１が補間点算出手段に相当する。
具体的に，まず前記制御装置１は，次の補間点の位置・姿勢ＸＹＺαβγ（ｔ）＝｛Ｘｓ＋ΔＸ・ｔ，Ｙｓ＋ΔＹ・ｔ，Ｚｓ＋ΔＺ・ｔ，αｓ＋Δα・ｔ，βｓ＋Δβ・ｔ，γｓ＋Δγ・ｔ｝を算出する。そして，前記制御装置１は，前記位置・姿勢ＸＹＺαβγ（ｔ）の逆運動学問題の解を求めることにより，次の補間点における前記駆動軸３１〜３６各々の角度θ１（ｔ），θ２（ｔ），θ３（ｔ），θ４（ｔ），θ５（ｔ），θ６（ｔ）を算出する。なお，前記制御開始フラグＦが１に設定された後は，前記角度θ１（ｔ）〜θ６（ｔ）を用いないため，当該ステップＳ５の処理は省略してもよい。
【００３０】
その後，ステップＳ６において，前記制御装置１は，前記制御開始フラグＦが１であるか否かを判断する。ここで，前記制御開始フラグＦが１であると判断されると（Ｓ６のＹｅｓ側），処理はステップＳ７に移行し，前記制御開始フラグＦが０であると判断されると（Ｓ６のＮｏ側），処理はステップＳ６１に移行する。
（ステップＳ６１）
ステップＳ６１において，前記制御装置１は，前記第１関節駆動系の特異姿勢の検出条件とは異なる予め設定された例外条件が充足したか否かを判定する。ここに，当該ステップＳ６１は，後述のステップＳ６４において前記第１関節駆動系及び前記第２関節駆動系の駆動軸３１〜３６各々が前記ステップＳ５で算出された角度θ１（ｔ）〜θ６（ｔ）に基づいて駆動されているときに実行される。ここに，係る判定処理を実行するときの前記制御装置１が例外条件判定手段に相当する。前記第１関節駆動系の特異姿勢の検出条件とは，例えば前記駆動軸３５の角度が０に近づいたことや前記駆動軸３４，３６の角度が急激に変化したことである。
【００３１】
具体的に，本実施の形態では，次の補間点からの前記エンドエフェクタ２の残りの移動時間の間に前記第１関節駆動系の駆動軸３４〜３６各々の角度を前記作業終了点Ｑ２における角度θ４ｅ〜θ６ｅに到達させる場合に前記第１関節駆動系の駆動軸３４〜３６の少なくとも一つの駆動軸の速度が予め設定された許容範囲を超えることが前記例外条件として予め設定されているものとする。即ち，前記制御装置１は，当該ステップＳ６１において，前記エンドエフェクタ２の残りの移動時間の間に，前記駆動軸３４〜３６各々の速度を前記許容範囲内に維持しつつ前記作業終了点Ｑ２に到達させることができるか否かを判定する。
【００３２】
例えば，前記制御装置１は，以下の（１１）式〜（１３）式のいずれかが成立する場合に，前記例外条件が充足したと判定することが可能である。ここに，Ｔ（ｔ）は次の補間点における残りの移動時間，θ４ｄ，θ５ｄ，θ６ｄは前記駆動軸３４〜３６の速度の許容範囲の上限値である。
｜（θ４ｅ−θ４（ｔ））／Ｔ（ｔ）｜＞θ４ｄ …（１１）
｜（θ５ｅ−θ５（ｔ））／Ｔ（ｔ）｜＞θ５ｄ …（１２）
｜（θ６ｅ−θ６（ｔ））／Ｔ（ｔ）｜＞θ６ｄ …（１３）
なお，前記エンドエフェクタ２の残りの移動時間に限らず，該エンドエフェクタ２の残りの移動距離（＝残りの移動時間×移動速度）の間に，前記駆動軸３４〜３６各々の速度を許容範囲内に維持しつつ，前記エンドエフェクタ２を前記作業終了点Ｑ２に到達させることができるか否かを判定してもよい。また，単に前記エンドエフェクタ２の残りの移動時間や残りの移動距離が予め設定された所定値以下に達することを前記例外条件として用いることも他の実施例として考えられる。
【００３３】
そして，当該ステップＳ６１において，前記例外条件が充足していないと判断されると（Ｓ６１のＮｏ側），処理はステップＳ６４に移行し，前記例外条件が充足したと判断されると（Ｓ６１のＹｅｓ側），処理はステップＳ６２に移行する。
（ステップＳ６４）
ステップＳ６４において，前記制御装置１は，前記駆動軸３１〜３６各々を前記ステップＳ５で算出された角度θ１（ｔ），θ２（ｔ），θ３（ｔ），θ４（ｔ），θ５（ｔ），θ６（ｔ）に基づいて駆動させ，処理をステップＳ１２に移行させる。なお，当該ステップＳ６４における処理は従来と同様であるため，詳細な説明は省略する。ここに，係る駆動処理を実行するときの前記制御装置１が駆動制御手段に相当する。
（ステップＳ６２〜Ｓ６３）
一方，前記例外条件が充足すると，続くステップＳ６２において，前記制御装置１は，前記制御開始フラグＦを１に設定する。これにより，今後，前記ステップＳ６においては，前記制御開始フラグＦが１であると判断され，処理は前記ステップＳ６１〜Ｓ６３を経由することなく後述のステップＳ７に移行することとなる。
【００３４】
次に，ステップＳ６３において，前記制御装置１は，前記駆動軸３４〜３６各々の現在の角度をθ４OLD ，θ５OLD ，θ６OLD として前記記憶部１２に記憶する。即ち，θ４OLD ＝θ４（ｔ−Δｔ），θ５OLD ＝θ５（ｔ−Δｔ），θ６OLD ＝θ６（ｔ−Δｔ）である。
（ステップＳ７〜Ｓ８）
そして，ステップＳ７において，前記制御装置１は，現在の補間点からの残りの移動時間Ｔ（ｔ−Δｔ）の間に，前記駆動軸３４〜３６各々の角度を前記作業終了点Ｑ２の角度に到達させるために必要な前記駆動軸３４〜３６各々の単位時間（１ｍｓｅｃ）あたりの速度Ｖθ４，Ｖθ５，Ｖθ６を算出する。ここに，速度Ｖθ４，Ｖθ５，Ｖθ６は，現在の前記駆動軸３４〜３６各々の角度θ４OLD ，θ５OLD ，θ６OLD を前記作業終了点Ｑ２における角度θ４ｅ，θ５ｅ，θ６ｅに線形に変化させるための速度である。具体的に，速度Ｖθ４，Ｖθ５，Ｖθ６は，以下の（２１）式〜（２３）式により算出可能である。
Ｖθ４＝（θ４ｅ−θ４OLD ）／Ｔ（ｔ−Δｔ） …（２１）
Ｖθ５＝（θ５ｅ−θ５OLD ）／Ｔ（ｔ−Δｔ） …（２２）
Ｖθ６＝（θ６ｅ−θ６OLD ）／Ｔ（ｔ−Δｔ） …（２３）
次に，ステップＳ８において，前記制御装置１は，前記ステップＳ７で算出された速度Ｖθ４，Ｖθ５，Ｖθ６に基づいて，次の補間点の前記駆動軸３４〜３６の角度θ４NOW，θ５NOW ，θ６NOW を算出する。ここに，角度θ４NOW ，θ５NOW ，θ６NOW は，以下の（３１）式〜（３３）式により算出可能である。
θ４NOW ＝θ４OLD ＋Ｖθ４・Δｔ …（３１）
θ５NOW ＝θ５OLD ＋Ｖθ５・Δｔ …（３２）
θ６NOW ＝θ６OLD ＋Ｖθ６・Δｔ …（３３）
（ステップＳ９〜Ｓ１０）
そして，ステップＳ９において，前記制御装置１は，前記ステップＳ８で算出された次の補間点の角度θ４NOW ，θ５NOW ，θ６NOW と，前記ステップＳ５で算出された次の補間点の位置座標ＸＹＺ（ｔ）とに基づいて，前記第２関節駆動系の駆動軸３１〜３３各々の次の補間点における角度θ１NOW ，θ２NOW ，θ３NOW を算出する。
【００３５】
ここで算出される前記駆動軸３１〜３３の角度θ１NOW ，θ２NOW ，θ３NOW は，前記ステップＳ５で算出された次の補間点の位置座標ＸＹＺ（ｔ）を維持しているため，前記エンドエフェクタ２の位置及び移動速度は，前記ステップＳ６４による駆動制御時における前記エンドエフェクタ２の位置及び移動速度を維持したものとなる。
なお，前記制御装置１は，ステップＳ１０において，前記駆動軸３４〜３６各々の角度θ４NOW ，θ５NOW ，θ６NOW を，次の制御周期で使用するためにθ４OLD ，θ５OLD ，θ６OLD として記憶する（θ４OLD ＝θ４NOW ，θ５OLD ＝θ５NOW ，θ６OLD ＝θ６NOW ）。
（ステップＳ１１〜Ｓ１２）
その後，ステップＳ１１において，前記制御装置１は，前記ステップＳ８で算出された次の補間点における前記駆動軸３４〜３６の角度θ４NOW ，θ５NOW ，θ６NOW と，前記ステップＳ９で算出された次の補間点における前記駆動軸３１〜３３の角度θ１NOW ，θ２NOW ，θ３NOW とに基づいて前記駆動軸３１〜３６を駆動させる。このように，当該ロボット制御処理では，前記例外条件が充足した後は，前記ステップＳ６４における駆動制御に代えて，前記ステップＳ１１における駆動制御が実行される。
【００３６】
なお，前記制御装置１は，続くステップＳ１２において，前記エンドエフェクタ２が前記作業終了点Ｑ２に到着したか否かを判断し，該エンドエフェクタ２が前記作業終了点Ｑ２に到着するまで（Ｓ１２のＮｏ側），前記ステップＳ４以後の処理を繰り返し実行する。一方，前記エンドエフェクタ２が前記作業終了点Ｑ２に到着すると（Ｓ１２のＹｅｓ側），当該ロボット制御処理は終了される。
【００３７】
以上説明したように，本発明の実施の形態に係る前記制御装置１は，前記例外条件が充足したと判定された後，前記エンドエフェクタ２が前記作業終了点Ｑ２に到達するまでの間，前記第１関節駆動系の駆動軸３４〜３６各々を前記作業終了点Ｑ２の角度を目標として線形に変化させるための該第１関節駆動系の駆動軸３４〜３６各々の角度を算出すると共に，該算出された第１関節駆動系の駆動軸３４〜３６各々の角度と前記ステップＳ５によって算出された補間点における前記エンドエフェクタ２の位置とに基づいて前記第２関節駆動系の駆動軸３１〜３３各々の角度を算出し，該算出結果に従って前記第１関節駆動系及び前記第２関節駆動系の駆動軸各々を駆動させている。ここに，係る処理を実行するときの前記制御装置１が例外動作手段に相当する。
【００３８】
したがって，前記ステップＳ５で算出された補間点における前記エンドエフェクタ２の位置及び移動速度を維持しつつ，前記第１関節駆動系の駆動軸３４〜３６各々の角度を前記作業終了点Ｑ２の角度に確実に到達させることができる。これにより，例えば前記作業終了点Ｑ２から次の教示点に向けて動作する場合に前記駆動軸３４〜３６が作動範囲を超えることを防止することができる。
【００３９】
特に，前記制御装置１は，前記作業開始点Ｑ１及び前記作業終了点Ｑ２の前記駆動軸３５の角度の符号が異符号であり，前記第１関節駆動系の特異姿勢（駆動軸３５の角度が０度）を経なければ前記駆動軸３５の角度を前記作業終了点Ｑ２の角度に到達させることができない場合に好適である。以下，この点について図４及び図５を参照しつつ説明する。
ここに，図４及び図５は，前記作業開始点Ｑ１及び前記作業終了点Ｑ２における前記駆動軸３５の符号が異符号である場合の動作例を示すものであって，図４は従来例を示すものであり，図５は前記ロボット制御処理（図３）の実行結果を示すものである。具体的に，前記作業開始点Ｑ１における前記駆動軸３５の角度θ５ｓが＋６５度，前記作業終了点Ｑ２における前記駆動軸３５の角度θ５ｅが−６５度である。
【００４０】
図４に示すように，通常は，前記作業開始点Ｑ１及び前記作業終了点Ｑ２における前記駆動軸３５の角度θ５ｓ，θ５ｅの符号が異符号である場合，前記作業開始点Ｑ１における前記駆動軸３５の符号が維持された解のまま前記エンドエフェクタ２が移動するため，前記駆動軸３４〜３６各々が前記作業終了点Ｑ２における角度に到達しない。そのため，例えば前記作業終了点Ｑ２から次の教示点に向けて動作する場合に前記駆動軸３４〜３６が作動範囲を逸脱するおそれがある。また，前記駆動軸３５が０度に近づくことや前記駆動軸３４，３６の速度が所定値以上となることを条件に前記第１関節駆動系の特異姿勢を検出し，異なる解に移行させる構成であっても，図４に示すように前記駆動軸３５が０度に近づかず，前記駆動軸３４，３６の速度が所定値以上とならない場合には，前記第１関節駆動系の特異姿勢を検出することができない。
【００４１】
これに対し，図５に示すように，前記ロボット制御処理（図３）が実行される場合には，前記例外条件が充足したと判定されると，その時点Ｌ１からは前記駆動軸３４〜３６が前記作業終了点Ｑ２の角度に向けて線形に変化するように前記駆動軸３１〜３６が駆動されるため，該駆動軸３４〜３６各々を前記作業開始点Ｑ２における角度に確実に到達させることができる。
【００４２】
もちろん，前記作業開始点Ｑ１及び前記作業終了点Ｑ２における前記駆動軸３５の符号が同符号である場合であっても同様に，前記例外条件が充足した時点から，前記駆動軸３４〜３６が前記作業終了点Ｑ２における角度に向けて線形に変化するため，該駆動軸３４〜３６各々を前記作業開始点Ｑ２における角度に確実に到達させることができる。
【実施例１】
【００４３】
本実施例１では，前記ロボット制御処理（図３参照）における前記例外条件の他の例について説明する。
具体的に，本実施例１では，前記制御装置１が，前記ステップＳ６１において，前記駆動軸３４〜３６の少なくとも一つの駆動軸の次の補間点における角度が予め設定された作動範囲を超える場合に，前記例外条件が充足したと判定する。例えば，前記制御装置１は，予め設定された前記駆動軸３４〜３６各々のプラス側の作動範囲がθ４+L ，θ５+L ，θ６+L ，前記駆動軸３４〜３６各々のマイナス側の作動範囲がθ４-L ，θ５-L ，θ６-L であるとき，以下の（４１）〜（４６）の少なくとも一つが成立した場合に前記例外条件が充足したと判定する。なお，前記駆動軸３４〜３６各々の作動範囲とは，例えば前記駆動軸３４〜３６の駆動モータの許容動作範囲である。
θ４（ｔ）＞θ４+L …（４１）
θ４（ｔ）＜θ４-L …（４２）
θ５（ｔ）＞θ５+L …（４３）
θ５（ｔ）＜θ５-L …（４４）
θ６（ｔ）＞θ６+L …（４５）
θ６（ｔ）＜θ６-L …（４６）
ここに，図６及び図７は，前記作業開始点Ｑ１及び前記作業終了点Ｑ２における前記駆動軸３５の符号が異符号である場合の動作例を示すものであって，図６は従来例を示すものであり，図７は本実施例１に係るロボット制御処理の実行結果を示すものである。具体的に，前記作業開始点Ｑ１における前記駆動軸３５の角度θ５ｓが−１０度，前記作業終了点Ｑ２における前記駆動軸３５の角度θ５ｅが＋１０度である。なお，前記駆動軸３４〜３６の角度の作動範囲は−１８０度〜＋１８０度であるものとする。
【００４４】
図６に示すように，前記作業開始点Ｑ１及び前記作業終了点Ｑ２における前記駆動軸３５の符号が異符号であるにもかかわらず，前記作業開始点Ｑ１における前記駆動軸３５の符号が維持された解のまま前記エンドエフェクタ２が移動すると，前記駆動軸３４，３６の角度が該駆動軸３４，３６の作動範囲（±１８０度）を超えて異常停止するおそれがある。
【００４５】
これに対し，図７に示すように，本実施例１に係るロボット制御処理が実行される場合には，前記駆動軸３４〜３６の少なくとも一つの駆動軸の次の補間点における角度が予め設定された作動範囲を超えるという前記例外条件が充足したと判定されると，その時点Ｌ２からは前記駆動軸３４〜３６が前記作業終了点Ｑ２の角度に向けて線形に変化するように前記駆動軸３１〜３６が駆動される。そして，一般に前記制御装置１では前記作業終了点Ｑ２が前記駆動軸３４〜３６の角度の作動範囲内でのみ設定可能である。そのため，前記駆動軸３４〜３６各々を許容範囲内で駆動させつつ前記作業開始点Ｑ２の角度に確実に到達させることができる。もちろん，前記作業開始点Ｑ１及び前記作業終了点Ｑ２における前記駆動軸３５の符号が同符号である場合であっても同様である。
【００４６】
ところで，図７に示す動作例では，前記駆動軸３４〜３６各々の角度が，前記作業終了点Ｑ２よりも少し前の時点で該作業終了点Ｑ２の近傍に到達し，その後，前記作業終了点Ｑ２における角度に向けて緩やかな速度で動作している。このように，前記駆動軸３４〜３６各々の速度を少しずつ落として前記作業終了点Ｑ２に緩やかに停止させれば，停止時の振動などを抑制することができる。
【００４７】
特に，このとき前記制御装置１は，前記駆動軸３４〜３６各々の速度を調節して該駆動軸３４〜３６各々を同時に前記作業終了位置Ｑ２における角度に到着させて停止させることが望ましい。これにより，前記駆動軸３４〜３６各々の駆動モータが個別に停止する場合に比べて該駆動モータの停止時に生じる振動などを抑制することができる。
さらに，前記例外条件が充足する時点Ｌ２における前記駆動軸３４〜３６各々の線形動作開始時の速度が緩やかになるように該駆動軸３４〜３６の角度を補正することも考えられる。例えば図８は，前記例外条件が充足する前後の前記駆動軸３６の角度の変化例を示している。図８に実線で示すように，前記駆動軸３６の角度が１８０度まで増加した後，急にマイナス方向に向けて変化すると，該駆動軸３６に急激なマイナス方向の加速度が生じ，振動などの原因となる。そこで，図８に破線で示すように，前記駆動軸３６の作動範囲の上限値１８０に到達することを事前に推定し，その時点から前記駆動軸３６の角度が緩やかにマイナス方向に変化するように，該駆動軸３６の角度を補正することが考えられる。これにより，前記例外条件が充足した直後に前記駆動軸３６の急激な加速度が生じないため，振動などを防止することができる。もちろん，前記駆動軸３５，３６についても同様である。
【００４８】
また，このように前記駆動軸３４〜３６の急激な停止や急激な加速度を防止するための制御手法は，前記実施の形態や後述の実施例２などの他の例外条件を用いる場合にも同様に適用することができる。
【実施例２】
【００４９】
本実施例２では，前記ロボット制御処理（図３参照）における前記例外条件のさらに他の例について説明する。
ここに，図９は，本実施例２に係るロボット制御処理の手順の一例を示すフローチャートである。なお，前記ロボット制御処理（図３参照）と同様の処理手順については同じ符号を付してその説明を省略する。
【００５０】
具体的に，図９に示すように，本実施例２に係るロボット制御処理では，前記ステップＳ２〜Ｓ４，Ｓ６１（図３参照）に代えて後述のステップＳ２１，Ｓ３１，Ｓ４１，Ｓ６１１が実行される。
（ステップＳ２１，Ｓ３１，Ｓ４１）
ステップＳ２１において，前記制御装置１は，前記作業終了点Ｑ２における前記駆動軸３４〜３６の角度θ４ｅ，θ５ｅ，θ６ｅと共に，前記作業開始点Ｑ１における前記駆動軸３４〜３６の角度θ４ｓ，θ５ｓ，θ６ｓを算出する。
【００５１】
また，前記制御装置１は，当該ステップＳ２１において，前記駆動軸３４〜３６を前記作業開始点Ｑ１から前記作業終了点Ｑ２まで一定速度で線形に動作させたときの単位時間（１ｍｓｅｃ）あたりの前記駆動軸３４〜３６の角度変動Δθ４，Δθ５，Δθ６も算出する。このとき，角度変動Δθ４，Δθ５，Δθ６は，前記作業開始点Ｑ１における前記エンドエフェクタ２の残りの移動時間をＴ（ｓ）とすると，以下の（５１）〜（５３）で
求められる。
Δθ４＝（θ４ｅ−θ４ｓ）／Ｔ（ｓ） …（５１）
Δθ５＝（θ５ｅ−θ５ｓ）／Ｔ（ｓ） …（５２）
Δθ６＝（θ６ｅ−θ６ｓ）／Ｔ（ｓ） …（５３）
次に，ステップＳ３１において，前記制御装置１は，制御時間ｔ及び制御開始フラグＦを０に初期設定すると共に，前記駆動軸３４〜３６に対応する基準角度θｋ（ｔ）＝｛θ４ｋ，θ５ｋ，θ６ｋ｝の初期設定を行う。前記基準角度θｋ（ｔ）は，後述のステップＳ６１１において例外条件の判断で参照されるものであって，その初期値θｋ（０）は，｛θ４ｋ，θ５ｋ，θ６ｋ｝＝｛θ４ｓ，θ５ｓ，θ６ｓ｝に設定される。
【００５２】
また，前記制御装置１は，ステップＳ４１において，前記制御装置１は前記制御時間ｔを更新し（ｔ＝ｔ＋Δｔ），前記基準角度θｋ（ｔ）を更新する。具体的に基準角度θｋ（ｔ）は，θ４ｋ＝θ４ｋ＋Δθ４・Δｔ，θ５ｋ＝θ５ｋ＋Δθ５・Δｔ，θ６ｋ＝θ６ｋ＋Δθ６・Δｔとなる。
即ち，前記基準角度θｋ（ｔ）は，前記作業開始点Ｑ１及び前記作業終了点Ｑ２に基づいて定まるものであって，前記作業開始点Ｑ１及び前記作業終了点Ｑ２を結ぶ直線上の値に随時更新される。なお，前記基準角度θｋ（ｔ）が前記作業開始点Ｑ１や前記作業終了点Ｑ２における前記駆動軸３４〜３６の角度であることも他の実施例として考えられる。
（ステップＳ６１１）
そして，本実施例２に係るロボット制御処理では，ステップＳ６１１の例外条件の判定処理において，前記制御装置１は，次の補間点における前記第１関節駆動系の駆動軸３４〜３６の角度θ４（ｔ），θ５（ｔ），θ６（ｔ）の少なくとも一つと前記基準角度θ４ｋ，θ５ｋ，θ６ｋとの離間角度が予め設定された所定角度θ４ｗ，θ５ｗ，θ６ｗを超えるか否かを判断する。前記所定角度θ４ｗ，θ５ｗ，θ６ｗは，前記駆動軸３４〜３６の角度θ４（ｔ），θ５（ｔ），θ６（ｔ）が前記基準角度θ４ｋ，θ５ｋ，θ６ｋから離れすぎて，該駆動軸３４〜３６を前記作業終了点Ｑ２に到達させるために該駆動軸３４〜３６の角度の急激な変化を伴う可能性が高いことを事前に推定するために予め設定されたものである。例えば，前記所定角度θ４ｗ，θ５ｗ，θ６ｗ各々は，９０度，１８０度，９０度に設定しておくことが考えられる。
【００５３】
例えば，前記制御装置１は，下記の（６１）式〜（６３）式のいずれかが成立する場合に前記例外条件が充足したと判定する。
｜θ４（ｔ）−θ４ｋ｜＞θ４ｗ …（６１）
｜θ５（ｔ）−θ５ｋ｜＞θ５ｗ …（６２）
｜θ６（ｔ）−θ６ｋ｜＞θ６ｗ …（６３）
ここに，図１０は，前記作業開始点Ｑ１及び前記作業終了点Ｑ２における前記駆動軸３５の符号が異符号である場合の動作例を示すものであって，本実施例２に係る前記ロボット制御処理の実行結果を示すものである。具体的に，前記作業開始点Ｑ１における前記駆動軸３５の角度θ５ｓが＋１０度，前記作業終了点Ｑ２における前記駆動軸３５の角度θ５ｅが−１０度である。また，ここでは前記所定角度θ６ｗが９０度に設定されているものとし，前記駆動軸３６の角度θ６（ｔ）が前記基準角度θ６ｋ（ｔ）から９０度離れる場合を例に挙げて説明する。もちろん，前記駆動軸３４，３５についても同様である。
【００５４】
図１０に示すように，本実施例２に係るロボット制御処理が実行される場合には，前記駆動軸３６の次の補間点の角度θ６（ｔ）が前記基準角度θ６ｋ（ｔ）から９０度以上離れると判断された時点Ｌ３から，前記駆動軸３４〜３６が前記作業終了点Ｑ２の角度に向けて線形に変化するように前記駆動軸３１〜３６が駆動されるため，該駆動軸３４〜３６各々を前記作業開始点Ｑ２における角度に確実に到達させることができる。特に，本実施例２に係るロボット制御処理によれば，前記駆動軸３４〜３６各々の角度が前記基準角度θｋ（ｔ）から離れ過ぎる前の早期の段階で，該駆動軸３４〜３６を前記作業終了点Ｑ２に向けて線形動作させることができ，前記駆動軸３４〜３６の急激な角度変動を未然に防止することができる。
【００５５】
ところで，前記制御装置１は，前記実施の形態，前記実施例１，当該実施例２で説明した複数の前記例外条件の少なくとも一つの充足の有無を判断するものであればよいが，複数の前記例外条件の充足の有無を同時に監視しておき，いずれか一つの例外条件が充足した場合に，前記ステップＳ７以後の線形動作処理を実行することも考えられる。これにより，前記実施の形態，前記実施例１，当該実施例２で得られる効果を共に奏する構成を具現することができる。
【実施例３】
【００５６】
本実施例３では，前記制御装置１が，前記エンドエフェクタ２が加工対象物の加工線に倣って移動するように前記多関節型ロボット３を制御する倣い制御を実行する構成を考える。この場合，前記多関節型ロボット３には，前記エンドエフェクタ２と前記加工線とのズレ量ΔＸＹＺαβγ（ｔ）を検出するセンサ（ズレ量検出手段に相当）が設けられる。
ここに，各時点において前記センサで検出される前記ズレ量ΔＸＹＺαβγ（ｔ）は，前記経路方程式Ｐ（ｔ）により算出されたその時点における補間点を基準として表した値である。そして，前記制御装置１は，前記センサによる検出結果に基づいて前記エンドエフェクタ２を前記加工線に倣って移動させる前記倣い制御を実行する。ここに，係る倣い制御を実行するときの前記制御装置１が倣い制御手段に相当する。
【００５７】
具体的に，前記制御装置１は，前記ロボット制御処理（図３参照）のステップＳ５において次の補間点を算出する際，現在前記センサによって検出されているズレ量ΔＸＹＺαβγ（ｔ）を前記経路方程式Ｐ（ｔ）に加算する。また，前記制御装置１は，その加算後の補間点の位置・姿勢Ｘ’Ｙ’Ｚ’α’β’γ’（ｔ）から逆運動学問題の解を求めることにより，次の補間点における前記駆動軸３１〜３６各々の角度θ１’（ｔ）〜θ６’（ｔ）を算出する。そして，前記例外条件が充足していない場合，前記制御装置１は，前記ステップＳ６４において，前記ズレ量ΔＸＹＺαβγ（ｔ）で修正された後の前記角度θ１’（ｔ）〜θ６’（ｔ）に基づいて前記駆動軸３１〜３６を駆動する。
【００５８】
また，前記制御装置１は，前記ステップＳ５において，前記作業終了点Ｑ２を，該作業終了点Ｑ２に前記ズレ量ΔＸＹＺαβγ（ｔ）を加算した値に更新する。このとき，更新後の前記作業終了点Ｑ２における前記駆動軸３４〜３６各々の角度をθ４ｅ（ｔ），θ５ｅ（ｔ），θ６ｅ（ｔ）とする。ここに，係る処理を実行するときの前記制御装置１が作業終了点更新手段に相当する。
【００５９】
これにより，前記例外条件の判断処理では，前記更新後の前記作業終了点Ｑ２に基づいて前記例外条件の充足の有無が判断される。例えば，前記実施の形態における前記ステップＳ６１の例外条件の判定指標となる上記（１１）式〜（１３）式は，以下の（１１’）式〜（１３’）式となる。
｜（θ４ｅ（ｔ）−θ４（ｔ））／Ｔ（ｔ）｜＞θ４ｄ …（１１’）
｜（θ５ｅ（ｔ）−θ５（ｔ））／Ｔ（ｔ）｜＞θ５ｄ …（１２’）
｜（θ６ｅ（ｔ）−θ６（ｔ））／Ｔ（ｔ）｜＞θ６ｄ …（１３’）
また，前記ステップＳ７において前記速度Ｖθ４，Ｖθ５，Ｖθ６を算出するための上記（２１）式〜（２３）式は，以下の（２１’）式〜（２３’）式となる。即ち，前記ステップＳ１１における前記駆動処理では，前記更新後の前記作業終了点Ｑ２を目標として前記駆動軸３４〜３６の線形動作処理が行われる。
Ｖθ４＝（θ４ｅ（ｔ）−θ４OLD ）／Ｔ（ｔ−Δｔ） …（２１’）
Ｖθ５＝（θ５ｅ（ｔ）−θ５OLD ）／Ｔ（ｔ−Δｔ） …（２２’）
Ｖθ６＝（θ６ｅ（ｔ）−θ６OLD ）／Ｔ（ｔ−Δｔ） …（２３’）
これにより，前記センサで検出されるズレ量に基づいて前記倣い制御が実行される場合であっても，前記駆動軸３４〜３６各々の角度を前記倣い制御で修正された後の作業終了点Ｑ２に確実に到達させることができる。
具体的に，図１１（ａ）〜（ｃ）に示すように，前記エンドエフェクタ２が前記作業開始点Ｑ１から前記作業終了点Ｑ２に向けて移動し，前記倣い制御により前記ズレ量ΔＸＹＺαβγ（ｔ）が修正されるとき，前記作業終了点Ｑ２は，前記ズレ量ΔＸＹＺαβγ（ｔ）が加算されて作業終了点Ｑ２（ｔ）に更新される。これにより，前記駆動軸３４〜３６各々の角度を前記作業終了点Ｑ２（ｔ）に到達させることができる。
【実施例４】
【００６０】
ところで，前記ロボット制御処理（図３参照）において，前記エンドエフェクタ２の姿勢を完全に無視することは望ましくない。そこで，本実施例４では，前記エンドエフェクタ２の姿勢のうち予め定められた特定成分の変動を抑制することのできる構成について説明する。
具体的に，本実施例４において，前記制御装置１は，前記ロボット制御処理（図３参照）の前記ステップＳ８と前記ステップＳ９との間において，後述の特定成分抑制処理（図１２参照）を実行する。なお，前記ロボット制御処理については，前記実施の形態で説明したものに限らず，前記実施例１〜３のいずれかに記載された内容を採用すればよい。
【００６１】
以下，図１２のフローチャートに従って，前記制御装置１によって実行される特定成分抑制処理の手順の一例について説明する。
（ステップＳ８１）
まず，ステップＳ８１において，前記制御装置１は，次の補間点について，図１３に示すように，前記エンドエフェクタ２が移動する方向（進行方向）の軸をＸ軸とし，該Ｘ軸と重力方向との外積（Ｘ軸×重力方向）によって表される軸をＹ軸とし，該Ｘ軸と該Ｙ軸との外積（Ｘ軸×Ｙ軸）によって表される方向の軸をＺ軸とする作業座標系（以下，「溶接線座標系Σline」という）における姿勢角度を求める。ここに，前記溶接線座標系Σlineにおける前記エンドエフェクタ２の姿勢角度は，図１４〜図１６に示すように，前記Ｘ軸の軸周りの回転角であるトーチ傾斜角Ｒｘ，前記Ｙ軸の軸周りの回転角であるトーチ前進角Ｒｙ，前記Ｚ軸の軸周りの回転角であるトーチ回転角Ｒｚで表される。
【００６２】
具体的に，前記制御装置１は，前記ステップＳ５で算出された次の補間点の前記ベース座標系Σbaseにおける位置・姿勢ＸＹＺαβγ（ｔ）を，前記溶接線座標系Σlineにおける姿勢角度（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）に変換する。
例えば，前記ベース座標系Σbaseから前記溶接線座標系Σlineへの変換は，以下の（１０１）式〜（１０７）式に従って行われる。なお，前記溶接線座標系ΣlineにおけるＸ軸，Ｙ軸，Ｚ軸の単位ベクトルをＸｍ，Ｙｍ，Ｚｍとする。
【００６３】
【数１】
0005730179.tif 000002
【００６４】
【数２】
0005730179.tif 000003
【００６５】
Ｒｘ＝ａｔａｎ（Ｒｍ２３／Ｒｍ３３） …（１０５）
Ｒｙ＝ａｓｉｎ（Ｒｍ１３） …（１０６）
Ｒｚ＝ａｔａｎ（Ｒｍ１２／Ｒｍ１１） …（１０７）
（ステップＳ８２）
次に，ステップＳ８２において，前記制御装置１は，前回の制御周期における前記駆動軸３４〜３６の速度を基準として，該駆動軸３４〜３６各々の次の補間点における速度の候補を複数生成する。ここに，係る処理を実行するときの前記制御装置１が角度候補算出手段に相当する。
【００６６】
例えば，前記制御装置１は，前回の制御周期における前記駆動軸３４〜３６の速度をＶθ４OLD ，Ｖθ５OLD ，Ｖθ６OLD としたとき，前記駆動軸３４の速度の候補として，Ｖθ４ｍ＝Ｖθ４OLD −Ｆ，Ｖθ４ｎ＝Ｖθ４OLD ，Ｖθ４ｐ＝Ｖθ４OLD ＋Ｆを生成する。また，前記制御装置１は，同じく前記駆動軸３５の速度の候補として，Ｖθ５ｍ＝Ｖθ５OLD−Ｇ，Ｖθ５ｎ＝Ｖθ５OLD ，Ｖθ５ｐ＝Ｖθ５OLD ＋Ｇ，前記駆動軸３６の速度の候補として，Ｖθ６ｍ＝Ｖθ６OLD −Ｈ，Ｖθ６ｎ＝Ｖθ６OLD ，Ｖθ６ｐ＝Ｖθ６OLD ＋Ｈを生成する。ここに，前記定数Ｆ，Ｇ，Ｈ各々は，１サンプル周期あたりの加速度の限界値や振動などが起きない範囲の値で予め定めたものである。
【００６７】
このように前記駆動軸３４〜３６各々について３通りの速度の候補を生成すると，以下（２０１）〜（２２７）の合計２７通りの異なる組み合わせが生じる。
（２０１）：Ｖθ４ｍ，Ｖθ５ｍ，Ｖθ６ｍ
（２０２）：Ｖθ４ｍ，Ｖθ５ｍ，Ｖθ６ｎ
（２０３）：Ｖθ４ｍ，Ｖθ５ｍ，Ｖθ６ｐ
（２０４）：Ｖθ４ｍ，Ｖθ５ｎ，Ｖθ６ｍ
（２０５）：Ｖθ４ｍ，Ｖθ５ｎ，Ｖθ６ｎ
（２０６）：Ｖθ４ｍ，Ｖθ５ｎ，Ｖθ６ｐ
（２０７）：Ｖθ４ｍ，Ｖθ５ｐ，Ｖθ６ｍ
（２０８）：Ｖθ４ｍ，Ｖθ５ｐ，Ｖθ６ｎ
（２０９）：Ｖθ４ｍ，Ｖθ５ｐ，Ｖθ６ｐ
（２１０）：Ｖθ４ｎ，Ｖθ５ｍ，Ｖθ６ｍ
（２１１）：Ｖθ４ｎ，Ｖθ５ｍ，Ｖθ６ｎ
（２１２）：Ｖθ４ｎ，Ｖθ５ｍ，Ｖθ６ｐ
（２１３）：Ｖθ４ｎ，Ｖθ５ｎ，Ｖθ６ｍ
（２１４）：Ｖθ４ｎ，Ｖθ５ｎ，Ｖθ６ｎ
（２１５）：Ｖθ４ｎ，Ｖθ５ｎ，Ｖθ６ｐ
（２１６）：Ｖθ４ｎ，Ｖθ５ｐ，Ｖθ６ｍ
（２１７）：Ｖθ４ｎ，Ｖθ５ｐ，Ｖθ６ｎ
（２１８）：Ｖθ４ｎ，Ｖθ５ｐ，Ｖθ６ｐ
（２１９）：Ｖθ４ｐ，Ｖθ５ｍ，Ｖθ６ｍ
（２２０）：Ｖθ４ｐ，Ｖθ５ｍ，Ｖθ６ｎ
（２２１）：Ｖθ４ｐ，Ｖθ５ｍ，Ｖθ６ｐ
（２２２）：Ｖθ４ｐ，Ｖθ５ｎ，Ｖθ６ｍ
（２２３）：Ｖθ４ｐ，Ｖθ５ｎ，Ｖθ６ｎ
（２２４）：Ｖθ４ｐ，Ｖθ５ｎ，Ｖθ６ｐ
（２２５）：Ｖθ４ｐ，Ｖθ５ｐ，Ｖθ６ｍ
（２２６）：Ｖθ４ｐ，Ｖθ５ｐ，Ｖθ６ｎ
（２２７）：Ｖθ４ｐ，Ｖθ５ｐ，Ｖθ６ｐ
（ステップＳ８３〜Ｓ８４）
次に，ステップＳ８３において，前記制御装置１は，前記組み合わせ（２０１）〜（２２７）のうち，ランダムに或いは予め設定された規則に基づいて３つの組み合わせを選択
する。このとき選択された３つの速度の候補をＶθａ（ｔ），Ｖθｂ（ｔ），Ｖθｃ（ｔ）とする。
【００６８】
続いて，ステップＳ８４において，前記制御装置１は，３つの速度の候補Ｖθａ（ｔ），Ｖθｂ（ｔ），Ｖθｃ（ｔ）各々が採用された場合の次の補間点における前記駆動軸３４〜３６の角度の候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）を生成する。なお，ここでは説明の便宜上３つの速度の候補及び角度の候補を生成しているが，その候補の数は，前記制御装置１の演算性能に応じて，前記多関節型ロボット３に要求される動作速度が満たされる範囲で任意に設定すればよい。
【００６９】
例えば，最初のサンプル周期において，以下の（３０１）〜（３０３）に示すように前記速度の候補Ｖθａ（ｔ），Ｖθｂ（ｔ），Ｖθｃ（ｔ）として前記組み合わせ（２０１），（２０２），（２０３）が選択されると，以下の（３０４）〜（３０６）に示すように前記角度の候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）が生成される。
Ｖθａ（ｔ）＝Ｖθ４ａ（ｔ），Ｖθ５ａ（ｔ），Ｖθ６ａ（ｔ）＝Ｖθ４ｍ，Ｖθ５ｍ，Ｖθ６ｍ …（３０１）
Ｖθｂ（ｔ）＝Ｖθ４ｂ（ｔ），Ｖθ５ｂ（ｔ），Ｖθ６ｂ（ｔ）＝Ｖθ４ｍ，Ｖθ５ｍ，Ｖθ６ｎ …（３０２）
Ｖθｃ（ｔ）＝Ｖθ４ｃ（ｔ），Ｖθ５ｃ（ｔ），Ｖθ６ｃ（ｔ）＝Ｖθ４ｍ，Ｖθ５ｍ，Ｖθ６ｐ …（３０３）
θａ（ｔ）＝θ４ａ（ｔ），θ５ａ（ｔ），θ６ａ（ｔ）＝θ４OLD ＋Ｖθ４ａ（ｔ）・Δｔ，θ５OLD ＋Ｖθ５ａ（ｔ）・Δｔ，θ６OLD ＋Ｖθ６ａ（ｔ）・Δｔ …（３０４）
θｂ（ｔ）＝θ４ｂ（ｔ），θ５ｂ（ｔ），θ６ｂ（ｔ）＝θ４OLD ＋Ｖθ４ｂ（ｔ）・Δｔ，θ５OLD ＋Ｖθ５ｂ（ｔ）・Δｔ，θ６OLD ＋Ｖθ６ｂ（ｔ）・Δｔ …（３０５）
θｃ（ｔ）＝θ４ｃ（ｔ），θ５ｃ（ｔ），θ６ｃ（ｔ）＝θ４OLD ＋Ｖθ４ｃ（ｔ）・Δｔ，θ５OLD ＋Ｖθ５ｃ（ｔ）・Δｔ，θ６OLD ＋Ｖθ６ｃ（ｔ）・Δｔ …（３０６）
また，次のサンプル周期においても同様に，以下の（３１１）〜（３１３）に示すように前記速度の候補Ｖθａ（ｔ），Ｖθｂ（ｔ），Ｖθｃ（ｔ）として前記組み合わせ（２０３），（２０６），（２０９）が選択された場合も，前記角度の候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）が生成される。
Ｖθａ（ｔ）＝Ｖθ４ａ（ｔ），Ｖθ５ａ（ｔ），Ｖθ６ａ（ｔ）＝Ｖθ４ｍ，Ｖθ５ｍ，Ｖθ６ｐ …（３１１）
Ｖθｂ（ｔ）＝Ｖθ４ｂ（ｔ），Ｖθ５ｂ（ｔ），Ｖθ６ｂ（ｔ）＝Ｖθ４ｍ，Ｖθ５ｎ，Ｖθ６ｐ …（３１２）
Ｖθｃ（ｔ）＝Ｖθ４ｃ（ｔ），Ｖθ５ｃ（ｔ），Ｖθ６ｃ（ｔ）＝Ｖθ４ｍ，Ｖθ５ｐ，Ｖθ６ｐ …（３１３）
その後，さらに次のサンプル周期においても同様に，以下の（３２１）〜（３２３）に示すように前記速度の候補Ｖθａ（ｔ），Ｖθｂ（ｔ），Ｖθｃ（ｔ）として前記組み合わせ（２０９），（２１８），（２２７）が選択された場合も，前記角度の候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）が生成される。
Ｖθａ（ｔ）＝Ｖθ４ａ（ｔ），Ｖθ５ａ（ｔ），Ｖθ６ａ（ｔ）＝Ｖθ４ｍ，Ｖθ５ｐ，Ｖθ６ｐ …（３２１）
Ｖθｂ（ｔ）＝Ｖθ４ｂ（ｔ），Ｖθ５ｂ（ｔ），Ｖθ６ｂ（ｔ）＝Ｖθ４ｎ，Ｖθ５ｐ，Ｖθ６ｐ …（３２２）
Ｖθｃ（ｔ）＝Ｖθ４ｃ（ｔ），Ｖθ５ｃ（ｔ），Ｖθ６ｃ（ｔ）＝Ｖθ４ｐ，Ｖθ５
ｐ，Ｖθ６ｐ …（３２３）
（ステップＳ８５）
そして，ステップＳ８５において，前記制御装置１は，前記第１関節駆動系の駆動軸３４〜３６各々を前記作業終了点Ｑ２の角度を目標として線形動作させる場合より該目標に近い角度の範囲内であるか否かを判断する。即ち，前記ステップＳ８４で算出された角度の候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）各々が，前記ステップＳ８で算出された角度θ４NOW ，θ５NOW ，θ６NOW と前記ステップＳ２で算出された角度θ４ｅ，θ５ｅ，θ６ｅとの間の角度であるか否かが判断される。
【００７０】
ここで，前記角度の候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）各々が，前記角度θ４NOW，θ５NOW ，θ６NOW と前記角度θ４ｅ，θ５ｅ，θ６ｅとの間の角度であると判断された場合には（Ｓ８５のＹｅｓ側），処理はステップＳ８６に移行する。しかし，そうでないと判断された場合には（Ｓ８５のＮｏ側），処理は前記ステップＳ８３に戻されて異なる組み合わせが選択され，続く前記ステップＳ８４において前記候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）が再度生成される。
【００７１】
このように，前記駆動軸３４〜３６各々の角度の候補が，常に前記角度θ４NOW ，θ５NOW ，θ６NOW と前記角度θ４ｅ，θ５ｅ，θ６ｅとの間の角度の範囲に限定されるため，該駆動軸３４〜３６各々の角度を前記作業終了位置Ｑ２における角度に確実に到達させることができる。なお，前記作業終了位置Ｑ２よりも手前で前記駆動軸３４〜３６各々の角度が該作業終了位置Ｑ２における角度に到達することも考えられるが，この場合には，その後，前記駆動軸３４〜３６各々の角度θ４NOW ＝θ４ｅ，θ５NOW ＝θ５ｅ，θ６NOW ＝θ６ｅとすればよい。ここに，前記ステップＳ８２〜Ｓ８５により複数の角度の候補を算出するときの前記制御装置１が角度候補算出手段に相当する。
（ステップＳ８６）
次に，ステップＳ８６において，前記制御装置１は，前記ステップＳ８４で生成された角度の候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）のそれぞれについて，前記溶接線座標系Σlineから見た前記エンドエフェクタ２の姿勢角度を求める。
【００７２】
具体的には，前記角度の候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）について順運動学問題の解を求めることにより，該候補ごとに対応する前記ベース座標系Σｂａｓｅにおける位置・姿勢を算出した後，該ベース座標系Σbaseから溶接線座標系Σlineに変換することが考えられる。なお，ベース座標系Σbaseから溶接線座標系Σlineへの変換は，前記ステップＳ８１と同様に行えばよいため，ここでは説明を省略する。
（ステップＳ８７〜Ｓ８８）
そして，ステップＳ８７において，前記制御装置１は，前記角度の候補θａ（ｔ）〜θｃ（ｔ）のうち，前記ステップＳ８６で求められた姿勢角度が前記ステップＳ８１で求められた姿勢角度に最も近い角度の候補を選択する。このとき，前記制御装置１は，前記溶接線座標系Σlineから見た前記エンドエフェクタ２の姿勢角度の３つの成分（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）のうち，予め定められた一つ又は二つの特定成分の変動を抑制したときに，前記ステップＳ８１で求められた姿勢角度に最も近いものを選択する。ここに，係る処理を実行するときの前記制御装置１が角度選択手段に相当する。なお，前記特定成分は，前記操作部１３による操作に応じて，或いは前記多関節型ロボット３の加工内容に応じて前記制御装置１によって予め設定される。
【００７３】
具体的に，前記制御装置１は，前記角度の候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）それぞれを採用した場合における姿勢角度と前記ステップＳ８１で求められた姿勢角度との変動量Ｕａ，Ｕｂ，Ｕｃを下記の（３３１）〜（３３３）に従って求める。ここに，定数Ｄ，Ｅ，Ｆは，前記エンドエフェクタ２の姿勢角度の３つの成分（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）のいずれの成分の変動を抑制するかによって定められる数値である。例えば，トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙの変動を抑制する場合には，Ｄ＝１，Ｅ＝１，Ｆ＝０であることが考えられる。
Ua＝D・(Rx-Rxa)2 + E・(Ry-Rya)2 + F・(Rz-Rza)2 （３３１）
Ub＝D・(Rx-Rxb)2 + E・(Ry-Ryb)2 + F・(Rz-Rzb)2 （３３２）
Uc＝D・(Rx-Rxc)2 + E・(Ry-Ryc)2 + F・(Rz-Rzc)2 （３３３）
そして，前記制御装置１は，前記変動量Ｕａ，Ｕｂ，Ｕｃのうちその値が最も小さいもの，即ち前記ステップＳ８１で求められた姿勢角度との間で前記特定成分の変動量が最も小さいものに対応する前記角度の候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）を選択する。
【００７４】
その後，ステップＳ８８において，前記制御装置１は，前記ステップＳ８で算出されたθ４NOW ，θ５NOW ，θ６NOW を，前記ステップＳ８７で選択された前記角度の候補θａ（ｔ），θｂ（ｔ），θｃ（ｔ）のいずれかに変更する。例えば，Ｕａ＜Ｕｂ＜Ｕｃであれば，変動量Ｕａに対応する前記角度の候補θａ（ｔ）が選択され，θ４NOW ＝θ４ａ（ｔ），θ５NOW ＝θ５ａ（ｔ），θ６NOW ＝θ６ａ（ｔ）となる。
【００７５】
これにより，その後の前記ステップＳ９〜Ｓ１１により前記駆動軸３１〜３６が駆動されたときの前記エンドエフェクタ２の前記特定成分の変動量を抑制することができる。
ここに，図１７は，前記実施の形態における前記ロボット制御処理（図３）において，前記特定成分抑制処理を実行した場合の前記駆動軸３４〜３６の駆動軌跡を示している。
なお，図１７における破線は，前記特定成分抑制処理を実行せずに前記駆動軸３４〜３６を線形動作させた場合の駆動軌跡を示している。
【００７６】
ここでは，前記多関節型ロボット３の加工内容が溶接であって，前記エンドエフェクタ２の前記溶接線座標系Σlineにおける姿勢角度（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）のうちトーチ傾斜角Ｒｘ，トーチ前進角Ｒｙの変動を抑制することが特に重要であり，該トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙが前記特定成分として予め設定されているものとする。
【００７７】
この場合，前記特定成分抑制処理（図１２参照）が実行されることにより，図１７に示すように，前記例外条件が充足した後，前記駆動軸３４〜３６は，前記作業終了点Ｑ２を目標として概ね線形動作することになるが，その経路は，前記エンドエフェクタ２の前記溶接線座標系Σlineにおけるトーチ傾斜角Ｒｘ，トーチ前進角Ｒｙを抑制するために微調整される。
【００７８】
これにより，図１８に示すように，溶接作業にとって重要でない（重み付けが小さい）トーチ回転角Ｒｚの変動を犠牲にして，溶接作業に重要な（重み付けが大きい）トーチ傾斜角Ｒｘとトーチ前進角Ｒｙとの変動を抑制することができる。なお，図１８における破線は，前記特定成分抑制処理を実行せずに前記駆動軸３４〜３６を線形動作させた場合の変動量を示している。
【００７９】
また，図１９は，前記実施例２における前記ロボット制御処理（図９）において，前記特定成分抑制処理を実行した場合の前記駆動軸３４〜３６の駆動軌跡を示している。なお，図１９における破線は，前記特定成分抑制処理を実行せずに前記駆動軸３４〜３６を線形動作させた場合の駆動軌跡を示している。
ここでも，前記多関節型ロボット３の加工内容が溶接であって，前記トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙが前記特定成分として予め設定されているものとする。
【００８０】
この場合，前記特定成分抑制処理（図１２参照）が実行されることにより，図１９に示すように，前記例外条件が充足した後，前記駆動軸３４〜３６は，前記作業終了点Ｑ２を目標として概ね線形動作することになるが，その経路は，前記エンドエフェクタ２の前記溶接線座標系Σlineにおけるトーチ傾斜角Ｒｘ，トーチ前進角Ｒｙを抑制するために微調整される。
【００８１】
これにより，図２０〜図２２に示すように，溶接作業にとって重要でない（重み付けが小さい）トーチ回転角Ｒｚの変動を犠牲にして，溶接作業に重要な（重み付けが大きい）トーチ傾斜角Ｒｘとトーチ前進角Ｒｙとの変動を抑制することができる。なお，図２０〜２２における破線は，前記特定成分抑制処理を実行せずに前記駆動軸３４〜３６を線形動作させた場合の変動量を示している。
【実施例５】
【００８２】
ところで，当該ステップＳ６４において，前記制御装置１は，前記第１関節駆動系が特異姿勢（特異点）に近づいたか否かを検出し，特異姿勢に近づいた場合には，前記ステップＳ５で算出される逆運動学問題の解のうち他方の解に滑らかに移行させるための処理を実行するものであってもよい。
さらに，前記逆運動学問題の一方の解から他方の解に移行するとき，前記実施例４のように，前記エンドエフェクタ２の前記溶接線座標系Σlineにおける姿勢角度（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）のうち予め設定された一つ又は二つの特定成分の変動を抑制するように複数の候補から一つの候補を選択しながら，前記駆動軸３４〜３６の角度を，前記逆運動学問題の一方の解から他方の解に滑らかに移行させることが望ましい。
【００８３】
上述した実施形態のような特異姿勢を回避する処理（回避処理）を行っている際は、この回避処理を行っていることを示す、即ち、多関節型ロボット３が特異姿勢を回避中であることを外部に報知する手段を設けることが好ましい。
具体的には、図２５に示すように、特異姿勢を回避中であることを示す「特異点回避中」を、操作部１３（教示ペンダント）の表示画面Ｍに表示する。なお、特異姿勢を回避中であることを示す表示は、「特異点回避中」という表現に限定されず、特異姿勢を回避していることが分かる表現であればどのような表現であってもよい。
【００８４】
このように、特異点回避中であることを表示することによって、多関節型ロボット３が特異点回避中の状態であるか、特異点回避中ではない状態であるかをオペレータが溶接中に把握することができる。そのため、例えば、作業対象物（材料）の溶接後に品質検査を行う場合、特異点回避中の状態で溶接した箇所を注視して検査することが可能となる。
また、例えば、特異点回避中の状態における多関節型ロボット３の全体の動きを、オペレータが目視などで監視することもでき、この監視に基づいて、次の溶接作業などで、再度、多関節型ロボット３を動かすときに、多関節型ロボット３の全体の動作を見直すこともできる。
【００８５】
なお、「特異点回避中」の表示の他に、処理プログラムが分かるように処理プログラムの内容（例えば、再生中プログラムの番号）を表示画面Ｍに表示したり、処理プログラムを実行している時間（再生時間）、エンドエフェクタの動き（例えば、直線移動）を表示画面Ｍに表示してもよい。さらに、予め教示されたトーチの情報（例えば、トーチ傾斜角、トーチ前進角、トーチ回転角など）と、現在のトーチの情報とを表示画面Ｍに表示してもよい。
【００８６】
これによれば、「特異点回避中」における多関節型ロボット３の詳しい状態を把握することができる。
上述した例では、特異点回避の処理を行っていることを表示画面Ｍに表示させているが、これに代えて、特異点回避を行っていることをスピーカ等によって音で知らせてもよいし、ランプなどの点灯や点滅など光で知らせてもよい。特異点回避を行っていることは、多関節型ロボットの教示ペンダント１３とは別の外部の表示装置に表示してもよい。
【００８７】
また、「特異点回避中」の時間を制御部１０の記憶部１２などにログとして記憶してもよい。そして、例えば、溶接終了後に操作部１３（教示ペンダント）の表示画面Ｍに特異点回避中の時間履歴を表示し、オペレータなどが特異点回避中の時間履歴を確認できるようにしてもよい。
【符号の説明】
【００８８】
１ ：多関節型ロボットの制御装置
１１：演算部
１２：記憶部
１３：操作部
２ ：エンドエフェクタ
３ ：多関節型ロボット
３１〜３６：駆動軸
【図１】
0005730179.tif 000004
【図２】
0005730179.tif 000005
【図３】
0005730179.tif 000006
【図４】
0005730179.tif 000007
【図５】
0005730179.tif 000008
【図６】
0005730179.tif 000009
【図７】
0005730179.tif 000010
【図８】
0005730179.tif 000011
【図９】
0005730179.tif 000012
【図１０】
0005730179.tif 000013
【図１１】
0005730179.tif 000014
【図１２】
0005730179.tif 000015
【図１３】
0005730179.tif 000016
【図１４】
0005730179.tif 000017
【図１５】
0005730179.tif 000018
【図１６】
0005730179.tif 000019
【図１７】
0005730179.tif 000020
【図１８】
0005730179.tif 000021
【図１９】
0005730179.tif 000022
【図２０】
0005730179.tif 000023
【図２１】
0005730179.tif 000024
【図２２】
0005730179.tif 000025
【図２３】
0005730179.tif 000026
【図２４】
0005730179.tif 000027
【図２５】
0005730179.tif 000028