JP2013117811A - 道路形状推定装置及びプログラム - Google Patents

Abstract

【課題】車線区分線が破線のカーブ路であっても、カメラの画角に依らずに道路の路面を撮影した画像から精度良く道路形状を推定する。
【解決手段】区分線検出部１０１１は、道路の路面を撮影した画像から車線区分線のエッジ情報を検出する。第１のパラメータ推定部１０１２は、第１の時間区間内の１つ以上の時刻における画像から検出されたエッジ情報に基づいて、道路の道路形状パラメータのうちの曲率及び幅を求め、格納部１０１３に格納する。第２のパラメータ推定部１０１４は、格納された曲率及び幅を第２の時間区間における曲率及び幅として用い、第２の時間区間における画像から検出されたエッジ情報に基づいて、曲率及び幅以外の道路形状パラメータを求める。
【選択図】図１０

Description

本発明は、道路形状推定装置及びプログラムに関する。

カメラを車両の進行方向又は進行方向と逆向きに取り付け、撮影された映像に含まれる車線区分線を検出して、車線区分線の情報から走行車線と車両との位置関係を求め、道路形状を推定する方法は古くから研究・開発されている。

例えば、カメラで撮影した車両前方の路面画像から、縦方向に複数個所で横方向に画像を走査し、輝度（濃淡）の変化箇所として車線区分線のエッジを検出し、エッジ情報に基づいて走行車線と車両との位置関係を推定する方法が知られている。この方法では、画像から検出されたエッジ点を縦方向に繋ぐことで車線区分線を識別し、道路形状モデルと車線区分線の位置とを照合して路面上の車線区分線の位置が決定される。

また、車線区分線が破線の場合に、車両の走行中における複数時刻の路面画像から得られる車線区分線の位置を重ね合わせて線分の間隙を埋めることで、破線を長い線分として検出する方法も知られている。この方法では、検出された長い線分を道路形状モデルと照合することで、安定して路面上の車線区分線の位置を決定することができる。

特開２００６−１２１９１号公報

高橋新，二宮芳樹，「走行レーン認識におけるロバストなレーンマーキング候補選択の一手法」，電子情報通信学会論文誌 Ｄ−ＩＩ Ｖｏｌ．Ｊ８１−Ｄ−ＩＩ Ｎｏ．８ ｐｐ．１９０４−１９１１，１９９８年８月

上述した従来の道路形状推定方法には、以下のような問題がある。
従来の道路形状推定方法は、車両に搭載されるカメラが、画角が３０度程度の比較的画角が狭いカメラを車両に搭載することを前提としている。狭角カメラを用いると、得られる画像の分解能が高いため、路面の奥行き方向で遠方まで解像度高く撮影でき、手前から奥までの長い範囲で撮影することができる。このため、車線区分線が破線であっても、画像中には進行方向に沿って複数の破線線素を観測することができる。

一方、ドライブレコーダ用カメラのような、画角が１２０度程度の広角カメラを用いると、幅広い角度範囲を撮影することができる代わりに、画像の分解能は低くなる。このため、路面上の進行方向で見ると手前側の短い距離までしか撮影できない制約があり、車線区分線が破線の場合は、車両に最も近い破線線素が走行車線の左右に１本ずつしか観測できない。以下の説明では、破線線素を破線ブロックと記載する場合がある。

図１は、セダン形状の車両のルームミラー裏に前向きにカメラを設置し、高速道路の破線カーブ路を撮影した画像を模式的に表している。この例では、破線ブロック（白線部分）の長さは８ｍで、破線ブロック間の間隙（非白線部分）の長さは１２ｍの一般的な状況を想定している。画像１０１は、画角３０度の狭角カメラで撮影した画像であり、画像１０２は、画角１２０度の広角カメラで撮影した画像である。画像１０２中の矩形領域１２１は、同じ状況で狭角カメラが撮影可能な範囲を表している。

図１から分かるように、狭角カメラでは、破線路であっても進行方向に沿って複数の破線ブロックが撮影できるため、単一時刻の画像から複数の破線ブロックを検出することで、道路形状を推定することが可能である。特に、破線カーブ路であっても、図２に示すように複数の破線ブロックが撮影できるため、単一時刻の画像からカーブの曲率を推定することもできる。このように、狭角カメラを使用する場合は、従来の方法でも道路形状を推定することができる。

一方、広角カメラの画像１０２では、手前側の黒色領域１２２が示すように、車内のダッシュボードと車両の前方ボンネット部分で路面が隠されてしまう上、分解能が不足するため路面の手前側しか撮影できない。このため、破線路の場合は、進行方向に沿って最も車両に近い破線ブロックが、短い線素として走行車線の左右に１本ずつしか観測できないことが分かる。

特に、破線カーブ路の場合は、図３に示すように、走行車線の左右に破線ブロック３０１及び３０２が観測されるのみである。このような状況で従来の道路形状推定方法を適用すると、推定される車線区分線３１１及び３１２の形状は、破線ブロック３０１及び３０２を延長した直線と誤認されることになる。このため、本来の車線区分線３２１及び３２２と推定される車線区分線３１１及び３１２との間に距離誤り３３１及び３３２を生じてしまう。

例えば、前方映像を用いた車線逸脱警報システムでは、車両の前方に映る映像から車線形状を推定する。画角が３０度の狭角カメラでは５０ｍ程度先の映像が用いられ、画角が１２０度の広角カメラでは２０ｍ程度先の映像が用いられる。そして、推定された車線形状を車両の真横まで延長してから、車両と車線との横方向の距離を算出する。このため、遠方で検出された破線ブロックの傾き（直線か曲線か）を誤って推定し、車線を真横まで延長すると、わずかな誤差であっても車両の真横では誤差が拡大される。例えば、広角カメラで検出された破線ブロックを直線的に延長すると、４０ｃｍ強の誤差を生じる。この結果、車線逸脱警報システムは誤った警報を発することになる。

このような誤りを防ぐために、複数時刻の路面画像から得られる複数の破線ブロックの情報を重ね合わせることで、破線ブロックの傾きを精度良く推定する方法が考えられる。しかしながら、車両の走行時の挙動によっては、進行方向に対して横方向に映像が変化する。横方向に車両が動くと、図４に示すように、時刻Ｔ１〜Ｔ３の３つの画像を重ね合わせても、車線区分線の破線が重ならずに不連続な形状となり、道路形状を正しく推定することはできない。

本発明の課題は、車線区分線が破線のカーブ路であっても、カメラの画角に依らずに道路の路面を撮影した画像から精度良く道路形状を推定することである。

道路形状推定装置は、区分線検出部、第１のパラメータ推定部、格納部、及び第２のパラメータ推定部を含む。
区分線検出部は、道路の路面を撮影した画像から車線区分線のエッジ情報を検出する。第１のパラメータ推定部は、区分線検出部が第１の時間区間内の１つ以上の時刻における画像から検出したエッジ情報に基づいて、道路の道路形状パラメータのうちの曲率及び幅を求める。格納部は、第１のパラメータ推定部が求めた曲率及び幅を格納する。

第２のパラメータ推定部は、第１のパラメータ推定部が求めた曲率及び幅を第２の時間区間における曲率及び幅として用い、区分線検出部が第２の時間区間における画像から検出したエッジ情報に基づいて、曲率及び幅以外の道路形状パラメータを求める。そして、曲率及び幅を含む道路形状パラメータを出力する。

上述した道路形状推定装置によれば、車線区分線が破線のカーブ路であっても、カメラの画角に依らずに道路の路面を撮影した画像から精度良く道路形状を推定することができる。

狭角カメラ及び広角カメラにより撮影された画像を示す図である。 狭角カメラにより撮影された破線カーブ路の画像を示す図である。 広角カメラにより撮影された破線カーブ路の画像を示す図である。 広角カメラにより撮影された画像の重ね合わせ結果を示す図である。 道路形状モデルを示す図である。 カメラの観測系モデルを示す図である。 道路形状パラメータの時間的な変化パターンを示す図である。 道路形状パラメータと観測情報の関係を示す図である。 時間区間を示す図である。 第１の道路形状推定装置の機能的構成図である。 道路形状推定処理のフローチャートである。 第２の道路形状推定装置の機能的構成図である。 区分線検出処理のフローチャートである。 デジタル画像を示す図である。 エッジ画像を示す図である。 エッジ画像から検出された境界エッジ線を示す図である。 区間決定処理のフローチャートである。 第３の道路形状推定装置の機能的構成図である。 車線逸脱警報システムの構成図である。 車両操舵システムの構成図である。 情報処理装置の構成図である。

以下、図面を参照しながら、実施形態を詳細に説明する。
図５は、道路形状モデルの例を示している。ここでは、車線の中央線Ｌｃにおける曲率半径をＲとし、車線の各位置で水平方向（Ｘ軸方向）に測って道路幅（車線幅）が常にＷとなるモデルを考える。このモデルでは、中央線Ｌｃに直交する方向での車線幅がＷとならないため、実際の車線とは異なるが、カメラによる観測点から比較的近い範囲では誤差が小さく、解析的な取り扱いが簡単になる。

図６に示すように、カメラの中心を原点Ｏとして、原点Ｏから横方向に路面座標系のＸ軸をとり、縦方向に路面座標系のＺ軸をとる。この場合、ＸＺ平面に垂直な方向が路面座標のＹ軸となる。そして、原点Ｏを路面上に垂直に投影した点をＲＯとする。図５の道路形状モデルにおける各記号の意味は以下の通りである。

Ｌｃ：車線の中央線
Ｌｒ：車線の右側の車線区分線
Ｌｌ：車線の左側の車線区分線
Ｗ：道路幅
Ｒ：曲率半径
ｅ：点ＲＯから中央線Ｌｃまでの距離
Ｐ：車線区分線Ｌｒ又はＬｌ上の着目点
Ａ：車線区分線ＬｒとＸＹ平面の交点
Ｂ：着目点ＰをＸＹ平面に垂直に投影した点
Ｃ：曲率中心
Ｄ：ＸＹ平面から着目点Ｐまでの距離

距離ｅは、カメラから道路中央までの距離（カメラの横方向オフセット）を表し、距離Ｄは、カメラから着目点Ｐまでの奥行き方向の距離を表す。このとき、着目点Ｐ（Ｘ，Ｚ）のＸ座標Ｐｘ及びＺ座標Ｐｚは、次式で与えられる。

式（１）が車線の路面上での位置を与える基礎式である。ただし、ｋは車線区分線Ｌｒ及びＬｌを区別するための定数で、着目点Ｐが右側の車線区分線Ｌｒ上にあるときはｋ＝１、左側の車線区分線Ｌｌ上にあるときはｋ＝−１である。曲率半径Ｒは、右カーブ、すなわち曲率中心ＣがＸ軸の正区間にある場合は正値とし、左カーブ、すなわち曲率中心ＣがＸ軸の負区間にある場合は負値とする。

基礎式（１）は、平方根を含んでいるため、解析的な取り扱いが難しい。そこで、曲率半径Ｒが極端に小さくない通常の走行時を想定して、基礎式（１）の近似化を行う。例えば、平方根の１次近似として、テーラー展開の第１項までを用いると、次式のようになる。

式（２）を用いると、基礎式（１）は以下のように書き改められる。
式（３）が１次近似での基礎式となる。

次に、路面上の車線区分線をカメラで観測したときの車線区分線の位置のモデルを導出する。

図６は、カメラの観測系モデルの例を示している。カメラの中心は路面から高さＨの位置にあり、カメラは車両の進行方向に対して、ピッチ角φ、ヨー角θ、ロール角０で設置されているとする。ピッチ角φ及びヨー角θは、カメラ座標系から路面座標系への座標系の変換を正の回転方向に取る。したがって、路面座標系からカメラ座標系へは、座標点の変換が回転の正方向となる。また、カメラの焦点距離はｆとする。このとき、路面座標（Ｘ，Ｙ，Ｚ）からカメラ座標（ｘｃ、ｙｃ、ｚｃ）への座標変換を考える。

路面座標系での点Ｐの座標（Ｘ，Ｙ，Ｚ）をピッチ角φとヨー角θを持つカメラ座標系での座標（ｘｃ，ｙｃ，ｚｃ）で表すと、次式のようになる。

この点をカメラで観測した画像の中央からｘ軸を右向きに、ｙ軸を下向きに取り、画像座標を（ｘ，ｙ）とすると、着目点Ｐの画像座標（ｐｘ，ｐｙ）は次式で与えられる。

式（５）のＸ及びＺに近似基礎式（３）の（Ｐｘ，Ｐｚ）を代入し、ＹにＨを代入すると、画像上での車線区分線の位置関係として次式が得られる。

式（６）から媒介変数であるＤを除けば、ｐｘとｐｙの関係式が求められる。実際には、式（６）の形のままではＤについて容易に解けないため、関係式の導出が困難である。そこで、式（６）中の各項のオーダーを考慮して、式（６）の近似化を行う。

具体的には、各回転角φ及びθは大きくても２０度程度であり、それほど大きくないため、ｓｉｎ系の値は０．１程度である。Ｄ、Ｒ、Ｗ、Ｈ、及びｅの単位をメートルとすると、式（６）中のｐｘ及びｐｙの分母において、第１項のｃｏｓ（φ）ｓｉｎ（θ）に乗算される数値は２程度であるから、第１項は０．２程度の値を持つ。また、第２項のＨの値も２程度であるから、第２項も０．２程度の値を持つ。これに対して、第３項のＤの値は３０程度であり、第３項はｃｏｓ系の積なので３０程度の値を持つ。したがって、分母では第３項が支配的で第１項及び第２項はほとんど影響を与えない。ｐｙの分子についても同様である。したがって、最初の近似式として、次式が得られる。

式（７）のｐｙの式からＤを求めて、ｐｘの式に代入し、（ｐｘ，ｐｙ）を（ｘ，ｙ）と書き改めると、次式が得られる。

式（８）が、画像中の車線区分線上の点のｘ座標とｙ座標の関係式であり、画像座標系における基礎式となる。ここで、回転角φ及びθを微小とみなして、ｓｉｎφ＝φ、ｃｏｓφ＝１、ｓｉｎθ＝θ、ｃｏｓθ＝１とすると、次式が得られる。

式（９）が、道路形状モデルを表す最終的な近似式である。式（９）の道路曲率ρは、曲率半径Ｒの逆数に相当する。なお、カメラの横方向オフセットｅとして、カメラと道路上の基準位置との横方向の相対距離を表す別のパラメータを用いることも可能である。

映像中の各時刻における画像（フレーム）から車線区分線の位置（ｘ，ｙ）が検出できれば、式（９）を用いて道路形状パラメータ（ρ，Ｗ，ｅ，φ，θ）を推定することができる。これにより、路面上の車線区分線の形状と車両の位置及び方向との関係を求めて、道路形状を推定することができる。

この道路形状推定方法は以下のように定式化できる。式（９）から分かるように、左右両方の車線区分線の位置が検出できる場合、道路形状を規定するには５つの道路形状パラメータ（ρ，Ｗ，ｅ，φ，θ）を定める必要がある。一方、短い直線状の線素として左右の破線ブロックが観測される場合の画像から得られる情報は、左右いずれか一方の破線ブロックの基準点の位置（ｘ，ｙ）、その破線ブロックの傾き、及び左右の破線ブロックの間の横方向の距離の合計４つの数値である。したがって、道路形状パラメータの自由度５に対して、１フレームから得られる観測情報の数は４であるから、拘束条件が不足し、一意に道路形状パラメータを求めることはできない。

道路形状パラメータと観測情報の関係は複数のフレームを用いた場合でも変わらない。Ｎフレームの画像から求めるべき道路形状パラメータの自由度は５Ｎであり、Ｎフレームの画像から得られる観測情報の数は４Ｎであるから、単に時系列での入力フレーム数を増やしても、道路形状パラメータを定めることはできない。

そこで、時間的に連続した複数時刻の画像を用いることを前提として、道路形状パラメータに適切な拘束条件を設けることで、求めるべき道路形状パラメータの数を削減することが考えられる。これにより、時系列に得られる破線ブロックの情報から道路形状パラメータを推定することが可能になる。

まず、破線ブロックが遠方から見え始める周期について考える。先にも示したように、高速道路では、破線ブロックの長さは８ｍで、破線ブロック間の間隙の長さは１２ｍであるから、破線ブロックと間隙の長さの合計は２０ｍである。この場合、車両の走行速度を１００ｋｍ／ｈとすると、破線ブロックが遠方で見え始める周期は０．７秒程度である。一例として、時系列の路面映像に対して、この破線ブロックが見え始めるタイミングを時間的な分割点として時間区間を定義することが可能である。このとき、１００ｋｍ／ｈでは０．７秒毎の時間区間に分割されることになる。

次に、道路形状パラメータ（ρ，Ｗ，ｅ，φ，θ）について考える。図７は、道路形状パラメータの時間的な変化パターンを示している。

図７から分かるように、道路曲率ρと道路幅Ｗは、地形によって変動するパラメータである。高速道路ではカーブ路に入る際に緩和区間が設けられており、道路曲率ρは、設計走行速度に対して３秒以上の時間で変化することが規定されている。このため、道路曲率ρは、３秒程度の時間スケールで変化する。例えば、１秒間に１０回程度の処理を行う場合、３秒程度の時間スケールの変化は非常に遅い変化である。したがって、区間内では、道路曲率ρと道路幅Ｗの変化はほとんどなく一定値とみなしても差し支えない。つまり、区間内では、道路曲率ρと道路幅Ｗの自由度は全体で２である。

また、オフセットｅとヨー角θは、車両の挙動によって変化する量であり、オフセットｅの変化速度の最大値は１．０ｍ／ｓ程度に見積もられることが一般的である。したがって、０．７秒程度の区間内では一定値とはみなせない。ただし、車両の挙動は連続的な運動であり、１秒程度の時間では、等速運動又は等加速度運動のような低次の運動モデルで近似することが可能である。例えば、等加速度運動で近似する場合、１パラメータ当たりの自由度は３であるから、区間内では、オフセットｅとヨー角θの自由度は全体で６である。

残りのピッチ角φは、路面の凹凸や傾き等により変化し、その変化速度は突発的で速い。このため、ピッチ角φは、フレーム毎に個別に表す必要がある。したがって、区間内のフレーム数をＮとすると、ピッチ角φの自由度はＮである。

以上より、Ｎフレームの区間内で求めるべき道路パラメータの自由度は、２＋６＋Ｎ＝８＋Ｎとなる。これに対して、Ｎフレームから得られる観測情報の数は４Ｎであるから、次の不等式が成り立つ場合に、道路パラメータを求めることができる。
８＋Ｎ＜４Ｎ （１１）

式（１１）より、フレーム数Ｎが３以上の場合に、道路パラメータを推定することが可能であることが分かる。

次に、道路パラメータにこのような拘束条件を設けた場合の道路パラメータ算出方法について述べる。区間内のフレームの順番（時刻）を表す添え字をｍ（ｍ＝１，２，．．．，Ｎ）とし、１フレームの画像から得られた複数の車線区分線のエッジ点を区別する添え字をｊ（ｊ＝１，２，．．．，Ｎｅ）として式（９）を書き改めると、次式のようになる。

式（２１）の各記号の意味は以下の通りである。
（ｘ^ｊ _ｍ，ｙ^ｊ _ｍ）：区間内のｍ番目のフレームから得られるｊ番目のエッジ点の座標
ρ_ｍ：区間内のｍ番目のフレームにおける道路曲率
Ｗ_ｍ：区間内のｍ番目のフレームにおける道路幅
ｅ_ｍ：区間内のｍ番目のフレームにおけるオフセット
θ_ｍ：区間内のｍ番目のフレームにおけるヨー角
φ_ｍ：区間内のｍ番目のフレームにおけるピッチ角

まず、道路曲率及び道路幅については、区間内では一定値とみなせるので、区間内における道路曲率及び道路幅をそれぞれρ^＊及びＷ^＊とする。
また、オフセットｅ_ｍ及びヨー角θ_ｍについては、それぞれ等加速度運動を仮定して、次式で近似する。

式（２２）において、Ａｅ、Ｂｅ、及びＣｅは、オフセットｅ_ｍの等加速度運動パラメータであり、Ａ_θ、Ｂ_θ、及びＣ_θは、ヨー角θ_ｍの等加速度運動パラメータである。このとき、式（２１）は、次式のように書き改められる。

式（２３）における道路形状パラメータは（ρ^＊，Ｗ^＊，φ_ｍ，Ａｅ，Ｂｅ，Ｃｅ，Ａ_θ，Ｂ_θ，Ｃ_θ）であり、区間内におけるパラメータの数は８＋Ｎである。ここで、式（２３）の右辺をＦ^ｊ _ｍ（ρ^＊，Ｗ^＊，φ_ｍ，Ａｅ，Ｂｅ，Ｃｅ，Ａ_θ，Ｂ_θ，Ｃ_θ）と記し、各パラメータを以下のように記す。

Ｆ^ｊ _ｍ（ρ^＊，Ｗ^＊，φ_ｍ，Ａｅ，Ｂｅ，Ｃｅ，Ａ_θ，Ｂ_θ，Ｃ_θ）は非線形関数であるが、これを式（２４）を用いて線形化した関数Ｇ^ｊ _ｍは、次式で与えられる。

ここで、残差ｓ^ｊ _ｍを次式のように定義する。

残差ｓ^ｊ _ｍの二乗値を、区間内の全フレーム（ｍ＝１，２，．．．，Ｎ）と各フレーム内のすべての車線区分線上のエッジ点（ｊ＝１，２，．．．，Ｎｅ）で積分した結果の１／２を総残差Ｓとする。

最適パラメータはＳを最小にする値なので、Ｓを各パラメータで偏微分した結果を０と置く。例えば、Ｓをρ^＊で偏微分した結果を０と置くと、次式が得られる。

式（２８）のｍに関する総和記号は、ｍが１からＮまでの総和を表す。他のパラメータについても同様の偏微分操作を行うことで、８＋Ｎ個の連立一次方程式が得られる。この連立一次方程式を解くことで、変化量を示すパラメータ（Δρ^＊，ΔＷ^＊，Δφ_ｍ，ΔＡｅ，ΔＢｅ，ΔＣｅ，ΔＡ_θ，ΔＢ_θ，ΔＣ_θ）が求められる。そして、各パラメータの値が十分小さくなるまで、次式の代入操作を行って演算を繰り返す。

このような繰り返し演算により、最終的な道路形状パラメータ（ρ^＊，Ｗ^＊，φ_ｍ，Ａｅ，Ｂｅ，Ｃｅ，Ａ_θ，Ｂ_θ，Ｃ_θ）が求められる。

図８は、道路形状パラメータと観測情報の数の関係を示している。道路形状パラメータに対して拘束条件を課さない場合は、道路形状パラメータを推定することができない。一方、道路曲率ρ及び道路幅Ｗに拘束条件を課した場合は、Ｎフレームの区間内の道路形状パラメータの自由度は２＋３Ｎであり、観測情報の数は４Ｎである。したがって、Ｎが２以上であれば、道路形状パラメータを推定することができる。さらにオフセットｅ及びヨー角θにも拘束条件を課した場合は、区間内の道路形状パラメータの自由度は８＋Ｎになるため、Ｎが３以上であれば、道路形状パラメータを推定することができる。

上述したように、破線ブロックが遠方から見え始めるタイミングを区間の分割点とすると、区間の長さは１００ｋｍ／ｈの走行速度で０．７秒程度となる。１秒当たり１０フレームの画像処理を前提とすると、０．７秒の区間内には７フレームが存在するので、Ｎ＝７となり、Ｎが３以上という条件を満足する。したがって、常に道路形状パラメータを推定することができることが分かる。

ただし、この方法では複数のフレームの情報を必要とするので、区間毎に間欠的にしか道路形状パラメータを求めることができない。一方、車線逸脱警報システム等のようにリアルタイム性を必要とするシステムでは、フレーム毎に道路形状パラメータを推定することが望ましい。

そこで、図９に示すように、映像を時系列の区間に分割して、現在の区間（ｔ）における各フレームの道路形状パラメータ推定には、前区間（ｔ−１）において推定された道路曲率ρ及び道路幅Ｗを用いる。上述したように、道路曲率ρ及び道路幅Ｗは変化速度が非常に遅いため、区間（ｔ−１）と区間（ｔ）ではほぼ同じ値とみなすことができる。

区間（ｔ）では、区間（ｔ−１）において推定された道路曲率ρ及び道路幅Ｗを固定値として用い、各フレームの画像から得られる車線区分線のエッジ情報を用いて残りのオフセットｅ、ピッチ角φ、及びヨー角θを推定する。次の区間（ｔ＋１）における各フレームの道路形状パラメータ推定には、現在の区間（ｔ）において推定された道路曲率ρ及び道路幅Ｗが用いられる。

このように、道路曲率ρ及び道路幅Ｗは、前区間の全時刻における情報を用いて間欠的に求めておき、他の道路形状パラメータは、各時刻におけるフレームから得られる情報を用いてフレーム毎に求める。これにより、処理対象のフレーム毎に、遅れることなく道路形状パラメータを推定することが可能になる。

ここで、区間（ｔ）における各フレームから求められる道路形状パラメータの自由度について言及しておく。左右両方の車線区分線が観測される場合、道路曲率ρ及び道路幅Ｗは区間（ｔ−１）で求めた値に固定されるので、求めるべき道路形状パラメータは、オフセットｅ、ピッチ角φ、及びヨー角θの３つである。これに対して、１フレームから得られる車線区分線の情報の数は４であり、拘束条件の数の方が多いため、一意に道路形状パラメータを推定することができる。

なお、車線区分線が実線である場合も、同様の方法により道路形状パラメータを推定することが可能である。この場合、車線区分線が途切れることなく常に遠方から見えているため、毎フレームが区間として認識されることになる。また、カーブ路では、広角カメラで撮影しても、狭角カメラで撮影したときと同じように道路曲率に応じて曲がった曲線として車線区分線を検出できるので、１フレームの画像でもすべての道路形状パラメータを推定することができる。したがって、区間毎に道路形状パラメータを推定する処理は、事実上フレーム毎に実行されることになる。

図１０は、道路形状推定装置の機能的構成例を示している。図１０の道路形状推定装置１００１は、区分線検出部１０１１、第１のパラメータ推定部１０１２、格納部１０１３、及び第２のパラメータ推定部１０１４を含む。

図１１は、図１０の道路形状推定装置１００１による道路形状推定処理の例を示すフローチャートである。まず、区分線検出部１０１１は、第１の時間区間内の１つ以上の時刻における道路の路面を撮影した画像から車線区分線のエッジ情報を検出する（ステップ１１０１）。次に、第１のパラメータ推定部１０１２は、ステップ１１０１において検出されたエッジ情報に基づいて、道路形状パラメータのうちの曲率及び幅を求め、格納部１０１３に格納する（ステップ１１０２）。

次に、区分線検出部１０１１は、第２の時間区間における画像から車線区分線のエッジ情報を検出する（ステップ１１０３）。第２のパラメータ推定部１０１４は、得られた曲率及び幅を第２の時間区間における曲率及び幅として用い、ステップ１１０３において検出されたエッジ情報に基づいて、曲率及び幅以外の道路形状パラメータを求める（ステップ１１０４）。そして、曲率及び幅を含む道路形状パラメータを出力する（ステップ１１０５）。

このような道路形状推定装置によれば、車線区分線が破線のカーブ路であっても、カメラの画角に依らずに道路の路面を撮影した画像から精度良く道路形状を推定することができる。

図１２は、道路形状推定装置の別の機能的構成例を示している。図１２の道路形状推定装置は、映像入力部１２０２、区分線検出部１２０３、第１のパラメータ推定部１２０４、格納部１２０５、及び第２のパラメータ推定部１２０６を含む。第１のパラメータ推定部１２０４は、区分線情報保存部１２１１、格納部１２１２、区間決定部１２１３、及び、曲率及び幅推定部１２１４を含む。

映像入力部１２０２は、外部のカメラ１２０１により撮影された映像を受け取り、区分線検出部１２０３に出力する。このとき、入力された映像がアナログ映像であればデジタル画像に変換し、カラー映像であればモノクロ濃淡画像に変換してから、区分線検出部１２０３に出力する。出力されるデジタル画像は、例えば、縦横にあらかじめ決められた大きさの２次元配列情報である。

区分線検出部１２０３は、入力されるデジタル画像から、路面上に存在する車線区分線を示す領域の境界線を構成するエッジ点群を検出し、そのエッジ点群の情報をエッジ情報としてパラメータ推定部１２０４及び１２０６に転送する。このとき、区分線検出部１２０３は、デジタル画像内で輝度値が急変する点をエッジ点として検出し、エッジ点の列を車線区分線の境界線として検出する。

パラメータ推定部１２０４の区分線情報保存部１２１１は、入力されたエッジ情報を格納部１２１２に格納する。区間決定部１２１３は、入力されたエッジ情報に基づいてエッジ点の位置を評価し、区間の完了（区間の切れ目）か否かを判定する。評価の結果、区間の完了が検出された場合、実行トリガを曲率及び幅推定部１２１４に出力し、区間の完了が検出されない場合は、実行トリガを出力しない。

曲率及び幅推定部１２１４は、実行トリガが入力された場合にのみ、格納部１２１２に格納された当該区間のエッジ情報から、区間の道路曲率及び道路幅を求め、得られた道路曲率及び道路幅を格納部１２０５に格納する。実行トリガが入力されない場合は、区間の道路曲率及び道路幅を求めない。

一方、パラメータ推定部１２０６は、入力されたエッジ情報と格納部１２０５に格納された道路曲率及び道路幅を用いて、残りの道路形状パラメータであるオフセット、ピッチ角、及びヨー角を求める。そして、道路形状パラメータを外部に出力し、ヨー角を格納部１２１２に格納する。

このような道路形状推定装置によれば、車線区分線が破線のカーブ路であっても、カメラの画角に依らずに道路の路面を撮影した画像から精度良く道路形状を推定することができる。特に、広角カメラを用いた場合でも、実線又は破線等の車線区分線の線種や、直線路又はカーブ路等の道路状態に影響されず、常に安定して道路形状を推定することができる。

図１３は、図１２の区分線検出部１２０３による区分線検出処理の例を示すフローチャートである。まず、区分線検出部１２０３は、入力されるデジタル画像に対してソベルオペレータ等の微分オペレータを適用し、輝度が急変するエッジ点の列からなるエッジ画像を生成する（ステップ１３０１）。例えば、図１４のようなデジタル画像からは、図１５のようなエッジ画像が生成される。

次に、エッジ画像内のあらかじめ路面領域として定めた探索範囲から、車線区分線の境界エッジ点を検出する（ステップ１３０２）。このとき、探索範囲に存在するエッジ点のうち、エッジ画像の左右の中央より左側にあるエッジ点を左側車線区分線のエッジ点とみなす。そして、そのエッジ点の左側の輝度が大きく（明るく）右側の輝度が低い（暗い）エッジ点を、左側車線区分線の境界エッジ点として検出する。また、探索範囲に存在するエッジ点のうち、エッジ画像の左右の中央より右側にあるエッジ点を右側車線区分線のエッジ点とみなす。そして、そのエッジ点の左側の輝度が小さく右側の輝度が大きいエッジ点を、右側車線区分線の境界エッジ点として検出する。

次に、得られた左側及び右側のそれぞれの境界エッジ点について、周囲画素の連結性を用いたラベル付け処理を行い、複数の境界エッジ点を統合して境界エッジ線を検出する（ステップ１３０３）。これにより、左側及び右側のそれぞれについて、１つ以上の境界エッジ線が得られる。例えば、図１５のエッジ画像の探索範囲１５０１からは、図１６のような左側の境界エッジ線１６０１と右側の境界エッジ線１６０２が検出される。

検出された左側の境界エッジ線の集合Ｓ_ＥＬは、次式のように記述される。

ＥＬ_ｉは、左側の境界エッジ線のうちｉ番目（ｉ＝１，２，．．．，ＮＥＬ）の境界エッジ線を表し、ＮＥＬは、左側の境界エッジ線の数を表す。同様に、右側の境界エッジ線の集合Ｓ_ＥＲは、次式のように記述される。

ＥＲ_ｉは、右側の境界エッジ線のうちｉ番目（ｉ＝１，２，．．．，ＮＥＬ）の境界エッジ線を表す。また、左側の境界エッジ線ＥＬ_ｉは、ＮＰＬ_ｉ個のエッジ点の列であり、次式のように記述される。

ＰＬ_ｉ ^ｊは、左側のｉ番目の境界エッジ線のｊ番目（ｊ＝１，２，．．．，ＮＰＬ_ｉ）のエッジ点の画像座標（ｘＬ_ｉ ^ｊ，ｙＬ_ｉ ^ｊ）を表す。同様に、右側の境界エッジ線ＥＲ_ｉは、ＮＰＲ_ｉ個のエッジ点の列であり、次式のように記述される。

ＰＲ_ｉ ^ｊは、右側のｉ番目の境界エッジ線のｊ番目（ｊ＝１，２，．．．，ＮＰＲ_ｉ）のエッジ点の画像座標（ｘＲ_ｉ ^ｊ，ｙＲ_ｉ ^ｊ）を表す。

区分線情報保存部１２１１は、入力された境界エッジ線の集合Ｓ_ＥＬ及びＳ_ＥＲを、格納部１２１２内の現在の区間におけるｍ番目のフレームに対応する格納領域にＳ_ＥＬ ^ｍ及びＳ_ＥＲ ^ｍとして格納する。

そして、次のフレームの格納領域を指し示すように、ｍを１だけインクリメントする。なお、ｍの値は、道路形状推定装置の起動時に１に初期化されているものとする。

区間決定部１２１３は、入力された境界エッジ線の集合Ｓ_ＥＬ及びＳ_ＥＲを用いて、次のような操作により、区間の更新の有無を決定する。

まず、以下のような２つの操作を定義する。ただし、図３に示したように、画像座標系のｙ軸は下向きが正である。

ＧＥＴ＿ＴＯＰ（Ｓ）：＝｛集合Ｓに含まれる各境界エッジ線の上端のエッジ点のうち、最も上側にあるエッジ点のｙ座標を返す。集合Ｓが空であればＮＵＬＬを返す。｝
ＧＥＴ＿ＢＯＴＨ＿ＴＯＰ（Ｓ１，Ｓ２）：＝ＭＩＮ（ＧＥＴ＿ＴＯＰ（Ｓ１），ＧＥＴ＿ＴＯＰ（Ｓ２））

図１７は、以上の操作を用いた区間決定処理の例を示すフローチャートである。まず、区間決定部１２１３は、左右の境界エッジ線のエッジ点のうち最も上側にあるエッジ点のｙ座標Ｙ_ＴＯＰを、次式により求める（ステップ１７０１）。
Ｙ_ＴＯＰ＝ＧＥＴ＿ＢＯＴＨ＿ＴＯＰ（Ｓ_ＥＬ，Ｓ_ＥＲ）

ここで、画像から境界エッジ線がまったく検出されなかった場合、つまりＳ_ＥＬ及びＳ_ＥＲが空の場合は、Ｙ_ＴＯＰ＝ＮＵＬＬとする。

次に、Ｙ_ＴＯＰがＮＵＬＬか否かをチェックする（ステップ１７０２）。Ｙ_ＴＯＰがＮＵＬＬであれば（ステップ１７０２，ＹＥＳ）、処理を終了する。Ｙ_ＴＯＰがＮＵＬＬでなければ（ステップ１７０２，ＮＯ）、Ｙ_ＴＯＰを、格納部１２１２に格納されたＹ_ＴＯＰ ^ＬＡＳＴと比較する（ステップ１７０３）。Ｙ_ＴＯＰ ^ＬＡＳＴは、前回の区間決定処理で求められた評価したＹ_ＴＯＰである。

Ｙ_ＴＯＰがＹ_ＴＯＰ ^ＬＡＳＴより小さければ（ステップ１７０３，ＹＥＳ）、検出された境界エッジ線が車両の手前で見切れて、次の境界エッジ線が遠方から見え始めたことが分かる。そこで、区間の更新が発生したと判断して、曲率及び幅推定部１２１４に実行トリガを出力する（ステップ１７０４）。Ｙ_ＴＯＰがＹ_ＴＯＰ ^ＬＡＳＴ以上であれば（ステップ１７０３，ＮＯ）、実行トリガを出力しない。

そして、ステップ１７０１で得られたＹ_ＴＯＰによりＹ_ＴＯＰ ^ＬＡＳＴを更新する（ステップ１７０５）。更新されたＹ_ＴＯＰ ^ＬＡＳＴは、次回の区間決定処理で用いられる。
曲率及び幅推定部１２１４は、区間決定部１２１３から実行トリガを受け取ると、格納部１２１２に格納されている（Ｓ_ＥＬ ^１，Ｓ_ＥＲ ^１），（Ｓ_ＥＬ ^２，Ｓ_ＥＲ ^２），．．．，（Ｓ_ＥＬ ^ｍ，Ｓ_ＥＲ ^ｍ）から、区間内の道路曲率ρ^＊及び道路幅Ｗ^＊を推定し、格納部１２０５に格納する。そして、格納部１２１２内の境界エッジ線の集合を消去し、区間内のフレームの順番ｍの値を１に初期化する。

区間内の道路曲率ρ^＊及び道路幅Ｗ^＊は、例えば、上述した式（２３）を用いて推定することができる。式（２３）の道路形状パラメータ（ρ^＊，Ｗ^＊，φ_ｍ，Ａｅ，Ｂｅ，Ｃｅ，Ａ_θ，Ｂ_θ，Ｃ_θ）のうち、ピッチ角φ_ｍは、パラメータ推定部１２０６によりフレーム毎に推定され格納部１２１２に格納されている値を用いることができる。したがって、ピッチ角φ_ｍは求めるべきパラメータから除外され、求めるべきパラメータは（ρ^＊，Ｗ^＊，Ａｅ，Ｂｅ，Ｃｅ，Ａ_θ，Ｂ_θ，Ｃ_θ）の８個である。これらの８個のパラメータに対して式（２３）は線形であるから、式（２３）の右辺Ｆ^ｊ _ｍは、次式のように記述することができる。

ここで、残差ｓ^ｊ _ｍを次式のように定義する。

総残差Ｓは、式（６１）の残差ｓ^ｊ _ｍを用いて式（２７）と同様に定義できる。最適パラメータはＳを最小にする値なので、Ｓを各パラメータで偏微分した結果を０と置く。例えば、Ｓをρ^＊で偏微分した結果を０と置くと、次式が得られる。

式（６３）のｊに関する総和記号は、ｊが１からＮｅまでの総和を表す。他のパラメータについても同様の偏微分操作を行うことで、８個の連立一次方程式が得られる。この連立一次方程式を解くことで、パラメータ（ρ^＊，Ｗ^＊，Ａｅ，Ｂｅ，Ｃｅ，Ａ_θ，Ｂ_θ，Ｃ_θ）が求められる。

このような曲率及び幅推定処理の手順は、以下のようになる。
１．Ｒ１１＝Ｒ１２＝Ｒ１３＝Ｒ１４＝Ｒ１５＝Ｒ１６＝Ｒ１７＝Ｒ１８＝Ｒ１９＝０
Ｒ２１＝Ｒ２２＝Ｒ２３＝Ｒ２４＝Ｒ２５＝Ｒ２６＝Ｒ２７＝Ｒ２８＝Ｒ２９＝０
Ｒ３１＝Ｒ３２＝Ｒ３３＝Ｒ３４＝Ｒ３５＝Ｒ３６＝Ｒ３７＝Ｒ３８＝Ｒ３９＝０
Ｒ４１＝Ｒ４２＝Ｒ４３＝Ｒ４４＝Ｒ４５＝Ｒ４６＝Ｒ４７＝Ｒ４８＝Ｒ４９＝０
Ｒ５１＝Ｒ５２＝Ｒ５３＝Ｒ５４＝Ｒ５５＝Ｒ５６＝Ｒ５７＝Ｒ５８＝Ｒ５９＝０
Ｒ６１＝Ｒ６２＝Ｒ６３＝Ｒ６４＝Ｒ６５＝Ｒ６６＝Ｒ６７＝Ｒ６８＝Ｒ６９＝０
Ｒ７１＝Ｒ７２＝Ｒ７３＝Ｒ７４＝Ｒ７５＝Ｒ７６＝Ｒ７７＝Ｒ７８＝Ｒ７９＝０
Ｒ８１＝Ｒ８２＝Ｒ８３＝Ｒ８４＝Ｒ８５＝Ｒ８６＝Ｒ８７＝Ｒ８８＝Ｒ８９＝０
２．ｔｇｔ＝−１
３．ｔｇｔ＝ｔｇｔ＋１
４．ｉｆ ｔｇｔ＝＝０ ｔｈｅｎ
エッジ集合Ｅ＝Ｓ_ＥＬ ^ｍ，ＮＥ＝Ｓ_ＥＬ ^ｍの要素数，ｋ＝−１
ｅｌｓｅ ｉｆ ｔｇｔ＝＝１ ｔｈｅｎ
エッジ集合Ｅ＝Ｓ_ＥＲ ^ｍ，ＮＥ＝Ｓ_ＥＲ ^ｍのエッジ数，ｋ＝１
ｅｌｓｅ ｇｏｔｏ １２ ｅｎｄｉｆ
５．ｊ＝０
６．ｊ＝ｊ＋１
７．ｉｆ ｊ＞＝ＮＥ ｔｈｅｎ ｇｏｔｏ ３ ｅｎｄｉｆ
８．エッジ集合Ｅからｊ番目の要素であるエッジ点の位置（ｘ^ｊ，ｙ^ｊ）を取得する。
９．式（４２）〜（５０）から（ａ_ｊ，ｂ_ｊ，ｃ_ｊ，ｄ_ｊ，ｅ_ｊ，ｆ_ｊ，ｇ_ｊ，ｈ_ｊ，ｑ_ｊ）を算出する。
１０．Ｒ１１＋＝ａ_ｊａ_ｊ，Ｒ１２＋＝ａ_ｊｂ_ｊ，Ｒ１３＋＝ａ_ｊｃ_ｊ，Ｒ１４＋＝ａ_ｊｄ_ｊ，
Ｒ１５＋＝ａ_ｊｅ_ｊ，Ｒ１６＋＝ａ_ｊｆ_ｊ，Ｒ１７＋＝ａ_ｊｇ_ｊ，Ｒ１８＋＝ａ_ｊｈ_ｊ，
Ｒ１９＋＝ａ_ｊｑ_ｊ，
Ｒ２１＋＝ｂ_ｊａ_ｊ，Ｒ２２＋＝ｂ_ｊｂ_ｊ，Ｒ２３＋＝ｂ_ｊｃ_ｊ，Ｒ２４＋＝ｂ_ｊｄ_ｊ，
Ｒ２５＋＝ｂ_ｊｅ_ｊ，Ｒ２６＋＝ｂ_ｊｆ_ｊ，Ｒ２７＋＝ｂ_ｊｇ_ｊ，Ｒ２８＋＝ｂ_ｊｈ_ｊ，
Ｒ２９＋＝ｂ_ｊｑ_ｊ，
Ｒ３１＋＝ｃ_ｊａ_ｊ，Ｒ３２＋＝ｃ_ｊｂ_ｊ，Ｒ３３＋＝ｃ_ｊｃ_ｊ，Ｒ３４＋＝ｃ_ｊｄ_ｊ，
Ｒ３５＋＝ｃ_ｊｅ_ｊ，Ｒ３６＋＝ｃ_ｊｆ_ｊ，Ｒ３７＋＝ａ_ｊｇ_ｊ，Ｒ３８＋＝ｃ_ｊｈ_ｊ，
Ｒ３９＋＝ｃ_ｊｑ_ｊ，
Ｒ４１＋＝ｄ_ｊａ_ｊ，Ｒ４２＋＝ｄ_ｊｂ_ｊ，Ｒ４３＋＝ｄ_ｊｃ_ｊ，Ｒ４４＋＝ｄ_ｊｄ_ｊ，
Ｒ４５＋＝ｄ_ｊｅ_ｊ，Ｒ４６＋＝ｄ_ｊｆ_ｊ，Ｒ４７＋＝ｄ_ｊｇ_ｊ，Ｒ４８＋＝ｄ_ｊｈ_ｊ，
Ｒ４９＋＝ｄ_ｊｑ_ｊ，
Ｒ５１＋＝ｅ_ｊａ_ｊ，Ｒ５２＋＝ｅ_ｊｂ_ｊ，Ｒ５３＋＝ｅ_ｊｃ_ｊ，Ｒ５４＋＝ｅ_ｊｄ_ｊ，
Ｒ５５＋＝ｅ_ｊｅ_ｊ，Ｒ５６＋＝ｅ_ｊｆ_ｊ，Ｒ５７＋＝ｅ_ｊｇ_ｊ，Ｒ５８＋＝ｅ_ｊｈ_ｊ，
Ｒ５９＋＝ｅ_ｊｑ_ｊ，
Ｒ６１＋＝ｆ_ｊａ_ｊ，Ｒ６２＋＝ｆ_ｊｂ_ｊ，Ｒ６３＋＝ｆ_ｊｃ_ｊ，Ｒ６４＋＝ｆ_ｊｄ_ｊ，
Ｒ６５＋＝ｆ_ｊｅ_ｊ，Ｒ６６＋＝ｆ_ｊｆ_ｊ，Ｒ６７＋＝ｆ_ｊｇ_ｊ，Ｒ６８＋＝ｆ_ｊｈ_ｊ，
Ｒ６９＋＝ｆ_ｊｑ_ｊ，
Ｒ７１＋＝ｇ_ｊａ_ｊ，Ｒ７２＋＝ｇ_ｊｂ_ｊ，Ｒ７３＋＝ｇ_ｊｃ_ｊ，Ｒ７４＋＝ｇ_ｊｄ_ｊ，
Ｒ７５＋＝ｇ_ｊｅ_ｊ，Ｒ７６＋＝ｇ_ｊｆ_ｊ，Ｒ７７＋＝ｇ_ｊｇ_ｊ，Ｒ７８＋＝ｇ_ｊｈ_ｊ，
Ｒ７９＋＝ｇ_ｊｑ_ｊ，
Ｒ８１＋＝ｑ_ｊａ_ｊ，Ｒ８２＋＝ｑ_ｊｂ_ｊ，Ｒ８３＋＝ｑ_ｊｃ_ｊ，Ｒ８４＋＝ｑ_ｊｄ_ｊ，
Ｒ８５＋＝ｑ_ｊｅ_ｊ，Ｒ８６＋＝ｑ_ｊｆ_ｊ，Ｒ８７＋＝ｑ_ｊｇ_ｊ，Ｒ８８＋＝ｑ_ｊｈ_ｊ，
Ｒ８９＋＝ｑ_ｊｑ_ｊ
１１．ｇｏｔｏ ６
１２．以下の連立方程式の解を数値計算により求める。
１３．終了

パラメータ推定部１２０６は、区分線検出部１２０３から入力されるＳ_ＥＬ及びＳ_ＥＲと格納部１２０５に格納されている道路曲率ρ^＊及び道路幅Ｗ^＊から、オフセットｅ、ピッチ角φ、及びヨー角θを求める。そして、ピッチ角φを、格納部１２１２内の現在の区間におけるｍ番目のフレームに対応する格納領域に格納し、道路曲率ρ^＊、道路幅Ｗ^＊、オフセットｅ、ピッチ角φ、及びヨー角θを外部に出力する。

オフセットｅ、ピッチ角φ、及びヨー角θは、例えば、上述した式（９）を用いて推定することができる。式（９）の道路曲率ρ及び道路幅Ｗを一定値とみなしてρ^＊及びＷ^＊と書くと、次式が得られる。

式（７１）の各記号の意味は以下の通りである。
（ｘ^ｊ，ｙ^ｊ）：現在のフレームから得られるｊ番目のエッジ点の座標
ｅ：現在のフレームにおけるオフセット
θ：現在のフレームにおけるヨー角
φ：現在のフレームにおけるピッチ角

式（７１）のパラメータは（φ，ｅ，θ）の３個である。ここで、式（７１）の右辺をＫ（φ，ｅ，θ）と記し、各パラメータを以下のように記す。

（φ_０，ｅ_０，θ_０）は初期値であり、前のフレームの処理により求められた値が初期値として用いられる。Ｋ（φ，ｅ，θ）は非線形関数であるが、これを式（７２）を用いて線形化した関数Ｊは、次式で与えられる。

ここで、残差ｓ^ｊを次式のように定義する。

残差ｓ^ｊの二乗値を、フレーム内のすべての車線区分線上のエッジ点（ｊ＝１，２，．．．，Ｎｅ）で積分した結果の１／２を総残差Ｓとする。

最適パラメータはＳを最小にする値なので、Ｓを各パラメータで偏微分した結果を０と置くと、次のような連立一次方程式が得られる。

この連立一次方程式を解くことで、変化量を示すパラメータ（Δφ，Δｅ，Δθ）が求められる。そして、各パラメータの値が十分小さくなるまで、次式の代入操作を行って演算を繰り返す。

このような繰り返し演算により、最終的な道路形状パラメータ（φ，ｅ，θ）が求められる。

このような道路形状パラメータ推定処理の手順は、以下のようになる。
１．（φ_０，ｅ_０，θ_０）を前のフレームの値に初期化する。
２．Ｒ１１＝Ｒ１２＝Ｒ１３＝Ｒ１４＝０
Ｒ２１＝Ｒ２２＝Ｒ２３＝Ｒ２４＝０
Ｒ３１＝Ｒ３２＝Ｒ３３＝Ｒ３４＝０
３．ｔｇｔ＝−１
４．ｔｇｔ＝ｔｇｔ＋１
５．ｉｆ ｔｇｔ＝＝０ ｔｈｅｎ
エッジ集合Ｅ＝Ｓ_ＥＬ，ＮＥ＝Ｓ_ＥＬの要素数，ｋ＝−１
ｅｌｓｅ ｉｆ ｔｇｔ＝＝１ ｔｈｅｎ
エッジ集合Ｅ＝Ｓ_ＥＲ，ＮＥ＝Ｓ_ＥＲのエッジ数，ｋ＝１
ｅｌｓｅ ｇｏｔｏ １３ ｅｎｄｉｆ
６．ｊ＝０
７．ｊ＝ｊ＋１
８．ｉｆ ｊ＞＝ＮＥ ｔｈｅｎ ｇｏｔｏ ４ ｅｎｄｉｆ
９．エッジ集合Ｅからｊ番目の要素であるエッジ点の位置（ｘ^ｊ，ｙ^ｊ）を取得する。
１０．式（７３）及び（７５）から（ａ_ｊ，ｂ_ｊ，ｃ_ｊ，ｄ_ｊ）を算出する。
１１．Ｒ１１＋＝ａ_ｊａ_ｊ，Ｒ１２＋＝ａ_ｊｂ_ｊ，Ｒ１３＋＝ａ_ｊｃ_ｊ，Ｒ１４＋＝ａ_ｊｄ_ｊ，
Ｒ２１＋＝ｂ_ｊａ_ｊ，Ｒ２２＋＝ｂ_ｊｂ_ｊ，Ｒ２３＋＝ｂ_ｊｃ_ｊ，Ｒ２４＋＝ｂ_ｊｄ_ｊ，
Ｒ３１＋＝ｃ_ｊａ_ｊ，Ｒ３２＋＝ｃ_ｊｂ_ｊ，Ｒ３３＋＝ｃ_ｊｃ_ｊ，Ｒ３４＋＝ｃ_ｊｄ_ｊ
１２．ｇｏｔｏ ７
１３．以下の連立方程式の解を数値計算により求める。
１４．解の収束性を検査する。
ｉｆ ｜Δφ｜＞ＴｈＲ ｏｒ ｜Δｅ｜＞ＴｈＲ ｏｒ ｜Δθ｜＞ＴｈＲ
ｔｈｅｎ
ｇｏｔｏ ２
ｅｌｓｅ
φ＝φ_０，ｅ＝ｅ_０，θ＝θ_０
ｅｎｄｉｆ
１５．終了
ＴｈＲは解の収束性を評価するための微小な閾値である。

図１２の道路形状推定装置では、区間の完了を検出する度に実行トリガを曲率及び幅推定部１２１４に出力しているが、所定数のフレームが入力される度に実行トリガを出力することも可能である。

図１８は、このような道路形状推定装置の機能的構成例を示している。図１８の道路形状推定装置は、図１２のパラメータ推定部１２０６をパラメータ推定部１８０１に置き換えた構成を有し、パラメータ推定部１８０１は、図１２の区間決定部１２１３を区間フレームカウント部１８１１に置き換えた構成を有する。

区間フレームカウント部１８１１は、区分線検出部１２０３からエッジ情報が入力される度にフレームカウンタの値ｍを１だけインクリメントし、ｍが所定値を超えたとき、実行トリガを曲率及び幅推定部１２１４に出力し、ｍを１に初期化する。ｍが所定値を超えていなければ、実行トリガを出力しない。この場合、フレームカウンタの初期値に対応する時刻から所定値を超えた時刻までの時間が区間として認識される。道路形状推定装置の他の動作は、図１２の場合と同様である。

図１２及び図１８の曲率及び幅推定部１２１４では、オフセットｅ_ｍ及びヨー角θ_ｍの拘束条件として等加速度運動モデルを用いているが、代わりに等速運動モデルを用いていることも可能である。この場合、オフセットｅ_ｍ及びヨー角θ_ｍについては、それぞれ等速度運動を仮定して、次式で近似する。

式（８１）において、Ａｅ及びＢｅは、オフセットｅ_ｍの等速度運動パラメータであり、Ａ_θ及びＢ_θは、ヨー角θ_ｍの等速度運動パラメータである。このとき、式（２１）は、次式のように書き改められる。

式（８２）の道路形状パラメータ（ρ^＊，Ｗ^＊，φ_ｍ，Ａｅ，Ｂｅ，Ａ_θ，Ｂ_θ）のうち、ピッチ角φ_ｍは求めるべきパラメータから除外され、求めるべきパラメータは（ρ^＊，Ｗ^＊，Ａｅ，Ｂｅ，Ａ_θ，Ｂ_θ）の６個である。これらの６個のパラメータに対して式（８２）は線形であるから、式（８２）の右辺Ｆ^ｊ _ｍは、次式のように記述することができる。

ここで、残差ｓ^ｊ _ｍを次式のように定義する。

総残差Ｓは、式（１０１）の残差ｓ^ｊ _ｍを用いて式（２７）と同様に定義できる。最適パラメータはＳを最小にする値なので、Ｓを各パラメータで偏微分した結果を０と置く。例えば、Ｓをρ^＊で偏微分した結果を０と置くと、次式が得られる。

他のパラメータについても同様の偏微分操作を行うことで、６個の連立一次方程式が得られる。この連立一次方程式を解くことで、パラメータ（ρ^＊，Ｗ^＊，Ａｅ，Ｂｅ，Ａ_θ，Ｂ_θ）が求められる。

このような曲率及び幅推定処理の手順は、以下のようになる。
１．Ｒ１１＝Ｒ１２＝Ｒ１３＝Ｒ１４＝Ｒ１５＝Ｒ１６＝Ｒ１７＝０
Ｒ２１＝Ｒ２２＝Ｒ２３＝Ｒ２４＝Ｒ２５＝Ｒ２６＝Ｒ２７＝０
Ｒ３１＝Ｒ３２＝Ｒ３３＝Ｒ３４＝Ｒ３５＝Ｒ３６＝Ｒ３７＝０
Ｒ４１＝Ｒ４２＝Ｒ４３＝Ｒ４４＝Ｒ４５＝Ｒ４６＝Ｒ４７＝０
Ｒ５１＝Ｒ５２＝Ｒ５３＝Ｒ５４＝Ｒ５５＝Ｒ５６＝Ｒ５７＝０
Ｒ６１＝Ｒ６２＝Ｒ６３＝Ｒ６４＝Ｒ６５＝Ｒ６６＝Ｒ６７＝０
２．ｔｇｔ＝−１
３．ｔｇｔ＝ｔｇｔ＋１
４．ｉｆ ｔｇｔ＝＝０ ｔｈｅｎ
エッジ集合Ｅ＝Ｓ_ＥＬ ^ｍ，ＮＥ＝Ｓ_ＥＬ ^ｍの要素数，ｋ＝−１
ｅｌｓｅ ｉｆ ｔｇｔ＝＝１ ｔｈｅｎ
エッジ集合Ｅ＝Ｓ_ＥＲ ^ｍ，ＮＥ＝Ｓ_ＥＲ ^ｍのエッジ数，ｋ＝１
ｅｌｓｅ ｇｏｔｏ １２ ｅｎｄｉｆ
５．ｊ＝０
６．ｊ＝ｊ＋１
７．ｉｆ ｊ＞＝ＮＥ ｔｈｅｎ ｇｏｔｏ ３ ｅｎｄｉｆ
８．エッジ集合Ｅからｊ番目の要素であるエッジ点の位置（ｘ^ｊ，ｙ^ｊ）を取得する。
９．式（８４）〜（９０）から（ａ_ｊ，ｂ_ｊ，ｃ_ｊ，ｄ_ｊ，ｅ_ｊ，ｆ_ｊ，ｑ_ｊ）を算出する。
１０．Ｒ１１＋＝ａ_ｊａ_ｊ，Ｒ１２＋＝ａ_ｊｂ_ｊ，Ｒ１３＋＝ａ_ｊｃ_ｊ，Ｒ１４＋＝ａ_ｊｄ_ｊ，
Ｒ１５＋＝ａ_ｊｅ_ｊ，Ｒ１６＋＝ａ_ｊｆ_ｊ，Ｒ１７＋＝ａ_ｊｑ_ｊ，
Ｒ２１＋＝ｂ_ｊａ_ｊ，Ｒ２２＋＝ｂ_ｊｂ_ｊ，Ｒ２３＋＝ｂ_ｊｃ_ｊ，Ｒ２４＋＝ｂ_ｊｄ_ｊ，
Ｒ２５＋＝ｂ_ｊｅ_ｊ，Ｒ２６＋＝ｂ_ｊｆ_ｊ，Ｒ２７＋＝ｂ_ｊｑ_ｊ，
Ｒ３１＋＝ｃ_ｊａ_ｊ，Ｒ３２＋＝ｃ_ｊｂ_ｊ，Ｒ３３＋＝ｃ_ｊｃ_ｊ，Ｒ３４＋＝ｃ_ｊｄ_ｊ，
Ｒ３５＋＝ｃ_ｊｅ_ｊ，Ｒ３６＋＝ｃ_ｊｆ_ｊ，Ｒ３７＋＝ｃ_ｊｑ_ｊ，
Ｒ４１＋＝ｄ_ｊａ_ｊ，Ｒ４２＋＝ｄ_ｊｂ_ｊ，Ｒ４３＋＝ｄ_ｊｃ_ｊ，Ｒ４４＋＝ｄ_ｊｄ_ｊ，
Ｒ４５＋＝ｄ_ｊｅ_ｊ，Ｒ４６＋＝ｄ_ｊｆ_ｊ，Ｒ４７＋＝ｄ_ｊｑ_ｊ，
Ｒ５１＋＝ｅ_ｊａ_ｊ，Ｒ５２＋＝ｅ_ｊｂ_ｊ，Ｒ５３＋＝ｅ_ｊｃ_ｊ，Ｒ５４＋＝ｅ_ｊｄ_ｊ，
Ｒ５５＋＝ｅ_ｊｅ_ｊ，Ｒ５６＋＝ｅ_ｊｆ_ｊ，Ｒ５７＋＝ｅ_ｊｑ_ｊ，
Ｒ６１＋＝ｆ_ｊａ_ｊ，Ｒ６２＋＝ｆ_ｊｂ_ｊ，Ｒ６３＋＝ｆ_ｊｃ_ｊ，Ｒ６４＋＝ｆ_ｊｄ_ｊ，
Ｒ６５＋＝ｆ_ｊｅ_ｊ，Ｒ６６＋＝ｆ_ｊｆ_ｊ，Ｒ６７＋＝ｆ_ｊｑ_ｊ
１１．ｇｏｔｏ ６
１２．以下の連立方程式の解を数値計算により求める。
１３．終了

ところで、オフセットｅ_ｍ及びヨー角θ_ｍに加えて、ピッチ角φ_ｍにも拘束条件を設けることが可能である。ここでは、ピッチ角φ_ｍに対して、時間に関する３次の項までを含む高次運動を仮定する。この場合、以下に示すように、式（２２）にピッチ角φ_ｍの近似式が追加される。

式（１１１）において、Ａｅ、Ｂｅ、及びＣｅは、オフセットｅ_ｍの等加速度運動パラメータであり、Ａ_θ、Ｂ_θ、及びＣ_θは、ヨー角θ_ｍの等加速度運動パラメータである。また、Ａ_φ、Ｂ_φ、Ｃ_φ、及びＤ_φは、ピッチ角φ_ｍの高次運動パラメータである。このとき、式（２１）は、次式のように書き改められる。

式（１１２）における道路形状パラメータは（ρ^＊，Ｗ^＊，Ａ_φ，Ｂ_φ，Ｃ_φ，Ｄ_φ，Ａｅ，Ｂｅ，Ｃｅ，Ａ_θ，Ｂ_θ，Ｃ_θ）であり、区間内におけるパラメータの数は１２である。ここで、式（１１２）の右辺をＦ^ｊ _ｍ（ρ^＊，Ｗ^＊，Ａ_φ，Ｂ_φ，Ｃ_φ，Ｄ_φ，Ａｅ，Ｂｅ，Ｃｅ，Ａ_θ，Ｂ_θ，Ｃ_θ）と記し、各パラメータを以下のように記す。

Ｆ^ｊ _ｍ（ρ^＊，Ｗ^＊，Ａ_φ，Ｂ_φ，Ｃ_φ，Ｄ_φ，Ａｅ，Ｂｅ，Ｃｅ，Ａ_θ，Ｂ_θ，Ｃ_θ）は非線形関数であるが、これを式（１１３）を用いて線形化した関数Ｇ^ｊ _ｍは、次式で与えられる。

ここで、残差ｓ^ｊ _ｍを次式のように定義する。

総残差Ｓは、式（１１５）の残差ｓ^ｊ _ｍを用いて式（２７）と同様に定義できる。最適パラメータはＳを最小にする値なので、Ｓを各パラメータで偏微分した結果を０と置く。例えば、Ｓをρ^＊で偏微分した結果を０と置くと、次式が得られる。

他のパラメータについても同様の偏微分操作を行うことで、１２個の連立一次方程式が得られる。この連立一次方程式を解くことで、変化量を示すパラメータ（Δρ^＊，ΔＷ^＊，ΔＡ_φ，ΔＢ_φ，ΔＣ_φ，ΔＤ_φ，ΔＡｅ，ΔＢｅ，ΔＣｅ，ΔＡ_θ，ΔＢ_θ，ΔＣ_θ）が求められる。そして、各パラメータの値が十分小さくなるまで、次式の代入操作を行って演算を繰り返す。

このような繰り返し演算により、最終的な道路形状パラメータ（ρ^＊，Ｗ^＊，Ａ_φ，Ｂ_φ，Ｃ_φ，Ｄ_φ，Ａｅ，Ｂｅ，Ｃｅ，Ａ_θ，Ｂ_θ，Ｃ_θ）が求められる。

このような曲率及び幅推定処理の手順は、以下のようになる。
１．（ρ^＊ _０，Ｗ^＊ _０，Ａ_φ０，Ｂ_φ０，Ｃ_φ０，Ｄ_φ０，Ａｅ_０，Ｂｅ_０，Ｃｅ_０，Ａ_θ０，Ｂ_θ０，Ｃ_θ０）を前のフレームの値に初期化する。
２．Ｒ０１０１＝Ｒ０１０２＝Ｒ０１０３＝Ｒ０１０４＝Ｒ０１０５
＝Ｒ０１０６＝Ｒ０１０７＝Ｒ０１０８＝Ｒ０１０９＝Ｒ０１１０
＝Ｒ０１１１＝Ｒ０１１２＝０Ｒ１１３＝０
Ｒ０２０１＝Ｒ０２０２＝Ｒ０２０３＝Ｒ０２０４＝Ｒ０２０５
＝Ｒ０２０６＝Ｒ０２０７＝Ｒ０２０８＝Ｒ０２０９＝Ｒ０２１０
＝Ｒ０２１１＝Ｒ０２１２＝Ｒ０２１３＝０
Ｒ０３０１＝Ｒ０３０２＝Ｒ０３０３＝Ｒ０３０４＝Ｒ０３０５
＝Ｒ０３０６＝Ｒ０３０７＝Ｒ０３０８＝Ｒ０３０９＝Ｒ０３１０
＝Ｒ０３１１＝Ｒ０３１２＝Ｒ０３１３＝０
Ｒ０４０１＝Ｒ０４０２＝Ｒ０４０３＝Ｒ０４０４＝Ｒ０４０５
＝Ｒ０４０６＝Ｒ０４０７＝Ｒ０４０８＝Ｒ０４０９＝Ｒ０４１０
＝Ｒ０４１１＝Ｒ０４１２＝Ｒ０４１３＝０
Ｒ０５０１＝Ｒ０５０２＝Ｒ０５０３＝Ｒ０５０４＝Ｒ０５０５
＝Ｒ０５０６＝Ｒ０５０７＝Ｒ０５０８＝Ｒ０５０９＝Ｒ０５１０
＝Ｒ０５１１＝Ｒ０５１２＝Ｒ０５１３＝０
Ｒ０６０１＝Ｒ０６０２＝Ｒ０６０３＝Ｒ０６０４＝Ｒ０６０５
＝Ｒ０６０６＝Ｒ０６０７＝Ｒ０６０８＝Ｒ０６０９＝Ｒ０６１０
＝Ｒ０６１１＝Ｒ０６１２＝Ｒ０６１３＝０
Ｒ０７０１＝Ｒ０７０２＝Ｒ０７０３＝Ｒ０７０４＝Ｒ０７０５
＝Ｒ０７０６＝Ｒ０７０７＝Ｒ０７０８＝Ｒ０７０９＝Ｒ０７１０
＝Ｒ０７１１＝Ｒ０７１２＝Ｒ０７１３＝０
Ｒ０８０１＝Ｒ０８０２＝Ｒ０８０３＝Ｒ０８０４＝Ｒ０８０５
＝Ｒ０８０６＝Ｒ０８０７＝Ｒ０８０８＝Ｒ０８０９＝Ｒ０８１０
＝Ｒ０８１１＝Ｒ０８１２＝Ｒ０８１３＝０
Ｒ０９０１＝Ｒ０９０２＝Ｒ０９０３＝Ｒ０９０４＝Ｒ０９０５
＝Ｒ０９０６＝Ｒ０９０７＝Ｒ０９０８＝Ｒ０９０９＝Ｒ０９１０
＝Ｒ０９１１＝Ｒ０９１２＝Ｒ０９１３＝０
Ｒ１００１＝Ｒ１００２＝Ｒ１００３＝Ｒ１００４＝Ｒ１００５
＝Ｒ１００６＝Ｒ１００７＝Ｒ１００８＝Ｒ１００９＝Ｒ１０１０
＝Ｒ１０１１＝Ｒ１０１２＝Ｒ１０１３＝０
Ｒ１１０１＝Ｒ１１０２＝Ｒ１１０３＝Ｒ１１０４＝Ｒ１１０５
＝Ｒ１１０６＝Ｒ１１０７＝Ｒ１１０８＝Ｒ１１０９＝Ｒ１１１０
＝Ｒ１１１１＝Ｒ１１１２＝Ｒ１１１３＝０
Ｒ１２０１＝Ｒ１２０２＝Ｒ１２０３＝Ｒ１２０４＝Ｒ１２０５
＝Ｒ１２０６＝Ｒ１２０７＝Ｒ１２０８＝Ｒ１２０９＝Ｒ１２１０
＝Ｒ１２１１＝Ｒ１２１２＝Ｒ１２１３＝０
３．ｔｇｔ＝−１
４．ｔｇｔ＝ｔｇｔ＋１
５．ｉｆ ｔｇｔ＝＝０ ｔｈｅｎ
エッジ集合Ｅ＝Ｓ_ＥＬ ^ｍ，ＮＥ＝Ｓ_ＥＬ ^ｍの要素数，ｋ＝−１
ｅｌｓｅ ｉｆ ｔｇｔ＝＝１ ｔｈｅｎ
エッジ集合Ｅ＝Ｓ_ＥＲ ^ｍ，ＮＥ＝Ｓ_ＥＲ ^ｍのエッジ数，ｋ＝１
ｅｌｓｅ ｇｏｔｏ １３ ｅｎｄｉｆ
６．ｊ＝０
７．ｊ＝ｊ＋１
８．ｉｆ ｊ＞＝ＮＥ ｔｈｅｎ ｇｏｔｏ ４ ｅｎｄｉｆ
９．エッジ集合Ｅからｊ番目の要素であるエッジ点の位置（ｘ^ｊ，ｙ^ｊ）を取得する。
１０．式（１１４）及び（１１６）から（ａ_ｊ，ｂ_ｊ，ｃ_ｊ，ｄ_ｊ，ｅ_ｊ，ｆ_ｊ，ｇ_ｊ，ｈ_ｊ，ｍ_ｊ，ｎ_ｊ，ｏ_ｊ，ｐ_ｊ，ｑ_ｊ）を算出する。
１１．Ｒ０１０１＋＝ａ_ｊａ_ｊ，Ｒ０１０２＋＝ａ_ｊｂ_ｊ，Ｒ０１０３＋＝ａ_ｊｃ_ｊ，
Ｒ０１０４＋＝ａ_ｊｄ_ｊ，Ｒ０１０５＋＝ａ_ｊｅ_ｊ，Ｒ０１０６＋＝ａ_ｊｆ_ｊ，
Ｒ０１０７＋＝ａ_ｊｇ_ｊ，Ｒ０１０８＋＝ａ_ｊｈ_ｊ，Ｒ０１０９＋＝ａ_ｊｍ_ｊ，
Ｒ０１１０＋＝ａ_ｊｎ_ｊ，Ｒ０１１１＋＝ａ_ｊｏ_ｊ，Ｒ０１１２＋＝ａ_ｊｐ_ｊ，
Ｒ０１１３＋＝ａ_ｊｑ_ｊ，
Ｒ０２０１＋＝ｂ_ｊａ_ｊ，Ｒ０２０２＋＝ｂ_ｊｂ_ｊ，Ｒ０２０３＋＝ｂ_ｊｃ_ｊ，
Ｒ０２０４＋＝ｂ_ｊｄ_ｊ，Ｒ０２０５＋＝ｂ_ｊｅ_ｊ，Ｒ０２０６＋＝ｂ_ｊｆ_ｊ，
Ｒ０２０７＋＝ｂ_ｊｇ_ｊ，Ｒ０２０８＋＝ｂ_ｊｈ_ｊ，Ｒ０２０９＋＝ｂ_ｊｍ_ｊ，
Ｒ０２１０＋＝ｂ_ｊｎ_ｊ，Ｒ０２１１＋＝ｂ_ｊｏ_ｊ，Ｒ０２１２＋＝ｂ_ｊｐ_ｊ，
Ｒ０２１３＋＝ｂ_ｊｑ_ｊ，
Ｒ０３０１＋＝ｃ_ｊａ_ｊ，Ｒ０３０２＋＝ｃ_ｊｂ_ｊ，Ｒ０３０３＋＝ｃ_ｊｃ_ｊ，
Ｒ０３０４＋＝ｃ_ｊｄ_ｊ，Ｒ０３０５＋＝ｃ_ｊｅ_ｊ，Ｒ０３０６＋＝ｃ_ｊｆ_ｊ，
Ｒ０３０７＋＝ｃ_ｊｇ_ｊ，Ｒ０３０８＋＝ｃ_ｊｈ_ｊ，Ｒ０３０９＋＝ｃ_ｊｍ_ｊ，
Ｒ０３１０＋＝ｃ_ｊｎ_ｊ，Ｒ０３１１＋＝ｃ_ｊｏ_ｊ，Ｒ０３１２＋＝ｃ_ｊｐ_ｊ，
Ｒ０３１３＋＝ｃ_ｊｑ_ｊ，
Ｒ０４０１＋＝ｄ_ｊａ_ｊ，Ｒ０４０２＋＝ｄ_ｊｂ_ｊ，Ｒ０４０３＋＝ｄ_ｊｃ_ｊ，
Ｒ０４０４＋＝ｄ_ｊｄ_ｊ，Ｒ０４０５＋＝ｄ_ｊｅ_ｊ，Ｒ０４０６＋＝ｄ_ｊｆ_ｊ，
Ｒ０４０７＋＝ｄ_ｊｇ_ｊ，Ｒ０４０８＋＝ｄ_ｊｈ_ｊ，Ｒ０４０９＋＝ｄ_ｊｍ_ｊ，
Ｒ０４１０＋＝ｄ_ｊｎ_ｊ，Ｒ０４１１＋＝ｄ_ｊｏ_ｊ，Ｒ０４１２＋＝ｄ_ｊｐ_ｊ，
Ｒ０４１３＋＝ｄ_ｊｑ_ｊ，
Ｒ０５０１＋＝ｅ_ｊａ_ｊ，Ｒ０５０２＋＝ｅ_ｊｂ_ｊ，Ｒ０５０３＋＝ｅ_ｊｃ_ｊ，
Ｒ０５０４＋＝ｅ_ｊｄ_ｊ，Ｒ０５０５＋＝ｅ_ｊｅ_ｊ，Ｒ０５０６＋＝ｅ_ｊｆ_ｊ，
Ｒ０５０７＋＝ｅ_ｊｇ_ｊ，Ｒ０５０８＋＝ｅ_ｊｈ_ｊ，Ｒ０５０９＋＝ｅ_ｊｍ_ｊ，
Ｒ０５１０＋＝ｅ_ｊｎ_ｊ，Ｒ０５１１＋＝ｅ_ｊｏ_ｊ，Ｒ０５１２＋＝ｅ_ｊｐ_ｊ，
Ｒ０５１３＋＝ｅ_ｊｑ_ｊ，
Ｒ０６０１＋＝ｆ_ｊａ_ｊ，Ｒ０６０２＋＝ｆ_ｊｂ_ｊ，Ｒ０６０３＋＝ｆ_ｊｃ_ｊ，
Ｒ０６０４＋＝ｆ_ｊｄ_ｊ，Ｒ０６０５＋＝ｆ_ｊｅ_ｊ，Ｒ０６０６＋＝ｆ_ｊｆ_ｊ，
Ｒ０６０７＋＝ｆ_ｊｇ_ｊ，Ｒ０６０８＋＝ｆ_ｊｈ_ｊ，Ｒ０６０９＋＝ｆ_ｊｍ_ｊ，
Ｒ０６１０＋＝ｆ_ｊｎ_ｊ，Ｒ０６１１＋＝ｆ_ｊｏ_ｊ，Ｒ０６１２＋＝ｆ_ｊｐ_ｊ，
Ｒ０６１３＋＝ｆ_ｊｑ_ｊ，
Ｒ０７０１＋＝ｇ_ｊａ_ｊ，Ｒ０７０２＋＝ｇ_ｊｂ_ｊ，Ｒ０７０３＋＝ｇ_ｊｃ_ｊ，
Ｒ０７０４＋＝ｇ_ｊｄ_ｊ，Ｒ０７０５＋＝ｇ_ｊｅ_ｊ，Ｒ０７０６＋＝ｇ_ｊｆ_ｊ，
Ｒ０７０７＋＝ｇ_ｊｇ_ｊ，Ｒ０７０８＋＝ｇ_ｊｈ_ｊ，Ｒ０７０９＋＝ｇ_ｊｍ_ｊ，
Ｒ０７１０＋＝ｇ_ｊｎ_ｊ，Ｒ０７１１＋＝ｇ_ｊｏ_ｊ，Ｒ０７１２＋＝ｇ_ｊｐ_ｊ，
Ｒ０７１３＋＝ｇ_ｊｑ_ｊ，
Ｒ０８０１＋＝ｈ_ｊａ_ｊ，Ｒ０８０２＋＝ｈ_ｊｂ_ｊ，Ｒ０８０３＋＝ｈ_ｊｃ_ｊ，
Ｒ０８０４＋＝ｈ_ｊｄ_ｊ，Ｒ０８０５＋＝ｈ_ｊｅ_ｊ，Ｒ０８０６＋＝ｈ_ｊｆ_ｊ，
Ｒ０８０７＋＝ｈ_ｊｇ_ｊ，Ｒ０８０８＋＝ｈ_ｊｈ_ｊ，Ｒ０８０９＋＝ｈ_ｊｍ_ｊ，
Ｒ０８１０＋＝ｈ_ｊｎ_ｊ，Ｒ０８１１＋＝ｈ_ｊｏ_ｊ，Ｒ０８１２＋＝ｈ_ｊｐ_ｊ，
Ｒ０８１３＋＝ｈ_ｊｑ_ｊ，
Ｒ０９０１＋＝ｍ_ｊａ_ｊ，Ｒ０９０２＋＝ｍ_ｊｂ_ｊ，Ｒ０９０３＋＝ｍ_ｊｃ_ｊ，
Ｒ０９０４＋＝ｍ_ｊｄ_ｊ，Ｒ０９０５＋＝ｍ_ｊｅ_ｊ，Ｒ０９０６＋＝ｍ_ｊｆ_ｊ，
Ｒ０９０７＋＝ｍ_ｊｇ_ｊ，Ｒ０９０８＋＝ｍ_ｊｈ_ｊ，Ｒ０９０９＋＝ｍ_ｊｍ_ｊ，
Ｒ０９１０＋＝ｍ_ｊｎ_ｊ，Ｒ０９１１＋＝ｍ_ｊｏ_ｊ，Ｒ０９１２＋＝ｍ_ｊｐ_ｊ，
Ｒ０９１３＋＝ｍ_ｊｑ_ｊ，
Ｒ１００１＋＝ｎ_ｊａ_ｊ，Ｒ１００２＋＝ｎ_ｊｂ_ｊ，Ｒ１００３＋＝ｎ_ｊｃ_ｊ，
Ｒ１００４＋＝ｎ_ｊｄ_ｊ，Ｒ１００５＋＝ｎ_ｊｅ_ｊ，Ｒ１００６＋＝ｎ_ｊｆ_ｊ，
Ｒ１００７＋＝ｎ_ｊｇ_ｊ，Ｒ１００８＋＝ｎ_ｊｈ_ｊ，Ｒ１００９＋＝ｎ_ｊｍ_ｊ，
Ｒ１０１０＋＝ｎ_ｊｎ_ｊ，Ｒ１０１１＋＝ｎ_ｊｏ_ｊ，Ｒ１０１２＋＝ｎ_ｊｐ_ｊ，
Ｒ１０１３＋＝ｎ_ｊｑ_ｊ，
Ｒ１１０１＋＝ｏ_ｊａ_ｊ，Ｒ１１０２＋＝ｏ_ｊｂ_ｊ，Ｒ１１０３＋＝ｏ_ｊｃ_ｊ，
Ｒ１１０４＋＝ｏ_ｊｄ_ｊ，Ｒ１１０５＋＝ｏ_ｊｅ_ｊ，Ｒ１１０６＋＝ｏ_ｊｆ_ｊ，
Ｒ１１０７＋＝ｏ_ｊｇ_ｊ，Ｒ１１０８＋＝ｏ_ｊｈ_ｊ，Ｒ１１０９＋＝ｏ_ｊｍ_ｊ，
Ｒ１１１０＋＝ｏ_ｊｎ_ｊ，Ｒ１１１１＋＝ｏ_ｊｏ_ｊ，Ｒ１１１２＋＝ｏ_ｊｐ_ｊ，
Ｒ１１１３＋＝ｏ_ｊｑ_ｊ，
Ｒ１２０１＋＝ｐ_ｊａ_ｊ，Ｒ１２０２＋＝ｐ_ｊｂ_ｊ，Ｒ１２０３＋＝ｐ_ｊｃ_ｊ，
Ｒ１２０４＋＝ｐ_ｊｄ_ｊ，Ｒ１２０５＋＝ｐ_ｊｅ_ｊ，Ｒ１２０６＋＝ｐ_ｊｆ_ｊ，
Ｒ１２０７＋＝ｐ_ｊｇ_ｊ，Ｒ１２０８＋＝ｐ_ｊｈ_ｊ，Ｒ１２０９＋＝ｐ_ｊｍ_ｊ，
Ｒ１２１０＋＝ｐ_ｊｎ_ｊ，Ｒ１２１１＋＝ｐ_ｊｏ_ｊ，Ｒ１２１２＋＝ｐ_ｊｐ_ｊ，
Ｒ１２１３＋＝ｐ_ｊｑ_ｊ
１２．ｇｏｔｏ ７
１３．以下の連立方程式の解を数値計算により求める。
１４．解の収束性を検査する。
ｉｆ ｜Δρ^＊ _０｜＞ＴｈＲ ｏｒ ｜ΔＷ^＊ _０｜＞ＴｈＲ ｏｒ
｜ΔＡ_φ０｜＞ＴｈＲ ｏｒ ｜ΔＢ_φ０｜＞ＴｈＲ ｏｒ
｜ΔＣ_φ０｜＞ＴｈＲ ｏｒ ｜ΔＤ_φ０｜＞ＴｈＲ ｏｒ
｜ΔＡｅ_０｜＞ＴｈＲ ｏｒ ｜ΔＢｅ_０｜＞ＴｈＲ ｏｒ
｜ΔＣｅ_０｜＞ＴｈＲ ｏｒ ｜ΔＡ_θ０｜＞ＴｈＲ ｏｒ
｜ΔＢ_θ０｜＞ＴｈＲ ｏｒ ｜ΔＣ_θ０｜＞ＴｈＲ
ｔｈｅｎ
ｇｏｔｏ ２
ｅｌｓｅ
ρ^＊＝ρ^＊ _０，Ｗ^＊＝Ｗ^＊ _０，
Ａ_φ＝Ａ_φ０，Ｂ_φ＝Ｂ_φ０，Ｃ_φ＝Ｃ_φ０，Ｄ_φ＝Ｄ_φ０，
Ａｅ＝Ａｅ_０，Ｂｅ＝Ｂｅ_０，Ｃｅ＝Ｃｅ_０，
Ａ_θ＝Ａ_θ０，Ｂ_θ＝Ｂ_θ０，Ｃ_θ＝Ｃ_θ０
ｅｎｄｉｆ
１５．終了
ＴｈＲは解の収束性を評価するための微小な閾値である。

なお、オフセットｅ_ｍ及びヨー角θ_ｍに対して等速運動を仮定してもよく、ピッチ角φ_ｍに対して時間に関するより高次の項までを含む高次運動を仮定してもよい。

以上説明した道路形状パラメータの計算式（１）〜（１１９）は一例に過ぎず、別の座標系、別の拘束条件等に基づいて異なる定式化を行っても構わない。

図１９は、道路形状推定装置を用いた車線逸脱警報システムの構成例を示している。図１９の車線逸脱警報システムは、カメラ１２０１、道路形状推定装置１９０１、距離計算装置１９０２、逸脱判定装置１９０３、及び逸脱警報装置１９０４を含む。道路形状推定装置１９０１の動作は上述した通りである。

距離計算装置１９０２は、道路形状推定装置１９０１から出力される道路形状パラメータに基づいて、車両と左右の車線区分線との距離を計算する。逸脱判定装置１９０３は、計算された距離を閾値と比較して、車両が車線を逸脱したか否かを判定する。逸脱警報装置１９０４は、車両が車線を逸脱した場合に警報を出力する。これにより、運転者は、直ちに車両を操舵して車線に戻すことが可能になる。

図２０は、道路形状推定装置を用いた車両操舵システムの構成例を示している。図２０の車両操舵システムは、カメラ１２０１、道路形状推定装置１９０１、距離計算装置１９０２、ふらつき判定装置２００１、及び車両操舵装置２００２を含む。

ふらつき判定装置２００１は、距離計算装置１９０２により計算された距離を閾値と比較して、車両がふらついているか否かを判定する。車両操舵装置２００２は、車両がふらついている場合に、車線上での走行を維持するように車両を自動操舵して、車両のふらつきを解消する。これにより、車線からの逸脱を未然に防止することが可能になる。

図１０、図１２、及び図１８−図２０の道路形状推定装置は、例えば、図２１に示すような情報処理装置（コンピュータ）を用いて実現可能である。

図２１の情報処理装置は、映像入力部１２０２、Central Processing Unit （ＣＰＵ）２１０１、メモリ２１０２、出力部２１０３、外部記憶装置２１０４、媒体駆動装置２１０５、及びネットワーク接続装置２１０６を備える。これらはバス２１０７により互いに接続されている。

メモリ２１０２は、例えば、Read Only Memory（ＲＯＭ）、Random Access Memory（ＲＡＭ）、フラッシュメモリ等の半導体メモリであり、処理に用いられるプログラム及びデータを格納する。例えば、ＣＰＵ２１０１は、メモリ２１０２を利用してプログラムを実行することにより、道路形状推定装置の処理を行う。メモリ２１０２は、図１０の格納部１０１３、又は図１２及び図１８の格納部１２０５及び１２１２としても使用できる。

映像入力部１２０２は、カメラからの映像信号を取り込み、時系列な画像としてメモリ２１０２に格納する。出力部２１０３は、例えば、外部装置とのインタフェースであり、道路形状パラメータを含む処理結果の出力に用いられる。

外部記憶装置２１０４は、例えば、磁気ディスク装置、光ディスク装置、光磁気ディスク装置、テープ装置等である。この外部記憶装置２１０４には、ハードディスクドライブも含まれる。情報処理装置は、外部記憶装置２１０４にプログラム及びデータを格納しておき、それらをメモリ２１０２にロードして使用することができる。

媒体駆動装置２１０５は、可搬型記録媒体２１０８を駆動し、その記録内容にアクセスする。可搬型記録媒体２１０８は、メモリデバイス、フレキシブルディスク、光ディスク、光磁気ディスク等である。この可搬型記録媒体２１０８には、Compact Disk Read Only Memory （ＣＤ−ＲＯＭ）、Digital Versatile Disk（ＤＶＤ）、Universal Serial Bus（ＵＳＢ）メモリ等も含まれる。ユーザ又はオペレータは、この可搬型記録媒体２１０８にプログラム及びデータを格納しておき、それらをメモリ２１０２にロードして使用することができる。

このように、各種処理に用いられるプログラム及びデータを格納するコンピュータ読み取り可能な記録媒体には、メモリ２１０２、外部記憶装置２１０４、及び可搬型記録媒体２１０８のような、物理的な（非一時的な）記録媒体が含まれる。

ネットワーク接続装置２１０５は、通信ネットワークに接続され、通信に伴うデータ変換を行う通信インタフェースである。情報処理装置は、プログラム及びデータを外部の装置からネットワーク接続装置２１０５を介して受け取り、それらをメモリ２１０２にロードして使用することもできる。

なお、情報処理装置が図２１のすべての構成要素を含む必要はなく、用途や条件に応じて一部の構成要素を省略することも可能である。例えば、情報処理装置が可搬型記録媒体２１０８にアクセスしない場合は、媒体駆動装置２１０５を省略してもよく、情報処理装置が通信ネットワークに接続されない場合は、ネットワーク接続装置２１０５を省略してもよい。

開示の実施形態とその利点について詳しく説明したが、当業者は、特許請求の範囲に明確に記載した本発明の範囲から逸脱することなく、様々な変更、追加、省略をすることができるであろう。

１０１、１０２ 画像
１２１ 矩形領域
１２２ 黒色領域
３０１、３０２ 破線ブロック
３１１、３１２、３２１、３２２、車線区分線
３３１、３３２ 距離誤り
１００１、１９０１ 道路形状推定装置
１０１１、１２０３ 区分線検出部
１０１２、１２０４、１８０１ 第１のパラメータ推定部
１０１３、１２０５、１２１２ 格納部
１０１４、１２０６ 第２のパラメータ推定部
１２０１ カメラ
１２０２ 映像入力部
１２１１ 区分線情報保存部
１２１３ 区間決定部
１２１４ 曲率及び幅推定部
１５０１ 探索範囲
１６０１、１６０２ 境界エッジ線
１８１１ 区間フレームカウント部
１９０２ 距離計算装置
１９０３ 逸脱判定装置
１９０４ 逸脱警報装置
２００１ ふらつき判定装置
２００２ 車両操舵装置
２１０１ ＣＰＵ
２１０２ メモリ
２１０３ 出力部
２１０４ 外部記憶装置
２１０５ 媒体駆動装置
２１０６ ネットワーク接続装置
２１０７ バス
２１０８ 可搬型記録媒体

Claims (8)

1. 道路の路面を撮影した画像から車線区分線のエッジ情報を検出する区分線検出部と、
   前記区分線検出部が第１の時間区間内の１つ以上の時刻における画像から検出したエッジ情報に基づいて、前記道路の道路形状パラメータのうちの曲率及び幅を求める第１のパラメータ推定部と、
   前記第１のパラメータ推定部が求めた曲率及び幅を格納する格納部と、
   前記第１のパラメータ推定部が求めた曲率及び幅を第２の時間区間における曲率及び幅として用い、前記区分線検出部が該第２の時間区間における画像から検出したエッジ情報に基づいて、前記道路形状パラメータのうちの曲率及び幅以外のパラメータを求め、該曲率及び幅を含む該道路形状パラメータを出力する第２のパラメータ推定部と
   を備えることを特徴とする道路形状推定装置。
2. 前記第１のパラメータ推定部は、前記車線区分線が破線である場合、前記区分線検出部が検出したエッジ情報に基づいて、該破線の第１の切れ目に対応する第１の画像と該破線の第２の切れ目に対応する第２の画像を検出し、該第１の画像を検出した第１の時刻から該第２の画像を検出した第２の時刻までを前記第１の時間区間とみなして前記曲率及び幅を求めることを特徴とする請求項１記載の道路形状推定装置。
3. 前記第１のパラメータ推定部は、前記区分線検出部がエッジ情報を検出する度にフレームカウンタの値をインクリメントし、該フレームカウンタの初期値に対応する第１の時刻から該フレームカウンタの値が所定値を超えた第２の時刻までを前記第１の時間区間とみなして前記曲率及び幅を求めることを特徴とする請求項１記載の道路形状推定装置。
4. 前記第１のパラメータ推定部は、前記道路形状パラメータに含まれる、前記路面を撮影したカメラの横方向オフセット及びヨー角のそれぞれを等速運動又は等加速度運動で表したモデルを用いて、前記曲率及び幅を求めることを特徴とする請求項１記載の道路形状推定装置。
5. 前記第１のパラメータ推定部は、前記カメラのピッチ角を等速運動、等加速度運動、又は時間に関してより高次の項を含む高次運動で表したモデルを用いて、前記曲率及び幅を求めることを特徴とする請求項４記載の道路形状推定装置。
6. 請求項１乃至５のいずれか１項に記載の道路形状推定装置と、
   前記道路形状推定装置から出力される道路形状パラメータに基づいて、車両が車線を逸脱したか否かを判定する逸脱判定装置と、
   前記車両が前記車線を逸脱した場合に警報を出力する逸脱警報装置と
   を備えることを特徴とする車線逸脱警報システム。
7. 請求項１乃至５のいずれか１項に記載の道路形状推定装置と、
   前記道路形状推定装置から出力される道路形状パラメータに基づいて、車両がふらついているか否かを判定するふらつき判定装置と、
   前記車両がふらついている場合に、車線上での走行を維持するように該車両を操舵する車両操舵装置と
   を備えることを特徴とする車両操舵システム。
8. 第１の時間区間内の１つ以上の時刻における道路の路面を撮影した画像から車線区分線のエッジ情報を検出し、
   前記１つ以上の時刻における画像から検出したエッジ情報に基づいて、前記道路の道路形状パラメータのうちの曲率及び幅を求め、
   第２の時間区間における画像から車線区分線のエッジ情報を検出し、
   得られた曲率及び幅を前記第２の時間区間における曲率及び幅として用い、該第２の時間区間における画像から検出したエッジ情報に基づいて、前記道路形状パラメータのうちの曲率及び幅以外のパラメータを求め、
   前記曲率及び幅を含む前記道路形状パラメータを出力する
   処理をコンピュータに実行させるプログラム。