IP Force 特許公報掲載プロジェクト 2022.1.31 β版

知財求人 - 知財ポータルサイト「IP Force」

▶ 株式会社豊田自動織機の特許一覧

<>
  • 特開-自己位置推定装置 図1
  • 特開-自己位置推定装置 図2
  • 特開-自己位置推定装置 図3
  • 特開-自己位置推定装置 図4
  • 特開-自己位置推定装置 図5
  • 特開-自己位置推定装置 図6
  • 特開-自己位置推定装置 図7
  • 特開-自己位置推定装置 図8
  • 特開-自己位置推定装置 図9
  • 特開-自己位置推定装置 図10
  • 特開-自己位置推定装置 図11
< >
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024127468
(43)【公開日】2024-09-20
(54)【発明の名称】自己位置推定装置
(51)【国際特許分類】
   G01C 21/30 20060101AFI20240912BHJP
   G05D 1/43 20240101ALI20240912BHJP
【FI】
G01C21/30
G05D1/02 H
【審査請求】未請求
【請求項の数】2
【出願形態】OL
(21)【出願番号】P 2023036639
(22)【出願日】2023-03-09
(71)【出願人】
【識別番号】000003218
【氏名又は名称】株式会社豊田自動織機
(74)【代理人】
【識別番号】100105957
【弁理士】
【氏名又は名称】恩田 誠
(74)【代理人】
【識別番号】100068755
【弁理士】
【氏名又は名称】恩田 博宣
(72)【発明者】
【氏名】青山 拓真
(72)【発明者】
【氏名】坂元 優太
【テーマコード(参考)】
2F129
5H301
【Fターム(参考)】
2F129AA03
2F129AA11
2F129AA20
2F129BB49
2F129GG17
2F129GG18
5H301AA10
5H301BB05
5H301BB07
5H301BB14
5H301DD01
5H301DD07
5H301DD15
5H301FF11
5H301GG08
5H301GG09
(57)【要約】
【課題】自己位置推定精度を向上させること。
【解決手段】制御装置は、センサから点群データを取得する。制御装置は、ローカル座標系で表される領域を分割する。制御装置は、分割点群データと地図データとの照合によって自己位置候補を推定する。制御装置は、分割点群データ毎に推定された全ての自己位置候補が互いに一致するか否かを判定する。判定の結果、全ての自己位置候補が一致する場合、制御装置は、当該自己位置候補に基づいて自己位置を決定する。判定の結果、少なくとも1つの自己位置候補が一致しない場合、制御装置は、自己位置候補毎に点群データと地図データとの照合によって信頼度を算出し、信頼度が最も高い自己位置候補を自己位置とする。
【選択図】図3
【特許請求の範囲】
【請求項1】
移動体の自己位置を推定する自己位置推定装置であって、
前記移動体が用いられる環境をグローバル座標系での座標で表した地図データを記憶する記憶装置と、
前記移動体の周辺環境をローカル座標系での座標を表す点として検出するセンサと、
制御装置と、を備え、
前記制御装置は、
前記センサから前記点の集合である点群データを取得し、
前記ローカル座標系で表される領域を複数の分割領域に分割し、
前記分割領域のそれぞれに当該分割領域に含まれる前記点を割り当て、
前記分割領域のそれぞれに割り当てられた前記点の集合である分割点群データ毎に前記地図データとの照合によって自己位置候補を推定し、
前記分割点群データ毎に推定された全ての前記自己位置候補が互いに一致するか否かを判定し、
前記判定の結果、全ての前記自己位置候補が一致する場合、当該自己位置候補に基づいて前記自己位置を決定し、
前記判定の結果、少なくとも1つの前記自己位置候補が一致しない場合、前記自己位置候補毎に前記点群データと前記地図データとの照合によって信頼度を算出し、前記信頼度が最も高い前記自己位置候補を前記自己位置とする、自己位置推定装置。
【請求項2】
前記制御装置は、前記判定の結果、全ての前記自己位置候補が一致する場合、当該自己位置候補に前記移動体が位置していると仮定して前記点群データと前記地図データとの照合を行うことによって前記自己位置を推定する、請求項1に記載の自己位置推定装置。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、自己位置推定装置に関する。
【背景技術】
【0002】
特許文献1に開示の自己位置推定装置は、移動体の仮の自己位置を推定する。自己位置推定装置は、仮の自己位置からセンサが点群データを取得可能な範囲を算出する。そして、自己位置推定装置は、地図データのうちセンサが点群データを取得可能な範囲と点群データとの照合によって自己位置を推定する。これにより、自己位置推定精度を向上させている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開2022-26832号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
特許文献1の自己位置推定装置であっても、推定した自己位置と移動体の実際の位置とに乖離が生じる場合がある。
【課題を解決するための手段】
【0005】
上記課題を解決する自己位置推定装置は、移動体の自己位置を推定する自己位置推定装置であって、前記移動体が用いられる環境をグローバル座標系での座標で表した地図データを記憶する記憶装置と、前記移動体の周辺環境をローカル座標系での座標を表す点として検出するセンサと、制御装置と、を備え、前記制御装置は、前記センサから前記点の集合である点群データを取得し、前記ローカル座標系で表される領域を複数の分割領域に分割し、前記分割領域のそれぞれに当該分割領域に含まれる前記点を割り当て、前記分割領域のそれぞれに割り当てられた前記点の集合である分割点群データ毎に前記地図データとの照合によって自己位置候補を推定し、前記分割点群データ毎に推定された全ての前記自己位置候補が互いに一致するか否かを判定し、前記判定の結果、全ての前記自己位置候補が一致する場合、当該自己位置候補に基づいて前記自己位置を決定し、前記判定の結果、少なくとも1つの前記自己位置候補が一致しない場合、前記自己位置候補毎に前記点群データと前記地図データとの照合によって信頼度を算出し、前記信頼度が最も高い前記自己位置候補を前記自己位置とする。
【0006】
制御装置は、分割点群データ毎に自己位置候補を推定する。全ての自己位置候補が一致する場合、当該自己位置候補の精度は高いとみなすことができる。従って、制御装置は、自己位置推定候補に基づいて自己位置を決定することができる。制御装置は、少なくとも1つの自己位置候補が一致しない場合、自己位置候補毎に点群データと地図データとの照合によって信頼度を算出する。制御装置は、信頼度が最も高い自己位置候補を自己位置とする。これにより、自己位置推定精度を向上させることができる。
【0007】
上記自己位置推定装置について、前記制御装置は、前記判定の結果、全ての前記自己位置候補が一致する場合、当該自己位置候補に前記移動体が位置していると仮定して前記点群データと前記地図データとの照合を行うことによって前記自己位置を推定してもよい。
【発明の効果】
【0008】
本発明によれば、自己位置推定精度を向上させることができる。
【図面の簡単な説明】
【0009】
図1】移動体が用いられる環境の模式図である。
図2図1の移動体の概略構成図である。
図3図2の制御装置が実行する自己位置推定制御を示すフローチャートである。
図4図3の自己位置推定制御で得られた分割点群データの一例を示す図である。
図5図3の自己位置推定制御で得られた第1自己位置候補の一例を示す図である。
図6図3の自己位置推定制御で得られた第2自己位置候補の一例を示す図である。
図7図3の自己位置推定制御で得られた第2自己位置候補の一例を示す図である。
図8図5に示す第1自己位置候補に移動体が位置していると仮定して点群データと地図データとの照合を行った結果を示す図である。
図9図7に示す第2自己位置候補に移動体が位置していると仮定して点群データと地図データとの照合を行った結果を示す図である。
図10】比較例の自己位置推定制御によって推定された仮の自己位置の一例を示す図である。
図11】比較例の自己位置推定制御によって推定された仮の自己位置の一例を示す図である。
【発明を実施するための形態】
【0010】
自己位置推定装置の一実施形態について説明する。
図1に示すように、移動体10が用いられる環境A1には種々の構造物O1,O2が含まれる。構造物O1,O2は、例えば、壁、柱、及び屋根など、建物を構成する部材を含む。
【0011】
構造物O1は、第1表面S1と、第2表面S2と、を備える。第1表面S1及び第2表面S2は、移動体10の通過可能な空間に露出する面である。第1表面S1と第2表面S2とは、例えば、構造物O1の互いに反対となる面である。第1表面S1と第2表面S2とは、互いに平行である。第1表面S1及び第2表面S2は、起伏なく直線状に延びる面である。
【0012】
構造物O2は、第3表面S3を備える。第3表面S3は、移動体10の通過可能な空間に露出する面である。構造物O2は、構造物O1に向けて突出する突部Cを備える。第3表面S3は、第1表面S1に向かい合う面である。第3表面S3は、第1部位S31と、第2部位S32と、第3部位S33と、を備える。第1部位S31及び第2部位S32は、第3表面S3のうち第1表面S1と平行な部位である。第3部位S33は、第1部位S31と第2部位S32とを接続する部位であって突部Cの表面となる部位である。
【0013】
移動体10は、例えば、車両、飛行体、及び多足歩行ロボットを含む。車両は、例えば、乗用車、及び産業車両を含む。産業車両は、例えば、フォークリフト、トーイングトラクタ、及び搬送車を含む。
【0014】
<自己位置推定装置>
図2に示すように、移動体10は、自己位置推定装置20を備える。自己位置推定装置20は、制御装置21を備える。
【0015】
制御装置21は、プロセッサ22と、記憶部23と、を備える。プロセッサ22は、例えば、CPU(Central Processing Unit)、GPU(Graphics Processing Unit)、又はDSP(Digital Signal Processor)である。記憶部23は、RAM(Random Access Memory)及びROM(Read Only Memory)を含む。記憶部23には、移動体10を動作させるためのプログラムが記憶されている。記憶部23は、処理をプロセッサ22に実行させるように構成されたプログラムコードまたは指令を格納しているといえる。記憶部23、すなわち、コンピュータ可読媒体は、汎用または専用のコンピュータでアクセスできるあらゆる利用可能な媒体を含む。制御装置21は、ASIC(Application Specific Integrated Circuit)やFPGA(Field Programmable Gate Array)等のハードウェア回路によって構成されていてもよい。処理回路である制御装置21は、コンピュータプログラムに従って動作する1つ以上のプロセッサ、ASICやFPGA等の1つ以上のハードウェア回路、或いは、それらの組み合わせを含み得る。
【0016】
移動体10は、補助記憶装置24を備える。補助記憶装置24は、データを書き換え可能な不揮発性記憶装置である。補助記憶装置24は、例えば、ハードディスクドライブ、又はソリッドステートドライブである。
【0017】
補助記憶装置24は、地図データD1を記憶している。補助記憶装置24は、地図データD1を記憶する記憶装置の一例である。地図データD1は、記憶部23に記憶されていてもよい。地図データD1は、グローバル座標系での座標によって環境A1を表している。詳細に言えば、地図データD1は、環境に含まれる構造物O1,O2の表面S1~S3をグローバル座標系での座標で表したものである。グローバル座標系は、原点の位置が固定である絶対座標系である。グローバル座標系は、少なくとも水平方向の位置を表すことができればよい。グローバル座標系は、互いに直交するX軸とY軸とで水平方向の座標を表す二次元座標系であってもよい。グローバル座標系は、互いに直交するX軸とY軸とで水平方向の座標を表し、X軸及びY軸に直交するZ軸で鉛直方向の座標を表す三次元座標系であってもよい。地図データD1は、例えば、SLAM(Simultaneous Localization and Mapping)を用いて作成される。地図データD1は、例えば、ポイントクラウドマップ、又は占有格子地図である。
【0018】
自己位置推定装置20は、センサ25を備える。センサ25は、移動体10の周辺環境をローカル座標系での座標として検出する。ローカル座標系での座標は、ローカル座標系での点として表される。この点は、構造物O1,O2を含む物体の表面を表す点である。センサ25は、例えば、LIDAR(Laser Imaging Detection and Ranging)、ステレオカメラ、又はミリ波レーダーである。ローカル座標系は、移動体10とともに移動する相対座標系である。ローカル座標系は、互いに直交するX軸とY軸とで水平方向の座標を表す二次元座標系であってもよい。ローカル座標系は、互いに直交するX軸とY軸とで水平方向の座標を表し、X軸及びY軸に直交するZ軸で鉛直方向の座標を表す三次元座標系であってもよい。
【0019】
制御装置21は、自己位置推定制御を行う。自己位置とは、グローバル座標系での移動体10の座標である。自己位置は、移動体10の一点を示す座標である。移動体10の一点としては、移動体10の中心位置など、任意の位置に設定することができる。以下、制御装置21が行う自己位置推定制御について説明する。自己位置推定制御は、例えば、移動体10が自律移動する際に行われる。
【0020】
<自己位置推定制御>
図3及び図4に示すように、ステップS10において、制御装置21は、点群データPCをセンサ25から取得する。点群データPCは、点Pの集合である。点Pは、構造物O1,O2を含む物体の表面をローカル座標系での座標で表したデータである。図4には、点群データPCの一例を示す。図4に示す点群データPCは、構造物O1と構造物O2との間に移動体10が位置している場合にセンサ25から取得したデータである。本実施形態では、一例として、点群データPCは、水平方向の座標を表すとする。
【0021】
次に、ステップS11において、制御装置21は、ローカル座標系で表される領域Rを複数の分割領域R1,R2に分割する。制御装置21は、領域Rを水平方向に対して分割する。本実施形態では、領域Rを移動体10の左右方向に分割する。例えば、ローカル座標系のX軸を移動体10の左右方向に延びる軸とし、ローカル座標系のY軸Aを移動体10の前後方向に延びる軸とした場合、制御装置21は、Y軸Aを中心として領域Rを分割する。Y軸Aを中心として領域Rを分割することにより得られた1つの分割領域R1を第1分割領域R1とし、第1分割領域R1とは異なる分割領域R2を第2分割領域R2とする。
【0022】
次に、ステップS12において、制御装置21は、点群データPCに含まれる点Pを各分割領域R1,R2のそれぞれに割り当てる。点Pの割り当ては、ローカル座標系の座標によって行うことができる。Y軸Aを中心として領域Rを分割している場合であれば、X座標が0以上か0未満かによって点Pを第1分割領域R1、又は第2分割領域R2に割り当てればよい。例えば、制御装置21は、X座標が0以上の点Pは第1分割領域R1に割り当てる。制御装置21は、X座標が0未満の点Pは第2分割領域R2に割り当てる。分割領域R1,R2のそれぞれに割り当てられた点Pの集合を分割点群データPC1,PC2とする。第1分割領域R1に割り当てられた点Pの集合を第1分割点群データPC1とし、第2分割領域R2に割り当てられた点Pの集合を第2分割点群データPC2とする。
【0023】
次に、ステップS13において、制御装置21は、分割点群データPC1,PC2を用いて自己位置推定を実行する。自己位置推定は、分割点群データPC1,PC2のそれぞれと地図データD1との照合によって行われる。分割点群データPC1,PC2のそれぞれと地図データD1との照合によって得られた推定結果を自己位置候補とする。自己位置候補は、自己位置の候補である。自己位置候補は、分割領域R1,R2と同数、即ち、分割点群データPC1,PC2と同数得られる。
【0024】
第1分割領域R1についての自己位置推定は、第1分割点群データPC1と地図データD1との照合によって行われる。制御装置21は、第1分割点群データPC1によって表される形状と特徴が類似する形状を地図データD1から探索する。制御装置21は、ローカル座標系における移動体10の位置をグローバル座標系に当てはめることによってグローバル座標系における移動体10の位置を推定する。これにより得られた移動体10の自己位置候補を第1自己位置候補とする。
【0025】
図5に示す例では、第1分割点群データPC1によって表される形状と特徴が類似する形状として第3表面S3が抽出された場合の第1自己位置候補10Aを示す。
第2分割領域R2についての自己位置推定は、第2分割点群データPC2と地図データD1との照合によって行われる。第2分割領域R2についての自己位置推定は、第1分割領域R1についての自己位置推定と同様の処理によって行われればよい。第2分割点群データPC2を用いて推定された移動体10の自己位置候補を第2自己位置候補とする。
【0026】
図6に示す例では、第2分割点群データPC2によって表される形状と特徴が類似する形状として第1表面S1が抽出された場合の第2自己位置候補10Bを示す。
図7に示す例では、第2分割点群データPC2によって表される形状と特徴が類似する形状として第2表面S2が抽出された場合の第2自己位置候補10Bを示す。
【0027】
図3に示すように、次に、ステップS14において、制御装置21は、分割点群データPC1,PC2毎に推定された全ての自己位置候補が互いに一致するか否かを判定する。本実施形態であれば、第1自己位置候補10Aと第2自己位置候補10Bとが互いに一致するかが判定される。制御装置21は、第1自己位置候補10Aと第2自己位置候補10Bとの差が予め定められた設定値未満であれば第1自己位置候補10Aと第2自己位置候補10Bとが互いに一致すると判断する。第1自己位置候補10Aと第2自己位置候補10Bとの差は、例えば、グローバル座標系における第1自己位置候補10Aと第2自己位置候補10Bとのユークリッド距離である。設定値は、例えば、自己位置推定によって生じる誤差を許容できるように設定されている。
【0028】
ステップS14の判定結果が肯定の場合、制御装置21は、ステップS15の処理を行う。ステップS13で図5に示す第1自己位置候補10Aと図6に示す第2自己位置候補10Bとが得られた場合、ステップS14の判定結果は肯定になる。ステップS14の判定結果が否定の場合、制御装置21は、ステップS17の処理を行う。ステップS13で図5に示す第1自己位置候補10Aと図7に示す第2自己位置候補10Bとが得られた場合、ステップS14の判定結果は否定になる。
【0029】
ステップS15において、制御装置21は、点群データPCを用いて自己位置推定を行う。詳細にいえば、制御装置21は、ステップS14で一致していると判定された自己位置候補に移動体10が位置していると仮定して点群データPCと地図データD1との照合を行う。点群データPCを用いた自己位置推定は、分割点群データPC1,PC2を用いて行った自己位置推定と同様の処理を、点群データPCを用いて行えばよい。制御装置21は、第1自己位置候補10Aに移動体10が位置していると仮定して点群データPCと地図データD1との照合を行ってもよい。制御装置21は、第2自己位置候補10Bに移動体10が位置していると仮定して点群データPCと地図データD1との照合を行ってもよい。
【0030】
次に、ステップS16において、制御装置21は、ステップS15の自己位置推定で得られた推定結果を自己位置であると決定する。
ステップS17において、制御装置21は、自己位置候補毎に点群データPCと地図データD1との照合によって信頼度を算出する。本実施形態であれば、第1自己位置候補10A及び第2自己位置候補10Bのそれぞれについて信頼度が算出される。制御装置21は、第1自己位置候補10Aに移動体10が位置していると仮定して点群データPCと地図データD1との照合を行う。制御装置21は、点群データPCに含まれる点Pと地図データD1によって表される構造物O1,O2の形状とが一致しているほど信頼度を高くする。
【0031】
図8に示すように、図5に示す第1自己位置候補10Aに移動体10が位置していると仮定して点群データPCと地図データD1との照合を行うと、全ての点Pが構造物O1,O2に一致する。信頼度を百分率で表すとすると、図5に示す第1自己位置候補10Aの信頼度は、例えば、100%である。
【0032】
図9に示すように、図7に示す第2自己位置候補10Bに移動体10が位置していると仮定して点群データPCと地図データD1との照合を行うと、一部の点Pが構造物O1,O2に一致しない。このため、図7に示す第2自己位置候補10Bの信頼度は、図5に示す第1自己位置候補10Aよりも低い。信頼度を百分率で表すとすると、図7に示す第2自己位置候補10Bの信頼度は、例えば、40%である。
【0033】
図3に示すように、次に、ステップS18において、制御装置21は、最も信頼度が高い自己位置候補を自己位置とする。図5に示す第1自己位置候補10Aと図7に示す第2自己位置候補10Bであれば、制御装置21は、第1自己位置候補10Aを自己位置とする。
【0034】
[比較例]
制御装置21が比較例の自己位置推定制御を行う場合について説明する。制御装置21は、移動体10の仮の自己位置を推定する。仮の自己位置は、点群データPCと地図データD1との照合によって行われる。制御装置21は、仮の自己位置からセンサ25が点群データPCを取得可能な範囲を算出する。そして、制御装置21は、地図データD1のうちセンサ25が点群データPCを取得可能な範囲と点群データPCとの照合によって自己位置を推定する。
【0035】
移動体10が構造物O1と構造物O2との間に位置しているとする。この場合、センサ25が点群データPCを取得可能な範囲には第1表面S1及び第3表面S3が位置している。制御装置21が点群データPCと地図データD1との照合により仮の自己位置を推定した場合に、点群データPCと第2表面S2とがマッチングしたとする。第1表面S1と第3表面S3との間の位置が仮の自己位置として推定された場合、センサ25が点群データPCを取得可能な範囲には第2表面S2が含まれないため、点群データPCと第2表面S2との誤マッチングを抑制できる。
【0036】
図10に示すように、移動体10が構造物O1と構造物O2との間に位置しているにも関わらず第1表面S1と第2表面S2との間の位置が仮の自己位置10Cとして推定されたとする。この場合、センサ25が点群データPCを取得可能な範囲に第1表面S1及び第2表面S2が位置している。このため、地図データD1のうちセンサ25が点群データPCを取得可能な範囲と点群データPCとを照合した場合であっても、点群データPCと第2表面S2との誤マッチングが生じる場合がある。
【0037】
図11に示すように、移動体10が構造物O1と構造物O2との間に位置しているとする。点群データPCの一部が第3表面S3のうち誤った箇所にマッチングすることで、第1表面S1と第3表面S3との間の位置が仮の自己位置10Cとして推定されたとする。この場合、センサ25が点群データPCを取得可能な範囲には第2表面S2が含まれないため、点群データPCと第2表面S2との誤マッチングを抑制できる。しかしながら、図11に示す例では、点群データPCと誤マッチングしているのは第2表面S2ではないため、自己位置推定精度の向上に寄与しない。このように、仮の自己位置を推定し、仮の自己位置からセンサ25が点群データPCを取得可能な範囲を算出したとしても、自己位置と実際の移動体10の位置に乖離が生じる場合がある。
【0038】
[本実施形態の作用]
制御装置21は、分割点群データPC1,PC2毎に自己位置候補を推定する。全ての自己位置候補が一致する場合、当該自己位置候補の精度は高いとみなすことができる。従って、制御装置21は、自己位置候補に基づいて自己位置を決定することができる。
【0039】
制御装置21は、少なくとも1つの自己位置候補が他の自己位置候補と一致しない場合、自己位置候補毎に点群データPCと地図データD1との照合によって信頼度を算出する。図5に示す例であれば、突部Cによって第3表面S3に特徴的な形状が生じているため、第1自己位置候補10Aの信頼度は高くなりやすい。図7に示す例であれば、第1表面S1及び第2表面S2は特徴の少ない平坦面であって、かつ、両者の形状が同一であるため第2分割点群データPC2と第2表面S2とが誤マッチングするおそれがある。このため、第2自己位置候補10Bの信頼度は第1自己位置候補10Aの信頼度より低くなりやすい。制御装置21は、信頼度が最も高い自己位置候補を自己位置とする。これにより、自己位置推定精度を向上させることができる。
【0040】
本実施形態の自己位置推定制御の場合、仮に、自己位置候補の1つが第1表面S1と第2表面S2との間の位置だったとしても、信頼度によって当該自己位置候補が自己位置となることを抑制できる。
【0041】
また、分割点群データPC1,PC2を用いることで、点群データPCの一部が第3表面S3のうち誤った箇所にマッチングすることを抑制できる。図11に示す例では、点群データPCと地図データD1とを照合している。点群データPCのうち特徴のある第3表面S3に対応する点Pよりも、特徴の少ない第1表面S1に対応する点Pの数が多いと、第3表面S3とマッチングする点Pより第1表面S1とマッチングする点Pが多くなる。これにより、点群データPCを用いて自己位置推定を行うと、点群データPCのうち第1表面S1に対応する点Pの影響によって自己位置推定精度が低下するおそれがある。これに対し、分割点群データPC1,PC2を用いて自己位置を推定する場合、点群データPCに特徴のある箇所に対応する点Pが含まれていれば、分割点群データPC1,PC2の少なくとも1つについては特徴のある箇所に対応する点Pによって自己位置候補が推定される。即ち、特徴の少ない箇所に対応する点Pによって希釈されていない分割点群データPC1,PC2を用いて自己位置候補を推定できる。そして、このように推定した自己位置候補は信頼度が高い。このため、他の自己位置候補の信頼度が低い場合であっても、精度の高い自己位置を得ることができる。
【0042】
[本実施形態の効果]
(1)制御装置21は、信頼度が最も高い自己位置候補を自己位置とする。これにより、自己位置推定精度を向上させることができる。
【0043】
(2)制御装置21は、全ての自己位置候補が一致する場合、当該自己位置候補に移動体10が位置していると仮定して点群データPCと地図データD1との照合を行う。点群データPCと地図データD1とをマッチングすることで、自己位置推定候補を自己位置とする場合よりも精度の高い自己位置を得ることができる。このため、自己位置推定精度を更に向上させることができる。
【0044】
[変更例]
実施形態は、以下のように変更して実施することができる。実施形態及び以下の変更例は、技術的に矛盾しない範囲で互いに組み合わせて実施することができる。
【0045】
○制御装置21は、ローカル座標系で表される領域Rを3つ以上の分割領域に分割してもよい。
○制御装置21は、ローカル座標系で表される領域Rを鉛直方向に分割してもよい。
【0046】
○制御装置21は、ローカル座標系で表される領域Rを鉛直方向及び水平方向の両方に分割してもよい。
○制御装置21は、全ての自己位置候補が一致する場合、当該自己位置候補を自己位置としてもよい。実施形態であれば、第1自己位置候補10Aと第2自己位置候補10Bのいずれを自己位置としてもよい。
【符号の説明】
【0047】
A1…環境、D1…地図データ、P…点、PC…点群データ、PC1…第1分割点群データ、PC2…第2分割点群データ、R…領域、R1…第1分割領域、R2…第2分割領域、10…移動体、10A…第1自己位置候補、10B…第2自己位置候補、21…制御装置、24…記憶装置である補助記憶装置、25…センサ。
図1
図2
図3
図4
図5
図6
図7
図8
図9
図10
図11