知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】5726052
(24)【登録日】2015年4月10日
(45)【発行日】2015年5月27日
(54)【発明の名称】多関節型ロボットの制御装置，制御方法及び制御プログラム
(51)【国際特許分類】
B25J 9/10 20060101AFI20150507BHJP
B23K 9/12 20060101ALI20150507BHJP
【ＦＩ】
B25J9/10 A
B23K9/12 331Q
【請求項の数】17
【全頁数】47
(21)【出願番号】特願2011-254806(P2011-254806)
(22)【出願日】2011年11月22日
(65)【公開番号】特開2012-196753(P2012-196753A)
(43)【公開日】2012年10月18日
【審査請求日】2013年9月2日
(31)【優先権主張番号】特願2011-50450(P2011-50450)
(32)【優先日】2011年3月8日
(33)【優先権主張国】JP
(73)【特許権者】
【識別番号】000001199
【氏名又は名称】株式会社神戸製鋼所
(74)【代理人】
【識別番号】100061745
【弁理士】
【氏名又は名称】安田 敏雄
(74)【代理人】
【識別番号】100120341
【弁理士】
【氏名又は名称】安田 幹雄
(72)【発明者】
【氏名】大根 努
(72)【発明者】
【氏名】西村 利彦
【審査官】
谷治 和文
(56)【参考文献】
【文献】
特開昭６３−３０５４０７（ＪＰ，Ａ）
【文献】
特開２００９−１１３１７２（ＪＰ，Ａ）
(58)【調査した分野】（Int.Cl.，ＤＢ名）
Ｂ２５Ｊ ９／１０
Ｂ２５Ｊ １３／００
(57)【特許請求の範囲】
【請求項1】
作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた第２関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御装置であって，
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段と，
前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段と，
前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出手段と，
前記補間データ算出手段により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出手段により算出された前記駆動
軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段と，
を備えてなり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出手段が，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出するものである
ことを特徴とする多関節型ロボットの制御装置。
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段と，
前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段と，
前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出手段と，
前記補間データ算出手段により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出手段により算出された前記駆動
軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段と，
を備えてなり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出手段が，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出するものである
ことを特徴とする多関節型ロボットの制御装置。
【請求項2】
作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた第２関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御装置であって，
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段と，
前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段と，
前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出手段と，
前記補間データ算出手段により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段と，
を備えてなり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出手段が，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御装置。
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段と，
前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段と，
前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出手段と，
前記補間データ算出手段により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段と，
を備えてなり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出手段が，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御装置。
【請求項3】
作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた第２関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御装置であって，
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段と，
前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段と，
前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出手段と，
前記補間データ算出手段により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段と，
を備えてなり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出手段が，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が同符号である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が異符号である場合において，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置にお
ける該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御装置。
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段と，
前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段と，
前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出手段と，
前記補間データ算出手段により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出手段と，
前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段と，
を備えてなり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出手段が，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が同符号である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が異符号である場合において，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置にお
ける該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御装置。
【請求項4】
前記再算出手段が，
現在の前記補間点における前記第１関節駆動系の駆動軸各々の角度に基づいて，該駆動軸各々の角度が前記角度算出手段で算出された次の前記補間点における角度に追従し，且つ前記第１関節駆動系における両端の２つの駆動軸の次の前記補間点における速度が前記許容範囲内となる該駆動軸各々の角度の候補を複数算出する角度候補算出手段と，
前記角度候補算出手段により算出された角度の候補各々を採用したときの次の前記補間点の前記作業部の姿勢を前記作業座標系で示す姿勢データをそれぞれ算出する姿勢データ算出手段と，
前記姿勢データ算出手段により算出された複数の姿勢データのうち，前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して前記特定成分の変動が最も抑制された姿勢データを選択し，該姿勢データに対応する角度を前記駆動軸各々の角度として選択する角度選択手段と，
を含んでなる請求項１〜３のいずれかに記載の多関節型ロボットの制御装置。
現在の前記補間点における前記第１関節駆動系の駆動軸各々の角度に基づいて，該駆動軸各々の角度が前記角度算出手段で算出された次の前記補間点における角度に追従し，且つ前記第１関節駆動系における両端の２つの駆動軸の次の前記補間点における速度が前記許容範囲内となる該駆動軸各々の角度の候補を複数算出する角度候補算出手段と，
前記角度候補算出手段により算出された角度の候補各々を採用したときの次の前記補間点の前記作業部の姿勢を前記作業座標系で示す姿勢データをそれぞれ算出する姿勢データ算出手段と，
前記姿勢データ算出手段により算出された複数の姿勢データのうち，前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して前記特定成分の変動が最も抑制された姿勢データを選択し，該姿勢データに対応する角度を前記駆動軸各々の角度として選択する角度選択手段と，
を含んでなる請求項１〜３のいずれかに記載の多関節型ロボットの制御装置。
【請求項5】
前記再算出手段が，前記姿勢データ変換手段により変換された前記作業部の姿勢データのうち予め定められた重み付けが大きい前記特定成分から順に変動の抑制量が大きくなるように前記駆動軸の角度を再度算出するものである請求項１〜４のいずれかに記載の多関節型ロボットの制御装置。
【請求項6】
前記作業部がトーチであり，
前記姿勢データが，前記作業部の移動方向の軸をＸ軸，前記Ｘ軸と重力方向との外積によって表される軸をＹ軸，前記Ｘ軸と前記Ｙ軸との外積によって表される方向の軸をＺ軸，前記Ｘ軸の軸周りの回転角をトーチ傾斜角，前記Ｙ軸の軸周りの回転角をトーチ前進角，前記Ｚ軸の軸周りの回転角をトーチ回転角とする作業座標系で表されるものであって，前記特定成分が，前記トーチ傾斜角，前記トーチ前進角，前記トーチ回転角のいずれか一つ又は二つである請求項１〜５のいずれかに記載の多関節型ロボットの制御装置。
前記姿勢データが，前記作業部の移動方向の軸をＸ軸，前記Ｘ軸と重力方向との外積によって表される軸をＹ軸，前記Ｘ軸と前記Ｙ軸との外積によって表される方向の軸をＺ軸，前記Ｘ軸の軸周りの回転角をトーチ傾斜角，前記Ｙ軸の軸周りの回転角をトーチ前進角，前記Ｚ軸の軸周りの回転角をトーチ回転角とする作業座標系で表されるものであって，前記特定成分が，前記トーチ傾斜角，前記トーチ前進角，前記トーチ回転角のいずれか一つ又は二つである請求項１〜５のいずれかに記載の多関節型ロボットの制御装置。
【請求項7】
前記作業部が，先端に前記作業対象物に対して作業上の効果を与える一つの作業点を有するシングルトーチであり，
前記特定成分が，前記トーチ傾斜角及び前記トーチ前進角である請求項６に記載の多関節型ロボットの制御装置。
前記特定成分が，前記トーチ傾斜角及び前記トーチ前進角である請求項６に記載の多関節型ロボットの制御装置。
【請求項8】
前記作業部が，先端に前記作業対象物に対して作業上の効果を与える２つの作業点を有するタンデムトーチであり，
前記特定成分が，前記トーチ傾斜角及び前記トーチ回転角である請求項６に記載の多関節型ロボットの制御装置。
前記特定成分が，前記トーチ傾斜角及び前記トーチ回転角である請求項６に記載の多関節型ロボットの制御装置。
【請求項9】
前記駆動指示手段が，前記速度許容判別手段により前記許容範囲外の駆動軸があると判別された後は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，その後，前記角度算出手段により算出された前記第１関節駆動系の角度と前記再算出手段により算出された前記第１関節駆動系の角度との差分が所定値以下となったことを条件に，前記再算出手段により算出された前記駆動軸各々の角度に代えて前記角度算出手段により算出される前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させるものである請求項１〜８のいずれかに記載の多関節型ロボットの制
御装置。
御装置。
【請求項10】
前記駆動指示手段が，前記再算出手段により算出された前記駆動軸各々の角度に基づく前記多関節ロボットの駆動制御から前記角度算出手段により算出された前記駆動軸各々の角度に基づく前記多関節ロボットの駆動制御に切り替える際に，該駆動軸各々の角度を所定の変動範囲内で徐々に変動させるものである請求項９に記載の多関節型ロボットの制御装置。
【請求項11】
前記速度算出手段が，速度に代えて加速度を算出するものであり，前記再算出手段及び前記駆動指示手段が，速度に代えて加速度に基づいて判別するものである請求項１〜１０のいずれかに記載の多関節型ロボットの制御装置。
【請求項12】
作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた第２関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御方法であって，
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，
を実行するものであり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出するものである
ことを特徴とする多関節型ロボットの制御方法。
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，
を実行するものであり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出するものである
ことを特徴とする多関節型ロボットの制御方法。
【請求項13】
作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた第２関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御方法であって，
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，
を実行するものであり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御方法。
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，
を実行するものであり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御方法。
【請求項14】
作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた
第２関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御方法であって，
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，
を実行するものであり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が同符号である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が異符号である場合において，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御方法。
第２関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御方法であって，
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，
を実行するものであり、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が同符号である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が異符号である場合において，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御方法。
【請求項15】
作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた
第２関節駆動系とを有する多関節型ロボットを制御するコンピュータによって実行される多関節型ロボットの制御プログラムであって，
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，をコンピュータに実行させるものであって、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出するものである
ことを特徴とする多関節型ロボットの制御プログラム。
第２関節駆動系とを有する多関節型ロボットを制御するコンピュータによって実行される多関節型ロボットの制御プログラムであって，
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，をコンピュータに実行させるものであって、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出するものである
ことを特徴とする多関節型ロボットの制御プログラム。
【請求項16】
作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた第２関節駆動系とを有する多関節型ロボットを制御するコンピュータによって実行される多関節型ロボットの制御プログラムであって，
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記
第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，
をコンピュータに実行させるものであって、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御プログラム。
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記
第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，
をコンピュータに実行させるものであって、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御プログラム。
【請求項17】
作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた第２関節駆動系とを有する多関節型ロボットを制御するコンピュータによって実行される多関節型ロボットの制御プログラムであって，
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を
示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，
をコンピュータに実行させるものであって、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が同符号である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が異符号である場合において，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御プログラム。
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と，
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と，
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出工程と，
前記補間データ算出工程により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を
示す姿勢データに変換する姿勢データ変換工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出工程と，
前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と，
をコンピュータに実行させるものであって、
前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，
前記角度算出工程が，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が同符号である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，
前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が異符号である場合において，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記速度算出工程により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする多関節型ロボットの制御プログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は，多関節型ロボットの制御装置，制御方法及び制御プログラムに関し，特に，手首軸の各駆動軸の速度が許容範囲を超える場合に，手首の姿勢を維持しつつ各駆動軸の速度を許容範囲内に維持する技術に関するものである。
【背景技術】
【0002】
ここに，図２３に示すような多関節型ロボットの一種である６軸マニピュレータにおいて，基台に装着された腕軸を制御する関節軸を基台から順にＪ１軸，Ｊ２軸，Ｊ３軸とし，手首軸を制御する関節軸を腕部分から手首の先端に向かって順にＪ４軸，Ｊ５軸，Ｊ６軸とする。この場合，Ｊ１軸〜Ｊ６軸の各関節角θ1〜θ6が与えられたとき，順運動学問題の解を求めることによって，直交座標系でのエンドエフェクタ（手先の先端部分に取り付けた溶接器等の効果器）の位置のＸＹＺ座標と，エンドエフェクタの姿勢角α，β，γ（尚，姿勢角α，β，γは，オイラー角やロール・ピッチ・ヨー角などで表す）とを求めることができる。また逆に，エンドフェクタの位置のＸＹＺ座標と姿勢角α，β，γとが与えられたとき，逆運動学問題の解を求めることによって，Ｊ１軸〜Ｊ６軸の各関節角θ1〜θ6を求めることができる。
【0003】
ところで，図２４に示すように，Ｊ５軸の関節角θ5が０度付近において，エンドエフェクタを一定速度で動かそうとする場合に，Ｊ４軸の関節角θ4とＪ６軸の関節角θ6との角度が急激に変化するために，Ｊ４軸とＪ６軸とを高速で回転させなければならない。しかし，マニピュレータの各関節軸は回転速度の上限値を超えることができず，無理に上限値を超えて動作させた場合には，エンドエフェクタの経路が予定経路から外れてしまったり，エンドエフェクタが振動してしまう問題が生じる。このように多関節マニピュレータにおいて，エンドエフェクタの位置や姿勢角を僅かに動かしただけで，極めて大きな回転速度が要求される関節軸が発生し，動作不能になる場合がある。このような動作不能が発生する場合には，各関節軸の速度とエンドエフェクタの移動経路の精度との両立が困難であり，移動経路の精度を確保するために各関節軸の回転速度が要求される速度よりも低下してしまったり，安全装置が働いてマニピュレータの動作が停止してしまう問題がある。
【0004】
そこで，ある関節軸の速度が許容範囲を超える場合に，各手首軸に急激な変化を生じさせず制限内の速度に保ったまま動作させる方法が，特許文献１〜３に開示されている。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特開昭６２−１６２１０９号公報
【特許文献2】特開平６−３２４７３０号公報
【特許文献3】特開２００３−３００１８３号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
しかし，ある関節軸の速度が許容範囲を超える場合に，各手首軸に急激な変化を生じさせず制限内の速度に保ったまま動作させて，予定経路を通過させようとする場合には，エンドエフェクタに要求された（１）位置（２）移動速度（３）姿勢角度の３要素を同時に満たすことができないことが一般的に知られている。そのため，一般的なマニピュレータでは，（１）〜（３）の要素の中から１つか２つの要素のみを満たすようにマニピュレータを動作させる制御がなされている。
【0007】
例えば，特許文献１及び特許文献２には，エンドエフェクタの（１）の位置と（３）の姿勢角度とを維持する方法が記載されているが，（２）の移動速度を維持することができず移動速度が遅くなるため，エンドエフェクタに溶接器を備えたような場合には，本来要求される時間よりも長い時間をかけて作業線を通過してしまい，他の部分の溶接時間よりも長くなり，強度が低下する等の問題がある。さらに，移動速度が遅くなり溶接時間が長くなった箇所が必要以上に盛り上がり，次工程（例えば２層目の溶接工程など）に影響を与えるおそれもある。
【0008】
また，特許文献３には，エンドエフェクタの移動速度又は位置のいずれか一方を維持する方法が記載されているが，移動速度を維持する場合には位置を維持することができず，エンドエフェクタに溶接器や塗装器を備えたような場合には，本来予定された作業線からずれ，溶接や塗装が必要な場所に効果を及ぼせない問題がある。従って，本発明は上記事情に鑑みてなされたものであり，その目的とするところは，手首軸を所定の許容量を超えて急激に変化させる必要がある場合に，溶接や塗装作業に特に必要な手首先端の位置及び手首先端の移動速度と手首軸の許容速度とを維持し，かつ目的に応じた作業座標系における姿勢角度の少なくとも一つの特定成分の変動を抑制するようにした多関節型ロボットの制御装置，制御方法及び制御プログラムを提供することにある。
【課題を解決するための手段】
【0009】
上記目的を達成するために本発明は，作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた第２関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御装置に適用されるものであって，下記の（１１）〜（１６）を備えてなることを特徴として構成される。（１１）前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段。
（１２）前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段。
（１３）前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出手段。
（１４）前記補間データ算出手段により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段。
（１５）前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ，前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出手段。
（１６）前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段。
【0010】
本発明によれば，前記第１関節駆動系の駆動軸を前記許容範囲を超えて急激に変化させる必要がある場合でも，溶接や塗装作業に特に必要な前記作業部の位置及び手首先端の移動速度と前記第１関節駆動系の駆動軸の許容速度とを維持し，かつ目的に応じた作業座標系における姿勢角度の少なくとも一つの特定成分の変動を抑制することができる。また，前記作業開始位置から前記作業終了位置までの間に存在する特異点領域や特異点回避経路などを算出する場合には，事前計算における過大な負荷に対応することのできる演算装置（ＣＰＵ）を設ける必要がありコストアップ要因となるが，本発明によれば，現在の状態と次の状態とに応じてその都度駆動が制御すればよいため，演算装置に瞬間的に要求される処理負荷が軽減されており低コスト化を図ることができる。
【0011】
例えば，前記再算出手段が，現在の前記補間点における前記第１関節駆動系の駆動軸各々の角度に基づいて，該駆動軸各々の角度が前記角度算出手段で算出された次の前記補間点における角度に追従し，且つ前記第１関節駆動系における両端の２つの駆動軸の次の前記補間点における速度が前記許容範囲内となる該駆動軸各々の角度の候補を複数算出する角度候補算出手段と，前記角度候補算出手段により算出された角度の候補各々を採用したときの次の前記補間点の前記作業部の姿勢を前記作業座標系で示す姿勢データをそれぞれ算出する姿勢データ算出手段と，前記姿勢データ算出手段により算出された複数の姿勢データのうち，前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して前記特定成分の変動が最も抑制された姿勢データを選択し，該姿勢データに対応する角度を前記駆動軸各々の角度として選択する角度選択手段とを含んでなることが考えられる。
【0012】
これにより，前記作業部の位置・姿勢を動かした際に，前記作業部の作業目的に応じた適切な特定成分の変動を抑制することができる。ところで，前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，前記角度算出手段は，例えば前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出する。これにより，前記第１関節駆動系の中央に位置する駆動軸の角度が０°を通過しないように制御して特異点を回避することができる。
【0013】
また，前記角度算出手段は，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであってもよい。
【0014】
さらに，これらを組み合わせた構成も考えられる。即ち，前記角度算出手段が，前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が同符号である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が異符号である場合において，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることが望ましい。これにより，前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度を教示された通りに到着させることができる。
【0015】
また，前記再算出手段は，前記姿勢データ変換手段により変換された前記作業部の姿勢データのうち予め定められた重み付けが大きい前記特定成分から順に変動の抑制量が大きくなるように前記駆動軸の角度を再度算出するものであることが考えられる。これにより，前記重み付けを適宜設定しておくことにより，前記特定成分の変動の抑制量を適宜調整することが可能となる。
【0016】
本願発明のより具体的な構成としては，前記作業部がトーチであり，前記姿勢データが，前記作業部の移動方向の軸をＸ軸，前記Ｘ軸と重力方向との外積によって表される軸をＹ軸，前記Ｘ軸と前記Ｙ軸との外積によって表される方向の軸をＺ軸，前記Ｘ軸の軸周りの回転角をトーチ傾斜角，前記Ｙ軸の軸周りの回転角をトーチ前進角，前記Ｚ軸の軸周りの回転角をトーチ回転角とする作業座標系で表されるものであって，前記特定成分が，前記トーチ傾斜角，前記トーチ前進角，前記トーチ回転角のいずれか一つ又は二つであることが考えられる。図２５に示すように，前記手首先端部に備えられた前記作業部の姿勢について，直交座標系Σbase上で表される手首先端位置ＸＹＺと手首先端姿勢αβγから，前記作業部が移動方向の軸をＸ軸とする溶接線座標系Σline（作業座標系）上で表すことによって，前記作業部の姿勢を図２６〜図２８に示すように各軸に対する傾き成分として表すことができる。変換された溶接線座標系Σlineの座標軸周りの回転角は，図２６，図２７に示すようにＸline軸周りの回転角を右ネジ方向を正方向として前記トーチ傾斜角Ｒxとし，図２６，図２８に示すようにＹline軸周りの回転角を右ネジ方向を正方向として前記トーチ前進角Ｒyとし，Ｚline軸周りの回転角を右ネジ方向を正方向として前記トーチ回転角Ｒzとする。
【0017】
このとき，前記作業部が，先端に前記作業対象物に対して作業上の効果を与える一つの作業点を有するシングルトーチである場合には，前記特定成分が，前記トーチ傾斜角及び前記トーチ前進角であることが考えられる。これにより，作業点が一つであるシングルトーチの作業に重要な前記トーチ傾斜角と前記トーチ前進角との変動を抑制することができる。
【0018】
他方，前記作業部が，先端に前記作業対象物に対して作業上の効果を与える２つの作業点を有するタンデムトーチである場合には，前記特定成分が，前記トーチ傾斜角及び前記トーチ回転角であることが考えられる。これにより，作業点が二つであるタンデムトーチの作業に重要な前記トーチ傾斜角と前記トーチ回転角との変動を抑制することができる。さらに，前記駆動指示手段が，前記速度許容判別手段により前記許容範囲外の駆動軸があると判別された後は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，その後，前記角度算出手段により算出された前記第１関節駆動系の角度と前記再算出手段により算出された前記第１関節駆動系の角度との差分が所定値以下となったことを条件に，前記再算出手段により算出された前記駆動軸各々の角度に代えて前記角度算出手段により算出される前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させるものであることが考えられる。
【0019】
このとき，前記駆動指示手段は，前記再算出手段により算出された前記駆動軸各々の角度に基づく前記多関節ロボットの駆動制御から前記角度算出手段により算出された前記駆動軸各々の角度に基づく前記多関節ロボットの駆動制御に切り替える際に，該駆動軸各々の角度を所定の変動範囲内で徐々に変動させるものであることが考えられる。これにより，前記作業対象物の作業線に則して移動する前記作業線が振動して作業線に則して移動しなくなることを防止することができる。例えば，前記再算出手段により算出された角度と前記多関節ロボットの駆動制御から前記角度算出手段により算出された角度との中間の角度であることが考えられる。
【0020】
また，前記速度算出手段が，速度に代えて加速度を算出するものであり，前記再算出手段及び前記駆動指示手段が，速度に代えて加速度に基づいて判別するものであってもよい。本発明にかかる多関節型ロボットの制御装置の最も好ましい形態は，作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた第２関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御装置であって，前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段と，前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段と，前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出手段と，前記補間データ算出手段により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段と，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出手段と，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段と，を備えてなり、前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，前記角度算出手段が，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出するものであることを特徴とする。
本発明にかかる多関節型ロボットの制御装置の最も好ましい別形態は，作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた第２関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御装置であって，前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段と，前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段と，前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出手段と，前記補間データ算出手段により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段と，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出手段と，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段と，を備えてなり、前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，前記角度算出手段が，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする。
本発明にかかる多関節型ロボットの制御装置の最も好ましい別形態は，作業対象物の作業線に則して移動させる作業部を先端に有し，その作業部に連結されて前記作業部の姿勢を変化させる３つの駆動軸を備えた第１関節駆動系と，前記第１関節駆動系に連結されて該第１関節駆動系の位置を変化させる少なくとも３つの駆動軸を備えた第２関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御装置であって，前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段と，前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより，前記補間点における前記作業部の位置及びその時の姿勢を示す前記第１関節駆動系及び前記第２関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段と，前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第１関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第１関節駆動系における両端の２つの駆動軸の速度を算出する速度算出手段と，前記補間データ算出手段により算出されたデータを，前記作業部の移動方向の軸，該軸に垂直な方向の軸，該２つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段と，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に，前記作業部の移動速度を変更することなく，且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第１関節駆動系における両端の２つの駆動軸の速度が前記許容範囲内となる前記第１関節駆動系の駆動軸各々の角度を再度算出し，該算出された前記第１関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第２関節駆動系の駆動軸各々の角度を再度算出する再算出手段と，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記角度算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段と，を備えてなり、前記第１関節駆動系の駆動軸の角度を±１８０°で表したとき，前記角度算出手段が，前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が同符号である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記作業開始位置及び前記作業終了位置における前記第１関節駆動系の中央の駆動軸の角度が異符号である場合において，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の両方の速度が前記許容範囲内である場合は，前記第１
関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し，前記速度算出手段により算出された前記第１関節駆動系における両端の２つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は，前記第１関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものであることを特徴とする。
なお，本発明は前記多関節型ロボットの制御装置で実行される各工程を実行する多関節型ロボットの制御方法の発明，或いは，その各工程をコンピュータに実行させる多関節型ロボットの制御プログラムの発明として捉えることができる。
【発明の効果】
【0021】
本発明によれば，手首先端を僅かに動かしただけで，手首軸が所定の許容量を超えて急激に変化する場合であっても，溶接や塗装作業に特に必要な手首先端の位置及び手首先端の移動速度と手首軸の許容速度とを維持し，かつ目的に応じた作業座標系における姿勢角度の少なくとも一つの特定成分の変動を抑制することが可能となる。また，前記作業開始位置から前記作業終了位置までの間に存在する特異点領域や特異点回避経路などを算出する場合には，事前計算における過大な負荷に対応することのできる演算装置（ＣＰＵ）を設ける必要がありコストアップ要因となるが，本発明によれば，現在の状態と次の状態とに応じてその都度駆動が制御すればよいため，演算装置に瞬間的に要求される処理負荷が軽減されており低コスト化を図ることができる。
【図面の簡単な説明】
【0022】
【図1】本発明の実施形態に係る多関節型ロボットの制御方法の一例によって制御される多関節型ロボットＸの概略構成を表すブロック図。
【図2】多関節型ロボットの制御方法の制御全体処理の一例を示すフローチャート。
【図3】補間点の各回転軸の速度・角度算出処理の一例を示すフローチャート。
【図4】速度抑制必要性判別処理の一例を示すフローチャート。
【図5】速度抑制処理の一例を示すフローチャート。
【図6】速度抑制処理からの復帰処理の一例を示すフローチャート。
【図7】補間点の経路の一例を示す図。
【図8】元の経路と例外経路とのＪ６軸の角度の変化の一例を示す図（Ａ）及び元の経路の補間点と例外経路の補間点との一例を示す図（Ｂ）。
【図9】特異点を回避する経路の第１関節駆動軸の角度変化の一例を示す図表。
【図10】Ｊ４軸の速度の変化の一例を示す図。
【図11】等速動作時における溶接線座標系の各姿勢角の変動の一例を示す図。
【図12】本発明の制御方法における溶接線座標系の各姿勢角の変動の一例を示す図。
【図13】第２関節駆動軸の他の例を示す図。
【図14】多関節型ロボットの制御方法の制御全体処理の他の例を示すフローチャート。
【図15】特異点を経由する経路の第１関節駆動軸の角度変化の一例を示す図表。
【図16】本実施例１に係る制御手法を採用した場合の第１関節駆動軸の角度変化の一例を示す図表。
【図17】等速動作時における溶接線座標系の各姿勢角の変動の一例を示す図。
【図18】本実施例１に係る制御手法を採用した場合の溶接線座標系の各姿勢角の変動の一例を示す図。
【図19】多関節型ロボットの制御方法の制御全体処理の他の例を示すフローチャート。
【図20】実施例３に係る２つの作業点を有するエンドエフェクタの一例を示す図。
【図21】２つの作業点を有するエンドエフェクタの多関節型ロボットの特異点を回避する経路の溶接線座標系の各姿勢角の変動の一例を示す図。
【図22】実施例４に係る多関節型ロボットの制御方法の一例によって制御される多関節型ロボットＸ２の概略構成を表すブロック図。
【図23】一般的な６軸のマニピュレータの概略構成を表す図。
【図24】一般的な６軸のマニピュレータが特異点を経由する経路における第１関節駆動軸の角度変化の一例を示す図表。
【図25】一般的な６軸のマニピュレータのエンドエフェクタの位置と姿勢を示す座標系を溶接線座標系に変換する場合の一例を示す図。
【図26】溶接線座標系における各軸毎の傾き成分の一例を示す図。
【図27】溶接線座標系におけるトーチ傾斜角の一例を示す図。
【図28】溶接線座標系におけるトーチ前進角とトーチ回転角の一例を示す図。
【図29】実施例５に係るコンピュータシステムＹの概略構成を示すブロック図。
【図30】特異点を回避中であることを報知する装置を説明するための図。
【発明を実施するための形態】
【0023】
以下添付図面を参照しながら，本発明の実施の形態について説明し，本発明の理解に供する。尚，以下の実施の形態は，本発明を具体化した一例であって，本発明の技術的範囲を限定する性格のものではない。まず，図１に示される概略構成図を参照しつつ，本発明の実施形態に係る多関節型ロボットＸの構成について説明する。
【0024】
多関節型ロボットＸは，制御部１０，操作部２１，マニピュレータ本体３０等を備えている。尚，多関節型ロボットＸは，図１に示す構成要素以外にも，一般的な多関節型ロボットが備える他の構成要素も備えているが，ここでは記載を省略している。制御部１０は，主制御部１１，記憶部１２，駆動指示部１３を備えており，本発明に係る多関節型ロボットの制御装置の一例である。
【0025】
マニピュレータ本体３０は，人間の腕に相当するＪ１軸３１，Ｊ２軸３２，Ｊ３軸３３を有する第２関節駆動系と，人間の手首に相当するＪ４軸３４，Ｊ５軸３５，Ｊ６軸３６を有する第１関節駆動系と，作業対象物に効果を発揮するエンドエフェクタ３９とを備え，操作された内容に従って駆動し，人間の上腕が行うのと同様の作業をするロボットである。尚，上記マニピュレータ本体３０は，多関節型ロボットの一例である。
【0026】
Ｊ１軸３１〜Ｊ６軸３６は，電気モータ等により構成され，駆動指示部１３からの指示により正負の方向に回転駆動し，複数の軸の回転駆動が連関することによって，人間の手首や腕の動きを実現する。特に，前記第２関節駆動系は，前記第１関節駆動系の駆動軸の動作にかかわらず，前記多関節型ロボットのベース座標系Σｂａｓｅにおける前記エンドエフェクタ３９の位置（Ｘ，Ｙ，Ｚ）を自由に作り出すことができる。
【0027】
エンドエフェクタ３９は，マニピュレータ本体３０の先端（Ｊ６軸３６の先）に取り付けられた効果器であり，１つの作業点により作業をする溶接器（トーチ），塗装器，工具，捕獲器，センサなどである。尚，上記エンドエフェクタ３９は，作業部の一例である。また，１つの作業点を備えるトーチをシングルトーチという。
操作部２１は，ユーザにより操作されるシートキーや操作ボタン，操作レバー等により構成され，ユーザによる操作入力を受け付ける入力インターフェースである。例えば，操作部２１は，前記エンドエフェクタ３９の作業開始位置及び姿勢と作業終了位置及び姿勢とこれを結ぶ経路との作業経路や作業時間，目的に応じた溶接線座標系Σline（作業座標系）におけるエンドエフェクタ３９の姿勢角度の各成分の変動を抑制するための重み付け情報，これらの内容が記載されたプログラムなどの入力を受け付けて制御部１０に出力する。即ち，操作部２１は，エンドエフェクタ３９の作業開始位置及び姿勢，作業終了位置及び姿勢を教示するための手段であり，前記重み付け情報を設定するための手段である。
【0028】
記憶部１２は，例えば，ハードディスクや，ＤＲＡＭ等により構成された揮発性メモリであり，エンドエフェクタ３９の作業開始位置及び姿勢と作業終了位置及び姿勢とこれを結ぶ経路との作業経路や作業時間，エンドエフェクタ３９の姿勢角度の各成分の変動を抑制するための重み付け情報，後述する主制御部１１により算出された補間点（特異点を経由する経路と，特異点を回避する経路との双方）における前記Ｊ１軸３１〜Ｊ６軸３６の角度と速度（場合によっては更に加速度）などを記憶するものである。
【0029】
駆動指示部１３は，主制御部１１からの制御指示に従ってサンプリング周期毎に記憶部１２に記憶された次の補間点（特異点を経由する経路又は特異点を回避する経路の一方）にエンドエフェクタ３９を動かすように前記Ｊ１軸３１〜Ｊ６軸３６の角度又は速度の情報を読み出して，マニピュレータ本体３０に出力する。尚，主制御部１１及び駆動指示部１３が，駆動指示工程を実行するための駆動指示手段の一例である。
【0030】
主制御部１１は，ＣＰＵ，ＲＯＭなどを備え，ＣＰＵは，当該多関節型ロボットＸの各種制御及び演算を行う演算手段であり，ＲＯＭは，ＣＰＵにより実行される制御プログラムや演算プログラム，ＣＰＵがそれらプログラムを実行する際に参照するデータ等が記憶されるメモリである。次に，図２〜図６のフローチャートを参照して，多関節型ロボットＸの主制御部１１が中心となって，マニピュレータ本体３０を作動させる処理手順の一例について説明する。尚，以下に示すＳ１１，Ｓ１２，…は，処理手順（ステップ）の識別符号を表す。
【0031】
まず，図２を参照して，一般的なマニピュレータ本体３０の制御と，この発明の一実施形態に係る多関節型ロボットＸの制御との処理手順の違いについて説明する。一般的なマニピュレータ本体３０の制御は，後述するように主制御部１１に作動軌跡を教示する教示工程（ステップＳ１１）を実施し，更に教示された作動軌跡上の補間点の位置・姿勢に作動させるために各駆動軸の角度を算出・記憶する工程（ステップＳ１２）と，算出された角度を読み出して作動を指示する工程（ステップＳ１４）と，作業終了位置・姿勢に到達したか否かを判別する工程（ステップＳ１５）とを実施して，マニピュレータ本体３０を現在の補間点の位置・姿勢から次の補間点の位置・姿勢に作動させ，以後，所定のサンプリング期間毎にステップＳ１２，Ｓ１４，Ｓ１５を繰り返すことにより作動軌跡に沿って作業開始位置・姿勢から作業終了位置・姿勢までマニピュレータ本体３０を作動させる。また，主制御部１１は，マニピュレータ本体３０のエンドエフェクタ３９を予め設定された一定速度で移動させることにより，該エンドエフェクタ３９による作業を実行する。但し，一定速度で移動させる場合には，第１関節駆動系のＪ５軸３５が０°近傍を通過するとき（特異点）に，Ｊ４軸３４やＪ６軸３６の速度が予め定められた許容範囲を超えるおそれがある。
【0032】
そこで，本実施の形態に係る多関節型ロボットＸの制御は，上記一般的なマニピュレータ本体３０の制御に必要な工程に加え，後述するように第１関節駆動系のＪ４軸３４，Ｊ６軸３６の少なくとも一方の速度が許容範囲を超えるため速度抑制処理が必要か否かを判別する工程（ステップＳ１３）と，速度抑制処理が必要な場合に（ステップＳ１３；ＹＥＳ），特異点を回避した作動軌跡上の補間点に作動させるために各駆動軸の角度を再度算出・記憶する速度抑制工程（ステップＳ２０）とを含み，特異点を回避してマニピュレータ本体３０を現在の補間点の位置・姿勢から再算出された次の補間点の位置・姿勢に作動させるものである。
【0033】
続いて，図２を参照して，主制御部１１によって実行される多関節型ロボットＸの制御全体処理について説明し，各処理の詳細については後述する。最初に，利用者が，ロボットの作動軌跡を，操作部２１によりポイントツーポイントで教示する。即ち，操作部２１によって各教示ポイントに移動させ，前記エンドエフェクタ３９の作業位置及び姿勢を制御部１０の記憶部１２に記憶させる（ステップＳ１１）。尚，ステップＳ１１が教示工程の一例である。
【0034】
主制御部１１は，上記の教示されたポイント間を所定のサンプリング周期毎（例えば，１／１６秒，１／３２秒など）に分割・補間し，エンドエフェクタ３９を次の補間点に動かすためのＪ１軸３１〜Ｊ６軸３６の角度と第１関節駆動系のＪ４軸３４〜Ｊ６軸３６の速度（場合によっては更に加速度）とを算出して記憶部１２に記憶させる（ステップＳ１２）。ここに，Ｊ１軸３１〜Ｊ６軸３６の角度は±１８０°で表される。もちろん，角度の指定方法は±１８０°に限らず，例えば０°〜３６０°などであってもよい。尚，このステップＳ１２の詳細については後述する。
【0035】
そして，主制御部１１は，前記ステップＳ１２で算出されたエンドエフェクタ３９を次の補間点に動かすための第１関節駆動系のＪ４軸３４及びＪ６軸３６の一方又は両方の速度や加速度が許容範囲内か外か（以後，速度抑制処理の要・不要という）又は，既に許容範囲外の駆動軸があると判別された後の回避処理中（以後，速度抑制処理中という）であるかを判別する（ステップＳ１３）。尚，このステップＳ１３の詳細については後述する。
【0036】
ここで，速度抑制処理の要又は速度抑制処理中と判別されると（ステップＳ１３；ＹＥＳ），主制御部１１が，エンドエフェクタ３９の移動速度を変更することなく，且つ前記ステップＳ１２により算出された次の補間点におけるエンドエフェクタ３９の姿勢データに対して一つ又は二つの特定成分の変動を抑制し速度や加速度が前記許容範囲内となるように該Ｊ４軸３４及びＪ６軸３６の一方又は両方の角度を再度算出し，これに対応する他の軸の角度を再算出して記憶部１２に記憶させる速度抑制処理を行う（ステップＳ２０）。尚，このステップＳ２０の上記の説明では，速度抑制処理の要・不要を判別する目安として，各軸の速度や加速度を問題としたが，該ステップＳ２０の具体例についての後述の説明では，速度について判別する場合を例に挙げて説明する。
【0037】
他方，速度抑制処理が不要であり速度抑制処理中でないと判別される（ステップＳ１３；ＮＯ）か，速度抑制処理（ステップＳ２０）がなされると，主制御部１１の指示により駆動指示部１３が，エンドエフェクタ３９を次の補間点に動かすためのＪ１軸３１〜Ｊ６軸３６の角度を記憶部１２から読み出して，マニピュレータ本体３０の各駆動軸のアクチュエータに出力する（ステップＳ１４）。このとき，速度抑制処理が不要であり速度抑制処理中でないと判別された場合には，前記ステップＳ１２で算出された次の補間点に動かすためのＪ１軸３１〜Ｊ６軸３６の角度に基づいてマニピュレータ本体３０が制御され，Ｊ４軸３４やＪ６軸３６の速度が許容範囲外となって速度抑制処理が実行された場合には，前記ステップＳ２０で算出された次の補間点に動かすためのＪ１軸３１〜Ｊ６軸３６の角度に基づいてマニピュレータ本体３０が制御される。ここに，係るステップＳ１４が駆動指示工程の一例であって，係る処理を実行するときの主制御部１１及び駆動支持部１３が駆動指示手段に相当する。
【0038】
続いて，主制御部１１は，移動後のエンドエフェクタ３９の位置及び姿勢が，作業部終了位置及び姿勢に到達したかを判別する（ステップＳ１５）。作業部終了位置及び姿勢に到達していない場合には（ステップＳ１５；ＮＯ），所定のサンプリング周期毎に，上述したステップＳ１２以下の工程を再度実施し，作業部終了位置及び姿勢に到達した場合には（ステップＳ１５；ＹＥＳ），処理を終了する。
【0039】
このように，主制御部１１は，所定のサンプリング周期毎に，エンドエフェクタ３９を現在位置及び姿勢から次の補間点に動かすためのＪ１軸３１〜Ｊ６軸３６の角度を算出して，多関節型ロボットＸを動作させる。次に，図３を参照して，図２のステップＳ１２に該当する多関節型ロボットＸの現在の補間点から次の補間点に移動するためのＪ１軸３１〜Ｊ６軸３６の角度と第１関節駆動系のＪ４軸３４〜Ｊ６軸３６の速度とを算出する処理手順の一例について説明する。
【0040】
まず，主制御部１１は，前記ステップＳ１１において予め教示された作業開始位置及び姿勢Ｐ0と作業終了位置及び姿勢Ｐnとの間をｎ（サンプリング周期毎に基づく，任意の値）個の点で補間したエンドエフェクタ３９の位置及び姿勢を算出する。この場合，１サンプリング周期毎の移動量は，ΔＰ＝（Ｐn−Ｐ0）／ｎ＝｛ΔＸ＝（Ｘn−Ｘ0）／ｎ，ΔＹ＝（Ｙn−Ｙ0）／ｎ，ΔＺ＝（Ｚn−Ｚ0）／ｎ，Δα＝（αn−α0）／ｎ，Δβ＝（βn−β0）／ｎ，Δγ＝（γn−γ0）／ｎ｝によって表せる。なお，前記補間点各々は，前記多関節型ロボットＸのベース座標系Σｂａｓｅにより表されるものである。
【0041】
主制御部１１は，図７に示すように，現在の位置の補間点がＰiである場合に，次の補間点Ｐi+1 の位置及び姿勢をＰi+1 ＝Ｐ0＋ΔＰ×（ｉ＋１）や，Ｐi+1 ＝Ｐi＋ΔＰによって算出する（ステップＳ１２１）。尚，このステップＳ１２１が補間データ算出工程の一例であり，係る処理を実行するときの主制御部１１が補間データ算出手段に相当する。続いて，主制御部１１は，次の補間点Ｐi+1の位置及び姿勢（Ｘi+1，Ｙi+1，Ｚi+1，αi+1，βi+1，i+1 ）から逆運動学問題の解を求めることによって，各Ｊ１軸３１〜Ｊ６軸３６の角度（θ1，θ2，θ3，θ4，θ5，θ6）を算出して，記憶部１２に記憶させる（ステップＳ１２２）。このとき，Ｊ４軸３４〜Ｊ６軸３６について逆運動学問題の２つの解が得られるが，ここでは，主制御部１１は，その２つの解のうち第１関節駆動系のＪ５軸３５の角度が，教示された前記エンドエフェクタ３９の作業開始位置におけるＪ５軸３５の角度と同符号となる逆運動学問題の解を採用する。これにより，Ｊ５軸３５の角度が０°を通過することなく推移するため特異点の通過を回避することができる。
【0042】
尚，主制御部１１は，後述する速度抑制処理を実行する場合には，特異点を経由する経路の補間点の各Ｊ１軸３１〜Ｊ６軸３６の角度ばかりではなく，特異点を回避する経路の補間点の各Ｊ１軸３１〜Ｊ６軸３６の角度をもそれぞれ算出して記憶部１２に記憶させて，後述する復帰処理（図５のステップＳ３１０及び図６のフローチャート）をする際の比較対象とする。
【0043】
ここに，ステップＳ１２２が角度算出工程の一例であり，係る処理を実行するときの主制御部１１が角度算出手段に相当する。次に，主制御部１１は，算出された次の補間点Ｐi+1 の各Ｊ４軸３４〜Ｊ６軸３６の角度から，記憶部１２に記憶された現在の補間点Ｐiの各Ｊ４軸３４〜Ｊ６軸３６の角度を差し引き，第１関節駆動系のＪ４軸３４〜Ｊ６軸３６の速度を算出して記憶部１２に記憶させる（ステップＳ１２３）。尚，後述するように，速度抑制処理（ステップＳ２０）を実行する場合には，特異点を経由する系の補間点の第１関節駆動系のＪ４軸３４〜Ｊ６軸３６の速度を算出し記憶部１２に記憶させ，後述するステップＳ１３１において速度の許容範囲を算出したり，ステップＳ２１１等において次の補間点の候補となる速度（必要に応じて加速度）を算出するために用いる。
【0044】
ここに，ステップＳ１２３が速度算出工程の一例であり，係る処理を実行するときの主制御部１１が速度算出手段に相当する。次に，図４を参照して，図２のステップＳ１３に該当する速度抑制必要性判別処理手順の一例について説明する。
まず，主制御部１１はエンドエフェクタ３９の位置及び姿勢を現在の補間点Ｐｉから次の補間点Ｐi+1 に動かす際の第１関節駆動系のＪ４軸３４，Ｊ６軸３６の速度の許容範囲を算出する（ステップＳ１３１）。例えば，現在の補間点Ｐｉの速度の値に所定の値や所定の割合を加減算して得られる値を上限・下限として，速度の許容範囲を算出する。尚，この各軸の速度の許容範囲は，エンドエフェクタ３９の作動条件（例えば，溶接する際の温度，塗装する際の塗装膜の厚さなどの条件）や多関節型ロボットが動作する際に振動などの異常動作を起こさないための条件など，作業対象物の品質に影響を及ぼさない範囲内で予め定めておいても良い。さらに，Ｊ４軸３４，Ｊ６軸３６ごとに個別の許容範囲を設定しても良い。
【0045】
次に，主制御部１１は，第１関節駆動系の次の補間点におけるＪ４軸３４及びＪ６軸３６の一方又は両方の速度が上記許容範囲を超えているか，或いは，速度抑制処理中を示すフラグが立っているか否かを判別する（ステップＳ１３２）。ここで，第１関節駆動系の次の補間点におけるＪ４軸３４及びＪ６軸３６の両方が前記許容範囲内であり，且つ速度抑制処理中でない場合には（ステップＳ１３２；ＮＯ），主制御部１１の指示により駆動指示部１３は記憶部１２から前記ステップＳ１２で算出された各Ｊ１軸３１〜Ｊ６軸３６の角度を読み出しマニピュレータ本体３０に出力する（ステップＳ１４）。他方，許容範囲外の軸がある，又は速度抑制処理中である場合には（ステップＳ１３２；ＹＥＳ），主制御部１１は以下に述べる速度抑制処理（ステップＳ２０）を実行する。
【0046】
以下，図５を参照して，図２のステップＳ２０に該当する多関節型ロボットＸの速度抑制処理の具体例について説明する。尚，以下では，図２４に示したＪ５軸３５が０度付近にあり，Ｊ４軸３４やＪ６軸３６の速度が許容範囲を超えるときに当該速度抑制処理を実行する場合を例に挙げて説明する
が，他の理由によってＪ４軸３４やＪ６軸３６の速度が許容範囲を超える場合にも同様である。従って，当該速度抑制処理は，第１関節駆動系のＪ４軸３４，Ｊ６軸３６の何れか１つ又は２つの駆動軸の速度が許容範囲を超える場合に実施される。尚，最初に次の補間点について許容範囲外の軸が発見された時に，現在の補間点での速度は許容範囲内である。
【0047】
まず，主制御部１１は，次の補間点について速度が前記許容範囲外である駆動軸がＪ４軸３４及びＪ６軸３６のいずれであるか又は両方であるか，或いは，既に速度抑制処理中のフラグが立っている場合には，該速度抑制処理がＪ４軸３４及びＪ６軸３６のいずれか一方又は両方について実行されているかを判別し，処理を分岐する（ステップＳ２０１）。許容範囲内か外かは，前記したように速度が許容範囲の上限・下限の間にあるか否かにより判別される。ここで，Ｊ４軸３４及びＪ６軸３６の両方が速度の許容範囲外でなく，且つ速度抑制処理中のフラグが立っていなければ（ステップＳ２０１；その他），主制御部１１は当該速度抑制処理を終了する。
【0048】
一方，Ｊ６軸３６の速度が許容範囲外である場合は処理がステップＳ２１１に移行し，Ｊ４軸３４の速度が許容範囲外である場合は処理がステップＳ２２１に移行し，Ｊ４軸３４及びＪ６軸３６の両方の速度が許容範囲外である場合は処理がステップＳ２３１に移行する。（Ｊ６軸３６の速度抑制処理ステップＳ２１１〜Ｓ２１７）
Ｊ６軸３６の速度が許容範囲外であるか又は，Ｊ６軸３６の速度抑制処理中を示すフラグが立っている場合（ステップＳ２０１；Ｊ６軸が範囲外），主制御部１１は，現在の補間点におけるＪ６軸３６の角度に基づいて，該Ｊ６軸３６の角度が前記ステップＳ１２で算出された次の補間点における角度に追従し，且つＪ６軸３６の次の補間点における速度が前記許容範囲内となる角度の候補を複数算出する（ステップＳ２１１）。
【0049】
例えば，主制御部１１は，以下の（２１）〜（２３）の手順により，Ｊ６軸３６の速度の候補を算出する。（２１）まず，下記（２１ａ）式により抑制基準速度Ｖ6b を算出する。ここに，θnow は次の補間点におけるＪ６軸３６の角度，θold は現在の補間点におけるＪ６軸３６の角度，Ｅは予め設定された係数（例えば０．１）である。
Ｖ6b ＝Ｅ（θnow −θold ） …（２１ａ）
（２２）次に，抑制基準速度Ｖ6b が予め定められた速度リミッタＶmax 以上であるか否かを判断し，該速度リミッタＶmax 以上であると判断した場合は，抑制基準速度Ｖ6b を前回の速度θold に設定する。前記速度リミッタＶmax は，例えば前記許容範囲内の速度の最大値である。
（２３）そして，抑制基準速度Ｖ6b と所定の値Ｄ（事前に定められた許容範囲内の加速度を保っていることが分かっている速度の変化分の絶対値など）とにより３つの速度の候補（Ｖθ6a ＝Ｖ6b −Ｄ，Ｖθ6b ＝Ｖ6b ，Ｖθ6c ＝Ｖ6b ＋Ｄ）を算出する。前記所定の値Ｄは，例えば前記速度リミッタＶmax の１０％程度の値である。また，Ｖθ6a が前記速度リミッタＶmax より大きい場合は，Ｖθ6a ＝Ｖmax とする。
【0050】
その後，主制御部１１は，現在の補間点におけるＪ６軸３６の角度θ6から上記の速度の各候補（Ｖθ6a ，Ｖθ6b ，Ｖθ6c ）に対応する次の補間点における３つのＪ６軸３６の角度の候補（θ6a ，θ6b ，θ6c ）を算出する。即ち，次の補間点として，少なくとも許容範囲を超えていない抑制基準速度Ｖ6b 又は現在の補間点における速度を継続した角度（当然，許容範囲内である）と，それにより僅かな量（±Ｄ（速度の変化分の絶対値に対応する））だけ離れた角度が算出される。ここに，係るステップＳ２１１の処理が角度候補算出工程の一例であり，係る処理を実行するときの主制御部１１が角度候補算出手段に相当する。
【0051】
次に，主制御部１１は算出された次の補間点におけるＪ６軸３６の角度の各候補に対応する他の駆動軸Ｊ１軸３１〜Ｊ５軸３５の角度を算出する（ステップＳ２１２）。例えば，以下の（１）〜（３）に示すように，Ｊ６軸３６の角度の候補（θ6a ，θ6b ，θ6c ）毎に，前記ステップＳ１２２において算出された次の補間点Ｐi+1 のエンドエフェクタ３９の位置（Ｘi+1 ，Ｙi+1 ，Ｚi+1 ）と，第１関節駆動系のＪ４軸３４，Ｊ５軸３５の角度（θ4，θ5）とを用いて，第１関節駆動系及び第２関節駆動系のＪ１軸３１〜Ｊ６軸３６の角度（θ1，θ2，θ3，θ4，θ5，θ6）を再算出する。（１）候補θ6a に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4，θ5，θ6a ］から［θ1a ，θ2a，θ3a ，θ4，θ5，θ6a ］を算出する。
（２）候補θ6b に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4，θ5，θ6b ］から［θ1b ，θ2b，θ3b ，θ4，θ5，θ6b ］を算出する。
（３）候補θ6c に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4，θ5，θ6c ］から［θ1c ，θ2c，θ3c ，θ4，θ5，θ6c ］を算出する。
【0052】
これによって，後に説明する図８に示すように，速度が許容範囲を超えない次の補間点の候補Ｐ’i+1 が計算され，この次の補間点の候補Ｐ’i+1 を次の補間点と見做すことにより，Ｊ６軸３６の速度が許容範囲を超えず，元の補間点Ｐi+1 （特異点）を回避した例外経路（図８（Ｂ）参照）を得ることができる。このようにしてＪ６軸３６の速度を許容範囲内に維持した次の補間点における３つの角度の候補（θ6a ，θ6b ，θ6c ）が得られると，次の補間点としてどの候補を選択すべきかの判断が行われる。ここまでの計算では，第１関節駆動系のＪ４軸３４〜Ｊ６軸３６の角度（θ4，θ5，θ6）が問題とされているが，最終的には作業の種類に応じて重要視されるエンドエフェクタ３９の姿勢に焦点を合わせて候補が選択されなければならない。従って，上記Ｊ１軸３１〜Ｊ６軸３６の角度からエンドエフェクタ３９の姿勢を示すデータへの変換が必要である。
【0053】
そして，次の補間点としてどの候補を選択すべきかの判断を行うため，主制御部１１は，算出された候補ごとに次の補間点における溶接線座標系Σlineのエンドエフェクタ３９の姿勢を示すデータの各成分（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）をそれぞれ算出する（ステップＳ２１３）。尚，このステップＳ２１３が，姿勢データ算出工程の一例であり，係る処理を実行するときの主制御部１１が姿勢データ算出手段に相当する。ここに，前記溶接線座標系Σlineは，図２５に示すように，エンドエフェクタ３９が移動する方向（進行方向）の軸をＸ軸とし，該Ｘ軸と重力方向との外積（Ｘ軸×重力方向）によって表される軸をＹ軸とし，該Ｘ軸と該Ｙ軸との外積（Ｘ軸×Ｙ軸）によって表される方向の軸をＺ軸とする作業座標系であり，図２６〜図２８に示すように，該Ｘ軸の軸周りの回転角がトーチ傾斜角Ｒｘで示され，該Ｙ軸の軸周りの回転角がトーチ前進角Ｒｙで示され，該Ｚ軸の軸周りの回転角がトーチ回転角Ｒｚで示される。例えば，上記の（１）〜（３）の候補θ6a 〜θ6c に対して，以下の（１’）〜（３’）によりエンドエフェクタ３９の姿勢を示すデータの各成分を算出する。
（１’）［θ1a ，θ2a ，θ3a ，θ4，θ5，θ6a ］から［Ｒxa ，Ｒya ，Ｒza ］を算出する。
（２’）［θ1b ，θ2b ，θ3b ，θ4，θ5，θ6b ］から［Ｒxb ，Ｒyb ，Ｒzb ］を算出する。
（３’）［θ1c ，θ2c ，θ3c ，θ4，θ5，θ6c ］から［Ｒxc ，Ｒyc ，Ｒzc ］を算出する。
【0054】
次に，主制御部１１は，前記ステップＳ１２１で算出された元の経路上の次の補間点Ｐi+1 におけるエンドエフェクタ３９の位置及び姿勢を直交座標系Σbaseによって表した（Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，αi+1 ，βi+1 ，γi+1 ）を，エンドエフェクタ３９が移動する方向の軸をＸ軸とする溶接線座標系Σline（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）によって表すためにデータを変換する（ステップＳ２１４）。尚，このステップＳ２１４が，姿勢データ変換工程の一例であり，係る処理を実行するときの主制御部１１が姿勢データ変換手段に相当する。尚，直交座標系Σbaseから溶接線座標系Σlineに変換する式を下記の（数１）に示す。
【0055】
【数1】
【0056】
そして，もともとＪ６軸３６の速度が許容範囲外であったので，軌道を維持しつつＪ６軸３６の速度を抑制するためには，Ｊ６軸３６以外のＪ１軸３１〜Ｊ５軸３５の中から余分に回転してもよい軸を選定し，その軸を余分に回転させることで，Ｊ６軸３６の超過速度に対応する変動量を抑制しなければならない。しかし，軌道を維持しつつＪ１軸３１〜Ｊ５軸３５のどの軸を余分に回転させればよいかは，角度θ1〜θ6によって表される領域では判断することができない。これに対して，エンドエフェクタ３９の作業の種類と密接に関係する溶接線座標系Σlineにおける各成分（Ｒｘ，Ｒｙ，Ｒｚ）によって表される領域では判断することができる。
【0057】
そこで，図５に示すように，主制御部１１は，前記ステップＳ２１３において変換された溶接線座標系Σlineにおけるエンドエフェクタ３９の姿勢を示すデータの各成分（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）の中から変動を抑制すべき成分を示す情報（重み付け情報）を取得する（ステップＳ２１５）。尚，主制御部１１は，前記操作部２１からの入力によりどの成分の重み付けを大きくするかを任意に設定してもよい。例えば，この実施形態では，エンドファクタ３９は作業点が１つの溶接トーチであり，溶接トーチを用いた溶接作業の性質上，トーチの作業対象物（材料）に対する角度やトーチの進行速度は重要であるが，トーチの軸周りの回転は重要でない。そのため，この実施形態では，前記重み付け情報が，トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙを重み付けが大きい（抑制すべき）成分とし，トーチ回転角Ｒｚを重み付けが小さい（抑制しない）成分として予め設定されているものとする。この重み付け情報は，後述するステップＳ２１６の変動を抑制した候補を選択する評価式の各成分に対応する係数として用いられる。
【0058】
次に，主制御部１１は算出された次の補間点における複数の候補の中から前述したステップＳ２１５により取得された抑制するための重み付けが大きい特定成分（トーチ傾斜角Ｒx，トーチ前進角Ｒy，トーチ回転角Ｒzのいずれか一つ又は二つ）の変動が最も少ない候補を選択する（ステップＳ２１６）。例えば，トーチ傾斜角と，トーチ前進角との変動を抑制し，トーチ回転角の変動を許容する場合，重み付け係数Ａ＝１，Ｂ＝１，Ｃ＝０とし（即ち特定成分はトーチ傾斜角及びトーチ前進角），上記の（１’）〜（３’）の候補に対して下記の（１”）〜（３”）の演算をする。なお，Ｒｘ，Ｒｙ，Ｒｚは元の経路上における次の補間点のトーチ傾斜角，トーチ前進角，トーチ回転角である。（１”）[Ｆa＝Ａ(Ｒｘ−Ｒxa )2＋Ｂ(Ｒｙ−Ｒya )2＋Ｃ(Ｒｚ−Ｒza )2]
（２”）[Ｆb＝Ａ(Ｒｘ−Ｒxb )2＋Ｂ(Ｒｙ−Ｒyb )2＋Ｃ(Ｒｚ−Ｒzb )2]
（３”）[Ｆc＝Ａ(Ｒｘ−Ｒxc )2＋Ｂ(Ｒｙ−Ｒyc )2＋Ｃ(Ｒｚ−Ｒzc )2]
上記（１”）〜（３”）において，変動を抑制する成分に重み付けをして，前記ステップＳ２１４で算出された元の経路上の次の補間点のエンドエフェクタ３９の姿勢角と，前記ステップＳ２１３で算出された例外経路上の次の補間点の各候補のエンドエフェクタ３９の姿勢角との差Ｆa〜Ｆcが求められるので，そのＦa〜Ｆcの中で最も値が小さい（変動が小さい）ものを選択し，選択された候補の駆動軸Ｊ１軸３１〜Ｊ６軸３６の角度を特異点を回避する動作時の各軸の角度として採用する。ここに，係るステップＳ２１６の処理が角度選択工程の一例であり，係る処理を実行するときの主制御部１１が角度選択手段に相当する。また，選択された候補の駆動軸Ｊ１軸３１〜Ｊ６軸３６の角度とＪ６軸３６の速度を記憶部１２に保存する。これにより，エンドエフェクタ３９の姿勢データのうち予め定められた重み付けが大きい成分から順に変動の抑制量が大きくなるように駆動軸Ｊ１軸３１〜Ｊ６軸３６の角度が再度算出される。例えば，重み付け係数Ａ＝０．２，Ｂ＝０，Ｃ＝１である場合，トーチ回転角Ｒzの変動が最も抑制され，トーチ前進角Ｒyの変動が最も大きくなる。
【0059】
次に，主制御部１１はＪ６軸３６の速度抑制処理中のフラグが立っていなければ，フラグを立ててＪ６軸３６に対する速度抑制処理中であることを示す（ステップＳ２１７）。尚，ステップＳ２１１〜Ｓ２１３，Ｓ２１６，Ｓ２１７が，再算出工程の一例であって，係る処理を実行するときの主制御部１１が再算出手段に相当する。
上述した処理（ステップＳ２１１〜Ｓ２１７）を実行することによって，図８（Ａ），図８（Ｂ）に示すように，現在の補間点の位置Ｐiから特異点を経由する元の経路上の次の補間点Ｐi+1 ではなく，特異点を回避する例外経路上の次の補間点Ｐ’i+1 が見つかる。
【0060】
この演算（ステップＳ２０１，Ｓ２１１〜Ｓ２１７）を次の補間点が元の経路の補間点になるまで（ステップＳ２０１；その他）繰り返して，特異点を回避する例外経路を算出する。但し，前記制御部１０では，この算出処理が事前にまとめて実行されるのではなく，現在の状態と次の状態とに応じてその都度，主制御部１１によって実行されるため，該主制御部１１に瞬間的に要求される処理負荷が軽減され，低コスト化を図ることができる。
【0061】
尚，次の補間点の角度の候補（θ6a ，θ6b ，θ6c ）の中には，元々許容範囲内に維持することができる前記抑制基準速度Ｖ6b 又は現在の補間点のＪ６軸３６の角度が含まれているため，採用された何れかの候補により次の補間点のＪ６軸３６の速度を許容範囲内に維持することができる。その後に主制御部１１が実行するＪ６軸３６の速度抑制処理からの復帰処理（ステップＳ３１０）の詳細については後述する。
【0062】
（Ｊ４軸３４の速度抑制処理ステップＳ２２１〜Ｓ２２７）Ｊ４軸３４の速度が許容範囲外であるか又は，Ｊ４軸３４の速度抑制処理中を示すフラグが立っている場合（ステップＳ２０１；Ｊ４軸が範囲外），主制御部１１は，前述したＪ６軸３６の速度と角度の候補を算出する場合と同様に，許容範囲内に収まる複数のＪ４軸３４の角度の候補を算出する（ステップＳ２２１）。即ち，Ｊ４軸３４の抑制基準速度Ｖ4b が算出されて，３つの速度の候補（Ｖθ4a ＝Ｖ4b −Ｇ，Ｖθ4b ＝Ｖ4b ，Ｖθ4c ＝Ｖ4b＋Ｇ）が算出され，３つの角度の候補（θ4a ，θ4b ，θ4c ）が算出される。なお，所定の値Ｇは，事前に定められた許容範囲内の加速度を保っていることが分かっている速度の変化分の絶対値などである。ここに，係るステップＳ２２１の処理が角度候補算出工程の一例である。
【0063】
以下のステップＳ２２２〜Ｓ２２７，Ｓ３２０の処理は，前述したＪ６軸３６のステップＳ２１２〜Ｓ２１７，Ｓ３１０の処理と同様の処理を実行するため，説明を省略する。（Ｊ４軸３４・Ｊ６軸３６の速度抑制処理ステップＳ２３１〜Ｓ２３７）
Ｊ４軸３４及びＪ６軸３６両方の速度が許容範囲外であるか又は，Ｊ４軸及びＪ６軸３６両方の速度抑制処理中を示すフラグが立っている場合（ステップＳ２０１；Ｊ４軸・Ｊ６軸が範囲外），主制御部１１は許容範囲内に収まる１又は複数のＪ４軸３４とＪ６軸３６との両方の速度と角度の候補を算出する（ステップＳ２３１）。
【0064】
例えば，前記ステップＳ２３１では，前記ステップＳ２１１と同様にＪ６軸３６の速度の候補を３つ算出して，前記ステップＳ２２１と同様にＪ４軸３４の速度の候補を３つ算出した後，Ｊ４軸３４の候補３つとＪ６軸３６の候補３つとを組み合わせて９つの候補を算出することが考えられる。つまり，主制御部１１はＪ６軸３６の前記抑制基準速度Ｖ6bと前記所定の値Ｄとから３つの速度の候補（Ｖθ6a ＝Ｖ6b −Ｄ，Ｖθ6b ＝Ｖ6b ，Ｖθ6a ＝Ｖ6b ＋Ｄ）を生成し，Ｊ４軸３４の前記抑制基準速度Ｖ4b と所定の値Ｇとから３つの速度の候補（Ｖθ4a ＝Ｖ4b −Ｇ，Ｖθ4b ＝Ｖ4b ，Ｖθ4a ＝Ｖ4b ＋Ｇ）を生成する。更に，主制御部１１は現在の補間点におけるＪ６軸３６の角度θ6から上記の速度の各候補に対応する次の補間点における３つのＪ６軸３６の角度の候補（θ6a ，θ6b ，θ6c ）を算出し，現在の補間点におけるＪ４軸３４の角度θ4から上記の速度の各候補に対応する次の補間点における３つのＪ４軸３４の角度の候補（θ4a ，θ4b ，θ4c ）を算出する。そして，これらＪ４軸３４の候補とＪ６軸３６の候補とを組合せ，（θ4a ，θ6a ），（θ4a ，θ6b ），（θ4a ，θ6c ），（θ4b ，θ6a ），（θ4b ，θ6b ），（θ4b ，θ6c ），（θ4c ，θ6a ），（θ4c ，θ6b ），（θ4c ，θ6c ）の９つの候補を算出する。即ち，次の補間点におけるＪ４軸３４・Ｊ６軸３６夫々に対して，許容範囲を超えていない現在の補間点における速度を継続した角度と，それにより僅かな量だけ離れた角度が算出される。ここに，係るステップＳ２３１の処理が角度候補算出工程の一例である。
【0065】
次に，主制御部１１は算出された次の補間点におけるＪ４軸３４，Ｊ６軸３６の角度の各候補に対応する他の駆動軸Ｊ１軸３１〜Ｊ３軸３３，Ｊ５軸３５の角度を算出する（ステップＳ２３２）。例えば，Ｊ４軸３４・Ｊ６軸３６の角度の候補（θ4a ，θ6a ）〜（θ4c ，θ6c ）の組毎に，前記ステップＳ１２２において算出された次の補間点Ｐi+1 のエンドエフェクタ３９の位置（Ｘi+1 ，Ｙi+1 ，Ｚi+1 ）と，第１関節駆動系のＪ５軸３５の角度（θ5）とを用いて第２関節駆動系のＪ１軸３１〜Ｊ３軸３３の角度（θ1，θ2，θ3）を再算出する。（１）候補（θ4a ，θ6a ）に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4a ，θ5，θ6a ］から［θ1aa ，θ2aa ，θ3aa ，θ4a ，θ5，θ6a ］を算出する。
（２）候補（θ4a ，θ6b ）に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4a ，θ5，θ6b ］から［θ1ab ，θ2ab ，θ3ab ，θ4a ，θ5，θ6b ］を算出する。
（３）候補（θ4a ，θ6c ）に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4a ，θ5，θ6c ］から［θ1ac ，θ2ac ，θ3ac ，θ4a ，θ5，θ6c ］を算出する。
（４）候補（θ4b ，θ6a ）に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4b ，θ5，θ6a ］から［θ1ba ，θ2ba ，θ3ba ，θ4b ，θ5，θ6a ］を算出する。
（５）候補（θ4b ，θ6b ）に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4b ，θ5，θ-6b］から［θ1bb ，θ2bb ，θ3bb ，θ4b ，θ5，θ6b ］を算出する。
（６）候補（θ4b ，θ6c ）に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4b ，θ5，θ6c ］から［θ1bc ，θ2bc ，θ3bc ，θ4b ，θ5，θ6c ］を算出する。
（７）候補（θ4c ，θ6a ）に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4c ，θ5，θ6a ］から［θ1ca ，θ2ca ，θ3ca ，θ4c ，θ5，θ6a ］を算出する。
（８）候補（θ4c ，θ6b ）に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4c ，θ5，θ6b ］から［θ1cb ，θ2cb ，θ3cb ，θ4c ，θ5，θ6b ］を算出する。
（９）候補（θ4c ，θ6c ）に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4c ，θ5，θ6c ］から［θ1cc ，θ2cc ，θ3cc ，θ4c ，θ5，θ6c ］を算出する。
【0066】
これによって，速度が許容範囲を超えない次の補間点の候補Ｐ’i+1 が計算され，この次の補間点の候補Ｐ’i+1 を次の補間点と見做すことにより，許容範囲を超えない次の補間点Ｐi+1 （特異点）を回避する例外経路を得ることができる。次に，エンドエフェクタ３９の姿勢に焦点を合わせて候補を選択するために，Ｊ１軸３１〜Ｊ６軸３６の角度の候補をエンドエフェクタ３９の姿勢を示すデータへ変換する。
【0067】
具体的に，主制御部１１は算出された次の補間点における前記溶接線座標系のエンドエフェクタ３９の姿勢を示すデータの各成分（トーチ傾斜角Ｒx，トーチ前進角Ｒy，トーチ回転角Ｒz）を算出する（ステップＳ２３３）。例えば，上記の（１）〜（９）の角度の候補に対して，以下の（１’）〜（９’）によりエンドエフェクタ３９の姿勢を示すデータの各成分を算出する。
（１’）［θ1aa ，θ2aa ，θ3aa ，θ4a ，θ5，θ6a ］から［Ｒxaa ，Ｒyaa ，Ｒzaa ］を算出する。
（２’）［θ1ab ，θ2ab ，θ3ab ，θ4a ，θ5，θ6b ］から［Ｒxab ，Ｒyab ，Ｒzab ］を算出する。
（３’）［θ1ac ，θ2ac ，θ3ac ，θ4a ，θ5，θ6c ］から［Ｒxac ，Ｒyac ，Ｒzac ］を算出する。
（４’）［θ1ba ，θ2ba ，θ3ba ，θ4b ，θ5，θ6a ］から［Ｒxba ，Ｒyba ，Ｒzba ］を算出する。
（５’）［θ1bb ，θ2bb ，θ3bb ，θ4b ，θ5，θ6b ］から［Ｒxbb ，Ｒybb ，Ｒzbb ］を算出する。
（６’）［θ1bc ，θ2bc ，θ3bc ，θ4b ，θ5，θ6c ］から［Ｒxbc ，Ｒybc ，Ｒzbc ］を算出する。
（７’）［θ1ca ，θ2ca ，θ3ca ，θ4c ，θ5，θ6a ］から［Ｒxca ，Ｒyca ，Ｒzca ］を算出する。
（８’）［θ1cb ，θ2cb ，θ3cb ，θ4c ，θ5，θ6b ］から［Ｒxcb ，Ｒycb ，Ｒzcb ］を算出する。
（９’）［θ1cc ，θ2cc ，θ3cc ，θ4c ，θ5，θ6c ］から［Ｒxcc ，Ｒycc ，Ｒzcc ］を算出する。
【0068】
そして，主制御部１１は，前記ステップＳ２３４で算出された次の補間点Ｐi+1 における前記溶接線座標系のエンドエフェクタ３９の姿勢を示すデータの各成分（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）を算出する（ステップＳ２３４）。尚，このステップＳ２３４が，姿勢データ変換工程の一例であり，係る処理を実行するときの主制御部１１が姿勢データ変換手段に相当する。
【0069】
次に，主制御部１１は，前記ステップＳ２３３において変換された溶接線座標系Σlineにおけるエンドエフェクタ３９の姿勢を示すデータの各成分（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）の中から変動を抑制すべき成分を示す情報（重み付け情報）を取得する（ステップＳ２３５）。さらに，主制御部１１は算出された次の補間点における複数の候補の中から前述したステップＳ２３５により取得された抑制するための重み付けが大きい特定成分（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚのいずれか）の変動が最も少ない候補を選択する（ステップＳ２３６）。例えば，トーチ傾斜角と，トーチ前進との変動を抑制し，トーチ回転角の変動を許容する場合，重み付け係数Ａ＝１，Ｂ＝１，Ｃ＝０とし，上記の（１’）〜（９’）の候補に対して下記の（１”）〜（９”）の演算する。
（１”）[Ｆaa ＝Ａ(Ｒｘ−Ｒxaa)2＋Ｂ(Ｒｙ−Ｒyaa )2＋Ｃ(Ｒｚ−Ｒzaa )2 ]
（２”）[Ｆab ＝Ａ(Ｒｘ−Ｒxab)2＋Ｂ(Ｒｙ−Ｒyab )2＋Ｃ(Ｒｚ−Ｒzab )2 ]
（３”）[Ｆac ＝Ａ(Ｒｘ−Ｒxac)2＋Ｂ(Ｒｙ−Ｒyac )2＋Ｃ(Ｒｚ−Ｒzac )2 ]
（４”）[Ｆba ＝Ａ(Ｒｘ−Ｒxba)2＋Ｂ(Ｒｙ−Ｒyba )2＋Ｃ(Ｒｚ−Ｒzba )2 ]
（５”）[Ｆbb ＝Ａ(Ｒｘ−Ｒxbb)2＋Ｂ(Ｒｙ−Ｒybb )2＋Ｃ(Ｒｚ−Ｒzbb )2 ]
（６”）[Ｆbc ＝Ａ(Ｒｘ−Ｒxbc)2＋Ｂ(Ｒｙ−Ｒybc )2＋Ｃ(Ｒｚ−Ｒzbc )2 ]
（７”）[Ｆca ＝Ａ(Ｒｘ−Ｒxca)2＋Ｂ(Ｒｙ−Ｒyca )2＋Ｃ(Ｒｚ−Ｒzca )2 ]
（８”）[Ｆcb ＝Ａ(Ｒｘ−Ｒxcb)2＋Ｂ(Ｒｙ−Ｒycb )2＋Ｃ(Ｒｚ−Ｒzcb )2 ]
（９”）[Ｆcc ＝Ａ(Ｒｘ−Ｒxcc)2＋Ｂ(Ｒｙ−Ｒycc )2＋Ｃ(Ｒｚ−Ｒzcc )2 ]
上記（１”）〜（９”）において，変動を抑制する成分に重み付けをして，前記ステップＳ２３４で算出された元の経路の次の補間点のエンドエフェクタ３９の姿勢角と，前記ステップＳ２３３で算出された例外経路の次の補間点の各候補のエンドエフェクタ３９の姿勢角との差Ｆaa 〜Ｆcc が求められるので，その差Ｆaa 〜Ｆcc の中で最も値が小さい（変動が小さい）ものを選択し，選択された候補の駆動軸Ｊ１軸３１〜Ｊ６軸３６の角度を特異点を回避する動作時の各軸の角度とする。ここに，係るステップＳ２３６の処理が角度選択工程の一例であり，係る処理を実行するときの主制御部１１が角度選択手段に相当する。また，選択された候補の駆動軸Ｊ１軸３１〜Ｊ６軸３６の角度とＪ４軸３４・Ｊ６軸３６の速度を記憶部１２に保存する。
【0070】
その後，主制御部１１はＪ４軸３４・Ｊ６軸３６の速度抑制処理中のフラグが立っていなければ，フラグを立ててＪ４軸３４・Ｊ６軸３６に対する速度抑制処理中であることを示す（ステップＳ２３７）。上述した処理（ステップＳ２３１〜Ｓ２３７）をすることによって，現在の補間点の位置Ｐiから特異点を経由する元の経路上の次の補間点Ｐi+1 ではなく，特異点を回避する例外経路上の次の補間点Ｐ’i+1 が見つかる。
主制御部１１が実行するＪ４軸３４・Ｊ６軸３６の速度抑制処理からの復帰処理（ステップＳ３３０）の詳細については後述する。
【0071】
次に，図６を参照して，図５のステップＳ３１０に該当する多関節型ロボットＸの速度抑制処理からの復帰処理の手順の一例について説明する。主制御部１１は，図８（Ｂ）に示されるように特異点区間Ｐi〜Ｐi+1 を回避した例外経路上の次の補間点Ｐ’i+1 から，特異点を経由する元の経路上の更に次の補間点Ｐi+2 と特異点を回避した例外経路上の更に次の補間点Ｐ’i+2 との角度の差分を計算して，その差が所定の範囲を超えていない場合にはＰi+3 へ進んでこの処理を終了するが，超えている場合にはＰ’i+3 へ進み，差分が所定の範囲を超えている限りこの処理を繰り返す。やがて差分が所定の範囲内に入る次の補間点が見つかった場合に（この例では，Ｐ’i+4 〜Ｐi+5 の場合）は，補間点Ｐ’i+4 から補間点Ｐi+5 へ進みこの処理を終了する。そこで，特異点を回避する経路上の現在の補間点Ｐ’i+4 から特異点を経由する経路上の次の補間点Ｐi+5 に復帰する場合を中心に説明する。
【0072】
（Ｊ６軸３６の速度抑制処理からの復帰処理）まず，主制御部１１は前記ステップＳ１２２により算出された特異点を経由した系の次の補間点Ｐi+5 のＪ６軸３６の角度θ6(i+5) と，前記ステップＳ２１４により選択された特異点を回避した系の次の補間点Ｐ’i+5 のＪ６軸３６の角度θ’6(i+5) とを取得する（ステップＳ３１１）。
【0073】
次に，主制御部１１は特異点を回避した系のＪ６軸３６の角度θ’6(i+5) と特異点を経由した系のＪ６軸３６の角度θ6(i+5) との差分の値が所定の範囲以内かを判別する（ステップＳ３１２）。尚，前記所定の範囲はＪ６軸３６の角度θ’6(i+5) を角度θ6(i+5) に置き換えてエンドエフェクタ３９を作動させた場合に，一般的には振動などの動作上の不都合が生じないと考えられる範囲内で定められた角度差の値とする。即ち，回避した経路上の補間点が，特異点を経由した元の経路上の補間点と見做せる範囲内に戻ったといえる程に充分近づいたかどうかを判断する。
【0074】
従って，差分の値が所定の範囲を超える場合（ステップＳ３１２；ＮＯ），主制御部１１はステップＳ３１０の復帰処理を終了して，ステップＳ１４に進む（図２参照）。差分の値が所定の範囲以内の場合（ステップＳ３１２；ＹＥＳ），主制御部１１はＪ６軸３６の角度θ’6(i+5) を角度θ6(i+5) に置き換える際の速度を算出する（ステップＳ３１３）。ここでは，特異点を経由した次の補間点のＪ６軸３６の角度θ6(i+5) と特異点を回避した経路の補間点のＪ６軸３６の角度θ’6(i+4) との差分に基づいて，Ｊ６軸３６の速度を算出する。
【0075】
そして，主制御部１１は，Ｊ６軸３６の速度の許容範囲を算出する（ステップＳ３１４）。例えば，特異点を回避した経路の補間点のＪ６軸３６の速度の値に所定の値や所定の割合を加減算して得られる値を上限・下限として，速度の許容範囲を算出する。尚，この各軸の速度の許容範囲は，エンドエフェクタ３９の作動条件（例えば，溶接する際の温度，塗装する際の塗装膜の厚さなどの条件）や多関節型ロボットが動作する際に振動などの異常動作を起こさないための条件など，作業対象物の品質に影響を及ぼさない範囲内で予め定めておいても良い。
【0076】
続いて，主制御部１１はＪ６軸３６の速度が所定の許容範囲内か否かを判別する（ステップＳ３１５）。また，この許容範囲は，エンドエフェクタ３９を動かした場合に振動などの動作上の不都合が生じない範囲で予め定められた値とする。ここで，Ｊ６軸３６の速度が所定の許容範囲内の場合（ステップＳ３１５；ＹＥＳ），主制御部１１は次の補間点をＰ’i+5 からＰi+5 に変更して，Ｊ６軸３６の角度をθ6(i+5)にし，これに対応する他の全ての駆動軸Ｊ１軸３１〜Ｊ５軸３５の角度を算出する（ステップＳ３１６）。主制御部１１は算出された全ての駆動軸Ｊ１軸３１〜Ｊ６軸３６の角度を記憶部１２に記憶させる。更に，主制御部１１はＪ６軸３６の速度抑制処理中を示すフラグを立ち下げる（ステップＳ３１７）。
【0077】
一方，Ｊ６軸３６の速度が所定の許容範囲外の場合（ステップＳ３１５；ＮＯ），主制御部１１は角度θ6(i+5) と角度θ’6(i+5) との中間の角度θ”6(i+5) をＪ６軸３６の角度にし，このＪ６軸３６の角度θ”6(i+5) を含む補間点を次の補間点Ｐ”i+5 にし（ステップＳ３１８），これに対応する他の全ての駆動軸Ｊ１軸３１〜Ｊ５軸３５の角度を算出する（ステップＳ３１９）。即ち，主制御部１１は，Ｊ６軸３６の角度θ’6(i+5) を角度θ6(i+5) に置き換える際に，該駆動軸Ｊ６軸３６の角度をθ’6(i+5) からθ6(i+5) に急激に変化させることなく，所定の変動範囲内で徐々に変動させる。
【0078】
更に主制御部１１は算出された全ての駆動軸Ｊ１軸３１〜Ｊ６軸３６の角度を記憶部１２に記憶させる。（Ｊ４軸３４の速度抑制処理からの復帰処理）
ステップＳ３２０にあたるＪ４軸３４の速度抑制処理からの復帰処理は，上述したＪ６軸３６の速度抑制処理からの復帰処理（ステップＳ３１１〜Ｓ３１９）と同様の処理をするため説明を省略する。
【0079】
（Ｊ４軸３４・Ｊ６軸３６の速度抑制処理からの復帰処理）ステップＳ３３０にあたるＪ４軸３４・Ｊ６軸３６の速度抑制処理からの復帰処理は，それぞれＪ４軸３４の復帰処理とＪ６軸３６の復帰処理との双方を共に実施する以外はＪ６軸３６の速度抑制処理からの復帰処理と同様のため説明を省略する。
以上に示した制御方法により，多関節型ロボットＸの主制御部１１が中心となって，変動を抑制するための重み付けが大きい特定成分（トーチ傾斜角Ｒx，トーチ前進角Ｒy，トーチ回転角Ｒzのいずれか一つ又は二つ）の変動を抑えつつ特異点を回避してマニピュレータ本体３０に備え付けられたエンドエフェクタ３９の位置及び姿勢を作動させることにより，本来の教示ポイントに基づく補間点を大きく外れることがなく，且つエンドエフェクタ３９の移動速度を変更せずに全ての軸の速度が許容範囲内に入る作業軌道を得ることができる。
【0080】
例えば，図２４に示したように，Ｊ５軸３５が０度付近で，Ｊ４軸３４とＪ６軸３６との角度が急激に変化する（つまり速度が許容範囲外になる）特異点がある場合に，上述した速度抑制処理を実行することによって，トーチ傾斜角Ｒx，トーチ前進角Ｒyの変動を抑制しつつ，図９に示すように，Ｊ４軸３４とＪ６軸３６との角度の変動を緩やかにして，Ｊ４軸３４とＪ６軸３６との速度を許容範囲内の速度にすることができる。
【0081】
また，図１０に示すように，速度制限をしない場合（一点鎖線）には特異点付近でＪ４軸３４の速度が急激に変化するが，単に速度制限をした場合（実線）と本発明の制御方法を実施した場合（破線）とは速度が許容範囲内に抑えられる。但し，図１１に示すように単に速度制限をした場合には，トーチ前進角やトーチ傾斜角の姿勢変動角が大きく変動してしまう。
【0082】
しかしながら，図１２に示すように，本発明の制御方法を実施すると，重み付けが小さい（速度を抑制する必要のない）トーチ回転角は大きく変動するが，重み付けが大きいトーチ傾斜角とトーチ前進角との姿勢変動角の変動を抑制することができる。また，図１３（Ａ）に示すように，Ｊ１軸３１が上下方向に駆動し，Ｊ２軸３２とＪ３軸３３との駆動する方向が同じ方向でもよい，更に図１３（Ｂ）に示すようにＪ１軸３１，Ｊ２軸３２が左右方向に駆動し，Ｊ３軸３３が上下方向に駆動するものでもよい。尚，駆動軸が上下方向に駆動する場合には，各駆動軸を上下させるためのモータが回転する角度の差分のみならず，各駆動軸が上下する移動距離の差分を元に速度を算出してもよい。
【0083】
尚，上記の説明では，速度が許容範囲を超える場合に，他の軸の変動量を増やすことにより該軸の速度を抑制する方法について述べたが，上記の説明で得られた速度の変化量（加速度）が，所定の許容範囲を超える場合に，該軸の変動量を抑して，加速度が許容範囲に入るようにする各軸の角度のデータを算出してもよい。手段としては，速度をその変化量である加速度に置き換えて計算すれば良い。
【0084】
例えば，上記ステップＳ１２３において，主制御部１１が，速度の変わりに加速度を算出し，上記ステップＳ１３１において，主制御部１１が，加速度の許容範囲を算出し，上記ステップＳ１３２において，主制御部１１が，ステップＳ１２３により算出された加速度が，ステップＳ１３１により算出された加速度の許容範囲外かを判別する。そして，許容範囲外である場合には，加速度が前記許容範囲に入るようにする各軸の角度のデータを算出する。このとき加速度の候補の算出時に用いる前記所定の値Ｄや前記所定の値Ｇは，事前に定められた許容範囲内の加加速度を保っていることが分かっている加速度の変化分の絶対値などである。尚，速度を加速度に置き換え，加速度を加加速度（躍度）に置き換えるだけなので，ここではその詳細な説明を省略する。
【実施例1】
【0085】
本実施例１では，前記実施の形態と異なる多関節型ロボットＸの制御方法について説明する。前記実施の形態で説明した手法により多関節型ロボットＸを制御すると，前記Ｊ５軸３５の角度の符号が作業開始位置及び作業終了位置で異符号になる場合でも，該Ｊ５軸３５の角度が０°を通過しないように制御されるため，該作業終了位置における該Ｊ５軸３５の角度が本来の目標値と異なる。この場合には，例えば図１５に示すように，残りのＪ４軸３４やＪ６軸３６の角度が急激に変化するおそれや動作限界角度（例えば±１８０°）
を超えるおそれがある。
【0086】
また，多関節型ロボットＸにおいて，一つの姿勢データから求められる逆運動学問題の解は２つ存在し，例えば図１５に示すように，Ｊ６軸３６の角度の変化量が急激に増減する場合には，前記Ｊ６軸３６の別解が該Ｊ６軸３６の現在の角度に近づくこととなる。そこで，本実施例１では，逆運動学問題により求まる２つの解を利用することにより，Ｊ４軸３４やＪ６軸３６の急激な変化を防止するための構成について説明する。
【0087】
ここに，図１４は，多関節型ロボットＸの他の制御方法を説明するためのフローチャートである。なお，前記実施の形態で説明した図２の処理内容と同様の処理手順については同じ符号を付してその詳細な説明を省略する。まず，本実施例１に係る構成でも，前記ステップＳ１３で速度抑制処理が必要であると判断されるまでの間は（Ｓ１３のＮｏ側），主制御部１１が，エンドエフェクタ３９を次の補間点に動かすためのＪ１軸３１〜Ｊ６軸３６の角度として，Ｊ５軸３５の角度の符号が前記作業開始位置における該Ｊ５軸３５の角度と同符号となる逆運動学問題の解を算出する（Ｓ１２）。なお，ステップＳ１２では，その算出されたＪ４軸３４〜Ｊ６軸３６に対応する速度も算出される。
【0088】
また，続くステップＳ１４において，主制御部１１は，駆動指示部１３を制御することにより，エンドエフェクタ３９を次の補間点に動かすためのＪ１軸３１〜Ｊ６軸３６の角度として，Ｊ５軸３５の角度の符号が前記作業開始位置における該Ｊ５軸３５の角度と同符号となる解を記憶部１２から読み出して，マニピュレータ本体３０の各駆動軸のアクチュエータに出力させる。これにより，Ｊ１軸３１〜Ｊ６軸３６は，エンドエフェクタ３９が作業終了位置における位置及び姿勢に移動するまで（Ｓ１５のＹｅｓ側），Ｊ５軸３５の角度の符号が前記作業開始位置における該Ｊ５軸３５の角度と同符号となる解を目標値として動作することとなる（Ｓ１５のＮｏ側）。
【0089】
（ステップＳ４１１〜Ｓ４１２）一方，前記ステップＳ１３で速度抑制処理が必要であると判断された場合（Ｓ１３のＹｅｓ側），主制御部１１は，続くステップＳ４１１において，前記ステップＳ１２と同様に，次の補間点におけるＪ１軸３１〜Ｊ６軸３６の角度とＪ４軸３４〜Ｊ６軸３６の速度を算出する。なお，ステップＳ４１１では，その算出されたＪ４軸３４〜Ｊ６軸３６に対応する速度も算出される。
【0090】
但し，このとき，主制御部１１は，エンドエフェクタ３９を次の補間点に動かすためのＪ１軸３１〜Ｊ６軸３６の角度として求められる逆運動学問題の２つの解のうち，Ｊ５軸３５の角度の符号が前記作業開始位置における該Ｊ５軸３５の角度と異符号となる逆運動学問題の解（別解）を算出する。ここに，逆運動学問題の２つの解として求められるＪ４軸３４，Ｊ５軸３５，Ｊ６軸３６の角度をθ４１，θ５１，θ６１とθ４１’，θ５１’，θ６１’とすると，その値には，θ４１−θ４１’＝±π，θ５１＋θ５１’＝０，θ６１−θ６１’＝±πとの関係がある。例えば，（θ４１，θ５１，θ６１）＝（９０°，７５°，０°）であるとき，（θ４１’，θ５１’，θ６１’）＝（−９０°，−７５°，１８０°）となる。
【0091】
このように，本実施例１において，主制御部１１は，Ｊ４軸３４やＪ６軸３６の速度が前記許容範囲内である場合は，Ｊ５軸３５の次の補間点における角度が作業開始位置における該Ｊ５軸３５の角度と同符号になる逆運動学問題の解を採用し，前記許容範囲外である場合は，Ｊ５軸３５の次の補間点における角度が作業開始位置における該Ｊ５軸３５の角度と異符号になる逆運動学問題の解を採用する。ここに，係る処理が角度算出工程の一例であり，係る処理を実行するときの主制御部１１が角度算出手段に相当する。
【0092】
そして，続くステップＳ４１２において，主制御部１１は，前記ステップＳ１３と同様に，前記ステップＳ４１２で算出されたＪ４軸３４，Ｊ６軸３６の速度が許容範囲外であるかや，Ｊ４軸３４，Ｊ６軸３６についての速度抑制処理が実行中であるかなど，該速度抑制処理の必要性を判断する。ここで，速度抑制処理が必要であると判断されると（Ｓ４１２のＹｅｓ側），処理はステップＳ４１３に移行し，速度抑制処理が必要でないと判断されると（Ｓ４１２のＮｏ側），処理はステップＳ４１４に移行する。
【0093】
（ステップＳ４１３）前記ステップＳ４１３では，図５に示した前記ステップＳ２０と同様に，Ｊ４軸３４及びＪ６軸３６のいずれか一方又は両方の速度を抑制するための速度抑制処理が実行される。但し，このとき，主制御部１１は，エンドエフェクタ３９を次の補間点に動かすためのＪ１軸３１〜Ｊ６軸３６の角度として求められる逆運動学問題の２つの解のうち，Ｊ５軸３５の角度の符号が前記作業開始位置における該Ｊ５軸３５の角度と異符号となる解を採用するため，Ｊ１軸３１〜Ｊ６軸３６はその解を目標値として動作することになる。なお，前記ステップＳ４１３における処理内容は，Ｊ４軸３４やＪ６軸３６の角度が異なるだけであり，その他は前記ステップＳ２０と同様であるためその説明を省略する。
【0094】
（ステップＳ４１４〜Ｓ４１５）そして，ステップＳ４１４では，前記ステップＳ１４と同様に，主制御部１１が，駆動指示部１３を制御することにより，エンドエフェクタ３９を次の補間点に動かすためのＪ１軸３１〜Ｊ６軸３６の角度が記憶部１２から読み出されて，マニピュレータ本体３０の各駆動軸のアクチュエータに出力される。このとき，Ｊ１軸３１〜Ｊ６軸３６の角度は，前記ステップＳ４１１又は前記ステップＳ４１３において，Ｊ５軸３５の角度の符号が前記作業開始位置における該Ｊ５軸３５の角度と異符号となる解を目標値として算出されたものである。従って，Ｊ５軸３５の角度は０°を通過して遷移することとなる。
【0095】
その後，Ｊ１軸３１〜Ｊ６軸３６は，エンドエフェクタ３９が作業終了位置における位置及び姿勢に移動するまで（Ｓ４１５のＹｅｓ側），Ｊ５軸３５の角度の符号が前記作業開始位置における該Ｊ５軸３５の角度と異符号となる解を目標値として動作することとなる（Ｓ４１５のＮｏ側）。ここに，図１５は，Ｊ５軸３５の角度の符号が作業開始位置及び作業終了位置で異なる場合（プラスからマイナス，マイナスからプラスに変化する場合），即ち０°を通過する場合に，従来の特異点を経由する経路のＪ４軸Ｊ５軸Ｊ６軸の動作結果の一例を示している。図１５に示す例では，Ｊ５軸３５の角度が０°を通過しないように制御され，作業終了位置におけるＪ５軸３５の角度がプラスとなっているため，他のＪ４軸３４やＪ６軸３６の角度の変化量が大きくなっていることがわかる。
【0096】
一方，図１６は，Ｊ５軸３５の角度の符号が作業開始位置及び作業終了位置で異なる場合に，本実施例１に係る多関節型ロボットＸの制御手法を採用した結果の一例を示している。図１６に示す例では，Ｊ５軸３５に０°を通過させることにより，他のＪ４軸３４やＪ６軸３６の角度の変化量が抑制されていることがわかる。さらに，図１７は，本実施例１に係る多関節型ロボットＸの制御手法を採用せずに等速動作させた場合のエンドエフェクタ３９の姿勢データの変化を示しており，図１８は，本実施例１に係る制御手法を採用した場合のエンドエフェクタ３９の姿勢データの変化を示している。なお，図１７，図１８において縦軸は本来の補間点におけるエンドエフェクタ３９の姿勢データに対する変動量，横軸は時間を示している。
【0097】
図１７に示すように，本実施例１に係る制御手法を採用しない場合には，傾斜角及び前進角の変動が大きいが，図１８に示すように，本実施例１に係る制御手法を採用して傾斜角及び前進角の重み付け（変動の抑制量の大きさ）を大きく設定しておけば，溶接に不要な回転角の精度を犠牲にして傾斜角及び前進角の変動を抑制することができる。
【実施例2】
【0098】
また，前記実施の形態で説明したように，多関節型ロボットＸにおいて，前記作業開始位置から前記作業終了位置の間でＪ５軸３５の角度が０°近傍となる特異点を通過しないように制御する場合には，前記作業開始位置及び前記作業終了位置におけるＪ５軸３５の角度の符号が異符号である場合に，該作業終了位置におけるＪ５軸３５の角度が所望の角度と異なってしまうことになる。
【0099】
一方，前記実施例１で説明したように，多関節型ロボットＸにおいて，前記作業開始位置から前記作業終了位置の間でＪ５軸３５の角度が０°近傍を通過するように制御する場合には，前記作業開始位置及び前記作業終了位置におけるＪ５軸３５の角度の符号が同符号である場合に，該作業終了位置におけるＪ５軸３５の角度が所望の角度と異なってしまうことになる。
【0100】
これらの場合には，例えば多関節型ロボットＸのＪ１軸３１〜Ｊ６軸３６やエンドエフェクタ３９などに接続された配線が絡まることや，その後の操作によりＪ４軸３４〜Ｊ６軸３６の駆動限界点を超えるおそれがあること等が問題となる。そこで，前記作業開始位置及び前記作業終了位置における前記第１関節駆動系のＪ５軸３５の符号が同符号であるか異符号であるかに応じて，前記実施の形態に係る制御手法と前記実施例１に係る制御手法とを切り替えて実行することが考えられる。
【0101】
ここに，図１９は，本実施例２に係る多関節型ロボットＸの制御方法を説明するためのフローチャートである。なお，前記実施の形態及び前記実施例１と同様の処理手順については同じ符号を付してその説明を省略する。具体的に，主制御部１１は，前記ステップＳ１３のＹｅｓ側と前記ステップＳ２０及び前記ステップＳ４１１との間において，前記作業開始位置及び前記作業終了位置における前記第１関節駆動系のＪ５軸３５の角度の符号が同符号であるか異符号であるかを判断するための処理（ステップＳ５１１のＹｅｓ側）を実行する。
【0102】
そして，主制御部１１は，前記作業開始位置及び前記作業終了位置における前記第１関節駆動系のＪ５軸３５の角度の符号が同符号である場合は（ステップＳ５１１のＹｅｓ側），前記ステップＳ２０に移行する。この場合は，前記第１関節駆動系のＪ５軸３５の角度の符号が前記作業開始位置における角度と同符号である逆運動学問題の解が採用される。
【0103】
一方，前記作業開始位置及び前記作業終了位置における前記第１関節駆動系のＪ５軸３５の角度の符号が異符号である場合（ステップＳ５１１のＮｏ側），主制御部１１は，処理をステップＳ４１１に移行させる。この場合は，前記第１関節駆動系のＪ５軸３５の角度の符号が前記作業開始位置における角度と異符号である逆運動学問題の解が採用される。即ち，前記作業開始位置及び前記作業終了位置における前記第１関節駆動系のＪ５軸３５の角度の符号が異符号である場合は，Ｊ５軸３５が０°近傍に近づいてＪ４軸３４やＪ６軸３６の速度が許容範囲内である間は，前記第１関節駆動系のＪ５軸３５の角度の符号が前記作業開始位置における角度と同符号である逆運動学問題の解が採用され，該Ｊ５軸３５が０°近傍に近づいてＪ４軸３４やＪ６軸３６の速度が許容範囲外となった場合は，前記第１関節駆動系のＪ５軸３５の角度の符号が前記作業開始位置における角度と異符号である逆運動学問題の解が採用される。係る処理が角度算出工程の一例であり，係る処理を実行するときの主制御部１１が角度算出手段に相当する。
【0104】
これにより，教示された作業終了位置におけるＪ５軸３５の角度の符号がその教示通りの符号と一致することになるため，例えば多関節型ロボットＸのＪ１軸３１〜Ｊ６軸３６やエンドエフェクタ３９などに接続された配線の絡まりや，その後の操作によりＪ４軸３４〜Ｊ６軸３６の駆動限界点を超えるおそれ等を防止することができる。
【実施例3】
【0105】
また，上記説明では，多関節型ロボットＸのエンドエフェクタ３９の作業点が１つの場合について説明したが，図２０に示すように，２つの作業点を有するエンドエフェクタ３９Ａを用いた多関節型ロボットＸ１を制御してもよい。多関節型ロボットＸ１は，エンドエフェクタ３９Ａを備え，エンドエフェクタ３９以外の他の構成は多関節型ロボットＸと同様である。また，前記多関節型ロボットＸ１の制御は，前記多関節型ロボットＸの制御処理とは，図５に示したステップＳ２１５，Ｓ２１６（ステップＳ２２５，Ｓ２２６及びＳ２３５，２３６を含む）の処理以外は同様の処理をする。尚以下の説明では，多関節型ロボットＸ１と多関節型ロボットＸとの同一の構成及び同一の処理は説明を省略する。
まず，図１に示した多関節型ロボットＸとの構成が異なる部分について説明する。
【0106】
エンドエフェクタ３９Ａは，マニピュレータ本体３０の先端（Ｊ６軸３６の先）に取り付けられ，作業対象物に対して効果を与える２つの作業点を有する効果器であり，作業点として溶接器（トーチ），塗装器，工具，捕獲器，センサなどがある。尚，２つの作業点を備えるトーチをタンデムトーチという。次に，図５のフローチャートを参照して，本発明の実施形態と処理が異なる速度抑制処理のステップＳ２１５，Ｓ２１６の処理を中心に説明する。
【0107】
まず，主制御部１１は，ステップＳ２０１を実行する。ここでは，Ｊ６軸３６の速度が許容範囲外であるか又は，Ｊ６軸３６の速度抑制処理中を示すフラグが立っている場合（ステップＳ２０１；Ｊ６軸が範囲外）に該当しているものとする。更にステップＳ２１１〜ステップＳ２１４を実行する。次に，前記ステップＳ２１５において，主制御部１１は，前記ステップＳ２１３において変換された溶接線座標系Σlineにおけるエンドエフェクタ３９Ａの姿勢を示すデータの各成分（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）の中から変動を抑制すべき成分の情報（重み付け情報）を取得する。その際，この実施例１では，エンドファクタ３９Ａは作業点が２つの溶接トーチであり，溶接トーチを用いた溶接作業の性質上，トーチの軸周りの回転角とトーチの進行速度とは重要である。そのため，この実施例１では，標準設定として，トーチ回転角Ｒｚを重み付けが大きい（抑制すべき）成分とし，トーチ前進角Ｒｙを重み付けが小さい（抑制しない）成分として予め設定される。
【0108】
次に，前記ステップＳ２１６において，主制御部１１は算出された次の補間点における複数の候補の中から前述したステップＳ２１５により取得された抑制するための重み付けが大きい特定成分（トーチ傾斜角Ｒx，トーチ前進角Ｒy，トーチ回転角Ｒzのいずれか一つ又は二つ）の変動が最も少ない候補を選択する。例えば，トーチ回転角Ｒｚの変動を抑制し，トーチ前進角Ｒｙの変動を許容する場合，重み付け係数Ａ＝０．２，Ｂ＝０，Ｃ＝１や，重み付け係数Ａ＝０，Ｂ＝０，Ｃ＝１（即ち特定成分は，トーチ傾斜角及びトーチ回転角，或いはトーチ回転角である）とし，前記ステップＳ２１３によって算出された各候補（１’）〜（３’）に対して下記の（１”）〜（３”）の演算をする。（１”）[Ｆa＝Ａ(Ｒｘ−Ｒxa )2＋Ｂ(Ｒｙ−Ｒya )2＋Ｃ(Ｒｚ−Ｒza )2]
（２”）[Ｆb＝Ａ(Ｒｘ−Ｒxb )2＋Ｂ(Ｒｙ−Ｒyb )2＋Ｃ(Ｒｚ−Ｒzb )2]
（３”）[Ｆc＝Ａ(Ｒｘ−Ｒxc )2＋Ｂ(Ｒｙ−Ｒyc )2＋Ｃ(Ｒｚ−Ｒzc )2]
（１”）〜（３”）において，特異点を経由する経路上の次の補間点のエンドエフェクタ３９Ａの姿勢角と，特異点を回避する経路上の次の補間点の各候補のエンドエフェクタ３９Ａの姿勢角との差が求められるので，Ｆa〜Ｆcの中で最も値が小さい（変動が小さい）ものを選択し，選択された候補の駆動軸Ｊ１軸３１〜Ｊ６軸３６の角度を特異点を回避する動作時の各軸の角度とする。
【0109】
続いて，主制御部１１はステップＳ２１７以降の処理をする。尚，実施形態と同様に，速度が許容範囲外と判別された各駆動軸に対応して，ステップＳ２１１〜Ｓ２１６の代わりに，ステップＳ２２１〜Ｓ２２６若しくはステップＳ２３１〜Ｓ２３６を実行する。以上に示した制御方法により，多関節型ロボットＸ１の主制御部１１が中心となって，抑制するための重み付けが大きいトーチ傾斜角Ｒｘ，トーチ回転角Ｒzの変動を抑えつつ特異点を回避してマニピュレータ本体３０に備え付けられたエンドエフェクタ３９Ａの位置及び姿勢を作動させ，本来の教示ポイントに基づく補間点を大きく外れることがなく，且つエンドエフェクタ３９Ａの移動速度を変更せずに全ての軸の速度が許容範囲内に入る作業軌道を得ることができる。
【0110】
また，図２１に示すように実施例１の制御方法を実施すると，抑制するための重み付けが小さいトーチ前進角Ｒｙは大きく変動するが，抑制するための重み付けが大きい回転角Ｒｚの姿勢変動角を抑制することができる。
【実施例4】
【0111】
上記説明では，多関節型ロボットＸの第２関節駆動系の駆動軸がＪ１軸３１〜Ｊ３軸３３の３つの軸を備えたマニピュレータ本体３０の制御方法について説明したが，第２関節駆動系の駆動軸が４以上のマニピュレータであってもよい。尚以下の説明では，多関節型ロボットＸと多関節型ロボットＸ２との同一の構成及び同一の処理は説明を省略する。まず，図１に示した多関節型ロボットＸと図２２に示される多関節型ロボットＸ２との構成が異なる部分について説明する。
【0112】
図２２に示される多関節型ロボットＸ１は，制御部１０，操作部２１，マニピュレータ本体３０Ａ等を備えている。マニピュレータ本体３０Ａは，第１関節駆動系の３つの駆動軸のＪ４軸３４，Ｊ５軸３５，Ｊ６軸３６と，第２関節駆動系の４つの駆動軸のＪ１軸３１，Ｊ２軸３２，Ｊ３軸３３，Ｊ７軸３７とを備える。Ｊ７軸３７は，Ｊ１軸３１〜Ｊ６軸３６と同様に，電気モータ等により構成され，駆動指示部１３からの指示により正負の方向に回転駆動し，複数の軸の回転駆動が連関することによって，人間の手首や腕の動きを実現する。その中でも上記Ｊ７軸３７を含む第２関節駆動系の４つの駆動軸は，人間の腕に相当する動作を行い，エンドエフェクタ３９の位置を移動する。
【0113】
次に，図２〜図６のフローチャートを参照して，本発明の実施形態と処理が異なる部分について説明する。実施形態においては，補間点の位置及び姿勢Ｐi（Ｘi，Ｙi，Ｚi，αi，βi，γi）から逆運動学問題の解として各Ｊ１軸３１〜Ｊ６軸３６の角度（θ1，θ2，θ3，θ4，θ5，θ6）を算出し，補間点における各Ｊ１軸３１〜Ｊ６軸３６の角度（θ1，θ2，θ3，θ4，θ5，θ6）から順運動学問題の解として位置及び姿勢Ｐi（Ｘi，Ｙi，Ｚi，αi，βi，γi）を算出したが，実施例４では，補間点の位置及び姿勢Ｐi（Ｘi，Ｙi，Ｚi，αi，βi，γi）から逆運動学問題の解として各Ｊ１軸３１〜Ｊ７軸３７の角度（θ1，θ2，θ3，θ4，θ5，θ6，θ7）を算出し，補間点における各Ｊ１軸３１〜Ｊ７軸３７の角度（θ1，θ2，θ3，θ4，θ5，θ6，θ7）から順運動学問題の解として位置及び姿勢Ｐi（Ｘi，Ｙi，Ｚi，αi，βi，γi）を算出するように，角度の変数の数が６か７かの点が異なる。
【0114】
前記ステップ１２２において，主制御部１１は，逆運動学問題の解を求めることによって，次の補間点の位置及び姿勢Ｐi+1 （Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，αi+1 ，βi+1 ，γi+1 ）から各Ｊ１軸３１〜Ｊ７軸３７の角度（θ1，θ2，θ3，θ4，θ5，θ6，θ7）を算出して，記憶部１２に記憶させる。次に，前記ステップＳ１２３以降と同様の処理を実行する。ここでは，速度抑制処理が必要であると判別され（Ｓ１３；ＹＥＳ），Ｊ６軸３６の速度が許容範囲外であるか又は，Ｊ６軸３６の速度抑制処理中を示すフラグが立っている場合（ステップＳ２０１；Ｊ６軸が範囲外）に該当しているものとする。ステップＳ２１１において，Ｊ６軸３６の角度の候補が算出されたものとする。
【0115】
次に，前記ステップＳ２１２において，主制御部１１は算出された次の補間点におけるＪ６軸３６の角度の各候補に対応する他の駆動軸Ｊ１軸３１〜Ｊ５軸３５，Ｊ７軸３７の角度を算出する。例えば，Ｊ６軸３６の角度の候補（θ6a ，θ6b ，θ6c ）毎に，前記ステップＳ１２２において算出された次の補間点Ｐi+1 のエンドエフェクタ３９の位置（Ｘi+1，Ｙi+1 ，Ｚi+1 ）と，第１関節駆動系のＪ４軸３４，Ｊ５軸３５の角度（θ4，θ5）と用いて，第２関節駆動系のＪ１軸３１〜Ｊ３軸３３，Ｊ７軸３７の角度（θ1，θ2，θ3，θ7）を再算出する。（１）候補θ6a に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4，θ5，θ6a ］から［θ1a ，θ2a，θ3a ，θ4，θ5，θ6a ，θ7］を算出する。
（２）候補θ6b に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4，θ5，θ6b ］から［θ1b ，θ2b，θ3b ，θ4，θ5，θ6b ，θ7］を算出する。
（３）候補θ6c に対して，［Ｘi+1 ，Ｙi+1 ，Ｚi+1 ，θ4，θ5，θ6c ］から［θ1c ，θ2c，θ3c ，θ4，θ5，θ6c ，θ7］を算出する。
【0116】
次に，前記ステップＳ２１３において，主制御部１１は算出された特異点を回避する系の次の補間点における前記溶接線座標系のエンドエフェクタ３９の姿勢を示すデータの各成分（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）を算出する。例えば，上記の（１）〜（３）の候補θ6a 〜θ6c に対して，
（１’）［θ1a ，θ2a ，θ3a ，θ4，θ5，θ6a ，θ7］から［Ｒxa ，Ｒya ，Ｒza ］を算出する。
（２’）［θ1b ，θ2b ，θ3b ，θ4，θ5，θ6b ，θ7］から［Ｒxb ，Ｒyb ，Ｒzb ］を算出する。
（３’）［θ1c ，θ2c ，θ3c ，θ4，θ5，θ6c ，θ7］から［Ｒxc ，Ｒyc ，Ｒzc ］を算出する。
【0117】
次に，ステップＳ２１４において，主制御部１１は算出された特異点を経由する系の次の補間点Ｐi+1 における前記溶接線座標系のエンドエフェクタ３９の姿勢を示すデータの各成分（トーチ傾斜角Ｒｘ，トーチ前進角Ｒｙ，トーチ回転角Ｒｚ）を算出する。続いて，前記ステップＳ２１５以降の処理を実行する。尚，実施形態と同様に，速度が許容範囲外と判別された各駆動軸に対応して，ステップＳ２１１〜Ｓ２１６の代わりに，ステップＳ２２１〜Ｓ２２６若しくはステップＳ２３１〜Ｓ２３６を実行する。
【0118】
以上に示した制御方法により，第２関節駆動系の軸が４軸あるマニピュレータ本体３０Ａを備えた多関節型ロボットＸ２の主制御部１１が中心となって，抑制するための重み付けが大きい特定成分（トーチ傾斜角Ｒx，トーチ前進角Ｒy，トーチ回転角Ｒzのいずれか）の変動を抑えつつ特異点を回避してエンドエフェクタ３９の位置及び姿勢を作動させ，本来の教示ポイントに基づく補間点を大きく外れることがなく，且つエンドエフェクタ３９の移動速度を変更せずに全ての軸の速度が許容範囲内に入る作業軌道を得ることができる。
【0119】
このように実施例４の制御方法は，補間点Ｐiにおける位置及び姿勢（Ｘi，Ｙi，Ｚi，αi，βi，γi）から第１関節駆動系の各軸の角度（θ4，θ5，θ6）を逆運動学問題の解として得られ，且つ，補間点Ｐiにおける位置及び第１関節駆動系の各軸の角度（Ｘi，Ｙi，Ｚi，θ4，θ5，θ6）から第２関節駆動系の各軸の角度（θ1，θ2，θ3，θ7）を逆運動学問題の解として得られる多数関節軸を備えたマニピュレータの制御に有用である。
【0120】
そのため，上述した６軸，７軸のマニピュレータを備えた多関節型ロボットに限らず，それ以上の軸を備えた多関節型ロボットにも適用することができる。
【実施例5】
【0121】
また，多関節型ロボットＸの制御部１０は，専用のシステムによらず，図２９に示されるネットワークに接続された通常のコンピュータシステムＹを用いて実現可能である。図２９のコンピュータシステムＹは，例えば，主制御部１１，記憶部１２，外記憶部１０３，入出力部１０４，表示部１０５，送信部１０６，内部バス１０９を備えている。主制御部１１はＣＰＵ（Central Processing Unit）を含み，記憶部１２はＲＡＭ（Random Access Memory）等を含む。主制御部１１は，記憶部１２に記憶されているプログラムを実行し，多関節型ロボットＸの制御装置である制御部１０を実現する。
【0122】
例えば，前記の処理を実行するためのプログラムを，コンピュータが読み取り可能な記録媒体（フレキシブルディスク，ＣＤ−ＲＯＭ，ＤＶＤ−ＲＯＭ等）に格納して配布し，当該プログラムをコンピュータにインストールすることにより，前記の処理を実行する多関節型ロボットＸの制御部１０を構成してもよい。また，インターネット等の通信ネットワーク上のサーバ装置が有する記憶装置に当該プログラムを格納しておき，通常のコンピュータシステムがダウンロード等することで多関節型ロボットＸの制御部１０を構成してもよい。
【0123】
以上，本発明の実施の形態について説明したが，設計上の都合やその他の要因によって必要となる様々な修正や組み合わせは，請求項に記載されている発明や発明の実施の形態に記載されている具体例に対応する発明の範囲に含まれると理解されるべきである。上述した実施形態では、例えば、第１関節駆動系のＪ４軸３４やＪ６軸３６の一方又は両方の速度や加速度が許容範囲外とならないように、Ｊ５軸３５の特異点を回避するという特異点回避の処理を行っている。ここで、特異点回避の処理を行っていることを外部に報知する手段を多関節型ロボットに設けてもよい。
【0124】
具体的には、図５に示す如く特異点回避のための処理として速度抑制処理を行っているとき、図３０に示すように、特異点回避の処理を行っていることを示す「特異点回避中」を、操作部２１（教示ペンダント）の表示画面Ｍに表示する。特異点回避の処理を行っていることを示す表示は、「特異点回避中」に限定されず、特異点回避の処理が行われていることが分かる表現であればどのような表現であってもよい。
【0125】
このように、特異点回避中であることを表示することによって、マニピュレータ本体３０が特異点回避中の状態（回避溶接状態）であるか、特異点回避中ではない状態（通常溶接状態）であるかをオペレータが溶接中に把握することができる。そのため、例えば、作業対象物（材料）の溶接後に品質検査を行う場合、回避溶接状態で溶接した箇所を注視して検査することができ、この検査結果に基づいて、回避溶接状態で溶接を行った時の溶接条件を適切に変更することができる。また、例えば、回避溶接状態におけるマニピュレータ本体３０の全体の動きを、オペレータが目視などで監視することもでき、この監視に基づいて、次の溶接作業などで、再度、マニピュレータ本体３０を動かすときに、マニピュレータ本体３０の全体の動作を見直すこともできる。
【0126】
なお、「特異点回避中」の表示の他に、処理プログラムが分かるように処理プログラムの内容（例えば、再生中プログラムの番号）を表示画面Ｍに表示したり、処理プログラムを実行している時間（再生時間）、エンドエフェクタ３９の動き（例えば、直線移動）を表示画面Ｍに表示してもよい。さらに、予め教示されたトーチの情報（例えば、トーチ傾斜角、トーチ前進角、トーチ回転角など）と、現在のトーチの情報とを表示画面Ｍに表示してもよい。
【0127】
これによれば、「特異点回避中」におけるマニピュレータ本体３０の詳しい状態を把握することができる。上述した例では、特異点回避の処理を行っていることを表示画面Ｍに表示させているが、これに代えて、特異点回避を行っていることをスピーカ等によって音で知らせてもよいし、ランプなどの点灯や点滅など光で知らせてもよい。また、特異点回避を行っていることは、多関節型ロボットの教示ペンダント２１とは別の外部の表示装置に表示してもよい。
【0128】
また、「特異点回避中」の時間を制御部１０の記憶部１２などに記憶してもよい。そして、例えば、溶接終了後に操作部２１（教示ペンダント）の表示画面Ｍに特異点回避中の時間履歴を表示し、オペレータなどが特異点回避中の時間履歴を確認できるようにしてもよい。
【符号の説明】
【0129】
Ｘ，Ｘ１，Ｘ２：多関節型ロボット１０：制御部
１１：主制御部
１２：記憶部
１３：駆動指示部
２１：操作部
３０，３０Ａ：マニピュレータ本体
３１：Ｊ１軸
３２：Ｊ２軸
３３：Ｊ３軸
３４：Ｊ４軸
３５：Ｊ５軸
３６：Ｊ６軸
３７：Ｊ７軸
３９，３９Ａ：エンドエフェクタ