特開 2024-164679 推定装置、推定プログラム、推定データ生成方法

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

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

(11)【公開番号】P2024164679

(43)【公開日】2024-11-27

(54)【発明の名称】推定装置、推定プログラム、推定データ生成方法

(51)【国際特許分類】

   G01S 17/89 20200101AFI20241120BHJP

   G01C 21/30 20060101ALI20241120BHJP

   G05D 1/43 20240101ALI20241120BHJP

   G01S 17/86 20200101ALI20241120BHJP

【ＦＩ】

G01S17/89

G01C21/30

G05D1/02 A

G01S17/86

【審査請求】未請求

【請求項の数】7

【出願形態】ＯＬ

(21)【出願番号】P 2023080340

(22)【出願日】2023-05-15

(71)【出願人】

【識別番号】000004260

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

(74)【代理人】

【氏名又は名称】矢作 和行

(74)【代理人】

【識別番号】100121991

【弁理士】

【氏名又は名称】野々部 泰平

(74)【代理人】

【識別番号】100145595

【弁理士】

【氏名又は名称】久保 貴則

(72)【発明者】

【氏名】南口 雄一

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

2F129

5H301

5J084

【Ｆターム（参考）】

2F129AA03

2F129BB08

2F129BB15

2F129BB20

2F129BB22

2F129BB33

2F129BB49

2F129GG17

2F129GG18

5H301AA01

5H301BB05

5H301BB14

5H301CC03

5H301CC06

5H301CC10

5H301EE06

5H301EE13

5H301FF04

5H301GG07

5H301GG08

5H301GG09

5H301GG10

5H301GG14

5H301GG16

5J084AA05

5J084AB20

5J084AC02

5J084AC07

5J084CA31

5J084CA70

5J084DA04

5J084DA07

5J084EA33

(57)【要約】

【課題】自己位置の推定精度に対してロバスト性を確保する推定装置の提供。
【解決手段】推定装置のプロセッサは、内界にて取得の内界データに基づくデッドレコニングによりメイン推定位置Ｐｄを取得することと、外界におけるルートマーカの観測により取得のマーカ観測データと外界におけるルートマーカのマーカ位置データとに基づくマーカマッチングによりサブ推定位置Ｐｍ１を取得することと、メイン推定位置Ｐｄとサブ推定位置Ｐｍ１とを各々の推定誤差同士の比率に基づき重み付けしたフュージョンにより自己位置Ｐｆの位置推定データＤｆを生成することとを、実行するように構成される。サブ推定位置Ｐｍ１の取得は、マーカ観測データからメイン推定位置Ｐｄを起点に予測のマーカ予測位置と、マーカ位置データにてマーカ予測位置にマッチングするマーカ設置位置との、偏差により補正のメイン推定位置Ｐｄをサブ推定位置Ｐｍ１として取得することを、含む。
【選択図】図８

【特許請求の範囲】

【請求項1】

プロセッサ（１２）を有し、外界を自律走行する自律走行装置（４）の自己位置を推定する推定装置（１）であって、
前記プロセッサは、
前記自律走行装置の内界において取得される内界データ（Ｄｉ）に基づく、デッドレコニングにより前記自己位置としてのメイン推定位置（Ｐｄ）を取得することと、
前記外界に複数設置されたルートマーカ（６）の前記自律走行装置からの観測により取得されるマーカ観測データ（Ｄｏ）と、前記外界において各前記ルートマーカの設定位置であるマーカ設置位置（Ｐｍｐ）を定義したマーカ位置データ（Ｄｍｐ）とに基づく、マーカマッチングにより前記自己位置としてのサブ推定位置（Ｐｍ１）を取得することと、
前記メイン推定位置と前記サブ推定位置とを各々の推定誤差同士の比率に基づき重み付けした、フュージョンにより前記自己位置の位置推定データ（Ｄｆ）を生成することとを、実行するように構成され、
前記サブ推定位置の取得は、
前記マーカ観測データから前記メイン推定位置を起点に予測されるマーカ予測位置（Ｐｐ）と、前記マーカ位置データにおいて前記マーカ予測位置にマッチングする前記マーカ設置位置との、偏差（δＰ）により補正された前記メイン推定位置を前記サブ推定位置として取得することを、含む推定装置。

【請求項2】

前記プロセッサは、
前記メイン推定位置を取得することと、
前記サブ推定位置である、第一サブ推定位置を取得することと、
前記自律走行装置からの前記外界の走査により取得される外界データ（Ｄｅ）と、前記外界をマッピングしたマップデータ（Ｄｍ）とに基づく、マップマッチングにより前記自己位置として第二サブ推定位置（Ｐｍ２）を取得することと、
前記マーカ観測データが取得されるタイミングにおいて、前記メイン推定位置と前記第一サブ推定位置とを各々の推定誤差同士の比率に基づき重み付けした、フュージョンにより前記位置推定データを生成して前記デッドレコニングへフィードバックすることと、
前記外界データが取得されるタイミングにおいて、前記メイン推定位置と前記第二サブ推定位置とを各々の推定誤差同士の比率に基づき重み付けした、フュージョンにより前記位置推定データを生成して前記デッドレコニングへフィードバックすることとを、実行するように構成される請求項１に記載の推定装置。

【請求項3】

前記外界データは、
前記外界を走査した走査点群（Ｃｓ）を、含み、
前記マップデータは、
前記外界を三次元に分割した複数のボクセル（Ｖｍ）別に、前記外界をマッピングしたマップ点群（Ｃｍ）を、含む請求項２に記載の推定装置。

【請求項4】

前記自律走行装置は、
ペアをなす前記ルートマーカ間を結ぶ仮想直線（Ｌ）に対しての直交方位に、前記自律走行装置の走行方向（Ｔｄ）を制御する請求項１又は２に記載の推定装置。

【請求項5】

前記位置推定データの生成は、
生成した前記位置推定データを出力することを、含む請求項１又は２に記載の推定装置。

【請求項6】

外界を自律走行する自律走行装置（４）の自己位置を推定するために記憶媒体（１０）に記憶され、プロセッサ（１２）に実行させる命令を含む推定プログラムであって、
前記自律走行装置の内界において取得される内界データ（Ｄｉ）に基づく、デッドレコニングにより前記自己位置としてのメイン推定位置（Ｐｄ）を取得することと、
前記外界に複数設置されたルートマーカ（６）の前記自律走行装置からの観測により取得されるマーカ観測データ（Ｄｏ）と、前記外界において各前記ルートマーカの設定位置であるマーカ設置位置（Ｐｍｐ）を定義したマーカ位置データ（Ｄｍｐ）とに基づく、マーカマッチングにより前記自己位置としてのサブ推定位置（Ｐｍ１）を取得することと、
前記メイン推定位置と前記サブ推定位置とを各々の推定誤差同士の比率に基づき重み付けした、フュージョンにより前記自己位置の位置推定データ（Ｄｆ）を生成することとを、実行させる前記命令を含み、
前記サブ推定位置の取得は、
前記マーカ観測データから前記メイン推定位置を起点に予測されるマーカ予測位置（Ｐｐ）と、前記マーカ位置データにおいて前記マーカ予測位置にマッチングする前記マーカ設置位置との、偏差（δＰ）により補正された前記メイン推定位置を前記サブ推定位置として取得することを、含む推定プログラム。

【請求項7】

外界を自律走行する自律走行装置（４）の自己位置を推定した位置推定データ（Ｄｆ）を生成するために、プロセッサ（１２）により実行される推定データ生成方法であって、
前記自律走行装置の内界において取得される内界データ（Ｄｉ）に基づく、デッドレコニングにより前記自己位置としてのメイン推定位置（Ｐｄ）を取得することと、
前記外界に複数設置されたルートマーカ（６）の前記自律走行装置からの観測により取得されるマーカ観測データ（Ｄｏ）と、前記外界において各前記ルートマーカの設定位置であるマーカ設置位置（Ｐｍｐ）を定義したマーカ位置データ（Ｄｍｐ）とに基づく、マーカマッチングにより前記自己位置としてのサブ推定位置（Ｐｍ１）を取得することと、
前記メイン推定位置と前記サブ推定位置とを各々の推定誤差同士の比率に基づき重み付けした、フュージョンにより前記位置推定データを生成することとを、含み、
前記サブ推定位置の取得は、
前記マーカ観測データから前記メイン推定位置を起点に予測されるマーカ予測位置（Ｐｐ）と、前記マーカ位置データにおいて前記マーカ予測位置にマッチングする前記マーカ設置位置との、偏差（δＰ）により補正された前記メイン推定位置を前記サブ推定位置として取得することを、含む推定データ生成方法。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、外界を自律走行する自律走行装置の自己位置を推定する、推定技術に関する。

【背景技術】

【0002】

特許文献１に開示の推定技術は、自律走行装置の自己位置を推定する推定手法を、予めシミュレーションされた領域別の推定精度に応じて切り替えている。具体的に第一領域では、自律走行装置からの外界の走査による点群データと、外界のマップデータとを用いて、自己位置が推定されている。一方で第二領域では、外界に複数設置された誘導体の自律走行装置からの検出結果と、外界における誘導体の位置情報とを用いて、自己位置が推定されている。

【先行技術文献】

【特許文献】

【0003】

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

【発明の概要】

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

【0004】

しかし、特許文献１に開示の推定技術において、各領域での推定手法別の推定精度は、例えば天候等の外界状況変化に起因して、時々刻々と変動する。そのため、推定手法を切り替えるだけでは、真の推定精度まで確保され得るとは言い難く、それはロバスト性に欠けるとも言える。

【0005】

本開示の課題は、自己位置の推定精度に対してロバスト性を確保する、推定装置を提供することにある。本開示の別の課題は、自己位置の推定精度に対してロバスト性を確保する、推定プログラムを提供することにある。本開示のさらに別の課題は、自己位置の推定精度に対してロバスト性を確保する、推定データ生成方法を、提供することにある。

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

【0006】

以下、課題を解決するための本開示の技術的手段について、説明する。尚、特許請求の範囲及び本欄に記載された括弧内の符号は、後に詳述する実施形態に記載された具体的手段との対応関係を示すものであり、本開示の技術的範囲を限定するものではない。

【0007】

本開示の第一態様は、
プロセッサ（１２）を有し、外界を自律走行する自律走行装置（４）の自己位置を推定する推定装置（１）であって、
プロセッサは、
自律走行装置の内界において取得される内界データ（Ｄｉ）に基づく、デッドレコニングにより自己位置としてのメイン推定位置（Ｐｄ）を取得することと、
外界に複数設置されたルートマーカ（６）の自律走行装置からの観測により取得されるマーカ観測データ（Ｄｏ）と、外界において各ルートマーカの設定位置であるマーカ設置位置（Ｐｍｐ）を定義したマーカ位置データ（Ｄｍｐ）とに基づく、マーカマッチングにより自己位置としてのサブ推定位置（Ｐｍ１）を取得することと、
メイン推定位置とサブ推定位置とを各々の推定誤差同士の比率に基づき重み付けした、フュージョンにより自己位置の位置推定データ（Ｄｆ）を生成することとを、実行するように構成され、
サブ推定位置の取得は、
マーカ観測データからメイン推定位置を起点に予測されるマーカ予測位置（Ｐｐ）と、マーカ位置データにおいてマーカ予測位置にマッチングするマーカ設置位置との、偏差（δＰ）により補正されたメイン推定位置をサブ推定位置として取得することを、含む。

【0008】

本開示の第二態様は、
外界を自律走行する自律走行装置（４）の自己位置を推定するために記憶媒体（１０）に記憶され、プロセッサ（１２）に実行させる命令を含む推定プログラムであって、
自律走行装置の内界において取得される内界データ（Ｄｉ）に基づく、デッドレコニングにより自己位置としてのメイン推定位置（Ｐｄ）を取得することと、
外界に複数設置されたルートマーカ（６）の自律走行装置からの観測により取得されるマーカ観測データ（Ｄｏ）と、外界において各ルートマーカの設定位置であるマーカ設置位置（Ｐｍｐ）を定義したマーカ位置データ（Ｄｍｐ）とに基づく、マーカマッチングにより自己位置としてのサブ推定位置（Ｐｍ１）を取得することと、
メイン推定位置とサブ推定位置とを各々の推定誤差同士の比率に基づき重み付けした、フュージョンにより自己位置の位置推定データ（Ｄｆ）を生成することとを、実行させる命令を含み、
サブ推定位置の取得は、
マーカ観測データからメイン推定位置を起点に予測されるマーカ予測位置（Ｐｐ）と、マーカ位置データにおいてマーカ予測位置にマッチングするマーカ設置位置との、偏差（δＰ）により補正されたメイン推定位置をサブ推定位置として取得することを、含む。

【0009】

本開示の第三態様は、
外界を自律走行する自律走行装置（４）の自己位置を推定した位置推定データ（Ｄｆ）を生成するために、プロセッサ（１２）により実行される推定データ生成方法であって、
自律走行装置の内界において取得される内界データ（Ｄｉ）に基づく、デッドレコニングにより自己位置としてのメイン推定位置（Ｐｄ）を取得することと、
外界に複数設置されたルートマーカ（６）の自律走行装置からの観測により取得されるマーカ観測データ（Ｄｏ）と、外界において各ルートマーカの設定位置であるマーカ設置位置（Ｐｍｐ）を定義したマーカ位置データ（Ｄｍｐ）とに基づく、マーカマッチングにより自己位置としてのサブ推定位置（Ｐｍ１）を取得することと、
メイン推定位置とサブ推定位置とを各々の推定誤差同士の比率に基づき重み付けした、フュージョンにより位置推定データを生成することとを、含み、
サブ推定位置の取得は、
マーカ観測データからメイン推定位置を起点に予測されるマーカ予測位置（Ｐｐ）と、マーカ位置データにおいてマーカ予測位置にマッチングするマーカ設置位置との、偏差（δＰ）により補正されたメイン推定位置をサブ推定位置として取得することを、含む。

【0010】

これら第一～第三態様によると、内界データに基づくデッドレコニングによるメイン推定位置と、マーカ観測データ及びマーカ位置データに基づくマーカマッチングによるサブ推定位置とが、取得される。ここでサブ推定位置の取得には、マーカ観測データからメイン推定位置を起点に予測されるマーカ予測位置と、マーカ位置データにおいて当該マーカ予測位置にマッチングするマーカ設置位置との、偏差が着目される。これによれば、マーカ予測位置とマーカ設置位置との偏差によってメイン推定位置が補正されることで取得される、サブ推定位置の推定誤差からは、メイン推定位置の推定誤差による影響分を相殺することができる。故に、メイン推定位置とサブ推定位置とが、外界状況変化を反映し得る各々の推定誤差同士での比率に基づき重み付けされてフュージョンされることで、自己位置の推定精度に対してロバスト性を確保した位置推定データの生成が可能となる。

【図面の簡単な説明】

【0011】

【図1】第一実施形態による推定装置の全体構成を示すブロック図である。

【図2】第一実施形態による推定装置が搭載される自律走行装置を、ルートマーカと共に示す斜視図である。

【図3】第一実施形態による推定装置が搭載される自律走行装置を、ルートマーカと共に示す平面図である。

【図4】第一実施形態による推定装置の機能構成を示すブロック図である。

【図5】第一実施形態による推定装置の推定原理を説明するための模式図である。

【図6】第一実施形態による推定装置の推定原理を説明するための模式図である。

【図7】第一実施形態による推定装置の推定原理を説明するための模式図である。

【図8】第一実施形態による推定フローを示すフローチャートである。

【図9】第二実施形態による推定装置が搭載される自律走行装置を、ルートマーカと共に示す平面図である。

【図10】第二実施形態による推定装置の推定原理を説明するための模式図である。

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

【0012】

以下、本開示の実施形態を図面に基づき複数説明する。尚、各実施形態において対応する構成要素には同一の符号を付すことで、重複する説明を省略する場合がある。また、各実施形態において構成の一部分のみを説明している場合、当該構成の他の部分については、先行して説明した他の実施形態の構成を適用することができる。さらに、各実施形態の説明において明示している構成の組み合わせばかりではなく、特に組み合わせに支障が生じなければ、明示していなくても複数の実施形態の構成同士を部分的に組み合わせることができる。

【0013】

（第一実施形態）
図１に示す第一実施形態の推定装置１は、外界を自律走行する自律走行装置４の、自己位置を推定する。ここで本実施形態では、インデックスがＸ，Ｙの水平二方向に対して、インデックスがＺの鉛直方向を想定した、三次元絶対座標系が定義される。さらに、そうした三次元直交座標系において自律走行に必要な姿勢を取る自律走行装置４に対して、インデックスがｘの縦方向と、インデックスがｙの横方向と、インデックスがｚの高さ方向とを想定した、三次元相対座標系が定義される。但し、三次元絶対座標系の鉛直方向（Ｚ方向）と三次元相対座標系の高さ方向（ｚ方向）とは、実質一致するように想定されるものとする。

【0014】

自律走行装置４は、自身の外界となる物流施設において倉庫内外の走行路を自律走行して荷物を運搬する、物流車両又は物流ロボットであってもよい（図２の例）。自律走行装置４は、自身の外界となる交通環境において道路を自律走行可能な、自動運転車両であってもよい。自律走行装置４は、自身の外界となる飲食店内又は病院内の走行路を自律走行して飲食物を配膳する、配膳ロボットであってもよい。自律走行装置４は、自身の外界となる災害地を自律走行して物資を運搬又は情報を収集する、災害支援ロボットであってもよい。自律走行装置４は、これら以外の装置であっても勿論よい。さらにいずれの種の自律走行装置４も、外部センタによりリモートでの走行支援又は走行制御を受けてもよい。

【0015】

自律走行装置４の外界において自律走行対象となる、走行路又は道路における少なくとも一部エリアには、図２，３に示すように複数のルートマーカ６が設置されている。各ルートマーカ６は、例えばフェライト磁石製等の、着磁された磁気マーカを主体に構成されている。各ルートマーカ６は、第一実施形態では自律走行装置４が自律走行する走行ルートにおいて、例えば三次元相対座標系での横方向中心位置等の代表走行位置を連続して与えるように想定される走行軌道Ｔｒ上に、間隔を空けて配置される。各ルートマーカ６は、それぞれの配置箇所における路面に対して、例えば保護シート等により貼り付け、又は埋設されている。全て又は一部のルートマーカ６は、ＲＦＩＤ（radio frequency identification）付きであってもよい。

【0016】

図１に示すように推定装置１は、センサユニット２及びマップユニット３と共に、自律走行装置４に搭載される。センサユニット２は、マーカセンサ２０、外界センサ２２及び内界センサ２４を含んで構成される。

【0017】

図１，２，４に示すマーカセンサ２０は、自律走行装置４の周辺環境である外界において自律走行装置４の運動推定に活用可能な、マーカ観測データＤｏを取得する。マーカセンサ２０は、外界に存在するルートマーカ６を自律走行装置４から観測することで、マーカ観測データＤｏを取得する、磁気センサ又は磁気レシーバを主体に構成されている。マーカセンサ２０は、ＲＦＩＤリーダを含んでいてもよい。こうしたマーカセンサ２０には、設定距離の観測レンジ内に入ったルートマーカ６を観測可能とするための、取付位置及び観測基準位置が自律走行装置４に対して予め設定されているとよい。

【0018】

図１，２，４に示す外界センサ２２は、自律走行装置４の周辺環境である外界において自律走行装置４の運動推定に活用可能な、マーカ観測データＤｏ以外の情報を、外界データＤｅとして取得する。外界センサ２２は、外界に存在する物体を自律走行装置４から検知することで、外界データＤｅを取得してもよい。この検知タイプの外界センサ２２は、例えばＬｉＤＡＲ（Light Detection and Ranging / Laser Imaging Detection and Ranging）、カメラ、レーダ及びソナー等のうち、一種類又は複数種類である。外界センサ２２は、外界に存在する無線通信システムからの無線信号を自律走行装置４から受信することで、外界データＤｅを取得してもよい。この受信タイプの外界センサ２２は、例えばＧＮＳＳ（Global Navigation Satellite System）の受信機及びＩＴＳ（Intelligent Transport Systems）の受信機等のうち、一種類又は複数種類である。以下では、外界をビーム走査して物標からの反射ビームを検知することで、外界データＤｅとして三次元点群画像を取得のＬｉＤＡＲが、外界センサ２２を構成する場合を例にとって説明する。

【0019】

図１，４に示す内界センサ２４は、自律走行装置４の内部環境である内界において自律走行装置４の運動推定に活用可能な情報を、内界データＤｉとして取得する。内界センサ２４は、自律走行装置４の内界において特定の運動物理量を検知することで、内界データＤｉを取得してもよい。この検知タイプの内界センサ２４は、例えば慣性センサ、速度センサ及び舵角センサ等のうち、一種類又は複数種類である。以下では、自律走行装置４の角速度及び／又は姿勢角を内界データＤｉとして取得する慣性センサと、自律走行装置４の速度を内界データＤｉとして取得する速度センサとが、内界センサ２４を構成する場合を例にとって説明する。

【0020】

図１，４に示すマップユニット３は、マップデータＤｍを非一時的に格納又は記憶する、例えば半導体メモリ、磁気媒体及び光学媒体等のうち、一種類又は複数種類の非遷移的実体的記憶媒体（non-transitory tangible storage medium）を含んで構成される。マップユニット３は、自律走行装置４の高度運転支援又は自動運転制御に利用される、ロケータのデータベースであってもよい。マップユニット３は、自律走行装置４の運転をナビゲートする、ナビゲーション装置のデータベースであってもよい。マップユニット３は、自律走行装置４の走行を計画するプランニングユニットの、データベースであってもよい。マップユニット３は、これらのデータベース等のうち複数種類の組み合わせにより、構成されていてもよい。

【0021】

マップユニット３は、例えば外部センタとの通信等により、最新のマップデータＤｍを取得して記憶する。マップデータＤｍは、自律走行装置４の自律走行する外界をマッピングした、マップ点群を含む三次元デジタルマップとして、例えば高精度ダイナミックマップ等が用いられる。こうしたマップデータＤｍは、例えば走行路又は道路の位置、形状、サイズ及び路面状態等のうち、一種類又は複数種類を表した道路情報を含んでいてもよい。マップデータＤｍは、例えば走行路又は道路に付属する標識、区画線及び信号機の位置、形状、サイズ並びに作動状態等のうち、一種類又は複数種類を表した標示情報を含んでいてもよい。マップデータＤｍは、例えば走行路又は道路に面する建造物、構造物及び植栽物の位置、形状、並びにサイズ等のうち、一種類又は複数種類を表した構造物情報を含んでいてもよい。

【0022】

図１，４に示す推定装置１は、例えばＬＡＮ（Local Area Network）、ワイヤハーネス、内部バス、及び無線通信回線等のうち、一種類又は複数種類を介してセンサユニット２及びマップユニット３に接続されている。推定装置１は、自律走行装置４の高度運転支援又は自動運転制御を実行する、運転制御専用のＥＣＵ（Electronic Control Unit）であってもよい。推定装置１は、自律走行装置４の高度運転支援又は自動運転制御に利用される、ロケータのＥＣＵであってもよい。推定装置１は、自律走行装置４の運転をナビゲートする、ナビゲーション装置のＥＣＵであってもよい。推定装置１は、自律走行装置４と外界との間の通信を制御する、通信制御装置のＥＣＵであってもよい。

【0023】

図１に示すように推定装置１は、メモリ１０及びプロセッサ１２を少なくとも一つずつ含んで構成される、専用のコンピュータである。メモリ１０は、コンピュータにより読み取り可能なプログラム及びデータ等を非一時的に格納又は記憶する、例えば半導体メモリ、磁気媒体及び光学媒体等のうち、一種類又は複数種類の非遷移的実体的記憶媒体（non-transitory tangible storage medium）である。プロセッサ１２は、例えばＣＰＵ（Central Processing Unit）、ＧＰＵ（Graphics Processing Unit）及びＲＩＳＣ（Reduced Instruction Set Computer）－ＣＰＵ等のうち、一種類又は複数種類をコアとして含む。

【0024】

プロセッサ１２は、メモリ１０に記憶された推定プログラムに含まれる複数の命令を、実行する。これにより推定装置１は、自律走行装置４の自己位置を推定するための複数の機能ブロックを、図４に示すように構築する。このように推定装置１では、自律走行装置４の自己位置を推定するためにメモリ１０に記憶された推定プログラムが複数の命令をプロセッサ１２に実行させることで、複数の機能ブロックが構築される。

【0025】

推定装置１により構築される複数の機能ブロックには、デッドレコニングブロック１００、マーカマッチングブロック１１０、マップマッチングブロック１２０及びフュージョンブロック１３０が、含まれる。デッドレコニングブロック１００は、内界データＤｉとダイナミクスモデル（Dynamics Model）とに基づくデッドレコニングにより、今回推定時刻ｔにおける自律走行装置４の自己位置を推定することで、当該推定結果をメイン推定位置Ｐｄとして取得する（後に詳述の図５参照）。ここでダイナミクスモデルとは、自律走行装置４の挙動を動力学に基づきモデル化した、例えば二輪モデル等である。

【0026】

そこで、今回推定時刻ｔでのメイン推定位置Ｐｄを構成する三次元位置座標Ｘｄ，Ｙｄ，Ｚｄはそれぞれ、前回推定時刻ｔ－１にて後に詳述の如くフュージョンブロック１３０により推定される三次元位置座標Ｘｆ，Ｙｆ，Ｚｆ（図５参照）を用いた、数１，２，３により定義される。ここで数１，２，３のＶには、内界センサ２４のうち速度センサにより今回推定時刻ｔに検知された内界データＤｉとして、自律走行装置４の速度が入力される。数１，２，３のδｔには、今回推定時刻ｔと前回推定時刻ｔ－１との推定間隔（即ち、サンプリング周期）が入力される。数３のωｐには、内界センサ２４のうち慣性センサにより今回推定時刻ｔに検知された内界データＤｉとして、自律走行装置４の姿勢角のうちピッチ角が入力される。尚、図５及び後述の図１０、並びに数１～３及び後述の数４～３４では、各推定時刻ｔ，ｔ－１での区別の必要なパラメータ（変数）が、それぞれ対応する推定時刻ｔ，ｔ－１の関数形式［ｔ］，［ｔ－１］にて表されている。

【数1】

【数2】

【数3】

【0027】

さらに数１，２のθｄは、今回推定時刻ｔでの姿勢角のうちヨー角であり、前回推定時刻ｔ－１において後に詳述の如くフュージョンブロック１３０により推定されるヨー角θｆ（図５参照）を用いた、数４により定義される。ここで数４のγには、内界センサ２４のうち慣性センサにより今回推定時刻ｔに検知された内界データＤｉとして、自律走行装置４のヨーレートが入力される。数４のδｔには、今回推定時刻ｔと前回推定時刻ｔ－１との推定間隔が入力される。

【数4】

【0028】

こうした、今回推定時刻ｔにおけるメイン推定位置Ｐｄの推定に伴ってデッドレコニングブロック１００は、当該推定に予測される誤差を、図４に示すメイン推定誤差ΔＰｄとして取得する。このとき、今回推定時刻ｔでのメイン推定誤差ΔＰｄは、推定された三次元位置座標Ｘｄ，Ｙｄ，Ｚｄの各々に関する、三次元位置誤差ΔＸｄ，ΔＹｄ，ΔＺｄにより構成される。

【0029】

そこで、今回推定時刻ｔでの三次元位置誤差ΔＸｄ，ΔＹｄ，ΔＺｄはそれぞれ、前回推定時刻ｔ－１において後に詳述の如くフュージョンブロック１３０により推定される三次元位置誤差ΔＸｆ，ΔＹｆ，ΔＺｆを用いた、数５，６，７により定義される。ここで数５，６，７のＶには、内界センサ２４のうち速度センサによる今回推定時刻ｔでの速度が入力される。数５，６，７のΔＶには、内界センサ２４のうち速度センサの誤差特性として予測される速度誤差が、入力される。数５，６，７のδｔには、今回推定時刻ｔと前回推定時刻ｔ－１との推定間隔が入力される。数５，６のθｄには、今回推定時刻ｔでの数４の推定ヨー角が入力される。数７のΔωｐには、内界センサ２４のうち慣性センサの誤差特性として予測されるピッチ角誤差が、入力される。

【数5】

【数6】

【数7】

【0030】

さらに数５，６のΔθｆは、前回推定時刻ｔ－１において後に詳述の如くフュージョンブロック１３０により推定されるヨー角誤差であり、今回推定時刻ｔに推定されるヨー角θｄの誤差Δθｄは、当該前回ヨー角誤差Δθｆを用いた、数８により定義される。ここで数８のΔγには、内界センサ２４のうち慣性センサの誤差特性として予測されるヨーレート誤差が、入力される。数８のδｔには、今回推定時刻ｔと前回推定時刻ｔ－１との推定間隔が入力される。

【数8】

【0031】

以上よりデッドレコニングブロック１００は、メイン推定位置Ｐｄを与える数１，２，３の三次元位置座標Ｘｄ，Ｙｄ，Ｚｄを、数４のヨー角θｄと共に、今回推定時刻ｔに関して推定する。それに伴ってデッドレコニングブロック１００は、メイン推定誤差ΔＰｄを与える数５，６，７の三次元位置誤差ΔＸｄ，ΔＹｄ，ΔＺｄを、数８のヨー角誤差Δθｄと共に、今回推定時刻ｔに関して推定する。そこでデッドレコニングブロック１００は、これらの推定結果を、今回推定時刻ｔと関連付けてメモリ１０に記憶する。

【0032】

図４に示すマーカマッチングブロック１１０は、マーカ観測データＤｏとマーカ位置データＤｍｐとに基づくマーカマッチングにより、今回推定時刻ｔにおける自律走行装置４の自己位置を推定することで、当該推定結果を第一サブ推定位置Ｐｍ１として取得する。このときマーカ観測データＤｏは、今回推定時刻ｔにマーカセンサ２０により観測された場合のルートマーカ６の当該観測位置ｐｏとして、図５に示すように自律走行装置４に対しての三次元相対座標ｘｏ，ｙｏ，ｚｏにより構成される。

【0033】

このため、マーカマッチングブロック１１０においてマーカマッチング処理の対象となる今回推定時刻ｔとは、入力されたマーカ観測データＤｏの取得タイミングとなる。ここでマーカ観測データＤｏの取得タイミングは、ルートマーカ６の設置エリアにおいてマーカセンサ２０によるルートマーカ６の観測周期毎に、設定されてもよい。マーカ観測データＤｏの取得タイミングは、ルートマーカ６の設置エリアにおいてマーカセンサ２０によりルートマーカ６が実際に観測されたタイミングに、設定されてもよい。

【0034】

そこでマーカマッチングブロック１１０は、今回推定時刻ｔでのマーカ観測データＤｏから、デッドレコニングブロック１００による同時刻ｔでのメイン推定位置Ｐｄを起点に、図５のマーカ予測位置Ｐｐを予測する。このため、マーカマッチングブロック１１０へメイン推定位置Ｐｄを入力することになるデッドレコニングブロック１００において、デッドレコニング処理の実行される今回推定時刻ｔは、マーカ観測データＤｏの取得タイミングと実質的に同期させられる。

【0035】

マーカ予測位置Ｐｐとして予測される三次元位置座標Ｘｐ，Ｙｐ，Ｚｐは、マーカ観測位置ｐｏの三次元相対座標ｘｏ，ｙｏ，ｚｏと、マーカセンサ２０の取付位置及び観測基準位置との、座標変換値によりメイン推定位置Ｐｄの三次元位置座標Ｘｄ，Ｙｄ，Ｚｄをそれぞれ補正することで、取得される。そのため、マーカ予測位置Ｐｐの三次元位置座標Ｘｐ，Ｙｐ，Ｚｐに関する予測誤差にはそれぞれ、メイン推定位置Ｐｄの三次元位置座標Ｘｄ，Ｙｄ，Ｚｄに関するメイン推定誤差ΔＰｄと、マーカ観測データＤｏの三次元相対座標ｘｏ，ｙｏ，ｚｏに関する観測誤差とが、含まれることとなる。

【0036】

こうしたマーカ観測データＤｏに対して図４のマーカ位置データＤｍｐは、外界における各ルートマーカ６の設定位置である図５のマーカ設置位置Ｐｍｐとして、それら各マーカ６の三次元位置座標Ｘｍｐ，Ｙｍｐ，Ｚｍｐを含んで入力される。このときマーカ位置データＤｍｐは、マップデータＤｍの一部としてマップユニット３から読み出されてもよい（図１，４の場合）。マーカ位置データＤｍｐは、上述したマップデータＤｍに準じてマップユニット３又はメモリ１０に記憶されていることで、当該記憶先から読み出されてもよい。

【0037】

そこでマーカマッチングブロック１１０は、マーカ位置データＤｍｐにおいて図５の如く今回推定時刻ｔでのマーカ予測位置Ｐｐにマッチングするマーカ設置位置Ｐｍｐと、当該マーカ予測位置Ｐｐとの偏差δＰを認識する。これによりマーカマッチングブロック１１０は、認識した偏差δＰにより今回推定時刻ｔでのメイン推定位置Ｐｄを補正することで、同時刻ｔでの第一サブ推定位置Ｐｍ１を取得する。

【0038】

このとき、第一サブ推定位置Ｐｍ１を構成する図４，５の三次元位置座標Ｘｍ１，Ｙｍ１，Ｚｍ１はそれぞれ、数９，１０，１１により定義される。即ち各三次元位置座標Ｘｍ１，Ｙｍ１，Ｚｍ１は、メイン推定位置Ｐｄの三次元位置座標Ｘｄ，Ｙｄ，Ｚｄから、マーカ予測位置Ｐｐの三次元位置座標Ｘｐ，Ｙｐ，Ｚｐとマーカ設置位置Ｐｍｐの三次元位置座標Ｘｍｐ，Ｙｍｐ，Ｚｍｐとの偏差を、それぞれ図５の偏差δＰとして差し引くことで取得される。それと共に、今回推定時刻ｔでの姿勢角のうち図４，５のヨー角θｍ１は、各位置Ｐｐ，Ｐｍｐの座標Ｙｐ，Ｙｍｐ間での偏差と、時刻ｔ，ｔ－１間での推定間隔における自律走行装置４の移動距離とに基づき、前回推定時刻ｔ－１での推定値θｆを補正することで、取得される。

【数9】

【数10】

【数11】

【0039】

こうした、今回推定時刻ｔにおける第一サブ推定位置Ｐｍ１の推定に伴ってマーカマッチングブロック１１０は、当該推定に予測される誤差を、図４に示す第一サブ推定誤差ΔＰｍ１として取得する。このとき、今回推定時刻ｔでの第一サブ推定誤差ΔＰｍ１は、推定された三次元位置座標Ｘｍ１，Ｙｍ１，Ｚｍ１の各々に関する、三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１により構成される。

【0040】

ここで数９，１０，１１の右辺第二項及び第三項の関係から、各三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１にはそれぞれ、マーカ予測位置Ｐｐの三次元位置座標Ｘｐ，Ｙｐ，Ｚｐに関する予測誤差と、マーカ設置位置Ｐｍｐの三次元位置座標Ｘｍｐ，Ｙｍｐ，Ｚｍｐに関するマップ誤差とが、影響する。これらの誤差のうち特に、右辺第二項の三次元位置座標Ｘｐ，Ｙｐ，Ｚｐに関する予測誤差には、メイン推定位置Ｐｄの三次元位置座標Ｘｄ，Ｙｄ，Ｚｄに関するメイン推定誤差ΔＰｄと、マーカ観測データＤｏの三次元相対座標ｘｏ，ｙｏ，ｚｏに関する観測誤差とが、上述の如く含まれる。

【0041】

そのため、三次元位置座標Ｘｄ，Ｙｄ，Ｚｄに関するメイン推定誤差ΔＰｄ分はそれぞれ、数９，１０，１１の右辺第一項及び第二項の関係により各三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１への影響を実質的に相殺される。故に各三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１にはそれぞれ、三次元相対座標ｘｏ，ｙｏ，ｚｏに関する観測誤差と、三次元位置座標Ｘｍｐ，Ｙｍｐ，Ｚｍｐに関するマップ誤差とが、残ることになる。そこで各三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１は、ルートマーカ６に対するマーカセンサ２０の誤差特性として予測される観測誤差と、ルートマーカ６に対するマーカ位置データＤｍｐの測量誤差とに基づき、それぞれ相異又は同一の固定値に予め設定される。このとき、今回推定時刻ｔでのヨー角θｍ１の推定に予測される誤差Δθｍ１も、同様な固定値に予め設定される。

【0042】

以上よりマーカマッチングブロック１１０は、第一サブ推定位置Ｐｍ１を与える数９，１０，１１の三次元位置座標Ｘｍ１，Ｙｍ１，Ｚｍ１を、ヨー角θｍ１と共に、今回推定時刻ｔに関して推定する。それに伴ってマーカマッチングブロック１１０は、第一サブ推定誤差ΔＰｍ１を与える三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１を、ヨー角誤差Δθｍ１と共に、今回推定時刻ｔに関して推定する。そこでマーカマッチングブロック１１０は、これらの推定結果を、今回推定時刻ｔと関連付けてメモリ１０に記憶する。

【0043】

図４に示すマップマッチングブロック１２０は、外界データＤｅとマップデータＤｍとに基づくマップマッチングにより、今回推定時刻ｔにおける自律走行装置４の自己位置を推定することで、当該推定結果を第二サブ推定位置Ｐｍ２として取得する。このとき外界データＤｅとしては、外界センサ２２のうちＬｉＤＡＲにより今回推定時刻ｔにて図６，７に示すように走査された、三次元相対座標系での走査点群Ｃｓを含む三次元点群画像Ｄｅｓが入力される。

【0044】

このため、マップマッチングブロック１２０においてマップマッチング処理の実行される今回推定時刻ｔは、外界データＤｅとして入力された三次元点群画像Ｄｅｓの取得タイミングとなる。ここで三次元点群画像Ｄｅｓの取得タイミングは、外界センサ２２のうちＬｉＤＡＲによる外界の走査周期毎に、設定される。

【0045】

こうした三次元点群画像Ｄｅｓに対して図６，７のマップデータＤｍとしては、自律走行装置４の外界において物標をマッピングした、マップ点群Ｃｍを含む三次元デジタルマップＤｍｍが入力される。このとき、三次元絶対座標系の三次元デジタルマップＤｍｍにおいて外界の物標をマッピングしたマップ点群Ｃｍは、それぞれの位置座標を三次元相対座標系に変換される。そこで、座標変換されたマップ点群Ｃｍは、三次元相対座標系において外界を分割した複数の三次元ボクセルＶｍ別に、割り振られることとなる。尚、図６，７では、三次元ボクセルＶｍが高さ方向（ｚ方向）の図示を省略した、縦方向（ｘ方向）及び横方向（ｙ方向）の二次元で模式的に図示されている。

【0046】

こうした座標変換及び割振を伴う入力を受けてマップマッチングブロック１２０は、図６，７に対比の如く三次元点群画像Ｄｅｓの走査点群Ｃｓに対してマッチングするマップ点群Ｃｍを、当該マップマッチングの対応関係にある走査点群Ｃｓと共に認識する。これによりマップマッチングブロック１２０は、走査点群Ｃｓとマッチングしたマップ点群Ｃｍの三次元相対座標系における位置座標を三次元絶対座標系に戻すことで、今回推定時刻ｔでの第二サブ推定位置Ｐｍ２を図４の如く構成する三次元位置座標Ｘｍ２，Ｙｍ２，Ｚｍ２を、取得する。このとき、今回推定時刻ｔでの姿勢角のうち図４のヨー角θｍ２も、マップマッチングにより推定される。

【0047】

こうした、今回推定時刻ｔにおける第二サブ推定位置Ｐｍ２の推定に伴ってマップマッチングブロック１２０は、当該推定に予測される誤差を、図４に示す第二サブ推定誤差ΔＰｍ２として取得する。このとき、今回推定時刻ｔでの第二サブ推定誤差ΔＰｍ２は、推定された三次元位置座標Ｘｍ２，Ｙｍ２，Ｚｍ２の各々に関する、三次元位置誤差ΔＸｍ２，ΔＹｍ２，ΔＺｍ２により構成される。

【0048】

そこで、今回推定時刻ｔでの三次元位置誤差ΔＸｍ２，ΔＹｍ２，ΔＺｍ２は、三次元相対座標系での点群量依存尤度Ｌｘｓ，Ｌｙｓ，Ｌｚｓ及びノイズ量依存尤度Ｌｎとの中から、それぞれ必要とするパラメータを用いた、数１２，１３，１４により定義される。ここで、数１２，１３，１４の関数ｆｘ，ｆｙ，ｆｚはそれぞれ、対応する尤度Ｌｘｓ，Ｌｙｓ，Ｌｚｓ，Ｌｎが増大するほど、対応する三次元位置誤差ΔＸｍ２，ΔＹｍ２，ΔＺｍ２を減少させる関数である。

【数12】

【数13】

【数14】

【0049】

さらにマップマッチングブロック１２０は、今回推定時刻ｔでのヨー角θｍ２の推定に予測される誤差Δθｍ２を、取得する。このときヨー角誤差Δθｍ２は、点群量依存尤度Ｌｙｓとノイズ量依存尤度Ｌｎとを用いた、数１５により定義される。ここで数１５の関数ｆθは、尤度Ｌｙｓ，Ｌｎが増大するほど、ヨー角誤差Δθｍ２を減少させる関数である。

【数15】

【0050】

このような数１２，１３，１４，１５において点群量依存尤度Ｌｘｓ，Ｌｙｓ，Ｌｚｓには、図６にドットハンチングを付して示すように走査点群Ｃｓとマッチングしたマップ点群Ｃｍを含む全ボクセルＶｍに亘って、当該マッチングのマップ点群Ｃｍに関する三次元相対座標系での各方向におけるバラツキ量の逆数が積算（総和）されることで、入力される。一方で数１２，１３，１４，１５においてノイズ量依存尤度Ｌｎには、図７にドットハンチングを付して示すようにマップ点群Ｃｍとはアンマッチングな走査点群Ｃｓを含む全ボクセルＶｍに関して、総数の逆数が入力される。

【0051】

以上よりマップマッチングブロック１２０は、第二サブ推定位置Ｐｍ２を与える三次元位置座標Ｘｍ２，Ｙｍ２，Ｚｍ２を、ヨー角θｍ２と共に、今回推定時刻ｔに関して推定する。それに伴ってマップマッチングブロック１２０は、第二サブ推定誤差ΔＰｍ２を与える三次元位置誤差ΔＸｍ２，ΔＹｍ２，ΔＺｍ２を、ヨー角誤差Δθｍ２と共に、今回推定時刻ｔに関して推定する。そこでマップマッチングブロック１２０は、これらの推定結果を、今回推定時刻ｔと関連付けてメモリ１０に記憶する。

【0052】

図４に示すフュージョンブロック１３０は、マーカ観測データＤｏと、外界データＤｅである三次元点群画像Ｄｅｓとのうち、今回推定時刻ｔでのいずれか一方での取得タイミングに応じたフュージョン方式を採用することで、自己位置Ｐｆを推定する。そこで、マーカ観測データＤｏの取得タイミングとなる今回推定時刻ｔにはフュージョンブロック１３０は、メイン推定位置Ｐｄと第一サブ推定位置Ｐｍ１とを、カルマンフィルタに通したフィルタリングによってフュージョンする。これにより、メイン推定位置Ｐｄと第一サブ推定位置Ｐｍ１とが各々の推定誤差ΔＰｄ，ΔＰｍ１同士の比率に基づき重み付けされてフュージョンされた自己位置Ｐｆを含むように、位置推定データＤｆが生成される。

【0053】

このとき、自己位置Ｐｆの三次元位置座標Ｘｆ，Ｙｆ，Ｚｆはそれぞれ、メイン推定位置Ｐｄの三次元位置座標Ｘｄ，Ｙｄ，Ｚｄと、第一サブ推定位置Ｐｍ１の三次元位置座標Ｘｍ１，Ｙｍ１，Ｚｍ１とを用いた、数１６，１７，１８により定義される。ここで数１６，１７，１８のカルマンゲインＫｘ，Ｋｙ，Ｋｚはそれぞれ、メイン推定位置Ｐｄの三次元位置誤差ΔＸｄ，ΔＹｄ，ΔＺｄと、第一サブ推定位置Ｐｍ１の三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１との、対応する同士での比率に基づき可変設定される。特に数１６，１７，１８では、三次元位置誤差ΔＸｄ，ΔＹｄ，ΔＺｄの三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１に対する相対比率が小さくなるほど、カルマンゲインＫｘ，Ｋｙ，Ｋｚが増大するように設定される。

【数16】

【数17】

【数18】

【0054】

こうした、マーカ観測データＤｏの取得タイミングとなる今回推定時刻ｔでの自己位置Ｐｆの推定に伴って、その推定誤差ΔＰｆとなる三次元位置誤差ΔＸｆ，ΔＹｆ，ΔＺｆも、図４に示すように推定されて位置推定データＤｆに追加される。そこで各三次元位置誤差ΔＸｆ，ΔＹｆ，ΔＺｆは、メイン推定誤差ΔＰｄの三次元位置誤差ΔＸｄ，ΔＹｄ，ΔＺｄと、第一サブ推定誤差ΔＰｍ１の三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１とを用いた、数１９，２０，２１により定義される。ここで数１９，２０，２１のカルマンゲインＫｘ，Ｋｙ，Ｋｚはそれぞれ、数１６，１７，１８と共通となる。

【数19】

【数20】

【数21】

【0055】

さらに、マーカ観測データＤｏの取得タイミングとなる今回推定時刻ｔには、自律走行装置４の姿勢角のうち図４に示すヨー角θｆ及びその誤差Δθｆも、自己位置Ｐｆに準ずるフュージョンによって推定される。このときヨー角θｆは、ヨー角θｄ，θｍ１を用いた、数２２により定義される。それと共にヨー角誤差Δθｆは、ヨー角誤差Δθｄ，Δθｍ１を用いた、数２３により定義される。ここで、数２２，２３に共通となるカルマンゲインＫθは、ヨー角誤差Δθｄ，Δθｍ１同士での比率に基づき可変設定される。特に数２２，２３では、ヨー角誤差Δθｄのヨー角誤差Δθｍ１に対する相対比率が小さくなるほど、カルマンゲインＫθが増大するように設定される。

【数22】

【数23】

【0056】

一方、三次元点群画像Ｄｅｓの取得タイミングとなる今回推定時刻ｔにはフュージョンブロック１３０は、メイン推定位置Ｐｄと第二サブ推定位置Ｐｍ２とを、カルマンフィルタに通したフィルタリングによってフュージョンする。これにより、メイン推定位置Ｐｄと第二サブ推定位置Ｐｍ２とが各々の推定誤差ΔＰｄ，ΔＰｍ２同士の比率に基づき重み付けされてフュージョンされた自己位置Ｐｆを含むように、位置推定データＤｆが生成される。

【0057】

このとき、自己位置Ｐｆの三次元位置座標Ｘｆ，Ｙｆ，Ｚｆはそれぞれ、メイン推定位置Ｐｄの三次元位置座標Ｘｄ，Ｙｄ，Ｚｄと、第二サブ推定位置Ｐｍ２の三次元位置座標Ｘｍ２，Ｙｍ２，Ｚｍ２とを用いた、数２４，２５，２６により定義される。ここで数２４，２５，２６のカルマンゲインｋｘ，ｋｙ，ｋｚはそれぞれ、メイン推定位置Ｐｄの三次元位置誤差ΔＸｄ，ΔＹｄ，ΔＺｄと、第二サブ推定位置Ｐｍ２の三次元位置誤差ΔＸｍ２，ΔＹｍ２，ΔＺｍ２との、対応する同士での比率に基づき可変設定される。特に数２４，２５，２６では、三次元位置誤差ΔＸｄ，ΔＹｄ，ΔＺｄの三次元位置誤差ΔＸｍ２，ΔＹｍ２，ΔＺｍ２に対する相対比率が小さくなるほど、カルマンゲインｋｘ，ｋｙ，ｋｚが増大するように設定される。

【数24】

【数25】

【数26】

【0058】

こうした、三次元点群画像Ｄｅｓの取得タイミングとなる今回推定時刻ｔでの自己位置Ｐｆの推定に伴って、その推定誤差ΔＰｆとなる三次元位置誤差ΔＸｆ，ΔＹｆ，ΔＺｆも、図４に示すように推定されて位置推定データＤｆに追加される。そこで各三次元位置誤差ΔＸｆ，ΔＹｆ，ΔＺｆは、メイン推定誤差ΔＰｄの三次元位置誤差ΔＸｄ，ΔＹｄ，ΔＺｄと、第二サブ推定誤差ΔＰｍ２の三次元位置誤差ΔＸｍ２，ΔＹｍ２，ΔＺｍ２とを用いた、数２７，２８，２９により定義される。ここで数２７，２８，２９のカルマンゲインｋｘ，ｋｙ，ｋｚはそれぞれ、数２４，２５，２６と共通となる。

【数27】

【数28】

【数29】

【0059】

さらに、三次元点群画像Ｄｅｓの取得タイミングとなる今回推定時刻ｔにも、自律走行装置４の姿勢角のうち図４に示すヨー角θｆ及びその誤差Δθｆが、自己位置Ｐｆに準ずるフュージョンによって推定される。このときヨー角θｆは、ヨー角θｄ，θｍ２を用いた、数３０により定義される。それと共にヨー角誤差Δθｆは、ヨー角誤差Δθｄ，Δθｍ２を用いた、数３１により定義される。ここで数３０，３１に共通となるカルマンゲインｋθは、ヨー角誤差Δθｄ，Δθｍ２同士での比率に基づき可変設定される。特に数３０，３１では、ヨー角誤差Δθｄのヨー角誤差Δθｍ２に対する相対比率が小さくなるほど、カルマンゲインｋθが増大するように設定される。

【数30】

【数31】

【0060】

以上いずれの取得タイミングにあっても、今回推定時刻ｔでの自己位置Ｐｆをフュージョンによって推定した位置推定データＤｆは、図４の如くフュージョンブロック１３０から出力される。これにより、自己位置Ｐｆとして出力された三次元位置座標Ｘｆ，Ｙｆ，Ｚｆは、今回推定時刻ｔと関連付けてメモリ１０に記憶された後、デッドレコニングブロック１００による次回処理時刻でのデッドレコニング処理にフィードバックされる。また、自己位置Ｐｆとして出力された三次元位置座標Ｘｆ，Ｙｆ，Ｚｆは、例えば自律走行装置４の高度運転支援又は自動運転制御等に活用されてもよい。

【0061】

ここまで説明したブロック１００，１１０，１２０，１３０の共同により、自律走行装置４の自己位置Ｐｆを推定する方法として特に、同位置Ｐｆを推定した位置推定データＤｆを生成する推定データ生成方法の、図８に従うフロー（以下、推定フローという）を、以下に説明する。尚、本推定フローは、自律走行装置４の起動中に、推定間隔毎に繰り返し実行される。また、本推定フローにおいて「Ｓ」とは、推定プログラムに含まれた複数命令により実行される複数ステップを、意味する。

【0062】

Ｓ１０においてデッドレコニングブロック１００は、今回推定時刻ｔがマーカ観測データＤｏの取得タイミングであるか否かを、判定する。その結果、肯定判定が下された場合には、推定フローがＳ２０へ移行する。Ｓ２０においてデッドレコニングブロック１００は、内界データＤｉとダイナミクスモデルとに基づくデッドレコニングにより、今回推定時刻ｔにおける自律走行装置４のメイン推定位置Ｐｄを取得する。

【0063】

Ｓ２０に続くＳ３０においてマーカマッチングブロック１１０は、マーカ観測データＤｏとマーカ位置データＤｍｐとに基づくマーカマッチングにより、今回推定時刻ｔにおける自律走行装置４の第一サブ推定位置Ｐｍ１を取得する。このとき、マーカ観測データＤｏからメイン推定位置Ｐｄを起点に予測されるマーカ予測位置Ｐｐと、マーカ位置データＤｍｐにて当該予測位置Ｐｐにマッチングするマーカ設置位置Ｐｍｐとの、偏差δＰにより補正されたメイン推定位置Ｐｄが第一サブ推定位置Ｐｍ１となる。

【0064】

Ｓ３０に続くＳ４０においてフュージョンブロック１３０は、今回推定時刻ｔでのメイン推定位置Ｐｄ及び第一サブ推定位置Ｐｍ１を各々の推定誤差ΔＰｄ，ΔＰｍ１同士の比率に基づき重み付けしたフュージョンにより、自己位置Ｐｆの位置推定データＤｆを生成する。こうして生成された位置推定データＤｆは、メモリ１０に出力されて記憶されることで、デッドレコニングブロック１００による次回推定時刻でのデッドレコニングへとフィードバックされる。このようなＳ４０の実行完了に応じて、推定フローの今回実行が終了する。

【0065】

一方、Ｓ１０において否定判定が下された場合には、推定フローがＳ５０へ移行する。Ｓ５０においてマップマッチングブロック１２０は、今回推定時刻ｔが外界データＤｅのうち三次元点群画像Ｄｅｓの取得タイミングであるか否かを、判定する。その結果、否定判定が下された場合には、推定フローの今回実行が終了する。それに対して肯定判定が下された場合には、推定フローがＳ６０へ移行する。Ｓ６０においてマップマッチングブロック１２０は、外界データＤｅとマップデータＤｍとに基づくマップマッチングにより、今回推定時刻ｔにおける自律走行装置４の第二サブ推定位置Ｐｍ２を取得する。

【0066】

Ｓ６０に続くＳ７０においてフュージョンブロック１３０は、今回推定時刻ｔでのメイン推定位置Ｐｄ及び第二サブ推定位置Ｐｍ２を各々の推定誤差ΔＰｄ，ΔＰｍ２同士の比率に基づき重み付けしたフュージョンにより、自己位置Ｐｆの位置推定データＤｆを生成する。こうして生成された位置推定データＤｆも、メモリ１０に出力されて記憶されることで、デッドレコニングブロック１００による次回推定時刻でのデッドレコニングへフィードバックされる。このようなＳ７０の実行完了に応じて、推定フローの今回実行が終了する。

【0067】

（作用効果）
以上説明した第一実施形態の作用効果を、以下に説明する。

【0068】

第一実施形態によると、内界データＤｉに基づくデッドレコニングによるメイン推定位置Ｐｄと、マーカ観測データＤｏ及びマーカ位置データＤｍｐに基づくマーカマッチングによる第一サブ推定位置Ｐｍ１とが、取得される。ここで第一サブ推定位置Ｐｍ１の取得には、マーカ観測データＤｏからメイン推定位置Ｐｄを起点に予測されるマーカ予測位置Ｐｐと、マーカ位置データＤｍｐにおいて当該マーカ予測位置Ｐｐにマッチングするマーカ設置位置Ｐｍｐとの、偏差δＰが着目される。これによれば、マーカ予測位置Ｐｐとマーカ設置位置Ｐｍｐとの偏差δＰによってメイン推定位置Ｐｄが補正されることで取得される、第一サブ推定位置Ｐｍ１の推定誤差ΔＰｍ１からは、メイン推定位置Ｐｄの推定誤差ΔＰｄによる影響分を相殺することができる。故に、メイン推定位置Ｐｄと第一サブ推定位置Ｐｍ１とが、外界状況変化を反映し得る各々の推定誤差ΔＰｄ，ΔＰｍ１同士での比率に基づき重み付けされてフュージョンされることで、自己位置Ｐｆの推定精度に対してロバスト性を確保した位置推定データＤｆの生成が可能となる。

【0069】

第一実施形態によると、マーカ観測データＤｏの取得タイミングには、デッドレコニングによるメイン推定位置Ｐｄと、マーカ観測データＤｏに基づくマーカマッチングによる第一サブ推定位置Ｐｍ１とを、各々の推定誤差同士の比率に基づき重み付けしたフュージョンにより、位置推定データＤｆが生成される。一方で外界データＤｅとしての三次元点群画像Ｄｅｓの取得タイミングには、デッドレコニングによるメイン推定位置Ｐｄと、当該取得画像Ｄｅｓに基づくマップマッチングによる第二サブ推定位置Ｐｍ２とを、各々の推定誤差同士の比率に基づき重み付けしたフュージョンにより、位置推定データＤｆが生成される。これらによれば、三次元点群画像Ｄｅｓの取得タイミングには当該取得画像Ｄｅｓを用いて第二サブ推定位置Ｐｍ２から推定された過去の自己位置Ｐｆに基づくデッドレコニングにより、マーカ観測データＤｏの取得タイミングには推定誤差補正のされた第一サブ推定位置Ｐｍ１が取得されて、高精度に最新の自己位置Ｐｆを推定することができる。故に、自己位置Ｐｆの推定精度に対してロバスト性を確保した位置推定データＤｆの生成における信頼性を、高めることが可能となる。

【0070】

（第二実施形態）
第二実施形態は、第一実施形態の変形例である。

【0071】

図９に示すように第二実施形態では、自律走行装置４の走行ルートに想定される走行軌道Ｔｒを三次元相対座標系での横方向に挟んで対称箇所に配置される各ルートマーカ６のペアが、当該軌道Ｔｒに沿って間隔を空けて設置されている。そこで第二実施形態の自律走行装置４は、図９の如くペアをなすルートマーカ６（以下、第二実施形態の説明ではペアマーカ６という）間を結ぶ仮想直線Ｌに対しての直交方位に、軌道Ｔｒ上での走行方向Ｔｄを制御される。このとき、推定装置１により推定されるヨー角θｆ（第一実施形態を参照）に基づき自律走行装置４の実ヨー角がフィードバック制御されることで、仮想直線Ｌに対する直交方位に合わせた走行方向Ｔｄが実現可能となる。

【0072】

そこで第二実施形態のマーカマッチングブロック１１０では、図１０に示す今回推定時刻ｔでの第一サブ推定位置Ｐｍ１の三次元位置座標Ｘｍ１，Ｙｍ１，Ｚｍ１がそれぞれ、ペアマーカ６の各サフィックスをｉ＝１，２とした、数３２，３３，３４により定義される。即ち各三次元位置座標Ｘｍ１，Ｙｍ１，Ｚｍ１は、各ペアマーカ６のマーカ予測位置Ｐｐ＿ｉである三次元位置座標Ｘｐ＿ｉ，Ｙｐ＿ｉ，Ｚｐ＿ｉと、各ペアマーカ６のマーカ設置位置Ｐｍｐ＿ｉである三次元位置座標Ｘｍｐ＿ｉ，Ｙｍｐ＿ｉ，Ｚｍｐ＿ｉとの、偏差平均を偏差δＰとして、図１０の如くメイン推定位置Ｐｄの三次元位置座標Ｘｄ，Ｙｄ，Ｚｄからそれぞれ差し引くことで、取得される。それと共に今回推定時刻ｔでのヨー角θｍ１は、軌道Ｔｒ上において自律走行装置４の走行方向Ｔｄ、即ち仮想直線Ｌに対しての直交方位を表した、姿勢角として取得される。

【数32】

【数33】

【数34】

【0073】

このような第二実施形態でも、今回推定時刻ｔでの第一サブ推定誤差ΔＰｍ１となる三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１（第一実施形態を参照）については、それぞれ数３２，３３，３４の関係から、三次元位置座標Ｘｄ，Ｙｄ，Ｚｄに関するメイン推定誤差ΔＰｄ分の影響が実質的に相殺された状態となる。故に各三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１にはそれぞれ、マーカ予測位置Ｐｐの三次元相対座標ｘｏ，ｙｏ，ｚｏに関する観測誤差と、マーカ設置位置Ｐｍｐの三次元位置座標Ｘｍｐ，Ｙｍｐ，Ｚｍｐに関するマップ誤差とが、第一実施形態と同様に残る。そこで各三次元位置誤差ΔＸｍ１，ΔＹｍ１，ΔＺｍ１は、ペアマーカ６に対するマーカセンサ２０の誤差特性として予測される観測誤差と、ペアマーカ６に対するマーカ位置データＤｍｐの測量誤差とに基づき、それぞれ相異又は同一の固定値に予め設定される。このとき、今回推定時刻ｔでのヨー角θｍ１の推定に予測される誤差Δθｍ１（第一実施形態を参照）も、同様な固定値に予め設定される。

【0074】

以上説明した第二実施形態によっても、第一実施形態と同様な作用効果の発揮が可能となる。しかも第二実施形態によれば、ペアマーカ６間を結ぶ仮想直線Ｌに対しての直交方位に、自律走行装置４の走行方向Ｔｄが制御されることで、第一サブ推定位置Ｐｍ１の推定誤差ΔＰｍ１に自律走行装置４のヨー角推定精度が影響するのを抑止することができる。故に、自己位置Ｐｆの推定精度に対してロバスト性を確保した位置推定データＤｆの生成における信頼性を、高めることが可能となる。

【0075】

（他の実施形態）
以上、複数の実施形態について説明したが、本開示は、それらの実施形態に限定して解釈されるものではなく、本開示の要旨を逸脱しない範囲内において種々の実施形態及び組み合わせに適用することができる。

【0076】

変形例において推定装置１を構成する専用コンピュータは、デジタル回路及びアナログ回路のうち、少なくとも一方をプロセッサとして有していてもよい。ここでデジタル回路とは、例えばＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field Programmable Gate Array）、ＳＯＣ（System on a Chip）、ＰＧＡ（Programmable Gate Array）、及びＣＰＬＤ（Complex Programmable Logic Device）等のうち、少なくとも一種類である。またこうしたデジタル回路は、プログラムを記憶したメモリを、有していてもよい。

【0077】

変形例では、マップマッチングブロック１２０が省略されることで、外界データＤｅのうち三次元点群画像Ｄｅｓの取得タイミングにおけるフュージョン処理を非実施としたフュージョンブロック１３０が、設けられてもよい。この場合には、推定フローにおいてＳ５０～Ｓ７０が省略されることで、Ｓ１０での否定判定に応じて推定フローの今回実行が終了するとよい。

【0078】

変形例のマーカマッチングブロック１１０では、メイン推定位置Ｐｄに代えて第二サブ推定位置Ｐｍ２が、外界データＤｅとしての三次元点群画像Ｄｅｓの取得タイミングに入力されて、マーカ予測位置Ｐｐの予測に用いられてもよい。変形例では、三次元絶対座標系及び三次元相対座標系に代えて、それぞれ鉛直方向（Ｚ方向）及び高さ方向（ｚ方向）は想定されない、二次元絶対座標系及び二次元相対座標系が定義されてもよく、この場合に「三次元」は「二次元」に読み替えて、各実施形態が実施されるとよい。

【0079】

ここまでの説明形態の他に上述の実施形態及び変形例は、自律走行車両４に搭載可能に構成されてプロセッサ１２及びメモリ１０を少なくとも一つずつ有する制御装置として、処理回路（例えば処理ＥＣＵ等）又は半導体装置（例えば半導体チップ等）の形態で実施されてもよい。

【符号の説明】

【0080】

１：推定装置、４：自律走行装置、６：ルートマーカ、１０：メモリ、１２：プロセッサ、Ｃｍ：マップ点群、Ｃｓ：走査点群、Ｄｅ：外界データ、Ｄｆ：位置推定データ、Ｄｉ：内界データ、Ｄｍ：マップデータ、Ｄｍｐ：マーカ位置データ、Ｄｏ：マーカ観測データ、Ｌ：仮想直線、Ｐｄ：メイン推定位置、Ｐｍ１：第一サブ推定位置、Ｐｍ２：第二サブ推定位置、Ｐｍｐ：マーカ設置位置、Ｐｐ：マーカ予測位置、Ｔｄ：走行方向、Ｖｍ：ボクセル、δＰ：偏差

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】