(57)【要約】【課題】より高精度に移動体の状態を算出可能な状態算出装置等を提供すること。【解決手段】状態算出装置は、車両において用いられる第一センサによって検知された車両の周辺に存在する複数の物体の方位および車両に対する相対速度を物標情報として受け取り、さらに、車両に搭載されて誤差分散を有する第二センサによって検知された車両の速度および移動方向を状態情報として受け取る入力部と、状態情報を用いて物標情報から抽出された複数の方位および相対速度に基づき車両の速度および移動方向を複数算出し、算出された複数の速度の平均値および誤差分散と、算出された複数の移動方向の平均値および誤差分散と、第二センサによって検知された速度および移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、車両の速度および移動方向のうち少なくとも一方を出力する制御部と、を備えている。【選択図】図１
2018055539.tif 000002
【特許請求の範囲】
【請求項１】
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取る入力部と、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力する制御部と、
を備えた状態算出装置。
【請求項２】
前記制御部は、前記状態情報を用いて、前記物標情報から、複数の方位にある静止物体であろう物標情報を２つずつ抽出し、複数の連立方程式の解を算出することで、前記車両の速度および移動方向を複数算出する、
請求項１に記載の状態算出装置。
【請求項３】
前記制御部は、前記車両の速度の平均値および誤差分散と、前記車両の移動方向の平均値および誤差分散と、前記状態情報を構成する前記車両の速度および移動方向と、それらの誤差分散とを、カルマンフィルタを用いてフィルタリングする、
請求項１に記載の状態算出装置。
【請求項４】
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
を備えた状態算出方法。
【請求項５】
コンピュータに、
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
を実行させるプログラム。 
【請求項６】
コンピュータに、
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
を実行させるプログラムを記録した記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本開示は、移動体の状態を示す情報を算出する状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体に関する。
【背景技術】
【０００２】
従来、この種の状態算出装置等としては、例えば、下記特許文献１に記載の車載装置がある。この車載装置は、車両に搭載された車両状態検知部から得られたデータをフィルタにかける際に、レーダ装置またはカメラから進路を予測した結果を反映する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１４−１９１５９６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１では、車両状態センサから得られる結果と、実際の車両の挙動との間には、様々な要因で誤差が生じる場合がある。このような場合、特許文献１の車載装置は車両の状態を正しく推定することが難しい。
【０００５】
そこで、本開示は、より高精度に移動体の状態を算出可能な状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本開示の一形態は、
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取る入力部と、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力する制御部と、
を備えた状態算出装置に向けられる。
【０００７】
本開示の他の形態は、
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
を備えた状態算出方法に向けられる。
【０００８】
本開示のさらに他の形態は、
コンピュータに、
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
を実行させるプログラムに向けられる。
【０００９】
本開示のさらに他の形態は、
コンピュータに、
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
を実行させるプログラムを記録した記録媒体に向けられる。
【発明の効果】
【００１０】
本開示によれば、より高精度に移動体の状態を算出可能な状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体を提供することが出来る。
【図面の簡単な説明】
【００１１】
【図１】本開示の一実施形態に係る状態算出装置の構成を示すブロック図
【図２】図１の第一センサ部の設置位置と、移動速度および移動方位との関係を示す図
【図３】図１の移動推定部で使用される方位−ドップラー速度のパワーマップを例示する図
【図４】図１の第一センサ部の視野角内における複数の静止物体を示す図
【図５】方位−ドップラー速度のパワーマップにおける静止物曲線と静止物体マージンを示す図
【図６】図１の移動推定部の処理手順を示すフロー図
【図７】図１のフィルタ部の詳細な構成を示すブロック図
【図８】図１のフィルタ部への情報入力のタイミングを示す図
【図９】誤差の共分散行列推定値における車速の誤差分散値の経時変化を示す図
【図１０】本開示の変形例に係る状態算出装置の構成を示すブロック図
【発明を実施するための形態】
【００１２】
＜１．実施形態の状態算出装置の構成＞
以下、図面を参照して、本開示の一実施形態に係る状態算出装置１を説明する。
【００１３】
図１において、状態算出装置１は、所謂センサフュージョンにより、後述の第一センサ部３からの物標情報と、第二センサ部５からの状態情報とを組み合わせることで、移動体の状態を高精度に算出する。
【００１４】
本開示では、図１に示すように、移動体の一例としての車両Ｍに、状態算出装置１の一形態としてのＥＣＵ１と、第一センサ部３と、第二センサ部５と、が搭載される。
【００１５】
まず、第一センサ部３について説明する。
第一センサ部３、例えば、ミリ波帯のレーダ送信波を用いたパルス方式、または、ＦＭＣＷ（Ｆｒｅｑｕｅｎｃｙ−Ｍｏｄｕｌａｔｅｄ  Ｃｏｎｔｉｎｕｏｕｓ  Ｗａｖｅ）方式のレーダセンサである。
【００１６】
具体的には、第一センサ部３は、アレイアンテナ（図示は省略）からレーダ送信波を、自身の視野内に向けて所定の角度刻みで出射する。各出射波が車両Ｍの周囲に存在する物体によって反射され、反射波の一部を戻り波として、第一センサ部３のアレイアンテナが受信する。また、第一センサ部３では、信号処理回路（図示せず）が、各アレイ素子に対応する複数のブランチの信号に対して周波数解析および方位推定を行う。その結果、第一センサ部３は、大略的には予め定められた基準方位に対する反射点の方位（視野角方位）、車両Ｍから反射点までの距離、戻り波の受信強度、車両Ｍに対する反射点のドップラー速度等を物標情報として算出し、例えばＣＡＮやＦｌｅｘＲａｙ等、予め定められたデータ伝送方式に従ってＥＣＵ１に送信する。
【００１７】
第二センサ部５は、状態算出装置１（ひいては車両Ｍ）の移動状態を検出する複数のセンサである。本開示では、第二センサ部５は、少なくとも、車両Ｍの速度（以下、車速という）と、ヨーレートまたはヨー角（以下、ヨーレート等という）を検出する。車速は、周知の車速センサにより、ヨー角はステアリングホイールに設けられた周知の舵角センサにより、また、ヨーレートは周知のヨーセンサ等により検出される。
【００１８】
第二センサ部５は、検出した車速と、ヨーレート等を、状態情報として、例えばＣＡＮ等、予め定められたデータ伝送方式に従ってＥＣＵ１に出力する。
【００１９】
ＥＣＵ１は、上記の通り、状態算出装置１の一実施形態であって、ケース内に収容された基板上に、入力部１１と、制御部１５と、を備えている。
【００２０】
入力部１１は、第一センサ部３からの物標情報を受け取るための入力インタフェイスである。入力部１１はさらに、制御部１５の制御下で、受け取った物標情報を制御部１５のワーキングメモリ（後述）に転送する。これにより、制御部１５は、物標情報を取得する。
【００２１】
入力部１１は、第二センサ部５からの状態情報を受け取るための入力インタフェイスでもある。入力部１１はさらに、制御部１５の制御下で、受け取った状態情報をワーキングメモリへ転送する。これにより、制御部１５は、状態情報を取得する。
【００２２】
制御部１５は、例えば、プログラムメモリと、ワーキングメモリと、マイコンと、を含む。
【００２３】
プログラムメモリは、例えばＥＥＰＲＯＭのような不揮発性メモリである。プログラムメモリには、後述の処理手順を記述したプログラムが予め格納される。
【００２４】
ワーキングメモリは、例えばＳＲＡＭ等の半導体メモリであって、マイコンがプログラムを実行する際に各種演算等に使用される。
【００２５】
マイコンは、ワーキングメモリを用いてプログラムを実行することで、少なくとも、移動推定部と、フィルタ部として機能する。
【００２６】
＜２．状態算出装置の処理＞
まず、移動推定部の処理について、図６のフロー図も参照にしつつ説明する。
移動推定部は、第一センサ部３から得られた物標情報から、反射点の方位およびドップラー速度を取得し、これらに基づき、第一センサ部３の移動速度Ｖｓと移動方位θｓを算出し、車両Ｍの移動速度ＶＶとヨーレートωＶを算出する。
【００２７】
ここで、移動方位θｓは、図２に示すように、第一センサ部３の軸方向（図２中θ＝０°の方位）を基準とする第一センサ部３の移動する方位である。なお、図２の例では、第一センサ部３は、車両Ｍの進行方向において前方左側に設置されている。より具体的には、第一センサ部３は、例えば車両Ｍのバンパー内に設置される。
【００２８】
図３は、移動推定部が取得した方位−ドップラー速度のパワーマップの例示である。
【００２９】
図３において、横軸は方位、縦軸はドップラー速度であり、各丸印が戻り波に対応し、各丸印の大きさがパワー（戻り波強度）を表している。
ここで、図２において、車両Ｍが走行状態で、第一センサ部３の軸方向を基準として移動方向はθｓであり、移動速度がＶｓである。図２の第一センサ部３が測定した静止物体のドップラー速度Ｖは下式（１）で表すことができる。
【００３０】
【数１】
2018055539.tif 000003
【００３１】
また、図２の静止物体Ａは、方位−ドップラー速度のパワーマップ上では、図３に例示したように表される。なお、図２および図３において、θａは、静止物体Ａの第一センサ部３の視野角方位である。
【００３２】
静止物体の視野角方位θとドップラー速度Ｖは観測値でありかつ既知であるので、上式（１）から下式（２）が成り立つ。
【００３３】
【数２】
2018055539.tif 000004
【００３４】
第一センサ部３の視野角内において、互いの異なる二つ方位、即ち、図４に例示するように、視野角方位θ１，θ２に静止物体Ｂ，Ｃが存在し、静止物体Ｂのドップラー速度がＶ１、Ｃのドップラー速度がＶ２であるとすると、次式（３），（４）からなる連立方程式が得られる。
【００３５】
【数３】
2018055539.tif 000005
【００３６】
上式（３），（４）からなる連立方程式に基づいて、二つの静止物体の視野角方位−ドップラー速度から、レーダ移動速度ＶＳと移動方位θＳとは、下式（５）と（６）で算出することができる。
【００３７】
【数４】
2018055539.tif 000006
【００３８】
移動推定部は、上式（５），（６）を用いて、移動方位θＳと移動速度ＶＳとを求める。しかし、第一センサ部３で得られた全ての物標情報に対し、上式（５），（６）を適用して良い訳では無く、静止物体の物標情報に限定適用する必要がある。具体的には、静止物体に関しては、前式（１）が成り立つため、上式（５），（６）により移動方位θＳと移動速度ＶＳとが導出可能であることに着目している。
【００３９】
移動推定部は、第二センサ部５から得られた状態情報（即ち、車速とヨーレート等）から、まず、方位−ドップラー速度のパワーマップにおける理論上の静止物曲線を前式（１）に基づいて算出する。静止物曲線とは、静止物体であれば自車両Ｍが移動していたとしても、理論上、方位−ドップラー速度のパワーマップにのる曲線であり、例えば、図５に示す実線で描いた曲線である。
【００４０】
移動推定部は、算出した静止物曲線を基準に、予め設定された設定値を用いて、第一センサ部３の視野角方位毎に、静止物体のドップラー速度値の範囲を算出する（図６のステップＳ００１）。ドップラー速度範囲は、例えば、図５において、二本の点線の曲線で表される。以下、各視野角方位におけるドップラー速度の上限および下限の間を、静止物体マージンと呼ぶ。
【００４１】
移動推定部は、方位−ドップラー速度のパワーマップにおいて、静止物体マージン内の戻り波強度が所定の閾値以上であるものを静止物体からの戻り波に由来するものと定義し、対応する方位θおよびドップラー速度Ｖを静止物体のサンプルとして集める（図６のステップＳ００３）。
【００４２】
移動推定部は、集めた静止物体サンプルに対し、同一方位θにある静止物体サンプルのドップラー速度Ｖの平均値を重心として算出する（図６のステップＳ００５）。なお、この処理は、静止物体が存在しない方位θではスキップされる。
【００４３】
上記処理で、例えばＮ個の方位θの静止物体の重心が算出されたとする。この場合（図６のステップＳ００７でＹＥＳ）、移動推定部は、得られたＮ個の（方位θ，方位θにおける重心）をペアリングし、Ｎ／２個のサンプルペア｛（θ１，Ｖ１），（θ２，Ｖ２）｝を作成する（図６のステップＳ００９，Ｓ０１１）。
【００４４】
次に、移動推定部は静止物体のサンプルペア毎に、前式（５），（６）を用いて移動方位θＳと移動速度ＶＳを算出する（図６のステップＳ０１３）。
【００４５】
次に、移動推定部が、ステップＳ０１３で算出したθＳ、ＶＳ及び第一センサの車両Ｍにおける設置位置情報を用いて、車両基準点（例えば車両の後輪中心）の速度ＶＶとヨーレートωＶを算出する（図６のステップＳ０１４）。
【００４６】
上記ステップＳ００９〜Ｓ０１４が全てのサンプルペアに対し行われると（図６のステップＳ０１５でＹＥＳ）、Ｎ／２個のωＶとＮ／２個のＶＶが算出されたことになる。
【００４７】
ここで、第一センサ部３が出力する物標情報には、反射点の方位（視野角方位）およびドップラー速度には誤差が存在するため、上記処理で算出されたＮ／２個のωＶとＮ／２個のＶＶには誤差が重畳されている。その誤差の影響を軽減するため、移動推定部は、算出結果であるＮ／２個のωＶとＮ／２個のＶＶに対し刈り込み平均処理を行い、それぞれの平均値を、ヨーレートωＶおよび移動速度ＶＶとして出力する（図６のステップＳ０１７）。
【００４８】
具体的には、移動推定部は、得られたＮ／２個のωＶを昇順または降順でソーティング処理し、上位および下位それぞれの所定の割合（例えば２０％）を控除した後、残りの中位（例えば６０％）のωＶから平均値を求めて、ヨーレートωＶとして出力する。移動推定部はさらに、中位に分布する複数のωＶの誤差分散ＰωＶも算出して出力する。
【００４９】
同様に、移動推定部は、得られたＮ／２個のＶＶを昇順または降順でソーティング処理し、上位および下位それぞれの所定の割合（例えば２０％）を控除した後、残りの中位（例えば６０％）のＶＶから平均値を求めて、移動速度ＶＶとして出力する。移動推定部はさらに、中位に分布する複数のＶＶの誤差分散ＰＶＶも算出して出力する。
【００５０】
次に、フィルタ部の処理について説明する。
まず、マイコンは、第二センサ部５から出力される状態情報（本開示では、車速とヨーレート等）から、車速の誤差分散およびヨーレート等の誤差分散を算出する。なお、車速の誤差分散およびヨーレート等の誤差分散は、第二センサ部５の特性であるため、必ずしもマイコンで算出される必要はない。この場合、車速の誤差分散およびヨーレート等の誤差分散はマイコンに予め保持される。
【００５１】
フィルタ部は、第二センサ部５から出力される車速とヨーレート等と、それらの誤差分散と、移動推定部から出力されるヨーレートωＶおよび移動速度ＶＶと、それらの誤差分散ＰωＶ，ＰＶＶと、を入力とする。フィルタ部は、入力信号に対してベイズフィルタ処理を適用する。ベイズフィルタ処理として、本開示ではカルマンフィルタを用いた処理を例示的に説明する。
【００５２】
ここで、図７は、フィルタ部の詳細な構成を示すブロック図である。
図７において、フィルタ部は、車速選択部１５９１と、ヨーレート選択部１５９３、観測更新部１５９５、車速予測部１５９７、ヨーレート予測部１５９９、車速分散値選択部１５１０１、ヨーレート分散値選択部１５１０３で構成される。
【００５３】
車速選択部１５９１は、第二センサ部５および移動推定部から入力される車速および移動速度ＶＶを一系統にまとめて、車速として出力する。第二センサ部５からは、例えば数十ミリ秒の間隔で車速が入力される。なお、第二センサ部５および移動推定部からの入力タイミングは異なっていても良い。また、もし第二センサ部５および移動推定部からの入力タイミングは実質的に同じであれば、車速選択部１５９１は、車速および移動速度ＶＶのいずれか一方を先に出力し、いずれか他方を後に出力する。
【００５４】
ヨーレート選択部１５９３は、第二センサ部５および移動推定部から入力されるヨーレート等およびヨーレートωＶを一系統にまとめて、ヨーレート等として出力する。第二センサ部５からは、例えば数十ミリ間隔でヨーレート等が入力される。上記と同様、第二センサ部５および移動推定部からの入力タイミングは異なっていても良い。また、もし第二センサ部５および移動推定部からの入力タイミングは実質的に同じであれば、ヨーレート選択部１５９３は、ヨーレート等とωVのいずれか一方を先に出力し、いずれか他方を後に出力する。
【００５５】
車速分散値選択部１５１０１は、第二センサ部５から入力される車速の誤差分散と、移動推定部から入力される誤差分散ＰＶＶとを一系統にまとめて、車速の誤差分散として出力する。第二センサ部５からは、車速とその誤差分散とは同期して、車速選択部１５９１および車速分散値選択部１５１０１に入力される。このため、車速分散値選択部１５１０１は、車速選択部１５９１と同様に、車速の誤差分散と、誤差分散ＰＶVを選択する。なお、第二センサ部５から車速の誤差分散の入力が無いか、移動推定部から誤差分散ＰＶVの入力が無い場合には、予め定められた固定的な誤差分散が車速分散値選択部１５１０１に与えられても良い。この場合、固定的な誤差分散は予め測定などにより、第一センサ部３および第二センサ部５が持つ誤差分散値を測定して車速分散値選択部１５１０１に与えておく。
【００５６】
ヨーレート分散値選択部１５１０３は、第二センサ部５から入力されるヨーレート等の誤差分散と、移動推定部から入力される誤差分散ＰωVとを一系統にまとめて、ヨーレート等の誤差分散として出力する。第二センサ部５からは、ヨーレート等とその誤差分散とは同期して、ヨーレート選択部１５９３およびヨーレート分散値選択部１５１０３に入力される。このため、ヨーレート分散値選択部１５１０３は、車速分散値選択部１５１０１と同様に、ヨーレート等の誤差分散と、誤差分散ＰωVを選択する。なお、第二センサ部５からヨーレート等の誤差分散の入力が無いか、移動推定部から誤差分散ＰωVの入力が無い場合には、予め定められた固定的な誤差分散がヨーレート分散値選択部１５１０３に与えられても良い。この場合、固定的な誤差分散は予め測定などにより、第一センサ部３および第二センサ部５が持つ誤差分散値を測定してヨーレート分散値選択部１５１０３に与えておく。
【００５７】
観測更新部１５９５は、車速選択部１５９１の出力（車速および移動速度ＶＳを）と、ヨーレート選択部１５９３の出力（ヨーレート等およびωV）と、車速予測部１５９７が出力する車速予測値と、ヨーレート予測部１５９９が出力するヨーレート予測値を入力とする。観測更新部１５９５は、カルマンフィルタにおける観測更新処理を行う。
【００５８】
ここで、カルマンフィルタについて説明する。カルマンフィルタは一般的な線形カルマンフィルタを使用する。カルマンフィルタは、システムの状態遷移を表す状態方程式と、センサの観測モデルを表す観測方程式とで推定するシステムをモデル化する。
【００５９】
【数５】
2018055539.tif 000007
【００６０】
上式（７）および（８）は、カルマンフィルタの状態方程式および観測方程式を示している。ここで、Ｆｋは、システム状態の時間遷移モデル、Ｇｋは、システムノイズの時間遷移モデル、ｗｋは平均ゼロで共分散行列Ｑｋのシステムノイズ、Ｈｋは観測モデル、ｖｋは平均ゼロで共分散行列Ｒｋの観測ノイズである。
【００６１】
以下に示すカルマンフィルタのアルゴリズムを用いて、上記で定義したシステムモデルｘｋを推定する。カルマンフィルタによる推定は予測ステップと観測更新ステップで構成される。
【００６２】
【数６】
2018055539.tif 000008
【００６３】
上式（９）および（１０）は、カルマンフィルタの予測ステップの演算を示している。特に、上式（９）は予測推定値を、上式（１０）は予測誤差分散行列の演算を示している。予測推定演算は、前回の推定値ｘ＾ｋ−１｜ｋ−１から、時間遷移モデルＦｋを用いて次の状態ｘ＾ｋ｜ｋ−１を予測する。予測誤差共分散行列の演算では、前回の共分散行列Ｐｋ−１｜ｋ−１とその時間遷移モデルＦｋで共分散行列の状態遷移を計算し、システムノイズ共分散行列Ｑｋとその時間遷移モデルＧｋでシステムノイズの増加分を計算し、それらを加算することにより予測共分散行列Ｐｋ｜ｋ−１を計算する。上式（９）の予測推定値と、上式（１０）の予測誤差共分散行列を予測ステップの出力とする。
【００６４】
【数７】
2018055539.tif 000009
【００６５】
上式（１１）〜（１５）はカルマンフィルタの観測更新ステップを示している。上式（４）の推定値ｘ＾ｋ｜ｋと、上式（５）の共分散行列推定値Ｐｋ｜ｋを計算するために、上式（１１）〜（１３）が算出される。観測更新ステップの推定値ｘ＾ｋ｜ｋは、上式（１３）で算出されるカルマンゲインと、上式（１）で算出される観測残差とを用いて計算される。観測残差は、予測値を観測値の空間へ変換し、観測値との残差から算出される。上式（１２）で算出される観測残差の共分散は、測定値の共分散と予測値の共分散から求められる。上式（１３）で算出されるカルマンゲインは、予測値の共分散と観測残差の共分散の比で計算される。
【００６６】
このように求めた（１１）〜（１３）の値を用いて、推定値ｘ＾ｋ｜ｋと、共分散行列推定値Ｐｋ｜ｋとが算出され、観測ステップの出力およびカルマンフィルタの出力とされる。
【００６７】
次に、カルマンフィルタで用いる変数ｘｋ、Ｆｋ、Ｇｋについて説明する。
変数ｘｋは、推定するシステムを表す。本開示の推定する状態は、速度ｖとヨーレートωである。よって、ｘｋ＝（ｖｋ,ωｋ）Ｔとする。
【００６８】
変数Ｆｋは、状態ｘｋの時間遷移を表し、下式（１６）の通りである。
変数Ｗｋは、システムノイズを表し、下式（１７）の通りである。なお、下式（１７）において、ａｖは車両Ｍの進行方向における加速度であり、ａωは、車両Ｍの旋回方向への加速度である。
また、変数Ｇｋは、システムノイズの時間遷移を表し、下式（１８）の通りである。なお、下式（１８）において、Δｔは時刻ｋと１クロック分の時刻だけ前のｋ−１との差を表す。
【００６９】
【数８】
2018055539.tif 000010
【００７０】
Ｚｋは、測定値の（ｖｋ，ωｋ）Ｔとする。Ｒｋは、測定値の誤差共分散行列である。それぞれには、第一センサ部３および第二センサ部５で測定したデータを用いる。これらをカルマンフィルタのアルゴリズムに代入して、ｘｋ（つまり、速度ｖとヨーレートω）を推定する。
【００７１】
ここで、図７を参照する。観測更新部１５９５の処理は、上記カルマンフィルタの説明における観測更新ステップを実行する。観測更新部１５９５は、車速とヨーレートとの推定結果を出力する。なお、観測更新部１５９５は必ずしも観測値の入力が無い場合、例えば下記のような処理を行う。すなわち、車速選択部１５９１から出力される車速や、ヨーレート選択部１５９３から出力されるヨーレート等が入力されない場合でも、観測更新部１５９５は、車速とヨーレート等の推定結果を出力する。この場合、観測更新部１５９５は、カルマンフィルタの観測更新ステップを実行せず、観測更新部１５９５へ入力される車速予測値とヨーレート予測値を推定結果として出力する。
【００７２】
車速予測部１５９７は、観測更新部１５９５からの車速の推定値を入力とする。車速予測部１５９７は、入力データに対して１クロック分の時刻だけ進んだｋ＋１の時間の車速を予測して出力する。予測処理は、上記カルマンフィルタの予測ステップに相当する。
【００７３】
ヨーレート予測部１５９９は、観測更新部１５９５からのヨーレート等の推定値を入力とする。ヨーレート予測部１５９９は、入力データに対して１クロック分の時刻だけ進んだｋ＋１の時間のヨーレートを予測して出力する。予測処理は上記カルマンフィルタの予測ステップに相当する。
【００７４】
図８は、第二センサ部５からフィルタ部への車速とヨーレート等と、移動推定部からフィルタ部への移動速度ＶＶおよびヨーレートωＶとの入力タイミングを示している。
【００７５】
上記から明らかなように、車両Ｍの速度ｖに関しては、第二センサ部５が出力する車速と、移動推定部から出力される移動速度ＶＶの二種類が存在する。ヨーレートωに関しても、第二センサ部５が出力するヨーレート等と、移動推定部から出力されるヨーレートωＶの二種類が存在する。
【００７６】
第二センサ部５が車速を出力する間隔と、移動速度ＶＶが出力される間隔とは異なることが多い。ヨーレート等およびωＶについても同様である。さらに、移動推定部からの移動速度ＶＶおよびヨーレートωＶに関しては一定間隔で出力される期間もあれば、そうでない期間も存在する。
【００７７】
フィルタ部は、車速ｖおよびヨーレートωを入力順に処理する。図８において、第二センサ部５からの車速が最初にフィルタ部に入力される（参照符号３０１を参照）。その後、移動速度ＶＶとヨーレートωＶが同時にフィルタ部に入力され（参照符号３０３，３０５を参照）、その後、第二センサ部５からのヨーレート等が入力され（参照符号３０７を参照）、さらに、移動速度ＶＶとヨーレートωＶが同時にフィルタ部に入力される（参照符号３０９，３０１１を参照）。その後、第二センサ部５からの車速およびヨーレート等が順番に複数回入力された後（参照符号３０１３，３０１５，３０１７，３０１９を参照）、移動速度ＶＶとヨーレートωＶが同時にフィルタ部に入力される（参照符号３０２１，３０２３を参照）。
【００７８】
＜３．状態算出装置の作用・効果＞
フィルタ部は、入力データを、入力順に処理する。このとき、誤差の共分散行列推定値Ｐｘ｜ｘにおける車速の誤差分散値の変化を図９に示す。
【００７９】
図９において、第二センサ部５からの車速と、移動推定部からの移動速度ＶＶのフィルタ部への入力タイミングは、図８に示した通りである。それ故、図９において、図８に示す車速等と対応するものには同一参照符号を付している。また、本開示では、第二センサ部５が出力する車速よりも、移動推定部により得られた移動速度ＶＶの方が高精度とする。
【００８０】
車速の誤差分散値４０１は、第二センサ部５からの車速のみがフィルタ部へ入力された場合における車速の誤差分散値の経時変化を示している。第二センサ部５からの車速３０１，３０１３，３０１７が入力されるたびに誤差の分散値が小さくなる。従って、第二センサ部５のみからの車速の入力がある程度連続すると、誤差の分散値はある程度の値に収束する。ここで、収束する値は第二センサ部５の精度に依存する。
【００８１】
上記に対し、車速の誤差分散値４０３は、第二センサ部５からの車速に加え、移動推定部の移動速度ＶＶがフィルタ部に入力された場合の車速の誤差分散値の経時変化を示している。第二センサ部５からの車速の入力開始後、誤差分散値４０３は、第二センサ部５だけの場合の誤差分散値４０１と比較して早く小さくなる。これは、移動推定部の測定精度が第二センサ部５よりも良いことと、データ入力間隔が短いことに起因する。第二センサ部５と移動推定部の両方からデータ入力がある時間区間の誤差分散値は、第二センサ部５のみの誤差分散値４０１に比べて小さくなる。このように、第二センサ部５と、移動推定部とが出力する車速を両方用いてカルマンフィルタ処理を実施することにより、車速の推定値の精度が改善できる。
【００８２】
上記の説明と同様に、ヨーレート等の場合も二種類のセンサからの測定結果をカルマンフィルタ処理することにより、車速と同様に、ヨーレート等の推定値の精度を向上させることができる。
【００８３】
さらに、本開示では車速およびヨーレート観測値とその分散値を入力として推定処理カルマンフィルタを実施した。これにより、分散値を固定値で入力する場合と比較して車速とヨーレートの推定値の精度を向上させることができる。
【００８４】
また、上記の通り、二種類の車速の測定結果をカルマンフィルタで処理することにより、推定精度が向上する概念を以下に説明する。一方のセンサの出力する測定値が平均ｘ１、誤差分散Ｐ１であるとする。他方のセンサの出力する測定値が平均ｘ２、誤差分散Ｐ２であるとする。これら誤差分散の異なる二種類の測定結果を重み付け平均することを考える。
【００８５】
誤差分散Ｐ１とＰ２を用いた重み付け平均結果をｘ、その誤差分散をＰとすると、ｘ，Ｐは、以下のように計算できる。
【数９】
2018055539.tif 000011
【００８６】
したがって、重み付け平均後の誤差分散Ｐは入力値の誤差分散Ｐ1およびＰ2よりも小さくなる。このように、第二センサ部５から得られる車速およびヨーレート等と、第一センサ部３の物標情報に基づき求めた移動速度ＶＶおよびヨーレートωＶを用いて、カルマンフィルタのような重み付け平均処理を行うことにより、例えば、車両Ｍがスリップ等による影響で第二センサ部５が実際の車両Ｍの挙動と異なる検出結果を出力した場合でも、本開示に係る状態算出装置１によれば、より高精度に車速およびヨーレート等を出力することが可能となる。
【００８７】
＜４．実施形態の付記＞
なお、上記の重みづけ平均の考え方でヨーレートの誤差分散も二種類のセンサの測定値を用いることにより、精度を改善できる。
【００８８】
また、上記実施形態の説明では、移動体として車両Ｍが例示された。しかし、移動体としては、自動二輪や産業用ロボットでも構わない。
【００８９】
また、状態算出装置１において、移動推定部およびフィルタ部はコンピュータプログラムで実現されても良い。コンピュータプログラムは、ＤＶＤ等のような配布媒体に格納されて提供されても良いし、ネットワークを介してダウンロード可能にネットワーク上のサーバ装置に格納されても良い。
【００９０】
＜５．変形例＞
次に、図１０を参照して、上記実施形態の変形例である状態算出装置１ａについて説明する。
【００９１】
図１０において、状態算出装置１ａは、前述の状態算出装置１と比較すると、上記プログラムとは別のプログラムを実行する点で相違する。それ以外に、両状態算出装置１，１ａの間に相違点は無い。それ故、図１０において、図１に示す構成に相当するものには同一参照符号を付け、それぞれの説明を省略する。
【００９２】
なお、マイコンは、別のプログラムを実行することで、移動推定部およびフィルタ部に加え、物体追従部、物体識別部およびアプリケーション部として機能する。
【００９３】
物体追従部は、第一センサ部３からの物標情報と、フィルタ部から出力された車速およびヨーレート等とに基づいて、物標の追従を行う。物標の追従とは、複数フレームの物標情報に亘り、第一センサ部３で観測された物標の位置、距離、移動速度と移動方向等の追跡を行って追従情報を生成することを意味する。追従を行う際に物標の状態が推定されるが、フィルタ部からの車速およびヨーレート等の測定精度が追従性能に大きく影響する。そのため、本開示におけるフィルタ部からの車速およびヨーレート等は物標の追従性能向上に大きく貢献する。物体追従部は、推定された全ての物標の追従情報を物体識別部およびアプリケーション部に出力する。
【００９４】
物体識別部は、物体追従部から出力された追従情報に基づいて、物体の識別を行う。物体の識別とは、追従した物標が例えば、自家用車なのか、トラックなどの大型車なのか、人なのか、バイクなのか、自転車なのか、猫や犬などの動物なのか、ビルや橋などの建築物なのかなどを判定することである。物体識別部は、識別結果をアプリケーション部に出力する。
【００９５】
アプリケーション部は、物体追従部から出力された追従情報と、物体識別部の識別結果とに基づいて、運転を支援するさまざまな機能を実現する。機能の種類は、多く存在するが、以下に例示列挙する。
【００９６】
ＡＣＣ（Ａｄａｐｔｉｖｅ  Ｃｒｕｉｓｅ  Ｃｏｎｔｒｏｌ）：自車両の前方を走行する車両との車間距離を一定に保つため自動的にアクセル、ブレーキの制御をおこなう。また必要に応じてドライバーへの警告を行う。
【００９７】
衝突被害軽減ブレーキ：前方の障害物との衝突を予想して警報し、衝突被害を軽減するために自車両の制動を制御する。
【００９８】
後側方車両検知警報：走行中の進路変更時、斜め後方に走行車両がいる場合に警報して確認を促す。
【００９９】
自動合流：高速道路の合流時に、合流レーンの他車の状況を判断し自動的に合流する。
【０１００】
＜６．変形例の付記＞
上記変形例の説明では、物体追従部、物体識別部およびアプリケーション部が、移動推定部およびフィルタ部と同一のＥＣＵ１ａに実装されるとして説明した。しかし、これに限らず、物体追従部、物体識別部およびアプリケーション部は、移動推定部およびフィルタ部と異なるＥＣＵに実装されても良い。 
【産業上の利用可能性】
【０１０１】
本開示に係る状態算出装置、状態算出方法、プログラムおよびこれを記録した記録媒体は、車両の状態を高精度に算出可能であり、車載用途に好適である。
【符号の説明】
【０１０２】
１，１ａ  状態算出装置
１１  入力部
１５  制御部
３  第一センサ部
５  第二センサ部
【図１】
2018055539.tif 000012
【図２】
2018055539.tif 000013
【図３】
2018055539.tif 000014
【図４】
2018055539.tif 000015
【図５】
2018055539.tif 000016
【図６】
2018055539.tif 000017
【図７】
2018055539.tif 000018
【図８】
2018055539.tif 000019
【図９】
2018055539.tif 000020
【図１０】
2018055539.tif 000021