(57)【要約】【課題】被検出部周辺の環境を考慮した被検出部の音速の算出を行い、位置の検出対象とする部分付近の音速をできるだけ正確に測定し、それを用いての位置の検出精度を高める。【解決手段】ロボットハンド１０１の先端部分１０４に超音波送信装置を配置し、さらに先端部分１０４を含むことが可能な多面体１３１〜１３３を設定し、それらの頂点に超音波送受信装置１１１〜１２６を配置する。先端部分１０４の位置検出にあたり、先端部分１０４に近い頂点を特定点として選択し、この特定点に向かう他の頂点からの経路の音速を算出する。この音速に基づいて、先端部分１０４から特定点への経路における音速を算出し、この音速の値と、先端部分１０４と特定点との間における超音波の伝播時間とに基づいて、先端部分１０４の特定点からの距離を算出する。この距離を複数の特定点について得ることで、先端部分１０４の位置を特定する。【選択図】図１
2011038801.tif 000002
【特許請求の範囲】
【請求項１】
被検出部に配置された超音波送信手段と、
前記被検出部の周囲に配置された複数の超音波送受信手段と、
前記複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出する音速算出手段と、
前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出する距離算出手段と
を備え、
前記超音波送受信装置間を結ぶ線分は複数あり、
前記複数の超音波送受信手段の間の音速として、前記複数の線分における音速が算出され、
前記被検出部における音速を算出する処理において、
前記複数の線分における音速のうち、前記被検出部に相対的に近い線分における音速が前記被検出部における音速に相対的に大きく影響し、前記被検出部に相対的に遠い線分における音速が前記被検出部における音速に相対的に小さく影響する処理が行われることを特徴とする位置検出装置。
【請求項２】
前記被検出部に相対的に近い線分における音速は、前記被検出部に最も近い線分における音速であることを特徴とする請求項１に記載の位置検出装置。
【請求項３】
前記複数の線分は、三角錐を構成し、
前記三角錐の中に前記被検出部が含まれることを特徴とする請求項１または２に記載の位置検出装置。
【請求項４】
前記音速算出手段における前記被検出部における音速の算出は、
前記被検出部から前記複数の線分のうちの一つに対する垂線の長さをＬi（ｉは０を含まない自然数）、
Ｌ＝Ｌ１＋Ｌ２＋Ｌ３＋・・・、
前記複数の線分のそれぞれにおける音速をｖｉ（ｉは０を含まない自然数）
として、
【数１】
2011038801.tif 000005
により行われることを特徴とする請求項１〜３のいずれか一項に記載の位置検出装置。
【請求項５】
前記被検出部はロボットハンドの先端部分であることを特徴とする請求項１〜４のいずれか一項に記載の位置検出装置。
【請求項６】
被検出部の周囲に配置された複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出するステップと、
前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出するステップと
を有し、
前記超音波送受信装置間を結ぶ線分は複数あり、
前記複数の超音波送受信手段の間の音速として、前記複数の線分における音速が算出され、
前記音速を算出するステップにおいて、
前記複数の線分における音速のうち、前記被検出部に相対的に近い線分における音速が前記被検出部における音速に相対的に大きく影響し、前記被検出部に相対的に遠い線分における音速が前記被検出部における音速に相対的に小さく影響する処理が行われることを特徴とする位置検出方法。
【請求項７】
コンピュータに、
被検出部の周囲に配置された複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出する処理と、
前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出する処理と
を実行させ、
前記超音波送受信装置間を結ぶ線分は複数あり、
前記複数の超音波送受信手段の間の音速として、前記複数の線分における音速が算出され、
前記被検出部における音速を算出する処理において、
前記複数の線分における音速のうち、前記被検出部に相対的に近い線分における音速が前記被検出部における音速に相対的に大きく影響し、前記被検出部に相対的に遠い線分における音速が前記被検出部における音速に相対的に小さく影響する処理を行わすことを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットアーム先端の位置等を超音波により検出する技術に係り、音速の影響による検出誤差を低減する技術に関する。
【背景技術】
【０００２】
超音波を用いて、物体の３次元位置を検出する技術が知られている（例えば、特許文献１参照）。この技術では、例えば、検出対象となる部分に超音波発生装置を配置し、そこから発生する超音波信号を異なる３箇所で受信する。この際、３箇所の受信側において、超音波の空気中を伝わる時間を計測することで、これら３箇所から検出対象までの距離ｒ１〜ｒ３が割り出される。そして、これら３箇所を中心とした半径ｒ１、ｒ２、ｒ３の球を描き、それら球の表面が交差する点が検出対象の位置として特定される。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平６−１０２３５０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上述した超音波を用いた位置の特定技術は、音波の速度の影響を受ける。音速は、空気中の温度、圧力、湿度、密度（気圧）、流れ（風）の影響を受ける。したがって、上記音速に影響を与えるパラメータが一定でない環境では、音波を用いた位置の検出は、精度が低下する。
【０００５】
例えば、ロボットアームを用いた製造技術現場において、ロボットアームの先端の位置を検出する技術が必要とされる。このような環境においては、温度、湿度、空気の流れ等の変化が、場所や時間によって変化する場合がある。この場合、音波を用いた位置の検出技術では、上述した音速の変化に起因して、検出した位置のデータに誤差が生じる。
【０００６】
このような背景において、本発明は、位置の検出対象とする部分付近の音速をできるだけ正確に測定し、それを用いての位置の検出精度を高める技術を提供することを目的とする。
【課題を解決するための手段】
【０００７】
請求項１に記載の発明は、被検出部に配置された超音波送信手段と、前記被検出部の周囲に配置された複数の超音波送受信手段と、前記複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出する音速算出手段と、前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出する距離算出手段とを備え、前記超音波送受信装置間を結ぶ線分は複数あり、前記複数の超音波送受信手段の間の音速として、前記複数の線分における音速が算出され、前記被検出部における音速を算出する処理において、前記複数の線分における音速のうち、前記被検出部に相対的に近い線分における音速が前記被検出部における音速に相対的に大きく影響し、前記被検出部に相対的に遠い線分における音速が前記被検出部における音速に相対的に小さく影響する処理が行われることを特徴とする位置検出装置である。
【０００８】
請求項１に記載の発明では、まず被検出部の周囲に配置された複数の超音波送受信手段間における音速が複数の経路上で算出される。そして、これら複数の経路上における音速の影響を当該経路と被検出部との間の距離に応じた重み付けを与えた状態に基づいて、被検出部における音速の算出が行われる。更に、被検出部から発信される超音波を特定の超音波送受信手段で受信し、上記の被検出部における音速とその伝播時間からその間の距離ｒを算出する。被検出部は、前記特定の超音波送受信手段を中心とする半径ｒの球の球面上に位置するので、このことに基づいて被検出部の位置が特定される。
【０００９】
請求項２に記載の発明は、請求項１に記載の発明において、前記被検出部に相対的に近い線分における音速は、前記被検出部に最も近い線分における音速であることを特徴とする。
【００１０】
請求項３に記載の発明は、請求項１または２に記載の発明において、前記複数の線分は、三角錐を構成し、前記三角錐の中に前記被検出部が含まれることを特徴とする。
【００１１】
請求項４に記載の発明は、請求項１〜３のいずれか一項に記載の発明において、前記音速算出手段における前記被検出部における音速の算出は、前記被検出部から前記複数の線分のうちの一つに対する垂線の長さをＬi（ｉは０を含まない自然数）、Ｌ＝Ｌ１＋Ｌ２＋Ｌ３＋・・・、前記複数の線分のそれぞれにおける音速をｖｉ（ｉは０を含まない自然数）として、下記数１により行われることを特徴とする。
【００１２】
【数１】
2011038801.tif 000003
【００１３】
請求項５に記載の発明は、請求項１〜４のいずれか一項に記載の発明において、前記被検出部はロボットハンドの先端部分であることを特徴とする。
【００１４】
請求項６に記載の発明は、被検出部の周囲に配置された複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出するステップと、前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出するステップとを有し、前記超音波送受信装置間を結ぶ線分は複数あり、前記複数の超音波送受信手段の間の音速として、前記複数の線分における音速が算出され、前記音速を算出するステップにおいて、前記複数の線分における音速のうち、前記被検出部に相対的に近い線分における音速が前記被検出部における音速に相対的に大きく影響し、前記被検出部に相対的に遠い線分における音速が前記被検出部における音速に相対的に小さく影響する処理が行われることを特徴とする位置検出方法である。
【００１５】
請求項６に記載の発明は、コンピュータに、被検出部の周囲に配置された複数の超音波送受信手段の間の音速を算出すると共に、前記複数の超音波送受信手段の間の音速に基づいて、前記被検出部における音速を算出する処理と、前記被検出部に配置された超音波送信手段から前記複数の超音波送受信手段に含まれる特定の超音波送受信手段までの超音波の伝播時間と、前記被検出部における音速とに基づいて、前記被検出部に配置された前記超音波送信手段から前記特定の超音波送受信手段までの距離を算出する処理とを実行させ、前記超音波送受信装置間を結ぶ線分は複数あり、前記複数の超音波送受信手段の間の音速として、前記複数の線分における音速が算出され、前記被検出部における音速を算出する処理において、前記複数の線分における音速のうち、前記被検出部に相対的に近い線分における音速が前記被検出部における音速に相対的に大きく影響し、前記被検出部に相対的に遠い線分における音速が前記被検出部における音速に相対的に小さく影響する処理を行わすことを特徴とするプログラムである。
【発明の効果】
【００１６】
請求項１に記載の発明によれば、被検出部周辺の環境の情報を取り込んだ上での被検出部の音速の算出において、被検出部からの距離に応じた影響度を考慮しての環境の状態の取り込みが行われるので、被検出部付近の音速の検出精度が高くなる。このため、被検出部付近の音速を用いての位置の検出精度を高めることができる位置検出装置が提供される。
【００１７】
請求項２に記載の発明によれば、被検出部に近い部分における環境の情報をより正確に取り込んだ位置の検出が可能となる。
【００１８】
請求項３に記載の発明によれば、三角錐の斜辺における音速に基づいて、三角錐の内部にある被検出部における音速の算出が行われるので、被検出部における音速の算出精度を更に高めることができる。
【００１９】
請求項４に記載の発明によれば、距離により影響度の違いを反映させた計算式により、被検出部における音速を求めることができる。
【００２０】
請求項５に記載の発明によれば、ロボットハンドの先端部分の位置検出精度を高めることができる。
【００２１】
請求項６に記載の発明によれば、被検出部周辺の環境の情報を取り込んだ上での被検出部の音速の算出において、被検出部からの距離に応じた影響度を考慮しての環境の状態の取り込みが行われるので、被検出部付近の音速の検出精度が高くなる。このため、被検出部付近の音速を用いての位置の検出精度を高めることができる位置検出方法が提供される。
【００２２】
請求項７に記載の発明によれば、被検出部周辺の環境の情報を取り込んだ上での被検出部の音速の算出において、被検出部からの距離に応じた影響度を考慮しての環境の状態の取り込みが行われるので、被検出部付近の音速の検出精度が高くなる。このため、被検出部付近の音速を用いての位置の検出精度を高めることができるプログラムが提供される。
【図面の簡単な説明】
【００２３】
【図１】実施形態の概念図である。
【図２】実施形態のブロック図である。
【図３】位置の検出を行う手順の一例を示すフローチャートである。
【図４】検出位置Ｐと頂点Ａ１とを結ぶ線上における音速を算出する原理を示す原理図である。
【図５】ロボットハンドの制御の一例を示すフロー図である。
【発明を実施するための形態】
【００２４】
（概要）
本実施形態では、図１に示すように、ロボットハンド１０１の先端部分１０４に超音波送信装置を配置し、さらに先端部分１０４を含むことが可能な多面体１３１〜１３３を設定し、それらの頂点に超音波送受信装置１１１〜１２６を配置する。先端部分１０４の位置検出にあたり、先端部分１０４に近い頂点を特定点として選択し、この特定点に向かう他の頂点からの経路の音速を算出する。この音速に基づいて、先端部分１０４から特定点への経路における音速を算出し、この音速の値と、先端部分１０４と特定点との間における超音波の伝播時間とに基づいて、先端部分１０４の特定点からの距離を算出する。この距離を複数の特定点について得ることで、先端部分１０４の位置を特定する。
【００２５】
（全体の構成）
図１は、実施形態の概念図である。図１には、ロボットハンドシステム１００が示されている。ロボットハンドシステム１００は、ロボットハンド１０１を備えている。ロボットハンド１０１は、その先端に複数種類のねじ回しを備えており、組み付けた部品の螺子を回す機能を備えている。ロボットハンド１０１の機能は、特に限定されず、部品の搬送、組み付け、塗装、溶接等、既存の機能のものを選択することが可能である。
【００２６】
ロボットハンド１０１は、ロボット制御装置１０２によってその位置が制御される。ロボット制御装置１０２は、位置検出装置１０３からロボットハンド１０１の先端部分１０４の３次元空間内における位置情報を受け取り、それに基づいて、ロボットハンド１０１の位置制御を行う。また、ロボット制御装置１０２は、位置検出装置１０３にロボットハンドの先端部分１０４の制御上における位置情報を出力する。
【００２７】
ロボットハンド１０１の先端部分１０４には、超音波送信装置が配置されている。この超音波送信装置は、位置検出装置１０３によりその動作が制御される。また、位置検出装置１０３は、１６個の超音波送受信装置１１１〜１２６の動作を制御すると共にそれらからの出力信号を処理する。なお、ここで利用する超音波の周波数は、特に限定されないが、例えば数十ｋＨｚのものが利用される。
【００２８】
超音波送受信装置１１１〜１２６は、３つの正六面体１３１〜１３３を連結した直方体の検出空間を構成している。正六面体１３１〜１３３は、単位検出空間として機能する。正六面体１３１〜１３３ぞれぞれの各頂点には、超音波送受信装置１１１〜１２６が配置されている。
【００２９】
すなわち、超音波送受信装置１１１、１１２、１１３、１１４、１１５、１１６、１１７および１１８は、正六面体１３１の８箇所の頂点に配置されている。超音波送受信装置１１５、１１６、１１７、１１８、１１９、１２０、１２１および１２２は、正六面体１３２の８箇所の頂点に配置されている。超音波送受信装置１１９、１２０、１２１、１２２、１２３、１２４、１２５および１２６は、正六面体１３３の８箇所の頂点に配置されている。
【００３０】
これら３つの正六面体１３１〜１３３により構成される立体空間内において、ロボットハンド１０１の先端部１０４の３次元位置が検出される。この検出のための制御系の構成および検出の方法については後述する。
【００３１】
（制御系な構成）
図２は、図１のロボットハンドシステムの制御系の概要を示すブロック図である。図２には、図１の位置検出装置１０３が示されている。位置検出装置１０３は、コンピュータとしての機能を有し、ＣＰＵ、メモリ、インターフェースを備えている。また、位置検出装置１０３は、後述する動作を実行するためのプログラムをメモリ内に記憶している。
【００３２】
位置検出装置１０３は、超音波送受信装置制御部２０１、音速算出部２０２、距離算出部２０３、位置算出部２０４、バッファメモリ部２０５を備えている。超音波送受信装置制御部２０１は、正六面体１３１〜１３３の内部における音速の検出に際して、超音波送受信装置１１１〜１２６の動作を制御する。
【００３３】
具体的にいうと、音速の検出が正六面体１３１の内部で行われる場合には、超音波送受信装置１１１、１１２、１１３、１１４、１１５、１１６、１１７および１１８の制御が、超音波送受信装置制御部２０１において行われる。音速の検出が正六面体１３２の内部で行われる場合には、超音波送受信装置１１５、１１６、１１７、１１８、１１９、１２０、１２１および１２２の制御が、超音波送受信装置制御部２０１において行われる。音速の検出が正六面体１３２の内部で行われる場合には、超音波送受信装置１１９、１２０、１２１、１２２、１２３、１２４、１２５および１２６の制御が、超音波送受信装置制御部２０１において行われる。また、超音波送受信装置制御部２０１は、ロボットハンド１０１の先端部分１０４に配置された超音波送信装置２０６の動作を制御する。
【００３４】
音速算出部２０２は、正六面体１３１〜１３３の中における音速の算出、およびこの算出に必要な演算を行う。音速算出部２０２において行われる処理の内容については後述する。距離算出部２０３は、音速算出部２０２における算出結果に基づいて、被検出位置（先端部分１０４）と計測基準となる特定の超音波送受信装置（この例では複数の頂点から選ばれた複数）との間の距離を算出する。
【００３５】
位置算出部２０４は、距離算出部２０３における算出結果に基づいて、被検出位置の３次元位置データ（３次元位置座標）を算出する。この座標データは、ロボット制御装置１０２に送られ、ロボットハンドの位置制御に利用される。バッファメモリ部２０５は、位置検出装置１０３内で行われる演算においてデータを一時的に記憶するワーキングエリアとして機能する。
【００３６】
（位置を検出するための処理）
以下、ロボットハンド１０１の先端部分１０４の位置を検出する処理の詳細な手順の一例を説明する。図３は、図２の位置検出装置１０３において行われる処理の手順を示すフローチャートである。図３に示す処理を実行するプログラムは、位置検出装置１０３内のメモリ領域に格納され、適当なメモリ領域に読み出されて位置検出装置１０３内のＣＰＵにより実行される。この動作プログラムは、外部の記憶媒体に記憶され、そこから提供される形態のものであってもよい。
【００３７】
先端部分１０４の位置を算出するための処理が開始されると（ステップＳ３０１）、まず検出位置（先端部分１０４）を含む単位測定空間が選択される（ステップＳ３０２）。この処理は、音速算出部２０２において行われる。ここで選択可能な単位測定空間は、図１の正六面体１３３、１３２、１３３の３つである。ステップＳ３０２では、ロボット制御装置１０２から得られる制御上の先端部分１０４の位置情報を参照し、検出位置（先端部分１０４）がいずれの正六面体に含まれているかが検索される。ロボット制御装置１０２から取得する先端部分１０４の位置データは、その位置に先端部分１０４を位置させるべく制御を行うためのデータに基づくもので、実際の位置からの誤差を含んでいる。
【００３８】
以下、ステップＳ３０３以下の処理を具体的な例示に即して説明する。図４は、処理の一例を説明するための概念図である。図４には、ロボットハンド１０１の先端部分１０４に相当する検出位置Ｐが示されている。また図４には、図１の正六面体１３１〜１３３の内のいずれかに相当する正六面体４００が示されている。
【００３９】
図４の正六面体４００が、例えば図１の正六面体１３２である場合、頂点Ａ１に図１の超音波送受信装置１１６が配置され、頂点Ａ２に超音波送受信装置１１８が配置され、頂点Ａ３に超音波送受信装置１１７が配置され、頂点Ａ４に超音波送受信装置１１５が配置され、頂点Ａ５に超音波送受信装置１２０が配置され、頂点Ａ６に超音波送受信装置１２２が配置され、頂点Ａ７に超音波送受信装置１２１が配置され、頂点Ａ８に超音波送受信装置１１９が配置される関係となる。
【００４０】
ステップＳ３０２の後、検出位置に近い上位３点の頂点を特定点として選択する（ステップＳ３０３）。図４の例でいうと、検出位置Ｐに近い上位３つの特定点として、頂点Ａ１、Ａ２、Ａ３が選択される。この処理は、音速算出部２０２において行われる。
【００４１】
ステップＳ３０３の処理において、検出位置Ｐと特定点との間にワークやロボットハンド１０１のアーム部等の障害物がはいる場合、検出位置Ｐに次に近い頂点を特定点として選択する。これは、超音波の伝播が障害物によって乱れることに起因する検出誤差を抑えるためである。
【００４２】
特定点を３点選んだら、その中の一つと他の頂点（ステップＳ３０２で選択した正六面体の各頂点）とを結ぶ線分の中から、検出位置に近い線分を上位３つの中から選択する（ステップＳ３０４）。図４の例でいうと、特定点Ａ１に関しては線分Ｃ１、Ｃ２、Ｃ３が選択される。この処理は、音速算出部２０２において行われ、処理の結果は、バッファメモリ部２０５に記憶される。この処理は、３つの特定点（Ａ１、Ａ２、Ａ３）の全てにおいて行われる。
【００４３】
この際、線分Ｃ１、Ｃ２、Ｃ３の間に障害物（ワークやロボットハンドのアーム部）が位置し、見通しが遮られない線分を選択する。見通しが遮られる線分がある場合、再度特定点の選択がやり直され、次に検出位置Ｐに近い線分が選択される。
【００４４】
次に、各特定点において、ステップＳ３０４において選択した３つの線分における当該特定点に向かう方向の音速を算出する（ステップＳ３０５）。図４の例でいうと、線分Ｃ１におけるＡ１に向かう音速、線分Ｃ２におけるＡ１に向かう音速、線分Ｃ３におけるＡ１に向かう音速が算出される。例えば、線分Ｃ１における音速は、位置が既知であるＡ１点とＡ３点との間で、両点の超音波送受信装置を動作させ、Ａ３点からＡ１点に向かう方向における音波の伝播時間から算出される。この際の超音波送受信装置の動作制御は、超音波送受信装置制御部２０１において行われる。処理の結果は、バッファメモリ部２０５に記憶される。同様の処理は、線分Ｃ２、Ｃ３に対しても行われる。これらの処理は、特定点Ａ２、Ａ３においても行われる。
【００４５】
次に、検出位置から３つの特定点に向かう方向の音速（検出位置における音速が含まれる）を音速算出部２０２において算出する（ステップＳ３０６）。図４の例でいうと、検出位置Ｐから頂点Ａ１に向かう方向の音速を算出し、検出位置Ｐから頂点Ａ２に向かう方向の音速を算出し、検出位置Ｐから頂点Ａ３に向かう方向の音速を算出する。
【００４６】
この際、検出位置Ｐと特定点とを結ぶ方向における音速を直接求めることはできない。なぜならば、検出位置Ｐの位置は制御データから得られた不正確な位置であるからである。そこで、図４の例では、例えば検出位置Ｐと特定点Ａ１との間を結ぶ線上の特定点Ａ１に向かう方向の音速を、線分Ｃ１上のＡ１に向かう音速、線分Ｃ２上のＡ１に向かう音速、線分Ｃ３上のＡ１に向かう音速から近似値として算出する。
【００４７】
この算出では、検出位置Ｐと特定点Ａ１とを結ぶ方向におけるＡ１に向かう音速に対して、検出位置Ｐに最も近い線分Ｃ２におけるＡ１に向かう音速の影響（関係）が最も大であり、次に検出位置Ｐに近い線分Ｃ３におけるＡ１に向かう音速の影響が中であり、次に検出位置Ｐに近い線分Ｃ１におけるＡ１に向かう音速の影響が小であると見積もり、各線分におけるＡ１に向かう音速に重み付けをした計算式を立てる。これにより、近似式ではあるが、検出位置Ｐと特定点Ａ１とを結ぶ方向におけるＡ１に向かう音速を算出する。同様にして、検出位置Ｐから頂点Ａ２に向かう方向の音速の算出、検出位置Ｐから頂点Ａ３に向かう方向の音速の算出も行われる。
【００４８】
以下、図４の場合を例に挙げ、具体的な計算式を説明する。まず図４において、Ｌ１は、検出位置Ｐから線分Ｃ１に降ろした垂線である。Ｌ２は、検出位置Ｐから線分Ｃ２に降ろした垂線である。Ｌ３は、検出位置Ｐから線分Ｃ３に降ろした垂線である。また、図４の例では、Ｌ２＜Ｌ３＜Ｌ１が成立している。この条件において、線分Ｃ１におけるＡ１に向かう音速をｖ１、線分Ｃ２におけるＡ１に向かう音速をｖ２、線分Ｃ３におけるＡ１に向かう音速をｖ３とすると、検出位置Ｐから頂点Ａ１に向う方向における音速Ｖは、下記数２から算出される。
【００４９】
【数２】
2011038801.tif 000004
【００５０】
数２においては、ｖ１、ｖ２、ｖ３の係数として、線分Ｃ１〜Ｃ３の検出位置Ｐに対する距離に応じた値を採用し、それによりｖ１、ｖ２、ｖ３のＶへの影響を重み付けする形としている。
【００５１】
これは、以下の考え方に基づいている。まず、線分Ｃ２上の音速ｖ２のベクトルは、線分Ｃ２がＰとＡ１とを結ぶ線分に最も近い（なす角度が最も小さく、位置も最も近い）ので、最もＶのベクトルに近い。次に線分Ｃ３上の音速ｖ３のベクトルは、線分Ｃ３がＰとＡ１とを結ぶ線分に次に近い（なす角度が２番目に小さく、位置も２番目に近い）ので、２番目にＶのベクトルに近い。更に線分Ｃ１上の音速ｖ１のベクトルは、線分Ｃ１がＰとＡ１とを結ぶ線分に３番目に近い（なす角度が３番目に小さく、位置も３番目に近い）ので、３番目にＶのベクトルに近い。したがって、Ｖは、距離に応じた重み付け係数を用いたｖ１、ｖ２、ｖ３の一次結合式で近似的に示すことができる。これが、上記数２である。
【００５２】
ステップＳ３０６において、検出位置から各特定点に向かう線上における音速を算出したら、超音波送受信装置制御部２０１から制御信号を出力し、ロボットハンド１０１（図１参照）の先端部１０４（検出位置）に配置された超音波送信装置２０６から超音波信号を出力させ、それを３つの特定点で受信する。そしてステップＳ３０５で算出した音速と、超音波送信装置２０６からの発信時刻と各特定点における受信時刻との差（つまり音波が伝わるのに要した時間）から、検出位置と３つの特定点との間の距離が算出される（ステップＳ３０８）。この処理は、距離算出部２０３において行われる。
【００５３】
図４の場合でいうと、検出位置Ｐと特定点Ａ１との間の距離ｒ１、検出位置Ｐと特定点Ａ２との間の距離ｒ２、検出位置Ｐと特定点Ａ３との間の距離ｒ３が算出される。
【００５４】
次いで、３点の特定点から検出位置までの距離を半径とした球を描き、その表面の交点を求める。この交点は、２点あるので、予定している検出位置（ロボットハンド１０１の先端部１０４の位置）に近い方（あるいは選択した正六面体内にある方）をロボットハンド１０１の先端部１０４の位置データとして取得する（ステップＳ３０９）。この処理は、位置算出部２０４において行われる。この位置を特定する原理は、ＧＰＳ(Global Positioning System)と同じである。
【００５５】
（ロボットハンドの位置制御）
図５は、ロボット制御装置１０２において行われるロボットハンド１０１の位置制御の流れを示すフロー図である。図５において、ワールド座標系というのは、ロボットハンド１０１が設置されている環境（ロボットハンドシステム１００）に固定されている座標系（例えば、３次元直交座標系や極座標系）である。ロボット座標系というのは、各関節軸を動かす際に利用されるロボット側に固定された座標系である。
【００５６】
位置制御にあたっては、まずワールド座標系での目標位置が設定される（Ｓ５０１）。この目標位置は、ステップＳ５０２に出力されると共に、ステップＳ５０７のズレ量を計算するステップに与えられる。ステップＳ５０２では、ステップ５０７で算出された目標位置と実測位置とのズレ量が与えられ、このズレ量とステップＳ５０１で与えられた目標位置のデータとが、ステップＳ５０３に与えられる。
【００５７】
ステップＳ５０３では、目標位置に対するズレを勘案した座標データが与えられ、これがロボット座標系に変換される。そして、変換されたロボット座標系の座標データに基づいて、各関節部を駆動するモータに動作指令が行われ（ステップＳ５０４）、ロボットハンド１０１（図１参照）の位置制御が行われる（ステップＳ５０５）。
【００５８】
この際、ロボットハンド１０１の先端部分１０４のワールド座標系における位置情報が、位置検出装置１０３における図３の処理により検出され（ステップＳ５０６）、それに基づき、先端部分１０４の目標とした位置と実際の位置との差（ズレ量）が算出される（ステップＳ５０７）。このズレ量は、ステップＳ５０２に与えられ、ロボットハンド１０１の位置制御に利用される。
【００５９】
この処理により、ロボットハンド１０１の先端部分１０４の位置の目標位置とのズレを補正しながらの位置制御が行われる。
【００６０】
（優位性）
以上示した実施形態によれば、ロボットハンド１０１の先端部分１０４の位置の検出が超音波を利用して行われる。この際、先端部分１０４から特定の受信装置までの音波の伝播経路における音速が、設定された単位検出空間（この例の場合正六面体）の各頂点に配置された超音波送受信装置間の音速に基づいて算出される。
【００６１】
この例では、先端部分１０４から特定の受信装置までの経路に近い３本の基準経路の音速を用い、しかも当該経路との距離に応じて重み付けされた計算式によって、上記３本の基準経路（図４のＣ１、Ｃ２、Ｃ３）の当該経路への影響が計算される。このため、近似的ではあるが、検出位置Ｐ（先端部分１０４）から特定の受信装置への経路における温度、湿度、密度、空気の流れといったパラメータの影響を受けた実際の値に近い音速を得ることができる。これにより、これらパラメータの時間的な変化や場所的な変化の影響を低減した状態で、先端部分１０４の位置の検出が行われる。つまり、温度や風の影響による音速の違いに起因する先端部分１０４の位置の検出誤差を抑えることができる。
【００６２】
また実施形態で示した方法では、先端部分１０４に近い上位３つの基準経路の音速を利用するので、検出位置Ｐの環境の状態をより正確に反映した形で、検出位置Ｐから特定の受信装置に向かう方向における音速（検出位置における音速）の値を算出できる。また、各基準経路の方向と、検出位置から特定の受信装置への経路の方向とのなす角度が４５度以下となるので、方向の違いによる音速の違いがある場合でも、精度良く検出位置から特定の受信装置に向かう方向における音速（検出位置における音速）の値を算出できる。
【００６３】
また、正六面体１３１〜１３３で示されるように、単位検出空間を複数用意しているので、より詳細な音速の変化を捉えることができる。これにより、場所による温度変化や空気の流れの影響が大きい環境であっても、先端部分１０４の位置の検出精度を高く保つことができる。
【００６４】
図４の例において、検出位置ＰからＡ１の位置の受信装置までの音速を算出する際に、この経路に最も近い基準経路Ｃ２におけるＡ１に向かう音速の影響を最も大きく見積もり、次に２番目に近い基準経路Ｃ３におけるＡ１に向かう音速の影響をその距離の違いに応じた重み付けで与え、更に、３番目に近い基準経路Ｃ１におけるＡ１に向かう音速の影響をその距離の違いに応じた重み付けで与えている（数２参照）。これにより、検出位置Ｐ付近の雰囲気（温度や風等の状態）をより正確に反映した検出位置Ｐにおける頂点Ａ１方向の音速を得ることができる。
【００６５】
また、検出位置ＰからＡ１の位置の受信装置までの音速Ｖを算出する際に、基準経路Ｃ１、Ｃ２、Ｃ３として、Ａ１を頂点とし、その内側に検出位置Ｐを含む三角錐の各辺を選択するので、各基準経路における音速から得られる音速Ｖの精度をより高くできる。
【００６６】
すなわち、検出位置ＰからＡ１の位置の受信装置までの音速Ｖを算出する際に、基準経路Ｃ１、Ｃ２、Ｃ３におけるＡ１に向かう音速は、音速Ｖの方向と近い方向であるので、基準経路Ｃ１、Ｃ２、Ｃ３における音速（ｖ１、ｖ２、ｖ３）から算出される音速Ｖの実際の値との誤差を抑えることができる。特にこの方法は、空気の流れの向きの影響（風の影響）を考慮に入れることができる。
【００６７】
また、特定点として、Ａ１、Ａ２、Ａ３を検出位置Ｐに近い上位３つから選ぶことで、音波の伝播距離が長くなることによる誤差の増大が抑えられる。
【００６８】
また、特定点をＡ１、Ａ２、Ａ３と３つ選択し、それらを中心とする３つの球の交差位置から検出位置Ｐを求めることで、３次元上の正確な位置の検出が行われる。
【００６９】
（その他）
上記の例示では、単位検出空間として正六面体を採用する例を示したが、多面体であれば、正六面体以外の形状も利用可能である。音波を伝える媒体は、空気に限定されず、気体であれば、不活性気体等であってもよい。また、霧や煙が存在する雰囲気において本発明を適用することもできる。位置の検出は、ロボットハンドの先端ではなく、ロボットハンドの間接部分等であってもよい。また、ロボットハンド以外に、ワークや治具の３次元的な位置を検出する目的に本発明を利用することもできる。
【００７０】
図１には、ロボットハンドを一つ供えたシステムが示されているが、ロボットハンドの数は複数であってもよい。また、単位検出空間（正六面体１３１〜１３３）の数を更に増やしてもよい。また、単位検出空間（正六面体１３１〜１３３）の数が一つであってもよい。
【００７１】
図４に示す例示では、頂点Ａ１、Ａ２、Ａ３の３点から検出位置Ｐまでの距離を算出することで、検出位置Ｐの位置を特定する例を説明したが、基準となる頂点（特定点）は、４箇所以上であってもよい。また、頂点Ａ１のみ、頂点Ａ１とＡ２のみを選択し、そこから検出位置Ｐまでの距離を算出し、その結果に基づいて検出位置Ｐの位置を特定する構成としてもよい。
【００７２】
検出位置までの距離を算出するための基準点を４点以上選択した場合、位置の算出精度をより高くすることができる。検出位置までの距離を算出するための基準点を２点とした場合、これら２点のそれぞれを中心とする球面の交差する円上に検出位置が存在することになる。この場合、他の手段（例えばロボットアームの制御データ）から得られる予測位置の修正に、上記円上に検出位置がある旨の算出データを利用することで、位置検出の制度を高めることができる。
【００７３】
検出位置までの距離を算出するための基準点を１点とした場合、この点を中心とする球面上に検出位置が存在することになる。この場合、他の手段（例えばロボットアームの制御データ）から得られる予測位置の修正に、上記球面上に検出位置がある旨の算出データを利用することで、位置検出の制度を高めることができる。
【００７４】
数２では、線分Ｃ１、Ｃ２、Ｃ３の３本が選択されているが、この数は１本または２本であってもよいし、４本以上であってもよい。検出位置Ｐにおける音速の算出精度は、利用する超音波送受信装置間を結ぶ線分（例えばＣ１、Ｃ２等）の数が多い程高くなるが、演算速度を速くし、また演算素子への負荷を低減する上では、その数を少なくした方が有利となる。
【産業上の利用可能性】
【００７５】
本発明は、超音波を用いた位置の検出を行う技術に利用することができる。また、超音波を用いたロボットハンドの特定の位置を検出する技術に利用することができる。
【符号の説明】
【００７６】
１００…ロボットハンドシステム、１０１…ロボットハンド、１０２…ロボット制御装置、１０３…位置検出装置、１０４…ロボットハンドの先端部分、１１１〜１２６…超音波送受信装置（正六面体の頂点）、１３１〜１３３…正六面体、４００…正六面体。
【図１】
2011038801.tif 000006
【図２】
2011038801.tif 000007
【図３】
2011038801.tif 000008
【図４】
2011038801.tif 000009
【図５】
2011038801.tif 000010