道路勾配推定装置及びプログラム
【課題】先行車や車線境界線等の特定物体が存在しない場合でも、前方道路の勾配を精度良く推定する。
【解決手段】路面反射点抽出部２２で、レーザレーダ１２の観測データから路面反射点を抽出し、第１の立体物候補抽出部２４で、残りの点群から第１の立体物候補を抽出する。また、第２の立体物候補抽出部２６で、撮像画像から垂直エッジの検出またはパターン認識により第２の立体物候補を抽出し、路面接地点算出部２８で、撮像画像上の第２の立体物候補から路面接地点を検出し、その第２の立体物候補に対応する第１の立体物候補の距離情報を用いて、路面接地点の３次元位置を算出する。道路勾配推定部３０で、路面接地点算出部２８で算出された路面接地点の３次元位置、及び路面反射点抽出部２２で抽出された路面反射点の３次元位置を、自車両を中心とする３次元座標空間にプロットして、路面モデルをフィッティングして道路形状を推定する。
【解決手段】路面反射点抽出部２２で、レーザレーダ１２の観測データから路面反射点を抽出し、第１の立体物候補抽出部２４で、残りの点群から第１の立体物候補を抽出する。また、第２の立体物候補抽出部２６で、撮像画像から垂直エッジの検出またはパターン認識により第２の立体物候補を抽出し、路面接地点算出部２８で、撮像画像上の第２の立体物候補から路面接地点を検出し、その第２の立体物候補に対応する第１の立体物候補の距離情報を用いて、路面接地点の３次元位置を算出する。道路勾配推定部３０で、路面接地点算出部２８で算出された路面接地点の３次元位置、及び路面反射点抽出部２２で抽出された路面反射点の３次元位置を、自車両を中心とする３次元座標空間にプロットして、路面モデルをフィッティングして道路形状を推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、道路勾配推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来、走査制御手段により、先行車の反射光の受光位置に追従して自車の測距用のレーザレーダのレーザの高さ方向の走査位置を可変し、車間距離演算手段により、レーザの照射と反射光の受光との時間差から自車と先行車との車間距離を計測し、起点検出手段により、高さ方向の走査位置が設定値以上変化した地点より計測車間距離前方の勾配起点を検出し、自車が勾配起点に到達したときに、勾配演算手段により、自車の走行距離と走査変化量検出手段が検出した高さ方向の走査位置の変化量の検出値とから、前方の路面の相対的な勾配を算出して推定する路面勾配推定装置が提案されている（例えば、特許文献１参照）。
【０００３】
また、映像カメラから前方の路面映像を受信し、路面映像から車線境界線を抽出し、抽出された車線境界線に基づいてレーダーの照射角度を調節し、調節された照射角度で照射されたレーダービームの反射波を利用して前方車両との距離を計算することにより、道路に屈曲のある場合でも前方車両との距離を検出する車間距離測定装置が提案されている（例えば、特許文献２参照）。
【０００４】
また、ジャイロセンサを用いて、車両の走行中のピッチ角を検出し、車両の前後方向に位置を異ならせた複数箇所で、車両の車体と路面との距離を検出することによって、車両傾き角を検出し、ピッチ角と車両傾き角との偏差に基づいて、走行中の道路の道路勾配を検出する道路勾配計測装置が提案されている（例えば、特許文献３参照）。特許文献３の道路勾配計測装置において、ピッチ角は、車両傾き角および道路勾配のみによって決定されるため、ジャイロセンサを用いて精度良くピッチ角を検出し、車両の前後で路面との距離を検出して車両傾き角を求めることで、ピッチ角と車両傾き角との偏差から、道路勾配を算出している。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００４−３１７３２３号公報
【特許文献２】特開２００４−１５１０８０号公報
【特許文献３】特開２００９−２７６１０９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１の技術では、先行車のリフレクタが検知されるスキャンラインの変化を利用するため、先行車が存在しない状況では、道路勾配を推定することができない、という問題がある。また、路面の凹凸などによって自車両にピッチ変動が生じた場合に、勾配起点の検出が困難になる場合もある、という問題がある。
【０００７】
また、特許文献２の技術では、車線境界線が整備されていない道路や、汚れや磨耗により車線境界線の検出が安定して行えない場合には、道路勾配を推定することが困難である、という問題がある。
【０００８】
また、特許文献３の技術では、車両直下の路面が計測対象となっており、車両が通過した道路の勾配を高精度に推定することはできるが、これから走行する前方道路の勾配を推定することはできない、という問題がある。
【０００９】
本発明は、上記の問題点を解決するためになされたもので、先行車や車線境界線等の特定物体が存在しない場合でも、前方道路の勾配を精度良く推定することができる道路勾配推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記目的を達成するために、本発明の道路勾配推定装置は、移動体周辺の複数の点各々の前記移動体を基準とする３次元位置、及び前記移動体周辺を該移動体に搭載された撮像手段により撮像した画像を取得する取得手段と、前記取得手段により取得された前記複数の点各々の３次元位置に基づいて、立体物を示す点を第１の立体物候補として抽出する第１の抽出手段と、前記取得手段により取得された画像の垂直エッジ情報及び予め定めた形状とのマッチング情報の少なくとも一方に基づいて、第２の立体物候補を抽出する第２の抽出手段と、前記第２の立体物候補に対応する前記第１の立体物候補の３次元位置に基づいて、該第２の立体物候補の路面との接地点の３次元位置を算出する算出手段と、前記算出手段により算出された接地点の３次元位置に基づいて、前記移動体周辺の道路勾配を推定する勾配推定手段と、を含んで構成されている。
【００１１】
本発明の道路勾配推定装置によれば、取得手段が、移動体周辺の複数の点各々の移動体を基準とする３次元位置、及び移動体周辺を該移動体に搭載された撮像手段により撮像した画像を取得する。そして、第１の抽出手段が、取得手段により取得された複数の点各々の３次元位置に基づいて、立体物を示す点を第１の立体物候補として抽出する。第１の抽出手段は、各点の３次元位置に基づいて立体物候補を抽出するため、移動体と立体物候補との距離について、精度の高い距離情報を有することができる。
【００１２】
また、第２の抽出手段が、取得手段により取得された画像の垂直エッジ情報及び予め定めた形状とのマッチング情報の少なくとも一方に基づいて、第２の立体物候補を抽出する。第２の抽出手段は、撮像手段により撮像された画像から立体物候補を抽出するため、画像上で立体物候補と路面との接地点を精度良く検出することができる。
【００１３】
そして、算出手段が、第２の立体物候補に対応する第１の立体物候補の３次元位置に基づいて、該第２の立体物候補の路面との接地点の３次元位置を算出し、勾配推定手段が、算出手段により算出された接地点の３次元位置に基づいて、移動体周辺の道路勾配を推定する。
【００１４】
このように、撮像手段により撮像された画像から検出した立体物候補と路面との接地点と、立体物候補までの精度の高い距離情報とに基づいて、接地点の３次元位置を算出するため、この接地点の３次元位置を用いて、先行車や車線境界線等の特定物体が存在しない場合でも、前方道路の勾配を精度良く推定することができる。
【００１５】
また、前記算出手段は、前記第２の立体物候補から求まる３次元座標空間の原点から前記接地点へ向かう方向、及び前記対応する第１の立体物候補の３次元位置に基づいて、前記接地点の３次元位置を算出することができる。
【００１６】
また、本発明の道路勾配推定装置は、所定期間の前記移動体の運動量に基づいて、過去に算出された前記接地点の３次元位置を現在の座標系に変換する変換手段を含んで構成することができ、前記勾配推定手段は、前記変換手段により変換された接地点の３次元位置も加えた分布に基づいて、前記道路勾配を推定することができる。これにより、道路勾配の推定に用いる観測点を増加させることができるため、推定精度を向上させることができる。
【００１７】
また、前記算出手段は、現在算出された接地点の３次元位置と過去に算出された接地点の３次元位置との照合結果に基づく信頼度を付与して、該現在算出された接地点の３次元位置を所定の記憶領域に記憶し、前記勾配推定手段は、前記所定の記憶領域に記憶された接地点の３次元位置のうち、前記信頼度が所定値以上の接地点の３次元位置を前記変換手段により変換した３次元位置を加えるか、または、前記所定の記憶領域に記憶された接地点の３次元位置を前記変換手段により変換して、前記信頼度に応じた重み付けをして加えることができる。このように、信頼度の高い過去の接地点の３次元位置を用いるため、または信頼度に応じた重み付けで過去の接地点の３次元位置を用いるため、より推定精度を向上させることができる。
【００１８】
また、前記第１の抽出手段は、前記複数の点各々から路面点を抽出し、該路面点以外の点を前記第１の立体物候補として抽出し、前記勾配推定手段は、前記路面点の３次元位置も加えた３次元位置の分布に基づいて、前記道路勾配を推定することができる。路面点は３次元位置を有しており、路面点を抽出すれば、そのまま道路勾配の推定に用いることができ、推定に用いる観測点を増加させることができるため、推定精度を向上させることができる。
【００１９】
また、前記変換手段は、所定期間の前記移動体の運動量に基づいて、過去に抽出された前記路面点の３次元位置を現在の座標系に変換し、前記勾配推定手段は、前記変換手段により変換された路面点の３次元位置も加えた分布に基づいて、前記道路勾配を推定することができる。接地点と同様、路面点についても過去の情報も用いることで、観測点を増加させることができるため、推定精度を向上させることができる。
【００２０】
また、前記第２の抽出手段は、前記画像から垂直エッジを抽出し、電柱、ポール、樹木、及び建築物の少なくとも１つを示す垂直エッジを、前記第２の立体物候補として抽出し、前記算出手段は、前記垂直エッジの最下端を前記接地点として検出することができる。
【００２１】
また、前記第２の抽出手段は、パターンマッチングにより、前記画像から円柱状の物体を前記第２の立体物候補として抽出し、前記算出手段は、前記円柱状の物体と路面とが交差する点を前記接地点として検出することができる。
【００２２】
また、前記複数の点各々の３次元位置を、複数の走査ラインを有するレーザレーダであって、該走査ラインの少なくとも１つが路面に照射されるように構成されたレーザレーダにより計測することができる。これにより、路面点が確実に計測されるため、道路勾配を推定するための観測点を確保することができる。
【００２３】
また、本発明の道路勾配推定プログラムは、コンピュータを、移動体周辺の複数の点各々の前記移動体を基準とする３次元位置、及び前記移動体周辺を該移動体に搭載された撮像手段により撮像した画像を取得する取得手段、前記取得手段により取得された前記複数の点各々の３次元位置に基づいて、立体物を示す点を第１の立体物候補として抽出する第１の抽出手段、前記取得手段により取得された画像の垂直エッジ情報及び予め定めた形状とのマッチング情報の少なくとも一方に基づいて、第２の立体物候補を抽出する第２の抽出手段と、前記第２の立体物候補に対応する前記第１の立体物候補の３次元位置に基づいて、該第２の立体物候補の路面との接地点の３次元位置を算出する算出手段、及び前記算出手段により算出された接地点の３次元位置に基づいて、前記移動体周辺の道路勾配を推定する勾配推定手段として機能させるためのプログラムである。
【００２４】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００２５】
以上説明したように、本発明の道路勾配推定装置及びプログラムによれば、撮像手段により撮像された画像から検出した立体物候補と路面との接地点、及び立体物候補までの精度の高い距離情報に基づいて、接地点の３次元位置が算出でき、この接地点の３次元位置を用いるため、先行車や車線境界線等の特定物体が存在しない場合でも、前方道路の勾配を精度良く推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２６】
【図１】第１の実施の形態に係る道路勾配推定装置を示すブロック図である。
【図２】レーザレーダの複数の走査ラインの一例を示す図である。
【図３】レーザレーダによる観測例の一例を示す図である。
【図４】レーザレーダで観測された観測データを画像化した一例を示すイメージ図である。
【図５】第２の立体物候補の抽出に用いる凹状パターンを示す図である。
【図６】（ａ）第２の立体物候補の抽出例と、（ｂ）対応する第１の立体物候補の抽出例を示す図である。
【図７】路面接地点の３次元位置の算出を説明するための図である。
【図８】路面勾配の推定を説明するための図である。
【図９】第１の実施の形態に係る道路勾配推定装置における道路勾配推定処理ルーチンの内容を示すフローチャートである。
【図１０】第２の実施の形態に係る道路勾配推定装置を示すブロック図である。
【図１１】第２の実施の形態に係る道路勾配推定装置における道路勾配推定処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００２７】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、車両がこれから走行する前方道路の勾配を推定する道路勾配推定装置に、本発明を適用した場合を例に説明する。
【００２８】
図１に示すように、第１の実施の形態に係る道路勾配推定装置１０は、自車両の前方に対して複数の走査ラインのレーザを水平方向に走査しながら照射し、レーザの反射によりレーザが照射された物体上の点の３次元位置を検出するレーザレーダ１２と、ＣＣＤカメラ等で構成され、車両周辺を撮像して取得された画像の画像データを出力する撮像装置１４と、前方道路の勾配を推定する処理を実行するコンピュータ２０と、を備えている。
【００２９】
レーザレーダ１２は、車両前方に設置され、車両の車体前後方向を中心とした視野角で周辺環境を観測し、装置を基準とする車両前方に存在する物体の方位及び距離を検出する装置である。レーザレーダ１２は、例えば、図２に示すように、出力する複数の走査ラインのレーザを水平方向に走査して、自車両前方に存在する複数の物体表面上の複数の点で反射されたレーザ光を受光する。レーザ光を照射してから受光するまでの時間により、レーザ光の反射点までの距離を算出する。また、レーザ光を照射した方向も既知であるため、レーザ光の反射点の３次元位置を観測することができる。また、図３に示すように、複数の走査ラインのうち少なくとも１ラインは、路面を観測することができるように俯角が設計されている。
【００３０】
レーザレーダ１２により観測される観測データは、自車両前方でのレーザ光の反射点の位置を表す３次元座標の集合である。レーザレーダ１２による観測処理は一定サイクルで実行され、レーザレーダ１２は、各時点での複数の自車両前方におけるレーザ光の反射点の３次元位置を示す観測データをコンピュータ２０に出力する。出力される観測データが示す各点の３次元位置は、レーザレーダ１２から各反射点までの距離及び方位で表される極座標であってもよいし、レーザレーダ１２を中心とする直交座標系上での３次元位置座標であってもよい。
【００３１】
撮像装置１４は、車両前方に設置され、車両の車体前後方向を中心とした視野角で、レーザレーダ１２と共通の観測領域を撮像して、画像データをコンピュータ２０に出力する。撮像装置１４が出力する画像データは、カラー画像であってもよいし、モノクロ画像であってもよい。
【００３２】
なお、レーザレーダ１２と撮像装置１４とは、道路勾配を推定したい範囲の視野角を有すればよく、センサの数や視野角は特に制限されない。さらに、レーザレーダ１２及び撮像装置１４の設置位置や角度は既知であるため、レーザレーダ１２で観測された物体の位置と、撮像装置１４で撮像された撮像画像内での物体の位置とは対応付けが可能である。
【００３３】
コンピュータ２０は、ＣＰＵ、後述する道路勾配推定処理ルーチンを実行するためのプログラム等各種プログラム及び各種データを記憶したＲＯＭ、データ等を一時的に記憶するＲＡＭ、各種情報が記憶されたメモリ、及びこれらを接続するバスを含んで構成されている。
【００３４】
このコンピュータ２０をハードウエアとソフトウエアとに基づいて定まる機能実現手段毎に分割した機能ブロックで説明すると、図１に示すように、レーザレーダ１２の観測データの中から、レーザ光が路面で反射した路面反射点を抽出する路面反射点抽出部２２と、路面反射点として抽出されなかった点群から第１の立体物候補を抽出する第１の立体物候補抽出部２４と、撮像装置１４により撮像された撮像画像から第２の立体物候補を抽出する第２の立体物候補抽出部２６と、第１の立体物候補と第２の立体物候補とを照合して立体物と路面との接地点の３次元位置を算出する路面接地点算出部２８と、算出された接地点の３次元位置の分布に基づいて、道路勾配を推定する道路勾配推定部３０と、を含んだ構成で表すことができる。
【００３５】
なお、路面反射点抽出部２２及び第１の立体物候補抽出部２４が、本発明の第１の抽出手段の一例である。
【００３６】
路面反射点抽出部２２は、レーザレーダ１２の観測結果に基づいて、観測データが示す各反射点の中からレーザ光が路面で反射した路面反射点を抽出する。具体的には、レーザ光のパルス形状や、受光したレーザ光の信号強度に基づいて、各反射点が路面反射点か否かを判定する。また、各反射点の３次元位置から高さ情報を用いて路面反射点か否かを判定してもよい。また、各反射点の並び（連続性）により、路面反射点か否かを判定するようにしてもよい。例えば、図４（ａ）のような周辺環境（撮像装置１４で撮像された画像）において、レーザレーダ１２で観測された観測データを画像化した一例を、同図（ｂ）に示す。一般的に路面は平面であるため、同一の走査ラインで観測される路面反射点は、同図（ｂ）に示すように円弧状となる。従って、隣接する反射点間で３次元位置の高さ方向の変化量が小さく、かつ反射点を連結した場合に車両前方の進行方向を横切るような円弧を描く反射点を、路面反射点として抽出することができる。
【００３７】
第１の立体物候補抽出部２４は、レーザレーダ１２の観測データが示す各反射点のうち、路面反射点抽出部２２で路面反射点として抽出されなかった反射点について、反射点間距離が所定距離より小さい反射点同士をグループ化し、近接する反射点群を１つの立体物とみなし、第１の立体物候補として抽出する。また、第１の立体物候補を構成する各反射点の３次元位置に基づいて、第１の立体物候補の位置を検出する。例えば、第１の立体物候補を構成する反射点群のうち高さ方向の位置が最小となる反射点の３次元位置を、その第１の立体物候補の３次元位置として検出することができる。
【００３８】
第２の立体物候補抽出部２６は、撮像装置１４で撮像された撮像画像から、垂直エッジの検出またはパターン認識により、電柱やポール等に相当する第２の立体物候補を抽出する。
【００３９】
まず、垂直エッジによる第２の立体物候補の抽出について説明する。第２の立体物候補は、電柱、ガードレールや標識の支柱等のポール、樹木などの円柱状の物体であり、さらに建物等の構造物の稜線等も含んでよい。このような立体物は垂直エッジを含む場合が多いため、撮像画像全体から垂直エッジを抽出する。抽出された垂直エッジを、エッジの長さ、傾き、エッジ勾配等に基づいてグループ化し、グループ化された垂直エッジ群を１つの立体物とみなし、第２の立体物候補として抽出する。
【００４０】
次に、パターン認識による第２の立体物候補の抽出について説明する。図５に示すように、電柱やポールなどの円柱状の立体物が路面と接地する箇所は、凹状のパターンとなる。そこで、パターン認識により、この凹状パターンに相当する部分を撮像画像から検出し、凹状パターンを含む物体を第２の立体物候補として抽出する。
【００４１】
第２の立体物候補抽出部２６では、上記の垂直エッジの検出及びパターン認識の少なくとも一方を実行する。
【００４２】
路面接地点算出部２８は、レーザレーダ１２及び撮像装置１４の姿勢に基づいて、第１の立体物候補と第２の立体物候補との対応を取る。具体的には、車両を中心とする３次元座標空間に、抽出された第１の立体物候補及び第２の立体物候補を投影し、第２の立体物候補の水平方向の位置を手掛かりに奥行き方向に探索して対応する第１の立体物候補を検出することにより、第１の立体物候補と第２の立体物候補との対応を取る。図６（ａ）に第２の立体物候補の抽出例、同図（ｂ）に第１の立体物候補の抽出例を示す。ここでは、説明のため、同図（ｂ）は、高さ方向の情報を削除した鳥瞰表示で表している。各図において１〜５の番号が付された立体物候補が各々対応している。なお、同図（ｂ）において、矩形枠で囲まれた反射点群が、グループ化された第１の立体物候補を表している。
【００４３】
また、路面接地点算出部２８は、立体物と路面との接地点（以下、「路面接地点」という）を、撮像画像上から検出する。具体的には、第２の立体物候補を垂直エッジの検出により抽出した場合には、垂直エッジの最下端を路面接地点として検出し、パターン認識により抽出した場合には、第２の立体物候補と路面とが交差する点、すなわち凹状パターンの一部を路面接地点として検出する。
【００４４】
また、路面接地点算出部２８は、第２の立体物候補求まる３次元座標空間の原点から路面接地点へ向かう方向、及び路面接地点を検出した第２の立体物候補に対応する第１の立体物候補と自車両との距離に基づいて、路面接地点の３次元位置を算出する。なお、第2の立体物候補からは、接地点への方向ベクトルが算出可能である。レーザレーダ１２は、距離の測定精度は高いが、特に遠方の計測点における解像度が低下するため、撮像画像上で検出された路面接地点の情報と照合することにより、精度良く路面接地点の３次元位置を算出することができる。
【００４５】
図７に示すように、例えば、車両を中心とする３次元座標空間の奥行き方向の軸を撮像装置１４の光軸、原点を撮像装置１４の撮像面とした場合には、車両を中心とする３次元座標空間の奥行き方向の軸（光軸）と、３次元座標空間の原点（撮像装置１４）と路面接地点とを結ぶ線分とのなす角は、水平方向の角度θＨ、及び鉛直方向の角度θＶとして求めることができる。水平方向の角度θＨは、撮像装置１４の光軸を含む鉛直面と、撮像装置１４と路面接地点とを結ぶ線分を含む鉛直面とのなす角であり、鉛直方向の角度θＶは、鉛直面に直交し、かつ撮像装置１４の光軸を含む平面と、鉛直面に直交し、かつ撮像装置１４と路面接地点とを結ぶ線分を含む平面とのなす角である。また、第１の立体物候補と自車両との距離は、第１の立体物候補抽出部２４で検出された第１の立体物候補の３次元位置に基づいて算出することができる。
【００４６】
道路勾配推定部３０は、路面接地点算出部２８で算出された路面接地点の３次元位置、及び路面反射点抽出部２２で抽出された路面反射点の３次元位置に基づいて、路面接地点及び路面反射点を、車両を中心とした座標系にプロットし、最小二乗法などの既知の技術により路面モデルをフィッティングして、道路形状を推定する。説明の簡略化のため、道路平面の勾配を推定する場合について具体的に説明する。図８に示すように、車両前後方向（ｚ軸）、及び高さ方向（ｙ軸）の２軸で構成される平面に、路面接地点及び路面反射点を投影する。この点群に、下記（１）式で表されるクロソイド曲線を路面モデルとしてフィッティングする。
【００４７】
【数１】
【００４８】
ただし、Ｃ０は曲率、Ｃ１は曲率の変化率、Ｃ２はｙ軸方向のオフセット、θｐはｚ＝０におけるｙｚ平面内での路面角度である。そして、投影された全ての点を用いて最小二乗法で４つのパラメータを最適推定する。推定した道路勾配は、他の運転支援システムや車両制御システムへ出力する。
【００４９】
なお、観測点が多い場合には、ランダムサンプリングによる観測点の選択、及び選択した観測点に基づくパラメータ推定を数回繰り返すようにしてもよい。これにより、計算量を抑えながら安定した推定結果を得られる。また、路面モデルは、クロソイド曲線に限定されない。また、連続関数である必要はなく、不連続な折れ線モデルでフィッティングするようにしてもよい。
【００５０】
次に、第１の実施の形態に係る道路勾配推定装置１０の作用について説明する。
【００５１】
まず、レーザレーダ１２によって、複数の走査ラインのレーザが自車両の前方を水平方向に走査されて、車両周辺の各反射点の３次元位置を特定する観測データが観測される。レーザレーダ１２によって観測される観測データは、レーザを走査する毎に得られる。また、レーザレーダの観測タイミングに同期して、撮像装置１４により車両周辺が撮像される。そして、コンピュータ２０によって、図９に示す道路勾配推定処理ルーチンが実行される。
【００５２】
ステップ１００で、レーザレーダ１２で観測された観測データ、及び撮像装置１４で撮像された撮像画像を取得する。
【００５３】
次に、ステップ１０２で、レーザ光のパルス形状や、受光したレーザ光の信号強度に基づいて、各反射点が路面反射点か否かを判定したり、各反射点の３次元位置から高さ情報を用いて路面反射点か否かを判定したり、各反射点の並び（連続性）により、路面反射点か否かを判定したりすることにより、上記ステップ１００で取得した観測データが示す各反射点の中からレーザ光が路面で反射した路面反射点を抽出する。
【００５４】
次に、ステップ１０４で、上記ステップ１００で取得した観測データが示す各反射点のうち、上記ステップ１０２で、路面反射点として抽出されなかった反射点について、反射点間距離が所定距離より小さい反射点同士をグループ化し、近接する反射点群を１つの立体物とみなし、第１の立体物候補として抽出する。また、第１の立体物候補に含まれる各反射点の３次元位置に基づいて、第１の立体物候補の位置を検出する。
【００５５】
次に、ステップ１０６で、上記ステップ１００で取得した撮像画像から、垂直エッジの検出またはパターン認識により、電柱やポール等に相当する第２の立体物候補を抽出する。
【００５６】
次に、ステップ１０８で、車両を中心とする３次元座標空間に、上記ステップ１０４で抽出されたた第１の立体物候補、及び上記ステップ１０６で抽出された第２の立体物候補を投影し、第２の立体物候補の水平方向の位置を手掛かりに奥行き方向に探索して対応する第１の立体物候補を検出することにより、第１の立体物候補と第２の立体物候補との対応を取る。
【００５７】
次に、ステップ１１０で、第２の立体物候補を垂直エッジの検出により抽出した場合には、撮像画像上の垂直エッジの最下端を、第２の立体物候補をパターン認識により抽出した場合には、第２の立体物候補と路面とが交差する点を、路面接地点として検出する。そして、車両を中心とする３次元座標空間の奥行き方向の軸と、３次元座標空間の原点と路面接地点とを結ぶ線分とのなす角、及び路面接地点を検出した第２の立体物候補に対応する第１の立体物候補と自車両との距離に基づいて、路面接地点の３次元位置を算出する。
【００５８】
次に、ステップ１１２で、上記ステップ１１０で算出された路面接地点の３次元位置、及び上記ステップ１０２で抽出された路面反射点の３次元位置に基づいて、路面接地点及び路面反射点を、車両を中心とした座標系にプロットし、最小二乗法などの既知の技術により道路平面をフィッティングして、道路形状を推定する。そして、推定結果を他の運転支援システムや車両制御システムへ出力して、処理を終了する。
【００５９】
以上説明したように、第１の実施の形態の道路勾配推定装置によれば、撮像画像から検出された路面接地点について、レーザレーダの距離情報を用いて３次元位置を算出し、この路面接地点の３次元位置を用いて道路平面を推定するため、先行車や車線境界線等の特定物体が存在しない場合でも、前方道路の勾配を精度良く推定することができる。
【００６０】
また、レーザレーダの観測データのうち、路面反射点の３次元位置もあわせて用いるため、観測点の情報を増加させることができ、推定精度を向上させることができる。
【００６１】
次に、第２の実施の形態について説明する。なお、第２の実施の形態の道路勾配推定装置について、第１の実施の形態の道路勾配推定装置１０と同様の構成については、同一の符号を付して、詳細な説明は省略する。
【００６２】
図１０に示すように、第２の実施の形態に係る道路勾配推定装置２１０は、レーザレーダ１２と、撮像装置１４と、車速センサ及びジャイロセンサ等を含んで構成された車両状態センサ１６と、コンピュータ２２０と、を備えている。このコンピュータ２２０をハードウエアとソフトウエアとに基づいて定まる機能実現手段毎に分割した機能ブロックで説明すると、路面反射点抽出部２２２と、第１の立体物候補抽出部と、第２の立体物候補抽出部２６と、路面接地点算出部２２８と、道路勾配推定部３０と、路面接地点及び路面反射点の３次元位置情報等を記憶する情報蓄積部３２と、車両状態センサ１６の検出値、現在及び過去のレーザレーダ１２の観測値に基づいて、自車両の運動を推定する自車運動推定部３４と、情報蓄積部３２に蓄積された過去の路面接地点及び路面反射点の３次元位置（以下、「蓄積データ」ともいう）を、推定された自車両の運動に基づいて、現在の座標系に変換する蓄積データ座標変換部３６と、を含んだ構成で表すことができる。
【００６３】
なお、路面反射点抽出部２２２及び第１の立体物候補抽出部２２４が、本発明の第１の抽出手段の一例であり、自車運動推定部３４及び蓄積データ座標変換部３６が、本発明の変換手段の一例である。
【００６４】
自車運動推定部３４は、情報蓄積部３２に蓄積された過去の観測データが示す点群と、現在観測された観測データが示す点群とを照合して、観測間の自車両の運動量を推定する。点群の照合による運動量の推定には、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｔ Ｐｏｉｎｔ）アルゴリズムを用いたり、点群から複数のプロファイル特徴を抽出し、それらの位置が全体的に最も一致するように推定したりすることができる。過去の点群と現在の点群との照合には、車両状態センサ１６の検出値である自車両の車速や姿勢角等を自車運動の初期値として利用することができる。これにより、照合の計算量を削減することができる。
【００６５】
また、自車運動推定部３４は、過去の点群と現在の点群とを照合する際に、一致する点群は静止物、一致しない点群は移動物または路面点と判定することができる。この判定結果を路面反射点の抽出や第１の立体物候補の抽出に利用するために、路面反射点抽出部２２２及び第１の立体物候補抽出部２２４へ出力する。
【００６６】
路面反射点抽出部２２２は、第１の実施の形態の路面反射点抽出部２２と同様に、レーザレーダ１２の観測結果に基づいて、観測データが示す各反射点の中からレーザ光が路面で反射した路面反射点を抽出する。その際、自車運動推定部３４から出力された判定結果も利用して、移動物または路面点と判定された反射点群（過去の点群と現在の点群が一致しなかった点群）から路面反射点を抽出する。また、抽出した路面反射点の３次元位置を情報蓄積部３２に保存する。
【００６７】
第１の立体物候補抽出部２２４は、第１の実施の形態の第１の立体物候補抽出部２４と同様に、レーザレーダ１２の観測データが示す各反射点のうち、路面反射点抽出部２２２で路面反射点として抽出されなかった反射点について、反射点間距離が所定距離より小さい反射点同士をグループ化し、近接する反射点群を１つの立体物とみなし、第１の立体物候補として抽出する。その際、自車運動推定部３４から出力された判定結果も利用して、静止物と判定された反射点群（過去の点群と現在の点群が一致した点群）の情報に基づいて、第１の立体物候補を構成する反射点とするか否かを再抽出したり、反射点群のグループ化をしたりする。また、第１の立体物候補を構成する各反射点の３次元位置に基づいて、第１の立体物候補の位置を検出する。
【００６８】
路面接地点算出部２２８は、第１の実施の形態の路面接地点算出部２８と同様に、第１の立体物候補と第２の立体物候補との対応を取り、路面接地点を検出して、路面接地点の３次元位置を算出する。また、算出した路面接地点の３次元位置を情報蓄積部３２に保存する。この際、既に情報蓄積部３２に保存されている蓄積データと現在の路面接地点の３次元位置とを照合し、照合が取れた路面接地点のデータに、照合回数が多いほど大きくなる信頼度を付与する。例えば、後述の蓄積データ座標変換部３６の処理により、情報蓄積部３２に保存された蓄積データの座標系を現在の座標系に変換し、その座標系を細かい格子状のセルに区画する。そして、既に蓄積データが観測されているセルに、現在の路面接地点の３次元位置が検出された場合には、そのセルに対応する信頼度をカウントアップする。なお、セルの大きさは、同一のセルに異なる路面点を示す点がプロットされないように、レーザレーダ１２の解像度を考慮して、十分細かくする。これにより、信頼性の高いデータと信頼性の低いデータとを判別することができる。なお、路面反射点抽出部２２２において、抽出された路面反射点を情報蓄積部３２に保存する際にも、同様に、信頼度を付与するようにしてもよい。
【００６９】
蓄積データ座標変換部３６は、推定された自車両の運動に基づいて、情報蓄積部３２に蓄積された蓄積データを現在の座標系に変換する。例えば、現在の時刻ｔの一時刻前（ｔ−１）の蓄積データのうち、信頼度が所定値以上の蓄積データを情報蓄積部３２から読み出して、時刻（ｔ−１）における車両を中心とする３次元座標空間にプロットする。そして、自車運動推定部３４で推定された時刻（ｔ−１）〜時刻ｔ間の自車両の運動量を取得して、時刻（ｔ−１）における３次元座標空間を、時刻ｔにおける３次元空間座標に変換する。
【００７０】
道路勾配推定部２３０は、第１の実施の形態の道路勾配推定部３０と同様に、路面接地点算出部２２８で算出された現在の路面接地点、及び路面反射点抽出部２２２で抽出された現在の路面反射点（以下、「現在のデータ」ともいう）を、車両を中心とした座標系にプロットすると共に、蓄積データ座標変換部３６により座標変換された蓄積データも、同じ座標系にプロットして、現在のデータと蓄積データとを照合して、一致するとみなせる点を統合する。例えば、上述のように、データをプロットする座標系を、細かい格子状のセルに区画し、同一のセルにプロットされた現在のデータ及び蓄積データについて、いずれかのデータを削除するか、両データの平均をとるなどして、１つのデータに統合する。セルにプロットされデータが１つの場合には、そのデータをそのまま採用する。
【００７１】
また、道路勾配推定部２３０は、上記のように、現在のデータ及び蓄積データを、車両を中心とした座標系にプロットして統合した後に、第１の実施の形態と同様に、最小二乗法などの既知の技術により道路平面をフィッティングして、道路形状を推定する。
【００７２】
次に、第２の実施の形態に係る道路勾配推定装置２１０の作用について説明する。
【００７３】
まず、レーザレーダ１２によって、複数の走査ラインのレーザが自車両の前方を水平方向に走査されて、車両周辺の各反射点の３次元位置を特定する観測データが観測される。レーザレーダ１２によって観測される観測データは、レーザを走査する毎に得られる。また、レーザレーダの観測タイミングに応じて、撮像装置１４により車両周辺が撮像される。そして、コンピュータ２０によって、図１１に示す道路勾配推定処理ルーチンが実行される。なお、第１の実施の形態の道路勾配推定処理と同一の処理については、同一の符号を付して詳細な説明は省略する。
【００７４】
ステップ２００で、レーザレーダ１２で観測された観測データ、撮像装置１４で撮像された撮像画像、及び車両状態センサ１６の検出値を取得する。
【００７５】
次に、ステップ２０２で、情報蓄積部３２に蓄積された過去の観測データが示す点群と、上記ステップ２００で取得された観測データが示す点群とを照合して、観測間の自車両の運動量を推定する。ここでは、時刻（ｔ−１）〜時刻ｔ間の自車両の運動量を推定するものとする。過去の点群と現在の点群との照合には、上記ステップ２００で取得した車両状態センサ１６の検出値である自車両の車速や姿勢角等を自車運動の初期値として利用する。そして、過去の点群と現在の点群とを照合する際に、一致する点群は静止物、一致しない点群は移動物または路面点と判定する。
【００７６】
次に、ステップ２０４で、上記ステップ２０２で移動物または路面点と判定された反射点の中から、受光パルスの形状や信号強度、高さ情報、反射点の連続性等に基づいて、レーザ光が路面で反射した路面反射点を抽出する。
【００７７】
次に、ステップ２０６で、上記ステップ２０４で抽出された路面反射点の３次元位置と、情報蓄積部３２に保存された蓄積データとを照合して、照合結果に応じて信頼度を更新した上で、路面反射点の３次元位置を情報蓄積部３２に保存する。
【００７８】
次に、ステップ２０８で、上記ステップ２００で取得した観測データが示す各反射点のうち、上記ステップ２０４で、路面反射点として抽出されなかった反射点であって、上記ステップ２０２で静止物と判定された反射点について、反射点間距離が所定距離より小さい反射点同士をグループ化して、第１の立体物候補として抽出する。また、第１の立体物候補に含まれる各反射点の３次元位置に基づいて、第１の立体物候補の位置を検出する。
【００７９】
次に、ステップ１０６で、上記ステップ２００で取得した撮像画像から、第２の立体物候補を抽出し、次に、ステップ１０８で、第１の立体物候補と第２の立体物候補との対応を取り、次に、ステップ１１０で、路面接地点の３次元位置を算出する。
【００８０】
次に、ステップ２０９で、上記ステップ１１０で算出された路面接地点の３次元位置と、情報蓄積部３２に保存された蓄積データとを照合して、照合結果に応じて信頼度を更新した上で、路面接地点の３次元位置を情報蓄積部３２に保存する。
【００８１】
次に、ステップ２１２で、現在の時刻ｔの一時刻前（ｔ−１）の蓄積データのうち、信頼度が所定値以上の蓄積データを情報蓄積部３２から読み出して、時刻（ｔ−１）における車両を中心とする３次元座標空間にプロットする。そして、上記ステップ２０２で推定された時刻（ｔ−１）〜時刻ｔ間の自車両の運動量に基づいて、時刻（ｔ−１）における３次元座標空間を、時刻ｔにおける３次元空間座標に変換する。
【００８２】
次に、ステップ２１４で、上記ステップ１１０で算出された路面接地点の３次元位置、上記ステップ２０４で抽出された路面反射点の３次元位置、及び上記ステップ２１２で座標変換された蓄積データに基づいて、路面接地点、路面反射点、及び蓄積データを、車両を中心とした座標系にプロットし、最小二乗法などの既知の技術により道路平面をフィッティングして、道路形状を推定する。そして、推定結果を他の運転支援システムや車両制御システムへ出力して、処理を終了する。
【００８３】
以上説明したように、第２の実施の形態の道路勾配推定装置によれば、過去に算出された路面接地点の３次元位置、及び過去に抽出された路面反射点の３次元位置も用いて、道路平面を推定するため、観測点の情報を増加させることができ、より推定精度を向上させることができる。
【００８４】
また、過去のデータを用いる際に、現在のデータとの照合回数に応じた信頼度が所定値以上のデータを用いることで、観測の度に出現したり消滅したりする観測ノイズの影響を低減して、より推定精度を向上させることができる。
【００８５】
なお、第２の実施の形態では、レーザレーダによる現在及び過去の観測データの照合により、観測間の自車両の運動量を推定する場合について説明したが、自車両の運動量を、車両状態センサの検出値に基づいて推定してもよいし、撮像画像から推定してもよい。
【００８６】
また、第２の実施の形態では、現在のデータに加えて、信頼度が所定値以上の蓄積データを用いる場合について説明したが、最小二乗法等で路面モデルをフィッティングする際に、誤差に掛ける重みとして、この信頼度を利用するようにしてもよい。
【００８７】
また、上記実施の形態では、レーザレーダにより車両周辺から複数の反射点の位置を特定する情報を観測する場合について説明したが、これに限定されるものではなく、ミリ波などの電磁波を用いてもよい。また、例えば、ステレオカメラによって撮像された画像を用いて、後処理により車両周辺の複数の点の位置を特定する情報を演算するようにしてもよい。
【符号の説明】
【００８８】
１０、２１０ 道路勾配推定装置
１２ レーザレーダ
１４ 撮像装置
１６ 車両状態センサ
２０、２２０ コンピュータ
２２、２２２ 路面反射点抽出部
２４、２２４ 第１の立体物候補抽出部
２６ 第２の立体物候補抽出部
２８、２２８ 路面接地点算出部
３０、２３０ 道路勾配推定部
３２ 情報蓄積部
３４ 自車運動推定部
３６ 蓄積データ座標変換部
【技術分野】
【０００１】
本発明は、道路勾配推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来、走査制御手段により、先行車の反射光の受光位置に追従して自車の測距用のレーザレーダのレーザの高さ方向の走査位置を可変し、車間距離演算手段により、レーザの照射と反射光の受光との時間差から自車と先行車との車間距離を計測し、起点検出手段により、高さ方向の走査位置が設定値以上変化した地点より計測車間距離前方の勾配起点を検出し、自車が勾配起点に到達したときに、勾配演算手段により、自車の走行距離と走査変化量検出手段が検出した高さ方向の走査位置の変化量の検出値とから、前方の路面の相対的な勾配を算出して推定する路面勾配推定装置が提案されている（例えば、特許文献１参照）。
【０００３】
また、映像カメラから前方の路面映像を受信し、路面映像から車線境界線を抽出し、抽出された車線境界線に基づいてレーダーの照射角度を調節し、調節された照射角度で照射されたレーダービームの反射波を利用して前方車両との距離を計算することにより、道路に屈曲のある場合でも前方車両との距離を検出する車間距離測定装置が提案されている（例えば、特許文献２参照）。
【０００４】
また、ジャイロセンサを用いて、車両の走行中のピッチ角を検出し、車両の前後方向に位置を異ならせた複数箇所で、車両の車体と路面との距離を検出することによって、車両傾き角を検出し、ピッチ角と車両傾き角との偏差に基づいて、走行中の道路の道路勾配を検出する道路勾配計測装置が提案されている（例えば、特許文献３参照）。特許文献３の道路勾配計測装置において、ピッチ角は、車両傾き角および道路勾配のみによって決定されるため、ジャイロセンサを用いて精度良くピッチ角を検出し、車両の前後で路面との距離を検出して車両傾き角を求めることで、ピッチ角と車両傾き角との偏差から、道路勾配を算出している。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００４−３１７３２３号公報
【特許文献２】特開２００４−１５１０８０号公報
【特許文献３】特開２００９−２７６１０９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１の技術では、先行車のリフレクタが検知されるスキャンラインの変化を利用するため、先行車が存在しない状況では、道路勾配を推定することができない、という問題がある。また、路面の凹凸などによって自車両にピッチ変動が生じた場合に、勾配起点の検出が困難になる場合もある、という問題がある。
【０００７】
また、特許文献２の技術では、車線境界線が整備されていない道路や、汚れや磨耗により車線境界線の検出が安定して行えない場合には、道路勾配を推定することが困難である、という問題がある。
【０００８】
また、特許文献３の技術では、車両直下の路面が計測対象となっており、車両が通過した道路の勾配を高精度に推定することはできるが、これから走行する前方道路の勾配を推定することはできない、という問題がある。
【０００９】
本発明は、上記の問題点を解決するためになされたもので、先行車や車線境界線等の特定物体が存在しない場合でも、前方道路の勾配を精度良く推定することができる道路勾配推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記目的を達成するために、本発明の道路勾配推定装置は、移動体周辺の複数の点各々の前記移動体を基準とする３次元位置、及び前記移動体周辺を該移動体に搭載された撮像手段により撮像した画像を取得する取得手段と、前記取得手段により取得された前記複数の点各々の３次元位置に基づいて、立体物を示す点を第１の立体物候補として抽出する第１の抽出手段と、前記取得手段により取得された画像の垂直エッジ情報及び予め定めた形状とのマッチング情報の少なくとも一方に基づいて、第２の立体物候補を抽出する第２の抽出手段と、前記第２の立体物候補に対応する前記第１の立体物候補の３次元位置に基づいて、該第２の立体物候補の路面との接地点の３次元位置を算出する算出手段と、前記算出手段により算出された接地点の３次元位置に基づいて、前記移動体周辺の道路勾配を推定する勾配推定手段と、を含んで構成されている。
【００１１】
本発明の道路勾配推定装置によれば、取得手段が、移動体周辺の複数の点各々の移動体を基準とする３次元位置、及び移動体周辺を該移動体に搭載された撮像手段により撮像した画像を取得する。そして、第１の抽出手段が、取得手段により取得された複数の点各々の３次元位置に基づいて、立体物を示す点を第１の立体物候補として抽出する。第１の抽出手段は、各点の３次元位置に基づいて立体物候補を抽出するため、移動体と立体物候補との距離について、精度の高い距離情報を有することができる。
【００１２】
また、第２の抽出手段が、取得手段により取得された画像の垂直エッジ情報及び予め定めた形状とのマッチング情報の少なくとも一方に基づいて、第２の立体物候補を抽出する。第２の抽出手段は、撮像手段により撮像された画像から立体物候補を抽出するため、画像上で立体物候補と路面との接地点を精度良く検出することができる。
【００１３】
そして、算出手段が、第２の立体物候補に対応する第１の立体物候補の３次元位置に基づいて、該第２の立体物候補の路面との接地点の３次元位置を算出し、勾配推定手段が、算出手段により算出された接地点の３次元位置に基づいて、移動体周辺の道路勾配を推定する。
【００１４】
このように、撮像手段により撮像された画像から検出した立体物候補と路面との接地点と、立体物候補までの精度の高い距離情報とに基づいて、接地点の３次元位置を算出するため、この接地点の３次元位置を用いて、先行車や車線境界線等の特定物体が存在しない場合でも、前方道路の勾配を精度良く推定することができる。
【００１５】
また、前記算出手段は、前記第２の立体物候補から求まる３次元座標空間の原点から前記接地点へ向かう方向、及び前記対応する第１の立体物候補の３次元位置に基づいて、前記接地点の３次元位置を算出することができる。
【００１６】
また、本発明の道路勾配推定装置は、所定期間の前記移動体の運動量に基づいて、過去に算出された前記接地点の３次元位置を現在の座標系に変換する変換手段を含んで構成することができ、前記勾配推定手段は、前記変換手段により変換された接地点の３次元位置も加えた分布に基づいて、前記道路勾配を推定することができる。これにより、道路勾配の推定に用いる観測点を増加させることができるため、推定精度を向上させることができる。
【００１７】
また、前記算出手段は、現在算出された接地点の３次元位置と過去に算出された接地点の３次元位置との照合結果に基づく信頼度を付与して、該現在算出された接地点の３次元位置を所定の記憶領域に記憶し、前記勾配推定手段は、前記所定の記憶領域に記憶された接地点の３次元位置のうち、前記信頼度が所定値以上の接地点の３次元位置を前記変換手段により変換した３次元位置を加えるか、または、前記所定の記憶領域に記憶された接地点の３次元位置を前記変換手段により変換して、前記信頼度に応じた重み付けをして加えることができる。このように、信頼度の高い過去の接地点の３次元位置を用いるため、または信頼度に応じた重み付けで過去の接地点の３次元位置を用いるため、より推定精度を向上させることができる。
【００１８】
また、前記第１の抽出手段は、前記複数の点各々から路面点を抽出し、該路面点以外の点を前記第１の立体物候補として抽出し、前記勾配推定手段は、前記路面点の３次元位置も加えた３次元位置の分布に基づいて、前記道路勾配を推定することができる。路面点は３次元位置を有しており、路面点を抽出すれば、そのまま道路勾配の推定に用いることができ、推定に用いる観測点を増加させることができるため、推定精度を向上させることができる。
【００１９】
また、前記変換手段は、所定期間の前記移動体の運動量に基づいて、過去に抽出された前記路面点の３次元位置を現在の座標系に変換し、前記勾配推定手段は、前記変換手段により変換された路面点の３次元位置も加えた分布に基づいて、前記道路勾配を推定することができる。接地点と同様、路面点についても過去の情報も用いることで、観測点を増加させることができるため、推定精度を向上させることができる。
【００２０】
また、前記第２の抽出手段は、前記画像から垂直エッジを抽出し、電柱、ポール、樹木、及び建築物の少なくとも１つを示す垂直エッジを、前記第２の立体物候補として抽出し、前記算出手段は、前記垂直エッジの最下端を前記接地点として検出することができる。
【００２１】
また、前記第２の抽出手段は、パターンマッチングにより、前記画像から円柱状の物体を前記第２の立体物候補として抽出し、前記算出手段は、前記円柱状の物体と路面とが交差する点を前記接地点として検出することができる。
【００２２】
また、前記複数の点各々の３次元位置を、複数の走査ラインを有するレーザレーダであって、該走査ラインの少なくとも１つが路面に照射されるように構成されたレーザレーダにより計測することができる。これにより、路面点が確実に計測されるため、道路勾配を推定するための観測点を確保することができる。
【００２３】
また、本発明の道路勾配推定プログラムは、コンピュータを、移動体周辺の複数の点各々の前記移動体を基準とする３次元位置、及び前記移動体周辺を該移動体に搭載された撮像手段により撮像した画像を取得する取得手段、前記取得手段により取得された前記複数の点各々の３次元位置に基づいて、立体物を示す点を第１の立体物候補として抽出する第１の抽出手段、前記取得手段により取得された画像の垂直エッジ情報及び予め定めた形状とのマッチング情報の少なくとも一方に基づいて、第２の立体物候補を抽出する第２の抽出手段と、前記第２の立体物候補に対応する前記第１の立体物候補の３次元位置に基づいて、該第２の立体物候補の路面との接地点の３次元位置を算出する算出手段、及び前記算出手段により算出された接地点の３次元位置に基づいて、前記移動体周辺の道路勾配を推定する勾配推定手段として機能させるためのプログラムである。
【００２４】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００２５】
以上説明したように、本発明の道路勾配推定装置及びプログラムによれば、撮像手段により撮像された画像から検出した立体物候補と路面との接地点、及び立体物候補までの精度の高い距離情報に基づいて、接地点の３次元位置が算出でき、この接地点の３次元位置を用いるため、先行車や車線境界線等の特定物体が存在しない場合でも、前方道路の勾配を精度良く推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２６】
【図１】第１の実施の形態に係る道路勾配推定装置を示すブロック図である。
【図２】レーザレーダの複数の走査ラインの一例を示す図である。
【図３】レーザレーダによる観測例の一例を示す図である。
【図４】レーザレーダで観測された観測データを画像化した一例を示すイメージ図である。
【図５】第２の立体物候補の抽出に用いる凹状パターンを示す図である。
【図６】（ａ）第２の立体物候補の抽出例と、（ｂ）対応する第１の立体物候補の抽出例を示す図である。
【図７】路面接地点の３次元位置の算出を説明するための図である。
【図８】路面勾配の推定を説明するための図である。
【図９】第１の実施の形態に係る道路勾配推定装置における道路勾配推定処理ルーチンの内容を示すフローチャートである。
【図１０】第２の実施の形態に係る道路勾配推定装置を示すブロック図である。
【図１１】第２の実施の形態に係る道路勾配推定装置における道路勾配推定処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００２７】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、車両がこれから走行する前方道路の勾配を推定する道路勾配推定装置に、本発明を適用した場合を例に説明する。
【００２８】
図１に示すように、第１の実施の形態に係る道路勾配推定装置１０は、自車両の前方に対して複数の走査ラインのレーザを水平方向に走査しながら照射し、レーザの反射によりレーザが照射された物体上の点の３次元位置を検出するレーザレーダ１２と、ＣＣＤカメラ等で構成され、車両周辺を撮像して取得された画像の画像データを出力する撮像装置１４と、前方道路の勾配を推定する処理を実行するコンピュータ２０と、を備えている。
【００２９】
レーザレーダ１２は、車両前方に設置され、車両の車体前後方向を中心とした視野角で周辺環境を観測し、装置を基準とする車両前方に存在する物体の方位及び距離を検出する装置である。レーザレーダ１２は、例えば、図２に示すように、出力する複数の走査ラインのレーザを水平方向に走査して、自車両前方に存在する複数の物体表面上の複数の点で反射されたレーザ光を受光する。レーザ光を照射してから受光するまでの時間により、レーザ光の反射点までの距離を算出する。また、レーザ光を照射した方向も既知であるため、レーザ光の反射点の３次元位置を観測することができる。また、図３に示すように、複数の走査ラインのうち少なくとも１ラインは、路面を観測することができるように俯角が設計されている。
【００３０】
レーザレーダ１２により観測される観測データは、自車両前方でのレーザ光の反射点の位置を表す３次元座標の集合である。レーザレーダ１２による観測処理は一定サイクルで実行され、レーザレーダ１２は、各時点での複数の自車両前方におけるレーザ光の反射点の３次元位置を示す観測データをコンピュータ２０に出力する。出力される観測データが示す各点の３次元位置は、レーザレーダ１２から各反射点までの距離及び方位で表される極座標であってもよいし、レーザレーダ１２を中心とする直交座標系上での３次元位置座標であってもよい。
【００３１】
撮像装置１４は、車両前方に設置され、車両の車体前後方向を中心とした視野角で、レーザレーダ１２と共通の観測領域を撮像して、画像データをコンピュータ２０に出力する。撮像装置１４が出力する画像データは、カラー画像であってもよいし、モノクロ画像であってもよい。
【００３２】
なお、レーザレーダ１２と撮像装置１４とは、道路勾配を推定したい範囲の視野角を有すればよく、センサの数や視野角は特に制限されない。さらに、レーザレーダ１２及び撮像装置１４の設置位置や角度は既知であるため、レーザレーダ１２で観測された物体の位置と、撮像装置１４で撮像された撮像画像内での物体の位置とは対応付けが可能である。
【００３３】
コンピュータ２０は、ＣＰＵ、後述する道路勾配推定処理ルーチンを実行するためのプログラム等各種プログラム及び各種データを記憶したＲＯＭ、データ等を一時的に記憶するＲＡＭ、各種情報が記憶されたメモリ、及びこれらを接続するバスを含んで構成されている。
【００３４】
このコンピュータ２０をハードウエアとソフトウエアとに基づいて定まる機能実現手段毎に分割した機能ブロックで説明すると、図１に示すように、レーザレーダ１２の観測データの中から、レーザ光が路面で反射した路面反射点を抽出する路面反射点抽出部２２と、路面反射点として抽出されなかった点群から第１の立体物候補を抽出する第１の立体物候補抽出部２４と、撮像装置１４により撮像された撮像画像から第２の立体物候補を抽出する第２の立体物候補抽出部２６と、第１の立体物候補と第２の立体物候補とを照合して立体物と路面との接地点の３次元位置を算出する路面接地点算出部２８と、算出された接地点の３次元位置の分布に基づいて、道路勾配を推定する道路勾配推定部３０と、を含んだ構成で表すことができる。
【００３５】
なお、路面反射点抽出部２２及び第１の立体物候補抽出部２４が、本発明の第１の抽出手段の一例である。
【００３６】
路面反射点抽出部２２は、レーザレーダ１２の観測結果に基づいて、観測データが示す各反射点の中からレーザ光が路面で反射した路面反射点を抽出する。具体的には、レーザ光のパルス形状や、受光したレーザ光の信号強度に基づいて、各反射点が路面反射点か否かを判定する。また、各反射点の３次元位置から高さ情報を用いて路面反射点か否かを判定してもよい。また、各反射点の並び（連続性）により、路面反射点か否かを判定するようにしてもよい。例えば、図４（ａ）のような周辺環境（撮像装置１４で撮像された画像）において、レーザレーダ１２で観測された観測データを画像化した一例を、同図（ｂ）に示す。一般的に路面は平面であるため、同一の走査ラインで観測される路面反射点は、同図（ｂ）に示すように円弧状となる。従って、隣接する反射点間で３次元位置の高さ方向の変化量が小さく、かつ反射点を連結した場合に車両前方の進行方向を横切るような円弧を描く反射点を、路面反射点として抽出することができる。
【００３７】
第１の立体物候補抽出部２４は、レーザレーダ１２の観測データが示す各反射点のうち、路面反射点抽出部２２で路面反射点として抽出されなかった反射点について、反射点間距離が所定距離より小さい反射点同士をグループ化し、近接する反射点群を１つの立体物とみなし、第１の立体物候補として抽出する。また、第１の立体物候補を構成する各反射点の３次元位置に基づいて、第１の立体物候補の位置を検出する。例えば、第１の立体物候補を構成する反射点群のうち高さ方向の位置が最小となる反射点の３次元位置を、その第１の立体物候補の３次元位置として検出することができる。
【００３８】
第２の立体物候補抽出部２６は、撮像装置１４で撮像された撮像画像から、垂直エッジの検出またはパターン認識により、電柱やポール等に相当する第２の立体物候補を抽出する。
【００３９】
まず、垂直エッジによる第２の立体物候補の抽出について説明する。第２の立体物候補は、電柱、ガードレールや標識の支柱等のポール、樹木などの円柱状の物体であり、さらに建物等の構造物の稜線等も含んでよい。このような立体物は垂直エッジを含む場合が多いため、撮像画像全体から垂直エッジを抽出する。抽出された垂直エッジを、エッジの長さ、傾き、エッジ勾配等に基づいてグループ化し、グループ化された垂直エッジ群を１つの立体物とみなし、第２の立体物候補として抽出する。
【００４０】
次に、パターン認識による第２の立体物候補の抽出について説明する。図５に示すように、電柱やポールなどの円柱状の立体物が路面と接地する箇所は、凹状のパターンとなる。そこで、パターン認識により、この凹状パターンに相当する部分を撮像画像から検出し、凹状パターンを含む物体を第２の立体物候補として抽出する。
【００４１】
第２の立体物候補抽出部２６では、上記の垂直エッジの検出及びパターン認識の少なくとも一方を実行する。
【００４２】
路面接地点算出部２８は、レーザレーダ１２及び撮像装置１４の姿勢に基づいて、第１の立体物候補と第２の立体物候補との対応を取る。具体的には、車両を中心とする３次元座標空間に、抽出された第１の立体物候補及び第２の立体物候補を投影し、第２の立体物候補の水平方向の位置を手掛かりに奥行き方向に探索して対応する第１の立体物候補を検出することにより、第１の立体物候補と第２の立体物候補との対応を取る。図６（ａ）に第２の立体物候補の抽出例、同図（ｂ）に第１の立体物候補の抽出例を示す。ここでは、説明のため、同図（ｂ）は、高さ方向の情報を削除した鳥瞰表示で表している。各図において１〜５の番号が付された立体物候補が各々対応している。なお、同図（ｂ）において、矩形枠で囲まれた反射点群が、グループ化された第１の立体物候補を表している。
【００４３】
また、路面接地点算出部２８は、立体物と路面との接地点（以下、「路面接地点」という）を、撮像画像上から検出する。具体的には、第２の立体物候補を垂直エッジの検出により抽出した場合には、垂直エッジの最下端を路面接地点として検出し、パターン認識により抽出した場合には、第２の立体物候補と路面とが交差する点、すなわち凹状パターンの一部を路面接地点として検出する。
【００４４】
また、路面接地点算出部２８は、第２の立体物候補求まる３次元座標空間の原点から路面接地点へ向かう方向、及び路面接地点を検出した第２の立体物候補に対応する第１の立体物候補と自車両との距離に基づいて、路面接地点の３次元位置を算出する。なお、第2の立体物候補からは、接地点への方向ベクトルが算出可能である。レーザレーダ１２は、距離の測定精度は高いが、特に遠方の計測点における解像度が低下するため、撮像画像上で検出された路面接地点の情報と照合することにより、精度良く路面接地点の３次元位置を算出することができる。
【００４５】
図７に示すように、例えば、車両を中心とする３次元座標空間の奥行き方向の軸を撮像装置１４の光軸、原点を撮像装置１４の撮像面とした場合には、車両を中心とする３次元座標空間の奥行き方向の軸（光軸）と、３次元座標空間の原点（撮像装置１４）と路面接地点とを結ぶ線分とのなす角は、水平方向の角度θＨ、及び鉛直方向の角度θＶとして求めることができる。水平方向の角度θＨは、撮像装置１４の光軸を含む鉛直面と、撮像装置１４と路面接地点とを結ぶ線分を含む鉛直面とのなす角であり、鉛直方向の角度θＶは、鉛直面に直交し、かつ撮像装置１４の光軸を含む平面と、鉛直面に直交し、かつ撮像装置１４と路面接地点とを結ぶ線分を含む平面とのなす角である。また、第１の立体物候補と自車両との距離は、第１の立体物候補抽出部２４で検出された第１の立体物候補の３次元位置に基づいて算出することができる。
【００４６】
道路勾配推定部３０は、路面接地点算出部２８で算出された路面接地点の３次元位置、及び路面反射点抽出部２２で抽出された路面反射点の３次元位置に基づいて、路面接地点及び路面反射点を、車両を中心とした座標系にプロットし、最小二乗法などの既知の技術により路面モデルをフィッティングして、道路形状を推定する。説明の簡略化のため、道路平面の勾配を推定する場合について具体的に説明する。図８に示すように、車両前後方向（ｚ軸）、及び高さ方向（ｙ軸）の２軸で構成される平面に、路面接地点及び路面反射点を投影する。この点群に、下記（１）式で表されるクロソイド曲線を路面モデルとしてフィッティングする。
【００４７】
【数１】
【００４８】
ただし、Ｃ０は曲率、Ｃ１は曲率の変化率、Ｃ２はｙ軸方向のオフセット、θｐはｚ＝０におけるｙｚ平面内での路面角度である。そして、投影された全ての点を用いて最小二乗法で４つのパラメータを最適推定する。推定した道路勾配は、他の運転支援システムや車両制御システムへ出力する。
【００４９】
なお、観測点が多い場合には、ランダムサンプリングによる観測点の選択、及び選択した観測点に基づくパラメータ推定を数回繰り返すようにしてもよい。これにより、計算量を抑えながら安定した推定結果を得られる。また、路面モデルは、クロソイド曲線に限定されない。また、連続関数である必要はなく、不連続な折れ線モデルでフィッティングするようにしてもよい。
【００５０】
次に、第１の実施の形態に係る道路勾配推定装置１０の作用について説明する。
【００５１】
まず、レーザレーダ１２によって、複数の走査ラインのレーザが自車両の前方を水平方向に走査されて、車両周辺の各反射点の３次元位置を特定する観測データが観測される。レーザレーダ１２によって観測される観測データは、レーザを走査する毎に得られる。また、レーザレーダの観測タイミングに同期して、撮像装置１４により車両周辺が撮像される。そして、コンピュータ２０によって、図９に示す道路勾配推定処理ルーチンが実行される。
【００５２】
ステップ１００で、レーザレーダ１２で観測された観測データ、及び撮像装置１４で撮像された撮像画像を取得する。
【００５３】
次に、ステップ１０２で、レーザ光のパルス形状や、受光したレーザ光の信号強度に基づいて、各反射点が路面反射点か否かを判定したり、各反射点の３次元位置から高さ情報を用いて路面反射点か否かを判定したり、各反射点の並び（連続性）により、路面反射点か否かを判定したりすることにより、上記ステップ１００で取得した観測データが示す各反射点の中からレーザ光が路面で反射した路面反射点を抽出する。
【００５４】
次に、ステップ１０４で、上記ステップ１００で取得した観測データが示す各反射点のうち、上記ステップ１０２で、路面反射点として抽出されなかった反射点について、反射点間距離が所定距離より小さい反射点同士をグループ化し、近接する反射点群を１つの立体物とみなし、第１の立体物候補として抽出する。また、第１の立体物候補に含まれる各反射点の３次元位置に基づいて、第１の立体物候補の位置を検出する。
【００５５】
次に、ステップ１０６で、上記ステップ１００で取得した撮像画像から、垂直エッジの検出またはパターン認識により、電柱やポール等に相当する第２の立体物候補を抽出する。
【００５６】
次に、ステップ１０８で、車両を中心とする３次元座標空間に、上記ステップ１０４で抽出されたた第１の立体物候補、及び上記ステップ１０６で抽出された第２の立体物候補を投影し、第２の立体物候補の水平方向の位置を手掛かりに奥行き方向に探索して対応する第１の立体物候補を検出することにより、第１の立体物候補と第２の立体物候補との対応を取る。
【００５７】
次に、ステップ１１０で、第２の立体物候補を垂直エッジの検出により抽出した場合には、撮像画像上の垂直エッジの最下端を、第２の立体物候補をパターン認識により抽出した場合には、第２の立体物候補と路面とが交差する点を、路面接地点として検出する。そして、車両を中心とする３次元座標空間の奥行き方向の軸と、３次元座標空間の原点と路面接地点とを結ぶ線分とのなす角、及び路面接地点を検出した第２の立体物候補に対応する第１の立体物候補と自車両との距離に基づいて、路面接地点の３次元位置を算出する。
【００５８】
次に、ステップ１１２で、上記ステップ１１０で算出された路面接地点の３次元位置、及び上記ステップ１０２で抽出された路面反射点の３次元位置に基づいて、路面接地点及び路面反射点を、車両を中心とした座標系にプロットし、最小二乗法などの既知の技術により道路平面をフィッティングして、道路形状を推定する。そして、推定結果を他の運転支援システムや車両制御システムへ出力して、処理を終了する。
【００５９】
以上説明したように、第１の実施の形態の道路勾配推定装置によれば、撮像画像から検出された路面接地点について、レーザレーダの距離情報を用いて３次元位置を算出し、この路面接地点の３次元位置を用いて道路平面を推定するため、先行車や車線境界線等の特定物体が存在しない場合でも、前方道路の勾配を精度良く推定することができる。
【００６０】
また、レーザレーダの観測データのうち、路面反射点の３次元位置もあわせて用いるため、観測点の情報を増加させることができ、推定精度を向上させることができる。
【００６１】
次に、第２の実施の形態について説明する。なお、第２の実施の形態の道路勾配推定装置について、第１の実施の形態の道路勾配推定装置１０と同様の構成については、同一の符号を付して、詳細な説明は省略する。
【００６２】
図１０に示すように、第２の実施の形態に係る道路勾配推定装置２１０は、レーザレーダ１２と、撮像装置１４と、車速センサ及びジャイロセンサ等を含んで構成された車両状態センサ１６と、コンピュータ２２０と、を備えている。このコンピュータ２２０をハードウエアとソフトウエアとに基づいて定まる機能実現手段毎に分割した機能ブロックで説明すると、路面反射点抽出部２２２と、第１の立体物候補抽出部と、第２の立体物候補抽出部２６と、路面接地点算出部２２８と、道路勾配推定部３０と、路面接地点及び路面反射点の３次元位置情報等を記憶する情報蓄積部３２と、車両状態センサ１６の検出値、現在及び過去のレーザレーダ１２の観測値に基づいて、自車両の運動を推定する自車運動推定部３４と、情報蓄積部３２に蓄積された過去の路面接地点及び路面反射点の３次元位置（以下、「蓄積データ」ともいう）を、推定された自車両の運動に基づいて、現在の座標系に変換する蓄積データ座標変換部３６と、を含んだ構成で表すことができる。
【００６３】
なお、路面反射点抽出部２２２及び第１の立体物候補抽出部２２４が、本発明の第１の抽出手段の一例であり、自車運動推定部３４及び蓄積データ座標変換部３６が、本発明の変換手段の一例である。
【００６４】
自車運動推定部３４は、情報蓄積部３２に蓄積された過去の観測データが示す点群と、現在観測された観測データが示す点群とを照合して、観測間の自車両の運動量を推定する。点群の照合による運動量の推定には、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｔ Ｐｏｉｎｔ）アルゴリズムを用いたり、点群から複数のプロファイル特徴を抽出し、それらの位置が全体的に最も一致するように推定したりすることができる。過去の点群と現在の点群との照合には、車両状態センサ１６の検出値である自車両の車速や姿勢角等を自車運動の初期値として利用することができる。これにより、照合の計算量を削減することができる。
【００６５】
また、自車運動推定部３４は、過去の点群と現在の点群とを照合する際に、一致する点群は静止物、一致しない点群は移動物または路面点と判定することができる。この判定結果を路面反射点の抽出や第１の立体物候補の抽出に利用するために、路面反射点抽出部２２２及び第１の立体物候補抽出部２２４へ出力する。
【００６６】
路面反射点抽出部２２２は、第１の実施の形態の路面反射点抽出部２２と同様に、レーザレーダ１２の観測結果に基づいて、観測データが示す各反射点の中からレーザ光が路面で反射した路面反射点を抽出する。その際、自車運動推定部３４から出力された判定結果も利用して、移動物または路面点と判定された反射点群（過去の点群と現在の点群が一致しなかった点群）から路面反射点を抽出する。また、抽出した路面反射点の３次元位置を情報蓄積部３２に保存する。
【００６７】
第１の立体物候補抽出部２２４は、第１の実施の形態の第１の立体物候補抽出部２４と同様に、レーザレーダ１２の観測データが示す各反射点のうち、路面反射点抽出部２２２で路面反射点として抽出されなかった反射点について、反射点間距離が所定距離より小さい反射点同士をグループ化し、近接する反射点群を１つの立体物とみなし、第１の立体物候補として抽出する。その際、自車運動推定部３４から出力された判定結果も利用して、静止物と判定された反射点群（過去の点群と現在の点群が一致した点群）の情報に基づいて、第１の立体物候補を構成する反射点とするか否かを再抽出したり、反射点群のグループ化をしたりする。また、第１の立体物候補を構成する各反射点の３次元位置に基づいて、第１の立体物候補の位置を検出する。
【００６８】
路面接地点算出部２２８は、第１の実施の形態の路面接地点算出部２８と同様に、第１の立体物候補と第２の立体物候補との対応を取り、路面接地点を検出して、路面接地点の３次元位置を算出する。また、算出した路面接地点の３次元位置を情報蓄積部３２に保存する。この際、既に情報蓄積部３２に保存されている蓄積データと現在の路面接地点の３次元位置とを照合し、照合が取れた路面接地点のデータに、照合回数が多いほど大きくなる信頼度を付与する。例えば、後述の蓄積データ座標変換部３６の処理により、情報蓄積部３２に保存された蓄積データの座標系を現在の座標系に変換し、その座標系を細かい格子状のセルに区画する。そして、既に蓄積データが観測されているセルに、現在の路面接地点の３次元位置が検出された場合には、そのセルに対応する信頼度をカウントアップする。なお、セルの大きさは、同一のセルに異なる路面点を示す点がプロットされないように、レーザレーダ１２の解像度を考慮して、十分細かくする。これにより、信頼性の高いデータと信頼性の低いデータとを判別することができる。なお、路面反射点抽出部２２２において、抽出された路面反射点を情報蓄積部３２に保存する際にも、同様に、信頼度を付与するようにしてもよい。
【００６９】
蓄積データ座標変換部３６は、推定された自車両の運動に基づいて、情報蓄積部３２に蓄積された蓄積データを現在の座標系に変換する。例えば、現在の時刻ｔの一時刻前（ｔ−１）の蓄積データのうち、信頼度が所定値以上の蓄積データを情報蓄積部３２から読み出して、時刻（ｔ−１）における車両を中心とする３次元座標空間にプロットする。そして、自車運動推定部３４で推定された時刻（ｔ−１）〜時刻ｔ間の自車両の運動量を取得して、時刻（ｔ−１）における３次元座標空間を、時刻ｔにおける３次元空間座標に変換する。
【００７０】
道路勾配推定部２３０は、第１の実施の形態の道路勾配推定部３０と同様に、路面接地点算出部２２８で算出された現在の路面接地点、及び路面反射点抽出部２２２で抽出された現在の路面反射点（以下、「現在のデータ」ともいう）を、車両を中心とした座標系にプロットすると共に、蓄積データ座標変換部３６により座標変換された蓄積データも、同じ座標系にプロットして、現在のデータと蓄積データとを照合して、一致するとみなせる点を統合する。例えば、上述のように、データをプロットする座標系を、細かい格子状のセルに区画し、同一のセルにプロットされた現在のデータ及び蓄積データについて、いずれかのデータを削除するか、両データの平均をとるなどして、１つのデータに統合する。セルにプロットされデータが１つの場合には、そのデータをそのまま採用する。
【００７１】
また、道路勾配推定部２３０は、上記のように、現在のデータ及び蓄積データを、車両を中心とした座標系にプロットして統合した後に、第１の実施の形態と同様に、最小二乗法などの既知の技術により道路平面をフィッティングして、道路形状を推定する。
【００７２】
次に、第２の実施の形態に係る道路勾配推定装置２１０の作用について説明する。
【００７３】
まず、レーザレーダ１２によって、複数の走査ラインのレーザが自車両の前方を水平方向に走査されて、車両周辺の各反射点の３次元位置を特定する観測データが観測される。レーザレーダ１２によって観測される観測データは、レーザを走査する毎に得られる。また、レーザレーダの観測タイミングに応じて、撮像装置１４により車両周辺が撮像される。そして、コンピュータ２０によって、図１１に示す道路勾配推定処理ルーチンが実行される。なお、第１の実施の形態の道路勾配推定処理と同一の処理については、同一の符号を付して詳細な説明は省略する。
【００７４】
ステップ２００で、レーザレーダ１２で観測された観測データ、撮像装置１４で撮像された撮像画像、及び車両状態センサ１６の検出値を取得する。
【００７５】
次に、ステップ２０２で、情報蓄積部３２に蓄積された過去の観測データが示す点群と、上記ステップ２００で取得された観測データが示す点群とを照合して、観測間の自車両の運動量を推定する。ここでは、時刻（ｔ−１）〜時刻ｔ間の自車両の運動量を推定するものとする。過去の点群と現在の点群との照合には、上記ステップ２００で取得した車両状態センサ１６の検出値である自車両の車速や姿勢角等を自車運動の初期値として利用する。そして、過去の点群と現在の点群とを照合する際に、一致する点群は静止物、一致しない点群は移動物または路面点と判定する。
【００７６】
次に、ステップ２０４で、上記ステップ２０２で移動物または路面点と判定された反射点の中から、受光パルスの形状や信号強度、高さ情報、反射点の連続性等に基づいて、レーザ光が路面で反射した路面反射点を抽出する。
【００７７】
次に、ステップ２０６で、上記ステップ２０４で抽出された路面反射点の３次元位置と、情報蓄積部３２に保存された蓄積データとを照合して、照合結果に応じて信頼度を更新した上で、路面反射点の３次元位置を情報蓄積部３２に保存する。
【００７８】
次に、ステップ２０８で、上記ステップ２００で取得した観測データが示す各反射点のうち、上記ステップ２０４で、路面反射点として抽出されなかった反射点であって、上記ステップ２０２で静止物と判定された反射点について、反射点間距離が所定距離より小さい反射点同士をグループ化して、第１の立体物候補として抽出する。また、第１の立体物候補に含まれる各反射点の３次元位置に基づいて、第１の立体物候補の位置を検出する。
【００７９】
次に、ステップ１０６で、上記ステップ２００で取得した撮像画像から、第２の立体物候補を抽出し、次に、ステップ１０８で、第１の立体物候補と第２の立体物候補との対応を取り、次に、ステップ１１０で、路面接地点の３次元位置を算出する。
【００８０】
次に、ステップ２０９で、上記ステップ１１０で算出された路面接地点の３次元位置と、情報蓄積部３２に保存された蓄積データとを照合して、照合結果に応じて信頼度を更新した上で、路面接地点の３次元位置を情報蓄積部３２に保存する。
【００８１】
次に、ステップ２１２で、現在の時刻ｔの一時刻前（ｔ−１）の蓄積データのうち、信頼度が所定値以上の蓄積データを情報蓄積部３２から読み出して、時刻（ｔ−１）における車両を中心とする３次元座標空間にプロットする。そして、上記ステップ２０２で推定された時刻（ｔ−１）〜時刻ｔ間の自車両の運動量に基づいて、時刻（ｔ−１）における３次元座標空間を、時刻ｔにおける３次元空間座標に変換する。
【００８２】
次に、ステップ２１４で、上記ステップ１１０で算出された路面接地点の３次元位置、上記ステップ２０４で抽出された路面反射点の３次元位置、及び上記ステップ２１２で座標変換された蓄積データに基づいて、路面接地点、路面反射点、及び蓄積データを、車両を中心とした座標系にプロットし、最小二乗法などの既知の技術により道路平面をフィッティングして、道路形状を推定する。そして、推定結果を他の運転支援システムや車両制御システムへ出力して、処理を終了する。
【００８３】
以上説明したように、第２の実施の形態の道路勾配推定装置によれば、過去に算出された路面接地点の３次元位置、及び過去に抽出された路面反射点の３次元位置も用いて、道路平面を推定するため、観測点の情報を増加させることができ、より推定精度を向上させることができる。
【００８４】
また、過去のデータを用いる際に、現在のデータとの照合回数に応じた信頼度が所定値以上のデータを用いることで、観測の度に出現したり消滅したりする観測ノイズの影響を低減して、より推定精度を向上させることができる。
【００８５】
なお、第２の実施の形態では、レーザレーダによる現在及び過去の観測データの照合により、観測間の自車両の運動量を推定する場合について説明したが、自車両の運動量を、車両状態センサの検出値に基づいて推定してもよいし、撮像画像から推定してもよい。
【００８６】
また、第２の実施の形態では、現在のデータに加えて、信頼度が所定値以上の蓄積データを用いる場合について説明したが、最小二乗法等で路面モデルをフィッティングする際に、誤差に掛ける重みとして、この信頼度を利用するようにしてもよい。
【００８７】
また、上記実施の形態では、レーザレーダにより車両周辺から複数の反射点の位置を特定する情報を観測する場合について説明したが、これに限定されるものではなく、ミリ波などの電磁波を用いてもよい。また、例えば、ステレオカメラによって撮像された画像を用いて、後処理により車両周辺の複数の点の位置を特定する情報を演算するようにしてもよい。
【符号の説明】
【００８８】
１０、２１０ 道路勾配推定装置
１２ レーザレーダ
１４ 撮像装置
１６ 車両状態センサ
２０、２２０ コンピュータ
２２、２２２ 路面反射点抽出部
２４、２２４ 第１の立体物候補抽出部
２６ 第２の立体物候補抽出部
２８、２２８ 路面接地点算出部
３０、２３０ 道路勾配推定部
３２ 情報蓄積部
３４ 自車運動推定部
３６ 蓄積データ座標変換部
【特許請求の範囲】
【請求項１】
移動体周辺の複数の点各々の前記移動体を基準とする３次元位置、及び前記移動体周辺を該移動体に搭載された撮像手段により撮像した画像を取得する取得手段と、
前記取得手段により取得された前記複数の点各々の３次元位置に基づいて、立体物を示す点を第１の立体物候補として抽出する第１の抽出手段と、
前記取得手段により取得された画像の垂直エッジ情報及び予め定めた形状とのマッチング情報の少なくとも一方に基づいて、第２の立体物候補を抽出する第２の抽出手段と、
前記第２の立体物候補に対応する前記第１の立体物候補の３次元位置に基づいて、該第２の立体物候補の路面との接地点の３次元位置を算出する算出手段と、
前記算出手段により算出された接地点の３次元位置に基づいて、前記移動体周辺の道路勾配を推定する勾配推定手段と、
を含む道路勾配推定装置。
【請求項２】
前記算出手段は、前記第２の立体物候補から求まる３次元座標空間の原点から前記接地点へ向かう方向、及び前記対応する第１の立体物候補の３次元位置に基づいて、前記接地点の３次元位置を算出する請求項１記載の道路勾配推定装置。
【請求項３】
所定期間の前記移動体の運動量に基づいて、過去に算出された前記接地点の３次元位置を現在の座標系に変換する変換手段を含み、
前記勾配推定手段は、前記変換手段により変換された接地点の３次元位置も加えた分布に基づいて、前記道路勾配を推定する
請求項１または請求項２記載の道路勾配推定装置。
【請求項４】
前記算出手段は、現在算出された接地点の３次元位置と過去に算出された接地点の３次元位置との照合結果に基づく信頼度を付与して、該現在算出された接地点の３次元位置を所定の記憶領域に記憶し、
前記勾配推定手段は、前記所定の記憶領域に記憶された接地点の３次元位置のうち、前記信頼度が所定値以上の接地点の３次元位置を前記変換手段により変換した３次元位置を加えるか、または、前記所定の記憶領域に記憶された接地点の３次元位置を前記変換手段により変換して、前記信頼度に応じた重み付けをして加える
請求項３記載の道路勾配推定装置。
【請求項５】
前記第１の抽出手段は、前記複数の点各々から路面点を抽出し、該路面点以外の点を前記第１の立体物候補として抽出し、
前記勾配推定手段は、前記路面点の３次元位置も加えた３次元位置の分布に基づいて、前記道路勾配を推定する
請求項１または請求項２記載の道路勾配推定装置。
【請求項６】
前記第１の抽出手段は、前記複数の点各々から路面点を抽出し、該路面点以外の点を前記第１の立体物候補として抽出し、
前記勾配推定手段は、前記路面点の３次元位置も加えた３次元位置の分布に基づいて、前記道路勾配を推定する
請求項３または請求項４記載の道路勾配推定装置。
【請求項７】
前記変換手段は、所定期間の前記移動体の運動量に基づいて、過去に抽出された前記路面点の３次元位置を現在の座標系に変換し、
前記勾配推定手段は、前記変換手段により変換された路面点の３次元位置も加えた分布に基づいて、前記道路勾配を推定する
請求項６記載の道路勾配推定装置。
【請求項８】
前記第２の抽出手段は、前記画像から垂直エッジを抽出し、電柱、ポール、樹木、及び建築物の少なくとも１つを示す垂直エッジを、前記第２の立体物候補として抽出し、
前記算出手段は、前記垂直エッジの最下端を前記接地点として検出する
請求項１〜請求項７のいずれか１項記載の道路勾配推定装置。
【請求項９】
前記第２の抽出手段は、パターンマッチングにより、前記画像から円柱状の物体を前記第２の立体物候補として抽出し、
前記算出手段は、前記円柱状の物体と路面とが交差する点を前記接地点として検出する
請求項１〜請求項８のいずれか１項記載の道路勾配推定装置。
【請求項１０】
前記複数の点各々の３次元位置を、複数の走査ラインを有するレーザレーダであって、該走査ラインの少なくとも１つが路面に照射されるように構成されたレーザレーダにより計測する請求項１〜請求項９のいずれか１項記載の道路勾配推定装置。
【請求項１１】
コンピュータを、
移動体周辺の複数の点各々の前記移動体を基準とする３次元位置、及び前記移動体周辺を該移動体に搭載された撮像手段により撮像した画像を取得する取得手段、
前記取得手段により取得された前記複数の点各々の３次元位置に基づいて、立体物を示す点を第１の立体物候補として抽出する第１の抽出手段、
前記取得手段により取得された画像の垂直エッジ情報及び予め定めた形状とのマッチング情報の少なくとも一方に基づいて、第２の立体物候補を抽出する第２の抽出手段と、
前記第２の立体物候補に対応する前記第１の立体物候補の３次元位置に基づいて、該第２の立体物候補の路面との接地点の３次元位置を算出する算出手段、及び
前記算出手段により算出された接地点の３次元位置に基づいて、前記移動体周辺の道路勾配を推定する勾配推定手段
として機能させるための道路勾配推定プログラム。
【請求項１２】
コンピュータを、請求項１〜請求項１０のいずれか１項記載の道路勾配推定装置を構成する各手段として機能
させるための道路勾配推定プログラム。
【請求項１】
移動体周辺の複数の点各々の前記移動体を基準とする３次元位置、及び前記移動体周辺を該移動体に搭載された撮像手段により撮像した画像を取得する取得手段と、
前記取得手段により取得された前記複数の点各々の３次元位置に基づいて、立体物を示す点を第１の立体物候補として抽出する第１の抽出手段と、
前記取得手段により取得された画像の垂直エッジ情報及び予め定めた形状とのマッチング情報の少なくとも一方に基づいて、第２の立体物候補を抽出する第２の抽出手段と、
前記第２の立体物候補に対応する前記第１の立体物候補の３次元位置に基づいて、該第２の立体物候補の路面との接地点の３次元位置を算出する算出手段と、
前記算出手段により算出された接地点の３次元位置に基づいて、前記移動体周辺の道路勾配を推定する勾配推定手段と、
を含む道路勾配推定装置。
【請求項２】
前記算出手段は、前記第２の立体物候補から求まる３次元座標空間の原点から前記接地点へ向かう方向、及び前記対応する第１の立体物候補の３次元位置に基づいて、前記接地点の３次元位置を算出する請求項１記載の道路勾配推定装置。
【請求項３】
所定期間の前記移動体の運動量に基づいて、過去に算出された前記接地点の３次元位置を現在の座標系に変換する変換手段を含み、
前記勾配推定手段は、前記変換手段により変換された接地点の３次元位置も加えた分布に基づいて、前記道路勾配を推定する
請求項１または請求項２記載の道路勾配推定装置。
【請求項４】
前記算出手段は、現在算出された接地点の３次元位置と過去に算出された接地点の３次元位置との照合結果に基づく信頼度を付与して、該現在算出された接地点の３次元位置を所定の記憶領域に記憶し、
前記勾配推定手段は、前記所定の記憶領域に記憶された接地点の３次元位置のうち、前記信頼度が所定値以上の接地点の３次元位置を前記変換手段により変換した３次元位置を加えるか、または、前記所定の記憶領域に記憶された接地点の３次元位置を前記変換手段により変換して、前記信頼度に応じた重み付けをして加える
請求項３記載の道路勾配推定装置。
【請求項５】
前記第１の抽出手段は、前記複数の点各々から路面点を抽出し、該路面点以外の点を前記第１の立体物候補として抽出し、
前記勾配推定手段は、前記路面点の３次元位置も加えた３次元位置の分布に基づいて、前記道路勾配を推定する
請求項１または請求項２記載の道路勾配推定装置。
【請求項６】
前記第１の抽出手段は、前記複数の点各々から路面点を抽出し、該路面点以外の点を前記第１の立体物候補として抽出し、
前記勾配推定手段は、前記路面点の３次元位置も加えた３次元位置の分布に基づいて、前記道路勾配を推定する
請求項３または請求項４記載の道路勾配推定装置。
【請求項７】
前記変換手段は、所定期間の前記移動体の運動量に基づいて、過去に抽出された前記路面点の３次元位置を現在の座標系に変換し、
前記勾配推定手段は、前記変換手段により変換された路面点の３次元位置も加えた分布に基づいて、前記道路勾配を推定する
請求項６記載の道路勾配推定装置。
【請求項８】
前記第２の抽出手段は、前記画像から垂直エッジを抽出し、電柱、ポール、樹木、及び建築物の少なくとも１つを示す垂直エッジを、前記第２の立体物候補として抽出し、
前記算出手段は、前記垂直エッジの最下端を前記接地点として検出する
請求項１〜請求項７のいずれか１項記載の道路勾配推定装置。
【請求項９】
前記第２の抽出手段は、パターンマッチングにより、前記画像から円柱状の物体を前記第２の立体物候補として抽出し、
前記算出手段は、前記円柱状の物体と路面とが交差する点を前記接地点として検出する
請求項１〜請求項８のいずれか１項記載の道路勾配推定装置。
【請求項１０】
前記複数の点各々の３次元位置を、複数の走査ラインを有するレーザレーダであって、該走査ラインの少なくとも１つが路面に照射されるように構成されたレーザレーダにより計測する請求項１〜請求項９のいずれか１項記載の道路勾配推定装置。
【請求項１１】
コンピュータを、
移動体周辺の複数の点各々の前記移動体を基準とする３次元位置、及び前記移動体周辺を該移動体に搭載された撮像手段により撮像した画像を取得する取得手段、
前記取得手段により取得された前記複数の点各々の３次元位置に基づいて、立体物を示す点を第１の立体物候補として抽出する第１の抽出手段、
前記取得手段により取得された画像の垂直エッジ情報及び予め定めた形状とのマッチング情報の少なくとも一方に基づいて、第２の立体物候補を抽出する第２の抽出手段と、
前記第２の立体物候補に対応する前記第１の立体物候補の３次元位置に基づいて、該第２の立体物候補の路面との接地点の３次元位置を算出する算出手段、及び
前記算出手段により算出された接地点の３次元位置に基づいて、前記移動体周辺の道路勾配を推定する勾配推定手段
として機能させるための道路勾配推定プログラム。
【請求項１２】
コンピュータを、請求項１〜請求項１０のいずれか１項記載の道路勾配推定装置を構成する各手段として機能
させるための道路勾配推定プログラム。
【図１】
【図２】
【図３】
【図５】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図４】
【図６】
【図２】
【図３】
【図５】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図４】
【図６】
【公開番号】特開２０１２−２２５８０６（Ｐ２０１２−２２５８０６Ａ）
【公開日】平成２４年１１月１５日（２０１２．１１．１５）
【国際特許分類】
【出願番号】特願２０１１−９４２５６（Ｐ２０１１−９４２５６）
【出願日】平成２３年４月２０日（２０１１．４．２０）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２４年１１月１５日（２０１２．１１．１５）
【国際特許分類】
【出願日】平成２３年４月２０日（２０１１．４．２０）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]