JP2005156250A

JP2005156250A - ３次元位置同定処理方法、３次元位置同定処理プログラムおよび３次元位置同定処理プログラムを記録した記録媒体

Info

Publication number: JP2005156250A
Application number: JP2003392581A
Authority: JP
Inventors: Yoshifumi Nishida; 佳史西田; Toshio Hori; 俊夫堀
Original assignee: National Institute of Advanced Industrial Science and Technology AIST
Current assignee: National Institute of Advanced Industrial Science and Technology AIST
Priority date: 2003-11-21
Filing date: 2003-11-21
Publication date: 2005-06-16
Anticipated expiration: 2023-11-21
Also published as: JP4154486B2

Abstract

【課題】
超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を複数の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理を行う。
【解決手段】
各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択し、選択した３個の受信器の距離情報より行列の各行として１つの受信器が受信した順に距離情報を並べた距離行列を作成し、作成した距離行列の各行について要素を入れ替えることで第２の距離行列を作成する。第２の距離行列の各列の距離の組み合わせに対して同じ発信器からの距離情報として仮定して当該発信器の３次元位置を計算し、選択した３個の受信器以外の受信器の距離情報を用いて、３次元位置を評価し、この処理を繰り返し全ての組み合わせの評価値を得て、その中の最小の評価値を与えた３次元位置を３次元位置推定値として出力する。
【選択図】図３

Description

本発明は、超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理をコンピュータの処理により行う３次元位置同定処理方法、３次元位置同定処理プログラムおよび３次元位置同定処理プログラムを記録した記録媒体に関するものである。

人間の日常活動を頑健に観察するシステムの構築を目的として、超音波３次元タグが開発されている。超音波３次元タグは、環境側に設置された超音波受信器と、人間が操作する物体に取付けられた超音波発信器からなる３次元位置同定装置であり、物体の３次元位置とその識別情報のＩＤを実時間で計測することができる。

この種の３次元位置計測装置に関係する公知文献として、例えば、特許文献１に記載されている「３次元位置測定方法および３次元位置測定装置」があげられる。この３次元位置測定装置においては、超音波源（超音波発信器）の送波タイミングを知ることなく超音波源の３次元の座標位置が求められる。
特開平６−２２２１３０号公報

ところで、これまでに開発された超音波式３次元位置計測システムでは、時分割で超音波を発信するタイミングを切り替えていたため、追跡する物体の数に比例してサンプリング周波数が低下するという問題点がある。例えば、１個の物体を計測する場合には、５０Ｈｚでのサンプリングが可能であるが、２個の物体を時分割で計測する場合には、２５Ｈｚにサンプリング周波数が低下してしまう。

したがって、更に多くの物体の３次元位置をリアルタイムに計測するには、更にサンプリング周波数が低下してしまうので、十分な精度で該当する物体の３次元位置を追跡できないという問題がある。このような問題を解決する方法として、例えば、複数の超音波周波数を利用する方法や、周波数を変調し符号情報を重畳させる方法などが検討されているが、いずれの方法を用いる場合にも、システムの構成が複雑になる。

本発明は、上記のような問題を解決するためになされたものであり、本発明の目的は、簡単な構成であっても十分な精度で物体の３次元位置を追跡できる超音波式３次元位置計測装置を提供することにある。具体的には、超音波式３次元位置計測装置において複数の超音波発信器から同時に発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理をコンピュータの処理により行う３次元位置同定処理方法、３次元位置同定処理プログラムおよび３次元位置同定処理プログラムを記録した記録媒体を提供することにある。

上記のような目的を達成するため、本発明は、第１の態様として、超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理を行う３次元位置同定処理方法であって、各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、選択した３個の受信器の距離情報より行列の各行として１つの受信器が受信した順に距離情報を並べた距離行列を作成し、作成した距離行列の各行について要素を入れ替えることで第２の距離行列を作成する第２ステップと、第２の距離行列の各列の距離の組み合わせに対して同じ発信器からの距離情報として仮定して当該発信器の３次元位置を計算する第３ステップと、前記選択した３個の受信器以外の受信器の距離情報を用いて第３ステップで求めた３次元位置を評価する第４ステップと、前記３ステップから第４ステップまでの処理を繰り返し全ての組み合わせの評価値を得る制御を行う第５ステップと、前記第１ステップから第５ステップまでの処理を所定回数繰り返し行い、その中の最小の評価値を与えた３次元位置を３次元位置推定値として出力する第６ステップとからなることを特徴とするものである。

また、本発明は、第２の態様として、超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理を行う３次元位置同定処理方法であって、各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、選択した３個の受信器の距離情報よりｍ個の発信器の３次元位置を計算する第２ステップと、計算で求めたｍ個の発信器の３次元位置から計算に使用しなかった残りの（ｎ−３）個の受信器への距離を求め、計算で求めた距離と、その受信器により得た距離の差を誤差ベクトルとして計算し、誤差ベクトルの長さをその受信器に対する誤差評価値とする第３ステップと、（ｎ−３）個の誤差評価値の合計をその組み合わせの全体に対する誤差評価値とする第４ステップと、全ての組み合わせについて誤差評価値をもとめ、最小の誤差評価値を与える組み合わせで得られたｍ個の３次元位置を、この３個の受信器の組み合わせにおけるｍ個の発信器の３次元位置の推定値とする第５ステップと、計算に用いる３個の受信器の組み合わせを変えて同様の計算を所定回数繰り返す第６ステップと、最小評価値を与える発信器の３次元位置をｍ個の３次元位置の推定値として出力する第７ステップとからなることを特徴とするものである。

また、本発明は、第３の態様として、超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理をコンピュータの処理により行う３次元位置同定処理プログラムであって、各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、選択した３個の受信器の距離情報より行列の各行として１つの受信器が受信した順に距離情報を並べた距離行列を作成し、作成した距離行列の各行について要素を入れ替えることで第２の距離行列を作成する第２ステップと、第２の距離行列の各列の距離の組み合わせに対して同じ発信器からの距離情報として仮定して当該発信器の３次元位置を計算する第３ステップと、前記選択した３個の受信器以外の受信器の距離情報を用いて第３ステップで求めた３次元位置を評価する第４ステップと、前記３ステップから第４ステップまでの処理を繰り返し全ての組み合わせの評価値を得る制御を行う第５ステップと、前記第１ステップから第５ステップまでの処理を所定回数繰り返し行い、その中の最小の評価値を与えた３次元位置を３次元位置推定値として出力する第６ステップとの処理をコンピュータに実行させるものである。

また、本発明は、第４の態様として、超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理をコンピュータの処理により行う３次元位置同定処理プログラムであって、各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、選択した３個の受信器の距離情報よりｍ個の発信器の３次元位置を計算する第２ステップと、計算で求めたｍ個の発信器の３次元位置から計算に使用しなかった残りの（ｎ−３）個の受信器への距離を求め、計算で求めた距離と、その受信器により得た距離の差を誤差ベクトルとして計算し、誤差ベクトルの長さをその受信器に対する誤差評価値とする第３ステップと、（ｎ−３）個の誤差評価値の合計をその組み合わせの全体に対する誤差評価値とする第４ステップと、全ての組み合わせについて誤差評価値をもとめ、最小の誤差評価値を与える組み合わせで得られたｍ個の３次元位置を、この３個の受信器の組み合わせにおけるｍ個の発信器の３次元位置の推定値とする第５ステップと、計算に用いる３個の受信器の組み合わせを変えて同様の計算を所定回数繰り返す第６ステップと、最小評価値を与える発信器の３次元位置をｍ個の３次元位置の推定値として出力する第７ステップとの処理をコンピュータに実行させるものである。

また、本発明は、第５の態様として、超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理をコンピュータの処理により行う３次元位置同定処理プログラムを記録した記録媒体であって、各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、選択した３個の受信器の距離情報より行列の各行として１つの受信器が受信した順に距離情報を並べた距離行列を作成し、作成した距離行列の各行について要素を入れ替えることで第２の距離行列を作成する第２ステップと、第２の距離行列の各列の距離の組み合わせに対して同じ発信器からの距離情報として仮定して当該発信器の３次元位置を計算する第３ステップと、前記選択した３個の受信器以外の受信器の距離情報を用いて第３ステップで求めた３次元位置を評価する第４ステップと、前記３ステップから第４ステップまでの処理を繰り返し全ての組み合わせの評価値を得る制御を行う第５ステップと、前記第１ステップから第５ステップまでの処理を所定回数繰り返し行い、その中の最小の評価値を与えた３次元位置を３次元位置推定値として出力する第６ステップとの処理をコンピュータに実行させるプログラムを記録した記録媒体であるようにしてもよい。

また、本発明は、第６の態様として、超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理をコンピュータの処理により行う３次元位置同定処理プログラムを記録した記録媒体であって、各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、選択した３個の受信器の距離情報よりｍ個の発信器の３次元位置を計算する第２ステップと、計算で求めたｍ個の発信器の３次元位置から計算に使用しなかった残りの（ｎ−３）個の受信器への距離を求め、計算で求めた距離と、その受信器により得た距離の差を誤差ベクトルとして計算し、誤差ベクトルの長さをその受信器に対する誤差評価値とする第３ステップと、（ｎ−３）個の誤差評価値の合計をその組み合わせの全体に対する誤差評価値とする第４ステップと、全ての組み合わせについて誤差評価値をもとめ、最小の誤差評価値を与える組み合わせで得られたｍ個の３次元位置を、この３個の受信器の組み合わせにおけるｍ個の発信器の３次元位置の推定値とする第５ステップと、計算に用いる３個の受信器の組み合わせを変えて同様の計算を所定回数繰り返す第６ステップと、最小評価値を与える発信器の３次元位置をｍ個の３次元位置の推定値として出力する第７ステップとの処理をコンピュータに実行させるプログラムを記録した記録媒体であるようにしてもよい。

本発明によれば、超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理をコンピュータの処理により行う３次元位置同定処理方法、３次元位置同定処理プログラムおよび３次元位置同定処理プログラムを記録した記録媒体が提供され、簡単な構成であっても十分な精度の超音波式３次元位置計測装置が得られる。すなわち、これにより、サンプリング周波数の低下の問題が解決され、複数物体に取付けられた発信器からの超音波を同時に受信し、得られた距離情報を用いて複数物体の３次元位置をリアルタイムに追跡できる位置推定装置が得られる。

以下、本発明を実施する場合の一形態について図面を参照して説明する。図１は、本発明の３次元位置同定処理方法を一態様で実施するシステムの構成を示す図である。図１に示すシステムは、超音波式３次元位置計測装置（超音波３次元タグシステム）を構成している。この超音波式３次元位置計測装置は、小型に構成された超音波発信装置を３次元タグとして用いるためのシステムであり、３次元タグとする複数個の超音波発信器１０から同時に発信される超音波パルスを少なくとも４つ以上の超音波受信器２０により共に受信して、当該超音波発信器１０の３次元位置を求めるシステムである。

図１に示すように、超音波３次元タグシステムは、所定のタイミングで超音波信号を発信する超音波発信器（超音波３次元タグ）１０、超音波信号を受信する超音波受信装置および増幅装置からなる超音波受信器２０、受信した超音波信号の位相差から距離を計測する距離計測装置３４、超音波信号を発信するタイミングを制御する無線式超音波発信制御装置３３、距離計測装置から得られたデータを収集するデータ収集装置３２、およびデータ処理を行うデータ処理装置（パーソナルコンピュータ：ＰＣ）３１から構成される。

複数の超音波発信器１０が発信した超音波パルスは、複数の超音波受信器２０により受信される。超音波受信器２０は、超音波受信装置部および増幅装置から構成されており、超音波パルスを受信し増幅して、距離計測装置３４に受信信号を供給する。距離計測装置３４は、超音波発信器１０から発射された超音波パルスが超音波受信器２０に到達するまでの時間を計測して、その間の距離を計測する。同じ超音波発信器１０から発射された超音波パルスを異なる超音波受信器２０によりそれぞれに受信することで、当該超音波発信器１０の３次元位置が求められる。３次元位置を求めるためのデータ処理は、後述するように、データ処理装置（パーソナルコンピュータ）３１により処理される。

超音波受信器２０においては、複数の超音波発信器１０をそれぞれに区別して、それぞれの超音波発信器１０から超音波パルスを受信するために、それぞれの超音波発信器１０の超音波パルスの発信タイミングが個別に制御される。つまり、距離計測装置では、超音波発信器が超音波パルスを発信するタイミングに合わせて時間計測を開始して、それぞれの超音波受信器が超音波パルスを受信するまでの時間と空気中を進む超音波の速度から、超音波発信器と超音波受信器との距離を計測する。

そのため、無線式超音波発信制御装置３３が設けられており、無線式超音波発信制御装置３３が、複数のそれぞれの超音波発信器１０に対して、同期信号と識別符号ＩＤを高周波信号（ＶＨＦ：３１４．９ＭＨｚ）により変調して送信し、それぞれの超音波発信器１０の超音波パルスの発信のタイミングが個別に制御する。無線式超音波発信制御装置３３は、超音波発信器１０に識別符号ＩＤを無線で送信すると共に、システム全体に同期信号を送信する。

データ収集装置３２は、距離計測装置３４により計測された時間に対応している距離情報のデータを収集して、データ処理装置（パーソナルコンピュータ：ＰＣ）３１に送信する。データ処理装置３１では、複数の超音波発信器１０と複数の超音波受信器２０との間の距離情報に基づいて３次元位置を計算する。

図２は、本発明による３次元位置同定処理を行う超音波式３次元位置計測装置における複数の発信器と複数の受信器との関係およびその組み合わせの関係を説明する図である。図２に示すように、超音波式３次元位置計測装置においては、複数の超音波発信器１０から発信された超音波信号を受信する複数の受信器２０により受信して、それぞれの受信器２０における距離情報を得て、距離情報が得られた少なくとも４個以上の受信器２１による距離情報を用いて、複数の超音波発信器１０の各３次元位置を求める処理を行う。

本発明による位置同定処理の原理について説明する。前提条件としては、利用するすべての受信器の３次元位置は予め別の方法によって正確に求められているものとする。

位置同定処理では、処理を開始すると、無線送信器はｍ個の発信器に対して同期信号とそれぞれの識別符号ＩＤを送信し、同時に距離計測装置での時間計測を開始するための同期信号をシステム全体に送信する。識別符号ＩＤを受信したｍ個の発信器は、超音波パルスを送信する。

超音波式３次元位置計測装置にＮ個の受信器があるとすれば、前項のｍ個の発信器から送信されたｍ個の超音波パルスをＮ個の受信器で受信することになる。ただし、複数の発信器から同じような距離にある受信器では、複数の超音波パルスが一部あるいは完全に重なって受信されて、これらを分離することはできない。そのため、実際には、ｍ個の超音波パルスを完全に分離して受信できる受信器の個数はＮ個よりも少なくなる。ここでは、これをｎ個とする。

受信器の３次元位置が既知の場合、１個の発信器の３次元位置を決定するためには、最低３個の受信器で、その発信器への距離データが必要である。しかし、発信器の個数が複数（２個以上）の場合には、発信器と受信器との位置関係によって、それぞれの受信器においては受信する超音波パルスの到達順序が変化するため、３個の受信器の距離データだけではそれぞれの発信器の３次元位置を決定できない。そのため、上記のｎが４より小さい場合には、発信器の３次元位置を同定不可能とする。

４個以上ｎ個の受信器がｍ個の超音波パルスを完全に分離して受信できたとすると、次の式（１）の行列のように、それぞれの距離データを各要素とするｎ×ｍ行列を作成することができる。これを距離行列と呼ぶ。距離行列の各行は個々の受信器に対応し、各行の要素は対応する受信器が受信したｍ個の距離データが昇順に整列されている。

次に、発信器の３次元位置の候補を求めるために、ｎ個の受信器から無作為に三個の受信器を選択する。これは式（１）の距離行列から三つの行を取り出すことに対応する。ここで、取り出した行をα、β、γ行目とすると、次の式（２）に示す部分距離行列が得られる。

ｍ個の発信器から三個の受信器α、β、γまでの距離は、式（２）の部分距離行列にすべて含まれているため、この適当な組み合わせを発見することにより、ｍ個の発信器の３次元位置を同定することができる。

式（２）の部分距離行列において、各行にはｍ個の要素があるため、これを並び替える並び替え方は「ｍ！＝１×２×３×…（ｍ−１）×ｍ」通り存在する。また、すべての行を並び替える必要はなく、１つの行については順序を固定した上で残りの２行だけを並び替えればよい。すなわち、式（２）の部分距離行列には、ｍ個の発信器までの距離データの組み合わせが（ｍ！）^２通り存在する。このすべての組み合わせについて、次に示す計算を行い、ｍ個の発信器の３次元位置の候補を求める。

式（２）の部分距離行列の第１行目（受信器αに対応）の要素の順序を固定して、残りの第２、第３行目（受信器β、γに対応）の要素の順序を入れ替えた行列は式（３）のようになる。式（３）に示す行列は前項に示した（ｍ！）^２通りのうちの一つである。

次に式（３）の各列がそれぞれの発信器までの距離データを示すものと考えて、三角測量法と同様の計算方法を利用して、発信器の３次元位置を計算する。例えば、式（３）の第１列は「受信器α、β、γから、それぞれｄ_α１、ｄ_β５、ｄ_γｍの距離にある発信器」に、第ｍ列は「受信器α、β、γから、それぞれｄ_αｍ、ｄ_β４、ｄ_γ２の距離にある発信器」に、それぞれ対応する。

この処理を実行するための前提条件として「受信器α、β、γの３次元位置は予め正確に求められている」としているため、これを、それぞれ（ｘ_α，ｙ_α，ｚ_α）、（ｘ_β，ｙ_β，ｚ_β）、（ｘ_γ，ｙ_γ，ｚ_γ）と表わすことにすれば、式（３）の第一列に対応する発信器の３次元位置は次の式（４）の連立方程式を解くことによって求められる。

式（４）は、第１式から第２式、第３式を引くことで三元連立一次方程式となるが、等式の数が２であるため、この式（４）だけでは解を求めることはできない。しかし、その他の拘束条件として、例えば、受信器は天井に設置されているために発信器のＺ座標が天井の高さ以下に限定されるという条件を設けることによって、一般には、式（４）から発信器の３次元位置（Ｘ，Ｙ，Ｚ）を計算できる。式（３）の第２列から第ｍ列についても、同様の連立方程式を解くことによって、ｍ個すべての発信器の３次元位置を計算できる。

ただし、式（４）は解を持たない場合が存在するため、第１列から第ｍ列まで連立方程式を解く途中で解が得られない距離データの組が発見された場合には、その組み合わせの処理（式（３）に対応）を打ち切り、次の組み合わせについて新たな式（３）を生成する。

また、距離計測装置で計測された距離データには、超音波パルスの到達時間計測に伴う誤差や、壁面などで反射した超音波パルスを受信した場合に生じる誤差などが含まれているため、式（３）から生成される連立方程式の解として得られるｍ個の発信器の３次元位置にも誤差が含まれる可能性があり、この誤差を評価する必要がある。その評価方法を次に示す。

いま、ここにｍ個の連立方程式（式（４））を解いて得られたｍ個の３次元位置、（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ），ｉ＝１，２，…，ｍがあるとする。部分距離行列の式（２）は距離行列の式（１）から三つの行を取り出したものであるから、残りの（ｎ−３）行の距離データを利用して、上に求めた３次元位置の誤差を評価する。そのため、ｍ個の３次元位置から式（２）を作るのに用いられなかった（ｎ−３）個の受信器までの距離を式（５）のようにして計算する。式（５）は受信器ｊと仮に求められた発信器ｉの３次元位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）との距離を示している。

この計算をｍ個の３次元位置に対して適用すると、受信器ｊと計算によって求められたｍ個の発信器との距離がｍ個得られる。次の計算に用いるため、ここで求められたｍ個の距離は式（６）のような横ベクトルとして昇順に整列しておく。

式（６）は計算によって仮に求められた発信器１〜ｍと受信器ｊとの距離ベクトルであるから、この誤差を評価するため、距離計測装置によって得られている式（１）の距離行列の第ｊ行（受信器ｊに対応）との差を計算する。この結果はｍ次元横ベクトルである式（７）となるので、そのベクトルの長さの式（８）をもって、計算によって求めた発信器の３次元位置の受信器ｊに関する誤差評価値ε_ｊ ^ｋｌとする。

誤差評価値ε_ｊ ^ｋｌは式（２）で利用されなかった（ｎ−３）個の受信器すべてについて計算できるので、その合計値を式（９）のようにして求めて、誤差ε^ｋｌをその組み合わせの誤差評価値として採用する。

同様の計算（ｍ！）^２を通りの組み合わせについて繰り返し行い、最小の誤差ε^ｋｌを与えるｍ個の３次元位置の組を、式（２）の部分距離行列Ｄ_ｓｕｂ ^ｋから求めたｍ個の発信器の３次元位置として、その誤差評価値ε^ｋｌと共に記憶する。

部分距離行列の式（２）を距離行列の式（１）から選ぶ選び方は、全体で_ｎＣ_３通りある。この選び方すべてに対して前項までの計算を行うことは、誤差を低減する上では有効である。しかし、この場合、ｎの数が非常に大きな値の場合には、計算速度との兼ね合いですべての場合を計算することは現実的ではない。このため、部分距離行列の無作為抽出回数には予め最大値Ｋを設定しておき、部分距離行列の無作為抽出によるｍ個の発信器の３次元位置同定処理の繰り返し回数Πは、_ｎＣ_３とＫとのいずれか小さい方の値に制限する。

繰り返し回数Πだけ部分距離行列の無作為抽出を行うと、その繰り返し毎に、ｍ個の３次元位置と、それに伴う誤差評価値ε^ｋが得られる。そこで、これらの中で、最も誤差評価値ε^ｋが小さな値となる場合のｍ個の３次元位置を、同時に超音波パルスを送信したｍ個の発信器の３次元位置の推定値として出力する。

このようにして、ｍ個の発信器から同時に超音波パルスを送信し、そのｍ個の超音波パルスを受信器で受信し、距離行列の式（１）を作成した後、（ｍ！）^２×Π回の繰り返し計算を行って、ｍ個の発信器の３次元位置を同定する。

本発明を利用すると、以上で示したようにｍ個の発信器の３次元位置を同時に同定することができる。その結果、発信器の３次元位置を１個ずつ求める場合に比べて、サンプリング周波数を低下させずに、発信器の３次元位置を同時に同定できるようになる。

図３は、本発明による３次元位置同定処理の概略の処理フローを示すフローチャートである。この処理は、上述したような原理に基づく処理を３次元位置計測装置のデータ処理装置において実行する処理である。図３を参照して、処理の概要を説明する。ここでの処理を開始すると、まず、ステップ１０１において、それぞれの受信に対して受信された距離情報（距離データ）の数を調べ、次のステップ１０２において、ｍ個の距離情報を持つ受信器の中から３個の受信器を選択する。すなわち、各受信器が得た距離データの数を調べて信号を発信した発信器の個数と同じ数の距離データを持つものの中から３個の受信器を選択する。

次に、ステップ１０３において、選択した受信器の持つ距離情報（ｍ個）から作成された距離行列を使い、各行の要素を入れ替えて、距離情報の組み合わせを作成する。すなわち、選択した３個の受信器の距離情報より行列の各行として１つの受信器が受信した順に距離情報を並べた距離行列を作成し、作成した距離行列の各行について要素を入れ替えることで第２の距離行列を作成する。

次に、ステップ１０４〜ステップ１０６の処理により、距離情報の組み合わせを変え、それぞれの組み合わせにおける３次元位置を計算し、その評価値を得る。すなわち、ステップ１０４において、ｉ番目の距離の組み合わせの距離行列Ｄｉに対して、ｍ個の３次元位置を計算し、ステップ１０５において、ｉ番目の距離情報の組み合わせに対して、３次元位置の計算に使わなかった残りの受信器が持つ距離情報に基づいて、求めた３次元位置を評価する。この処理をステップ１０６の制御により、距離情報の組み合わせを変えて、繰り返し行う。

すなわち、第２の距離行列の各列の距離の組み合わせに対して同じ発信器からの距離情報として仮定して当該発信器の３次元位置を計算し、選択した３個の受信器以外の受信器の距離情報を用いて、先の処理で求めた３次元位置を評価し、この処理を繰り返し全ての組み合わせの評価値を得る。この処理は、それぞれに求めた３次元位置のエラーが最小になる３次元位置を推定値として出力するために行う。

前記第ステップ１０２からステップ１０６までの処理を所定回数繰り返し行う処理の制御を行い、その処理が終了すると、ステップ１０８において、その中の最小の評価値を与えた３次元位置を３次元位置推定値として出力する。

図４は、本発明による３次元位置同定処理の別の実施例の概略の処理フローを示すフローチャートである。この処理は、上述したような原理に基づく処理を３次元位置計測装置のデータ処理装置において実行する処理である。図４を参照して、処理の概要を説明する。処理を開始すると、まず、各受信器が得た距離データの数を調べて信号を発信した発信器の個数と同じ数の距離データを持つものの中から３個の受信器を選択し、選択した３個の受信器の距離データよりｍ個の発信器の３次元位置を計算する（ステップ２０１〜ステップ２０５）。

次に、計算で求めたｍ個の発信器の３次元位置から計算に使用しなかった残りの（ｎ−３）個の受信器への距離を求め、計算で求めた距離と、その受信器により得た距離データの差を誤差ベクトルとして計算し、誤差ベクトルの長さをその受信器に対する誤差評価値とする（ステップ２０６）。

（ｎ−３）個の誤差評価値の合計をその組み合わせの全体に対する誤差評価値とし、全ての組み合わせについて誤差評価値をもとめ、最小の誤差評価値を与える組み合わせで得られたｍ個の３次元位置を、この３個の受信器の組み合わせにおけるｍ個の発信器の３次元位置の推定値とし、計算に用いる３個の受信器の組み合わせを変えて同様の計算を所定回数繰り返す（ステップ２０７〜ステップ２０９）。そして、最小評価値を与える発信器の３次元位置をｍ個の３次元位置の推定値として出力する（ステップ２０７〜ステップ２１０）。

本発明の３次元位置同定処理方法を一態様で実施するシステムの構成を示す図である。３次元位置同定処理を行う超音波式３次元位置計測装置における複数の発信器と複数の受信器との関係およびその組み合わせの関係を説明する図である。本発明による３次元位置同定処理の概略の処理フローを示すフローチャートである。本発明による３次元位置同定処理の別の実施例の概略の処理フローを示すフローチャートである。

符号の説明

１０超音波発信器、
２０超音波受信器、
２１超音波受信器、
３１データ処理装置、
３２データ収集装置、
３３無線式超音波発信制御装置、
３４距離計測装置、

Claims

超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理を行う３次元位置同定処理方法であって、
各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、
選択した３個の受信器の距離情報より行列の各行として１つの受信器が受信した順に距離情報を並べた距離行列を作成し、作成した距離行列の各行について要素を入れ替えることで第２の距離行列を作成する第２ステップと、
第２の距離行列の各列の距離の組み合わせに対して同じ発信器からの距離情報として仮定して当該発信器の３次元位置を計算する第３ステップと、
前記選択した３個の受信器以外の受信器の距離情報を用いて第３ステップで求めた３次元位置を評価する第４ステップと、
前記３ステップから第４ステップまでの処理を繰り返し全ての組み合わせの評価値を得る制御を行う第５ステップと、
前記第１ステップから第５ステップまでの処理を所定回数繰り返し行い、その中の最小の評価値を与えた３次元位置を３次元位置推定値として出力する第６ステップと
からなることを特徴とする３次元位置同定処理方法。
超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理を行う３次元位置同定処理方法であって、
各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、
選択した３個の受信器の距離情報よりｍ個の発信器の３次元位置を計算する第２ステップと、
計算で求めたｍ個の発信器の３次元位置から計算に使用しなかった残りの（ｎ−３）個の受信器への距離を求め、計算で求めた距離と、その受信器により得た距離の差を誤差ベクトルとして計算し、誤差ベクトルの長さをその受信器に対する誤差評価値とする第３ステップと、
（ｎ−３）個の誤差評価値の合計をその組み合わせの全体に対する誤差評価値とする第４ステップと、
全ての組み合わせについて誤差評価値をもとめ、最小の誤差評価値を与える組み合わせで得られたｍ個の３次元位置を、この３個の受信器の組み合わせにおけるｍ個の発信器の３次元位置の推定値とする第５ステップと、
計算に用いる３個の受信器の組み合わせを変えて同様の計算を所定回数繰り返す第６ステップと、
最小評価値を与える発信器の３次元位置をｍ個の３次元位置の推定値として出力する第７ステップと
からなることを特徴とする３次元位置同定処理方法。
超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理をコンピュータの処理により行う３次元位置同定処理プログラムであって、
各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、
選択した３個の受信器の距離情報より行列の各行として１つの受信器が受信した順に距離情報を並べた距離行列を作成し、作成した距離行列の各行について要素を入れ替えることで第２の距離行列を作成する第２ステップと、
第２の距離行列の各列の距離の組み合わせに対して同じ発信器からの距離情報として仮定して当該発信器の３次元位置を計算する第３ステップと、
前記選択した３個の受信器以外の受信器の距離情報を用いて第３ステップで求めた３次元位置を評価する第４ステップと、
前記３ステップから第４ステップまでの処理を繰り返し全ての組み合わせの評価値を得る制御を行う第５ステップと、
前記第１ステップから第５ステップまでの処理を所定回数繰り返し行い、その中の最小の評価値を与えた３次元位置を３次元位置推定値として出力する第６ステップと
の処理をコンピュータに実行させる３次元位置同定処理プログラム。
超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理をコンピュータの処理により行う３次元位置同定処理プログラムであって、
各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、
選択した３個の受信器の距離情報よりｍ個の発信器の３次元位置を計算する第２ステップと、
計算で求めたｍ個の発信器の３次元位置から計算に使用しなかった残りの（ｎ−３）個の受信器への距離を求め、計算で求めた距離と、その受信器により得た距離の差を誤差ベクトルとして計算し、誤差ベクトルの長さをその受信器に対する誤差評価値とする第３ステップと、
（ｎ−３）個の誤差評価値の合計をその組み合わせの全体に対する誤差評価値とする第４ステップと、
全ての組み合わせについて誤差評価値をもとめ、最小の誤差評価値を与える組み合わせで得られたｍ個の３次元位置を、この３個の受信器の組み合わせにおけるｍ個の発信器の３次元位置の推定値とする第５ステップと、
計算に用いる３個の受信器の組み合わせを変えて同様の計算を所定回数繰り返す第６ステップと、
最小評価値を与える発信器の３次元位置をｍ個の３次元位置の推定値として出力する第７ステップと
の処理をコンピュータに実行させる３次元位置同定処理プログラム。
超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理をコンピュータの処理により行う３次元位置同定処理プログラムを記録した記録媒体であって、
各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、
選択した３個の受信器の距離情報より行列の各行として１つの受信器が受信した順に距離情報を並べた距離行列を作成し、作成した距離行列の各行について要素を入れ替えることで第２の距離行列を作成する第２ステップと、
第２の距離行列の各列の距離の組み合わせに対して同じ発信器からの距離情報として仮定して当該発信器の３次元位置を計算する第３ステップと、
前記選択した３個の受信器以外の受信器の距離情報を用いて第３ステップで求めた３次元位置を評価する第４ステップと、
前記３ステップから第４ステップまでの処理を繰り返し全ての組み合わせの評価値を得る制御を行う第５ステップと、
前記第１ステップから第５ステップまでの処理を所定回数繰り返し行い、その中の最小の評価値を与えた３次元位置を３次元位置推定値として出力する第６ステップと
の処理をコンピュータに実行させるプログラムを記録した記録媒体。
超音波式３次元位置計測装置において複数の超音波発信器から発信された超音波信号を受信する少なくとも４個以上の受信器により受信して得た距離情報により複数の超音波発信器の各３次元位置を求める処理をコンピュータの処理により行う３次元位置同定処理プログラムを記録した記録媒体であって、
各受信器が得た距離情報の数を調べて信号を発信した発信器の個数と同じ数の距離情報を持つものの中から３個の受信器を選択する第１ステップと、
選択した３個の受信器の距離情報よりｍ個の発信器の３次元位置を計算する第２ステップと、
計算で求めたｍ個の発信器の３次元位置から計算に使用しなかった残りの（ｎ−３）個の受信器への距離を求め、計算で求めた距離と、その受信器により得た距離の差を誤差ベクトルとして計算し、誤差ベクトルの長さをその受信器に対する誤差評価値とする第３ステップと、
（ｎ−３）個の誤差評価値の合計をその組み合わせの全体に対する誤差評価値とする第４ステップと、
全ての組み合わせについて誤差評価値をもとめ、最小の誤差評価値を与える組み合わせで得られたｍ個の３次元位置を、この３個の受信器の組み合わせにおけるｍ個の発信器の３次元位置の推定値とする第５ステップと、
計算に用いる３個の受信器の組み合わせを変えて同様の計算を所定回数繰り返す第６ステップと、
最小評価値を与える発信器の３次元位置をｍ個の３次元位置の推定値として出力する第７ステップと
の処理をコンピュータに実行させるプログラムを記録した記録媒体。