特許 7409037 推定装置、推定方法、推定プログラム

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

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

(11)【特許番号】

(24)【登録日】2023-12-25

(45)【発行日】2024-01-09

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

(51)【国際特許分類】

   G01C 21/30 20060101AFI20231226BHJP

   G08G 1/0969 20060101ALI20231226BHJP

   G09B 29/10 20060101ALI20231226BHJP

   G09B 29/00 20060101ALN20231226BHJP

【ＦＩ】

G01C21/30

G08G1/0969

G09B29/10 A

G09B29/00 A

【請求項の数】 15

(21)【出願番号】P 2019210764

(22)【出願日】2019-11-21

(65)【公開番号】P2021081370

(43)【公開日】2021-05-27

【審査請求日】2021-12-08

(73)【特許権者】

【識別番号】000004260

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

(74)【代理人】

【識別番号】100106149

【弁理士】

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

(74)【代理人】

【識別番号】100121991

【弁理士】

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

(74)【代理人】

【識別番号】100145595

【弁理士】

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

(72)【発明者】

【氏名】南口 雄一

【審査官】藤村 泰智

(56)【参考文献】

【文献】特開２００６－１３８８３４（ＪＰ，Ａ）

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

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

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

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

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

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

【文献】米国特許出願公開第２００５／００２１２２９（ＵＳ，Ａ１）

【文献】特表２０１７－５０８９６６（ＪＰ，Ａ）

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

Ｇ０１Ｃ ２１／００ ～ ２１／３６

Ｇ０８Ｇ １／００ ～ １／１６

Ｇ０９Ｂ ２９／１０

Ｇ０９Ｂ ２９／００

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

【請求項1】

車両（４）の位置を含む状態量を推定する推定装置（１）であって、
前記車両の挙動を変化させる動的パラメータ（ｐｔ）を含んだダイナミクスモデル（ＤＭ）と、前記車両の内界から取得される内界情報（Ｉｉ）とに基づいたデッドレコニングにより、前記状態量を推定するデッドレコニング部（１００）と、
前記車両の走行環境を表す地図情報（Ｉｍ）と、前記車両の外界から取得される外界情報（Ｉｏ）とに基づいたマップマッチングにより、前記状態量を観測するマップマッチング部（１２０）と、
前記マップマッチングにより観測される前記状態量に対して、前記デッドレコニングにより推定される前記状態量がオフセットしたオフセット量（δｚ）に基づき、前記デッドレコニングへフィードバックする前記動的パラメータを、補正するパラメータ補正部（１４０）とを、備え、
前記パラメータ補正部は、前記オフセット量のうち、前記デッドレコニングにより推定される前記状態量が前記動的パラメータの変動量（δｐ）に応じてずれたずれ分（δｚｐ）を、予測し、当該予測のずれ分に対応した前記変動量により、前記動的パラメータを補正する推定装置。

【請求項2】

前記パラメータ補正部は、前記オフセット量との差を最小にする前記ずれ分を、予測する請求項１に記載の推定装置。

【請求項3】

前記パラメータ補正部は、前記挙動以外の要因に応じて変動する前記動的パラメータを、補正する請求項１又は２に記載の推定装置。

【請求項4】

前記パラメータ補正部は、前記車両が走行する路面の動摩擦係数を含んだ前記動的パラメータを、補正する請求項３に記載の推定装置。

【請求項5】

前記マップマッチングにより観測される前記状態量に基づいたフィルタリングにより、前記デッドレコニングにより推定される前記状態量の誤差を補償する誤差補償部（１６０）を、さらに備える請求項１～４のいずれか一項に記載の推定装置。

【請求項6】

プロセッサ（１２）により実行され、車両（４）の位置を含む状態量を推定する推定方法であって、
前記車両の挙動を変化させる動的パラメータ（ｐｔ）を含んだダイナミクスモデル（ＤＭ）と、前記車両の内界から取得される内界情報（Ｉｉ）とに基づいたデッドレコニングにより、前記状態量を推定するデッドレコニングプロセス（Ｓ１０１）と、
前記車両の走行環境を表す地図情報（Ｉｍ）と、前記車両の外界から取得される外界情報（Ｉｏ）とに基づいたマップマッチングにより、前記状態量を観測するマップマッチングプロセス（Ｓ１０２）と、
前記マップマッチングにより観測される前記状態量に対して、前記デッドレコニングにより推定される前記状態量がオフセットしたオフセット量（δｚ）に基づき、前記デッドレコニングへフィードバックする前記動的パラメータを、補正するパラメータ補正プロセス（Ｓ１０３）とを、含み、
前記パラメータ補正プロセスは、前記オフセット量のうち、前記デッドレコニングにより推定される前記状態量が前記動的パラメータの変動量（δｐ）に応じてずれたずれ分（δｚｐ）を、予測し、当該予測のずれ分に対応した前記変動量により、前記動的パラメータを補正する推定方法。

【請求項7】

前記パラメータ補正プロセスは、前記オフセット量との差を最小にする前記ずれ分を、予測する請求項６に記載の推定方法。

【請求項8】

前記パラメータ補正プロセスは、前記挙動以外の要因に応じて変動する前記動的パラメータを、補正する請求項６又は７に記載の推定方法。

【請求項9】

前記パラメータ補正プロセスは、前記車両が走行する路面の動摩擦係数を含んだ前記動的パラメータを、補正する請求項８に記載の推定方法。

【請求項10】

前記マップマッチングにより観測される前記状態量に基づいたフィルタリングにより、前記デッドレコニングにより推定される前記状態量の誤差を補償する誤差補償プロセス（Ｓ１０４）を、さらに含む請求項６～９のいずれか一項に記載の推定方法。

【請求項11】

車両（４）の位置を含む状態量を推定するために記憶媒体（１０）に格納され、プロセッサ（１２）に実行させる命令を含む推定プログラムであって、
前記命令は、
前記車両の挙動を変化させる動的パラメータ（ｐｔ）を含んだダイナミクスモデル（ＤＭ）と、前記車両の内界から取得される内界情報（Ｉｉ）とに基づいたデッドレコニングにより、前記状態量を推定させるデッドレコニングプロセス（Ｓ１０１）と、
前記車両の走行環境を表す地図情報（Ｉｍ）と、前記車両の外界から取得される外界情報（Ｉｏ）とに基づいたマップマッチングにより、前記状態量を観測させるマップマッチングプロセス（Ｓ１０２）と、
前記マップマッチングにより観測される前記状態量に対して、前記デッドレコニングにより推定される前記状態量がオフセットしたオフセット量（δｚ）に基づき、前記デッドレコニングへフィードバックさせる前記動的パラメータを、補正させるパラメータ補正プロセス（Ｓ１０３）とを、含み、
前記パラメータ補正プロセスは、前記オフセット量のうち、前記デッドレコニングにより推定される前記状態量が前記動的パラメータの変動量（δｐ）に応じてずれたずれ分（δｚｐ）を、予測させ、当該予測のずれ分に対応した前記変動量により、前記動的パラメータを補正させる推定プログラム。

【請求項12】

前記パラメータ補正プロセスは、前記オフセット量との差を最小にする前記ずれ分を、予測させる請求項１１に記載の推定プログラム。

【請求項13】

前記パラメータ補正プロセスは、前記挙動以外の要因に応じて変動する前記動的パラメータを、補正させる請求項１１又は１２に記載の推定プログラム。

【請求項14】

前記パラメータ補正プロセスは、前記車両が走行する路面の動摩擦係数を含んだ前記動的パラメータを、補正させる請求項１３に記載の推定プログラム。

【請求項15】

前記命令は、
前記マップマッチングにより観測される前記状態量に基づいたフィルタリングにより、前記デッドレコニングにより推定される前記状態量の誤差を補償させる誤差補償プロセス（Ｓ１０４）を、さらに含む請求項１１～１４のいずれか一項に記載の推定プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、車両の状態量を推定する推定技術に、関する。

【背景技術】

【0002】

従来、車両の内界から取得される内界情報に基づいたデッドレコニングにより、車両の位置を含む状態量が推定されている。このような推定技術の一種として特許文献１の開示技術は、デッドレコニングでの推定結果に含まれる航法誤差を補正するために、ダイナミクスをモデル化した状態方程式に基づくカルマンフィルタを、用いている。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２００８－２４９６３９号公報

【発明の概要】

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

【0004】

しかし、特許文献１の開示技術では、車両挙動を変化させる動的パラメータとしてダイナミクスモデルに含まれるパラメータ自体に誤差が生じると、デッドレコニングの精度が低下する。この動的パラメータでは、例えば路面状況等、車両挙動以外の要因に応じた変動分が誤差となる。しかし、こうした動的パラメータの誤差は、カルマンフィルタによっては正確には補正又は補償され得ず、状態量の推定精度を低下させてしまう。

【0005】

本開示の課題は、状態量の推定精度を高める推定装置を、提供することにある。本開示の別の課題は、状態量の推定精度を高める推定方法を、提供することにある。本開示のさらに別の課題は、状態量の推定精度を高める推定プログラムを、提供することにある。

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

【0006】

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

【0007】

本開示の第一態様は、
車両（４）の位置を含む状態量を推定する推定装置（１）であって、
車両の挙動を変化させる動的パラメータ（ｐｔ）を含んだダイナミクスモデル（ＤＭ）と、車両の内界から取得される内界情報（Ｉｉ）とに基づいたデッドレコニングにより、状態量を推定するデッドレコニング部（１００）と、
車両の走行環境を表す地図情報（Ｉｍ）と、車両の外界から取得される外界情報（Ｉｏ）とに基づいたマップマッチングにより、状態量を観測するマップマッチング部（１２０）と、
マップマッチングにより観測される状態量に対して、デッドレコニングにより推定される状態量がオフセットしたオフセット量（δｚ）に基づき、デッドレコニングへフィードバックする動的パラメータを、補正するパラメータ補正部（１４０）とを、備え、
パラメータ補正部は、オフセット量のうち、デッドレコニングにより推定される状態量が動的パラメータの変動量（δｐ）に応じてずれたずれ分（δｚｐ）を、予測し、当該予測のずれ分に対応した変動量により、動的パラメータを補正する。

【0008】

本開示の第二態様は、
プロセッサ（１２）により実行され、車両（４）の位置を含む状態量を推定する推定方法であって、
車両の挙動を変化させる動的パラメータ（ｐｔ）を含んだダイナミクスモデル（ＤＭ）と、車両の内界から取得される内界情報（Ｉｉ）とに基づいたデッドレコニングにより、状態量を推定するデッドレコニングプロセス（Ｓ１０１）と、
車両の走行環境を表す地図情報（Ｉｍ）と、車両の外界から取得される外界情報（Ｉｏ）とに基づいたマップマッチングにより、状態量を観測するマップマッチングプロセス（Ｓ１０２）と、
マップマッチングにより観測される状態量に対して、デッドレコニングにより推定される状態量がオフセットしたオフセット量（δｚ）に基づき、デッドレコニングへフィードバックする動的パラメータを、補正するパラメータ補正プロセス（Ｓ１０３）とを、含み、
パラメータ補正プロセスは、オフセット量のうち、デッドレコニングにより推定される状態量が動的パラメータの変動量（δｐ）に応じてずれたずれ分（δｚｐ）を、予測し、当該予測のずれ分に対応した変動量により、動的パラメータを補正する。

【0009】

本開示の第三態様は、
車両（４）の位置を含む状態量を推定するために記憶媒体（１０）に格納され、プロセッサ（１２）に実行させる命令を含む推定プログラムであって、
命令は、
車両の挙動を変化させる動的パラメータ（ｐｔ）を含んだダイナミクスモデル（ＤＭ）と、車両の内界から取得される内界情報（Ｉｉ）とに基づいたデッドレコニングにより、状態量を推定させるデッドレコニングプロセス（Ｓ１０１）と、
車両の走行環境を表す地図情報（Ｉｍ）と、車両の外界から取得される外界情報（Ｉｏ）とに基づいたマップマッチングにより、状態量を観測させるマップマッチングプロセス（Ｓ１０２）と、
マップマッチングにより観測される状態量に対して、デッドレコニングにより推定される状態量がオフセットしたオフセット量（δｚ）に基づき、デッドレコニングへフィードバックさせる動的パラメータを、補正させるパラメータ補正プロセス（Ｓ１０３）とを、含み、
パラメータ補正プロセスは、オフセット量のうち、デッドレコニングにより推定される状態量が動的パラメータの変動量（δｐ）に応じてずれたずれ分（δｚｐ）を、予測させ、当該予測のずれ分に対応した変動量により、動的パラメータを補正させる。

【0010】

これら第一～第三態様によると、車両挙動を変化させる動的パラメータの補正は、マップマッチングによる観測状態量に対してデッドレコニングによる推定状態量がオフセットしたオフセット量に、基づくこととなる。この補正によれば、動的パラメータを正確に補正してデッドレコニングへとフィードバックすることができる。故に、フィードバックされた動的パラメータを含むダイナミクスモデルに基づくことで、デッドレコニングによる状態量の推定精度を高めることが可能となる。

【図面の簡単な説明】

【0011】

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

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

【図3】一実施形態によるデッドレコニングブロックについて説明するための模式図である。

【図4】一実施形態によるデッドレコニングブロックについて説明するための模式図である。

【図5】一実施形態の変形例による推定装置の詳細構成を示すブロック図である。

【図6】一実施形態によるマップマッチングブロックについて説明するための模式図である。

【図7】一実施形態によるパラメータ補正ブロックについて説明するための模式図である。

【図8】一実施形態の変形例による推定装置の詳細構成を示すブロック図である。

【図9】一実施形態の変形例による推定装置の詳細構成を示すブロック図である。

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

【図11】一実施形態の変形例による推定方法を示すフローチャートである。

【図12】一実施形態の変形例による推定方法を示すフローチャートである。

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

【0012】

以下、本開示の一実施形態を図面に基づいて説明する。

【0013】

図１及び図２に示すように一実施形態の推定装置１は、センサユニット２及び地図ユニット３と共に、車両４に搭載される。センサユニット２は、外界センサ２２及び内界センサ２４を含んで構成される。

【0014】

外界センサ２２は、車両４の周辺環境となる外界から、車両４の運動推定に活用可能な情報を、外界情報Ｉ_ｏとして取得する。外界センサ２２は、車両４の外界に存在する物体を検知することで、外界情報Ｉ_ｏを取得してもよい。この検知タイプの外界センサ２２は、例えばＬＩＤＡＲ（Light Detection and Ranging / Laser Imaging Detection and Ranging）、カメラ、レーダ及びソナー等のうち、一種類又は複数種類である。外界センサ２２は、車両４の外界に存在する無線通信システムから信号を受信することで、外界情報Ｉ_ｏを取得してもよい。この受信タイプの外界センサ２２は、例えばＧＮＳＳ（Global Navigation Satellite System）の受信機及びＩＴＳ（Intelligent Transport Systems）の受信機等のうち、一種類又は複数種類である。以下、外界への光照射により反射点からの反射光を検知して点群画像を生成するＬＩＤＡＲが、外界センサ２２として車両４に搭載される場合を例にとって、本実施形態を説明する。

【0015】

内界センサ２４は、車両４の内部環境となる内界から、車両４の運動推定に活用可能な情報を、内界情報Ｉ_ｉとして取得する。内界センサ２４は、車両４の内界において特定の運動物理量を検知することで、内界情報Ｉ_ｉを取得してもよい。この検知タイプの内界センサ２４は、例えば慣性センサ、車速センサ及び舵角センサ等のうち、一種類又は複数種類である。以下、車体の角速度を検知する慣性センサとしてのジャイロセンサと、車体の速度を検知する車速センサと、車体に対する車輪の舵角を検出する舵角センサとが、内界センサ２４として車両４に搭載される場合を例にとって、本実施形態を説明する。

【0016】

地図ユニット３は、地図情報Ｉ_ｍを非一時的に格納又は記憶する、例えば半導体メモリ、磁気媒体及び光学媒体等のうち、一種類又は複数種類の非遷移的実体的記憶媒体（non-transitory tangible storage medium）を含んで構成される。地図ユニット３は、車両４の高度運転支援又は自動運転制御に利用される、ロケータのデータベースであってもよい。地図ユニット３は、車両４の運転をナビゲートする、ナビゲーション装置のデータベースであってもよい。

【0017】

地図情報Ｉ_ｍは、車両４の走行環境を表す情報として、二次元又は三次元にデータ化されている。地図情報Ｉ_ｍは、例えば道路自体の位置、形状及び路面状態等のうち、一種類又は複数種類を表した道路情報を、少なくとも含んでいる。地図情報Ｉ_ｍは、例えば道路に付属する標識及び区画線の位置並びに形状等のうち、一種類又は複数種類を表した標示情報を、含んでいてもよい。地図情報Ｉ_ｍは、例えば道路に面する建造物及び信号機の位置並びに形状等のうち、一種類又は複数種類を表した構造物情報を、含んでいてもよい。

【0018】

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

【0019】

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

【0020】

推定装置１により構築される複数の機能ブロックには、デッドレコニングブロック１００、マップマッチングブロック１２０、パラメータ補正ブロック１４０及び誤差補償ブロック１６０が、含まれる。デッドレコニングブロック１００は、ダイナミクスモデルＤＭ（Dynamics Model）と内界情報Ｉ_ｉとに基づいたデッドレコニングにより、車両４の状態量を推定する。そのためにデッドレコニングブロック１００は、挙動推定サブブロック１０２及び状態量推定サブブロック１０４を有している。以下の説明においてデッドレコニングは、ＤＲ（Dead Reckoning）と表記されるものとする。

【0021】

挙動推定サブブロック１０２は、最新時刻ｔでの車両４の運転状態となる挙動（以下、単に車両挙動と表記される）を、図３に示すダイナミクスモデルＤＭによって推定する。具体的にダイナミクスモデルＤＭは、車両挙動を動力学に基づきモデル化した、例えば二輪モデル等である。本実施形態のダイナミクスモデルＤＭは、車両挙動を特定する運動物理量として、滑り角βを推定可能にモデル化されている。図３においてγ及びψは、それぞれ車両４の角速度及び舵角である。図３においてＦ、Ｇ及びＬは、それぞれ車両４における横力、横加速度及びホイールベースである。図３においてβ、Ｆ及びＬに付されたｒは、車両４のリアモデルの成分を示すサフィックスである。図３においてβ、Ｆ及びＬに付されたｆは、車両４のフロントモデルの成分を示すサフィックスである。

【0022】

図２に示す挙動推定サブブロック１０２には、内界センサ２４により最新時刻ｔに取得された内界情報Ｉ_ｉのうち角速度γ_ｔ及び舵角ψ_ｔが、入力される。挙動推定サブブロック１０２には、後に詳述するパラメータ補正ブロック１４０により補正且つフィードバックされる、最新時刻ｔに関しての動的パラメータｐ_ｔが、入力される。動的パラメータｐ_ｔは、車両挙動以外の要因に応じて変動する物理量のうち、一種類又は複数種類を対象として含んでいる。本実施形態の動的パラメータｐ_ｔは、車両４が走行する路面の動摩擦係数を、少なくとも含んでいる。この動摩擦係数の他に動的パラメータｐ_ｔは、乗員体重を含めた車両４の重量を、含んでいてもよい。

【0023】

角速度γ_ｔ及び舵角ψ_ｔに加えて挙動推定サブブロック１０２へと入力される動的パラメータｐ_ｔは、本実施形態のダイナミクスモデルＤＭにより最新時刻ｔに関して推定される車両挙動としての滑り角β_ｔを、同パラメータｐ_ｔの変動に応じて変化させる。そこで挙動推定サブブロック１０２は、角速度γ_ｔ及び舵角ψ_ｔと共に動的パラメータｐ_ｔを変数とする、ダイナミクスモデルＤＭに基づいたモデル関数Ｍ_ｄとしての下記式１により、最新時刻ｔの滑り角β_ｔを推定演算する。

【数1】

【0024】

図２に示す状態量推定サブブロック１０４は、挙動推定サブブロック１０２のダイナミクスモデルＤＭによる推定結果に応じて、最新時刻ｔにおける車両４の状態量Ｚ_ｔを推定する。具体的に最新時刻ｔでの状態量Ｚ_ｔは、車両４の二次元座標位置ｘ_ｔ、ｙ_ｔ及びヨー角θ_ｔを用いたベクトル式としての下記式２により、定義される。

【数2】

【0025】

図４に示すように最新時刻ｔでの状態量Ｚ_ｔを構成する二次元座標位置ｘ_ｔ、ｙ_ｔは、それぞれ前回推定時刻ｔ－１での二次元座標位置ｘ_ｔ－１、ｙ_ｔ－１を用いた線形関数としての下記式３及び４により、表される。式３及び式４においてβ_ｔは、挙動推定サブブロック１０２により推定される、最新時刻ｔでの滑り角である。式３及び式４においてＶ_ｔは、車両４の車速である。式３及び式４においてθ_ｔは、最新時刻ｔでの状態量Ｚ_ｔを構成するヨー角である。このヨー角θ_ｔは、前回推定時刻ｔ－１での値θ_ｔ－１を用いた下記式５により、表される。式３、式４及び式５においてΔｔは、最新時刻ｔと前回推定時刻ｔ－１との差分、即ち推定時刻間隔である。式５においてγ_ｔは、最新時刻ｔでの車両４の角速度である。

【数3】

【数4】

【数5】

【0026】

図２に示すように状態量推定サブブロック１０４には、内界センサ２４により最新時刻ｔに取得された内界情報Ｉ_ｉのうち車速Ｖ_ｔ及び角速度γ_ｔが、入力される。状態量推定サブブロック１０４には、挙動推定サブブロック１０２のダイナミクスモデルＤＭにより推定された最新時刻ｔの滑り角β_ｔが、入力される。状態量推定サブブロック１０４にはさらに、前回推定において前回時刻ｔ－１に関連付けて記憶された状態量Ｚ_ｔ－１が、メモリ１０における推定記憶領域１８０からの読み出しにより入力される。これらの入力に基づき状態量推定サブブロック１０４は、式３、式４及び式５を代入した式２の状態量Ｚ_ｔを構成する二次元座標位置ｘ_ｔ、ｙ_ｔ及びヨー角θ_ｔを、最新時刻ｔに関して推定演算する。状態量推定サブブロック１０４は、これら今回推定の結果を、最新時刻ｔと関連付けて推定記憶領域１８０に記憶する。

【0027】

ここで、式１を代入した式３及び式４の各右辺第二項と、式５の右辺第二項とは、推定時刻間隔Δｔでの状態量の推定変位量をベクトル式により表す、変位量関数ΔＺとしての下記式６に集約される。この式６は、滑り角β_ｔを表したダイナミクスモデルＤＭのモデル関数Ｍ_ｄを用いることで、動的パラメータｐ_ｔを変数とした変位量関数ΔＺを、定義しているとも言える。以上より最新時刻ｔでの状態量Ｚ_ｔは、前回推定時刻ｔ－１での状態量Ｚ_ｔ－１及び最新時刻ｔでの変位量関数ΔＺを用いた下記式７により、表される。そこで、図５に変形例を示すようにＤＲブロック１００は、角速度γ_ｔ、舵角ψ_ｔ、車速Ｖ_ｔ、動的パラメータｐ_ｔ、及び前回状態量Ｚ_ｔ－１の入力に基づくことで、式６を代入した式７の状態量Ｚ_ｔを構成する二次元座標位置ｘ_ｔ、ｙ_ｔ及びヨー角θ_ｔを、当該滑り角β_ｔの演算なしに推定してもよい。

【数6】

【数7】

【0028】

図２に戻ってマップマッチングブロック１２０は、地図情報Ｉ_ｍと外界情報Ｉ_ｏとに基づいたマップマッチングにより、車両４の状態量を観測する。以下の説明においてマップマッチングは、ＭＭ（Map Matching）と表記されるものとする。

【0029】

具体的にＭＭブロック１２０には、ＤＲブロック１００により推定された最新時刻ｔの状態量Ｚ_ｔが、入力される。ＭＭブロック１２０には、入力された状態量Ｚ_ｔのうち二次元座標位置ｘ_ｔ、ｙ_ｔに対応する地図情報Ｉ_ｍが、地図ユニット３からの読み出しにより入力される。ＭＭブロック１２０には、外界センサ２２のうちＬＩＤＡＲにより最新時刻ｔに取得された点群画像が、外界情報Ｉ_ｏとして入力される。これらの入力に基づきＭＭブロック１２０は、点群画像において外界物体を観測した観測点群Ｓ_ｏに対して、図６に示すようにマッチングする複数特徴点Ｓ_ｍを、地図情報Ｉ_ｍから抽出する。尚、図６では、観測点群Ｓ_ｏが白丸により表されている一方、複数特徴点Ｓ_ｍが黒丸により表されている。

【0030】

ＭＭブロック１２０は、観測点群Ｓ_ｏとマッチングした複数特徴点Ｓ_ｍに基づくことで、最新時刻ｔにおける車両４の状態量Ｚ_ｍを観測する。このときＭＭブロック１２０は、車両４の二次元座標位置ｘ_ｍ、ｙ_ｍ及びヨー角θ_ｍを用いて定義される下記式８のベクトル式により、状態量Ｚ_ｍを推定演算する。

【数8】

【0031】

図２に示すパラメータ補正ブロック１４０は、ＭＭブロック１２０により観測される状態量Ｚ_ｍに対して、ＤＲにより推定される状態量Ｚ_ｔが図７に示すようにオフセットした、オフセット量δｚに基づき動的パラメータｐ_ｔを補正する。そのためにパラメータ補正ブロック１４０は、最小化サブブロック１４２及び調整サブブロック１４４を有している。

【0032】

最小化サブブロック１４２には、ＤＲブロック１００により推定された最新時刻ｔの状態量Ｚ_ｔと、ＭＭブロック１２０により観測された同時刻ｔの状態量Ｚ_ｍとが、入力される。この入力に基づき最小化サブブロック１４２は、観測状態量Ｚ_ｍに対して推定状態量Ｚ_ｔのオフセットしたオフセット量δｚを、下記式９の差分演算により取得する。

【数9】

【0033】

最小化サブブロック１４２は、図７に示すように取得したオフセット量δｚのうち、ＤＲブロック１００による最新時刻ｔの推定状態量Ｚ_ｔが動的パラメータｐ_ｔの変動量δｐに応じてずれた、ずれ分δｚ_ｐを予測する。具体的にずれ分δｚ_ｐは、動的パラメータｐ_ｔ、ｐ_ｔ－１をそれぞれ変数とした最新時刻ｔ及び前回時刻ｔ－１での変位量関数ΔＺ間の差分値として、下記式１０により定義される。ここで式１０は、最新時刻ｔ及び前回時刻ｔ－１での動的パラメータｐ_ｔ、ｐ_ｔ－１間の変動量δｐを用いることで、下記式１１に変形される。ずれ分δｚ_ｐは、最新時刻ｔ及び前回時刻ｔ－１での動的パラメータｐ_ｔ、ｐ_ｔ－１間の変動量δｐを変数とした変位量関数ΔＺの値として、下記式１２により近似されてもよい。

【数10】

【数11】

【数12】

【0034】

そこで最小化サブブロック１４２には、前回時刻ｔ－１での動的パラメータｐ_ｔ－１が、メモリ１０における推定記憶領域１８０からの読み出しにより入力される。この入力に基づき最小化サブブロック１４２は、オフセット量δｚとの差の絶対値（即ち、絶対差）を最小にするずれ分δｚ_ｐを、下記式１３の最小化関数Ｍ_ａを用いた最適化演算により、取得する。このとき最小化サブブロック１４２は、動的パラメータｐ_ｔ－１を代入演算した式１１又は式１２に基づくことで、ずれ分δｚ_ｐに対応する変動量δｐの最適値も、取得する。

【数13】

【0035】

図２に示す調整サブブロック１４４には、前回時刻ｔ－１での動的パラメータｐ_ｔ－１が、推定記憶領域１８０からの読み出しにより入力される。調整サブブロック１４４には、ずれ分δｚ_ｐと対応して最小化サブブロック１４２により予測された変動量δｐの最適値が、入力される。これらの入力に基づき調整サブブロック１４４は、前回時刻ｔ－１の動的パラメータｐ_ｔ－１と変動量δｐの最適値と共にカルマンゲインＫ_ｐを用いた下記式１４の重み付け演算により、最新時刻ｔでの動的パラメータｐ_ｔを取得する。これは、カルマンフィルタを通すことで、動的パラメータｐ_ｔを変動量δｐの最適値により補正しているとも言える。調整サブブロック１４４は、こうして補正された動的パラメータｐ_ｔを、最新時刻ｔと関連付けて推定記憶領域１８０に記憶すると共に、次回の最新時刻に関する動的パラメータとしてＤＲブロック１００のＤＲへとフィードバックする。

【数14】

【0036】

図２に示す誤差補償ブロック１６０は、ＤＲブロック１００により推定される状態量Ｚ_ｔの誤差を、ＭＭブロック１２０により観測される状態量Ｚ_ｍに基づいたフィルタリングにより、補償する。

【0037】

具体的に誤差補償ブロック１６０には、ＤＲブロック１００により推定された最新時刻ｔの状態量Ｚ_ｔと、ＭＭブロック１２０により観測された最新時刻ｔの状態量Ｚ_ｍとが、入力される。この入力に基づき誤差補償ブロック１６０は、ＭＭブロック１２０による観測状態量Ｚ_ｍと共にカルマンゲインＫ_ｚを用いた下記式１５の重み付け演算により、ＤＲブロック１００による推定状態量Ｚ_ｔの誤差を補償する。これは、カルマンフィルタを通して観測状態量Ｚ_ｍをフュージョンさせることで、推定状態量Ｚ_ｔを確定しているとも言える。尚、誤差補償により確定された推定状態量Ｚ_ｔは、誤差補償ブロック１６０からの出力により、例えば車両４の高度運転支援又は自動運転制御等に活用される。

【数15】

【0038】

ここで式１５は、オフセット量δｚを用いて変形されることで、下記式１６により表される。そこで、図８及び図９にそれぞれ変形例を示すように誤差補償ブロック１６０は、最小化サブブロック１４２による取得量δｚと共にカルマンゲインＫ_ｚを用いた下記式１６の重み付け演算により、ＤＲブロック１００による推定状態量Ｚ_ｔの誤差を補償してもよい。尚、図８は、図２に対しての変形例を示している一方、図９は、図５の変形例に対してのさらなる変形例を示している。

【数16】

【0039】

以上より本実施形態では、ＤＲブロック１００が「デッドレコニング部」に相当し、ＭＭブロック１２０が「マップマッチング部」に相当する。また本実施形態では、パラメータ補正ブロック１４０が「パラメータ補正部」に相当し、誤差補償ブロック１６０が「誤差補償部」に相当する。

【0040】

機能ブロック１００、１２０、１４０及び１６０の共同により、推定装置１が車両４の状態量を推定する推定方法のフローを、図１０に従って以下に説明する。尚、本フローは、車両４が推定を必要する推定タイミング毎に、実行される。また、本フローにおいて「Ｓ」とは、推定プログラムに含まれた複数命令により実行される複数ステップを、意味する。

【0041】

Ｓ１０１においてＤＲブロック１００は、車両挙動を変化させる動的パラメータｐ_ｔを含んだダイナミクスモデルＤＭと、車両４の内界から取得される内界情報Ｉ_ｉとに基づいたＤＲにより、車両４の状態量Ｚ_ｔを推定する。Ｓ１０２においてＭＭブロック１２０は、車両４の走行環境を表す地図情報Ｉ_ｍと、車両４の外界から取得される外界情報Ｉ_ｏとに基づいたＭＭにより、車両４の状態量Ｚ_ｍを観測する。

【0042】

Ｓ１０３においてパラメータ補正ブロック１４０は、Ｓ１０２のＭＭにより観測される状態量Ｚ_ｍに対して、Ｓ１０１のＤＲにより推定される状態量Ｚ_ｔのオフセットしたオフセット量δｚに基づき、動的パラメータｐ_ｔを補正する。このときパラメータ補正ブロック１４０は、車両挙動以外の要因に応じて変動する動的パラメータｐ_ｔとして、車両４が走行する路面の動摩擦係数を含んだパラメータを、補正対象とする。そこで、パラメータ補正ブロック１４０は、オフセット量δｚのうち、補正対象とした動的パラメータｐ_ｔの変動量δｐに応じてＤＲによる推定状態量Ｚ_ｔがずれた、ずれ分δｚ_ｐを予測する。このときラメータ補正ブロック１４０は、オフセット量δｚとの差の絶対値を最小にするように、ずれ分δｚ_ｐを予測する。パラメータ補正ブロック１４０はまた、予測のずれ分δｚ_ｐに対応した変動量δｐの最適値により、動的パラメータｐ_ｔを補正する。パラメータ補正ブロック１４０はさらに、補正した動的パラメータｐ_ｔを、次回フローのＳ１０１によるＤＲブロック１００でのＤＲに向けてフィードバックする。

【0043】

Ｓ１０４において誤差補償ブロック１６０は、Ｓ１０２のＭＭにより観測される状態量Ｚ_ｍに基づいたフィルタリングにより、Ｓ１０１のＤＲにより推定される状態量Ｚ_ｔの誤差を補償して確定する。以上により、本フローの今回実行が終了する。尚、Ｓ１０３とＳ１０４とは、図１０に示すようにステップナンバーの順に実行されてもよいし、図１１に変形例を示すように逆順に実行されてもよいし、図１２に別の変形例を示すように同時並行的に実行されてもよい。

【0044】

以上より本実施形態では、Ｓ１０１が「デッドレコニングプロセス」に相当し、Ｓ１０２が「マップマッチングプロセス」に相当する。また本実施形態では、Ｓ１０３の組が「パラメータ補正プロセス」に相当し、Ｓ１０４が「誤差補償プロセス」に相当する。

【0045】

（作用効果）
以上説明した本実施形態の作用効果を、以下に説明する。
本実施形態によると、車両挙動を変化させる動的パラメータｐ_ｔの補正は、ＭＭによる観測状態量Ｚ_ｍに対してＤＲによる推定状態量Ｚ_ｔがオフセットしたオフセット量δｚに、基づくこととなる。この補正によれば、動的パラメータｐ_ｔを正確に補正してＤＲへとフィードバックすることができる。故に、フィードバックされた動的パラメータｐ_ｔを含むダイナミクスモデルＤＭに基づくことで、ＤＲによる状態量Ｚ_ｔの推定精度を高めることが可能となる。

【0046】

本実施形態によると、オフセット量δｚのうち、ＤＲによる推定状態量Ｚ_ｔが動的パラメータｐ_ｔの変動量δｐに応じてずれたずれ分δｚ_ｐの予測下、当該予測のずれ分δｚ_ｐに対応した変動量δｐにより、動的パラメータｐ_ｔが補正される。この補正によれば、オフセット量δｚの発生要因となっている変動量δｐが正確に反映された動的パラメータｐ_ｔを、ＤＲへとフィードバックすることができる。故に、フィードバックされた動的パラメータｐ_ｔを含むダイナミクスモデルＤＭに基づくことで、ＤＲによる状態量Ｚ_ｔの推定精度を高めることが可能となる。

【0047】

本実施形態によると、オフセット量δｚとの差を最小にするずれ分δｚ_ｐに対応した変動量δｐは、それにより補正される動的パラメータｐ_ｔを含んでのダイナミクスモデルＤＭに基づくＤＲにより、推定状態量Ｚ_ｔを観測状態量Ｚ_ｍに可及的に近づけることができる。即ち、ＤＲによる状態量Ｚ_ｔの推定精度を高めることが可能となる。

【0048】

本実施形態によると、車両が走行する路面の動摩擦係数を含んだ動的パラメータｐ_ｔのように、車両挙動以外の要因に応じて変動する動的パラメータｐ_ｔが、オフセット量δｚに基づき補正される。この補正によれば、車両挙動以外の要因に応じた変動分（即ち、変動量δｐ）が誤差となる動的パラメータｐ_ｔを狙って、当該誤差を補正することができる。故に、この補正に応じてフィードバックされた動的パラメータｐ_ｔを含むダイナミクスモデルＤＭに基づくことで、ＤＲによる状態量Ｚ_ｔの推定精度を高めることが可能となる。

【0049】

本実施形態によると、ＭＭによる観測状態量Ｚ_ｍに基づいたフィルタリングにより、ＤＲによる推定状態量Ｚ_ｔの誤差が補償される。この誤差補償によれば、オフセット量δｚに基づく補正に応じてフィードバックされる動的パラメータｐ_ｔを含んだ、ダイナミクスモデルＤＭの利用と相俟って、状態量Ｚ_ｔの高い推定精度を担保することが可能となる。

【0050】

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

【0051】

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

【0052】

変形例の推定装置１では、推定及び観測対象の状態量Ｚ_ｔ、Ｚ_ｍとして、それぞれヨー角θ_ｔ、θ_ｍに代えて又は加えて、例えば速度等の車両関連物理量が含まれていてもよい。変形例の推定装置１では、カルマンフィルタでの重み付け演算に代えて、前回時刻ｔ－１での動的パラメータｐ_ｔ－１に対する変動量δｐの加算演算により、動的パラメータｐ_ｔがパラメータ補正ブロック１４０及びＳ１０３において補正されてもよい。

【0053】

変形例の推定装置１では、ＤＲブロック１００及びＳ１０１による推定状態量Ｚ_ｔの誤差を、ＭＭブロック１２０及びＳ１０２とは異なる原理により推定された状態量に基づき補償するように、誤差補償ブロック１６０及びＳ１０４が実施されてもよい。変形例の推定装置１では、誤差補償ブロック１６０及びＳ１０４が省略されることで、ＤＲブロック１００及びＳ１０１による推定状態量Ｚ_ｔがそのまま確定値として出力されてもよい。

【符号の説明】

【0054】

１ 推定装置、４ 車両、１０ メモリ、１２ プロセッサ、１００ デッドレコニング（ＤＲ）ブロック、１２０ マップマッチング（ＭＭ）ブロック、１４０ パラメータ補正ブロック、１６０ 誤差補償ブロック、ＤＭ ダイナミクスモデル、Ｉ_ｉ 内界情報、Ｉ_ｍ 地図情報、Ｉ_ｏ 外界情報、Ｚ_ｍ （観測）状態量、Ｚ_ｔ （推定）状態量、ｐ_ｔ 動的パラメータ、δｐ 変動量、δｚ オフセット量、δｚ_ｐ ずれ分

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】