特許 6208724 物体の姿勢算出システム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6208724

(24)【登録日】2017年9月15日

(45)【発行日】2017年10月4日

(54)【発明の名称】物体の姿勢算出システム

(51)【国際特許分類】

   B25J 13/08 20060101AFI20170925BHJP

【ＦＩ】

   B25J13/08 Z

【請求項の数】13

【全頁数】48

(21)【出願番号】特願2015-177471(P2015-177471)

(22)【出願日】2015年9月9日

(65)【公開番号】特開2017-52049(P2017-52049A)

(43)【公開日】2017年3月16日

【審査請求日】2016年11月16日

(73)【特許権者】

【識別番号】390008235

【氏名又は名称】ファナック株式会社

(74)【代理人】

【識別番号】100099759

【弁理士】

【氏名又は名称】青木 篤

(74)【代理人】

【識別番号】100102819

【弁理士】

【氏名又は名称】島田 哲郎

(74)【代理人】

【識別番号】100123582

【弁理士】

【氏名又は名称】三橋 真二

(74)【代理人】

【識別番号】100112357

【弁理士】

【氏名又は名称】廣瀬 繁樹

(74)【代理人】

【識別番号】100157211

【弁理士】

【氏名又は名称】前島 一夫

(74)【代理人】

【識別番号】100159684

【弁理士】

【氏名又は名称】田原 正宏

(72)【発明者】

【氏名】岩竹 隆裕

(72)【発明者】

【氏名】王 凱濛

(72)【発明者】

【氏名】陳 文杰

【審査官】 中田 善邦

(56)【参考文献】

【文献】 特開平４−３４８８８５（ＪＰ，Ａ）

【文献】 特開平６−１４３１７１（ＪＰ，Ａ）

【文献】 特開２０１５−８５４９９（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ１／００−２１／０２

(57)【特許請求の範囲】

【請求項1】

一方の物体と他方の物体との相対的な姿勢を算出する、または前記一方の物体と前記他方の物体との相対的な位置および姿勢を算出する物体の姿勢算出システムにおいて、
凸部を包含する物体と凹部を包含する物体の組み合わせである、かつ、前記凸部と前記凹部とを３つの接触点において接触させることが可能な物体である、かつ、前記凹部における前記３つの接触点は前記凹部の穴の端部に位置する、かつ、前記凸部に対する軸であって前記３つの接触点を含む平面と直交しない軸を凸部中心軸とすると前記接触点と前記凸部中心軸との各最短距離が一定値であるか、または前記接触点から前記凸部中心軸に対して垂線を引いたときの前記凸部中心軸との交点と前記凸部中心軸上の前記交点以外の点との距離に基づいて前記接触点と前記凸部中心軸との各最短距離が定まる、
前記一方の物体および前記他方の物体と、
前記一方の物体と前記他方の物体とを相対的に移動させる、かつ、
前記一方の物体および前記他方の物体の内、少なくとも１つの物体を保持し移動させる１つ以上の駆動部と、
前記一方の物体と前記他方の物体との間に作用する力を計測する力計測部と、
前記駆動部によって前記一方の物体と前記他方の物体とを相対的に移動させて前記一方の物体と前記他方の物体とを接触させ、前記一方の物体と前記他方の物体とが前記３つの接触点で接触するときに前記力計測部によって計測される力および前記最短距離に基づいて、前記一方の物体と前記他方の物体との相対的な姿勢を算出する、または前記一方の物体と前記他方の物体との相対的な位置および姿勢を算出する物体の姿勢算出部と、
を備える、
ことを特徴とする物体の姿勢算出システム。

【請求項2】

前記一方の物体は前記凸部もしくは前記凹部を２つ以上包含する、または、
前記他方の物体は前記凸部もしくは前記凹部を２つ以上包含する、または、
前記一方の物体および前記他方の物体は前記凸部もしくは前記凹部を２つ以上包含する、
請求項１に記載の物体の姿勢算出システム。

【請求項3】

前記一方の物体または前記他方の物体に包含される、前記凸部および前記凹部の内、少なくとも１つの物体は前記包含される前記一方の物体または前記他方の物体に対して着脱可能な物体である、
請求項１または請求項２に記載の物体の姿勢算出システム。

【請求項4】

前記一方の物体と前記他方の物体とを前記３つの接触点で接触させる前に、
前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、
請求項１から請求項３のいずれか一項に記載の物体の姿勢算出システム。

【請求項5】

前記力計測部は、さらに、前記駆動部または前記一方の物体または前記他方の物体に作用する力を計測し、
前記一方の物体と前記他方の物体とを前記３つの接触点で接触させる前に、
前記力計測部によって計測される力に基づいて、
前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、
請求項１から請求項３のいずれか一項に記載の物体の姿勢算出システム。

【請求項6】

前記物体の姿勢算出システムは、さらに、
前記一方の物体または前記他方の物体の位置および姿勢の少なくとも一方を所定の範囲以内で検出することが可能な物体検出部を備え、
前記一方の物体と前記他方の物体とを前記３つの接触点で接触させる前に、
前記物体検出部の検出結果に基づいて、
前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、
請求項１から請求項３のいずれか一項に記載の物体の姿勢算出システム。

【請求項7】

前記物体の姿勢算出システムは、さらに、
空間内の位置または位置および姿勢を指定する装置と前記指定した位置または位置および姿勢を認識する装置とで構成される空間位置指定部を備え、
前記一方の物体と前記他方の物体とを前記３つの接触点で接触させる前に、
前記空間位置指定部によって指定される位置または位置および姿勢に基づいて、
前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、
請求項１から請求項３のいずれか一項に記載の物体の姿勢算出システム。

【請求項8】

前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、
前記駆動部に対する、前記一方の物体または前記他方の物体の位置および姿勢の少なくとも一方の算出を行う、
請求項１から請求項７のいずれか一項に記載の物体の姿勢算出システム。

【請求項9】

前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、
前記駆動部によって移動される前記一方の物体または前記他方の物体または前記一方の物体および前記他方の物体、を移動させる位置または姿勢または位置および姿勢を補正する、
請求項１から請求項８のいずれか一項に記載の物体の姿勢算出システム。

【請求項10】

前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、
前記一方の物体または前記他方の物体の良否判定、または前記一方の物体もしくは前記他方の物体の種類判定、または前記駆動部によって保持される前記一方の物体もしくは前記他方の物体の保持状態の判定の内、少なくとも１つの判定を実行する、
請求項１から請求項９のいずれか一項に記載の物体の姿勢算出システム。

【請求項11】

前記一方の物体および前記他方の物体における、前記凸部と前記凹部との組み合わせにおいて、前記凸部と前記凹部とは嵌合させることが可能であり、
前記駆動部は、前記一方の物体と前記他方の物体とを相対的に移動させる、かつ、
前記一方の物体および前記他方の物体の内、少なくとも１つの物体を保持し移動させる、かつ、前記一方の物体と前記他方の物体とを嵌合させるように移動が可能であり、
前記物体の姿勢算出システムは、さらに、
前記力計測部によって計測される力に基づいて、前記駆動部によって前記一方の物体と前記他方の物体とを相対的に移動させて力制御を実行し、前記一方の物体と前記他方の物体とを嵌合させる嵌合実行部を備え、
前記嵌合実行部は、前記物体の姿勢算出部によって算出される、前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記一方の物体と前記他方の物体とを嵌合させる、
請求項１から請求項１０のいずれか一項に記載の物体の姿勢算出システム。

【請求項12】

前記嵌合実行部は、
前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、
前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢を修正するように移動させた後、
前記一方の物体と前記他方の物体とを嵌合させる、
請求項１１に記載の物体の姿勢算出システム。

【請求項13】

前記嵌合実行部は、
前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、
前記一方の物体と前記他方の物体との相対的な姿勢が大きいときには、前記一方の物体と前記他方の物体との相対的な姿勢を小さくする移動速度が大きくなるように前記力計測部によって計測される力に基づいて力制御を行い、
前記一方の物体と前記他方の物体とを嵌合させる、
請求項１１に記載の物体の姿勢算出システム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、一方の物体と他方の物体とが３つの接触点で接触しているときに、一方の物体と他方の物体との間に作用する力に基づいて、一方の物体と他方の物体との相対的な姿勢、または相対的な位置および姿勢を算出する物体の姿勢算出システムに関する。

【背景技術】

【0002】

一方の物体と他方の物体との相対姿勢を算出するとき、カメラや三次元センサなどの非接触式のセンサを用いて、相対的な姿勢を測定、算出するといった方法が一般的に知られている。

【0003】

また、一方の物体と他方の物体との間に作用する力を制御することが可能なシステムである場合、一方の物体の所定の部分を他方の物体の所定の部分に面を合わせたり、一方の物体を他方の物体に対して嵌合させたりすることによって、動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出するといった方法が一般的に知られている。

【0004】

特許文献１には、一方の物体と他方の物体との間に作用する力が目標値となるように、コンプライアンス機構の運動モデルに基づいて一方の物体の位置を制御する方法が開示されている。

【0005】

一方の物体と他方の物体とが嵌合可能な物体である場合、このような方法によって、一方の物体と他方の物体との間に作用する力に基づいて、一方と他方の物体とを嵌合させ、動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出することができる。

【0006】

また特許文献２には、アプローチ点の姿勢誤差を修正するために、予め、穴部分と直交する平面に所定の力で押しつけて面接触させて、面を合わせることによって、姿勢誤差を算出する方法が記載されている。

【0007】

この方法では、一方の物体と他方の物体との間に作用する力を制御して、一方の物体を他方の物体に対して面を合わせ、一方の物体の動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出する。さらに、前記算出された相対的な姿勢に基づいて、嵌合作業の前のアプローチ点を修正するという方法が開示されている。

【先行技術文献】

【特許文献】

【0008】

【特許文献1】特公平０４−０４３７４４号公報

【特許文献2】特開平０９−０９１０２６号公報

【発明の概要】

【発明が解決しようとする課題】

【0009】

カメラや三次元センサなどの非接触式のセンサを用いて、一方の物体と他方の物体との相対的な姿勢を測定、算出する場合、システムとして複雑になったり、コストが高くなったりすることがある。また、一方の物体および他方の物体の状態によって、例えば一方の物体と他方の物体との位置関係、周辺の物体との干渉状況、一方の物体と他方の物体の表面形状などの状況によって、一方の物体と他方の物体との相対的な姿勢の算出が困難な場合も存在する。

【0010】

一方の物体と他方の物体との間に作用する力を制御することが可能なシステムを用いて、一方の物体を他方の物体に対して嵌合させるときの、動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出する場合、嵌合作業に時間がかかりシステムのサイクルタイムが増大する。また、一方の物体および他方の物体は、嵌合させることが可能な物体でないといけない。また、嵌合作業のために一方の物体を移動させなければならない。また、嵌合作業時の一方の物体の移動に伴って、周辺の物体との干渉などの問題が発生することがある。また、一方の物体と他方の物体との嵌合が正常に完了しない限り、一方の物体と他方の物体との相対的な姿勢を算出することはできない。さらに、一方の物体と他方の物体との相対的な姿勢が大きい場合、嵌合させることは困難な場合が多い。

【0011】

一方の物体と他方の物体との間に作用する力を制御することが可能なシステムを用いて、一方の物体の面を他方の物体の面に対して合わせるときの、動作開始時の姿勢と、動作終了時の姿勢とから、一方の物体と他方の物体との相対的な姿勢を算出する場合、面を合わせる作業に時間がかかりシステムのサイクルタイムが増大する。また、一方の物体および他方の物体は、面を合わせることが可能な物体でないといけない。また、面合わせ作業のために一方の物体を移動させなければならない。また、面合わせ作業時の一方の物体の移動に伴って、周辺の物体との干渉などの問題が発生することがある。

【0012】

そこで本発明は、一方の物体と他方の物体との相対的な姿勢、または相対的な位置および姿勢を好適に算出するシステムを提供することを目的とする。

【課題を解決するための手段】

【0013】

上記目的を達成するために、本願第１の発明は、一方の物体と他方の物体との相対的な姿勢を算出する、または前記一方の物体と前記他方の物体との相対的な位置および姿勢を算出する物体の姿勢算出システムにおいて、凸部を包含する物体と凹部を包含する物体の組み合わせである、かつ、前記凸部と前記凹部とを３つの接触点において接触させることが可能な物体である、かつ、前記凹部における前記３つの接触点は前記凹部の穴の端部に位置する、かつ、前記凸部に対する軸であって前記３つの接触点を含む平面と直交しない軸を凸部中心軸とすると前記接触点と前記凸部中心軸との各最短距離が一定値であるか、または前記接触点から前記凸部中心軸に対して垂線を引いたときの前記凸部中心軸との交点と前記凸部中心軸上の前記交点以外の点との距離に基づいて前記接触点と前記凸部中心軸との各最短距離が定まる、前記一方の物体および前記他方の物体と、前記一方の物体と前記他方の物体とを相対的に移動させる、かつ、前記一方の物体および前記他方の物体の内、少なくとも１つの物体を保持し移動させる１つ以上の駆動部と、前記一方の物体と前記他方の物体との間に作用する力を計測する力計測部と、前記駆動部によって前記一方の物体と前記他方の物体とを相対的に移動させて前記一方の物体と前記他方の物体とを接触させ、前記一方の物体と前記他方の物体とが前記３つの接触点で接触するときに前記力計測部によって計測される力および前記最短距離に基づいて、前記一方の物体と前記他方の物体との相対的な姿勢を算出する、または前記一方の物体と前記他方の物体との相対的な位置および姿勢を算出する物体の姿勢算出部と、を備える、ことを特徴とする物体の姿勢算出システムを提供する。

【0014】

第２の発明は、第１の発明において、前記一方の物体は前記凸部もしくは前記凹部を２つ以上包含する、または、前記他方の物体は前記凸部もしくは前記凹部を２つ以上包含する、または、前記一方の物体および前記他方の物体は前記凸部もしくは前記凹部を２つ以上包含する、物体の姿勢算出システムを提供する。

【0015】

第３の発明は、第１または第２の発明において、前記一方の物体または前記他方の物体に包含される、前記凸部および前記凹部の内、少なくとも１つの物体は前記包含される前記一方の物体または前記他方の物体に対して着脱可能な物体である、物体の姿勢算出システムを提供する。

【0016】

第４の発明は、第１〜第３のいずれか１つの発明において、前記一方の物体と前記他方の物体とを前記３つの接触点で接触させる前に、前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムを提供する。

【0017】

第５の発明は、第１〜第３のいずれか１つの発明において、前記力計測部は、さらに、前記駆動部または前記一方の物体または前記他方の物体に作用する力を計測し、前記一方の物体と前記他方の物体とを前記３つの接触点で接触させる前に、前記力計測部によって計測される力に基づいて、前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムを提供する。

【0018】

第６の発明は、第１〜第３のいずれか１つの発明において、前記物体の姿勢算出システムは、さらに、前記一方の物体または前記他方の物体の位置および姿勢の少なくとも一方を所定の範囲以内で検出することが可能な物体検出部を備え、前記一方の物体と前記他方の物体とを前記３つの接触点で接触させる前に、前記物体検出部の検出結果に基づいて、前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムを提供する。

【0019】

第７の発明は、第１〜第３のいずれか１つの発明において、前記物体の姿勢算出システムは、さらに、空間内の位置または位置および姿勢を指定する装置と前記指定した位置または位置および姿勢を認識する装置とで構成される空間位置指定部を備え、前記一方の物体と前記他方の物体とを前記３つの接触点で接触させる前に、前記空間位置指定部によって指定される位置または位置および姿勢に基づいて、前記一方の物体と前記他方の物体との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め前記一方の物体と他方の物体とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムを提供する。

【0020】

第８の発明は、第１〜第７のいずれか１つの発明において、前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記駆動部に対する、前記一方の物体または前記他方の物体の位置および姿勢の少なくとも一方の算出を行う、物体の姿勢算出システムを提供する。

【0021】

第９の発明は、第１〜第８のいずれか１つの発明において、前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記駆動部によって移動される前記一方の物体または前記他方の物体または前記一方の物体および前記他方の物体、を移動させる位置または姿勢または位置および姿勢を補正する、物体の姿勢算出システムを提供する。

【0022】

第１０の発明は、第１〜第９のいずれか１つの発明において、前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記一方の物体または前記他方の物体の良否判定、または前記一方の物体もしくは前記他方の物体の種類判定、または前記駆動部によって保持される前記一方の物体もしくは前記他方の物体の保持状態の判定の内、少なくとも１つの判定を実行する、物体の姿勢算出システムを提供する。

【0023】

第１１の発明は、第１〜第１０のいずれか１つの発明において、前記一方の物体および前記他方の物体における、前記凸部と前記凹部との組み合わせにおいて、前記凸部と前記凹部とは嵌合させることが可能であり、前記駆動部は、前記一方の物体と前記他方の物体とを相対的に移動させる、かつ、前記一方の物体および前記他方の物体の内、少なくとも１つの物体を保持し移動させる、かつ、前記一方の物体と前記他方の物体とを嵌合させるように移動が可能であり、前記物体の姿勢算出システムは、さらに、前記力計測部によって計測される力に基づいて、前記駆動部によって前記一方の物体と前記他方の物体とを相対的に移動させて力制御を実行し、前記一方の物体と前記他方の物体とを嵌合させる嵌合実行部を備え、前記嵌合実行部は、前記物体の姿勢算出部によって算出される、前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記一方の物体と前記他方の物体とを嵌合させる、物体の姿勢算出システムを提供する。

【0024】

第１２の発明は、第１１の発明において、前記嵌合実行部は、前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢を修正するように移動させた後、前記一方の物体と前記他方の物体とを嵌合させる、物体の姿勢算出システムを提供する。

【0025】

第１３の発明は、第１１の発明において、前記嵌合実行部は、前記物体の姿勢算出部が算出した前記一方の物体と前記他方の物体との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記一方の物体と前記他方の物体との相対的な姿勢が大きいときには、前記一方の物体と前記他方の物体との相対的な姿勢を小さくする移動速度が大きくなるように前記力計測部によって計測される力に基づいて力制御を行い、前記一方の物体と前記他方の物体とを嵌合させる、物体の姿勢算出システムを提供する。

【発明の効果】

【0026】

本発明では、一方の物体と他方の物体とが３つの接触点で接触しているときの、一方の物体と他方の物体との間に作用する力に基づいて、一方の物体と他方の物体との相対的な姿勢を算出するので、一方の物体と他方の物体を接触させることによって、一方の物体と他方の物体との相対的な姿勢が簡易に算出できる。

【0027】

このため、本発明によれば、システムとして一方の物体と他方の物体とを３つの接触点で接触させ、一方の物体と他方の物体との間に作用する力を計測、算出することができるだけでよく、物体の位置および姿勢を高精度に検出可能なカメラや三次元センサなどの非接触式のセンサを必要とせず、システムを簡素化し、コストを抑えることが可能となる。また、一方の物体および他方の物体は、カメラや三次元センサなどの非接触式のセンサで、例えば表面状況などによって、検出できない物体、検出し難い物体でもよい。また、システムの状況による干渉などの問題のために、カメラや三次元センサなどの非接触式のセンサで一方の物体および他方の物体が検出できない状況にも本発明は適用できる。

【0028】

また、本発明によれば、一方の物体と他方の物体との相対的な姿勢を算出するために、一方の物体を他方の物体に対して嵌合させる動作を必要としない。このため、嵌合作業にかかる時間が不要となり、システムのサイクルタイムを短くすることができる。また、嵌合作業の成功または失敗に依らず、相対的な位置および姿勢を算出できる。また、一方の物体および他方の物体は、嵌合させることが可能な物体でなくてもよい。また、一方の物体と他方の物体との相対的な姿勢が大きい場合に、一方の物体と他方の物体との相対的な姿勢を算出することができる。

【0029】

また、本発明によれば、一方の物体と他方の物体との相対的な姿勢を算出するために、一方の物体を他方の物体に対して面を合わせる動作を必要としない。このため、面合わせ作業にかかる時間がなく、システムのサイクルタイムを短くすることができる。また、面合わせ作業の成功または失敗に依らず、相対的な位置および姿勢を算出できる。また、一方の物体および他方の物体は、面を合わせることが可能な物体でなくてもよい。

【図面の簡単な説明】

【0030】

【図1】本発明の第一の実施形態に係るシステムの概略構成を示す図である。

【図2】本発明の第一の実施形態に係るシステムの概略構成を示す図である。

【図3】一方の物体と他方の物体とを３つの接触点で接触させる態様を示す図である。

【図4】一方の物体および他方の物体が凸部や凹部を包含する態様を示す図である。

【図5】一方の物体および他方の物体の少なくともいずれかの物体に包含される凸部の形状の一例を示す図である。

【図6】一方の物体および他方の物体の少なくともいずれかの物体に包含される凸部の断面形状の例を示す図である。

【図7】一方の物体および他方の物体の少なくともいずれかの物体に包含される凹部の形状の例を示す図である。

【図8】一方の物体および他方の物体の少なくともいずれかの物体に包含される凹部の断面形状の一例を示す図である。

【図9】一方の物体および他方の物体における、凸部と凹部の組み合わせの一例を示す図である。

【図10】一方の物体と他方の物体とを３つの接触点で接触させた態様を示す図である。

【図11】一方の物体と他方の物体とを３つの接触点で接触させた態様を示す図である。

【図12】一方の物体と他方の物体とを３つの接触点で接触させた態様を示す図である。

【図13】一方の物体と他方の物体とを２つの接触点で接触させた態様を示す図である。

【図14】姿勢算出用の力計測座標系の設定の一例を示す図である。

【図15】一方の物体と他方の物体とを３つの接触点で接触させる動作の一例を示す図である。

【図16】姿勢算出用の力計測座標系の設定の一例を示す図である。

【図17】他方の物体に複数の凹部を包含させた他方の物体の形状を示す図である。

【図18】一方の物体および他方の物体に包含させる、複数の凸部および凹部を包含した形状を示す図である。

【発明を実施するための形態】

【0031】

以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。

【0032】

本発明において、「力」とは、特に断りがない限り、力の並進方向成分および力のモーメント成分を含むものとする。また、「Ａおよび／またはＢ」とは、ＡおよびＢの少なくとも一方を含むものとし、例えば、「位置および／または姿勢」とは、位置および姿勢の少なくとも一方を含むものとする。

【0033】

本実施例では、空間に対して設定した座標系とは、空間に対して固定させた直交座標系上で、駆動部の先端部分や、駆動部の先端部分に保持された（取りつけられた状態も含む）物体の位置および／または姿勢を表すことが可能な座標系とする。なお、駆動部の先端部分とは、駆動部において、物体を保持して移動させることが可能な部分とする。また、ツール座標系とは、駆動部の先端部分または駆動部の先端部分に保持された物体に対して設定される座標系であり、駆動部の台座に対して設定される座標系上または空間に対して設定した基準座標系上の、駆動部の先端部分または駆動部の先端部分に保持された物体の位置および／または姿勢を表すことが可能な座標系とする。また、制御点とは、駆動部の先端部分や先端部分に保持される物体の位置を表す点とする。また、力制御座標系とは、一方（第１）の物体と他方（第２）の物体との間に作用する力に基づいて、駆動部の先端部分に保持された物体の位置および／または姿勢を制御するために、制御点に原点を持つように設定される座標系とする。また、姿勢算出用の力計測座標系とは、一方の物体と他方の物体との間に作用する力に基づいて、物体の姿勢算出部が一方の物体と他方の物体との、相対的な姿勢、または、相対的な位置および相対的な姿勢を算出するために設定される座標系とする。また、或る座標系上の力に変換するとは、或る力について、或る座標系の原点に対して作用する力として、或る座標系上で表すこととする。

【0034】

本実施例の説明において、数式における、”・”は行列とベクトルの乗算、”×”は外積、”＊”は乗算、“／”は商算、“Ｃｏｓ（ｘ）”は余弦関数、“Ｓｉｎ（ｘ）”は正弦関数、“Ａｒｃｃｏｓ（ｘ）”は余弦関数の逆三角関数、“Ａｒｃｓｉｎ（ｘ）”は正弦関数の逆三角関数、“Ａｒｃｔａｎ（ｘ）”は正接関数の逆三角関数、“≒”は左辺を右辺と近似することを表す。また、Ｐ（点ｂ＿座標系ａ）は、座標系ａ上の点ｂの位置と表す。また、（ベクトルＡ）＾Ｔは、ベクトルＡを転置した転置ベクトルを表す。

【0035】

物体の姿勢算出システムは、一方の物体と他方の物体との相対的な姿勢を算出するシステムである。図１は本発明の１番目の実施形態に係る物体の姿勢算出システム１１の構成例を示す概略図である。物体の姿勢算出システム１１は、一方（第１）の物体２１と、他方（第２）の物体２２と、一方の物体２１および他方の物体２２の内、少なくとも１つの物体を保持し移動させる駆動部として一方の物体２１を移動させる駆動部５０と、一方の物体２１と他方の物体２２との間に作用する力を計測する力計測部３１と、一方の物体２１と他方の物体２２との相対的な姿勢を算出する物体の姿勢算出部３２とを備える。
物体の姿勢算出システム１１は、さらに、駆動部制御装置５２を備える。駆動部制御装置５２は、駆動部５０の動作指令を生成する動作指令生成部３３の他に、力計測部３１、物体の姿勢算出部３２を備える。本実施例では、力計測部３１、物体の姿勢算出部３２は、駆動部制御装置５２に備わるとしたが、駆動部制御装置５２と接続された中央制御装置や、別の制御装置や演算処理装置などに含まれるようにしてもよい。

【0036】

駆動部５０は１つ以上の制御軸によって構成され、各制御軸は駆動部制御装置５２によって所定の制御周期毎に制御され、それにより駆動部５０の先端部分が指定された場所に位置決めされ得るようになっている。駆動部制御装置５２は、ＣＰＵ、ＲＯＭ、ＲＡＭなどを含むハードウェア構成を有していて、図示しないが、他に、記憶部、表示出力部、入力部、各種信号を出力する信号出力部なども具備し、後述する種々の機能を実行する。
図１に図示される駆動部５０、図２に図示される駆動部５０’は、６つの軸によって構成される垂直多関節型ロボットとするが、一方の物体２１および他方の物体２２を相対的に並進移動または回転移動または並進移動および回転移動することが可能であれば、他の形態を有する任意の公知のロボットや、移動機構を備える公知の駆動装置にも同様に適用可能である。他方の物体２２が移動するとき、他方の物体２２の位置および姿勢に基づいて、一方の物体２１を移動させるようにしてもよい。

【0037】

動作指令生成部３３は、駆動部５０が滑らかに、かつ、所望の位置および／または姿勢に移動できるように、動作指令を生成し、駆動部５０の各制御軸への動作指令を出力する。また、動作指令生成部３３は、加加速度が急に大きくならないようにフィルタリング処理をしたり、また、駆動部５０の動作の振動が少なくなるように振動の低減の処理をしたり、また、目的とする位置に正確に移動できるように駆動部５０の撓みなどを考慮して指令の位置および／または姿勢を補正したりして動作指令を調整してもよい。図２に示されるように、駆動部制御装置５２が、駆動部５０、駆動部５０’を移動させる場合、駆動部制御装置５２は駆動部５０’についても、駆動部５０と同様に、駆動部５０’が滑らかに、かつ、所望の位置および／または姿勢に移動できるように、動作指令を生成し、駆動部５０’の各制御軸への動作指令を出力する。

【0038】

図２は、本発明の１番目の別の一実施形態に係る物体の姿勢算出システム１１aの構成例を示す概略図である。物体の姿勢算出システム１１aは、一方の物体２１と、他方の物体２２と、一方の物体２１および他方の物体２２の内、少なくとも１つの物体を保持し移動させる駆動部として一方の物体２１を保持し移動させる駆動部５０と他方の物体２２を保持し移動させる駆動部５０’と、力計測部３１と、物体の姿勢算出部３２と、を備える。駆動部５０’の先端部分には６軸の力センサ５１’が取りつけられていている。

【0039】

物体の姿勢算出システム１１aのように、物体の姿勢算出システム１１に対して、さらに、他方の物体２２を移動させる、駆動部５０’を備えるようにしてもよい。このように、物体の姿勢算出システムは、一方の物体と他方の物体を独立に移動させることができるように、駆動部を２つ備えるようにしてもよい。駆動部５０’は、駆動部制御装置５２によって各制御軸が制御され移動する。物体の姿勢算出システム１１aにおける駆動部制御装置５２は、物体の姿勢算出システム１１における駆動部制御装置５２に対して、さらに、動作指令生成部３３が駆動部５０’の動作指令も生成する。また、力計測部３１は、駆動部５０’の他方の物体２２を保持する部分に作用する力に基づいて、一方の物体２１と他方の物体２２との間に作用する力を計測するようにしてもよい。また、駆動部制御装置５２とは別に、駆動部５０’を移動させる駆動部制御装置を備えるようにしてもよい。

【0040】

図２に示す駆動部５０’を使用する場合、一方の物体２１と他方の物体２２とを接触させるとき、駆動部５０が干渉などせずに移動しやすいようにしたり、また、駆動部５０が並進移動または回転移動だけすればよいようにしたりと、駆動部５０が一方の物体２１の凸部と他方の物体２２の凹部とを接触させやすいように、駆動部５０の移動可能な範囲、移動可能な方向、周辺装置の状況などを考慮して、駆動部５０’によって他方の物体２２を移動させるようにしてもよい。また、駆動部５０’が他方の物体２２を移動させるとき、その移動量を考慮して、駆動部５０を移動させることが好ましい。また、駆動部５０’の先端部分に備わる力センサ５１’を使用し、その出力に基づいて、一方の物体２１と他方の物体２２との間に作用する力を力計測部３１によって計測してもよい。

【0041】

以下では、本発明の一実施形態について、図１に示す本発明の一実施形態に係る物体の姿勢算出システム１１を用いて説明する。

【0042】

物体の姿勢算出システム１１は、駆動部５０によって、一方の物体２１と他方の物体２２とを相対的に移動させ一方の物体２１と他方の物体２２とを接触させ、一方の物体２１と他方の物体２２とが３つの接触点で接触するときに、一方の物体２１と他方の物体２２との間に作用する力を計測する力計測部３１の出力に基づいて、物体の姿勢算出部３２によって一方の物体２１と他方の物体２２との相対的な姿勢または相対的な位置および姿勢を算出する。

【0043】

一方の物体２１および他方の物体２２は、凸部を包含する物体と凹部を包含する物体の組み合わせであり、かつ、前記凸部と前記凹部とを３つの接触点において接触させることが可能な物体である。つまり、一方の物体２１が凸部を包含するときには、他方の物体２２は前記凸部と３つの接触点において接触させることが可能な凹部を１つ以上、包含する。また、一方の物体２１が凹部を包含するときには、他方の物体２２は前記凹部と３つの接触点において接触させることが可能な凸部を１つ以上、包含する。また、前記凹部における前記３つの接触点は前記凹部の穴の端部に位置する。また、前記凸部に対する軸であって前記３つの接触点を含む平面と直交しない軸を凸部中心軸とすると前記接触点と前記凸部中心軸との各最短距離が所定の値（一定値）となる、または、前記接触点から前記凸部中心軸に対して垂線を引いたときの前記凸部中心軸との交点と前記凸部中心軸上の所定の点（前記交点以外の点）との距離に基づいて前記接触点と前記凸部中心軸との各最短距離が定まる。

【0044】

図３には、一方の物体２１および他方の物体２２に包含される、凸部と凹部とを３つの接触点において接触させる態様が図示されている。図３（ａ）には、一方の物体２１に包含される円筒形の外側部分を凸部として、他方の物体２２に包含される円筒形の内側部分を凹部として、一方の物体２１の凸部と他方の物体２２の凹部とを３つの接触点において接触させる態様が示されている。図３（ｂ）には、一方の物体２１に包含される円筒形の内側部分を凹部として、他方の物体２２に包含される円筒形の外側部分を凸部として、一方の物体２１の凹部と他方の物体２２の凸部とを３つの接触点において接触させる態様が示されている。このように、一方の物体２１の凸部を他方の物体２２の凹部に対して３つの接触点で接触させてもよいし、一方の物体２１の凹部を他方の物体２２の凸部に対して３つの接触点で接触させてもよい。また、同じ大きさ程度の円筒形である場合、互いに凸部にも凹部にもなりうる。このように、円筒形のように、適切な大きさの物体であって、物体の中に空間がある物体である場合、凸部にも、凹部にもなりうる。また、図３（ａ）に示されるように、一方の物体２１の凸部は、他方の物体２２の凹部に対して、接触させる円筒の径が大きくても、小さくてもよい。また、図３（ｂ）に示されるように、一方の物体２１に包含される凹部は、他方の物体２２に包含される凸部に対して、接触させる円筒の径が大きくても、小さくてもよい。

【0045】

図４には、一方の物体２１および他方の物体２２が、凸部や凹部を包含する態様が示されている。一方の物体２１および他方の物体２２は、各々が少なくとも１組以上の凸部と凹部を有し、一方の物体２１の凹部と他方の物体２２の凸部、または一方の物体２１の凸部と他方の物体２２の凹部とは、３つの接触点において互いに接触させることが可能となっている。また、一方の物体２１および他方の物体２２は、大きさや形状の異なる、凸部や凹部を複数、包含するようにしてもよい。

【0046】

図５には、一方の物体２１および他方の物体２２の少なくともいずれかの物体に包含される凸部２３の形状の一例が図５に例示されている。凸部２３は、図５（ａ）に示すように円柱形状の物体としてもよいし、図５（ｂ）に示すように断面形状が円の一部が欠けた形状の物体としてもよいし、図５（ｃ）に示すように断面形状が、図５（ｂ）に示す断面形状よりもさらに円が欠けた形状の物体としてもよい。或いは、凸部２３は、図５（ｄ）に示すように断面形状が、円の３箇所が欠けた形状の物体としてもよいし、図５（ｅ）に示すように、所定の位置までは断面形状が、円の２箇所が欠けた形状の物体であり、所定の位置以外の部分は円柱形状の物体としてもよい。或いは、凸部２３は、図５（ｆ）に示すように、円筒形状の物体のように、物体の内側に物体が存在しない空間がある物体としてもよいし、図５（ｇ）、図５（ｈ）に示すように、テーパー形状のように、先端からの距離に対し径が変化する物体としてもよい。テーパー形状の場合、図５（ｇ）に示すように、径を線形的に増加させたり、減少させたりするようにしてもよいし、図５（ｈ）に示すように、径を指数関数的、２次関数的など非線形的に、増加させたり、減少させたりするようにしてもよい。図５（ａ）、図５（ｆ）、図５（ｇ）、図５（ｈ）に示される物体の場合、凸部２３が軸対称となる軸を、凸部中心軸とすることができる。また、図５（ｂ）、図５（ｃ）、図５（ｄ）、図５（ｅ）に示される物体の場合、凸部２３を最小限に包含する円柱形状の中心軸を、凸部中心軸とすることができる。

【0047】

図６には、一方の物体２１および他方の物体２２の少なくともいずれかの物体に包含される凸部２３における、凸部２３に対して設定した凸部中心軸に沿った断面形状の一例が図６（ａ）、図６（ｂ）に例示されている。凸部２３は、凸部２３に対して設定した凸部中心軸に沿った断面形状で見た場合、図６（ａ）に示す断面形状のように、先端部分に、例えば半球形状のような凸形状を保有し、それ以外の部分は円柱形状であるような物体としてもよい。この場合、凸部の先端部分が半球形状のような凸形状となっているので、凸部２３は他方の物体２２の凹部の端部などに存在する平面に対して面を合わせることができないため、他方の物体２２に存在する面に対して面を合わせることによって凸部と凹部との相対的な姿勢を算出することはできない。また、凸部２３は、凸部２３に対して設定した凸部中心軸に沿った断面形状で見た場合、図６（ｂ）に示す断面形状のように、先端部分に、例えば半球形状の窪みのような、凹形状を有し、それ以外の部分は円柱形状であるような物体としてもよい。

【0048】

このように、一方の物体２１および他方の物体２２の少なくともいずれかに包含される凸部は、先端部分の断面形状が円形状や、円形状の一部を包含する形状を構成する形状であり、その断面形状が所定位置まで続き、所定位置以降は円柱形状を成す凸部の形状の部分が包含される形状であってもよい。また、一方の物体２１および他方の物体２２の少なくともいずれかに包含される凸部は、中空であったり、テーパーであったりしてもよい。また、一方の物体２１および他方の物体２２の少なくともいずれかに包含される凸部は、その凸部の、先端部分に凸形状や凹形状を有する物体であってもよい。このように、一方の物体２１および他方の物体２２の少なくともいずれかに包含される凸部は、前記条件を満たしていれば、凸部と凹部が３つの接触点で接触する部分以外はどのような形状であってもよい。

【0049】

図７には、一方の物体２１および他方の物体２２の少なくともいずれかの物体に包含される凹部２４の穴の端部の形状の一例が例示されている。凹部２４は、図７（ａ）に示すように、凹部の穴の端部の形状が、円形状である物体としてもよいし、図７（ｂ）に示すように、凹部の穴の端部の形状が、円の一部が欠けた形状である物体としてもよい。或いは、凹部２４は、図７（ｃ）に示すように、凹部の穴の端部の形状が、図７（ｂ）に示す凹部の穴の端部の形状よりもさらに円が欠けた形状の物体としてもよいし、図７（ｄ）に示すように、凹部の穴の端部の形状が、円の３箇所が欠けた形状の物体としてもよい。

【0050】

図８には、一方の物体２１および他方の物体２２の少なくともいずれかの物体に包含される凹部２４における、凹部の穴の中心軸方向に沿った断面形状の一例が例示されている。凹部２４は、図８（ａ）に示すように、凹部の穴の端部と同じ形状が所定の位置まで続く形状である物体としてもよいし、図８（ｂ）に示すように、例えば半球形状の窪みの形状が存在するような、凹部２４の穴の中心軸方向に沿って、凹部２４の穴の中心軸と直交する平面における凹部の断面形状が変わるような物体としてもよい。

【0051】

このように、一方の物体２１および他方の物体２２の少なくともいずれかの物体に包含される凹部は、凹部の穴の端部の形状が円形状や、円形状の一部を包含する形状であるような、窪みとしてもよい。また、一方の物体２１および他方の物体２２の少なくともいずれかの物体に包含される凹部は、凹部の穴の中心軸方向に沿って、前記凹部の穴の中心軸と直交する平面における凹部の断面形状が変わるなど、凹部の穴の端部の形状と同じ形状が続くような物体でなくてもよい。また、一方の物体２１および他方の物体２２の少なくともいずれかの物体に包含される凹部は、凹部の穴の中心軸方向に沿って、凹部の穴を囲う、周囲の形状が変化していく、例えば半球形状の凸形状の物体に前記凹部の穴が存在するようにした形状などであってもよい。このように、一方の物体２１および他方の物体２２の少なくともいずれかに包含される凹部は、前記条件を満たしていれば、凸部と凹部が３つの接触点で接触する部分以外はどのような形状であってもよい。

【0052】

図９には、前述のように説明した、一方の物体２１および他方の物体２２のいずれかに包含される凸部２３と、前記物体とは異なる一方の物体２１および他方の物体２２のいずれかに包含される凹部２４の、組み合わせの一例が例示されている。図９（ａ）には、凸部２３が円柱形状であり、凹部２４が円筒形状である場合が示されている。図９（ｂ）には、凸部２３、凹部２４が共に円筒形状である場合が示されている。図９（ｃ）には、凸部２３が円柱形状であり、凹部２４が、半球のような凸形状の物体に凹部の穴が存在する形状である場合が示されている。図９（ｄ）には、凸部２３が、円柱形状の先端に、半球のような凸形状の物体が存在する形状であり、凹部２４が円筒形状である場合が示されている。

【0053】

また、一方の物体２１および他方の物体２２の少なくともいずれかに包含される凸部の部分と、一方の物体２１および他方の物体２２の少なくともいずれかに包含される凹部の部分の内、少なくとも１つの部分を、包含される物体に対して、着脱可能な物体としてもよい。つまり、一方の物体２１に包含される凸部の部分や凹部の部分は、一方の物体２１に対して、着脱可能な物体であってもよい。また、他方の物体２２に包含される凸部の部分や凹部の部分は、他方の物体２２に対して、着脱可能な物体であってもよい。

【0054】

駆動部は、物体を移動させる機構部が１つ以上存在し、一方の物体２１と他方の物体２２とを相対的に移動させることができるように構成され、一方の物体２１および他方の物体２２の内、少なくとも１つの物体を保持し移動させる。本実施例において、駆動部が物体を保持するとは、駆動部が物体を把持したり、駆動部に物体が設置されたり、取りつけられたりするなど、駆動部によって物体を移動することができるように物体を存在させた状態にする動作とする。図２を参照して説明したように、一方の物体２１を移動させる場合は駆動部５０を使用し、他方の物体２２を移動させる場合は駆動部５０’を使用するようにしてもよい。このように、前述のような１つ以上の駆動部によって、一方の物体２１と他方の物体２２とを相対的に移動させるようにしてもよい。駆動部は、並進移動または回転移動または並進移動および回転移動によって、一方の物体２１と他方の物体２２とを相対的に移動させ、一方の物体２１と他方の物体２２とを３つの接触点において接触させることができれば、どのような移動機構を備える機構部であってもよい。図１に示す駆動部５０の一方の物体２１を保持する先端部分には、駆動部５０の先端部分に作用する力を検出するための、力センサ５１が取りつけられている。

【0055】

力センサ５１は、一方の物体２１と他方の物体２２との間に作用する力を検出する検出装置である。また、力センサ５１によって、駆動部５０の先端部分に作用する力を検出することができる。駆動部制御装置５２は、所定時間毎に検出された力センサ５１の出力をもとに、力計測部３１によって、一方の物体２１と他方の物体２２との間に作用する力を計測する。力センサ５１は、本実施例では６軸の力センサとするが、作業に必要な自由度を有する力センサであればよい。また、力センサ５１の取付位置は、一方の物体２１と他方の物体２２との間に作用する力を検出できる限り、駆動部５０の先端部分、駆動部５０の各制御軸、駆動部５０の設置部分、他方の物体２２の設置部分、図２に示した駆動部５０’の先端部分など、任意の箇所であってよい。なお、本実施例においては、力計測部３１は力センサ５１の出力をもとに一方の物体２１と他方の物体２２との間に作用する力を計測するが、力センサ５１を使用せず、他の物理的情報を検出して、一方の物体２１と他方の物体２２との間に作用する力を計測するようにしてもよい。

【0056】

力計測部３１は、力センサ５１によって検出される力データに基づいて、一方の物体２１と他方の物体２２との間に作用する力を計測する。また力計測部３１は、一方の物体２１と他方の物体２２との間に作用する力を計測するために、駆動部５０の先端部分に取りつけられた力センサ５１を使用しなくともよく、その場合は、駆動部５０を構成する軸を移動させるアクチュエータがモータである場合は電流値、または、駆動部５０を構成する制御軸の指令位置と実際の制御軸の位置との間の偏差、または、駆動部５０の各制御軸に取付けられたトルクセンサの出力などをもとに、一方の物体２１と他方の物体２２との間に作用する力を推定して算出することによって、計測するようにしてもよい。

【0057】

また、力計測部３１は、駆動部５０、一方の物体２１、他方の物体２２に作用する力を計測するようにしてもよい。このとき、駆動部５０に取りつけた力センサ５１、または、駆動部５０を構成する軸を移動させるアクチュエータがモータである場合は電流値、または、駆動部５０を構成する制御軸の指令位置と実際の制御軸の位置との間の偏差、または、駆動部５０の各制御軸に取付けられたトルクセンサの出力などをもとに、駆動部５０（駆動部５０を構成する機構部、駆動部５０の先端部分などを含む）や、駆動部５０が保持して移動させる物体に作用する力を計測するようにする。また、駆動部５０が移動させない物体である他方の物体２２については、他方の物体２２の下に設置した力センサを用いたり、別の駆動部５０’が他方の物体２２を保持している場合、駆動部５０の場合と同様に計測したりしてもよい。

【0058】

物体の姿勢算出部３２は、駆動部５０によって一方の物体２１と他方の物体２２とを相対的に移動させて一方の物体２１と他方の物体２２とを接触させ、一方の物体２１と他方の物体２２とが３つの接触点で接触するときに力計測部３１によって計測される力、および接触点と凸部中心軸との最短距離に基づいて、一方の物体２１と他方の物体２２との相対的な姿勢を算出する、または一方の物体２１と他方の物体２２との相対的な位置および姿勢を算出する。

【0059】

次に図１０から図１７を用いて、一方の物体２１と他方の物体２２との相対的な姿勢または相対的な位置および姿勢を算出する方法の一実施例について説明する。

【0060】

図１０（ａ）には、一方の物体２１、他方の物体２２の形状などに関する情報が図示されている。図１０（ｂ）には、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触している状態を、前記接触している３つの接触点を含む面に直交し、かつ、一方の物体２１の凸部中心軸を含む平面を断面として見た様子が図示されている。

【0061】

図１１には、一方の物体２１と他方の物体２２とを３つの接触点で接触させた状況が図示されている。図１１（ａ）には、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触している状態が図示されている。図１１（ｂ）には、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触している状態を、前記接触している３つの接触点を含む面に直交し、かつ、一方の物体２１の凸部中心軸を含む平面を断面として見た様子が図示されている。図１１（ｃ）には、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触している状態を、パラメータの説明のために、仮想的に一方の物体２１の凸部と他方の物体２２の凹部とを離した様子が図示されている。

【0062】

図１２（ａ）には、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触している状態が図示されている。図１２（ｂ）には、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触している状態を、前記接触している３つの接触点を含む面に直交し、かつ、一方の物体２１の凸部中心軸を含む平面を断面として見た様子が図示されている。

【0063】

図１３には、一方の物体２１と他方の物体２２とが２点で接触している状況が図示されている。図１４には、一方の物体２１に設定する姿勢算出用の力計測座標系の設定の様子が図示されている。図１５には、一方の物体２１と他方の物体２２とを３つの接触点で接触させるときの動作の状況が図示されている。図１６には、一方の物体２１に設定する姿勢算出用の力計測座標系の設定の様子が図示されている。図１７には、他方の物体２２に包含される複数の凹部を利用して、一方の物体２１に対する他方の物体２２の位置および姿勢を算出する説明に関する図が記載されている。

【0064】

本実施例では、説明の簡単化のために、図１０（ａ）に示すように、一方の物体２１に凸部が包含されていて、前記凸部が円柱形状である物体とする。また、他方の物体２２に凹部が包含されていて、前記凹部は前記凸部の円柱形状と嵌合が可能である穴とする。一方の物体２１の凸部と、他方の物体２２の凹部とは、相対的な姿勢が所定の値以上であるとき、前述のように、３つの接触点において接触させることが可能な物体とする。

【0065】

図１０（ａ）に示されるように、一方の物体２１の凸部の円柱形状の半径をｒとし、直径をｄとする。他方の物体２２の凹部の穴の半径をＲとし、直径をＤとする。一方の物体２１の凸部の円柱は、凸部と凹部が前述のように３つの接触点で接触することができるように、円柱の中心軸方向に所定の長さを有しているものとする。一方の物体２１の凸部の円柱形状の先端の中心点を点Ｏａとする。一方の物体２１の凸部に対する軸として円柱形状の中心軸に凸部中心軸Ｌａが設定されている。なお、一方の物体２１の凸部は、凸部中心軸Ｌａについて軸対称となっている。他方の物体２２の凹部の穴の端部の中心点を点Ｏｂと表す。

【0066】

図１１（ａ）に一方の物体２１の凸部と他方の物体２２の凹部が前述のように３つの接触点で接触している様子が図示されている。図１１（ａ）に図示されるように、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触するとき、凸部に対する軸である凸部中心軸Ｌａは、３つの接触点を含む平面と直交しない。さらに、他方の物体２２の凹部における３つの接触点は凹部の穴の端部に位置する。さらに、３つの接触点と凸部中心軸との各最短距離が所定の値（一定値）であるか、または３つの接触点から凸部中心軸に対して垂線を引いたときの凸部中心軸との交点と凸部中心軸上の所定の点（該交点以外の点、ここでは点Ｏａ）との距離に基づいて接触点と凸部中心軸との各最短距離が定まる（但し凸部が円柱形状の場合、前記交点と前記交点以外の点との距離が異なっても、前記各最短距離は一定値となる）。

【0067】

一方の物体２１の凸部と他方の物体２２の凹部とが前述のように３つの接触点で接触している場合の３点を、点Ｐｅ１、点Ｐｅ２、点Ｐｓとする。他方の物体２２の凹部に対して、点Ｐｅ１、点Ｐｅ２、点Ｐｓの３つの接触点は、他方の物体２２の凹部の穴の端部であって一方の物体２１側の端部に存在し、中心点Ｏｂを中心とする円周上に存在する。一方の物体２１の凸部に対して、３つの接触点は、円柱形状の先端部分であって他方の物体２２側の端部である円周上に点Ｐｅ１、点Ｐｅ２の２点が存在し、円柱形状の側面上に点Ｐｓの１点が存在する。一方の物体２１の凸部における、他方の物体２２と接触する３点と、凸部中心軸Ｌａとの最短距離はそれぞれｒとなる。

【0068】

なお、一方の物体２１の凸部の円柱形状の半径ｒ、直径ｄ、他方の物体２２の凹部の穴の半径Ｒ、直径Ｄは、既知とするが、一方の物体２１および他方の物体２２の内、いずれかの物体について、事前に算出するようにしてもよい。例えば、一方の物体２１を壁、台、別のワークなどの別の物体に対して傾け、Ｚ軸を一方の物体２１の凸部中心軸Ｌａの軸に設定したツール座標系で、Ｚ軸方向であって別の物体に近づく方向に押しつけたときのＺ軸方向の力と、それ以外の軸の軸回りの力を基に算出してもよい。または、壁、台、別のワークなどの、別の物体に接触させたときの位置を基に、一方の物体２１の径を算出してもよい。例えば、まず、固定させた別の物体に対して、一方の物体２１の凸部の円形状の端部を接触させる。次に、一方の物体２１の凸部を、凸部中心軸Ｌａの軸回りに所定の角度だけ回転させて、固定させた別の物体に対して、一方の物体２１の凸部の円形状の端部を接触させる。このとき、一方の物体２１の凸部を、凸部中心軸Ｌａの軸回りに１８０度回転させて接触させる、または、適当な角度で３回以上、接触動作と位置検出を行って、一方の物体２１の凸部における半径ｒや直径ｄを求める処理などを実行する。他方の物体２２の凹部の穴の半径Ｒ、直径Ｄについては、一方の物体２１の凸部における半径ｒや直径ｄとほぼ同じとして求める。また、他方の物体２２の凹部の穴の半径Ｒ、直径Ｄは、駆動部５０に取り付けたピンにＴＣＰ（Ｔｏｏｌ Ｃｅｎｔｅｒ Ｐｏｉｎｔ）、制御点、ツール座標系を設定して、前記ピンで他方の物体２２の凹部の穴の径を予め算出しておくようにしてもよい。

【0069】

駆動部５０が、一方の物体２１を保持して移動させる。このとき、予め、一方の物体２１の凸部に対して、点Ｏａに制御点を設定し、前記制御点を原点とするツール座標系を設定する。ツール座標系を設定しておくことによって、駆動部５０に対する一方の物体２１の凸部の位置および姿勢が定まるようにしておく。ツール座標系は、互いに直交するＸ軸、Ｙ軸、Ｚ軸によって構成される。Ｚ軸は、凸部中心軸と一致するように設定され、＋方向は、他方の物体２２の凹部から離れる方向の成分を含む方向としている。Ｘ軸、Ｙ軸の正負の方向は、一方の物体２１に対する適切な方向を設定しておけばよい。

【0070】

図１１（ｂ）に図示するように、一方の物体２１の凸部と他方の物体２２の凹部が前述のように３つの接触点で接触するとき、３つの接触点を含む面の法線と一方の物体２１の凸部に対して設定される凸部中心軸Ｌａとの成す角度を角度αとする。後述するように、角度αは、他方の物体２２の凹部の穴に対して設定した座標系ＣＦｂのＺ軸の＋方向と、一方の物体２１の凸部に対して設定した姿勢算出用の力計測座標系ＣＦａのＺ軸の＋方向との成す角度となる。

【0071】

ここで、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触するときの、一方の物体２１の凸部の円柱形状の直径ｄと、他方の物体２２の凹部の穴の直径Ｄと、角度αの関係について考える。

【0072】

図１０（ｂ）には、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触している状態を、３つの接触点を含む面に直交し、かつ、一方の物体２１の凸部中心軸Ｌａを含む平面を断面として見た様子が図示されている。

【0073】

図１０（ｂ）に図示されるように、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触するとき、３つの接触点を含む平面、ここでは他方の物体２２の凹部の穴の端部を含む平面に対して、一方の物体２１の凸部を、凸部中心軸Ｌａの方向に射影する。前記３つの接触点を含む平面に射影された部分と、３つの接触点を含む面に直交かつ凸部中心軸Ｌａを含む平面とが交わる部分は、その部分の端を点Ｐｓと端点Ｐｑとするとき、点Ｐｓと端点Ｐｑとを結ぶ直線となる。

【0074】

点Ｐｓと端点Ｐｑとを結ぶ直線の長さは、ｄ／（Ｃｏｓ(α)）となる。一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触する場合、点Ｐｓと端点Ｐｑとを結ぶ直線の長さはＤ以上であり、以下の条件を満たす。
ｄ／（Ｃｏｓ(α)）≧Ｄ （１）
ここで、Ｄ＞ｄである。
式（１）より式（２）が求められる。
α≧Ａｒｃｃｏｓ（ｄ／Ｄ） （２）

【0075】

式（２）より、一方の物体２１の凸部の円柱形状の直径ｄと、他方の物体２２の凹部の穴の直径Ｄの大きさが近くなるほど、角度αの値が小さくなり、相対的な姿勢が小さくても３つの接触点において接触することが分かる。また、式（２）より、例えば、凸部の直径が（３０−０．００５）ｍｍで、凹部の直径が（３０＋０．００５）ｍｍの場合、角度αが１．４８度程度以上であって９０度より小さい値程度までの場合、凸部と凹部は３つの接触点において接触することが分かる。また、凸部の直径が（５０−０．００５）ｍｍで、凹部の直径が（５０＋０．００５）ｍｍの場合、角度αが１．１５度程度以上であって９０度より小さい値程度までの場合、凸部と凹部は３つの接触点において接触することが分かる。

【0076】

なお、凸部と凹部を嵌合させる場合であって、凸部と凹部との相対的な姿勢のずれが大きい場合、通常、一方の物体２１の凸部と他方の物体２２の凹部との間に作用する力に基づく力制御によって嵌合させることや、所定の時間内に嵌合させることは、困難な場合が多い。このような場合は、本発明によって、一方の物体２１の凸部と他方の物体２２の凹部との相対的な姿勢、または相対的な位置および姿勢を算出し、算出した相対的な姿勢、または相対的な位置および姿勢を利用して、例えば、一方の物体２１の凸部と他方の物体２２の凹部とを、前記算出した相対的な姿勢分を補正した位置関係に移動させた後、力制御により嵌合させることなどによって、一方の物体２１の凸部と他方の物体２２の凹部をなるべく少ない失敗回数で、またはなるべく短い時間で嵌合させることが容易となる。

【0077】

一方の物体２１と他方の物体２２とが３つの接触点で接触するときに力計測部３１によって計測される力、および接触点と凸部中心軸との最短距離に基づいて、一方の物体２１と他方の物体２２との相対的な姿勢、または、相対的な位置と相対的な姿勢を算出するときには、姿勢算出用の力計測座標系を用いる。一方の物体２１と他方の物体２２との間に作用する力を姿勢算出用の力計測座標系上の力に変換した力に基づいて、一方の物体２１と他方の物体２２との相対的な姿勢、または、相対的な位置と相対的な姿勢を算出する。図１１（ａ）、図１１（ｃ）に図示するように、一方の物体２１の凸部に対して設定した姿勢算出用の力計測座標系を座標系ＣＦａとする。なお、一方の物体２１の凸部と他方の物体２２の凹部との相対的な位置および姿勢が或る範囲以内にあるとき、または一方の物体２１と他方の物体２２とが接触しているとき、一方の物体２１の凸部に対して設定する姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向を含む方向に所定の目標力で押しつけると共に、Ｚ軸と直交かつ互いに直交する２方向について目標力を０とするような受動的な力制御を実行して、一方の物体２１の凸部を移動させれば、一方の物体２１の凸部と他方の物体２２の凹部とを２つまたは３つの接触点で接触させることが可能であるとする。また、以下では、一方の物体２１の凸部と他方の物体２２の凹部とは、姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向を含む方向に所定の目標力で押しつけると共に、Ｚ軸と直交かつ互いに直交する２方向について目標力を０とするような受動的な力制御を実行して、一方の物体２１の凸部を移動させることによって、一方の物体２１の凸部と他方の物体２２の凹部とを２つまたは３つの接触点で接触させることが可能な状態にあるとする。

【0078】

姿勢算出用の力計測座標系ＣＦａの座標系の設定方法について説明する。姿勢算出用の力計測座標系ＣＦａが次のように設定されたとき、姿勢算出用の力計測座標系ＣＦａの座標系の設定が完了した状態とする。まず、姿勢算出用の力計測座標系ＣＦａは、互いに直交するＸ軸、Ｙ軸、Ｚ軸によって構成される。座標系の原点は、凸部中心軸上であって、一方の物体２１の凸部の円形状の中心点Ｏａに設定する。姿勢算出用の力計測座標系ＣＦａの、Ｘ軸、Ｙ軸、Ｚ軸の設定は、一方の物体２１の凸部と他方の物体２２の凹部が接触するときに以下のような条件を満たすように設定する。Ｚ軸は、凸部中心軸と一致するように設定され、＋方向は、他方の物体２２の凹部から離れる方向の成分を含む方向とする。次に、Ｙ軸は、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触するときの前述の接触点Ｐｅ１と接触点Ｐｅ２とを結んだ直線と平行となるように設定する。Ｘ軸の＋方向は、他方の物体２２の凹部から離れる方向の成分を含む方向となるようにする。

【0079】

姿勢算出用の力計測座標系ＣＦａの設定を、次のようにして完了させる。

【0080】

まず、互いに直交するＸ軸、Ｙ軸、Ｚ軸によって構成される座標系の原点を一方の物体２１の凸部の円形状の中心点Ｏａに設定する。Ｚ軸を凸部中心軸（この場合、円柱形状の中心の軸）と一致するように設定し、＋方向は、他方の物体２２の凹部から離れる方向の成分を含む方向となるように設定する。以降で、Ｘ軸、Ｙ軸について、Ｚ軸の軸回りの方向を定める。なお、Ｘ軸、Ｙ軸のどちらかが定まれば、別の軸の向きも定まる。

【0081】

一方の物体２１の凸部を他方の物体２２の凹部に対して、姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向、または一方の物体２１に設定したツール座標系のＺ軸の−方向、または空間に設定した座標系上での他方の物体２２の凹部に対して近づく方向など、一方の物体２１の凸部と他方の物体２２の凹部が接触する方向に移動させる。一方の物体２１の凸部と他方の物体２２の凹部とが接触した後は、姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向に所定の目標力で押しつけると共に、Ｚ軸と直交かつ互いに直交する２方向について目標力を０とするような受動的な力制御を実行して、一方の物体２１の凸部を移動させることによって、一方の物体２１の凸部と他方の物体２２の凹部とを２つまたは３つの接触点で接触させるようにする。このとき、一方の物体２１が移動しなくなったときに、一方の物体２１の凸部と他方の物体２２の凹部とは、２つまたは３つの接触点で接触したと判定する。

【0082】

ここでは、このように一方の物体２１の凸部を移動させて、一方の物体２１の凸部と他方の物体２２の凹部とを、図１３（ａ）、図１３（ｂ）で図示されるように、点Ｐｅ１と点Ｐｅ２の２点で接触させる場合について説明するが、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触する場合も同様である。図１３（ｂ）では、図１３（ａ）に図示されるように、一方の物体２１の凸部と他方の物体２２の凹部とが２点で接触する状況を、他方の物体２２の凹部の穴の端部に直交し、凸部中心軸Ｌａを含む断面から見た様子を図示している。また、その断面に接触点Ｐｅ１と作用する力Ｆｅを射影している。図１３（ｂ）に図示されるように、一方の物体２１の凸部と他方の物体２２の凹部が２点で接触している状況であり、他方の物体２２の凹部に対して一方の物体２１の凸部を姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向である押付方向Ｐｄ１に所定の目標力で押しつけると共に、Ｚ軸と直交かつ互いに直交する２方向について目標力を０とするような受動的な力制御を実行して、一方の物体２１の凸部を移動させることによって、一方の物体２１の凸部と他方の物体２２の凹部とを２つの接触点で接触させる。なお、この移動によって、３つの接触点で接触することもがあるが、説明については同様である。

【0083】

図１４（ａ）には、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触するとき、一方の物体２１の凸部の先端部分を、姿勢算出用の力計測座標系ＣＦａのＸ軸、Ｙ軸によって構成される平面上で見た態様が図示されている。姿勢算出用の力計測座標系ＣＦａのＸ軸、Ｙ軸によって構成される平面には、接触点Ｐｅ１、接触点Ｐｅ２が存在する。角度βは、姿勢算出用の力計測座標系上のＸ軸とＹ軸とで構成される平面上において、Ｙ軸の−方向と、点Ｏａと一方の物体２１の凸部における接触点Ｐｅ１とを結ぶ線分との成す角度とする。

【0084】

ここで、姿勢算出用の力計測座標系ＣＦａの設定が完了した状態になっていて、姿勢算出用の力計測座標系ＣＦａのＺ軸に対して、姿勢算出用の力計測座標系ＣＦａのＸ軸、Ｙ軸は図１４（ａ）に図示されるような或る姿勢になっているとする。

【0085】

なお、姿勢算出用の力計測座標系ＣＦａのＸ軸方向の力をＦｘ、Ｙ軸方向の力をＦｙ、Ｚ軸方向の力をＦｚ、Ｘ軸の軸回りの力をＭｘ、Ｙ軸の軸回りの力をＭｙ、Ｚ軸の軸回りの力をＭｚとする。前述のように、角度βは、姿勢算出用の力計測座標系上のＸ軸とＹ軸とで構成される平面上において、Ｙ軸の−方向と、点Ｏａと一方の物体２１の凸部における接触点Ｐｅ１とを結ぶ線分との成す角度とする。μは、静止摩擦係数とする。接触点Ｐｅ１には、姿勢算出用の力計測座標系上のＺ軸方向に作用する力であるＦｅと、姿勢算出用の力計測座標系上のＸ軸方向に作用する力であるμ＊Ｆｅが作用する。接触点Ｐｅ２には、姿勢算出用の力計測座標系上のＺ軸方向に作用する力であるＦｅと、姿勢算出用の力計測座標系上のＸ軸方向に作用する力であるμ＊Ｆｅが作用する。接触点Ｐｅ１と接触点Ｐｅ２は、一方の物体２１の凸部および他方の物体２２の凹部において、他方の物体２２の凹部の穴の端部を含む面に直交し、かつ、点Ｏａ、凸部中心軸Ｌａを含む面に対して面対称となる。つまり、姿勢算出用の力計測座標系ＣＦａ上での、接触点Ｐｅ１と接触点Ｐｅ２の位置については、Ｘ軸上、Ｚ軸上の値は同じであり、Ｙ軸上の値は正負が異なる値となる。また、他方の物体２２の凹部に対して設定した座標系ＣＦｂ上での、接触点Ｐｅ１と接触点Ｐｅ２の位置については、Ｘ軸上、Ｚ軸上の値は同じであり、Ｙ軸上の値は正負が異なる値となる。

【0086】

このとき、静止摩擦係数μが０に近似できるとすると、Ｆｘ、Ｆｙ、Ｆｚ、Ｍｘ、Ｍｙ、Ｍｚは、それぞれ以下の式（３）−（８）で表される。
Ｆｘ ＝ ０ （３）
Ｆｙ ＝ ０ （４）
Ｆｚ ＝ ２＊Ｆｅ （５）
Ｍｘ ＝ ０ （６）
Ｍｙ ＝ ２＊Ｆｅ＊ｒ＊Ｓｉｎ（β） （７）
Ｍｚ ＝ ０ （８）

【0087】

次に、別の或る姿勢算出用の力計測座標系ＣＦａについて考える。一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触するとき、Ｚ軸は同じで、上記の座標系を基準の座標系とすると、上記の基準の座標系のＸ軸をＸ’軸、Ｙ軸をＹ’軸とし、Ｚ軸の軸回りにθだけ回転したＸ軸、Ｙ軸で構成される座標系を、前記別の或る座標系とする。つまり、別の或る座標系をＺ軸の軸回りに−θだけ回転させれば、図１４（ａ）に示される基準の座標系と同じ座標系となる。一方の物体２１の凸部と他方の物体２２の凹部との間に作用する力を前記別の或る座標系上に変換した力を考える。

【0088】

図１４（ｂ）には、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触するとき、姿勢算出用の力計測座標系ＣＦａのＸ軸、Ｙ軸によって構成される平面上で見た態様が図示されている。姿勢算出用の力計測座標系ＣＦａのＸ軸、Ｙ軸によって構成される平面には、接触点Ｐｅ１、接触点Ｐｅ２が存在する。角度βは、姿勢算出用の力計測座標系上のＸ軸とＹ軸とで構成される平面上において、Ｙ軸の−方向と、点Ｏａと一方の物体２１の凸部における接触点Ｐｅ１とを結ぶ線分との成す角度とする。また、図１４（ｂ）には、前記基準の座標系のＸ’軸とＹ’軸と、別の或る座標系のＸ軸、Ｙ軸とが図示されている。

【0089】

このとき、一方の物体２１の凸部と他方の物体２２の凹部との間に作用する力を前記別の或る座標系上に変換した力について、静止摩擦係数μが０に近似できるとすると、以下の式（９）−（１４）が成立する。
Ｆｘ ＝ ０ （９）
Ｆｙ ＝ ０ （１０）
Ｆｚ ＝ ２＊Ｆｅ （１１）
Ｍｘ ＝ Ｆｅ＊ｒ＊（Ｓｉｎ（−θ＋π／２−β）＋Ｓｉｎ（−θ−π／２＋β））
（１２）
Ｍｙ ＝ Ｆｅ＊ｒ＊（Ｃｏｓ（−θ＋π／２−β）＋Ｃｏｓ（−θ−π／２＋β））
（１３）
Ｍｚ ＝ ０ （１４）

【0090】

式（１２）、（１３）より、以下の式（１５）によってθが算出される。
θ ＝ Ａｒｃｔａｎ（−Ｍｘ／Ｍｙ） （１５）
また、一方の物体２１の凸部と他方の物体２２の凹部を接触させるために近づけた方向や、Ｚ軸の−方向に他方の物体２２の凹部が存在するという条件から、Ｘ軸の＋方向が他方の物体２２の凹部から離れる方向の成分を含む方向となるように角度θを求める。算出したθに基づいて、前記別の或る基準座標系から、前記基準の座標系が求められる。このようにして式（３）から式（８）を満たす、姿勢算出用の力計測座標系ＣＦａを設定することができる。

【0091】

これによって、Ｙ軸を、点Ｐｅ１と点Ｐｅ２とを結んだ直線と平行となるように設定し、Ｘ軸の＋方向は、他方の物体２２の凹部から離れる方向の成分を含む方向となるようにする座標系を設定することができる。この設定方法は、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触する場合も同様である。以上によって、姿勢算出用の力計測座標系ＣＦａの設定を完了させることができる。

【0092】

また、次のように、姿勢算出用の力計測座標系ＣＦａの設定を完了させるようにしてもよい。

【0093】

姿勢算出用の力計測座標系ＣＦａの原点とＺ軸を設定し、一方の物体２１の凸部と他方の物体２２の凹部とを２つまたは３つの接触点で接触させる方法は前述と同様とする。これによって、一方の物体２１の凸部と他方の物体２２の凹部とは、図１３に示されるように、前述のように、２つまたは３つの接触点で接触したとする。以降で、Ｘ軸、Ｙ軸について、Ｚ軸の軸回りの方向を定める。なお、Ｘ軸、Ｙ軸のどちらかが定まれば、別の軸の向きも定まる。

【0094】

図１６（ａ）には、一方の物体２１の凸部と他方の物体２２の凹部とが２点で接触する状況を、他方の物体２２の凹部の穴の端部に直交し、凸部中心軸Ｌａを含む断面から見た様子を図示している。ここでは２点の接触の場合について説明するが、３点の場合でも同様である。図１６（ａ）に図示されるように、一方の物体２１の凸部と他方の物体２２の凹部とが接触した後、Ｚ軸の−方向である押付方向Ｐｄ１に、他方の物体２２の凹部に対して一方の物体２１の凸部を所定の目標力で押しつけ、Ｚ軸と直交かつ互いに直交する２方向について目標力を０とするような受動的な力制御を実行している。

【0095】

このときに、押付方向Ｐｄ１と直交する、次に述べる方向について、現在の位置から、所定の距離を上限として移動させるときに、力がかかる場合、所定の目標力で押しつけるように力制御して所定時間だけ経過したら停止させる。このとき、停止させるまでの移動量を記録しておく。その後、移動させる前の位置に戻すように移動させる。このような力制御とそのときの移動量を検出する動作を実行する。このときの移動方向であって押付方向とする方向は、点Ｏａを含み、Ｚ軸と直交する平面内における、点Ｏａを基準とした正方向のベクトルと、そのベクトルに対してＺ軸について軸対称な、点Ｏａを基準とした逆方向のベクトルに定められる方向とする。この或る方向とその逆方向への往復動作を１回以上させながら、前記移動量を検出する動作をＺ軸の軸回りに方向を変えながら実行する。例えば、正方向の＋Ｍｖ１方向と、逆方向の−Ｍｖ１方向の前記移動量を検出する往復動作を１回以上、繰り返し、次に、＋Ｍｖ２方向と、−Ｍｖ２方向の前記移動量を検出する往復動作を１回以上、繰り返すといったことを、複数の方向である、＋Ｍｖ１方向と−Ｍｖ１方向、＋Ｍｖ２方向と−Ｍｖ２方向、＋Ｍｖ３方向と−Ｍｖ３方向、＋Ｍｖ４方向と−Ｍｖ４方向、について繰り返す。

【0096】

このように、一方の物体２１の凸部を移動させようとしたときに、一方の物体２１の凸部の位置が移動しない、または所定の値以下である、または最小となるときの、正方向および逆方向をＹ軸上の方向と判断するという、Ｙ軸の探索動作を実行する。このような方向の探索方法などによって、一方の物体２１の凸部の先端部分の２点の接触点を結ぶ線分と平行な方向を探索して検出する。なお、前記のような方法とは異なる探索方法であって一方の物体２１の凸部の先端部分の２点の接触点を結ぶ線分と平行な方向が探索可能な方法を用いてもよい。一方の物体２１の凸部と他方の物体２２の凹部を接触させるために近づけた方向や、Ｚ軸の−方向に他方の物体２２の凹部が存在するという条件から、Ｘ軸の＋方向が他方の物体２２の凹部から離れる方向の成分を含む方向となるように設定することによって、Ｙ軸の正負の方向も定まる。または、一度、Ｘ軸、Ｙ軸を設定した後、Ｘ軸の−方向に移動させるときに所定の値を超える力を検出したら停止させるように移動させるときに、Ｙ軸の軸回りのモーメントが大きくなる場合には設定が正しく、Ｙ軸の軸回りのモーメントが小さくなる場合には、Ｘ軸、Ｙ軸の正負が逆であると判定して、Ｘ軸、Ｙ軸の正負の方向を変えるようにしてもよい。

【0097】

また、姿勢算出用の力計測座標系ＣＦａの設定を、次のようにして、完了させた状態にするとしてもよい。

【0098】

一方の物体２１の凸部と他方の物体２２の凹部とが接触するときに、一方の物体２１に対する姿勢算出用の力計測座標系ＣＦａの設定が完了した状態となるように、予め、一方の物体２１の凸部に対して姿勢算出用の力計測座標系ＣＦａを設定しておき、一方の物体２１の凸部を他方の物体２２の凹部に移動させる。この場合、一方の物体２１の凸部と他方の物体２２の凹部とが接触する３つの接触点を含む平面と直交し、かつ、凸部中心軸Ｌａを含む平面が既知の平面となるように、一方の物体２１の凸部と他方の物体２２の凹部とを接触させるようにする。前述のように、一方の物体２１に対する姿勢算出用の力計測座標系ＣＦａの設定が予め完了している場合、一方の物体２１の凸部を姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向に所定の目標力で押しつけると共に、姿勢算出用の力計測座標系ＣＦａのＸ軸の−方向に所定の目標力で押しつけるようにし、Ｙ軸方向については目標力を０とするような受動的な力制御を実行して、Ｚ軸、Ｘ軸方向について、所定の目標力で押しつけ、一方の物体２１が移動しなくなったとき、一方の物体２１の凸部と他方の物体２２の凹部とは３つの接触点で接触したと判定する。なお、所定の目標力は、各方向について異なる大きさであってもよい。また、一方の物体２１の凸部を姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向に所定の目標力で押しつけると共に、Ｚ軸と直交かつ互いに直交する２方向について目標力を０とするような受動的な力制御を実行して、一方の物体２１の凸部がＺ軸方向について所定の目標力で押しつけるようになり移動しなくなった後、一方の物体２１の凸部を姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向に所定の目標力で押しつけると共に、姿勢算出用の力計測座標系ＣＦａのＸ軸の−方向に所定の目標力で押しつけるようにし、Ｙ軸方向については目標力を０とするような受動的な力制御を実行して、一方の物体２１の凸部をＺ軸、Ｘ軸方向について、所定の目標力で押しつけるようにして、移動させるようにしてもよい。

【0099】

前述のように姿勢算出用の力計測座標系ＣＦａを設定するために、一方の物体２１の凸部と他方の物体２２の凹部との姿勢が拘束されるように、別の機構部などの物体を取りつけたり、一方の物体２１の凸部の姿勢に関する移動動作を制限するようにしたりするなどしてもよい。このようにして、一方の物体２１の凸部と他方の物体２２の凹部とが相対的に姿勢算出用の力計測座標系ＣＦａのＸ軸とＺ軸を含む平面と、平行な平面上に拘束されるようにしておく。一方の物体２１の凸部と他方の物体２２の凹部に対して前記のような姿勢に関する条件が求められるとしても、そのような条件が成立する機構を保有する装置や機構部、またそのような条件の下で駆動部５０が一方の物体２１を他方の物体に対して移動させる場合などでは、一方の物体２１の凸部に対する姿勢算出用の力計測座標系ＣＦａのＺ軸の軸回りの姿勢を推定する必要がなく、必要最低限の方法によって、一方の物体２１の凸部と他方の物体２２の凹部との相対的な位置および姿勢をより正確に算出することができるようになる。このように、一方の物体２１と他方の物体２２とが、同一平面内にあって相対的な角度が変わる場合に、一方の物体２１と他方の物体２２との間に作用する力に基づいて、一方の物体２１と他方の物体２２との相対的な姿勢を算出することができるので、別の作業や状態監視などのために力センサを備えている場合に、他のセンサなどを利用することなく、一方の物体２１と他方の物体２２との相対的な姿勢を算出することができる。

【0100】

以上のようにして、姿勢算出用の力計測座標系ＣＦａの設定を完了させることによって、空間に対して設定した座標系や、駆動部５０に設定される座標系、ツール座標系に対する、一方の物体２１に対して設定する姿勢算出用の力計測座標系ＣＦａの同次変換行列や位置および姿勢が設定される。

【0101】

次に、姿勢算出用の力計測座標系ＣＦａの設定が完了した状態で、または姿勢算出用の力計測座標系ＣＦａの設定を完了させるために、一方の物体２１の凸部を姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向に所定の目標力で押しつけて、一方の物体２１の凸部と他方の物体２２の凹部とを３点または３点で接触させる前の２点で接触させるときの移動について考える。なお、一方の物体２１の凸部と他方の物体２２の凹部との相対的な姿勢が、３つの接触点で接触させることが可能な相対的な姿勢であるとき、一方の物体２１の凸部と他方の物体２２の凹部とは２つの接触点で接触させることが可能である。一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触する条件についての前述の説明のように、一方の物体２１の凸部と他方の物体２２の凹部との相対的な姿勢である角度αが所定の値より大きければ、一方の物体２１の凸部と他方の物体２２の凹部とは、３つの接触点で接触させることができる。

【0102】

一方の物体２１の凸部と、他方の物体２２の凹部との相対的な姿勢が大きく、３つの接触点で接触させることが可能であることが分かっている場合、姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向、または空間に設定した座標系上での他方の物体２２の凹部に対して近づく方向など、一方の物体２１の凸部と他方の物体２２の凹部とが接触する方向に、一方の物体２１の凸部を移動させる。一方の物体２１の凸部と他方の物体２２の凹部とが接触した後は、一方の物体２１の凸部を姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向に所定の目標力で押しつけると共に、Ｚ軸と直交かつ互いに直交する２方向について目標力を０とするような受動的な力制御を実行して、一方の物体２１の凸部を移動させることによって、一方の物体２１の凸部と他方の物体２２の凹部とを２つまたは３つの接触点で接触させることができる。このとき、一方の物体２１が移動しなくなったときに、一方の物体２１の凸部と他方の物体２２の凹部とは、２つまたは３つの接触点で接触したと判定する。

【0103】

一方の物体２１の凸部と、他方の物体２２の凹部との相対的な姿勢が、３つの接触点で接触させることが可能な姿勢となっているか否かが分からない場合、一方の物体２１の凸部と他方の物体２２の凹部との相対的な姿勢を所定の値だけ変わるように移動させる。この場合、相対的な姿勢が変わる方向、相対的な姿勢がどの程度の範囲以内か、などの情報を予め持っておき、それらに基づいて、所定の値だけ姿勢を変えることによって、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触することが可能な相対的な姿勢となるようにする（このときの姿勢の移動量を記録しておき、後で一方の物体２１の凸部と他方の物体２２の凹部との相対的な位置および姿勢を算出した後、その移動量分を補正して、前記姿勢の移動をさせる前の、一方の物体２１の凸部と他方の物体２２の凹部との相対的な姿勢を算出する）。その後、姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向、または空間に設定した座標系上での他方の物体２２の凹部に対して近づく方向など、一方の物体２１の凸部と他方の物体２２の凹部が接触する方向に、一方の物体２１の凸部を移動させる。一方の物体２１の凸部と他方の物体２２の凹部とが接触した後は、一方の物体２１の凸部を姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向に所定の目標力で押しつけると共に、Ｚ軸と直交かつ互いに直交する２方向について目標力を０とするような受動的な力制御を実行して、一方の物体２１の凸部を移動させることによって、一方の物体２１の凸部と他方の物体２２の凹部とを２つまたは３つの接触点で接触させることができる。このとき、一方の物体２１が移動しなくなったときに、一方の物体２１の凸部と他方の物体２２の凹部とは、２つまたは３つの接触点で接触したと判定する。

【0104】

一方の物体２１の凸部と、他方の物体２２の凹部との相対的な姿勢の範囲が小さく、３つの接触点で接触させることができない姿勢である場合、一方の物体２１の凸部と他方の物体２２の凹部との相対的な姿勢を、一方の物体２１の凸部と他方の物体２２の凹部とを３つの接触点で接触できる相対的な姿勢とするように、所定の角度だけ姿勢が変わるように移動させる（このときの姿勢の移動量を記録しておき、後で一方の物体２１の凸部と他方の物体２２の凹部との相対的な位置および姿勢を算出した後、その移動させた分を補正して、前記姿勢の移動をさせる前の、一方の物体２１の凸部と他方の物体２２の凹部との相対的な姿勢を算出する）。その後、姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向、または空間に設定した座標系上での他方の物体２２の凹部に対して近づく方向など、一方の物体２１の凸部と他方の物体２２の凹部が接触する方向に、一方の物体２１の凸部を移動させる。一方の物体２１の凸部と他方の物体２２の凹部とが接触した後は、姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向に所定の目標力で押しつけると共に、Ｚ軸と直交かつ互いに直交する２方向について目標力を０とするような受動的な力制御を実行して、一方の物体２１の凸部を移動させることによって、一方の物体２１の凸部と他方の物体２２の凹部とを２つまたは３つの接触点で接触させることができる。このとき、一方の物体２１が移動しなくなったときに、一方の物体２１の凸部と他方の物体２２の凹部とは、２つまたは３つの接触点で接触したと判定する。

【0105】

また、上記で、一方の物体２１と他方の物体２２とが、３つの接触点で接触させることが可能な姿勢となっているかは、一方の物体２１の凸部と他方の物体２２の凹部とを接触させたときのモーメントの大きさに基づいて判断してもよい。

【0106】

このように、一方の物体２１の凸部と他方の物体２２の凹部とを３つの接触点で接触させる状態にすることができる、相対的な姿勢とするように、一方の物体２１および他方の物体２２の少なくとも１つを所定の値以上だけ姿勢を変えて、そのときの姿勢の移動量を記録しておき、一方の物体２１と他方の物体２２との相対的な姿勢を算出した後、前記記録しておいた移動量分を補正して、一方の物体２１と他方の物体２２との、初めの相対的な姿勢を算出するようにしてもよい。

【0107】

一方の物体２１の凸部に対する、姿勢算出用の力計測座標系ＣＦａの設定が完了した後、一方の物体２１を移動させ、一方の物体２１の凸部と他方の物体２２の凹部とを、３つの接触点で接触させる。

【0108】

図１５（ａ）、図１５（ｂ）には、一方の物体２１の凸部と他方の物体２２の凹部とを接触させる状況を、他方の物体２２の凹部の穴の端部を含む平面に直交し、かつ、凸部中心軸Ｌａを含む平面を断面として見た様子が図示されている。また、姿勢算出用の力計測座標系ＣＦａのＹ軸が紙面に対して直交している。

【0109】

姿勢算出用の力計測座標系ＣＦａの設定が完了した後、Ｚ軸の−方向である押付方向Ｐｄ１と、Ｘ軸の−方向である押付方向Ｐｄ２について、所定の目標力で押しつけるようにし、Ｙ軸方向については、目標力を０とするような受動的な力制御を実行して、一方の物体２１の凸部を移動させ、押付方向Ｐｄ１、押付方向Ｐｄ２について、所定の目標力で押しつけ、一方の物体２１が移動しなくなったとき、一方の物体２１の凸部と他方の物体２２の凹部とは３つの接触点で接触したと判断する。図１５（ａ）には、前述のように、一方の物体２１の凸部と他方の物体２２の凹部とを２つの接触点で接触させ、姿勢算出用の力計測座標系ＣＦａの設定が完了させた時の一方の物体２１の凸部の位置が一方の物体２１ａと示されている。また、図１５（ａ）には、その後、前述のように移動させ、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触した時の様子が図示されている。

【0110】

また、一方の物体２１の凸部に対する、姿勢算出用の力計測座標系ＣＦａの設定が完了した後、一方の物体２１の凸部と他方の物体２２の凹部とを、別の移動方法によって移動させ、３つの接触点で接触させるようにしてもよい。一方の物体２１の凸部を姿勢算出用の力計測座標系ＣＦａのＺ軸の−方向である押付方向Ｐｄ１に所定の目標力で押しつけると共に、Ｚ軸と直交かつ互いに直交する２方向について目標力を０とするような受動的な力制御を実行して、一方の物体２１の凸部を移動させることによって、一方の物体２１の凸部と他方の物体２２の凹部とを２つまたは３つの接触点で接触させる。このとき、押付方向Ｐｄ１に所定の目標力で押しつけ、一方の物体２１が移動しなくなったとき、一方の物体２１の凸部と他方の物体２２の凹部とは２つまたは３つの接触点で接触したと判断する。前述のように、姿勢算出用の力計測座標系ＣＦａの設定が完了した後、一方の物体２１の凸部をＺ軸の−方向である押付方向Ｐｄ１について、所定の目標力で押しつけながら、Ｚ軸と直交かつ互いに直交する２方向について目標力を０とするような受動的な力制御を実行して、Ｙ軸の軸回りについて−方向であるＲＹ方向に、Ｙ軸を中心として回転させていき、Ｙ軸の軸回りの力が所定の目標力となる、または接触を検出したときに、３つの接触点で接触したと判定し、回転移動を停止させる。これによって、一方の物体２１の凸部と他方の物体２２の凹部とを３つの接触点で接触させることができる。図１５（ｂ）には、姿勢算出用の力計測座標系ＣＦａの設定が完了した時の一方の物体２１の凸部の位置が一方の物体２１ａと示され、押付方向Ｐｄ１、Ｙ軸の軸回りの方向ＲＹに移動させ、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触した時の様子が図示されている。このときＹ軸の軸回りに回転させた角度を記録しておき、凸部中心軸Ｌａと他方の物体２２の凹部に設定した座標系ＣＦｂのＺ軸との相対角度を算出した後、前記記録した角度分を補正することによって、Ｙ軸の軸回りに回転させる前の一方の物体２１の凸部と他方の物体２２の凹部について、凸部中心軸Ｌａと他方の物体２２の凹部に設定した座標系ＣＦｂのＺ軸との相対角度を算出することができる。

【0111】

なお、図５（ｂ）、図５（ｃ）、図５（ｄ）、図５（ｅ）に示されるように、凸部の先端部分が円の欠けた形状などである場合、凸部と凹部とを前述のように２つの接触点や３つの接触点で接触させることが可能なように、凸部と凹部とを接触させる前に、或る所定の範囲の位置および姿勢に移動させておくことが好ましい。

【0112】

前述のように、姿勢算出用の力計測座標系ＣＦａの設定を完了させた状態にして、その設定を基に一方の物体２１の凸部と他方の物体２２と凹部とを３つの接触点で接触させる。これによって、一方の物体２１の凸部と他方の物体２２の凹部の穴部の端部とを３つの接触点で接触させ、設定された姿勢算出用の力計測座標系ＣＦａと３つの接触点との位置および姿勢の関係を図１１に示すような状況とすることができる。

【0113】

一方の物体２１の凸部と他方の物体２２の凹部とを３つの接触点で接触させた状態について考える。

【0114】

一方の物体２１の凸部と他方の物体２２の凹部とが接触点Ｐｅ１、接触点Ｐｅ２、接触点Ｐｓで接触するとき、３つの接触点と、凸部中心軸Ｌａでもある、姿勢算出用の力計測座標系ＣＦａのＺ軸との各最短距離は、一定値であるｒとなる（本実施例では、接触点と、姿勢算出用の力計測座標系ＣＦａのＺ軸との各最短距離は一定値とするが、接触点と、姿勢算出用の力計測座標系ＣＦａのＺ軸との各最短距離は姿勢算出用の力計測座標系ＣＦａのＺ軸方向の長さに基づいて定まるようにしてもよい。その場合においても、一方の物体２１と他方の物体２２との相対的な姿勢、または相対的な位置および姿勢の算出方法は、本実施例における説明と同様である）。また、凸部中心軸Ｌａでもある、姿勢算出用の力計測座標系ＣＦａのＺ軸は、３つの接触点を含む平面と直交しない。また、３つの接触点は、他方の物体２２の凹部の穴の端部に位置する。また、姿勢算出用の力計測座標系ＣＦａのＸ軸、Ｙ軸、Ｚ軸と、３つの接触点との位置および姿勢の関係は、次のように設定される。姿勢算出用の力計測座標系ＣＦａのＹ軸は、接触点Ｐｅ１と接触点Ｐｅ２とを結ぶ直線と平行、かつ、接触点Ｐｅ１から接触点Ｐｅ２への方向と平行な方向が＋方向となる。また、Ｚ軸の−方向、Ｘ軸の−方向は、他方の物体２２の凹部に近づく方向を成分に含む。また、姿勢算出用の力計測座標系ＣＦａのＺ軸とＸ軸とを含む平面は、３つの接触点を含む平面と直交する。

【0115】

以降では、前述のように定められた姿勢算出用の力計測座標系ＣＦａと、３つの接触点と、一方の物体２１の凸部と他方の物体２２の凹部との間に作用する力を姿勢算出用の力計測座標系ＣＦａ上の力に変換した力を用いて、姿勢算出用の力計測座標系ＣＦａと、他方の物体２２の凹部の穴の端部に対して設定する座標系との位置および姿勢の関係を算出する。

【0116】

一方の物体２１の凸部に対して設定した姿勢算出用の力計測座標系ＣＦａに対する、他方の物体２２の凹部の位置および姿勢を算出するために、図１１（ｂ）、図１１（ｃ）に図示するように、他方の物体２２の凹部の穴に対して座標系ＣＦｂを設定する。座標系ＣＦｂは互いに直交するＸ軸、Ｙ軸、Ｚ軸によって構成される。座標系ＣＦｂの原点は、他方の物体２２の凹部の穴の端部の中心点Ｏｂとする。点Ｏｂは、一方の物体２１の凸部と他方の物体２２の凹部とが接触する、接触点Ｐｅ１、接触点Ｐｅ２、接触点Ｐｓの３つの接触点の中心点となる。３つの接触点を含む平面と直交する軸を座標系ＣＦｂのＺ軸とする。Ｚ軸の＋方向は、他方の物体２２の凹部の穴から離れる方向の成分を含む方向とする。一方の物体２１の凸部と他方の物体２２の凹部が３つの接触点で接触するときの姿勢算出用の力計測座標系ＣＦａのＹ軸と平行であり、かつ、＋方向が同じ向きとなるように、座標系ＣＦｂのＹ軸を設定する。座標系ＣＦｂのＸ軸は、座標系ＣＦｂのＺ軸と、座標系ＣＦｂのＹ軸とから一意に定まる。

【0117】

図１１（ａ）、図１１（ｂ）、図１１（ｃ）を参照しながら、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触する場合に、一方の物体２１の凸部と他方の物体２２の凹部との間に作用する力を、姿勢算出用の力計測座標系ＣＦａ上の力で表す。つまり、姿勢算出用の力計測座標系ＣＦａの原点に対して或る力が作用するとして、或る力を姿勢算出用の力計測座標系ＣＦａ上で表す。

【0118】

ここで、角度αは、前述のように、図１１（ｂ）に図示されるように、一方の物体２１の凸部と他方の物体２２の凹部が３つの接触点で接触するとき、３つの接触点を含む面の法線と一方の物体２１の凸部に対して設定される凸部中心軸Ｌａとの成す角度であって、他方の物体２２の凹部の穴に対して設定した座標系ＣＦｂのＺ軸の＋方向と、姿勢算出用の力計測座標系ＣＦａのＺ軸の＋方向との成す角度とする。μは、静止摩擦係数とする。また、図１１（ａ）に示されるように、接触点Ｐｅ１には、姿勢算出用の力計測座標系上のＺ軸の正方向に作用する力であるＦｅと、姿勢算出用の力計測座標系上のＸ軸の正方向に作用する力であるμ＊Ｆｅが作用する。接触点Ｐｅ２には、姿勢算出用の力計測座標系上のＺ軸の正方向に作用する力であるＦｅと、姿勢算出用の力計測座標系上のＸ軸の正方向に作用する力であるμ＊Ｆｅが作用する。接触点Ｐｓには、姿勢算出用の力計測座標系上のＸ軸の正方向に作用する力であるＦｓと、姿勢算出用の力計測座標系上のＺ軸の正方向に作用する力であるμ＊Ｆｅが作用する。

【0119】

また、ｈは、接触点Ｐｓから凸部中心軸Ｌａに垂線を下ろしたときの交点を点Ｐｔとするとき、中心点Ｏａから点Ｐｔまでの距離であり、姿勢算出用の力計測座標系ＣＦａのＺ軸上の点Ｐｔの位置とする。角度βは、姿勢算出用の力計測座標系ＣＦａのＸ軸とＹ軸とで構成される平面上において、Ｙ軸の−方向と、点Ｏａと接触点Ｐｅ１とを結ぶ線分との成す角度とする。角度β’は、図１１（ｃ）に図示されるように、他方の物体２２の凹部に対して設定した座標系ＣＦｂのＸ軸とＹ軸とで構成される平面上において、Ｙ軸の−方向と、点Ｏｂと他方の物体２２の凹部における接触点Ｐｅ１とを結ぶ線分との成す角度とする。なお、図１１（ｃ）では、角度β’の説明のために、一方の物体２１の凸部と他方の物体２２の凹部とを仮想的に離して図示しており、一方の物体２１の凸部と他方の物体２２の凹部とは接触点Ｐｅ１、接触点Ｐｅ２、接触点Ｐｓにおいて接触している。

【0120】

図１１（ａ）に図示されるように、接触点Ｐｅ１と接触点Ｐｅ２は、一方の物体２１の凸部および他方の物体２２の凹部において、一方の物体２１の凸部と他方の物体２２の凹部とが接触している３つの接触点を含む面に直交する面であって、かつ、凸部中心軸Ｌａを含む面に対して、面対称となる。つまり、姿勢算出用の力計測座標系ＣＦａ上での、接触点Ｐｅ１と接触点Ｐｅ２の位置については、Ｘ軸上、Ｚ軸上の値は同じであり、Ｙ軸上の値は大きさが同じであり正負が異なる値となる。また、他方の物体２２の凹部に対して設定した座標系ＣＦｂ上での、接触点Ｐｅ１と接触点Ｐｅ２の位置については、Ｘ軸上、Ｚ軸上の値は同じでありＹ軸上の値は大きさが同じであり正負が異なる値となる。

【0121】

一方の物体２１の凸部と他方の物体２２の凹部との間に作用する力を、姿勢算出用の力計測座標系ＣＦａ上の力、つまり、姿勢算出用の力計測座標系ＣＦａの原点に作用する力に変換し、姿勢算出用の力計測座標系ＣＦａ上のＸ軸方向の力をＦｘ、Ｙ軸方向の力をＦｙ、Ｚ軸方向の力をＦｚ、Ｘ軸の軸回りの力をＭｘ、Ｙ軸の軸回りの力をＭｙ、Ｚ軸の軸回りの力をＭｚとする。Ｆｘ、Ｆｙ、Ｆｚ、Ｍｘ、Ｍｙ、Ｍｚは、それぞれ以下の式（１６）−（２１）によって表される。

【0122】

Ｆｘ ＝ Ｆｓ＋２＊μ＊Ｆｅ （１６）
Ｆｚ ＝ μ＊Ｆｓ＋２＊Ｆｅ （１７）
Ｍｙ ＝ Ｆｓ＊ｈ＋μ＊Ｆｓ＊ｒ＋２＊Ｆｅ＊ｒ＊Ｓｉｎ（β）
（１８）
Ｆｙ ＝ ０ （１９）
Ｍｘ ＝ ０ （２０）
Ｍｚ ＝ ０ （２１）

【0123】

ここで、μが既知である場合、以下の式（２２）−（２４）が成り立つ。
Ｆｅ ＝ （Ｆｚ−μ＊Ｆｘ）／（２＊（１−μ＊μ）） （２２）
Ｆｓ ＝ （Ｆｘ−μ＊Ｆｚ）／（１−μ＊μ） （２３）
（ｒ＊Ｆｚ−Ｍｙ）＊μ＊μ＋
｛ｈ＊Ｆｚ＋［Ｓｉｎ（β）−１］＊ｒ＊Ｆｘ｝＊μ＋
［Ｍｙ−ｈ＊Ｆｘ−Ｓｉｎ（β）＊ｒ＊Ｆｚ］ ＝ ０
（２４）
ここで、μ≒０（μが０と近似できるくらい小さい）である場合、Ｍｙは以下の式（２５）で近似することができる。
Ｍｙ ≒ ｈ＊Ｆｘ＋Ｓｉｎ（β）＊ｒ＊Ｆｚ （２５）

【0124】

姿勢算出用の力計測座標系ＣＦａと、一方の物体２１の凸部と他方の物体２２の凹部とが接触する接触点Ｐｅ１、接触点Ｐｅ２、接触点Ｐｓの３つの接触点と、他方の物体２２の凹部に設定した座標系ＣＦｂの、位置および姿勢の関係について考える。

【0125】

姿勢算出用の力計測座標系ＣＦａに対する、接触点Ｐｅ１、接触点Ｐｅ２、接触点Ｐｓの位置は、それぞれ以下の式（２６）、（２８）、（３０）のように表され、他方の物体２２の凹部に対して設定した座標系ＣＦｂに対する、接触点Ｐｅ１、接触点Ｐｅ２、接触点Ｐｓの位置は、それぞれ以下の式（２７）、（２９）、（３１）のように表される。
Ｐ（点Ｐｓ_座標系ＣＦａ） ＝ ［−ｒ、０、ｈ］＾Ｔ （２６）
Ｐ（点Ｐｓ＿座標系ＣＦｂ） ＝ ［−Ｒ、０、０］＾Ｔ （２７）
Ｐ（点Ｐｅ１＿座標系ＣＦａ）
＝ ［−ｒ＊Ｓｉｎ（β）、−ｒ＊Ｃｏｓ（β）、０］＾Ｔ （２８）
Ｐ（点Ｐｅ１＿座標系ＣＦｂ）
＝ ［Ｒ＊Ｓｉｎ（β’）、−Ｒ＊Ｃｏｓ（β’）、０］＾Ｔ （２９）
Ｐ（点Ｐｅ２＿座標系ＣＦａ）
＝ ［−ｒ＊Ｓｉｎ（β）、ｒ＊Ｃｏｓ（β）、０］＾Ｔ （３０）
Ｐ（点Ｐｅ２＿座標系ＣＦｂ）
＝ ［Ｒ＊Ｓｉｎ（β’）、Ｒ＊Ｃｏｓ（β’）、０］＾Ｔ （３１）

【0126】

また、β、β’については、以下の式（３２）で示される条件が成立する。
Ｒ＊Ｃｏｓ（β’） ＝ ｒ＊Ｃｏｓ（β） （３２）

【0127】

他方の物体２２の凹部に対して設定した座標系ＣＦｂから、姿勢算出用の力計測座標系ＣＦａへの同次変換行列Ｔｂは、以下の式（３３）のように表される。
ここで、ΔＰｘは、他方の物体２２の凹部に対して設定した座標系ＣＦｂにおける、Ｘ軸方向の点Ｏａの位置とする。また、ΔＰｚは、他方の物体２２の凹部に対して設定した座標系ＣＦｂにおける、Ｚ軸方向の点Ｏａの位置とする。

【数1】

【0128】

座標系ＣＦｂ上の接触点Ｐｓ、接触点Ｐｅ１、接触点Ｐｅ２の位置は、同時変換行列Ｔｂと、座標系ＣＦａ上の接触点Ｐｓ、接触点Ｐｅ１、接触点Ｐｅ２の位置を用いて、それぞれ以下の式（３４）−（３６）で表される。
［Ｐ（点Ｐｓ＿座標系ＣＦｂ）＾Ｔ、１］＾Ｔ ＝ Ｔｂ・［Ｐ（点Ｐｓ_座標系ＣＦａ）＾Ｔ、１］＾Ｔ （３４）
［Ｐ（点Ｐｅ１＿座標系ＣＦｂ）＾Ｔ、１］＾Ｔ ＝ Ｔｂ・［Ｐ（点Ｐｅ１＿座標系ＣＦａ）＾Ｔ、１］＾Ｔ （３５）
［Ｐ（点Ｐｅ２＿座標系ＣＦｂ）＾Ｔ、１］＾Ｔ ＝ Ｔｂ・［Ｐ（点Ｐｅ２＿座標系ＣＦａ）＾Ｔ、１］＾Ｔ （３６）

【0129】

式（２６）から式（３６）によって、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触しているとき、他方の物体２２の凹部に設定した座標系ＣＦｂ上において、以下の式（３７）から（４１）に表される条件が導かれる。
Ｒ＊Ｓｉｎ（β’）＋ｒ＊Ｓｉｎ（β）＊Ｃｏｓ（α） ＝ ΔＰｘ （３７）
Ｒ＊Ｃｏｓ（β’） ＝ ｒ＊Ｃｏｓ（β） （３８）
ｒ＊Ｓｉｎ（β）＊Ｓｉｎ（α） ＝ ΔＰｚ （３９）
ｒ＊Ｃｏｓ（α）＋ｈ＊Ｓｉｎ（α） ＝ Ｒ＋ΔＰｘ （４０）
−ｒ＊Ｓｉｎ（α）＋ｈ＊Ｃｏｓ（α） ＝ −ΔＰｚ （４１）

【0130】

ここで、Ｒの大きさは、ｒの大きさと同じ程度であり近似できるとする。このとき、βはβ’と近似することができる。
式（３７）から（４１）を用いて、ｈ、β、ΔＰｘ、ΔＰｚが、それぞれ以下の式（４２）−（４５）で算出される。
ｈ ＝ ２＊ｒ＊（１−Ｃｏｓ（α））／Ｓｉｎ（α） （４２）
β ＝ Ａｒｃｓｉｎ（（１−Ｃｏｓ（α））／（１＋Ｃｏｓ（α））） （４３）
ΔＰｘ ＝ ｒ＊（１−Ｃｏｓ（α）） （４４）
ΔＰｚ ＝
ｒ＊Ｓｉｎ（α）＊（１−Ｃｏｓ（α））／（１＋Ｃｏｓ（α）） （４５）

【0131】

式（４２）と式（４３）を、式（２５）に代入することによって、角度αは以下の式（４６）で算出される。

【数2】

また、他方の物体２２の凹部に設定した座標系ＣＦｂ上のＸ軸、Ｙ軸、Ｚ軸上の、姿勢算出用の力計測座標系ＣＦａの原点Ｏａの位置は、以下の式（４７）で算出される。
Ｐ（点Ｏａ_座標系ＣＦｂ） ＝
［ｒ＊（１−Ｃｏｓ（α））、０、
ｒ＊Ｓｉｎ（α）＊（１−Ｃｏｓ（α））／（１＋Ｃｏｓ（α））］＾Ｔ
（４７）

【0132】

以上のように、他方の物体２２の凹部に設定した座標系ＣＦｂに対する、姿勢算出用の力計測座標系ＣＦａの、角度α、位置が算出される。また、他方の物体２２の凹部に設定した座標系ＣＦｂに対する、３つの接触点の位置も算出される。これによって、他方の物体２２の凹部に対する、一方の物体２１の凸部の相対的な位置および姿勢が算出される。

【0133】

ここで、式（４２）によって、一方の物体２１の凸部と他方の物体２２の凹部が３つの接触点で接触するためには、一方の物体２１の凸部の円柱形状は、凸部中心軸Ｌａの方向に、２＊ｒ＊（１−Ｃｏｓ（α））／Ｓｉｎ（α）以上の長さがあればよいことが分かる。また、式（４２）、式（４３）によって、ｈ、βが求まるので、接触点Ｐｅ１、接触点Ｐｅ２、接触点Ｐｓの座標系ＣＦａの位置を求めることができ、一方の物体２１における接触点の位置が求められる。

【0134】

また、前述の説明においては、一方の物体２１の凸部の形状を円柱形状としたが、図５（ｇ）、図５（ｈ）に図示されるような、テーパー形状など、一方の物体２１の凸部の先端からの凸部中心軸Ｌａの軸方向の距離によって径が変化する形状のように、接触点から凸部中心軸に対して垂線を引いたときの凸部中心軸との交点と凸部中心軸上の所定の点（前記交点以外の点）との距離に基づいて接触点と凸部中心軸との各最短距離が定まる形状である場合について説明する。

【0135】

図１２（ａ）には、一方の物体２１の凸部が円錐台形状で、他方の物体２２が凹部が穴であり、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触している状態が図示されている。図１２（ｂ）には、一方の物体２１の凸部が円錐台形状で、他方の物体２２が凹部が穴であり、一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触している状態を、接触している３つの接触点を含む面に直交し、かつ、一方の物体２１の凸部中心軸を含む平面を断面として見た様子が図示されている。一方の物体２１の凸部を、図１１に示されるような前述の円柱形状から、図１２に示すような、凸部の先端の円の形状は円柱形状の場合と同じで、凸部中心軸Ｌａの軸方向に沿ってｈの大きさによって径が変わる、円錐台などの形状にして、他方の物体２２の凹部に対して３つの接触点で接触させる場合を考える。また、凸部中心軸Ｌａの軸方向に沿って凸部の物体の径が変わること以外の状況や説明は、前述と同様とする。なお、前記凸部中心軸上の所定の点は、姿勢算出用の力計測座標系の原点とする。この場合、接触点Ｐｓの姿勢算出用の力計測座標系ＣＦａ上のＸ軸の−方向の位置だけが変わることになる。このような状況であって、一方の物体２１の凸部の径ｒ’がｈから定まる場合には、式（３７）から式（４１）の内、式（４０）、式（４１）における、ｒを、ｈから算出される距離ｒ’、例えば、定数Ａ＊ｈ＋定数Ｂ、定数Ａ＊ｈ＊ｈなど、に変更すれば、前記のような物体とした一方の物体２１の凸部と他方の物体２２の凹部とが３つの接触点で接触しているときの、他方の物体２２の凹部に設定した座標系ＣＦｂ上における幾何的な条件が前述と同様に導かれる。前述の条件で、接触点Ｐｓと凸部中心軸との最短距離がｒ’と表される場合、式（４０）、式（４１）は、それぞれ以下の式（４８）、（４９）のようになる。
ｒ’＊Ｃｏｓ（α）＋ｈ＊Ｓｉｎ（α） ＝ Ｒ＋ΔＰｘ （４８）
−ｒ’＊Ｓｉｎ（α）＋ｈ＊Ｃｏｓ（α） ＝ −ΔＰｚ （４９）

【0136】

前述の説明において、具体的に、一方の物体２１の凸部の円柱形状を円錐台の形状とした場合について考える。図１２（ａ）、図１２（ｂ）に示されるように、一方の物体２１の凸部の円錐台形状と、他方の物体２２の凹部とは接触点Ｐｅ１、接触点Ｐｅ２、接触点Ｐｓの３つの接触点で接触する。図１２（ａ）、図１２（ｂ）に示されるパラメータなどは、図１１（ａ）、図１１（ｂ）や前述の説明と同様とする。図１１の状況とは、凸部中心軸Ｌａの軸方向に沿って凸部の物体の径が変わることが異なる。図１２に示されるように、接触点Ｐｅ１、接触点Ｐｅ２から、凸部中心軸Ｌａまでの各最短距離はｒとする。また、接触点Ｐｓから凸部中心軸Ｌａに対して垂線を引いたときの凸部中心軸との交点をＰｔとする。交点Ｐｔと、凸部中心軸上の所定の点とする点Ｏａとの距離をｈとする。接触点Ｐｓと凸部中心軸Ｌａとの最短距離は、接触点Ｐｓと交点Ｐｔとの距離であり、一方の物体２１の凸部の円錐台の形状によって、ｒ＋ａ＊ｈと定まるとする。ここで、ａは定数とする。このように一方の物体２１の凸部が円錐台形状の場合、一方の物体２１の凸部が円柱形状の場合の前述の式（４０）、式（４１）は、それぞれ以下の式（５０）、（５１）のようになる。
（ｒ＋ａ＊ｈ）＊Ｃｏｓ（α）＋ｈ＊Ｓｉｎ（α） ＝ Ｒ＋ΔＰｘ （５０）
−（ｒ＋ａ＊ｈ）＊Ｓｉｎ（α）＋ｈ＊Ｃｏｓ（α） ＝ −ΔＰｚ （５１）
変更した式（５０）、式（５１）と、もとの式（３７）、式（３８）、式（３９）、式（４１）、式（２５）を用いることによって、前述と同様に、角度αを算出することができる。

【0137】

以上のように、接触点と凸部中心軸との各最短距離が一定値となる場合だけでなく、接触点から凸部中心軸に対して垂線を引いたときの凸部中心軸との交点と凸部中心軸上の所定の点（前記交点以外の点）との距離に基づいて接触点と凸部中心軸との各最短距離が定まる場合においても、他方の物体２２の凹部に設定した座標系ＣＦｂに対する、姿勢算出用の力計測座標系ＣＦａの、角度α、位置が算出され、他方の物体２２の凹部に対する、一方の物体２１の凸部の相対的な位置および姿勢が算出される。なお、一方の物体２１の凸部の円錐台形状の側面を、他方の物体２２の凹部の穴の端部を含む平面など、他方の物体２２に存在する平面に対して、押しつけることを考える場合、テーパー部分の角度θｃを求めて、角度αと角度θｃを用いて、一方の物体２１の凸部の円錐台を回転させて、押しつけるようにすればよい。

【0138】

姿勢算出用の力計測座標系ＣＦａから他方の物体２２の凹部に設定した座標系ＣＦｂへの同次変換行列は、式（３３）、式（４４）、式（４５）、式（４６）によって表される同次変換行列Ｔｂの逆行列から算出される。つまり、姿勢算出用の力計測座標系ＣＦａと、他方の物体２２の凹部の穴の端部に対して設定する座標系との位置および姿勢の関係が求まる。これによって、一方の物体２１の凸部に対する、他方の物体２２の凹部の穴の端部の中心位置および端部が存在する面の姿勢が算出され、一方の物体２１の凸部に対する、他方の物体２２の凹部の相対的な位置および姿勢を算出することができる。なお、他方の物体２２の凹部の穴の端部の中心位置および端部が存在する面の姿勢が求まれば十分である場合、他方の物体２２の凹部に設定した座標系ＣＦｂと他方の物体２２の位置および姿勢を表す座標系との位置および姿勢の関係は不要となる。

【0139】

また、空間に対して設定した座標系や、駆動部５０に設定される座標系、ツール座標系などに対する、姿勢算出用の力計測座標系ＣＦａへの同次変換行列や位置および姿勢は、前述のようにして設定または算出することができる。

【0140】

また、空間に対して設定した座標系や、駆動部５０に設定される座標系、ツール座標系などに対する、他方の物体２２の凹部に設定した座標系ＣＦｂへの同次変換行列や位置および姿勢は、それらの座標系に対する姿勢算出用の力計測座標系ＣＦａへの同次変換行列と、姿勢算出用の力計測座標系ＣＦａから他方の物体２２の凹部に設定した座標系ＣＦｂへの同次変換行列から算出することができる。

【0141】

また、他方の物体２２の凹部に設定した座標系ＣＦｂと、他方の物体２２の位置および姿勢を表す座標系との関係が設定されたり、既知であったり、算出されたりしている場合、他方の物体２２の凹部に設定した座標系ＣＦｂから他方の物体２２の位置および姿勢を表す座標系への同次変換行列と、空間に対して設定した座標系や、駆動部５０に設定される座標系、ツール座標系などから他方の物体２２の凹部に設定した座標系ＣＦｂへの同次変換行列とに基づいて、空間に対して設定した座標系や、駆動部５０に設定される座標系、ツール座標系などに対する、他方の物体２２の位置および姿勢を算出することができる。

【0142】

他方の物体２２の凹部に設定した座標系ＣＦｂと、他方の物体２２の位置および姿勢を表す座標系との関係が、他方の物体２２の凹部に設定した座標系ＣＦｂのＺ軸の軸回りの姿勢だけが異なる場合、他方の物体２２に存在する別の凹部に存在する面の姿勢を算出することによって、または他方の物体２２の別の部分に一方の物体２１を接触させて検出するようにして座標系ＣＦｂのＺ軸の軸回りの角度を算出することによって、前記座標系ＣＦｂのＺ軸の軸回りの姿勢を算出するようにしてもよい。このように、他方の物体２２の形状の特徴を利用して、算出できていない姿勢についても求めるようにしてもよい。

【0143】

また、他方の物体２２の位置および姿勢を表す座標系を、予め、一方の物体２１の凸部と他方の物体２２の凹部の接触点である点Ｐｅ１と点Ｐｅ２とを結ぶ直線と平行な軸がＹ軸となるように設定し、一方の物体２１の凸部と他方の物体２２の凹部とを接触させる状況を考慮してＸ軸の＋方向を設定しておいてもよい。

【0144】

また、他方の物体２２に存在し、互いの位置および／または姿勢の関係が既知な複数の凹部を利用して、姿勢算出用の力計測座標系ＣＦａと他方の物体２２の位置および姿勢を表す座標系との位置および姿勢の関係を算出するようにしてもよい。まず、姿勢算出用の力計測座標系ＣＦａと他方の物体２２に存在する或る凹部に設定した座標系ＣＦｂとの位置および姿勢の関係を算出する。次に、他方の物体２２に存在する別の凹部を利用して、姿勢算出用の力計測座標系ＣＦａと他方の物体２２の別の凹部に設定した座標系ＣＦｂ’との位置および姿勢の関係を算出する。この結果と、他方に存在する或る凹部と他方の物体２２に存在する別の凹部との既知の位置および／または姿勢の関係とに基づいて、姿勢算出用の力計測座標系ＣＦａと他方の物体２２の位置および姿勢を表す座標系との位置および姿勢の関係を算出するようにしてもよい。このとき、複数の凹部に対して、姿勢算出用の力計測座標系ＣＦａとの位置および姿勢の関係を算出して、他方の物体２２の位置および姿勢をより高い次元で算出できるようにしたり、一方の物体２１と他方の物体２２の相対的な位置および姿勢をより正確に算出したりしてもよい。このように、一方の物体２１および他方の物体２２の形状や、一方の物体２１や他方の物体２２に存在する複数の凸部や凹部を利用して、他方の物体２２の位置および姿勢をより高い次元で算出できるようにしたり、一方の物体２１と他方の物体２２の相対的な位置および姿勢をより正確に算出したりしてもよい。

【0145】

他方の物体２２に存在する別の凹部を利用して、他方の物体２２の位置および姿勢を表す座標系と、他方の物体２２の凹部に設定した座標系ＣＦｂとが、座標系ＣＦｂのＺ軸の軸回りの姿勢だけが異なる場合の、前記Ｚ軸の軸回りの姿勢のずれを算出する方法について図１７を参照しながら以下で述べる。

【0146】

図１７に図示されるように、他方の物体２２は、凹部２２ａおよび凹部２２ｂを含む。ここで、他方の物体２２の凹部２２ａおよび他方の物体２２の凹部２２ｂは径が同じである円筒形状とする。また、一方の物体２１の凸部も円筒形状であり、他方の物体２２の凹部２２ａおよび他方の物体２２の凹部２２ｂと同じ径の円筒形状とする。また、他方の物体２２の凹部２２ａの円筒形状の中心軸と、他方の物体２２の凹部２２ｂの円筒形状の中心軸とは、直交するように構成されているとする。他方の物体２２の位置および姿勢は、座標系ＣＦ０によって表されるとする。座標系ＣＦ０は、図１７にＸ０と示すＸ軸、Ｙ０と示すＹ軸、Ｚ１と示すＺ軸の互いに直交する３軸によって構成され、座標系原点は点Ｏｂとする。一方の物体２１の凸部と他方の物体２２の凹部とを３つの接触点で接触させることによって、一方の物体２１の凸部と他方の物体２２の凹部２２ａとの相対的な位置および姿勢を算出するときに、他方の物体２２に対して設定する座標系を座標系ＣＦｂとする。座標系ＣＦｂは互いに直交する、図１７にＸ１と示すＸ軸、Ｙ１と示すＹ軸、Ｚ１と示すＺ軸で構成され、座標系の原点は点Ｏｂとする。座標系ＣＦ０と座標系ＣＦｂとは、座標系の原点が一致し、またＺ軸は一致している。座標系ＣＦ０と座標系ＣＦｂとは、座標系ＣＦｂのＺ軸の軸回りにだけ姿勢が異なっているとする。

【0147】

次に、一方の物体２１の凸部と他方の物体２２の凹部２２ｂとを３つの接触点で接触させることによって、一方の物体２１の凸部と他方の物体２２の凹部２２ｂとの相対的な位置および姿勢を算出するときに、他方の物体２２に対して設定する座標系を座標系ＣＦｂ２とする。座標系ＣＦｂ２は互いに直交する、図１７にＸ２と示すＸ軸、Ｙ２と示すＹ軸、Ｚ２と示すＺ軸で構成され、座標系の原点は点Ｏｂ２とする。これによって、座標系ＣＦａの位置および姿勢に基づいて、座標系ＣＦｂと座標系ＣＦｂ２の相対的な位置や相対的な姿勢を算出することができる。

【0148】

座標系ＣＦｂを座標系ＣＦｂのＺ軸の軸回りに回転させて、座標系ＣＦ０を算出することを考える。座標系ＣＦｂを座標系ＣＦｂのＺ軸の軸回りに回転させるとき、座標系ＣＦｂ２のＸ軸とＹ軸とを含む平面と座標系ＣＦｂのＹ軸が平行となるように、かつ、回転後のＸ軸の正方向に点Ｏｂ２が存在するように、またはＸ軸が座標系ＣＦｂ２のＺ軸と平行かつ同じ向きとなるようにする。これによって、座標系ＣＦｂを座標系ＣＦｂのＺ軸の軸回りに回転させ、座標系ＣＦ０を算出することができる。このように、他方の物体２２に存在する複数の凹部の相対的な位置および姿勢の関係を用いることによって、一方の物体２１に対する他方の物体２２の３次元の位置および姿勢を算出することができ、より多自由度の姿勢を算出することができるようになる。また、他方の物体２２に存在する複数の凹部の位置および姿勢をもとに、他方の物体２２の位置および姿勢をより正確に算出することができる。

【0149】

以上のように、一方の物体２１や他方の物体２２に存在する複数の凸部や凹部や、その物体における複数の凸部や凹部の相対的な位置および姿勢の関係を用いることによって、多自由度の姿勢を算出することができたり、より正確な位置および姿勢を算出したりすることができるようになる。また、他方の物体２２の凹部２２ａと他方の物体２２の凹部２２ｂとの相対的な位置や姿勢に基づいて、他方の物体２２の良否判定、つまり、ワークやツールなどの物体の良否を判定してもよい。例えば、判定対象とするワークやツールが、意図した状態とは異なった状態の物になっていないか、欠陥のある不良品でないか、意図した種類とは異なった種類の物でないか、ワークに損傷が生じていないかなどを判定してもよい。

【0150】

前記のように、一方の物体２１や他方の物体２２に、複数の凸部や凹部を存在させるようにした形状の別の一実施例について説明する。

【0151】

図１８は、一方の物体２１、他方の物体２２に、複数の凸部、凹部を包含させる場合の別の例を示す図である。図１８（ａ）に示すように、一方の物体２１、他方の物体２２に包含される凸部または凹部として、３つの円筒形状の部分を設け、前記３つの円筒形状の中心軸が互いに直交するように構成してもよい。また、図１８（ｂ）に示すように、一方の物体２１、他方の物体２２に含まれる凸部または凹部として、２つの円筒形状の部分を設け、前記２つの円筒形状の中心軸が互いに直交するように構成してもよい。また、図１８（ｃ）に示すように、一方の物体２１、他方の物体２２に含まれる凹部として、３つの穴を設け、３つの穴の中心軸が互いに直交するように構成してもよい。また、図１８（ｄ）に示すように、一方の物体２１、他方の物体２２に含まれる凹部として、２つの穴を設け、２つの穴の中心軸が互いに直交するように構成してもよい。一方の物体２１、他方の物体２２には、図１８（ａ）、図１８（ｂ）、図１８（ｃ）、図１８（ｄ）に図示するように、複数の凸部または凹部が包含されるようにしてもよい。また、図１８（ａ）、図１８（ｂ）、図１８（ｃ）、図１８（ｄ）に図示するような、複数の凸部または凹部を包含する物体は、一方の物体２１、他方の物体２２に、着脱することができるようにした物体であってもよい。

【0152】

以上のようにして、物体の姿勢算出部３２は、駆動部５０によって一方の物体２１と他方の物体２２とを相対的に移動させて一方の物体２１と他方の物体２２とを接触させ、一方の物体２１と他方の物体２２とが３つの接触点で接触するときに力計測部３１によって計測される力、および接触点と凸部中心軸との最短距離に基づいて、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を算出することができる。また、算出した一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を利用して、空間に対して設定した座標系や、駆動部５０に設定される座標系、ツール座標系などの座標系に対する一方の物体２１の位置および／または姿勢を算出してもよい。また、空間に対して設定した座標系や、駆動部５０に設定される座標系、ツール座標系などの座標系に対する他方の物体２２の位置および／または姿勢を算出してもよい。また、他方の物体２２に対する、空間に対して設定した座標系や、駆動部５０に設定される座標系、ツール座標系などの座標系の位置および／または姿勢を算出してもよい。

【0153】

本発明の２番目の実施形態においては、本発明の１番目の実施形態の物体の姿勢算出システム１１に対して、一方の物体２１は前記凸部もしくは前記凹部を２つ以上包含する、または、他方の物体２２は前記凸部もしくは前記凹部を２つ以上包含する、または、一方の物体２１および他方の物体２２は前記凸部もしくは前記凹部を２つ以上包含する、物体の姿勢算出システムとすることが好ましい。

【0154】

本発明の１番目の実施形態で説明した凸部と凹部について、例えば、他方の物体２２が前記凹部を２つ以上包含し、一方の物体２１が前記各凹部に対して３つの接触点で接触可能な前記凸部を１つ以上包含するようにしてもよい。また、他方の物体２２が前記凹部を１つ以上、前記凸部を１つ以上包含し、一方の物体２１が、他方の物体２２の前記凹部に対して３つの接触点で接触可能な前記凸部を１つ以上、他方の物体２２の前記凸部に対して３つの接触点で接触可能な前記凹部を１つ以上包含するようにしてもよい。

【0155】

複数の前記凸部または前記凹部を包含させておくことによって、駆動部５０の機構部として移動可能な領域、物体との干渉によって移動可能な領域、物体における接触可能な部分などの、システムの状況に応じて、移動可能な場所にある前記凸部または前記凹部に対して、３つの接触点で接触させることが可能な物体を接触させることができる。また、１つの物体における複数の前記凸部または前記凹部の部分について相対的な位置および／または姿勢を算出することによって、また、１つの物体における複数の互いの位置および／または姿勢の関係が既知な前記凸部または前記凹部の部分について相対的な位置および／または姿勢を算出することによって、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢について、より正確な相対的な位置および／または姿勢の算出、また３次元での任意の位置および姿勢を算出することができるようになる。

【0156】

例えば、前述のように、他方の物体２２に、互いの位置および姿勢が分かっている凹部を２つ取りつけ、２つの凹部の位置および姿勢の情報から、他方の物体２２の位置および姿勢を算出してもよい。また、例えば、他方の物体２２に複数の前記凸部または前記凹部をとりつけ、その前記凸部または前記凹部の位置のみを前記複数の部分について算出し、その複数の位置の情報から、他方の物体２２の位置および姿勢を算出するようにしてもよい。

【0157】

本発明の３番目の実施形態においては、本発明の１番目または２番目の実施形態の物体の姿勢算出システム１１に対して、一方の物体２１または他方の物体２２に包含される、前記凸部および前記凹部の内、少なくとも１つの物体は前記包含される一方の物体２１または他方の物体２２に対して着脱可能な物体である、物体の姿勢算出システムとすることが好ましい。

【0158】

本発明の１番目の実施形態で説明した凸部と凹部について、前記凸部および前記凹部の内、少なくともいずれか１つは、当該物体が取りつけられた一方の物体２１または他方の物体２２に対して着脱可能であることによって、つまり、前記凸部や前記凹部を着脱可能な物体とすることによって、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を算出したいときにだけ、前記凸部および前記凹部の内、少なくともいずれか１つを一方の物体２１または他方の物体２２に取りつければよい。

【0159】

これによって、前記凸部または前記凹部のような部分がない物体でも、必要に応じて、前記凸部または前記凹部を包含させることができるようになる。なお、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を算出した後は取り外すようにすればよい。

【0160】

また、一方の物体２１または他方の物体２２の或る部分に前記凸部または前記凹部を取りつけて、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を算出した後、取り付けた前記凸部または前記凹部を取り外す。次に、一方の物体２１または他方の物体２２の別の或る部分に前記凸部または前記凹部を取りつけて、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を算出した後、取り付けた前記凸部または前記凹部を取り外す。前記操作を複数回、実行することによって、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢をより正確に算出したり、３次元の任意の位置および姿勢を算出することができるようにしたりしてもよい。

【0161】

本発明の４番目の実施形態においては、本発明の１番目から３番目のいずれかの実施形態の物体の姿勢算出システム１１に対して、一方の物体２１と他方の物体２２とを前記３つの接触点で接触させる前に、一方の物体２１と他方の物体２２との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め一方の物体２１と他方の物体２２とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムとすることが好ましい。

【0162】

一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させれば、本発明の１番目の実施形態で説明したように、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を算出することができる。

【0163】

このとき、力センサ、トルクセンサ、触覚センサ、接触センサなどの、力や接触の感覚、接触を検出する接触式のセンサを用いて、自動的に、一方の物体２１または他方の物体２２の大体の、位置および姿勢を検出させた後、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。

【0164】

また、カメラ、レーザ、ＬＥＤ、超音波などを利用した非接触式のセンサを用いて、自動的に、一方の物体２１または他方の物体２２の大体の、位置および姿勢を検出させた後、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。この場合、物体の位置および／または姿勢をあまり高い精度で検出できない、または、物体の大体の位置および／または姿勢しか認識できないようなセンサを利用して、大体の範囲の位置および姿勢に移動させた後、一方の物体２１と他方の物体２２との相対的な位置および姿勢をより正確に算出することができる。

【0165】

また、前記接触式のセンサや前記非接触式のセンサを利用して、作業者や教示者が、駆動部５０を誘導するように移動させて、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。

【0166】

また、加速度センサやジャイロセンサなどによって構成される教示操作装置を用いて、前記教示操作装置の３次元空間での傾き、加速度などを利用して、駆動部の並進移動や回転移動を操作し、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。

【0167】

また、カメラ、レーザ、ＬＥＤ、超音波などを利用した、空間内の位置または位置および姿勢を指定する装置を利用して、空間内の指定した位置または位置および姿勢に、駆動部５０が移動させる一方の物体２１または他方の物体２２を移動させて、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。

【0168】

また、パーソナルコンピュータや、駆動部制御装置５２と有線または無線で接続した教示装置などにおいてシステムのＣＡＤモデルなどを利用して、一方の物体２１と他方の物体２２との相対的な位置および姿勢を設定し、その設定に基づいて、駆動部５０によって実際の一方の物体２１と他方の物体２２とを相対的に移動させたときに、一方の物体２１と他方の物体２２とが、所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢になる場合であってもよい。

【0169】

以上のように、様々な方法によって、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に、大まかに移動させた場合であっても、本発明の１番目の実施形態で説明したように、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を算出させることができる。これによって、算出した相対的な位置および／または姿勢に基づいて、駆動部５０が移動させる一方の物体２１または他方の物体２２を所望の位置および／または姿勢に、正確に移動させたり、所望の作業を適切に実行させたりすることができるようになる。

【0170】

本発明の５番目の実施形態においては、本発明の１番目から３番目のいずれかの実施形態の物体の姿勢算出システム１１に対して、前記力計測部は、さらに、前記駆動部または一方の物体２１または他方の物体２２に作用する力を計測し、一方の物体２１と他方の物体２２とを前記３つの接触点で接触させる前に、前記力計測部によって計測される力に基づいて、一方の物体２１と他方の物体２２との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め一方の物体２１と他方の物体２２とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムとすることが好ましい。

【0171】

作業者は、駆動部５０に対して、または、駆動部５０が移動させる一方の物体２１もしくは他方の物体２２に対して、作用させる力を力計測部３１が計測し、作用された力に基づいて、駆動部５０を誘導するように移動させて、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。これによって、作業者は、駆動部５０を直感的に、また直接的に、駆動部５０が移動させる物体を移動させることができる。

【0172】

また、作業者は、駆動部５０が移動させない一方の物体２１または他方の物体２２に対して、作用させる力を力計測部３１が計測し、作用された力に基づいて、駆動部５０を誘導するように移動させて、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。なお、このとき、駆動部５０が移動させない一方の物体２１または他方の物体２２に対して、作業者が作用させる力を検出できるように、別の力センサを設置したり、別の駆動部５０’に駆動部５０が移動させない一方の物体２１または他方の物体２２がある場合には駆動部５０’に作用する力を計測して算出できるようにしたりしておく。これによって、作業者は、一方の物体２１と他方の物体２２とを相対的に移動させるときに、駆動部５０が移動させない一方の物体２１または他方の物体２２から見た、相対的に移動させたい所望の方向に容易に移動させることができる。

【0173】

また、駆動部５０または駆動部５０が移動させる物体に作用する力を、力計測部３１が計測する場合、駆動部５０が移動させる物体でない方の物体に対して、駆動部５０を移動させて接触させることによって、駆動部５０が移動させる物体でない方の物体の大体の位置および姿勢を算出する。この算出した位置および姿勢に基づいて、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。

【0174】

このように、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に、大まかに移動させた場合であっても、本発明の１番目の実施形態で説明したように、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を算出させることができる。これによって、算出した相対的な位置および／または姿勢に基づいて、駆動部５０が移動させる一方の物体２１または他方の物体２２を所望の位置および／または姿勢に、正確に移動させたり、所望の作業を適切に実行させたりすることができるようになる。

【0175】

本発明の６番目の実施形態においては、本発明の１番目から３番目のいずれかの実施形態の物体の姿勢算出システム１１に対して、前記物体の姿勢算出システムは、さらに、一方の物体２１または他方の物体２２の位置および／または姿勢を所定の範囲以内で検出することが可能な物体検出部を備え、一方の物体２１と他方の物体２２とを前記３つの接触点で接触させる前に、前記物体検出部の検出結果に基づいて、一方の物体２１と他方の物体２２との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め一方の物体２１と他方の物体２２とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムとすることが好ましい。

【0176】

前記物体の姿勢算出システムは、さらに、カメラ、レーザ、ＬＥＤ、超音波などを利用した非接触式のセンサ、または、力センサ、トルクセンサ、触覚センサ、接触センサなどの力や接触の感覚、接触を検出する接触式のセンサを用いた、一方の物体２１または他方の物体２２や、その凸部または凹部の位置および／または姿勢を所定の範囲以内で検出することが可能な、または、駆動部５０が移動させる物体ではない方の物体や、その凸部または凹部の位置および／または姿勢を所定の範囲以内で検出することが可能な物体検出部を備える。なお、例えば、駆動部５０が移動させる物体を一方の物体２１としたとき、一方の物体２１および他方の物体２２の物体形状が複雑であり、干渉などのため、一方の物体２１を他方の物体２２に対する相対的な位置および／または姿勢を所定の範囲以内に移動させる場合に、一方の物体２１の物体の形状をある程度、認識する必要がある場合、駆動部５０が移動させる物体を一方の物体２１の位置および／または姿勢を所定の範囲内で検出するようにしてもよい。

【0177】

物体検出部によって、一方の物体２１または他方の物体２２の位置および／または姿勢を検出した後、前記検出結果に基づいて、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させる。

【0178】

このように、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に、大まかに移動させた場合であっても、本発明の１番目の実施形態で説明したように、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を算出させることができる。これによって、算出した相対的な位置および／または姿勢に基づいて、駆動部５０が移動させる一方の物体２１または他方の物体２２を所望の位置および／または姿勢に、正確に移動させたり、所望の作業を適切に実行させたりすることができるようになる。

【0179】

物体検出部は、物体の位置および／または姿勢を所定の範囲以内で検出することができればよいので、必要最低限の情報が取得できるようなセンサなどで構成されればよい。このため、物体の位置および／または姿勢を精度よく検出ができるようなセンサなどは必要なく、また、複雑なセンサを使用しないためにセンサの故障などを減らすことができる。このような、物体の位置および／または姿勢をあまり高い精度で検出できない、または、物体の大体の位置および／または姿勢しか認識できないようなセンサを利用して、大体の範囲の位置および姿勢に移動させた後、一方の物体２１と他方の物体２２との相対的な位置および姿勢をより正確に算出することができる。このため、安価で、システムの運用上の安定性の高いシステムとすることができる。

【0180】

本発明の７番目の実施形態においては、本発明の１番目から３番目のいずれかの実施形態の物体の姿勢算出システム１１に対して、前記物体の姿勢算出システムは、さらに、空間内の位置または位置および姿勢を指定する装置と前記指定した位置または位置および姿勢を認識する装置とで構成される空間位置指定部を備え、一方の物体２１と他方の物体２２とを前記３つの接触点で接触させる前に、前記空間位置指定部によって指定される位置または位置および姿勢に基づいて、一方の物体２１と他方の物体２２との相対的な位置および姿勢が、所定の位置および姿勢の範囲内となるように、予め一方の物体２１と他方の物体２２とを相対的な概位置および概姿勢に移動させる、物体の姿勢算出システムとすることが好ましい。

【0181】

物体の姿勢算出システムは、さらに、カメラ、レーザ、ＬＥＤ、超音波などを利用した、空間内の位置または位置および姿勢を指定する装置と前記指定した位置または位置および姿勢を認識する装置とで構成される空間位置指定部を備えるようにすることが好ましい。例えば、位置または位置および姿勢を指定できるように、マーカーを配置しそれをカメラで認識したり、位置または位置および姿勢を指定したい凸部または凹部に対して、レーザで点や十字を照射したり、ＬＥＤや超音波を照射したりして、その受光器や受信機を用いるようにして、空間位置指定部を構成させるようにしてもよい。

【0182】

前記空間位置指定部によって、指定した位置または位置および姿勢を認識した、前記認識結果に基づいて、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に移動させてもよい。

【0183】

このように、一方の物体２１と他方の物体２２とを所定の位置および姿勢の範囲内の大体の、相対的な位置および姿勢に、大まかに移動させた場合であっても、本発明の１番目の実施形態で説明したように、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を算出させることができる。これによって、算出した相対的な位置および／または姿勢に基づいて、駆動部５０が移動させる一方の物体２１または他方の物体２２を所望の位置および／または姿勢に、正確に移動させたり、所望の作業を適切に実行させたりすることができるようになる。

【0184】

本発明の８番目の実施形態においては、本発明の１番目から７番目のいずれかの実施形態の物体の姿勢算出システム１１に対して、前記物体の姿勢算出部が算出した一方の物体２１と他方の物体２２との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記駆動部に対する、一方の物体２１または他方の物体２２の位置および／または姿勢の算出を行う、物体の姿勢算出システムとすることが好ましい。

【0185】

物体の姿勢算出システムは、前記物体の姿勢算出部３２が算出した一方の物体２１と他方の物体２２との、相対的な位置および／または姿勢に基づいて、駆動部５０に対する、一方の物体２１または他方の物体２２の位置および／または姿勢を算出するようにすることが好ましい。なお、駆動部５０が凸部を包含する一方の物体２１を移動させ、一方の物体２１に対する姿勢算出用の力計測座標系ＣＦａが算出され、ツール座標系と姿勢が異なる場合、ツール座標系を補正するようにしてもよい。また、駆動部５０が保持して移動させる一方の物体２１の凸部の位置は分かっているが、一方の物体２１の姿勢が分からない場合、一方の物体２１に対する姿勢算出用の力計測座標系ＣＦａを算出することによって、一方の物体２１の姿勢が求められる。また、駆動部５０が保持して移動させる一方の物体２１における、他方の物体２２の凹部と接触させる凸部の位置は分かっているが、一方の物体２１に存在する別の複数の凸部の正確な位置および／または姿勢が分からず、一方の物体２１の位置および姿勢を定める座標系の位置および姿勢が分からない場合、つまり、他方の物体２２の凹部と接触させる一方の物体２１の凸部に設定した或る点について一方の物体２１の姿勢が変わる場合、一方の物体２１に存在する別の複数の凸部を、他方の物体２２の凹部と接触させることによって求められる一方の物体２１に存在する別の複数の凸部の位置および／または姿勢から、一方の物体２１の位置および姿勢を定める座標系の位置および姿勢を求め、駆動部５０に対する一方の物体２１の方の物体２１の位置および姿勢を求めるようにしてもよい。

【0186】

駆動部５０に対する、一方の物体２１または他方の物体２２の位置および／または姿勢を算出することによって、駆動部５０に対する、一方の物体２１または他方の物体２２の位置および／または姿勢が分かるので、所望の作業をより適切に、正確に実行できるようになる。

【0187】

本発明の９番目の実施形態においては、本発明の１番目から８番目のいずれかの実施形態の物体の姿勢算出システム１１に対して、前記物体の姿勢算出部が算出した一方の物体２１と他方の物体２２との、相対的な姿勢または相対的な位置および姿勢に基づいて、前記駆動部によって移動される一方の物体２１および／または他方の物体２２を移動させる位置および／または姿勢を補正する、物体の姿勢算出システムとすることが好ましい。

【0188】

前記物体の姿勢算出部３２が算出した一方の物体２１と他方の物体２２との、相対的な姿勢または相対的な位置および姿勢に基づいて、駆動部５０によって移動される一方の物体２１および／または他方の物体２２を移動させる位置および／または姿勢を補正することによって、一方の物体２１と他方の物体２２とを用いた作業、例えば、組立て作業や加工作業などにおいて、より適切な、正確な作業を実行することができる。

【0189】

また、物体の姿勢算出システムにおける駆動部が、図２に示されるように、一方の物体２１を移動させる駆動部５０と、他方の物体２２を移動させる駆動部５０’の２つが存在する場合であって、駆動部５０と駆動部５０’との位置および姿勢の関係は既知であり、駆動部５０に対する一方の物体２１の位置および姿勢は既知であり、駆動部５０’に対する他方の物体２２の位置および姿勢が分からない場合、本発明の１番目の実施形態で説明したように、一方の物体２１と他方の物体２２との相対的な位置および姿勢を算出させることができる。これによって、算出した相対的な位置および姿勢に基づいて、駆動部５０’に対する他方の物体２２の位置および姿勢が既知となるので、駆動部５０’が他方の物体２２を所望の位置および／または姿勢に、正確に移動させたり、所望の作業を適切に実行させたりすることができるようになる。

【0190】

本発明の１０番目の実施形態においては、本発明の１番目から９番目のいずれかの実施形態の物体の姿勢算出システム１１に対して、前記物体の姿勢算出部が算出した一方の物体２１と他方の物体２２との、相対的な姿勢または相対的な位置および姿勢に基づいて、一方の物体２１または他方の物体２２の良否判定と、一方の物体２１または他方の物体２２の種類判定と、前記駆動部５０によって保持される一方の物体２１または他方の物体２２の保持状態の判定との内、少なくとも１つの判定を実行する、物体の姿勢算出システムとすることが好ましい。

【0191】

本発明の１番目の実施形態で説明したように、一方の物体２１と他方の物体２２とを３つの接触点で接触させることによって、一方の物体２１と他方の物体２２との相対的な位置および／または姿勢を算出することができる。また、一方の物体２１および他方の物体２２に対する前記３つの接触点の位置についても算出することができる。また、一方の物体２１と他方の物体２２との相対的な姿勢または相対的な位置および姿勢に基づいて、駆動部５０に対する、駆動部５０が移動させない物体の姿勢または位置および姿勢を算出することができる。これらの算出された結果に基づいて、一方の物体２１または他方の物体２２について、良否判定や種類判定などの各種の判定を実行したり、駆動部５０が移動させる物体であって保持している一方の物体２１および／または他方の物体２２の保持状態の判定をしたりするようにしてもよい。

【0192】

ここで、物体の良否判定とは、物体が意図したものであるか否かのワークやツールなどの物体の良否を判定する。例えば、判定対象とするワークやツールなどが意図した状態とは異なった状態の物になっていないか、欠陥のある不良品でないか、意図した種類とは異なった種類の物でないか、ワークに損傷が生じていないかなどを判定する。

【0193】

また、物体の種類判定は、物体の種類についての判定とする。また、物体の保持状態の判定とは、駆動部５０の先端部分と、保持されている物体とが所定の姿勢の関係になっているか否かなどの判定とする。

【0194】

駆動部５０が一方の物体２１を移動させるとき、一方の物体２１に対する他方の物体２２の位置および／または姿勢が、所定の位置および／または姿勢の範囲内にある場合は、他方の物体２２は意図した物体であり良と判定とし、所定の位置および／または姿勢の範囲内にない場合は、他方の物体２２は意図した物体ではなく良ではないと良否判定をしてもよい。また、駆動部５０が一方の物体２１を移動させるとき、一方の物体２１に対する他方の物体２２の位置および／または姿勢が、所定の位置および／または姿勢の範囲内にある場合は、一方の物体２１は意図した保持状態であると判定とし、所定の位置および／または姿勢の範囲内にない場合は、一方の物体２１は意図した保持状態ではないと判定するように、保持状態の判定をしてもよい。

【0195】

また、駆動部５０が一方の物体２１を移動させるとき、一方の物体２１に対する他方の物体２２の位置および／または姿勢が、所定の位置および／または姿勢の範囲内にある場合は、他方の物体２２は或る物体であると判定し、所定の位置および／または姿勢の範囲内にない場合は、別の物体と種類判定する。このとき、所定の位置および／または姿勢の範囲によって、物体の種類を細かく判定するように種類判定してもよい。

【0196】

また、一方の物体２１に凸部が存在するとし、駆動部５０が一方の物体２１を移動させるとき、他方の物体２２の凹部と接触させることによって、一方の物体２１の凸部に対して姿勢算出用の力計測座標系が設定される。このとき設定が完了した姿勢算出用の力計測座標系の姿勢と、一方の物体２１の凸部に対して設定したツール座標系の姿勢の違いによって、駆動部５０に対して、一方の物体２１が所望の姿勢になっているか、所望の姿勢に近い状態になっているか、意図しない姿勢になっているかなどの保持状態の判定をする。また、設定が完了した姿勢算出用の力計測座標系の姿勢と、一方の物体２１の凸部に対して設定したツール座標系の姿勢との相対的な姿勢の大きさによって、望ましくない姿勢で取得され保持されているか、保持されている一方の物体２１の種類を判定したり、保持されている物体が意図したツールやワークなどの物体になっているかを判定したりしてもよい。

【0197】

また、一方の物体２１または他方の物体２２に対する前記３つの接触点の位置に基づいて、一方の物体２１や他方の物体２２が意図した物体になっているか否かの良否判定や、一方の物体２１や他方の物体２２の物体の種類を判定する種類判定や、保持されている物体が所望の姿勢や所望の姿勢に近い状態か、また、保持されている物体の種類や保持されている物体が意図した物体であるか否かなどの保持状態の判定などの、判定を実行してもよい。

【0198】

なお、前述の、良否判定、種類判定、保持状態の判定の内、少なくとも１つの判定を実行するように、複数の判定を同時に実行するようにしてもよい。このとき、複数の判定を組み合わせることによって、各種判定の正確さを高めることができるようになる。

【0199】

一方の物体２１または他方の物体２２に、複数の、凸部または凹部が存在するときは、算出された複数の凸部または凹部の相対的な位置および／または姿勢に基づいて、複数の、凸部または凹部を包含する前記物体の良否判定、種類判定を実行してもよい。また、既知の複数の凸部または凹部の位置および／または姿勢の関係と、算出された複数の凸部または凹部の相対的な位置および／または姿勢に基づいて、複数の、凸部または凹部を包含する前記物体の良否判定、種類判定を実行してもよい。

【0200】

本発明の１１番目の実施形態においては、本発明の１番目から１０番目のいずれかの実施形態の物体の姿勢算出システム１１に対して、一方の物体２１および他方の物体２２における、前記凸部と前記凹部との組み合わせにおいて、前記凸部と前記凹部とは嵌合させることが可能であり、前記駆動部は、一方の物体２１と他方の物体２２とを相対的に移動させる、かつ、一方の物体２１および他方の物体２２の内、少なくとも１つの物体を保持し移動させる、かつ、一方の物体２１と他方の物体２２とを嵌合させるように移動が可能であり、前記物体の姿勢算出システムは、さらに、前記力計測部によって計測される力に基づいて、前記駆動部によって一方の物体２１と他方の物体２２とを相対的に移動させて力制御を実行し、一方の物体２１と他方の物体２２とを嵌合させる嵌合実行部を備え、前記嵌合実行部は、前記物体の姿勢算出部によって算出される、一方の物体２１と他方の物体２２との、相対的な姿勢または相対的な位置および姿勢に基づいて、一方の物体２１と他方の物体２２とを嵌合させる、物体の姿勢算出システムとすることが好ましい。

【0201】

一方の物体２１の凸部と他方の物体２２の凹部とが、または一方の物体２１の凹部と他方の物体２２の凸部とが、本発明の１番目の実施形態で説明したように、所定の条件を満たすと共に、本発明の１番目の実施形態で説明したような嵌合可能な物体であるとする。また、嵌合可能な前記凸部または前記凹部は包含される一方の物体２１または他方の物体２２に対して着脱可能であってもよい。また、本発明の１番目の実施形態のように、駆動部５０は６つの軸によって構成される垂直多関節型ロボットのように、一方の物体２１と他方の物体２２とを嵌合させるように移動可能であるとする。また、前記物体の姿勢算出システム１１は、さらに、力計測部３１によって計測される力に基づいて、駆動部５０によって一方の物体２１と他方の物体２２とを相対的に移動させて力制御を実行し、一方の物体２１と他方の物体２２とを嵌合させる嵌合実行部を備えることが好ましい。

【0202】

嵌合実行部は、物体の姿勢算出部３２によって算出される、一方の物体２１と他方の物体２２との、相対的な姿勢または相対的な位置および姿勢を利用することによって、相対的な姿勢が大きい場合でも、一方の物体２１と他方の物体２２とを、確実に、また、短い時間で嵌合させることができるようになる。一方の物体２１と他方の物体２２との相対的な姿勢が大きい場合、力制御によって、一方の物体２１と他方の物体２２とを嵌合させることは、困難であることが多い。本発明によって、予め相対的な姿勢が小さくなるように移動させた後に力制御によって嵌合させるようにしたり、算出された相対的な姿勢または相対的な位置および姿勢に基づいて力制御しながら嵌合させたりすることによって、一方の物体２１と他方の物体２２とを確実に嵌合できるようにすると共に、短い時間で嵌合させることが可能となる。

【0203】

本発明の１２番目の実施形態においては、本発明の１１番目の実施形態の物体の姿勢算出システム１１に対して、前記嵌合実行部は、前記物体の姿勢算出部が算出した一方の物体２１と他方の物体２２との、相対的な姿勢または相対的な位置および姿勢に基づいて、一方の物体２１と他方の物体２２との、相対的な姿勢または相対的な位置および姿勢を修正するように移動させた後、一方の物体２１と他方の物体２２とを嵌合させる、物体の姿勢算出システムとすることが好ましい。

【0204】

一方の物体２１と他方の物体２２との相対的な姿勢が大きい場合、予め相対的な姿勢が小さくなるように移動させた後、嵌合作業を実行することによって、一方の物体２１と他方の物体２２とを確実に嵌合できるようにすると共に、短い時間で嵌合させることが可能となる。

【0205】

本発明の１３番目の実施形態においては、本発明の１１番目の実施形態の物体の姿勢算出システム１１に対して、前記嵌合実行部は、前記物体の姿勢算出部が算出した一方の物体２１と他方の物体２２との、相対的な姿勢または相対的な位置および姿勢に基づいて、一方の物体２１と他方の物体２２との相対的な姿勢が大きいときには、一方の物体２１と他方の物体２２との相対的な姿勢を小さくする移動速度が大きくなるように前記力計測部によって計測される力に基づいて力制御を行い、一方の物体２１と他方の物体２２とを嵌合させる、物体の姿勢算出システムとすることが好ましい。

【0206】

一方の物体２１と他方の物体２２とを力制御によって嵌合するとき、一方の物体２１と他方の物体２２との相対的な姿勢が大きいときには、つまり、予め算出された相対的な姿勢を利用して、相対的な姿勢を小さくする方向に、相対的な姿勢の大きさに基づく所定の角度だけ一方の物体２１と他方の物体２２とを相対的に移動させるときには、回転移動速度が大きくなるように、力制御の力制御ゲインを小さくして力に応じた移動指令量を小さくて、その移動指令量に対して足しこむ回転速度指令を大きくするなどして速度を調整する。これによって、相対的な姿勢の大きさが小さくなると想定されるまでは大まかに素早く移動させることができる。

【0207】

相対的な姿勢を小さくする方向に、相対的な姿勢の大きさに基づく所定の角度だけ、一方の物体２１と他方の物体２２とを相対的に移動させた後は、力制御の力制御ゲインを大きくして力に応じた移動指令量を大きくして、その移動指令量に足しこむ回転速度指令を小さくするなどして速度を調整する。これによって、相対的な姿勢の大きさが小さくなったと想定される状況では、力に応じて細かく移動させ精密な動きをさせることができる。

【0208】

また、一方の物体２１と他方の物体２２との相対的な姿勢を変えていくとき、一方の物体２１と他方の物体２２との予め算出された相対的な位置に基づいて、図１１に示される一方の物体２１の凸部中心軸Ｌａと、他方の物体２２の凹部の穴の中心軸とが重なる状態に近づくまでは、つまり、凸部中心軸Ｌａ上と点Ｏｂの最短距離が所定の値より大きいときは大まかに素早く移動させ、凸部中心軸Ｌａ上と点Ｏｂの最短距離が所定の値以下となったときは力に応じて細かく移動させ精密な動きをさせる。このように、並進方向についても、他方の物体２２に対して設定した座標系ＣＦｂのＸ軸、Ｙ軸方向については、相対的な位置の大きさが小さくなると想定されるまでは大まかに素早く移動させ、相対的な位置の大きさが小さくなったと想定される状況では、力に応じて細かく移動させ精密な動きをさせるようにして、状況に応じて力制御の動作を変えることが好ましい。

【0209】

前述のように状況に応じて力制御の動作を変えることによって、一方の物体２１と他方の物体２２とを確実に嵌合できるようにすると共に、短い時間で嵌合させることが可能となる。

【0210】

以上、本発明の種々の実施形態および変形例を説明したが、他の実施形態および変形例によっても本発明の意図される作用効果を奏することができることは当業者に自明である。特に、本発明の範囲を逸脱することなく前述した実施形態および変形例の構成要素を削除ないし置換することが可能であるし、公知の手段をさらに付加することが可能である。また、本明細書において明示的または暗示的に開示される複数の実施形態の特徴を任意に組合せることによっても本発明を実施できることは当業者に自明である。

【符号の説明】

【0211】

１１、１１ａ 物体の姿勢算出システム
２１、２１ａ 一方の物体
２２ 他方の物体
２２ａ、２２ｂ 他方の物体の凹部
２３ 凸部
２４ 凹部
３１ 力計測部
３２ 物体の姿勢算出部
３３ 動作指令生成部
５０、５０’ 駆動部
５１、５１’ 力センサ
５２ 駆動部制御装置
ｄ 一方の物体の凸部の円柱形状の直径
ｒ 一方の物体の凸部の円柱形状の半径
Ｏａ 一方の物体の凸部の円柱形状の先端の中心点
Ｌａ 一方の物体の凸部の凸部中心軸
Ｄ 他方の物体の凹部の穴の直径
Ｒ 他方の物体の凹部の穴の半径
Ｏｂ 他方の物体の凹部の穴の端部の中心点
Ｏｂ２ 他方の物体の凹部の穴の端部の中心点
Ｌａ 一方の物体の凸部に対して設定される凸部中心軸
α ３つの接触点を含む面の法線と凸部中心軸Ｌａとの成す角度
Ｐｑ 射影した部分の端点
Ｐｓ、Ｐｅ１、Ｐｅ２ 接触点
Ｐｔ 接触点から凸部中心軸Ｌａに対して垂線をひいたときの凸部中心軸との交点
ＣＦａ 姿勢算出用の力計測座標系
ＣＦｂ 他方の物体の凹部に対して設定した座標系
ＣＦｂ２ 他方の物体の凹部に対して設定した座標系
ＣＦ０ 他方の物体に対して設定した座標系
β 座標系ＣＦａのＹ軸の−方向と、点Ｏａと接触点Ｐｅ１とを結んだ線分との成す角度
β’ 座標系ＣＦｂのＹ軸の−方向と、点Ｏｂと接触点Ｐｅ１とを結んだ線分との成す角度
ｈ 点Ｐｔと点Ｏａとの距離
Ｆｓ、Ｆｅ 力
θｃ テーパーの角度
Ｐｄ１、Ｐｄ２ 押付方向
ＲＹ 回転方向
＋Ｍｖ１、−Ｍｖ１、＋Ｍｖ２、−Ｍｖ２、＋Ｍｖ３、−Ｍｖ３、＋Ｍｖ４、−Ｍｖ４ 移動方向

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】