(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-09-09
(45)【発行日】2024-09-18
(54)【発明の名称】位置推定装置、位置推定方法、及びプログラム
(51)【国際特許分類】
G01B 11/00 20060101AFI20240910BHJP
G01C 21/28 20060101ALI20240910BHJP
G05D 1/43 20240101ALI20240910BHJP
G06T 7/70 20170101ALI20240910BHJP
【ＦＩ】
G01B11/00 H
G01C21/28
G05D1/43
G06T7/70 Z
【請求項の数】
12
(21)【出願番号】P 2020170942
(22)【出願日】2020-10-09
(65)【公開番号】P2022062807
(43)【公開日】2022-04-21
【審査請求日】2023-09-04
(73)【特許権者】
【識別番号】000232092
【氏名又は名称】ＮＥＣソリューションイノベータ株式会社
(74)【代理人】
【識別番号】110002044
【氏名又は名称】弁理士法人ブライタス
(72)【発明者】
【氏名】山下 喜宏
【審査官】國田 正久
(56)【参考文献】
【文献】国際公開第２０２０／１３７３１３（ＷＯ，Ａ１）
【文献】特開２０２０－０７６７１４（ＪＰ，Ａ）
【文献】国際公開第２０１４／０８３９１０（ＷＯ，Ａ１）
【文献】特開２０１９－０１８７４３（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０１Ｂ １１／００
Ｇ０１Ｃ ２１／２８
Ｇ０５Ｄ １／４３
Ｇ０６Ｔ ７／７０
(57)【特許請求の範囲】
【請求項1】
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得部と、前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得部と、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測部と、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索部と、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定部と、
を備え、
前記カメラ姿勢予測部が、前記フレームの取得後に取得された前記センサデータにカルマンフィルタを適用して事前予測値を算出し、前記フレームでのカメラ姿勢と、算出した事前予測値と、前記センサデータとを用いて、前記予測値を算出し、
前記特徴点探索部が、前記探索ウィンドウにおける、前記次フレームのエピポーラ線に並行な方向の長さが、当該方向に垂直な方向の長さよりも長くなるように、前記探索ウィンドウを設定する、
ことを特徴とする位置推定装置。
【請求項2】
請求項１に記載の位置推定装置であって、前記センサが、慣性計測装置、加速度センサ、及び角速度センサのうちの少なくとも１つを含む、
ことを特徴とする位置推定装置。
【請求項3】
請求項１に記載の位置推定装置であって、前記カメラ姿勢が、前記カメラの回転行列及び並進ベクトルを含む、
ことを特徴とする位置推定装置。
【請求項4】
請求項１に記載の位置推定装置であって、前記カメラ姿勢予測部が、前記フレームの取得後に取得された前記センサデータをカルマンフィルタに適用して、事前予測値を算出し、算出した前記事前予測値と前記フレームでのカメラ姿勢とを用いて、前記予測値を算出し、
前記位置推定部が、前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出した後に、前記予測値、算出した前記次フレームでのカメラ姿勢、及びカルマンゲインを用いて、前記次フレームにおける事後予測値を算出し、更に、前記事後予測値を用いて、前記次フレームにおける前記移動体の位置を推定する、
ことを特徴とする位置推定装置。
【請求項5】
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得ステップと、前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得ステップと、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測ステップと、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索ステップと、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定ステップと、
を有し、
前記カメラ姿勢予測ステップにおいて、前記フレームの取得後に取得された前記センサデータにカルマンフィルタを適用して事前予測値を算出し、前記フレームでのカメラ姿勢と、算出した事前予測値と、前記センサデータとを用いて、前記予測値を算出し、
前記特徴点探索ステップにおいて、前記探索ウィンドウにおける、前記次フレームのエピポーラ線に並行な方向の長さが、当該方向に垂直な方向の長さよりも長くなるように、前記探索ウィンドウを設定する、
ことを特徴とする位置推定方法。
【請求項6】
請求項５に記載の位置推定方法であって、前記センサが、慣性計測装置、加速度センサ、及び角速度センサのうちの少なくとも１つを含む、
ことを特徴とする位置推定方法。
【請求項7】
請求項５に記載の位置推定方法であって、前記カメラ姿勢が、前記カメラの回転行列及び並進ベクトルを含む、
ことを特徴とする位置推定方法。
【請求項8】
請求項５に記載の位置推定方法であって、前記カメラ姿勢予測ステップにおいて、前記フレームの取得後に取得された前記センサデータをカルマンフィルタに適用して、事前予測値を算出し、算出した前記事前予測値と前記フレームでのカメラ姿勢とを用いて、前記予測値を算出し、
前記位置推定ステップにおいて、前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出した後に、前記予測値、算出した前記次フレームでのカメラ姿勢、及びカルマンゲインを用いて、前記次フレームにおける事後予測値を算出し、更に、前記事後予測値を用いて、前記次フレームにおける前記移動体の位置を推定する、
ことを特徴とする位置推定方法。
【請求項9】
コンピュータに、移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得ステップと、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得ステップと、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測ステップと、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索ステップと、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定ステップと、
を実行させ、
前記カメラ姿勢予測ステップにおいて、前記フレームの取得後に取得された前記センサデータにカルマンフィルタを適用して事前予測値を算出し、前記フレームでのカメラ姿勢と、算出した事前予測値と、前記センサデータとを用いて、前記予測値を算出し、
前記特徴点探索ステップにおいて、前記探索ウィンドウにおける、前記次フレームのエピポーラ線に並行な方向の長さが、当該方向に垂直な方向の長さよりも長くなるように、前記探索ウィンドウを設定する、
プログラム。
【請求項10】
請求項９に記載のプログラムであって、前記センサが、慣性計測装置、加速度センサ、及び角速度センサのうちの少なくとも１つを含む、
ことを特徴とするプログラム。
【請求項11】
請求項９に記載のプログラムであって、前記カメラ姿勢が、前記カメラの回転行列及び並進ベクトルを含む、
ことを特徴とするプログラム。
【請求項12】
請求項９に記載のプログラムであって、前記カメラ姿勢予測ステップにおいて、前記フレームの取得後に取得された前記センサデータをカルマンフィルタに適用して、事前予測値を算出し、算出した前記事前予測値と前記フレームでのカメラ姿勢とを用いて、前記予測値を算出し、
前記位置推定ステップにおいて、前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出した後に、前記予測値、算出した前記次フレームでのカメラ姿勢、及びカルマンゲインを用いて、前記次フレームにおける事後予測値を算出し、更に、前記事後予測値を用いて、前記次フレームにおける前記移動体の位置を推定する、
ことを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体の位置の推定するための、位置推定装置、及び位置推定方法に関し、更には、これらを実現するためのプログラムに関する。
【背景技術】
【0002】
近年、センシング技術の向上により、ＳＬＡＭ（Simultaneous Localization and Mapping）が注目されている。ＳＬＡＭは、センサを搭載した移動体によって、自己位置の推定と環境地図の作成とを同時に実行する技術である（例えば、特許文献１及び非特許文献１参照）。ＳＬＡＭによれば、自律走行するロボットは、ランダムに移動する必要がなく、環境地図に沿って移動できるため、その移動効率は向上することになる。
【0003】
具体的には、ＳＬＡＭでは、撮像装置を搭載した移動体が用いられる。撮像装置は、設定間隔をおいてフレーム単位で画像データ（以下「フレーム」と表記する）を出力する。そして、従来からのＳＬＡＭにおいては、最初に、画像データを用いて、初期の環境地図が作成される。
【0004】
続いて、初期の環境地図が作成されると、最新のフレームにおいて、過去のフレームから抽出された特徴点に対応する特徴点が探索され、対応する特徴点同士で構成された特徴点の組が抽出される。
【0005】
次に、特徴点の組を用いて、最新のフレームにおけるカメラ行列が算出され、更に、カメラの位置（移動体の位置）が推定される。そして、特徴点の組毎に、カメラ行列と、特徴点のフレームにおける２次元座標とを用いて、特徴点の３次元座標が算出され、算出された３次元座標を用いて環境地図が更新される。
【先行技術文献】
【特許文献】
【0006】
【文献】特開２０２０－７６７１４号公報
【非特許文献】
【0007】
【文献】Raul Mur-Artal, J. M. M. Montiel, Juan D. Tardos, “ORB-SLAM: A Versatile and Accurate Monocular SLAM System”,[online], IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 5, OCTOBER 2015,[2020/7/29 検索], インターネット＜URL： https://ieeexplore.ieee.org/document/7219438?arnumber=7219438＞
【発明の概要】
【発明が解決しようとする課題】
【0008】
ところで、従来からのＳＬＡＭにおいては、特徴点の探索処理にかかる時間を短縮するため、最新フレームにおいて、対応する特徴点が存在する領域を予測し、予測した領域において特徴点の探索が行われている。また、領域の予測は、最新のフレームにおいて、移動体の移動ベクトルを推定することによって行われている。
【0009】
しかしながら、従来からのＳＬＡＭでは、移動体が急に旋回する等して急激に動きが変化してしまうと、推定した移動ベクトルと実際の移動ベクトルとの差が大きくなり、結果、最新フレームにおける特徴点の探索対象となる領域の予測に大きな誤差が生じてしまう。このような場合、対応する特徴点を抽出できず、移動体の位置を推定できない「退化」という問題が発生してしまう。
【0010】
本発明の目的の一例は、上記問題を解消し、移動体の位置の推定において、移動体の動きによる推定精度の変動を抑制し得る、位置推定装置、位置推定方法、及びプログラムを提供することにある。
【課題を解決するための手段】
【0011】
上記目的を達成するため、本発明の一側面における位置推定装置は、
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得部と、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得部と、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測部と、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索部と、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定部と、
を備えている、ことを特徴とする。
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得部と、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得部と、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測部と、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索部と、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定部と、
を備えている、ことを特徴とする。
【0012】
また、上記目的を達成するため、本発明の一側面における位置推定方法は、
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得ステップと、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得ステップと、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測ステップと、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索ステップと、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定ステップと、
を有する、ことを特徴とする。
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得ステップと、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得ステップと、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測ステップと、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索ステップと、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定ステップと、
を有する、ことを特徴とする。
【0013】
更に、上記目的を達成するため、本発明の一側面におけるプログラムは、
コンピュータに、
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得ステップと、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得ステップと、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測ステップと、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索ステップと、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定ステップと、
を実行させる、ことを特徴とする。
コンピュータに、
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得ステップと、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得ステップと、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測ステップと、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索ステップと、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定ステップと、
を実行させる、ことを特徴とする。
【発明の効果】
【0014】
以上のように本発明によれば、移動体の位置の推定において、移動体の動きによる推定精度の変動を抑制することができる。
【図面の簡単な説明】
【0015】
【発明を実施するための形態】
【0016】
【0017】
【0018】
図１に示す位置推定装置１０は、移動体の位置を推定するための装置である。図１に示すように、位置推定装置１０は、画像データ取得部１１と、センサデータ取得部１２と、カメラ姿勢予測部１３と、特徴点探索部１４と、位置推定部１５とを備えている。
【0019】
画像データ取得部１１は、移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する。画像データ取得部１１が取得しているフレームのうち、最後に取得したフレームを「現フレーム」とする。センサデータ取得部１２は、移動体の姿勢を検出するセンサからセンサデータを取得する。
【0020】
カメラ姿勢予測部１３は、画像データ取得部１１が取得した現フレームでのカメラ姿勢と、現フレームの取得後にセンサデータ取得部１２が取得したセンサデータとから、現フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する。
【0021】
特徴点探索部１４は、画像データ取得部１１が次フレームを取得すると、取得後に、予測値を用いて、次フレーム上に探索ウィンドウを設定し、探索ウィンドウ内で、現フレーム内の特徴点に対応する特徴点を探索する。次フレームは、上述の現フレームの取得後に取得されており、最新のフレームである。
【0022】
位置推定部１５は、カメラ姿勢予測部１３が算出した予測値とは別に、特徴点探索部１４が探索した特徴点に基づいて、次フレームでのカメラ姿勢を算出する。更に、位置推定部１５は、算出した次フレームでのカメラ姿勢に基づいて、次フレームにおける移動体の位置を推定する。
【0023】
このように、位置推定装置１０は、現フレームでのカメラ姿勢と、センサデータから算出した移動体の姿勢とを用いて、次フレームにおける探索ウィンドウを設定しており、探索ウィンドウの設定における誤差を抑制している。このため、位置推定装置１０によれば、移動体の位置の推定において、移動体の動きによる推定精度の変動を抑制することができる。
【0024】
【0025】
図２に示すように、実施の形態では、位置推定装置１０は、移動体１００に搭載されている。移動体１００は、例えば、自走式のロボット、車両などである。移動体１００は、位置推定装置１０に加えて、カメラ２０と、センサ３０と、制御装置４０と、駆動装置５０と、操舵装置６０とを備えている。
【0026】
カメラ２０は、移動体の進行方向側に取り付けられており、進行方向における画像の画像データを、フレーム単位で設定間隔をおいて出力する。
【0027】
センサ３０は、移動体１００の姿勢を検出するセンサである。センサ３０としては、加速度センサ、角速度センサ、これらを備えたＩＭＵ（Inertial Measurement Unit：慣性計測装置）が挙げられる。なお、実施の形態では、センサ３０は、加速度センサ及び角速度センサを備えたＩＭＵであるとする。
【0028】
制御装置４０は、位置推定装置１０によって推定された位置に基づいて、移動体１００の進行方向及び移動速度を決定し、決定した進行方向及び移動速度に応じて、駆動装置５０及び操舵装置６０を制御する。
【0029】
駆動装置５０は、電動モータなどの原動機、原動機の動力を車輪に伝える伝達機構等を備え、制御装置４０の指示に応じて、移動体１００を進行させる。操舵装置６０は、操舵輪を動かすための機構を備えており、制御装置４０の指示に応じて、操舵輪を操作する。
【0030】
また、実施の形態では、移動体１００は、上述の態様に限定されるものではない。移動体１００は、例えば、操舵装置６０を備えず、代わりに、左右の駆動輪のトルクを制御することによって操舵を行う態様であっても良い。
【0031】
画像データ取得部１１は、実施の形態では、カメラ２０が出力する画像データをフレーム単位で取得する。センサデータ取得部１２は、実施の形態では、ＩＭＵであるセンサ３０から、移動体１００における加速度データ（ａｘ，ａｙ，ａｚ）及び角速度データ（ωｘ，ωｙ，ωｚ）を取得する。
【0032】
また、実施の形態では、位置推定装置１０は、最初に、移動体１００の位置の推定に加えて、環境地図の作成も実行する。まず、図３に示すように、特徴点探索部１４と、位置推定部１５とによって、初期の環境地図が作成される。図３は、環境地図の作成処理の一例を示す図である。
【0033】
具体的には、図３に示すように、最初に、特徴点探索部１４は、現フレームから、過去のフレームで抽出されている特徴点に対応する特徴点（以下「対応特徴点」と表記する。）を抽出する。特徴点が抽出されている過去のフレームがない場合は、特徴点探索部１４は、現フレームから特徴点を抽出し、次フレームの取得後に、次フレームを新たな現フレームとして、先に抽出した特徴点に対応する対応特徴点を抽出する。
【0034】
続いて、位置推定部１５は、現フレームと過去のフレームとにおける特徴点の組合せを用いて、現フレームにおけるカメラ姿勢を算出する。ここで、実施の形態において、カメラ姿勢は、回転行列Ｒｔと並進ベクトルｔｔとで構成される。また、カメラ姿勢の算出は、下記の参考文献１に示す既知の手法によって行うことができる。
（参考文献１）
David Nister, “An Efficient Solution to the Five-Point Relative Pose Problem”, Published 2004, DOI:10.1109/TPAMI.2004.17, Corpus ID:886598, <URL:https://pdfs.semanticscholar.org/c288/7c83751d2c36c63139e68d46516ba3038909.pdf>
（参考文献１）
David Nister, “An Efficient Solution to the Five-Point Relative Pose Problem”, Published 2004, DOI:10.1109/TPAMI.2004.17, Corpus ID:886598, <URL:https://pdfs.semanticscholar.org/c288/7c83751d2c36c63139e68d46516ba3038909.pdf>
【0035】
更に、位置推定部１５は、現フレーム及び過去のフレームそれぞれにおける、カメラ姿勢と、対応特徴点の２次元座標とを用いて、三角測量を行うことによって、対応特徴点の３次元座標（点群データ）を算出する。移動体１００の移動に合わせて、特徴点探索部１４及び位置推定部１５による上述の処理が何回か繰り返して実行されると、移動体の周囲の物体が３次元の点群データで表現された、初期の環境地図が作成される。
【0036】
そして、位置推定装置１０は、初期の環境地図が作成できると、カメラ姿勢予測部１３によってセンセデータを用いた予測値の算出を行って、移動体の位置推定を行う。また、位置推定装置１０は、推定した位置を用いて、環境地図を拡張することもできる。以下に移動体の位置推定について詳細に説明する。
【0037】
カメラ姿勢予測部１３は、実施の形態では、現フレームでのカメラ姿勢と、現フレームの取得後に取得されたセンサデータとを用いた、ベイズ推定を行い、これによって次フレームにおけるカメラ姿勢の予測値を算出する。現フレームでのカメラ姿勢は、既に位置推定部１５による処理が行われている場合は、位置推定部１５によって算出されている。
【0038】
具体的には、カメラ姿勢予測部１３は、現フレームの取得後に取得されたセンサデータをカルマンフィルタに適用して、後述する事前予測値を算出し、算出した事前予測値を用いて、次フレームでのカメラ姿勢を算出する。カメラ姿勢は、上述したように、回転行列Ｒｔと並進ベクトルｔｔとで構成される。以下、「カメラ姿勢［Ｒ］［ｔ］」と表記する。
【0039】
カルマンフィルタは、反復推定器（反復推定型フィルタ）であり、システム（系）の現在の観測量と、１ステップ前の状態推定値とから、現在の状態推定値と１ステップ先の状態予測値とを与える。ここで、図４を用いてカルマンフィルタについて説明する。図４は、カルマンフィルタで行われる処理の一例を示す図である。
【0040】
図４に示すように、カルマンフィルタは、まず、事前予測値ハットｘｔ、共分散行列Ｐｔの初期値（ｔ＝０）を設定し、続いて、数１に基づいて事前予測値及び共分散行列を更新する。更に、カルマンフィルタは、計測データ（センサデータ）ｙｔの入力を受け付け、数２に基づいて計測結果を反映した補正を実行する。更に、これらの処理は繰り返し行われ、最新の事前予測値が出力される。
【0041】
下記数１において、ｆｔ（ｘｔ－１）は、時刻ｔにおける事前予測値ハットｘｔを求めるための関数である。数２において、Ｋｔは、最適カルマンゲインである。数１において、ｗｔ－１は、観測誤差ある。数１において、ハットＦＴ
ｔ－１は、ｆｔ（ｘｔ－１）のヤコビ行列である。Ｔは、行列の転置である。数１において、Ｑｔ－１は、ｗｔ－１がガウス分布とした場合の共分散行列である。
【0042】
【数1】
【0043】
【数2】
【0044】
実施の形態では、事前予測値ハットｘｔとしては、例えば、移動体１００の位置ｐ、速度ｖ、クォータ二オンｑが用いられる。クォータニオンｑは、移動体１００における回転を表すパラメータである。計測データｙｔとしては、ＩＭＵから出力された加速度データ（ａｘ，ａｙ，ａｚ）及び角速度データ（ωｘ，ωｙ，ωｚ）が用いられる。
【0045】
カメラ姿勢予測部１３は、事前予測値ハットｘｔとして、移動体１００の位置ｐ、速度ｖ、及びクォータ二オンｑを出力し、出力した事前予測値と、現のフレームでのカメラ姿勢［Ｒ］［ｔ］と、計測データｙｔとを用いて、次フレームでのカメラ姿勢［Ｒ］［ｔ］の予測値を算出する。
【0046】
また、カメラ姿勢予測部１３は、以前の状態のヤコビ行列をＦｔ－１、以前の状態の共分散行列をＰｔとし、そして、計測データｙｔ＝ｈ（ｘｔ）＋ｒとした場合のｈ（ｘｔ）のヤコビ行列をＨｔ、ノイズｒの共分散行列をＲｔとして、最適カルマンゲインＫｔを算出することができる。この場合は、後述するように、位置推定部１５は、この最適カルマンゲインＫｔを用いて事後予測値ｘｔを算出する。
【0047】
特徴点探索部１４は、実施の形態では、図５に示すように、カメラ姿勢［Ｒ］［ｔ］の予測値を用いて、次フレーム上に探索ウィンドウを設定する。図５は、実施の形態において設定される探索ウィンドウの一例を示す図である。
【0048】
具体的には、特徴点探索部１４は、カメラ姿勢［Ｒ］［ｔ］の予測値を下記の数３に適用して、現フレームの特徴点の３次元座標（世界座標系（Ｗ））（ＸＷ，ＹＷ，ＺＷ）を、カメラ座標系（Ｃ）の座標（ＸＣ，ＹＣ，ＺＣ）に変換する。次に、特徴点探索部１４は、ＺＣ＝１に設定して、変換後の特徴点の座標を下記の数４を用いて正規化する。そして、特徴点探索部１４は、図５に示すように、正規化によって得られた特徴点の座標（Ｘ’Ｃ，Ｙ’Ｃ）を、下記の数４を用いて、次フレーム上に再投影し、再投影した特徴点の２次元座標を中心に、探索ウィンドウを設定する。
【0049】
【数3】
【0050】
【数4】
【0051】
【数5】
【0052】
また、特徴点探索部１４は、図５に示すように、探索ウィンドウにおける、次フレームのエピポーラ線に並行な方向の長さが、その方向に垂直な方向の長さよりも長くなるように、次フレーム上に探索ウィンドウを設定する。すなわち、特徴点探索部１４は、水平方向の長さの方が垂直方向の長さよりも長くなるように、探索ウィンドウを設定する。これにより、水平方向における動きが垂直方向における動きよりも大きい移動体１００、例えば、自走式のロボット、車両などにおいて、探索ウィンドウの位置の推定誤差を小さくすることができる。
【0053】
特徴点探索部１４は、設定した探索ウィンドウ内で、現フレームの特徴点とマッチングする次フレームの特徴点を探索する。特徴点探索部１４は、いくつかの特徴点それぞれについて、上述した処理を実行することによって、現フレームの特徴点と次フレームの特徴点との組み合わせを複数組抽出する。
【0054】
位置推定部１５は、まず、次フレームでのカメラ姿勢を算出すると、カメラ姿勢予測部１３によって予測された予測値、算出した次フレームでのカメラ姿勢、及びカルマンゲインを用いて、次フレームにおける事後予測値ｘｔを算出する。事後予測値ｘｔとしても、例えば、移動体１００の位置ｐ、速度ｖ、クォータ二オンｑが用いられる。カルマンゲインとしては、上述したように、カメラ姿勢予測部１３によって算出される最適カルマンゲインＫｔが用いられる。
【0055】
続いて、位置推定部１５は、抽出された特徴点の組合せ毎に、事後予測値を用いて、次フレームでの真のカメラの姿勢［Ｒ］［ｔ］、言い換えると、移動体の３次元座標を算出する。
【0056】
［装置動作］
次に、実施の形態における位置推定装置１０の動作について図６を用いて説明する。図６は、実施の形態における位置推定装置の動作を示すフロー図である。以下の説明においては、適宜図１～図５を参照する。また、実施の形態では、位置推定装置１０を動作させることによって、位置推定方法が実施される。よって、実施の形態における位置推定方法の説明は、以下の位置推定装置１０の動作説明に代える。
次に、実施の形態における位置推定装置１０の動作について図６を用いて説明する。図６は、実施の形態における位置推定装置の動作を示すフロー図である。以下の説明においては、適宜図１～図５を参照する。また、実施の形態では、位置推定装置１０を動作させることによって、位置推定方法が実施される。よって、実施の形態における位置推定方法の説明は、以下の位置推定装置１０の動作説明に代える。
【0057】
まず、前提として、初期の環境地図が作成されているとする。カメラ２０は、移動体１００に搭載されており、一定のフレームレートで画像データを出力している。画像データ取得部１１は、カメラ２０が出力する画像データをフレーム毎に取得している。
【0058】
図６に示すように、初期の環境地図が作成されると、最初に、カメラ姿勢予測部１３は、事前予測値である、移動体１００の位置ｐ、速度ｖ、及びクォータ二オンｑの初期値と、共分散行列Ｐｔの初期値（ｔ＝０）とを設定する（ステップＡ１）。
【0059】
次に、センサデータ取得部１２は、ＩＭＵであるセンサ３０から、移動体１００における加速度データ（ａｘ，ａｙ，ａｚ）及び角速度データ（ωｘ，ωｙ，ωｚ）を取得する（ステップＡ２）。
【0060】
次に、ステップＡ２によって加速度データ及び角速度データが取得されると、カメラ姿勢予測部１３は、上述した数１及び数２に、取得した加速度データ及び角速度データを適用して、最新の事前予測値として、位置ｐ、速度ｖ、及びクォータ二オンｑを算出する（ステップＡ３）。また、ステップＡ３では、共分散行列Ｐｔの事前予測値も算出される。なお、ステップＡ３が初めて実行される場合は、ステップＡ１で設定された初期値が用いられる。
【0061】
次に、カメラ姿勢予測部１３は、ステップＡ３で事前予測値として算出した、移動体１００の位置ｐ及びクォータ二オンｑを用いて、次フレームでのカメラ姿勢［Ｒ｜ｔ］の予測値を算出する（ステップＡ４）。また、ステップＡ４では、カメラ姿勢予測部１３は、最適カルマンゲインＫｔも算出する。
【0062】
次に、特徴点探索部１４は、画像データ取得部１１が次フレームを取得していると、ステップＡ４で算出されたカメラ姿勢［Ｒ｜ｔ］の予測値を用いて、次フレーム上に探索ウィンドウを設定する（ステップＡ５）。ステップＡ５では、探索ウィンドウの設定は、次フレームのエピポーラ線に並行な方向の長さが、その方向に垂直な方向の長さよりも長くなるように行われる。
【0063】
次に、特徴点探索部１４は、ステップＡ５で設定した探索ウィンドウ内で、現フレームの特徴点とマッチングする次フレームの特徴点を探索し、現フレームの特徴点と次フレームの特徴点との組み合わせを複数組抽出する（ステップＡ６）。
【0064】
次に、位置推定部１５は、ステップＡ６で抽出された特徴点の組合せ毎に、次フレーム上の特徴点の２次元座標と、環境地図（点群データ）での特徴点の３次元座標とを用いて、ＰｎＰ法を実行して、次フレームでのカメラ姿勢を算出する（ステップＡ７）。
【0065】
次に、位置推定部１５は、ステップＡ４で算出された予測値と、同じくステップＡ４で算出された最適カルマンゲインＫｔと、ステップＡ７で算出された次フレームでのカメラ姿勢とを用いて、次フレームにおける事後予測値として、位置ｐ、速度ｖ、及びクォータ二オンｑを算出する（ステップＡ８）。
【0066】
次に、位置推定部１５は、ステップＡ８で算出された位置ｐ、及びクォータ二オンｑを用いて、移動体の３次元座標（次フレームでの真のカメラの姿勢［Ｒ］［ｔ］）を算出する（ステップＡ９）。
【0067】
次に、カメラ姿勢予測部１３は、移動体１００が稼働停止しているといった終了条件が満たされているかどうかを判定する（ステップＡ１０）。ステップＡ１０の判定の結果、満たされていない場合は、カメラ姿勢予測部１３は、再度ステップＡ２を実行させる。一方、ステップＡ１０の判定の結果、満たされている場合は、位置推定装置１０における処理は終了する。
【0068】
［実施の形態による効果］
以上のように、実施の形態では、移動体１００に取り付けられたセンサ３０（ＩＭＵ）からの情報を用いて、探索ウィンドウが設定されるので、移動体１００が急に旋回する等して急激に動きが変化した場合でも、適切に探索ウィンドウが設定されることになる。この結果、位置推定装置１０によれば、移動体１００の位置の推定において、移動体１００の動きによる推定精度の変動を抑制することができる。
以上のように、実施の形態では、移動体１００に取り付けられたセンサ３０（ＩＭＵ）からの情報を用いて、探索ウィンドウが設定されるので、移動体１００が急に旋回する等して急激に動きが変化した場合でも、適切に探索ウィンドウが設定されることになる。この結果、位置推定装置１０によれば、移動体１００の位置の推定において、移動体１００の動きによる推定精度の変動を抑制することができる。
【0069】
［プログラム］
実施の形態におけるプログラムとしては、コンピュータに、図６に示すステップＡ１～Ａ１０を実行させるプログラムが挙げられる。このプログラムをコンピュータにインストールし、実行することによって、実施の形態における位置推定装置１０と位置推定方法とを実現することができる。この場合、コンピュータのプロセッサは、画像データ取得部１１、センサデータ取得部１２、カメラ姿勢予測部１３、特徴点探索部１４、及び位置推定部１５として機能し、処理を行なう。
実施の形態におけるプログラムとしては、コンピュータに、図６に示すステップＡ１～Ａ１０を実行させるプログラムが挙げられる。このプログラムをコンピュータにインストールし、実行することによって、実施の形態における位置推定装置１０と位置推定方法とを実現することができる。この場合、コンピュータのプロセッサは、画像データ取得部１１、センサデータ取得部１２、カメラ姿勢予測部１３、特徴点探索部１４、及び位置推定部１５として機能し、処理を行なう。
【0070】
また、コンピュータとしては、汎用のＰＣの他に、スマートフォン、タブレット型端末装置が挙げられる。更に、コンピュータは、移動体１００に搭載されたコンピュータ（制御装置４０）であっても良い。
【0071】
また、実施の形態におけるプログラムは、複数のコンピュータによって構築されたコンピュータシステムによって実行されても良い。この場合は、例えば、各コンピュータが、それぞれ、画像データ取得部１１、センサデータ取得部１２、カメラ姿勢予測部１３、特徴点探索部１４、及び位置推定部１５のいずれかとして機能しても良い。
【0072】
（物理構成）
ここで、実施の形態におけるプログラムを実行することによって、位置推定装置１０を実現するコンピュータについて図７を用いて説明する。図７は、実施の形態における位置推定装置を実現するコンピュータの一例を示すブロック図である。
ここで、実施の形態におけるプログラムを実行することによって、位置推定装置１０を実現するコンピュータについて図７を用いて説明する。図７は、実施の形態における位置推定装置を実現するコンピュータの一例を示すブロック図である。
【0073】
図７に示すように、コンピュータ１１０は、ＣＰＵ（CentralProcessing Unit）１１１と、メインメモリ１１２と、記憶装置１１３と、入力インターフェイス１１４と、表示コントローラ１１５と、データリーダ／ライタ１１６と、通信インターフェイス１１７とを備える。これらの各部は、バス１２１を介して、互いにデータ通信可能に接続される。
【0074】
また、コンピュータ１１０は、ＣＰＵ１１１に加えて、又はＣＰＵ１１１に代えて、ＧＰＵ（Graphics Processing Unit）、又はＦＰＧＡ（Field-ProgrammableGate Array）を備えていても良い。この態様では、ＧＰＵ又はＦＰＧＡが、実施の形態におけるプログラムを実行することができる。
【0075】
ＣＰＵ１１１は、記憶装置１１３に格納された、コード群で構成された実施の形態におけるプログラムをメインメモリ１１２に展開し、各コードを所定順序で実行することにより、各種の演算を実施する。メインメモリ１１２は、典型的には、ＤＲＡＭ（Dynamic Random Access Memory）等の揮発性の記憶装置である。
【0076】
また、実施の形態におけるプログラムは、コンピュータ読み取り可能な記録媒体１２０に格納された状態で提供される。なお、本実施の形態におけるプログラムは、通信インターフェイス１１７を介して接続されたインターネット上で流通するものであっても良い。
【0077】
また、記憶装置１１３の具体例としては、ハードディスクドライブの他、フラッシュメモリ等の半導体記憶装置が挙げられる。入力インターフェイス１１４は、ＣＰＵ１１１と、キーボード及びマウスといった入力機器１１８との間のデータ伝送を仲介する。表示コントローラ１１５は、ディスプレイ装置１１９と接続され、ディスプレイ装置１１９での表示を制御する。
【0078】
データリーダ／ライタ１１６は、ＣＰＵ１１１と記録媒体１２０との間のデータ伝送を仲介し、記録媒体１２０からのプログラムの読み出し、及びコンピュータ１１０における処理結果の記録媒体１２０への書き込みを実行する。通信インターフェイス１１７は、ＣＰＵ１１１と、他のコンピュータとの間のデータ伝送を仲介する。
【0079】
また、記録媒体１２０の具体例としては、ＣＦ（Compact Flash（登録商標））及びＳＤ（Secure Digital）等の汎用的な半導体記憶デバイス、フレキシブルディスク（Flexible Disk）等の磁気記録媒体、又はＣＤ－ＲＯＭ（Compact DiskRead Only Memory）などの光学記録媒体が挙げられる。
【0080】
実施の形態における位置推定装置１０は、プログラムがインストールされたコンピュータではなく、各部に対応したハードウェアを用いることによっても実現可能である。更に、位置推定装置１０は、一部がプログラムで実現され、残りの部分がハードウェアで実現されていてもよい。
【実施例】
【0081】
続いて、実施の形態における位置推定装置１０の実施例について説明する。また、以下の説明は、図６に示したステップＡ１～Ａ１０に沿って行う。更に、以下の説明では、下記の文献が参照される。
（参照文献）
Joan Sola,"Quaternionkinematics for the error-state Kalman filter",2017,
<URL:https://arxiv.org/abs/1711.02508>
（参照文献）
Joan Sola,"Quaternionkinematics for the error-state Kalman filter",2017,
<URL:https://arxiv.org/abs/1711.02508>
【0082】
（ステップＡ１）
カメラ姿勢予測部１３は、まず、移動体１００の位置ｐ及び速度ｖの初期値を下記の数６に示すように設定する。但し、ＩＭＵが計測を行った時点を「ｔ－１」とし、初期の環境地図の生成中のカメラ姿勢を［Ｒ｜ｔ］とする。
カメラ姿勢予測部１３は、まず、移動体１００の位置ｐ及び速度ｖの初期値を下記の数６に示すように設定する。但し、ＩＭＵが計測を行った時点を「ｔ－１」とし、初期の環境地図の生成中のカメラ姿勢を［Ｒ｜ｔ］とする。
【0083】
【数6】
【0084】
また、回転行列Ｒは数７のように定義される。クォータ二オンｑは、回転行列の各要素を用いて、数７及び数８によって定義される。従って、クォータ二オンｑの初期値は、「ｔ－１」における回転行列から求められる。
【0085】
【数7】
【0086】
【数8】
【0087】
【数9】
【0088】
（ステップＡ２）
センサデータ取得部１２は、ＩＭＵであるセンサ３０から、移動体１００における加速度データ（ａｘ，ａｙ，ａｚ）及び角速度データ（ωｘ，ωｙ，ωｚ）を取得する。ここで、加速度データから得られる実測置をＲ（ａｍ－ａｂ）とし、角速度データから得られる実測置を（ωｍ－ωｂ）とする。なお、ａｍ及びωｍは計測値であり、ａｂ及びωｂは誤差である。
センサデータ取得部１２は、ＩＭＵであるセンサ３０から、移動体１００における加速度データ（ａｘ，ａｙ，ａｚ）及び角速度データ（ωｘ，ωｙ，ωｚ）を取得する。ここで、加速度データから得られる実測置をＲ（ａｍ－ａｂ）とし、角速度データから得られる実測置を（ωｍ－ωｂ）とする。なお、ａｍ及びωｍは計測値であり、ａｂ及びωｂは誤差である。
【0089】
（ステップＡ３）
カメラ姿勢予測部１３は、下記の数１０に、角度データ及び角速度データの実測置を適用して、位置ｐ、速度ｖ、及びクォータ二オンｑの最新の事前予測値を算出する。また、カメラ姿勢予測部１３は、下記の数１１を用いて、共分散行列Ｐｔの事前予測値を算出する。数１１のＦｘおよびＱｉは数１０から算出されるヤコビ行列である。
カメラ姿勢予測部１３は、下記の数１０に、角度データ及び角速度データの実測置を適用して、位置ｐ、速度ｖ、及びクォータ二オンｑの最新の事前予測値を算出する。また、カメラ姿勢予測部１３は、下記の数１１を用いて、共分散行列Ｐｔの事前予測値を算出する。数１１のＦｘおよびＱｉは数１０から算出されるヤコビ行列である。
【0090】
【数10】
【0091】
【数11】
【0092】
（ステップＡ４）
カメラ姿勢予測部１３は、クォータ二オンｑの事前予測値を、下記の数１２に適用して、次フレームでのカメラ姿勢［Ｒ｜ｔ］の予測値を算出する。また、カメラの次フレームでの位置は、ステップＡ３で算出された位置ｐとする。
カメラ姿勢予測部１３は、クォータ二オンｑの事前予測値を、下記の数１２に適用して、次フレームでのカメラ姿勢［Ｒ｜ｔ］の予測値を算出する。また、カメラの次フレームでの位置は、ステップＡ３で算出された位置ｐとする。
【0093】
【数12】
【0094】
また、カルマンフィルタにおける最適カルマンゲインＫｔの算出式は、下記数１３に示す通りであり、関数ｈのヤコビアン行列Ｈから算出される。従って、カメラ姿勢予測部１３は、下記の数１３を用いて最適カルマンゲインＫｔを算出する。
【0095】
【数13】
【0096】
（ステップＡ５）
特徴点探索部１４は、画像データ取得部１１が次フレームを取得していると、ステップＡ４で算出したカメラ姿勢［Ｒ｜ｔ］の予測値を用いて、次フレーム上に探索ウィンドウを設定する。
特徴点探索部１４は、画像データ取得部１１が次フレームを取得していると、ステップＡ４で算出したカメラ姿勢［Ｒ｜ｔ］の予測値を用いて、次フレーム上に探索ウィンドウを設定する。
【0097】
具体的には、特徴点探索部１４は、カメラ姿勢［Ｒ］［ｔ］の予測値を下記の数３に適用して、現フレームの特徴点の３次元座標（世界座標系（Ｗ））（ＸＷ，ＹＷ，ＺＷ）を、カメラ座標系（Ｃ）の座標（ＸＣ，ＹＣ，ＺＣ）に変換する。次に、特徴点探索部１４は、ＺＣ＝１に設定して、変換後の特徴点の座標を、上記数４を用いて正規化する。そして、特徴点探索部１４は、図５に示すように、正規化によって得られた特徴点の座標（Ｘ’Ｃ，Ｙ’Ｃ）を、上記数４を用いて、次フレーム上に再投影し、再投影した特徴点の２次元座標を中心に、探索ウィンドウを設定する。
【0098】
また、特徴点探索部１４は、図５に示したように、探索ウィンドウにおける、次フレームのエピポーラ線に並行な方向の長さが、その方向に垂直な方向の長さよりも長くなるように、次フレーム上に探索ウィンドウを設定する。
【0099】
（ステップＡ６）
特徴点探索部１４は、ステップＡ５で設定した探索ウィンドウ内で、現フレームの特徴点とマッチングする次フレームの特徴点を探索し、現フレームの特徴点と次フレームの特徴点との組み合わせを複数組抽出する。
特徴点探索部１４は、ステップＡ５で設定した探索ウィンドウ内で、現フレームの特徴点とマッチングする次フレームの特徴点を探索し、現フレームの特徴点と次フレームの特徴点との組み合わせを複数組抽出する。
【0100】
（ステップＡ７）
位置推定部１５は、ステップＡ６で抽出された特徴点の組合せ毎に、次フレーム上の特徴点の２次元座標と、環境地図（点群データ）での特徴点の３次元座標とを用いて、ＰｎＰ法を実行して、次フレームでのカメラ姿勢を算出する。
位置推定部１５は、ステップＡ６で抽出された特徴点の組合せ毎に、次フレーム上の特徴点の２次元座標と、環境地図（点群データ）での特徴点の３次元座標とを用いて、ＰｎＰ法を実行して、次フレームでのカメラ姿勢を算出する。
【0101】
（ステップＡ８）
まず、ステップＡ７で推定された次フレームでのカメラ姿勢から、移動体１００の自己位置ｙが求められる。そして、時刻ｔにおけるセンサデータａｔ及びωｔと、次フレームでの移動体１００の自己位置ｙとは、下記数１４に示すように、関数ｈで表現される。また、数１４においてｈ（）は、システム状態を表す非線形関数であり、ここでのｖは共分散行列Ｖで表されるガウスノイズである。
まず、ステップＡ７で推定された次フレームでのカメラ姿勢から、移動体１００の自己位置ｙが求められる。そして、時刻ｔにおけるセンサデータａｔ及びωｔと、次フレームでの移動体１００の自己位置ｙとは、下記数１４に示すように、関数ｈで表現される。また、数１４においてｈ（）は、システム状態を表す非線形関数であり、ここでのｖは共分散行列Ｖで表されるガウスノイズである。
【0102】
【数14】
【0103】
そして、カルマン係数Ｋと、関数ｈとの間には、以下の数１５が成立する。
【0104】
【数15】
【0105】
更に、上述の数１５におけるハットδｘは、下記の数１６に示すように、事後予測値ｘｔとの間に関係がある。従って、位置推定部１５は、ステップＡ８において、下記の数１７を用いて、位置ｐ、速度ｖ、及びクォータ二オンｑの事後予測値を算出する。
【0106】
【数16】
【0107】
【数17】
【0108】
（ステップＡ９）
ステップＡ９では、位置推定部１５は、ステップＡ６で抽出された特徴点の組合せ毎に、事後予測値（位置ｐ、速度ｖ、及びクォータ二オンｑ）を用いて、移動体の３次元座標（次フレームでの真のカメラの姿勢［Ｒ］［ｔ］）を算出する。
ステップＡ９では、位置推定部１５は、ステップＡ６で抽出された特徴点の組合せ毎に、事後予測値（位置ｐ、速度ｖ、及びクォータ二オンｑ）を用いて、移動体の３次元座標（次フレームでの真のカメラの姿勢［Ｒ］［ｔ］）を算出する。
【0109】
（ステップＡ１０）
その後、ステップＡ９が実行され、終了条件が満たされていない場合は、再度ステップＡ２が実行され、終了条件が満たされている場合は、位置推定装置１０での処理が終了する。
その後、ステップＡ９が実行され、終了条件が満たされていない場合は、再度ステップＡ２が実行され、終了条件が満たされている場合は、位置推定装置１０での処理が終了する。
【0110】
上述した実施の形態及び実施例の一部又は全部は、以下に記載する（付記１）～（付記１８）によって表現することができるが、以下の記載に限定されるものではない。
【0111】
（付記１）
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得部と、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得部と、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測部と、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索部と、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定部と、
を備えている、ことを特徴とする位置推定装置。
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得部と、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得部と、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測部と、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索部と、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定部と、
を備えている、ことを特徴とする位置推定装置。
【0112】
（付記２）
付記１に記載の位置推定装置であって、
前記カメラ姿勢予測部が、前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとを用いた、ベイズ推定を行うことによって、前記予測値を算出する、
ことを特徴とする位置推定装置。
付記１に記載の位置推定装置であって、
前記カメラ姿勢予測部が、前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとを用いた、ベイズ推定を行うことによって、前記予測値を算出する、
ことを特徴とする位置推定装置。
【0113】
（付記３）
付記１または２に記載の位置推定装置であって、
前記特徴点探索部が、前記探索ウィンドウにおける、前記次フレームのエピポーラ線に並行な方向の長さが、当該方向に垂直な方向の長さよりも長くなるように、前記探索ウィンドウを設定する、
ことを特徴とする位置推定装置。
付記１または２に記載の位置推定装置であって、
前記特徴点探索部が、前記探索ウィンドウにおける、前記次フレームのエピポーラ線に並行な方向の長さが、当該方向に垂直な方向の長さよりも長くなるように、前記探索ウィンドウを設定する、
ことを特徴とする位置推定装置。
【0114】
（付記４）
付記１～３のいずれかに記載の位置推定装置であって、
前記センサが、慣性計測装置、加速度センサ、及び角速度センサのうちの少なくとも１つを含む、
ことを特徴とする位置推定装置。
付記１～３のいずれかに記載の位置推定装置であって、
前記センサが、慣性計測装置、加速度センサ、及び角速度センサのうちの少なくとも１つを含む、
ことを特徴とする位置推定装置。
【0115】
（付記５）
付記１～４のいずれかに記載の位置推定装置であって、
前記カメラ姿勢が、前記カメラの回転行列及び並進ベクトルを含む、
ことを特徴とする位置推定装置。
付記１～４のいずれかに記載の位置推定装置であって、
前記カメラ姿勢が、前記カメラの回転行列及び並進ベクトルを含む、
ことを特徴とする位置推定装置。
【0116】
（付記６）
付記２に記載の位置推定装置であって、
前記カメラ姿勢予測部が、前記フレームの取得後に取得された前記センサデータをカルマンフィルタに適用して、事前予測値を算出し、算出した前記事前予測値と前記フレームでのカメラ姿勢とを用いて、前記予測値を算出し、
前記位置推定部が、前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出した後に、前記予測値、算出した前記次フレームでのカメラ姿勢、及びカルマンゲインを用いて、前記次フレームにおける事後予測値を算出し、更に、前記事後予測値を用いて、前記次フレームにおける前記移動体の位置を推定する、
ことを特徴とする位置推定装置。
付記２に記載の位置推定装置であって、
前記カメラ姿勢予測部が、前記フレームの取得後に取得された前記センサデータをカルマンフィルタに適用して、事前予測値を算出し、算出した前記事前予測値と前記フレームでのカメラ姿勢とを用いて、前記予測値を算出し、
前記位置推定部が、前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出した後に、前記予測値、算出した前記次フレームでのカメラ姿勢、及びカルマンゲインを用いて、前記次フレームにおける事後予測値を算出し、更に、前記事後予測値を用いて、前記次フレームにおける前記移動体の位置を推定する、
ことを特徴とする位置推定装置。
【0117】
（付記７）
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得ステップと、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得ステップと、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測ステップと、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索ステップと、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定ステップと、
を有する、ことを特徴とする位置推定方法。
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得ステップと、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得ステップと、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測ステップと、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索ステップと、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定ステップと、
を有する、ことを特徴とする位置推定方法。
【0118】
（付記８）
付記７に記載の位置推定方法であって、
前記カメラ姿勢予測ステップにおいて、前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとを用いた、ベイズ推定を行うことによって、前記予測値を算出する、
ことを特徴とする位置推定方法。
付記７に記載の位置推定方法であって、
前記カメラ姿勢予測ステップにおいて、前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとを用いた、ベイズ推定を行うことによって、前記予測値を算出する、
ことを特徴とする位置推定方法。
【0119】
（付記９）
付記７または８に記載の位置推定方法であって、
前記特徴点探索ステップにおいて、前記探索ウィンドウにおける、前記次フレームのエピポーラ線に並行な方向の長さが、当該方向に垂直な方向の長さよりも長くなるように、前記探索ウィンドウを設定する、
ことを特徴とする位置推定方法。
付記７または８に記載の位置推定方法であって、
前記特徴点探索ステップにおいて、前記探索ウィンドウにおける、前記次フレームのエピポーラ線に並行な方向の長さが、当該方向に垂直な方向の長さよりも長くなるように、前記探索ウィンドウを設定する、
ことを特徴とする位置推定方法。
【0120】
（付記１０）
付記７～９のいずれかに記載の位置推定方法であって、
前記センサが、慣性計測装置、加速度センサ、及び角速度センサのうちの少なくとも１つを含む、
ことを特徴とする位置推定方法。
付記７～９のいずれかに記載の位置推定方法であって、
前記センサが、慣性計測装置、加速度センサ、及び角速度センサのうちの少なくとも１つを含む、
ことを特徴とする位置推定方法。
【0121】
（付記１１）
付記７～１０のいずれかに記載の位置推定方法であって、
前記カメラ姿勢が、前記カメラの回転行列及び並進ベクトルを含む、
ことを特徴とする位置推定方法。
付記７～１０のいずれかに記載の位置推定方法であって、
前記カメラ姿勢が、前記カメラの回転行列及び並進ベクトルを含む、
ことを特徴とする位置推定方法。
【0122】
（付記１２）
付記８に記載の位置推定方法であって、
前記カメラ姿勢予測ステップにおいて、前記フレームの取得後に取得された前記センサデータをカルマンフィルタに適用して、事前予測値を算出し、算出した前記事前予測値と前記フレームでのカメラ姿勢とを用いて、前記予測値を算出し、
前記位置推定ステップにおいて、前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出した後に、前記予測値、算出した前記次フレームでのカメラ姿勢、及びカルマンゲインを用いて、前記次フレームにおける事後予測値を算出し、更に、前記事後予測値を用いて、前記次フレームにおける前記移動体の位置を推定する、
ことを特徴とする位置推定方法。
付記８に記載の位置推定方法であって、
前記カメラ姿勢予測ステップにおいて、前記フレームの取得後に取得された前記センサデータをカルマンフィルタに適用して、事前予測値を算出し、算出した前記事前予測値と前記フレームでのカメラ姿勢とを用いて、前記予測値を算出し、
前記位置推定ステップにおいて、前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出した後に、前記予測値、算出した前記次フレームでのカメラ姿勢、及びカルマンゲインを用いて、前記次フレームにおける事後予測値を算出し、更に、前記事後予測値を用いて、前記次フレームにおける前記移動体の位置を推定する、
ことを特徴とする位置推定方法。
【0123】
（付記１３）
コンピュータに、
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得ステップと、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得ステップと、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測ステップと、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索ステップと、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定ステップと、
を実行させる、プログラム。
コンピュータに、
移動体に搭載されたカメラから出力された画像データをフレーム毎に取得する、画像データ取得ステップと、
前記移動体の姿勢を検出するセンサからセンサデータを取得する、センサデータ取得ステップと、
取得された前記フレームでのカメラ姿勢を求め、求めた前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとから、前記フレームの後に取得される次フレームでのカメラ姿勢の予測値を算出する、カメラ姿勢予測ステップと、
前記次フレームの取得後に、前記予測値を用いて、前記次フレーム上に探索ウィンドウを設定し、前記探索ウィンドウ内で、前記フレーム内の特徴点に対応する特徴点を探索する、特徴点探索ステップと、
前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出し、算出した前記次フレームでのカメラ姿勢に基づいて、前記次フレームにおける前記移動体の位置を推定する、位置推定ステップと、
を実行させる、プログラム。
【0124】
（付記１４）
付記１３に記載のプログラムであって、
前記カメラ姿勢予測ステップにおいて、前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとを用いた、ベイズ推定を行うことによって、前記予測値を算出する、
ことを特徴とするプログラム。
付記１３に記載のプログラムであって、
前記カメラ姿勢予測ステップにおいて、前記フレームでのカメラ姿勢と、前記フレームの取得後に取得された前記センサデータとを用いた、ベイズ推定を行うことによって、前記予測値を算出する、
ことを特徴とするプログラム。
【0125】
（付記１５）
付記１３または１４に記載のプログラムであって、
前記特徴点探索ステップにおいて、前記探索ウィンドウにおける、前記次フレームのエピポーラ線に並行な方向の長さが、当該方向に垂直な方向の長さよりも長くなるように、前記探索ウィンドウを設定する、
ことを特徴とするプログラム。
付記１３または１４に記載のプログラムであって、
前記特徴点探索ステップにおいて、前記探索ウィンドウにおける、前記次フレームのエピポーラ線に並行な方向の長さが、当該方向に垂直な方向の長さよりも長くなるように、前記探索ウィンドウを設定する、
ことを特徴とするプログラム。
【0126】
（付記１６）
付記１３～１５のいずれかに記載のプログラムであって、
前記センサが、慣性計測装置、加速度センサ、及び角速度センサのうちの少なくとも１つを含む、
ことを特徴とするプログラム。
付記１３～１５のいずれかに記載のプログラムであって、
前記センサが、慣性計測装置、加速度センサ、及び角速度センサのうちの少なくとも１つを含む、
ことを特徴とするプログラム。
【0127】
（付記１７）
付記１３～１６のいずれかに記載のプログラムであって、
前記カメラ姿勢が、前記カメラの回転行列及び並進ベクトルを含む、
ことを特徴とするプログラム。
付記１３～１６のいずれかに記載のプログラムであって、
前記カメラ姿勢が、前記カメラの回転行列及び並進ベクトルを含む、
ことを特徴とするプログラム。
【0128】
（付記１８）
付記１４に記載のプログラムであって、
前記カメラ姿勢予測ステップにおいて、前記フレームの取得後に取得された前記センサデータをカルマンフィルタに適用して、事前予測値を算出し、算出した前記事前予測値と前記フレームでのカメラ姿勢とを用いて、前記予測値を算出し、
前記位置推定ステップにおいて、前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出した後に、前記予測値、算出した前記次フレームでのカメラ姿勢、及びカルマンゲインを用いて、前記次フレームにおける事後予測値を算出し、更に、前記事後予測値を用いて、前記次フレームにおける前記移動体の位置を推定する、
ことを特徴とするプログラム。
付記１４に記載のプログラムであって、
前記カメラ姿勢予測ステップにおいて、前記フレームの取得後に取得された前記センサデータをカルマンフィルタに適用して、事前予測値を算出し、算出した前記事前予測値と前記フレームでのカメラ姿勢とを用いて、前記予測値を算出し、
前記位置推定ステップにおいて、前記予測値とは別に、探索された特徴点に基づいて、前記次フレームでのカメラ姿勢を算出した後に、前記予測値、算出した前記次フレームでのカメラ姿勢、及びカルマンゲインを用いて、前記次フレームにおける事後予測値を算出し、更に、前記事後予測値を用いて、前記次フレームにおける前記移動体の位置を推定する、
ことを特徴とするプログラム。
【産業上の利用可能性】
【0129】
以上のように本発明によれば、移動体の位置の推定において、移動体の動きによる推定精度の変動を抑制することができる。本発明は、ＳＬＡＭを利用するシステムに有用である。
【符号の説明】
【0130】
１０ 位置推定装置
１１ 画像データ取得部
１２ センサデータ取得部
１３ カメラ姿勢予測部
１４ 特徴点探索部
１５ 位置推定部
２０ カメラ
３０ センサ
４０ 制御装置
５０ 駆動装置
６０ 操舵装置
７０ 測距装置
１００ 移動体
１１０ コンピュータ
１１１ ＣＰＵ
１１２ メインメモリ
１１３ 記憶装置
１１４ 入力インターフェイス
１１５ 表示コントローラ
１１６ データリーダ／ライタ
１１７ 通信インターフェイス
１１８ 入力機器
１１９ ディスプレイ装置
１２０ 記録媒体
１２１ バス
１１ 画像データ取得部
１２ センサデータ取得部
１３ カメラ姿勢予測部
１４ 特徴点探索部
１５ 位置推定部
２０ カメラ
３０ センサ
４０ 制御装置
５０ 駆動装置
６０ 操舵装置
７０ 測距装置
１００ 移動体
１１０ コンピュータ
１１１ ＣＰＵ
１１２ メインメモリ
１１３ 記憶装置
１１４ 入力インターフェイス
１１５ 表示コントローラ
１１６ データリーダ／ライタ
１１７ 通信インターフェイス
１１８ 入力機器
１１９ ディスプレイ装置
１２０ 記録媒体
１２１ バス
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】