(57)【要約】【課題】測距機能を有する撮像装置と、測距装置との位置姿勢を簡便に推定する。【解決手段】運転支援装置１は、撮像装置２および測距装置３から情報を取得して、自動車の運転を支援するための処理を実行する。位置姿勢推定装置１１は、撮像装置２により得られる画像情報と第１の測距情報から複数の平面領域を検出する撮像データ平面検出部１１１と、測距装置３により得られる第２の測距情報から複数の平面領域を検出する測距データ平面検出部１１２を備える。位置姿勢推定部１１３は、撮像データ平面検出部１１１により検出された第１の平面領域と、測距データ平面検出部１１２により検出された第２の平面領域を用いて位置合わせを行うことで、撮像装置２と測距装置３との相対的な位置および姿勢を推定する。【選択図】  図１
2018155709.tif 000002
【特許請求の範囲】
【請求項１】
測距機能を有する撮像装置と測距装置との相対的な位置または姿勢を推定する位置姿勢推定装置であって、
前記撮像装置により取得される画像情報および第１の測距情報から画像内の第１の平面領域を検出する第１の検出手段と、
前記測距装置により取得される第２の測距情報から前記第１の平面領域に対応する第２の平面領域を検出する第２の検出手段と、
前記第１の平面領域と前記第２の平面領域とのズレ量を算出することにより、前記撮像装置と前記測距装置との位置または姿勢を推定する推定手段と、を備える
ことを特徴とする位置姿勢推定装置。
【請求項２】
前記第１の検出手段は、前記撮像装置により撮像された画像のエッジ成分を検出して前記第１の平面領域の候補領域を抽出し、前記候補領域ごとに前記第１の測距情報を用いて前記第１の平面領域を検出する
ことを特徴とする請求項１に記載の位置姿勢推定装置。
【請求項３】
前記第１の検出手段は、撮像された画像における複数の線分から消失点を検出して複数の前記第１の平面領域の候補領域を抽出する
ことを特徴とする請求項２に記載の位置姿勢推定装置。
【請求項４】
前記第２の検出手段は、前記第１の平面領域の候補領域を用いて、前記第２の測距情報から前記第２の平面領域の候補領域を抽出する
ことを特徴とする請求項２または請求項３に記載の位置姿勢推定装置。
【請求項５】
前記撮像装置の測距点の数は、前記測距装置の測距点の数よりも多い
ことを特徴とする請求項１乃至４のいずれか１項に記載の位置姿勢推定装置。
【請求項６】
前記撮像装置は、複数のマイクロレンズと、各マイクロレンズに対応する複数の光電変換部を有する撮像素子を備え、前記複数の光電変換部の出力から前記第１の測距情報を取得する
ことを特徴とする請求項１乃至５のいずれか１項に記載の位置姿勢推定装置。
【請求項７】
前記撮像装置は、視点の異なる複数の撮像手段を備え、当該複数の撮像手段の出力から前記第１の測距情報を取得する
ことを特徴とする請求項１乃至５のいずれか１項に記載の位置姿勢推定装置。
【請求項８】
前記推定手段は、前記撮像装置もしくは前記測距装置に設定された座標系または前記撮像装置および測距装置を備える移動体に設定された座標系を基準として回転および並進の演算を行い、前記第１の平面領域と前記第２の平面領域とのズレ量を算出する
ことを特徴とする請求項１乃至７のいずれか１項に記載の位置姿勢推定装置。
【請求項９】
前記第１の平面領域と前記第２の平面領域とのズレが検出された場合、前記第２の測距情報を用いて前記第１の測距情報を補正する補正手段を備える
ことを特徴とする請求項１乃至８のいずれか１項に記載の位置姿勢推定装置。
【請求項１０】
前記第１の平面領域と前記第２の平面領域とのズレが検出された場合、前記推定手段は前記撮像装置と前記測距装置との位置または姿勢が変化したことを通知する処理を行う
ことを特徴とする請求項１乃至８のいずれか１項に記載の位置姿勢推定装置。
【請求項１１】
請求項１乃至１０のいずれか１項に記載の位置姿勢推定装置を備える、移動体の運転支援装置であって、
前記撮像装置から取得される画像情報を用いて画像における被検出物の位置を検出し、前記被検出物までの距離を、前記第１および第２の測距情報と前記位置姿勢推定装置により推定された位置または姿勢の情報によって算出する第３の検出手段と、
前記第３の検出手段により検出された前記被検出物と、前記移動体との衝突が発生するか否かを判定する判定手段と、を有する
ことを特徴とする運転支援装置。
【請求項１２】
前記推定手段は、前記移動体が停止しているときに前記位置および姿勢を推定し、前記第１の平面領域と前記第２の平面領域とのズレが検出された場合、前記撮像装置と前記測距装置との位置または姿勢が変化したことを通知する処理を行う
ことを特徴とする請求項１１に記載の運転支援装置。
【請求項１３】
測距機能を有する撮像装置と測距装置との相対的な位置または姿勢を推定する位置姿勢推定装置にて実行される位置姿勢推定方法であって、
前記撮像装置により取得される画像情報および第１の測距情報から画像内の第１の平面領域を検出するとともに、前記測距装置により取得される第２の測距情報から前記第１の平面領域に対応する第２の平面領域を検出する工程と、
前記第１の平面領域と前記第２の平面領域とのズレ量を算出することにより、前記撮像装置と前記測距装置との位置または姿勢を推定する工程と、を有する
ことを特徴とする位置姿勢推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、測距機能を有する撮像装置と測距装置との位置姿勢を推定する技術に関する。
【背景技術】
【０００２】
自動車やロボット等の移動体を自律的に制御する技術では、移動体に搭載された撮像装置および測距装置によって周辺環境を認識する処理が行われる。まず撮像装置から得られる画像情報の分析が行われて、障害物（車、歩行者等）が検出され、測距装置により取得された距離情報から、障害物までの距離が特定される。次に、検出された障害物との衝突可能性について判断処理が実行され、停止あるいは回避等の行動計画が作成される。行動計画にしたがって移動体の制御が行われる。このような技術は、例えば自動車の運転を支援するための機能である運転支援、ＡＤＡＳ（高度運転支援システム）、自動運転等と呼ばれている。
【０００３】
運転支援の制御では、複数の装置によりそれぞれ取得される情報を、矛盾なく統一して認識することが重要である。つまり、撮像装置と測距装置との位置姿勢関係は、自律的に移動する移動体にとって非常に重要である。しかし、一般に、測距装置は計測点数が少ないので測定対象の判別が難しく、撮像装置と測距装置からそれぞれ得られる距離情報の対応付けが非常に難しい。非特許文献１では、特定のチャート像の設置場所を多数変えて（文献中では１００シーン程度）取得し、そのチャート像に対応するそれぞれの領域を手動で対応付けて装置間の位置姿勢を推定する技術が記載されている。また、非特許文献２では、測距機能を有する撮像装置と測距装置を搭載して外界を高精度に認識して、ナビゲーションを行う自律移動ロボットが提案されている。非特許文献２においても装置間の位置姿勢の推定は、非特許文献１に記載の方法を利用している。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】Ｚｈａｎｇ，Ｑ、ｅｔ  ａｌ． ，“Ｅｘｔｒｉｎｓｉｃ  Ｃａｌｉｂｒａｔｉｏｎ  ｏｆ  ａ  Ｃａｍｅｒａ  ａｎｄ  Ｌａｓｅｒ  Ｒａｎｇｅ  Ｆｉｎｄｅｒ”，Ｐｒｏｃｅｅｄｉｎｇ  ｏｆ  ＩＥＥＥ／ＲＳＪ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ｒｏｂｏｔｓ  ａｎｄ  Ｓｙｓｔｅｍｓ，２００３
【非特許文献２】Ｈ．Ｓｏｎｇ，ｅｔ  ａｌ． ，“Ｔａｒｇｅｔ  ｌｏｃａｌｉｚａｔｉｏｎ  ｕｓｉｎｇ  ＲＧＢ−Ｄ  ｃａｍｅｒａ  ａｎｄ  ＬｉＤＡＲ  ｓｅｎｓｏｒ  ｆｕｓｉｏｎ  ｆｏｒ  ｒｅｌａｔｉｖｅ  ｎａｖｉｇａｔｉｏｎ”，Ｐｒｏｃｅｅｄｉｎｇｓ  ｏｆ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ａｕｔｏｍａｔｉｃ  Ｃｏｎｔｒｏｌ  Ｃｏｎｆｅｒｅｎｃｅ（ＣＡＣＳ），２０１４
【発明の概要】
【発明が解決しようとする課題】
【０００５】
従来の技術では、撮像装置における測距機能の搭載の有無に関わらず、撮像装置と測距装置との位置姿勢関係を推定するために、手動による距離情報の対応付けが必要である。このため、非常に煩雑で多くの時間がかかるという課題があった。その結果、装置間の位置姿勢の設定は設置時または調整時にのみ行われる。よって、装置間の位置姿勢が経時的に変化した場合や、車両の衝突等により偶発的に変化した場合に、再調整を行わないと自動運転装置が正規の機能を発揮し得なくなる可能性がある。
本発明の目的は、測距機能を有する撮像装置と、測距装置との位置姿勢を簡便に推定することである。
【課題を解決するための手段】
【０００６】
本発明の一実施形態の位置姿勢推定装置は、測距機能を有する撮像装置と測距装置との相対的な位置または姿勢を推定する位置姿勢推定装置であって、前記撮像装置により取得される画像情報および第１の測距情報から画像内の第１の平面領域を検出する第１の検出手段と、前記測距装置により取得される第２の測距情報から前記第１の平面領域に対応する第２の平面領域を検出する第２の検出手段と、前記第１の平面領域と前記第２の平面領域とのズレ量を算出することにより、前記撮像装置と前記測距装置との位置または姿勢を推定する推定手段と、を備える。
【発明の効果】
【０００７】
本発明の位置姿勢推定装置によれば、測距機能を有する撮像装置と、測距装置との位置姿勢を簡便に推定することができる。
【図面の簡単な説明】
【０００８】
【図１】本実施形態に係る運転支援装置の一例を示す模式図である。
【図２】本実施形態に係る撮像装置の模式図である。
【図３】本実施形態に係る撮像素子の模式図である。
【図４】本実施形態に係る撮像装置の測距方法の説明図である。
【図５】位置ズレ量とデフォーカス量の関係についての説明図である。
【図６】本実施形態に係る測距装置の模式図である。
【図７】本実施形態に係る位置姿勢推定処理のフローチャートである。
【図８】図７（Ａ）のＳ６０４および運転支援制御のフローチャートである。
【図９】本実施形態に係る平面検出方法を説明する模式図である。
【図１０】本実施形態における平面間のズレを説明する模式図である。
【図１１】撮像装置および測距装置の設置状況を示す概略図である。
【発明を実施するための形態】
【０００９】
以下、本発明の実施形態について図面を参照して詳細に説明する。本発明は、自律的に移動可能な自動車やロボット等の移動体の環境認識の技術に関連し、撮像装置および測距装置によって取得される情報を統合的に認識するために利用される。本実施形態では、自動車の運転支援装置への適用例を説明する。尚、図を参照した説明において、原則として同一または同様の部位には、同じ符号を付すことで重複した説明を避ける。
【００１０】
運転支援装置の構成を説明する前に、撮像装置と測距装置との位置姿勢関係について、図１１を用いて詳細に説明する。図１１（Ａ）は、車両における撮像装置および測距装置の設置状況を示す概略図である。撮像装置２は、鮮明な画像を常時取得するために車内に設置される。例えば、撮像装置２はフロントウィンドシールドの上部に取り付けられる。測距装置３は、装置の大きさや測距範囲、測距原理等の関係から、車外に設置される。例えば、測距装置３は周囲全体を測距範囲とした場合を想定して、車両天井部の前端寄りの位置に取り付けられる。あるいは、測距範囲を車両の前部のみに限定する場合には、車両のフロントノーズ部に複数台の測距装置が設置される場合もある。
【００１１】
このように、互いに離れて設置される撮像装置２と測距装置３によってそれぞれ取得される情報を統合するためには、両装置の位置姿勢関係を把握する必要がある。図１１（Ｂ）は、撮像装置２の座標系原点９０１と、測距装置３の座標系原点９１１を示す側面図である。撮像装置２の座標系原点９０１と、測距装置３の座標系原点９１１との位置姿勢関係、即ち、原点間の３次元回転量Ｒと３次元並進量Ｔは、事前に計測しておくことが必要である。
【００１２】
図１１（Ｃ）および（Ｄ）を参照して、自車両と前方の走行車両との距離を推定する様子を説明する。図１１（Ｃ）は撮像装置２によって取得された画像を示す模式図である。撮像された画像内には、障害物として車両が存在する領域９０２が検出される。領域９０２内に位置する前方車両までの距離は、測距装置３からの測距情報を用いて算出される。
【００１３】
図１１（Ｄ）は撮像装置２により得られた画像に、測距装置３の測距データ９１２を投影した状態を示す。菱形の色の違いが距離の違いを表している。領域９０２内の前方車両までの距離は、領域９０２内の測距データ９１２を用いて算出される。一般的には、領域９０２内は概ね検出対象の障害物で占められている。このため、領域９０２内の測距値の最頻値が用いられる。例えば、前方車両に対応する領域９１３内の測距値が、領域９０２の代表距離として決定される。この代表距離と自車速度に基づき、衝突危険性等の判定処理が実行される。図１１（Ｄ）は撮像装置２と測距装置３との位置姿勢関係にズレが発生していない場合を示しており、代表距離は前方車両までの距離を正確に表すものとする。
【００１４】
図１１（Ｅ）は撮像装置２と測距装置３との位置姿勢関係にズレが発生している場合を示す。つまり、測距装置３の測距データ９１２が全体に右方向にずれているため、本来、領域９１３に対応する測距値が領域９０２からずれている状態である。この状態では、領域９０２の代表距離に関し、領域９１３の測距データが用いられず、領域９０２内に入っている測距データが用いられる。この例では、本来の前方車両までの距離よりも遠くの距離が領域９０２の代表距離として算出されることとなる。その結果、代表距離が遠いと判断され、衝突危険性が低いと判断される可能性がある。
【００１５】
撮像装置２と測距装置３との位置姿勢の情報は、運転支援等のように自律的に移動する移動体には非常に重要である。一般に、測距装置３は、図１１（Ｄ）および（Ｅ）に示すように、計測点数が少ない。このため、撮像装置２から取得される画像とは異なり、測定した対象が何かを判別することが難しく、撮像装置２と測距装置３から得られる距離情報の対応付けが難しい。
【００１６】
そこで本実施形態では、測距機能を有する撮像装置と測距装置の位置姿勢とを簡便に推定することが可能な処理を説明する。推定結果に基づいて撮像装置と測距装置との相対的な位置または姿勢のズレをユーザに通知する処理や、ズレ量に応じて測距情報を補正する処理等が実行される。
【００１７】
図１は、本実施形態における、装置間の位置姿勢推定装置を車両の運転支援装置に適用した場合の構成例を模式的に示す。運転支援装置１は、位置姿勢推定装置１１、障害物検出部１２、衝突判定部１３、記憶部１４、車両情報入出力部１５、行動計画作成部１６を備える。
【００１８】
位置姿勢推定装置１１は、運転支援装置１に接続されている撮像装置２と測距装置３との位置姿勢関係を推定する。撮像装置２は測距機能を有し、撮像装置２から被写体までの距離情報を取得可能である。障害物検出部１２は、周辺環境の車両や歩行者、二輪車等の障害物を検出する。障害物検出には、撮像装置２および測距装置３から取得した情報が用いられる。衝突判定部１３は、車両情報入出力部１５より入力される自車速度等の走行状態情報と、障害物検出部１２で検出された情報を取得し、自車と障害物との衝突可能性を判定する。行動計画作成部１６は、衝突判定部１３による判定結果に基づき、停止や回避といった行動計画を作成する。作成された行動計画に基づく車両制御情報は、車両情報入出力部１５から車両制御装置４へ出力される。記憶部１４は、撮像装置２と測距装置３から入力された画像情報や距離情報を一時記憶し、位置姿勢関係や障害物検出部１２で障害物検出のために用いる辞書情報等を記憶している。車両情報入出力部１５は、車両制御装置４との間で、車両速度や角速度等の車両走行情報の入出力処理を行う。
【００１９】
上記装置の具体的な実装形態としては、ソフトウェア（プログラム）による実装形態と、ハードウェアによる実装形態のいずれも可能である。例えば、車両に内蔵されたコンピュータ（マイクロコンピュータ、ＦＰＧＡ（field-programmable gate array）等）のメモリにプログラムが格納されており、当該プログラムをコンピュータが実行する。また、本発明に係る一部または全部の処理を論理回路により実現するＡＳＩＣ等の専用プロセッサーを設けてもよい。
【００２０】
次に図２、図３を参照して、測距機能を有する撮像装置２の構成について説明する。図２（Ａ）は撮像装置２の構成を示す概略図であり、撮像装置２は、結像光学系２１、撮像素子２２を備える。撮像素子２２の出力信号を取得して画像データおよび距離情報を生成する生成部２３や、レンズや絞り等の光学部材を駆動する駆動制御部、画像信号を記録媒体に格納する記録処理部２４等が撮像装置２の内部に配置されている。
【００２１】
結像光学系２１は被写体の像を撮像素子２２の受光面上に形成する。結像光学系２１は複数のレンズ群を備え、撮像素子２２から所定の距離をもって離れた位置に射出瞳２５を有する。なお、図２（Ａ）に示す結像光学系２１の光軸２６は、ｚ軸と平行な軸であり、ｚ軸に対して垂直な２軸をｘ軸およびｙ軸と定義し、ｘ軸とｙ軸は互いに直交する。図２（Ａ）の上下方向の軸をｘ軸とし、図２（Ａ）の紙面に直交する軸をｙ軸とする。
【００２２】
次に撮像素子２２の構成を説明する。撮像素子２２はＣＭＯＳ（相補型金属酸化膜半導体）やＣＣＤ（電荷結合素子）を用いたイメージセンサであり、撮像面位相差検出方式による測距機能を有する。被写体からの光は結像光学系２１を介して撮像素子２２上に結像し、撮像素子２２により光電変換されることで、被写体像に基づく画像信号が生成される。撮像素子２２から取得される画像信号に対して、生成部２３は現像処理を施し、観賞用画像信号を生成する。また、生成後の観賞用画像信号は記録処理部２４により記録媒体に格納される。以下、撮像素子２２について、図２（Ｂ）、および図３（Ａ）〜（Ｄ）を用いてより詳細に説明する。
【００２３】
図２（Ｂ）は、撮像素子２２をｚ軸方向から見た場合の構成を示す模式図である。撮像素子２２は、複数の画素群を２次元アレイ状に配列することで構成される。撮像用画素群２１０は２行２列の画素群であり、対角方向に配置された緑画素２１０Ｇ１および２１０Ｇ２と、赤画素２１０Ｒおよび青画素２１０Ｂから構成されている。撮像用画素群２１０は、青、緑、赤の３つの色情報を含むカラー画像信号を出力する。本実施形態では、青、緑、赤の３原色の色情報についてのみ説明するが、他の波長帯の色情報を用いてもよい。測距用（焦点検出用）画素群２２０は２行２列の画素群であり、対角方向に配置された一対の第１の測距用画素２２１と、別の対角方向に配置された一対の第２の測距用画素２２２から構成される。第１の測距用画素２２１は測距用画像信号である第１の画像信号を出力し、第２の測距用画素２２２は測距用画像信号である第２の画像信号を出力する。
【００２４】
図３（Ａ）は、撮像用画素群２１０の断面構造を示す模式図であり、図２（Ｂ）のＩ−Ｉ*線に沿う青画素２１０Ｂと緑画素２１０Ｇ２の断面を示す。各画素は導光層２１４と受光層２１５から構成される。導光層２１４にはマイクロレンズ２１１とカラーフィルタ２１２が設けられる。マイクロレンズ２１１は画素へ入射した光束を光電変換部２１３へ効率良く導く。カラーフィルタ２１２は所定の波長帯域の光を通過させる。受光層２１５には光電変換部２１３が配置される。この他、画像読み出し用および画素駆動用の配線等が配置されるが、図示を省略する。
【００２５】
図３（Ｂ）は青、緑、赤の３種類のカラーフィルタ２１２の特性を示す。横軸は波長を表し、縦軸は感度を表す。青画素２１０Ｂ、緑画素２１０Ｇ１および２１０Ｇ２、赤画素２１０Ｒの分光感度特性をそれぞれ示す。
【００２６】
図３（Ｃ）は、測距用画素群２２０の断面構造を示す模式図であり、図２（Ｂ）のＪ−Ｊ*線に沿う第１および第２の測距用画素の断面を示す。各画素は導光層２２４と受光層２２５から構成される。導光層２２４は、画素へ入射した光束を光電変換部２１３へ効率良く導くためのマイクロレンズ２１１を有する。遮光部２２３は、光電変換部２１３に入射する光を制限する。受光層２２５には、光電変換部２１３が配置される。この他、画像読み出し用および画素駆動用の配線（不図示）等が配置されている。測距用画素群２２０の場合には、カラーフィルタが配置されていない。これはカラーフィルタによる減光を無くして受光量を増やすためである。
【００２７】
図３（Ｄ）は、第１および第２の測距用画素の分光感度特性を示す。横軸は波長を表し、縦軸は感度を表す。測距用画素の特性は、光電変換部２１３の分光感度と赤外カットフィルタの分光感度を掛け合わせた分光感度特性となる。第１の測距用画素と第２測距用画素の分光感度は、図３（Ｂ）に示す青画素２１０Ｂと緑画素２１０Ｇ１と赤画素２１０Ｒの分光感度を加算したような分光感度である。
【００２８】
次に、撮像面位相差検出方式の距離計測原理を説明する。図４を参照して、撮像素子２２が備える複数の光電変換部が受光する光束について説明する。図４は、結像光学系２１の射出瞳２５と、撮像素子２２中に配置される第１および第２の測距用画素を示す概略図である。図４（Ａ）の上下方向の軸をｘ軸とし、図４の紙面に直交する軸をｙ軸とし、左右方向のｚ軸の方向を光軸方向とする。画素内のマイクロレンズ２１１は、射出瞳２５と受光層２２５が光学的に共役関係になるように配置されている。その結果、図４（Ａ）において射出瞳２５に内包される第１の瞳領域４１０を通過した光束は第１の測距用画素２２１の光電変換部２１３（第１の光電変換部２１３Ａと表記する）に入射する。また図４（Ｂ）において、射出瞳２５に内包される第２の瞳領域４２０を通過した光束は第２の測距用画素２２２の光電変換部２１３（第２の光電変換部２１３Ｂと表記する）に入射する。
【００２９】
各画素に設けられる第１の光電変換部２１３Ａは、受光した光束を光電変換して第１の画像信号を生成する。また、各画素に設けられる第２の光電変換部２１３Ｂは、受光した光束を光電変換して第２の画像信号を生成する。第１の画像信号からは、第１の瞳領域４１０を主に通過した光束が撮像素子２２上に形成する像の強度分布を得ることができる。第２の画像信号からは、第２の瞳領域４２０を主に通過した光束が撮像素子２２上に形成する像の強度分布を得ることができる。第１の画像信号と第２の画像信号との間の相対的な位置ズレ量は、デフォーカス量に対応した量となる。位置ズレ量とデフォーカス量の関係について、図５（Ａ）から（Ｃ）を用いて説明する。これらの図は、撮像素子２２、結像光学系２１の位置関係と結像状態を示す概略図である。図中の光束４１１は第１の瞳領域４１０を通過する第１の光束を示し、光束４２１は第２の瞳領域４２０を通過する第２の光束を示す。図５（Ａ）は合焦状態を示し、図５（Ｂ）および（Ｃ）はデフォーカス状態を示す。
【００３０】
図５（Ａ）に示す合焦状態では、第１の光束４１１と第２の光束４２１が撮像素子２２の受光面上で収束している。この時、第１の光束４１１により形成される第１の画像信号と第２の光束４２１により形成される第２の画像信号との間の相対的な位置ズレ量はゼロとなる。これに対し、図５（Ｂ）は像側でｚ軸の負方向にデフォーカスした前ピン状態を示している。この時、第１の光束により形成される第１の画像信号と第２の光束により形成される第２の画像信号との間の相対的な位置ズレ量はゼロとはならず、負値である。図５（Ｃ）は像側でｚ軸の正方向にデフォーカスした後ピン状態を示している。この時、第１の光束により形成される第１の画像信号と第２の光束により形成される第２の画像信号との間の相対的な位置ズレ量はゼロとはならず、正値である。
【００３１】
図５（Ｂ）と図５（Ｃ）との比較から分かるように、デフォーカス量の符号（正負）に応じて、位置ズレの方向が入れ替わる。また、幾何光学的な関係から、デフォーカス量に応じた位置ズレが生じることが分かる。従って、第１の画像信号と第２の画像信号との位置ズレ量を、後述する領域ベースのマッチング法により検出し、検出された位置ズレ量に対し、所定の変換係数を介してデフォーカス量に変換することができる。なお、像側のデフォーカス量から、物体側の被写体距離への変換については、結像光学系２１の結像関係式を用いることで容易に変換できる。また、位置ズレ量をデフォーカス量に変換するための変換係数は、撮像素子２２が備える画素の受光感度の入射角度依存、射出瞳２５の形状、射出瞳２５と撮像素子２２との距離により決定される。
【００３２】
次に図６を参照して、測距装置３の構成例を説明する。測距装置３は、投光系および受光系の各構成部を備える。投光系は、投射光学系３１、レーザ３２、投射制御部３３から構成される。受光系は受光光学系３４、検出器３５、測距演算部３６、出力部３７から構成される。
【００３３】
レーザ３２は、パルス状のレーザ光を出射する半導体レーザダイオードである。レーザ３２からの光は、スキャン系を有する投射光学系３１により集光および照射される。本実施形態では半導体レーザを用いるが、特に限定は無い。指向性、収束性の良好なレーザ光が得られるのであれば各種レーザを使用できるが、安全性を考慮すると、赤外線波長帯のレーザ光の使用が好ましい。投射制御部３３はレーザ３２によるレーザ光の出射を制御する。投射制御部３３は、レーザ３２を発光させる信号、例えばパルス状駆動信号を生成してレーザ３２および測距演算部３６に出力する。投射光学系３１を構成するスキャン光学系は、レーザ３２から出射されたレーザ光を、水平方向に所定の周期で走査する。スキャン光学系は、ポリゴンミラーやガルバノミラー等を用いた構成である。自動車の運転支援を目的とする場合には、垂直方向に複数のポリゴンミラーを積層し、上下方向に並んで配置された複数のレーザ光を水平に走査する構造のレーザスキャナが使用される。
【００３４】
レーザ光を照射された物体（被検出物）はレーザ光を反射する。反射されたレーザ光は受光光学系３４を介して検出器３５に入射する。検出器３５は受光素子であるフォトダイオードを備え、反射光の強度に対応する電圧値の電気信号を出力する。検出器３５の出力信号は測距演算部３６に入力される。測距演算部３６は、投射制御部３３からレーザ３２の駆動信号が出力された時点から、検出器３５により検出される受光信号が発生されるまでの時間を測定する。この時間はレーザ光が出射した時刻と、反射光が受光された時刻との時間差であり、測距装置３から被検出物までの距離の２倍に相当する。測距演算部３６は時間差を、被検出物までの距離に換算する演算を行い、照射した電磁波が反射された物体までの距離を取得する。
【００３５】
次に図１の位置姿勢推定装置１１の構成を説明する。撮像データ平面検出部１１１は、撮像装置２により取得された画像データおよび測距情報から、平面領域の候補となる領域（以下、平面候補領域という）を抽出する。平面候補領域から平面領域が検出される。測距データ平面検出部１１２は、撮像データ平面検出部１１１が検出した平面候補領域に基づいて、測距装置３により取得された測距情報から平面候補領域を抽出し、平面領域を検出する。位置姿勢推定部１１３は、撮像データ平面検出部１１１、測距データ平面検出部１１２によるそれぞれの検出結果に基づいて、撮像装置２と測距装置３との位置姿勢を推定する。
【００３６】
図７および図８のフローチャートを用いて、撮像装置２と測距装置３の位置姿勢推定の処理の流れを説明する。撮像装置２と測距装置３は、概略同じ方向となるように車両に設置されており、両装置間の距離は手動で測定されて距離情報が記憶部１４に事前に記録されているものとする。また、各装置から出力される画像データおよび測距値に関する装置固有の補正は各装置内で実施される。例えば画像データは撮像装置２にて歪曲等が補正される。測距データは測距装置３にて測距値の直線性や、空間的な均一性等が補正される。本処理は、例えばユーザが運転支援装置１に対して指示部により、撮像装置２と測距装置３の位置姿勢関係を調整する指示操作を行った場合に開始する。
【００３７】
まず、図７（Ａ）のステップＳ６００において撮像データ平面検出部１１１は、撮像装置２から取得された画像データおよび測距データを用いて平面候補領域および平面領域を検出する。これらの検出において、撮像装置２からの画像データおよび測距データは、測距装置３に比して面内分解能が高いため、撮像装置２は平面等の構造物の検出に好適である。Ｓ６００の処理の詳細については、図７（Ｂ）および図９を用いて後で説明する。
【００３８】
次のステップＳ６０１で位置姿勢推定装置１１は、撮像装置２からの画像データおよび測距データに基づいて、Ｓ６００で検出された平面領域の数を、所定の閾値と比較する。所定の閾値に関しては、大きすぎる場合、処理時間が長くなり、また小さすぎると後段の装置間の位置姿勢推定精度が低下する可能性があるため、２〜１０程度の範囲内で設定される。平面領域の数が閾値以下である場合、その旨を不図示の表示部の画面に表示する処理が行われた後、本処理を終了する。Ｓ６００で検出された平面領域の数が閾値より大きい場合、Ｓ６０２に処理を進める。
【００３９】
ステップＳ６０２にて測距データ平面検出部１１２は、測距装置３により得られた測距情報を用いて平面検出を行い、平面候補領域を抽出し、平面領域を検出する。その際には、ステップＳ６００で検出された平面候補領域の情報および記憶部１４に記憶されている装置設置時の初期位置姿勢情報を利用することにより、安定した処理を行うことができる。処理の詳細については図７（Ｃ）を用いて後述する。
【００４０】
ステップＳ６０３で位置姿勢推定装置１１は、ステップＳ６０２で検出された平面領域数を閾値と比較し、閾値より大きいか否かを判定する。閾値は、例えば２〜１０程度の範囲内で設定される。Ｓ６０２で検出された平面領域の数が閾値以下である場合、その旨を不図示の表示部の画面に表示する処理が行われた後、本処理を終了する。Ｓ６０２で検出された平面領域の数が閾値より大きい場合、Ｓ６０４に処理を進める。
【００４１】
ステップＳ６０４において位置姿勢推定部１１３は、撮像装置２と測距装置３との位置姿勢を、ステップＳ６００およびステップＳ６０２でそれぞれ検出された平面候補領域および平面領域の対応関係に基づいて推定し、処理を終える。処理の詳細については図８（Ａ）を用いて後述する。
【００４２】
図７（Ｂ）および図９を参照して、図７（Ａ）のステップＳ６００の処理を説明する。図７（Ｂ）は処理例を示すフローチャートであり、図９は画像例を示す模式図である。
【００４３】
図７（Ｂ）のステップＳ６１０では、撮像装置２により取得された画像データに基づいて線分および消失点が検出される。図９（Ａ）および（Ｂ）を用いて具体例を説明する。画像内の直線（正確には線分）を検出し、検出した直線から消失点を検出する具体的な処理では、取得された画像データに対して、ＬＰＦ（ローパスフィルタ）処理、エッジ検出処理、線分検出処理が実行される。検出された複数の線分の交点から消失点が検出される。ＬＰＦ処理では、ガウシアンフィルタ等の平滑化効果のある処理が行われ、線分検出に不要な模様やノイズ成分が抑制される。エッジ検出処理ではＣａｎｎｙオペレータやＳｏｂｅｌフィルタ等を用いて、エッジ成分の検出が行われる。線分検出処理では、Ｈｏｕｇｈ変換を用いて、直線の可能性の高いエッジ成分が抽出される。これらの処理結果を、図９（Ａ）に示す。その後、ある程度の大きさを有する平面候補領域を検出するために、所定の閾値以上の長さの線分が検出される。検出された線分が、略１点に集中している点が消失点として検出される。図９（Ｂ）の例では４本の線分から消失点７００が検出される。
【００４４】
図７（Ｂ）のステップＳ６１１において、平面候補領域が検出される。平面候補領域は検出された線分によって区分けされる領域である。図９（Ｂ）の例では、消失点７００に関して４本の直線が検出されている。４直線でそれぞれ挟まれる領域ａ〜ｄが、４つの平面候補領域として検出される。
【００４５】
次のステップＳ６１２では、ステップＳ６１１で検出された平面候補領域と、撮像装置２から取得される測距値を用いて、平面領域が検出される。図９に示すように、道路等の奥行き方向に延在する平面は、画面に対して横方向に概略同じ距離を有する。一方で、壁等のように高さ方向に延在する平面は、画面に対して縦方向に概略同じ距離を有する。このような特徴を用いることにより平面領域を検出できる。具体的には、図９（Ｃ）および（Ｅ）に示すように画面を複数の部分領域群に分割する処理が行われる。図９（Ｃ）は横方向に延在する部分領域群７１１の例を示し、図９（Ｅ）は縦方向に延在する部分領域群７２１の例を示す。撮像装置２による測距データは画像と同軸で取得されるため、縦方向と横方向の部分領域ごとに、測距値が概略同一であると見なせる領域を特定することができる。図９（Ｄ）は図９（Ｃ）に対応し、領域ｂ内の部分領域群７１２の例を示す。図９（Ｆ）は図９（Ｅ）に対応し、領域ａ内の部分領域群７２２および領域ｃ内の部分領域群７２３の例を示す。この結果とステップＳ６１１で検出された領域ａ〜ｄとを統合し、平面領域が検出される。具体的には、図９（Ｂ）に示す領域ａ、ｂ、ｃとそれぞれに対応する部分領域群７２２、７１２、７２３の各測距値が取得され、各平面領域に属する測距値となる。撮像データ平面検出部１１１は以上の処理により、撮像装置２から得られた画像および測距値を解析し、解析結果に基づいて平面候補領域を特定して抽出し、平面領域を検出する。
【００４６】
図７（Ｃ）および図９を参照して、図７（Ａ）のステップＳ６０２の処理を説明する。図７（Ｃ）は処理例を示すフローチャートである。まず、ステップＳ６２０では、測距装置３により取得された測距データのうち、平面か否かの判定を行う領域に分割する処理が実行される。測距データについて、総当たり的に平面か否かを判断する方法も可能であるが、処理に時間がかかる。そこで処理時間を短縮しつつ、検出精度を向上させるため、ステップＳ６００で検出された平面領域の情報が用いられる。ここで、測距装置３による測距データをＸｒと表記し、測距装置３の原点Ｘｒ０から撮像装置２の原点Ｘｉ０までの回転量、並進量をそれぞれ、Ｒｒｃ、Ｔｒｃと表記する。
【００４７】
測距データＸｒを、撮像装置２の測距データの座標系へ変換する場合の座標変換は、下記式により表現できる。
Ｘｒｃ＝Ｍ・Ｘｒ、ただし、Ｍ＝［Ｒｒｃ，Ｔｒｃ；０，１］
【００４８】
次に、測距装置３の測距データＸｒのすべてに対して、撮像装置２の画像へ投影する処理が行われる。これは、「ｘｒｉ＝Ｋ・Ｘｒｃ」でカメラ行列Ｋにより算出できる。カメラ行列Ｋは、３次元空間を２次元画像座標へ射影する際の主点、焦点距離、歪曲等を表現した４行４列の行列であり、装置固有の値として、事前に計測されているものとする。この概要を、図９（Ｇ）に示す。図９（Ｇ）は、測距装置３による測距データを画像上に投影した様子を示しており、図中の菱形の色の違いが距離の違いを表している。ここで、測距データが、ステップＳ６１２で撮像装置２による情報から検出された平面領域ａ、ｂ、ｃのうち、どの領域に入るかによってグループ分けの処理が行われる。ただし、画像に射影される測距データは、装置間の位置姿勢の初期値を利用して得られるので、各領域は一定の大きさのマージンを有し、各領域の間で重なってもよいものとする。
【００４９】
ステップＳ６２１に移り、ステップＳ６２０で分割された測距データの各グループにおいて、平面領域を検出する処理が実行される。この検出には、最小二乗法、ロバスト推定法、ＲＡＮＳＡＣ（Ｒａｎｄｏｍ  Ｓａｍｐｌｅ  Ｃｏｎｓｅｎｓｕｓ）等の方法が用いられる。本処理において、検出された測距点数が閾値以下である場合、そのグループについては平面領域を検出できなかったと判断されて、別のグループの平面領域にある測距データ検出に移る。この結果を図９（Ｇ）のデータ群ａｒ、ｂｒ、ｃｒに示す。こうして、測距装置３からの測距データを平面候補領域に分類することが可能となる。
【００５０】
本実施例においては、位置姿勢推定装置１１で推定した位置姿勢を用いて、検出した物体までの距離を推定し、衝突可否判定を行う。運転支援を行うために、測距装置３の測距データを撮像装置２で得られる座標系に統合し、位置姿勢情報を求める処理が実行される。座標系については、撮像装置２で得られる座標系に統合する必要は無く、測距装置３の座標系や車両の所定位置を基準とする座標系等、いずれかの座標系に統一しても構わない。図８（Ａ）を参照して説明する。
【００５１】
まず、ステップＳ６３０で位置姿勢推定装置１１は、図７（Ｃ）のステップＳ６２０と同様、記憶部１４に記憶されている位置姿勢情報、即ち、回転量Ｒ０（３行３列の行列）、並進Ｔ０（３行１列ベクトル）を初期値Ｒ，Ｔとする。
次のステップＳ６３１では、式１の通り、測距装置３の測距データを撮像装置２の座標系でのデータへ変換する処理が行われる。
Ｘｒｃ＝Ｍ・Ｘｒ            （式１）
ただし、Ｘｒは測距装置３の測距点座標であり、Ｍは回転行列Ｒと並進ベクトルＴを式２の通りに合成した４行４列の行列である。
Ｍ＝［Ｒ，Ｔ；０，１］      （式２）
【００５２】
次にステップＳ６３２に移り、ステップＳ６００で検出された撮像データの平面領域とステップＳ６０２で検出された測距データの平面領域との位置ズレを評価する。この様子を、図１０も参照して具体的に説明する。図１０（Ａ）は撮像装置２の測距データＸｃと平面Ｐｃを示し、図１０（Ｂ）は、測距データＸｒｃと平面Ｐｃとの位置関係を示す。
【００５３】
まず位置姿勢推定装置１１は、ステップＳ６００で検出された平面領域に属する、撮像装置２の測距データＸｃを用いて、平面Ｐｃ（ａｘｃ＋ｂｙｃ＋ｃｚｃ＋ｄ＝０）を推定する。推定には最小二乗法、ロバスト推定法、ＲＡＮＳＡＣ等の方法を用いる。検出された平面グループごとに推定処理が実行される。次に位置姿勢推定装置１１は平面間の距離を定義する。ステップＳ６０２で検出されて、撮像装置２の座標系へ変換された測距装置３の測距データＸｒｃと、推定された平面方程式への垂線の足との距離をｄとして、その距離ｄを用いて平面間の距離が定義される。点Ｘｒｃ（ｘｒｃ，ｙｒｃ，ｚｒｃ）と平面Ｐｃとの距離は、式３の通りとなる。
δ＝｜ａｘｒｃ＋ｂｘｒｃ＋ｃｚｒｃ＋ｄ｜／√（ａ２＋ｂ２＋ｃ２）    （式３）
【００５４】
これらを、式４によって、検出した平面グループ、各平面グループに属する測距点について総計し、現在の装置間の回転量Ｒ、並進量Ｔのよるズレ量とする。
Ｄ＝ΣｐΣｘ（δ）    （式４）
【００５５】
次にステップＳ６３３に移り、位置姿勢推定装置１１はズレ量Ｄが所定の閾値以下であるか、および処理が所定の回数繰り返されたかを判断する。ズレ量Ｄが所定の閾値以下である場合には、位置姿勢が正しく推定できたと判断されて本処理を終了する。ズレ量Ｄが所定の閾値より大きい場合にはステップＳ６３４に移行する。ただし、ズレ量Ｄが所定の閾値より大きい場合でも処理が所定の回数繰り返された場合には、正しく推定できなかったと判断されて、本処理を終了する。
【００５６】
ステップＳ６３４で位置姿勢推定装置１１は、ズレ量Ｄを低減するため、式５に示すように行列Ｍを行列Ｍ＊、即ち、回転Ｒ、並進Ｔに更新していく。
Ｍ＊＝ａｒｇｍｉｎＭ  ｜｜Ｄ｜｜＝ａｒｇｍｉｎＭ  ｜｜ΣｐΣｘ（δ）｜｜  （式５）
この最小化では、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｔｄｔ法等の、公知の方法を用いて更新が行われる。
【００５７】
以上により、撮像装置２から得られた画像および各測距データの解析に基づき、撮像装置２と測距装置３との位置姿勢関係を算出することが可能となる。なお、上記の説明では、測距装置３の測距データを撮像装置２の座標系でのデータへ変換する例を説明したが、その逆や車両の基準となる座標系等、任意の座標系で実現可能である。また、平面のズレに関しても、各測距データで検出された、それぞれの平面領域で平面方程式を推定し、対応する平面間の法線のズレや、平面が交わる角度等、任意の距離を定義してもよい。また、回転、並進の変化のさせ方に関して、同時に変化させてもよいし、どちらか一方ずつ変化させてもよく、本実施形態のように、概略、各装置の姿勢が同じように設置される場合には、並進量を主に調整する等、特に制限は無い。
【００５８】
図８（Ｂ）のフローチャートを参照して、位置姿勢推定装置１１による推定結果を用いて障害物を検出し、危険な場合に警告を行う運転支援装置１の動作について説明する。本処理は、ステップＳ６４０において、撮像装置２により取得した画像データから、車両や歩行者等の走行路中の障害物を検出するところから開始する。事前に登録された障害物の画像データを用いたパターンマッチングによる検出方法や、ＳＩＦＴやＨＯＧといった画像特徴量を事前に学習済みの教師データにより識別する検出方法がある。ＳＩＦＴは“Scale-Invariant Feature Transform”の略号であり、ＨＯＧは“Histograms of Oriented Gradients”の略号である。ステップＳ６４１にて障害物検出部１２により障害物が検出された場合、ステップＳ６４２に進み、障害物が検出されない場合には本処理を終了する。
【００５９】
ステップＳ６４２では、検出された障害物までの測距データが取得される。具体的には、図１１（Ｃ）の画像内の領域９０２から障害物が検出される。点線で囲まれた領域に対応する撮像装置２による測距データ、および、測距装置３による測距データを、記憶部１４内の位置姿勢情報およびカメラ行列を用いて画像に投影した測距データが選択される。複数の障害物が検出されている場合には、各障害物の検出領域において撮像装置２と測距装置３の各測距データが選択される。
【００６０】
次にステップＳ６４３で障害物検出部１２は、ステップＳ６４２で選択された測距データを用いて、検出した障害物の領域までの代表距離を算出する。具体的には、各測距データの信頼性を表す信頼度と測距値が用いられる。撮像装置２により取得される測距データについては、その測距原理から、エッジ等、テクスチャのある領域の測距値の信頼性が高いのに対し、テクスチャの無い領域では信頼性は低い。一方、測距装置３により取得される測距データについては、テクスチャには左右されず、反射率が高い物体であれば信頼性が高い。また測距装置３の測距点の数は撮像装置２の測距点の数に比して少ない。それぞれの信頼性が高い測距データの統計量として、最頻値等の統計量によって代表測距値を算出する処理が実行される。
【００６１】
ステップＳ６４４で衝突判定部１３は、ステップＳ６４３で算出された代表測距値と、車両情報入出力部１５を介して入力された車速データから、障害物との衝突の危険性を判断する。ステップＳ６４５において、衝突の危険性が高いと判断された場合、ステップＳ６４６に進み、危険性が無いと判断された場合には本処理を終了する。
【００６２】
ステップＳ６４６では行動計画作成部１６は行動計画を作成する。行動計画には、障害物までの距離によって緊急停止を行う制御、運転者へ警告する処理、あるいは周辺状況に応じて回避経路をとる制御等がある。ステップＳ６４７で車両情報入出力部１５は、ステップＳ６４６で作成された行動計画に基づいて決定される、車両の加速度および角速度、即ち、アクセルやブレーキの制御量、ハンドルの操舵角等の情報を車両制御装置４へ出力する。車両制御装置４により走行制御や警告処理等が実行される。
【００６３】
本実施形態では、撮像装置２と測距装置３の位置姿勢関係を用いて、障害物検出等の運転支援機能を高精度に実現できる。位置姿勢推定機能に関しては、信号待ち状態等での長時間の停車中や、偶発的に車両が衝突した場合に、運転者の指示または自動で処理が開始される。例えば、位置姿勢推定装置１１は車両情報入出力部１５から車両の速度情報を取得し、車両の停止状態が所定の閾値時間以上継続した場合に、位置姿勢を推定し、撮像装置２と測距装置３との位置または姿勢が変化したことを運転者に通知する処理を行う。あるいは、前回に推定された撮像装置２と測距装置３の位置姿勢から大きなズレが発生していることを運転者に警告する処理が実行される。例えば検出された平面領域同士のズレが検出された場合、位置姿勢推定装置１１は撮像装置２と測距装置３との位置姿勢が前回の設定からずれていることを表示部の画面に表示し、または音声出力により運転者に通知する処理を行う。装置間の経時変化や偶発的変化等による位置または姿勢のズレが原因で運転支援処理が正しく機能しなくなることを防止できるという効果が得られる。
【００６４】
本実施形態によれば、測距機能を有する撮像装置による測距情報と、測距装置による測距情報を取得し、検出した複数の平面領域に基づいて位置合わせを行うことで、簡便な位置姿勢推定を実現できる。
【００６５】
［変形例］
測距可能な撮像装置として、変形例では、遮光部２２３を有する撮像素子に代えて、複数のマイクロレンズと、各マイクロレンズに対応する複数の光電変換部を有する撮像素子が使用される。例えば、各画素部は、１つのマイクロレンズ、および該マイクロレンズに対応する２つの光電変換部を有する。撮像光学系の異なる瞳部分領域をそれぞれ通過した光を各光電変換部が受光して光電変換を行って電気信号を出力する。位相差検出部が対をなす電気信号の位相差を検出することにより、像ずれ量からデフォーカス量や距離情報を算出することができる。また、複数の撮像装置を用いるシステムによって被写体の距離情報を取得できる。例えば、２台以上のカメラで構成されるステレオカメラによって、異なる視点画像を取得して被写体の距離を算出することができる。画像を取得可能であって、かつ、同時に測距が可能な撮像装置であれば、特に限定は無い。
【００６６】
さらに変形例では、測距機能を有する撮像装置と測距装置との位置姿勢関係を推定して、統一した座標系で比較することで、測距値の補正を行う。一般に、測距装置３は環境に対して安定しているが、撮像装置２の光学系等は温度等の条件によって変化することがある。この場合、変形例に係る位置姿勢推定装置内の測距データ補正部１１４(図１)は、測距装置３の測距値を用いて撮像装置２の測距値を補正する。その際、撮像画面中に占める平面領域が画面中の半分以上を超える場合（例えば、ほとんどが道路や建物である場合）に、測距データ補正部１１４は平面領域に応じた補正係数を算出して補正前の測距値に乗算する。
【００６７】
（その他の実施形態）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【符号の説明】
【００６８】
１      運転支援装置
２      撮像装置
３      測距装置
１１    位置姿勢推定装置
１１１  撮像データ平面検出部
１１２  測距データ平面検出部
１１３  位置姿勢推定部
１１４  測距データ補正部
【図１】
2018155709.tif 000003
【図２】
2018155709.tif 000004
【図３】
2018155709.tif 000005
【図４】
2018155709.tif 000006
【図５】
2018155709.tif 000007
【図６】
2018155709.tif 000008
【図７】
2018155709.tif 000009
【図８】
2018155709.tif 000010
【図９】
2018155709.tif 000011
【図１０】
2018155709.tif 000012
【図１１】
2018155709.tif 000013