JP5712373B2 - 距離センサのキャリブレーション装置、キャリブレーションプログラムおよびキャリブレーション方法 - Google Patents

Description

この発明は距離センサのキャリブレーション装置、キャリブレーションプログラムおよびキャリブレーション方法に関し、特にたとえば、或る環境に配置される２以上の距離センサの位置および向きをキャリブレーションする、距離センサのキャリブレーション装置、キャリブレーションプログラムおよびキャリブレーション方法に関する。

この種の従来の距離センサのキャリブレーション装置の一例が特許文献１に開示されている。この特許文献１に開示される移動体推定位置システムでは、測定領域内に互いに分散して配置され、互いの移動体までの距離を３個以上の距離センサで計測し、推定位置算出装置にて、距離センサそれぞれから任意の時刻のセンサ計測値を取得し蓄積する。取得蓄積された各距離センサのセンサ計測値に対して距離に応じた信頼性の度合いを示す距離信頼度を付与し、取得蓄積されたセンサ計測値のうち信頼度の高い計測値を採用して距離センサおよび移動体それぞれの位置を推定する。また、推定位置の処理は、移動体の移動前後の位置それぞれの２個以上の距離センサから得られるセンサ計測値を用いて各距離センサの位置のキャリブレーションおよび移動体の移動位置の推定を行う。

特開２０１０−１２７６５０号［G01S 5/14］

しかし、特許文献１の移動体推定位置システムでは、各センサ位置を算出する際、各センサ位置の初期値が必要であるため、たとえば、計測する環境が変化する度に、初期値を計測等する必要があり、面倒である。

それゆえに、この発明の主たる目的は、新規な、距離センサのキャリブレーション装置、キャリブレーションプログラムおよびキャリブレーション方法を提供することである。

また、この発明の他の目的は、初期値を与えなくても或る環境に任意に配置した複数の距離センサの位置および向きを推定することができる、距離センサのキャリブレーション装置、キャリブレーションプログラムおよびキャリブレーション方法を提供することである。

本発明は、上記の課題を解決するために、以下の構成を採用した。なお、括弧内の参照符号および補足説明等は、本発明の理解を助けるために後述する実施の形態との対応関係を示したものであって、本発明を何ら限定するものではない。

第１の発明は、或る環境に配置され、一定の角度毎の距離を検出する複数の距離センサの位置および向きをキャリブレーションする、距離センサのキャリブレーション装置であって、複数の距離センサの出力に基づいて、当該距離センサ毎に、或る環境内の人間の位置を検出する人間位置検出手段、人間位置検出手段によって検出された人間の位置に基づいて、距離センサ毎に、当該人間の移動軌跡を生成する軌跡生成手段、軌跡生成手段によって距離センサ毎に生成された移動軌跡を、当該移動軌跡の特徴に基づいて、異なる２つの距離センサ間で同定する軌跡同定手段、軌跡同定手段によって同定された移動軌跡において、任意に選択された２つの人間の位置に基づいて、２つの距離センサ間の距離および当該２つの距離センサの相対的な向きについてのセンサ制約を、当該同定された移動軌跡を生成した２つの距離センサの組毎に算出するセンサ制約算出手段、センサ制約算出手段によって算出されたセンサ制約を用いて、各々の距離センサ間の距離と相対的な向きを保つように、複数の距離センサを仮想のマップ上に順次配置して、或る環境における当該複数の距離センサの各々の位置および向きを推定する推定手段、および複数の距離センサの各々の位置および向きを、推定手段によって推定された他の複数の距離センサの各々の位置および向きと、センサ制約に基づいて算出するセンサ位置算出手段を備える、距離センサのキャリブレーション装置である。

第１の発明では、距離センサのキャリブレーション装置（１２）は、或る環境に配置され、一定の角度毎の距離を検出する複数の距離センサ（１４）の位置および向きをキャリブレーションする。このキャリブレーション装置は、人間位置検手段（１２、Ｓ３）、軌跡生成手段（１２、Ｓ５）、軌跡同定手段（１２、Ｓ７）、センサ制約算出手段（１２、Ｓ１１）、推定手段（１２、Ｓ１５）、およびセンサ位置算出手段（１２、Ｓ２１５、Ｓ２１７）を備える。人間位置検出手段は、複数の距離センサの出力に基づいて、当該距離センサ毎に、或る環境内の人間の位置を検出する。軌跡生成手段は、人間位置検出手段によって検出された人間の位置に基づいて、距離センサ毎に、当該人間の移動軌跡を生成する。つまり、人間の位置の時間変化が移動軌跡として生成される。軌跡同定手段は、軌跡生成手段によって距離センサ毎に生成された移動軌跡を、当該移動軌跡の特徴に基づいて、異なる２つの距離センサ間で同定する。たとえば、異なる２つの移動軌跡について、速度変化が近似する移動軌跡同士を一致するものと判定する。センサ制約算出手段は、軌跡同定手段によって同定された移動軌跡において、任意に選択された２つの人間の位置に基づいて、２つの距離センサ間の距離および当該２つの距離センサの相対的な向きについてのセンサ制約を、当該同定された移動軌跡を生成した２つの距離センサの組毎に算出する。推定手段は、センサ制約算出手段によって算出されたセンサ制約を用いて、各々の距離センサ間の距離と相対的な向きを保つように、複数の距離センサを仮想のマップ上に順次配置して、或る環境における複数の距離センサの位置および向きを推定する。センサ位置算出手段は、複数の距離センサの各々の位置および向きを、推定手段によって推定された他の複数の距離センサの各々の位置および向きと、センサ制約に基づいて算出する。

第１の発明によれば、複数の距離センサの出力に基づいて、当該複数の距離センサの位置および向きを算出するので、初期値を与えなくても、或る環境に配置した距離センサの位置および向きをキャリブレーションすることができる。

第２の発明は、第１の発明に従属し、センサ制約算出手段は、軌跡同定手段によって同定された移動軌跡において、任意に選択される２つの人間の位置のうちの少なくとも一方を変化させることにより、２つの距離センサの組毎に、複数のセンサ制約を算出し、複数のセンサ制約を平均するセンサ制約平均手段を含む。

第２の発明では、センサ制約算出手段は、センサ制約平均手段（１２、Ｓ１６５）を含む。センサ制約平均手段は、軌跡同定手段によって同定された移動軌跡において、任意に選択される２つの人間の位置のうちの少なくとも一方を変化させることにより、２つの距離センサの組毎に、複数のセンサ制約を算出し、複数のセンサ制約を平均する。つまり、２つの距離センサのセンサ制約を複数算出し、それらの平均値を求める。それを、２つの距離センサの組毎に算出する。

第２の発明によれば、センサ制約の平均を求めるので、距離センサ間の距離の誤差を考慮して、複数の距離センサの位置を推定することができる。

第３の発明は、第２の発明に従属し、複数のセンサ制約の分散を算出する分散算出手段をさらに備え、推定手段は、分散算出手段によって算出された分散が最小となる２つの距離センサを当該２つの距離センサのセンサ制約の平均に従って仮想のマップ上に配置する基準センサ配置手段、および基準センサ配置手段によって配置された２つの距離センサに続いて、分散の小さい順に、既に配置された距離センサとのセンサ制約の平均を有する距離センサを当該センサ制約の平均に従って仮想のマップ上に順次配置する順次配置手段を含む。

第３の発明では、キャリブレーション装置は、分散算出手段（１２、Ｓ１６５）をさらに備える。また、推定手段は、基準センサ配置手段（１２、Ｓ１８３）および順次配置手段（１２、Ｓ１８９、Ｓ１９１、Ｓ１９３、Ｓ１９５）を含む。分散算出手段は、複数のセンサ制約の分散を算出する。つまり、距離センサ間の距離の誤差を算出する。基準センサ配置手段は、分散算出手段によって算出された分散が最小となる２つの距離センサを当該２つの距離センサのセンサ制約の平均に従って仮想のマップ上に配置する。つまり、最も信頼度の高いセンサ制約を有する２つの距離センサが配置される。順次配置手段は、基準センサ配置手段によって配置された２つの距離センサに続いて、分散の小さい順に、既に配置された距離センサとのセンサ制約の平均を有する距離センサを当該センサ制約の平均に従って仮想のマップ上に順次配置する。つまり、信頼度が高く、既に配置された距離センサとのセンサ制約を有する距離センサが順次配置される。

第３の発明によれば、センサ制約の平均の信頼度の高い順に、距離センサを仮想のマップ上に配置するので、距離センサの推定位置の確度を高くすることができる。

第４の発明は、第１ないし第３の発明のいずれかに従属し、センサ制約が示す距離センサの各組の距離のそれぞれと、センサ位置算出手段によって算出された複数の距離センサの位置に基づく当該距離センサの各組の距離のそれぞれとの差分を算出する差分算出手段、差分算出手段によって算出された差分の合計値が最小値であるかどうかを判断する最小値判断手段、および最小値判断手段によって差分の合計値が最小値になるまで、センサ算出手段によって算出された複数の距離センサの各々の位置および向きを調整する位置調整手段をさらに備える。

第４の発明では、キャリブレーション装置は、差分算出手段（１２、Ｓ２５５）、最小値判断手段（１２、Ｓ２２５）および位置調整手段（１２、Ｓ２１５、Ｓ２１７、Ｓ２２５）をさらに備える。差分算出手段は、センサ制約が示す距離センサの各組の距離のそれぞれと、推定手段によって推定された複数の距離センサの位置に基づく当該距離センサの各組の距離のそれぞれとの差分を算出する。最小値判断手段は、差分算出手段によって算出された差分の合計値が最小値であるかどうかを判断する。位置調整手段は、最小値判断手段によって差分の合計値が最小値になるまで、センサ位置算出手段によって算出された複数の距離センサの各々の位置および向きを調整する。

第４の発明によれば、センサ制約の示す距離センサ間の距離と、算出された距離センサの位置に基づく当該距離センサ間の距離との誤差が最小になるように、距離センサの位置および向きを調整するので、距離センサの位置を出来る限り正確に推定することができる。

第５の発明は、第４の発明に従属し、分散算出手段によって算出された分散に基づいて、推定手段によって推定された複数の距離センサの各々の位置の誤差を算出する位置誤差算出手段、位置誤差算出手段によって算出された複数の距離センサの各々の位置の誤差の平均値を算出する誤差平均値算出手段、誤差平均値算出手段によって算出された平均値が所定値以下であるかどうかを判断する位置誤差判断手段、および位置誤差判断手段によって平均値が所定値以下であると判断されるまで、人間位置検出手段、軌跡生成手段、軌跡同定手段、センサ制約算出手段、推定手段、センサ位置算出手段および位置調整手段を繰り返し機能させる繰り返し制御手段をさらに備える。

第５の発明では、キャリブレーション装置は、位置誤差算出手段（１２、Ｓ１８５、Ｓ１９７）、誤差平均値算出手段（１２）、位置誤差判断手段（１２、Ｓ１９）、繰り返し制御手段（１２）をさらに備える。位置誤差算出手段は、分散算出手段によって算出された分散に基づいて、推定手段によって推定された複数の距離センサの各々の位置の誤差を算出する。つまり、距離センサの位置には、センサ間の距離の誤差による誤差（位置誤差）が含まれており、距離センサを配置する度に、センサ間の距離の誤差が累積的に蓄積されるのである。誤差平均値算出手段は、位置誤差算出手段によって算出された複数の距離センサの各々の位置の誤差の平均値を算出する。位置誤差判断出手段は、誤差平均値算出手段によって算出された平均値が所定値以下であるかどうかを判断する。繰り返し制御手段は、位置誤差判断手段によって平均値が所定値以下であると判断されるまで、人間位置検出手段、軌跡生成手段、軌跡同定手段、センサ制約算出手段、推定手段、センサ位置算出手段および位置調整手段を繰り返し機能させる。

第５の発明によれば、距離センサの位置誤差の平均値が所定値以下になるまで、距離センサの位置を推定するとともに、これを調整するので、正確に距離センサの位置をキャリブレーションすることができる。

第６の発明は、或る環境に配置され、一定の角度毎の距離を検出する複数の距離センサの位置および向きをキャリブレーションする、距離センサのキャリブレーション装置のキャリブレーションプログラムであって、キャリブレーション装置のプロセッサに、複数の距離センサの出力に基づいて、当該距離センサ毎に、或る環境内の人間の位置を検出する人間位置検出ステップ、人間位置検出ステップによって検出された人間の位置に基づいて、距離センサ毎に、当該人間の移動軌跡を生成する軌跡生成ステップ、軌跡生成ステップによって距離センサ毎に生成された移動軌跡を、当該移動軌跡の特徴に基づいて、異なる２つの距離センサ間で同定する軌跡同定ステップ、軌跡同定ステップによって同定された移動軌跡において、任意に選択された２つの人間の位置に基づいて、２つの距離センサ間の距離および当該２つの距離センサの相対的な向きについてのセンサ制約を、当該同定された移動軌跡を生成した２つの距離センサの組毎に算出するセンサ制約算出ステップ、センサ制約算出ステップによって算出されたセンサ制約を用いて、各々の距離センサ間の距離と相対的な向きを保つように、複数の距離センサを仮想のマップ上に順次配置して、或る環境における当該複数の距離センサの各々の位置および向きを推定する推定ステップ、および複数の距離センサの各々の位置および向きを、推定ステップによって推定された他の前記複数の距離センサの各々の位置および向きと、センサ制約に基づいて算出するセンサ位置算出ステップを実行させる、キャリブレーションプログラムである。

第７の発明は、或る環境に配置され、一定の角度毎の距離を検出する複数の距離センサの位置および向きをキャリブレーションする、距離センサのキャリブレーション方法であって、（ａ）複数の距離センサの出力に基づいて、当該距離センサ毎に、或る環境内の人間の位置を検出し、（ｂ）ステップ（ａ）によって検出された人間の位置に基づいて、距離センサ毎に、当該人間の移動軌跡を生成し、（ｃ）ステップ（ｂ）によって距離センサ毎に生成された移動軌跡を、当該移動軌跡の特徴に基づいて、異なる２つの距離センサ間で同定し、（ｄ）ステップ（ｃ）によって同定された移動軌跡において、任意に選択された２つの人間の位置に基づいて、２つの距離センサ間の距離および当該２つの距離センサの相対的な向きについてのセンサ制約を、当該同定された移動軌跡を生成した２つの距離センサの組毎に算出し、（ｅ）ステップ（ｄ）によって算出されたセンサ制約を用いて、各々の距離センサ間の距離と相対的な向きを保つように、複数の距離センサを仮想のマップ上に順次配置して、或る環境における当該複数の距離センサの各々の位置および向きを推定し、そして（ｆ）複数の距離センサの各々の位置および向きを、ステップ（ｅ）によって推定された他の複数の距離センサの各々の位置および向きと、センサ制約に基づいて算出する、距離センサのキャリブレーション方法である。

第６および第７の発明においても、第１の発明と同様に、初期値を与えなくても、或る環境に配置した距離センサの位置および向きをキャリブレーションすることができる。

この発明によれば、異なる距離センサで検出した移動軌跡を同定し、同定した移動軌跡上であり、異なる２以上の人間の位置を用いて２つの距離センサ間の距離および相対角度についてのセンサ制約を算出し、距離センサの各組についてのセンサ制約を用いて、複数の距離センサの位置および向きを推定するので、初期値を与えなくても、或る環境に任意に配置した複数の距離センサの位置および向きをキャリブレーションすることができる。

この発明の上述の目的，その他の目的，特徴および利点は、図面を参照して行う以下の実施例の詳細な説明から一層明らかとなろう。

図１はこの発明の計測装置の一例を示す図解図である。 図２は図１に示す１の距離センサの検出範囲を示す図解図である。 図３は図１に示す複数の距離センサの配置状況およびそれらの全検出範囲を示す図解図である。 図４は図１に示す各距離センサで検出されるセンサ情報の例を示す図解図および背景スキャン処理の結果から得られる或る角度について検出された距離の個数の分布を示すグラフである。 図５は背景スキャン処理の概要を説明するための図解図および人観測点を検出する方法を説明するための図解図である。 図６は図１に示す各距離センサで検出される人間についての人観測点リストおよび各距離センサで検出される移動軌跡についてのローカル軌跡リストを示す図解図である。 図７は或る２つの距離センサで検出される移動軌跡の例を示す図解図である。 図８は或る距離センサで検出された移動軌跡のうちの着目する１の移動軌跡の速度変化と、他の距離センサで検出されたすべての移動軌跡の速度変化とを示すグラフおよびそれらの速度差を示すグラフである。 図９は異なる距離センサで検出された移動軌跡のうち、速度差が閾値以下である移動軌跡同士を関連付けした関連付けテーブルを示す図解図および異なる距離センサで検出された共有の人観測点についての共有の人観測点リストを示す図解図である。 図１０は共有の人観測点とこれを検出した２つの異なる距離センサとについての各パラメータを示す図解図およびセンサ間の制約を算出不能な場合の例を示す図解図である。 図１１は距離センサ間の制約を算出可能な場合の例を示す図解図およびその算出方法を説明するための図解図である。 図１２は距離センサ間の制約および推定値の分散を示す制約リストを示す図解図である。 図１３は距離センサ間の制約を用いてすべてのセンサを配置する場合の配置方法を説明するための図解図である。 図１４は配置された距離センサについてのグローバル座標系における推定位置、角度および距離センサ間の誤差を示すセンサ位置リストを示す図解図である。 図１に示したコンピュータに内蔵されるメモリのメモリマップを示す図解図である。 図１６は図１５に示すデータ記憶領域の具体的な内容を示す図解図である。 図１７は図１に示すコンピュータ（ＣＰＵ）のキャリブレーション処理を示すフロー図である。 図１８は図１に示すコンピュータ（ＣＰＵ）の背景スキャン処理を示すフロー図である。 図１９は図１に示すコンピュータ（ＣＰＵ）の人間の位置検出処理を示すフロー図である。 図２０は図１に示すコンピュータ（ＣＰＵ）のローカル軌跡の生成処理を示すフロー図である。 図２１は図１に示すコンピュータ（ＣＰＵ）の軌跡一致処理の一部を示すフロー図である。 図２２は図１に示すコンピュータ（ＣＰＵ）の軌跡一致処理の他の一部であって、図２１に後続するフロー図である。 図２３は図１に示すコンピュータ（ＣＰＵ）の共有の人観測点リストの作成処理の一部を示すフロー図である。 図２４は図１に示すコンピュータ（ＣＰＵ）の共有の人観測点リストの作成処理の他の一部であって、図２３に後続するフロー図である。 図２５は図１に示すコンピュータ（ＣＰＵ）のセンサ間の制約の算出処理を示すフロー図である。 図２６は図１に示すコンピュータ（ＣＰＵ）の全センサの配置処理の一部を示すフロー図である。 図２７は図１に示すコンピュータ（ＣＰＵ）の全センサの配置処理の他の一部であって、図２６に後続するフロー図である。 図２８は図１に示すコンピュータ（ＣＰＵ）のセンサ位置の調整処理を示すフロー図である。 図２９は図１に示すコンピュータ（ＣＰＵ）の全体誤差の算出処理の一部を示すフロー図である。 図３０は図１に示すコンピュータ（ＣＰＵ）の全体誤差の算出処理の他の一部であって、図２９に後続するフロー図である。 図３１は異なる２つのセンサで検出された移動軌跡を同定する他の方法を説明するための図解図である。 図３２は異なる２つのセンサで検出された移動軌跡を同定するその他の方法を説明するための図解図である。

図１を参照して、この実施例の計測装置１０は、コンピュータ１２を含む。コンピュータ１２は、汎用のコンピュータであり、人間を追跡（検出）する装置として機能するとともに、距離センサ（以下、単に「センサ」という）１４のキャリブレーション装置としても機能する。図１からも分かるように、コンピュータ１２には、複数（この実施例では、６台）のセンサ１４ａ、１４ｂ、１４ｃ、１４ｄ、１４ｅ、１４ｆが接続される。

以下、この実施例において、センサ１４ａ−１４ｆを識別する必要が無い場合には、単に「センサ１４」と表記することにする。

なお、この実施例の計測装置１０では、或る領域（図３参照）における被験者を追跡するために、６つのセンサ１４を設けるようにしてあるが、これは単なる例示である。したがって、領域の大きさ、形状、障害物の位置や個数に応じて、センサ１４の数は適宜変更される。詳細な説明は省略するが、この実施例の計測装置１０では、被験者の移動方向も計測するようにしてあるため、少なくとも２つのセンサ１４が必要である。

コンピュータ１２は、センサ１４とはＲＳ２３２Ｃのようなインターフェイスを介してシリアルに接続される。この実施例では、１台のコンピュータ１２にすべてのセンサ１４を接続するようにしてあるが、ＬＡＮのようなネットワークで接続される２台以上のコンピュータで分担するようにしてもよい。かかる場合には、いずれか１台のコンピュータが統括コンピュータとして機能すればよい。

センサ１４は、たとえば、レーザーレンジファインダであり、レーザーを照射し、物体に反射して戻ってくるまでの時間から当該物体までの距離を計測するものである。たとえば、レーザーレンジファインダ（センサ１４）は、トランスミッタ（図示せず）から照射したレーザーを回転ミラー（図示せず）で反射させて、前方を扇状に一定角度（たとえば、０．５度）ずつスキャンする。

ここで、センサ１４としては、ＳＩＣＫ社製の距離センサ（型式 LMS 200）を用いることができる。この距離センサを用いた場合には、距離８０００ｍｍを±１５ｍｍ程度の誤差で計測可能である。具体的には、図２に示すように、センサ１４の検出範囲は、半径Ｒ（Ｒ≒８０００ｍｍ）の半円形状（扇形）で示される。つまり、センサ１４は、その正面方向を中心とした場合に、左右９０°の方向を所定の距離（Ｒ）以内で計測可能である。ただし、正面方向を０°として、正面方向から左向きが検出される角度のプラス方向であり、正面方向から右向きが検出される角度のマイナス方向である。

また、使用しているレーザーは、日本工業規格 JIS C 6802 「レーザー製品の安全基準」におけるクラス１レーザーであり、人の眼に対して影響を及ぼさない安全なレベルである。たとえば、センサ１４のサンプリングレートは、３７Hzに設定される。これは、歩行するなどにより移動する人間を追跡（検出）するためである。

さらに、後述するように、センサ１４は、或る領域を検出可能な任意の位置に配置される。具体的には、センサ１４の各々は、少なくとも２つ以上のセンサ１４の検出領域が重なるように配置され、図示は省略するが、床面から約９００ｍｍの高さに固定される。この高さは、人間の胴体を検出可能とするためであり、たとえば、日本人の成人の平均身長から算出される。したがって、計測装置１０を設ける場所（地域ないし国）や被験者（人間）の年齢ないし年代（たとえば、子供、大人）に応じて、センサ１４を固定する高さを適宜変更するようにしてよい。

上述したように、センサ１４は、或る領域を検出可能な任意の位置に配置される。通常、このような計測装置１０では、各センサ１４の位置や向きを実測することにより全体の検出範囲（全検出範囲）を設定したり、各センサ１４を予め決定された位置および向きで配置したりするが、いずれの場合も手間である。さらに、人間を追跡する環境（計測装置１０を適用する場所）を頻繁に変更したり、人間を追跡する環境が同じであっても、センサ１４の位置や向きを頻繁に変更したりする場合には、きわめて面倒である。

そこで、この実施例では、各センサ１４の検出結果に基づいて、各センサ１４の位置および向きを推定し、それらを調整（キャリブレーション）するようにしてある。

簡単に説明すると、各センサ１４からの出力に基づいて、センサ１４毎に、人間の位置（人観測点）を検出し、その人観測点の時間変化から当該人間の移動軌跡を算出（生成）する。次に、各センサ１４の出力に基づいて生成された移動軌跡を、センサ１４間で同定する（一致させる）。一致させた移動軌跡上の２つ以上の人観測点が所定のルールに従って抽出され、抽出された２つ以上の人観測点を用いて、この移動軌跡を生成したセンサ１４間の制約（センサ制約）の平均およびその分散を算出する。ただし、センサ制約は、センサ１４間の距離およびセンサ１４間の相対的な角度（向き）である。また、分散は、センサ１４間の距離の誤差である。このようなセンサ制約は、移動軌跡の同定が行われたセンサ１４の各組について算出される。そして、算出されたセンサ制約の平均およびその分散を用いて、すべてのセンサ１４の位置および向きが推定される。推定されたすべてのセンサ１４の位置が調整され、最適化される。以下、詳細に説明する。

図３（Ａ）は、センサ１４ａ−１４ｆが設置された環境を真上方向から見た地図を示す図解図である。この実施例では、図３（Ａ）に示される環境は、或るショッピングモールである。図３（Ａ）からも分かるように、センサ１４ａ、１４ｃ、１４ｄは、地図の上側に設置されており、センサ１４ｂ、１４ｅ、１４ｆは地図の下側に設置されている。また、センサ１４ａ−１４ｆの各々から延びる矢印は、各センサ１４ａ−１４ｆの正面方向を示してある。そして、図３（Ｂ）に示すように、２台以上のセンサ１４の検出範囲が重なる範囲（全検出範囲）は、斜線を付した範囲（領域）で示される。

ただし、上述したように、センサ１４の位置および向き（位置関係）についての初期値は与えられないため、コンピュータ１２は、図３（Ａ）および図３（Ｂ）に示すような位置関係や全検出範囲を予め知ることができない。

したがって、コンピュータ１２は、各センサ１４で検出される情報（センサ情報）から背景を抽出する。つまり、図３（Ｂ）の斜線を付した範囲と、斜線を付していない範囲との境界を求めることにより、全検出範囲を画定する。これは、計測装置１０によって、人間を検出ないし追跡するためである。したがって、扉や壁で遮られる範囲や柱などの固定物が配置されている範囲は背景として抽出される。このように、予め背景を抽出しておくことにより、センサ１４によって背景の手前に検出されるものを、人間等の観測点として容易に判断することができる。

この実施例では、コンピュータ１２が背景を走査する処理（背景スキャン処理）を実行し、その結果から背景を抽出する。具体的には、各センサ１４で検出されるセンサ情報から背景を抽出する。センサ情報の一例が図４（Ａ）に示される。図４（Ａ）に示すように、センサ情報は、フレーム毎に、―９０°から９０°の範囲において一定の角度（０．５°）毎に検出された距離（ｍｍ）についての情報である。

なお、簡単のため、図４（Ａ）では、５°単位で距離を検出しているように記載してあるが、実際には、上述したように、０．５°単位で距離を検出する。

また、この実施例では、フレームは、センサ１４がその検出範囲の全角度についての距離を検出する単位時間であり、たとえば２７ｍｓに設定される。したがって、各センサ１４では、１フレーム（２７ｍｓ）の間に、０．５°単位で１８０°分の距離を検出するとともに、検出した距離をコンピュータ１２に送信する。

詳細な説明は省略するが、各センサ１４のセンサ情報は、各センサ１４の出力に基づいてコンピュータ１２が生成するため、各センサ１４のセンサ情報のフレーム数の同期は取られている。

このような背景スキャン処理が或る程度長い時間（たとえば、１０分）行われると、各角度についての検出可能な最大距離が分かる。図４（Ｂ）には、背景スキャン処理が実行された結果から得られる、或る角度について検出された距離の個数の分布を表したグラフが示される。このグラフから分かるように、最も検出された個数の多い距離が検出可能な最大距離すなわち境界に設定され、境界を超える距離については背景とみなされる。

したがって、図５（Ａ）に示すように、背景スキャン処理を実行することにより、センサ１４の検出範囲（図２参照）のうち、背景と重なる部分を除く範囲、すなわち実際に人間を検出することが可能な範囲（人検出可能範囲）が画定されるのである。図５（Ａ）では、人検出可能範囲を、網掛模様を付することにより示してある。この人検出可能範囲が、上述した全検出範囲を構成することになる。

なお、図５（Ａ）に示すように、背景スキャン処理の実行中に人間Ｈや他の物を検出したり、計測誤差が発生したりすることがあるため、図４（Ｂ）のグラフからも分かるように、境界とは異なる距離が検出されることがある。

また、図５（Ａ）に示すように、センサ１４と壁などの背景との距離が、センサ１４の検出範囲（最大距離Ｒ）を超えている場合には、センサ情報としては、最大距離Ｒ（８０００ｍｍ）が記述される。

さらに、ここでは、背景スキャン処理を或る程度長い時間実行して最も多く検出された距離を境界として決定するようにしてあるが、実際には、所定数（たとえば、１００個）同じ距離が検出された場合に、当該距離を境界として決定するようにしてある。したがって、全てのセンサ１４の全ての角度についての境界が得られたときに、背景スキャン処理が終了される。

詳細な説明は省略するが、背景データ（図１６参照）としては、センサ１４毎に、各角度についての境界までの距離または最大距離Ｒのデータが記憶されるのである。

背景スキャン処理が終了されると、人間と思われる観測点（人観測点）との距離およびその角度についての情報を、各センサ１４のセンサ情報から検出する。ただし、ここで検出される距離および角度は、各センサ１４のローカル座標系における距離および角度である。たとえば、各センサ１４のローカル座標は、自身の中心が原点Ｏに設定され、原点Ｏを通りセンサ１４の正面方向と重なる軸がＹ軸に設定され、原点Ｏを通りＹ軸に直交する軸がＸ軸に設定される。また、センサ１４の正面方向がＹ軸のプラス方向であり、その正面方向から左に９０°回転した方向がＸ軸のプラス方向である。

この実施例では、人検出可能範囲内で連続的に検出された観測点間の長さ（幅）が所定の範囲内であり、かつ観測点と当該観測点を検出した角度についての背景との間の長さ（深さ）が所定値以上である場合に、人間を検出したとみなし、そのときの距離と角度とをリスト（人観測点リスト）に記録（追加）する。この人観測点リストは、背景スキャン処理の実行により、背景が抽出された後に検出されるセンサ情報に基づいて生成される。

ここで、図５（Ｂ）を用いて、或るセンサ１４によって人間Ｈ（人観測点）が検出される場合について説明することにする。図５（Ｂ）に示すように、センサ１４で、或るフレームにおいて或る５つの角度について、連続的に点Ｐ１、Ｐ２、Ｐ３、Ｐ４、Ｐ５との距離が計測されたと仮定する。ただし、簡単のため、他の角度で計測された距離については省略してある。

図５（Ｂ）からも分かるように、点Ｐ１および点Ｐ５は、境界上の点であり、背景と判定され、観測点から排除される。点Ｐ２、点Ｐ３および点Ｐ４は、境界よりも手間であるため、観測点として抽出される。次に、抽出した観測点を用いて、両端の観測点間の長さ（幅）および各観測点とこの各観測点を検出した角度における境界との間の長さ（深さ）が算出される。

上述したように、センサ１４のローカル座標は、その中心を原点Ｏとして設定しているため、図５（Ｂ）に示す例では、センサ１４で検出された点Ｐ２までの距離（第１距離Ｌ１）およびそのときの角度（第１角度φ_１）と、センサ１４で検出された点Ｐ４までの距離（第２距離Ｌ２）およびそのときの角度（第２角度φ_２）とを用いて、点Ｐ２と点Ｐ４との距離（幅）Ｗが算出される。具体的には、数１に従って算出される。

[数１]
Ｗ＝√{（ｘ１−ｘ２）^２＋（ｙ１−ｙ２）^２}
ただし、ｘ１＝Ｌ１×ｓｉｎφ_１、ｘ２＝Ｌ２×ｓｉｎφ_２、ｙ１＝Ｌ１×ｃｏｓφ_１、ｙ２＝Ｌ２×ｃｏｓφ_２である。

また、各観測点について、当該各観測点を検出した角度における境界上の点との間の距離（深さ）ｄｅｐが検出される。つまり、図５（Ｂ）に示す点線の長さ（深さ）ｄｅｐが算出される。これは、背景スキャン処理で得られた背景までの距離または最大距離Ｒから観測点までの距離を減算することにより算出される。

そして、幅Ｗが所定の範囲内であり、かつ深さｄｅｐが所定距離以上であるかどうかを判断するのである。具体的には、数２を満たすかどうかを判断する。数２で示される条件は、１人の人間を判別するために設定したものである。つまり、センサ１４の高さは、人間の胴体付近に設定されているため、数２に示す数値は、胴体かどうかを判別可能に設定される。また、幅Ｗについては、人間が正面を向いていたり、横を向いていたりすることがあるため、比較的広い範囲で設定してある。

［数２］
３００（ｍｍ）≦Ｗ≦８００（ｍｍ）
３００（ｍｍ）≦ｄｅｐ
ただし、数２の数値は、計測する人間の年齢や体格に応じて適宜変更するようにしてもよい。ただし、夏と冬とでは、服装が全く異なり、その厚みが大幅に違うため、季節に応じて異なる数値を設定する必要もある。

上述したように、幅Ｗを所定の範囲内に設定してあるのは、１人の人間を検出するためである。したがって、たとえば、センサ１４から見て、２人以上の人間が並んでいたり、重なっていたりする場合については、個々の人間を区別する方法をさらに導入する必要がある。

また、深さｄｅｐを所定距離以上にしているのは、或る程度の厚みが無い場合には人間と判断するべきではないからである。

なお、図５（Ｂ）では、１つのセンサ１４によって１人の人間Ｈが検出される場合について示してあるが、１つのセンサ１４から見て重ならずに離れた場所に存在する２人以上の人間Ｈが検出される場合もある。

数２に示す条件を満たすと、人間Ｈを検出（観測）したと判断し、その観測点（人観測点）が各センサ１４のローカル座標系における極座標（距離および角度）で記憶される。ただし、図５（Ｂ）からも分かるように、１フレームにおいて同一の人間Ｈについて、複数の人観測点が検出されるため、ここではその代表値（距離および角度の平均値、または、距離の最大値または最小値とそのときの角度）が記憶される。

具体的には、図６（Ａ）に示すような人観測点のリスト（人観測点リスト）が作成され、この人観測点リストに今回（現フレームで）検出された人観測点が記憶（追加）される。図６（Ａ）に示すように、人観測点リストは、インデックスに対応して、フレーム数、距離、角度および速度が記述される。インデックスは、人観測点に付された識別情報であり、この実施例では、アルファベットと数字とによって表される。ただし、これに限定される必要は無く、インデックスは、アルファベットまたは数字のみで表しても良いし、人間が解読不能な情報で表してもよい。フレーム数は、当該人観測点を検出した時点のフレーム数（フレームの番目）である。距離（ｍｍ）は、当該センサ１４によって検出され、対応するインデックスが示す人観測点までの距離である。角度（°）は、当該センサ１４によって、対応するインデックスが示す人観測点を検出したときの当該センサ１４の正面方向に対する角度である。速度（ｍｍ／ｓ）は、対応するインデックスが示す人観測点の移動速度である。ただし、速度（ｍｍ／ｓ）は、同一人物の移動速度であり、後述する移動軌跡（ローカル軌跡リスト）が生成されたときに、連続する人観測点間の単位時間当たりの距離（移動速度）が算出され、人観測点リストに記載される。したがって、人観測点リストの作成時には、速度（ｍｍ／ｓ）としてはｎｕｌｌが記載される。

このような人観測点リストを参照することにより、各センサ１４で検出される人間Ｈの移動軌跡が生成される。この実施例では、所定時間（１フレーム）における人観測点間の距離が所定値（たとえば、５００ｍｍ）以下である場合には、同一の移動軌跡上の点であると判定する。つまり、かかる場合には、同一の人間Ｈについての人観測点が時間に従って変化していると判断するのである。一方、人観測点間の距離が所定値を超える場合には、異なる移動軌跡上の点であると判定する。かかる場合には、異なる人間Ｈの移動軌跡も生成される。詳細な説明は省略するが、人観測点間の距離は、上述した数１に従って算出される。

このように、同一の移動軌跡上の人観測点であるかどうかが判定され、その判定結果に基づいて、図６（Ｂ）に示すようなローカル軌跡リストが生成される。つまり、移動軌跡が生成（検出）される。ローカル軌跡リストは、センサ１４毎に作成される。つまり、人間Ｈの移動軌跡が各センサ１４のローカル座標系のセンサ情報に基づいて生成され、それぞれのローカル軌跡リストが作成されるのである。

図６（Ｂ）に示すように、ローカル軌跡リストでは、ローカルＩＤに対応して、人観測点リストのインデックスが記載される。ローカルＩＤは、当該センサ１４で検出される移動軌跡を識別するための識別情報であり、アルファベットと数字とによって表される。ただし、ローカルＩＤは、アルファベットまたは数字のみで表されても良く、人間が解読不能な情報で表されてもよい。また、人観測点リストのインデックスは、対応するローカルＩＤが示す移動軌跡上の人観測点についてのインデックスであり、２つ以上のインデックスが時系列に従って記載される。

すべてのセンサ１４について、ローカル軌跡リストが作成されると、異なる２つのセンサ１４の各々のセンサ情報に基づいて検出された移動軌跡のうち、同一であると考えられる２つの移動軌跡が互いに関連付けられる。つまり、異なる２つのセンサ１４の出力に基づいて生成された移動軌跡のうち、同一の人間Ｈの移動軌跡であると考えられるものを同定するのである。

たとえば、図７に示すような場合には、センサ１４ａおよびセンサ１４ｂのそれぞれにおいて、２つの移動軌跡が検出される。上述したように、各センサ１４は、ローカル座標系で人観測点を検出しており、互いに他のセンサ１４の存在を認識できていない。このため、各センサ１４で生成された移動軌跡が、他のセンサ１４で生成されたいずれの移動軌跡と一致するのかを直ぐに知ることはできない。したがって、この実施例では、移動軌跡から分かる人観測点の時間変化（人間の移動速度）に基づいて、異なる２つのセンサ１４で検出された移動軌跡のうち、共通する（同一の）移動軌跡を検出するようにしてある。

具体的には、一方のセンサ１４で検出された移動軌跡のうちの着目する１つの移動軌跡（説明の都合上、「移動軌跡Ｔ_ｍ」という）の速度変化と、他方のセンサ１４で検出された全ての移動軌跡（説明の都合上、「移動軌跡Ｔ_ｎ」という）の各々の速度変化とを比較し、速度変化の誤差（速度差）が最も小さい移動軌跡Ｔ_ｎが移動軌跡Ｔ_ｍと同一であると判定する。ただし、速度差（ＲＭＳ誤差）は、数３に従って算出される。

[数３]

ただし、この実施例では、速度差が最も小さい移動軌跡Ｔ_ｎと同程度の速度差を有する他の移動軌跡Ｔ_ｎが存在する場合には、速度差が最も小さい移動軌跡Ｔ_ｎであっても、移動軌跡Ｔ_ｍと同一であると判定しない。また、この実施例では、速度差が最も小さい移動軌跡Ｔ_ｎが唯一存在する場合であっても、この速度差が一定値（この実施例では、１００ｍｍ／ｓ）以上である場合には、移動軌跡Ｔ_ｍと同一であると判定しない。これらの場合には、速度差が最小であっても、移動軌跡Ｔ_ｍと移動軌跡Ｔ_ｎとが一致すると決定することは適切でないと考えられるからである。

図８（Ａ）には、或るセンサ１４で検出された移動軌跡のうちの着目する１つの移動軌跡Ｔ_ｍの速度変化（実線）と、図面では分かり難いが、他のセンサ１４で検出された７つの移動軌跡Ｔ_ｎ（ここでは、ｎ＝１、２、…、７）の速度変化（破線等）とが記載される。

図８（Ｂ）は、図８（Ａ）に示した１つの移動軌跡Ｔ_ｍと、７つの移動軌跡Ｔ_ｎの各々との速度差を示すグラフである。たとえば、速度差は、速度変化における一定時間（この実施例では、５フレーム）毎のサンプル点での速度差の絶対値を累積したものである。図８（Ｂ）から分かるように、着目する１の移動軌跡Ｔ_ｍでは、移動軌跡Ｔ_ｎ（ｎ＝５）との速度差が最小である。また、この移動軌跡Ｔ_ｎ（ｎ＝５）の速度差は、他の移動軌跡Ｔ_ｎ（ｎ＝１−４、６、７）の速度差と同程度でない。さらに、移動軌跡Ｔ_ｎ（ｎ＝５）の速度差は、一定値（この実施例では、１００ｍｍ／ｓ）未満である。したがって、移動軌跡Ｔ_ｍと移動軌跡Ｔ_ｎ（ｎ＝５）とは同一であると判定され、関連付けが行われる。

このような判定処理が行われると、次に着目する移動軌跡Ｔ_ｍとすべての移動軌跡Ｔ_ｎとの間で、速度差が求められ、同一の移動軌跡が有るかどうかが判定される。すべての移動軌跡Ｔ_ｍについてすべての移動軌跡Ｔ_ｎとの判定処理が実行されると、センサ１４の次の組についての判定処理が実行される。この実施例では、６個のセンサ１４が用いられるため、全部で１５（_６Ｃ_２）通りのセンサ１４の組について判定処理が実行される。以下、この実施例において、センサ１４の組という場合には、（Ａ，Ｂ）、（Ａ，Ｃ）、（Ａ，Ｄ）、（Ａ，Ｅ）、（Ａ，Ｆ）、（Ｂ，Ｃ）、（Ｂ，Ｄ）、（Ｂ，Ｅ）、（Ｂ，Ｆ）、（Ｃ，Ｄ）、（Ｃ，Ｅ）、（Ｃ，Ｆ）、（Ｄ，Ｅ）、（Ｄ，Ｆ），（Ｅ，Ｆ）の各組を意味し、センサ１４の各組について処理を実行する場合には、この順番に従って処理が実行されるものとする。ただし、括弧内のアルファベットはセンサ１４の識別情報である。この実施例では、センサ１４ａの識別情報が「Ａ」であり、センサ１４ｂの識別情報が「Ｂ」であり、センサ１４ｃの識別情報が「Ｃ」であり、センサ１４ｄの識別情報が「Ｄ」であり、センサ１４ｅの識別情報が「Ｅ」であり、そして、センサ１４ｆの識別情報が「Ｆ」である。ただし、センサ１４の識別情報はアルファベットで示す必要はなく、数字やアルファベットと数字との組み合わせで表してもよく、また、人間が解読できない情報で表してもよい。

図９（Ａ）には、同一の移動軌跡であることが判定され、関連付けが行われた移動軌跡についてのテーブル（関連付けテーブル）の例が示される。この関連付けテーブルでは、グローバルＩＤに対応して、第１センサＩＤ、第１ローカルＩＤ、第２センサＩＤおよび第２ローカルＩＤが記述される。

グローバルＩＤは、上述したように、同一であることが判定された移動軌跡すなわちグローバル座標系における１つの移動軌跡についての識別情報であり、数字で表される。ただし、グローバルＩＤは、アルファベットで表されも良く、数字とアルファベットとで表されても良く、人間が解読できない情報で表されても良い。

第１センサＩＤは、同一の移動軌跡であると判定された場合の移動軌跡Ｔ_ｍを検出したセンサ１４の識別情報である。第１ローカルＩＤは、同一の移動軌跡と判定された場合の移動軌跡Ｔ_ｍについてのローカルＩＤである。第２センサＩＤは、同一の移動軌跡であると判定された場合の移動軌跡Ｔ_ｎを検出したセンサ１４の識別情報である。第２ローカルＩＤは、同一の移動軌跡と判定された場合の移動軌跡Ｔ_ｎについてのローカルＩＤである。

たとえば、図９（Ａ）に示すように、グローバルＩＤが「３５」である移動軌跡は、第１センサＩＤが「Ａ」であるセンサ１４ａで検出された第１ローカルＩＤが「Ｔ_１」の移動軌跡Ｔ_ｍおよび第２センサＩＤが「Ｂ」であるセンサ１４ｂで検出された第２ローカルＩＤが「Ｔ_１」の移動軌跡Ｔ_ｎであることを意味する。説明は省略するが、他の場合についても同様である。

なお、第１ローカルＩＤと第２ローカルＩＤは、同じＩＤが付されている場合もあるが、第１センサＩＤおよび第２センサＩＤによって区別されるため、それらが示す移動軌跡は識別可能である。

同一の移動軌跡が関連付けられると、つまり関連付けテーブルが作成されると、同一であると判定された移動軌跡Ｔ_ｍ上および移動軌跡Ｔ_ｎ上の人観測点であって、同じタイミング（同一フレーム）で検出された人観測点（共有の人観測点）をそれぞれの移動軌跡Ｔ_ｍ、Ｔ_ｎから抽出する。

図９（Ｂ）は、抽出した共有の人観測点についてのリスト（共有の人観測点リスト）の例を示す。図９（Ｂ）に示すように、共有の人観測点リストは、第１センサＩＤおよび第２センサＩＤに対応して、第１距離、第１角度、第２距離および第２角度が記述される。

上述したように、第１センサＩＤは、同一の移動軌跡であると判定された移動軌跡Ｔ_ｍを検出したセンサ１４の識別情報である。同様に、第２センサＩＤは、同一の移動軌跡であると判定された移動軌跡Ｔ_ｎを検出したセンサ１４の識別情報である。

第１距離は、第１センサＩＤが示すセンサ１４と共有の人観測点ＯＢＳとの距離（ｍｍ）である。第１角度は、第１センサＩＤが示すセンサ１４の正面方向に対する、当該センサ１４から見た共有の人観測点ＯＢＳの角度（°）である。

第２距離は、第２センサＩＤが示すセンサ１４と共有の人観測点ＯＢＳとの距離（ｍｍ）である。第２角度は、第２センサＩＤが示すセンサ１４の正面方向に対する、当該センサ１４から見た共有の人観測点ＯＢＳの角度（°）である。

たとえば、図９（Ｂ）に示すように、第１センサＩＤが「Ａ」であるセンサ１４ａと、第２センサＩＤが「Ｂ」であるセンサ１４ｂとの共有の人観測点ＯＢＳは、センサ１４ａとの第１距離が３５００ｍｍであり、センサ１４ａの正面方向に対する第１角度が４５°であり、センサ１４ｂとの第２距離が２０００ｍｍであり、そして、センサ１４ｂの正面方向に対する第２角度が３０°である。説明は省略するが、他の場合についても同様である。

ただし、人観測点ＯＢＳは、上述したように、センサ１４毎の人観測点リストに登録されている人観測点（Ｈ１，Ｈ２，Ｈ３，Ｈ４，Ｈ５，…）である。

図１０（Ａ）には、第１センサＩＤが示すセンサ１４（センサＳ_１）と第２センサＩＤが示すセンサ１４（センサＳ_２）とによって検出され、同一であると判定された移動軌跡上の共有の人観測点ＯＢＳと、センサＳ_１およびセンサＳ_２との位置関係（距離および角度）を示す図解図である。ただし、図１０（Ａ）においては（後述する図１０（Ｂ）も同様）、第１距離を「ｄ_１」、第１角度を「θ_１」、第２距離を「ｄ_２」、そして第２角度を「θ_２」で示してある。

なお、図１０（Ａ）では、１つの共有の人観測点ＯＢＳを示してあるが、実際には、同一と判定された移動軌跡上には、複数の共有の人観測点ＯＢＳが存在する。

次に、センサ１４間の制約（位置関係）を算出方法について説明する。ただし、センサ１４間の制約とは、２つのセンサ１４間の距離Ｄとこの２つのセンサ１４間の相対的な向き（α、β）とを意味する（図１１（Ａ）、（Ｂ）参照）。

上述したように、人観測点ＯＢＳは各センサ１４のローカル座標系における極座標で表されるため、たとえば、図１０（Ｂ）に示すように、センサＳ_１が距離ｄ_１と角度θ_１とを保ったまま任意の角度γだけ回転した場合には、角度γだけ回転しない場合と比較して、センサＳ_１とセンサＳ_２との制約が全く異なってしまう。つまり、共有の人観測点ＯＢＳが１つの場合には、当該人観測点ＯＢＳと、センサＳ_１およびセンサＳ_２との位置関係（極座標）から、それらのセンサ間の制約（位置関係）を正しく算出することができない。

なお、図示は省略するが、センサＳ_２が距離ｄ_２と角度θ_２とを保ったまま任意の角度だけ回転した場合も、図１０（Ｂ）に示した場合と同様である。

したがって、この実施例では、２つの共有の人観測点ＯＢＳ_ｐ、ＯＢＳ_ｑと、センサＳ_１およびセンサＳ_２との位置関係から、センサＳ_１とセンサＳ_２との制約（位置関係）を算出するようにしてある。

図１１（Ａ）は、２つの共有の人観測点ＯＢＳ_ｐ、ＯＢＳ_ｑと、センサＳ_１およびセンサＳ_２との位置関係を示す図解図である。図示は省略するが、人観測点ＯＢＳ_ｐおよび人観測点ＯＢＳ_ｑは、同一であると判定された移動軌跡上の点である。

ここで、図１１（Ａ）および図１１（Ｂ）を用いて、センサ間の制約の算出方法について説明する。ただし、図１１（Ｂ）では、図１１（Ａ）で示した２つのセンサＳ_１、センサＳ_２の位置を点Ａおよび点Ｂで示すとともに、２つの共有の人観測点ＯＢＳ_ｐ、ＯＢＳ_ｑの位置を点Ｐおよび点Ｑで示してある。また、上述した２つのセンサ間の距離Ｄおよび２つのセンサ間の相対的な角度α、βが示される。具体的には、図１１（Ｂ）に示す、線分ＡＢの長さが距離Ｄであり、線分ＡＢとセンサＳ_１の正面方向が為す角度が角度αであり、そして、線分ＡＢとセンサＳ_２正面方向が為す角度が角度βである。

上述したように、図１１（Ａ）に示す２つの共有の人観測点ＯＢＳ_ｐ、ＯＢＳ_ｑは、所定のルールに従って抽出（選択）される。たとえば、共有の人観測点ＯＢＳ_ｐは時間的に最も新しい（最後に検出された）人観測点であり、共有の人観測点ＯＢＳ_ｑは、共有の人観測点ＯＢＳ_ｐよりも古い（時間的に前に検出された）人観測点であり、任意（ランダム）に選択される。ただし、２つの共有の人観測点ＯＢＳ_ｐ、ＯＢＳ_ｑをランダムに選択するようにしてもよい。

（１）まず、余弦定理を用いて、線分ＰＱの長さｄ_ＰＱが算出される。ここで、図１１（Ａ）に示す第１角度θ_１および第３角度θ_３は、共有の人観測点リストに含まれるため、既知の値である。したがって、図１１（Ｂ）に示す角度∠ＰＡＱは、第１角度θ_１の大きさと第３角度θ_３の大きさとを用いて算出することができる。ただし、センサＳ_１の向きによって第１角度θ_１と第３角度θ_３との符号（＋，−）が変わるため、基準の人観測点ＯＢＳ_ｐに対する第１角度θ１から他の人観測点ＯＢＳ_ｑに対する第３角度θ_３を減算することにより算出される。具体的には、角度∠ＰＡＱは、数４に従って算出される。同様にして、角度∠ＰＢＱは、第２角度θ_２と第４角度θ_４とを用いることにより、数５に従って算出される。ただし、数４および数５において、｜・｜は絶対値を意味する。以下、この実施例において同じ。また、図１１（Ａ）に示す第２角度θ_２および第４角度θ_４は、センサＳ_２で検出されたセンサ情報に含まれるため、既知の値である。以下、同様である。

[数４]
∠ＰＡＱ＝｜θ_１−θ_３｜
[数５]
∠ＰＢＱ＝｜θ_２−θ_４｜
（２）次に、三角形△ＡＰＱについて余弦定理を用いることにより、線分ＰＱの長さを算出する。ここで、線分ＡＰの長さｄ_ＡＰおよび線分ＡＱの長さｄ_ＡＱは、図１１（Ａ）に示す第１距離ｄ_１および第３距離ｄ_３の各々に相当し、これらは共有の人観測点リストに含まれるため、既知である。したがって、数６に示すように、余弦定理を用いて、線分ＰＱの長さｄ_ＰＱが求められる。

[数６]
ｄ_ＰＱ＝√（（ｄ_ＡＰ）^２＋（ｄ_ＡＱ）^２−２×ｄ_ＡＰ×ｄ_ＡＱ×ｃｏｓ∠ＰＡＱ）
なお、詳細な説明は省略するが、角度∠ＰＢＱを数５に従って算出してあるため、三角形△ＢＰＱについて余弦定理を用いることにより、線分ＰＱの長さを算出することも可能である。

（３）次に、角度∠ＡＰＱおよび角度∠ＢＰＱが算出され、さらに、角度∠ＡＰＢが算出される。角度∠ＡＰＱは、三角形△ＡＰＱについて正弦定理を用いることにより、数７に従って算出される。また、角度∠ＢＰＱは、三角形△ＢＰＱについて正弦定理を用いることにより、数８に従って算出される。ただし、角度∠ＡＰＢは、角度∠ＡＰＱと角度∠ＢＰＱとの和であるため、数式は省略する。また、線分ＢＱの長さｄ_ＢＱは、図１１（Ａ）に示す第４距離ｄ_４に相当し、共有の人観測点リストに含まれるため、既知である。

[数７]
∠ＡＰＱ＝ｓｉｎ^−１（ｄ_ＡＱ／ｄ_ＰＱ×ｓｉｎ∠ＰＡＱ）
[数８]
∠ＢＰＱ＝ｓｉｎ^−１（ｄ_ＢＱ／ｄ_ＰＱ×ｓｉｎ∠ＰＢＱ）
（４）続いて、線分ＡＢの長さｄＡＢが、三角形△ＰＡＢについて余弦定理を用いることにより、数９に従って算出される。ただし、線分ＢＰの長さｄ_ＢＰは、図１１（Ａ）に示す第３距離ｄ_３に相当し、共有の人観測点リストに含まれるため、既知である。

[数９]
ｄ_ＡＢ＝√（（ｄ_ＡＰ）^２＋（ｄ_ＰＢ）^２−２×ｄ_ＡＰ×ｄ_ＰＢ×ｃｏｓ∠ＡＰＢ）
（５）さらに、角度∠ＰＡＢおよび∠ＰＢＡが、三角形△ＰＡＢについて正弦定理を用いることにより、数１０および数１１に従ってそれぞれ算出される。

[数１０]
∠ＰＡＢ＝ｓｉｎ^−１（ｄ_ＢＰ／ｄ_ＡＢ×ｓｉｎ∠ＡＰＢ）
[数１１]
∠ＰＢＡ＝ｓｉｎ^−１（ｄ_ＡＰ／ｄ_ＡＢ×ｓｉｎ∠ＡＰＢ）
角度∠ＰＡＢと角度∠ＰＢＡが算出されると、センサＳ_１と人観測点ＯＢＳ_ｐとの角度θ_１およびセンサＳ_２と人観測点ＯＢＳ_ｐとの角度θ_２は既知であるため、センサＳ_１の正面方向と線分ＡＢとの為す角度α（∠ＰＡＢ−｜θ１｜）およびセンサＳ_２の正面方向と線分ＡＢとの為す角度β（∠ＰＢＡ−｜θ２｜）は簡単に求めることができる。

このように、数９−数１１を用いて、２つのセンサ間の制約（Ｄ、α、β）が算出される。ただし、上述したように、観測点ＯＢＳ_ｑはランダムに選択されるため、複数（この実施例では、３つ）の観測点ＯＢＳ_ｑをランダムに選択し、それぞれの場合について、２つのセンサ間の制約を算出するようにしてある。観測点ＯＢＳ_ｐと複数の観測点ＯＢＳ_ｑについて、２つのセンサ間の制約が算出されると、それらについての平均およびその推定値の分散（誤差）が算出される。この実施例では、センサ間の制約の平均およびその推定値の分散は、カルマンフィルタによって算出される。ここで、その算出方法について簡単に説明する。

まず、共有の人観測点ＯＢＳから生成された２つのセンサ（ここでは、センサｉとセンサｊ）についての新しい制約（ｄ_ｉｊ，θ_ｉｊ）は、数１２に示すように、センサ間の相対座標系において、座標デカルトの差Ｚ_ｉｊに変換される。

[数１２]

[数１３]

[数１４]

[数１５]

このように算出されたセンサ１４の各組についてのセンサ間の制約および推定値の分散は、図１２に示すようなリスト（制約リスト）に登録（追加）される。図１２に示すように、制約リストは、第１センサＩＤ、第２センサＩＤ、センサ間の距離、角度、および推定値の分散が記述されている。

なお、図１１（Ａ）および（Ｂ）では、センサ間の制約は、センサＳ_１とセンサＳ_２との距離Ｄおよびそれらの相対角度α、βとして、その算出方法を説明した。ただし、相対角度αは、着目するセンサＳ_１からセンサＳ_２を見た場合のセンサＳ_１の正面方向の向きであり、相対角度βは、着目するセンサＳ_２からセンサＳ_１を見た場合のセンサＳ_２の正面方向の向きである。したがって、制約リストでは、それぞれの場合を別のセンサ制約として記載してある。

第１センサＩＤは、着目するセンサ１４の識別情報であり、第２センサＩＤは、第１センサＩＤが示すセンサ１４から見たセンサ１４の識別情報である。センサ間距離、上述のようにして算出された、第１センサＩＤが示すセンサ１４と第２センサＩＤが示すセンサ１４との距離（ｍｍ）である。角度は、上述のようにして算出された角度（°）であり、具体的には、第１センサＩＤが示すセンサ１４から第２センサＩＤが示すセンサ１４を見た場合の第１センサＩＤが示すセンサ１４の正面方向の向きである。推定値の分散は、上述のようにして算出された推定値の分散σであり、ここでは、対応する第１センサＩＤと第２センサＩＤとを添えて示してある。

制約リストが作成されると、この制約リストを用いて、すべてのセンサ１４の位置が推定される。たとえば、仮想のマップ上に、すべてのセンサ１４が配置される。そして、センサ間の制約について緩和計算を繰り返すことにより、それらの推定を精錬する。

まず、制約リストは、分散σが小さい順（信頼度の高い順番）に並べ替えられる（ソートされる）。そして、最も信頼度の高い場合におけるセンサ間の制約に従って２つのセンサ１４を仮想のマップ上に配置する。このとき、一方のセンサ１４が基準として設定される。たとえば、２つのセンサ１４のうち、括弧書きのセンサ１４の組において左側（または右側でもよい）に記載されたセンサ１４が基準として設定される。つまり、基準として設定されたセンサ１４の位置および角度を基準値に設定する。この実施例では、基準値として、（Ｘ座標，Ｙ座標，角度）＝（０，０，０）が設定される。つまり、基準のセンサ１４の位置が仮想のマップ上の原点に設定される。ただし、センサ１４の正面方向が仮想のマップ上のＹ軸のプラス方向に設定される。そして、他方のセンサ１４が、上述のようにして算出したセンサの制約の平均に従って仮想のマップ上に配置される。

たとえば、最も信頼度の高いセンサ１４の組が（Ａ，Ｃ）であると仮定すると、図１３（Ａ）に示すように、初めに、センサ１４ａを基準として、センサ１４ａおよびセンサ１４ｃがグローバル座標系の仮想のマップ上に配置される。図１３（Ａ）からも分かるように、センサ１４ａが原点Ｏ（０，０）に配置され、その向きがＹ軸のプラス方向に設定される。次に、センサ１４ｃが、センサ１４ａとの間におけるセンサ間の制約（ここでは、距離Ｄ_１、制約角度α_１およびβ_１）を用いて配置される。したがって、センサ１４ｃの位置が（Ｄ_１×ｓｉｎ｜α_１｜，Ｄ_１×ｃｏｓ｜α_１｜）で表される。ただし、上述したように、センサ間の制約には偏差すなわち誤差（センサ間の誤差）が含まれる。したがって、センサ１４ｃが配置された位置は推定位置と言える。また、センサ１４ｃの向きが−（１８０°−｜α｜−｜β｜）に決定される。

なお、この実施例では、角度については分散（誤差）は算出していない。

また、グローバル座標系においては、Ｙ軸方向を基準とした場合に、右側（第１象限、第４象限側）がセンサ１４の向き（角度）のプラス方向であり、左側（第２象限、第３象限側）がセンサ１４の角度のマイナス方向である。

信頼度の最も高い組についての２つのセンサ１４（ここでは、Ａ，Ｃ）が配置されると、配置されたセンサ１４（Ａ，Ｃ）のいずれか一方とセンサ間の制約を有する他のセンサ１４が当該センサ間の制約に従って配置される。図１３（Ａ）に示す例では、センサ１４ａまたはセンサ１４ｃとのセンサ間の制約を有するセンサ１４が、次に仮想のマップ上に配置されるのである。ただし、次に配置するセンサ１４を決定する際には、制約リストを参照することにより、信頼度の高い順にセンサ間の制約が読み出される。

図１３（Ｂ）は、図１３（Ａ）に示す状態から次のセンサ１４（ここでは、Ｂ）を配置する場合についての例を示す。ここでは、並べ替えられた制約リストを一列目からサーチした際に、センサ１４ａまたはセンサ１４ｃのうち、センサ１４ａとのセンサ間の制約を有するセンサ１４ｂが検出され、このセンサ１４ｂが配置されるのである。

図１３（Ｂ）に示すように、すでに配置されているセンサ１４ａとのセンサ間の制約に従ってセンサ１４ｂが仮想のマップ上に配置される。グローバル座標系において、センサ１４ｂの推定位置は（−Ｄ_２×ｓｉｎ｜α_２｜，Ｄ２×ｃｏｓ｜α_２｜）で表される。また、センサ１４ｂの角度は、１８０°−｜α_２｜−｜β_２｜で表される。

このようにして、既に配置されたセンサ１４とのセンサ間の制約を有するセンサ１４が仮想のマップ上に順番に配置される。また、センサ１４が仮想のマップ上に配置されると、図１４に示すようなセンサ位置リストに、配置された情報が登録（追加）される。図１４に示すように、センサ位置リストは、配置センサＩＤに対応して、対センサＩＤ、推定位置、角度およびセンサ間の制約誤差が記述される。配置センサＩＤは、今回配置したセンサ１４についての識別情報である。対センサＩＤは、配置センサＩＤが示すセンサ１４を配置する際に用いたセンサ間の制約が示す他方の（対となる）センサ１４の識別情報を示す。

なお、基準のセンサ１４は、位置が原点Ｏに設定されるため、（ｘ１，ｙ１）＝（０，０）であり、基準のセンサ１４の正面方向がＹ軸方向に設定されるため、角度は０°である。

推定位置は、今回配置されたセンサ１４のグローバル座標系における位置を示す２次元座標である。角度は、今回配置されたセンサ１４のグローバル座標系における正面方向のＹ軸のプラス方向に対する角度である。センサ間の誤差は、センサ間の制約リストに記述された分散（誤差）σ_ｉｊであり、今回配置したセンサ１４（センサｊ）と対となるセンサ１４（センサｉ）についての誤差σ_ｉｊの数値が記述される。

このセンサ位置リストの配置センサＩＤの欄にすべてのセンサ１４の識別情報（ここでは、Ａ−Ｆ）が記述されると、すべてのセンサ１４が仮想のマップ上に配置されたことになる。すべてのセンサ１４が配置されると、全体についての配置位置の誤差（全体誤差）が所定の閾値以下になるように、緩和計算によって、各センサ１４の位置が調整される。つまり、緩和計算によって、センサの推定位置が精錬される。

緩和計算の方法および収束（精錬）の証明は、文献（T. Duckett, S. Marsland, and J. Shapiro, Learning globally consistent maps by relaxaＴｍon, in Proc. IEEE InternaＴｍonal Conference on RoboＴｍcs & AutomaＴｍon (ICRA), San Francisco, CA, April 2000.）に詳細に説明されている。

簡単に説明すると、緩和計算では、推定位置は、各センサｉと、それに関連する近傍の位置（ｘ_ｊ，ｙ_ｊ）に配置されるセンサｊとについて、数１６に従って生成される。ただし、センサｉとセンサｊとは共有の観測点ＯＢＳを検出した２つのセンサ１４である。また、これらの算出では、絶対座標系における角度の制約θ_ｊｉを算出するために、数１７に示すように、推定された角度の制約が用いられる。

[数１６]

[数１７]

次に、各推定位置の誤差ν_ｊｉが、近傍のセンサｊの推定位置の誤差ν_ｊと２つのセンサ間の制約の推定についての誤差ｕ_ｊｉとの合計として、数１８に従って算出される。

[数１８]

これらの推定位置を組み合わせることにより、数１９に従って、センサ１４の新しい推定位置（ｘ_ｉ，ｙ_ｉ）とその誤差ν_ｊとが算出される。ただし、数１９においてＮ_ｉは、センサｉ以外のすべてのセンサｊのセットを表す。ただし、ｉ≠ｊである。

[数１９]

そして、すべてのセンサ１４の角度が、数２０を用いて、角度の制約と推定されたセンサ１４の位置との間の誤差の平均値が最小になるように調整される。

[数２０]

このような手順が任意の停止点まで繰り返される。この実施例の計測装置１０では、上述したような手順を所定回数（たとえば、３回）繰り返すと、正確に収束する傾向にある。ただし、この実施例では、全体誤差の最小値が得られるまで、上述した手順を繰り返すようにしてある。ここで、全体誤差は、センサ間の制約に従ってすべのセンサ１４を仮想のマップ上に配置した場合に得られるセンサ間の距離と、算出したセンサ間の制約におけるセンサ間の距離との差の合計である。ただし、仮想のマップ上に配置したセンサ１４間の距離は、センサ間の制約を有しているセンサ１４間についてのみ算出される。このようにして、すべてのセンサ１４の位置が調整される。このようにして、或る環境に配置された各センサ１４の位置および角度が同一の座標系（グローバル座標系）において認識される。つまり、センサ１４の位置および角度がキャリブレーションされる。

ただし、すべてのセンサ１４についての位置誤差の平均値が所定値（この実施例では、５０ｍｍ−１００ｍｍ）を超えている場合には、位置誤差の平均値が所定値以下になるまで、人間の位置検出処理からセンサ位置の調整処理までが繰り返し実行される。ここで、センサ１４の位置誤差は、推定されたセンサ１４の位置に含まれる誤差を意味する。図１４を参照して説明すると、センサ１４ａとセンサ１４ｃとを配置した際、これらはセンサ間の制約を算出した際の分差σ_ＡＣ，σ_ＣＡ（＝２００ｍｍ）を含むため、センサ１４ａおよびセンサ１４ｃの位置には、その誤差（位置誤差）が含まれることになる。次に、センサ１４ｂを配置する場合には、センサ１４ａの位置誤差に加えて、センサ１４ａとセンサ１４ｂとのセンサ間の制約の分差σ_ＡＢ（＝５００ｍｍ）が含まれるため、センサ１４ｂの位置誤差は２００ｍｍ＋５００ｍｍとなる。このように、センサ１４を配置する度に、位置誤差が増大されるのである。この位置誤差の平均値を所定値以下にして、複数のセンサ１４の位置を最適化しているのである。

図１５は、図１に示したコンピュータ１２に内蔵されるメモリ（ＲＡＭ）１２ａのメモリマップ３００の一例を示す。図１５に示すように、メモリ１２ａは、プログラム記憶領域３０２およびデータ記憶領域３０４を含む。プログラム記憶領域３０２には、キャリブレーションプログラムが記憶され、このキャリブレーションプログラムは、メイン処理プログラム３１２、背景スキャンプログラム３１４、人間位置検出プログラム３１６、軌跡生成プログラム３１８、軌跡一致処理プログラム３２０、共有人観測点リスト作成プログラム３２２、センサ間制約算出プログラム３２４、センサ間制約平均プログラム３２６、センサ位置推定プログラム３２８およびセンサ位置調整プログラム３３０などによって構成される。

メイン処理プログラム３１２は、この実施例のキャリブレーション処理のメインルーチンを処理するためのプログラムである。背景スキャンプログラム３１４は、センサ１４ａ−１４ｆの各々の出力（センサ情報）から背景となる壁や柱などを検出範囲から除外し、センサ１４ａ−１４ｆの各々について、所定角度（この実施例では、０．５°）毎の検出可能な最大距離を抽出するためのプログラムである。

人間位置検出プログラム３１６は、背景スキャンの結果を用いて、センサ１４ａ−１４ｆの各々の出力（センサ情報）から人間を検出するとともに、その位置（人観測点ＯＢＳ）を検出するためのプログラムである。ただし、人間の位置は、センサ１４ａ−１４ｆの各々のローカル座標系における距離および角度で表され、図６（Ａ）に示したように、センサ１４ａ−１４ｆの各々について作成される人観測点リストに登録（追加）される。

軌跡生成プログラム３１８は、センサ１４ａ−１４ｆの各々について、人観測点リストを参照して、連続する人観測点であると判断される人観測点を時系列に並べることにより、ローカル座標系における移動軌跡を生成し、それぞれのローカル軌跡リストを作成するためのプログラムである。軌跡一致処理プログラム３２０は、異なる２つのセンサ１４の出力に基づいてそれぞれ生成された移動軌跡を、移動軌跡の速度変化に基づいて一致するかどうかを判断し、一致する２つの移動軌跡を関連付ける関連付けテーブルを作成するためのプログラムである。

共有人観測点リスト作成プログラム３２２は、関連付けられた２つのローカル軌跡上の人観測点ＯＢＳのうち、同時（同じフレーム）に検出された人観測点ＯＢＳが共有の人観測点ＯＢＳであると判断し、共有の人観測点リストを作成するためのプログラムである。

センサ間制約算出プログラム３２４は、２つのセンサ１４の距離および２つのセンサ１４の相対的な角度（センサ間の制約）を算出するためのプログラムである。センサ間制約平均プログラム３２６は、センサ間制約算出プログラム３２４に従って算出された各センサ１４間の複数の制約についての平均およびその分散をそれぞれ算出するためのプログラムである。この実施例では、上述したように、カルマンフィルタを用いて、各センサ１４間の制約の平均およびその分散が算出される。

センサ位置推定プログラム３２８は、センサ間制約平均プログラム３２６に従って算出された各センサ間の制約の平均およびその分散を用いて、全センサ１４を同一の座標系（グローバル座標系）に配置するためのプログラムである。センサ位置調整プログラム３３０は、センサ位置推定プログラム３２８によって配置された全センサ１４の位置および角度を、緩和計算により、調整（精錬）するためのプログラムである。

図１６は、図１５に示したデータ記憶領域３０４の具体的な内容を示す図解図である。図１６に示すように、データ記憶領域３０４には、センサ情報データ３５０、背景データ３５２、人観測点リストデータ３５４、ローカル軌跡リストデータ３５６、関連付けテーブルデータ３５８、共有の人観測点リストデータ３６０、制約リストデータ３６２、センサ位置リストデータ３６４およびセンサ位置誤差データ３６６などが記憶される。

センサ情報データ３５０は、図４に示したように、各フレームで一定の角度毎に検出された距離を記述したセンサ情報（テーブル）のデータであって、各センサ１４について作成および記憶されている。背景データ３５２は、一定の角度毎の境界までの距離または最大距離Ｒについての数値データであって、センサ１４毎に作成および記憶される。

人観測点リストデータ３５４は、図６（Ａ）に示したように、センサ情報に基づいて検出された人間の位置（人観測点ＯＢＳ）を記述した人観測点リストについてのデータであって、各センサ１４について作成および記憶される。ローカル軌跡リストデータ３５６は、図６（Ｂ）に示したように、人観測点リストに基づいて生成（検出）された移動軌跡のリストのデータであって、各センサ１４について作成および記憶される。

関連付けテーブルデータ３５８は、図９（Ａ）に示したように、異なるセンサ１４で生成（検出）された２つの移動軌跡であって、一致すると判定された（同定された）移動軌跡について、互いに関連付けた関連付けテーブルについてのデータである。共有の人観測点リストデータ３６０は、図９（Ｂ）に示したように、関連付けされた２つの移動軌跡上の人観測点ＯＢＳのうち、同時（同じフレーム）に検出された人観測点ＯＢＳを共有の人観測点ＯＢＳとして登録したリストについてのデータである。制約リストデータ３６２は、図１２に示したように、センサ間の制約（センサ１４間の距離および相対的な角度）およびその推定値の分散についての制約リストのデータである。

センサ位置リストデータ３６４は、複数のセンサ１４を仮想のマップ上に配置する際に、当該仮想マップに配置したセンサ１４についての情報（推定位置、角度、センサ間の誤差など）を示すリストについてのデータである。センサ位置誤差データ３１６は、仮想のマップ上に配置された複数のセンサ１４のそれぞれについての位置誤差を示す数値データである。

なお、図示は省略するが、データ記憶領域３０４には、キャリブレーションプログラムの実行に必要な、他のデータが記憶されたり、カウンタ（タイマ）やレジスタが設けられたりする。

図１７は、図１に示したコンピュータ１２（ＣＰＵ）のキャリブレーション処理を示すフロー図である。図１７に示すように、コンピュータ１２は、キャリブレーション処理を開始すると、ステップＳ１で、後述する背景スキャン処理（図１８参照）を実行し、次のステップＳ３で、後述する人間の位置検出処理（図１９参照）を実行する。

なお、図示は省略するが、キャリブレーション処理が開始されると同時またはほぼ同時に、各センサ１４からのスキャンデータの検出処理が開始される。この検出処理は、キャリブレーション処理を並列的に実行される。

続いて、ステップＳ５で、後述するローカル軌跡の生成処理（図２０参照）を実行し、ステップＳ７で、後述する軌跡一致処理（図２１および図２２）を実行する。さらに、ステップＳ９で、後述する共有の人観測点リストの作成処理（図２３および図２４参照）を実行し、ステップＳ１１で、後述するセンサ間の制約の算出処理（図２５参照）を実行して、ステップＳ１３で、すべてのセンサ１４を接続するためのセンサ間の制約を表せたかどうかを判断する。つまり、コンピュータ１２は、センサ間の制約を算出していないセンサ１４が存在しないかどうかを判断する。

ステップＳ１３で“ＮＯ”であれば、つまりすべてのセンサ１４を接続するためのセンサ間の制約を表せていない場合には、ステップＳ３に戻って、ステップＳ３−Ｓ１１の処理を再度実行する。一方、ステップＳ１３で“ＹＥＳ”であれば、つまりすべてのセンサ１４を接続するためのセンサ間の制約を表せている場合には、ステップＳ１５で、後述する全センサの配置処理（図２６および図２７参照）を実行し、ステップＳ１７で、後述するセンサ位置の調整処理（図２８参照）を実行する。

そして、ステップＳ１９で、推定された位置誤差の平均が閾値（たとえば、５０ｍｍ−１００ｍｍ）以内であるかどうかを判断する。ステップＳ１９で“ＮＯ”であれば、つまり推定された位置誤差の平均が閾値以内でなければ、ステップＳ３に戻る。一方、ステップＳ１９で“ＹＥＳ”であれば、つまり推定された位置誤差の平均が閾値以内であれば、キャリブレーション処理を終了する。

図１８は、図１７のステップＳ１に示した背景スキャン処理のフロー図である。この背景スキャン処理は、マルチタスクによって、センサ１４毎に実行される。コンピュータ１２（ＣＰＵ）は、背景スキャン処理を開始すると、ステップＳ３１で、時間の経過に従って、スキャンデータを収集する。つまり、フレーム毎に、一定の角度毎の距離が検出される。次のステップＳ３３では、角度毎にスキャンデータが示す距離の代表値を算出する。この実施例では、代表値は、最頻出のスキャンデータが示す距離の値である。そして、ステップＳ３５では、代表値についての距離を示すスキャンデータの個数が所定値（この実施例では、１００個）以上かどうかを判断する。

ステップＳ３５で“ＮＯ”であれば、つまり代表値についての距離を示すスキャンデータの個数が所定値未満であれば、ステップＳ３１に戻る。一方、ステップＳ３５で“ＹＥＳ”であれば、つまり代表値についての距離を示すスキャンデータの個数が所定値以上であれば、ステップＳ３７で、角度毎の代表値の数値データを当該センサ１４の背景データとしてデータ記憶領域３０４に記憶して、図１７に示したキャリブレーション処理にリターンする。この背景処理が各センサ１４について実行され、図１６に示した背景データ３５２が生成される。

図１９は、図１７に示したステップＳ３の人間の位置検出処理のフロー図である。この人間の位置検出処理もまた、マルチタスクによって、センサ１４毎に実行される。図１９に示すように、コンピュータ１２（ＣＰＵ）は、人間の位置検出処理を開始すると、ステップＳ５１で、１フレーム分のスキャンデータを収集する。つまり、１フレームにおける、各角度の距離が検出される。次のステップＳ５３では、背景との差分を検出する。つまり、コンピュータ１２は、背景データ３５２を参照して、当該センサ１４の背景データが示す各角度の距離と、ステップＳ５１で検出した現フレームの各角度の距離との差分をそれぞれ算出するのである。

続くステップＳ５５では、人間幅の閾値以内（この実施例では、数２で示した範囲内）で、背景とは異なるセグメントを特定する。つまり、人間が特定される。続くステップＳ５７では、特定されたセグメントについての位置（人観測点）を人観測点リストに登録（追加）して、図１７に示したキャリブレーション処理にリターンする。ただし、上述したように、人観測点として、距離と角度とが人観測点リストに登録される。この人間の位置検出処理が各センサ１４について実行されることにより、人観測点リストデータ３５４が作成ないし更新される。

図２０は、図１７に示したステップＳ５のローカル軌跡の生成処理のフロー図である。このローカル軌跡の生成処理もまた、マルチタスクによって、センサ１４毎に実行される。図２０に示すように、コンピュータ１２は、ローカル軌跡の生成処理を開始すると、ステップＳ７１で、今回検出された人観測点ＯＢＳのうち、或る１つの人観測点ＯＢＳを読み出す。

次のステップＳ７３では、今回検出された人観測点ＯＢＳが、前回（直前のフレーム以前のフレームで）検出されたいずれかの人観測点ＯＢＳの５００ｍｍ以内であるかどうかを判断する。ステップＳ７３で“ＮＯ”であれば、つまり今回検出された人観測点ＯＢＳが、前回検出されたいずれかの人観測点ＯＢＳの５００ｍｍ以内でなければ、そのままステップＳ８１に進む。一方、ステップＳ７３で“ＹＥＳ”であれば、つまり今回検出された人観測点ＯＢＳが、前回検出されたいずれかの人観測点ＯＢＳの５００ｍｍ以内であれば、ステップＳ７５で、当該前回検出された人観測点ＯＢＳがローカル軌跡リストに記載されているかどうかを判断する。このとき、コンピュータ１２は、ローカル軌跡リストデータ３５６を参照する。

ステップＳ７５で“ＹＥＳ”であれば、つまり当該前回検出された人観測点ＯＢＳがローカル軌跡リストに記載されている場合には、ステップＳ７７で、今回検出された人観測点ＯＢＳを、既存のローカル軌跡に追加して、ステップＳ８１に進む。つまり、ステップＳ７７では、ローカル軌跡リストデータ３５６が示すローカル軌跡リストにおいて、既に存在するローカルＩＤに対応して、今回検出された人観測点ＯＢＳのインデックスを記載する。

一方、ステップＳ７５で“ＮＯ”であれば、つまり当該前回検出された人観測点ＯＢＳがローカル軌跡リストに記載されていない場合には、当該前回検出された人観測点ＯＢＳと今回の検出された人観測点ＯＢＳとを含む新しい移動軌跡を生成して、ステップＳ８１に進む。つまり、ステップＳ７９では、ローカル軌跡リストデータ３５６が示すローカル軌跡リストに、ローカルＩＤを追加するとともに、当該ローカルＩＤに対応して、当該前回検出された人観測点ＯＢＳのインデックスと今回の検出された人観測点ＯＢＳのインデックスとを記載する。

ステップＳ８１では、今回検出されたすべて人観測点ＯＢＳについて、ローカル軌跡を生成するための処理（ステップＳ７３−Ｓ７９）を実行したかどうかを判断する。ステップＳ８１で“ＮＯ”であれば、つまり今回検出された人観測点ＯＢＳのうち、ローカル軌跡を生成するための処理を実行していない人観測点ＯＢＳが存在する場合には、ステップＳ８３で、今回検出された人観測点ＯＢＳのうち、次の人観測点ＯＢＳを読み出して、ステップＳ７３に戻る。ステップＳ８３では、人観測点リストにおいて、次に（下側に）記載された人観測点ＯＢＳが読み出される。一方、ステップＳ８１で“ＹＥＳ”であれば、つまり今回検出されたすべての人観測点ＯＢＳについて、ローカル軌跡を生成するための処理を実行した場合には、図１７に示したキャリブレーション処理にリターンする。

図２１および図２２は、図１７に示したステップＳ７の軌跡一致処理のフロー図である。この軌跡一致処理は、マルチタスクによって、センサ１４の組毎に実行される。図２１に示すように、コンピュータ１２（ＣＰＵ）は、軌跡一致処理を開始すると、ステップＳ１０１で、変数ｍを初期化する（ｍ＝１）。ｍは、センサ１４の組のうちの一方のセンサ１４の出力に基づいて生成されたローカル軌跡を個別に識別するものである。また、この軌跡一致処理では、一方のセンサ１４についてのローカル軌跡リストの一列目から順に参照される。

続くステップＳ１０３では、変数ｎを初期化する（ｎ＝１）。ｎは、センサ１４の組のうちの他方のセンサ１４の出力に基づいて生成されたローカル軌跡を個別に識別するものである。また、この軌跡一致処理では、他方のセンサ１４についてのローカル軌跡リストの一列目から順に参照される。

続いて、ステップＳ１０５では、移動軌跡Ｔ_ｍと移動軌跡Ｔ_ｎとのＲＭＳ誤差を数３に従って算出する。次のステップＳ１０７では、移動軌跡Ｔ_ｍと移動軌跡Ｔ_ｎとの速度が最低速度の閾値よりも大きいかどうかを判断する。この最低速度の閾値は、人間が歩いている（移動している）と判断するための速度に設定される。ただし、観測する領域（環境）において、人間の数が多い場合には、この最低速度の閾値は比較的小さく設定され、人間の数が少ない場合やセンサの計測結果にノイズが多く含まれるには、この最低速度の閾値は比較的大きく設定される。このように、人間の数が多い場合には、最低速度の閾値を小さくして、人間が移動しているかどうかを判断し易くしてある。逆に、人間の数が少ない場合には、最低速度の閾値を或る程度大きくしても、人間が移動しているかどうかを判断することができる。また、ノイズの影響を受けないようにするためには、フィルタリングのために、最低速度の閾値を大きくする必要がある。

ステップＳ１０７で“ＮＯ”であれば、つまり移動軌跡Ｔ_ｍおよび移動軌跡Ｔ_ｎの速度の少なくとも一方が最低速度の閾値以下であれば、そのままステップＳ１１３に進む。一方、ステップＳ１０７で“ＹＥＳ”であれば、つまり移動軌跡Ｔ_ｍおよび移動軌跡Ｔ_ｎの速度がいずれも最低速度の閾値よりも大きい場合には、ステップＳ１０９で、ＲＭＳ誤差が誤差閾値（この実施例では、１００ｍｍ／ｓ）よりも小さいかどうかを判断する。

ステップＳ１０９で“ＮＯ”であれば、つまりＲＭＳ誤差が誤差閾値以上であれば、そのままステップＳ１１３に進む。一方、ステップＳ１０９で“ＹＥＳ”であれば、つまりＲＭＳ誤差が誤差閾値よりも小さければ、移動軌跡Ｔ_ｍと移動軌跡Ｔ_ｎとが一致する可能性があると判断して、ステップＳ１１１で、ＲＭＳ誤差に対応して移動軌跡Ｔ_ｎをメモリ１２ａのワーク領域（図示せず）に記憶する。

ステップＳ１１３では、変数ｎを１加算する（ｎ＝ｎ＋１）。続くステップＳ１１５では、変数ｎが最大数を超えたかどうかを判断する。つまり、コンピュータ１２は、他方のセンサ１４についてのローカル軌跡リストに記述されたすべての移動軌跡Ｔ_ｎについて、移動軌跡Ｔ_ｍと一致するかどうかを判定する処理（判定処理）を実行したかどうかを判断する。

ステップＳ１１５で“ＮＯ”であれば、つまり変数ｎが最大数を超えていなければ、次の移動軌跡Ｔ_ｎについての判定処理に移行する。一方、ステップＳ１１５で“ＹＥＳ”であれば、つまり変数ｎが最大数を超えていれば、図２２に示すステップＳ１１７で、移動軌跡Ｔ_ｍに一致する唯一の移動軌跡Ｔ_ｎが有るかどうかを判断する。つまり、コンピュータ１２は、メモリ１２ａのワーク領域を参照して、１つのＲＭＳ誤差に対応して移動軌跡Ｔ_ｎが記載されているかどうかを判断する。

ステップＳ１１７で“ＮＯ”であれば、つまりメモリ１２ａのワーク領域に、ＲＭＳ誤差に対応して移動軌跡Ｔ_ｎが記載されていない場合や２つ以上のＲＭＳ誤差のそれぞれに対応して移動軌跡Ｔ_ｎが記載されている場合には、移動軌跡Ｔ_ｍに一致する唯一の移動軌跡Ｔ_ｎが無いと判断して、そのままステップＳ１２１に進む。一方、ステップＳ１１７で“ＹＥＳ”であれば、つまりメモリ１２ａのワーク領域に、１つのＲＭＳ誤差に対応して移動軌跡Ｔ_ｎが記載されている場合には、移動軌跡Ｔ_ｍに一致する唯一の移動軌跡Ｔ_ｎが有ると判断して、移動軌跡Ｔ_ｍと移動軌跡Ｔ_ｎとを関連付けテーブルに登録（追加）して、ステップＳ１２１に進む。

ステップＳ１２１では、変数ｍを１加算する（ｍ＝ｍ＋１）。続くステップＳ１２３では、変数ｍが最大数を超えたかどうかを判断する。つまり、コンピュータ１２は、一方のセンサ１４についてのローカル軌跡リストに記述されたすべての移動軌跡Ｔ_ｍについて、他方のセンサ１４についてのローカル軌跡リストに記述されたすべての移動軌跡Ｔ_ｎと一致するかどうかの判定処理を実行したかどうかを判断するのである。

ステップＳ１２３で“ＮＯ”であれば、つまり変数ｍが最大数以下であれば、図２１に示したステップＳ１０３に戻る。つまり、コンピュータ１２は、一方のセンサ１４についてのローカル軌跡リストに記述された次のローカル軌跡Ｔ_ｍと他方のセンサ１４についてのローカル軌跡リストに記述された移動軌跡Ｔ_ｎのそれぞれとが一致するかどうかの判定処理に移行するのである。一方、ステップＳ１２３で“ＹＥＳ”であれば、つまり変数ｍが最大数を超えていれば、図１７に示したキャリブレーション処理にリターンする。

図２３および図２４は、図１７に示したステップＳ９の共有の人観測点リストの作成処理のフロー図である。この共有の人観測点リストの作成処理もまた、マルチタスクによって、センサ１４の組毎に実行される。図２３に示すように、コンピュータ１２（ＣＰＵ）は、共有の人観測点リストの作成処理を開始すると、ステップＳ１４１で、関連付けテーブルに存在する最初の移動軌跡Ｔ_ｍと移動軌跡Ｔ_ｎとのそれぞれを構成する人観測点ＯＢＳ（ＯＢＳ_ｖ、ＯＢＳ_ｗ）およびその人観測点ＯＢＳを検出したフレーム数を読み込む。

次のステップＳ１４３では、変数ｆ_ｃに初期値を設定する（ｆ_ｃ＝１）。ただし、ｆ_ｃはフレーム数である。次のステップＳ１４５では、このフレームにおいて、移動軌跡Ｔ_ｍを構成する人観測点ＯＢＳ_ｖが有るかどうかを判断する。つまり、コンピュータ１２は、移動軌跡Ｔ_ｍを構成する人観測点ＯＢＳ_ｖのうち、変数ｆ_ｃが示すフレーム数の人観測点ＯＢＳ_ｖが存在するかどうかを、人観測点リストデータ３５４およびローカル軌跡リストデータ３５６を参照して判断する。この処理は、次のステップＳ１４７も同様である。

ステップＳ１４５で“ＮＯ”であれば、つまりこのフレームにおいて、移動軌跡Ｔ_ｍを構成する人観測点ＯＢＳ_ｖが無い場合には、そのままステップＳ１５１に進む。一方、ステップＳ１４７で“ＹＥＳ”であれば、つまりこのフレームにおいて、移動軌跡Ｔ_ｍを構成する人観測点ＯＢＳ_ｖが有る場合には、ステップＳ１４７で、このフレームにおいて、移動軌跡Ｔ_ｎを構成する人観測点ＯＢＳ_ｗが有るかどうかを判断する。

ステップＳ１４７で“ＮＯ”であれば、つまりこのフレームにおいて、移動軌跡Ｔ_ｎを構成する人観測点ＯＢＳ_ｗが無い場合には、そのままステップＳ１５１に進む。一方、ステップＳ１４７で“ＹＥＳ”であれば、つまりこのフレームにおいて、移動軌跡Ｔ_ｎを構成する人観測点ＯＢＳ_ｗが有る場合には、ステップＳ１４９で、このフレームにおける人観測点ＯＢＳ_ｖ、ＯＢＳ_ｗを、図９（Ｂ）に示したような、共有の人観測点リストに登録（追加）して、ステップＳ１５１に進む。

ステップＳ１５１では、このフレームが最大フレーム（現時点の最終フレーム）であるかどうかを判断する。ステップＳ１５１で“ＮＯ”であれば、つまりこのフレームが最大フレームでなければ、ステップＳ１５３で、変数ｆ_ｃを１加算して（ｆ_ｃ＝ｆ_ｃ＋１）、ステップＳ１４５に戻る。つまり、コンピュータ１２は、次のフレームにおいて、共有の人観測点ＯＢＳが有るかどうかの判断処理を実行するのである。

一方、ステップＳ１５１で“ＹＥＳ”であれば、つまりこのフレームが最大フレームであれば、図２４に示すステップＳ１５５で、次の移動軌跡Ｔ_ｍと移動軌跡Ｔ_ｎとが関連付けテーブルに存在するかどうかを判断する。つまり、コンピュータ１２は、関連付けテーブルデータ３５８を参照して、このセンサの組において、一致することが判断された他の移動軌跡Ｔ_ｍおよび移動軌跡Ｔ_ｎが有るかどうかを判断するのである。

ステップＳ１５５で“ＮＯ”であれば、つまり次の移動軌跡Ｔ_ｍと移動軌跡Ｔ_ｎとが関連付けてテーブルに無い場合には、図１７に示したキャリブレーション処理にリターンする。一方、ステップＳ１５５で“ＹＥＳ”であれば、つまり次の移動軌跡Ｔ_ｍと移動軌跡Ｔ_ｎとが関連付けテーブルに有る場合には、ステップＳ１５７で、関連付けてテーブルに存在する次の移動軌跡Ｔ_ｍと移動軌跡Ｔ_ｎとのそれぞれを構成する人観測点ＯＢＳ（ＯＢＳ_ｖ、ＯＢＳ_ｗ）およびその人観測点ＯＢＳを検出したフレーム数を読み込み、図２３に示したステップＳ１４３に戻る。

図２５は、図１７に示したステップＳ１１のセンサ間の制約の算出処理のフロー図である。このセンサ間の制約の算出処理は、マルチタスクによって、センサ１４の組毎に実行される。図２５に示すように、コンピュータ１２（ＣＰＵ）はセンサ間の制約の算出処理を開始すると、ステップＳ１６１で、最新の共有の人観測点ＯＢＳ_ｐと、１つ以上（たとえば、３つ）の古い共有の人観測点ＯＢＳ_ｑとを抽出する。ただし、上述したように、古い共有の人観測点ＯＢＳ_ｑについては、ランダムに抽出される。

次のステップＳ１６３では、三角形の幾何学を用いて、センサ間の制約（距離および角度）を算出する。つまり、コンピュータ１２は、数４−数１１に従ってセンサ間の制約を算出する。これは、最新の共有の人観測点ＯＢＳ_ｐと、１つ以上（この実施例では、３つ）の古い共有の人観測点ＯＢＳ_ｑのそれぞれとについて算出する。そして、ステップＳ１６５で、数１２−数１５を繰り返し用いて、ステップＳ１６３で算出した複数のセンサ間の制約の平均とその推定値の分散（誤差）とを算出する。そして、ステップＳ１６７で、センサ間の制約の平均およびその推定値の分散を図１１に示した制約リストに記憶（追加）して、図１７に示したキャリブレーション処理にリターンする。

図２６および図２７は、図１７に示したステップＳ１５の全センサの配置処理のフロー図である。図２６に示すように、コンピュータ１２（ＣＰＵ）は、全センサの配置処理を開始すると、ステップＳ１８１で、信頼度の高い順に制約リストを並べ替える。つまり、コンピュータ１２は、推定値の分散（誤差）の小さい順に制約リストを並べ替える。

次のステップＳ１８３では、最も高い信頼度の制約に従って、基準のセンサ１４と対のセンサ１４とを図１４に示したようなセンサ位置リストに登録する。このとき、図１３（Ａ）を用いて説明したように、仮想のマップ上に、基準のセンサ１４および対のセンサ１４が配置される。次のステップＳ１８５では、位置誤差に、センサ間の制約誤差すなわち最も高い信頼度の制約についての推定値の分散（誤差）を設定する。図示は省略するが、このとき、最も高い信頼度の制約についての推定値の分散がセンサ間の誤差として、センサ位置リストの基準のセンサ１４および対のセンサ１４に対応して登録される。つまり、図１４に示したセンサ位置リストのセンサ間の誤差の項目の１行目および２行目に、最も高い信頼度の制約についての推定値の分散が登録される。したがって、基準のセンサ１４と対のセンサ１４とのそれぞれに対応する位置誤差のデータがセンサ位置誤差データ３６６として記憶される。

続いて、ステップＳ１８７では、変数ｋを初期化する（ｋ＝１）。ここで、ｋはセンサ１４の各組を個別に識別するとともに、センサ１４の各組について順次ステップＳ１８９以降の処理を実行するための変数である。図２７に示す次のステップＳ１８９では、センサｉの推定位置が有るかどうかを判断する。ここで、センサｉは、上述したように、括弧書きで示すセンサ１４の組のうち、左側に記載された識別情報が割り当てられたセンサ１４を意味する。また、後述するセンサｊは、括弧書きで示すセンサ１４の組のうち、右側に記載された識別情報が割り当てられたセンサ１４を意味する。このステップＳ１８９では、コンピュータ１２は、センサ位置リストにセンサｉの推定位置が登録されているかどうかを判断しているのである。このことは、後述するステップＳ１９１についても同様である。

ステップＳ１８９で“ＮＯ”であれば、つまりセンサｉの推定位置が無ければ、センサｊの位置を推定するために使用するセンサｉが配置されていないため、そのままステップＳ２０１に進む。一方、ステップＳ１８９で“ＹＥＳ”であれば、つまりセンサｉの推定位置が有れば、ステップＳ１９１で、センサｊの推定位置が有るかどうかを判断する。ステップＳ１９１で“ＹＥＳ”であれば、つまりセンサｊの推定位置が有れば、当該センサｊの位置を推定する必要が無いため、そのままステップＳ２０１に進む。

一方、ステップＳ１９１で“ＮＯ”であれば、つまりセンサｊの推定位置が無ければ、ステップＳ１９３で、センサｉとセンサｊとの間の制約が制約リストにあるかどうかを判断する。ステップＳ１９３で“ＮＯ”であれば、つまりセンサｉとセンサｊとの間の制約が制約リストに無ければ、センサｊを配置できないと判断して、そのままステップＳ２０１に進む。一方、ステップＳ１９３で“ＹＥＳ”であれば、つまりセンサｉとセンサｊとの間の制約が制約リストに有れば、ステップＳ１９５で、センサｉとの制約に従って、センサｊの位置を推定し、センサｊの推定位置をセンサ位置リストに追加する。図示は省略するが、このとき、センサｊの角度も算出され、センサ位置リストに登録される。

続くステップＳ１９７では、センサｉの位置誤差に、センサ間の誤差を加えて、センサｊの位置誤差として設定する。ただし、センサ間の誤差は、上述したように、センサｉとセンサｊとの間の推定値の偏差σ_ｉｊである。なお、ステップＳ１８５およびＳ１９７で算出された位置誤差は、後述のセンサ位置の調整処理（図２８参照）等で用いられる。

続いて、ステップＳ１９９では、すべてのセンサ１４についての位置を推定したかどうかを判断する。つまり、コンピュータ１２（ＣＰＵ）は、センサ位置リストの配置センサＩＤの欄にすべてのセンサ１４の識別情報（この実施例では、Ａ−Ｆ）が記述されたかどうかを判断する。または、コンピュータ１２は、センサ位置リストに記載された識別情報がセンサ１４の総数（この実施例では、６）になったかどうかを判断する。

ステップＳ１９９で“ＹＥＳ”であれば、つまりすべてのセンサ１４の位置を推定した場合には、図１７に示したキャリブレーション処理にリターンする。一方、ステップＳ１９９で“ＮＯ”であれば、つまり位置を推定していないセンサ１４が有る場合には、ステップＳ２０１で、変数ｋを１加算する（ｋ＝ｋ＋１）。そして、ステップＳ２０３で、変数ｋが最大数（この実施例では、１５）を超えたかどうかを判断する。つまり、センサ１４のすべての組について、ステップＳ１８９以降の処理を実行したかどうかを判断するのである。

ステップＳ２０３で“ＹＥＳ”であれば、つまり変数ｋが最大数を超えた場合には、キャリブレーション処理にリターンする。一方、ステップＳ２０３で“ＮＯ”であれば、つまり変数ｋが最大数以下である場合には、ステップＳ１８９に戻って、センサ１４の次の組について、ステップＳ１８９以降の処理を実行する。

図２８は図１７に示したステップＳ１７のセンサ位置の調整処理のフロー図である。図２８に示すように、コンピュータ１２は、センサ位置の調整処理を開始すると、ステップＳ２１１で、後述する全体誤差の算出処理（図２９）を実行する。次のステップＳ２１３では、変数ｉを初期化する（ｉ＝１）。この変数ｉは、センサ１４を個別に識別するための変数である。

次に、ステップＳ２１５では、他のすべてのセンサｊ（ｊ≠ｉ）の推定位置とセンサ間の制約とを用いて、センサｉの新しい位置および位置誤差を算出する。つまり、コンピュータ１２は、上述した数１６−数１９に従って、新しいセンサｉの位置（ｘ_ｉ，ｙ_ｉ）および位置誤差ν_ｉを算出するのである。次のステップＳ２１７では、他のすべてのセンサｊの推定位置に対する現実角度と、制約によって推測される角度との最小誤差をセンサｉの角度に設定する。つまり、コンピュータ１２は、上述した数２０に従って、センサｉの角度θ_ｉを算出するのである。

続いて、ステップＳ２１９では、変数ｉを１加算する（ｉ＝ｉ＋１）。そして、ステップＳ２２１で、変数ｉが最大数（この実施例では、６）を超えたかどうかを判断する。ステップＳ２２１で“ＮＯ”であれば、つまり変数ｉが最大数以下であれば、ステップＳ２１５に戻って、次のセンサｉについてステップＳ２１５およびＳ２１７の処理を実行する。一方、ステップＳ２２１で“ＹＥＳ”であれば、つまり変数ｉが最大数を超えていれば、ステップＳ２２３で、全体誤差の算出処理を実行し、ステップＳ２２５で、今回算出された全体誤差が前回算出された全体誤差（初回では、ステップＳ２１１で算出された全体誤差）よりも小さいかどうかを判断する。

ステップＳ２２５で“ＹＥＳ”であれば、つまり今回算出された全体誤差が前回算出された全体誤差よりも小さい場合には、ステップＳ２１３に戻る。つまり、コンピュータ１２は、さらにセンサ位置の調整を行うのである。一方、ステップＳ２２５で“ＮＯ”であれば、つまり今回算出された全体誤差が前回算出された全体誤差以上であれば、ステップＳ２２７で、前回算出（推定）したセンサ位置を採用して、図１７に示したキャリブレーション処理にリターンする。

図２９および図３０は、図２８に示したステップＳ２１１、Ｓ２２３の全体誤差の算出処理を示すフロー図である。図２９に示すように、コンピュータ１２（ＣＰＵ）は、全体誤差の算出処理を開始すると、ステップＳ２４１で、全体誤差値を０（初期値）に設定する。続くステップＳ２４３では、変数ｒを初期化する（ｒ＝１）。次のステップＳ２４５では、変数ｓを初期化する（ｓ＝１）。ただし、変数ｒおよび変数ｓは、各センサ１４を個別に識別するための変数である。

次のステップＳ２４７では、変数ｒと変数ｓとが一致するかどうかを判断する。ステップＳ２４７で“ＹＥＳ”であれば、つまり変数ｒと変数ｓとが一致する場合には、そのまま図３０に示すステップＳ２５７に進む。一方、ステップＳ２４７で“ＮＯ”であれば、つまり変数ｒと変数ｓと一致しない場合には、ステップＳ２４９で、センサｒとセンサｓとの間のセンサ制約の平均が制約リストに有るかどうかを判断する。

ステップＳ２４９で“ＮＯ”であれば、つまりセンサｒとセンサｓとの間のセンサ制約の平均が制約リストに無ければ、後述する誤差（ｄ_ｅｓｔ−ｄ_ｃｓｔｒ）を算出することができないと判断して、そのままステップＳ２５７に進む。一方、ステップＳ２４９で“ＹＥＳ”であれば、つまりセンサｒとセンサｓとの間のセンサ制約の平均が制約リストに有れば、ステップＳ２５１で、変数ｄ_ｅｓｔにセンサｒの推定位置とセンサｓの推定位置との距離を設定する。続くステップＳ２５３では、変数ｄ_ｃｓｔｒをセンサｒとセンサｓとのセンサ制約の平均によって特定された距離で定義する。そして、ステップＳ２５５で、全体誤差に誤差（ｄ_ｅｓｔ−ｄ_ｃｓｔｒ）を加算して、ステップＳ２５７に進む。

図３０に示すように、ステップＳ２５７では、変数ｓを１加算する（ｓ＝ｓ＋１）。そして、ステップＳ２５９で、変数ｓが最大数（この実施例では、６）を超えたかどうかを判断する。ステップＳ２５９で“ＮＯ”であれば、ステップＳ２４７に戻る。つまり、センサｒと次のセンサｓとについての誤差（ｄ_ｅｓｔ−ｄ_ｃｓｔｒ）を算出し、全体誤差を更新する。

一方、ステップＳ２５９で“ＹＥＳ”であれば、ステップＳ２６１で、変数ｒを１加算する（ｒ＝ｒ＋１）。そして、Ｓ２６３で、変数ｒが最大数（この実施例では６）を超えたかどうかを判断する。ステップＳ２６３で“ＮＯ”であれば、図２９に示したステップＳ２４５に戻る。つまり、次のセンサｒと各センサｓとの誤差（ｄ_ｅｓｔ−ｄ_ｃｓｔｒ）を算出し、全体誤差を更新する。一方、ステップＳ２６３で“ＹＥＳ”であれば、つまり変数ｒが最大数を超えると、全体誤差の算出処理を終了したと判断して、図２８に示したセンサ位置の調整処理にリターンする。

この実施例によれば、或る環境に配置した複数の距離センサの出力に基づいて、各距離センサの位置および角度を推定するので、初期値を与えなくても、距離センサの位置および向きをキャリブレーションすることができる。したがって、計測する環境が変化したり、距離センサの配置位置を変えたりしても、ユーザの手を煩わすことが無い。

なお、この実施例では、センサ間の制約の平均および分散(誤差）をカルマンフィルタを用いて算出するようにしたが、これに限定される必要はない。他の例として、算術平均（Arithmetic mean）やメジアンフィルタを用いるようにしてもよい。算術平均を用いる場合には、標準分散を算出することにより、分散（誤差）を得ることができる。また、メジアンフィルタを用いる場合には、中央絶対偏差（median absolute deviation）を算出することにより、近似値として分散（誤差）を得ることができる。たとえば、カルマンフィルタ、算術平均またはメジアンフィルタのいずれを用いるかは、環境（人の数の大小、人の歩く速度など）に応じて選択するようにしてもよい。

また、この実施例では、距離センサとして、レーザーレンジファインダ（レーザー距離計）を用いた場合についてのみ説明したが、これに限定される必要はない。角度毎の距離が検出可能であれば、超音波、赤外線、マイクロ波、音波（指向性有り）を用いた距離センサを適用することも可能である。

さらに、この実施例では、２次元方向（ＸＹ平面）におけるセンサの位置および角度についてのみキャリブレーションするようにしてある。これは、上下方向（Ｚ方向）については、たとえば加速度センサやジャイロセンサをセンサに設けることにより、他のセンサとは無関係に調整することができるためである。

さらにまた、この実施例では、移動軌跡の特徴として、速度変化を用いることにより、その差に基づいて、異なるセンサで検出された移動軌跡が同一であるかどうかを判定するようにしたが、これに限定される必要はない。

たとえば、速度に代えて、加速度（フレーム毎の加速度）を用いて、その差に基づいて、異なる２つのセンサで検出された移動軌跡が同一であるかどうかを判定してもよい。ただし、加速度は、移動軌跡の速度を微分することにより得ることができる。また、移動軌跡が同一であるかどうかの判定方法は、上述の実施例と同様である。

また、移動軌跡の曲率に基づいて、異なる２つのセンサで検出された移動軌跡が同一であるかどうかを判定してもよい。かかる場合には、移動軌跡を構成する複数の点を、ＩＣＰ(Iterative Closest Point)のようなアルゴリズムを用いて抽出し、抽出した複数の点から、移動軌跡の進行方向に向かう順番で曲率を算出する。すると、進行方向を基準とした場合に、移動軌跡の曲がっている方向（右または左）を時系列に従って抽出（ラべリング）することができる。たとえば、図３１（Ａ）に示すように、或るセンサで検出した或る移動軌跡から、複数の点Ｚ１、Ｚ２、…、Ｚ１５が抽出される。ただし、点Ｚ１が時間的に最も古く、点Ｚ１５が時間的に最も新しい。抽出された点Ｚ１−Ｚ１５を用いて、時系列に従って、局部の曲率が算出され、進行方向に対する方向が決定される。したがって、移動軌跡に対して、「右、右、右、右、左、左、右、左、左、右、左」のように、ラベリングすることができる。ただし、曲率は、抽出された複数の点のうち、連続する３点が時系列に従う順序（Ｚ１−Ｚ３、Ｚ２−Ｚ４、Ｚ３−Ｚ５、…、Ｚ１３−Ｚ１５）で着目され、その着目される３点で形成される三角形の外接円に基づいて算出される。また、たとえば、図３１（Ａ）に示すうように、進行方向は、抽出された複数の点のうち、時間的に最も古い（最初の）点（ここでは、点Ｚ１）から、時間的に最も新しい（最後の）点（ここでは、点Ｚ１５）に向かう方向に決定される。他のセンサについても、同様に、移動軌跡に対してラベリングされる。そして、異なる２つのセンサにおいて、同じラベリングの移動軌跡が唯一存在する場合に、それらを同一の移動軌跡であると判定することができる。

なお、上述の実施例と同様に、異なる２つのセンサにおいて、３つ以上同じラベリングの移動軌跡が存在する場合には、同一の移動軌跡とは判定しない。共通の移動軌跡とは言えないからである。

さらに、移動軌跡の特徴として、人間の行動を抽出（ラベリング）することもできる。この場合には、移動軌跡に含まれる人観測点の時間変化に基づいて、人間の局所行動を判定することができる。局所行動を判定する方法としては、本件出願人が先に出願し、既に公開された特開２０１０−２３１３５９号に開示された方法を採用することができる。この先行技術文献に従えば、「歩き方」、「歩く速さ」、「短時間の歩き方」および「短時間の歩く速さ」の４つのクラスが定義され、各クラスにおいて、複数のカテゴリ（局所行動）に分類される。「歩き方」のクラス（「短時間の歩き方」のクラスも同様）では、「まっすぐ」、「右に曲がる」、「左に曲がる」、「うろうろする」、「Ｕターン」および「不明」の６つのカテゴリに分類される。また、「歩く速さ」のクラス（「短時間の歩く速さ」のクラスも同様）では、「走っている」、「忙しく歩く」、「ゆっくり歩く」、「止まっている」および「待っている」の５つのカテゴリに分類される。移動軌跡から特徴量（特徴量データ）が抽出され、抽出された特徴量データに対してカテゴリがラベル付けされ、ラベル付けされた特徴量データをＳＶＭ(Support Vector Machine)に学習させることにより、移動軌跡から人間の歩き方が判別される。そして、異なる２つのセンサにおいて、同じ歩き方と判定される移動軌跡が唯一存在する場合に、それらを同一の移動軌跡であると判定することができる。

なお、かかる場合にも、上述の実施例と同様に、異なる２つのセンサにおいて、３つ以上同じ歩き方と判定される移動軌跡が存在する場合には、同一の移動軌跡とは判定しない。

さらに、移動軌跡の特徴として、その大きさに基づいて、異なる２つのセンサで検出された移動軌跡が同一であるかどうかを判定してもよい。たとえば、図３１（Ｂ）に示すように、移動軌跡を囲む円が描画され、異なるセンサにおいて、円の大きさが同じまたは略同じと判定される移動軌跡が唯一存在する場合に、それらを同一の移動軌跡であると判定することができる。詳細な説明は省略するが、移動軌跡を囲む円は、移動軌跡を構成する複数の点の中央値を算出し、移動軌跡のうち、中央値と、この中央値から最も遠い点（基本的には、移動軌跡の端点のいずれか一方が選択される）との距離を半径とする円である。ただし、中央値は、複数の点についてのローカル座標系におけるＸＹ座標の平均値である。また、実際に円を描画する必要は無く、半径の長さが同じまたは略同じであるか否かを判定すればよい。

なお、かかる場合にも、上述の実施例と同様に、異なる２つのセンサにおいて、円の大きさが同じまたは略同じであると判定される移動軌跡が３つ以上存在する場合には、同一の移動軌跡とは判定しない。

ただし、図３１（Ｃ）に示すように、円の大きさだけでは、異なる移動軌跡であっても、同じ移動軌跡であると判定されてしまうことがある。このため、移動軌跡の大きさのみならず、移動軌跡を構成する複数の点の分散も考慮するために、図３２（Ａ）に示すように、移動軌跡の大きさを近似する楕円を描画するようにしてもよい。たとえば、中央値が算出され、この中央値から最も遠い点（第１点）が検出される。ただし、中央値は、上述した方法により、求められる。さらに、中央値を中心として第１点と点対称となる点（第２点）が検出され、第１点と第２点とを結ぶ線分が楕円の長軸に決定される。また、長軸と直交する方向で、長軸から最も遠い点までの距離が検出され、中央値を通り、長軸と直交する方向に当該距離だけ離れた点（第３点）が算出される。さらに、中央値を中心として第３点と点対象となる点（第４点）が検出され、第３点と第４点とを結ぶ線分が短軸に決定される。異なる２つのセンサにおいて、楕円の大きさ、すなわち長軸および短軸の長さが同じまたはほぼ同じと判定される移動軌跡が唯一存在する場合には、それらの移動軌跡は同一であると判定される。

なお、かかる場合にも、上述の実施例と同様に、異なる２つのセンサにおいて、楕円の大きさが同じまたは略同じであると判定される移動軌跡が３つ以上存在する場合には、同一の移動軌跡とは判定しない。

また、図３２（Ａ）からも分かるように、移動軌跡の大きさを楕円で近似する場合には、円で囲む場合とは異なり、楕円内に移動軌跡が収まらない場合がある。

さらに、移動軌跡の形状に基づいて、異なる２つのセンサで検出された移動軌跡が同一であるかどうかを判定してもよい。たとえば、図３２（Ｂ）の上側に示すように、図３１（Ａ）に示した場合と同様に、ＩＣＰのアルゴリズムを用いて移動軌跡から複数の点Ｚ１−Ｚ１５を抽出する。次に、図３２(Ｂ）の下側に示すように、抽出した複数の点を用いて、時系列に従って、隣接する２点の間の距離（ここでは、ｄ_Ｚ１、ｄ_Ｚ２、…、ｄ_Ｚ１４）を算出する。距離は、上述の実施例で示した数１を用いて算出することができる。ただし、隣接する２点の座標を（ｘ１，ｙ１）および（ｘ２，ｙ２）とする。異なるなる２つのセンサにおいて、時系列に従って算出される距離の差を求め、その差の累積値が最も近似する移動軌跡が唯一存在する場合に、それらの移動軌跡が同一であると判定する。ただし、ＩＣＰのアルゴリズムを用いずに、フレーム間の距離を算出する場合には、移動軌跡の速度を積分することにより得ることもできる。かかる場合には、フレーム間の距離を算出するため、差を求めるための演算量が膨大であるが、移動軌跡の形状を正しく比較することができると考えられる。

１０ …計測装置
１２ …コンピュータ
１４ …距離センサ

Claims (7)

1. 或る環境に配置され、一定の角度毎の距離を検出する複数の距離センサの位置および向きをキャリブレーションする、距離センサのキャリブレーション装置であって、
   前記複数の距離センサの出力に基づいて、当該距離センサ毎に、前記或る環境内の人間の位置を検出する人間位置検出手段、
   前記人間位置検出手段によって検出された人間の位置に基づいて、前記距離センサ毎に、当該人間の移動軌跡を生成する軌跡生成手段、
   前記軌跡生成手段によって前記距離センサ毎に生成された移動軌跡を、当該移動軌跡の特徴に基づいて、異なる２つの前記距離センサ間で同定する軌跡同定手段、
   前記軌跡同定手段によって同定された移動軌跡において、任意に選択された２つの人間の位置に基づいて、前記２つの距離センサ間の距離および当該２つの距離センサの相対的な向きについてのセンサ制約を、当該同定された移動軌跡を生成した前記２つの距離センサの組毎に算出するセンサ制約算出手段、
   前記センサ制約算出手段によって算出されたセンサ制約を用いて、各々の前記距離センサ間の距離と相対的な向きを保つように、複数の前記距離センサを仮想のマップ上に順次配置して、前記或る環境における前記複数の距離センサの各々の位置および向きを推定する推定手段、および
   前記複数の距離センサの各々の位置および向きを、前記推定手段によって推定された他の前記複数の距離センサの各々の位置および向きと、前記センサ制約に基づいて算出するセンサ位置算出手段を備える、距離センサのキャリブレーション装置。
2. 前記センサ制約算出手段は、前記軌跡同定手段によって同定された移動軌跡において、任意に選択される２つの人間の位置のうちの少なくとも一方を変化させることにより、前記２つの距離センサの組毎に、複数の前記センサ制約を算出し、複数のセンサ制約を平均するセンサ制約平均手段を含む、請求項１記載の距離センサのキャリブレーション装置。
3. 前記複数のセンサ制約の分散を算出する分散算出手段をさらに備え、
   前記推定手段は、
   前記分散算出手段によって算出された分散が最小となる２つの距離センサを当該２つの距離センサのセンサ制約の平均に従って前記仮想のマップ上に配置する基準センサ配置手段、および
   前記基準センサ配置手段によって配置された２つの距離センサに続いて、前記分散の小さい順に、既に配置された距離センサとのセンサ制約の平均を有する距離センサを当該センサ制約の平均に従って前記仮想のマップ上に順次配置する順次配置手段を含む、請求項２記載の距離センサのキャリブレーション装置。
4. 前記センサ制約が示す前記距離センサの各組の距離のそれぞれと、前記センサ位置算出手段によって算出された前記複数の距離センサの位置に基づく当該距離センサの各組の距離のそれぞれとの差分を算出する差分算出手段、
   前記差分算出手段によって算出された差分の合計値が最小値であるかどうかを判断する最小値判断手段、および
   前記最小値判断手段によって前記差分の合計値が最小値になるまで、前記センサ位置算出手段によって算出された前記複数の距離センサの各々の位置および向きを調整する位置調整手段をさらに備える、請求項１ないし３のいずれかに記載の距離センサのキャリブレーション装置。
5. 前記分散算出手段によって算出された分散に基づいて、前記推定手段によって推定された前記複数の距離センサの各々の位置の誤差を算出する位置誤差算出手段、
   前記位置誤差算出手段によって算出された前記複数の距離センサの各々の位置の誤差の平均値を算出する誤差平均値算出手段、
   前記誤差平均値算出手段によって算出された平均値が所定値以下であるかどうかを判断する位置誤差判断手段、および
   前記位置誤差判断手段によって前記平均値が所定値以下であると判断されるまで、前記人間位置検出手段、前記軌跡生成手段、前記軌跡同定手段、前記センサ制約算出手段、前記推定手段、前記センサ位置算出手段、および前記位置調整手段を繰り返し機能させる繰り返し制御手段をさらに備える、請求項４記載の距離センサのキャリブレーション装置。
6. 或る環境に配置され、一定の角度毎の距離を検出する複数の距離センサの位置および向きをキャリブレーションする、距離センサのキャリブレーション装置のキャリブレーションプログラムであって、
   前記キャリブレーション装置のプロセッサに、
   前記複数の距離センサの出力に基づいて、当該距離センサ毎に、前記或る環境内の人間の位置を検出する人間位置検出ステップ、
   前記人間位置検出ステップによって検出された人間の位置に基づいて、前記距離センサ毎に、当該人間の移動軌跡を生成する軌跡生成ステップ、
   前記軌跡生成ステップによって前記距離センサ毎に生成された移動軌跡を、当該移動軌跡の特徴に基づいて、異なる２つの前記距離センサ間で同定する軌跡同定ステップ、
   前記軌跡同定ステップによって同定された移動軌跡において、任意に選択された２つの人間の位置に基づいて、前記２つの距離センサ間の距離および当該２つの距離センサの相対的な向きについてのセンサ制約を、当該同定された移動軌跡を生成した前記２つの距離センサの組毎に算出するセンサ制約算出ステップ、
   前記センサ制約算出ステップによって算出されたセンサ制約を用いて、各々の前記距離センサ間の距離と相対的な向きを保つように、複数の前記距離センサを仮想のマップ上に順次配置して、前記或る環境における前記複数の距離センサの各々の位置および向きを推定する推定ステップ、および
   前記複数の距離センサの各々の位置および向きを、前記推定ステップによって推定された他の前記複数の距離センサの各々の位置および向きと、前記センサ制約に基づいて算出するセンサ位置算出ステップを実行させる、キャリブレーションプログラム。
7. 或る環境に配置され、一定の角度毎の距離を検出する複数の距離センサの位置および向きをキャリブレーションする、距離センサのキャリブレーション方法であって、
   （ａ）前記複数の距離センサの出力に基づいて、当該距離センサ毎に、前記或る環境内の人間の位置を検出し、
   （ｂ）前記ステップ（ａ）によって検出された人間の位置に基づいて、前記距離センサ毎に、当該人間の移動軌跡を生成し、
   （ｃ）前記ステップ（ｂ）によって前記距離センサ毎に生成された移動軌跡を、当該移動軌跡の特徴に基づいて、異なる２つの前記距離センサ間で同定し、
   （ｄ）前記ステップ（ｃ）によって同定された移動軌跡において、任意に選択された２つの人間の位置に基づいて、前記２つの距離センサ間の距離および当該２つの距離センサの相対的な向きについてのセンサ制約を、当該同定された移動軌跡を生成した前記２つの距離センサの組毎に算出し、
   （ｅ）前記ステップ（ｄ）によって算出されたセンサ制約を用いて、各々の前記距離センサ間の距離と相対的な向きを保つように、複数の前記距離センサを仮想のマップ上に順次配置して、前記或る環境における前記複数の距離センサの各々の位置および向きを推定し、そして
   （ｆ）前記複数の距離センサの各々の位置および向きを、前記ステップ（ｅ）によって推定された他の前記複数の距離センサの各々の位置および向きと、前記センサ制約に基づいて算出する、距離センサのキャリブレーション方法。

Images