WO2017150136A1

WO2017150136A1 - 外乱除去装置

Info

Publication number: WO2017150136A1
Application number: PCT/JP2017/004839
Authority: WO
Inventors: 孝之稗方
Original assignee: 株式会社神戸製鋼所; コベルコ建機株式会社
Priority date: 2016-03-02
Filing date: 2017-02-10
Publication date: 2017-09-08
Also published as: EP3416129A4; EP3416129A1; JP6625446B2; JP2017157017A; CN108713217A; EP3416129B1; US20190066270A1; CN108713217B; US10565690B2

Abstract

抽出部（１５１）は、測距センサ（１６）が取得した各距離画像に対して、距離が小さい順に一つ以上の所定個数の画素を、外乱物の候補画素として抽出する。算出部（１５２）は、現フレームの距離画像中の各候補画素と、各候補画素に対応する過去フレームの距離画像中の各候補画素との時間的な変化の特徴を示す特徴量を算出する。除去部（１５３）は、算出部（１５２）により算出された特徴量が、所定の基準特徴量よりも大きな候補画素を、外乱物を示す画素として決定し、現フレームの距離画像から除去する。

Description

外乱除去装置

　本発明は、測距センサで計測された距離画像に含まれる外乱物を除去する技術に関するものである。

　例えば、油圧ショベルや油圧クレーン等の建設機械では、ブームやアタッチメント等の作業装置が、建設機械の本体と干渉することを防止するために、本体に測距センサを取付け、本体と作業装置との距離を検知することで、作業装置による本体への干渉防止が図られている。

　例えば、特許文献１には、複数の超音波センサを用いて、運転室の前方に設定された干渉危険領域にバケットが侵入したか否かを判定する干渉防止装置が開示されている。

　しかし、測距センサを備える干渉防止装置では、降雨時や降雪時に、雨滴及び雪粒等の建設機械の近くを飛来する粒状の外乱物が、本体に接近する作業装置や人物であると誤検出され、オペレータに余計な警告が発せられたり、作業装置が自動停止されたりして、オペレータの作業の妨げになるという問題がある。

　また、特許文献１では、外乱物のことが全く考慮されていないので、外乱物が作業装置として誤検出されることを防止できない。

特開２００１－６４９９２号公報

　本発明の目的は、距離画像から対象物を認識する際に不要となる外乱物を除去する外乱除去装置を提供することである。

　本発明の一態様に係る外乱除去装置は、認識対象となる対象物の周囲を飛来する粒状の外乱物を距離画像から除去する外乱除去装置であって、
　周囲環境の距離分布を示す複数の距離画像を時系列に取得する測距センサと、
　前記測距センサが取得した各距離画像に対して、距離が小さい順に一つ以上の所定個数の画素を、前記外乱物の候補画素として抽出する抽出部と、
　現フレームの距離画像中の各候補画素と、前記各候補画素に対応する過去フレームの距離画像中の各候補画素との時間的な変化の度合いが大きいほど大きな値を示す特徴量を算出する算出部と、
　前記算出部により算出された特徴量が所定の基準特徴量よりも大きな候補画素を、前記外乱物を示す画素として決定し、前記現フレームの距離画像から除去する除去部とを備えるものである。

　本構成によれば、対象物よりも距離センサの近くに位置する外乱物が対象物として誤検出されることを防止できる。

本発明の実施の形態に係る外乱除去装置が適用された建設機械の一例を示す図である。図１に示す建設機械の内部構成を示すブロック図である。本発明の実施の形態に係る外乱除去装置の処理の一例を示すフローチャートである。候補画素の探索例を示す模式図である。特徴量の算出処理を説明する図である。

　［基本構成］
　図１は、本発明の実施の形態に係る外乱除去装置が適用された建設機械１の一例を示す図である。ここでは、建設機械１として、油圧ショベルを例に挙げて説明するが、これは一例であり、油圧クレーン等の、ブーム等の作業装置が建設機械の本体部と干渉する危険性のある建設機械、或いは、周囲の人物と干渉する危険性のある建設機械であれば、どのような建設機械が採用されてもよい。また、外乱除去装置は、建設機械の他にも、ヒューマン型ロボット、及び溶接ロボットというように、人物と干渉する危険性のある可動部を有する機械、或いは、人物と干渉する可能性のある自動車に適用されてもよい。

　図１に示す建設機械１は、クローラ式の下部走行体２と、下部走行体２上に旋回可能に設けられた上部旋回体３と、上部旋回体３に取り付けられた作業装置４とを備えている。下部走行体２及び上部旋回体３は建設機械１の本体部を構成する。

　本明細書において、下部走行体２の前進方向を前方、下部走行体２の後進方向を後方、前方及び後方を総称した方向を前後方向と呼ぶ。また、下部走行体２を後方から前方に見たときの左側の方向を左方、下部走行体２を後方から前方に見たときの右側の方向を右方、左方及び右方を総称して左右方向と呼ぶ。また、建設機械１の上側の方向を上方、建設機械１の下側の方向を下方、上方及び下方を総称して上下方向と呼ぶ。なお、図１では、左方から右方を見たときの建設機械１が示されている。

　作業装置４は、上部旋回体３に対して起伏可能に取り付けられたブーム４１と、ブーム４１の先端部に対して揺動可能に取り付けられたアーム４２と、アーム４２の先端部に対して揺動可能に取り付けられたアタッチメント４３とを備える。ブーム４１、アーム４２、及びアタッチメント４３は、それぞれに対応する油圧シリンダ（図略）によって、起伏或いは揺動するように駆動される。図１では、アタッチメント４３として、例えば掘削用のバケットが採用されているが、これは一例である。

　上部旋回体３には、オペレータが搭乗するキャビン３１と、建設機械１の全体制御を司るコントローラ１５と、オペレータに建設機械１の状態を報知するための報知部１７とが設けられている。

　上部旋回体３において、前面３Ｆ及び後面３Ｂの上端にはそれぞれ、測距センサ１６Ｆ，１６Ｂが配置されている。測距センサ１６Ｆ，１６Ｂは、上部旋回体３の近傍の物体（例えば、後方の人物や前方の作業装置４等）の距離を直接計測可能なセンサである。

　測距センサ１６Ｆ，１６Ｂは、例えば、深度センサやステレオカメラで構成され、周囲の物体までの距離分布を示す複数の距離画像を所定のフレームレート（例えば、５～５０ｆｐｓ程度）で時系列に取得する。

　深度センサとしては、例えば、照射した赤外光が物体で反射されて戻ってくるまでの時間を赤外カメラで計測することで、周囲環境の距離分布を計測するＴＯＦ（ｔｉｍｅ　ｏｆ　ｆｌｉｇｈｔ）方式の測距センサが採用できる。ステレオカメラは、例えば、複数のカメラで撮像された複数の画像内の対応点同士の視差を求めることで、周囲環境の距離分布を計測する測距センサである。

　測距センサ１６Ｆは、前面３Ｆ及び下部走行体２の前面の全域が少なくとも測定エリア１６１ａに含まれるように、前面３Ｆに取り付けられている。これにより、測距センサ１６Ｆは、上部旋回体３及び下部走行体２の前方に位置する作業装置４及び人物を検出できる。

　測距センサ１６Ｂは、後面３Ｂ及び下部走行体２の後面の全域が少なくとも測定エリア１６１ｂに含まれるように、後面３Ｂに取り付けられている。これにより、測距センサ１６Ｂは、上部旋回体３及び下部走行体２の後方に位置する人物を検出できる。以下、測距センサ１６Ｆ，１６Ｂを区別しない場合、測距センサ１６と記述する。

　測距センサ１６で取得された距離画像は、コントローラ１５に取り込まれ、後述する処理が行われ、外乱物が除去され、干渉防止のために利用される。

　図２は、図１に示す建設機械１の内部構成を示すブロック図である。建設機械１は、図１に示す測距センサ１６、報知部１７、及びコントローラ１５の他、エンジン１１、油圧ポンプ１２、コントロールバルブ１３、アクチュエータ１４、操作部１８、及び姿勢センサ１９を備えている。なお、図２において、作動油の流れは太線で示し、制御信号の流れは細線で示している。また、図１において、測距センサ１６及びコントローラ１５が本発明の実施の形態に係る外乱除去装置の構成要素となる。

　エンジン１１は、例えばディーゼル式のエンジンで構成されている。油圧ポンプ１２は、エンジン１１の駆動軸１１Ｘと接続され、エンジン１１の動力によって駆動し、作動油を吐出する。

　コントロールバルブ１３は、例えば、油圧ポンプ１２及びアクチュエータ１４間に介在する油圧パイロット切替弁と、当該油圧パイロット切替弁に入力されるパイロット圧を調整する電磁弁とを含み、コントローラ１５の制御の下、油圧ポンプ１２からアクチュエータ１４への作動油の供給量を調整することで、アクチュエータ１４を作動させる。

　アクチュエータ１４は、例えば、ブーム４１を下部走行体に対して起伏させるブームシリンダ、アーム４２をブーム４１に対して揺動させるアームシリンダ、及び、アタッチメント４３をアーム４２に対して揺動させるアタッチメントシリンダで構成されている。ここでは、アクチュエータ１４として、シリンダを採用したが、これは一例であり、下部走行体２を走行させる走行モータや上部旋回体３を旋回させる旋回モータが採用されてもよい。

　コントローラ１５は、例えば、ＣＰＵ、ＲＯＭ、及びＲＡＭ等を備えるマイクロコンピュータで構成され、建設機械１の全体の制御を司る。本実施の形態では、コントローラ１５は、抽出部１５１、算出部１５２、除去部１５３、干渉判定部１５４、及び干渉防止部１５５を備える。抽出部１５１～干渉防止部１５５は、例えば、ＣＰＵがプログラムを実行することで実現される。

　抽出部１５１は、測距センサ１６が取得した各距離画像に対して、距離が小さい順に一つ以上の所定個数の画素を、外乱物の候補画素として抽出する。ここで、所定個数としては、例えば、１０個程度の値が採用されるが、これは一例であり、コントローラ１５の処理能力との兼ね合いから妥当な値が採用されればよい。

　算出部１５２は、現フレームの距離画像中の各候補画素と、各候補画素に対応する過去フレームの距離画像中の各候補画素との時間的な変化の特徴を示す特徴量を算出する。特徴量の詳細は後述する。

　除去部１５３は、算出部１５２により算出された特徴量が、所定の基準特徴量よりも大きな候補画素を、外乱物を示す画素として決定し、現フレームの距離画像から除去する。

　本実施の形態では、外乱物として、雨滴、雪、みぞれ、あられ、ハエや蚊等の小さな虫等の大気中を飛来する粒状の微小物を想定している。このような微小物は、認識対象となる対象物（人物や作業装置４）に比べて、速度や加速度や移動方向の変化が大きいという特徴を持つ。したがって、候補画素の時間的な変化の特徴を示す特徴量を算出し、その特徴量が基準特徴量よりも大きな候補画素を除外することで、距離画像内に対象物を残存させつつ、外乱物のみを正確に除去することができる。

　干渉判定部１５４は、除去部１５３により外乱物を示す画素（以下、「外乱画素」と呼ぶ。）が除去された距離画像を用いて、干渉の危険性を判定する。ここで、干渉判定部１５４は、外乱画素が除去された距離画像から、建設機械１の周囲に位置する人物及び作業装置４の先端部を認識する。そして、干渉判定部１５４は、認識した人物及び作業装置４の先端部が、建設機械１に対して事前に設定された干渉防止領域に侵入していれば、干渉の危険性があると判定する。干渉防止領域としては、例えば、下部走行体２及び上部旋回体３の側壁から一定の距離範囲内の領域が採用できる。

　作業装置４の先端部とは、アタッチメント４３の先端がアーム４２の先端よりもキャビン３１側に位置しているのであれば、アタッチメント４３の先端が該当し、アーム４２の先端がアタッチメント４３の先端よりもキャビン３１側に位置しているのであれば、アーム４２の先端が該当する。また、干渉判定部１５４は、作業装置４が解体物を把持しており、その解体物がアタッチメント４３の先端及びアーム４２の先端よりもキャビン３１側に位置しているのであれば、解体物が干渉防止領域に侵入している場合、干渉の危険性があると判定してもよい。

　干渉防止部１５５は、干渉判定部１５４により干渉の危険性があると判定された場合、報知部１７を用いてオペレータに警報を行う。

　また、干渉防止部１５５は、姿勢センサ１９による計測データをモニタしながら、操作部１８の操作量に応じた姿勢を作業装置４がとるように、コントロールバルブ１３に制御信号を出力する。ここで、干渉防止部１５５は、干渉判定部１５４により干渉の危険性があると判定された場合、操作部１８の操作量に拘わらず、作業装置４を自動停止させるための制御信号をコントロールバルブ１３に出力する。ここで、干渉防止部１５５は、干渉防止領域よりも狭い自動停止領域に人物及び作業装置４が侵入した場合、作業装置４を自動停止させてもよい。

　報知部１７は、表示装置及びスピーカで構成され、干渉の危険性をオペレータに報知する。ここで、報知部１７は、干渉の危険性を報知するための音声メッセージや警報音をスピーカに出力してもよいし、干渉の危険性を報知するためのメッセージや警報ランプを表示装置に表示してもよい。

　操作部１８は、例えば、作業装置４の姿勢を変更する操作を受け付ける操作レバーで構成され、操作レバーの傾倒量に応じた操作量をコントローラ１５に出力する。

　姿勢センサ１９は、ブーム４１の起伏角度を検出する角度センサ、アーム４２の揺動角度を検出する角度センサ、及びアタッチメント４３の揺動角度を検出する角度センサで構成され、これらの角度センサが検出した計測データを、作業装置４の姿勢を示す計測データとして、コントローラ１５に出力する。

　［フローチャート］
　図３は、本発明の実施の形態に係る外乱除去装置の処理の一例を示すフローチャートである。このフローチャートは、例えば、建設機械１が稼動を始める時、つまり、エンジン１１が始動した時に開始される。

　まず、測距センサ１６は、現フレームの距離画像Ｆ（ｎ）を取得し、抽出部１５１に出力する（Ｓ３０１）。このとき、抽出部１５１は、距離画像Ｆ（ｎ）に対して平滑フィルタを用いたフィルタ処理を前処理として実行し、距離画像Ｆ（ｎ）に含まれる高周波ノイズを除去してもよい。ここで、ｎは距離画像Ｆのフレーム番号を規定するインデックスであり、時間が進むほど値が増大する正の整数である。

　次に、抽出部１５１は、距離画像に設定された探索範囲の中から距離（深度成分）が最小の画素を候補画素として抽出する（Ｓ３０２）。これが一つの外乱画素の候補（雨滴などの外乱物を示す画素の候補）となる。

　次に、抽出部１５１は、距離画像Ｆ（ｎ）において、候補画素を中心に一定の範囲を除く残りの領域を探索範囲として設定する（Ｓ３０３）。

　次に、抽出部１５１は、候補画素を抽出する処理が所定回数実行されていなければ（Ｓ３０４でＮＯ）、処理をＳ３０２に戻し、Ｓ３０２～Ｓ３０４の処理を実行する。一方、候補画素を抽出する処理が所定回数実行されていれば（Ｓ３０４でＹＥＳ）、処理をＳ３０５に進める。雨滴、雪、虫などの外乱物は距離画像上に一つとは限らない。そこで、本実施の形態では、所定個数の候補画素を抽出するために、Ｓ３０２～Ｓ３０４の処理を所定回数実行しているのである。

　図４は、候補画素の探索例を示す模式図である。まず、抽出部１５１は、距離画像Ｆ（ｎ）の全域を探索範囲として設定し、この探索範囲の中から距離が最小の画素を候補画素Ｐ１（ｎ）として抽出する。次に、抽出部１５１は、候補画素Ｐ１（ｎ）から一定の半径の範囲Ｄ１（ｎ）を距離画像Ｆ（ｎ）から除いた領域を探索範囲として設定し、この探索範囲の中から距離が最小の画素を候補画素Ｐ２（ｎ）として抽出する。次に、抽出部１５１は、範囲Ｄ１（ｎ）と、候補画素Ｐ２（ｎ）から一定の範囲の範囲Ｄ２（ｎ）とを距離画像Ｆ（ｎ）から除いた領域を探索範囲として設定し、この探索範囲の中から距離が最小の画素を候補画素Ｐ３（ｎ）として抽出する。

　このように、抽出部１５１は、距離画像Ｆ（ｎ）に対して、抽出済みの候補画素を含む一定の範囲Ｄ１（ｎ），Ｄ２（ｎ），Ｄ３（ｎ）を除去して探索範囲を設定し、設定した探索範囲から距離が最小の画素を抽出する処理を所定回数繰り返すことで、所定個数の候補画素を抽出する。

　候補画素から一定の範囲内に含まれる画素は、同一の外乱物からの反射光を受光している恐れがある。そこで、本実施の形態では、探索済みの候補画素から一定の範囲を除く領域に探索範囲を設定し、同一の外乱物から複数の候補画素が抽出されることを防止している。したがって、一定の範囲のサイズとしては、例えば、除去対象となる外乱物の想定される最大サイズや、その最大サイズに一定のマージンを設けたサイズが採用されればよい。

　図３に参照を戻す。Ｓ３０５では、算出部１５２は、Ｓ３０２において距離画像Ｆ（ｎ）から抽出された所定個数の候補画素のそれぞれについて、１フレーム前の距離画像Ｆ（ｎ－１）内での対応する候補画素を決定し、対応する候補画素同士を用いて特徴量を算出する（Ｓ３０５）。

　図４の例では、距離画像Ｆ（ｎ）に対して、候補画素Ｐ１（ｎ）、Ｐ２（ｎ）、Ｐ３（ｎ）が抽出され、距離画像Ｆ（ｎ－１）に対して候補画素Ｐ１（ｎ－１）、Ｐ２（ｎ－１）、Ｐ３（ｎ－１）が抽出されている。

　この場合、算出部１５２は、候補画素Ｐ１（ｎ）については、距離画像Ｆ（ｎ－１）で抽出された候補画素Ｐ１（ｎ－１）、Ｐ２（ｎ－１）、Ｐ３（ｎ－１）のうち、距離画像上での位置が最も近い候補画素Ｐ１（ｎ－１）を対応する候補画素として抽出する。同様に、算出部１５２は、候補画素Ｐ２（ｎ）については候補画素Ｐ２（ｎ－１）を対応付け、候補画素Ｐ３（ｎ）については候補画素Ｐ３（ｎ－１）を対応付ける。距離画像を構成する各画素は、距離画像上での位置を示すＸ、Ｙ成分と、物体までの距離を示すＺ成分（深度成分）との３成分で表される。よって、算出部１５２は、Ｘ成分とＹ成分とが示す位置が近い候補画素同士を対応付ければよい。

　ここで、距離画像上での位置が最も近い候補画素同士を対応付けるのは、両候補画素が同じ外乱物を表しているとの考えに基づいている。したがって、複数のフレームに亘って候補画素同士を対応付けていくことで、フレームアウトするまで同一の外乱物を追跡することができる。

　なお、本実施の形態の手法は、１フレームの時間内で通過しきってしまうような高速で移動する雨等の外乱物を追跡できない。但し、このような高速な外乱物に対して本実施の形態の手法を適用すると、異なる外乱物同士が対応付けられ、動きが複雑になるので、認識対象の対象物に比べて十分に大きな特徴量が得られる。そのため、本実施の形態の手法は、高速で移動する外乱物も、正確に検出できる。

　図５は、特徴量の算出処理を説明する図である。図５に示すように距離画像Ｆ（ｎ）のある１つの候補画素ｐ（ｎ）は、距離画像Ｆ（ｎ－１）で抽出された候補画素のうち、位置が最も近い候補画素ｑ（ｎ－１）と対応付けられている。更に、候補画素ｑ（ｎ－１）は、距離画像Ｆ（ｎ－２）で抽出された候補画素のうち、位置が最も近い候補画素ｑ（ｎ－２）と対応付けられている。更に、候補画素ｑ（ｎ－２）は、距離画像Ｆ（ｎ－３）で抽出された候補画素のうち、位置が最も近い候補画素ｑ（ｎ－３）と対応付けられている。

　そして、このようにして対応付けられた一連の候補画素を用いて特徴量が算出される。本実施の形態では、対応付けられた一連の候補画素を用いて、速度、角度、及び加速度の特徴量が算出され、外乱画素が決定される。以下、Ｓ３０５以降の処理を、速度、角度、及び加速度の特徴量を採用した場合に分けて説明する。

　［速度］
　図５の例では、速度ｖ（ｎ），ｖ（ｎ－１），ｖ（ｎ－２）は以下の式で計算される。

　ｖ（ｎ）＝ｐ（ｎ）－ｑ（ｎ－１）
　ｖ（ｎ－１）＝ｑ（ｎ－１）－ｑ（ｎ－２）
　ｖ（ｎ－２）＝ｑ（ｎ－２）－ｑ（ｎ－３）　　　式（１）
　ｖ（ｎ），ｖ（ｎ－１），ｖ（ｎ－２）は、それぞれ、距離画像Ｆ（ｎ），Ｆ（ｎ－１），Ｆ（ｎ－２）における速度を示す。以下の説明では、速度及び加速度に関し、小文字の符号を付した物理量はベクトルを示し、大文字の符号を付した物理量はスカラーを示すものとする。

　ｐ（ｎ），ｑ（ｎ－１），ｑ（ｎ－２），ｑ（ｎ－３）は、それぞれ、距離画像Ｆ（ｎ），Ｆ（ｎ－１），Ｆ（ｎ－２），Ｆ（ｎ－３）に亘って対応付けられた候補画素であり、Ｘ，Ｙ，Ｚ成分からなる３次元データである。したがって、速度ｖ（ｎ），ｖ（ｎ－１），ｖ（ｎ－２）は、３次元データ（３次元ベクトル）となる。

　算出部１５２は、速度ｖ（ｎ）を利用して速度Ｖｉを算出する（Ｓ３０５）。速度Ｖｉは式（２）で表される。

　Ｖｉ＝｜｜ｖｉ｜｜　　　式（２）
　ここで、速度ｖｉとしては、速度ｖ（ｎ）が採用でき、速度Ｖｉとしては、速度ｖｉの３次元のユークリッド距離が採用できる。なお、式（２）において、右辺のノルムの中身の速度ｖｉはベクトルであり、速度Ｖｉはスカラーである。ここでは、速度ｖｉとしては、速度ｖ（ｎ）を採用したが、これは一例であり、ｖ（ｎ），ｖ（ｎ－１）等の現フレームと過去フレームとの平均速度を採用してもよい。この場合、ｖ（ｎ），ｖ（ｎ－２）というように時系列的に連続していない速度の平均速度が採用されてもよい。

　次に、除去部１５３は、速度Ｖｉが所定の基準速度Ｖｍａｘ（基準特徴量の一例）を超えるか否かにより候補画素ｐ（ｎ）が外乱画素であるか否かを判定する（Ｓ３０６）。そして、速度Ｖｉが事前に定められた基準速度Ｖｍａｘを超える場合、除去部１５３は、候補画素ｐ（ｎ）を外乱画素と判定し、候補画素ｐ（ｎ）を距離画像Ｆ（ｎ）から除去し（Ｓ３０７）、処理をＳ３０１に戻す。一方、除去部１５３は、速度Ｖｉが基準速度Ｖｍａｘを超えていなければ、候補画素ｐ（ｎ）を除去しない。

　ここで、基準速度Ｖｍａｘは、例えば、認識対象となる対象物（人物及び作業装置４）の想定される最大の速度、或いはその最大の速度に一定のマージンを加えた速度が採用できる。これにより、対象物が候補画素として抽出されたとしても、速度Ｖｉが基準速度Ｖｍａｘを下回るので、この候補画素が外乱画素として除去されることが防止できる。

　ここでは、基準速度Ｖｍａｘとして、人物及び作業装置４の想定される最大の速度を採用したが、これは一例であり、認識対象となる対象物に応じて、基準速度Ｖｍａｘは決定されればよい。また、除去部１５３は、外乱画素として判定した候補画素ｐ（ｎ）を除去するに際し、候補画素ｐ（ｎ）を含む一定の範囲Ｄ（ｎ）を距離画像Ｆ（ｎ）から除去すればよい。これにより、同一の外乱物の一部が残存することを防止できる。このことは、後述する角度及び加速度を採用した場合でも同じである。

　また、図５では、候補画素ｐ（ｎ）に対する処理を示したが、他の候補画素ｐ（ｎ）についても同一の処理が行われ、外乱画素であるか否かが判定される。

　雨滴等の外乱物は、人物や作業装置４に比べて高速に落下するので、速度を用いて外乱物を除去する手法は、雨滴等の外乱物を除去する際に有効である。

　［角度］
　図５の例では、角度θ（ｎ），θ（ｎ－１）は以下の式で計算される。

　θ（ｎ）＝∠（ｖ（ｎ），ｖ（ｎ－１））
　θ（ｎ－１）＝∠（ｖ（ｎ－１），ｖ（ｎ－２））　　　式（３）
　θ（ｎ）は、速度ｖ（ｎ）と速度ｖ（ｎ－１）とのベクトル間の角度を示す。

　算出部１５２は、角度θ（ｎ）を角度の特徴量として算出する（Ｓ３０５）。

　次に、除去部１５３は、角度θ（ｎ）が基準角度θｍａｘ（基準特徴量の一例）を超えるか否かにより候補画素ｐ（ｎ）が外乱画素であるか否かを判定する（Ｓ３０６）。そして、角度θ（ｎ）が基準角度θｍａｘを超える場合、除去部１５３は、候補画素ｐ（ｎ）を外乱画素と判定し、候補画素ｐ（ｎ）を距離画像Ｆ（ｎ）から除去し（Ｓ３０７）、処理をＳ３０１に戻す。一方、除去部１５３は、角度θ（ｎ）が基準角度θｍａｘを超えない場合、候補画素ｐ（ｎ）を除去しない。

　ここで、基準角度θｍａｘは、例えば、対象物（作業装置４及び人物）を測距センサ１６で計測して角度θ（ｎ）を求めたときに想定される最大の角度θ（ｎ）、或いはその最大の角度θ（ｎ）に一定のマージンを加えた角度が採用できる。これにより、対象物が候補画素として抽出されたとしても、角度θ（ｎ）が基準角度θｍａｘを下回るので、この候補画素が外乱画素として除去されることが防止できる。

　虫や雪等の外乱物は、急激に方向が変化することが多いので、角度θ（ｎ）を用いて外乱画素を除去する手法は、虫や雪等のような外乱物を除去する際に有効となる。

　［加速度］
　図５の例では、加速度ａ（ｎ），ａ（ｎ－１）は以下の式で計算される。

　ａ（ｎ）＝ｖ（ｎ）－ｖ（ｎ－１）
　ａ（ｎ－１）＝ｖ（ｎ－１）－ｖ（ｎ－２）　　　式（４）
　ここで、ａ（ｎ）は距離画像Ｆ（ｎ）での加速度を示し、ａ（ｎ－１）は距離画像Ｆ（ｎ－１）での加速度を示す。

　算出部１５２は、加速度ａ（ｎ）を利用して加速度Ａｉを求める。加速度Ａｉは、式（５）で表される。

　Ａｉ＝｜｜ａｉ｜｜　　　式（５）
　ここで、加速度ａｉとしては、加速度ａ（ｎ）が採用でき、加速度Ａｉとしては、加速度ａｉの３次元のユークリッド距離が採用できる。なお、式（５）において、右辺のノルムの中身の加速度ａｉはベクトルであり、加速度Ａｉはスカラーである。ここでは、加速度ａｉとしては、加速度ａ（ｎ）を採用したが、これは一例であり、ａ（ｎ），ａ（ｎ－１）等の現フレームと過去フレームとの平均加速度を採用してもよい。この場合、ａ（ｎ），ａ（ｎ－２）というように時系列的に連続していない加速度の平均加速度が採用されてもよい。

　次に、除去部１５３は、加速度Ａｉが所定の基準加速度Ａｍａｘ（基準特徴量の一例）を超えるか否かにより候補画素ｐ（ｎ）が外乱画素であるか否かを判定する（Ｓ３０６）。そして、加速度Ａｉが基準加速度Ａｍａｘを超える場合、除去部１５３は、候補画素ｐ（ｎ）を外乱画素と判定し、候補画素ｐ（ｎ）を距離画像Ｆ（ｎ）から除去し（Ｓ３０７）、処理をＳ３０１に戻す。一方、除去部１５３は、加速度Ａｉが基準加速度Ａｍａｘを超えなければ、候補画素ｐ（ｎ）を除去しない。

　ここで、基準加速度Ａｍａｘは、例えば、認識対象となる対象物（人物及び作業装置４）の想定される最大の加速度、或いはその最大の加速度に一定のマージンを加えた加速度が採用できる。これにより、対象物が候補画素として抽出されたとしても、加速度Ａｉが基準加速度Ａｍａｘを下回るので、この候補画素が外乱画素として除去されることが防止できる。

　ここでは、基準加速度Ａｍａｘとして、人物及び作業装置４の想定される最大の加速度を採用したが、これは一例であり、認識対象となる対象物に応じて、基準加速度Ａｍａｘは決定されればよい。

　距離画像のフレームレートが低いと、算出部１５２は、外乱物をうまく追跡できない可能性がある。この場合、外乱物の速度ｖ（ｎ）がランダムに変化して観測され、大きな加速度ａ（ｎ）が観測される。一方、作業装置４や人物は外乱物に比べて低速なので、フレームレートが低くても、算出部１５２は作業装置４や人物をうまく追跡することができる。この場合、大きな加速度ａ（ｎ）が観測されない。したがって、加速度を用いて外乱物を除去する手法は、フレームレートが低く、外乱物をうまく追跡できない場合に有効となる。

　また、［角度］の欄で述べたように虫や雪は方向の変化が大きいので、虫や雪を測距センサ１６で計測した場合、大きな加速度が観測できる。したがって、加速度を用いて外乱物を除去する手法は、虫や雪に対しても有効である。また、距離画像に含まれるショットノイズもランダムに変化するので、加速度を用いて外乱物を除去する手法はショットノイズに対しても有効である。

　［組み合わせ］
　上記説明では、速度、角度、及び加速度を用いて外乱物を除去する手法は、単独で使用されたが、組み合わされて使用されても良い。この場合、除去部１５３は、候補画素ｐ（ｎ）について、速度、角度、加速度の手法のそれぞれを適用して、外乱画素であるか否かを判定し、少なくとも１つの手法で外乱画素と判定したのであれば、候補画素ｐ（ｎ）は外乱画素であると判定すればよい。

　或いは、除去部１５３は、候補画素ｐ（ｎ）に対し、まず、速度の手法を適用して外乱画素であるか否かを判定し、外乱画素と判定できなかった場合、次に、角度の手法を適用して外乱画素であるか否かを判定し、外乱画素と判定できなかった場合、次に、加速度の手法を適用して外乱画素であるか否かを判定してもよい。

　［変形例］
　（１）上記説明では、認識対象となる対象物として、人物、作業装置４を採用したが、これは一例であり、外乱除去装置が自動車に適用されるのであれば、人物が認識対象の対象物として採用されればよい。

　（２）上記説明では、対象物が自動運転領域に侵入した場合、作業装置４を自動停止させるとして説明したが、下部走行体２の走行動作を自動停止させてもよいし、上部旋回体３の旋回動作を自動停止させてもよい。また、干渉防止部１５５は、作業装置４を自動停止させる代わりに、本体部と干渉しないように動作制限を行ってもよい。

　（３）本発明において、外乱画素を距離画像から除去する態様には、外乱画素を実際には除去せずに、外乱画素を無効画素として設定し、干渉判定部１５４が干渉の危険性を判定するに際して無視する態様も含まれる。

　上述した実施形態の特徴をまとめると次のとおりである。

　本発明の一態様に係る外乱除去装置は、認識対象となる対象物の周囲を飛来する粒状の外乱物を距離画像から除去する外乱除去装置であって、
　周囲環境の距離分布を示す複数の距離画像を時系列に取得する測距センサと、
　前記測距センサが取得した各距離画像に対して、距離が小さい順に一つ以上の所定個数の画素を、前記外乱物の候補画素として抽出する抽出部と、
　現フレームの距離画像中の各候補画素と、前記各候補画素に対応する過去フレームの距離画像中の各候補画素との時間的な変化の度合いが大きいほど大きな値を示す特徴量を算出する算出部と、
　前記算出部により算出された特徴量が所定の基準特徴量よりも大きな候補画素を、前記外乱物を示す画素として決定し、前記現フレームの距離画像から除去する除去部とを備える。

　本態様では、距離画像から、距離が小さい順に一つ以上の所定個数の画素が、外乱物の候補画素として抽出され、時間的な変化の特徴を示す特徴量が、所定の基準特徴量よりも大きな候補画素が、外乱物を示す画素として決定され、現フレームの距離画像から除去される。そのため、測距センサから見て、対象物よりも近くの位置を飛来する粒状の外乱物を距離画像から除去できる。

　上記態様において、前記算出部は、前記現フレームの距離画像中の各候補画素と、各候補画素に対応する、前記過去フレームの距離画像中の各候補画素との位置の時間的な変化を示す速度を、前記特徴量として算出してもよい。

　本態様によれば、高速で飛来する雨滴等の粒状の微小物を外乱物として正確に除去できる。

　上記態様において、前記算出部は、前記現フレームの距離画像中の各候補画素の速度ベクトルと、各候補画素に対応する、前記過去フレームの距離画像中の各候補画素の速度ベクトルとの角度を、前記特徴量として算出してもよい。

　本態様によれば、対象物と比較して飛来する方向が急激に変化する虫や雪等の粒状の微小物を外乱物として正確に除去できる。

　上記態様において、前記算出部は、前記現フレームの距離画像中の各候補画素と、各候補画素に対応する、前記過去フレームの距離画像中の各候補画素との速度の時間的な変化を示す加速度を、前記特徴量として算出してもよい。

　本態様によれば、対象物と比較して速度変化が大きな雪、虫等の粒状の微小物に加え、測距センサで生じるショットノイズも外乱物として正確に除去できる。

　上記態様において、前記算出部は、前記現フレームの距離画像中の各候補画素と、各候補画素に対応する、前記過去フレームの距離画像中の各候補画素との、位置の時間的な変化を示す速度、速度ベクトルの角度、及び前記速度の時間的な変化を示す加速度のそれぞれを前記特徴量として算出し、
　前記基準特徴量は、前記速度、前記角度、及び前記加速度のそれぞれに対応する基準特徴量を含み、
　前記除去部は、前記速度、前記角度、及び前記加速度のうちの少なくとも１つが、前記対応する基準特徴量よりも大きな候補画素を前記外乱物を示す画素として、前記現フレームの距離画像から除去してもよい。

　本態様によれば、速度、速度ベクトルの角度、及び加速度のうち少なくとも１つが、対応する基準特徴量よりも大きな候補画素が外乱物を示す画素として除去されているので、外乱物の検出精度を高めることができる。

　上記態様において、前記抽出部は、前記現フレームの距離画像に対して、抽出済みの候補画素を含む一定の範囲を除去して探索範囲を設定し、前記設定した探索範囲から距離が最小の画素を抽出する処理を所定回数繰り返すことで、前記所定個数の候補画素を抽出してもよい。

　本態様によれば、抽出済みの候補画素から一定の範囲が除去された探索範囲内から、距離が最小の画素を抽出する処理を繰り返すことで候補画素が抽出されているので、同一の外乱物に対して複数の候補画素が抽出されることを防止できる。

　上記態様において、前記対象物は、建設機械が備える、姿勢が変更可能な作業装置であり、
　前記基準特徴量は、想定される前記作業装置の最高速度よりも大きな速度に基づいて決定されてもよい。

　本態様によれば、基準特徴量は、想定される作業装置の最高速度よりも大きな速度に基づいて決定されているので、作業装置を示す画素が外乱物として除去される事態を回避できる。

　上記態様において、前記対象物は、建設機械が備える、姿勢が変更可能な作業装置であり、
　前記基準特徴量は、想定される前記作業装置の最大の角度の変化量よりも大きな角度に基づいて決定されもよい。

　本態様によれば、基準特徴量は、想定される作業装置の最大の角度の変化量よりも大きな角度に基づいて決定されているので、作業装置を示す画素が外乱物として除去される事態を回避できる。

　上記態様において、前記対象物は、建設機械が備える、姿勢が変更可能な作業装置であり、
　前記基準特徴量は、想定される前記作業装置の最高加速度よりも大きな加速度に基づいて決定されてもよい。

　本態様によれば、基準特徴量は、想定される作業装置の最高加速度よりも大きな加速度に基づいて決定されているので、作業装置を示す画素が外乱物として除去される事態を回避できる。

　上記態様において、前記外乱物は、雨、雪、及び虫の少なくとも１つを含む微小物で構成されていてもよい。

　本態様によれば、雨、雪、及び虫等の微小物を外乱物として除去できる。

　上記態様において、前記外乱除去装置は、本体部と、前記本体部に対して姿勢が変更可能な作業装置とを備える建設機械に適用され、
　前記外乱物が除去された距離画像から前記作業装置を前記対象物として認識し、前記認識した対象物が前記本体部に対して一定の距離範囲に位置する場合、前記対象物が前記本体部に干渉する危険性があると判定する干渉判定部と、
　前記干渉判定部により干渉する危険性があると判定された場合、前記作業装置の動作制限、及び警報のいずれか一方を行う干渉防止部とを更に備えてもよい。

　本態様によれば、外乱物が除去された距離画像を用いて対象物が認識され、干渉の危険性が判定されているので、無用な動作制限や警報が頻発されることを防止できる。

Claims

　認識対象となる対象物の周囲を飛来する粒状の外乱物を距離画像から除去する外乱除去装置であって、
　周囲環境の距離分布を示す複数の距離画像を時系列に取得する測距センサと、
　前記測距センサが取得した各距離画像に対して、距離が小さい順に一つ以上の所定個数の画素を、前記外乱物の候補画素として抽出する抽出部と、
　現フレームの距離画像中の各候補画素と、前記各候補画素に対応する過去フレームの距離画像中の各候補画素との時間的な変化の度合いが大きいほど大きな値を示す特徴量を算出する算出部と、
　前記算出部により算出された特徴量が所定の基準特徴量よりも大きな候補画素を、前記外乱物を示す画素として決定し、前記現フレームの距離画像から除去する除去部とを備える外乱除去装置。
　前記算出部は、前記現フレームの距離画像中の各候補画素と、各候補画素に対応する、前記過去フレームの距離画像中の各候補画素との位置の時間的な変化を示す速度を、前記特徴量として算出する請求項１記載の外乱除去装置。
　前記算出部は、前記現フレームの距離画像中の各候補画素の速度ベクトルと、各候補画素に対応する、前記過去フレームの距離画像中の各候補画素の速度ベクトルとの角度を、前記特徴量として算出する請求項１又は２記載の外乱除去装置。
　前記算出部は、前記現フレームの距離画像中の各候補画素と、各候補画素に対応する、前記過去フレームの距離画像中の各候補画素との速度の時間的な変化を示す加速度を、前記特徴量として算出する請求項１～３のいずれかに記載の外乱除去装置。
　前記算出部は、前記現フレームの距離画像中の各候補画素と、各候補画素に対応する、前記過去フレームの距離画像中の各候補画素との、位置の時間的な変化を示す速度、速度ベクトルの角度、及び前記速度の時間的な変化を示す加速度のそれぞれを前記特徴量として算出し、
　前記基準特徴量は、前記速度、前記角度、及び前記加速度のそれぞれに対応する基準特徴量を含み、
　前記除去部は、前記速度、前記角度、及び前記加速度のうちの少なくとも１つが、前記対応する基準特徴量よりも大きな候補画素を前記外乱物を示す画素として、前記現フレームの距離画像から除去する請求項１～４のいずれかに記載の外乱除去装置。
　前記抽出部は、前記現フレームの距離画像に対して、抽出済みの候補画素を含む一定の範囲を除去して探索範囲を設定し、前記設定した探索範囲から距離が最小の画素を抽出する処理を所定回数繰り返すことで、前記所定個数の候補画素を抽出する請求項１～５のいずれかに記載の外乱除去装置。
　前記対象物は、建設機械が備える、姿勢が変更可能な作業装置であり、
　前記基準特徴量は、想定される前記作業装置の最高速度よりも大きな速度に基づいて決定されている請求項１～６のいずれかに記載の外乱除去装置。
　前記対象物は、建設機械が備える、姿勢が変更可能な作業装置であり、
　前記基準特徴量は、想定される前記作業装置の最大の角度の変化量よりも大きな角度に基づいて決定されている請求項１～６のいずれかに記載の外乱除去装置。
　前記対象物は、建設機械が備える、姿勢が変更可能な作業装置であり、
　前記基準特徴量は、想定される前記作業装置の最高加速度よりも大きな加速度に基づいて決定されている請求項１～６のいずれかに記載の外乱除去装置。
　前記外乱物は、雨、雪、及び虫の少なくとも１つを含む微小物で構成されている請求項１～９のいずれかに記載の外乱除去装置。
　前記外乱除去装置は、本体部と、前記本体部に対して姿勢が変更可能な作業装置とを備える建設機械に適用され、
　前記外乱物が除去された距離画像から前記作業装置を前記対象物として認識し、前記認識した対象物が前記本体部に対して一定の距離範囲に位置する場合、前記対象物が前記本体部に干渉する危険性があると判定する干渉判定部と、
　前記干渉判定部により干渉する危険性があると判定された場合、前記作業装置の動作制限、及び警報のいずれか一方を行う干渉防止部とを更に備える請求項１～１０のいずれかに記載の外乱除去装置。