特開 2022-112754 自律走行車

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

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

(11)【公開番号】P2022112754

(43)【公開日】2022-08-03

(54)【発明の名称】自律走行車

(51)【国際特許分類】

   G05D 1/02 20200101AFI20220727BHJP

【ＦＩ】

G05D1/02 H

【審査請求】未請求

【請求項の数】3

【出願形態】ＯＬ

(21)【出願番号】P 2021008678

(22)【出願日】2021-01-22

(71)【出願人】

【識別番号】000003218

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

(74)【代理人】

【識別番号】100105957

【弁理士】

【氏名又は名称】恩田 誠

(74)【代理人】

【識別番号】100068755

【弁理士】

【氏名又は名称】恩田 博宣

(72)【発明者】

【氏名】松浦 翼

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

5H301

【Ｆターム（参考）】

5H301AA03

5H301AA10

5H301CC03

5H301CC06

5H301CC08

5H301CC10

5H301FF11

5H301GG08

5H301GG09

5H301HH01

(57)【要約】

【課題】相対的な信頼度を算出でき、その信頼度に基づいて統合した車体の位置座標及び姿勢を算出することができる自律走行車を提供する。
【解決手段】Ｌｉｄａｒスキャナ５０により取得したデータにより車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を算出するＬｉｄａｒ用自己位置推定部３１と、路面撮影カメラ６０により取得したデータにより車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を、Ｌｉｄａｒ用自己位置推定部３１における信頼度算出アルゴリズムで算出する路面撮影カメラ用自己位置推定部３２を備える。制御装置２０は、Ｌｉｄａｒ用自己位置推定部３１による車体の位置座標及び姿勢の推定結果及びその信頼度と路面撮影カメラ用自己位置推定部３２による車体の位置座標及び姿勢の推定結果及びその信頼度に基づいて、統合した車体の位置座標及び姿勢を算出する。
【選択図】図２

【特許請求の範囲】

【請求項1】

車体に設置され、自己位置推定するための第１のデータ取得装置と、
前記車体に設置され、自己位置推定するための第２のデータ取得装置と、
前記車体に設置され、前記第１のデータ取得装置により取得したデータにより前記車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を算出する第１の自己位置推定部と、
前記車体に設置され、前記第２のデータ取得装置により取得したデータにより前記車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を、前記第１の自己位置推定部における信頼度算出アルゴリズムで算出する第２の自己位置推定部と、
前記車体に設置され、前記第１の自己位置推定部による前記車体の位置座標及び姿勢の推定結果及びその信頼度と前記第２の自己位置推定部による前記車体の位置座標及び姿勢の推定結果及びその信頼度に基づいて、統合した前記車体の位置座標及び姿勢を算出する統合自己位置算出部と、
を備えることを特徴とする自律走行車。

【請求項2】

前記車体に設置され、前記車体の位置座標及び姿勢が対応付けられた地図データを記憶する記憶部を更に備え、
前記第１の自己位置推定部は、前記第１のデータ取得装置により取得したデータと前記地図データを比較することにより前記車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を算出し、
前記第２の自己位置推定部は、前記第２のデータ取得装置により取得したデータと前記地図データを比較することにより前記車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を、前記第１の自己位置推定部における信頼度算出アルゴリズムで算出することを特徴とする請求項１に記載の自律走行車。

【請求項3】

前記信頼度は、地図データの点群と、データ取得装置により得られた点群との対比において各点についてのずれ量の総和によるものであることを特徴とする請求項２に記載の自律走行車。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自律走行車に関するものである。

【背景技術】

【0002】

自律走行車において、車体が地図のどの位置に存在するかを把握することは極めて重要である。自己位置検出方法は今日まで、使用するセンサからアルゴリズムを含め、多種多様に存在する。一方で、一つのアルゴリズムだけで、精度のよい自己位置推定が常にできるわけではない。このため、複数のセンサ、アルゴリズム等を用いて、自己位置を推定する方法がある。特許文献１に開示の移動体においては、第１のセンサによって取得された移動体の周囲の情報を用いて移動体の位置を推定することによって、第１の推定位置と、当該第１の推定位置に対応する第１の信頼度とを取得する。また、第２のセンサによって取得された移動体の周囲の情報を用いて移動体の位置を推定することによって、第２の推定位置と、当該第２の推定位置に対応する第２の信頼度とを取得する。さらに、第１及び第２の推定位置と、第１及び第２の信頼度とを用いて、移動体の現在位置を取得する。

【先行技術文献】

【特許文献】

【0003】

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

【発明の概要】

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

【0004】

ところで、複数のアルゴリズムを併用して自己位置推定を行う場合、信頼度、即ち、どの推定結果が最も確からしいかを表す指標が必要であるが、信頼度はアルゴリズムごとに算出方法や値の範囲が異なる。例えば、第１のアルゴリズムでの信頼度算出方法においては信頼度が０～１００の範囲の値をとり、第２のアルゴリズムでの信頼度算出方法においては信頼度が０～１の範囲の値をとる。このため、信頼度を重み付けとした自己位置推定結果の統合（マージ）は困難である。

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

【0005】

上記課題を解決するための自律走行車は、車体に設置され、自己位置推定するための第１のデータ取得装置と、前記車体に設置され、自己位置推定するための第２のデータ取得装置と、前記車体に設置され、前記第１のデータ取得装置により取得したデータにより前記車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を算出する第１の自己位置推定部と、前記車体に設置され、前記第２のデータ取得装置により取得したデータにより前記車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を、前記第１の自己位置推定部における信頼度算出アルゴリズムで算出する第２の自己位置推定部と、前記車体に設置され、前記第１の自己位置推定部による前記車体の位置座標及び姿勢の推定結果及びその信頼度と前記第２の自己位置推定部による前記車体の位置座標及び姿勢の推定結果及びその信頼度に基づいて、統合した前記車体の位置座標及び姿勢を算出する統合自己位置算出部と、を備えることを要旨とする。

【0006】

これによれば、第１の自己位置推定部においては、第１のデータ取得装置により取得したデータにより車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を算出する。また、第２のデータ取得装置においては、第２のデータ取得装置により取得したデータにより車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を、第１の自己位置推定部における信頼度算出アルゴリズムで算出する。これにより、相対的な信頼度を算出でき、その信頼度に基づいて統合した車体の位置座標及び姿勢を算出することができる。

【0007】

また、自律走行車において、前記車体に設置され、前記車体の位置座標及び姿勢が対応付けられた地図データを記憶する記憶部を更に備え、前記第１の自己位置推定部は、前記第１のデータ取得装置により取得したデータと前記地図データを比較することにより前記車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を算出し、前記第２の自己位置推定部は、前記第２のデータ取得装置により取得したデータと前記地図データを比較することにより前記車体の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を、前記第１の自己位置推定部における信頼度算出アルゴリズムで算出するとよい。

【0008】

また、自律走行車において、前記信頼度は、地図データの点群と、データ取得装置により得られた点群との対比において各点についてのずれ量の総和によるものであるとよい。

【発明の効果】

【0009】

本発明によれば、相対的な信頼度を算出でき、その信頼度に基づいて統合した車体の位置座標及び姿勢を算出することができる。

【図面の簡単な説明】

【0010】

【図1】実施形態における自律走行車の側面図。

【図2】自律走行車の電気的構成を示すブロック図。

【図3】作用を説明するためのフローチャート。

【図4】環境と自律走行車の位置を示す図。

【図5】（ａ）取得した点群を示す図、（ｂ）地図データでの点群を示す図。

【図6】取得した環境と自律走行車を示す図。

【図7】取得した点群と地図データでの点群を重ねた状態を示す図。

【図8】取得した点群と地図データでの点群を重ねた状態を示す図。

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

【0011】

以下、本発明を具体化した一実施形態を図面に従って説明する。
図１に示すように、自律走行車１０は、四輪車両であって、車体１１と、車体１１の下部に配置された駆動輪１２と、車体１１の下部に配置された操舵輪１３と、を備える。自律走行車１０は、駆動輪１２の回転により走行されるとともに操舵輪１３の向きの変更により操舵される。ここで、自律走行車１０の進行方向をＸ方向とし、進行方向（Ｘ方向）に直交する方向（自律走行車１０の幅方向）をＹ方向としている。また、水平方向であるＸ－Ｙ平面に対し直交する上下方向をＺ方向としている。

【0012】

図２に示すように、自律走行車１０は、制御装置２０と、モータドライバ２１，２２と、走行モータ２３と操舵モータ２４を備える。制御装置２０は、モータドライバ２１を介して走行モータ２３を制御して駆動輪１２を駆動することができるようになっている。制御装置２０は、モータドライバ２２を介して操舵モータ２４を制御して操舵輪１３を駆動することができるようになっている。

【0013】

図２に示すように、制御装置２０は、処理部３０と、記憶部４０を備える。記憶部４０には、自律走行車１０を制御するための種々のプログラムが記憶されている。
制御装置２０は、各種処理のうち少なくとも一部の処理を実行する専用のハードウェア、例えば、特定用途向け集積回路：ＡＳＩＣを備えていてもよい。制御装置２０は、コンピュータプログラムに従って動作する１つ以上のプロセッサ、ＡＳＩＣ等の１つ以上の専用のハードウェア回路、あるいは、それらの組み合わせを含む回路として構成し得る。プロセッサは、ＣＰＵ、並びに、ＲＡＭ及びＲＯＭ等のメモリを含む。メモリは、処理をＣＰＵに実行させるように構成されたプログラムコードまたは指令を格納している。メモリ、即ち、コンピュータ可読媒体は、汎用または専用のコンピュータでアクセスできるあらゆるものを含む。

【0014】

制御装置２０は、記憶部４０に記憶されたプログラムに従い、走行モータ２３及び操舵モータ２４を制御することで、自律走行車１０を動作させる。本実施形態の自律走行車１０は、搭乗者による操作が行われることなく、制御装置２０による制御によって自動で走行、操舵の動作を行う車両である。

【0015】

図１に示すように、自律走行車１０は、自己位置推定するためのデータ取得装置として、Ｌｉｄａｒ（Light Detection and Ranging又はLaser Imaging Detection and Ranging）スキャナ５０と、路面撮影カメラ６０と、ＧＰＳ（Global Positioning System）アンテナ７０を備える。

【0016】

Ｌｉｄａｒスキャナ５０は車体１１の上面に設置されている。Ｌｉｄａｒスキャナ５０は、レーザを自律走行車１０の周囲に照射する照射部と、その反射光を受光する受光部を有している。そして、レーザの照射から受光までの時間を計測することにより周囲に存在する物体までの距離が検出される。レーザの照射は自律走行車１０の進行方向を基準として例えば右側に１３５度、左側に１３５度の計２７０度の範囲にわたりスキャンしながら、その角度に対応付けて物体までの距離が測定される。

【0017】

Ｌｉｄａｒスキャナ５０は、水平方向での物体検出を行う２次元方式のものでも、水平方向及び上下方向での物体検出を行う３次元方式のものでもよい。
図２に示すように、制御装置２０とＬｉｄａｒスキャナ５０が接続されている。制御装置２０には、Ｌｉｄａｒスキャナ５０による測定データが取り込まれる。これにより、自律走行車１０の周囲環境を把握することができる。

【0018】

図１に示すように、路面撮影カメラ６０は、車体１１の下面中央部に設置され、車体１１の下方の路面Ｓｒを撮影することができる。
図１に示すように、自律走行車１０は、照明用の光源６１，６２を備える。光源６１，６２は、車体１１の下面に設置されている。光源６１，６２は、例えば、発光ダイオード（ＬＥＤ）よりなる。光源６１，６２は、路面Ｓｒにおける路面撮影カメラ６０の撮影領域に光を照射するためのものである。光源６１，６２は、車体１１の下面において路面撮影カメラ６０の周囲を囲むように配置されている。光源６１，６２は、路面撮影カメラ６０の撮影タイミングに同期して点灯して路面撮影カメラ６０の撮影領域に光を照射する。

【0019】

図２に示すように、自律走行車１０は、照明ドライバ６３を備える。制御装置２０は、照明ドライバ６３を介して光源６１，６２を制御することができる。
図１に示すように、ＧＰＳアンテナ７０は、車体１１の上面に設置されている。ＧＰＳアンテナ７０は、人工衛星から電波を受信することができる。図２に示すように、自律走行車１０にはＧＰＳ受信機７１が搭載されている。ＧＰＳアンテナ７０は、ＧＰＳ受信機７１を介して制御装置２０と接続されている。制御装置２０は、ＧＰＳアンテナ７０による測量情報を取り込むことができる。

【0020】

図２に示すように、制御装置２０の処理部３０は、Ｌｉｄａｒ用自己位置推定部３１と、路面撮影カメラ用自己位置推定部３２と、ＧＰＳ用自己位置推定部３３を備える。
記憶部４０には、地図データ４１が記憶されている。地図データ４１として、周囲環境データが記憶されている。周辺環境データは、地理的位置情報が対応付けられた状態で作成されている。また、地図データ４１として、予め路面Ｓｒを撮影しておいた地図画像が記憶されている。地図画像は、地理的位置情報が対応付けられた状態で作成されている。

【0021】

このように、自律走行車１０は、車体１１に設置され、車体１１の位置座標及び姿勢が対応付けられた地図データ４１を記憶する記憶部４０を備える。
Ｌｉｄａｒ用自己位置推定部３１は、Ｌｉｄａｒスキャナ５０により計測された周囲環境データから抽出された特徴（点群）と、記憶部４０の地図データ４１から抽出された特徴とを比較することにより自律走行車の自己位置を推定することができる。自己位置とは、車体１１の位置座標及び姿勢（例えば図６の姿勢角θ参照）である。車体１１の位置座標は、車体１１の一点を示す座標であり、例えば、車体１１の水平方向におけるＬｉｄａｒスキャナ５０の設置位置での座標である。詳しくは、現時点での周辺環境（実際の周辺環境）から特徴点を検出するとともに、その特徴点についての特徴量、即ち、特徴点に対する周りでの輝度の大きさの程度を表す特徴量を検出する。同様に、予め求めておいた地図データから特徴点を検出するとともに、その特徴点についての特徴量、即ち、特徴点に対する周りの輝度の大きさの程度を表す特徴量を検出する。そして、現時点での周辺環境（実際の周辺環境）における各特徴点の特徴量と、予め求めておいた地図データにおける各特徴点の特徴量を比較することにより自律走行車の自己位置を推定する。

【0022】

地図データ（周辺環境データ）４１を予め記憶部４０に記憶する場合、周辺環境の座標を環境地図として記憶する。環境地図は、ＳＬＡＭ（Simultaneous Localization and Mapping）によるマッピングにより作成される。ＳＬＡＭは、移動体の自己位置推定と環境地図作成を同時に行うものであり、移動体が未知の環境下で環境地図を作成できる。構築した地図情報を使って特定のタスクを遂行する。自己位置推定の際、Ｌｉｄａｒスキャナ５０で取得した周辺環境と事前に取得した地図データを比較することで自律走行車の自己位置を推定する。

【0023】

路面撮影カメラ用自己位置推定部３２は、路面撮影カメラ６０により撮影された路面Ｓｒの画像から抽出された特徴（点群）と、記憶部４０の地図データ（地図画像）４１から抽出された特徴とを比較することにより自律走行車の自己位置を推定することができる。自己位置とは、車体１１の位置座標及び姿勢である。車体１１の位置座標は、車体１１の一点を示す座標であり、例えば、車体１１の水平方向における路面撮影カメラ６０の設置位置での座標である。詳しくは、現時点での路面画像（実際の画像）である画像から特徴点を検出するとともに、その特徴点についての特徴量、即ち、特徴点のあるピクセルに対する周りのピクセルでの輝度の大きさの程度を表す特徴量を検出する。同様に、予め撮影しておいた地図画像から特徴点を検出するとともに、その特徴点についての特徴量、即ち、特徴点のあるピクセルに対する周りのピクセルでの輝度の大きさの程度を表す特徴量を検出する。そして、現時点での路面画像（実際の画像）における各特徴点の特徴量と、予め撮影しておいた地図画像における各特徴点の特徴量を比較することにより自律走行車の自己位置を推定する。

【0024】

地図データ（地図画像）４１を予め記憶部４０に記憶する場合、路面の模様の座標を環境地図として記憶する。環境地図は、ＳＬＡＭ（Simultaneous Localization and Mapping）によるマッピングにより作成される。ＳＬＡＭは、移動体の自己位置推定と環境地図作成を同時に行うものであり、移動体が未知の環境下で環境地図を作成できる。構築した地図情報を使って特定のタスクを遂行する。自己位置推定の際、カメラで取得した路面画像と事前に取得した地図画像を比較することで自律走行車の自己位置を推定する。

【0025】

ＧＰＳ用自己位置推定部３３は、ＧＰＳアンテナ７０による測量情報から自律走行車の自己位置を推定することができる。自己位置とは、車体１１の位置座標及び姿勢である。車体１１の位置座標は、車体１１の一点を示す座標であり、例えば、車体１１の水平方向におけるＧＰＳアンテナ７０の設置位置での座標である。

【0026】

制御装置２０は、地図上での自律走行車１０の位置を推定する自己位置推定を行いながら走行モータ２３及び操舵モータ２４を制御することで、所望の位置に自律走行車１０を移動させることが可能である。

【0027】

次に、作用について説明する。
図３に、制御装置２０が実行する処理を示す。
前提条件として、３つのアルゴリズムを用いて、自己位置推定を行う。つまり、アルゴリズムＡを用いて自己位置推定を行うとともにアルゴリズムＢを用いて自己位置推定を行う。さらに、アルゴリズムＣを用いて自己位置推定を行う。そして、アルゴリズムＡを用いて算出された位置推定結果をＬＡとするとともに、アルゴリズムＢを用いて算出された位置推定結果をＬＢとする。さらに、アルゴリズムＣを用いて算出された位置推定結果をＬＣとする。

【0028】

具体的には、アルゴリズムＡを、Ｌｉｄａｒスキャナ５０を用いたＬｉｄａｒによる自己位置推定とする。また、アルゴリズムＢを、路面撮影カメラ６０を用いた路面画像による自己位置推定とする。また、アルゴリズムＣを、ＧＰＳアンテナ７０を用いたＧＰＳ測位による自己位置推定とする。

【0029】

制御装置２０は、ステップＳ１０において、アルゴリズムごとに自己位置推定を行う。具体的には、Ｌｉｄａｒスキャナ５０による周辺環境データと地図データを参照して車体１１の位置座標及び姿勢を算出する。また、路面撮影カメラ６０による路面の撮像画面と地図データを参照して車体１１の位置座標及び姿勢を算出する。さらに、ＧＰＳアンテナ７０による人工衛星からのデータから車体１１の位置座標及び姿勢を算出する。

【0030】

制御装置２０は、ステップＳ１１において、位置推定結果がＬＡとした場合の信頼度をアルゴリズムＡに基づいて算出する。具体的には、Ｌｉｄａｒスキャナ５０による周辺環境データに基づく車体１１の位置座標及び姿勢について信頼度をアルゴリズムＡに基づいて算出する。

【0031】

制御装置２０は、ステップＳ１２において、位置推定結果がＬＢとした場合の信頼度をアルゴリズムＡに基づいて算出する。具体的には、路面撮影カメラ６０による路面の撮像画面に基づく車体１１の位置座標及び姿勢について信頼度をアルゴリズムＡに基づいて算出する。

【0032】

制御装置２０は、ステップＳ１３において、位置推定結果がＬＣとした場合の信頼度をアルゴリズムＡに基づいて算出する。具体的には、ＧＰＳアンテナ７０による人工衛星からのデータに基づく車体１１の位置座標及び姿勢について信頼度をアルゴリズムＡに基づいて算出する。

【0033】

ここまでの処理を経て、アルゴリズムＡの観点における、位置推定結果ＬＡ、位置推定結果ＬＢ及び位置推定結果ＬＣの信頼度が分かる。
制御装置２０は、ステップＳ１４において、それぞれの信頼度に基づいて、統合（マージ）した位置推定結果を算出して出力する。具体的には、例えば、重み付き和（重み和）とし、信頼度に応じた重み付けを行い、その総和を自己位置とする。

【0034】

以下、信頼度算出方法の例について説明する。
前提条件として、図４に示すような環境、位置に自律走行車１０がいるとする。図４において、周辺環境として、自車の周囲には物体による直線Ｌ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５，Ｌ６，Ｌ７，Ｌ８を有する。この場合、図５（ａ）に示すような点群、即ち、点Ｐ１～Ｐ３６が取得できる。また、図４の環境下で地図作成をあらかじめ行い、図５（ｂ）に示すようなその地図の点群、即ち、点Ｐ１０１～Ｐ１３６も持っているとする。

【0035】

つまり、図４は、自律走行車１０のいる環境と位置座標及び姿勢を示している。図５（ａ）は、自律走行車１０が取得した点群（点Ｐ１～Ｐ３６）を示している。図５（ｂ）は、自律走行車１０が持っている地図の点群（点Ｐ１０１～Ｐ１３６）を示している。

【0036】

信頼度を算出するときの一例を説明する。
前提条件として、アルゴリズムＡをＬｉｄａｒスキャナ５０による自己位置推定とし、アルゴリズムＢを路面撮影カメラ６０による自己位置推定とする。

【0037】

第１段階として、自律走行車１０の位置座標及び姿勢が、図６のように推定されたとする。図６は、推定された自律走行車１０の位置座標及び姿勢を示しており、図６において、自律走行車１０の進行方向（Ｘ方向）と所定方位でなす角度が姿勢角θである。図４では姿勢角θ＝０である。

【0038】

第２段階として、図７に示すように、第１段階で推定した自律走行車１０の位置座標及び姿勢と、持っている地図データを重ねる。
第３段階として、第２段階で重ね合わせた結果に対し、図８に示すように、例えば、点Ｐ３に最も近い地図での点Ｐ１３５を見つけてその距離ΔＬ１を計測してスコア（点数）Ｓｃとする。同様に、点Ｐ４に最も近い地図での点Ｐ１３６を見つけてその距離ΔＬ２を計測してスコア（点数）Ｓｃとする。以下同様に、各点ごとのスコアＳｃを算出する。

【0039】

このように、アルゴリズムＡによる信頼度の算出方法としては、推定された位置にＬｉｄａｒスキャナ５０があると仮定した際の実際の入力点それぞれに対し、最も近い地図データの点との距離を算出する。

【0040】

第４段階として、第３段階で求めたスコア（点数）Ｓｃを全点に対し計算し、その合計を自律走行車１０の位置座標及び姿勢に対する信頼度とする。つまり、実際に入力した点と、地図データ上での点との間の距離を求めて、各距離の合計の値を信頼度とする。これが信頼度（スコアＳｃ）となる。具体的には、Ｌｉｄａｒによって算出された位置推定結果をＬＡとして、この位置でのスコアＳｃを信頼度として算出する。

【0041】

同様に、路面画像によって算出された位置推定結果をＬＢとして、この位置でのスコアＳｃを信頼度として算出する。位置推定結果ＬＢにおけるスコアＳｃは、同じ時点で得られたＬｉｄａｒの点群データ（アルゴリズムＡでは位置推定結果ＬＡと推定されるデータ）を位置推定結果ＬＢに合わせた入力点と、最も近い地図データでの点との距離を算出して求める。

【0042】

そして、Ｌｉｄａｒによって算出された位置推定結果ＬＡの信頼度と、路面画像によって算出された位置推定結果ＬＢの信頼度に基づいて統合した位置を算出する。
このように、Ｌｉｄａｒ用自己位置推定部３１は、Ｌｉｄａｒスキャナ５０により取得したデータにより車体１１の位置座標及び姿勢を推定する。詳しくは、Ｌｉｄａｒ用自己位置推定部３１は、Ｌｉｄａｒスキャナ５０により取得したデータと地図データ４１を比較することにより車体１１の位置座標及び姿勢を推定する。一方、路面撮影カメラ用自己位置推定部３２は、路面撮影カメラ６０により取得したデータにより車体１１の位置座標及び姿勢を推定する。詳しくは、路面撮影カメラ用自己位置推定部３２は、路面撮影カメラ６０により取得したデータと地図データ４１を比較することにより車体１１の位置座標及び姿勢を推定する。

【0043】

さらに、Ｌｉｄａｒ用自己位置推定部３１は、Ｌｉｄａｒスキャナ５０により取得したデータにより車体１１の位置座標及び姿勢を推定した際の当該推定の結果の信頼度を算出する。また、路面撮影カメラ用自己位置推定部３２は、路面撮影カメラ６０により取得したデータにより車体１１の位置座標及び姿勢を推定する際に、当該推定の結果の信頼度を、Ｌｉｄａｒ用自己位置推定部３１における信頼度算出アルゴリズムで算出する。

【0044】

そして、制御装置２０は、Ｌｉｄａｒ用自己位置推定部３１による車体１１の位置座標及び姿勢の推定結果及びその信頼度と路面撮影カメラ用自己位置推定部３２による車体１１の位置座標及び姿勢の推定結果及びその信頼度に基づいて、統合した車体１１の位置座標及び姿勢を算出して出力する。

【0045】

信頼度について、言及する。
車体１１におけるデータ取得装置の設置位置により信頼度算出アルゴリズムが使える。同じ位置から見ているので、それぞれの自己位置推定ソフトによって信頼度の求め方が違うが、本実施形態では、異なる方法で求めた自己位置に対して同一の信頼度算出アルゴリズムを使用する。

【0046】

また、図８を用いて説明したように、推定した自己位置における点群と地図データの点群とのずれ量の総和は当該位置座標でこの姿勢だと推定した時のＬｉｄａｒスキャナ５０による点群と地図データの点群とのずれ量となる。よって、信頼度は、路面撮影カメラ６０の設置位置がＬｉｄａｒスキャナ５０の設置位置にあったとすると上記信頼度算出アルゴリズムを使って信頼度を求めることができる。

【0047】

従来、複数のアルゴリズムを併用して自己位置推定を行う場合、信頼度はアルゴリズムごとに算出方法や値の範囲が異なり、例えば、第１のアルゴリズムでの信頼度算出方法においては信頼度が０～１００の範囲の値をとり、第２のアルゴリズムでの信頼度算出方法においては信頼度が０～１の範囲の値をとる。このため、信頼度を重み付けとした自己位置推定結果の統合（マージ）は困難であった。本実施形態においては、路面撮影カメラ用自己位置推定部３２において、路面撮影カメラ６０により取得したデータにより車体１１の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を、Ｌｉｄａｒ用自己位置推定部３１における信頼度算出アルゴリズムで算出する。よって、信頼度はアルゴリズムごとの値の範囲が等しくなり、相対的な信頼度を算出でき、その信頼度に基づいて統合した車体１１の位置座標及び姿勢を算出することができる。

【0048】

上記実施形態によれば、以下のような効果を得ることができる。
（１）自律走行車１０の構成として、車体１１に設置され、自己位置推定するための第１のデータ取得装置としてのＬｉｄａｒスキャナ５０と、車体１１に設置され、自己位置推定するための第２のデータ取得装置としての路面撮影カメラ６０を備える。車体１１に設置され、Ｌｉｄａｒスキャナ５０により取得したデータにより車体１１の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を算出する第１の自己位置推定部としてのＬｉｄａｒ用自己位置推定部３１を備える。車体１１に設置され、路面撮影カメラ６０により取得したデータにより車体１１の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を、Ｌｉｄａｒ用自己位置推定部３１における信頼度算出アルゴリズムで算出する第２の自己位置推定部としての路面撮影カメラ用自己位置推定部３２を備える。車体１１に設置され、Ｌｉｄａｒ用自己位置推定部３１による車体１１の位置座標及び姿勢の推定結果及びその信頼度と路面撮影カメラ用自己位置推定部３２による車体１１の位置座標及び姿勢の推定結果及びその信頼度に基づいて、統合した車体１１の位置座標及び姿勢を算出する統合自己位置算出部としての制御装置２０を備える。

【0049】

よって、第１の自己位置推定部としてのＬｉｄａｒ用自己位置推定部３１においては、Ｌｉｄａｒスキャナ５０により取得したデータにより車体１１の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を算出する。また、第２のデータ取得装置としての路面撮影カメラ用自己位置推定部３２においては、路面撮影カメラ６０により取得したデータにより車体１１の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を、第１の自己位置推定部としてのＬｉｄａｒ用自己位置推定部３１における信頼度算出アルゴリズムで算出する。これにより、相対的な信頼度を算出でき、その信頼度に基づいて統合した車体１１の位置座標及び姿勢を算出することができる。

【0050】

（２）車体１１に設置され、車体１１の位置座標及び姿勢が対応付けられた地図データ４１を記憶する記憶部４０を更に備える。第１の自己位置推定部としてのＬｉｄａｒ用自己位置推定部３１は、第１のデータ取得装置としてのＬｉｄａｒスキャナ５０により取得したデータと地図データ４１を比較することにより車体１１の位置座標及び姿勢を推定するとともに、当該推定の結果の信頼度を算出する。第２の自己位置推定部としての路面撮影カメラ用自己位置推定部３２は、第２のデータ取得装置としての路面撮影カメラ６０により取得したデータと地図データ４１を比較することにより車体１１の位置座標及び姿勢を推定する。また、第２の自己位置推定部としての路面撮影カメラ用自己位置推定部３２は、当該推定の結果の信頼度を、第１の自己位置推定部としてのＬｉｄａｒ用自己位置推定部３１における信頼度算出アルゴリズムで算出する。よって、実用的である。

【0051】

（３）信頼度は、地図データ４１の点群と、データ取得装置（Ｌｉｄａｒスキャナ５０、路面撮影カメラ６０）により得られた点群との対比において各点についてのずれ量の総和によるものであるので、実用的である。

【0052】

実施形態は前記に限定されるものではなく、例えば、次のように具体化してもよい。
・上記実施形態ではアルゴリズムは、アルゴリズムＡ，Ｂ，Ｃの３つ用いたが、アルゴリズムは２つでもよい。また、アルゴリズムは４つ以上でもよい。要は、アルゴリズムは、２以上で実施することができる。

【0053】

例えば、上記実施形態では、自律走行車１０は、Ｌｉｄａｒスキャナ５０と、路面撮影カメラ６０と、ＧＰＳアンテナ７０を備えていた。路面撮影カメラ６０に代わり、床面に敷設した反射板を検出する光学素子を設けて、反射板に沿って走行してもよい。

【0054】

・信頼度に基づいたマージ方法（統合する方法）は問わない。重み和以外にも、例えば、カルマンフィルタ等でもよい。
・位置推定結果がＬＡとした場合の信頼度をアルゴリズムＡに基づいて算出し、位置推定結果がＬＢとした場合の信頼度をアルゴリズムＡに基づいて算出し、位置推定結果がＬＣとした場合の信頼度をアルゴリズムＡに基づいて算出する。その後において、各アルゴリズムで算出された自己位置推定結果を他のアルゴリズムで再度検証（クロスチェック）してもよい。

【符号の説明】

【0055】

１０…自律走行車、１１…車体、２０…制御装置、３１…Ｌｉｄａｒ用自己位置推定部、３２…路面撮影カメラ用自己位置推定部、３３…ＧＰＳ用自己位置推定部、５０…Ｌｉｄａｒスキャナ、６０…路面撮影カメラ、７０…ＧＰＳアンテナ。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】