冗長関節部を有する冗長ロボットの制御装置および制御方法
【課題】 設定部分を目的とする固定位置に正確に維持したうえで、各関節部の角度を変更可能に冗長ロボットを制御する制御方法を提供する。
【解決手段】 第1〜第4関節部9〜12のうちで冗長関節部を除く残余の関節部の数が3つであり、対象点Phが3次元位置で固定される。対象点Phの3次元位置(phx,phy,phz)と、第1〜第4関節部9〜12の各角度q1〜q4を変数として対象点Phの3次元位置を求めるための順変換関係式と、冗長関節部の冗長角度とに基づいて、対象点Phが3次元位置で固定されるべき、第1〜第4関節部9〜12の角度q1〜q4をそれぞれ演算する。これによって未知数が3つの3元連立方程式を解析的に解くことができ、対象点Phを予め定める3次元位置に配置した場合における、第1〜第4関節部9〜12の角度q1〜q4を正確に求めることができる。
【解決手段】 第1〜第4関節部9〜12のうちで冗長関節部を除く残余の関節部の数が3つであり、対象点Phが3次元位置で固定される。対象点Phの3次元位置(phx,phy,phz)と、第1〜第4関節部9〜12の各角度q1〜q4を変数として対象点Phの3次元位置を求めるための順変換関係式と、冗長関節部の冗長角度とに基づいて、対象点Phが3次元位置で固定されるべき、第1〜第4関節部9〜12の角度q1〜q4をそれぞれ演算する。これによって未知数が3つの3元連立方程式を解析的に解くことができ、対象点Phを予め定める3次元位置に配置した場合における、第1〜第4関節部9〜12の角度q1〜q4を正確に求めることができる。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、冗長関節部を含む複数の関節部を有する多関節ロボットを制御する制御装置および多関節ロボットを制御する制御方法に関する。
【背景技術】
【0002】
7つ以上の複数の関節部を有する多関節ロボットの制御方法に関する従来技術として、たとえば特許文献1に開示されている。この従来技術では、冗長性を有する多関節ロボットに設定される設定部分が予め設定される固定位置に維持した状態で、ロボットを構成する複数の軸のうち、任意の1つの冗長関節部の微小移動が可能に設定される。操作者が冗長関節部を微小移動させた場合、ロボット制御装置は、設定部分が固定位置に維持されるように冗長関節部以外の残余の各関節部を微小移動させる。
【0003】
特許文献1に開示される従来技術では、ヤコビ行列に基づいて、冗長関節部以外の残余の各関節部の移動量を決定する。ここで、ヤコビ行列は、設定部分の微小変化と、ロボット各関節部の微小変化との関係を示す関数である。従来技術のロボット制御装置は、設定部分の微小変化をゼロとし、ロボットの各関節部の微小変化を示す行列のうちで冗長設定関節部の微小変化を既知として、残余の各関節部の微小変化の解を求めることによって、設定部分を固定位置範囲に維持すべき、残余の各関節部の微小移動量を求める。
【0004】
【特許文献1】特許3379540号公報
【発明の開示】
【発明が解決しようとする課題】
【0005】
従来技術では、複数の関節部のうちで任意の関節部を冗長関節部として選択可能に設定され、ヤコビ行列に基づいて、冗長関節部以外の残余の各関節部の微小移動量を求める。任意の関節部を冗長関節部として選択可能とすると、各関節部の移動量について、厳密解を求めることが困難であり、近似解を求めることになる。近似解を基に各関節部を決定すると、冗長設定関節部を変位移動させた場合に、目的とする固定位置に設定部分を正確に維持することができないという問題がある。
【0006】
設定部分のずれ量は、冗長関節部を高速移動または長距離移動する場合に顕著となる。設定部分がずれてしまうと、ロボット動作に不具合が生じる場合がある。また設定部分の位置ずれを防ぐために収束演算の回数を増やした場合には、各関節部の移動量を短時間で求めることができない。
【0007】
たとえば冗長性ロボットによって搬送対象物を搬送する場合、ロボットアームが障害物に衝突しないように位置教示を行う。位置教示時に、搬送対象物の位置姿勢を正確に維持したうえで、障害物近傍のアームを冗長関節部として微小移動させることができず、搬送不良が生じてしまうおそれがある。またたとえば冗長性を有するロボットについて、複数の関節部のうちから冗長関節部として選択する関節部を順次変更して、各冗長関節部について微小移動を繰返すと、設定部分のずれ方が複雑となり、設定部分の位置を前の状態に戻すことが困難となる。
【0008】
したがって本発明の目的は、設定部分を目的とする固定位置に正確に維持したうえで、ロボットの各関節部の角度を変更可能に、冗長ロボットを制御する制御方法および制御装置を提供することである。
【課題を解決するための手段】
【0009】
本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットの制御装置であって、
前記設定部分を固定すべき固定条件が入力される固定条件入力手段と、
冗長関節部が入力される冗長関節部入力手段であって、前記複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つが冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
固定条件入力手段によって入力される固定条件と、冗長角度入力手段によって入力される冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき複数の関節部の角度をそれぞれ演算する角度演算手段と、
前記角度演算手段で演算された演算結果を多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置である。
【0010】
また本発明は、固定条件として、直列方向他端部の関節部に対する設定部分の3つの自由度が拘束される条件が入力され、
冗長関節部入力手段は、前記複数の関節部のうちで、直列方向一端部から直列方向他端部に向かって並ぶ2つの関節部のうちの1つの関節部と、直列方向他端部から直列方向一端部に向かって並ぶ2つの関節部のうちの1つの関節部とを冗長関節部として選択可能に構成されることを特徴とする。
【0011】
また本発明は、角度演算手段は、入力前の状態から冗長角度入力手段に入力された冗長角度に冗長関節部を角変位させる間に、冗長関節部が可動限界に到達することを判断した場合、その可動限界に到達した状態の関節部の角度を演算することを特徴とする。
【0012】
また本発明は、角度演算手段は、冗長角度入力手段に入力された冗長角度に冗長関節部が角変位する間に、冗長関節部が可動限界に到達した状態から脱出することを判断した場合、冗長関節部以外の関節部の角度が取り得る2つの組合せのうち、可動限界に到達する前に取り得た角度の組合せと異なる角度の組合せを演算することを特徴とする。
【0013】
また本発明は、前記複数の関節部の角度変化を設定部分の位置変化に変換するヤコビ行列と、冗長角度入力手段に入力される冗長角度とに基づいて、設定部分を固定条件で拘束して、冗長関節部を冗長角度に角変位するときに、複数の関節部の角変位する方向を予測し、予測結果を報知する予測報知手段をさらに備えることを特徴とする。
【0014】
また本発明は、前記複数の関節部のいずれかが可動限界近傍に設定される警告範囲に到達したことを判断すると、判断結果を報知する警告報知手段をさらに備えることを特徴とする。
【0015】
また本発明は、警告報知手段は、警告範囲に到達した関節部が、可動限界から遠ざかるような角変位方向を報知することを特徴とする。
【0016】
また本発明は、隣接する2つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを含む多関節ロボットを制御することを特徴とする。
【0017】
また本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ7つの関節部を含み、7つの関節部のうちで直列方向一端部から直列方向他端部に向かって並ぶ3つの関節部の回転軸線が一点で交差する3軸交点を形成する3つの交点形成関節部を有する7自由度の多関節ロボットの制御装置であって、
7つの関節部のうちで直列方向一端部の関節部に設定される設定部分を固定すべき位置および姿勢が固定条件として入力される固定条件入力手段と、
3つの交点形成関節部以外となる4つの基本関節部のうち1つが、冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
設定部分が固定すべき位置および姿勢となる状態での3軸交点の固定位置を演算する3軸交点位置演算手段と、
3軸交点位置演算手段によって演算される3軸交点の位置と、冗長関節部入力手段によって入力される冗長角度とに基づいて、4つの基本関節部の各角度を変数として3軸交点の位置を求めるための関係式を解いて、3軸交点が固定位置に位置するような、4つの基本関節部の角度をそれぞれ演算する基本関節部角度演算手段と、
注目関節部角度演算手段によって演算された演算結果と、固定条件入力手段によって入力される設定部分の位置および姿勢とに基づいて、3つの交点形成関節部の角度をそれぞれ演算する交点形成関節部角度演算手段と、
前記各角度演算手段で演算された演算結果とを多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置である。
【0018】
また本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットを制御する制御方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と、
角度演算工程に従ってロボットを制御する制御工程とを備えることを特徴とする多関節ロボットの制御方法である。
【0019】
また本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットにおける各関節部の角度を演算する演算方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と備えることを特徴とする多関節ロボットの各関節部の角度の演算方法である。
【発明の効果】
【0020】
請求項1記載の本発明に従えば、多関節ロボットは、冗長性を有し、設定部分を予め定める固定条件で固定した状態で、各関節部の取り得る角度の組合せが複数通り存在する。本発明では、冗長関節部入力手段によって、冗長関節部が入力される。これによって複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致する。また固定条件入力手段によって、設定部分を固定すべき固定条件が既知となるとともに、冗長角度入力手段によって、冗長関節部の冗長角度が既知となる。
【0021】
このように入力手段に各種情報が入力された場合、冗長関節部の冗長角度と、固定条件とを代入することで、複数の関節部のうちで、冗長関節部以外の各関節部の角度を解析的に求めることが可能な計算式が存在する。角度演算手段は、前記計算式を表わす計算プログラムを記憶し、その計算プログラムに従うことで、各入力手段に与えられた各種情報を前記計算式に代入することで、設定部分が固定条件で拘束される場合における、注目する関節部の角度を解析的にそれぞれ求めることができる。
【0022】
出力手段は、冗長角度入力手段によって入力された冗長角度と、角度演算手段によって演算された残余の関節部の各角度との情報を多関節ロボットに与える。多関節ロボットは、出力手段から与えられた情報に基づいて、複数の関節部を角変位駆動する。これによって設定部分を予め定める固定条件で拘束するとともに、冗長関節部を冗長角度に保つように、多関節ロボットの各関節部を変位駆動することができる。ここで、残余の関節部の各角度は解析解、すなわち厳密解として求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。
【0023】
このように本発明によれば、各関節部の角度の解析解をそれぞれ演算することができるので、近似解に基づいて各関節部の角度をそれぞれ演算する場合に比べて、設定部分を予め定める固定状態に正確に保ち、冗長関節部を冗長角度にすることができる。すなわち設定部分がずれることを防ぐことができる。たとえば入力前の状態から冗長関節部の角度を高速移動または長距離移動する場合であっても、設定部分の固定状態を正確に維持することができる。また解析解を求めることに付随して、各関節部の特異点について正確に判断することができ、利便性を向上することができる。
【0024】
また、たとえば冗長関節部および冗長関節部に連なるアーム体が障害物に衝突することを回避するために、設定部分の位置を固定した状態で冗長関節部を角変位させることができる。この場合、設定部分の位置を正確に保ちつつ、障害物とアーム体との干渉を回避することができるので、設定部分の位置ずれに起因するロボットの動作不良を防ぐことができる。
【0025】
請求項2記載の本発明に従えば、固定条件として、設定部分の3つの自由度が拘束される。3自由度の拘束として、たとえば直列方向他端部の関節部に対する、設定部分のx座標、y座標、z座標が決定される。またたとえば直列方向他端部の関節部に対する、x座標、y座標および他端部の関節部に設定されるY軸まわりの姿勢が決定される。
【0026】
直列方向一端部側の2つの関節部のうちの1つの関節部と、直列方向他端部側の2つの関節部のうちの1つの関節部とを冗長関節部として選択可能とすることで、それら2つの冗長関節部の冗長角度を調整することで、設定部分を固定状態に維持したうえで、注目する複数の関節部が取り得ることが可能な全ての角度の組合せを演算可能となる。
【0027】
このように本発明によれば、4つの関節部のうちの2つを冗長関節部として選択可能とすることで、各関節部が取り得ることが可能な全ての角度の組合せを求めることができるので、4つの関節部の全て冗長関節部として選択可能とする必要がなく、利便性を向上することができる。たとえば操作者に対する操作性を向上することができるとともに、角度演算手段が記憶する計算プログラムの数を減らすことができる。
【0028】
請求項3記載の本発明に従えば、設定部分を固定状態に維持するための各関節部の角度を解析的に求めることができるので、冗長関節部が可動限界に到達したか否かを正確に判断することができる。角度演算手段は、冗長関節部が可動限界に到達することを判断すると、冗長関節部が可動限界に到達した状態の関節部の角度を演算する。また可動限界を超えるような冗長角度が入力されても、可動限界に到達した状態の関節部の角度を演算する。
【0029】
このように本発明によれば、冗長関節部の可動範囲内となる状態の関節部の角度を演算する。これによって機構的に移動不可能な移動指令値を、多関節ロボットに与えることを防ぐことができ、関節部の損傷を防ぐことができる。また解析的に求められる特異点に基づくことによって、可動限界を正確に求めることができる。したがって角度演算手段は、冗長関節部が可動限界に到達したか否かを精度よく判断することができ、可動限界に近い領域まで冗長関節部を角変位させることができる。
【0030】
請求項4記載の本発明に従えば、冗長角度入力手段化からの冗長角度の入力によって、冗長関節部が可動限界に到達した状態から脱出して、可動範囲内に移動する場合、角度演算手段は、可動限界に到達する前に取り得た角度の組合せと異なる角度の組合せを演算する。これによって、設定部分を固定した状態で、多関節ロボットが回転揺動状態となる場合に、冗長関節部として選択した関節部が揺動角変位する場合、冗長関節部が可動限界に到達する前と、可動限界から脱出した後とで、ロボットの形態を変更することができ、複数の関節部が取り得ることが可能な全ての角度の組合せを演算可能となる。したがって複数の関節部の全てを冗長関節部として選択する必要がなく、利便性を向上することができる。
【0031】
このように本発明によれば、冗長関節部を1つ選択するだけで、複数の関節部が取り得ることが可能な全ての角度の組合せを演算可能となる。したがって複数の関節部の全てを冗長関節部として選択可能とする必要がなく、利便性を向上することができる。たとえば操作者に対する操作性を向上することができるとともに、角度演算手段が記憶する計算プログラムの数を減らすことができる。
【0032】
請求項5記載の本発明に従えば、予測報知手段によって各関節部の角変位に関する予測結果を報知する。これによって操作者は、各関節部の予測情報、たとえば角変位方向および角度を知ることができ、利便性を向上することができる。たとえばロボットが障害物に衝突するか否かについて、操作者が確認しながら冗長関節部の冗長角度を指定する場合に、各関節部の動きを予測報知する。これによって操作者は、冗長角度を指定した場合に障害物に関節部が衝突するか否かを把握しやすく、利便性を向上することができる。また予測結果として示される情報は、実際に各関節部の角度を決定する場合に比べて、精度が要求されない。したがってヤコビ行列に基づく近似式で十分であり、ヤコビ行列に基づくことで、簡単にかつ短時間に予測結果を演算することができる。
【0033】
請求項6記載の本発明に従えば、注目する複数の関節部のいずれかが警告範囲に到達したことを判断すると、警告報知手段によって関節部が警告範囲に到達したことを報知する。これによって操作者は、関節部が警告限界に近づいたことを判断することができ、利便性を向上することができる。たとえば操作者は、設定部分が固定状態に維持された状態で、注目する関節部が可動限界から遠ざかるように、冗長角度を設定することによって、設定部分を固定状態に維持した状態で、関節部を可動限界から遠ざけることができ、可動限界に対する余裕を、ロボットに与えることができる。
【0034】
請求項7記載の本発明に従えば、注目する複数の関節部のいずれかが警告範囲に到達したことを判断すると、関節部が可動限界から遠ざかるような角変位方向を報知する。これによって操作者は、関節部が可動限界から遠ざかる方向を判断して、関節部が可動限界に到達することを容易に回避することができ、利便性を向上することができる。
【0035】
請求項8記載の本発明に従えば、多関節ロボットは、傾斜関節部と同軸関節部とを有する。このような多関節ロボットの場合には、多関節ロボットの動きが複雑であり、設定部分を固定状態に維持したうえで、各関節部を角変位させる指令を手動で与えるのは困難である。これに対して、上述したように本発明の制御装置を用いれば、冗長関節部の選択指令と、冗長関節部の冗長角度を与えることによって、設定部分の固定状態を正確に維持したうえで、各関節部を角変位させることができ、利便性を向上することができる。
【0036】
請求項9記載の本発明に従えば、多関節ロボットは、7つの関節部を含んで7自由度の多関節ロボットであり、設定部分を予め定める位置および姿勢に固定した状態で、各関節部の取り得る角度の組合せが複数通り存在する。本発明では、固定条件入力手段によって、設定部分を固定すべき位置および姿勢が入力される。設定部分の位置および姿勢が決定される場合、3軸交点位置演算手段によって、3軸交点の3次元位置を一意的に求めることができる。
【0037】
また冗長関節部入力手段によって、3つの交点形成関節部以外となる4つの注目する関節部のうち1つが、冗長関節部として入力される。これによって4つの注目する関節部のうちで冗長関節部を除く残余の関節部の数が3つとなり、3軸交点の拘束される自由度の数、すなわち3軸交点の拘束される座標の数と一致する。また冗長角度入力手段によって、冗長関節部の冗長角度が既知となる。
【0038】
この場合、冗長関節部の冗長角度と、3軸交点の3次元位置とを代入することで、注目する4つの関節部のうちで、冗長関節部以外の各関節部の角度を解析的に求めることが可能な計算式が存在する。注目関節部角度演算手段は、前記計算式を表わす計算プログラムを記憶し、その計算プログラムに従うことで、各入力手段に入力された各種情報に基づいて、3軸交点が3次元位置に位置する場合における、注目する4つの関節部の角度を解析的にそれぞれ求めることができる。
【0039】
また注目する4つの関節部の角度と、設定部分の位置および姿勢とを代入することで、3軸交点形成関節部の角度を解析的に求めることが可能な計算式が存在する。交点関節部角度演算手段は、前記計算式を表わす計算プログラムを記憶し、その計算プログラムに従うことで、注目関節部角度演算手段の演算結果と、固定条件入力手段に入力された情報に基づいて、設定部分が予め定める位置および姿勢に位置する場合における、3軸交点形成関節部の角度を解析的にそれぞれ求めることができる。
【0040】
出力手段は、冗長角度入力手段によって入力された冗長角度と、各角度演算手段によって演算された関節部の各角度とを多関節ロボットに与える。これによって多関節ロボットは、設定部分を予め定める位置および姿勢で固定するとともに、冗長関節部を冗長角度に保つように、各関節部を変位駆動することができる。ここで、各関節部の角度は解析解、すなわち厳密解に基づいてそれぞれ求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。
【0041】
このように本発明によれば、冗長関節部を冗長角度にしたうえで、設定部分を予め定める固定状態に正確に保つことができる。たとえば設定部分の位置および姿勢を固定した状態で、冗長関節部および冗長関節部に連なるアーム体が障害物に衝突することを回避するために、冗長関節部を角変位させる場合、設定部分の位置および姿勢を正確に保ちつつ、障害物とアーム体との干渉を回避することができる。
【0042】
請求項10記載の本発明に従えば、設定部分を固定すべき固定条件を決定するとともに、複数の関節部のうちから冗長関節部と、冗長角度を決定する。この場合、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致する。次に、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式に、各入力工程によって決定した固定条件と冗長角度とを代入することで、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する。これによって複数の関節部の角度を解析的に求めることができる。このようにして求められた演算結果にしたがって、複数の関節部をそれぞれ角変位させることによって、設定部分を固定条件で固定した状態で、冗長関節部を冗長角度とすることができる。
【0043】
本発明によれば、冗長関節部以外の残余の関節部の各角度は解析解、すなわち厳密解に基づいて求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。したがって残余の関節部の各角度を近似解に基づいて決定する場合に比べて、固定すべき固定条件から設定部分がずれることを防ぐことができる。また解析解を求めることに起因して、各関節部の特異点を正確に判断することができ、利便性を向上することができる。
【0044】
請求項11記載の本発明に従えば、設定部分を固定すべき固定条件を決定するとともに、複数の関節部のうちから冗長関節部と、冗長角度を決定する。この場合、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致する。次に、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式に、各入力工程によって決定した固定条件と冗長角度とを代入することで、設定部分が固定条件で固定されるべき、注目する複数の関節部の角度をそれぞれ演算する。これによって注目する複数の関節部の角度を解析的に求めることができる。
【0045】
本発明によれば、冗長関節部以外の残余の関節部の各角度は解析解、すなわち厳密解に基づいて求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。したがって残余の関節部の各角度を近似解に基づいて決定する場合に比べて、固定すべき固定条件から設定部分がずれることを防ぐことができる。また解析解を求めることに起因して、各関節部の特異点を正確に判断することができ、利便性を向上することができる。
【図面の簡単な説明】
【0046】
【図1】本発明の第1実施形態である多関節ロボット30の制御装置2を示すブロック図である。
【図2】入力部20を示す正面図である。
【図3】中央演算処理部19の形態変更動作の手順を説明するためのフローチャートである。
【図4】対象点Phの位置を予め定める位置に固定した状態で、第1関節部9を角変位させた多関節ロボット30の状態を示す正面図である。
【図5】第1関節部9の角度q1を冗長角度として設定した場合における第2〜第4関節部10〜12の角度q2〜q4を示すグラフである。
【図6】対象点Phを固定した場合に多関節ロボット30が取り得る状態の種類を説明するための1軸冗長ロボット60を示す正面図である。
【図7】1軸冗長ロボット60が回転揺動状態となる場合において、各関節部が取り得る角度の組合せを示す図である。
【図8】図1に示す多関節ロボット30において、第1関節部9を冗長関節部として、その角度q1を順次角変位させた状態を示す正面図である。
【図9】図8において各関節部9〜12の角度q1〜q4の時間変化を示すグラフである。
【図10】図1に示す多関節ロボット30において、第4関節部12を冗長関節部として、その角度q4を順次角変位させた状態を示す正面図である。
【図11】図10において各関節部9〜12の角度q1〜q4の時間変化を示すグラフである。
【図12】中央演算処理部19の他の形態変更動作の手順を説明するためのフローチャートである。
【図13】中央演算処理部19が図12に示す形態動作の手順を行った場合における第1〜第4関節部9〜12の角度q1〜q4を示すグラフである。
【図14】形態変更動作における中央演算処理部19の予測報知動作の手順を説明するためのフローチャートである。
【図15】本発明の第2実施形態である多関節ロボット1の制御装置2を示すブロック図である。
【図16】図12に示す多関節ロボットを制御する制御装置2の入力部20を示す正面図である。
【図17】第2実施形態における中央演算処理部19の形態変更動作の手順を説明するためのフローチャートである。
【図18】移動予測動作と、形態変更動作と、移動教示動作とを行う場合における中央演算処理部19の動作手順を説明するためのフローチャートである。
【図19】入力部20の表示部42に表示される表示画面を示す図である。
【発明を実施するための最良の形態】
【0047】
図1は、本発明の第1実施形態である多関節ロボット30の制御装置2を示すブロック図である。制御装置2によって制御される多関節ロボット30は、4自由度を有する1軸冗長ロボットであり、4つのアーム体3〜6と、隣接する2つのアーム体を回転自在に連結する4つの関節部9〜12と、基台17とを有する。各アーム体3〜6は、それぞれ連結されて直列状に延びるアーム構成体16を構成する。また基台17は、アーム構成体16の基端部に連結され、壁面または床面などの固定位置に予め固定される。
【0048】
各関節部9〜12は、同軸関節部と傾斜関節部とのうちのいずれかである。同軸関節部は、隣接する2つのアーム体を、その各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する。傾斜関節部は、隣接する2つのアーム体のうちの一方を、各アーム体の軸線に対して傾斜する回転軸線まわりに円錐回転自在に連結する。
【0049】
各関節部9〜12は、基台17から第1〜第4関節部9〜12の順で並ぶ。第1関節部9は、同軸関節部である。また第2関節部10、第3関節部11および第4関節部12は、傾斜関節部である。また本発明において、用語「回転」とは、回転軸線まわりの360度以上の回転だけでなく、軸線まわりに360度以下の角度で角変位する状態も含むものとする。
【0050】
第1アーム体3の一端部は、第1関節部9によって、第1アーム体3の軸線A3と同軸の第1回転軸線A9まわりに回転自在に基台17に連結されている。また第2アーム体4の一端部は、第2関節部10によって、第1アーム体3の軸線A3および第2アーム体4の軸線A4に対して所定の角度、本実施の形態では45度で傾斜する第2回転軸線A10まわりに回転自在に、第1アーム体3の他端部に連結される。また第3アーム体5の一端部は、第3関節部11によって、第2アーム体4の軸線A4および第3アーム体5の軸線A5に対して所定の角度、本実施の形態では45度で傾斜する第3回転軸線A11まわりに回転自在に、第2アーム体4の他端部に連結される。
【0051】
第4アーム体6の一端部は、第4関節部12によって、第3アーム体5の軸線A5および第4アーム体6の軸線A6に対して所定の角度、本実施の形態では45度で傾斜する第4回転軸線A12まわりに回転自在に、第3アーム体5の他端部に連結される。第4アーム体6の他端部には、設定部分となる対象点Phが予め設定される。したがって対象点Phは、アーム構成体16の先端部に配置され、基台17は、アーム構成体16の基端部に配置される。
【0052】
このように各関節部9〜12は、隣接するアーム体を互いに回転可能に連結する。また各関節部9〜12が角変位することで、アーム構成体16は、図1に示すように、アーム体3〜6のそれぞれの軸線A3〜A6が同軸に配置されて一直線状に延びる状態に変形可能に構成される。各アーム体3〜6の軸線A3〜A6が一直線状に延びた状態では、第2回転軸線A10と第3回転軸線A11とは平行となり、第3回転軸線A11と第4回転軸線A12とは垂直となる。
【0053】
多関節ロボット30は、各関節部9〜12を回転させることで、アーム構成体16を蛇のように変形させ、対象点Phを任意の3次元位置に位置決め可能である。たとえば多関節ロボット30は、設備等が複雑に入り組んで作業経路が複雑な場合や、他のロボットと近接して並列に配置されて動作範囲を大きくすることができない場合に好適に用いられる。
【0054】
このような4つの関節部9〜12を有する多関節ロボット30は、4自由度を有するので、可動範囲内で対象点Phを予め定める3次元位置に位置合わせした状態で、第4アーム体6の取り得る姿勢が複数通り存在する。言換えると、多関節ロボット30は、対象点Phを予め定める3次元位置に位置合わせした状態で、4つの関節部9〜12のそれぞれの取り得る角度の組合せが複数通り存在する。
【0055】
各アーム体3〜6は、各アーム体3〜6をそれぞれ個別に回転駆動するモータを内蔵する。モータは、各アーム体3〜6を予め定める任意の角度に変位可能に構成される。
【0056】
各アーム体3〜6を対応する回転軸線A9〜A12まわりに回転可能に連結するための、各関節部9〜12の回転機構は、入力側部材と出力側部材とを備え、それらが相対的に回転可能に設けられる。入力側部材は、各関節部9〜12によって連結する2つの部材の一方に連結され、出力側部材は、前記2つの部材のうち他方に連結される。モータからの回転力が入力側部材に与えられると、入力側部材と出力側部材とが相対的に回転する。これによって一方の部材と他方の部材とを相対的に回転させることができる。同軸関節部9では、その回転軸線A9が、入力側部材および出力側部材に連結されるアーム体と一致する。傾斜関節部10〜12では、その回転軸線A10〜A12が、入力側部材および出力側部材にそれぞれ連結されるアーム体の軸線に対してそれぞれ傾斜する。
【0057】
このような回転機構が設けられることによって、隣接する2つの部材を相対的に回転させることができる。ここで、本発明における「関節部の角度」とは、関節部における入力側部材に対する出力側部材の角度を意味する。言い換えると、関節部によって連結される2つの部材のうち、入力側部材に連結される一方の部材に対する、出力側部材に連結される他方の部材の関節部の回転軸線まわりの角度を意味する。また本発明において「関節部の移動」とは、関節部の角度の角変位移動を示す。
【0058】
制御装置2は、入力部20と、中央演算処理部19と、出力部22とを含んで構成される。入力部20および出力部22は、中央演算処理部19に電気的に接続される。入力部20は、操作者からの各種指令情報が入力され、入力された各種情報を中央演算処理部19に与える。本実施の形態では、中央演算処理部19と出力部22とは、制御装置本体21に設けられ、入力部20は、制御装置本体21とは別体に設けられる。具体的には、入力部20は、操作者が多関節ロボットを直接制御するためのティーチペンダントによって実現され、制御装置本体21であるロボットコントローラに対して、ケーブルによって電気的に接続される。
【0059】
中央演算処理部19は、予め記憶される動作プログラムに従って、入力部20から与えられる各種情報に応答して、多関節ロボット30を制御するために必要な演算を行う。本実施の形態では、中央演算処理部19は、第4アーム体6に設定される対象点Phを予め定める目標位置に移動させるのに必要な、各関節部9〜12の角度q1〜q4をそれぞれ演算する。中央演算処理部19は、CPUなどの処理回路と、メモリなどの記憶回路とを含んで実現される。記憶回路には、動作プログラムとともに、演算に必要な計算式、アーム体の長さ、関節部の種類などを記憶する。
【0060】
出力部22は、中央演算処理部19の演算結果を多関節ロボット30に与える出力手段となる。本実施の形態では、出力部22は、中央演算処理部19の演算結果を多関節ロボット30に設けられる電源回路に与える。電源回路は、中央演算処理部19の演算結果に応じた動力を、関節部9〜12毎に設けられるモータにそれぞれ個別に与える。動力が与えられた各モータは、中央演算処理部19の演算結果に従って、対応する関節部を所定の角度に角変位させる。
【0061】
また制御装置2は、第4アーム体6の対象点Phを予め指定される移動経路に沿って移動させるにあたって、回転すべき各関節部9〜12の角度を演算する。この移動経路は、ロボット動作前に、操作者によって入力部20から中央演算処理部19に入力される。また多関節ロボット30は、4つの関節部9〜12を有することで、基台17に対して、対象点Phを予め定める位置で固定した状態で、各関節部の取り得る角度の組合せが複数通り存在する。
【0062】
本実施形態の制御装置2は、ロボットの動作を教示するための教示動作を行うために用いられ、教示動作のための形態変更動作を行う。形態変更動作とは、対象点Phを予め定める固定条件で固定した状態で、第1〜第4関節部9〜12の角度を変化させて、ロボットの形状および姿勢などの外形的形態を変更する動作である。本実施の形態では形態変更動作は、基台17に設定されるベース座標に対する対象点Phの3次元位置を固定した状態で、第1〜第4関節部9〜12の角度を変化させる動作となる。言換えると、形態変更動作では、アーム構成体16のうちで直列方向一端部に設定される対象点Phが、アーム構成体16の他端部に設定される座標系に従って求められる拘束条件で固定される。
【0063】
形態変更動作を行うにあたって、制御装置2は、第1〜第4関節部9〜12のうちから選択された1つの冗長関節部と、その冗長関節部の角度である冗長角度と、対象点Phを固定すべき3次元位置とが入力部20を介して与えられる。中央演算処理部19は、対象点Phを固定すべき固定状態に固定したうえで、冗長関節部が冗長角度となる場合の、残余の関節部の角度を演算する。出力部22は、中央演算処理部19の演算結果を多関節ロボット30に与える。演算結果が与えられた多関節ロボット30は、冗長関節部を冗長角度とするとともに、残余の関節部の角度を演算結果に応じて位置合わせして、対象点Phを指定された固定状態とする。
【0064】
形態変更動作を行うことによって、操作者は、入力部20を介して冗長関節部を選択するとともに、その冗長角度を入力することで、対象点Phを固定した状態で、アーム構成体16の形態を変更させることができる。この場合、操作者は、中央演算処理部19に形態変更動作を行わせて、多関節ロボット30の形態を確認することによって、対象点Phを固定した状態で、障害物との衝突を回避するアーム構成体16の形態を実現する各関節部9〜12の角度を容易に決定することができる。操作者は、障害物との衝突を回避する各関節部9〜12の角度q1〜q4を決定すると、その各関節部9〜12の角度q1〜q4をロボットの移動動作を行う場合の情報として用いる。
【0065】
図2は、入力部20を示す正面図である。入力部20には、ロボット教示のための情報および指令が入力される操作部41と、操作画面を表示する表示部42とを含む。操作部41には、各関節部操作スイッチ43と、ツール移動スイッチ44と、形態変更スイッチ45とを含む。また操作部41は、ロボットの動作プログラムを入力するための命令キー、カーソルキーおよびテンキーなどをさらに含む。
【0066】
関節部操作スイッチ43は、各関節部9〜12の角度q1〜q4を入力するためのスイッチであって、各関節部9〜12について、プラス方向およびマイナス方向への予め定める角変位量の角変位指令が与えられる。ツール移動スイッチ44は、ベース座標軸またはツール座標軸に従う対象点Phの移動指令が与えられる。ここで、ベース座標軸とは、基台17に設定される座標であって、ツール座標軸とは、第4アーム体6に設定される座標である。また形態変更スイッチ45は、対象点Phを予め定める位置に固定したまま、アーム構成体16の形態を変化させるための形態変更指令が与えられる。
【0067】
形態変更動作を行う場合、ツール移動スイッチ44は、対象点Phを固定すべき位置の情報が入力される固定条件入力手段となる。また形態変更スイッチ45が押下された状態で、関節部操作スイッチ43によって1つの関節部の角変位量が指定されると、指定された関節部が冗長関節部として入力される。また、その角変位量に対応する角度が冗長関節部の角度として入力される。したがって形態変更スイッチ45と関節部操作スイッチ43とによって、冗長関節部が入力される冗長関節部入力手段と、冗長関節部の角度である冗長角度が入力される冗長角度入力手段とが構成される。
【0068】
図3は、中央演算処理部19の形態変更動作の手順を説明するためのフローチャートである。まずステップa0で、中央演算処理部19は、多関節ロボット30を制御可能な状態で、形態変更スイッチ45が押下されたことを判断すると、ステップa1に進み、形態変更動作を開始する。
【0069】
ステップa1では、中央演算処理部19は、ツール移動スイッチ44によって対象点Phの移動指令が与えられると、対象点Phを指定された位置に移動するための演算結果を出力部22に与える。またツール移動スイッチ44による移動指令がなければ、現時点の対象点Phの位置を維持させる。中央演算処理部19は、関節部操作スイッチ43が押下されたと判断すると、その時点の対象点Phの位置を固定すべき固定条件として決定し、ステップa2に進む。
【0070】
ステップa2では、中央演算処理部19は、関節部操作スイッチ43によって指定された関節部を、冗長関節部として決定する。冗長関節部は、第1〜第4関節部9〜12のいずれか1つとなる。また関節部操作スイッチ43によって指定された角度を、冗長角度として決定する。本実施の形態では、関節部操作スイッチ43のうち、プラス方向移動スイッチが押圧されると、関節部操作スイッチ43が押下される前の状態から、予め定める角変位量で冗長関節部がプラス方向に軸線まわりに角変位した角度を冗長角度として決定する。またマイナス方向移動スイッチが押圧されると、関節部操作スイッチ43が押下される前の状態から、予め定める角変位量で冗長関節部がマイナス方向に軸線まわりに角変位した角度を冗長角度として決定する。このように冗長関節部および冗長角度を決定するとステップa3に進む。
【0071】
ステップa3では、中央演算処理部19は、ステップa1で決定した対象点Phの3次元位置に、対象点Phが位置する場合における、第1〜第4関節部9〜12の角度q1〜q4を求める。具体的には、第1〜第4関節部9〜12のそれぞれの角度q1〜q4を変数として、対象点Phの3次元位置を求めるための順変換関係式と、ステップa2で決定した冗長関節部の冗長角度と、ステップa1で決定した対象点Phの3次元位置とに基づくことによって、第1〜第4関節部9〜12の角度q1〜q4を解析的に求めることができる。
【0072】
順変換問題を解くことによって、対象点Phのx座標位置を表わすx座標式、対象点Phのy座標位置を表わすy座標式、対象点Phのz座標位置を表わすz座標式は、第1〜第4関節部9〜12の各角度q1〜q4を変数とする関数で表わされる。順変換問題によるx座標式、y座標式およびz座標式に、ステップa1で求めた対象点Phの3次元位置を代入することで、それらの座標式は、第1〜第4関節部9〜12の角度q1〜q4の数、すなわち4つの未知数を有する三元連立方程式となる。
【0073】
また第1〜第4関節部9〜12の各角度q1〜q4のうちの1つは、冗長角度としてステップa2で決定しているので、冗長角度を上述した3元連立方程式に代入することによって、3つの未知数を有する三元連立方程式となる。この3元連立方程式を変形することで、3つの未知数をそれぞれ求める方程式を表わす計算式を導出することができる。この方程式に、対象点Phの座標、冗長関節部および冗長関節部の角度を代入することで、第1〜第4関節部9〜12のうちで、冗長関節部以外の関節部の各角度をそれぞれ解析的に求めることができる。
【0074】
中央演算処理部19は、冗長関節部の冗長角度と、対象点Phのx座標、y座標、z座標を代入することで、第1〜第4関節部9〜12のうちで冗長関節部以外の各関節部の角度を解析的に求めることが可能な方程式を表わす計算プログラムを記憶している。中央演算処理部19は、第1〜第4関節部9〜12の計算プログラムにしたがって、第1〜第4関節部9〜12のうちで、冗長関節部以外の各関節部の角度を解析的に演算し、ステップa4に進む。冗長関節部の角度は、ステップa2で既に決定しているので、演算することなく求めることができる。このように中央演算処理部19は、第1〜第4関節部9〜12の角度q1〜q4をそれぞれ演算する角度演算手段となる。
【0075】
ステップa4では、中央演算処理部19は、求めた各関節部9〜12の各角度q1〜q4が可動限界に到達していないことを判断するとステップa5に進み、各角度q1〜q4のいずれかが可動限界に到達していること、または可動限界を超えていることを判断すると、ステップa6に進む。たとえば中央演算処理部19は、ステップa3で求めた各関節部の角度が、解析的に求められる最大値または最小値となると、可動限界に到達したことを判断する。また各関節部の角度を求められない場合に可動限界を超えていると判断する。
【0076】
ステップa5では、ステップa3で解析的に求めた第1〜第4関節部9〜12の角度q1〜q4を演算結果として、出力部22に与える。出力部22は、与えられた演算結果を多関節ロボット30に与える。これによって多関節ロボット30は、各関節部9〜12が角変位し、対象点Phを固定位置に維持した状態で、冗長関節部を冗長角度とする。このとき、第1〜第4関節部9〜12の角度q1〜q4は、収束演算などの近似法とは異なり、解析的な演算方法によって求められるので、冗長関節部の移動前と移動後とで、固定位置から対象点Phがずれることがない。このように演算結果を出力部22に与えて、多関節ロボット30を動作させると、ステップa6に進む。
【0077】
ステップa6では、中央演算処理部19は、形態変更動作を終了する終了条件を満足するか否かを判断する。たとえば形態変更解除スイッチが押下されることで、終了条件を満足することを判断してもよい。中央演算処理部19は、形態変更動作の終了条件を満足していないと判断すると、ステップa1に戻り、関節部操作スイッチ43が再び押下されるのを待機する。この状態で、関節部操作スイッチ43が再度押下されると、その指示に従ってステップa1〜a5を順に行う。またステップa6で終了条件を満足することを判断すると、ステップa7に進み、ステップa7で形態変更動作を終了する。
【0078】
このようにして、中央演算処理部19が形態変更動作を行うことによって、操作者が、対象点Phの固定位置と、冗長関節部と、その冗長角度とを指定することによって、対象点Phの位置を確実に固定したまま、各関節部9〜12を角変位させて、多関節ロボット30の形態を変更させることができる。このとき各関節部9〜12の角度q1〜q4を解析解として求めることができるので、収束演算を用いて解を求める場合に比べて短時間で各関節部9〜12の角度を求めることができ、対象点Phの位置を固定したままロボットの形態を滑らかに変更することができる。
【0079】
また操作者は、多関節ロボット30の形態を確認しながら、中央演算処理部19に形態変更動作を行わせることができる。これによって、操作者は、対象点Phを固定位置に位置させることができる各関節部9〜12の角度q1〜q4の複数の組合せのうちから、1つを容易に選択することができる。これによって操作者は、障害物に衝突することを回避した多関節ロボット30の形態で、対象点Phを固定位置に配置させることができ、各関節部9〜12の角度q1〜q4を求めることができる。
【0080】
図4は、対象点Phの位置を予め定める位置に固定した状態で、第1関節部9を角変位させた多関節ロボット30の状態を示す正面図である。第1関節部9の角度q1をプラス方向に角変位するに伴って、多関節ロボット30は、図4(1)〜図4(8)の順で変形する。また図5は、第1関節部9の角度q1を冗長角度として設定した場合における第2〜第4関節部10〜12の角度q2〜q4を示すグラフである。図5(1)は、横軸に第1関節部9の角度q1を示し、縦軸に第2関節部10の角度q2を示す。また図5(2)は、横軸に第1関節部9の角度q1を示し、縦軸に第3関節部11の角度q3を示す。図5(3)は、横軸に第1関節部9の角度q1を示し、縦軸に第4関節部12の角度q4を示す。
【0081】
詳細については後述するが、本実施の形態では、対象点Phを予め定める3次元位置に固定した状態に維持する、各関節部9〜12の角度q1〜q4の解析解を決定することができる。これによって各関節部9〜12の角度q1〜q4の近似解を決定する場合に比べて、対象点Phが固定位置からずれることを防ぐことができる。たとえばステップa2において、関節部操作スイッチ43のうち、プラス方向またはマイナス方向移動スイッチが1回押圧された場合に、冗長関節部が角変位する角変位量が大きく設定される場合であっても、対象点Phの固定状態を正確に維持することができる。同様に、ステップa2において、冗長関節部が高速で移動する場合であっても、対象点Phの固定状態を正確に維持することができる。
【0082】
これによって形態変更動作によって、多関節ロボット30の障害物回避形態の探索を行った場合、対象点Phの位置を正確に固定した状態で、各関節部9〜12を角変位させることができる。また対象点Phを固定位置に正確に保ちつつ、障害物と各アーム体3〜6との干渉を回避することができるので、対象点Phの位置ずれに起因する多関節ロボットの動作不良を防ぐことができる。
【0083】
また中央演算処理部19は、各関節部9〜12の角度q1〜q4の解析解を求めることに付随して、冗長関節部が可動限界に到達したか否かを判断することができる。ステップa4に示すように、中央演算処理部19は、冗長関節部が可動限界に到達することを判断すると、冗長関節部が可動限界に到達した状態の関節部の角度を演算する。また可動限界を超えるような冗長角度が入力されても、可動限界に到達した状態の関節部の角度を演算する。
【0084】
これによって中央演算処理部19が、機構的に移動不可能な移動指令値を、多関節ロボットに与えることを防ぐことができ、関節部の損傷を防ぐことができる。また中央演算処理部19は、冗長関節部が可動限界に到達したか否かを精度よく判断することができるので、可動限界に近い領域まで冗長関節部を角変位させることができ、ロボットの可動範囲を広くすることができる。
【0085】
また中央演算処理部19は、冗長関節部が可動限界に到達する前に、冗長関節部が可動限界近傍の警告領域に到達したことを判断すると、その判断結果を入力部20の表示部42を介して報知してもよい。たとえば中央演算処理部19は、演算によって決定される冗長関節部の可動限界角度よりも予め定める所定角度分手前の角度に、冗長角度が達した場合に、警告領域に到達したことを判断する。この場合、中央演算処理部19は、警告範囲に到達したことを判断して報知する警告報知手段となる。
【0086】
たとえば図5の場合では、冗長関節部が第1関節部9であって、第3関節部11の角度q3が180度となった場合に可動限界に達する。この場合、中央演算処理部19は、第3関節部11の角度q3が175度に達すると、警告領域に到達したことを判断し、判断結果を表示部42に表示させる。これによって入力部20を操作する操作者は、冗長関節部が可動限界に近づいていることを判断することができ、利便性を向上することができる。さらに中央演算処理部19は、冗長関節部が警告範囲に入った場合、その関節部の角度が警告範囲を脱出する角変位方向を入力部20の表示部42に表示することが好ましい。
【0087】
本実施形態の制御装置2が制御する多関節ロボット30は、傾斜関節部を含むので、形態変更動作における多関節ロボット30の変形形態は変則的となる。この場合、操作者は、各関節部9〜12の変形を直感的に予測することが困難である。本実施の形態では、中央演算処理部19は、警告範囲および警告範囲を脱出するための角変位方向を報知するで、操作者は、警告範囲および脱出方向を容易に判断することができ、利便性を向上することができる。
【0088】
次に、ステップa3における第1〜第4角度q1〜q4の演算工程の詳細について説明する。順変換問題に基づくことによって、第1〜第4関節部9〜12のそれぞれの角度q1〜q4が決定されると、対象点Phのx座標位置phx、y座標位置phy、z座標位置phzが一義的に決定される。すなわち対象点Phのx座標、y座標、z座標を求めるための式は、第1〜第4関節部9〜12のそれぞれの角度q1〜q4を変数とする関数によって表わされ、本実施の形態では、以下に示す(1)式の関係を有する。
【0089】
【数1】
【0090】
(1)式において第1関節部9の角度をq1とし、第2関節部10の角度をq2とし、第3関節部11の角度をq3とし、第4関節部12の角度をq4とする。また第1関節部9から第2関節部10までの第1アーム体3の距離をL1とし、第2関節部10から第3関節部11までの第2アーム体4の距離をL2とし、第3関節部11から第4関節部12までの第3アーム体5の距離をL3とし、第4関節部12から対象点Phまでの距離をL4とする。またsin(θ)はθの正弦関数(サイン関数)を示し、cos(θ)は、θの余弦関数(コサイン関数)を示す。
【0091】
また(1)式において、対象点Phの座標phx,phy,phzは、基台17に設定されるベース座標軸に従って、第1アーム体3の代表点p1を原点とした場合の座標である。ここでベース座標軸は、各アーム体の軸線が一直線状に配置された状態で、その直線と同軸に延びる軸をz軸とし、z軸に垂直な平面に沿って延びてz軸を通過する軸をx軸およびy軸とする。x軸およびy軸は、互いに直交する。また各関節部9〜12の角度q1〜q4は、各アーム体3〜6の軸線が一直線状に延びた状態から関節部が角変位した角度とする。また第1アーム体3の代表点p1は、第1アーム体の軸線A1と、第2関節部10の回転軸線A10との交点に設定される。
【0092】
対象点Phを固定すべき位置のx座標phx、y座標phy、z座標phzの数値を(1)式に代入することによって、第1〜第4関節部9〜12の4つの角度q1〜q4が未知数となる3元連立方程式となる。第1〜第4関節部9〜12のうちの1つを冗長関節部として決定し、冗長関節部の冗長角度を(1)式に代入することによって、第1〜第4関節部9〜12の4つの角度q1〜q4のうちの1つが既知となり、未知数が3つの3元連立方程式となる。これによって(1)式を連立して解くことによって、対象点Phを固定位置に位置すべき、各関節部9〜12の角度q1〜q4を解析的に求めることができる。
【0093】
たとえば第1関節部9を冗長関節部として決定し、第1関節部9を角変位すべき角度q1を冗長角度として決定する。この場合、(1)式を変形することによって、以下の(2)式に示すような、第1関節部9と第4関節部12の角度q1,q4とが未知数となる方程式に変形することができる。(2)式に、対象点Phのz座標phz、冗長関節部となる第1関節部9の角度q1を代入することによって、第4関節部12の角度q4を求めることができる。得られた第4関節部12の角度q4は、対象点Phを固定位置に位置させ、かつ第1関節部9を冗長角度である第1角度q1に角変位した場合における第4関節部12の角度q4を意味する。
【0094】
【数2】
【0095】
(2)式を、以下の(3)式に変形し、第1関節部9の角度q1を代入することによって、第4関節部12の角度q4を求めることができる。
【0096】
【数3】
【0097】
ここで、arccosは、逆余弦関数(アークコサイン関数)を意味する。また(3)式におけるプラスマイナスの2つの符号を離散形態符号と称する。予め定める条件、たとえば計算前に設定されている第4関節部12の角度q4などに基づいて、2つの離散形態符号のいずれかを決定することで、対象点Phを固定位置に固定する場合における、第4関節部12の角度q4を演算することができる。
【0098】
また(3)式において解が1つしかない場合、すなわちアークコサインの変数が、1または−1となる場合、第4関節部12は特異点に位置する。具体的には、第4関節部12の角度q4が、0度または180度となると、特異点となる。またアークコサインの変数の絶対値が1を超えると、解が存在しない。このような判定を行うことで、中央演算処理部19は、特異点でない解が存在するか、解が特異点となるか、解が存在しないかを判断することができる。
【0099】
次に、(1)式を変形することによって、以下の(4)式に示すような、第3関節部11と第4関節部12との角度q3,q4が未知数となる方程式に変形することができる。(4)式に、(3)式に基づいて決定した第4関節部12の角度q4を代入することによって、第3関節部11の角度q3を求めることができる。得られた第3関節部11の角度q3は、対象点Phを固定位置に保ち、かつ第1関節部9を冗長角度である第1角度q1に角変位した場合における第3関節部11の角度q3を意味する。
【0100】
【数4】
【0101】
次に、(1)式のうちのphx、phy、phzを求めるそれぞれの式のいずれか2つを連立させた式に、冗長関節部として設定される第1関節部9の角度q1と、(3)式に基づいて求めた第4関節部12の角度q4と、(4)式に基づいて求めた第3関節部11の角度q3とを代入することによって、第2関節部10の角度q2における、cos(q2)と、sin(q2)を求めることができ、これらから第2関節部10の角度q2を求めることができる。
【0102】
このようにして、第1〜第4関節部9〜12の角度q1〜q4をそれぞれ求めることができる。この場合、中央演算処理部19は、上述した(3)式、(4)式、第2関節部10の角度q2を求める式をそれぞれ示す計算プログラムを記憶することで、第1関節部9の第1角度q1が決定されることで、解が存在する場合、対象点Phを固定位置に保った場合における第2〜第4関節部10〜12の角度q2〜q4をそれぞれ求めることができる。
【0103】
また中央演算処理部19は、冗長関節部の冗長角度を正または負の方向に徐々に角変位させた場合に、冗長関節部の角度が取り得ることが可能な極大値または極小値となった場合、その角度を可動限界として判断することができる。たとえば冗長角度を大きくしていくと、冗長関節部以外の残余の関節部の角度の解を求めることができなくなる場合が生じる。この場合、中央演算処理部19は、各関節部の角度の解を求めることができる冗長角度のうちで、最大値を可動限界として判断する。同様に冗長角度を小さくしていくと、冗長関節部以外の残余の関節部の角度の解を求めることができなくなる場合が生じる。この場合、中央演算処理部19は、各関節部の角度の解を求めることができる冗長角度のうちで、最小値を可動限界として判断する。
【0104】
中央演算処理部19は、上述するステップa1およびステップa2を終えた状態で、冗長関節部の可動限界を演算することができる。そして指定された冗長角度と、可動限界角度との偏差を求めることができる。この場合、その偏差が予め定める値以下となる場合に、可動限界近傍に設定される警告領域に、冗長関節部が到達したことを判断することができる。
【0105】
また中央演算処理部19は、現時点での冗長角度に対して、プラス方向に冗長角度を増加させた場合と、マイナス方向に冗長角度を増加させた場合とで、前記偏差の変化量を調べることで、偏差が大きくなる冗長角度の角変位方向を求めることができる。中央演算処理部19は、偏差が大きくなる冗長角度の角変位方向を、冗長角度が可動限界から遠ざかって、警告領域から脱出する方向として判断することができる。
【0106】
また冗長関節部として、第1関節部以外の関節部が選択される場合には、同様にして(1)式を変形して連立方程式を解くことによって、解が存在する場合、冗長関節部以外の各関節部の角度をそれぞれ求めることができる。したがって中央演算処理部19は、冗長関節部として選択された関節部に応じて、残余の関節部を求める計算プログラムをそれぞれ記憶することで、第1〜第4関節部9〜12のいずれが冗長関節部として選択されたとしても、対象点Phを固定位置に維持しかつ、冗長関節部が冗長角度となる場合の、残余の関節部の角度をそれぞれ演算することができる。
【0107】
図6は、対象点Phを固定した場合に多関節ロボット30が取り得る状態の種類を説明するための1軸冗長ロボット60を示す正面図である。図6に示す1軸冗長ロボットは、第1〜第4アーム体50〜53と、隣接するアーム体を連結する複数の関節部54〜57とを有する。各アーム体50〜53は、予め定めるx軸とy軸とを含むxy平面上に配置される。第1アーム体50の基端部50aは、固定部58に第1関節部54によって連結され、第1関節部54によって固定部58に対して、xy平面に垂直なz軸まわりに角変位可能となる。
【0108】
また第2アーム体51の基端部51aは、第2関節部55によって第1アーム体50の先端部50bに連結され、第2関節部55によって第1アーム体50の先端部50bに対して、z軸まわりに角変位可能となる。また第3アーム体52の基端部52aは、第3関節部56によって第2アーム体51の先端部51bに連結され、第3関節部56によって第2アーム体51の先端部51bに対して、z軸まわりに角変位可能となる。また第4アーム体53の基端部53aは、第4関節部57によって第3アーム体52の先端部52bに連結され、第4関節部57によって第3アーム体52の先端部52bに対して、z軸まわりに角変位可能となる。
【0109】
したがって図6に示す1軸冗長ロボット60は、第4アーム体53をxy平面内で移動可能であるとともに、第4アーム体53をz軸まわりに回転可能な4関節平面ロボットとなる。
【0110】
このような1軸冗長ロボット60に関して、第4アーム体53の先端部53bのx座標およびy座標と、第4アーム体53のz軸まわりの姿勢とを固定する固定条件、すなわち3つの自由度を拘束する固定条件を設定する。この固定条件で第4アーム53を固定した場合における第1〜第4関節部54〜57の角度を考える。
【0111】
1軸冗長ロボット60は、上述した固定条件で第4アーム体53を固定した状態で、第1〜第4関節部54〜57の取り得る組合せが複数存在する。ここで、4つの関節部54〜57のうちの1つの角度を設定すれば、固定条件を満足する各関節部の角度を、逆変換により求めることができる。
【0112】
4自由度を有する1軸冗長ロボット60は、対象点Phの3つの自由度を拘束した固定状態では、各アーム体の長さと、第1関節部と第4関節部との間の距離とに応じて、3つの形態を取り得ることになる。
【0113】
第1の形態では回転揺動状態となる。この場合、1軸冗長ロボット60は、図6(1)に示すように、第4アーム体53を固定した状態で、第1アーム体50が固定部58に対して360度以上回転可能でかつ、第3アーム体52が第4アーム体53に対して360度以下揺動角変位可能となる。また第2アーム体51は、第1アーム体50に対して360度以上回転可能となり、第3アーム体52は、第2アーム体51に対して360度未満揺動角変位可能となる。
【0114】
第1関節部54と第2関節部55との間の距離をL1とし、第2関節部55と第3関節部56との間の距離をL2とし、第3関節部56と第4関節部57との間の距離をL3とし、第1関節部54と第4関節部57との間の距離をLwとすると、Lw+L2>L1+L3、Lw>L2、L1>L3、Lw−L2<L3−L1である場合に、回転揺動状態となる。
【0115】
また第2の形態では、2重揺動状態となる。この場合、1軸冗長ロボット60は、図6(2)に示すように、第4アーム体53を固定した状態で、第1アーム体50が固定部58に対して360度未満揺動角変位可能となり、第3アーム体52が第4アーム体53に対して360度未満揺動角変位可能となる。また第2アーム体51は、第1アーム体50に対して360度未満揺動角変位可能となり、第3アーム体52は、第2アーム体51に対して360度未満揺動角変位可能となる。
【0116】
また第3の形態では、2重回転状態となる。この場合、1軸冗長ロボット60は、図6(3)に示すように、第4アーム体53を固定した状態で、第1アーム体50が固定部58に対して360度以上回転可能となり、第3アーム体52が第4アーム体53に対して360度以上回転可能となる。また第2アーム体51は、第1アーム体50に対して360度以上回転可能となり、第3アーム体52は、第2アーム体51に対して360度以上回転可能となる。
【0117】
2重揺動状態および2重回転状態では、第1〜第4関節部54〜57のいずれか1つの角度を設定すれば、第4アーム体53が固定状態となる場合に第1〜第4関節部54〜57が取り得る角度の組合せは、1つとなる。したがって第1〜第4関節部54〜57のうちから、1つの冗長関節部を設定し、その角度を指定することで、1軸冗長ロボットが取り得る範囲を全て計算することができる。これに対して回転揺動状態では、第1〜第4関節部54〜57のうちから1つの冗長関節部を設定しただけでは、1軸冗長ロボット60が取り得る範囲を全て計算することができない。
【0118】
図7は、1軸冗長ロボット60が回転揺動状態となる場合において、各関節部が取り得る角度の組合せを示す図である。図7(1)は、1軸冗長ロボット60が初期状態である場合を示し、第1アーム体50が固定部58に対して角変位する状態を、図7(2)〜図7(5)に順に示す。
【0119】
第4関節部57を冗長関節部として選択する場合、図7(3)に示すように、第3アーム体52を第4アーム体53に対して一方向(図7では反時計方向)に角変位して、揺動限界角度に到達すると、それ以上、第3アーム体52を第4アーム体53に対して、一方向に角変位させることができなくなる。この場合、第1アーム体50も、固定部58に対して一方向にそれ以上角変位することができなくなる。揺動限界角度に到達した状態から、第3アーム体52を第4アーム体53に対して他方向(図7では時計方向)に角変位すると、第1アーム体50も、固定部58に対して他方向に角変位する。このように第4関節部を冗長関節部として選択すると、第1アーム体50を固定部58に対して360度回転させることができず、1軸冗長ロボット60が取り得る範囲を全て計算することができない。これに対して、第1関節部54を冗長関節部として選択する場合、図7(1)〜図7(5)に示すように、第1アーム体50を固定部58に対して一回転させることができ、1軸冗長ロボット60が取り得る範囲を全て計算することができる。
【0120】
したがって1軸冗長ロボット60が、回転揺動状態となる場合、第1関節部54と第2関節部55とのうちのいずれか一方と、第3関節部56と第4関節部57とのいずれか一方との2つを冗長関節部として選択可能とすることによって、2つのうちいずれかが、回転揺動状態のうちで360度以上アーム体が回転する関節部を冗長関節部として選択可能となり、1軸冗長ロボット60が取り得る範囲を全て計算することができる。
【0121】
1軸冗長ロボット60の各アーム体50〜53の長さが決定しているとしても、第1関節部54と第4関節部57との間の距離Lwの長さが変化することによって、回転揺動状態となるか、2重揺動状態および2重回転状態となるかが変化する。上述したように、第1関節部54と第2関節部55とのうちのいずれか一方と、第3関節部56と第4関節部57とのいずれか一方との2つを、冗長関節部として選択可能とすることによって、仮に回転揺動状態となったとしても、1軸冗長ロボット60が取り得る範囲を全て計算することができる。このことは、4つの関節部を有する1軸冗長ロボットに共通することである。したがって図1に示す多関節ロボット30であっても同様となる。
【0122】
図8は、図1に示す多関節ロボット30において、第1関節部9を冗長関節部として、その角度q1を順次角変位させた状態を示す正面図であり、図8(1)〜図8(3)の順に変化する。また図9は、図8において各関節部9〜12の角度q1〜q4の時間変化を示すグラフである。図9(1)は、第1関節部9の角度q1の時間変化を示し、図9(2)は、第2関節部10の角度q2の時間変化を示し、図9(3)は、第3関節部11の角度q3の時間変化を示し、図9(4)は、第4関節部12の角度q4の時間変化を示す。
【0123】
図8および図9には、対象点Phを固定位置に位置させた場合に、多関節ロボット30が回転揺動状態となり、第1関節部9および第2関節部10が揺動可能であって、第3関節部11および第4関節部12が回転可能である場合を示す。この場合、第1関節部9の角度q1を予め定める時間毎に予め定める角変位量、順次変化させると、対象点Phを所定位置に位置させた状態で、第1関節部9の角変位とともに、第2〜第4関節部10〜12が角変位する。
【0124】
図8(1)に示す初期状態から、第1関節部9の角度q1を、たとえばマイナス方向に角変位すると、図8(2)〜図8(3)に順に示すように、多関節ロボット30の形態が変化する。第1関節部9の角度q1をマイナス方向に角変位させた場合に、図8(3)に示すように、第1関節部9が0度となることで極小値の可動限界となる。この場合、第4関節部12の角度q4が180度の状態で、第1アーム体3と第2アーム体4とが突っ張ってしまい、さらなる角変位が不可能な状態となる。
【0125】
図10は、図1に示す多関節ロボット30において、第4関節部12を冗長関節部として、その角度q4を順次角変位させた状態を示す正面図であり、図10(1)〜図10(5)の順に変化する。また図11は、図10において各関節部9〜12の角度q1〜q4の時間変化を示すグラフである。図11(1)は、第1関節部9の角度q1の時間変化を示し、図11(2)は、第2関節部10の角度q2の時間変化を示し、図11(3)は、第3関節部11の角度q3の時間変化を示し、図11(4)は、第4関節部12の角度q4の時間変化を示す。
【0126】
図10および図11には、対象点Phを固定位置に位置させた場合に、多関節ロボットが回転揺動状態となり、第1関節部9および第2関節部10が揺動可能であって、第3関節部11および第4関節部12が回転可能である場合を示す。この場合、第4関節部12を予め定める時間毎に予め定める角変位量、順次変化させると、対象点Phを所定位置に位置させた状態で、第4関節部12の角変位とともに、第1〜第3関節部9〜11が角変位する。
【0127】
図11(1)に示す初期状態から、第4関節部12の角度q4を、たとえばプラス方向に角変位すると、図11(2)〜図11(5)に順に示すように、多関節ロボット30の形態が変化する。第4関節部12の角度q4を指定してプラス方向に角変位させた場合には、第4関節部12の角度q4が180度以上となっても、第1アーム体3と第2アーム体4とが突っ張ることがなく、第1関節部9を冗長関節部として選択した場合に比べて、第4関節部12のさらなる角変位が可能となる。
【0128】
このように、第4関節部12を冗長関節部として選択することで、対象点Phを固定状態に維持したうえで、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる。実際には、対象点Phの固定位置によっては、第1または第2関節部を冗長関節部として選択したほうが、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる場合がある。
【0129】
したがって第1関節部9と第2関節部10との2つのうちの1つと、第3関節部11と第4関節部12との2つのうちの1つとを冗長関節部として選択することで、対象点Phを固定状態に維持したうえで、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる。
【0130】
このことから本発明の第2実施形態として、図2に示す関節部操作スイッチ43は、第1関節部9と第2関節部10のいずれかと、第3関節部11と第4関節部12のいずれかとの2つが選択可能に配置される。このように4つではなく2つのスイッチが設けられるだけでも、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる。これによって操作者は、4つの関節部9〜12の全てを冗長関節部として選択可能とする必要がなく、形態変更動作における入力部20の操作性を向上することができる。また関節部操作スイッチ43を、4つから2つのスイッチとすることで、形態変更動作に必要なスイッチを減らすことができ、操作性を向上するとともに、スイッチとして必要な部品点数を減らすことができる。
【0131】
また中央演算処理部19は、第1および第2関節部9,10のいずれかが冗長関節部として選択された場合と、第3および第4関節部11,12のいずれかが冗長関節部として選択された場合とにおいてのみ各関節部を演算するだけでよく、各関節部の角度を演算するのに必要な計算プログラムを2つにすることができ、計算プログラムを4つ用意する場合に比べて、中央演算処理部19に必要な記憶容量を小さくすることができる。また本実施の形態では、第1関節部9と、第4関節部12とが冗長関節部として選択可能に設定される。このように第1関節部9と第4関節部12とを冗長関節部として選択可能とすることによって、他の関節部を冗長関節部として選択する場合に比べて、各関節部の角度を演算するための計算プログラムを簡単化することができ、短時間に各関節部の角度を求めることができる。
【0132】
図12は、中央演算処理部19の他の形態変更動作の手順を説明するためのフローチャートである。まずステップb0で、中央演算処理部19は、多関節ロボット30を制御可能な状態で、形態変更スイッチ45が押下されたことを判断すると、ステップb1に進み、形態変更動作を開始する。
【0133】
中央演算処理部19は、ステップb1〜b2で、上述したステップa1〜a2と同様の同様の手順を順に行い、ステップb3に進む。ステップb3では、中央演算処理部19は、上述するステップa3と同様にして、第1〜第4関節部9〜12の角度を求めるための計算プログラムにしたがって、第1〜第4関節部9〜12のうちで、冗長関節部以外の各関節部の角度を解析的に求める。また冗長関節部の角度は、ステップb2で既に決定しているので、演算することなく求めることができる。このようにして第1〜第4関節部9〜12の各角度q1〜q4を求めるとステップb4に進む。
【0134】
ステップb4では、中央演算処理部19は、ステップa4と同様にして、ステップb3の演算結果に基づいて、ステップb2で決定された冗長関節部の角度が可動限界に到達していないことを判断すると、ステップb5に進む。またステップb2で決定された冗長関節部の角度が可動限界に到達または超えていることを判断するとステップb9に進む。
【0135】
ステップb5では、現時点での冗長関節部の角度が可動限界に到達しているか否かを判断する。現時点での冗長関節部が可動限界に到達していないと判断するとステップb6に進み、現時点での冗長関節部が可動限界に到達していると判断するとステップb7に進む。
【0136】
ステップb6では、各関節部9〜12において演算した角度が2とおり求められた場合、以前に、冗長関節部以外の各関節部の角度を求めるために用いた離散形態符号を用いて、2つの角度のうちから1つの角度を選択する。たとえば前回角度がプラスの離散形態符号を用いて決定される場合には、ステップb6では、プラスの離散形態符号を用いた場合に得られる角度を関節部が移動すべき角度として選択する。このように関節部が取り得る2とおりの角度のうちから1つの角度を選択するとステップb8に進む。
【0137】
ステップb7では、各関節部9〜12において演算した角度が2とおり求められた場合、以前に、冗長関節部以外の各関節部の角度を求めるために用いた離散形態符号を反転した離散形態符号を用いて、2つの角度のうちから1つの角度を選択する。たとえば前回角度がプラスの離散形態符号を用いて決定される場合には、ステップb7では、マイナスの離散形態符号を用いた場合に得られる角度を関節部が移動すべき角度として選択する。このように関節部が取り得る2とおりの角度のうちから1つの角度を選択するとステップb8に進む。
【0138】
ステップb8では、ステップb6またはステップb7で求めた第1〜第4関節部9〜12の角度q1〜q4を演算結果として、出力部22に与え、ステップb9に進む。出力部22は、与えられた演算結果を多関節ロボット30に与える。これによって多関節ロボット30は、対象点Phの位置を維持した状態で、指定された冗長関節部が冗長角度となるように、各関節部9〜12を角変位する。
【0139】
ステップb9では、中央演算処理部19は、形態変更動作を終了する終了指令が与えられた否かを判断する。たとえば再び形態変更スイッチ45が押下されることで、終了指令が与えられたことを判断してもよい。中央演算処理部19は、形態変更動作を終了する指令が与えられていないと判断すると、ステップb1に戻り、関節部操作スイッチ43が再び押下されるのを待機する。またステップb9で形態変更動作を終了する指令が与えられたことを判断すると、ステップb10に進み、ステップb10で形態変更動作を終了する。
【0140】
図13は、中央演算処理部19が図12に示す形態動作の手順を行った場合における第1〜第4関節部9〜12の角度q1〜q4を示すグラフである。図13(1)は、第1関節部9の角度q1の時間変化を示し、図13(2)は、第2関節部10の角度q2の時間変化を示し、図13(3)は、第3関節部11の角度q3の時間変化を示し、図13(4)は、第4関節部12の角度q4の時間変化を示す。
【0141】
対象点Phの位置を決定した状態で、操作者が入力部20を操作して、第1関節部9に対応するマイナス方向移動スイッチを一度押下する。この場合、中央演算処理部19は、第1関節部9を冗長関節部として決定する。また中央演算処理部19は、第1関節部9について、マイナス方向移動スイッチ押下前の角度から予め定める角変位量で、マイナス方向に角変位した位置を冗長角度として決定する。
【0142】
図12に示す形態動作の手順を行う場合、マイナス方向移動スイッチが順次押下されると、中央演算処理部19は、ステップb1からステップb6、ステップb8を経由してステップb9に達する動作を行う。これによって中央演算処理部19は、対象点Phの位置を保ちつつ、図13(1)に示すように、第1関節部9の角度q1を時間経過とともに小さくさせるとともに、図13(2)〜(4)に示すように、第2関節部10〜第4関節部12の角度q2〜q4をそれぞれ変化させる。
【0143】
マイナス方向移動スイッチが押下され続けると、第1関節部9の角度q1が可動限界となる角度(図13では0度)に達する。この場合、中央演算処理部19は、ステップb5〜b8を経由せずに、ステップb9に達する動作を行う。これによって中央演算処理部19は、関節部の角変位指令を演算せず、各関節部9〜12の角変位を停止した状態で維持させる。可動限界に達した第1時期t1を経過後、さらにマイナス方向移動スイッチが押下されても、各関節部9〜12の角度は、可動限界に達した位置を維持する。
【0144】
また第1時期t1より後となる第2時期t2で、第1関節部9に対応するプラス方向移動スイッチが押下されると、中央演算処理部19は、第1関節部9について、プラス方向移動スイッチ押下前の角度から予め定める角変位量で、プラス方向に角変位した位置を冗長角度として決定する。これによって第1関節部9の角度q1が可動限界から脱出する。第2時期t2では、中央演算処理部19は、ステップb1からステップb7、ステップb8を経由してステップb9に達する動作を行う。この場合、中央演算処理部19は、第1時期t1以前に、第4関節部12の角度q4を求めるために用いた離散形態符号を反転して、第4関節部12の角度q4を求める。
【0145】
第2時期t2より後で、プラス方向移動スイッチが押下されつづけると、中央演算処理部19は、ステップb1からステップb6、ステップb8を経由してステップb9に達する動作を行う。この場合、中央演算処理部19は、第2時期t2で用いた離散形態符号を用いて、第4関節部12の角度q4を求める。これによって中央演算処理部19は、対象点Phの位置を保ちつつ、図13(1)に示すように、第1関節部9の角度q1を時間経過とともに大きくさせるとともに、図13(2)〜(4)に示すように、第2関節部10〜第4関節部12の角度q2〜q4をそれぞれ変化させる。
【0146】
このように第1時期t1より前と、第2時期より後とでは、各関節部9〜12の角度q1〜q4を求めるのに用いた離散形態符号が異なる。したがって1つの冗長関節部を選択するだけで、対象点Phを固定状態に維持したうえで、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる。言換えると、多関節ロボット30が回転揺動状態であって、揺動角変位する関節部が選択された場合であっても、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる。したがって操作者は、4つの関節部9〜12のうちの1つを選択するだけでよく、全てを冗長関節部として選択可能とする必要がなく、形態変更動作における入力部20の操作性を向上することができる。
【0147】
また上述する形態変更動作を中央演算処理部19が行う場合には、関節部操作スイッチ43を、1つのスイッチとすることができる。この場合、形態変更動作に必要なスイッチを減らすことができ、スイッチとして必要な部品点数を減らすことができる。また各関節部の角度を演算するのに必要な計算プログラムを1つにすることができる。また第1関節部を冗長関節部として選択可能とすることによって、計算プログラムをより簡単化することができる。
【0148】
図14は、形態変更動作における中央演算処理部19の予測報知動作の手順を説明するためのフローチャートである。本実施の形態では、形態変更動作にあたって、中央演算処理部19は、冗長関節部を決定した場合に、残余の関節部が角変位する方向を報知するとともに、各アーム体の移動量および移動方向を報知する予測報知動作を行うことができる。予測報知動作は、図3および図12に示す形態変更動作のいずれにも併用して行うことができる。したがって中央演算処理部19は、複数の関節部の角変位する方向を予測して報知する予測報知手段となる。
【0149】
まずステップc0で、中央演算処理部19は、対象点Phの位置が決定された状態で、予測報知動作の動作指令が与えられたことを判断すると、ステップc1に進み、予測報知動作を開始する。ステップc1では、中央演算処理部19は、移動すべき冗長角度の移動量を決定し、ステップc2に進む。以下、移動量は、移動方向も含むものとする。たとえば、たとえば関節部操作スイッチ43のうち、プラス方向移動スイッチまたはマイナス方向移動スイッチが直前に押下されていると、同じ移動スイッチが次に押下された場合に冗長関節部が角変位する移動量と移動方向とを決定する。
【0150】
ステップc2では、中央演算処理部19は、対象点Phの移動量と、各関節部9〜12の角度q1〜q4との関係を示す第1ヤコビ行列Jhに基づいて、対象点Phが固定位置となる場合の各関節部9〜12の角度q1〜q4の移動方向を決定する。具体的には、中央演算処理部19は、第1ヤコビ行列Jhを変形した行列式に、ステップc1で求めた移動量とを代入することによって、残余の角度の移動量を求めることができる。このようにして冗長関節部の移動量を決定した場合において、残余の角度の移動量を演算すると、ステップc3に進む。
【0151】
ステップc3では、中央演算処理部19は、設定されるアーム体の移動量と、各関節部9〜12の角度q1〜q4との関係を示す第2ヤコビ行列Jeに基づいて、対象点Phが固定位置となる場合の各関節部9〜12の角度q1〜q4の移動量とを決定する。具体的には、中央演算処理部19は、第2ヤコビ行列Jeに基づく行列式に、ステップc1およびc2で求めた各関節部9〜12の角度q1〜q4を代入することによって、設定されるアーム体の移動量を求めることができる。このようにして冗長関節部の移動量を決定した場合において、設定されるアーム体の移動量を演算すると、ステップc4に進む。
【0152】
ステップc4では、中央演算処理部19は、ステップc2およびステップc3で演算した演算結果を入力部20の表示部42によって報知し、ステップc1に戻る。中央演算処理部19は、予め定める終了条件を満足するまで、ステップc1〜c4を繰返し、終了条件を満足すると、予測報知動作を終了する。たとえば中央演算処理部19は、形態変更動作を終了するとともに予測報知動作を終了する。
【0153】
このように予測報知動作を行うことによって、形態変更動作において、冗長関節部をプラスまたはマイナス方向に角変位した場合に、残余の関節部がプラスまたはマイナス方向のいずれに角変位するかを報知することができ、操作性を向上することができる。
【0154】
同様に設定されるアーム体が、プラスまたはマイナス方向のいずれに角変位するかを報知することができ、操作性を向上することができる。たとえば設定されるアーム体は、第1〜第4アーム体3〜6の全てであってもよく、また操作者が注目したい任意のアーム体であってもよい。本実施の形態では、第3アーム体5と第4アーム体6との間の部分となる肘位置p2の移動量を報知することによって、中央演算処理部19は、多関節ロボット30のおおよその形態を把握させることができる。
【0155】
操作者は、報知された情報を確認することで、各関節部および設定されるアーム体が動くであろう方向を容易に把握することができ、利便性を向上することができる。たとえば、形態変更動作を行って、障害物に衝突することを回避する形態に多関節ロボットを変形させる場合、ロボットの形態変化を予測することができるので、操作者は、ロボットが障害物に衝突することをより確実に防ぐことができる。
【0156】
また上述した予測報知動作における演算は、ヤコビ行列Jh,Jeに基づいて行われる。したがって演算結果で有る各関節部9〜12の移動量および設定されるアーム体の移動量は、解析解でなく近似解となる。しかしながら報知情報は、厳密な情報が要求されず、短時間および容易に演算されることが望まれる。本実施の形態に示すように、ヤコビ行列に基づいて、予測報知に用いる演算結果を求めることで、短時間でかつ容易に演算することができる。
【0157】
次に、ステップc2における各関節部9〜12の移動量演算工程の詳細について説明する。上述した(1)式に基づいて、対象点Phの移動量を求める第1ヤコビ行列Jhは、次の(5)式によって表わされる。
【0158】
【数5】
【0159】
ここで、phx、phyおよびphzは、(1)式で定義した関数であり、Jhは、第1ヤコビ行列を示す。
【0160】
また第4アーム体5と第3アーム体6との間の肘位置p2を求める第2ヤコビ行列Jeは、(5)式に示す第1ヤコビ行列Jhのphx、phyおよびphzの関数に、L4=ゼロを代入した行列によって表わされる。
【0161】
対象点Phの微小移動量Δphと、各関節部の微小移動量Δqとの間には、次の(6)式の関係を有する。
【0162】
【数6】
【0163】
形態変更動作では、対象点Phの位置が固定されるので、(6)式の左辺は、ゼロである。これによって(6)式は、次の(7)式となる。
【0164】
【数7】
【0165】
ここで、Δq1は、第1関節部9の微小移動量Δq1である。この微小移動量Δq1は、ステップc1で決定される。たとえば関節部を移動する速度が予め決定されている場合、その速度に制御周期をかけた値となる。第1関節部9をプラス方向に移動させる場合にはΔq1が正となり、マイナス方向に移動させる場合には、Δq1が負となる。
【0166】
(7)式に、第1関節部9の微小移動量Δq1を代入することによって、第2〜第4関節部10〜12の微小移動量Δq2〜q4を求めることができる。中央演算処理部19は、(7)式を記憶する。ステップc2において、(7)式に冗長関節部の移動量Δq1を代入することによって、冗長関節部である第1関節部以外の各関節部10〜12の微小移動量Δq2〜Δq4を求めることができる。求めた各関節部の移動方向の正負を表示部42に表示させることによって、操作者は、冗長関節部の移動方向を選択した場合に残余の関節部が移動する移動方向を把握することができる。
【0167】
次に、ステップc3における各関節部9〜12の移動量演算工程の詳細について説明する。肘位置p2の微小移動量Δpeと、各関節部の微小移動量Δqとには、次の(8)式に示す関係がある。
【0168】
【数8】
【0169】
ここで、ΔPeは肘位置p2の微小移動量ΔPeであり、Jeは、上述した第2ヤコビ行列Jeである。(8)式に、(7)式で求めた各関節部10〜12の微小移動量Δq2〜q4と、冗長関節部である第1関節部9の微小移動量Δq1を代入することによって、肘位置p2の微小移動量ΔPeを求めることができる。
【0170】
中央演算処理部19は、(8)式を記憶する。ステップc3において、(8)式に冗長関節部の移動量Δq1、ステップc1およびステップc2で求めた各関節部9〜12の微小移動量Δp1〜Δp4を代入することによって、肘位置p2の微小移動量ΔPeを求めることができる。求めた肘位置p2の移動方向の正負を表示部42に表示させることによって、操作者は、冗長関節部の移動方向を選択した場合に肘位置p2が移動する移動方向たとえば基台17を基準として、上下左右に移動する方向を把握することができる。
【0171】
図15は、本発明の第2実施形態である多関節ロボット1の制御装置2を示すブロック図である。図15に示す多関節ロボット1および制御装置2は、図1に示す多関節ロボット30および制御装置2と類似した構成を示し、類似した構成については同様の参照符号を付して、説明を省略する。
【0172】
図15に示す多関節ロボット1は、7自由度を有する冗長ロボットであり、6つのアーム体3〜8と、隣接する2つのアーム体を回転自在に連結する7つの関節部9〜15とを有する。各アーム体3〜8は、それぞれ連結されて直列状に延びるアーム構成体16を構成する。また多関節ロボット1は、アーム構成体16の基端部に連結される基台17と、アーム構成体16の先端部に連結されるエンドエフェクタ18とを有する。
【0173】
各関節部9〜15は、同軸関節部と傾斜関節部とのうちのいずれかである。具体的には、第1関節部9、第5関節部13および第7関節部15は、同軸関節部であり、第2関節部10、第3関節部11、第4関節部12および第6関節部14は、傾斜関節部である。したがって第1〜第4アーム体3〜6および、第1〜第4関節部9〜12は、図1に示す多関節ロボット30と同様の構成を有する。
【0174】
第4アーム体6の一端部は、第4関節部12によって、第3アーム体5の軸線A5および第4アーム体6の軸線A6に対して所定の角度、本実施の形態では45度で傾斜する第4回転軸線A12まわりに回転自在に、第3アーム体5の他端部に連結される。第5アーム体7の一端部は、第5関節部13によって、第4アーム体6の軸線A6および第5アーム体7の軸線A7と同軸の第5回転軸線A13まわりに回転自在に、第4アーム体6の他端部に連結される。
【0175】
アーム構成体16の他端部である第6アーム体8の一端部は、第6関節部14によって、第5アーム体7の軸線A7および第6アーム体8の軸線A8に対して所定の角度、本実施の形態では45度で傾斜する第6回転軸線A14まわりに回転自在に、第5アーム体7の他端部に連結される。多関節ロボット1の先端部となるエンドエフェクタ18は、第6アーム体8の他端部に、第7関節部15によって、第6アーム体8の軸線A8と同軸の第7回転軸線A15まわりに回転自在に連結されている。エンドエフェクタ18は、多関節ロボット1のうちで、位置および姿勢を制御すべき設定部分となる。
【0176】
このように各関節部9〜15は、アーム体を介して互いに角変位可能に連結される。また各関節部9〜15が角変位することで、アーム構成体16は、図15に示すように、アーム体のそれぞれの軸線が同軸に配置されて一直線状に延びる状態に変形可能に構成される。各アーム体3〜6の軸線A3〜A6が一直線状に延びた状態では、第2回転軸線A10と第3回転軸線A11とは平行となり、第3回転軸線A11と第4回転軸線A12とは垂直となる。また第4アーム体6に設定される対象点Phは、アーム構成体16の先端部に配置され、基台17は、アーム構成体16の他端部に配置される。
【0177】
また直列方向一端部から直列方向他端部に向かって並ぶ3つの関節部のそれぞれの回転軸線、すなわち第5関節部13の回転軸線A6と、第6関節部14の回転軸線A7と、第7関節部15の回転軸線A8とは、予め定められる一点となる3軸交点Pで交差する。このように第5〜第7関節部13〜15は、3軸交点Pを構成する3つの3軸交点形成関節部となる。
【0178】
多関節ロボット1は7自由度を有し、各関節部9〜15を回転させることで、エンドエフェクタ18を任意の位置および姿勢に位置決め可能である。多関節ロボット1は、設備等が複雑に入り組んで作業経路が複雑な場合や、他のロボットと近接して並列に配置されて動作範囲を大きくすることができない場合に好適に用いられる。エンドエフェクタ18は、たとえば溶接トーチおよびハンドリング装置などの手先装置であってもよい。各アーム体3〜8を回転させる回転機構などは、図1に示す多関節ロボット30と同様の構成を有する。また制御装置2についても、その構成は、図1に示す制御装置2と同様の構成を有する。
【0179】
中央演算処理部19は、予め記憶される動作プログラムに従って、入力部20から与えられる各種情報に応答して、多関節ロボット1を制御するために必要な演算を行う。本実施の形態では、中央演算処理部19は、エンドエフェクタ18を予め定める目標位置および目標姿勢に移動させるのに必要な、各関節部9〜15の角度q1〜q7をそれぞれ演算する。
【0180】
制御装置2は、エンドエフェクタ18を予め指定される移動経路に沿って移動させるにあたって、回転すべき各関節部9〜15の角度q1〜q7を演算する。この移動経路は、ロボット動作前に、操作者によって入力部20から中央演算処理部19に入力される。本発明の制御装置2に制御される多関節ロボット1は、7つの関節部9〜15を有することで、冗長性を有する。すなわち、基台17に対して、エンドエフェクタ18を予め定める位置および姿勢で固定した状態で、各関節部9〜15の取り得る角度q1〜q7の組合せが複数通り存在する。
【0181】
本実施形態の制御装置2は、上述した形態変更動作を行うことができる。形態変更動作を行うにあたって、制御装置2は、複数の関節部9〜15のうちから選択された冗長関節部と、その冗長関節部の冗長角度と、エンドエフェクタ18を固定すべき位置および姿勢とが入力部20を介して与えられる。中央演算処理部19は、エンドエフェクタ18を固定すべき固定状態に固定したうえで、冗長関節部が冗長角度となる場合の、残余の関節部の角度を演算する演算手段となる。出力部22は、中央演算処理部19の演算結果を多関節ロボットに与える。演算結果が与えられた多関節ロボットは、冗長関節部を冗長角度とするとともに、残余の関節部の角度を演算結果に応じて位置合わせして、エンドエフェクタ18を指定された固定状態とする。
【0182】
このように形態変更動作を行うことによって、エンドエフェクタ18を固定した状態で、アーム構成体16の形状および姿勢などの形態を変更させることができる。操作者は、ロボットを動作させながらロボットの形態を確認することによって、エンドエフェクタ18を固定した状態で、障害物との衝突を回避するアーム構成体16の形態を実現する各関節部9〜15の角度を容易に決定することができる。
【0183】
図16は、図12に示す多関節ロボットを制御する制御装置2の入力部20を示す正面図である。入力部20の構成は、図2と同様の構成を有する。具体的には、入力部20は、操作部41と、表示部42とを含む。操作部41には、各関節部操作スイッチ43と、ツール移動スイッチ44と、形態変更スイッチ45とを含む。また操作部41は、ロボットの動作プログラムを入力するための命令キー、カーソルキーおよびテンキーなどをさらに含む。
【0184】
関節部操作スイッチ43は、各関節部9〜15の角度q1〜q7を入力するためのスイッチであって、各関節部9〜15について、プラス方向およびマイナス方向への予め定める角変位量の角変位指令が与えられる。ツール移動スイッチ44は、ベース座標軸またはツール座標軸に従ってエンドエフェクタ18の移動指令が与えられる。また形態変更スイッチ45は、対象点Phを予め定める位置に固定したまま、アーム構成体16の形態を変化させるための形態変更指令が与えられる。
【0185】
形態変更動作を行う場合、ツール移動スイッチ44は、エンドエフェクタ18を固定すべき位置および姿勢が入力される固定条件入力手段となる。また形態変更スイッチ45と関節部操作スイッチ43とによって、冗長関節部が入力される冗長関節部入力手段と、冗長関節部の角度である冗長角度が入力される冗長角度入力手段とが構成される。
【0186】
図17は、第2実施形態における中央演算処理部19の形態変更動作の手順を説明するためのフローチャートである。まずステップd0で、中央演算処理部19は、多関節ロボット1を制御可能な状態で、形態変更スイッチ45が押下されたことを判断すると、ステップd1に進み、形態変更動作を開始する。
【0187】
ステップd1では、中央演算処理部19は、ツール移動スイッチ44によってエンドエフェクタ18の移動指令が与えられると、エンドエフェクタ18を指定された位置および姿勢に移動するための演算結果を出力部22に与える。またツール移動スイッチ44による移動指令がなければ、現時点のエンドエフェクタ18の位置および姿勢を維持させる。中央演算処理部19は、関節部操作スイッチ43が押下されたと判断すると、その時点のエンドエフェクタ18の位置および姿勢を固定すべき固定条件として決定し、ステップd2に進む。
【0188】
ステップd2では、中央演算処理部19は、関節部操作スイッチ43によって指定された関節部を、冗長関節部として決定する。冗長関節部は、第1〜第4関節部9〜12のいずれか1つとなる。また関節部操作スイッチ43によって指定された角度を、冗長角度として決定する。このように冗長関節部および冗長角度を決定するとステップd3に進む。
【0189】
ステップd3では、中央演算処理部19は、ステップd1で決定したエンドエフェクタ18の位置および姿勢に基づいて、3軸交点Pの座標を演算する。3軸交点Pは、第5関節部13、第6関節部14、第7関節部15の各回転軸線A13,A14,A15が一点で交わる点であるので、エンドエフェクタ18の位置および姿勢が決定されると、一意的に求めることができる。このように中央演算処理部19は、3軸交点Pを演算する3軸交点演算手段となり、3軸交点Pを求めるとステップd4に進む。
【0190】
ステップd4では、中央演算処理部19は、ステップd3で演算した3軸交点Pの3次元位置に、3軸交点Pが位置する場合における、第1〜第4関節部9〜12の角度q1〜q4を求める。具体的には、第1〜第4関節部9〜12のそれぞれの角度q1〜q4を変数として、3軸交点Pの座標を表わす順変換関係式と、ステップd2で決定した冗長関節部の冗長角度と、ステップd3で演算した3軸交点Pの座標とに基づくことによって、第1〜第4関節部9〜12の角度q1〜q4を解析的に求めることができる。第1〜第4関節部9〜12の角度q1〜q4を解析的に求める場合、対象点Phの座標を3軸交点Pの座標に置き変えることで、上述するステップa3と同様の方法を用いて求めることができる。
【0191】
したがって中央演算処理部19は、冗長関節部の冗長角度と、3軸交点Pのx座標、y座標、z座標を代入することで、第1〜第4関節部9〜12のうちで冗長関節部以外の各関節部の角度を解析的に求めることが可能な方程式を表わす計算プログラムを記憶している。中央演算処理部19は、第1〜第4関節部9〜12の計算プログラムにしたがって、第1〜第4関節部9〜12のうちで、冗長関節部以外の各関節部の角度を解析的に演算し、ステップd5に進む。このように中央演算処理部19は、注目する第1〜第4関節部9〜12の角度q1〜q4をそれぞれ演算する注目関節部角度演算手段となる。
【0192】
ステップd5では、中央演算処理部19は、ステップd2で決定された冗長関節部の冗長角度が可動限界に到達していないことを判断するとステップd6に進み、可動限界に到達しているか、または可動限界を超えているかを判断すると、ステップd8に進む。
【0193】
ステップd6では、中央演算処理部19は、エンドエフェクタ18が固定位置および固定姿勢となる、第5〜第7関節部13〜15の角度q5〜q7を求める。ステップd4で求めた第1〜第4関節部9〜12の角度q1〜q4から、第4アーム体6の姿勢を求めることができる。第4アーム体6の姿勢が決定された状態では、エンドエフェクタ18が予め定める位置および姿勢となる場合の第5〜第7関節部13〜15の角度q5〜q7は、一意に決定することができる。
【0194】
具体的には、中央演算処理部19は、第1〜第4関節部9〜12の角度q1〜q4の角度と、エンドエフェクタ18の位置および姿勢と用いて、第5〜第7関節部13〜15の角度を解析的に求めることが可能な逆変換方程式を表わす第5〜第7関節部計算プログラムを記憶している。
【0195】
中央演算処理部19は、第5〜第7関節部計算プログラムにしたがって、第5〜第7関節部13〜15の角度q5〜q7をそれぞれ求める。このように中央演算処理部19は、第5〜第7関節部13〜15の角度q5〜q7をそれぞれ演算する交点形成関節部角度演算手段となり、各角度を求めるとステップd7に進む。
【0196】
ステップd7では、ステップd4およびステップd6で得られた第1〜第7関節部9〜15の角度q1〜q7を演算結果として、出力部22に与える。出力部22は、与えられた演算結果を多関節ロボット1に与える。これによって多関節ロボット1は、各関節部9〜15が角変位し、エンドエフェクタ18を固定位置および固定姿勢に維持した状態で、冗長関節部が冗長角度となる。このとき、第1〜第7関節部9〜15の角度q1〜q7は、収束演算などの近似法とは異なり、解析的な演算方法によって求められるので、冗長関節部の移動前と移動後とで、固定位置および固定姿勢からエンドエフェクタ18がずれることがない。このように演算結果を出力部22に与えて、多関節ロボット1を動作させると、ステップd8に進む。
【0197】
ステップd8では、中央演算処理部19は、形態変更動作を終了する終了条件を満足するか否かを判断する。たとえば形態変更解除スイッチが押下されることで、終了条件を満足することを判断してもよい。中央演算処理部19は、終了条件を満足していないと判断すると、ステップd1に戻り、関節部操作スイッチ43が押下されるのを待機する。この状態で、関節部操作スイッチ43が再度押下されると、その指示に従ってステップd1〜d8を繰返す。そしてステップa7で形態変更動作を終了する指令が与えられたことを判断すると、ステップa8に進み形態変更動作を終了する。
【0198】
このようにして、中央演算処理部19が形態変更動作を行うことによって、エンドエフェクタ18の位置および姿勢を固定したまま、各関節部9〜15を角変位させて、多関節ロボット1の形態を変更させることができる。したがって第1実施形態と同様の効果を得ることができる。
【0199】
また図15に示す多関節ロボット1は、第5〜第7関節部13〜15のうちの1つが冗長関節部として選択されてしまうと、解析的に求めることができない。これに対して、本実施の形態では、第1〜第4関節部9〜12のいずれかを冗長関節部として選択することによって、解析解を得ることができる。また7つの関節部9〜15から1つを選択するよりも、4つの関節部9〜12から1つを選択するほうが、操作性を向上することができる。
【0200】
また図16に示すように、各関節部操作スイッチ43を用いて、冗長関節部を決定する場合、形態変更スイッチ45が押下された状態では、第1関節部〜第4関節部9〜12の選択スイッチを有効とし、第5〜第7関節部13〜15の選択スイッチが無効とする。これによって冗長関節部としては、第1〜第4関節部9〜12のいずれかについて選択可能となり、誤動作を防ぐことができる。
【0201】
また本実施の形態では、第5〜第7関節部13〜15で三軸交点を構成したが、これに限らない。たとえば第1〜第3関節部9〜11で3軸交点を構成した場合であっても、同様に解析解を求めることができる。この場合には、第4〜第7関節部12〜15のうちから1つの冗長関節部が選択される。そしてエンドエフェクタ18に設定される座標系から、第1関節部を見た座標系に反転した座標系に変換することによって、同様に解析的に求めることができる。
【0202】
すなわち、アーム体を介して互いに角変位可能に連結されて直列に並ぶ7つの関節部を含み、7つの関節部9〜15のうちで直列方向一端部から直列方向他端部に向かって並ぶ3つの関節部の回転軸線が一点で交差する3軸交点を形成する3つの交点形成関節部を有する7自由度の多関節ロボットにおいて、アーム構成体の一端部に対する他端部の位置および姿勢が固定条件として入力される場合に、3つの交点形成関節部以外となる4つの注目する関節部のうち1つが、冗長関節部として入力されることで、形態変更動作において各関節部9〜15の角度を解析的に求めることができる。
【0203】
また上述したように、第2実施形態であっても第1および第2関節部9,10のいずれか1つと、第3および第4関節部11,12のいずれか1つを冗長関節部として選択させるようにしてもよい。また図12に示す形態変更動作を行わせてもよく、移動予測動作を行わせてもよい。移動予測動作と、形態変更動作とは、エンドエフェクタ18を移動経路に沿って移動させて移動経路を教示する移動教示動作と併用して行ってもよい。
【0204】
図18は、移動予測動作と、形態変更動作と、移動教示動作とを行う場合における中央演算処理部19の動作手順を説明するためのフローチャートである。まずステップe0で、中央演算処理部19は、多関節ロボット1を制御可能な状態で、入力部20のツール移動スイッチ44によって、エンドエフェクタ18の移動指令が与えられると、ステップe1に進む。
【0205】
ステップe1では、中央演算処理部19は、ツール移動スイッチ44に与えられた情報に従って、エンドエフェクタ18を移動させる移動経路を演算する。エンドエフェクタ18が指定された移動経路に従って移動するにあたって、各関節部の角度の組合せは、複数存在する場合がある。この場合、中央演算処理部19は、予め定める規則にしたがって複数の角度の組合せから1つを選択する。たとえばエンドエフェクタ18側の関節部の角変位が少なくなるような角度の組合せを選択する。中央演算処理部19は、演算結果を出力部22を介して多関節ロボット1に与え、ステップe2に進む。これによって多関節ロボット1は、エンドエフェクタ18を指定された移動経路に沿って移動させる。
【0206】
ステップe2では、第1〜第4関節部9〜12のいずれかが特異点近傍の警告範囲に到達するか否かを判断する。たとえば中央演算処理部19は、第1〜第4関節部9〜12の角度q1〜q4を、3軸交点位置に変換するヤコビ行列の行列式の絶対値が、予め定める設定値以下であると判定されると、第1〜第4関節部9〜12の角度q1〜q4のいずれかが、特異点近傍の警告領域に到達したことを判断する。
【0207】
ステップe2において、第1〜第4関節部9〜12のいずれかが警告範囲に到達したことを判断するとステップe3に進み、そうでないとステップe6に進む。ステップe3では、図14に示す予測報知動作を行う。図19は、入力部20の表示部42に表示される表示画面を示す図である。たとえば図19に示すように、中央演算処理部19は、可動限界に近づいている冗長関節部を表示させる。また中央演算処理部19は、可動限界に近づいている関節部が、特異点から脱出する角変位方向と、脱出する方向に移動したときの肘位置p2が動く方向を表示させる。このように予測情報を表示させると、ステップe4に進む。
【0208】
ステップe4では、中央演算処理部19は、形態変更指令が与えられたか否かを判断する。形態変更が与えられたと判断するとステップe5に進み、そうでないとステップe6に進む。ステップe5では、エンドエフェクタ18の移動を停止して、図3または図12に示す形態変更動作を開始し、形態変形動作が終了したことを判断するとステップe6に進む。
【0209】
ステップe6では、中央演算処理部19は、移動動作を終了する終了条件を満足するか否かを判断する。中央演算処理部19は、移動動作の終了条件を満足していないと判断すると、ステップe1に戻り、ツール移動スイッチ44が再び押下されるのを待機する。この状態で、ツール移動スイッチ44が再度押下されると、その指示に従ってステップe11〜e6を順に行う。またステップe6で終了条件を満足することを判断すると、ステップe7に進み、ステップe7で移動動作を終了する。
【0210】
これによって操作者は、移動動作を行う途中に、エンドエフェクタ18の位置を固定したまま、ロボットの形態を変更して、冗長関節部を可動限界から離して、再び、移動指令を与えた方向にエンドエフェクタ18を移動させることができる。このように移動動作に、中央演算処理部19による予測動作と形態変更動作とを行わせることによって、利便性を向上することができる。
【0211】
上述した本実施の形態は、発明の例示に過ぎず、発明の範囲内において構成を変更することができる。たとえば本実施の形態では、オンラインティーチング、すなわちロボットを動作させながら各関節部の角度を求めて、ロボットの動作計画の教示に用いたが、これに限らない。したがって中央演算処理部19のうち、各関節部の角度を求めるプログラムを実行するコンピュータも本発明に属する。コンピュータに、ロボットの関節の種類、アーム体の長さなどの条件を入力した状態で、対象点Phおよびエンドエフェクタ18を予め定める固定状態に保って、冗長関節部が冗長角度となる各関節部の角度を解析的に求めることができ、この解析結果をロボットに与えてもよい。すなわち本発明の方法をオフラインティーチングやロボットの動作シミュレーションに用いることも可能である。また本実施の形態では、(1)式に示す対象点の位置を求めるための関係式を変形して、連立方程式を解いた計算式に基づいて、各関節部の角度を求めたが、対象点の姿勢を求めるための関係式、対象点の位置の一部および姿勢の一部を求めるための関係式を変形して、連立方程式を解いた計算式に基づいて、各関節部の角度を求めてもよい。
【0212】
また本実施の形態では、傾斜関節部と同軸関節部とを有する多関節ロボットを用いて説明したが、冗長性を有する他のロボットであっても同様に各関節部の角度を解析的に求めることができる。たとえば図6に示す4関節ロボットであっても同様に各関節部の角度を求めることができ、設定部分の固定についても、位置のほか姿勢が固定されてもよい。
【0213】
また本実施の形態では、1軸冗長ロボットについて説明したが、2軸以上の冗長性を有するロボットであっても同様に求めることができる。この場合、2つ以上の冗長関節部が選択されることで、解析的に各関節部の解を求めることができる。すなわち、注目する複数の関節部のうちで冗長関節部を除く残余の関節部の数が、設定部分を予め定める固定条件に従って拘束される自由度の数と一致するように、注目する複数の関節部の少なくとも1つが冗長関節部として入力されることで、連立方程式を解くことができ、解析的に各関節部の角度を求めることができる。
【0214】
すなわち注目する複数(N)の関節部のうちで、入力された冗長関節部の数(n)を除いた数(N−n)が、設定部分が拘束される自由度の数(M)と一致する必要がある。この場合、順変換関係式に含まれる変数を未知数とすると、設定部分が拘束される自由度の成分をそれぞれ解とする順変換関係式の数(M)と、順変換関係式に含まれる未知数の数(N−n)とが等しくなる。これらの複数の順変換関係式を連立して解くことによって、各変数の値、すなわち残余の関節部の各角度を解析的に求めることができる。たとえば2軸冗長ロボットにおいて、オンラインティーチングを行う場合、2つの関節部を冗長関節部として決定し、そのうち第1冗長関節部を任意の角度に固定し、第2冗長関節部を変化させる。これによって、上述した演算方法と同様の演算方法を適用することができ、第2冗長角度を変化した場合に各関節部が取り得る角度の組合せの解析解を順次求めることができる。
【符号の説明】
【0215】
1 多関節ロボット
2 制御装置
3 第1アーム体
4 第2アーム体
5 第3アーム体
6 第4アーム体
9 第1関節部
10 第2関節部
11 第3関節部
12 第4関節部
19 中央演算処理部
20 入力部
22 出力部
ph 対象点
【技術分野】
【0001】
本発明は、冗長関節部を含む複数の関節部を有する多関節ロボットを制御する制御装置および多関節ロボットを制御する制御方法に関する。
【背景技術】
【0002】
7つ以上の複数の関節部を有する多関節ロボットの制御方法に関する従来技術として、たとえば特許文献1に開示されている。この従来技術では、冗長性を有する多関節ロボットに設定される設定部分が予め設定される固定位置に維持した状態で、ロボットを構成する複数の軸のうち、任意の1つの冗長関節部の微小移動が可能に設定される。操作者が冗長関節部を微小移動させた場合、ロボット制御装置は、設定部分が固定位置に維持されるように冗長関節部以外の残余の各関節部を微小移動させる。
【0003】
特許文献1に開示される従来技術では、ヤコビ行列に基づいて、冗長関節部以外の残余の各関節部の移動量を決定する。ここで、ヤコビ行列は、設定部分の微小変化と、ロボット各関節部の微小変化との関係を示す関数である。従来技術のロボット制御装置は、設定部分の微小変化をゼロとし、ロボットの各関節部の微小変化を示す行列のうちで冗長設定関節部の微小変化を既知として、残余の各関節部の微小変化の解を求めることによって、設定部分を固定位置範囲に維持すべき、残余の各関節部の微小移動量を求める。
【0004】
【特許文献1】特許3379540号公報
【発明の開示】
【発明が解決しようとする課題】
【0005】
従来技術では、複数の関節部のうちで任意の関節部を冗長関節部として選択可能に設定され、ヤコビ行列に基づいて、冗長関節部以外の残余の各関節部の微小移動量を求める。任意の関節部を冗長関節部として選択可能とすると、各関節部の移動量について、厳密解を求めることが困難であり、近似解を求めることになる。近似解を基に各関節部を決定すると、冗長設定関節部を変位移動させた場合に、目的とする固定位置に設定部分を正確に維持することができないという問題がある。
【0006】
設定部分のずれ量は、冗長関節部を高速移動または長距離移動する場合に顕著となる。設定部分がずれてしまうと、ロボット動作に不具合が生じる場合がある。また設定部分の位置ずれを防ぐために収束演算の回数を増やした場合には、各関節部の移動量を短時間で求めることができない。
【0007】
たとえば冗長性ロボットによって搬送対象物を搬送する場合、ロボットアームが障害物に衝突しないように位置教示を行う。位置教示時に、搬送対象物の位置姿勢を正確に維持したうえで、障害物近傍のアームを冗長関節部として微小移動させることができず、搬送不良が生じてしまうおそれがある。またたとえば冗長性を有するロボットについて、複数の関節部のうちから冗長関節部として選択する関節部を順次変更して、各冗長関節部について微小移動を繰返すと、設定部分のずれ方が複雑となり、設定部分の位置を前の状態に戻すことが困難となる。
【0008】
したがって本発明の目的は、設定部分を目的とする固定位置に正確に維持したうえで、ロボットの各関節部の角度を変更可能に、冗長ロボットを制御する制御方法および制御装置を提供することである。
【課題を解決するための手段】
【0009】
本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットの制御装置であって、
前記設定部分を固定すべき固定条件が入力される固定条件入力手段と、
冗長関節部が入力される冗長関節部入力手段であって、前記複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つが冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
固定条件入力手段によって入力される固定条件と、冗長角度入力手段によって入力される冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき複数の関節部の角度をそれぞれ演算する角度演算手段と、
前記角度演算手段で演算された演算結果を多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置である。
【0010】
また本発明は、固定条件として、直列方向他端部の関節部に対する設定部分の3つの自由度が拘束される条件が入力され、
冗長関節部入力手段は、前記複数の関節部のうちで、直列方向一端部から直列方向他端部に向かって並ぶ2つの関節部のうちの1つの関節部と、直列方向他端部から直列方向一端部に向かって並ぶ2つの関節部のうちの1つの関節部とを冗長関節部として選択可能に構成されることを特徴とする。
【0011】
また本発明は、角度演算手段は、入力前の状態から冗長角度入力手段に入力された冗長角度に冗長関節部を角変位させる間に、冗長関節部が可動限界に到達することを判断した場合、その可動限界に到達した状態の関節部の角度を演算することを特徴とする。
【0012】
また本発明は、角度演算手段は、冗長角度入力手段に入力された冗長角度に冗長関節部が角変位する間に、冗長関節部が可動限界に到達した状態から脱出することを判断した場合、冗長関節部以外の関節部の角度が取り得る2つの組合せのうち、可動限界に到達する前に取り得た角度の組合せと異なる角度の組合せを演算することを特徴とする。
【0013】
また本発明は、前記複数の関節部の角度変化を設定部分の位置変化に変換するヤコビ行列と、冗長角度入力手段に入力される冗長角度とに基づいて、設定部分を固定条件で拘束して、冗長関節部を冗長角度に角変位するときに、複数の関節部の角変位する方向を予測し、予測結果を報知する予測報知手段をさらに備えることを特徴とする。
【0014】
また本発明は、前記複数の関節部のいずれかが可動限界近傍に設定される警告範囲に到達したことを判断すると、判断結果を報知する警告報知手段をさらに備えることを特徴とする。
【0015】
また本発明は、警告報知手段は、警告範囲に到達した関節部が、可動限界から遠ざかるような角変位方向を報知することを特徴とする。
【0016】
また本発明は、隣接する2つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを含む多関節ロボットを制御することを特徴とする。
【0017】
また本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ7つの関節部を含み、7つの関節部のうちで直列方向一端部から直列方向他端部に向かって並ぶ3つの関節部の回転軸線が一点で交差する3軸交点を形成する3つの交点形成関節部を有する7自由度の多関節ロボットの制御装置であって、
7つの関節部のうちで直列方向一端部の関節部に設定される設定部分を固定すべき位置および姿勢が固定条件として入力される固定条件入力手段と、
3つの交点形成関節部以外となる4つの基本関節部のうち1つが、冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
設定部分が固定すべき位置および姿勢となる状態での3軸交点の固定位置を演算する3軸交点位置演算手段と、
3軸交点位置演算手段によって演算される3軸交点の位置と、冗長関節部入力手段によって入力される冗長角度とに基づいて、4つの基本関節部の各角度を変数として3軸交点の位置を求めるための関係式を解いて、3軸交点が固定位置に位置するような、4つの基本関節部の角度をそれぞれ演算する基本関節部角度演算手段と、
注目関節部角度演算手段によって演算された演算結果と、固定条件入力手段によって入力される設定部分の位置および姿勢とに基づいて、3つの交点形成関節部の角度をそれぞれ演算する交点形成関節部角度演算手段と、
前記各角度演算手段で演算された演算結果とを多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置である。
【0018】
また本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットを制御する制御方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と、
角度演算工程に従ってロボットを制御する制御工程とを備えることを特徴とする多関節ロボットの制御方法である。
【0019】
また本発明は、アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットにおける各関節部の角度を演算する演算方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と備えることを特徴とする多関節ロボットの各関節部の角度の演算方法である。
【発明の効果】
【0020】
請求項1記載の本発明に従えば、多関節ロボットは、冗長性を有し、設定部分を予め定める固定条件で固定した状態で、各関節部の取り得る角度の組合せが複数通り存在する。本発明では、冗長関節部入力手段によって、冗長関節部が入力される。これによって複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致する。また固定条件入力手段によって、設定部分を固定すべき固定条件が既知となるとともに、冗長角度入力手段によって、冗長関節部の冗長角度が既知となる。
【0021】
このように入力手段に各種情報が入力された場合、冗長関節部の冗長角度と、固定条件とを代入することで、複数の関節部のうちで、冗長関節部以外の各関節部の角度を解析的に求めることが可能な計算式が存在する。角度演算手段は、前記計算式を表わす計算プログラムを記憶し、その計算プログラムに従うことで、各入力手段に与えられた各種情報を前記計算式に代入することで、設定部分が固定条件で拘束される場合における、注目する関節部の角度を解析的にそれぞれ求めることができる。
【0022】
出力手段は、冗長角度入力手段によって入力された冗長角度と、角度演算手段によって演算された残余の関節部の各角度との情報を多関節ロボットに与える。多関節ロボットは、出力手段から与えられた情報に基づいて、複数の関節部を角変位駆動する。これによって設定部分を予め定める固定条件で拘束するとともに、冗長関節部を冗長角度に保つように、多関節ロボットの各関節部を変位駆動することができる。ここで、残余の関節部の各角度は解析解、すなわち厳密解として求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。
【0023】
このように本発明によれば、各関節部の角度の解析解をそれぞれ演算することができるので、近似解に基づいて各関節部の角度をそれぞれ演算する場合に比べて、設定部分を予め定める固定状態に正確に保ち、冗長関節部を冗長角度にすることができる。すなわち設定部分がずれることを防ぐことができる。たとえば入力前の状態から冗長関節部の角度を高速移動または長距離移動する場合であっても、設定部分の固定状態を正確に維持することができる。また解析解を求めることに付随して、各関節部の特異点について正確に判断することができ、利便性を向上することができる。
【0024】
また、たとえば冗長関節部および冗長関節部に連なるアーム体が障害物に衝突することを回避するために、設定部分の位置を固定した状態で冗長関節部を角変位させることができる。この場合、設定部分の位置を正確に保ちつつ、障害物とアーム体との干渉を回避することができるので、設定部分の位置ずれに起因するロボットの動作不良を防ぐことができる。
【0025】
請求項2記載の本発明に従えば、固定条件として、設定部分の3つの自由度が拘束される。3自由度の拘束として、たとえば直列方向他端部の関節部に対する、設定部分のx座標、y座標、z座標が決定される。またたとえば直列方向他端部の関節部に対する、x座標、y座標および他端部の関節部に設定されるY軸まわりの姿勢が決定される。
【0026】
直列方向一端部側の2つの関節部のうちの1つの関節部と、直列方向他端部側の2つの関節部のうちの1つの関節部とを冗長関節部として選択可能とすることで、それら2つの冗長関節部の冗長角度を調整することで、設定部分を固定状態に維持したうえで、注目する複数の関節部が取り得ることが可能な全ての角度の組合せを演算可能となる。
【0027】
このように本発明によれば、4つの関節部のうちの2つを冗長関節部として選択可能とすることで、各関節部が取り得ることが可能な全ての角度の組合せを求めることができるので、4つの関節部の全て冗長関節部として選択可能とする必要がなく、利便性を向上することができる。たとえば操作者に対する操作性を向上することができるとともに、角度演算手段が記憶する計算プログラムの数を減らすことができる。
【0028】
請求項3記載の本発明に従えば、設定部分を固定状態に維持するための各関節部の角度を解析的に求めることができるので、冗長関節部が可動限界に到達したか否かを正確に判断することができる。角度演算手段は、冗長関節部が可動限界に到達することを判断すると、冗長関節部が可動限界に到達した状態の関節部の角度を演算する。また可動限界を超えるような冗長角度が入力されても、可動限界に到達した状態の関節部の角度を演算する。
【0029】
このように本発明によれば、冗長関節部の可動範囲内となる状態の関節部の角度を演算する。これによって機構的に移動不可能な移動指令値を、多関節ロボットに与えることを防ぐことができ、関節部の損傷を防ぐことができる。また解析的に求められる特異点に基づくことによって、可動限界を正確に求めることができる。したがって角度演算手段は、冗長関節部が可動限界に到達したか否かを精度よく判断することができ、可動限界に近い領域まで冗長関節部を角変位させることができる。
【0030】
請求項4記載の本発明に従えば、冗長角度入力手段化からの冗長角度の入力によって、冗長関節部が可動限界に到達した状態から脱出して、可動範囲内に移動する場合、角度演算手段は、可動限界に到達する前に取り得た角度の組合せと異なる角度の組合せを演算する。これによって、設定部分を固定した状態で、多関節ロボットが回転揺動状態となる場合に、冗長関節部として選択した関節部が揺動角変位する場合、冗長関節部が可動限界に到達する前と、可動限界から脱出した後とで、ロボットの形態を変更することができ、複数の関節部が取り得ることが可能な全ての角度の組合せを演算可能となる。したがって複数の関節部の全てを冗長関節部として選択する必要がなく、利便性を向上することができる。
【0031】
このように本発明によれば、冗長関節部を1つ選択するだけで、複数の関節部が取り得ることが可能な全ての角度の組合せを演算可能となる。したがって複数の関節部の全てを冗長関節部として選択可能とする必要がなく、利便性を向上することができる。たとえば操作者に対する操作性を向上することができるとともに、角度演算手段が記憶する計算プログラムの数を減らすことができる。
【0032】
請求項5記載の本発明に従えば、予測報知手段によって各関節部の角変位に関する予測結果を報知する。これによって操作者は、各関節部の予測情報、たとえば角変位方向および角度を知ることができ、利便性を向上することができる。たとえばロボットが障害物に衝突するか否かについて、操作者が確認しながら冗長関節部の冗長角度を指定する場合に、各関節部の動きを予測報知する。これによって操作者は、冗長角度を指定した場合に障害物に関節部が衝突するか否かを把握しやすく、利便性を向上することができる。また予測結果として示される情報は、実際に各関節部の角度を決定する場合に比べて、精度が要求されない。したがってヤコビ行列に基づく近似式で十分であり、ヤコビ行列に基づくことで、簡単にかつ短時間に予測結果を演算することができる。
【0033】
請求項6記載の本発明に従えば、注目する複数の関節部のいずれかが警告範囲に到達したことを判断すると、警告報知手段によって関節部が警告範囲に到達したことを報知する。これによって操作者は、関節部が警告限界に近づいたことを判断することができ、利便性を向上することができる。たとえば操作者は、設定部分が固定状態に維持された状態で、注目する関節部が可動限界から遠ざかるように、冗長角度を設定することによって、設定部分を固定状態に維持した状態で、関節部を可動限界から遠ざけることができ、可動限界に対する余裕を、ロボットに与えることができる。
【0034】
請求項7記載の本発明に従えば、注目する複数の関節部のいずれかが警告範囲に到達したことを判断すると、関節部が可動限界から遠ざかるような角変位方向を報知する。これによって操作者は、関節部が可動限界から遠ざかる方向を判断して、関節部が可動限界に到達することを容易に回避することができ、利便性を向上することができる。
【0035】
請求項8記載の本発明に従えば、多関節ロボットは、傾斜関節部と同軸関節部とを有する。このような多関節ロボットの場合には、多関節ロボットの動きが複雑であり、設定部分を固定状態に維持したうえで、各関節部を角変位させる指令を手動で与えるのは困難である。これに対して、上述したように本発明の制御装置を用いれば、冗長関節部の選択指令と、冗長関節部の冗長角度を与えることによって、設定部分の固定状態を正確に維持したうえで、各関節部を角変位させることができ、利便性を向上することができる。
【0036】
請求項9記載の本発明に従えば、多関節ロボットは、7つの関節部を含んで7自由度の多関節ロボットであり、設定部分を予め定める位置および姿勢に固定した状態で、各関節部の取り得る角度の組合せが複数通り存在する。本発明では、固定条件入力手段によって、設定部分を固定すべき位置および姿勢が入力される。設定部分の位置および姿勢が決定される場合、3軸交点位置演算手段によって、3軸交点の3次元位置を一意的に求めることができる。
【0037】
また冗長関節部入力手段によって、3つの交点形成関節部以外となる4つの注目する関節部のうち1つが、冗長関節部として入力される。これによって4つの注目する関節部のうちで冗長関節部を除く残余の関節部の数が3つとなり、3軸交点の拘束される自由度の数、すなわち3軸交点の拘束される座標の数と一致する。また冗長角度入力手段によって、冗長関節部の冗長角度が既知となる。
【0038】
この場合、冗長関節部の冗長角度と、3軸交点の3次元位置とを代入することで、注目する4つの関節部のうちで、冗長関節部以外の各関節部の角度を解析的に求めることが可能な計算式が存在する。注目関節部角度演算手段は、前記計算式を表わす計算プログラムを記憶し、その計算プログラムに従うことで、各入力手段に入力された各種情報に基づいて、3軸交点が3次元位置に位置する場合における、注目する4つの関節部の角度を解析的にそれぞれ求めることができる。
【0039】
また注目する4つの関節部の角度と、設定部分の位置および姿勢とを代入することで、3軸交点形成関節部の角度を解析的に求めることが可能な計算式が存在する。交点関節部角度演算手段は、前記計算式を表わす計算プログラムを記憶し、その計算プログラムに従うことで、注目関節部角度演算手段の演算結果と、固定条件入力手段に入力された情報に基づいて、設定部分が予め定める位置および姿勢に位置する場合における、3軸交点形成関節部の角度を解析的にそれぞれ求めることができる。
【0040】
出力手段は、冗長角度入力手段によって入力された冗長角度と、各角度演算手段によって演算された関節部の各角度とを多関節ロボットに与える。これによって多関節ロボットは、設定部分を予め定める位置および姿勢で固定するとともに、冗長関節部を冗長角度に保つように、各関節部を変位駆動することができる。ここで、各関節部の角度は解析解、すなわち厳密解に基づいてそれぞれ求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。
【0041】
このように本発明によれば、冗長関節部を冗長角度にしたうえで、設定部分を予め定める固定状態に正確に保つことができる。たとえば設定部分の位置および姿勢を固定した状態で、冗長関節部および冗長関節部に連なるアーム体が障害物に衝突することを回避するために、冗長関節部を角変位させる場合、設定部分の位置および姿勢を正確に保ちつつ、障害物とアーム体との干渉を回避することができる。
【0042】
請求項10記載の本発明に従えば、設定部分を固定すべき固定条件を決定するとともに、複数の関節部のうちから冗長関節部と、冗長角度を決定する。この場合、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致する。次に、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式に、各入力工程によって決定した固定条件と冗長角度とを代入することで、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する。これによって複数の関節部の角度を解析的に求めることができる。このようにして求められた演算結果にしたがって、複数の関節部をそれぞれ角変位させることによって、設定部分を固定条件で固定した状態で、冗長関節部を冗長角度とすることができる。
【0043】
本発明によれば、冗長関節部以外の残余の関節部の各角度は解析解、すなわち厳密解に基づいて求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。したがって残余の関節部の各角度を近似解に基づいて決定する場合に比べて、固定すべき固定条件から設定部分がずれることを防ぐことができる。また解析解を求めることに起因して、各関節部の特異点を正確に判断することができ、利便性を向上することができる。
【0044】
請求項11記載の本発明に従えば、設定部分を固定すべき固定条件を決定するとともに、複数の関節部のうちから冗長関節部と、冗長角度を決定する。この場合、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致する。次に、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式に、各入力工程によって決定した固定条件と冗長角度とを代入することで、設定部分が固定条件で固定されるべき、注目する複数の関節部の角度をそれぞれ演算する。これによって注目する複数の関節部の角度を解析的に求めることができる。
【0045】
本発明によれば、冗長関節部以外の残余の関節部の各角度は解析解、すなわち厳密解に基づいて求められるので、設定部分を予め定める固定条件に正確に維持した状態で、冗長関節部を冗長角度に角変位することができる。したがって残余の関節部の各角度を近似解に基づいて決定する場合に比べて、固定すべき固定条件から設定部分がずれることを防ぐことができる。また解析解を求めることに起因して、各関節部の特異点を正確に判断することができ、利便性を向上することができる。
【図面の簡単な説明】
【0046】
【図1】本発明の第1実施形態である多関節ロボット30の制御装置2を示すブロック図である。
【図2】入力部20を示す正面図である。
【図3】中央演算処理部19の形態変更動作の手順を説明するためのフローチャートである。
【図4】対象点Phの位置を予め定める位置に固定した状態で、第1関節部9を角変位させた多関節ロボット30の状態を示す正面図である。
【図5】第1関節部9の角度q1を冗長角度として設定した場合における第2〜第4関節部10〜12の角度q2〜q4を示すグラフである。
【図6】対象点Phを固定した場合に多関節ロボット30が取り得る状態の種類を説明するための1軸冗長ロボット60を示す正面図である。
【図7】1軸冗長ロボット60が回転揺動状態となる場合において、各関節部が取り得る角度の組合せを示す図である。
【図8】図1に示す多関節ロボット30において、第1関節部9を冗長関節部として、その角度q1を順次角変位させた状態を示す正面図である。
【図9】図8において各関節部9〜12の角度q1〜q4の時間変化を示すグラフである。
【図10】図1に示す多関節ロボット30において、第4関節部12を冗長関節部として、その角度q4を順次角変位させた状態を示す正面図である。
【図11】図10において各関節部9〜12の角度q1〜q4の時間変化を示すグラフである。
【図12】中央演算処理部19の他の形態変更動作の手順を説明するためのフローチャートである。
【図13】中央演算処理部19が図12に示す形態動作の手順を行った場合における第1〜第4関節部9〜12の角度q1〜q4を示すグラフである。
【図14】形態変更動作における中央演算処理部19の予測報知動作の手順を説明するためのフローチャートである。
【図15】本発明の第2実施形態である多関節ロボット1の制御装置2を示すブロック図である。
【図16】図12に示す多関節ロボットを制御する制御装置2の入力部20を示す正面図である。
【図17】第2実施形態における中央演算処理部19の形態変更動作の手順を説明するためのフローチャートである。
【図18】移動予測動作と、形態変更動作と、移動教示動作とを行う場合における中央演算処理部19の動作手順を説明するためのフローチャートである。
【図19】入力部20の表示部42に表示される表示画面を示す図である。
【発明を実施するための最良の形態】
【0047】
図1は、本発明の第1実施形態である多関節ロボット30の制御装置2を示すブロック図である。制御装置2によって制御される多関節ロボット30は、4自由度を有する1軸冗長ロボットであり、4つのアーム体3〜6と、隣接する2つのアーム体を回転自在に連結する4つの関節部9〜12と、基台17とを有する。各アーム体3〜6は、それぞれ連結されて直列状に延びるアーム構成体16を構成する。また基台17は、アーム構成体16の基端部に連結され、壁面または床面などの固定位置に予め固定される。
【0048】
各関節部9〜12は、同軸関節部と傾斜関節部とのうちのいずれかである。同軸関節部は、隣接する2つのアーム体を、その各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する。傾斜関節部は、隣接する2つのアーム体のうちの一方を、各アーム体の軸線に対して傾斜する回転軸線まわりに円錐回転自在に連結する。
【0049】
各関節部9〜12は、基台17から第1〜第4関節部9〜12の順で並ぶ。第1関節部9は、同軸関節部である。また第2関節部10、第3関節部11および第4関節部12は、傾斜関節部である。また本発明において、用語「回転」とは、回転軸線まわりの360度以上の回転だけでなく、軸線まわりに360度以下の角度で角変位する状態も含むものとする。
【0050】
第1アーム体3の一端部は、第1関節部9によって、第1アーム体3の軸線A3と同軸の第1回転軸線A9まわりに回転自在に基台17に連結されている。また第2アーム体4の一端部は、第2関節部10によって、第1アーム体3の軸線A3および第2アーム体4の軸線A4に対して所定の角度、本実施の形態では45度で傾斜する第2回転軸線A10まわりに回転自在に、第1アーム体3の他端部に連結される。また第3アーム体5の一端部は、第3関節部11によって、第2アーム体4の軸線A4および第3アーム体5の軸線A5に対して所定の角度、本実施の形態では45度で傾斜する第3回転軸線A11まわりに回転自在に、第2アーム体4の他端部に連結される。
【0051】
第4アーム体6の一端部は、第4関節部12によって、第3アーム体5の軸線A5および第4アーム体6の軸線A6に対して所定の角度、本実施の形態では45度で傾斜する第4回転軸線A12まわりに回転自在に、第3アーム体5の他端部に連結される。第4アーム体6の他端部には、設定部分となる対象点Phが予め設定される。したがって対象点Phは、アーム構成体16の先端部に配置され、基台17は、アーム構成体16の基端部に配置される。
【0052】
このように各関節部9〜12は、隣接するアーム体を互いに回転可能に連結する。また各関節部9〜12が角変位することで、アーム構成体16は、図1に示すように、アーム体3〜6のそれぞれの軸線A3〜A6が同軸に配置されて一直線状に延びる状態に変形可能に構成される。各アーム体3〜6の軸線A3〜A6が一直線状に延びた状態では、第2回転軸線A10と第3回転軸線A11とは平行となり、第3回転軸線A11と第4回転軸線A12とは垂直となる。
【0053】
多関節ロボット30は、各関節部9〜12を回転させることで、アーム構成体16を蛇のように変形させ、対象点Phを任意の3次元位置に位置決め可能である。たとえば多関節ロボット30は、設備等が複雑に入り組んで作業経路が複雑な場合や、他のロボットと近接して並列に配置されて動作範囲を大きくすることができない場合に好適に用いられる。
【0054】
このような4つの関節部9〜12を有する多関節ロボット30は、4自由度を有するので、可動範囲内で対象点Phを予め定める3次元位置に位置合わせした状態で、第4アーム体6の取り得る姿勢が複数通り存在する。言換えると、多関節ロボット30は、対象点Phを予め定める3次元位置に位置合わせした状態で、4つの関節部9〜12のそれぞれの取り得る角度の組合せが複数通り存在する。
【0055】
各アーム体3〜6は、各アーム体3〜6をそれぞれ個別に回転駆動するモータを内蔵する。モータは、各アーム体3〜6を予め定める任意の角度に変位可能に構成される。
【0056】
各アーム体3〜6を対応する回転軸線A9〜A12まわりに回転可能に連結するための、各関節部9〜12の回転機構は、入力側部材と出力側部材とを備え、それらが相対的に回転可能に設けられる。入力側部材は、各関節部9〜12によって連結する2つの部材の一方に連結され、出力側部材は、前記2つの部材のうち他方に連結される。モータからの回転力が入力側部材に与えられると、入力側部材と出力側部材とが相対的に回転する。これによって一方の部材と他方の部材とを相対的に回転させることができる。同軸関節部9では、その回転軸線A9が、入力側部材および出力側部材に連結されるアーム体と一致する。傾斜関節部10〜12では、その回転軸線A10〜A12が、入力側部材および出力側部材にそれぞれ連結されるアーム体の軸線に対してそれぞれ傾斜する。
【0057】
このような回転機構が設けられることによって、隣接する2つの部材を相対的に回転させることができる。ここで、本発明における「関節部の角度」とは、関節部における入力側部材に対する出力側部材の角度を意味する。言い換えると、関節部によって連結される2つの部材のうち、入力側部材に連結される一方の部材に対する、出力側部材に連結される他方の部材の関節部の回転軸線まわりの角度を意味する。また本発明において「関節部の移動」とは、関節部の角度の角変位移動を示す。
【0058】
制御装置2は、入力部20と、中央演算処理部19と、出力部22とを含んで構成される。入力部20および出力部22は、中央演算処理部19に電気的に接続される。入力部20は、操作者からの各種指令情報が入力され、入力された各種情報を中央演算処理部19に与える。本実施の形態では、中央演算処理部19と出力部22とは、制御装置本体21に設けられ、入力部20は、制御装置本体21とは別体に設けられる。具体的には、入力部20は、操作者が多関節ロボットを直接制御するためのティーチペンダントによって実現され、制御装置本体21であるロボットコントローラに対して、ケーブルによって電気的に接続される。
【0059】
中央演算処理部19は、予め記憶される動作プログラムに従って、入力部20から与えられる各種情報に応答して、多関節ロボット30を制御するために必要な演算を行う。本実施の形態では、中央演算処理部19は、第4アーム体6に設定される対象点Phを予め定める目標位置に移動させるのに必要な、各関節部9〜12の角度q1〜q4をそれぞれ演算する。中央演算処理部19は、CPUなどの処理回路と、メモリなどの記憶回路とを含んで実現される。記憶回路には、動作プログラムとともに、演算に必要な計算式、アーム体の長さ、関節部の種類などを記憶する。
【0060】
出力部22は、中央演算処理部19の演算結果を多関節ロボット30に与える出力手段となる。本実施の形態では、出力部22は、中央演算処理部19の演算結果を多関節ロボット30に設けられる電源回路に与える。電源回路は、中央演算処理部19の演算結果に応じた動力を、関節部9〜12毎に設けられるモータにそれぞれ個別に与える。動力が与えられた各モータは、中央演算処理部19の演算結果に従って、対応する関節部を所定の角度に角変位させる。
【0061】
また制御装置2は、第4アーム体6の対象点Phを予め指定される移動経路に沿って移動させるにあたって、回転すべき各関節部9〜12の角度を演算する。この移動経路は、ロボット動作前に、操作者によって入力部20から中央演算処理部19に入力される。また多関節ロボット30は、4つの関節部9〜12を有することで、基台17に対して、対象点Phを予め定める位置で固定した状態で、各関節部の取り得る角度の組合せが複数通り存在する。
【0062】
本実施形態の制御装置2は、ロボットの動作を教示するための教示動作を行うために用いられ、教示動作のための形態変更動作を行う。形態変更動作とは、対象点Phを予め定める固定条件で固定した状態で、第1〜第4関節部9〜12の角度を変化させて、ロボットの形状および姿勢などの外形的形態を変更する動作である。本実施の形態では形態変更動作は、基台17に設定されるベース座標に対する対象点Phの3次元位置を固定した状態で、第1〜第4関節部9〜12の角度を変化させる動作となる。言換えると、形態変更動作では、アーム構成体16のうちで直列方向一端部に設定される対象点Phが、アーム構成体16の他端部に設定される座標系に従って求められる拘束条件で固定される。
【0063】
形態変更動作を行うにあたって、制御装置2は、第1〜第4関節部9〜12のうちから選択された1つの冗長関節部と、その冗長関節部の角度である冗長角度と、対象点Phを固定すべき3次元位置とが入力部20を介して与えられる。中央演算処理部19は、対象点Phを固定すべき固定状態に固定したうえで、冗長関節部が冗長角度となる場合の、残余の関節部の角度を演算する。出力部22は、中央演算処理部19の演算結果を多関節ロボット30に与える。演算結果が与えられた多関節ロボット30は、冗長関節部を冗長角度とするとともに、残余の関節部の角度を演算結果に応じて位置合わせして、対象点Phを指定された固定状態とする。
【0064】
形態変更動作を行うことによって、操作者は、入力部20を介して冗長関節部を選択するとともに、その冗長角度を入力することで、対象点Phを固定した状態で、アーム構成体16の形態を変更させることができる。この場合、操作者は、中央演算処理部19に形態変更動作を行わせて、多関節ロボット30の形態を確認することによって、対象点Phを固定した状態で、障害物との衝突を回避するアーム構成体16の形態を実現する各関節部9〜12の角度を容易に決定することができる。操作者は、障害物との衝突を回避する各関節部9〜12の角度q1〜q4を決定すると、その各関節部9〜12の角度q1〜q4をロボットの移動動作を行う場合の情報として用いる。
【0065】
図2は、入力部20を示す正面図である。入力部20には、ロボット教示のための情報および指令が入力される操作部41と、操作画面を表示する表示部42とを含む。操作部41には、各関節部操作スイッチ43と、ツール移動スイッチ44と、形態変更スイッチ45とを含む。また操作部41は、ロボットの動作プログラムを入力するための命令キー、カーソルキーおよびテンキーなどをさらに含む。
【0066】
関節部操作スイッチ43は、各関節部9〜12の角度q1〜q4を入力するためのスイッチであって、各関節部9〜12について、プラス方向およびマイナス方向への予め定める角変位量の角変位指令が与えられる。ツール移動スイッチ44は、ベース座標軸またはツール座標軸に従う対象点Phの移動指令が与えられる。ここで、ベース座標軸とは、基台17に設定される座標であって、ツール座標軸とは、第4アーム体6に設定される座標である。また形態変更スイッチ45は、対象点Phを予め定める位置に固定したまま、アーム構成体16の形態を変化させるための形態変更指令が与えられる。
【0067】
形態変更動作を行う場合、ツール移動スイッチ44は、対象点Phを固定すべき位置の情報が入力される固定条件入力手段となる。また形態変更スイッチ45が押下された状態で、関節部操作スイッチ43によって1つの関節部の角変位量が指定されると、指定された関節部が冗長関節部として入力される。また、その角変位量に対応する角度が冗長関節部の角度として入力される。したがって形態変更スイッチ45と関節部操作スイッチ43とによって、冗長関節部が入力される冗長関節部入力手段と、冗長関節部の角度である冗長角度が入力される冗長角度入力手段とが構成される。
【0068】
図3は、中央演算処理部19の形態変更動作の手順を説明するためのフローチャートである。まずステップa0で、中央演算処理部19は、多関節ロボット30を制御可能な状態で、形態変更スイッチ45が押下されたことを判断すると、ステップa1に進み、形態変更動作を開始する。
【0069】
ステップa1では、中央演算処理部19は、ツール移動スイッチ44によって対象点Phの移動指令が与えられると、対象点Phを指定された位置に移動するための演算結果を出力部22に与える。またツール移動スイッチ44による移動指令がなければ、現時点の対象点Phの位置を維持させる。中央演算処理部19は、関節部操作スイッチ43が押下されたと判断すると、その時点の対象点Phの位置を固定すべき固定条件として決定し、ステップa2に進む。
【0070】
ステップa2では、中央演算処理部19は、関節部操作スイッチ43によって指定された関節部を、冗長関節部として決定する。冗長関節部は、第1〜第4関節部9〜12のいずれか1つとなる。また関節部操作スイッチ43によって指定された角度を、冗長角度として決定する。本実施の形態では、関節部操作スイッチ43のうち、プラス方向移動スイッチが押圧されると、関節部操作スイッチ43が押下される前の状態から、予め定める角変位量で冗長関節部がプラス方向に軸線まわりに角変位した角度を冗長角度として決定する。またマイナス方向移動スイッチが押圧されると、関節部操作スイッチ43が押下される前の状態から、予め定める角変位量で冗長関節部がマイナス方向に軸線まわりに角変位した角度を冗長角度として決定する。このように冗長関節部および冗長角度を決定するとステップa3に進む。
【0071】
ステップa3では、中央演算処理部19は、ステップa1で決定した対象点Phの3次元位置に、対象点Phが位置する場合における、第1〜第4関節部9〜12の角度q1〜q4を求める。具体的には、第1〜第4関節部9〜12のそれぞれの角度q1〜q4を変数として、対象点Phの3次元位置を求めるための順変換関係式と、ステップa2で決定した冗長関節部の冗長角度と、ステップa1で決定した対象点Phの3次元位置とに基づくことによって、第1〜第4関節部9〜12の角度q1〜q4を解析的に求めることができる。
【0072】
順変換問題を解くことによって、対象点Phのx座標位置を表わすx座標式、対象点Phのy座標位置を表わすy座標式、対象点Phのz座標位置を表わすz座標式は、第1〜第4関節部9〜12の各角度q1〜q4を変数とする関数で表わされる。順変換問題によるx座標式、y座標式およびz座標式に、ステップa1で求めた対象点Phの3次元位置を代入することで、それらの座標式は、第1〜第4関節部9〜12の角度q1〜q4の数、すなわち4つの未知数を有する三元連立方程式となる。
【0073】
また第1〜第4関節部9〜12の各角度q1〜q4のうちの1つは、冗長角度としてステップa2で決定しているので、冗長角度を上述した3元連立方程式に代入することによって、3つの未知数を有する三元連立方程式となる。この3元連立方程式を変形することで、3つの未知数をそれぞれ求める方程式を表わす計算式を導出することができる。この方程式に、対象点Phの座標、冗長関節部および冗長関節部の角度を代入することで、第1〜第4関節部9〜12のうちで、冗長関節部以外の関節部の各角度をそれぞれ解析的に求めることができる。
【0074】
中央演算処理部19は、冗長関節部の冗長角度と、対象点Phのx座標、y座標、z座標を代入することで、第1〜第4関節部9〜12のうちで冗長関節部以外の各関節部の角度を解析的に求めることが可能な方程式を表わす計算プログラムを記憶している。中央演算処理部19は、第1〜第4関節部9〜12の計算プログラムにしたがって、第1〜第4関節部9〜12のうちで、冗長関節部以外の各関節部の角度を解析的に演算し、ステップa4に進む。冗長関節部の角度は、ステップa2で既に決定しているので、演算することなく求めることができる。このように中央演算処理部19は、第1〜第4関節部9〜12の角度q1〜q4をそれぞれ演算する角度演算手段となる。
【0075】
ステップa4では、中央演算処理部19は、求めた各関節部9〜12の各角度q1〜q4が可動限界に到達していないことを判断するとステップa5に進み、各角度q1〜q4のいずれかが可動限界に到達していること、または可動限界を超えていることを判断すると、ステップa6に進む。たとえば中央演算処理部19は、ステップa3で求めた各関節部の角度が、解析的に求められる最大値または最小値となると、可動限界に到達したことを判断する。また各関節部の角度を求められない場合に可動限界を超えていると判断する。
【0076】
ステップa5では、ステップa3で解析的に求めた第1〜第4関節部9〜12の角度q1〜q4を演算結果として、出力部22に与える。出力部22は、与えられた演算結果を多関節ロボット30に与える。これによって多関節ロボット30は、各関節部9〜12が角変位し、対象点Phを固定位置に維持した状態で、冗長関節部を冗長角度とする。このとき、第1〜第4関節部9〜12の角度q1〜q4は、収束演算などの近似法とは異なり、解析的な演算方法によって求められるので、冗長関節部の移動前と移動後とで、固定位置から対象点Phがずれることがない。このように演算結果を出力部22に与えて、多関節ロボット30を動作させると、ステップa6に進む。
【0077】
ステップa6では、中央演算処理部19は、形態変更動作を終了する終了条件を満足するか否かを判断する。たとえば形態変更解除スイッチが押下されることで、終了条件を満足することを判断してもよい。中央演算処理部19は、形態変更動作の終了条件を満足していないと判断すると、ステップa1に戻り、関節部操作スイッチ43が再び押下されるのを待機する。この状態で、関節部操作スイッチ43が再度押下されると、その指示に従ってステップa1〜a5を順に行う。またステップa6で終了条件を満足することを判断すると、ステップa7に進み、ステップa7で形態変更動作を終了する。
【0078】
このようにして、中央演算処理部19が形態変更動作を行うことによって、操作者が、対象点Phの固定位置と、冗長関節部と、その冗長角度とを指定することによって、対象点Phの位置を確実に固定したまま、各関節部9〜12を角変位させて、多関節ロボット30の形態を変更させることができる。このとき各関節部9〜12の角度q1〜q4を解析解として求めることができるので、収束演算を用いて解を求める場合に比べて短時間で各関節部9〜12の角度を求めることができ、対象点Phの位置を固定したままロボットの形態を滑らかに変更することができる。
【0079】
また操作者は、多関節ロボット30の形態を確認しながら、中央演算処理部19に形態変更動作を行わせることができる。これによって、操作者は、対象点Phを固定位置に位置させることができる各関節部9〜12の角度q1〜q4の複数の組合せのうちから、1つを容易に選択することができる。これによって操作者は、障害物に衝突することを回避した多関節ロボット30の形態で、対象点Phを固定位置に配置させることができ、各関節部9〜12の角度q1〜q4を求めることができる。
【0080】
図4は、対象点Phの位置を予め定める位置に固定した状態で、第1関節部9を角変位させた多関節ロボット30の状態を示す正面図である。第1関節部9の角度q1をプラス方向に角変位するに伴って、多関節ロボット30は、図4(1)〜図4(8)の順で変形する。また図5は、第1関節部9の角度q1を冗長角度として設定した場合における第2〜第4関節部10〜12の角度q2〜q4を示すグラフである。図5(1)は、横軸に第1関節部9の角度q1を示し、縦軸に第2関節部10の角度q2を示す。また図5(2)は、横軸に第1関節部9の角度q1を示し、縦軸に第3関節部11の角度q3を示す。図5(3)は、横軸に第1関節部9の角度q1を示し、縦軸に第4関節部12の角度q4を示す。
【0081】
詳細については後述するが、本実施の形態では、対象点Phを予め定める3次元位置に固定した状態に維持する、各関節部9〜12の角度q1〜q4の解析解を決定することができる。これによって各関節部9〜12の角度q1〜q4の近似解を決定する場合に比べて、対象点Phが固定位置からずれることを防ぐことができる。たとえばステップa2において、関節部操作スイッチ43のうち、プラス方向またはマイナス方向移動スイッチが1回押圧された場合に、冗長関節部が角変位する角変位量が大きく設定される場合であっても、対象点Phの固定状態を正確に維持することができる。同様に、ステップa2において、冗長関節部が高速で移動する場合であっても、対象点Phの固定状態を正確に維持することができる。
【0082】
これによって形態変更動作によって、多関節ロボット30の障害物回避形態の探索を行った場合、対象点Phの位置を正確に固定した状態で、各関節部9〜12を角変位させることができる。また対象点Phを固定位置に正確に保ちつつ、障害物と各アーム体3〜6との干渉を回避することができるので、対象点Phの位置ずれに起因する多関節ロボットの動作不良を防ぐことができる。
【0083】
また中央演算処理部19は、各関節部9〜12の角度q1〜q4の解析解を求めることに付随して、冗長関節部が可動限界に到達したか否かを判断することができる。ステップa4に示すように、中央演算処理部19は、冗長関節部が可動限界に到達することを判断すると、冗長関節部が可動限界に到達した状態の関節部の角度を演算する。また可動限界を超えるような冗長角度が入力されても、可動限界に到達した状態の関節部の角度を演算する。
【0084】
これによって中央演算処理部19が、機構的に移動不可能な移動指令値を、多関節ロボットに与えることを防ぐことができ、関節部の損傷を防ぐことができる。また中央演算処理部19は、冗長関節部が可動限界に到達したか否かを精度よく判断することができるので、可動限界に近い領域まで冗長関節部を角変位させることができ、ロボットの可動範囲を広くすることができる。
【0085】
また中央演算処理部19は、冗長関節部が可動限界に到達する前に、冗長関節部が可動限界近傍の警告領域に到達したことを判断すると、その判断結果を入力部20の表示部42を介して報知してもよい。たとえば中央演算処理部19は、演算によって決定される冗長関節部の可動限界角度よりも予め定める所定角度分手前の角度に、冗長角度が達した場合に、警告領域に到達したことを判断する。この場合、中央演算処理部19は、警告範囲に到達したことを判断して報知する警告報知手段となる。
【0086】
たとえば図5の場合では、冗長関節部が第1関節部9であって、第3関節部11の角度q3が180度となった場合に可動限界に達する。この場合、中央演算処理部19は、第3関節部11の角度q3が175度に達すると、警告領域に到達したことを判断し、判断結果を表示部42に表示させる。これによって入力部20を操作する操作者は、冗長関節部が可動限界に近づいていることを判断することができ、利便性を向上することができる。さらに中央演算処理部19は、冗長関節部が警告範囲に入った場合、その関節部の角度が警告範囲を脱出する角変位方向を入力部20の表示部42に表示することが好ましい。
【0087】
本実施形態の制御装置2が制御する多関節ロボット30は、傾斜関節部を含むので、形態変更動作における多関節ロボット30の変形形態は変則的となる。この場合、操作者は、各関節部9〜12の変形を直感的に予測することが困難である。本実施の形態では、中央演算処理部19は、警告範囲および警告範囲を脱出するための角変位方向を報知するで、操作者は、警告範囲および脱出方向を容易に判断することができ、利便性を向上することができる。
【0088】
次に、ステップa3における第1〜第4角度q1〜q4の演算工程の詳細について説明する。順変換問題に基づくことによって、第1〜第4関節部9〜12のそれぞれの角度q1〜q4が決定されると、対象点Phのx座標位置phx、y座標位置phy、z座標位置phzが一義的に決定される。すなわち対象点Phのx座標、y座標、z座標を求めるための式は、第1〜第4関節部9〜12のそれぞれの角度q1〜q4を変数とする関数によって表わされ、本実施の形態では、以下に示す(1)式の関係を有する。
【0089】
【数1】
【0090】
(1)式において第1関節部9の角度をq1とし、第2関節部10の角度をq2とし、第3関節部11の角度をq3とし、第4関節部12の角度をq4とする。また第1関節部9から第2関節部10までの第1アーム体3の距離をL1とし、第2関節部10から第3関節部11までの第2アーム体4の距離をL2とし、第3関節部11から第4関節部12までの第3アーム体5の距離をL3とし、第4関節部12から対象点Phまでの距離をL4とする。またsin(θ)はθの正弦関数(サイン関数)を示し、cos(θ)は、θの余弦関数(コサイン関数)を示す。
【0091】
また(1)式において、対象点Phの座標phx,phy,phzは、基台17に設定されるベース座標軸に従って、第1アーム体3の代表点p1を原点とした場合の座標である。ここでベース座標軸は、各アーム体の軸線が一直線状に配置された状態で、その直線と同軸に延びる軸をz軸とし、z軸に垂直な平面に沿って延びてz軸を通過する軸をx軸およびy軸とする。x軸およびy軸は、互いに直交する。また各関節部9〜12の角度q1〜q4は、各アーム体3〜6の軸線が一直線状に延びた状態から関節部が角変位した角度とする。また第1アーム体3の代表点p1は、第1アーム体の軸線A1と、第2関節部10の回転軸線A10との交点に設定される。
【0092】
対象点Phを固定すべき位置のx座標phx、y座標phy、z座標phzの数値を(1)式に代入することによって、第1〜第4関節部9〜12の4つの角度q1〜q4が未知数となる3元連立方程式となる。第1〜第4関節部9〜12のうちの1つを冗長関節部として決定し、冗長関節部の冗長角度を(1)式に代入することによって、第1〜第4関節部9〜12の4つの角度q1〜q4のうちの1つが既知となり、未知数が3つの3元連立方程式となる。これによって(1)式を連立して解くことによって、対象点Phを固定位置に位置すべき、各関節部9〜12の角度q1〜q4を解析的に求めることができる。
【0093】
たとえば第1関節部9を冗長関節部として決定し、第1関節部9を角変位すべき角度q1を冗長角度として決定する。この場合、(1)式を変形することによって、以下の(2)式に示すような、第1関節部9と第4関節部12の角度q1,q4とが未知数となる方程式に変形することができる。(2)式に、対象点Phのz座標phz、冗長関節部となる第1関節部9の角度q1を代入することによって、第4関節部12の角度q4を求めることができる。得られた第4関節部12の角度q4は、対象点Phを固定位置に位置させ、かつ第1関節部9を冗長角度である第1角度q1に角変位した場合における第4関節部12の角度q4を意味する。
【0094】
【数2】
【0095】
(2)式を、以下の(3)式に変形し、第1関節部9の角度q1を代入することによって、第4関節部12の角度q4を求めることができる。
【0096】
【数3】
【0097】
ここで、arccosは、逆余弦関数(アークコサイン関数)を意味する。また(3)式におけるプラスマイナスの2つの符号を離散形態符号と称する。予め定める条件、たとえば計算前に設定されている第4関節部12の角度q4などに基づいて、2つの離散形態符号のいずれかを決定することで、対象点Phを固定位置に固定する場合における、第4関節部12の角度q4を演算することができる。
【0098】
また(3)式において解が1つしかない場合、すなわちアークコサインの変数が、1または−1となる場合、第4関節部12は特異点に位置する。具体的には、第4関節部12の角度q4が、0度または180度となると、特異点となる。またアークコサインの変数の絶対値が1を超えると、解が存在しない。このような判定を行うことで、中央演算処理部19は、特異点でない解が存在するか、解が特異点となるか、解が存在しないかを判断することができる。
【0099】
次に、(1)式を変形することによって、以下の(4)式に示すような、第3関節部11と第4関節部12との角度q3,q4が未知数となる方程式に変形することができる。(4)式に、(3)式に基づいて決定した第4関節部12の角度q4を代入することによって、第3関節部11の角度q3を求めることができる。得られた第3関節部11の角度q3は、対象点Phを固定位置に保ち、かつ第1関節部9を冗長角度である第1角度q1に角変位した場合における第3関節部11の角度q3を意味する。
【0100】
【数4】
【0101】
次に、(1)式のうちのphx、phy、phzを求めるそれぞれの式のいずれか2つを連立させた式に、冗長関節部として設定される第1関節部9の角度q1と、(3)式に基づいて求めた第4関節部12の角度q4と、(4)式に基づいて求めた第3関節部11の角度q3とを代入することによって、第2関節部10の角度q2における、cos(q2)と、sin(q2)を求めることができ、これらから第2関節部10の角度q2を求めることができる。
【0102】
このようにして、第1〜第4関節部9〜12の角度q1〜q4をそれぞれ求めることができる。この場合、中央演算処理部19は、上述した(3)式、(4)式、第2関節部10の角度q2を求める式をそれぞれ示す計算プログラムを記憶することで、第1関節部9の第1角度q1が決定されることで、解が存在する場合、対象点Phを固定位置に保った場合における第2〜第4関節部10〜12の角度q2〜q4をそれぞれ求めることができる。
【0103】
また中央演算処理部19は、冗長関節部の冗長角度を正または負の方向に徐々に角変位させた場合に、冗長関節部の角度が取り得ることが可能な極大値または極小値となった場合、その角度を可動限界として判断することができる。たとえば冗長角度を大きくしていくと、冗長関節部以外の残余の関節部の角度の解を求めることができなくなる場合が生じる。この場合、中央演算処理部19は、各関節部の角度の解を求めることができる冗長角度のうちで、最大値を可動限界として判断する。同様に冗長角度を小さくしていくと、冗長関節部以外の残余の関節部の角度の解を求めることができなくなる場合が生じる。この場合、中央演算処理部19は、各関節部の角度の解を求めることができる冗長角度のうちで、最小値を可動限界として判断する。
【0104】
中央演算処理部19は、上述するステップa1およびステップa2を終えた状態で、冗長関節部の可動限界を演算することができる。そして指定された冗長角度と、可動限界角度との偏差を求めることができる。この場合、その偏差が予め定める値以下となる場合に、可動限界近傍に設定される警告領域に、冗長関節部が到達したことを判断することができる。
【0105】
また中央演算処理部19は、現時点での冗長角度に対して、プラス方向に冗長角度を増加させた場合と、マイナス方向に冗長角度を増加させた場合とで、前記偏差の変化量を調べることで、偏差が大きくなる冗長角度の角変位方向を求めることができる。中央演算処理部19は、偏差が大きくなる冗長角度の角変位方向を、冗長角度が可動限界から遠ざかって、警告領域から脱出する方向として判断することができる。
【0106】
また冗長関節部として、第1関節部以外の関節部が選択される場合には、同様にして(1)式を変形して連立方程式を解くことによって、解が存在する場合、冗長関節部以外の各関節部の角度をそれぞれ求めることができる。したがって中央演算処理部19は、冗長関節部として選択された関節部に応じて、残余の関節部を求める計算プログラムをそれぞれ記憶することで、第1〜第4関節部9〜12のいずれが冗長関節部として選択されたとしても、対象点Phを固定位置に維持しかつ、冗長関節部が冗長角度となる場合の、残余の関節部の角度をそれぞれ演算することができる。
【0107】
図6は、対象点Phを固定した場合に多関節ロボット30が取り得る状態の種類を説明するための1軸冗長ロボット60を示す正面図である。図6に示す1軸冗長ロボットは、第1〜第4アーム体50〜53と、隣接するアーム体を連結する複数の関節部54〜57とを有する。各アーム体50〜53は、予め定めるx軸とy軸とを含むxy平面上に配置される。第1アーム体50の基端部50aは、固定部58に第1関節部54によって連結され、第1関節部54によって固定部58に対して、xy平面に垂直なz軸まわりに角変位可能となる。
【0108】
また第2アーム体51の基端部51aは、第2関節部55によって第1アーム体50の先端部50bに連結され、第2関節部55によって第1アーム体50の先端部50bに対して、z軸まわりに角変位可能となる。また第3アーム体52の基端部52aは、第3関節部56によって第2アーム体51の先端部51bに連結され、第3関節部56によって第2アーム体51の先端部51bに対して、z軸まわりに角変位可能となる。また第4アーム体53の基端部53aは、第4関節部57によって第3アーム体52の先端部52bに連結され、第4関節部57によって第3アーム体52の先端部52bに対して、z軸まわりに角変位可能となる。
【0109】
したがって図6に示す1軸冗長ロボット60は、第4アーム体53をxy平面内で移動可能であるとともに、第4アーム体53をz軸まわりに回転可能な4関節平面ロボットとなる。
【0110】
このような1軸冗長ロボット60に関して、第4アーム体53の先端部53bのx座標およびy座標と、第4アーム体53のz軸まわりの姿勢とを固定する固定条件、すなわち3つの自由度を拘束する固定条件を設定する。この固定条件で第4アーム53を固定した場合における第1〜第4関節部54〜57の角度を考える。
【0111】
1軸冗長ロボット60は、上述した固定条件で第4アーム体53を固定した状態で、第1〜第4関節部54〜57の取り得る組合せが複数存在する。ここで、4つの関節部54〜57のうちの1つの角度を設定すれば、固定条件を満足する各関節部の角度を、逆変換により求めることができる。
【0112】
4自由度を有する1軸冗長ロボット60は、対象点Phの3つの自由度を拘束した固定状態では、各アーム体の長さと、第1関節部と第4関節部との間の距離とに応じて、3つの形態を取り得ることになる。
【0113】
第1の形態では回転揺動状態となる。この場合、1軸冗長ロボット60は、図6(1)に示すように、第4アーム体53を固定した状態で、第1アーム体50が固定部58に対して360度以上回転可能でかつ、第3アーム体52が第4アーム体53に対して360度以下揺動角変位可能となる。また第2アーム体51は、第1アーム体50に対して360度以上回転可能となり、第3アーム体52は、第2アーム体51に対して360度未満揺動角変位可能となる。
【0114】
第1関節部54と第2関節部55との間の距離をL1とし、第2関節部55と第3関節部56との間の距離をL2とし、第3関節部56と第4関節部57との間の距離をL3とし、第1関節部54と第4関節部57との間の距離をLwとすると、Lw+L2>L1+L3、Lw>L2、L1>L3、Lw−L2<L3−L1である場合に、回転揺動状態となる。
【0115】
また第2の形態では、2重揺動状態となる。この場合、1軸冗長ロボット60は、図6(2)に示すように、第4アーム体53を固定した状態で、第1アーム体50が固定部58に対して360度未満揺動角変位可能となり、第3アーム体52が第4アーム体53に対して360度未満揺動角変位可能となる。また第2アーム体51は、第1アーム体50に対して360度未満揺動角変位可能となり、第3アーム体52は、第2アーム体51に対して360度未満揺動角変位可能となる。
【0116】
また第3の形態では、2重回転状態となる。この場合、1軸冗長ロボット60は、図6(3)に示すように、第4アーム体53を固定した状態で、第1アーム体50が固定部58に対して360度以上回転可能となり、第3アーム体52が第4アーム体53に対して360度以上回転可能となる。また第2アーム体51は、第1アーム体50に対して360度以上回転可能となり、第3アーム体52は、第2アーム体51に対して360度以上回転可能となる。
【0117】
2重揺動状態および2重回転状態では、第1〜第4関節部54〜57のいずれか1つの角度を設定すれば、第4アーム体53が固定状態となる場合に第1〜第4関節部54〜57が取り得る角度の組合せは、1つとなる。したがって第1〜第4関節部54〜57のうちから、1つの冗長関節部を設定し、その角度を指定することで、1軸冗長ロボットが取り得る範囲を全て計算することができる。これに対して回転揺動状態では、第1〜第4関節部54〜57のうちから1つの冗長関節部を設定しただけでは、1軸冗長ロボット60が取り得る範囲を全て計算することができない。
【0118】
図7は、1軸冗長ロボット60が回転揺動状態となる場合において、各関節部が取り得る角度の組合せを示す図である。図7(1)は、1軸冗長ロボット60が初期状態である場合を示し、第1アーム体50が固定部58に対して角変位する状態を、図7(2)〜図7(5)に順に示す。
【0119】
第4関節部57を冗長関節部として選択する場合、図7(3)に示すように、第3アーム体52を第4アーム体53に対して一方向(図7では反時計方向)に角変位して、揺動限界角度に到達すると、それ以上、第3アーム体52を第4アーム体53に対して、一方向に角変位させることができなくなる。この場合、第1アーム体50も、固定部58に対して一方向にそれ以上角変位することができなくなる。揺動限界角度に到達した状態から、第3アーム体52を第4アーム体53に対して他方向(図7では時計方向)に角変位すると、第1アーム体50も、固定部58に対して他方向に角変位する。このように第4関節部を冗長関節部として選択すると、第1アーム体50を固定部58に対して360度回転させることができず、1軸冗長ロボット60が取り得る範囲を全て計算することができない。これに対して、第1関節部54を冗長関節部として選択する場合、図7(1)〜図7(5)に示すように、第1アーム体50を固定部58に対して一回転させることができ、1軸冗長ロボット60が取り得る範囲を全て計算することができる。
【0120】
したがって1軸冗長ロボット60が、回転揺動状態となる場合、第1関節部54と第2関節部55とのうちのいずれか一方と、第3関節部56と第4関節部57とのいずれか一方との2つを冗長関節部として選択可能とすることによって、2つのうちいずれかが、回転揺動状態のうちで360度以上アーム体が回転する関節部を冗長関節部として選択可能となり、1軸冗長ロボット60が取り得る範囲を全て計算することができる。
【0121】
1軸冗長ロボット60の各アーム体50〜53の長さが決定しているとしても、第1関節部54と第4関節部57との間の距離Lwの長さが変化することによって、回転揺動状態となるか、2重揺動状態および2重回転状態となるかが変化する。上述したように、第1関節部54と第2関節部55とのうちのいずれか一方と、第3関節部56と第4関節部57とのいずれか一方との2つを、冗長関節部として選択可能とすることによって、仮に回転揺動状態となったとしても、1軸冗長ロボット60が取り得る範囲を全て計算することができる。このことは、4つの関節部を有する1軸冗長ロボットに共通することである。したがって図1に示す多関節ロボット30であっても同様となる。
【0122】
図8は、図1に示す多関節ロボット30において、第1関節部9を冗長関節部として、その角度q1を順次角変位させた状態を示す正面図であり、図8(1)〜図8(3)の順に変化する。また図9は、図8において各関節部9〜12の角度q1〜q4の時間変化を示すグラフである。図9(1)は、第1関節部9の角度q1の時間変化を示し、図9(2)は、第2関節部10の角度q2の時間変化を示し、図9(3)は、第3関節部11の角度q3の時間変化を示し、図9(4)は、第4関節部12の角度q4の時間変化を示す。
【0123】
図8および図9には、対象点Phを固定位置に位置させた場合に、多関節ロボット30が回転揺動状態となり、第1関節部9および第2関節部10が揺動可能であって、第3関節部11および第4関節部12が回転可能である場合を示す。この場合、第1関節部9の角度q1を予め定める時間毎に予め定める角変位量、順次変化させると、対象点Phを所定位置に位置させた状態で、第1関節部9の角変位とともに、第2〜第4関節部10〜12が角変位する。
【0124】
図8(1)に示す初期状態から、第1関節部9の角度q1を、たとえばマイナス方向に角変位すると、図8(2)〜図8(3)に順に示すように、多関節ロボット30の形態が変化する。第1関節部9の角度q1をマイナス方向に角変位させた場合に、図8(3)に示すように、第1関節部9が0度となることで極小値の可動限界となる。この場合、第4関節部12の角度q4が180度の状態で、第1アーム体3と第2アーム体4とが突っ張ってしまい、さらなる角変位が不可能な状態となる。
【0125】
図10は、図1に示す多関節ロボット30において、第4関節部12を冗長関節部として、その角度q4を順次角変位させた状態を示す正面図であり、図10(1)〜図10(5)の順に変化する。また図11は、図10において各関節部9〜12の角度q1〜q4の時間変化を示すグラフである。図11(1)は、第1関節部9の角度q1の時間変化を示し、図11(2)は、第2関節部10の角度q2の時間変化を示し、図11(3)は、第3関節部11の角度q3の時間変化を示し、図11(4)は、第4関節部12の角度q4の時間変化を示す。
【0126】
図10および図11には、対象点Phを固定位置に位置させた場合に、多関節ロボットが回転揺動状態となり、第1関節部9および第2関節部10が揺動可能であって、第3関節部11および第4関節部12が回転可能である場合を示す。この場合、第4関節部12を予め定める時間毎に予め定める角変位量、順次変化させると、対象点Phを所定位置に位置させた状態で、第4関節部12の角変位とともに、第1〜第3関節部9〜11が角変位する。
【0127】
図11(1)に示す初期状態から、第4関節部12の角度q4を、たとえばプラス方向に角変位すると、図11(2)〜図11(5)に順に示すように、多関節ロボット30の形態が変化する。第4関節部12の角度q4を指定してプラス方向に角変位させた場合には、第4関節部12の角度q4が180度以上となっても、第1アーム体3と第2アーム体4とが突っ張ることがなく、第1関節部9を冗長関節部として選択した場合に比べて、第4関節部12のさらなる角変位が可能となる。
【0128】
このように、第4関節部12を冗長関節部として選択することで、対象点Phを固定状態に維持したうえで、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる。実際には、対象点Phの固定位置によっては、第1または第2関節部を冗長関節部として選択したほうが、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる場合がある。
【0129】
したがって第1関節部9と第2関節部10との2つのうちの1つと、第3関節部11と第4関節部12との2つのうちの1つとを冗長関節部として選択することで、対象点Phを固定状態に維持したうえで、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる。
【0130】
このことから本発明の第2実施形態として、図2に示す関節部操作スイッチ43は、第1関節部9と第2関節部10のいずれかと、第3関節部11と第4関節部12のいずれかとの2つが選択可能に配置される。このように4つではなく2つのスイッチが設けられるだけでも、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる。これによって操作者は、4つの関節部9〜12の全てを冗長関節部として選択可能とする必要がなく、形態変更動作における入力部20の操作性を向上することができる。また関節部操作スイッチ43を、4つから2つのスイッチとすることで、形態変更動作に必要なスイッチを減らすことができ、操作性を向上するとともに、スイッチとして必要な部品点数を減らすことができる。
【0131】
また中央演算処理部19は、第1および第2関節部9,10のいずれかが冗長関節部として選択された場合と、第3および第4関節部11,12のいずれかが冗長関節部として選択された場合とにおいてのみ各関節部を演算するだけでよく、各関節部の角度を演算するのに必要な計算プログラムを2つにすることができ、計算プログラムを4つ用意する場合に比べて、中央演算処理部19に必要な記憶容量を小さくすることができる。また本実施の形態では、第1関節部9と、第4関節部12とが冗長関節部として選択可能に設定される。このように第1関節部9と第4関節部12とを冗長関節部として選択可能とすることによって、他の関節部を冗長関節部として選択する場合に比べて、各関節部の角度を演算するための計算プログラムを簡単化することができ、短時間に各関節部の角度を求めることができる。
【0132】
図12は、中央演算処理部19の他の形態変更動作の手順を説明するためのフローチャートである。まずステップb0で、中央演算処理部19は、多関節ロボット30を制御可能な状態で、形態変更スイッチ45が押下されたことを判断すると、ステップb1に進み、形態変更動作を開始する。
【0133】
中央演算処理部19は、ステップb1〜b2で、上述したステップa1〜a2と同様の同様の手順を順に行い、ステップb3に進む。ステップb3では、中央演算処理部19は、上述するステップa3と同様にして、第1〜第4関節部9〜12の角度を求めるための計算プログラムにしたがって、第1〜第4関節部9〜12のうちで、冗長関節部以外の各関節部の角度を解析的に求める。また冗長関節部の角度は、ステップb2で既に決定しているので、演算することなく求めることができる。このようにして第1〜第4関節部9〜12の各角度q1〜q4を求めるとステップb4に進む。
【0134】
ステップb4では、中央演算処理部19は、ステップa4と同様にして、ステップb3の演算結果に基づいて、ステップb2で決定された冗長関節部の角度が可動限界に到達していないことを判断すると、ステップb5に進む。またステップb2で決定された冗長関節部の角度が可動限界に到達または超えていることを判断するとステップb9に進む。
【0135】
ステップb5では、現時点での冗長関節部の角度が可動限界に到達しているか否かを判断する。現時点での冗長関節部が可動限界に到達していないと判断するとステップb6に進み、現時点での冗長関節部が可動限界に到達していると判断するとステップb7に進む。
【0136】
ステップb6では、各関節部9〜12において演算した角度が2とおり求められた場合、以前に、冗長関節部以外の各関節部の角度を求めるために用いた離散形態符号を用いて、2つの角度のうちから1つの角度を選択する。たとえば前回角度がプラスの離散形態符号を用いて決定される場合には、ステップb6では、プラスの離散形態符号を用いた場合に得られる角度を関節部が移動すべき角度として選択する。このように関節部が取り得る2とおりの角度のうちから1つの角度を選択するとステップb8に進む。
【0137】
ステップb7では、各関節部9〜12において演算した角度が2とおり求められた場合、以前に、冗長関節部以外の各関節部の角度を求めるために用いた離散形態符号を反転した離散形態符号を用いて、2つの角度のうちから1つの角度を選択する。たとえば前回角度がプラスの離散形態符号を用いて決定される場合には、ステップb7では、マイナスの離散形態符号を用いた場合に得られる角度を関節部が移動すべき角度として選択する。このように関節部が取り得る2とおりの角度のうちから1つの角度を選択するとステップb8に進む。
【0138】
ステップb8では、ステップb6またはステップb7で求めた第1〜第4関節部9〜12の角度q1〜q4を演算結果として、出力部22に与え、ステップb9に進む。出力部22は、与えられた演算結果を多関節ロボット30に与える。これによって多関節ロボット30は、対象点Phの位置を維持した状態で、指定された冗長関節部が冗長角度となるように、各関節部9〜12を角変位する。
【0139】
ステップb9では、中央演算処理部19は、形態変更動作を終了する終了指令が与えられた否かを判断する。たとえば再び形態変更スイッチ45が押下されることで、終了指令が与えられたことを判断してもよい。中央演算処理部19は、形態変更動作を終了する指令が与えられていないと判断すると、ステップb1に戻り、関節部操作スイッチ43が再び押下されるのを待機する。またステップb9で形態変更動作を終了する指令が与えられたことを判断すると、ステップb10に進み、ステップb10で形態変更動作を終了する。
【0140】
図13は、中央演算処理部19が図12に示す形態動作の手順を行った場合における第1〜第4関節部9〜12の角度q1〜q4を示すグラフである。図13(1)は、第1関節部9の角度q1の時間変化を示し、図13(2)は、第2関節部10の角度q2の時間変化を示し、図13(3)は、第3関節部11の角度q3の時間変化を示し、図13(4)は、第4関節部12の角度q4の時間変化を示す。
【0141】
対象点Phの位置を決定した状態で、操作者が入力部20を操作して、第1関節部9に対応するマイナス方向移動スイッチを一度押下する。この場合、中央演算処理部19は、第1関節部9を冗長関節部として決定する。また中央演算処理部19は、第1関節部9について、マイナス方向移動スイッチ押下前の角度から予め定める角変位量で、マイナス方向に角変位した位置を冗長角度として決定する。
【0142】
図12に示す形態動作の手順を行う場合、マイナス方向移動スイッチが順次押下されると、中央演算処理部19は、ステップb1からステップb6、ステップb8を経由してステップb9に達する動作を行う。これによって中央演算処理部19は、対象点Phの位置を保ちつつ、図13(1)に示すように、第1関節部9の角度q1を時間経過とともに小さくさせるとともに、図13(2)〜(4)に示すように、第2関節部10〜第4関節部12の角度q2〜q4をそれぞれ変化させる。
【0143】
マイナス方向移動スイッチが押下され続けると、第1関節部9の角度q1が可動限界となる角度(図13では0度)に達する。この場合、中央演算処理部19は、ステップb5〜b8を経由せずに、ステップb9に達する動作を行う。これによって中央演算処理部19は、関節部の角変位指令を演算せず、各関節部9〜12の角変位を停止した状態で維持させる。可動限界に達した第1時期t1を経過後、さらにマイナス方向移動スイッチが押下されても、各関節部9〜12の角度は、可動限界に達した位置を維持する。
【0144】
また第1時期t1より後となる第2時期t2で、第1関節部9に対応するプラス方向移動スイッチが押下されると、中央演算処理部19は、第1関節部9について、プラス方向移動スイッチ押下前の角度から予め定める角変位量で、プラス方向に角変位した位置を冗長角度として決定する。これによって第1関節部9の角度q1が可動限界から脱出する。第2時期t2では、中央演算処理部19は、ステップb1からステップb7、ステップb8を経由してステップb9に達する動作を行う。この場合、中央演算処理部19は、第1時期t1以前に、第4関節部12の角度q4を求めるために用いた離散形態符号を反転して、第4関節部12の角度q4を求める。
【0145】
第2時期t2より後で、プラス方向移動スイッチが押下されつづけると、中央演算処理部19は、ステップb1からステップb6、ステップb8を経由してステップb9に達する動作を行う。この場合、中央演算処理部19は、第2時期t2で用いた離散形態符号を用いて、第4関節部12の角度q4を求める。これによって中央演算処理部19は、対象点Phの位置を保ちつつ、図13(1)に示すように、第1関節部9の角度q1を時間経過とともに大きくさせるとともに、図13(2)〜(4)に示すように、第2関節部10〜第4関節部12の角度q2〜q4をそれぞれ変化させる。
【0146】
このように第1時期t1より前と、第2時期より後とでは、各関節部9〜12の角度q1〜q4を求めるのに用いた離散形態符号が異なる。したがって1つの冗長関節部を選択するだけで、対象点Phを固定状態に維持したうえで、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる。言換えると、多関節ロボット30が回転揺動状態であって、揺動角変位する関節部が選択された場合であっても、第1〜第4関節部9〜12が取り得ることが可能な全ての角度の組合せを演算可能となる。したがって操作者は、4つの関節部9〜12のうちの1つを選択するだけでよく、全てを冗長関節部として選択可能とする必要がなく、形態変更動作における入力部20の操作性を向上することができる。
【0147】
また上述する形態変更動作を中央演算処理部19が行う場合には、関節部操作スイッチ43を、1つのスイッチとすることができる。この場合、形態変更動作に必要なスイッチを減らすことができ、スイッチとして必要な部品点数を減らすことができる。また各関節部の角度を演算するのに必要な計算プログラムを1つにすることができる。また第1関節部を冗長関節部として選択可能とすることによって、計算プログラムをより簡単化することができる。
【0148】
図14は、形態変更動作における中央演算処理部19の予測報知動作の手順を説明するためのフローチャートである。本実施の形態では、形態変更動作にあたって、中央演算処理部19は、冗長関節部を決定した場合に、残余の関節部が角変位する方向を報知するとともに、各アーム体の移動量および移動方向を報知する予測報知動作を行うことができる。予測報知動作は、図3および図12に示す形態変更動作のいずれにも併用して行うことができる。したがって中央演算処理部19は、複数の関節部の角変位する方向を予測して報知する予測報知手段となる。
【0149】
まずステップc0で、中央演算処理部19は、対象点Phの位置が決定された状態で、予測報知動作の動作指令が与えられたことを判断すると、ステップc1に進み、予測報知動作を開始する。ステップc1では、中央演算処理部19は、移動すべき冗長角度の移動量を決定し、ステップc2に進む。以下、移動量は、移動方向も含むものとする。たとえば、たとえば関節部操作スイッチ43のうち、プラス方向移動スイッチまたはマイナス方向移動スイッチが直前に押下されていると、同じ移動スイッチが次に押下された場合に冗長関節部が角変位する移動量と移動方向とを決定する。
【0150】
ステップc2では、中央演算処理部19は、対象点Phの移動量と、各関節部9〜12の角度q1〜q4との関係を示す第1ヤコビ行列Jhに基づいて、対象点Phが固定位置となる場合の各関節部9〜12の角度q1〜q4の移動方向を決定する。具体的には、中央演算処理部19は、第1ヤコビ行列Jhを変形した行列式に、ステップc1で求めた移動量とを代入することによって、残余の角度の移動量を求めることができる。このようにして冗長関節部の移動量を決定した場合において、残余の角度の移動量を演算すると、ステップc3に進む。
【0151】
ステップc3では、中央演算処理部19は、設定されるアーム体の移動量と、各関節部9〜12の角度q1〜q4との関係を示す第2ヤコビ行列Jeに基づいて、対象点Phが固定位置となる場合の各関節部9〜12の角度q1〜q4の移動量とを決定する。具体的には、中央演算処理部19は、第2ヤコビ行列Jeに基づく行列式に、ステップc1およびc2で求めた各関節部9〜12の角度q1〜q4を代入することによって、設定されるアーム体の移動量を求めることができる。このようにして冗長関節部の移動量を決定した場合において、設定されるアーム体の移動量を演算すると、ステップc4に進む。
【0152】
ステップc4では、中央演算処理部19は、ステップc2およびステップc3で演算した演算結果を入力部20の表示部42によって報知し、ステップc1に戻る。中央演算処理部19は、予め定める終了条件を満足するまで、ステップc1〜c4を繰返し、終了条件を満足すると、予測報知動作を終了する。たとえば中央演算処理部19は、形態変更動作を終了するとともに予測報知動作を終了する。
【0153】
このように予測報知動作を行うことによって、形態変更動作において、冗長関節部をプラスまたはマイナス方向に角変位した場合に、残余の関節部がプラスまたはマイナス方向のいずれに角変位するかを報知することができ、操作性を向上することができる。
【0154】
同様に設定されるアーム体が、プラスまたはマイナス方向のいずれに角変位するかを報知することができ、操作性を向上することができる。たとえば設定されるアーム体は、第1〜第4アーム体3〜6の全てであってもよく、また操作者が注目したい任意のアーム体であってもよい。本実施の形態では、第3アーム体5と第4アーム体6との間の部分となる肘位置p2の移動量を報知することによって、中央演算処理部19は、多関節ロボット30のおおよその形態を把握させることができる。
【0155】
操作者は、報知された情報を確認することで、各関節部および設定されるアーム体が動くであろう方向を容易に把握することができ、利便性を向上することができる。たとえば、形態変更動作を行って、障害物に衝突することを回避する形態に多関節ロボットを変形させる場合、ロボットの形態変化を予測することができるので、操作者は、ロボットが障害物に衝突することをより確実に防ぐことができる。
【0156】
また上述した予測報知動作における演算は、ヤコビ行列Jh,Jeに基づいて行われる。したがって演算結果で有る各関節部9〜12の移動量および設定されるアーム体の移動量は、解析解でなく近似解となる。しかしながら報知情報は、厳密な情報が要求されず、短時間および容易に演算されることが望まれる。本実施の形態に示すように、ヤコビ行列に基づいて、予測報知に用いる演算結果を求めることで、短時間でかつ容易に演算することができる。
【0157】
次に、ステップc2における各関節部9〜12の移動量演算工程の詳細について説明する。上述した(1)式に基づいて、対象点Phの移動量を求める第1ヤコビ行列Jhは、次の(5)式によって表わされる。
【0158】
【数5】
【0159】
ここで、phx、phyおよびphzは、(1)式で定義した関数であり、Jhは、第1ヤコビ行列を示す。
【0160】
また第4アーム体5と第3アーム体6との間の肘位置p2を求める第2ヤコビ行列Jeは、(5)式に示す第1ヤコビ行列Jhのphx、phyおよびphzの関数に、L4=ゼロを代入した行列によって表わされる。
【0161】
対象点Phの微小移動量Δphと、各関節部の微小移動量Δqとの間には、次の(6)式の関係を有する。
【0162】
【数6】
【0163】
形態変更動作では、対象点Phの位置が固定されるので、(6)式の左辺は、ゼロである。これによって(6)式は、次の(7)式となる。
【0164】
【数7】
【0165】
ここで、Δq1は、第1関節部9の微小移動量Δq1である。この微小移動量Δq1は、ステップc1で決定される。たとえば関節部を移動する速度が予め決定されている場合、その速度に制御周期をかけた値となる。第1関節部9をプラス方向に移動させる場合にはΔq1が正となり、マイナス方向に移動させる場合には、Δq1が負となる。
【0166】
(7)式に、第1関節部9の微小移動量Δq1を代入することによって、第2〜第4関節部10〜12の微小移動量Δq2〜q4を求めることができる。中央演算処理部19は、(7)式を記憶する。ステップc2において、(7)式に冗長関節部の移動量Δq1を代入することによって、冗長関節部である第1関節部以外の各関節部10〜12の微小移動量Δq2〜Δq4を求めることができる。求めた各関節部の移動方向の正負を表示部42に表示させることによって、操作者は、冗長関節部の移動方向を選択した場合に残余の関節部が移動する移動方向を把握することができる。
【0167】
次に、ステップc3における各関節部9〜12の移動量演算工程の詳細について説明する。肘位置p2の微小移動量Δpeと、各関節部の微小移動量Δqとには、次の(8)式に示す関係がある。
【0168】
【数8】
【0169】
ここで、ΔPeは肘位置p2の微小移動量ΔPeであり、Jeは、上述した第2ヤコビ行列Jeである。(8)式に、(7)式で求めた各関節部10〜12の微小移動量Δq2〜q4と、冗長関節部である第1関節部9の微小移動量Δq1を代入することによって、肘位置p2の微小移動量ΔPeを求めることができる。
【0170】
中央演算処理部19は、(8)式を記憶する。ステップc3において、(8)式に冗長関節部の移動量Δq1、ステップc1およびステップc2で求めた各関節部9〜12の微小移動量Δp1〜Δp4を代入することによって、肘位置p2の微小移動量ΔPeを求めることができる。求めた肘位置p2の移動方向の正負を表示部42に表示させることによって、操作者は、冗長関節部の移動方向を選択した場合に肘位置p2が移動する移動方向たとえば基台17を基準として、上下左右に移動する方向を把握することができる。
【0171】
図15は、本発明の第2実施形態である多関節ロボット1の制御装置2を示すブロック図である。図15に示す多関節ロボット1および制御装置2は、図1に示す多関節ロボット30および制御装置2と類似した構成を示し、類似した構成については同様の参照符号を付して、説明を省略する。
【0172】
図15に示す多関節ロボット1は、7自由度を有する冗長ロボットであり、6つのアーム体3〜8と、隣接する2つのアーム体を回転自在に連結する7つの関節部9〜15とを有する。各アーム体3〜8は、それぞれ連結されて直列状に延びるアーム構成体16を構成する。また多関節ロボット1は、アーム構成体16の基端部に連結される基台17と、アーム構成体16の先端部に連結されるエンドエフェクタ18とを有する。
【0173】
各関節部9〜15は、同軸関節部と傾斜関節部とのうちのいずれかである。具体的には、第1関節部9、第5関節部13および第7関節部15は、同軸関節部であり、第2関節部10、第3関節部11、第4関節部12および第6関節部14は、傾斜関節部である。したがって第1〜第4アーム体3〜6および、第1〜第4関節部9〜12は、図1に示す多関節ロボット30と同様の構成を有する。
【0174】
第4アーム体6の一端部は、第4関節部12によって、第3アーム体5の軸線A5および第4アーム体6の軸線A6に対して所定の角度、本実施の形態では45度で傾斜する第4回転軸線A12まわりに回転自在に、第3アーム体5の他端部に連結される。第5アーム体7の一端部は、第5関節部13によって、第4アーム体6の軸線A6および第5アーム体7の軸線A7と同軸の第5回転軸線A13まわりに回転自在に、第4アーム体6の他端部に連結される。
【0175】
アーム構成体16の他端部である第6アーム体8の一端部は、第6関節部14によって、第5アーム体7の軸線A7および第6アーム体8の軸線A8に対して所定の角度、本実施の形態では45度で傾斜する第6回転軸線A14まわりに回転自在に、第5アーム体7の他端部に連結される。多関節ロボット1の先端部となるエンドエフェクタ18は、第6アーム体8の他端部に、第7関節部15によって、第6アーム体8の軸線A8と同軸の第7回転軸線A15まわりに回転自在に連結されている。エンドエフェクタ18は、多関節ロボット1のうちで、位置および姿勢を制御すべき設定部分となる。
【0176】
このように各関節部9〜15は、アーム体を介して互いに角変位可能に連結される。また各関節部9〜15が角変位することで、アーム構成体16は、図15に示すように、アーム体のそれぞれの軸線が同軸に配置されて一直線状に延びる状態に変形可能に構成される。各アーム体3〜6の軸線A3〜A6が一直線状に延びた状態では、第2回転軸線A10と第3回転軸線A11とは平行となり、第3回転軸線A11と第4回転軸線A12とは垂直となる。また第4アーム体6に設定される対象点Phは、アーム構成体16の先端部に配置され、基台17は、アーム構成体16の他端部に配置される。
【0177】
また直列方向一端部から直列方向他端部に向かって並ぶ3つの関節部のそれぞれの回転軸線、すなわち第5関節部13の回転軸線A6と、第6関節部14の回転軸線A7と、第7関節部15の回転軸線A8とは、予め定められる一点となる3軸交点Pで交差する。このように第5〜第7関節部13〜15は、3軸交点Pを構成する3つの3軸交点形成関節部となる。
【0178】
多関節ロボット1は7自由度を有し、各関節部9〜15を回転させることで、エンドエフェクタ18を任意の位置および姿勢に位置決め可能である。多関節ロボット1は、設備等が複雑に入り組んで作業経路が複雑な場合や、他のロボットと近接して並列に配置されて動作範囲を大きくすることができない場合に好適に用いられる。エンドエフェクタ18は、たとえば溶接トーチおよびハンドリング装置などの手先装置であってもよい。各アーム体3〜8を回転させる回転機構などは、図1に示す多関節ロボット30と同様の構成を有する。また制御装置2についても、その構成は、図1に示す制御装置2と同様の構成を有する。
【0179】
中央演算処理部19は、予め記憶される動作プログラムに従って、入力部20から与えられる各種情報に応答して、多関節ロボット1を制御するために必要な演算を行う。本実施の形態では、中央演算処理部19は、エンドエフェクタ18を予め定める目標位置および目標姿勢に移動させるのに必要な、各関節部9〜15の角度q1〜q7をそれぞれ演算する。
【0180】
制御装置2は、エンドエフェクタ18を予め指定される移動経路に沿って移動させるにあたって、回転すべき各関節部9〜15の角度q1〜q7を演算する。この移動経路は、ロボット動作前に、操作者によって入力部20から中央演算処理部19に入力される。本発明の制御装置2に制御される多関節ロボット1は、7つの関節部9〜15を有することで、冗長性を有する。すなわち、基台17に対して、エンドエフェクタ18を予め定める位置および姿勢で固定した状態で、各関節部9〜15の取り得る角度q1〜q7の組合せが複数通り存在する。
【0181】
本実施形態の制御装置2は、上述した形態変更動作を行うことができる。形態変更動作を行うにあたって、制御装置2は、複数の関節部9〜15のうちから選択された冗長関節部と、その冗長関節部の冗長角度と、エンドエフェクタ18を固定すべき位置および姿勢とが入力部20を介して与えられる。中央演算処理部19は、エンドエフェクタ18を固定すべき固定状態に固定したうえで、冗長関節部が冗長角度となる場合の、残余の関節部の角度を演算する演算手段となる。出力部22は、中央演算処理部19の演算結果を多関節ロボットに与える。演算結果が与えられた多関節ロボットは、冗長関節部を冗長角度とするとともに、残余の関節部の角度を演算結果に応じて位置合わせして、エンドエフェクタ18を指定された固定状態とする。
【0182】
このように形態変更動作を行うことによって、エンドエフェクタ18を固定した状態で、アーム構成体16の形状および姿勢などの形態を変更させることができる。操作者は、ロボットを動作させながらロボットの形態を確認することによって、エンドエフェクタ18を固定した状態で、障害物との衝突を回避するアーム構成体16の形態を実現する各関節部9〜15の角度を容易に決定することができる。
【0183】
図16は、図12に示す多関節ロボットを制御する制御装置2の入力部20を示す正面図である。入力部20の構成は、図2と同様の構成を有する。具体的には、入力部20は、操作部41と、表示部42とを含む。操作部41には、各関節部操作スイッチ43と、ツール移動スイッチ44と、形態変更スイッチ45とを含む。また操作部41は、ロボットの動作プログラムを入力するための命令キー、カーソルキーおよびテンキーなどをさらに含む。
【0184】
関節部操作スイッチ43は、各関節部9〜15の角度q1〜q7を入力するためのスイッチであって、各関節部9〜15について、プラス方向およびマイナス方向への予め定める角変位量の角変位指令が与えられる。ツール移動スイッチ44は、ベース座標軸またはツール座標軸に従ってエンドエフェクタ18の移動指令が与えられる。また形態変更スイッチ45は、対象点Phを予め定める位置に固定したまま、アーム構成体16の形態を変化させるための形態変更指令が与えられる。
【0185】
形態変更動作を行う場合、ツール移動スイッチ44は、エンドエフェクタ18を固定すべき位置および姿勢が入力される固定条件入力手段となる。また形態変更スイッチ45と関節部操作スイッチ43とによって、冗長関節部が入力される冗長関節部入力手段と、冗長関節部の角度である冗長角度が入力される冗長角度入力手段とが構成される。
【0186】
図17は、第2実施形態における中央演算処理部19の形態変更動作の手順を説明するためのフローチャートである。まずステップd0で、中央演算処理部19は、多関節ロボット1を制御可能な状態で、形態変更スイッチ45が押下されたことを判断すると、ステップd1に進み、形態変更動作を開始する。
【0187】
ステップd1では、中央演算処理部19は、ツール移動スイッチ44によってエンドエフェクタ18の移動指令が与えられると、エンドエフェクタ18を指定された位置および姿勢に移動するための演算結果を出力部22に与える。またツール移動スイッチ44による移動指令がなければ、現時点のエンドエフェクタ18の位置および姿勢を維持させる。中央演算処理部19は、関節部操作スイッチ43が押下されたと判断すると、その時点のエンドエフェクタ18の位置および姿勢を固定すべき固定条件として決定し、ステップd2に進む。
【0188】
ステップd2では、中央演算処理部19は、関節部操作スイッチ43によって指定された関節部を、冗長関節部として決定する。冗長関節部は、第1〜第4関節部9〜12のいずれか1つとなる。また関節部操作スイッチ43によって指定された角度を、冗長角度として決定する。このように冗長関節部および冗長角度を決定するとステップd3に進む。
【0189】
ステップd3では、中央演算処理部19は、ステップd1で決定したエンドエフェクタ18の位置および姿勢に基づいて、3軸交点Pの座標を演算する。3軸交点Pは、第5関節部13、第6関節部14、第7関節部15の各回転軸線A13,A14,A15が一点で交わる点であるので、エンドエフェクタ18の位置および姿勢が決定されると、一意的に求めることができる。このように中央演算処理部19は、3軸交点Pを演算する3軸交点演算手段となり、3軸交点Pを求めるとステップd4に進む。
【0190】
ステップd4では、中央演算処理部19は、ステップd3で演算した3軸交点Pの3次元位置に、3軸交点Pが位置する場合における、第1〜第4関節部9〜12の角度q1〜q4を求める。具体的には、第1〜第4関節部9〜12のそれぞれの角度q1〜q4を変数として、3軸交点Pの座標を表わす順変換関係式と、ステップd2で決定した冗長関節部の冗長角度と、ステップd3で演算した3軸交点Pの座標とに基づくことによって、第1〜第4関節部9〜12の角度q1〜q4を解析的に求めることができる。第1〜第4関節部9〜12の角度q1〜q4を解析的に求める場合、対象点Phの座標を3軸交点Pの座標に置き変えることで、上述するステップa3と同様の方法を用いて求めることができる。
【0191】
したがって中央演算処理部19は、冗長関節部の冗長角度と、3軸交点Pのx座標、y座標、z座標を代入することで、第1〜第4関節部9〜12のうちで冗長関節部以外の各関節部の角度を解析的に求めることが可能な方程式を表わす計算プログラムを記憶している。中央演算処理部19は、第1〜第4関節部9〜12の計算プログラムにしたがって、第1〜第4関節部9〜12のうちで、冗長関節部以外の各関節部の角度を解析的に演算し、ステップd5に進む。このように中央演算処理部19は、注目する第1〜第4関節部9〜12の角度q1〜q4をそれぞれ演算する注目関節部角度演算手段となる。
【0192】
ステップd5では、中央演算処理部19は、ステップd2で決定された冗長関節部の冗長角度が可動限界に到達していないことを判断するとステップd6に進み、可動限界に到達しているか、または可動限界を超えているかを判断すると、ステップd8に進む。
【0193】
ステップd6では、中央演算処理部19は、エンドエフェクタ18が固定位置および固定姿勢となる、第5〜第7関節部13〜15の角度q5〜q7を求める。ステップd4で求めた第1〜第4関節部9〜12の角度q1〜q4から、第4アーム体6の姿勢を求めることができる。第4アーム体6の姿勢が決定された状態では、エンドエフェクタ18が予め定める位置および姿勢となる場合の第5〜第7関節部13〜15の角度q5〜q7は、一意に決定することができる。
【0194】
具体的には、中央演算処理部19は、第1〜第4関節部9〜12の角度q1〜q4の角度と、エンドエフェクタ18の位置および姿勢と用いて、第5〜第7関節部13〜15の角度を解析的に求めることが可能な逆変換方程式を表わす第5〜第7関節部計算プログラムを記憶している。
【0195】
中央演算処理部19は、第5〜第7関節部計算プログラムにしたがって、第5〜第7関節部13〜15の角度q5〜q7をそれぞれ求める。このように中央演算処理部19は、第5〜第7関節部13〜15の角度q5〜q7をそれぞれ演算する交点形成関節部角度演算手段となり、各角度を求めるとステップd7に進む。
【0196】
ステップd7では、ステップd4およびステップd6で得られた第1〜第7関節部9〜15の角度q1〜q7を演算結果として、出力部22に与える。出力部22は、与えられた演算結果を多関節ロボット1に与える。これによって多関節ロボット1は、各関節部9〜15が角変位し、エンドエフェクタ18を固定位置および固定姿勢に維持した状態で、冗長関節部が冗長角度となる。このとき、第1〜第7関節部9〜15の角度q1〜q7は、収束演算などの近似法とは異なり、解析的な演算方法によって求められるので、冗長関節部の移動前と移動後とで、固定位置および固定姿勢からエンドエフェクタ18がずれることがない。このように演算結果を出力部22に与えて、多関節ロボット1を動作させると、ステップd8に進む。
【0197】
ステップd8では、中央演算処理部19は、形態変更動作を終了する終了条件を満足するか否かを判断する。たとえば形態変更解除スイッチが押下されることで、終了条件を満足することを判断してもよい。中央演算処理部19は、終了条件を満足していないと判断すると、ステップd1に戻り、関節部操作スイッチ43が押下されるのを待機する。この状態で、関節部操作スイッチ43が再度押下されると、その指示に従ってステップd1〜d8を繰返す。そしてステップa7で形態変更動作を終了する指令が与えられたことを判断すると、ステップa8に進み形態変更動作を終了する。
【0198】
このようにして、中央演算処理部19が形態変更動作を行うことによって、エンドエフェクタ18の位置および姿勢を固定したまま、各関節部9〜15を角変位させて、多関節ロボット1の形態を変更させることができる。したがって第1実施形態と同様の効果を得ることができる。
【0199】
また図15に示す多関節ロボット1は、第5〜第7関節部13〜15のうちの1つが冗長関節部として選択されてしまうと、解析的に求めることができない。これに対して、本実施の形態では、第1〜第4関節部9〜12のいずれかを冗長関節部として選択することによって、解析解を得ることができる。また7つの関節部9〜15から1つを選択するよりも、4つの関節部9〜12から1つを選択するほうが、操作性を向上することができる。
【0200】
また図16に示すように、各関節部操作スイッチ43を用いて、冗長関節部を決定する場合、形態変更スイッチ45が押下された状態では、第1関節部〜第4関節部9〜12の選択スイッチを有効とし、第5〜第7関節部13〜15の選択スイッチが無効とする。これによって冗長関節部としては、第1〜第4関節部9〜12のいずれかについて選択可能となり、誤動作を防ぐことができる。
【0201】
また本実施の形態では、第5〜第7関節部13〜15で三軸交点を構成したが、これに限らない。たとえば第1〜第3関節部9〜11で3軸交点を構成した場合であっても、同様に解析解を求めることができる。この場合には、第4〜第7関節部12〜15のうちから1つの冗長関節部が選択される。そしてエンドエフェクタ18に設定される座標系から、第1関節部を見た座標系に反転した座標系に変換することによって、同様に解析的に求めることができる。
【0202】
すなわち、アーム体を介して互いに角変位可能に連結されて直列に並ぶ7つの関節部を含み、7つの関節部9〜15のうちで直列方向一端部から直列方向他端部に向かって並ぶ3つの関節部の回転軸線が一点で交差する3軸交点を形成する3つの交点形成関節部を有する7自由度の多関節ロボットにおいて、アーム構成体の一端部に対する他端部の位置および姿勢が固定条件として入力される場合に、3つの交点形成関節部以外となる4つの注目する関節部のうち1つが、冗長関節部として入力されることで、形態変更動作において各関節部9〜15の角度を解析的に求めることができる。
【0203】
また上述したように、第2実施形態であっても第1および第2関節部9,10のいずれか1つと、第3および第4関節部11,12のいずれか1つを冗長関節部として選択させるようにしてもよい。また図12に示す形態変更動作を行わせてもよく、移動予測動作を行わせてもよい。移動予測動作と、形態変更動作とは、エンドエフェクタ18を移動経路に沿って移動させて移動経路を教示する移動教示動作と併用して行ってもよい。
【0204】
図18は、移動予測動作と、形態変更動作と、移動教示動作とを行う場合における中央演算処理部19の動作手順を説明するためのフローチャートである。まずステップe0で、中央演算処理部19は、多関節ロボット1を制御可能な状態で、入力部20のツール移動スイッチ44によって、エンドエフェクタ18の移動指令が与えられると、ステップe1に進む。
【0205】
ステップe1では、中央演算処理部19は、ツール移動スイッチ44に与えられた情報に従って、エンドエフェクタ18を移動させる移動経路を演算する。エンドエフェクタ18が指定された移動経路に従って移動するにあたって、各関節部の角度の組合せは、複数存在する場合がある。この場合、中央演算処理部19は、予め定める規則にしたがって複数の角度の組合せから1つを選択する。たとえばエンドエフェクタ18側の関節部の角変位が少なくなるような角度の組合せを選択する。中央演算処理部19は、演算結果を出力部22を介して多関節ロボット1に与え、ステップe2に進む。これによって多関節ロボット1は、エンドエフェクタ18を指定された移動経路に沿って移動させる。
【0206】
ステップe2では、第1〜第4関節部9〜12のいずれかが特異点近傍の警告範囲に到達するか否かを判断する。たとえば中央演算処理部19は、第1〜第4関節部9〜12の角度q1〜q4を、3軸交点位置に変換するヤコビ行列の行列式の絶対値が、予め定める設定値以下であると判定されると、第1〜第4関節部9〜12の角度q1〜q4のいずれかが、特異点近傍の警告領域に到達したことを判断する。
【0207】
ステップe2において、第1〜第4関節部9〜12のいずれかが警告範囲に到達したことを判断するとステップe3に進み、そうでないとステップe6に進む。ステップe3では、図14に示す予測報知動作を行う。図19は、入力部20の表示部42に表示される表示画面を示す図である。たとえば図19に示すように、中央演算処理部19は、可動限界に近づいている冗長関節部を表示させる。また中央演算処理部19は、可動限界に近づいている関節部が、特異点から脱出する角変位方向と、脱出する方向に移動したときの肘位置p2が動く方向を表示させる。このように予測情報を表示させると、ステップe4に進む。
【0208】
ステップe4では、中央演算処理部19は、形態変更指令が与えられたか否かを判断する。形態変更が与えられたと判断するとステップe5に進み、そうでないとステップe6に進む。ステップe5では、エンドエフェクタ18の移動を停止して、図3または図12に示す形態変更動作を開始し、形態変形動作が終了したことを判断するとステップe6に進む。
【0209】
ステップe6では、中央演算処理部19は、移動動作を終了する終了条件を満足するか否かを判断する。中央演算処理部19は、移動動作の終了条件を満足していないと判断すると、ステップe1に戻り、ツール移動スイッチ44が再び押下されるのを待機する。この状態で、ツール移動スイッチ44が再度押下されると、その指示に従ってステップe11〜e6を順に行う。またステップe6で終了条件を満足することを判断すると、ステップe7に進み、ステップe7で移動動作を終了する。
【0210】
これによって操作者は、移動動作を行う途中に、エンドエフェクタ18の位置を固定したまま、ロボットの形態を変更して、冗長関節部を可動限界から離して、再び、移動指令を与えた方向にエンドエフェクタ18を移動させることができる。このように移動動作に、中央演算処理部19による予測動作と形態変更動作とを行わせることによって、利便性を向上することができる。
【0211】
上述した本実施の形態は、発明の例示に過ぎず、発明の範囲内において構成を変更することができる。たとえば本実施の形態では、オンラインティーチング、すなわちロボットを動作させながら各関節部の角度を求めて、ロボットの動作計画の教示に用いたが、これに限らない。したがって中央演算処理部19のうち、各関節部の角度を求めるプログラムを実行するコンピュータも本発明に属する。コンピュータに、ロボットの関節の種類、アーム体の長さなどの条件を入力した状態で、対象点Phおよびエンドエフェクタ18を予め定める固定状態に保って、冗長関節部が冗長角度となる各関節部の角度を解析的に求めることができ、この解析結果をロボットに与えてもよい。すなわち本発明の方法をオフラインティーチングやロボットの動作シミュレーションに用いることも可能である。また本実施の形態では、(1)式に示す対象点の位置を求めるための関係式を変形して、連立方程式を解いた計算式に基づいて、各関節部の角度を求めたが、対象点の姿勢を求めるための関係式、対象点の位置の一部および姿勢の一部を求めるための関係式を変形して、連立方程式を解いた計算式に基づいて、各関節部の角度を求めてもよい。
【0212】
また本実施の形態では、傾斜関節部と同軸関節部とを有する多関節ロボットを用いて説明したが、冗長性を有する他のロボットであっても同様に各関節部の角度を解析的に求めることができる。たとえば図6に示す4関節ロボットであっても同様に各関節部の角度を求めることができ、設定部分の固定についても、位置のほか姿勢が固定されてもよい。
【0213】
また本実施の形態では、1軸冗長ロボットについて説明したが、2軸以上の冗長性を有するロボットであっても同様に求めることができる。この場合、2つ以上の冗長関節部が選択されることで、解析的に各関節部の解を求めることができる。すなわち、注目する複数の関節部のうちで冗長関節部を除く残余の関節部の数が、設定部分を予め定める固定条件に従って拘束される自由度の数と一致するように、注目する複数の関節部の少なくとも1つが冗長関節部として入力されることで、連立方程式を解くことができ、解析的に各関節部の角度を求めることができる。
【0214】
すなわち注目する複数(N)の関節部のうちで、入力された冗長関節部の数(n)を除いた数(N−n)が、設定部分が拘束される自由度の数(M)と一致する必要がある。この場合、順変換関係式に含まれる変数を未知数とすると、設定部分が拘束される自由度の成分をそれぞれ解とする順変換関係式の数(M)と、順変換関係式に含まれる未知数の数(N−n)とが等しくなる。これらの複数の順変換関係式を連立して解くことによって、各変数の値、すなわち残余の関節部の各角度を解析的に求めることができる。たとえば2軸冗長ロボットにおいて、オンラインティーチングを行う場合、2つの関節部を冗長関節部として決定し、そのうち第1冗長関節部を任意の角度に固定し、第2冗長関節部を変化させる。これによって、上述した演算方法と同様の演算方法を適用することができ、第2冗長角度を変化した場合に各関節部が取り得る角度の組合せの解析解を順次求めることができる。
【符号の説明】
【0215】
1 多関節ロボット
2 制御装置
3 第1アーム体
4 第2アーム体
5 第3アーム体
6 第4アーム体
9 第1関節部
10 第2関節部
11 第3関節部
12 第4関節部
19 中央演算処理部
20 入力部
22 出力部
ph 対象点
【特許請求の範囲】
【請求項1】
アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットの制御装置であって、
前記設定部分を固定すべき固定条件が入力される固定条件入力手段と、
冗長関節部が入力される冗長関節部入力手段であって、前記複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つが冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
固定条件入力手段によって入力される固定条件と、冗長角度入力手段によって入力される冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき複数の関節部の角度をそれぞれ演算する角度演算手段と、
前記角度演算手段で演算された演算結果を多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置。
【請求項2】
固定条件として、直列方向他端部の関節部に対する設定部分の3つの自由度が拘束される条件が入力され、
冗長関節部入力手段は、前記複数の関節部のうちで、直列方向一端部から直列方向他端部に向かって並ぶ2つの関節部のうちの1つの関節部と、直列方向他端部から直列方向一端部に向かって並ぶ2つの関節部のうちの1つの関節部とを冗長関節部として選択可能に構成されることを特徴とする請求項1記載の多関節ロボットの制御装置。
【請求項3】
角度演算手段は、入力前の状態から冗長角度入力手段に入力された冗長角度に冗長関節部を角変位させる間に、冗長関節部が可動限界に到達することを判断した場合、その可動限界に到達した状態の関節部の角度を演算することを特徴とする請求項1または2記載の多関節ロボット。
【請求項4】
角度演算手段は、冗長角度入力手段に入力された冗長角度に冗長関節部が角変位する間に、冗長関節部が可動限界に到達した状態から脱出することを判断した場合、冗長関節部以外の関節部の角度が取り得る2つの組合せのうち、可動限界に到達する前に取り得た角度の組合せと異なる角度の組合せを演算することを特徴とする請求項3記載の多関節ロボットの制御装置。
【請求項5】
前記複数の関節部の角度変化を設定部分の位置変化に変換するヤコビ行列と、冗長角度入力手段に入力される冗長角度とに基づいて、設定部分を固定条件で拘束して、冗長関節部を冗長角度に角変位するときに、複数の関節部の角変位する方向を予測し、予測結果を報知する予測報知手段をさらに備えることを特徴とする請求項1〜4のいずれか1つに記載の多関節ロボットの制御装置。
【請求項6】
前記複数の関節部のいずれかが可動限界近傍に設定される警告範囲に到達したことを判断すると、判断結果を報知する警告報知手段をさらに備えることを特徴とする請求項1〜5のいずれか1つに記載の多関節ロボットの制御装置。
【請求項7】
警告報知手段は、警告範囲に到達した関節部が、可動限界から遠ざかるような角変位方向を報知することを特徴とする請求項6記載の多関節ロボットの制御装置。
【請求項8】
隣接する2つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを含む多関節ロボットを制御することを特徴とする請求項1〜7のいずれか1つに記載の多関節ロボットの制御装置。
【請求項9】
アーム体を介して互いに角変位可能に連結されて直列に並ぶ7つの関節部を含み、7つの関節部のうちで直列方向一端部から直列方向他端部に向かって並ぶ3つの関節部の回転軸線が一点で交差する3軸交点を形成する3つの交点形成関節部を有する7自由度の多関節ロボットの制御装置であって、
7つの関節部のうちで直列方向一端部の関節部に設定される設定部分を固定すべき位置および姿勢が固定条件として入力される固定条件入力手段と、
3つの交点形成関節部以外となる4つの基本関節部のうち1つが、冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
設定部分が固定すべき位置および姿勢となる状態での3軸交点の固定位置を演算する3軸交点位置演算手段と、
3軸交点位置演算手段によって演算される3軸交点の位置と、冗長関節部入力手段によって入力される冗長角度とに基づいて、4つの基本関節部の各角度を変数として3軸交点の位置を求めるための関係式を解いて、3軸交点が固定位置に位置するような、4つの基本関節部の角度をそれぞれ演算する基本関節部角度演算手段と、
注目関節部角度演算手段によって演算された演算結果と、固定条件入力手段によって入力される設定部分の位置および姿勢とに基づいて、3つの交点形成関節部の角度をそれぞれ演算する交点形成関節部角度演算手段と、
前記各角度演算手段で演算された演算結果とを多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置。
【請求項10】
アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットを制御する制御方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と、
角度演算工程に従ってロボットを制御する制御工程とを備えることを特徴とする多関節ロボットの制御方法。
【請求項11】
アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットにおける各関節部の角度を演算する演算方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と備えることを特徴とする多関節ロボットの各関節部の角度の演算方法。
【請求項1】
アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットの制御装置であって、
前記設定部分を固定すべき固定条件が入力される固定条件入力手段と、
冗長関節部が入力される冗長関節部入力手段であって、前記複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つが冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
固定条件入力手段によって入力される固定条件と、冗長角度入力手段によって入力される冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき複数の関節部の角度をそれぞれ演算する角度演算手段と、
前記角度演算手段で演算された演算結果を多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置。
【請求項2】
固定条件として、直列方向他端部の関節部に対する設定部分の3つの自由度が拘束される条件が入力され、
冗長関節部入力手段は、前記複数の関節部のうちで、直列方向一端部から直列方向他端部に向かって並ぶ2つの関節部のうちの1つの関節部と、直列方向他端部から直列方向一端部に向かって並ぶ2つの関節部のうちの1つの関節部とを冗長関節部として選択可能に構成されることを特徴とする請求項1記載の多関節ロボットの制御装置。
【請求項3】
角度演算手段は、入力前の状態から冗長角度入力手段に入力された冗長角度に冗長関節部を角変位させる間に、冗長関節部が可動限界に到達することを判断した場合、その可動限界に到達した状態の関節部の角度を演算することを特徴とする請求項1または2記載の多関節ロボット。
【請求項4】
角度演算手段は、冗長角度入力手段に入力された冗長角度に冗長関節部が角変位する間に、冗長関節部が可動限界に到達した状態から脱出することを判断した場合、冗長関節部以外の関節部の角度が取り得る2つの組合せのうち、可動限界に到達する前に取り得た角度の組合せと異なる角度の組合せを演算することを特徴とする請求項3記載の多関節ロボットの制御装置。
【請求項5】
前記複数の関節部の角度変化を設定部分の位置変化に変換するヤコビ行列と、冗長角度入力手段に入力される冗長角度とに基づいて、設定部分を固定条件で拘束して、冗長関節部を冗長角度に角変位するときに、複数の関節部の角変位する方向を予測し、予測結果を報知する予測報知手段をさらに備えることを特徴とする請求項1〜4のいずれか1つに記載の多関節ロボットの制御装置。
【請求項6】
前記複数の関節部のいずれかが可動限界近傍に設定される警告範囲に到達したことを判断すると、判断結果を報知する警告報知手段をさらに備えることを特徴とする請求項1〜5のいずれか1つに記載の多関節ロボットの制御装置。
【請求項7】
警告報知手段は、警告範囲に到達した関節部が、可動限界から遠ざかるような角変位方向を報知することを特徴とする請求項6記載の多関節ロボットの制御装置。
【請求項8】
隣接する2つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを含む多関節ロボットを制御することを特徴とする請求項1〜7のいずれか1つに記載の多関節ロボットの制御装置。
【請求項9】
アーム体を介して互いに角変位可能に連結されて直列に並ぶ7つの関節部を含み、7つの関節部のうちで直列方向一端部から直列方向他端部に向かって並ぶ3つの関節部の回転軸線が一点で交差する3軸交点を形成する3つの交点形成関節部を有する7自由度の多関節ロボットの制御装置であって、
7つの関節部のうちで直列方向一端部の関節部に設定される設定部分を固定すべき位置および姿勢が固定条件として入力される固定条件入力手段と、
3つの交点形成関節部以外となる4つの基本関節部のうち1つが、冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
設定部分が固定すべき位置および姿勢となる状態での3軸交点の固定位置を演算する3軸交点位置演算手段と、
3軸交点位置演算手段によって演算される3軸交点の位置と、冗長関節部入力手段によって入力される冗長角度とに基づいて、4つの基本関節部の各角度を変数として3軸交点の位置を求めるための関係式を解いて、3軸交点が固定位置に位置するような、4つの基本関節部の角度をそれぞれ演算する基本関節部角度演算手段と、
注目関節部角度演算手段によって演算された演算結果と、固定条件入力手段によって入力される設定部分の位置および姿勢とに基づいて、3つの交点形成関節部の角度をそれぞれ演算する交点形成関節部角度演算手段と、
前記各角度演算手段で演算された演算結果とを多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置。
【請求項10】
アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットを制御する制御方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と、
角度演算工程に従ってロボットを制御する制御工程とを備えることを特徴とする多関節ロボットの制御方法。
【請求項11】
アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットにおける各関節部の角度を演算する演算方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも1つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と備えることを特徴とする多関節ロボットの各関節部の角度の演算方法。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【公開番号】特開2012−192518(P2012−192518A)
【公開日】平成24年10月11日(2012.10.11)
【国際特許分類】
【出願番号】特願2012−156985(P2012−156985)
【出願日】平成24年7月12日(2012.7.12)
【分割の表示】特願2005−332021(P2005−332021)の分割
【原出願日】平成17年11月16日(2005.11.16)
【出願人】(000000974)川崎重工業株式会社 (1,710)
【Fターム(参考)】
【公開日】平成24年10月11日(2012.10.11)
【国際特許分類】
【出願日】平成24年7月12日(2012.7.12)
【分割の表示】特願2005−332021(P2005−332021)の分割
【原出願日】平成17年11月16日(2005.11.16)
【出願人】(000000974)川崎重工業株式会社 (1,710)
【Fターム(参考)】
[ Back to top ]