JP5227139B2

JP5227139B2 - 建設機械

Info

Publication number: JP5227139B2
Application number: JP2008290388A
Authority: JP
Inventors: 薫熊谷; 昌史棚橋; 仁志大谷
Original assignee: Topcon Corp
Current assignee: Topcon Corp
Priority date: 2008-11-12
Filing date: 2008-11-12
Publication date: 2013-07-03
Anticipated expiration: 2028-11-12
Also published as: EP2187166A3; US20100121540A1; JP2010117230A; US8412418B2; EP2187166B1; EP2187166A2

Description

本発明はブルドーザ、パワーショベル等の建設機械、特に排土板、バケット等の工事作業具の建設機械本体に対する位置、姿勢を検出可能とした建設機械に関するものである。

土木工事、例えばブルドーザによる整地作業で、整地面に対する整地器具（ブレード）の位置、傾斜等の姿勢を正確に把握することは精度の高い整地作業を実行する上で不可欠であり、又建設機械操縦者が勘に頼らず計画された整地作業を実行する上で、整地器具の位置、傾斜等の姿勢を正確に検出することが必要である。

従来、建設機械で整地器具の高さ位置を検出するものとして特許文献１に示されるものがある。

特許文献１の図６には、レーザ光線５を回転照射して基準面を形成する回転レーザ装置１、及び整地器具２にポール３を立設し、該ポール３に取付けられた受光器４が示されている。該受光器４でレーザ光線５を受光し、前記基準面を検出し、前記受光器４が検出した基準面の位置に基づき前記整地器具２の鉛直方向の位置を検出していた。

特許文献１では、基準面、即ち整地面に対する整地器具２の位置を検出することができるが、建設機械本体６に対する整地器具２の位置、姿勢を検出することはできなかった。一方、操縦者が整地器具２を操作する場合は、建設機械本体６を基準とした位置調整、傾斜等の姿勢調整となる。従って、得られた検出結果に基づく調整作業が、作業者の感覚と一致するとは限らず、誤作業を招く虞れがあった。

又、特許文献２では、建設機械としてブルドーザ７が示され、該ブルドーザ７のブレード８にポール９が立設され、該ポール９の上端に第１のＧＰＳアンテナ１１が設けられ、更にブルドーザ７自体にも前後に第２、第３のＧＰＳアンテナ１２，１３が設けられたことが開示されている。

特許文献２では、図７に示される様に、第１のＧＰＳアンテナ１１によりブレード８の上方方向の絶対位置が検出でき、又第２、第３のＧＰＳアンテナ１２，１３により、ブルドーザ７の位置、方向が検出できる。又、第１のＧＰＳアンテナ１１による位置検出結果と第２、第３のＧＰＳアンテナ１２，１３による位置検出結果に基づき前記ブルドーザ７に対するブレード８の相対位置を演算により求めることも可能である。

ところが、ＧＰＳアンテナを含む位置検出装置は高価であり、更にブレードの前記相対位置を求めるには３組を必要とする等、非常に高価なものとなる。

又、前記ブレード８が前後方向に傾斜した場合、前記ポール９も傾斜し、前記第１のＧＰＳアンテナ１１で検出する水平位置、上下位置と実際の前記ブレード８の位置にずれが生じる。又、前記ポール９を中心として前記ブレード８が水平方向に回転した場合は、前記第１のＧＰＳアンテナ１１の検出結果に変化がない為、前記ブレード８の水平方向の回転位置は検出することができない。

又、ブレード８のブルドーザ７に対する傾斜をチルトセンサで検出する場合もあるが、チルトセンサの応答性が悪く、又振動に対しても反応するので、動いているものの傾斜検出には適しているとはいえない。

特開２００５−３３７７４１号公報

米国特許７３１７９７７Ｂ２号公報

本発明は斯かる実情に鑑み、簡便な構成で建設機械本体に対する工事作業具の位置、傾斜等を含む姿勢を正確に、而もリアルタイムで検出可能とした姿勢検出装置、及び該姿勢検出装置を具備した建設機械を提供するものである。

本発明は、建設機本体と、該建設機本体に設けられた工事作業具と、前記建設機本体の機械中心に対して既知の位置に設けられた２つのカメラと、前記工事作業具に設けられ、前記２つのカメラの視野にそれぞれ含まれる様に配置された少なくとも３のターゲットと、前記２つのカメラで取得したステレオ画像から各画像中で共通する少なくとも３のターゲット像を抽出し、該各ターゲット像の３次元位置を求め、求めた３次元位置に基づき前記工事作業具の前記建設機本体に対する位置、姿勢を算出する演算処理装置とを具備する建設機械に係るものである。

又本発明は、前記カメラはビデオカメラであり、前記演算処理装置は、各フレーム画像毎にターゲットの位置、姿勢を算出する様にした建設機械に係り、又前記ターゲットは円形の反射部、及び該反射部の周囲に非反射部が形成された板又は板状である建設機械に係り、又前記ターゲットの反射部が形成するターゲット像は、カメラ受光部の少なくとも５画素以上を占め、前記演算処理装置はターゲット像の中心をモーメント法により算出する建設機械に係り、又前記建設機本体に設けられ、該建設機本体の位置座標を求めるＧＰＳ装置を更に備え、前記演算処理装置は前記ＧＰＳ装置が測定した位置座標に基づき、前記工事作業具の位置座標を演算する建設機械に係り、更に又前記ＧＰＳ装置は、前記２つのカメラの視野に含まれる様配置されたターゲットを有し、前記演算処理装置は前記ＧＰＳ装置が測定した位置座標に基づき、前記工事作業具の位置座標を演算する建設機械に係るものである。

本発明によれば、建設機本体と、該建設機本体に設けられた工事作業具と、前記建設機本体の機械中心に対して既知の位置に設けられた２つのカメラと、前記工事作業具に設けられ、前記２つのカメラの視野にそれぞれ含まれる様に配置された少なくとも３のターゲットと、前記２つのカメラで取得したステレオ画像から各画像中で共通する少なくとも３のターゲット像を抽出し、該各ターゲット像の３次元位置を求め、求めた３次元位置に基づき前記工事作業具の前記建設機本体に対する位置、姿勢を算出する演算処理装置とを具備するので、複雑で高価な装置を用いることなく、建設機械本体に対する工事作業具の位置姿勢を簡単に、而もノイズ、機器の応答遅れを考慮することなくリアルタイムで検出することができる。

又本発明によれば、前記カメラはビデオカメラであり、前記演算処理装置は、各フレーム画像毎にターゲットの位置、姿勢を算出する様にしたので、動作中の工事作業具の位置姿勢を簡単に、而もリアルタイムで検出することができる。

又本発明によれば、前記ターゲットは円形の反射部、及び該反射部の周囲に非反射部が形成された板又は板状であるので、複雑な画像処理をすることなく、画像中から簡単にターゲット像を抽出でき、又反射部は円形であるので、ターゲットがカメラ光軸に対して傾いたとしても、中心位置は正確に検出することができる。

又本発明によれば、前記ターゲットの反射部が形成するターゲット像は、カメラ受光部の少なくとも５画素以上を占め、前記演算処理装置はターゲット像の中心をモーメント法により算出するので、中心位置の検出精度が向上する。

又本発明によれば、前記建設機本体に設けられ、該建設機本体の位置座標を求めるＧＰＳ装置を更に備え、前記演算処理装置は前記ＧＰＳ装置が測定した位置座標に基づき、前記工事作業具の位置座標を演算し、更に又本発明によれば、前記ＧＰＳ装置は、前記２つのカメラの視野に含まれる様配置されたターゲットを有し、前記演算処理装置は前記ＧＰＳ装置が測定した位置座標に基づき、前記工事作業具の位置座標を演算するので、工事作業具の位置を基にした位置情報の管理・施工が可能となる等の優れた効果を発揮する。

以下、図面を参照しつつ本発明を実施する為の最良の形態を説明する。

先ず、図１に於いて、本発明が実施される建設機械の一例を説明する。

図１では、建設機械としてブルドーザ７が示されており、該ブルドーザ７は前面に工事作業具としてブレード８を具備している。該ブレード８はブルドーザ本体１４に設けられたアーム１５の先端に設けられている。又、前記ブレード８は多機能の使用に対応して、種々の姿勢をとり得る様に、前記ブルドーザ７に対して４軸方向に回転可能となっている。

前記アーム１５は前記ブルドーザ本体１４に水平なＯ軸を中心に傾動可能に設けられ、前記ブレード８は前記アーム１５に対して、水平なＰ軸を中心に回転可能に、又鉛直なＱ軸を中心に回転可能に、又前記ブルドーザ本体１４の前後方向に水平なＲ軸を中心に回転可能に設けられている。

尚、特に図示していないが、各軸に対応して駆動用に油圧シリンダ（図示せず）が設けられ、前記アーム１５を傾動させ、又前記ブレード８をＰ軸、Ｑ軸、Ｒ軸を中心にそれぞれ独立して回転可能となっている。

又、前記ブレード８について基準位置が設定される。該ブレード８の基準位置として、前記Ｏ軸、Ｐ軸、Ｑ軸、Ｒ軸のそれぞれについて、基準位置（０゜）が設定される。基準位置としては、例えば、前記ブレード８の下縁が水平であり、又前記ブルドーザ本体１４の走行面と高さで一致し、又該ブルドーザ本体１４の進行方向に対して直角であり、更に又前記ブレード８の下面が水平である状態で前記Ｏ軸、Ｐ軸、Ｑ軸、Ｒ軸に於ける角度が０゜とされる。

次に、本発明に係る姿勢検出装置１７は、主に、所要位置、例えば運転室２０の屋根の前部に設けられた２台のカメラ（第１撮像部，第２撮像部）１８，１９、該カメラ１８，１９の視野に入る様に所要箇所、所要数、例えば５箇所に設けられたブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅ、照明装置２２及び図２に示される演算処理装置２３によって構成される。

前記ブレード８は泥や水、埃により汚れ易いので、該ブレード８に貼付けたブレード検出用ターゲット２１も同様に汚れ易く前記カメラ１８，１９で認識できなくなる虞れがある。そこで、前記ブレード８には出来るだけ多数の前記ブレード検出用ターゲット２１を貼付ける様にする。

前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅは、それぞれ所定の外形形状、例えば正方形或は円形の板又は板状物である。前記ブレード検出用ターゲット２１は、低反射部の中央に所定の大きさの円形の高反射部が形成されたものとなっており、低反射部と高反射部との間に所定値以上の反射率の差異があればよい。例えば、黒色を背景として中央に反射塗料が塗布されたものが用いられる。尚、後述する様に、前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅは、少なくとも３あればよいが、ブレード検出用ターゲット２１が死角に入ったり、或は泥等が付着して反射光が得られなくなることを想定し、４以上のブレード検出用ターゲット２１が設置される。

前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅの設置場所は、前記ブレード８に対する設置位置を特定することで、該ブレード８と前記ブルドーザ本体１４の中心位置（以下機械中心）（図示せず）との機械的な関係から、前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅと前記機械中心との関係（距離、水平角、鉛直角等）を特定できる。

又、前記カメラ１８，１９についても、前記運転室２０の屋根に対して設置位置が確定できれば、屋根と機械中心との機械的関係から機械中心と前記カメラ１８，１９との位置関係が特定できる。更に、前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅと前記カメラ１８，１９とについても、機械中心を介して位置関係が特定できる。

即ち、前記ブレード８を基準位置（例えば、Ｏ軸、Ｐ軸、Ｑ軸、Ｒ軸に関する回転角がそれぞれ０°）とした状態で、前記カメラ１８，１９と前記各ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅとの距離、前記カメラ１８，１９の視準中心線に対する前記各ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅの鉛直角、水平角等が特定できる。得られた位置関係については、後述する記憶部に設定入力される。

又、前記ブレード８の基準位置で、前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅの取付ける向きが、それぞれ前記カメラ１８，１９に対して同等の角度となる様に設定される。例えば、１つのブレード検出用ターゲット２１の中心を通る垂線が、ターゲット中心と前記カメラ１８とを結ぶ直線と、ターゲット中心と前記カメラ１９とを結ぶ直線とが成す角の２等分線となる様に設置される。

又、前記カメラ１８と前記カメラ１９との関係についても、該カメラ１８，１９間の距離、該カメラ１８，１９の２つの光軸間の角度等を機械的な位置関係から求め、予め特定する。

更に、後述する様に前記カメラ１８，１９で取得した画像のマッチングを実行する為に、前記カメラ１８，１９の位置関係に基づきキャリブレーションを行う。

尚、前記カメラ１８，１９についてはデジタルカメラが使用され、前記カメラ１８，１９の受光部は画素の集合体であるＣＣＤ、或はＣＭＯＳ等である。受光部に於いて、受光する画素の位置が特定でき、又画素の位置から画角が求められる様になっている。

前記カメラ１８，１９としてはビデオカメラが用いられ、或は１０枚／秒以上、好ましくは２０枚／秒程度で静止画像が撮像できるカメラが用いられる。

又、前記ブルドーザ本体１４の所要位置、例えば前記運転室２０の屋根にＧＰＳアンテナ（図示せず）が設けられ、前記運転室２０等所要位置に前記ＧＰＳアンテナからの信号により地上での位置（絶対座標位置）を測定するＧＰＳ信号処理装置（図示せず）が設けられている。前記ＧＰＳアンテナ、前記ＧＰＳ信号処理装置は、ＧＰＳ装置３２（後述）を構成する。

図２に於いて、前記姿勢検出装置１７を説明する。

前記カメラ１８，１９はステレオカメラ２５を形成し、又撮像した画像データは同期制御部２６を介して制御演算部２７、記憶部２８に入力され、前記制御演算部２７、前記記憶部２８には画像処理部２９が接続されている。前記演算処理装置２３は前記同期制御部２６、前記制御演算部２７、前記記憶部２８、前記画像処理部２９により構成される。

前記制御演算部２７は、前記画像処理部２９による画像処理の結果を基に前記ブレード８の位置姿勢を検出し、検出結果に基づき該ブレード８の位置制御、姿勢制御を行う為の指令信号をブレード制御部３１に発し、該ブレード制御部３１は指令信号に基づき図示しない油圧シリンダを駆動する様になっている。

前記制御演算部２７にはＧＰＳ装置３２からの信号が入力され、前記制御演算部２７は前記信号を基に前記ブルドーザ本体１４の位置を演算する。

尚、前記ＧＰＳ装置３２によって前記ブルドーザ本体１４の位置（絶対座標位置）が測定され、該ブルドーザ本体１４の機械中心に対する前記ＧＰＳアンテナの位置、前記ブレード８の位置は既知であるので、該ブレード８の絶対座標位置も測定できる。

又、前記ＧＰＳ装置３２の測定結果から、前記ブルドーザ本体１４の機械中心の座標位置を求めることができるので、前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅの画像処理により求められた、前記ブルドーザ本体１４との位置関係に基づき、前記ブレード８の位置を前記ブルドーザ本体１４の位置座標と同じ座標系で求めることが可能となる。従って、前記ブルドーザ７の前記ブレード８の位置を、グローバルな測地座標系上の座標で扱うことが可能となる。

前記画像処理部２９は、ターゲット探索部３３、ターゲット位置検出部３４等を有し、該ターゲット位置検出部３４は、前記カメラ１８，１９で取得したそれぞれのカメラ毎の画像データについて１フレーム毎の画像からブレード検出用ターゲット２１を検出し、又フレーム間でブレード検出用ターゲット２１の追尾処理を行い、又ブレード検出用ターゲット２１を検出し、前記カメラ１８の画像と前記カメラ１９の画像間でブレード検出用ターゲット２１のマッチングを行い、又検出したブレード検出用ターゲット２１の中心位置を検出する。

前記記憶部２８には、前記ブルドーザ本体１４の機械中心に対する前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅの基準位置データ、更に前記機械中心に対する前記カメラ１８，１９の位置データ等のデータが格納される。又、ブレード検出用ターゲット２１の位置情報から、前記ブレード８の前記ブルドーザ本体１４に対する高さ、傾きから得られる前記ブレード８の姿勢を演算するブレード位置、姿勢演算プログラム、又演算されたブレード８の位置、姿勢に基づき該ブレード８の位置、姿勢を制御する姿勢制御プログラム等のプログラムが格納されている。

本発明では、前記カメラ１８，１９で取得した２つの画像から、前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅの位置を測定し、得られたブレード検出用ターゲット２１の位置情報から前記ブレード８の前記ブルドーザ本体１４に対する相対的な高さ、相対的な傾き、姿勢を演算により求めるものである。

図３を参照して、本発明の作用について説明する。

作業に入る前に標定作業を行う。標定作業は２つのカメラとターゲットの位置を関係づける作業であり、２つのカメラから撮像した、一対の画像を用い、両画像中で相互に対応付られたターゲット像を用いて標定を行う。

ＳＴＥＰ：０１前記照明装置２２を点灯し、前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅに照射する。尚、昼間で該ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅから充分な反射光が得られる場合は、前記照明装置２２は点灯する必要がない。

前記ブレード８を基準位置に設定し、前記カメラ１８，１９により前記ブレード８を撮像する。取得した画像中には前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅが含まれる。取得した画像は、デジタル画像データとして前記記憶部２８に格納される。

ＳＴＥＰ：０２前記画像処理部２９に於いて、前記記憶部２８に格納された画像について、画像中からターゲット像を抽出する。ターゲット像の抽出は、予め取得したターゲット像のテンプレート取得画像とのテンプレートマッチングにより行うか、或は夜間等ブレード検出用ターゲット２１から反射光が背景より際だっている場合は画像データを２値化し、ブレード検出用ターゲット２１を抽出する。又、前記ブレード８にブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅを設けているので、ブレード検出用ターゲット２１自体が特徴点となり、画像処理により特徴点を抽出する必要がない。又、ブレード検出用ターゲット２１から突出した反射光が得られるので、前記画像処理部２９による画像処理の負担が少なくなり、又低画質の画像でも容易に前記ブレード検出用ターゲット２１の抽出が可能となる。

尚、前記ブレード制御部３１により、前記ブレード８の位置、姿勢は油圧シリンダを通じて制御されている。従って、前記ブレード制御部３１の制御の状態に基づいて、前記カメラ１８，１９又は、機械中心に対する前記ブレード８の位置、姿勢を予測することが可能である。例えば、前記演算処理装置２３は、前記ブレード制御部３１から得られた制御信号に基づいて、前記ブレード８の位置、姿勢を推定し、前記カメラ１８，１９で取得される画像の内、ブレード検出用ターゲット２１が配列されているブレード上のラインを推測する。推定されたラインに沿ってブレード検出用ターゲット２１を検出する為の画像処理を行うことで、ターゲット検出のスピードを速めることができる。或は、前記ブレード８の位置、姿勢の予測から、ブレード検出用ターゲット２１の位置を予測し、予測した位置に基づいてブレード検出用ターゲット２１を検出することで、ターゲット検出のスピードを速めることが可能となる。

上記、テンプレートマッチングの方法としては、ＳＳＤＡ法（ｓｅｑｕｅｎｔｉａｌｓｉｍｉｌａｒｉｔｙｄｅｔｅｃｔｉｏｎａｌｇｏｒｉｔｈｍ：テンプレートとマッチングウィンドウとの濃淡値の残差を求め、残差が最小となる位置を求める）、或は正規化相互相関法（Ｃｒｏｓｓｃｏｒｒｅｌａｔｉｏｎ：テンプレートとマッチングウィンドウとの相関値を算出して対応点を検索する）、或は最小二乗マッチング法（Ｌｅａｓｔｓｑｕａｒｅｓｍａｔｃｈｉｎｇ：アフィン変換を用いてマッチングウィンドウの形状も未知パラメータとして設定し、テンプレートに対するマッチングウィンドウの位置及び形状を求める）等がある。

撮像した画像中の前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅの画像と、前記ブレード８に設けられた前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅとの関連付けは、画像上の前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅの相互位置、及び前記ブレード８の基準位置と前記カメラ１８，１９との既知の位置関係により特定できる。

前記カメラ１８と前記カメラ１９の両画像中のターゲット像の位置関係を比較し、ターゲット像で共通するものがあるかどうかが判断され、更に共通する位置にあるターゲット像が所定数、少なくとも３、例えばブレード検出用ターゲット２１ａ，２１ｂ，２１ｃが選択される。

ＳＴＥＰ：０３選択したターゲット像について、それぞれの中心を演算により求める。上記した様にターゲット即ち反射部は円形であるので、前記カメラ１８，１９の光軸に対してブレード検出用ターゲット２１が傾斜し、取得画像が楕円となっていても、中心は正確に検出できる。

画像中からターゲットの中心を検出する一例としては、モーメント法が用いられる。

モーメント法は、ターゲット像を２値化（０，１）し、設定した座標上で１を示す座標の各モーメントを求め、各モーメントの総和に基づきターゲットの中心座標を求める。

即ち、前記ターゲット像を含む領域をＳとし、又ターゲットを含む２値化処理した小画像を、

ｆ（ｉ，ｊ）＝｛０（ｉ，ｊ）∈（属さない）Ｓ；１（ｉ，ｊ）∈（属する）Ｓ｝
と表したとき、領域Ｓのモーメントは次式で定義される。
Ｍ_pq＝Σｉ^pｊ^qｆ（ｉ，ｊ）
この時、ターゲットの中心座標（ｘ，ｙ）は、次式によって算出できる。
ｘ＝Ｍ10／Ｍ00
ｙ＝Ｍ01／Ｍ00

上記モーメント法により中心座標を求める場合、１を示す信号が好ましくは１０、少なくとも５以上得られる様にする。即ち、１の信号を出力する受光素子の画素が少なくとも５以上となる様に、ブレード検出用ターゲット２１の大きさ、該ブレード検出用ターゲット２１と前記カメラ１８，１９間の距離を設定する。

所定数の画素について、モーメント法により中心座標を算出することで、位置精度が向上する。例えば、１０画素について、中心座標を算出した場合、１画素について座標位置を算出した場合に対して精度は１０倍に向上する。

ＳＴＥＰ：０４前記カメラ１８，１９の２つの画像（ステレオ画像）により各ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃの３次元座標が算出される。

図４に示される様に、ブレード検出用ターゲット２１の３次元座標をＰ（Ｘ，Ｙ，Ｚ）、前記カメラ１８の受光面３６、前記カメラ１９の受光面３７、前記カメラ１８の焦点位置Ｏ１、前記カメラ１９の焦点位置Ｏ２、前記カメラ１８，１９の焦点距離ｆ、焦点位置Ｏ１と焦点位置Ｏ２との距離Ｂ、前記受光面３６，３７上のターゲット中心位置ｐ１，ｐ２とすると、３次元座標Ｐ（Ｘ，Ｙ，Ｚ）の算出は、以下の式によって算出することができる。

Ｘ＝ｘ１×Ｂ／（ｘ１−ｘ２）
Ｙ＝ｙ１×Ｂ／（ｘ１−ｘ２）
Ｚ＝−ｆ×Ｂ／（ｘ１−ｘ２）
Ｂ：ステレオカメラの基線長、ｆ：カメラの焦点距離、
ターゲットの画像座標左座標：ｐ１（ｘ１、ｙ１）
右座標：ｐ２（ｘ２、ｙ２）

ＳＴＥＰ：０５前記制御演算部２７は、各ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃについて得られた３次元座標に基づき、前記ブレード８の前記ブルドーザ本体１４に対する相対位置、姿勢を演算し、演算結果に基づき前記ブレード制御部３１に制御信号を発して、該ブレード制御部３１を介して前記ブレード８の位置、姿勢を制御する。

ＳＴＥＰ：０６該ブレード８の位置、姿勢を連続的に制御する場合は、前記カメラ１８で経時的に得られた画像毎に前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅの抽出が行われると共に画像間でのトラッキングが実行される。同様に、前記カメラ１９で経時的に得られた画像についても、ターゲット像の抽出、トラッキングが行われる。

更に、抽出したターゲット像について、３次元座標が算出される。前記カメラ１８，１９がビデオカメラである場合は、前記カメラ１８，１９によりブレード検出用ターゲット２１を連続的に撮像し続け、連続性のあるブレード検出用ターゲット２１の画像を取得し、取得した動画像についてリアルタイムで画像処理を行う。連続して撮像しているので、前画像データと次画像データとのずれは僅かであり、同一のブレード検出用ターゲット２１は前画像データと、次画像データとで関連付けることができる。従って、ブレード検出用ターゲット２１それぞれを連続した画像に於いて順次（ＩＤ等の認識による識別を用いないで）特定すること、即ちトラッキングすることができる。（途中で消えた場合）前画像と、次画像のブレード検出用ターゲット２１の画像に整合性が無くなった場合は、画像データによるトラッキングを中止する。又は、明瞭に整合が取れる画像から順次に使用して、トラッキングを継続する。少なくとも３点のターゲット像が取得できればよい。

各フレーム画像毎に３次元座標の算出が実行される。従って、１秒間に取得される画像が３０フレームであった場合は、３０フレーム画像について３次元座標の算出が実行されるので、リアルタイムで前記ブレード８の３次元位置、姿勢が得られる。得られた位置、姿勢に基づきＳＴＥＰ：０１〜ＳＴＥＰ：０５が繰返し実行され、前記ブレード８のリアルタイムでの位置制御が可能となる。

尚、上記実施の形態では、本発明がブルドーザに実施された場合を示したが、他の建設機械、例えばパワーショベルに実施され、バケットの位置、姿勢を検出する様にしてもよい。

上記説明中、ＳＴＥＰ：０３に於いてターゲット中心を検出する方法として、モーメント法を説明したが、他の検出方法でも行うことができる。以下に他の検出方法を説明する。

照明装置２２を点灯、又は点滅させて、ブレード検出用ターゲット２１に照射し、該ブレード検出用ターゲット２１を強く光らせる。画像処理部２９は、画像中からブレード検出用ターゲット２１を抽出する際、反射光が明るい順に解析に用いる。ブレード検出用ターゲット２１を強く光らせ、ターゲット像の２値化処理からその中心位置を高精度に検出する様にする。又、ブレード検出用ターゲット２１は中心部（高反射部）の大きさが既知であり、２値化処理で検出した円形（ターゲット像）の大きさ（直径）を判別し、既知の値に対して所定の偏差以上の大きさである場合、そのブレード検出用ターゲット２１を除外する。ブレード検出用ターゲット２１の大きさは、ステレオカメラ２５とブレード検出用ターゲット２１との距離Ｈは予め行う標定により既知なので、ブレード検出用ターゲット２１の円形の大きさ：ΔＸＹを計算できる。
ΔＸＹ＝Ｈ×ΔＰ／ｆ ΔＰは画素サイズ、ｆは焦点距離

図５は本発明の応用例を示している。

ブルドーザ本体１４にＧＰＳ装置３２を設ける。該ＧＰＳ装置３２のアンテナに対して所定の位置となる様ブレード検出用ターゲット２１ｇを前記ブルドーザ本体１４に設ける。又、前記ＧＰＳ装置３２と前記ブレード検出用ターゲット２１ｇを一体的に設け、前記ＧＰＳ装置３２の測定位置と前記ブレード検出用ターゲット２１ｇの位置を略同一とみなすか、或は離して設置する場合は互いの位置関係を予め求めておく。尚、図５では前記ＧＰＳ装置３２のアンテナを前記ブレード検出用ターゲット２１ｇと一体に設けた場合を示している。又、前記ブレード検出用ターゲット２１ｇはカメラ１８，１９の視野内に配置される。

該カメラ１８，１９でブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅと前記ＧＰＳ装置３２の前記ブレード検出用ターゲット２１ｇのステレオ画像データを取得する。

前記ＧＰＳ装置３２により、前記ブレード検出用ターゲット２１ｇの位置は、ＧＰＳ信号の受信により求められた座標に基づき求めることが可能である。前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅと、ステレオ画像に写りこんだ前記ブレード検出用ターゲット２１ｇとの関係、及び該ブレード検出用ターゲット２１ｇの座標位置に基づき前記ブレード検出用ターゲット２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅのグローバルな測地座標系上の座標を演算することで、ブレード８の位置、姿勢を求めることが可能となり、該ブレード８の位置を基にした位置情報の管理、施工が可能となる。

本発明がブルドーザに実施された場合を示す斜視図である。本発明の姿勢検出装置の概略構成を示すブロック図である。本発明に於ける位置、姿勢検出の作動を示すフローチャートである。ステレオ画像による位置測定を示す説明図である。本発明の応用例の説明図である。従来例の整地器具の位置検出を示す説明図である。他の従来例のブレードの位置検出を示す斜視図である。

符号の説明

７ブルドーザ
８ブレード
１４ブルドーザ本体
１７姿勢検出装置
１８カメラ
１９カメラ
２１ブレード検出用ターゲット
２２照明装置
２３演算処理装置
２６同期制御部
２７制御演算部
２８記憶部
２９画像処理部
３１ブレード制御部

Claims

建設機本体と、該建設機本体に設けられた工事作業具と、前記建設機本体の機械中心に対して既知の位置に設けられた２つのカメラと、前記工事作業具に設けられ、前記２つのカメラの視野にそれぞれ含まれる様に配置された少なくとも３のターゲットと、前記２つのカメラで取得したステレオ画像から各画像中で共通する少なくとも３のターゲット像を抽出し、該各ターゲット像の３次元位置を求め、求めた３次元位置に基づき前記工事作業具の前記建設機本体に対する位置、姿勢を算出する演算処理装置とを具備することを特徴とする建設機械。
前記カメラはビデオカメラであり、前記演算処理装置は、各フレーム画像毎にターゲットの位置、姿勢を算出する様にした請求項１の建設機械。
前記ターゲットは円形の反射部、及び該反射部の周囲に非反射部が形成された板又は板状である請求項１の建設機械。
前記ターゲットの反射部が形成するターゲット像は、カメラ受光部の少なくとも５画素以上を占め、前記演算処理装置はターゲット像の中心をモーメント法により算出する請求項１の建設機械。
前記建設機本体に設けられ、該建設機本体の位置座標を求めるＧＰＳ装置を更に備え、前記演算処理装置は前記ＧＰＳ装置が測定した位置座標に基づき、前記工事作業具の位置座標を演算する請求項１の建設機械。
前記ＧＰＳ装置は、前記２つのカメラの視野に含まれる様配置されたターゲットを有し、前記演算処理装置は前記ＧＰＳ装置が測定した位置座標に基づき、前記工事作業具の位置座標を演算する請求項５の建設機械。