JP6257947B2 - 作業車の周囲情報取得装置 - Google Patents

Description

この発明は、作業車の周囲にある物体の３次元位置情報を取得する作業車の周囲情報取得装置に関する。

従来から、作業機械のブームの前面にステレオカメラを設け、作業機械の前方にある作業対象物を立体撮影するようにしたステレオカメラ装置が知られている（特許文献１参照）。

作業機械は、下部走行体と、この下部走行体の上に設けた上部旋回体と、この上部旋回体に設けた起伏可能なブームと、このブームの先端部に設けたジブと、ジブの先端部に設けたアームと、このアームの先端部に設けた破砕機と、ブームの前面に設けた一対のステレオカメラとを備えている。

一対のステレオカメラは前方に向けられており、作業機械の前方にある破砕する作業対象物を立体撮影するもので、立体撮影によって作業対象の前後方向の奥行きを求めるようにしたものである。

特開２０１０−２４８７７７号公報

しかしながら、このような作業機械では、前方にある作業対象物を立体撮影するだけなので、作業機械を中心にした周辺にある各物体の３次元位置情報を取得することはできないという問題がある。

この発明の目的は、作業車の周囲にある物体の３次元位置情報を取得することのできる作業車の周囲情報取得装置を提供することにある。

請求項１の発明は、作業車のブームの先端部に、この先端部と物体の位置関係を測定する３次元位置測定装置を設け、
この３次元位置測定装置が測定するブーム先端と物体との位置の測定データから物体の３次元位置情報を求める３次元位置情報取得装置を設け、
前記ブームを移動させて作業車を中心にした周囲にある物体の３次元位置情報を取得することを特徴とする。

この発明によれば、作業車を中心にした広範囲の周囲の物体の３次元位置情報を取得することができる。

この発明に係る周囲情報取得装置を搭載した移動式クレーンを示した側面図である。 伸縮ブームの先端部に設けたステレオカメラを示した説明図である。 周囲情報取得装置の構成を示したブロック図である。 図３に示す画像処理コントローラの構成を示したブロック図である。 クレーンとステレオカメラと建物の位置関係と、座標系とを示した説明図である。 クレーンの座標系とステレオカメラの座標系と建物とを示した平面図である。 モニタの表示画面の一例を示した説明図である。 ブームの旋回中心の位置以外を原点に設定する場合を示した説明図である。

以下、この発明に係る作業車の周囲情報取得装置の実施の形態である実施例を図面に基づいて説明する。

［第１実施例］
図１に周囲情報取得装置を搭載したクレーン（作業車）としてラフテレーンクレーン１０を示す。このラフテレーンクレーン１０（以下クレーンとして記載する）は、走行機能を有する車両の本体部分となるキャリア１１と、このキャリア１１の前側に設けられた左右一対の前側アウトリガ１２と、キャリア１１の後側に設けられた左右一対の後側アウトリガ１３と、キャリア１１の上部に水平旋回可能に取り付けられた旋回台１４と、旋回台１４に設けたキャビン２０と、旋回台１４に固定されたブラケット１５に取り付けられた伸縮ブーム１６等とを備えている。

伸縮ブーム１６は、その基端部が支持軸１７を介して取り付けられており、支持軸１７を中心に起伏可能となっている。ブラケット１５と伸縮ブーム１６との間には起伏用シリンダ１８が介装され、この起伏用シリンダ１８の伸縮により伸縮ブーム１６が起伏される。

伸縮ブーム１６は、ベースブーム１６Ａと中間ブーム１６Ｂと先端ブーム１６Ｃとを有し、この順序でベースブーム１６Ａ内に外側から内側に入れ子式に組み合わされて構成されている。また、伸縮ブーム１６は伸縮シリンダ(図示せず)によって伸縮するようになっている。

先端ブーム１６Ｃの先端部にはシーブ(図示せず)が設けられており、このシーブにワイヤロープ（以下ワイヤと表記する）Ｗが掛けられ、このワイヤＷによってフックブロック１９が吊されている。フックブロック１９にはフック２１が取り付けられている。

ワイヤＷは、図示しないウインチによって巻き取られたり、送り出されたりする。

先端ブーム１６Ｃの先端部には、図２に示すように、ＴＶカメラなどからなるステレオカメラ（３次元位置測定装置）３０が図示しないダンパを介して取り付けられている。ステレオカメラ３０は、真下に向けられており、チルト方向とパン方向へ垂直軸線に対して任意の角度に傾斜可能となっている。また、ステレオカメラ３０は、図示しないダンパにより風などにより向きが変わらないようになっている。

ステレオカメラ３０の傾斜（向き）の操作はキャビン２０内に設けた操作部(図示せず)によって行われる。ステレオカメラ３０の傾斜角度は、チルト角検出センサＳ１（図３参照）とパン角検出センサＳ２とで検出する。

図３は、周囲情報取得装置１００の構成を示すブロック図である。

周囲情報取得装置１００は、伸縮ブーム１６の先端部に設けられたステレオカメラ３０と、伸縮ブーム１６の姿勢を検出するブーム姿勢検出センサ５０と、ステレオカメラ３０の傾斜角度を検出するチルト角検出センサＳ１及びパン角検出センサＳ２と、ステレオカメラ３０から出力される画像信号を処理して撮像した物体の特徴点を抽出したり、この特徴点に基づいて各物体の３次元位置情報を求めたりする３次元位置情報取得装置である画像処理コントローラ６０と、取得した３次元位置情報やステレオカメラ３０が撮像した画像を表示するモニタ５１とを備えている。
［ブーム姿勢検出センサ］
ブーム姿勢検出センサ５０は、伸縮ブーム１６の伸長した長さと、伸縮ブーム１６の起仰角と、伸縮ブーム１６の旋回角とを検出するものであり、それぞれを検出する各センサ(図示せず)を有している。
［画像処理コントローラ］
画像処理コントローラ６０は、ステレオカメラ３０で撮像した画像により、クレーン１０の周囲の３次元位置情報として処理するものであり、この実施例では周囲物体の特徴点をクレーン１０の基準点を基準して周囲の３次元位置情報を取得する事例を示す。

画像処理コントローラ６０は、図４に示すように、ステレオカメラ３０の左右の単眼カメラ（撮像手段）３０Ｌ,３０Ｒが撮像した画像を取り込む第１,第２フレームメモリ部６１Ｌ,６１Ｒと、第１,第２フレームメモリ部６１Ｌ,６１Ｒに記憶された画像から物体の特徴点（図５の説明では物体１の特徴点Ｐａ）を抽出する特徴点抽出部６２Ｌ,６２Ｒと、第１,第２フレームメモリ部６１Ｌ,６１Ｒに記憶された画像からクレーン１０のキャリア１１の上面に記した３つの基準点Ｐ１,Ｐ２,Ｐ３（図５参照）を抽出する基準点抽出部６３Ｌ,６３Ｒと、特徴点抽出部６２Ｌ,６２Ｒが抽出した特徴点からステレオカメラ３０の座標系での特徴点位置を算出するカメラ座標系位置算出部６４と、基準点抽出部６３Ｌ,６３Ｒが抽出した３つの基準点からステレオカメラ３０の座標系での３つの基準点位置を算出するカメラ座標系基準点位置算出部６５と、このカメラ座標系基準点位置算出部６５が算出した３つの基準点位置からカメラ座標系とクレーンの座標系との相関関係を算出する相関関係算出部６６と、カメラ座標系位置算出部６４が算出した特徴点の位置を相関関係算出部６６が算出した相関関係に基づいてクレーンの座標系の位置に変換して求めるクレーン座標系位置算出部６７と、クレーン座標系位置算出部６７が算出した位置を記憶していくメモリ６８と、ブーム姿勢検出センサ５０が検出する検出信号に基づいて単眼カメラ３０Ｌ,３０Ｒの画像を第１,第２フレームメモリ部６１Ｌ,６１Ｒに取り込ませたりする制御部７０とを備えている。

制御部７０は、特徴点抽出部６２Ｌ,６２Ｒ及び基準点抽出部６３Ｌ,６３Ｒの抽出処理の制御や、カメラ座標系位置算出部６４及びカメラ座標系基準点位置算出部６５の演算処理の制御を行ったり、相関関係算出部６６の相関関係の算出制御やクレーン座標系位置算出部６７の算出制御を行ったりする。

そして、第１,第２フレームメモリ６１Ｌ,６１Ｒと特徴点抽出部６２Ｌ,６２Ｒとカメラ座標系位置算出部６４とで画像認識手段が構成される。
［動 作］
次に、上記のように構成される周囲情報取得装置１００の動作について説明する。

クレーン１０のキャリア１１の上面には、前述のように、３つの基準点Ｐ１,Ｐ２,Ｐ３が記されており、この基準点Ｐ１,Ｐ２,Ｐ３の位置は、伸縮ブーム１６の旋回中心の位置を原点としたクレーン１０のＸ,Ｙ,Ｚ座標系の位置として、図６に示すように、Ｐ１＝（Ｘ１,Ｙ１,Ｚ１）、Ｐ２＝（Ｘ２,Ｙ２,Ｚ２）、Ｐ３＝（Ｘ３,Ｙ３,Ｚ３）が予め設定されている。

先ず、クレーン１０を図１に示すように作業位置に固定する。次に、伸縮ブーム１６を図５に示すように起仰させて伸長させていき、ステレオカメラ３０で撮像していく。この撮像した一方の画像がモニタ５１に表示されていく。

制御部７０は、伸縮ブーム１６が所定の長さに伸長したら、ステレオカメラ３０が撮像した画像を第１,第２フレームメモリ部６１Ｌ,６１Ｒに取り込ませる。

ここで、例えば、ステレオカメラ３０の単眼カメラ３０Ｌ,３０Ｒが図５に示すように、構造物（物体）である建物１とクレーン１０とを撮像しているものとして説明する。

第１,第２フレームメモリ部６１Ｌ,６１Ｒにステレオカメラ３０の単眼カメラ３０Ｌ,３０Ｒが撮像した画像が取り込まれて記憶されると、特徴点抽出部６２Ｌ,６２Ｒが第１,第２フレームメモリ部６１Ｌ,６１Ｒに記憶された画像の特徴点を抽出する。

この特徴点の抽出は、例えば建物１の画像の輪郭線（エッジ）を画像処理により抽出するとともに、建物１の角部となる点Ｐａを輪郭線の特徴点として画像処理により抽出する。

カメラ座標系位置算出部６４は、特徴点抽出部６２Ｌ,６２Ｒが建物１の特徴点Ｐａをそれぞれ抽出すると、これら特徴点Ｐａの単眼カメラ３０Ｌ,３０Ｒの撮像素子上(図示せず)の画像の位置に基づいて、例えば単眼カメラ３０Ｌの焦点の位置を原点（撮像素子上の光軸位置）にしたカメラ座標系における特徴点Ｐａの位置（ｘ０,ｙ０,ｚ０）を求める。

一方、基準点抽出部６３Ｌ,６３Ｒは、第１,第２フレームメモリ部６１Ｌ,６１Ｒに記憶された画像からクレーン１０のキャリア１１上の３つの基準点Ｐ１,Ｐ２,Ｐ３をそれぞれ抽出する。

カメラ座標系基準点位置算出部６５は、基準点抽出部６３Ｌ,６３Ｒが基準点Ｐ１,Ｐ２,Ｐ３を抽出すると、基準点Ｐ１,Ｐ２,Ｐ３の単眼カメラ３０Ｌ,３０Ｒの画像上の位置に基づいて、単眼カメラ３０Ｌの焦点の位置（図示せず）を原点にしたカメラ座標系の位置（ｘ１,ｙ１,ｚ１）、（ｘ２,ｙ２,ｚ２）、（ｘ３,ｙ３,ｚ３）を求める。

相関関係算出部６６は、カメラ座標系基準点位置算出部６５が求めた基準点Ｐ１,Ｐ２,Ｐ３のカメラ座標系の位置（ｘ１,ｙ１,ｚ１）、（ｘ２,ｙ２,ｚ２）、（ｘ３,ｙ３,ｚ３）と、図６に示すように予め設定されている基準点Ｐ１,Ｐ２,Ｐ３のクレーンのＸ,Ｙ,Ｚ座標系の位置（Ｘ１,Ｙ１,Ｚ１）、（Ｘ２,Ｙ２,Ｚ２）、（Ｘ３,Ｙ３,Ｚ３）とから、カメラの座標系の位置とクレーン１０の座標系の位置との相関関係を求める。

クレーン座標系位置算出部６７は、相関関係算出部６６が求めた相関関係から、カメラ座標系の特徴点Ｐａの位置（ｘ０,ｙ０,ｚ０）をクレーン１０の座標系の位置（Ｘ０,Ｙ０,Ｚ０）として求める。

クレーン座標系位置算出部６７が求めた特徴点Ｐａのクレーン１０の座標系の位置（Ｘ０,Ｙ０,Ｚ０）はメモリ６８に記憶される。

次に、伸縮ブーム１６を旋回させていき、伸縮ブーム１６が例えば所定角度回転していく毎に、ステレオカメラ３０が撮像した画像を第１,第２フレームメモリ部６１Ｌ,６１Ｒに取り込ませていき、第１,第２フレームメモリ部６１Ｌ,６１Ｒに画像が取り込まれる毎に上記動作を繰り返し行い、建物の特徴点を抽出していくとともに、その特徴点の３次元位置を求めていくことにより、クレーン１０を中心にした周辺にある物体の３次元位置情報を取得する。

モニタ５１には、例えば、図７に示すようにステレオカメラ３０が撮像したクレーン１０を中心にした画像を平面視にして表示する。平面視の画像は、例えばステレオカメラ３０の単眼カメラ３０Ｌが撮像した画像の輪郭線を抽出し、３次元位置情報と基にして平面視に変換して表示するものである。

ここでは、伸縮ブーム１６を旋回しながら撮像した画像を図示しない画像メモリに記憶させていき、この画像メモリに記憶した画像を１つの画像に処理し、さらに輪郭線を抽出するとともに、その輪郭線の特徴点の３次元位置情報を基にして、その輪郭線を平面視に変換してモニタ５１に表示させる。

図７に示す画像は、クレーン１０の周辺に２つの建物１,２（図示せず）があった場合の例を示す。建物１が平面視の建物画像１Ｇとして、図示しない建物２が平面視の建物画像２Ｇとしてモニタ５１に表示されるとともに、特徴点を示すマーク画像Ｍａ,Ｍｂとその３次元位置を示す座標とが表示される。また、モニタ５１の中央部には、クレーン１０を示すクレーン画像１０Ｇが表示される。クレーン画像１０Ｇは、グラフィック画像でもよく、ステレオカメラ３０の例えば単眼カメラ３０Ｌが撮像したクレーン１０の画像であってもよい。

このように、図７に示すようにクレーン１０の周辺全体を示す平面視の画像を表示することにより、クレーン１０の周囲状況をクレーン運転者が瞬時に把握することができる。

そして、抽出した建物１,２の特徴点Ｐａ,Ｐｂ（図示せず）を示す部分にマーク画像Ｍａ,Ｍｂを表示させるとともに、そのマーク画像Ｍａ,Ｍｂに３次元位置情報をモニタ５１に表示する。

このように、モニタ５１には、クレーン１０の周辺の建物１,２の全てが表示されるとともに、ブーム１６の旋回中心を中心にした建物の位置や高さ等が表示されるので、オペレータはクレーン１０の周辺状況を把握することができる。

この実施例では、ステレオカメラ３０で撮像したクレーン１０のキャリア１１上の基準点Ｐ１,Ｐ２,Ｐ３の画像に基づいて、建物１の３次元位置情報を求めているので、伸縮ブーム１６の先端部の位置をブームの姿勢を検出するセンサ等によって求めなくてもよく、このため、伸縮ブーム１６の撓みなどの影響を受けることなく、クレーン１０を中心にした周囲の建物１,２の３次元位置情報を正確に取得することができる。

また、クレーン１０を作業位置に固定した後に、ステレオカメラ３０で撮像した画像に基づいて、周囲の３次元位置情報を取得しているので、伸縮ブーム１６の移動によるステレオカメラ３０の移動によってクレーン１０の周囲全体の３次元位置情報を取得できる。また、クレーン１０の座標系と周囲の座標系とを同一にすることができ、このため３次元位置情報を取得するための演算負荷を軽減することができる。

上記実施例では、伸縮ブーム１６の先端部にステレオカメラ３０を設けているが、必ずしもステレオカメラ３０を設ける必要はない。例えば、単眼カメラを伸縮ブーム１６の先端部に設け、伸縮ブーム１６の移動によって得られる単眼カメラの画像からステレオカメラの原理で建物１,２の３次元位置情報を取得するようにしてもよい。

上記実施例では、ブーム１６の旋回中心の位置を原点としたクレーン１０のＸ,Ｙ,Ｚ座標系を設定して、建物１，２の３次元位置情報を取得するようにしているが、例えば、図８に示すように、クレーン１０のキャリア１１の上面の所定位置Ｐｃや地面の所定位置Ｐｄを原点とした座標系で建物の３次元位置情報を取得するようにしてもよい。なお、ｈは地面からキャリア１１の上面までの高さである。

この場合、所定位置Ｐｂ，Ｐｃは、Ｘ,Ｙ,Ｚの座標系に対する位置を予め求めておく必要がある。すなわち、ブーム１６の旋回中心の位置Ｏを原点としたＸ,Ｙ,Ｚの座標系に対する位置が分かれば、どのような位置であってもその位置を原点として座標を設定すればその座標に対する建物１，２の３次元位置情報を取得することができる。

上記実施例では、クレーン１０のキャリア１１上に３つの基準点Ｐ１,Ｐ２,Ｐ３を設定して、建物１,２の３次元位置情報を取得するようにしているが、伸縮ブーム１６の先端部の位置を、伸縮ブーム１６の起伏角度や長さや旋回角度やたわみ量から求めれば、３つの基準点Ｐ１,Ｐ２,Ｐ３を設定しなくてもよい。

また、ステレオカメラ３０を真下（鉛直方向）に向けて撮像する場合、Ｚ軸とｚ軸が同じ方向となれば２つの基準点でもよい。

また、伸縮ブーム１６の先端部にレーザ測距装置を設け、その先端部から建物１,２の特徴点Ｐａ,Ｐｂ（図示せず）までの距離と角度を求めて、建物１,２の３次元位置情報を取得するようにしてもよい。

また、モニタ５１に表示する画像は、図７に示すような平面視の画像でなくても、撮像画像を表示して、その高さ情報を表示するようにしてもよい。

この発明は、上記実施例に限られるものではなく、特許請求の範囲の各請求項に係る発明の要旨を逸脱しない限り、設計の変更や追加等は許容される。

１ 建物（物体）
１０ クレーン
１６ 伸縮ブーム
３０ ステレオカメラ（３次元位置測定装置）
５１ モニタ
６０ 画像処理コントローラ（３次元位置情報取得装置）

Claims (3)

1. 作業車のブームの先端部に、この先端部と物体の位置関係を測定する３次元位置測定装置を設け、
   この３次元位置測定装置が測定するブーム先端と物体との位置の測定データから物体の３次元位置情報を求める３次元位置情報取得装置を設け、
   前記３次元位置測定装置は、撮像手段と、この撮像手段で撮像した画像を認識する画像認識手段とを有し、
   作業車上に少なくとも３点の基準点が設けられ、
   前記３次元位置情報取得装置は、前記ブームを移動させて作業車を中心にした周囲にある物体の３次元位置情報を取得するに際して、前記画像認識手段が認識した３点の基準点の画像上の位置を基にして、前記作業車の所定位置を中心にした周囲にある物体の３次元位置情報を取得することを特徴とする作業車の周囲情報取得装置。
2. 前記作業車を作業位置に固定した後に、前記ブームを移動させて３次元位置情報の取得を開始することを特徴とする請求項１に記載の作業車の周囲情報取得装置。
3. 前記作業車の所定位置は、前記ブームの旋回中心位置であることを特徴とする請求項１又は２に記載の作業車の周囲情報取得装置。