校正装置、物標検知装置および校正方法
【課題】レーダから得られた物標の座標と、カメラから得られた物標の座標とを用いて、物標同士を容易にマッチングさせることの可能な校正装置を提供する。
【解決手段】制御部は、レーダ情報から各物標のレーダ座標系の３次元座標を取得する。制御部は、右カメラの２次元画像から各物標の２次元座標を、左カメラの２次元画像から各物標の２次元座標を抽出し、各物標の右カメラ座標系および左カメラ座標系の２次元座標から視差情報を取得し、視差情報を用いて各物標の右カメラ座標系および左カメラ座標系の２次元座標を、右カメラ座標系および左カメラ座標系の３次元座標に変換する。制御部は、レーダ情報から得られた各物標のレーダ座標系の３次元座標と、ステレオ画像から得られた各物標の右カメラ座標系および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を右カメラ座標系および左カメラ座標系の３次元座標に変換する。
【解決手段】制御部は、レーダ情報から各物標のレーダ座標系の３次元座標を取得する。制御部は、右カメラの２次元画像から各物標の２次元座標を、左カメラの２次元画像から各物標の２次元座標を抽出し、各物標の右カメラ座標系および左カメラ座標系の２次元座標から視差情報を取得し、視差情報を用いて各物標の右カメラ座標系および左カメラ座標系の２次元座標を、右カメラ座標系および左カメラ座標系の３次元座標に変換する。制御部は、レーダ情報から得られた各物標のレーダ座標系の３次元座標と、ステレオ画像から得られた各物標の右カメラ座標系および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を右カメラ座標系および左カメラ座標系の３次元座標に変換する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、レーダによって検知された座標と、ステレオカメラによって検知された座標とを利用して物標を検知する物標検知装置、この物標検知装置を校正するための校正装置およびその校正方法に関する。
【背景技術】
【０００２】
近年、自動車やバイクなどの前方走行車や、歩行者、障害物などの物標との衝突を防止する衝突防止装置や、物標との車間距離を一定の範囲内に保つための車間距離制御装置など、自動車やバイクなどの運転の安全性や快適性を高めるための様々な運転支援装置が開発されている。これら運転支援装置では、物標を検知することが極めて重要であり、運転支援装置にはそのような物標を検知するための物標検知装置が搭載されている。物標検知装置は、例えば、レーザ光またはミリ波電磁波などで物標を検知するレーダや、撮影した画像の解析から物標を検知するカメラ、レーダおよびカメラを含んで構成されたセンサフュージョンなどにより構成されている。
【０００３】
ここで、レーダは、一般に、物標との距離や、相対速度を高精度に測定することができるだけでなく、濃霧などの悪天候によって前方の視界が悪い状況であっても、物標の検知が可能であるという利点を有するが、横方向の分解能が低いので物標の境界の検知が容易でなく、また、検知範囲の上下方向の角度が小さいので視野が狭いなどの欠点を有する。カメラは、一般に、平面分解能が高いので物標の境界の検知が容易であり、視野角も広いという利点を有するが、物標との距離や、相対速度の精度があまり高くなく、天候や照明条件の影響を受けやすいので悪天候や、夜間の場合に物標の検知能力が低下しやすいという欠点を有する。他方、センサフュージョンは、レーダおよびカメラの双方の欠点を双方の利点で補うことが可能であり、最近の開発の主流となっている。例えば、特許文献１，２記載の物標検知装置は、１台のレーダと、１台のカメラとを含んで構成されたセンサフュージョンを備えており、レーダによって検知された物標の座標と、カメラによって検知された物標の座標とを利用して物標を検知するようになっている。
【０００４】
【特許文献１】特開２００３−３１５４４１号公報
【特許文献２】特開２００３−３１５４４２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかし、このような装置では、カメラから得られた座標には、物標との距離についての正確な情報が含まれていないので、レーダから得られた座標と、カメラから得られた座標とを用いて、物標同士を整合（マッチング）させることが容易ではないという問題がある。
【０００６】
本発明はかかる問題点に鑑みてなされたものであり、その目的は、レーダから得られた物標の座標と、カメラから得られた物標の座標とを用いて、物標同士を容易に整合（マッチング）させることの可能な物標検知装置、この物標検知装置を校正するための校正装置およびその校正方法を提供することにある。
【課題を解決するための手段】
【０００７】
本発明の校正装置は、車両前方のレーダ情報を取得するためのレーダと、車両前方のステレオ画像を取得するための右カメラおよび左カメラを有するステレオカメラと、レーダで取得したレーダ情報、ならびに右カメラで取得した２次元画像および左カメラで取得した２次元画像からなるステレオ画像を処理するための制御部とを備えたものである。 ここで、制御部は、以下の（Ａ）〜（Ｅ）の各手段を有している。
（Ａ）レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得するレーダ側３次元座標取得手段
（Ｂ）右カメラによって得られた２次元画像の中から各物標の右カメラ座標系の２次元座標を、左カメラによって得られた２次元画像の中から各物標の左カメラ座標系の２次元座標をそれぞれ抽出するカメラ側２次元座標取得手段
（Ｃ）各物標の右カメラ座標系の２次元座標および各物標の左カメラ座標系の２次元座標から視差情報を取得する視差情報取得手段
（Ｄ）視差情報を用いて、各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するカメラ側３次元座標取得手段
（Ｅ）レーダ情報に基づいて得られた各物標のレーダ座標系の３次元座標と、ステレオ画像に基づいて得られた各物標の右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求める変換係数取得手段
【０００８】
本発明の校正方法は、以下の（Ａ）〜（Ｆ）の各工程を含んでいる。
（Ａ）車両前方のレーダ情報を取得するためのレーダと、車両前方のステレオ画像を取得するための右カメラおよび左カメラを有するステレオカメラと、レーダで取得したレーダ情報、ならびに右カメラで取得した２次元画像および左カメラで取得した２次元画像からなるステレオ画像を処理するための制御部とを用意する工程
（Ｂ）レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得する工程
（Ｃ）右カメラによって得られた２次元画像の中から各物標の右カメラ座標系の２次元座標を、左カメラによって得られた２次元画像の中から各物標の左カメラ座標系の２次元座標をそれぞれ抽出する工程
（Ｄ）各物標の右カメラ座標系の２次元座標および各物標の左カメラ座標系の２次元座標から視差情報を取得する工程
（Ｅ）視差情報を用いて、各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換する工程
（Ｆ）レーダ情報に基づいて得られた各物標のレーダ座標系の３次元座標と、ステレオ画像に基づいて得られた各物標の右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求める工程
【０００９】
本願発明の校正装置および校正方法では、ステレオカメラから得られるステレオ画像から物標との距離に関係する視差情報を得るようにしたので、ステレオカメラから得られた右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換することができる。これにより、レーダから得られた各物標の座標と、視差情報に基づいて変換された右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求めることができる。
【００１０】
本発明の物標検知装置は、上記の校正装置と同様、レーダ、ステレオカメラおよび制御部とを備え、さらに、レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数が格納されている記憶部を備えたものである。ここで、制御部は、以下の（Ａ）〜（Ｄ）の各手段を有している。
（Ａ）レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得するレーダ側３次元座標取得手段
（Ｂ）変換係数を用いて、各物標のレーダ座標系の３次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換すると共に、その変換された各物標の右カメラ座標系の３次元座標および左カメラ座標系の３次元座標を、右カメラ座標系の２次元座標および左カメラ座標系の２次元座標に変換するレーダ側２次元座標取得手段
（Ｃ）右カメラによって得られた２次元画像の中から各物標の右カメラ座標系の２次元座標を、左カメラによって得られた２次元画像の中から各物標の左カメラ座標系の２次元座標をそれぞれ抽出するカメラ側２次元座標取得手段
（Ｄ）レーダ情報に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標と、ステレオ画像に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標とを対比して、車両前方の物標を特定する物標特定手段
【００１１】
本願発明の物標検知装置では、変換係数を用いるようにしたので、レーダによって得られたレーダ情報から、奥行き情報を実質的に含んた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を求めることができる。
【発明の効果】
【００１２】
本発明の校正装置および校正方法によれば、ステレオカメラから得られるステレオ画像から物標との距離に関係する視差情報を得るようにしたので、レーダ座標系の３次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求めることができる。これにより、走行中に障害物の位置を特定する際に、上記のようにして求められた変換係数を用いて、レーダから得られた物標の座標と、ステレオカメラから得られた物標の座標とを対比することができるので、存在可能性の高い物標を特定することができる。従って、レーダから得られた物標の座標と、ステレオカメラから得られた物標の座標とを用いて、物標同士を容易にマッチングさせることができる。
【００１３】
本発明の物標検知装置によれば、変換係数を用いるようにしたので、レーダによって得られたレーダ情報から、奥行き情報を実質的に含んた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を求めることができる。これにより、レーダ情報に基づいて得られた、奥行き情報を実質的に含んた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標と、ステレオ画像に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標とを対比することができるので、。存在可能性の高い物標を特定することができる。従って、レーダから得られた物標の座標と、ステレオカメラから得られた物標の座標とを用いて、物標同士を容易にマッチングさせることができる。
【発明を実施するための最良の形態】
【００１４】
以下、本発明の実施の形態について、図面を参照して詳細に説明する。
【００１５】
図１は、本発明の一実施の形態に係るセンサフュージョン（物標検知装置）の概略構成を表したものである。このセンサフュージョンは、自動車やバイクなどの前方走行車や、歩行者、障害物などの物標を検知するシステムであり、例えば、自動車Ｃに搭載されるものである。
【００１６】
このセンサフュージョンは、レーダ１、ステレオカメラ２、制御部３、記憶部４、表示部５を含んで構成されたものである。レーダ１は、例えば、レーザ光を放射するレーザレーダ、またはミリ波電磁波を放射するミリ波レーダからなり、自動車Ｃのバンパの中央部分などに取り付けられている。このレーダ１は、自動車Ｃの前方にレーザ光やミリ波電磁波を放射すると共に、自動車Ｃの前方に放射したレーザ光の反射光やミリ波電磁波の反射波を検知し、さらに、検知した反射光や反射波から自動車Ｃの前方に存在する物標の方位θ、距離ｄおよび相対速度ｖなどを計測するようになっている。ステレオカメラ２は、右カメラ２１および左カメラ２２を備えている。右カメラ２１および左カメラ２２は、例えば、電荷結合素子（ＣＣＤ）からなり、自動車Ｃのフロントガラスの内壁であって、互いに所定の間隔を隔てると共に、路面から同じ高さのところに取り付けられている。このステレオカメラ２は、自動車Ｃの前方を互いに異なる視点から撮影して、右カメラ２１で撮影された２次元画像と左カメラ２２で撮影された２次元画像とからなるステレオ画像を取得するようになっている。
【００１７】
制御部３は、例えば、ＤＳＰ（Digital Signal Processor）などにより構成され、レーダ１およびステレオカメラ２によって得られた情報を処理して自動車Ｃの前方の障害物の位置や種類などを所定の演算により特定するようになっている。記憶部４は、例えば、ＲＡＭ（Random Access Memory）およびＨＤ（hard disk)からなり、記憶部４には、センサフュージョンを校正するためのプログラムや、障害物を検知するためのプログラム、制御部３で得られた演算結果などが格納されている。表示部５は、例えば、液晶表示装置であり、制御部３で得られた演算結果に基づく情報を表示するようになっている。
【００１８】
次に、センサフュージョンをキャリブレーションする手順について説明する。ただし、これに先立って、このキャリブレーションに用いる反射板６について説明する。
【００１９】
（反射板６）
図２および図３は反射板６の一例を示したものであり、図２はレーダ１がレーザレーダのときの反射板を、図３はレーダ１がミリ波レーダのときの反射板をそれぞれ例示したものである。図４は、センサフュージョンをキャリブレーションする際の、反射板６の配置例を表すものである。
【００２０】
図２の反射板６は、反射ブロック６Ａと、吸収ブロック６Ｂとを交互に配置してなる格子状のパターンを有している。反射ブロック６Ａはレーザ光を強く反射する材質により構成されており、吸収ブロック６Ｂはレーザ光を強く吸収する材質により構成されている。他方、図３の反射板６は、平板６Ｃと、リフレクタ６Ｄとを有している。平板６Ｃは、ミリ波電磁波を透過する材料（布や発砲スチロールなど）からなり、リフレクタ６Ｄを裏面で固定するようになっている。リフレクタ６Ｄは、例えば、三角錐形状の金属板からなり、リフレクタ６Ｄの裏面に格子状に配列されている。このリフレクタ６Ｄは、開口部側から入射したミリ波電磁波を反射するようになっている。
【００２１】
ここで、反射ブロック６Ａや平板６Ｃのうちリフレクタ６Ｄの配置されている部分は、レーザ光またはミリ波電磁波を強く反射するので、反射ブロック６Ａ等の横幅Ｄ１と同じサイズの横幅を有する物標がそこに存在する、とレーダ１に認識され得るものである。他方、吸収ブロック６Ｂや平板６Ｃのうちリフレクタ６Ｄの配置されていない部分は、レーザ光やミリ波電磁波をほとんど反射しないので、吸収ブロック６Ｂ等の横幅Ｄ２と同じサイズの横幅の空間がそこに存在する、とレーダ１に認識され得るものである。
【００２２】
各反射ブロック６Ａおよび各吸収ブロック６Ｂのサイズは、レーダ１の分解能や、レーダ１と反射板６との距離に応じて設定される。反射ブロック６Ａ等の横幅Ｄ１は、レーダ１が分解能と同程度の大きさの物標を認識することは困難であることから、少なくともレーダ１の分解能の２倍程度であることが必要である。吸収ブロック６Ｂ等の横幅Ｄ２は、レーダ１は複数の物標が互いに近接配置されるとこれらを別個の物標として明確に区別することが困難となることから、少なくともレーダ１の分解能の３倍程度であることが必要である。反射ブロック６Ａ等および吸収ブロック６Ｂ等の縦方向のサイズＨは、レーダ１のスキャン平面に垂直な方向の誤差の範囲内に複数の物標が存在するとこれらを別個の物標として明確に区別することが困難となることから、少なくともレーダ１のスキャン平面に垂直な方向の誤差程度であることが必要である。
【００２３】
なお、反射板６の横方向のサイズは、レーダ１によってスキャンされる範囲と同程度の大きさとなっており、反射板６の縦方向のサイズは、反射ブロック６Ａを縦方向に複数配置できる大きさ以上の大きさとなっていれば特に制限されるものではない。また、キャリブレーションの便宜上、最下行の反射ブロック６Ａ等は、自動車Ｃに搭載されたレーダ１と同じ高さのところに配置されているものとする。
【００２４】
（レーダ１自身のキャリブレーション、図４〜図８）
まず、作業者Ｐは反射板６を設置する（ステップＳ１）。具体的には、図４に示したように、反射板６を自動車Ｃの車軸Ｘ１と垂直に交わるようにして、自動車Ｃの前方（自動車Ｃに設置されたレーダ１およびステレオカメラ２の前方）の所定の距離に配置する。続いて、レーダ１がレーザレーダの場合には、反射板６の格子状のパターンのうち地面に最も近い最下行のパターンだけを残して、その他の部分を、レーザ光を強く吸収するカバー（図示せず）で覆い、レーダ１がミリ波レーダの場合には、平板６Ｃに取り付けられたリフレクタ６Ｄのうち地面に最も近い最下行のリフレクタ６Ｄだけを残して、その他のリフレクタ６Ｄを平板６Ｃから取り外す。
【００２５】
次に、作業者Ｐは、制御部３にレーダ１自身のキャリブレーションの実行を要求する（ステップＳ２）。すると、制御部３は、最下行のパターンに含まれる各物標（反射ブロック６Ａ等）の３次元座標と、レーダ１の３次元座標、俯角ξおよびずれ角φとを計測する。
【００２６】
ここで、俯角ξとは、図５（図１の側面図）に示したように、レーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）において、レーダ１のスキャン平面Ａと、（Ｘｍ，Ｚｍ）平面Ｂ（一般には路面と平行な平面）とが交わる角度（微小偏移角）のことである。なお、レーダ１がレーザレーダの場合には、レーザ光がある程度の広がり角を有しているので、その広がり角を二等分する面をスキャン平面Ａとする。ずれ角φとは、図６（図１の上面図）に示したように、レーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の（Ｘｍ，Ｚｍ）平面と平行な平面において直線Ｘ２（後述）と垂直に交わる直線Ｘ３と、自動車Ｃの車軸Ｘ１とが交わる角度のことである。
【００２７】
具体的には、まず、制御部３は、レーダ１に対してスキャンを要求する（ステップＳ３）。レーダ１はスキャンの要求を受信する（ステップＳ４）と、最下行のパターンをスキャンして、スキャン平面内に存在する物標（反射ブロック６Ａ等）の方位θｉを検知すると共に、検知した最下行の物標ごとに、距離ｄｉおよび相対速度ｖｉを計算したのち（ステップＳ５）、最下行の各物標のレーダ情報（θｉ，ｄｉ，ｖｉ）を制御部３に送信する（ステップＳ６）。なお、θや、ｄ、ｖのサフィックスｉは１以上、最下行のパターンに含まれる反射ブロック６Ａの数ｎ以下の整数である。
【００２８】
制御部３はレーダ１から最下行の各物標のレーダ情報（θｉ，ｄｉ，ｖｉ）を受信したのち（ステップＳ７）、レーダ１によって検知されるはずの物標が全て検知されているか否かを判定する（ステップＳ８）。レーダ１によって検知されるはずの物標が全て検知されている場合には、制御部３は、以下の式（１）を用いて、検知された最下行の各物標のレーダ情報（θｉ，ｄｉ，ｖｉ）をレーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）へ変換したのち（ステップＳ９）、最下行の各物標のレーダ情報（θｉ，ｄｉ，ｖｉ）と、最下行の各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）とを記憶部４に格納する（ステップＳ１０）。
【００２９】
【数１】
【００３０】
なお、式（１）はレーダによる計測データをレーダ座標系の３次元座標へ変換する式である。
【００３１】
一方、レーダ１によって検知されるはずの全ての物標が検知されていない場合には、制御部３はその旨の表示を表示部５に要求する（ステップＳ１１）。表示部５は、その要求を受信すると、全ての物標が検知されていない旨を表示する（ステップＳ１２）。作業者Ｐは、その表示を確認し、最下行の全ての物標が検知されるようにレーダ１の向き（俯角ξ、ずれ角φ）を調整し直したのち（ステップＳ１３）、再度、ステップＳ１〜Ｓ８を実行する。すなわち、エイミングを実行する。このとき、最下行の全ての物標が検知されるようにするためには、例えば、最下行の各物標のうち中央に配置された物標に対応する反射光または反射波の強度が最も大きくなるように俯角ξおよびずれ角φを調整する。
【００３２】
その結果、レーダ１によって最下行の全ての物標が検知された場合には制御部３は上記のステップＳ９，Ｓ１０を順次実行し、検知されていなかった場合には、制御部３は再度、ステップＳ１１を実行し、最終的にはステップ１０まで実行する。
【００３３】
次に、制御部３は、レーダ座標系へ変換された各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が、レーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の（Ｘｍ，Ｙｍ）平面上に分布しているか否かを判定する（ステップＳ１４）。各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が（Ｘｍ，Ｙｍ）平面上に分布していない場合には、各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が反射板６上に分布していない、つまり、レーダ１が反射板６に対して正体していないということになるので、レーダ１の傾き具合を正確に求めておくことにより、レーダ１が反射板６に対して正体しているときの、各物標のレーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の３次元座標（Ｘｍｉ′′，Ｙｍｉ′′，Ｚｍｉ′′）（後述）を求めることが可能となる。
【００３４】
判定の結果、（Ｘｍ，Ｙｍ）平面上に分布していない場合には、制御部３は、例えば、各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）（図６の黒丸）を最小二乗法を用いて直線近似を行い、最小二乗法により得られた各物標の３次元座標（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）（図６の白丸）と、それら３次元座標（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）を含む直線Ｘ２と、レーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の（Ｘｍ，Ｚｍ）平面において直線Ｘ２と垂直に交わる直線Ｘ３とを求める。続いて、三角測量法を用いて、レーダ１の座標、俯角ξおよびずれ角φを計算する（ステップＳ１５）。
【００３５】
ここで、レーダ１の座標は、例えば、図６に示したように、（Ｘｍ，Ｚｍ）平面であって、かつ直線Ｘ２上の各物標の３次元座標（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）を含む平面内に、各物標の３次元座標（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）のうち両端の物標の３次元座標（Ｘｍ１′，Ｙｍ１′，Ｚｍ１′），（Ｘｍｎ′，Ｙｍｎ′，Ｚｍｎ′）の位置を中心として、その座標に対応する距離ｄ１，ｄｎを半径とする円弧をそれぞれ描き、双方の円弧の交わる２つの点のうち自動車Ｃ側の点に対応する座標を計算することにより求められる。俯角ξは、上記したように、例えば、最下行の各物標のうち中央に配置された物標に対応する反射光または反射波の強度が最も大きくなるように調整されており、ほとんど０°となるので、例えば、デフォルトで０°としてもよい。ずれ角φは、例えば、レーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の（Ｘｍ，Ｚｍ）平面において直線Ｘ２と垂直に交わる直線Ｘ３と、自動車Ｃの車軸Ｘ１とが交わる角度を計算することにより求められる。
【００３６】
次に、制御部３は、俯角ξおよびずれ角φを用いて、各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）または（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）が反射板６上に分布するように、これら３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）または（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）を補正する（ステップＳ１６）。
【００３７】
一方、レーダ座標系へ変換された各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が、（Ｘｍ，Ｙｍ）平面上に分布している場合には、各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が反射板６上に分布していることを意味するので、レーダ１の俯角ξおよびずれ角φはほとんど０°になっていると推測できる。従って、この場合には、３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）をわざわざ補正する必要はないので、３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）の補正を行わない。もちろん、レーダ１の俯角ξおよびずれ角φを求めるようにしてもよいが、その場合には、上記のステップＳ１５，Ｓ１６を実行することとなる。
【００３８】
なお、以下では、３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）の補正が行われた場合の手順について説明するが、この手順は、補正の行われていない３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）についてももちろん適用可能である。
【００３９】
次に、上記ステップ１６で補正することにより得られた、最下行の各物標の３次元座標（Ｘｍｉ′′，Ｙｍｉ′′，Ｚｍｉ′′）、俯角ξおよびずれ角φを記憶部４に格納する（ステップＳ１７）。
【００４０】
次に、制御部３は、最下行の各物標の３次元座標（Ｘｍｉ′′，Ｙｍｉ′′，Ｚｍｉ′′）を利用して、最下行以外の各物標（反射ブロック６Ａ）の座標を推定する（ステップＳ１８）。例えば、反射ブロック６Ａおよび吸収ブロック６Ｂの高さは共にＨなので、最下行から１行上がるごとに、ＹｍｉにＨを加えることにより、最下行以外の各物標の座標（Ｘｍｉ′′，Ｙｍｉ′′＋Ｈ×（ｓ−１），Ｚｍｉ′′）を求める。ここで、ｓは、最下行から数えたブロックの数である（最下行はｓ＝１）。その後、最下行以外の各物標の座標（Ｘｍｉ′′，Ｙｍｉ′′＋Ｈ×（ｓ−１），Ｚｍｉ′′）を記憶部４に格納する（ステップＳ１９）。
【００４１】
次に、制御部３は、レーダ１自身のキャリブレーションの完了の出力を表示部５に要求する（ステップＳ２０）。表示部５は、その要求を受信すると、レーダ１自身のキャリブレーションが完了した旨を出力する（ステップＳ２１）。このようにして、レーダ１自身のキャリブレーションが完了する。
【００４２】
（ステレオカメラ２自身のキャリブレーション、図９）
次に、作業者Ｐは、反射板６を調整する（ステップＳ２２）。具体的には、レーダ１がレーザレーダの場合には反射板６を覆っているカバーを外して反射板６の格子状パターンを全て露出させ、他方、レーダ１がミリ波レーダの場合には先に取り外したリフレクタ６Ｄを元の位置に取り付ける。続いて、作業者Ｐはステレオカメラ２自身のキャリブレーションを制御部３に要求する（ステップＳ２３）。すると、制御部３はステレオカメラ２に第１回目の撮影を要求する（ステップＳ２４）。ステレオカメラ２は、第１回目の撮影の要求を受信する（ステップＳ２５）と、反射板６のステレオ画像を撮影する（ステップＳ２６）。
【００４３】
次に、制御部３は、作業者Ｐに対して反射板６の車軸Ｘ１に対する角度を所定の角度に変更する指令を表示するように表示部５に要求する（ステップＳ２７）。表示部５は、制御部３からの表示要求を受信すると、その指令を表示する（ステップＳ２８）。作業者Ｐは、その指令に従って反射板６の角度を変更する（ステップＳ２９）。続いて、制御部３はステレオカメラ２に次の撮影を要求する（ステップＳ３０）。ステレオカメラ２は、次の撮影の要求を受信する（ステップＳ３１）と、反射板６のステレオ画像を撮影する（ステップＳ３２）。
【００４４】
引き続き、反射板６の角度を徐々に変えながらステップＳ２７〜Ｓ３３を複数回繰り返したのち、撮影回数が所定の回数に到達する（ステップＳ３３）と、制御部３は撮影した全てのステレオ画像の送信をステレオカメラ２に要求する（ステップＳ３４）。ステレオカメラ２は、その要求を受信する（ステップＳ３５）と、撮影した全てのステレオ画像を制御部３に送信する（ステップＳ３６）。
【００４５】
制御部３は、ステレオ画像を受信する（ステップＳ３７）と、受信したステレオ画像を用いて、ステレオカメラ２の内部パラメータ（例えば、焦点距離Ｆ、光学中心位置および光学歪補正係数）と、外部パラメータ（例えば、右カメラ２１および左カメラ２２の間の並進行列式および回転行列式）とを計算したのち（ステップＳ３８）、内部パラメータおよび外部パラメータと、第１回目に撮影されたステレオ画像とを記憶部４に格納する（ステップＳ３９）。
【００４６】
次に、制御部３は、ステレオカメラ２自身のキャリブレーションの完了の出力を表示部５に要求する（ステップＳ４０）。表示部５は、その要求を受信すると、ステレオカメラ２自身のキャリブレーションが完了した旨を出力する（ステップＳ４１）。このようにして、ステレオカメラ２自身のキャリブレーションが完了する。
【００４７】
（レーダ１およびステレオカメラ２の相互間のキャリブレーション、図１０）
制御部３は、記憶部４に格納された第１回目のステレオ画像を読み出し、そのステレオ画像の中から、各物標の右カメラ画像座系の２次元座標（Ｘｒｊ，Ｙｒｊ）および左カメラ画像座系の２次元座標（Ｘｌｊ，Ｙｌｊ）を抽出する（ステップＳ４２）。具体的には、反射ブロック６Ａの４つの角を抽出し、その４つの角の中央の座標を物標の座標として抽出する。なお、Ｘｒ、Ｙｒ、Ｘｌ、Ｙｌのサフィックスｊは１以上、ステレオカメラ２によって検知された物標の数以下の整数である。
【００４８】
続いて、制御部３は、２次元座標（Ｘｒｊ，Ｙｒｊ），（Ｘｌｊ，Ｙｌｊ）と、以下の式（２）〜（７）とを用いて、各物標の右カメラ座標系の３次元座標（ＸＲｊ，ＹＲｊ，ＺＲｊ）および左カメラ座標系の３次元座標（ＸＬｊ，ＹＬｊ，ＺＬｊ）を求める（ステップＳ４３）。
【００４９】
【数２】
【数３】
【数４】
【数５】
【数６】
【数７】
【００５０】
ここで、ｂはステレオカメラ２の基線長、すなわち、右カメラ２１および左カメラ２２のそれぞれの中心位置間の距離である。また、ｄ（Ｘｒｊ，Ｙｒｊ）は右カメラの２次元座標の視差情報であり、ｄ（Ｘｌｊ，Ｙｌｊ）は左カメラの２次元座標の視差情報である。視差情報ｄ（Ｘｒｊ，Ｙｒｊ）、ｄ（Ｘｌｊ，Ｙｌｊ）は、物標との距離に関係するものであり、右カメラ２１で撮影された２次元画像と、左カメラ２２で撮影された２次元画像とを用いて求めることができる。具体的には、右カメラ２１および左カメラ２２が互いに等しい高さに配置されており、右カメラ２１で撮影された２次元画像中の各物標の２次元座標（Ｘｒｊ，Ｙｒｊ）、および左カメラ２２で撮影された２次元画像中の各物標の２次元座標（Ｘｌｊ，Ｙｌｊ）のうち、同一の物標のＹ座標は互いに一致することから、同一の物標のＸ座標の差分の絶対値｜Ｘｒｊ−Ｘｌｊ｜が視差情報ｄ（Ｘｒｊ，Ｙｒｊ）、ｄ（Ｘｌｊ，Ｙｌｊ）となる。従って、この場合の視差情報ｄ（Ｘｒｊ，Ｙｒｊ）、ｄ（Ｘｌｊ，Ｙｌｊ）は、Ｙ座標とは無関係な値となる。
【００５１】
次に、制御部３は、レーダ１によって計測され、または推定によって求められた各物標の座標（Ｘｍｉ′′，Ｙｍｉ′′＋Ｈ×（ｓ−１），Ｚｍｉ′′）を記憶部４から読み出したのち、レーダ１から得られたレーダ座標系の３次元座標（Ｘｍｉ′′，Ｙｍｉ′′＋Ｈ×（ｓ−１），Ｚｍｉ′′）と、ステレオカメラ２から得られた右カメラ座標系の３次元座標（ＸＲｊ，ＹＲｊ，ＺＲｊ）および左カメラ座標系の３次元座標（ＸＬｊ，ＹＬｊ，ＺＬｊ）とを、以下の式（８），（９）に代入して、各物標に対するＲｒｊ，Ｒｌｊ，Ｔｒｊ，Ｔｌｊを求める（ステップＳ４４）。ここで、Ｒｒｊ，Ｒｌｊはレーダ座標系からカメラ座標系（ＸＲ，ＹＲ，ＺＲ），（ＸＬ，ＹＬ，ＺＬ）への回転行列式であり、Ｔｒｊ，Ｔｌｊはレーダ座標系からカメラ座標系への並進行列式である。
【００５２】
【数８】
【数９】
【００５３】
なお、式（８）はレーダ座標系の３次元座標を右カメラ座標系の３次元座標へ変換する式であり、式（９）はレーダ座標系の３次元座標を右カメラ座標系の３次元座標へ変換する式である。
【００５４】
続いて、制御部３は、各物標に対するＲｒｊ，Ｒｌｊ，Ｔｒｊ，Ｔｌｊの平均値Ｒｒ，Ｒｌ，Ｔｒ，Ｔｌを求めたのち（ステップＳ４５）、これらを記憶部４に格納する（ステップＳ４６）。
【００５５】
このように、本実施の形態では、レーダ１からのレーダ情報を補正するにより得られた各物標の座標（Ｘｍｉ′′，Ｙｍｉ′′＋Ｈ×（ｓ−１），Ｚｍｉ′′）と、ステレオカメラ２から得られた右カメラ座標系の３次元座標（ＸＲｊ，ＹＲｊ，ＺＲｊ）および左カメラ座標系の３次元座標（ＸＬｊ，ＹＬｊ，ＺＬｊ）とを用いて、レーダ座標系の３次元座標と、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを相互に変換するためのＲｒ，Ｒｌ，Ｔｒ，Ｔｌを求めることができる。
【００５６】
次に、制御部３は、センサフュージョンのキャリブレーションの完了の出力を表示部５に要求する（ステップＳ４７）。表示部５は、その要求を受信すると、センサフュージョンのキャリブレーションが完了した旨を出力する（ステップＳ４８）。このようにして、レーダ１およびステレオカメラ２の相互間のキャリブレーションが行われ、センサフュージョンのキャリブレーションが完了する。
【００５７】
このように、本実施の形態では、レーダ１から各物標の座標を得ると共に、ステレオカメラ２から各物標の座標を得ることにより、レーダ１自身や、ステレオカメラ２自身だけでなく、レーダ１およびステレオカメラ２の相互間のキャリブレーションをも一括して行うことができる。つまり、レーダ１およびステレオカメラ２を含んで構成されたセンサフュージョンを同時キャリブレーションすることができる。
【００５８】
次に、自動車Ｃの走行中に、自動車Ｃの前方に存在する障害物の存在、位置および種類を特定する手順について説明する。
【００５９】
（障害物の位置の特定、図１１〜図１４）
まず、制御部３は、レーダ１に対してスキャンの要求を、ステレオカメラ２に対して撮影の要求を、互いに同期して行う（ステップＳ４９）。レーダ１はスキャンの要求を受信する（ステップＳ５０）と、スキャン平面内をスキャンして、そのスキャン平面内に存在する物標の方位θｊを検知すると共に、検知した物標ごとに、距離ｄｊおよび相対速度ｖｊを計算したのち（ステップＳ５１）、計測されたレーダ情報（θｉ，ｄｉ，ｖｉ）を制御部３に送信する（ステップＳ５２）。なお、ここでのθや、ｄ、ｖのサフィックスｉは１以上、レーダ１によって検知された物標の数以下の整数である。他方、ステレオカメラ２は、撮影の要求を受信する（ステップＳ５３）と、自動車Ｃの前方のステレオ画像を撮影する（ステップＳ５４）と共に、そのステレオ画像を制御部３に送信する（ステップＳ５５）。
【００６０】
次に、制御部３は、レーダ１からのレーダ情報（θｉ，ｄｉ，ｖｉ）およびステレオカメラ２からのステレオ画像を受信する（ステップＳ５６）。すると、まず、制御部３は、式（１）および以下の式（１０）〜（１３）を用いて、受信したレーダ情報（θｉ，ｄｉ，ｖｉ）を右カメラ座標系の３次元座標（ＸｍＲｉ，ＹｍＲｉ，ＺｍＲｉ）および左カメラ座標系の３次元座標（ＸｍＬｉ，ＹｍＬｉ，ＺｍＬｉ）に変換すると共に、右カメラ座標系の２次元座標（Ｘｍｒｉ，Ｙｍｒｉ）および左カメラ座標系の２次元座標（Ｘｍｌｉ，Ｙｍｌｉ）に変換する（ステップＳ５７）。続いて、制御部３は、受信したステレオ画像の中から、物標の角を抽出することにより、各物標の右カメラ座標系の２次元座標（Ｘｒｊ，Ｙｒｊ）および左カメラ座標系の２次元座標（Ｘｌｊ，Ｙｌｊ）を求める（ステップＳ５８）。なお、ここでのＸｒ，Ｙｒ，Ｘｌ，Ｙｌのサフィックスｊは１以上、ステレオ画像の中から検知された物標の総数以下の整数である。
【００６１】
【数１０】
【数１１】
【数１２】
【数１３】
【００６２】
なお、式（１０）はレーダ座標系の３次元座標を右カメラ座標系の３次元座標へ変換する式であり、式（１１）はレーダ座標系の３次元座標を左カメラ座標系の３次元座標へ変換する式である。式（１２）は式（１０）で得られた３次元座標を２次元座標へ変換する式であり、式（１３）は式（１１）で得られた３次元座標を２次元座標へ変換する式である。ここで、λは斎次カメラ座標系用係数であり、（Ｘｒ，Ｙｒ）は右カメラ座標系の２次元座標であり、（Ｘｌ，Ｙｌ）は左カメラ座標系の２次元座標であり、（ＸＲ，ＹＲ，ＺＲ）は右カメラ座標系の３次元座標であり、（ＸＬ，ＹＬ，ＺＬ）は左カメラ座標系の３次元座標であり、ｆｘｒ，ｆｙｒは右カメラの焦点距離を画素の横方向または縦方向のサイズで割った値であり、ｆｘｌ，ｆｙｌは左カメラの焦点距離を画素の縦方向または縦方向のサイズで割った値であり、ｕｏ，ｖｏは右カメラおよび左カメラのそれぞれの光学中心位置の画像座標である。なお、ｆｘｌ，ｆｙｌ，ｕｏおよびｖｏは、上記したステレオカメラ２自身のキャリブレーションにおいて得られたものである。
【００６３】
次に、制御部３は、レーダ１によって検知された１または複数の物標が、ステレオカメラ２によって検知された１または複数の物標に含まれているか否かを判定する。すなわちレーダ１によって検知された１または複数の物標と、ステレオカメラ２によって検知された１または複数の物標とのマッチングを行う（ステップＳ５９）。
【００６４】
具体的には、まず、レーダ１から得られた２次元座標（Ｘｍｒｉ，Ｙｍｒｉ），（Ｘｍｌｉ，Ｙｍｌｉ）と、ステレオカメラ２から得られた２次元座標（Ｘｒｊ，Ｙｒｊ），（Ｘｌｊ，Ｙｌｊ）とを対比して、互いに同一または近似する座標が存在するか否かを判定する。その結果、互いに同一または近似する座標が存在する場合には、その座標には物標が存在する可能性が高いことになる。続いて、レーダ１から得られた２次元座標（Ｘｍｒｉ，Ｙｍｒｉ），（Ｘｍｌｉ，Ｙｍｌｉ）に対応する距離ｄｉと、ステレオカメラ２から得られた、レーダ１で検知された物標と同一と思われる物標の２次元座標（Ｘｒｊ，Ｙｒｊ），（Ｘｌｊ，Ｙｌｊ）の、式（７）によって得られる奥行きＺＬｊとを、存在可能性の高い物標について対比する。その結果、距離ｄｉと、奥行きＺＬｊとが一致または近似する（マッチする）場合には、レーダ１が検知した物標と、ステレオカメラ２が検知した物標とが互いに同一の物標であることになり、逆に、マッチしない場合には、レーダ１が検知した物標と、ステレオカメラ２が検知した物標とが互いに異なる物標であることになる。制御部３は、このようにしてマッチングした物標を障害物候補として抽出する（ステップＳ６０）。
【００６５】
次に、制御部３は、ステップＳ５０からステップ５９を繰り返し行ったときに、障害物候補の物標の距離ｄｉの変化の割合と、その奥行きＺＬｊの変化の割合とを対比する（ステップＳ６１）。その結果、距離ｄｉの変化の割合と、奥行きＺＬｊの変化の割合とが互いに全く異なる場合には、その物標は実在しない可能性が極めて高いので、その物標を障害物候補から除外する（ステップＳ６２）。なお、互いにほぼ等しい場合には、その物標が実在する可能性が極めて高いので、その物標を障害物候補として残存させる。
【００６６】
次に、制御部３は、障害物候補の物標が障害物存在可能領域内にあるか否かを判定する。ここで、障害物存在可能領域とは、障害物が存在することの可能な領域のことであり、例えば、図１４に示したような走行路面Ｓ１および路側Ｓ２を含む。走行路面Ｓ１とは、自動車Ｃが走行可能な路面のことであり、路側Ｓ２とは、走行路面Ｓ１に沿って設けられた路面のことである。なお、図１４では、走行路面Ｓ１および路側Ｓ２は、車線区分線Ｌによって区分されている。
【００６７】
判定の結果、障害物候補の物標が、例えば図１４に示したように、走行路面Ｓ１上にある場合には、その物標は、走行路面Ｓ１上に存在しうる障害物のリスト（例えば、自動車、バイク、自転車、落下物）の中の一の障害物であることになり、他方、距離ｄｉの物標が路側Ｓ２上にある場合には、その物標は、路側Ｓ２上に存在しうる障害物のリスト（例えば、ガードレール、道路看板）の中の一の障害物であることになる。従って、障害物候補の物標が障害物存在可能領域の外にある場合には、その物標は自動車Ｃにとって障害物とはなり得ない物標であることになるので、その場合には、その物標を障害物候補から除外する（ステップＳ６３）。このようにして、障害物候補の物標が特定されると共に、その物標の位置が特定される。
【００６８】
（障害物７の種類の特定、図１２〜図１４）
次に、制御部３は、例えば、ステレオカメラ２から得られた２次元座標（Ｘｒｊ，Ｙｒｊ），（Ｘｌｊ，Ｙｌｊ）と、レーダ１から得られた２次元座標（Ｘｍｒｉ，Ｙｍｒｉ），（Ｘｍｌｉ，Ｙｍｌｉ）に対応する距離ｄｉとから、障害物害物候補の物標の形状および大きさを推定する（ステップＳ６４）。続いて、推定した形状および大きさならびに相対速度ｖｉと対応する障害物をリストの中から特定する（ステップＳ６５）。例えば、障害物害物候補の物標が図１４の障害物７であり、障害物７が相対速度ｖｉで走行している場合には、走行路面Ｓ１上に存在しうる障害物のリスト（例えば、自動車、バイク、自転車、落下物）の中からバイクが特定される。このようにして、障害物の種類が特定される。
【００６９】
以上のように、本実施の形態では、ステレオカメラ２から得られるステレオ画像から物標との距離に関係する視差情報を得るようにしたので、ステレオカメラ２から得られた右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換することができる。これにより、レーダ１から得られた各物標の座標と、視差情報に基づいて変換された右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するためのＲｒ，Ｒｌ，Ｔｒ，Ｔｌを求めることができる。その結果、走行中に障害物の位置を特定する際に、上記のようにして求められたＲｒ，Ｒｌ，Ｔｒ，Ｔｌを用いることにより、レーダ１から得られた物標の座標と、ステレオカメラ２から得られた物標の座標とを対比することが可能となる。その結果、存在可能性の高い物標を特定することができる。
【００７０】
従って、本実施の形態では、走行中に障害物の位置を特定する際に、レーダ１によって検知された物標の座標と、ステレオカメラ２によって検知された物標の座標とを用いて、物標同士を容易にマッチングさせることができる。
【００７１】
特に、本実施の形態では、ステレオカメラ２から得られるステレオ画像から、Ｙ座標とは無関係な、物標との距離に関係する視差情報を得るようにしたので、ステレオカメラ２の俯角が自動車Ｃの前方の物標を撮影することが可能な範囲内のどのような値になっていようとも、ステレオカメラ２から得られた右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に常に正確に変換することができる。これにより、レーダ１から得られた各物標の座標と、視差情報に基づいて変換された右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するためのＲｒ，Ｒｌ，Ｔｒ，Ｔｌを常に正確に求めることができる。その結果、走行中に障害物の位置を特定する際に、例えば、自動車Ｃのサスペンションによりステレオカメラ２の俯角が経時的に変動するような場合であっても、上記のようにして求められたＲｒ，Ｒｌ，Ｔｒ，Ｔｌを用いることにより、レーダ１から得られた物標の座標と、ステレオカメラ２から得られた物標の座標とを正確に対比することが可能となる。その結果、存在可能性の高い物標を正確に特定することができる。
【００７２】
また、本実施の形態では、走行中に障害物の位置を特定する際に、Ｒｒ，Ｒｌ，Ｔｒ，Ｔｌを用いているので、レーダ１によって得られた各物標のレーダ座標系の３次元座標を、奥行き情報を実質的に含んた右カメラ座標系の２次元座標および左カメラ座標系の２次元座標に変換することができる。これにより、レーダ１から得られた、奥行き情報を実質的に含んた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標と、ステレオ画像に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標とを対比することができるので、存在可能性の高い物標を特定することができる。従って、レーダから得られた物標の座標と、ステレオカメラから得られた物標の座標とを用いて、物標同士を容易にマッチングさせることができる。
【００７３】
さらに、本実施の形態では、走行中に障害物の位置を特定する際に、示唆情報から物標の奥行きＺＬｊを求めるようにしたので、レーダ１から得られた物標との距離ｄｉと、ステレオカメラ２から得られた、レーダ１で検知された物標と同一と思われる物標の奥行きＺＬｊとを対比することができる。これにより、例えば、レーダ１から得られた物標の２次元座標と、ステレオカメラ２から得られた物標の２次元座標とが互いに近接するものの、距離ｄｉと奥行きＺＬｊとが互いに大きく異なる場合には、これらを互いに別個の物標として認識することができる。一方、例えば、レーダ１から得られた物標の２次元座標と、ステレオカメラ２から得られた物標の２次元座標とが互いに等しいかまたは近接している場合であって、かつ、距離ｄｉと奥行きＺＬｊとが互いに等しいかまたは近接しているときには、これらを互いに同一の物標として認識することができる。
【００７４】
このように、走行中に障害物の位置を特定する際に、レーダ１から得られた物標の２次元座標と、ステレオカメラ２から得られた物標の２次元座標とを互いに対比するだけでなく、距離ｄｉと奥行きＺＬｊとを互いに対比するようにしたので、レーダ１から得られた物標の２次元座標と、ステレオカメラ２から得られた物標の２次元座標とが互いに近接している場合であっても、それらが互いに異なるものなのか、互いに同一のものなのかを正確に特定することができる。
【００７５】
また、本実施の形態では、キャリブレーションにおいて、レーダ１によって検知された各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が反射板６上に分布していない場合には、演算により求めたレーダ１の俯角ξおよびずれ角φを用いて、３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）を補正するようにしたので、レーダ１の向きに起因する誤差をＲｒ，Ｒｌ，Ｔｒ，Ｔｌから取り除くことができる。これにより、レーダ１から得られた物標の座標と、ステレオカメラ２から得られた物標の座標とを極めて正確に対比することが可能となるので、走行中に障害物の位置を特定する際に、レーダ１によって検知された物標の座標と、ステレオカメラ２によって検知された物標の座標とを用いて、物標同士を極めて正確にマッチングさせることができる。
【００７６】
また、本実施の形態では、走行中に障害物の位置を特定する際に、レーダ１から得られた物標との距離ｄｉの変化の割合と、ステレオカメラ２から得られた、レーダ１で検知された物標と同一と思われる物標の奥行きＺＬｊの変化の割合とを対比するようにしたので、レーダ１から得られた物標の２次元座標と、ステレオカメラ２から得られた物標の２次元座標とが互いに近接している場合であっても、それらが互いに異なるものなのか、互いに同一のものなのかを極めて正確に特定することができる。これにより、走行中に障害物の位置を特定する際に、レーダ１によって検知された物標の座標と、ステレオカメラ２によって検知された物標の座標とを用いて、物標同士を極めて正確にマッチングさせることができる。
【００７７】
また、本実施の形態では、障害物７の位置および種類を特定したのちに、レーダ１によって検知された障害物７のレーダ情報（θｉ，ｄｉ，ｖｉ）およびステレオカメラ２によって検知された障害物７のステレオ画像のいずれか一方に何らかの不具合（例えばノイズの混入）が発生して、いずれか一方の情報を使うことができなくなった場合であっても、使うことの可能な方の情報だけを用いて、障害物７の位置を追跡することができる。この場合に、双方の情報を用いて障害物７の位置を追跡していたときから、一方の情報だけを用いて障害物７の位置を追跡するように切り換えた瞬間に、障害物７の位置が不連続に変動することはない。障害物７の位置を追跡する際には、キャリブレーションによって得られた定数を含む式（１）、（７）、（１０）〜（１３）を用いているので、レーダ１によって検知された物標の座標と、ステレオカメラ２によって検知された物標の座標とを、一の座標系のほぼ同一の座標に変換することができるからである。
【００７８】
以上、実施の形態および実施例を挙げて本発明を説明したが、本発明は、これらに限定されるものではなく、種々の変形が可能である。
【図面の簡単な説明】
【００７９】
【図１】本発明の一実施の形態に係る物標検知装置の概略構成図である。
【図２】図１の反射板の一構成例を説明するための図である。
【図３】図１の反射板の他の構成例を説明するための図である。
【図４】物標検知装置のキャリブレーションを行う際の配置図である。
【図５】俯角ξを説明するための側面図である。
【図６】ずれ角φを説明するための上面図である。
【図７】物標検知装置のキャリブレーション手順を説明するための流れ図である。
【図８】図７に続く手順を説明するための流れ図である。
【図９】図８に続く手順を説明するための流れ図である。
【図１０】図９に続く手順を説明するための流れ図である。
【図１１】障害物の検知についての手順を説明するための流れ図である。
【図１２】図１１に続く手順を説明するための流れ図である。
【図１３】図１２に続く手順を説明するための流れ図である。
【図１４】障害物の検知についての手順を説明するための斜視図である。
【符号の説明】
【００８０】
１…レーダ、２…ステレオカメラ、２１…右カメラ、２２…左カメラ、３…制御部、４…記憶部、５…表示部、６…反射板、６Ａ…反射ブロック、６Ｂ…吸収ブロック、６Ｃ…平板、６Ｄ…リフレクタ、７…障害物、Ａ…スキャン平面、Ｂ…（Ｘｍ，Ｚｍ）平面、Ｃ…自動車、ｄ１，ｄ２…半径、Ｓ１…走行路面、Ｓ２…非走行路面、Ｘ１…車軸、Ｘ２，Ｘ３…線分、ξ…俯角、φ…ずれ角。
【技術分野】
【０００１】
本発明は、レーダによって検知された座標と、ステレオカメラによって検知された座標とを利用して物標を検知する物標検知装置、この物標検知装置を校正するための校正装置およびその校正方法に関する。
【背景技術】
【０００２】
近年、自動車やバイクなどの前方走行車や、歩行者、障害物などの物標との衝突を防止する衝突防止装置や、物標との車間距離を一定の範囲内に保つための車間距離制御装置など、自動車やバイクなどの運転の安全性や快適性を高めるための様々な運転支援装置が開発されている。これら運転支援装置では、物標を検知することが極めて重要であり、運転支援装置にはそのような物標を検知するための物標検知装置が搭載されている。物標検知装置は、例えば、レーザ光またはミリ波電磁波などで物標を検知するレーダや、撮影した画像の解析から物標を検知するカメラ、レーダおよびカメラを含んで構成されたセンサフュージョンなどにより構成されている。
【０００３】
ここで、レーダは、一般に、物標との距離や、相対速度を高精度に測定することができるだけでなく、濃霧などの悪天候によって前方の視界が悪い状況であっても、物標の検知が可能であるという利点を有するが、横方向の分解能が低いので物標の境界の検知が容易でなく、また、検知範囲の上下方向の角度が小さいので視野が狭いなどの欠点を有する。カメラは、一般に、平面分解能が高いので物標の境界の検知が容易であり、視野角も広いという利点を有するが、物標との距離や、相対速度の精度があまり高くなく、天候や照明条件の影響を受けやすいので悪天候や、夜間の場合に物標の検知能力が低下しやすいという欠点を有する。他方、センサフュージョンは、レーダおよびカメラの双方の欠点を双方の利点で補うことが可能であり、最近の開発の主流となっている。例えば、特許文献１，２記載の物標検知装置は、１台のレーダと、１台のカメラとを含んで構成されたセンサフュージョンを備えており、レーダによって検知された物標の座標と、カメラによって検知された物標の座標とを利用して物標を検知するようになっている。
【０００４】
【特許文献１】特開２００３−３１５４４１号公報
【特許文献２】特開２００３−３１５４４２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかし、このような装置では、カメラから得られた座標には、物標との距離についての正確な情報が含まれていないので、レーダから得られた座標と、カメラから得られた座標とを用いて、物標同士を整合（マッチング）させることが容易ではないという問題がある。
【０００６】
本発明はかかる問題点に鑑みてなされたものであり、その目的は、レーダから得られた物標の座標と、カメラから得られた物標の座標とを用いて、物標同士を容易に整合（マッチング）させることの可能な物標検知装置、この物標検知装置を校正するための校正装置およびその校正方法を提供することにある。
【課題を解決するための手段】
【０００７】
本発明の校正装置は、車両前方のレーダ情報を取得するためのレーダと、車両前方のステレオ画像を取得するための右カメラおよび左カメラを有するステレオカメラと、レーダで取得したレーダ情報、ならびに右カメラで取得した２次元画像および左カメラで取得した２次元画像からなるステレオ画像を処理するための制御部とを備えたものである。 ここで、制御部は、以下の（Ａ）〜（Ｅ）の各手段を有している。
（Ａ）レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得するレーダ側３次元座標取得手段
（Ｂ）右カメラによって得られた２次元画像の中から各物標の右カメラ座標系の２次元座標を、左カメラによって得られた２次元画像の中から各物標の左カメラ座標系の２次元座標をそれぞれ抽出するカメラ側２次元座標取得手段
（Ｃ）各物標の右カメラ座標系の２次元座標および各物標の左カメラ座標系の２次元座標から視差情報を取得する視差情報取得手段
（Ｄ）視差情報を用いて、各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するカメラ側３次元座標取得手段
（Ｅ）レーダ情報に基づいて得られた各物標のレーダ座標系の３次元座標と、ステレオ画像に基づいて得られた各物標の右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求める変換係数取得手段
【０００８】
本発明の校正方法は、以下の（Ａ）〜（Ｆ）の各工程を含んでいる。
（Ａ）車両前方のレーダ情報を取得するためのレーダと、車両前方のステレオ画像を取得するための右カメラおよび左カメラを有するステレオカメラと、レーダで取得したレーダ情報、ならびに右カメラで取得した２次元画像および左カメラで取得した２次元画像からなるステレオ画像を処理するための制御部とを用意する工程
（Ｂ）レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得する工程
（Ｃ）右カメラによって得られた２次元画像の中から各物標の右カメラ座標系の２次元座標を、左カメラによって得られた２次元画像の中から各物標の左カメラ座標系の２次元座標をそれぞれ抽出する工程
（Ｄ）各物標の右カメラ座標系の２次元座標および各物標の左カメラ座標系の２次元座標から視差情報を取得する工程
（Ｅ）視差情報を用いて、各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換する工程
（Ｆ）レーダ情報に基づいて得られた各物標のレーダ座標系の３次元座標と、ステレオ画像に基づいて得られた各物標の右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求める工程
【０００９】
本願発明の校正装置および校正方法では、ステレオカメラから得られるステレオ画像から物標との距離に関係する視差情報を得るようにしたので、ステレオカメラから得られた右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換することができる。これにより、レーダから得られた各物標の座標と、視差情報に基づいて変換された右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求めることができる。
【００１０】
本発明の物標検知装置は、上記の校正装置と同様、レーダ、ステレオカメラおよび制御部とを備え、さらに、レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数が格納されている記憶部を備えたものである。ここで、制御部は、以下の（Ａ）〜（Ｄ）の各手段を有している。
（Ａ）レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得するレーダ側３次元座標取得手段
（Ｂ）変換係数を用いて、各物標のレーダ座標系の３次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換すると共に、その変換された各物標の右カメラ座標系の３次元座標および左カメラ座標系の３次元座標を、右カメラ座標系の２次元座標および左カメラ座標系の２次元座標に変換するレーダ側２次元座標取得手段
（Ｃ）右カメラによって得られた２次元画像の中から各物標の右カメラ座標系の２次元座標を、左カメラによって得られた２次元画像の中から各物標の左カメラ座標系の２次元座標をそれぞれ抽出するカメラ側２次元座標取得手段
（Ｄ）レーダ情報に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標と、ステレオ画像に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標とを対比して、車両前方の物標を特定する物標特定手段
【００１１】
本願発明の物標検知装置では、変換係数を用いるようにしたので、レーダによって得られたレーダ情報から、奥行き情報を実質的に含んた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を求めることができる。
【発明の効果】
【００１２】
本発明の校正装置および校正方法によれば、ステレオカメラから得られるステレオ画像から物標との距離に関係する視差情報を得るようにしたので、レーダ座標系の３次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求めることができる。これにより、走行中に障害物の位置を特定する際に、上記のようにして求められた変換係数を用いて、レーダから得られた物標の座標と、ステレオカメラから得られた物標の座標とを対比することができるので、存在可能性の高い物標を特定することができる。従って、レーダから得られた物標の座標と、ステレオカメラから得られた物標の座標とを用いて、物標同士を容易にマッチングさせることができる。
【００１３】
本発明の物標検知装置によれば、変換係数を用いるようにしたので、レーダによって得られたレーダ情報から、奥行き情報を実質的に含んた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を求めることができる。これにより、レーダ情報に基づいて得られた、奥行き情報を実質的に含んた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標と、ステレオ画像に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標とを対比することができるので、。存在可能性の高い物標を特定することができる。従って、レーダから得られた物標の座標と、ステレオカメラから得られた物標の座標とを用いて、物標同士を容易にマッチングさせることができる。
【発明を実施するための最良の形態】
【００１４】
以下、本発明の実施の形態について、図面を参照して詳細に説明する。
【００１５】
図１は、本発明の一実施の形態に係るセンサフュージョン（物標検知装置）の概略構成を表したものである。このセンサフュージョンは、自動車やバイクなどの前方走行車や、歩行者、障害物などの物標を検知するシステムであり、例えば、自動車Ｃに搭載されるものである。
【００１６】
このセンサフュージョンは、レーダ１、ステレオカメラ２、制御部３、記憶部４、表示部５を含んで構成されたものである。レーダ１は、例えば、レーザ光を放射するレーザレーダ、またはミリ波電磁波を放射するミリ波レーダからなり、自動車Ｃのバンパの中央部分などに取り付けられている。このレーダ１は、自動車Ｃの前方にレーザ光やミリ波電磁波を放射すると共に、自動車Ｃの前方に放射したレーザ光の反射光やミリ波電磁波の反射波を検知し、さらに、検知した反射光や反射波から自動車Ｃの前方に存在する物標の方位θ、距離ｄおよび相対速度ｖなどを計測するようになっている。ステレオカメラ２は、右カメラ２１および左カメラ２２を備えている。右カメラ２１および左カメラ２２は、例えば、電荷結合素子（ＣＣＤ）からなり、自動車Ｃのフロントガラスの内壁であって、互いに所定の間隔を隔てると共に、路面から同じ高さのところに取り付けられている。このステレオカメラ２は、自動車Ｃの前方を互いに異なる視点から撮影して、右カメラ２１で撮影された２次元画像と左カメラ２２で撮影された２次元画像とからなるステレオ画像を取得するようになっている。
【００１７】
制御部３は、例えば、ＤＳＰ（Digital Signal Processor）などにより構成され、レーダ１およびステレオカメラ２によって得られた情報を処理して自動車Ｃの前方の障害物の位置や種類などを所定の演算により特定するようになっている。記憶部４は、例えば、ＲＡＭ（Random Access Memory）およびＨＤ（hard disk)からなり、記憶部４には、センサフュージョンを校正するためのプログラムや、障害物を検知するためのプログラム、制御部３で得られた演算結果などが格納されている。表示部５は、例えば、液晶表示装置であり、制御部３で得られた演算結果に基づく情報を表示するようになっている。
【００１８】
次に、センサフュージョンをキャリブレーションする手順について説明する。ただし、これに先立って、このキャリブレーションに用いる反射板６について説明する。
【００１９】
（反射板６）
図２および図３は反射板６の一例を示したものであり、図２はレーダ１がレーザレーダのときの反射板を、図３はレーダ１がミリ波レーダのときの反射板をそれぞれ例示したものである。図４は、センサフュージョンをキャリブレーションする際の、反射板６の配置例を表すものである。
【００２０】
図２の反射板６は、反射ブロック６Ａと、吸収ブロック６Ｂとを交互に配置してなる格子状のパターンを有している。反射ブロック６Ａはレーザ光を強く反射する材質により構成されており、吸収ブロック６Ｂはレーザ光を強く吸収する材質により構成されている。他方、図３の反射板６は、平板６Ｃと、リフレクタ６Ｄとを有している。平板６Ｃは、ミリ波電磁波を透過する材料（布や発砲スチロールなど）からなり、リフレクタ６Ｄを裏面で固定するようになっている。リフレクタ６Ｄは、例えば、三角錐形状の金属板からなり、リフレクタ６Ｄの裏面に格子状に配列されている。このリフレクタ６Ｄは、開口部側から入射したミリ波電磁波を反射するようになっている。
【００２１】
ここで、反射ブロック６Ａや平板６Ｃのうちリフレクタ６Ｄの配置されている部分は、レーザ光またはミリ波電磁波を強く反射するので、反射ブロック６Ａ等の横幅Ｄ１と同じサイズの横幅を有する物標がそこに存在する、とレーダ１に認識され得るものである。他方、吸収ブロック６Ｂや平板６Ｃのうちリフレクタ６Ｄの配置されていない部分は、レーザ光やミリ波電磁波をほとんど反射しないので、吸収ブロック６Ｂ等の横幅Ｄ２と同じサイズの横幅の空間がそこに存在する、とレーダ１に認識され得るものである。
【００２２】
各反射ブロック６Ａおよび各吸収ブロック６Ｂのサイズは、レーダ１の分解能や、レーダ１と反射板６との距離に応じて設定される。反射ブロック６Ａ等の横幅Ｄ１は、レーダ１が分解能と同程度の大きさの物標を認識することは困難であることから、少なくともレーダ１の分解能の２倍程度であることが必要である。吸収ブロック６Ｂ等の横幅Ｄ２は、レーダ１は複数の物標が互いに近接配置されるとこれらを別個の物標として明確に区別することが困難となることから、少なくともレーダ１の分解能の３倍程度であることが必要である。反射ブロック６Ａ等および吸収ブロック６Ｂ等の縦方向のサイズＨは、レーダ１のスキャン平面に垂直な方向の誤差の範囲内に複数の物標が存在するとこれらを別個の物標として明確に区別することが困難となることから、少なくともレーダ１のスキャン平面に垂直な方向の誤差程度であることが必要である。
【００２３】
なお、反射板６の横方向のサイズは、レーダ１によってスキャンされる範囲と同程度の大きさとなっており、反射板６の縦方向のサイズは、反射ブロック６Ａを縦方向に複数配置できる大きさ以上の大きさとなっていれば特に制限されるものではない。また、キャリブレーションの便宜上、最下行の反射ブロック６Ａ等は、自動車Ｃに搭載されたレーダ１と同じ高さのところに配置されているものとする。
【００２４】
（レーダ１自身のキャリブレーション、図４〜図８）
まず、作業者Ｐは反射板６を設置する（ステップＳ１）。具体的には、図４に示したように、反射板６を自動車Ｃの車軸Ｘ１と垂直に交わるようにして、自動車Ｃの前方（自動車Ｃに設置されたレーダ１およびステレオカメラ２の前方）の所定の距離に配置する。続いて、レーダ１がレーザレーダの場合には、反射板６の格子状のパターンのうち地面に最も近い最下行のパターンだけを残して、その他の部分を、レーザ光を強く吸収するカバー（図示せず）で覆い、レーダ１がミリ波レーダの場合には、平板６Ｃに取り付けられたリフレクタ６Ｄのうち地面に最も近い最下行のリフレクタ６Ｄだけを残して、その他のリフレクタ６Ｄを平板６Ｃから取り外す。
【００２５】
次に、作業者Ｐは、制御部３にレーダ１自身のキャリブレーションの実行を要求する（ステップＳ２）。すると、制御部３は、最下行のパターンに含まれる各物標（反射ブロック６Ａ等）の３次元座標と、レーダ１の３次元座標、俯角ξおよびずれ角φとを計測する。
【００２６】
ここで、俯角ξとは、図５（図１の側面図）に示したように、レーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）において、レーダ１のスキャン平面Ａと、（Ｘｍ，Ｚｍ）平面Ｂ（一般には路面と平行な平面）とが交わる角度（微小偏移角）のことである。なお、レーダ１がレーザレーダの場合には、レーザ光がある程度の広がり角を有しているので、その広がり角を二等分する面をスキャン平面Ａとする。ずれ角φとは、図６（図１の上面図）に示したように、レーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の（Ｘｍ，Ｚｍ）平面と平行な平面において直線Ｘ２（後述）と垂直に交わる直線Ｘ３と、自動車Ｃの車軸Ｘ１とが交わる角度のことである。
【００２７】
具体的には、まず、制御部３は、レーダ１に対してスキャンを要求する（ステップＳ３）。レーダ１はスキャンの要求を受信する（ステップＳ４）と、最下行のパターンをスキャンして、スキャン平面内に存在する物標（反射ブロック６Ａ等）の方位θｉを検知すると共に、検知した最下行の物標ごとに、距離ｄｉおよび相対速度ｖｉを計算したのち（ステップＳ５）、最下行の各物標のレーダ情報（θｉ，ｄｉ，ｖｉ）を制御部３に送信する（ステップＳ６）。なお、θや、ｄ、ｖのサフィックスｉは１以上、最下行のパターンに含まれる反射ブロック６Ａの数ｎ以下の整数である。
【００２８】
制御部３はレーダ１から最下行の各物標のレーダ情報（θｉ，ｄｉ，ｖｉ）を受信したのち（ステップＳ７）、レーダ１によって検知されるはずの物標が全て検知されているか否かを判定する（ステップＳ８）。レーダ１によって検知されるはずの物標が全て検知されている場合には、制御部３は、以下の式（１）を用いて、検知された最下行の各物標のレーダ情報（θｉ，ｄｉ，ｖｉ）をレーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）へ変換したのち（ステップＳ９）、最下行の各物標のレーダ情報（θｉ，ｄｉ，ｖｉ）と、最下行の各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）とを記憶部４に格納する（ステップＳ１０）。
【００２９】
【数１】
【００３０】
なお、式（１）はレーダによる計測データをレーダ座標系の３次元座標へ変換する式である。
【００３１】
一方、レーダ１によって検知されるはずの全ての物標が検知されていない場合には、制御部３はその旨の表示を表示部５に要求する（ステップＳ１１）。表示部５は、その要求を受信すると、全ての物標が検知されていない旨を表示する（ステップＳ１２）。作業者Ｐは、その表示を確認し、最下行の全ての物標が検知されるようにレーダ１の向き（俯角ξ、ずれ角φ）を調整し直したのち（ステップＳ１３）、再度、ステップＳ１〜Ｓ８を実行する。すなわち、エイミングを実行する。このとき、最下行の全ての物標が検知されるようにするためには、例えば、最下行の各物標のうち中央に配置された物標に対応する反射光または反射波の強度が最も大きくなるように俯角ξおよびずれ角φを調整する。
【００３２】
その結果、レーダ１によって最下行の全ての物標が検知された場合には制御部３は上記のステップＳ９，Ｓ１０を順次実行し、検知されていなかった場合には、制御部３は再度、ステップＳ１１を実行し、最終的にはステップ１０まで実行する。
【００３３】
次に、制御部３は、レーダ座標系へ変換された各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が、レーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の（Ｘｍ，Ｙｍ）平面上に分布しているか否かを判定する（ステップＳ１４）。各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が（Ｘｍ，Ｙｍ）平面上に分布していない場合には、各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が反射板６上に分布していない、つまり、レーダ１が反射板６に対して正体していないということになるので、レーダ１の傾き具合を正確に求めておくことにより、レーダ１が反射板６に対して正体しているときの、各物標のレーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の３次元座標（Ｘｍｉ′′，Ｙｍｉ′′，Ｚｍｉ′′）（後述）を求めることが可能となる。
【００３４】
判定の結果、（Ｘｍ，Ｙｍ）平面上に分布していない場合には、制御部３は、例えば、各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）（図６の黒丸）を最小二乗法を用いて直線近似を行い、最小二乗法により得られた各物標の３次元座標（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）（図６の白丸）と、それら３次元座標（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）を含む直線Ｘ２と、レーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の（Ｘｍ，Ｚｍ）平面において直線Ｘ２と垂直に交わる直線Ｘ３とを求める。続いて、三角測量法を用いて、レーダ１の座標、俯角ξおよびずれ角φを計算する（ステップＳ１５）。
【００３５】
ここで、レーダ１の座標は、例えば、図６に示したように、（Ｘｍ，Ｚｍ）平面であって、かつ直線Ｘ２上の各物標の３次元座標（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）を含む平面内に、各物標の３次元座標（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）のうち両端の物標の３次元座標（Ｘｍ１′，Ｙｍ１′，Ｚｍ１′），（Ｘｍｎ′，Ｙｍｎ′，Ｚｍｎ′）の位置を中心として、その座標に対応する距離ｄ１，ｄｎを半径とする円弧をそれぞれ描き、双方の円弧の交わる２つの点のうち自動車Ｃ側の点に対応する座標を計算することにより求められる。俯角ξは、上記したように、例えば、最下行の各物標のうち中央に配置された物標に対応する反射光または反射波の強度が最も大きくなるように調整されており、ほとんど０°となるので、例えば、デフォルトで０°としてもよい。ずれ角φは、例えば、レーダ座標系（Ｘｍ，Ｙｍ，Ｚｍ）の（Ｘｍ，Ｚｍ）平面において直線Ｘ２と垂直に交わる直線Ｘ３と、自動車Ｃの車軸Ｘ１とが交わる角度を計算することにより求められる。
【００３６】
次に、制御部３は、俯角ξおよびずれ角φを用いて、各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）または（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）が反射板６上に分布するように、これら３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）または（Ｘｍｉ′，Ｙｍｉ′，Ｚｍｉ′）を補正する（ステップＳ１６）。
【００３７】
一方、レーダ座標系へ変換された各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が、（Ｘｍ，Ｙｍ）平面上に分布している場合には、各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が反射板６上に分布していることを意味するので、レーダ１の俯角ξおよびずれ角φはほとんど０°になっていると推測できる。従って、この場合には、３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）をわざわざ補正する必要はないので、３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）の補正を行わない。もちろん、レーダ１の俯角ξおよびずれ角φを求めるようにしてもよいが、その場合には、上記のステップＳ１５，Ｓ１６を実行することとなる。
【００３８】
なお、以下では、３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）の補正が行われた場合の手順について説明するが、この手順は、補正の行われていない３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）についてももちろん適用可能である。
【００３９】
次に、上記ステップ１６で補正することにより得られた、最下行の各物標の３次元座標（Ｘｍｉ′′，Ｙｍｉ′′，Ｚｍｉ′′）、俯角ξおよびずれ角φを記憶部４に格納する（ステップＳ１７）。
【００４０】
次に、制御部３は、最下行の各物標の３次元座標（Ｘｍｉ′′，Ｙｍｉ′′，Ｚｍｉ′′）を利用して、最下行以外の各物標（反射ブロック６Ａ）の座標を推定する（ステップＳ１８）。例えば、反射ブロック６Ａおよび吸収ブロック６Ｂの高さは共にＨなので、最下行から１行上がるごとに、ＹｍｉにＨを加えることにより、最下行以外の各物標の座標（Ｘｍｉ′′，Ｙｍｉ′′＋Ｈ×（ｓ−１），Ｚｍｉ′′）を求める。ここで、ｓは、最下行から数えたブロックの数である（最下行はｓ＝１）。その後、最下行以外の各物標の座標（Ｘｍｉ′′，Ｙｍｉ′′＋Ｈ×（ｓ−１），Ｚｍｉ′′）を記憶部４に格納する（ステップＳ１９）。
【００４１】
次に、制御部３は、レーダ１自身のキャリブレーションの完了の出力を表示部５に要求する（ステップＳ２０）。表示部５は、その要求を受信すると、レーダ１自身のキャリブレーションが完了した旨を出力する（ステップＳ２１）。このようにして、レーダ１自身のキャリブレーションが完了する。
【００４２】
（ステレオカメラ２自身のキャリブレーション、図９）
次に、作業者Ｐは、反射板６を調整する（ステップＳ２２）。具体的には、レーダ１がレーザレーダの場合には反射板６を覆っているカバーを外して反射板６の格子状パターンを全て露出させ、他方、レーダ１がミリ波レーダの場合には先に取り外したリフレクタ６Ｄを元の位置に取り付ける。続いて、作業者Ｐはステレオカメラ２自身のキャリブレーションを制御部３に要求する（ステップＳ２３）。すると、制御部３はステレオカメラ２に第１回目の撮影を要求する（ステップＳ２４）。ステレオカメラ２は、第１回目の撮影の要求を受信する（ステップＳ２５）と、反射板６のステレオ画像を撮影する（ステップＳ２６）。
【００４３】
次に、制御部３は、作業者Ｐに対して反射板６の車軸Ｘ１に対する角度を所定の角度に変更する指令を表示するように表示部５に要求する（ステップＳ２７）。表示部５は、制御部３からの表示要求を受信すると、その指令を表示する（ステップＳ２８）。作業者Ｐは、その指令に従って反射板６の角度を変更する（ステップＳ２９）。続いて、制御部３はステレオカメラ２に次の撮影を要求する（ステップＳ３０）。ステレオカメラ２は、次の撮影の要求を受信する（ステップＳ３１）と、反射板６のステレオ画像を撮影する（ステップＳ３２）。
【００４４】
引き続き、反射板６の角度を徐々に変えながらステップＳ２７〜Ｓ３３を複数回繰り返したのち、撮影回数が所定の回数に到達する（ステップＳ３３）と、制御部３は撮影した全てのステレオ画像の送信をステレオカメラ２に要求する（ステップＳ３４）。ステレオカメラ２は、その要求を受信する（ステップＳ３５）と、撮影した全てのステレオ画像を制御部３に送信する（ステップＳ３６）。
【００４５】
制御部３は、ステレオ画像を受信する（ステップＳ３７）と、受信したステレオ画像を用いて、ステレオカメラ２の内部パラメータ（例えば、焦点距離Ｆ、光学中心位置および光学歪補正係数）と、外部パラメータ（例えば、右カメラ２１および左カメラ２２の間の並進行列式および回転行列式）とを計算したのち（ステップＳ３８）、内部パラメータおよび外部パラメータと、第１回目に撮影されたステレオ画像とを記憶部４に格納する（ステップＳ３９）。
【００４６】
次に、制御部３は、ステレオカメラ２自身のキャリブレーションの完了の出力を表示部５に要求する（ステップＳ４０）。表示部５は、その要求を受信すると、ステレオカメラ２自身のキャリブレーションが完了した旨を出力する（ステップＳ４１）。このようにして、ステレオカメラ２自身のキャリブレーションが完了する。
【００４７】
（レーダ１およびステレオカメラ２の相互間のキャリブレーション、図１０）
制御部３は、記憶部４に格納された第１回目のステレオ画像を読み出し、そのステレオ画像の中から、各物標の右カメラ画像座系の２次元座標（Ｘｒｊ，Ｙｒｊ）および左カメラ画像座系の２次元座標（Ｘｌｊ，Ｙｌｊ）を抽出する（ステップＳ４２）。具体的には、反射ブロック６Ａの４つの角を抽出し、その４つの角の中央の座標を物標の座標として抽出する。なお、Ｘｒ、Ｙｒ、Ｘｌ、Ｙｌのサフィックスｊは１以上、ステレオカメラ２によって検知された物標の数以下の整数である。
【００４８】
続いて、制御部３は、２次元座標（Ｘｒｊ，Ｙｒｊ），（Ｘｌｊ，Ｙｌｊ）と、以下の式（２）〜（７）とを用いて、各物標の右カメラ座標系の３次元座標（ＸＲｊ，ＹＲｊ，ＺＲｊ）および左カメラ座標系の３次元座標（ＸＬｊ，ＹＬｊ，ＺＬｊ）を求める（ステップＳ４３）。
【００４９】
【数２】
【数３】
【数４】
【数５】
【数６】
【数７】
【００５０】
ここで、ｂはステレオカメラ２の基線長、すなわち、右カメラ２１および左カメラ２２のそれぞれの中心位置間の距離である。また、ｄ（Ｘｒｊ，Ｙｒｊ）は右カメラの２次元座標の視差情報であり、ｄ（Ｘｌｊ，Ｙｌｊ）は左カメラの２次元座標の視差情報である。視差情報ｄ（Ｘｒｊ，Ｙｒｊ）、ｄ（Ｘｌｊ，Ｙｌｊ）は、物標との距離に関係するものであり、右カメラ２１で撮影された２次元画像と、左カメラ２２で撮影された２次元画像とを用いて求めることができる。具体的には、右カメラ２１および左カメラ２２が互いに等しい高さに配置されており、右カメラ２１で撮影された２次元画像中の各物標の２次元座標（Ｘｒｊ，Ｙｒｊ）、および左カメラ２２で撮影された２次元画像中の各物標の２次元座標（Ｘｌｊ，Ｙｌｊ）のうち、同一の物標のＹ座標は互いに一致することから、同一の物標のＸ座標の差分の絶対値｜Ｘｒｊ−Ｘｌｊ｜が視差情報ｄ（Ｘｒｊ，Ｙｒｊ）、ｄ（Ｘｌｊ，Ｙｌｊ）となる。従って、この場合の視差情報ｄ（Ｘｒｊ，Ｙｒｊ）、ｄ（Ｘｌｊ，Ｙｌｊ）は、Ｙ座標とは無関係な値となる。
【００５１】
次に、制御部３は、レーダ１によって計測され、または推定によって求められた各物標の座標（Ｘｍｉ′′，Ｙｍｉ′′＋Ｈ×（ｓ−１），Ｚｍｉ′′）を記憶部４から読み出したのち、レーダ１から得られたレーダ座標系の３次元座標（Ｘｍｉ′′，Ｙｍｉ′′＋Ｈ×（ｓ−１），Ｚｍｉ′′）と、ステレオカメラ２から得られた右カメラ座標系の３次元座標（ＸＲｊ，ＹＲｊ，ＺＲｊ）および左カメラ座標系の３次元座標（ＸＬｊ，ＹＬｊ，ＺＬｊ）とを、以下の式（８），（９）に代入して、各物標に対するＲｒｊ，Ｒｌｊ，Ｔｒｊ，Ｔｌｊを求める（ステップＳ４４）。ここで、Ｒｒｊ，Ｒｌｊはレーダ座標系からカメラ座標系（ＸＲ，ＹＲ，ＺＲ），（ＸＬ，ＹＬ，ＺＬ）への回転行列式であり、Ｔｒｊ，Ｔｌｊはレーダ座標系からカメラ座標系への並進行列式である。
【００５２】
【数８】
【数９】
【００５３】
なお、式（８）はレーダ座標系の３次元座標を右カメラ座標系の３次元座標へ変換する式であり、式（９）はレーダ座標系の３次元座標を右カメラ座標系の３次元座標へ変換する式である。
【００５４】
続いて、制御部３は、各物標に対するＲｒｊ，Ｒｌｊ，Ｔｒｊ，Ｔｌｊの平均値Ｒｒ，Ｒｌ，Ｔｒ，Ｔｌを求めたのち（ステップＳ４５）、これらを記憶部４に格納する（ステップＳ４６）。
【００５５】
このように、本実施の形態では、レーダ１からのレーダ情報を補正するにより得られた各物標の座標（Ｘｍｉ′′，Ｙｍｉ′′＋Ｈ×（ｓ−１），Ｚｍｉ′′）と、ステレオカメラ２から得られた右カメラ座標系の３次元座標（ＸＲｊ，ＹＲｊ，ＺＲｊ）および左カメラ座標系の３次元座標（ＸＬｊ，ＹＬｊ，ＺＬｊ）とを用いて、レーダ座標系の３次元座標と、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを相互に変換するためのＲｒ，Ｒｌ，Ｔｒ，Ｔｌを求めることができる。
【００５６】
次に、制御部３は、センサフュージョンのキャリブレーションの完了の出力を表示部５に要求する（ステップＳ４７）。表示部５は、その要求を受信すると、センサフュージョンのキャリブレーションが完了した旨を出力する（ステップＳ４８）。このようにして、レーダ１およびステレオカメラ２の相互間のキャリブレーションが行われ、センサフュージョンのキャリブレーションが完了する。
【００５７】
このように、本実施の形態では、レーダ１から各物標の座標を得ると共に、ステレオカメラ２から各物標の座標を得ることにより、レーダ１自身や、ステレオカメラ２自身だけでなく、レーダ１およびステレオカメラ２の相互間のキャリブレーションをも一括して行うことができる。つまり、レーダ１およびステレオカメラ２を含んで構成されたセンサフュージョンを同時キャリブレーションすることができる。
【００５８】
次に、自動車Ｃの走行中に、自動車Ｃの前方に存在する障害物の存在、位置および種類を特定する手順について説明する。
【００５９】
（障害物の位置の特定、図１１〜図１４）
まず、制御部３は、レーダ１に対してスキャンの要求を、ステレオカメラ２に対して撮影の要求を、互いに同期して行う（ステップＳ４９）。レーダ１はスキャンの要求を受信する（ステップＳ５０）と、スキャン平面内をスキャンして、そのスキャン平面内に存在する物標の方位θｊを検知すると共に、検知した物標ごとに、距離ｄｊおよび相対速度ｖｊを計算したのち（ステップＳ５１）、計測されたレーダ情報（θｉ，ｄｉ，ｖｉ）を制御部３に送信する（ステップＳ５２）。なお、ここでのθや、ｄ、ｖのサフィックスｉは１以上、レーダ１によって検知された物標の数以下の整数である。他方、ステレオカメラ２は、撮影の要求を受信する（ステップＳ５３）と、自動車Ｃの前方のステレオ画像を撮影する（ステップＳ５４）と共に、そのステレオ画像を制御部３に送信する（ステップＳ５５）。
【００６０】
次に、制御部３は、レーダ１からのレーダ情報（θｉ，ｄｉ，ｖｉ）およびステレオカメラ２からのステレオ画像を受信する（ステップＳ５６）。すると、まず、制御部３は、式（１）および以下の式（１０）〜（１３）を用いて、受信したレーダ情報（θｉ，ｄｉ，ｖｉ）を右カメラ座標系の３次元座標（ＸｍＲｉ，ＹｍＲｉ，ＺｍＲｉ）および左カメラ座標系の３次元座標（ＸｍＬｉ，ＹｍＬｉ，ＺｍＬｉ）に変換すると共に、右カメラ座標系の２次元座標（Ｘｍｒｉ，Ｙｍｒｉ）および左カメラ座標系の２次元座標（Ｘｍｌｉ，Ｙｍｌｉ）に変換する（ステップＳ５７）。続いて、制御部３は、受信したステレオ画像の中から、物標の角を抽出することにより、各物標の右カメラ座標系の２次元座標（Ｘｒｊ，Ｙｒｊ）および左カメラ座標系の２次元座標（Ｘｌｊ，Ｙｌｊ）を求める（ステップＳ５８）。なお、ここでのＸｒ，Ｙｒ，Ｘｌ，Ｙｌのサフィックスｊは１以上、ステレオ画像の中から検知された物標の総数以下の整数である。
【００６１】
【数１０】
【数１１】
【数１２】
【数１３】
【００６２】
なお、式（１０）はレーダ座標系の３次元座標を右カメラ座標系の３次元座標へ変換する式であり、式（１１）はレーダ座標系の３次元座標を左カメラ座標系の３次元座標へ変換する式である。式（１２）は式（１０）で得られた３次元座標を２次元座標へ変換する式であり、式（１３）は式（１１）で得られた３次元座標を２次元座標へ変換する式である。ここで、λは斎次カメラ座標系用係数であり、（Ｘｒ，Ｙｒ）は右カメラ座標系の２次元座標であり、（Ｘｌ，Ｙｌ）は左カメラ座標系の２次元座標であり、（ＸＲ，ＹＲ，ＺＲ）は右カメラ座標系の３次元座標であり、（ＸＬ，ＹＬ，ＺＬ）は左カメラ座標系の３次元座標であり、ｆｘｒ，ｆｙｒは右カメラの焦点距離を画素の横方向または縦方向のサイズで割った値であり、ｆｘｌ，ｆｙｌは左カメラの焦点距離を画素の縦方向または縦方向のサイズで割った値であり、ｕｏ，ｖｏは右カメラおよび左カメラのそれぞれの光学中心位置の画像座標である。なお、ｆｘｌ，ｆｙｌ，ｕｏおよびｖｏは、上記したステレオカメラ２自身のキャリブレーションにおいて得られたものである。
【００６３】
次に、制御部３は、レーダ１によって検知された１または複数の物標が、ステレオカメラ２によって検知された１または複数の物標に含まれているか否かを判定する。すなわちレーダ１によって検知された１または複数の物標と、ステレオカメラ２によって検知された１または複数の物標とのマッチングを行う（ステップＳ５９）。
【００６４】
具体的には、まず、レーダ１から得られた２次元座標（Ｘｍｒｉ，Ｙｍｒｉ），（Ｘｍｌｉ，Ｙｍｌｉ）と、ステレオカメラ２から得られた２次元座標（Ｘｒｊ，Ｙｒｊ），（Ｘｌｊ，Ｙｌｊ）とを対比して、互いに同一または近似する座標が存在するか否かを判定する。その結果、互いに同一または近似する座標が存在する場合には、その座標には物標が存在する可能性が高いことになる。続いて、レーダ１から得られた２次元座標（Ｘｍｒｉ，Ｙｍｒｉ），（Ｘｍｌｉ，Ｙｍｌｉ）に対応する距離ｄｉと、ステレオカメラ２から得られた、レーダ１で検知された物標と同一と思われる物標の２次元座標（Ｘｒｊ，Ｙｒｊ），（Ｘｌｊ，Ｙｌｊ）の、式（７）によって得られる奥行きＺＬｊとを、存在可能性の高い物標について対比する。その結果、距離ｄｉと、奥行きＺＬｊとが一致または近似する（マッチする）場合には、レーダ１が検知した物標と、ステレオカメラ２が検知した物標とが互いに同一の物標であることになり、逆に、マッチしない場合には、レーダ１が検知した物標と、ステレオカメラ２が検知した物標とが互いに異なる物標であることになる。制御部３は、このようにしてマッチングした物標を障害物候補として抽出する（ステップＳ６０）。
【００６５】
次に、制御部３は、ステップＳ５０からステップ５９を繰り返し行ったときに、障害物候補の物標の距離ｄｉの変化の割合と、その奥行きＺＬｊの変化の割合とを対比する（ステップＳ６１）。その結果、距離ｄｉの変化の割合と、奥行きＺＬｊの変化の割合とが互いに全く異なる場合には、その物標は実在しない可能性が極めて高いので、その物標を障害物候補から除外する（ステップＳ６２）。なお、互いにほぼ等しい場合には、その物標が実在する可能性が極めて高いので、その物標を障害物候補として残存させる。
【００６６】
次に、制御部３は、障害物候補の物標が障害物存在可能領域内にあるか否かを判定する。ここで、障害物存在可能領域とは、障害物が存在することの可能な領域のことであり、例えば、図１４に示したような走行路面Ｓ１および路側Ｓ２を含む。走行路面Ｓ１とは、自動車Ｃが走行可能な路面のことであり、路側Ｓ２とは、走行路面Ｓ１に沿って設けられた路面のことである。なお、図１４では、走行路面Ｓ１および路側Ｓ２は、車線区分線Ｌによって区分されている。
【００６７】
判定の結果、障害物候補の物標が、例えば図１４に示したように、走行路面Ｓ１上にある場合には、その物標は、走行路面Ｓ１上に存在しうる障害物のリスト（例えば、自動車、バイク、自転車、落下物）の中の一の障害物であることになり、他方、距離ｄｉの物標が路側Ｓ２上にある場合には、その物標は、路側Ｓ２上に存在しうる障害物のリスト（例えば、ガードレール、道路看板）の中の一の障害物であることになる。従って、障害物候補の物標が障害物存在可能領域の外にある場合には、その物標は自動車Ｃにとって障害物とはなり得ない物標であることになるので、その場合には、その物標を障害物候補から除外する（ステップＳ６３）。このようにして、障害物候補の物標が特定されると共に、その物標の位置が特定される。
【００６８】
（障害物７の種類の特定、図１２〜図１４）
次に、制御部３は、例えば、ステレオカメラ２から得られた２次元座標（Ｘｒｊ，Ｙｒｊ），（Ｘｌｊ，Ｙｌｊ）と、レーダ１から得られた２次元座標（Ｘｍｒｉ，Ｙｍｒｉ），（Ｘｍｌｉ，Ｙｍｌｉ）に対応する距離ｄｉとから、障害物害物候補の物標の形状および大きさを推定する（ステップＳ６４）。続いて、推定した形状および大きさならびに相対速度ｖｉと対応する障害物をリストの中から特定する（ステップＳ６５）。例えば、障害物害物候補の物標が図１４の障害物７であり、障害物７が相対速度ｖｉで走行している場合には、走行路面Ｓ１上に存在しうる障害物のリスト（例えば、自動車、バイク、自転車、落下物）の中からバイクが特定される。このようにして、障害物の種類が特定される。
【００６９】
以上のように、本実施の形態では、ステレオカメラ２から得られるステレオ画像から物標との距離に関係する視差情報を得るようにしたので、ステレオカメラ２から得られた右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換することができる。これにより、レーダ１から得られた各物標の座標と、視差情報に基づいて変換された右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するためのＲｒ，Ｒｌ，Ｔｒ，Ｔｌを求めることができる。その結果、走行中に障害物の位置を特定する際に、上記のようにして求められたＲｒ，Ｒｌ，Ｔｒ，Ｔｌを用いることにより、レーダ１から得られた物標の座標と、ステレオカメラ２から得られた物標の座標とを対比することが可能となる。その結果、存在可能性の高い物標を特定することができる。
【００７０】
従って、本実施の形態では、走行中に障害物の位置を特定する際に、レーダ１によって検知された物標の座標と、ステレオカメラ２によって検知された物標の座標とを用いて、物標同士を容易にマッチングさせることができる。
【００７１】
特に、本実施の形態では、ステレオカメラ２から得られるステレオ画像から、Ｙ座標とは無関係な、物標との距離に関係する視差情報を得るようにしたので、ステレオカメラ２の俯角が自動車Ｃの前方の物標を撮影することが可能な範囲内のどのような値になっていようとも、ステレオカメラ２から得られた右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に常に正確に変換することができる。これにより、レーダ１から得られた各物標の座標と、視差情報に基づいて変換された右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するためのＲｒ，Ｒｌ，Ｔｒ，Ｔｌを常に正確に求めることができる。その結果、走行中に障害物の位置を特定する際に、例えば、自動車Ｃのサスペンションによりステレオカメラ２の俯角が経時的に変動するような場合であっても、上記のようにして求められたＲｒ，Ｒｌ，Ｔｒ，Ｔｌを用いることにより、レーダ１から得られた物標の座標と、ステレオカメラ２から得られた物標の座標とを正確に対比することが可能となる。その結果、存在可能性の高い物標を正確に特定することができる。
【００７２】
また、本実施の形態では、走行中に障害物の位置を特定する際に、Ｒｒ，Ｒｌ，Ｔｒ，Ｔｌを用いているので、レーダ１によって得られた各物標のレーダ座標系の３次元座標を、奥行き情報を実質的に含んた右カメラ座標系の２次元座標および左カメラ座標系の２次元座標に変換することができる。これにより、レーダ１から得られた、奥行き情報を実質的に含んた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標と、ステレオ画像に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標とを対比することができるので、存在可能性の高い物標を特定することができる。従って、レーダから得られた物標の座標と、ステレオカメラから得られた物標の座標とを用いて、物標同士を容易にマッチングさせることができる。
【００７３】
さらに、本実施の形態では、走行中に障害物の位置を特定する際に、示唆情報から物標の奥行きＺＬｊを求めるようにしたので、レーダ１から得られた物標との距離ｄｉと、ステレオカメラ２から得られた、レーダ１で検知された物標と同一と思われる物標の奥行きＺＬｊとを対比することができる。これにより、例えば、レーダ１から得られた物標の２次元座標と、ステレオカメラ２から得られた物標の２次元座標とが互いに近接するものの、距離ｄｉと奥行きＺＬｊとが互いに大きく異なる場合には、これらを互いに別個の物標として認識することができる。一方、例えば、レーダ１から得られた物標の２次元座標と、ステレオカメラ２から得られた物標の２次元座標とが互いに等しいかまたは近接している場合であって、かつ、距離ｄｉと奥行きＺＬｊとが互いに等しいかまたは近接しているときには、これらを互いに同一の物標として認識することができる。
【００７４】
このように、走行中に障害物の位置を特定する際に、レーダ１から得られた物標の２次元座標と、ステレオカメラ２から得られた物標の２次元座標とを互いに対比するだけでなく、距離ｄｉと奥行きＺＬｊとを互いに対比するようにしたので、レーダ１から得られた物標の２次元座標と、ステレオカメラ２から得られた物標の２次元座標とが互いに近接している場合であっても、それらが互いに異なるものなのか、互いに同一のものなのかを正確に特定することができる。
【００７５】
また、本実施の形態では、キャリブレーションにおいて、レーダ１によって検知された各物標の３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）が反射板６上に分布していない場合には、演算により求めたレーダ１の俯角ξおよびずれ角φを用いて、３次元座標（Ｘｍｉ，Ｙｍｉ，Ｚｍｉ）を補正するようにしたので、レーダ１の向きに起因する誤差をＲｒ，Ｒｌ，Ｔｒ，Ｔｌから取り除くことができる。これにより、レーダ１から得られた物標の座標と、ステレオカメラ２から得られた物標の座標とを極めて正確に対比することが可能となるので、走行中に障害物の位置を特定する際に、レーダ１によって検知された物標の座標と、ステレオカメラ２によって検知された物標の座標とを用いて、物標同士を極めて正確にマッチングさせることができる。
【００７６】
また、本実施の形態では、走行中に障害物の位置を特定する際に、レーダ１から得られた物標との距離ｄｉの変化の割合と、ステレオカメラ２から得られた、レーダ１で検知された物標と同一と思われる物標の奥行きＺＬｊの変化の割合とを対比するようにしたので、レーダ１から得られた物標の２次元座標と、ステレオカメラ２から得られた物標の２次元座標とが互いに近接している場合であっても、それらが互いに異なるものなのか、互いに同一のものなのかを極めて正確に特定することができる。これにより、走行中に障害物の位置を特定する際に、レーダ１によって検知された物標の座標と、ステレオカメラ２によって検知された物標の座標とを用いて、物標同士を極めて正確にマッチングさせることができる。
【００７７】
また、本実施の形態では、障害物７の位置および種類を特定したのちに、レーダ１によって検知された障害物７のレーダ情報（θｉ，ｄｉ，ｖｉ）およびステレオカメラ２によって検知された障害物７のステレオ画像のいずれか一方に何らかの不具合（例えばノイズの混入）が発生して、いずれか一方の情報を使うことができなくなった場合であっても、使うことの可能な方の情報だけを用いて、障害物７の位置を追跡することができる。この場合に、双方の情報を用いて障害物７の位置を追跡していたときから、一方の情報だけを用いて障害物７の位置を追跡するように切り換えた瞬間に、障害物７の位置が不連続に変動することはない。障害物７の位置を追跡する際には、キャリブレーションによって得られた定数を含む式（１）、（７）、（１０）〜（１３）を用いているので、レーダ１によって検知された物標の座標と、ステレオカメラ２によって検知された物標の座標とを、一の座標系のほぼ同一の座標に変換することができるからである。
【００７８】
以上、実施の形態および実施例を挙げて本発明を説明したが、本発明は、これらに限定されるものではなく、種々の変形が可能である。
【図面の簡単な説明】
【００７９】
【図１】本発明の一実施の形態に係る物標検知装置の概略構成図である。
【図２】図１の反射板の一構成例を説明するための図である。
【図３】図１の反射板の他の構成例を説明するための図である。
【図４】物標検知装置のキャリブレーションを行う際の配置図である。
【図５】俯角ξを説明するための側面図である。
【図６】ずれ角φを説明するための上面図である。
【図７】物標検知装置のキャリブレーション手順を説明するための流れ図である。
【図８】図７に続く手順を説明するための流れ図である。
【図９】図８に続く手順を説明するための流れ図である。
【図１０】図９に続く手順を説明するための流れ図である。
【図１１】障害物の検知についての手順を説明するための流れ図である。
【図１２】図１１に続く手順を説明するための流れ図である。
【図１３】図１２に続く手順を説明するための流れ図である。
【図１４】障害物の検知についての手順を説明するための斜視図である。
【符号の説明】
【００８０】
１…レーダ、２…ステレオカメラ、２１…右カメラ、２２…左カメラ、３…制御部、４…記憶部、５…表示部、６…反射板、６Ａ…反射ブロック、６Ｂ…吸収ブロック、６Ｃ…平板、６Ｄ…リフレクタ、７…障害物、Ａ…スキャン平面、Ｂ…（Ｘｍ，Ｚｍ）平面、Ｃ…自動車、ｄ１，ｄ２…半径、Ｓ１…走行路面、Ｓ２…非走行路面、Ｘ１…車軸、Ｘ２，Ｘ３…線分、ξ…俯角、φ…ずれ角。
【特許請求の範囲】
【請求項１】
車両前方のレーダ情報を取得するためのレーダと、
車両前方のステレオ画像を取得するための右カメラおよび左カメラを有するステレオカメラと、
前記レーダで取得したレーダ情報、ならびに前記右カメラで取得した２次元画像および前記左カメラで取得した２次元画像からなるステレオ画像を処理するための制御部と、
を備え、
前記制御部は、
前記レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得するレーダ側３次元座標取得手段と、
前記右カメラによって得られた２次元画像の中から前記各物標の右カメラ座標系の２次元座標を、前記左カメラによって得られた２次元画像の中から前記各物標の左カメラ座標系の２次元座標をそれぞれ抽出するカメラ側２次元座標取得手段と、
前記各物標の右カメラ座標系の２次元座標および前記各物標の左カメラ座標系の２次元座標から視差情報を取得する視差情報取得手段と、
前記視差情報を用いて、前記各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するカメラ側３次元座標取得手段と、
前記レーダ情報に基づいて得られた各物標のレーダ座標系の３次元座標と、前記ステレオ画像に基づいて得られた各物標の右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求める変換係数取得手段と
を有する
ことを特徴とする校正装置。
【請求項２】
前記右カメラおよび左カメラは、互いに等しい高さに配置されている
ことを特徴とする請求項１に記載の校正装置。
【請求項３】
前記レーダ側３次元座標取得手段は、前記レーダ情報から取得した各物標のレーダ座標系の３次元座標が一の平面上に分布していない場合には、前記レーダの俯角およびずれ角を用いて、その３次元座標を補正する
ことを特徴とする請求項１に記載の校正装置。
【請求項４】
前記レーダ側３次元座標取得手段は、前記レーダ情報から取得した各物標のレーダ座標系の３次元座標を最小二乗法を用いて直線近似を行い、それにより得られた各物標の３次元座標を含む第１直線と前記一の平面に垂直に交わる平面において垂直に交わる第２直線を求め、前記第２直線と自動車の車軸とが交わる角度を前記レーダのずれ角として求める
ことを特徴とする請求項３に記載の校正装置。
【請求項５】
車両前方のレーダ情報を取得するためのレーダと、
車両前方のステレオ画像を取得するための右カメラおよび左カメラを有するステレオカメラと、
前記レーダで取得したレーダ情報、ならびに前記右カメラで取得した２次元画像および前記左カメラで取得した２次元画像からなるステレオ画像を処理するための制御部と、
レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数が格納されている記憶部と
を備え、
前記制御部は、
前記レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得するレーダ側３次元座標取得手段と、
前記変換係数を用いて、前記各物標のレーダ座標系の３次元座標を、右カメラ座標系の３次元座標および前記左カメラ座標系の３次元座標に変換すると共に、その変換された各物標の右カメラ座標系の３次元座標および前記左カメラ座標系の３次元座標を、右カメラ座標系の２次元座標および左カメラ座標系の２次元座標に変換するレーダ側２次元座標取得手段と、
前記右カメラによって得られた２次元画像の中から前記各物標の右カメラ座標系の２次元座標を、前記左カメラによって得られた２次元画像の中から前記各物標の左カメラ座標系の２次元座標をそれぞれ抽出するカメラ側２次元座標取得手段と、
前記レーダ情報に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標と、前記ステレオ画像に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標とを対比して、車両前方の物標を特定する物標特定手段と
を有する
ことを特徴とする物標検知装置。
【請求項６】
前記右カメラおよび左カメラは、互いに等しい高さに配置されている
ことを特徴とする請求項５に記載の物標検知装置。
【請求項７】
前記記憶部には、前記ステレオカメラの視差情報がさらに格納されており、
前記物標位置特定手段は、前記レーダ情報から得られた各物標との距離と、前記視差情報から得られた各物標の奥行きとを対比して、車両前方の物標を特定する
ことを特徴とする請求項５に記載の物標検知装置。
【請求項８】
前記記憶部には、前記ステレオカメラの視差情報がさらに格納されており、
前記物標位置特定手段は、前記レーダ情報から得られた各物標との距離の変化の割合と、前記視差情報から得られた各物標の奥行きの変化の割合とを対比して、車両前方の物標を特定する
ことを特徴とする請求項５に記載の物標検知装置。
【請求項９】
車両前方のレーダ情報を取得するためのレーダと、車両前方のステレオ画像を取得するための右カメラおよび左カメラを有するステレオカメラと、前記レーダで取得したレーダ情報、ならびに前記右カメラで取得した２次元画像および前記左カメラで取得した２次元画像からなるステレオ画像を処理するための制御部とを用意し、
前記レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得し、
前記右カメラによって得られた２次元画像の中から前記各物標の右カメラ座標系の２次元座標を、前記左カメラによって得られた２次元画像の中から前記各物標の左カメラ座標系の２次元座標をそれぞれ抽出し、
前記各物標の右カメラ座標系の２次元座標および前記各物標の左カメラ座標系の２次元座標から視差情報を取得し、
前記視差情報を用いて、前記各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換し、
前記レーダ情報に基づいて得られた各物標のレーダ座標系の３次元座標と、前記ステレオ画像に基づいて得られた各物標の右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求める
ことを特徴とする校正方法。
【請求項１】
車両前方のレーダ情報を取得するためのレーダと、
車両前方のステレオ画像を取得するための右カメラおよび左カメラを有するステレオカメラと、
前記レーダで取得したレーダ情報、ならびに前記右カメラで取得した２次元画像および前記左カメラで取得した２次元画像からなるステレオ画像を処理するための制御部と、
を備え、
前記制御部は、
前記レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得するレーダ側３次元座標取得手段と、
前記右カメラによって得られた２次元画像の中から前記各物標の右カメラ座標系の２次元座標を、前記左カメラによって得られた２次元画像の中から前記各物標の左カメラ座標系の２次元座標をそれぞれ抽出するカメラ側２次元座標取得手段と、
前記各物標の右カメラ座標系の２次元座標および前記各物標の左カメラ座標系の２次元座標から視差情報を取得する視差情報取得手段と、
前記視差情報を用いて、前記各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するカメラ側３次元座標取得手段と、
前記レーダ情報に基づいて得られた各物標のレーダ座標系の３次元座標と、前記ステレオ画像に基づいて得られた各物標の右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求める変換係数取得手段と
を有する
ことを特徴とする校正装置。
【請求項２】
前記右カメラおよび左カメラは、互いに等しい高さに配置されている
ことを特徴とする請求項１に記載の校正装置。
【請求項３】
前記レーダ側３次元座標取得手段は、前記レーダ情報から取得した各物標のレーダ座標系の３次元座標が一の平面上に分布していない場合には、前記レーダの俯角およびずれ角を用いて、その３次元座標を補正する
ことを特徴とする請求項１に記載の校正装置。
【請求項４】
前記レーダ側３次元座標取得手段は、前記レーダ情報から取得した各物標のレーダ座標系の３次元座標を最小二乗法を用いて直線近似を行い、それにより得られた各物標の３次元座標を含む第１直線と前記一の平面に垂直に交わる平面において垂直に交わる第２直線を求め、前記第２直線と自動車の車軸とが交わる角度を前記レーダのずれ角として求める
ことを特徴とする請求項３に記載の校正装置。
【請求項５】
車両前方のレーダ情報を取得するためのレーダと、
車両前方のステレオ画像を取得するための右カメラおよび左カメラを有するステレオカメラと、
前記レーダで取得したレーダ情報、ならびに前記右カメラで取得した２次元画像および前記左カメラで取得した２次元画像からなるステレオ画像を処理するための制御部と、
レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数が格納されている記憶部と
を備え、
前記制御部は、
前記レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得するレーダ側３次元座標取得手段と、
前記変換係数を用いて、前記各物標のレーダ座標系の３次元座標を、右カメラ座標系の３次元座標および前記左カメラ座標系の３次元座標に変換すると共に、その変換された各物標の右カメラ座標系の３次元座標および前記左カメラ座標系の３次元座標を、右カメラ座標系の２次元座標および左カメラ座標系の２次元座標に変換するレーダ側２次元座標取得手段と、
前記右カメラによって得られた２次元画像の中から前記各物標の右カメラ座標系の２次元座標を、前記左カメラによって得られた２次元画像の中から前記各物標の左カメラ座標系の２次元座標をそれぞれ抽出するカメラ側２次元座標取得手段と、
前記レーダ情報に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標と、前記ステレオ画像に基づいて得られた各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標とを対比して、車両前方の物標を特定する物標特定手段と
を有する
ことを特徴とする物標検知装置。
【請求項６】
前記右カメラおよび左カメラは、互いに等しい高さに配置されている
ことを特徴とする請求項５に記載の物標検知装置。
【請求項７】
前記記憶部には、前記ステレオカメラの視差情報がさらに格納されており、
前記物標位置特定手段は、前記レーダ情報から得られた各物標との距離と、前記視差情報から得られた各物標の奥行きとを対比して、車両前方の物標を特定する
ことを特徴とする請求項５に記載の物標検知装置。
【請求項８】
前記記憶部には、前記ステレオカメラの視差情報がさらに格納されており、
前記物標位置特定手段は、前記レーダ情報から得られた各物標との距離の変化の割合と、前記視差情報から得られた各物標の奥行きの変化の割合とを対比して、車両前方の物標を特定する
ことを特徴とする請求項５に記載の物標検知装置。
【請求項９】
車両前方のレーダ情報を取得するためのレーダと、車両前方のステレオ画像を取得するための右カメラおよび左カメラを有するステレオカメラと、前記レーダで取得したレーダ情報、ならびに前記右カメラで取得した２次元画像および前記左カメラで取得した２次元画像からなるステレオ画像を処理するための制御部とを用意し、
前記レーダによって得られたレーダ情報から各物標のレーダ座標系の３次元座標を取得し、
前記右カメラによって得られた２次元画像の中から前記各物標の右カメラ座標系の２次元座標を、前記左カメラによって得られた２次元画像の中から前記各物標の左カメラ座標系の２次元座標をそれぞれ抽出し、
前記各物標の右カメラ座標系の２次元座標および前記各物標の左カメラ座標系の２次元座標から視差情報を取得し、
前記視差情報を用いて、前記各物標の右カメラ座標系の２次元座標および左カメラ座標系の２次元座標を、右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換し、
前記レーダ情報に基づいて得られた各物標のレーダ座標系の３次元座標と、前記ステレオ画像に基づいて得られた各物標の右カメラ座標系の３次元座標および左カメラ座標系の３次元座標とを用いて、レーダ座標系の３次元座標を右カメラ座標系の３次元座標および左カメラ座標系の３次元座標に変換するための変換係数を求める
ことを特徴とする校正方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２００７−２１８７３８（Ｐ２００７−２１８７３８Ａ）
【公開日】平成１９年８月３０日（２００７．８．３０）
【国際特許分類】
【出願番号】特願２００６−３９５６３（Ｐ２００６−３９５６３）
【出願日】平成１８年２月１６日（２００６．２．１６）
【出願人】（５０４１５９２３５）国立大学法人 熊本大学 (314)
【Ｆターム（参考）】
【公開日】平成１９年８月３０日（２００７．８．３０）
【国際特許分類】
【出願日】平成１８年２月１６日（２００６．２．１６）
【出願人】（５０４１５９２３５）国立大学法人 熊本大学 (314)
【Ｆターム（参考）】
[ Back to top ]