JP6192157B2 - 計測装置及びそのプログラム - Google Patents

Description

本発明は、微分形式の動座標系を用いて、計測点と計測対象点との距離を計測する計測装置及びそのプログラムに関する。

従来から、計測技術として、三点からの距離を用いる三点法が知られている。この計測技術は、無線、赤外線、レーザ、カメラなどの計測手段を用いて実現されている。近年、ＩＴ産業の進展に伴い、例えば、ゲームソフト製作の人体構造生成技術や動画像処理技術の分野でも、このような計測技術が利用されている（非特許文献１，２）。

また、従来から、三点法以外の計測技術も知られている（非特許文献３）。この非特許文献３に記載の技術は、計測対象物の付近にある、サイズが既知のスケールを撮影することで、このスケールを基準として、計測対象物の位置及び寸法を求めるものである。

株式会社エル・エー・ビー、モーションキャプチャ＆アニメーション統合ソフトウェア「ＰＶ ＳＴＵＤＩＯ ３Ｄ」、http://www.privatestudio.co.jp/pvs3d.html 株式会社ライブラリー、動画像処理専用機「なのはなシリーズ」、http://www.library-inc.co.jp/products/nanohana/03.htm 株式会社アイティーティー、単眼ビデオカメラによるリアルタイム三次元計測「シングルビュー ３Ｄ」、http://www.ittc.co.jp/sproduct/photocalc/SingleView3D/SingleView3D.htm

しかし、前記した計測技術では、静止座標系を用いているため、バイアス誤差及びランダム誤差などの誤差に関する問題と、計測点数及び不定に関する問題とが生じる。計測点（基準点）又は計測対象点の一方が移動する場合、これら問題の影響を受け、特に、計測点と計測対象点の双方が移動する場合、これら問題が顕著となる。

まず、誤差に関する問題について説明する。
任意点Ｐの位置の真値を（ｘ＾，ｙ＾，ｚ＾）とし、計測誤差を（ε_ｘ，ε_ｙ，ε_ｚ）とする。この場合、任意点Ｐの位置の計測値が（ｘ＾＋ε_ｘ，ｙ＾＋ε_ｙ，ｚ＾＋ε_ｚ）となり、静止座標系では個々の計測値の真値に誤差が加算されることになる。

次に、計測点数及び不定に関する問題について説明する。
非特許文献１，２に記載の技術では、三点法の原理により３点以上の計測が必要になり、誤差を軽減するためには多くの計測点（例えば、非特許文献２に記載の技術では８点）が必要となる。また、非特許文献３に記載の技術では、計測点が３点未満のため、寸法が既知のスケールが複数（例えば、５点）必要になる。そして、非特許文献１〜３に記載の技術では、それぞれの計測条件が整わない場合、計測値が不定になるという問題がある。

そこで、本願発明は、前記した問題を解決し、正確な計測を可能とした計測装置及びそのプログラムを提供することを課題とする。

前記した課題に鑑みて、本願第１発明に係る計測装置は、距離の計測基準となる計測基準物と、距離の計測対象である計測対象物との両方が移動しているときに、計測基準物が位置する計測点と計測対象物が位置する計測対象点との距離を計測する計測装置であって、角度算出手段と、距離算出手段と、を備えることを特徴とする。

かかる構成によれば、計測装置は、角度算出手段によって、時刻ｔ，ｔ＋１の計測値として、計測基準物の位置Ｐ _ｔ ，Ｐ _ｔ＋１ 及び計測対象物に対する計測基準物の方向Ｄ _ｔ ，Ｄ _ｔ＋１ を示す位置方向情報が入力され、計測基準物の位置Ｐ _ｔ ，Ｐ _ｔ＋１ について時刻ｔ，ｔ＋１の差分ΔＰ _ｔ＋１ 、及び、計測基準物の方向Ｄ _ｔ ，Ｄ _ｔ＋１ について時刻ｔ，ｔ＋１の差分Δφ _ｔ＋１ ，Δθ _ｔ＋１ （但し、φ _ｔ＋１ がＸ軸及びＹ軸上での方向、θ _ｔ＋１ がＸ軸、Ｙ軸及びＺ軸上での方向）を式（１）に示すように動座標基底ベクトルＥ１ _ｔ+１ ，Ｅ２ _ｔ+１ ，Ｅ３ _ｔ+１ に分解することで、微分形式の動座標系で前記方向の変化を示す角度Δφ _ｔ＋１ ，Δθ _ｔ＋１ を算出する。
この位置方向情報は、計測基準物の位置と、計測対象物に対する計測基準物の方向とを示す。

また、計測装置は、距離算出手段によって、式（２）及び式（３）で表される距離算出式により、動座標基底ベクトルＥ１ _ｔ+１ ，Ｅ２ _ｔ+１ ，Ｅ３ _ｔ+１ と、角度Δφ _ｔ+１ ，Δθ _ｔ+１ とに基づいて、微分形式の動座標系で計測点と計測対象点との距離ｒ _ｔ+１ を算出する。

ここで、任意点Ｐの位置の真値を（ｘ＾，ｙ＾，ｚ＾）とし、（Ｐ＋ΔＰ）点の位置の真値を（ｘ＾＋Δｘ＾，ｙ＾＋Δｙ＾，ｚ＾＋Δｚ＾）とし、Ｐ点とΔＰ点との位置の計測誤差を（ε_ｘ，ε_ｙ，ε_ｚ）とする。この場合、（Ｐ＋ΔＰ）点とＰ点との位置の差が（Δｘ＾，Δｙ＾，Δｚ＾）となり、バイアス誤差が相殺される。このように、計測装置は、相対的な角度を算出することで、誤差を相殺することができる。

また、前記した課題に鑑みて、参考発明に係る計測装置は、距離の計測基準となる計測基準物と、距離の計測対象である計測対象物との少なくとも一方が静止しているときに、計測基準物が位置する計測点と計測対象物が位置する計測対象点との距離を計測する計測装置であって、角度算出手段と、距離算出手段と、を備えることを特徴とする。

かかる構成によれば、計測装置は、角度算出手段によって、位置方向情報と、スケール情報とが入力され、位置方向情報及びスケール情報を動座標基底ベクトルに分解することで、微分形式の動座標系で方向の変化を示す角度を算出する。
このスケール情報は、距離を計測するためのスケールの寸法を示す。

また、計測装置は、距離算出手段によって、角度算出手段で算出した角度から、動座標数理により、微分形式の動座標系で計測点と計測対象点との距離を算出する。

ここで、任意点Ｐの位置の真値を（ｘ＾，ｙ＾，ｚ＾）とし、（Ｐ＋ΔＰ）点の位置の真値を（ｘ＾＋Δｘ＾，ｙ＾＋Δｙ＾，ｚ＾＋Δｚ＾）とし、Ｐ点とΔＰ点との位置の計測誤差を（ε_ｘ，ε_ｙ，ε_ｚ）とする。この場合、（Ｐ＋ΔＰ）点とＰ点との位置の差が（Δｘ＾，Δｙ＾，Δｚ＾）となり、バイアス誤差が相殺される。このように、計測装置は、相対的な角度を算出することで、誤差を相殺することができる。

また、本願第２発明に係る計測装置は、計測基準物としての撮影カメラで撮影された撮影画像に含まれる計測対象物をテンプレートマッチングにより抽出し、抽出した計測対象物を近似する図形に変換することで、撮影画像に含まれる計測対象物を認識する計測対象物認識手段と、角度算出手段で算出された角度と、距離算出手段で算出された距離と、計測対象物認識手段で認識された計測対象物の面積とを用いて、撮影カメラを制御するカメラ制御手段と、をさらに備えることを特徴とする。
かかる構成によれば、計測装置は、計測対象物を向くように撮影カメラを正確に制御することができる。

また、本願第３発明に係る計測装置は、位置方向情報が示す計測点の位置及び方向と、距離算出手段で算出された距離とから、予め設定された位置算出式により、計測対象点の位置を算出する位置算出手段、をさらに備えることを特徴とする。

なお、本願第１発明に係る計測装置は、コンピュータが備えるＣＰＵ（Central Processing Unit）、メモリ、ＨＤＤ（Hard Disk Drive）などのハードウェア資源を、前記した各手段として協調動作させるための計測プログラムとして実現することもできる。この計測プログラムは、通信回線を介して配布してもよく、ＣＤ−ＲＯＭやフラッシュメモリ等の記録媒体に書き込んで配布してもよい（本願第４発明）。

本願発明によれば、以下のような優れた効果を奏する。
本願第１，４発明によれば、相対的な角度を算出するため、誤差が相殺され、正確に計測を行うことができる。
本願第２発明によれば、計測対象物を向くように撮影カメラを正確に制御することができる。
本願第３発明によれば、正確な計測対象物の位置を提示することができる。

本願発明の第１，２実施形態に係る位置計測装置の構成を示すブロック図である。 本願発明の第１実施形態において、対象物の変換を説明する説明図であり、（ａ）はカメラ映像から抽出した対象物を示し、（ｂ）は対象物に近似する形状の図形を示す。 本願発明の第１実施形態において、カメラ位置情報及び撮影方向情報を説明する説明図である。 本願発明の第１実施形態において、動座標基底ベクトルへの分解を説明する説明図である。 図１の位置計測装置の動作を示すフローチャートである。 本願発明の第２実施形態において、スケールを説明する説明図である。 本願発明の第２実施形態において、動座標基底ベクトルへの分解を説明する説明図である。 本願発明の第３実施形態に係るカメラ制御装置の構成を示すブロック図である。 本願発明の第３実施形態において、動座標基底ベクトルへの分解を説明する説明図である。 本願発明の第３実施形態において、角度及び距離の算出を説明する説明図であり、（ａ）は仮想的な２台の撮影カメラを示し、（ｂ）は（ａ）の一点鎖線部分の拡大図であり、（ｃ）は（ｂ）の交点部分の拡大図である。 本願発明の第３実施形態において、距離の移動平均を説明する説明図である。 本願発明に係る計測装置の構成を示すブロック図である。 参考例１において、計測装置の動作原理を説明する説明図である。

以下、本発明の実施形態について、適宜図面を参照しながら詳細に説明する。なお、各実施形態において、同一の機能を有する手段には同一の符号を付し、説明を省略した。

[位置計測装置の概略]
図１を参照し、位置計測装置１の概略について、説明する。
位置計測装置（計測装置）１は、計測基準物が位置する計測点と対象物が位置する計測対象点との距離を計測し、この距離に基づいて対象物の位置を計測するものである。

本実施形態では、距離の計測基準となる計測基準物が、１台の撮影カメラ９２（図３）であることとする。この計測基準物は、撮影カメラ９２に限定されず、無線器、赤外線照射装置又はレーザ照射装置であってもよい。
また、本実施形態では、距離の計測対象である対象物９０（図３）が、撮影カメラ９２で撮影される被写体であることとする。
なお、対象物９０が、請求項に記載の計測対象物に相当する。

位置計測装置１は、撮影カメラ９２及び対象物９０の両方が移動する通常モード、又は、撮影カメラ９２が静止する静止モードの何れかで処理を行うこととする。以下、第１実施形態で通常モードを説明した後、第２実施形態で静止モードを説明する。

（第１実施形態：通常モード）
[位置計測装置の構成]
位置計測装置１の構成について、説明する。
図１に示すように、位置計測装置１は、モード選択手段１０と、対象物認識手段（計測対象物認識手段）１１と、入力手段１２と、動座標基底ベクトル分解手段（角度算出手段）１３と、対象物位置検出手段１４と、地図表示手段１５とを備える。

モード選択手段１０は、通常モード又は静止モードの何れか一方が、手動で選択されるものである。本実施形態では、モード選択手段１０は、通常モードが予め選択されていることとする。つまり、位置計測装置１は、通常モードの場合、第１実施形態で説明する処理を行い、静止モードの場合、第２実施形態で説明する処理を行う。

また、モード選択手段１０は、位置計測装置１が通常サンプリングを行う通常サンプリングモード、又は、高速サンプリングを行う高速サンプリングモードの何れか一方が、手動で選択されてもよい。例えば、モード選択手段１０は、対象物が高速で移動する場合、高速サンプリングモードが選択される。そして、モード選択手段１０は、通常サンプリングモード又は高速サンプリングモードを示す選択モード情報を、入力手段１２に出力する。

対象物認識手段１１は、撮影カメラ９２で対象物が撮影されたカメラ映像（撮影画像）が入力され、このカメラ映像に含まれる対象物を認識するものである。そして、対象物認識手段１１は、対象物の認識結果を入力手段１２に出力する。

図２に示すように、対象物認識手段１１は、カメラ映像に含まれる対象物９０を抽出し、抽出した対象物９０を近似する図形９１に変換することが好ましい。例えば、対象物認識手段１１は、対象物９０を様々な角度から撮影した事前データ（テンプレート画像）を予め登録しておく。また、対象物認識手段１１は、事前データを用いたテンプレートマッチングを行い、カメラ映像から対象物９０を抽出する（図２（ａ））。そして、対象物認識手段１１は、抽出した対象物９０の輪郭に最も近い図形９１を求める（図２（ｂ））。すなわち、対象物認識手段１１は、対象物９０を、円形、矩形、三角形のように、この対象物９０に近似した簡易な形状の図形９１に変換する。これによって、対象物認識手段１１は、対象物９０の認識率を向上させることができる。

入力手段１２は、外部から、カメラ情報と、カメラ位置情報と、撮影方向情報とが入力されるものである。また、入力手段１２は、モード選択手段１０から選択モード情報が入力され、対象物認識手段１１から対象物９０の認識結果が入力される。

カメラ情報は、撮影カメラ９２のピントを示す情報である。
カメラ位置情報は、撮影カメラ９２の位置を示す情報である。
撮影方向情報は、対象物９０に対する撮影カメラ９２の方向を示す情報である。
このカメラ位置情報及び撮影方向情報が、特許請求の範囲に記載の位置方向情報に相当する。

なお、入力手段１２は、モード選択手段１０から入力された選択モード情報が高速サンプリングモードを示す場合、高いサンプリング周波数で対象物９０を認識することを対象物認識手段１１に指示してもよい。

＜カメラ位置情報及び撮影方向情報＞
図３を参照し、カメラ位置情報及び撮影方向情報について、具体的に説明する（適宜図１参照）。

図３では、撮影カメラ９２を黒丸で図示し、対象物９０を黒三角で図示した（図４，図６，図７も同様）。また、図３では、時刻ｔにおける対象物の位置を符号９０_ｔと図示し、時刻ｔ+１における対象物の位置を符号９０_ｔ+１と図示した（図４も同様）。

図３に示すように、入力手段１２は、時刻ｔの計測値として、カメラ位置Ｐ_ｔ＝（ｘ_ｔ，ｙ_ｔ，ｚ_ｔ）を示すカメラ位置情報と、撮影方向Ｄ_ｔ＝（φ_ｔ，θ_ｔ）を示す撮影方向情報とが入力される。この撮影方向Ｄ_ｔは、カメラ位置Ｐ_ｔから対象物の位置９０_ｔを指している。また、時刻ｔにおいて、φ_ｔがＸ軸及びＹ軸上での撮影方向を示し、θ_ｔがＸ軸、Ｙ軸及びＺ軸上での撮影方向を示している。

また、入力手段１２は、時刻ｔ＋１の計測値として、カメラ位置Ｐ_ｔ+１＝（ｘ_ｔ+１，ｙ_ｔ+１，ｚ_ｔ+１）を示すカメラ位置情報と、撮影方向Ｄ_ｔ+１＝（φ_ｔ+１，θ_ｔ+１）を示す撮影方向情報とが入力される。この撮影方向Ｄ_ｔ+１は、カメラ位置Ｐ_ｔ+１から対象物の位置９０_ｔ+１を指している。また、時刻ｔ＋１において、φ_ｔ+１がＸ軸及びＹ軸上での撮影方向を示し、θ_ｔ+１がＸ軸、Ｙ軸及びＺ軸上での撮影方向を示している。

このように、入力手段１２は、時刻ｔ，ｔ＋１という２点分のカメラ位置情報及び撮影方向情報が入力される。そして、入力手段１２は、入力された２点分のカメラ位置Ｐ_ｔ，Ｐ_ｔ+１と、撮影方向Ｄ_ｔ，Ｄ_ｔ+１と、対象物９０の認識結果とを、動座標基底ベクトル分解手段１３に出力する。

図１に戻り、位置計測装置１の構成について、説明を続ける。
動座標基底ベクトル分解手段１３は、入力手段１２から入力された２点分のカメラ位置情報及び撮影方向情報の差分を動座標基底ベクトルに分解することで、微分形式の動座標系で撮影方向の変化を示す角度を算出するものである。
この角度は、後記する差分（Δφ_ｔ+１，Δθ_ｔ+１）のことである。

＜動座標基底ベクトルへの分解＞
図４を参照し、動座標基底ベクトルへの分解について、具体的に説明する（適宜図１参照）。

図４に示すように、動座標基底ベクトル分解手段１３は、カメラ位置Ｐ_ｔ，Ｐ_ｔ+１について、時刻ｔと時刻ｔ＋１との差分ΔＰ_ｔ+１＝（ｘ_ｔ+１−ｘ_ｔ，ｙ_ｔ+１−ｙ_ｔ，ｚ_ｔ+１−ｚ_ｔ）を算出する。また、動座標基底ベクトル分解手段１３は、撮影方向Ｄ_ｔ，Ｄ_ｔ+１について、時刻ｔと時刻ｔ＋１との差分（Δφ_ｔ+１，Δθ_ｔ+１）＝（φ_ｔ+１−φ_ｔ，θ_ｔ+１−θ_ｔ）を算出する。

そして、動座標基底ベクトル分解手段１３は、下記の式（１）に示すように、カメラ位置の差分ΔＰ_ｔ+１と、撮影方向の差分Δφ_ｔ+１，Δθ_ｔ+１とを、動座標基底ベクトルＥ１_ｔ+１，Ｅ２_ｔ+１，Ｅ３_ｔ+１に分解する。

微分形式（微分又は差分）の動座標系は、動座標数理を用いた座標系であり、例えば、下記の参考文献１，２に記載されている。
参考文献１：栗田稔、“微分形式とその応用”、２００２年６月２２日、現代数学社
参考文献２：Ｈ．フランダース、“微分形式の理論”、１９６７年６月３０日、岩波書店

動座標基底ベクトルとは、微分形式の動座標系を表すための基底ベクトルのことである。ここで、微分形式の動座標系が３次元であるため、３つの動座標基底ベクトルＥ１_ｔ+１，Ｅ２_ｔ+１，Ｅ３_ｔ+１が用いられる。

その後、動座標基底ベクトル分解手段１３は、撮影方向Ｄ_ｔ，Ｄ_ｔ+１の変化を示す角度Δφ_ｔ+１，Δθ_ｔ+１を対象物位置検出手段１４に出力する。さらに、動座標基底ベクトル分解手段１３は、動座標基底ベクトルＥ１_ｔ+１，Ｅ２_ｔ+１，Ｅ３_ｔ+１と、カメラ位置Ｐ_ｔ+１と、撮影方向Ｄ_ｔ+１とを、対象物位置検出手段１４に出力する。

図１に戻り、位置計測装置１の構成について、説明を続ける。
対象物位置検出手段１４は、動座標基底ベクトル分解手段１３より入力された角度Δφ_ｔ+１，Δθ_ｔ+１から、動座標数理により、微分形式の動座標系で計測点と計測対象点との距離を算出するものである。さらに、対象物位置検出手段１４は、算出した距離と、カメラ位置Ｐ_ｔ+１と、撮影方向Ｄ_ｔ+１とから、予め設定された位置算出式により、計測対象点の位置を算出する。
この対象物位置検出手段１４が、特許請求の範囲に記載の距離算出手段及び位置算出手段に相当する。

具体的には、対象物位置検出手段１４は、式（２）及び式（３）で表される距離算出式により、動座標基底ベクトルＥ１_ｔ+１，Ｅ２_ｔ+１，Ｅ３_ｔ+１と、角度Δφ_ｔ+１，Δθ_ｔ+１とに基づいて、計測点から計測対象点までの距離ｒ_ｔ+１を算出する。

次に、対象物位置検出手段１４は、式（４）で表される位置算出式により、カメラ位置Ｐ_ｔ+１＝（ｘ_ｔ+１，ｙ_ｔ+１，ｚ_ｔ+１）と、撮影方向Ｄ_ｔ+１＝（φ_ｔ+１，θ_ｔ+１）と、距離ｒ_ｔ+１とに基づいて、対象物の位置Ｐｍ_ｔ+１を算出する。

その後、対象物位置検出手段１４は、算出した対象物の位置Ｐｍ_ｔ+１を、地図表示手段１５に出力する。

地図表示手段１５は、対象物位置検出手段１４から入力された対象物の位置Ｐｍ_ｔ+１を、地図上に表示するものである。例えば、地図表示手段１５は、この対象物の位置Ｐｍ_ｔ+１を地図座標系に変換して、変換された対象物の位置Ｐｍ_ｔ+１を示すマーカを、ディスプレイに表示された地図に合成する。

[位置計測装置の動作]
図５を参照し、位置計測装置１の動作について、説明する（適宜図１参照）。
位置計測装置１は、対象物認識手段１１によって、カメラ映像に含まれる対象物９０を認識する（ステップＳ１）。

位置計測装置１は、入力手段１２によって、カメラ情報、カメラ位置情報、撮影方向情報などの各種情報が入力される（ステップＳ２）。
位置計測装置１は、動座標基底ベクトル分解手段１３によって、動座標基底ベクトルへの分解を行う（ステップＳ３）。

位置計測装置１は、対象物位置検出手段１４によって、微分形式の動座標系で計測点と計測対象点との距離を算出する（ステップＳ４）。
位置計測装置１は、対象物位置検出手段１４によって、計測対象点の位置を算出する（ステップＳ５）。
位置計測装置１は、地図表示手段１５によって、対象物の位置Ｐｍ_ｔ+１を地図上に表示する（ステップＳ６）。

[作用・効果]
以上のように、位置計測装置１は、相対的な角度Δφ_ｔ+１，Δθ_ｔ+１を算出するため、バイアス誤差を相殺し、正確に距離ｒ_ｔ+１を算出することができる。その結果、位置計測装置１は、対象物の正確な位置Ｐｍ_ｔ+１を地図上に表示することができる。

さらに、位置計測装置１は、時刻ｔ，時刻ｔ＋１，…というように順次処理を繰り返すことで、対象物認識手段１１における対象物の認識結果に基づいて、移動する対象物を補足（追跡）し、その位置を地図上にリアルタイムで表示することができる。

なお、位置計測装置１（対象物位置検出手段１４）は、例えば、撮影カメラ９２が自動でピントを合わせる場合、カメラ情報を用いて、撮影カメラから対象物までの距離を求めてもよい。

（第２実施形態：静止モード）
[位置計測装置の構成]
図１に戻り、位置計測装置１Ｂの構成について、第１実施形態と異なる点を説明する。
本実施形態では、第１実施形態と異なり、撮影カメラが静止し、対象物が移動していることとする。従って、モード選択手段１０では、静止モードが予め選択されている。

また、本実施形態では、位置計測装置１を符号１Ｂと記載し、入力手段１２を符号１２Ｂと記載し、動座標基底ベクトル分解手段１３を符号１３Ｂと記載し、対象物位置検出手段１４を符号１４Ｂと記載し、第１実施形態と区別することとした。
なお、対象物認識手段１１及び地図表示手段１５は、第１実施形態と同様のため、説明を省略する。

入力手段１２Ｂは、外部から、カメラ情報、カメラ位置情報及び撮影方向情報に加え、スケール情報がさらに入力されるものである。

図６に示すように、スケール９３とは、対象物９０の付近にあり、距離を計測するための物体（例えば、普通自動車）である。
スケール情報とは、スケール９３の寸法を示す情報である。図６の例では、スケール情報は、普通自動車の高さが１．５メートルで幅が２メートルといった情報である。この他、スケール情報は、電柱の高さが１４メートル、大人の身長が１．６メートル、又は、２階建て住居の高さが６メートルといった情報であってもよい。

そして、入力手段１２Ｂは、カメラ位置Ｐ_ｔ＝（ｘ_ｔ，ｙ_ｔ，ｚ_ｔ）と、撮影方向Ｄ_ｔ＝（φ_ｔ，θ_ｔ）と、対象物９０の認識結果と、スケールΔＰ_ｔ＝（Δｘ_ｔ，Δｙ_ｔ，Δｚ_ｔ）とを、動座標基底ベクトル分解手段１３Ｂに出力する。

動座標基底ベクトル分解手段１３Ｂは、入力手段１２Ｂから入力された撮影方向情報とスケール情報とを動座標基底ベクトルに分解することで、微分形式の動座標系で撮影方向の変化を示す角度を算出するものである。

＜動座標基底ベクトルへの分解＞
図７を参照し、動座標基底ベクトルへの分解について、具体的に説明する（適宜図１参照）。

図７に示すように、動座標基底ベクトル分解手段１３Ｂは、下記の式（５）に示すように、撮影方向Ｄ_ｔ＝（φ_ｔ，θ_ｔ）と、対象物の認識結果と、スケール情報ΔＰ_ｔ＝（Δｘ_ｔ，Δｙ_ｔ，Δｚ_ｔ）とを、動座標基底ベクトルＥ１_ｔ，Ｅ２_ｔ，Ｅ３_ｔに分解する。

その後、動座標基底ベクトル分解手段１３Ｂは、撮影方向Ｄ_ｔの変化を示す角度Δφ_ｔ，Δθ_ｔを対象物位置検出手段１４Ｂに出力する。さらに、動座標基底ベクトル分解手段１３Ｂは、動座標基底ベクトルＥ１_ｔ，Ｅ２_ｔ，Ｅ３_ｔと、カメラ位置Ｐ_ｔと、撮影方向Ｄ_ｔとを、対象物位置検出手段１４Ｂに出力する。

図１に戻り、位置計測装置１Ｂの構成について、説明を続ける。
対象物位置検出手段１４Ｂは、動座標基底ベクトル分解手段１３Ｂより入力された角度Δφ_ｔ，Δθ_ｔから、動座標数理により、微分形式の動座標系で計測点と計測対象点との距離を算出するものである。さらに、対象物位置検出手段１４Ｂは、算出した距離と、カメラ位置Ｐ_ｔと、撮影方向Ｄ_ｔとから、予め設定された位置算出式により、計測対象点の位置を算出する。

具体的には、対象物位置検出手段１４Ｂは、式（６）で表される距離算出式により、動座標基底ベクトルＥ２_ｔ，Ｅ３_ｔと、角度Δφ_ｔ，Δθ_ｔと、撮影方向φ_ｔとに基づいて、計測点から計測対象点までの距離ｒ_ｔを算出する。

次に、対象物位置検出手段１４Ｂは、式（７）で表される位置算出式により、カメラ位置Ｐ_ｔ＝（ｘ_ｔ，ｙ_ｔ，ｚ_ｔ）と、撮影方向Ｄ_ｔ＝（φ_ｔ，θ_ｔ）と、距離ｒ_ｔとに基づいて、対象物の位置Ｐｍ_ｔを算出する。

その後、対象物位置検出手段１４Ｂは、算出した対象物の位置Ｐｍ_ｔを、地図表示手段１５に出力する。

[作用・効果]
以上のように、位置計測装置１Ｂは、撮影カメラが静止している場合でも、第１実施形態と同様の効果を奏する。
また、第１，２実施形態では、対象物が移動することとして説明したが、撮影カメラ（計測基準物）が移動し、対象物（計測対象物）が移動又は静止してもよい。

（第３実施形態）
[カメラ制御装置の構成]
図８を参照し、カメラ制御装置２の構成について、説明する。

カメラ制御装置（計測装置）２は、計測点と計測対象点との方向が変化した角度、及び、計測点と計測対象点との距離を算出し、これら角度及び距離に基づいて、撮影カメラ９２を制御するものである。図８に示すように、カメラ制御装置２は、対象物認識手段２０と、角度算出手段２１_１，２１_２と、距離算出手段２２と、面積算出手段２３と、ズーム制御手段２４と、パン・チルト制御手段２５とを備える。

本実施形態では、計測基準物が静止した被写体９０（図３）であり、対象物が撮影カメラ９２であることとする。
また、本実施形態では、１台の撮影カメラ９２が移動することとし、移動前の撮影カメラ９２_１と、移動後の撮影カメラ９２_２とを仮想的に２台の撮影カメラ９２として扱うこととする。
以後、１台目の撮影カメラに符号９２_１を付し、２台目の撮影カメラに符号９２_２を付して説明する。

撮影カメラ９２は、カメラ本体９２ａと、雲台９２ｂとを備える。
カメラ本体９２ａは、ズーム制御信号に基づいて、レンズのズームを駆動する。
雲台９２ｂは、カメラ本体９２ａを搭載すると共に、パン・チルト制御信号に基づいて、カメラ本体９２ａをパン方向及びチルト方向に駆動する。

対象物認識手段２０は、画像認識・追跡手段２０_１，２０_２を備える。
画像認識・追跡手段２０_１は、撮影カメラ９２_１のカメラ映像（撮影画像）が入力され、このカメラ映像に含まれる被写体９０を認識するものである。そして、対象物認識手段２０_１は、被写体９０の認識結果（画面上の座標）を角度算出手段２１_１及び面積算出手段２３に出力する。

画像認識・追跡手段２０_２は、撮影カメラ９２_２のカメラ映像を対象とする以外、画像認識・追跡手段２０_１と同様のため、説明を省略する。

角度算出手段２１_１は、撮影カメラ９２_１について、カメラ位置情報及び撮影方向情報の差分を動座標基底ベクトルに分解することで、微分形式の動座標系で方向の変化を示す角度を算出するものである。
なお、本実施形態では、被写体９０の位置が既知（固定）であり、撮影カメラ９２に対する姿勢制御の結果として、撮影方向が定まるため、カメラ位置情報及び撮影方向情報を外部から入力せずともよい。

具体的には、角度算出手段２１_１は、図９に示すように、下記の式（８）〜式（１０）を用いて、動座標基底ベクトルｅ_ｘ１〜ｅ_ｚ３への分解を行い、角度φ_，θを算出する。

この図９では、撮影カメラ９２_１が原点Ｏ＝（０，０，０）に位置し、撮影カメラ９２_２（図９では省略）が任意の場所に位置することとする。また、被写体９０の位置Ｐは、デカルト座標で（ｘ，ｙ，ｚ）で表される。また、点Ｏ−Ｐ間の距離がｒである。すなわち、この図９は、デカルト座標（ｘ，ｙ，ｚ）と、球座標系（ｒ，θ，φ）との対応関係を表している。ここで、距離ｒも、距離の差分ｄｒも未知であることとする。

その後、角度算出手段２１_１は、算出した動座標基底ベクトルｅ_ｘ１〜ｅ_ｚ３と、角度φ_，θとを、距離算出手段２２に出力する。
角度算出手段２１_２は、撮影カメラ９２_２を対象とする以外、角度算出手段２１_１と同様のため、説明を省略する。

距離算出手段２２は、角度算出手段２１_１，２１_２より入力された角度から、動座標数理により、微分形式の動座標系で計測点と計測対象点との距離を算出するものである。

ここで、距離ｒを求める説明を簡易にするため、式（９）のｄｘ及びｄｙが変化し、ｄｚが一定の場合を考える。すなわち、ｄｚ＝０、ｄθ＝０、ｓｉｎθ＝１が成立する。この場合、下記の式（１１）から式（１２）を導き出すことができる。

そして、式（１２）を下記の式（１３）に代入して、式（１４）を導き出すことができる。さらに、式（１４）を距離ｒで解いて、式（１５）を導き出すことができる。従って、距離算出手段２２は、式（１５）で表される距離算出式を用いて、距離ｒを算出する。

ここで、ｄφを微小とし、ｄφがｓｉｎｄφに近似する場合、下記の式（１６）が成立する。従って、距離算出手段２２は、下記の式（１７）及び式（１８）を用いて、距離の差分ｄｒを算出する。ここで、ｄｓは、任意の２点間の距離を表す。

図１０（ａ）に示すように、時刻ｔにおける撮影カメラ９２_１を符号９２_ｔで図示し、時刻ｔ+１における撮影カメラ９２_２を符号９２_ｔ+１で図示した。この場合、図１０（ｂ）に示すように、距離算出手段２２は、撮影カメラ９２_ｔ，９２_ｔ+１の光軸交点で、基底による動きの抽出を行って、時刻ｔにおける距離方向の基底ベクトルｅ_ｒｔと、角度方向の基底ベクトルｅ_θｔ，ｅ_φｔとを抽出する（時刻ｔ+１も同様）。図１０（ｃ）に示すように、距離算出手段２２は、動座標数理による動きの接続を行って、基底ベクトルｅ_ｒｔと基底ベクトルｅ_ｒｔ+１との差分を、距離の差分ｄｒとして算出する。

その後、距離算出手段２２は、距離ｒ及び距離の差分ｄｒをズーム制御手段２４に出力し、角度φ_，θをパン・チルト制御手段２５に出力する。
なお、距離算出手段２２に入力された直交座標は、デカルト座標系で撮影カメラ９２と被写体９０との位置を表すために用いられる。

図８に戻り、カメラ制御装置２の構成について、説明を続ける。
面積算出手段２３は、角度算出手段２１_１，２１_２から入力された被写体９０の面積を算出するものである。例えば、面積算出手段２３は、被写体９０のエッジを抽出し、このエッジ領域の面積を求める。そして、面積算出手段２３は、算出した被写体９０の面積をズーム制御手段２４に出力する。

ズーム制御手段２４は、距離算出手段２２から入力された距離ｒ及び距離の差分ｄｒと、面積算出手段２３から入力された被写体９０の面積とに基づいて、撮影カメラ９２（カメラ本体９２ａ）のズームを制御するものである。

例えば、ズーム制御手段２４は、距離ｒが遠くなる程、撮影カメラ９２がズームインし、距離ｒが近くなる程、撮影カメラ９２がズームアウトするようなズーム制御信号を生成し、カメラ本体９２ａに出力する。

このとき、ズーム制御手段２４は、距離ｒの移動平均を用いて、ズーム制御を行うことが好ましい。図１１に示すように、ズーム制御手段２４は、距離ｒの計算値（黒丸で図示）から、距離ｒの移動平均９４を算出する。そして、ズーム制御手段２４は、この移動平均９４で表される距離ｒに基づいて、ズーム制御信号を生成する。これによって、カメラ制御装置２は、ズーム制御の誤差を低減することができる。

また、例えば、ズーム制御手段２４は、被写体９０の面積が狭くなる程、撮影カメラ９２がズームインし、被写体９０の面積が広くなる程、撮影カメラ９２がズームアウトするようなズーム制御信号を生成し、カメラ本体９２ａに出力する。

パン・チルト制御手段２５は、距離算出手段２２から入力された角度φ_，θに基づいて、撮影カメラ９２（雲台９２ｂ）のパン及びチルトを制御するものである。つまり、パン・チルト制御手段２５は、この角度φ_，θに基づいて、撮影カメラ９２が被写体９０を向くようなパン・チルト制御信号を生成し、雲台９２ｂに出力する。
ズーム制御手段２４及びパン・チルト制御手段２５が、特許請求の範囲に記載のカメラ制御手段に相当する。

[作用・効果]
以上のように、カメラ制御装置２は、相対的な角度φ，θ_ｔを算出するため、バイアス誤差が相殺され、正確に距離ｒを算出することができる。その結果、カメラ制御装置２は、被写体９０を向くように撮影カメラ９２を正確に制御することができる。

なお、前記した実施形態では、本願発明に係る計測装置の一例として、位置計測装置１，１Ｂ及びカメラ制御装置２を説明したが、本願発明はこれらに限定されない。すなわち、図１２に示すように、本願発明に係る計測装置３は、角度算出手段２１_１，２１_２と、距離算出手段２２とを備えることで、実現可能である。

また、第３実施形態では、被写体９０の位置を計測しないこととして説明したが、図１の対象物位置検出手段１４（１４Ｂ）と同様の手法で、被写体９０の位置を計測してもよい。この場合、カメラ制御装置２は、計測した被写体９０の位置に基づいて、この撮影カメラ９２のズーム比を制御することができる。

（参考例１：計測装置の動作原理）
図１３を参照し、参考例１として、本願発明に係る計測装置の動作原理について、説明する。

図１３に示すように、１台目の撮影カメラ９２_１が原点（０，０，０）に配置され、２台目の撮影カメラ９２_２が座標（ｘ_０，ｙ_０，ｚ_０）に配置されていることとする。

また、対象物の位置がＰ_１からＰ_２に移動し、その移動量ΔＰ＝（Δｘ，Δｙ，Δｚ）が不明であることとする。
また、撮影カメラ９２_１から位置Ｐ_１までの距離がｒ_１であり、撮影カメラ９２_１から位置Ｐ_２までの距離がｒ_１´である。

また、撮影カメラ９２_２から位置Ｐ_１までの距離がｒ_２であり、撮影カメラ９２_２から位置Ｐ_２までの距離がｒ_２´である。
また、Δｒ_１＝ｒ_１−ｒ_２であり、Δｒ_２＝ｒ_１´−ｒ_２´である。また、撮影カメラ９２_１の角度がφ_１，θ_１であり、撮影カメラ９２_２の角度がφ_２，θ_２である。

撮影カメラ９２_１から見て、ΔＰは、下記の式（１９）で表すことができる。また、撮影カメラ９２_２から見て、ΔＰは、下記の式（２０）で表すことができる。

式（１９）及び式（２０）より、距離ｒ_１，ｒ_２は、下記の式（２１）で表すことができる。

また、撮影カメラ９２_１について、式（２２）〜式（２４）に示すように、角度φ_１，θ_１を動座標基底ベクトルｅ_１１，ｅ_２１，ｅ_３１に分解する。

また、撮影カメラ９２_２について、式（２５）〜式（２７）に示すように、角度φ_２，θ_２を動座標基底ベクトルｅ_１２，ｅ_２２，ｅ_３２に分解する。

前記した式（２１）〜式（２７）を解くことで、距離ｒ_１，ｒ_２と、移動量ΔＰとを求めることができる。

（参考例２：参考例１の別解法）
参考例２として、参考例１の別解法について、説明する。

ここで、動座標基底ベクトルｅ_１，ｅ_２，ｅ_３は、下記の式（２８）で表される。また、移動量ｄＰ＝Ｐ_２−Ｐ_１は、下記の式（２９）で表される。

式（２８）及び式（２９）を連立方程式で解くと、下記の式（３０）が成立し、２台の撮影カメラ９２_１，９２_２から見て、移動量ｄＰが等しくなる。この式（３０）を、式（３１）の微分方程式として解く。

なお、φ_Ａ，θ_Ａ，ｅ_１Ａ〜ｅ_３Ａは、撮影カメラ９２_１についての角度φ_，θと、動座標基底ベクトルｅ_１〜ｅ_３である。また、φ_Ｂ，θ_Ｂ，ｅ_１Ｂ〜ｅ_３Ｂは、撮影カメラ９２_２についての角度φ_，θと、動座標基底ベクトルｅ_１〜ｅ_３である。
ここで、式（３２）が成立し、ｔ＝０のときにｒ＝ｒ_１であるから、式（３３）が成立する。

式（３３）の‘Ｃ’を式（３２）に代入して式（３４）が得られ、この式を２台の撮影カメラに対応した式（３５）及び式（３６）に変換する。

ここで、式（３７）のように位置Ｐを定義し、Ｘ軸方向で考えると、式（３８）及び式（３９）が成立する。

また、Ｘ軸方向と同様、Ｙ軸方向で考えると、式（４０）及び式（４１）が成立する。

また、Ｘ軸方向と同様、Ｚ軸方向で考えると、式（４２）及び式（４３）が成立する。

前記した式（３９）、式（４１）及び式（４３）により、距離ｒ_Ａｘ，ｒ_Ｂｘ，ｒ_Ａｙ，ｒ_Ｂｙ，ｒ_Ａｚ，ｒ_Ｂｚを求めることができる。以上のように、参考例１，２の何れでも、計算結果が一致する。

１，１Ｂ 位置計測装置
１０ モード選択手段
１１，２０ 対象物認識手段（計測対象物認識手段）
１２，１２Ｂ 入力手段
１３，１３Ｂ 動座標基底ベクトル分解手段（角度算出手段）
１４，１４Ｂ 対象物位置検出手段（距離算出手段及び位置算出手段）
１５ 地図表示手段
２ カメラ制御装置
２０_１，２０_２ 画像認識・追跡手段
２１_１，２１_２ 角度算出手段
２２ 距離算出手段
２３ 面積算出手段
２４ ズーム制御手段（カメラ制御手段）
２５ パン・チルト制御手段（カメラ制御手段）

Claims (4)

1. 距離の計測基準となる計測基準物と、前記距離の計測対象である計測対象物との両方が移動しているときに、前記計測基準物が位置する計測点と前記計測対象物が位置する計測対象点との距離を計測する計測装置であって、
   時刻ｔ，ｔ＋１の計測値として、前記計測基準物の位置Ｐ _ｔ ，Ｐ _ｔ＋１ 及び前記計測対象物に対する計測基準物の方向Ｄ _ｔ ，Ｄ _ｔ＋１ を示す位置方向情報が入力され、前記計測基準物の位置Ｐ _ｔ ，Ｐ _ｔ＋１ について前記時刻ｔ，ｔ＋１の差分ΔＰ _ｔ＋１ 、及び、前記計測基準物の方向Ｄ _ｔ ，Ｄ _ｔ＋１ について前記時刻ｔ，ｔ＋１の差分Δφ _ｔ＋１ ，Δθ _ｔ＋１ （但し、φ _ｔ＋１ がＸ軸及びＹ軸上での方向、θ _ｔ＋１ がＸ軸、Ｙ軸及びＺ軸上での方向）を式（１）に示すように動座標基底ベクトルＥ１ _ｔ+１ ，Ｅ２ _ｔ+１ ，Ｅ３ _ｔ+１ に分解することで、微分形式の動座標系で前記方向の変化を示す角度Δφ _ｔ＋１ ，Δθ _ｔ＋１ を算出する角度算出手段と、
   

   

   式（２）及び式（３）で表される距離算出式により、前記動座標基底ベクトルＥ１ _ｔ+１ ，Ｅ２ _ｔ+１ ，Ｅ３ _ｔ+１ と、前記角度Δφ _ｔ+１ ，Δθ _ｔ+１ とに基づいて、前記微分形式の動座標系で前記計測点と前記計測対象点との距離ｒ _ｔ+１ を算出する距離算出手段と、
   

   

   

   を備えることを特徴とする計測装置。
2. 前記計測基準物としての撮影カメラで撮影された撮影画像に含まれる計測対象物をテンプレートマッチングにより抽出し、抽出した前記計測対象物を近似する図形に変換し、前記撮影画像に含まれる計測対象物を認識する計測対象物認識手段と、
   前記角度算出手段で算出された角度と、前記距離算出手段で算出された距離と、前記計測対象物認識手段で認識された計測対象物の面積とを用いて、前記撮影カメラを制御するカメラ制御手段と、
   をさらに備えることを特徴とする請求項１に記載の計測装置。
3. 前記位置方向情報が示す計測点の位置及び方向と、前記距離算出手段で算出された距離とから、予め設定された位置算出式により、前記計測対象点の位置を算出する位置算出手段、
   をさらに備えることを特徴とする請求項１又は請求項２に記載の計測装置。
4. コンピュータを、請求項１に記載の計測装置として機能させるための計測プログラム。

Images