JP2012008999A - 道路形状推定装置及びコンピュータプログラム及び道路形状推定方法 - Google Patents

Abstract

【課題】物体の誤検出や、静止判定の誤判定の影響を排除し、道路形状を推定する。
【解決手段】レーダ１１０（周辺物体観測装置）は、移動体の周辺に存在する物体について、移動体を基準とした物体の相対位置を繰り返し観測する。停止物識別部１３０（静止物体判定部）は、レーダ１１０が相対位置を観測した物体が静止しているか否かを判定する。道路近似曲線仮計算部１４０（物体相関部）は、レーダ１１０が観測した複数の相対位置のなかから、同一の物体についてレーダ１１０が観測した複数の相対位置を判定する。道路近似曲線本計算部１６０（近似曲線算出部）は、停止物識別部１３０の判定結果と、道路近似曲線仮計算部１４０の判定結果とに基づいて、移動体がいる道路の形状を近似する近似曲線を算出する。
【選択図】図１

Description

この発明は、移動体がいる道路の形状を推定する道路形状推定装置に関する。

移動体の進行方向を監視するカメラやレーダなどの観測結果に基づいて、道路形状を推定する装置がある。道路の縁には、例えば、デリニエータやガードレールなどの停止物体が存在するのに対し、道路上には停止物体が存在しない。このため、停止物の位置がわかれば、道路の形状が推定できる。

特開２００１−２５０１９７号公報 特開２００７−６６０４７号公報

レーダなどの観測装置は、ノイズやマルチパスなどの影響で、実際には存在しない物体を観測する場合がある。また、道路上には、先行車両などの移動物体が存在する場合があるので、停止物体と移動物体とを判別する必要がある。判別の精度が低い場合、移動物体を誤って停止物体であると判定する可能性がある。
この発明は、例えば上記のような課題を解決するためになされたものであり、誤検出や誤判定の影響を排除し、道路形状を近似する近似曲線の信頼性を高めることを目的とする。

この発明にかかる道路形状推定装置は、
相対位置取得部と、静止物体判定部と、物体相関部と、近似曲線算出部とを有し、
上記相対位置取得部は、移動体の周辺に存在する物体について、上記移動体を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置が観測した観測結果を取得し、
上記静止物体判定部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が相対位置を観測した物体が静止しているか否かを判定し、
上記物体相関部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置が観測した複数の相対位置を判定し、
上記近似曲線算出部は、上記静止物体判定部が判定した判定結果と、上記物体相関部が判定した判定結果とに基づいて、上記移動体がいる道路の形状を近似する近似曲線を算出することを特徴とする。

この発明にかかる道路形状推定装置によれば、静止物体判定部の判定結果と、物体相関部の判定結果とに基づいて、近似曲線を算出するので、誤検出や誤判定の影響を排除し、算出する近似曲線の信頼性を高めることができる。

実施の形態１における道路形状認識装置１００の構成を示すブロック図。 実施の形態１における停止物の追尾処理を説明するための図。 実施の形態１における道路近似曲線仮計算部１４０の一例（一括処理）のブロック図。 実施の形態１における道路近似曲線仮計算部１４０の処理フロー図。 実施の形態１における道路近似曲線仮計算部１４０の別の例（逐次処理）のブロック図。 実施の形態１における道路縁判定部１５０による道路縁判定の説明図。 実施の形態１における道路近似曲線本計算部１６０のブロック図。 実施の形態１における道路近似曲線本計算部１６０の追尾フィルタＡ１６３のブロック図。 実施の形態１における近似曲線選択部１７０のブロック図。 実施の形態１における近似曲線選択部１７０の近似曲線選択フロー図。 道路縁ごとの相関停止物の違いを説明するための図。 実施の形態１における曲線品質計算部１７２におけるカーブ品質値算出方式の一例を説明するための図。 実施の形態１における曲線品質計算部１７２におけるカーブ品質値算出方式の別の例を説明するための図。 実施の形態２における道路近似曲線仮計算部１４０の構成を示すブロック図。 実施の形態３における近似曲線選択部１７０の構成を示すブロック図。 近似曲線の選択問題を説明するための説明図。 実施の形態４における道路形状推定装置２００のハードウェア構成の一例を示すハードウェア構成図。 実施の形態４における道路形状推定装置２００の機能ブロックの構成の一例を示すブロック構成図。 移動体８０１の移動速度と、物体７０１，７０２の移動体８０１に対する相対速度との関係を説明するための図。 物体の移動体に対する相対速度の観測時刻７８１〜７８３と、移動体の移動速度の観測時刻７９１〜７９９との関係を説明するための図。 移動体８０１の移動軌跡７５１と、物体７０３〜７０８の移動体８０１に対する相対位置の軌跡７６１〜７６６との関係を説明するための図。 実施の形態４における物体相関部２３０の動作を説明するための図。 物体の移動体８０１に対する相対位置７４３ａ〜７４３ｃと近似曲線７５０との関係の一例を示す図。 実施の形態４における道路形状推定処理Ｓ５００の流れの一例を示すフローチャート図。 実施の形態４における観測結果取得工程Ｓ５１０の流れの一例を示すフローチャート図。 実施の形態４における静止判定工程Ｓ５２０の流れの一例を示すフローチャート図。 実施の形態４における物体相関工程Ｓ５３０の流れの一例を示すフローチャート図。 実施の形態４における近似曲線算出工程Ｓ５５０の流れの一例を示すフローチャート図。 実施の形態５における道路形状推定システム８００の全体構成の一例を示すシステム構成図。 物体相関部２３０が相関処理に用いる平行移動量および回転角度と、移動体８０１の移動量の関係を説明するための図。 実施の形態５における初期値算出部２５０の動作を説明するための説明図。 実施の形態５における近似曲線算出部２４０が算出する近似曲線の一例を示す図。 実施の形態５における道路形状推定処理Ｓ５００の流れの一例を示すフローチャート図。 実施の形態６における道路形状認識装置１００の全体の構成を示すブロック図。 直線路の一例を示す図。 カーブ手前の一例を示す図。 カーブ路の一例を示す図。 カーブ出口の一例を示す図。 実施の形態６における自車走行路の状態遷移モデルの一例を示す状態遷移図。 実施の形態６におけるカーブ手前判定条件の一例を説明するための図。 実施の形態６における直線路（状態０）からカーブ手前（状態１）への判定フロー図。 実施の形態６におけるカーブ手前（状態１）からカーブ路（状態２）及びカーブ出口（状態３）への判定フロー図。 ヨーレートによるカーブ路判定条件の説明図。 ヨーレートによる短カーブ判定条件の説明図。 実施の形態６におけるカーブ路（状態２）からカーブ出口（状態３）への判定フロー図。 道路近似曲線によるカーブ出口判定条件の説明図。 実施の形態６におけるカーブ出口（状態３）から直線路（状態０）及びカーブ手前（状態１）への判定フロー図。 道路近似曲線によるカーブ終了判定条件の説明図。 道路近似曲線によるカーブ連続判定条件の説明図。 実施の形態７における道路形状認識装置１００の構成の一部の一例を示すブロック図。 道路近似曲線の正誤判定方式の一例の説明図。 道路近似曲線の初期化方式の一例の説明図。 実施の形態８における道路形状推定システム８００の全体構成の一例を示すシステム構成図。

道路形状認識装置（道路形状推定装置）は、車両に搭載されているレーダ装置を用いて、車両前方を監視するに際して、車両前方の道路形状を認識する。

車両用前方監視装置は、車両に搭載し、光波・ミリ波などの送信波を車両前方の所定角度に照射して、その反射波を受信することによって、自車両の前方を走行する車両や、自車両の前方の障害物を認識する。車両用前方監視装置は、検出した物体の中から、自車両と同一の車線を走行する先行車両を選択する必要がある。このとき、自車より遠距離で前方を走行する先行車両を選択するためには、自車両が走行している道路形状を正確に認識することが必要である。
道路形状を認識する方式としては、操舵角センサやヨーレートセンサを用いて道路曲率を算出する方式がある。しかし、この方式で得られる道路曲率は自車両の走行位置近辺におけるものであるため、たとえば自車は道路の直線部分を走行しているが、道路が遠距離前方でカーブしており、カーブ部分を先行車両が走行している場合には、正しく先行車両を選択できない。また、操舵の揺れや横風などの影響を受けやすい。そのため、道路縁の停止物から道路形状を認識する。

例えば、道路形状を半径Ｒの円とみなして停止物の位置と速度より予測したＸ軸（自車進行方向に対して横方向に相当する軸）との交差点と現在位置を結ぶことで道路形状を認識する。しかし、実際の道路走行環境では停止物としてマルチパス波による不要信号が観測される場合があり、これによって道路形状を誤認識する可能性がある。また、停止物のレーダ検出位置には観測誤差が生じるため、上記観測誤差によって道路形状の精度が劣化する。

また、例えば、左右道路形状を同じとみなして、各時刻で得られた複数の停止物観測値を用いて最小二乗法によって近似曲線の係数を推定することで道路形状を認識する。しかし、左右道路形状が同じであることが前提であるため、分岐路などのように道路形状が左右で異なる場面では正確に形状を推定できない。また、近似曲線係数の時系列データを追尾フィルタに入力し、係数を平滑化する場合、係数の観測誤差を定義することは困難であり、係数を正確に求めることはできない。

以下に説明する道路形状推定装置は、不要信号を含む停止物の中から時間方向に連続性のある確かな停止物を抽出し、近似曲線係数を算出することで、不要信号環境下でも安定して道路形状が推定できる。さらに、上記近似曲線係数を初期値として追尾フィルタへ全停止物の位置を直接入力し、近似曲線係数を推定することによって、前記曲線係数の推定精度がより一層向上する。

実施の形態１．
実施の形態１について、図１〜図１３を用いて説明する。

図１は、この実施の形態における道路形状認識装置１００の構成を示すブロック図である。
道路形状認識装置１００は、例えば、レーダ１１０と、車速センサ１２０と、停止物識別部１３０と、道路近似曲線仮計算部１４０と、道路縁判定部１５０と、道路近似曲線本計算部１６０と、近似曲線選択部１７０とを有する。
レーダ１１０（周辺物体観測装置）は、車両前方の所定角度範囲内にレーダ波を照射し、物体に反射されたレーダ波を受信して、その物体の位置及び相対速度を検出する。
車速センサ１２０（移動速度観測装置）は、車両の速度を検出する。
停止物識別部１３０（静止物体判定部）は、レーダ１１０により検出された物体の相対速度と、車速センサ１２０により検出された車両の速度に基づいて当該物体が停止物体であるか否かを識別する。停止物識別部１３０は、停止物（の位置）などを出力する。
道路近似曲線仮計算部１４０（初期値算出部）は、停止物識別部１３０により停止物体であると識別された複数の停止物の中から追尾フィルタによって時間方向に相関のある有効停止物を抽出し、有効停止物の時系列データを用いて有効近似曲線係数を推定する。道路近似曲線仮計算部１４０は、有効近似曲線係数、有効停止物の推定位置や速度、相関カウンタＣｎｔなどを出力する。
道路縁判定部１５０は、有効近似曲線のＸ方向（車軸方向）切片より当該道路縁への振り分けを行う。道路縁判定部１５０は、有効近似曲線係数、有効停止物の推定位置や速度、道路縁識別フラグなどを出力する。
道路近似曲線本計算部１６０（近似曲線算出部）は、道路縁判定部１５０により当該道路縁へ振り分けられた有効停止物の有効近似曲線係数を初期値とした追尾フィルタを用いて、停止物識別部１３０により停止物体であると識別された全停止物を入力として、近似曲線係数を更新（推定）する。道路近似曲線本計算部１６０は、近似曲線係数の推定値や相関停止物（の位置）などを出力する。
近似曲線選択部１７０は、道路近似曲線本計算部１６０にて算出した各道路縁の近似曲線とその近似曲線と相関のとれた停止物を用いて、最終的に先行車判定に用いる近似曲線係数推定値を算出する。近似曲線選択部１７０は、近似曲線係数の推定値や補正用係数初期値などを出力する。

次に、動作を説明する。

図２は、この実施の形態における停止物の追尾処理を説明するための図である。
道路近似曲線仮計算部１４０では、この図のように追尾フィルタ（カルマンフィルタやα−β（−γ）フィルタなどの公知のフィルタ）によって複数の停止物の中から時間方向に相関のとれる有効な停止物を抽出する。この図は、時刻ｔ_１〜ｔ_４まで時間方向に連続して相関のとれた停止物７１１〜７１４の様子を表している。ただし、相関データは多いほど道路の近似精度が向上するが、道路を認識するまでに要する時間が大きくなるというトレードオフがあるため、道路近似曲線仮計算部１４０は、あらかじめ定めた回数Ｃｎ分、相関がとれた前記有効停止物７１１〜７１４を用いて道路近似曲線７２０を推定する。また、前記近似曲線７２０を推定する方法としては最小二乗法やハフ変換などの公知の技術を用いても良いものとする。近似曲線７２０は、２次、３次以上の高次の曲線、円、クロソイド曲線などとしても良いこととする。

図３は、この実施の形態における道路近似曲線仮計算部１４０の一例（一括処理）のブロック図である。
道路近似曲線仮計算部１４０は、例えば、追尾フィルタ１４３と、相関データ蓄積部１４８と、近似曲線推定部１４９とを有する。追尾フィルタ１４３は、相関部１４４と、平滑部１４５と、予測部１４６と、遅延部１４７とを有する。
道路近似曲線仮計算部１４０の相関部１４４は、停止物の相関を取る。
道路近似曲線仮計算部１４０の平滑部１４５は、相関の取れた停止物の位置や速度を平滑して、停止物の位置や速度を推定する。
道路近似曲線仮計算部１４０の近似曲線推定部１４９は、ある停止物の時系列データを蓄積して、時系列データより、最小二乗法を用いて近似曲線を既定する係数を推定する。

図４は、この実施の形態における道路近似曲線仮計算部１４０の処理フロー図である。
道路近似曲線仮計算部１４０は、この図の処理フローのように、追尾フィルタで相関がある停止物がある場合は相関カウンタＣｎｔを１ずつ増やし、閾値Ｃｎを超えた時点で、蓄積した有効停止物の位置を用いて有効近似曲線係数を推定する。

なお、道路近似曲線仮計算部１４０は、相関カウンタＣｎｔの値に関わらず、移動体から停止物までの距離が所定の閾値より近くなった場合に、その停止物について蓄積した位置を用いて有効近似曲線係数を推定する構成であってもよい。あるいは、道路近似曲線仮計算部１４０は、相関カウンタＣｎｔの値が閾値Ｃｎを超えるか、移動体から停止物までの距離が閾値より近くなるか、どちらか一方でも条件を満たした場合に、その停止物について蓄積した位置を用いて有効近似曲線係数を推定する構成であってもよい。

図５は、この実施の形態における道路近似曲線仮計算部１４０の別の例（逐次処理）のブロック図である。
道路近似曲線仮計算部１４０は、例えば、相関部１４４と、平滑部１４５と、予測部１４６と、遅延部１４７とを有する。
道路近似曲線仮計算部１４０は、拡張カルマンフィルタによる逐次処理によって、近似曲線の係数と停止物の位置や速度を同時推定する。
この図のように、逐次処理によって有効近似曲線の係数を推定することが可能となるため、データを保存するメモリのサイズを少なくすることが可能となる。

以降、説明の都合上、近似曲線を２次曲線と仮定する。例えば、式（１）のように近似曲線を傾きａ、Ｘ切片ｂの２次曲線として、拡張カルマンフィルタの観測モデルを式（２）〜（５）で定義する。

ここで、ｘ，ｙは、自車レーダを原点、車軸方向をＸ軸、進行方向をＹ軸とした相対座標系における停止物の位置を意味する。また、状態ベクトルＸ_ｋは、停止物の位置ｘ，ｙと速度ｘドット（ｘの上に点。以下、同様。），ｙドット、２次曲線の係数ａ，ｂで構成される。ｚ_ｏｋは停止物の観測位置、ｈ（Ｘ_ｋ）は停止物の真の位置、ｖ_ｋは観測雑音ベクトルとする。すると、毎時刻、状態ベクトルＸ_ｋの推定値が更新されるため、有効近似曲線の係数ａ，ｂが算出できる。

前記道路近似曲線仮計算部１４０は、前記有効停止物の最新時刻の推定位置・速度と前記有効近似曲線の係数（と相関カウンタＣｎｔ）を道路縁判定部１５０へ出力する。

図６は、この実施の形態における道路縁判定部１５０による道路縁判定の説明図である。
道路縁判定部１５０は、有効停止物より算出した近似曲線のＸ方向（車軸方向）切片と車線幅より当該道路縁へ振り分けを行う。例えば、道路縁判定部１５０では、前記有効近似曲線係数を入力として、その近似曲線のＸ軸との交差位置（Ｘ切片ｂ）を用いて当該道路縁へ分類する。分類方法としては、最初に自車の幅を考慮して自車位置（原点）より閾値ＴＨｃ以内に近似曲線のＸ切片ｂが入る場合は除外する。そして、この図のように、車線幅ＴＨｗを考慮して分類する。

例えば、この図のＲ１の道路縁に振り分けられる近似曲線の係数ｂは式（６）を満たす。Ｒ２の道路縁へ振り分けられる近似曲線の係数ｂは式（７）を満たす。例えば、式（６）を満たす場合は道路縁識別フラグをＲ１、式（７）を満たす場合はＲ２と設定する。道路縁判定部１５０では、前記有効停止物の推定位置・速度、前記有効近似曲線係数、その道路縁識別フラグを道路近似曲線本計算手段へ出力する。

例えば、ｉ番目の有効停止物については、式（８）のように最新時刻における有効停止物の推定位置ｘ，ｙ、推定速度ｘドット，ｙドット、有効近似（２次）曲線の係数ａ，ｂ、相関カウンタＣｎｔ、道路縁識別フラグｆｌａｇＲを道路近似曲線本計算部１６０へ出力する。

図７は、この実施の形態における道路近似曲線本計算部１６０のブロック図である。
道路近似曲線本計算部１６０は、例えば、初期値設定部１６１と、グルーピング処理部１６２と、追尾フィルタＡ１６３とを有する。

道路近似曲線本計算部１６０は、同一道路縁へ振り分けられた複数の前記有効停止物の中から時間方向の相関回数が最も高い前記有効停止物の前記有効近似曲線の係数を追尾フィルタの初期値として設定する。例えば、道路近似曲線本計算部１６０の初期値設定部１６１では、前記有効近似曲線係数と、その道路縁識別フラグを入力とし、有効停止物ごとの道路縁識別フラグより、有効停止物が同一道路縁に複数存在する場合は、有効近似曲線係数の平均値を後述の追尾フィルタの初期値として設定する。または、相関カウンタＣｎｔ値の最も大きい停止物より推定した有効近似曲線係数を初期値として設定しても良い。

さらに、道路近似曲線本計算部１６０は、有効停止物ごとの道路縁識別フラグと有効停止物の推定位置より、有効停止物周辺のあらかじめ定めた閾値範囲内に入る停止物をグルーピングし、同一グループとみなされた停止物を当該道路縁へ振り分ける。例えば、道路近似曲線本計算部１６０のグルーピング処理部１６２では、前記道路縁判定部１５０より出力された有効停止物ごとの道路縁識別フラグと有効停止物の推定位置や速度より、有効停止物周辺のあらかじめ定めた閾値範囲内に入る停止物をグルーピングし、同一グループとみなされた停止物を当該道路縁へ振り分ける。

図８は、この実施の形態における道路近似曲線本計算部１６０の追尾フィルタＡ１６３のブロック図である。
追尾フィルタＡ１６３は、例えば、相関部１６４と、平滑部１６５と、予測部１６６と、遅延部１６７とを有する。

道路近似曲線本計算部１６０は、全停止物を入力し、追尾フィルタの相関処理によって、当該道路縁と相関する停止物を抽出し、前記相関停止物を用いて追尾フィルタによって近似曲線係数を算出する。道路近似曲線本計算部１６０は、全停止物の中から道路縁へ割り当て済みの停止物については追尾フィルタの相関処理を介さずに、当該道路縁の近似曲線係数を推定する追尾フィルタへ直接入力する。例えば、道路近似曲線本計算部１６０の追尾フィルタＡ１６３では、この図のようにいずれかの道路縁へ割り当て済みの停止物については、平滑部１６５に直接入力することで当該道路縁の近似曲線の係数推定値を更新する。また、道路縁へ割り当てられなかった停止物については、相関部１６４にて、近似曲線と相関をとることとする。ある道路縁へ相関済みの停止物については、当該道路縁の近似曲線係数推定値を更新する。例えば、下記の式（９）〜（１１）で定義した観測モデルに基づくカルマンフィルタを用いて係数ａ，ｂを更新する。

状態ベクトルＸ_ｋは、２次曲線の係数ａ，ｂで構成する。ｘ_ｏｋは停止物のＸ方向の観測位置、Ｈは観測行列、ｖ_ｘ，ｋは観測雑音ベクトル、Ｆは状態遷移行列、ｗ_ｘ，ｋは駆動雑音ベクトルとする。追尾フィルタとしては、演算負荷の軽いα−βフィルタを用いても良いこととする。

なお、状態ベクトルＸ_ｋに、係数ａ，ｂの変化率ａドット，ｂドットを加える構成であってもよい。

また、道路近似曲線本計算部１６０は、同一道路縁へ振り分けられた複数の前記有効近似曲線の係数を平均化した値を追尾フィルタの初期値として設定する。例えば、道路縁の近似曲線係数の初期値は、初期値設定部１６１から出力される係数初期値を用いる。各道路縁の近似曲線係数推定に用いた停止物を相関停止物と呼び、道路縁ごとの近似曲線係数推定値と前記相関停止物を近似曲線選択部１７０へ出力する。

図９は、この実施の形態における近似曲線選択部１７０のブロック図である。
近似曲線選択部１７０は、有効性判定部１７１と、曲線品質計算部１７２と、選択部１７３と、補正部１７４とを有する。
近似曲線選択部１７０の有効性判定部１７１（信頼度算出部）では、下記３条件を全て満たす近似曲線係数推定値の有効フラグを１、一つでも満たさない係数推定値がある場合は有効フラグを０と設定する。

条件（１）：相関停止物のｙ方向距離最大値ｙ_ｏ ^ｍａｘが閾値ｙ_ｍａｘ以上。
条件（２）：相関停止物のｙ方向の距離分布ｙ_ｏ ^ｄｉｖが閾値ｙ_ｄｉｖ以上。
条件（３）：相関停止物数Ｍが所定値Ｍ_ｍｉｎ以上。

ここで、相関停止物の最小値をｙ_ｏ ^ｍｉｎ、最大値をｙ_ｏ ^ｍａｘとすると、ｙ_ｏ ^ｄｉｖを式（１２）で定義する。

有効性判定部１７１は、有効フラグ１の近似曲線係数推定値とその相関停止物を曲線品質計算部１７２へ出力し、全ての近似曲線係数推定値と有効フラグを補正部１７４へ出力する。

図１０は、この実施の形態における近似曲線選択部１７０の近似曲線選択フロー図である。
近似曲線選択部１７０は、道路縁の近似曲線と相関する停止物の自車進行方向（Ｙ方向）の距離と相関停止物数を用いて有効性を判断して、有効性がある近似曲線の係数のみを出力し、有効性がある近似曲線が存在しない場合は前時刻の近似曲線係数を出力する。

近似曲線選択部１７０の曲線品質計算部１７２（信頼度算出部）では、有効フラグが１となる道路縁が複数ある場合は、後述の曲線品質値を計算し、前記曲線品質値が最も高い道路縁の近似曲線係数を出力する。

図１１は、道路縁ごとの相関停止物の違いを説明するための図である。
この図のように、多くの停止物と相関するものの相関停止物が局在する曲線は、推定値が安定しない。これに対し、安定している曲線は、相関停止物がＹ方向距離に満遍なく分布している。

図１２は、この実施の形態における曲線品質計算部１７２におけるカーブ品質値算出方式の一例を説明するための図である。
道路縁Ａのほうが道路縁Ｂよりも相関停止物の数は多い。しかし、道路縁Ａの相関停止物は、１０ｍ付近に固まっている。これに対し、道路縁Ｂの相関停止物は、１０ｍ、２０ｍ、５０ｍ、９０ｍと広い範囲に満遍なく分布している。このため、道路縁Ｂのほうが道路縁Ａよりも近似曲線係数の推定値が安定し、信頼性が高い。
このように、停止物がＹ方向に満遍なく分布するほど推定値が安定する特徴を活かして、曲線品質計算部１７２は、この図のように、Ｙ方向の分布を考慮して、品質値を算出することとする。

近似曲線選択部１７０は、道路縁の近似曲線と相関する停止物の自車進行方向（Ｙ方向）の距離を用いてクラスタに分けて、クラスタ数を曲線品質値として曲線品質値の高い道路縁の近似曲線の傾きもしくは道路曲率を出力する。具体的には、曲線品質計算部１７２は、まず一定間隔（例えば１０ｍ）でＹ方向距離を区切って、そのＹ方向距離に近い停止物を一つのクラスタとしてまとめる。例えば、相関停止物が４つあり、それら停止物のＹ方向距離が８ｍ、１２ｍ、１８ｍ、２２ｍであった場合は、１０ｍと２０ｍの２つのクラスタに割り当てられる。Ｙ方向距離が８ｍ、１８ｍ、３２ｍであった場合は、１０ｍ、２０ｍと３０ｍの３つのクラスタに割り当てられる。その後、曲線品質計算部１７２は、道路縁ごとにクラスタ数をカウントし、一定期間（Ｌサンプル）積算した値を曲線品質値として計算する。曲線品質値の算出式は式（１３）のようになる。

ここで、Ｎ_ｃはクラスタ総数、Ｍ_ｋ（ｊ）はある時刻ｔ_ｋで計算したクラスタｊにおけるフラグを表し、式（１４）のように停止物がある場合は１、ない場合は０とする。

図１３は、この実施の形態における曲線品質計算部１７２におけるカーブ品質値算出方式の別の例を説明するための図である。
また、曲線品質計算部１７２は、近似曲線の係数もしくはその係数を道路曲率へ変換した値の時間変化により、品質値を算出する構成であってもよい。実際の道路は車両の運転を考慮しており、カーブ進入時では道路曲率が徐々に増してゆくように設計されている。このため、この図の左側の道路縁のように道路曲率の変化率が大きい場合は推定値が安定していないとみなすことができ、右側の道路縁のように近似曲線の傾きが徐々に変化する場合は安定しているとみなすことができる。
近似曲線選択部１７０は、道路縁の近似曲線の係数推定値もしくは道路曲率推定値の時間変化の分散の逆数を曲線品質値として曲線品質値の高い道路縁の近似曲線傾きもしくは道路曲率を出力する。例えば、曲線品質計算部１７２は、前記道路曲率の変化率の時系列データを蓄積し、その分散の逆数を品質値として計算する。
曲線品質計算部１７２で算出した曲線品質値は、選択部１７３へ出力される。

近似曲線選択部１７０の選択部１７３では、最も高い曲線品質値を持つ近似曲線係数推定値を選択し、係数のうちＸ切片（道路幅の推定値に相当する）以外の値を補正部１７４へ出力する。例えば式（１）の２次曲線の場合、選択部１７３は、傾きａのみを出力する。また、選択部１７３は、傾きａを道路の曲率（Ｃｕｒｖａｔｕｒｅ）へ変換して出力しても良い。Ｘ切片ｂは道路幅に相当するため、他の道路縁を補正する際は必要ない。

近似曲線選択部１７０は、有効性があり、曲線品質値が最も高い近似曲線の係数を初期値として、有効性がない近似曲線の係数を初期化する。例えば、近似曲線選択部１７０の補正部１７４では、近似曲線選択部１７０の選択部１７３から出力される近似曲線係数推定値と有効性判定部１７１から入力される全ての道路縁の有効フラグを用いて、有効フラグが０の道路縁の近似曲線推定値を初期化する。有効フラグ０の道路縁の近似曲線係数初期化のために、最も高い曲線品質値を持つ道路縁の近似曲線係数を初期値として用いる。補正部からは補正用の係数初期値を道路近似曲線本計算部１６０へ出力する。

このように、不要信号を含む停止物の中から時間方向に連続性のある確かな停止物を抽出し、近似曲線係数を算出することで、不要信号環境下でも安定して道路形状が推定できる。さらに、上記近似曲線係数を初期値として追尾フィルタへ全停止物の位置を直接入力し、近似曲線係数を推定することによって、前記曲線係数の推定精度がより一層向上する効果が得られる。

実施の形態２．
実施の形態２について、図１４を用いて説明する。
なお、実施の形態１と共通する部分については、同一の符号を付し、説明を省略する。

図１４は、この実施の形態における道路近似曲線仮計算部１４０の構成を示すブロック図である。
道路近似曲線仮計算部１４０は、実施の形態１で説明した構成に加えて、停止物クラスタリング部１４１を有する。
道路近似曲線仮計算部１４０は、停止物をクラスタリングし、クラスタを追尾する。

次に、動作を説明する。
実際の道路走行環境下では視線誘導標（デリニエータ）、ガードレール、壁などの停止物からの反射がある。特に、ガードレールや壁などの表面積の広い停止物からは複数の反射点が得られ、反射点１点を追尾することが困難な場合がある。そのため、この図のように、道路近似曲線仮計算部１４０の追尾フィルタ１４３の前段で停止物のクラスタリングを実施することで、表面積の大きい停止物から得られる複数反射点をまとめ、そのクラスタを追尾することで道路近似曲線仮計算部１４０の相関性能を高める。
その他の処理は、実施の形態１と同様のため省略する。

実施の形態３．
実施の形態３について、図１５〜図１６を用いて説明する。
なお、実施の形態１〜実施の形態２と共通する部分については、同一の符号を付し、説明を省略する。

図１５は、この実施の形態における近似曲線選択部１７０の構成を示すブロック図である。
近似曲線選択部１７０は、実施の形態１で説明した構成に加えて、平滑部１７５と、係数推定値蓄積部１７６とを有する。
近似曲線選択部１７０は、前時刻の近似曲線の傾きと現時刻で選択された近似曲線の傾きを用いて近似曲線の係数を平滑化する。

次に、動作を説明する。
図１６は、近似曲線の選択問題を説明するための説明図である。
左右の道路縁の近似曲線の傾きが大きく異なる場合、交互に選択すると、傾きが急激に変動することになる。近似曲線選択部１７０の選択部１７３では、各時刻で複数の近似曲線の中からある一つの道路縁の近似曲線係数を選択するため、当該道路縁の曲率に相当する係数の値（式（１）では傾きａ）が道路縁間で異なる場合、時間方向に急激に変化する可能性がある。
そのため、近似曲線選択部１７０の選択部１７３では、新たに平滑部１７５と係数推定値蓄積部１７６を設ける。平滑部１７５は、推定値が急激に変化することが無いように前時刻の係数を用いて平滑化する。平滑化に既存の追尾フィルタを用いる。

その他の処理は、実施の形態１もしくは実施の形態２と同様のため省略する。

実施の形態４．
実施の形態４について、図１７〜図２８を用いて説明する。
なお、実施の形態１〜実施の形態３と共通する部分については、同一の符号を付し、説明を省略する。

図１７は、この実施の形態における道路形状推定装置２００のハードウェア構成の一例を示すハードウェア構成図である。
道路形状推定装置２００は、例えば、処理装置９１１と、記憶装置９１４と、入力装置９０２と、出力装置９０１とを有するコンピュータである。
処理装置９１１は、コンピュータプログラムを実行することにより、データを処理し、道路形状推定装置２００全体を制御する。
記憶装置９１４は、処理装置９１１が実行するコンピュータプログラムや、処理装置９１１が処理するデータなどを記憶する。記憶装置９１４は、例えば、揮発性メモリ、不揮発性メモリ、フラッシュメモリ、磁気ディスク装置、光学ディスク装置などである。
入力装置９０２は、道路形状推定装置２００の外部から信号や情報を入力し、処理装置９１１が処理できる形式のデータに変換する。入力装置９０２が変換したデータは、処理装置９１１が直接処理してもよいし、記憶装置９１４が一時的に記憶してもよい。入力装置９０２は、例えば、キーボードやマウスなど利用者の操作を入力する操作入力装置、アナログ信号をデジタルデータに変換するアナログデジタル変換装置などの変換装置、他の装置が送信した信号を受信する受信装置などのインターフェイス装置などがある。
出力装置９０１は、処理装置９１１が処理したデータや記憶装置９１４が記憶したデータを、道路形状推定装置２００の外部に出力できる形式に変換して出力する。出力装置９０１は、例えば、画像を表示する表示装置やスピーカーなど人の五感で感知できる形式にデータを変換して出力する装置、デジタルデータをアナログ信号に変換するデジタルアナログ変換装置などの変換装置や、他の装置に対して信号を送信する送信装置などのインターフェイス装置など、他の装置に入力できる形式にデータを変換して出力する装置などがある。

道路形状推定装置２００の機能ブロックは、記憶装置９１４が記憶したコンピュータプログラムを処理装置９１１が実行することにより実現される。なお、道路形状推定装置２００の機能ブロックは、必ずしもコンピュータにより実現される構成でなくてもよく、デジタル回路やアナログ回路などの電子回路によって実現される構成であってもよいし、機械的構成など電気的構成以外の構成によって実現される構成であってもよい。
また、実施の形態１〜実施の形態３で説明した道路形状認識装置１００の機能ブロックも、同様に、コンピュータにより実現される構成であってもよいし、その他の構成により実現される構成であってもよい。

図１８は、この実施の形態における道路形状推定装置２００の機能ブロックの構成の一例を示すブロック構成図である。
道路形状推定装置２００は、自動車などの移動体に搭載されている。移動体には、道路形状推定装置２００のほか、周辺物体観測装置８１１、移動速度観測装置８２１などが搭載されている。

周辺物体観測装置８１１は、移動体の周辺に存在する物体の位置や速度などを観測する。周辺物体観測装置８１１は、例えば、レーダである。例えば、周辺物体観測装置８１１は、電波やレーザ光などの放射波を放射して物体に当たって反射した反射波を検知することにより、伝搬時間を測定し、物体までの距離を算出する。周辺物体観測装置８１１は、放射波を放射する方向もしくは反射波を受信する方向を走査するなどして、物体が存在する方向を判定する。周辺物体観測装置８１１が放射波を１回走査するのにかかる時間は、例えば０．２秒である。したがって、移動体と物体との間の相対位置関係が変化しない場合、周辺物体観測装置８１１は、同一の物体を、例えば０．２秒ごとに繰り返し観測する。周辺物体観測装置８１１は、算出した距離と、判定した方向とに基づいて、移動体に対する物体の相対位置を算出する。また、周辺物体観測装置８１１は、例えば、ドップラー効果による放射波と反射波との間の波長のずれを検出することにより、移動体に対する物体の相対速度を検出する。周辺物体観測装置８１１は、物体の相対位置や相対速度などの観測結果を表わす信号を出力する。

移動速度観測装置８２１は、移動体の移動速度を観測する。移動速度観測装置８２１は、例えば、自動車の車軸に取り付けた回転数計により車軸の回転数を測定し、移動速度を算出する。移動速度観測装置８２１は、例えば０．１秒の間の車軸の回転数を計数する。移動速度観測装置８２１は、計数した回転数にタイヤの一周の長さを乗じることにより、０．１秒の間に移動体が進んだ距離を算出する。移動速度観測装置８２１は、算出した距離を１０倍することにより、移動体の秒速を算出する。この例の場合、移動速度観測装置８２１は、０．１秒に１回ずつ繰り返し移動体の移動速度を観測する。移動速度観測装置８２１は、移動体の移動速度を観測した観測結果を表わす信号を出力する。
なお、移動速度観測装置８２１は、移動体の移動速度を算出せず、測定した車軸の回転数をそのまま表わす信号を出力する構成であってもよい。その場合、後述する移動速度取得部２１２が、移動速度観測装置８２１に代わって、上述した計算を実行し、移動体の移動速度を算出する。

道路形状推定装置２００は、周辺物体観測装置８１１や移動速度観測装置８２１の観測結果に基づいて、移動体が走行している道路の形状を推定する。道路形状推定装置２００は、相対位置取得部２１１と、移動速度取得部２１２と、静止物体判定部２２０と、物体相関部２３０と、近似曲線算出部２４０とを有する。

相対位置取得部２１１は、入力装置９０２を用いて、周辺物体観測装置８１１が出力した信号を入力し、周辺物体観測装置８１１が観測した観測結果を取得する。
相対位置取得部２１１は、処理装置９１１を用いて、周辺物体観測装置８１１が出力した信号を入力した時刻を判定し、周辺物体観測装置８１１が物体を観測した観測時刻を算出する。相対位置取得部２１１は、信号を入力した時刻をそのまま観測時刻とみなす構成であってもよいし、周辺物体観測装置８１１における遅延時間を考慮して、信号を入力した時刻よりも遅延時間だけ戻った時刻を観測時刻とする構成であってもよい。なお、周辺物体観測装置８１１が出力する信号に、周辺物体観測装置８１１が物体を観測した観測時刻を表わす情報が含まれる構成であってもよい。その場合、相対位置取得部２１１は、処理装置９１１を用いて、入力した信号から観測時刻を取得する。
相対位置取得部２１１は、記憶装置９１４を用いて、周辺物体観測装置８１１が物体を観測した観測時刻や、観測された物体の相対位置および相対速度を表わすデータを記憶する。相対位置取得部２１１が記憶するデータを「周辺物体観測データ」と呼ぶ。「周辺物体観測データ」のうち、観測時刻を表わすデータを「物体観測時刻データ」、物体の相対位置を表わすデータを「相対位置観測値データ」、物体の相対速度を表わすデータを「相対速度観測値データ」と呼ぶ。

移動速度取得部２１２は、入力装置９０２を用いて、移動速度観測装置８２１が出力した信号を入力し、移動速度観測装置８２１が観測した観測結果を取得する。移動速度取得部２１２は、処理装置９１１を用いて、相対位置取得部２１１と同様の処理をして、移動速度観測装置８２１が移動体の移動速度を観測した観測時刻を求める。移動速度取得部２１２は、記憶装置９１４を用いて、移動速度観測装置８２１が移動体の移動速度を観測した観測時刻や、観測された移動体の移動速度を表わすデータを記憶する。移動速度取得部２１２が記憶するデータを「移動速度データ」と呼ぶ。「移動速度データ」のうち、観測時刻を表わすデータを「移動速度観測時刻データ」、移動体の移動速度を表わすデータを「移動速度観測値データ」と呼ぶ。

静止物体判定部２２０は、処理装置９１１を用いて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。静止物体判定部２２０は、記憶装置９１４を用いて、判定した判定結果を表わすデータを記憶する。静止物体判定部２２０が記憶するデータを「静止判定結果データ」と呼ぶ。
例えば、静止物体判定部２２０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データと、移動速度取得部２１２が記憶した移動速度データとを入力する。静止物体判定部２２０は、処理装置９１１を用いて、入力した周辺物体観測データから、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度を表わす相対速度観測値データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、入力した移動速度データから、移動体の移動速度を表わす移動速度観測値データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、相対速度観測値データが表わす相対速度と、移動速度観測値データが表わす移動速度とに基づいて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。

図１９は、移動体８０１の移動速度と、物体７０１，７０２の移動体８０１に対する相対速度との関係を説明するための図である。
矢印７１１，７１３は、それぞれ、移動体８０１，物体７０２の移動速度を表わす。物体７０１は、静止しているものとする。矢印７２１，７２２は、それぞれ、物体７０１，７０２の移動体８０１に対する相対速度を表わす。
物体７０１は静止しているので、移動体８０１を基準にすると、物体７０１は、移動体８０１の移動速度とちょうど逆向きの方向に、移動体８０１の移動速度とちょうど同じ速度で移動しているように見える。これに対して、物体７０２は移動しているので、移動体８０１を基準にすると、物体７０２は、移動体８０１の移動による見かけの速度（矢印７２３）と、物体７０２の移動による実際の速度（矢印７１３）とを合成した速度で移動しているように見える。したがって、物体の移動体に対する相対速度（見かけの速度）は、次の式で表わすことができる。

ただし、ｖ_ｒは、物体の移動体に対する相対速度を表わすベクトルである。ｖ_ｔは、物体の移動速度を表わすベクトルである。ｖ_ｓは、移動体の移動速度を表わすベクトルである。

例えば、静止物体判定部２２０は、処理装置９１１を用いて、物体の移動体に対する相対速度を表わすベクトルｖ_ｒと、移動体の移動速度を表わすベクトルｖ_ｓとを加算して、物体の移動速度を表わすベクトルｖ_ｔを算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出したベクトルｖ_ｔの絶対値を算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出した絶対値を所定の閾値と比較する。閾値は、物体の移動体に対する相対速度や移動体の移動速度の観測誤差を考慮してあらかじめ設定しておく。ベクトルｖ_ｔの絶対値が閾値より小さい場合、静止物体判定部２２０は、処理装置９１１を用いて、その物体が静止していると判定する。

なお、ドップラー効果を利用して物体の移動体に対する相対速度を観測する場合、観測される相対速度は、物体の見かけの速度（矢印７２１，７２２）のうち、物体と移動体との距離方向の成分（矢印７２５，７２６）だけであり、移動体８０１を軸とする回転方向の成分（矢印７２７，７２８）は観測されない。観測される相対速度は、次の式で表わすことができる。

ただし、ｖ_ｄは、物体の移動体に対する相対速度の距離方向成分を表わす実数である。αは、物体の移動体に対する相対速度の方向と、物体から見た移動体が見える方向との間の角度を表わす実数である。
物体の移動体に対する相対速度のうち、移動体８０１を軸とする回転方向の成分（矢印７２７，７２８）が観測されないので、角度αは未知である。しかし、物体７０１は静止しているので、物体７０１の角度α（７３３）は、移動体８０１の移動方向と、移動体８０１から見た物体７０１が見える方向との間の角度７３１と一致する。これに対し、物体７０２は移動しているので、物体７０２の角度α（７３４）は、移動体８０１の移動方向と、移動体８０１から見た物体７０２が見える方向との間の角度７３２と必ずしも一致しない。

例えば、静止物体判定部２２０は、処理装置９１１を用いて、移動体の移動方向と、移動体から見た物体が見える方向との間の角度の余弦（コサイン）を算出する。静止物体判定部２２０は、処理装置９１１を用いて、移動体の移動速度を表わすベクトルｖ_ｓの絶対値と、算出した余弦との積を算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出した積から、物体の移動体に対する相対速度の距離方向成分を表わす実数ｖ_ｄを差し引いた差を算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出した差を、所定の閾値と比較する。閾値は、物体の移動体に対する相対速度や移動体の移動速度の観測誤差を考慮してあらかじめ設定しておく。差が閾値より小さい場合、静止物体判定部２２０は、処理装置９１１を用いて、その物体が静止していると判定する。

図２０は、物体の移動体に対する相対速度の観測時刻７８１〜７８３と、移動体の移動速度の観測時刻７９１〜７９９との関係を説明するための図である。
上述したように、周辺物体観測装置８１１が物体の移動体に対する相対位置や相対速度を観測する観測周期７７１と、移動速度観測装置８２１が移動体の移動速度を観測する観測周期７７２とは、異なっている。また、周辺物体観測装置８１１が物体の移動体に対する相対位置や相対速度を観測する観測時刻７８１〜７８３は、物体が存在する方向を走査した時刻であるから、物体が存在する方向が変われば、１回の観測周期のなかでのタイミングが変化し、不定期である。これに対し、移動速度観測装置８２１が移動体の移動速度を観測する観測時刻７９１〜７９９の間隔は、一定であり、観測周期７７２と等しい。
したがって、物体の移動体に対する相対速度の観測時刻７８１〜７８３と、移動体の移動速度の観測時刻７９１〜７９９とが一致する可能性はほとんどない。しかし、物体が静止しているか否かを静止物体判定部２２０が判定するために用いる物体の移動体に対する相対速度と、移動体の移動速度とは、同じ時刻に観測したものである必要がある。

車軸の回転数を計数することにより移動体の移動速度を観測する場合、観測される移動速度は、計数を開始した時刻から計数を終了した時刻までの移動速度の平均値である。例えば、時刻７９３に観測した移動速度は、時刻７９２から時刻７９３までの移動速度の平均値である。したがって、静止物体判定部２２０は、時刻７８１に観測した物体の移動体に対する相対速度を、時刻７９３に観測した移動体の移動速度と比較すればよい。

例えば、静止物体判定部２２０は、処理装置９１１を用いて、移動速度観測装置８２１の観測結果を移動速度取得部２１２が取得するたびに処理を実行する。静止物体判定部２２０は、処理装置９１１を用いて、移動速度取得部２１２が記憶した今回の移動速度データと、１つ前の移動速度データとを入力する。静止物体判定部２２０は、処理装置９１１を用いて、入力した２つの移動速度データから、移動速度観測装置８２１が移動体の移動速度を観測した今回の観測時刻と前回の観測時刻とをそれぞれ表わす２つの移動速度観測時刻データを取得する。
次に、静止物体判定部２２０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、未処理の周辺物体観測データを入力する。静止物体判定部２２０は、処理装置９１１を用いて、入力した周辺物体観測データから、周辺物体観測装置８１１が物体を観測した観測時刻を表わす物体観測時刻データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した物体観測時刻データが表わす観測時刻と、取得した２つの移動速度観測時刻データが表わす観測時刻とを比較する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した前回の観測時刻より前である場合は、移動速度観測装置８２１が前回観測した移動体の移動速度を使って、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。静止物体判定部２２０は、処理装置９１１を用いて、入力した１つ前の移動速度データから、移動速度観測装置８２１が観測した移動速度を表わす移動速度観測値データを取得する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した前回の観測時刻より後で、かつ、今回の観測時刻より前である場合は、移動速度観測装置８２１が今回観測した移動体の移動速度を使って、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。静止物体判定部２２０は、処理装置９１１を用いて、入力した今回の移動速度データから、移動速度観測装置８２１が観測した移動速度を表わす移動速度観測値データを取得する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した今回の観測時刻より後である場合、移動速度観測装置８２１が次回観測する移動体の移動速度を使って、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。移動体の移動速度の次回の観測値はまだ得られていないので、静止物体判定部２２０は、その周辺物体観測データについて今回は処理をしない。

例えばこのようにして、静止物体判定部２２０は、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定するために使用する移動体の移動速度を、移動速度観測装置８２１が観測した観測結果のなかから選択する。あるいは、静止物体判定部２２０は、移動速度観測装置８２１が観測した観測結果に基づいて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定するために使用する移動体の移動速度を算出する構成であってもよい。

例えば、静止物体判定部２２０は、処理装置９１１を用いて、移動速度取得部２１２が記憶した移動速度データが表わす観測時刻と移動速度とに基づいて、平滑処理をする。周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも前である場合、静止物体判定部２２０は、処理装置９１１を用いて、平滑処理の結果に基づいて、周辺物体観測装置８１１が物体を観測した観測時刻における移動体の移動速度を推定する。あるいは、周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも前である場合、静止物体判定部２２０は、処理装置９１１を用いて、平滑処理の結果に基づいて、周辺物体観測装置８１１が物体を観測した観測時刻における移動体の移動速度を予測する。静止物体判定部２２０は、処理装置９１１を用いて、推定あるいは予測した移動体の移動速度を用いて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。

物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が観測した物体のうち、静止物体判定部２２０が静止していると判定した物体について、相関処理をする。すなわち、物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が物体を観測した観測結果のなかから、静止している同じ物体を複数回観測した観測結果を判定する。物体相関部２３０は、記憶装置９１４を用いて、判定した判定結果を表わすデータを記憶する。物体相関部２３０が記憶するデータを「相関結果データ」と呼ぶ。
例えば、物体相関部２３０は、処理装置９１１を用いて、静止物体判定部２２０が記憶した静止判定結果データを入力する。物体相関部２３０は、処理装置９１１を用いて、入力した静止判定結果データが表わす判定結果に基づいて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、静止物体判定部２２０が静止していると判定した物体についての周辺物体観測データであって、未処理の周辺物体観測データを入力する。物体相関部２３０は、処理装置９１１を用いて、入力した周辺物体観測データから、周辺物体観測装置８１１が物体を観測した観測時刻を表わす物体観測時刻データと、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置を表わす相対位置観測値データとを取得する。
処理済の周辺物体観測データから推定した物体の移動体に対する相対位置の軌跡がある場合、物体相関部２３０は、処理装置９１１を用いて、取得したデータが表わす観測時刻及び相対位置がその軌跡に適合するか否かを判定する。適合する軌跡が複数ある場合、物体相関部２３０は、処理装置９１１を用いて、取得したデータが表わす観測時刻及び相対位置が最もよく適合する軌跡を判定する。物体相関部２３０は、処理装置９１１を用いて、取得したデータが表わす観測時刻及び相対位置を、（最もよく）適合する軌跡に組み入れて、軌跡を更新する。
取得したデータが表わす観測時刻及び相対位置が適合する軌跡がない場合、物体相関部２３０は、処理装置９１１を用いて、適合する軌跡がない他の周辺物体観測データのなかに、新たな軌跡となり得る周辺物体観測データがあるか否かを判定する。新たな軌跡となり得る周辺物体観測データがある場合、物体相関部２３０は、処理装置９１１を用いて、新たな軌跡を生成する。
物体相関部２３０は、記憶装置９１４を用いて、生成あるいは更新した軌跡を表わすデータを記憶する。

なお、移動体の絶対位置や向きがわかっていないので、物体相関部２３０における相関処理は、物体の移動体に対する相対位置（見かけの位置）に基づいて行う。

図２１は、移動体８０１の移動軌跡７５１と、物体７０３〜７０８の移動体８０１に対する相対位置の軌跡７６１〜７６６との関係を説明するための図である。
例えば、移動体８０１が、移動軌跡７５１に示すように蛇行して移動したものとする。また、物体７０３〜７０８は、静止しているものとする。

時刻７８５において、周辺物体観測装置８１１は、物体７０３〜７０８について、移動体８０１に対する相対位置７４１ａ〜７４１ｆを観測する。
時刻７８６において、周辺物体観測装置８１１は、物体７０３〜７０８について、移動体８０１に対する相対位置７４２ａ〜７４２ｆを観測する。
時刻７８７において、周辺物体観測装置８１１は、物体７０３〜７０８について、移動体８０１に対する相対位置７４３ａ〜７４３ｆを観測する。
物体相関部２３０は、これらの観測結果に基づいて、相関処理を行い、軌跡７６１〜７６６を生成する。

このように、静止している物体７０３〜７０８の移動体８０１に対する相対位置は、移動体８０１の移動に伴って変化する。特に、移動体８０１が旋回した場合、物体７０３〜７０８が見える方向が大きく変化するので、物体７０３〜７０８の移動体８０１に対する相対位置も大きく変化する。
このため、静止している物体７０３〜７０８の移動体８０１に対する相対位置の軌跡７６１〜７６６は、このように複雑な形状になる場合がある。

周辺物体観測装置８１１が複数の静止している物体７０３〜７０８を観測した場合、それぞれの物体７０３〜７０８の移動体８０１に対する相対位置は、移動体８０１の移動や旋回に伴って大きく変化するが、物体７０３〜７０８間の位置関係は変わらない。物体７０３〜７０８の移動体８０１に対する相対位置は、移動体８０１の旋回に伴って方向が変化するだけでなく、移動体８０１の移動に伴って方向や距離が変化する。これに対して、物体７０３〜７０８は静止しているので、移動体８０１が移動や旋回をしても、物体７０３〜７０８間の距離は変わらず、移動体８０１の旋回に伴って物体７０３〜７０８間の方向が変化するだけである。

物体相関部２３０は、このことを利用し、処理装置９１１を用いて、周辺物体観測装置８１１が観測する物体の移動体８０１に対する相対位置を予測する。物体相関部２３０は、処理装置９１１を用いて、予測した結果を使って相関処理をする。

例えば、物体相関部２３０は、処理装置９１１を用いて、１回の走査周期内に周辺物体観測装置８１１が観測した複数の物体のなかから、静止している物体を１つ選択する。物体相関部２３０は、処理装置９１１を用いて、選択した物体の座標が原点に一致するよう、周辺物体観測装置８１１が今回の走査周期内に観測した複数の静止している物体の移動体８０１に対する相対位置の座標を平行移動する。また、物体相関部２３０は、処理装置９１１を用いて、前回の走査周期内に周辺物体観測装置８１１が観測した複数の物体のなかからも同様に、静止している物体を１つ選択する。物体相関部２３０は、処理装置９１１を用いて、選択した物体の座標が原点に一致するよう、周辺物体観測装置８１１が前回の走査周期内に観測した複数の静止している物体の移動体８０１に対する相対位置の座標を平行移動する。
物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が今回の走査周期内に観測した物体の相対位置を平行移動した座標を、原点を中心に回転する。物体相関部２３０は、処理装置９１１を用いて、回転した座標それぞれと、周辺物体観測装置８１１が前回の走査周期内に観測した物体の相対位置を平行移動した座標それぞれとの間の距離を算出する。算出した距離が所定の閾値より小さい場合、物体相関部２３０は、処理装置９１１を用いて、その２つの座標に対応する物体が同一の物体であるとみなし、同一の物体であるとみなせる座標の組の数を数える。
物体相関部２３０は、処理装置９１１を用いて、回転する角度を変えてこれを繰り返し、同一の物体であるとみなせる座標の組の数が最も多い角度を求める。
物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が今回の走査周期内に観測した複数の物体のなかから選択する物体と、周辺物体観測装置８１１が前回の走査周期内に観測した複数の物体のなかから選択する物体との組み合わせを変えてこれを繰り返し、同一の物体であるとみなせる座標の組の数が最も多い組み合わせと回転角度とを求める。

図２２は、この実施の形態における物体相関部２３０の動作を説明するための図である。
物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が前回の走査周期内に観測した静止している物体の移動体８０１に対する相対位置７４１ａ〜７４１ｆのなかから、例えば相対位置７４１ｃを選択する。物体相関部２３０は、処理装置９１１を用いて、相対位置７４１ｃの座標が原点に一致するよう、相対位置７４１ａ〜７４１ｆの座標を平行移動する。
次に、物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が今回の走査周期内に観測した静止している物体の移動体８０１に対する相対位置７４２ａ〜７４２ｆのなかから、例えば相対位置７４２ｃを選択する。物体相関部２３０は、処理装置９１１を用いて、相対位置７４２ｃの座標が原点に一致するよう、相対位置７４２ａ〜７４２ｆの座標を平行移動する。
物体相関部２３０は、処理装置９１１を用いて、平行移動した相対位置７４２ａ〜７４２ｆの座標を、原点を中心として回転する。物体相関部２３０は、処理装置９１１を用いて、回転した相対位置７４２ａ〜７４２ｆの座標と、平行移動した相対位置７４１ａ〜７４１ｆの座標とを比較して、同一の物体であるとみなせる座標の組の数を数える。

物体相関部２３０が選択した相対位置の組み合わせが、周辺物体観測装置８１１が実際に同一の物体を観測した相対位置の組であり、かつ、回転した角度が、移動体８０１が旋回した角度と一致する場合、同一の物体であるとみなせる座標の組の数が最も多くなる。
物体相関部２３０は、処理装置９１１を用いて、同一の物体であるとみなせる座標の組の数が最も多い相対位置の組み合わせを、周辺物体観測装置８１１が同一の物体を観測した相対位置であると判定する。また、そのときに同一の物体であるとみなされた座標の組に対応する相対位置の組み合わせについても、物体相関部２３０は、周辺物体観測装置８１１が同一の物体を観測した相対位置であると判定する。

このように、静止物体判定部２２０が静止していると判定した物体についてのみ、物体相関部２３０が相関処理をするので、移動体８０１の移動軌跡がわからなくても、周辺物体観測装置８１１が観測した物体を相関追尾することができる。

なお、物体相関部２３０は、相対位置の組み合わせを総当たりですべて試してみる構成であってもよいし、移動速度観測装置８２１が観測した移動体８０１の移動速度などの情報を用いて、試してみる相対位置の組み合わせを絞り込む構成であってもよい。
また、物体相関部２３０は、平行移動した座標を回転する角度の範囲を限定せずに試してみる構成であってもよいし、移動体８０１の角速度を観測する角速度観測装置が観測した角速度などの情報を用いて、試してみる角度を絞り込む構成であってもよい。

また、物体相関部２３０は、同一の物体であるとみなせる座標の組の数を比較するのではなく、同一の物体であるとみなせる座標の組を頂点とする凸多角形の面積を比較して、面積が最も広い相対位置の組み合わせや回転角度を判定する構成であってもよい。同一の物体であるとみなせる座標の組が狭い範囲に集中している場合、１つの物体の細部である可能性があるからである。

近似曲線算出部２４０は、処理装置９１１を用いて、静止物体判定部２２０が判定した判定結果と、物体相関部２３０が判定した判定結果とに基づいて、移動体８０１がいる道路の形状を近似する近似曲線を算出する。
例えば、近似曲線算出部２４０は、同一の静止している物体を周辺物体観測装置８１１が観測した観測結果であると物体相関部２３０が判定した物体の移動体８０１に対する相対位置を利用する。
近似曲線算出部２４０は、処理装置９１１を用いて、静止している複数の物体の移動体８０１に対する相対位置を算出する時刻を決定する。近似曲線算出部２４０は、観測結果が複数あると物体相関部２３０が判定した複数の物体それぞれについて、その物体を周辺物体観測装置８１１が観測した複数の観測結果に基づいて、処理装置９１１を用いて、決定した時刻におけるその物体の移動体８０１に対する相対位置を算出する。

静止している物体が道路の縁に存在すると仮定すると、道路の縁の形状は、静止している物体の位置を繋ぐことにより推定することができる。ただし、観測誤差などがあるので、静止している物体の位置を繋いだ線を平滑化する必要がある。近似曲線算出部２４０は、道路の縁の形状を、二次曲線や円など所定の関数で近似する。
近似曲線算出部２４０は、静止している複数の物体について算出した同時刻における複数の相対位置に基づいて、処理装置９１１を用いて、道路の形状を近似する近似曲線を算出する。近似曲線算出部２４０は、処理装置９１１を用いて、近似曲線の係数を算出する。近似曲線算出部２４０は、例えば、最小二乗法などにより、近似曲線の係数を算出する。

ここで、近似曲線算出部２４０が近似曲線を算出するために使用する相対位置は、静止している物体であると静止物体判定部２２０が判定し、かつ、２回以上観測されたと物体相関部２３０が判定した物体の相対位置である。したがって、ノイズなどにより誤って観測された信号である可能性や、静止していない物体を誤って静止していると判定したものである可能性は低い。
これにより、誤検出や誤判定の影響を排除し、道路形状推定の精度を高めることができる。

なお、近似曲線算出部２４０は、同一の物体が所定の回数（例えば５回）以上観測されたと物体相関部２３０が判定した物体の相対位置だけを使用して、近似曲線を算出する構成であってもよい。閾値が大きければ大きいほど、誤検出や誤判定である可能性は低くなるので、信頼性が高くなる。しかし、閾値が大きいと、近似曲線の算出に使用できる物体の数が少なくなる。また、周辺物体観測装置８１１が観測できる範囲内に物体が入ってから、その物体の相対位置を近似曲線の算出に使用できるようになるまでに時間がかかるので、遠くの道路形状を推定することができなくなる。このため、閾値は、例えば４〜６回程度とするのが望ましい。

また、近似曲線算出部２４０は、同一の物体が観測された回数を重み付けとした最小二乗法により、近似曲線の係数を算出する構成であってもよい。すなわち、同一の物体が観測された回数が多いほど、誤検出や誤判定である可能性が低いから、重み付けを重くする。逆に、同一の物体が観測された回数が少なければ、誤検出や誤判定である可能性があるので、重み付けを軽くする。近似曲線算出部２４０は、処理装置９１１を用いて、重み付けした残差の二乗和が最小になる近似曲線を算出する。これにより、誤検出や誤判定の影響を排除しつつ、道路形状を遠くまで推定することができる。

図２３は、物体の移動体８０１に対する相対位置７４３ａ〜７４３ｃと近似曲線７５０との関係の一例を示す図である。
例えば、近似曲線算出部２４０が算出する近似曲線７５０は、円である。移動体８０１を原点とし、進行方向をＹ軸、進行方向に対して直角右方向をＸ軸とした相対座標系における近似曲線７５０は、例えば、次の方程式で表わすことができる。

ここで、Ｏ_ｘは、近似曲線７５０である円の中心７５１のｘ座標である。Ｒは、近似曲線７５０である円の半径７５２である。
なお、この例において、中心７５１は、Ｘ軸上にあると仮定している。移動体８０１が道路形状に沿って移動していれば、円の中心は、移動体８０１の真横にあるはずである。このとき、移動体８０１から道路縁までの距離７５３は、中心７５１のｘ座標Ｏ_ｘと、半径７５２との差である。

相関が取れた物体の数をｎとし、相関が取れたｎ個の物体のうちｉ番目の物体について、近似曲線算出部２４０が算出した移動体８０１に対する相対座標を（ｘ_ｉ，ｙ_ｉ）とする。残差７５４は、その物体と中心７５１との間の距離から、半径７５２を差し引いた差として定義することができる。

ここで、ε_ｉは、ｉ番目の物体の残差７５４である。

近似曲線算出部２４０は、処理装置９１１を用いて、次の式で定義される重み付き残差二乗和が最小になる、中心７５１のＸ座標Ｏ_ｘ及び半径Ｒを算出する。

ここで、Ｊは、重み付き残差二乗和である。Ｗ_ｉは、ｉ番目の物体についての重み付けである。例えば、重み付けＷ_ｉは、ｉ番目の物体の相関回数である。

図２４は、この実施の形態における道路形状推定処理Ｓ５００の流れの一例を示すフローチャート図である。
道路形状推定処理Ｓ５００において、道路形状推定装置２００は、移動体の角速度などを推定する。道路形状推定処理Ｓ５００は、例えば、観測結果取得工程Ｓ５１０と、静止判定工程Ｓ５２０と、物体相関工程Ｓ５３０と、近似曲線算出工程Ｓ５５０とを有する。

観測結果取得工程Ｓ５１０において、相対位置取得部２１１及び移動速度取得部２１２は、それぞれ、周辺物体観測装置８１１及び移動速度観測装置８２１が観測した観測結果を取得する。
静止判定工程Ｓ５２０において、静止物体判定部２２０は、観測結果取得工程Ｓ５１０で取得した観測結果に基づいて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。
物体相関工程Ｓ５３０において、物体相関部２３０は、静止判定工程Ｓ５２０で静止物体判定部２２０が静止していると判定した物体について、相関処理をする。
近似曲線算出工程Ｓ５５０において、近似曲線算出部２４０は、観測結果取得工程Ｓ５１０で取得した観測結果や物体相関工程Ｓ５３０で物体相関部２３０が相関処理した結果に基づいて、近似曲線を算出する。
道路形状推定装置２００は、観測結果取得工程Ｓ５１０に処理を戻し、処理を繰り返す。

図２５は、この実施の形態における観測結果取得工程Ｓ５１０の流れの一例を示すフローチャート図である。
観測結果取得工程Ｓ５１０は、例えば、相対位置取得工程Ｓ５１１と、移動速度取得工程Ｓ５１２とを有する。

相対位置取得工程Ｓ５１１において、相対位置取得部２１１は、処理装置９１１を用いて、周辺物体観測装置８１１が観測結果を表わす信号を出力しているか否かを判定する。周辺物体観測装置８１１が信号を出力していると判定した場合、相対位置取得部２１１は、入力装置９０２を用いて、周辺物体観測装置８１１が出力した信号を入力する。相対位置取得部２１１は、処理装置９１１を用いて、入力した信号が表わす観測結果を取得する。相対位置取得部２１１は、処理装置９１１を用いて、取得した観測結果を表わす周辺物体観測データを生成する。相対位置取得部２１１が生成する周辺物体観測データは、例えば、物体観測時刻データ、相対位置観測値データ、相対速度観測値データを含む。
物体観測時刻データは、周辺物体観測装置８１１がその物体を観測した時刻を表わす。例えば、物体観測時刻データは、所定の時刻からの経過時間をミリ秒単位で表わす整数値データである。あるいは、物体観測時刻データは、フレームの番号を表わす整数値データと、そのフレームの開始時刻からの経過時間をミリ秒単位で表わす整数値データとの組である。なお、１つのフレームは、周辺物体観測装置８１１による１回の走査周期である。１つのフレームの長さは、例えば０．２秒である。
相対位置観測値データは、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置を表わす。例えば、相対位置観測値データは、移動体を原点、移動体の進行方向をＹ軸、移動体の進行方向に対して直角右方向をＸ軸とする移動体基準の相対座標系において、周辺物体観測装置８１１が観測した物体の座標をメートル単位で表わす２つの実数値データの組である。あるいは、相対位置観測値データは、周辺物体観測装置８１１が観測した物体と移動体との間の距離をメートル単位で表わす実数値データと、移動体から見て周辺物体観測装置８１１が観測した物体が見える方向と移動体の進行方向とがなす角度をラジアン単位で表わす実数値データとの組である。
相対速度観測値データは、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度を表わす。例えば、相対速度観測値データは、移動体基準の相対座標系において、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度ベクトルの成分をメートル毎秒単位で表わす２つの実数値データの組である。あるいは、相対速度観測値データは、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度の物体と移動体との距離方向成分をメートル毎秒単位で表わす実数値データである。
相対位置取得部２１１は、記憶装置９１４を用いて、生成した周辺物体観測データを記憶する。

移動速度取得工程Ｓ５１２において、移動速度取得部２１２は、処理装置９１１を用いて、移動速度観測装置８２１が観測結果を表わす信号を出力しているか否かを判定する。移動速度観測装置８２１が信号を出力している場合、移動速度取得部２１２は、入力装置９０２を用いて、移動速度観測装置８２１が出力した信号を入力する。移動速度取得部２１２は、処理装置９１１を用いて、入力した信号が表わす観測結果を取得する。移動速度取得部２１２は、処理装置９１１を用いて、取得した観測結果を表わす移動速度データを生成する。移動速度取得部２１２が生成する移動速度データは、例えば、移動速度観測時刻データと、移動速度観測値データとを含む。
移動速度観測時刻データは、移動速度観測装置８２１が移動体の移動速度を観測した時刻を表わす。例えば、移動速度観測時刻データは、所定の時刻からの経過時間をミリ秒単位で表わす整数値データである。
移動速度観測値データは、移動速度観測装置８２１が観測した移動体の移動速度を表わす。例えば、移動速度観測値データは、移動速度観測装置８２１が観測した移動体の移動速度をメートル毎秒単位で表わす実数値データである。
移動速度取得部２１２は、記憶装置９１４を用いて、生成した移動速度データを記憶する。

なお、物体観測時刻データと、移動速度観測時刻データとにおける観測時刻の表現形式は、相互に変換が可能であれば、異なる形式であってもよい。

図２６は、この実施の形態における静止判定工程Ｓ５２０の流れの一例を示すフローチャート図である。
静止判定工程Ｓ５２０は、例えば、移動速度観測時刻取得工程Ｓ５２１と、物体選択工程Ｓ５２２と、観測時刻比較工程Ｓ５２３と、速度比較工程Ｓ５２４とを有する。

移動速度観測時刻取得工程Ｓ５２１において、静止物体判定部２２０は、処理装置９１１を用いて、移動速度取得工程Ｓ５１２で移動速度取得部２１２が記憶した移動速度データのうち、観測時刻が最も新しい移動速度データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した移動速度データに含まれる移動速度観測時刻データから、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻を取得する。また、静止物体判定部２２０は、処理装置９１１を用いて、取得した移動速度データに含まれる移動速度観測値データから、移動速度観測装置８２１が観測した移動体の移動速度を取得する。
なお、静止判定工程Ｓ５２０を前回実行した後に移動速度観測装置８２１が新しい移動速度を観測していない場合、静止物体判定部２２０は、以降の処理をせず、静止判定工程Ｓ５２０を終了する構成であってもよい。例えば、静止物体判定部２２０は、処理装置９１１を用いて、移動速度観測時刻取得工程Ｓ５２１を前回実行したときに取得した最新の観測時刻と、今回取得した最新の観測時刻とを比較する。観測時刻が同じ場合、静止物体判定部２２０は、静止判定工程Ｓ５２０を終了する。

物体選択工程Ｓ５２２において、静止物体判定部２２０は、処理装置９１１を用いて、相対位置取得工程Ｓ５１１で相対位置取得部２１１が記憶した周辺物体観測データのなかから、その物体が静止しているか否かをまだ判定していない周辺物体観測データを選択する。
例えば、後述する速度比較工程Ｓ５２４において、静止物体判定部２２０は、記憶装置９１４を用いて、ある周辺物体観測データについて静止しているか否かを判定した場合に、その判定結果を表わす静止判定結果データを、その周辺物体観測データに対応づけて記憶しておく。
静止物体判定部２２０は、処理装置９１１を用いて、ある周辺物体観測データについて、対応づけられた静止判定結果データを記憶しているか否かを判定することにより、その周辺物体観測データについて静止しているか否かを判定済である否かを判定する。
選択すべき周辺物体観測データが存在しない場合、静止物体判定部２２０は、処理装置９１１を用いて、静止判定工程Ｓ５２０を終了する。
選択すべき周辺物体観測データが１つ以上存在する場合、静止物体判定部２２０は、処理装置９１１を用いて、そのなかから周辺物体観測データを１つ選択する。静止物体判定部２２０は、処理装置９１１を用いて、観測時刻比較工程Ｓ５２３へ処理を進める。

観測時刻比較工程Ｓ５２３において、静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２で選択した周辺物体観測データに含まれる物体観測時刻データから、周辺物体観測装置８１１がその物体を観測した観測時刻を取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した観測時刻と、移動速度観測時刻取得工程Ｓ５２１で取得した観測時刻とを比較する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも後である場合、静止物体判定部２２０は、まだ、その周辺物体観測データについて静止しているか否かの判定をしない。静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２に処理を戻し、次の周辺物体観測データを選択する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも前である場合、静止物体判定部２２０は、その周辺物体観測データについて静止しているか否かの判定をする。静止物体判定部２２０は、処理装置９１１を用いて、速度比較工程Ｓ５２４へ処理を進める。

速度比較工程Ｓ５２４において、静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２で選択した周辺物体観測データに含まれる相対速度観測値データから、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度を取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した相対速度と、移動速度観測時刻取得工程Ｓ５２１で取得した移動体の移動速度とに基づいて、その物体が静止しているか否かを判定する。静止物体判定部２２０は、処理装置９１１を用いて、判定した判定結果を表わす静止判定データを生成する。静止物体判定部２２０は、記憶装置９１４を用いて、生成した静止判定データを、物体選択工程Ｓ５２２で選択した周辺物体観測データに対応づけて記憶する。
静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２に処理を戻し、次の周辺物体観測データを選択する。

図２７は、この実施の形態における物体相関工程Ｓ５３０の流れの一例を示すフローチャート図である。
物体相関工程Ｓ５３０は、前回静止物体取得工程Ｓ５３１と、今回静止物体取得工程Ｓ５３２と、前回物体選択工程Ｓ５３３と、前回物体平行移動工程Ｓ５３４と、今回物体選択工程Ｓ５３５と、今回物体平行移動工程Ｓ５３６と、回転角度選択工程Ｓ５３７と、今回物体回転工程Ｓ５３８と、距離算出工程Ｓ５３９と、同一物体判定工程Ｓ５４０と、閾値判定工程Ｓ５４１と、同一物体記憶工程Ｓ５４２とを有する。

前回静止物体取得工程Ｓ５３１において、物体相関部２３０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、１つ前のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データであって、その物体が静止していると静止判定工程Ｓ５２０で静止物体判定部２２０が判定した周辺物体観測データをすべて取得する。
物体相関部２３０は、処理装置９１１を用いて、取得した周辺物体観測データそれぞれについて、２つ以上前のフレームにおける周辺物体観測データとの間で、周辺物体観測装置８１１が同一の物体を観測したものであると物体相関部２３０が判定した周辺物体観測データがあるフレームの数を求める。
例えば、後述する同一物体記憶工程Ｓ５４２において、物体相関部２３０は、処理装置９１１を用いて、あるフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データが、それより前のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データと同一の物体を観測したものであると判定した場合、相関回数を表わす相関回数データを生成する。相関回数は、同一の物体と判定したフレームの数である。物体相関部２３０は、記憶装置９１４を用いて、その周辺物体観測データに対応づけて相関回数データを記憶する。同一の物体を観測したものであると判定した相手の周辺物体観測データに対応づけられた相関回数データがない場合、物体相関部２３０は、相関回数を「１」とした相関回数データを生成する。同一の物体を観測したものであると判定した相手の周辺物体観測データに対応づけられた相関回数データがある場合、物体相関部２３０は、その相関回数データが表わす相関回数に「１」を加えたものを相関回数とする相関回数データを生成する。
物体相関部２３０は、処理装置９１１を用いて、取得した周辺物体観測データに対応づけて記憶した相関回数データが表わす相関回数を取得することにより、同一の物体と判定したフレームの数を求める。
物体相関部２３０は、処理装置９１１を用いて、取得したすべての周辺物体観測データを、取得したフレームの数が多い順に並べる。相関の取れたフレームの数が多いほど、その周辺物体観測データがノイズなどによる誤検出である可能性が低く、また、移動している物体を誤って静止していると認識したものである可能性も低い。このため、今回のフレームにおける周辺物体観測データとも相関が取れる可能性が高い。あらかじめ相関が取れる可能性が高い順に周辺物体観測データを並べておくことにより、物体相関工程Ｓ５３０の計算量を抑えることができる。

今回静止物体取得工程Ｓ５３２において、物体相関部２３０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、最新のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データであって、その物体が静止していると静止判定工程Ｓ５２０で静止物体判定部２２０が判定した周辺物体観測データをすべて取得する。

前回物体選択工程Ｓ５３３において、物体相関部２３０は、処理装置９１１を用いて、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データのなかから、前回静止物体取得工程Ｓ５３１で並べだ順に、周辺物体観測データを１つ選択する。
前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データがすべて選択済であり、選択すべき周辺物体観測データが存在しない場合、物体相関部２３０は、処理装置９１１を用いて、同一物体記憶工程Ｓ５４２へ処理を進める。

未選択の周辺物体観測データが存在する場合、物体相関部２３０は、処理装置９１１を用いて、未選択の周辺物体観測データを１つ選択する。物体相関部２３０は、処理装置９１１を用いて、選択した周辺物体観測データと同一の物体を今回のフレームで周辺物体観測装置８１１が観測する相対位置を予測する。例えば、物体相関部２３０は、処理装置９１１を用いて、１つ前のフレームまでの間に相関の取れた物体の移動体に対する相対位置の軌跡などの状態量に基づいて、予測を行う。

前回物体平行移動工程Ｓ５３４において、物体相関部２３０は、処理装置９１１を用いて、前回静止物体取得工程Ｓ５３１で取得したすべての周辺物体観測データについて、その周辺物体観測データに含まれる相対位置観測値データが表わす周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の座標から、今回静止物体取得工程Ｓ５３２で選択した周辺物体観測データに含まれる相対位置観測値データが表わすその物体の移動体に対する相対位置の座標を差し引くことにより、物体の座標を平行移動する。物体相関部２３０は、記憶装置９１４を用いて、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データそれぞれについて、平行移動した座標を表わすデータを記憶する。

今回物体選択工程Ｓ５３５において、物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データを、その周辺物体観測データに含まれる相対位置観測値データが表わす周辺物体観測装置８１１が観測した物体の移動体に対する相対位置が、前回物体選択工程Ｓ５３３で予測した相対位置に近い順に並べる。予測した相対位置との距離が短いほうが同一の物体を観測した観測結果である可能性が高いからである。なお、物体相関部２３０は、予測した相対位置との距離が所定の閾値より短いものだけを抽出して、近い順に並べる構成であってもよい。
物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データ（もしくは、前回物体選択工程Ｓ５３３で予測した相対位置との距離が所定の閾値より短いものだけを抽出した周辺物体観測データ）のなかから、前回物体選択工程Ｓ５３３で予測した相対位置との距離が近い順に、周辺物体観測データを１つ選択する。
今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データ（もしくは、前回物体選択工程Ｓ５３３で予測した相対位置との距離が所定の閾値より短いものだけを抽出した周辺物体観測データ）がすべて選択済であり、選択すべき周辺物体観測データが存在しない場合、物体相関部２３０は、処理装置９１１を用いて、前回物体選択工程Ｓ５３３に処理を戻し、前回静止物体取得工程Ｓ５３１で選択した周辺物体観測データのなかから次の周辺物体観測データを選択する。
未選択の周辺物体観測データが存在する場合、物体相関部２３０は、処理装置９１１を用いて、未選択の周辺物体観測データを１つ選択し、今回物体平行移動工程Ｓ５３６へ処理を進める。

今回物体平行移動工程Ｓ５３６において、物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得したすべての周辺物体観測データについて、その周辺物体観測データに含まれる相対位置観測値データが表わす周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の座標から、今回物体選択工程Ｓ５３５で選択した周辺物体観測データに含まれる相対位置観測値データが表わすその物体の移動体に対する相対位置の座標を差し引くことにより、物体の座標を平行移動する。物体相関部２３０は、記憶装置９１４を用いて、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データそれぞれについて、平行移動した座標を表わすデータを記憶する。

回転角度選択工程Ｓ５３７において、物体相関部２３０は、前回物体選択工程Ｓ５３３で選択した周辺物体観測データと、今回物体選択工程Ｓ５３５で選択した周辺物体観測データとが、同一の物体を観測した観測結果を表わす周辺物体観測データであると仮定して、処理装置９１１を用いて、移動体の旋回角度を推定する。物体相関部２３０は、移動体の角速度を観測する角速度観測装置が観測した角速度などを、旋回角度の推定に利用する構成であってもよい。物体相関部２３０は、処理装置９１１を用いて、推定した旋回角度に基づいて、試してみる複数の回転角度を決定する。
物体相関部２３０は、処理装置９１１を用いて、決定した複数の回転角度のなかから、推定した旋回角度に近い順に、回転角度を１つ選択する。
決定した複数の回転角度をすべて選択済であり、選択すべき回転角度が存在しない場合、物体相関部２３０は、処理装置９１１を用いて、今回物体選択工程Ｓ５３５に処理を戻し、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データのなかから次の周辺物体観測データを選択する。
未選択の回転角度が存在する場合、物体相関部２３０は、処理装置９１１を用いて、未選択の回転角度を１つ選択し、今回物体回転工程Ｓ５３８へ処理を進める。

今回物体回転工程Ｓ５３８において、物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得したすべての周辺物体観測データについて今回物体平行移動工程Ｓ５３６で平行移動した座標を、原点を中心として、回転角度選択工程Ｓ５３７で選択した回転角度だけ回転移動する。

距離算出工程Ｓ５３９において、物体相関部２３０は、処理装置９１１を用いて、前回静止物体取得工程Ｓ５３１で取得したすべての周辺物体観測データと、今回静止物体取得工程Ｓ５３２で取得したすべての周辺物体観測データとのすべての組み合わせについて、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データについて前回物体平行移動工程Ｓ５３４で平行移動した座標と、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データについて今回物体回転工程Ｓ５３８で回転移動した座標との間の距離を算出する。物体相関部２３０は、処理装置９１１を用いて、算出した距離が近い順に、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データと、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データとのペアを生成する。

同一物体判定工程Ｓ５４０において、物体相関部２３０は、処理装置９１１を用いて、距離算出工程Ｓ５３９で生成したペアのうち、算出した距離が所定の閾値より短いペアの数を数える。物体相関部２３０は、処理装置９１１を用いて、数えたペア数を、物体相関工程Ｓ５３０を今回実行したなかで、それまでに同一物体判定工程Ｓ５４０で数えたペア数の最大値と比較する。
今回数えたペア数がそれまでのペア数の最大値より小さい場合、物体相関部２３０は、回転角度選択工程Ｓ５３７に処理を戻し、次の回転角度を選択する。
今回数えたペア数がそれまでのペア数の最大値より大きい場合、物体相関部２３０は、記憶装置９１４を用いて、距離算出工程Ｓ５３９で生成したペアのうち算出した距離が閾値より短いペアと、数えたペア数とを表わすデータを記憶する。物体相関部２３０は、処理装置９１１を用いて、閾値判定工程Ｓ５４１へ処理を進める。

閾値判定工程Ｓ５４１において、物体相関部２３０は、処理装置９１１を用いて、距離算出工程Ｓ５３９で数えたペア数を所定の閾値と比較する。
数えたペア数が閾値より小さい場合、物体相関部２３０は、処理装置９１１を用いて、回転角度選択工程Ｓ５３７に処理を戻し、次の回転角度を選択する。
数えたペア数が閾値より大きい場合、物体相関部２３０は、処理装置９１１を用いて、同一物体記憶工程Ｓ５４２へ処理を進める。相関が取れる可能性が高い順に試行するので、早い段階で正しい組み合わせと回転角度が見つかる可能性が高く、それ以降の試行をしないことにより、計算量を抑えるためである。
なお、比較の対象となる閾値は、あらかじめ定めた定数であってもよい。また、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データの数や、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データの数に基づいて、物体相関部２３０が閾値を算出する構成であってもよい。例えば、物体相関部２３０は、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データの数と、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データの数とを比較して小さいほうの数を求め、求めた数に所定の定数（例えば０．６）を乗じて、閾値を算出する。

同一物体記憶工程Ｓ５４２において、物体相関部２３０は、処理装置９１１を用いて、同一物体判定工程Ｓ５４０で記憶したデータが表わすペアが、同一の物体を周辺物体観測装置８１１が観測した観測結果を表わす周辺物体観測データであると判定する。物体相関部２３０は、処理装置９１１を用いて、判定した結果を表わす相関結果データとして、上述した相関回数データを生成する。また、物体相関部２３０は、相関結果データとして、処理装置９１１を用いて、どの周辺物体観測データが、同一の物体を周辺物体観測装置８１１が観測した観測結果を表わす周辺物体観測データであるかを表わすデータを生成する。例えば、物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が観測した物体に番号を振る。物体相関部２３０は、記憶装置９１４を用いて、振った番号を表わすデータを、周辺物体観測データに対応づけて記憶する。物体相関部２３０は、１つ前のフレームにおける周辺物体観測データと同一の物体を周辺物体観測装置８１１が観測した観測結果を表わすと判定した周辺物体観測データには、１つ前のフレームにおけるその周辺物体観測データと同一の番号を振る。物体相関部２３０は、それ以外の周辺物体観測データには、いずれの周辺物体観測データとも異なる番号を振る。

図２８は、この実施の形態における近似曲線算出工程Ｓ５５０の流れの一例を示すフローチャート図である。
近似曲線推定工程Ｓ５５０は、相関物体選択工程Ｓ５５１と、重み付け算出工程Ｓ５５２と、相対位置算出工程Ｓ５５３と、係数算出工程Ｓ５５４とを有する。

相関物体選択工程Ｓ５５１において、近似曲線算出部２４０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、最新のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データであって、それより前のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データと同一の物体を観測した観測結果を表わすと物体相関工程Ｓ５３０で物体相関部２３０が判定した周辺物体観測データを１つ選択する。例えば、近似曲線算出部２４０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、対応づけられた相関回数データを物体相関部２３０が記憶した周辺物体観測データを選択する。
相関が取れた周辺物体観測データがすべて選択済であり、選択すべき周辺物体観測データが存在しない場合、近似曲線算出部２４０は、処理装置９１１を用いて、係数算出工程Ｓ５５４へ処理を進める。
相関が取れた周辺物体観測データのなかに未選択の周辺物体観測データが存在する場合、近似曲線算出部２４０は、処理装置９１１を用いて、未選択の周辺物体観測データを１つ選択し、重み付け算出工程Ｓ５５２へ処理を進める。

重み付け算出工程Ｓ５５２において、近似曲線算出部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１で選択した周辺物体観測データの相関回数に基づいて、重み付けＷ_ｉを算出する。

相対位置算出工程Ｓ５５３において、近似曲線算出部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１で選択した周辺物体観測データが表わす物体のある時刻における移動体に対する相対位置（ｘ_ｉ，ｙ_ｉ）を算出する。
近似曲線算出部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１に処理を戻し、次の周辺物体観測データを選択する。

係数算出工程Ｓ５５４において、近似曲線算出部２４０は、処理装置９１１を用いて、相関の取れたすべての物体について、重み付け算出工程Ｓ５５２で算出した重み付けＷ_ｉと、相対位置算出工程Ｓ５５３で算出した相対位置（ｘ_ｉ，ｙ_ｉ）とに基づいて、重み付き残差二乗和Ｊが最小になる係数Ｏ_ｘ及びＲを算出する。例えば、近似曲線算出部２４０は、処理装置９１１を用いて、重み付き残差二乗和Ｊを最小にする係数Ｏ_ｘ及びＲを求める連立方程式を生成する。近似曲線算出部２４０は、処理装置９１１を用いて、生成した連立方程式を解いて、係数Ｏ_ｘ及びＲを算出する。

実施の形態５．
実施の形態５について、図２９〜図３３を用いて説明する。
なお、実施の形態１〜実施の形態４と共通する部分については、同一の符号を付し、説明を省略する。

図２９は、この実施の形態における道路形状推定システム８００の全体構成の一例を示すシステム構成図である。
道路形状推定装置２００は、実施の形態４で説明した構成に加えて、初期値算出部２５０、信頼度算出部２６０、近似曲線選択部２７０を有する。

近似曲線算出部２４０は、各時刻における近似曲線をそれぞれ別々に算出するのではなく、前の時刻に算出した近似曲線を更新することにより、新しい時刻の近似曲線を算出する。近似曲線算出部２４０は、周辺物体観測装置８１１が観測した物体が静止していると静止物体判定部２２０が判定するたび、処理装置９１１を用いて、周辺物体観測装置８１１が観測した観測結果に基づいて、近似曲線を更新する。例えば、近似曲線算出部２４０は、拡張カルマンフィルタなどの追尾フィルタを使用して、近似曲線を更新する。

初期値算出部２５０は、近似曲線算出部２４０が算出する近似曲線の初期値を算出する。初期値算出部２５０は、処理装置９１１を用いて、物体相関部２３０が判定した判定結果に基づいて、近似曲線の初期値を算出する。

信頼度算出部２６０は、近似曲線算出部２４０が算出した近似曲線の信頼度を算出する。

近似曲線選択部２７０は、近似曲線算出部２４０が算出した近似曲線が複数ある場合に、信頼度算出部２６０が算出した信頼度に基づいて、近似曲線算出部２４０が算出した近似曲線のなかから、採用する近似曲線を選択する。

周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の軌跡は、移動体自身の移動や向きの変化を反映している。したがって、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の軌跡から、移動体自身の移動量がわかる。

図３０は、物体相関部２３０が相関処理に用いる平行移動量および回転角度と、移動体８０１の移動量の関係を説明するための図である。
物体相関部２３０は、あるフレームにおいて周辺物体観測装置８１１が観測した静止物体の移動体８０１に対する相対位置７４１ａ〜７４１ｆと、別のフレームにおいて周辺物体観測装置８１１が観測した静止物体の移動体８０１に対する相対位置７４２ａ〜７４２ｆとの相関を取ったとする。物体相関部２３０は、ベクトル７５５を用いて相対位置７４１ａ〜７４１ｆの座標を平行移動して、相対位置７４１ｃの座標を原点に合わせる。また、物体相関部２３０は、ベクトル７５６を用いて相対位置７４２ａ〜７４２ｆの座標を平行移動して、相対位置７４２ｃの座標を原点に合わせ、回転角度７５７だけ回転移動することにより、相関が取れたとする。

この場合、ベクトル７５６を回転角度７５７だけ回転したベクトル７５８から、ベクトル７５５を差し引いた差ベクトル７５９が、その２つのフレームの間における移動体基準の相対座標系における移動体８０１の移動量である。
また、回転角度７５７は、その２つのフレームの間における移動体８０１の旋回角度である。

例えば、物体相関部２３０は、図２７で説明した同一物体判定工程Ｓ５４０で、今回数えたペア数がそれまでのペア数の最大値より大きい場合、記憶装置９１４を用いて、前回物体選択工程Ｓ５３３で選択した周辺物体観測データと、今回物体選択工程Ｓ５３５で選択した周辺物体観測データと、回転角度選択工程Ｓ５３７で選択した回転角度を表わすデータとを記憶する。

初期値算出部２５０は、物体相関部２３０が記憶した２つの周辺物体観測データと、回転角度を表わすデータとに基づいて、移動体の移動量および旋回角度を推定する。例えば、初期値算出部２５０は、処理装置９１１を用いて、今回物体選択工程Ｓ５３５で物体相関部２３０が選択した周辺物体観測データから、最新のフレームにおいて周辺物体観測装置８１１が観測したその物体の移動体に対する相対位置を取得する。初期値算出部２５０は、処理装置９１１を用いて、取得した相対位置を表わす座標を、原点を中心として、回転角度選択工程Ｓ５３７で物体相関部２３０が選択した回転角度だけ回転移動する。初期値算出部２５０は、処理装置９１１を用いて、前回物体選択工程Ｓ５３３で物体相関部２３０が選択した周辺物体観測データから、１つ前のフレームにおいて周辺物体観測装置８１１が観測したその物体の移動体に対する相対位置を取得する。初期値算出部２５０は、処理装置９１１を用いて、取得した相対位置を表わす座標から、回転移動した座標を差し引いた差を算出して、移動体の移動量の推定値とする。また、初期値算出部２５０は、処理装置９１１を用いて、回転角度選択工程Ｓ５３７で物体相関部２３０が選択した回転角度を、移動体の旋回角度の推定値とする。

自動車などの移動体が道路上を走行している場合、通常は、道路形状に沿った動きをすると考えられる。このため、初期値算出部２５０が推定した移動体の移動量や旋回角度は、移動体が走行している道路のうち、既に通過した部分の形状を反映している。
初期値算出部２５０は、処理装置９１１を用いて、推定した移動体の移動量と旋回角度とに基づいて、移動体が走行している道路の曲率を算出する。例えば、初期値算出部２５０は、処理装置９１１を用いて、移動体が移動した距離で、旋回角度を割った商を算出して、道路の曲率とする。

なお、移動体の移動速度が遅い場合には、必ずしも、移動体が道路形状に沿った動きをするとは限らない。このため、初期値算出部２５０は、処理装置９１１を用いて、移動速度取得部２１２が記憶した移動速度データに基づいて、移動速度観測装置８２１が観測した移動体の移動速度を取得し、取得した移動速度を所定の閾値と比較して、移動体の移動速度が閾値より速い場合のみ、道路の曲率を算出する構成であってもよい。

また、初期値算出部２５０は、静止している物体が道路に沿って存在している位置と、移動体が走行している位置との間の距離を算出する。例えば、初期値算出部２５０は、処理装置９１１を用いて、算出した曲率に基づいて、道路形状を近似する円の中心を算出する。初期値算出部２５０は、処理装置９１１を用いて、物体相関部２３０が相関を取った物体の移動体に対する相対位置のうち、移動体からの距離が所定の閾値より近い相対位置を取得する。初期値算出部２５０は、処理装置９１１を用いて、算出した中心を中心として、その物体が移動体の真横に来るよう、取得した相対位置を回転して、その物体が移動体の真横を通過するときの位置を推定する。初期値算出部２５０は、処理装置９１１を用いて、推定した位置に基づいて、静止している物体が道路に沿って存在している位置と、移動体が走行している位置との間の距離を算出する。初期値算出部２５０が算出した距離は、近似曲線のＸ切片の初期値となる。

物体相関部２３０が相関を取った物体の移動体に対する相対位置のうち、移動体からの距離が所定の閾値より近い相対位置が複数ある場合、初期値算出部２５０は、それぞれの相対位置について、処理装置９１１を用いて、その物体が移動体の真横を通過するときの位置を推定する。
初期値算出部２５０は、推定した位置に基づいて、処理装置９１１を用いて、複数の相対位置をグループ分けする。推定した位置の間の距離が所定の閾値より小さい場合、それは、一列に並んだ物体であると考えられる。例えば、初期値算出部２５０は、処理装置９１１を用いて、算出した複数の相対位置のうち２つの相対位置の間の距離を算出し、閾値と比較する。２つの相対位置の間の距離が閾値より小さい場合、初期値算出部２５０は、処理装置９１１を用いて、その２つの相対位置を同じグループに分類する。例えば、そのようなグループが、移動体の右側に１つ、左側に１つある場合、右側のグループは道路の右縁であり、左側のグループは道路の左縁であると考えられる。また、そのようなグループが移動体の右側に２つある場合、（左側通行であれば）移動体に近いほうのグループは中央分離帯であり、移動体から遠いほうのグループは道路の右縁であると考えられる。
初期値算出部２５０は、処理装置９１１を用いて、各グループに分類された相対位置について推定した位置を平均して、そのグループについて推定した位置とする。例えば、そのようなグループが３つある場合、初期値算出部２５０は、３つの位置を推定する。初期値算出部２５０は、近似曲線のＸ切片の初期値を３つ算出する。これに基づいて、近似曲線算出部２４０は、３つの近似曲線を算出する。

上述したように、初期値算出部２５０が移動量と旋回角度とに基づいて算出する曲率は、道路のうち、移動体が既に通過した部分の曲率である。したがって、道路の曲率が途中で変わっている場合には、その先の部分の曲率はわからない。このため、近似曲線算出部２４０は、初期値算出部２５０が算出する曲率を初期値として、曲率が変化した先も含めた道路形状を近似する近似曲線を算出する。

図３１は、この実施の形態における初期値算出部２５０の動作を説明するための説明図である。

例えば、移動体８０１が位置８０２にいたとき、周辺物体観測装置８１１は、９つの物体の相対位置７４４ａ〜７４４ｉを観測する。静止物体判定部２２０は、それぞれの相対位置７４４ａ〜７４４ｉに観測された物体が静止していると判定する。
移動体８０１が移動して現在の位置にいるとき、周辺物体観測装置８１１は、９つの物体の相対位置７４５ａ〜７４５ｉを観測する。静止物体判定部２２０は、それぞれの相対位置７４５ａ〜７４５ｉに観測された物体が静止していると判定する。
物体相関部２３０は、相対位置７４４ａと相対位置７４５ａとが同一の物体を観測した結果であると判定する。同様に、物体相関部２３０は、相対位置７４４ｂと相対位置７４５ｂ、相対位置７４４ｃと相対位置７４５ｃ、相対位置７４４ｄと相対位置７４５ｄ、相対位置７４４ｅと相対位置７４５ｅ、相対位置７４４ｆと相対位置７４５ｆ、相対位置７４４ｇと相対位置７４５ｇ、相対位置７４４ｈと相対位置７４５ｈ、相対位置７４４ｉと相対位置７４５ｉが、それぞれ同一の物体を観測した結果であると判定する。

この結果に基づいて、初期値算出部２５０は、移動体の移動距離と旋回角度７７８を算出する。初期値算出部２５０は、算出した移動距離と旋回角度７７８とに基づいて、旋回中心７７９を算出する。初期値算出部２５０は、相関が取れた静止している９つの物体の相対位置７４５ａ〜７４５ｉのうち、移動体８０１からの距離が閾値７７６より短い範囲７７７内にある６つの物体の相対位置７４５ｂ、７４５ｃ，７４５ｅ，７４５ｆ，７４５ｈ，７４５ｉについて、旋回中心７７９を中心として回転し、移動体８０１の真横に来る位置７８６ｂ，７８６ｃ，７８６ｅ，７８６ｆ，７８６ｈ，７８６ｉを算出する。初期値算出部２５０は、算出した６つの位置をグループ化して、３つのグループ７８７ａ〜７８７ｃを生成する。初期値算出部２５０は、生成した３つのグループ７８７ａ〜７８７ｃそれぞれについて、そのグループ内に分類された位置の平均を取り、近似曲線のＸ切片の初期値とする。

この例では、移動体８０１がちょうどカーブを抜けて直線道路に入ろうとしているところであり、それまでの移動体８０１が走行してきた道路の曲率と、これから移動体８０１が走行する道路の曲率とが異なっている。これが、初期値算出部２５０が算出する近似曲線のＸ切片の初期値の誤差要因となる。曲率の変化による誤差は、移動体８０１からの距離が大きくなるほど大きくなる。このため、初期値算出部２５０は、移動体８０１からの距離が閾値７７６より大きい物体の相対位置７４５ａ，７４５ｄ，７４５ｇについては、移動体８０１の真横に来る位置７８６ａ，７８６ｄ，７８６ｇを算出しない。

図３２は、この実施の形態における近似曲線算出部２４０が算出する近似曲線の一例を示す図である。
曲線７１６は、移動体８０１からの距離と、道路の曲率との関係を示す。この例では、道路の曲率が連続的に変化すると仮定した近似曲線で、道路の形状を近似する。移動体が現在いる位置での曲率をχ_０、遙か彼方での曲率をχ_∞として、移動体からの距離が大きくなるほど、道路の曲率がχ_∞に近づく。
曲線７１７は、移動体が走行する経路を近似する曲線である。曲線７１７の曲率は、曲線７１６で示した関係にしたがう。２つの曲線７１８ａ，７１８ｂは、道路の縁の形状を近似する近似曲線である。曲線７１８ａ，７１８ｂは、曲線７１７からの距離が一定の曲線である。

ここで、ｓは、道路に沿った移動体からの距離である。ｓは、０以上である。χは、道路の曲率である。χは、距離ｓの関数である。χ_０は、ｓが０のときの曲率である。χ_∞は、ｓが大きくなるにつれて漸近する曲率である。ａは、道路の曲率の変化率である。ａは、０より小さい。θは、ｓが０のときの道路の方向を基準とした道路の方向である。θは、距離ｓの関数である。Ｃは、近似曲線である。ｗは、移動体の走行位置と道路縁との間の距離である。

例えば、近似曲線算出部２４０は、この式で表される近似曲線を算出する。近似曲線算出部２４０は、曲率χ_０、曲率χ_∞、距離ｗの初期値として、初期値算出部２５０が算出した初期値を使用する。初期値算出部２５０が算出した初期値のうち、曲率の初期値が、χ_０及びχ_∞の初期値であり、Ｘ切片の初期値がｗの初期値である。変化率ａは、初期値算出部２５０が初期値を算出しないので、近似曲線算出部２４０は、所定の値（例えば−１×１０の−５乗）を初期値として設定する。

なお、近似曲線は、この式で表される曲線に限らず、例えば、円や放物線などの円錐曲線、クロソイド曲線、楕円曲線や超楕円曲線などであってもよい。

近似曲線算出部２４０は、追尾中の近似曲線がない場合、処理装置９１１を用いて、初期値算出部２５０が算出した初期値を設定した近似曲線を生成し、追尾を開始する。
追尾中の近似曲線がある場合、近似曲線算出部２４０は、処理装置９１１を用いて、初期値算出部２５０が算出した初期値が、追尾中の近似曲線のものであるか否かを判定する。例えば、近似曲線算出部２４０は、処理装置９１１を用いて、追尾中の近似曲線のＸ切片から、初期値算出部２５０が算出したＸ切片の初期値を差し引いた差を算出する。近似曲線算出部２４０は、処理装置９１１を用いて、算出した差の絶対値と、所定の閾値とを比較する。差の絶対値が閾値より小さい場合、近似曲線算出部２４０は、その初期値が、その近似曲線のものであると判定する。差の絶対値が閾値より大きい場合、近似曲線算出部２４０は、その初期値が、その近似曲線のものではないと判定する。
初期値算出部２５０が算出した初期値が、近似曲線算出部２４０が追尾中のいずれの近似曲線のものでもないと判定した場合、近似曲線算出部２４０は、処理装置９１１を用いて、その初期値を設定した近似曲線を新たに生成する。

周辺物体観測装置８１１が観測した物体が静止していると静止物体判定部２２０が判定した場合、近似曲線算出部２４０は、処理装置９１１を用いて、静止物体判定部２２０が静止していると判定した物体の相対位置が、追尾中のいずれかの近似曲線に当てはめることができるか否かを判定する。例えば、近似曲線算出部２４０は、処理装置９１１を用いて、追尾中のすべての近似曲線を時間遷移して、周辺物体観測装置８１１がその物体を観測した時刻に合わせる。近似曲線算出部２４０は、処理装置９１１を用いて、時間遷移したそれぞれの近似曲線と、周辺物体観測装置８１１がその物体を観測した相対位置との間の距離を算出する。近似曲線算出部２４０は、処理装置９１１を用いて、算出した距離が最も短い近似曲線を判定する。近似曲線算出部２４０は、処理装置９１１を用いて、判定した近似曲線について算出した距離を、所定の閾値と比較する。その物体の相対位置とその近似曲線との間の距離が閾値より小さい場合、近似曲線算出部２４０は、処理装置９１１を用いて、その物体の相対位置を観測値として、その近似曲線を更新する。

近似曲線算出部２４０が算出する近似曲線が上述した近似曲線であり、近似曲線算出部２４０が近似曲線の更新に使用する追尾フィルタが拡張カルマンフィルタである場合、近似曲線算出部２４０は、例えば、次の式で示す状態方程式を使う。

ここで、ｘは、状態量を表わすベクトルである。χ_０，χ_∞，ａ，ｗは、上述した近似曲線Ｃの係数である。ｖは、移動体の移動速度である。ｆは、状態遷移モデルを表わす関数である。Δｔは、前回の更新からの経過時間である。ｚは、観測量を表わすベクトルである。ｘ_ｏは、移動体を基準とする相対座標系における周辺物体観測装置８１１が観測した物体のＸ座標である。ｙ_ｏは、移動体を基準とする相対座標系における周辺物体観測装置８１１が観測した物体のＹ座標である。ｖ_ｓは、移動速度観測装置８２１が観測した移動体の移動速度である。ｈは、観測モデルを表わす関数である。ｓ_ｏは、近似曲線Ｃ上で、移動体から、周辺物体観測装置８１１が観測した物体の相対位置に最も近い点までの距離である。

近似曲線算出部２４０は、周辺物体観測装置８１１が静止している物体の相対位置を観測するたびに、追尾中のすべての近似曲線を時間遷移させ、その相対位置と相関の取れた近似曲線だけを更新する。
なお、近似曲線算出部２４０は、相対位置と近似曲線との間の距離が閾値より小さい近似曲線が複数ある場合、相対位置と近似曲線との間の距離が最小の近似曲線だけを更新するのではなく、相対位置と近似曲線との間の距離が閾値より小さいすべての近似曲線を更新する構成であってもよい。

信頼度算出部２６０は、処理装置９１１を用いて、近似曲線算出部２４０が算出した近似曲線の信頼度を算出する。例えば、信頼度算出部２６０は、近似曲線算出部２４０が近似曲線の更新に使った相対位置の空間的分散度に基づいて、近似曲線の信頼度を算出する。

例えば、信頼度算出部２６０は、記憶装置９１４を用いて、近似曲線算出部２４０が算出した近似曲線ごとに、更新に利用した相対位置について、近似曲線上で、移動体からその相対位置に最も近い点までの距離ｓ_ｏを蓄積して記憶する。
近似曲線算出部２４０が時間遷移処理をした場合、信頼度算出部２６０は、処理装置９１１を用いて、記憶したすべての距離ｓ_ｏについて、移動体の移動速度ｖと遷移時間Δｔとの積を、距離ｓ_ｏから差し引いた差を算出する。信頼度算出部２６０は、算出した差で距離ｓ_ｏを更新し、記憶装置９１４を用いて、更新した距離ｓ_ｏを記憶する。なお、距離ｓ_ｏが負になった場合、信頼度算出部２６０は、その距離ｓ_ｏを削除し、更新した距離ｓ_ｏを記憶しない。
近似曲線算出部２４０が近似曲線の更新処理をした場合、信頼度算出部２６０は、処理装置９１１を用いて、近似曲線算出部２４０が更新した近似曲線の番号など近似曲線を識別する情報と、距離ｓ_ｏとを取得する。信頼度算出部２６０は、記憶装置９１４を用いて、取得した距離ｓ_ｏをその近似曲線について既に記憶している距離ｓ_ｏとともに記憶する。

このようにして記憶した距離ｓ_ｏに基づいて、信頼度算出部２６０は、距離ｓ_ｏの分散σ^２を算出する。それぞれの近似曲線について、信頼度算出部２６０は、処理装置９１１を用いて、記憶した距離ｓ_ｏの合計値Σｓ_ｏを算出する。信頼度算出部２６０は、処理装置９１１を用いて、算出した合計値Σｓ_ｏに基づいて、平均値ｓ_ａを算出する。信頼度算出部２６０は、処理装置９１１を用いて、記憶した距離ｓ_ｏと算出した平均値ｓ_ａとの差の二乗の合計値Σ（ｓ_ｏ−ｓ_ａ）^２を算出する。信頼度算出部２６０は、処理装置９１１を用いて、算出した合計値Σ（ｓ_ｏ−ｓ_ａ）^２に基づいて、分散σ^２を算出する。
信頼度算出部２６０は、それぞれの近似曲線について算出した分散σ^２をそのままその近似曲線の信頼度とする。あるいは、信頼度算出部２６０は、処理装置９１１を用いて、算出した分散σ^２を正規化し、近似曲線の信頼度とする構成であってもよい。

なお、信頼度算出部２６０は、近似曲線算出部２４０が近似曲線を算出する過程で算出する誤差共分散行列を、信頼度の算出に利用する構成であってもよい。

また、近似曲線算出部２４０は、信頼度算出部２６０が算出した信頼度が低い近似曲線を破棄する構成であってもよい。例えば、近似曲線算出部２４０は、追尾中の近似曲線それぞれについて、処理装置９１１を用いて、信頼度算出部２６０が算出した信頼度を取得する。近似曲線算出部２４０は、処理装置９１１を用いて、取得した信頼度を所定の閾値と比較する。信頼度が閾値より低い場合、近似曲線算出部２４０は、処理装置９１１を用いて、その近似曲線の追尾を中止する。あるいは、近似曲線算出部２４０は、処理装置９１１を用いて，その近似曲線の係数のうちχ_０やχ_∞を、追尾中の他の近似曲線の係数を使って再初期化する構成であってもよい。

近似曲線選択部２７０は、近似曲線算出部２４０が算出した近似曲線のなかから、信頼度算出部２６０が算出した信頼度に基づいて、採用する近似曲線を選択する。例えば、近似曲線選択部２７０は、処理装置９１１を用いて、近似曲線算出部２４０が算出した近似曲線のなかで最も信頼度が高い近似曲線を判定し、判定した近似曲線を採用する。近似曲線選択部２７０は、処理装置９１１を用いて、採用した近似曲線の係数を出力する。
なお、近似曲線選択部２７０は、最も信頼度が高い近似曲線を１つだけ採用するのではなく、例えば、信頼度が所定の閾値より高い近似曲線が複数ある場合、処理装置９１１を用いて、信頼度が閾値より高い複数の近似曲線を採用する構成であってもよい。あるいは、近似曲線選択部２７０は、近似曲線算出部２４０が算出した近似曲線の係数を、信頼度算出部２６０が算出した信頼度で重み付けして平均して、平均した係数を持つ近似曲線を採用する構成であってもよい。

図３３は、この実施の形態における道路形状推定処理Ｓ５００の流れの一例を示すフローチャート図である。
道路形状推定処理Ｓ５００は、実施の形態４で説明した観測結果取得工程Ｓ５１０、静止判定工程Ｓ５２０、物体相関工程Ｓ５３０に加えて、初期値算出工程Ｓ５６１、新規曲線判定工程Ｓ５６２、近似曲線遷移工程Ｓ５６３、近似曲線相関工程Ｓ５６４、近似曲線更新工程Ｓ５６５、信頼度算出工程Ｓ５６６、破棄判定工程Ｓ５６７、近似曲線破棄工程Ｓ５６８、近似曲線出力工程Ｓ５６９を有する。

物体相関工程Ｓ５３０で、周辺物体観測装置８１１が新たに観測した静止物体の相対位置に、相関の取れる相手があると判定した場合、物体相関部２３０は、処理装置９１１を用いて、初期値算出５６１へ処理を進める。
相関の取れる相手がないと判定した場合、物体相関部２３０は、処理装置９１１を用いて、近似曲線遷移工程Ｓ５６３へ処理を進める。

初期値算出工程Ｓ５６１において、初期値算出部２５０は、処理装置９１１を用いて、物体相関工程Ｓ５３０で物体相関部２３０が相関を取った相対位置に基づいて、移動体の移動量や旋回角度を推定する。初期値算出部２５０は、処理装置９１１を用いて、推定した移動体の移動量や旋回角度と、観測結果取得工程Ｓ５１０で相対位置取得部２１１が記憶した相対位置とに基づいて、近似曲線の係数の初期値を算出する。

新規曲線判定工程Ｓ５６２において、近似曲線算出部２４０は、処理装置９１１を用いて、初期値算出工程Ｓ５６１で初期値算出部２５０が算出した初期値が、追尾中の近似曲線のものであるか否かを判定する。追尾中のいずれの近似曲線のものでもないと判定した初期値がある場合、近似曲線算出部２４０は、処理装置９１１を用いて、新たな近似曲線を生成し、その初期値を使って初期化する。

近似曲線遷移工程Ｓ５６３において、近似曲線算出部２４０は、処理装置９１１を用いて、追尾中のすべての近似曲線について、前回時間遷移処理をした観測時刻と、周辺物体観測装置８１１が新たに静止物体を観測した観測時刻との差に基づいて、時間遷移処理をする。また、信頼度算出部２６０は、処理装置９１１を用いて、記憶したすべての距離ｓ_ｏについて、時間遷移処理をする。

近似曲線相関工程５６４において、近似曲線算出部２４０は、処理装置９１１を用いて、周辺物体観測装置８１１が新たに観測した静止物体の相対位置について、追尾中のすべての近似曲線のなかから、相関の取れる近似曲線を判定する。
相関の取れる近似曲線がある場合、近似曲線算出部２４０は、処理装置９１１を用いて、近似曲線更新工程Ｓ５６５へ処理を進める。
相関の取れる近似曲線がない場合、近似曲線算出部２４０は、処理装置９１１を用いて、信頼度算出工程Ｓ５６６へ処理を進める。

近似曲線更新工程Ｓ５６５において、近似曲線算出部２４０は、処理装置９１１を用いて、近似曲線相関工程Ｓ５６４で相関が取れると判定したすべての近似曲線について、周辺物体観測装置８１１が新たに観測した静止物体の相対位置を使って更新処理をする。また、信頼度算出部２６０は、記憶装置９１４を用いて、その近似曲線について、距離ｓ_ｏを記憶する。

信頼度算出工程Ｓ５６６において、信頼度算出部２６０は、追尾中のすべての近似曲線について、処理装置９１１を用いて、記憶した距離ｓ_ｏに基づいて、近似曲線の信頼度を算出する。

破棄判定工程Ｓ５６７において、近似曲線算出部２４０は、追尾中のすべての近似曲線について、処理装置９１１を用いて、信頼度算出工程Ｓ５６６で信頼度算出部２６０が算出した信頼度を所定の閾値と比較する。
信頼度が閾値より低い近似曲線がある場合、近似曲線算出部２４０は、処理装置９１１を用いて、近似曲線破棄工程Ｓ５６８へ処理を進める。
すべての近似曲線について信頼度が閾値より高い場合、近似曲線算出部２４０は、処理装置９１１を用いて、近似曲線出力工程Ｓ５６９へ処理を進める。

近似曲線破棄工程Ｓ５６８において、近似曲線算出部２４０は、処理装置９１１を用いて、破棄判定工程Ｓ５６７で信頼度が閾値より低いと判定した近似曲線を破棄する。

近似曲線出力工程Ｓ５６９において、近似曲線選択部２７０は、処理装置９１１を用いて、信頼度算出工程Ｓ５６６で信頼度算出部２６０が算出した信頼度に基づいて、採用する近似曲線を選択する。近似曲線選択部２７０は、処理装置９１１を用いて、採用した近似曲線を出力する。

実施の形態６．
実施の形態６について、図３４〜図４９を用いて説明する。
なお、実施の形態１〜実施の形態５と共通する部分については、同一の符号を付し、説明を省略する。

図３４は、この実施の形態における道路形状認識装置１００の全体の構成を示すブロック図である。
道路形状認識装置１００（道路形状推定装置）は、実施の形態１で説明した機能ブロックに加えて、更に、ヨーレートセンサ１２５と、道路曲率変換部１２６と、自車走行路判定部１８０と、推定値選択部１９０（推定値選択／統合部）とを有する。なお、破線で囲んだ部分１００’は、実施の形態１における道路形状認識装置１００に相当する部分である。道路形状認識装置１００は、部分１００’を、実施の形態２〜実施の形態５のいずれかで説明した道路形状認識装置１００に置き換えた構成であってもよい。
道路形状認識装置１００は、走行路に応じてヨーレートによる曲率推定値と、停止物による道路曲率推定値（近似曲線係数でも可）とを切り替えて先行車判定を行う。

道路形状認識装置１００は、走行路を「直線路」「カーブ手前（曲線路手前）」「カーブ路（曲線路）」「カーブ出口（曲線路出口）」の４種類に類別する。道路形状認識装置１００は、走行路の種類に応じて、推定値を切り替える。

図３５は、直線路の一例を示す図である。
直線路では、ヨーレートは操舵のふらつきを受ける。このため、停止物による曲率推定値の方がヨーレートより正確である。

図３６は、カーブ手前の一例を示す図である。
カーブ手前とは、この図のように、自車が直線路にいて、先行車はカーブ路にいる状況を意味する。レーダ１１０は、遠方の停止物を観測可能であるから、カーブ形状を認識できる。そのため、ヨーレートより停止物による曲率推定値のほうが正確である。

図３７は、カーブ路の一例を示す図である。
カーブ路では、自車がカーブ走行中のため一定の操舵角が入力される。停止物による推定値は、停止物の観測誤差などの影響を受けるため、ヨーレートによる推定値のほうがより安定しており、正確である。

図３８は、カーブ出口の一例を示す図である。
カーブ出口とは、この図のように、自車がカーブ路にいて、先行車は直線路にいる状況を意味する。この場合、カーブ手前と同様、ヨーレートより停止物による曲率推定値のほうが正確である。

以上を踏まえ、道路形状認識装置１００は、走行路の種別を判定し、走行路の種別に応じて推定値を切り替える処理を行う。

次に動作を説明する。

道路曲率変換部１２６は、例えば次の式を用いて、曲率推定値を算出する。

ここで、χ_ｙａｗは曲率、Ｖは自車速、ωはヨーレートを表わす。道路曲率変換部１２６は、ヨーレートωとして、ヨーレートセンサ１２５が観測した観測値を用いる。もしくは、道路曲率変換部１２６は、ヨーレートωとして、既知の平滑用フィルタ（カルマンフィルタやαフィルタなどの公知のフィルタ）を用いて、ヨーレートセンサ１２５が観測した観測値に平滑処理を施した後のヨーレート推定値を用いる。道路曲率変換部１２６は、ヨーレートωを、車速センサ１２０が観測した車両の速度で割った商を計算することにより、曲率χ_ｙａｗを算出する。

自車走行路判定部１８０は、前記ヨーレートωと、道路形状認識装置１００の近似曲線選択部１７０より得られる近似曲線係数推定値とを用いて、自車走行路の種別を判定する。自車走行路判定部１８０は、例えば状態遷移モデルを用いて、より正確に走行路の状態を判定する。

図３９は、この実施の形態における自車走行路の状態遷移モデルの一例を示す状態遷移図である。
初期状態（初期値）は、直線路（状態０）である。直線路（状態０）において、所定の条件（以下「カーブ手前判定条件」と呼ぶ。）を満たすと、直線路（状態０）からカーブ手前（状態１）へ状態遷移する。
カーブ手前（状態１）において、ある所定の条件（以下「カーブ路判定条件」と呼ぶ。）を満たすと、カーブ路（状態２）へ状態遷移し、別の所定の条件（以下「短カーブ判定条件」と呼ぶ。）を満たすと、カーブ出口（状態３）へ状態遷移する。
カーブ路（状態２）において、所定の条件（以下「カーブ出口判定条件」と呼ぶ。）を満たすと、カーブ出口（状態３）へ状態遷移する。
カーブ出口（状態３）において、ある所定の条件（以下「カーブ終了判定条件」と呼ぶ。）を満たすと、直線路（状態０）へ状態遷移し、別の所定の条件（以下「カーブ連続判定条件」と呼ぶ。）を満たすと、カーブ手前（状態１）へ状態遷移する。

図４０は、この実施の形態におけるカーブ手前判定条件の一例を説明するための図である。
停止物から算出した道路形状の近似曲線として、実施の形態１で説明した式（１）で表わされる２次曲線を用いる場合、自車走行路判定部１８０は、例えば、停止物による近似曲線の係数（傾き）ａがある閾値以上であることをカーブ手前判定条件とする。

図４１は、この実施の形態における直線路（状態０）からカーブ手前（状態１）への判定フロー図である。
自車走行路判定部１８０は、道路近似曲線本計算部１６０が算出した道路近似曲線に基づいて、カーブ手前判定条件が満たされるか否かを判定する。自車走行路判定部１８０は、道路近似曲線本計算部１６０が算出した道路近似曲線の係数ａの絶対値と、閾値Ｔｈ_ａとを比較する。係数ａの絶対値が閾値Ｔｈ_ａより大きい場合、自車走行路判定部１８０は、自車走行路の状態をカーブ手前（状態１）に遷移させる。

あるいは、自車走行路判定部１８０は、道路基準曲線上において、進行方向の基準距離Ｙ_ｒｅｆ離れた地点における車軸方向の位置Ｘ_ｒｅｆが所定の閾値以上であることを、カーブ手前判定条件とする構成であってもよい。自車走行路判定部１８０は、道路近似曲線本計算部１６０が算出した傾きａと式（１）からある進行方向をＹ軸とした場合の基準距離Ｙ_ｒｅｆにおける車軸方向の位置Ｘ_ｒｅｆの値を算出し、算出した位置Ｘ_ｒｅｆを閾値と比較する。なお、自車走行路判定部１８０は、Ｘ切片ｂとして道路近似曲線本計算部１６０が算出した値ではなく、０を用いて、車軸方向の位置Ｘ_ｒｅｆを計算する。自車走行路判定部１８０は、次の式を用いて、車軸方向の位置Ｘ_ｒｅｆを計算する。

図４２は、この実施の形態におけるカーブ手前（状態１）からカーブ路（状態２）及びカーブ出口（状態３）への判定フロー図である。

自車走行路判定部１８０は、ヨーレートωの絶対値と、所定の閾値Ｔｈ_ωとを比較する。
ヨーレートωが閾値Ｔｈ_ω以上である場合、自車走行路判定部１８０は、例えば、最新のヨーレートと、過去Ｎサンプリング前のヨーレートとに基づいて、ヨーレート変化率ｄωを算出する。自車走行路判定部１８０は、次の式を用いて、ヨーレート変化率ｄωを算出する。

ただし、ω_ｔは時刻ｔにおけるヨーレート、ω_{ｔ−Ｎ・ΔＴ}は時刻（ｔ−Ｎ・ΔＴ）におけるヨーレートを表わす。ΔＴはサンプリング間隔である。

自車走行路判定部１８０は、算出したヨーレート変化率ｄωの絶対値と、所定の閾値Ｔｈ_ｄωとを比較する。
ヨーレート変化率ｄωが閾値Ｔｈ_ｄω以上である場合、自車走行路判定部１８０は、自車走行路の状態をカーブ路（状態２）に遷移させる。
ヨーレート変化率ｄωが閾値Ｔｈ_ｄω未満である場合、自車走行路判定部１８０は、ヨーレートωの符号とヨーレートｄωの符号とを比較する。
ヨーレートωの符号とヨーレートｄωの符号とが異なる場合、自車走行路判定部１８０は、自車走行路の状態をカーブ出口（状態３）に遷移させる。

図４３は、ヨーレートによるカーブ路判定条件の説明図である。
この図のように、カーブ路では、ヨーレートは定常値に収束する。自車走行路判定部１８０は、例えば、カーブ手前（状態１）において、ヨーレートωの絶対値が閾値Ｔｈ_ω以上で、かつ、ヨーレート変化率ｄωの絶対値が閾値Ｔｈ_ｄω以下であることをカーブ路判定条件とする。自車走行路判定部１８０は、カーブ路判定条件が満たされた場合に、カーブ路（状態２）へ状態を遷移させる。

図４４は、ヨーレートによる短カーブ判定条件の説明図である。
ヨーレートωの絶対値が閾値Ｔｈ_ω以上で、かつ、ヨーレート変化率ｄωの絶対値が閾値Ｔｈ_ｄω以上である場合、この図のように、カーブ路が短くカーブ出口に入っている状況である可能性がある。自車走行路判定部１８０は、例えば、カーブ手前（状態１）において、カーブの曲がりと逆の方向にヨーレートが変化することを短カーブ判定条件とする。自車走行路判定部１８０は、短カーブ判定条件が満たされた場合に、カーブ出口（状態３）へ状態を遷移させる。

図４５は、この実施の形態におけるカーブ路（状態２）からカーブ出口（状態３）への判定フロー図である。
自車走行路判定部１８０は、例えば、次の式を用いて、最新の近似曲線係数（傾き）ａの推定値ａ_ｔと、過去Ｎサンプリング前の近似曲線係数（傾き）ａの推定値ａ_{ｔ−Ｎ・ΔＴ}とから、近似曲線係数（傾き）ａの変化率ｄａを算出する。

ただし、ΔＴはサンプリング間隔である。
自車走行路判定部１８０は、算出した変化率ｄａの絶対値と、所定の閾値Ｔｈ_ｄａとを比較する。変化率ｄａの絶対値が閾値Ｔｈ_ｄａ以上である場合、自車走行路判定部１８０は、近似曲線係数ａの符号と、変化率ｄａの符号とを比較する。近似曲線係数ａの符号と、変化率ｄａの符号とが異なる場合、自車走行路判定部１８０は、自車走行路の状態をカーブ出口（状態３）に遷移させる。

図４６は、道路近似曲線によるカーブ出口判定条件の説明図である。
この図のように、カーブ出口では、近似曲線が直線へと変化する。自車走行路判定部１８０は、例えば、カーブ路（状態２）において、近似曲線係数（傾き）ａの変化率ｄａの絶対値が閾値Ｔｈ_ｄａ以上で、かつ、近似曲線係数（傾き）ａの符号と前記変化率ｄａの符号とが逆であることをカーブ出口判定条件とする。自車走行路判定部１８０は、カーブ出口判定条件が満たされた場合に、カーブ出口（状態３）へ状態を遷移させる。

図４７は、この実施の形態におけるカーブ出口（状態３）から直線路（状態０）及びカーブ手前（状態１）への判定フロー図である。
自車走行路判定部１８０は、例えば、道路近似曲線本計算部１６０が算出した近似曲線の係数ａの絶対値と、閾値Ｔｈ_ａとを比較する。
近似曲線の係数ａの絶対値が閾値Ｔｈ_ａ以下である場合、自車走行路判定部１８０は、自車走行路の状態を、直線路（状態０）に遷移させる。
近似曲線の係数ａの絶対値が閾値Ｔｈ_ａ超である場合、自車走行路判定部１８０は、係数ａの変化率ｄａを算出する。自車走行路判定部１８０は、算出した変化率ｄａの絶対値と、閾値Ｔｈ_ｄａとを比較する。
変化率ｄａの絶対値が閾値Ｔｈ_ｄａ以上である場合、自車走行路判定部１８０は、係数ａの符号と、変化率ｄａの符号とを比較する。係数ａの符号と、変化率ｄａの符号とが同じである場合、自車走行路判定部１８０は、自車走行路の状態を、カーブ手前（状態１）に遷移させる。

図４８は、道路近似曲線によるカーブ終了判定条件の説明図である。
この図のように、直線路では、近似曲線係数（傾き）ａの絶対値が閾値Ｔｈ_ａ以下になる。自車走行路判定部１８０は、例えば、カーブ出口（状態３）において、近似曲線係数ａの絶対値が閾値Ｔｈ_ａ以下であることをカーブ終了判定条件とする。自車走行路判定部１８０は、カーブ終了判定条件が満たされた場合に、直線路（状態０）へ状態を遷移させる。

図４９は、道路近似曲線によるカーブ連続判定条件の説明図である。
この図のように、カーブ路が連続する場合は、直線路がないため、近似曲線係数（傾き）ａの絶対値が閾値Ｔｈ_ａ以下となる時刻がない可能性がある。自車走行路判定部１８０は、例えば、近似曲線係数（傾き）ａの変化率ｄａが閾値以上で、かつ、近似曲線係数（傾き）ａの符号と前記変化率ｄａの符号とが同一であることをカーブ連続判定条件とする。自車走行路判定部１８０は、カーブ連続判定条件が満たされた場合に、カーブ手前（状態１）へ状態を遷移させる。

なお、自車走行路判定部１８０は、１回の判定で条件を満たした場合に状態を遷移させるのではなく、過去Ｎ回の判定において、Ｍ回以上条件を満たした場合に状態を遷移させる構成であってもよい。これにより、状態遷移の誤りを少なくすることができる。

推定値選択部１９０（推定値選択／統合部）は、直線路（状態０）、カーブ手前（状態１）、カーブ出口（状態３）の場合、停止物による曲率推定値χ_ｆｉｘを出力する。カーブ路（状態２）の場合、推定値選択部１９０は、ヨーレートによる曲率推定値χ_ｙａｗを出力する。

あるいは、推定値選択部１９０は、状態によって重み付けを変えて、停止物による曲率推定値χ_ｆｉｘと、ヨーレートによる曲率推定値χ_ｙａｗとを統合する構成であってもよい。例えば、推定値選択部１９０は、次の式を用いて、統合した曲率推定値χを算出する。

ただし、λ_ｙａｗ及びλ_ｆｉｘは、自車走行路の状態に基づいて推定値選択部１９０が設定する重み付けである。χ_ｆｉｘは、近似曲線選択部１７０が選択した道路近似曲線から算出した自車走行路の曲率である。推定値選択部１９０は、例えば、次の近似式を用いて、曲率推定値χ_ｆｉｘを算出する。

ただし、ｙ’は、道路近似曲線上で、曲率を求めるＹ座標位置として、あらかじめ設定された値である。

推定値選択部１９０は、走行路状態が「０」（直線路）である場合、例えば、λ_ｙａｗを０．５、λ_ｆｉｘを０．５に設定して、曲率推定値χを算出する。走行路状態が「１」（カーブ手前）あるいは「３」（カーブ出口）である場合、推定値選択部１９０は、λ_ｙａｗを０．１、λ_ｆｉｘを０．９に設定して、曲率推定値χを算出する。走行路状態が「２」（カーブ路）である場合、推定値選択部１９０は、λ_ｙａｗを０．９、λ_ｆｉｘを０．１に設定して、曲率推定値χを算出する。

また、推定値選択部１９０は、曲率推定値を出力するのではなく、道路近似曲線を出力する構成であってもよい。例えば、推定値選択部１９０は、直線路（状態０）、カーブ手前（状態１）、カーブ出口（状態３）の場合、近似曲線選択部１７０が選択した道路近似曲線を出力する。カーブ路（状態２）の場合、推定値選択部１９０は、ヨーレートによる曲率推定値χ_ｙａｗに基づいて、曲率が一定の曲線を算出し、道路近似曲線として出力する。例えば、推定値選択部１９０は、次の式で表わされる曲線を、道路近似曲線として出力する。

ただし、χ_ｙａｗは、ヨーレートセンサ１２５が測定したヨーレートωに基づいて道路曲率変換部１２６が算出した曲率である。ｂは、近似曲線選択部１７０が選択した道路近似曲線の係数ｂ（Ｘ切片）である。

なお、推定値選択部１９０は、上の式をテーラー展開により近似した次の式で表わされる二次曲線を、道路近似曲線として出力する構成であってもよい。

ただし、ｙ’は、道路近似曲線上で、曲率を求めるＹ座標位置として、あらかじめ設定された値である。
これにより、推定値選択部１９０が出力する道路近似曲線の式の形は、自車走行路の状態にかかわらず同じであり、道路近似曲線の係数の値が異なるだけになるので、先行車両判定など後段の処理が容易になる。

あるいは、推定値選択部１９０は、近似曲線選択部１７０が選択した道路近似曲線と、ヨーレートによる曲率推定値χ_ｙａｗに基づく道路近似曲線とを、重み付け統合した曲線を算出し、道路近似曲線として出力する構成であってもよい。例えば、推定値選択部１９０は、次の式で表わされる曲線を、道路近似曲線として出力する。

ただし、λ_ｙａｗ及びλ_ｆｉｘは、自車走行路の状態に基づいて推定値選択部１９０が設定する重み付けである。ａ及びｂは、近似曲線選択部１７０が選択した道路近似曲線の係数である。

上記道路形状推定装置（道路形状認識装置１００）は、道路曲率変換部（１２６）と、自車走行路判定部（１８０）と、推定値選択／統合部（推定値選択部１９０）とを有する。
上記道路曲率変換部（１２６）は、ヨーレートを道路曲率へ変換する。
上記自車走行路判定部（１８０）は、前記ヨーレートと前記近似曲線係数より自車走行路を判定する。
上記推定値選択／統合部（１９０）は、上記自車走行路判定結果に応じた曲率推定値を選択する。

上記推定値選択／統合部（１９０）は、上記自車走行路判定結果に応じて曲率推定値を重み付け統合する。

上記自車走行路判定部（１８０）は、自車走行路の状態遷移に基づき、各状態遷移の判定式をＮ回中Ｍ回満たした場合において状態遷移を行い、状態を判定する。

この実施の形態における道路形状推定装置によれば、自車走行路の種別を判別することにより、走行路に応じて適した推定値を選択もしくは重みを付けて統合するので、より正確な先行車判定をすることができる。

実施の形態７．
実施の形態７について、図５０〜図５２を用いて説明する。
なお、実施の形態１〜実施の形態６と共通する部分については、同一の符号を付し、説明を省略する。

図５０は、この実施の形態における道路形状認識装置１００の構成の一部の一例を示すブロック図である。
道路形状認識装置１００（道路形状推定装置）は、実施の形態１で説明した機能ブロックに加えて、前方車追尾部１３５を有する。なお、道路形状認識装置１００は、実施の形態１で説明した道路形状認識装置１００ではなく、実施の形態２〜実施の形態６のいずれかで説明した道路形状認識装置１００に、前方車追尾部１３５を付加した構成であってもよい。

図５１は、道路近似曲線の正誤判定方式の一例の説明図である。
この図のように、道路近似曲線（推定曲線）が交差する場合、観測値の取り合いが起こり、道路近似曲線の精度が劣化する可能性がある。ある近似曲線に割り当てられるべき観測値を、別の近似曲線が取ることにより、誤った道路近似曲線が算出され、最終的に出力される可能性がある。これにより、先行車の判定を誤る可能性が生じる。

道路形状認識装置１００は、前方車と近似曲線の位置関係から誤った近似曲線を補正する。例えば、この図のように、道路左縁より外に前方車両が存在する場合、道路形状認識装置１００は、左縁近似曲線の推定値が誤っていると判断して、補正する。

次に動作を説明する。

前方車追尾部１３５は、レーダ１１０が観測した物体の位置や相対速度などの観測値に基づいて、自車の前方を走行している車両を追尾する。前方車追尾部１３５は、追尾した前方車両の航跡を出力する。前方車両が複数ある場合、前方車追尾部１３５は、それぞれの前方車両を追尾して、複数の前方車両にそれぞれ対応する複数の航跡を出力する。

近似曲線選択部１７０の補正部１７４は、左右縁の近似曲線が進行方向における所定の距離で交差し、かつ、ある近似曲線が前方車両の位置より内側に存在する場合、前記近似曲線を初期化する。

例えば、補正部１７４は、それぞれの道路近似曲線について、自車進行方向Ｙ軸において自車から所定の距離Ｙ_Ｄ離れた位置における自車の車軸方向の位置Ｘを算出する。例えば、選択部１７３が選択した道路近似曲線が、式（１）によって表わされる曲線である場合、補正部１７４は、次の式を用いて、位置Ｘを算出する。

ただし、ａ及びｂは、近似曲線選択部１７０が選択したそれぞれの道路近似曲線の係数である。

補正部１７４は、近似曲線選択部１７０が選択した道路近似曲線それぞれについて、位置Ｘを算出する。例えば、近似曲線選択部１７０が、道路左縁の近似曲線と、道路右縁の近似曲線との２つの道路近似曲線を選択した場合、補正部１７４は、道路左縁の近似曲線についての位置Ｘ（以下「Ｘ_ｃｕｖＬ」と記す。）を算出し、道路右縁の近似曲線についての位置Ｘ（以下「Ｘ_ｃｕｖＲ」と記す。）を算出する。

補正部１７４は、複数の道路近似曲線について算出した複数の位置Ｘに基づいて、道路近似曲線が交差しているか否かを判定する。例えば、補正部１７４は、道路左縁の近似曲線について算出した位置Ｘ_ｃｕｖＬと、道路右縁の近似曲線について算出した位置Ｘ_ｃｕｖＲとに基づいて、次の式が満たされているか否かを判定し、満たされている場合に、道路近似曲線が交差していると判定する。

近似曲線選択部１７０が選択した道路近似曲線が３以上ある場合、補正部１７４は、例えば、近似曲線選択部１７０が選択した道路近似曲線を、係数ｂ（Ｘ切片）が小さい順に、位置Ｘを算出する。補正部１７４は、ある道路近似曲線について算出した位置Ｘが、その道路近似曲線よりも係数ｂが小さい道路近似曲線について算出した位置Ｘより小さいか等しい場合に、その２つの道路近似曲線が交差していると判定する。

２つの道路近似曲線が交差していると判定した場合、補正部１７４は、前方車追尾部１３５が出力した追尾航跡それぞれに基づいて、前方車両の位置と、道路近似曲線との関係が正常であるか否かを判定する。例えば、補正部１７４は、それぞれの追尾航跡について、自車の進行方向における前方車両の相対位置Ｙ_ｔｒｋに基づいて、それぞれの道路近似曲線について、距離Ｙ_ｔｒｋ離れた位置における自車の車軸方向の位置Ｘを算出する。例えば、選択部１７３が選択した道路近似曲線が式（１）によって表わされる曲線である場合、補正部１７４は、次の式を用いて、位置Ｘを算出する。

ただし、ａ及びｂは、近似曲線選択部１７０が選択したそれぞれの道路近似曲線の係数である。

例えば、道路左縁の近似曲線と、道路右縁の近似曲線とが交差していると判定した場合において、補正部１７４は、それぞれの追尾航跡について、道路左縁の近似曲線について位置Ｘ（以下「Ｘ_{ｃｕｖＬ，ｉ}」と記す。なお、ｉは追尾航跡の番号を表わす。）を算出し、道路右縁の近似曲線について位置Ｘ（以下「Ｘ_{ｃｕｖＲ，ｉ}」と記す。）を算出する。

補正部１７４は、例えば、それぞれの追尾航跡について、次の式を満たされているか否かを判定する。

ただし、Ｘ_{ｔｒｋ，ｉ}は、番号ｉの追尾航跡において、自車の車軸方向における前方車両の位置である。
上の式が満たされている場合、補正部１７４は、前方車両と道路左縁の近似曲線との関係が正常ではなく、道路左縁の近似曲線が誤っていると判断する。

また、補正部１７４は、例えば、それぞれの追尾航跡について、次の式が満たされているか否かを判定する。

ただし、Ｘ_{ｔｒｋ，ｉ}は、番号ｉの追尾航跡において、自車の車軸方向における前方車両の位置である。
上の式が満たされている場合、補正部１７４は、前方車両と道路右縁の近似曲線との関係が正常ではなく、道路右縁の近似曲線が誤っていると判断する。

近似曲線選択部１７０が選択した道路近似曲線が３以上ある場合、補正部１７４は、例えば、それぞれの追尾航跡について、自車の前方の領域を道路近似曲線によって分けた複数の領域のうちどの領域に前方車両がいるかを判定する。補正部１７４は、道路近似曲線が交差しているか否かに関わらず、前方車両がいる領域を判定し、判定した結果を記憶しておく。
道路近似曲線が交差していると判定した場合において、補正部１７４は、前方車両がいる領域の判定結果を前回の判定結果と比較して、交差している道路近似曲線のいずれかを前方車両が横切ったか否かを判定する。交差している道路近似曲線を前方車両が横切った場合、補正部１７４は、前方車両が横切った道路近似曲線が誤っていると判定する。

なお、補正部１７４は、前方車追尾部１３５が出力したすべての追尾航跡に基づいて道路近似曲線が誤っているか否かの判定をするのではなく、安定している追尾航跡だけを使って、道路近似曲線が誤っているか否かの判定をする構成であってもよい。例えば、補正部１７４は、数サンプル間継続して追尾している航跡のみを用いる。これにより、マルチパス波によって誤検出された偽像の影響を排除することができる。

補正部１７４は、誤っていると判定した道路近似曲線を初期化する。

例えば、補正部１７４は、選択部１７３が選択した道路近似曲線のうち、誤っていると判定した道路近似曲線以外の道路近似曲線のなかから、曲線品質計算部１７２が算出した曲線品質値が最も高い道路近似曲線を選択する。補正部１７４は、選択した道路近似曲線の係数を用いて、誤っていると判定した道路近似曲線の係数を初期化する。たとえば、補正部１７４は、誤っていると判定した道路近似曲線の係数ａを、選択した道路近似曲線の係数ａで置き換える。

あるいは、補正部１７４は、前方車追尾部１３５が出力した追尾航跡を用いて、誤っていると判定した道路近似曲線を初期化する構成であってもよい。

図５２は、道路近似曲線の初期化方式の一例の説明図である。
補正部１７４は、この図のように、誤っていると判定した道路近似曲線と、前方車両の位置との間の関係が正常になるように、道路近似曲線を初期化する。

例えば、補正部１７４は、誤っていると判定した道路近似曲線との関係が正常でないと判定した前方車両の位置（Ｘ_{ｔｒｋ，ｉ}，Ｙ_{ｔｒｋ，ｉ}）を、道路近似曲線が通るよう、道路近似曲線を初期化する。例えば、誤っていると判定した道路近似曲線が式（１）で表わされる曲線である場合、補正部１７４は、次の式を用いて、道路近似曲線の新しい係数を算出し、道路近似曲線の係数を、算出した新しい係数で初期化する。

ただし、ｂは、補正部１７４が誤っていると判定した道路近似曲線の係数である。ａ’及びｂ’は、補正部１７４が算出する新しい係数ａ及びｂの初期値である。

なお、補正部１７４は、前方車両の位置を道路近似曲線が通るように道路近似曲線を初期化するのではなく、自車の車軸方向において、前方車両から所定の距離だけ離れた位置を道路近似曲線が通るように道路近似曲線を初期化する構成であってもよい。例えば、補正部１７４は、前方車両の右側を通っている道路近似曲線が誤っていて、前方車両の左側を通るべきであると判定した場合、次の式を用いて、道路近似曲線の新しい係数を算出する。

ただし、ΔＸは、前方車両と道路近似曲線との間の距離の最小値としてあらかじめ設定された値である。なお、Ｘ軸は、右方向が正であるものとする。

逆に、前方車両の左側を通っている道路近似曲線が誤っていて、前方車両の右側を通るべきであると判定した場合、補正部１７４は、次の式を用いて、道路近似曲線の新しい係数を算出する。

あるいは、補正部１７４は、誤っていると判定した道路近似曲線を初期化するのではなく、誤っていると判定した道路近似曲線を破棄する構成であってもよい。

上記道路形状推定装置（道路形状認識装置１００）は、近似曲線選択部（１７０）と、前方車両追尾部（１３５）とを有する。
上記前方車両追尾部（１３５）は、複数の前方車追尾航跡を算出する。
上記近似曲線選択部（１７０）の補正部（１７４）は、複数の近似曲線が所定の距離で交差している場合、各近似曲線と前記追尾航跡の位置関係から近似曲線の正誤判定を行い，誤りである近似曲線を初期化する。

上記近似曲線選択部（１７０）の補正部（１７４）は、上記近似曲線を初期化する際に前方車の位置を用いて補正した近似曲線係数を設定する。

以上のように、この実施の形態における道路形状推定装置（道路形状認識装置１００）によれば、近似曲線と前方車との位置関係を利用することで誤った近似曲線を補正するので、より正確な先行車判定をすることができる。

実施の形態８．
実施の形態８について、図５３を用いて説明する。
なお、実施の形態１〜実施の形態７と共通する部分については、同一の符号を付し、説明を省略する。

図５３は、この実施の形態における道路形状推定システム８００の全体構成の一例を示すシステム構成図である。
道路形状推定システム８００は、自動車などの移動体に搭載されている。道路形状推定システム８００は、周辺物体観測装置８１１と、移動速度観測装置８２１と、角速度観測装置８３１と、道路形状推定装置２００とを有する。

角速度観測装置８３１は、道路形状推定システム８００が搭載された移動体の角速度を観測する。角速度観測装置８３１は、例えばジャイロセンサである。角速度観測装置８３１は、観測した移動体の角速度などの観測結果を表わす信号を出力する。

道路形状推定装置２００は、実施の形態４で説明した道路形状推定装置２００に加えて、角速度取得部２１３と、曲率算出部２４５と、曲率一定曲線算出部２４６と、前方移動物体追尾部２３５と、近似曲線交差判定部２５５と、近似曲線初期化部２５６と、近似曲線統合部２７５とを有する。

角速度取得部２１３は、入力装置９０２を用いて、角速度観測装置８３１が出力した信号を入力し、角速度観測装置８３１が観測した観測結果を取得する。角速度取得部２１３は、処理装置９１１を用いて、角速度観測装置８３１が出力した信号を入力した時刻を判定し、角速度観測装置８３１が移動体の角速度を観測した時刻を算出する。角速度取得部２１３は、記憶装置９１４を用いて、角速度観測装置８３１が移動体の角速度を観測した観測時刻や、角速度観測装置８３１が観測した移動体の角速度を表わすデータを記憶する。角速度取得部２１３が記憶するデータを「角速度観測データ」と呼ぶ。「角速度観測データ」のうち、観測時刻を表わすデータを「角速度観測時刻データ」、移動体の角速度を表わすデータを「角速度観測値データ」と呼ぶ。

曲率算出部２４５は、処理装置９１１を用いて、移動体の軌跡の曲率を算出する。例えば、曲率算出部２４５は、処理装置９１１を用いて、移動速度取得部２１２が記憶した移動速度観測データと、角速度取得部２１３が記憶した角速度観測データとを入力する。曲率算出部２４５は、処理装置９１１を用いて、入力した移動速度観測データから、移動速度観測装置８２１が観測した移動体の移動速度を表わす移動速度観測値データを取得する。曲率算出部２４５は、処理装置９１１を用いて、入力した角速度観測データから、角速度観測装置８３１が観測した移動体の角速度を表わす角速度観測値データを取得する。曲率算出部２４５は、処理装置９１１を用いて、取得した移動速度観測値データが表わす移動体の移動速度と、取得した角速度観測値データが表わす移動体の角速度とに基づいて、移動体の軌跡の曲率を算出する。曲率算出部２４５は、処理装置９１１を用いて、移動体の角速度を、移動体の移動速度で割った商を計算することにより、移動体の軌跡の曲率を算出する。
なお、移動体の移動速度の観測時刻と、移動体の角速度の観測時刻とが異なる場合、曲率算出部２４５は、処理装置９１１を用いて、時刻合わせ処理をして、時刻合わせ処理をした移動体の移動速度及び角速度に基づいて、移動体の軌跡の曲率を算出する構成であってもよい。また、曲率算出部２４５は、処理装置９１１を用いて、移動体の移動速度及び角速度をそれぞれ平滑化し、平滑化した移動速度及び角速度に基づいて、移動体の軌跡の曲率を算出する構成であってもよい。

曲率一定曲線算出部２４６は、処理装置９１１を用いて、道路の曲率が一定であるとの仮定に基づいて、道路の形状を近似する近似曲線（以下「曲率一定曲線」と呼ぶ。）を算出する。例えば、曲率一定曲線算出部２４６は、処理装置９１１を用いて、近似曲線算出部２４０が算出した近似曲線に基づいて、移動体の真横方向における移動体と近似曲線との間の距離を算出する。曲率一定曲線算出部２４６は、処理装置９１１を用いて、曲率算出部２４５が算出した移動体の軌跡の曲率に基づいて、移動体の軌跡の中心点の座標を算出する。曲率一定曲線算出部２４６は、処理装置９１１を用いて、算出した移動体の中心点の座標を中心とし、移動体の真横方向において近似曲線算出部２４０が算出した近似曲線と同じ点を通る円の方程式の係数を算出する。曲率一定曲線算出部２４６は、処理装置９１１を用いて、算出した係数を有する円の方程式を、曲率一定曲線を表わす式とする。

近似曲線統合部２７５は、処理装置９１１を用いて、近似曲線算出部２４０が算出した近似曲線と、曲率一定曲線算出部２４６が算出した曲率一定曲線とを統合して、移動体がいる道路の形状を近似する近似曲線を算出する。例えば、近似曲線統合部２７５は、処理装置９１１を用いて、移動速度取得部２１２が取得した移動体の移動速度と角速度取得部２１３が取得した移動体の角速度とに基づいて、近似曲線算出部２４０が算出した近似曲線に対する重み付けと、曲率一定曲線算出部２４６が算出した曲率一定曲線に対する重み付けとを算出する。例えば、移動体の角速度の変化が小さい場合、移動体がいる道路の曲率が一定である可能性が高いので、近似曲線統合部２７５は、曲率一定曲線に対する重み付けを大きくする。また、移動体の移動速度が大きい場合、移動体が道路の形状に沿って移動している可能性が高いので、近似曲線統合部２７５は、曲率一定曲線に対する重み付けを大きくする。例えば、近似曲線統合部２７５は、処理装置９１１を用いて、次の式の右辺を計算することにより、重み付けを算出する。

ただし、λ_ｙａｗは、曲率一定曲線算出部２４６が算出した曲率一定曲線に対する重み付けである。λ_ｆｉｘは、近似曲線算出部２４０が算出した近似曲線に対する重み付けである。Ｖは、移動速度観測装置８２１が観測し、移動速度取得部２１２が取得した移動体の移動速度である。ωは、角速度観測装置８３１が観測し、角速度取得部２１３が取得した移動体の角速度の所定時間における変化量である。Ｖ_０は、重み付け算出の基準としてあらかじめ設定された移動体の移動速度である。Δω_０は、重み付け算出の基準としてあらかじめ設定された移動体の角速度の変化量である。

近似曲線統合部２７５は、処理装置９１１を用いて、算出した重み付けに基づいて、近似曲線算出部２４０が算出した近似曲線と、曲率一定曲線算出部２４６が算出した曲率一定曲線とを統合することにより、移動体がいる道路の形状を近似する近似曲線を算出する。

なお、近似曲線統合部２７５の代わりに、近似曲線選択部を設ける構成であってもよい。近似曲線選択部は、処理装置９１１を用いて、近似曲線算出部２４０が算出した近似曲線と、曲率一定曲線算出部２４６が算出した曲率一定曲線とのうち、いずれか一方を、移動体がいる道路の形状を近似する近似曲線として採用する。例えば、近似曲線選択部は、処理装置９１１を用いて、移動速度取得部２１２が取得した移動体の移動速度が所定の閾値より大きく、かつ、角速度取得部２１３が取得した移動体の角速度の変化量が所定の閾値より小さい場合に、曲率一定曲線算出部２４６が算出した曲率一定曲線を、移動体がいる道路の形状を近似する近似曲線として採用する。それ以外の場合、近似曲線選択部は、処理装置９１１を用いて、近似曲線算出部２４０が算出した近似曲線を、移動体がいる道路の形状を近似する近似曲線として採用する。その場合、曲率一定曲線算出部２４６は、近似曲線選択部によって採用されることが決まってから、曲率一定曲線を算出する構成であってもよい。

前方移動物体追尾部２３５は、処理装置９１１を用いて、相対位置取得部２１１が取得した周辺物体観測装置８１１が観測した物体の移動体に対する相対位置及び相対速度に基づいて、移動体の前方で、移動体とほぼ同一の方向に移動している物体を追尾する。例えば、前方移動物体追尾部２３５は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データを入力する。前方移動物体追尾部２３５は、処理装置９１１を用いて、入力した周辺物体観測データから、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置を表わす相対位置観測値データと、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度を表わす相対速度観測値データとを取得する。前方移動物体追尾部２３５は、取得した相対速度観測値データが表わす相対速度の絶対値を所定の閾値と比較し、相対速度の絶対値が閾値より小さい場合に、その物体が、移動体とほぼ同一の方向に移動していると判定する。前方移動物体追尾部２３５は、移動体とほぼ同一の方向に移動していると判定した物体について、取得した相対位置データが表わす相対位置に基づいて、その物体を追尾して、その物体の移動体に対する相対位置の軌跡を求める。

近似曲線交差判定部２５５は、処理装置９１１を用いて、近似曲線算出部２４０が算出した近似曲線が複数ある場合に、いずれか２つの近似曲線が交差しているか否かを判定する。例えば、近似曲線交差判定部２５５は、処理装置９１１を用いて、近似曲線算出部２４０が算出した複数の近似曲線から、２つの近似曲線を選択し、選択した近似曲線の係数に基づいて、その２つの近似曲線が交差しているか否かを判別する判別式の値を計算する。近似曲線交差判定部２５５は、処理装置９１１を用いて、算出した判別式の値に基づいて、選択した２つの近似曲線が交差しているか否かを判定する。
なお、近似曲線交差判定部２５５は、２つの近似曲線が交差している場合であっても、近似曲線が交差している点が移動体から遠い場合には、誤差範囲内であるとみなし、２つの近似曲線が交差していないものとして取り扱う構成であってもよい。例えば、判別式の値から２つの近似曲線が交差していると判定した場合、近似曲線交差判定部２５５は、処理装置９１１を用いて、２つの近似曲線が交差している点の移動体に対する相対座標を算出する。近似曲線交差判定部２５５は、処理装置９１１を用いて、算出した相対座標から、２つの近似曲線が交差している点と、移動体との間の距離を算出する。近似曲線交差判定部２５５は、処理装置９１１を用いて、算出した距離を所定の閾値と比較する。算出した距離が閾値より小さい場合に、近似曲線交差判定部２５５は、処理装置９１１を用いて、選択した２つの近似曲線が交差していると判定する。それ以外の場合、近似曲線交差判定部２５５は、処理装置９１１を用いて、選択した２つの近似曲線が交差していないと判定する。

近似曲線初期化部２５６は、処理装置９１１を用いて、近似曲線交差判定部２５５が交差していると判定した近似曲線それぞれについて、その近似曲線が正しいか否かを検証する。例えば、近似曲線初期化部２５６は、処理装置９１１を用いて、近似曲線交差判定部２５５が交差していると判定した近似曲線それぞれについて、前方移動物体追尾部２３５が算出した物体の移動体に対する相対位置の軌跡とが交わっているか否かを判定する。前方移動物体追尾部２３５が算出した物体の移動体に対する相対位置の軌跡と交わっていると判定した場合、近似曲線初期化部２５６は、処理装置９１１を用いて、その近似曲線が正しくないと判定する。

なお、近似曲線初期化部２５６は、他の方式によって、近似曲線が正しいか否かを検証する構成であってもよい。例えば、近似曲線初期化部２５６は、処理装置９１１を用いて、近似曲線交差判定部２５５が交差していると判定した近似曲線それぞれについて、移動体の真横における曲率を算出する。近似曲線初期化部２５６は、処理装置９１１を用いて、算出した曲率と、曲率算出部２４５が算出した曲率との間の差の絶対値を計算する。近似曲線初期化部２５６は、処理装置９１１を用いて、計算した曲率の差の絶対値を所定の閾値と比較し、曲率の差の絶対値が閾値より大きい場合に、その近似曲線が正しくないと判定する。

ある近似曲線が正しくないと判定した場合、近似曲線初期化部２５６は、処理装置９１１を用いて、正しくないと判定した近似曲線の係数を初期化する。例えば、近似曲線初期化部２５６は、処理装置９１１を用いて、正しくないと判定した近似曲線の係数の値を、近似曲線算出部２４０が算出した他の近似曲線や、前方移動物体追尾部２３５が算出した物体の移動体に対する相対位置の軌跡と交わらなくなるよう、修正する。

あるいは、近似曲線算出部２４０は、近似曲線初期化部２５６が正しくないと判定した近似曲線を破棄する構成であってもよい。

以上、各実施の形態で説明した構成は、一例であり、異なる実施の形態で説明した構成を組み合わせた構成としてもよい。また、主要でない部分の構成を既存の技術など他の構成と置き換えるなどの変形をした構成としてもよい。

以上説明した道路形状推定装置（２００；道路形状認識装置１００）は、相対位置取得部（２１１；停止物識別部１３０）と、静止物体判定部（２２０；停止物識別部１３０）と、物体相関部（２３０；道路近似曲線仮計算部１４０）と、近似曲線算出部（２４０；道路近似曲線本計算部１６０）とを有する。
上記相対位置取得部（２１１；１３０）は、移動体（８０１）の周辺に存在する物体について、上記移動体（８０１）を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置（８１１；レーダ１１０）が観測した観測結果を取得する。
上記静止物体判定部（２２０；１３０）は、上記相対位置取得部（２１１；１３０）が取得した観測結果に基づいて、上記周辺物体観測装置（８１１；１１０）が相対位置を観測した物体が静止しているか否かを判定する。
上記物体相関部（２３０；１４０）は、上記相対位置取得部（２１１；１３０）が取得した観測結果に基づいて、上記周辺物体観測装置（８１１；１１０）が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置（８１１；１１０）が観測した複数の相対位置を判定する。
上記近似曲線算出部（２４０；１６０）は、上記静止物体判定部（２２０；１３０）が判定した判定結果と、上記物体相関部（２３０；１４０）が判定した判定結果とに基づいて、上記移動体（８０１）がいる道路の形状を近似する近似曲線を算出する。

静止物体判定部の判定結果と、物体相関部の判定結果とに基づいて、近似曲線を算出するので、誤検出や誤判定の影響を排除し、算出する近似曲線の信頼性を高めることができる。

上記近似曲線算出部（２４０；１６０）は、上記近似曲線を表わす所定の関数の係数を状態量とし、上記相対位置取得部（２１１；１３０）が取得した観測結果のうち、上記静止物体判定部（２２０；１３０）が静止していると判定した物体について上記周辺物体観測装置（８１１；１１０）が観測した相対位置を観測量とする追尾フィルタを使って、上記近似曲線を表わす関数の係数を推定する。

追尾フィルタを使って近似曲線を表わす関数の係数を推定するので、観測時刻の異なる観測結果を近似曲線の算出に使うことができる。これにより、近似曲線の算出に使う観測結果の数が増えるので、算出する近似曲線の信頼性を高めることができる。

上記近似曲線算出部（２４０；１６０）は、上記周辺物体観測装置（８１１；１１０）が観測した相対位置と、上記近似曲線との間の距離が、所定の閾値より短い場合に、上記相対位置を観測量として上記追尾フィルタの更新処理をする。

周辺物体観測装置８１１が観測した相対位置と近似曲線との間の距離に基づいて、相対位置を近似曲線に相関させるので、その相対位置を、相関の取れた近似曲線の更新に利用することができる。

上記道路形状推定装置（２００；１００）は、初期値算出部（２５０；道路近似曲線仮計算部１４０）を有する。
上記初期値算出部（２５０；１４０）は、同一の物体について上記周辺物体観測装置（８１１；１１０）が観測した複数の相対位置であると上記物体相関部（２３０；１４０）が判定した複数の相対位置について、上記複数の相対位置の軌跡を上記近似曲線を表わす所定の関数で近似し、上記複数の相対位置の軌跡を近似する関数の係数を算出する。
上記近似曲線算出部（２４０；１６０）は、上記初期値算出部（２５０；１４０）が算出した係数を、上記追尾フィルタの状態量の推定値の初期値とする。

相対位置の軌跡は、移動体の動きを反映しているので、移動体が道路に沿って移動していれば、道路形状を反映する。相対位置の軌跡から算出した初期値は、道路形状を正確に近似している近似曲線の係数とは言えないが、追尾処理の初期値としては、十分に有効な精度を有する。これにより、少ない計算量で、追尾処理に必要な初期値を得ることができる。

上記初期値算出部（２５０；１４０）は、同一の物体について上記周辺物体観測装置（８１１；１１０）が観測した複数の相対位置であると上記物体相関部（２３０；１４０）が判定した複数の相対位置のうち、一つの物体を観測した複数の相対位置の数（相関回数）が、所定の閾値より多い複数の相対位置に基づいて、上記複数の相対位置の軌跡を近似する関数の係数を算出する。

相関回数が閾値より多い相対位置に基づいて初期値を算出するので、算出する初期値の精度が高くなる。

上記初期値算出部（２５０；１４０）は、同一の物体について上記周辺物体観測装置（８１１；１１０）が観測した複数の相対位置であると上記物体相関部（２３０；１４０）が判定した複数の相対位置のうち、一つの物体を観測した複数の相対位置のなかに上記物体と上記移動体（８０１）との間の距離が、所定の閾値より近い相対位置がある複数の相対位置に基づいて、上記複数の相対位置の軌跡を近似する関数の係数を算出する。

移動体からの距離が閾値より小さくなった物体の相対位置に基づいて初期値を算出するので、算出する初期値の精度が高くなる。

上記道路形状推定装置（２００；１００）は、信頼度算出部（２６０；近似曲線選択部１７０）を有する。
上記信頼度算出部（２６０；１７０）は、上記近似曲線算出部（２４０；１６０）が算出した近似曲線の信頼度を算出する。
上記近似曲線算出部（２４０；１６０）は、上記信頼度算出部（２６０；１７０）が算出した信頼度が所定の閾値より低い場合に、算出した近似曲線を破棄する。

信頼度が低い近似曲線を破棄することにより、近似曲線の信頼性を維持することができる。

上記道路形状推定装置（２００；１００）は、信頼度算出部（２６０；１７０）と、近似曲線選択部（２７０；１７０）とを有する。
上記近似曲線算出部（２４０；１６０）は、複数の近似曲線を算出する。
上記信頼度算出部（２６０；１７０）は、上記近似曲線算出部（２４０；１６０）が算出した複数の近似曲線それぞれの信頼度を算出する。
上記近似曲線選択部（２７０；１７０）は、上記信頼度算出部（２６０；１７０）が算出した信頼度に基づいて、上記近似曲線算出部（２４０；１６０）が算出した複数の近似曲線のなかから、採用する近似曲線を選択する。

これにより、信頼度が高い近似曲線を採用することができる。

上記信頼度算出部（２６０；１７０）は、上記近似曲線算出部（２４０；１６０）が上記近似曲線の算出に使った相対位置と、上記移動体（８０１）との間の距離の分散度を算出し、算出した分散度が大きいほど信頼度を高くする。

近似曲線の算出に使った相対位置が空間的に分散していれば、近似曲線の変化が少なく、安定していると言える。安定している近似曲線の信頼度を高くすることにより、近似曲線の信頼性を高めることができる。

上記道路形状推定装置（２００；１００）は、移動速度取得部（２１２；道路曲率変換部１２６）と、角速度取得部（２１３；道路曲率変換部１２６）と、曲率算出部（２４５；道路曲率変換部１２６）と、曲率一定曲線算出部（２４６；推定値選択部１９０）と、近似曲線選択部（推定値選択部１９０）とを有する。
上記移動速度取得部（２１２；１２６）は、上記移動体の移動速度を観測する移動速度観測装置（８２１；車速センサ１２０）が観測した観測結果を取得する。
上記角速度取得部（２１３；１２６）は、上記移動体の角速度を観測する角速度観測装置（８３１；ヨーレートセンサ１２５）が観測した観測結果を取得する。
上記曲率算出部（２４５；１２６）は、上記移動速度取得部（２１２；１２６）が取得した観測結果と、上記角速度取得部（２１３；１２６）が取得した観測結果とに基づいて、上記移動体の軌跡の曲率として、上記移動体の角速度を上記移動体の移動速度で割った商を算出する。
上記曲率一定曲線算出部（２４６；１９０）は、上記曲率算出部（２４５；１２６）が算出した曲率に基づいて、上記移動体がいる道路の形状を近似する近似曲線として、曲率が一定である曲率一定曲線を算出する。
上記近似曲線選択部（１９０）は、上記近似曲線算出部（２４０；１６０）が算出した近似曲線と上記曲率一定曲線算出部（２４６；１９０）が算出した曲率一定曲線とのなかから、上記移動体がいる道路の形状を近似する近似曲線として採用する近似曲線を選択する。

移動体の走行状態などに応じて、最適な近似曲線を選択するので、道路形状推定の精度が向上する。

上記近似曲線選択部（自車走行路判定部１８０、推定値選択部１９０）は、少なくとも直線路と曲線路手前と曲線路と曲線路出口との４種類に類別された道路形状種別のなかから、上記移動体がいる道路の形状の道路形状種別を判定し、判定した道路形状種別が曲線路である場合に、上記曲率一定曲線算出部（２４６；１９０）が算出した曲率一定曲線を、上記移動体がいる道路の形状を近似する近似曲線として選択する。

曲率が一定の曲線路では、移動体の角速度から算出した曲率によって、道路形状を精度よく近似することができるので、道路形状推定の精度が向上する。

上記道路形状推定装置（２００；１００）は、移動速度取得部（２１２；１２６）と、角速度取得部（２１３；１２６）と、曲率算出部（２４５；１２６）と、曲率一定曲線算出部（２４６；１９０）と、近似曲線統合部（２７５；１９０）とを有する。
上記移動速度取得部（２１２；１２６）は、上記移動体の移動速度を観測する移動速度観測装置（８２１；１２０）が観測した観測結果を取得する。
上記角速度取得部（２１３；１２６）は、上記移動体の角速度を観測する角速度観測装置（８３１；１２５）が観測した観測結果を取得する。
上記曲率算出部（２４５；１２６）は、上記移動速度取得部（２１２；１２６）が取得した観測結果と、上記角速度取得部（２１３；１２６）が取得した観測結果とに基づいて、上記移動体の軌跡の曲率として、上記移動体の角速度を上記移動体の移動速度で割った商を算出する。
上記曲率一定曲線算出部（２４６；１９０）は、上記曲率算出部（２４５；１２６）が算出した曲率に基づいて、上記移動体がいる道路の形状を近似する近似曲線として、曲率が一定である曲率一定曲線を算出する。
上記近似曲線統合部（２７５；１９０）は、上記近似曲線算出部（２４０；１６０）が算出した近似曲線に対する重み付けと、上記曲率一定曲線算出部（２４６；１９０）が算出した曲率一定曲線に対する重み付けとを算出し、算出した重み付けに基づいて、上記近似曲線算出部（２４０；１６０）が算出した近似曲線と上記曲率一定曲線算出部（２４６；１９０）が算出した曲率一定曲線とを統合して、上記移動体がいる道路の形状を近似する近似曲線を算出する。

移動体の走行状態などに応じて重み付けを変え、２種類の曲線を統合するので、道路形状推定の精度が向上する。

上記近似曲線統合部（２７５；１８０、１９０）は、少なくとも直線路と曲線路手前と曲線路と曲線路出口との４種類に類別された道路形状種別のなかから、上記移動体がいる道路の形状の道路形状種別を判定し、判定した道路形状種別が曲線路である場合に、上記曲率一定曲線算出部（２４６；１９０）が算出した曲率一定曲線に対する重み付けを重くする。

曲率が一定の曲線路では、移動体の角速度から算出した曲率によって、道路形状を精度よく近似することができるので、道路形状推定の精度が向上する。

上記道路形状推定装置（２００；１００）は、前方移動物体追尾部（２３５；前方車追尾部１３５）と、近似曲線交差判定部（２５５；補正部１７４）と、近似曲線初期化部（２５６；補正部１７４）とを有する。
上記前方移動物体追尾部（２３５；１３５）は、上記相対位置取得部（２１１；１３５）が取得した観測結果に基づいて、上記移動体の前方を上記移動体と略同一の方向に移動している物体を追尾する。
上記近似曲線交差判定部（２５５；１７４）は、上記近似曲線算出部（２４０；１６０）が複数の近似曲線を算出した場合に、上記複数の近似曲線が交差しているか否かを判定する。
上記近似曲線初期化部（２５６；１７４）は、上記複数の近似曲線が交差していると上記近似曲線交差判定部（２５５；１７４）が判定した場合に、上記近似曲線交差判定部（２５５；１７４）が交差していると判定した複数の近似曲線それぞれについて、上記前方移動物体追尾部（２３５；１３５）が追尾した物体との位置関係に基づいて、上記複数の近似曲線それぞれが正しいか否かを判定し、正しくないと判定した近似曲線を初期化する。

複数の近似曲線が交差している場合、交差している近似曲線のいずれかが正しくない可能性が高い。移動体の前方を移動体と略同一の方向に移動している物体は、道路形状に沿って移動している可能性が高いので、近似曲線との位置関係に基づいて、どの近似曲線が正しくないかを判定できる。このようにして、正しくない近似曲線を判定し、初期化するので、道路形状推定の精度が向上する。

以上説明した道路形状認識装置（１００；２００）は、コンピュータが実行することにより、コンピュータを道路形状推定装置として機能させるコンピュータプログラムにより実現することができる。

１００ 道路形状認識装置、１１０ レーダ、１２０ 車速センサ、１２５ ヨーレートセンサ、１２６ 道路曲率変換部、１３０ 停止物識別部、１３５ 前方車追尾部、１４０ 道路近似曲線仮計算部、１４１ 停止物クラスタリング部、１４３ 追尾フィルタ、１４４，１６４ 相関部、１４５，１６５ 平滑部、１４６，１６６ 予測部、１４７，１６７ 遅延部、１４８ 相関データ蓄積部、１４９ 近似曲線推定部、１５０ 道路縁判定部、１６０ 道路近似曲線本計算部、１６１ 初期値設定部、１６２ グルーピング処理部、１６３ 追尾フィルタＡ、１７０ 近似曲線選択部、１７１ 有効性判定部、１７２ 曲線品質計算部、１７３ 選択部、１７４ 補正部、１７５ 平滑部、１７６ 係数推定値蓄積部、１８０ 自車走行路判定部、１９０ 推定値選択部、２００ 道路形状推定装置、２１１ 相対位置取得部、２１２ 移動速度取得部、２１３ 角速度取得部、２２０ 静止物体判定部、２３０ 物体相関部、２３５ 前方移動物体追尾部、２４０ 近似曲線算出部、２４５ 曲率算出部、２４６ 曲率一定曲線算出部、２５０ 初期値算出部、２５５ 近似曲線交差判定部、２５６ 近似曲線初期化部、２６０ 信頼度算出部、２７０ 近似曲線選択部、２７５ 近似曲線統合部、８００ 道路形状推定システム、８１１ 周辺物体観測装置、８２１ 移動速度観測装置、８３１ 角速度観測装置、９０１ 出力装置、９０２ 入力装置、９１１ 処理装置、９１４ 記憶装置。

Claims (16)

1. 相対位置取得部と、静止物体判定部と、物体相関部と、近似曲線算出部とを有し、
   上記相対位置取得部は、移動体の周辺に存在する物体について、上記移動体を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置が観測した観測結果を取得し、
   上記静止物体判定部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が相対位置を観測した物体が静止しているか否かを判定し、
   上記物体相関部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置が観測した複数の相対位置を判定し、
   上記近似曲線算出部は、上記静止物体判定部が判定した判定結果と、上記物体相関部が判定した判定結果とに基づいて、上記移動体がいる道路の形状を近似する近似曲線を算出することを特徴とする道路形状推定装置。
2. 上記近似曲線算出部は、上記近似曲線を表わす所定の関数の係数を状態量とし、上記相対位置取得部が取得した観測結果のうち、上記静止物体判定部が静止していると判定した物体について上記周辺物体観測装置が観測した相対位置を観測量とする追尾フィルタを使って、上記近似曲線を表わす関数の係数を推定することを特徴とする請求項１に記載の道路形状推定装置。
3. 上記近似曲線算出部は、上記周辺物体観測装置が観測した相対位置と、上記近似曲線との間の距離が、所定の閾値より短い場合に、上記相対位置を観測量として上記追尾フィルタの更新処理をすることを特徴とする請求項２に記載の道路形状推定装置。
4. 上記道路形状推定装置は、初期値算出部を有し、
   上記初期値算出部は、同一の物体について上記周辺物体観測装置が観測した複数の相対位置であると上記物体相関部が判定した複数の相対位置について、上記複数の相対位置の軌跡を上記近似曲線を表わす所定の関数で近似し、上記複数の相対位置の軌跡を近似する関数の係数を算出し、
   上記近似曲線算出部は、上記初期値算出部が算出した係数を、上記追尾フィルタの状態量の推定値の初期値とすることを特徴とする請求項２または請求項３に記載の道路形状推定装置。
5. 上記初期値算出部は、同一の物体について上記周辺物体観測装置が観測した複数の相対位置であると上記物体相関部が判定した複数の相対位置のうち、一つの物体を観測した複数の相対位置の数が、所定の閾値より多い複数の相対位置に基づいて、上記複数の相対位置の軌跡を近似する関数の係数を算出することを特徴とする請求項４に記載の道路形状推定装置。
6. 上記初期値算出部は、同一の物体について上記周辺物体観測装置が観測した複数の相対位置であると上記物体相関部が判定した複数の相対位置のうち、一つの物体を観測した複数の相対位置のなかに上記物体と上記移動体との間の距離が、所定の閾値より近い相対位置がある複数の相対位置に基づいて、上記複数の相対位置の軌跡を近似する関数の係数を算出することを特徴とする請求項４または請求項５に記載の道路形状推定装置。
7. 上記道路形状推定装置は、信頼度算出部を有し、
   上記信頼度算出部は、上記近似曲線算出部が算出した近似曲線の信頼度を算出し、
   上記近似曲線算出部は、上記信頼度算出部が算出した信頼度が所定の閾値より低い場合に、算出した近似曲線を破棄することを特徴とする請求項１乃至請求項６のいずれかに記載の道路形状推定装置。
8. 上記道路形状推定装置は、信頼度算出部と、近似曲線選択部とを有し、
   上記近似曲線算出部は、複数の近似曲線を算出し、
   上記信頼度算出部は、上記近似曲線算出部が算出した複数の近似曲線それぞれの信頼度を算出し、
   上記近似曲線選択部は、上記信頼度算出部が算出した信頼度に基づいて、上記近似曲線算出部が算出した複数の近似曲線のなかから、採用する近似曲線を選択することを特徴とする請求項１乃至請求項６のいずれかに記載の道路形状推定装置。
9. 上記信頼度算出部は、上記近似曲線算出部が上記近似曲線の算出に使った相対位置と、上記移動体との間の距離の分散度を算出し、算出した分散度が大きいほど信頼度を高くすることを特徴とする請求項７または請求項８に記載の道路形状推定装置。
10. 上記道路形状推定装置は、移動速度取得部と、角速度取得部と、曲率算出部と、曲率一定曲線算出部と、近似曲線選択部とを有し、
    上記移動速度取得部は、上記移動体の移動速度を観測する移動速度観測装置が観測した観測結果を取得し、
    上記角速度取得部は、上記移動体の角速度を観測する角速度観測装置が観測した観測結果を取得し、
    上記曲率算出部は、上記移動速度取得部が取得した観測結果と、上記角速度取得部が取得した観測結果とに基づいて、上記移動体の軌跡の曲率として、上記移動体の角速度を上記移動体の移動速度で割った商を算出し、
    上記曲率一定曲線算出部は、上記曲率算出部が算出した曲率に基づいて、上記移動体がいる道路の形状を近似する近似曲線として、曲率が一定である曲率一定曲線を算出し、
    上記近似曲線選択部は、上記近似曲線算出部が算出した近似曲線と上記曲率一定曲線算出部が算出した曲率一定曲線とのなかから、上記移動体がいる道路の形状を近似する近似曲線として採用する近似曲線を選択することを特徴とする請求項１乃至請求項９のいずれかに記載の道路形状推定装置。
11. 上記近似曲線選択部は、少なくとも直線路と曲線路手前と曲線路と曲線路出口との４種類に類別された道路形状種別のなかから、上記移動体がいる道路の形状の道路形状種別を判定し、判定した道路形状種別が曲線路である場合に、上記曲率一定曲線算出部が算出した曲率一定曲線を、上記移動体がいる道路の形状を近似する近似曲線として選択することを特徴とする請求項１０に記載の道路形状推定装置。
12. 上記道路形状推定装置は、移動速度取得部と、角速度取得部と、曲率算出部と、曲率一定曲線算出部と、近似曲線統合部とを有し、
    上記移動速度取得部は、上記移動体の移動速度を観測する移動速度観測装置が観測した観測結果を取得し、
    上記角速度取得部は、上記移動体の角速度を観測する角速度観測装置が観測した観測結果を取得し、
    上記曲率算出部は、上記移動速度取得部が取得した観測結果と、上記角速度取得部が取得した観測結果とに基づいて、上記移動体の軌跡の曲率として、上記移動体の角速度を上記移動体の移動速度で割った商を算出し、
    上記曲率一定曲線算出部は、上記曲率算出部が算出した曲率に基づいて、上記移動体がいる道路の形状を近似する近似曲線として、曲率が一定である曲率一定曲線を算出し、
    上記近似曲線統合部は、上記近似曲線算出部が算出した近似曲線に対する重み付けと、上記曲率一定曲線算出部が算出した曲率一定曲線に対する重み付けとを算出し、算出した重み付けに基づいて、上記近似曲線算出部が算出した近似曲線と上記曲率一定曲線算出部が算出した曲率一定曲線とを統合して、上記移動体がいる道路の形状を近似する近似曲線を算出することを特徴とする請求項１乃至請求項９のいずれかに記載の道路形状推定装置。
13. 上記近似曲線統合部は、少なくとも直線路と曲線路手前と曲線路と曲線路出口との４種類に類別された道路形状種別のなかから、上記移動体がいる道路の形状の道路形状種別を判定し、判定した道路形状種別が曲線路である場合に、上記曲率一定曲線算出部が算出した曲率一定曲線に対する重み付けを重くすることを特徴とする請求項１２に記載の道路形状推定装置。
14. 上記道路形状推定装置は、前方移動物体追尾部と、近似曲線交差判定部と、近似曲線初期化部とを有し、
    上記前方移動物体追尾部は、上記相対位置取得部が取得した観測結果に基づいて、上記移動体の前方を上記移動体と略同一の方向に移動している物体を追尾し、
    上記近似曲線交差判定部は、上記近似曲線算出部が複数の近似曲線を算出した場合に、上記複数の近似曲線が交差しているか否かを判定し、
    上記近似曲線初期化部は、上記複数の近似曲線が交差していると上記近似曲線交差判定部が判定した場合に、上記近似曲線交差判定部が交差していると判定した複数の近似曲線それぞれについて、上記前方移動物体追尾部が追尾した物体との位置関係に基づいて、上記複数の近似曲線それぞれが正しいか否かを判定し、正しくないと判定した近似曲線を初期化することを特徴とする請求項１乃至請求項１３のいずれかに記載の道路形状推定装置。
15. コンピュータが実行することにより、上記コンピュータが請求項１乃至請求項１４のいずれかに記載の道路形状推定装置として機能することを特徴とするコンピュータプログラム。
16. 相対位置取得部と、静止物体判定部と、物体相関部と、近似曲線算出部とを有する道路形状推定装置が、移動体がいる道路の形状を推定する道路形状推定方法において、
    上記相対位置取得部が、移動体の周辺に存在する物体について、上記移動体を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置が観測した観測結果を取得し、
    上記静止物体判定部が、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が相対位置を観測した物体が静止しているか否かを判定し、
    上記物体相関部が、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置が観測した複数の相対位置を判定し、
    上記近似曲線算出部が、上記静止物体判定部が判定した判定結果と、上記物体相関部が判定した判定結果とに基づいて、上記移動体がいる道路の形状を近似する近似曲線を算出することを特徴とする道路形状推定方法。

Images