JP2017052049A - 物体の姿勢算出システム - Google Patents
物体の姿勢算出システム Download PDFInfo
- Publication number
- JP2017052049A JP2017052049A JP2015177471A JP2015177471A JP2017052049A JP 2017052049 A JP2017052049 A JP 2017052049A JP 2015177471 A JP2015177471 A JP 2015177471A JP 2015177471 A JP2015177471 A JP 2015177471A JP 2017052049 A JP2017052049 A JP 2017052049A
- Authority
- JP
- Japan
- Prior art keywords
- posture
- coordinate system
- contact
- axis
- convex portion
- 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.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B5/00—Measuring arrangements characterised by the use of mechanical techniques
- G01B5/004—Measuring arrangements characterised by the use of mechanical techniques for measuring coordinates of points
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B21/00—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant
- G01B21/02—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant for measuring length, width, or thickness
- G01B21/04—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant for measuring length, width, or thickness by measuring coordinates of points
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manipulator (AREA)
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
Abstract
【課題】一方の物体と他方の物体との相対的な姿勢または相対的な位置および姿勢を算出するシステムを提供する。
【解決手段】3つの接触点で接触可能な一方の物体および他方の物体と、一方の物体と他方の物体とを接触させる駆動部と、一方の物体と他方の物体との間に作用する力を計測する力計測部と、一方の物体と他方の物体とが3つの接触点で接触するときに力計測部によって計測される力に基づいて一方の物体と他方の物体との相対的な姿勢または相対的な位置および姿勢を算出する物体の姿勢算出部と、を備える物体の姿勢算出システムが提供される。
【選択図】図2
【解決手段】3つの接触点で接触可能な一方の物体および他方の物体と、一方の物体と他方の物体とを接触させる駆動部と、一方の物体と他方の物体との間に作用する力を計測する力計測部と、一方の物体と他方の物体とが3つの接触点で接触するときに力計測部によって計測される力に基づいて一方の物体と他方の物体との相対的な姿勢または相対的な位置および姿勢を算出する物体の姿勢算出部と、を備える物体の姿勢算出システムが提供される。
【選択図】図2
Description
本発明は、一方の物体と他方の物体とが3つの接触点で接触しているときに、一方の物体と他方の物体との間に作用する力に基づいて、一方の物体と他方の物体との相対的な姿勢、または相対的な位置および姿勢を算出する物体の姿勢算出システムに関する。
一方の物体と他方の物体との相対姿勢を算出するとき、カメラや三次元センサなどの非接触式のセンサを用いて、相対的な姿勢を測定、算出するといった方法が一般的に知られている。
また、一方の物体と他方の物体との間に作用する力を制御することが可能なシステムである場合、一方の物体の所定の部分を他方の物体の所定の部分に面を合わせたり、一方の物体を他方の物体に対して嵌合させたりすることによって、動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出するといった方法が一般的に知られている。
特許文献1には、一方の物体と他方の物体との間に作用する力が目標値となるように、コンプライアンス機構の運動モデルに基づいて一方の物体の位置を制御する方法が開示されている。
一方の物体と他方の物体とが嵌合可能な物体である場合、このような方法によって、一方の物体と他方の物体との間に作用する力に基づいて、一方と他方の物体とを嵌合させ、動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出することができる。
また特許文献2には、アプローチ点の姿勢誤差を修正するために、予め、穴部分と直交する平面に所定の力で押しつけて面接触させて、面を合わせることによって、姿勢誤差を算出する方法が記載されている。
この方法では、一方の物体と他方の物体との間に作用する力を制御して、一方の物体を他方の物体に対して面を合わせ、一方の物体の動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出する。さらに、前記算出された相対的な姿勢に基づいて、嵌合作業の前のアプローチ点を修正するという方法が開示されている。
カメラや三次元センサなどの非接触式のセンサを用いて、一方の物体と他方の物体との相対的な姿勢を測定、算出する場合、システムとして複雑になったり、コストが高くなったりすることがある。また、一方の物体および他方の物体の状態によって、例えば一方の物体と他方の物体との位置関係、周辺の物体との干渉状況、一方の物体と他方の物体の表面形状などの状況によって、一方の物体と他方の物体との相対的な姿勢の算出が困難な場合も存在する。
一方の物体と他方の物体との間に作用する力を制御することが可能なシステムを用いて、一方の物体を他方の物体に対して嵌合させるときの、動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出する場合、嵌合作業に時間がかかりシステムのサイクルタイムが増大する。また、一方の物体および他方の物体は、嵌合させることが可能な物体でないといけない。また、嵌合作業のために一方の物体を移動させなければならない。また、嵌合作業時の一方の物体の移動に伴って、周辺の物体との干渉などの問題が発生することがある。また、一方の物体と他方の物体の物体との嵌合が正常に完了しない限り、一方の物体と他方の物体との相対的な姿勢を算出することはできない。さらに、一方の物体と他方の物体との相対的な姿勢が大きい場合、嵌合させることは困難な場合が多い。
一方の物体と他方の物体との間に作用する力を制御することが可能なシステムを用いて、一方の物体の面を他方の物体の面に対して合わせるときの、動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出する場合、面を合わせる作業に時間がかかりシステムのサイクルタイムが増大する。また、一方の物体の所定および他方の物体は、面を合わせることが可能な物体でないといけない。また、面合わせ作業のために一方の物体を移動させなければならない。また、面合わせ作業時の一方の物体の移動に伴って、周辺の物体との干渉などの問題が発生することがある。
そこで本発明は、一方の物体と他方の物体との相対的な姿勢、または相対的な位置および姿勢を好適に算出するシステムを提供することを目的とする。
上記目的を達成するために、本願第1の発明は、一方の物体と他方の物体との相対的な姿勢を算出する、または前記一方の物体と前記他方の物体との相対的な位置および姿勢を算出する物体の姿勢算出システムにおいて、凸部を包含する物体と凹部を包含する物体の組み合わせである、かつ、前記凸部と前記凹部とを3つの接触点において接触させることが可能な物体である、かつ、前記凹部における前記3つの接触点は前記凹部の穴の端部に位置する、かつ、前記凸部に対する軸であって前記3つの接触点を含む平面と直交しない軸を凸部中心軸とすると前記接触点と前記凸部中心軸との各最短距離が一定値であるか、または前記接触点から前記凸部中心軸に対して垂線を引いたときの前記凸部中心軸との交点と前記凸部中心軸上の前記交点以外の点との距離に基づいて前記接触点と前記凸部中心軸との各最短距離が定まる、前記一方の物体および前記他方の物体と、前記一方の物体と前記他方の物体とを相対的に移動させる、かつ、前記一方の物体および前記他方の物体の内、少なくとも1つの物体を保持し移動させる1つ以上の駆動部と、前記一方の物体と前記他方の物体との間に作用する力を計測する力計測部と、前記駆動部によって前記一方の物体と前記他方の物体とを相対的に移動させて前記一方の物体と前記他方の物体とを接触させ、前記一方の物体と前記他方の物体とが前記3つの接触点で接触するときに前記力計測部によって計測される力および前記最短距離に基づいて、前記一方の物体と前記他方の物体との相対的な姿勢を算出する、または前記一方の物体と前記他方の物体との相対的な位置および姿勢を算出する物体の姿勢算出部と、を備える、ことを特徴とする物体の姿勢算出システムを提供する。
第2の発明は、第1の発明において、前記一方の物体は前記凸部もしくは前記凹部を2つ以上包含する、または、前記他方の物体は前記凸部もしくは前記凹部を2つ以上包含する、または、前記一方の物体および前記他方の物体は前記凸部もしくは前記凹部を2つ以上包含する、物体の姿勢算出システムを提供する。
第3の発明は、第1または第2の発明において、前記一方の物体または前記他方の物体に包含される、前記凸部および前記凹部の内、少なくとも1つの物体は前記包含される前記一方の物体または前記他方の物体に対して着脱可能な物体である、物体の姿勢算出システムを提供する。
第4の発明は、第1〜第3のいずれか1つの発明において、前記一方の物体と前記他方の物体とを前記3つの接触点で接触させる前に、前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムを提供する。
第5の発明は、第1〜第3のいずれか1つの発明において、前記力計測部は、さらに、前記駆動部または前記一方の物体または前記他方の物体に作用する力を計測し、前記一方の物体と前記他方の物体とを前記3つの接触点で接触させる前に、前記力計測部によって計測される力に基づいて、前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムを提供する。
第6の発明は、第1〜第3のいずれか1つの発明において、前記物体の姿勢算出システムは、さらに、前記一方の物体または前記他方の物体の位置および姿勢の少なくとも一方を所定の範囲以内で検出することが可能な物体検出部を備え、前記一方の物体と前記他方の物体とを前記3つの接触点で接触させる前に、前記物体検出部の検出結果に基づいて、前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムを提供する。
第7の発明は、第1〜第3のいずれか1つの発明において、前記物体の姿勢算出システムは、さらに、空間内の位置または位置および姿勢を指定する装置と前記指定した位置または位置および姿勢を認識する装置とで構成される空間位置指定部を備え、前記一方の物体と前記他方の物体とを前記3つの接触点で接触させる前に、前記空間位置指定部によって指定される位置または位置および姿勢に基づいて、前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムを提供する。
第8の発明は、第1〜第7のいずれか1つの発明において、前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記駆動部に対する、前記一方の物体または前記他方の物体の位置および姿勢の少なくとも一方の算出を行う、物体の姿勢算出システムを提供する。
第9の発明は、第1〜第8のいずれか1つの発明において、前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記駆動部によって移動される前記一方の物体または前記他方の物体または前記一方の物体および前記他方の物体、を移動させる位置または姿勢または位置および姿勢を補正する、物体の姿勢算出システムを提供する。
第10の発明は、第1〜第9のいずれか1つの発明において、前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記一方の物体または前記他方の物体の良否判定、または前記一方の物体もしくは前記他方の物体の種類判定、または前記駆動部によって保持される前記一方の物体もしくは前記他方の物体の保持状態の判定の内、少なくとも1つの判定を実行する、物体の姿勢算出システムを提供する。
第11の発明は、第1〜第10のいずれか1つの発明において、前記一方の物体および前記他方の物体における、前記凸部と前記凹部との組み合わせにおいて、前記凸部と前記凹部とは嵌合させることが可能であり、前記駆動部は、前記一方の物体と前記他方の物体とを相対的に移動させる、かつ、前記一方の物体および前記他方の物体の内、少なくとも1つの物体を保持し移動させる、かつ、前記一方の物体と前記他方の物体とを嵌合させるように移動が可能であり、前記物体の姿勢算出システムは、さらに、前記力計測部によって計測される力に基づいて、前記駆動部によって前記一方の物体と前記他方の物体とを相対的に移動させて力制御を実行し、前記一方の物体と前記他方の物体とを嵌合させる嵌合実行部を備え、前記嵌合実行部は、前記物体の姿勢算出部によって算出される、前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記一方の物体と前記他方の物体とを嵌合させる、物体の姿勢算出システムを提供する。
第12の発明は、第11の発明において、前記嵌合実行部は、前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢を修正するように移動させた後、前記一方の物体と前記他方の物体とを嵌合させる、物体の姿勢算出システムを提供する。
第13の発明は、第11の発明において、前記嵌合実行部は、前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記一方の物体と前記他方の物体との相対的な姿勢が大きいときには、前記一方の物体と前記他方の物体との相対的な姿勢を小さくする移動速度が大きくなるように前記力計測部によって計測される力に基づいて力制御を行い、前記一方の物体と前記他方の物体とを嵌合させる、物体の姿勢算出システムを提供する。
本発明では、一方の物体と他方の物体とが3つの接触点で接触しているときの、一方の物体と他方の物体との間に作用する力に基づいて、一方の物体と他方の物体との相対的な姿勢を算出するので、一方の物体と他方の物体を接触させることによって、一方の物体と他方の物体との相対的な姿勢が簡易に算出できる。
このため、本発明によれば、システムとして一方の物体と他方の物体とを3つの接触点で接触させ、一方の物体と他方の物体との間に作用する力を計測、算出することができるだけでよく、物体の位置および姿勢を高精度に検出可能なカメラや三次元センサなどの非接触式のセンサを必要とせず、システムを簡素化し、コストを抑えることが可能となる。また、一方の物体および他方の物体は、カメラや三次元センサなどの非接触式のセンサで、例えば表面状況などによって、検出できない物体、検出し難い物体でもよい。また、システムの状況による干渉などの問題のために、カメラや三次元センサなどの非接触式のセンサで一方の物体および他方の物体が検出できない状況にも本発明は適用できる。
また、本発明によれば、一方の物体と他方の物体との相対的な姿勢を算出するために、一方の物体を他方の物体に対して嵌合させる動作を必要としない。このため、嵌合作業にかかる時間が不要となり、システムのサイクルタイムを短くすることができる。また、嵌合作業の成功または失敗に依らず、相対的な位置および姿勢を算出できる。また、一方の物体および他方の物体は、嵌合させることが可能な物体でなくてもよい。また、一方の物体と他方の物体との相対的な姿勢が大きい場合に、一方の物体と他方の物体との相対的な姿勢を算出することができる。
また、本発明によれば、一方の物体と他方の物体との相対的な姿勢を算出するために、一方の物体を他方の物体に対して面を合わせる動作を必要としない。このため、面合わせ作業にかかる時間がなく、システムのサイクルタイムを短くすることができる。また、面合わせ作業の成功または失敗に依らず、相対的な位置および姿勢を算出できる。また、一方の物体および他方の物体は、面を合わせることが可能な物体でなくてもよい。
以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
本発明において、「力」とは、特に断りがない限り、力の並進方向成分および力のモーメント成分を含むものとする。また、「Aおよび/またはB」とは、AおよびBの少なくとも一方を含むものとし、例えば、「位置および/または姿勢」とは、位置および姿勢の少なくとも一方を含むものとする。
本実施例では、空間に対して設定した座標系とは、空間に対して固定させた直交座標系上で、駆動部の先端部分や、駆動部の先端部分に保持された(取りつけられた状態も含む)物体の位置および/または姿勢を表すことが可能な座標系とする。なお、駆動部の先端部分とは、駆動部において、物体を保持して移動させることが可能な部分とする。また、ツール座標系とは、駆動部の先端部分または駆動部の先端部分に保持された物体に対して設定される座標系であり、駆動部の台座に対して設定される座標系上または空間に対して設定した基準座標系上の、駆動部の先端部分または駆動部の先端部分に保持された物体の位置および/または姿勢を表すことが可能な座標系とする。また、制御点とは、駆動部の先端部分や先端部分に保持される物体の位置を表す点とする。また、力制御座標系とは、一方(第1)の物体と他方(第2)の物体との間に作用する力に基づいて、駆動部の先端部分に保持された物体の位置および/または姿勢を制御するために、制御点に原点を持つように設定される座標系とする。また、姿勢算出用の力計測座標系とは、一方の物体と他方の物体との間に作用する力に基づいて、物体の姿勢算出部が一方の物体と他方の物体との、相対的な姿勢、または、相対的な位置および相対的な姿勢を算出するために設定される座標系とする。また、或る座標系上の力に変換するとは、或る力について、或る座標系の原点に対して作用する力として、或る座標系上で表すこととする。
本実施例の説明において、数式における、”・”は行列とベクトルの乗算、”×”は外積、”*”は乗算、“/”は商算、“Cos(x)”は余弦関数、“Sin(x)”は正弦関数、“Arccos(x)”は余弦関数の逆三角関数、“Arcsin(x)”は正弦関数の逆三角関数、“Arctan(x)”は正接関数の逆三角関数、“≒”は左辺を右辺と近似することを表す。また、P(点b_座標系a)は、座標系a上の点bの位置と表す。また、(ベクトルA)^Tは、ベクトルAを転置した転置ベクトルを表す。
物体の姿勢算出システムは、一方の物体と他方の物体との相対的な姿勢を算出するシステムである。図1は本発明の1番目の実施形態に係る物体の姿勢算出システム11の構成例を示す概略図である。物体の姿勢算出システム11は、一方(第1)の物体21と、他方(第2)の物体22と、一方の物体21および他方の物体22の内、少なくとも1つの物体を保持し移動させる駆動部として一方の物体21を移動させる駆動部50と、一方の物体21と他方の物体22との間に作用する力を計測する力計測部31と、一方の物体21と他方の物体22との相対的な姿勢を算出する物体の姿勢算出部32とを備える。
物体の姿勢算出システム11は、さらに、駆動部制御装置52を備える。駆動部制御装置52は、駆動部50の動作指令を生成する動作指令生成部33の他に、力計測部31、物体の姿勢算出部32を備える。本実施例では、力計測部31、物体の姿勢算出部32は、駆動部制御装置52に備わるとしたが、駆動部制御装置52と接続された中央制御装置や、別の制御装置や演算処理装置などに含まれるようにしてもよい。
物体の姿勢算出システム11は、さらに、駆動部制御装置52を備える。駆動部制御装置52は、駆動部50の動作指令を生成する動作指令生成部33の他に、力計測部31、物体の姿勢算出部32を備える。本実施例では、力計測部31、物体の姿勢算出部32は、駆動部制御装置52に備わるとしたが、駆動部制御装置52と接続された中央制御装置や、別の制御装置や演算処理装置などに含まれるようにしてもよい。
駆動部50は1つ以上の制御軸によって構成され、各制御軸は駆動部制御装置52によって所定の制御周期毎に制御され、それにより駆動部50の先端部分が指定された場所に位置決めされ得るようになっている。駆動部制御装置52は、CPU、ROM、RAMなどを含むハードウェア構成を有していて、図示しないが、他に、記憶部、表示出力部、入力部、各種信号を出力する信号出力部なども具備し、後述する種々の機能を実行する。
図1に図示される駆動部50、図2に図示される駆動部50’は、6つの軸によって構成される垂直多関節型ロボットとするが、一方の物体21および他方の物体22を相対的に並進移動または回転移動または並進移動および回転移動することが可能であれば、他の形態を有する任意の公知のロボットや、移動機構を備える公知の駆動装置にも同様に適用可能である。他方の物体22が移動するとき、他方の物体22の位置および姿勢に基づいて、一方の物体21を移動させるようにしてもよい。
図1に図示される駆動部50、図2に図示される駆動部50’は、6つの軸によって構成される垂直多関節型ロボットとするが、一方の物体21および他方の物体22を相対的に並進移動または回転移動または並進移動および回転移動することが可能であれば、他の形態を有する任意の公知のロボットや、移動機構を備える公知の駆動装置にも同様に適用可能である。他方の物体22が移動するとき、他方の物体22の位置および姿勢に基づいて、一方の物体21を移動させるようにしてもよい。
動作指令生成部33は、駆動部50が滑らかに、かつ、所望の位置および/または姿勢に移動できるように、動作指令を生成し、駆動部50の各制御軸への動作指令を出力する。また、動作指令生成部38は、加加速度が急に大きくならないようにフィルタリング処理をしたり、また、駆動部50の動作の振動が少なくなるように振動の低減の処理をしたり、また、目的とする位置に正確に移動できるように駆動部50の撓みなどを考慮して指令の位置および/または姿勢を補正したりして動作指令を調整してもよい。図2に示されるように、駆動部制御装置52が、駆動部50、駆動部50’を移動させる場合、駆動部制御装置52は駆動部50’についても、駆動部50と同様に、駆動部50’が滑らかに、かつ、所望の位置および/または姿勢に移動できるように、動作指令を生成し、駆動部50’の各制御軸への動作指令を出力する。
図2は、本発明の1番目の別の一実施形態に係る物体の姿勢算出システム11aの構成例を示す概略図である。物体の姿勢算出システム11aは、一方の物体21と、他方の物体22と、一方の物体21および他方の物体22の内、少なくとも1つの物体を保持し移動させる駆動部として一方の物体21を保持し移動させる駆動部50と他方の物体22を保持し移動させる駆動部50’と、力計測部31と、物体の姿勢算出部32と、を備える。駆動部50’の先端部分には6軸の力センサ51’が取りつけられていている。
物体の姿勢算出システム11aのように、物体の姿勢算出システム11に対して、さらに、他方の物体22を移動させる、駆動部50’を備えるようにしてもよい。このように、物体の姿勢算出システムは、一方の物体と他方の物体を独立に移動させることができるように、駆動部を2つ備えるようにしてもよい。駆動部50’は、駆動部制御装置52によって各制御軸が制御され移動する。物体の姿勢算出システム11aにおける駆動部制御装置52は、物体の姿勢算出システム11における駆動部制御装置52に対して、さらに、動作指令生成部33が駆動部50’の動作指令も生成する。また、力計測部31は、駆動部50’の他方の物体22を保持する部分に作用する力に基づいて、一方の物体21と他方の物体22との間に作用する力を計測するようにしてもよい。また、駆動部制御装置52とは別に、駆動部50’を移動させる駆動部制御装置を備えるようにしてもよい。
図2に示す駆動部50’を使用する場合、一方の物体21と他方の物体22とを接触させるとき、駆動部50が干渉などせずに移動しやすいようにしたり、また、駆動部50が並進移動または回転移動だけすればよいようにしたりと、駆動部50が一方の物体21の凸部と他方の物体22の凹部とを接触させやすいように、駆動部50の移動可能な範囲、移動可能な方向、周辺装置の状況などを考慮して、駆動部50’によって他方の物体22を移動させるようにしてもよい。また、駆動部50’が他方の物体22を移動させるとき、その移動量を考慮して、駆動部50を移動させることが好ましい。また、駆動部50’の先端部分に備わる力センサ51’を使用し、その出力に基づいて、一方の物体21と他方の物体22との間に作用する力を力計測部31によって計測してもよい。
以下では、本発明の一実施形態について、図1に示す本発明の一実施形態に係る物体の姿勢算出システム11を用いて説明する。
物体の姿勢算出システム11は、駆動部50によって、一方の物体21と他方の物体22とを相対的に移動させ一方の物体21と他方の物体22とを接触させ、一方の物体21と他方の物体22とが3つの接触点で接触するときに、一方の物体21と他方の物体22との間に作用する力を計測する力計測部31の出力に基づいて、物体の姿勢算出部32によって一方の物体21と他方の物体22との相対的な姿勢または相対的な位置および姿勢を算出する。
一方の物体21および他方の物体22は、凸部を包含する物体と凹部を包含する物体の組み合わせであり、かつ、前記凸部と前記凹部とを3つの接触点において接触させることが可能な物体である。つまり、一方の物体21が凸部を包含するときには、他方の物体22は前記凸部と3つの接触点において接触させることが可能な凹部を1つ以上、包含する。また、一方の物体21が凹部を包含するときには、他方の物体22は前記凹部と3つの接触点において接触させることが可能な凸部を1つ以上、包含する。また、前記凹部における前記3つの接触点は前記凹部の穴の端部に位置する。また、前記凸部に対する軸であって前記3つの接触点を含む平面と直交しない軸を凸部中心軸とすると前記接触点と前記凸部中心軸との各最短距離が所定の値(一定値)となる、または、前記接触点から前記凸部中心軸に対して垂線を引いたときの前記凸部中心軸との交点と前記凸部中心軸上の所定の点(前記交点以外の点)との距離に基づいて前記接触点と前記凸部中心軸との各最短距離が定まる。
図3には、一方の物体21および他方の物体22に包含される、凸部と凹部とを3つの接触点において接触させる態様が図示されている。図3(a)には、一方の物体21に包含される円筒形の外側部分を凸部として、他方の物体22に包含される円筒形の内側部分を凹部として、一方の物体21の凸部と他方の物体22の凹部とを3つの接触点において接触させる態様が示されている。図3(b)には、一方の物体21に包含される円筒形の内側部分を凹部として、他方の物体22に包含される円筒形の外側部分を凸部として、一方の物体21の凸部と他方の物体22の凸部とを3つの接触点において接触させる態様が示されている。このように、一方の物体21の凸部を他方の物体22の凹部に対して3つの接触点で接触させてもよいし、一方の物体21の凹部を他方の物体22の凸部に対して3つの接触点で接触させてもよい。また、同じ大きさ程度の円筒形である場合、互いに凸部にも凹部にもなりうる。このように、円筒形のように、適切な大きさの物体であって、物体の中に空間がある物体である場合、凸部にも、凹部にもなりうる。また、図3(a)に示されるように、一方の物体21の凸部は、他方の物体22の凹部に対して、接触させる円筒の径が大きくても、小さくてもよい。また、図3(b)に示されるように、一方の物体21に包含される凹部は、他方の物体22に包含される凸部に対して、接触させる円筒の径が大きくても、小さくてもよい。
図4には、一方の物体21および他方の物体22が、凸部や凹部を包含する態様が示されている。一方の物体21および他方の物体22は、各々が少なくとも1組以上の凸部と凹部を有し、一方の物体21の凹部と他方の物体22の凸部、または一方の物体21の凸部と他方の物体22の凹部とは、3つの接触点において互いに接触させることが可能となっている。また、一方の物体21および他方の物体22は、大きさや形状の異なる、凸部や凹部を複数、包含するようにしてもよい。
図5には、一方の物体21および他方の物体22の少なくともいずれかの物体に包含される凸部23の形状の一例が図5に例示されている。凸部23は、図5(a)に示すように円柱形状の物体としてもよいし、図5(b)に示すように断面形状が円の一部が欠けた形状の物体としてもよいし、図5(c)に示すように断面形状が、図5(b)に示す断面形状よりもさらに円が欠けた形状の物体としてもよい。或いは、凸部23は、図5(d)に示すように断面形状が、円の3箇所が欠けた形状の物体としてもよいし、図5(e)に示すように、所定の位置までは断面形状が、円の2箇所が欠けた形状の物体であり、所定の位置以外の部分は円柱形状の物体としてもよい。或いは、凸部23は、図5(f)に示すように、円筒形状の物体のように、物体の内側に物体が存在しない空間がある物体としてもよいし、図5(g)、図5(h)に示すように、テーパー形状のように、先端からの距離に対し径が変化する物体としてもよい。テーパー形状の場合、図5(g)に示すように、径を線形的に増加させたり、減少させたりするようにしてもよいし、図5(h)に示すように、径を指数関数的、2次関数的など非線形的に、増加させたり、減少させたりするようにしてもよい。図5(a)、図5(f)、図5(g)、図5(h)に示される物体の場合、凸部23が軸対称となる軸を、凸部中心軸とすることができる。また、図5(b)、図5(c)、図5(d)、図5(e)に示される物体の場合、凸部23を最小限に包含する円柱形状の中心軸を、凸部中心軸とすることができる。
図6には、一方の物体21および他方の物体22の少なくともいずれかの物体に包含される凸部23における、凸部23に対して設定した凸部中心軸に沿った断面形状の一例が図6(a)、図6(b)に例示されている。凸部23は、凸部23に対して設定した凸部中心軸に沿った断面形状で見た場合、図6(a)に示す断面形状のように、先端部分に、例えば半球形状のような凸形状を保有し、それ以外の部分は円柱形状であるような物体としてもよい。この場合、凸部の先端部分が半球形状のような凸形状となっているので、凸部23は他方の物体22の凹部の端部などに存在する平面に対して面を合わせることができないため、他方の物体22に存在する面に対して面を合わせることによって凸部と凹部との相対的な姿勢を算出することはできない。また、凸部23は、凸部23に対して設定した凸部中心軸に沿った断面形状で見た場合、図6(b)に示す断面形状のように、先端部分に、例えば半球形状の窪みのような、凹形状を有し、それ以外の部分は円柱形状であるような物体としてもよい。
このように、一方の物体21および他方の物体22の少なくともいずれかに包含される凸部は、先端部分の断面形状が円形状や、円形状の一部を包含する形状を構成する形状であり、その断面形状が所定位置まで続き、所定位置以降は円柱形状を成す凸部の形状の部分が包含される形状であってもよい。また、一方の物体21および他方の物体22の少なくともいずれかに包含される凸部は、中空であったり、テーパーであったりしてもよい。また、一方の物体21および他方の物体22の少なくともいずれかに包含される凸部は、その凸部の、先端部分に凸形状や凹形状を有する物体であってもよい。このように、一方の物体21および他方の物体22の少なくともいずれかに包含される凸部は、前記条件を満たしていれば、凸部と凹部が3つの接触点で接触する部分以外はどのような形状であってもよい。
図7には、一方の物体21および他方の物体22の少なくともいずれかの物体に包含される凹部24の穴の端部の形状の一例が例示されている。凹部24は、図7(a)に示すように、凹部の穴の端部の形状が、円形状である物体としてもよいし、図7(b)に示すように、凹部の穴の端部の形状が、円の一部が欠けた形状である物体としてもよい。或いは、凹部24は、図7(c)に示すように、凹部の穴の端部の形状が、図7(b)に示す凹部の穴の端部の形状よりもさらに円が欠けた形状の物体としてもよいし、図7(d)に示すように、凹部の穴の端部の形状が、円の3箇所が欠けた形状の物体としてもよい。
図8には、一方の物体21および他方の物体22の少なくともいずれかの物体に包含される凹部24における、凹部の穴の中心軸方向に沿った断面形状の一例が例示されている。凹部24は、図8(a)に示すように、凹部の穴の端部と同じ形状が所定の位置まで続く形状である物体としてもよいし、図8(b)に示すように、例えば半球形状の窪みの形状が存在するような、凹部24の穴の中心軸方向に沿って、凹部24の穴の中心軸と直交する平面における凹部の断面形状が変わるような物体としてもよい。
このように、一方の物体21および他方の物体22の少なくともいずれかの物体に包含される凹部は、凹部の穴の端部の形状が円形状や、円形状の一部を包含する形状であるような、窪みとしてもよい。また、一方の物体21および他方の物体22の少なくともいずれかの物体に包含される凹部は、凹部の穴の中心軸方向に沿って、前記凹部の穴の中心軸と直交する平面における凹部の断面形状が変わるなど、凹部の穴の端部の形状と同じ形状が続くような物体でなくてもよい。また、一方の物体21および他方の物体22の少なくともいずれかの物体に包含される凹部は、凹部の穴の中心軸方向に沿って、凹部の穴を囲う、周囲の形状が変化していく、例えば半球形状の凸形状の物体に前記凹部の穴が存在するようにした形状などであってもよい。このように、一方の物体21および他方の物体22の少なくともいずれかに包含される凹部は、前記条件を満たしていれば、凸部と凹部が3つの接触点で接触する部分以外はどのような形状であってもよい。
図9には、前述のように説明した、一方の物体21および他方の物体22のいずれかに包含される凸部23と、前記物体とは異なる一方の物体21および他方の物体22のいずれかに包含される凹部24の、組み合わせの一例が例示されている。図9(a)には、凸部23が円柱形状であり、凹部24が円筒形状である場合が示されている。図9(b)には、凸部23、凹部24が共に円筒形状である場合が示されている。図9(c)には、凸部24が円柱形状であり、凹部24が、半球のような凸形状の物体に凹部の穴が存在する形状である場合が示されている。図9(d)には、凸部23が、円柱形状の先端に、半球のような凸形状の物体が存在する形状であり、凹部24が円筒形状である場合が示されている。
また、一方の物体21および他方の物体22の少なくともいずれかに包含される凸部の部分と、一方の物体21および他方の物体22の少なくともいずれかに包含される凹部の部分の内、少なくとも1つの部分を、包含される物体に対して、着脱可能な物体としてもよい。つまり、一方の物体21に包含される凸部の部分や凹部の部分は、一方の物体21に対して、着脱可能な物体であってもよい。また、他方の物体22に包含される凸部の部分や凹部の部分は、他方の物体22に対して、着脱可能な物体であってもよい。
駆動部は、物体を移動させる機構部が1つ以上存在し、一方の物体21と他方の物体22とを相対的に移動させることができるように構成され、一方の物体21および他方の物体22の内、少なくとも1つの物体を保持し移動させる。本実施例において、駆動部が物体を保持するとは、駆動部が物体を把持したり、駆動部に物体が設置されたり、取りつけられたりするなど、駆動部によって物体を移動することができるように物体を存在させた状態にする動作とする。図2を参照して説明したように、一方の物体21を移動させる場合は駆動部50を使用し、他方の物体22を移動させる場合は駆動部50’を使用するようにしてもよい。このように、前述のような1つ以上の駆動部によって、一方の物体21と他方の物体22とを相対的に移動させるようにしてもよい。駆動部は、並進移動または回転移動または並進移動および回転移動によって、一方の物体21と他方の物体22とを相対的に移動させ、一方の物体21と他方の物体22とを3つの接触点において接触させることができれば、どのような移動機構を備える機構部であってもよい。図1に示す駆動部50の一方の物体21を保持する先端部分には、駆動部50の先端部分に作用する力を検出するための、力センサ51が取りつけられている。
力センサ51は、一方の物体21と他方の物体22との間に作用する力を検出する検出装置である。また、力センサ51によって、駆動部50の先端部分に作用する力を検出することができる。駆動部制御装置52は、所定時間毎に検出された力センサ51の出力をもとに、力計測部31によって、一方の物体21と他方の物体22との間に作用する力を計測する。力センサ51は、本実施例では6軸の力センサとするが、作業に必要な自由度を有する力センサであればよい。また、力センサ51の取付位置は、一方の物体21と他方の物体22との間に作用する力を検出できる限り、駆動部50の先端部分、駆動部50の各制御軸、駆動部50の設置部分、他方の物体22の設置部分、図2に示した駆動部50’の先端部分など、任意の箇所であってよい。なお、本実施例においては、力計測部31は力センサ51の出力をもとに一方の物体21と他方の物体22との間に作用する力を計測するが、力センサ51を使用せず、他の物理的情報を検出して、一方の物体21と他方の物体22との間に作用する力を計測するようにしてもよい。
力計測部31は、力センサ51によって検出される力データに基づいて、一方の物体21と他方の物体22との間に作用する力を計測する。また力計測部31は、一方の物体21と他方の物体22との間に作用する力を計測するために、駆動部50の先端部分に取りつけられた力センサ51を使用しなくともよく、その場合は、駆動部50を構成する軸を移動させるアクチュエータがモータである場合は電流値、または、駆動部50を構成する制御軸の指令位置と実際の制御軸の位置との間の偏差、または、駆動部50の各制御軸に取付けられたトルクセンサの出力などをもとに、一方の物体21と他方の物体22との間に作用する力を推定して算出することによって、計測するようにしてもよい。
また、力計測部31は、駆動部50、一方の物体21、他方の物体22に作用する力を計測するようにしてもよい。このとき、駆動部50に取りつけた力センサ51、または、駆動部50を構成する軸を移動させるアクチュエータがモータである場合は電流値、または、駆動部50を構成する制御軸の指令位置と実際の制御軸の位置との間の偏差、または、駆動部50の各制御軸に取付けられたトルクセンサの出力などをもとに、駆動部50(駆動部50を構成する機構部、駆動部50の先端部分などを含む)や、駆動部50が保持して移動させる物体に作用する力を計測するようにする。また、駆動部50が移動させない物体である他方の物体22については、他方の物体22の下に設置した力センサを用いたり、別の駆動部50’が他方の物体22を保持している場合、駆動部50の場合と同様に計測したりしてもよい。
物体の姿勢算出部32は、駆動部50によって一方の物体21と他方の物体22とを相対的に移動させて一方の物体21と他方の物体22とを接触させ、一方の物体21と他方の物体22とが3つの接触点で接触するときに力計測部31によって計測される力、および接触点と凸部中心軸との最短距離に基づいて、一方の物体21と他方の物体22との相対的な姿勢を算出する、または一方の物体21と他方の物体22との相対的な位置および姿勢を算出する。
次に図10から図17を用いて、一方の物体21と他方の物体22との相対的な姿勢または相対的な位置および姿勢を算出する方法の一実施例について説明する。
図10(a)には、一方の物体21、他方の物体22の形状などに関する情報が図示されている。図10(b)には、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触している状態を、前記接触している3つの接触点を含む面に直交し、かつ、一方の物体21の凸部中心軸を含む平面を断面として見た様子が図示されている。
図11には、一方の物体21と他方の物体22とを3つの接触点で接触させた状況が図示されている。図11(a)には、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触している状態が図示されている。図11(b)には、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触している状態を、前記接触している3つの接触点を含む面に直交し、かつ、一方の物体21の凸部中心軸を含む平面を断面として見た様子が図示されている。図11(c)には、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触している状態を、パラメータの説明のために、仮想的に一方の物体21の凸部と他方の物体22の凹部とを離した様子が図示されている。
図12(a)には、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触している状態が図示されている。図12(b)には、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触している状態を、前記接触している3つの接触点を含む面に直交し、かつ、一方の物体21の凸部中心軸を含む平面を断面として見た様子が図示されている。
図13には、一方の物体21と他方の物体22とが2点で接触している状況が図示されている。図14には、一方の物体21に設定する姿勢算出用の力計測座標系の設定の様子が図示されている。図15には、一方の物体21と他方の物体22とを3つの接触点で接触させるときの動作の状況が図示されている。図16には、一方の物体21に設定する姿勢算出用の力計測座標系の設定の様子が図示されている。図17には、他方の物体22に包含される複数の凹部を利用して、一方の物体21に対する他方の物体22の位置および姿勢を算出する説明に関する図が記載されている。
本実施例では、説明の簡単化のために、図10(a)に示すように、一方の物体21に凸部が包含されていて、前記凸部が円柱形状である物体とする。また、他方の物体22に凹部が包含されていて、前記凹部は前記凸部の円柱形状と嵌合が可能である穴とする。一方の物体21の凸部と、他方の物体22の凹部とは、相対的な姿勢が所定の値以上であるとき、前述のように、3つの接触点において接触させることが可能な物体とする。
図10(a)に示されるように、一方の物体21の凸部の円柱形状の半径をrとし、直径をdとする。他方の物体22の凹部の穴の半径をRとし、直径をDとする。一方の物体21の凸部の円柱は、凸部と凹部が前述のように3つの接触点で接触することができるように、円柱の中心軸方向に所定の長さを有しているものとする。一方の物体21の凸部の円柱形状の先端の中心点を点Oaとする。一方の物体21の凸部に対する軸として円柱形状の中心軸に凸部中心軸Laが設定されている。なお、一方の物体21の凸部は、凸部中心軸Laについて軸対称となっている。他方の物体22の凹部の穴の端部の中心点を点Obと表す。
図11(a)に一方の物体21の凸部と他方の物体22の凹部が前述のように3つの接触点で接触している様子が図示されている。図11(a)に図示されるように、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触するとき、凸部に対する軸である凸部中心軸Laは、3つの接触点を含む平面と直交しない。さらに、他方の物体22の凹部における3つの接触点は凹部の穴の端部に位置する。さらに、3つの接触点と凸部中心軸との各最短距離が所定の値(一定値)であるか、または3つの接触点から凸部中心軸に対して垂線を引いたときの凸部中心軸との交点と凸部中心軸上の所定の点(該交点以外の点、ここでは点Oa)との距離に基づいて接触点と凸部中心軸との各最短距離が定まる(但し凸部が円柱形状の場合、前記交点と前記交点以外の点との距離が異なっても、前記各最短距離は一定値となる)。
一方の物体21の凸部と他方の物体22の凹部とが前述のように3つの接触点で接触している場合の3点を、点Pe1、点Pe2、点Psとする。他方の物体22の凹部に対して、点Pe1、点Pe2、点Psの3つの接触点は、他方の物体22の凹部の穴の端部であって一方の物体21側の端部に存在し、中心点Obを中心とする円周上に存在する。一方の物体21の凸部に対して、3つの接触点は、円柱形状の先端部分であって他方の物体22側の端部である円周上に点Pe1、点Pe2の2点が存在し、円柱形状の側面上に点Psの1点が存在する。一方の物体21の凸部における、他方の物体22と接触する3点と、凸部中心軸Laとの最短距離はそれぞれrとなる。
なお、一方の物体21の凸部の円柱形状の半径r、直径d、他方の物体22の凹部の穴の半径R、直径Dは、既知とするが、一方の物体21および他方の物体22の内、いずれかの物体について、事前に算出するようにしてもよい。例えば、一方の物体21を壁、台、別のワークなどの別の物体に対して傾け、Z軸を一方の物体21の凸部中心軸Laの軸に設定したツール座標系で、Z軸方向であって別の物体に近づく方向に押しつけたときのZ軸方向の力と、それ以外の軸の軸回りの力を基に算出してもよい。または、壁、台、別のワークなどの、別の物体に接触させたときの位置を基に、一方の物体21の径を算出してもよい。例えば、まず、固定させた別の物体に対して、一方の物体21の凸部の円形状の端部を接触させる。次に、一方の物体21の凸部を、凸部中心軸Laの軸回りに所定の角度だけ回転させて、固定させた別の物体に対して、一方の物体21の凸部の円形状の端部を接触させる。このとき、一方の物体21の凸部を、凸部中心軸Laの軸回りに180度回転させて接触させる、または、適当な角度で3回以上、接触動作と位置検出を行って、一方の物体21の凸部における半径rや直径dを求める処理などを実行する。他方の物体22の凹部の穴の半径R、直径Dについては、一方の物体21の凸部における半径rや直径dとほぼ同じとして求める。また、他方の物体22の凹部の穴の半径R、直径Dは、駆動部50に取り付けたピンにTCP(Tool Center Point)、制御点、ツール座標系を設定して、前記ピンを他方の物体22の凹部の穴の径を予め算出しておくようにしてもよい。
駆動部50が、一方の物体21を保持して移動させる。このとき、予め、一方の物体21の凸部に対して、点Oaに制御点を設定し、前記制御点を原点とするツール座標系を設定する。ツール座標系を設定しておくことによって、駆動部50に対する一方の物体21の凸部の位置および姿勢が定まるようにしておく。ツール座標系は、互いに直交するX軸、Y軸、Z軸によって構成される。Z軸は、凸部中心軸と一致するように設定され、+方向は、他方の物体22の凹部から離れる方向の成分を含む方向としている。X軸、Y軸の正負の方向は、一方の物体21に対する適切な方向を設定しておけばよい。
図11(b)に図示するように、一方の物体21の凸部と他方の物体22の凹部が前述のように3つの接触点で接触するとき、3つの接触点を含む面の法線と一方の物体21の凸部に対して設定される凸部中心軸Laとの成す角度を角度αとする。後述するように、角度αは、他方の物体22の凹部の穴に対して設定した座標系CFbのZ軸の+方向と、一方の物体21の凸部に対して設定した姿勢算出用の力計測座標系CFaのZ軸の+方向との成す角度となる。
ここで、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触するときの、一方の物体21の凸部の円柱形状の直径dと、他方の物体22の凹部の穴の直径Dと、角度αの関係について考える。
図10(b)には、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触している状態を、3つの接触点を含む面に直交し、かつ、一方の物体21の凸部中心軸Laを含む平面を断面として見た様子が図示されている。
図10(b)に図示されるように、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触するとき、3つの接触点を含む平面、ここでは他方の物体22の凹部の穴の端部を含む平面に対して、一方の物体21の凸部を、凸部中心軸Laの方向に射影する。前記3つの接触点を含む平面に射影された部分と、3つの接触点を含む面に直交かつ凸部中心軸Laを含む平面とが交わる部分は、その部分の端を点Psと端点Pqとするとき、点Psと端点Pqとを結ぶ直線となる。
点Psと端点Pqとを結ぶ直線の長さは、d/(Cos(α))となる。一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触する場合、点Psと端点Pqとを結ぶ直線の長さはD以上であり、以下の条件を満たす。
d/(Cos(α))≧D (1)
ここで、D>dである。
式(1)より式(2)が求められる。
α≧Arccos(d/D) (2)
d/(Cos(α))≧D (1)
ここで、D>dである。
式(1)より式(2)が求められる。
α≧Arccos(d/D) (2)
式(2)より、一方の物体21の凸部の円柱形状の直径dと、他方の物体22の凹部の穴の直径Dの大きさが近くなるほど、角度αの値が小さくなり、相対的な姿勢が小さくても3つの接触点において接触することが分かる。また、式(2)より、例えば、凸部の直径が(30−0.005)mmで、凹部の直径が(30+0.005)mmの場合、角度αが1.48度程度以上であって90度より小さい値程度までの場合、凸部と凹部は3つの接触点において接触することが分かる。また、凸部の直径が(50−0.005)mmで、凹部の直径が(50+0.005)mmの場合、角度αが1.15度程度以上であって90度より小さい値程度までの場合、凸部と凹部は3つの接触点において接触することが分かる。
なお、凸部と凹部を嵌合させる場合であって、凸部と凹部との相対的な姿勢のずれが大きい場合、通常、一方の物体21の凸部と他方の物体22の凹部との間に作用する力に基づく力制御によって嵌合させることや、所定の時間内に嵌合させることは、困難な場合が多い。このような場合は、本発明によって、一方の物体21の凸部と他方の物体22の凹部との相対的な姿勢、または相対的な位置および姿勢を算出し、算出した相対的な姿勢、または相対的な位置および姿勢を利用して、例えば、一方の物体21の凸部と他方の物体22の凹部とを、前記算出した相対的な姿勢分を補正した位置関係に移動させた後、力制御により嵌合させることなどによって、一方の物体21の凸部と他方の物体22の凹部をなるべく少ない失敗回数で、またはなるべく短い時間で嵌合させることが容易となる。
一方の物体21と他方の物体22とが3つの接触点で接触するときに力計測部31によって計測される力、および接触点と凸部中心軸との最短距離に基づいて、一方の物体21と他方の物体22との相対的な姿勢、または、相対的な位置と相対的な姿勢を算出するときには、姿勢算出用の力計測座標系を用いる。一方の物体21と他方の物体22との間に作用する力を姿勢算出用の力計測座標系上の力に変換した力に基づいて、一方の物体21と他方の物体22との相対的な姿勢、または、相対的な位置と相対的な姿勢を算出する。図11(a)、図11(c)に図示するように、一方の物体21の凸部に対して設定した姿勢算出用の力計測座標系を座標系CFaとする。なお、一方の物体21の凸部と他方の物体22の凹部との相対的な位置および姿勢が或る範囲以内にあるとき、または一方の物体21と他方の物体22とが接触しているとき、一方の物体21の凸部に対して設定する姿勢算出用の力計測座標系CFaのZ軸の−方向を含む方向に所定の目標力で押しつけると共に、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部を移動させれば、一方の物体21の凸部と他方の物体22の凹部とを2つまたは3つの接触点で接触させることが可能であるとする。また、以下では、一方の物体21の凸部と他方の物体22の凹部とは、姿勢算出用の力計測座標系CFaのZ軸の−方向を含む方向に所定の目標力で押しつけると共に、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部を移動させることによって、一方の物体21の凸部と他方の物体22の凹部とを2つまたは3つの接触点で接触させることが可能な状態にあるとする。
姿勢算出用の力計測座標系CFaの座標系の設定方法について説明する。姿勢算出用の力計測座標系CFaが次のように設定されたとき、姿勢算出用の力計測座標系CFaの座標系の設定が完了した状態とする。まず、姿勢算出用の力計測座標系CFaは、互いに直交するX軸、Y軸、Z軸によって構成される。座標系の原点は、凸部中心軸上であって、一方の物体21の凸部の円形状の中心点Oaに設定する。姿勢算出用の力計測座標系CFaの、X軸、Y軸、Z軸の設定は、一方の物体21の凸部と他方の物体22の凹部が接触するときに以下のような条件を満たすように設定する。Z軸は、凸部中心軸と一致するように設定され、+方向は、他方の物体22の凹部から離れる方向の成分を含む方向とする。次に、Y軸は、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触するときの前述の接触点Pe1と接触点Pe2とを結んだ直線と平行となるように設定する。X軸の+方向は、他方の物体22の凹部から離れる方向の成分を含む方向となるようにする。
姿勢算出用の力計測座標系CFaの設定を、次のようにして完了させる。
まず、互いに直交するX軸、Y軸、Z軸によって構成される座標系の原点を一方の物体21の凸部の円形状の中心点Oaに設定する。Z軸を凸部中心軸(この場合、円柱形状の中心の軸)と一致するように設定し、+方向は、他方の物体22の凹部から離れる方向の成分を含む方向となるように設定する。以降で、X軸、Y軸について、Z軸の軸回りの方向を定める。なお、X軸、Y軸のどちらかが定まれば、別の軸の向きも定まる。
一方の物体21の凸部を他方の物体22の凹部に対して、姿勢算出用の力計測座標系CFaのZ軸の−方向、または一方の物体21に設定したツール座標系のZ軸の−方向、または空間に設定した座標系上での他方の物体22の凹部に対して近づく方向など、一方の物体21の凸部と他方の物体22の凹部が接触する方向に移動させる。一方の物体21の凸部と他方の物体22の凹部とが接触した後は、姿勢算出用の力計測座標系CFaのZ軸の−方向に所定の目標力で押しつけると共に、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部を移動させることによって、一方の物体21の凸部と他方の物体22の凹部とを2つまたは3つの接触点で接触させるようにする。このとき、一方の物体21が移動しなくなったときに、一方の物体21の凸部と他方の物体22の凹部とは、2つまたは3つの接触点で接触したと判定する。
ここでは、このように一方の物体21の凸部を移動させて、一方の物体21の凸部と他方の物体22の凹部とを、図13(a)、図13(b)で図示されるように、点Pe1と点Pe2の2点で接触させる場合について説明するが、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触する場合も同様である。図13(b)では、図13(a)に図示されるように、一方の物体21の凸部と他方の物体22の凹部とが2点で接触する状況を、他方の物体22の凹部の穴の端部に直交し、凸部中心軸Laを含む断面から見た様子を図示している。また、その断面に接触点Pe1と作用する力Feを射影している。図13(b)に図示されるように、一方の物体21の凸部と他方の物体22の凸部が2点で接触している状況であり、他方の物体22の凹部に対して一方の物体21の凸部を姿勢算出用の力計測座標系CFaのZ軸の−方向である押付方向Pd1に所定の目標力で押しつけると共に、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部を移動させることによって、一方の物体21の凸部と他方の物体22の凹部とを2つの接触点で接触させる。なお、この移動によって、3つの接触点で接触することもがあるが、説明については同様である。
図14(a)には、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触するとき、一方の物体21の凸部の先端部分を、姿勢算出用の力計測座標系CFaのX軸、Y軸によって構成される平面上で見た態様が図示されている。姿勢算出用の力計測座標系CFaのX軸、Y軸によって構成される平面には、接触点Pe1、接触点Pe2が存在する。角度βは、姿勢算出用の力計測座標系上のX軸とY軸とで構成される平面上において、Y軸の−方向と、点Oaと一方の物体21の凸部における接触点Pe1とを結ぶ線分との成す角度とする。
ここで、姿勢算出用の力計測座標系CFaの設定が完了した状態になっていて、姿勢算出用の力計測座標系CFaのZ軸に対して、姿勢算出用の力計測座標系CFaのX軸、Y軸は図14(a)に図示されるような或る姿勢になっているとする。
なお、姿勢算出用の力計測座標系CFaのX軸方向の力をFx、Y軸方向の力をFy、Z軸方向の力をFz、X軸の軸回りの力をMx、Y軸の軸回りの力をMy、Z軸の軸回りの力をMzとする。前述のように、角度βは、姿勢算出用の力計測座標系上のX軸とY軸とで構成される平面上において、Y軸の−方向と、点Oaと一方の物体21の凸部における接触点Pe1とを結ぶ線分との成す角度とする。μは、静止摩擦係数とする。接触点Pe1には、姿勢算出用の力計測座標系上のZ軸方向に作用する力であるFeと、姿勢算出用の力計測座標系上のX軸方向に作用する力であるμ*Feが作用する。接触点Pe2には、姿勢算出用の力計測座標系上のZ軸方向に作用する力であるFeと、姿勢算出用の力計測座標系上のX軸方向に作用する力であるμ*Feが作用する。接触点Pe1と接触点Pe2は、一方の物体21の凸部および他方の物体22の凹部において、他方の物体22の凹部の穴の端部を含む面に直交し、かつ、点Oa、凸部中心軸Laを含む面に対して面対称となる。つまり、姿勢算出用の力計測座標系CFa上での、接触点Pe1と接触点Pe2の位置については、X軸上、Z軸上の値は同じであり、Y軸上の値は正負が異なる値となる。また、他方の物体22の凹部に対して設定した座標系CFb上での、接触点Pe1と接触点Pe2の位置については、X軸上、Z軸上の値は同じであり、Y軸上の値は正負が異なる値となる。
このとき、静止摩擦係数μが0に近似できるとすると、Fx、Fy、Fz、Mx、My、Mzは、それぞれ以下の式(3)−(8)で表される。
Fx = 0 (3)
Fy = 0 (4)
Fz = 2*Fe (5)
Mx = 0 (6)
My = 2*Fe*r*Sin(β) (7)
Mz = 0 (8)
Fx = 0 (3)
Fy = 0 (4)
Fz = 2*Fe (5)
Mx = 0 (6)
My = 2*Fe*r*Sin(β) (7)
Mz = 0 (8)
次に、別の或る姿勢算出用の力計測座標系CFaについて考える。一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触するとき、Z軸は同じで、上記の座標系を基準の座標系とすると、上記の基準の座標系のX軸をX’軸、Y軸をY’軸とし、Z軸の軸回りにθだけ回転したX軸、Y軸で構成される座標系を、前記別の或る座標系とする。つまり、別の或る座標系をZ軸の軸回りに−θだけ回転させれば、図14(a)に示される基準の座標系と同じ座標系となる。一方の物体21の凸部と他方の物体22の凹部との間に作用する力を前記別の或る座標系上に変換した力を考える。
図14(b)には、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触するとき、姿勢算出用の力計測座標系CFaのX軸、Y軸によって構成される平面上で見た態様が図示されている。姿勢算出用の力計測座標系CFaのX軸、Y軸によって構成される平面には、接触点Pe1、接触点Pe2が存在する。角度βは、姿勢算出用の力計測座標系上のX軸とY軸とで構成される平面上において、Y軸の−方向と、点Oaと一方の物体21の凸部における接触点Pe1とを結ぶ線分との成す角度とする。また、図14(b)には、前記基準の座標系のX’軸とY’軸と、別の或る座標系のX軸、Y軸とが図示されている。
このとき、一方の物体21の凸部と他方の物体22の凹部との間に作用する力を前記別の或る座標系上に変換した力について、静止摩擦係数μが0に近似できるとすると、以下の式(9)−(14)が成立する。
Fx = 0 (9)
Fy = 0 (10)
Fz = 2*Fe (11)
Mx = Fe*r*(Sin(−θ+π/2−β)+Sin(−θ−π/2+β))
(12)
My = Fe*r*(Cos(−θ+π/2−β)+Cos(−θ−π/2+β))
(13)
Mz = 0 (14)
Fx = 0 (9)
Fy = 0 (10)
Fz = 2*Fe (11)
Mx = Fe*r*(Sin(−θ+π/2−β)+Sin(−θ−π/2+β))
(12)
My = Fe*r*(Cos(−θ+π/2−β)+Cos(−θ−π/2+β))
(13)
Mz = 0 (14)
式(12)、(13)より、以下の式(15)によってθが算出される。
θ = Arctan(−Mx/My) (15)
また、一方の物体21の凸部と他方の物体22の凹部を接触させるために近づけた方向や、Z軸の−方向に他方の物体22の凹部が存在するという条件から、X軸の+方向が他方の物体22の凹部から離れる方向の成分を含む方向となるように角度θを求める。算出したθに基づいて、前記別の或る基準座標系から、前記基準の座標系が求められる。このようにして式(3)から式(8)を満たす、姿勢算出用の力計測座標系CFaを設定することができる。
θ = Arctan(−Mx/My) (15)
また、一方の物体21の凸部と他方の物体22の凹部を接触させるために近づけた方向や、Z軸の−方向に他方の物体22の凹部が存在するという条件から、X軸の+方向が他方の物体22の凹部から離れる方向の成分を含む方向となるように角度θを求める。算出したθに基づいて、前記別の或る基準座標系から、前記基準の座標系が求められる。このようにして式(3)から式(8)を満たす、姿勢算出用の力計測座標系CFaを設定することができる。
これによって、Y軸を、点Pe1と点Pe2とを結んだ直線と平行となるように設定し、X軸の+方向は、他方の物体22の凹部から離れる方向の成分を含む方向となるようにする座標系を設定することができる。この設定方法は、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触する場合も同様である。以上によって、姿勢算出用の力計測座標系CFaの設定を完了させることができる。
また、次のように、姿勢算出用の力計測座標系CFaの設定を完了させるようにしてもよい。
姿勢算出用の力計測座標系CFaの原点とZ軸を設定し、一方の物体21の凸部と他方の物体22の凹部とを2つまたは3つの接触点で接触させる方法は前述と同様とする。これによって、一方の物体21の凸部と他方の物体22の凹部とは、図13に示されるように、前述のように、2つまたは3つの接触点で接触したとする。以降で、X軸、Y軸について、Z軸の軸回りの方向を定める。なお、X軸、Y軸のどちらかが定まれば、別の軸の向きも定まる。
図16(a)には、一方の物体21の凸部と他方の物体22の凹部とが2点で接触する状況を、他方の物体22の凹部の穴の端部に直交し、凸部中心軸Laを含む断面から見た様子を図示している。ここでは2点の接触の場合について説明するが、3点の場合でも同様である。図16(a)に図示されるように、一方の物体21の凸部と他方の物体22の凹部とが接触した後、Z軸の−方向である押付方向Pd1に、他方の物体22の凹部に対して一方の物体21の凸部を所定の目標力で押しつけ、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行している。
このときに、押付方向Pd1と直交する、次に述べる方向について、現在の位置から、所定の距離を上限として移動させるときに、力がかかる場合、所定の目標力で押しつけるように力制御して所定時間だけ経過したら停止させる。このとき、停止させるまでの移動量を記録しておく。その後、移動させる前の位置に戻すように移動させる。このような力制御とそのときの移動量を検出する動作を実行する。このときの移動方向であって押付方向とする方向は、点Oaを含み、Z軸と直交する平面内における、点Oaを基準とした正方向のベクトルと、そのベクトルに対してZ軸について軸対称な、点Oaを基準とした逆方向のベクトルに定められる方向とする。この或る方向とその逆方向への往復動作を1回以上させながら、前記移動量を検出する動作をZ軸の軸回りに方向を変えながら実行する。例えば、正方向の+Mv1方向と、逆方向の−Mv1方向の前記移動量を検出する往復動作を1回以上、繰り返し、次に、+Mv2方向と、−Mv2方向の前記移動量を検出する往復動作を1回以上、繰り返すといったことを、複数の方向である、+Mv1方向と−Mv1方向、+Mv2方向と−Mv2方向、+Mv3方向と−Mv3方向、+Mv4方向と−Mv4方向、について繰り返す。
このように、一方の物体21の凸部を移動させようとしたときに、一方の物体21の凸部の位置が移動しない、または所定の値以下である、または最小となるときの、正方向および逆方向をY軸上の方向と判断するという、Y軸の探索動作を実行する。このような方向の探索方法などによって、一方の物体21の凸部の先端部分の2点の接触点を結ぶ線分と平行な方向を探索して検出する。なお、前記のような方法とは異なる探索方法であって一方の物体21の凸部の先端部分の2点の接触点を結ぶ線分と平行な方向が探索可能な方法を用いてもよい。一方の物体21の凸部と他方の物体22の凹部を接触させるために近づけた方向や、Z軸の−方向に他方の物体22の凹部が存在するという条件から、X軸の+方向が他方の物体22の凹部から離れる方向の成分を含む方向となるように設定することによって、Y軸の正負の方向も定まる。または、一度、X軸、Y軸を設定した後、X軸の−方向に移動させるときに所定の値を超える力を検出したら停止させるように移動させるときに、Y軸の軸回りのモーメントが大きくなる場合には設定が正しく、Y軸の軸回りのモーメントが小さくなる場合には、X軸、Y軸の正負が逆であると判定して、X軸、Y軸の正負の方向を変えるようにしてもよい。
また、姿勢算出用の力計測座標系CFaの設定を、次のようにして、完了させた状態にするとしてもよい。
一方の物体21の凸部と他方の物体22の凹部とが接触するときに、一方の物体21に対する姿勢算出用の力計測座標系CFaの設定が完了した状態となるように、予め、一方の物体21の凸部に対して姿勢算出用の力計測座標系CFaを設定しておき、一方の物体21の凸部を他方の物体22の凹部に移動させる。この場合、一方の物体21の凸部と他方の物体22の凹部とが接触する3つの接触点を含む平面と直交し、かつ、凸部中心軸Laを含む平面が既知の平面となるように、一方の物体21の凸部と他方の物体22の凹部とを接触させるようにする。前述のように、一方の物体21に対する姿勢算出用の力計測座標系CFaの設定が予め完了している場合、一方の物体21の凸部を姿勢算出用の力計測座標系CFaのZ軸の−方向に所定の目標力で押しつけると共に、姿勢算出用の力計測座標系CFaのX軸の−方向に所定の目標力で押しつけるようにし、Y軸方向については目標力を0とするような受動的な力制御を実行して、Z軸、X軸方向について、所定の目標力で押しつけ、一方の物体21が移動しなくなったとき、一方の物体21の凸部と他方の物体22の凹部とは3つの接触点で接触したと判定する。なお、所定の目標力は、各方向について異なる大きさであってもよい。また、一方の物体21の凸部を姿勢算出用の力計測座標系CFaのZ軸の−方向に所定の目標力で押しつけると共に、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部がZ軸方向について所定の目標力で押しつけるようになり移動しなくなった後、一方の物体21の凸部を姿勢算出用の力計測座標系CFaのZ軸の−方向に所定の目標力で押しつけると共に、姿勢算出用の力計測座標系CFaのX軸の−方向に所定の目標力で押しつけるようにし、Y軸方向については目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部をZ軸、X軸方向について、所定の目標力で押しつけるようにして、移動させるようにしてもよい。
前述のように姿勢算出用の力計測座標系CFaを設定するために、一方の物体21の凸部と他方の物体22の凹部との姿勢が拘束されるように、別の機構部などの物体を取りつけたり、一方の物体21の凸部の姿勢に関する移動動作を制限するようにしたりするなどしてもよい。このようにして、一方の物体21の凸部と他方の物体22の凹部とが相対的に姿勢算出用の力計測座標系CFaのX軸とZ軸を含む平面と、平行な平面上に拘束されるようにしておく。一方の物体21の凸部と他方の物体22の凹部に対して前記のような姿勢に関する条件が求められるとしても、そのような条件が成立する機構を保有する装置や機構部、またそのような条件の下で駆動部50が一方の物体21を他方の物体に対して移動させる場合などでは、一方の物体21の凸部に対する姿勢算出用の力計測座標系CFaのZ軸の軸回りの姿勢を推定する必要がなく、必要最低限の方法によって、一方の物体21の凸部と他方の物体22の凹部との相対的な位置および姿勢をより正確に算出することができるようになる。このように、一方の物体21と他方の物体22とが、同一平面内にあって相対的な角度が変わる場合に、一方の物体21と他方の物体22との間に作用する力に基づいて、一方の物体21と他方の物体22との相対的な姿勢を算出することができるので、別の作業や状態監視などのために力センサを備えている場合に、他のセンサなどを利用することなく、一方の物体21と他方の物体22との相対的な姿勢を算出することができる。
以上のようにして、姿勢算出用の力計測座標系CFaの設定を完了させることによって、空間に対して設定した座標系や、駆動部50に設定される座標系、ツール座標系に対する、一方の物体21に対して設定する姿勢算出用の力計測座標系CFaの同次変換行列や位置および姿勢が設定される。
次に、姿勢算出用の力計測座標系CFaの設定が完了した状態で、または姿勢算出用の力計測座標系CFaの設定を完了させるために、一方の物体21の凸部を姿勢算出用の力計測座標系CFaのZ軸の−方向に所定の目標力で押しつけて、一方の物体21の凸部と他方の物体22の凹部とを3点または3点で接触させる前の2点で接触させるときの移動について考える。なお、一方の物体21の凸部と他方の物体22の凹部との相対的な姿勢が、3つの接触点で接触させることが可能な相対的な姿勢であるとき、一方の物体21の凸部と他方の物体22の凹部とは2つの接触点で接触させることが可能である。一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触する条件についての前述の説明のように、一方の物体21の凸部と他方の物体22の凹部との相対的な姿勢である角度αが所定の値より大きければ、一方の物体21の凸部と他方の物体22の凹部とは、3つの接触点で接触させることができる。
一方の物体21の凸部と、他方の物体22の凹部との相対的な姿勢が大きく、3つの接触点で接触させることが可能であることが分かっている場合、姿勢算出用の力計測座標系CFaのZ軸の−方向、または空間に設定した座標系上での他方の物体22の凹部に対して近づく方向など、一方の物体21の凸部と他方の物体22の凹部とが接触する方向に、一方の物体21の凸部を移動させる。一方の物体21の凸部と他方の物体22の凹部とが接触した後は、一方の物体21の凸部を姿勢算出用の力計測座標系CFaのZ軸の−方向に所定の目標力で押しつけると共に、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部を移動させることによって、一方の物体21の凸部と他方の物体22の凹部とを2つまたは3つの接触点で接触させることができる。このとき、一方の物体21が移動しなくなったときに、一方の物体21の凸部と他方の物体22の凹部とは、2つまたは3つの接触点で接触したと判定する。
一方の物体21の凸部と、他方の物体22の凹部との相対的な姿勢が、3つの接触点で接触させることが可能な姿勢となっているか否かが分からない場合、一方の物体21の凸部と他方の物体22の凹部との相対的な姿勢を所定の値だけ変わるように移動させる。この場合、相対的な姿勢が変わる方向、相対的な姿勢がどの程度の範囲以内か、などの情報を予め持っておき、それらに基づいて、所定の値だけ姿勢を変えることによって、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触することが可能な相対的な姿勢となるようにする(このときの姿勢の移動量を記録しておき、後で一方の物体21の凸部と他方の物体22の凹部との相対的な位置および姿勢を算出した後、その移動量分を補正して、前記姿勢の移動をさせる前の、一方の物体21の凸部と他方の物体22の凹部との相対的な姿勢を算出する)。その後、姿勢算出用の力計測座標系CFaのZ軸の−方向、または空間に設定した座標系上での他方の物体22の凹部に対して近づく方向など、一方の物体21の凸部と他方の物体22の凹部が接触する方向に、一方の物体21の凸部を移動させる。一方の物体21の凸部と他方の物体22の凹部とが接触した後は、一方の物体21の凸部を姿勢算出用の力計測座標系CFaのZ軸の−方向に所定の目標力で押しつけると共に、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部を移動させることによって、一方の物体21の凸部と他方の物体22の凹部とを2つまたは3つの接触点で接触させることができる。このとき、一方の物体21が移動しなくなったときに、一方の物体21の凸部と他方の物体22の凹部とは、2つまたは3つの接触点で接触したと判定する。
一方の物体21の凸部と、他方の物体22の凹部との相対的な姿勢の範囲が小さく、3つの接触点で接触させることができない姿勢である場合、一方の物体21の凸部と他方の物体22の凹部との相対的な姿勢を、一方の物体21の凸部と他方の物体22の凹部とを3つの接触点で接触できる相対的な姿勢とするように、所定の角度だけ姿勢が変わるように移動させる(このときの姿勢の移動量を記録しておき、後で一方の物体21の凸部と他方の物体22の凹部との相対的な位置および姿勢を算出した後、その移動させた分を補正して、前記姿勢の移動をさせる前の、一方の物体21の凸部と他方の物体22の凹部との相対的な姿勢を算出する)。その後、姿勢算出用の力計測座標系CFaのZ軸の−方向、または空間に設定した座標系上での他方の物体22の凹部に対して近づく方向など、一方の物体21の凸部と他方の物体22の凹部が接触する方向に、一方の物体21の凸部を移動させる。一方の物体21の凸部と他方の物体22の凹部とが接触した後は、姿勢算出用の力計測座標系CFaのZ軸の−方向に所定の目標力で押しつけると共に、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部を移動させることによって、一方の物体21の凸部と他方の物体22の凹部とを2つまたは3つの接触点で接触させることができる。このとき、一方の物体21が移動しなくなったときに、一方の物体21の凸部と他方の物体22の凹部とは、2つまたは3つの接触点で接触したと判定する。
また、上記で、一方の物体21と他方の物体22とが、3つの接触点で接触させることが可能な姿勢となっているかは、一方の物体21の凸部と他方の物体22の凹部とを接触させたときのモーメントの大きさに基づいて判断してもよい。
このように、一方の物体21の凸部と他方の物体22の凹部とを3つの接触点で接触させる状態にすることができる、相対的な姿勢とするように、一方の物体21および他方の物体22の少なくとも1つを所定の値以上だけ姿勢を変えて、そのときの姿勢の移動量を記録しておき、一方の物体21と他方の物体22との相対的な姿勢を算出した後、前記記録しておいた移動量分を補正して、一方の物体21と他方の物体22との、初めの相対的な姿勢を算出するようにしてもよい。
一方の物体21の凸部に対する、姿勢算出用の力計測座標系CFaの設定が完了した後、一方の物体21を移動させ、一方の物体21の凸部と他方の物体22の凹部とを、3つの接触点で接触させる。
図15(a)、図15(b)には、一方の物体21の凸部と他方の物体22の凹部とを接触させる状況を、他方の物体22の凹部の穴の端部を含む平面に直交し、かつ、凸部中心軸Laを含む平面を断面として見た様子が図示されている。また、姿勢算出用の力計測座標系CFaのY軸が紙面に対して直交している。
姿勢算出用の力計測座標系CFaの設定が完了した後、Z軸の−方向である押付方向Pd1と、X軸の−方向である押付方向Pd2について、所定の目標力で押しつけるようにし、Y軸方向については、目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部を移動させ、押付方向Pd1、押付方向Pd2について、所定の目標力で押しつけ、一方の物体21が移動しなくなったとき、一方の物体21の凸部と他方の物体22の凹部とは3つの接触点で接触したと判断する。図15(a)には、前述のように、一方の物体21の凸部と他方の物体22の凹部とを2つの接触点で接触させ、姿勢算出用の力計測座標系CFaの設定が完了させた時の一方の物体21の凸部の位置が一方の物体21aと示されている。また、図15(a)には、その後、前述のように移動させ、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触した時の様子が図示されている。
また、一方の物体21の凸部に対する、姿勢算出用の力計測座標系CFaの設定が完了した後、一方の物体21の凸部と他方の物体22の凹部とを、別の移動方法によって移動させ、3つの接触点で接触させるようにしてもよい。一方の物体21の凸部を姿勢算出用の力計測座標系CFaのZ軸の−方向である押付方向Pd1に所定の目標力で押しつけると共に、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部を移動させることによって、一方の物体21の凸部と他方の物体22の凹部とを2つまたは3つの接触点で接触させる。このとき、押付方向Pd1に所定の目標力で押しつけ、一方の物体21が移動しなくなったとき、一方の物体21の凸部と他方の物体22の凹部とは2つまたは3つの接触点で接触したと判断する。前述のように、姿勢算出用の力計測座標系CFaの設定が完了した後、一方の物体21の凸部をZ軸の−方向である押付方向Pd1について、所定の目標力で押しつけながら、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行して、Y軸の軸回りについて−方向であるRY方向に、Y軸を中心として回転させていき、Y軸の軸回りの力が所定の目標力となる、または接触を検出したときに、3つの接触点で接触したと判定し、回転移動を停止させる。これによって、一方の物体21の凸部と他方の物体22の凹部とを3つの接触点で接触させることができる。図15(b)には、姿勢算出用の力計測座標系CFaの設定が完了した時の一方の物体21の凸部の位置が一方の物体21aと示され、押付方向Pd1、Y軸の軸回りの方向RYに移動させ、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触した時の様子が図示されている。このときY軸の軸回りに回転させた角度を記録しておき、凸部中心軸Laと他方の物体22の凹部に設定した座標系CFbのZ軸との相対角度を算出した後、前記記録した角度分を補正することによって、Y軸の軸回りに回転させる前の一方の物体21の凸部と他方の物体22の凹部について、凸部中心軸Laと他方の物体22の凹部に設定した座標系CFbのZ軸との相対角度を算出することができる。
なお、図5(b)、図5(c)、図5(d)、図5(e)に示されるように、凸部の先端部分が円の欠けた形状などである場合、凸部と凹部とを前述のように2つの接触点や3つの接触点で接触させることが可能なように、凸部と凹部とを接触させる前に、或る所定の範囲の位置および姿勢に移動させておくことが好ましい。
前述のように、姿勢算出用の力計測座標系CFaの設定を完了させた状態にして、その設定を基に一方の物体21の凸部と他方の物体22と凹部とを3つの接触点で接触させる。これによって、一方の物体21の凸部と他方の物体22の凹部の穴部の端部とを3つの接触点で接触させ、設定された姿勢算出用の力計測座標系CFaと3つの接触点との位置および姿勢の関係を図11に示すような状況とすることができる。
一方の物体21の凸部と他方の物体22の凹部とを3つの接触点で接触させた状態について考える。
一方の物体21の凸部と他方の物体22の凹部とが接触点Pe1、接触点Pe2、接触点Psで接触するとき、3つの接触点と、凸部中心軸Laでもある、姿勢算出用の力計測座標系CFaのZ軸との各最短距離は、一定値であるrとなる(本実施例では、接触点と、姿勢算出用の力計測座標系CFaのZ軸との各最短距離は一定値とするが、接触点と、姿勢算出用の力計測座標系CFaのZ軸との各最短距離は姿勢算出用の力計測座標系CFaのZ軸方向の長さに基づいて定まるようにしてもよい。その場合においても、一方の物体21と他方の物体22との相対的な姿勢、または相対的な位置および姿勢の算出方法は、本実施例における説明と同様である)。また、凸部中心軸Laでもある、姿勢算出用の力計測座標系CFaのZ軸は、3つの接触点を含む平面と直交しない。また、3つの接触点は、他方の物体22の凹部の穴の端部に位置する。また、姿勢算出用の力計測座標系CFaのX軸、Y軸、Z軸と、3つの接触点との位置および姿勢の関係は、次のように設定される。姿勢算出用の力計測座標系CFaのY軸は、接触点Pe1と接触点Pe2とを結ぶ直線と平行、かつ、接触点Pe1から接触点Pe2への方向と平行な方向が+方向となる。また、Z軸の−方向、X軸の−方向は、他方の物体22の凹部に近づく方向を成分に含む。また、姿勢算出用の力計測座標系CFaのZ軸とX軸とを含む平面は、3つの接触点を含む平面と直交する。
以降では、前述のように定められた姿勢算出用の力計測座標系CFaと、3つの接触点と、一方の物体21の凸部と他方の物体22の凹部との間に作用する力を姿勢算出用の力計測座標系CFa上の力に変換した力を用いて、姿勢算出用の力計測座標系CFaと、他方の物体22の凹部の穴の端部に対して設定する座標系との位置および姿勢の関係を算出する。
一方の物体21の凸部に対して設定した姿勢算出用の力計測座標系CFaに対する、他方の物体22の凹部の位置および姿勢を算出するために、図11(b)、図11(c)に図示するように、他方の物体22の凹部の穴に対して座標系CFbを設定する。座標系CFbは互いに直交するX軸、Y軸、Z軸によって構成される。座標系CFbの原点は、他方の物体22の凹部の穴の端部の中心点Obとする。点Obは、一方の物体21の凸部と他方の物体22の凹部とが接触する、接触点Pe1、接触点Pe2、接触点Psの3つの接触点の中心点となる。3つの接触点を含む平面と直交する軸を座標系CFbのZ軸とする。Z軸の+方向は、他方の物体22の凹部の穴から離れる方向の成分を含む方向とする。一方の物体21の凸部と他方の物体22の凹部が3つの接触点で接触するときの姿勢算出用の力計測座標系CFaのY軸と平行であり、かつ、+方向が同じ向きとなるように、座標系CFbのY軸を設定する。座標系CFbのX軸は、座標系CFbのZ軸と、座標系CFbのY軸とから一意に定まる。
図11(a)、図11(b)、図11(c)を参照しながら、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触する場合に、一方の物体21の凸部と他方の物体22の凹部との間に作用する力を、姿勢算出用の力計測座標系CFa上の力で表す。つまり、姿勢算出用の力計測座標系CFaの原点に対して或る力が作用するとして、或る力を姿勢算出用の力計測座標系CFa上で表す。
ここで、角度αは、前述のように、図11(b)に図示されるように、一方の物体21の凸部と他方の物体22の凹部が3つの接触点で接触するとき、3つの接触点を含む面の法線と一方の物体21の凸部に対して設定される凸部中心軸Laとの成す角度であって、他方の物体22の凹部の穴に対して設定した座標系CFbのZ軸の+方向と、姿勢算出用の力計測座標系CFaのZ軸の+方向との成す角度とする。μは、静止摩擦係数とする。また、図11(a)に示されるように、接触点Pe1には、姿勢算出用の力計測座標系上のZ軸の正方向に作用する力であるFeと、姿勢算出用の力計測座標系上のX軸の正方向に作用する力であるμ*Feが作用する。接触点Pe2には、姿勢算出用の力計測座標系上のZ軸の正方向に作用する力であるFeと、姿勢算出用の力計測座標系上のX軸の正方向に作用する力であるμ*Feが作用する。接触点Psには、姿勢算出用の力計測座標系上のX軸の正方向に作用する力であるFsと、姿勢算出用の力計測座標系上のZ軸の正方向に作用する力であるμ*Feが作用する。
また、hは、接触点Psから凸部中心軸Laに垂線を下ろしたときの交点を点Ptとするとき、中心点Oaから点Ptまでの距離であり、姿勢算出用の力計測座標系CFaのZ軸上の点Ptの位置とする。角度βは、姿勢算出用の力計測座標系CFaのX軸とY軸とで構成される平面上において、Y軸の−方向と、点Oaと接触点Pe1とを結ぶ線分との成す角度とする。角度β’は、図11(c)に図示されるように、他方の物体22の凹部に対して設定した座標系CFbのX軸とY軸とで構成される平面上において、Y軸の−方向と、点Obと他方の物体22の凹部における接触点Pe1とを結ぶ線分との成す角度とする。なお、図11(c)では、角度β’の説明のために、一方の物体21の凸部と他方の物体22の凹部とを仮想的に離して図示しており、一方の物体21の凸部と他方の物体22の凹部とは接触点Pe1、接触点Pe2、接触点Psにおいて接触している。
図11(a)に図示されるように、接触点Pe1と接触点Pe2は、一方の物体21の凸部および他方の物体22の凹部において、一方の物体21の凸部と他方の物体22の凹部とが接触している3つの接触点を含む面に直交する面であって、かつ、凸部中心軸Laを含む面に対して、面対称となる。つまり、姿勢算出用の力計測座標系CFa上での、接触点Pe1と接触点Pe2の位置については、X軸上、Z軸上の値は同じであり、Y軸上の値は大きさが同じであり正負が異なる値となる。また、他方の物体22の凹部に対して設定した座標系CFb上での、接触点Pe1と接触点Pe2の位置については、X軸上、Z軸上の値は同じでありY軸上の値は大きさが同じであり正負が異なる値となる。
一方の物体21の凸部と他方の物体22の凹部との間に作用する力を、姿勢算出用の力計測座標系CFa上の力、つまり、姿勢算出用の力計測座標系CFaの原点に作用する力に変換し、姿勢算出用の力計測座標系CFa上のX軸方向の力をFx、Y軸方向の力をFy、Z軸方向の力をFz、X軸の軸回りの力をMx、Y軸の軸回りの力をMy、Z軸の軸回りの力をMzとする。Fx、Fy、Fz、Mx、My、Mzは、それぞれ以下の式(16)−(21)によって表される。
Fx = Fs+2*μ*Fe (16)
Fz = μ*Fs+2*Fe (17)
My = Fs*h+μ*Fs*r+2*Fe*r*Sin(β)
(18)
Fy = 0 (19)
Mx = 0 (20)
Mz = 0 (21)
Fz = μ*Fs+2*Fe (17)
My = Fs*h+μ*Fs*r+2*Fe*r*Sin(β)
(18)
Fy = 0 (19)
Mx = 0 (20)
Mz = 0 (21)
ここで、μが既知である場合、以下の式(22)−(24)が成り立つ。
Fe = (Fz−μ*Fx)/(2*(1−μ*μ)) (22)
Fs = (Fx−μ*Fz)/(1−μ*μ) (23)
(r*Fz−My)*μ*μ+
{h*Fz+[Sin(β)−1]*r*Fx}*μ+
[My−h*Fx−Sin(α)*r*Fz] = 0
(24)
ここで、μ≒0(μが0と近似できるくらい小さい)である場合、Myは以下の式(25)で近似することができる。
My ≒ h*Fx+Sin(β)*r*Fz (25)
Fe = (Fz−μ*Fx)/(2*(1−μ*μ)) (22)
Fs = (Fx−μ*Fz)/(1−μ*μ) (23)
(r*Fz−My)*μ*μ+
{h*Fz+[Sin(β)−1]*r*Fx}*μ+
[My−h*Fx−Sin(α)*r*Fz] = 0
(24)
ここで、μ≒0(μが0と近似できるくらい小さい)である場合、Myは以下の式(25)で近似することができる。
My ≒ h*Fx+Sin(β)*r*Fz (25)
姿勢算出用の力計測座標系CFaと、一方の物体21の凸部と他方の物体22の凹部とが接触する接触点Pe1、接触点Pe2、接触点Psの3つの接触点と、他方の物体22の凹部に設定した座標系CFbの、位置および姿勢の関係について考える。
姿勢算出用の力計測座標系CFaに対する、接触点Pe1、接触点Pe2、接触点Psの位置は、それぞれ以下の式(26)、(28)、(30)のように表され、他方の物体22の凹部に対して設定した座標系CFbに対する、接触点Pe1、接触点Pe2、接触点Psの位置は、それぞれ以下の式(27)、(29)、(31)のように表される。
P(点Ps_座標系CFa) = [−r、0、h]^T (26)
P(点Ps_座標系CFb) = [−R、0、0]^T (27)
P(点Pe1_座標系CFa)
= [−r*Sin(β)、−r*Cos(β)、0]^T (28)
P(点Pe1_座標系CFb)
= [R*Sin(β’)、−R*Cos(β’)、0]^T (29)
P(点Pe2_座標系CFa)
= [−r*Sin(β)、r*Cos(β)、0]^T (30)
P(点Pe2_座標系CFb)
= [R*Sin(β’)、R*Cos(β’)、0]^T (31)
P(点Ps_座標系CFa) = [−r、0、h]^T (26)
P(点Ps_座標系CFb) = [−R、0、0]^T (27)
P(点Pe1_座標系CFa)
= [−r*Sin(β)、−r*Cos(β)、0]^T (28)
P(点Pe1_座標系CFb)
= [R*Sin(β’)、−R*Cos(β’)、0]^T (29)
P(点Pe2_座標系CFa)
= [−r*Sin(β)、r*Cos(β)、0]^T (30)
P(点Pe2_座標系CFb)
= [R*Sin(β’)、R*Cos(β’)、0]^T (31)
また、β、β’については、以下の式(32)で示される条件が成立する。
R*Cos(β’) = r*Cos(β) (32)
R*Cos(β’) = r*Cos(β) (32)
他方の物体22の凹部に対して設定した座標系CFbから、姿勢算出用の力計測座標系CFaへの同次変換行列Tbは、以下の式(33)のように表される。
ここで、ΔPxは、他方の物体22の凹部に対して設定した座標系CFbにおける、X軸方向の点Oaの位置とする。また、ΔPzは、他方の物体22の凹部に対して設定した座標系CFbにおける、Z軸方向の点Oaの位置とする。
ここで、ΔPxは、他方の物体22の凹部に対して設定した座標系CFbにおける、X軸方向の点Oaの位置とする。また、ΔPzは、他方の物体22の凹部に対して設定した座標系CFbにおける、Z軸方向の点Oaの位置とする。
座標系CFb上の接触点Ps、接触点Pe1、接触点Pe2の位置は、同時変換行列Tbと、座標系CFa上の接触点Ps、接触点Pe1、接触点Pe2の位置を用いて、それぞれ以下の式(34)−(36)で表される。
P(点Ps_座標系CFb) = Tb・P(点Ps_座標系CFa) (34)
P(点Pe1_座標系CFb) = Tb・P(点Pe1_座標系CFa) (35)
P(点Pe2_座標系CFb) = Tb・P(点Pe2_座標系CFa) (36)
P(点Ps_座標系CFb) = Tb・P(点Ps_座標系CFa) (34)
P(点Pe1_座標系CFb) = Tb・P(点Pe1_座標系CFa) (35)
P(点Pe2_座標系CFb) = Tb・P(点Pe2_座標系CFa) (36)
式(26)から式(36)によって、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触しているとき、他方の物体22の凹部に設定した座標系CFb上において、以下の式(37)から(41)に表される条件が導かれる。
R*Sin(β’)+r*Sin(β)*Cos(α) = ΔPx (37)
R*Cos(β’) = r*Cos(β) (38)
r*Sin(β)*Sin(α) = ΔPz (39)
r*Cos(α)+h*Sin(α) = R+ΔPx (40)
−r*Sin(α)+h*Cos(α) = −ΔPz (41)
R*Sin(β’)+r*Sin(β)*Cos(α) = ΔPx (37)
R*Cos(β’) = r*Cos(β) (38)
r*Sin(β)*Sin(α) = ΔPz (39)
r*Cos(α)+h*Sin(α) = R+ΔPx (40)
−r*Sin(α)+h*Cos(α) = −ΔPz (41)
ここで、Rの大きさは、rの大きさと同じ程度であり近似できるとする。このとき、βはβ’と近似することができる。
式(37)から(41)を用いて、h、β、ΔPx、ΔPzが、それぞれ以下の式(42)−(45)で算出される。
h = 2*r*(1−Cos(α))/Sin(α) (42)
β = Arcsin((1−Cos(α))/(1+Cos(α))) (43)
ΔPx = r*(1−Cos(α)) (44)
ΔPz =
r*Sin(α)*(1−Cos(α))/(1+Cos(α)) (45)
式(37)から(41)を用いて、h、β、ΔPx、ΔPzが、それぞれ以下の式(42)−(45)で算出される。
h = 2*r*(1−Cos(α))/Sin(α) (42)
β = Arcsin((1−Cos(α))/(1+Cos(α))) (43)
ΔPx = r*(1−Cos(α)) (44)
ΔPz =
r*Sin(α)*(1−Cos(α))/(1+Cos(α)) (45)
式(42)と式(43)を、式(25)に代入することによって、角度αは以下の式(46)で算出される。
また、他方の物体22の凹部に設定した座標系CFb上のX軸、Y軸、Z軸上の、姿勢算出用の力計測座標系CFaの原点Oaの位置は、以下の式(47)で算出される。
P(点Oa_座標系CFa) =
[r*(1−Cos(α))、0、
r*Sin(α)*(1−Cos(α))/(1+Cos(α))]^T
(47)
P(点Oa_座標系CFa) =
[r*(1−Cos(α))、0、
r*Sin(α)*(1−Cos(α))/(1+Cos(α))]^T
(47)
以上のように、他方の物体22の凹部に設定した座標系CFbに対する、姿勢算出用の力計測座標系CFaの、角度α、位置が算出される。また、他方の物体22の凹部に設定した座標系CFbに対する、3つの接触点の位置も算出される。これによって、他方の物体22の凹部に対する、一方の物体21の凸部の相対的な位置および姿勢が算出される。
ここで、式(42)によって、一方の物体21の凸部と他方の物体22の凹部が3つの接触点で接触するためには、一方の物体21の凸部の円柱形状は、凸部中心軸Laの方向に、2*r*(1−Cos(α))/Sin(α)以上の長さがあればよいことが分かる。また、式(42)、式(43)によって、h、βが求まるので、接触点Pe1、接触点Pe2、接触点Psの座標系CFaの位置を求めることができ、一方の物体21における接触点の位置が求められる。
また、前述の説明においては、一方の物体21の凸部の形状を円柱形状としたが、図5(g)、図5(h)に図示されるような、テーパー形状など、一方の物体21の凸部の先端からの凸部中心軸Laの軸方向の距離によって径が変化する形状のように、接触点から凸部中心軸に対して垂線を引いたときの凸部中心軸との交点と凸部中心軸上の所定の点(前記交点以外の点)との距離に基づいて接触点と凸部中心軸との各最短距離が定まる形状である場合について説明する。
図12(a)には、一方の物体21の凸部が円錐台形状で、他方の物体22が凹部が穴であり、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触している状態が図示されている。図12(b)には、一方の物体21の凸部が円錐台形状で、他方の物体22が凹部が穴であり、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触している状態を、接触している3つの接触点を含む面に直交し、かつ、一方の物体21の凸部中心軸を含む平面を断面として見た様子が図示されている。一方の物体21の凸部を、図11に示されるような前述の円柱形状から、図12に示すような、凸部の先端の円の形状は円柱形状の場合と同じで、凸部中心軸Laの軸方向に沿ってhの大きさによって径が変わる、円錐台などの形状にして、他方の物体22の凸部に対して3つの接触点で接触させる場合を考える。また、凸部中心軸Laの軸方向に沿って凸部の物体の径が変わること以外の状況や説明は、前述と同様とする。なお、前記凸部中心軸上の所定の点は、姿勢算出用の力計測座標系の原点とする。この場合、接触点Psの姿勢算出用の力計測座標系CFa上のX軸の−方向の位置だけが変わることになる。このような状況であって、一方の物体21の凸部の径r’がhから定まる場合には、式(37)から式(41)の内、式(40)、式(41)における、rを、hから算出される距離r’、例えば、定数A*h+定数B、定数A*h*hなど、に変更すれば、前記のような物体とした一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触しているときの、他方の物体22の凹部に設定した座標系CFb上における幾何的な条件が前述と同様に導かれる。前述の条件で、接触点Psと凸部中心軸との最短距離がr’と表される場合、式(40)、式(41)は、それぞれ以下の式(48)、(49)のようになる。
r’*Cos(α)+h*Sin(α) = R+ΔPx (48)
−r’*Sin(α)+h*Cos(α) = −ΔPz (49)
r’*Cos(α)+h*Sin(α) = R+ΔPx (48)
−r’*Sin(α)+h*Cos(α) = −ΔPz (49)
前述の説明において、具体的に、一方の物体21の凸部の円柱形状を円錐台の形状とした場合について考える。図12(a)、図12(b)に示されるように、一方の物体21の凸部の円錐台形状と、他方の物体22の凹部とは接触点Pe1、接触点Pe2、接触点Psの3つの接触点で接触する。図12(a)、図12(b)に示されるパラメータなどは、図11(a)、図11(b)や前述の説明と同様とする。図11の状況とは、凸部中心軸Laの軸方向に沿って凸部の物体の径が変わることが異なる。図12に示されるように、接触点Pe1、接触点Pe2から、凸部中心軸Laまでの各最短距離はrとする。また、接触点Psから凸部中心軸Laに対して垂線を引いたときの凸部中心軸との交点をPtとする。交点Ptと、凸部中心軸上の所定の点とする点Oaとの距離をhとする。接触点Psと凸部中心軸Laとの最短距離は、接触点Psと接触点Ptとの距離であり、一方の物体21の凸部の円錐台の形状によって、r+a*hと定まるとする。ここで、aは定数とする。このように一方の物体21の凸部が円錐台形状の場合、一方の物体21の凸部が円柱形状の場合の前述の式(40)、式(41)は、それぞれ以下の式(50)、(51)のようになる。
(r+a*h)*Cos(α)+h*Sin(α) = R+ΔPx (50)
−(r+a*h)*Sin(α)+h*Cos(α) = −ΔPz (51)
変更した式(50)、式(51)と、もとの式(37)、式(38)、式(39)、式(41)、式(25)を用いることによって、前述と同様に、角度αを算出することができる。
(r+a*h)*Cos(α)+h*Sin(α) = R+ΔPx (50)
−(r+a*h)*Sin(α)+h*Cos(α) = −ΔPz (51)
変更した式(50)、式(51)と、もとの式(37)、式(38)、式(39)、式(41)、式(25)を用いることによって、前述と同様に、角度αを算出することができる。
以上のように、接触点と凸部中心軸との各最短距離が一定値となる場合だけでなく、接触点から凸部中心軸に対して垂線を引いたときの凸部中心軸との交点と凸部中心軸上の所定の点(前記交点以外の点)との距離に基づいて接触点と凸部中心軸との各最短距離が定まる場合においても、他方の物体22の凹部に設定した座標系CFbに対する、姿勢算出用の力計測座標系CFaの、角度α、位置が算出され、他方の物体22の凹部に対する、一方の物体21の凸部の相対的な位置および姿勢が算出される。なお、一方の物体21の凸部の円錐台形状の側面を、他方の物体22の凸部の穴の端部を含む平面など、他方の物体22に存在する平面に対して、押しつけることを考える場合、テーパー部分の角度θcを求めて、角度αと角度θcを用いて、一方の物体21の凸部の円錐台を回転させて、押しつけるようにすればよい。
姿勢算出用の力計測座標系CFaから他方の物体22の凹部に設定した座標系CFbへの同次変換行列は、式(33)、式(44)、式(45)、式(46)によって表される同次変換行列Tbの逆行列から算出される。つまり、姿勢算出用の力計測座標系CFaと、他方の物体22の凹部の穴の端部に対して設定する座標系との位置および姿勢の関係が求まる。これによって、一方の物体21の凸部に対する、他方の物体22の凹部の穴の端部の中心位置および端部が存在する面の姿勢が算出され、一方の物体21の凸部に対する、他方の物体22の凸部の相対的な位置および姿勢を算出することができる。なお、他方の物体22の凹部の穴の端部の中心位置および端部が存在する面の姿勢が求まれば十分である場合、他方の物体22の凹部に設定した座標系CFbと他方の物体22の位置および姿勢を表す座標系との位置および姿勢の関係は不要となる。
また、空間に対して設定した座標系や、駆動部50に設定される座標系、ツール座標系などに対する、姿勢算出用の力計測座標系CFaへの同次変換行列や位置および姿勢は、前述のようにして設定または算出することができる。
また、空間に対して設定した座標系や、駆動部50に設定される座標系、ツール座標系などに対する、他方の物体22の凹部に設定した座標系CFbへの同次変換行列や位置および姿勢は、それらの座標系に対する姿勢算出用の力計測座標系CFaへの同次変換行列と、姿勢算出用の力計測座標系CFaから他方の物体22の凹部に設定した座標系CFbへの同次変換行列から算出することができる。
また、他方の物体22の凹部に設定した座標系CFbと、他方の物体22の位置および姿勢を表す座標系との関係が設定されたり、既知であったり、算出されたりしている場合、他方の物体22の凹部に設定した座標系CFbから他方の物体22の位置および姿勢を表す座標系への同次変換行列と、空間に対して設定した座標系や、駆動部50に設定される座標系、ツール座標系などから他方の物体22の凹部に設定した座標系CFbへの同次変換行列とに基づいて、空間に対して設定した座標系や、駆動部50に設定される座標系、ツール座標系などに対する、他方の物体22の位置および姿勢を算出することができる。
他方の物体22の凹部に設定した座標系CFbと、他方の物体22の位置および姿勢を表す座標系との関係が、他方の物体22の凹部に設定した座標系CFbのZ軸の軸回りの姿勢だけが異なる場合、他方の物体22に存在する別の凹部に存在する面の姿勢を算出することによって、または他方の物体22の別の部分に一方の物体21を接触させて検出するようにして座標系CFbのZ軸の軸回りの角度を算出することによって、前記座標系CFbのZ軸の軸回りの姿勢を算出するようにしてもよい。このように、他方の物体22の形状の特徴を利用して、算出できていない姿勢についても求めるようにしてもよい。
また、他方の物体22の位置および姿勢を表す座標系を、予め、一方の物体21の凸部と他方の物体22の凹部の接触点である点Pe1と点Pe2とを結ぶ直線と平行な軸がY軸となるように設定し、一方の物体21の凸部と他方の物体22の凹部とを接触させる状況を考慮してX軸の+方向を設定しておいてもよい。
また、他方の物体22に存在し、互いの位置および/または姿勢の関係が既知な複数の凹部を利用して、姿勢算出用の力計測座標系CFaと他方の物体22の位置および姿勢を表す座標系との位置および姿勢の関係を算出するようにしてもよい。まず、姿勢算出用の力計測座標系CFaと他方の物体22に存在する或る凹部に設定した座標系CFbとの位置および姿勢の関係を算出する。次に、他方の物体22に存在する別の凹部を利用して、姿勢算出用の力計測座標系CFaと他方の物体22の別の凹部に設定した座標系CFb’との位置および姿勢の関係を算出する。この結果と、他方に存在する或る凹部と他方の物体22に存在する別の凹部との既知の位置および/または姿勢の関係とに基づいて、姿勢算出用の力計測座標系CFaと他方の物体22の位置および姿勢を表す座標系との位置および姿勢の関係を算出するようにしてもよい。このとき、複数の凹部に対して、姿勢算出用の力計測座標系CFaとの位置および姿勢の関係を算出して、他方の物体22の位置および姿勢をより高い次元で算出できるようにしたり、一方の物体21と他方の物体22の相対的な位置および姿勢をより正確に算出したりしてもよい。このように、一方の物体21および他方の物体22の形状や、一方の物体21や他方の物体22に存在する複数の凸部や凹部を利用して、他方の物体22の位置および姿勢をより高い次元で算出できるようにしたり、一方の物体21と他方の物体22の相対的な位置および姿勢をより正確に算出したりしてもよい。
他方の物体22に存在する別の凹部を利用して、他方の物体22の位置および姿勢を表す座標系と、他方の物体22の凹部に設定した座標系CFbとが、座標系CFbのZ軸の軸回りの姿勢だけが異なる場合の、前記Z軸の軸回りの姿勢のずれを算出する方法について図17を参照しながら以下で述べる。
図17に図示されるように、他方の物体22は、凹部22aおよび凹部22bを含む。ここで、他方の物体22の凹部22aおよび他方の物体22の凹部22bは径が同じである円筒形状とする。また、一方の物体21の凸部も円筒形状であり、他方の物体22の凹部22aおよび他方の物体22の凹部22bと同じ径の円筒形状とする。また、他方の物体22の凹部22aの円筒形状の中心軸と、他方の物体22の凹部22bの円筒形状の中心軸とは、直交するように構成されているとする。他方の物体22の位置および姿勢は、座標系CF0によって表されるとする。座標系CF0は、図17にX0と示すX軸、Y0と示すY軸、Z1と示すZ軸の互いに直交する3軸によって構成され、座標系原点は点Obとする。一方の物体21の凸部と他方の物体22の凸部とを3つの接触点で接触させることによって、一方の物体21の凸部と他方の物体22の凹部22aとの相対的な位置および姿勢を算出するときに、他方の物体22に対して設定する座標系を座標系CFbとする。座標系CFbは互いに直交する、図17にX1と示すX軸、Y1と示すY軸、Z1と示すZ軸で構成され、座標系の原点は点Obとする。座標系CF0と座標系CFbとは、座標系の原点が一致し、またZ軸は一致している。座標系CF0と座標系CFbとは、座標系CFbのZ軸の軸回りにだけ姿勢が異なっているとする。
次に、一方の物体21の凸部と他方の物体22の凹部22bとを3つの接触点で接触させることによって、一方の物体21の凸部と他方の物体22の凹部22bとの相対的な位置および姿勢を算出するときに、他方の物体22に対して設定する座標系を座標系CFb2とする。座標系CFb2は互いに直交する、図17にX2と示すX軸、Y2と示すY軸、Z2と示すZ軸で構成され、座標系の原点は点Ob2とする。これによって、座標系CFaの位置および姿勢に基づいて、座標系CFbと座標系CFbの相対的な位置や相対的な姿勢を算出することができる。
座標系CFbを座標系CFbのZ軸の軸回りに回転させて、座標系CF0を算出することを考える。座標系CFbを座標系CFbのZ軸の軸回りに回転させるとき、座標系CFb2のX軸とY軸とを含む平面と座標系CFbのY軸が平行となるように、かつ、回転後のX軸の正方向に点Ob2が存在するように、またはX軸が座標系CFb2のZ軸と平行かつ同じ向きとなるようにする。これによって、座標系CFbを座標系CFbのZ軸の軸回りに回転させ、座標系CF0を算出することができる。このように、他方の物体22に存在する複数の凹部の相対的な位置および姿勢の関係を用いることによって、一方の物体21に対する他方の物体22の3次元の位置および姿勢を算出することができ、より多自由度の姿勢を算出することができるようになる。また、他方の物体22に存在する複数の凹部の位置および姿勢をもとに、他方の物体22の位置および姿勢をより正確に算出することができる。
以上のように、一方の物体21や他方の物体22に存在する複数の凸部や凹部や、その物体における複数の凸部や凹部の相対的な位置および姿勢の関係を用いることによって、多自由度の姿勢を算出することができたり、より正確な位置および姿勢を算出したりすることができるようになる。また、他方の物体22の凹部22aと他方の物体22の凹部22bとの相対的な位置や姿勢に基づいて、他方の物体22の良否判定、つまり、ワークやツールなどの物体の良否を判定してもよい。例えば、判定対象とするワークやツールが、意図した状態とは異なった状態の物になっていないか、欠陥のある不良品でないか、意図した種類とは異なった種類の物でないか、ワークに損傷が生じていないかなどを判定してもよい。
前記のように、一方の物体21や他方の物体22に、複数の凸部や凹部を存在させるようにした形状の別の一実施例について説明する。
図18は、一方の物体21、他方の物体22に、複数の凸部、凹部を包含させる場合の別の例を示す図である。図18(a)に示すように、一方の物体21、他方の物体22に包含される凸部または凹部として、3つの円筒形状の部分を設け、前記3つの円筒形状の中心軸が互いに直交するように構成してもよい。また、図18(b)に示すように、一方の物体21、他方の物体22に含まれる凸部または凹部として、2つの円筒形状の部分を設け、前記2つの円筒形状の中心軸が互いに直交するように構成してもよい。また、図18(c)に示すように、一方の物体21、他方の物体22に含まれる凹部として、3つの穴を設け、3つの穴の中心軸が互いに直交するように構成してもよい。また、図18(d)に示すように、一方の物体21、他方の物体22に含まれる凹部として、2つの穴を設け、2つの穴の中心軸が互いに直交するように構成してもよい。一方の物体21、他方の物体22には、図18(a)、図18(b)、図18(c)、図18(d)に図示するように、複数の凸部または凹部が包含されるようにしてもよい。また、図18(a)、図18(b)、図18(c)、図18(d)に図示するような、複数の凸部または凹部を包含する物体は、一方の物体21、他方の物体22に、着脱することができるようにした物体であってもよい。
以上のようにして、物体の姿勢算出部32は、駆動部50によって一方の物体21と他方の物体22とを相対的に移動させて一方の物体21と他方の物体22とを接触させ、一方の物体21と他方の物体22とが3つの接触点で接触するときに力計測部31によって計測される力、および接触点と凸部中心軸との最短距離に基づいて、一方の物体21と他方の物体22との相対的な位置および/または姿勢を算出することができる。また、算出した一方の物体21と他方の物体22との相対的な位置および/または姿勢を利用して、空間に対して設定した座標系や、駆動部50に設定される座標系、ツール座標系などの座標系に対する一方の物体21の位置および/または姿勢を算出してもよい。また、空間に対して設定した座標系や、駆動部50に設定される座標系、ツール座標系などの座標系に対する他方の物体22の位置および/または姿勢を算出してもよい。また、他方の物体22に対する、空間に対して設定した座標系や、駆動部50に設定される座標系、ツール座標系などの座標系の位置および/または姿勢を算出してもよい。
本発明の2番目の実施形態においては、本発明の1番目の実施形態の物体の姿勢算出システム11に対して、一方の物体21は前記凸部もしくは前記凹部を2つ以上包含する、または、他方の物体22は前記凸部もしくは前記凹部を2つ以上包含する、または、一方の物体21および他方の物体22は前記凸部もしくは前記凹部を2つ以上包含する、物体の姿勢算出システムとすることが好ましい。
本発明の1番目の実施形態で説明した凸部と凹部について、例えば、他方の物体22が前記凹部を2つ以上包含し、一方の物体21が前記各凹部に対して3つの接触点で接触可能な前記凸部を1つ以上包含するようにしてもよい。また、他方の物体22が前記凹部を2つ以上包含し、一方の物体21が前記各凹部に対して3つの接触点で接触可能な前記凸部を1つ以上包含するようにしてもよい。また、他方の物体22が前記凹部を1つ以上、前記凸部を1つ以上包含し、一方の物体21が、他方の物体22の前記凹部に対して3つの接触点で接触可能な前記凸部を1つ以上、他方の物体22の前記凸部に対して3つの接触点で接触可能な前記凹部を1つ以上包含するようにしてもよい。
複数の前記凸部または前記凹部を包含させておくことによって、駆動部50の機構部として移動可能な領域、物体との干渉によって移動可能な領域、物体における接触可能な部分などの、システムの状況に応じて、移動可能な場所にある前記凸部または前記凹部に対して、3つの接触点で接触させることが可能な物体を接触させることができる。また、1つの物体における複数の前記凸部または前記凹部の部分について相対的な位置および/または姿勢を算出することによって、また、1つの物体における複数の互いの位置および/または姿勢の関係が既知な前記凸部または前記凹部の部分について相対的な位置および/または姿勢を算出することによって、一方の物体21と他方の物体22との相対的な位置および/または姿勢について、より正確な相対的な位置および/または姿勢の算出、また3次元での任意の位置および姿勢を算出することができるようになる。
例えば、前述のように、他方の物体22に、互いの位置および姿勢が分かっている凹部を2つ取りつけ、2つの凹部の位置および姿勢の情報から、他方の物体22の位置および姿勢を算出してもよい。また、例えば、他方の物体22に複数の前記凸部または前記凹部をとりつけ、その前記凸部または前記凹部の位置のみを前記複数の部分について算出し、その複数の位置の情報から、他方の物体22の位置および姿勢を算出するようにしてもよい。
本発明の3番目の実施形態においては、本発明の1番目または2番目の実施形態の物体の姿勢算出システム11に対して、一方の物体21または他方の物体22に包含される、前記凸部および前記凹部の内、少なくとも1つの物体は前記包含される一方の物体21または他方の物体22に対して着脱可能な物体である、物体の姿勢算出システムとすることが好ましい。
本発明の1番目の実施形態で説明した凸部と凹部について、前記凸部および前記凹部の内、少なくともいずれか1つは、当該物体が取りつけられた一方の物体21または他方の物体22に対して着脱可能であることによって、つまり、前記凸部や前記凹部を着脱可能な物体とすることによって、一方の物体21と他方の物体22との相対的な位置および/または姿勢を算出したいときにだけ、前記凸部および前記凹部の内、少なくともいずれか1つを一方の物体21または他方の物体22に取りつければよい。
これによって、前記凸部または前記凹部のような部分がない物体でも、必要に応じて、前記凸部または前記凹部を包含させることができるようになる。なお、一方の物体21と他方の物体22との相対的な位置および/または姿勢を算出した後は取り外すようにすればよい。
また、一方の物体21または他方の物体22の或る部分に前記凸部または前記凹部を取りつけて、一方の物体21と他方の物体22との相対的な位置および/または姿勢を算出した後、取り付けた前記凸部または前記凹部を取り外す。次に、一方の物体21または他方の物体22の別の或る部分に前記凸部または前記凹部を取りつけて、一方の物体21と他方の物体22との相対的な位置および/または姿勢を算出した後、取り付けた前記凸部または前記凹部を取り外す。前記操作を複数回、実行することによって、一方の物体21と他方の物体22との相対的な位置および/または姿勢をより正確に算出したり、3次元の任意の位置および姿勢を算出することができるようにしたりしてもよい。
本発明の4番目の実施形態においては、本発明の1番目から3番目のいずれかの実施形態の物体の姿勢算出システム11に対して、一方の物体21と他方の物体22とを前記3つの接触点で接触させる前に、一方の物体21と他方の物体22との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め一方の物体21と他方の物体22とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムとすることが好ましい。
一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させれば、本発明の1番目の実施形態で説明したように、一方の物体21と他方の物体22との相対的な位置および/または姿勢を算出することができる。
このとき、力センサ、トルクセンサ、触覚センサ、接触センサなどの、力や接触の感覚、接触を検出する接触式のセンサを用いて、自動的に、一方の物体21または他方の物体22の大体の、位置および姿勢を検出させた後、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。
また、カメラ、レーザ、LED、超音波などを利用した非接触式のセンサを用いて、自動的に、一方の物体21または他方の物体22の大体の、位置および姿勢を検出させた後、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。この場合、物体の位置および/または姿勢をあまり高い精度で検出できない、または、物体の大体の位置および/または姿勢しか認識できないようなセンサを利用して、大体の範囲の位置および姿勢に移動させた後、一方の物体21と他方の物体22との相対的な位置および姿勢をより正確に算出することができる。
また、前記接触式のセンサや前記非接触式のセンサを利用して、作業者や教示者が、駆動部50を誘導するように移動させて、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。
また、加速度センサやジャイロセンサなどによって構成される教示操作装置を用いて、前記教示操作装置の3次元空間での傾き、加速度などを利用して、駆動部の並進移動や回転移動を操作し、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。
また、カメラ、レーザ、LED、超音波などを利用した、空間内の位置または位置および姿勢を指定する装置を利用して、空間内の指定した位置または位置および姿勢に、駆動部50が移動させる一方の物体21または他方の物体22を移動させて、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。
また、パーソナルコンピュータや、駆動部制御装置52と有線または無線で接続した教示装置などにおいてシステムのCADモデルなどを利用して、一方の物体21と他方の物体22との相対的な位置および姿勢を設定し、その設定に基づいて、駆動部50によって実際の一方の物体21と他方の物体22とを相対的に移動させたときに、一方の物体21と他方の物体22とが、所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢になる場合であってもよい。
以上のように、様々な方法によって、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に、大まかに移動させた場合であっても、本発明の1番目の実施形態で説明したように、一方の物体21と他方の物体22との相対的な位置および/または姿勢を算出させることができる。これによって、算出した相対的な位置および/または姿勢に基づいて、駆動部50が移動させる一方の物体21または他方の物体22を所望の位置および/または姿勢に、正確に移動させたり、所望の作業を適切に実行させたりすることができるようになる。
本発明の5番目の実施形態においては、本発明の1番目から3番目のいずれかの実施形態の物体の姿勢算出システム11に対して、前記力計測部は、さらに、前記駆動部または一方の物体21または他方の物体22に作用する力を計測し、一方の物体21と他方の物体22とを前記3つの接触点で接触させる前に、前記力計測部によって計測される力に基づいて、一方の物体21と他方の物体22との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め一方の物体21と他方の物体22とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムとすることが好ましい。
作業者は、駆動部50に対して、または、駆動部50が移動させる一方の物体21もしくは他方の物体22に対して、作用させる力を力計測部31が計測し、作用された力に基づいて、駆動部50を誘導するように移動させて、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。これによって、作業者は、駆動部50を直感的に、また直接的に、駆動部50が移動させる物体を移動させることができる。
また、作業者は、駆動部50が移動させない一方の物体21または他方の物体22に対して、作用させる力を力計測部31が計測し、作用された力に基づいて、駆動部50を誘導するように移動させて、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。なお、このとき、駆動部50が移動させない一方の物体21または他方の物体22に対して、作業者が作用させる力を検出できるように、別の力センサを設置したり、別の駆動部50’に駆動部50が移動させない一方の物体21または他方の物体22がある場合には駆動部50’に作用する力を計測して算出できるようにしたりしておく。これによって、作業者は、一方の物体21と他方の物体22とを相対的に移動させるときに、駆動部50が移動させない一方の物体21または他方の物体22から見た、相対的に移動させたい所望の方向に容易に移動させることができる。
また、駆動部50または駆動部50が移動させる物体に作用する力を、力計測部31が計測する場合、駆動部50が移動させる物体でない方の物体に対して、駆動部50を移動させて接触させることによって、駆動部50が移動させる物体でない方の物体の大体の位置および姿勢を算出する。この算出した位置および姿勢に基づいて、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。
このように、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に、大まかに移動させた場合であっても、本発明の1番目の実施形態で説明したように、一方の物体21と他方の物体22との相対的な位置および/または姿勢を算出させることができる。これによって、算出した相対的な位置および/または姿勢に基づいて、駆動部50が移動させる一方の物体21または他方の物体22を所望の位置および/または姿勢に、正確に移動させたり、所望の作業を適切に実行させたりすることができるようになる。
本発明の6番目の実施形態においては、本発明の1番目から3番目のいずれかの実施形態の物体の姿勢算出システム11に対して、前記物体の姿勢算出システムは、さらに、一方の物体21または他方の物体22の位置および/または姿勢を所定の範囲以内で検出することが可能な物体検出部を備え、一方の物体21と他方の物体22とを前記3つの接触点で接触させる前に、前記物体検出部の検出結果に基づいて、一方の物体21と他方の物体22との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め一方の物体21と他方の物体22とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムとすることが好ましい。
前記物体の姿勢算出システムは、さらに、カメラ、レーザ、LED、超音波などを利用した非接触式のセンサ、または、力センサ、トルクセンサ、触覚センサ、接触センサなどの力や接触の感覚、接触を検出する接触式のセンサを用いた、一方の物体21または他方の物体22や、その凸部または凹部の位置および/または姿勢を所定の範囲以内で検出することが可能な、または、駆動部50が移動させる物体ではない方の物体や、その凸部または凹部の位置および/または姿勢を所定の範囲以内で検出することが可能な物体検出部を備える。なお、例えば、駆動部50が移動させる物体を一方の物体21としたとき、一方の物体21および他方の物体22の物体形状が複雑であり、干渉などのため、一方の物体21を他方の物体22に対する相対的な位置および/または姿勢を所定の範囲以内に移動させる場合に、一方の物体21の物体の形状をある程度、認識する必要がある場合、駆動部50が移動させる物体を一方の物体21の位置および/または姿勢を所定の範囲内で検出するようにしてもよい。
物体検出部によって、一方の物体21または他方の物体22の位置および/または姿勢を検出した後、前記検出結果に基づいて、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させる。
このように、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に、大まかに移動させた場合であっても、本発明の1番目の実施形態で説明したように、一方の物体21と他方の物体22との相対的な位置および/または姿勢を算出させることができる。これによって、算出した相対的な位置および/または姿勢に基づいて、駆動部50が移動させる一方の物体21または他方の物体22を所望の位置および/または姿勢に、正確に移動させたり、所望の作業を適切に実行させたりすることができるようになる。
物体検出部は、物体の位置および/または姿勢を所定の範囲以内で検出することができればよいので、必要最低限の情報が取得できるようなセンサなどで構成されればよい。このため、物体の位置および/または姿勢を精度よく検出ができるようなセンサなどは必要なく、また、複雑なセンサを使用しないためにセンサの故障などを減らすことができる。このような、物体の位置および/または姿勢をあまり高い精度で検出できない、または、物体の大体の位置および/または姿勢しか認識できないようなセンサを利用して、大体の範囲の位置および姿勢に移動させた後、一方の物体21と他方の物体22との相対的な位置および姿勢をより正確に算出することができる。このため、安価で、システムの運用上の安定性の高いシステムとすることができる。
本発明の7番目の実施形態においては、本発明の1番目から3番目のいずれかの実施形態の物体の姿勢算出システム11に対して、前記物体の姿勢算出システムは、さらに、空間内の位置または位置および姿勢を指定する装置と前記指定した位置または位置および姿勢を認識する装置とで構成される空間位置指定部を備え、一方の物体21と他方の物体22とを前記3つの接触点で接触させる前に、前記空間位置指定部によって指定される位置または位置および姿勢に基づいて、一方の物体21と他方の物体22との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め一方の物体21と他方の物体22とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムとすることが好ましい。
物体の姿勢算出システムは、さらに、カメラ、レーザ、LED、超音波などを利用した、空間内の位置または位置および姿勢を指定する装置と前記指定した位置または位置および姿勢を認識する装置とで構成される空間位置指定部を備えるようにすることが好ましい。例えば、位置または位置および姿勢を指定できるように、マーカーを配置しそれをカメラで認識したり、位置または位置および姿勢を指定したい凸部または凹部に対して、レーザで点や十字を照射したり、LEDや超音波を照射したりして、その受光器や受信機を用いるようにして、空間位置指定部を構成させるようにしてもよい。
前記空間位置指定部によって、指定した位置または位置および姿勢を認識した、前記認識結果に基づいて、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。
このように、一方の物体21と他方の物体22とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に、大まかに移動させた場合であっても、本発明の1番目の実施形態で説明したように、一方の物体21と他方の物体22との相対的な位置および/または姿勢を算出させることができる。これによって、算出した相対的な位置および/または姿勢に基づいて、駆動部50が移動させる一方の物体21または他方の物体22を所望の位置および/または姿勢に、正確に移動させたり、所望の作業を適切に実行させたりすることができるようになる。
本発明の8番目の実施形態においては、本発明の1番目から7番目のいずれかの実施形態の物体の姿勢算出システム11に対して、前記物体の姿勢算出部が算出した一方の物体21と他方の物体22との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記駆動部に対する、一方の物体21または他方の物体22の位置および/または姿勢の算出を行う、物体の姿勢算出システムとすることが好ましい。
物体の姿勢算出システムは、前記物体の姿勢算出部32が算出した一方の物体21と他方の物体22との、相対的な位置および/または姿勢に基づいて、駆動部50に対する、一方の物体21または他方の物体22の位置および/または姿勢を算出するようにすることが好ましい。なお、駆動部50が凸部を包含する一方の物体21を移動させ、一方の物体21に対する姿勢算出用の力計測座標系CFaが算出され、ツール座標系と姿勢が異なる場合、ツール座標系を補正するようにしてもよい。また、駆動部50が保持して移動させる一方の物体21の凸部の位置は分かっているが、一方の物体21の姿勢が分からない場合、一方の物体21に対する姿勢算出用の力計測座標系CFaを算出することによって、一方の物体21の姿勢が求められる。また、駆動部50が保持して移動させる一方の物体21における、他方の物体22の凹部と接触させる凸部の位置は分かっているが、一方の物体21に存在する別の複数の凸部の正確な位置および/または姿勢が分からず、一方の物体21の位置および姿勢を定める座標系の位置および姿勢が分からない場合、つまり、他方の物体22の凹部と接触させる一方の物体21の凸部に設定した或る点について一方の物体21の姿勢が変わる場合、一方の物体21に存在する別の複数の凸部を、他方の物体22の凹部と接触させることによって求められる一方の物体21に存在する別の複数の凸部の位置および/または姿勢から、一方の物体21の位置および姿勢を定める座標系の位置および姿勢を求め、駆動部50に対する一方の物体21の方の物体21の位置および姿勢を求めるようにしてもよい。
駆動部50に対する、一方の物体21または他方の物体22の位置および/または姿勢を算出することによって、駆動部50に対する、一方の物体21または他方の物体22の位置および/または姿勢が分かるので、所望の作業をより適切に、正確に実行できるようになる。
本発明の9番目の実施形態においては、本発明の1番目から8番目のいずれかの実施形態の物体の姿勢算出システム11に対して、前記物体の姿勢算出部が算出した一方の物体21と他方の物体22との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記駆動部によって移動される一方の物体21および/または他方の物体22を移動させる位置および/または姿勢を補正する、物体の姿勢算出システムとすることが好ましい。
前記物体の姿勢算出部32が算出した一方の物体21と他方の物体22との、相対的な姿勢または相対的な位置および姿勢に基づいて、駆動部50によって移動される一方の物体21および/または他方の物体22を移動させる位置および/または姿勢を補正することによって、一方の物体21と他方の物体22とを用いた作業、例えば、組立て作業や加工作業などにおいて、より適切な、正確な作業を実行することができる。
また、物体の姿勢算出システムにおける駆動部が、図2に示されるように、一方の物体21を移動させる駆動部50と、他方の物体22を移動させる駆動部50’の2つが存在する場合であって、駆動部50と駆動部50’との位置および姿勢の関係は既知であり、駆動部50に対する一方の物体21の位置および姿勢は既知であり、駆動部50’に対する他方の物体22の位置および姿勢が分からない場合、本発明の1番目の実施形態で説明したように、一方の物体21と他方の物体22との相対的な位置および姿勢を算出させることができる。これによって、算出した相対的な位置および姿勢に基づいて、駆動部50’に対する他方の物体22の位置および姿勢が既知となるので、駆動部50’が他方の物体22を所望の位置および/または姿勢に、正確に移動させたり、所望の作業を適切に実行させたりすることができるようになる。
本発明の10番目の実施形態においては、本発明の1番目から9番目のいずれかの実施形態の物体の姿勢算出システム11に対して、前記物体の姿勢算出部が算出した一方の物体21と他方の物体22との、相対的な姿勢または相対的な位置および姿勢に基づいて、一方の物体21または他方の物体22の良否判定と、一方の物体21または他方の物体22の種類判定と、前記駆動部50によって保持される一方の物体21または他方の物体22の保持状態の判定との内、少なくとも1つの判定を実行する、物体の姿勢算出システムとすることが好ましい。
本発明の1番目の実施形態で説明したように、一方の物体21と他方の物体22とを3つの接触点で接触させることによって、一方の物体21と他方の物体22との相対的な位置および/または姿勢を算出することができる。また、一方の物体21および他方の物体22に対する前記3つの接触点の位置についても算出することができる。また、一方の物体21と他方の物体22との相対的な姿勢または相対的な位置および姿勢に基づいて、駆動部50に対する、駆動部50が移動させない物体の姿勢または位置および姿勢を算出することができる。これらの算出された結果に基づいて、一方の物体21または他方の物体22について、良否判定や種類判定などの各種の判定を実行したり、駆動部50が移動させる物体であって保持している一方の物体21および/または他方の物体22の保持状態の判定をしたりするようにしてもよい。
ここで、物体の良否判定とは、物体が意図したものであるか否かのワークやツールなどの物体の良否を判定する。例えば、判定対象とするワークやツールなどが意図した状態とは異なった状態の物になっていないか、欠陥のある不良品でないか、意図した種類とは異なった種類の物でないか、ワークに損傷が生じていないかなどを判定する。
また、物体の種類判定は、物体の種類についての判定とする。また、物体の保持状態の判定とは、駆動部50の先端部分と、保持されている物体とが所定の姿勢の関係になっているか否かなどの判定とする。
駆動部50が一方の物体21を移動させるとき、一方の物体21に対する他方の物体22の位置および/または姿勢が、所定の位置および/または姿勢の範囲内にある場合は、他方の物体22は意図した物体であり良と判定とし、所定の位置および/または姿勢の範囲内にない場合は、他方の物体22は意図した物体ではなく良ではないと良否判定をしてもよい。また、駆動部50が一方の物体21を移動させるとき、一方の物体21に対する他方の物体22の位置および/または姿勢が、所定の位置および/または姿勢の範囲内にある場合は、一方の物体21は意図した保持状態であると判定とし、所定の位置および/または姿勢の範囲内にない場合は、一方の物体21は意図した保持状態ではないと判定するように、保持状態の判定をしてもよい。
また、駆動部50が一方の物体21を移動させるとき、一方の物体21に対する他方の物体22の位置および/または姿勢が、所定の位置および/または姿勢の範囲内にある場合は、他方の物体22は或る物体であると判定し、所定の位置および/または姿勢の範囲内にない場合は、別の物体と種類判定する。このとき、所定の位置および/または姿勢の範囲によって、物体の種類を細かく判定するように種類判定してもよい。
また、一方の物体21に凸部が存在するとし、駆動部50が一方の物体21を移動させるとき、他方の物体22の凹部と接触させることによって、一方の物体21の凸部に対して姿勢算出用の力計測座標系が設定される。このとき設定が完了した姿勢算出用の力計測座標系の姿勢と、一方の物体21の凸部に対して設定したツール座標系の姿勢の違いによって、駆動部50に対して、一方の物体21が所望の姿勢になっているか、所望の姿勢に近い状態になっているか、意図しない姿勢になっているかなどの保持状態の判定をする。また、設定が完了した姿勢算出用の力計測座標系の姿勢と、一方の物体21の凸部に対して設定したツール座標系の姿勢との相対的な姿勢の大きさによって、望ましくない姿勢で取得され保持されているか、保持されている一方の物体21の種類を判定したり、保持されている物体が意図した物体が意図したツールやワークなどの物体になっているかを判定したりしてもよい。
また、一方の物体21または他方の物体22に対する前記3つの接触点の位置に基づいて、一方の物体21や他方の物体22が意図した物体になっているか否かの良否判定や、一方の物体21や他方の物体22の物体の種類を判定する種類判定や、保持されている物体が所望の姿勢や所望の姿勢に近い状態か、また、保持されている物体の種類や保持されている物体が意図した物体であるか否かなどの保持状態の判定などの、判定を実行してもよい。
なお、前述の、良否判定、種類判定、保持状態の判定の内、少なくとも1つの判定を実行するように、複数の判定を同時に実行するようにしてもよい。このとき、複数の判定を組み合わせることによって、各種判定の正確さを高めることができるようになる。
一方の物体21または他方の物体22に、複数の、凸部または凹部が存在するときは、算出された複数の凸部または凹部の相対的な位置および/または姿勢に基づいて、複数の、凸部または凹部を包含する前記物体の良否判定、種類判定を実行してもよい。また、既知の複数の凸部または凹部の位置および/または姿勢の関係と、算出された複数の凸部または凹部の相対的な位置および/または姿勢に基づいて、複数の、凸部または凹部を包含する前記物体の良否判定、種類判定を実行してもよい。
本発明の11番目の実施形態においては、本発明の1番目から10番目のいずれかの実施形態の物体の姿勢算出システム11に対して、一方の物体21および他方の物体22における、前記凸部と前記凹部との組み合わせにおいて、前記凸部と前記凹部とは嵌合させることが可能であり、前記駆動部は、一方の物体21と他方の物体22とを相対的に移動させる、かつ、一方の物体21および他方の物体22の内、少なくとも1つの物体を保持し移動させる、かつ、一方の物体21と他方の物体22とを嵌合させるように移動が可能であり、前記物体の姿勢算出システムは、さらに、前記力計測部によって計測される力に基づいて、前記駆動部によって一方の物体21と他方の物体22とを相対的に移動させて力制御を実行し、一方の物体21と他方の物体22とを嵌合させる嵌合実行部を備え、前記嵌合実行部は、前記物体の姿勢算出部によって算出される、一方の物体21と他方の物体22との、相対的な姿勢または相対的な位置および姿勢に基づいて、一方の物体21と他方の物体22とを嵌合させる、物体の姿勢算出システムとすることが好ましい。
一方の物体21の凸部と他方の物体22の凹部とが、または一方の物体21の凹部と他方の物体22の凸部とが、本発明の1番目の実施形態で説明したように、所定の条件を満たすと共に、本発明の1番目の実施形態で説明したような嵌合可能な物体であるとする。また、嵌合可能な前記凸部または前記凹部は包含される一方の物体21または他方の物体22に対して着脱可能であってもよい。また、本発明の1番目の実施形態のように、駆動部50は6つの軸によって構成される垂直多関節型ロボットのように、一方の物体21と他方の物体22とを嵌合させるように移動可能であるとする。また、前記物体の姿勢算出システム11は、さらに、力計測部31によって計測される力に基づいて、駆動部50によって一方の物体21と他方の物体22とを相対的に移動させて力制御を実行し、一方の物体21と他方の物体22とを嵌合させる嵌合実行部を備えることが好ましい。
嵌合実行部は、物体の姿勢算出部32によって算出される、一方の物体21と他方の物体22との、相対的な姿勢または相対的な位置および姿勢を利用することによって、相対的な姿勢が大きい場合でも、一方の物体21と他方の物体22とを、確実に、また、短い時間で嵌合させることができるようになる。一方の物体21と他方の物体22との相対的な姿勢が大きい場合、力制御によって、一方の物体21と他方の物体22とを嵌合させることは、困難であることが多い。本発明によって、予め相対的な姿勢が小さくなるように移動させた後に力制御によって嵌合させるようにしたり、算出された相対的な姿勢または相対的な位置および姿勢に基づいて力制御しながら嵌合させたりすることによって、一方の物体21と他方の物体22とを確実に嵌合できるようにすると共に、短い時間で嵌合させることが可能となる。
本発明の12番目の実施形態においては、本発明の11番目の実施形態の物体の姿勢算出システム11に対して、前記嵌合実行部は、前記物体の姿勢算出部が算出した一方の物体21と他方の物体22との、相対的な姿勢または相対的な位置および姿勢に基づいて、一方の物体21と他方の物体22との、相対的な姿勢または相対的な位置および姿勢を修正するように移動させた後、一方の物体21と他方の物体22とを嵌合させる、物体の姿勢算出システムとすることが好ましい。
一方の物体21と他方の物体22との相対的な姿勢が大きい場合、予め相対的な姿勢が小さくなるように移動させた後、嵌合作業を実行することによって、一方の物体21と他方の物体22とを確実に嵌合できるようにすると共に、短い時間で嵌合させることが可能となる。
本発明の13番目の実施形態においては、本発明の11番目の実施形態の物体の姿勢算出システム11に対して、前記嵌合実行部は、前記物体の姿勢算出部が算出した一方の物体21と他方の物体22との、相対的な姿勢または相対的な位置および姿勢に基づいて、一方の物体21と他方の物体22との相対的な姿勢が大きいときには、一方の物体21と他方の物体22との相対的な姿勢を小さくする移動速度が大きくなるように前記力計測部によって計測される力に基づいて力制御を行い、一方の物体21と他方の物体22とを嵌合させる、物体の姿勢算出システムとすることが好ましい。
一方の物体21と他方の物体22とを力制御によって嵌合するとき、一方の物体21と他方の物体22との相対的な姿勢が大きいときには、つまり、予め算出された相対的な姿勢を利用して、相対的な姿勢を小さくする方向に、相対的な姿勢の大きさに基づく所定の角度だけ一方の物体21と他方の物体22とを相対的に移動させるときには、回転移動速度が大きくなるように、力制御の力制御ゲインを小さくして力に応じた移動指令量を小さくて、その移動指令量に対して足しこむ回転速度指令を大きくするなどして速度を調整する。これによって、相対的な姿勢の大きさが小さくなると想定されるまでは大まかに素早く移動させることができる。
相対的な姿勢を小さくする方向に、相対的な姿勢の大きさに基づく所定の角度だけ、一方の物体21と他方の物体22とを相対的に移動させた後は、力制御の力制御ゲインを大きくして力に応じた移動指令量を大きくして、その移動指令量に足しこむ回転速度指令を小さくするなどして速度を調整する。これによって、相対的な姿勢の大きさが小さくなったと想定される状況では、力に応じて細かく移動させ精密な動きをさせることができる。
また、一方の物体21と他方の物体22との相対的な姿勢を変えていくとき、一方の物体21と他方の物体22との予め算出された相対的な位置に基づいて、図11に示される一方の物体21の凸部中心軸Laと、他方の物体22の凹部の穴の中心軸とが重なる状態に近づくまでは、つまり、凸部中心軸La上と点Obの最短距離が所定の値より大きいときは大まかに素早く移動させ、凸部中心軸La上と点Obの最短距離が所定の値以下となったときは力に応じて細かく移動させ精密な動きをさせる。このように、並進方向についても、他方の物体22に対して設定した座標系CFbのX軸、Y軸方向については、相対的な位置の大きさが小さくなると想定されるまでは大まかに素早く移動させ、相対的な位置の大きさが小さくなったと想定される状況では、力に応じて細かく移動させ精密な動きをさせるようにして、状況に応じて力制御の動作を変えることが好ましい。
前述のように状況に応じて力制御の動作を変えることによって、一方の物体21と他方の物体22とを確実に嵌合できるようにすると共に、短い時間で嵌合させることが可能となる。
以上、本発明の種々の実施形態および変形例を説明したが、他の実施形態および変形例によっても本発明の意図される作用効果を奏することができることは当業者に自明である。特に、本発明の範囲を逸脱することなく前述した実施形態および変形例の構成要素を削除ないし置換することが可能であるし、公知の手段をさらに付加することが可能である。また、本明細書において明示的または暗示的に開示される複数の実施形態の特徴を任意に組合せることによっても本発明を実施できることは当業者に自明である。
11、11a 物体の姿勢算出システム
21、21a 一方の物体
22 他方の物体
22a、22b 他方の物体の凹部
23 凸部
24 凹部
31 力計測部
32 物体の姿勢算出部
33 動作指令生成部
50、50’ 駆動部
51、51’ 力センサ
52 駆動部制御装置
d 一方の物体の凸部の円柱形状の直径
r 一方の物体の凸部の円柱形状の半径
Oa 一方の物体の凸部の円柱形状の先端の中心点
La 一方の物体の凸部の凸部中心軸
D 他方の物体の凹部の穴の直径
R 他方の物体の凹部の穴の半径
Ob 他方の物体の凹部の穴の端部の中心点
Ob2 他方の物体の凹部の穴の端部の中心点
La 一方の物体の凸部に対して設定される凸部中心軸
α 3つの接触点を含む面の法線と凸部中心軸Laとの成す角度
Pq 射影した部分の端点
Ps、Pe1、Pe2 接触点
Pt 接触点から凸部中心軸Laに対して垂線をひいたときの凸部中心軸との交点
CFa 姿勢算出用の力計測座標系
CFb 他方の物体の凹部に対して設定した座標系
CFb2 他方の物体の凹部に対して設定した座標系
CF0 他方の物体に対して設定した座標系
β 座標系CFaのY軸の−方向と、点Oaと接触点Pe1とを結んだ線分との成す角度
β’ 座標系CFbのY軸の−方向と、点Obと接触点Pe1とを結んだ線分との成す角度
h 点Ptと点Oaとの距離
Fs、Fe 力
θc テーパーの角度
Pd1、Pd2 押付方向
RY 回転方向
+Mv1、−Mv1、+Mv2、−Mv2、+Mv3、−Mv3、+Mv4、−Mv4 移動方向
21、21a 一方の物体
22 他方の物体
22a、22b 他方の物体の凹部
23 凸部
24 凹部
31 力計測部
32 物体の姿勢算出部
33 動作指令生成部
50、50’ 駆動部
51、51’ 力センサ
52 駆動部制御装置
d 一方の物体の凸部の円柱形状の直径
r 一方の物体の凸部の円柱形状の半径
Oa 一方の物体の凸部の円柱形状の先端の中心点
La 一方の物体の凸部の凸部中心軸
D 他方の物体の凹部の穴の直径
R 他方の物体の凹部の穴の半径
Ob 他方の物体の凹部の穴の端部の中心点
Ob2 他方の物体の凹部の穴の端部の中心点
La 一方の物体の凸部に対して設定される凸部中心軸
α 3つの接触点を含む面の法線と凸部中心軸Laとの成す角度
Pq 射影した部分の端点
Ps、Pe1、Pe2 接触点
Pt 接触点から凸部中心軸Laに対して垂線をひいたときの凸部中心軸との交点
CFa 姿勢算出用の力計測座標系
CFb 他方の物体の凹部に対して設定した座標系
CFb2 他方の物体の凹部に対して設定した座標系
CF0 他方の物体に対して設定した座標系
β 座標系CFaのY軸の−方向と、点Oaと接触点Pe1とを結んだ線分との成す角度
β’ 座標系CFbのY軸の−方向と、点Obと接触点Pe1とを結んだ線分との成す角度
h 点Ptと点Oaとの距離
Fs、Fe 力
θc テーパーの角度
Pd1、Pd2 押付方向
RY 回転方向
+Mv1、−Mv1、+Mv2、−Mv2、+Mv3、−Mv3、+Mv4、−Mv4 移動方向
動作指令生成部33は、駆動部50が滑らかに、かつ、所望の位置および/または姿勢に移動できるように、動作指令を生成し、駆動部50の各制御軸への動作指令を出力する。また、動作指令生成部33は、加加速度が急に大きくならないようにフィルタリング処理をしたり、また、駆動部50の動作の振動が少なくなるように振動の低減の処理をしたり、また、目的とする位置に正確に移動できるように駆動部50の撓みなどを考慮して指令の位置および/または姿勢を補正したりして動作指令を調整してもよい。図2に示されるように、駆動部制御装置52が、駆動部50、駆動部50’を移動させる場合、駆動部制御装置52は駆動部50’についても、駆動部50と同様に、駆動部50’が滑らかに、かつ、所望の位置および/または姿勢に移動できるように、動作指令を生成し、駆動部50’の各制御軸への動作指令を出力する。
ここで、μが既知である場合、以下の式(22)−(24)が成り立つ。
Fe = (Fz−μ*Fx)/(2*(1−μ*μ)) (22)
Fs = (Fx−μ*Fz)/(1−μ*μ) (23)
(r*Fz−My)*μ*μ+
{h*Fz+[Sin(β)−1]*r*Fx}*μ+
[My−h*Fx−Sin(β)*r*Fz] = 0
(24)
ここで、μ≒0(μが0と近似できるくらい小さい)である場合、Myは以下の式(25)で近似することができる。
My ≒ h*Fx+Sin(β)*r*Fz (25)
Fe = (Fz−μ*Fx)/(2*(1−μ*μ)) (22)
Fs = (Fx−μ*Fz)/(1−μ*μ) (23)
(r*Fz−My)*μ*μ+
{h*Fz+[Sin(β)−1]*r*Fx}*μ+
[My−h*Fx−Sin(β)*r*Fz] = 0
(24)
ここで、μ≒0(μが0と近似できるくらい小さい)である場合、Myは以下の式(25)で近似することができる。
My ≒ h*Fx+Sin(β)*r*Fz (25)
座標系CFb上の接触点Ps、接触点Pe1、接触点Pe2の位置は、同時変換行列Tbと、座標系CFa上の接触点Ps、接触点Pe1、接触点Pe2の位置を用いて、それぞれ以下の式(34)−(36)で表される。
[P(点Ps_座標系CFb)^T、1]^T = Tb・[P(点Ps_座標系CFa)^T、1]^T (34)
[P(点Pe1_座標系CFb)^T、1]^T = Tb・[P(点Pe1_座標系CFa)^T、1]^T (35)
[P(点Pe2_座標系CFb)^T、1]^T = Tb・[P(点Pe2_座標系CFa)^T、1]^T (36)
[P(点Ps_座標系CFb)^T、1]^T = Tb・[P(点Ps_座標系CFa)^T、1]^T (34)
[P(点Pe1_座標系CFb)^T、1]^T = Tb・[P(点Pe1_座標系CFa)^T、1]^T (35)
[P(点Pe2_座標系CFb)^T、1]^T = Tb・[P(点Pe2_座標系CFa)^T、1]^T (36)
式(42)と式(43)を、式(25)に代入することによって、角度αは以下の式(46)で算出される。
また、他方の物体22の凹部に設定した座標系CFb上のX軸、Y軸、Z軸上の、姿勢算出用の力計測座標系CFaの原点Oaの位置は、以下の式(47)で算出される。
P(点Oa_座標系CFb) =
[r*(1−Cos(α))、0、
r*Sin(α)*(1−Cos(α))/(1+Cos(α))]^T
(47)
P(点Oa_座標系CFb) =
[r*(1−Cos(α))、0、
r*Sin(α)*(1−Cos(α))/(1+Cos(α))]^T
(47)
一方の物体と他方の物体との間に作用する力を制御することが可能なシステムを用いて、一方の物体を他方の物体に対して嵌合させるときの、動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出する場合、嵌合作業に時間がかかりシステムのサイクルタイムが増大する。また、一方の物体および他方の物体は、嵌合させることが可能な物体でないといけない。また、嵌合作業のために一方の物体を移動させなければならない。また、嵌合作業時の一方の物体の移動に伴って、周辺の物体との干渉などの問題が発生することがある。また、一方の物体と他方の物体との嵌合が正常に完了しない限り、一方の物体と他方の物体との相対的な姿勢を算出することはできない。さらに、一方の物体と他方の物体との相対的な姿勢が大きい場合、嵌合させることは困難な場合が多い。
一方の物体と他方の物体との間に作用する力を制御することが可能なシステムを用いて、一方の物体の面を他方の物体の面に対して合わせるときの、動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出する場合、面を合わせる作業に時間がかかりシステムのサイクルタイムが増大する。また、一方の物体および他方の物体は、面を合わせることが可能な物体でないといけない。また、面合わせ作業のために一方の物体を移動させなければならない。また、面合わせ作業時の一方の物体の移動に伴って、周辺の物体との干渉などの問題が発生することがある。
図3には、一方の物体21および他方の物体22に包含される、凸部と凹部とを3つの接触点において接触させる態様が図示されている。図3(a)には、一方の物体21に包含される円筒形の外側部分を凸部として、他方の物体22に包含される円筒形の内側部分を凹部として、一方の物体21の凸部と他方の物体22の凹部とを3つの接触点において接触させる態様が示されている。図3(b)には、一方の物体21に包含される円筒形の内側部分を凹部として、他方の物体22に包含される円筒形の外側部分を凸部として、一方の物体21の凹部と他方の物体22の凸部とを3つの接触点において接触させる態様が示されている。このように、一方の物体21の凸部を他方の物体22の凹部に対して3つの接触点で接触させてもよいし、一方の物体21の凹部を他方の物体22の凸部に対して3つの接触点で接触させてもよい。また、同じ大きさ程度の円筒形である場合、互いに凸部にも凹部にもなりうる。このように、円筒形のように、適切な大きさの物体であって、物体の中に空間がある物体である場合、凸部にも、凹部にもなりうる。また、図3(a)に示されるように、一方の物体21の凸部は、他方の物体22の凹部に対して、接触させる円筒の径が大きくても、小さくてもよい。また、図3(b)に示されるように、一方の物体21に包含される凹部は、他方の物体22に包含される凸部に対して、接触させる円筒の径が大きくても、小さくてもよい。
図9には、前述のように説明した、一方の物体21および他方の物体22のいずれかに包含される凸部23と、前記物体とは異なる一方の物体21および他方の物体22のいずれかに包含される凹部24の、組み合わせの一例が例示されている。図9(a)には、凸部23が円柱形状であり、凹部24が円筒形状である場合が示されている。図9(b)には、凸部23、凹部24が共に円筒形状である場合が示されている。図9(c)には、凸部23が円柱形状であり、凹部24が、半球のような凸形状の物体に凹部の穴が存在する形状である場合が示されている。図9(d)には、凸部23が、円柱形状の先端に、半球のような凸形状の物体が存在する形状であり、凹部24が円筒形状である場合が示されている。
なお、一方の物体21の凸部の円柱形状の半径r、直径d、他方の物体22の凹部の穴の半径R、直径Dは、既知とするが、一方の物体21および他方の物体22の内、いずれかの物体について、事前に算出するようにしてもよい。例えば、一方の物体21を壁、台、別のワークなどの別の物体に対して傾け、Z軸を一方の物体21の凸部中心軸Laの軸に設定したツール座標系で、Z軸方向であって別の物体に近づく方向に押しつけたときのZ軸方向の力と、それ以外の軸の軸回りの力を基に算出してもよい。または、壁、台、別のワークなどの、別の物体に接触させたときの位置を基に、一方の物体21の径を算出してもよい。例えば、まず、固定させた別の物体に対して、一方の物体21の凸部の円形状の端部を接触させる。次に、一方の物体21の凸部を、凸部中心軸Laの軸回りに所定の角度だけ回転させて、固定させた別の物体に対して、一方の物体21の凸部の円形状の端部を接触させる。このとき、一方の物体21の凸部を、凸部中心軸Laの軸回りに180度回転させて接触させる、または、適当な角度で3回以上、接触動作と位置検出を行って、一方の物体21の凸部における半径rや直径dを求める処理などを実行する。他方の物体22の凹部の穴の半径R、直径Dについては、一方の物体21の凸部における半径rや直径dとほぼ同じとして求める。また、他方の物体22の凹部の穴の半径R、直径Dは、駆動部50に取り付けたピンにTCP(Tool Center Point)、制御点、ツール座標系を設定して、前記ピンで他方の物体22の凹部の穴の径を予め算出しておくようにしてもよい。
ここでは、このように一方の物体21の凸部を移動させて、一方の物体21の凸部と他方の物体22の凹部とを、図13(a)、図13(b)で図示されるように、点Pe1と点Pe2の2点で接触させる場合について説明するが、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触する場合も同様である。図13(b)では、図13(a)に図示されるように、一方の物体21の凸部と他方の物体22の凹部とが2点で接触する状況を、他方の物体22の凹部の穴の端部に直交し、凸部中心軸Laを含む断面から見た様子を図示している。また、その断面に接触点Pe1と作用する力Feを射影している。図13(b)に図示されるように、一方の物体21の凸部と他方の物体22の凹部が2点で接触している状況であり、他方の物体22の凹部に対して一方の物体21の凸部を姿勢算出用の力計測座標系CFaのZ軸の−方向である押付方向Pd1に所定の目標力で押しつけると共に、Z軸と直交かつ互いに直交する2方向について目標力を0とするような受動的な力制御を実行して、一方の物体21の凸部を移動させることによって、一方の物体21の凸部と他方の物体22の凹部とを2つの接触点で接触させる。なお、この移動によって、3つの接触点で接触することもがあるが、説明については同様である。
図12(a)には、一方の物体21の凸部が円錐台形状で、他方の物体22が凹部が穴であり、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触している状態が図示されている。図12(b)には、一方の物体21の凸部が円錐台形状で、他方の物体22が凹部が穴であり、一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触している状態を、接触している3つの接触点を含む面に直交し、かつ、一方の物体21の凸部中心軸を含む平面を断面として見た様子が図示されている。一方の物体21の凸部を、図11に示されるような前述の円柱形状から、図12に示すような、凸部の先端の円の形状は円柱形状の場合と同じで、凸部中心軸Laの軸方向に沿ってhの大きさによって径が変わる、円錐台などの形状にして、他方の物体22の凹部に対して3つの接触点で接触させる場合を考える。また、凸部中心軸Laの軸方向に沿って凸部の物体の径が変わること以外の状況や説明は、前述と同様とする。なお、前記凸部中心軸上の所定の点は、姿勢算出用の力計測座標系の原点とする。この場合、接触点Psの姿勢算出用の力計測座標系CFa上のX軸の−方向の位置だけが変わることになる。このような状況であって、一方の物体21の凸部の径r’がhから定まる場合には、式(37)から式(41)の内、式(40)、式(41)における、rを、hから算出される距離r’、例えば、定数A*h+定数B、定数A*h*hなど、に変更すれば、前記のような物体とした一方の物体21の凸部と他方の物体22の凹部とが3つの接触点で接触しているときの、他方の物体22の凹部に設定した座標系CFb上における幾何的な条件が前述と同様に導かれる。前述の条件で、接触点Psと凸部中心軸との最短距離がr’と表される場合、式(40)、式(41)は、それぞれ以下の式(48)、(49)のようになる。
r’*Cos(α)+h*Sin(α) = R+ΔPx (48)
−r’*Sin(α)+h*Cos(α) = −ΔPz (49)
r’*Cos(α)+h*Sin(α) = R+ΔPx (48)
−r’*Sin(α)+h*Cos(α) = −ΔPz (49)
前述の説明において、具体的に、一方の物体21の凸部の円柱形状を円錐台の形状とした場合について考える。図12(a)、図12(b)に示されるように、一方の物体21の凸部の円錐台形状と、他方の物体22の凹部とは接触点Pe1、接触点Pe2、接触点Psの3つの接触点で接触する。図12(a)、図12(b)に示されるパラメータなどは、図11(a)、図11(b)や前述の説明と同様とする。図11の状況とは、凸部中心軸Laの軸方向に沿って凸部の物体の径が変わることが異なる。図12に示されるように、接触点Pe1、接触点Pe2から、凸部中心軸Laまでの各最短距離はrとする。また、接触点Psから凸部中心軸Laに対して垂線を引いたときの凸部中心軸との交点をPtとする。交点Ptと、凸部中心軸上の所定の点とする点Oaとの距離をhとする。接触点Psと凸部中心軸Laとの最短距離は、接触点Psと交点Ptとの距離であり、一方の物体21の凸部の円錐台の形状によって、r+a*hと定まるとする。ここで、aは定数とする。このように一方の物体21の凸部が円錐台形状の場合、一方の物体21の凸部が円柱形状の場合の前述の式(40)、式(41)は、それぞれ以下の式(50)、(51)のようになる。
(r+a*h)*Cos(α)+h*Sin(α) = R+ΔPx (50)
−(r+a*h)*Sin(α)+h*Cos(α) = −ΔPz (51)
変更した式(50)、式(51)と、もとの式(37)、式(38)、式(39)、式(41)、式(25)を用いることによって、前述と同様に、角度αを算出することができる。
(r+a*h)*Cos(α)+h*Sin(α) = R+ΔPx (50)
−(r+a*h)*Sin(α)+h*Cos(α) = −ΔPz (51)
変更した式(50)、式(51)と、もとの式(37)、式(38)、式(39)、式(41)、式(25)を用いることによって、前述と同様に、角度αを算出することができる。
以上のように、接触点と凸部中心軸との各最短距離が一定値となる場合だけでなく、接触点から凸部中心軸に対して垂線を引いたときの凸部中心軸との交点と凸部中心軸上の所定の点(前記交点以外の点)との距離に基づいて接触点と凸部中心軸との各最短距離が定まる場合においても、他方の物体22の凹部に設定した座標系CFbに対する、姿勢算出用の力計測座標系CFaの、角度α、位置が算出され、他方の物体22の凹部に対する、一方の物体21の凸部の相対的な位置および姿勢が算出される。なお、一方の物体21の凸部の円錐台形状の側面を、他方の物体22の凹部の穴の端部を含む平面など、他方の物体22に存在する平面に対して、押しつけることを考える場合、テーパー部分の角度θcを求めて、角度αと角度θcを用いて、一方の物体21の凸部の円錐台を回転させて、押しつけるようにすればよい。
姿勢算出用の力計測座標系CFaから他方の物体22の凹部に設定した座標系CFbへの同次変換行列は、式(33)、式(44)、式(45)、式(46)によって表される同次変換行列Tbの逆行列から算出される。つまり、姿勢算出用の力計測座標系CFaと、他方の物体22の凹部の穴の端部に対して設定する座標系との位置および姿勢の関係が求まる。これによって、一方の物体21の凸部に対する、他方の物体22の凹部の穴の端部の中心位置および端部が存在する面の姿勢が算出され、一方の物体21の凸部に対する、他方の物体22の凹部の相対的な位置および姿勢を算出することができる。なお、他方の物体22の凹部の穴の端部の中心位置および端部が存在する面の姿勢が求まれば十分である場合、他方の物体22の凹部に設定した座標系CFbと他方の物体22の位置および姿勢を表す座標系との位置および姿勢の関係は不要となる。
図17に図示されるように、他方の物体22は、凹部22aおよび凹部22bを含む。ここで、他方の物体22の凹部22aおよび他方の物体22の凹部22bは径が同じである円筒形状とする。また、一方の物体21の凸部も円筒形状であり、他方の物体22の凹部22aおよび他方の物体22の凹部22bと同じ径の円筒形状とする。また、他方の物体22の凹部22aの円筒形状の中心軸と、他方の物体22の凹部22bの円筒形状の中心軸とは、直交するように構成されているとする。他方の物体22の位置および姿勢は、座標系CF0によって表されるとする。座標系CF0は、図17にX0と示すX軸、Y0と示すY軸、Z1と示すZ軸の互いに直交する3軸によって構成され、座標系原点は点Obとする。一方の物体21の凸部と他方の物体22の凹部とを3つの接触点で接触させることによって、一方の物体21の凸部と他方の物体22の凹部22aとの相対的な位置および姿勢を算出するときに、他方の物体22に対して設定する座標系を座標系CFbとする。座標系CFbは互いに直交する、図17にX1と示すX軸、Y1と示すY軸、Z1と示すZ軸で構成され、座標系の原点は点Obとする。座標系CF0と座標系CFbとは、座標系の原点が一致し、またZ軸は一致している。座標系CF0と座標系CFbとは、座標系CFbのZ軸の軸回りにだけ姿勢が異なっているとする。
次に、一方の物体21の凸部と他方の物体22の凹部22bとを3つの接触点で接触させることによって、一方の物体21の凸部と他方の物体22の凹部22bとの相対的な位置および姿勢を算出するときに、他方の物体22に対して設定する座標系を座標系CFb2とする。座標系CFb2は互いに直交する、図17にX2と示すX軸、Y2と示すY軸、Z2と示すZ軸で構成され、座標系の原点は点Ob2とする。これによって、座標系CFaの位置および姿勢に基づいて、座標系CFbと座標系CFb2の相対的な位置や相対的な姿勢を算出することができる。
本発明の1番目の実施形態で説明した凸部と凹部について、例えば、他方の物体22が前記凹部を2つ以上包含し、一方の物体21が前記各凹部に対して3つの接触点で接触可能な前記凸部を1つ以上包含するようにしてもよい。また、他方の物体22が前記凹部を1つ以上、前記凸部を1つ以上包含し、一方の物体21が、他方の物体22の前記凹部に対して3つの接触点で接触可能な前記凸部を1つ以上、他方の物体22の前記凸部に対して3つの接触点で接触可能な前記凹部を1つ以上包含するようにしてもよい。
また、一方の物体21に凸部が存在するとし、駆動部50が一方の物体21を移動させるとき、他方の物体22の凹部と接触させることによって、一方の物体21の凸部に対して姿勢算出用の力計測座標系が設定される。このとき設定が完了した姿勢算出用の力計測座標系の姿勢と、一方の物体21の凸部に対して設定したツール座標系の姿勢の違いによって、駆動部50に対して、一方の物体21が所望の姿勢になっているか、所望の姿勢に近い状態になっているか、意図しない姿勢になっているかなどの保持状態の判定をする。また、設定が完了した姿勢算出用の力計測座標系の姿勢と、一方の物体21の凸部に対して設定したツール座標系の姿勢との相対的な姿勢の大きさによって、望ましくない姿勢で取得され保持されているか、保持されている一方の物体21の種類を判定したり、保持されている物体が意図したツールやワークなどの物体になっているかを判定したりしてもよい。
Claims (13)
- 一方の物体と他方の物体との相対的な姿勢を算出する、または前記一方の物体と前記他方の物体との相対的な位置および姿勢を算出する物体の姿勢算出システムにおいて、
凸部を包含する物体と凹部を包含する物体の組み合わせである、かつ、前記凸部と前記凹部とを3つの接触点において接触させることが可能な物体である、かつ、前記凹部における前記3つの接触点は前記凹部の穴の端部に位置する、かつ、前記凸部に対する軸であって前記3つの接触点を含む平面と直交しない軸を凸部中心軸とすると前記接触点と前記凸部中心軸との各最短距離が一定値であるか、または前記接触点から前記凸部中心軸に対して垂線を引いたときの前記凸部中心軸との交点と前記凸部中心軸上の前記交点以外の点との距離に基づいて前記接触点と前記凸部中心軸との各最短距離が定まる、
前記一方の物体および前記他方の物体と、
前記一方の物体と前記他方の物体とを相対的に移動させる、かつ、
前記一方の物体および前記他方の物体の内、少なくとも1つの物体を保持し移動させる1つ以上の駆動部と、
前記一方の物体と前記他方の物体との間に作用する力を計測する力計測部と、
前記駆動部によって前記一方の物体と前記他方の物体とを相対的に移動させて前記一方の物体と前記他方の物体とを接触させ、前記一方の物体と前記他方の物体とが前記3つの接触点で接触するときに前記力計測部によって計測される力および前記最短距離に基づいて、前記一方の物体と前記他方の物体との相対的な姿勢を算出する、または前記一方の物体と前記他方の物体との相対的な位置および姿勢を算出する物体の姿勢算出部と、
を備える、
ことを特徴とする物体の姿勢算出システム。 - 前記一方の物体は前記凸部もしくは前記凹部を2つ以上包含する、または、
前記他方の物体は前記凸部もしくは前記凹部を2つ以上包含する、または、
前記一方の物体および前記他方の物体は前記凸部もしくは前記凹部を2つ以上包含する、
請求項1に記載の物体の姿勢算出システム。 - 前記一方の物体または前記他方の物体に包含される、前記凸部および前記凹部の内、少なくとも1つの物体は前記包含される前記一方の物体または前記他方の物体に対して着脱可能な物体である、
請求項1または請求項2に記載の物体の姿勢算出システム。 - 前記一方の物体と前記他方の物体とを前記3つの接触点で接触させる前に、
前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、
請求項1から請求項3のいずれか一項に記載の物体の姿勢算出システム。 - 前記力計測部は、さらに、前記駆動部または前記一方の物体または前記他方の物体に作用する力を計測し、
前記一方の物体と前記他方の物体とを前記3つの接触点で接触させる前に、
前記力計測部によって計測される力に基づいて、
前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、
請求項1から請求項3のいずれか一項に記載の物体の姿勢算出システム。 - 前記物体の姿勢算出システムは、さらに、
前記一方の物体または前記他方の物体の位置および姿勢の少なくとも一方を所定の範囲以内で検出することが可能な物体検出部を備え、
前記一方の物体と前記他方の物体とを前記3つの接触点で接触させる前に、
前記物体検出部の検出結果に基づいて、
前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、
請求項1から請求項3のいずれか一項に記載の物体の姿勢算出システム。 - 前記物体の姿勢算出システムは、さらに、
空間内の位置または位置および姿勢を指定する装置と前記指定した位置または位置および姿勢を認識する装置とで構成される空間位置指定部を備え、
前記一方の物体と前記他方の物体とを前記3つの接触点で接触させる前に、
前記空間位置指定部によって指定される位置または位置および姿勢に基づいて、
前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、
請求項1から請求項3のいずれか一項に記載の物体の姿勢算出システム。 - 前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、
前記駆動部に対する、前記一方の物体または前記他方の物体の位置および姿勢の少なくとも一方の算出を行う、
請求項1から請求項7のいずれか一項に記載の物体の姿勢算出システム。 - 前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、
前記駆動部によって移動される前記一方の物体または前記他方の物体または前記一方の物体および前記他方の物体、を移動させる位置または姿勢または位置および姿勢を補正する、
請求項1から請求項8のいずれか一項に記載の物体の姿勢算出システム。 - 前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、
前記一方の物体または前記他方の物体の良否判定、または前記一方の物体もしくは前記他方の物体の種類判定、または前記駆動部によって保持される前記一方の物体もしくは前記他方の物体の保持状態の判定の内、少なくとも1つの判定を実行する、
請求項1から請求項9のいずれか一項に記載の物体の姿勢算出システム。 - 前記一方の物体および前記他方の物体における、前記凸部と前記凹部との組み合わせにおいて、前記凸部と前記凹部とは嵌合させることが可能であり、
前記駆動部は、前記一方の物体と前記他方の物体とを相対的に移動させる、かつ、
前記一方の物体および前記他方の物体の内、少なくとも1つの物体を保持し移動させる、かつ、前記一方の物体と前記他方の物体とを嵌合させるように移動が可能であり、
前記物体の姿勢算出システムは、さらに、
前記力計測部によって計測される力に基づいて、前記駆動部によって前記一方の物体と前記他方の物体とを相対的に移動させて力制御を実行し、前記一方の物体と前記他方の物体とを嵌合させる嵌合実行部を備え、
前記嵌合実行部は、前記物体の姿勢算出部によって算出される、前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記一方の物体と前記他方の物体とを嵌合させる、
請求項1から請求項10のいずれか一項に記載の物体の姿勢算出システム。 - 前記嵌合実行部は、
前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、
前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢を修正するように移動させた後、
前記一方の物体と前記他方の物体とを嵌合させる、
請求項11に記載の物体の姿勢算出システム。 - 前記嵌合実行部は、
前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、
前記一方の物体と前記他方の物体との相対的な姿勢が大きいときには、前記一方の物体と前記他方の物体との相対的な姿勢を小さくする移動速度が大きくなるように前記力計測部によって計測される力に基づいて力制御を行い、
前記一方の物体と前記他方の物体とを嵌合させる、
請求項11に記載の物体の姿勢算出システム。
Priority Applications (4)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2015177471A JP6208724B2 (ja) | 2015-09-09 | 2015-09-09 | 物体の姿勢算出システム |
DE102016116404.6A DE102016116404B4 (de) | 2015-09-09 | 2016-09-02 | Objektstellungsberechnungssystem |
US15/259,118 US9903698B2 (en) | 2015-09-09 | 2016-09-08 | Object posture calculation system |
CN201610814842.XA CN106514712B (zh) | 2015-09-09 | 2016-09-09 | 物体的姿势计算*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2015177471A JP6208724B2 (ja) | 2015-09-09 | 2015-09-09 | 物体の姿勢算出システム |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2017052049A true JP2017052049A (ja) | 2017-03-16 |
JP6208724B2 JP6208724B2 (ja) | 2017-10-04 |
Family
ID=58055037
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2015177471A Active JP6208724B2 (ja) | 2015-09-09 | 2015-09-09 | 物体の姿勢算出システム |
Country Status (4)
Country | Link |
---|---|
US (1) | US9903698B2 (ja) |
JP (1) | JP6208724B2 (ja) |
CN (1) | CN106514712B (ja) |
DE (1) | DE102016116404B4 (ja) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2019022916A (ja) * | 2017-07-24 | 2019-02-14 | セイコーエプソン株式会社 | ロボット制御装置、ロボット制御方法、ロボットシステムおよびシミュレーション装置 |
JP2019084650A (ja) * | 2017-11-09 | 2019-06-06 | キヤノン株式会社 | ロボット装置及び組立品の製造方法 |
JP2020196076A (ja) * | 2019-05-31 | 2020-12-10 | 三菱電機株式会社 | ロボットコントローラ及び組付方法 |
Families Citing this family (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6925794B2 (ja) * | 2016-09-02 | 2021-08-25 | 株式会社安川電機 | コントローラ、作業制御装置、多軸動作制御装置、及び駆動制御装置 |
CN109382826B (zh) * | 2017-08-10 | 2023-05-16 | 精工爱普生株式会社 | 控制装置、机器人及机器人*** |
US11034024B2 (en) * | 2019-02-15 | 2021-06-15 | GM Global Technology Operations LLC | Fixtureless component assembly |
US11364623B2 (en) * | 2019-02-15 | 2022-06-21 | GM Global Technology Operations LLC | Component assembly system |
US10899012B2 (en) * | 2019-02-15 | 2021-01-26 | GM Global Technology Operations LLC | Coordinated robot to robot component assembly |
JP7351667B2 (ja) * | 2019-08-01 | 2023-09-27 | ファナック株式会社 | 複数点嵌合を行うロボット制御システム、制御方法、コンピュータのプログラム、および制御装置 |
CN112797931B (zh) * | 2020-12-28 | 2022-09-27 | 韩莉莉 | 工业机器人位姿准确度和位姿重复性检测装置及检测方法 |
CN116216305B (zh) * | 2022-12-29 | 2023-11-07 | 长园视觉科技(珠海)有限公司 | 上下料控制方法、控制器、上下料设备及存储介质 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH04348885A (ja) * | 1991-05-24 | 1992-12-03 | Nippon Telegr & Teleph Corp <Ntt> | ロボットを用いた部品組立作業における嵌合位置検出方法 |
JPH06143171A (ja) * | 1992-10-28 | 1994-05-24 | Toshiba Corp | ロボット |
JP2015085499A (ja) * | 2013-11-01 | 2015-05-07 | セイコーエプソン株式会社 | ロボット、ロボットシステム、制御装置及び制御方法 |
Family Cites Families (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS603010A (ja) | 1983-06-20 | 1985-01-09 | Hitachi Ltd | ロボットの制御方法及びそのロボット |
JPS60131239A (ja) | 1983-12-20 | 1985-07-12 | 豊田合成株式会社 | 樹脂成形品 |
JPH0991026A (ja) | 1995-09-27 | 1997-04-04 | Fanuc Ltd | 力制御ロボットの姿勢誤差修正方法 |
DE19854011A1 (de) | 1998-11-12 | 2000-05-25 | Knoll Alois | Einrichtung und Verfahren zum Vermessen von Mechanismen und ihrer Stellung |
JP3443030B2 (ja) * | 1999-03-31 | 2003-09-02 | オークマ株式会社 | 測定装置 |
JP2008296310A (ja) | 2007-05-30 | 2008-12-11 | Fanuc Ltd | 加工ロボットの制御装置 |
JP5071362B2 (ja) * | 2008-12-09 | 2012-11-14 | 株式会社安川電機 | 組み立て作業ロボットの制御方法および組み立て作業ロボット |
JP4768086B2 (ja) | 2009-07-02 | 2011-09-07 | パナソニック株式会社 | ロボット、ロボットアームの制御装置、及びロボットアームの制御プログラム |
JP5311294B2 (ja) | 2010-04-28 | 2013-10-09 | 株式会社安川電機 | ロボットの接触位置検出装置 |
JP2013101045A (ja) | 2011-11-08 | 2013-05-23 | Fanuc Ltd | 物品の3次元位置姿勢の認識装置及び認識方法 |
CN102581852B (zh) | 2012-01-20 | 2015-01-14 | 上海交通大学 | 用于机器人重载装配和搬运作业的位姿调整*** |
JP5915214B2 (ja) | 2012-02-01 | 2016-05-11 | セイコーエプソン株式会社 | ロボット装置、組立て方法、及び組立てプログラム |
JP5698789B2 (ja) | 2013-04-18 | 2015-04-08 | ファナック株式会社 | ワークを搬送するロボットの制御装置 |
JP6380828B2 (ja) * | 2014-03-07 | 2018-08-29 | セイコーエプソン株式会社 | ロボット、ロボットシステム、制御装置、及び制御方法 |
-
2015
- 2015-09-09 JP JP2015177471A patent/JP6208724B2/ja active Active
-
2016
- 2016-09-02 DE DE102016116404.6A patent/DE102016116404B4/de active Active
- 2016-09-08 US US15/259,118 patent/US9903698B2/en active Active
- 2016-09-09 CN CN201610814842.XA patent/CN106514712B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH04348885A (ja) * | 1991-05-24 | 1992-12-03 | Nippon Telegr & Teleph Corp <Ntt> | ロボットを用いた部品組立作業における嵌合位置検出方法 |
JPH06143171A (ja) * | 1992-10-28 | 1994-05-24 | Toshiba Corp | ロボット |
JP2015085499A (ja) * | 2013-11-01 | 2015-05-07 | セイコーエプソン株式会社 | ロボット、ロボットシステム、制御装置及び制御方法 |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2019022916A (ja) * | 2017-07-24 | 2019-02-14 | セイコーエプソン株式会社 | ロボット制御装置、ロボット制御方法、ロボットシステムおよびシミュレーション装置 |
JP7187765B2 (ja) | 2017-07-24 | 2022-12-13 | セイコーエプソン株式会社 | ロボット制御装置 |
JP2019084650A (ja) * | 2017-11-09 | 2019-06-06 | キヤノン株式会社 | ロボット装置及び組立品の製造方法 |
JP7154748B2 (ja) | 2017-11-09 | 2022-10-18 | キヤノン株式会社 | ロボットシステム、物品の製造方法、制御方法、制御プログラム、及び記録媒体 |
JP2020196076A (ja) * | 2019-05-31 | 2020-12-10 | 三菱電機株式会社 | ロボットコントローラ及び組付方法 |
Also Published As
Publication number | Publication date |
---|---|
DE102016116404A1 (de) | 2017-03-09 |
US20170067728A1 (en) | 2017-03-09 |
US9903698B2 (en) | 2018-02-27 |
DE102016116404B4 (de) | 2019-05-09 |
JP6208724B2 (ja) | 2017-10-04 |
CN106514712B (zh) | 2018-04-06 |
CN106514712A (zh) | 2017-03-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6208724B2 (ja) | 物体の姿勢算出システム | |
US20210114221A1 (en) | Method of teaching robot and robot system | |
US11059169B2 (en) | Method of controlling robot, method of teaching robot, and robot system | |
JP5366018B2 (ja) | ロボットの教示手順校正装置および方法 | |
JP5311294B2 (ja) | ロボットの接触位置検出装置 | |
US9592608B1 (en) | Methods and systems for providing feedback during teach mode | |
CN107924174B (zh) | 用于确定工具偏移的***和方法 | |
JP2014176943A (ja) | ロボットシステム、校正方法及び被加工物の製造方法 | |
US12011825B2 (en) | Device, method and program for estimating weight and position of gravity center of load by using robot | |
US20200132450A1 (en) | Coordinate measuring system | |
JP2017052015A (ja) | 制御装置、ロボットおよびロボットシステム | |
Shirinzadeh et al. | A hybrid contact state analysis methodology for robotic-based adjustment of cylindrical pair | |
JP5787646B2 (ja) | ロボットシステム及び部品の製造方法 | |
JP2010276604A (ja) | 自動位置合わせプローブアセンブリを有する非破壊検査システム | |
JP2016028227A (ja) | 力覚センサを利用して対象物を検査する検査システム | |
JP2015085499A (ja) | ロボット、ロボットシステム、制御装置及び制御方法 | |
JP5447150B2 (ja) | ロボットの制御装置およびロボットを制御する方法 | |
Oh et al. | A modified perturbation/correlation method for force-guided assembly | |
EP3002088A2 (en) | Orthogonal positioning instrument, system and method for automatic machines | |
Kim et al. | Vision-force guided precise robotic assembly for 2.5 D components in a semistructured environment | |
JP2019155523A (ja) | ロボット制御装置、ロボット制御方法、ロボット制御装置を用いた物品の組立方法、プログラム及び記録媒体 | |
WO2024113286A1 (en) | Method and system for programming an industrial robot | |
US20230328372A1 (en) | Image processing system and image processing method | |
Yussof et al. | Tactile sensing-based control system for dexterous robot manipulation | |
JP2017030068A (ja) | ロボット制御装置、ロボットおよびロボットシステム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
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: 20170815 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20170907 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 6208724 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |