(57)【要約】【課題】物体の地球固定系座標が不明な場合でも、移動体の状態変数を推定する。【解決手段】参照フレーム比較部２０３は、カメラ１０３により時刻Ａで撮影された、寸法が既知の静止している参照物体が表されるカメラフレームＡと、参照物体が表される、予め生成された参照フレームとを比較し、時刻Ａでのカメラ１０３の位置及び姿勢を算出し、時刻Ｂでカメラ１０３により撮影された、参照物体が表されるカメラフレームＢと参照フレームとを比較して、時刻Ｂでのカメラ１０３の位置及び姿勢を算出する。フレーム差分生成部２０４は、時刻Ａ及び時刻Ｂでのカメラ１０３の位置及び姿勢から、時刻Ａと時刻Ｂとの間のカメラ１０３の変位を算出する。状態推定部２０１は、カメラ１０３の変位に基づいて、時刻Ｂにおける車体１０の状態変数を推定する。【選択図】図１
2020144651.tif 000002
【特許請求の範囲】
【請求項１】
移動体に固定されたカメラにより第１の時刻で撮影された、寸法が既知の静止物体である参照物体が表される第１のカメラ画像と、前記参照物体が表される、予め生成された画像である参照画像とを比較し、前記第１のカメラ画像に表される前記参照物体と前記参照画像に表される前記参照物体との差異に基づいて、前記第１の時刻での前記カメラの位置及び姿勢を算出する第１のカメラ画像比較部と、
前記第１の時刻の後の第２の時刻で前記カメラにより撮影された、前記参照物体が表される第２のカメラ画像と、前記参照画像とを比較して、前記第２のカメラ画像に表される前記参照物体と前記参照画像に表される前記参照物体との差異に基づいて、前記第２の時刻での前記カメラの位置及び姿勢を算出する第２のカメラ画像比較部と、
前記第１の時刻での前記カメラの位置及び姿勢と前記第２の時刻での前記カメラの位置及び姿勢とから、前記第１の時刻と前記第２の時刻との間の前記カメラの変位を算出する変位算出部と、
前記カメラの変位に基づいて、前記第２の時刻における前記移動体の状態変数を推定する状態推定部とを有する計算機装置。
【請求項２】
前記第１のカメラ画像比較部は、
前記参照画像と対応付けられている三次元座標系である参照ワールド座標系における前記第１の時刻での前記カメラの位置及び姿勢を算出し、
前記第２のカメラ画像比較部は、
前記参照ワールド座標系における前記第２の時刻での前記カメラの位置及び姿勢を算出し、
前記変位算出部は、
前記第１の時刻と前記第２の時刻との間の前記参照ワールド座標系における前記カメラの変位を算出する請求項１に記載の計算機装置。
【請求項３】
前記第１のカメラ画像比較部は、
前記カメラの位置を基準とするカメラ座標系における前記第１のカメラ画像での前記参照物体の位置を第１の参照物体位置として算出し、
前記第２のカメラ画像比較部は、
前記カメラ座標系における前記第２のカメラ画像での前記参照物体の位置を第２の参照物体位置として算出し、
前記状態推定部は、
前記第１の時刻と前記第２の時刻との間の前記カメラの変位に加えて、前記第１の参照物体位置と前記第２の参照物体位置との差異に基づいて、前記第２の時刻における前記移動体の状態変数を推定する請求項１に記載の計算機装置。
【請求項４】
前記状態推定部は、
ＩＭＵ（Ｉｎｅｒｔｉａｌ  Ｍｅａｓｕｒｅｍｅｎｔ  Ｕｎｉｔ）による計測結果及びＧＮＳＳ（Ｇｌｏｂａｌ  Ｎａｖｉｇａｔｉｏｎ  Ｓａｔｅｌｌｉｔｅ  Ｓｙｓｔｅｍ）による計測結果の少なくともいずれかに加えて、前記第１の時刻と前記第２の時刻との間の前記カメラの変位に基づいて、前記第２の時刻における前記移動体の状態変数を推定する請求項１に記載の計算機装置。
【請求項５】
前記計算機装置は、
前記状態推定部により推定された状態変数を用いて地図を生成する地図生成部を有する請求項１に記載の計算機装置。
【請求項６】
コンピュータが、移動体に固定されたカメラにより第１の時刻で撮影された、寸法が既知の静止物体である参照物体が表される第１のカメラ画像と、前記参照物体が表される、予め生成された画像である参照画像とを比較し、前記第１のカメラ画像に表される前記参照物体と前記参照画像に表される前記参照物体との差異に基づいて、前記第１の時刻での前記カメラの位置及び姿勢を算出する第１のカメラ画像比較処理と、
前記コンピュータが、前記第１の時刻の後の第２の時刻で前記カメラにより撮影された、前記静止物体が表される第２のカメラ画像と、前記参照画像とを比較して、前記第２のカメラ画像に表される前記参照物体と前記参照画像に表される前記参照物体との差異に基づいて、前記第２の時刻での前記カメラの位置及び姿勢を算出する第２のカメラ画像比較処理と、
前記コンピュータが、前記第１の時刻での前記カメラの位置及び姿勢と前記第２の時刻での前記カメラの位置及び姿勢とから、前記第１の時刻と前記第２の時刻との間の前記カメラの変位を算出する変位算出処理と、
前記コンピュータが、前記カメラの変位に基づいて、前記第２の時刻における前記移動体の状態変数を推定する状態推定処理とを有する状態変数推定方法。
【請求項７】
移動体に固定されたカメラにより第１の時刻で撮影された、寸法が既知の静止物体である参照物体が表される第１のカメラ画像と、前記参照物体が表される、予め生成された画像である参照画像とを比較し、前記第１のカメラ画像に表される前記参照物体と前記参照画像に表される前記参照物体との差異に基づいて、前記第１の時刻での前記カメラの位置及び姿勢を算出する第１のカメラ画像比較処理と、
前記第１の時刻の後の第２の時刻で前記カメラにより撮影された、前記静止物体が表される第２のカメラ画像と、前記参照画像とを比較して、前記第２のカメラ画像に表される前記参照物体と前記参照画像に表される前記参照物体との差異に基づいて、前記第２の時刻での前記カメラの位置及び姿勢を算出する第２のカメラ画像比較処理と、
前記第１の時刻での前記カメラの位置及び姿勢と前記第２の時刻での前記カメラの位置及び姿勢とから、前記第１の時刻と前記第２の時刻との間の前記カメラの変位を算出する変位算出処理と、
前記カメラの変位に基づいて、前記第２の時刻における前記移動体の状態変数を推定する状態推定処理とをコンピュータに実行させる状態変数推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の状態変数の推定に関するものである。
【背景技術】
【０００２】
車両等の安全な自動走行を実現するために、高精度の三次元地図が求められている。三次元地図の生成手段として、モービルマッピングシステムが挙げられる。モービルマッピングシステムは、車体等の移動体に取り付けられた各種のセンサのセンサ情報を用いて、移動体の状態変数（位置、姿勢及びこれらの算出に付随するパラメータ）を基に三次元地図の生成を行うシステムである。移動体に取り付けられるセンサの例として、加速度及び角速度を測定するＩＭＵ（Ｉｎｅｒｔｉａｌ  Ｍｅａｓｕｒｅｍｅｎｔ  Ｕｎｉｔ）、ＧＮＳＳ（Ｇｌｏｂａｌ  Ｎａｖｉｇａｔｉｏｎ  Ｓａｔｅｌｌｉｔｅ  Ｓｙｓｔｅｍ）からの信号を受信するためのＧＮＳＳ受信機、あるいは照射対象までの距離を測るレーザレンジファインダが挙げられる。
【０００３】
モービルマッピングシステムにおいては、移動履歴中の移動体の状態変数の精度が生成される三次元地図の精度に直結する。ＧＮＳＳが使用できる状況ならば高頻度の衛星観測から状態変数の更新を行うことが可能である。しかし、トンネル内、高層ビル群の谷間等、衛星観測が十分に行うことができない状況では、状態変数の更新はＩＭＵから測定できる加速度及び角速度の積分に頼ることになる。この積分が観測に付随するノイズに対して強く影響されるため、長時間続けた場合には、実際の値と比較した際に誤差の蓄積が避けられない。従って高精度の状態変数推定のためには、ＧＮＳＳを用いない観測の利用が求められる。
【０００４】
ＧＮＳＳに頼らない観測の利用として、オドメータの利用による車体速度の観測が挙げられる。ただしオドメータの使用のためには、通常システムを搭載している車体の改造が必要となる。またレーザ測距計を用いたランドマーク更新も既に提案されているが、こちらもランドマークが地球固定座標系でどこに位置しているか、予め知る必要がある。
【０００５】
映像からの情報を用いて状態変数を推定する地図生成装置として、従来、特許文献１のものが知られている。特許文献１では映像フレームより特徴点を抽出し、特徴点の集合を単純な図形の集合に置き換えた上で、カメラの位置及び姿勢を表すカメラベクトル（ＣＶ）をフレーム間における単純図形の変化から求めている。この時得られる座標は相対的なものであるため、ＧＮＳＳデータや既知の地図データを組み合わせることによって地球固定座標系を求めている。また当手法によって得られた三次元地図を用いて、三次元地図の周辺の映像を取得した際の位置及び姿勢の推定手法も合わせて示している。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特許第５２２７０６５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
以上の通り、ＧＮＳＳを用いない観測更新には、予め基準となる物体の地球固定座標系上の位置を知る必要があるか、あるいは追加の観測装置を取り付ける必要が生じる。
特許文献１に記載された手法においても、地図生成時は姿勢推定を行っておらず、また映像を用いた位置及び姿勢の推定においては、予め特許文献１に拠る方法に従って生成した三次元地図の使用を前提としている。従って地図生成のために初めて走行する場合、あるいは周辺環境が大きく変化した場合においては十分な精度が確保できない可能性が生じる。
【０００８】
本発明は、かかる問題を解決することを主な目的とする。
より具体的には、本発明は、環境中の物体の地球固定系座標が分かっていない場合においても、移動体の状態変数を推定できるようにすることを主な目的とする。
【課題を解決するための手段】
【０００９】
本発明に係る計算機装置は、
移動体に固定されたカメラにより第１の時刻で撮影された、寸法が既知の静止物体である参照物体が表される第１のカメラ画像と、前記参照物体が表される、予め生成された画像である参照画像とを比較し、前記第１のカメラ画像に表される前記参照物体と前記参照画像に表される前記参照物体との差異に基づいて、前記第１の時刻での前記カメラの位置及び姿勢を算出する第１のカメラ画像比較部と、
前記第１の時刻の後の第２の時刻で前記カメラにより撮影された、前記参照物体が表される第２のカメラ画像と、前記参照画像とを比較して、前記第２のカメラ画像に表される前記参照物体と前記参照画像に表される前記参照物体との差異に基づいて、前記第２の時刻での前記カメラの位置及び姿勢を算出する第２のカメラ画像比較部と、
前記第１の時刻での前記カメラの位置及び姿勢と前記第２の時刻での前記カメラの位置及び姿勢とから、前記第１の時刻と前記第２の時刻との間の前記カメラの変位を算出する変位算出部と、
前記カメラの変位に基づいて、前記第２の時刻における前記移動体の状態変数を推定する状態推定部とを有する。
【発明の効果】
【００１０】
本発明によれば、環境中の物体の地球固定系座標が分かっていない場合においても、環境中にある物体を移動体のカメラによって撮影した画像を利用することで移動体の状態変数を推定することができる。
【図面の簡単な説明】
【００１１】
【図１】実施の形態１に係る地図生成装置の構成例を示す図。
【図２】実施の形態１に係る計算機装置の動作例を示すフローチャート。
【図３】実施の形態１に係るカメラ位置とカメラフレームの例を示す図。
【図４】実施の形態１に係るワールド座標系から画像座標系への変換式を示す図。
【図５】実施の形態１に係るカメラ座標系における参照物体位置の導出式を示す図。
【図６】実施の形態１に係る、二つの異なる時刻の画像フレーム観測から導かれる、それぞれの時刻における位置及び姿勢の拘束を示す図。
【図７】実施の形態１に係る計算機装置のハードウェア構成例を示す。
【発明を実施するための形態】
【００１２】
以下、本発明の実施の形態について、図を用いて説明する。以下の実施の形態の説明及び図面において、同一の符号を付したものは、同一の部分又は相当する部分を示す。
【００１３】
実施の形態１．
＊＊＊構成の説明＊＊＊
図１は、本実施の形態に係る地図生成装置１の構成例を示す。
地図生成装置１は、車体１０と、計算機装置２０からなる。
車体１０には、ＩＭＵ１０１、ＧＮＳＳ受信機１０２、カメラ１０３、及びレーザレンジスキャナ１０４が取り付けられている。なお、車体１０は、移動体の例である。
計算機装置２０は、コンピュータである。計算機装置２０は、機能構成として、状態推定部２０１、地図生成部２０２、参照フレーム比較部２０３、フレーム差分生成部２０４、参照フレーム記憶部２０５、及びカメラフレーム記憶部２０６を有する。
計算機装置２０により行われる動作は、状態変数推定処理及び状態変数推定プログラムに相当する。
【００１４】
１０１〜１０４の各センサは、車体１０が走行することによって車体１０及び周辺環境に関する情報を取得する。取得された情報を計算機装置２０に与えることによって三次元地図が生成されていく。
なお、計算機装置２０は車体に取り付けられている必要はなく、無線あるいは有線を介して車体１０に取り付けられたセンサのセンサ情報を取得できればよい。また、１０１〜１０４の各センサ情報はリアルタイムに計算機装置２０に入力する必要はなく、一旦任意の記憶装置に保存し、記憶装置から時系列順に計算機装置２０に対して入力することも可能である。この場合、記憶装置からセンサ情報を入力することは各センサから直接入力することと同等であるため、記憶装置の図示は省略している。
【００１５】
ＩＭＵ１０１は、車体１０の加速度及び角速度を出力する。また、ＧＮＳＳ受信機１０２は、ＧＮＳＳアンテナの位置を取得する。
【００１６】
カメラ１０３及びレーザレンジスキャナ１０４は、カメラ画像又はスキャン画像を車体１０の位置及び姿勢の情報と合わせることで三次元地図の生成に用いられる。加えて、本実施の形態においてカメラ１０３は、衛星測位が利用できないときの車体１０の位置及び姿勢の推定の観測の役割を持つ。
【００１７】
計算機装置２０は、図７に示すように、ハードウェア構成として、プロセッサ９０１、主記憶装置９０２、補助記憶装置９０３及び通信装置９０４を備える。
補助記憶装置９０３には、状態推定部２０１、地図生成部２０２、参照フレーム比較部２０３、フレーム差分生成部２０４の機能を実現するプログラムが記憶されている。つまり、状態推定部２０１、地図生成部２０２、参照フレーム比較部２０３、フレーム差分生成部２０４はプログラムにより実現される。
これらプログラムは、補助記憶装置９０３から主記憶装置９０２にロードされる。そして、プロセッサ９０１がこれらプログラムを実行して、状態推定部２０１、地図生成部２０２、参照フレーム比較部２０３、フレーム差分生成部２０４の動作を行う。
図７では、プロセッサ９０１が状態推定部２０１、地図生成部２０２、参照フレーム比較部２０３、フレーム差分生成部２０４の機能を実現するプログラムを実行している状態を模式的に表している。
【００１８】
状態推定部２０１は、ＩＭＵ１０１、ＧＮＳＳ受信機１０２、及びフレーム差分生成部２０４からの情報を用いて車体１０の状態変数を推定する。状態推定部２０１により行われる処理は状態推定処理に相当する。
ＩＭＵ１０１及びＧＮＳＳ受信機１０２からの情報は直接状態推定部２０１に入力されるが、カメラ１０３からのカメラ画像であるカメラフレームは一旦参照フレーム比較部２０３に入力され、参照フレーム記憶部２０５に蓄積された画像フレーム（参照フレーム）との照合が行われる。照合の結果、参照フレームと同じ被写体がカメラフレームにある場合に、当該カメラフレームはカメラフレーム記憶部２０６に保存される。そして、当該カメラフレームと、カメラフレーム記憶部２０６に蓄積された過去の同一被写体を写したカメラフレームがフレーム差分生成部２０４に入力される。
フレーム差分生成部２０４において二枚のカメラフレーム間のカメラ移動に関する拘束が導出され、導出された拘束が状態推定部２０１に入力される。
参照フレーム比較部２０３、フレーム差分生成部２０４における処理については後述する。
【００１９】
状態推定部２０１は、ＩＭＵ１０１による状態変数の積分、ＧＮＳＳ観測による位置観測、及びフレーム差分生成部２０４からの状態変数拘束を用いて、入力時刻における推定位置及び姿勢を求める。例えば積分を時間更新、ＧＮＳＳによる位置観測及び状態変数拘束を観測更新とするカルマンフィルタを利用することが考えられる。
【００２０】
状態推定部２０１によって得られた推定位置及び姿勢は、当該時刻におけるカメラ１０３の情報、及びレーザレンジスキャナ１０４の情報と紐付けられ、地図生成部２０２において、公知の手法を用いて三次元地図を生成する。つまり、地図生成部２０２は、状態推定部２０１により推定された状態変数を用いて地図を生成する
【００２１】
なお、本実施の形態においては、通常のモービルマッピング装置に対して追加のセンサを用いることなく状態変数推定の改善を図ることができる。一般にモービルマッピングシステムにおいては、三次元地図生成のためにカメラが取り付けられているためである。
【００２２】
＊＊＊動作の説明＊＊＊
次に、本実施の形態に係る計算機装置２０の動作例を説明する。
より具体的には、カメラフレーム及び参照フレームの照合から状態変数の推定を行う動作を説明する。
【００２３】
以下では、寸法が既知の静止物体を参照物体という。そして、参照物体を、規定の位置及び姿勢から撮影した画像を以下では参照フレームという。参照フレームは、カメラ１０３による撮影よりも前に予め生成されている。参照フレームは参照画像の例である。
参照物体は、遍在し、寸法が既知であり、また確実に静止していると言える物体である必要がある。例えば法令によって寸法が規定されている交通標識や車種別に形状が分かっている、停車している車両などが挙げられる。なお、ここに挙げたもの以外のものも参照物体となりうる。
【００２４】
本実施の形態では、参照フレーム比較部２０３が、車体１０に固定されたカメラ１０３により任意の時刻（以下、時刻Ａという）で撮影された、参照物体が表されるカメラフレーム（以下、カメラフレームＡという）を取得する。なお、時刻Ａは第１の時刻の例であり、また、カメラフレームＡは第１のカメラ画像の例である。なお、カメラフレームＡに表される参照物体と参照フレームに表される参照物体は同一物である必要はない。例えば、交通標識を参照物体とする場合は、参照フレームに表される交通標識と同じ種類の交通標識がカメラフレームＡに表されていればよい。
参照フレーム比較部２０３は、カメラフレームＡと参照フレームとを比較する。そして、参照フレーム比較部２０３は、カメラフレームＡに表される参照物体と参照フレームに表される参照物体との差異に基づいて、時刻Ａでのカメラ１０３の位置及び姿勢を算出する。より具体的には、参照フレーム比較部２０３は、参照ワールド座標系における時刻Ａでのカメラ１０３の位置及び姿勢を算出する。参照ワールド座標系とは、参照フレームと対応付けられている三次元座標系である。つまり、参照ワールド座標系とは、参照フレームの生成時に設定された三次元座標系である。参照ワールド座標系では、参照物体が所在する空間の任意の点が原点となる。
【００２５】
次に、参照フレーム比較部２０３は、時刻Ａの後の任意の時刻（以下、時刻Ｂという）でカメラ１０３により撮影された、参照物体が表されるカメラフレーム（以下、カメラフレームＢという）を取得する。なお、時刻Ｂは第２の時刻の例であり、また、カメラフレームＢは第２のカメラ画像の例である。また、時刻Ａと時刻Ｂは必ずしも連続した時刻である必要はない。
カメラフレームＢに表される参照物体とカメラフレームＡに表される参照物体は同一物である必要がある。
参照フレーム比較部２０３は、カメラフレームＢと参照フレームとを比較する。そして、参照フレーム比較部２０３は、カメラフレームＢに表される参照物体と参照フレームに表される参照物体との差異に基づいて、時刻Ｂでのカメラ１０３の位置及び姿勢を算出する。より具体的には、参照フレーム比較部２０３は、参照ワールド座標系における時刻Ｂでのカメラ１０３の位置及び姿勢を算出する。
【００２６】
なお、参照フレーム比較部２０３は、第１のカメラ画像比較部及び第２のカメラ画像比較部の例である。
また、参照フレーム比較部２０３により行われる処理は、第１のカメラ画像比較処理及び第２のカメラ画像比較処理に相当する。
【００２７】
ここで、参照物体の地球固定座標系に対する位置及び姿勢が変化していなければ２つのカメラフレームのカメラ座標系から、時刻Ａと時刻Ｂとの間の時間におけるカメラ１０３の位置の変化及びカメラ１０３の姿勢の変化が決定する。
地球固定座標系とは、緯度及び経度で構成される座標系である。カメラ座標系とは、カメラ１０３の位置を原点とする座標系である。
本実施の形態では、フレーム差分生成部２０４が、時刻Ａでのカメラ１０３の位置及び姿勢と時刻Ｂでのカメラ１０３の位置及び姿勢とから、時刻Ａと時刻Ｂとの間のカメラ１０３の変位を算出する。
そして、フレーム差分生成部２０４は、時刻Ａと時刻Ｂとの間のカメラ１０３の変位と、時刻Ａと時刻Ｂとの間のＩＭＵによる積分とを比較することによって、ＩＭＵによる積分を補正することが可能となる。
なお、フレーム差分生成部２０４は、変位算出部の例である。また、フレーム差分生成部２０４により行われる処理は、変位算出処理の例である。
【００２８】
以上により、対象の物体の地球固定系座標が直接分かっていない場合においても、環境中にある物体を利用することで車体１０の位置の推定が可能となる。参照物体、参照フレームと参照フレーム取得時のカメラ位置、及び各時刻におけるフレーム及びそれぞれを取得したカメラ位置の概要を図３に示す。
【００２９】
図３では、参照フレーム３２０は、参照物体３００を符号３１０のカメラ位置で撮影して得られた画像である。カメラフレームＡ３２１は、参照物体３００を符号３１１のカメラ位置で撮影して得られた画像である。カメラフレームＢ３２２は、参照物体３００を符号３１２のカメラ位置で撮影して得られた画像である。
図３では、文字「Ｑ」が参照フレーム３２０、カメラフレームＡ３２１及びカメラフレームＢ３２２の全てに存在するので、参照フレーム比較部２０３は、３つのフレームが同じ参照物体３００を撮影した画像であると認識することができる。
参照ワールド座標系は、前述のように、参照フレームの撮影時に設定された三次元直交座標系である。図３では、参照物体３００が所在する環境における任意の点Ｏを原点とする三次元直交座標系である。
カメラ座標系は、符号３１０のカメラ位置、符号３１１のカメラ位置及び符号３１２のカメラ位置の各々を原点とする三次元直交座標位置である。
【００３０】
次に、本実施の形態に係る計算機装置２０の動作の詳細を図２のフローチャートを参照して説明する。
【００３１】
ステップＳ５０１では、前処理として、参照フレームが生成され、生成された参照フレームが参照フレーム記憶部２０５に保存される。
参照フレームは実際に参照物体を撮影することで生成することができる。あるいはカメラ１０３の特性を考慮したシミュレータ上で、三次元モデルを二次元面に投影することによっても生成できる。参照フレームは一つである必要はなく、複数種類用意することが可能である。
また、参照フレームは、参照フレーム上の各座標が参照ワールド座標系の三次元座標と対応付けさせた上で、参照フレーム記憶部２０５に保存される。つまり、参照フレームが生成されたときのカメラ位置及び姿勢、参照物体の位置が参照ワールド座標系の座標と対応付けられて、参照フレームが参照フレーム記憶部２０５に保存される。
【００３２】
時刻Ａにおいてカメラ１０３からカメラフレームＡが参照フレーム比較部２０３に入力されると、ステップＳ５１１で、参照フレーム比較部２０３が、入力されたカメラフレームＡと、参照フレーム記憶部２０５の参照フレームとに同一の参照物体が存在しているか否かを判定する。
【００３３】
カメラフレームＡと参照フレームとに同一の参照物体が存在している場合は、ステップＳ５１２で、参照フレーム比較部２０３が、カメラフレームＡの参照物体と参照フレームの参照物体との差異に基づき、参照ワールド座標系における時刻Ａでのカメラ１０３の位置及び姿勢を算出する。また、ステップＳ５１２では、参照フレーム比較部２０３は、カメラ座標系におけるカメラフレームＡでの参照物体の位置を算出する。なお、ステップＳ５１２で参照フレーム比較部２０３が算出する、カメラ座標系におけるカメラフレームＡでの参照物体の位置は第１の参照物体位置に相当する。
また、状態推定部２０１が時刻Ａにおける車体１０の状態変数を推定する。
【００３４】
ステップＳ５１３では、カメラフレーム記憶部２０６がステップＳ５１２で算出された各値を保存する。
つまり、カメラフレーム記憶部２０６は、推定された状態変数と、参照ワールド座標系における時刻Ａでのカメラ１０３の位置及び姿勢と、カメラ座標系におけるカメラフレームＡでの参照物体の位置を保存する。
【００３５】
また、時刻Ａより後の時刻Ｂに対しても、ステップＳ５１１〜Ｓ５１３と同様の処理ステップＳ５２１〜Ｓ５２３が行われる。
【００３６】
すなわち、時刻Ｂにおいてカメラ１０３からカメラフレームＢが参照フレーム比較部２０３に入力されると、ステップＳ５２１で、参照フレーム比較部２０３が、入力されたカメラフレームＢと、参照フレーム記憶部２０５の参照フレームとに同一の参照物体が存在しているか否かを判定する。
【００３７】
カメラフレームＢと参照フレームとに同一の参照物体が存在している場合は、ステップＳ５２２で、参照フレーム比較部２０３が、カメラフレームＢの参照物体と参照フレームの参照物体との差異に基づき、参照ワールド座標系における時刻Ｂでのカメラ１０３の位置及び姿勢を算出する。また、ステップＳ５２２では、参照フレーム比較部２０３は、カメラ座標系におけるカメラフレームＢでの参照物体の位置を算出する。なお、ステップＳ５２２で参照フレーム比較部２０３が算出する、カメラ座標系におけるカメラフレームＢでの参照物体の位置は第１の参照物体位置に相当する。
また、状態推定部２０１が時刻Ｂにおける車体１０の状態変数を推定する。
【００３８】
ステップＳ５２３では、カメラフレーム記憶部２０６がステップＳ５２２で算出された各値を保存する。
つまり、カメラフレーム記憶部２０６は、推定された状態変数と、参照ワールド座標系における時刻Ｂでのカメラ１０３の位置及び姿勢と、カメラ座標系におけるカメラフレームＢでの参照物体の位置を保存する。
【００３９】
ステップＳ５３１では、フレーム差分生成部２０４が、カメラフレーム記憶部２０６から時刻Ａ及び時刻Ｂの各値を受け取り、二つの時刻の間のカメラ１０３の位置及び姿勢に関する拘束条件を計算する。
拘束条件の計算の詳細は後述する。
【００４０】
ステップＳ５３２では、状態推定部２０１が、ＩＭＵ観測値、ＧＮＳＳ観測値、及びフレーム差分生成部２０４が計算する拘束条件から時刻Ｂの状態変数の推定を行う。
時刻Ｂの状態変数の推定の詳細は後述する。
【００４１】
最後に、ステップＳ５３３では、ステップＳ５３２で得られた時刻Ｂの状態変数をカメラフレーム記憶部２０６が保存する。
ステップＳ５３３で保存された時刻Ｂの状態変数は、更に後の時刻についてのステップＳ５３１の入力として用いられる。
【００４２】
また、状態推定部２０１は、ステップＳ５３２で得られた状態変数を地図生成部２０２に出力する。
【００４３】
三次元座標を画像座標に射影する一般式を図４に示す。
空間内における任意の三次元直交座標系をワールド座標系とし、このワールド座標系で記述された点４０４は線形変換によって二次元の画像座標４０１に射影できる。カメラ内部パラメータ４０２はワールド座標系におけるカメラの位置及び姿勢に拠らず一定である。一方、カメラ外部パラメータ４０３はワールド座標系におけるカメラの位置及び姿勢によって決定する。従って、カメラ外部パラメータ４０３が決定すれば、符号４０５及び４０６で示される、カメラ１０３と当該座標との相対的な位置及び姿勢が決定する。
【００４４】
次に、走行結果から得られる各フレームにおいて、参照フレーム記憶部２０５に保存されたいずれかの参照フレームと同一形状及び寸法の物体を参照フレーム比較部２０３が探索する。探索についてはＨＯＧ（Ｈｉｓｔｏｇｒａｍｓ  ｏｆ  Ｏｒｉｅｎｔｅｄ  Ｇｒａｄｉｅｎｔｓ）特徴量とサポートベクターマシンの組み合わせなど、特徴量抽出とパターン認識を基にした既知の手法を適用する。
【００４５】
次に、カメラフレームＡに対する処理内容を示す。
カメラフレームＡにおいて参照フレームと同一の参照物体が同定できたときは、カメラフレームＡ上に存在する参照物体の座標と、参照フレーム上の座標のうち幾つかが対応付けられていることとなる。また、前述したとおり、参照フレーム上の座標と参照ワールド座標系の三次元座標はあらかじめ対応付けられている。これによって、カメラフレームＡ上に存在する参照物体の特徴点に対応する、参照ワールド座標系の三次元座標が決定する。三次元座標と、それに対応する画像フレーム上の座標が既知ならば、既知の手法であるＤｉｒｅｃｔ  Ｌｉｎｅａｒ  Ｔｒａｎｓｆｏｒｍａｔｉｏｎ（ＤＬＴ）法を用いることによって、外部パラメータが決定する。これを用いて、カメラ座標系における参照物体位置は、図５に示すとおり、時刻Ａのカメラ外部パラメータ、及び参照フレームを撮影した時の物体の三次元座標から求めることができる。一方、時刻Ａにおける各状態変数推定は、状態推定部２０１が計算したものを使用する。カメラフレームＡ、時刻Ａにおける各状態変数推定、カメラ座標系における参照物体位置はカメラフレーム記憶部２０６が記録する。
【００４６】
時刻Ｂに対しても、時刻Ａの場合と同様にカメラ座標系における参照物体位置を参照フレーム比較部２０３が計算する。時刻Ｂにおける各状態変数推定は暫定の値として、ＩＭＵの積分とＧＮＳＳ観測を用いて状態推定部２０１が計算する値を用いる。カメラフレームＢ、時刻Ｂにおける各状態変数推定、カメラ座標系における参照物体位置Ｂはカメラフレーム記憶部２０６が記録する。
【００４７】
時刻Ａから時刻Ｂの間に参照物体の位置及び姿勢が変化していないならば、図６の通り、二つの時刻間の参照物体の地球固定座標系が等しい、とする拘束が生じる。ここでカメラ座標系から車体座標系への変換４０８、車体座標系から局所水平座標系への変換４０９、局所水平座標系から地球固定座標系への変換４１０はいずれも既知であるか、あるいは推定したい状態の関数で記述することができる。図６の拘束を用いて、状態推定部２０１がカメラフレーム記憶部２０６からの入力を用いて、時刻Ｂの状態変数を推定し直すことが可能となる。例えば時刻Ａと時刻Ｂの間における、地球固定座標系の参照物体位置の誤差を観測誤差とみなすことができる。この観測誤差を用いて、カルマンフィルタ等などの推定処理を状態推定部２０１が行うことによって、時刻Ｂにおける状態変数推定が可能となる。推定され直された時刻Ｂにおける各状態変数推定は改めてカメラフレームＢ、カメラ座標系における参照物体位置Ｂと共にカメラフレーム記憶部２０６が保存する。
【００４８】
＊＊＊実施の形態の効果の説明＊＊＊
以上、本実施の形態によれば、参照画像を予め生成することにより、参照物体の環境中における位置及び姿勢の推定が可能となる。
つまり、本実施の形態によれば、同一物体を異なる位置及び姿勢から撮影した二つの画像フレームを利用して自己位置の推定を行い、結果的にＩＭＵの積分誤差を抑制し、ＧＮＳＳが利用できない環境下における状態変数推定の精度向上が期待できるため、生成される三次元地図の精度が向上する。
【００４９】
以上の実施の形態に示した構成は、本発明の内容の一例を示すものであり、別の公知の技術と組み合わせることも可能であるし、本発明の要旨を逸脱しない範囲で、構成の一部
を省略、変更することも可能である。
【００５０】
＊＊＊ハードウェア構成の説明＊＊＊
最後に、計算機装置２０のハードウェア構成の補足説明を行う。
図７に示すプロセッサ９０１は、ＩＣ（Ｉｎｔｅｇｒａｔｅｄ  Ｃｉｒｃｕｉｔ）である。
プロセッサ９０１は、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）、ＤＳＰ（Ｄｉｇｉｔａｌ  Ｓｉｇｎａｌ  Ｐｒｏｃｅｓｓｏｒ）等である。
図７に示す主記憶装置９０２は、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）である。
図７に示す補助記憶装置９０３は、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、フラッシュメモリ、ＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）等である。
図７に示す通信装置９０４は、データの通信処理を実行する電子回路である。
通信装置９０４は、例えば、通信チップ又はＮＩＣ（Ｎｅｔｗｏｒｋ  Ｉｎｔｅｒｆａｃｅ  Ｃａｒｄ）である。
【００５１】
また、補助記憶装置９０３には、ＯＳ（Ｏｐｅｒａｔｉｎｇ  Ｓｙｓｔｅｍ）も記憶されている。
そして、ＯＳの少なくとも一部がプロセッサ９０１により実行される。
プロセッサ９０１はＯＳの少なくとも一部を実行しながら、状態推定部２０１、地図生成部２０２、参照フレーム比較部２０３、フレーム差分生成部２０４の機能を実現するプログラムを実行する。
プロセッサ９０１がＯＳを実行することで、タスク管理、メモリ管理、ファイル管理、通信制御等が行われる。
また、状態推定部２０１、地図生成部２０２、参照フレーム比較部２０３、フレーム差分生成部２０４の処理の結果を示す情報、データ、信号値及び変数値の少なくともいずれかが、主記憶装置９０２、補助記憶装置９０３、プロセッサ９０１内のレジスタ及びキャッシュメモリの少なくともいずれかに記憶される。
また、状態推定部２０１、地図生成部２０２、参照フレーム比較部２０３、フレーム差分生成部２０４の機能を実現するプログラムは、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ブルーレイ（登録商標）ディスク、ＤＶＤ等の可搬記録媒体に格納されていてもよい。そして、状態推定部２０１、地図生成部２０２、参照フレーム比較部２０３、フレーム差分生成部２０４の機能を実現するプログラムが格納された可搬記録媒体を商業的に流通させてもよい。
【００５２】
また、状態推定部２０１、地図生成部２０２、参照フレーム比較部２０３、フレーム差分生成部２０４の「部」を、「回路」又は「工程」又は「手順」又は「処理」に読み替えてもよい。
また、計算機装置２０は、処理回路により実現されてもよい。処理回路は、例えば、ロジックＩＣ（Ｉｎｔｅｇｒａｔｅｄ  Ｃｉｒｃｕｉｔ）、ＧＡ（Ｇａｔｅ  Ａｒｒａｙ）、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ  Ｓｐｅｃｉｆｉｃ  Ｉｎｔｅｇｒａｔｅｄ  Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ  Ｇａｔｅ  Ａｒｒａｙ）である。
この場合は、状態推定部２０１、地図生成部２０２、参照フレーム比較部２０３、フレーム差分生成部２０４は、それぞれ処理回路の一部として実現される。
【符号の説明】
【００５３】
１  地図生成装置、１０  車体、２０  計算機装置、１０１  ＩＭＵ、１０２  ＧＮＳＳ受信機、１０３  カメラ、１０４  レーザレンジスキャナ、２０１  状態推定部、２０２  地図生成部、２０３  参照フレーム比較部、２０４  フレーム差分生成部、２０５  参照フレーム記憶部、２０６  カメラフレーム記憶部、３００  参照物体、３１０  参照フレーム生成時のカメラ、３１１  時刻Ａにおけるカメラ、３１２  時刻Ｂにおけるカメラ、３２０  参照フレーム、３２１  カメラフレームＡ、３２２  カメラフレームＢ、４０１  同次形式でフレーム上の座標位置を表すベクトルあるいは行列、４０２  奥行き情報を含むフレーム上の座標位置を表すベクトルあるいは行列、４０３  カメラ内部パラメータ、４０４  カメラ外部パラメータ、４０５  同次形式でワールド座標系の座標位置を表すベクトル、４０６  ワールド座標系からカメラ座標系への変換を表す行列、４０７  ワールド座標系でカメラ位置を表すベクトル、４０８  ワールド座標系で物体位置を表すベクトル、４０９  カメラ座標系で物体位置を表すベクトル、４１０  カメラ座標系から車体座標系への変換を表す行列、４１１  車体座標系から局所水平座標系への変換を表す行列、４１２  局所水平座標系から地球固定座標系への変換を表す行列、４１３  時刻Ａの状態を表す添字、４１４  時刻Ｂの状態を表す添字。
【図１】
2020144651.tif 000003
【図２】
2020144651.tif 000004
【図３】
2020144651.tif 000005
【図４】
2020144651.tif 000006
【図５】
2020144651.tif 000007
【図６】
2020144651.tif 000008
【図７】
2020144651.tif 000009