JP2007136590A - 冗長関節部を有する冗長ロボットの制御装置および制御方法 - Google Patents

Abstract

【課題】 設定部分を目的とする固定位置に正確に維持したうえで、各関節部の角度を変更可能に冗長ロボットを制御する制御方法を提供する。
【解決手段】 第１〜第４関節部９〜１２のうちで冗長関節部を除く残余の関節部の数が３つであり、対象点ｐｈが３次元位置で固定される。対象点ｐｈの３次元位置（ｐｈｘ，ｐｈｙ，ｐｈｚ）と、第１〜第４関節部９〜１２の各角度ｑ１〜ｑ４を変数として対象点ｐｈの３次元位置を求めるための順変換関係式と、冗長関節部の冗長角度とに基づいて、対象点ｐｈが３次元位置で固定されるべき、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４をそれぞれ演算する。これによって未知数が３つの３元連立方程式を解析的に解くことができ、対象点ｐｈを予め定める３次元位置に配置した場合における、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を正確に求めることができる。
【選択図】 図１

Description

本発明は、冗長関節部を含む複数の関節部を有する多関節ロボットを制御する制御装置および多関節ロボットを制御する制御方法に関する。

７つ以上の複数の関節部を有する多関節ロボットの制御方法に関する従来技術として、たとえば特許文献１に開示されている。この従来技術では、冗長性を有する多関節ロボットに設定される設定部分が予め設定される固定位置に維持した状態で、ロボットを構成する複数の軸のうち、任意の１つの冗長関節部の微小移動が可能に設定される。操作者が冗長関節部を微小移動させた場合、ロボット制御装置は、設定部分が固定位置に維持されるように冗長関節部以外の残余の各関節部を微小移動させる。

特許文献１に開示される従来技術では、ヤコビ行列に基づいて、冗長関節部以外の残余の各関節部の移動量を決定する。ここで、ヤコビ行列は、設定部分の微小変化と、ロボット各関節部の微小変化との関係を示す関数である。従来技術のロボット制御装置は、設定部分の微小変化をゼロとし、ロボットの各関節部の微小変化を示す行列のうちで冗長設定関節部の微小変化を既知として、残余の各関節部の微小変化の解を求めることによって、設定部分を固定位置範囲に維持すべき、残余の各関節部の微小移動量を求める。

特許３３７９５４０号公報

従来技術では、複数の関節部のうちで任意の関節部を冗長関節部として選択可能に設定され、ヤコビ行列に基づいて、冗長関節部以外の残余の各関節部の微小移動量を求める。任意の関節部を冗長関節部として選択可能とすると、各関節部の移動量について、厳密解を求めることが困難であり、近似解を求めることになる。近似解を基に各関節部を決定すると、冗長設定関節部を変位移動させた場合に、目的とする固定位置に設定部分を正確に維持することができないという問題がある。

設定部分のずれ量は、冗長関節部を高速移動または長距離移動する場合に顕著となる。設定部分がずれてしまうと、ロボット動作に不具合が生じる場合がある。また設定部分の位置ずれを防ぐために収束演算の回数を増やした場合には、各関節部の移動量を短時間で求めることができない。

たとえば冗長性ロボットによって搬送対象物を搬送する場合、ロボットアームが障害物に衝突しないように位置教示を行う。位置教示時に、搬送対象物の位置姿勢を正確に維持したうえで、障害物近傍のアームを冗長関節部として微小移動させることができず、搬送不良が生じてしまうおそれがある。またたとえば冗長性を有するロボットについて、複数の関節部のうちから冗長関節部として選択する関節部を順次変更して、各冗長関節部について微小移動を繰返すと、設定部分のずれ方が複雑となり、設定部分の位置を前の状態に戻すことが困難となる。

したがって本発明の目的は、設定部分を目的とする固定位置に正確に維持したうえで、ロボットの各関節部の角度を変更可能に、冗長ロボットを制御する制御方法および制御装置を提供することである。

本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットの制御装置であって、
前記設定部分を固定すべき固定条件が入力される固定条件入力手段と、
冗長関節部が入力される冗長関節部入力手段であって、前記複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも１つが冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
固定条件入力手段によって入力される固定条件と、冗長角度入力手段によって入力される冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき複数の関節部の角度をそれぞれ演算する角度演算手段と、
前記角度演算手段で演算された演算結果を多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置である。

また本発明は、固定条件として、直列方向他端部の関節部に対する設定部分の３つの自由度が拘束される条件が入力され、
冗長関節部入力手段は、前記複数の関節部のうちで、直列方向一端部から直列方向他端部に向かって並ぶ２つの関節部のうちの１つの関節部と、直列方向他端部から直列方向一端部に向かって並ぶ２つの関節部のうちの１つの関節部とを冗長関節部として選択可能に構成されることを特徴とする。

また本発明は、角度演算手段は、入力前の状態から冗長角度入力手段に入力された冗長角度に冗長関節部を角変位させる間に、冗長関節部が可動限界に到達することを判断した場合、その可動限界に到達した状態の関節部の角度を演算することを特徴とする。

また本発明は、角度演算手段は、冗長角度入力手段に入力された冗長角度に冗長関節部が角変位する間に、冗長関節部が可動限界に到達した状態から脱出することを判断した場合、冗長関節部以外の関節部の角度が取り得る２つの組合せのうち、可動限界に到達する前に取り得た角度の組合せと異なる角度の組合せを演算することを特徴とすることを特徴とする。

また本発明は、前記複数の関節部の角度変化を設定部分の位置変化に変換するヤコビ行列と、冗長角度入力手段に入力される冗長角度とに基づいて、設定部分を固定条件で拘束して、冗長関節部を冗長角度に角変位するときに、複数の関節部の角変位する方向を予測し、予測結果を報知する予測報知手段をさらに備えることを特徴とする。

また本発明は、前記複数の関節部のいずれかが可動限界近傍に設定される警告範囲に到達したことを判断すると、判断結果を報知する警告報知手段をさらに備えることを特徴とする。

また本発明は、警告報知手段は、警告範囲に到達した関節部が、可動限界から遠ざかるような角変位方向を報知することを特徴とする。

また本発明は、隣接する２つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを含む多関節ロボットを制御することを特徴とする。

また本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ７つの関節部を含み、７つの関節部のうちで直列方向一端部から直列方向他端部に向かって並ぶ３つの関節部の回転軸線が一点で交差する３軸交点を形成する３つの交点形成関節部を有する７自由度の多関節ロボットの制御装置であって、
７つの関節部のうちで直列方向一端部の関節部に設定される設定部分を固定すべき位置および姿勢が固定条件として入力される固定条件入力手段と、
３つの交点形成関節部以外となる４つの基本関節部のうち１つが、冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
設定部分が固定すべき位置および姿勢となる状態での３軸交点の固定位置を演算する３軸交点位置演算手段と、
３軸交点位置演算手段によって演算される３軸交点の位置と、冗長関節部入力手段によって入力される冗長角度とに基づいて、４つの基本関節部の各角度を変数として３軸交点の位置を求めるための関係式を解いて、３軸交点が固定位置に位置するような、４つの基本関節部の角度をそれぞれ演算する基本関節部角度演算手段と、
注目関節部角度演算手段によって演算された演算結果と、固定条件入力手段によって入力される設定部分の位置および姿勢とに基づいて、３つの交点形成関節部の角度をそれぞれ演算する交点形成関節部角度演算手段と、
前記各角度演算手段で演算された演算結果とを多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置である。

また本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットを制御する制御方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも１つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と、
角度演算工程に従ってロボットを制御する制御工程とを備えることを特徴とする多関節ロボットの制御方法である。

また本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットにおける各関節部の角度を演算する演算方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも１つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と備えることを特徴とする多関節ロボットの各関節部の角度の演算方法である。

請求項１記載の本発明に従えば、多関節ロボットは、冗長性を有し、設定部分を予め定める固定条件で固定した状態で、各関節部の取り得る角度の組合せが複数通り存在する。本発明では、冗長関節部入力手段によって、冗長関節部が入力される。これによって複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致する。また固定条件入力手段によって、設定部分を固定すべき固定条件が既知となるとともに、冗長角度入力手段によって、冗長関節部の冗長角度が既知となる。

このように入力手段に各種情報が入力された場合、冗長関節部の冗長角度と、固定条件とを代入することで、複数の関節部のうちで、冗長関節部以外の各関節部の角度を解析的に求めることが可能な計算式が存在する。角度演算手段は、前記計算式を表わす計算プログラムを記憶し、その計算プログラムに従うことで、各入力手段に与えられた各種情報を前記計算式に代入することで、設定部分が固定条件で拘束される場合における、注目する関節部の角度を解析的にそれぞれ求めることができる。

出力手段は、冗長角度入力手段によって入力された冗長角度と、角度演算手段によって演算された残余の関節部の各角度との情報を多関節ロボットに与える。多関節ロボットは、出力手段から与えられた情報に基づいて、複数の関節部を角変位駆動する。これによって設定部分を予め定める固定条件で拘束するとともに、冗長関節部を冗長角度に保つように、多関節ロボットの各関節部を変位駆動することができる。ここで、残余の関節部の各角度は解析解、すなわち厳密解として求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。

このように本発明によれば、各関節部の角度の解析解をそれぞれ演算することができるので、近似解に基づいて各関節部の角度をそれぞれ演算する場合に比べて、設定部分を予め定める固定状態に正確に保ち、冗長関節部を冗長角度にすることができる。すなわち設定部分がずれることを防ぐことができる。たとえば入力前の状態から冗長関節部の角度を高速移動または長距離移動する場合であっても、設定部分の固定状態を正確に維持することができる。また解析解を求めることに付随して、各関節部の特異点について正確に判断することができ、利便性を向上することができる。

また、たとえば冗長関節部および冗長関節部に連なるアーム体が障害物に衝突することを回避するために、設定部分の位置を固定した状態で冗長関節部を角変位させることができる。この場合、設定部分の位置を正確に保ちつつ、障害物とアーム体との干渉を回避することができるので、設定部分の位置ずれに起因するロボットの動作不良を防ぐことができる。

請求項２記載の本発明に従えば、固定条件として、設定部分の３つの自由度が拘束される。３自由度の拘束として、たとえば直列方向他端部の関節部に対する、設定部分のｘ座標、ｙ座標、ｚ座標が決定される。またたとえば直列方向他端部の関節部に対する、ｘ座標、ｙ座標および他端部の関節部に設定されるＹ軸まわりの姿勢が決定される。

直列方向一端部側の２つの関節部のうちの１つの関節部と、直列方向他端部側の２つの関節部のうちの１つの関節部とを冗長関節部として選択可能とすることで、それら２つの冗長関節部の冗長角度を調整することで、設定部分を固定状態に維持したうえで、注目する複数の関節部が取り得ることが可能な全ての角度の組合せを演算可能となる。

このように本発明によれば、４つの関節部のうちの２つを冗長関節部として選択可能とすることで、各関節部が取り得ることが可能な全ての角度の組合せを求めることができるので、４つの関節部の全て冗長関節部として選択可能とする必要がなく、利便性を向上することができる。たとえば操作者に対する操作性を向上することができるとともに、角度演算手段が記憶する計算プログラムの数を減らすことができる。

請求項３記載の本発明に従えば、設定部分を固定状態に維持するための各関節部の角度を解析的に求めることができるので、冗長関節部が可動限界に到達したか否かを正確に判断することができる。角度演算手段は、冗長関節部が可動限界に到達することを判断すると、冗長関節部が可動限界に到達した状態の関節部の角度を演算する。また可動限界を超えるような冗長角度が入力されても、可動限界に到達した状態の関節部の角度を演算する。

このように本発明によれば、冗長関節部の可動範囲内となる状態の関節部の角度を演算する。これによって機構的に移動不可能な移動指令値を、多関節ロボットに与えることを防ぐことができ、関節部の損傷を防ぐことができる。また解析的に求められる特異点に基づくことによって、可動限界を正確に求めることができる。したがって角度演算手段は、冗長関節部が可動限界に到達したか否かを精度よく判断することができ、可動限界に近い領域まで冗長関節部を角変位させることができる。

請求項４記載の本発明に従えば、冗長角度入力手段化からの冗長角度の入力によって、冗長関節部が可動限界に到達した状態から脱出して、可動範囲内に移動する場合、角度演算手段は、可動限界に到達する前に取り得た角度の組合せと異なる角度の組合せを演算する。これによって、設定部分を固定した状態で、多関節ロボットが回転揺動状態となる場合に、冗長関節部として選択した関節部が揺動角変位する場合、冗長関節部が可動限界に到達する前と、可動限界から脱出した後とで、ロボットの形態を変更することができ、複数の関節部が取り得ることが可能な全ての角度の組合せを演算可能となる。したがって複数の関節部の全てを冗長関節部として選択する必要がなく、利便性を向上することができる。

このように本発明によれば、冗長関節部を１つ選択するだけで、複数の関節部が取り得ることが可能な全ての角度の組合せを演算可能となる。したがって複数の関節部の全てを冗長関節部として選択可能とする必要がなく、利便性を向上することができる。たとえば操作者に対する操作性を向上することができるとともに、角度演算手段が記憶する計算プログラムの数を減らすことができる。

請求項５記載の本発明に従えば、予測報知手段によって各関節部の角変位に関する予測結果を報知する。これによって操作者は、各関節部の予測情報、たとえば角変位方向および角度を知ることができ、利便性を向上することができる。たとえばロボットが障害物に衝突するか否かについて、操作者が確認しながら冗長関節部の冗長角度を指定する場合に、各関節部の動きを予測報知する。これによって操作者は、冗長角度を指定した場合に障害物に関節部が衝突するか否かを把握しやすく、利便性を向上することができる。また予測結果として示される情報は、実際に各関節部の角度を決定する場合に比べて、精度が要求されない。したがってヤコビ行列に基づく近似式で十分であり、ヤコビ行列に基づくことで、簡単にかつ短時間に予測結果を演算することができる。

請求項６記載の本発明に従えば、注目する複数の関節部のいずれかが警告範囲に到達したことを判断すると、警告報知手段によって関節部が警告範囲に到達したことを報知する。これによって操作者は、関節部が警告限界に近づいたことを判断することができ、利便性を向上することができる。たとえば操作者は、設定部分が固定状態に維持された状態で、注目する関節部が可動限界から遠ざかるように、冗長角度を設定することによって、設定部分を固定状態に維持した状態で、関節部を可動限界から遠ざけることができ、可動限界に対する余裕を、ロボットに与えることができる。

請求項７記載の本発明に従えば、注目する複数の関節部のいずれかが警告範囲に到達したことを判断すると、関節部が可動限界から遠ざかるような角変位方向を報知する。これによって操作者は、関節部が可動限界から遠ざかる方向を判断して、関節部が可動限界に到達することを容易に回避することができ、利便性を向上することができる。

請求項８記載の本発明に従えば、多関節ロボットは、傾斜関節部と同軸関節部とを有する。このような多関節ロボットの場合には、多関節ロボットの動きが複雑であり、設定部分を固定状態に維持したうえで、各関節部を角変位させる指令を手動で与えるのは困難である。これに対して、上述したように本発明の制御装置を用いれば、冗長関節部の選択指令と、冗長関節部の冗長角度を与えることによって、設定部分の固定状態を正確に維持したうえで、各関節部を角変位させることができ、利便性を向上することができる。

請求項９記載の本発明に従えば、多関節ロボットは、７つの関節部を含んで７自由度の多関節ロボットであり、設定部分を予め定める位置および姿勢に固定した状態で、各関節部の取り得る角度の組合せが複数通り存在する。本発明では、固定条件入力手段によって、設定部分を固定すべき位置および姿勢が入力される。設定部分の位置および姿勢が決定される場合、３軸交点位置演算手段によって、３軸交点の３次元位置を一意的に求めることができる。

また冗長関節部入力手段によって、３つの交点形成関節部以外となる４つの注目する関節部のうち１つが、冗長関節部として入力される。これによって４つの注目する関節部のうちで冗長関節部を除く残余の関節部の数が３つとなり、３軸交点の拘束される自由度の数、すなわち３軸交点の拘束される座標の数と一致する。また冗長角度入力手段によって、冗長関節部の冗長角度が既知となる。

この場合、冗長関節部の冗長角度と、３軸交点の３次元位置とを代入することで、注目する４つの関節部のうちで、冗長関節部以外の各関節部の角度を解析的に求めることが可能な計算式が存在する。注目関節部角度演算手段は、前記計算式を表わす計算プログラムを記憶し、その計算プログラムに従うことで、各入力手段に入力された各種情報に基づいて、３軸交点が３次元位置に位置する場合における、注目する４つの関節部の角度を解析的にそれぞれ求めることができる。

また注目する４つの関節部の角度と、設定部分の位置および姿勢とを代入することで、３軸交点形成関節部の角度を解析的に求めることが可能な計算式が存在する。交点関節部角度演算手段は、前記計算式を表わす計算プログラムを記憶し、その計算プログラムに従うことで、注目関節部角度演算手段の演算結果と、固定条件入力手段に入力された情報に基づいて、設定部分が予め定める位置および姿勢に位置する場合における、３軸交点形成関節部の角度を解析的にそれぞれ求めることができる。

出力手段は、冗長角度入力手段によって入力された冗長角度と、各角度演算手段によって演算された関節部の各角度とを多関節ロボットに与える。これによって多関節ロボットは、設定部分を予め定める位置および姿勢で固定するとともに、冗長関節部を冗長角度に保つように、各関節部を変位駆動することができる。ここで、各関節部の角度は解析解、すなわち厳密解に基づいてそれぞれ求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。

このように本発明によれば、冗長関節部を冗長角度にしたうえで、設定部分を予め定める固定状態に正確に保つことができる。たとえば設定部分の位置および姿勢を固定した状態で、冗長関節部および冗長関節部に連なるアーム体が障害物に衝突することを回避するために、冗長関節部を角変位させる場合、設定部分の位置および姿勢を正確に保ちつつ、障害物とアーム体との干渉を回避することができる。

請求項１０記載の本発明に従えば、設定部分を固定すべき固定条件を決定するとともに、複数の関節部のうちから冗長関節部と、冗長角度を決定する。この場合、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致する。次に、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式に、各入力工程によって決定した固定条件と冗長角度とを代入することで、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する。これによって複数の関節部の角度を解析的に求めることができる。このようにして求められた演算結果にしたがって、複数の関節部をそれぞれ角変位させることによって、設定部分を固定条件で固定した状態で、冗長関節部を冗長角度とすることができる。

本発明によれば、冗長関節部以外の残余の関節部の各角度は解析解、すなわち厳密解に基づいて求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。したがって残余の関節部の各角度を近似解に基づいて決定する場合に比べて、固定すべき固定条件から設定部分がずれることを防ぐことができる。また解析解を求めることに起因して、各関節部の特異点を正確に判断することができ、利便性を向上することができる。

請求項１１記載の本発明に従えば、設定部分を固定すべき固定条件を決定するとともに、複数の関節部のうちから冗長関節部と、冗長角度を決定する。この場合、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致する。次に、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式に、各入力工程によって決定した固定条件と冗長角度とを代入することで、設定部分が固定条件で固定されるべき、注目する複数の関節部の角度をそれぞれ演算する。これによって注目する複数の関節部の角度を解析的に求めることができる。

本発明によれば、冗長関節部以外の残余の関節部の各角度は解析解、すなわち厳密解に基づいて求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。したがって残余の関節部の各角度を近似解に基づいて決定する場合に比べて、固定すべき固定条件から設定部分がずれることを防ぐことができる。また解析解を求めることに起因して、各関節部の特異点を正確に判断することができ、利便性を向上することができる。

図１は、本発明の第１実施形態である多関節ロボット３０の制御装置２を示すブロック図である。制御装置２によって制御される多関節ロボット３０は、４自由度を有する一軸冗長ロボットであり、４つのアーム体３〜６と、隣接する２つのアーム体を回転自在に連結する４つの関節部９〜１２と、基台１７とを有する。各アーム体３〜６は、それぞれ連結されて直列状に延びるアーム構成体１６を構成する。また基台１７は、アーム構成体１６の基端部に連結され、壁面または床面などの固定位置に予め固定される。

各関節部９〜１２は、同軸関節部と傾斜関節部とのうちのいずれかである。同軸関節部は、隣接する２つのアーム体を、その各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する。傾斜関節部は、隣接する２つのアーム体のうちの一方を、各アーム体の軸線に対して傾斜する回転軸線まわりに円錐回転自在に連結する。

各関節部９〜１２は、基台１７から第１〜第４関節部９〜１２の順で並ぶ。第１関節部９は、同軸関節部である。また第２関節部１０、第３関節部１１および第４関節部１２は、傾斜関節部である。また本発明において、用語「回転」とは、回転軸線まわりの３６０度以上の回転だけでなく、軸線まわりに３６０度以下の角度で角変位する状態も含むものとする。

第１アーム体３の一端部は、第１関節部９によって、第１アーム体３の軸線Ａ３と同軸の第１回転軸線Ａ９まわりに回転自在に基台１７に連結されている。また第２アーム体４の一端部は、第２関節部１０によって、第１アーム体３の軸線Ａ３および第２アーム体４の軸線Ａ４に対して所定の角度、本実施の形態では４５度で傾斜する第２回転軸線Ａ１０まわりに回転自在に、第１アーム体３の他端部に連結される。また第３アーム体５の一端部は、第３関節部１１によって、第２アーム体４の軸線Ａ４および第３アーム体５の軸線Ａ５に対して所定の角度、本実施の形態では４５度で傾斜する第３回転軸線Ａ１１まわりに回転自在に、第２アーム体４の他端部に連結される。

第４アーム体６の一端部は、第４関節部１２によって、第３アーム体５の軸線Ａ５および第４アーム体６の軸線Ａ６に対して所定の角度、本実施の形態では４５度で傾斜する第４回転軸線Ａ１２まわりに回転自在に、第３アーム体５の他端部に連結される。第４アーム体７の他端部には、設定部分となる対象点ｐｈが予め設定される。したがって対象点ｐｈは、アーム構成体１６の先端部に配置され、基台１７は、アーム構成体１６の基端部に配置される。

このように各関節部９〜１２は、隣接するアーム体を互いに回転可能に連結する。また各関節部９〜１２が角変位することで、アーム構成体１６は、図１に示すように、アーム体３〜６のそれぞれの軸線Ａ３〜Ａ６が同軸に配置されて一直線状に延びる状態に変形可能に構成される。各アーム体３〜６の軸線Ａ３〜Ａ６が一直線状に延びた状態では、第２回転軸線Ａ１０と第３回転軸線Ａ１１とは平行となり、第３回転軸線Ａ１１と第４回転軸線Ａ１２とは垂直となる。

多関節ロボット３０は、各関節部９〜１２を回転させることで、アーム構成体１６を蛇のように変形させ、対象点ｐｈを任意の３次元位置に位置決め可能である。たとえば多関節ロボット３０は、設備等が複雑に入り組んで作業経路が複雑な場合や、他のロボットと近接して並列に配置されて動作範囲を大きくすることができない場合に好適に用いられる。

このような４つの関節部９〜１２を有する多関節ロボット３０は、４自由度を有するので、可動範囲内で対象点Ｐｈを予め定める３次元位置に位置合わせした状態で、第４アーム体６の取り得る姿勢が複数通り存在する。言換えると、４関節ロボット３０は、対象点Ｐｈを予め定める３次元位置に位置合わせした状態で、４つの関節部９〜１２のそれぞれの取り得る角度の組合せが複数通り存在する。

各アーム体３〜６は、各アーム体３〜６をそれぞれ個別に回転駆動するモータを内蔵する。モータは、各アーム体３〜６を予め定める任意の角度に変位可能に構成される。

各アーム体３〜６を対応する回転軸線Ａ９〜Ａ１２まわりに回転可能に連結するための、各関節部９〜１２の回転機構は、入力側部材と出力側部材とを備え、それらが相対的に回転可能に設けられる。入力側部材は、各関節部９〜１２によって連結する２つの部材の一方に連結され、出力側部材は、前記２つの部材のうち他方に連結される。モータからの回転力が入力側部材に与えられると、入力側部材と出力側部材とが相対的に回転する。これによって一方の部材と他方の部材とを相対的に回転させることができる。同軸関節部９では、その回転軸線Ａ９が、入力側部材および出力側部材に連結されるアーム体と一致する。傾斜関節部１０〜１２では、その回転軸線Ａ１０〜Ａ１２が、入力側部材および出力側部材にそれぞれ連結されるアーム体の軸線に対してそれぞれ傾斜する。

このような回転機構が設けられることによって、隣接する２つの部材を相対的に回転させることができる。ここで、本発明における「関節部の角度」とは、関節部における入力側部材に対する出力側部材の角度を意味する。言い換えると、関節部によって連結される２つの部材のうち、入力側部材に連結される一方の部材に対する、出力側部材に連結される他方の部材の関節部の回転軸線まわりの角度を意味する。また本発明において「関節部の移動」とは、関節部の角度の角変位移動を示す。

制御装置２は、入力部２０と、中央演算処理部１９と、出力部２２とを含んで構成される。入力部２０および出力部２２は、中央演算処理部１９に電気的に接続される。入力部２０は、操作者からの各種指令情報が入力され、入力された各種情報を中央演算処理部１９に与える。本実施の形態では、中央演算処理部１９と出力部２２とは、制御装置本体２１に設けられ、入力部２０は、制御装置本体２１とは別体に設けられる。具体的には、入力部２０は、操作者が多関節ロボットを直接制御するためのティーチペンダントによって実現され、制御装置本体２１であるロボットコントローラに対して、ケーブルによって電気的に接続される。

中央演算処理部１９は、予め記憶される動作プログラムに従って、入力部２０から与えられる各種情報に応答して、多関節ロボット３０を制御するために必要な演算を行う。本実施の形態では、中央演算処理部１９は、第４アーム体６に設定される対象点ｐｈを予め定める目標位置に移動させるのに必要な、各関節部９〜１２の角度ｑ１〜ｑ４をそれぞれ演算する。中央演算処理部１９は、ＣＰＵなどの処理回路と、メモリなどの記憶回路とを含んで実現される。記憶回路には、動作プログラムとともに、演算に必要な計算式、アーム体の長さ、関節部の種類などを記憶する。

出力部２２は、中央演算処理部１９の演算結果を多関節ロボット３０に与える出力手段となる。本実施の形態では、出力部２２は、中央演算処理部１９の演算結果を多関節ロボット３０に設けられる電源回路に与える。電源回路は、中央演算処理部１９の演算結果に応じた動力を、関節部９〜１２毎に設けられるモータにそれぞれ個別に与える。動力が与えられた各モータは、中央演算処理部１９の演算結果に従って、対応する関節部を所定の角度に角変位させる。

また制御装置２は、第４アーム体６の対象点ｐｈを予め指定される移動経路に沿って移動させるにあたって、回転すべき各関節部９〜１２の角度を演算する。この移動経路は、ロボット動作前に、操作者によって入力部２０から中央演算処理部１９に入力される。また多関節ロボット３０は、４つの関節部９〜１２を有することで、基台１７に対して、対象点ｐｈを予め定める位置で固定した状態で、各関節部の取り得る角度の組合せが複数通り存在する。

本実施形態の制御装置２は、ロボットの動作を教示するための教示動作を行うために用いられ、教示動作のための形態変更動作を行う。形態変更動作とは、対象点ｐｈを予め定める固定条件で固定した状態で、第１〜第４関節部９〜１２の角度を変化させて、ロボットの形状および姿勢などの外形的形態を変更する動作である。本実施の形態では形態変動動作は、基台１７に設定されるベース座標に対する対象点ｐｈの３次元位置を固定した状態で、第１〜第４関節部９〜１２の角度を変化させる動作となる。言換えると、形態変更動作では、アーム構成体１６のうちで直列方向一端部に設定される対象点ｐｈが、アーム構成体１６の他端部に設定される座標系に従って求められる拘束条件で固定される。

形態変更動作を行うにあたって、制御装置２は、第１〜第４関節部９〜１２のうちから選択された１つの冗長関節部と、その冗長関節部の角度である冗長角度と、対象点ｐｈを固定すべき３次元位置とが入力部２０を介して与えられる。中央演算処理部１９は、対象点ｐｈを固定すべき固定状態に固定したうえで、冗長関節部が冗長角度となる場合の、残余の関節部の角度を演算する。出力部２２は、中央演算処理部１９の演算結果を多関節ロボット３０に与える。演算結果が与えられた多関節ロボット３０は、冗長関節部を冗長角度とするとともに、残余の関節部の角度を演算結果に応じて位置合わせして、対象点ｐｈを指定された固定状態とする。

形態変更動作を行うことによって、操作者は、入力部２０を介して冗長関節部を選択するとともに、その冗長角度を入力することで、対象点ｐｈを固定した状態で、アーム構成体１６の形態を変更させることができる。この場合、操作者は、中央演算処理部１９に形態変更動作を行わせて、多関節ロボット３０の形態を確認することによって、対象点ｐｈを固定した状態で、障害物との衝突を回避するアーム構成体１６の形態を実現する各関節部９〜１２の角度を容易に決定することができる。操作者は、障害物との衝突を回避する各関節部９〜１２の角度ｑ１〜ｑ４を決定すると、その各関節部９〜１２の角度ｑ１〜ｑ４をロボットの移動動作を行う場合の情報として用いる。

図２は、入力部２０を示す正面図である。入力部２０には、ロボット教示のための情報および指令が入力される操作部４１と、操作画面を表示する表示部４２とを含む。操作部４１には、各関節部操作スイッチ４３と、ツール位置スイッチ４４と、形態変更スイッチ４５とを含む。また操作部４１は、ロボットの動作プログラムを入力するための命令キー、カーソルキーおよびテンキーなどをさらに含む。

関節部操作スイッチ４３は、各関節部９〜１２の角度ｑ１〜ｑ４を入力するためのスイッチであって、各関節部９〜１２について、プラス方向およびマイナス方向への予め定める角変位量の角変位指令が与えられる。ツール移動スイッチ４４は、ベース座標軸またはツール座標軸に従う対象点ｐｈの移動指令が与えられる。ここで、ベース座標軸とは、基台１７に設定される座標であって、ツール座標軸とは、第４アーム体６に設定される座標である。また形態変更スイッチ４５は、対象点ｐｈを予め定める位置に固定したまま、アーム構成体１６の形態を変化させるための形態変更指令が与えられる。

形態変更動作を行う場合、ツール移動スイッチ４４は、対象点ｐｈを固定すべき位置の情報が入力される固定条件入力手段となる。また形態変更スイッチ４５が押下された状態で、関節部操作スイッチ４３によって１つの関節部の角変位量が指定されると、指定された関節部が冗長関節部として入力される。また、その角変位量に対応する角度が冗長関節部の角度として入力される。したがって形態変更スイッチ４５と関節部操作スイッチ４３とによって、冗長関節部が入力される冗長関節部入力手段と、冗長関節部の角度である冗長角度が入力される冗長角度入力手段とが構成される。

図３は、中央演算処理部１９の形態変更動作の手順を説明するためのフローチャートである。まずステップａ０で、中央演算処理部１９は、多関節ロボット３０を制御可能な状態で、形態変更スイッチ４５が押下されたことを判断すると、ステップａ１に進み、形態変更動作を開始する。

ステップａ１では、中央演算処理部１９は、ツール移動スイッチ４４によって対象点ｐｈの移動指令が与えられると、対象点ｐｈを指定された位置に移動するための演算結果を出力部２２に与える。またツール移動スイッチ４４による移動指令がなければ、現時点の対象点ｐｈの位置を維持させる。中央演算処理部１９は、関節部操作スイッチ４３が押下されたと判断すると、その時点の対象点ｐｈの位置を固定すべき固定条件として決定し、ステップａ２に進む。

ステップａ２では、中央演算処理部１９は、関節部操作スイッチ４３によって指定された関節部を、冗長関節部として決定する。冗長関節部は、第１〜第４関節部９〜１２のいずれか１つとなる。また関節部操作スイッチ４３によって指定された角度を、冗長角度として決定する。本実施の形態では、関節部操作スイッチ４３のうち、プラス方向移動スイッチが押圧されると、関節部操作スイッチ４３が押下される前の状態から、予め定める角変位量で冗長関節部がプラス方向に軸線まわりに角変位した角度を冗長角度として決定する。またマイナス方向移動スイッチが押圧されると、関節部操作スイッチ４３が押下される前の状態から、予め定める角変位量で冗長関節部がマイナス方向に軸線まわりに角変位した角度を冗長角度として決定する。このように冗長関節部および冗長角度を決定するとステップａ３に進む。

ステップａ３では、中央演算処理部１９は、ステップａ１で決定した対象点ｐｈの３次元位置に、対象点Ｐｈが位置する場合における、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を求める。具体的には、第１〜第４関節部９〜１２のそれぞれの角度ｑ１〜ｑ４を変数として、対象点ｐｈの３次元位置を求めるための順変換関係式と、ステップａ２で決定した冗長関節部の冗長角度と、ステップａ１で決定した対象点ｐｈの３次元位置とに基づくことによって、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を解析的に求めることができる。

順変換問題を解くことによって、対象点Ｐｈのｘ座標位置を表わすｘ座標式、対象点Ｐｈのｙ座標位置を表わすｙ座標式、対象点ｐｈのｚ座標位置を表わすｚ座標式は、第１〜第４関節部９〜１２の各角度ｑ１〜ｑ４を変数とする関数で表わされる。順変換問題によるｘ座標式、ｙ座標式およびｚ座標式に、ステップａ１で求めた対象点ｐｈの３次元位置を代入することで、それらの座標式は、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４の数、すなわち４つの未知数を有する三元連立方程式となる。

また第１〜第４関節部９〜１２の各角度ｑ１〜ｑ４のうちの１つは、冗長角度としてステップａ２で決定しているので、冗長角度を上述した３元連立方程式に代入することによって、３つの未知数を有する三元連立方程式となる。この３元連立方程式を変形することで、３つの未知数をそれぞれ求める方程式を表わす計算式を導出することができる。この方程式に、対象点ｐｈの座標、冗長関節部および冗長関節部の角度を代入することで、第１〜第４関節部９〜１２のうちで、冗長関節部以外の関節部の各角度をそれぞれ解析的に求めることができる。

中央演算処理部１９は、冗長関節部の冗長角度と、対象点Ｐｈのｘ座標、ｙ座標、ｚ座標を代入することで、第１〜第４関節部９〜１２のうちで冗長関節部以外の各関節部の角度を解析的に求めることが可能な方程式を表わす計算プログラムを記憶している。中央演算処理部１９は、第１〜第４関節部９〜１２の計算プログラムにしたがって、第１〜第４関節部９〜１２のうちで、冗長関節部以外の各関節部の角度を解析的に演算し、ステップａ４に進む。冗長関節部の角度は、ステップａ２で既に決定しているので、演算することなく求めることができる。このように中央演算処理部１９は、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４をそれぞれ演算する角度演算手段となる。

ステップａ４では、中央演算処理部１９は、求めた各関節部９〜１２の各角度ｑ１〜ｑ４が可動限界に到達していないことを判断するとステップａ５に進み、各角度ｑ１〜ｑ４のいずれかが可動限界に到達していること、または可動限界を超えていることを判断すると、ステップａ６に進む。たとえば中央演算処理部１９は、ステップａ３で求めた各関節部の角度が、解析的に求められる最大値または最小値となると、可動限界に到達したことを判断する。また各関節部の角度を求められない場合に可動限界を超えていると判断する。

ステップａ５では、ステップａ３で解析的に求めた第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を演算結果として、出力部２２に与える。出力部２２は、与えられた演算結果を多関節ロボット３０に与える。これによって多関節ロボット３０は、各関節部９〜１２が角変位し、対象点ｐｈを固定位置に維持した状態で、冗長関節部を冗長角度とする。このとき、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４は、収束演算などの近似法とは異なり、解析的な演算方法によって求められるので、冗長関節部の移動前と移動後とで、固定位置から対象点ｐｈがずれることがない。このように演算結果を出力部２２に与えて、多関節ロボット３０を動作させると、ステップａ６に進む。

ステップａ６では、中央演算処理部１９は、形態変更動作を終了する終了条件を満足するか否かを判断する。たとえば形態変更解除スイッチが押下されることで、終了条件を満足することを判断してもよい。中央演算処理部１９は、形態変更動作の終了条件を満足していないと判断すると、ステップａ１に戻り、関節部操作スイッチ４３が再び押下されるのを待機する。この状態で、関節部操作スイッチ４３が再度押下されると、その指示に従ってステップａ１〜ａ５を順に行う。またステップａ６で終了条件を満足することを判断すると、ステップａ７に進み、ステップａ７で形態変更動作を終了する。

このようにして、中央演算処理部１９が形態変更動作を行うことによって、操作者が、対象点ｐｈの固定位置と、冗長関節部と、その冗長角度とを指定することによって、対象点ｐｈの位置を確実に固定したまま、各関節部９〜１２を角変位させて、多関節ロボット３０の形態を変更させることができる。このとき各関節部９〜１２の角度ｑ１〜ｑ４を解析解として求めることができるので、収束演算を用いて解を求める場合に比べて短時間で各関節部９〜１２の角度を求めることができ、対象点ｐｈの位置を固定したままロボットの形態を滑らかに変更することができる。

また操作者は、多関節ロボット３０の形態を確認しながら、中央演算処理部１９に形態変更動作を行わせることができる。これによって、操作者は、対象点ｐｈを固定位置に位置させることができる各関節部９〜１２の角度ｑ１〜ｑ４の複数の組合せのうちから、１つを容易に選択することができる。これによって操作者は、障害物に衝突することを回避した多関節ロボット３０の形態で、対象点ｐｈを固定位置に配置させることができ、各関節部９〜１２の角度ｑ１〜ｑ４を求めることができる。

図４は、対象点ｐｈの位置を予め定める位置に固定した状態で、第１関節部９を角変位させた多関節ロボット３０の状態を示す正面図である。第１関節部９の角度ｑ１をプラス方向に角変位するに伴って、多関節ロボット３０は、図４（１）〜図４（８）の順で変形する。また図５は、第１関節部９の角度ｑ１を冗長角度として設定した場合における第２〜第４関節部１０〜１２の角度ｑ２〜ｑ４を示すグラフである。図５（１）は、横軸に第１関節部９の角度ｑ１を示し、縦軸に第２関節部１０の角度ｑ２を示す。また図５（２）は、横軸に第１関節部９の角度ｑ１を示し、縦軸に第３関節部１１の角度ｑ３を示す。図５（３）は、横軸に第１関節部９の角度ｑ１を示し、縦軸に第４関節部１２の角度ｑ４を示す。

詳細については後述するが、本実施の形態では、対象点ｐｈを予め定める３次元位置に固定した状態に維持する、各関節部９〜１２の角度ｑ１〜ｑ４の解析解を決定することができる。これによって各関節部９〜１２の角度ｑ１〜ｑ４の近似解を決定する場合に比べて、対象点ｐｈが固定位置からずれることを防ぐことができる。たとえばステップａ２において、関節部操作スイッチ４３のうち、プラス方向またはマイナス方向移動スイッチが１回押圧された場合に、冗長関節部が角変位する角変位量が大きく設定される場合であっても、対象点ｐｈの固定状態を正確に維持することができる。同様に、ステップａ２において、冗長関節部が高速で移動する場合であっても、対象点ｐｈの固定状態を正確に維持することができる。

これによって形態変更動作によって、多関節ロボット３０の障害物回避形態の探索を行った場合、対象点ｐｈの位置を正確に固定した状態で、各関節部９〜１２を角変位させることができる。また対象点ｐｈを固定位置に正確に保ちつつ、障害物と各アーム体３〜６との干渉を回避することができるので、対象点ｐｈの位置ずれに起因する多関節ロボットの動作不良を防ぐことができる。

また中央演算処理部１９は、各関節部９〜１２の角度ｑ１〜ｑ４の解析解を求めることに付随して、冗長関節部が可動限界に到達したか否かを判断することができる。ステップａ４に示すように、中央演算処理部１９は、冗長関節部が可動限界に到達することを判断すると、冗長関節部が可動限界に到達した状態の関節部の角度を演算する。また可動限界を超えるような冗長角度が入力されても、可動限界に到達した状態の関節部の角度を演算する。

これによって中央演算処理部１９が、機構的に移動不可能な移動指令値を、多関節ロボットに与えることを防ぐことができ、関節部の損傷を防ぐことができる。また中央演算処理部１９は、冗長関節部が可動限界に到達したか否かを精度よく判断することができるので、可動限界に近い領域まで冗長関節部を角変位させることができ、ロボットの可動範囲を広くすることができる。

また中央演算処理部１９は、冗長関節部が可動限界に到達する前に、冗長関節部が可動限界近傍の警告領域に到達したことを判断すると、その判断結果を入力部２０の表示部４２を介して報知してもよい。たとえば中央演算処理部１９は、演算によって決定される冗長関節部の可動限界角度よりも予め定める所定角度分手前の角度に、冗長角度が達した場合に、警告領域に到達したことを判断する。この場合、中央演算処理部１９は、警告範囲に到達したことを判断して報知する警告報知手段となる。

たとえば図５の場合では、冗長関節部が第１関節部９であって、第３関節部１１の角度ｑ３が１８０度となった場合に可動限界に達する。この場合、中央演算処理部１９は、第３関節部１１の角度ｑ３が１７５度に達すると、警告領域に到達したことを判断し、判断結果を表示部４２に表示させる。これによって入力部２０を操作する操作者は、冗長関節部が可動限界に近づいていることを判断することができ、利便性を向上することができる。さらに中央演算処理部１９は、冗長関節部が警告範囲に入った場合、その関節部の角度が警告範囲を脱出する角変位方向を入力部２０の表示部４２に表示することが好ましい。

本実施形態の制御装置２が制御する多関節ロボット３０は、傾斜関節部を含むので、形態変更動作における多関節ロボット３０の変形形態は変則的となる。この場合、操作者は、各関節部９〜１２の変形を直感的に予測することが困難である。本実施の形態では、中央演算処理部１９は、警告範囲および警告範囲を脱出するための角変位方向を報知するで、操作者は、警告範囲および脱出方向を容易に判断することができ、利便性を向上することができる。

次に、ステップａ３における第１〜第４角度ｑ１〜ｑ４の演算工程の詳細について説明する。順変換問題に基づくことによって、第１〜第４関節部９〜１２のそれぞれの角度ｑ１〜ｑ４が決定されると、対象点Ｐｈのｘ座標位置ｐｈｘ、ｙ座標位置ｐｈｙ、ｚ座標位置ｐｈｚが一義的に決定される。すなわち対象点ｐｈのｘ座標、ｙ座標、ｚ座標を求めるための式は、第１〜第４関節部９〜１２のそれぞれの角度ｑ１〜ｑ４を変数とする関数によって表わされ、本実施の形態では、以下に示す（１）式の関係を有する。

（１）式において第１関節部９の角度をｑ１とし、第２関節部１０の角度をｑ２とし、第３関節部１１の角度をｑ３とし、第４関節部１２の角度をｑ４とする。また第１関節部９から第２関節部１０までの第１アーム体３の距離をＬ１とし、第２関節部１０から第３関節部１１までの第２アーム体４の距離をＬ２とし、第３関節部１１から第４関節部１２までの第３アーム体５の距離をＬ３とし、第４関節部１２から対象点ｐｈまでの距離をＬ４とする。またｓｉｎ（θ）はθの正弦関数（サイン関数）を示し、ｃｏｓ（θ）は、θの余弦関数（コサイン関数）を示す。

また（１）式において、対象点Ｐの座標ｐｈｘ，ｐｈｙ，ｐｈｚは、基台１７に設定されるベース座標軸に従って、第１アーム体３の代表点ｐ１を原点とした場合の座標である。ここでベース座標軸は、各アーム体の軸線が一直線状に配置された状態で、その直線と同軸に延びる軸をｚ軸とし、ｚ軸に垂直な平面に沿って延びてｚ軸を通過する軸をｘ軸およびｙ軸とする。ｘ軸およびｙ軸は、互いに直交する。また各関節部９〜１２の角度ｑ１〜ｑ４は、各アーム体３〜６の軸線が一直線状に延びた状態から関節部が角変位した角度とする。また第１アーム体３の代表点ｐ１は、第１アーム体の軸線Ａ１と、第２関節部１０の回転軸線Ａ１０との交点に設定される。

対象点ｐｈを固定すべき位置のｘ座標ｐｈｘ、ｙ座標ｐｈｙ、ｚ座標ｐｈｚの数値を（１）式に代入することによって、第１〜第４関節部９〜１２の４つの角度ｑ１〜ｑ４が未知数となる３元連立方程式となる。第１〜第４関節部９〜１２のうちの１つを冗長関節部として決定し、冗長関節部の冗長角度を（１）式に代入することによって、第１〜第４関節部９〜１２の４つの角度ｑ１〜ｑ４のうちの１つが既知となり、未知数が３つの３元連立方程式となる。これによって（１）式を連立して解くことによって、対象点ｐｈを固定位置に位置すべき、各関節部９〜１２の角度ｑ１〜ｑ４を解析的に求めることができる。

たとえば第１関節部９を冗長関節部として決定し、第１関節部９を角変位すべき角度ｑ１を冗長角度として決定する。この場合、（１）式を変形することによって、以下の（２）式に示すような、第１関節部９と第４関節部１２の角度ｑ１，ｑ４とが未知数となる方程式に変形することができる。（２）式に、対象点ｐｈのｚ座標ｐｈｚ、冗長関節部となる第１関節部９の角度ｑ１を代入することによって、第４関節部１２の角度ｑ４を求めることができる。得られた第４関節部１２の角度ｑ４は、対象点ｐｈを固定位置に位置させ、かつ第１関節部９を冗長角度である第１角度ｑ１に角変位した場合における第４関節部１２の角度ｑ４を意味する。

（２）式を、以下の（３）式に変形し、第１関節部９の角度ｑ１を代入することによって、第４関節部１２の角度ｑ４を求めることができる。

ここで、ａｒｃｃｏｓは、逆余弦関数（アークコサイン関数）を意味する。また（３）式におけるプラスマイナスの２つの符号を離散形態符号と称する。予め定める条件、たとえば計算前に設定されている第４関節部１２の角度ｑ４などに基づいて、２つの離散形態符号のいずれかを決定することで、対象点ｐｈを固定位置に固定する場合における、第４関節部１２の角度ｑ４を演算することができる。

また（３）式において解が１つしかない場合、すなわちアークコサインの変数が、１または−１となる場合、第４関節部１２は特異点に位置する。具体的には、第４関節部１２の角度ｑ４が、０度または１８０度となると、特異点となる。またアークコサインの変数の絶対値が１を超えると、解が存在しない。このような判定を行うことで、中央演算処理部１９は、特異点でない解が存在するか、解が特異点となるか、解が存在しないかを判断することができる。

次に、（１）式を変形することによって、以下の（４）式に示すような、第３関節部１１と第４関節部１２との角度ｑ３，ｑ４が未知数となる方程式に変形することができる。（４）式に、（３）式に基づいて決定した第４関節部１２の角度ｑ４を代入することによって、第３関節部１１の角度ｑ３を求めることができる。得られた第３関節部１１の角度ｑ３は、対象点ｐｈを固定位置に保ち、かつ第１関節部９を冗長角度である第１角度ｑ１に角変位した場合における第３関節部１１の角度ｑ３を意味する。

次に、（１）式のうちのｐｈｘ、ｐｈｙ、ｐｈｚを求めるそれぞれの式のいずれか２つを連立させた式に、冗長関節部として設定される第１関節部９の角度と、（３）式に基づいて求めた第４関節部１２の角度ｑ４と、（４）式に基づいて求めた第３関節部１１の角度ｑ３とを代入することによって、第２関節部１０の角度ｑ２における、ｃｏｓ（ｑ２）と、ｓｉｎ（ｑ２）を求めることができる、これらから第２関節部１１の角度ｑ２を求めることができる。

このようにして、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４をそれぞれ求めることができる。この場合、中央演算処理部１９は、上述した（３）式、（４）式、第２関節部１１の角度ｑ２を求める式をそれぞれ示す計算プログラムを記憶することで、第１関節部９の第１角度ｑ１が決定されることで、解が存在する場合、対象点ｐｈを固定位置に保った場合における第２〜第４関節部１０〜１２の角度ｑ２〜ｑ４をそれぞれ求めることができる。

また中央演算処理部１９は、冗長関節部の冗長角度を正または負の方向に徐々に角変位させた場合に、冗長関節部の角度が取り得ることが可能な極大値または極小値となった場合、その角度を可動限界として判断することができる。たとえば冗長角度を大きくしていくと、冗長関節部以外の残余の関節部の角度の解を求めることができなくなる場合が生じる。この場合、中央演算処理部１９は、各関節部の角度の解を求めることができる冗長角度のうちで、最大値を可動限界として判断する。同様に冗長角度を小さくしていくと、冗長関節部以外の残余の関節部の角度の解を求めることができなくなる場合が生じる。この場合、中央演算処理部１９は、各関節部の角度の解を求めることができる冗長角度のうちで、最小値を可動限界として判断する。

中央演算処理部１９は、上述するステップａ１およびステップａ２を終えた状態で、冗長関節部の可動限界を演算することができる。そして指定された冗長角度と、可動限界角度との偏差を求めることができる。この場合、その偏差が予め定める値以下となる場合に、可動限界近傍に設定される警告領域に、冗長関節部が到達したことを判断することができる。

また中央演算処理部１９は、現時点での冗長角度に対して、プラス方向に冗長角度を増加させた場合と、マイナス方向に冗長角度を増加させた場合とで、前記偏差の変化量を調べることで、偏差が大きくなる冗長角度の角変位方向を求めることができる。中央演算処理部１９は、偏差が大きくなる冗長角度の角変位方向を、冗長角度が可動限界から遠ざかって、警告領域から脱出する方向として判断することができる。

また冗長関節部として、第１関節部以外の関節部が選択される場合には、同様にして（１）式を変形して連立方程式を解くことによって、解が存在する場合、冗長関節部以外の各関節部の角度をそれぞれ求めることができる。したがって中央演算処理部１９は、冗長関節部として選択された関節部に応じて、残余の関節部を求める計算プログラムをそれぞれ記憶することで、第１〜第４関節部９〜１２のいずれが冗長関節部として選択されたとしても、対象点ｐｈを固定位置に維持しかつ、冗長関節部が冗長角度となる場合の、残余の関節部の角度をそれぞれ演算することができる。

図６は、対象点ｐｈを固定した場合に多関節ロボット３０が取り得る状態の種類を説明するための１軸冗長ロボット６０を示す正面図である。図６に示す１軸冗長ロボットは、第１〜第４アーム体５０〜５３と、隣接するアーム体を連結する複数の関節部５４〜５７とを有する。各アーム体５０〜５３は、予め定めるｘ軸とｙ軸とを含むｘｙ平面上に配置される。第１アーム体５０の基端部５０ａは、固定部５８に第１関節部５４によって連結され、第１関節部５４によって固定部５８に対して、ｘｙ平面に垂直なｚ軸まわりに角変位可能となる。

また第２アーム体５１の基端部５１ａは、第２関節部５５によって第１アーム体５０の先端部５０ｂに連結され、第２関節部５５によって第１アーム体５０の先端部５０ｂに対して、ｚ軸まわりに角変位可能となる。また第３アーム体５２の基端部５２ａは、第３関節部５６によって第２アーム体５１の先端部５１ｂに連結され、第３関節部５６によって第２アーム体５１の先端部５１ｂに対して、ｚ軸まわりに角変位可能となる。また第４アーム体５３の基端部５３ａは、第４関節部５６によって第３アーム体５２の先端部５２ｂに連結され、第４関節部５６によって第３アーム体５２の先端部５２ｂに対して、ｚ軸まわりに角変位可能となる。

したがって図６に示す一軸冗長ロボット６０は、第４アーム体５３をｘｙ平面内で移動可能であるとともに、第４アーム体５３をｚ軸まわりに回転可能な４関節平面ロボットとなる。

このような１軸冗長ロボット６０に関して、第４アーム体５３の先端部５３ｂのｘ座標およびｙ座標と、第４アーム体５３のｚ軸まわりの姿勢とを固定する固定条件、すなわち３つの自由度を拘束する固定条件を設定する。この固定条件で第４アーム５３を固定した場合における第１〜第４関節部５４〜５７の角度を考える。

１軸冗長ロボット６０は、上述した固定条件で第４アーム体５３を固定した状態で、第１〜第４関節部５４〜５７の取り得る組合せが複数存在する。ここで、４つの関節部５４〜５７のうちの１つの角度を設定すれば、固定条件を満足する各関節部の角度を、逆変換により求めることができる。

４自由度を有する一軸冗長ロボット６０は、対象点ｐｈの３つの自由度を拘束した固定状態では、各アーム体の長さと、第１関節部と第４関節部との間の距離とに応じて、３つの形態を取り得ることになる。

第１の形態では回転揺動状態となる。この場合、１軸冗長ロボット６０は、図６（１）に示すように、第４アーム体５３を固定した状態で、第１アーム体５０が固定部５８に対して３６０度以上回転可能でかつ、第３アーム体５２が第４アーム体５３に対して３６０度以下揺動角変位可能となる。また第２アーム体５１は、第１アーム体５０に対して３６０度以上回転可能となり、第３アーム体５２は、第２アーム体５１に対して３６０度未満揺動角変位可能となる。

第１関節部５４と第２関節部５５との間の距離をＬ１とし、第２関節部５５と第３関節部５６との間の距離をＬ２とし、第３関節部５６と第４関節部５７との間の距離をＬ３とし、第１関節部５４と第４関節部５７との間の距離をＬｗとすると、Ｌｗ＋Ｌ２＞Ｌ１＋Ｌ３、Ｌｗ＞Ｌ２、Ｌ１＞Ｌ３、Ｌｗ−Ｌ２＜Ｌ３−Ｌ１である場合に、回転揺動状態となる。

また第２の形態では、２重揺動状態となる。この場合、１軸冗長ロボット６０は、図６（２）に示すように、第４アーム体５３を固定した状態で、第１アーム体５０が固定部５８に対して３６０度未満揺動角変位可能となり、第３アーム体５２が第４アーム体５３に対して３６０度未満揺動角変位可能となる。また第２アーム体５１は、第１アーム体５０に対して３６０度未満揺動角変位可能となり、第３アーム体５２は、第２アーム体５１に対して３６０度未満揺動角変位可能となる。

また第３の形態では、２重回転状態となる。この場合、１軸冗長ロボット６０は、図６（３）に示すように、第４アーム体５３を固定した状態で、第１アーム体５０が固定部５８に対して３６０度以上回転可能となり、第３アーム体５２が第４アーム体５３に対して３６０度以上回転可能となる。また第２アーム体５１は、第１アーム体５０に対して３６０度以上回転可能となり、第３アーム体５２は、第２アーム体５１に対して３６０度以上回転可能となる。

２重揺動状態および２重回転状態では、第１〜第４関節部５４〜５７のいずれか１つの角度を設定すれば、第４アーム体５３が固定状態となる場合に第１〜第４関節部５４〜５７が取り得る角度の組合せは、１つとなる。したがって第１〜第４関節部５４〜５７のうちから、１つの冗長関節部を設定し、その角度を指定することで、一軸冗長ロボットが取り得る範囲を全て計算することができる。これに対して回転揺動状態では、第１〜第４関節部５４〜５７のうちから１つの冗長関節部を設定しただけでは、１軸冗長ロボット６０が取り得る範囲を全て計算することができない。

図７は、１軸冗長ロボット６０が回転揺動状態となる場合において、各関節部が取り得る角度の組合せを示す図である。図７（１）は、１軸冗長ロボット６０が初期状態である場合を示し、第１アーム体５０が固定部５８に対して角変位する状態を、図７（２）〜図７（５）に順に示す。

第４関節部５７を冗長関節部として選択する場合、図７（３）に示すように、第３アーム体５２を第４アーム体５３に対して一方向（図７では反時計方向）に角変位して、揺動限界角度に到達すると、それ以上、第３アーム体５２を第４アーム体５３に対して、一方向に角変位させることができなくなる。この場合、第１アーム体５０も、固定部５８に対して一方向にそれ以上角変位することができなくなる。揺動限界角度に到達した状態から、第３アーム体５２を第４アーム５３に対して他方向（図７では時計方向）に角変位すると、第１アーム体５０も、固定部５８に対して他方向に角変位する。このように第４関節部を冗長関節部として選択すると、第１アーム体５０を固定部５８に対して３６０度回転させることができず、一軸冗長ロボット６０が取り得る範囲を全て計算することができない。これに対して、第１関節部５４を冗長関節部として選択する場合、図７（１）〜図７（５）に示すように、第１アーム体５０を固定部５８に対して一回転させることができ、一軸冗長ロボット６０が取り得る範囲を全て計算することができる。

したがって一軸冗長ロボット６０が、回転揺動状態となる場合、第１関節部５４と第２関節部５５とのうちのいずれか一方と、第３関節部５６と第４関節部５７とのいずれか一方との２つを冗長関節部として選択可能とすることによって、２つのうちいずれかが、回転揺動状態のうちで３６０度以上アーム体が回転する関節部を冗長関節部として選択可能となり、一軸冗長ロボット６０が取り得る範囲を全て計算することができる。

一軸冗長ロボット６０の各アーム体５０〜５３の長さが決定しているとしても、第１関節部５４と第４関節部５７との間の距離Ｌｗの長さが変化することによって、回転揺動状態となるか、２重揺動状態および２重回転状態となるかが変化する。上述したように、第１関節部５４と第２関節部５５とのうちのいずれか一方と、第３関節部５６と第４関節部５７とのいずれか一方との２つを、冗長関節部として選択可能とすることによって、仮に回転揺動状態となったとしても、一軸冗長ロボット６０が取り得る範囲を全て計算することができる。このことは、４つの関節部を有する一軸冗長ロボットに共通することである。したがって図１に示す多関節ロボット３０であっても同様となる。

図８は、図１に示す多関節ロボット３０において、第１関節部９を冗長関節部として、その角度ｑ１を順次角変位させた状態を示す正面図であり、図８（１）〜図８（３）の順に変化する。また図９は、図８において各関節部９〜１２の角度ｑ１〜ｑ４の時間変化を示すグラフである。図９（１）は、第１関節部９の角度ｑ１の時間変化を示し、図９（２）は、第２関節部１０の角度ｑ２の時間変化を示し、図９（３）は、第３関節部１１の角度ｑ３の時間変化を示し、図９（４）は、第４関節部１２の角度ｑ４の時間変化を示す。

図８および図９には、対象点ｐｈを固定位置に位置させた場合に、多関節ロボット３０が回転揺動状態となり、第１関節部９および第２関節部１０が揺動可能であって、第３関節部１１および第４関節部１２が回転可能である場合を示す。この場合、第１関節部９の角度ｑ１を予め定める時間毎に予め定める角変位量、順次変化させると、対象点ｐｈを所定位置に位置させた状態で、第１関節部９の角変位とともに、第２〜第４関節部１０〜１２が角変位する。

図８（１）に示す初期状態から、第１関節部９の角度ｑ１を、たとえばマイナス方向に角変位すると、図８（２）〜図８（３）に順に示すように、多関節ロボット３０の形態が変化する。第１関節部９の角度ｑ１をマイナス方向に角変位させた場合に、図８（３）に示すように、第１関節部９が０度となることで極小値の可動限界となる。この場合、第４関節部１２の角度ｑ４が１８０度の状態で、第１アーム体３と第２アーム体４とが突っ張ってしまい、さらなる角変位が不可能な状態となる。

図１０は、図１に示す多関節ロボット３０において、第４関節部１２を冗長関節部として、その角度ｑ４を順次角変位させた状態を示す正面図であり、図１０（１）〜図１０（５）の順に変化する。また図１１は、図１０において各関節部９〜１２の角度ｑ１〜ｑ４の時間変化を示すグラフである。図１１（１）は、第１関節部９の角度ｑ１の時間変化を示し、図１１（２）は、第２関節部１０の角度ｑ２の時間変化を示し、図１１（３）は、第３関節部１１の角度ｑ３の時間変化を示し、図１１（４）は、第４関節部１２の角度ｑ４の時間変化を示す。

図１０および図１１には、対象点ｐｈを固定位置に位置させた場合に、多関節ロボットが回転揺動状態となり、第１関節部９および第２関節部１０が揺動可能であって、第３関節部１１および第４関節部１２が回転可能である場合を示す。この場合、第４関節部１２を予め定める時間毎に予め定める角変位量、順次変化させると、対象点ｐｈを所定位置に位置させた状態で、第４関節部１２の角変位とともに、第１〜第３関節部９〜１１が角変位する。

図１１（１）に示す初期状態から、第４関節部１２の角度ｑ４を、たとえばプラス方向に角変位すると、図１１（２）〜図１１（５）に順に示すように、多関節ロボット３０の形態が変化する。第４関節部１２の角度ｑ４を指定してプラス方向に角変位させた場合には、第４関節部１２の角度ｑ４が１８０度以上となっても、第１アーム体３と第２アーム体４とが突っ張ることがなく、第１関節部９を冗長関節部として選択した場合に比べて、第４関節部１２のさらなる角変位が可能となる。

このように、第４関節部１２を冗長関節部として選択することで、対象点ｐｈを固定状態に維持したうえで、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる。実際には、対象点ｐｈの固定位置によっては、第１または第２関節部を冗長関節部として選択したほうが、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる場合がある。

したがって第１関節部９と第２関節部１０との２つのうちの１つと、第３関節部１１と第４関節部１２との２つのうちの１つとを冗長関節部として選択することで、対象点ｐｈを固定状態に維持したうえで、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる。

このことから本発明の第２実施形態として、図２に示す関節部操作スイッチ４３は、第１関節部９と第２関節部１０のいずれかと、第３関節部１１と第４関節部１２のいずれかとの２つが選択可能に配置される。このように４つではなく２つのスイッチが設けられるだけでも、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる。これによって操作者は、４つの関節部９〜１２の全てを冗長関節部として選択可能とする必要がなく、形態変更動作における入力部２０の操作性を向上することができる。また関節部操作スイッチ４３を、４つから２つのスイッチとすることで、形態変更動作に必要なスイッチを減らすことができ、操作性を向上するとともに、スイッチとして必要な部品点数を減らすことができる。

また中央演算処理部１９は、第１および第２関節部９，１０のいずれかが冗長関節部として選択された場合と、第３および第４関節部１１，１２のいずれかが冗長関節部として選択された場合とにおいてのみ各関節部を演算するだけでよく、各関節部の角度を演算するのに必要な計算プログラムを２つにすることができ、計算プログラムを４つ用意する場合に比べて、中央演算処理部１９に必要な記憶容量を小さくすることができる。また本実施の形態では、第１関節部９と、第４関節部１２とが冗長関節部として選択可能に設定される。このように第１関節部９と第４関節部１２とを冗長関節部として選択可能とすることによって、他の関節部を冗長関節部として選択する場合に比べて、各関節部の角度を演算するための計算プログラムを簡単化することができ、短時間に各関節部の角度を求めることができる。

図１２は、中央演算処理部１９の他の形態変更動作の手順を説明するためのフローチャートである。まずステップｂ０で、中央演算処理部１９は、多関節ロボット３０を制御可能な状態で、形態変更スイッチ４５が押下されたことを判断すると、ステップｂ１に進み、形態変更動作を開始する。

中央演算処理部１９は、ステップｂ１〜ｂ２で、上述したステップａ１〜ａ２と同様の同様の手順を順に行い、ステップｂ３に進む。ステップｂ３では、中央演算処理部１９は、上述するステップａ３と同様にして、第１〜第４関節部９〜１２の角度を求めるための計算プログラムにしたがって、第１〜第４関節部９〜１２のうちで、冗長関節部以外の各関節部の角度を解析的に求める。また冗長関節部の角度は、ステップｂ２で既に決定しているので、演算することなく求めることができる。このようにして第１〜第４関節部９〜１２の各角度ｑ１〜ｑ４を求めるとステップｂ４に進む。

ステップｂ４では、中央演算処理部１９は、ステップａ４と同様にして、ステップｂ３の演算結果に基づいて、ステップｂ２で決定された冗長関節部の角度が可動限界に到達していないことを判断すると、ステップｂ５に進む。またステップｂ２で決定された冗長関節部の角度が可動限界に到達または超えていることを判断するとステップｂ９に進む。

ステップｂ５では、現時点での冗長関節部の角度が可動限界に到達しているか否かを判断する。現時点での冗長関節部が可動限界に到達していないと判断するとステップｂ６に進み、現時点での冗長関節部が可動限界に到達していると判断するとステップｂ７に進む。

ステップｂ６では、各関節部９〜１２において演算した角度が２とおり求められた場合、以前に、冗長関節部以外の各関節部の角度を求めるために用いた離散形態符号を用いて、２つの角度のうちから１つの角度を選択する。たとえば前回角度がプラスの離散形態符号を用いて決定される場合には、ステップｂ６では、プラスの離散形態符号を用いた場合に得られる角度を関節部が移動すべき角度として選択する。このように関節部が取り得る２とおりの角度のうちから１つの角度を選択するとステップｂ８に進む。

ステップｂ７では、各関節部９〜１２において演算した角度が２とおり求められた場合、以前に、冗長関節部以外の各関節部の角度を求めるために用いた離散形態符号を反転した離散形態符号を用いて、２つの角度のうちから１つの角度を選択する。たとえば前回角度がプラスの離散形態符号を用いて決定される場合には、ステップｂ７では、マイナスの離散形態符号を用いた場合に得られる角度を関節部が移動すべき角度として選択する。このように関節部が取り得る２とおりの角度のうちから１つの角度を選択するとステップｂ８に進む。

ステップｂ８では、ステップｂ６またはステップｂ７で求めた第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を演算結果として、出力部２２に与え、ステップｂ９に進む。出力部２２は、与えられた演算結果を多関節ロボット３０に与える。これによって多関節ロボット３０は、対象点ｐｈの位置を維持した状態で、指定された冗長関節部が冗長角度となるように、各関節部９〜１２を角変位する。

ステップｂ９では、中央演算処理部１９は、形態変更動作を終了する終了指令が与えられた否かを判断する。たとえば再び形態変更スイッチ４５が押下されることで、終了指令が与えられたことを判断してもよい。中央演算処理部１９は、形態変更動作を終了する指令が与えられていないと判断すると、ステップｂ１に戻り、関節部操作スイッチ４３が再び押下されるのを待機する。またステップｂ９で形態変更動作を終了する指令が与えられたことを判断すると、ステップｂ１０に進み、ステップｂ１０で形態変更動作を終了する。

図１３は、中央演算処理部１９が図１２に示す形態動作の手順を行った場合における第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を示すグラフである。図１３（１）は、第１関節部９の角度ｑ１の時間変化を示し、図１３（２）は、第２関節部１０の角度ｑ２の時間変化を示し、図１３（３）は、第３関節部１１の角度ｑ３の時間変化を示し、図１３（４）は、第４関節部１２の角度ｑ４の時間変化を示す。

対象点ｐｈの位置を決定した状態で、操作者が入力部２０を操作して、第１関節部９に対応するマイナス方向移動スイッチを一度押下する。この場合、中央演算処理部１９は、第１関節部９を冗長関節部として決定する。また中央演算処理部１９は、第１関節部９について、マイナス方向移動スイッチ押下前の角度から予め定める角変位量で、マイナス方向に角変位した位置を冗長角度として決定する。

図１２に示す形態動作の手順を行う場合、マイナス方向移動スイッチが順次押下されると、中央演算処理部１９は、ステップｂ１からステップｂ６、ステップｂ８を経由してステップｂ９に達する動作を行う。これによって中央演算処理部１９は、対象点ｐｈの位置を保ちつつ、図１３（１）に示すように、第１関節部９の角度ｑ１を時間経過とともに小さくさせるとともに、図１３（２）〜（４）に示すように、第２関節部１０〜第４関節部１２の角度ｑ２〜ｑ４をそれぞれ変化させる。

マイナス方向移動スイッチが押下され続けると、第１関節部９の角度ｑ１が可動限界となる角度（図１３では０度）に達する。この場合、中央演算処理部１９は、ステップｂ５〜ｂ８を経由せずに、ステップｂ９に達する動作を行う。これによって中央演算処理部１９は、関節部の角変位指令を演算せず、各関節部９〜１２の角変位を停止した状態で維持させる。可動限界に達した第１時期ｔ１を経過後、さらにマイナス方向移動スイッチが押下されても、各関節部９〜１２の角度は、可動限界に達した位置を維持する。

また第１時期ｔ１より後となる第２時期ｔ２で、第１関節部９に対応するプラス方向移動スイッチが押下されると、中央演算処理部１９は、第１関節部９について、プラス方向移動スイッチ押下前の角度から予め定める角変位量で、プラス方向に角変位した位置を冗長角度として決定する。これによって第１関節部９の角度ｑ１が可動限界から脱出する。第２時期ｔ２では、中央演算処理部１９は、ステップｂ１からステップｂ７、ステップｂ８を経由してステップｂ９に達する動作を行う。この場合、中央演算処理部１９は、第１時期ｔ１以前に、第４関節部１２の角度ｑ４を求めるために用いた離散形態符号を反転して、第４関節部１２の角度ｑ４を求める。

第２時期ｔ２より後で、プラス方向移動スイッチが押下されつづけると、中央演算処理部１９は、ステップｂ１からステップｂ６、ステップｂ８を経由してステップｂ９に達する動作を行う。この場合、中央演算処理部１９は、第２時期ｔ２で用いた離散形態符号を用いて、第４関節部１２の角度ｑ４を求める。これによって中央演算処理部１９は、対象点ｐｈの位置を保ちつつ、図１３（１）に示すように、第１関節部９の角度ｑ１を時間経過とともに大きくさせるとともに、図１３（２）〜（４）に示すように、第２関節部１０〜第４関節部１２の角度ｑ２〜ｑ４をそれぞれ変化させる。

このように第１時期ｔ１より前と、第２時期より後とでは、各関節部９〜１２の角度ｑ１〜ｑ４を求めるのに用いた離散形態符号が異なる。したがって１つの冗長関節部を選択するだけで、対象点ｐｈを固定状態に維持したうえで、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる。言換えると、多関節ロボット３０が回転揺動状態であって、揺動角変位する関節部が選択された場合であっても、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる。したがって操作者は、４つの関節部９〜１２のうちの１つを選択するだけでよく、全てを冗長関節部として選択可能とする必要がなく、形態変更動作における入力部２０の操作性を向上することができる。

また上述する形態変更動作を中央演算処理部１９が行う場合には、関節部操作スイッチ４３を、１つのスイッチとすることができる。この場合、形態変更動作に必要なスイッチを減らすことができ、スイッチとして必要な部品点数を減らすことができる。また各関節部の角度を演算するのに必要な計算プログラムを１つにすることができる。また第１関節部を冗長関節部として選択可能とすることによって、計算プログラムをより簡単化することができる。

図１４は、形態変更動作における中央演算処理部１９の予測報知動作の手順を説明するためのフローチャートである。本実施の形態では、形態変更動作にあたって、中央演算処理部１９は、冗長関節部を決定した場合に、残余の関節部が角変位する方向を報知するとともに、各アーム体の移動量および移動方向を報知する予測報知動作を行うことができる。予測報知動作は、図３および図１２に示す形態変更動作のいずれにも併用して行うことができる。したがって中央演算処理部１９は、複数の関節部の角変位する方向を予測して報知する予測報知手段となる。

まずステップｃ０で、中央演算処理部１９は、対象点ｐｈの位置が決定された状態で、予測報知動作の動作指令が与えられたことを判断すると、ステップｃ１に進み、予測報知動作を開始する。ステップｃ１では、中央演算処理部１９は、移動すべき冗長角度の移動量を決定し、ステップｃ２に進む。以下、移動量は、移動方向も含むものとする。たとえば、たとえば関節部操作スイッチ４３のうち、プラス方向移動スイッチまたはマイナス方向移動スイッチが直前に押下されていると、同じ移動スイッチが次に押下された場合に冗長関節部が角変位する移動量と移動方向とを決定する。

ステップｃ２では、中央演算処理部１９は、対象点ｐｈの移動量と、各関節部９〜１２の角度ｑ１〜ｑ４との関係を示す第１ヤコビ行列Ｊｈに基づいて、対象点ｐｈが固定位置となる場合の各関節部９〜１２の角度ｑ１〜ｑ４の移動方向を決定する。具体的には、中央演算処理部１９は、第１ヤコビ行列Ｊｈを変形した行列式に、ステップｃ１で求めた移動量とを代入することによって、残余の角度の移動量を求めることができる。このようにして冗長関節部の移動量を決定した場合において、残余の角度の移動量を演算すると、ステップｃ３に進む。

ステップｃ３では、中央演算処理部１９は、設定されるアーム体の移動量と、各関節部９〜１２の角度ｑ１〜ｑ４との関係を示す第２ヤコビ行列Ｊｅに基づいて、対象点ｐｈが固定位置となる場合の各関節部９〜１２の角度ｑ１〜ｑ４の移動量とを決定する。具体的には、中央演算処理部１９は、第２ヤコビ行列Ｊｅに基づく行列式に、ステップｃ１およびｃ２で求めた各関節部９〜１２の角度ｑ１〜ｑ４を代入することによって、設定されるアーム体の移動量を求めることができる。このようにして冗長関節部の移動量を決定した場合において、設定されるアーム体の移動量を演算すると、ステップｃ４に進む。

ステップｃ４では、中央演算処理部１９は、ステップｃ２およびステップｃ３で演算した演算結果を入力部２０の表示部４２によって報知し、ステップｃ１に戻る。中央演算処理部１９は、予め定める終了条件を満足するまで、ステップｃ１〜ｃ４を繰返し、終了条件を満足すると、予測報知動作を終了する。たとえば中央演算処理部１９は、形態変更動作を終了するとともに予測報知動作を終了する。

このように予測報知動作を行うことによって、形態変更動作において、冗長関節部をプラスまたはマイナス方向に角変位した場合に、残余の関節部がプラスまたはマイナス方向のいずれに角変位するかを報知することができ、操作性を向上することができる。

同様に設定されるアーム体が、プラスまたはマイナス方向のいずれに角変位するかを報知することができ、操作性を向上することができる。たとえば設定されるアーム体は、第１〜第４アーム体３〜６の全てであってもよく、また操作者が注目したい任意のアーム体であってもよい。本実施の形態では、第４アーム体５と第３アーム体６との間の部分となる肘位置ｐ２の移動量を報知することによって、中央演算処理部１９は、多関節ロボット３０のおおよその形態を把握させることができる。

操作者は、報知された情報を確認することで、各関節部および設定されるアーム体が動くであろう方向を容易に把握することができ、利便性を向上することができる。たとえば、形態変更動作を行って、障害物に衝突することを回避する形態に多関節ロボットを変形させる場合、ロボットの形態変化を予測することができるので、操作者は、ロボットが障害物に衝突することをより確実に防ぐことができる。

また上述した予測報知動作における演算は、ヤコビ行列Ｊｈ，Ｊｅに基づいて行われる。したがって演算結果で有る各関節部９〜１２の移動量および設定されるアーム体の移動量は、解析解でなく近似解となる。しかしながら報知情報は、厳密な情報が要求されず、短時間および容易に演算されることが望まれる。本実施の形態に示すように、ヤコビ行列に基づいて、予測報知に用いる演算結果を求めることで、短時間でかつ容易に演算することができる。

次に、ステップｃ２における各関節部９〜１２の移動量演算工程の詳細について説明する。上述した（１）式に基づいて、対象点ｐｈの移動量を求める第１ヤコビ行列Ｊｈは、次の（５）式によって表わされる。

ここで、ｐｈｘ、ｐｈｙおよびｐｈｚは、（１）式で定義した関数であり、Ｊｈは、第１ヤコビ行列を示す。

また第４アーム体５と第３アーム体６との間の肘位置ｐ２を求める第２ヤコビ行列Ｊｅは、（５）式に示す第１ヤコビ行列Ｊｈのｐｈｘ、ｐｈｙおよびｐｈｚの関数に、Ｌ４＝ゼロを代入した行列によって表わされる。

対象点ｐｈの微小移動量Δｐｈと、各関節部の微小移動量Δｑとの間には、次の（６）式の関係を有する。

形態変更動作では、対象点ｐｈの位置が固定されるので、（６）式の左辺は、ゼロである。これによって（６）式は、次の（７）式となる。

ここで、Δｑ１は、第１関節部９の微小移動量Δｑ１である。この微小移動量Δｑ１は、ステップｃ１で決定される。たとえば関節部を移動する速度が予め決定されている場合、その速度に制御周期をかけた値となる。第１関節部９をプラス方向に移動させる場合にはΔｑ１が正となり、マイナス方向に移動させる場合には、Δｑ１が負となる。

（７）式に、第１関節部９の微小移動量Δｑ１を代入することによって、第２〜第３関節部１０〜１２の微小移動量Δｑ２〜ｑ４を求めることができる。中央演算処理部１９は、（７）式を記憶する。ステップｃ２において、（７）式に冗長関節部の移動量Δｑ１を代入することによって、冗長関節部である第１関節部以外の各関節部１０〜１２の微小移動量Δｑ２〜Δｑ４を求めることができる。求めた各関節部の移動方向の正負を表示部４２に表示させることによって、操作者は、冗長関節部の移動方向を選択した場合に残余の関節部が移動する移動方向を把握することができる。

次に、ステップｃ３における各関節部９〜１２の移動量演算工程の詳細について説明する。肘位置ｐ２の微小移動量Δｐｅと、各関節部の微小移動量Δｑとには、次の（８）式に示す関係がある。

ここで、ΔＰｅは肘位置ｐ２の微小移動量ΔＰｅであり、Ｊｅは、上述した第２ヤコビ行列Ｊｅである。（８）式に、（７）式で求めた各関節部１０〜１２の微小移動量Δｑ２〜ｑ４と、冗長関節部である第１関節部９の微小移動量Δｑ１を代入することによって、肘位置ｐ２の微小移動量ΔＰｅを求めることができる。

中央演算処理部１９は、（８）式を記憶する。ステップｃ３において、（８）式に冗長関節部の移動量Δｑ１、ステップｃ１およびステップｃ２で求めた各関節部９〜１２の微小移動量Δｐ１〜Δｐ４を代入することによって、肘位置ｐ２の微小移動量ΔＰｅを求めることができる。求めた肘位置ｐ２の移動方向の正負を表示部４２に表示させることによって、操作者は、冗長関節部の移動方向を選択した場合に肘位置ｐ２が移動する移動方向たとえば基台１７を基準として、上下左右に移動する方向を把握することができる。

図１５は、本発明の第２実施形態である多関節ロボット１の制御装置２を示すブロック図である。図１５に示す多関節ロボット１および制御装置２は、図１に示す多関節ロボット３０および制御装置２と類似した構成を示し、類似した構成については同様の参照符号を付して、説明を省略する。

図１５に示す多関節ロボット１は、７自由度を有する冗長ロボットであり、６つのアーム体３〜８と、隣接する２つのアーム体を回転自在に連結する７つの関節部９〜１５とを有する。各アーム体３〜８は、それぞれ連結されて直列状に延びるアーム構成体１６を構成する。また多関節ロボット１は、アーム構成体１６の基端部に連結される基台１７と、アーム構成体１６の先端部に連結されるエンドエフェクタ１８とを有する。

各関節部９〜１５は、同軸関節部と傾斜関節部とのうちのいずれかである。具体的には、第１関節部９、第５関節部１３および第７関節部１５は、同軸関節部であり、第２関節部１０、第３関節部１１、第４関節部１２および第６関節部１４は、傾斜関節部である。したがって第１〜第４アーム体３〜６および、第１〜第４関節部９〜１２は、図１に示す多関節ロボット３０と同様の構成を有する。

第４アーム体６の一端部は、第４関節部１２によって、第３アーム体５の軸線Ａ５および第４アーム体６の軸線Ａ６に対して所定の角度、本実施の形態では４５度で傾斜する第４回転軸線Ａ１２まわりに回転自在に、第３アーム体５の他端部に連結される。第５アーム体７の一端部は、第５関節部１３によって、第４アーム体６の軸線Ａ６および第５アーム体７の軸線Ａ７と同軸の第５回転軸線Ａ１３まわりに回転自在に、第４アーム体６の他端部に連結される。

アーム構成体１６の他端部である第６アーム体８の一端部は、第６関節部１４によって、第５アーム体７の軸線Ａ７および第６アーム体８の軸線Ａ８に対して所定の角度、本実施の形態では４５度で傾斜する第６回転軸線Ａ１４まわりに回転自在に、第５アーム体７の他端部に連結される。多関節ロボット１の先端部となるエンドエフェクタ１８は、第６アーム体８の他端部に、第７関節部１５によって、第６アーム体８の軸線Ａ８と同軸の第７回転軸線Ａ１５まわりに回転自在に連結されている。エンドエフェクタ１８は、多関節ロボット１のうちで、位置および姿勢を制御すべき設定部分となる。

このように各関節部９〜１５は、アーム体を介して互いに角変位可能に連結される。また各関節部９〜１５が角変位することで、アーム構成体１６は、図１５に示すように、アーム体のそれぞれの軸線が同軸に配置されて一直線状に延びる状態に変形可能に構成される。各アーム体３〜６の軸線Ａ３〜Ａ６が一直線状に延びた状態では、第２回転軸線Ａ１０と第３回転軸線Ａ１１とは平行となり、第３回転軸線Ａ１１と第４回転軸線Ａ１２とは垂直となる。また第４アーム体６に設定される対象点ｐｈは、アーム構成体１６の先端部に配置され、基部１７は、アーム構成体１６の他端部に配置される。

また直列方向一端部から直列方向他端部に向かって並ぶ３つの関節部のそれぞれの回転軸線、すなわち第５関節部１３の回転軸線Ａ６と、第６関節部１４の回転軸線Ａ７と、第７関節部１５の回転軸線Ａ８とは、予め定められる一点となる３軸交点Ｐで交差する。このように第５〜第７関節部１３〜１５は、３軸交点Ｐを構成する３つの３軸交点形成関節部となる。

多関節ロボット１は７自由度を有し、各関節部９〜１５を回転させることで、エンドエフェクタ１８を任意の位置および姿勢に位置決め可能である。多関節ロボット６は、設備等が複雑に入り組んで作業経路が複雑な場合や、他のロボットと近接して並列に配置されて動作範囲を大きくすることができない場合に好適に用いられる。エンドエフェクタ１８は、たとえば溶接トーチおよびハンドリング装置などの手先装置であってもよい。各アーム体３〜８を回転させる回転機構などは、図１に示す多関節ロボット３０と同様の構成を有する。また制御装置２についても、その構成は、図１に示す制御装置２と同様の構成を有する。

中央演算処理部１９は、予め記憶される動作プログラムに従って、入力部２０から与えられる各種情報に応答して、多関節ロボット１を制御するために必要な演算を行う。本実施の形態では、中央演算処理部１９は、エンドエフェクタ１８を予め定める目標位置および目標姿勢に移動させるのに必要な、各関節部９〜１５の角度ｑ１〜ｑ７をそれぞれ演算する。

制御装置２は、エンドエフェクタ１８を予め指定される移動経路に沿って移動させるにあたって、回転すべき各関節部９〜１５の角度ｑ１〜ｑ７を演算する。この移動経路は、ロボット動作前に、操作者によって入力部２０から中央演算処理部１９に入力される。本発明の制御装置２に制御される多関節ロボット１は、７つの関節部９〜１５を有することで、冗長性を有する。すなわち、基台１７に対して、エンドエフェクタ１８を予め定める位置および姿勢で固定した状態で、各関節部９〜１５の取り得る角度ｑ１〜ｑ７の組合せが複数通り存在する。

本実施形態の制御装置２は、上述した形態変更動作を行うことができる。形態変更動作を行うにあたって、制御装置２は、複数の関節部９〜１５のうちから選択された冗長関節部と、その冗長関節部の冗長角度と、エンドエフェクタ１８を固定すべき位置および姿勢とが入力部２０を介して与えられる。中央演算処理部１９は、エンドエフェクタ１８を固定すべき固定状態に固定したうえで、冗長関節部が冗長角度となる場合の、残余の関節部の角度を演算する演算手段となる。出力部２２は、中央演算処理部１９の演算結果を多関節ロボットに与える。演算結果が与えられた多関節ロボットは、冗長関節部を冗長角度とするとともに、残余の関節部の角度を演算結果に応じて位置合わせして、エンドエフェクタ１８を指定された固定状態とする。

このように形態変更動作を行うことによって、エンドエフェクタ１８を固定した状態で、アーム構成体１６の形状および姿勢などの形態を変更させることができる。操作者は、ロボットを動作させながらロボットの形態を確認することによって、エンドエフェクタ１８を固定した状態で、障害物との衝突を回避するアーム構成体１６の形態を実現する各関節部９〜１５の角度を容易に決定することができる。

図１６は、図１２に示す多関節ロボットを制御する制御装置２の入力部２０を示す正面図である。入力部２０の構成は、図２と同様の構成を有する。具体的には、入力部２０は、操作部４１と、表示部４２とを含む。操作部４１には、各関節部操作スイッチ４３と、ツール位置スイッチ４４と、形態変更スイッチ４５とを含む。また操作部４１は、ロボットの動作プログラムを入力するための命令キー、カーソルキーおよびテンキーなどをさらに含む。

関節部操作スイッチ４３は、各関節部９〜１５の角度ｑ１〜ｑ７を入力するためのスイッチであって、各関節部９〜１５について、プラス方向およびマイナス方向への予め定める角変位量の角変位指令が与えられる。ツール移動スイッチ４４は、ベース座標軸またはツール座標軸に従ってエンドエフェクタ１８の移動指令が与えられる。また形態変更スイッチ４５は、対象点ｐｈを予め定める位置に固定したまま、アーム構成体１６の形態を変化させるための形態変更指令が与えられる。

形態変更動作を行う場合、ツール移動スイッチ４４は、エンドエフェクタ１８を固定すべき位置および姿勢が入力される固定条件入力手段となる。また形態変更スイッチ４５と関節部操作スイッチ４３とによって、冗長関節部が入力される冗長関節部入力手段と、冗長関節部の角度である冗長角度が入力される冗長角度入力手段とが構成される。

図１７は、第２実施形態における中央演算処理部１９の形態変更動作の手順を説明するためのフローチャートである。まずステップｄ０で、中央演算処理部１９は、多関節ロボット１を制御可能な状態で、形態変更スイッチ４５が押下されたことを判断すると、ステップｄ１に進み、形態変更動作を開始する。

ステップｄ１では、中央演算処理部１９は、ツール移動スイッチ４４によってエンドエフェクタ１８の移動指令が与えられると、エンドエフェクタ１８を指定された位置および姿勢に移動するための演算結果を出力部２２に与える。またツール移動スイッチ４４による移動指令がなければ、現時点のエンドエフェクタ１８の位置および姿勢を維持させる。中央演算処理部１９は、関節部操作スイッチ４３が押下されたと判断すると、その時点のエンドエフェクタ１８の位置および姿勢を固定すべき固定条件として決定し、ステップｄ２に進む。

ステップｄ２では、中央演算処理部１９は、関節部操作スイッチ４３によって指定された関節部を、冗長関節部として決定する。冗長関節部は、第１〜第４関節部９〜１２のいずれか１つとなる。また関節部操作スイッチ４３によって指定された角度を、冗長角度として決定する。このように冗長関節部および冗長角度を決定するとステップｄ３に進む。

ステップｄ３では、中央演算処理部１９は、ステップｄ１で決定したエンドエフェクタ１８の位置および姿勢に基づいて、３軸交点Ｐの座標を演算する。３軸交点Ｐは、第５関節部１３、第６関節部１４、第７関節部１５の各回転軸線Ａ１３，Ａ１４，Ａ１５が一点で交わる点であるので、エンドエフェクタ１８の位置および姿勢が決定されると、一意的に求めることができる。このように中央演算処理部１９は、３軸交点Ｐを演算する３軸交点演算手段となり、３軸交点Ｐを求めるとステップｄ４に進む。

ステップｄ４では、中央演算処理部１９は、ステップｄ３で演算した３軸交点Ｐの３次元位置に、３軸交点Ｐが位置する場合における、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を求める。具体的には、第１〜第４関節部９〜１２のそれぞれの角度ｑ１〜ｑ４を変数として、３軸交点Ｐの座標を表わす順変換関係式と、ステップｄ２で決定した冗長関節部の冗長角度と、ステップｄ３で演算した３軸交点Ｐの座標とに基づくことによって、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を解析的に求めることができる。第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を解析的に求める場合、対象点ｐｈの座標を３軸交点Ｐの座標に置き変えることで、上述するステップａ３と同様の方法を用いて求めることができる。

したがって中央演算処理部１９は、冗長関節部の冗長角度と、３軸交点Ｐのｘ座標、ｙ座標、ｚ座標を代入することで、第１〜第４関節部９〜１２のうちで冗長関節部以外の各関節部の角度を解析的に求めることが可能な方程式を表わす計算プログラムを記憶している。中央演算処理部１９は、第１〜第４関節部９〜１２の計算プログラムにしたがって、第１〜第４関節部９〜１２のうちで、冗長関節部以外の各関節部の角度を解析的に演算し、ステップｄ５に進む。このように中央演算処理部１９は、注目する第１〜第４関節部９〜１２の角度ｑ１〜ｑ４をそれぞれ演算する注目関節部角度演算手段となる。

ステップｄ５では、中央演算処理部１９は、ステップｄ２で決定された冗長関節部の冗長角度が可動限界に到達していないことを判断するとステップｄ６に進み、可動限界に到達しているか、または可動限界を超えているかを判断すると、ステップｄ８に進む。

ステップｄ６では、中央演算処理部１９は、エンドエフェクタ１８が固定位置および固定姿勢となる、第５〜第７関節部１３〜１５の角度ｑ５〜ｑ７を求める。ステップｄ４で求めた第１〜第４関節部９〜１２の角度ｑ１〜ｑ４から、第４アーム体６の姿勢を求めることができる。第４アーム体６の姿勢が決定された状態では、マニピュレータ１８が予め定める位置および姿勢となる場合の第５〜第７関節部１３〜１５の角度ｑ５〜ｑ７は、一意に決定することができる。

具体的には、中央演算処理部１９は、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４の角度と、エンドエフェクタ１８の位置および姿勢と用いて、第５〜第７関節部１３〜１５の角度を解析的に求めることが可能な逆変換方程式を表わす第５〜第７関節部計算プログラムを記憶している。

中央演算処理部１９は、第５〜第７関節部計算プログラムにしたがって、第５〜第７関節部１３〜１５の角度ｑ５〜ｑ７をそれぞれ求める。このように中央演算処理部１９は、第５〜第７関節部１３〜１５の角度ｑ５〜ｑ７をそれぞれ演算する交点形成関節部角度演算手段となり、各角度を求めるとステップｄ７に進む。

ステップｄ７では、ステップｄ４およびステップｄ６で得られた第１〜第７関節部９〜１５の角度ｑ１〜ｑ７を演算結果として、出力部２２に与える。出力部２２は、与えられた演算結果を多関節ロボット１に与える。これによって多関節ロボット１は、各関節部９〜１５が角変位し、エンドエフェクタ１８を固定位置および固定姿勢に維持した状態で、冗長関節部が冗長角度となる。このとき、第１〜第７関節部９〜１５の角度ｑ１〜ｑ７は、収束演算などの近似法とは異なり、解析的な演算方法によって求められるので、冗長関節部の移動前と移動後とで、固定位置および固定姿勢からエンドエフェクタ１８がずれることがない。このように演算結果を出力部２２に与えて、多関節ロボット１を動作させると、ステップｄ８に進む。

ステップｄ８では、中央演算処理部１９は、形態変更動作を終了する終了条件を満足するか否かを判断する。たとえば形態変更解除スイッチが押下されることで、終了条件を満足することを判断してもよい。中央演算処理部１９は、終了条件を満足していないと判断すると、ステップｄ１に戻り、関節部操作スイッチ４３が押下されるのを待機する。この状態で、関節部操作スイッチ４３が再度押下されると、その指示に従ってステップｄ１〜ｄ８を繰返す。そしてステップａ７で形態変更動作を終了する指令が与えられたことを判断すると、ステップａ８に進み形態変更動作を終了する。

このようにして、中央演算処理部１９が形態変更動作を行うことによって、エンドエフェクタ１８の位置および姿勢を固定したまま、各関節部９〜１５を角変位させて、多関節ロボット１の形態を変更させることができる。したがって第１実施形態と同様の効果を得ることができる。

また図１５に示す多関節ロボット１は、第５〜第７関節部１３〜１５のうちの１つが冗長関節部として選択されてしまうと、解析的に求めることができない。これに対して、本実施の形態では、第１〜第４関節部９〜１２のいずれかを冗長関節部として選択することによって、解析解を得ることができる。また７つの関節部９〜１５から１つを選択するよりも、４つの関節部９〜１２から１つを選択するほうが、操作性を向上することができる。

また図１６に示すように、各関節部操作スイッチ４３を用いて、冗長関節部を決定する場合、形態変更スイッチ４５が押下された状態では、第１関節部〜第４関節部９〜１２の選択スイッチを有効とし、第５〜第７関節部１３〜１５の選択スイッチが無効とする。これによって冗長関節部としては、第１〜第４関節部９〜１２のいずれかについて選択可能となり、誤動作を防ぐことができる。

また本実施の形態では、第５〜第７関節部１３〜１５で三軸交点を構成したが、これに限らない。たとえば第１〜第３関節部９〜１１で３軸交点を構成した場合であっても、同様に解析解を求めることができる。この場合には、第４〜第７関節部１２〜１５のうちから１つの冗長関節部が選択される。そしてエンドエフェクタ１８に設定される座標系から、第１関節部を見た座標系に反転した座標系に変換することによって、同様に解析的に求めることができる。

すなわち、アーム体を介して互いに角変位可能に連結されて直列に並ぶ７つの関節部を含み、７つの関節部９〜１５のうちで直列方向一端部から直列方向他端部に向かって並ぶ３つの関節部の回転軸線が一点で交差する３軸交点を形成する３つの交点形成関節部を有する７自由度の多関節ロボットにおいて、アーム構成体の一端部に対する他端部の位置および姿勢が固定条件として入力される場合に、３つの交点形成関節部以外となる４つの注目する関節部のうち１つが、冗長関節部として入力されることで、形態変更動作において各関節部９〜１５の角度を解析的に求めることができる。

また上述したように、第２実施形態であっても第１および第２関節部９，１０のいずれか１つと、第３および第４関節部１１，１２のいずれか１つを冗長関節部として選択させるようにしてもよい。また図１２に示す形態変更動作を行わせてもよく、移動予測動作を行わせてもよい。移動予測動作と、形態変更動作とは、エンドエフェクタ１８を移動経路に沿って移動させて移動経路を教示する移動教示動作と併用して行ってもよい。

図１８は、移動予測動作と、形態変更動作と、移動教示動作とを行う場合における中央演算処理部１９の動作手順を説明するためのフローチャートである。まずステップｅ０で、中央演算処理部１９は、多関節ロボット１を制御可能な状態で、入力部２０のツール移動スイッチ４４によって、エンドエフェクタ１８の移動指令が与えられると、ステップｅ１に進む。

ステップｅ１では、中央演算処理部１９は、ツール移動スイッチ４４に与えられた情報に従って、エンドエフェクタ１８を移動させる移動経路を演算する。エンドエフェクタ１８が指定された移動経路に従って移動するにあたって、各関節部の角度の組合せは、複数存在する場合がある。この場合、中央演算処理部１９は、予め定める規則にしたがって複数の角度の組合せから１つを選択する。たとえばエンドエフェクタ１８側の関節部の角変位が少なくなるような角度の組合せを選択する。中央演算処理部１９は、演算結果を出力部２２を介して多関節ロボット１に与え、ステップｅ２に進む。これによって多関節ロボット１は、エンドエフェクタ１８を指定された移動経路に沿って移動させる。

ステップｅ２では、第１〜第４関節部９〜１２のいずれかが特異点近傍の警告範囲に到達するか否かを判断する。たとえば中央演算処理部１９は、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を、３軸交点位置に変換するヤコビ行列の行列式の絶対値が、予め定める設定値以下であると判定されると、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４のいずれかが、特異点近傍の警告領域に到達したことを判断する。

ステップｅ２において、第１〜第４関節部９〜１２のいずれかが警告範囲に到達したことを判断するとステップｅ３に進み、そうでないとステップｅ６に進む。ステップｅ３では、図１４に示す予測報知動作を行う。図１９は、入力部２０の表示部４２に表示される表示画面を示す図である。たとえば図１９に示すように、中央演算処理部１９は、可動限界に近づいている冗長関節部を表示させる。また中央演算処理部１９は、可動限界に近づいている関節部が、特異点から脱出する角変位方向と、脱出する方向に移動したときの肘位置ｐ２が動く方向を表示させる。このように予測情報を表示させると、ステップｅ４に進む。

ステップｅ４では、中央演算処理部１９は、形態変更指令が与えられたか否かを判断する。形態変更が与えられたと判断するとステップｅ５に進み、そうでないとステップｅ６に進む。ステップｅ５では、エンドエフェクタ１８の移動を停止して、図３または図１２に示す形態変更動作を開始し、形態変形動作が終了したことを判断するとステップｅ６に進む。

ステップｅ６では、中央演算処理部１９は、移動動作を終了する終了条件を満足するか否かを判断する。中央演算処理部１９は、移動動作の終了条件を満足していないと判断すると、ステップｅ１に戻り、ツール移動スイッチ４４が再び押下されるのを待機する。この状態で、ツール移動スイッチ４４が再度押下されると、その指示に従ってステップｅ１１〜ｅ６を順に行う。またステップｅ６で終了条件を満足することを判断すると、ステップｅ７に進み、ステップｅ７で移動動作を終了する。

これによって操作者は、移動動作を行う途中に、エンドエフェクタ１８の位置を固定したまま、ロボットの形態を変更して、冗長関節部を可動限界から離して、再び、移動指令を与えた方向にエンドエフェクタ１８を移動させることができる。このように移動動作に、中央演算処理部１９による予測動作と形態変更動作とを行わせることによって、利便性を向上することができる。

上述した本実施の形態は、発明の例示に過ぎず、発明の範囲内において構成を変更することができる。たとえば本実施の形態では、オンラインティーチング、すなわちロボットを動作させながら各関節部の角度を求めて、ロボットの動作計画の教示に用いたが、これに限らない。したがって中央演算処理部１９のうち、各関節部の角度を求めるプログラムを実行するコンピュータも本発明に属する。コンピュータに、ロボットの関節の種類、アーム体の長さなどの条件を入力した状態で、対象点ｐｈおよびエンドエフェクタ１８を予め定める固定状態に保って、冗長関節部が冗長角度となる各関節部の角度を解析的に求めることができ、この解析結果をロボットに与えてもよい。すなわち本発明の方法をオフラインティーチングやロボットの動作シミュレーションに用いることも可能である。また本実施の形態では、（１）式に示す対象点の位置を求めるための関係式を変形して、連立方程式を解いた計算式に基づいて、各関節部の角度を求めたが、対象点の姿勢を求めるための関係式、対象点の位置の一部および姿勢の一部を求めるための関係式を変形して、連立方程式を解いた計算式に基づいて、各関節部の角度を求めてもよい。

また本実施の形態では、傾斜関節部と同軸関節部とを有する多関節ロボットを用いて説明したが、冗長性を有する他のロボットであっても同様に各関節部の角度を解析的に求めることができる。たとえば図６に示す４関節ロボットであっても同様に各関節部の角度を求めることができ、設定部分の固定についても、位置のほか姿勢が固定されてもよい。

また本実施の形態では、一軸冗長ロボットについて説明したが、２軸以上の冗長性を有するロボットであっても同様に求めることができる。この場合、２つ以上の冗長関節部が選択されることで、解析的に各関節部の解を求めることができる。すなわち、注目する複数の関節部のうちで冗長関節部を除く残余の関節部の数が、設定部分を予め定める固定条件に従って拘束される自由度の数と一致するように、注目する複数の関節部の少なくとも１つが冗長関節部として入力されることで、連立方程式を解くことができ、解析的に各関節部の角度を求めることができる。

すなわち注目する複数（Ｎ）の関節部のうちで、入力された冗長関節部の数（ｎ）を除いた数（Ｎ−ｎ）が、設定部分が拘束される自由度の数（Ｍ）と一致する必要がある。この場合、順変換関係式に含まれる変数を未知数とすると、設定部分が拘束される自由度の成分をそれぞれ解とする順変換関係式の数（Ｍ）と、順変換関係式に含まれる未知数の数（Ｎ−ｎ）とが等しくなる。これらの複数の順変換関係式を連立して解くことによって、各変数の値、すなわち残余の関節部の各角度を解析的に求めることができる。たとえば２軸冗長ロボットにおいて、オンラインティーチングを行う場合、２つの関節部を冗長関節部として決定し、そのうち第１冗長関節部を任意の角度に固定し、第２冗長関節部を変化させる。これによって、上述した演算方法と同様の演算方法を適用することができ、第２冗長角度を変化した場合に各関節部が取り得る角度の組合せの解析解を順次求めることができる。

本発明の第１実施形態である多関節ロボット３０の制御装置２を示すブロック図である。 入力部２０を示す正面図である。 中央演算処理部１９の形態変更動作の手順を説明するためのフローチャートである。 対象点ｐｈの位置を予め定める位置に固定した状態で、第１関節部９を角変位させた多関節ロボット３０の状態を示す正面図である。 第１関節部９の角度ｑ１を冗長角度として設定した場合における第２〜第４関節部１０〜１２の角度ｑ２〜ｑ４を示すグラフである。 対象点ｐｈを固定した場合に多関節ロボット３０が取り得る状態の種類を説明するための１軸冗長ロボット６０を示す正面図である。 １軸冗長ロボット６０が回転揺動状態となる場合において、各関節部が取り得る角度の組合せを示す図である。 図１に示す多関節ロボット３０において、第１関節部９を冗長関節部として、その角度ｑ１を順次角変位させた状態を示す正面図である。 図８において各関節部９〜１２の角度ｑ１〜ｑ４の時間変化を示すグラフである。 図１に示す多関節ロボット３０において、第４関節部１２を冗長関節部として、その角度ｑ４を順次角変位させた状態を示す正面図である。 図１０において各関節部９〜１２の角度ｑ１〜ｑ４の時間変化を示すグラフである。 中央演算処理部１９の他の形態変更動作の手順を説明するためのフローチャートである。 中央演算処理部１９が図１２に示す形態動作の手順を行った場合における第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を示すグラフである。 形態変更動作における中央演算処理部１９の予測報知動作の手順を説明するためのフローチャートである。 本発明の第２実施形態である多関節ロボット１の制御装置２を示すブロック図である。 図１２に示す多関節ロボットを制御する制御装置２の入力部２０を示す正面図である。 第２実施形態における中央演算処理部１９の形態変更動作の手順を説明するためのフローチャートである。 移動予測動作と、形態変更動作と、移動教示動作とを行う場合における中央演算処理部１９の動作手順を説明するためのフローチャートである。 入力部２０の表示部４２に表示される表示画面を示す図である。

符号の説明

１ 多関節ロボット
２ 制御装置
３ 第１アーム体
４ 第２アーム体
５ 第３アーム体
６ 第４アーム体
９ 第１関節部
１０ 第２関節部
１１ 第３関節部
１２ 第４関節部
１９ 中央演算処理部
２０ 入力部
２２ 出力部
ｐｈ 対象点

Claims (11)

1. アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットの制御装置であって、
   前記設定部分を固定すべき固定条件が入力される固定条件入力手段と、
   冗長関節部が入力される冗長関節部入力手段であって、前記複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも１つが冗長関節部として入力される冗長関節部入力手段と、
   冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
   固定条件入力手段によって入力される固定条件と、冗長角度入力手段によって入力される冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき複数の関節部の角度をそれぞれ演算する角度演算手段と、
   前記角度演算手段で演算された演算結果を多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置。
2. 固定条件として、直列方向他端部の関節部に対する設定部分の３つの自由度が拘束される条件が入力され、
   冗長関節部入力手段は、前記複数の関節部のうちで、直列方向一端部から直列方向他端部に向かって並ぶ２つの関節部のうちの１つの関節部と、直列方向他端部から直列方向一端部に向かって並ぶ２つの関節部のうちの１つの関節部とを冗長関節部として選択可能に構成されることを特徴とする請求項１記載の多関節ロボットの制御装置。
3. 角度演算手段は、入力前の状態から冗長角度入力手段に入力された冗長角度に冗長関節部を角変位させる間に、冗長関節部が可動限界に到達することを判断した場合、その可動限界に到達した状態の関節部の角度を演算することを特徴とする請求項１または２記載の多関節ロボット。
4. 角度演算手段は、冗長角度入力手段に入力された冗長角度に冗長関節部が角変位する間に、冗長関節部が可動限界に到達した状態から脱出することを判断した場合、冗長関節部以外の関節部の角度が取り得る２つの組合せのうち、可動限界に到達する前に取り得た角度の組合せと異なる角度の組合せを演算することを特徴とすることを特徴とする請求項３記載の多関節ロボットの制御装置。
5. 前記複数の関節部の角度変化を設定部分の位置変化に変換するヤコビ行列と、冗長角度入力手段に入力される冗長角度とに基づいて、設定部分を固定条件で拘束して、冗長関節部を冗長角度に角変位するときに、複数の関節部の角変位する方向を予測し、予測結果を報知する予測報知手段をさらに備えることを特徴とする請求項１〜４のいずれか１つに記載の多関節ロボットの制御装置。
6. 前記複数の関節部のいずれかが可動限界近傍に設定される警告範囲に到達したことを判断すると、判断結果を報知する警告報知手段をさらに備えることを特徴とする請求項１〜５のいずれか１つに記載の多関節ロボットの制御装置。
7. 警告報知手段は、警告範囲に到達した関節部が、可動限界から遠ざかるような角変位方向を報知することを特徴とする請求項６記載の多関節ロボットの制御装置。
8. 隣接する２つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを含む多関節ロボットを制御することを特徴とする請求項１〜７のいずれか１つに記載の多関節ロボットの制御装置。
9. アーム体を介して互いに角変位可能に連結されて直列に並ぶ７つの関節部を含み、７つの関節部のうちで直列方向一端部から直列方向他端部に向かって並ぶ３つの関節部の回転軸線が一点で交差する３軸交点を形成する３つの交点形成関節部を有する７自由度の多関節ロボットの制御装置であって、
   ７つの関節部のうちで直列方向一端部の関節部に設定される設定部分を固定すべき位置および姿勢が固定条件として入力される固定条件入力手段と、
   ３つの交点形成関節部以外となる４つの基本関節部のうち１つが、冗長関節部として入力される冗長関節部入力手段と、
   冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
   設定部分が固定すべき位置および姿勢となる状態での３軸交点の固定位置を演算する３軸交点位置演算手段と、
   ３軸交点位置演算手段によって演算される３軸交点の位置と、冗長関節部入力手段によって入力される冗長角度とに基づいて、４つの基本関節部の各角度を変数として３軸交点の位置を求めるための関係式を解いて、３軸交点が固定位置に位置するような、４つの基本関節部の角度をそれぞれ演算する基本関節部角度演算手段と、
   注目関節部角度演算手段によって演算された演算結果と、固定条件入力手段によって入力される設定部分の位置および姿勢とに基づいて、３つの交点形成関節部の角度をそれぞれ演算する交点形成関節部角度演算手段と、
   前記各角度演算手段で演算された演算結果とを多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置。
10. アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットを制御する制御方法であって、
    前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
    冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも１つを冗長関節部として決定する冗長関節部決定工程と、
    冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
    固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と、
    角度演算工程に従ってロボットを制御する制御工程とを備えることを特徴とする多関節ロボットの制御方法。
11. アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットにおける各関節部の角度を演算する演算方法であって、
    前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
    冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも１つを冗長関節部として決定する冗長関節部決定工程と、
    冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
    固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と備えることを特徴とする多関節ロボットの各関節部の角度の演算方法。

Images