JP2017134515A - 状態推定装置 - Google Patents

Abstract

【課題】推定初期段階において計測値が不必要に棄却される現象が生じることを抑制する状態推定装置を提供する。【解決手段】状態推定装置１は、カルマンフィルタ１０によって、他車両３３の位置及び速度を推定する装置であり、処理（ｔ−１）の推定値Ｍt-1に基づいて予測値ｍtを得る予測部１１と、観測値ｚtを得る計測部３と、観測値ｚtと予測値ｍtとに基づいて推定値候補Ｍ'tを得る観測更新部１３と、予測値ｍtと推定値候補Ｍ'tとの差が所定範囲外である場合には、観測更新部１３で用いられるパラメータである観測ノイズ共分散Ｑを変更し、それ以外の場合には、推定値候補Ｍ'tを処理（ｔ）の推定値Ｍtとして出力する収束確認部１５と、を備え、上記差が所定範囲外である場合には、上記差が所定範囲内になるまで、観測ノイズ共分散Ｑの変更と観測更新部１３による演算とが繰り返し実行される。【選択図】図１

Description

本発明は、状態推定装置に関するものである。

従来、下記特許文献１のように、カルマンフィルタを用いて先行車の位置と速度とを推定する推定装置が知られている。この特許文献１の推定装置では、カルマンフィルタが予測する先行車の位置の予測値と、センサで計測した先行車の位置の計測値との差が所定値以上の場合には、センサの計測値を棄却しカルマンフィルタに反映させないようにしている。

特開平9-96521号公報

しかしながら、上記推定装置では、カルマンフィルタによる予測値とセンサによる計測値との差を判断基準として、計測値を棄却するか否かを決定しているので、カルマンフィルタによる予測値が不正確である推定初期段階では、センサの計測値が不必要に棄却されるおそれがあった。

本発明は、推定初期段階において計測値が不必要に棄却される現象が生じることを抑制する状態推定装置を提供することを目的とする。

本発明の状態推定装置は、前回得られた物体の状態の推定値に基づいて物体の状態の予測値を得ると共に、物体の状態の観測値に基づいて予測値を更新し、更新された予測値に基づいて今回の推定値を得るカルマンフィルタによって、物体の状態を推定する状態推定装置であって、前回の推定値に基づいて予測値を得る予測部と、物体の状態を観測して観測値を得る観測部と、観測値と予測値とに基づいて演算を行い今回の推定値の候補である推定値候補を得る観測更新部と、予測値と推定値候補との差が所定範囲外である場合には、観測更新部による演算で用いられる所定のパラメータを変更し、差が所定範囲内である場合には、推定値候補を今回の推定値として出力する収束確認部と、を備え、差が所定範囲外である場合には、差が所定範囲内になるまで、収束確認部によるパラメータの変更と、変更後の当該パラメータを用いた観測更新部による演算と、が繰り返し実行される。

本発明によれば、推定初期段階において計測値が不必要に棄却される現象が生じることを抑制する状態推定装置を提供することができる。

実施形態に係る状態推定装置を示すブロック図である。 状態推定装置が搭載される自車両と状態推定の対象である他車両とを示す平面図である。 状態推定装置による処理（ｔ）での処理を示すフローチャートである。

以下、図面を参照しつつ本発明に係る状態推定装置の実施形態について詳細に説明する。図１に示される状態推定装置１は、カルマンフィルタ１０を用いて、物体の状態を推定するものである。すなわち、状態推定装置１は、カルマンフィルタ１０を用いて、前回の処理で得られた物体の状態の推定値に基づいて物体の状態の予測値を得ると共に、物体の状態の観測値に基づいて予測値を更新し今回の処理における推定値を得る。状態推定装置１では、推定値を得るための上記のような状態推定処理が繰返し実行される。以下の説明においては、繰返し実行される状態推定処理のうちのｊ回目の処理を「処理（ｊ）」と表すものとする（ｊは自然数）。状態推定装置１は、例えば、物体を検出する物体検出装置の一部として存在するものである。

図２に示されるように、状態推定装置１が自車両３１に搭載される場合、状態を推定する対象の物体は、例えば、自車両３１の周辺に存在し後述する計測部３（例えばレーザレーダ）により検出可能な範囲に位置する障害物である。図２の例の場合、上記障害物は、自車両３１の周辺を走行する他車両Ｄのうちの一台（以下「他車両３３」）である。物体の状態とは、例えば、自車両３１に対する他車両３３の相対位置及び相対速度である。以下の説明では、自車両３１に搭載された状態推定装置１が、他車両３３の相対位置及び相対速度を推定する場合を例として説明する。なお、以下の説明において、他車両３３の状態に関して、単に「位置」、「速度」と言う場合には、自車両３１に対する他車両３３の相対位置及び相対速度を意味するものとする。

図１に示されるように、状態推定装置１は、計測部（観測部）３とＥＣＵ[EngineControl Unit]５とを備える。

例えば、計測部３は、自車両３１に搭載されるレーザレーダ(LIDAR［Laser Imaging Detection and Ranging］)であってもよい。レーザレーダは、自車両３１の前端に設けられ、レーザーを利用して車両前方の障害物（例えば、他車両３３）を検出する。レーザレーダは、レーザーを車両前方に送信し、障害物に反射したレーザーを受信することで障害物を検出する。レーザレーダは、検出した障害物に関する障害物情報をＥＣＵ５に送信する。なお、レーザレーダに代えて、ミリ波レーダ等を用いてもよい。ミリ波レーダは、自車両３１の車体前端及び自車両３１の車体後端に設けられ、ミリ波を利用して自車両３１前後の障害物を検出する。ミリ波レーダは、更に自車両３１の側面に設けられ、自車両３１の側方の障害物を検出してもよい。ミリ波レーダは、ミリ波を自車両３１の前後に送信し、他車両等の障害物に反射したミリ波を受信することで障害物を検出する。ミリ波レーダは、検出した障害物に関する障害物情報をＥＣＵ５に送信する。なお、計測部３からＥＣＵ５に送信される上記の障害物情報には、障害物（例えば、他車両３３）の位置情報や速度情報が含まれる。

ＥＣＵ５は、ＣＰＵ[Central Processing Unit]、ＲＯＭ[Read Only Memory]、ＲＡＭ[Random Access Memory]、ＣＡＮ［Controller Area Network］通信回路等を有する電子制御ユニットである。ＥＣＵ５では、例えば、ＲＯＭに記憶されているプログラムをＣＡＮ通信回路を介してＲＡＭにロードし、ＲＡＭにロードされたプログラムをＣＰＵで実行することにより、後述する予測部１１、観測更新部１３、及び収束確認部１５等の各種の機能を実現する。ＥＣＵ５は、複数の電子制御ユニットから構成されていてもよい。

ＥＣＵ５は、前述の通り、他車両３３の位置及び速度を、カルマンフィルタ１０を用いて推定する状態推定処理を実行する。具体的には、状態推定処理では、他車両３３のｘ方向の位置（ｘ）、ｙ方向の位置（ｙ）、ｘ方向の速度（ｖ_x）及びｙ方向の速度（ｖ_y）が推定される。任意のｔ回目の状態推定処理（処理（ｔ））で得られる、他車両３３の位置及び速度の推定値Ｍ_tは、下式(1)で定義される。

また、ＥＣＵ５には、計測部３で計測された他車両３３の位置（ｘ，ｙ）の計測値が入力される。上記計測値は、前述の通り、他車両３３の位置情報として、計測部３から送信された障害物情報に含まれている。この計測値は、上記カルマンフィルタ１０に入力される観測値ｚ_tとして状態推定処理に使用される。この観測値ｚ_tは、下式(2)で定義される。

続いて、ＥＣＵ５の機能的構成について説明を行う。ＥＣＵ５は、カルマンフィルタ１０を構成する、予測部１１と、観測更新部１３と、収束確認部１５と、を備えている。また、収束確認部１５は、収束判定部２１と、切替部２３と、パラメータ変更部２５と、を有している。

予測部１１は、前回の処理（ｔ−１）で得られた、他車両３３の位置及び速度の推定値Ｍ_t-1に基づいて、今回の処理（ｔ）における位置及び速度の予測値ｍ_tを更新し出力する。具体的には、予測部１１は、下式(3),(4)によって、予測値ｍ_tを算出する。

ここでΔｔは、処理（ｔ−１）から処理（ｔ）までに要する時間を表す。なお、予測部１１による予測値ｍ_tの算出においては、自車両３１の走行制御に関する他の制御入力等の情報が更にカルマンフィルタ１０に入力され、当該制御入力等の情報が予測値ｍ_tに反映されてもよい。また、他車両３３の走行制御に関する制御入力の情報が通信等で取得可能な場合には、当該他車両３３の情報が更にカルマンフィルタ１０に入力され、当該他車両３３の情報が予測値ｍ_tに反映されてもよい。

更に、予測部１１は、前回の処理（ｔ−１）の推定誤差共分散Ｓ_t-1に基づいて、今回の処理（ｔ）における予測誤差共分散ｓ_tを算出し出力する。具体的には、予測部１１は、下式(5),(6)によって、予測誤差共分散ｓ_tを算出する。

ここで、Ｒは、処理（ｔ）での予測に伴う推定誤差の拡大を表す行列であり、システムノイズ共分散と呼ばれる。σ_x ²，σ_y ²，σ_vx ²，及びσ_vy ²は、他車両３３のｘ方向の位置、ｙ方向の位置、ｘ方向の速度、及びｙ方向の速度、のそれぞれの分散値であり、これらの値は、状態推定装置１の特性に応じて予め設定され、例えばＥＣＵ５の記憶領域に保存されている。

観測更新部１３は、予測部１１で得られた予測値ｍ_t及び予測誤差共分散ｓ_tと、計測部３から入力された観測値ｚ_tと、に基づいて、推定値候補Ｍ'_tを算出し出力する。具体的には、観測更新部１３は、下式(7),(8),(9),(10)によって、推定値候補Ｍ'_tを算出する。推定値候補Ｍ'_tは、処理（ｔ）における推定値Ｍ_tの候補となる。

ここで、Ｑは処理（ｔ）での観測に伴う誤差を表す行列であり、観測ノイズ共分散と呼ばれる。また、Ｋ_tは、状態推定装置１における予測値と観測の誤差に対するゲインであり、カルマンゲインと呼ばれる。観測ノイズ共分散Ｑの値は、状態推定装置１の特性に応じて予め設定され、例えばＥＣＵ５の記憶領域に保存されている。

更に、観測更新部１３は、予測部１１で得られた予測誤差共分散ｓ_tに基づき、下式(11)によって、推定誤差共分散Ｓ_tを算出し出力する。

収束判定部２１は、予測部１１から入力される予測値ｍ_tと、観測更新部１３から入力される推定値候補Ｍ'_tとの差（｜Ｍ'_t−ｍ_ｔ｜）が、予め設定された閾値θ未満であるか否かを判定する。閾値θは、下式(12)のように定義される。具体的な閾値θの値は、予め設定され、例えばＥＣＵ５の記憶領域に保存されている。

そして、下式(13)に示されるように、上記差が閾値θ未満である場合には、収束判定部２１はtrueフラグを出力し、それ以外の場合には、収束判定部２１はfalseフラグを出力する。

より詳細には、｜Ｍ'_t−ｍ_ｔ｜＝[Δｘ，Δｙ，Δｖ_x，Δｖ_y]と定義すれば、Δｘ＜Δｘ_maxと、Δｙ＜Δｙ_maxと、Δｖ_x＜Δｖ_x,maxと、Δｖ_y＜Δｖ_y,maxと、がすべて満たされた場合に、収束判定部２１はtrueフラグを出力し、上記以外の場合には収束判定部２１はfalseフラグを出力する。なお、数式(8)から、観測更新部１３による修正量Ｍ'_t−ｍ_ｔは下式(14)のように求められる。また、前述の「予測値ｍ_tと推定値候補Ｍ'_tとの差」は、当該修正量の絶対値（｜Ｍ'_t−ｍ_ｔ｜）である。

切替部２３は、収束判定部２１からの入力がtrueであるかfalseであるかによって、観測更新部１３からの入力を、後段の他の処理部２９に出力するか、パラメータ変更部２５側に出力するかを切り替える切替器として機能する。具体的には、切替部２３は、収束判定部２１からの入力がfalseである場合には、観測更新部１３からの入力をパラメータ変更部２５に出力する。これに対応して、パラメータ変更部２５は、演算のパラメータである観測ノイズ共分散Ｑを観測更新部１３から取得する。観測ノイズ共分散Ｑは、観測更新部１３において推定値候補Ｍ'_tの算出の演算に使用されるパラメータの一つである。そして、パラメータ変更部２５は、当該観測ノイズ共分散Ｑを変更する。具体的には、パラメータ変更部２５は、取得された観測ノイズ共分散Ｑに係数ｋを乗じてｋＱとし、変更後の新たな観測ノイズ共分散ｋＱを観測更新部１３に出力する。

その後、処理は観測更新部１３に戻され、観測更新部１３は、新たな観測ノイズ共分散ｋＱを用いて、式(7)〜(10)に従って新たな推定値候補Ｍ'_tを算出すると共に、式(11)に従って新たな推定誤差共分散Ｓ_tを算出する。上記の係数ｋは、例えば、１＜ｋを満たす固定値とされてもよい。この場合、パラメータ変更部２５による変更処理ごとに、観測ノイズ共分散Ｑは大きくなる。

その一方、切替部２３は、収束判定部２１からの入力がtrueである場合には、観測更新部１３から入力された推定値候補Ｍ'_t及び推定誤差共分散Ｓ_tを、処理（ｔ）における推定値Ｍ_t及び推定誤差共分散Ｓ_tとして、後段の処理部２９に出力する。これらの推定値Ｍ_t及び推定誤差共分散Ｓ_tは、他車両３３の位置及び速度の推定値であり、状態推定処理の成果として後段の処理部２９に引渡され利用される。また、推定値Ｍ_t及び推定誤差共分散Ｓ_tは、予測部１１にも再び入力され、次回の処理（ｔ＋１）に利用される。なお上記の処理部２９は、他車両３３の位置及び速度に基づいて、例えば、自車両３１の所定の運転支援プログラム等を実行するものである。

上述のような収束確認部１５によれば、前述の修正量の絶対値（｜Ｍ'_t−ｍ_ｔ｜）が閾値θ未満の場合にのみ、推定値Ｍ_tと推定誤差共分散Ｓ_tとが状態推定装置１から出力される。そして、修正量の絶対値が閾値θ以上の場合には、修正量の絶対値がθ未満になるまで、パラメータ変更部２５による観測ノイズ共分散Ｑの変更と、変更後の観測ノイズ共分散ｋＱを用いた観測更新部１３による推定値候補Ｍ'_t及び推定誤差共分散Ｓ_tの算出と、が繰り返し実行される。

また、数式(7),(14)によれば、修正量Ｍ'_t−ｍ_ｔは、予測誤差共分散ｓ_tと観測ノイズ共分散Ｑとの相対関係によって決まる。予測誤差共分散ｓ_tが固定で観測ノイズ共分散Ｑが増加すれば、予測値ｍ_tの誤差に対する観測値ｚ_tの誤差が相対的に大きくなる。このため、観測ノイズ共分散Ｑが大きくなった場合に、この状態推定処理におけるカルマンフィルタ１０は、観測値ｚ_tに基づく修正量Ｍ'_t−ｍ_ｔを小さくする。このような観測ノイズ共分散Ｑと修正量Ｍ'_t−ｍ_ｔとの関係を利用して、修正量の絶対値が閾値θ未満となるような観測ノイズ共分散Ｑが求められる。また、上記のような観測ノイズ共分散Ｑと修正量Ｍ'_t−ｍ_ｔとの関係を勘案して係数ｋを決定することで、推定値候補Ｍ'_tの再計算の回数を削減することができる。上記の係数ｋは、前述の通り、１＜ｋを満たす固定値としてもよく、二分木探索等の手法により｜Ｍ'_t−ｍ_ｔ｜＜θを満たす最小の係数ｋを求めてもよい。

続いて、任意の処理（ｔ）において実行される状態推定装置１の処理について、図３のフローチャートを参照しながら説明する。まず、処理（ｔ）では、予測部１１が、前回の処理（ｔ−１）で得られた推定値Ｍ_t-1及び推定誤差共分散Ｓ_t-1を取得する（図３のＳ１０１）。予測部１１は、推定値Ｍ_t-1に基づいて、数式(3),(4)により予測値ｍ_tを算出する。また、予測部１１は、推定誤差共分散Ｓ_t-1に基づいて、数式(5),(6)により予測誤差共分散ｓ_tを算出する（図３のＳ１０３）。

観測更新部１３は、計測部３から入力された観測値ｚ_tを取得し（図３のＳ１０５）、観測値ｚ_tと予測値ｍ_tとに基づいて、数式(7)〜(10)により推定値候補Ｍ'_tを算出する。更に、観測更新部１３は、予測誤差共分散ｓ_tに基づいて、数式(11)により推定誤差共分散Ｓ_tを算出する（図３のＳ１０７）。

次に、収束判定部２１は、予測値ｍ_tと推定値候補Ｍ'_tとに基づき、｜Ｍ'_t−ｍ_ｔ｜＜θの条件が満たされているか否かを判定する（図３のＳ１０９）。ここで、上記条件が満たされていない場合には、パラメータ変更部２５が、観測ノイズ共分散ＱをｋＱに変更し（Ｓ１１１）、変更後の観測ノイズ共分散ｋＱを観測更新部１３に引き渡す。そして、処理は前述のＳ１０７に戻り、観測更新部１３では、観測ノイズ共分散Ｑを観測ノイズ共分散ｋＱに変更して、推定値候補Ｍ'_tと推定誤差共分散Ｓ_tとが再計算される（図３のＳ１０７）。

一方、Ｓ１０９の判定において、｜Ｍ'_t−ｍ_ｔ｜＜θの条件が満たされている場合には、推定値候補Ｍ'_tが処理（ｔ）における推定値Ｍ_tとして出力されると共に、推定誤差共分散Ｓ_tが出力され（図３のＳ１１３）、処理（ｔ）が終了する。なお、処理（ｔ）が終了すると、観測ノイズ共分散Ｑの値は初期値に戻る。

上述のＳ１０７，Ｓ１０９，及びＳ１１１の処理によれば、｜Ｍ'_t−ｍ_ｔ｜＜θの条件が満たされていない場合には、｜Ｍ'_t−ｍ_ｔ｜＜θの条件が満たされるまで、推定値候補Ｍ'_tの算出に用いられるパラメータ（観測ノイズ共分散Ｑ）を変えながら、推定値候補Ｍ'_tの再計算が繰り返され、｜Ｍ'_t−ｍ_ｔ｜＜θの条件を満足する推定値Ｍ_tの導出が繰り返し試行される。

以上説明したような状態推定装置１による作用効果について説明する。従来のカルマンフィルタでは、例えば、計測部が、対象の物体の一部を欠損して計測したり、複数の物体を結合して計測したりするなど、想定外の事象が発生した場合には、予測値と観測値との残差が大きくなる。この場合、予測値に対して観測値に基づく過剰な修正が加わり、推定値が急変動するといった問題があった。これに対し、状態推定装置１によれば、収束確認部１５の機能により、修正量の絶対値が所定以上の場合には推定値Ｍ_tが出力されず、修正量の絶対値が所定未満の場合にのみ推定値Ｍ_tが出力されるので、状態推定処理における推定値Ｍ_tの急激な変動を抑えることができる。

また、前述の特許文献１の推定装置では、カルマンフィルタによる予測値とセンサによる計測値との差を判断基準として、当該差が所定値以上の場合には、センサの計測値を棄却しカルマンフィルタに反映させないようにしているので、カルマンフィルタによる予測値が不正確である推定初期段階では、センサの計測値が不必要に棄却されるという問題があった。これに対して状態推定装置１では、修正量の絶対値が所定以上の場合には、修正量の絶対値が所定未満になるまで推定値候補Ｍ'_tの再計算が繰り返され、修正量の絶対値が所定未満となったときに推定値候補Ｍ'_tが推定値Ｍ_tとして出力される。従って、推定初期段階においても、計測値が不必要に棄却されることを抑えることができる。

また、特許文献１の推定装置では、前述のように想定外の事象が発生した場合に、推定値が実際の値から徐々に乖離してしまうという問題があるところ、これに対して状態推定装置１では、修正量が所定未満である推定値Ｍ_tが得られるので、推定値Ｍ_tを常に計測値に近づけることができる。

また、観測更新部１３による推定値候補Ｍ'_tの再計算は、その演算に用いるパラメータを変更しながら繰り返される。ここで、変更するパラメータとしては、計測部３の計測誤差に関連するパラメータである観測ノイズ共分散Ｑが採用されている。このような観測ノイズ共分散Ｑを増加させながら上記再計算を繰り返すことにより、修正量の絶対値は、再計算ごとに減少するように推移する。従って、推定値候補Ｍ'_tの再計算の回数が低減され、推定値Ｍ_tが出力されるまでの時間が短縮される。

本発明は、上述した実施形態を始めとして、当業者の知識に基づいて種々の変更、改良を施した様々な形態で実施することができる。また、上述した実施形態に記載されている技術的事項を利用して実施例の変形例を構成することも可能である。各実施形態の構成を適宜組み合わせて使用してもよい。

１…状態推定装置、３…計測部（観測部）、１０…カルマンフィルタ、１１…予測部、１３…観測更新部、１５…収束確認部、３３…他車両（物体）。

Claims (1)

1. 前回得られた物体の状態の推定値に基づいて前記物体の状態の予測値を得ると共に、前記物体の状態の観測値に基づいて前記予測値を更新し、更新された前記予測値に基づいて今回の推定値を得るカルマンフィルタによって、前記物体の状態を推定する状態推定装置であって、
   前回の前記推定値に基づいて前記予測値を得る予測部と、
   前記物体の状態を観測して前記観測値を得る観測部と、
   前記観測値と前記予測値とに基づいて演算を行い今回の前記推定値の候補である推定値候補を得る観測更新部と、
   前記予測値と前記推定値候補との差が所定範囲外である場合には、前記観測更新部による前記演算で用いられる所定のパラメータを変更し、前記差が前記所定範囲内である場合には、前記推定値候補を今回の前記推定値として出力する収束確認部と、を備え、
   前記差が前記所定範囲外である場合には、前記差が前記所定範囲内になるまで、前記収束確認部による前記パラメータの変更と、変更後の当該パラメータを用いた前記観測更新部による前記演算と、が繰り返し実行される、状態推定装置。

Images