JP4230196B2 - 位置決め演算方法および位置決め演算装置 - Google Patents
位置決め演算方法および位置決め演算装置 Download PDFInfo
- Publication number
- JP4230196B2 JP4230196B2 JP2002316760A JP2002316760A JP4230196B2 JP 4230196 B2 JP4230196 B2 JP 4230196B2 JP 2002316760 A JP2002316760 A JP 2002316760A JP 2002316760 A JP2002316760 A JP 2002316760A JP 4230196 B2 JP4230196 B2 JP 4230196B2
- Authority
- JP
- Japan
- Prior art keywords
- model
- coordinate
- arm body
- respect
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1653—Programme controls characterised by the control loop parameters identification, estimation, stiffness, accuracy, error analysis
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/39—Robotics, robotics to robotics hand
- G05B2219/39018—Inverse calibration, find exact joint angles for given location in world space
Description
【０００１】
【発明の属する技術分野】
本発明は、複数のアーム体と各アーム体を回転自在に連結する関節部を有する多関節マニピュレータの各アーム体の相対回転角度を求める位置決め演算方法および位置決め演算装置に関する。
【０００２】
【従来の技術】
従来の技術として、傾斜関節部によって２つのアーム体を連結する多関節マニピュレータが提案されている。傾斜関節部は、連結される２つのアーム体に関して、一方のアームを他方のアームに対して円錐回転自在に連結する。円錐回転するアーム体は、各アーム体の軸線に所定角度傾斜した回転軸線まわりに回転する（たとえば、特許文献１参照〜特許文献３参照）。
【０００３】
【特許文献１】
特開昭６２−１４８１８２号公報
【特許文献２】
特開昭５６−１６３６２４号公報
【特開昭文献３】
特開２００１−１３８２７９号公報
【０００４】
【発明が解決しようとする課題】
たとえば多関節マニピュレータは、直列に設けられる複数のアーム体と、隣接する２つのアーム体を回転自在に連結する複数の関節部とを有する。各関節部は、傾斜関節部と同軸関節部とを有する。同軸関節部は、隣接する２つのアーム体を、その各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する。また傾斜関節部は、アーム体の軸線に対して傾斜する回転軸線まわりに円錐回転自在に連結する。
【０００５】
多関節マニピュレータは、先端部に連結される手先装置を予め定める経路に沿って移動させる。多関節マニピュレータによって手先装置を移動させるためには、手先装置の運動を各アーム体の回転運動に変換する必要がある。すなわち手先装置が配置される位置および姿勢に基づいて、各アーム体の回転角度を求める逆変換を行う必要がある。
【０００６】
特許文献２に開示される従来の技術では、同軸関節部、傾斜関節部、同軸関節部、傾斜関節部、同軸関節部、傾斜関節部、同軸関節部の順に並んで各アーム体を連結する７つの関節部を有する多関節マニピュレータでなければ逆変換を行うことができない。すなわち７つ未満の関節部を有する多関節マニピュレータでは、逆変換して解析解を得ることができない。したがって６つ以下の関節部を有する多関節マニピュレータは、存在しなかった。
【０００７】
仮に６つの関節部を有する多関節マニピュレータが存在したとしても、逆変換するためには複雑な連立方程式を解く必要がある。この複雑な方程式を求める場合には、数式処理プログラムを実行するコンピュータを用いても、その解析解の導出が困難である。したがって６つ以下の関節部を有する多関節マニピュレータの場合には、収束演算法、たとえばニュートンラプソン法などによって各アーム体の回転角度を求めなければならない。このように収束演算によって各アーム体の回転角度を求める場合には、各アーム体の回転角度を短時間で求めることができない。
【０００８】
また特許文献３に開示される従来の技術として、補間近似して各アーム体の回転角度を求める方法がある。この方法は、解析的な順変換式によって、各アーム体の回転角度を決定した場合の多関節マニピュレータの先端部の代表点を求め、複数の代表点における各アーム体の回転角度をデータベース化して予め記憶しておく。逆変換するにあたっては、記憶しているデータベースから補間近似して各アーム体の回転角度を求める。このようにして得られる各アーム体の回転角度は、近似解であるので７つ未満の関節部を有する多関節マニピュレータの先端部を正確な位置および姿勢に位置決めすることができない。
【０００９】
図１０は、補間近似して姿勢制御される多関節マニピュレータ１の一部を示す図である。従来、多関節マニピュレータ１は、手先装置１０が代表点に配置されたときの各アーム体の回転角度を記憶している。たとえば第１の代表点Ａから第２の代表点Ｂに手先装置１０を移動させる場合、多関節マニピュレータ１は、手先装置１０が第１の代表点Ａにあるときの各アーム体の回転角度と、手先装置１０が第２の代表点Ｂにあるときの各アーム体の回転角度とをデータベースから読出す。次に手先装置１０が第１の代表点Ａにあるときの各アーム体の回転角度と、手先装置１０が第２の代表点Ｂにあるときの各アーム体の回転角度とを複数の段階に補間した各回転角度を演算する。多関節マニピュレータ１は、補間した各回転角度に各アーム体を回転させて、手先装置１０を第１の代表点Ａから第２の代表点Ｂに移動させる。
【００１０】
この場合、各アーム体は補間近似した回転角度に動作するので、正確に手先装置１０を位置決めすることができない。したがって手先装置１０を、予め定める移動経路に沿ってスムーズに移動させることができないという問題がある。
【００１１】
また手先装置１０を目標の位置および姿勢にするための、各アーム体の各回転角度は、複数の組合せが存在する。しかし上述したように収束演算または補間近似によって各アーム体の回転角度を求める場合には、各回転角度のうち１つの組合せしか求めることができない。したがって最適なアーム体の回転角度の組合せを求めることができないという問題がある。
【００１２】
以上のように従来の技術では、６つ以下の関節部を有する多関節マニピュレータでは、手先装置１０を目標とする位置および姿勢に配置するときに各アーム体の回転角度を解析的に求めることができない。したがって手先装置を目標とする位置および姿勢に高速かつ正確に配置することができない。また各アーム体の角度の複数の組合せを求めることができないという問題がある。
【００１３】
したがって、本発明の目的は、対象物が目標とする位置および姿勢に配置されるときの各アーム体の角度を解析的に求めることができる位置決め演算方法および位置決め演算装置を提供することである。
【００１４】
【課題を解決するための手段】
本発明は、基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に回転自在に連結され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、基台と第１のアーム体との回転軸線と、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する３軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算方法であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力工程と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算する工程と、
反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記３軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求める３軸交点演算工程と、
反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記３軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標に反転モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標から反転モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求める第１〜第３角度演算工程と、
これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、第２のアーム体に対する第１のアーム体の回転角度である第５角度、および第１のアーム体に対する基台の回転角度である第６角度を求める第４〜第６角度演算工程と、
反転モデルの第１〜第６角度を真のモデルの第１〜第６角度に変換する工程とを含むことを特徴とする位置決め演算方法である。
本発明は、基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に回転自在に連結され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、基台と第１のアーム体との回転軸線と、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する３軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算装置であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力手段と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算し、反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記３軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記における３軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標に反転モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標から反転モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求め、これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、第２のアーム体に対する第１のアーム体の回転角度である第５角度、および第１のアーム体に対する基台の回転角度である第６角度を求め、反転モデルの第１〜第６角度を真のモデルの第１〜第６角度に変換する演算手段と、
演算手段が演算した演算結果を出力する出力手段とを備えることを特徴とする位置決め演算装置である。
【００１５】
本発明に従えば、姿勢制御対象とする多関節マニピュレータの第６のアーム体に対象物が固定され、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、真のモデルの座標系において対象物の位置および姿勢が入力され、反転モデルの座標系から見た基台の位置および姿勢を演算し、３軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、反転モデルの座標系に対して、第１〜第３角度を変数として用い、ｘ座標式とｙｚ加算座標式とｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求め、さらに第４〜第６角度を求め、こうして得られた反転モデルの第１〜第６角度を真のモデルの第１〜第６角度に変換する。
したがって対象物の位置および姿勢が決定されると、３軸交点の座標を一意的に求めることができる。
【００１６】
各角度を容易に求めるために、多関節マニピュレータを真のモデルとし、その真のモデルに対する反転モデルを仮定して各アーム体の角度を求める。
【００１７】
このように第１〜第６角度を短時間で正確に求めることができる。また対象物が目標とする位置に移動して希望する姿勢をとるにあたり、各アーム体の角度のすべての組合せを求めることができる。
【００１８】
本発明は、基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に固定され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する２軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算方法であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力工程と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算する工程と、
反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記２軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求める２軸交点演算工程と、
反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標に反転モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標から反転モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求める第１〜第３角度演算工程と、
これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、および第２のアーム体に対する第１のアーム体の回転角度である第５角度を求める第４〜第５角度演算工程と、
反転モデルの第１〜第５角度を真のモデルの第１〜第５角度に変換する工程とを含むことを特徴とする位置決め演算方法である。
本発明は、基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に固定され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する２軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算装置であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力手段と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算し、反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記２軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標に反転モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標から反転モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求め、これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、および第２のアーム体に対する第１のアーム体の回転角度である第５角度を求め、反転モデルの第１〜第５角度を真のモデルの第１〜第５角度に変換する演算手段と、
演算手段が演算した演算結果を出力する出力手段とを備えることを特徴とする位置決め演算装置である。
【００１９】
本発明に従えば、第１のアーム体は基台に固定され、前述と同様に反転モデルを仮定し、２軸交点に関して反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、さらに座標式を解いて第１〜第３角度を求め、さらに第４および第５角度を求め、こうして得られた反転モデルの第１〜第５角度を真のモデルの第１〜第５角度に変換する。
【００２０】
各角度を容易に求めるために、前述と同様に真のモデルに対する反転モデルを仮定して、反転モデルの第１〜第５角度を求め、これによって真のモデルの第１〜第５角度を容易に求める。
【００２１】
このように、前述と同様に第１〜第５角度を短時間で正確に求めることができ、対象物が目標とする位置、姿勢に対する各角度のすべての組合せを求めることができる。
【００２２】
本発明は、適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記３軸交点演算工程では、前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を、
等価モデルにおける前記３軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、
前記第１〜第３角度演算工程では、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標に等価モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標から等価モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算する工程を含むことを特徴とする。
本発明は、適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記演算手段は、前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を、等価モデルにおける前記３軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標に等価モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標から等価モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算することを特徴とする。
【００２３】
本発明に従えば、第３のアーム体と第４のアーム体とを連結する関節部が傾斜関節部であり、この関節部を第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、この等価モデルの座標系が対象物に固定され、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢とし、３軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、等価モデルにおいて第１〜第３角度を変数として座標式を解いて等価モデルの第１〜第３角度を求めて、反転モデルの第１〜第３角度に変換する。
【００２４】
本発明は、適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記２軸交点演算工程では、前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を、等価モデルにおける前記２軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、
前記第１〜第３角度演算工程では、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標に等価モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標から等価モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算する工程を含むことを特徴とする。
本発明は、適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記演算手段は、前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を、等価モデルにおける前記２軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標に等価モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標から等価モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算することを特徴とする。
【００２５】
本発明に従えば、各角度を容易に求めるために、真のモデルに対する反転モデルと、第３および第４のアーム体の関節部を傾斜関節部の代りに垂直な関節部に置き換えた等価モデルを仮定することによって、３軸交点および２軸交点をそれぞれ有する多関節マニピュレータにおける第１〜第６角度および第１〜第５角度をさらに容易に求めることができる。
【００３６】
【発明の実施の形態】
図１は、本発明の実施の一形態である多関節マニピュレータ２０を示す正面図である。本発明の実施の一形態である多関節マニピュレータは、複数のアーム体と、アーム体を回転自在に連結する複数の関節部とを有する。各アーム体は、それぞれ連結されて直列状に延びるアーム連結体を構成する。
【００３７】
各関節部は、同軸関節部と傾斜関節部とが組み合わされて設けられる。同軸関節部は、隣接する２つのアーム体を、その各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する。また傾斜関節部は、隣接する２つのアーム体のうち一方を、各アーム体の軸線に対して傾斜する回転軸線まわりに円錐回転自在に連結する。本発明において、「円錐回転」は、回転軸線を中心とし、２つのアーム体の連結部分を頂点とする仮想円錐の周面に沿って相対的に回転することである。また本発明において、「回転」は、回転軸線まわりに３６０度以下の角度で角変位する状態も含む。
【００３８】
このような同軸関節および傾斜関節を含む多関節マニピュレータは、各アーム体をそれぞれ回転させることによって、蛇のような動きをさせて、３次元位置決めが可能である。たとえば設備等が複雑に入り組み、作業経路が複雑な場合、であっても、好適に本発明の多関節マニピュレータを動作させることができる。
【００３９】
多関節マニピュレータは、産業用ロボットに用いられる。たとえば多関節マニピュレータの遊端部に各種手先装置、いわゆるエンドエフェクタを連結し、手先装置を目標位置に配置して手先装置を動作させることによって、狭隘な作業空間におけるハンドリング、シーリング、塗装またはアーク溶接などを行うことができる。
【００４０】
図１に示すように、多関節マニピュレータ２０は、６自由度を有して実現される。多関節マニピュレータ２０は、直列に設けられる第１〜第６のアーム体ｃ１〜ｃ６と、隣接する２つのアーム体を回転自在に連結する複数の関節部ｄ１〜ｄ６とを有する。各アーム体ｃ１〜ｃ６は、連結されてアーム連結体を構成する。各関節部は、３つの同軸関節部ｄ１，ｄ３，ｄ５と３つの傾斜回転部ｄ２，ｄ４，ｄ６とを有する。傾斜回転部ｄ２，ｄ４，ｄ６は、隣接する２つの各アーム体の軸線に対して４５度の角度を成して傾斜する回転軸線まわりに回転自在に２つのアーム体を連結する。
【００４１】
複数のアーム体ｃ１〜ｃ６は、直列方向に並んで配置される。そのうち一端部に第１のアーム体ｃ１が配置され、第２〜第６のアーム体ｃ２〜ｃ６が順に連結される。第６のアーム体ｃ６には、手先装置２２が連結される。第１〜第６のアーム体ｃ１〜ｃ６は、図１に示すように、それぞれの軸線が同軸に配置されて一直線状に延びる状態に変形可能である。
【００４２】
第１のアーム体ｃ１は、その一端部２３が同軸関節部ｄ１によって、予め定める対象物である基台２１に連結される。基台２１は、予め定める固定位置に固定されても、移動可能に設けられてもよい。第１のアーム体ｃ１は、その軸線と同軸の回転軸線Ｌ１まわりに基台２１に対して回転自在に連結される。また第１のアーム体ｃ１のうち、基台２１と反対側の端部２４には、傾斜関節部ｄ２によって第２のアーム体ｃ２が連結される。
【００４３】
第１のアーム体ｃ１と第２のアーム体ｃ２とは、第１のアーム体ｃ１および第２のアーム体ｃ２の軸線に対して、４５度の角度を成して傾斜する回転軸線Ｌ２まわりに回転自在に連結される。第２のアーム体ｃ２のうち、第１のアーム体ｃ１と反対側の端部２５には、同軸関節部ｄ３によって第３のアーム体ｃ３が連結される。
【００４４】
第２のアーム体ｃ２と第３のアーム体ｃ３とは、互いの軸線と同軸な回転軸線Ｌ３まわりに回転自在に連結される。第３のアーム体ｃ３のうち、第２のアーム体ｃ２と反対側の端部２６には、傾斜関節部ｄ４によって第４のアーム体ｃ４が連結される。
【００４５】
第３のアーム体ｃ３と第４のアーム体ｃ４とは、第３のアーム体ｃ３および第４のアーム体ｃ４の軸線に対して、４５度の角度を成して傾斜する回転軸線Ｌ４まわりに回転自在に連結される。第４のアーム体ｃ４のうち、第３のアーム体ｃ３と反対側の端部２７には、同軸関節部ｄ５によって第５のアーム体ｃ５が連結される。
【００４６】
第４のアーム体ｃ４と第５のアーム体ｃ５とは、互いの軸線と同軸な回転軸線Ｌ５まわりに回転自在に連結される。第５のアーム体ｃ５のうち、第４のアーム体ｃ４と反対側の端部２８には、傾斜関節部ｄ６によって第６のアーム体ｃ６が連結される。
【００４７】
第５のアーム体ｃ５と第６のアーム体ｃ６とは、第５のアーム体ｃ５および第６のアーム体ｃ６の軸線に対して、４５度の角度を成して傾斜する回転軸線Ｌ６まわりに回転自在に連結される。また本発明の実施の形態では、第６のアーム体ｃ６のうち、第５のアーム体ｃ５と反対側の端部には、手先装置２２が連結される。手先装置２２は、たとえば部品を把持および把持解除するためのハンドリング装置であってもよい。
【００４８】
このように複数のアーム体ｃ１〜ｃ６は、同軸関節部と傾斜関節部とが交互に配置される。また各アーム体ｃ１〜ｃ６の各軸線は、すべての軸線が同軸に配置されて一直線状に連結可能に形成される。多関節マニピュレータ２０は、基台２１と第１のアーム体との回転軸線Ｌ１と、第１のアーム体と第２のアーム体との回転軸線Ｌ２と、第２のアーム体と第３のアーム体との回転軸線Ｌ３とが交差する３軸交点を有する。
【００４９】
各アーム体ｃ１〜ｃ６は、各アーム体を回転駆動する回転駆動モータを内蔵する。また各アーム体ｃ１〜ｃ６は、その軸線を挿通する中空空間が形成される。この中空空間に１または複数の配線が内蔵される。各配線は、回転駆動モータを回転させるための電力、回転駆動モータへの回転指示、手先装置２２へ圧縮空気などを供給する。
【００５０】
各アーム体を回転軸線まわりに回転する回転機構は、従来の機構と同様の機構であってもよい。たとえば一方のアーム体と他方のアーム体とを回転軸線まわりに回転自在に支持するベアリングと、中空形状の波動歯車機構たとえばハーモニックドライブ（登録商標）とを備える。波動歯車機構は、入力側部材と出力側部材とを備え、それらが相対的に回転する。一方のアーム体は、入力側部材に連結され、他方のアーム体は、出力側部材に連結される。
【００５１】
回転駆動モータからの回転が入力側部材に与えられると、入力側部材と出力側部材とが相対的に回転する。これによって一方のアーム体と他方のアーム体とを相対的に回転させることができる。このような回転機構が、各アーム体ｃ１〜ｃ６に設けられることによって、隣接する２つのアーム体を、相対的に回転させることができる。さらに中空形状のハーモニックドライブ機構を用いることによって、連結される２つのアーム体の各挿通空間に各配線を挿通させた状態で、２つのアーム体を回転することができる。さらに多関節マニピュレータ全体、または挿通空間内を挿通する各配線を耐水性、耐熱性あるいは耐衝撃性のある材料によって構成されるカバーで覆うことによって、防塵、防爆、防水性を必要とする作業環境に好適に用いることができる。
【００５２】
図１に示すように、多関節マニピュレータ２０は、さらに各アーム体の相対回転角度を求める位置決め演算装置３０を備える。位置決め演算装置３０は、目標とする位置および姿勢に手先装置２２を移動させるにあたって、手先装置２２を目標とする位置および姿勢に移動させるのに必要な各アーム体ｃ１〜ｃ６の相対回転角度θ１〜θ６を求める。
【００５３】
位置決め演算装置３０は、入力手段３１と、演算手段３２と、出力手段３３とを有する。入力手段３１は、演算座標系における手先装置２２の位置および姿勢を入力する。また演算手段３２は、入力手段３１から入力された手先装置２２の位置および姿勢と、多関節マニピュレータの諸形状とに基づいて、手先装置２２を目標とする位置および姿勢に移動させるのに必要な各アーム体の相対回転角度θ１〜θ６を演算する。出力手段３３は、演算手段３３による演算結果に基づいて、各アーム体を回転するための電力と回転指令を与え、演算結果に応じて各アーム体ｃ１〜ｃ６を回転させる。
【００５４】
位置決め演算装置３０は、たとえばコンピュータによって実現される。入力手段３１は、キーボード、マウスなどのポインティングデバイスおよび作業者が多関節マニピュレータ２を直接制御するコントローラであるティーチペンダントによって実現される。また入力手段３１は、位置決め演算装置３０に接続される他の装置から手先装置２２の位置および姿勢を入力されてもよい。出力手段３３は、各駆動モータを回転させるための回転指令信号を、演算結果に応じて生成し、生成した回転指令信号を各駆動モータに与える。出力手段３３は、たとえば駆動回路によって実現される。位置決め演算装置３０は、多関節マニピュレータ２０に内蔵されていてもよく、また別途設けられていてもよい。
【００５５】
また演算手段３２は、演算プログラムを実行する中央処理装置（Central
Processing Unit）を含んで実現される。中央処理装置は、予め演算プログラムを記憶していてもよく、またコンピュータ読取可能な記憶媒体に記録される演算プログラムを読み取ることによって実行してもよい。演算プログラムは、後述する演算方法を示す。
【００５６】
図２は、位置決め演算装置３０によって姿勢制御される多関節マニピュレータ２０を示す図である。位置決め演算装置３０は、解析計算式に基づいて、手先装置２２を目的の位置および姿勢に配置させるための各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を数値的に求める。上述する６軸多関節マニピュレータ２０では、手先装置２２を目標とする位置および姿勢に配置する場合、各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６は、８通りの組合せがある。演算装置３０がこの各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を解析計算式に基づいて求めることによって、この８通りの組合せを同時に求めることができる。図２（１）〜図２（８）に示すように、手先装置２２を予め定める位置および姿勢に配置する場合の各アーム体の回転角度の組合せを複数求めることができるので、エネルギーコストおよび障害物の有無に応じて、アーム体ｃ１〜ｃ６の回転角度θ１〜θ６の組合せを選択することができる。
【００５７】
図３は、手先装置２２を移動させる場合に位置決め演算装置３０によって姿勢制御される多関節マニピュレータ２０を説明するための図である。たとえば第１の代表点Ａから第２の代表点Ｂに手先装置２２を移動させる場合には、第１の代表点Ａと第２の代表点Ｂとの間を手先装置２２が通過する通過点Ｕ１〜Ｕ３を求め、通過点Ｕ１〜Ｕ３に配置されるときの各アーム体の回転角度を求めることができる。これによって手先装置２２が移動する経路に沿って通過点を順に設定し、通過点ごとに各アーム体の回転角度を求めることによって、指定した経路に沿って手先装置２２を円滑に移動させることができる。
【００５８】
また位置決め演算装置３０は、従来のように収束演算によって、各アーム体の回転角度を求める必要がないので短時間で求めることができる。また演算手段３１に高い処理能力を必要としない。したがって手先装置２２の移動時であっても各アーム体の回転角度を演算することができる。さらに従来の技術のように、代表点Ａ，Ｂにおける各アーム体の回転角度をデータベース化して記憶する必要がなく、必要なデータ記憶容量を低減させることができる。
【００５９】
図４は、本発明の位置決め演算方法を大略的に示すフローチャートである。まず、ステップｓ１で、制御対象である多関節マニピュレータ２０の各アーム体の寸法およびアーム体を連結する関節の種類などの諸条件が与えられ、各アーム体の回転角度を演算可能な状態となると、ステップｓ２に進む。
【００６０】
ステップｓ２では、多関節マニピュレータ２０が移動させる対象物の位置および姿勢を取得する。たとえば対象物は、第６のアーム体ｃ６の先端に連結される手先装置２２であり、固定して設けられる座標系に対する手先装置２２の位置および姿勢が入力される。このように対象物を配置すべき目標の位置および姿勢を取得するとステップｓ３に進む。
【００６１】
ステップｓ３では、各アーム体のうち直列方向に順に並ぶ３つのアーム体のそれぞれの回転軸線が互いに交差する３軸交点の座標を求める。３軸交点は、対象物の位置および姿勢が決定されると、その座標を一意的に求めることができる。したがってステップｓ２によって取得した対象物の位置および姿勢に基づいて、３軸交点の座標を求めることができる。３軸交点の座標を求めるとステップｓ４に進む。
【００６２】
ステップｓ４では、３軸交点を通過する３つの回転軸線以外の残余の３つの回転軸線まわりに回転する３つのアーム体の相対的な回転角度である第１〜第３角度を変数として用いて、３軸交点の座標を表わす座標式を生成する。すなわち３軸交点のｘ座標を表わすｘ座標式と、ｙ座標を表わすｙ座標式と、ｚ座標を表わすｚ座標式とを生成する。次に、ｙ座標式およびｚ座標式に基づいて、ｙ座標にｚ座標を加算した加算値を表すｙｚ加算座標式と、ｙ座標からｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を生成する。このように各座標式を生成すると、ステップｓ５に進む。
【００６３】
ステップｓ５では、ステップｓ３で求められる３軸交点の座標と、ステップｓ４で求められたｘ座標式、ｙｚ加算座標式、ｙｚ減算座標式に基づいて、第１〜第３角度を求める。さらに第１〜第３角度に基づいて、残りの第４〜第６角度を求める。このようにして各アーム体の回転角度を求めると、ステップｓ６に進み、動作を終了する。
【００６４】
図１に示す多関節マニピュレータ２０の場合、各角度をより容易に求めるために、本実施の形態における多関節マニピュレータ２０を真のモデルとし、真のモデルの座標系の原点は基台に固定され、その真のモデルに対してｚ軸に反転した反転モデルと、反転モデルに対して等価なモデルであって１つの関節部の構成が異なる等価モデルとを仮定する。第６のアーム体ｃ６には、対象物としての手先装置２２が固定される。反転モデルは、対象物に対して基台の位置を制御する形態のモデルであり、反転モデルの座標系の原点は対象物に固定される。等価モデルは、後述の図８のように、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルであり、等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としている。真のモデルを、反転モデルおよび等価モデルに変換して各アーム体の回転角度を求めることによって、より容易に各アーム体の回転角度を求めることができる。
【００６５】
図５は、図１に示す多関節マニピュレータ２０の位置決め演算処理方法を具体的に示すフローチャートである。演算処理方法として、ステップｓ１１で真のモデルにおける多関節マニピュレータの各アーム体の寸法およびアーム体を連結する関節の種類などの諸条件が与えられ、各アーム体の回転角度を演算可能な状態となると、ステップｓ１２に進む。
【００６６】
ステップｓ１２では、真のモデルにおける基台２１に対する手先装置２２の位置および姿勢を取得し、ステップｓ１３に進む。ステップｓ１３では、真のモデルにおける座標系および位置を反転モデルにおける座標系および位置に変換する。次に反転モデルにおける手先装置２２に対する基台２１の位置および姿勢を演算する。すなわち多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算する。その後、ステップｓ１４に進む。ステップｓ１４では、反転モデルの座標系に対する基台２１の位置および姿勢に基づいて、反転モデルにおける３軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標から成る位置を求め、ステップｓ１５に進む。
【００６７】
ステップｓ１５では、等価モデルにおいて、３軸交点を通過する３つの回転軸線以外の残余の３つの回転軸線まわりに回転する３つのアーム体の相対的な回転角度である第１〜第３角度を変数として３軸交点の位置を示す座標式を生成する。具体的には等価モデルにおいて、３軸交点のｘ座標を表わすｘ座標式と、ｙ座標を表わすｙ座標式と、ｚ座標を表わすｚ座標式とを生成する。次に、ｙ座標式およびｚ座標式に基づいて、ｙ座標にｚ座標を加算した加算値を表すｙｚ加算座標式と、ｙ座標からｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を生成する。このような数式が求められるとステップｓ１６に進む。等価モデルの座標系に対する基台の位置および姿勢に基づいて、等価モデルにおける３軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求める。さらに等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標に等価モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標から等価モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を得る。
【００６８】
ステップｓ１６では、ステップｓ１４において求められた反転モデルにおける３軸交点の位置と、ステップｓ１５によって求められたｘ座標式と、ｙｚ加算座標式と、ｙｚ減算座標式とに基づいて、等価モデルにおける第１〜第３角度を求め、ステップｓ１７に進む。
【００６９】
ステップｓ１７では、ステップｓ１６で求められた第１〜第３角度を等価モデルから反転モデルに変換し、反転モデルにおける第１〜第３角度を演算する。反転モデルにおける第１〜第３角度が求まるとステップｓ１８に進む。ステップｓ１８では、ステップｓ１７で求められた反転モデルの第１〜第３角度に基づいて、反転モデルの第４〜第６角度を求め、ステップｓ１９に進む。反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、第２のアーム体に対する第１のアーム体の回転角度である第５角度、および第１のアーム体に対する基台の回転角度である第６角度を求める。ステップｓ１９では、ステップｓ１７およびステップｓ１８で求められた反転モデルにおける第１〜第６角度ｑ１〜ｑ６を真のモデルにおける第１〜第６角度θ１〜θ６に変換し、ステップｓ２０に進み、動作を終了する。
【００７０】
図６は、本発明の多関節マニピュレータ２０をモデル化した真のモデルを示す図である。多関節マニピュレータ２０には、基台２１に固定的に設けられ、相互に直交する演算座標系Σｒｂが設定される。演算座標系Σｒｂは、直交３軸の座標系であって、予め定める演算原点を通過し第１の方向に延びる軸である演算ｘ軸、演算原点を通過し演算ｘ軸に直交する第２の方向に延びる軸である演算ｙ軸、および演算原点を通過し演算ｘ軸および演算ｙ軸にともに直交する第３の方向に延びる軸である演算ｚ軸を有する。
【００７１】
また各アーム体ｃ１〜ｃ６、手先装置２２は、それぞれに固定して予め定める各対象座標系が設定される。各対象座標系は、直交３軸の座標系であって、対応する各アーム体ｃ１〜ｃ６または手先装置２２の代表点を原点とする。またその原点を通過し第１の方向に延びる軸である対象ｘ軸、対象原点を通過し対象ｘ軸に直交する第２の方向に延びる軸である対象ｙ軸、および対象原点を通過し対象ｘ軸および対象ｙ軸にともに直交する第３の方向に延びる軸である対象ｚ軸を有する。
【００７２】
図６に示す真のモデルでは、第１のアーム体ｃ１と基台２１との回転軸線Ｌ１に沿って延びる仮想線が通過する一点であって、基台２１に定める基台代表点Ｓを原点とする演算座標系Σｒｂが設定される。また手先装置２２と第６のアーム体ｃ６とは固定されているので、手先装置２２の位置および姿勢を決定することと、第６のアーム体ｃ６の位置および姿勢を決定することとは等価である。したがって、手先装置２２の位置は、第４のアーム体ｃ４と第５のアーム体ｃ５との回転軸線Ｌ５に沿って延びる仮想線が通過する一点であって、第６のアーム体ｃ６の代表点Ｔの座標で表わされる。
【００７３】
また手先装置２２に固定して設けられる対象座標系である手先座標系Σｒｔは、第６のアーム体ｃ６の代表点Ｔを原点とする直交３軸の座標系である。各アーム体ｃ１〜ｃ６が互いに相対的に回転することによって、演算座標系Σｒｂに対して第６のアーム体ｃ６の代表点Ｔが移動する。
【００７４】
代表点Ｔの位置（Ｔｘ，Ｔｙ，Ｔｚ）は、代表点Ｔにおける演算ｚ軸の座標Ｔｘ、演算ｙ座標の座標Ｔｙおよび演算ｚ軸座標Ｔｚで表わす。また各アーム体の軸線が同軸に配置されて一直線状に連結される一連結状態から各アーム体ｃ１〜ｃ６が互いに相対的に回転することによって、演算座標系Σｒｂに対して手先座標系Σｒｔが回転し、第６のアーム体ｃ６の姿勢が変化する。第６のアーム体ｃ６の姿勢（ｑｏ，ｑａ，ｑｔ）は、手先座標系Σｒｔが、演算ｚ軸まわりに回転する角度ｑｏ、演算ｙ軸まわりに回転する角度ｑａ、演算ｚ軸まわりに回転する角度ｑｔで表わす。
【００７５】
各アーム体ｃ１〜ｃ６は、前記一連結状態を基準状態とし、前記一連結状態から基台２１に対する第１のアーム体ｃ１の回転角度を第１角度θ１とし、第１のアーム体ｃ１に対する第２のアーム体ｃ２の回転角度を第２角度θ２とし、第２のアーム体ｃ２に対する第３のアーム体ｃ３の回転角度を第３角度θ３とし、第３のアーム体ｃ３に対する第４のアーム体ｃ４の回転角度を第４角度θ４とし、第４のアーム体ｃ４に対する第５のアーム体ｃ５の回転角度を第５角度θ５とし、第５のアーム体ｃ５に対する第６のアーム体ｃ６の回転角度を第６角度θ６とする。
【００７６】
図７は、図６に示すの真のモデルを反転した反転モデルの多関節マニピュレータ２０Ａを示す図である。また図８は、図７の反転モデルにおける多関節マニピュレータ２０Ａの１つの関節部を等価な関節部に置き換えた等価モデルの多関節マニピュレータ２０Ｂを示す図である。図７および図８に示すモデルは、理解を容易にするために、真のモデルに対して異なる名称および参照符号とする。
【００７７】
すなわち反転および等価モデルにおける第１のアーム体ｅ１は、真のモデルにおける第６のアーム体ｃ６に対応する。また反転および等価モデルにおける第２のアーム体ｅ２は、真のモデルにおける第５のアーム体ｃ５に対応する。また反転および等価モデルにおける第３のアーム体ｅ３は、真のモデルにおける第４のアーム体ｃ４に対応する。また反転および等価モデルにおける第４のアーム体ｅ４は、真のモデルにおける第３のアーム体ｃ３に対応する。また反転および等価モデルにおける第５のアーム体ｅ５は、真のモデルにおける第２のアーム体ｃ２に対応する。また反転および等価モデルにおける第６のアーム体ｅ６は、真のモデルにおける第１のアーム体ｃ１に対応する。このように反転および等価モデルにおける第１〜第６のアーム体ｅ１〜ｅ６は、真のモデルにおける第６〜第１のアーム体ｃ６〜ｃ１に反転してそれぞれ対応する。
【００７８】
また同様に反転モデルにおける各関節部ｆ１〜ｆ６、各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６および各アーム体の軸線方向寸法ｉ０〜ｉ６もまた、真のモデルに対して異なる参照符号を付する。
【００７９】
また等価モデルにおける多関節マニピュレータ２０Ｂは、反転モデルにおける多関節マニピュレータ２０Ａに対して、第３のアーム体ｅ３と第４のアーム体ｅ４とを連結する関節部が異なる。等価モデルにおける多関節マニピュレータ２０Ｂでは、この関節部Ｆ３は、第３および第４のアーム体ｅ３，ｅ４の軸線に垂直な回転軸線Ｌ３まわりに各アーム体ｅ３，ｅ４の軸線を回転自在に連結する。この関節部ｆ３によって、第３および第４のアーム体ｅ３，ｅ４は、互いの軸線を含む一平面上を回転する。
【００８０】
反転モデルでは、反転モデルにおける第１のアーム体ｅ１、すなわち真のモデルの第６のアーム体ｃ６の代表点Ｔを、演算座標系Σｓｂの演算原点とする。また反転モデルにおいて、基台２１に固定して設けられる対象座標系である基台座標系Σｓｔは、基台代表点Ｓを原点とする。反転モデルは、各アーム体ｅ１〜ｅ６の軸線が同軸に配置されて一直線状に連結される一連結状態において、基台座標系Σｓｔの対象ｘ軸、対象ｙ軸および対象ｚ軸は、演算座標系Σｓｂの演算ｘ軸、演算ｙ軸および演算ｚ軸に対してそれぞれ同一方向に延びる。また一連結状態において演算座標系Σｓｂおよび基台座標系Σｓｔは、各アーム体ｃ１〜ｃ６の軸線方向に沿う方向に演算ｚ軸および対象ｚ軸が延びる。演算ｚ軸および対象ｚ軸は、演算座標系Σｓｂの原点から基台座標系Σｓｔの原点に向かう方向を正とする。
【００８１】
本発明の実施の形態の多関節マニピュレータ２０では、手先装置２２を固定し、基台２１を変位可能とした場合に、ステップｓ１〜ステップｓ６を実行する。すなわち反転モデルにおいて、基台２１を対象物とみなして、目標の位置および姿勢に移動させるための各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６を求める。
【００８２】
次に反転モデルとして求めた各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６を、真のモデルに対応するように変換する。これによって真のモデルの多関節マニピュレータ２０における、手先装置２２を目標の位置および姿勢に移動させるための各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を求めることができる。
【００８３】
多関節マニピュレータにおいて、基台側すなわち固定側に３軸交点が存在する場合よりも、手先装置側すなわち遊端部側に３軸交点が存在する場合のほうが、各アーム体の回転角度を容易に求めることができる。したがって本実施の多関節マニピュレータ２０のように、基台側に３軸交点がある場合には、まず反転モデルに変換して各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６を求め、その後、求められた反転モデルにおける各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６を真のモデルに再変換することによって、各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を容易に求めることができる。
【００８４】
以下、図５に示すフローチャートを参照して演算方法の手順を説明する。まずステップｓ１１で、各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を求めるための準備工程を行う。準備工程では、各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を求めるにあたって、真のモデルにおける各アーム体ｃ１〜ｃ６の各軸線方向寸法Ｈ０〜Ｈ６、第１のアーム体ｃ１と第２のアーム体ｃ２との回転軸線Ｌ２が各軸線から傾斜する角度、第３のアーム体ｃ３と第４のアーム体ｃ４との回転軸線Ｌ４が各軸線から傾斜する角度、第５のアーム体ｃ５と第６のアーム体ｃ６との回転軸線Ｌ６が各軸線から傾斜する角度とを取得し、多関節マニピュレータの諸条件を取得する。本実施の形態では、軸線から傾斜する回転軸線Ｌ２，Ｌ４，Ｌ６が、各アーム体の軸線から傾斜する角度αがともに４５度の場合について説明する。このように多関節マニピュレータの諸条件が決定されると、ステップｓ１２に進む。
【００８５】
ステップｓ１２では、真のモデルにおける演算座標系Σｒｂに対する手先装置２２の位置および姿勢を取得する。言換えると、第６のアーム体ｃ６の代表点Ｔの位置（Ｔｘ，Ｔｙ，Ｔｚ）および姿勢（ｑｏ，ｑａ，ｑｔ）を取得し、ステップｓ１３に進む。
【００８６】
ステップｓ１３では、まず真のモデルを反転モデルに変換するための変換行列を生成し、変換行列に基づいて、反転モデルにおける基台２１の姿勢を求める。反転モデルにおける基台２１の姿勢は、基台座標系Σｓｔが演算座標系Σｓｂの演算ｘ軸まわりに回転する角度ｑｘ、基台座標系Σｓｔが演算座標系Σｓｂの演算ｙ軸まわりに回転する角度ｑｙ、基台座標系Σｓｔが演算座標系Σｓｂの演算ｚ軸まわりに回転する角度ｑｚで表わされる。
【００８７】
基台２１の姿勢（ｑｘ，ｑｙ，ｑｚ）が求まると、反転モデルにおける基台代表点Ｓの位置を求める。この基台代表点Ｓの位置は、演算座標系Σｓｂの演算ｘ軸Ｐｘ、演算ｙ軸Ｐｙ、演算ｚ軸の座標Ｐｚで表わされる。反転モデルにおける基台２１の姿勢（ｑｘ，ｑｙ，ｑｚ）と基台代表点Ｓの位置（Ｐｘ，Ｐｙ，Ｐｚ）とを求めるとステップｓ１４に進む。
【００８８】
ステップｓ１４では、３軸交点の位置を求める。３軸交点は、第５のアーム体の代表点ｐ５であって、図１に示すマニピュレータ２０の場合、第５のアーム体ｅ５と第６のアーム体ｅ６との回転軸線Ｌ５と第５のアーム体ｅ５と第６のアーム体ｅ６の軸線とが交差する点であって、第５のアーム体ｅ５と第６のアーム体ｅ６とを連結する部分に設けられる。この３軸交点の位置を求めるために、まず演算座標系を第５のアーム体ｅ５に固定して設けられる対象座標系に変換する変換行列を求める。この変換行列Ｒ０５は、次式によって表わされる。
【００８９】
【数１】
【００９０】
ここで、Ｒ０１〜Ｒ０５は、演算座標系を各アーム体ｅ１〜ｅ５に固定して設けられる対象座標系に変換する変換行列Ｒ０１〜Ｒ０３である。またαは上述したように傾斜関節部における各アーム体の回転軸線が各アーム体の軸線に対して傾斜する角度である。Ｒ０１におけるαは、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１が各アーム体ｅ１，ｅ２の軸線に対して傾斜する角度の値を示す。またＲ０３におけるαは、第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３が各アーム体ｅ３，ｅ４の軸線に対して傾斜する角度の値を示す。またＲ０５におけるαは、第５のアーム体ｅ５と第６のアーム体ｅ６との回転軸線Ｌ５が各アーム体ｅ５，ｅ６の軸線に対して傾斜する角度の値を示す。またｑ１〜ｑ６は、各アーム体の回転角度であって、第１のアーム体ｅ１に対する第２のアーム体ｅ２の回転角度を、第１角度ｑ１とし、第２のアーム体ｅ２に対する第３のアーム体ｅ３の回転角度を第２角度ｑ２とし、第３のアーム体ｅ３に対する第４のアーム体ｅ４の回転角度を第３角度ｑ３とする。また第４のアーム体ｅ４に対する第５のアーム体ｅ５の回転角度を第４角度ｑ４とし、第５のアーム体ｅ５に対する第６のアーム体ｅ６の回転角度である第５角度ｑ５とし、第６のアーム体ｅ６に対する基台２１の回転角度である第６角度ｑ６とする。各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６は、各アーム体ｅ１〜ｅ６が一直線状に連結される一連結状態において、各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６をゼロとする。
【００９１】
またＲｚ［θ］、Ｒｙ［θ］、Ｒｘ［θ］は、演算座標系に対して対象座標系の姿勢を回転させるための回転行列である。Ｒｚ［θ］は、対象座標系を演算ｚ軸まわりにθ度回転させる回転行列であり、次式にその成分を示す。またＲｙ［θ］は、対象座標系を演算ｙ軸まわりにθ度回転させる回転行列であり次式にその成分を示す。またＲｘ［θ］は、対象座標系を演算ｘ軸まわりにθ度回転させる回転行列でありその成分を次式に示す。
【００９２】
【数２】
【００９３】
ここでθは、変数であり、数値または任意の変数が代入される。上式において、たとえばＲｚ［ｑ１］は、（２）式に示す各成分におけるθにそれぞれｑ１が代入された行列を示す。また本発明においてｓｉｎ θは、θのサインの値を示し、ｃｏｓ θは、θのコサインの値を示す。また記号「・」は、乗算を示す。
また第５のアーム体ｅ５の代表点ｐ５の位置は、次式で表わされる。
【００９４】
【数３】
【００９５】
ここで手先代表点Ｔから第１のアーム体ｅ１の代表点ｐ１までの距離をｉ０とし、第１のアーム体ｅ１の代表点ｐ１から第２のアーム体ｅ２の代表点ｐ２までの距離をｉ１とし、第２のアーム体ｅ２の代表点ｐ２から第２のアーム体ｅ３の代表点ｐ３までの距離をｉ２とし、第３のアーム体ｅ３の代表点ｐ３から第４のアーム体ｅ４の代表点ｐ４までの距離をｉ３とする。また第４のアーム体ｅ４の代表点ｐ４から第５のアーム体ｅ５の代表点ｐ５までの距離をｉ４とし、第５のアーム体ｅ５の代表点ｐ５から第６のアーム体ｅ６の代表点ｐ６までの距離をｉ５とする。また第６のアーム体ｅ６の代表点ｐ６から基台代表点Ｓまでの距離をｉ６とする。
【００９６】
また第１のアーム体ｅ１の代表点ｐ１は、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１と第１のアーム体ｅ１の軸線とが交差する点であって、第１のアーム体ｅ１と第２のアーム体ｅ２を連結する部分に設けられる。また第２のアーム体ｅ２の代表点ｐ２は、第２のアーム体ｅ２と第３のアーム体ｅ３との回転軸線Ｌ２が挿通し、第２のアーム体ｅ２と第３のアーム体ｅ３を連結部分に設けられる。また第３のアーム体ｅ３の代表点ｐ３は、第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３と第３のアーム体ｅ３の軸線とが交差する点であって、第３のアーム体ｅ３と第４のアーム体ｅ４を連結する部分に設けられる。
【００９７】
また第４のアーム体ｅ４の代表点ｐ４は、第４のアーム体ｅ４と第５のアーム体ｅ５との回転軸線Ｌ４が挿通し、第４のアーム体ｅ４と第５のアーム体ｅ５を連結する部分に設けられる。また第５のアーム体ｅ５の代表点ｐ５は、第５のアーム体ｅ５と第６のアーム体ｅ６との回転軸線Ｌ５と第５のアーム体ｅ５の軸線とが交差する点であって、第５のアーム体ｅ５と第６のアーム体ｅ６を連結する部分に設けられる。また第６のアーム体ｅ６の代表点ｐ６は、第６のアーム体ｅ６と基台２１との回転軸線Ｌ６が挿通し、第６のアーム体ｅ６と基台２２とを連結する部分に設けられる。
【００９８】
（５）式から、３軸交点の位置、すなわち第５のアーム体の代表点ｐ５を求めることができる。次に演算座標系Σｓｂに対する基台座標系Σｓｔの変換行列Ｒｕｈを求める。この変換行列Ｒｕｈは、次式によって求められる。
【００９９】
【数４】
【０１００】
また（７）式によって、第５のアーム体ｅ５の代表点ｐ５の座標（Ｐｗｘ，Ｐｗｙ，Ｐｗｚ）の数値を求めることができる。さらに後述する動作を行うために、第５のアーム体ｅ５の代表点ｐ５の座標を（８）式に変換する。
【０１０１】
【数５】
【０１０２】
すなわち第５のアーム体ｅ５の代表点ｐ５のｘ座標を表すｘ座標値Ｐｘｎを求める。またｙ座標にｚ座標を加算し、さらに手先代表点Ｔから第１のアーム体ｅ１の代表点ｐ１までの距離ｉ０を減算したｙｚ加算値Ｐｙｎを求める。またｙ座標からｚ座標を減算し、さらに手先代表点Ｔから第１のアーム体ｅ１の代表点ｐ１までの距離ｉ０を減算したｙｚ減算値Ｐｚｎを求める。
【０１０３】
上述したように第５のアーム体ｅ５の代表点ｐ５は、３軸交点となる。したがって（７）式を演算することによって、３軸交点ｐ５の位置を求めることができる。３軸交点の位置を求めると、ステップｓ１５に進む。
【０１０４】
ステップｓ１５では、まず反転モデルにおける多関節マニピュレータ２０Ａを、等価モデルとなる多関節マニピュレータ２０Ｂに変換する。図８に示す等価モデルにおいて、第１〜第３のアーム体ｅ１〜ｅ６の回転角度Ｑ１，Ｑ２，Ｑ３を求める。
【０１０５】
等価モデルにおいて、演算座標系Σｓｂから第１〜第３のアーム体ｅ１〜ｅ３の代表点ｐ１〜ｐ３の姿勢にそれぞれ変換する変換行列Ｒ０１〜Ｒ０３は、（９）式で表わされる。（９）式においてαは、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１が、各アーム体ｅ１，ｅ２の軸線に対して傾斜する角度である。また図８に示すモデルにおける第５のアーム体ｅ５の代表点ｐ５の位置は、次の（１０）式で表わされる。
【０１０６】
【数６】
【０１０７】
【数７】
【０１０８】
ここで、手先基準位置Ｔから第１のアーム体ｅ１の代表点ｐ１までの距離ｉ０を無視した場合における、第１のアーム体ｅ１の代表点ｐ１から第３のアーム体ｅ３の代表点ｐ１までの距離（ｉ１＋ｉ２）を１とし、第３のアーム体ｅ３の代表点ｐ３から第５のアーム体ｅ５の代表点ｐ５までの距離（ｉ３＋ｉ４）を第１のアーム体ｅ１の代表点ｐ１から第３のアーム体ｅ３の代表点ｐ１までの距離（ｉ１＋ｉ２）の除算値（ｉ３＋ｉ４）／（ｉ１＋ｉ２）をｎｒとする。
【０１０９】
実際の第５のアーム体ｅ５の代表点ｐ５は、上式で得られる値に、第１のアーム体ｅ１の代表点ｐ１から第３のアーム体ｅ３の代表点ｐ１までの距離（ｉ１＋ｉ２）を乗算し、さらにその値に手先代表点Ｔから第１のアーム体ｅ１の代表点ｐ１までの距離ｉ０を加算した値となる。（１０）式に基づいて、第５のアーム体ｅ５の代表点ｐ５の位置を求めると次の（１１）式となる。
【０１１０】
【数８】
【０１１１】
（１１）式で第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１とそれらの各アーム体ｅ１，ｅ２の軸線との角度αを４５度とすると、（１２）式となる。
【０１１２】
【数９】
【０１１３】
また第５のアーム体ｅ５の代表点ｐ５の演算ｘ軸座標ｐ５ｘを示す座標式Ｐｘｎ、第５のアーム体ｅ５の代表点ｐ５の演算ｙ軸座標ｐ５ｙと、第５のアーム体ｅ５の代表点ｐ５の演算ｚ軸座標ｐ５ｚとを加算したｙｚ加算値を表すｙｚ加算座標式Ｐｙｎ、第５のアーム体ｅ５の代表点ｐ５の演算ｚ軸座標ｐｚを減算したｙｚ減算値を表すｙｚ減算座標式Ｐｚｎを（１３）式に示す。
【０１１４】
【数１０】
【０１１５】
このように式を変形し、（１３）式を連立して解くことによって、（１４）式に示すようにｃｏｓ Ｑ３を表わすことができ、第３角度Ｑ３を求めることができる。これによって等価モデルにおける第１〜第３角度Ｑ１〜Ｑ３の解析式を容易に求めることができ、ステップｓ１６に進む。
【０１１６】
【数１１】
【０１１７】
ステップｓ１６では、ステップｓ１４の（８）式で求めた第５のアーム体ｅ５の代表位置ｐ５のｘ座標値Ｐｘｎ、ｙｚ加算値Ｐｙｎ、ｙｚ減算値Ｐｚｎを、ステップｓ１５で求めた第１〜第３角度Ｑ１〜Ｑ３を示す式に代入することによって、第１〜第３角度Ｑ１〜Ｑ３の数値解を求めることができる。数値解は、目標とする基台２１の位置が特異点でないかぎり、４通りの組合せが得られる。このように等価モデルにおける第１〜第３角度Ｑ１〜Ｑ３の数値解が求まると、ステップｓ１７に進む。
【０１１８】
ステップｓ１７では、ステップｓ１６で求めた等価モデルの第１〜第３角度Ｑ１〜Ｑ３を反転モデルの第１〜第３角度ｑ１〜ｑ３に変換する。このときｃｏｓｑ３は、次式で表わされる。（１５）式〜（１７）式においてαは、第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３が各アーム体ｅ３，ｅ４に対して傾斜する角度である。
【０１１９】
【数１２】
【０１２０】
ここでｃｏｓｅｃ αは、αのコセカントの値を示す。またｃｏｔ αは、コタンジェントの値を示す。等価モデルの第３角度Ｑ３を（１５）式に代入することによって、反転モデルにおける第３角度ｑ３を求めることができる。
またｃｏｓ ｑ２およびｓｉｎ ｑ２は、次の（１６）式によって求められる。
【０１２１】
【数１３】
【０１２２】
ここでＤ２は、（１７）式によって与えられる。
【０１２３】
【数１４】
【０１２４】
（１６）式によって反転モデルにおける第２角度ｑ２を求めることができる。また等価モデルにおける第１角度Ｑ１は、反転モデルの第１角度ｑ１と同じ値になる。反転モデルにおける第１〜第３角度ｑ１〜ｑ３は、３軸交点が特異点でないかぎり、等価モデルの場合と同様に４つの組合せが求められる。
【０１２５】
ステップｓ１７で反転モデルにおける第１〜第３角度ｑ１〜ｑ３が求まると、ステップｓ１８に進み、反転モデルにおける第４〜第６角度ｑ４〜ｑ６を求める。まず第５のアーム体ｅ５に固定して設けられる座標系の姿勢を求める。第５のアーム体ｅ５の変換行列Ｒｗｈは、次の（１８）式によって求められる。
【０１２６】
【数１５】
【０１２７】
すなわち変換行列Ｒ０３を転置した行列に、演算座標系Σｓｂに対する基台座標系Σｓｔの変換行列Ｒｕｈを乗算する。変換行列Ｒ０３は、第１〜第３角度ｑ１〜ｑ３が求まっているので、（１）式から求めることができる。また変換行列Ｒｕｈは、上式からその数値解を求めることができる。また変換行列Ｒｕｈは、（６）式から求めることができる。変換行列Ｒｗｈの各成分の数値解が求められると、（１９）式を解くことによって、第４〜第６角度ｑ４〜ｑ６を求めることができる。（１９）式においてαは、第５のアーム体ｅ５と第６のアーム体ｅ６との回転軸線Ｌ５と各アーム体ｅ５，ｅ６の軸線に対して傾斜する角度である。
【０１２８】
【数１６】
【０１２９】
反転モデルにおける第４〜第６角度ｑ４〜ｑ６は、基台代表点が特異点でないかぎり第１〜第３角度ｑ１〜ｑ３の１つの組合せに対して２つの組合せが求められる。したがって３軸交点および基台代表点が特異点でないかぎり、８とおりの第１〜第６角度ｑ１〜ｑ６の組合せを求めることができる。このようにステップｓ１８で反転モデルにおける第４〜第６角度ｑ４〜ｑ６を求めると、ステップｓ１９に進む。
【０１３０】
ステップｓ１９では、反転モデルにおける各アーム体ｅ１〜ｅ６の第１〜第６角度ｑ１〜ｑ６を（２０）式に代入することによって、真のモデルにおける各アーム体ｃ１〜ｃ６の回転角度である第１〜第６角度θ１〜θ６に変換することができる。真のモデルにおける各アーム体ｃ１〜ｃ６の第１〜第６の回転角度θ１〜θ６が求まるとステップｓ２０に進み、動作を終了する。
θ１ ＝ ｑ６
θ２ ＝ ｑ５
θ３ ＝ ｑ４
θ４ ＝ ｑ３
θ５ ＝ ｑ２
θ６ ＝ ｑ１ …（20）
【０１３１】
以上のように演算することで、真のモデルにおける各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を容易に求めることができる。これによって手先装置２２を目標とする位置および姿勢に配置するための各アーム体の回転角度を求めることができる。また演算結果に従って多関節マニピュレータの各アーム体を制御することによって、手先装置２２を目標とする位置および姿勢に配置することができる。上述する演算は、コンピュータを用いて計算してもよく、また手動で計算してもよい。
【０１３２】
演算された各アーム体の回転角度は解析解である。したがって、収束演算および補間近似によって各アーム体の回転角度を求める場合に比べて、短時間でかつ正確に求めることができる。またコンピュータを用いて演算した場合には、その処理能力が低い場合であっても、各アーム体の回転角度を求めることができる。さらに上述したように各回転角度の８つの組合せを得ることができ、エネルギーコストおよび障害物の有無を考慮して８つの組合せのうちから最適な各アーム体の回転角度の組合せを選択することができる。さらに図1に示す多関節マニピュレータ２０の場合には、真のモデルを反転モデルに変換した。これによって容易に各アーム体ｃ１〜ｃ６の第１〜第３の回転角度θ１〜θ６を求めることができる。
【０１３３】
また上述した演算結果を用いて、多関節マニピュレータの各アーム体の代表点間の距離Ｈ０〜Ｈ６、回転軸Ｌ１〜Ｌ６と各アーム体との軸線とが傾斜する角度α、手先装置２２の位置Ｔ（Ｔｘ，Ｔｙ，Ｔｚ）、手先装置２２の姿勢（ｑｏ，ｑａ，ｑｔ）を変数として、真のアーム体ｃ１〜ｃ６の角度θ１〜θ６を表わした解析式を求めておき、この解析式に各変数を示す数値を代入することによって、真のアーム体ｃ１〜ｃ６の角度θ１〜θ６を求めてもよい。
【０１３４】
また本実施の形態では、傾斜関節部における各アーム体の回転軸線と各アーム体との軸線の傾斜角度は、共通の角度であって４５度である多関節マニピュレータの各アーム体の回転角度を求めた。傾斜関節部における各アーム体の回転軸線と各アーム体との軸線の傾斜角度は、共通でなくてもまた４５度以外であっても、各アーム体の回転角度を求めることができる。
【０１３５】
たとえば図７に示す反転モデルの多関節マニピュレータ２０Ａにおいて、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ２と各アーム体ｃ１，ｃ２との軸線との傾斜角度がα度であって、第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３と各アーム体ｃ３，ｃ４との軸線との傾斜角度がβ度であって、第５のアーム体ｅ５と第６のアーム体ｅ６との回転軸線Ｌ５と各アーム体ｃ５，ｃ６との軸線との傾斜角度がγ度である場合であっても、ｙｚ加算座標式Ｐｙｎおよびｙｚ減算式Ｐｚｎに適切な係数を乗算することによって各アーム体ｅ１〜ｅ６の角度ｑ１〜ｑ６を求めることができる。
【０１３６】
また３軸交点で交差する回転軸線以外の残余の回転軸線のうち、少なくとも１つが対応する各アーム体の軸線に対して４５度の場合には、より簡単に第１〜第３角度を求めることができる。
【０１３７】
反転モデルにおいて、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１と各アーム体の軸線との傾斜角度が４５度であって、第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３と各アーム体の軸線との傾斜角度が４５でない場合であっても、第３のアーム体ｅ３と第４のアーム体ｅ４を連結する関節部を図８に示すように等価モデルに変換することによって簡単に求めることができる。具体的には、３軸交点のｙ座標とｚ座標とを加算した式を表わす加算式、ｙ座標からｚ座標を減算した式を表わす減算式を簡略化することができ、さらに容易に第１〜第３角度を求めることができる。
【０１３８】
同様に第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３と、各アーム体の軸線と、の傾斜角度が４５度であって、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１と各アーム体の軸線との傾斜角度が４５でない場合であっても、第１のアーム体ｅ１と第２のアーム体ｅ２を連結する関節部を等価モデルに変換することによって簡単に求めることができる。
【０１３９】
したがって、上述した３軸交点を有する多関節マニピュレータのうち、直列に順に並んで設けられる第１〜第６のアーム体のうち第１〜第３のアーム体を連結する関節部のうち少なくとも１つの傾斜関節部によって連結される２つのアーム体の回転軸線が、これら２つのアーム体の軸線に対して４５度を成して傾斜する多関節マニピュレータを用いることによって、各アーム体の回転角度をより簡単に求めることができる。また本発明において、真のモデルは、基台側に３軸交点が設けられたが、手先側に３軸交点が設けられてもよい。この場合には、反転モデルに変換することなく、容易に各アーム体の回転角度を求めることができる。
【０１４０】
したがって本実施の形態の真のモデルに示すように、固定端部側の回転軸線Ｌ１，Ｌ２，Ｌ３が３軸交点で交差し、第３のアーム体と第４のアーム体との回転軸線Ｌ３と各アーム体の軸線との傾斜角が４５度であって、第５のアーム体と第６のアーム体との回転軸線Ｌ６と各アーム体の軸線との傾斜角が任意の角度である第１の多関節マニピュレータ、または固定端部側の回転軸線Ｌ１，Ｌ２，Ｌ３が３軸交点で交差し、第３のアーム体と第４のアーム体との回転軸線Ｌ３と各アーム体の軸線との傾斜角が任意の角度であって、第５のアーム体と第６のアーム体との回転軸線Ｌ６と各アーム体の軸線との傾斜角が４５度である第２の多関節マニピュレータにおいては、反転モデルに変換し、さらに傾斜角が任意の角度となる関節部を等価モデルに変換することによってより簡単に各アーム体の回転角度を求めることができる。
【０１４１】
また反転モデルに示すように遊端部側の回転軸線Ｌ４，Ｌ５，Ｌ６が３軸交点で交差し、第１のアーム体と第２のアーム体との回転軸線Ｌ１と各アーム体の軸線との傾斜角が４５度であって、第３のアーム体と第４のアーム体との回転軸線Ｌ３と各アーム体の軸線との傾斜角が任意の角度である第３の多関節マニピュレータ、または遊端部側の回転軸線Ｌ４，Ｌ５，Ｌ６が３軸交点で交差し、第１のアーム体と第２のアーム体との回転軸線Ｌ１と各アーム体の軸線との傾斜角が任意の角度であって、第３のアーム体と第４のアーム体との回転軸線Ｌ３と各アーム体の軸線との傾斜角が４５度である第４の多関節マニピュレータにおいては、傾斜角が任意の角度となる関節部を等価モデルに変換することによってより簡単に各アーム体の回転角度を求めることができる。また図８に示すような、第３のアーム体ｅ３と第４のアーム体ｅ４とを連結する関節部が第３および第四のアーム体ｅ３，ｅ４に垂直な場合であっても、各アーム体の回転角度を求めることができる。
【０１４２】
図９は、本発明の他の多関節マニピュレータをモデル化した他のモデルを示す図である。他のモデルは、図９に示す反転モデルの第６のアーム体ｅ６と基台２１とが固定されるモデルである。上述した方法と同様の演算方法によって、第１〜第５のアーム体ｅ１〜ｅ５の回転角度ｑ１〜ｑ５を求めることができる。
【０１４３】
【発明の効果】
以上のように本発明によれば、対象物が目標とする位置および姿勢で配置されるときの、第１〜第６角度を短時間で求めることができる。これによって対象物の移動ごとに各アーム体の角度を求めることが可能である。たとえば予め定められる経路を移動する対象物の位置および姿勢を連続的に入力することによって、その連続的に変化する入力値に応じて各アーム体の角度を順次変化させることができる。これによって対象物を目標とする位置に移動させるときに、高速でかつスムーズに移動させることができる。
【０１４４】
また各角度の解析解を求めることで、対象物が目標とする位置に移動するにあたって、各角度のすべての組合せを求めることができる。６自由度を有する多関節マニピュレータの場合、８つの組合せの回転角度を求めることができる。これによって対象物が目標とする位置および姿勢に配置した状態で、多関節マニピュレータの各アーム体の角度を複数の組合せの中から選択することができる。たとえばエネルギーコストおよび障害物の有無などを考慮して複数の組合せから最適な各アーム体の回転角度の組合せを選択できる。
【０１４５】
また本発明によれば、対象物が目標とする位置および姿勢で配置されるときの、第１〜第５角度を短時間で求めることができる。これによって対象物の移動ごとに各アーム体の角度を求めることが可能である。たとえば予め定められる経路を移動する対象物の位置および姿勢を連続的に入力することによって、その連続的に変化する入力値に応じて各アーム体の角度を順次変化させることができる。これによって対象物を目標とする位置に移動させるときに、高速でかつスムーズに移動させることができる。
【図面の簡単な説明】
【図１】本発明の実施の一形態である多関節マニピュレータ２０を示す正面図である。
【図２】位置決め演算装置３０によって姿勢制御される多関節マニピュレータ２０を示す図である。
【図３】手先装置２２を移動させる場合に位置決め演算装置３０によって姿勢制御される多関節マニピュレータ１を説明するための図である。
【図４】本発明の位置決め演算方法を大略的に示すフローチャートである。
【図５】本発明の実施の一形態の演算処理方法を具体的に示すフローチャートである。
【図６】本発明の多関節マニピュレータをモデル化した真のモデルを示す図である。
【図７】図６に示すの真のモデルを反転した反転モデルを示す図である。
【図８】図７の反転モデルにおける多関節マニピュレータの１つの関節部を等価な関節部に置き換えた等価モデルを示す図である。
【図９】本発明の他の多関節マニピュレータをモデル化した他のモデルを示す図である。
【図１０】姿勢制御される従来の多関節マニピュレータ１の一部を示す図である。
【符号の説明】
２０ 多関節マニピュレータ
２１ 基台
２２ 手先装置
３０ 位置決め演算装置
３１ 入力手段
３２ 演算手段
３３ 出力手段
ｃ１〜ｃ６、ｅ１〜ｅ６ アーム体
ｄ１〜ｄ６、ｆ１〜ｆ６ 関節部
θ１〜θ６、ｑ１〜ｑ６ 回転角度
ｐ１〜ｐ６ 各アーム体の代表点
Ｓ 基台代表点
Ｔ 手先代表点
【発明の属する技術分野】
本発明は、複数のアーム体と各アーム体を回転自在に連結する関節部を有する多関節マニピュレータの各アーム体の相対回転角度を求める位置決め演算方法および位置決め演算装置に関する。
【０００２】
【従来の技術】
従来の技術として、傾斜関節部によって２つのアーム体を連結する多関節マニピュレータが提案されている。傾斜関節部は、連結される２つのアーム体に関して、一方のアームを他方のアームに対して円錐回転自在に連結する。円錐回転するアーム体は、各アーム体の軸線に所定角度傾斜した回転軸線まわりに回転する（たとえば、特許文献１参照〜特許文献３参照）。
【０００３】
【特許文献１】
特開昭６２−１４８１８２号公報
【特許文献２】
特開昭５６−１６３６２４号公報
【特開昭文献３】
特開２００１−１３８２７９号公報
【０００４】
【発明が解決しようとする課題】
たとえば多関節マニピュレータは、直列に設けられる複数のアーム体と、隣接する２つのアーム体を回転自在に連結する複数の関節部とを有する。各関節部は、傾斜関節部と同軸関節部とを有する。同軸関節部は、隣接する２つのアーム体を、その各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する。また傾斜関節部は、アーム体の軸線に対して傾斜する回転軸線まわりに円錐回転自在に連結する。
【０００５】
多関節マニピュレータは、先端部に連結される手先装置を予め定める経路に沿って移動させる。多関節マニピュレータによって手先装置を移動させるためには、手先装置の運動を各アーム体の回転運動に変換する必要がある。すなわち手先装置が配置される位置および姿勢に基づいて、各アーム体の回転角度を求める逆変換を行う必要がある。
【０００６】
特許文献２に開示される従来の技術では、同軸関節部、傾斜関節部、同軸関節部、傾斜関節部、同軸関節部、傾斜関節部、同軸関節部の順に並んで各アーム体を連結する７つの関節部を有する多関節マニピュレータでなければ逆変換を行うことができない。すなわち７つ未満の関節部を有する多関節マニピュレータでは、逆変換して解析解を得ることができない。したがって６つ以下の関節部を有する多関節マニピュレータは、存在しなかった。
【０００７】
仮に６つの関節部を有する多関節マニピュレータが存在したとしても、逆変換するためには複雑な連立方程式を解く必要がある。この複雑な方程式を求める場合には、数式処理プログラムを実行するコンピュータを用いても、その解析解の導出が困難である。したがって６つ以下の関節部を有する多関節マニピュレータの場合には、収束演算法、たとえばニュートンラプソン法などによって各アーム体の回転角度を求めなければならない。このように収束演算によって各アーム体の回転角度を求める場合には、各アーム体の回転角度を短時間で求めることができない。
【０００８】
また特許文献３に開示される従来の技術として、補間近似して各アーム体の回転角度を求める方法がある。この方法は、解析的な順変換式によって、各アーム体の回転角度を決定した場合の多関節マニピュレータの先端部の代表点を求め、複数の代表点における各アーム体の回転角度をデータベース化して予め記憶しておく。逆変換するにあたっては、記憶しているデータベースから補間近似して各アーム体の回転角度を求める。このようにして得られる各アーム体の回転角度は、近似解であるので７つ未満の関節部を有する多関節マニピュレータの先端部を正確な位置および姿勢に位置決めすることができない。
【０００９】
図１０は、補間近似して姿勢制御される多関節マニピュレータ１の一部を示す図である。従来、多関節マニピュレータ１は、手先装置１０が代表点に配置されたときの各アーム体の回転角度を記憶している。たとえば第１の代表点Ａから第２の代表点Ｂに手先装置１０を移動させる場合、多関節マニピュレータ１は、手先装置１０が第１の代表点Ａにあるときの各アーム体の回転角度と、手先装置１０が第２の代表点Ｂにあるときの各アーム体の回転角度とをデータベースから読出す。次に手先装置１０が第１の代表点Ａにあるときの各アーム体の回転角度と、手先装置１０が第２の代表点Ｂにあるときの各アーム体の回転角度とを複数の段階に補間した各回転角度を演算する。多関節マニピュレータ１は、補間した各回転角度に各アーム体を回転させて、手先装置１０を第１の代表点Ａから第２の代表点Ｂに移動させる。
【００１０】
この場合、各アーム体は補間近似した回転角度に動作するので、正確に手先装置１０を位置決めすることができない。したがって手先装置１０を、予め定める移動経路に沿ってスムーズに移動させることができないという問題がある。
【００１１】
また手先装置１０を目標の位置および姿勢にするための、各アーム体の各回転角度は、複数の組合せが存在する。しかし上述したように収束演算または補間近似によって各アーム体の回転角度を求める場合には、各回転角度のうち１つの組合せしか求めることができない。したがって最適なアーム体の回転角度の組合せを求めることができないという問題がある。
【００１２】
以上のように従来の技術では、６つ以下の関節部を有する多関節マニピュレータでは、手先装置１０を目標とする位置および姿勢に配置するときに各アーム体の回転角度を解析的に求めることができない。したがって手先装置を目標とする位置および姿勢に高速かつ正確に配置することができない。また各アーム体の角度の複数の組合せを求めることができないという問題がある。
【００１３】
したがって、本発明の目的は、対象物が目標とする位置および姿勢に配置されるときの各アーム体の角度を解析的に求めることができる位置決め演算方法および位置決め演算装置を提供することである。
【００１４】
【課題を解決するための手段】
本発明は、基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に回転自在に連結され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、基台と第１のアーム体との回転軸線と、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する３軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算方法であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力工程と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算する工程と、
反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記３軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求める３軸交点演算工程と、
反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記３軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標に反転モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標から反転モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求める第１〜第３角度演算工程と、
これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、第２のアーム体に対する第１のアーム体の回転角度である第５角度、および第１のアーム体に対する基台の回転角度である第６角度を求める第４〜第６角度演算工程と、
反転モデルの第１〜第６角度を真のモデルの第１〜第６角度に変換する工程とを含むことを特徴とする位置決め演算方法である。
本発明は、基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に回転自在に連結され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、基台と第１のアーム体との回転軸線と、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する３軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算装置であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力手段と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算し、反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記３軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記における３軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標に反転モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標から反転モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求め、これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、第２のアーム体に対する第１のアーム体の回転角度である第５角度、および第１のアーム体に対する基台の回転角度である第６角度を求め、反転モデルの第１〜第６角度を真のモデルの第１〜第６角度に変換する演算手段と、
演算手段が演算した演算結果を出力する出力手段とを備えることを特徴とする位置決め演算装置である。
【００１５】
本発明に従えば、姿勢制御対象とする多関節マニピュレータの第６のアーム体に対象物が固定され、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、真のモデルの座標系において対象物の位置および姿勢が入力され、反転モデルの座標系から見た基台の位置および姿勢を演算し、３軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、反転モデルの座標系に対して、第１〜第３角度を変数として用い、ｘ座標式とｙｚ加算座標式とｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求め、さらに第４〜第６角度を求め、こうして得られた反転モデルの第１〜第６角度を真のモデルの第１〜第６角度に変換する。
したがって対象物の位置および姿勢が決定されると、３軸交点の座標を一意的に求めることができる。
【００１６】
各角度を容易に求めるために、多関節マニピュレータを真のモデルとし、その真のモデルに対する反転モデルを仮定して各アーム体の角度を求める。
【００１７】
このように第１〜第６角度を短時間で正確に求めることができる。また対象物が目標とする位置に移動して希望する姿勢をとるにあたり、各アーム体の角度のすべての組合せを求めることができる。
【００１８】
本発明は、基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に固定され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する２軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算方法であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力工程と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算する工程と、
反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記２軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求める２軸交点演算工程と、
反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標に反転モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標から反転モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求める第１〜第３角度演算工程と、
これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、および第２のアーム体に対する第１のアーム体の回転角度である第５角度を求める第４〜第５角度演算工程と、
反転モデルの第１〜第５角度を真のモデルの第１〜第５角度に変換する工程とを含むことを特徴とする位置決め演算方法である。
本発明は、基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に固定され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する２軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算装置であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力手段と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算し、反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記２軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標に反転モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標から反転モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求め、これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、および第２のアーム体に対する第１のアーム体の回転角度である第５角度を求め、反転モデルの第１〜第５角度を真のモデルの第１〜第５角度に変換する演算手段と、
演算手段が演算した演算結果を出力する出力手段とを備えることを特徴とする位置決め演算装置である。
【００１９】
本発明に従えば、第１のアーム体は基台に固定され、前述と同様に反転モデルを仮定し、２軸交点に関して反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、さらに座標式を解いて第１〜第３角度を求め、さらに第４および第５角度を求め、こうして得られた反転モデルの第１〜第５角度を真のモデルの第１〜第５角度に変換する。
【００２０】
各角度を容易に求めるために、前述と同様に真のモデルに対する反転モデルを仮定して、反転モデルの第１〜第５角度を求め、これによって真のモデルの第１〜第５角度を容易に求める。
【００２１】
このように、前述と同様に第１〜第５角度を短時間で正確に求めることができ、対象物が目標とする位置、姿勢に対する各角度のすべての組合せを求めることができる。
【００２２】
本発明は、適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記３軸交点演算工程では、前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を、
等価モデルにおける前記３軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、
前記第１〜第３角度演算工程では、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標に等価モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標から等価モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算する工程を含むことを特徴とする。
本発明は、適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記演算手段は、前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を、等価モデルにおける前記３軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標に等価モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標から等価モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算することを特徴とする。
【００２３】
本発明に従えば、第３のアーム体と第４のアーム体とを連結する関節部が傾斜関節部であり、この関節部を第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、この等価モデルの座標系が対象物に固定され、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢とし、３軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、等価モデルにおいて第１〜第３角度を変数として座標式を解いて等価モデルの第１〜第３角度を求めて、反転モデルの第１〜第３角度に変換する。
【００２４】
本発明は、適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記２軸交点演算工程では、前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を、等価モデルにおける前記２軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、
前記第１〜第３角度演算工程では、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標に等価モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標から等価モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算する工程を含むことを特徴とする。
本発明は、適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記演算手段は、前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を、等価モデルにおける前記２軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標に等価モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標から等価モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算することを特徴とする。
【００２５】
本発明に従えば、各角度を容易に求めるために、真のモデルに対する反転モデルと、第３および第４のアーム体の関節部を傾斜関節部の代りに垂直な関節部に置き換えた等価モデルを仮定することによって、３軸交点および２軸交点をそれぞれ有する多関節マニピュレータにおける第１〜第６角度および第１〜第５角度をさらに容易に求めることができる。
【００３６】
【発明の実施の形態】
図１は、本発明の実施の一形態である多関節マニピュレータ２０を示す正面図である。本発明の実施の一形態である多関節マニピュレータは、複数のアーム体と、アーム体を回転自在に連結する複数の関節部とを有する。各アーム体は、それぞれ連結されて直列状に延びるアーム連結体を構成する。
【００３７】
各関節部は、同軸関節部と傾斜関節部とが組み合わされて設けられる。同軸関節部は、隣接する２つのアーム体を、その各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する。また傾斜関節部は、隣接する２つのアーム体のうち一方を、各アーム体の軸線に対して傾斜する回転軸線まわりに円錐回転自在に連結する。本発明において、「円錐回転」は、回転軸線を中心とし、２つのアーム体の連結部分を頂点とする仮想円錐の周面に沿って相対的に回転することである。また本発明において、「回転」は、回転軸線まわりに３６０度以下の角度で角変位する状態も含む。
【００３８】
このような同軸関節および傾斜関節を含む多関節マニピュレータは、各アーム体をそれぞれ回転させることによって、蛇のような動きをさせて、３次元位置決めが可能である。たとえば設備等が複雑に入り組み、作業経路が複雑な場合、であっても、好適に本発明の多関節マニピュレータを動作させることができる。
【００３９】
多関節マニピュレータは、産業用ロボットに用いられる。たとえば多関節マニピュレータの遊端部に各種手先装置、いわゆるエンドエフェクタを連結し、手先装置を目標位置に配置して手先装置を動作させることによって、狭隘な作業空間におけるハンドリング、シーリング、塗装またはアーク溶接などを行うことができる。
【００４０】
図１に示すように、多関節マニピュレータ２０は、６自由度を有して実現される。多関節マニピュレータ２０は、直列に設けられる第１〜第６のアーム体ｃ１〜ｃ６と、隣接する２つのアーム体を回転自在に連結する複数の関節部ｄ１〜ｄ６とを有する。各アーム体ｃ１〜ｃ６は、連結されてアーム連結体を構成する。各関節部は、３つの同軸関節部ｄ１，ｄ３，ｄ５と３つの傾斜回転部ｄ２，ｄ４，ｄ６とを有する。傾斜回転部ｄ２，ｄ４，ｄ６は、隣接する２つの各アーム体の軸線に対して４５度の角度を成して傾斜する回転軸線まわりに回転自在に２つのアーム体を連結する。
【００４１】
複数のアーム体ｃ１〜ｃ６は、直列方向に並んで配置される。そのうち一端部に第１のアーム体ｃ１が配置され、第２〜第６のアーム体ｃ２〜ｃ６が順に連結される。第６のアーム体ｃ６には、手先装置２２が連結される。第１〜第６のアーム体ｃ１〜ｃ６は、図１に示すように、それぞれの軸線が同軸に配置されて一直線状に延びる状態に変形可能である。
【００４２】
第１のアーム体ｃ１は、その一端部２３が同軸関節部ｄ１によって、予め定める対象物である基台２１に連結される。基台２１は、予め定める固定位置に固定されても、移動可能に設けられてもよい。第１のアーム体ｃ１は、その軸線と同軸の回転軸線Ｌ１まわりに基台２１に対して回転自在に連結される。また第１のアーム体ｃ１のうち、基台２１と反対側の端部２４には、傾斜関節部ｄ２によって第２のアーム体ｃ２が連結される。
【００４３】
第１のアーム体ｃ１と第２のアーム体ｃ２とは、第１のアーム体ｃ１および第２のアーム体ｃ２の軸線に対して、４５度の角度を成して傾斜する回転軸線Ｌ２まわりに回転自在に連結される。第２のアーム体ｃ２のうち、第１のアーム体ｃ１と反対側の端部２５には、同軸関節部ｄ３によって第３のアーム体ｃ３が連結される。
【００４４】
第２のアーム体ｃ２と第３のアーム体ｃ３とは、互いの軸線と同軸な回転軸線Ｌ３まわりに回転自在に連結される。第３のアーム体ｃ３のうち、第２のアーム体ｃ２と反対側の端部２６には、傾斜関節部ｄ４によって第４のアーム体ｃ４が連結される。
【００４５】
第３のアーム体ｃ３と第４のアーム体ｃ４とは、第３のアーム体ｃ３および第４のアーム体ｃ４の軸線に対して、４５度の角度を成して傾斜する回転軸線Ｌ４まわりに回転自在に連結される。第４のアーム体ｃ４のうち、第３のアーム体ｃ３と反対側の端部２７には、同軸関節部ｄ５によって第５のアーム体ｃ５が連結される。
【００４６】
第４のアーム体ｃ４と第５のアーム体ｃ５とは、互いの軸線と同軸な回転軸線Ｌ５まわりに回転自在に連結される。第５のアーム体ｃ５のうち、第４のアーム体ｃ４と反対側の端部２８には、傾斜関節部ｄ６によって第６のアーム体ｃ６が連結される。
【００４７】
第５のアーム体ｃ５と第６のアーム体ｃ６とは、第５のアーム体ｃ５および第６のアーム体ｃ６の軸線に対して、４５度の角度を成して傾斜する回転軸線Ｌ６まわりに回転自在に連結される。また本発明の実施の形態では、第６のアーム体ｃ６のうち、第５のアーム体ｃ５と反対側の端部には、手先装置２２が連結される。手先装置２２は、たとえば部品を把持および把持解除するためのハンドリング装置であってもよい。
【００４８】
このように複数のアーム体ｃ１〜ｃ６は、同軸関節部と傾斜関節部とが交互に配置される。また各アーム体ｃ１〜ｃ６の各軸線は、すべての軸線が同軸に配置されて一直線状に連結可能に形成される。多関節マニピュレータ２０は、基台２１と第１のアーム体との回転軸線Ｌ１と、第１のアーム体と第２のアーム体との回転軸線Ｌ２と、第２のアーム体と第３のアーム体との回転軸線Ｌ３とが交差する３軸交点を有する。
【００４９】
各アーム体ｃ１〜ｃ６は、各アーム体を回転駆動する回転駆動モータを内蔵する。また各アーム体ｃ１〜ｃ６は、その軸線を挿通する中空空間が形成される。この中空空間に１または複数の配線が内蔵される。各配線は、回転駆動モータを回転させるための電力、回転駆動モータへの回転指示、手先装置２２へ圧縮空気などを供給する。
【００５０】
各アーム体を回転軸線まわりに回転する回転機構は、従来の機構と同様の機構であってもよい。たとえば一方のアーム体と他方のアーム体とを回転軸線まわりに回転自在に支持するベアリングと、中空形状の波動歯車機構たとえばハーモニックドライブ（登録商標）とを備える。波動歯車機構は、入力側部材と出力側部材とを備え、それらが相対的に回転する。一方のアーム体は、入力側部材に連結され、他方のアーム体は、出力側部材に連結される。
【００５１】
回転駆動モータからの回転が入力側部材に与えられると、入力側部材と出力側部材とが相対的に回転する。これによって一方のアーム体と他方のアーム体とを相対的に回転させることができる。このような回転機構が、各アーム体ｃ１〜ｃ６に設けられることによって、隣接する２つのアーム体を、相対的に回転させることができる。さらに中空形状のハーモニックドライブ機構を用いることによって、連結される２つのアーム体の各挿通空間に各配線を挿通させた状態で、２つのアーム体を回転することができる。さらに多関節マニピュレータ全体、または挿通空間内を挿通する各配線を耐水性、耐熱性あるいは耐衝撃性のある材料によって構成されるカバーで覆うことによって、防塵、防爆、防水性を必要とする作業環境に好適に用いることができる。
【００５２】
図１に示すように、多関節マニピュレータ２０は、さらに各アーム体の相対回転角度を求める位置決め演算装置３０を備える。位置決め演算装置３０は、目標とする位置および姿勢に手先装置２２を移動させるにあたって、手先装置２２を目標とする位置および姿勢に移動させるのに必要な各アーム体ｃ１〜ｃ６の相対回転角度θ１〜θ６を求める。
【００５３】
位置決め演算装置３０は、入力手段３１と、演算手段３２と、出力手段３３とを有する。入力手段３１は、演算座標系における手先装置２２の位置および姿勢を入力する。また演算手段３２は、入力手段３１から入力された手先装置２２の位置および姿勢と、多関節マニピュレータの諸形状とに基づいて、手先装置２２を目標とする位置および姿勢に移動させるのに必要な各アーム体の相対回転角度θ１〜θ６を演算する。出力手段３３は、演算手段３３による演算結果に基づいて、各アーム体を回転するための電力と回転指令を与え、演算結果に応じて各アーム体ｃ１〜ｃ６を回転させる。
【００５４】
位置決め演算装置３０は、たとえばコンピュータによって実現される。入力手段３１は、キーボード、マウスなどのポインティングデバイスおよび作業者が多関節マニピュレータ２を直接制御するコントローラであるティーチペンダントによって実現される。また入力手段３１は、位置決め演算装置３０に接続される他の装置から手先装置２２の位置および姿勢を入力されてもよい。出力手段３３は、各駆動モータを回転させるための回転指令信号を、演算結果に応じて生成し、生成した回転指令信号を各駆動モータに与える。出力手段３３は、たとえば駆動回路によって実現される。位置決め演算装置３０は、多関節マニピュレータ２０に内蔵されていてもよく、また別途設けられていてもよい。
【００５５】
また演算手段３２は、演算プログラムを実行する中央処理装置（Central
Processing Unit）を含んで実現される。中央処理装置は、予め演算プログラムを記憶していてもよく、またコンピュータ読取可能な記憶媒体に記録される演算プログラムを読み取ることによって実行してもよい。演算プログラムは、後述する演算方法を示す。
【００５６】
図２は、位置決め演算装置３０によって姿勢制御される多関節マニピュレータ２０を示す図である。位置決め演算装置３０は、解析計算式に基づいて、手先装置２２を目的の位置および姿勢に配置させるための各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を数値的に求める。上述する６軸多関節マニピュレータ２０では、手先装置２２を目標とする位置および姿勢に配置する場合、各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６は、８通りの組合せがある。演算装置３０がこの各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を解析計算式に基づいて求めることによって、この８通りの組合せを同時に求めることができる。図２（１）〜図２（８）に示すように、手先装置２２を予め定める位置および姿勢に配置する場合の各アーム体の回転角度の組合せを複数求めることができるので、エネルギーコストおよび障害物の有無に応じて、アーム体ｃ１〜ｃ６の回転角度θ１〜θ６の組合せを選択することができる。
【００５７】
図３は、手先装置２２を移動させる場合に位置決め演算装置３０によって姿勢制御される多関節マニピュレータ２０を説明するための図である。たとえば第１の代表点Ａから第２の代表点Ｂに手先装置２２を移動させる場合には、第１の代表点Ａと第２の代表点Ｂとの間を手先装置２２が通過する通過点Ｕ１〜Ｕ３を求め、通過点Ｕ１〜Ｕ３に配置されるときの各アーム体の回転角度を求めることができる。これによって手先装置２２が移動する経路に沿って通過点を順に設定し、通過点ごとに各アーム体の回転角度を求めることによって、指定した経路に沿って手先装置２２を円滑に移動させることができる。
【００５８】
また位置決め演算装置３０は、従来のように収束演算によって、各アーム体の回転角度を求める必要がないので短時間で求めることができる。また演算手段３１に高い処理能力を必要としない。したがって手先装置２２の移動時であっても各アーム体の回転角度を演算することができる。さらに従来の技術のように、代表点Ａ，Ｂにおける各アーム体の回転角度をデータベース化して記憶する必要がなく、必要なデータ記憶容量を低減させることができる。
【００５９】
図４は、本発明の位置決め演算方法を大略的に示すフローチャートである。まず、ステップｓ１で、制御対象である多関節マニピュレータ２０の各アーム体の寸法およびアーム体を連結する関節の種類などの諸条件が与えられ、各アーム体の回転角度を演算可能な状態となると、ステップｓ２に進む。
【００６０】
ステップｓ２では、多関節マニピュレータ２０が移動させる対象物の位置および姿勢を取得する。たとえば対象物は、第６のアーム体ｃ６の先端に連結される手先装置２２であり、固定して設けられる座標系に対する手先装置２２の位置および姿勢が入力される。このように対象物を配置すべき目標の位置および姿勢を取得するとステップｓ３に進む。
【００６１】
ステップｓ３では、各アーム体のうち直列方向に順に並ぶ３つのアーム体のそれぞれの回転軸線が互いに交差する３軸交点の座標を求める。３軸交点は、対象物の位置および姿勢が決定されると、その座標を一意的に求めることができる。したがってステップｓ２によって取得した対象物の位置および姿勢に基づいて、３軸交点の座標を求めることができる。３軸交点の座標を求めるとステップｓ４に進む。
【００６２】
ステップｓ４では、３軸交点を通過する３つの回転軸線以外の残余の３つの回転軸線まわりに回転する３つのアーム体の相対的な回転角度である第１〜第３角度を変数として用いて、３軸交点の座標を表わす座標式を生成する。すなわち３軸交点のｘ座標を表わすｘ座標式と、ｙ座標を表わすｙ座標式と、ｚ座標を表わすｚ座標式とを生成する。次に、ｙ座標式およびｚ座標式に基づいて、ｙ座標にｚ座標を加算した加算値を表すｙｚ加算座標式と、ｙ座標からｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を生成する。このように各座標式を生成すると、ステップｓ５に進む。
【００６３】
ステップｓ５では、ステップｓ３で求められる３軸交点の座標と、ステップｓ４で求められたｘ座標式、ｙｚ加算座標式、ｙｚ減算座標式に基づいて、第１〜第３角度を求める。さらに第１〜第３角度に基づいて、残りの第４〜第６角度を求める。このようにして各アーム体の回転角度を求めると、ステップｓ６に進み、動作を終了する。
【００６４】
図１に示す多関節マニピュレータ２０の場合、各角度をより容易に求めるために、本実施の形態における多関節マニピュレータ２０を真のモデルとし、真のモデルの座標系の原点は基台に固定され、その真のモデルに対してｚ軸に反転した反転モデルと、反転モデルに対して等価なモデルであって１つの関節部の構成が異なる等価モデルとを仮定する。第６のアーム体ｃ６には、対象物としての手先装置２２が固定される。反転モデルは、対象物に対して基台の位置を制御する形態のモデルであり、反転モデルの座標系の原点は対象物に固定される。等価モデルは、後述の図８のように、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルであり、等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としている。真のモデルを、反転モデルおよび等価モデルに変換して各アーム体の回転角度を求めることによって、より容易に各アーム体の回転角度を求めることができる。
【００６５】
図５は、図１に示す多関節マニピュレータ２０の位置決め演算処理方法を具体的に示すフローチャートである。演算処理方法として、ステップｓ１１で真のモデルにおける多関節マニピュレータの各アーム体の寸法およびアーム体を連結する関節の種類などの諸条件が与えられ、各アーム体の回転角度を演算可能な状態となると、ステップｓ１２に進む。
【００６６】
ステップｓ１２では、真のモデルにおける基台２１に対する手先装置２２の位置および姿勢を取得し、ステップｓ１３に進む。ステップｓ１３では、真のモデルにおける座標系および位置を反転モデルにおける座標系および位置に変換する。次に反転モデルにおける手先装置２２に対する基台２１の位置および姿勢を演算する。すなわち多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算する。その後、ステップｓ１４に進む。ステップｓ１４では、反転モデルの座標系に対する基台２１の位置および姿勢に基づいて、反転モデルにおける３軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標から成る位置を求め、ステップｓ１５に進む。
【００６７】
ステップｓ１５では、等価モデルにおいて、３軸交点を通過する３つの回転軸線以外の残余の３つの回転軸線まわりに回転する３つのアーム体の相対的な回転角度である第１〜第３角度を変数として３軸交点の位置を示す座標式を生成する。具体的には等価モデルにおいて、３軸交点のｘ座標を表わすｘ座標式と、ｙ座標を表わすｙ座標式と、ｚ座標を表わすｚ座標式とを生成する。次に、ｙ座標式およびｚ座標式に基づいて、ｙ座標にｚ座標を加算した加算値を表すｙｚ加算座標式と、ｙ座標からｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を生成する。このような数式が求められるとステップｓ１６に進む。等価モデルの座標系に対する基台の位置および姿勢に基づいて、等価モデルにおける３軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求める。さらに等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標に等価モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標から等価モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を得る。
【００６８】
ステップｓ１６では、ステップｓ１４において求められた反転モデルにおける３軸交点の位置と、ステップｓ１５によって求められたｘ座標式と、ｙｚ加算座標式と、ｙｚ減算座標式とに基づいて、等価モデルにおける第１〜第３角度を求め、ステップｓ１７に進む。
【００６９】
ステップｓ１７では、ステップｓ１６で求められた第１〜第３角度を等価モデルから反転モデルに変換し、反転モデルにおける第１〜第３角度を演算する。反転モデルにおける第１〜第３角度が求まるとステップｓ１８に進む。ステップｓ１８では、ステップｓ１７で求められた反転モデルの第１〜第３角度に基づいて、反転モデルの第４〜第６角度を求め、ステップｓ１９に進む。反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、第２のアーム体に対する第１のアーム体の回転角度である第５角度、および第１のアーム体に対する基台の回転角度である第６角度を求める。ステップｓ１９では、ステップｓ１７およびステップｓ１８で求められた反転モデルにおける第１〜第６角度ｑ１〜ｑ６を真のモデルにおける第１〜第６角度θ１〜θ６に変換し、ステップｓ２０に進み、動作を終了する。
【００７０】
図６は、本発明の多関節マニピュレータ２０をモデル化した真のモデルを示す図である。多関節マニピュレータ２０には、基台２１に固定的に設けられ、相互に直交する演算座標系Σｒｂが設定される。演算座標系Σｒｂは、直交３軸の座標系であって、予め定める演算原点を通過し第１の方向に延びる軸である演算ｘ軸、演算原点を通過し演算ｘ軸に直交する第２の方向に延びる軸である演算ｙ軸、および演算原点を通過し演算ｘ軸および演算ｙ軸にともに直交する第３の方向に延びる軸である演算ｚ軸を有する。
【００７１】
また各アーム体ｃ１〜ｃ６、手先装置２２は、それぞれに固定して予め定める各対象座標系が設定される。各対象座標系は、直交３軸の座標系であって、対応する各アーム体ｃ１〜ｃ６または手先装置２２の代表点を原点とする。またその原点を通過し第１の方向に延びる軸である対象ｘ軸、対象原点を通過し対象ｘ軸に直交する第２の方向に延びる軸である対象ｙ軸、および対象原点を通過し対象ｘ軸および対象ｙ軸にともに直交する第３の方向に延びる軸である対象ｚ軸を有する。
【００７２】
図６に示す真のモデルでは、第１のアーム体ｃ１と基台２１との回転軸線Ｌ１に沿って延びる仮想線が通過する一点であって、基台２１に定める基台代表点Ｓを原点とする演算座標系Σｒｂが設定される。また手先装置２２と第６のアーム体ｃ６とは固定されているので、手先装置２２の位置および姿勢を決定することと、第６のアーム体ｃ６の位置および姿勢を決定することとは等価である。したがって、手先装置２２の位置は、第４のアーム体ｃ４と第５のアーム体ｃ５との回転軸線Ｌ５に沿って延びる仮想線が通過する一点であって、第６のアーム体ｃ６の代表点Ｔの座標で表わされる。
【００７３】
また手先装置２２に固定して設けられる対象座標系である手先座標系Σｒｔは、第６のアーム体ｃ６の代表点Ｔを原点とする直交３軸の座標系である。各アーム体ｃ１〜ｃ６が互いに相対的に回転することによって、演算座標系Σｒｂに対して第６のアーム体ｃ６の代表点Ｔが移動する。
【００７４】
代表点Ｔの位置（Ｔｘ，Ｔｙ，Ｔｚ）は、代表点Ｔにおける演算ｚ軸の座標Ｔｘ、演算ｙ座標の座標Ｔｙおよび演算ｚ軸座標Ｔｚで表わす。また各アーム体の軸線が同軸に配置されて一直線状に連結される一連結状態から各アーム体ｃ１〜ｃ６が互いに相対的に回転することによって、演算座標系Σｒｂに対して手先座標系Σｒｔが回転し、第６のアーム体ｃ６の姿勢が変化する。第６のアーム体ｃ６の姿勢（ｑｏ，ｑａ，ｑｔ）は、手先座標系Σｒｔが、演算ｚ軸まわりに回転する角度ｑｏ、演算ｙ軸まわりに回転する角度ｑａ、演算ｚ軸まわりに回転する角度ｑｔで表わす。
【００７５】
各アーム体ｃ１〜ｃ６は、前記一連結状態を基準状態とし、前記一連結状態から基台２１に対する第１のアーム体ｃ１の回転角度を第１角度θ１とし、第１のアーム体ｃ１に対する第２のアーム体ｃ２の回転角度を第２角度θ２とし、第２のアーム体ｃ２に対する第３のアーム体ｃ３の回転角度を第３角度θ３とし、第３のアーム体ｃ３に対する第４のアーム体ｃ４の回転角度を第４角度θ４とし、第４のアーム体ｃ４に対する第５のアーム体ｃ５の回転角度を第５角度θ５とし、第５のアーム体ｃ５に対する第６のアーム体ｃ６の回転角度を第６角度θ６とする。
【００７６】
図７は、図６に示すの真のモデルを反転した反転モデルの多関節マニピュレータ２０Ａを示す図である。また図８は、図７の反転モデルにおける多関節マニピュレータ２０Ａの１つの関節部を等価な関節部に置き換えた等価モデルの多関節マニピュレータ２０Ｂを示す図である。図７および図８に示すモデルは、理解を容易にするために、真のモデルに対して異なる名称および参照符号とする。
【００７７】
すなわち反転および等価モデルにおける第１のアーム体ｅ１は、真のモデルにおける第６のアーム体ｃ６に対応する。また反転および等価モデルにおける第２のアーム体ｅ２は、真のモデルにおける第５のアーム体ｃ５に対応する。また反転および等価モデルにおける第３のアーム体ｅ３は、真のモデルにおける第４のアーム体ｃ４に対応する。また反転および等価モデルにおける第４のアーム体ｅ４は、真のモデルにおける第３のアーム体ｃ３に対応する。また反転および等価モデルにおける第５のアーム体ｅ５は、真のモデルにおける第２のアーム体ｃ２に対応する。また反転および等価モデルにおける第６のアーム体ｅ６は、真のモデルにおける第１のアーム体ｃ１に対応する。このように反転および等価モデルにおける第１〜第６のアーム体ｅ１〜ｅ６は、真のモデルにおける第６〜第１のアーム体ｃ６〜ｃ１に反転してそれぞれ対応する。
【００７８】
また同様に反転モデルにおける各関節部ｆ１〜ｆ６、各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６および各アーム体の軸線方向寸法ｉ０〜ｉ６もまた、真のモデルに対して異なる参照符号を付する。
【００７９】
また等価モデルにおける多関節マニピュレータ２０Ｂは、反転モデルにおける多関節マニピュレータ２０Ａに対して、第３のアーム体ｅ３と第４のアーム体ｅ４とを連結する関節部が異なる。等価モデルにおける多関節マニピュレータ２０Ｂでは、この関節部Ｆ３は、第３および第４のアーム体ｅ３，ｅ４の軸線に垂直な回転軸線Ｌ３まわりに各アーム体ｅ３，ｅ４の軸線を回転自在に連結する。この関節部ｆ３によって、第３および第４のアーム体ｅ３，ｅ４は、互いの軸線を含む一平面上を回転する。
【００８０】
反転モデルでは、反転モデルにおける第１のアーム体ｅ１、すなわち真のモデルの第６のアーム体ｃ６の代表点Ｔを、演算座標系Σｓｂの演算原点とする。また反転モデルにおいて、基台２１に固定して設けられる対象座標系である基台座標系Σｓｔは、基台代表点Ｓを原点とする。反転モデルは、各アーム体ｅ１〜ｅ６の軸線が同軸に配置されて一直線状に連結される一連結状態において、基台座標系Σｓｔの対象ｘ軸、対象ｙ軸および対象ｚ軸は、演算座標系Σｓｂの演算ｘ軸、演算ｙ軸および演算ｚ軸に対してそれぞれ同一方向に延びる。また一連結状態において演算座標系Σｓｂおよび基台座標系Σｓｔは、各アーム体ｃ１〜ｃ６の軸線方向に沿う方向に演算ｚ軸および対象ｚ軸が延びる。演算ｚ軸および対象ｚ軸は、演算座標系Σｓｂの原点から基台座標系Σｓｔの原点に向かう方向を正とする。
【００８１】
本発明の実施の形態の多関節マニピュレータ２０では、手先装置２２を固定し、基台２１を変位可能とした場合に、ステップｓ１〜ステップｓ６を実行する。すなわち反転モデルにおいて、基台２１を対象物とみなして、目標の位置および姿勢に移動させるための各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６を求める。
【００８２】
次に反転モデルとして求めた各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６を、真のモデルに対応するように変換する。これによって真のモデルの多関節マニピュレータ２０における、手先装置２２を目標の位置および姿勢に移動させるための各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を求めることができる。
【００８３】
多関節マニピュレータにおいて、基台側すなわち固定側に３軸交点が存在する場合よりも、手先装置側すなわち遊端部側に３軸交点が存在する場合のほうが、各アーム体の回転角度を容易に求めることができる。したがって本実施の多関節マニピュレータ２０のように、基台側に３軸交点がある場合には、まず反転モデルに変換して各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６を求め、その後、求められた反転モデルにおける各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６を真のモデルに再変換することによって、各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を容易に求めることができる。
【００８４】
以下、図５に示すフローチャートを参照して演算方法の手順を説明する。まずステップｓ１１で、各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を求めるための準備工程を行う。準備工程では、各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を求めるにあたって、真のモデルにおける各アーム体ｃ１〜ｃ６の各軸線方向寸法Ｈ０〜Ｈ６、第１のアーム体ｃ１と第２のアーム体ｃ２との回転軸線Ｌ２が各軸線から傾斜する角度、第３のアーム体ｃ３と第４のアーム体ｃ４との回転軸線Ｌ４が各軸線から傾斜する角度、第５のアーム体ｃ５と第６のアーム体ｃ６との回転軸線Ｌ６が各軸線から傾斜する角度とを取得し、多関節マニピュレータの諸条件を取得する。本実施の形態では、軸線から傾斜する回転軸線Ｌ２，Ｌ４，Ｌ６が、各アーム体の軸線から傾斜する角度αがともに４５度の場合について説明する。このように多関節マニピュレータの諸条件が決定されると、ステップｓ１２に進む。
【００８５】
ステップｓ１２では、真のモデルにおける演算座標系Σｒｂに対する手先装置２２の位置および姿勢を取得する。言換えると、第６のアーム体ｃ６の代表点Ｔの位置（Ｔｘ，Ｔｙ，Ｔｚ）および姿勢（ｑｏ，ｑａ，ｑｔ）を取得し、ステップｓ１３に進む。
【００８６】
ステップｓ１３では、まず真のモデルを反転モデルに変換するための変換行列を生成し、変換行列に基づいて、反転モデルにおける基台２１の姿勢を求める。反転モデルにおける基台２１の姿勢は、基台座標系Σｓｔが演算座標系Σｓｂの演算ｘ軸まわりに回転する角度ｑｘ、基台座標系Σｓｔが演算座標系Σｓｂの演算ｙ軸まわりに回転する角度ｑｙ、基台座標系Σｓｔが演算座標系Σｓｂの演算ｚ軸まわりに回転する角度ｑｚで表わされる。
【００８７】
基台２１の姿勢（ｑｘ，ｑｙ，ｑｚ）が求まると、反転モデルにおける基台代表点Ｓの位置を求める。この基台代表点Ｓの位置は、演算座標系Σｓｂの演算ｘ軸Ｐｘ、演算ｙ軸Ｐｙ、演算ｚ軸の座標Ｐｚで表わされる。反転モデルにおける基台２１の姿勢（ｑｘ，ｑｙ，ｑｚ）と基台代表点Ｓの位置（Ｐｘ，Ｐｙ，Ｐｚ）とを求めるとステップｓ１４に進む。
【００８８】
ステップｓ１４では、３軸交点の位置を求める。３軸交点は、第５のアーム体の代表点ｐ５であって、図１に示すマニピュレータ２０の場合、第５のアーム体ｅ５と第６のアーム体ｅ６との回転軸線Ｌ５と第５のアーム体ｅ５と第６のアーム体ｅ６の軸線とが交差する点であって、第５のアーム体ｅ５と第６のアーム体ｅ６とを連結する部分に設けられる。この３軸交点の位置を求めるために、まず演算座標系を第５のアーム体ｅ５に固定して設けられる対象座標系に変換する変換行列を求める。この変換行列Ｒ０５は、次式によって表わされる。
【００８９】
【数１】
ここで、Ｒ０１〜Ｒ０５は、演算座標系を各アーム体ｅ１〜ｅ５に固定して設けられる対象座標系に変換する変換行列Ｒ０１〜Ｒ０３である。またαは上述したように傾斜関節部における各アーム体の回転軸線が各アーム体の軸線に対して傾斜する角度である。Ｒ０１におけるαは、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１が各アーム体ｅ１，ｅ２の軸線に対して傾斜する角度の値を示す。またＲ０３におけるαは、第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３が各アーム体ｅ３，ｅ４の軸線に対して傾斜する角度の値を示す。またＲ０５におけるαは、第５のアーム体ｅ５と第６のアーム体ｅ６との回転軸線Ｌ５が各アーム体ｅ５，ｅ６の軸線に対して傾斜する角度の値を示す。またｑ１〜ｑ６は、各アーム体の回転角度であって、第１のアーム体ｅ１に対する第２のアーム体ｅ２の回転角度を、第１角度ｑ１とし、第２のアーム体ｅ２に対する第３のアーム体ｅ３の回転角度を第２角度ｑ２とし、第３のアーム体ｅ３に対する第４のアーム体ｅ４の回転角度を第３角度ｑ３とする。また第４のアーム体ｅ４に対する第５のアーム体ｅ５の回転角度を第４角度ｑ４とし、第５のアーム体ｅ５に対する第６のアーム体ｅ６の回転角度である第５角度ｑ５とし、第６のアーム体ｅ６に対する基台２１の回転角度である第６角度ｑ６とする。各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６は、各アーム体ｅ１〜ｅ６が一直線状に連結される一連結状態において、各アーム体ｅ１〜ｅ６の回転角度ｑ１〜ｑ６をゼロとする。
【００９１】
またＲｚ［θ］、Ｒｙ［θ］、Ｒｘ［θ］は、演算座標系に対して対象座標系の姿勢を回転させるための回転行列である。Ｒｚ［θ］は、対象座標系を演算ｚ軸まわりにθ度回転させる回転行列であり、次式にその成分を示す。またＲｙ［θ］は、対象座標系を演算ｙ軸まわりにθ度回転させる回転行列であり次式にその成分を示す。またＲｘ［θ］は、対象座標系を演算ｘ軸まわりにθ度回転させる回転行列でありその成分を次式に示す。
【００９２】
【数２】
ここでθは、変数であり、数値または任意の変数が代入される。上式において、たとえばＲｚ［ｑ１］は、（２）式に示す各成分におけるθにそれぞれｑ１が代入された行列を示す。また本発明においてｓｉｎ θは、θのサインの値を示し、ｃｏｓ θは、θのコサインの値を示す。また記号「・」は、乗算を示す。
また第５のアーム体ｅ５の代表点ｐ５の位置は、次式で表わされる。
【００９４】
【数３】
ここで手先代表点Ｔから第１のアーム体ｅ１の代表点ｐ１までの距離をｉ０とし、第１のアーム体ｅ１の代表点ｐ１から第２のアーム体ｅ２の代表点ｐ２までの距離をｉ１とし、第２のアーム体ｅ２の代表点ｐ２から第２のアーム体ｅ３の代表点ｐ３までの距離をｉ２とし、第３のアーム体ｅ３の代表点ｐ３から第４のアーム体ｅ４の代表点ｐ４までの距離をｉ３とする。また第４のアーム体ｅ４の代表点ｐ４から第５のアーム体ｅ５の代表点ｐ５までの距離をｉ４とし、第５のアーム体ｅ５の代表点ｐ５から第６のアーム体ｅ６の代表点ｐ６までの距離をｉ５とする。また第６のアーム体ｅ６の代表点ｐ６から基台代表点Ｓまでの距離をｉ６とする。
【００９６】
また第１のアーム体ｅ１の代表点ｐ１は、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１と第１のアーム体ｅ１の軸線とが交差する点であって、第１のアーム体ｅ１と第２のアーム体ｅ２を連結する部分に設けられる。また第２のアーム体ｅ２の代表点ｐ２は、第２のアーム体ｅ２と第３のアーム体ｅ３との回転軸線Ｌ２が挿通し、第２のアーム体ｅ２と第３のアーム体ｅ３を連結部分に設けられる。また第３のアーム体ｅ３の代表点ｐ３は、第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３と第３のアーム体ｅ３の軸線とが交差する点であって、第３のアーム体ｅ３と第４のアーム体ｅ４を連結する部分に設けられる。
【００９７】
また第４のアーム体ｅ４の代表点ｐ４は、第４のアーム体ｅ４と第５のアーム体ｅ５との回転軸線Ｌ４が挿通し、第４のアーム体ｅ４と第５のアーム体ｅ５を連結する部分に設けられる。また第５のアーム体ｅ５の代表点ｐ５は、第５のアーム体ｅ５と第６のアーム体ｅ６との回転軸線Ｌ５と第５のアーム体ｅ５の軸線とが交差する点であって、第５のアーム体ｅ５と第６のアーム体ｅ６を連結する部分に設けられる。また第６のアーム体ｅ６の代表点ｐ６は、第６のアーム体ｅ６と基台２１との回転軸線Ｌ６が挿通し、第６のアーム体ｅ６と基台２２とを連結する部分に設けられる。
【００９８】
（５）式から、３軸交点の位置、すなわち第５のアーム体の代表点ｐ５を求めることができる。次に演算座標系Σｓｂに対する基台座標系Σｓｔの変換行列Ｒｕｈを求める。この変換行列Ｒｕｈは、次式によって求められる。
【００９９】
【数４】
また（７）式によって、第５のアーム体ｅ５の代表点ｐ５の座標（Ｐｗｘ，Ｐｗｙ，Ｐｗｚ）の数値を求めることができる。さらに後述する動作を行うために、第５のアーム体ｅ５の代表点ｐ５の座標を（８）式に変換する。
【０１０１】
【数５】
すなわち第５のアーム体ｅ５の代表点ｐ５のｘ座標を表すｘ座標値Ｐｘｎを求める。またｙ座標にｚ座標を加算し、さらに手先代表点Ｔから第１のアーム体ｅ１の代表点ｐ１までの距離ｉ０を減算したｙｚ加算値Ｐｙｎを求める。またｙ座標からｚ座標を減算し、さらに手先代表点Ｔから第１のアーム体ｅ１の代表点ｐ１までの距離ｉ０を減算したｙｚ減算値Ｐｚｎを求める。
【０１０３】
上述したように第５のアーム体ｅ５の代表点ｐ５は、３軸交点となる。したがって（７）式を演算することによって、３軸交点ｐ５の位置を求めることができる。３軸交点の位置を求めると、ステップｓ１５に進む。
【０１０４】
ステップｓ１５では、まず反転モデルにおける多関節マニピュレータ２０Ａを、等価モデルとなる多関節マニピュレータ２０Ｂに変換する。図８に示す等価モデルにおいて、第１〜第３のアーム体ｅ１〜ｅ６の回転角度Ｑ１，Ｑ２，Ｑ３を求める。
【０１０５】
等価モデルにおいて、演算座標系Σｓｂから第１〜第３のアーム体ｅ１〜ｅ３の代表点ｐ１〜ｐ３の姿勢にそれぞれ変換する変換行列Ｒ０１〜Ｒ０３は、（９）式で表わされる。（９）式においてαは、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１が、各アーム体ｅ１，ｅ２の軸線に対して傾斜する角度である。また図８に示すモデルにおける第５のアーム体ｅ５の代表点ｐ５の位置は、次の（１０）式で表わされる。
【０１０６】
【数６】
【数７】
ここで、手先基準位置Ｔから第１のアーム体ｅ１の代表点ｐ１までの距離ｉ０を無視した場合における、第１のアーム体ｅ１の代表点ｐ１から第３のアーム体ｅ３の代表点ｐ１までの距離（ｉ１＋ｉ２）を１とし、第３のアーム体ｅ３の代表点ｐ３から第５のアーム体ｅ５の代表点ｐ５までの距離（ｉ３＋ｉ４）を第１のアーム体ｅ１の代表点ｐ１から第３のアーム体ｅ３の代表点ｐ１までの距離（ｉ１＋ｉ２）の除算値（ｉ３＋ｉ４）／（ｉ１＋ｉ２）をｎｒとする。
【０１０９】
実際の第５のアーム体ｅ５の代表点ｐ５は、上式で得られる値に、第１のアーム体ｅ１の代表点ｐ１から第３のアーム体ｅ３の代表点ｐ１までの距離（ｉ１＋ｉ２）を乗算し、さらにその値に手先代表点Ｔから第１のアーム体ｅ１の代表点ｐ１までの距離ｉ０を加算した値となる。（１０）式に基づいて、第５のアーム体ｅ５の代表点ｐ５の位置を求めると次の（１１）式となる。
【０１１０】
【数８】
（１１）式で第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１とそれらの各アーム体ｅ１，ｅ２の軸線との角度αを４５度とすると、（１２）式となる。
【０１１２】
【数９】
また第５のアーム体ｅ５の代表点ｐ５の演算ｘ軸座標ｐ５ｘを示す座標式Ｐｘｎ、第５のアーム体ｅ５の代表点ｐ５の演算ｙ軸座標ｐ５ｙと、第５のアーム体ｅ５の代表点ｐ５の演算ｚ軸座標ｐ５ｚとを加算したｙｚ加算値を表すｙｚ加算座標式Ｐｙｎ、第５のアーム体ｅ５の代表点ｐ５の演算ｚ軸座標ｐｚを減算したｙｚ減算値を表すｙｚ減算座標式Ｐｚｎを（１３）式に示す。
【０１１４】
【数１０】
このように式を変形し、（１３）式を連立して解くことによって、（１４）式に示すようにｃｏｓ Ｑ３を表わすことができ、第３角度Ｑ３を求めることができる。これによって等価モデルにおける第１〜第３角度Ｑ１〜Ｑ３の解析式を容易に求めることができ、ステップｓ１６に進む。
【０１１６】
【数１１】
ステップｓ１６では、ステップｓ１４の（８）式で求めた第５のアーム体ｅ５の代表位置ｐ５のｘ座標値Ｐｘｎ、ｙｚ加算値Ｐｙｎ、ｙｚ減算値Ｐｚｎを、ステップｓ１５で求めた第１〜第３角度Ｑ１〜Ｑ３を示す式に代入することによって、第１〜第３角度Ｑ１〜Ｑ３の数値解を求めることができる。数値解は、目標とする基台２１の位置が特異点でないかぎり、４通りの組合せが得られる。このように等価モデルにおける第１〜第３角度Ｑ１〜Ｑ３の数値解が求まると、ステップｓ１７に進む。
【０１１８】
ステップｓ１７では、ステップｓ１６で求めた等価モデルの第１〜第３角度Ｑ１〜Ｑ３を反転モデルの第１〜第３角度ｑ１〜ｑ３に変換する。このときｃｏｓｑ３は、次式で表わされる。（１５）式〜（１７）式においてαは、第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３が各アーム体ｅ３，ｅ４に対して傾斜する角度である。
【０１１９】
【数１２】
ここでｃｏｓｅｃ αは、αのコセカントの値を示す。またｃｏｔ αは、コタンジェントの値を示す。等価モデルの第３角度Ｑ３を（１５）式に代入することによって、反転モデルにおける第３角度ｑ３を求めることができる。
またｃｏｓ ｑ２およびｓｉｎ ｑ２は、次の（１６）式によって求められる。
【０１２１】
【数１３】
ここでＤ２は、（１７）式によって与えられる。
【０１２３】
【数１４】
（１６）式によって反転モデルにおける第２角度ｑ２を求めることができる。また等価モデルにおける第１角度Ｑ１は、反転モデルの第１角度ｑ１と同じ値になる。反転モデルにおける第１〜第３角度ｑ１〜ｑ３は、３軸交点が特異点でないかぎり、等価モデルの場合と同様に４つの組合せが求められる。
【０１２５】
ステップｓ１７で反転モデルにおける第１〜第３角度ｑ１〜ｑ３が求まると、ステップｓ１８に進み、反転モデルにおける第４〜第６角度ｑ４〜ｑ６を求める。まず第５のアーム体ｅ５に固定して設けられる座標系の姿勢を求める。第５のアーム体ｅ５の変換行列Ｒｗｈは、次の（１８）式によって求められる。
【０１２６】
【数１５】
すなわち変換行列Ｒ０３を転置した行列に、演算座標系Σｓｂに対する基台座標系Σｓｔの変換行列Ｒｕｈを乗算する。変換行列Ｒ０３は、第１〜第３角度ｑ１〜ｑ３が求まっているので、（１）式から求めることができる。また変換行列Ｒｕｈは、上式からその数値解を求めることができる。また変換行列Ｒｕｈは、（６）式から求めることができる。変換行列Ｒｗｈの各成分の数値解が求められると、（１９）式を解くことによって、第４〜第６角度ｑ４〜ｑ６を求めることができる。（１９）式においてαは、第５のアーム体ｅ５と第６のアーム体ｅ６との回転軸線Ｌ５と各アーム体ｅ５，ｅ６の軸線に対して傾斜する角度である。
【０１２８】
【数１６】
反転モデルにおける第４〜第６角度ｑ４〜ｑ６は、基台代表点が特異点でないかぎり第１〜第３角度ｑ１〜ｑ３の１つの組合せに対して２つの組合せが求められる。したがって３軸交点および基台代表点が特異点でないかぎり、８とおりの第１〜第６角度ｑ１〜ｑ６の組合せを求めることができる。このようにステップｓ１８で反転モデルにおける第４〜第６角度ｑ４〜ｑ６を求めると、ステップｓ１９に進む。
【０１３０】
ステップｓ１９では、反転モデルにおける各アーム体ｅ１〜ｅ６の第１〜第６角度ｑ１〜ｑ６を（２０）式に代入することによって、真のモデルにおける各アーム体ｃ１〜ｃ６の回転角度である第１〜第６角度θ１〜θ６に変換することができる。真のモデルにおける各アーム体ｃ１〜ｃ６の第１〜第６の回転角度θ１〜θ６が求まるとステップｓ２０に進み、動作を終了する。
θ１ ＝ ｑ６
θ２ ＝ ｑ５
θ３ ＝ ｑ４
θ４ ＝ ｑ３
θ５ ＝ ｑ２
θ６ ＝ ｑ１ …（20）
【０１３１】
以上のように演算することで、真のモデルにおける各アーム体ｃ１〜ｃ６の回転角度θ１〜θ６を容易に求めることができる。これによって手先装置２２を目標とする位置および姿勢に配置するための各アーム体の回転角度を求めることができる。また演算結果に従って多関節マニピュレータの各アーム体を制御することによって、手先装置２２を目標とする位置および姿勢に配置することができる。上述する演算は、コンピュータを用いて計算してもよく、また手動で計算してもよい。
【０１３２】
演算された各アーム体の回転角度は解析解である。したがって、収束演算および補間近似によって各アーム体の回転角度を求める場合に比べて、短時間でかつ正確に求めることができる。またコンピュータを用いて演算した場合には、その処理能力が低い場合であっても、各アーム体の回転角度を求めることができる。さらに上述したように各回転角度の８つの組合せを得ることができ、エネルギーコストおよび障害物の有無を考慮して８つの組合せのうちから最適な各アーム体の回転角度の組合せを選択することができる。さらに図1に示す多関節マニピュレータ２０の場合には、真のモデルを反転モデルに変換した。これによって容易に各アーム体ｃ１〜ｃ６の第１〜第３の回転角度θ１〜θ６を求めることができる。
【０１３３】
また上述した演算結果を用いて、多関節マニピュレータの各アーム体の代表点間の距離Ｈ０〜Ｈ６、回転軸Ｌ１〜Ｌ６と各アーム体との軸線とが傾斜する角度α、手先装置２２の位置Ｔ（Ｔｘ，Ｔｙ，Ｔｚ）、手先装置２２の姿勢（ｑｏ，ｑａ，ｑｔ）を変数として、真のアーム体ｃ１〜ｃ６の角度θ１〜θ６を表わした解析式を求めておき、この解析式に各変数を示す数値を代入することによって、真のアーム体ｃ１〜ｃ６の角度θ１〜θ６を求めてもよい。
【０１３４】
また本実施の形態では、傾斜関節部における各アーム体の回転軸線と各アーム体との軸線の傾斜角度は、共通の角度であって４５度である多関節マニピュレータの各アーム体の回転角度を求めた。傾斜関節部における各アーム体の回転軸線と各アーム体との軸線の傾斜角度は、共通でなくてもまた４５度以外であっても、各アーム体の回転角度を求めることができる。
【０１３５】
たとえば図７に示す反転モデルの多関節マニピュレータ２０Ａにおいて、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ２と各アーム体ｃ１，ｃ２との軸線との傾斜角度がα度であって、第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３と各アーム体ｃ３，ｃ４との軸線との傾斜角度がβ度であって、第５のアーム体ｅ５と第６のアーム体ｅ６との回転軸線Ｌ５と各アーム体ｃ５，ｃ６との軸線との傾斜角度がγ度である場合であっても、ｙｚ加算座標式Ｐｙｎおよびｙｚ減算式Ｐｚｎに適切な係数を乗算することによって各アーム体ｅ１〜ｅ６の角度ｑ１〜ｑ６を求めることができる。
【０１３６】
また３軸交点で交差する回転軸線以外の残余の回転軸線のうち、少なくとも１つが対応する各アーム体の軸線に対して４５度の場合には、より簡単に第１〜第３角度を求めることができる。
【０１３７】
反転モデルにおいて、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１と各アーム体の軸線との傾斜角度が４５度であって、第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３と各アーム体の軸線との傾斜角度が４５でない場合であっても、第３のアーム体ｅ３と第４のアーム体ｅ４を連結する関節部を図８に示すように等価モデルに変換することによって簡単に求めることができる。具体的には、３軸交点のｙ座標とｚ座標とを加算した式を表わす加算式、ｙ座標からｚ座標を減算した式を表わす減算式を簡略化することができ、さらに容易に第１〜第３角度を求めることができる。
【０１３８】
同様に第３のアーム体ｅ３と第４のアーム体ｅ４との回転軸線Ｌ３と、各アーム体の軸線と、の傾斜角度が４５度であって、第１のアーム体ｅ１と第２のアーム体ｅ２との回転軸線Ｌ１と各アーム体の軸線との傾斜角度が４５でない場合であっても、第１のアーム体ｅ１と第２のアーム体ｅ２を連結する関節部を等価モデルに変換することによって簡単に求めることができる。
【０１３９】
したがって、上述した３軸交点を有する多関節マニピュレータのうち、直列に順に並んで設けられる第１〜第６のアーム体のうち第１〜第３のアーム体を連結する関節部のうち少なくとも１つの傾斜関節部によって連結される２つのアーム体の回転軸線が、これら２つのアーム体の軸線に対して４５度を成して傾斜する多関節マニピュレータを用いることによって、各アーム体の回転角度をより簡単に求めることができる。また本発明において、真のモデルは、基台側に３軸交点が設けられたが、手先側に３軸交点が設けられてもよい。この場合には、反転モデルに変換することなく、容易に各アーム体の回転角度を求めることができる。
【０１４０】
したがって本実施の形態の真のモデルに示すように、固定端部側の回転軸線Ｌ１，Ｌ２，Ｌ３が３軸交点で交差し、第３のアーム体と第４のアーム体との回転軸線Ｌ３と各アーム体の軸線との傾斜角が４５度であって、第５のアーム体と第６のアーム体との回転軸線Ｌ６と各アーム体の軸線との傾斜角が任意の角度である第１の多関節マニピュレータ、または固定端部側の回転軸線Ｌ１，Ｌ２，Ｌ３が３軸交点で交差し、第３のアーム体と第４のアーム体との回転軸線Ｌ３と各アーム体の軸線との傾斜角が任意の角度であって、第５のアーム体と第６のアーム体との回転軸線Ｌ６と各アーム体の軸線との傾斜角が４５度である第２の多関節マニピュレータにおいては、反転モデルに変換し、さらに傾斜角が任意の角度となる関節部を等価モデルに変換することによってより簡単に各アーム体の回転角度を求めることができる。
【０１４１】
また反転モデルに示すように遊端部側の回転軸線Ｌ４，Ｌ５，Ｌ６が３軸交点で交差し、第１のアーム体と第２のアーム体との回転軸線Ｌ１と各アーム体の軸線との傾斜角が４５度であって、第３のアーム体と第４のアーム体との回転軸線Ｌ３と各アーム体の軸線との傾斜角が任意の角度である第３の多関節マニピュレータ、または遊端部側の回転軸線Ｌ４，Ｌ５，Ｌ６が３軸交点で交差し、第１のアーム体と第２のアーム体との回転軸線Ｌ１と各アーム体の軸線との傾斜角が任意の角度であって、第３のアーム体と第４のアーム体との回転軸線Ｌ３と各アーム体の軸線との傾斜角が４５度である第４の多関節マニピュレータにおいては、傾斜角が任意の角度となる関節部を等価モデルに変換することによってより簡単に各アーム体の回転角度を求めることができる。また図８に示すような、第３のアーム体ｅ３と第４のアーム体ｅ４とを連結する関節部が第３および第四のアーム体ｅ３，ｅ４に垂直な場合であっても、各アーム体の回転角度を求めることができる。
【０１４２】
図９は、本発明の他の多関節マニピュレータをモデル化した他のモデルを示す図である。他のモデルは、図９に示す反転モデルの第６のアーム体ｅ６と基台２１とが固定されるモデルである。上述した方法と同様の演算方法によって、第１〜第５のアーム体ｅ１〜ｅ５の回転角度ｑ１〜ｑ５を求めることができる。
【０１４３】
【発明の効果】
以上のように本発明によれば、対象物が目標とする位置および姿勢で配置されるときの、第１〜第６角度を短時間で求めることができる。これによって対象物の移動ごとに各アーム体の角度を求めることが可能である。たとえば予め定められる経路を移動する対象物の位置および姿勢を連続的に入力することによって、その連続的に変化する入力値に応じて各アーム体の角度を順次変化させることができる。これによって対象物を目標とする位置に移動させるときに、高速でかつスムーズに移動させることができる。
【０１４４】
また各角度の解析解を求めることで、対象物が目標とする位置に移動するにあたって、各角度のすべての組合せを求めることができる。６自由度を有する多関節マニピュレータの場合、８つの組合せの回転角度を求めることができる。これによって対象物が目標とする位置および姿勢に配置した状態で、多関節マニピュレータの各アーム体の角度を複数の組合せの中から選択することができる。たとえばエネルギーコストおよび障害物の有無などを考慮して複数の組合せから最適な各アーム体の回転角度の組合せを選択できる。
【０１４５】
また本発明によれば、対象物が目標とする位置および姿勢で配置されるときの、第１〜第５角度を短時間で求めることができる。これによって対象物の移動ごとに各アーム体の角度を求めることが可能である。たとえば予め定められる経路を移動する対象物の位置および姿勢を連続的に入力することによって、その連続的に変化する入力値に応じて各アーム体の角度を順次変化させることができる。これによって対象物を目標とする位置に移動させるときに、高速でかつスムーズに移動させることができる。
【図面の簡単な説明】
【図１】本発明の実施の一形態である多関節マニピュレータ２０を示す正面図である。
【図２】位置決め演算装置３０によって姿勢制御される多関節マニピュレータ２０を示す図である。
【図３】手先装置２２を移動させる場合に位置決め演算装置３０によって姿勢制御される多関節マニピュレータ１を説明するための図である。
【図４】本発明の位置決め演算方法を大略的に示すフローチャートである。
【図５】本発明の実施の一形態の演算処理方法を具体的に示すフローチャートである。
【図６】本発明の多関節マニピュレータをモデル化した真のモデルを示す図である。
【図７】図６に示すの真のモデルを反転した反転モデルを示す図である。
【図８】図７の反転モデルにおける多関節マニピュレータの１つの関節部を等価な関節部に置き換えた等価モデルを示す図である。
【図９】本発明の他の多関節マニピュレータをモデル化した他のモデルを示す図である。
【図１０】姿勢制御される従来の多関節マニピュレータ１の一部を示す図である。
【符号の説明】
２０ 多関節マニピュレータ
２１ 基台
２２ 手先装置
３０ 位置決め演算装置
３１ 入力手段
３２ 演算手段
３３ 出力手段
ｃ１〜ｃ６、ｅ１〜ｅ６ アーム体
ｄ１〜ｄ６、ｆ１〜ｆ６ 関節部
θ１〜θ６、ｑ１〜ｑ６ 回転角度
ｐ１〜ｐ６ 各アーム体の代表点
Ｓ 基台代表点
Ｔ 手先代表点
Claims (8)
- 基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に回転自在に連結され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、基台と第１のアーム体との回転軸線と、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する３軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算方法であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力工程と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算する工程と、
反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記３軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求める３軸交点演算工程と、
反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記３軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標に反転モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標から反転モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求める第１〜第３角度演算工程と、
これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、第２のアーム体に対する第１のアーム体の回転角度である第５角度、および第１のアーム体に対する基台の回転角度である第６角度を求める第４〜第６角度演算工程と、
反転モデルの第１〜第６角度を真のモデルの第１〜第６角度に変換する工程とを含むことを特徴とする位置決め演算方法。 - 適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記３軸交点演算工程では、前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を、
等価モデルにおける前記３軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、
前記第１〜第３角度演算工程では、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標に等価モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標から等価モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算する工程を含むことを特徴とする請求項１に記載の位置決め演算方法。 - 基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に固定され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する２軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算方法であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力工程と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算する工程と、
反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記２軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求める２軸交点演算工程と、
反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標に反転モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標から反転モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求める第１〜第３角度演算工程と、
これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、および第２のアーム体に対する第１のアーム体の回転角度である第５角度を求める第４〜第５角度演算工程と、
反転モデルの第１〜第５角度を真のモデルの第１〜第５角度に変換する工程とを含むことを特徴とする位置決め演算方法。 - 適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記２軸交点演算工程では、前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を、等価モデルにおける前記２軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、
前記第１〜第３角度演算工程では、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標に等価モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標から等価モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算する工程を含むことを特徴とする請求項３に記載の位置決め演算方法。 - 基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に回転自在に連結され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、基台と第１のアーム体との回転軸線と、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する３軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算装置であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力手段と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算し、反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記３軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記における３軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標に反転モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記３軸交点のｙ座標から反転モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求め、これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、第２のアーム体に対する第１のアーム体の回転角度である第５角度、および第１のアーム体に対する基台の回転角度である第６角度を求め、反転モデルの第１〜第６角度を真のモデルの第１〜第６角度に変換する演算手段と、
演算手段が演算した演算結果を出力する出力手段とを備えることを特徴とする位置決め演算装置。 - 適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記演算手段は、前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記３軸交点のｘ座標、ｙ座標およびｚ座標を、等価モデルにおける前記３軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標に等価モデルの座標系に対する前記３軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記３軸交点のｙ座標から等価モデルの座標系に対する前記３軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算することを特徴とする請求項５に記載の位置決め演算装置。 - 基台と、基台に直列に順に並んで設けられる第１〜第６のアーム体と、第６のアーム体に固定される対象物と、隣接する２つのアーム体を各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、隣接する２つのアーム体をこれら２つのアーム体の軸線に対して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有し、第１のアーム体は、基台に固定され、第１のアーム体と第２のアーム体との回転関節軸が、傾斜関節部であり、第１のアーム体と第２のアーム体との回転軸線と、第２のアーム体と第３のアーム体との回転軸線とが交差する２軸交点を有する多関節マニピュレータの各アーム体の相対回転角度を求める演算装置であって、
前記第１〜第６のアーム体がこの順に並んで設けられる多関節マニピュレータのモデルを真のモデルとし、
アーム体の並び順が真のモデルと同じであり、対象物に対して基台の位置を制御する形態のモデルを反転モデルとし、
真のモデルの座標系の原点は基台に固定され、相互に直交するｘ軸、ｙ軸およびｚ軸を有する真のモデルの座標系において、対象物の位置および姿勢が入力される入力手段と、
反転モデルの座標系の原点は対象物に固定され、多関節マニピュレータが真のモデルの座標系から見た対象物の位置および姿勢を実現する各関節軸角度をとった際の、反転モデルの座標系から見た基台の位置および姿勢を演算し、反転モデルにおける演算された反転モデルの座標系に対する基台の位置および姿勢に基づいて、反転モデルにおける前記２軸交点の、反転モデルの座標系に対するｘ座標、ｙ座標およびｚ座標を求め、反転モデルにおいて、反転モデルの第６のアーム体に対する第５のアーム体の回転角度である第１角度、第５のアーム体に対する第４のアーム体の回転角度である第２角度、第４のアーム体に対する第３のアーム体の回転角度である第３角度を変数として用い、反転モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標に反転モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、反転モデルの座標系に対する前記２軸交点のｙ座標から反転モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、反転モデルの第１〜第３角度を求め、これら反転モデルにおける反転モデルの座標系に対する基台の姿勢および第１〜第３角度に基づいて、反転モデルの第３のアーム体に対する第２のアーム体の回転角度である第４角度、および第２のアーム体に対する第１のアーム体の回転角度である第５角度を求め、反転モデルの第１〜第５角度を真のモデルの第１〜第５角度に変換する演算手段と、
演算手段が演算した演算結果を出力する出力手段とを備えることを特徴とする位置決め演算装置。 - 適用対象である多関節マニピュレータにおいて、第３のアーム体と第４のアーム体とを連結する関節部が、傾斜関節部であり、
前記反転モデルに対して等価な等価モデルであって、反転モデルにおける第３のアーム体と第４のアーム体とを連結する関節部が、第３および第４のアーム体の軸線に垂直な回転軸線まわりに回転自在に連結される関節部に置き換えられたモデルを等価モデルとし、
等価モデルの座標系の原点は対象物に固定され、等価モデルにおいて、真のモデルにおける基台と対象物との相対位置および相対姿勢を実現した場合の、等価モデルの座標系から見た基台の位置および姿勢を、等価モデルにおける基台の位置および姿勢としており、
前記演算手段は、前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を求めるとともに、こうして求めた前記反転モデルにおける反転モデルの座標系に対する前記２軸交点のｘ座標、ｙ座標およびｚ座標を、等価モデルにおける前記２軸交点の、等価モデルの座標系に対するｘ座標、ｙ座標およびｚ座標として採用し、等価モデルにおいて、等価モデルの第１角度、第２角度および第３角度を変数として用い、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｘ座標を表すｘ座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標に等価モデルの座標系に対する前記２軸交点のｚ座標を加算した加算値を表すｙｚ加算座標式と、等価モデルにおける等価モデルの座標系に対する前記２軸交点のｙ座標から等価モデルの座標系に対する前記２軸交点のｚ座標を減算した減算値を表すｙｚ減算座標式とを含む座標式を解いて、等価モデルの第１〜第３角度を求め、等価モデルの第１〜第３角度を反転モデルの第１〜第３角度に変換し、反転モデルの第１〜第３角度を演算することを特徴とする請求項７に記載の位置決め演算装置。
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002316760A JP4230196B2 (ja) | 2002-10-30 | 2002-10-30 | 位置決め演算方法および位置決め演算装置 |
| US10/691,668 US6978193B2 (en) | 2002-10-30 | 2003-10-24 | Positioning data calculating procedure, positioning data calculating apparatus and articulated manipulator |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002316760A JP4230196B2 (ja) | 2002-10-30 | 2002-10-30 | 位置決め演算方法および位置決め演算装置 |
Publications (3)
| Publication Number | Publication Date |
|---|---|
| JP2004148448A JP2004148448A (ja) | 2004-05-27 |
| JP2004148448A5 JP2004148448A5 (ja) | 2005-11-24 |
| JP4230196B2 true JP4230196B2 (ja) | 2009-02-25 |
Family
ID=32211695
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2002316760A Expired - Fee Related JP4230196B2 (ja) | 2002-10-30 | 2002-10-30 | 位置決め演算方法および位置決め演算装置 |
Country Status (2)
| Country | Link |
|---|---|
| US (1) | US6978193B2 (ja) |
| JP (1) | JP4230196B2 (ja) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7571027B2 (en) * | 2005-05-31 | 2009-08-04 | The Boeing Company | Kinematic singular point compensation systems and methods |
| US7689522B2 (en) * | 2006-01-12 | 2010-03-30 | Ronan Sorensen | Method and system of organizing information based on human thought processes |
| CA2739727C (en) * | 2008-10-06 | 2017-06-27 | Kinova | Portable robotic arm |
| JPWO2016152987A1 (ja) * | 2015-03-25 | 2018-01-18 | ソニー・オリンパスメディカルソリューションズ株式会社 | 医療用観察装置、手術用観察装置及び医療用観察システム |
| FR3040145B1 (fr) * | 2015-08-21 | 2018-02-09 | Nimbl'bot | Bras de robot articule |
| CN111771711B (zh) * | 2020-06-05 | 2022-09-30 | 江苏理工学院 | 授粉机器人及其机械臂的位姿计算方法、位姿计算装置 |
Family Cites Families (22)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS56163624A (en) | 1980-05-20 | 1981-12-16 | Umetani Youji | Active cord mechanism |
| US4702668A (en) * | 1985-01-24 | 1987-10-27 | Adept Technology, Inc. | Direct drive robotic system |
| US4907937A (en) * | 1985-07-08 | 1990-03-13 | Ford Motor Company | Non-singular industrial robot wrist |
| JPS62148182A (ja) | 1985-12-24 | 1987-07-02 | 株式会社小松製作所 | 関節装置 |
| SE8605070L (sv) | 1986-11-26 | 1988-05-27 | Komatsu Mfg Co Ltd | Bojlig robotarm |
| JPH04115592U (ja) | 1991-03-29 | 1992-10-14 | 邦男 高木 | ロボツトアーム |
| JP3412704B2 (ja) * | 1993-02-26 | 2003-06-03 | 株式会社ニコン | 投影露光方法及び装置、並びに露光装置 |
| JPH10225881A (ja) | 1997-02-14 | 1998-08-25 | Natl Aerospace Lab | オフセット回転関節及び該オフセット回転関節を有する多関節ロボット |
| JP3443077B2 (ja) * | 1999-09-20 | 2003-09-02 | ソニー株式会社 | ロボットの運動パターン生成装置及び運動パターン生成方法、並びにロボット |
| JP3326472B2 (ja) | 1999-11-10 | 2002-09-24 | 独立行政法人 航空宇宙技術研究所 | 多関節ロボット |
| JP3555107B2 (ja) * | 1999-11-24 | 2004-08-18 | ソニー株式会社 | 脚式移動ロボット及び脚式移動ロボットの動作制御方法 |
| US6374158B1 (en) * | 2000-02-15 | 2002-04-16 | General Electric Company | Robotic laser pointer |
| JP3511088B2 (ja) * | 2000-04-10 | 2004-03-29 | 独立行政法人航空宇宙技術研究所 | 多関節介護ロボット制御用の圧力分布センサ |
| SE0003912D0 (sv) * | 2000-10-24 | 2000-10-24 | Abb Ab | Industrirobot |
| JP4119080B2 (ja) * | 2000-11-17 | 2008-07-16 | 本田技研工業株式会社 | 人間型ロボットの腕構造 |
| JP4252721B2 (ja) * | 2000-11-17 | 2009-04-08 | 本田技研工業株式会社 | 二足歩行ロボット |
| US6516248B2 (en) * | 2001-06-07 | 2003-02-04 | Fanuc Robotics North America | Robot calibration system and method of determining a position of a robot relative to an electrically-charged calibration object |
| US6604021B2 (en) * | 2001-06-21 | 2003-08-05 | Advanced Telecommunications Research Institute International | Communication robot |
| JP2003025269A (ja) | 2001-07-12 | 2003-01-29 | National Aerospace Laboratory Of Japan | 回転修正機構付きオフセット回転関節ユニット |
| US6580971B2 (en) * | 2001-11-13 | 2003-06-17 | Thierica, Inc. | Multipoint inspection system |
| US6882901B2 (en) * | 2002-10-10 | 2005-04-19 | Shao-Wei Gong | Ultra-precision robotic system |
| JP2004148449A (ja) * | 2002-10-30 | 2004-05-27 | Kawasaki Heavy Ind Ltd | 多関節マニピュレータ |
-
2002
- 2002-10-30 JP JP2002316760A patent/JP4230196B2/ja not_active Expired - Fee Related
-
2003
- 2003-10-24 US US10/691,668 patent/US6978193B2/en not_active Expired - Lifetime
Also Published As
| Publication number | Publication date |
|---|---|
| US6978193B2 (en) | 2005-12-20 |
| US20040093123A1 (en) | 2004-05-13 |
| JP2004148448A (ja) | 2004-05-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3207728B2 (ja) | 冗長マニピュレータの制御方法 | |
| JP3655083B2 (ja) | ロボットの位置決めを行うインタフェース装置 | |
| Tsai | Robot analysis: the mechanics of serial and parallel manipulators | |
| Qassem et al. | Modeling and Simulation of 5 DOF educational robot arm | |
| JPH079606B2 (ja) | ロボット制御装置 | |
| JPH0820894B2 (ja) | 産業用ロボツトの動作制御方法 | |
| JP3415427B2 (ja) | ロボットシミュレーションにおけるキャリブレーション装置 | |
| WO2022258054A1 (zh) | 重建器械术野中心的控制方法、系统和存储介质 | |
| Allen et al. | Closed-form non-singular constant-curvature continuum manipulator kinematics | |
| Cocota et al. | A low-cost robot manipulator for education | |
| JP4230196B2 (ja) | 位置決め演算方法および位置決め演算装置 | |
| JPH01267706A (ja) | ロボットの制御方法 | |
| JP5371733B2 (ja) | ７軸多関節ロボットの制御方法及び制御プログラム | |
| JP3206765B2 (ja) | マスタスレーブマニピュレータの制御方法 | |
| JP2021186929A (ja) | 多軸ロボットの制御方法 | |
| JP4647919B2 (ja) | 制御方法および制御装置 | |
| JP3146463B2 (ja) | 冗長軸マニピュレータの肘移動制御装置 | |
| JP3431026B2 (ja) | 冗長軸を有する７軸以上のマニピュレータの制御方法 | |
| Lai | A fast task planning system for 6R articulated robots based on inverse kinematics | |
| JPS61253510A (ja) | ロボツト制御デ−タの確認方法 | |
| Du Plessis | An optimization approach to the determination of manipulator workspaces | |
| Mihelj et al. | yControl-open architecture controller for Yaskawa Motoman MH5 robot | |
| JP2005193306A (ja) | 産業用ロボットおよびその制御方法 | |
| Gül | Trajectory control of a delta robot for telescopic mast systems assembly | |
| JPS63184806A (ja) | ロボツトによる相対座標系情報テイ−チング方式 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20051006 |
|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20051006 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20070411 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20070424 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070625 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20070807 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20071009 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20080422 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20080619 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20080701 |
|
| A911 | Transfer of reconsideration by examiner before appeal (zenchi) |
Free format text: JAPANESE INTERMEDIATE CODE: A911 Effective date: 20080804 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20080909 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20081105 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20081202 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20081203 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111212 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111212 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121212 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121212 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131212 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20141212 Year of fee payment: 6 |
|
| LAPS | Cancellation because of no payment of annual fees |