JP2011038799A - 位置検出装置、位置検出方法およびプログラム - Google Patents

Abstract

【課題】被検出部周辺の環境を考慮した被検出部の音速の算出を行い、位置の検出対象とする部分付近の音速をできるだけ正確に測定し、それを用いての位置の検出精度を高める。
【解決手段】ロボットハンド１０１の先端部分１０４に超音波送信装置を配置し、さらに先端部分１０４を含むことが可能な多面体１３１〜１３３を設定し、それらの頂点に超音波送受信装置１１１〜１２６を配置する。先端部分１０４の位置検出にあたり、先端部分１０４に近い頂点を特定点として選択し、この特定点に向かう他の頂点からの経路の音速を算出する。この音速に基づいて、先端部分１０４から特定点への経路における音速を算出し、この音速の値と、先端部分１０４と特定点との間における超音波の伝播時間とに基づいて、先端部分１０４の特定点からの距離を算出する。この距離を複数の特定点について得ることで、先端部分１０４の位置を特定する。
【選択図】図１

Description

本発明は、ロボットアーム先端の位置等を超音波により検出する技術に係り、音速の影響による検出誤差を低減する技術に関する。

超音波を用いて、物体の３次元位置を検出する技術が知られている（例えば、特許文献１参照）。この技術では、例えば、検出対象となる部分に超音波発生装置を配置し、そこから発生する超音波信号を異なる３箇所で受信する。この際、３箇所の受信側において、超音波の空気中を伝わる時間を計測することで、これら３箇所から検出対象までの距離ｒ１〜ｒ３が割り出される。そして、これら３箇所を中心とした半径ｒ１、ｒ２、ｒ３の球を描き、それら球の表面が交差する点が検出対象の位置として特定される。

特開平６−１０２３５０号公報

上述した超音波を用いた位置の特定技術は、音波の速度の影響を受ける。音速は、空気中の温度、圧力、湿度、密度（気圧）、流れ（風）の影響を受ける。したがって、上記音速に影響を与えるパラメータが一定でない環境では、音波を用いた位置の検出は、精度が低下する。

例えば、ロボットアームを用いた製造技術現場において、ロボットアームの先端の位置を検出する技術が必要とされる。このような環境においては、温度、湿度、空気の流れ等の変化が、場所や時間によって変化する場合がある。この場合、音波を用いた位置の検出技術では、上述した音速の変化に起因して、検出した位置のデータに誤差が生じる。

このような背景において、本発明は、位置の検出対象とする部分付近の音速をできるだけ正確に測定し、それを用いての位置の検出精度を高める技術を提供することを目的とする。

請求項１に記載の発明は、被検出部に配置された超音波送信手段と、前記被検出部の周囲に配置された複数の超音波送受信手段と、前記複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出する音速算出手段と、前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出する距離算出手段とを備えることを特徴とする位置検出装置である。

請求項１に記載の発明では、まず被検出部の周囲に配置された複数の超音波送受信手段間における音速が算出され、この音速に基づいて、被検出部における音速が算出される。そして、被検出部から発信される超音波を特定の超音波送受信手段で受信し、その伝播時間からその間の距離ｒを算出する。被検出部は、前記特定の超音波送受信手段を中心とした半径ｒの球の球面上に位置するので、このことに基づいて被検出部の位置が特定される。

請求項２に記載の発明は、請求項１に記載の発明において、前記複数の超音波送受信手段は多面体の頂点に配置されており、前記多面体の内部に前記被検出部が位置することを特徴とする。請求項２に記載の発明では、多面体の頂点に超音波送受信手段が配置され、多面体の内部に被検出部が位置する。この構成では、多面体の頂点から被検出部の位置までの距離が算出されることで、被検出部の位置が特定される。

請求項３に記載の発明は、請求項１に記載の発明において、前記複数の超音波送受信手段は複数の多面体の頂点に配置されており、前記複数の多面体のうちの一つに前記被検出部が含まれ、前記音速算出手段は、前記被検出部を含む多面体の複数の頂点に位置する複数の前記超音波送受信手段の間における音速から前記被検出部における音速の算出を行うことを特徴とする。請求項３に記載の発明では、複数の多面体が用意され、その中の一つが被検出部を含む構成とされる。

請求項４に記載の発明は、請求項１〜３のいずれか一項に記載の発明において、前記被検出部はロボットハンドの先端部分であることを特徴とする。請求項４に記載の発明によれば、ロボットハンドの先端部分の位置が検出される。

請求項５に記載の発明は、被検出部の周囲に配置された複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出するステップと、前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出するステップとを有することを特徴とする位置検出方法である。

請求項６に記載の発明は、コンピュータに、被検出部の周囲に配置された複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出する処理と、前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出する処理とを実行させることを特徴とするプログラムである。

請求項１に記載の発明によれば、被検出部周辺の環境の情報を取り込んだ上での被検出部の音速の算出が行われるので、被検出部付近の音速をできるだけ正確に測定できる。このため、被検出部付近の音速を用いての位置の検出精度を高めることができる位置検出装置が提供される。

請求項２に記載の発明によれば、多面体の頂点に送受信機が位置し、この頂点を結んだ線分が被検出部の近くを通るため、被検出部付近の環境の情報をより正確に取り込んだ位置の検出が可能となる。

請求項３に記載の発明によれば、多面体を複数とすることで、検出対象となる空間をより細かく設定でき、位置の検出精度を高めることができる。

請求項４に記載の発明によれば、ロボットハンドの先端部分の位置を精度よく検出することができる。

請求項５に記載の発明によれば、被検出部周辺の環境の情報を取り込んだ上での被検出部の音速の算出が行われるので、被検出部付近の音速をできるだけ正確に測定できる。このため、被検出部付近の音速を用いての位置の検出精度を高めることができる位置の検出方法が提供される。

請求項６に記載の発明によれば、被検出部周辺の環境の情報を取り込んだ上での被検出部の音速の算出が行われるので、被検出部付近の音速をできるだけ正確に測定できる。このため、被検出部付近の音速を用いての位置の検出精度を高めることができる位置検出を行うためのプログラムが提供される。

実施形態の概念図である。 実施形態のブロック図である。 位置の検出を行う手順の一例を示すフローチャートである。 検出位置Ｐと頂点Ａ１とを結ぶ線上における音速を算出する原理を示す原理図である。 ロボットハンドの制御の一例を示すフロー図である。

（概要）
本実施形態では、図１に示すように、ロボットハンド１０１の先端部分１０４に超音波送信装置を配置し、さらに先端部分１０４を含むことが可能な多面体１３１〜１３３を設定し、それらの頂点に超音波送受信装置１１１〜１２６を配置する。先端部分１０４の位置検出にあたり、先端部分１０４に近い頂点を特定点として選択し、この特定点に向かう他の頂点からの経路の音速を算出する。この音速に基づいて、先端部分１０４から特定点への経路における音速を算出し、この音速の値と、先端部分１０４と特定点との間における超音波の伝播時間とに基づいて、先端部分１０４の特定点からの距離を算出する。この距離を複数の特定点について得ることで、先端部分１０４の位置を特定する。

（全体の構成）
図１は、実施形態の概念図である。図１には、ロボットハンドシステム１００が示されている。ロボットハンドシステム１００は、ロボットハンド１０１を備えている。ロボットハンド１０１は、その先端に複数種類のねじ回しを備えており、組み付けた部品の螺子を回す機能を備えている。ロボットハンド１０１の機能は、特に限定されず、部品の搬送、組み付け、塗装、溶接等、既存の機能のものを選択することが可能である。

ロボットハンド１０１は、ロボット制御装置１０２によってその位置が制御される。ロボット制御装置１０２は、位置検出装置１０３からロボットハンド１０１の先端部分１０４の３次元空間内における位置情報を受け取り、それに基づいて、ロボットハンド１０１の位置制御を行う。また、ロボット制御装置１０２は、位置検出装置１０３にロボットハンドの先端部分１０４の制御上における位置情報を出力する。

ロボットハンド１０１の先端部分１０４には、超音波送信装置が配置されている。この超音波送信装置は、位置検出装置１０３によりその動作が制御される。また、位置検出装置１０３は、１６個の超音波送受信装置１１１〜１２６の動作を制御すると共にそれらからの出力信号を処理する。なお、ここで利用する超音波の周波数は、特に限定されないが、例えば数十ｋＨｚのものが利用される。

超音波送受信装置１１１〜１２６は、３つの正六面体１３１〜１３３を連結した直方体の検出空間を構成している。正六面体１３１〜１３３は、単位検出空間として機能する。正六面体１３１〜１３３ぞれぞれの各頂点には、超音波送受信装置１１１〜１２６が配置されている。

すなわち、超音波送受信装置１１１、１１２、１１３、１１４、１１５、１１６、１１７および１１８は、正六面体１３１の８箇所の頂点に配置されている。超音波送受信装置１１５、１１６、１１７、１１８、１１９、１２０、１２１および１２２は、正六面体１３２の８箇所の頂点に配置されている。超音波送受信装置１１９、１２０、１２１、１２２、１２３、１２４、１２５および１２６は、正六面体１３３の８箇所の頂点に配置されている。

これら３つの正六面体１３１〜１３３により構成される立体空間内において、ロボットハンド１０１の先端部１０４の３次元位置が検出される。この検出のための制御系の構成および検出の方法については後述する。

（制御系な構成）
図２は、図１のロボットハンドシステムの制御系の概要を示すブロック図である。図２には、図１の位置検出装置１０３が示されている。位置検出装置１０３は、コンピュータとしての機能を有し、ＣＰＵ、メモリ、インターフェースを備えている。また、位置検出装置１０３は、後述する動作を実行するためのプログラムをメモリ内に記憶している。

位置検出装置１０３は、超音波送受信装置制御部２０１、音速算出部２０２、距離算出部２０３、位置算出部２０４、バッファメモリ部２０５を備えている。超音波送受信装置制御部２０１は、正六面体１３１〜１３３の内部における音速の検出に際して、超音波送受信装置１１１〜１２６の動作を制御する。

具体的にいうと、音速の検出が正六面体１３１の内部で行われる場合には、超音波送受信装置１１１、１１２、１１３、１１４、１１５、１１６、１１７および１１８の制御が、超音波送受信装置制御部２０１において行われる。音速の検出が正六面体１３２の内部で行われる場合には、超音波送受信装置１１５、１１６、１１７、１１８、１１９、１２０、１２１および１２２の制御が、超音波送受信装置制御部２０１において行われる。音速の検出が正六面体１３２の内部で行われる場合には、超音波送受信装置１１９、１２０、１２１、１２２、１２３、１２４、１２５および１２６の制御が、超音波送受信装置制御部２０１において行われる。また、超音波送受信装置制御部２０１は、ロボットハンド１０１の先端部分１０４に配置された超音波送信装置２０６の動作を制御する。

音速算出部２０２は、正六面体１３１〜１３３の中における音速の算出、およびこの算出に必要な演算を行う。音速算出部２０２において行われる処理の内容については後述する。距離算出部２０３は、音速算出部２０２における算出結果に基づいて、被検出位置（先端部分１０４）と計測基準となる特定の超音波送受信装置（この例では複数の頂点から選ばれた複数）との間の距離を算出する。

位置算出部２０４は、距離算出部２０３における算出結果に基づいて、被検出位置の３次元位置データ（３次元位置座標）を算出する。この座標データは、ロボット制御装置１０２に送られ、ロボットハンドの位置制御に利用される。バッファメモリ部２０５は、位置検出装置１０３内で行われる演算においてデータを一時的に記憶するワーキングエリアとして機能する。

（位置を検出するための処理）
以下、ロボットハンド１０１の先端部分１０４の位置を検出する処理の詳細な手順の一例を説明する。図３は、図２の位置検出装置１０３において行われる処理の手順を示すフローチャートである。図３に示す処理を実行するプログラムは、位置検出装置１０３内のメモリ領域に格納され、適当なメモリ領域に読み出されて位置検出装置１０３内のＣＰＵにより実行される。この動作プログラムは、外部の記憶媒体に記憶され、そこから提供される形態のものであってもよい。

先端部分１０４の位置を算出するための処理が開始されると（ステップＳ３０１）、まず検出位置（先端部分１０４）を含む単位測定空間が選択される（ステップＳ３０２）。この処理は、音速算出部２０２において行われる。ここで選択可能な単位測定空間は、図１の正六面体１３３、１３２、１３３の３つである。ステップＳ３０２では、ロボット制御装置１０２から得られる制御上の先端部分１０４の位置情報を参照し、検出位置（先端部分１０４）がいずれの正六面体に含まれているかが検索される。ロボット制御装置１０２から取得する先端部分１０４の位置データは、その位置に先端部分１０４を位置させるべく制御を行うためのデータに基づくもので、実際の位置からの誤差を含んでいる。

以下、ステップＳ３０３以下の処理を具体的な例示に即して説明する。図４は、処理の一例を説明するための概念図である。図４には、ロボットハンド１０１の先端部分１０４に相当する検出位置Ｐが示されている。また図４には、図１の正六面体１３１〜１３３の内のいずれかに相当する正六面体４００が示されている。

図４の正六面体４００が、例えば図１の正六面体１３２である場合、頂点Ａ１に図１の超音波送受信装置１１６が配置され、頂点Ａ２に超音波送受信装置１１８が配置され、頂点Ａ３に超音波送受信装置１１７が配置され、頂点Ａ４に超音波送受信装置１１５が配置され、頂点Ａ５に超音波送受信装置１２０が配置され、頂点Ａ６に超音波送受信装置１２２が配置され、頂点Ａ７に超音波送受信装置１２１が配置され、頂点Ａ８に超音波送受信装置１１９が配置される関係となる。

ステップＳ３０２の後、検出位置に近い上位３点の頂点を特定点として選択する（ステップＳ３０３）。図４の例でいうと、検出位置Ｐに近い上位３つの特定点として、頂点Ａ１、Ａ２、Ａ３が選択される。この処理は、音速算出部２０２において行われる。

ステップＳ３０３の処理において、検出位置Ｐと特定点との間にワークやロボットハンド１０１のアーム部等の障害物がはいる場合、検出位置Ｐに次に近い頂点を特定点として選択する。これは、超音波の伝播が障害物によって乱れることに起因する検出誤差を抑えるためである。

特定点を３点選んだら、その中の一つと他の頂点（ステップＳ３０２で選択した正六面体の各頂点）とを結ぶ線分の中から、検出位置に近い線分を上位３つの中から選択する（ステップＳ３０４）。図４の例でいうと、特定点Ａ１に関しては線分Ｃ_１、Ｃ_２、Ｃ_３が選択される。この処理は、音速算出部２０２において行われ、処理の結果は、バッファメモリ部２０５に記憶される。この処理は、３つの特定点（Ａ１、Ａ２、Ａ３）の全てにおいて行われる。

この際、線分Ｃ_１、Ｃ_２、Ｃ_３の間に障害物（ワークやロボットハンドのアーム部）が位置し、見通しが遮られない線分を選択する。見通しが遮られる線分がある場合、再度特定点の選択がやり直され、次に検出位置Ｐに近い線分が選択される。

次に、各特定点において、ステップＳ３０４において選択した３つの線分における当該特定点に向かう方向の音速を算出する（ステップＳ３０５）。図４の例でいうと、線分Ｃ_１におけるＡ１に向かう音速、線分Ｃ２におけるＡ１に向かう音速、線分Ｃ３におけるＡ１に向かう音速が算出される。例えば、線分Ｃ_１における音速は、位置が既知であるＡ１点とＡ３点との間で、両点の超音波送受信装置を動作させ、Ａ３点からＡ１点に向かう方向における音波の伝播時間から算出される。この際の超音波送受信装置の動作制御は、超音波送受信装置制御部２０１において行われる。処理の結果は、バッファメモリ部２０５に記憶される。同様の処理は、線分Ｃ_２、Ｃ_３に対しても行われる。これらの処理は、特定点Ａ２、Ａ３においても行われる。

次に、検出位置から３つの特定点に向かう方向の音速（検出位置における音速が含まれる）を音速算出部２０２において算出する（ステップＳ３０６）。図４の例でいうと、検出位置Ｐから頂点Ａ１に向かう方向の音速を算出し、検出位置Ｐから頂点Ａ２に向かう方向の音速を算出し、検出位置Ｐから頂点Ａ３に向かう方向の音速を算出する。

この際、検出位置Ｐと特定点とを結ぶ方向における音速を直接求めることはできない。なぜならば、検出位置Ｐの位置は制御データから得られた不正確な位置であるからである。そこで、図４の例では、例えば検出位置Ｐと特定点Ａ１との間を結ぶ線上の特定点Ａ１に向かう方向の音速を、線分Ｃ_１上のＡ１に向かう音速、線分Ｃ_２上のＡ１に向かう音速、線分Ｃ_３上のＡ１に向かう音速から近似値として算出する。

この算出では、検出位置Ｐと特定点Ａ１とを結ぶ方向におけるＡ１に向かう音速に対して、検出位置Ｐに最も近い線分Ｃ_２におけるＡ１に向かう音速の影響（関係）が最も大であり、次に検出位置Ｐに近い線分Ｃ_３におけるＡ１に向かう音速の影響が中であり、次に検出位置Ｐに近い線分Ｃ_１におけるＡ１に向かう音速の影響が小であると見積もり、各線分におけるＡ１に向かう音速に重み付けをした計算式を立てる。これにより、近似式ではあるが、検出位置Ｐと特定点Ａ１とを結ぶ方向におけるＡ１に向かう音速を算出する。同様にして、検出位置Ｐから頂点Ａ２に向かう方向の音速の算出、検出位置Ｐから頂点Ａ３に向かう方向の音速の算出も行われる。

以下、図４の場合を例に挙げ、具体的な計算式を説明する。まず図４において、Ｌ_１は、検出位置Ｐから線分Ｃ_１に降ろした垂線である。Ｌ_２は、検出位置Ｐから線分Ｃ_２に降ろした垂線である。Ｌ_３は、検出位置Ｐから線分Ｃ_３に降ろした垂線である。また、図４の例では、Ｌ_２＜Ｌ_３＜Ｌ_１が成立している。この条件において、線分Ｃ_１におけるＡ１に向かう音速をｖ１、線分Ｃ_２におけるＡ１に向かう音速をｖ２、線分Ｃ_３におけるＡ１に向かう音速をｖ３とすると、検出位置Ｐから頂点Ａ１に向う方向における音速Ｖは、下記数１から算出される。

数１においては、ｖ１、ｖ２、ｖ３の係数として、線分Ｃ_１〜Ｃ_３の検出位置Ｐに対する距離に応じた値を採用し、それによりｖ１、ｖ２、ｖ３のＶへの影響を重み付けする形としている。

これは、以下の考え方に基づいている。まず、線分Ｃ_２上の音速ｖ２のベクトルは、線分Ｃ_２がＰとＡ１とを結ぶ線分に最も近い（なす角度が最も小さく、位置も最も近い）ので、最もＶのベクトルに近い。次に線分Ｃ_３上の音速ｖ３のベクトルは、線分Ｃ_３がＰとＡ１とを結ぶ線分に次に近い（なす角度が２番目に小さく、位置も２番目に近い）ので、２番目にＶのベクトルに近い。更に線分Ｃ_１上の音速ｖ１のベクトルは、線分Ｃ_１がＰとＡ１とを結ぶ線分に３番目に近い（なす角度が３番目に小さく、位置も３番目に近い）ので、３番目にＶのベクトルに近い。したがって、Ｖは、距離に応じた重み付け係数を用いたｖ１、ｖ２、ｖ３の一次結合式で近似的に示すことができる。これが、上記数１である。

ステップＳ３０６において、検出位置から各特定点に向かう線上における音速を算出したら、超音波送受信装置制御部２０１から制御信号を出力し、ロボットハンド１０１（図１参照）の先端部１０４（検出位置）に配置された超音波送信装置２０６から超音波信号を出力させ、それを３つの特定点で受信する。そしてステップＳ３０５で算出した音速と、超音波送信装置２０６からの発信時刻と各特定点における受信時刻との差（つまり音波が伝わるのに要した時間）から、検出位置と３つの特定点との間の距離が算出される（ステップＳ３０８）。この処理は、距離算出部２０３において行われる。

図４の場合でいうと、検出位置Ｐと特定点Ａ１との間の距離ｒ１、検出位置Ｐと特定点Ａ２との間の距離ｒ２、検出位置Ｐと特定点Ａ３との間の距離ｒ３が算出される。

次いで、３点の特定点から検出位置までの距離を半径とした球を描き、その表面の交点を求める。この交点は、２点あるので、予定している検出位置（ロボットハンド１０１の先端部１０４の位置）に近い方（あるいは選択した正六面体内にある方）をロボットハンド１０１の先端部１０４の位置データとして取得する（ステップＳ３０９）。この処理は、位置算出部２０４において行われる。この位置を特定する原理は、ＧＰＳ(Global Positioning System)と同じである。

（ロボットハンドの位置制御）
図５は、ロボット制御装置１０２において行われるロボットハンド１０１の位置制御の流れを示すフロー図である。図５において、ワールド座標系というのは、ロボットハンド１０１が設置されている環境（ロボットハンドシステム１００）に固定されている座標系（例えば、３次元直交座標系や極座標系）である。ロボット座標系というのは、各関節軸を動かす際に利用されるロボット側に固定された座標系である。

位置制御にあたっては、まずワールド座標系での目標位置が設定される（Ｓ５０１）。この目標位置は、ステップＳ５０２に出力されると共に、ステップＳ５０７のズレ量を計算するステップに与えられる。ステップＳ５０２では、ステップ５０７で算出された目標位置と実測位置とのズレ量が与えられ、このズレ量とステップＳ５０１で与えられた目標位置のデータとが、ステップＳ５０３に与えられる。

ステップＳ５０３では、目標位置に対するズレを勘案した座標データが与えられ、これがロボット座標系に変換される。そして、変換されたロボット座標系の座標データに基づいて、各関節部を駆動するモータに動作指令が行われ（ステップＳ５０４）、ロボットハンド１０１（図１参照）の位置制御が行われる（ステップＳ５０５）。

この際、ロボットハンド１０１の先端部分１０４のワールド座標系における位置情報が、位置検出装置１０３における図３の処理により検出され（ステップＳ５０６）、それに基づき、先端部分１０４の目標とした位置と実際の位置との差（ズレ量）が算出される（ステップＳ５０７）。このズレ量は、ステップＳ５０２に与えられ、ロボットハンド１０１の位置制御に利用される。

この処理により、ロボットハンド１０１の先端部分１０４の位置の目標位置とのズレを補正しながらの位置制御が行われる。

（優位性）
以上示した実施形態によれば、ロボットハンド１０１の先端部分１０４の位置の検出が超音波を利用して行われる。この際、先端部分１０４から特定の受信装置までの音波の伝播経路における音速が、設定された単位検出空間（この例の場合正六面体）の各頂点に配置された超音波送受信装置間の音速に基づいて算出される。

この例では、先端部分１０４から特定の受信装置までの経路に近い３本の基準経路の音速を用い、しかも当該経路との距離に応じて重み付けされた計算式によって、上記３本の基準経路（図４のＣ_１、Ｃ_２、Ｃ_３）の当該経路への影響が計算される。このため、近似的ではあるが、検出位置Ｐ（先端部分１０４）から特定の受信装置への経路における温度、湿度、密度、空気の流れといったパラメータの影響を受けた実際の値に近い音速を得ることができる。これにより、これらパラメータの時間的な変化や場所的な変化の影響を低減した状態で、先端部分１０４の位置の検出が行われる。つまり、温度や風の影響による音速の違いに起因する先端部分１０４の位置の検出誤差を抑えることができる。

また実施形態で示した方法では、先端部分１０４に近い上位３つの基準経路の音速を利用するので、検出位置Ｐの環境の状態をより正確に反映した形で、検出位置Ｐから特定の受信装置に向かう方向における音速（検出位置における音速）の値を算出できる。また、各基準経路の方向と、検出位置から特定の受信装置への経路の方向とのなす角度が４５度以下となるので、方向の違いによる音速の違いがある場合でも、精度良く検出位置から特定の受信装置に向かう方向における音速（検出位置における音速）の値を算出できる。

また、正六面体１３１〜１３３で示されるように、単位検出空間を複数用意しているので、より詳細な音速の変化を捉えることができる。これにより、場所による温度変化や空気の流れの影響が大きい環境であっても、先端部分１０４の位置の検出精度を高く保つことができる。

図４の例において、検出位置ＰからＡ１の位置の受信装置までの音速を算出する際に、この経路に最も近い基準経路Ｃ_２におけるＡ１に向かう音速の影響を最も大きく見積もり、次に２番目に近い基準経路Ｃ_３におけるＡ１に向かう音速の影響をその距離の違いに応じた重み付けで与え、更に、３番目に近い基準経路Ｃ_１におけるＡ１に向かう音速の影響をその距離の違いに応じた重み付けで与えている（数１参照）。これにより、検出位置Ｐ付近の雰囲気（温度や風等の状態）をより正確に反映した検出位置Ｐにおける頂点Ａ１方向の音速を得ることができる。

また、検出位置ＰからＡ１の位置の受信装置までの音速Ｖを算出する際に、基準経路Ｃ_１、Ｃ_２、Ｃ_３として、Ａ１を頂点とし、その内側に検出位置Ｐを含む三角錐の各辺を選択するので、各基準経路における音速から得られる音速Ｖの精度をより高くできる。

すなわち、検出位置ＰからＡ１の位置の受信装置までの音速Ｖを算出する際に、基準経路Ｃ_１、Ｃ_２、Ｃ_３におけるＡ１に向かう音速は、音速Ｖの方向と近い方向であるので、基準経路Ｃ_１、Ｃ_２、Ｃ_３における音速（ｖ１、ｖ２、ｖ３）から算出される音速Ｖの実際の値との誤差を抑えることができる。特にこの方法は、空気の流れの向きの影響（風の影響）を考慮にいれることができる。

また、特定点として、Ａ１、Ａ２、Ａ３を検出位置Ｐに近い上位３つから選ぶことで、音波の伝播距離が長くなることによる誤差の増大が抑えられる。

また、特定点をＡ１、Ａ２、Ａ３と３つ選択し、それらを中心とする３つの球の交差位置から検出位置Ｐを求めることで、３次元上の正確な位置の検出が行われる。

（その他）
上記の例示では、単位検出空間として正六面体を採用する例を示したが、多面体であれば、正六面体以外の形状も利用可能である。音波を伝える媒体は、空気に限定されず、気体であれば、不活性気体等であってもよい。また、霧や煙が存在する雰囲気において本発明を適用することもできる。位置の検出は、ロボットハンドの先端ではなく、ロボットハンドの間接部分等であってもよい。また、ロボットハンド以外に、ワークや治具の３次元的な位置を検出する目的に本発明を利用することもできる。

図１には、ロボットハンドを一つ供えたシステムが示されているが、ロボットハンドの数は複数であってもよい。また、単位検出空間（正六面体１３１〜１３３）の数を更に増やしてもよい。また、単位検出空間（正六面体１３１〜１３３）の数が一つであってもよい。

図４に示す例示では、頂点Ａ１、Ａ２、Ａ３の３点から検出位置Ｐまでの距離を算出することで、検出位置Ｐの位置を特定する例を説明したが、基準となる頂点（特定点）は、４箇所以上であってもよい。また、頂点Ａ１のみ、頂点Ａ１とＡ２のみを選択し、そこから検出位置Ｐまでの距離を算出し、その結果に基づいて検出位置Ｐの位置を特定する構成としてもよい。

検出位置までの距離を算出するための基準点を４点以上選択した場合、位置の算出精度をより高くすることができる。検出位置までの距離を算出するための基準点を２点とした場合、これら２点のそれぞれを中心とする球面の交差する円上に検出位置が存在することになる。この場合、他の手段（例えばロボットアームの制御データ）から得られる予測位置の修正に、上記円上に検出位置がある旨の算出データを利用することで、位置検出の制度を高めることができる。

検出位置までの距離を算出するための基準点を１点とした場合、この点を中心とする球面上に検出位置が存在することになる。この場合、他の手段（例えばロボットアームの制御データ）から得られる予測位置の修正に、上記球面上に検出位置がある旨の算出データを利用することで、位置検出の制度を高めることができる。

本発明は、超音波を用いた位置の検出を行う技術に利用することができる。また、超音波を用いたロボットハンドの特定の位置を検出する技術に利用することができる。

１００…ロボットハンドシステム、１０１…ロボットハンド、１０２…ロボット制御装置、１０３…位置検出装置、１０４…ロボットハンドの先端部分、１１１〜１２６…超音波送受信装置（正六面体の頂点）、１３１〜１３３…正六面体、４００…正六面体。

Claims (6)

1. 被検出部に配置された超音波送信手段と、
   前記被検出部の周囲に配置された複数の超音波送受信手段と、
   前記複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出する音速算出手段と、
   前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出する距離算出手段と
   を備えることを特徴とする位置検出装置。
2. 前記複数の超音波送受信手段は多面体の頂点に配置されており、
   前記多面体の内部に前記被検出部が位置することを特徴とする請求項１に記載の位置検出装置。
3. 前記複数の超音波送受信手段は複数の多面体の頂点に配置されており、
   前記複数の多面体のうちの一つに前記被検出部が含まれ、
   前記音速算出手段は、前記被検出部を含む多面体の複数の頂点に位置する複数の前記超音波送受信手段の間における音速から前記被検出部における音速の算出を行うことを特徴とする請求項１に記載の位置検出装置。
4. 前記被検出部はロボットハンドの先端部分であることを特徴とする請求項１〜３のいずれか一項に記載の位置検出装置。
5. 被検出部の周囲に配置された複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出するステップと、
   前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出するステップと
   を有することを特徴とする位置検出方法。
6. コンピュータに、
   被検出部の周囲に配置された複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出する処理と、
   前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出する処理と
   を実行させることを特徴とするプログラム。

Images