特許 7540559 自己位置推定装置及び自己位置推定方法

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

(12)【公報種別】特許公報(B1)

(11)【特許番号】

(24)【登録日】2024-08-19

(45)【発行日】2024-08-27

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

(51)【国際特許分類】

   G01C 21/28 20060101AFI20240820BHJP

   G09B 29/10 20060101ALI20240820BHJP

【ＦＩ】

G01C21/28

G09B29/10 Z

【請求項の数】 6

(21)【出願番号】P 2023132010

(22)【出願日】2023-08-14

【審査請求日】2023-08-14

(73)【特許権者】

【識別番号】000000170

【氏名又は名称】いすゞ自動車株式会社

(74)【代理人】

【識別番号】110004222

【氏名又は名称】弁理士法人創光国際特許事務所

(74)【代理人】

【識別番号】100166006

【弁理士】

【氏名又は名称】泉 通博

(74)【代理人】

【識別番号】100154070

【弁理士】

【氏名又は名称】久恒 京範

(74)【代理人】

【識別番号】100153280

【弁理士】

【氏名又は名称】寺川 賢祐

(74)【代理人】

【識別番号】100167793

【弁理士】

【氏名又は名称】鈴木 学

(72)【発明者】

【氏名】吉野 恭司

【審査官】秋山 誠

(56)【参考文献】

【文献】特開２０２３－９５９０４（ＪＰ，Ａ）

【文献】特開２０２０－７６７２６（ＪＰ，Ａ）

【文献】特開２０２２－２２２８７（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｃ ２１／２８

Ｇ０９Ｂ ２９／１０

(57)【特許請求の範囲】

【請求項1】

単位時間が経過する時刻毎に、車両の周囲の物体の外郭線を示す外郭線情報を取得する取得部と、
所定時刻において、前記所定時刻よりも前の時刻に前記取得部が取得した前記外郭線情報に基づいて、所定座標系における、前記物体の外郭線に対応する座標群を算出し、前記車両の周囲の領域を分割した複数の分割領域毎に、前記座標群のうち前記分割領域に含まれる座標の平均と分散とを算出する算出部と、
前記車両の移動量を含む評価関数に、前記算出部が算出した前記平均及び前記分散と、前記所定時刻に前記取得部が取得した前記座標群と、を入力した評価結果が最小になるように、前記所定時刻より前記単位時間前の時刻から前記所定時刻までの前記車両の移動量を特定する特定部と、
前記特定部が特定した前記車両の移動量と前記算出部が算出した前記平均及び前記分散とに基づいて、前記所定時刻における前記車両の第１位置に対応する座標と前記物体の外郭線に対応する座標群とを推定することにより、前記車両の走行経路と前記物体の位置とを示す地図情報を生成する生成部と、を有し、
前記算出部は、前記単位時間が経過する時刻毎に小さくなる重み係数を、前記外郭線情報毎に、前記取得部が取得した時刻に基づいて特定し、前記外郭線情報に対応する重み係数を用いた重みつき平均及び重みつき分散を、前記平均及び前記分散として算出する、
自己位置推定装置。

【請求項2】

前記算出部は、所定時間における前記車両の移動量の合計が閾値未満である場合、前記重み係数が取り得る値の最大値を、前記所定時間が経過した時刻における前記重み係数として特定する、
請求項１に記載の自己位置推定装置。

【請求項3】

前記特定部は、前記単位時間が経過する時刻毎に特定した前記車両の移動量と、前記車両の移動量を特定した前記時刻とを記憶部に記憶させ、
前記算出部は、前記記憶部に記憶された前記車両の移動量と前記時刻とを参照することにより、前記所定時間における前記車両の移動量の合計を算出する、
請求項２に記載の自己位置推定装置。

【請求項4】

前記取得部は、前記車両の前記第１位置と異なる前記車両の第２位置を外部装置から取得し、
前記算出部は、前記車両の前記第１位置と前記車両の前記第２位置との距離が閾値以上である場合、前記重み係数が取り得る値の最大値を、前記単位時間が経過した時刻における前記重み係数として特定する、
請求項１に記載の自己位置推定装置。

【請求項5】

前記算出部は、前記単位時間が経過した回数に所定値を加算した加算値の逆数を、前記重み係数として特定する、
請求項１から４のいずれか一項に記載の自己位置推定装置。

【請求項6】

単位時間が経過する時刻毎に、車両の周囲の物体の外郭線を示す外郭線情報を取得する取得工程と、
所定時刻において、前記所定時刻よりも前の時刻に前記取得工程において取得した前記外郭線情報に基づいて、所定座標系における、前記物体の外郭線に対応する座標群を算出し、前記車両の周囲の領域を分割した複数の分割領域毎に、前記座標群のうち前記分割領域に含まれる座標の平均と分散とを算出する算出工程と、
前記車両の移動量を含む評価関数に、前記算出工程において算出した前記平均及び前記分散と、前記所定時刻に前記取得工程において取得した前記座標群と、を入力した評価結果が最小になるように、前記所定時刻より前記単位時間前の時刻から前記所定時刻までの前記車両の移動量を特定する特定工程と、
前記特定工程において特定した前記車両の移動量と前記算出工程において算出した前記平均及び前記分散とに基づいて、前記所定時刻における前記車両の第１位置に対応する座標と前記物体の外郭線に対応する座標群とを推定することにより、前記車両の走行経路と前記物体の位置とを示す地図情報を生成する生成工程と、を有し、
前記算出工程において、前記単位時間が経過する時刻毎に小さくなる重み係数を、前記外郭線情報毎に、前記取得工程において取得した時刻に基づいて特定し、前記外郭線情報に対応する重み係数を用いた重みつき平均及び重みつき分散を、前記平均及び前記分散として算出する、
自己位置推定方法。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

特許文献１の地図生成装置は、構造物の位置を示す点群データを正規分布により表した複数のボクセルデータと、センサが出力した点群データとを用いてＮＤＴ（Normal Distributions Transform）スキャンマッチングを実施することにより、車両の位置を推定し、地図を生成する。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０２０－３４７７１号公報

【発明の概要】

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

【0004】

ＳＬＡＭ（Simultaneous Localization And Mapping）においては、センサの測定誤差、センサが障害物を測定する向きの誤差、点群データの量の不足に起因して、車両が同じ位置を走行するたびに当該位置に対応した異なる位置を推定し、地図を更新する場合がある。そこで、車両が周回経路を周回した際に、周回経路に含まれる所定の位置において推定した位置が同じになるように補正する処理（いわゆる、ループ閉じこみ）をすることが考えられるが、周回経路を検出するための処理負荷が大きいため、ループ閉じこみよりも小さい処理負荷で、異なる位置を推定することによる地図の更新を抑制することが求められている。

【0005】

そこで、本発明はこれらの点に鑑みてなされたものであり、推定した位置に基づく地図の更新の処理負荷を小さくすることを目的とする。

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

【0006】

本発明の第１の態様に係る自己位置推定装置は、単位時間が経過する時刻毎に、車両の周囲の物体の外郭線を示す外郭線情報を取得する取得部と、所定時刻において、前記所定時刻よりも前の時刻に前記取得部が取得した前記外郭線情報に基づいて、所定座標系における、前記物体の外郭線に対応する座標群を算出し、前記車両の周囲の領域を分割した複数の分割領域毎に、前記座標群のうち前記分割領域に含まれる座標の平均と分散とを算出する算出部と、前記車両の移動量を含む評価関数に、前記算出部が算出した前記平均及び前記分散と、前記所定時刻に前記取得部が取得した前記座標群と、を入力した評価結果が最小になるように、前記所定時刻より前記単位時間前の時刻から前記所定時刻までの前記車両の移動量を特定する特定部と、前記特定部が特定した前記車両の移動量と前記算出部が算出した前記平均及び前記分散とに基づいて、前記所定時刻における前記車両の第１位置に対応する座標と前記物体の外郭線に対応する座標群とを推定することにより、前記車両の走行経路と前記物体の位置とを示す地図情報を生成する生成部と、を有し、前記算出部は、前記単位時間が経過する時刻毎に小さくなる重み係数を、前記外郭線情報毎に、前記取得部が取得した時刻に基づいて特定し、前記外郭線情報に対応する重み係数を用いた重みつき平均及び重みつき分散を、前記平均及び前記分散として算出する。

【0007】

前記算出部は、所定時間における前記車両の移動量の合計が閾値未満である場合、前記重み係数が取り得る値の最大値を、前記所定時間が経過した時刻における前記重み係数として特定してもよい。

【0008】

前記特定部は、前記単位時間が経過する時刻毎に特定した前記車両の移動量と、前記車両の移動量を特定した前記時刻とを記憶部に記憶させ、前記算出部は、前記記憶部に記憶された前記車両の移動量と前記時刻とを参照することにより、前記所定時間における前記車両の移動量の合計を算出してもよい。

【0009】

前記取得部は、前記車両の前記第１位置と異なる前記車両の第２位置を外部装置から取得し、前記算出部は、前記車両の前記第１位置と前記車両の前記第２位置との距離が閾値以上である場合、前記重み係数が取り得る値の最大値を、前記単位時間が経過した時刻における前記重み係数として特定してもよい。

【0010】

前記算出部は、前記単位時間が経過した回数に所定値を加算した加算値の逆数を、前記重み係数として特定してもよい。

【0011】

本発明の第２の態様に係る自己位置推定方法は、単位時間が経過する時刻毎に、車両の周囲の物体の外郭線を示す外郭線情報を取得する取得工程と、所定時刻において、前記所定時刻よりも前の時刻に前記取得工程において取得した前記外郭線情報に基づいて、所定座標系における、前記物体の外郭線に対応する座標群を算出し、前記車両の周囲の領域を分割した複数の分割領域毎に、前記座標群のうち前記分割領域に含まれる座標の平均と分散とを算出する算出工程と、前記車両の移動量を含む評価関数に、前記算出工程において算出した前記平均及び前記分散と、前記所定時刻に前記取得工程において取得した前記座標群と、を入力した評価結果が最小になるように、前記所定時刻より前記単位時間前の時刻から前記所定時刻までの前記車両の移動量を特定する特定工程と、前記特定工程において特定した前記車両の移動量と前記算出工程において算出した前記平均及び前記分散とに基づいて、前記所定時刻における前記車両の第１位置に対応する座標と前記物体の外郭線に対応する座標群とを推定することにより、前記車両の走行経路と前記物体の位置とを示す地図情報を生成する生成工程と、を有し、前記算出工程において、前記単位時間が経過する時刻毎に小さくなる重み係数を、前記外郭線情報毎に、前記取得工程において取得した時刻に基づいて特定し、前記外郭線情報に対応する重み係数を用いた重みつき平均及び重みつき分散を、前記平均及び前記分散として算出する。

【発明の効果】

【0012】

本発明によれば、推定した位置に基づく地図の更新の処理負荷を小さくするという効果を奏する。

【図面の簡単な説明】

【0013】

【図1】本実施形態に係る車両Ｓの概要を説明するための図である。

【図2】物体の外郭線に対応する座標群の一例を示す図である。

【図3】重みつき平均と重みつき分散とを算出する動作を示す図である。

【図4】地図情報を更新する動作を示すシーケンスである。

【図5】重み係数を特定する動作を示すシーケンスである。

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

【0014】

＜車両Ｓの概要＞
図１は、本実施形態に係る車両Ｓの概要を説明するための図である。図１に示す車両Ｓは、測定装置１０と、姿勢角検出装置１１、受信装置１２と、表示装置１３と、自己位置推定装置２０と、を備える。車両Ｓは、測定装置１０が検出した、車両Ｓの周囲の物体と車両Ｓとの距離に基づいて、車両Ｓの位置を推定し、車両Ｓの走行経路と物体の位置とを示す地図情報を生成する機能を有する。周囲の物体は、例えば、人、動植物、壁、及び車両Ｓの走行が困難な段差である。周囲の物体は、車両Ｓと異なる他の車両を含んでもよい。

【0015】

測定装置１０は、車両Ｓの周囲の物体と当該車両Ｓとの距離を測定するための三次元レーザ測定機であり、例えばＬｉＤＡＲ（Light Detection And Ranging）である。測定装置１０は、車両Ｓの周囲の物体と車両Ｓとの距離を測定することにより生成した点群データに基づいて、車両Ｓの周囲の物体の外郭線の位置を示す外郭線情報を生成し、自己位置推定装置２０に出力する。

【0016】

姿勢角検出装置１１は、車両Ｓの向きを示す姿勢角（例えば、鉛直軸に対するロール角、ピッチ角）を検出するための装置である。姿勢角検出装置１１は、例えば、車両Ｓが備えるホイールエンコーダ（不図示）が検出した並進速度と、車両Ｓが備えるジャイロスコープ（不図示）が検出した角速度とに基づいて、車両Ｓの姿勢角を検出し、自己位置推定装置２０に出力する。なお、本実施形態においては、説明を簡単にするために、時刻ｔにおける車両Ｓの姿勢角を「姿勢角θ_ｔ」と記載する。

【0017】

受信装置１２は、例えば、ＧＮＳＳ（Global Navigation Satellite System）等の外部の測位システムから電波を受信するための装置である。受信装置１２は、受信した電波に含まれている情報に基づいて算出した緯度と経度とを、車両Ｓの位置情報として自己位置推定装置２０に出力する。表示装置１３は、車両Ｓが生成した地図情報を表示するためのディスプレイである。

【0018】

自己位置推定装置２０は、電子部品を含む筐体又は電子部品が実装されたプリント基板である。自己位置推定装置２０は、単位時間（例えば、０．１秒）が経過する時刻毎に、測定装置１０から取得した外郭線情報と、姿勢角検出装置１１から取得した姿勢角とに基づいて車両Ｓの位置を推定し、地図情報を更新する処理を実行する。自己位置推定装置２０は、例えば、ＳＬＡＭ（Simultaneous Localization And Mapping）処理を実行する装置である。

【0019】

ＳＬＡＭにおいては、一定時間毎に車両Ｓの位置を推定し地図情報を更新するため、現在の時刻より前の時刻に車両Ｓが走行した位置であっても、車両Ｓの位置を改めて推定し地図情報を更新する。また、ＳＬＡＭにおいては、測定装置１０が測定した際の車両Ｓの向き、測定装置１０の測定誤差により、車両Ｓが同じ位置を走行するたびに異なる位置を推定する場合がある。これらの動作から、ＳＬＡＭ処理を実行する自己位置推定装置２０においては、例えば、周回経路を周回するほど地図情報が示す物体の位置と車両Ｓの走行位置とが異なる位置に更新されてしまう。

【0020】

これに対して、自己位置推定装置２０は、周回経路に含まれる所定の位置において推定した位置が同じになるように補正する処理（ループ閉じこみ）を実行することで、周回するほど物体の位置と車両Ｓの走行位置とを異なる位置に更新することを抑制できる。しかしながら、ループ閉じこみは、周回経路を検出するための処理負荷が大きいため、ループ閉じこみを用いずに、同じ位置を走行した車両Ｓが同じ位置を推定することが求められている。

【0021】

そこで、自己位置推定装置２０は、ＳＬＡＭ処理に含まれるスキャンマッチング処理における、測定装置１０が生成した外郭線情報の平均と分散との算出において、単位時間が経過する時刻毎に小さくなる重み係数を用いた重み平均と重み分散とを算出する。スキャンマッチング処理は、所定の時刻の位置データと、所定の時刻より前の時刻の位置データとを重ね合わせ、車両Ｓの移動量を推定する処理である。本実施形態においては、ＮＤＴ（Normal Distribution Transform）スキャンマッチング処理を実施する。

【0022】

自己位置推定装置２０がこのように動作することで、所定の位置を最初に走行した際の位置データの重み係数を大きくし、２回目以降に走行した際の位置データの重み係数を小さくできる。その結果、最初に走行した際の位置が、位置を推定する結果に最も影響を与えることができるため、同じ位置を２回目以降走行しても誤って別の位置として推定することを抑制できる。さらに、ループ閉じこみを用いずに車両Ｓの位置を推定できるため、処理負荷を小さくすることができる。
以下、自己位置推定装置２０の構成及び動作を詳細に説明する。

【0023】

＜自己位置推定装置２０の構成＞
自己位置推定装置２０は、記憶部２１と、制御部２２と、を有する。制御部２２は、取得部２２１と、算出部２２２と、特定部２２３と、生成部２２４と、表示制御部２２５と、を有する。

【0024】

記憶部２１は、例えば、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、ＨＤＤ（Hard Disk Drive）又はＳＳＤ（Solid State Drive）等の記憶媒体を有する。記憶部２１は、制御部２２が実行するプログラムと、車両Ｓが走行する位置を推定し、地図情報を生成及び更新するための各種の情報とを記憶している。記憶部２１は、生成及び更新した地図情報を記憶する。

【0025】

制御部２２は、例えばＣＰＵ（Central Processing Unit）又はＥＣＵ（Electronic Control Unit）等のプロセッサである。制御部２２は、記憶部２１に記憶されたプログラムを実行することにより、取得部２２１、算出部２２２、特定部２２３、生成部２２４及び表示制御部２２５として機能する。なお、制御部２２は、１つのプロセッサで構成されていてもよいし、複数のプロセッサ又は１以上のプロセッサと電子回路との組み合わせにより構成されていてもよい。
以下、制御部２２により実現される各部の構成を説明する。

【0026】

取得部２２１は、単位時間が経過する時刻毎に、車両Ｓの周囲の物体の外郭線を示す外郭線情報を取得する。単位時間は、例えば、０．１秒である。取得部２２１は、例えば、測定装置１０が検出した、車両Ｓの周囲の物体を示す点群データを含む外郭線情報を取得する。取得部２２１は、単位時間が経過する時刻毎に、姿勢角検出装置１１が検出した車両Ｓの姿勢角を取得する。取得部２２１は、測定装置１０から取得した外郭線情報と姿勢角検出装置１１から取得した姿勢角とに基づいて推定した車両Ｓの第１位置と異なる車両Ｓの第２位置を受信装置１２から取得する。

【0027】

算出部２２２は、ＮＤＴスキャンマッチング処理における、外郭線情報に含まれる点群データの平均と分散を算出する。算出部２２２は、所定時刻において、所定時刻よりも前の時刻に取得部２２１が取得した外郭線情報に基づいて、所定座標系における、物体の外郭線に対応する座標群を算出する。そして、算出部２２２は、車両Ｓの周囲の領域を分割した複数の分割領域毎に、座標群のうち分割領域に含まれる座標の平均と分散とを算出する。所定座標系は、例えば、車両Ｓが走行を開始した位置を中心座標とする座標系（以下、「地図座標系」という）である。

【0028】

図２は、物体の外郭線に対応する座標群の一例を示す図である。領域Ｒは、例えば、地図座標系における車両Ｓの周囲の領域のうち、車両Ｓの進行方向前方を示す領域である。領域Ｒは、複数の分割領域（図２に示す領域Ｂ１から領域Ｂ１２）に分割されている。図２に示す座標Ｐ０１から座標Ｐ０６は、所定時刻（例えば時刻ｔ）において、所定時刻よりも前の時刻（例えば時刻ｔ－１）に取得部２２１が取得した外郭線情報に基づいて算出部２２２が算出した、物体の外郭線に対応する座標群である。図２に示す座標Ｐ１１から座標Ｐ１５は、時刻ｔ－１において、時刻ｔ－１よりも前の時刻（例えば時刻ｔ－２）に取得部２２１が取得した外郭線情報に基づいて算出部２２２が算出した、物体の外郭線に対応する座標群である。一例として、時刻ｔ－２から時刻ｔ－１までの時間、及び時刻ｔ－１から時刻ｔまでの時間は、単位時間（０．１秒）である。

【0029】

図２に示すように、算出部２２２は、時刻ｔ－１において、時刻ｔ－２に取得部２２１が取得した外郭線情報に含まれる点群データを、時刻ｔ－２に推定した車両Ｓの位置に基づいて、地図座標系における座標データに変換することにより座標群（座標Ｐ１１から座標Ｐ１５）を算出する。同様に、算出部２２２は、時刻ｔにおいて、時刻ｔ－１に取得部２２１が取得した外郭線情報に含まれる点群データを、時刻ｔ－１に推定した車両Ｓの位置に基づいて、地図座標系における座標データに変換することにより座標群（座標Ｐ０１から座標Ｐ０６）を算出する。算出部２２２は、複数の分割領域（領域Ｂ１から領域Ｂ１２）毎に、座標Ｐ１１から座標Ｐ１５及び座標Ｐ０１から座標Ｐ０６のうち、分割領域に含まれる座標の平均と分散とを算出する。

【0030】

算出部２２２は、単位時間が経過する時刻毎に小さくなる重み係数を、外郭線情報毎に、取得部２２１が取得した時刻に基づいて特定し、外郭線情報に対応する重み係数を用いた重みつき平均及び重みつき分散を、平均及び分散として算出する。算出部２２２がこのように動作することで、所定の位置を最初に走行した際の位置データの重み係数を大きくし、当該所定の位置を２回目以降に走行した際の位置データの重み係数を小さくすることができる。その結果、同じ位置を２回目以降走行しても異なる位置を推定しにくくなる。
以下、特定した重み係数を用いた重みつき平均及び重みつき分散の算出方法と、重みつき平均及び重みつき分散を用いたＮＤＴスキャンマッチングである「重みつきＮＤＴスキャンマッチング」について詳細に説明する。なお、以下においては、説明を簡単にするために、２次元座標を用いる場合がある。

【0031】

最初に、ＮＤＴスキャンマッチングについて説明する。時刻ｔの点群データＡ、時刻ｔ－１の点群データＢ、点群データＡと点群データＢとが重なる位置変換行列Ｔ、各点の座標（ｘ，ｙ，１）^Ｔ、時刻ｔ－１における姿勢角θ_ｔ－１、時刻ｔ－１から時刻ｔまでの時間における車両Ｓの移動量（ｔ_ｘ，ｔ_ｙ，φ）^Ｔを定義すると、位置変換行列Ｔは、式（１）のように与えることができる。

【数1】

【0032】

ＮＤＴスキャンマッチングにおいては、最近傍点として点群データＡに対応付けした点ａ_ｉと点群データＢに対応付けした点ｂ_ｉとについて、点群データＢに対応付けした点ｂ_ｉが属する分割領域（図２に示す領域Ｂ１から領域Ｂ１２のいずれか）内での他の点を用いて正規分布でモデル化する。ｂ_ｉが属する分割領域ｍの中に点がＮ個存在する場合、Ｎ個の点の平均は式（２）、Ｎ個の点の分散は式（３）のように求められる。また、式（２）及び式（３）を用いることで、式（４）のように表現できる。

【数2】

【0033】

ところで、ｂ_ｉに対応づけられたａ_ｉに正しい位置変換行列Ｔを作用させたＴａ_ｉは、同じ分割領域ｍ内に存在し、ｂ_ｉと同じ分布に従う蓋然性が高いため、式（５）のように表現できる。

【数3】

【0034】

式（５）から、確率分布（平均及び分散）からＴａ_ｉが観測される確率が最大となるときの位置変換行列Ｔが最適な位置変換行列であり、式（６）のように算出できる。

【数4】

【0035】

正規分布で表されるｐ（Ｔａ_ｉ）を最大化するには、式（５）に示す定数（－１／２）を除いた指数部分を最小化すればよい。したがって、点群データが存在する分割領域の数をＭ個とした場合、車両Ｓの移動量を特定するための評価関数を式（７）のように表現できる。後述する特定部は、式（７）の評価関数の出力Ｃ（ａ_ｉ，ｂ_ｉ）が最小になるように、車両Ｓの移動量を特定する。

【数5】

【0036】

次に、重みつきＮＤＴスキャンマッチングについて説明する。一般的な重みつきＮＤＴスキャンマッチングにおいては、データｘ_ｉに対応する重み係数をω_ｉと定義すると、重みつき平均を式（８）、重みつき分散を式（９）のように表現できる。

【数6】

【0037】

ここで、本実施形態においては、重み係数ω_kを、サイクルｋを用いて式（１０）のように定義する。サイクルｋは、自己位置推定装置２０が車両Ｓの位置を推定する動作を開始した時刻において初期値（すなわち、ｋ＝０）であり、単位時間が経過した時刻毎に１が加算される。

【数7】

式（１０）に示すように、算出部２２２は、単位時間が経過した回数に所定値（すなわち、１）を加算した加算値の逆数を、重み係数として特定する。

【0038】

図３は、重みつき平均と重みつき分散とを算出する動作を示す図である。図３の横軸は、時刻を示す。図３の縦軸は、サイクルｋ、重み係数、取得部２２１が取得した外郭線情報に含まれる点群データ、重みつき平均及び重みつき分散を示す。図３においては、サイクルｋは、時刻Ｔ０、時刻Ｔ１、時刻Ｔ２及び時刻Ｔ３が経過した時刻（すなわち、時刻Ｔ０、時刻Ｔ１、時刻Ｔ２及び時刻Ｔ３の直後の時刻）に１が加算される。なお、図３に示す重み係数は、小数点第１位までの値を示しており、小数点第２位以下を省略している。

【0039】

例えば、図３に示すように、時刻Ｔ０において、算出部２２２は、サイクルｋが０であることにより、重み係数が１であることを特定する。そして、算出部２２２は、時刻Ｔ０に取得部２２１が取得した外郭線情報Ｌ０に含まれる点群データに対応する重み係数が１であることを特定する。続いて、時刻Ｔ１において、算出部２２２は、サイクルｋが１であることにより、時刻Ｔ１に取得部２２１が取得した外郭線情報Ｌ１に対応する重み係数が０．５であることを特定する。また、算出部２２２は、時刻Ｔ０に取得部２２１が取得した外郭線情報Ｌ０と重み係数１とを用いて重みつき平均Ａ１と重みつき分散Ｄ１とを算出する。図３から明らかなように、時刻の経過と共に、重み係数の値が漸次小さくなっている。

【0040】

ここで、図３に示す時刻Ｔ０、時刻Ｔ１及び時刻Ｔ２における重みつき平均と重みつき分散の算出式について説明する。図３に示す時刻Ｔ０（サイクルｋ＝０）において、分割領域ｍ内にｎ_０個の点が観測された外郭線情報Ｌ０を取得部２２１が取得したとする。時刻Ｔ０においては、直前までの点群が存在しないため、算出部２２２は、重みつき平均及び重みつき分散を算出しない。

【0041】

図３に示す時刻Ｔ１（サイクルｋ＝１）において、分割領域ｍ内にｎ_１個の点が観測された外郭線情報Ｌ１を取得部２２１が取得したとする。時刻Ｔ１において、算出部２２２は、時刻Ｔ０に取得部２２１が取得した外郭線情報Ｌ０を用いて、重みつき平均Ａ１と重みつき分散Ｄ１とを算出する。重みつき平均Ａ１は式（１１）、重みつき分散Ｄ１は式（１２）のように表現できる。なお、ｘ_ｋ，ｉは、サイクルがｋである時刻に取得部２２１が取得した外郭線情報に含まれる点群データを地図座標系に変換した座標であり、ｘ_ｋ，ｉ＝（ｘ_ｋ，ｉ，ｙ_ｋ，ｉ，１）^Ｔとする。

【数8】

【0042】

図３に示す時刻Ｔ２（サイクルｋ＝２）において、分割領域ｍ内にｎ_２個の点が観測された外郭線情報Ｌ２を取得部２２１が取得したとする。時刻Ｔ２において、算出部２２２は、時刻Ｔ０に取得部２２１が取得した外郭線情報Ｌ０と時刻Ｔ１に取得部２２１が取得した外郭線情報Ｌ１とを用いて、重みつき平均Ａ２と重みつき分散Ｄ２とを算出する。重みつき平均Ａ２は式（１３）、重みつき分散Ｄ２は式（１４）のように表現できる。

【数9】

【0043】

また、式（１１）及び式（１２）を用いて、式（１５）及び式（１６）の漸化式を導出し、式（１３）及び式（１４）に適用すると、時刻Ｔ２（サイクルｋ＝２）における重みつき平均Ａ２は式（１７）、重みつき分散Ｄ２は式（１８）のように表現できる。

【数10】

【0044】

さらに、式（１５）及び式（１６）と同様に、時刻Ｔ３（サイクルｋ＝３）における漸化式を式（１９）及び式（２０）のように導出することで、任意のサイクルｋにおける分割領域ｍの重みつき平均を式（２１）、重みつき分散を式（２２）のように表現できる。

【数11】

【0045】

そして、式（２１）及び式（２２）を式（７）に代入することにより、重みつき平均と重みつき分散とを用いた評価関数による重みつきＮＤＴスキャンマッチングを実行することができる。なお、上記の式（７）、式（２１）及び式（２２）は、ｘ_ｉが３次元座標の場合であっても成立する。
以上、重み係数を用いた重みつき平均及び重みつき分散の算出方法と、重みつきＮＤＴスキャンマッチングについて説明した。

【0046】

ところで、車両Ｓは、車線が渋滞していることにより停止したり、信号が停止を指示している場合（いわゆる赤信号の場合）に停止したりする。この場合、車両Ｓの位置が変わらなくてもサイクルｋが加算されるため、走行を再開した後に適切な重み係数が特定できない場合がある。そこで、算出部２２２は、所定時間における車両Ｓの移動量の合計が閾値未満である場合、重み係数が取り得る値の最大値（すなわち、１）を、所定時間が経過した時刻における重み係数として特定してもよい。所定時間は、信号又は渋滞により車両Ｓが停止する時間に相当する時間であり、実験又はシミュレーションにより定められる。閾値は、適切な重み係数を特定できる移動量の下限値であり、実験又はシミュレーションにより定められる。

【0047】

算出部２２２は、例えば、単位時間が経過する時刻毎に、記憶部２１に記憶された車両Ｓの移動量と時刻とを参照することにより、所定時間における車両Ｓの移動量の合計を算出する。算出部２２２は、算出した車両Ｓの移動量の合計が閾値未満である場合、サイクルｋに０を代入し（すなわちサイクルｋを初期化し）、重み係数を１にする。算出部２２２がこのように動作することで、車両Ｓの走行状態が変化した場合にサイクルｋ及び重み係数を初期化できるため、車両Ｓの位置を高い精度で推定できる。

【0048】

ＳＬＡＭにおいては、位置の推定を開始した後の車両Ｓの移動量の合計が大きいほど推定誤差が生じやすくなる。そこで、算出部２２２は、受信装置１２が受信した電波に含まれる位置情報（緯度及び経度）と、自己位置推定装置２０が推定した車両Ｓの位置との距離に基づいて、サイクルｋ及び重み係数を初期化してもよい。算出部２２２は、例えば、生成部２２４が推定した車両Ｓの第１位置と、受信装置１２が受信した車両Ｓの第２位置との距離が閾値以上である場合、重み係数が取り得る値の最大値を、単位時間が経過した時刻における重み係数として特定する。閾値は、例えば、ＧＮＳＳ等の外部の測位システムによる測定誤差（例えば、数メートル）よりも大きい値である。算出部２２２がこのように動作することで、車両Ｓの移動量の合計が大きくなった場合であっても重み係数を初期化することができるため、車両Ｓの位置を高い精度で推定できる。

【0049】

特定部２２３は、単位時間が経過した時刻毎に車両Ｓの移動量を特定する。特定部２２３は、車両Ｓの移動量を含む評価関数に、算出部２２２が算出した平均及び分散と、所定時刻に取得部２２１が取得した座標群と、を入力した評価結果が最小になるように、所定時刻より単位時間前の時刻から所定時刻までの車両Ｓの移動量を特定する。特定部２２３は、例えば、式（７）に示す評価関数のａ_ｉとｂ_ｉとが一致するような車両Ｓの移動量を特定するために、式（７）の出力Ｃ（ａ_ｉ，ｂ_ｉ）が最小になるように、車両Ｓの移動量を特定する。

【0050】

例えば、特定部２２３は、算出部２２２が式（２１）を用いて算出した重みつき平均と、式（２２）を用いて算出した重みつき分散と、取得部２２１が取得した外郭線情報に含まれる点群データを地図座標系に変換した複数の座標データ（座標群）と、を式（７）に入力する。そして、特定部２２３は、式（７）の出力結果が最小になるように（すなわち、ａ_ｉとｂ_ｉとが一致するように）車両Ｓの移動量を特定する。特定部２２３がこのように動作することで、単位時間が経過した時刻毎に、車両Ｓの移動量を高い精度で特定することができる。

【0051】

また、特定部２２３は、単位時間が経過する時刻毎に特定した車両Ｓの移動量と、車両Ｓの移動量を特定した時刻とを記憶部２１に記憶させてもよい。特定部２２３がこのように記憶部２１に記憶させることで、算出部２２２が所定時間における車両Ｓの移動量の合計値を算出することができるため、適切なタイミングで重み係数を初期化できる。

【0052】

生成部２２４は、車両Ｓの走行経路と車両Ｓの周囲の物体の位置とを示す地図情報を生成する。生成部２２４は、例えば、特定部２２３が特定した車両Ｓの移動量と算出部２２２が算出した重みつき平均及び重みつき分散とに基づいて、所定時刻における車両Ｓの位置に対応する座標と物体の外郭線に対応する座標群とを推定する。そして、生成部２２４は、推定した車両Ｓの座標と車両Ｓの周囲の物体の外郭線に対応する座標群とに基づいて、記憶部２１に記憶された地図情報を更新することにより、地図情報を生成する。表示制御部２２５は、生成部２２４が生成した地図情報を表示装置１３に表示させる。

【0053】

＜自己位置推定装置２０における処理シーケンス＞
図４及び図５は、自己位置推定装置２０における処理シーケンスの例を示す図である。図４に示す処理シーケンスは、地図情報を更新する動作を示すシーケンスであり、図５に示す処理シーケンスは、重み係数を特定する動作を示すシーケンスである。図５に示す動作は、図４に示すＳ１２の動作である。自己位置推定装置２０は、図４及び図５に示す処理シーケンスを一定時間毎に（単位時間が経過した時刻毎に）繰り返す。

【0054】

まず、地図情報を更新する動作を説明する。図４において、取得部２２１は、姿勢角検出装置１１が検出した車両Ｓの姿勢角を取得する（Ｓ１０）。取得部２２１は、測定装置１０が検出した、車両Ｓの周囲の物体の外郭線を示す外郭線情報を取得する（Ｓ１１）。

【0055】

算出部２２２は、自己位置推定装置２０が車両Ｓの位置の推定を開始した時刻から現在の時刻までの時間に経過したサイクル数（１サイクルは、例えば、０．１秒）を式（１０）に代入することにより、重み係数を特定する（Ｓ１２）。Ｓ１２の動作の詳細は、後述する。算出部２２２は、式（２１）及び式（２２）を用いて重みつき平均と重みつき分散とを算出する（Ｓ１３）。特定部２２３は、式（７）の評価関数の出力結果が最小になるような車両Ｓの移動量を特定する（Ｓ１４）。生成部２２４は、特定部２２３が特定した車両Ｓの移動量と算出部２２２が算出した重みつき平均及び重みつき分散とに基づいて、車両Ｓの位置に対応する座標と車両Ｓの周囲の物体の外郭線を示す座標群とを推定し、記憶部２１に記憶された地図情報を更新する（Ｓ１５）。

【0056】

次に、算出部２２２が、重み係数を特定する動作を説明する。図５において、算出部２２２は、単位時間が経過していない場合（Ｓ２０のＮＯ）、単位時間が経過した時刻までＳ２０の処理を繰り返す。単位時間が経過した時刻である場合（Ｓ２０のＹＥＳ）、算出部２２２は、自己位置推定装置２０が車両Ｓの位置の推定を開始した時刻から、現在時刻までに単位時間が経過した回数が１より大きいか否かを特定する（Ｓ２１）。

【0057】

回数が１以下である場合（Ｓ２１のＮＯ）、算出部２２２は、式（１０）のサイクルｋに０を代入する（Ｓ２２）。回数が２以上である場合（Ｓ２１のＹＥＳ）、算出部２２２は、記憶部２１に記憶された車両Ｓの移動量と時刻とを参照することにより、所定時間における車両Ｓの移動量の合計を算出する（Ｓ２３）。算出した車両Ｓの移動量の合計が第１閾値未満である場合（Ｓ２４のＮＯ）、算出部２２２は、式（１０）のサイクルｋに０を代入する（Ｓ２２）。車両Ｓの移動量の合計が第１閾値以上である場合（Ｓ２４のＹＥＳ）、算出部２２２は、受信装置１２が現在時刻より単位時間前の時刻に検出して取得部２２１に出力した車両Ｓの位置情報を取得する（Ｓ２５）。そして、算出部２２２は、取得した位置情報が示す位置と、現在時刻より単位時間前の時刻に生成部２２４が推定した車両Ｓの位置との距離を算出する（Ｓ２６）。

【0058】

算出した距離が第２閾値以上である場合（Ｓ２７のＮＯ）、算出部２２２は、式（１０）のサイクルｋに０を代入する（Ｓ２２）。算出した距離が第２閾値未満である場合（Ｓ２７のＹＥＳ）、算出部２２２は、サイクルｋに１を加算する（Ｓ２８）。そして、算出部２２２は、式（１０）に示す重み係数を算出する（Ｓ２９）。自己位置推定装置２０は、処理を終了する操作を受け付けていない場合（Ｓ３０のＮＯ）、Ｓ２０からＳ２９までの処理を繰り返す。処理を終了する操作を受け付けた場合（Ｓ３０のＹＥＳ）、自己位置推定装置２０は、処理を終了する。

【0059】

＜自己位置推定装置２０による効果＞
以上説明したように、自己位置推定装置２０は、所定時刻において、所定時刻よりも前の時刻に取得部２２１が取得した外郭線情報に基づいて、地図座標系における物体の外郭線に対応する座標群を算出し、車両Ｓの周囲の領域を分割した複数の分割領域毎に、座標群のうち分割領域に含まれる座標の平均と分散とを算出する算出部２２２と、車両Ｓの移動量を含む評価関数に、算出部２２２が算出した平均及び分散と、所定時刻に取得部２２１が取得した座標群と、を入力した評価結果が最小になるように、所定時刻より単位時間前の時刻から所定時刻までの車両Ｓの移動量を特定する特定部２２３と、を有する。

【0060】

そして、算出部２２２は、単位時間が経過する時刻毎に小さくなる重み係数を、外郭線情報毎に、取得部２２１が取得した時刻に基づいて特定し、外郭線情報に対応する重み係数を用いた重みつき平均と重みつき分散とを、平均及び分散として算出する。自己位置推定装置２０がこのように構成されることで、所定の位置を最初に走行した際の位置データの重み係数を大きくし、２回目以降に走行した際の位置データの重み係数を小さくできる。その結果、最初に走行した際の位置が、位置を推定する結果に最も影響を与えることができるため、同じ位置を２回目以降走行しても誤って別の位置として推定することを抑制できる。さらに、ループ閉じこみを用いずに車両Ｓの位置を推定できるため、処理負荷を小さくすることができる。

【0061】

以上、本発明を実施の形態を用いて説明したが、本発明の技術的範囲は上記実施の形態に記載の範囲には限定されず、その要旨の範囲内で種々の変形及び変更が可能である。例えば、装置の全部又は一部は、任意の単位で機能的又は物理的に分散・統合して構成することができる。また、複数の実施の形態の任意の組み合わせによって生じる新たな実施の形態も、本発明の実施の形態に含まれる。組み合わせによって生じる新たな実施の形態の効果は、もとの実施の形態の効果を併せ持つ。

【符号の説明】

【0062】

１０ 測定装置
１１ 姿勢角検出装置
１２ 受信装置
１３ 表示装置
２０ 自己位置推定装置
２１ 記憶部
２２ 制御部
２２１ 取得部
２２２ 算出部
２２３ 特定部
２２４ 生成部
２２５ 表示制御部

【要約】      （修正有）

【課題】推定した位置に基づく地図の更新の処理負荷を小さくする。
【解決手段】自己位置推定装置２０は、所定時刻において、所定時刻よりも前の時刻に取得した外郭線情報に基づいて、物体の外郭線に対応する座標群を算出し、車両の周囲の領域を分割した複数の分割領域毎に、座標群のうち分割領域に含まれる座標の平均と分散とを算出する算出部２２２と、車両の移動量を含む評価関数に、算出部２２２が算出した平均及び分散と、所定時刻に取得した座標群と、を入力した評価結果が最小になるように、所定時刻より単位時間前の時刻から所定時刻までの車両の移動量を特定する特定部２２３と、を有し、算出部２２２は、単位時間が経過する時刻毎に小さくなる重み係数を、外郭線情報毎に、取得部が取得した時刻に基づいて特定し、外郭線情報に対応する重み係数を用いた重みつき平均及び重みつき分散を、平均及び分散として算出する。
【選択図】図１

【図1】

【図2】

【図3】

【図4】

【図5】