(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2023-10-10
(45)【発行日】2023-10-18
(54)【発明の名称】計測装置、計測システム、計測方法およびプログラム
(51)【国際特許分類】
G01B 11/26 20060101AFI20231011BHJP
G06T 7/70 20170101ALI20231011BHJP
G01C 3/06 20060101ALI20231011BHJP
【ＦＩ】
G01B11/26 H
G06T7/70 Z
G01C3/06 120S
【請求項の数】
10
(21)【出願番号】P 2019162982
(22)【出願日】2019-09-06
(65)【公開番号】P2021042978
(43)【公開日】2021-03-18
【審査請求日】2022-08-15
(73)【特許権者】
【識別番号】000004237
【氏名又は名称】日本電気株式会社
(74)【代理人】
【識別番号】100109313
【弁理士】
【氏名又は名称】机 昌彦
(74)【代理人】
【識別番号】100149618
【弁理士】
【氏名又は名称】北嶋 啓至
(72)【発明者】
【氏名】中川 大輔
【審査官】山▲崎▼ 和子
(56)【参考文献】
【文献】特開２０００－２７５０１３（ＪＰ，Ａ）
【文献】特開２０１８－１８５２３９（ＪＰ，Ａ）
【文献】特開２０００－３３７８８７（ＪＰ，Ａ）
【文献】特開２００９－０３１２０６（ＪＰ，Ａ）
【文献】特開２０１１－１２３００８（ＪＰ，Ａ）
【文献】特開２００５－３１５７４６（ＪＰ，Ａ）
【文献】米国特許出願公開第２００２／００５２７０９（ＵＳ，Ａ１）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０１Ｂ １１／００－１１／３０
Ｇ０１Ｃ １／００－１５／１４
Ｇ０６Ｔ １／００－１／４０
３／００－５／５０
７／００－７／９０
Ｇ０６Ｖ １０／００－２０／９０
３０／４１８、４０／１６、４０／２０
(57)【特許請求の範囲】
【請求項1】
移動体が有するカメラが撮影した前記移動体の周囲の画像を取得する画像取得手段と、前記画像から複数の特徴点を抽出する特徴点抽出手段と、
前記特徴点に対応する構造物を特定する特定手段と、
前記構造物ごとにあらかじめ保存された前記構造物の大きさに関するデータを基に、特定した前記構造物から前記移動体までの距離を計算する距離計算手段と、
前記移動体から複数の前記構造物までの距離および前記構造物のそれぞれの座標データを基に、前記移動体と前記構造物それぞれの間の直線がなす角度を計算し、計算結果に基づいて、前記移動体の座標および姿勢を決定し、前記移動体の座標および姿勢の情報を出力する姿勢決定手段と
を備えることを特徴とする計測装置。
【請求項2】
前記移動体の周囲の構造物と、前記構造物上の特徴点の情報をデータベースとして登録している特徴点登録手段をさらに備え、前記特定手段は、前記データベースを参照して前記構造物を特定することを特徴とする請求項１に記載の計測装置。
【請求項3】
前記距離計算手段は、特定した前記構造物の画像中における大きさと、前記データベースに登録された前記構造物の大きさに関するデータを基に、前記構造物から前記移動体までの距離を計算することを特徴とする請求項２に記載の計測装置。
【請求項4】
前記特定手段は、少なくとも３個の前記構造物を特定することを特徴とする請求項１から３いずれかに記載の計測装置。
【請求項5】
前記特徴点抽出手段は、前記画像の輝度変化を基に前記特徴点を抽出することを特徴とする請求項１から４いずれかに記載の計測装置。
【請求項6】
周囲を撮影し、撮影した画像のデータを出力するカメラと、請求項１から５いずれかに記載の計測装置と
を備え、
前記計測装置の前記画像取得手段は、前記カメラから前記画像を取得することを特徴とする計測システム。
【請求項7】
移動体が有するカメラが撮影した前記移動体の周囲の画像を取得し、前記画像から複数の特徴点を抽出し、
前記特徴点に対応する構造物を特定し、
前記構造物ごとにあらかじめ保存された前記構造物の大きさに関するデータを基に、特定した前記構造物から前記移動体までの距離を計算し、
前記移動体から複数の前記構造物までの距離および前記構造物のそれぞれの座標データを基に、前記移動体と前記構造物それぞれの間の直線がなす角度を計算し、
計算結果に基づいて、前記移動体の座標および姿勢を決定し、
前記移動体の座標および姿勢の情報を出力することを特徴とする計測方法。
【請求項8】
前記移動体の周囲の構造物と、前記構造物上の特徴点の情報をデータベースとして保持し、前記データベースを参照して前記構造物を特定することを特徴とする請求項７に記載の計測方法。
【請求項9】
少なくとも３個の前記構造物を特定することを特徴とする請求項７または８に記載の計測方法。
【請求項10】
移動体が有するカメラが撮影した前記移動体の周囲の画像を取得する処理と、前記画像から複数の特徴点を抽出する処理と、
前記特徴点に対応する構造物を特定する処理と、
前記構造物ごとにあらかじめ保存された前記構造物の大きさに関するデータを基に、特定した前記構造物から前記移動体までの距離を計算する処理と、
前記移動体から複数の前記構造物までの距離および前記構造物のそれぞれの座標データを基に、前記移動体と前記構造物それぞれの間の直線がなす角度を計算し、計算結果に基づいて、前記移動体の座標および姿勢を決定し、前記移動体の座標および姿勢の情報を出力する処理と
をコンピュータに実行させることを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体の位置および姿勢の情報を取得する技術に関するものである。
【背景技術】
【0002】
移動体の制御を行う上で、移動体の位置および姿勢は重要な情報である。また、移動体の大型化や重量の増加を避けるため、移動体の位置および姿勢を計測する装置は、大型化や複雑化が抑制されることが望ましい。
【0003】
移動体の制御装置は、ミリ波レーダー、ＬＩＤＡＲ（Light Detection and Ranging）、加速度センサ、ジャイロセンサおよびカメラのうち、いくつかを組み合わせた構成によって周囲の環境や移動体の情報を取得することが多い。上記のうち、ミリ波レーダーやＬＩＤＡＲは、周囲の障害物までの距離の情報を取得するために用いられる。加速度センサやジャイロセンサは移動体の運動の方向や姿勢の情報を取得するために用いられる。また、カメラは、回避対象の移動体等の検出や認識のために用いられる。しかし、ミリ波レーダーやＬＩＤＡＲは、高価であるとともに、運動方向を知るために、加速度センサやジャイロセンサを組み合わせる必要があるため装置の構成が複雑なものとなり得る。そのため、計測装置の大型化や複雑化を抑制しつつ、移動体の制御に必要な位置および姿勢の情報を取得できることが望ましい。そのような、移動体の制御に位置や姿勢の情報を計測装置の大型化や複雑化を抑制しつつ得るための技術としては、例えば、特許文献１のような技術が開示されている。
【0004】
特許文献１は、取得した画像からカメラの位置および姿勢を推定する位置・姿勢推定装置に関するものである。特許文献１の位置・姿勢推定装置は、画像上の特徴点と、あらかじめマップ点として登録されている基準となる点との誤差を計算し、カメラの姿勢と位置の推定を行っている。
【先行技術文献】
【特許文献】
【0005】
【文献】特開２０１６－９１０６５号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
しかしながら、特許文献１の技術は次のような点で十分ではない。特許文献１では、カメラの位置と姿勢を推定する際に２台のカメラで撮像した画像が必要となる。よって、特許文献１の技術では、移動体にカメラを複数、搭載する必要があるため装置の大型化する恐れがある。そのため、特許文献１の技術は、装置の大型化や複雑化を抑制しつつ移動体の位置および姿勢を計測する技術としては十分ではない。
【0007】
本発明は、装置を大型化することなく移動体の位置および姿勢を計測することができる計測装置を提供することを目的としている。
【課題を解決するための手段】
【0008】
上記の課題を解決するため、本発明の計測装置は、計測装置は、画像取得手段と、特徴点抽出手段と、特定手段と、距離計算手段と、姿勢決定手段を備えている。画像取得手段は、移動体が有するカメラが撮影した移動体の周囲の画像を取得する。特徴点抽出手段は、画像から複数の特徴点を抽出する。特定手段は、特徴点に対応する構造物を特定する。距離計算手段は、造物ごとにあらかじめ保存された構造物の大きさに関するデータを基に、特定した構造物から移動体までの距離を計算する。姿勢特定手段は、移動体から複数の構造物までの距離および構造物のそれぞれの座標データを基に移動体の座標および姿勢を決定し、移動体の座標および姿勢の情報を出力する。
【0009】
本発明の計測方法は、移動体が有するカメラが撮影した前記移動体の周囲の画像を取得し、画像から複数の特徴点を抽出し、特徴点に対応する構造物を特定する。本発明の距離姿勢特定方法は、構造物ごとにあらかじめ保存された構造物の大きさに関するデータを基に、特定した構造物から移動体までの距離を計算する。本発明の距離姿勢特定方法は、移動体から複数の構造物までの距離および構造物のそれぞれの座標データを基に移動体の座標および姿勢を決定し、移動体の座標および姿勢の情報を出力する。
【発明の効果】
【0010】
本発明によると、装置を大型化することなく画像を撮影したカメラの位置および姿勢を計測することができる。
【図面の簡単な説明】
【0011】
【図1】本発明の第１の実施形態の構成の概要を示す図である。
【図2】本発明の第２の実施形態の構成の概要を示す図である。
【図3】本発明の第２の実施形態の動作を示すフローチャートである。
【図4】本発明の第２の実施形態における撮影部のカメラと各構造物の特徴点を模式的に示した図である。
【図5】本発明の第２の実施形態における撮影部のカメラと撮影対象物の大きさと距離の関係を模式的に示した図である。
【図6】本発明の第２の実施形態における撮影部のカメラと構造物の位置を模式的に示した図である。
【図7】本発明の第２の実施形態の計測装置の他の構成の例を示した図である。
【発明を実施するための形態】
【0012】
（第１の実施形態）
本発明の第１の実施形態について図を参照して詳細に説明する。図１は、本実施形態の計測装置の構成の概要を示した図である。本実施形態の計測装置は、画像取得部１と、特徴点抽出部２と、特定部３と、距離計算部４と、姿勢決定部５を備えている。画像取得部１は、移動体が有するカメラが撮影した移動体の周囲の画像を取得する。特徴点抽出部２は、画像から複数の特徴点を抽出する。特定部３は、抽出された特徴点に対応する構造物を特定する。ここで、特定とは、識別または推定に相当する。距離計算部４は、構造物ごとにあらかじめ保存された構造物の大きさに関するデータを基に、特定した前記構造物から移動体までの距離を計算する。姿勢決定部５は、移動体から複数の構造物までの距離および構造物のそれぞれの座標データを基に移動体の座標および姿勢を決定し、移動体の座標および姿勢の情報を出力する。
本発明の第１の実施形態について図を参照して詳細に説明する。図１は、本実施形態の計測装置の構成の概要を示した図である。本実施形態の計測装置は、画像取得部１と、特徴点抽出部２と、特定部３と、距離計算部４と、姿勢決定部５を備えている。画像取得部１は、移動体が有するカメラが撮影した移動体の周囲の画像を取得する。特徴点抽出部２は、画像から複数の特徴点を抽出する。特定部３は、抽出された特徴点に対応する構造物を特定する。ここで、特定とは、識別または推定に相当する。距離計算部４は、構造物ごとにあらかじめ保存された構造物の大きさに関するデータを基に、特定した前記構造物から移動体までの距離を計算する。姿勢決定部５は、移動体から複数の構造物までの距離および構造物のそれぞれの座標データを基に移動体の座標および姿勢を決定し、移動体の座標および姿勢の情報を出力する。
【0013】
本実施形態の計測装置は、画像取得部１において取得した画像から特徴点抽出部２において特徴点を抽出し、特定部３が特徴点に対応する構造物を特定する。また、本実施形態の計測装置は、距離計算部４において構造物から移動体までの距離を計算し、姿勢決定部５において移動体の座標と姿勢を決定する。本実施形態の計測装置は、１台のカメラにおいて取得した画像から移動体の座標と姿勢を決定できる。そのため、装置の大型化を抑制することができる。
【0014】
（第２の実施形態）
本発明の第２の実施形態について図を参照して詳細に説明する。図２は、本実施形態の計測システムの構成の概要を示した図である。本実施形態の計測システムは、撮影部１０と、距離・姿勢計測部２０を備えている。本実施形態の計測システムは、に移動体の位置および姿勢を計測し、移動体の動作を制御する制御部に移動体の位置および姿勢の情報を出力するシステムである。本実施形態の計測システムは、撮影部１０のカメラが撮影した画像を基に、移動体の位置および姿勢の計測を行う。本実施形態の計測システムは、例えば、ドローン等の無人航空機、自律制御によって飛行する飛行体または自動運転車等の移動体の制御部に移動体の位置および姿勢の情報を提供するシステムとして用いることができる。
本発明の第２の実施形態について図を参照して詳細に説明する。図２は、本実施形態の計測システムの構成の概要を示した図である。本実施形態の計測システムは、撮影部１０と、距離・姿勢計測部２０を備えている。本実施形態の計測システムは、に移動体の位置および姿勢を計測し、移動体の動作を制御する制御部に移動体の位置および姿勢の情報を出力するシステムである。本実施形態の計測システムは、撮影部１０のカメラが撮影した画像を基に、移動体の位置および姿勢の計測を行う。本実施形態の計測システムは、例えば、ドローン等の無人航空機、自律制御によって飛行する飛行体または自動運転車等の移動体の制御部に移動体の位置および姿勢の情報を提供するシステムとして用いることができる。
【0015】
撮影部１０は、移動体の位置と姿勢を計測する際に用いる移動体の周辺環境の画像を撮影する機能を有する。本実施形態の撮影部１０は、可視光カメラを用いて構成されている。撮影部１０は、赤外線カメラを備えていてもよい。撮影部１０は、撮影した画像のデータを距離・姿勢計測部２０に送る。また、撮影部１０と距離・姿勢計測部２０は、一体の装置として形成されていてもよく、また、別に形成された撮影部１０と距離・姿勢計測部２０の間で通信ケーブルを介した通信や無線通信により通信を行う構成であってもよい。
【0016】
距離・姿勢計測部２０の構成について説明する。距離・姿勢計測部２０は、画像取得部２１と、特徴点抽出部２２と、特定部２３と、距離計算部２４と、姿勢決定部２５と、特徴点登録部２６を備えている。
【0017】
画像取得部２１は、撮影部１０から画像データを取得するインタフェースとしての機能を有する。画像取得部２１は、撮影部１０から取得した画像データを距離・姿勢計測部２０内部で用いるデータ形式に変換して特徴点抽出部２２に出力する。
【0018】
特徴点抽出部２２は、撮影部１０のカメラが撮影した移動体の周辺の画像から特徴点を抽出する機能を有する。特徴点抽出部２２は、移動体の周辺の画像から特徴点として画像中の輝度変化が大きい点を抽出し、抽出した点と周囲との輝度勾配および輝度変化の向きを計算する。輝度変化が大きい点とは、例えば、所定のピクセル数あたりの輝度変化が周辺領域の輝度変化よりもあらかじめ設定された値以上、大きい点のことをいう。特徴点抽出部２２は、例えば、輝度変化が大きい点である構造物の各辺の端点などを特徴点として抽出する。また、特徴点抽出部２２は、ＳＩＦＴ（Scale Invariant Feature Transform）を用いて特徴点を検出し、特徴点ベクトルを特徴点の情報として出力してもよい。特徴点抽出部２２は、抽出した特徴点の情報を特定部２３に出力する。
【0019】
特定部２３は、特徴点登録部２６のデータベースを参照して、特徴点抽出部２２が抽出した特徴点が対応する構造物を特定する。特定部２３は、特徴点抽出部２２が抽出した特徴点と一致する構造物を特定する。特定部２３は、データベース上の構造物の拡大と縮小を行った際に、構造物上の特徴点の相互の位置関係が画像から抽出された特徴点の相互の位置関係と一致する構造物を抽出し、画像中の特徴点が対応する構造物として特定する。特定部２３は、特徴点が対応するとして構造物として特定した構造物の座標データを距離計算部２４に出力する。
【0020】
距離計算部２４は、構造物の座標データを基に、画像中の構造物と撮影部１０の距離を計算する。構造物や移動体の位置を示す座標は、あらかじめ設定された基準点を原点として３次元の座標系として設定されている。距離計算部２４における構造物と撮影部１０の距離の計算方法については後で説明する。距離計算部２４は、計算した構造物と撮影部１０の距離の情報と、構造物の座標データを姿勢決定部２５に出力する。
【0021】
姿勢決定部２５は、計算した構造物と撮影部１０との距離の情報と、構造物の座標データを基に、撮影部１０の姿勢を決定する。姿勢決定部２５は、撮影部１０を基準とした場合に２個の構造物のなす角度を計算する。すなわち、姿勢決定部２５は、撮影部１０から２個の構造物それぞれに直線を引いた場合の、２本の直線のなす角度を計算する。姿勢決定部２５は、例えば、３個の構造物のうち２個の構造物のなす角度を、構造物の組み合わせを変えてそれぞれ計算し、撮影部１０の姿勢を決定する。姿勢決定部２５における構造物のなす角度の計算方法については後で説明する。画像取得部２１、特徴点抽出部２２、特定部２３、距離計算部２４および姿勢決定部２５は、例えば、単数または複数のＦＰＧＡ（Field Programmable Gate array）等の半導体装置を用いて構成されている。画像取得部２１、特徴点抽出部２２、特定部２３、距離計算部２４および姿勢決定部２５の各部位における処理は、ＣＰＵ（Central Processing Unit）がコンピュータプログラムを実行することで行われてもよい。
【0022】
特徴点登録部２６は、周囲の環境の構造物上の点が特徴点として登録されたデータベースを有している。特徴点登録部２６に保存されているデータベースでは、移動体が動作すると想定される範囲内における周辺の構造物の座標、構造物上の特徴点の情報および特徴点の座標が互いに関連づけて保存されている。特徴点として登録の対象となる構造物には、形状や大きさが規格として定められている構造物や、既知である構造物が選定されている。特徴点の情報は、あらかじめ取得された周囲の画像から、その画像中の構造物の輝度変化が計算され特徴点付近の輝度勾配や輝度変化の向きが特徴点の情報として、特徴点の座標と関連づけられて保存されている。特徴点登録部２６は、不揮発性の半導体メモリやハードディスクドライブまたはそれらの組み合わせによって構成されている。また、特徴点登録部２６は、移動体が存在すると推定される範囲に存在する構造物の特徴点のデータを無線通信回線等を介して管理サーバ等から取得し、取得したデータをデータベースとして保存する構成であってもよい。
【0023】
本実施形態の計測システムの動作について説明する。図３は、本実施形態の計測システムにおいて撮影部１０の位置および姿勢を特定する際の動作フローを示している。
【0024】
始めに撮影部１０のカメラによって周囲の撮影が行われる。撮影は、例えば、計測システムが備えられている移動体の制御部からの要求に応じて行われる。また、撮影は、あらかじめ設定された時間ごとに行われてもよい。
【0025】
撮影を行うと、撮影部１０は、撮影した画像のデータを距離・姿勢計測部２０の画像取得部２１に送る。撮影部１０から画像データを取得すると、画像取得部２１は、受け取った画像データを特徴点抽出部２２に出力する（ステップＳ１０１）。
【0026】
画像データを受け取ると、特徴点抽出部２２は、画像から特徴点を抽出する（ステップＳ１０２）。特徴点抽出部２２は、例えば、画像中の輝度変化の大きい点を特徴点として抽出する。輝度変化の大きい点を特徴点として抽出すると、特徴点抽出部２２は、特徴点の輝度勾配と輝度変化の向きを計算する。特徴点の情報を抽出すると、特徴点抽出部２２は、抽出した特徴点の情報を特定部２３に出力する。
【0027】
特徴点の情報を受け取ると、特定部２３は、特徴点登録部２６のデータベースを参照し、画像から抽出した特徴点が対応する構造物を特定する（ステップＳ１０３）。特定部２３は、データベース上の構造物の拡大と縮小を行った際に、構造物上の特徴点の相互の位置関係が画像から抽出された特徴点の相互の位置関係と一致する構造物を画像中の特徴点が対応する構造物として特定する。特徴点に対応する構造物を特定すると、特定部２３は、データベースから特定した構造物の各特徴点の座標データをデータベースから検索する。
【0028】
各特徴点の座標データを検索すると、特定部は、各構造物の特徴点の座標データ（座標の情報ともいう）を距離計算部２４に出力する。
【0029】
各構造部の特徴点の座標データを受け取ると、距離計算部２４は、各構造物から撮影部１０までの距離を計算する（ステップＳ１０４）。距離計算部２４における各構造物から撮影部１０までの距離の計算方法について、図４および図５を参照して説明する。図４は、撮影部１０のカメラと撮影対象となる周囲の環境中の各構造物の特徴点を模式的に示した図である。また、図５は、撮影部１０のカメラのイメージセンサの大きさ、撮影対象物である構造部の大きさおよびカメラと撮影対象物の距離の関係を模式的に示した図である。
【0030】
構造物のサイズをＬ１、画素ピッチ×構造物の画素数すなわちイメージセンサ上の大きさをＬ２、撮影部１０のレンズから構造物までの距離をｘ、焦点距離すなわちレンズからイメージセンサまでの既知の焦点距離をＦとすると、焦点距離Ｆは、下記の式（１）のように表される。
Ｆ＝ｘＬ２／Ｌ１ ・・・（式１）
ここでサイズＬ１は、既知であり、例えば、実際の構造物の最も上の位置と最も下の位置との距離が用いられる。また、イメージセンサ上の大きさＬ２は、イメージセンサ上の構造物の像の最も上の特徴点と最も下の特徴点との間の長さから算出される。
Ｆ＝ｘＬ２／Ｌ１ ・・・（式１）
ここでサイズＬ１は、既知であり、例えば、実際の構造物の最も上の位置と最も下の位置との距離が用いられる。また、イメージセンサ上の大きさＬ２は、イメージセンサ上の構造物の像の最も上の特徴点と最も下の特徴点との間の長さから算出される。
【0031】
また、上記の式１を変形すると、下記の式２のようになる。
ｘ＝ＦＬ1／Ｌ2 ・・・（式２）
上記の式２を用いて、レンズから対象物までの距離ｘを計算することができる。撮影部１０のレンズから構造物までの距離ｘは、各構造物についてそれぞれ計算される。
ｘ＝ＦＬ1／Ｌ2 ・・・（式２）
上記の式２を用いて、レンズから対象物までの距離ｘを計算することができる。撮影部１０のレンズから構造物までの距離ｘは、各構造物についてそれぞれ計算される。
【0032】
各構造物から撮影部１０までの距離を計算すると、距離計算部２４は、各構造物からの距離と各構造物の座標の情報を姿勢決定部２５に出力する。
【0033】
各構造物からの距離と座標の情報を受け取ると、姿勢決定部２５は、複数の構造物との間の距離から撮影部１０の位置と姿勢を特定する(ステップＳ１０５）。姿勢決定部２５は、始めに、撮影部１０の位置を計算する。姿勢決定部２５における撮影部１０のカメラの位置およじ姿勢の計算方法について、図６を参照して説明する。図６は撮影部１０のカメラと構造物の位置を模式的に示した図である。以下では、図６に示すように３個の構造物Ａ、構造物Ｂおよび構造物Ｃとの距離の情報を基に、撮影部１０の姿勢を特定する方法を例に説明する。
【0034】
構造物Ａの座標が（ｘｉ，ｙｉ，ｚｉ）、構造物Ｃの座標が（ｘｊ，ｙｊ，ｚｊ）、構造物Ｃの座標が（ｘｋ，ｙｋ，ｚｋ）であるとする。各構造物の座標には、構造物上の代表的な点があらかじめ設定されている。構造物の代表的な点の座標には、例えば、構造物上の特徴点から計算される重心位置の座標が用いられる。また、撮影部１０の座標（カメラのレンズの焦点位置の座標）が（ｘ，ｙ，ｚ）であるとする。このとき、撮影部１０のカメラと構造物Ａの距離がｒ１、撮影部１０のカメラと構造物Ｂの距離がｒ２、撮影部１０のカメラと構造物Ｃの距離がｒ３とすると、撮影部１０のカメラと構造物の距離は、以下のように表すことができる。
（ｘ－ｘｉ）２＋（ｙ－ｙｉ）２＋（ｚ－ｚi）２＝ｒ１ ２ ・・・（式３）
（ｘ－ｘｊ）２＋（ｙ－ｙｊ）２＋（ｚ－ｚｊ）２＝ｒ２ ２ ・・・（式４）
（ｘ－ｘｋ）２＋（ｙ－ｙｋ）２＋（ｚ－ｚｋ）２＝ｒ３ ２ ・・・（式５）
式４および式５から式３を引いて整理すると以下のようになる。
（ｘｉ－ｘｊ）ｘ＋（ｙｉ－ｙｊ）ｙ＋（ｚi－ｚｊ）ｚ＝Ｄ ・・・（式６）
（ｘｉ－ｘｋ）ｘ＋（ｙｉ－ｙｋ）ｙ＋（ｚi－ｚｋ）ｚ＝Ｅ ・・・（式７）
上記の式６のＤ、式７のＥは、それぞれ
Ｄ＝（（ｒ１ ２－ｘｉ ２－ｙｉ ２－ｚi ２）－（ｒ２ ２－ｘｊ ２－ｙｊ ２－ｚｊ ２））／２
Ｅ＝（（ｒ１ ２－ｘｉ ２－ｙｉ ２－ｚi ２）－（ｒ３ ２－ｘｋ ２－ｙｋ ２－ｚｋ ２））／２
としている。
（ｘ－ｘｉ）２＋（ｙ－ｙｉ）２＋（ｚ－ｚi）２＝ｒ１ ２ ・・・（式３）
（ｘ－ｘｊ）２＋（ｙ－ｙｊ）２＋（ｚ－ｚｊ）２＝ｒ２ ２ ・・・（式４）
（ｘ－ｘｋ）２＋（ｙ－ｙｋ）２＋（ｚ－ｚｋ）２＝ｒ３ ２ ・・・（式５）
式４および式５から式３を引いて整理すると以下のようになる。
（ｘｉ－ｘｊ）ｘ＋（ｙｉ－ｙｊ）ｙ＋（ｚi－ｚｊ）ｚ＝Ｄ ・・・（式６）
（ｘｉ－ｘｋ）ｘ＋（ｙｉ－ｙｋ）ｙ＋（ｚi－ｚｋ）ｚ＝Ｅ ・・・（式７）
上記の式６のＤ、式７のＥは、それぞれ
Ｄ＝（（ｒ１ ２－ｘｉ ２－ｙｉ ２－ｚi ２）－（ｒ２ ２－ｘｊ ２－ｙｊ ２－ｚｊ ２））／２
Ｅ＝（（ｒ１ ２－ｘｉ ２－ｙｉ ２－ｚi ２）－（ｒ３ ２－ｘｋ ２－ｙｋ ２－ｚｋ ２））／２
としている。
【0035】
ここで、ｘｉｊ＝ｘｉ－ｘｊ、ｙｉｊ＝ｙｉ－ｙｊ、ｚｉｊ＝ｚi－ｚｊ、ｘｉｋ＝ｘｉ－ｘｋ、ｙｉｋ＝ｙｉ－ｙｋ、ｚｉｋ＝ｚi－ｚｋとすると、式６と式７は、それぞれ以下のように表すことができる。
ｘｉｊｘ＋ｙｉｊｙ＋ｚｉｊｚ＝Ｄ ・・・（式８）
ｘｉｋｘ＋ｙｉｋｙ＋ｚｉｋｚ＝Ｅ ・・・（式９）
式８と式９を変形すると、
ｘ＝（（Ｄｙｉｋ－Ｅｙｉｊ）＋（ｙｉｊｚｉｋ－ｙｉｋｚｉｊ）ｚ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）
ｙ＝（（Ｅｘｉｊ－Ｄｘｉｋ）＋（ｘｉｋｚｉｊ－ｘｉｊｚｉｋ）ｚ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）
となる。
ｘｉｊｘ＋ｙｉｊｙ＋ｚｉｊｚ＝Ｄ ・・・（式８）
ｘｉｋｘ＋ｙｉｋｙ＋ｚｉｋｚ＝Ｅ ・・・（式９）
式８と式９を変形すると、
ｘ＝（（Ｄｙｉｋ－Ｅｙｉｊ）＋（ｙｉｊｚｉｋ－ｙｉｋｚｉｊ）ｚ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）
ｙ＝（（Ｅｘｉｊ－Ｄｘｉｋ）＋（ｘｉｋｚｉｊ－ｘｉｊｚｉｋ）ｚ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）
となる。
【0036】
上記の式をさらに整理すると下記のように表される。
ｘ＝Ａ０＋Ａ１ｚ ・・・（式１０）
ｙ＝Ｂ０＋Ｂ１ｚ ・・・（式１１）
式１０および式１１では、Ａ０＝（Ｄｙｉｋ－Ｅｙｉｊ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）、Ａ１＝（ｙｉｊｚｉｋ－ｙｉｋｚｉｊ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）、Ｂ０＝（Ｅｘｉｊ－Ｄｘｉｋ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）、Ｂ１＝（ｘｉｋｚｉｊ－ｘｉｊｚｉｋ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）としている。
ｘ＝Ａ０＋Ａ１ｚ ・・・（式１０）
ｙ＝Ｂ０＋Ｂ１ｚ ・・・（式１１）
式１０および式１１では、Ａ０＝（Ｄｙｉｋ－Ｅｙｉｊ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）、Ａ１＝（ｙｉｊｚｉｋ－ｙｉｋｚｉｊ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）、Ｂ０＝（Ｅｘｉｊ－Ｄｘｉｋ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）、Ｂ１＝（ｘｉｋｚｉｊ－ｘｉｊｚｉｋ）／（ｘｉｊｙｉｋ－ｘｉｋｙｉｊ）としている。
【0037】
ここで、式１０および式１１を式３に代入すると、ｚに関する二次方程式ができる。
Ｆｚ２＋２Ｇｚ＋Ｈ＝０ ・・・（式１２）
式１２では、Ｆ＝Ａ１ ２＋Ｂ１ ２＋１、Ｇ＝Ａ１（Ａ０－ｘｉ）＋Ｂ１（Ｂ０－ｙｉ）－ｚｉ、Ｈ＝（Ａ０－ｘｉ）２＋（Ｂ０－ｙｉ）２－ｚｉ ２－ｒ１ ２としている。
Ｆｚ２＋２Ｇｚ＋Ｈ＝０ ・・・（式１２）
式１２では、Ｆ＝Ａ１ ２＋Ｂ１ ２＋１、Ｇ＝Ａ１（Ａ０－ｘｉ）＋Ｂ１（Ｂ０－ｙｉ）－ｚｉ、Ｈ＝（Ａ０－ｘｉ）２＋（Ｂ０－ｙｉ）２－ｚｉ ２－ｒ１ ２としている。
【0038】
式１２を解くと以下のようになる。
ｚ＝（－Ｇ±（Ｇ２－ＦＨ）０．５）／Ｆ
上記のｚを式１０と式１１にそれぞれ代入すると、撮影部１０の位置の座標のうちのｘ、ｙが計算される。
ｚ＝（－Ｇ±（Ｇ２－ＦＨ）０．５）／Ｆ
上記のｚを式１０と式１１にそれぞれ代入すると、撮影部１０の位置の座標のうちのｘ、ｙが計算される。
【0039】
撮影部１０の座標を計算すると、姿勢決定部２５は、撮影部１０を基準とした際の２個の構造物間の角度を計算する。姿勢決定部２５は、複数の構造物のうち２つの構造物の組み合わせごとに、２個の構造物間の角度を計算する。
【0040】
姿勢決定部２５は、角度θｉｊ、角度θｉｋ、角度θｋｊを下記のように計算する。
ｃｏｓθｉｊ＝（ｒ１ ２＋ｒ２ ２－ｄｉｊ ２）／２ｒ１ｒ２
ｃｏｓθｉｋ＝（ｒ１ ２＋ｒ３ ２－ｄｉｋ ２）／２ｒ１ｒ３
ｃｏｓθｋｊ＝（ｒ２ ２＋ｒ３ ２－ｄｋｊ ２）／２ｒ２ｒ３
実際の構造物間の距離を示すｄｉｊ、ｄｉｋおよびｄｋｊは、それぞれデータベースに登録されている値または実際の構造物上の計測された２点の座標から計算された値が使われる。
ｃｏｓθｉｊ＝（ｒ１ ２＋ｒ２ ２－ｄｉｊ ２）／２ｒ１ｒ２
ｃｏｓθｉｋ＝（ｒ１ ２＋ｒ３ ２－ｄｉｋ ２）／２ｒ１ｒ３
ｃｏｓθｋｊ＝（ｒ２ ２＋ｒ３ ２－ｄｋｊ ２）／２ｒ２ｒ３
実際の構造物間の距離を示すｄｉｊ、ｄｉｋおよびｄｋｊは、それぞれデータベースに登録されている値または実際の構造物上の計測された２点の座標から計算された値が使われる。
【0041】
構造物間の角度を計算すると、姿勢決定部２５は、撮影部１０を基準とした場合の各構造物間の角度に対応する撮影部１０の姿勢の情報をデータベースから検索する。撮影部１０を基準とした場合の各構造物間の角度と撮影部１０の姿勢の関係を示す情報は、データベースに保存されている。
【0042】
撮影部１０の姿勢を決定すると、姿勢決定部２５は、計測システムが備えられている移動体の制御部に撮影部１０の座標と姿勢の情報を出力する。計測システムが備えられている移動体の制御部は、撮影部１０の座標と姿勢の情報を基に、移動体の位置および姿勢を算出して、移動体の動作を制御する。移動体の制御部は、撮影部１０の移動体への取り付け角度の情報をあらかじめ保持している。距離・姿勢計測部２０は、あらかじめ設定された補正値を基に、撮影部１０を基準とした場合の各構造物間の角度と撮影部１０の座標を、移動体の位置および姿勢の情報に換算して移動体の制御部に出力してもよい、
本実施形態の計測システムは、撮影部１０において撮影された画像から、距離・姿勢計測部２０において特徴点を抽出し、抽出した特徴点から対応する構造物を特定している。また、本実施形態の計測システムは、距離・姿勢計測部２０において撮影部１０と構造物との距離を計算し、複数の構造物との距離の情報を基に、撮影部１０の位置と姿勢を計算している。そのため、本実施形態の計測システムは、１台のカメラのみで撮影部１０の位置と姿勢、すなわち、撮影部１０が備えられている移動体の位置と姿勢を決定することができる。その結果、本実施形態の計測システムは、装置の大型化を抑制しつつ移動体の位置と姿勢を計測することができる。
本実施形態の計測システムは、撮影部１０において撮影された画像から、距離・姿勢計測部２０において特徴点を抽出し、抽出した特徴点から対応する構造物を特定している。また、本実施形態の計測システムは、距離・姿勢計測部２０において撮影部１０と構造物との距離を計算し、複数の構造物との距離の情報を基に、撮影部１０の位置と姿勢を計算している。そのため、本実施形態の計測システムは、１台のカメラのみで撮影部１０の位置と姿勢、すなわち、撮影部１０が備えられている移動体の位置と姿勢を決定することができる。その結果、本実施形態の計測システムは、装置の大型化を抑制しつつ移動体の位置と姿勢を計測することができる。
【0043】
第１の実施形態の計測装置および第２の実施形態の距離・姿勢計測部２０における各処理は、コンピュータプログラムをコンピュータで実行することによって行われてもよい。図７は、距離・姿勢計測部２０における各処理を行うコンピュータプログラムを実行するコンピュータ３０の構成の例を示したものである。コンピュータ３０は、ＣＰＵ３１と、メモリ３２と、記憶装置３３と、Ｉ／Ｆ（Interface）部３４を備えている。また、第１の実施形態の計測装置も同様の構成を備えている。
【0044】
ＣＰＵ３１は、記憶装置３３から各処理を行うコンピュータプログラムを読み出して実行する。メモリ３２は、ＤＲＡＭ（Dynamic Random Access Memory）等によって構成され、ＣＰＵ３１が実行するコンピュータプログラムや処理中のデータが一時保存される。記憶装置３３は、ＣＰＵ３１が実行するコンピュータプログラム、周囲環境における構造物部の特徴点のデータベースを保存している。記憶装置３３は、例えば、不揮発性の半導体記憶装置によって構成されている。記憶装置３３には、ハードディスクドライブ等の他の記憶装置が用いられてもよい。Ｉ／Ｆ部３４は、撮影部１０や計測システムが備えられている移動体の制御部等との間でデータの入出力を行うインタフェースである。コンピュータ３０は、通信ネットワークを介して他の情報処理装置と通信を行う通信モジュールをさらに備えていてもよい。
【0045】
また、各処理に行うコンピュータプログラムは、記録媒体に格納して頒布することもできる。記録媒体としては、例えば、データ記録用磁気テープや、ハードディスクなどの磁気ディスクを用いることができる。また、記録媒体としては、ＣＤ-ＲＯＭ（Compact Disc Read Only Memory）等の光ディスクを用いることもできる。不揮発性の半導体メモリを記録媒体として用いてもよい。
【符号の説明】
【0046】
１ 画像取得部
２ 特徴点抽出部
３ 特定部
４ 距離計算部
５ 姿勢決定部
１０ 撮影部
２０ 距離・姿勢計測部
２１ 画像取得部
２２ 特徴点抽出部
２３ 特定部
２４ 距離計算部
２５ 姿勢決定部
２６ 特徴点登録部
３０ コンピュータ
３１ ＣＰＵ
３２ メモリ
３３ 記憶装置
３４ Ｉ／Ｆ部
２ 特徴点抽出部
３ 特定部
４ 距離計算部
５ 姿勢決定部
１０ 撮影部
２０ 距離・姿勢計測部
２１ 画像取得部
２２ 特徴点抽出部
２３ 特定部
２４ 距離計算部
２５ 姿勢決定部
２６ 特徴点登録部
３０ コンピュータ
３１ ＣＰＵ
３２ メモリ
３３ 記憶装置
３４ Ｉ／Ｆ部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】