(57)【特許請求の範囲】
【請求項１】
目標の観測データを取得するセンサ部と、
前記目標が移動可能な経路の形状を表す経路データを格納する経路データ格納部と、
前記目標の運動諸元の予測値及び予測誤差の前記経路に平行な成分と垂直な成分をそれぞれ算出する経路成分予測部と、
前記経路成分予測部により算出された運動諸元の予測値及び予測誤差を、前記観測データを表す第１の座標系と同じ座標系に変換する第１座標変換部と、
前記センサ部で取得された観測データと、前記経路データ格納部に格納された経路データと、前記第１座標変換部で変換された運動諸元の予測値及び予測誤差を用いて、前記目標の運動諸元の推定値及び推定誤差を算出するフィルタリング部と、
前記フィルタリング部により算出された前記目標の運動諸元の推定値及び推定誤差を、前記経路に平行な成分と垂直な成分を座標軸とする第２の座標系に変換する第２座標変換部と、
前記第２座標変換部によって変換された前記目標の運動諸元の推定値及び推定誤差を用いて、前記目標の運動状態を判定する状態判定部と、
を備えたことを特徴とする運動状態判定装置。
【請求項２】
前記経路成分予測部は、
前記目標の運動諸元の予測値及び予測誤差の前記経路に平行な成分を算出する経路平行成分予測部と、
前記目標の運動諸元の予測値及び予測誤差の前記経路に垂直な成分を算出する経路垂直成分予測部
を備えたことを特徴とする請求項１に記載の運動状態判定装置。
【請求項３】
前記経路成分予測部は、前記経路に対して平行方向と垂直方向で異なる運動モデルを設定して、前記経路に平行な成分と垂直な成分をそれぞれ算出する
ことを特徴とする請求項１又は請求項２に記載の運動状態判定装置。
【請求項４】
前記経路成分予測部は、Ｎ組の前記目標の運動諸元の予測値及び予測誤差に対してそれぞれ前記経路に平行な成分と垂直な成分を算出し、
前記第１座標変換部は、前記経路成分予測部により算出されたＮ組の運動諸元の予測値及び予測誤差を、前記第１の座標系に変換し、
前記フィルタリング部は、前記第１座標変換部で変換されたＮ組の運動諸元の予測値及び予測誤差のそれぞれの組に対して、前記目標の運動諸元の推定値及び推定誤差を算出し、
前記第２座標変換部は、前記フィルタリング部により算出されたＮ組の前記目標の運動諸元の推定値及び推定誤差をそれぞれ前記第２の座標系に変換し、
前記状態判定部は、前記第２座標変換部により変換されたＮ組の前記目標の運動諸元の推定値及び推定誤差を用いて、前記目標の運動状態を判定する
ことを特徴とする請求項１から３のいずれか１項に記載の運動状態判定装置。
【請求項５】
前記経路成分予測部は、Ｎ組の前記目標の運動諸元の予測値及び予測誤差に対してそれぞれ異なる運動モデルを設定して、前記経路に平行な成分と垂直な成分を算出する
ことを特徴とする請求項１又は請求項２に記載の運動状態判定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、目標の運動状態を判定する運動状態判定装置に関する。
【背景技術】
【０００２】
従来から、目標の運動状態を観測データから分類する方法する技術が知られている。ここで目標とは、車両、船舶、航空機、ロボット、人、自転車などの移動可能な物体と定義する。例えば特許文献１では、車両の位置を表す観測データから、車両の横方向の運動の向きが変化する点を抽出し、変化点間の距離が一定値以上ならば、その車両の運動状態は蛇行であると判定する装置が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１４−６５７６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
目標の観測データに誤差が含まれる場合、この観測誤差によって、運動状態の判定の誤りが頻発するという問題がある。特許文献１に記載された従来の技術では、観測誤差を含む観測データから車両の蛇行を判定するために、横位置の変化点の距離が一定値未満の場合、すなわち車両の横位置が細かく変化する場合は、その運動方向の変化は観測誤差によるものであるとみなし、その車両は蛇行していないと判定する。しかし一般に、観測を行うセンサから目標までの距離が遠いほど位置の観測誤差は大きくなる。そのため、特許文献１のように目標の位置のみによってのみ蛇行を判定する技術では、センサから遠い目標ほど、観測誤差を蛇行と判定する誤りが生じやすくなる。また、このような判定の誤りを減らすために、観測誤差が大きいことを前提とした場合、センサから近い目標ほど、目標の微小な蛇行を観測誤差と判定する誤りが生じやすくなる。【０００５】
本発明は上記の課題を解決するものであり、観測データに観測誤差が含まれる場合、特にセンサと目標との距離が変動し観測誤差が一定ではない環境においても、目標の運動状態に関する判定の誤り頻度を減らすことを目的とする。
【課題を解決するための手段】
【０００６】
この発明に係る運動状態判定装置は、目標の観測データを取得するセンサ部と、前記目標が移動可能な経路の形状を表す経路データを格納する経路データ格納部と、前記目標の運動諸元の予測値及び予測誤差の前記経路に平行な成分と垂直な成分をそれぞれ算出する経路成分予測部と、前記経路成分予測部により算出された運動諸元の予測値及び予測誤差を、前記観測データを表す第１の座標系と同じ座標系に変換する第１座標変換部と、前記センサ部で取得された観測データと、前記経路データ格納部に格納された経路データと、前記第１座標変換部で変換された運動諸元の予測値及び予測誤差を用いて、前記第１の座標系における前記目標の運動諸元の推定値及び推定誤差を算出するフィルタリング部と、前記フィルタリング部により算出された前記目標の運動諸元の推定値及び推定誤差を、前記経路に平行な成分と垂直な成分を座標軸とする第２の座標系に変換する第２座標変換部と、前記第２座標変換部によって変換された前記目標の運動諸元の推定値及び推定誤差を用いて、前記目標の運動状態を判定する状態判定部と、を備えたことを特徴とする。
【発明の効果】
【０００７】
本発明によれば、観測誤差による観測データ上の目標の位置変動によって目標の運動状態の判定を誤る頻度を減らすことができる。
【図面の簡単な説明】
【０００８】
【図１】実施の形態１に係る運動状態判定装置１００の構成を示すブロック図。
【図２】実施の形態１において対象車両２１に関する経路平行方向の位置と、経路垂直方向の位置を表した概念図。
【図３】実施の形態１において経路座標と観測座標との間の座標変換を表した概念図。
【図４】実施の形態１に係る運動状態判定装置１００のハードウェア構成を示すブロック図。
【図５】実施の形態１に係る運動状態判定装置１００の動作を示すフローチャート。
【図６】対象車両２１の観測座標における運動諸元ベクトルの例を示した概念図。
【図７】対象車両２１の経路座標における運動諸元ベクトルの例を示した概念図。
【図８】実施の形態２に係る運動状態判定装置１００の構成を示すブロック図。
【図９】実施の形態２に係る運動状態判定装置１００の動作を示すフローチャート。
【発明を実施するための形態】
【０００９】
実施の形態１．
以下、この発明の実施の形態について説明する。
【００１０】
図１は、実施の形態１に係る運動状態判定装置１００の構成を示すブロック図である。この実施の形態では特に、目標の種類を自動車とし、目標の運動状態は「車両が蛇行している」「車両は蛇行していない」の２種類とする。また、運動状態を判定する対象の自動車は、観測を行うセンサを搭載している自動車（自車）から離れた位置を走行する自動車とする。なお、運動状態を判定する対象の自動車は自車であってもよい。以降、運動状態を判定する対象の目標を「対象車両」と記載し、対象車両の運動状態は「蛇行あり」「蛇行なし」の２種類である場合について説明する。
【００１１】
運動状態判定装置１００は、図１に示すように、経路平行成分予測部１０、経路垂直成分予測部１１、経路データ格納部１２、第１座標変換部１３、センサ部１４、フィルタリング部１５、第２座標変換部１６、状態判定部１７を備える。ここで、経路平行成分予測部１０、経路垂直成分予測部１１はまとめて経路成分予測部１として扱える。運動状態判定装置１００は、センサから取得した観測データと、経路データ格納部１２に保存されている経路データを基に、対象車両２１の蛇行有無を判定する。
【００１２】
ここで、観測データとは、対象車両２１の運動諸元（位置、速度およびそれらの時間微分量）を示した情報と定義する。以降では、観測データが対象車両２１の位置を表すデータである場合について説明する。また、経路データとは、対象車両２１の典型的な移動経路の形状を示した情報と定義する。以降では、経路データが、対象車両２１が走行している車線の形状を表すデータである場合について説明する。特に、車線の中央を通る線を点の集合として近似した場合における、各々の中央点の位置データを以降では経路データと呼ぶ。なお、目標は自動車の他に、船舶、航空機、ロボット、人、自転車などであってもよい。例えば目標が船舶の場合、観測データは船舶の位置を表す通信信号（自動船舶識別装置の信号など）とし、経路データは典型的な航路としてもよい。また例えば目標が人の場合、観測データは人を撮影した映像とし、経路データは歩道の形状としてもよい。
【００１３】
続いて、運動状態判定装置１００の説明における用語として、「経路平行方向」と「経路垂直方向」を定義する。図２は、実施の形態１に係る運動状態判定装置１００における経路平行方向と経路垂直方向の一例を表す概念図である。
【００１４】
まず経路平行方向の軸は、経路データである車線の中央点を前後に繋いだ線分の集合と定義する。そして、ある車線中央点の原点から、対象車両２１との距離が最小となる経路平行方向の軸上の点（経路座標軸交点）までの距離を、対象車両２１に関する経路平行方向の位置と定義する。また、この経路平行方向の位置およびその位置を時間微分した運動諸元を「運動諸元の経路平行方向成分」と呼ぶ。
【００１５】
続いて、経路垂直方向の軸は、経路座標軸交点における経路平行方向の軸に対し垂直な直線と定義する。そして、経路座標軸交点から、経路垂直方向の軸と対象車両２１の位置が交わる点までの距離を、対象車両２１に関する経路垂直方向の位置と定義する。また、この経路垂直方向の位置およびその位置を時間微分した運動諸元を「運動諸元の経路垂直方向成分」と呼ぶ。
【００１６】
上記の定義を踏まえ、運動状態判定装置１００の動作を図３を用いて説明する。図３は、実施の形態１に係る運動状態判定装置１００における、動作の一例を表す概念図である。図３左側は、センサを搭載した自車２０を基準とした座標系において、対象車両２１が蛇行している場合、蛇行に伴う観測データ２３が得られる様子を表す概念図である。図３左側において自車２０の進行方向に関する位置を「縦方向位置」、縦方向に対し垂直な方向の位置を「横方向位置」とし、観測データ２３はこれら縦方向、横方向の位置として表されるとする。以降では、この観測データ２３における座標系を「観測座標」と呼ぶ。
【００１７】
一方、図３右側は、左側と同様に蛇行する対象車両２１の推定位置の時間変化を、経路平行方向および経路垂直方向において表した場合の概念図である。以降では、経路平行方向と経路垂直方向で運動諸元を表す座標系を「経路座標」と呼ぶ。また、図３中央の矢印で示すとおり、観測座標から経路座標への座標変換は「第１座標変換部１３」で行い、経路座標から観測座標への変換を「第２座標変換部１６」で行う。運動状態判定装置１００は、経路座標上において次の時刻フレームにおける対象車両２１の運動諸元の予測を行い、予測値を観測座標上へ変換したのちに、観測座標上において観測データ２３に基づく対象車両２１の運動諸元の推定（フィルタリング）を行う。そして再び座標変化によって推定値を経路座標へ変換し、経路座標上の推定値を基に対象車両２１の蛇行有無を判定する。
【００１８】
このような動作により、例えば図３に示すように湾曲した経路に沿って対象車両２１が蛇行している場合においても、経路座標上では経路に直交した方向の蛇行が抽出できる。また、特に、運動諸元と併せて、推定された運動諸元の誤差（推定誤差）も座標変換し、この推定誤差を用いて蛇行有無を判定する点が、運動状態推定装置の特徴である。
【００１９】
以下、図１における各機能ブロック間の関係を示す。なお、以降では、センサが観測データを出力する時間の区切りを「時刻フレーム」と記載する。また処理を実行する時刻フレームを「現時刻」と記載し、現時刻の１時刻フレーム前を「前時刻」と記載する。
【００２０】
経路成分予測部１における経路平行成分予測部１０は、第２座標変換部１６から前時刻における対象車両２１の運動諸元の経路平行成分推定値と、経路平行成分の推定誤差を受け取り、現時刻における対象車両２１の運動諸元の経路平行成分予測値と、経路平行成分の予測誤差を第１座標変換部１３へ送る。
【００２１】
経路成分予測部１における経路垂直成分予測部１１は、第２座標変換部１６から前時刻における対象車両２１の運動諸元の経路垂直成分推定値と、経路垂直成分の推定誤差を受け取り、現時刻における対象車両２１の運動諸元の経路垂直成分予測値と、経路垂直成分の予測誤差を第１座標変換部１３へ送る。
【００２２】
経路データ格納部１２は、経路データを第１座標変換部１３および第２座標変換部１６が実行される際に、経路データを第１座標変換部１３および第２座標変換部１６へ送る。なお、経路データは予め保存されたものでも、処理に用いる際に、その都度、通信手段などによって取得して保存してもよいとする。経路データを取得する通信手段としては、例えば測位衛星との通信などの既存の技術を用いてもよい。
【００２３】
第１座標変換部１３は、経路平行成分予測部１０から現時刻における対象車両２１の運動諸元の経路平行成分予測値と、経路平行成分の予測誤差を受け取り、経路垂直成分予測部１１から現時刻における対象車両２１の運動諸元の経路垂直成分予測値と、経路垂直成分の予測誤差を受け取り、経路データ格納部１２から経路データを受け取る。そして、現時刻における対象車両２１の観測座標上運動諸元の予測値と、観測座標上運動諸元の予測誤差をフィルタリング部１５へ送る。
【００２４】
センサ部１４は、対象車両２１の位置を観測するための機器から構成される。例えば、単眼カメラ、ステレオカメラ、ミリ波レーダ、レーザセンサ、無線信号受信器などの既存の機器である。センサ部１４は現時刻における対象車両２１の観測データを、フィルタリング部１５へ送る。なお、観測機器が取得した画像や測距値などから、観測データ（対象車両２１の位置など）を取得する手段は、例えば映像から車両の特徴的な形状を抽出するアルゴリズムなどの、観測機器ごとの既存技術を用いる。
【００２５】
フィルタリング部１５は、第１座標変換部１３から現時刻フレームにおける対象車両２１の観測座標上運動諸元の予測値と、観測座標上運動諸元の予測誤差を受け取り、センサ部１４から現時刻における対象車両２１の観測データを受け取る。そして、現時刻における対象車両２１の観測座標上運動諸元の推定値と、観測座標上運動諸元の推定誤差を第２変換部へ送る。
【００２６】
第２座標変換部１６は、フィルタリング部１５から現時刻における対象車両２１の観測座標上運動諸元の推定値と、観測座標上運動諸元の推定誤差を受け取り、経路データ格納部１２から経路データを受け取る。そして、状態判定部１７へ現時刻における対象車両２１の経路座標上運動諸元の推定値と、経路座標上運動諸元の推定誤差を送る。また次の時刻フレームにおいて、経路平行成分予測部１０に前時刻における対象車両２１の運動諸元の経路平行成分推定値と、経路平行成分の推定誤差を送り、経路垂直成分予測部１１に前時刻における対象車両２１の運動諸元の経路垂直成分推定値と、経路垂直成分の推定誤差を送る。
【００２７】
状態判定部１７は、第２座標変換部１６から現時刻における対象車両２１の経路座標上運動諸元の推定値と、経路座標上運動諸元の推定誤差を受け取り、現時刻における対象車両２１の蛇行の有無を運動状態判定装置１００の出力として出力する。
【００２８】
図４は、実施の形態１に係るハードウェア構成の一例を示す図である。各ハードウェアはバス等の有線回路またはネットワークによって互いに繋がっている。運動状態判定装置１００における、経路平行成分予測部１０、経路垂直成分予測部１１、経路データ格納部１２、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６、状態判定部１７は記録装置４２に格納されており、経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６、状態判定部１７は演算装置４１によって実行される。
【００２９】
また、経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６、状態判定部１７は、実行中に記録装置４２へ入出力値および処理中間データの書き込み、読み出し、削除を行う。演算装置４１はＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）、ＥＣＵ（Ｅｌｅｃｔｒｏｎｉｃ  Ｃｏｎｔｒｏｌ  Ｕｎｉｔ）またはＧＰＵ（Ｇｒａｐｈｉｃｓ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）等である。記録装置４２はＨＤＤ（Ｈａｒｄ  Ｄｉｓｃｋ  Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ  Ｓｔａｔｅ  Ｄｒｉｖｅ）、ＤＲＡＭ（Ｄｙｎａｍｉｃ  Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）、またはフラッシュメモリ等である。出力インターフェース４３はＤＶＩ（Ｄｉｇｉｔａｌ  Ｖｉｓｕａｌ  Ｉｎｔｅｒｆａｃｅ、登録商標）、ＨＤＭＩ（Ｈｉｇｈ−Ｄｅｆｉｎｉｔｉｏｎ  Ｍｕｌｔｉｍｅｄｉａ  Ｉｎｔｅｒｆａｃｅ、登録商標）、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ  Ｓｅｒｉａｌ  Ｂｕｓ）、イーサネット（登録商標）、ＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ  Ａｒｅａ  Ｎｅｔｗｏｒｋ）バス等である。
【００３０】
次に、図５のフローチャートを用いて、実施の形態１に係る運動状態判定装置１００の動作の詳細を説明する。図５は、経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、センサ部１４、フィルタリング部１５、第２座標変換部１６、状態判定部１７の、現時刻における動作を示したフローチャートである。
【００３１】
動作の説明の準備として、観測データを表す位置ベクトル、経路データを表す位置ベクトル、対象車両２１の運動諸元の推定値を表すベクトル、運動諸元の推定誤差を表す行列を定義する。観測データは、ここでは前記のとおり、対象車両２１の位置を表すデータとする。以降では、あるｋ番目の時刻フレームにおける対象車両２１の位置を表す観測データを、式（１）で定義する２行１列の位置ベクトルｚｋで表す。
0006952914.tif 000002
（１）
ここで、ｚｋは観測座標上での位置ベクトルとし、ｚｋ（１）は対象車両２１の横位置を表す観測データ、ｚｋ（２）は対象車両２１の縦位置を表す観測データとする。また、Tの上添字は行列の転置を表す。
【００３２】
経路データは、ここでは前記のとおり、対象車両２１が走行している車線の中央点の位置を表すデータとする。以降では、対象車両２１が走行している車線のｉ番目の中央点の位置を、式（２）で定義する２行１列の位置ベクトルｓｉで表す。
0006952914.tif 000003
（２）
ここで、ｓｉは観測座標上での位置ベクトルとし、ｓｉ（１）は中央点の横位置を表し、ｓｉ（２）は中央点の縦位置を表す。各中央点の通し番号であるｉは１以上Ｉ以下（Ｉは２以上の整数）とし、経路データはＩ個の位置ベクトルｓｉによって表されるとする。なお、各中央点の通し番号であるｉは、縦位置の昇順に並べ替えられているとする。すなわち、任意の通し番号ｉに対して、ｓｉ（２）＜ｓｉ＋１（２）が常に成り立つとする。
【００３３】
また、経路データである車線の中央点は、自車２０から対象車両２１までの距離に比べて、十分に遠方まで取得されており、いずれかの中央点の縦方向の中間に他車が観測されるものとする。すなわち、各中央点の通し番号ｉが１以上Ｉ以下の場合（Ｉは２以上の整数）、任意の第ｋ時刻フレームにおいて以下が常に成立することを前提とする。
0006952914.tif 000004
（３）
続いて、対象車両２１の運動諸元の推定値を表すベクトルを、観測座標と経路座標についてそれぞれ定義する。まず時刻フレームｋでの観測座標における対象車両２１の運動諸元推定値は、以下の４行１列のベクトルｘ（оｂｓ）ｋで表す。
0006952914.tif 000005
（４）
ここで、ｘｋ（оｂｓ、１）は対象車両２１の横方向位置、ｘｋ（оｂｓ、２）は対象車両２１の縦方向位置、ｘｋ（оｂｓ、３）は対象車両２１の横方向速度、ｘｋ（оｂｓ、４）は対象車両２１の縦方向速度の推定値を表す。
【００３４】
続いて、時刻フレームｋでの経路座標における対象車両２１の運動諸元推定値は、経路平行成分の運動諸元推定値を表す以下の２行１列のベクトルｘｋ（ｐａｒａ）と、経路垂直成分の運動諸元推定値を表す以下の２行１列のベクトルｘｋ（ｖｅｒｔ）で表す。それぞれ以下のとおり定義する。
0006952914.tif 000006
（５）
0006952914.tif 000007
（６）
ここで、ｘｋ（ｐａｒａ、１）は対象車両２１の経路平行方向の位置、ｘｋ（ｐａｒａ、２）は対象車両２１の経路平行方向の速度、ｘｋ（ｖｅｒｔ、１）は対象車両２１の経路垂直方向の位置、ｘｋ（ｖｅｒｔ、２）は対象車両２１の経路垂直方向の速度の推定値を表す。
【００３５】
上記のように定義した運動諸元推定値のベクトルについて、その概念図を図６と図７に示す。図６では観測座標における運動諸元推定値ｘｋ（оｂｓ）の位置成分を、図７では経路座標における運動諸元推定値ｘｋ（ｐａｒａ）、ｘｋ（ｖｅｒｔ）の位置成分を示す。ただし、図７の経路座標に関しては、図示を簡略化するために経路データｓｉを経路平行方向の原点として図示した。
【００３６】
次に、運動諸元の推定誤差を表す行列について定義する。推定誤差は、運動諸元を表すベクトルの誤差共分散行列の形で表す。以降では、観測座標における運動諸元推定値ベクトルｘｋ（оｂｓ）の誤差はＰｋ（оｂｓ）と表す。また、経路座標における運動諸元推定値ベクトルｘｋ（ｐａｒａ）、ｘｋ（ｖｅｒｔ）の誤差はそれぞれＰｋ（ｐａｒａ）、Ｐｋ（ｖｅｒｔ）と表す。なお、ここでは、例として運動諸元を位置および速度の２種類とした。しかし運動諸元は必ずしも位置と速度の２種類でなくてもよく、例えば加速度、ジャーク成分などを加えた３種類以上の運動諸元を用いてもよいものとする。
【００３７】
以下、図５の各ステップについて説明する。ステップST１では、経路平行成分予測部１０が、現時刻における経路平行成分の運動諸元を予測し、その運動諸元予測値の誤差を算出する。このステップでは、前時刻における運動諸元推定値ｘｋ（ｐａｒａ）と推定誤差Ｐｋ（ｐａｒａ）を入力として、対象車両２１の経路平行位置が等速移動するという運動モデルに基づき、以下の式により運動諸元予測値ｘｋ｜ｋ−１（ｐａｒａ）と予測誤差Ｐｋ｜ｋ−１（ｐａｒａ）を算出する。
0006952914.tif 000008
（７）
0006952914.tif 000009
（８）
ここで、Ｑｋ（ｐaｒa）は、運動モデルの誤差を表す２行２列の誤差共分散行列であり、あらかじめ設定するパラメタである。またΦｋ（ｐaｒa）は、運動モデルに基づき運動諸元ベクトルを前時刻から現時刻へ遷移させる遷移行列であり、以下の行列とする。
0006952914.tif 000010
（９）
ここで、τｋは前時刻から現時刻までの経過時間を表す。なお、上記の式では運動モデルとして等速移動を仮定したが、他の運動モデル、例えば特定の加速度による等加速度運動モデルなどとしてもよい。
【００３８】
ステップST２では、経路垂直成分予測部１１が、現時刻における経路垂直成分の運動諸元を予測し、その運動諸元予測値の誤差を算出する。このステップでは、前時刻における運動諸元推定値ｘｋ（ｖｅｒｔ）と推定誤差Ｐｋ（ｖｅｒｔ）を入力として、対象車両２１の経路垂直位置が等速移動するという運動モデルに基づき、以下の式により運動諸元予測値ｘｋ｜ｋ−１（ｖｅｒｔ）と予測誤差Ｐｋ｜ｋ−１（ｖｅｒｔ）を算出する。
0006952914.tif 000011
（１０）
0006952914.tif 000012
（１１）
ここで、Qｋ（ｖｅｒｔ）は、運動モデルの誤差を表す２行２列の誤差共分散行列であり、あらかじめ設定するパラメタである。またΦｋ（ｖｅｒｔ）は、運動モデルに基づき運動諸元ベクトルを前時刻から現時刻へ遷移させる遷移行列であり、以下の行列とする。
0006952914.tif 000013
（１２）
なお、上記の式では運動モデルとして等速移動を仮定したが、他の運動モデル、例えば特定の加速度による等加速度運動モデルなどとしてもよい。また、ステップＳＴ１とステップＳＴ２が仮定する運動モデルおよびパラメタは異なっていてもよい。
【００３９】
ステップST３では、第１座標変換部１３が、経路座標の運動諸元予測値から観測座標の運動諸元予測値へ変換する。また、経路座標の運動諸元予測誤差から、観測座標の運動諸元予測誤差へ変換する。変換においては、経路データ格納部１２から経路データｓｉを用いる。このステップでは、ｘｋ｜ｋ−１（ｐａｒａ）、Ｐｋ｜ｋ−１（ｐａｒａ）、ｘｋ｜ｋ−１（ｖｅｒｔ）、Ｐｋ｜ｋ−１（ｖｅｒｔ）、ｓｉを入力として、以下の式により観測座標の運動諸元予測値ｘｋ｜ｋ−１（оｂｓ）と、観測座標の運動諸元予測誤差Ｐｋ｜ｋ−１（оｂｓ）を算出する。
0006952914.tif 000014
（１３）
0006952914.tif 000015
（１４）
ここで、回転行列Ｇｊ（ｐａｒａ）、Ｇｊ（ｖｅｒｔ）と、並進ベクトルｇｊは以下のとおり定義する。
0006952914.tif 000016
（１５）
0006952914.tif 000017
（１６）
0006952914.tif 000018
（１７）
上記の式における、経路データを構成する線分に平行な単位ベクトルｅｊ（ｐａｒａ）と、ｅｊ（ｐａｒａ）に垂直な単位ベクトルｅｊ（ｖｅｒｔ）と、経路方向原点からｓｊまでの経路方向の長さλｊは以下のとおり算出する。
0006952914.tif 000019
（１８）
0006952914.tif 000020
（１９）
0006952914.tif 000021
（２０）
ベクトルに対する絶対値記号（｜｜）はベクトルのユークリッド距離と定義する。なお、上記における経路データの通し番号ｊは、対象車両２１を前後に挟む中央点とし、以下の条件を満たす１以上Ｉ以下のｊと定義する。
0006952914.tif 000022
（２１）
【００４０】
ステップST４では、センサ部１４が観測データを取得し、フィルタリング部１５が観測データと対象車両２１との対応付けを行う。センサの種類を問わず一般に、対象車両２１の位置をセンサにより観測する場合、対象車両２１の位置を表す観測データの他に、対象車両２１以外に由来する誤った観測データが取得される事態が生じうる。例えばセンサ部１４がカメラなどの可視光画像に基づく車両観測センサである場合、看板など車両に似た物体の位置が誤った観測データとなる。また例えばセンサ部１４がレーダなどの電磁波の反射強度に基づく車両観測センサである場合、ガードレール等の電波反射物体の位置が誤った観測データとなる。
【００４１】
またセンサの種類を問わず一般に、対象車両２１の位置をセンサにより観測する場合、観測データの中に対象車両２１に由来する観測データが含まれていない事態が生じうる。例えばセンサ部１４がカメラの場合、遮蔽物によってカメラから見た対象車両２１が隠れた際に、対象車両２１に由来する観測データは取得されない。また例えばセンサ部１４がレーダの場合、対象車両２１の反射電波が一時的に信号雑音に埋もれた際に、対象車両２１に由来する観測データは取得されない。
【００４２】
上記の理由により、このステップＳＴ４では、現時刻に取得した観測データと、運動諸元予測値との比較により、対象車両２１に由来する観測データが存在するか、また存在するならばいずれの観測データであるかの対応付けを行う。対応付けにおいては、各々の観測データｚｋに対して、観測データと予測位置との残差を表す以下のマハラノビス距離δを計算する。
0006952914.tif 000023
（２２）
ここでＳｋは対象車両２１の予測位置と観測データとの残差に関する誤差共分散行列であり
0006952914.tif 000024
（２３）
より算出する。ここでＲは観測データｚｋの誤差共分散行列であり、あらかじめ設定するパラメタである。またＨは、観測座標における運動諸元から位置ベクトルを取り出すための行列であり
0006952914.tif 000025
（２４）
と定義する。
【００４３】
式（２２）により算出した残差δが小さいほど、その観測データは対象車両２１の予測値と整合していることを表すため、複数の観測データｚｋのうち、δが最小となる観測データを対象車両２１に由来すると対応付け、その観測データをステップＳＴ５で処理する観測データｚｋとする。また、前記のように対象車両２１に由来する観測データが１つも取得されなかった場合も起こりうるため、残差δが一定値未満の観測データが１つもなかった場合、または現時刻で取得した観測データが０個であった場合、ステップＳＴ５で処理する観測データには無効値を代入する。
【００４４】
ステップST５では、フィルタリング部１５が、観測データに基づき現時刻における対象車両２１の運動諸元推定値と、推定誤差を算出する。ここで用いる観測データｚｋは、ステップＳＴ４において対象車両２１と対応付けられた観測データとする。
現時刻における対象車両２１の運動諸元推定値ｘｋ（оｂｓ）と運動諸元推定値の誤差共分散行列Ｐｋ（оｂｓ）は、以下の式により算出する。
0006952914.tif 000026
（２５）
0006952914.tif 000027
（２６）
ここで、Ｈは式（２４）と同じとする。また、Ｋｋは以下のとおり定義する。
0006952914.tif 000028
（２７）
ここで、Ｓｋは式（２３）と同じとし、上添字の「−１」は逆行列を表すとする。なお、観測データｚｋが無効値であった場合、現時刻では対象車両２１が観測されなかったとみなし、以下のように予測値を推定値とする。
0006952914.tif 000029
（２８）
0006952914.tif 000030
（２９）  
【００４５】
ステップST６では、第２座標変換部１６が、観測座標の運動諸元推定値から経路座標の運動諸元推定値へ変換する。また、観測座標の運動諸元推定誤差から、経路座標の運動諸元推定値へ変換する。変換においては、経路データ格納部１２から経路データｓｉを用いる。
このステップでは、ｘｋ（оｂｓ）、Ｐｋ（оｂｓ）、ｓｉを入力として、以下の式により経路座標の運動諸元推定値ｘｋ（ｐａｒａ）、ｘｋ（ｖｅｒｔ）および経路座標の運動諸元推定誤差Ｐｋ（ｐａｒａ）、Ｐｋ（ｖｅｒｔ）を算出する。
0006952914.tif 000031
（３０）
0006952914.tif 000032
（３１）
0006952914.tif 000033
（３２）
0006952914.tif 000034
（３３）
ここで、回転行列回転行列Ｇｊ（ｐａｒａ）、Ｇｊ（ｖｅｒｔ）はステップＳＴ３の式（１５）、（１６）と同じ定義とし、並進ベクトルｇｊもステップＳＴ３の式（１７）と同じ定義とする。ただし、経路データの通し番号ｊの算出方法は、経路平行方向の位置において、対象車両２１を前後に挟む中央点とし、以下の条件を満たす１以上Ｉ以下のｊと定義する。
0006952914.tif 000035
（３４）
なお、λｊの定義はステップＳＴ３の式（２０）と同じとする。
【００４６】
ステップST７では、状態判定部１７が、経路座標の運動諸元推定値と推定誤差を基に、対象車両２１の運動状態として対象車両２１の蛇行有無を判定する。判定にあたり、以下のマハラノビス距離Δｋを算出する。
0006952914.tif 000036
（３５）
ここでｈは、経路垂直方向の運動諸元から位置成分を取り出すための行列であり、
0006952914.tif 000037
（３６）
と定義した。
【００４７】
式（３５）のΔｋが大きいほど、経路の中心から対象車両２１推定位置までの距離は、位置の推定誤差に比べ有意に離れていることを表している。したがってΔｋの時系列グラフが、ある一定以上の振幅をもって振動しているならば、対象車両２１は蛇行していると判定する。Δｋの振幅を判定する際には、例えばある一定時刻フレーム間でのΔｋの最大値と最小値の差を振幅とする、Δｋを短時間フーリエ変換し周波数成分のピーク値を振幅とする、などの方法を用いる。
【００４８】
なお、上記では経路垂直方向の運動諸元とその推定誤差のみに基づき蛇行の有無を判定したが、経路平行方向の運動諸元とその推定誤差を用いてもよい。例えば、経路平行方向の位置がその推定誤差に比べどの程度大きいか、すなわち対象車両２１とセンサがどれだけ離れているかに応じて、蛇行を判定するΔｋの振幅しきい値を連続的に高くするなどとしてもよい。
【００４９】
以上のように構成された実施の形態１に係る運動状態判定装置１００では、次の効果が得られる。まず初めに、実施の形態１に係る運動状態判定装置１００では、運動諸元の推定値の誤差を逐次算出し、この誤差に基づき対象車両２１の運動状態を判定する。判定に用いられる運動諸元推定値の誤差には、フィルタリング部１５において想定した観測データの誤差が反映されている。すなわち状態判定部１７では、運動諸元の予測値を用いて算出された観測データの誤差に基づき対象車両２１の運動状態を判定している。この構成により、観測データの誤差による運動諸元の変動と、真の運動諸元の変動との区別が可能となる。この効果は、特許文献１のような対象車両２１の位置のみに基づき運動状態を判定する従来技術と比べ、対象車両２１がセンサより遠方を走行しており観測データの誤差が大きい場合において、観測誤差による観測データ上の対象車両２１の位置変動が、対象車両２１の蛇行として誤って判定される事態の頻度を減らすことができる点で、顕著な効果として現れる。
【００５０】
さらに、実施の形態１に係る運動状態判定装置１００では、対象車両２１の運動諸元と運動諸元の推定誤差を、経路垂直方向の成分と経路平行方向の成分についてそれぞれ算出し、これら経路座標上の運動諸元推定値と推定誤差の値に基づき対象車両２１の運動状態を判定する。この構成により、対象車両２１が走行する経路が任意の形状に湾曲している場合においても、経路に対して垂直な方向の運動諸元推定値とその推定誤差、経路に対して平行な方向の運動諸元とその推定誤差に関する時系列データに基づく状態判定が可能となる。この効果は、対象車両２１の走行する経路自体が蛇行している場合において、例えば方角を基準とした座標系上での運動諸元推定値と推定誤差に基づき対象車両２１の蛇行を判定するという単純な手段と比較すると、単純な手段では経路に沿った蛇行と経路から逸れた方向の蛇行とが区別できず、一方の実施の形態１に係る運動状態判定装置１００ではこれらを区別できる点で顕著な効果として現れる。
【００５１】
さらに、実施の形態１に係る運動状態判定装置１００では、経路に対して平行方向の運動諸元と、経路に対して垂直方向の運動諸元をそれぞれ予測する。一般に自動車や二輪車などは、経路に対し平行な方向の加速減速に比べ、経路に対し垂直な方向に加速減速する頻度は少ない。従って、経路に対して平行方向と垂直方向で異なる運動モデル、異なる運動モデルの誤差を設定することで、より実際の対象車両２１の運動に則した運動諸元の予測値が可能となる。この効果は、例えば方角を基準とした座標系上において運動諸元を予測する単純な手段に比べ、運動諸元の予測誤差を低下させることが可能となり、その結果推定誤差が低下し、推定誤差との比較による運動諸元の微小な変動に対する判定が可能となり、最終的に運動状態判定の誤り頻度を低下できる点で、顕著な効果として現れる。
【００５２】
このように、実施の形態１に係る運動状態判定装置１００では、目標（例えば、対象車両２１）の観測データを取得するセンサ部１４と、前記目標が移動可能な経路の形状を表す経路データを格納する経路データ格納部１２と、前記目標の運動諸元の予測値及び予測誤差の前記経路に平行な成分と垂直な成分をそれぞれ算出する経路成分予測部１と、前記経路成分予測部１により算出された運動諸元の予測値及び予測誤差を、前記観測データを表す第１の座標系と同じ座標系に変換する第１座標変換部１３と、前記センサ部１４で取得された観測データと、前記経路データ格納部１２に格納された経路データと、前記第１座標変換部１３で変換された運動諸元の予測値及び予測誤差を用いて、前記目標の運動諸元の推定値及び推定誤差を算出するフィルタリング部１５と、前記フィルタリング部１５により算出された前記目標の運動諸元の推定値及び推定誤差を、前記経路に平行な成分と垂直な成分を座標軸とする第２の座標系に変換する第２座標変換部１６と、前記第２座標変換部１６によって変換された前記目標の運動諸元の推定値及び推定誤差を用いて、前記目標の運動状態を判定する状態判定部１７と、を備えたことを特徴とする。本発明によれば、観測誤差による観測データ上の目標の位置変動によって、目標の運動状態の判定を誤る頻度を減らすことができる。特に、本発明では、観測データに誤差がある場合であっても目標の運動諸元の予測を行うことによって、目標の運動諸元の推定誤差を小さくすることができる。その結果、観測データのみを用いる従来技術に対して、目標の運動状態の判定を誤る頻度を減らすことができる。また、目標が走行する経路が任意の形状に湾曲している場合においても、経路に対して垂直な方向の運動諸元推定値とその推定誤差、経路に対して平行な方向の運動諸元とその推定誤差に関する時系列データに基づく状態判定が可能となるため、目標の運動状態の判定を誤る頻度を減らすことができる。
【００５３】
また、実施の形態１に係る運動状態判定装置１００では、経路成分予測部１は、前記経路に対して平行方向と垂直方向で異なる運動モデルを設定して、前記経路に平行な成分と垂直な成分をそれぞれ算出することを特徴とする。この構成を用いることによって、より実際の対象車両２１の運動に則した運動諸元の予測値が可能となる。
【００５４】
実施の形態２．
実施の形態１では、１つの運動モデルに基づき経路平行方向の運動諸元を予測し、もう１つの運動モデルに基づき経路垂直方向の運動諸元を予測する。しかし、対象車両２１がとりうる運動は多様であり、経路方向と垂直方向の運動がそれぞれ１種類の運動モデルによって妥当に表現できるとは限らない。
【００５５】
例えば、対象車両２１以外の車両によって経路上が混雑している場合、車間を調整するために対象車両２１は経路平行方向の加速減速を頻発させるが、経路上が空いている場合、対象車両２１の経路平行方向の運動は等速を維持する可能性が高い。また、経路垂直方向の運動は、対象車両２１の重量や路面環境などに依存し、例えば対象車両２１が大型車両の場合は小型車両に比べ経路垂直方向の加速減速は生じにくい。また、対象車両２１の運動を特徴づける、混雑状況や車両重量、路面環境などは自車２０が常に取得可能とは限らない。従って、対象車両２１の属性や路面環境などが多様であり、かつそれらの条件が取得できない場合において、経路平行方向の運動諸元と経路垂直方向の運動諸元を、それぞれ１種類の運動モデルを仮定し予測すると、予測誤差が大きくなり、最終的に推定誤差に基づく運動状態の判定誤りが頻発する。
【００５６】
そこで、実施の形態２では、経路平行方向の運動諸元と、経路垂直方向の運動諸元について、運動モデルが異なる複数の予測値をそれぞれ算出する。また、それぞれの予測値について運動諸元の推定値を算出し、それらのうち妥当な推定値に基づき対象車両２１の運動状態を判定する。この構成により、対象車両２１の運動モデルが１種類で表現できないほど多様な場合においても、運動状態の判定を誤る頻度を減らすことができる。
【００５７】
図８は、実施の形態２に係る運動状態判定装置１００の構成を示すブロック図である。図８に示すように、実施の形態２に係る運動状態判定装置１００は、経路平行成分予測部１０、経路垂直成分予測部１１、経路データ格納部１２、第１座標変換部１３、センサ部１４、フィルタリング部１５、第２座標変換部１６、状態判定部１７から構成される。ただし、経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６はそれぞれＮ個が並列されている（Ｎは２以上の整数）。ここで、経路データ格納部１２、センサ部１４は実施の形態１と同じである。また、第１座標変換部１３、第２座標変換部１６は、Ｎ個が並列されている点が実施の形態１と異なるが、それぞれの内部処理は実施の形態１と同じである。
【００５８】
以下、図８における各機能ブロック間の関係を示す。なお、Ｎ個存在する経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６に関しては、ｎ番目の機能ブロックに関して説明する（ｎは１以上Ｎ以下の整数）。
【００５９】
ｎ番目の経路平行成分予測部１０は、ｎ番目の第２座標変換部１６から前時刻における対象車両２１のｎ番目の運動諸元の経路平行成分推定値と、経路平行成分の推定誤差を受け取り、現時刻における対象車両２１のｎ番目の運動諸元の経路平行成分予測値と、経路平行成分の予測誤差をｎ番目の第１座標変換部１３へ送る。
【００６０】
ｎ番目の経路垂直成分予測部１１は、ｎ番目の第２座標変換部１６から前時刻における対象車両２１のｎ番目の運動諸元の経路垂直成分推定値と、経路垂直成分の推定誤差を受け取り、現時刻における対象車両２１のｎ番目の運動諸元の経路垂直成分予測値と、経路垂直成分の予測誤差をｎ番目の第１座標変換部１３へ送る。
【００６１】
ｎ番目の第１座標変換部１３は、ｎ番目の経路平行成分予測部１０から現時刻における対象車両２１のｎ番目の運動諸元の経路平行成分予測値と、経路平行成分の予測誤差を受け取り、ｎ番目の経路垂直成分予測部１１から現時刻における対象車両２１のｎ番目の運動諸元の経路垂直成分予測値と、経路垂直成分の予測誤差を受け取り、経路データ格納部１２から経路データを受け取る。そして、現時刻における対象車両２１のｎ番目の観測座標上運動諸元の予測値と、観測座標上運動諸元の予測誤差をｎ番目のフィルタリング部１５へ送る。
【００６２】
ｎ番目のフィルタリング部１５は、ｎ番目の第１座標変換部１３から現時刻フレームにおける対象車両２１のｎ番目の観測座標上運動諸元の予測値と、観測座標上運動諸元の予測誤差を受け取り、センサ部１４から現時刻における対象車両２１の観測データを受け取る。そして、現時刻における対象車両２１のｎ番目の観測座標上運動諸元の推定値と、観測座標上運動諸元の推定誤差をｎ番目の第２変換部へ送る。また、現時刻における対象車両２１のｎ番目の予測位置と観測データとの残差を、状態判定部１７へ送る。
【００６３】
ｎ番目の第２座標変換部１６は、ｎ番目のフィルタリング部１５から現時刻における対象車両２１のｎ番目の観測座標上運動諸元の推定値と、観測座標上運動諸元の推定誤差を受け取り、経路データ格納部１２から経路データを受け取る。そして、状態判定部１７へ現時刻における対象車両２１のｎ番目の経路座標上運動諸元の推定値と、経路座標上運動諸元の推定誤差を送る。また次の時刻フレームにおいて、ｎ番目の経路平行成分予測部１０に前時刻における対象車両２１のｎ番目の運動諸元の経路平行成分推定値と、経路平行成分の推定誤差を送り、ｎ番目の経路垂直成分予測部１１に前時刻における対象車両２１のｎ番目の運動諸元の経路垂直成分推定値と、経路垂直成分の推定誤差を送る。
【００６４】
状態判定部１７は、１〜Ｎ番目の第２座標変換部１６から現時刻における対象車両２１の１〜Ｎ番目の経路座標上運動諸元の推定値と、経路座標上運動諸元の推定誤差を受け取り、また、１〜Ｎ番目のフィルタリング部１５から、現時刻における対象車両２１の１〜Ｎ番目の予測位置と観測データとの残差を受けとる。そして、現時刻における対象車両２１の蛇行の有無を運動状態判定装置１００の出力として出力する。
【００６５】
実施の形態２に係るハードウェア構成は、実施の形態１と同様、図４にて示す。実施の形態２に係る運動状態判定装置１００における、１〜Ｎ番目の経路平行成分予測部１０、１〜Ｎ番目の経路垂直成分予測部１１、１〜Ｎ番目の第１座標変換部１３、１〜Ｎ番目のフィルタリング部１５、１〜Ｎ番目の第２座標変換部１６、状態判定部１７は記録装置４２に格納されており、１〜Ｎ番目の経路平行成分予測部１０、１〜Ｎ番目の経路垂直成分予測部１１、１〜Ｎ番目の第１座標変換部１３、１〜Ｎ番目のフィルタリング部１５、１〜Ｎ番目の第２座標変換部１６、状態判定部１７は演算装置４１によって実行される。また、経路データ格納部１２は記録装置４２に格納されている。
また、１〜Ｎ番目の経路平行成分予測部１０、１〜Ｎ番目の経路垂直成分予測部１１、１〜Ｎ番目の第１座標変換部１３、１〜Ｎ番目のフィルタリング部１５、１〜Ｎ番目の第２座標変換部１６、状態判定部１７は、実行中に記録装置４２へ入出力値および処理中間データの書き込み、読み出し、削除を行う。
【００６６】
次に、図９のフローチャートを用いて、実施の形態２に係る運動状態判定装置１００の動作の詳細を説明する。図９は、１〜Ｎ番目の経路平行成分予測部１０、１〜Ｎ番目の経路垂直成分予測部１１、１〜Ｎ番目の第１座標変換部１３、センサ部１４、１〜Ｎ番目のフィルタリング部１５、１〜Ｎ番目の第２座標変換部１６、状態判定部１７の、現時刻における動作を示したフローチャートである。なお、以降の説明における記号および添字のうち、説明が無いものは実施の形態１と同じ意味であるとする。
【００６７】
動作の説明の準備として、ｎ番目の経路平行成分予測部１０、ｎ番目の経路垂直成分予測部１１、ｎ番目の第１座標変換部１３、ｎ番目のフィルタリング部１５、ｎ番目の第２座標変換部１６で算出される、対象車両２１のｎ番目の運動諸元に関する記号を定義する（ｎは１以上Ｎ以下の整数）。
【００６８】
まず、ｎ番目の、時刻フレームｋでの観測座標における対象車両２１の運動諸元推定値は、以下のベクトルｘｋ，ｎ（оｂｓ）で表す。
0006952914.tif 000038
（３７）
ここで、ｘｋ、ｎ（оｂｓ、１）は対象車両２１の横方向位置、ｘｋ、ｎ（оｂｓ、２）は対象車両２１の縦方向位置、ｘｋ、ｎ（оｂｓ、３）は対象車両２１の横方向速度、ｘｋ、ｎ（оｂｓ、４）は対象車両２１の縦方向速度の推定値を表す。
【００６９】
また、ｎ番目の、時刻フレームｋでの経路座標における対象車両２１の運動諸元推定値は、経路平行成分の運動諸元推定値を表す以下の２行１列のベクトルｘｋ、ｎ（ｐａｒａ）と、経路垂直成分の運動諸元推定値を表す以下の２行１列のベクトルｘｋ、ｎ（ｖｅｒｔ）で表す。それぞれ以下のとおり定義する。
0006952914.tif 000039
（３８）
0006952914.tif 000040
（３９）
ここで、ｘｋ、ｎ（ｐａｒａ、１）は対象車両２１の経路平行方向の位置、ｘｋ、ｎ（ｐａｒａ、２）は対象車両２１の経路平行方向の速度、ｘｋ、ｎ（ｖｅｒｔ、１）は対象車両２１の経路垂直方向の位置、ｘｋ、ｎ（ｖｅｒｔ、２）は対象車両２１の経路垂直方向の速度の推定値を表す。
【００７０】
次に、各運動諸元の推定誤差に関しても定義する。観測座標における運動諸元推定値ベクトルｘｋ、ｎ（оｂｓ）の誤差はＰｋ、ｎ（оｂｓ）と表す。また、経路座標における運動諸元推定値ベクトルｘｋ、ｎ（ｐａｒａ）、ｘｋ、ｎ（ｖｅｒｔ）の誤差はそれぞれＰｋ、ｎ（ｐａｒａ）、Ｐｋ、ｎ（ｖｅｒｔ）と表す。
【００７１】
以下、図９の各ステップについて説明する。ステップＳＴ８では、運動状態判定装置１００が、現時刻の処理において未選択の１以上Ｎ以下の整数番号を選択する。なお、未選択の番号のうち、いずれの番号を選択するかの方法は任意とする。以降のステップＳＴ９〜１３の説明においては、ここでｎという番号が選択された場合について処理を説明する。
【００７２】
ステップＳＴ９では、ｎ番目の経路平行成分予測部１０が、現時刻におけるｎ番目の経路平行成分の運動諸元を予測し、その運動諸元予測値の誤差を算出する。
このステップでは、前時刻におけるｎ番目の運動諸元推定値ｘｋ、ｎ（ｐａｒａ）と推定誤差Ｐｋ、ｎ（ｐａｒａ）を入力として、対象車両２１の経路平行位置が等速移動するというｎ番目の運動モデルに基づき、以下の式によりｎ番目の運動諸元予測値ｘｋ｜ｋ−１、ｎ（ｐａｒａ）と予測誤差Ｐｋ｜ｋ−１、ｎ（ｐａｒａ）を算出する。
0006952914.tif 000041
（４０）
0006952914.tif 000042
（４１）
【００７３】
ここで、Qｋ、ｎ（ｐaｒa）は、経路平行方向に関するｎ番目の運動モデルの誤差を表す２行２列の誤差共分散行列であり、あらかじめ設定するパラメタである。またΦｋ、ｎ（ｐaｒa）は、経路平行方向に関するｎ番目の運動モデルに基づき運動諸元ベクトルを前時刻から現時刻へ遷移させる遷移行列であり、例えば以下の行列とする。
0006952914.tif 000043
（４２）
【００７４】
なお、上記の式では経路平行成分に関するｎ番目の運動モデルとして等速移動を仮定したが、他の運動モデルとしてもよい。例えば、等加速度運動を仮定し、式（４０）の代わりに
0006952914.tif 000044
（４３）
などとしてもよい。ここで、ａｎ(ｐａｒａ)は経路平行成分に関する加速度を表すパラメータとした。
【００７５】
ステップＳＴ１０では、ｎ番目の経路垂直成分予測部１１が、現時刻におけるｎ番目の経路垂直成分の運動諸元を予測し、その運動諸元予測値の誤差を算出する。このステップでは、前時刻におけるｎ番目の運動諸元推定値ｘｋ、ｎ（ｖｅｒｔ）と推定誤差Ｐｋ、ｎ（ｖｅｒｔ）を入力として、対象車両２１の経路垂直位置が等速移動するというｎ番目の運動モデルに基づき、以下の式によりｎ番目の運動諸元予測値ｘｋ｜ｋ−１、ｎ（ｖｅｒｔ）と予測誤差Ｐｋ｜ｋ−１、ｎ（ｖｅｒｔ）を算出する。
0006952914.tif 000045
（４４）
0006952914.tif 000046
（４５）
ここで、Qｋ、ｎ（ｖｅｒｔ）は、経路垂直方向に関するｎ番目の運動モデルの誤差を表す２行２列の誤差共分散行列であり、あらかじめ設定するパラメタである。またΦｋ、ｎ（ｖｅｒｔ）は、経路垂直方向に関するｎ番目の運動モデルに基づき運動諸元ベクトルを前時刻から現時刻へ遷移させる遷移行列であり、例えば以下の行列とする。
0006952914.tif 000047
（４６）
【００７６】
なお、上記の式では経路平行成分に関するｎ番目の運動モデルとして等速移動を仮定したが、他の運動モデルとしてもよい。例えば、等加速度運動を仮定し、式（４４）の代わりに
0006952914.tif 000048
（４７）
などとしてもよい。ここでａｎ(ｖeｒｔ)は経路垂直成分に関する加速度を表すパラメタとした。また、ステップＳＴ９とステップＳＴ１０が仮定する運動モデルおよびパラメタは異なっていてもよい。
【００７７】
ステップＳＴ１１では、ｎ番目の第１座標変換部１３が、経路座標のｎ番目の運動諸元予測値から観測座標のｎ番目の運動諸元予測値へ変換する。また、経路座標のｎ番目の運動諸元予測誤差から、観測座標のｎ番目の運動諸元予測誤差へ変換する。変換においては、経路データ格納部１２から経路データｓｉを用いる。このステップでは、ｘｋ｜ｋ−１、ｎ（ｐａｒａ）、Ｐｋ｜ｋ−１、ｎ（ｐａｒａ）、ｘｋ｜ｋ−１、ｎ（ｖｅｒｔ）、Ｐｋ｜ｋ−１、ｎ（ｖｅｒｔ）、ｓｉを入力として、以下の式により観測座標の運動諸元予測値ｘｋ｜ｋ−１、ｎ（оｂｓ）と、観測座標の運動諸元予測誤差Ｐｋ｜ｋ−１、ｎ（оｂｓ）を算出する。
0006952914.tif 000049
（４８）
0006952914.tif 000050
（４９）
ここで、回転行列Ｇｊ（ｐａｒａ）、Ｇｊ（ｖｅｒｔ）および並進ベクトルｇｊは実施の形態１のステップＳＴ３と同じ定義とする。なお、上記における経路データの通し番号ｊは、対象車両２１を前後に挟む中央点とし、以下の条件を満たす１以上Ｉ以下のｊと定義する
0006952914.tif 000051
（５０）
λｊの定義はステップＳＴ３の式（２０）と同じとする。
【００７８】
ステップＳＴ１２では、センサ部１４が観測データを取得し、フィルタリング部１５が観測データと対象車両２１との対応付けを行う。対応付けにおいては、各々の観測データｚｋに対して、観測データと予測位置との残差を表す以下のマハラノビス距離δｎを計算する。
0006952914.tif 000052
（５１）
ここで、Ｓｋ、ｎは対象車両２１のｎ番目の予測位置と観測データとの残差に関する誤差共分散行列であり
0006952914.tif 000053
（５２）
より算出する。
【００７９】
複数の観測データｚｋのうち、δｎが最小となる観測データを対象車両２１由来であると対応付け、その観測データを直後のＳＴ１３で処理する観測データｚｋとする。また、残差δｎが一定値未満の観測データが１つもなかった場合、または現時刻で取得した観測データが０個であった場合、ステップＳＴ１３で処理する観測データには無効値を代入する。なお、実施の形態２ではここで算出した残差δｎを、ステップＳＴ１６の運動状態判定でも用いる。
【００８０】
ステップＳＴ１３では、ｎ番目のフィルタリング部１５が、観測データに基づき現時刻における対象車両２１のｎ番目の運動諸元推定値と、推定誤差を算出をする。ここで用いる観測データｚｋは、ステップＳＴ４において対象車両２１と対応付けられた観測データとする。現時刻における対象車両２１のｎ番目の運動諸元推定値ｘｋ、ｎ（оｂｓ）と運動諸元推定値の誤差共分散行列Ｐｋ、ｎ（оｂｓ）は、以下の式により算出する。
0006952914.tif 000054
（５３）
0006952914.tif 000055
（５４）
ここで、Ｋｋ、ｎは以下のとおり定義する。
0006952914.tif 000056
（５５）
ここで、Ｓｋ、ｎは式（５２）と同じとする。
【００８１】
なお、観測データｚｋが無効値であった場合、現時刻では対象車両２１が観測されなかったとみなし、以下のように予測値を推定値とする。
0006952914.tif 000057
（５６）
0006952914.tif 000058
（５７）
【００８２】
ステップＳＴ１４では、ｎ番目の第２座標変換部１６が、観測座標のｎ番目の運動諸元推定値から経路座標のｎ番目の運動諸元推定値へ変換する。また、観測座標のｎ番目の運動諸元推定誤差から、経路座標のｎ番目の運動諸元推定値へ変換する。変換においては、経路データ格納部１２から経路データｓｉを用いる。
【００８３】
このステップでは、ｘｋ、ｎ（оｂｓ）、Ｐｋ、ｎ（оｂｓ）、ｓｉを入力として、以下の式により経路座標の運動諸元推定値ｘｋ、ｎ（ｐａｒａ）、ｘｋ、ｎ（ｖｅｒｔ）および経路座標の運動諸元推定誤差Ｐｋ、ｎ（ｐａｒａ）、Ｐｋ、ｎ（ｖｅｒｔ）を算出する。
0006952914.tif 000059
（５８）
0006952914.tif 000060
（５９）
0006952914.tif 000061
（６０）
0006952914.tif 000062
（６１）
ここで、回転行列回転行列Ｇｊ（ｐａｒａ）、Ｇｊ（ｖｅｒｔ）および並進ベクトルｇｊは実施の形態１のステップＳＴ６と同じ定義とする。なお、経路データの通し番号ｊの算出方法は、経路平行方向の位置において、対象車両２１を前後に挟む中央点とし、以下の条件を満たす１以上Ｉ以下のｊと定義する。
0006952914.tif 000063
（６２）
λｊの定義はステップＳＴ３の式（２０）と同じとする。
【００８４】
ステップＳＴ１５では、運動状態判定装置１００が、現時刻の処理において未選択の１以上Ｎ未満の整数番号が存在するか否かを判定する。未選択の番号が存在するならばステップＳＴ８へ遷移し、存在しないならばステップＳＴ１６へ遷移する。
【００８５】
ステップＳＴ１６では、状態判定部１７が、Ｎ個の経路座標における運動諸元推定値と推定誤差、そして予測位置と観測データとの残差を基に、対象車両２１の運動状態として対象車両２１の蛇行有無を判定する。
まず、式（５１）によって算出された、Ｎ個の、予測位置と観測データとの残差δｎを用い、以下の番号ｎ＊を求める。
0006952914.tif 000064
（６３）
前述のδｎの定義から明らかなように、δｎが小さいほど、ｎ番目の運動モデルによって予測された位置が観測データに近いことを表している。従って、最も小さなδｎを与えるｎ＊番目の予測値が、N種類の運動モデルのうち最も対象車両２１の実態に即した運動モデルによって算出された予測値とみなすことができる。
【００８６】
ｎ＊を算出した後に、以下のマハラノビス距離Δｋ、＊を求める。
0006952914.tif 000065
（６４）
このΔｋ、＊が大きいほど、対象車両２１の位置が経路の中心から有意に離れていることを表している。従ってΔｋ、＊の時系列グラフが、ある一定以上の振幅をもって振動しているならば、対象車両２１は蛇行していると判定する。Δｋ、＊の振幅を判定する際には、例えばある一定時刻フレーム間でのΔｋ、＊の最大値と最小値の差を振幅とする、Δｋを短時間フーリエ変換し周波数成分のピーク値を振幅とする、などの方法を用いる。
【００８７】
なお、上記では経路垂直方向の運動諸元とその推定誤差のみに基づき蛇行の有無を判定したが、経路平行方向の運動諸元とその推定誤差を用いてもよい。例えば、経路平行方向の位置がその推定誤差に比べどの程度大きいか、すなわち対象車両２１とセンサがどれだけ離れているかに応じて、蛇行を判定するΔｋ、＊の振幅しきい値を連続的に高くするなどとしてもよい。
【００８８】
また、上記では残差を表すδｎの大きさに基づき、ｎ＊を算出したが、運動モデルの妥当性を表す指標、例えばＡＩＣ（Akaike Information Criteria）やＢＩＣ（Bayesian Information Criteria）などの指標を用いてｎ＊を算出してもよい。
【００８９】
なお、図９のフローチャートでは、ステップＳＴ９〜１４の処理を順にＮ回実行する動作としたが、代わりにステップＳＴ９〜１４を複数のスレッドで並列処理し、Ｎ個の経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６の処理が完了したのちに、ステップＳＴ１６を実行してもよい。
【００９０】
以上のように構成された実施の形態２に係る運動状態判定装置１００では、次の効果が得られる。実施の形態２に係る運動状態判定装置１００では、Ｎ個の経路平行成分予測部１０と、Ｎ個の経路垂直成分予測部１１によって、予測における運動モデルの前提またはパラメタ値が異なるＮ通りの運動諸元予測値を算出し、それぞれの予測値に基づき運動諸元を推定し、それらの予測値と予測誤差、また予測値と観測データとの残差に基づき対象車両２１の運動状態を判定するよう構成した。この構成により、対象車両２１が多様な運動をとりうる場合においても、実際の対象車両２１の運動に則した運動モデルに基づく運動諸元を予測し、その予測値から推定された運動諸元に基づく運動状態判定が可能となる。なお、Ｎ個の経路平行成分予測部１０によってＮ通りの運動諸元予測値及び予測誤差を算出することは、経路平行成分予測部１０によってＮ組の運動諸元予測値を算出する状態として扱うこともできる。
【００９１】
この効果は、例えば対象車両２１の属性や対象車両２１が走行する路面状態などが多様であり、かつそれらの情報が取得できない場合において、１種類の運動モデルに基づき推定された運動諸元から運動状態を判定した場合に比べ、予測誤差および推定誤差が小さい運動諸元を用いて運動状態の判定が可能となるため、最終的に運動状態判定を誤る頻度を減らすことができる点が、特に顕著な効果として現れる。
【００９２】
このように、実施の形態２に係る運動状態判定装置１００では、前記経路成分予測部１は、Ｎ組の前記目標の運動諸元の予測値及び予測誤差に対してそれぞれ前記経路に平行な成分と垂直な成分を算出し、前記第１座標変換部１３は、前記経路成分予測部１により算出されたＮ組の運動諸元の予測値及び予測誤差を、前記第１の座標系に変換し、前記フィルタリング部１５は、前記第１座標変換部１３で変換されたＮ組の運動諸元の予測値及び予測誤差のそれぞれの組に対して、前記目標の運動諸元の推定値及び推定誤差を算出し、前記第２座標変換部１６は、前記フィルタリング部１５により算出されたＮ組の前記目標の運動諸元の推定値及び推定誤差をそれぞれ前記第２の座標系に変換し、前記状態判定部１７は、前記第２座標変換部１６により変換されたＮ組の前記目標の運動諸元の推定値及び推定誤差を用いて、前記目標の運動状態を判定することを特徴とする。この構成によって、目標が多様な運動をとりうる場合においても、実際の目標の運動に則した運動モデルに基づく運動諸元を予測し、その予測値から推定された運動諸元に基づく運動状態判定が可能となる。
【００９３】
また、実施の形態２に係る運動状態判定装置１００では、前記経路成分予測部１は、Ｎ組の前記目標の運動諸元の予測値及び予測誤差に対してそれぞれ異なる運動モデルを設定して、前記経路に平行な成分と垂直な成分を算出することを特徴とする。この構成によって、さまざまな運動モデルの中から実際の目標の運動に則した運動モデルを選択することができ、最終的に運動状態判定を誤る頻度を減らすことができる。
【符号の説明】
【００９４】
１：経路成分予測部、１０：経路平行成分予測部、１１：経路垂直成分予測部、１２：経路データ格納部、１３：第１座標変換部、１４：センサ部、１５：フィルタリング部、１６：第２座標変換部、１７：状態判定部、２０：自車、２１：対象車両、２３：観測データ、４１：演算装置、４２：記録装置、４３：出力インターフェース、１００：運動状態判定装置
【図１】
0006952914.tif 000066
【図２】
0006952914.tif 000067
【図３】
0006952914.tif 000068
【図４】
0006952914.tif 000069
【図５】
0006952914.tif 000070
【図６】
0006952914.tif 000071
【図７】
0006952914.tif 000072
【図８】
0006952914.tif 000073
【図９】
0006952914.tif 000074