知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024021158
(43)【公開日】2024-02-16
(54)【発明の名称】位置推定システム及び位置推定方法
(51)【国際特許分類】
G01C 21/30 20060101AFI20240208BHJP
【ＦＩ】
G01C21/30
【審査請求】未請求
【請求項の数】5
【出願形態】ＯＬ
(21)【出願番号】P 2022123801
(22)【出願日】2022-08-03
(71)【出願人】
【識別番号】000006208
【氏名又は名称】三菱重工業株式会社
(74)【代理人】
【識別番号】110002147
【氏名又は名称】弁理士法人酒井国際特許事務所
(72)【発明者】
【氏名】小早川 豊範
(72)【発明者】
【氏名】門脇 光哉
(72)【発明者】
【氏名】菊池 惟子
(72)【発明者】
【氏名】村田 直史
【テーマコード（参考）】
2F129
【Ｆターム（参考）】
2F129AA03
2F129BB12
2F129BB22
2F129BB26
2F129BB49
2F129GG17
(57)【要約】
【課題】特徴の少ない地形であっても、移動体の位置を精度よく推定する。
【解決手段】移動体の位置を推定する位置推定システムにおいて、外環境の第１の地形情報を取得する地形情報取得部と、移動体の移動量を検出する移動量検出センサと、外環境の第２の地形情報を含む地形マップ情報を記憶する記憶部と、地形マップ情報の位置座標における移動体の絶対位置を推定する演算部と、を備え、演算部は、移動体の初期位置において、複数のパーティクルを生成し、移動量に基づいて算出した初期位置に対する相対位置において、複数のパーティクルを遷移させ、第１の地形情報を取得し、遷移した複数のパーティクルのそれぞれの位置における第２の地形情報を地形マップ情報から取得し、第１の地形情報と第２の地形情報とのマッチングにより取得した誤差に基づいて、複数のパーティクルの尤度を算出し、算出した尤度に基づいて、移動体の絶対位置を推定する。
【選択図】図３
【解決手段】移動体の位置を推定する位置推定システムにおいて、外環境の第１の地形情報を取得する地形情報取得部と、移動体の移動量を検出する移動量検出センサと、外環境の第２の地形情報を含む地形マップ情報を記憶する記憶部と、地形マップ情報の位置座標における移動体の絶対位置を推定する演算部と、を備え、演算部は、移動体の初期位置において、複数のパーティクルを生成し、移動量に基づいて算出した初期位置に対する相対位置において、複数のパーティクルを遷移させ、第１の地形情報を取得し、遷移した複数のパーティクルのそれぞれの位置における第２の地形情報を地形マップ情報から取得し、第１の地形情報と第２の地形情報とのマッチングにより取得した誤差に基づいて、複数のパーティクルの尤度を算出し、算出した尤度に基づいて、移動体の絶対位置を推定する。
【選択図】図３
【特許請求の範囲】
【請求項1】
外環境を移動する移動体の位置を推定する位置推定システムにおいて、
前記移動体に設けられ、前記外環境の第１の地形情報を取得する地形情報取得部と、
前記移動体の移動に関する物理量を検出する移動量検出センサと、
前記外環境の位置座標と、前記位置座標に対応付けられると共に予め取得された前記外環境の第２の地形情報とを含む地形マップ情報を記憶する記憶部と、
前記地形マップ情報の前記位置座標における前記移動体の位置を絶対位置として推定する演算部と、を備え、
前記演算部は、
前記地形マップ情報の前記位置座標において、前記移動体の初期位置を中心として、複数のパーティクルを生成するステップと、
前記移動量検出センサによって検出した前記物理量に基づいて、前記初期位置に対する前記移動体の相対位置を算出し、算出した前記相対位置を中心として、生成した前記複数のパーティクルを遷移させるステップと、
前記地形情報取得部により前記相対位置における前記第１の地形情報を取得するステップと、
遷移した前記複数のパーティクルのそれぞれの位置における前記第２の地形情報を前記地形マップ情報から取得し、取得した前記第１の地形情報と前記第２の地形情報とのマッチングをそれぞれ行って誤差を取得し、取得した前記誤差に基づいて、前記複数のパーティクルのそれぞれにおける尤度を算出するステップと、
算出した前記複数のパーティクルの尤度に基づいて、前記移動体の前記絶対位置を推定するステップと、を実行する位置推定システム。
前記移動体に設けられ、前記外環境の第１の地形情報を取得する地形情報取得部と、
前記移動体の移動に関する物理量を検出する移動量検出センサと、
前記外環境の位置座標と、前記位置座標に対応付けられると共に予め取得された前記外環境の第２の地形情報とを含む地形マップ情報を記憶する記憶部と、
前記地形マップ情報の前記位置座標における前記移動体の位置を絶対位置として推定する演算部と、を備え、
前記演算部は、
前記地形マップ情報の前記位置座標において、前記移動体の初期位置を中心として、複数のパーティクルを生成するステップと、
前記移動量検出センサによって検出した前記物理量に基づいて、前記初期位置に対する前記移動体の相対位置を算出し、算出した前記相対位置を中心として、生成した前記複数のパーティクルを遷移させるステップと、
前記地形情報取得部により前記相対位置における前記第１の地形情報を取得するステップと、
遷移した前記複数のパーティクルのそれぞれの位置における前記第２の地形情報を前記地形マップ情報から取得し、取得した前記第１の地形情報と前記第２の地形情報とのマッチングをそれぞれ行って誤差を取得し、取得した前記誤差に基づいて、前記複数のパーティクルのそれぞれにおける尤度を算出するステップと、
算出した前記複数のパーティクルの尤度に基づいて、前記移動体の前記絶対位置を推定するステップと、を実行する位置推定システム。
【請求項2】
前記外環境は、地上であり、
前記地形情報取得部は、地形を撮像する撮像部であり、
前記第１の地形情報及び前記第２の地形情報は、前記地形の外郭となる情報である第１の稜線情報及び第２の稜線情報であり、
前記演算部は、
前記第１の地形情報を取得するステップにおいて、
前記撮像部によって撮像した前記相対位置における画像を取得するステップと、
取得した前記画像に含まれる前記第１の稜線情報を取得するステップと、を実行し、
前記尤度を算出するステップにおいて、
前記第１の稜線情報と、遷移した前記複数のパーティクルのそれぞれの位置における前記第２の稜線情報とのマッチングをそれぞれ行う請求項１に記載の位置推定システム。
前記地形情報取得部は、地形を撮像する撮像部であり、
前記第１の地形情報及び前記第２の地形情報は、前記地形の外郭となる情報である第１の稜線情報及び第２の稜線情報であり、
前記演算部は、
前記第１の地形情報を取得するステップにおいて、
前記撮像部によって撮像した前記相対位置における画像を取得するステップと、
取得した前記画像に含まれる前記第１の稜線情報を取得するステップと、を実行し、
前記尤度を算出するステップにおいて、
前記第１の稜線情報と、遷移した前記複数のパーティクルのそれぞれの位置における前記第２の稜線情報とのマッチングをそれぞれ行う請求項１に記載の位置推定システム。
【請求項3】
前記演算部は、
前記移動体の前記絶対位置を推定するステップにおいて、算出した前記複数のパーティクルの尤度を重み付けとする重み付き平均値により前記絶対位置を推定する請求項１に記載の位置推定システム。
前記移動体の前記絶対位置を推定するステップにおいて、算出した前記複数のパーティクルの尤度を重み付けとする重み付き平均値により前記絶対位置を推定する請求項１に記載の位置推定システム。
【請求項4】
前記移動体の姿勢に関する物理量を検出する姿勢検出センサを、さらに備え、
前記演算部は、
前記姿勢検出センサにより検出された前記移動体の姿勢に基づいて、前記第２の地形情報を補正する請求項１に記載の位置推定システム。
前記演算部は、
前記姿勢検出センサにより検出された前記移動体の姿勢に基づいて、前記第２の地形情報を補正する請求項１に記載の位置推定システム。
【請求項5】
外環境を移動する移動体の位置を推定する位置推定システムにより実行される位置推定方法において、
前記外環境の位置座標と、前記位置座標に対応付けられる前記外環境の第２の地形情報とを含む地形マップ情報が予め取得されており、
前記地形マップ情報の前記位置座標において、前記移動体の初期位置を中心として、複数のパーティクルを生成するステップと、
前記移動体の移動に関する物理量に基づいて、前記初期位置に対する前記移動体の相対位置を算出し、算出した前記相対位置を中心として、生成した前記複数のパーティクルを遷移させるステップと、
前記相対位置における前記外環境の第１の地形情報を取得するステップと、
遷移した前記複数のパーティクルのそれぞれの位置における前記第２の地形情報を前記地形マップ情報から取得し、取得した前記第１の地形情報と前記第２の地形情報とのマッチングをそれぞれ行って誤差を取得し、取得した前記誤差に基づいて、前記複数のパーティクルのそれぞれにおける尤度を算出するステップと、
算出した前記複数のパーティクルの尤度に基づいて、前記移動体の前記絶対位置を推定するステップと、を前記位置推定システムに実行させる位置推定方法。
前記外環境の位置座標と、前記位置座標に対応付けられる前記外環境の第２の地形情報とを含む地形マップ情報が予め取得されており、
前記地形マップ情報の前記位置座標において、前記移動体の初期位置を中心として、複数のパーティクルを生成するステップと、
前記移動体の移動に関する物理量に基づいて、前記初期位置に対する前記移動体の相対位置を算出し、算出した前記相対位置を中心として、生成した前記複数のパーティクルを遷移させるステップと、
前記相対位置における前記外環境の第１の地形情報を取得するステップと、
遷移した前記複数のパーティクルのそれぞれの位置における前記第２の地形情報を前記地形マップ情報から取得し、取得した前記第１の地形情報と前記第２の地形情報とのマッチングをそれぞれ行って誤差を取得し、取得した前記誤差に基づいて、前記複数のパーティクルのそれぞれにおける尤度を算出するステップと、
算出した前記複数のパーティクルの尤度に基づいて、前記移動体の前記絶対位置を推定するステップと、を前記位置推定システムに実行させる位置推定方法。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、位置推定システム及び位置推定方法に関するものである。
【背景技術】
【0002】
従来、複数のカメラにより撮影したカメラ画像から抽出される特徴点の位置と、地図データに記憶されている特徴点の位置とに基づいて、移動体の位置を推定する移動体の位置推定装置が知られている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】国際公開第２０２１／１００６５０号
【発明の概要】
【発明が解決しようとする課題】
【0004】
また、位置を推定する方法として、例えば、スカイラインマッチングと呼ばれる手法がある。スカイラインマッチングでは、移動体の周囲の環境における地形の稜線と、予め取得された地形データにおける地形の稜線とをマッチングして、その一致度から移動体の絶対位置を推定している。
【0005】
ところで、移動体が移動する外環境として、特徴の少ない地形である水中または月面等の環境がある。このような外環境である場合、特許文献１の位置推定装置及びスカイラインマッチングでは、特徴点に基づく位置の推定を精度よく行うことが困難となる。
【0006】
そこで、本開示は、特徴の少ない地形であっても、移動体の位置を精度よく推定することができる位置推定システム及び位置推定方法を提供することを課題とする。
【課題を解決するための手段】
【0007】
本開示の位置推定システムは、外環境を移動する移動体の位置を推定する位置推定システムにおいて、前記移動体に設けられ、前記外環境の第１の地形情報を取得する地形情報取得部と、前記移動体の移動に関する物理量を検出する移動量検出センサと、前記外環境の位置座標と、前記位置座標に対応付けられると共に予め取得された前記外環境の第２の地形情報とを含む地形マップ情報を記憶する記憶部と、前記地形マップ情報の前記位置座標における前記移動体の位置を絶対位置として推定する演算部と、を備え、前記演算部は、前記地形マップ情報の前記位置座標において、前記移動体の初期位置を中心として、複数のパーティクルを生成するステップと、前記移動量検出センサによって検出した前記物理量に基づいて、前記初期位置に対する前記移動体の相対位置を算出し、算出した前記相対位置を中心として、生成した前記複数のパーティクルを遷移させるステップと、前記地形情報取得部により前記相対位置における前記第１の地形情報を取得するステップと、遷移した前記複数のパーティクルのそれぞれの位置における前記第２の地形情報を前記地形マップ情報から取得し、取得した前記第１の地形情報と前記第２の地形情報とのマッチングをそれぞれ行って誤差を取得し、取得した前記誤差に基づいて、前記複数のパーティクルのそれぞれにおける尤度を算出するステップと、算出した前記複数のパーティクルの尤度に基づいて、前記移動体の前記絶対位置を推定するステップと、を実行する。
【0008】
本開示の位置推定方法は、外環境を移動する移動体の位置を推定する位置推定システムにより実行される位置推定方法において、前記外環境の位置座標と、前記位置座標に対応付けられる前記外環境の第２の地形情報とを含む地形マップ情報が予め取得されており、前記地形マップ情報の前記位置座標において、前記移動体の初期位置を中心として、複数のパーティクルを生成するステップと、前記移動体の移動に関する物理量に基づいて、前記初期位置に対する前記移動体の相対位置を算出し、算出した前記相対位置を中心として、生成した前記複数のパーティクルを遷移させるステップと、前記相対位置における前記外環境の第１の地形情報を取得するステップと、遷移した前記複数のパーティクルのそれぞれの位置における前記第２の地形情報を前記地形マップ情報から取得し、取得した前記第１の地形情報と前記第２の地形情報とのマッチングをそれぞれ行って誤差を取得し、取得した前記誤差に基づいて、前記複数のパーティクルのそれぞれにおける尤度を算出するステップと、出した前記複数のパーティクルの尤度に基づいて、前記移動体の前記絶対位置を推定するステップと、を前記位置推定システムに実行させる。
【発明の効果】
【0009】
本開示によれば、特徴の少ない地形であっても、移動体の位置を精度よく推定することができる。
【図面の簡単な説明】
【0010】
【発明を実施するための形態】
【0011】
以下に、本開示に係る実施形態を図面に基づいて詳細に説明する。なお、この実施形態によりこの開示が限定されるものではない。また、下記実施形態における構成要素には、当業者が置換可能かつ容易なもの、あるいは実質的に同一のものが含まれる。さらに、以下に記載した構成要素は適宜組み合わせることが可能であり、また、実施形態が複数ある場合には、各実施形態を組み合わせることも可能である。
【0012】
［本実施形態］
本実施形態に係る位置推定システム１は、外環境を移動する移動体５の位置を推定するシステムとなっている。本実施形態の位置推定システム１が適用される外環境としては、例えば、月面または水中等の特徴的な地形が少ない環境である。なお、以下では、外環境として、月面に適用した場合について説明する。また、移動体５としては、例えば、月面上を移動する探査車（ローバー）である。
本実施形態に係る位置推定システム１は、外環境を移動する移動体５の位置を推定するシステムとなっている。本実施形態の位置推定システム１が適用される外環境としては、例えば、月面または水中等の特徴的な地形が少ない環境である。なお、以下では、外環境として、月面に適用した場合について説明する。また、移動体５としては、例えば、月面上を移動する探査車（ローバー）である。
【0013】
図１は、本実施形態に係る位置推定システムを模式的に表した図である。図２は、本実施形態に係る位置推定システムに関するブロック図である。図３は、本実施形態に係る位置推定方法に関するフローチャートである。図４は、本実施形態に係る位置推定方法を説明する説明図である。図５は、本実施形態に係る位置推定方法により推定される絶対位置に関する説明図である。図６は、本実施形態に係る位置推定方法におけるリサンプリング前後の複数のパーティクルに関する説明図である。図７は、本実施形態と従来との推定される絶対位置を比較した図である。図８は、本実施形態と従来との推定される誤差を比較した図である。
【0014】
（位置推定システム）
図１及び図２を参照して、位置推定システム１について説明する。位置推定システム１は、移動体５に設けられる各種センサと、通信ネットワーク７を介して接続される位置推定装置６と、を備えている。移動体５は月面上を移動する一方で、位置推定装置６は地球上または宇宙空間等の月面以外の空間に設けられており、移動体５と位置推定装置６は、無線通信によって通信している。
図１及び図２を参照して、位置推定システム１について説明する。位置推定システム１は、移動体５に設けられる各種センサと、通信ネットワーク７を介して接続される位置推定装置６と、を備えている。移動体５は月面上を移動する一方で、位置推定装置６は地球上または宇宙空間等の月面以外の空間に設けられており、移動体５と位置推定装置６は、無線通信によって通信している。
【0015】
（移動体）
移動体５は、カメラ１１と、移動量検出センサ１２と、姿勢検出センサ１３と、を有している。カメラ１１は、移動体５が移動する地形を撮像しており、地形の画像を取得している。移動量検出センサ１２は、移動体５の移動に関する物理量を検出するセンサであり、例えば、加速度センサ、ジャイロセンサが適用され、加速度及び角速度の情報を取得している。移動体５は、加速度及び角速度の情報に基づいて、基準となる所定の位置からの移動量を算出して取得する。姿勢検出センサ１３は、例えば、スタートラッカ、太陽センサ、ジャイロセンサ等が適用され、移動体５の姿勢の情報を取得している。移動体５は、移動体５の姿勢の情報に基づいて、例えば、移動体５のロール角及びピッチ角の角度を算出して取得する。移動体５は、取得した画像と、移動量検出センサ１２により検出した移動量の情報と、姿勢検出センサ１３により検出した姿勢の情報とを、位置推定装置６に送信している。
移動体５は、カメラ１１と、移動量検出センサ１２と、姿勢検出センサ１３と、を有している。カメラ１１は、移動体５が移動する地形を撮像しており、地形の画像を取得している。移動量検出センサ１２は、移動体５の移動に関する物理量を検出するセンサであり、例えば、加速度センサ、ジャイロセンサが適用され、加速度及び角速度の情報を取得している。移動体５は、加速度及び角速度の情報に基づいて、基準となる所定の位置からの移動量を算出して取得する。姿勢検出センサ１３は、例えば、スタートラッカ、太陽センサ、ジャイロセンサ等が適用され、移動体５の姿勢の情報を取得している。移動体５は、移動体５の姿勢の情報に基づいて、例えば、移動体５のロール角及びピッチ角の角度を算出して取得する。移動体５は、取得した画像と、移動量検出センサ１２により検出した移動量の情報と、姿勢検出センサ１３により検出した姿勢の情報とを、位置推定装置６に送信している。
【0016】
（位置推定装置）
位置推定装置６は、演算部２１と、記憶部２２と、を有する。演算部２１は、例えば、ＣＰＵ（Central Processing Unit）等の集積回路を含んでいる。演算部２１は、移動体５から送信される情報に基づいて、移動体５の位置を推定している。
位置推定装置６は、演算部２１と、記憶部２２と、を有する。演算部２１は、例えば、ＣＰＵ（Central Processing Unit）等の集積回路を含んでいる。演算部２１は、移動体５から送信される情報に基づいて、移動体５の位置を推定している。
【0017】
記憶部２２は、半導体記憶デバイス及び磁気記憶デバイス等の任意の記憶デバイスである。記憶部２２は、各種プログラム及び各種データを記憶している。記憶部２２は、各種データとして、地形マップ情報Ｍを記憶している。地形マップ情報Ｍは、外環境の位置座標と、位置座標に対応付けられると共に予め取得された外環境の地形情報（第２の地形情報）とを含んでいる。地形情報としては、地形の稜線に関する稜線情報であり、事前に計測された地形情報となっている。
【0018】
（位置推定方法）
次に、図３から図６を参照して、位置推定システム１により実行される位置推定方法について説明する。図３及び図４に示すように、位置推定方法では、先ず、位置推定装置６の演算部２１が、地形マップ情報の位置座標において、移動体５の初期位置Ｔ１を中心として、複数（Ｍ個）のパーティクルＰを生成する（ステップＳ１）。ステップＳ１では、初期位置Ｔ１を、地形マップ情報の位置座標における初期の絶対位置としており、位置推定の開始地点における位置としている。なお、初期位置Ｔ１は、ステップＳ１において、位置推定の開始地点における位置としたが、位置推定後において、推定後の絶対位置を初期位置Ｔ１として更新してもよい。また、ステップＳ１では、複数のパーティクルＰを、分散σ２の正規乱数に基づいて分散させている。なお、複数のパーティクルＰは、前述の分散に特に限定されない。
次に、図３から図６を参照して、位置推定システム１により実行される位置推定方法について説明する。図３及び図４に示すように、位置推定方法では、先ず、位置推定装置６の演算部２１が、地形マップ情報の位置座標において、移動体５の初期位置Ｔ１を中心として、複数（Ｍ個）のパーティクルＰを生成する（ステップＳ１）。ステップＳ１では、初期位置Ｔ１を、地形マップ情報の位置座標における初期の絶対位置としており、位置推定の開始地点における位置としている。なお、初期位置Ｔ１は、ステップＳ１において、位置推定の開始地点における位置としたが、位置推定後において、推定後の絶対位置を初期位置Ｔ１として更新してもよい。また、ステップＳ１では、複数のパーティクルＰを、分散σ２の正規乱数に基づいて分散させている。なお、複数のパーティクルＰは、前述の分散に特に限定されない。
【0019】
続いて、位置推定方法では、演算部２１が、移動量検出センサ１２によって検出した移動量に基づいて、初期位置Ｔ１に対する移動体５の相対位置Ｔ２を算出し、算出した相対位置Ｔ２を中心として、生成した複数のパーティクルＰを遷移させる（ステップＳ２）。ステップＳ２では、移動体５から送信された移動量の情報を、位置推定装置６が取得し、位置推定装置６の演算部２１が、初期位置Ｔ１から変位した移動量に基づいて、相対位置Ｔ２を算出している。
【0020】
この後、位置推定方法では、演算部２１が、移動体５に設けられるカメラ１１により撮像した相対位置Ｔ２における地形の画像を取得したか否かを判定する（ステップＳ３）。演算部２１は、地形の画像を取得していないと判定する（ステップＳ３：Ｎｏ）と、遷移後の複数のパーティクルＰに基づいて絶対位置を推定する（ステップＳ４）。ステップＳ４では、遷移後の複数のパーティクルＰの位置の平均値を推定位置としている。演算部２１は、ステップＳ４の実行後、ステップＳ２に進み、地形の画像を取得するまで、ステップＳ２からステップＳ４を繰り返し実行する。
【0021】
演算部２１は、地形の画像を取得したと判定する（ステップＳ３：Ｙｅｓ）と、遷移後の複数のパーティクルＰの位置において、地形マップ情報Ｍから得られた地形情報と、カメラ１１の画像から得られた地形情報とのマッチングを行って、マッチング結果に基づいて、各パーティクルＰの尤度を算出する（ステップＳ５）。具体的に、ステップＳ５では、演算部２１が、地形マップ情報Ｍを用いて、移動体５から各パーティクルＰの位置を見たときの予測される地形の稜線情報Ｄ２（以下、第２の稜線情報Ｄ２という）を取得する。また、ステップＳ５では、演算部２１が、カメラ１１の画像を用いて、移動体５から各パーティクルＰの位置を見たときの現実の地形の稜線情報Ｄ１（以下、第１の稜線情報Ｄ１という）を取得する。そして、ステップＳ５では、演算部２１が、各パーティクルＰにおいて、第１の稜線情報Ｄ１と第２の稜線情報Ｄ２との二乗和誤差を算出し、算出した二乗和誤差に基づいて、各パーティクルＰの尤度を算出する。
【0022】
ここで、パーティクルＰの尤度は、移動体５の存在確率として定義する。移動体５の存在確率は、第１の稜線情報Ｄ１と第２の稜線情報Ｄ２との二乗和誤差が小さいほど、パーティクルＰにおける移動体５の存在確率が高いと解釈される。換言すれば、演算部２１は、第１の稜線情報Ｄ１と第２の稜線情報Ｄ２との二乗和誤差から、移動体５の存在確率を算出し、移動体５の存在確率をパーティクルＰの尤度として取得する。このため、二乗和誤差が小さいほど、パーティクルＰの尤度は高くなり、二乗和誤差が大きいほど、パーティクルＰの尤度は低くなる。
【0023】
また、演算部２１は、地形マップ情報Ｍに基づいて第２の稜線情報Ｄ２を取得する場合、移動体５の姿勢の情報に基づく姿勢補正を行っている。つまり、演算部２１は、相対位置における移動体５の姿勢の情報、すなわち、ロール角及びピッチ角を取得し、水平を基準とするカメラ座標系に対して、取得したロール角及びピッチ角に基づく補正を行った上で、第２の稜線情報Ｄ２を取得している。そして、演算部２１は、姿勢補正後の第２の稜線情報Ｄと、第１の稜線情報Ｄ１とのマッチングを実行している。
【0024】
続いて、位置推定方法では、演算部２１が、各パーティクルＰの尤度を重みとし、複数のパーティクルの重み付き平均値を絶対位置Ｔ３として推定する（ステップＳ６）。図５には、算出された複数のパーティクルＰの尤度と、推定される絶対位置Ｔ３と、真値Ｔ４とが図示されている。図５は、その横軸がＸ方向における位置となっており、その縦軸がＹ方向における位置となっている。
【0025】
そして、位置推定方法では、演算部２１が、移動体５の絶対位置Ｔ３の推定を終了するか否かを判定し（ステップＳ７）、推定を終了すると判定した場合（ステップＳ７：Ｙｅｓ）、位置推定方法を終了する。一方で、演算部２１は、推定を終了すると判定しない場合（ステップＳ７：Ｎｏ）、各パーティクルＰの尤度に応じて、複数（Ｍ個）のパーティクルＰを選択（リサンプリング）する（ステップＳ８）。また、ステップＳ８では、演算部２１が推定した絶対位置Ｔ３を、初期位置Ｔ１として取り扱う。なお、ステップＳ８において、リサンプリングされた後の複数のパーティクルＰの尤度は、その値がリセットされる。
【0026】
図６には、リサンプリング前後の複数のパーティクルＰを示している。図６は、その横軸がＸ方向における位置となっており、その縦軸がＹ方向における位置となっている。リサンプリング前の複数のパーティクルＰの尤度は、絶対位置Ｔ３に近いほうが、移動体５の存在確率が高い。このため、リサンプリング後の複数のパーティクルＰは、リサンプリング前の複数のパーティクルＰに比して、推定される絶対位置Ｔ３に近いパーティクルＰがより多く選択される。
【0027】
位置推定方法では、演算部２１が、ステップＳ８の実行後、再びステップＳ２に進み、絶対位置Ｔ３の推定が終わるまで、ステップＳ２からステップＳ８を繰り返し実行する。
【0028】
【0029】
図７には、算出された複数のパーティクルＰの尤度と、本実施形態において推定される絶対位置Ｔ３と、従来において推定される絶対位置Ｔ５と、真値Ｔ４とが図示されている。図７は、その横軸がＸ方向における位置となっており、その縦軸がＹ方向における位置となっている。従来の絶対位置Ｔ５は、スカイラインマッチングにより推定された絶対位置である。図７を見るに、本実施形態の絶対位置Ｔ３と真値Ｔ４との誤差は、従来の絶対位置Ｔ５と真値Ｔ４との誤差に比して、小さいことが確認された。
【0030】
図８には、カメラ１１から取得される第１の稜線情報Ｄ１と、本実施形態において地形マップ情報Ｍから得られる姿勢補正後の第２の稜線情報Ｄ２と、従来において地形マップ情報Ｍから得られる姿勢補正していない第２の稜線情報Ｄ３と、が図示されている。図８は、その横軸が移動体５（カメラ１１）を中心とする方位角となっており、その縦軸が移動体５（カメラ１１）を中心とする仰角となっている。図８を見るに、第１の稜線情報Ｄ１と本実施形態の第２の稜線情報Ｄ２との誤差は、第１の稜線情報Ｄ１と従来の第２の稜線情報Ｄ３との誤差に比して、小さいことが確認された。
【0031】
なお、本実施形態では、外環境として、月面に適用して説明したが、水中に適用してもよい。水中に適用する場合、地形情報を取得する地形情報取得部として、カメラ１１またはソナー等を用いて、地形情報を取得してもよい。この場合においても、カメラ１１またはソナー等により取得した地形情報（第１の地形情報）と、地形マップ情報Ｍから取得した地形情報（第２の地形情報）とをマッチングして、その誤差から、各パーティクルＰの尤度を算出する。
【0032】
また、本実施形態では、複数のパーティクルＰの尤度を重み付けとし、重み付き平均値を絶対位置Ｔ３として推定したが、この構成に特に限定されない。例えば、複数のパーティクルＰの尤度のうち、最も高い尤度のパーティクルＰの位置を、絶対位置Ｔ３として推定してもよい。
【0033】
以上のように、本実施形態に記載の位置推定システム１及び位置推定方法は、例えば、以下のように把握される。
【0034】
第１の態様に係る位置推定システム１は、外環境を移動する移動体５の位置を推定する位置推定システム１において、前記移動体５に設けられ、前記外環境の第１の地形情報（第１の稜線情報Ｄ１）を取得する地形情報取得部（カメラ１１）と、前記移動体の移動に関する物理量を検出する移動量検出センサ１２と、前記外環境の位置座標と、前記位置座標に対応付けられると共に予め取得された前記外環境の第２の地形情報（第２の稜線情報Ｄ２）とを含む地形マップ情報Ｍを記憶する記憶部２２と、前記地形マップ情報Ｍの前記位置座標における前記移動体５の位置を絶対位置Ｔ３として推定する演算部２１と、を備え、前記演算部２１は、前記地形マップ情報Ｍの前記位置座標において、前記移動体５の初期位置Ｔ１を中心として、複数のパーティクルＰを生成するステップＳ１と、前記移動量検出センサ１２によって検出した前記物理量に基づいて、前記初期位置Ｔ１に対する前記移動体５の相対位置Ｔ２を算出し、算出した前記相対位置Ｔ２を中心として、生成した前記複数のパーティクルＰを遷移させるステップＳ２と、前記地形情報取得部により前記相対位置における前記第１の地形情報を取得するステップＳ３と、遷移した前記複数のパーティクルＰのそれぞれの位置における前記第２の地形情報を前記地形マップ情報Ｍから取得し、取得した前記第１の地形情報と前記第２の地形情報とのマッチングをそれぞれ行って誤差を取得し、取得した前記誤差に基づいて、前記複数のパーティクルＰのそれぞれにおける尤度を算出するステップＳ５と、算出した前記複数のパーティクルＰの尤度に基づいて、前記移動体５の前記絶対位置Ｔ３を推定するステップＳ６と、を実行する。
【0035】
この構成によれば、地形情報取得部及び移動量検出センサ１２を用いたセンサフュージョンによって、移動体５の初期位置Ｔ１（絶対位置Ｔ３）と相対位置Ｔ２とから、複数のパーティクルＰの尤度に基づく移動体５の絶対位置Ｔ３を推定することができる。このため、特徴の少ない地形であっても、存在確率が高い移動体５の絶対位置Ｔ３を精度良く推定することができる。
【0036】
第２の態様として、第１の態様に係る位置推定システム１において、前記外環境は、地上であり、前記地形情報取得部は、地形を撮像する撮像部（カメラ１１）であり、前記第１の地形情報及び前記第２の地形情報は、前記地形の外郭となる情報である第１の稜線情報Ｄ１及び第２の稜線情報Ｄ２であり、前記演算部２１は、前記第１の地形情報を取得するステップＳ３、Ｓ５において、前記撮像部によって撮像した前記相対位置における画像を取得するステップＳ３と、取得した前記画像に含まれる前記第１の稜線情報Ｄ１を取得するステップＳ５と、を実行し、前記尤度を算出するステップＳ５において、前記第１の稜線情報と、遷移した前記複数のパーティクルＰのそれぞれの位置における前記第２の稜線情報Ｄ２とのマッチングをそれぞれ行う。
【0037】
この構成によれば、移動体５に設けられたカメラ１１を用いて、地形の第１の稜線情報Ｄ１を取得することができると共に、移動体５の絶対位置Ｔ３を推定することができる。
【0038】
第３の態様として、第１または第２の態様に係る位置推定システム１において、前記演算部２１は、前記移動体５の前記絶対位置Ｔ３を推定するステップＳ６において、算出した前記複数のパーティクルＰの尤度を重み付けとする重み付き平均値により前記絶対位置Ｔ３を推定する。
【0039】
この構成によれば、統計的に絶対位置Ｔ３を推定することができるため、信頼性の高い絶対位置Ｔ３として推定することができる。
【0040】
第４の態様として、第１から第３のいずれか１つの態様に係る位置推定システム１において、前記移動体５の姿勢に関する物理量を検出する姿勢検出センサ１３を、さらに備え、前記演算部２１は、前記姿勢検出センサ１３により検出された前記移動体５の姿勢に基づいて、前記第２の地形情報を補正する。
【0041】
この構成によれば、移動体５の姿勢に基づいて、第２の地形情報を補正することにより、姿勢補正後の第２の地形情報と、第１の地形情報との誤差を小さくすることができる。このため、移動体５の絶対位置の推定精度を向上させることができる。
【0042】
第５の態様に係る位置推定方法は、外環境を移動する移動体５の位置を推定する位置推定システム１により実行される位置推定方法において、前記外環境の位置座標と、前記位置座標に対応付けられる前記外環境の第２の地形情報（第２の稜線情報Ｄ２）とを含む地形マップ情報Ｍが予め取得されており、前記地形マップ情報Ｍの前記位置座標において、前記移動体５の初期位置Ｔ１を中心として、複数のパーティクルＰを生成するステップＳ１と、前記移動量検出センサ１２によって検出した前記物理量に基づいて、前記初期位置Ｔ１に対する前記移動体５の相対位置Ｔ２を算出し、算出した前記相対位置Ｔ２を中心として、生成した前記複数のパーティクルＰを遷移させるステップＳ２と、前記地形情報取得部により前記相対位置における前記第１の地形情報を取得するステップＳ３と、遷移した前記複数のパーティクルＰのそれぞれの位置における前記第２の地形情報を前記地形マップ情報Ｍから取得し、取得した前記第１の地形情報と前記第２の地形情報とのマッチングをそれぞれ行って誤差を取得し、取得した前記誤差に基づいて、前記複数のパーティクルＰのそれぞれにおける尤度を算出するステップＳ５と、算出した前記複数のパーティクルＰの尤度に基づいて、前記移動体５の前記絶対位置Ｔ３を推定するステップＳ６と、を前記位置推定システム１に実行させる。
【0043】
この構成によれば、地形情報取得部及び移動量検出センサ１２を用いたセンサフュージョンによって、移動体５の初期位置Ｔ１（絶対位置Ｔ３）と相対位置Ｔ２とから、複数のパーティクルＰの尤度に基づく移動体５の絶対位置Ｔ３を推定することができる。このため、特徴の少ない地形であっても、存在確率が高い移動体５の絶対位置Ｔ３を精度良く推定することができる。
【符号の説明】
【0044】
１ 位置推定システム
５ 移動体
６ 位置推定装置
１１ カメラ
１２ 移動量検出センサ
１３ 姿勢検出センサ
２１ 演算部
２２ 記憶部
Ｍ 地形マップ情報
Ｔ１ 初期位置
Ｔ２ 相対位置
Ｔ３ 絶対位置
Ｐ パーティクル
Ｄ１ 第１の稜線情報
Ｄ２ 第２の稜線情報
５ 移動体
６ 位置推定装置
１１ カメラ
１２ 移動量検出センサ
１３ 姿勢検出センサ
２１ 演算部
２２ 記憶部
Ｍ 地形マップ情報
Ｔ１ 初期位置
Ｔ２ 相対位置
Ｔ３ 絶対位置
Ｐ パーティクル
Ｄ１ 第１の稜線情報
Ｄ２ 第２の稜線情報
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】