(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2023004517
(43)【公開日】2023-01-17
(54)【発明の名称】位置推定装置、位置推定方法、及びプログラム
(51)【国際特許分類】
G05D 1/02 20200101AFI20230110BHJP
【ＦＩ】
G05D1/02 K
【審査請求】未請求
【請求項の数】7
【出願形態】ＯＬ
(21)【出願番号】P 2021106241
(22)【出願日】2021-06-28
(71)【出願人】
【識別番号】000232092
【氏名又は名称】ＮＥＣソリューションイノベータ株式会社
(74)【代理人】
【識別番号】110002044
【氏名又は名称】弁理士法人ブライタス
(72)【発明者】
【氏名】山下 喜宏
(72)【発明者】
【氏名】杉本 聖弥
【テーマコード（参考）】
5H301
【Ｆターム（参考）】
5H301AA01
5H301BB14
5H301CC03
5H301CC06
5H301DD01
5H301GG08
5H301GG09
5H301HH10
(57)【要約】
【課題】コストの上昇を抑制しつつ、自己位置推定の精度の向上を図る。
【解決手段】位置推定装置１０は、移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける移動体の３次元座標を推定する、第１の位置推定部１１と、移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した深度データを用いて、移動体の２次元座標を推定する、第２の位置推定部１２と、推定された移動体の２次元座標についての設定条件が満たされているかどうかを判定し、設定条件が満たされている場合に、移動体の２次元座標を用いて、移動体の３次元座標を補正する、補正処理部１３と、を備えている。
【選択図】図１
【解決手段】位置推定装置１０は、移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける移動体の３次元座標を推定する、第１の位置推定部１１と、移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した深度データを用いて、移動体の２次元座標を推定する、第２の位置推定部１２と、推定された移動体の２次元座標についての設定条件が満たされているかどうかを判定し、設定条件が満たされている場合に、移動体の２次元座標を用いて、移動体の３次元座標を補正する、補正処理部１３と、を備えている。
【選択図】図１
【特許請求の範囲】
【請求項1】
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定部と、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定部と、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理部と、
を備えている、
ことを特徴とする位置推定装置。
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定部と、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理部と、
を備えている、
ことを特徴とする位置推定装置。
【請求項2】
請求項１に記載の位置推定装置であって、
前記第１の位置推定部が、対応する特徴点の組を用いて、前記特徴点の３次元座標を算出し、前記３次元座標が算出された前記特徴点を用いて、点群データで構成された３次元の環境地図を生成又は更新する、
ことを特徴とする位置推定装置。
前記第１の位置推定部が、対応する特徴点の組を用いて、前記特徴点の３次元座標を算出し、前記３次元座標が算出された前記特徴点を用いて、点群データで構成された３次元の環境地図を生成又は更新する、
ことを特徴とする位置推定装置。
【請求項3】
請求項１または２に記載の位置推定装置であって、
前記補正処理部が、前記移動体の２次元座標と前記移動体の３次元座標との差分を求め、求めた差分に基づいて、前記移動体の３次元座標、及び前記３次元の環境地図を補正する、
ことを特徴とする位置推定装置。
前記補正処理部が、前記移動体の２次元座標と前記移動体の３次元座標との差分を求め、求めた差分に基づいて、前記移動体の３次元座標、及び前記３次元の環境地図を補正する、
ことを特徴とする位置推定装置。
【請求項4】
請求項１～３のいずれかに記載の位置推定装置であって、
前記第２の位置推定部が、取得した前記深度データを用いて、前記移動体の周辺の２次元の点群データを生成し、生成した前記２次元の点群データと、予め作成されている２次元の自律走行用地図とを用いて、前記移動体の２次元座標を推定する、
ことを特徴とする位置推定装置。
前記第２の位置推定部が、取得した前記深度データを用いて、前記移動体の周辺の２次元の点群データを生成し、生成した前記２次元の点群データと、予め作成されている２次元の自律走行用地図とを用いて、前記移動体の２次元座標を推定する、
ことを特徴とする位置推定装置。
【請求項5】
請求項１～４のいずれかに記載の位置推定装置であって、
前記補正処理部が、前記２次元深度センサが一回に出力した深度データにおけるバラツキを算出し、算出したバラツキが閾値以上となる場合に、前記設定条件が満たされていると判定する、
ことを特徴とする位置推定装置。
前記補正処理部が、前記２次元深度センサが一回に出力した深度データにおけるバラツキを算出し、算出したバラツキが閾値以上となる場合に、前記設定条件が満たされていると判定する、
ことを特徴とする位置推定装置。
【請求項6】
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定ステップと、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を有する、
ことを特徴とする位置推定方法。
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を有する、
ことを特徴とする位置推定方法。
【請求項7】
コンピュータに、
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定ステップと、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を実行させる、プログラム。
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定ステップと、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を実行させる、プログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体の位置の推定するための、位置推定装置、及び位置推定方法に関し、更には、これらを実現するためのプログラムに関する。
【背景技術】
【0002】
近年、センシング技術の向上により、ＳＬＡＭ（Simultaneous Localization and Mapping）が注目されている。ＳＬＡＭは、センサを搭載した移動体によって、自己位置の推定と環境地図の作成とを同時に実行する技術である（例えば、特許文献１及び非特許文献１参照）。ＳＬＡＭによれば、自律走行するロボット等の移動体は、ランダムに移動する必要がなく、環境地図から得られる自律走行用地図に沿って移動できるため、その移動効率は向上することになる。
【0003】
特許文献１及び非特許文献１においては、センサとして、カメラが用いられている。具体的には、特許文献１及び非特許文献１では、移動体は、カメラから出力される画像データをフレーム毎に取得し、最新のフレームにおいて、過去のフレームから抽出された特徴点に対応する特徴点を探索し、対応する特徴点同士で構成された特徴点の組を抽出する。
【0004】
そして、移動体は、特徴点の組を用いて、最新のフレームにおけるカメラ行列を算出し、特徴点の組毎に、カメラ行列と、特徴点のフレームにおける２次元座標とを用いて、特徴点の３次元座標を算出する。また、移動体は、カメラ行列を算出すると、カメラ行列からカメラの位置を算出し、自己位置推定を行う。更に、移動体は、３次元座標が算出された特徴点を用いて、３次元の点群で構成された環境地図（以下「３次元環境地図」と表記する。）を生成又は更新する。
【0005】
その後、移動体は、３次元環境地図を２次元に変換することによって、２次元の環境地図を生成する。但し、カメラを用いたＳＬＡＭでは、３次元環境地図を構成する点群は、画像上の特徴点であるので、点群の密度を高めることが難しいという問題がある。このため、自律走行用地図においては、特徴的な部分が少なく、この自律走行用地図では、移動体による精度の高い自律走行は困難となる。
【0006】
一方、センサとして、ＬｉＤＡＲ（light detection and ranging）に代表される深度センサを用いた場合は、上述の問題を解消できる。ＬｉＤＡＲには、３次元空間での深度を測定する３次元ＬｉＤＡＲと、特定の平面での深度を測定する２次元ＬｉＤＡＲとがあるが、一般的にＳＬＡＭで用いられるのは３次元ＬｉＤＡＲである。
【0007】
３次元ＬｉＤＡＲによれば、撮影画像の画素毎に、深度を測定することができるため、特徴点とならない点の３次元座標も算出でき、３次元環境地図における点群の密度を高めることができる。この結果、自律走行用地図が特徴的な部分が多くなるので、移動体による精度の高い自律走行が可能になると考えられる。また、３次元ＬｉＤＡＲによれば、正確な自己位置推定も可能となる。
【先行技術文献】
【特許文献】
【0008】
【特許文献1】特開２０２０－７６７１４号公報
【非特許文献】
【0009】
【非特許文献1】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＞
【発明の概要】
【発明が解決しようとする課題】
【0010】
しかしながら、３次元ＬｉＤＡＲにはコストが高いという問題、システムにおける処理負担が大きいという問題がある。一方、２次元ＬｉＤＡＲを用いれば、コストが高いという問題は解消されるが、２次元ＬｉＤＡＲでは、３次元環境地図を作成することができないという問題がある。また、２次元ＬｉＤＡＲでは、凹凸の少ない環境において特徴的な部分を抽出することが難しく、２次元ＬｉＤＡＲを用いた場合は、自己位置推定の精度が低下してしまう。
【0011】
本発明の開示の一例は、コストの上昇を抑制しつつ、自己位置推定の精度の向上を図りうる、位置推定装置、位置推定方法、及びプログラムを提供することにある。
【課題を解決するための手段】
【0012】
上記目的を達成するため、本開示の一側面における位置推定装置は、
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定部と、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定部と、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理部と、
を備えている、ことを特徴とする。
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定部と、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定部と、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理部と、
を備えている、ことを特徴とする。
【0013】
また、上記目的を達成するため、本開示の一側面における位置推定方法は、
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定ステップと、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を有する、ことを特徴とする。
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定ステップと、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を有する、ことを特徴とする。
【0014】
更に、上記目的を達成するため、本開示の一側面におけるプログラムは、
コンピュータに、
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定ステップと、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を実行させる、ことを特徴とする。
コンピュータに、
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定ステップと、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を実行させる、ことを特徴とする。
【発明の効果】
【0015】
以上のように本開示によれば、コストの上昇を抑制しつつ、自己位置推定の精度を向上させることができる。
【図面の簡単な説明】
【0016】
【発明を実施するための形態】
【0017】
【0018】
【0019】
図１に示す実施の形態における位置推定装置１０は、移動体の位置を推定するための
装置である。図１に示すように、位置推定装置１０は、第１の位置推定部１１と、第２の位置推定部１２と、補正処理部１３とを備えている。
装置である。図１に示すように、位置推定装置１０は、第１の位置推定部１１と、第２の位置推定部１２と、補正処理部１３とを備えている。
【0020】
第１の位置推定部１１は、移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、そのフレームの画像データから、別のフレームとの間で対応する特徴点を抽出する。そして、第１の位置推定部１１は、抽出した特徴点に基づいて、フレーム毎に、そのフレームにおける移動体の３次元座標を推定する。
【0021】
第２の位置推定部１２は、移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した深度データを用いて、移動体の２次元座標を推定する。
【0022】
補正処理部１３は、推定された移動体の２次元座標についての設定条件が満たされているかどうかを判定し、設定条件が満たされている場合に、移動体の２次元座標を用いて、移動体の３次元座標を補正する。
【0023】
このように、位置推定装置１０では、画像から推定された３次元座標が、２次元深度センサからの深度データを用いて補正されるので、自己位置推定の精度が向上する。また、位置推定装置１０では、３次元のＬｉＤＡＲ等の３次元の深度センサを用いる必要がないため、これを用いる場合に比べて、コストの上昇が抑制される。
【0024】
続いて、図２～図７を用いて、実施の形態における、位置推定装置の構成及び機能について具体的に説明する。図２は、実施の形態における位置推定装置の構成の一例を具体的に示す構成図である。図３は、第１の位置推定部における処理を説明する図である。図４（ａ）は、２次元深度センサから出力された深度データの一例を示す図であり、図４（ｂ）は、予め作成されている２次元の自律走行用地図（２次元環境地図）の一例を示す図である。図５は、第２の位置推定部における処理を示す図であり、図５（ａ）及び図５（ｂ）は一連の処理の流れを示している。図６は、補正処理部における設定条件の判定処理を示す図である。図７は、補正処理部における処理を説明する図であり、図７（ａ）及び図７（ｂ）は一連の処理の流れを示している。
【0025】
図２に示すように、実施の形態では、位置推定装置１０は、移動体１００に搭載されている。移動体１００は、位置推定装置１０に加えて、カメラ２０と、２次元深度センサ３０と、制御装置４０と、駆動装置５０とを備えている。
【0026】
カメラ２０は、固体撮像素子を備えた撮像装置である。カメラ２０は、移動体１００の周辺を撮影可能となるように取り付けられており、移動体１００の周辺の２次元画像の画像データを出力する。
【0027】
２次元深度センサ３０は、それを中心とする２次元平面上において、対象物までの距離（深度）を計測し、計測した距離を特定する情報を、深度データとして連続して出力する。２次元深度センサ３０の具体例としては、２次元ＬｉＤＡＲが挙げられる。２次元ＬｉＤＡＲは、それを中心とする２次元平面上で、周辺を走査するようにレーザ光を連続して出射し、物体で反射されたレーザ光が入射するまでの時間を計測することによって、２次元平面上に存在する物体までの距離を測定し、深度データを出力する。２次元深度センサ３０が出力した深度データの一例は、図４（ａ）に示す通りである。
【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】
具体的には、現フレームにおけるカメラ２０の３次元座標を推定することは、ＰＮＰ問題を解くことに相当する。よって、第１の位置推定部１１は、例えば、ＤＬＴ法（Direct Linear Transformation method）を用いて、透視投影行列を求め、求めた透視投影行列を回転行列Ｒｔと並進ベクトルｔｔとに分解することによって、カメラ２０の３次元座標を算出する。第１の位置推定部１１は、生成した３次元の環境地図、及び推定した３次元座標の情報を、記憶部１４に格納する。
【0039】
第２の位置推定部１２は、実施の形態では、２次元深度センサ３０から、例えば、図４（ａ）に示す深度データを取得し、取得した深度データを記憶部１４に格納する。そして、第２の位置推定部１２は、取得した深度データを用いて、移動体１００の周辺の２次元の点群データを生成する。
【0040】
また、第２の位置推定部１２は、生成した２次元の点群データと、図４（ｂ）に示す２次元の自律走行用地図とを用いて、移動体１００の２次元座標を推定する。２次元の自律走行用地図は予め作成されている地図であり、２次元環境地図とも呼ばれる。具体的には、第２の位置推定部１２は、図５（ａ）に示すように、２次元の点群データと２次元の自律走行用地図とを対比し、図５（ｂ）に示すように両者をマッチングする。そして、２次元の点群データにおける移動体１００の位置を、２次元の自律走行用地図に当てはめて、移動体１００の２次元座標を推定する。第２の位置推定部１２は、推定した２次元座標の情報を、記憶部１４に格納する。
【0041】
補正処理部１３は、実施の形態では、２次元深度センサ３０が一回に出力した深度データにおけるバラツキを算出し、算出したバラツキが閾値以上であることを設定条件として、設定条件が満たされているかどうか、即ちバラツキが閾値以上であるかどうかを判定する。これは、バラツキが閾値未満である場合は、移動体の周辺が平らな壁等であると考えられ、この場合においては、第２の位置推定部１２による移動体の２次元座標の推定精度が低下している可能性があるからである。一方、バラツキが閾値以上である場合は、移動体の周辺に凹凸が存在し、この場合においては、第２の位置推定部１２による移動体の２次元座標の推定精度が確保される。
【0042】
具体的には、図６に示すように、補正処理部１３は、一回に出力された深度データにおいて始点及び終点を特定し、更に、始点及び終点を通る軸をｘ軸、ｘ軸に垂直な軸をｙ軸に設定する。そして、補正処理部１３は、隣接する２点毎にｄｘ／ｄｙを算出し、更に、算出したｄｘ／ｄｙを積算し、得られた積算値（Σｄｘ／ｄｙ）をバラツキとする。その後、補正処理部１３は、バラツキを表す積算値（Σｄｘ／ｄｙ）が閾値α以上である場合は、設定条件を満たすと判定し、そうでない場合は設定条件を満たさないと判定する。
【0043】
補正処理部１３は、設定条件が満たされていると判定する場合は、移動体１００の２次元座標と移動体１００の３次元座標との差分を求め、求めた差分に基づいて、移動体１００の３次元座標、及び３次元の環境地図を補正する。
【0044】
具体的には、補正処理部１３は、まず、図７（ａ）に示すように、３次元環境地図上での移動体１００の座標を、２次元の自律走行用地図の２次元座標に投影し、２次元での移動体１００の座標（図中「■」）を特定する。そして、補正処理部１３は、移動体１００の２次元座標（図中「×」）と、３次元座標から投影された移動体の座標（図中「■」）との差分を求める。
【0045】
続いて、補正処理部１３は、２次元の自律走行用地図が表す２次元座標において、差分に基づいて、３次元の環境地図を構成する点群それぞれの座標を補正すると共に、移動体１００の３次元座標についても補正する。補正された座標が、現フレームにおける最終的な移動体の座標となる。
【0046】
［装置動作］
次に、実施の形態における位置推定装置１０の動作について図８～図１０を用いて説明する。以下の説明においては、適宜図１～図７を参照する。また、実施の形態では、位置推定装置１０を動作させることによって、位置推定方法が実施される。よって、実施の形態における位置推定方法の説明は、以下の位置推定装置１０の動作説明に代える。
次に、実施の形態における位置推定装置１０の動作について図８～図１０を用いて説明する。以下の説明においては、適宜図１～図７を参照する。また、実施の形態では、位置推定装置１０を動作させることによって、位置推定方法が実施される。よって、実施の形態における位置推定方法の説明は、以下の位置推定装置１０の動作説明に代える。
【0047】
【0048】
図８に示すように、最初に、第２の位置推定部１２は、２次元深度センサ３０から、深度データ（図４（ａ）参照）を取得し、取得した深度データを用いて、移動体１００の周辺の２次元の点群データを生成する（ステップＡ１）。２次元深度センサ３０は、設定間隔をおいて、深度データを出力している。ステップＡ１では、第２の位置推定部１２は、一回に出力した深度データを取得する。
【0049】
次に、第２の位置推定部１２は、図５（ａ）及び図５（ｂ）に示したように、ステップＡ１で生成した２次元の点群データと、予め作成されている２次元の自律走行用地図とを対比し、両者をマッチングする（ステップＡ２）。
【0050】
そして、第２の位置推定部１２は、２次元の点群データにおける移動体１００の位置を、２次元の自律走行用地図に当てはめて、移動体１００の２次元座標を推定する（ステップＡ３）。
【0051】
その後、２次元深度センサ３０が新たに深度データを出力すると、再度ステップＡ１～Ａ３が実行される。つまり、ステップＡ１～Ａ３は、２次元深度センサ３０が深度データを出力する度に実行される。
【0052】
【0053】
図９に示すように、最初に、第１の位置推定部１１は、カメラ２０から出力された画像データを１フレーム取得する（ステップＢ１）。次に、第１の位置推定部１１は、ステップＢ１で取得したフレーム（現フレーム）の画像データから、別のフレームとの間で対応する特徴点を抽出し、対応する特徴点の組を設定する（ステップＢ２）。
【0054】
次に、第１の位置推定部１１は、ステップＢ２で設定した特徴点の組を用いて、各特徴点の３次元座標を算出する（ステップＢ３）。更に、第１の位置推定部１１は、算出した各特徴点の３次元座標を用いて、３次元の環境地図を更新する（ステップＢ４）。なお、３次元の環境地図が未だ生成されていない場合は、第１の位置推定部１１は、算出した各特徴点の３次元座標を用いて、３次元の環境地図を生成する。ステップＢ１からステップＢ４は繰り返し実行され、その度に、３次元の環境地図は拡張する。
【0055】
カメラ２０が新たに画像データを出力すると、再度ステップＢ１～Ｂ４が実行される。つまり、ステップＢ１～Ｂ４は、カメラ２０が画像データを出力する度に実行される。
【0056】
【0057】
最初に、次に、第１の位置推定部１１は、図９に示したステップＢ１で取得した現フレームにおける各特徴点の２次元座標を取得し、更に、現フレーム以前に取得された過去フレームについてのステップＢ３で３次元座標が算出された各特徴点を特定し、取得した各特徴点の２次元座標と、特定した各特徴点の３次元座標とを、１つの組データとして設定する（ステップＣ１）。
【0058】
次に、第１の位置推定部１１は、ステップＣ１で設定した組データを用いて、現フレームにおけるカメラ２０の３次元座標、即ち、移動体１００の３次元の環境地図上での３次元座標を推定する（ステップＣ２）。具体的には、ステップＣ２では、第１の位置推定部１１は、ＤＬＴ法に組データを適用して、透視投影行列を求め、求めた透視投影行列を回転行列Ｒｔと並進ベクトルｔｔとに分解することによって、カメラ２０の３次元座標を算出する。
【0059】
次に、補正処理部１３は、移動体１００の２次元座標についての設定条件が満たされているかどうかを判定する（ステップＣ３）。具体的には、ステップＣ３では、補正処理部１３は、２次元深度センサ３０が一回に出力した深度データにおけるバラツキを算出し、算出したバラツキが閾値以上であるかどうかを判定する。
【0060】
ステップＣ３の判定の結果、設定条件を満たしている場合は、補正処理部１３は、図８に示したステップＡ３で推定された移動体１００の２次元座標と、ステップＣ２で推定された移動体１００の３次元座標との差分を求める（ステップＣ４）。
【0061】
次に、補正処理部１３は、ステップＣ４で求めた差分に基づいて、移動体１００の３次元座標、及び３次元の環境地図を補正する（ステップＣ５）。
【0062】
その後、補正処理部１３は、補正された移動体１００の３次元座標を、制御装置４０に出力する（ステップＣ６）。
【0063】
また、ステップＣ３の判定の結果、設定条件を満たしていない場合は、補正処理部１３は、ステップＣ２で推定された３次元座標を、制御装置４０に出力する（ステップＣ６）。
【0064】
ステップＣ６が実行されると、制御装置４０は、出力されてきた３次元座標に基づいて、移動体１００の進行方向及び移動速度を決定する。また、制御装置４０は、決定した進行方向及び移動速度に応じて駆動装置５０を制御して、予め設定されたルートに沿って移動体１００を移動させる。
【0065】
ステップＣ１～Ｃ６は、移動体１００が移動している間、繰り返し実行される。また、実施の形態では、ステップＣ１～Ｃ６は、ステップＢ１～Ｂ４の実行サイクルから独立して実行される。
【0066】
以上のように実施の形態によれば、画像から推定された移動体１００の３次元座標は、設定条件が満たされる場合は、２次元深度センサ３０からの深度データを用いて補正される。このため、移動体の位置推定における精度は大きく向上する。また、実施の形態では、コストの高い３次元の深度センサ、例えば、３次元のＬｉＤＡＲを用いる必要がないため、これを用いる場合に比べて、コストの上昇が抑制される。
【0067】
［プログラム］
実施の形態におけるプログラムは、コンピュータに、図８に示すステップＡ１～Ａ３、図９に示すステップＢ１～Ｂ４、図１０に示すステップＣ１～Ｃ６を実行させるプログラムであれば良い。このプログラムをコンピュータにインストールし、実行することによって、実施の形態における位置推定装置１０と位置推定方法とを実現することができる。この場合、コンピュータのプロセッサは、第１の位置推定部１１、第２の位置推定部１２、及び補正処理部１３として機能し、処理を行なう。また、コンピュータの記憶装置は、記憶部１４として機能する。
実施の形態におけるプログラムは、コンピュータに、図８に示すステップＡ１～Ａ３、図９に示すステップＢ１～Ｂ４、図１０に示すステップＣ１～Ｃ６を実行させるプログラムであれば良い。このプログラムをコンピュータにインストールし、実行することによって、実施の形態における位置推定装置１０と位置推定方法とを実現することができる。この場合、コンピュータのプロセッサは、第１の位置推定部１１、第２の位置推定部１２、及び補正処理部１３として機能し、処理を行なう。また、コンピュータの記憶装置は、記憶部１４として機能する。
【0068】
また、コンピュータとしては、汎用のＰＣの他に、スマートフォン、タブレット型端末装置が挙げられる。更に、コンピュータは、移動体１００の制御装置４０として機能するコンピュータであっても良い。この場合、位置推定装置１０は、移動体１００の制御装置４０のオペレーティングシステム上に構築されることになる。
【0069】
また、実施の形態におけるプログラムは、複数のコンピュータによって構築されたコンピュータシステムによって実行されても良い。この場合は、例えば、各コンピュータが、それぞれ、第１の位置推定部１１、第２の位置推定部１２、及び補正処理部１３のいずれかとして機能しても良い。
【0070】
［物理構成］
ここで、実施の形態におけるプログラムを実行することによって、位置推定装置１０を実現するコンピュータについて図１１を用いて説明する。図１１は、実施の形態における位置推定装置を実現するコンピュータの一例を示すブロック図である。
ここで、実施の形態におけるプログラムを実行することによって、位置推定装置１０を実現するコンピュータについて図１１を用いて説明する。図１１は、実施の形態における位置推定装置を実現するコンピュータの一例を示すブロック図である。
【0071】
図１０に示すように、コンピュータ１１０は、ＣＰＵ（Central Processing Unit）１１１と、メインメモリ１１２と、記憶装置１１３と、入力インターフェイス１１４と、表示コントローラ１１５と、データリーダ／ライタ１１６と、通信インターフェイス１１７とを備える。これらの各部は、バス１２１を介して、互いにデータ通信可能に接続される。
【0072】
また、コンピュータ１１０は、ＣＰＵ１１１に加えて、又はＣＰＵ１１１に代えて、ＧＰＵ（Graphics Processing Unit）、又はＦＰＧＡ（Field-Programmable Gate Array）を備えていても良い。この態様では、ＧＰＵ又はＦＰＧＡが、実施の形態におけるプログラムを実行することができる。
【0073】
ＣＰＵ１１１は、記憶装置１１３に格納された、コード群で構成された実施の形態におけるプログラムをメインメモリ１１２に展開し、各コードを所定順序で実行することにより、各種の演算を実施する。メインメモリ１１２は、典型的には、ＤＲＡＭ（Dynamic Random Access Memory）等の揮発性の記憶装置である。
【0074】
また、実施の形態におけるプログラムは、コンピュータ読み取り可能な記録媒体１２０に格納された状態で提供される。なお、本実施の形態におけるプログラムは、通信インターフェイス１１７を介して接続されたインターネット上で流通するものであっても良い。
【0075】
また、記憶装置１１３の具体例としては、ハードディスクドライブの他、フラッシュメモリ等の半導体記憶装置が挙げられる。入力インターフェイス１１４は、ＣＰＵ１１１と、キーボード及びマウスといった入力機器１１８との間のデータ伝送を仲介する。表示コントローラ１１５は、ディスプレイ装置１１９と接続され、ディスプレイ装置１１９での表示を制御する。
【0076】
データリーダ／ライタ１１６は、ＣＰＵ１１１と記録媒体１２０との間のデータ伝送を仲介し、記録媒体１２０からのプログラムの読み出し、及びコンピュータ１１０における処理結果の記録媒体１２０への書き込みを実行する。通信インターフェイス１１７は、ＣＰＵ１１１と、他のコンピュータとの間のデータ伝送を仲介する。
【0077】
また、記録媒体１２０の具体例としては、ＣＦ（Compact Flash（登録商標））及びＳＤ（Secure Digital）等の汎用的な半導体記憶デバイス、フレキシブルディスク（Flexible Disk）等の磁気記録媒体、又はＣＤ－ＲＯＭ（Compact Disk Read Only Memory）などの光学記録媒体が挙げられる。
【0078】
なお、本実施の形態における位置推定装置１０は、プログラムがインストールされたコンピュータではなく、各部に対応したハードウェアを用いることによっても実現可能である。更に、位置推定装置１０は、一部がプログラムで実現され、残りの部分がハードウェアで実現されていてもよい。ハードウェアとしては、電子回路が挙げられる。
【0079】
上述した実施の形態の一部又は全部は、以下に記載する（付記１）～（付記１５）によって表現することができるが、以下の記載に限定されるものではない。
【0080】
（付記１）
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定部と、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定部と、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理部と、
を備えている、
ことを特徴とする位置推定装置。
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定部と、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定部と、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理部と、
を備えている、
ことを特徴とする位置推定装置。
【0081】
（付記２）
付記１に記載の位置推定装置であって、
前記第１の位置推定部が、対応する特徴点の組を用いて、前記特徴点の３次元座標を算出し、前記３次元座標が算出された前記特徴点を用いて、点群データで構成された３次元の環境地図を生成又は更新する、
ことを特徴とする位置推定装置。
付記１に記載の位置推定装置であって、
前記第１の位置推定部が、対応する特徴点の組を用いて、前記特徴点の３次元座標を算出し、前記３次元座標が算出された前記特徴点を用いて、点群データで構成された３次元の環境地図を生成又は更新する、
ことを特徴とする位置推定装置。
【0082】
（付記３）
付記１または２に記載の位置推定装置であって、
前記補正処理部が、前記移動体の２次元座標と前記移動体の３次元座標との差分を求め、求めた差分に基づいて、前記移動体の３次元座標、及び前記３次元の環境地図を補正する、
ことを特徴とする位置推定装置。
付記１または２に記載の位置推定装置であって、
前記補正処理部が、前記移動体の２次元座標と前記移動体の３次元座標との差分を求め、求めた差分に基づいて、前記移動体の３次元座標、及び前記３次元の環境地図を補正する、
ことを特徴とする位置推定装置。
【0083】
（付記４）
付記１～３のいずれかに記載の位置推定装置であって、
前記第２の位置推定部が、取得した前記深度データを用いて、前記移動体の周辺の２次元の点群データを生成し、生成した前記２次元の点群データと、予め作成されている２次元の自律走行用地図とを用いて、前記移動体の２次元座標を推定する、
ことを特徴とする位置推定装置。
付記１～３のいずれかに記載の位置推定装置であって、
前記第２の位置推定部が、取得した前記深度データを用いて、前記移動体の周辺の２次元の点群データを生成し、生成した前記２次元の点群データと、予め作成されている２次元の自律走行用地図とを用いて、前記移動体の２次元座標を推定する、
ことを特徴とする位置推定装置。
【0084】
（付記５）
付記１～４のいずれかに記載の位置推定装置であって、
前記補正処理部が、前記２次元深度センサが一回に出力した深度データにおけるバラツキを算出し、算出したバラツキが閾値以上となる場合に、前記設定条件が満たされていると判定する、
ことを特徴とする位置推定装置。
付記１～４のいずれかに記載の位置推定装置であって、
前記補正処理部が、前記２次元深度センサが一回に出力した深度データにおけるバラツキを算出し、算出したバラツキが閾値以上となる場合に、前記設定条件が満たされていると判定する、
ことを特徴とする位置推定装置。
【0085】
（付記６）
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定ステップと、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を有する、
ことを特徴とする位置推定方法。
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定ステップと、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を有する、
ことを特徴とする位置推定方法。
【0086】
（付記７）
付記６に記載の位置推定方法であって、
前記第１の位置推定ステップにおいて、対応する特徴点の組を用いて、前記特徴点の３次元座標を算出し、前記３次元座標が算出された前記特徴点を用いて、点群データで構成された３次元の環境地図を生成又は更新する、
ことを特徴とする位置推定方法。
付記６に記載の位置推定方法であって、
前記第１の位置推定ステップにおいて、対応する特徴点の組を用いて、前記特徴点の３次元座標を算出し、前記３次元座標が算出された前記特徴点を用いて、点群データで構成された３次元の環境地図を生成又は更新する、
ことを特徴とする位置推定方法。
【0087】
（付記８）
付記６または７に記載の位置推定方法であって、
前記補正処理ステップにおいて、前記移動体の２次元座標と前記移動体の３次元座標との差分を求め、求めた差分に基づいて、前記移動体の３次元座標、及び前記３次元の環境地図を補正する、
ことを特徴とする位置推定方法。
付記６または７に記載の位置推定方法であって、
前記補正処理ステップにおいて、前記移動体の２次元座標と前記移動体の３次元座標との差分を求め、求めた差分に基づいて、前記移動体の３次元座標、及び前記３次元の環境地図を補正する、
ことを特徴とする位置推定方法。
【0088】
（付記９）
付記６～８のいずれかに記載の位置推定方法であって、
前記第２の位置推定ステップにおいて、取得した前記深度データを用いて、前記移動体の周辺の２次元の点群データを生成し、生成した前記２次元の点群データと、予め作成されている２次元の自律走行用地図とを用いて、前記移動体の２次元座標を推定する、
ことを特徴とする位置推定方法。
付記６～８のいずれかに記載の位置推定方法であって、
前記第２の位置推定ステップにおいて、取得した前記深度データを用いて、前記移動体の周辺の２次元の点群データを生成し、生成した前記２次元の点群データと、予め作成されている２次元の自律走行用地図とを用いて、前記移動体の２次元座標を推定する、
ことを特徴とする位置推定方法。
【0089】
（付記１０）
付記６～９のいずれかに記載の位置推定方法であって、
前記補正処理ステップにおいて、前記２次元深度センサが一回に出力した深度データにおけるバラツキを算出し、算出したバラツキが閾値以上となる場合に、前記設定条件が満たされていると判定する、
ことを特徴とする位置推定方法。
付記６～９のいずれかに記載の位置推定方法であって、
前記補正処理ステップにおいて、前記２次元深度センサが一回に出力した深度データにおけるバラツキを算出し、算出したバラツキが閾値以上となる場合に、前記設定条件が満たされていると判定する、
ことを特徴とする位置推定方法。
【0090】
（付記１１）
コンピュータに、
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定ステップと、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を実行させる、プログラム。
コンピュータに、
移動体に搭載されたカメラから出力される画像データをフレーム単位で取得し、フレーム毎に、当該フレームの画像データから、別のフレームとの間で対応する特徴点を抽出し、抽出した特徴点に基づいて、当該フレームにおける前記移動体の３次元座標を推定する、第１の位置推定ステップと、
前記移動体に搭載された２次元深度センサから連続して出力される深度データを取得し、取得した前記深度データを用いて、前記移動体の２次元座標を推定する、第２の位置推定ステップと、
推定された前記移動体の２次元座標についての設定条件が満たされているかどうかを判定し、前記設定条件が満たされている場合に、前記移動体の２次元座標を用いて、前記移動体の３次元座標を補正する、補正処理ステップと、
を実行させる、プログラム。
【0091】
（付記１２）
付記１１に記載のプログラムであって、
前記第１の位置推定ステップにおいて、対応する特徴点の組を用いて、前記特徴点の３次元座標を算出し、前記３次元座標が算出された前記特徴点を用いて、点群データで構成された３次元の環境地図を生成又は更新する、
ことを特徴とするプログラム。
付記１１に記載のプログラムであって、
前記第１の位置推定ステップにおいて、対応する特徴点の組を用いて、前記特徴点の３次元座標を算出し、前記３次元座標が算出された前記特徴点を用いて、点群データで構成された３次元の環境地図を生成又は更新する、
ことを特徴とするプログラム。
【0092】
（付記１３）
付記１１または１２に記載のプログラムであって、
前記補正処理ステップにおいて、前記移動体の２次元座標と前記移動体の３次元座標との差分を求め、求めた差分に基づいて、前記移動体の３次元座標、及び前記３次元の環境地図を補正する、
ことを特徴とするプログラム。
付記１１または１２に記載のプログラムであって、
前記補正処理ステップにおいて、前記移動体の２次元座標と前記移動体の３次元座標との差分を求め、求めた差分に基づいて、前記移動体の３次元座標、及び前記３次元の環境地図を補正する、
ことを特徴とするプログラム。
【0093】
（付記１４）
付記１１～１３のいずれかに記載のプログラムであって、
前記第２の位置推定ステップにおいて、取得した前記深度データを用いて、前記移動体の周辺の２次元の点群データを生成し、生成した前記２次元の点群データと、予め作成されている２次元の自律走行用地図とを用いて、前記移動体の２次元座標を推定する、
ことを特徴とするプログラム。
付記１１～１３のいずれかに記載のプログラムであって、
前記第２の位置推定ステップにおいて、取得した前記深度データを用いて、前記移動体の周辺の２次元の点群データを生成し、生成した前記２次元の点群データと、予め作成されている２次元の自律走行用地図とを用いて、前記移動体の２次元座標を推定する、
ことを特徴とするプログラム。
【0094】
（付記１５）
付記１１～１４のいずれかに記載のプログラムであって、
前記補正処理ステップにおいて、前記２次元深度センサが一回に出力した深度データにおけるバラツキを算出し、算出したバラツキが閾値以上となる場合に、前記設定条件が満たされていると判定する、
ことを特徴とするプログラム。
付記１１～１４のいずれかに記載のプログラムであって、
前記補正処理ステップにおいて、前記２次元深度センサが一回に出力した深度データにおけるバラツキを算出し、算出したバラツキが閾値以上となる場合に、前記設定条件が満たされていると判定する、
ことを特徴とするプログラム。
【産業上の利用可能性】
【0095】
以上のように本開示によれば、コストの上昇を抑制しつつ、自己位置推定の精度を向上させることができる。本発明は、移動体の自律走行システムに有用である。
【符号の説明】
【0096】
１０ 位置推定装置
１１ 第１の位置推定部
１２ 第２の位置推定部
１３ 補正処理部
１４ 記憶部
２０ カメラ
３０ ２次元深度センサ
４０ 制御装置
５０ 駆動装置
１００ 移動体
１１０ コンピュータ
１１１ ＣＰＵ
１１２ メインメモリ
１１３ 記憶装置
１１４ 入力インターフェイス
１１５ 表示コントローラ
１１６ データリーダ／ライタ
１１７ 通信インターフェイス
１１８ 入力機器
１１９ ディスプレイ装置
１２０ 記録媒体
１２１ バス
１１ 第１の位置推定部
１２ 第２の位置推定部
１３ 補正処理部
１４ 記憶部
２０ カメラ
３０ ２次元深度センサ
４０ 制御装置
５０ 駆動装置
１００ 移動体
１１０ コンピュータ
１１１ ＣＰＵ
１１２ メインメモリ
１１３ 記憶装置
１１４ 入力インターフェイス
１１５ 表示コントローラ
１１６ データリーダ／ライタ
１１７ 通信インターフェイス
１１８ 入力機器
１１９ ディスプレイ装置
１２０ 記録媒体
１２１ バス
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】