JP6221607B2

JP6221607B2 - 物体検出装置

Info

Publication number: JP6221607B2
Application number: JP2013211340A
Authority: JP
Inventors: 直己二反田; 一樹加藤
Original assignee: Denso Corp
Current assignee: Denso Corp
Priority date: 2013-10-08
Filing date: 2013-10-08
Publication date: 2017-11-01
Anticipated expiration: 2033-10-08
Also published as: JP2015075382A

Description

本発明は、車両に搭載され、レーザレーダの取付姿勢と、レーザレーダから照射されるレーザ光の照射角度とに基づいてレーザレーダが検出した物体の相対位置を算出する物体検出装置に関する。

従来、スキャン型のレーザレーダを用いて車両周辺に存在する歩行者や先行車両などを検出する物体検出装置がある。この種の物体検出装置では、レーザレーダが照射したレーザ光が、物体（これを測距点とする）で反射されて返ってくるまでに要する時間を計測することで、レーザレーダから測距点までの距離を測定する。

なお、物体検出装置で用いられるレーザレーダは、レーザ光を車幅方向および車高方向において、それぞれ所定の角度範囲で、例えば一定角度毎にパルス状のレーザ光を掃引照射（すなわちスキャン）する。

物体検出装置は、測距点までの距離、当該測距点を検出したレーザ光の照射角度、及びレーザレーダの取付姿勢から、レーザレーダに対する測距点の相対位置を算出する。ここでいう取付姿勢とは、レーザレーダが搭載されている位置の路面からの高さや、レーザレーダの路面に対するピッチ角、ロール角、ヨー角を指す。また、照射角度とは、照射されたレーザ光がレーザレーダの正面方向と為す角度を指す。

ところで、一般に物体検出装置は、レーザレーダが予め設計された取付姿勢であることを前提として相対的な位置を算出する。このため、レーザレーダの取付姿勢が予め設計された取付姿勢と一致していない場合には、物体検出装置が検出する測距点の相対位置が、実際の相対位置からずれてしまうといった問題が生じる。

この問題を解決する手段として、例えば特許文献１には、車両に搭載されるレーザレーダの取付姿勢を調整する技術が開示されている。特許文献１では、製造ラインなどにおいて、レーザレーダが正確な取付姿勢となっている場合において正面方向となる位置（以下、基準位置）にリフレクタを設置しておく。そして、レーザレーダが当該リフレクタをこの基準位置に検出するように取付姿勢を調整する。

特開平８−２９５３６号公報

しかしながら、特許文献１に開示の技術では、レーザレーダに対するリフレクタ等の試験用の設備の設置位置など、試験環境自体の精度も要求される。また、その試験結果を受けて取付姿勢を調整する必要があるため、製造ラインにおいて、取付姿勢の調整に要する時間や人員、さらには、試験環境自体を調整するためのコストがかかってしまう。

また、レーザレーダが正確に車両に設置されている場合であっても、タイヤの空気圧の変化や、夏タイヤ／冬タイヤの交換によって路面に対する車両の姿勢（すなわち、車体の傾き）は変化する。さらに、乗員の乗車位置や、積載物などによっても、路面に対する車両の姿勢は変化する。路面に対する車両の姿勢が変化してしまうと、これに伴ってレーザレーダの路面に対する姿勢（すなわち取付姿勢）も変化するため、測距点の相対位置を正確に算出できなくなってしまう。

本発明は、以上の事情を鑑みて為されたものであって、その目的とするところは、製造ラインにおける取付姿勢の調整を簡略化でき、かつ、路面に対する車両の姿勢の変化の影響を低減できる物体検出装置を提供することにある。

その目的を達成するための本発明は、車両に搭載され、垂直方向において複数の異なる角度毎に、水平方向において所定の角度範囲でレーザ光を逐次掃引照射するレーザレーダ（１）から入力される信号に基づいて、レーザレーダからレーザ光を反射した測距点までの距離を算出する距離算出部（２１）と、路面に対するレーザレーダの取付姿勢を記憶している記憶部（２０）と、記憶部に記憶されている取付姿勢と、レーザ光を照射した方向と、距離算出部が算出した距離とに基づいて、測距点の車両に対する相対位置を検出する位置算出部（２２）と、取付姿勢を推定する取付姿勢推定部（２４）と、を備え、記憶部には、取付姿勢として所定の初期設定値が予め登録されてあって、取付姿勢推定部は、垂直方向におけるレーザ光の照射方向である照射仰俯角毎に、水平方向におけるレーザ光の照射方向である照射方位角を変化させることで得られる複数の測距点について、測距点との距離と照射方位角とを対応付けた一次元信号を生成する一次元信号生成部（Ｓ３０３）と、一次元信号生成部が生成した一次元信号の周波数成分に基づいて、一次元信号の要素となっている測距点のうち、路面を検出している測距点である路面候補点を抽出する路面候補点抽出部（Ｓ３）と、路面候補点抽出部で抽出された複数の路面候補点に基づいて、路面を表す平面である路面平面を算出する路面平面算出部（Ｓ４）と、路面平面算出部が算出した路面平面と、取付姿勢から定まる路面を表す平面である基準面とが為す角度と、から、取付姿勢を推定する第１取付姿勢推定処理部（Ｓ５）と、第１取付姿勢推定処理部で推定した取付姿勢に基づいて、記憶部に記憶されている取付姿勢を更新する第１取付姿勢更新部（Ｓ８）と、を備えることを特徴とする。

以上の構成では、レーザレーダが掃引照射することによって得られる複数の測距点から、路面候補点抽出部が、路面を検出している路面候補点を抽出する。そして、複数の路面候補点から算出される路面平面と、基準面とが為す角度から、第１取付姿勢推定処理部がレーザレーダの取付姿勢を推定し、取付姿勢更新部が記憶部に記憶されている取付姿勢を更新する。

以上の構成によれば、位置算出部は、取付姿勢更新部で更新された取付姿勢を用いて、相対位置を算出するため、設計値との間に誤差があった場合でも、測距点の相対位置をより正確に取得することができる。

言い換えれば、予め記憶部に記憶されている取付姿勢と実際の取付姿勢との間の誤差が、測距点の相対位置の検出精度に与える影響を低減することができる。このため、製造ラインでの厳密な取付姿勢の調整を簡略化することが出来るとともに、試験環境の精度の要件を緩和することが出来る。したがって、取付姿勢の調整に伴うコストを低減することが出来る。

また、取付姿勢推定部が実施する取付姿勢の推定は、リフレクタなどの試験用の設備を用いずに実施することができるため、車両走行時において実施することが出来る。すなわち、タイヤの空気圧の変化や、乗員の乗車位置や、積載物などによって、路面に対する車両の姿勢は変化した場合であっても、その変化した車両の姿勢における取付姿勢を推定することが出来る。そして、位置算出部は、その推定された取付姿勢を用いて相対位置を算出するため、相対位置の算出における路面に対する車両の姿勢の変化の影響を低減できる。

本実施形態に係るシステム構成の概略を示すブロック図である。高さ方向においてレーザレーダ１が照射するレーザ光の照射方向を説明するための図である。レーザレーダ１の検知エリアを説明するための図である。取付姿勢推定処理の流れの一例を示すフローチャートである。路面候補点抽出処理の流れの一例を示すフローチャートである。路面候補点抽出処理の作動を説明するための模式図である。路面推定処理の流れの一例を示すフローチャートである。路面推定処理の作動を説明するための概念図である。ピッチ角θｐを推定する処理を説明するための模式図である。ピッチ角θｐを推定する処理を説明するための模式図である。ロール角θｒを推定する処理を説明するための模式図である。ロール角θｒを推定する処理を説明するための模式図である。搭載高さを推定する処理を説明するための模式図である。安定性評価処理の流れの一例を示すフローチャートである。安定性評価処理の作動を説明するための概念図である。ヨー角θｙを推定する処理を説明するための概念図である。前方領域Ｆｒ、後方領域Ｒｒ、右側方領域Ｒｔ、左側方領域Ｌｆの一例を示す概念図である。車両前後方向における勾配の変化量を説明するための模式図である。応用例２の効果を説明するための図である。変形例２における路面候補点抽出処理の流れの一例を示すフローチャートである。

＜全体の構成＞
以下、本発明の実施形態の一例について図を用いて説明する。図１に、本発明が適用された物体検出システム１００の構成の一例を示す。図１に示す物体検出システム１００は、車両５に搭載され、車両５の左右前後など全方位に存在する物体との距離を検出する。物体検出システム１００は、レーザレーダ１及びＥＣＵ（ＥｌｅｃｔｒｏｎｉｃＣｏｎｔｒｏｌＵｎｉｔ）２を備えている。

また、図１中の運転支援システム３は、物体検出システム１００の検出結果を利用する機能の一例である。運転支援システム３は、物体検出システム１００の検出結果に基づいて、ドライバの運転を支援する種々の制御を実施する（詳細は後述）。もちろん、運転支援システム３に限らず、車両に搭載される種々のシステムが、物体検出システム１００の検出結果を利用することができる。

レーザレーダ１は、モータなどからなる回転機構を備え、当該回転機構を回転させることによって、車両高さ方向から見た時の、車両５の前後方向に対するレーザ光の照射方向を変化させる。すなわち、レーザレーダ１は、この回転機構を回転させることによって、車両周囲３６０°（すなわち、全方位）に向けてレーザ光を照射する。回転機構の回転軸は、レーザレーダ１の中心軸とも称する。また、回転軸方向は、高さ方向と一致する。なお、ここでの一致とは、完全な一致だけでなく、略一致も含む。これは、後述するように路面６に対するレーザレーダ１の姿勢によって、ずれが生じるためである。

また、回転機構が中立位置（すなわち、回転角度が０°）となっている場合の照射方向をレーザレーダ１の正面方向とし、レーザレーダ１の前後方向とも称する。また、レーザレーダ１の前後方向及び回転軸方向のそれぞれと直交する方向を、レーザレーダ１の左右方向とする。すなわち、レーザレーダ１の左右方向は、回転機構の回転角度が９０°（および２７０°）となっている場合の照射方向と一致する。

一例として本実施形態では、レーザレーダ１の正面方向と車両５の正面方向とを一致（前述と同様に略一致を含む）させる構成とするが、もちろん、他の態様として、回転機構の回転角度が１８０°となる方向を車両５の正面方向と一致させても良い。

ここで、レーザレーダ１の構成及び作動について説明する前に、その取付位置や路面６に対するレーザレーダ１の姿勢（これを取付姿勢とする）について、路面６に対する車両５の姿勢に基づいて定まる路面座標系を適宜用いて説明する。

路面座標系は、レーザレーダ１の取り付け位置の中心を、路面６に正射影した点を原点とし、車幅方向にＸ軸を、車両前後方向にＹ軸を、高さ方向にＺ軸を、それぞれ直交するように備えている。したがって、路面座標系のＸＹ平面は、路面６に相当する。

レーザレーダ１は、図２及び図３に示すように、例えば車両ルーフの上面に、所定の取付姿勢となるように設置されている。ここでいう取付姿勢とは、レーザレーダ１の路面６に対するピッチ角θｐ、ロール角θｒ、ヨー角θｙ、及び搭載高さｈを指す。搭載高さｈとは、路面６からのレーザレーダ１（厳密には発光部１１）までの高さｈを指す。

ピッチ角θｐとは、レーザレーダ１の中心軸が、路面座標系のＹＺ平面においてＺ軸との為す角を指し、ロール角θｒとは、レーザレーダ１の回転軸がＸＺ平面においてＺ軸との為す角を指す。また、ヨー角θｙとは、レーザレーダ１の正面方向がＸＹ平面においてＹ軸との為す角を指す。

また、ピッチ角θｐは、Ｚ軸方向からＹ軸方向への回転方向を正の値にとり、ロール角θｒは、Ｚ軸方向からＸ軸方向への回転方向を正の値にとる。また、ヨー角θｙは、照射方位角βと同様の回転方向を正の値とする。なお、路面座標系におけるレーザレーダ１の位置は、（０，０，ｈ）で表される。

再び図１に戻って、レーザレーダ１の構成及び作動について説明する。レーザレーダ１は、発光部１１、受光部１２、及びＴＯＦ（ＴｉｍｅｏｆＦｌｉｇｈｔ）算出部１３を備えている。

発光部１１は、レーザダイオードを備えており、ＥＣＵ２からの駆動信号に従って、回転軸方向において所定角度の範囲（これを回転軸方向照射範囲とする）で不連続にレーザ光を照射する。より具体的には、回転軸方向照射範囲をＬ個（Ｌは正の整数）の照射方向に分割してなるレイヤ毎に、パルス状のレーザ光を照射する。この回転軸方向が請求項に記載の垂直方向に相当し、回転軸方向に垂直な方向が請求項に記載の水平方向に相当する。

本実施形態において、発光部１１の回転軸方向照射範囲は、回転軸に垂直な方向（すなわち光軸の方向）に対して−１０°から＋３０°の範囲とし、当該回転軸方向照射範囲をＬ＝３２個のレイヤに分割してレーザ光を照射する。すなわちレーザ光は、レイヤ毎に、高さ方向において１．２５°ずつずれて照射される。各レイヤの番号は、回転軸方向において光軸と為す角度αが小さい順に０，１，２，…，３１とする。

なお、回転軸方向においてレーザ光が光軸と為す角度（これを照射仰俯角とする）は、回転軸方向からｙ軸方向への回転角度を正の値で表すものとする。照射仰俯角αは、レイヤ毎に定まっており、例えばレイヤ０であればα＝−１０°であり、レイヤ３１であれば、α＝３０°である。

そして、発光部１１は、回転機構が一定角度（例えば０．１°）回転する毎に、各レイヤのレーザ光を逐次照射することによって、レーザレーダ１は、レイヤ毎に、車両全方位に向けてレーザ光を掃引照射する。なお、各レイヤの回転軸方向の照射角度の概念を図２に、また、３６０°スキャンした際に、各レイヤのレーザ光が路面６上の点を検出した場合の測距点の軌跡を図３に示している。測距点とは、レーザ光を反射した点であり、レーザレーダ１によって検出された点を指す。

受光部１２は、発光部１１から照射されたレーザ光が測距点で反射された反射光を、受光レンズで受光し、受光素子（フォトダイオード）に与える。受光素子は、反射光の強度に対応する電圧を出力する。この受光素子の出力電圧は、増幅器にて増幅された後にコンパレータに出力される。コンパレータは増幅器の出力電圧を基準電圧と比較し、出力電圧が基準電圧よりも大きくなったとき、所定の受光信号をＴＯＦ算出部１３へ出力する。

ＴＯＦ算出部１３には、ＥＣＵ２から発光部１１へ出力される駆動信号も入力されている。このＴＯＦ算出部１３は、駆動信号を出力してから受光信号が発生するまでの時間、すなわちレーザ光を出射した時刻と反射光を受光した時刻との時間差（既出のＴＯＦを意味する）を計測し、この時間差を示す信号（以下、ＴＯＦ信号）をＥＣＵ２に入力する。

発光部１１がレーザ光を照射する際には、レーザ光を照射角度毎に異なるパターンで照射するように、ＥＣＵ２から発光部１１に駆動信号が出力される。すなわち、レーザ光のパターンは、照射角度毎に定められているので、反射光を受光した場合には、その反射光のパターンから、その反射光の元となっているレーザ光の照射角度を特定することができる。

レーザ光の照射角度とは、照射仰俯角αと、照射方位角βで表される。照射仰俯角αは前述の通り、回転軸方向においてレーザ光が光軸と為す角度であって、レイヤ毎に定まっている。照射方位角βとは、回転機構の中立位置からの回転角度である。照射方位角βは、図３に示すように時計回りの角度を正の値で表す。

ＥＣＵ２は、ＣＰＵ、ＲＯＭ、ＲＡＭ、バックアップＲＡＭ等よりなるマイクロコンピュータを主体として構成され、入力される情報をもとに、ＲＯＭに記憶された各種の制御プログラムを実行することで各種の処理を実行する。このＥＣＵ２が、請求項の物体検出装置に相当する。より具体的にＥＣＵ２は、図１に示すように種々の処理を実施するための機能ブロックとして、記憶部２０、距離算出部２１、位置算出部２２、車両情報取得部２３、及び取付姿勢推定部２４を備えている。

記憶部２０は、書き換え可能な不揮発性の記憶媒体で構成されている。記憶部２０には、レーザレーダ１の取付姿勢の初期値（これを初期取付姿勢とする）が登録されているとともに、後述する取付姿勢推定部２４で推定される取付姿勢など種々のデータを記憶する。記憶部２０が記憶している取付姿勢のうち、後述する位置算出部２２で用いられる取付姿勢は、第１取付姿勢更新処理（図４ステップＳ８）及び第２取付姿勢更新処理（Ｓ１２）で更新されていく。

初期取付姿勢は、例えば製造ラインなど、乗員がおらず、かつ、各タイヤの空気圧が規定値となっている状態において計測した結果を用いればよい。本実施形態では、初期取付姿勢として、ピッチ角θｐ、ロール角θｒ、ヨー角θｙは何れも、ずれが生じていない（すなわち０°）と設定する。また、初期取付姿勢としての搭載高さｈは、予め設計されている値を用いる構成とする。

なお、搭載高さｈについては、ピッチ角θｐ、ロール角θｒともに０°として、任意のレーザ光の照射仰俯角αと、当該レーザ光で検出した測距点とレーザレーダ１との距離ｒとから、ｈ＝ｒ×ｓｉｎαとして算出した値を用いてもよい。さらに、初期取付姿勢は、比較的ラフに手で計測した結果を用いても良い。初期取付姿勢を従来よりもラフに設定しても良い理由は、測距点の相対位置を算出する際に用いられる取付姿勢は、後述する取付姿勢推定処理によって更新されるからである。

距離算出部２１は、ＴＯＦ算出部１３からＴＯＦ信号（つまり、レーザ光の出射時刻と反射光の受光時刻との時間差）が入力された場合、その時間差に基づいてレーザ光の反射点（これを測距点とする）までの距離ｒ（図３参照）を算出する。

位置算出部２２は、距離算出部２１が算出した測距点までの距離ｒと反射光が得られたレーザ光の照射角度と、記憶部２０に記憶されている取付姿勢と、をもとにして、測距点の位置データを作成する。ここでの位置データは、次に述べるセンサ座標系で表される。

センサ座標系は、互いに直交するようにｘ軸、ｙ軸、ｚ軸を備え、当該センサ座標系におけるレーザレーダ１の位置が（０，０，ｈ）となるように原点を備えている。ＥＣＵ２は、ｙ軸方向が車両前後方向に、ｘ軸方向が車幅方向に、ｚ軸方向が高さ方向に、それぞれ対応するものと認識する。

位置算出部２２は、現在の取付姿勢を考慮して求めた測距点の相対位置をセンサ座標系の座標（ｘ、ｙ、ｚ）で表す。言い換えれば、センサ座標系の各軸および原点は、現在位置算出部２２で採用されている取付姿勢に基づいて設定されるものである。

このセンサ座標系は、レーザレーダ１の検出結果に基づいてＥＣＵ２が認識する、レーザレーダ１が検出した測距点（すなわち物体）の車両５に対する相対的な位置を表している。これに対し、路面座標系は、レーザレーダ１が検出した物体の、車両に対する実際の相対位置を表す。

これらセンサ座標系と路面座標系とは、レーザレーダ１が正確に（設計通りに）車両５に設置されてあって、かつ、路面６に対する車両５の姿勢も規定値通りである場合には一致するものである。すなわち、レーザレーダ１の中心軸（すなわちｚ軸）は、車両５の高さ方向と一致し、レーザレーダ１の正面方向（すなわちｙ軸）は、車両５の前後方向における前方と一致する。もちろん、ｘ軸とＸ軸についても一致する。

しかしながら、課題欄でも言及した通り、車両５に対して正確にレーザレーダ１を設置するためには、リフレクタなどを用いて厳密な光軸調整が必要となる。また、光軸調整のための試験環境（リフレクタなどの設置位置）自体の精度も要求されるため、製造ラインにおいて、光軸調整に要する時間や人員、試験環境などのコストがかかってしまう。

また、正確に車両５にレーザレーダ１が設置されている場合であっても、タイヤの空気圧の変化や、夏タイヤ／冬タイヤの交換によって、例えば車体の傾きなどといった、路面６に対する車両５の姿勢（これを車両姿勢とする）は変化する。さらには、乗員の乗車位置や、積載物などによっても車両姿勢は変化する。車両姿勢が変化してしまうと、路面６に対するレーザレーダ１の姿勢（すなわち取付姿勢）も変化する。

そして、実際の車両５の利用環境においては、乗員数や積載物の変化は大きく、タイヤの状態もまた変動することが想定されるため、路面座標系とセンサ座標系とが一致しない場合が多いことが考えられる。これに伴って、レーザレーダ１の検出結果には、レーザレーダ１の取付姿勢による誤差が含まれてしまう。言い換えれば、レーザレーダ１の取付姿勢による誤差とは、センサ座標系と路面座標系とのずれに相当する。

以降では、説明簡略化のため、初期取付姿勢に基づいて、センサ座標系が設定されているものとして説明する。すなわち、本実施形態の初期取付姿勢では、ピッチ角θｐ、ロール角θｒ、ヨー角θｙは何れも０°であるため、ｙ軸はレーザレーダ１の正面方向に、ｘ軸はレーザレーダ１の左右方向に、ｚ軸はレーザレーダ１の回転軸方向に備えられる。また、原点は当該センサ座標系において、レーザレーダ１の位置が（０，０，ｈ）となる点である。

車両情報取得部２３は、自車両５の情報を検出するセンサ群からなる車両情報検出部４と、図示しない車内ＬＡＮで接続されている。車両情報検出部４は、例えば自車の走行速度を検出する車速センサ、自車に作用するヨーレートの大きさを検出するヨーレートセンサ、及びステアリングホイールの操舵角を検出する操舵角センサ（何れも図示略）を備えている。車両情報取得部２３は、これらのセンサからのセンサ信号を用いて、自車両５の走行速度、ヨーレートの大きさ、移動量などを算出する。

取付姿勢推定部２４は、レーザレーダ１の検出結果から、レーザレーダ１の取付姿勢を推定する。この取付姿勢推定部２４の作動については、図４に示すフローチャートを用いて後で詳細に説明する。

運転支援システム３は、物体検出システム１００の検出結果に基づいて、例えば公知のＡＣＣ（ＡｄａｐｔｉｖｅＣｒｕｉｓｅＣｏｎｔｒｏｌ）や、衝突被害軽減ブレーキ機能、レーンキープアシスト（ＬＫＡ：ＬａｎｅＫｅｅｐｉｎｇＡｓｓｉｓｔ）などを実施する。例えばＡＣＣであれば、物体検出システム１００が検出する先行車との距離に基づいて、車間距離を一定に保つように、駆動系及び制動系システムを制御する。

また、本実施形態における運転支援システム３は、上述した機能に加えて、車両５を自動で走行させる自動運転機能や、駐車位置に車両５を自動で駐車させる自動駐車機能を実施する。

＜取付姿勢推定処理＞
次に、図４に示すフローチャートを用いて、取付姿勢推定部２４が実施する取付姿勢推定処理の流れを説明する。図４に示すフローチャートは、例えば図示しないイグニッションスイッチがオンになって、物体検出システム１００に電力が供給されている場合において、逐次（例えば１００ミリ秒毎に）実施される。

まず、ステップＳ１では、取付姿勢読み出し処理を実施してステップＳ２に移る。ステップＳ１の取付姿勢読み出し処理では、記憶部２０に格納されている取付姿勢のうち、位置算出部２２で用いるための取付姿勢を読み出す。なお、イグニッションスイッチがオンとなってから本フローを初めて実施する場合には、一例として初期取付姿勢を読み出すものとする。また、後述するステップＳ８やＳ１２で取付姿勢が更新されている場合には、その更新された取付姿勢を読み出す。

ステップＳ２では、スキャン処理を実施してステップＳ３に移る。ステップＳ２のスキャン処理では、レーザレーダ１に駆動信号を出力し、全てのレイヤにおいてレーザ光を３６０°スキャンさせる。そして、距離算出部２１が、ＴＯＦ算出部１３から入力される時間差データに基づいて、各測距点に対する距離ｒを算出するとともに、照射方位角βと測距点との距離ｒとを対応付けた、照射方位角−距離データを生成する。位置算出部２２は、測距点毎に、距離算出部２１が算出した距離ｒ、そのレーザ光の照射角度、及びステップＳ１で読みだした取付姿勢から相対位置を算出する。

なお、本実施形態におけるレーザレーダ１は、車両全周囲を０°から３６０°まで０．１°ずつレーザ光を照射するため、１つのレイヤにおいて３６０１回レーザ光を照射することになる。また、レイヤ数Ｌは３２であるから、レーザレーダ１が３６０°スキャニングすることによって、ＥＣＵ２は最大１１５２３２点の測距点を得ることになる。なお、ここで最大値として表現している理由は、発光部１１から照射されたレーザ光のうち、受光部１２で反射光を受光できず、測距点を取得できない場合があるためである。

ステップＳ３では、路面候補点抽出処理を実施してステップＳ４に移る。このステップＳ３の路面候補点抽出処理の作動については、別途図５に示すフローチャート及び図６に示す概念図を用いて後述する。この路面候補点抽出処理を実施することで、ステップＳ２で取得した全ての測距点のうち、路面６を検出したと推定される測距点（これを路面候補点とする）を抽出する。このステップＳ３を実施するＥＣＵ２が、路面候補点抽出部に相当する。

ステップＳ４では、路面推定処理を実施してステップＳ５に移る。このステップＳ４の路面推定処理の作動については、別途図７に示すフローチャート及び図８に示す概念図を用いて後述する。この路面推定処理を実施することで、センサ座標系における路面６に相当する平面（これを路面平面とする）Ｓを推定する。路面平面Ｓは、センサ座標系において、Ｓ＝ａｘ＋ｂｙ＋ｃｚ＋ｅ＝０（ａ、ｂ、ｃ、ｅは、路面推定処理で定まる係数）で表される。もちろん、センサ座標系と路面座標系とが一致する場合には、この路面平面Ｓは、ｚ＝０の平面と一致するものである。このステップＳ４を実施するＥＣＵ２が、請求項に記載の路面平面算出部に相当する。

ステップＳ５では、第１取付姿勢推定処理を実施してステップＳ６に移る。このステップＳ５の第１取付姿勢推定処理では、前ステップＳ４で推定した路面平面Ｓを用いて、取付姿勢のうち、ピッチ角θｐ、ロール角θｒ、及び搭載高さｈについて以下の方法によって推定する。

まず、ピッチ角θｐを推定する方法について、図９及び図１０を用いて説明する。図９は、センサ座標系のｙｚ平面の概念図である。図９は、ｚ＝０とする面（これを基準面とする）が、便宜上、路面６（路面座標系においてＺ＝０）と一致するように示しており、また、図中における一点鎖線は、路面平面Ｓとｙｚ平面とが交差してなる直線（Ｓｙｚとする）を表している。この直線Ｓｙｚは、路面平面Ｓにおいてｘ＝０とすればよく、Ｓｙｚ＝ｂｙ＋ｃｚ＋ｅ＝０となる。

ここで、路面平面Ｓは、路面候補点から算出された平面であるため、本来、路面平面Ｓと路面６とは一致するはずである。路面平面Ｓを路面６と一致するようにセンサ座標系を回転させた図を、図１０に示す。図１０に示すように、ｙ軸と路面平面Ｓとが為す角度、すなわち、ｙ軸と直線Ｓｙｚとが為す角度が、レーザレーダ１のピッチ角θｐに相当する。ピッチ角θｐ（−９０°≦θｐ≦９０°）は、直線Ｓｙｚ＝０をｚについて変形することで、

式２のように求めることができる。

次に、ロール角θｒを推定する方法について、図１１及び図１２を用いて説明する。ロール角θｒは、前述のピッチ角θｐの導出方法と同様の考え方に基づいて導出できるため、一部省略して説明する。

図１１は、センサ座標系のｘｚ平面の概念図である。基準面ｚ＝０と路面６（Ｚ＝０）とが一致するように示しており、図中における一点鎖線が、路面平面Ｓとｘｚ平面とが交差してなる直線（Ｓｘｚ）を表している。直線Ｓｘｚは、路面平面Ｓにおいてｙ＝０とすればよく、Ｓｘｚ＝ａｘ＋ｃｚ＋ｅ＝０となる。

また、路面６と路面平面Ｓとが一致するようにセンサ座標系を回転させた図を、図１２に示す。図１２に示すように、ｘ軸と路面平面Ｓとが為す角度、すなわち、ｘ軸と直線Ｓｘｚとが為す角度が、レーザレーダ１のロール角θｒに相当する。ここで、直線Ｓｘｚ＝０をｚについて式変形することで、ロール角θｒ（−９０°≦θｒ≦９０°）は、

式４のように求めることができる。

搭載高さｈについては、例えば照射方位角β＝０における任意の路面候補点との距離ｒと、その路面候補点を検出したレーザ光の照射仰俯角αと、ピッチ角θｐとを用いて算出する。具体的は、照射方位角β＝０となっている場合には、図１３に示すように、当該路面候補点を検出したレーザ光と路面６とが為す角度の大きさは、レーザ光の照射仰俯角αと、ピッチ角θｐとの和で表される。したがって、搭載高さｈは、下記の式５で求める事ができる。

以上のようにして求めた、ピッチ角θｐ、ロール角θｒ、及び搭載高さｈは、これらを求めた時刻を表すタイムスタンプを付与して記憶部２０に格納する。なお、タイムスタンプは、時刻を表すものにかぎらず、各推定結果の時系列が識別できるものであればよく、番号などであってもよい。この第１取付姿勢推定処理を実施するＥＣＵ２が、請求項に記載の第１取付姿勢推定処理部に相当する。

ステップＳ６では、安定性評価処理を実施する。このステップＳ６の安定性評価処理の詳細な作動については、別途図１４に示すフローチャート及び図１５に示す概念図を用いて後述する。ここでは、この安定性評価処理の概要について述べる。安定性評価処理では、これまでの取付姿勢の推定結果を時系列に並べた時系列データから、推定結果のばらつき（すなわち分散）σを算出し、ステップＳ５で求めた推定結果の尤もらしさ（信頼性ともいうことができる）を評価する。

安定性評価処理では、分散σが所定の閾値Σよりも小さい場合には、推定結果が安定していると判定する。また、分散σが閾値Σ以上となっている場合には、推定結果が安定してないと判定する。

一般に、レーザレーダ１の検出結果は、車両状態や走行環境の影響を受ける。ここでの車両状態とは、急な加減速や急な旋回操作などのユーザ操作が為されているか否かを指し、走行環境とは、路面６の凹凸や段差の有無などを指す。例えば、車両状態が安定していない例として、急なブレーキ操作によって前輪において急な制動を実施した場合には、慣性に従って後輪が浮き上がり、車両姿勢が変化する。また、走行環境が安定していない例として、車両が段差を乗り上げた前後においては、車両姿勢は振動する。

このように車両状態や走行環境が安定していない場合には、車両姿勢は時間変動するため、レーザレーダ１の検出結果には、車両姿勢に起因する誤差が含まれる。これに伴って、ステップＳ５での推定結果もまた、時間変動する。一方、車両状態及び走行環境が安定している場合には、車両姿勢の変化は相対的に小さいため、推定結果も安定する。

すなわち、ステップＳ６で、推定結果が安定しているということは、車両状態及び走行環境の両方が安定している状態においてレーザレーダ１の取付姿勢を推定していることを意味する。また、推定結果が安定していないということは、車両状態及び走行環境の少なくとも何れか一方が安定していない状態においてレーザレーダ１の取付姿勢を推定していることを意味する。

ステップＳ６において、推定結果が安定していると判定した場合には、ステップＳ７がＹＥＳとなってステップＳ８に移る。一方、ステップＳ６において、推定結果が安定していないと判定した場合には、ステップＳ７がＮＯとなって、本フローを終了する。

ステップＳ８では、第１取付姿勢更新処理を実施してステップＳ９に移る。このステップＳ８では、取付姿勢のうち、ステップＳ５で推定したピッチ角θｐ、ロール角θｒ、及び搭載高さｈを用いて、位置算出部２２で用いられる取付姿勢を更新する。このステップＳ８を実施するＥＣＵ２が、請求項に記載の第１取付姿勢更新部に相当する。

ステップＳ９では、直進性評価処理を実施する。このステップＳ９では車両５が一定時間（例えば５秒）過去の時点から現在までの間、直進しているか否かを判定する。車両５が直進しているか否かは、車両情報取得部２３が車両情報検出部４から取得する車両情報から判定すればよい。例えば、車速が０よりも大きく、かつ、ヨーレートが０であれば、直進していると判定する。一方、それ以外の場合には、直進していないと判定する。なお、ヨーレートが０である場合とは、完全に０である場合だけでなく、所定の閾値以下であれば０と見なす場合も含む。

ステップＳ９において直進していると判定した場合には、ステップＳ１０がＹＥＳとなってステップＳ１１に移る。一方、ステップＳ９において直進していないと判定した場合にはステップＳ１０がＮＯとなって、本フローを終了する。

ステップＳ１１では、第２取付姿勢推定処理を実施してステップＳ１２に移る。この第２取付姿勢推定処理では、取付姿勢のうちのヨー角θｙを推定する。この第２取付姿勢推定処理について、図１６を用いて説明する。この第２取付姿勢推定処理を実施するＥＣＵ２が、請求項に記載の第２取付姿勢推定処理部に相当する。

図１６に示す表８の左列は、路面座標系のＸＹ平面におけるレーザレーダ１の位置（すなわち路面座標系の原点）と、任意の測距点Ｐｙとの位置関係を表しており、図中の一点鎖線は、センサ座標系のｙ軸の位置を仮想的に表している。Ｙ軸とｙ軸とが為す角がヨー角θｙに相当する。また、表８の右列は、センサ座標系のｘｙ平面におけるレーザレーダ１の位置（すなわちセンサ座標系の原点）と、測距点Ｐｙとの位置関係を表している。

また、図１６に示す表８の上段は時刻Ｔ−Ｔｃにおけるレーザレーダ１に対する測距点Ｐｙの相対位置を表しており、下段は、時刻Ｔにおけるレーザレーダ１に対する測距点Ｐｙの相対位置を表している。Ｔｃは予め設計される時間であって、例えば、５００ミリ秒とする。

本来、自車両５の前後方向とレーザレーダ１の正面方向（すなわちｙ軸方向）とが一致している場合には、左列と右列の図は一致するものである。しかしながら、ヨー角θｙが０°ではない場合には、実際には車両５にとって表８の左列に示す位置にある測距点Ｐｙは、ＥＣＵ２にとっては表８の右列の位置にあると認識される。

したがって、ＥＣＵ２が認識する測距点Ｐｙの相対位置は、図１６の表８の外側に示すような軌跡（これを相対測距点軌跡とする）γで移動していくように認識される。ここで、センサ座標系のｙ軸方向と、相対測距点軌跡γとの為す角度θｖの大きさは、車両５の進行方向（すなわち路面座標系のＺ軸方向）とレーザレーダ１の為す角度、すなわちヨー角θｙの大きさと等しい。また、角度θｖと、ヨー角θｙとは符号が逆である。

したがって、取付姿勢推定部２４は、周知の追跡（トラッキング）手法を用いて任意の測距点Ｐｙを追跡し、当該測距点Ｐｙの軌跡γとｙ軸方向とが為す角度θｖを算出することによって、ヨー角θｙ（＝−θｖ）を算出することができる。ここで、測距点Ｐｙは、静止物から取得すればよく、静止物であるか否かの判定方法は、周知の方法を用いればよい。以上のようにして求めたヨー角θｙは、ステップＳ５で求めたピッチ角θｐなどと同様に、これを求めた時刻を表すタイムスタンプを付与して記憶部２０に格納する。

ステップＳ１２では、ステップＳ１１で推定したヨー角θｙを用いて、位置算出部２２が用いるヨー角θｙを更新して、本フローを終了する。このステップＳ１２を実施するＥＣＵ２が、請求項に記載の第２取付姿勢更新部に相当する。

以上では、センサ座標系が初期取付姿勢に基づいて設定されている場合を例にとって説明したが、もちろん、ステップＳ８やステップＳ１２によって更新された取付姿勢を用いて、繰り返し上述の取付姿勢推定処理を実施してもよい。

なお、本実施形態では、路面平面Ｓを算出する際に用いる路面候補点の位置として、位置算出部２２がレーザレーダ１の取付姿勢を考慮して算出した位置を用いる構成としている。このため、上述の取付姿勢推定処理を実施して新たに算出される取付姿勢（例えばピッチ角θｐなど）は、現在採用している取付姿勢との差分を表すことになる。

したがって、ステップＳ８やステップＳ１２において取付姿勢を更新する際には、現在採用している取付姿勢に、新たに算出した取付姿勢を加算した値を、路面６に対する取付姿勢として更新する必要がある。また、同様に、取付姿勢の時系列データを生成する際にも、各時点での取付姿勢は、その時点において採用されている取付姿勢に、新たに算出した取付姿勢を加えた値を用いる必要がある。

もちろん、他の構成として、路面平面Ｓを算出する際に用いる路面候補点の位置として、位置算出部２２がピッチ角θｐ、ロール角θｒ、ヨー角θｙを何れも０として算出した値を用いる場合（これを変形例１とする）には、新たに算出される取付姿勢が、そのまま現在の取付姿勢を表す。したがって、変形例１の場合には、時系列データなども、算出したそのままの値を用いれば良い。

＜路面候補点抽出処理＞
図５に示すフローチャートを用いて、取付姿勢推定部２４が実施する路面候補点抽出処理の流れを説明する。図５に示すフローチャートは、図４に示す取付姿勢推定処理において、ステップＳ３に移ったときに実施される。

本処理の説明で用いるＬは、前述のとおり、レーザレーダ１が備えるレイヤ数であり、本実施形態では３２である。また、ｌｙｒは、整数であって、以下の処理を実施する上で用いる変数である。ｌｙｒは、レーザレーダ１が備える全レイヤのうち、現在処理の対象としているレイヤを特定するためのものであって、現在処理の対象としているレイヤの番号を表す。

まず、ステップＳ３０１では、ｌｙｒを０に設定（すなわち、ｌｙｒの初期化）し、ステップＳ３０２に移る。ステップＳ３０２では、変数ｌｙｒがレイヤ数Ｌよりも小さいか否かを判定する。ｌｙｒがレイヤ数Ｌよりも小さい場合には、ステップＳ３０２がＹＥＳとなってステップＳ３０３に移る。一方、ｌｙｒがレイヤ数Ｌ以上となっている場合には、ステップＳ３０２がＮＯとなって本フローを終了する。

ステップＳ３０３では、スキャン処理の結果、距離算出部２１が生成した照射方位角−距離データから、レイヤ番号がｌｙｒとなっているレイヤが検出した測距点を抽出して、一次元信号を生成する。すなわち、当該一次元信号は、レイヤ番号がｌｙｒとなっているレイヤにおける、照射方位角βと、測距点との距離ｒの対応関係を示す。このステップＳ３０３を実施するＥＣＵ２が、請求項に記載の一次元信号生成部に相当する。

ステップＳ３０４では、ステップＳ３０３で生成した一次元信号に対して、周知の高速フーリエ変換（ＦＦＴ：ＦａｓｔＦｏｕｒｉｅｒＴｒａｎｓｆｏｒｍ）を施すことで、周波数解析する。なお、本実施形態では、一次元信号を周波数成分毎に分解するための方法として、ＦＦＴを用いるが、もちろんその他の手法であってもよく、例えば離散ウェーブレット変換（ＤＷＴ：ＤｉｓｃｒｅｔｅＷａｖｅｌｅｔＴｒａｎｓｆｏｒｍ）などであってもよい。

ステップＳ３０５では、ステップＳ３０４で周波数分解された一次元信号を、周知のローパスフィルタ（ＬＰＦ：ｌｏｗＰａｓｓＦｉｌｔｅｒ）に通すことで、低周波成分を抽出する。ローパスフィルタが通過させる周波数の最大値（いわゆるカットオフ周波数）は、適宜設計されればよい。ステップＳ３０６では、ステップＳ３０５で得られた低周波成分に対して、周知の逆フーリエ変換（ＩＦＦＴ：ＩｎｖｅｒｓｅＦＦＴ）を施す。

ステップＳ３０４〜３０６の処理を施すことによって、ステップＳ３０２で生成した（すなわち、元の）一次元信号から低周波成分を抽出する。ここで、図６を用いて、一次元信号の低周波成分を抽出することの意義について説明する。

図６は、一次元信号における照射方位角βと距離ｒとの対応関係を表したグラフである。図６に示すグラフにおいて、横軸が照射方位角βを表し、縦軸がレーザレーダ１から測距点までの距離ｒを表している。また、図６中の実線部分（厳密には０．１°ずつ離れた点の列）が照射方位角毎の測距点を表し、点線は、ＬＰＦを通して得られる一次元信号の低周波成分を表している。

通常、発光部１１から照射されたレーザ光は、そのレーザ光の進行方向に先行車両や人、輪止めなどの立体物などが存在しない場合には、路面６まで到達する。一方、立体物がレーザ光の進行方向上に存在する場合には、レーザ光は路面６まで到達せず、当該立体物で反射されて返ってくる。したがって、立体物を検出している場合の距離ｒは、路面６を検出している場合の距離ｒよりも小さくなる。

また、路面６を検出している場合には、その検出結果（すなわち距離ｒ）は、その隣接する照射方位角においてもほぼ等しい距離ｒとなる一方、路面６を検出している状態から照射方位角βが変化して立体物を検出した場合には、距離ｒは急激に変化する。すなわち、図６に示すグラフにおいて、距離ｒが急激に変化している部分は立体物を検出していることを意味する。また、距離ｒが急激に変化している部分は一次元信号の周波数成分において高周波成分として表れる。

したがって、一次元信号の周波数成分における高周波成分は、立体物を検出している部分に相当し、相対的に低周波成分は路面６を検出している部分に相当することになる。すなわち、ステップＳ３０４〜３０６の処理を施すことによって、ステップＳ３０２で生成した（すなわち、元の）一次元信号から低周波成分を抽出することは、路面６を検出している測距点、すなわち路面候補点を抽出することに相当する。

再び図５に戻って路面候補点抽出処理についての説明を続ける。ステップＳ３０７では、ステップＳ３０６で得られた路面候補点を記憶部２０に保存してステップＳ３０８に移る。記憶部２０には、例えば路面候補点を保存するための配列を用意しておき、路面候補点は算出される順番に当該配列に格納していけば良い。

ステップＳ３０８では、ｌｙｒに１を加算してステップＳ３０２に戻る。すなわち、これまで処理の対象となっていたレイヤよりも、そのレイヤ番号が１つ大きいレイヤ（すなわち、次のレイヤ）を、以降での処理の対象に設定して、ステップＳ３０３〜３０７を実施する。

そして、以上の処理をｌｙｒがＬ以上となるまで繰り返すことで、全てのレイヤにおける路面候補点を抽出することができる。

＜路面推定処理＞
図７に示すフローチャートを用いて、取付姿勢推定部２４が実施する路面推定処理の流れを説明する。図７に示すフローチャートは、図４に示す取付姿勢推定処理において、ステップＳ４に移ったときに実施される。以降において、Ｎは路面候補点抽出処理で取得した全ての路面候補点の数を表し、整数Ｍ（Ｍ≧１）は、予め設定された繰り返し回数であって、例えば１０とする。また、ｍ、ｎは整数であって、以下の処理を実施する上で用いる変数である。

まず、ステップＳ４０１では、ｍ＝０（すなわち、ｍの初期化）としてステップＳ４０２に移る。ステップＳ４０２では、変数ｍが繰り返し回数Ｍよりも小さいか否かを判定する。ｍが繰り返し回数Ｍよりも小さい場合には、ステップＳ４０２がＹＥＳとなってステップＳ４０３に移る。一方、ｍが繰り返し回数Ｍ以上となっている場合には、ステップＳ４０２がＮＯとなってステップＳ４３０に移る。

ステップＳ４０３では、全ての路面候補点の中からランダムに３点を選択して、ステップＳ４０４に移る。このステップＳ４０３で選択した路面候補点を、Ｐａ（ｘ_ａ，ｙ_ａ，ｚ_ａ）、Ｐｂ（ｘ_ｂ，ｙ_ｂ，ｚ_ｂ）、Ｐｃ（ｘ_ｃ，ｙ_ｃ，ｚ_ｃ）とする。各点Ｐａ、Ｐｂ、Ｐｃの位置を表す座標は、位置算出部２２がそれぞれの路面候補点に対して算出した値を用いればよい。

ステップＳ４０４では、ステップＳ４０３で選択されている３つの路面候補点Ｐａ、Ｐｂ、Ｐｃから仮路面平面Ｓｔを算出する。このステップＳ４０４を実施するＥＣＵ２が、請求項に記載の仮路面平面算出部に相当する。なお、ｍ＝０のときに求めた仮路面平面ＳｔをＳｔ（０）と表し、ｍ＝１のとき求めた仮路面平面ＳｔはＳｔ（１）と表す。すなわち、平面Ｓｔ（ｍ）は、任意のｍのときに算出された仮路面平面Ｓｔを表す。

仮路面平面Ｓｔは、路面候補点Ｐａ、Ｐｂ、Ｐｃの３点を通る平面であるため、図８に示すように、ＰａからＰｂへ向かうベクトルＴと、ＰａからＰｃへ向かうベクトルＵの外積から算出する事ができる。より具体的には、ベクトルＴの各成分を（Ｔ_ｘ，Ｔ_ｙ，Ｔ_ｚ）とし、ベクトルＵの各成分を（Ｕ_ｘ，Ｕ_ｙ，Ｕ_ｚ）とすると、それぞれ式６、式７となる。

そして、センサ座標系における仮路面平面Ｓｔを表す式を係数ａ，ｂ，ｃ，ｅを用いてａｘ＋ｂｙ＋ｃｚ＋ｅ＝０と表すと、係数ａ，ｂ，ｃは、ベクトルＴとベクトルＵの外積を計算した結果得られる式８で求めることができる。また、係数ｅは、式８で算出されたａ，ｂ，ｃと、Ｐａの座標（ｘ_ａ，ｙ_ａ，ｚ_ａ）を用いて式９となる。

再び図７に戻り、路面推定処理の説明を続ける。ステップＳ４０５では、ｎ＝０としてステップＳ４１０に移る。ステップＳ４１０では、変数ｎが路面候補点の数Ｎよりも小さいか否かを判定する。ｎが路面候補点の数Ｎよりも小さい場合には、ステップＳ４１０がＹＥＳとなってステップＳ４１１に移る。一方、ｎが路面候補点の数Ｎ以上となっている場合には、ステップＳ４１０がＮＯとなってステップＳ４２０に移る。

ステップＳ４１１では、記憶部２０に格納されている路面候補点と、ステップＳ４０４で算出した仮路面平面Ｓｔ（ｍ）との距離ｄを算出する。なお、先頭からｎ＋１個目に記憶されている路面候補点をＰ（ｎ＋１）と表し、路面候補点Ｐ（ｎ）と仮路面平面Ｓｔ（ｍ）との距離ｄをｄ（ｎ）と表す。例えば、Ｐ（０）は、記憶部２０の設けられている配列において先頭に格納されている路面候補点を指し、ｄ（０）は、Ｐ（０）と仮路面平面Ｓｔ（ｍ）との距離を表す。

路面候補点Ｐ（ｎ）と、ステップＳ４０４で算出した仮路面平面Ｓｔ（ｍ）との距離ｄ（ｎ）は、図８に示すように、Ｐ（ｎ）から仮路面平面Ｓｔ（ｍ）に降ろした垂線の長さであって、次に式１０で求めることができる。ここでは便宜上、路面候補点Ｐ（ｎ）の座標は、（ｘ_ｎ，ｙ_ｎ，ｚ_ｎ）で表すが、位置算出部２２が算出した座標を用いれば良い。

ステップＳ４１２では、ステップＳ４１１で算出した距離ｄ（ｎ）を記憶部２０に格納してステップＳ４１３に移る。ステップＳ４１３では、ｎに１を加算してステップＳ４１０に戻る。これによって、次に実施するステップＳ４１１では、次に記憶されている路面候補点Ｐ（ｎ＋１）と、ステップＳ４０４で算出した仮路面平面Ｓｔ（ｍ）との距離ｄ（ｎ＋１）を算出する。

すなわち、ステップＳ４１０〜Ｓ４１３をｎがＮ以上（ステップＳ４１０がＮＯ）となるまで繰り返すことで、全ての路面候補点Ｐ（０）〜Ｐ（Ｎ−１）と、仮路面平面Ｓｔ（ｍ）との距離ｄ（０）〜ｄ（Ｎ−１）を算出する。

ステップＳ４２０では、以上の処理で得られたｄ（０）〜ｄ（Ｎ−１）の中央値ｄ＿ｍｅｄを求めてステップＳ４２１に移る。仮路面平面Ｓｔ（ｍ）に対して求めた中央値ｄ＿ｍｅｄをｄ＿ｍｅｄ（ｍ）で表す。中央値ｄ＿ｍｅｄ（ｍ）を求める方法は、周知の方法を求めればよい。本実施形態では、ｄ（０）〜ｄ（Ｎ−１）を小さい順にソートした配列を生成し、その配列の中央に格納されている値を中央値ｄ＿ｍｅｄ（ｍ）とする。ソートは、バブルソートやクイックソートなど周知の手法を適用すればよい。

仮に、全ての路面候補点Ｐ（０）〜Ｐ（Ｎ−１）が、誤差なく同一平面上の点を検出している場合には、距離ｄ（０）〜ｄ（Ｎ−１）は何れも０となり、中央値ｄ＿ｍｅｄ（ｍ）も０となる。しかしながら、実際には、レーザレーダ１自体の検出精度の影響を受けることに加えて、ステップＳ３の路面候補点抽出処理において、路面６以外を検出している測距点も一部、路面候補点として選択してしまう。このため、距離ｄ（０）〜ｄ（Ｎ−１）は必ずしも０とはならない。

また、仮路面平面Ｓｔ（ｍ）を算出するために選択された３点Ｐａ，Ｐｂ，Ｐｃが相対的に誤差の大きい路面候補点であった場合には、距離ｄ≠０となる路面候補点が増えるとともに、各路面候補点との距離ｄは相対的に大きい値となる。

路面候補点は、いずれも路面６を検出している点として選択されているため、全ての路面候補点を代表する平面は、センサ座標系において路面６に相当する平面、すなわち路面平面Ｓである。

また、ステップＳ４２０で求めた中央値ｄ＿ｍｅｄ（ｍ）は、仮路面平面Ｓｔ（ｍ）と全ての路面候補点との距離を代表する値であるため、中央値ｄ＿ｍｅｄ（ｍ）は、仮路面平面Ｓｔ（ｍ）と全ての路面平面Ｓとの距離（すなわち誤差）を表していることになる。したがって、中央値ｄ＿ｍｅｄ（ｍ）が小さいほど、路面平面Ｓに近い平面であることを表す。このステップＳ４２０を実施するＥＣＵ２が、請求項に記載の路面尤度評価部に相当する。

ステップＳ４２１では、ステップＳ４２０で求めた中央値ｄ＿ｍｅｄ（ｍ）と仮路面平面Ｓｔ（ｍ）とを対応付けて保存して、ステップＳ４２２に移る。ステップＳ４２２では、ｍに１を加算してステップＳ４０２に戻る。そして、ステップＳ４０２からステップＳ４２２を繰り返すことによって、Ｍ個の仮路面平面Ｓｔ、及び、各仮路面平面Ｓｔに対する中央値ｄ＿ｍｅｄを得る。

ステップＳ４３０では、以上の処理で得られたＭ個の仮路面平面Ｓｔのうち、その仮路面平面Ｓｔに対応付けられている中央値ｄ＿ｍｅｄが最小となっている仮路面平面Ｓｔを、路面平面Ｓに設定して、本フローを終了する。

なお、本実施形態では、Ｍ＝１０とするが、その他の値であってもよく、例えばＭ＝５０などであってもよい。Ｍが大きいほど、生成される仮路面平面Ｓｔの数は増えるため、より適切な仮路面平面Ｓｔを路面平面Ｓに設定できるようになる。また、他の構成としてステップＳ４２０で中央値ｄ＿ｍｅｄ（ｍ）を求めたがこれに限られるものではなく、ｄ（０）〜ｄ（Ｎ−１）の平均値を求め、利用しても良い。さらに、本実施形態では、仮路面平面の、路面平面Ｓとしての尤もらしさを距離ｄの中央値ｄ＿ｍｅｄで評価したが、距離ｄの二乗の和で評価してもよい。

＜安定性評価処理＞
図１４に示すフローチャートを用いて、取付姿勢推定部２４が実施する安定性評価処理の流れを説明する。図１４に示すフローチャートは、図４に示す取付姿勢推定処理において、ステップＳ６に移ったときに実施される。以降において、整数Ｉは、予め設定された読み出しフレーム数であって、例えば１０とする。ここでの読み出しフレーム数とは、現時点から過去に遡って読み出す取付姿勢の推定結果の数を指す。また、ｉは整数であって、以下の処理を実施する上で用いる変数である。

取付姿勢の推定結果としては、少なくともピッチ角θｐ、ロール角θｒ、及び搭載高さｈがあるが、ここでは簡単のため、安定性を評価するために用いる取付姿勢の推定結果としては、ピッチ角θｐのみを用いる。もちろん、他の構成として、他のロール角θｒ、及び搭載高さｈも用いて以降の安定性評価処理を実施してもよい。

まず、ステップＳ６０１では、ｉ＝０として、ステップＳ６０２に移る。ステップＳ６０２では、変数ｉが読み出しフレーム数Ｉよりも小さいか否かを判定する。変数ｉが読み出しフレーム数Ｉよりも小さい場合には、ステップＳ６０２がＹＥＳとなってステップＳ６０３に移る。一方、変数ｉが読み出しフレーム数Ｉ以上となっている場合には、ステップＳ６０２がＮＯとなってステップＳ６１０に移る。

ステップＳ６０３では、現時点よりもｉフレーム過去に算出したピッチ角θｐを読み出してステップＳ６０４に移る。ｉ＝０の場合には、現時点よりも０フレーム過去に算出したピッチ角θｐを読み出すことになるが、これは今回の取付姿勢推定処理のステップＳ５で算出したピッチ角θｐを指す。また、ｉ＝１の場合には、前回の取付姿勢推定処理で算出したピッチ角θｐを読み出す。

ステップＳ６０４では、ｉに１を加算してステップＳ６０２に戻って、変数ｉが読み出しフレーム数Ｉ以上となるまで、ステップＳ６０３及びステップＳ６０４を繰り返す。すなわち、ステップＳ６０２がＮＯと判定された場合には、今回の取付姿勢推定処理で算出したピッチ角θｐも含めて合計Ｉ＝１０個のピッチ角θｐを読み出していることを意味する。

ステップＳ６１０では、直近Ｉ個のピッチ角θｐの分散σを算出してステップＳ６１１に移る。ステップＳ６１１では、ステップＳ６１０で算出した分散σが、予め設定した閾値Σよりも小さいか否かを判定する。分散σが、予め設定した閾値Σよりも小さい場合には、ステップＳ６１１がＹＥＳとなってステップＳ６１２に移る。一方、分散σが、予め設定した閾値Σ以上となっている場合には、ステップＳ６１１がＮＯとなってステップＳ６１３に移る。

ステップＳ６１２では、推定結果が安定していると判定して本フローを終了する。また、ステップＳ６１３では、推定結果が不安定であると判定して本フローを終了する。本実施形態では、推定結果が安定しているか否かを、分散σを用いて判定するが、その他、標準偏差などによって判定しても良い。また、他の構成として、前時刻における推定結果との差分が所定の閾値以下である場合に安定していると判定しても良い。

ここで、図１５を用いて、推定結果が安定している状態と、安定していない状態とを説明する。推定結果としてピッチ角θｐについてここでは述べるが、ロール角θｒなどの他の要素についても同様である。図１５は、ピッチ角θｐの時系列データを表したグラフであって、縦軸はピッチ角θｐの角度を、横軸は時間を表している。なお、図１５中のθｐ０及びΔθｐについては、応用例２で説明する。

現在時刻をＴとすると、１フレーム前のピッチ角θｐは、時刻Ｔ−１において推定されたピッチ角である。すなわち、読み出しフレーム数Ｉ＝１０であるから、図１４に示す安定性評価処理のステップＳ６０１〜Ｓ６０４では、時刻Ｔから時刻Ｔ−９までのピッチ角θｐが読み出される。そして、これらのピッチ角θｐを母集団として分散σを計算して安定性を評価する（ステップＳ６１０、Ｓ６１１）。

ここで、分散σは、ピッチ角θｐのばらつきを評価してものであるため、分散σが閾値Σよりも小さい場合とは、図１５において、縦軸の変化が比較的小さく、安定した状態が継続していることを表す。例えば、時刻ＴからＴ−１０までの区間は、安定している状態を示している。

一方、車両５が段差を乗り上げたりするなどした場合には、ピッチ角θｐは、例えば図１５の時刻Ｔ−１３やＴ−１４のように大きく変動する。これらの変動量（請求項に記載の時間変動幅）が大きい要素を母集団に含んでいる場合には、分散σもまた閾値Σ以上となる可能性が高い。すなわち、分散σが閾値Σ以上となる場合には、分散σの母集団としている時系列データにおいて、車両５が段差を乗り上げたりするなど、車体の路面６に対する姿勢の変動があったことを意味する。

また、砂利道など、舗装されていない道路を走行している場合には、定常的に車両５が振動するため、ピッチ角θｐの時系列データの振幅も大きくなるため、分散σは大きくなる。

したがって、前述の安定性評価処理を実施することによって、今回の取付姿勢推定処理で推定した結果が、車両５（レーザレーダ１）の路面６に対する相対位置が安定している状態で推定されたものであるか否かを判定することができる。

（実施形態のまとめ）
以上の構成では、レーザレーダ１がスキャン処理することによって得られる複数の測距点から、路面候補点を抽出する（図４ステップＳ３）。そして、ランダムに選択した路面候補点Ｐａ、Ｐｂ、Ｐｃから算出される路面平面Ｓと、ｚ＝０とする基準面とが為す角度から、レーザレーダ１のピッチ角θｐ、ロール角θｒ、及び搭載高さｈを推定する。

また、ステップＳ１１では、任意の測距点Ｐｙ（ただし、静止物）の相対測距点軌跡γを算出し、当該相対測距点軌跡ｙとｙ軸とが為す角度から、ヨー角θｙも求めることができる。

そして、取付姿勢推定部が推定した取付姿勢の時間変動が所定の閾値より小さい場合は、推定された取付姿勢を実際の取付姿勢として採用（ステップＳ８、Ｓ１２）し、相対位置の算出に用いる。言い換えれば、設計値や工場出荷時の取付姿勢ではなく、実際の取付姿勢を推定した値を用いて測距点の相対位置を算出する。

したがって、記憶部２０に記憶されている初期取付姿勢と、設計値との間に誤差があった場合でも、測距点の相対位置をより正確に取得することができる。すなわち、当該初期取付姿勢の誤差に起因する相対位置の誤差を低減することができる。このため、製造ラインでの厳密な取付姿勢の調整を簡略化することができるとともに、試験環境の精度の要件を緩和することができる。したがって、取付姿勢の調整に伴うコストを低減することができる。

また、以上の取付姿勢推定処理は、リフレクタなどの試験用の設備を用いずに実施することができるため、例えば、走行中において実施することができる。このため、タイヤの空気圧の変化や、車内における乗員や積載物の位置などによって、車両姿勢は変化した場合であっても、その車両姿勢が変化した状態における取付姿勢を推定することができる。

そして、位置算出部は、その推定された取付姿勢を用いて相対位置を算出するため、相対位置の算出における車両姿勢の変化の影響を低減できる。したがって、本実施形態によれば、車両姿勢が工場出荷時などの基準とする姿勢と異なる姿勢となっている場合でも、より精度良く、相対位置を算出することができる。本実施形態は、乗員や積載量の変化が発生しやすいレンタカーやカーシェアリングに用いられる車両に対して、より効果的に作用すると言える。

また、設計値と、実際の取付姿勢との誤差に起因する測距点の検出位置の誤差は、レーザレーダ１からの距離が大きくなるほど大きくなる。このため、本実施形態は、遠方（例えば４０ｍ〜７０ｍ）の物体検出時に、より一層の効果を奏することになる。

さらに、以上の本実施形態によれば、レーザレーダ１の取付姿勢を高精度に推定することができるため、測距点の相対位置もより精度良く算出でき、その結果、路面６と非路面を高精度に分離することができる。

従来は、ＡＣＣやＰＣＳなどへの適用を想定しており、車両のような比較的大きい物体の検出に主眼を置いてきたため、上述したような車両姿勢の動的な変化に起因する誤差は、問題とされてこなかった。

しかしながら、今後、自動運転や自動駐車などのアプリケーションへの適用を想定すると、走行路の両側にある縁石や、駐車場の輪留めのような相対的に小さい物体まで、検出できることが好ましい。

本実施形態によれば、路面６と非路面を高精度に分離することができるため、ＡＣＣやＰＣＳだけでなく、自動運転や自動駐車などといった、より検出精度が要求されるアプリケーションにも応用することができる。また、本実施形態は、以下に示す応用例１〜３のように応用することで、さらなる効果を奏することができる。

（応用例１）
以上で述べた実施形態によれば、取付姿勢がずれない状態において、取付姿勢の更新を少なくとも一度実施し、Ｓ２のスキャン処理で得られる測距点から、路面６の前後方向の勾配の変化量、及び車幅方向の勾配の変化量を推定することができる。以下、この応用例１について、図１７を用いて説明する。なお、取付姿勢がずれない状態とは、レーザレーダ１が車両５に十分に固定されており、車両５に対するレーザレーダ１の姿勢が変化しない状態を指す。

図１７は、物体検出システム１００による検出範囲を、前方領域Ｆｒ、後方領域Ｒｒ、右側方領域Ｒｔ、及び左側方領域Ｌｆの４つの領域に分割した図である。前方領域Ｆｒは、車両５の前方（真正面だけでなく斜め前を含む）に広がる検出エリアを指しており、ここでは一例として照射方位角βが−４５°〜＋４５°の範囲とする。また、後方領域Ｒｒは、車両５の後方（背面だけでなく斜め後ろを含む）に広がる検出エリアを指しており、照射方位角βが１３５°〜２２５°の範囲とする。

右側方領域Ｒｔは、車両５の右側方に広がる検出エリアを指しており、照射方位角βが４５°〜１３５°の範囲とする。左側方領域Ｌｆは、車両５の左側方に広がる検出エリアを指しており、照射方位角βが２２５°〜３１５°の範囲とする。

以上のように車両周囲を複数に分割した領域を用いて、まず、前後方向の勾配の変化量を推定する方法について説明する。ＥＣＵ２において勾配の変化量を推定する処理部を勾配推定部とすると、勾配推定部は、路面候補点抽出処理（図４ステップＳ３）によって得られる路面候補点のうち、前方領域Ｆｒに属する路面候補点を抽出する。そして、前方領域Ｆｒに属する路面候補点を母集団とする路面推定処理（ステップＳ４）を実施することによって、前方領域Ｆｒの路面平面（これを前方平面とする）Ｓｆｒを推定する。

また、勾配推定部は、路面候補点抽出処理によって得られる路面候補点から後方領域Ｒｒに属する路面候補点を抽出し、同様の処理によって、後方領域Ｒｒの路面平面（これを後方平面とする）Ｓｒｒを推定する。

ここで、前方平面Ｓｆｒと後方平面Ｓｒｒとがｙｚ平面において為す角度を求めることによって、車両前後方向における勾配の変化量を推定することができる。例えば、後方平面Ｓｒｒに対して、前方平面Ｓｆｒのｙｚ平面上の傾きが大きい場合には、図１８のように、上り勾配が始まる地点差し掛かったことを意味し、その後方平面Ｓｒｒと前方平面Ｓｆｒとがｙｚ平面において為す角度が、勾配の変化量である。

また、車両５が上り勾配を登っている途中の場合には、車両５の前方後方ともに、路面候補点を取得することができる一方、上り勾配の頂点に近づくにつれて、前方領域Ｆｒに照射したレーザ光に対する路面６からの反射光は取得しにくくなる。すなわち、前方領域Ｆｒに属する路面候補点の数が減少する。したがって、例えば、後方領域Ｒｒにおいて後方平面を算出できた一方、前方領域Ｆｒにおける路面候補点の不足から、前方平面が算出できなかった場合には、上り勾配の頂点付近に差し掛かったと推定することができる。

また、上り勾配の頂点付近においては、レーザ光の照射仰俯角αが小さいほど（レイヤ番号が小さいほど）、反射光は取得しにくくなるため、前方領域Ｆｒにおけるレイヤ毎の路面候補点の数は減少する。すなわち、前方領域Ｆｒにおけるレイヤ毎の路面候補点の数の時間推移からも、車両５が上り勾配の頂点に差し掛かっているか否かを判定することができる。

また、上り勾配を登り切った地点においては、後方領域Ｒｒにおいても、路面６に対するレーザ光の入射角が大きくなる為、路面６からの反射光を取得しにくくなる。すなわち、後方領域Ｒｒに属する路面候補点の数が減少する。

したがって、前方平面Ｓｆｒを算出できる一方、後方領域Ｒｒにおける路面候補点の不足から、後方平面Ｓｒｒが算出できなかった場合には、上り勾配の頂点に到達したと推定することができる。なお、路面候補点の不足から、前方平面Ｓｆｒ及び後方平面Ｓｒｒの両方を算出できなかった場合には、上り勾配の道路と下り勾配の道路の間に位置することと推定してもよい。

以上のようにして、各領域に属する路面候補点を分母とする平面推定処理を実施することで、路面６の前後方向における勾配を推定することができる。また、更には、前方領域Ｆｒ及び後方領域Ｒｒのそれぞれにおける路面候補点の数や、各領域におけるレイヤ毎の路面候補点の数から、車両前後方向の勾配が存在する路面６に対する車両５の位置を推定することができる。そして、勾配推定部が推定した道路勾配の情報は、様々なアプリケーションに利用することができる。

例えば、物体検出システム１００が、車両５が上り勾配に差し掛かったことを検出した場合には、運転支援システム３は、当該検出結果を受けて、駆動力をアシストする制御を実施してもよい。

また、従来の物体検出システムでは、車両前方に存在する急な上り勾配となっている道路を立体物と誤認識してしまう場合もある。物体検出システムが、車両前方に存在する急な上り勾配となっている道路を立体物と誤認識した場合には、運転支援システム３が、衝突を回避するための制動処理（すなわち衝突被害軽減ブレーキ機能）を実施させてしまう。一方、本応用例１の物体検出システム１００によれば、路面上の立体物と、道路勾配とを区別することができるので、上述したような、誤認識及び誤制動を抑制することができる。

さらに、スタート地点の勾配を基準として、例えば前後方向の勾配の変化量を積算していくことで、現在走行している道路の、スタート地点に対する勾配を推定することができる。特に、スタート地点が水平面である場合には、実際の道路の勾配を推定できるようになる。

車幅方向の勾配を推定する方法についても、前述の前後方向における勾配を推定する方法と同様である。すなわち、勾配推定部は、路面候補点抽出処理によって得られる路面候補点のうち、右側方領域Ｒｔに属する路面候補点を母集団とする路面推定処理を実施することによって、右側方領域Ｒｔの路面平面（これを右側方平面とする）Ｓｒｔを推定する。

さらに、勾配推定部は、路面候補点抽出処理によって得られる路面候補点のうち、左側方領域Ｌｆに属する路面候補点を母集団とする路面推定処理を実施することによって、左側方領域Ｌｆの路面平面（これを左側方平面とする）Ｓｌｆを推定する。

そして、右側方平面Ｓｒｔと左側方平面Ｓｌｆとがｘｚ平面において為す角度を求めることによって、車幅方向における勾配の変化量を推定する。例えば、ｘｚ平面において時計回りの回転角を正の値とした時に、左側方平面Ｓｌｆに対して右側方平面Ｓｒｔが正の角度を為している場合には、車両前後方向から見た路面６の断面が凸状になっている、または右肩下がり路面６であると推定することができる。また、左側方平面Ｓｌｆに対して右側方平面Ｓｒｔが負の角度を為している場合には、路面６が車幅方向においては凹状、または右肩上がりであると推定することができる。

なお、上述した各領域が指す範囲は一例であって、適宜設計されればよい。例えばその他形態として、各領域の間に所定の間隙が生じるように設定してもよい。すなわち、照射方位角βが−２０°〜＋２０°の範囲を前方領域Ｆｒ、１６０°〜２００°の範囲を後方領域Ｒｒ、６０°〜１２０°の範囲を右側方領域Ｒｔ、２４０°〜３００°の範囲を左側方領域Ｌｆとしてもよい。

（応用例２）
さらに、前述の実施形態で求めるレーザレーダ１の取付姿勢、又は前述の応用例１で求める路面６の勾配の時系列データから、車両姿勢の急激な変化（ピッチングとする）を推定することができる。ピッチングは、車両５が路面６上の段差を乗り越えたり、ドライバが急なブレーキ操作をする等した場合に生じる。以下、この応用例２について、図１５を用いて説明する。ここでは簡単のため、レーザレーダ１のピッチ角θｐの時系列データから、車両前後方向におけるピッチングを検出する構成について説明するが、ロール角θｒや路面勾配を用いた場合であっても同様である。

図１５は、前述の通りピッチ角θｐの時系列データを表したグラフであって、時刻Ｔ−１４及びＴ−１３において急激にピッチ角θｐが変動している。すなわち、これらの時刻においてピッチングが発生したことを表している。また、θｐ０は、安定性評価処理で安定していると判定された状態におけるピッチ角θｐの平均値を表している。

ピッチングが発生したか否かの判定方法としては、ピッチ角θｐの変動量が所定の閾値を超えた場合とすればよい。例えば、ある時刻（ここではＴ−１４、Ｔ−１３）において推定されたピッチ角θｐが、ピッチ角θｐ０の一定倍（例えば２倍）以上となった場合に、ピッチングが発生したと判定すればよい。また、ピッチングが発生したか否かの判定に用いる閾値は、θｐ０に所定の倍数を乗算した数に限らず、θｐ０に一定値（例えば１°）を加えたであってもよい。図１５中のΔθｐは、ピッチ角θｐ０と時刻Ｔ−１４におけるピッチ角θｐの差分を示す。

以上のようにして、ピッチングの発生を検出し、及びピッチングの度合いΔθｐを検出することで、これらの情報を様々なアプリケーションに提供することができる。

例えば、既存の運転支援システム３として、図示しない車載カメラが撮影した画像に画像処理を施すことによって、歩行者を検出し、当該歩行者までの距離を検出するシステムがある。車載カメラは、一例として、車両前方の適宜設計された範囲（撮影範囲とする）を撮影するように、ルーフミラー付近に取り付けられているものとする。

一般に、このような運転支援システム３では、画像処理を実施するＣＰＵの処理負荷を軽減するために、撮影範囲の中でも画像処理を実施する範囲（これを処理範囲とする）が予め設定されている。そして、処理範囲中に歩行者を検出した場合には、予め設計されているカメラの取付姿勢（設置位置や撮影方向）と、撮影画像中における歩行者の足元の位置と、に基づいて、歩行者までの距離を推定する。

より具体的には、図１９の左側に示すように、カメラの撮影範囲における車両５に近い側の端部（車両側端部とする）から、歩行者の足元までの撮影画像中の距離から、車両５の前端から歩行者までの距離を推定する。図１９の左側に示す例は、車両５にピッチングが生じておらず、車載カメラも路面６に対して予め設計された取付姿勢となっている場合を表している。すなわち、車両５の前端から歩行者までの距離を精度よく検出できている場合を表しており、このときの歩行者までの距離を仮に１０ｍとする。

しかしながら、ピッチングが生じた場合には、歩行者が処理範囲から外れて、歩行者を検出できなくなったり、或いは、推定される歩行者までの距離が実際の距離と乖離したりしてしまう可能性がある。

例えば、車両５が前傾姿勢となるようにピッチングが生じた場合の図１９の右側に示す。図１９右側に示すように、前傾姿勢となっている場合には、車載カメラの視野角の中心線が水平面と為す角度は、予め設計された値よりも大きくなり（すなわち、視野角の中心線が路面６の方を向き）、撮影範囲は設計値よりも進行方向手前側に寄ってしまう。

このとき、撮影範囲における車両側端部から、歩行者の足元までの撮影画像中の距離は、ピッチングが起きていない場合に比べて長くなるため、車両５から歩行者までの距離は、実際の値（１０ｍ）よりも長く推定されてしまう（例えば３０ｍ）。

このような誤検出をしてしまった場合には、本当は歩行者に接近しているにも関わらず、衝突被害軽減ブレーキ機能が作動しない可能性が生じる。

本応用例２によれば、車両５のピッチングを検出することができるため、ピッチングが生じている場合には、車載カメラの撮影画像に基づく歩行者の検出処理をキャンセルすることによって、歩行者との距離を誤検出してしまう可能性を低減することができる。または、車載カメラの取付姿勢をピッチング角Δθｐで補正することによって、ピッチングが生じている場合の検出距離の精度を向上させることができる。

（応用例３）
さらに、前述の応用例１で求める路面６の勾配の時系列データから、推定した路面勾配の時間変化と、車両情報を同時に観察することで、急な加減速時や、コーナー走行時の車両姿勢を推定することができる。

例えば、安定して走行している時刻の路面勾配を記憶しておき、車両情報検出部４から入力されている車速が急変した際に、当該時刻において推定した路面勾配と、記憶部２０に記憶しておいた前時刻における路面勾配との差を算出する。なお、この算出された差は、前述のとおり、ピッチングによる車両５の傾きに相当する。言い換えれば、この算出された路面勾配の前時刻との差は、路面６に対する車両姿勢の変化量に相当する。

以上のようにして、急な加減速時やコーナー走行時の車両姿勢を推定することによって、例えば、路面６に対する車両姿勢に応じて、図示しないサスペンションなどを制御し、乗員の乗り心地や操縦安定性などを向上させることができる。

（特許文献１との比較）
なお、特許文献１には、光軸の垂直方向のずれ（すなわちピッチ角）を調整する方法として、次に述べる方法が開示されている。すなわち、特許文献１では、予め正確な取付姿勢となっていることが確認されているレーザレーダを用いて、所定の照射方位角で照射した複数のレイヤの測距点間の距離を登録しておく。なお、これら複数のレイヤは何れも路面を検出する照射仰俯角となっているレイヤである。

そして、調整時には、対応するレイヤの測距点間の距離と、予め登録されてある値とを比較することで、レーザレーダの光軸の垂直方向のずれ度合いを評価することで、当該ずれ度合いが０となるようにピッチ角を調整する。なお、ヨー角については、背景技術欄で述べたようにリフレクタなど試験用の設備を用いて調整する。

この特許文献１では、ピッチ角とヨー角のずれにしか言及されておらず、搭載高さやロール角のずれについては考慮されていない。しかしながら、複数のレイヤの測距点間の距離は、実際にはピッチ角だけではなく、搭載高さやロール角のずれによっても変化する。そして、ピッチ角、搭載高さ、及びロール角それぞれのずれは、相互に作用しあった和となって、各レイヤの測距点間の距離として表れる。

したがって、特許文献１の方法では、搭載高さやロール角がずれていた場合には、正しい取付姿勢に調整する事ができない。また、ヨー角については、やはりリフレクタを用いないと調整できない。すなわち、ヨー角の誤差を評価するためには、修理工場などに車両を持ち込む必要がある。

これに対し、本実施形態では、ピッチ角θｐ、ロール角θｒ、及び搭載高さｈのそれぞれに対して誤差を評価することができる。また、ヨー角θｙについてもリフレクタなどの試験用の設備を用いずに誤差を評価することができるため、ヨー角θｙの誤差を評価する場合にも、修理工場などに持ち込む必要はない。

（変形例２）
前述の実施形態における路面候補点抽出処理では、ＦＦＴやＬＰＦ、ＩＦＦＴを用いて測距点から路面候補点を抽出する例を示したがこれに限らない。図４のステップＳ２で取得した全ての測距点を母集団として、図２０に示す処理を実施することによって路面候補点を求めても良い。図２０に示すフローチャートは、図４に示す取付姿勢推定処理において、ステップＳ３に移ったときに実施される。

本処理の説明で用いるＫは、照射方位角βが取りうる角度の数であって、ここではレーザ光を０．１°ずつ３６０°照射するため、３６０１となる。また、整数ｋ（０≦ｋ＜Ｋ）は、照射方位角β＝０のときに照射したレーザ光を０番とした時に、何番目に照射されたレーザ光かを表す変数である。すなわち、変数ｋと照射方位角βとは、β＝０．１×ｋの関係を満たす。ここで、任意のｋ（すなわちβ＝０．１×ｋ）のときに照射されたレーザ光によって検出された測距点との距離ｒをｒ（ｋ）と表す。例えば、ｋ＝０（すなわちβ＝０）のときに照射されたレーザ光によって検出された測距点との距離ｒは、ｒ（０）と表す。

まず、ステップＴ３０１では、ｌｙｒ＝０としてステップＴ３０２に移る。ステップＴ３０２では、変数ｌｙｒがレイヤ数Ｌよりも小さいか否かを判定する。ｌｙｒがレイヤ数Ｌよりも小さい場合には、ステップＴ３０２がＹＥＳとなってステップＴ３０３に移る。一方、ｌｙｒがレイヤ数Ｌ以上となっている場合には、ステップＴ３０２がＮＯとなって本フローを終了する。

ステップＴ３０３では、ｋ＝１として、ステップＴ３１０に移る。ステップＴ３１０では、変数ｋが照射方位角数Ｋよりも小さいか否かを判定する。ｋがＫよりも小さい場合には、ステップＴ３１０がＹＥＳとなってステップＴ３１１に移る。一方、ｋがＫ以上となっている場合には、ステップＴ３１０がＮＯとなってステップＴ３２０に移る。

ステップＴ３１１では、現在処理の対象としている（すなわち、レイヤ番号がｌｙｒの）レイヤにおいて、ｋ番目の測距点との距離ｒ（ｋ）と、ｋ−１番目の測距点との距離ｒ（ｋ−１）との差分（これを距離変化量とする）ｆを算出してステップＴ３１２に移る。

ステップＴ３１２では、距離変化量ｆが、予め設定されている閾値（例えば０．２ｍ）Ｔｈｒｅｓよりも小さいか否かを判定する。距離変化量ｆが閾値Ｔｈｒｅｓよりも小さい場合には、ステップＴ３１２がＹＥＳとなって、ステップＴ３１３に移る。一方、距離変化量ｆが閾値Ｔｈｒｅｓより以上となっている場合には、ステップＴ３１２がＮＯとなって、ステップＴ３１４に移る。

ステップＴ３１３では、ｋ番目の測距点を仮路面候補点として記憶部２０に格納してステップＴ３１４に移る。ステップＴ３１４では、Ｋに１を加算してステップＴ３１０に戻る。ステップＴ３１０からステップＴ３１４を、ステップＴ３１０においてＮＯと判定されるまで繰り返すことで、レイヤ番号がｌｙｒのレイヤにおける測距点から、仮路面候補点を抽出することができる。

ステップＴ３２０では、複数の仮路面候補点を母集団とする距離ｒの分布を評価することで、路面６を検出している仮路面候補点を抽出してステップＴ３２１に移る。より具体的は、図６に示すように、路面６を検出している仮路面候補点はそれぞれ比較的近い値を取るため、距離ｒの分布において１つのグループにまとめることができる。また、同一の立体物を検出している仮路面候補点もまた、比較的近い値を取るため、グルーピングすることができる。

ここで、図５の説明でも述べた通り、路面６を検出している測距点は、立体物を検出している測距点よりも距離ｒが大きくなる。したがって、距離ｒの分布において形成される複数のグループのうち、その中央値が最も大きい値となっているグループが路面６を検出している仮路面候補点からなるグループであると推定することができる。このステップＴ３２０では、複数のグループのうち、距離ｒの中央値が最も大きい値となっているグループに属する仮路面候補点を抽出する。

ステップＴ３２１では、前ステップＴ３２０で抽出された仮路面候補点を路面候補点として保存してステップＴ３２２に移る。ステップＴ３２２では、ｌｙｒに１を加算してステップＴ３０２に戻る。ステップＴ３０２からステップＴ３２０を、ステップＴ３０２においてＮＯと判定されるまで繰り返すことで、全てのレイヤにおける路面候補点を抽出することになる。

なお、以上では、レーザレーダ１として、全方位３６０°に向けてレーザ光を掃引照射するものを用いたが、もちろん、水平方向において一定の角度範囲（例えば図１７に示す前方領域Ｆｒのみ）に向けてレーザ光を掃引照射するものであってもよい。また、レーザレーダとして垂直方向に３２ライン照射するものを用いたが、これに限定するものではなく、最低１ラインあれば良い。

１００物体検出システム、１レーザレーダ、２ＥＣＵ（物体検出装置）、３運転支援システム、４車両情報検出部、５車両、６路面、２０記憶部、２１距離算出部、２２位置検出部、２３車両情報取得部、２４取付姿勢推定部、Ｓ３路面候補点抽出部、Ｓ３０３一次元信号生成部、Ｓ４路面平面算出部、Ｓ４０４仮路面平面算出部、Ｓ４２０路面尤度評価部、Ｓ５第１取付姿勢推定処理部、Ｓ８第１取付姿勢更新部、Ｓ１１第２取付姿勢推定処理部、Ｓ１２第２取付姿勢更新部

Claims

車両に搭載され、
垂直方向において複数の異なる角度毎に、水平方向において所定の角度範囲でレーザ光を逐次掃引照射するレーザレーダ（１）から入力される信号に基づいて、前記レーザレーダから前記レーザ光を反射した測距点までの距離を算出する距離算出部（２１）と、
路面に対する前記レーザレーダの取付姿勢を記憶している記憶部（２０）と、
前記記憶部に記憶されている取付姿勢と、前記レーザ光を照射した方向と、前記距離算出部が算出した距離とに基づいて、前記測距点の前記車両に対する相対位置を検出する位置算出部（２２）と、
前記取付姿勢を推定する取付姿勢推定部（２４）と、を備え、
前記記憶部には、前記取付姿勢として所定の初期設定値が予め登録されてあって、
前記取付姿勢推定部は、
前記垂直方向における前記レーザ光の照射方向である照射仰俯角毎に、前記水平方向における前記レーザ光の照射方向である照射方位角を変化させることで得られる複数の前記測距点について、前記測距点との前記距離と前記照射方位角とを対応付けた一次元信号を生成する一次元信号生成部（Ｓ３０３）と、
前記一次元信号生成部が生成した前記一次元信号の周波数成分に基づいて、前記一次元信号の要素となっている前記測距点のうち、前記路面を検出している前記測距点である路面候補点を抽出する路面候補点抽出部（Ｓ３）と、
前記路面候補点抽出部で抽出された複数の前記路面候補点に基づいて、前記路面を表す平面である路面平面を算出する路面平面算出部（Ｓ４）と、
前記路面平面算出部が算出した前記路面平面と、前記取付姿勢から定まる前記路面を表す平面である基準面とが為す角度と、から、前記取付姿勢を推定する第１取付姿勢推定処理部（Ｓ５）と、
前記第１取付姿勢推定処理部で推定した前記取付姿勢に基づいて、前記記憶部に記憶されている前記取付姿勢を更新する第１取付姿勢更新部（Ｓ８）と、を備えることを特徴とする物体検出装置。
請求項１において、
前記取付姿勢は、前記路面から前記レーザレーダまでの高さである搭載高さと、前記レーザレーダの前記路面に対するピッチ角と、前記レーザレーダの前記路面に対するロール角と、のうち少なくとも１つを含むことを特徴とする物体検出装置。
請求項１または２において、
前記取付姿勢は、前記レーザレーダの前記路面に対するヨー角を含み、
前記物体検出装置は、逐次掃引照射するものであって、
複数の時点における前記測距点のデータから、同一の測距点の相対位置の軌跡を算出することで前記ヨー角を推定する第２取付姿勢推定処理部（Ｓ１１）と、
前記第２取付姿勢推定処理部が推定した前記ヨー角を用いて、前記記憶部に記憶されている前記ヨー角を更新する第２取付姿勢更新部（Ｓ１２）と、を備えることを特徴とする物体検出装置。
請求項１から３の何れか１項において、
前記路面候補点抽出部は、前記一次元信号を周波数分解し、予め設定した周波数よりも低い周波数成分を抽出することで、前記路面候補点を抽出することを特徴とする物体検出装置。
請求項１から４の何れか１項において、
前記路面平面算出部は、
複数の前記路面候補点から少なくとも３点選択し、当該選択された前記路面候補点から、前記路面平面の候補である仮路面平面を算出する仮路面平面算出部（Ｓ４０４）と、
選択された前記路面候補点以外の他の路面候補点と、前記仮路面平面との距離を計算することで仮路面平面の路面平面としての尤もらしさを評価する路面尤度評価部（Ｓ４２０）と、を備え、
前記仮路面平面算出部は、それぞれ異なる前記路面候補点の組み合わせを用いて複数の異なる前記仮路面平面を算出し、
前記路面尤度評価部は、複数の前記仮路面平面のそれぞれに対して前記尤もらしさを評価し、
前記路面平面算出部は、最も前記尤もらしさが高い前記仮路面平面を前記路面平面に設定することを特徴とする物体検出装置。
請求項５において、
前記路面尤度評価部は、選択された前記路面候補点以外の前記路面候補点毎に前記仮路面平面との距離を算出することで得られる複数の距離の中央値が小さいほど、前記尤もらしさを高く評価することを特徴とする物体検出装置。
請求項１から６の何れか１項において、
前記記憶部は、前記取付姿勢推定部で推定した前記取付姿勢を時系列に並べた時系列データを記憶し、
前記取付姿勢推定部は、前記記憶部が記憶している時系列データに基づいて、前記取付姿勢の推定結果が安定しているか否かを判定する処理である安定性評価処理を実施するものであって、
前記第１取付姿勢更新部は、前記安定性評価処理の結果として前記取付姿勢の推定結果は安定していると判定されている場合には、前記第１取付姿勢推定処理部で推定した前記取付姿勢に基づいて前記記憶部に記憶されている前記取付姿勢を更新する一方、前記安定性評価処理の結果として前記取付姿勢の推定結果は安定していないと判定された場合には前記取付姿勢を更新しないことを特徴とする物体検出装置。
請求項１から６の何れか１項において、
前記記憶部は、前記取付姿勢推定部で推定した前記取付姿勢を時系列に並べた時系列データを記憶し、
前記第１取付姿勢更新部は、
前記取付姿勢推定部で推定した前記取付姿勢を時系列に並べた時系列データにおいて、前記取付姿勢推定部が推定した前記取付姿勢の時間変動幅が予め設定されている閾値よりも小さい場合に、前記取付姿勢を更新することを特徴とする物体検出装置。
請求項１から８の何れか１項において、
前記レーザレーダは、前記車両の周囲の全方位に前記レーザ光を掃引照射するものであって、
前記車両の前方に位置する前記路面候補点から算出される路面平面である前方平面と、前記車両の後方に位置する前記路面候補点から算出される路面平面である後方平面とが為す角度から、車両前後方向における前記路面の勾配の変化量を推定することを特徴とする物体検出装置。
請求項９において、
前記車両の左側方に位置する前記路面候補点から算出される路面平面である左側方平面と、前記車両の右側方に位置する前記路面候補点から算出される路面平面である右側方平面とが為す角度から、車幅方向における前記路面の勾配の変化量を推定することを特徴とする物体検出装置。
請求項１から１０の何れか１項において、
前記記憶部は、前記取付姿勢推定部で推定した前記取付姿勢を時系列に並べた時系列データを記憶し、
前記取付姿勢推定部で推定した前記取付姿勢を時系列に並べた時系列データにおける前記取付姿勢の変化量から、前記路面に対する車両姿勢の変化量を推定することを特徴とする物体検出装置。