ホーム > 特許ランキング > 株式会社豊田自動織機
知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024064664
(43)【公開日】2024-05-14
(54)【発明の名称】自己位置推定装置
(51)【国際特許分類】
G01C 21/28 20060101AFI20240507BHJP
G05D 1/49 20240101ALI20240507BHJP
【ＦＩ】
G01C21/28
G05D1/08 Z
【審査請求】未請求
【請求項の数】2
【出願形態】ＯＬ
(21)【出願番号】P 2022173422
(22)【出願日】2022-10-28
(71)【出願人】
【識別番号】000003218
【氏名又は名称】株式会社豊田自動織機
(74)【代理人】
【識別番号】100105957
【弁理士】
【氏名又は名称】恩田 誠
(74)【代理人】
【識別番号】100068755
【弁理士】
【氏名又は名称】恩田 博宣
(72)【発明者】
【氏名】坂元 優太
【テーマコード（参考）】
2F129
5H301
【Ｆターム（参考）】
2F129AA03
2F129BB02
2F129BB33
2F129GG17
2F129GG18
5H301AA03
5H301AA06
5H301BB01
5H301BB07
5H301BB14
5H301CC03
5H301CC04
5H301CC06
5H301CC07
5H301CC08
5H301GG08
5H301GG09
(57)【要約】
【課題】初期位置の精度を向上させること。
【解決手段】制御装置は、移動体の自己位置を表すグローバル座標系での三次元座標のうち水平方向の座標を表す水平方向座標を取得する。制御装置は、地図データ上で、水平方向座標を中心とする立体柱であってＺ軸に延びる立体柱を生成する。制御装置は、Ｚ座標の変化と、立体柱に収容される点の密度であって水平面に分布する点の密度の変化との対応関係を算出する。制御装置は、点の密度が極大値となるＺ座標に基づいて、自己位置を表すグローバル座標系での三次元座標のうち鉛直方向の座標を表す鉛直方向座標を算出する。制御装置は、水平方向座標と鉛直方向座標で表されるグローバル座標系での三次元座標を初期位置として設定する。
【選択図】図４
【解決手段】制御装置は、移動体の自己位置を表すグローバル座標系での三次元座標のうち水平方向の座標を表す水平方向座標を取得する。制御装置は、地図データ上で、水平方向座標を中心とする立体柱であってＺ軸に延びる立体柱を生成する。制御装置は、Ｚ座標の変化と、立体柱に収容される点の密度であって水平面に分布する点の密度の変化との対応関係を算出する。制御装置は、点の密度が極大値となるＺ座標に基づいて、自己位置を表すグローバル座標系での三次元座標のうち鉛直方向の座標を表す鉛直方向座標を算出する。制御装置は、水平方向座標と鉛直方向座標で表されるグローバル座標系での三次元座標を初期位置として設定する。
【選択図】図４
【特許請求の範囲】
【請求項1】
移動体に搭載される自己位置推定装置であって、
グローバル座標系での三次元座標を表す点によって環境地図を表した地図データを記憶する記憶装置と、
前記移動体の周辺環境をローカル座標系での三次元座標として検出するセンサと、
制御装置と、を備え、
前記制御装置は、
前記移動体の自己位置を表す前記グローバル座標系での三次元座標のうち水平方向の座標を表す水平方向座標を取得し、
前記グローバル座標系の座標軸のうち上下方向を表す座標軸をＺ軸とすると、前記地図データ上で、前記水平方向座標を中心とする立体柱であって前記Ｚ軸に延びる立体柱を生成し、
前記Ｚ軸の座標であるＺ座標の変化と、前記立体柱に収容される前記点の密度であって水平面に分布する前記点の密度の変化との対応関係を算出し、
前記点の密度が極大値となる前記Ｚ座標に基づいて、前記自己位置を表す前記グローバル座標系での三次元座標のうち鉛直方向の座標を表す鉛直方向座標を算出し、
前記水平方向座標と前記鉛直方向座標で表される前記グローバル座標系での三次元座標を初期位置として設定してスキャンマッチングを開始する、自己位置推定装置。
グローバル座標系での三次元座標を表す点によって環境地図を表した地図データを記憶する記憶装置と、
前記移動体の周辺環境をローカル座標系での三次元座標として検出するセンサと、
制御装置と、を備え、
前記制御装置は、
前記移動体の自己位置を表す前記グローバル座標系での三次元座標のうち水平方向の座標を表す水平方向座標を取得し、
前記グローバル座標系の座標軸のうち上下方向を表す座標軸をＺ軸とすると、前記地図データ上で、前記水平方向座標を中心とする立体柱であって前記Ｚ軸に延びる立体柱を生成し、
前記Ｚ軸の座標であるＺ座標の変化と、前記立体柱に収容される前記点の密度であって水平面に分布する前記点の密度の変化との対応関係を算出し、
前記点の密度が極大値となる前記Ｚ座標に基づいて、前記自己位置を表す前記グローバル座標系での三次元座標のうち鉛直方向の座標を表す鉛直方向座標を算出し、
前記水平方向座標と前記鉛直方向座標で表される前記グローバル座標系での三次元座標を初期位置として設定してスキャンマッチングを開始する、自己位置推定装置。
【請求項2】
前記制御装置は、
前記点の密度が前記極大値となる前記Ｚ座標が複数存在する場合、前記点の密度が前記極大値となる前記Ｚ座標のそれぞれに基づいて算出された前記鉛直方向座標毎に信頼度を算出し、
前記水平方向座標と最も前記信頼度が高い前記鉛直方向座標を、前記初期位置として設定する、請求項１に記載の自己位置推定装置。
前記点の密度が前記極大値となる前記Ｚ座標が複数存在する場合、前記点の密度が前記極大値となる前記Ｚ座標のそれぞれに基づいて算出された前記鉛直方向座標毎に信頼度を算出し、
前記水平方向座標と最も前記信頼度が高い前記鉛直方向座標を、前記初期位置として設定する、請求項１に記載の自己位置推定装置。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、自己位置推定装置に関する。
【背景技術】
【0002】
特許文献１に開示の自己位置推定方法は、移動体の自己位置を推定する。自己位置推定方法は、センサから得られたローカル座標系での点と、地図データによって表されるグローバル座標系での点とのマッチングを行うことで、グローバル座標系での自己位置を推定する。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開２０２２－４２６３０号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
グローバル座標系での全域に対してマッチングを行うと処理負荷が大きいため、マッチングを開始する前には、グローバル座標系での初期位置を設定する必要がある。この際、初期位置のうちグローバル座標系での上下方向の座標であるＺ座標の精度が低くなる場合がある。例えば、初期位置は、GPS(Global Positioning System)を用いて設定されるが、GPSを用いる場合にはＺ座標のばらつきが大きくなりやすい。
【課題を解決するための手段】
【0005】
上記課題を解決する自己位置推定装置は、移動体に搭載される自己位置推定装置であって、グローバル座標系での三次元座標を表す点によって環境地図を表した地図データを記憶する記憶装置と、前記移動体の周辺環境をローカル座標系での三次元座標として検出するセンサと、制御装置と、を備え、前記制御装置は、前記移動体の自己位置を表す前記グローバル座標系での三次元座標のうち水平方向の座標を表す水平方向座標を取得し、前記グローバル座標系の座標軸のうち上下方向を表す座標軸をＺ軸とすると、前記地図データ上で、前記水平方向座標を中心とする立体柱であって前記Ｚ軸に延びる立体柱を生成し、前記Ｚ軸の座標であるＺ座標の変化と、前記立体柱に収容される前記点の密度であって水平面に分布する前記点の密度の変化との対応関係を算出し、前記点の密度が極大値となる前記Ｚ座標に基づいて、前記自己位置を表す前記グローバル座標系での三次元座標のうち鉛直方向の座標を表す鉛直方向座標を算出し、前記水平方向座標と前記鉛直方向座標で表される前記グローバル座標系での三次元座標を初期位置として設定してスキャンマッチングを開始する。
【0006】
Ｚ座標の変化と、立体柱に収容される点の密度の変化を算出すると、立体柱に収容される点の密度には極大値が生じる。地図データ上では、路面に対応するＺ座標には、点が密に存在する一方で、路面に対応するＺ座標より下方向には点が存在しない。このため、Ｚ座標を変化させると、路面に対応するＺ座標を境にして立体柱に収容される点の密度に極大値が生じることになる。制御装置は、点の密度が極大値となるＺ座標に基づいて鉛直方向座標を算出する。これにより、初期位置の精度を向上させることができる。
【0007】
上記自己位置推定装置について、前記制御装置は、前記点の密度が前記極大値となる前記Ｚ座標が複数存在する場合、前記点の密度が前記極大値となる前記Ｚ座標のそれぞれに基づいて算出された前記鉛直方向座標毎に信頼度を算出し、前記水平方向座標と最も前記信頼度が高い前記鉛直方向座標を、前記初期位置として設定してもよい。
【発明の効果】
【0008】
本発明によれば、初期位置の精度を向上させることができる。
【図面の簡単な説明】
【0009】
【発明を実施するための形態】
【0010】
自己位置推定装置の一実施形態について説明する。
＜環境＞
図１に示すように、移動体Ｖ１が用いられる環境Ａ１には種々の構造物Ｏ１，Ｏ２，Ｏ３が含まれる。構造物Ｏ１，Ｏ２，Ｏ３は、例えば、建物を含む。詳細にいえば、構造物Ｏ１，Ｏ２，Ｏ３は、壁、柱、及び屋根など、建物を構成する部材を含む。環境Ａ１は、路面Ｒ１を含む。移動体Ｖ１は、例えば、車両、飛行体、及び多足歩行ロボットを含む。車両は、例えば、乗用車、及び産業車両を含む。産業車両は、例えば、フォークリフト、トーイングトラクタ、及び搬送車を含む。
＜環境＞
図１に示すように、移動体Ｖ１が用いられる環境Ａ１には種々の構造物Ｏ１，Ｏ２，Ｏ３が含まれる。構造物Ｏ１，Ｏ２，Ｏ３は、例えば、建物を含む。詳細にいえば、構造物Ｏ１，Ｏ２，Ｏ３は、壁、柱、及び屋根など、建物を構成する部材を含む。環境Ａ１は、路面Ｒ１を含む。移動体Ｖ１は、例えば、車両、飛行体、及び多足歩行ロボットを含む。車両は、例えば、乗用車、及び産業車両を含む。産業車両は、例えば、フォークリフト、トーイングトラクタ、及び搬送車を含む。
【0011】
＜自己位置推定装置＞
図２に示すように、移動体Ｖ１には、自己位置推定装置１００が搭載されている。
自己位置推定装置１００は、制御装置１０を備える。制御装置１０は、プロセッサ１１と、記憶部１２と、を備える。プロセッサ１１としては、例えば、CPU（Central Processing Unit）、GPU（Graphics Processing Unit）、又はDSP（Digital Signal Processor）が用いられる。記憶部１２は、RAM（Random Access Memory）及びROM（Read Only Memory）を含む。記憶部１２には、移動体Ｖ１を動作させるためのプログラムが記憶されている。記憶部１２は、処理をプロセッサ１１に実行させるように構成されたプログラムコードまたは指令を格納しているといえる。記憶部１２、すなわち、コンピュータ可読媒体は、汎用または専用のコンピュータでアクセスできるあらゆる利用可能な媒体を含む。制御装置１０は、ASIC（Application Specific Integrated Circuit）やFPGA（Field Programmable Gate Array）等のハードウェア回路によって構成されていてもよい。処理回路である制御装置１０は、コンピュータプログラムに従って動作する１つ以上のプロセッサ、ASICやFPGA等の１つ以上のハードウェア回路、或いは、それらの組み合わせを含み得る。
図２に示すように、移動体Ｖ１には、自己位置推定装置１００が搭載されている。
自己位置推定装置１００は、制御装置１０を備える。制御装置１０は、プロセッサ１１と、記憶部１２と、を備える。プロセッサ１１としては、例えば、CPU（Central Processing Unit）、GPU（Graphics Processing Unit）、又はDSP（Digital Signal Processor）が用いられる。記憶部１２は、RAM（Random Access Memory）及びROM（Read Only Memory）を含む。記憶部１２には、移動体Ｖ１を動作させるためのプログラムが記憶されている。記憶部１２は、処理をプロセッサ１１に実行させるように構成されたプログラムコードまたは指令を格納しているといえる。記憶部１２、すなわち、コンピュータ可読媒体は、汎用または専用のコンピュータでアクセスできるあらゆる利用可能な媒体を含む。制御装置１０は、ASIC（Application Specific Integrated Circuit）やFPGA（Field Programmable Gate Array）等のハードウェア回路によって構成されていてもよい。処理回路である制御装置１０は、コンピュータプログラムに従って動作する１つ以上のプロセッサ、ASICやFPGA等の１つ以上のハードウェア回路、或いは、それらの組み合わせを含み得る。
【0012】
移動体Ｖ１は、補助記憶装置１３を備える。補助記憶装置１３は、データを書き換え可能な不揮発性記憶装置である。補助記憶装置１３は、例えば、ハードディスクドライブ、又はソリッドステートドライブである。
【0013】
補助記憶装置１３は、地図データＤ１を記憶している。補助記憶装置１３は、地図データＤ１を記憶する記憶装置である。地図データＤ１は、グローバル座標系での三次元座標によって環境地図を表している。環境地図は、環境Ａ１に含まれる構造物Ｏ１，Ｏ２，Ｏ３、及び路面Ｒ１を表した地図である。グローバル座標系は、原点の位置が固定である絶対座標系である。
【0014】
図３に示すように、グローバル座標系での三次元座標は、グローバル座標系での点Ｐ１，Ｐ２によって表される。この点Ｐ１，Ｐ２は、構造物Ｏ１，Ｏ２，Ｏ３、及び路面Ｒ１の表面を表す点Ｐ１，Ｐ２である。地図データＤ１は、例えば、三次元座標を取得することができる装置によって環境Ａ１の三次元座標を取得した上で、当該三次元座標をグローバル座標系での三次元座標に変換することで作成できる。三次元座標を取得することができる装置としては、例えば、LIDAR（Laser Imaging Detection and Ranging）、ステレオカメラ、及びミリ波レーダーを挙げることができる。
【0015】
図３に示す例では、説明の便宜上、路面Ｒ１を表す点Ｐ１と、構造物Ｏ１，Ｏ２，Ｏ３を表す点Ｐ２とを分類して示している。グローバル座標系は、Ｘ軸と、Ｙ軸と、Ｚ軸と、を備える。Ｘ軸及びＹ軸は、水平方向を表す座標軸である。Ｚ軸は、上下方向を表す座標軸である。上下方向は、水平方向に直交する鉛直方向である。点Ｐ１，Ｐ２は、グローバル座標系での三次元座標（Ｘ，Ｙ，Ｚ）を表す。Ｘは任意のＸ座標、Ｙは任意のＹ座標、Ｚは任意のＺ座標である。Ｘ座標はＸ軸の座標、Ｙ座標はＹ軸の座標、Ｚ座標はＺ軸の座標である。
【0016】
図２に示すように、自己位置推定装置１００は、センサ２１を備える。センサ２１は、移動体Ｖ１の周辺環境をローカル座標系での三次元座標として検出する。ローカル座標系での三次元座標は、ローカル座標系での点として表される。この点は、構造物Ｏ１，Ｏ２，Ｏ３、及び路面Ｒ１の表面を表す点である。周辺環境は、例えば、構造物Ｏ１，Ｏ２，Ｏ３や路面Ｒ１などの物体を含む。センサ２１は、例えば、LIDAR、ステレオカメラ、又はミリ波レーダーである。ローカル座標系は、移動体Ｖ１とともに移動する相対座標系である。例えば、ローカル座標系は、センサ２１を原点とする座標系である。ローカル座標系は、三軸直交座標系である。
【0017】
自己位置推定装置１００は、衛星航法装置２２を備える。衛星航法装置２２は、GNSS（Global Navigation Satellite System）衛星から送信される衛星信号を受信する。GNSSは、例えば、GPS（Global Positioning System）、IRNSS（Indian Regional Navigational Satellite System）、又はQZSS（Quasi-Zenith Satellite System）である。本実施形態のGNSSは、GPSである。衛星航法装置２２は、衛星信号を用いてグローバル座標系での三次元座標を測定する。
【0018】
制御装置１０は、自己位置推定を行う。自己位置とは、グローバル座標系での移動体Ｖ１の座標である。自己位置は、移動体Ｖ１の一点を示す座標である。移動体Ｖ１の一点としては、移動体Ｖ１の中心位置など、任意の位置に設定することができる。以下、制御装置１０が行う自己位置推定について説明する。自己位置推定は、例えば、移動体Ｖ１が自律移動する際に行われる。
【0019】
＜自己位置推定＞
図４に示すように、ステップＳ１において、制御装置１０は、衛星航法装置２２からグローバル座標系での三次元座標を取得する。ステップＳ１で取得した三次元座標（Ｘ，Ｙ，Ｚ）を三次元座標（α，β，γ）とする。Ｘ座標及びＹ座標は、三次元座標のうち水平方向の座標を表す水平方向座標（α，β）である。制御装置１０は、ステップＳ１で水平方向座標を取得している。
図４に示すように、ステップＳ１において、制御装置１０は、衛星航法装置２２からグローバル座標系での三次元座標を取得する。ステップＳ１で取得した三次元座標（Ｘ，Ｙ，Ｚ）を三次元座標（α，β，γ）とする。Ｘ座標及びＹ座標は、三次元座標のうち水平方向の座標を表す水平方向座標（α，β）である。制御装置１０は、ステップＳ１で水平方向座標を取得している。
【0020】
次に、ステップＳ２において、制御装置１０は、地図データＤ１上で、水平面であるＸＹ平面に対して立体柱を生成する。
図３に示すように、立体柱Ｃ１は、水平方向座標を中心とする立体柱Ｃ１であってＺ軸に延びる。本実施形態において、立体柱Ｃ１は、水平方向座標（α，β）を中心とする立体柱である。立体柱Ｃ１は、Ｚ軸の＋側、及び－側に延びている。Ｚ軸の＋側は原点よりも上方向である。Ｚ軸の－側は原点よりも下方向である。立体柱Ｃ１のＺ軸方向の寸法は、例えば、原点から下方向に立体柱Ｃ１が第１所定距離延び、かつ、原点から上方向に立体柱Ｃ１が第２所定距離延びるように設定されている。第１所定距離は、例えば、路面Ｒ１よりも下方向の範囲が立体柱Ｃ１に含まれるように設定されている。第２所定距離は、例えば、路面Ｒ１上の構造物Ｏ１，Ｏ２，Ｏ３が立体柱Ｃ１に含まれるように設定されている。立体柱Ｃ１は、ＸＹ平面に対する断面積が一様な柱である。立体柱Ｃ１のＸＹ平面に対する断面積は、水平方向に拡がる面を備える構造物Ｏ１，Ｏ２，Ｏ３が立体柱Ｃ１に収容されている場合に、当該水平方向に拡がる面の面積よりも立体柱Ｃ１のＸＹ平面に対する断面積が大きくなるように設定されている。即ち、立体柱Ｃ１のＸＹ平面に対する断面積は、立体柱Ｃ１内で水平方向に拡がる面のうち路面Ｒ１の面積が最も大きくなるように設定されている。立体柱Ｃ１は、例えば、円柱、四角柱、又は六角柱である。
図３に示すように、立体柱Ｃ１は、水平方向座標を中心とする立体柱Ｃ１であってＺ軸に延びる。本実施形態において、立体柱Ｃ１は、水平方向座標（α，β）を中心とする立体柱である。立体柱Ｃ１は、Ｚ軸の＋側、及び－側に延びている。Ｚ軸の＋側は原点よりも上方向である。Ｚ軸の－側は原点よりも下方向である。立体柱Ｃ１のＺ軸方向の寸法は、例えば、原点から下方向に立体柱Ｃ１が第１所定距離延び、かつ、原点から上方向に立体柱Ｃ１が第２所定距離延びるように設定されている。第１所定距離は、例えば、路面Ｒ１よりも下方向の範囲が立体柱Ｃ１に含まれるように設定されている。第２所定距離は、例えば、路面Ｒ１上の構造物Ｏ１，Ｏ２，Ｏ３が立体柱Ｃ１に含まれるように設定されている。立体柱Ｃ１は、ＸＹ平面に対する断面積が一様な柱である。立体柱Ｃ１のＸＹ平面に対する断面積は、水平方向に拡がる面を備える構造物Ｏ１，Ｏ２，Ｏ３が立体柱Ｃ１に収容されている場合に、当該水平方向に拡がる面の面積よりも立体柱Ｃ１のＸＹ平面に対する断面積が大きくなるように設定されている。即ち、立体柱Ｃ１のＸＹ平面に対する断面積は、立体柱Ｃ１内で水平方向に拡がる面のうち路面Ｒ１の面積が最も大きくなるように設定されている。立体柱Ｃ１は、例えば、円柱、四角柱、又は六角柱である。
【0021】
図４に示すように、次に、ステップＳ３において、制御装置１０は、地図データＤ１上で立体柱Ｃ１に収容される点Ｐ１，Ｐ２を抽出する。
次に、ステップＳ４において、制御装置１０は、Ｚ座標の変化と、立体柱Ｃ１に収容される点Ｐ１，Ｐ２の密度であってＸＹ平面に分布する点Ｐ１，Ｐ２の密度の変化との対応関係を算出する。即ち、制御装置１０は、Ｚ座標の変化に対する点Ｐ１，Ｐ２の密度の変化を算出する。Ｚ座標の変化に対する点Ｐ１，Ｐ２の密度の変化は、Ｚ座標を連続的に変化させた場合の点Ｐ１，Ｐ２の密度の変化であってもよい。Ｚ座標の変化に対する点Ｐ１，Ｐ２の密度の変化は、Ｚ座標を一定範囲で変化させた場合に、当該一定範囲に含まれる点Ｐ１，Ｐ２の密度の変化であってもよい。
次に、ステップＳ４において、制御装置１０は、Ｚ座標の変化と、立体柱Ｃ１に収容される点Ｐ１，Ｐ２の密度であってＸＹ平面に分布する点Ｐ１，Ｐ２の密度の変化との対応関係を算出する。即ち、制御装置１０は、Ｚ座標の変化に対する点Ｐ１，Ｐ２の密度の変化を算出する。Ｚ座標の変化に対する点Ｐ１，Ｐ２の密度の変化は、Ｚ座標を連続的に変化させた場合の点Ｐ１，Ｐ２の密度の変化であってもよい。Ｚ座標の変化に対する点Ｐ１，Ｐ２の密度の変化は、Ｚ座標を一定範囲で変化させた場合に、当該一定範囲に含まれる点Ｐ１，Ｐ２の密度の変化であってもよい。
【0022】
次に、ステップＳ５において、制御装置１０は、鉛直方向座標を算出する。鉛直方向座標は、自己位置を表すグローバル座標系での三次元座標のうち鉛直方向の座標を表す座標である。
【0023】
図５に示すように、Ｚ座標の変化に対する点Ｐ１，Ｐ２の密度の変化を算出すると、点Ｐ１，Ｐ２の密度には極大値が生じる。地図データＤ１において、路面Ｒ１に対応するＺ座標には、点Ｐ１が密に存在する一方で、路面Ｒ１に対応するＺ座標より下方向には点Ｐ１，Ｐ２が存在しない。このため、Ｚ座標を変化させると、路面Ｒ１に対応するＺ座標を境にして点Ｐ１，Ｐ２の密度に極大値が生じることになる。制御装置１０は、点Ｐ１，Ｐ２の密度が極大値となるＺ座標を路面Ｒ１のＺ座標であると判定する。制御装置１０は、点Ｐ１，Ｐ２の密度が極大値となるＺ座標を探索する際に、Ｚ座標の－側から＋側に向けて探索を行ってもよい。この場合、点Ｐ１，Ｐ２が存在しないＺ座標から路面Ｒ１に対応するＺ座標を境にして点Ｐ１，Ｐ２の密度が急激に増加する。制御装置１０は、点Ｐ１，Ｐ２の密度が極大値となるＺ座標を探索する際に、Ｚ座標の＋側から－側に向けて探索を行ってもよい。この場合、構造物Ｏ１，Ｏ２，Ｏ３の影響によって路面Ｒ１に対応するＺ座標よりも上方向で点Ｐ１，Ｐ２の密度が０より大きくなるが、路面Ｒ１に対応するＺ座標を境にして点Ｐ１，Ｐ２の密度が急激に増加する。
【0024】
制御装置１０は、点Ｐ１，Ｐ２の密度が極大値となるＺ座標に基づいて鉛直方向座標を算出する。本実施形態において、制御装置１０は、点Ｐ１，Ｐ２の密度が極大値となるＺ座標に補正値を加算する。点Ｐ１，Ｐ２の密度が極大値となるＺ座標に補正値を加算することで得られた値が鉛直方向座標である。補正値は、移動体Ｖ１の地上高やセンサ２１の取り付け高に基づいて設定される値である。自己位置として推定されるＺ座標を路面Ｒ１のＺ座標に対して、どの程度上方に位置させるかによって補正値は任意の値とすることができる。
【0025】
図４に示すように、次に、ステップＳ６において、制御装置１０は、鉛直方向座標が複数得られたか否かを判定する。鉛直方向座標が複数得られる場合とは、点Ｐ１，Ｐ２の密度が極大値となるＺ座標が複数存在する場合である。Ｚ座標の変化に対する点Ｐ１，Ｐ２の密度の変化を算出すると、点Ｐ１，Ｐ２の密度に複数の極大値が生じる場合がある。例えば、複数の路面Ｒ１が上下方向に重なり合う立体交差の存在する場所では、点Ｐ１，Ｐ２の密度に複数の極大値が生じる場合がある。そして、点Ｐ１，Ｐ２の密度に複数の極大値が生じた場合、それぞれの極大値に対応したＺ座標毎に鉛直方向座標が算出される。ステップＳ６の判定結果が否定の場合、制御装置１０は、ステップＳ７の処理を行う。ステップＳ６の判定結果が肯定の場合、制御装置１０は、ステップＳ９の処理を行う。
【0026】
ステップＳ７において、制御装置１０は、初期位置の設定を行う。初期位置とは、グローバル座標系での三次元座標であって自己位置推定を行う際に、最初に設定される移動体Ｖ１の自己位置である。
【0027】
図５に示すように、路面Ｒ１のＺ座標としてδが得られたとする。補正値をεとする。この場合、制御装置１０は、δ＋εを、鉛直方向座標とする。制御装置１０は、ステップＳ１で得られた三次元座標（α，β，γ）のγをδ＋εに置き換えて三次元座標（α，β，δ＋ε）とする。そして、制御装置１０は、三次元座標（α，β，δ＋ε）を初期位置として設定する。
【0028】
次に、ステップＳ８において、制御装置１０は、スキャンマッチングを開始する。スキャンマッチングは、センサ２１から得られたローカル座標系での点と、地図データＤ１によって表されるグローバル座標系での点Ｐ１，Ｐ２とのマッチングを行うことによって自己位置を推定することである。例えば、制御装置１０は、センサ２１から得られたローカル座標系での点のペアとなるグローバル座標系での点Ｐ１，Ｐ２を地図データＤ１から探索することによってマッチングを行う。スキャンマッチングが開始されると、以降は、スキャンマッチングを周期的に行うことによって自己位置を継続的に推定する。即ち、初期位置の設定は、移動体Ｖ１の起動時など、制御装置１０が自己位置を認識していない際に初期の自己位置を認識するために行われる。スキャンマッチングとしては、例えば、ICP（Iterative Closest Points）又はNDT（Normal Distribution Transform）が用いられる。
【0029】
ステップＳ９において、制御装置１０は、路面Ｒ１のＺ座標、即ち、点Ｐ１，Ｐ２の密度が極大値となるＺ座標のそれぞれについて基づいて算出された鉛直方向座標の信頼度を算出する。制御装置１０は、複数得られた鉛直方向座標のそれぞれを初期位置としてスキャンマッチングを行う。例えば、ステップＳ５において、鉛直方向座標としてδ１＋ε、及びδ２＋εが得られていたとする。δ１及びδ２は、それぞれ、路面Ｒ１のＺ座標である。この場合、制御装置１０は、三次元座標（α，β，δ１＋ε）を初期位置とするスキャンマッチングと、三次元座標（α，β，δ２＋ε）を初期位置とするスキャンマッチングと、を行う。そして、制御装置１０は、それぞれのスキャンマッチングに基づいて鉛直方向座標の信頼度を算出する。信頼度は、例えば、センサ２１から得られたローカル座標系での点のペアとなるグローバル座標系での点Ｐ１，Ｐ２が多いほど高くなるように算出される。
【0030】
次に、ステップＳ１０において、制御装置１０は、初期位置の設定を行う。制御装置１０は、水平方向座標と最も信頼度が高い鉛直方向座標を初期位置として設定する。例えば、三次元座標（α，β，δ１＋ε）、及び三次元座標（α，β，δ２＋ε）のうち、三次元座標（α，β，δ２＋ε）の信頼度が高かったとする。この場合、三次元座標（α，β，δ２＋ε）が初期位置として設定される。ステップＳ１０の処理を終えると、制御装置１０は、ステップＳ８の処理を行う。
【0031】
［本実施形態の作用］
制御装置１０は、Ｚ座標の変化に対する点Ｐ１，Ｐ２の密度の変化から、路面Ｒ１のＺ座標を算出する。そして、制御装置１０は、路面Ｒ１のＺ座標から鉛直方向座標を算出する。地図データＤ１上では、路面Ｒ１に対応するＺ座標には、点Ｐ１が密に存在する一方で、路面Ｒ１に対応するＺ座標より下方向には点Ｐ１，Ｐ２が存在しない。このため、点Ｐ１，Ｐ２の密度を用いることによって路面Ｒ１のＺ座標を精度良く算出できる。そして、路面Ｒ１のＺ座標に基づいて鉛直方向座標を算出することで、初期位置の精度を向上させることができる。
制御装置１０は、Ｚ座標の変化に対する点Ｐ１，Ｐ２の密度の変化から、路面Ｒ１のＺ座標を算出する。そして、制御装置１０は、路面Ｒ１のＺ座標から鉛直方向座標を算出する。地図データＤ１上では、路面Ｒ１に対応するＺ座標には、点Ｐ１が密に存在する一方で、路面Ｒ１に対応するＺ座標より下方向には点Ｐ１，Ｐ２が存在しない。このため、点Ｐ１，Ｐ２の密度を用いることによって路面Ｒ１のＺ座標を精度良く算出できる。そして、路面Ｒ１のＺ座標に基づいて鉛直方向座標を算出することで、初期位置の精度を向上させることができる。
【0032】
初期位置の精度の低下を抑制するため、鉛直方向座標として固定のＺ座標を用いることも考えられる。この場合、路面Ｒ１のＺ座標の変化が小さい環境で移動体Ｖ１を用いる必要がある。これに対し、実施形態の移動体Ｖ１であれば、路面Ｒ１のＺ座標に基づいた鉛直方向座標を算出することができる。従って、路面Ｒ１のＺ座標の変化が大きい環境Ａ１であっても初期位置を設定することができる。
【0033】
［本実施形態の効果］
（１）制御装置１０は、点Ｐ１，Ｐ２の密度が極大値となるＺ座標に基づいて鉛直方向座標を算出する。このため、初期位置の精度を向上させることができる。
（１）制御装置１０は、点Ｐ１，Ｐ２の密度が極大値となるＺ座標に基づいて鉛直方向座標を算出する。このため、初期位置の精度を向上させることができる。
【0034】
（２）制御装置１０は、点Ｐ１，Ｐ２の密度が極大値となるＺ座標が複数存在する場合、点Ｐ１，Ｐ２の密度が極大値となるＺ座標のそれぞれに基づいて算出された鉛直方向座標の信頼度を算出する。制御装置１０は、信頼度が最も高い鉛直方向座標を、初期位置の鉛直方向座標とする。このため、複数の路面Ｒ１が上下方向に重なり合う立体交差のような場所であっても初期位置を設定することができる。
【0035】
［変更例］
実施形態は、以下のように変更して実施することができる。実施形態及び以下の変更例は、技術的に矛盾しない範囲で互いに組み合わせて実施することができる。
実施形態は、以下のように変更して実施することができる。実施形態及び以下の変更例は、技術的に矛盾しない範囲で互いに組み合わせて実施することができる。
【0036】
○制御装置１０は、点Ｐ１，Ｐ２の密度が極大値となるＺ座標が複数存在する場合に、点Ｐ１，Ｐ２の密度が極大値となるＺ座標のうち点Ｐ１，Ｐ２の密度が最大値となるＺ座標に基づいて鉛直方向座標を算出してもよい。そして、制御装置１０は、当該鉛直方向座標を初期位置の鉛直方向座標としてもよい。
【0037】
○制御装置１０は、移動体Ｖ１のユーザーからの入力によって水平方向座標を取得してもよい。例えば、移動体Ｖ１は、ユーザーからの入力を受け付ける入力部を備える。そして、ユーザーは、入力部によって水平方向座標を入力してもよい。入力部は、例えば、タッチパネルである。タッチパネルには、環境地図の俯瞰図が表示される。ユーザーは、タッチパネルに表示された環境地図上での移動体Ｖ１の位置をタッチすることによって、移動体Ｖ１の水平方向座標を入力してもよい。この場合、移動体Ｖ１は、衛星航法装置２２を備えていてもよいし、備えていなくてもよい。
【0038】
○自己位置を路面Ｒ１のＺ座標に設定している場合、点Ｐ１，Ｐ２の密度が極大値となるＺ座標に補正値を加算しなくてもよい。この場合、制御装置１０は、点Ｐ１，Ｐ２の密度が極大値となるＺ座標を鉛直方向座標とする。
【符号の説明】
【0039】
Ｃ１…立体柱、Ｄ１…地図データ、Ｐ１，Ｐ２…点、Ｒ１…路面、Ｖ１…移動体、１０…制御装置、１３…記憶装置としての補助記憶装置、２１…センサ、１００…自己位置推定装置。
【図1】
【図2】
【図3】
【図4】
【図5】