JP6345138B2 - 道路形状推定装置及び道路形状推定方法 - Google Patents

Description

本発明は、車両などの移動体に搭載されたセンシング機器（レーザー、レーダー、カメラなど）を用いて得られた観測データ群から道路形状を推定する技術に関する。

従来の道路形状推定技術として、例えば、下記の非特許文献１や非特許文献２に開示されている技術が存在する。非特許文献１や非特許文献２には、センシングされた道路端の観測データ群に基づいて、道路形状を推定する手法が開示されている。

センシング機器によって道路がセンシングされると、道路の左右に存在する物体（縁石、ガードレール、壁など）の位置情報を含む観測データ群が得られる。観測データ群は、道路の延伸方向（道路上を走行する車両の進行方向）に沿って、部分的に途切れた線や点の集合として得られる。とりわけ、点の集合としてセンシングされる場合の様子を図５４（ａ）に示す。図５４（ａ）には、記号「＋」によって、センシングされた物体の座標（観測点）が表されている。これらは多くの場合、車両に設置されたカメラやレーザー、レーダーなどのセンシング機器によって、車両の近傍から遠方（５０ｍから１５０ｍ先）までセンシングされる。なお、本明細書では、センシング機器によるセンシングで得られるデータを観測データ、複数の観測データを観測データ群、センシングされた物体及びその位置を観測点と呼ぶ。

道路形状を推定するためには、得られた部分的な観測データをつないで、道路端左右の線として復元しなければならない。その復元した結果の例を図５４（ｂ）に示す。

非特許文献１や非特許文献２に開示されている従来の道路形状推定技術では、センシングされた道路端の観測データ群のうち、どの観測データが道路の左端又は右端に属するかが既知となっており、どちらか片方の集合のみを選別して所定の曲線にフィットさせ、片方の道路端形状を推定する。フィットさせる曲線にはクロソイド曲線や多項式が用いられている。

また、下記の特許文献１には、センサーによってセンシングされた観測データ群に対して所定の曲線（クロソイド曲線又は曲率半径が一定の曲線）を適用して道路の形状を推定するシステムにおいて、道路における白線の画像を取得し、取得された白線の画像に基づいて道路の曲率半径を推定する技術が開示されている。

再公表特許 ＷＯ２０１２／１３７３５４（請求項７）

IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, No.2 Feb 1992, "Recursive 3-D road and relative ego-state recognition", Dickmanns, E.D. and Mysliwetz, B.D Sensors 2013, 13, 3270-3298; "Robust Lane Sensing and Departure Warning under Shadows and Occlusions" Rodolfo Tapia-Espinoza and Miguel Torres-Torriti

上述のように、非特許文献１や非特許文献２の開示技術では、センシングされた道路端の観測データが道路の左端又は右端のどちらに属するかが既知である故、道路の左端又は右端のどちらか一方の観測データ群の集合を用いて、片方の道路端形状の推定が可能である。しかしながら、センシングされた道路端の観測データが道路の左端又は右端のどちらに属するか既知ではない場合も考えられる。

例えば、センシングされた観測点がセンサーから遠く離れている場合（例えば１００ｍ以上の長距離の場合）、センシングされた道路端の観測データが、道路の左側の観測データなのか、右側の観測データなのかを区別することが容易ではない場合がある。その状況を図５４（ｃ）に示す。

センシングされた道路端の観測データ群のうち、センサーから近距離に存在する観測点の観測データについては、道路の左端又は右端のどちらに属するかを正確に推定することができる。例えば、図５４（ｃ）に図示されている観測点Ａは、センサーの左手（車両の進行方向に対して左側）から検知されており、道路の左端のものと推定できる。

しかしながら、センシングされた道路端の観測データ群のうち、センサーから遠距離に存在する観測点の観測データは、道路全体の形状に影響されて、道路の左端又は右端のどちらに属するかを正確に推定することができない場合がある。例えば、図５４（ｃ）に示す観測点Ｂは、道路全体が右に曲がっているため、センサーの右手（車両の進行方向に対して右側）から検知される。その結果、実際には観測点Ｂは道路の左端に存在しているにもかかわらず、道路の右端に位置する観測点であると勘違いしてしまう可能性がある。このような誤認識は、観測点がセンサーから遠方であるほど、あるいは観測データにノイズが多く含まれているほど問題となってくる。本明細書では、上記の誤認識に係る問題を「観測データ左右判定問題」と呼ぶ。

特許文献１の開示技術によれば、センシングされた観測データ群に加えて、道路の白線の画像を用いてセンサーから遠距離の道路の形状（道路の曲率）を推定することが可能である。その結果、道路の形状を正確に推定することができた場合には、センシングされた観測データが道路の左端又は右端のどちらに属するかを正確に推定し、これによって「観測データ左右判定問題」が解決されるかもしれない。しかしながら、例えば、道路の白線が部分的な点としてしか検知できない場合には道路の形状を正確に推定できず、依然として「観測データ左右判定問題」は解決されない。

本発明は、上記の問題を考慮して、センシングされた観測データが道路の左端又は右端のどちらに属するかをより精度良く推定することで、道路の形状をより正確に推定できるようにする道路形状推定装置及び道路形状推定方法を提供することを目的とする。

上記目的を達成するため、本発明によれば、道路を走行する移動体に設置されたセンシング機器によってセンシングされた観測データ群を用いて、前記道路の形状を推定する道路形状推定装置であって、
前記センシングにおける所定の範囲内に存在する観測データ群を取得する観測データ群取得部と、
位置座標及び進行方向をパラメータとして有するフィルタを前記観測データ群が分布する仮想空間に配置し、前記観測データ群取得部によって取得された前記観測データ群を用いて前記フィルタの進行方向を計算する進行方向計算部と、
前記進行方向計算部で計算された前記フィルタの進行方向に沿って、前記フィルタを前記仮想空間内で移動させるフィルタ移動部と、
前記フィルタ移動部によって移動された前記フィルタの移動軌跡を、前記道路の延伸方向に沿った経路として出力する道路形状出力部とを有し、
前記進行方向計算部が、
前記フィルタの現在の位置を基準として所定の範囲内に存在する前記観測データを選択する選択部と、
前記選択された観測データの位置から所定の距離だけ、前記フィルタの現在の進行方向に対して垂直な方向に移動させた投影点を計算する投影点計算部と、
前記フィルタの現在の位置と前記投影点とを結ぶベクトルの方向を計算するベクトル方向計算部と、
前記計算されたベクトルの方向を用いて前記フィルタの新たな進行方向を設定する進行方向設定部と、
前記フィルタの新たな進行方向に向かって所定の移動距離だけ前記フィルタを移動させて、前記フィルタの現在の座標が移動後の前記フィルタの位置と設定されるよう更新し、前記フィルタの現在の進行方向が前記フィルタの新たな進行方向と設定されるよう更新処理部とを、
有する道路形状推定装置が提供される。

上記目的を達成するため、本発明によれば、道路を走行する移動体に設置されたセンシング機器によってセンシングされた観測データ群を用いて、前記道路の形状を推定する道路形状推定方法であって、
前記センシングにおける所定の範囲内に存在する観測データ群を取得する観測データ群取得ステップと、
位置座標及び進行方向をパラメータとして有するフィルタを前記観測データ群が分布する仮想空間に配置し、前記観測データ群取得ステップで取得された前記観測データ群を用いて前記フィルタの進行方向を計算する進行方向計算ステップと、
前記進行方向計算ステップで計算された前記フィルタの進行方向に沿って、前記フィルタを前記仮想空間内で移動させるフィルタ移動ステップと、
前記フィルタ移動ステップで移動された前記フィルタの移動軌跡を、前記道路の延伸方向に沿った経路として出力する道路形状出力ステップとを有し、
前記進行方向計算ステップが、
前記フィルタの現在の位置を基準として所定の範囲内に存在する前記観測データを選択する選択ステップと、
前記選択された観測データの位置から所定の距離だけ、前記フィルタの現在の進行方向に対して垂直な方向に移動させた投影点を計算する投影点計算ステップと、
前記フィルタの現在の位置と前記投影点とを結ぶベクトルの方向を計算するベクトル方向計算ステップと、
前記計算されたベクトルの方向を用いて前記フィルタの新たな進行方向を設定する進行方向設定ステップと、
前記フィルタの新たな進行方向に向かって所定の移動距離だけ前記フィルタを移動させて、前記フィルタの現在の座標が移動後の前記フィルタの位置と設定されるよう更新し、前記フィルタの現在の進行方向が前記フィルタの新たな進行方向と設定されるよう更新処理ステップとを、
有する道路形状推定方法が提供される。

本発明の道路形状推定装置及び道路形状推定方法は、道路形状をより正確に推定できるようになるという効果を奏する。特に、道路形状として、道路の延伸方向に沿った経路（例えば、道路の中央を通る経路）の形状を推定することが可能となる。さらに、道路幅を推定することによって、道路幅を持った道路形状を推定することも可能である。

本発明において、観測データ群の分布する空間をスキャンするためのフィルタ（スイーパー）を示す図である。 本発明の第１の実施の形態における基本的な処理の一例を示すフローチャートである 本発明の第１の実施の形態において、観測データ群の読み取り範囲の一例を示す図である。 本発明の第１の実施の形態におけるスイーパー計算例１の一例を示すフローチャートである。 本発明の第１の実施の形態において、スイーパー計算例１による観測点Ａに係る処理の一例を示す図である。 本発明の第１の実施の形態において、スイーパー計算例１による観測点Ｂに係る処理の一例を示す図である。 本発明の第１の実施の形態において、スイーパー計算例１による観測点Ｃに係る処理の一例を示す図である。 本発明の第１の実施の形態において、スイーパー計算例１によって得られるスイーパーの移動軌跡の一例を示す図である。 本発明の第１の実施の形態におけるスイーパー計算例２の一例を示すフローチャートである。 本発明の第１の実施の形態において、スイーパー計算例２による観測点Ａ、Ｂ、Ｃに係る処理の一例を示す図である。 本発明の第１の実施の形態において、スイーパー計算例２によって、得られるスイーパーの進行方向の一例を示す図である。 本発明の第１の実施の形態において、スイーパー計算例２によって得られるスイーパーの移動軌跡の一例を示す図である。 本発明の第１の実施の形態におけるスイーパー計算例３の一例を示すフローチャートである。 本発明の第１の実施の形態において、スイーパー計算例３による観測点Ａに係る処理（スイーパー回転前）の例を示す図である 本発明の第１の実施の形態において、スイーパー計算例３による観測点Ａに係る処理（スイーパー回転後）の例を示す図である。 本発明の第１の実施の形態において、スイーパー計算例３による観測点Ｂに係る処理（スイーパー回転前）の例を示す図である 本発明の第１の実施の形態において、スイーパー計算例３による観測点Ｂに係る処理（スイーパー回転後）の例を示す図である。 本発明の第１の実施の形態において、スイーパー計算例３による観測点Ｃに係る処理（スイーパー回転前）の例を示す図である 本発明の第１の実施の形態において、スイーパー計算例３によって得られる進行方向及び移動後の位置の一例を示す図である。 （ａ）は、本発明の第１の実施の形態において、スイーパーの初期位置が定まらない状態の一例を示す図であり、（ｂ）は、本発明の第１の実施の形態において、スイーパーの初期位置を定める方法の一例を示す図である。 本発明の第１の実施の形態における道路形状推定装置の構成の一例を示すブロック図である。 本発明の第１の実施の形態において、真の道路端の真上に観測点が存在する状態の一例を示す図である。 本発明の第１の実施の形態において、真の道路端の真上に存在する観測点から得られる投影点の一例を示す図である。 本発明の第１の実施の形態において、真の道路端の真上に観測点が存在する場合のスイーパーの移動軌跡の一例を示す図である。 本発明の第１の実施の形態において、真の道路端の真上に観測点が存在する場合のスイーパーの進行方向をプロットしたグラフの一例である。 本発明の第１の実施の形態において、真の道路端の真上に観測点が存在していない状態の一例を示す図である。 図２６の状態で計算されるベクトルに誤差が含まれることを示す図である。 本発明の第１の実施の形態において、真の道路端の真上に観測点が存在していない場合のスイーパーの進行方向をプロットしたグラフの一例である。 本発明の第２の実施の形態において用いられるカルマンフィルタの計算式の一例である。 本発明の第２の実施の形態において、カルマンフィルタを用いて得られるスイーパーの進行方向をプロットしたグラフの一例である。 本発明の第２の実施の形態において、道路が曲がっている場合に得られるスーパーの移動軌跡の一例を示す図である。 本発明の第２の実施の形態において、道路が曲がっている場合のスイーパーの進行方向をプロットしたグラフである。 （ａ）〜（ｃ）は、本発明の第２の実施の形態に係る処理によって復元された道路形状の第１〜３のそれぞれの例を示す図である。 本発明の第３の実施の形態における道路方向ベクトルの一例を示す図である。（ａ）〜（ｃ）は、本発明の第２の実施の形態に係る処理によって復元された道路形状の第１〜３のそれぞれの例を示す図である。 本発明の第３の実施の形態において、スイーパーと観測点との距離が遠い場合に角度誤差が大きくなることを示す図である。 本発明の第３の実施の形態において、スイーパーと観測点との距離が近い場合に角度誤差が小さくなることを示す図である。 本発明の第３の実施の形態において、統計的な見積もりに用いられた道路モデルの一例である。 図３７の道路モデルから得られた角度誤差の頻度の一例（スイーパーと観測点との距離が近い場合）を示すヒストグラムである。 図３７の道路モデルから得られた角度誤差の頻度の一例を示すヒストグラムである。 図３７の道路モデルから得られた角度誤差の頻度の一例（スイーパーと観測点との距離が遠い場合）を示すヒストグラムである。 図３８〜４０のヒストグラムに示す分布から得られた分散値を示すグラフである。 本発明の第４の実施の形態に係る問題点を説明するための図であり、２つの観測点から正確な道路幅が得られる一例を示す図である。 本発明の第４の実施の形態に係る問題点を説明するための図であり、左右観測データのずれを示す図である。 本発明の第４の実施の形態に係る問題点を説明するための図であり、道路が曲がっている場合に推定される道路幅が誤差を含むことを示す図である。 本発明の第４の実施の形態に係る問題点を説明するための図であり、道路幅が変化している場合に推定される道路幅が誤差を含むことを示す図である。 本発明の第４の実施の形態において、統計的な見積もりに用いられた道路モデルの一例である。 図４６の道路モデルから得られた道路幅の誤差の頻度の一例（左右観測データのずれが小さい場合）を示すヒストグラムである。 図４６の道路モデルから得られた角度誤差の頻度の一例を示すヒストグラムである。 図４６の道路モデルから得られた角度誤差の頻度の一例（左右観測データのずれが大きい場合）を示すヒストグラムである。 図４７〜４９のヒストグラムに示す分布から得られた分散値を示すグラフである。 本発明の第４の実施の形態において用いられるカルマンフィルタの計算式の一例である。 本発明の第４の実施の形態における道路幅推定処理の一例を示すフローチャートである。 本発明の第４の実施の形態における道路形状推定装置の構成の一例を示すブロック図である。 （ａ）はセンサーによってセンシングされた観測データ群（複数の観測点）の一例を示す図であり、（ｂ）は観測データ群から復元された道路形状の一例を示す図であり、（ｃ）は、従来の技術において観測データ左右判定問題が起こる一例を示す図である。

以下、図面を参照しながら、本発明の第１〜第４の実施の形態について説明する。本発明の第１の実施の形態では、本発明の基本的な概念について説明する。本発明の第２の実施の形態では、道路の延伸方向に対する左右ずれの影響を抑えるスムージング処理について説明する。本発明の第３の実施の形態では、遠方の観測点の誤差による影響を抑えるスムージング処理について説明する。本発明の第４の実施の形態では、道路幅を推定する処理について説明する。

＜本発明の第１の実施の形態＞
まず、本発明の第１の実施の形態において、本発明の基本的な概念について説明する。本発明では、センシングされた道路の左端又は右端のどちらかだけの観測データ群を選んで片方の道路端形状を先に推定するという従来技術の手順を行わず、その代わり、センシングされた道路の観測データを随時読み取って、道路の左端又は右端のどちらの観測データであるかを判断しながら道路形状（道路の延伸方向に沿った経路）を推定する。本発明によって推定された道路の中央を通る経路は、例えば道路幅の分だけ左右に幅を持たせることによって、道路幅のある道路形状を復元することが可能である。また、走行中の車両などにおいてリアルタイムに道路の中央を通る経路を推定することによって、安全な走行路を予測するための情報として、自動運転や自動レーンキープなどに利用することも可能である。

具体的には、本発明では、図１に模式的に示すように、観測データ群の分布する空間（仮想的な空間）をスキャンするためのフィルタ（本明細書では、このフィルタをスイーパー（Sweeper：掃く）と呼ぶ）を設け、当該フィルタを移動させて観測データ群のスキャンを行う。スイーパーは、センシングされた観測データを随時読み取って、観測データが道路の左端又は右端のどちらに属しているかを判断しながら、道路の中央と推定される経路を移動する。本発明で用いられるスイーパーは、周辺の観測データを、道路の左右両方から随時サンプリングして道路形状を推定していくため、道路の片側の情報に欠損している部分があったとしても、もう片側の情報で補いながら道路形状を推定することができる。

なお、ここで言うスイーパーは、車両に設置されているセンサーによってセンシングされた観測データ群が分布するスナップショットの空間内を移動して計算処理を行うフィルタである。図中では箱型のスイーパーが模式的に図示されているが、スイーパーは、観測データ群が分布する空間内の位置座標及び進行方向をパラメータとして有している。スイーパーの進行方向を示す角度は、紙面上の右向きを０°、上向きを９０°とした反時計回りの極座標によって表すことにする。

本発明の基本的な処理は、図２のフローチャートに示すように、観測データ群読み取り処理（ステップＳ１１）と、スイーパー計算処理（ステップＳ１２）とを有し、これらの処理を繰り返し行うことによって道路の中央を通る経路を徐々に推定していく。

ステップＳ１１の観測データ群読み取り処理では、スイーパーの現在の位置座標から遠く離れた位置に存在する観測データを無理に読み込まず、スイーパーの現在の位置座標を基準として所定の距離より近い観測データ群（所定の範囲内に存在する観測データ群）のみサンプリングする。これは、遠方に存在する観測点データによって発生し得る「観測データ左右判定問題」を解消することを意図している。例えば図３に示すように、スイーパー前方の一定の範囲（図３の例では、θ＝９０°±５０°、半径＝１０ｍの扇形）内に分布する観測データ群をサンプリングする。

また、ステップＳ１２のスイーパー計算処理では、サンプリングされた観測データ群を用いてスイーパーの進行方向を計算する処理、計算されたスイーパーの進行方向に沿ってスイーパーを移動させる処理が行われる。スイーパー計算処理においては、サンプリングされた観測データ１つ１つに対してスイーパーの進行方向を決定しながらスイーパーを移動させてもよい（後述のスイーパー計算例１）。また、サンプリングされた複数の観測データに基づいてスイーパーの進行方向を決定しながらスイーパーを移動させてもよく、この場合、サンプリングされた複数の観測データ群のそれぞれから得られるベクトルを合成することでスイーパーの進行方向を計算してもよく（後述のスイーパー計算例２）、サンプリングされた複数の観測データ群のそれぞれについてスイーパーの進行方向を暫定的に計算しながら、最終的なスイーパーの進行方向を計算してもよい（後述のスイーパー計算例３）。

以下、スイーパー計算例１〜３について説明する。なお、これらのスイーパー計算例１〜３では、図３に示すように、スイーパー前方の所定の範囲内に３つの観測点Ａ〜Ｃが存在し、スイーパーの位置座標（初期位置座標）及び現在の進行方向（初期進行方向）が設定されている状態を想定して説明する。

（スイーパー計算例１）
スイーパーの進行方向を計算する方法として、例えば、観測点１つ１つに対してスイーパーの進行方向を決定しながら、スイーパーを移動させる方法を採用することが可能である。具体的には、例えばスイーパーから最も近い位置に存在する観測点を用いて、スイーパーの進行方向を計算し、計算された進行方向に沿ってスイーパーを移動させる動作を繰り返し行う。

図４は、本発明の第１の実施の形態におけるスイーパー計算例１の一例を示すフローチャートである。なお、図４のフローチャートに係る処理は、図２のステップＳ１２のスイーパー計算処理に対応している。スイーパー計算例１では、まず、スイーパー前方の最も近い観測点を選択する（ステップＳ１２１１）。例えば図３の例では、スイーパーに最も近い観測点として、スイーパーの進行方向から見て右側に観測点Ａが存在している。なお、観測点がスイーパーの現在の進行方向に対して左側又は右側のどちらに存在しているかによって、その観測点が道路の左端又は右端のどちらに属しているかを把握することができる。

次に、スイーパーの現在の進行方向と、処理対象としている観測データ（観測点の位置座標）とを用いて、その観測点に対応する投影点を計算し（ステップＳ１２１２）、スイーパーの現在の位置座標と投影点とを結ぶベクトルの方向を計算する（ステップＳ１２１３）。例えば図５に示すように、観測点Ａからスイーパーの現在の進行方向（基準方向）に対して垂直に、道路幅Ｗの半分の距離（Ｗ／２）だけ投影させた投影点Ａを計算し、スイーパーの現在の位置座標と投影点Ａとを結ぶベクトルの方向を計算する。

スイーパー計算例１では、スイーパーの現在の位置座標と投影点とを結ぶベクトルの方向を、スイーパーの新たな進行方向として設定して、この方向に沿ってスイーパーを所定の距離だけ移動させる（ステップＳ１２１４）。なお、このときのスイーパーの移動距離は任意に設定可能である。例えば、所定の移動距離だけスイーパーを移動させてもよく、あるいは、投影点までスイーパーを移動させてもよい。スイーパーを移動させた後は、スイーパーの新たな進行方向をスイーパーの現在の進行方向とし、スイーパーの移動後の位置座標をスイーパーの現在の位置座標とする。

上述のように、観測点Ａを用いて計算された新たな進行方向に沿ってスイーパーを移動させると、別の観測点を処理対象とする（ステップＳ１２１５）。図６に示すように観測点Ｂに対して同様の処理を行い、新たな進行方向を計算してスイーパーを移動させる。さらに、観測点Ｂを用いて計算された新たな進行方向に沿ってスイーパーを移動させた後も同様に、図７に示すように、観測点Ｃに対して同様の処理を行い、新たな進行方向を計算してスイーパーを移動させる。

スイーパー前方の所定の範囲内に存在するすべての観測点に対する処理が完了すると、スイーパー計算処理（すなわち、図２のステップＳ１２のスイーパー計算処理）は終了となる。この後は、図２のステップＳ１１の観測データ群の読み取り処理に戻って、スイーパー前方の観測データ群を新たにサンプリングし、再び同様のスイーパー計算処理を繰り返す。

スイーパー計算例１では、所定の範囲内に存在する観測点の個数に等しい回数だけスイーパーを移動させ、図８に示すようなスイーパーの一連の移動軌跡が得られる。スイーパーは、観測点を道路端とした場合に、道路端から道路幅Ｗの半分の距離（Ｗ／２）だけ離れた位置を保つように移動することから、スイーパーの移動軌跡を道路の中央を通る経路とみなすことができる。

（スイーパー計算例２）
また、スイーパーの進行方向を計算する方法として、例えば、所定の範囲内に存在する複数の観測点のそれぞれについてスイーパーの進行方向を計算し、これらの計算結果をまとめたものを新たな進行方向として設定する方法を採用することも可能である。具体的には、所定の範囲内に分布する観測データ群から進行方向を計算し、計算された進行方向に沿ってスイーパーを移動させる動作を繰り返し行う。

図９は、本発明の第１の実施の形態におけるスイーパー計算例２の一例を示すフローチャートである。なお、図９のフローチャートに係る処理は、図２のステップＳ１２のスイーパー計算処理に対応している。スイーパー計算例２では、任意の観測点の位置座標を選択し（ステップＳ１２２１）、スイーパーの現在の進行方向と、処理対象としている観測データ（観測点の位置座標）とを用いて、処理対象としている観測点に対応する投影点を計算し（ステップＳ１２２２）、スイーパーの現在の位置座標と投影点とを結ぶベクトルの方向を計算する（ステップＳ１２２３）。そして、これらの処理をすべての観測点に対して行う（ステップＳ１２２４）。その結果、例えば図１０に示すように、スイーパーの現在の位置座標と、観測点Ａ〜Ｃのそれぞれに対応する投影点Ａ〜Ｃとを結ぶベクトルの方向が計算される。

スイーパー前方の所定の範囲内に存在するすべての観測点に対する処理が完了すると、各観測点から新たに計算されたベクトルの方向をまとめたものをスイーパーの新たな進行方向として設定して、この方向に沿ってスイーパーを所定の距離だけ移動させる（ステップＳ１２２５）。各観測点から新たに計算されたベクトルの方向をまとめる計算は、任意の計算方法が可能であるが、例えば、これら複数のベクトル（あるいは正規化した後のベクトル）を合成してもよい。また、スイーパーの移動距離についても任意に設定可能である。例えば、所定の移動距離だけスイーパーを移動させてもよく、あるいは、観測点を取得する際に用いた所定の範囲に応じた移動距離（例えば、所定の範囲が半径＝１０ｍの扇形の場合には、移動距離を１０ｍとする）だけスイーパーを移動させてもよい。例えば、図１１に図示されているように、観測点Ａ〜Ｃから計算されたベクトル（図１１では点線で図示）を合成して、その合成ベクトルの方向に１０ｍ移動させた位置を移動後のスイーパーの位置とすることが可能である。

スイーパーを移動させると、スイーパーの新たな進行方向をスイーパーの現在の進行方向とし、スイーパーの移動後の位置座標をスイーパーの現在の位置座標として、スイーパー計算処理（すなわち、図２のステップＳ１２のスイーパー計算処理）は終了となる。この後は、図２のステップＳ１１の観測データ群の取得処理に戻って新たな観測データ群を取得し、再び同様のスイーパー計算処理を繰り返す。

スイーパー計算例２では、所定の範囲内に複数の観測点が存在する場合でも、スイーパーの移動回数は１回のみである。しかしながら、所定の範囲内の観測データ群から進行方向を計算してスイーパーを移動させる処理を繰り返し行うことによって、図１２に示すようなスイーパーの一連の移動軌跡が得られる。スイーパーは、観測点を道路端とした場合に、道路端から道路幅Ｗの半分の距離（Ｗ／２）だけ離れた位置を保ちながら移動するよう計算されており、スイーパーの移動軌跡を道路の中央を通る経路とみなすことができる。

（スイーパー計算例３）
また、スイーパーの進行方向を計算する方法として、例えば、所定の範囲内に存在する複数の観測点のそれぞれについてスイーパーの進行方向を順次計算することで、新たな進行方向を求める方法を採用することも可能である。上述のスイーパー計算例２では、各観測点に対応するベクトルを独立して計算した後に、計算されたベクトルをまとめているのに対し、当該スイーパー計算例３では、ある観測点を用いて計算されたベクトルの方向を利用して、別の観測点に対応するベクトルの方向を計算する。

図１３は、本発明の第１の実施の形態におけるスイーパー計算例３の一例を示すフローチャートである。なお、図１３のフローチャートに係る処理は、図２のステップＳ１２のスイーパー計算処理に対応している。スイーパー計算例３では、任意の観測点の位置座標を選択し（ステップＳ１２３１）、スイーパーの現在の進行方向と、処理対象としている観測点の位置座標とを用いて、処理対象としている観測データ（観測点の位置座標）に対応する投影点を計算し（ステップＳ１２３２）、スイーパーの現在の位置座標と投影点とを結ぶベクトルの方向を計算する（ステップＳ１２３３）。

ここで、すべての観測点に係る処理が完了していない場合には、未処理の観測点に対して新たに計算を行う処理に移行するが（ステップＳ１２３４）、このとき、ステップＳ１２３３で計算されたベクトルの方向をスイーパーの現在の進行方向に設定する（ステップＳ１２３５）。この処理は、スイーパーをその場で回転させて、次に選択する観測点から投影点を計算する際に用いられる基準方向を変更する動作であると言うことができる。

スイーパー前方の所定の範囲内に存在するすべての観測点に対する処理が完了すると、最終的に計算されたベクトルの方向をスイーパーの新たな進行方向として設定して、スイーパーを所定の距離だけ移動させる。なお、スイーパーの移動距離は、スイーパー計算例についても任意に設定可能である。スイーパー計算処理（すなわち、図２のステップＳ１２のスイーパー計算処理）は終了となる。この後は、図２のステップＳ１１の観測データ群の取得処理に戻って新たな観測データ群を取得し、再び同様のスイーパー計算処理を繰り返す。

以下、図１４〜１９を参照しながら、スイーパー計算例３の動作について具体的に説明する。例えば、まず観測点Ａを選択した場合には、図１４に示すように、観測点Ａからスイーパーの現在の進行方向（基準方向）に対して垂直に、道路幅Ｗの半分の距離（Ｗ／２）だけ投影させた点を観測点Ａに対応する投影点Ａとし、スイーパーの現在の位置座標と投影点Ａとを結ぶベクトルの方向を計算する。スイーパー計算例３におけるスイーパーは、例えばスイーパーの現在の位置座標を中心として回転可能であり、図１３のステップＳ１２３５の処理が行われると、図１５に示すように、スイーパーの現在の進行方向が新たに計算されたベクトルの方向と一致するよう、現在の位置座標を中心としてスイーパーを回転させる。

続いて、この状態において、図１６に示すように、別の観測点（例えば観測点Ｂ）に対応する投影点Ｂを計算する。そして、再び図１３のステップＳ１２３５の処理が行われて、図１７に示すように、スイーパーの現在の進行方向が新たに計算されたベクトルの方向と一致するよう、現在の位置座標を中心としてスイーパーを回転させる。

さらに、この状態において、図１８に示すように、別の観測点（例えば観測点Ｃ）に対応する投影点Ｃを計算する。ここで、スイーパー前方の所定の範囲内に存在するすべての観測点Ａ〜Ｃに対する処理が完了したので、図１３のステップＳ１２３６の処理が行われ、最終的に計算されたベクトルの方向（図１８に図示されている新たに計算された方向）をスイーパーの新たな進行方向として設定し、スイーパーを所定の距離だけ移動させる。例えば、図１９に図示されているように、スイーパーの新たな進行方向に１０ｍ移動させた位置を移動後のスイーパーの位置とすることが可能である。

上述のスイーパー計算例２と同様に、スイーパー計算例３においても、所定の範囲内に複数の観測点が存在する場合でもスイーパーの移動回数は１回のみである。しかしながら、所定の範囲内の観測データ群から進行方向を計算してスイーパーを移動させる処理を繰り返し行うことによって、図１２に示すようなスイーパーの一連の移動軌跡が得られる。スイーパーは、観測点を道路端とした場合に、道路端から道路幅Ｗの半分の距離（Ｗ／２）だけ離れた位置を保ちながら移動するよう計算されており、スイーパーの移動軌跡を道路の中央を通る経路とみなすことができる。

スイーパー計算例１では、スイーパーが移動するたびに、次の観測点に基づいてスイーパーの新たな進行方向を定める動作が行われる。また、スイーパー計算例２及びスイーパー計算例３では、スイーパーが移動するたびに、所定の範囲内に存在する複数の観測点に基づいてスイーパーの新たな進行方向を定める動作が行われる。これらの動作はいずれも、あたかも幅Ｗの廊下を歩く際に、ときおり左右の壁に手を当てて壁から廊下の幅Ｗの半分（Ｗ／２）の位置を自分の位置として保ちながら、廊下の中央をなぞって進んでいくような状態に例えることができる。なお、本明細書では、道路の延伸方向に沿った経路として、道路の中央を通る経路を推定している。これは、左右の道路端（左右の観測点）から均等な距離（道路幅の半分の距離）をなぞって進むよう計算していることによるが、必ずしも道路幅の半分の距離を計算に用いる必要はない。例えば、右側の観測点から見て道路幅の４分の１の距離、左側の観測点から見て道路幅の４分の３の距離などのように、道路幅を不均等に分割してもよい。

また、スイーパー計算例１〜３のいずれにおいても、スイーパーの進行方向を計算する際に道路幅Ｗが用いられる。本発明では、道路幅を取得又は推定する方法を限定するものではなく、任意の方法（例えば、道路幅を推定する機構を別途設けたり、あらかじめ道路幅として適切な値を設定しておいたりするなど）を採用することが可能である。また、本発明の第４の実施の形態で説明するように、観測データ群を利用して道路幅を推定してもよい。

また、上述したスイーパー計算例１〜３の説明では、スイーパーの位置座標（初期位置座標）及び現在の進行方向（初期進行方向）が設定されている状態を想定しているが、スイーパーの移動スタート時点では、スイーパーの位置及び進行方向はまだ計算されておらず、どこが道路の中央の位置であり、どの進行方向にスイーパーを移動させるべきか分からない。正確には、縦軸（すなわち、道路の延伸方向に平行な軸）に関してはセンサーの存在する位置（センサー表面の前面）をスイーパーのスタート地点とみなせるのに対し、横軸（すなわち、道路の延伸方向に垂直な軸）に関しては、初期位置としてスイーパーをどこに配置すればよいか特定できない。その様子を図２０（ａ）に示す。

そこでまず、センサー、若しくはセンサーを搭載した移動体は、常に道路の延伸方向を向いていると仮定し、センサー表面の前面は、縦軸（すなわち、道路の延伸方向に平行な軸）に垂直であるとする。これにより、スイーパーの初期進行方向は、センサー表面の前面に垂直な方向であると定めることができる。また、この仮定の下で、センサー近傍のいくつかの観測データ群を読み込んで、各観測点からセンサー前面に対して垂直に投影した点の集合を計算し、これらの点の集合を左右２つのグループに分け、各グループの平均位置が道路の左右両端の初期位置であるとみなすことで、スイーパーの初期左右端の位置を特定する。これにより、スイーパーの初期左右端を結ぶ線分を等分する位置をスイーパーの初期位置（道路の中央の初期位置）と定めることができる。その様子を図２０（ｂ）に示す。

なお、初期条件が多少ずれていても、スイーパーの位置及び進行方向は移動と共に道路の中央へ収束していくことから、必ずしも正確な初期条件を設定する必要はない。大まかに初期条件を設定した場合であっても、所望の計算結果を得ることができる。

また、本発明者が、上述のスイーパー計算例１〜３の各方法を用いて実際に道路形状を推定する動作を検証したところ、精度の高い結果を効率的に得られることから、スイーパー計算例３の方法が相対的に有用であるという結果を得ることができている。

次に、本発明の第１の実施の形態における道路形状推定装置の構成について説明する。図２１は、本発明の第１の実施の形態における道路形状推定装置の構成の一例を示すブロック図である。図２１に図示されている道路形状推定装置１０は、観測データ群を読み取る観測データ群読み取り部２０、観測データ群読み取り部２０で読み取られた観測データ群に関して、観測データ群が分布する仮想空間内でスイーパーを移動させながらスイーパー（フィルタ）の進行方向を計算する経路推定フィルタ処理部３０、経路推定フィルタ処理部３０による計算結果（道路の中心を通る経路とみなせるスイーパーの移動軌跡）から道路形状（道路の中心を通る経路）を復元する道路形状復元部４０を有する。経路推定フィルタ処理部３０は、上述したスイーパー計算例１〜３のいずれかの処理を行うことが可能であり、さらに、後述の本発明の第２及び第３の実施の形態におけるカルマンフィルタを用いた計算処理を行えるようにしてもよい。

観測データ群格納部５０には、移動体（例えば、自動車やオートバイ、自転車などの車両）に搭載されたセンサーによって道路端がセンシングされた観測データ群が格納される。道路形状推定装置１０は、これらの観測データ群の中から、処理対象とする観測データをサンプリングして処理を行う。道路形状推定装置１０は、センサーが搭載された移動体に設置され、移動体の移動に合わせてセンシングを行うセンサーから出力される観測データ群（観測データ群格納部５０に一時的に格納された観測データ群）を読み取って、リアルタイムに道路の形状を復元してもよい。また、移動体にデータ通信機能を設け、センサーから出力される観測データ群を遠隔の道路形状推定装置１０に対して送信してもよい。さらに、移動体の移動時にセンサーから出力される観測データ群を観測データ群格納部５０に蓄積しておき、移動体の移動が終了した後で観測データ群格納部５０を回収して、道路形状推定装置１０に観測データ群を読み取らせてもよい。

図２１では機能ブロックを用いて道路形状推定装置１０の装置構成を表しているが、各機能ブロックに係るプログラムを作成してコンピュータのＣＰＵ（中央集積部：Central Processing Unit）に実行させることで実現されてもよく、各機能ブロックを集積回路によって構成してもよい。また、当該道路形状推定装置１０に接続されている表示装置（ディスプレイなど）や入力装置（マウスやキーボードなど）を用いて、オペレータが各機能に係る設定や各機能における処理タイミングなどを適宜制御できるようにしてもよい。

次に、本発明の第１の実施の形態において、スイーパーの移動軌跡の左右揺れについて説明する。まず、単純な例として、道路が直進であって道路幅Ｗが既知であり、かつ、センシングされた観測データ群にはノイズや誤差がない場合について考える。図２２では、真の道路の左右両端を示す線が点線で図示されており、所定の範囲（θ＝９０°±５０°、半径＝１０ｍの扇形）内に存在する３つの観測点Ａ〜Ｃが記号「＋」によって表されている。センシングされた観測データ群にはノイズや誤差がないことから、各観測点Ａ〜Ｃは真の道路端の真上に位置している。また、ここでは、スイーパーの初期位置座標を真の道路中央の真上に置き、スイーパーの初期進行方向を真の道路の延伸方向（θ＝９０°）とする。

この場合、図２３に示すように、観測点Ａ〜Ｃのそれぞれに対応する投影点Ａ〜Ｃは、いずれも真の道路中央の真上に位置する。したがって、上述のスイーパー計算例１〜３のいずれの方法においても、スイーパーの進行方向は常に真の道路の延伸方向（θ＝９０°）を向く。上述の図１２と同様の描き方をした場合、スイーパー計算例３によってスイーパーの進行方向を計算した場合の移動軌跡は、図２４のように表すことができる。

また、図２５には、スイーパーの移動時の進行方向の角度θをプロットしたグラフが図示されている。グラフの縦軸はスイーパーの進行方向の角度θを表しており、グラフの横軸は移動回数（ターン数）を表している。ここでは、道路が直進であって道路幅Ｗが既知であり、かつ、センシングされた観測データ群にはノイズや誤差がない場合を考えているので、スイーパーの進行方向の角度θは常に９０°を取り、スイーパーの移動軌跡の左右揺れは起こらない。

次に、道路が直進であって道路幅Ｗが既知であるが、センシングされた観測データ群が真の道路端からずれている場合について考える。センサーによって実際にセンシングされた観測データ群は、例えば図２６に示すように、ノイズや誤差などの影響によって真の道路端からずれてセンシングされてしまう場合が多い。

図２６では、図２２に示す状態とは異なり、観測点「＋」は真の道路端の真上にはない。そのため、上述の第１の実施の形態に係る処理に基づいて、センシングされた観測点からスイーパーの進行方向のベクトルに対して垂直に、道路幅Ｗの半分の距離（Ｗ／２）だけ投影させた点を計算した場合、図２７に示すように、スイーパーの現在の進行方向ベクトルとスイーパーの位置から投影させた点までのベクトルとは、スイーパーの現在の進行方向に対して角度ｄθ分だけずれた状態となる。

所定の範囲に存在する他の観測点についても同様に、スイーパーの現在の進行方向ベクトルとスイーパーの位置から投影させた点までのベクトルのなす角度にずれが生じており、所定の範囲内に存在するすべての観測点から計算されたスイーパーの新たな進行方向は、真の道路の延伸方向（θ＝９０°）とは一致せずにずれた状態となる。その結果、図２８に示すように、スイーパーの軌跡は道路の真の中央（θ＝９０°）を中心として左右に大きく分散し、スイーパーは左右に揺さぶられながら移動する。

このように、スイーパーの軌跡に不連続な揺れが生じたとしても、スイーパーの軌跡は道路の真の中央（θ＝９０°）を挟んで推移することから、誤差のある観測データ群に対して、本発明の第１の実施の形態に係る処理を適用することは有用であると言える。しかしながら、より精度の高い結果を得るためには、本発明の第２の実施の形態として説明するスムージング処理を行うことが可能である。

＜第２の実施の形態＞
以下、本発明の第２の実施の形態について説明する。本発明の第２の実施の形態では、スイーパーの軌跡が左右に大きく揺れないよう、スイーパーの新たな進行方向を計算する際にカルマンフィルタを用いたスムージングを行う。カルマンフィルタを用いたスムージングは、スイーパーの新たな進行方向を計算する時点で個々の観測点の誤差による角度の飛びを吸収することができ、スイーパーの軌跡を得た後に平滑化処理を行う場合と比べて、リアルタイムにスイーパーの進行方向の大きな揺れを抑えることが可能である。

本発明の第２の実施の形態では、スイーパーの移動軌跡が左右に大きく揺れないよう、例えばスイーパーの現在の位置座標と投影点とを結ぶベクトルの方向（角度の値によって表される）に対して、カルマンフィルタを用いたスムージングを行う。カルマンフィルタを用いたスムージングは、スイーパーの現在の位置座標と投影点とを結ぶベクトルを計算する時点で、個々の観測点の誤差による角度の飛びを吸収することができる。また、スイーパーの移動軌跡を得た後に平滑化処理を行う場合と比べて、より高い精度でリアルタイムにスイーパーの進行方向の大きな揺れを抑えることが可能である。

スイーパーの現在の位置座標と投影点とを結ぶベクトルの方向は、例えば、上述のスイーパー計算例１では図４のステップＳ１２１３の処理、上述のスイーパー計算例２では図９のステップＳ１２２３の処理、上述のスイーパー計算例１では図１３のステップＳ１２３３の処理で計算される。本発明の第２の実施の形態では、これらの処理によって得られた方向（角度の値）に関してカルマンフィルタ処理を行う。

具体的なカルマンフィルタ処理の計算式の例は、例えば図２９に示すとおりである。本発明の第２の実施の形態で用いられるカルマンフィルタでは、状態ｘの変数は、スイーパーの現在の位置座標と投影点とを結ぶベクトルの方向を表す角度θのみなので、スカラーカルマンフィルタとなり、単純に予測と更新のステップを経て、状態ｘを更新していく。また、スムージング処理を目的としてカルマンフィルタを用いているため、カルマンフィルタの制御操作はなく、制御行列Ｆ＝１、観測行列Ｈ＝１と設定できる。また、制御誤差分散Ｑは０以外の値を使用し、観測誤差ＲはＲ＝（π／２）^２と置くことができる。

道路が直進であって道路幅Ｗが既知であるが、センシングされた観測データ群が真の道路端からずれている場合（図２６に図示されている例）に、上述のカルマンフィルタ処理を適用して得られたグラフを図３０に示す。カルマンフィルタ処理を適用していない場合（図２８に示すグラフ）と比べ、スイーパーの進行方向の角度θはスムージングがなされて、左右に値が大きく分散しなくなり、スイーパーの移動軌跡の揺れが抑えられていることがわかる。

また、図３１は、本発明の第２の実施の形態に係るカルマンフィルタ処理を適用して得られたスイーパーの移動軌跡の一例を示す図であり、図３２は、本発明の第２の実施の形態に係るカルマンフィルタ処理を適用して得られたスイーパーの進行方向の角度θをプロットしたグラフである。図３１には、曲がった道路の道路端からセンシングされた観測データ群が真の道路端からずれている場合に計算されたスイーパーの移動軌跡が図示されている。また、図３２のグラフには、図３１に示す状況において、カルマンフィルタを用いて計算されたスイーパーの進行方向の角度θが黒丸でプロットされている。なお、図３２のグラフには、比較対象として、カルマンフィルタを用いずに計算されたスイーパーの進行方向の角度θが白抜きの三角でプロットされている。

カルマンフィルタを用いて処理を行うことの利点は、例えば、図３１及び３２に図示されているように道路が曲がっている場合、個々の観測データの誤差による角度の飛びは吸収される一方、道路の曲がりというおおらかな角度の変化はフィルタ処理で保存されているという点にある。すなわち、カルマンフィルタは、ローパスフィルタとしての役割を有している。

また、図３３（ａ）〜（ｃ）には、本発明の第２の実施の形態におけるカルマンフィルタ処理を用いる手法によって実際に得られた道路の中央を通る経路（軌跡）と、既知の道路幅を用いて復元された道路幅を有する道路が図示されている。図３３（ａ）の道路形状の復元では、実際のミリ波レーダーによる観測データ群が用いられている。また、図３３（ｂ）及び（ｃ）では、観測点データが分布している空間をスイーパーが紙面の下から上方向へ移動しており、スイーパーの移動と共に道路端が推定されていることがわかる。また、推定された道路端は、正解の道路端とほぼ一致しており、高い精度で推定が行われていることもわかる。

＜第３の実施の形態＞
以下、本発明の第３の実施の形態について説明する。上述のように、本発明では「観測データ左右判定問題」を解消するため、スイーパーの現在の位置座標から遠く離れた位置に存在する観測データを無理に読み込まず、スイーパーの現在の位置座標を基準として所定の距離より近い観測データ群（所定の範囲内に存在する観測データ群）のみを読み取るようにしている。本発明の第３の実施の形態では、さらにそれを発展させ、所定の範囲内にあるかないかの“２選択”判断ではなく、観測データがスイーパーから遠方にあればあるほど、“徐々に”左右の判定が困難になるという意味合いを、統計的に見積もり、分散値としてカルマンフィルタに取り込む。

ある道路における、ある観測データ周辺の道路の曲がり具合を示すベクトル（「道路方向ベクトル」）を図３４に示す。「道路方向ベクトル」は、道路の延伸方向を向いた局所的なベクトルであり、観測点から道路の中心を通る経路曲線に垂直に交わる点において、当該経路曲線に接する接線と定義できる。スイーパーが計算で予測しようとしているのは、まさにこの接線であり。観測データを読み取って、その観測データの「道路方向ベクトル」がスイーパーの進行方向ベクトルとなるよう、更新計算をしていく。

図３５及び図３６に、スイーパーの位置が観測データに近いほど、スイーパーの現在の位置座標と投影点とを結ぶベクトルが、正解値である「道路形状を表す方向ベクトル」に近づいて平行になっていく様子を示す。すなわち、図３５と比較して、図３６ではスイーパーと観測点との距離がより近くなっており、スイーパーの現在の位置座標と投影点とを結ぶベクトルと「道路形状を表す方向ベクトル」との角度差分ｄθが小さくなっていることがわかる。

また、図３７に示すような道路形状のモデルを用いて、スイーパーがある観測点に近づくほど、その観測点に係る観測データを用いて「道路方向ベクトル」を算出できるのかを統計的に見積もった結果を図３８〜４０に示す。図３７は、本発明の第３の実施の形態において、統計データ取得に用いた道路形状を示す図である。なお、この統計的な見積もりでは、図３７に図示されている渦巻き状の道路モデルに加えて、逆巻の道路モデルも用いている。また、図３８〜４０は、この道路上に存在し得る無数のスイーパーに関して、それぞれに無数の観測データを読んだ場合の、スイーパーの現在の位置座標と投影点とを結ぶベクトルと観測データから計算された「道路方向ベクトル」との角度差分を誤差として計算し、その計算結果をヒストグラムの分布として表したものである。この計算では、道路の中央に位置する多数の点のそれぞれにおいて、多数の観測点を設定し、当該多数の観測点のそれぞれにおいて、スイーパーの現在の位置座標と投影点とを結ぶベクトルと、その観測点データから計算された「道路方向ベクトル」との角度差分を計算している。図３８〜４０の横軸は、計算された角度差分（角度の誤差）の大きさを表しており、図３８〜４０の縦軸は、誤差の頻度を表している

図３８は、計算において設定した距離ｒのパラメータが０から１０の範囲内の場合（スイーパーと観測点との距離が近い場合）、図３９は、２０から３０の範囲内の場合、図４０は、６０から７０の範囲内の場合（スイーパーと観測点との距離が遠い場合）の誤差分布をそれぞれ表している。

スイーパーと観測点との距離が近い図３８のヒストグラムでは、スイーパーの現在の位置座標と投影点とを結ぶベクトルと「道路方向ベクトル」との角度差分が０に近い場合が多く存在する。すなわち、スイーパーと観測点との距離の値ｒが小さい場合ほど、計算されたベクトルの多くが、正しく道路形状に沿った方向である「道路方向ベクトル」の方向に向いていることを意味する。

一方、スイーパーの現在の位置座標と投影点とを結ぶベクトルと「道路方向ベクトル」との角度差分が大きい図４０のヒストグラムでは、スイーパーと観測点との距離の値ｒが大きく、スイーパーから離れた位置の観測点に基づいてベクトルを計算するため、角度差分（角度の誤差）が大きい場合が分布として多く見られ、必ずしも観測データから計算されたベクトルが正確ではないことを示している。

この統計的見積もりの結果から、図３８〜４０のヒストグラムによって表される角度差分の分布（誤差分布）をガウス関数の形状であると仮定することができ、角度差分分布の分散値の関数ｆ（ｒ）を計算することができる。図４１は、角度差分分布の分散値の関数ｆ（ｒ）を示すグラフである。図４１の横軸は、スイーパーから観測点までの距離ｒを表しており、図４１の縦軸は分散値を表している。図４１に示すように、分散値の関数ｆ（ｒ）は距離ｒが大きくなると分散値も大きくなる傾向を表している。

上述のように、スイーパーの現在の位置座標と投影点とを結ぶベクトルと、その観測点データから計算された「道路方向ベクトル」との角度差分がガウス分布に従うと仮定することが可能であることから、角度差分分布の分散値の関数ｆ（ｒ）をカルマンフィルタに取り込むことで、この誤差が最小となるように計算することができる。

具体的なカルマンフィルタ処理の計算式の例は、例えば図２９に示されているカルマンフィルタの計算の４行目の式を以下のように置換する。
S = H*P*H' + R+ f(r) ・・・（式１）
カルマンフィルタは、ある異なる分散同士が独立である場合には単純に足し合わせることができるという性質（分散の加法性）を有している。上記の新たに設定する式は、この分散の加法性を用いて、観測誤差分散Ｒに関数ｆ（ｒ）の項を並列して加えたものである。

このように、カルマンフィルタの計算の４行目の式にｆ（ｒ）の項を加えることで、スイーパーと観測点との距離ｒが近い場合には分散ｆ（ｒ）が小さくなって、更新の値に大きな影響を及ぼし、一方、スイーパーと観測点との距離ｒが遠い場合には分散ｆ（ｒ）が大きくなって信頼性が薄くなり、更新の値に大きな影響を及ぼさないようになる。

＜第４の実施の形態＞
以下、本発明の第４の実施の形態について説明する。上述した本発明の第１〜３の実施の形態では、道路幅を取得又は推定する方法を特に限定せずに、道路幅は既知であるとして説明を行ったが、本発明の第４の実施の形態では、観測データ群を用いて道路幅の推定を行う場合について説明する。

まず、観測データ群を用いて道路幅の推定を行った場合における問題点について説明する。

図４２には、道路の延伸方向に対して垂直な線上に観測点が存在する場合に道路幅を見積もる方法が図示されている。道路の左右両方に存在する観測点を結ぶ直線が、道路の延伸方向を向いた直線（観測点から道路の中心を通る経路曲線に垂直に交わる点において、当該経路曲線に接する接線）に対して垂直になる場合には、道路幅は単純にこれら２つの観測点の間の距離と等しくなる。

しかしながら、図４２のように２つの観測点が同一の垂直な線上に存在することはまれであり、実際には、図４３に示すように２つの観測点が同一の垂直な線上に存在せずにずれた状態になっていることが多い。本明細書では、このずれを「左右観測データのずれ」と呼び、２つの観測点のそれぞれから道路の延伸方向を向いた直線に対して垂直に下ろした点（以下、観測点から下ろした垂線の足と呼ぶ）間の距離ｄによって表す。例えば、距離ｄが０の場合には「左右観測データのずれ」はなく、距離ｄが０より大きい値を取る場合には「左右観測データのずれ」が存在している。

道路幅は、例えば、観測点から道路の延伸方向に対して垂直に下ろした垂線の足と、観測点との距離を利用して推定することが可能である。例えば、図４３の例において、観測点Ａとその垂線の足ａとの距離がＷＡ、観測点Ｂとその垂線の足ｂとの距離がＷＢの場合、道路幅をこれらの距離の和ＷＡ＋ＷＢと推定することができる。

この道路幅の推定方法によれば、道路幅が一定の直線形状の道路の場合には、真の道路幅に近い値を得ることができる。しかしながら、道路形状が曲がっている場合や道路幅が変化している場合などにおいては、真の道路幅との誤差が大きくなる。

例えば、図４４に図示されているように道路形状が曲がっている場合には、「左右観測データのずれ」のある２つの観測点Ａ、Ｂを用いて推定される道路幅ＷＡ＋ＷＢと真の道路幅との間に誤差が生じる。「左右観測データのずれ」の距離ｄが大きいほど、あるいは道路の曲がり具合（曲率）が大きいほど、この誤差は大きくなる。

また、例えば、図４５に図示されているように道路幅が変化している場合も同様に、「左右観測データのずれ」のある２つの観測点Ａ、Ｂを用いて推定される道路幅ＷＡ＋ＷＢと真の道路幅との間に誤差が生じる。「左右観測データのずれ」の距離ｄが大きいほど、あるいは道路幅の変化率が大きいほど、この誤差は大きくなる。

上述の問題点を考慮して、本発明の第４の実施の形態では、道路幅が滑らかに変化するよう、推定すべき道路幅に対してカルマンフィルタを用いたスムージングを行う。カルマンフィルタを用いたスムージングは、道路幅の大きな揺れをリアルタイムに抑えることができ、道路幅の計算に用いられる個々の観測点の誤差による飛びを吸収することができる。さらに、「左右観測データのずれ」が大きいほど、道路幅の推定結果が不正確になっていく様子を統計的に見積もり、その見積もり結果を分散値としてカルマンフィルタに取り込むことを提案する。

以下、ある道路形状での、「左右観測データのずれ」が大きくなるほど、見積もった道路幅が真の道路幅といかに異なっていくかを示した統計結果を示す。

図４６は、本発明の第４の実施の形態において、「左右観測データのずれ」が道路幅の推定結果に与える影響を調べるために用いた道路形状を示す図である。また、図４７〜４９は、図４６に示す道路の中央から読み取ることができる観測データに関して、上述の単純な道路幅推定方法によって算出された道路幅ＷＡ＋ＷＢと真の道路幅との誤差を計算した結果を示すヒストグラムである。この計算では、道路の中央に位置する多数の点のそれぞれにおいて、道路の中央を挟んで左右の２つの観測点のセットを多数設定し、それぞれのセットにおいて推定した道路幅ＷＡ＋ＷＢと、真の道路幅との誤差を計算している。図４７〜４９の横軸は、推定された道路幅ＷＡ＋ＷＢと、真の道路幅との誤差の大きさを表しており、図４７〜４９の縦軸は、誤差の頻度を表している。

図４７は、計算において設定した距離ｄのパラメータが０から１．２５の範囲内の場合（「左右観測データのずれ」が小さい場合）、図４８は、１．２５から２．５の範囲内の場合、図４９は、３．７５から５の範囲内の場合（「左右観測データのずれ」が大きい場合）の誤差分布をそれぞれ表している。

「左右観測データのずれ」が小さい図４７のヒストグラムでは、推定された道路幅と真の道路幅との誤差が０に近い場合が多く存在する。すなわち、「左右観測データのずれ」の距離の値ｄが小さい場合（サンプリングされた左右２つの観測点が道路の延伸方向を垂直に横切る同一線上の近くにある場合）ほど、正確に道路幅を推定できることを意味する。

一方、「左右観測データのずれ」が大きい図４９のヒストグラムでは、「左右観測データのずれ」の距離の値ｄが大きいため、推定された道路幅と真の道路幅との誤差が大きい場合が分布として多く見られ、必ずしも推定された道路幅が正確ではないことを示している。

この統計的見積もりの結果から、図４７〜４９のヒストグラムによって表される道路幅推定の誤差分布をガウス関数の半分の形状であると仮定することができ、その分散値の関数ｇ（ｄ）を計算することができる。図５０は、道路幅推定の誤差分布の分散値の関数ｇ（ｄ）を示すグラフである。図５０の横軸は、「左右観測データのずれ」の距離ｄを表しており、図５０の縦軸は分散値を表している。図５０に示すように、分散値の関数ｇ（ｄ）は距離ｄが大きくなると分散値も大きくなる傾向を表している。

上述のように、推定された道路幅と真の道路幅との誤差がガウス分布に従うと仮定することが可能であることから、道路幅推定における「左右観測データのずれ」に係る誤差分布の分散値の関数ｇ（ｄ）をカルマンフィルタに取り込むことで、この誤差が最小となるように計算することができる。

具体的なカルマンフィルタ処理の計算例は、例えば図５１に示すとおりである。本発明の第４の実施の形態で用いられるカルマンフィルタでは、状態ｘ_Ｗの変数は、道路幅Ｗのみなので、スカラーカルマンフィルタとなり、単純に予測と更新のステップを経て、状態ｘ_Ｗを更新していく。カルマンフィルタの制御操作はないことから、制御行列Ｆ＝１、観測行列Ｈ＝１と設定できる。また、制御誤差分散Ｑ及び観測誤差Ｒは１と置くことができる。

道路幅推定の誤差の分散値の関数ｇ（ｄ）は、図５１に示すカルマンフィルタの計算の４行目の式に以下のように反映されている。
S = H*P*H' + R+ g(d) ・・・（式２）
この式も、本発明の第３の実施の形態で言及した分散の加法性を利用しており、観測誤差分散Ｒに、道路幅推定の誤差分布の分散値の関数ｇ（ｄ）の項を並列して加えたものである。

このように、カルマンフィルタの計算の４行目の式にｇ（ｄ）の項を加えることで、「左右観測データのずれ」の距離ｄが小さい場合には分散ｇ（ｄ）が小さくなって、更新の値に大きな影響を及ぼし、一方、「左右観測データのずれ」の距離ｄが大きい場合には分散ｇ（ｄ）が大きくなって信頼性が薄くなり、更新の値に大きな影響を及ぼさないようになる。

次に、本発明の第４の実施の形態における処理について説明する。図５２は、本発明の第４の実施の形態における道路幅の推定処理の一例を示すフローチャートである。

本発明の第４の実施の形態における道路幅の推定処理では、スイーパーが、観測データ群が分布している空間内を移動しながら観測データ群のスキャンを行う。このスイーパーによる観測データ群の読み取りは、上述した道路の中央を通る経路を推定するための処理における観測データの読み取り処理を兼ねていてもよく、例えば、図３に図示されているような所定の範囲内の観測データ群を読み取ることで、遠方に存在する観測点データによって発生し得る「観測データ左右判定問題」を防ぐことが可能となる。

スイーパーは、所定の範囲内に存在する観測データ群を手前（スイーパーに近い位置）からスキャンしていき、道路の右側に位置すると推定される観測データをサンプリングして記録する（ステップＳ２１）。また、道路の左側に位置すると推定される観測データも同様にサンプリングして記録する（ステップＳ２２）。なお、スイーパーは、その進行方向の右側に存在する観測データを、道路の右側に位置する観測データと推定し、進行方向の左側に存在する観測データを、道路の左側に位置する観測データと推定することができる。サンプリングされた観測データに道路の左右両方に位置する観測データ（すなわち、左右両方のセットとなる観測データ）がない場合には、左右両方のセットとなる観測データが揃うまで、上記のステップＳ２１及びＳ２２の処理を繰り返し行う（ステップＳ２３）。

一方、サンプリングされた観測データに道路の左右両方に位置する観測データがある場合には、最も新しく観測された道路の左右の観測データから道路幅Ｗを計算する（ステップＳ２４）。この計算では、例えば、観測点から道路の延伸方向に対して垂直に下ろした垂線の足と観測点との距離を利用することが可能である。例えば、図４３の例において、観測点Ａとその垂線の足ａとの距離ＷＡ、観測点Ｂとその垂線の足ｂとの距離ＷＢをそれぞれ計算して、これらの和ＷＡ＋ＷＢを道路幅（暫定的な道路幅）とすることができる。

さらに、最も新しく観測された道路の左右の観測データ同士のずれｄを計算する。この計算では、観測点から道路の延伸方向に対して垂直に下ろした垂線の足の間の距離を求めればよい。例えば、図４３の例では、垂線の足ａと垂線の足ｂとの距離（左右観測データのずれｄ）を計算する（ステップＳ２５）。

そして、ステップＳ２４で計算された道路幅Ｗ（暫定的な道路幅）、ステップＳ２５で計算された左右観測データのずれｄを、図５１に示すようなカルマンフィルタに入力する（ステップＳ２６）。なお、ステップＳ２４で計算された道路幅Ｗは状態変数ｘ_Ｗに入力され、ステップＳ２５で計算された左右観測データのずれｄは、カルマンフィルタに取り込まれた道路幅推定の誤差の分散値の関数ｇ（ｄ）の計算に用いられる。カルマンフィルタによって処理されると、カルマンフィルタから更新された状態変数（すなわち、更新された道路幅）が出力される。カルマンフィルタから出力される道路幅は、観測データの計測時に含まれる誤差やノイズなどの影響を抑えるようスムージングされたものであり、かつ、左右観測データのずれｄの影響を反映したものである。そして、所定の範囲内に存在するすべての観測データ群再びステップＳ２１へ戻り、所定の範囲内に存在するすべての観測データ群に関して、カルマンフィルタを用いた道路幅の計算を繰り返す。所定の範囲内に存在するすべての観測データ群に関して処理が済むと、スイーパーは道路の延伸方向に向かって所定の距離だけ移動して、図５２のフローチャートに係る処理を行うことで、スイーパーを移動させながら道路の経路に沿って少しずつ道路幅を推定することが可能となる。

また、図５３は、本発明の第４の実施の形態における道路形状推定装置の構成の一例を示すブロック図である。図２１に図示されている道路形状推定装置１０の構成に加えて、観測データ群読み取り部２０で読み取られた観測データ群に関して、観測データ群が分布する仮想空間内でスイーパーを移動させながら道路幅を計算する道路幅推定フィルタ処理部５０を有する。

道路幅推定フィルタ処理部５０は、上述した道路幅の推定処理を行うことが可能である。道路幅推定フィルタ処理部５０によって推定された道路幅は、経路推定フィルタ処理部３０におけるスイーパーの進行方向の計算や、道路形状復元部４０における幅のある道路形状の復元などの処理に用いることが可能である。

なお、図５３では、上述した本発明の第２及び第３の実施の形態で行われるカルマンフィルタ処理とは独立して、本発明の第４の実施の形態におけるカルマンフィルタ処理を行うことが可能であることから、経路推定フィルタ処理部３０及び道路幅推定フィルタ処理部５０を異なるブロックによって図示している。ただし、例えば経路推定フィルタ処理部３０による計算処理（道路の中央を通る経路を推定する処理）で、道路幅推定フィルタ処理部５０によって推定された道路幅を利用できることなどから、２つのフィルタ処理部を不可分な構成としてもよい。

本発明は、道路形状を推定するための道路形状推定技術に適用可能であり、特に道路形状として、道路の延伸方向に沿った経路（例えば、道路の中央を通る経路）の形状を推定するための技術に適用可能である。また、移動体に搭載することで、走行中の移動体の前方又は周辺の道路形状の推定が可能となり、移動体を安全に走行させるための技術に適用可能である。

１０ 道路形状推定装置
２０ 観測データ群読み取り部
３０ 経路推定フィルタ処理部
４０ 道路形状復元部
５０ 道路幅推定フィルタ処理部

Claims (18)

1. 道路を走行する移動体に設置されたセンシング機器によってセンシングされた観測データ群を用いて、前記道路の形状を推定する道路形状推定装置であって、
   前記センシングにおける所定の範囲内に存在する観測データ群を取得する観測データ群取得部と、
   位置座標及び進行方向をパラメータとして有するフィルタを前記観測データ群が分布する仮想空間に配置し、前記観測データ群取得部によって取得された前記観測データ群を用いて前記フィルタの進行方向を計算する進行方向計算部と、
   前記進行方向計算部で計算された前記フィルタの進行方向に沿って、前記フィルタを前記仮想空間内で移動させるフィルタ移動部と、
   前記フィルタ移動部によって移動された前記フィルタの移動軌跡を、前記道路の延伸方向に沿った経路として出力する道路形状出力部とを有し、
   前記進行方向計算部が、
   前記フィルタの現在の位置を基準として所定の範囲内に存在する前記観測データを選択する選択部と、
   前記選択された観測データの位置から所定の距離だけ、前記フィルタの現在の進行方向に対して垂直な方向に移動させた投影点を計算する投影点計算部と、
   前記フィルタの現在の位置と前記投影点とを結ぶベクトルの方向を計算するベクトル方向計算部と、
   前記計算されたベクトルの方向を用いて前記フィルタの新たな進行方向を設定する進行方向設定部と、
   前記フィルタの新たな進行方向に向かって所定の移動距離だけ前記フィルタを移動させて、前記フィルタの現在の座標が移動後の前記フィルタの位置と設定されるよう更新し、前記フィルタの現在の進行方向が前記フィルタの新たな進行方向と設定されるよう更新処理部とを、
   有する道路形状推定装置。
2. 前記投影点計算部が、前記所定の範囲内に存在する複数の観測データ群のうちの第１の観測データの位置を用いて第１の投影点を計算し、
   前記ベクトル方向計算部が、前記フィルタの現在の位置と前記第１の投影点とを結ぶ第１のベクトルの方向を計算して、前記第１のベクトルの方向を、前記第１の観測データとは異なる第２の観測データを用いた計算を行うための前記フィルタの現在の進行方向として設定するよう構成されており、
   前記投影点計算部及び前記ベクトル方向計算部による一連の処理を、前記複数の観測データのそれぞれに関して順次繰り返し行い、すべての観測点に関して計算が終了した際、前記進行方向設定部が、前記ベクトル方向計算部によって最終的に計算されたベクトルの方向を前記フィルタの新たな進行方向として設定するよう構成されている請求項１に記載の道路形状推定装置。
3. 前記ベクトル方向計算部によって計算された前記ベクトルの方向に対して、前記フィルタの現在の進行方向との角度のずれを抑えるスムージング処理を行うようスムージング処理部を更に有する請求項１又は２に記載の道路形状推定装置。
4. 前記スムージング処理部が、前記フィルタの現在の位置から前記観測データの位置までの距離が大きいほど、前記ベクトル方向計算部によって計算された前記ベクトルの方向と前記フィルタの現在の進行方向との間の角度のずれを抑えるよう構成されている請求項３に記載の道路形状推定装置。
5. 前記道路の延伸方向に沿った経路の正解率の分布の分散を、前記フィルタの位置と観測データとの間の距離の関数として統計的に求めておき、前記フィルタの現在の位置から前記観測データの位置までの距離に対応する分散値を前記スムージング処理部による計算に用いるよう構成されている請求項４に記載の道路形状推定装置。
6. 前記スムージング処理部が、カルマンフィルタを用いてスムージング処理を行うよう構成されている請求項３から５のいずれか１つに記載の道路形状推定装置。
7. 前記観測データ群取得部によって取得された前記観測データ群を用いて前記道路幅を計算する道路幅計算部を更に有し、
   前記道路幅計算部が、
   前記フィルタの進行方向を基準として、前記道路の右側に位置する観測データと、前記道路の左側に位置する観測データとを選別する左右選別部と、
   前記道路の右側に位置する観測データの位置から前記フィルタの進行方向に対して垂直な方向に投影した、前記道路の延伸方向に沿った経路上にある第１の投影点を計算するとともに、前記道路の左側に位置する観測データの位置から前記フィルタの進行方向に対して垂直な方向に投影した、前記道路の延伸方向に沿った経路上にある第２の投影点を計算する道路幅投影点計算部と、
   暫定的な道路幅として、前記道路の右側に位置する観測データの位置と前記第１の投影点との距離と、前記道路の右側に位置する観測データの位置と前記第２の投影点との距離との和を計算する暫定道路幅計算部と、
   前記投影点計算部によって計算された前記第１及び第２の投影点の間の距離を計算する投影点ずれ計算部と、
   前記暫定道路幅計算部によって計算された前記暫定的な道路幅ベクトルの方向に対して、前記フィルタの移動前に計算された道路幅とのずれを抑えながらスムージング処理によって前記道路幅を計算する道路幅スムージング処理部とを有し、
   前記道路幅スムージング処理部において前記フィルタの移動前に計算された道路幅と前記暫定的な道路幅とを用いて前記道路幅を計算する際に、前記第１及び第２の投影点の間の距離が大きいほど、前記推定される結果である道路幅への前記暫定的な道路幅による影響を抑えるよう構成されている請求項１から６のいずれか１つに道路形状推定装置。
8. 前記道路幅の正解率の分布の分散を、前記道路の右側及び左側に位置する２つの投影点の間の距離の関数として統計的に求めておき、前記第１及び第２の投影点の間の距離に対応する分散値を前記道路幅スムージング処理部による前記道路幅の計算に用いるよう構成されている請求項７に記載の道路形状推定装置。
9. 前記道路幅スムージング処理部が、カルマンフィルタを用いてスムージング処理を行うよう構成されている請求項７又は８に記載の道路形状推定装置。
10. 道路を走行する移動体に設置されたセンシング機器によってセンシングされた観測データ群を用いて、前記道路の形状を推定する道路形状推定方法であって、
    前記センシングにおける所定の範囲内に存在する観測データ群を取得する観測データ群取得ステップと、
    位置座標及び進行方向をパラメータとして有するフィルタを前記観測データ群が分布する仮想空間に配置し、前記観測データ群取得ステップで取得された前記観測データ群を用いて前記フィルタの進行方向を計算する進行方向計算ステップと、
    前記進行方向計算ステップで計算された前記フィルタの進行方向に沿って、前記フィルタを前記仮想空間内で移動させるフィルタ移動ステップと、
    前記フィルタ移動ステップで移動された前記フィルタの移動軌跡を、前記道路の延伸方向に沿った経路として出力する道路形状出力ステップとを有し、
    前記進行方向計算ステップが、
    前記フィルタの現在の位置を基準として所定の範囲内に存在する前記観測データを選択する選択ステップと、
    前記選択された観測データの位置から所定の距離だけ、前記フィルタの現在の進行方向に対して垂直な方向に移動させた投影点を計算する投影点計算ステップと、
    前記フィルタの現在の位置と前記投影点とを結ぶベクトルの方向を計算するベクトル方向計算ステップと、
    前記計算されたベクトルの方向を用いて前記フィルタの新たな進行方向を設定する進行方向設定ステップと、
    前記フィルタの新たな進行方向に向かって所定の移動距離だけ前記フィルタを移動させて、前記フィルタの現在の座標が移動後の前記フィルタの位置と設定されるよう更新し、前記フィルタの現在の進行方向が前記フィルタの新たな進行方向と設定されるよう更新処理ステップとを、
    有する道路形状推定方法。
11. 前記投影点計算ステップにおいて、前記所定の範囲内に存在する複数の観測データ群のうちの第１の観測データの位置を用いて第１の投影点を計算し、
    前記ベクトル方向計算ステップにおいて、前記フィルタの現在の位置と前記第１の投影点とを結ぶ第１のベクトルの方向を計算して、前記第１のベクトルの方向を、前記第１の観測データとは異なる第２の観測データを用いた計算を行うための前記フィルタの現在の進行方向として設定し、
    前記投影点計算ステップ及び前記ベクトル方向計算ステップにおける一連の処理を、前記複数の観測データのそれぞれに関して順次繰り返し行い、すべての観測点に関して計算が終了した際、前記進行方向設定ステップにおいて、前記ベクトル方向計算ステップで最終的に計算されたベクトルの方向を前記フィルタの新たな進行方向として設定する請求項１０に記載の道路形状推定方法。
12. 前記ベクトル方向計算ステップで計算された前記ベクトルの方向に対して、前記フィルタの現在の進行方向との角度のずれを抑えるスムージング処理を行うようスムージング処理ステップを更に有する請求項１０又は１１に記載の道路形状推定方法。
13. 前記スムージング処理ステップにおいて、前記フィルタの現在の位置から前記観測データの位置までの距離が大きいほど、前記ベクトル方向計算ステップで計算された前記ベクトルの方向と前記フィルタの現在の進行方向との間の角度のずれを抑える請求項１２に記載の道路形状推定方法。
14. 前記道路の延伸方向に沿った経路の正解率の分布の分散を、前記フィルタの位置と観測データとの間の距離の関数として統計的に求めておき、前記フィルタの現在の位置から前記観測データの位置までの距離に対応する分散値を前記スムージング処理ステップにおける計算に用いる請求項１３に記載の道路形状推定方法。
15. 前記スムージング処理ステップにおいて、カルマンフィルタを用いてスムージング処理を行う請求項１２から１４のいずれか１つに記載の道路形状推定方法。
16. 前記観測データ群取得ステップで取得された前記観測データ群を用いて前記道路幅を計算する道路幅計算ステップを更に有し、
    前記道路幅計算ステップにおいて、
    前記フィルタの進行方向を基準として、前記道路の右側に位置する観測データと、前記道路の左側に位置する観測データとを選別する左右選別ステップと、
    前記道路の右側に位置する観測データの位置から前記フィルタの進行方向に対して垂直な方向に投影した、前記道路の延伸方向に沿った経路上にある第１の投影点を計算するとともに、前記道路の左側に位置する観測データの位置から前記フィルタの進行方向に対して垂直な方向に投影した、前記道路の延伸方向に沿った経路上にある第２の投影点を計算する道路幅投影点計算ステップと、
    暫定的な道路幅として、前記道路の右側に位置する観測データの位置と前記第１の投影点との距離と、前記道路の右側に位置する観測データの位置と前記第２の投影点との距離との和を計算する暫定道路幅計算ステップと、
    前記投影点計算ステップで計算された前記第１及び第２の投影点の間の距離を計算する投影点ずれ計算ステップと、
    前記暫定道路幅計算ステップで計算された前記暫定的な道路幅ベクトルの方向に対して、前記フィルタの移動前に計算された道路幅とのずれを抑えながらスムージング処理によって前記道路幅を計算する道路幅スムージング処理ステップとを有し、
    前記道路幅スムージング処理ステップにおいて前記フィルタの移動前に計算された道路幅と前記暫定的な道路幅とを用いて前記道路幅を計算する際に、前記第１及び第２の投影点の間の距離が大きいほど、前記推定される結果である道路幅への前記暫定的な道路幅による影響を抑える請求項１０から１５のいずれか１つに道路形状推定方法。
17. 前記道路幅の正解率の分布の分散を、前記道路の右側及び左側に位置する２つの投影点の間の距離の関数として統計的に求めておき、前記第１及び第２の投影点の間の距離に対応する分散値を前記道路幅スムージング処理ステップにおける前記道路幅の計算に用いる請求項１６に記載の道路形状推定方法。
18. 前記道路幅スムージング処理ステップにおいて、カルマンフィルタを用いてスムージング処理を行う請求項１６又は１７に記載の道路形状推定方法。

Images