JP7179687B2

JP7179687B2 - 障害物検知装置

Info

Publication number: JP7179687B2
Application number: JP2019112244A
Authority: JP
Inventors: 智和若杉
Original assignee: Toshiba Corp; Toshiba Electronic Devices and Storage Corp
Current assignee: Toshiba Corp; Toshiba Electronic Devices and Storage Corp
Priority date: 2019-06-17
Filing date: 2019-06-17
Publication date: 2022-11-29
Anticipated expiration: 2039-06-17
Also published as: JP2020204525A

Description

本実施形態は、障害物検知装置に関する。

近年、例えば、ステレオカメラを車両などの移動体に搭載し、このカメラから一定間隔で出力される画像に基づいて障害物を検出し、当該障害物との接触を回避するように車両の走行に係る自動制御を行って運転の支援をするようなシステム（安全運転システム）が実用化されつつある。

このような安全運転システムを用いて移動体の制御を行う場合、障害物を精度よく検出することが重要となる。

特開２０１３－２３９９０５号公報

本実施形態は、障害物を精度よく検出することができる、障害物検知装置を提供することを目的とする。

本実施形態の障害物検知装置は、移動体に設けられたセンサにより得られる撮影画像に基づき、前記移動体外部に存在する障害物を検知する障害物検知装置であって、前記撮影画像から複数の特徴点を検出しそれぞれの前記特徴点について三次元点の位置である三次元座標を推定する三次元点検出部と、検出された複数の前記三次元点からなる三次元点群を蓄積する三次元点群蓄積部と、前記三次元点群から前記三次元座標が誤推定されている前記三次元点をノイズ点として検出し除去するノイズ点除去部と、を有する。
前記ノイズ点除去部は、一の前記三次元点をノイズ候補点として抽出し、前記三次元点群を前記撮影画像に投影した際に、前記撮影画像において前記ノイズ候補点の投影位置を中心とする所定の領域内に投影された一つ以上の前記三次元点である判定対象三次元点を抽出し、前記ノイズ候補点の前記センサからの距離と、前記判定対象三次元点の前記センサからの平均距離とを比較し、前記ノイズ候補点がノイズ点であるか否かを判定する。

本実施形態に係わる障害物検知装置を搭載する移動体１００の一例を示す図。本実施形態に係わる障害物検知装置を含む、移動体１００の構成の一例を示すブロック図。移動体１００に搭載された画像処理装置１０の一例を示すブロック図。画像処理装置１０における、障害物検知装置に係わる構成の一例を示す概略ブロック図。画像テンプレートマッチングと三角測量を用いた三次元座標計算の説明図。画像テンプレートマッチングと三角測量を用いた三次元座標計算の説明図。ノイズ点発生のメカニズムの一例を説明する図。ノイズ点発生のメカニズムの一例を説明する図。第１の実施形態に係わる三次元点群からノイズ点を除去する手順を説明するフローチャート。ノイズ点除去方法の一例を説明する図。ノイズ点除去方法の一例を説明する図。第２の実施形態に係わる三次元点群からノイズ点を除去する手順を説明するフローチャート。積分画像による評価領域内の値の総和計算手順を説明する図。

以下、図面を参照して実施形態を説明する。
（第１の実施形態）
図１は、本実施形態に係わる障害物検知装置を搭載する移動体１００の一例を示す図である。

移動体１００は、画像処理装置１０と、出力部１００Ａと、センサ１００Ｂと、動力制御部１００Ｇと、動力部１００Ｈと、を備える。

移動体１００は、移動可能な物体である。移動体１００は、例えば、車両（自動二輪車、自動四輪車、自転車）、台車、ロボット、船舶、飛翔体（飛行機、無人航空機（UAV：Unmanned Aerial Vehicle）など）である。移動体１００は、例えば、人による運転操作を介して走行する移動体や、人による運転操作を介さずに自動的に走行（自律走行）可能な移動体である。自律走行可能な移動体は、例えば、自動運転車両である。本実施形態の移動体１００は、自律走行可能であり、かつ、人による運転操作も可能な車両を一例として説明する。

出力部１００Ａは、各種情報を出力する。例えば出力部１００Ａは、各種処理による出力情報を出力する。

出力部１００Ａは、例えば、出力情報を送信する通信機能、出力情報を表示する表示機能、出力情報を示す音を出力する音出力機能、などを備える。例えば、出力部１００Ａは、通信部１００Ｄと、ディスプレイ１００Ｅと、スピーカ１００Ｆと、を含む。

通信部１００Ｄは、外部装置と通信する。通信部１００Ｄは、VICS（登録商標）通信回路やダイナミックマップ通信回路である。通信部１００Ｄは、出力情報を外部装置へ送信する。また、通信部１００Ｄは、道路情報などを外部装置から受信する。道路情報は、信号、標識、周囲の建物、各車線の道幅、レーン中心線などである。道路情報は、画像処理装置１０内に設けたRAM、ROMといったメモリ１０ｂ内に記憶されても、移動体内に別に設けたメモリ内に記憶してもよい。なお、メモリ１０ｂは、後述する図３における画像処理装置１０の説明において、具体的な構成を詳述する。

ディスプレイ１００Ｅは、出力情報を表示する。ディスプレイ１００Ｅは、例えば、公知のLCD（Liquid Crystal Display）や投影装置やライトなどである。スピーカ１００Ｆは、出力情報を示す音を出力する。

センサ１００Ｂは、移動体１００の走行環境を取得するセンサである。走行環境は、例えば、移動体１００の観測情報や、移動体１００の周辺情報である。センサ１００Ｂは、例えば、外界センサや内界センサである。

内界センサは、観測情報を観測するセンサである。観測情報は、例えば、移動体１００の加速度、移動体１００の速度、移動体１００の角速度（ヨー軸角速度）を含む。

内界センサは、例えば、慣性計測装置（IMU：Inertial Measurement Unit）、加速度センサ、速度センサ、ロータリエンコーダ、ヨーレートセンサ、などである。IMUは、移動体１００の三軸加速度および三軸角速度を含む観測情報を観測する。

外界センサは、移動体１００の周辺情報を観測する。外界センサは、移動体１００に搭載されていてもよいし、該移動体１００の外部（例えば、他の移動体や外部装置など）に搭載されていてもよい。

周辺情報は、移動体１００の周辺の状況を示す情報である。移動体１００の周辺とは、該移動体１００から予め定めた範囲内の領域である。この範囲は、外界センサの観測可能な範囲である。この範囲は、予め設定すればよい。

周辺情報は、例えば、移動体１００の周辺の撮影画像および距離情報などである。なお、周辺情報は、移動体１００の位置情報を含んでいてもよい。撮影画像は、撮影によって得られる撮影画像データである（以下、単に、撮影画像と称する場合がある）。距離情報は、移動体１００から対象までの距離を示す情報である。対象は、外界における、外界センサによって観測可能な箇所である。位置情報は、相対位置であってもよいし、絶対位置であってもよい。

外界センサは、例えば、撮影によって撮影画像を得る撮影装置（カメラ）、距離センサ（ミリ波レーダ、レーザセンサ、距離画像センサ）、位置センサ（GNSS（Global Navigation Satellite System）、GPS（Global Positioning System）、無線通信装置）などである。

撮影画像は、画素ごとに画素値を規定したデジタル画像データや、画素毎にセンサ１００Ｂからの距離を規定したデプスマップ（距離画像）などである。レーザセンサは、例えば、水平面に対して平行に設置された二次元LIDAR（Laser Imaging Detection and Ranging）センサや、三次元LIDARセンサである。

動力制御部１００Ｇは、動力部１００Ｈを制御する。動力部１００Ｈは、移動体１００に搭載された、駆動するデバイスである。動力部１００Ｈは、例えば、エンジン、モータ、車輪、などである。

動力部１００Ｈは、動力制御部１００Ｇの制御によって駆動する。本実施形態の移動体１００は、自律走行可能であるので、動力制御部１００Ｇは、画像処理装置１０で生成された出力情報や、センサ１００Ｂから得られた情報などに基づいて、周辺の状況を判断し、アクセル量、ブレーキ量、操舵角などの制御を行う。すなわち、移動体１００の前方に検出された障害物が移動体１００に衝突する可能性がある場合には、動力部１００Ｈを制御して、移動体１００と障害物との接触を回避させる。

次に、移動体１００の電気的構成について詳細に説明する。図２は、本実施形態に係わる障害物検知装置を含む、移動体１００の構成の一例を示すブロック図である。

移動体１００は、画像処理装置１０と、出力部１００Ａと、センサ１００Ｂと、動力制御部１００Ｇと、動力部１００Ｈと、を備える。出力部１００Ａは、上述したように、通信部１００Ｄと、ディスプレイ１００Ｅと、スピーカ１００Ｆと、を含む。本実施形態の障害物検知装置１は、画像処理装置１０を含んで構成される。

画像処理装置１０、出力部１００Ａ、センサ１００Ｂ、および動力制御部１００Ｇは、バス１００Ｉを介して接続されている。動力部１００Ｈは、動力制御部１００Ｇに接続されている。

なお、出力部１００Ａ（通信部１００Ｄ、ディスプレイ１００Ｅ、スピーカ１００Ｆ）、センサ１００Ｂ、および動力制御部１００Ｇ、の少なくとも１つは、有線または無線で画像処理装置１０に接続すればよい。また、出力部１００Ａ（通信部１００Ｄ、ディスプレイ１００Ｅ、スピーカ１００Ｆ）、センサ１００Ｂ、および動力制御部１００Ｇの少なくとも１つと、画像処理装置１０とを、ネットワークを介して接続してもよい。

図３は、移動体１００に搭載された画像処理装置１０の一例を示すブロック図である。画像処理装置１０は、Ｉ／Ｆ１０ｃ、メモリ１０ｂ、プロセッサ１０ａを含む。

Ｉ／Ｆ１０ｃは、他のシステムとのネットワーク（N／W）等に接続されている。また、Ｉ／Ｆ１０ｃは、通信部１００Ｄとの間の情報の送受信を司る。Ｉ／Ｆ１０ｃを介して、人などの認識された対象の情報、認識された対象までの距離の情報が出力される。

メモリ１０ｂは、各種データを記憶する。メモリ１０ｂは、例えば、RAM（Random Access Memory）、フラッシュメモリ等の半導体メモリ素子、ハードディスク、光ディスク等である。なお、メモリ１０ｂは、画像処理装置１０の外部に設けてもよい。ROMは、プロセッサ１０ａによって実行されるプログラムや必要なデータを保持する。RAMは、プロセッサ１０ａの作業領域として機能する。また、メモリ１０ｂは、移動体１００の外部に設けてもよい。例えば、メモリ１０ｂを、クラウド上に設置されたサーバ装置に配置してもよい。

また、メモリ１０ｂは、記憶媒体であってもよい。具体的には、記憶媒体は、プログラムや各種情報を、LAN（Local Area Network）やインターネットなどを介してダウンロードして記憶または一時記憶したものであってもよい。また、メモリ１０ｂを、複数の記憶媒体から構成してもよい。

プロセッサ１０ａにおける各処理機能は、コンピュータによって実行可能なプログラムの形態でメモリ１０ｂへ記憶されている。プロセッサ１０ａは、プログラムをメモリ１０ｂから読出、実行することで、各プログラムに対応する機能部を実現するプロセッサである。

なお、各機能の各々を実現するための独立した複数のプロセッサを組み合わせて処理回路１０ｅを構成してもよい。この場合、各プロセッサがプログラムを実行することにより各機能を実現する。また、各処理機能がプログラムとして構成され、１つの処理回路１０ｅが各プログラムを実行してもよいし、専用回路として画像処理アクセラレータ１０ｄを設け、特定の機能が独立したプログラム実行回路に実装してもよい。

プロセッサ１０ａは、メモリ１０ｂに保存されたプログラムを読み出し実行することで機能を実現する。なお、メモリ１０ｂにプログラムを保存する代わりに、プロセッサをＦＰＧＡなどで構成し、回路内にプログラムを直接組み込むよう構成しても構わない。この場合、プロセッサは回路内に組み込まれたプログラムを読み出し実行することで機能を実現する。

図４は、画像処理装置１０の構成の一例を示す概略ブロック図である。図４には、障害物検知装置に係わる構成のみを図示している。画像処理装置１０は、画像処理部１１と、障害物特定部１２とから構成されている。

画像処理部１１は、センサ１００Ｂに含まれているカメラに関し、３次元空間での姿勢（以下、カメラ姿勢と示す）を推定する。また、カメラ前方の障害物を３次元点群として抽出する。画像処理部１１は、画像処理制御部１１１と、カメラ姿勢推定部１１２と、三次元点群検出部１１３と、三次元点群蓄積部１１４と、ノイズ点除去部１１５とを有して構成される。

障害物特定部１２は、画像処理部１１によって推定されたカメラ姿勢と、抽出された三次元点群に基づき、カメラ前方にある障害物を特定する。

画像処理制御部１１１は、カメラ姿勢を推定して三次元点群を抽出するよう、画像処理部１１の他の構成要素の動作を制御する。

カメラ姿勢推定部１１２は、センサ１００Ｂに含まれるカメラから、時系列で撮影画像を取得する。時系列の撮影画像は一定時間間隔で入力される。以下の説明において、撮影画像を入力した時刻を、入力した順に０、１、２、…、ｔ－１、ｔ、ｔ＋１、…のように番号で表す。また、以下の説明において、カメラから取得した撮影画像を入力画像と示し、時刻ｔの入力画像をフレームｔと呼ぶ。カメラ姿勢推定部１１２は、取得した時系列の撮影画像を解析し、SfM(Structure from Motion)などの手法を用いて、カメラ姿勢を推定する。

三次元点検出部としての三次元点群検出部１１３は、入力画像から特徴点を検出し、当該特徴点が実際に存在する位置の三次元座標を算出する。以下、特徴点が実際に存在すると推定される点を三次元点と示し、三次元点の位置を三次元座標と示す。三次元点群検出部１１３における、特徴点から三次元座標を算出する方法について説明する。まず、三次元点群検出部１１３は、例えば、sobelフィルタなどを用いて、入力画像（フレームｔ）中に存在する物体等の境界（エッジ）を検出し、特徴点とする。なお、特徴点の検出は、GFTT(Good Features To Track)など、他の手法やアルゴリズムを用いて行ってもよい。

次に、三次元点群検出部１１３は、現在のフレーム（フレームｔ）において検出した特徴点と、過去のフレーム（例えば、フレームｔ－１）において検出した特徴点との対応付けを行い、対応点ペアを特定する。すなわち、対応点ペアとは、過去フレームと現在フレームの双方に存在する、同一構造物の同一部分について検出された特徴点のペアである。対応点ペアは、例えば画像テンプレートマッチングにより特定することができる。

具体的には、現在のフレームと過去フレームにおいて検出された特徴点について、当該特徴点を中心とする小領域を設定する。そして、現在のフレームに設定された小領域と、過去フレームにおいて設定された小領域との間の類似度を算出する。類似度が設定された閾値以上である場合、現在のフレームの小領域内の特徴点と、過去フレームの小領域内の特徴点とを対応点ペアとする。類似度の計算は、ZNCC(Zero-mean Normalization Cross-Correction、ゼロ平均正規化相関)やNCC(Normalization Cross-Correction、正規化相関)、SAD(Sum of Squared Difference)、SSD(Sum of Absolute Difference)など、一般的に知られた類似度計算を用いて行う。なお、対応点ペアの特定は、上述の画像テンプレートマッチングに限定されない。例えば、ORB特徴量(Oriented FAST and Rotated BRIEF)など、特徴点マッチングの手法を用いて対応点ペアを特定してもよい。

最後に、特定した各対応点ペアについて、（１）過去フレームと現在フレームのカメラ姿勢、（２）過去フレームと現在フレームにおける特徴点の入力画像上での座標、の各情報に基づき、特徴点から三次元座標を推定する。座標の推定には、三角測量など一般的な三次元座標推定手法を用いる。

三次元点群検出部１１３における特徴点から三次元座標を推定する方法について、図５、６を用いて具体的に説明する。図５、６は画像テンプレートマッチングと三角測量を用いた三次元座標計算の説明図である。図５は、三次元点群検出部１１３が取得する入力画像であり、現在の入力画像であるフレームｔと、過去の入力画像であるフレームｔ－１とを左から右へ時系列に並べて示している。なお、以下の図面において、移動体１００が走行する路面に直交する方向をＹ方向、路面と平行であって移動体１００の進行する方向（奥行き方向）をＺ方向、路面と平行であって移動体１００の進行方向と直交する方向をＸ方向とする。

まず、三次元点群検出部１１３は、現在のフレームであるフレームｔにおいて、特徴点ＰＦｔを検出する。図５では、路上に設置されている箱状の障害物のエッジ（角部）を、特徴点ＰＦｔとして検出した例を示している。次に、フレームｔの特徴点ＰＦｔと、過去のフレームであるフレームｔ－１において既に検出されている特徴点との対応付けを行い、対応点ペアを特定する。図５に示す一例の場合、画像テンプレートマッチングによって、フレームｔの特徴点ＰＦｔと、フレームｔ－１の特徴点ＰＦｔ－１とが対応点ペアとして特定される。

最後に、三次元点群検出部１１３は、三角測量などを用い、フレームｔの特徴点ＰＦｔの三次元点を推定して三次元座標を算出する。具体的な方法を、図６を用いて説明する。まず、フレームｔの撮影時におけるカメラ１００Ｂ（ｔ）のレンズの光学中心と、フレームｔ上の特徴点ＰＦｔとを結ぶ直線をひく。次に、フレームｔ－１の撮影時におけるカメラ１００Ｂ（ｔ－１）のレンズの光学中心と、フレームｔ－１上の特徴点ＰＦｔ－１とを結ぶ直線をひく。そして、２本の直線の交点を、特徴点ＰＦｔとして検出した障害物のエッジ（角部）の実在点（三次元点Ｐｔｄ）であると推定する。

三次元点Ｐｔｄの位置座標は、（ｉ）フレームｔ－１、及び、フレームｔのカメラ姿勢、（ｉｉ）フレームｔ－１における特徴点ＰＦｔ－１の座標、（ｉｉｉ）フレームｔにおける特徴点ＰＦｔの座標、（ｉｖ）カメラの焦点距離（カメラのレンズの光学中心からフレームまでの距離）、の各情報を用いて算出される。

三次元点群蓄積部１１４は、三次元点群検出部１１３において算出された三次元点を、バッファなどに蓄積する。一般的に、１つのフレームについて、複数の特徴点が検出される。そして、それぞれの特徴点について、対応点ペアが特定された特徴点については、三次元点が推定され三次元座標が算出される。なお、以下の説明において、三次元点の集合を三次元点群と示す。すなわち、三次元点群蓄積部１１４は、三次元点群検出部１１３から三次元点群の情報（三次元座標）を取得し、蓄積する。

三次元点群蓄積部１１４には、過去のフレームにおいて算出された三次元点群の情報が累積されている。三次元点群検出部１１３に新しいフレームが入力され、当該フレーム（現在のフレーム）について特徴点の三次元座標が算出される都度、三次元点群蓄積部１１４から三次元点群蓄積部１１４に三次元点群の情報が出力される。三次元点群蓄積部１１４には、すでに蓄積されている三次元点群の情報に加えて、入力された三次元点群の情報が格納される。１つのフレームの三次元点の個数が多く、また、入力されるフレーム数が多い場合、三次元点群蓄積部１１４に入力される三次元点の個数が膨大となり、全ての情報を格納しようとすると、膨大な容量のバッファが必要となる。一般的に、バッファの容量は限られているため、リングバッファ等を用い、FIFO(First in first out)方式で保存するデータの置き換えを行うことが好ましい。

ノイズ点除去部１１５は、三次元点群蓄積部１１４に蓄積されている三次元点の中からノイズ点を抽出して除去する。同じ構造物の局所部分に検出された三次元点群に含まれる三次元点は、正しく検出されていれば、カメラからの距離（Ｚ方向の座標、奥行き）が同程度になる。従って、該三次元点群の中に、明らかに他の三次元点のＺ座標と位置が異なる座標を有する三次元点が含まれている場合、当該三次元点は、正しく三次元座標が算出されていない可能性が高い。従って、このような三次元点をノイズ点として抽出し、三次元点群から除去することにより、後段の障害物検知において、検出精度を向上させることができる。

ここで、ノイズ点の発生メカニズムについて、図７、８を用いて説明する。図７、８は、ノイズ点発生のメカニズムの一例を説明する図である。正しく三次元座標が算出されない主な原因の一つに、対応点ペアの誤特定があげられる。画像中に繰り返しパターンが存在すると、繰り返しパターン上には類似度が近しい特徴点が複数存在する。繰り返しパターンの一例として、図７に示すような導流体（一定間隔で路上に白線が描かれている道路標示、ゼブラゾーンとも呼ばれる）があげられる。過去の入力画像であるフレームｔ－１において、導流体の白線のうちＹ方向上から５本目の白線部分に特徴点ＰＦｔ－１が検出されているものとする。また、現在の入力画像であるフレームｔにおいて、導流体の白線のうちＹ方向上から６本目の白線部分に特徴点ＰＦｔ´が検出され、Ｙ方向上から５本目の白線部分に特徴点ＰＦｔが検出されたものとする。

画像テンプレートマッチングにより対応点ペアを特定する場合、それぞれの特徴点を中心とする微小領域を切り出して類似度を算出する。正しくは、フレームｔ－１の特徴点ＰＦｔ－１と、フレームｔの特徴点ＰＦｔとが対応点ペアである。（共に、導流体の白線のうちＹ方向上から５番目の白線部分に位置する特徴点である。）ところが、特徴点ＰＦｔ´と特徴点ＰＦｔは、どちらも導流体の白線上に存在している。このため、特徴点ＰＦｔ´の微小領域と特徴点ＰＦｔの微小領域とが極めて類似しており、フレームｔ－１の特徴点ＰＦｔ－１と、フレームｔの特徴点ＰＦｔ´とが対応点ペアとして誤特定される可能性がある。

特徴点ＰＦｔを対応点ペアとして正しい三次元点を推定すると、図８に示すように、推定された三次元点Ｐｔｄは、路面上（Ｙ＝０）であってカメラ１００Ｂ（ｔ）からの奥行がＺ＝ｄ１の位置であると算出される。一方、間違った特徴点ＰＦｔ´を対応点ペアとして間違った三次元点を推定すると、推定された三次元点Ｐｔｄ´は、路面から高さＹ＝ｈ２の位置であってカメラ１００Ｂ（ｔ）からの奥行がＺ＝ｄ２の位置であると算出される。

このように、間違った対応点ペアを用いて三次元点を推定してしまった場合、正しく推定した三次元点と異なる三次元座標が算出されてしまう。特に、奥行き方向であるＺ軸は、正しい三次元点の位置と間違った三次元点の位置との差が大きく算出される傾向にある。

ノイズ点除去部１１５は、上述したノイズ点の性質に基づき、三次元点群蓄積部１１４に蓄積されている三次元点の中からノイズ点を除去する。すなわち、ある三次元点のカメラ１００Ｂからの奥行方向の距離が、画像上における近傍領域に存在する他の三次元点群と大きく異なる場合、当該三次元点をノイズ点と推定し除去する。

図９は、第１の実施形態に係わる三次元点群からノイズ点を除去する手順を説明するフローチャートである。まず、三次元点群蓄積部１１４に蓄積されている三次元点群を取得し、現在フレーム（フレームｔ）に投影する。そして、それぞれの三次元点について、フレーム上での座標（画像座標）を算出する（Ｓ１）。例えば、三次元点群蓄積部１１４にＴ個の三次元点群が蓄積されている場合、Ｓ１ではＴ個の画像座標が算出される。

次に、Ｔ個の三次元点から、ノイズ点か否かの判定を最初に行う三次元点をセットする（Ｓ２）。例えば、Ｔ個の三次元点のそれぞれに対し１～Ｔの連番を付与しておく。Ｓ２では、１番が付与された三次元点が、最初にノイズ点判定を行う点としてセットされる。図９の手順では、ノイズ点の判定を行う三次元点の番号を変数ｎに代入して保持することにより、ノイズ候補点としてのノイズ点判定対象点が特定される。

続いて、ノイズ点判定対象点である三次元点について、当該三次元点を含む評価領域Ｒｅを設定する（Ｓ３）。図１０、及び、図１１は、ノイズ点除去方法の一例を説明する図である。図１０は、三次元点群蓄積部１１４に蓄積されている三次元点群を現在フレームに投影して得られる画像である。図１１は、評価領域Ｒｅに含まれる三次元点の三次元座標を説明する図である。なお、図１０、１１において、網掛けされた丸印で表される三次元点Ｐｔｄは正しく推定された三次元点を示しており、バツ印を含む白丸印で表される三次元点Ｐｔｄ´は誤推定された三次元点を示している。

まず、図１１に示すように、三次元点群蓄積部１１４に１５個の三次元点からなる三次元点群ＰＧが蓄積されているものとする。これら１５個の三次元点群ＰＧを現在フレームに投影すると、図１０に示す画像が得らえる。例えば、Ｓ２において、ノイズ点判定対象点として三次元点Ｐ１がセットされたものとする。この場合、Ｓ３では、点Ｐ１を中心する所定の大きさの矩形領域が、評価領域Ｒｅとして設定される。

そして、評価領域Ｒｅに含まれる三次元点を抽出する（Ｓ４）。図１０に示す一例の場合、三次元点Ｐ１を含む４つの三次元点が抽出される。現在フレームに投影された三次元点が評価領域Ｒｅに含まれるか否かの判定は、Ｓ１で算出された画素座標を用いて行われる。

続いて、Ｓ４で抽出された三次元点の平均距離（カメラからそれぞれの三次元点までの奥行方向の距離の平均値）を算出する（Ｓ５）。すなわち、Ｓ４で抽出された三次元点の三次元座標について、Ｚ方向の平均値を算出する。例えば、図１１に示す一例の場合、評価領域Ｒｅに含まれる三次元点は、Ｐｔｄ１´、Ｐｔｄ２、Ｐｔｄ３、Ｐｔｄ４、の４点となる。従って、これら４点のＺ座標の平均値を算出する。

そして、ノイズ点判定対象点のカメラからの奥行方向の距離と、Ｓ４で算出した平均距離の差分を算出する。更に、算出した差分を平均距離で割り、ノイズ評価値を算出する（Ｓ６）。例えば、Ｓ６におけるノイズ評価値の算出は、以下の（１）式を用いて計算される。

ノイズ評価値＝｜判定対象点の距離－平均距離｜／平均距離 …（１）式
（１）式において、ノイズ評価値は、判定対象点の距離（カメラからの奥行方向の距離）が平均距離に近いほどゼロに近づく。逆に、判定対象点の距離（カメラからの奥行方向の距離）が平均距離から遠いほど大きな値となる。

次に、Ｓ６で算出されたノイズ評価値と予め設定された閾値とを比較する。ノイズ評価値が閾値よりも大きい場合（Ｓ７、ＹＥＳ）、判定対象点をノイズ点と判定し、三次元点群から除去する（Ｓ８）。Ｓ８が終了すると、Ｓ９に進む。一方、ノイズ評価値が閾値以下の場合（Ｓ７、ＮＯ）、判定対象点はノイズ点ではない（正しく推定された三次元点である）と判定されるため、Ｓ８をスキップしてＳ９に進む。なお、Ｓ７においては、距離差分を平均距離で割ったものをノイズ評価値としているので、平均距離の大きさにかかわらず、ひとつの閾値で全ての判定対象点のノイズ判定を行うことができる。

Ｓ９では、Ｓ１で取得した全ての三次元点についてノイズ点判定を行ったか否かを確認する。未判定の三次元点が存在する場合（Ｓ９、ＮＯ）、次の判定対象点をセットし（Ｓ１０）、Ｓ３からＳ８の一連のノイズ点判定及び除去手順を繰り返し実行する。未判定の三次元点が存在しない場合、つまり、全ての三次元点についてノイズ点判定を実施した場合（Ｓ９、ＹＥＳ）、三次元点群からノイズ点を除去する一連の手順を終了する。

例えば、Ｔ個の三次元点のそれぞれに対し１～Ｔの連番を付与しておき、１番から順に判定対象点としていく場合、Ｓ９において、変数ｎの値である現在の判定対象点がＴである場合（ｎ＝＝Ｔ）、全ての三次元点の判定が終了したとみなすことができる。Ｓ９において、ｎ＜Ｔの場合、次の番号の三次元点を判定対象点として（ｎ＝ｎ＋１）、Ｓ３からＳ８の一連のノイズ点判定及び除去手順を繰り返し実行する。

ノイズ点除去部１１５は、ノイズ点除去が終了した三次元点群を、障害物特定部１２へ出力する。

このように、本実施形態によれば、入力画像から推定された三次元点群に含まれる三次元点について、奥行き位置に基づき三次元座標が正しく算出されているか否かを判定する。正しく三次元座標が算出されていないと推定される三次元点は、ノイズ点として三次元点群から除去してから、障害物特定部１２へ当該三次元点群を入力する。従って、障害物特定部１２は、ノイズ点が除去された確度の高い三次元点を用いて障害物の特定を行うことができるので、何もない空間に誤って障害物を誤検知する可能性が低減され、障害物を精度よく検出することができる。

なお、入力画像から検出される特徴点の個数は、画像によって大きく変動する。図１０及び図１１に示す一例では、特徴点を１５個としているが、実際には数百～数千個のオーダーで検知される場合もあり得る。

また、上述の一例では、カメラにより撮像された撮影画像を入力画像としているが、レーザセンサなどの距離センサやGPSなどの位置センサにより得られたデブスマップ（距離画像）を入力画像としてもよい。
（第２の実施形態）
上述の第１の実施形態では、三次元点群から評価領域に含まれる三次元点を抽出する際に、個々の三次元点の画素座標が評価領域内に存在するか否かを判定している。この場合う、１フレームあたりの判定処理回数は、三次元点の数の二乗程度となる。従って、三次元点数が増えるにつれ当該判定処理回数が二次関数的に増加してしまうため、処理時間が長くなってしまい、リアルタイム処理が困難な場合がある。

これに対し、本実施形態においては、積分画像を用いて三次元点が評価領域に含まれるか否かを判定し、処理を高速化している点が異なっている。本実施形態における障害物検知装置の構成は、図１～図４に示す第１の障害物検知装置と同様であるので、説明を省略する。

図１２は、第２の実施形態に係わる三次元点群からノイズ点を除去する手順を説明するフローチャートである。まず、三次元点群蓄積部１１４に蓄積されている三次元点群を取得し、現在フレーム（フレームｔ）に投影する。そして、それぞれの三次元点について、フレーム上での座標（画像座標）を算出する（Ｓ１１）。

次に、距離画像と点群数画像とを作成する（Ｓ１２）。距離画像とは、Ｓ１１で算出された三次元点の画像座標の画素値を、当該三次元点の三次元座標のＺ方向の値とする画像である。例えば、三次元点を現在フレームに投影して得られる画像座標が（Ｘ，Ｙ）＝（１００、１００）でり、当該三次元点の奥行方向の距離が２５である場合、作成される距離画像の（Ｘ，Ｙ）＝（１００、１００）の画素値は２５となる。なお、三次元点を投影したフレームにおいて、三次元点が存在しない座標の画素値は０とする。また、１つの画素に複数の三次元点が投影されている場合、距離画像における当該画素の画素値は、該画素に投影された複数の三次元点のＺ方向の値の和とする。

点群数画像とは、Ｓ１１で三次元点を投影したフレームにおいて、各座標に投影された三次元点の数を画素値とする画像である。例えば、４つの三次元点を現在フレームに投影して得られる画像座標が（Ｘ，Ｙ）＝（１００、１００）、（１００、１００）、（１００、１０１）、（９９、１００）である場合、作成される点群数画像の（Ｘ，Ｙ）＝（１００、１００）の画素値は２、（Ｘ，Ｙ）＝（１００、１０１）、（９９、１００）の画素値は１となる。三次元点を投影したフレームにおいて、三次元点が存在しない座標の画素値は０とする。

次に、Ｓ１２で作成した距離画像の積分画像（以下、距離積分画像と示す）と、点群数画像の積分画像（以下、点群数積分画像を示す）を作成する（Ｓ１３）。積分画像とは、画像の画素値を積分した画像であり、元の画像中に任意に設定した矩形領域内の画素値の層和を高速に計算するために作成する画像である。積分画像における各画素の画素値は、元の画像における同じ座標の画素を右下角、原点（（Ｘ、Ｙ）＝（０、０）の点）を左上角とする矩形領域内の画素値の総和である。Ｓ１３では、距離画像と点群数画像について、それぞれ積分画像を作成する。

続いて、Ｓ１１で取得した三次元点から、ノイズ点か否かの判定を最初に行う三次元点をセットする（Ｓ１４）。例えば、Ｔ個の三次元点のそれぞれに対し１～Ｔの連番を付与しておく。Ｓ１４では、１番が付与された三次元点が、最初にノイズ点判定を行う点としてセットされる。図１２の手順では、ノイズ点の判定を行う三次元点の番号を変数ｎに代入して保持することにより、ノイズ点判定対象点が特定される。

次に、ノイズ点判定対象点である三次元点について、当該三次元点を含む評価領域Ｒｅを設定し（Ｓ１５）、評価領域Ｒｅに含まれる三次元点群の平均距離（カメラからそれぞれの三次元点までの奥行方向の距離の平均値）を算出する（Ｓ１６）。このＳ１６における平均距離の算出は、Ｓ１３で作成した積分画像を用いて行う。

図１３は、積分画像による評価領域内の値の総和計算手順を説明する図である。図１３に示すように、評価領域Ｒｅに含まれる画素値の総和は、以下に示す（２）式を用いて計算される。

ＳＲｅ＝ＳＲａ－ＳＲｂ－ＳＲｃ＋ＳＲｄ …（２）式
ただし、（２）式において、ＳＲｘは矩形領域Ｒｘにおける画素値の総和を示している。
ＳＲａ、ＳＲｂ、ＳＲｃ、ＳＲｄは、それぞれ積分画像における矩形領域Ｒａ、Ｒｂ、Ｒｃ、Ｒｄの右下角の画素値として取得できる。従って、（２）式は、積分画像から４つの画素位置の画素値を取得し、和差演算を３回行うことで算出することができる。

平均距離は、評価領域Ｒｅに含まれる三次元点の距離の和を、評価領域Ｒｅに含まれる三次元点の個数で割ることで算出できる。従って、Ｓ１２で作成した距離積分画像における評価領域Ｒｅに含まれる画素値の総和を、Ｓ１２で作成した点群数積分画像における評価領域Ｒｅに含まれる画素値の総和で割り、平均距離を算出することができる。

そして、ノイズ点判定対象点のカメラからの奥行方向の距離と、Ｓ１６で算出した平均距離の差分を算出する。更に、算出した差分を平均距離で割り、ノイズ評価値を算出する（Ｓ１７）。Ｓ１７におけるノイズ評価値の算出は、図９を用いて説明した第１の実施形態のＳ６と同様である。そして、Ｓ１７で算出されたノイズ評価値と予め設定された閾値とを比較し、ノイズ評価値が閾値よりも大きい場合（Ｓ１８、ＹＥＳ）、判定対象点をノイズ点と判定し、三次元点群から除去する（Ｓ１９）。Ｓ１９が終了すると、Ｓ２０に進む。一方、ノイズ評価値が閾値以下の場合（Ｓ１８、ＮＯ）、判定対象点はノイズ点ではない（正しく推定された三次元点である）と判定されるため、Ｓ１９をスキップしてＳ２０に進む。

Ｓ２０では、Ｓ１１で取得した全ての三次元点についてノイズ点判定を行ったか否かを確認する。未判定の三次元点が存在する場合（Ｓ２０、ＮＯ）、次の判定対象点をセットし（Ｓ２１）、Ｓ１５からＳ１９の一連のノイズ点判定及び除去手順を繰り返し実行する。未判定の三次元点が存在しない場合、つまり、全ての三次元点についてノイズ点判定を実施した場合（Ｓ２０、ＹＥＳ）、三次元点群からノイズ点を除去する一連の手順を終了する。

上述した第１の実施形態におけるノイズ点の除去手順においては、判定対象点を次の三次元点にセットする都度、個々の三次元点の画素座標が評価領域内に存在するか否かを判定するために三次元点の二乗回程度の演算が必要であった。そして、評価領域内に存在する三次元点を用い、ノイズ評価値を算出する必要があった。これに対し、本実施形態では、判定対象点の三次元点が変わっても、Ｓ１３で作成した距離画像を用いて四則演算を７回行うだけでノイズ評価値を算出することができる。従って、演算量を低減し、処理コストを削減することが可能であるため、演算の高速化を図ることができる。

なお、ノイズ点除去部１１５は、三次元点の個数に応じて、第１の実施形態によるノイズ点除去手順と第２の実施形態によるノイズ除去手順とを切り替えて用いるように構成してもよい。

本発明のいくつかの実施形態を説明したが、これらの実施形態は、一例として示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれると共に、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

１…障害物検知装置
１０…画像処理装置
１０ａ…プロセッサ
１０ｂ…メモリ
１０ｄ…画像処理アクセラレータ
１０ｅ…処理回路
１１…画像処理部
１２…障害物特定部
１００…移動体
１００Ａ…出力部
１００Ｂ…センサ（カメラ）
１００Ｄ…通信部
１００Ｅ…ディスプレイ
１００Ｆ…スピーカ
１００Ｇ…動力制御部
１００Ｈ…動力部
１００Ｉ…バス
１１１…画像処理制御部
１１２…カメラ姿勢推定部
１１３…三次元点群検出部
１１４…三次元点群蓄積部
１１５…ノイズ点除去部

Claims

移動体に設けられたセンサにより得られる撮影画像に基づき、前記移動体外部に存在する障害物を検知する障害物検知装置であって、
前記撮影画像から複数の特徴点を検出し、それぞれの前記特徴点について、三次元点の位置である三次元座標を推定する三次元点検出部と、
検出された複数の前記三次元点からなる三次元点群を蓄積する三次元点群蓄積部と、
前記三次元点群から、前記三次元座標が誤推定されている前記三次元点をノイズ点として検出し除去するノイズ点除去部と、
を有し、
前記ノイズ点除去部は、一の前記三次元点をノイズ候補点として抽出し、前記三次元点群を前記撮影画像に投影した際に、前記撮影画像において前記ノイズ候補点の投影位置を中心とする所定の領域内に投影された一つ以上の前記三次元点である判定対象三次元点を抽出し、前記ノイズ候補点の前記センサからの距離と、前記判定対象三次元点の前記センサからの平均距離とを比較し、前記ノイズ候補点がノイズ点であるか否かを判定する、障害物検知装置。
前記ノイズ点除去部は、前記ノイズ候補点の前記センサからの距離と前記判定対象三次元点の前記センサからの平均距離との差を、前記平均距離で割った評価値が、所定の閾値より大きい場合に前記ノイズ候補点がノイズ点であると判定することを特徴とする、請求項１に記載の障害物検知装置。
前記ノイズ点が除去された前記三次元点群を用いて前記障害物を特定する障害物特定部を更に有することを特徴とする、請求項１に記載の障害物検知装置。
前記ノイズ点除去部は、前記三次元点群を前記撮影画像に投影した投影画像において、前記三次元点群の前記センサからの距離についての第一積分画像と、前記三次元点群の数についての第二積分画像とを作成し、前記第一積分画像と前記第二積分画像とを用いて前記判定対象三次元点の前記センサからの平均距離を算出することを特徴とする、請求項１に記載の障害物検知装置。