特開 2024-80881 自己位置推定装置、自己位置推定方法および自己位置推定プログラム

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

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

(11)【公開番号】P2024080881

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

(54)【発明の名称】自己位置推定装置、自己位置推定方法および自己位置推定プログラム

(51)【国際特許分類】

   G01C 21/30 20060101AFI20240610BHJP

   G06T 7/70 20170101ALI20240610BHJP

   G06T 7/00 20170101ALI20240610BHJP

【ＦＩ】

G01C21/30

G06T7/70 A

G06T7/00 650A

G06T7/00 350B

【審査請求】未請求

【請求項の数】7

【出願形態】ＯＬ

(21)【出願番号】P 2022194207

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

(71)【出願人】

【識別番号】519373914

【氏名又は名称】株式会社Ｊ－ＱｕＡＤ ＤＹＮＡＭＩＣＳ

(71)【出願人】

【識別番号】000004260

【氏名又は名称】株式会社デンソー

(74)【代理人】

【識別番号】110000028

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

(72)【発明者】

【氏名】池田 勇二

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

2F129

5L096

【Ｆターム（参考）】

2F129AA03

2F129BB03

2F129BB33

2F129BB50

2F129BB66

2F129DD13

2F129DD21

2F129EE02

2F129EE65

2F129EE78

2F129FF02

2F129GG17

2F129GG18

5L096AA09

5L096BA04

5L096EA15

5L096EA16

5L096EA39

5L096FA01

5L096FA06

5L096FA66

5L096JA03

5L096KA04

5L096KA15

(57)【要約】

【課題】自己位置推定の精度を向上させることが可能な自己位置推定装置、自己位置推定方法、および自己位置推定プログラムを提供する。
【解決手段】自己位置推定装置は、地図情報取得部（１１２）と、模擬セマンティック画像生成部（１１３）と、実セマンティック画像生成部（１１４）と、最適化処理部（１１５）と、自己位置算出部（１１６）と、を備える。最適化処理部（１１５）は、評価関数を用いて模擬セマンティック画像と実セマンティック画像との一致度を算出し、一致度が高くなるように模擬セマンティック画像を位置合わせして最適化を実行し、模擬セマンティック画像が最適化されたときの相対姿勢行列を導出する。評価関数において、模擬セマンティック画像に含まれるピクセルに設定される重み変数を用いて、最適化の際の一致度を算出する。
【選択図】図２

【特許請求の範囲】

【請求項1】

周辺を撮像するカメラ（１２２）を有する移動体（１０）に搭載され、前記移動体の位置を推定する自己位置推定装置であって、
前記移動体の位置を表す初期位置情報を取得する初期位置取得部（１１１）と、
セマンティックラベルが付与された３Ｄ地図を表す３Ｄ地図情報を取得する地図情報取得部（１１２）と、
前記３Ｄ地図情報上において前記初期位置情報が示す位置から前記カメラを通して見える画像であって深度情報を含む模擬セマンティック画像、を生成する模擬セマンティック画像生成部（１１３）と、
前記カメラが撮像した周辺画像を対象にセマンティックセグメンテーションを実行して得られる画像である実セマンティック画像を生成する実セマンティック画像生成部（１１４）と、
評価関数を用いて前記模擬セマンティック画像と前記実セマンティック画像との一致度を算出し、前記一致度が高くなるように前記模擬セマンティック画像を位置合わせして最適化を実行し、前記模擬セマンティック画像が最適化されたときの相対姿勢行列を導出する最適化処理部（１１５）と、
前記最適化処理部により導出された前記相対姿勢行列と、前記初期位置情報と、を用いて、前記移動体の自己位置を算出する自己位置算出部（１１６）と、
を備え、
前記最適化処理部は、前記評価関数において、前記模擬セマンティック画像に含まれるピクセルに設定される重み変数を用いて、前記最適化の際の前記一致度を算出する、自己位置推定装置。

【請求項2】

前記重み変数は、前記ピクセルの属するクラスの出現頻度に応じて設定され、前記出現頻度が低いクラスに属する前記ピクセルは、前記出現頻度が高いクラスに属する前記ピクセルよりも前記重み変数が大きく設定されている、請求項１に記載の自己位置推定装置。

【請求項3】

前記重み変数は、前記ピクセルの前記カメラからの距離に応じて設定され、前記距離が近い前記ピクセルは、前記距離が遠い前記ピクセルよりも前記重み変数が小さく設定されている、請求項１または請求項２に記載の自己位置推定装置。

【請求項4】

前記重み変数は、前記ピクセルの属する物体の性質に応じて設定されている、請求項１または請求項２に記載の自己位置推定装置。

【請求項5】

前記重み変数は、位置誤差が小さくなるように前記評価関数を最小化する値として、機械学習により設定されている、請求項１または請求項２に記載の自己位置推定装置。

【請求項6】

周辺を撮像するカメラ（１２２）を有する移動体（１０）に搭載された自己位置推定装置を用いて、前記移動体の位置を推定する自己位置推定方法であって、
前記自己位置推定装置は、初期位置取得部（１１１）と、地図情報取得部（１１２）と、模擬セマンティック画像生成部（１１３）と、実セマンティック画像生成部（１１４）と、最適化処理部（１１５）と、自己位置算出部（１１６）と、を備え、
前記初期位置取得部により、前記移動体の位置を表す初期位置情報を取得する工程と、
前記地図情報取得部により、セマンティックラベルが付与された３Ｄ地図を表す３Ｄ地図情報を取得する工程と、
前記模擬セマンティック画像生成部により、前記３Ｄ地図情報上において前記初期位置情報が示す位置から前記カメラを通して見える画像であって深度情報を含む模擬セマンティック画像、を生成する工程と、
前記実セマンティック画像生成部により、前記カメラが撮像した周辺画像を対象にセマンティックセグメンテーションを実行して得られる画像である実セマンティック画像を生成する工程と、
前記最適化処理部により、評価関数を用いて前記模擬セマンティック画像と前記実セマンティック画像との一致度を算出して、前記一致度が高くなるように前記模擬セマンティック画像を位置合わせして最適化を実行し、前記模擬セマンティック画像が最適化されたときの相対姿勢行列を導出する工程と、
前記自己位置算出部により、前記最適化処理部により導出された前記相対姿勢行列と、前記初期位置情報と、を用いて、前記移動体の自己位置を算出する工程と、
を備え、
前記最適化処理部は、前記評価関数において、前記模擬セマンティック画像に含まれるピクセルに設定される重み変数を用いて、前記最適化の際の前記一致度を算出する、自己位置推定方法。

【請求項7】

周辺を撮像するカメラ（１２２）を有する移動体（１０）に搭載され、前記移動体の位置を推定する自己位置推定プログラムであって、
前記移動体の位置を表す初期位置情報を取得する機能と、
セマンティックラベルが付与された３Ｄ地図を表す３Ｄ地図情報を取得する機能と、
前記３Ｄ地図情報上において前記初期位置情報が示す位置から前記カメラを通して見える画像であって深度情報を含む模擬セマンティック画像、を生成する機能と、
前記カメラが撮像した周辺画像を対象にセマンティックセグメンテーションを実行して得られる画像である実セマンティック画像を生成する機能と、
前記模擬セマンティック画像に含まれるピクセルに設定される重み変数を含む評価関数を用いて前記模擬セマンティック画像と前記実セマンティック画像との一致度を算出し、前記一致度が高くなるように前記模擬セマンティック画像を位置合わせして最適化を実行し、前記模擬セマンティック画像が最適化されたときの相対姿勢行列を導出する機能と、
導出された前記相対姿勢行列と、前記初期位置情報と、を用いて、前記移動体の自己位置を算出する機能と、
をコンピュータに実現させる、自己位置推定プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、自己位置推定装置、自己位置推定方法および自己位置推定プログラムに関する。

【背景技術】

【0002】

例えば、非特許文献１には、画像解析技術としてセマンティックセグメンテーションを用いた自己位置推定の手法が開示されている。上記文献では、まず、ＧＰＳ等から得られる不正確な初期位置を用いて、セマンティックラベルが付与された３Ｄモデル上でレンダリングすることにより深度情報を含む模擬セマンティック画像を生成する。他方、移動体に搭載されたカメラから得た画像を用いて、セマンティックセグメンテーションを通して実セマンティック画像を生成する。そして、模擬セマンティック画像と実セマンティック画像とを、一つの評価関数を用いてマッチングさせることにより、自己位置を特定する。

【先行技術文献】

【非特許文献】

【0003】

【非特許文献1】Markus Herb, Matthias Lemberger, Marcel M. Schmitt, Alexander Kurz, Tobias Weiherer, Nassir Navab and Federico Tombari(2021),Semantic Image Alignment for Vehicle Localization

【発明の概要】

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

【0004】

しかし、上記非特許文献１による構成では、高速道路や幹線道路のような特徴が少ない単調な場面において、道路等の大部分を占めるピクセルについて、模擬セマンティック画像と実セマンティック画像と合っていると、実際には位置がずれていても模擬セマンティック画像と実セマンティック画像とが一致していると判断されるため、位置を正しく推定することができないという問題があった。そこで、自己位置推定の精度を向上させることが可能な自己位置推定装置が望まれる。

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

【0005】

本開示は、以下の形態として実現することが可能である。

【0006】

本開示の一形態によれば、自己位置推定装置が提供される。この自己位置推定装置は、周辺を撮像するカメラ（１２２）を有する移動体（１０）に搭載され、前記移動体の位置を推定する自己位置推定装置であって、前記移動体の位置を表す初期位置情報を取得する初期位置取得部（１１１）と、セマンティックラベルが付与された３Ｄ地図を表す３Ｄ地図情報を取得する地図情報取得部（１１２）と、前記３Ｄ地図情報上において前記初期位置情報が示す位置から前記カメラを通して見える画像であって深度情報を含む模擬セマンティック画像、を生成する模擬セマンティック画像生成部（１１３）と、前記カメラが撮像した周辺画像を対象にセマンティックセグメンテーションを実行して得られる画像である実セマンティック画像を生成する実セマンティック画像生成部（１１４）と、評価関数を用いて前記模擬セマンティック画像と前記実セマンティック画像との一致度を算出し、前記一致度が高くなるように前記模擬セマンティック画像を位置合わせして最適化を実行し、前記模擬セマンティック画像が最適化されたときの相対姿勢行列を導出する最適化処理部（１１５）と、前記最適化処理部により導出された前記相対姿勢行列と、前記初期位置情報と、を用いて、前記移動体の自己位置を算出する自己位置算出部（１１６）と、を備え、前記最適化処理部は、前記評価関数において、前記模擬セマンティック画像に含まれるピクセルに設定される重み変数を用いて、前記最適化の際の前記一致度を算出する。

【0007】

この形態の自己位置推定装置によれば、最適化処理部により、模擬セマンティック画像と実セマンティック画像の一致度が高くなるように、模擬セマンティック画像を位置合わせすることにより、模擬セマンティック画像が最適化されたときの相対姿勢行列が導出される。そして、最適化処理により得られた相対姿勢行列と、初期位置取得部により取得された初期位置情報とを用いて、移動体の位置姿勢が推定される。また、模擬セマンティック画像と実セマンティック画像との最適化の一致度を照合する際には、模擬セマンティック画像に含まれるピクセルに設定される重み変数を用いた評価関数により行われる。このため、例えば位置を一意に定めやすい目印となるピクセルに関しては、最適化において重みを大きくするなどの調整が可能となる。これにより、模擬セマンティック画像と実セマンティック画像との一致を照合する際の精度を、重み変数を持たない評価関数を用いる場合と比較して向上させることができる。ひいては、自己位置推定の精度を向上させることができる。

【図面の簡単な説明】

【0008】

【図1】本開示の第１実施形態における自己位置推定装置の概略的な機能的構成を示すブロック図である。

【図2】自己位置推定装置が実行する自己位置推定処理の手順を示すフローチャートである。

【図3】第１実施形態の手法による最適化後の縦位置誤差をプロットしたグラフである。

【図4】比較形態の手法による最適化後の縦位置誤差をプロットしたグラフである。

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

【0009】

以下、実施形態について図面に基づいて説明する。
Ａ．第１実施形態：
Ａ１．自己位置推定装置１１０の構成：
第１実施形態の自己位置推定装置１１０の構成について、図１を参照して説明する。第１実施形態の自己位置推定装置１１０は、移動体としての車両１０に搭載され、走行中の車両１０の位置を推定する。自己位置推定装置１１０は、各面にセマンティックラベルが付与された３Ｄ地図と、車両１０が有するカメラ１２２により実際に撮像したカメラ画像から得られるセマンティック画像を位置合わせすることにより自車の位置姿勢を推定する。車両１０は、自己位置推定装置１１０により推定された自己位置に基づいて自動運転による移動を行うことができる。なお、「自己位置」とは、移動体の位置および向きを含むものであり、位置姿勢を意味する。

【0010】

図１に示すように、車両１０は、自己位置推定装置１１０と、カメラ１２２と、物体センサ１２４と、車両位置センサ１２６と、地図情報記憶部１３０と、通信部１４０と、を備える。車両１０は、その他、図示しない車両制御部を備えており、自己位置推定装置１１０と車両制御部とは電気的に接続されている。

【0011】

カメラ１２２は、車両１０の周辺を撮像して画像を取得する。物体センサ１２４は、地物の位置の情報を取得する。物体センサ１２４として、例えば、レーザーレーダー、ミリ波レーダー、超音波センサ等の反射波を利用した物体センサが挙げられる。本実施形態において、物体センサ１２４は、レーザにより赤外線を照射して赤外線の反射光に基づいて地物の位置の情報を取得する赤外線センサ１２５を有する。本実施形態において、「地物」は、街路樹における葉の部分のみや、路上に積もった雪も含む概念である。

【0012】

車両位置センサ１２６は、現在の車両１０の位置を検出する。車両位置センサ１２６として、例えば、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅm）などの汎地球航法衛星システム（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ（ｓ）（ＧＮＳＳ））が挙げられる。本実施形態において、車両位置センサ１２６が検出する車両１０の位置を表す情報を初期位置情報という。

【0013】

地図情報記憶部１３０は、地図情報を記憶する。「地図情報」は、各地物の位置を示す情報である。本実施形態において、地図情報は、セマンティックラベルが付与された３Ｄ地図情報である。

【0014】

自己位置推定装置１１０は、初期位置取得部１１１と、地図情報取得部１１２と、模擬セマンティック画像生成部１１３と、実セマンティック画像生成部１１４と、最適化処理部１１５と、自己位置算出部１１６と、を備える。自己位置推定装置１１０は、中央処理装置（ＣＰＵ）や、ＲＡＭ、ＲＯＭにより構成されたマイクロコンピュータ等からなり、予めインストールされたプログラムをマイクロコンピュータが実行することによって、これらの各部の機能を実現する。ただし、これらの各部の機能の一部又は全部をハードウェア回路で実現してもよい。

【0015】

初期位置取得部１１１は、車両位置センサ１２６から、車両１０の現在の位置の情報である初期位置情報を取得する。初期位置情報は、車両１０の正確な位置情報に対して誤差を含んでいる。

【0016】

地図情報取得部１１２は、地図情報記憶部１３０から、初期位置情報が表す位置に応じた範囲に含まれる地物の位置の情報である３Ｄ地図情報を取得する。

【0017】

模擬セマンティック画像生成部１１３は、初期位置情報を用いて、取得された３Ｄ地図情報上で初期位置情報からカメラ１２２を通して見える画像であって、深度情報を含む模擬セマンティック画像を生成する。

【0018】

実セマンティック画像生成部１１４は、カメラ１２２が撮像した実際の周辺画像を対象にセマンティックセグメンテーションを実行して得られる画像である実セマンティック画像を生成する。

【0019】

最適化処理部１１５は、評価関数を用いて、模擬セマンティック画像と実セマンティック画像との一致度を算出し、一致度が高くなるように、模擬セマンティック画像を位置合わせして最適化を実行する。そして、模擬セマンティック画像が最適化されたときの相対姿勢行列を導出する。なお、具体的な評価関数および、模擬セマンティック画像や実セマンティック画像の生成を含めた自己位置推定の内容の詳細については、後述の「自己位置推定方法」において詳細に説明する。

【0020】

自己位置算出部１１６は、最適化処理部１１５により導出された相対姿勢行列と、初期位置情報と、を用いて、車両１０の自己位置を算出する。

【0021】

Ａ２．自己位置推定方法：
次に、上記自己位置推定装置１１０を用いた車両１０の自己位置推定方法について、説明する。自己位置推定の基本的な流れについて説明する。まず、予めＧＰＳ等の車両位置センサ１２６から初期位置姿勢Ｐ_Ｒを取得し、セマンティックラベルが付与された３Ｄ地図上でその姿勢Ｐ_Ｒからカメラ１２２を通して見えるセマンティック画像をシミュレーションする。つまり、模擬セマンティック画像を生成する。このとき生成された模擬セマンティック画像は、３Ｄ地図情報を基に作成しているため、ピクセル毎に深度情報を持っている。

【0022】

次に、カメラ１２２により実際に撮像して得られた車載カメラ画像をセマンティックセグメンテーションに通し、実セマンティック画像を生成する。そして、模擬カメラセマンティック画像を、実セマンティック画像へ近づけるように回転・並進移動させる。回転・並進移動は、相対姿勢行列「^ＦＰ_Ｒ」で表すことができる。この相対姿勢行列^ＦＰ_Ｒを調整することにより、実セマンティック画像との一致度が高くなるように位置合わせを行う。この最適化処理により得られた相対姿勢行列^ＦＰ_Ｒと、初期位置姿勢Ｐ_Ｒとを用いて、車両１０の位置姿勢Ｐ_Ｆは次の式（１）のように求めることができる。

【数1】

以上より、相対姿勢行列^ＦＰ_Ｒを求めることができれば、自車カメラ１２２の位置姿勢、すなわち車両１０の自己位置の推定が行える。すなわち、本処理は、誤差を含む不正確な初期位置姿勢Ｐ_Ｒから、セマンティック画像処理を用いて、現在のカメラ１２２の位置姿勢Ｐ_Ｆを精度よく求めるものである。

【0023】

次に、自己位置推定方法の具体的な処理手順について、図２を参照して説明する。この処理は、車両１０の走行中、自己位置推定装置１１０により繰り返し実行される処理であり、例えば１００ｍｓ毎に繰り返し実行される処理である。

【0024】

自己位置推定処理が開始されると、ステップ１０１（以下、「ステップ」を「Ｓ」と略す）において、初期位置取得部１１１により、車両位置センサ１２６から初期位置情報が取得される。

【0025】

次に、Ｓ１０２において、模擬セマンティック画像生成部１１３により、初期位置情報が示す位置からカメラ１２２を通して見える画像であって、深度情報を含む模擬セマンティック画像が生成される。このとき、初期位置情報が表す位置に応じた範囲に含まれる地物の位置の情報である３Ｄ地図情報が、地図情報取得部１１２により地図情報記憶部１３０から取得される。そして、取得された３Ｄ地図情報が用いられることにより、模擬セマンティック画像が生成される。ここで、模擬セマンティック画像は、３Ｄ地図から切り出しているため、各ピクセルは深度情報を持っている。

【0026】

次に、Ｓ１０３において、実セマンティック画像生成部１１４により、カメラ１２２が撮像した実際の周辺画像をセマンティックセグメンテーションを通すことにより、実セマンティック画像が生成される。

【0027】

次に、Ｓ１０４において、最適化処理部１１５により、相対姿勢行列^ＦＰ_Ｒにより模擬セマンティック画像が回転・並進移動される。相対姿勢初期値では、模擬セマンティック画像の回転・並進移動は行わない。次に、Ｓ１０５において、模擬セマンティック画像と実セマンティック画像との一致度が計算される。

【0028】

そして、Ｓ１０６において、一致度を基に相対姿勢行列^ＦＰ_Ｒの最適化が実施される。Ｓ１０７において、Ｓ１０４以降の処理が予め定められた所定回数であるＮ回繰り返されたか否かが判断される。Ｎ回の繰り返しが終了していない場合には（Ｓ１０７：ＮＯ）、Ｓ１０４に戻り、Ｓ１０４からＳ１０６の処理が繰り返される。Ｓ１０４～Ｓ１０７の処理は、各移動パラメータを適宜変化させて模擬セマンティック画像の回転・並進移動を行い、位置合わせすることにより、最適な相対姿勢行列^ＦＰ_Ｒを求める最適化ループを回す処理である。

【0029】

具体的には、模擬セマンティック画像におけるセマンティックの境界でエッジを抽出して、３Ｄのエッジ点画像を作製する。そして、作製された点群を回転・並進移動させることで実セマンティック画像へ近づけていく処理を行っている。すなわち、模擬セマンティック画像のエッジ点画像と実セマンティック画像との一致度が高まるように、エッジ点を回転・並進移動する行例の最適化が実施される。

【0030】

一方、Ｎ回の繰り返しが終了した場合には（Ｓ１０７：ＹＥＳ）、Ｓ１０８に進み、自己位置算出部１１６により、Ｓ１０６で得られた最適化された相対姿勢行列^ＦＰ_Ｒと初期位置姿勢Ｐ_Ｒとを上記式（１）に代入することにより、車両１０の自己位置姿勢Ｐ_Ｆが算出される。以上で、本処理ルーチンは終了する。

【0031】

以下、最適化処理部１１５による、上記Ｓ１０４～Ｓ１０６の最適化処理についてさらに詳しく説明する。同じカメラから得られた異なる２枚の画像Ｉ_１，Ｉ_２があるとし、その画像を取った位置・姿勢をそれぞれＰ_１，Ｐ_２、画像の深度はそれぞれ、Ｄ_１，Ｄ_２とする。ここで、画像Ｉを、以下の式（２）として仮定する。深度Ｄを、以下の式（３）として仮定する。

【数2】

【数3】

【0032】

画像Ｉ_１中のピクセル位置ｘを画像Ｉ_２中のピクセル位置に変換する射影関数を、以下の式（４）と表す。

【数4】

ピクセル位置ｘにおける深度をＤ（ｘ）、相対変換を^２Ｐ_１と表す。
また、以下の式（５）により表される３次元の点Ｐを、カメラパラメータＫを用いて画像上の位置ｘに投影する関数をπ_ｋとすると、射影関数は以下の式（６）のように表される。

【数5】

【数6】

【0033】

本実施形態では、輝度的な一致度の代わりにセマンティックの一致度を最適化している。模擬セマンティック画像と実セマンティック画像との一致度は、以下の式（７）により算出される。

【数7】

ここで、入力ｘはカメラセマンティック画像のあるピクセルＰにおける予測ロジットであり、Ｎクラスのそれぞれに対しての予測値となっており、以下の式（８）で表される。

【数8】

【0034】

また、ｃはピクセルＰに対応する模擬セマンティック画像の正解クラスのインデックスである。これを基に、最適化対象の評価関数は、以下の式（９）として定義できる。

【数9】

最適化により、上記式（９）を最小とする^Ｆξ_Ｒを見つける。exp（^Ｆξ_Ｒ）＝^ＦＰ_Ｒであるので、^Ｆξ_Ｒを見つけることは、^ＦＰ_Ｒを見つけることと同義である。ここで、ｉは画像中のピクセル、ｘｉは模擬セマンティック画像上でのピクセルｉに対応する画素位置を表す。相対姿勢行列^ＦＰ_Ｒを定める方法としては、ここではガウス・ニュートン法を用いる。その他、レーベンバーグ・マルカート法を用いた最適化手法、またパーティクルフィルターを用いる方法などでもよい。

【0035】

上記式（９）において、「ｋ」は、ピクセルに応じて設定される重み変数である。重み変数ｋは、ピクセルの属するクラスの出現頻度に応じて異なっている。重み変数ｋは、画像内での出現頻度が低いクラスに属するピクセルほど、出現頻度が高いクラスに属するピクセルよりも大きく設定される。「出現頻度」は、点の数で把握することができる。

【0036】

例えば、出現頻度が低い電柱や看板を固定する支柱等のピクセルほど重み変数ｋは大きく、出現頻度が高い道路等のピクセルほど重み変数ｋは小さい。一般に、出現頻度が低いクラスは、位置を一意に定めやすく、出現頻度が高いクラスは、特徴が少なく位置を一意に定めにくい。このため、位置を一意に定めやすい目印となるピクセルに関しては、最適化において重みを大きくし、高速道路や幹線道路のような特徴が少ない物体のピクセルに関しては、重みを小さくする。これにより、模擬セマンティック画像と実セマンティック画像との一致を照合する際の精度を、重み変数ｋを持たない評価関数を用いる場合と比較して向上させることができる。

【0037】

画像一致の手法では、一度の最適化により正しい姿勢へ収束することは稀であるため、複数の周期で最適化を継続して行い、徐々に真値へ近づける。具体的には、任意の１フレーム（１周期目）での最適化により推定した位置姿勢に、オドメトリセンサ（慣性計測ユニット（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）、内界センサ等）から得た移動量を足し合わせた位置姿勢を次のフレーム（２周期目）での初期位置とし、最適化を実施する。

【0038】

［効果］
上記第１実施形態の自己位置推定装置１１０および自己位置推定方法では、模擬セマンティック画像を実セマンティック画像へ近づける最適化処理において、模擬セマンティック画像を、実セマンティック画像と一致する点群が多くなる方向に移動させていく。その際に、重み変数ｋを用いた評価関数（上記式（９））を用いることにより、一致する点群として判定する重みが調整される。

【0039】

比較形態として上記非特許文献１に記載の手法（以下、単に「比較形態の手法」ともいう）では、評価関数として、重み変数ｋを持たない以下の式（１０）を用いている。

【数10】

上記式（１０）を用いた手法では、ピクセル毎に一致度の総和をとっているため、画像中で占める割合が多いクラス、例えば、直線路走行シーンにおける道路などが実セマンティック画像と一致していると、最適化が行われていることになってしまい、それが原因で実際には位置が十分に合わないという問題があった。

【0040】

これに対し、上記第１実施形態では、ピクセル毎に重み変数ｋが設定された評価関数（上記式（９））を用いて一致度が照合されるため、高速道路や幹線道路のような特徴が少ない単調な場面において、道路等の大部分を占めるピクセルが合っている場合に、模擬セマンティック画像と実セマンティック画像とが一致しているものと不正確に判断されることを抑制できる。つまり、模擬セマンティック画像と実セマンティック画像とにおいて、見かけ上、多くのピクセルが一致していたとしても、重みがかけられることによって調整されるため、そのピクセルが仮に位置を一意に定めにくいものであれば、一致度は高くないと判定できる。以上の処理により、最適な相対姿勢行列^ＦＰ_Ｒを導出できる。ひいては、相対姿勢行列^ＦＰ_Ｒを用いた自己位置算出の精度を向上させることができる。

【0041】

図３は、直進路である幹線道路を車両１０が直進しているケースでの、上記第１実施形態の手法による最適化後の縦位置誤差をプロットしたグラフである。図４は、図３と同様に直進路である幹線道路を車両１０が直進しているケースでの、上記比較形態の手法による最適化後の縦位置誤差をプロットしたグラフである。図３，図４において、横軸は最適化実施回数(１目盛り＝２００ｍｓ、１５０目盛り＝３０ｓ）であり、縦軸は縦位置誤差（ｍ）である。比較形態と上記第１実施形態とにおいて、それぞれ８回の推定試行を行い、そのデータを示している。「縦位置誤差」とは、車両１０の進行方向における位置の誤差である。なお、ここでの「最適化実施回数」は、上記Ｓ１０７での「Ｎ回」に相当するものではなく、Ｓ１０４～Ｓ１０７の処理をＮ回繰り返す一連の工程を「１回」としてカウントしたものである。

【0042】

図４に示すように、比較形態の手法では、最適化を実施しても、全体的に縦位置誤差が３０ｃｍ内に収束しない結果となった。なお、図３，図４において、縦位置誤差＝３０ｃｍのラインを水平な破線で図示している。また、最適化後の平均縦位置誤差μは、３０秒経過時点で、μ＝１．３７９ｍであった。一方、図３に示すように、上記第１実施形態の手法では、ポールの重みをｋ＝１００として試行した結果、６／８件で収束し、収束したケースでの平均縦位置誤差はμ＝０．１７９ｍとなり、改善の効果が確認できた。つまり、上記第１実施形態によれば、自己位置推定の精度を向上させることができる。

【0043】

Ｂ．第２実施形態：
次に、本開示の第２実施形態の自己位置推定装置１１０および自己位置推定方法について説明する。第１実施形態と同様の構成については同じ符号を付し、説明を省略する。なお、第２実施形態を含め以下複数の実施形態は、重み変数ｋの設定の仕方のみが上記第１実施形態とは異なる。

【0044】

第２実施形態の重み変数ｋは、ピクセルのカメラ１２２からの距離に応じて異なり、距離が近いピクセルほど、距離が遠いピクセルよりも小さく設定される。カメラ１２２に近い点ほど画像を占める画素の割合が多くなる一方で、小さな位置誤差が画素ずれに大きく影響するため、姿勢推定の誤差を生じる原因になると考えられる。そこで、奥行き距離が近いピクセルの重みを小さくし、中遠方のピクセルを大きくする。より具体的には、対象点の距離に比例して重みを変えてもよい。

【0045】

第２実施形態によれば、上記第１実施形態と同様に、自己位置推定の精度を向上させることができる。

【0046】

Ｃ．第３実施形態：
次に、本開示の第３実施形態の自己位置推定装置１１０および自己位置推定方法について説明する。第３実施形態の重み変数ｋは、ピクセルの属する物体の性質に応じて異なって設定される。具体的には、木の葉や枝といった植物等の変化しやすい物体の重みを小さくする。変化しやすい物体については、予め記憶された３Ｄ地図情報と、実際のカメラ画像とで異なる場合があり、姿勢推定の誤差を生じる原因になると考えられる。このため、そのような物体のピクセルの重みは小さくして、一致度への影響を小さくすることにより、誤った姿勢へ合わされるのを防ぐことができる。

【0047】

第３実施形態によれば、上記第１実施形態と同様に、自己位置推定の精度を向上させることができる。

【0048】

Ｄ．第４実施形態：
次に、本開示の第４実施形態の自己位置推定装置１１０および自己位置推定方法について説明する。第４実施形態の重み変数ｋは、機械学習により調整されるパラメータとする。重み変数ｋは、位置誤差が小さくなるように評価関数を最小化する値として機械学習により調整され、設定される。

【0049】

第４実施形態によれば、機械学習により適宜最適な重み変数ｋが設定されるため、上記第１実施形態と同様に、自己位置推定の精度を向上させることができる。

【0050】

Ｅ．他の実施形態：
（Ｅ１）上記各実施形態の自己位置推定装置１１０は、車両１０に搭載されるものとしたが、その他、船舶やドローン、自律移動型ロボットなどの物体や、人等の移動体に搭載されていてもよい。

【0051】

（Ｅ２）上記各実施形態の自己位置推定装置１１０および自己位置推定方法では、評価関数として上記式（９）に示すものを採用したが、その他の式であってもよい。ピクセルに応じて設定される重み変数ｋを有するものであって、相対姿勢行列^ＦＰ_Ｒの最適化を行える関数であればよい。

【0052】

（Ｅ３）上記各実施形態の自己位置推定装置１１０および自己位置推定方法の重み変数ｋの設定において、各実施形態における設定手法を適宜組合わせてもよい。例えば、クラスの出現頻度と、対象ピクセルの距離との要素を組合わせて、重み変数ｋを設定してもよい。その他、物体の性質や、機械学習による調整といった要素についても適宜組合わせて重み変数ｋを設定してもよい。

【0053】

（Ｅ４）本開示に記載の自己位置推定装置１１０及びそれら手法は、コンピュータプログラムにより具体化された一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリを構成することによって提供された専用コンピュータにより、実現されてもよい。あるいは、本開示に記載の自己位置推定装置１１０及びそれら手法は、一つ以上の専用ハードウェア論理回路によってプロセッサを構成することによって提供された専用コンピュータにより実現されてもよい。もしくは、本開示に記載の自己位置推定装置１１０及びそれら手法は、一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリと一つ以上のハードウェア論理回路によって構成されたプロセッサとの組合わせにより構成された一つ以上の専用コンピュータにより実現されてもよい。また、コンピュータプログラム（自己位置推定プログラム）は、コンピュータにより実行されるインストラクションとして、コンピュータ読み取り可能な非遷移有形記録媒体に記憶されていてもよい。

【0054】

本開示は、上述の実施形態に限られるものではなく、その趣旨を逸脱しない範囲において種々の構成で実現することができる。例えば、発明の概要の欄に記載した形態中の技術的特徴に対応する各実施形態中の技術的特徴は、上述の課題の一部又は全部を解決するために、あるいは、上述の効果の一部又は全部を達成するために、適宜、差し替えや、組み合わせを行うことが可能である。また、その技術的特徴が本明細書中に必須なものとして説明されていなければ、適宜、削除することが可能である。

【符号の説明】

【0055】

１１０…自己位置推定装置、１１１…初期位置取得部、１１２…地図情報取得部、１１３…模擬セマンティック画像生成部、１１４…実セマンティック画像生成部、１１５…最適化処理部、１１６…自己位置算出部、１２２…カメラ、１２４…物体センサ、１２５…赤外線センサ、１２６・・・車両位置センサ、１３０・・・地図情報記憶部、１４０・・・通信部

【図1】

【図2】

【図3】

【図4】