特開 2024-82790 自己位置推定装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024082790

(43)【公開日】2024-06-20

(54)【発明の名称】自己位置推定装置

(51)【国際特許分類】

   G09B 29/10 20060101AFI20240613BHJP

   G09B 29/00 20060101ALI20240613BHJP

   G01S 17/89 20200101ALI20240613BHJP

   G01S 17/931 20200101ALI20240613BHJP

   G05D 1/43 20240101ALN20240613BHJP

【ＦＩ】

G09B29/10 A

G09B29/00 A

G09B29/00 Z

G01S17/89

G01S17/931

G05D1/02 L

【審査請求】未請求

【請求項の数】6

【出願形態】ＯＬ

(21)【出願番号】P 2022196892

(22)【出願日】2022-12-09

(71)【出願人】

【識別番号】000003218

【氏名又は名称】株式会社豊田自動織機

(74)【代理人】

【識別番号】100088155

【弁理士】

【氏名又は名称】長谷川 芳樹

(74)【代理人】

【識別番号】100113435

【弁理士】

【氏名又は名称】黒木 義樹

(74)【代理人】

【識別番号】100124062

【弁理士】

【氏名又は名称】三上 敬史

(74)【代理人】

【識別番号】100148013

【弁理士】

【氏名又は名称】中山 浩光

(72)【発明者】

【氏名】坂元 優太

【テーマコード（参考）】

2C032

5H301

5J084

【Ｆターム（参考）】

2C032HB11

2C032HC08

2C032HC17

2C032HD29

5H301GG08

5H301LL01

5H301LL03

5J084AB01

5J084AB16

5J084AC02

5J084BA03

(57)【要約】

【課題】自己位置推定に利用される特徴物が少ない環境において、移動体の自己位置の推定精度を向上させることができる自己位置推定装置を提供する。
【解決手段】自己位置推定装置１は、点群地図データに存在しない新規構造物に相当する点群がレーザセンサ３により取得された検出点群データ上にあるかどうかを判断する第１判断部１１と、新規構造物に相当する点群が検出点群データ上にあると判断されたときに、ＨＤマップに基づいて、新規構造物が定常物であるか移動物であるかを判断する第２判断部１３と、新規構造物が定常物であると判断されたときに、新規構造物に相当する点群を点群地図データ上に追加する地図更新部１４と、新規構造物が定常物であると判断されたときに、検出点群データと更新された点群地図データとに基づいて、自車両２の自己位置を推定する自己位置推定部１６とを備える。
【選択図】図１

【特許請求の範囲】

【請求項1】

移動体の自己位置の推定を行う自己位置推定装置において、
前記移動体が走行するエリアの点群地図データと道路情報を含む運転用地図データとを記憶する記憶部と、
前記移動体の周囲に存在する物体を検出し、前記物体の検出点群データを取得する点群検出部と、
前記点群検出部により取得された検出点群データと前記記憶部に記憶された点群地図データとを照合し、前記点群地図データに存在しない新規構造物に相当する点群が前記検出点群データ上にあるかどうかを判断する第１判断部と、
前記第１判断部により前記新規構造物に相当する点群が前記検出点群データ上にあると判断されたときに、前記記憶部に記憶された運転用地図データに基づいて、前記新規構造物が定常物であるか移動物であるかを判断する第２判断部と、
前記第２判断部により前記新規構造物が前記定常物であると判断されたときに、前記新規構造物に相当する点群を前記点群地図データ上に追加することにより、前記点群地図データを更新する地図更新部と、
前記第２判断部により前記新規構造物が前記定常物であると判断されたときに、前記点群検出部により取得された検出点群データと前記地図更新部により更新された点群地図データとに基づいて、前記移動体の自己位置を推定する自己位置推定部とを備える自己位置推定装置。

【請求項2】

前記第２判断部により前記新規構造物が前記移動物であると判断されたときに、前記新規構造物に相当する点群を前記検出点群データ上から除去する点群除去部を更に備え、
前記自己位置推定部は、前記第２判断部により前記新規構造物が前記定常物であると判断されたときは、前記点群検出部により取得された検出点群データと前記地図更新部により更新された点群地図データとに基づいて、前記移動体の自己位置を推定し、前記第２判断部により前記新規構造物が前記移動物であると判断されたときは、前記点群除去部により前記新規構造物に相当する点群が除去された検出点群データと前記記憶部に記憶された点群地図データとに基づいて、前記移動体の自己位置を推定する請求項１記載の自己位置推定装置。

【請求項3】

前記第１判断部により前記新規構造物に相当する点群が前記検出点群データ上にあると判断されたときに、前記運転用地図データにおける前記新規構造物の位置を特定する特定部を更に備え、
前記第２判断部は、前記特定部により特定された前記運転用地図データにおける前記新規構造物の位置に基づいて、前記新規構造物が前記定常物である前記移動物であるかを判断する請求項１記載の自己位置推定装置。

【請求項4】

前記第２判断部は、前記運転用地図データにおいて前記新規構造物が道路の車線上に位置するときは、前記新規構造物が前記移動物であると判断する請求項３記載の自己位置推定装置。

【請求項5】

前記第１判断部により前記新規構造物に相当する点群が前記検出点群データ上にあると判断されたときに、前記運転用地図データにおける前記新規構造物の位置及び寸法を特定する特定部を更に備え、
前記第２判断部は、前記特定部により特定された前記運転用地図データにおける前記新規構造物の位置及び寸法に基づいて、前記新規構造物が前記定常物であるか前記移動物であるかを判断する請求項１記載の自己位置推定装置。

【請求項6】

前記第２判断部は、前記運転用地図データにおいて前記新規構造物が道路の車線外に位置すると共に、前記新規構造物の寸法が予め決められた規定値以上であるときは、前記新規構造物が前記定常物であると判断する請求項５記載の自己位置推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自己位置推定装置に関する。

【背景技術】

【0002】

従来の自己位置推定装置としては、例えば特許文献１に記載されている技術が知られている。特許文献１に記載の自己位置推定装置は、外部センサにより車両の周囲を検出し、検出データにより特定される検出領域に含まれる移動物体を認識し、移動物体が含まれない領域において、ＳＬＡＭのアルゴリズムに従って地図作成処理と車両位置推定処理とを行う。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０２２－８７９１４号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

ところで、ＳＬＡＭ手法を用いて自己位置推定を行うためには、特徴となる形状及び点が必要である。しかし、連続した壁及び直線道路といった単一構造が繰り返される環境では、自己位置推定に利用される特徴物が極めて少ないため、自己位置の推定精度が悪化してしまうという課題がある。

【0005】

本発明の目的は、自己位置推定に利用される特徴物が少ない環境において、移動体の自己位置の推定精度を向上させることができる自己位置推定装置を提供することである。

【課題を解決するための手段】

【0006】

（１）本発明の一態様は、移動体の自己位置の推定を行う自己位置推定装置において、移動体が走行するエリアの点群地図データと道路情報を含む運転用地図データとを記憶する記憶部と、移動体の周囲に存在する物体を検出し、物体の検出点群データを取得する点群検出部と、点群検出部により取得された検出点群データと記憶部に記憶された点群地図データとを照合し、点群地図データに存在しない新規構造物に相当する点群が検出点群データ上にあるかどうかを判断する第１判断部と、第１判断部により新規構造物に相当する点群が検出点群データ上にあると判断されたときに、記憶部に記憶された運転用地図データに基づいて、新規構造物が定常物であるか移動物であるかを判断する第２判断部と、第２判断部により新規構造物が定常物であると判断されたときに、新規構造物に相当する点群を点群地図データ上に追加することにより、点群地図データを更新する地図更新部と、第２判断部により新規構造物が定常物であると判断されたときに、点群検出部により取得された検出点群データと地図更新部により更新された点群地図データとに基づいて、移動体の自己位置を推定する自己位置推定部とを備える。

【0007】

このような自己位置推定装置においては、移動体の周囲に存在する物体が検出され、物体の検出点群データが取得される。そして、検出点群データと予め記憶された点群地図データとが照合され、点群地図データに存在しない新規構造物に相当する点群が検出点群データ上にあるかどうかが判断される。新規構造物に相当する点群が検出点群データ上にあると判断されたときは、予め記憶された運転用地図データに基づいて、新規構造物が定常物であるか移動物であるかどうかが判断される。新規構造物が定常物であると判断されたときは、新規構造物に相当する点群を点群地図データ上に追加することにより、点群地図データが更新される。そして、検出点群データと更新された点群地図データとに基づいて、移動体の自己位置が推定される。このように新規構造物が定常物であるときは、新規構造物が暫定特徴物として利用され、新規構造物に相当する点群を含む検出点群データと新規構造物に相当する点群が追加された点群地図データとを用いて、移動体の自己位置推定が行われることとなる。これにより、自己位置推定に利用される特徴物が少ない環境において、移動体の自己位置の推定精度が向上する。

【0008】

（２）上記の（１）において、自己位置推定装置は、第２判断部により新規構造物が移動物であると判断されたときに、新規構造物に相当する点群を検出点群データ上から除去する点群除去部を更に備え、自己位置推定部は、第２判断部により新規構造物が定常物であると判断されたときは、点群検出部により取得された検出点群データと地図更新部により更新された点群地図データとに基づいて、移動体の自己位置を推定し、第２判断部により新規構造物が移動物であると判断されたときは、点群除去部により新規構造物に相当する点群が除去された検出点群データと記憶部に記憶された点群地図データとに基づいて、移動体の自己位置を推定してもよい。

【0009】

このような構成では、新規構造物が移動物であるときは、新規構造物が暫定特徴物として利用されず、新規構造物に相当する点群が除去された検出点群データと予め記憶された点群地図データとを用いて、移動体の自己位置推定が行われることとなる。このため、移動物である新規構造物の影響による自己位置の推定精度の低下が抑制される。従って、自己位置推定に利用される特徴物が少ない環境において、移動体の自己位置の推定精度が更に向上する。

【0010】

（３）上記の（１）または（２）において、自己位置推定装置は、第１判断部により新規構造物が検出点群データ上にあると判断されたときに、運転用地図データにおける新規構造物の位置を特定する特定部を更に備え、第２判断部は、特定部により特定された運転用地図データにおける新規構造物の位置に基づいて、新規構造物が定常物である移動物であるかを判断してもよい。

【0011】

移動物は、移動体の移動範囲内に存在することが多い。定常物は、移動体の移動範囲内に存在しないことが多い。そこで、運転用地図データにおける新規構造物の位置を特定することにより、新規構造物が定常物であるか移動物であるかの判断が行いやすくなる。

【0012】

（４）上記の（３）において、第２判断部は、運転用地図データにおいて新規構造物が道路の車線上に位置するときは、新規構造物が移動物であると判断してもよい。

【0013】

このような構成では、運転用地図データにおいて道路の車線上に位置する新規構造物を、移動物の一つである走行車両と容易に判断することができる。

【0014】

（５）上記の（１）または（２）において、自己位置推定装置は、第１判断部により新規構造物が検出点群データ上にあると判断されたときに、運転用地図データにおける新規構造物の位置及び寸法を特定する特定部を更に備え、第２判断部は、特定部により特定された運転用地図データにおける新規構造物の位置及び寸法に基づいて、新規構造物が定常物であるか移動物であるかを判断してもよい。

【0015】

移動物は、移動体の移動範囲内に存在することが多い。定常物は、移動体の移動範囲内に存在しないことが多い。また、定常物は、一定以上の寸法を有していることが多い。そこで、運転用地図データにおける新規構造物の位置及び寸法を特定することにより、新規構造物が定常物であるか移動物であるかの判断が行いやすくなる。

【0016】

（６）上記の（５）において、第２判断部は、運転用地図データにおいて新規構造物が道路の車線外に位置すると共に、新規構造物の寸法が予め決められた規定値以上であるときは、新規構造物が定常物であると判断してもよい。

【0017】

このような構成では、運転用地図データにおいて道路の車線外に位置すると共に規定値以上の寸法を有する新規構造物を、定常物の一つである建物と容易に判断することができる。

【発明の効果】

【0018】

本発明によれば、自己位置推定に利用される特徴物が少ない環境において、移動体の自己位置の推定精度を向上させることができる。

【図面の簡単な説明】

【0019】

【図1】本発明の一実施形態に係る自己位置推定装置の構成を示すブロック図である。

【図2】図１に示されたコントローラにより実行される自己位置推定処理の手順を示すフローチャートである。

【図3】自車両が道路を走行する様子を示す平面図である。

【図4】図３に示された道路を自車両が走行する場合に、レーザセンサにより取得される検出点群データを示す概念図である。

【図5】点群地図データに存在しない新規構造物の例を示す平面図である。

【図6】特徴物が無い直線道路と直線道路に存在する新規構造物とを示す平面図である。

【発明を実施するための形態】

【0020】

以下、本発明の実施形態について、図面を参照して詳細に説明する。

【0021】

図１は、本発明の一実施形態に係る自己位置推定装置の構成を示すブロック図である。図１において、本実施形態の自己位置推定装置１は、道路Ｒを走行する自車両２（移動体）に搭載されている（図３参照）。自己位置推定装置１は、自車両２の自動運転時に、自車両２の自己位置の推定を行う装置である。

【0022】

自己位置推定装置１は、レーザセンサ３と、地図メモリ４と、コントローラ１０とを備えている。

【0023】

レーザセンサ３は、自車両２の周囲にレーザを照射し、レーザの反射光を受光することにより、自車両２の周囲に存在する物体２０を検出する（図３参照）。物体２０は、例えば建物、壁、柱または他車両等である。レーザセンサ３の検出エリアＬは、３６０度全周でもよいし（図３参照）、所定の角度範囲（例えば２７０度）でもよい。レーザセンサ３としては、例えばＬＩＤＡＲ（Light Detection and Ranging）またはレーザレンジファインダ等が使用される。

【0024】

レーザセンサ３は、自車両２の周囲に存在する物体２０を検出し、物体２０の検出点群データＤｐ（図４（ｂ）参照）を取得する点群検出部である。点群ＰＧは、レーザセンサ３から照射されたレーザが物体２０の表面に当たって反射する点（反射点）Ｐの集まりである（図４参照）。

【0025】

地図メモリ４は、点群地図記憶部５と、ＨＤマップ記憶部６とを有している。点群地図記憶部５は、自車両２が走行するエリアの点群地図データＤｍ（図４（ａ）参照）を記憶する記憶部である。点群地図データＤｍは、レーザセンサ３を用いて予め生成されている。

【0026】

ＨＤマップ記憶部６は、ベクターマップ等のＨＤマップ（高精度３次元地図）を記憶する記憶部である。ＨＤマップは、道路の車線、路肩縁及び標識等の道路情報を含む３次元デジタル地図データである。ＨＤマップは、自車両２の運転時に使用される運転用地図データである。

【0027】

コントローラ１０は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ１０は、第１判断部１１と、位置・寸法特定部１２（特定部）と、第２判断部１３と、地図更新部１４と、点群除去部１５と、自己位置推定部１６とを有している。

【0028】

第１判断部１１は、点群地図記憶部５に記憶された点群地図データＤｍ（図４（ａ）参照）とレーザセンサ３により取得された検出点群データＤｐ（図４（ｂ）参照）とを照合し、点群地図データＤｍに存在しない新規構造物に相当する点群が検出点群データＤｐ上にあるかどうかを判断する。

【0029】

位置・寸法特定部１２は、第１判断部１１により新規構造物に相当する点群が検出点群データＤｐ上にあると判断されたときに、ＨＤマップ記憶部６に記憶されたＨＤマップ上における新規構造物の位置及び寸法を特定する。

【0030】

第２判断部１３は、新規構造物に相当する点群Ｎが検出点群データＤｐ上にあると判断されたときに、ＨＤマップに基づいて、新規構造物が定常物であるか移動物であるかを判断する。具体的には、第２判断部１３は、第１判断部１１により新規構造物に相当する点群が検出点群データＤｐ上にあると判断されたときに、位置・寸法特定部１２により特定されたＨＤマップ上における新規構造物の位置及び寸法に基づいて、新規構造物が定常物であるか移動物であるかを判断する。

【0031】

地図更新部１４は、第２判断部１３により新規構造物が定常物であると判断されたときに、定常物である新規構造物に相当する点群を点群地図データＤｍ上に追加することにより、点群地図データＤｍを更新する。

【0032】

点群除去部１５は、第２判断部１３により新規構造物が移動物であると判断されたときに、移動物である新規構造物に相当する点群を検出点群データＤｐ上から除去する。

【0033】

自己位置推定部１６は、第２判断部１３により新規構造物が定常物であると判断されたときは、レーザセンサ３により取得された検出点群データＤｐと地図更新部１４により更新された点群地図データＤｍとに基づいて、自車両２の自己位置を推定する。自己位置推定部１６は、第２判断部１３により新規構造物が移動物であると判断されたときは、点群除去部１５により新規構造物に相当する点群が除去された検出点群データＤｐと点群地図記憶部５に記憶された点群地図データＤｍとに基づいて、自車両２の自己位置を推定する。

【0034】

自己位置推定部１６は、ＳＬＡＭ（simultaneous localization andmapping）手法を用いて、検出点群データＤｐと点群地図データＤｍとをマッチングさせて自車両２の自己位置を推定する。ＳＬＡＭは、センサデータ及び地図データを用いて自己位置推定を行う自己位置推定技術である。

【0035】

図２は、コントローラ１０により実行される自己位置推定処理の手順を示すフローチャートである。本処理は、自車両２の自動走行の開始が指示されると実行される。本処理は、一定時間毎（例えば数百ｍｓ毎）に繰り返し実行される。コントローラ１０は、本処理の開始時には、自車両２の初期位置を認識している。

【0036】

図２において、コントローラ１０は、まずレーザセンサ３から出力された検出点群データＤｐを取得する（手順Ｓ１０１）。また、コントローラ１０は、点群地図記憶部５に記憶された点群地図データを読み込む（手順Ｓ１０２）。

【0037】

ここで、点群地図データＤｍは、図３（ａ）に示されるように、レーザセンサ３を用いて、道路Ｒの脇に駐車車両及び停車車両が存在していない状態で生成される。道路Ｒの一方の脇には、車両が駐停車する退避スペースＳｅが設置されている。このため、点群地図データＤｍの生成時には、図４（ａ）に示されるように、道路Ｒの左右両側に配置されたガードレール等の壁２１に相当する点群ＰＧが取得される。なお、点群地図データＤｍの生成時に取得される点群は、グローバルＳＬＡＭ点群と称される。

【0038】

一方、自車両２の自動走行時には、図３（ｂ）に示されるように、道路Ｒの退避スペースＳｅに駐車車両２２が存在していることがある。この場合には、図４（ｂ）に示されるように、レーザセンサ３により壁２１及び駐車車両２２に相当する点群ＰＧが検出点群データＤｐとして取得されることとなる。なお、自車両２の自動走行時に取得される点群は、ローカルＳＬＡＭ点群と称される。

【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】

１…自己位置推定装置、２…自車両（移動体）、３…レーザセンサ（点群検出部）、５…点群地図記憶部（記憶部）、６…ＨＤマップ記憶部（記憶部）、１１…第１判断部、１２…位置・寸法特定部（特定部）、１３…第２判断部、１４…地図更新部、１５…点群除去部、１６…自己位置推定部、２０…物体、２５Ａ～２５Ｃ…新規構造物、Ｄｐ…検出点群データ、Ｄｍ…点群地図データ、Ｍｖ…ＨＤマップ（運転用地図データ）、ＰＧ…点群、ＰＧｎ…点群、Ｒ…道路。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】