(57)【要約】【課題】走路に勾配変化がある場合であっても、精度良く走路の境界線を推定することができるようにする。【解決手段】境界線候補点抽出部１４２が、移動体に搭載された撮像装置１１０によって撮像された移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出し、走路パラメータ推定部１４４が、複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、境界線に対する移動体のオフセット、及び姿勢角とを含む状態ベクトルを境界線候補点と状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する。【選択図】図３
2019101821.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭戦された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出する境界線候補点抽出部と、
前記複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを前記境界線候補点と前記状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する走路パラメータ推定部と、
を含む走路推定装置。
【請求項２】
移動体に搭載された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出する境界線候補点抽出部と、
前記複数の境界線候補点を観測値として、カルマンフィルタにより、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを推定する走路パラメータ推定部と、
を含む走路推定装置。
【請求項３】
前記境界線の線形に関するパラメータは、水平面における前記境界線の曲率及び曲率変化率と、垂直断面における前記境界線の曲率とである請求項１又は２記載の走路推定装置。
【請求項４】
前記境界線の線形に関するパラメータは、水平面における前記境界線の曲率及び曲率変化率と、及び垂直断面における前記境界線の曲率及び曲率変化率とである請求項１又は２記載の走路推定装置。
【請求項５】
前記境界線候補点抽出部は、前記複数の境界線候補点から境界線幅を算出し、
前記状態ベクトルは、前記境界線幅を更に含む
請求項１乃至４の何れか１項記載の走路推定装置。
【請求項６】
前記状態ベクトルは、前記複数の境界線候補点の各々までの実空間上の距離のうち、最遠点である前記境界線候補点の実空間上の距離と、前記境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット及び姿勢角とを含む請求項１乃至５の何れか１項記載の走路推定装置。
【請求項７】
前記走路パラメータ推定部は、カルマンフィルタの時間更新行列として、前記境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角と、前記少なくとも１つの境界線候補点の各々までの実空間上の距離との各々に対応する要素を含む行列を用いて、前記状態ベクトルを推定する請求項２記載の走路推定装置。
【請求項８】
前記移動体の車速と、前記移動体の姿勢角とを取得する取得部を更に含み、
前記走路パラメータ推定部は、前記車速と、前記姿勢角とに対応する要素を更に含む前記時間更新行列を用いて、前記状態ベクトルを推定する請求項７記載の走路推定装置。
【請求項９】
コンピュータを、
移動体に搭戦された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出する境界線候補点抽出部、及び
前記複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを前記境界線候補点と前記状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する走路パラメータ推定部
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、走路推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、車載カメラで前方の走路の平面線形とともに、勾配変化等の縦断線形を推定する方法として、（ａ）ステレオカメラを利用して道路の三次元構造を復元する方法（特許文献１〜３）、（ｂ）単眼カメラで地図情報を利用する方法（特許文献４）、（ｃ）単眼カメラで車線幅一定の仮定を利用する方法（特許文献５）、（ｄ）単眼カメラで白線の間隔が一定の破線であることを仮定する方法（特許文献６）、（ｅ）車載単眼カメラの移動ステレオにより路面勾配変化を推定する方法（特許文献７）などが提案されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平９−５０５９２号広報
【特許文献２】特開平９−３２５０２６号広報
【特許文献３】特開２０１１−２８６５９号広報
【特許文献４】特開平１１−３９４６４号広報
【特許文献５】特開平９−４８２９９号広報
【特許文献６】特開２０１２−２５５７０３号広報
【特許文献７】特開２０１３−２３９１４３号広報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上述の方法には、以下のような問題がある。
【０００５】
上記特許文献１〜３記載の方法では、単眼カメラに比べて、カメラ数が増加し、ステレオカメラ画像間のマッチング計算に専用ハードが必要で、コストが増大する。
【０００６】
上記特許文献４記載の方法では、道路の高さを含む３次元構造を表現する地図情報が必要となり、その地図の作成コストが増大する。車両位置と地図とマッチングをとるために測位装置が必要となる。
【０００７】
上記特許文献５記載の方法では、左右の車線境界線（白線）が無いと計算できない。すなわち、片方の白線が摩耗して掠れていたり、分岐や合流箇所で片方の白線が無かったりする箇所では適用できない。
【０００８】
上記特許文献６記載の方法では、白線が実線の場合に適用できない。すなわち、実線の場合は、白線は単調な形状であるため、連続する二時刻間で白線上の点の一対一照合が困難であり、画像処理の分野で窓問題（Ａｐｅｒｔｕｒｅ  Ｐｒｏｂｌｅｍ）に陥ってしまう。そのため、単眼ステレオによる三次元復元が困難である。
【０００９】
上記特許文献７記載の方法では、２時刻間の画像のマッチング計算に専用ハードが必要で、コストが増大する。また、特に白線に関しては、画像処理の窓問題（Ａｐｅｒｔｕｒｅ  Ｐｒｏｂｌｅｍ）が発生して、２時刻の白線候補点間のマッチング点が一意に決まらず計算困難である。
【００１０】
すなわち、撮像画像に写る道路上の車線境界線（白線）が左右どちらか一方だけ単独の場合、かつ、路面勾配変化がある場合には、精度良く走路を推定することができない、という問題があった。
【００１１】
本発明は、上記問題点を解決するためになされたもので、走路に勾配変化がある場合であっても、精度良く走路の境界線を推定することができる走路推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１２】
上記の目的を達成するために、本発明の走路推定装置は、移動体に搭戦された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出する境界線候補点抽出部と、前記複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを前記境界線候補点と前記状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する走路パラメータ推定部と、を含んで構成される。
【００１３】
また、本発明のプログラムは、コンピュータを、移動体に搭戦された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出する境界線候補点抽出部、及び前記複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを前記境界線候補点と前記状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する走路パラメータ推定部として機能させるためのプログラムである。
【００１４】
本発明の走路推定装置及びプログラムによれば、境界線候補点抽出部が、移動体に搭戦された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出し、走路パラメータ推定部が、前記複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを前記境界線候補点と前記状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する。
【００１５】
このように、走路の画像から、線状に並んだ複数の境界線候補点を抽出し、複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、境界線に対する移動体のオフセット、及び姿勢角とを含む状態ベクトルを境界線候補点と状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定することにより、走路に勾配変化がある場合であっても、精度良く走路を推定することができる。
【００１６】
また、本発明の走路推定装置は、移動体に搭載された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出する境界線候補点抽出部と、前記複数の境界線候補点を観測値として、カルマンフィルタにより、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを推定する走路パラメータ推定部と、を含んで構成される。
【００１７】
本発明に係る走路推定装置によれば、境界線候補点抽出部が、移動体に搭載された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出し、走路パラメータ推定部が、前記複数の境界線候補点を観測値として、カルマンフィルタにより、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを推定する。
【００１８】
このように、走路の画像から、線状に並んだ複数の境界線候補点を抽出し、複数の境界線候補点を観測値として、カルマンフィルタにより、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、境界線に対する移動体のオフセット、及び姿勢角とを含む状態ベクトルを推定することにより、走路に勾配変化がある場合であっても、精度良く走路を推定することができる。
【００１９】
また、本発明の走路推定装置は、前記境界線の線形に関するパラメータは、水平面における前記境界線の曲率及び曲率変化率と、垂直断面における前記境界線の曲率とであるとすることができる。
【００２０】
また、本発明の走路推定装置は、前記境界線の線形に関するパラメータは、水平面における前記境界線の曲率及び曲率変化率と、及び垂直断面における前記境界線の曲率及び曲率変化率とであるとすることができる。
【００２１】
また、本発明の走路推定装置は、前記境界線候補点抽出部は、前記複数の境界線候補点から境界線幅を算出し、前記状態ベクトルは、前記境界線幅を更に含むことができる。
【００２２】
また、本発明の走路推定装置は、前記状態ベクトルは、前記複数の境界線候補点の各々までの実空間上の距離のうち、最遠点である前記境界線候補点の実空間上の距離と、前記境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット及び姿勢角とを含むことができる。
【００２３】
また、本発明の走路推定装置は、前記走路パラメータ推定部は、カルマンフィルタの時間更新行列として、前記境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角と、前記複数の境界線候補点の各々までの実空間上の距離との各々に対応する要素を含む行列を用いて、前記状態ベクトルを推定することができる。
【００２４】
また、本発明の走路推定装置は、前記移動体の車速と、前記移動体の姿勢角とを取得する取得部を更に含み、前記走路パラメータ推定部は、前記車速と、前記姿勢角とに対応する要素を更に含む前記時間更新行列を用いて、前記状態ベクトルを推定することができる。
【発明の効果】
【００２５】
以上説明したように、本発明の走路推定装置及びプログラムによれば、走路の画像から、線状に並んだ複数の境界線候補点を抽出し、複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、境界線に対する移動体のオフセット、及び姿勢角とを含む状態ベクトルを境界線候補点と状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定することにより、走路に勾配変化がある場合であっても、精度良く走路を推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２６】
【図１】従来技術と本発明の実施の形態との違いを示すイメージ図である。
【図２】本発明の実施の形態における処理の流れの概要を表す図である。
【図３】本発明の実施の形態における走路推定装置の概略構成を示すブロック図である。
【図４】本発明の第１の実施の形態における道路モデルの例を示す図である。
【図５】本発明の実施の形態における走路パラメータ推定処理ルーチンの内容を示すフローチャートである。
【図６】本発明の実施の形態における数値実験（Ａ）のシミュレーション結果を示す図である。
【図７】本発明の実施の形態における数値実験（Ｂ）のシミュレーション結果を示す図である。
【図８】本発明の第２の実施の形態における道路モデルの例を示す図である。
【発明を実施するための形態】
【００２７】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００２８】
＜本発明の実施の形態の概要＞
従来技術（特許文献７）では、単眼移動ステレオを用いて路面の三次元構造を復元して、路面の凹凸を推定する方法が提案されていた。この従来技術では、２時刻間で同一の路面特徴点を照合して、その照合結果と車速から、三角測量の原理で距離を算出し、路面の垂直輪郭を検出していた。
【００２９】
しかし、白線は単調な形状であるため、２時刻間の白線点を一対一で高精度に照合することは難しかった（図１）。そのため、個々の白線点の照合に基づく、白線の３次元構造の復元は困難だった。
【００３０】
本発明の実施の形態では、２時刻間の個々の白線点を照合せずに、白線の３次元構造（平面線形、縦断線形）を推定する。このことにより、路面勾配変化（路面の起伏）箇所での、平面曲率の誤差を低減し、走路認識に基づく自動走行を安定化させる。
【００３１】
解決のアプローチは、白線の３次元構造（平面線形、縦断線形）を滑らかな関数（３次関数）でモデル表現して、自車両の移動による画像上の白線パターン全体の形状変化を追跡することにより３次元構造（平面線形、縦断線形）を復元する。また、白線パターン全体の形状パターンを追跡するので、個々の白線点の照合は不要となる。
【００３２】
車載単眼カメラ画像による走路認識において、３次元の白線パターン全体を２時刻間で照合・追跡することにより、従来推定困難だった路面勾配変化・片側単独白線の場合でも走路推定が可能となる。
【００３３】
本実施の形態の処理の流れの概略を図２に示す。車載単眼カメラによって道路画像を撮像し、そして入力された道路画像上をスキャンして、背景（路面）から輝度が明るく変化する点（白線の稜線部）から数点の観測点（以後、白線点）を抽出する。
【００３４】
この白線点を観測値として、道路モデルのパラメータ推定値をカルマンフィルタで更新して、後段の警報や制御へ推定値を出力する。
【００３５】
また、本実施の形態における道路モデルは、パラメータとして、白線の平面線形（平面曲線曲率とその曲率変化率）、白線の縦断線形（縦断曲線曲率とその曲率変化率）、白線上の着目点までの三次元距離、および、白線とカメラ（車両）との相対位置姿勢、を含むものである。
【００３６】
当該道路モデルに、パラメータとして、白線の縦断線形と、白線上の着目点までの三次元距離とを道路モデルに含むことにより、路面勾配変化・片側単独白線の場合の曲率、ヨー角、曲率等の推定精度が改善することができる。
【００３７】
以下、車載単眼カメラ画像による走路推定へ適用する場合について説明し、実験により、路面勾配変化・片側単独白線の場合の曲率、ヨー角、曲率等の推定精度が改善できたことを説明する。なお、本実施形態では、簡単のために白線上の着目点は観測点と一致させている。
【００３８】
＜本発明の第１の実施形態に係る走路推定装置１００の構成＞
図３に示すように、本発明の第１の実施の形態に係る走路推定装置１００は、自車両の前方を撮像する撮像装置１１０と、ヨーレイトセンサ１２０と、車速センサ１３０と、撮像装置１１０によって撮像された自車両の前方の道路画像、ヨーレイトセンサ１２０によって検出された自車両のヨーレイト、及び車速センサ１３０によって検出された自車両の車速に基づいて、走路パラメータを推定するコンピュータ１４０とを備えている。
【００３９】
撮像装置１１０は、自車両の前方の道路を撮像する車載単眼カメラ画像である。撮像装置１１０は、例えば車両のリアビューミラー付近に設置され、自車両が走行する走路の画像の一例として、自車両の前方の道路画像を撮像する。
【００４０】
コンピュータ１４０は、ＣＰＵと、ＲＡＭと、後述する走路パラメータ推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【００４１】
コンピュータ１４０は、境界線候補点抽出部１４２と、走路パラメータ推定部１４４と、走路パラメータ記憶部１４６とを備えている。
【００４２】
境界線候補点抽出部１４２は、撮像装置１１０によって撮像された道路画像に基づいて、自車両が走行する走行車線の境界として、線状に並んだ複数の白線の候補点である白線点を抽出する。
【００４３】
具体的には、境界線候補点抽出部１４２は、道路画像から走行車線と白線との境界線の候補点の位置の座標値を抽出する。例えば、自車両が走行中の車線の左又は右の白線との境界線を抽出する。
【００４４】
一般には、走路には白線が敷設されているため、境界線候補点抽出部１４２は、白線点を走行車線の境界線の候補点として抽出する。白線は、道路画像上では走路面に対して輝度が高いため、境界線候補点抽出部１４２は、例えば、道路画像を既知のＳｏｂｅｌフィルタ等によって処理し、得られた処理結果が閾値よりも大きい点を走行車線の白線点として抽出する。なお、白線点は、走行車線と隣接する車線と白線との境界線の候補点として抽出されてもよい。
【００４５】
さらに、白線は、局所的には進行方向に直線状に連続しているため、Ｈｏｕｇｈ変換等の手法により、直線状に連続している箇所を、車線の白線点として絞り込んでもよい。
【００４６】
そして、境界線候補点抽出部１４２は、抽出した複数の白線点の道路画像上の座標値を算出し、走路パラメータ推定部１４４に渡す。
【００４７】
走路パラメータ推定部１４４は、境界線候補点抽出部１４２により抽出した複数の白線点の座標に基づいて、走路パラメータを、境界線候補点と状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する。例えば、走路パラメータ推定部１４４は、カルマンフィルタを用いて、当該状態ベクトルを推定する。
【００４８】
ここで、走路パラメータを、カルマンフィルタを用いて推定する原理について説明する。
【００４９】
まず、本実施の形態における道路モデルについて説明する。
【００５０】
道路上の白線の平面線形と縦断線形は、カメラ位置を原点として三次元直交座標系ＸＹＺ上の曲線として表現する（図４）。
【００５１】
白線の平面線形は、下記式（１）で表す。
【００５２】
【数１】
2019101821.tif 000003
【００５３】
また、白線の縦断線形は、下記式（２）で表す。
【００５４】
【数２】
2019101821.tif 000004
【００５５】
ここで、ｅは白線のオフセット［ｍ］、ｃ０は道路の曲率［１／ｍ］、ｃ１は道路の曲率変化率（
2019101821.tif 000005
）［１／ｍ２］、ｃｖ０は道路の縦断曲率［１／ｍ］、ｃｖ１は道路の縦断曲率の変化率（
2019101821.tif 000006
）［１／ｍ２］、ｈｃは路面平面からのカメラの高さ［ｍ］、ｘｊはｊ番目の白線点（観測値）のＸ座標値［画素］、ｙｊはｊ番目の白線点（観測値）のＹ座標値［画素］、ｚｊはｊ番目の白線点（観測値）のＺ座標値（距離）［画素］である。
【００５６】
次に、下記式（３）〜（５）から、ヨー角θ（単位は［ｒａｄ］）による回転座標変換後の白線点座標値を得る。
【００５７】
【数３】
2019101821.tif 000007
【００５８】
次に、下記式（６）〜（８）から、ピッチ角φ（単位は［ｒａｄ］）による回転座標変換後の白線点座標値を得る。
【００５９】
【数４】
2019101821.tif 000008
【００６０】
そして、下記式（９）及び（１０）で表す射影変換により、画像上のｊ番目の白線点座標値（ｉｘｊ，ｉｙｊ）が計算できる。
【００６１】
【数５】
2019101821.tif 000009
【００６２】
このとき、ｉｘｊは、画像上のｊ番目の白線点の水平方向の座標値［画素］、ｉｙｊは、画像上のｊ番目の白線点の垂直方向の座標値［画素］である。
【００６３】
また、ｆは焦点距離［ｍ］、ｉｘｃは画像中心の水平方向の座標値［画素］、ｉｙｃは画像中心の垂直方向の座標値［画素］、ｒｈは画像の水平方向の解像度［ｍ／画素］、ｒｖは画像の垂直方向の解像度［ｍ／画素］である。
【００６４】
上記をまとめると、ｊ番目の白線点に対して、下記式（１１）の観測関数が得られる。
【００６５】
【数６】
2019101821.tif 000010
【００６６】
そして、観測するｍ個の白線点をまとめた観測関数ｈは、下記式（１２）で表される。
【００６７】
【数７】
2019101821.tif 000011
【００６８】
以上のカルマンフィルタの推定パラメータを要素とした状態ベクトルｘを、下記式（１３）に示す。
【００６９】
【数８】
2019101821.tif 000012
【００７０】
このとき、Ｔは転置記号である。
【００７１】
このように、本実施の形態に係る状態ベクトルｘは、パラメータとして、白線の平面線形に関するパラメータ（平面曲線曲率ｃ０とその曲率変化率ｃ１）、白線の縦断線形に関するパラメータ（縦断曲線曲率ｃｖ０とその曲率変化率ｃｖ１）、白線点までの三次元距離ｚ０，・・・，ｚｍ−１を含む。
【００７２】
次に、この状態ベクトルｘの時刻ｔ−１からｔへの離散時間更新モデルを下記式（１４）〜（１６）に示す。
【００７３】
【数９】
2019101821.tif 000013
【００７４】
このとき、Δｔは時刻ｔ−１から時刻ｔまでの更新時間［ｓ］、Δｚは更新時間Δｔ内の移動距離（＝車速×Δｔ）［ｍ］、ωはヨーレイト［ｒａｄ／ｓ］、ｕはシステムノイズ、Ｆは状態ベクトルｘの時間更新行列、Ｂはヨーレイトωから状態ベクトルｘへの寄与項である。
【００７５】
次に、カルマンフィルタの更新式を示す。カルマンフィルタはダイナミクスによる時間更新と、白線点の観測による観測更新の２ステップを更新時間毎に交互に実行する。
【００７６】
時間更新について、下記式（１７）及び（１８）に示す。
【００７７】
【数１０】
2019101821.tif 000014
【００７８】
観測更新について、下記式（１９）〜式（２２）に示す。
【００７９】
【数１１】
2019101821.tif 000015
【００８０】
このとき、Ｐは誤差共分散行列、Ｈは観測ヤコビアン行列、Ｒは観測ノイズ分散行列、Ｑはシステムノイズ共分散行列（Ｑ＝Ｅ［ｕｕＴ］，Ｋはカルマンゲイン、ｙは観測値ベクトルである。
【００８１】
ここで、観測値ベクトルｙは、道路画像から境界線候補点抽出部１４２により抽出されたｍ個の白線点の座標値（ｉｘｊ，ｉｙｊ）を、順に並べたベクトルである（下記式（２３））。
【００８２】
【数１２】
2019101821.tif 000016
【００８３】
また、観測ヤコビアン行列Ｈは、下記式（２４）のように、０，・・・，ｍ−１番目の観測関数ｈｉを状態ベクトルｘで偏微分した観測ヤコビアン行列Ｈｊを縦に積み重ねた形となる。
【００８４】
【数１３】
2019101821.tif 000017
【００８５】
このとき、ｊ番目の白線点の観測ヤコビアン行列Ｈｊは、下記式（２５）で表される。
【００８６】
【数１４】
2019101821.tif 000018
【００８７】
以上説明した原理により、走路パラメータ推定部１４４は、抽出した複数の白線点の座標である観測値ベクトルｙに基づいて、走路パラメータを含む状態ベクトルｘを推定する。
【００８８】
また、走路パラメータ推定部１４４は、状態ベクトルｘについて、曲率変化率ｃｖ１を含まないものとすることができる。この場合、上記の各行列（Ｆ、Ｂ、Ｈ等）から曲率変化率ｃｖ１に対応する要素を除くように構成すればよい。白線の縦断線形に関するパラメータ（縦断曲線曲率ｃｖ０）を考慮しつつ、曲率変化率ｃｖ１を含めた場合よりも計算量を抑えることができるためである。
【００８９】
また、走路パラメータ推定部１４４は、白線点までの実空間上の距離のパラメータを、最遠点の白線点に限定する構成としてもよい。この場合、状態ベクトルｘについて、最遠点以外の白線点までの実空間上の距離のパラメータを含まないものとし、上記の各行列（Ｆ、Ｂ、Ｈ等）から最遠点以外の白線点までの実空間上の距離のパラメータに対応する要素を除くように構成すればよい。最遠点の境界線候補点の有無が推定精度に最も寄与するものであるため、最遠点の境界線候補点以外の境界候補点については、走路パラメータの推定に用いない構成とすることができる。このような構成により、走路パラメータの推定精度を維持しつつ、状態ベクトルｘのパラメータ数と、観測値数とを低減することができ、計算量の低減を図ることができるからである。
【００９０】
最遠点の白線点に限定する構成とする場合、例えば、状態ベクトルｘを示す上記式（１３）において、白線点までの三次元距離ｚ０，・・・，ｚｍ−１のうち、最も遠い三次元距離（例えば、ｚｍ−１）のみを用い、他の三次元距離（ｚ０，・・・，ｚｍ−２）については、状態ベクトルｘに含めないものとする。この場合、上記の各行列（Ｆ、Ｂ、Ｈ等）から他の三次元距離（ｚ０，・・・，ｚｍ−２）に対応する要素を除くように構成する。また、この場合、上記式（２３）において、観測値ベクトルｙは、ｉｘｍ−１と、ｉｙｍ−１とを要素とする。
【００９１】
具体的には、走路パラメータ推定部１４４は、境界線候補点抽出部１４２により抽出された走行車線の境界の位置（道路画像上の走行車線の白線点の座標値）に基づいて、カルマンフィルタにより走路パラメータの推定処理を行う。
【００９２】
カルマンフィルタによる推定処理は、予測ステップとフィルタリングステップとを含んで構成される。以下、走路パラメータ推定部１４４によって実行されるカルマンフィルタによる予測ステップとフィルタリングステップとについて説明する。
【００９３】
（１）予測ステップ
まず、予測ステップにおける処理について説明する。走路パラメータ推定部１４４は、予測ステップにおいて、走路パラメータ記憶部１４６に記憶されている前回のフィルタリングステップで算出された時刻ｔ−１の走路パラメータｘｔ−１と、上記式（１６）に示す時間更新行列Ｆとに基づいて、上記式（１７）に従って、時刻ｔの走路パラメータｘｔ，ｔ−１を算出する。
【００９４】
そして、走路パラメータ推定部１４４は、上記式（１６）に示す時間更新行列Ｆと、走路パラメータ記憶部１４６に記憶されている前回のフィルタリングステップで推定された誤差共分散行列Ｐｔ−１と、システムノイズ共分散行列Ｑとに基づいて、上記式（１８）に従って、誤差共分散行列Ｐｔ，ｔ−１を算出する。
【００９５】
（２）フィルタリングステップ
次に、フィルタリングステップにおける処理について説明する。走路パラメータ推定部１４４は、フィルタリングステップにおいて、予測ステップで算出された誤差共分散行列Ｐｔ，ｔ−１と、観測ヤコビアン行列Ｈと、観測ノイズ分散行列Ｒとに基づいて、上記式（２１）に従って、カルマンゲインＫを算出する。
【００９６】
走路パラメータ推定部１４４は、算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルｘｔ，ｔ−１と、境界線候補点抽出部１４２により算出された白線点の座標値を用いて得られる観測値ベクトルｙとに基づいて、上記式（１９）に従って、時刻ｔにおける走路パラメータｘｔを推定する。
【００９７】
そして、走路パラメータ推定部１４４は、予測ステップで推定された誤差共分散行列Ｐｔ，ｔ−１と、算出されたカルマンゲインＫと、上記式（２２）に示す観測ヤコビアン行列Ｈとに基づいて、上記式（２０）に従って、誤差共分散行列Ｐｔを算出する。
【００９８】
走路パラメータ推定部１４４は、フィルタリングステップで算出された各値（カルマンゲインＫ、走路パラメータｘｔ、誤差共分散行列Ｐｔ）を走路パラメータ記憶部１４６へ渡す。当該各値は、次回の予測ステップにおける処理で用いられる。
【００９９】
また、走路パラメータ推定部１４４は、推定された走路パラメータを、警報装置１５０及び車両制御装置１６０へ出力する。
【０１００】
走路パラメータ記憶部１４６は、走路パラメータ推定部１４４で算出された各値（カルマンゲインＫ、走路パラメータｘｔ、誤差共分散行列Ｐｔ）を記憶する。
【０１０１】
警報装置１５０は、走路パラメータに基づいて、自車両のドライバーに対し警報を出力する。例えば、走路パラメータに含まれる自車両のオフセット（横位置）と、自車両のヨー角に基づいて、車線逸脱を示す警報を出力する。
【０１０２】
車両制御装置１６０は、走路パラメータの各々に基づいて、運転支援や自動運転を行う。
【０１０３】
＜本発明の第１の実施の形態に係る走路推定装置１００の動作＞
次に、本実施形態に係る走路推定装置１００の動作について説明する。まず、自車両が走行し、撮像装置１１０によって自車両の前方が逐次撮像され、ヨーレイトセンサ１２０によって自車両のヨーレイトが逐次検出され、車速センサ１３０によって自車両の車速が逐次検出されているときに、コンピュータ１４０において、図５に示す走路パラメータ推定処理ルーチンが実行される。
【０１０４】
まず、ステップＳ１００において、コンピュータ１４０は、撮像装置１１０から道路画像と、ヨーレイトセンサ１２０からヨーレイトと、車速センサ１３０から車速とを取得する。
【０１０５】
次に、ステップＳ１１０において、境界線候補点抽出部１４２は、撮像装置１１０によって撮像された道路画像に基づいて、線状に並んだ複数の白線の候補点である白線点を抽出する。
【０１０６】
ステップＳ１２０において、境界線候補点抽出部１４２は、白線点の道路画像上の座標値を算出する。
【０１０７】
ステップＳ１３０において、境界線候補点抽出部１４２は、全ての白線点について座標値を算出したか否かを判定する。
【０１０８】
全ての白線点について座標値を算出していない場合（ステップＳ１３０のＮＯ）、ステップＳ１２０に戻り、次の白線点を選択して座標値を算出する。
【０１０９】
全ての白線点について座標値を算出した場合（ステップＳ１３０のＹＥＳ）、ステップＳ１４０において、走路パラメータ推定部１４４は、カルマンフィルタの予測ステップにおいて、走路パラメータ記憶部１４６に記憶されている前回のフィルタリングステップで算出された時刻ｔ−１の走路パラメータｘｔ−１と、上記式（１６）に示す時間更新行列Ｆとに基づいて、上記式（１７）に従って、時刻ｔの走路パラメータｘｔ，ｔ−１を算出する。
【０１１０】
ステップＳ１５０において、走路パラメータ推定部１４４は、上記式（１６）に示す時間更新行列Ｆと、走路パラメータ記憶部１４６に記憶されている前回のフィルタリングステップで推定された誤差共分散行列Ｐｔ−１と、システムノイズ共分散行列Ｑとに基づいて、上記式（１８）に従って、誤差共分散行列Ｐｔ，ｔ−１を算出する。
【０１１１】
ステップＳ１６０において、走路パラメータ推定部１４４は、フィルタリングステップにおいて、予測ステップで算出された誤差共分散行列Ｐｔ，ｔ−１と、観測ヤコビアン行列Ｈと、観測ノイズ分散行列Ｒとに基づいて、上記式（２１）に従って、カルマンゲインＫを算出する。
【０１１２】
ステップＳ１７０において、走路パラメータ推定部１４４は、算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルｘｔ，ｔ−１と、上記ステップＳ１２０で算出された全ての白線点の座標値を用いて得られる観測値ベクトルｙとに基づいて、上記式（１９）に従って、時刻ｔにおける走路パラメータｘｔを推定する。
【０１１３】
ステップＳ１８０において、走路パラメータ推定部１４４は、予測ステップで推定された誤差共分散行列Ｐｔ，ｔ−１と、算出されたカルマンゲインＫと、上記式（２２）に示す観測ヤコビアン行列Ｈとに基づいて、上記式（２０）に従って、誤差共分散行列Ｐｔを算出する。
【０１１４】
ステップＳ１９０において、走路パラメータ推定部１４４は、フィルタリングステップで算出された各値（カルマンゲインＫ、走路パラメータｘｔ、誤差共分散行列Ｐｔ）を走路パラメータ記憶部１４６へ渡すと共に、推定された走路パラメータを、警報装置１５０及び車両制御装置１６０へ出力して、ステップＳ１００に戻る。
【０１１５】
＜本発明の第１の実施の形態に係る実験結果＞
次に、本実施の形態に係る手法を確認した数値実験について説明する。
【０１１６】
＜＜数値実験の内容＞＞
数値実験は、（Ａ）４５０ｍＲの縦断曲線半径の凹型・凸型縦断線形が連続する直線路、（Ｂ）平坦路面で４６０ｍＲのＳ字カーブ路、のそれぞれについて、本実施の形態に係る手法と、従来法の動作とを比較して行った。
【０１１７】
本数値実験は、認識距離内の時々刻々に観測される白線点座標値をシミュレーションし、カルマンフィルタで推定値を更新した。また、従来法は、平坦路面を仮定して、ピッチ角の推定値を０に固定して推定した。
【０１１８】
観測値は、前方道路画像までの右側白線から５点を抽出した。また、オフセットは０．７５ｍ、ヨー角とピッチ角は０ｒａｄに設定した。
【０１１９】
＜＜数値実験の結果＞＞
図６は、数値実験（Ａ）のシミュレーション結果を、図７は、数値実験（Ｂ）のシミュレーション結果を示した図である。
【０１２０】
図６（ａ）及び図７（ａ）が、曲率についての従来法の推定値（破線）と、本実施の形態に係る手法の推定値（実線）と、真値（点線）とをプロットしたものである。図６（ｂ）及び図７（ｂ）が、縦断曲率についての従来法の推定値（破線）と、本実施の形態に係る手法の推定値（実線）と、真値（点線）とをプロットしたものである。図６（ｃ）及び図７（ｃ）が、オフセットについての従来法の推定値（破線）と、本実施の形態に係る手法の推定値（実線）と、真値（点線）とをプロットしたものである。図６（ｄ）及び図７（ｄ）が、ヨー角についての従来法の推定値（破線）と、本実施の形態に係る手法の推定値（実線）と、真値（点線）とをプロットしたものである。
【０１２１】
図６より、路面勾配変化がある場合、本実施の形態に係る手法により縦断曲率と白線点までの距離が推定されて、従来法に比べて推定パラメータの誤差が低減されていることが分かる。
【０１２２】
また、図７より、平坦路面のカーブであっても本実施の形態に係る手法には副作用は無く、推定ができていることが確認された。
【０１２３】
以上説明したように、本発明の実施の形態に係る走路推定装置によれば、走路の画像から、線状に並んだ複数の境界線候補点を抽出し、複数の境界線候補点を観測値として、カルマンフィルタにより、境界線の平面線形に関するパラメータと境界線の断面線形に関するパラメータと少なくとも１つの境界線候補点の各々までの実空間上の距離とを含む状態ベクトルを推定することにより、走路に勾配変化がある場合であっても、精度良く走路の境界線を推定することができる。
【０１２４】
＜本発明の第２の実施の形態に係る走路推定装置１００の構成＞
次に、本発明の第２の実施の形態に係る走路推定装置１００の構成について説明する。単独白線の白線幅が画像上で観測できる場合には、白線幅は距離によって画像上の幅が変化して見えるため、距離推定の補助的な情報として有用である。そこで、本実施形態では、状態ベクトルに白線幅を更に含んで走路パラメータを推定する。以下、本実施形態において、上記の実施形態と異なる構成に係る部分について説明する。
【０１２５】
本実施形態に係る境界線候補点抽出部１４２は、撮像装置１１０によって撮像された道路画像に基づいて、自車両が走行する走行車線の境界として、２本の線状に並んだ複数の白線点を抽出し、白線幅を抽出する。
【０１２６】
具体的には、本実施形態に係る境界線候補点抽出部１４２は、２本のうちの一方として線状に並んだ複数の白線点の位置の座標値と、２本のうち一方として線状に並んだ複数の白線点の位置の座標値とから、白線の幅を算出する。例えば、２本のうち一方として線状に並んだ複数の白線点と、２本のうちの他方として線状に並んだ複数の白線点との間で、距離が近い白線点の組合せを抽出し、当該組合せから、当該白線点での画像上の白線幅を算出する。
【０１２７】
そして、本実施形態に係る境界線候補点抽出部１４２は、２本のうち一方として線状に並んだ複数の白線点の位置の座標値と、複数の白線点の各々について算出した画像上の白線幅とを、走路パラメータ推定部１４４に渡す。
【０１２８】
本実施形態に係る走路パラメータ推定部１４４は、白線幅を更に追加した状態ベクトルを推定する。
【０１２９】
本実施形態における道路モデルについて説明する。
【０１３０】
道路上の白線の平面線形と縦断線形は、カメラ位置を原点として三次元直交座標系ＸＹＺ上の曲線として表現する（図８）。
【０１３１】
ここで、図８に示すｊ番目の白線点に対応する画面上の白線幅ｉｗｊは、下記式（２６）で表すことができる。
【０１３２】
【数１５】
2019101821.tif 000019
【０１３３】
ここで、ｗｌｍは走行平面上の白線幅［ｍ］、ｉｗｊは画像上のｊ番目の白線幅（観測値）［画素］、αｊは下記式（２７）で表せられるｊ番目の白線点における接線の傾きである。
【０１３４】
【数１６】
2019101821.tif 000020
【０１３５】
白線幅パラメータｗｌｍを含む状態ベクトルｘは、上記式（１３）を変形した下記式（２８）となる。
【０１３６】
【数１７】
2019101821.tif 000021
【０１３７】
このとき、離散時間更新モデルの行列ＢとＦとは、下記式（２９）及び式（３０）となる。
【０１３８】
【数１８】
2019101821.tif 000022
【０１３９】
また、ｊ番目の白線点に対して、下記式（３１）の観測関数ｈｊ（ｘ）が得られる。
【０１４０】
【数１９】
2019101821.tif 000023
【０１４１】
また、観測値ベクトルｙは、境界線候補点抽出部１４２で抽出された、２本のうちの一方として線状に並んだｍ個の白線点の座標値（ｉｘｊ，ｉｙｊ）と対応する白線幅ｉｗｊを順に並べたベクトルであり、下記式（３２）で表される。
【０１４２】
【数２０】
2019101821.tif 000024
【０１４３】
また、ｊ番目の白線点に対する観測ヤコビアン行列Ｈｊは、下記式（３３）のように観測関数ｈｊ（ｘ）を、状態ベクトルｘで偏微分した形となる。
【０１４４】
【数２１】
2019101821.tif 000025
【０１４５】
そして、本実施形態に係る走路パラメータ推定部１４４は、以上説明した走路モデルにより、抽出した複数の白線点の座標である観測値ベクトルｙに基づいて、走路パラメータを含む状態ベクトルｘを推定する。
【０１４６】
以上説明したように、本実施の形態における実施形態に係る走路推定装置によれば、境界線幅を追加した状態ベクトルを推定することにより、直線路とカーブ路の間に緩和曲線が無い場合のように、急峻に曲率が変化するカーブ路において、応答遅れやオーバーシュートを低減することができる。
【０１４７】
＜本発明の第３の実施の形態に係る走路推定装置１００の構成＞
次に、本発明の第３の実施の形態に係る走路推定装置１００の構成について説明する。第２の実施形態においても、第１の実施形態と同様に、走路パラメータ推定部１４４は、状態ベクトルｘについて、曲率変化率ｃｖ１を含まないものとして、計算量を削減することができる。さらに、走路パラメータ推定部１４４は、白線点までの実空間上のパラメータを最遠点の白線点に限定する構成として、計算量を削減することができる。
【０１４８】
そこで、本実施形態では、第２の実施形態において、曲率変化率ｃｖ１を含まず、かつ、白線点までの実空間上のパラメータを最遠点の白線点に限定する場合について説明する。なお、本実施形態において、第２の実施形態と異なる構成に係る部分について説明する。
【０１４９】
走路パラメータ推定部１４４は、状態ベクトルｘを、下記式（３４）で表す。
【０１５０】
【数２２】
2019101821.tif 000026
【０１５１】
このとき、縦断線形は、下記式（３５）に示す２次式で表現することができる。
【０１５２】
【数２３】
2019101821.tif 000027
【０１５３】
また、本実施形態では、観測ベクトルｙを、下記式（３６）に示す白線点のｉｘ座標を５個、ｉｙ座標を１個、及び白線幅ｉｗを２個を順に並べたベクトルを用いることとする。
【０１５４】
【数２４】
2019101821.tif 000028
【０１５５】
このとき、観測行列Ｈは、下記式（３７）のように示すことができる。
【０１５６】
【数２５】
2019101821.tif 000029
【０１５７】
なお、推定パラメータから外したｚ０〜ｚ３は、観測した白線点のｉｙ座標から、以下の二次方程式の解として求めて、観測行列計算で使用する。
【０１５８】
【数２６】
2019101821.tif 000030
【０１５９】
＜本発明の第３の実施の形態に係る実験結果＞
本実施形態に係るシミュレーションを行うことにより確認したところ、状態ベクトルｘの次元数を上記式（３４）のように少なくとも８とし、観測ベクトルｙの次元数を上記式（３６）のように少なくとも８とすることにより、第１の実施の形態に係る実験結果の性能を極端に劣化させることなく、かつ、計算量を低減させることができた。
【０１６０】
以上説明したように、本実施の形態における実施形態に係る走路推定装置によれば、走路に勾配変化がある場合であっても、精度良く、かつ、高速に走路の境界線を推定することができる。
【０１６１】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【０１６２】
例えば、上記の実施の形態では、移動体として車両を対象とする場合を例に説明したが、これに限定されるものではなく、他の移動体を対象としてもよい。
【０１６３】
また、走路の境界線が白線である場合を例に説明したが、これに限定されるものではなく、橙線などの他の色で引かれた境界線であっても良い。
【０１６４】
また、走路の境界線としての白線が片側にのみ１つ存在する場合を例に説明したが、これに限定されるものではなく、走路の境界線としての白線が両側にある場合であってもよい。この場合、白線毎に、カルマンフィルタによる推定を行って、走路パラメータを推定すればよい。
【０１６５】
また、走路の境界線が曲線がある場合について説明したが、走路の境界線が直線である場合でも、本発明を適用することが可能である。この場合、走路パラメータ推定部１４４は、直線に近似できる近傍の境界線から、走路の境界線を推定することができる。
【０１６６】
また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。
【符号の説明】
【０１６７】
１００ 走路推定装置
１１０ 撮像装置
１２０ ヨーレイトセンサ
１３０ 車速センサ
１４０ コンピュータ
１４２ 境界線候補点抽出部
１４４ 走路パラメータ推定部
１４６ 走路パラメータ記憶部
１５０ 警報装置
１６０ 車両制御装置
【図１】
2019101821.tif 000031
【図２】
2019101821.tif 000032
【図３】
2019101821.tif 000033
【図４】
2019101821.tif 000034
【図５】
2019101821.tif 000035
【図６】
2019101821.tif 000036
【図７】
2019101821.tif 000037
【図８】
2019101821.tif 000038