特開 2023-182325 移動体の自己位置推定装置

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

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

(11)【公開番号】P2023182325

(43)【公開日】2023-12-26

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

(51)【国際特許分類】

   G05D 1/02 20200101AFI20231219BHJP

   G01C 21/30 20060101ALI20231219BHJP

   G01S 17/89 20200101ALI20231219BHJP

【ＦＩ】

G05D1/02 J

G01C21/30

G01S17/89

【審査請求】未請求

【請求項の数】5

【出願形態】ＯＬ

(21)【出願番号】P 2022095859

(22)【出願日】2022-06-14

(71)【出願人】

【識別番号】000002082

【氏名又は名称】スズキ株式会社

(74)【代理人】

【識別番号】100099623

【弁理士】

【氏名又は名称】奥山 尚一

(74)【代理人】

【識別番号】100125380

【弁理士】

【氏名又は名称】中村 綾子

(74)【代理人】

【識別番号】100142996

【弁理士】

【氏名又は名称】森本 聡二

(74)【代理人】

【識別番号】100166268

【弁理士】

【氏名又は名称】田中 祐

(74)【代理人】

【識別番号】100217076

【弁理士】

【氏名又は名称】宅間 邦俊

(74)【代理人】

【識別番号】100169018

【弁理士】

【氏名又は名称】網屋 美湖

(74)【代理人】

【氏名又は名称】有原 幸一

(72)【発明者】

【氏名】吉岡 皓成

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

2F129

5H301

5J084

【Ｆターム（参考）】

2F129AA03

2F129BB15

2F129BB20

2F129BB22

2F129BB33

2F129BB49

5H301CC03

5H301CC06

5H301GG08

5H301GG14

5H301GG17

5J084AA04

5J084AA10

5J084AB20

5J084AC02

5J084BA03

5J084BA49

5J084CA03

5J084CA12

5J084CA31

5J084CA70

5J084DA01

5J084DA07

5J084DA09

5J084EA04

(57)【要約】

【課題】外界センサによって取得された点群データを用いた自己位置推定において推定精度を向上させる。
【解決手段】移動体１の周囲環境をスキャンして所定周期で点群データを取得する外界センサ１１と、環境地図を格納する地図データベース１３と、を有し、移動体１の位置および姿勢を推定するように構成された移動体１の自己位置推定装置１０であって、外界センサ１１によって取得された点群データを保存するデータ保存部１４１と、外界センサ１１によって新たな点群データが取得された場合に、新たな点群データと、データ保存部１４１に保存された、１周期前の点群データを含むＮ周期分の点群データとに基づいて集合点群データＣＰを生成する集合点群データ生成部１４３と、集合点群データ生成部１４３によって生成された集合点群データＣＰと、環境地図とのマッチングを行うことにより、環境地図における移動体１の位置および姿勢を推定する位置推定部１４４とを備える。
【選択図】図２

【特許請求の範囲】

【請求項1】

移動体の周囲環境をスキャンして所定周期で点群データを取得する外界センサと、
環境地図を格納する地図データベースと、
を有し、移動体の位置および姿勢を推定するように構成された移動体の自己位置推定装置であって、
前記外界センサによって取得された前記点群データを保存するデータ保存部と、
前記外界センサによって新たな点群データが取得された場合に、前記新たな点群データと、前記データ保存部に保存された、１周期前の点群データを含むＮ周期分の点群データとに基づいて集合点群データを生成する集合点群データ生成部と、
前記集合点群データ生成部によって生成された前記集合点群データと、前記環境地図とのマッチングを行うことにより、前記環境地図における前記移動体の位置および姿勢を推定する位置推定部と、
を備える、移動体の自己位置推定装置。

【請求項2】

前記所定周期の１周期における前記移動体の移動量を求める移動量算出部を備え、
前記集合点群データ生成部は、前記移動体の前記移動量を用いて、前記新たな点群データに対し、前記Ｎ周期分の点群データを移動して一体化することにより、前記集合点群データを生成する、請求項１に記載の移動体の自己位置推定装置。

【請求項3】

前記集合点群データ生成部は、前記移動体の前記移動量を用いて前記Ｎ周期分の点群データを移動した後、反復最近接点アルゴリズムを用いて前記新たな点群データに対して前記Ｎ周期分の点群データを位置合わせすることにより、前記集合点群データを生成する、請求項２に記載の移動体の自己位置推定装置。

【請求項4】

前記集合点群データは、前記新たな点群データと前記Ｎ周期分の点群データとを一体化することにより重複した点を含み、
前記位置推定部は、前記環境地図の点群データを格子状に分割して各格子内における正規分布を準備し、各格子内における前記環境地図の点群データの前記正規分布と前記集合点群データとのマッチングを行うことにより、前記移動体の位置および姿勢を推定する、請求項１から３のいずれか一項に記載の移動体の自己位置推定装置。

【請求項5】

前記Ｎは２以上の自然数であり、
前記データ保存部にＮ＋１周期分の点群データが保存されている場合、前記集合点群データ生成部は、前記外界センサによって前記新たな点群データが取得されると、前記データ保存部に保存されている前記Ｎ＋１周期分の点群データのうち、最も古い周期の点群データを削除し、前記最も古い周期の点群データ削除後の前記Ｎ周期分の点群データと前記新たな点群データとに基づいて前記集合点群データを生成する、請求項１から３のいずれか一項に記載の移動体の自己位置推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、移動体の自己位置推定装置に関し、さらに詳しくは、外界センサによって取得された点群データと環境地図とのマッチングを行って自己位置推定を行う装置に関する。

【背景技術】

【0002】

近年、自動運転車、無人搬送車（ＡＧＶ）、および自律移動ロボット（ＡＭＲ）等の自律移動可能な移動体に関する技術の開発が進められている。このような移動体は、目的地へ到達するために自己位置情報を取得することが必要である。移動体の自己位置の推定方法としては、慣性計測装置（ＩＭＵ）やホイールエンコーダを利用したデッドレコニング、全球測位衛星システム（ＧＮＳＳ）、ＬｉＤＡＲ（Light Detection and Ranging）を利用したスキャンマッチング等、さまざまな手法を採用することができる。

【0003】

例えば、特許文献１には、スキャンマッチングを採用した自己位置推定システムが開示されている。このシステムにおいては、環境をスキャンしてスキャンデータを周期的に出力する外界センサから新たにスキャンデータを取得したときに、新たに取得した最新スキャンデータと参照地図とのマッチングを行うことにより、参照地図上における外界センサの位置および姿勢を推定する。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】国際公開番号ＷＯ２０１９／０４４５００号

【発明の概要】

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

【0005】

動的環境下では、移動体は様々な外乱の影響を受ける。例えば、移動体と周囲構造物との間に他車両が存在する状態で外界センサによるスキャンを行った場合、取得された点群データには他車両の検知結果が含まれてしまう。このように、外界センサによって取得された点群データが外乱の影響を受けると、参照地図とのマッチング精度が低下し、移動体の位置および姿勢の推定精度が悪化するおそれがある。

【0006】

本発明は、上記のような実状に鑑みてなされたものであり、その目的は、外界センサによって取得された点群データを用いた自己位置推定において推定精度を向上させることにある。

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

【0007】

本発明の一態様によれば、移動体の周囲環境をスキャンして所定周期で点群データを取得する外界センサと、環境地図を格納する地図データベースと、を有し、移動体の位置および姿勢を推定するように構成された移動体の自己位置推定装置であって、前記外界センサによって取得された前記点群データを保存するデータ保存部と、前記外界センサによって新たな点群データが取得された場合に、前記新たな点群データと、前記データ保存部に保存された、１周期前の点群データを含むＮ周期分の点群データとに基づいて集合点群データを生成する集合点群データ生成部と、前記集合点群データ生成部によって生成された前記集合点群データと、前記環境地図とのマッチングを行うことにより、前記環境地図における前記移動体の位置および姿勢を推定する位置推定部と、を備える。

【発明の効果】

【0008】

本発明に係る移動体の自己位置推定装置は、点群データを用いた自己位置推定における推定精度を向上させることができる。

【図面の簡単な説明】

【0009】

【図1】図１（ａ）～（ｃ）は、外乱による点群データへの影響を説明する図である。

【図2】図２は、本発明の一実施の形態における移動体の自己位置推定装置の概略構成を示す図である。

【図3】図３は、移動体の移動とともに取得される複数周期分の点群データを模式的に示す図である。

【図4】図４は、新たな点群データに対して過去の点群データを移動して一体化する方法を説明する図である。

【図5】図５は、新たな点群データに対して過去の点群データを位置合わせする方法を説明する図である。

【図6】図６は、自己位置推定処理の流れを説明するフローチャートである。

【図7】図７は、集合点群データ生成処理の流れを説明するフローチャートである。

【図8】図８は、複数の外界センサのスキャン範囲を概略的に示す図である。

【図9】図９は、複数周期分の点群データを重ね合わせることによる効果を説明する図である。

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

【0010】

本発明の一実施の形態による移動体の自己位置推定装置は、周囲環境をスキャンする外界センサによって取得される点群データを用いて、高精度な環境地図とのマッチングを行うことにより、環境地図における移動体の位置および姿勢を推定するように構成されている。

【0011】

ここで、動的環境下において、移動体は様々な外乱の影響を受けるため、外界センサによって取得される点群データも外乱の影響を受ける場合がある。例えば、（ｉ）移動体が動的もしくは準動的な障害物で囲まれて、検知対象である周囲構造物が隠されてしまうオクルージョンが発生する場合、（ｉｉ）開けた土地など、検知対象となるランドマークなどの特徴が少ない場合、（ｉｉｉ）移動体のロール・ピッチ運動により外界センサの光軸が水平方向からずれた場合、などが想定される。

【0012】

図１（ａ）に、（ｉ）オクルージョンが発生する場合の具体例を示す。図１（ａ）に示す例においては、移動体の一例として車両１を示し、周囲構造物の例として建物２ａ，２ｂおよび樹木２ｃを示している。動的もしくは準動的な障害物の例として、隣接車線上のトラック３ａとバス３ｂを示しており、車両１は車両１よりも大きな障害物で囲まれている。外界センサによる１周期分のスキャン範囲４内では、トラック３ａおよびバス３ｂによって建物２ａ，２ｂおよび樹木２ｃが少なくとも部分的に隠されてしまうため、建物２ａの一部に対応する点群しか取得することができない。飛来物などで外界センサが一時的に覆われた場合にも、オクルージョンが発生し得る。

【0013】

図１（ｂ）に、（ｉｉ）ランドマークなどの特徴が少ない場合の具体例を示す。図１（ｂ）に示すように、田んぼや畑のあぜ道、湖畔や海岸線を通る道など、開けた土地を走行する場合は、１周期分のスキャン範囲４内に建物２ａ，２ｂおよび樹木２ｃなどの周囲構造物が存在しないことがある。このような場合は、周囲構造物の点群を得ることができない。

【0014】

図１（ｃ）に、（ｉｉｉ）車両１のロール・ピッチ運動によって外界センサの光軸が水平方向からずれた場合の具体例を示す。外界センサは、水平方向に対して上下方向に所定角度の範囲でスキャンを行うことができるものがあるが、例えば、車両１が段差を乗り越えるときのピッチ運動、または車両１が急なカーブを走行するときのロール運動により、外界センサの光軸が水平方向からずれて空や地面方向を向いてしまう場合がある。このような場合、１周期分のスキャン範囲４内に周囲構造物の一例として建造物２ｄが存在していても、建造物２ｄの一部に対応する点群しか取得できない。

【0015】

上述した（ｉ）～（ｉｉｉ）の場合のように点群データが外乱の影響を受けた場合、外界センサによって取得された１周期分の点群データでは周囲構造物に対応する十分な点群を得ることができない。点群が少ないと、環境地図とのマッチング処理が収束しにくくなり、移動体の自己位置の推定精度が低下してしまう。

【0016】

そこで、本実施の形態に係る移動体の自己位置推定装置においては、外界センサによって取得される複数周期の点群データを重ね合わせて集合点群データを生成し、生成した集合点群データを用いて高精度な環境地図とのマッチングを行うことにより、環境地図における移動体の位置および姿勢を高精度に推定する。以下に、図面を参照して、本実施の形態に係る移動体の自己位置推定装置について詳細に説明する。

【0017】

図２は、本実施の形態における移動体の自己位置推定装置１０の概略構成を示す図である。以下では、移動体として車両１に自己位置推定装置１０を搭載した例を説明する。車両１に搭載される自己位置推定装置１０は、車両周囲環境を検知する外界センサ１１、車両情報を検知する内界センサ１２、地図情報データベース１３、および自己位置推定装置１０の全体の制御を行うコントローラ１４等を備えている。

【0018】

外界センサ１１は、車両１の周囲環境をスキャンして所定周期でスキャンデータを取得するセンサである。外界センサ１１としては、例えばＬｉＤＡＲ（Light Detection and Ranging）、レーザレンジファインダ、ミリ波レーダなどがあるが、ここではＬｉＤＡＲを用いる例を説明する。

【0019】

ＬｉＤＡＲは、パルス状のレーザ光を対象物に照射し、対象物で反射された反射光を測定することにより、対象物までの距離や方向を測定してスキャンデータを取得する。ＬｉＤＡＲのスキャンデータは、光と対象物との交点を点群データとして抽出したものであり、点群データは、外界センサ１１が設置される車両１におけるローカル座標系によって規定される位置座標を持つ点の集合体である。車両１の位置は、ローカル座標系の原点として定義され、車両１の姿勢（向き）は、ローカル座標系の向きとして定義することができる。なお、上述した（ｉｉ）のケースのように、スキャン範囲４内に周囲構造物が存在しない場合、点群データに点群が含まれない場合もあり得る。

【0020】

外界センサ１１は、例えば車両１の上部に設置され、回転しながらパルス状のレーザ光を照射し、車両１の周囲３６０度をスキャンすることができる。外界センサ１１によって取得された点群データは、コントローラ１４に入力される。

【0021】

内界センサ１２は、車速センサ（例えば、車輪速センサ）、ヨーレイトセンサ（例えば、ジャイロセンサ）などを含む。内界センサ１２によって取得された各測定値は、コントローラ１４に入力される。

【0022】

地図データベース１３は、予め準備された高精度な環境地図を格納している。自己位置の高精度な推定のために、環境地図として３次元地図を用いることが好ましいが、マッチングの手法に合わせて２次元地図を用いることも可能である。

【0023】

コントローラ１４は、データ保存部１４１、移動量算出部１４２、集合点群データ生成部１４３、および、位置推定部１４４を含む。コントローラ１４は、コンピュータ、すなわち、プログラム及びデータを記憶したＲＯＭ、演算処理を行うＣＰＵ、前記プログラム及びデータを読出し、動的データや演算処理結果を記憶するＲＡＭ、および、入出力インターフェースなどで構成され、以下に説明する自己位置推定装置１０の制御を実行する。

【0024】

データ保存部１４１は、外界センサ１１によって取得された点群データを保存するメモリである。データ保存部１４１は、外界センサ１１によって取得された複数周期分の点群データをタイムスタンプとともに保存可能に構成されている。複数周期分の点群データは、時間軸上の連続するタイミング（フレーム）で取得された複数フレームのデータである。データ保存部１４１は、複数周期分の点群データとして、複数周期の点群データを重ね合わせた集合点群データを保存することもできる。

【0025】

移動量算出部１４２は、内界センサ１２から入力される測定値に基づいて、外界センサ１１によるスキャンの１周期、すなわち１サンプリング周期（時間間隔）Δｔにおける車両１の移動量を求める。車両１の移動量は、具体的には、１サンプリング周期における車両１の移動量であり、車速センサによって検出される車速Ｖｉ×サンプリング周期Δｔから算出される移動距離Δｌｉ、およびジャイロセンサによって検出されるヨーレイトγｉ×サンプリング周期Δｔから算出されるヨー角Δψｉを求めることができる。

【0026】

集合点群データ生成部１４３は、外界センサ１１によって取得された複数周期分の点群データを重ね合わせて集合点群データを生成する。具体的には、集合点群データ生成部１４３は、外界センサ１１によって新たな点群データが取得された場合に、新たな点群データと、データ保存部１４１に保存された、１周期前の点群データを含むＮ周期分の点群データとに基づいて、集合点群データを生成する。Ｎは１以上の任意の自然数とすることができる。すなわち、集合点群データは、時系列的に連続する複数フレームの点群データの集合体である。

【0027】

位置推定部１４４は、集合点群データ生成部１４３によって生成された集合点群データと、地図データベース１３の環境地図とのマッチングを行うことにより、環境地図における車両１の位置および姿勢を推定する。

【0028】

ここで、集合点群データの生成処理の詳細を説明する。図３に、時間軸上で連続する複数周期分の点群データ、すなわち複数フレームの点群データを模式的に示す。図３に示すように、車両１の移動とともに、車両１に設置された外界センサ１１のスキャン範囲４も移動する。スキャン範囲４は、車両１の移動に伴って、１周期目の時点ｔ１におけるスキャン範囲４ａ、２周期目の時点ｔ２におけるスキャン範囲４ｂ、３周期目の時点ｔ３におけるスキャン範囲４ｃ、４周期目の時点ｔ４におけるスキャン範囲４ｄ、のように移動する。

【0029】

１周期目では、スキャン範囲４ａと周囲構造物２との交点が時点ｔ１における点群データＰ（ｔ１）として抽出される。同様に、２周期目では、スキャン範囲４ｂと周囲構造物２との交点が時点ｔ２における点群データＰ（ｔ２）として抽出され、３周期目では、スキャン範囲４ｃと周囲構造物２との交点が時点ｔ３における点群データＰ（ｔ３）として抽出され、４周期目では、スキャン範囲４ｄと周囲構造物２との交点が時点ｔ４における点群データＰ（ｔ４）として抽出される。点群データＰ（ｔ１）、Ｐ（ｔ２）、Ｐ（ｔ３）、Ｐ（ｔ４）を構成する各点はそれぞれ、白丸、黒丸、白三角、黒三角で表している。

【0030】

外界センサ１１のスキャン時間間隔は例えば数ミリ秒と短いため、図３に示すように、時間的に連続するタイミングでのスキャン範囲４ａ～４ｄは一部が互いに重複している。したがって、スキャン範囲４ａ～４ｄで得られる点群データＰ（ｔ１）～Ｐ（ｔ４）も一部が互いに重複している。

【0031】

上述したように、車両１が外乱の影響を受けた場合、時点ｔ１～ｔ４で取得される点群データＰ（ｔ１）～Ｐ（ｔ４）のそれぞれにおいて、十分な点群が得られない可能性がある。そこで、複数フレームの点群データを重ね合わせることにより、十分な数の点群を有する集合点群データＣＰを生成する。

【0032】

複数フレームの点群データの重ね合わせは、大まかな位置合わせと詳細な位置補正の２段階で行われる。まず、大まかな位置合わせのため、車両１の移動量を用いて、新たな点群データに対し、過去の少なくとも１つの点群データを移動して一体化する。複数フレームの点群データは、それぞれ異なる時点ｔにおける車両１のローカル座標系によって規定される位置座標を持っている。そこで、新たな点群データのローカル座標系を基準として過去の点群データを一体化させるために、車両１の１サンプリング周期における移動量を用いて、過去の点群データを並進および回転させる。

【0033】

図４に、図３に示した３周期目の点群データＰ（ｔ３）に対し、２周期目の点群データＰ（ｔ２）と１周期目の点群データＰ（ｔ１）を一体化する例を示す。ここでは、３周期目の点群データＰ（ｔ３）が、上述した新たな点群データに相当する。

【0034】

車両１の移動量は、上述したように移動量算出部１４２で算出される前回周期からの車両１の移動距離Δｌｉおよびヨー角Δψｉを用いることができる。まず、３周期目の点群データＰ（ｔ３）を基準として、３周期目の時点ｔ３における移動距離Δｌｉ（ｔ３）とヨー角Δψｉ（ｔ３）を用いて、２周期目の点群データＰ（ｔ２）を並進および回転（剛体変換）させる。さらに、３周期目の点群データＰ（ｔ３）を基準として、２周期目の時点ｔ２における移動距離Δｌｉ（ｔ２）とヨー角Δψｉ（ｔ２）を用いて、１周期目の点群データＰ（ｔ１）を並進および回転させる。これにより、１周期目から３周期目の点群データＰ（ｔ１）～Ｐ（ｔ３）が一体化される。

【0035】

なお、車両１の移動量を用いて点群データを一体化する手法としては、上述したように新たな点群データに対してＮ周期分の点群データを１周期分ずつ移動させるものには限定されない。例えば、前回周期において複数の点群データを重ね合わせた集合点群データがすでに生成されている場合は、前回周期で生成した集合点群データをまとめて、新たな点群データに対して移動することもできる。この場合、新たな点群データを取得した時点ｔにおける移動距離Δｌｉ（ｔ）とヨー角Δψｉ（ｔ）を用いて、前回周期で生成した集合点群データを並進および回転させる。

【0036】

ここで、車両１の移動量は、車速センサやヨーレイトセンサの測定値に基づいて算出されるため、車両１のタイヤスリップの発生などの影響を受けてしまう。そのため、車両１の移動量を用いた点群データの重ね合わせだけでは、正確な位置合わせを行うことができない可能性がある。

【0037】

そこで、車両１の移動量を用いた一体化を行った後、複数フレームの点群データ間の位置ずれが残っている場合に、さらに、点群データの位置補正を行う。具体的には、反復最近接点（ＩＰＣ：Iterative Closest Point）アルゴリズムを用いて、新たな点群データに対して過去の点群データを位置合わせする。これにより、複数フレームの点群データを重ね合わせた集合点群データＣＰが生成される。

【0038】

一体化した点群データの位置補正を行うためのＩＣＰアルゴリズムは、２つの点群データ間の距離を最小化する剛体変換（並進、回転）を繰り返し行うことによって２つの点群データを整列させるものである。具体的には、例えば図５に示す２つの点群データＰ（ｔ３），Ｐ（ｔ２）に含まれる点群における最近傍点を探索して仮の対応点を決定し、対応点間距離を誤差として誤差の二乗和を最小化するような剛体変換量を推定する。この場合、新たな点群データＰ（ｔ３）を基準として、過去の点群データＰ（ｔ２）を移動する。繰り返し計算により誤差の二乗和が所定値以内に収束すると、位置合わせが完了する。

【0039】

ＩＣＰアルゴリズムを用いた位置合わせは、初期値に大きく依存するため、初期値が最適解から大きくずれていると位置合わせが収束しない可能性がある。しかし、上述したように車両１の移動量を用いた点群データの一体化を行ってからＩＣＰアルゴリズムを用いた位置補正を行うので、良好な位置合わせを行うことができる。これにより、移動量を用いた点群データの一体化後に残っていた位置ずれを解消し、良好な位置合わせが行われた集合点群データＣＰを生成することができる。

【0040】

以下に、図６を参照して、本実施の形態における車両１の自己位置推定処理について詳細に説明する。図６に示す処理は、自己位置推定装置１０のコントローラ１４において所定周期で繰り返し実行され、例えば、自己位置推定装置１０の結果が出力される不図示の自動走行システムの動作がオフとなると終了する。

【0041】

図６に示すように、コントローラ１４は、（１）集合点群データ生成処理（Ｓ１０）、（２）マッチング処理（Ｓ２０）、および（３）推定結果出力（Ｓ３０）を実行する。以下、各ステップにおける処理を説明する。

【0042】

（１）集合点群データ生成処理（Ｓ１０）
コントローラ１４は、外界センサ１１から取得される新たな点群データと、データ保存部１４１に保存された直近のＮ周期分の点群データとに基づいて、Ｎ＋１個の点群データからなる集合点群データを生成する。Ｎは１以上の任意の自然数とすることができるが、ここでは、Ｎ＝２である例について説明する。なお、外界センサ１１は、水平方向に加えて、上下方向においても所定角度の範囲でスキャンを行い、３次元の点群データを取得可能に構成することができるが、以下では説明を簡単にするため、外界センサ１１で取得される点群データを地面から所定の高さにおける２次元のデータとして説明する。

【0043】

図７のフローチャートを参照して、集合点群データ生成処理の流れを説明する。以下では、図３に示すように、時点ｔ４において新たな点群データＰ（ｔ４）が取得されたと仮定して説明する。まずステップＳ１０１で、外界センサ１１から新たな点群データＰ（ｔ４）を取得するとともに、内角センサ１２から車速およびヨーレイト等の測定値を取得する。コントローラ１４には、これらのデータとともにタイムスタンプも入力される。

【0044】

ステップＳ１０２では、前回周期以前に取得されてデータ保存部１４１に保存されている点群データのフレーム数がＮ個以上であるか否かを判定する。保存されている点群データのフレーム数がＮ個よりも少ない場合は、ステップＳ１１０へ進む。一方、保存されている点群データのフレーム数がＮ個以上の場合は、ステップＳ１０３へ進み、保存されている点群データのフレーム数がＮ個であるか否かを判定する。フレーム数＝Ｎである場合は、ステップＳ１０５へ進む。一方、ステップＳ１０３が否定判定され、フレーム数がＮ個よりも大きい場合は、ステップＳ１０４へ進む。

【0045】

ステップＳ１０４では、データ保存部１４１に保存されている複数フレームの点群データのうち、タイムスタンプにより最も古い周期の点群データを特定し、削除する。例えば、図３に示した時点ｔ１～ｔ３における点群データＰ（ｔ１）～Ｐ（ｔ３）がデータ保存部１４１に保存されていた場合、最も古い周期の時点ｔ１における点群データＰ（ｔ１）が削除される。これにより、データ保存部１４１には、Ｎ個、すなわち２個の点群データＰ（ｔ２）、Ｐ（ｔ３）が保存されていることになる。

【0046】

つづくステップＳ１０５では、移動量算出部１４２によって、車両１の移動量を算出する。具体的には、ステップＳ１０１で入力された内角センサ１２からの測定値に基づいて、車両１の移動量として、最新の時点ｔ４における車両１の移動距離Δｌｉ（ｔ４）およびヨー角Δψｉ（ｔ４）を求める。

【0047】

ステップＳ１０６では、ステップＳ１０１で入力された新たな点群データＰ（ｔ４）に対し、データ保存部１４１に保存されている過去の点群データＰ（ｔ２）、Ｐ（ｔ３）を移動して一体化する。具体的には、上述したように時点ｔ４における車両１の移動距離Δｌｉ（ｔ４）およびヨー角Δψｉ（ｔ４）を用いて過去の点群データＰ（ｔ２）、Ｐ（ｔ３）を並進および回転し、新たな点群データＰ（ｔ４）に一体化させる。

【0048】

具体的には、時点ｔ４における車両１の移動距離Δｌｉ（ｔ４）およびヨー角Δψｉ（ｔ４）を用いて、前回周期の時点ｔ３において生成され、上記ステップＳ１０４において最も古い時点ｔ１における点群データＰ（ｔ１）が削除された集合点群データＣＰ（ｔ３）を並進および回転させる。

【0049】

なお、上述したように、新たな点群データＰ（ｔ４）に対してＮ周期分の点群データＰ（ｔ２）、Ｐ（ｔ３）を１周期分ずつ並進および回転させることにより、一体化を行ってもよい。例えば、上述したステップＳ１０４を経由していない場合、すなわち、前回周期においてＮ＋１個の点群データからなる集合点群データＣＰ（ｔ３）が生成されていない場合は、新たな点群データＰ（ｔ４）に対してＮ周期分の点群データＰ（ｔ２）、Ｐ（ｔ３）を１周期分ずつ移動させる。

【0050】

つづくステップＳ１０７では、ステップＳ１０６で一体化した点群データＰ（ｔ２）、Ｐ（ｔ３）、Ｐ（ｔ４）について、ＩＰＣアルゴリズムを用いた位置補正を行う。上述したように、新たな点群データＰ（ｔ４）を基準として、過去の点群データＰ（ｔ２）、Ｐ（ｔ３）の距離を最小化するように繰り返し剛体変換を行う。

【0051】

具体的には、上記ステップＳ１０６における一体化と同様に、前回周期の時点ｔ３において生成され、上記ステップＳ１０４において最も古い時点ｔ１における点群データＰ（ｔ１）が削除された集合点群データＣＰ（ｔ３）を、新たな点群データＰ（ｔ４）に対して位置合わせする。また、新たな点群データＰ（ｔ４）に対して、Ｎ周期分の点群データＰ（ｔ２）、Ｐ（ｔ３）を１周期分ずつ位置合わせしてもよい。

【0052】

これにより、新たな点群データＰ（ｔ４）に対して過去のＮ個の点群データＰ（ｔ２）、Ｐ（ｔ３）を一体化した集合点群データＣＰ（ｔ４）が生成される。

【0053】

ステップＳ１０８では、ステップＳ１０７で生成された集合点群データＣＰ（ｔ４）に対し、必要に応じてなまし処理を行う。ここで、なまし処理としては、例えば、点群データを曲線または曲面に近似するための処理や、ノイズ除去のための平滑化処理などを含む。

【0054】

ステップＳ１０９では、生成された集合点群データＣＰ（ｔ４）を出力する。出力された集合点群データＣＰは、位置推定部１４４におけるマッチングに使用される。

【0055】

ステップＳ１１０では、生成した集合点群データＣＰ（ｔ４）をデータ保存部１４１に保存する。このとき、集合点群データＣＰ（ｔ４）は、集合点群データＣＰ（ｔ４）を構成するＮ個の点群データＰ（ｔ２）、Ｐ（ｔ３）、Ｐ（ｔ４）のそれぞれのタイムスタンプと紐づけられて保存される。これにより、後の処理において最も古い周期の点群データを特定することが可能となる。また、上述したステップＳ１０２において、保存されている点群データのフレーム数がＮよりも少ないと判定された場合は、ステップＳ１０１で取得された点群データＰ（ｔ４）をタイムスタンプとともにデータ保存部１４１に保存する。

【0056】

なお、新たな点群データＰ（ｔ４）に対してＮ周期分の点群データＰ（ｔ２）、Ｐ（ｔ３）を１周期分ずつ並進および回転させる場合は、集合点群データＣＰ（ｔ４）そのものとして保存する代わりに、集合点群データＣＰ（ｔ４）を構成するＮ個の点群データＰ（ｔ２）、Ｐ（ｔ３）、Ｐ（ｔ４）を保存するように構成してもよい。

【0057】

（３）マッチング処理（Ｓ２０）
位置推定部１４４は、ステップＳ１０で生成された集合点群データＣＰと、地図データベース１３に格納された環境地図とのマッチングを行うことにより、環境地図における車両１の位置および姿勢を推定する。環境地図とのマッチングは、種々のアルゴリズムによるスキャンマッチングを利用することができる。上述したＩＣＰアルゴリズムもスキャンマッチングの一例であるが、ここでは、ＮＤＴ（Normal Distribution Transform）アルゴリズムを用いてマッチングを行う。

【0058】

ＮＤＴアルゴリズムは、環境地図の点群データを格子状に分割して各格子内における正規分布を準備し、各格子内における環境地図の点群データの正規分布と集合点群データＣＰとのマッチングを行うことにより、車両１の位置および姿勢を推定する。

【0059】

具体的には、環境地図において車両１の周囲の探索区間内にある点群データを一定の大きさの格子状に分割する。点群データが３次元の場合は、ボクセルに分割し、点群データが２次元の場合は、グリッドに分割する。分割された各格子内の点群データの平均と分散を算出し、点群データを正規分布で表現する。そして、並進および回転を表すパラメータを含む変換行列によって、集合点群データＣＰの座標系を環境地図の点群データの座標系に変換する。変換後に点群データ同士がどの程度マッチングしているかを評価し、評価値が所定値以内に収束するまで座標変換を繰り返す。これにより、マッチングが完了する。

【0060】

マッチングにより得られた環境地図上における車両１の位置は、環境地図の座標系における車両１のローカル座標系の原点の座標値として表される。また、車両１の姿勢（向き）は、環境地図の座標系に対する車両１のローカル座標系の向き（角度）として表される。

【0061】

上述したＩＣＰアルゴリズムは、最新の点群データに対する、過去の点群データの各点の誤差を算出するため、点群の数が多くなるほど計算量が増え、コントローラ１４の計算負荷が高まる。これに対し、ＮＤＴアルゴリズムは、点群が存在する環境地図の探索区間を格子状に分割し、各格子において求めた正規分布を用いて点群の表現およびマッチングを行うため、ＩＣＰに比べて計算量を削減することができる。

【0062】

（３）推定結果出力（Ｓ３０）
位置推定部１４４は、ステップＳ２０で推定した車両１の位置および姿勢を、例えば車両１の不図示の自動走行システムに出力する。また、自己位置推定装置１０は、車両１の位置および姿勢を保存するように構成してもよく、これにより、車両１の位置および姿勢の履歴が保存される。

【0063】

上述したように、最新の点群データに対し、過去のＮ周期分の点群データを重ね合わせることにより、外乱等の影響によって１周期分の点群データでは取得することができなかった点群を得ることが可能となる。例えば、図９に示すように、最新の点群データＰ（ｔ）と過去の点群データＰ（ｔ－１）とを重ね合わせた場合、図１（ａ）に示すような、（ｉ）オクルージョンが発生する状況では検知することができなかった建物２ｂおよび樹木２ｃに対応する点群を取得することが可能となる。

【0064】

また、以上では、点群データが２次元のデータである例を説明したが、点群データが３次元のデータである場合でも、同様の手法により集合点群データを生成することができる。この場合は、図１（ｃ）に示すような、（ｉｉｉ）移動体のロール・ピッチ運動により外界センサの光軸が水平方向からずれた場合においても、建造物２ｄに対応する点群を得ることが可能となる。

【0065】

以上説明した本実施の形態による車両１の自己位置推定装置１０においては、以下のような作用効果を奏することができる。

【0066】

（１）移動体の自己位置推定装置１０は、車両１の周囲環境をスキャンして所定周期で点群データを取得する外界センサ１１と、環境地図を格納する地図データベース１３と、を有し、移動体の位置および姿勢を推定するように構成されている。自己位置推定装置１０は、さらに、外界センサ１１によって取得された点群データを保存するデータ保存部１４１と、外界センサ１１によって新たな点群データが取得された場合に、新たな点群データと、データ保存部１４１に保存された、１周期前の点群データを含むＮ周期分の点群データとに基づいて集合点群データを生成する集合点群データ生成部１４３と、集合点群データ生成部１４３によって生成された集合点群データと、環境地図とのマッチングを行うことにより、環境地図における車両１の位置および姿勢を推定する位置推定部１４４と、を備えている。

【0067】

新たな点群データと、1周期前の点群データを含むＮ周期分の点群データとに基づいて時間軸上で連続する複数の点群データからなる集合点群データを生成することにより、外乱が発生した場合であっても、十分な点群を得ることが可能となる。これにより、環境地図とのマッチングを良好に行うことができ、車両１の自己位置推定における推定精度を向上させることができる。図１（ａ）に示した（ｉ）オクルージョンの発生状況や、図１（ｂ）に示した（ｉｉ）ランドマークの少ない場所においても、複数の点群データからなる集合点群データを生成することにより、より多くのシーンで自己位置の推定が可能となる。また、図１（ｃ）に示した（ｉｉｉ）ロール・ピッチ運動のような外乱が発生する状況においても、ピッチ発生前の時点における点群データを含む集合点群データを生成することが可能となり、ロール・ピッチ運動に対してロバストな自己位置推定を行うことが可能となる。

【0068】

また、複数周期分の点群データを重ね合わせることで、高精度な環境地図とのマッチングに利用する点群数の増加につながるので、外界センサ１１の回転周期や、外界センサ１１から発射されるレーザ光のレイヤ（例えば、レーザ光の発光間隔、発光時間、照射方向の数など）を落とすことができる。これにより、外界センサ１１のハードウェア要件を緩和することができ、自己位置推定装置１０全体としてのコストを削減することが可能となる。

【0069】

（２）所定周期の１周期における車両１の移動量を求める移動量算出部１４２をさらに備え、集合点群データ生成部１４３は、車両１の移動量を用いて、新たな点群データに対し、Ｎ周期分の点群データを移動して一体化することにより、集合点群データを生成する。外乱の発生により、時間軸上で連続する複数フレームの点群データから得られる点群の点配置にばらつきが生じるおそれがある。複数フレームの点群データの点群同士が大きくずれてしまうと、点群データの重ね合わせが困難になり、集合点群データの生成に時間がかかってしまう。そこで、１周期、すなわち時間軸上で連続するスキャンタイミング間の車両１の移動量を利用して、連続する点群データを一体化することにより、点群同士がばらつくような場合でも大まかな位置合わせを行うことができる。また、車両１の移動量を用いることにより、コントローラ１４の計算負荷を低減することができる。

【0070】

（３）集合点群データ生成部１４３は、車両１の移動量を用いてＮ周期分の点群データを移動した後、反復最近接点（ＩＣＰ）アルゴリズムを用いて新たな点群データに対してＮ周期分の点群データを位置合わせすることにより、集合点群データを生成する。車両１の移動量を用いて点群データの一体化を行った後、さらに、複数フレームの点群データの対応点間距離（誤差）を小さくするように点群データの位置を補正するので、より精度の高い集合点群データの生成が可能となり、環境地図とのマッチングによる車両１の自己位置の推定精度を向上させることができる。

【0071】

（４）集合点群データは、新たな点群データとＮ周期分の点群データとを一体化することにより重複した点を含んでおり、位置推定部１４４は、ＮＤＴアルゴリズムを利用して、環境地図の点群データを格子状に分割して各格子内における正規分布を準備し、各格子内における環境地図の点群データの正規分布と集合点群データとのマッチングを行うことにより、車両１の位置および姿勢を推定する。集合点群データにおける重複する点の存在は、その点の信頼度の高さを表すものであり、重複する点を維持してＮＤＴアルゴリズムを用いたマッチングを行うことにより、自己位置の推定精度を高めることができる。

【0072】

（５）Ｎが２以上の自然数である場合であって、データ保存部１４１にＮ＋１周期分の点群データが保存されている場合、集合点群データ生成部１４３は、外界センサ１１によって新たな点群データが取得されると、データ保存部１４１に保存されているＮ＋１周期分の点群データのうち、最も古い周期の点群データを削除し、最も古い周期の点群データ削除後のＮ周期分の点群データと新たな点群データとに基づいて集合点群データを生成する。これにより、集合点群データは、常に所定の数（Ｎ＋１個）の点群データが一体化された集合体となり、環境地図とのマッチングにおけるコントローラ１４の計算負荷の増加を抑制できる。点群データの削除の際には、集合点群データに含まれる点群の数をカウントすることなく、時間軸上において最も古い周期の点群データを一括して削除すればよいので、コントローラ１４の計算負荷の増加を抑制することができる。

【0073】

－変形例－
（１）上述した実施の形態においては、外界センサ１１を、車両１の上部に設置して回転しながら車両１の周囲３６０度をスキャンするように構成したが、外界センサ１１の構成はこれには限定されない。例えば、外界センサ１１として、車両１に複数のＬｉＤＡＲを固定し、それぞれ特定の方向をスキャンする複数のＬｉＤＡＲからの点群データを融合して１周期分の点群データとして取得するように構成することもできる。

【0074】

この場合、例えば図８に示すように、車両１の前方の所定範囲４１をスキャンする外界センサ１１１，車両１の左側方の所定範囲４２をスキャンする外界センサ１１２，および車両１の右側方の所定範囲４３をスキャンする外界センサ１１３を車両１に設置することができる。外界センサ１１１，１１２，１１３によって取得される点群データは、それぞれのローカル座標系であるセンサ座標系で規定されるため、点群データを融合するためには、各センサ座標系から車両１のローカル座標系である車体座標系に変換する必要がある。

【0075】

以下の（式１）に、センサ座標系から車体座標系への変換行列を示す。

【数1】

（式１）
（式１）において、ｉは各外界センサ１１１，１１２，１１３に割り当てる番号、θは車体座標系から各センサ座標系への回転角、Ｔｘｉは車体座標系の原点からみた各外界センサ座標系の原点のｘ座標、Ｔｙｉは、車体座標系の原点からみた各外界センサ座標系の原点のｙ座標である。

【0076】

なお、外界センサ１１１～１１３によって３次元の点群データを取得する場合は、３次元回転行列を用いて座標変換を行えばよい。また、車両１の後方の所定範囲をスキャンする外界センサをさらに設け、外界センサ１１１～１１３と合わせて車両１の周囲３６０度をスキャンするように構成してもよい。

【0077】

（２）上述した実施の形態においては、複数フレームの点群データの重ね合わせを、車両１の移動量を用いた大まかな位置合わせと，ＩＣＰアルゴリズムを用いた詳細な位置補正の２段階で行った。ただし、これには限定されず、移動量を用いた位置合わせを行わず、ＩＣＰアルゴリズムを用いた位置合わせのみを行うように構成してもよい。移動量を用いた位置合わせは、ＩＣＰアルゴリズムを用いた位置合わせのための初期値設定としての役割もあるので、点群データ同士のばらつきがそれほど大きくない場合は、移動量を用いた位置合わせを行わなくても、複数周期分の点群データの良好な一体化が期待できる。

【0078】

（３）上述した実施の形態においては、内界センサ１２によって測定される車速（移動体の速度）とヨーレイト（角速度）から、移動量を算出したが、移動量の算出に移動体の加速度を含めてもよい。また、ジャイロセンサを用いず、車両１の前輪舵角と車速からヨーレイトを推定することも可能である。

【0079】

（４）上述した実施の形態においては、環境地図と集合点群データのマッチングに、ＮＤＴアルゴリズムを利用した。ただし、これには限定されず、例えばＩＣＰアルゴリズムなど、別のアルゴリズムを利用してマッチングを行うこともできる。

【0080】

（５）上述した実施の形態においては、図６に示すフローチャートを用いて集合点群データ生成処理とマッチング処理について説明したが、これらの処理周期は、同じであっても、それぞれ異なっていてもよい。一般的に、マッチング処理は計算負荷が高いため、集合点群データ生成処理よりも遅い周期で処理を実行するようにしてもよい。例えば、集合点群データ生成処理を１０ｍｓ周期で実行し、マッチング処理は１００ｍｓ周期で行うこともできる。

【0081】

（６）上述した実施の形態においては、外界センサ１１によって新たな点群データが取得されると、データ保存部１４１に保存されているＮ＋１周期分の点群データのうち、最も古い周期の点群データを削除するように構成した。ただし、これには限定されず、例えば、データ保存部１４１に保存されているデータ容量が所定の上限値に達すると、最も古い周期の点群データを削除するように構成してもよい。この場合、最も古い周期の点群データの削除後にデータ保存部１４１に保存されている複数周期分の点群データの中から、Ｎ周期分の点群データを読み出して新たな点群データとの重ね合わせに利用することができる。

【0082】

（７）上述した実施の形態においては、コントローラ１４がデータ保存部１４１を含む構成を説明したが、これには限定されず、データ保存部１４１を、コントローラ１４の外部のメモリとして構成してもよい。

【0083】

（８）上述した実施の形態においては、新たな点群データを重ね合わせる点群データのフレーム数Ｎについて、Ｎ＝２の場合を例として説明したが、これには限定されず、Ｎは１であっても、３以上の自然数であってもよい。

【0084】

（９）上述した実施の形態においては、移動体の一例として車両１について説明したが、車両１以外の移動体についても、本発明を同様に適用することができる。

【0085】

以上、本発明のいくつかの実施形態について述べたが、本発明は上記実施形態に限定されるものではなく、本発明の範囲内においてさらに各種の変形および変更が可能であることを付言する。

【符号の説明】

【0086】

１ 車両
１０ 自己位置推定装置
１１，１１１，１１２，１１３ 外界センサ
１２ 内界センサ
１３ 地図データベース
１４ コントローラ
１４１ データ保存部
１４２ 移動量算出部
１４３ 集合点群データ生成部
１４４ 位置推定部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】