JP2018128397A - 位置計測システム、作業機械、及び位置計測方法 - Google Patents

Abstract

【課題】再較正処理を円滑に実施でき、作業効率の低下を抑制できることを目的とする。【解決手段】位置計測システムは、作業機械に設けられた第１ステレオカメラの第１カメラで撮影された第１画像データ及び第１ステレオカメラの第２カメラで撮影された第２画像データを取得する画像データ取得部と、第１画像データと、第２画像データと、第１カメラ及び第２カメラに関するパラメータとに基づいて、ステレオ計測を実施するステレオ計測部と、パラメータの少なくとも一部を変更して、ステレオ計測された第１視差画像データのステレオ率を調整する第１調整部と、パラメータの少なくとも一部を変更して、第１視差画像データから求められる第１の３次元データのスケールを調整する第２調整部と、を備える。【選択図】図４

Description

本発明は、位置計測システム、作業機械、及び位置計測方法に関する。

作業機械に係る技術分野において、特許文献１に開示されているような、ステレオカメラを有する油圧ショベルが知られている。

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

ステレオカメラは、第１カメラ及び第２カメラを有し、三角測量の原理に基づいて３次元計測する。ステレオカメラにおいては、第１カメラと第２カメラとの相対位置を調整する較正処理が実施される。ステレオカメラの較正処理は、第１カメラ及び第２カメラでキャリブレーションターゲットを撮影する処理を含む。ステレオカメラの較正処理後、第１カメラと第２カメラとの相対位置が変動してしまった場合、ステレオカメラを再較正処理する必要が生じる。再較正処理の度にキャリブレーションターゲットを撮影しなければならない場合、再較正処理の作業が煩雑となり、作業機械の作業効率が低下する。

本発明の態様は、再較正処理を円滑に実施して、作業効率の低下を抑制することを目的とする。

本発明の態様に従えば、作業機械に設けられた第１ステレオカメラの第１カメラで撮影された第１画像データ及び前記第１ステレオカメラの第２カメラで撮影された第２画像データを取得する画像データ取得部と、前記第１画像データと、前記第２画像データと、前記第１カメラ及び前記第２カメラに関するパラメータとに基づいて、ステレオ計測を実施するステレオ計測部と、前記パラメータの少なくとも一部を変更して、ステレオ計測された第１視差画像データのステレオ率を調整する第１調整部と、前記パラメータの少なくとも一部を変更して、前記第１視差画像データから求められる第１の３次元データのスケールを調整する第２調整部と、を備える位置計測システムが提供される。

本発明の態様によれば、再較正処理を円滑に実施でき、作業効率の低下を抑制できる。

図１は、第１実施形態に係る作業機械の一例を示す斜視図である。 図２は、第１実施形態に係るステレオカメラの一例を示す斜視図である。 図３は、第１実施形態に係る検出システムの一例を模式的に示す図である。 図４は、第１実施形態に係る位置計測システムの一例を示す機能ブロック図である。 図５は、第１実施形態に係るステレオ計測方法の一例を説明するための模式図である。 図６は、第１実施形態に係るステレオカメラの一例を模式的に示す図である。 図７は、第１実施形態に係る第１カメラによって撮影された第１画像データ及び第２カメラによって撮影された第２画像データの一例を模式的に示す図である。 図８は、第１実施形態に係る第１カメラによって撮影された第１画像データ及び第２カメラによって撮影された第２画像データの一例を模式的に示す図である。 図９は、第１実施形態に係る第１カメラと第２カメラとの相対位置を説明するための模式図である。 図１０は、第１実施形態に係る探索失敗したときの視差画像データの一例を模式的に示す図である。 図１１は、第１実施形態に係る第１調整部によるステレオ率の調整処理の一例を説明するための模式図である。 図１２は、第１実施形態に係る第１調整部によるステレオ率の調整方法の一例を説明するための模式図である。 図１３は、第１実施形態に係る探索成功したときの視差画像データの一例を模式的に示す図である。 図１４は、第１実施形態に係る第２調整部によるスケールの調整処理の一例を説明するための模式図である。 図１５は、第１実施形態に係る第２調整部によるスケールの調整方法の一例を説明するための模式図である。 図１６は、第１実施形態に係る位置計測方法の一例を示すフローチャートである。 図１７は、第１実施形態に係る撮影対象の一例を模式的に示す図である。 図１８は、第１実施形態に係る第１ステレオカメラで取得された第１画像データ及び第２画像データに基づいて生成された視差画像データの一例を模式的に示す図である。 図１９は、第１実施形態に係る第１視差画像データ及び第２視差画像データの一例を示す図である。 図２０は、第２実施形態に係る位置計測方法の一例を説明するための模式図である。 図２１は、第３実施形態に係る位置計測方法の一例を説明するための模式図である。 図２２は、第３実施形態に係る第２調整部によるスケールの調整方法の一例を説明するための模式図である。 図２３は、第５実施形態に係る位置計測システムの一例を模式的に示す図である。 図２４は、第６実施形態に係る位置計測方法の一例を説明するための模式図である。

以下、本発明に係る実施形態について図面を参照しながら説明するが、本発明はこれに限定されない。以下で説明する実施形態の構成要素は適宜組み合わせることができる。また、一部の構成要素を用いない場合もある。

以下の説明においては、３次元のグローバル座標系（Ｘｇ，Ｙｇ，Ｚｇ）、３次元の車体座標系（Ｘｍ，Ｙｍ，Ｚｍ）、及び３次元のカメラ座標系（Ｘｓ，Ｙｓ，Ｚｓ）を規定して、各部の位置関係について説明する。

グローバル座標系は、地球に固定された原点を基準とする座標系である。グローバル座標系は、ＧＮＳＳ（Global Navigation Satellite System）によって規定される座標系である。ＧＮＳＳとは、全地球航法衛星システムをいう。全地球航法衛星システムの一例として、ＧＰＳ（Global Positioning System）が挙げられる。

グローバル座標系は、水平面のＸｇ軸と、Ｘｇ軸と直交する水平面のＹｇ軸と、Ｘｇ軸及びＹｇ軸と直交するＺｇ軸とによって規定される。Ｘｇ軸を中心とする回転又は傾斜方向をθＸｇ方向とし、Ｙｇ軸を中心とする回転又は傾斜方向をθＹｇ方向とし、Ｚｇ軸を中心とする回転又は傾斜方向をθＺｇ方向とする。Ｚｇ軸方向は鉛直方向である。

車体座標系は、作業機械の車体に規定された原点を基準とする第１所定面のＸｍ軸と、Ｘｍ軸と直交する第１所定面のＹｍ軸と、Ｘｍ軸及びＹｍ軸と直交するＺｍ軸とによって規定される。Ｘｍ軸を中心とする回転又は傾斜方向をθＸｍ方向とし、Ｙｍ軸を中心とする回転又は傾斜方向をθＹｍ方向とし、Ｚｍ軸を中心とする回転又は傾斜方向をθＺｍ方向とする。Ｘｍ軸方向は作業機械の前後方向であり、Ｙｍ軸方向は作業機械の車幅方向であり、Ｚｍ軸方向は作業機械の上下方向である。

カメラ座標系は、カメラに規定された原点を基準とする第２所定面のＸｓ軸と、Ｘｓ軸と直交する第２所定面のＹｓ軸と、Ｘｓ軸及びＹｓ軸と直交するＺｓ軸とによって規定される。Ｘｓ軸を中心とする回転又は傾斜方向をθＸｓ方向とし、Ｙｓ軸を中心とする回転又は傾斜方向をθＹｓ方向とし、Ｚｓ軸を中心とする回転又は傾斜方向をθＺｓ方向とする。Ｘｓ軸方向はカメラの上下方向であり、Ｙｓ軸方向はカメラの幅方向であり、Ｚｓ軸方向はカメラの前後方向である。Ｚｓ軸方向はカメラの光学系の光軸と平行である。

第１実施形態．
［作業機械］
図１は、本実施形態に係る作業機械１の一例を示す斜視図である。本実施形態においては、作業機械１が油圧ショベルである例について説明する。以下の説明においては、作業機械１を適宜、油圧ショベル１、と称する。

図１に示すように、油圧ショベル１は、車体１Ｂと、作業機２とを有する。車体１Ｂは、旋回体３と、旋回体３を旋回可能に支持する走行体５とを有する。

旋回体３は、運転室４を有する。油圧ポンプ及び内燃機関が旋回体３に配置される。旋回体３は、旋回軸Ｚｒを中心に旋回可能である。旋回軸Ｚｒは、車体座標系のＺｍ軸と平行である。本実施形態において、車体座標系の原点は、旋回体３のスイングサークルの中心に規定される。スイングサークルの中心は、旋回体３の旋回軸Ｚｒに位置する。

走行体５は、履帯５Ａ，５Ｂを有する。履帯５Ａ，５Ｂが回転することにより、油圧ショベル１が走行する。本実施形態において、車体座標系のＺｍ軸は、履帯５Ａ，５Ｂの接地面と直交する。車体座標系の上方（＋Ｚｍ方向）は、履帯５Ａ，５Ｂの接地面から離れる方向であり、車体座標系の下方（−Ｚｍ方向）は、車体座標系の上方とは反対の方向である。

作業機２は、旋回体３に連結される。車体座標系において、作業機２の少なくとも一部は、旋回体３よりも前方に配置される。車体座標系の前方（＋Ｘｍ方向）は、旋回体３を基準として作業機２が存在する方向であり、車体座標系の後方（−Ｘｍ方向）は、車体座標系の前方とは反対の方向である。

作業機２は、旋回体３に連結されるブーム６と、ブーム６に連結されるアーム７と、アーム７に連結されるバケット８と、ブーム６を駆動するブームシリンダ１０と、アーム７を駆動するアームシリンダ１１と、バケット８を駆動するバケットシリンダ１２とを有する。ブームシリンダ１０、アームシリンダ１１、及びバケットシリンダ１２はそれぞれ、油圧によって駆動される油圧シリンダである。

ブーム６は、ブームピン１３を介して旋回体３に回転可能に連結される。アーム７は、アームピン１４を介してブーム６の先端部に回転可能に連結される。バケット８は、バケットピン１５を介してアーム７の先端部に回転可能に連結される。ブームピン１３は、旋回体３に対するブーム６の回転軸ＡＸ１を含む。アームピン１４は、ブーム６に対するアーム７の回転軸ＡＸ２を含む。バケットピン１５は、アーム７に対するバケット８の回転軸ＡＸ３を含む。ブーム６の回転軸ＡＸ１、アーム７の回転軸ＡＸ２、及びバケット８の回転軸ＡＸ３は、車体座標系のＹｍ軸と平行である。

［ステレオカメラ］
次に、本実施形態に係るステレオカメラ３００について説明する。図２は、本実施形態に係るステレオカメラ３００の一例を示す斜視図である。図２に示すように、油圧ショベル１は、ステレオカメラ３００を有する。ステレオカメラ３００とは、撮影対象ＳＢを複数の異なる方向から同時に撮影することにより、撮影対象ＳＢの奥行き方向のデータも取得可能なカメラをいう。

本実施形態において、撮影対象ＳＢは、施工現場において施工される施工対象を含む。施工対象は、油圧ショベル１の作業機２で掘削される掘削対象を含む。なお、施工対象は、油圧ショベル１とは別の作業機械によって施工される施工対象でもよいし、作業者によって施工される施工対象でもよい。また、施工対象は、施工前の施工対象、施工中の施工対象、及び施工後の施工対象を含む概念である。

ステレオカメラ３００は、旋回体３に設けられる。本実施形態において、ステレオカメラ３００は、運転室４の内側に設けられる。ステレオカメラ３００は、例えば運転室４の前方（＋Ｘｍ方向）かつ上方（＋Ｚｍ方向）に配置される。ステレオカメラ３００は、油圧ショベル１の前方の撮影対象ＳＢを撮影する。

ステレオカメラ３００は、複数のカメラ３０を有する。カメラ３０は、光学系と、イメージセンサとを有する。イメージセンサは、ＣＣＤ（Couple Charged Device）イメージセンサ又はＣＭＯＳ（Complementary Metal Oxide Semiconductor）イメージセンサを含む。本実施形態において、カメラ３０は、４つのカメラ３０Ａ，３０Ｂ，３０Ｃ，３０Ｄを含む。なお、ステレオカメラ３００は、４つのカメラ３０を有さなくてもよく、少なくとも一対のカメラ３０を有すればよい。

複数のカメラ３０のうち２つのカメラ３０の組合せによりステレオカメラ３００が構成される。本実施形態において、ステレオカメラ３００は、カメラ３０Ａ，３０Ｂの組合せにより構成される第１ステレオカメラ３０１と、カメラ３０Ｃ，３０Ｄの組合せにより構成される第２ステレオカメラ３０２とを含む。

カメラ３０Ａ，３０Ｃは、カメラ３０Ｂ，３０Ｄよりも＋Ｙｍ側（作業機２側）に配置される。カメラ３０Ａとカメラ３０Ｂとは、Ｙｍ軸方向に間隔をあけて配置される。カメラ３０Ｃとカメラ３０Ｄとは、Ｙｍ軸方向に間隔をあけて配置される。カメラ３０Ａ，３０Ｂは、カメラ３０Ｃ，３０Ｄよりも＋Ｚｍ側に配置される。Ｚｍ軸方向において、カメラ３０Ａとカメラ３０Ｂとは、実質的に同一の位置に配置される。Ｚｍ軸方向において、カメラ３０Ｃとカメラ３０Ｄとは、実質的に同一の位置に配置される。

本実施形態において、カメラ３０Ａ，３０Ｂは、上方（＋Ｚｍ方向）を向く。カメラ３０Ｃ，３０Ｄは、下方（−Ｚｍ方向）を向く。また、カメラ３０Ａ，３０Ｃは、前方（＋Ｘｍ方向）を向く。カメラ３０Ｂ，３０Ｄは、前方よりも僅かに＋Ｙｍ側（作業機２側）を向く。すなわち、カメラ３０Ａ，３０Ｃは、旋回体３の正面を向き、カメラ３０Ｂ，３０Ｄは、カメラ３０Ａ，３０Ｃ側を向く。なお、カメラ３０Ｂ，３０Ｄが旋回体３の正面を向き、カメラ３０Ａ，３０Ｃがカメラ３０Ｂ，３０Ｄ側を向いてもよい。

カメラ３０は、旋回体３の前方に存在する撮影対象ＳＢをステレオ撮影する。一対のカメラ３０によるステレオ画像データを用いて撮影対象ＳＢが３次元計測され、撮影対象ＳＢの３次元データＤＧが算出される。撮影対象ＳＢの３次元データＤＧは、施工対象の表面である地表の３次元データＤＧである。撮影対象ＳＢの３次元データＤＧは、グローバル座標系における撮影対象ＳＢの３次元形状データを含む。

複数のカメラ３０のそれぞれについてカメラ座標系が規定される。カメラ座標系は、カメラ３０に固定された原点を基準とする座標系である。カメラ座標系のＺｓ軸は、カメラ３０の光学系の光軸と一致する。

以下の説明においては、第１ステレオカメラ３０１のうち一方のカメラ３０Ａを適宜、第１カメラ３０Ａ、と称し、他方のカメラ３０Ｂを適宜、第２カメラ３０Ｂ、と称する。また、第２ステレオカメラ３０２のうち一方のカメラ３０Ｃを適宜、第３カメラ３０Ｃ、と称し、他方のカメラ３０Ｄを適宜、第４カメラ３０Ｄ、と称する。

図２に示すように、運転席４Ｓ及び操作装置３５が運転室４に配置される。操作装置３５は、作業機２及び旋回体３の操作のために運転者に操作される。操作装置３５は、右操作レバー３５Ｒ及び左操作レバー３５Ｌを含む。運転室４に搭乗した運転者は、操作装置３５を操作して、作業機２の駆動及び旋回体３の旋回を実施する。

［検出システム］
次に、本実施形態に係る油圧ショベル１の検出システム２０について説明する。図３は、本実施形態に係る検出システム２０の一例を模式的に示す図である。図３に示すように、検出システム２０は、作業機２の角度を検出する作業機角度検出器２２と、旋回体３の位置を検出する位置検出器２３と、旋回体３の姿勢を検出する姿勢検出器２４と、旋回体３の方位を検出する方位検出器２５とを有する。

位置検出器２３は、ＧＰＳ受信機を含む。位置検出器２３は、旋回体３に設けられる。位置検出器２３は、グローバル座標系で規定される旋回体３の位置である絶対位置を検出する。旋回体３の絶対位置は、Ｘｇ軸方向の座標データ、Ｙｇ軸方向の座標データ、及びＺｇ軸方向の座標データを含む。

ＧＰＳアンテナ２１が旋回体３に設けられる。ＧＰＳアンテナ２１は、例えば車体座標系のＹｍ軸方向に２つ配置される。ＧＰＳアンテナ２１は、ＧＰＳ衛星から電波を受信して、受信した電波に基づいて生成した信号を位置検出器２３に出力する。位置検出器２３は、ＧＰＳアンテナ２１から供給された信号に基づいて、グローバル座標系で規定されるＧＰＳアンテナ２１の位置である絶対位置を検出する。

位置検出器２３は、２つのＧＰＳアンテナ２１の絶対位置の少なくとも一方に基づいて演算処理を実施して、旋回体３の絶対位置を算出する。旋回体３の絶対位置は、一方のＧＰＳアンテナ２１の絶対位置でもよいし、一方のＧＰＳアンテナ２１の絶対位置と他方のＧＰＳアンテナ２１の絶対位置との間の位置でもよい。

姿勢検出器２４は、慣性計測装置（ＩＭＵ：Inertial Measurement Unit）を含む。姿勢検出器２４は、旋回体３に設けられる。姿勢検出器２４は、グローバル座標系で規定される水平面（ＸｇＹｇ平面）に対する旋回体３の傾斜角度を算出する。水平面に対する旋回体３の傾斜角度は、Ｙｍ軸を中心とする回転方向における旋回体３の傾斜角度と、Ｘｍ軸を中心とする回転方向における旋回体３の傾斜角度とを含む。

姿勢検出器２４は、姿勢検出器２４に作用する加速度及び角速度を検出する。姿勢検出器２４に作用する加速度及び角速度が検出されることにより、旋回体３に作用する加速度及び角速度が検出される。旋回体３に作用する加速度及び角速度に基づいて、旋回体３の姿勢が導出される。

方位検出器２５は、一方のＧＰＳアンテナ２１の絶対位置と他方のＧＰＳアンテナ２１の絶対位置とに基づいて、グローバル座標系で規定される基準方位に対する旋回体３の方位を算出する。基準方位は、例えば北である。方位検出器２５は、一方のＧＰＳアンテナ２１の絶対位置と他方のＧＰＳアンテナ２１の絶対位置とを結ぶ直線を算出し、算出した直線と基準方位とがなす角度に基づいて、基準方位に対する旋回体３の方位を算出する。基準方位に対する旋回体３の方位は、基準方位と旋回体３の方位とがなす角度を示す方位角を含む。

作業機２は、ブームシリンダ１０に配置されブームシリンダ１０の駆動量を示すブームストロークを検出するブームストロークセンサ１６と、アームシリンダ１１に配置されアームシリンダ１１の駆動量を示すアームストロークを検出するアームストロークセンサ１７と、バケットシリンダ１２に配置されバケットシリンダ１２の駆動量を示すバケットストロークを検出するバケットストロークセンサ１８とを有する。

作業機角度検出器２２は、ブーム６の角度、アーム７の角度、及びバケット８の角度を検出する。作業機角度検出器２２は、ブームストロークセンサ１６で検出されたブームストロークに基づいて、車体座標系のＺｍ軸に対するブーム６の傾斜角度を示すブーム角度を算出する。作業機角度検出器２２は、アームストロークセンサ１７で検出されたアームストロークに基づいて、ブーム６に対するアーム７の傾斜角度を示すアーム角度を算出する。作業機角度検出器２２は、バケットストロークセンサ１８で検出されたバケットストロークに基づいて、アーム７に対するバケット８の刃先８ＢＴの傾斜角度を示すバケット角度を算出する。

なお、ブーム角度、アーム角度、及びバケット角度は、ストロークセンサを用いずに、例えば、作業機２に設けられた角度センサにより検出されてもよい。

［位置計測システム］
次に、本実施形態に係る位置計測システム５０について説明する。図４は、本実施形態に係る位置計測システム５０の一例を示す機能ブロック図である。本実施形態において、位置計測システム５０は、油圧ショベル１に設けられる。

位置計測システム５０は、第１ステレオカメラ３０１及び第２ステレオカメラ３０２を含むステレオカメラ３００と、作業機角度検出器２２と、位置検出器２３と、姿勢検出器２４と、方位検出器２５と、表示装置２６と、制御装置１００とを備える。制御装置１００は、油圧ショベル１の旋回体３に設けられる。

表示装置２６は、液晶ディスプレイ（ＬＣＤ：Liquid Crystal Display）又は有機ＥＬディスプレイ（ＯＥＬＤ：Organic Electroluminescence Display）のようなフラットパネルディスプレイを含む。

制御装置１００は、コンピュータシステムを含む。制御装置１００は、ＣＰＵ（Central Processing Unit）のようなプロセッサを含む演算処理装置と、ＲＡＭ（Random Access Memory）のような揮発性メモリ及びＲＯＭ（Read Only Memory）のような不揮発性メモリを含む記憶装置と、入出力インターフェースとを有する。

制御装置１００は、画像データ取得部１０１と、ステレオ計測部１０２と、第１調整部１０３と、第２調整部１０４と、位置データ算出部１０５と、作業機位置データ算出部１０６と、マップデータ作成部１０７と、表示制御部１０８と、記憶部１０９と、入出力部１１０とを有する。

演算処理装置は、画像データ取得部１０１、ステレオ計測部１０２、第１調整部１０３、第２調整部１０４、位置データ算出部１０５、作業機位置データ算出部１０６、マップデータ作成部１０７、及び表示制御部１０８それぞれの機能を有する。記憶装置は、記憶部１０９の機能を有する。入出力インターフェースは、入出力部１１０の機能を有する。

ステレオカメラ３００、作業機角度検出器２２、位置検出器２３、姿勢検出器２４、方位検出器２５、及び表示装置２６は、入出力部１１０と接続される。画像データ取得部１０１と、ステレオ計測部１０２と、第１調整部１０３と、第２調整部１０４と、位置データ算出部１０５と、作業機位置データ算出部１０６と、マップデータ作成部１０７と、表示制御部１０８と、記憶部１０９と、ステレオカメラ３００と、作業機角度検出器２２と、位置検出器２３と、姿勢検出器２４と、方位検出器２５と、表示装置２６とは、入出力部１１０を介してデータ通信可能である。

画像データ取得部１０１は、油圧ショベル１に設けられた第１カメラ３０Ａで撮影された撮影対象ＳＢの第１画像データＭＲ１、第２カメラ３０Ｂで撮影された撮影対象ＳＢの第２画像データＭＬ１、第３カメラ３０Ｃで撮影された撮影対象の第３画像データＭＲ２、及び第４カメラ３０Ｄで撮影された撮影対象の第４画像データＭＬ２を取得する。すなわち、画像データ取得部１０１は、ステレオカメラ３００の少なくとも一対のカメラ３０で撮影されたステレオ画像データを取得する。

記憶部１０９は、第１ステレオカメラ３０１の第１カメラ３０Ａ及び第２カメラ３０Ｂに関する複数のパラメータ、及び第２ステレオカメラ３０２の第３カメラ３０Ｃ及び第４カメラ３０Ｄに関する複数のパラメータを記憶する。パラメータは、所定の較正作業によって求めることができる。

パラメータは、第１ステレオカメラ３０１の第１カメラ３０Ａと第２カメラ３０Ｂとの相対位置を規定する複数の外部パラメータ、及び第２ステレオカメラ３０２の第３カメラ３０Ｃと第４カメラ３０Ｄとの相対位置を規定する複数の外部パラメータを含む。

外部パラメータは、Ｘｓ軸方向、Ｙｓ軸方向、Ｚｓ軸方向、θＸｓ軸方向、θＹｓ軸方向、及びθＺｓ軸方向の６つの方向に関するステレオカメラ３００の一対のカメラ３０の相対位置を示すパラメータを含む。

また、パラメータは、第１カメラ３０Ａ、第２カメラ３０Ｂ、第３カメラ３０Ｃ、及び第４カメラ３０Ｄそれぞれの内部パラメータを含む。内部パラメータは、複数のカメラ３０それぞれの固有データを規定する。内部パラメータは、例えば、カメラ３０の光学系の焦点距離、及びカメラ３０の光学系の光軸とイメージセンサの撮像面との交点とイメージセンサにおける画像中心との位置ずれ量を含む。

ステレオ計測部１０２は、画像データ取得部１０１で取得された第１画像データＭＲ１と第２画像データＭＬ１とをステレオ方式で画像処理して、カメラ座標系における撮影対象ＳＢの３次元データＤＧを算出する。また、ステレオ計測部１０２は、画像データ取得部１０１で取得された第３画像データＭＲ２と第４画像データＭＬ２とをステレオ方式で画像処理して、カメラ座標系における撮影対象ＳＢの３次元データＤＧを算出する。

また、ステレオ計測部１０２は、カメラ座標系における撮影対象ＳＢの３次元データＤＧを座標変換して、車体座標系における撮影対象ＳＢの３次元データＤＧを算出する。また、ステレオ計測部１０２は、車体座標系における撮影対象ＳＢの３次元データＤＧを座標変換して、グローバル座標系における撮影対象ＳＢの３次元データＤＧを算出する。

撮影対象ＳＢの３次元データＤＧは、撮影対象ＳＢ内の複数の点における座標データを含む。ステレオ計測部１０２は、異なる２つのカメラ３０で撮影された撮影対象ＳＢの２つの画像データＭＲ（ＭＲ１，ＭＲ２），ＭＬ（ＭＬ１，ＭＬ２）をステレオ方式で画像処理して視差画像データＳＧを生成して、演算処理によって３次元データＤＧを求める。

本実施形態において、ステレオ計測部１０２は、画像データ取得部１０１で取得された第１画像データＭＲ１と、画像データ取得部１０１で取得された第２画像データＭＬ１と、記憶部１０９に記憶されているパラメータとに基づいて、ステレオ計測を実施する。また、ステレオ計測部１０２は、画像データ取得部１０１で取得された第３画像データＭＲ２と、画像データ取得部１０１で取得された第４画像データＭＬ２と、記憶部１０９に記憶されているパラメータとに基づいて、ステレオ計測を実施する。

第１調整部１０３は、記憶部１０９に記憶されている複数のパラメータの少なくとも一部を変更して、ステレオ率を調整する。ステレオ率は、ステレオ計測される視差画像データＳＧの複数の画素のうちステレオ計測が成功した画素の割合を示す。本実施形態において、第１調整部１０３は、例えばステレオカメラ３００の一方のカメラ３０の位置がずれてステレオ率が低下してしまった場合、ステレオ率が向上するように、記憶部１０９に記憶されているパラメータを変更する。

第２調整部１０４は、記憶部１０９に記憶されている複数のパラメータの少なくとも一部を変更して、ステレオ計測部１０２で生成された３次元データＤＧのスケールを調整する。本実施形態において、第２調整部１０４は、基準スケールと３次元データＤＧのスケールとの差が小さくなるように、記憶部１０９に記憶されているパラメータを変更する。

位置データ算出部１０５は、旋回体３の位置データと、旋回体３の姿勢データと、旋回体３の方位データと、車体座標系におけるカメラ３０の位置データとに基づいて、グローバル座標系におけるカメラ３０の位置データを算出する。

位置データ算出部１０５は、位置検出器２３から旋回体３の位置データを取得し、姿勢検出器２４から旋回体３の姿勢データを取得し、方位検出器２５から旋回体３の方位データを取得する。旋回体３の位置データは、グローバル座標系における旋回体３の絶対位置を示す。旋回体３の姿勢データは、グローバル座標系における旋回体３の姿勢を示す。旋回体３の方位データは、グローバル座標系における旋回体３の方位を示す。

記憶部１０９は、車体座標系におけるカメラ３０の位置データを記憶する。車体座標系におけるカメラ３０の位置データは、油圧ショベル１及びカメラ３０の設計データ又は諸元データから導出される既知データであり、記憶部１０９に記憶される。位置データ算出部１０５は、旋回体３の位置データと、旋回体３の姿勢データと、旋回体３の方位データとに基づいて、グローバル座標系における車体座標系の原点の位置を算出する。位置データ算出部１０５は、旋回体３の位置データと、旋回体３の姿勢データと、旋回体３の方位データと、記憶部１０９に記憶されている車体座標系におけるカメラ３０の位置データとに基づいて、グローバル座標系におけるカメラ３０の位置データを算出する。

作業機位置データ算出部１０６は、作業機２の角度を示す作業機角度データを作業機角度検出器２２から取得する。作業機角度データは、ブーム角度、アーム角度、及びバケット角度を含む。作業機位置データ算出部１０６は、作業機角度検出器２２から取得した作業機角度データと、記憶部１０９に記憶されている作業機データとに基づいて、車体座標系におけるブーム６の位置データ、アーム７の位置データ、及びバケット８の位置データを算出する。ブーム６、アーム７、及びバケット８のそれぞれの位置データは、ブーム６の複数の部位、アーム７の複数の部位、及びバケット８の複数の部位のそれぞれの座標データを含む。

また、作業機位置データ算出部１０６は、旋回体３の位置データと、旋回体３の姿勢データと、旋回体３の方位データと、作業機角度データと、記憶部１０９に記憶されている作業機データとに基づいて、グローバル座標系におけるブーム６、アーム７、及びバケット８のそれぞれの位置データを算出する。

作業機データは、作業機２の設計データ又は諸元データを含む。作業機２の設計データは、作業機２の３次元ＣＡＤデータを含む。作業機データは、作業機２の外形データ及び作業機２の寸法データの少なくとも一方を含む。作業機データは、ブーム長さ、アーム長さ、及びバケット長さを含む。ブーム長さは、回転軸ＡＸ１と回転軸ＡＸ２との距離である。アーム長さは、回転軸ＡＸ２と回転軸ＡＸ３との距離である。バケット長さは、回転軸ＡＸ３とバケット８の刃先８ＢＴとの距離である。

マップデータ作成部１０７は、視差画像データＳＧに基づいて、３次元データＤＧを作成する。３次元データＤＧは、視差画像データＳＧ、車体座標系等の３次元データ、及び後述する３次元マップデータ（エレベーションマップデータ）を含む。

表示制御部１０８は、画像データ取得部１０１で取得された第１画像データＭＲ１、第２画像データＭＬ１、第３画像データＭＲ２、及び第４画像データＭＬ２を表示装置２６に表示させる。また、表示制御部１０８は、ステレオ計測部１０２で生成された視差画像データＳＧを表示装置２６に表示させる。また、表示制御部１０８は、マップデータ作成部１０７で作成された３次元データＤＧを表示装置２６に表示させる。

［ステレオ計測］
次に、ステレオ計測について説明する。図５は、本実施形態に係るステレオ計測方法の一例を説明するための模式図である。ステレオ計測は、画像データ取得部１０１で取得された画像データＭＲ（ＭＲ１，ＭＲ２）と画像データＭＬ（ＭＬ１，ＭＬ２）とをステレオ方式で画像処理して、撮影対象の３次元データＤＧを算出する処理である。

以下の説明においては、ステレオカメラ３００を構成する一方のカメラ３０を適宜、第１カメラ３０Ｒ（３０Ａ，３０Ｃ）と称し、他方のカメラ３０を適宜、第２カメラ３０Ｌ（３０Ｂ，３０Ｄ）と称する。また、以下の説明においては、第１カメラ３０Ｒ（３０Ａ，３０Ｃ）で撮影された画像データＭＲを適宜、第１画像データＭＲ（ＭＲ１，ＭＲ２）、と称し、第２カメラ３０Ｌ（３０Ｂ，３０Ｄ）で撮影された画像データＭＬを適宜、第２画像データ（ＭＬ１，ＭＬ２）、と称する。第１ステレオカメラ３０１の第１カメラ３０Ａ及び第２カメラ３０Ｂによって３次元データＤＧが算出される方法と、第２ステレオカメラ３０２の第３カメラ３０Ｃ及び第４カメラ３０Ｄによって３次元データＤＧが算出される方法とは、同様である。

なお、本実施形態においては、右側のカメラ３０が第１カメラ３０Ｒであり、左側のカメラ３０が第２カメラ３０Ｌであるが、左側のカメラ３０が第１カメラ３０Ｒであり、右側のカメラ３０が第２カメラ３０Ｌでもよい。

第１カメラ３０Ｒの位置データ及び第２カメラ３０Ｌの位置データが記憶部１０９に記憶されている。第１カメラ３０Ｒの位置データは、第１カメラ３０Ｒの光学中心ＯＲの位置及び第１カメラ３０Ｒの光学系の光軸の向きを含む。第２カメラ３０Ｌの位置データは、第２カメラ３０Ｌの光学中心ＯＬの位置及び第２カメラ３０Ｌの光学系の光軸の向きを含む。

また、第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置データが記憶部１０９に記憶されている。第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置データは、第１カメラ３０Ｒの光学中心ＯＲと第２カメラ３０Ｌの光学中心ＯＬとを結ぶベースラインＢＬの寸法を含む。

図５において、３次元空間に存在する撮影対象ＳＢの計測点Ｐの像が、第１カメラ３０Ｒの投影面及び第２カメラ３０Ｌの投影面のそれぞれに投影される。また、第１カメラ３０Ｒの投影面に計測点Ｐの像及び第２カメラ３０Ｌの投影面の点ＥＬの像が投影され、エピポーラ線が規定される。同様に、第２カメラ３０Ｌの投影面に計測点Ｐの像及び第１カメラ３０Ｒの投影面の点ＥＲの像が投影され、エピポーラ線が規定される。また、計測点Ｐと点ＥＲと点ＥＬとによりエピポーラ平面が規定される。

第１カメラ３０Ｒの投影面は、第１カメラ３０Ｒのイメージセンサの撮像面を含む。第２カメラ３０Ｌの投影面は、第２カメラ３０Ｌのイメージセンサの撮像面を含む。

画像データ取得部１０１は、第１カメラ３０Ｒで撮影された第１画像データＭＲと、第２カメラ３０Ｌで撮影された第２画像データＭＬとを取得する。第１カメラ３０Ｒで撮影された第１画像データＭＲ及び第２カメラ３０Ｂで撮影された第２画像データＭＬはそれぞれ、投影面に投影された２次元画像データである。

画像データ取得部１０１で取得された第１画像データＭＲ及び第２画像データＭＬは、ステレオ計測部１０２に出力される。ステレオ計測部１０２は、カメラ座標系において規定される第１画像データＭＲにおける計測点Ｐの像の座標データと、第２画像データＭＬにおける計測点Ｐの像の座標データと、エピポーラ平面とに基づいて、カメラ座標系における計測点Ｐの３次元座標データを算出する。

ステレオ計測部１０２は、第１画像データＭＲ及び第２画像データＭＬに基づいて、撮影対象ＳＢの複数の計測点Ｐのそれぞれの３次元座標データを算出する。これにより、撮影対象ＳＢの３次元データＤＧが算出される。

本実施形態において、ステレオ計測部１０２は、ステレオ計測によりカメラ座標系における複数の計測点Ｐの３次元座標データを算出した後、座標変換することによって、車体座標系における複数の計測点Ｐの３次元座標データ、及びグローバル座標系における複数の計測点Ｐの３次元座標データを算出する。

［計測点までの距離の算出］
ステレオ計測部１０２は、第１画像データＭＲ及び第２画像データＭＬをステレオ方式で画像処理して、撮影対象ＳＢの３次元データＤＧを算出する。ステレオ計測部１０２は、三角測量の原理に基づいて、ステレオカメラ３００のベースラインＢＬから撮影対象ＳＢの計測点Ｐまでの距離ＺＰを算出する。

図６は、本実施形態に係るステレオカメラ３００の一例を模式的に示す図である。図６に示すように、例えばステレオカメラ３００の第１カメラ３０Ｒと第２カメラ３０Ｌとは、カメラ座標系のＹｓ軸と平行な方向に配置することができる。その場合、第１カメラ３０Ｒのカメラ座標系のＹｓ軸と、第２カメラ３０Ｌのカメラ座標系のＹｓ軸とは、同一直線上に配置され、同一方向を向く。

第１カメラ３０Ｒは、イメージセンサ３１Ｒを有する。カメラ座標系における第１カメラ３０Ｒの原点は、光学中心ＯＲに規定される。第１カメラ３０Ｒの光学系の光軸は、Ｚｓ軸と平行であり、光学中心ＯＲを通る。撮影対象ＳＢを含む第１画像データＭＲが第１カメラ３０Ｒによって取得される。

第２カメラ３０Ｌは、イメージセンサ３１Ｌを有する。カメラ座標系における第２カメラ３０Ｌの原点は、光学中心ＯＬに規定される。第２カメラ３０Ｌの光学系の光軸は、Ｚｓ軸と平行であり、光学中心ＯＬを通る。撮影対象ＳＢを含む第２画像データＭＬが第２カメラ３０Ｌによって取得される。

図６において、第２カメラ３０Ｌの光学中心ＯＬを原点とする３次元座標系（Ｘ，Ｙ，Ｚ）が設定される。Ｘ軸とＸｓ軸とは実質的に平行である。Ｙ軸とＹｓ軸とは実質的に平行である。Ｚ軸とＺｓ軸とは実質的に平行である。

図６において、イメージセンサ３１Ｒの撮像面に規定される第１カメラ３０Ｒの投影面と、イメージセンサ３１Ｌの撮像面に規定される第２カメラ３０Ｌの投影面とは、同一平面内に配置される。また、Ｚ軸方向において、第１カメラ３０Ｒの投影面と、第２カメラ３０Ｌの投影面とは、同一位置に配置される。

第１画像データＭＲは、第１カメラ３０Ｒの投影面に形成される撮影対象ＳＢの投影画像を含む。第２画像データＭＬは、第２カメラ３０Ｌの投影面に形成される撮影対象ＳＢの投影画像を含む。

ベースラインＢＬの寸法はｂである。第１カメラ３０Ｒによって撮影された第１画像データＭＲのうち計測点Ｐを含む画素ＰＸｒのＹ軸方向の位置はＹＲである。第２カメラ３０Ｂによって撮影された第２画像データＭＬのうち計測点Ｐを含む画素ＰＸｌのＹ軸方向の位置はＹＬである。第２カメラ３０Ｌの投影面とベースラインＢＬとのＺ軸方向の距離はｆである。なお、距離ｆは第２カメラ３０Ｌの光学系の焦点距離と等しい。

第１画像データＭＲにおける計測点Ｐの投影点と第２画像データＭＬにおける計測点Ｐの投影点との距離を示す視差は、ｄである。視差ｄは、計測点Ｐを含む画素ＰＸｒと計測点Ｐを含む画素ＰＸｌとのずれ量（単位：pixel）でもよいし、計測点Ｐを含む画素ＰＸｒと計測点Ｐを含む画素ＰＸｌと画素ＰＸｒとの距離でもよい。

ステレオカメラ３００のベースラインＢＬから撮影対象ＳＢの計測点Ｐまでの距離ＺＰは、（１）式に基づいて算出される。

［視差画像データ］
図７及び図８は、本実施形態に係る第１カメラ３０Ｒによって撮影された第１画像データＭＲ及び第２カメラ３０Ｌによって撮影された第２画像データＭＬの一例を模式的に示す図である。

図７及び図８に示すように、第１画像データＭＲにおける計測点Ｐの投影点と第２画像データＭＬにおける計測点Ｐの投影点との距離を示す視差ｄに起因して、第１画像データＭＲにおける計測点Ｐの位置と、第２画像データＭＬにおける計測点Ｐの位置とは、Ｙｓ軸方向において異なる。図７に示すように、視差ｄとは、第１画像データＭＲにおける計測点Ｐの投影点（画素）と第２画像データＭＬにおける計測点Ｐの投影点（画素）とのＹｓ軸方向のずれ量をいう。

ステレオ計測部１０２は、第１画像データＭＲと第２画像データＭＬとをステレオ方式で画像処理して、撮影対象ＳＢの視差画像データＳＧを生成する。具体的には、ステレオ計測部１０２は、第１画像データＭＲ及び第２画像データＭＬについて、ステレオ対応点探索を実施する。ステレオ対応点探索とは、第１画像データＭＲ及び第２画像データＭＬのそれぞれにおいて、同一の計測点Ｐが投影された画素ＰＸｒ及び画素ＰＸｌを探索する処理をいう。

ステレオ計測部１０２は、第１画像データＭＲを基準画像データとし、第２画像データＭＬを参照画像データとする。図８に示すように、ステレオ計測部１０２は、計測点Ｐの投影点を含む第１画像データＭＲの画素ＰＸｒに対して、同一の計測点Ｐの投影点を含む画素ＰＸｌを第２画像データＭＬの中から探索する。本実施形態において、ステレオ計測部１０２は、第２画像データＭＬのエピポーラ線上に存在する複数の画素ＰＸｌの中から、計測点Ｐの投影点を含む画素ＰＭｌを探索する。

なお、ステレオ対応点探索に用いる画素ＰＸｒ，ＰＸｌは、イメージセンサの１つの画素でもよいし、複数の画素の集合体でもよい。

以下の説明においては、第１画像データＭＲのうち計測点Ｐの投影点を含む画素ＰＸｒを適宜、注目画素ＰＸｒ、と称し、第２画像データＭＬのうち同一の計測点Ｐの投影点を含む画素ＰＸｌを適宜、対応画素ＰＸｌ、と称する。

また、以下の説明においては、注目画素ＰＸｒについて対応画素ＰＸｌを探索した結果、対応画素ＰＸｌの探索に成功した状態を適宜、探索成功、と称する。また、注目画素ＰＸｒについて対応画素ＰＸｌを探索した結果、対応画素ＰＸｌの探索に失敗した状態を適宜、探索失敗、と称する。

探索成功したとき、ステレオ計測部１０２は、注目画素ＰＸｒと対応画素ＰＸｌとの距離に基づいて、視差ｄ（例えば、ずれている画素の数）を求めることができる。（１）式において、ベースラインＢＬの寸法ｂ及び焦点距離ｆは、カメラ３０の設計データ又は諸元データから導出される既知データであり、記憶部１０９に記憶されている。したがって、探索成功し、視差ｄが算出されたとき、ステレオ計測部１０２は、記憶部１０９に記憶されている寸法ｂ及び焦点距離ｆと、算出した視差ｄとに基づいて、計測点Ｐまでの距離ＺＰを算出することができる。

一方、探索失敗したとき、ステレオ計測部１０２は、視差ｄを算出することができず、計測点Ｐまでの距離ＺＰを算出することができない。

探索成功したとき、ステレオ計測部１０２は、探索成功した注目画素ＰＸｒ及び対応画素ＰＸｌに基づいて、画素ＰＸｓを生成する。一方、探索失敗したとき、ステレオ計測部１０２は、探索失敗した注目画素ＰＸｒ及び対応画素ＰＸｌに基づいて、画素ＰＸｓを生成する。

ステレオ計測部１０２は、第１画像データＭＲの複数の画素ＰＸｒのそれぞれについて、ステレオ対応点探索を実施する。これにより、複数の画素ＰＸｓが生成され、複数の画素ＰＸｓが２次元配列された視差画像データＳＧが生成される。視差画像データＳＧとは、ステレオ計測部１０２によって算出された、各画素ＰＸｓにおける視差の大きさを可視化した画像データであり、例えば図８のように視差の大きさに応じて各画素をグレーの濃淡（視差が大きい場合は白く、視差が小さい場合は黒く表示）により表現したものである。視差画像は、濃淡以外の表現でもよく、例えば視差の大きさに応じて変化する色彩により表現されてもよい。

ステレオ計測部１０２は、探索成功した注目画素ＰＸｒ及び対応画素ＰＸｌに基づいて生成された画素ＰＸｓに探索成功したことを示す成功データを付与し、探索失敗した注目画素ＰＸｒ及び対応画素ＰＸｌに基づいて生成された画素ＰＸｓに探索失敗したことを示す失敗データを付与してもよい。画素ＰＸｓに失敗データが付与されることにより、画素ＰＸｓは例えば黒く表示される。

以下の説明においては、探索成功した注目画素ＰＸｒ及び対応画素ＰＸｌに基づいて生成された画素ＰＸｓを単に、探索成功した画素ＰＸｓ、と称し、探索失敗した注目画素ＰＸｒ及び対応画素ＰＸｌに基づいて生成された画素ＰＸｓを単に、探索失敗した画素ＰＸｓ、と称する。

ステレオ計測部１０２は、探索成功した複数の画素ＰＸｓのそれぞれを視差ｄに基づいて表現して、視差画像を生成することができる。また、ステレオ計測部１０２は、探索成功した複数の画素ＰＸｓのそれぞれを距離ＺＰに基づいて表現して、距離画像を生成することができる。距離画像とは、画像データにおける各画素ＰＸｓのそれぞれに、何らかの形でカメラ３０から対象点Ｐまでの距離ＺＰを格納した画像データであってもよいし、各画素ＰＸｓのカメラ３０から対象点Ｐまでの距離ＺＰを可視的に表現した画像でもよい。視差画像データは、距離画像データを含む。

［ステレオ率］
次に、ステレオ率について説明する。ステレオ率とは、例えばステレオ計測部１０２で生成された視差画像データＳＧの複数の画素ＰＸｓのうちステレオ計測が成功した画素ＰＸｓの割合をいう。ステレオ計測が成功した画素ＰＸｓとは、注目画素ＰＸｒについてステレオ対応点検索した結果、対応画素ＰＸｌの探索に成功し、その注目画素ＰＸｒと探索成功した対応画素ＰＸｌとに基づいて生成された画素ＰＸｓをいう。

ステレオ率ＳＴＲは、以下の（２）式で示される。

（２）式において、ＰＸＡは、視差画像データＳＧの画素ＰＸｓの総数である。ＰＸＳは、視差画像データＳＧのうちステレオ計測が成功した画素ＰＸｓの総数である。

なお、視差画像データＳＧの全ての画素ＰＸｓから模様が存在しない領域に相当する画素ＰＸｓを除いた画素ＰＸｓを（２）式の分母としてもよい。

なお、ステレオ率ＳＴＲは、視差画像データＳＧのうちステレオ計測が成功した画素ＰＸｓの総数でもよい。

［パラメータ］
次に、記憶部１０９に記憶される第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置を規定するパラメータについて説明する。以下の説明においては、第１カメラ３０Ｒが基準カメラであり、第２カメラ３０Ｌが参照カメラであることとする。

図９は、本実施形態に係るステレオカメラ３００の第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置を説明するための模式図である。図９に示すように、第１カメラ３０Ｒと第２カメラ３０Ｌとは、例えばＹｓ軸と平行な方向に配置されてもよい。

本実施形態においては、第１カメラ３０Ｒ及び第２カメラ３０Ｌに関する複数のパラメータが予め求められ、記憶部１０９に記憶されている。本実施形態において、記憶部１０９は、第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置を規定する複数の外部パラメータを記憶する。

外部パラメータは、第１カメラ３０Ｒに対する第２カメラ３０Ｌの回転方向のパラメータα，β，γと、第１カメラ３０Ｒに対する第２カメラ３０Ｌの並進方向のパラメータＴｘ，Ｔｙ，Ｔｚとを含む。

パラメータαは、第１カメラ３０Ｒに対するθＸｓ方向の第２カメラ３０Ｌの光学系の光軸の相対角度を示す。パラメータβは、第１カメラ３０Ｒに対するθＹｓ方向の第２カメラ３０Ｌの光学系の光軸の相対角度を示す。パラメータγは、第１カメラ３０Ｒに対するθＺｓ方向の第２カメラ３０Ｌの光学系の光軸の相対角度を示す。

以下の説明においては、パラメータαを適宜、ヨー角α、と称し、パラメータβを適宜、ピッチ角β、と称し、パラメータγを適宜、ロール角γ、と称する。

パラメータＴｘは、第１カメラ３０Ｒに対するＸｓ軸方向の第２カメラ３０Ｌの相対位置を示す。パラメータＴｙは、第１カメラ３０Ｒに対するＹｓ軸方向の第２カメラ３０Ｌの相対位置を示す。パラメータＴｚは、第１カメラ３０Ｒに対するＺｓ軸方向の第２カメラ３０Ｌの相対位置を示す。

以下の説明においては、パラメータＴｘを適宜、シフト量Ｔｘ、と称し、パラメータＴｙを適宜、シフト量Ｔｙ、と称し、パラメータＴｚを適宜、シフト量Ｔｚ、と称する。

本実施形態において、記憶部１０９は、外部パラメータとして、ヨー角α、ピッチ角β、ロール角γ、シフト量Ｔｘ、シフト量Ｔｙ、及びシフト量Ｔｚを記憶する。

第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置は、上述の複数の外部パラメータを含む下記の（３）式で規定される。

（３）式において、（ｘ_１，ｙ_１，ｚ_１）は、第１カメラ３０Ｒのカメラ座標系における第１カメラ３０Ｒの座標を示す。（ｘ_２，ｙ_２，ｚ_２）は、第２カメラ３０Ｌのカメラ座標系における第２カメラ３０Ｌの座標を示す。

（３）式は、ヨー角α、ピッチ角β、及びロール角γのそれぞれによって規定される３つの回転行列と、シフト量Ｔｘ、シフト量Ｔｙ、及びシフト量Ｔｚによって規定される平行移動ベクトルとを含む。（３）式は、第２カメラ３０Ｌのカメラ座標系における第２カメラ３０Ｌの位置を、第１カメラ３０Ｒのカメラ座標系における位置に変換する変換式である。

本実施形態においては、（３）式に基づいて、第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置が規定される。第１調整部１０３及び第２調整部１０４は、ヨー角α、ピッチ角β、ロール角γ、シフト量Ｔｘ、シフト量Ｔｙ、及びシフト量Ｔｚの少なくとも一つを調整することにより、第１カメラ３０Ｒのカメラ座標系において、第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置を調整することができる。

油圧ショベル１の工場出荷時においては、ステレオ率がステレオ閾値以上になるように、第１カメラ３０Ｒに対する第２カメラ３０Ｌのヨー角α、ピッチ角β、ロール角γ、シフト量Ｔｘ、シフト量Ｔｙ、及びシフト量Ｔｚが調整されている。ステレオ閾値は、ステレオ率について規定された閾値である。以下の説明においては、ステレオ率がステレオ閾値以上になるように第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置を調整することを適宜、外部較正処理、と称する。

外部較正処理において導出されたヨー角α、ピッチ角β、ロール角γ、シフト量Ｔｘ、シフト量Ｔｙ、及びシフト量Ｔｚを含む変換式は、油圧ショベル１の工場出荷時において記憶部１０９に記憶される。

ステレオ計測部１０２は、記憶部１０９に記憶されているパラメータを用いて、第１画像データＭＲと第２画像データＭＬとをステレオ方式で画像処理して、視差画像データＳＧを生成する。

外部較正処理が実施されたステレオカメラ３００が搭載された油圧ショベル１の工場出荷後、例えば振動、衝撃、又は熱のような外的要因により、カメラ３０が物理的に動いてしまう可能性がある。外的要因により、例えば第１カメラ３０Ｒ及び第２カメラ３０Ｌの一方のカメラ３０に対して他方のカメラ３０のＸｓ軸方向、Ｙｓ軸方向、Ｚｓ軸方向、θＸｓ方向、θＹｓ方向、及びθＺｓ方向の少なくとも１つの方向の位置がずれてしまう可能性がある。

第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置が物理的に変動した場合、工場出荷時において記憶部１０９に記憶されている外部パラメータによって規定される第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置と、カメラ３０が動いてしまった後の実際の第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置とに差異が生じる。その場合、第１画像データＭＲと第２画像データＭＬとをステレオ計測により対応点を探索しても、探索失敗する可能性が高い。その結果、ステレオ率が低下し、撮影対象ＳＢのステレオ計測を正確に実施することが困難となる。

本実施形態において、第１調整部１０３は、第１カメラ３０Ｒ及び第２カメラ３０Ｌの少なくとも一方のカメラ３０の位置がずれてしまいステレオ率が低下した場合において、ステレオ率が再度向上するように、（３）式に含まれる複数の外部パラメータの少なくとも一部を変更する。また、第２調整部１０４は、第１カメラ３０Ｒ及び第２カメラ３０Ｌの少なくとも一方のカメラ３０の位置がずれてしまいステレオカメラ３００による計測結果の縮尺（カメラ３０から計測点Ｐまでの距離又はスケール）が不正確になった場合において、（３）式に含まれる複数の外部パラメータの少なくとも一部を変更して、３次元データＤＧのスケールを調整する。

すなわち、本実施形態において、制御装置１００は、カメラ３０の位置が物理的に動いてしまった場合、記憶部１０９に記憶されている外部パラメータを変更して、第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置の変動を演算処理によって自動修正する。

［ステレオ率の調整］
次に、本実施形態に係るステレオ率の調整について説明する。第１調整部１０３は、外部パラメータを調整して、ステレオ率の調整処理を実施する。

表示制御部１０８は、ステレオ計測部１０２で生成された視差画像データＳＧを表示装置２６に表示させる。ステレオ計測部１０２は、グレー表示させるための成功データを探索成功した画素ＰＸｓに付与し、黒表示させるための失敗データを探索失敗した画素ＰＸｓに付与する。

図１０は、本実施形態に係る探索失敗したときの視差画像データＳＧの一例を模式的に示す図である。ステレオカメラ３００の第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置が物理的に変動した場合、第１画像データＭＲと第２画像データＭＬとをステレオ対応点探索しても、探索失敗する可能性が高い。その結果、図１０に示すように、ステレオ率が低下し、視差画像データＳＧの大部分の画素ＰＸｓが黒表示される。

図１１は、本実施形態に係る第１調整部１０３によるステレオ率の調整処理の一例を説明するための模式図である。ステレオ率が低下したとき、第１調整部１０３は、記憶部１０９に記憶されている複数の外部パラメータのうち、例えばピッチ角βを変更する。図１１に示すように、外部パラメータのピッチ角βが調整されることにより、第２カメラ３０Ｌは、疑似的にθＹｓ方向に回転する。

上述のように、ステレオ対応点探索においては、ステレオ計測部１０２は、第２画像データＭＬのエピポーラ線上に存在する複数の画素ＰＸｌの中から、計測点Ｐの投影点を含む画素ＰＸｌを探索する。本実施形態において、第１カメラ３０Ｒと第２カメラ３０Ｌとは、Ｙｓ軸方向に配置され、エピポーラ線は、Ｙｓ軸方向に延在する。第１カメラ３０Ｒと第２カメラ３０ＬとがＹｓ軸方向に配置された場合、ステレオ処理における探索の成否は、ピッチ角βのずれによって大きく影響される。そのため、第１調整部１０３は、外部パラメータのピッチ角βを変更することにより、計測点Ｐの投影点を含む対応画素ＰＸｌが存在する可能性が高いエピポーラ線上の複数の画素ＰＸｌを探索することができる。

第１調整部１０３は、視差画像データＳＧのステレオ率が向上するように、ピッチ角βを調整する。本実施形態において、記憶部１０９は、基準ピッチ角β_０を記憶する。基準ピッチ角β_０は、工場出荷時の外部較正処理において、ステレオ率が最大又はステレオ閾値以上になるように調整されたピッチ角βである。第１調整部１０３は、基準ピッチ角β_０を含む所定範囲において第２カメラ３０Ｌのピッチ角βを変更して、例えばステレオ率の最大値を算出する。

図１２は、本実施形態に係る第１調整部１０３によるステレオ率の調整方法の一例を説明するための模式図である。図１２に示すグラフにおいて、横軸は、外部パラメータのピッチ角βを示し、縦軸は、ステレオ率を示す。

第１調整部１０３は、基準ピッチ角β_０を基準として、プラス方向及びマイナス方向の所定範囲において、ピッチ角βを所定角度ずつ変更する。ステレオ計測部１０２は、ピッチ角βを変更する毎に、ステレオ率を算出する。図１２に示すように、ステレオ率は、ピッチ角βの変更に応じて変化する。第１調整部１０３は、例えば所定範囲においてステレオ率の最大値が得られたときのピッチ角βを正解ピッチ角βｒに決定する。

なお、正解ピッチ角βｒを決定する方法は、上述の方法でなくてもよい。ステレオ率がステレオ閾値以上であるピッチ角βであれば、任意のピッチ角βを正解ピッチ角βｒとしてもよい。

図１３は、本実施形態に係る探索成功したときの視差画像データＳＧの一例を模式的に示す図である。第１調整部１０３は、記憶部１０９に記憶されているピッチ角β（基準ピッチ角β_０）を正解ピッチ角βｒに変更する。すなわち、（３）式の回転行列のピッチ角βが、基準ピッチ角β_０から正解ピッチ角βｒに変更される。ステレオ計測部１０２は、第１画像データＭＲと、第２画像データＭＬと、正解ピッチ角βｒを含む外部パラメータとに基づいて、ステレオ計測を実施して、視差画像データＳＧを生成する。ピッチ角βが基準ピッチ角β_０から正解ピッチ角βｒに変更されることにより、記憶部１０９に記憶されている外部パラメータは、相対位置が物理的に変動した後の第１カメラ３０Ｒ及び第２カメラ３０Ｌに最適化されている。そのため、ステレオカメラ３００の第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置が物理的に変動した後において、ステレオ計測部１０２が第１画像データＭＲと第２画像データＭＬとを用いてステレオ対応点探索したとき、探索成功する可能性が高くなる。これにより、図１３に示すように、ステレオ率は向上し、視差画像データＳＧの大部分の画素ＰＸｓがグレー表示される。

［スケールの調整］
次に、本実施形態に係るステレオ率の調整について説明する。第２調整部１０４は、外部パラメータを調整して、３次元データＤＧのスケールの調整処理を実施する。

ステレオ率の調整は、外部パラメータを変更することにより、物理的に位置が変動したカメラ３０の位置を疑似的に調整することによってステレオ率を向上させる処理である。しかし、ステレオ率を向上させただけでは、実際の現況地形とステレオカメラ３００によって計測された３次元データＤＧとのスケールの差が大きく、十分な計測精度が得られない。そのため、本実施形態においては、上述の方法によりステレオ率が向上された３次元データＤＧのスケールの調整が実施される。スケールの調整は、３次元データＤＧがステレオカメラ３００からどれくらいの距離だけ離れたところに存在するのかを把握し、実際の現況地形と整合するように調整する処理である。スケールの調整により、実際の現況地形と整合する、精度の高い又は誤差の小さい３次元データＤＧが得られる。

図１４は、本実施形態に係る第２調整部１０４によるスケールの調整処理の一例を説明するための模式図である。３次元データＤＧのスケールを調整するとき、第２調整部１０４は、記憶部１０９に記憶されている複数の外部パラメータのうち、例えばヨー角αを変更する。図１４に示すように、外部パラメータのヨー角αが調整されることにより、第２カメラ３０Ｌは、疑似的にθＸｓ方向に回転する。

本実施形態において、第１カメラ３０Ｒと第２カメラ３０ＬとがＹｓ軸方向に配置され、三角測量の原理に基づいて計測点Ｐまでの距離ＺＰが算出される。そのため、第２調整部１０４は、第２カメラ３０Ｌの外部パラメータのヨー角αを変更することにより、視差画像データＳＧにおける計測点Ｐが投影される画素の視差を調整することができる。視差画像データＳＧにおける視差が調整されることによって、間接的に３次元データＤＧのスケールが調整される。

第２調整部１０４は、後述する基準スケールと視差画像データＳＧにより求められる３次元データＤＧのスケールとの差が小さくなるように、ステレオカメラ３００について規定されている外部パラメータのヨー角αを変更する。基準スケールとは、視差画像データＳＧから求められる３次元データＤＧが実際の現況地形と整合するように、カメラ３０から計測点Ｐまでの縮尺（距離又はスケール）を調整するときに用いる基準となるスケールである。第２調整部１０４は、基準スケールと３次元データＤＧのスケールとの差が最小又は閾値以下となる正解ヨー角αｒを決定する。

本実施形態において、記憶部１０９は、基準ヨー角α_０を記憶する。基準ヨー角α_０は、工場出荷時の外部較正処理において視差画像データを調整されたヨー角αである。第２調整部１０４は、基準ヨー角α_０を含む所定範囲において第２カメラ３０Ｌのヨー角αを変更して、基準スケールと３次元データＤＧのスケールとの差の最小値を算出する。

第２調整部１０４は、記憶部１０９に記憶されているヨー角α（基準ヨー角α_０）を正解ヨー角αｒに変更する。すなわち、（３）式の回転行列のヨー角αが、基準ヨー角α_０から正解ヨー角αｒに変更される。ステレオ計測部１０２は、第１画像データＭＲと、第２画像データＭＬと、正解ヨー角αｒを含む外部パラメータとに基づいて、ステレオ計測を実施して、視差画像データＳＧを生成する。ヨー角αが基準ヨー角α_０から正解ヨー角αｒに変更されることにより、第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置が物理的に変動した後において、記憶部１０９に記憶されている外部パラメータは、相対位置が物理的に変動した後の第１カメラ３０Ｒ及び第２カメラ３０Ｌに最適化されている。そのため、ステレオカメラ３００の第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置が物理的に変動した後において、３次元データＤＧのスケールは、実際の施工現場と同様のスケールとなる。すなわち、３次元データＤＧの形状は、実際の施工現場の形状と同様の形状となる。

［形状計測方法］
次に、本実施形態に係る位置計測方法について説明する。図１５は、本実施形態に係る位置計測方法の一例を示すフローチャートである。

油圧ショベル１に設けられた第１ステレオカメラ３０１は、撮影対象ＳＢの第１領域である遠景領域を撮影する。油圧ショベル１に設けられた第２ステレオカメラ３０２は、撮影対象ＳＢの第２領域である近景領域を撮影する。

画像データ取得部１０１は、第１ステレオカメラ３０１の第１カメラ３０Ａ及び第２カメラ３０Ｂで撮影された第１画像データＭＲ１及び第２画像データＭＬ１と、第２ステレオカメラ３０２の第３カメラ３０Ｃ及び第４カメラ３０Ｄで撮影された第３画像データＭＲ２及び第４画像データＭＬ２を取得する（ステップＳＰ１０）。

第１ステレオカメラ３０１で撮影された第１画像データＭＲ１及び第２画像データＭＬ１は、撮影対象ＳＢの遠景領域を含む。第２ステレオカメラ３０２で撮影された第３画像データＭＲ２及び第４画像データＭＬ２は、撮影対象ＳＢの近景領域を含む。

ステレオ計測部１０２は、第１ステレオカメラ３０１で撮影された第１画像データＭＲ１及び第２画像データＭＬ１と、記憶部１０９に記憶されている複数のパラメータとに基づいて、ステレオ計測を実施する。ステレオ計測部１０２は、ステレオ計測を実施して、撮影対象ＳＢの遠景領域を示す第１視差画像データＳＧ１を生成する。

また、ステレオ計測部１０２は、第２ステレオカメラ３０２で撮影された第３画像データＭＲ２及び第４画像データＭＬ２と、記憶部１０９に記憶されている複数のパラメータとに基づいて、ステレオ計測を実施する。ステレオ計測部１０２は、ステレオ計測を実施して、撮影対象ＳＢの近景領域を示す第２視差画像データＳＧ２を生成する（ステップＳＰ２０）。

図１６は、本実施形態に係る撮影対象ＳＢを第１カメラ３０Ａにより撮影した第１画像データＭＲ１を模式的に示す図である。図１６に示すように、第１ステレオカメラ３０１は、撮影対象ＳＢの遠景領域を撮影する。撮影対象ＳＢの遠景領域は、施工対象である地面のみならず、例えば空又は空に浮かぶ雲のような、第１ステレオカメラ３０１に対して無限遠点に配置されている物体を含む。

図１７は、本実施形態に係る第１ステレオカメラ３０１で取得された第１画像データＭＲ１及び第２画像データＭＬ１に基づいて生成された第１視差画像データＳＧ１の一例を模式的に示す図である。なお、図１７において、撮影対象ＳＢのうち第１ステレオカメラ３０１に対して無限遠点に存在する物体の第１画像データＭＲ１と第２画像データＭＬ１との視差ｄはゼロとなる。そのため、第１視差画像データＳＧ１のうち、空及び雲に相当する部分は、黒表示される。

本実施形態においては、例えば工場出荷時に第１ステレオカメラ３０１及び第２ステレオカメラ３０２の外部較正処理が実施される。以下の説明においては、第２ステレオカメラ３０２については、第２ステレオカメラ３０２で取得された第３画像データＭＲ２及び第４画像データＭＬ２に関するステレオ対応点探索が成功し、第２視差画像データＳＧ２のステレオ率はステレオ閾値以上であることとする。一方、第１ステレオカメラ３０１については、外部較正処理後に第１カメラ３０Ａ及び第２カメラ３０Ｂの少なくとも一方にずれが生じてしまい、第１ステレオカメラ３０１で取得された第１画像データＭＲ１及び第２画像データＭＬ１に関するステレオ対応点探索が失敗し、第１視差画像データＳＧ１のステレオ率はステレオ閾値よりも小さいこととする。第１視差画像データＳＧ１の画素の大部分においてステレオ対応点探索が失敗しているとき、例えば図１０に示すように、視差画像の大部分が黒い領域となる。

第１調整部１０３は、第１ステレオカメラ３０１について規定されている複数のパラメータのうち少なくとも一部のパラメータを変更して、第１視差画像データＳＧ１のステレオ率を調整して、ステレオ率を向上させる（ステップＳＰ３０）。

本実施形態において、ステレオ率を調整することは、ピッチ角βを変更することを含む。第１調整部１０３は、大部分において探索が失敗した第１視差画像データＳＧ１のステレオ率が向上するように、上述の方法により、第１ステレオカメラ３０１について規定されている外部パラメータのピッチ角βを変更する。第１調整部１０３は、ステレオ率が最大又はステレオ閾値以上となる正解ピッチ角βｒを決定する。

ステレオ計測部１０２は、正解ピッチ角βｒを用いて、第１画像データＭＲ１及び第２画像データＭＬ１をステレオ計測する。これにより、例えば図１３に示す視差画像のように、ステレオ率が向上した第１視差画像データＳＧ１が生成される。

次に、第２調整部１０４は、第１ステレオカメラ３０１について正解ピッチ角βｒを用いてステレオ率が最大又は閾値以上となった第１視差画像データＳＧ１により求められる第１の３次元データＤＧ１のスケールを調整する（ステップＳＰ４０）。

本実施形態において、スケールを調整することは、ヨー角αを調整することを含む。第２調整部１０４は、基準スケールと第１の３次元データＤＧ１のスケールとの差が小さくなるように、第１ステレオカメラ３０１について規定されている外部パラメータのヨー角αを変更する。第２調整部１０４は、基準スケールと第１の３次元データＤＧ１のスケールとの差が最小又は閾値以下となる正解ヨー角αｒを決定する。

本実施形態において、基準スケールは、第２ステレオカメラ３０２から取得された第３画像データＭＲ２及び第４画像データＭＬ２に基づいて生成された第２視差画像データＳＧ２により求められる第２の３次元データＤＧ２のスケールを含む。第２調整部１０４は、第２ステレオカメラ３０２で撮影された第３画像データＭＲ２及び第４画像データＭＬ２に基づいて生成された第２の３次元データＤＧ２のスケールと、第１ステレオカメラ３０１で撮影された第１画像データＭＲ１及び第２画像データＭＬ１に基づいて生成された第１の３次元データＤＧ１のスケールとの差が小さくなるように、記憶部１０９に記憶されている外部パラメータのヨー角α（基準ヨー角α_０）を変更する。

上述のように、本実施形態において、第２視差画像データＳＧ２のステレオ率は、ステレオ閾値以上である。したがって、工場出荷時から、第２ステレオカメラ３０２の第３カメラ３０Ｃと第４カメラ３０Ｄとの相対位置は変動していないとみなすことができる。工場出荷時における外部較正処理においては、ステレオ率の調整のみならずスケールの調整も実施される。すなわち、外部較正処理においては、ステレオ率がステレオ閾値以上となり、かつ３次元データＤＧのスケールが正解値となるように、ステレオカメラ３００の一対のカメラ３０の相対位置の調整が実施されている。そのため、第２視差画像データＳＧ２のステレオ率がステレオ閾値以上であるとき、第２ステレオカメラ３０２の第３カメラ３０Ｃと第４カメラ３０Ｄとの相対位置は変動していないとみなすことができる。つまり、第２の３次元データＤＧ２のスケールは正解値であると推定することができる。そのため、本実施形態においては、第２調整部１０４は、基準スケールとして、第２の３次元データＤＧ２のスケールを使用する。

上述のように、第１ステレオカメラ３０１は、撮影対象ＳＢの遠景領域を撮影する。第２ステレオカメラ３０２は、撮影対象ＳＢの近景領域を撮影する。本実施形態において、第１ステレオカメラ３０１と第２ステレオカメラ３０２とは、遠景領域と近景領域の一部とが重複するように、撮影対象ＳＢを撮影する。

第２調整部１０４は、撮影対象ＳＢの遠景領域と近景領域との重複領域において、第１の３次元データＤＧ１のスケールと第２の３次元データＤＧ２のスケールとの差が小さくなるように、外部パラメータのヨー角αを変更する。

図１９は、本実施形態に係る第１の３次元データＤＧ１及び第２の３次元データＤＧ２の一例を示す図である。図１９に示すように、表示制御部１０８は、ステレオ計測部１０２で生成された第１の３次元データＤＧ１及び第２の３次元データＤＧ２を表示させる。

第１の３次元データＤＧ１は、第１ステレオカメラ３０１から撮影対象ＳＢの複数の部位までのそれぞれとの距離ＺＰを示すデータである。距離ＺＰは、（１）式に基づいて第１視差画像データＳＧ１の複数の画素ＰＸｓ毎に算出される。マップデータ作成部１０７は、複数の画素ＰＸｓ毎に算出された距離ＺＰに基づいて、カメラ座標系における３次元マップデータを作成する。カメラ座標系における３次元データＤＧが座標変換されることにより、車体座標系における３次元データＤＧ、及びグローバル座標系における３次元データＤＧが作成される。また、グローバル座標系における３次元データＤＧを用いて、所定間隔（ｘ、ｙ）で設けられたメッシュ毎の高さが算出されることにより、第１の３次元マップデータ（エレベーションマップデータ）ＤＧ１が作成される。グローバル座標系における３次元データＤＧは、画素の数だけ３次元データが存在するが、３次元マップデータは、予め規定されたメッシュ毎にしか３次元データが存在しない。

また、マップデータ作成部１０７は、第１視差画像データＳＧ１の３次元マップデータの作成方法と同様の作成方法で、第２視差画像データＳＧ２に基づいて第２の３次元マップデータＤＧ２を作成する。３次元データＤＧは、視差画像データＳＧ、車体座標系等の３次元データ、及び３次元マップデータ（エレベーションマップデータ）を含む。

図１８に示すように、マップデータ作成部１０７は、撮影対象ＳＢの遠景領域と近景領域との重複領域、すなわち第１の３次元マップデータＤＧ１と第２の３次元マップデータＤＧ２との重複領域におけるデータである重複領域データＤＡを作成する。

重複領域データＤＡは、第１の３次元マップデータＤＧ１と第２の３次元マップデータＤＧ２において、グローバル座標系におけるＸＹ平面内の各同一の位置（メッシュ）又は各対応する位置における、Ｚ軸方向の差分データを含む。本実施形態においては、マップデータ作成部１０７は、重複領域内の各位置における第１の３次元マップデータＤＧ１の高さデータ（ｚ）と第２の３次元マップデータＤＧ２の高さデータ（ｚ）との差分を算出し、各差分の重複領域全体の総和が最小又は閾値以下となるようにスケールを調整する。

第１の３次元データＤＧ１のスケールと第２の３次元データＤＧ２のスケールとの差が大きい場合、重複領域内の各位置における第１の３次元マップデータＤＧ１の高さデータと第２の３次元マップデータＤＧ２の高さデータとの差分は大きくなり、すなわち各差分の重複領域全体の総和も大きくなる。

本実施形態においては、上述のように第２の３次元データＤＧ２のスケールが正解値（基準スケール）であるため、第２調整部１０４は、第１の３次元データＤＧ１のスケールが第２の３次元データＤＧ２のスケールに一致するように、外部パラメータのヨー角αを調整する。具体的には、第２調整部１０４は、撮影対象ＳＢの遠景領域を撮影した第１の３次元データＤＧ１と撮影対象ＳＢの近景領域を撮影した第２の３次元データＤＧ２との重複領域全体における、第１の３次元データＤＧ１の高さデータと第２の３次元データＤＧ２の高さデータとの差分の総和が小さくなるように、外部パラメータのヨー角αを変更する。

図１９は、本実施形態に係る第２調整部１０４によるスケールの調整方法の一例を説明するための模式図である。図１９に示すグラフにおいて、横軸は、外部パラメータのヨー角αを示し、縦軸は、第１の３次元データＤＧ１の高さデータと第２の３次元データＤＧ２の高さデータとの差分の総和との差を示す。

第２調整部１０４は、基準ヨー角α_０を基準として、プラス方向及びマイナス方向の所定範囲において、ヨー角αを所定角度ずつ変更する。ステレオ計測部１０２は、ヨー角αを変更する毎に、第１の３次元データＤＧ１の高さデータと第２の３次元データＤＧ２の高さデータとの差分の総和との差を算出する。図１９に示すように、第１の３次元データＤＧ１の高さデータと第２の３次元データＤＧ２の高さデータとの差分の総和との差は、ヨー角αの変更に応じて変化する。第２調整部１０４は、所定範囲において第１の３次元データＤＧ１の高さデータと第２の３次元データＤＧ２の高さデータとの差分の総和との差の最小値が得られたときのヨー角αを正解ヨー角αｒに決定する。

なお、正解ヨー角αｒを決定する方法は、上述の方法でなくてもよい。第１の３次元データＤＧ１の高さデータと第２の３次元データＤＧ２の高さデータとの差分の総和との差が閾値以下であるヨー角αであれば、任意のヨー角αを正解ヨー角αｒとしてもよい。

ステップＳＰ３０において正解ピッチ角βｒが決定され、ステップＳＰ４０において正解ヨー角αｒが決定された後、ステレオ計測部１０２は、正解ピッチ角βｒ及び正解ヨー角αｒを含む外部パラメータを用いて、第１ステレオカメラ３０１で撮影された第１画像データＭＲ１及び第２画像データＭＬ１のステレオ計測を実施する。これにより、ステレオ率がステレオ閾値以上であり、かつスケールが正解値である第１視差画像データＳＧ１および第１の３次元データＤＧ１が生成される（ステップＳＰ５０）。

本実施形態において、上述のステップＳＰ１０からステップＳＰ５０の処理は、所定のサンプリング周期で実施される。何らかの外的要因によりカメラ３０が物理的に動いてしまったとき、ステレオ率が向上しスケールが正解値になるように、ステレオカメラ３００の自動較正処理が実施される。

［作用及び効果］
以上説明したように、本実施形態によれば、制御装置１００は、第１カメラ３０Ｒ及び第２カメラ３０Ｌについて規定されているパラメータを変更し、変更されたパラメータに基づいて演算処理を実施して、ステレオ率及びスケールを調整することができる。ステレオ率を向上させることにより、ステレオカメラ３００によって広範囲に現況地形を計測することができる。また、スケールを調整することにより、ステレオカメラ３００によって計測された３次元データＤＧは、実際の現況地形との差分または誤差が少ない高精度なデータになる。これにより、外部較正処理の後、第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置が変動してしまっても、制御装置１００は、記憶部１０９に記憶されているパラメータを変更するだけで、煩雑な再較正処理を実施することなく、ステレオ率及びスケールが調整された３次元データＤＧ及び視差画像データＳＧを計測することができる。第１カメラ３０Ｒと第２カメラ３０Ｌとの相対位置が変動してしまっても、自動較正処理が実施されることにより、油圧ショベル１の作業効率の低下が抑制される。

また、本実施形態においては、第２調整部１０４は、基準スケールとして第２の３次元データＤＧ２を利用し、遠景領域と近景領域との重複領域において、第１の３次元データＤＧ１のスケールと第２の３次元データＤＧ２のスケールとの差が小さくなるように、パラメータを変更する。これにより、第１の３次元データＤＧ１のスケールを効率良く調整することができる。

なお、本実施形態においては、外部パラメータのピッチ角βを変更することによりステレオ率を向上させることとしたが、（３）式におけるヨー角α又はロール角γ、シフト量Ｔｘ、シフト量Ｔｙ、シフト量Ｔｚを変更してステレオ率を向上させてもよいし、それら複数の外部パラメータを同時に変更してもよい。

なお、本実施形態においては、外部パラメータのヨー角αを変更することによりスケールを調整することとしたが、（３）式におけるピッチ角β又はロール角γ、シフト量Ｔｘ、シフト量Ｔｙ、シフト量Ｔｚを変更してスケールを調整してもよいし、それら複数の外部パラメータを同時に変更してもよい。

第２実施形態．
第２実施形態について説明する。以下の説明において、上述の実施形態と同一又は同等の構成要素については同一の符号を付し、その説明を簡略又は省略する。

上述の実施形態においては、第２ステレオカメラ３０２に基づいて生成された第２の３次元データＤＧ２を基準スケールとして、第１ステレオカメラ３０１に基づいて生成された第１の３次元データＤＧ１のスケールを調整することとした。本実施形態においては、基準スケールとして基準物体を用いて、３次元データＤＧのスケールを調整する例について説明する。なお、本実施形態においても、上述の実施形態に従って、事前にステレオ率を向上する処理が実施されていてもよい。

図２０は、本実施形態に係る位置計測方法の一例を説明するための模式図である。本実施形態においては、基準物体であるターゲットＴＧがステレオカメラ３００の撮影領域に配置される。ターゲットＴＧは、例えば白色の表面を有する板部材と、板部材の白色の表面に黒色で描かれた基準マークとを含む。基準マークは、１つでもよいし、複数でもよい。本実施形態において、基準マークは、円形マークである。なお、ターゲットＴＧは、基準マークを有しなくてもよい。

ターゲットＴＧは、グローバル座標系における位置が既知である所定位置に配置される。すなわち、本実施形態において、グローバル座標系におけるターゲットＴＧの位置を示す位置データは、既知データである。ターゲットＴＧの位置データは、位置データ算出部１０５に入力される。

上述のように、位置データ算出部１０５は、グローバル座標系におけるカメラ３０の位置データを算出可能である。

位置データ算出部１０５は、グローバル座標系におけるステレオカメラ３００とターゲットＴＧとの相対位置データを算出する。位置データ算出部１０５は、グローバル座標系におけるステレオカメラ３００とターゲットＴＧとの相対位置データから、基準距離ＺＦを演算する。

第２調整部１０４は、基準スケールと３次元データＤＧのスケールとの差が小さくなるように、外部パラメータのヨー角αを変更する。本実施形態において、基準スケールは、ステレオカメラ３００とターゲットＴＧとの距離である基準距離ＺＦを含む。

ステレオカメラ３００は、ターゲットＴＧを含む撮影対象ＳＢを撮影する。ステレオ計測部１０２は、上述の実施形態に従って、視差画像データＳＧを生成する。視差画像データＳＧの生成において、ステレオ率がステレオ閾値未満のとき、上述の実施形態に従って、第１調整部１０３によりステレオ率の調整が実施される。ステレオ計測部１０２は、視差画像データＳＧに基づいて、３次元データＤＧを求める。

本実施形態において、視差画像データＳＧ又は３次元データＤＧは、ターゲットＴＧを含む。ステレオ計測部１０２は、視差画像データＳＧ又は３次元データＤＧに基づいて、ステレオカメラ３００からターゲットＴＧまでの距離ＺＰを算出する。

視差画像データＳＧ又は３次元データＤＧにおけるどの画素がターゲットＴＧに該当するか否かの判別は、例えば画像処理により基準マークの中心を判別することによって実施される。なお、ターゲットＴＧに該当する画素を判別することができれば、任意の方法を用いてもよい。例えば基準マークを用いなくてもターゲットＴＧの形状から判別してもよいし、所定の入力装置を用いてターゲットＴＧに該当する画素等のデータを指定してもよい。

第２調整部１０４は、位置データ算出部１０５で算出された基準距離ＺＦと、視差画像データＳＧ又は３次元データＤＧにおけるステレオカメラ３００とターゲットＴＧとの距離ＺＰとの差が小さくなるように、外部パラメータのヨー角αを変更する。

以上説明したように、本実施形態においては、絶対位置が既知であるターゲットＴＧを使って、３次元データＤＧのスケールを調整することができる。本実施形態によれば、第１ステレオカメラ３０１の第１カメラ３０Ａと第２カメラ３０Ｂとの相対位置、及び第２ステレオカメラ３０２の第３カメラ３０Ｃと第４カメラ３０Ｄとの相対位置が共に変動してしまった場合でも、第１ステレオカメラ３０１の自動較正処理、及び第２ステレオカメラ３０２の自動較正処理を個別に実施することができる。

例えば、第１ステレオカメラ３０１の第１カメラ３０Ａと第２カメラ３０Ｂとの相対位置が変動し、第１視差画像データＳＧ１のステレオ率が低下した場合、第１調整部１０３は、上述の実施形態に従って、ステレオ率を向上させる調整処理を実施することができる。ステレオ率が向上された後、そのステレオ率が向上された第１ステレオカメラ３０１でターゲットＴＧを撮影することにより、第２調整部１０４は、本実施形態に係るスケールの調整方法に従って、第１の３次元データＤＧ１のスケールを調整することができる。第２ステレオカメラ３０２についても同様である。

なお、本実施形態に係る位置計測方法は、油圧ショベル１が第１ステレオカメラ３０１しか有していない場合にも適用できる。

なお、本実施形態において、第１調整部１０３を用いてステレオ率を向上する処理を実施せずに、本実施形態に係る位置計測方法を用いて、第２調整部１０４がスケールを調整する処理を実施してもよい。

なお、本実施形態においては、ステレオカメラ３００の代わりにレーザースキャナを使用する場合、スケールが不正確になった場合でもスケールを調整できる。

第３実施形態．
第３実施形態について説明する。以下の説明において、上述の実施形態と同一又は同等の構成要素については同一の符号を付し、その説明を簡略又は省略する。

本実施形態においては、基準スケールとしてステレオカメラ３００の無限遠点に配置された基準物体を用いて、３次元データＤＧのスケールを調整する例について説明する。なお、本実施形態においても、上述の実施形態に従って、ステレオ率を向上する処理が実施される。

ステレオカメラ３００は、図１７に示した空に浮かぶ雲のような、ステレオカメラ３００の無限遠点に配置された基準物体を撮影する。第１カメラ３０Ｒで撮影された第１画像データＭＲ及び第２カメラ３０Ｌで撮影された第２画像データＭＬのそれぞれは、ステレオカメラ３００の無限遠点に配置された基準物体の画像データを含む。

外部較正処理によりスケールが正解値に調整されているステレオカメラ３００で無限遠点の基準物体の計測点Ｐを撮影した場合、視差ｄはゼロになる。一方、スケールが正解値ではないステレオカメラ３００で無限遠点の基準物体の計測点Ｐを撮影した場合、視差ｄはゼロにならない。

本実施形態において、基準スケールは、スケールが正解値に調整されているステレオカメラ３００においてステレオ計測された無限遠点の基準物体までの距離であり、すなわち無限遠を含む。

図２１は、本実施形態に係る位置計測方法の一例を説明するための模式図である。図２１は、例えばステレオカメラ３００によって、図１７に示したような無限遠点に存在する物体が占める割合が大きい撮影対象ＳＢを撮影したときの、撮影対象ＳＢの各計測点Ｐが投影された画素において生じた視差ｄと、視差画像データＳＧ全体における各画素において生じた視差ｄを積み上げた結果である頻度との関係を示すヒストグラム図である。図２１に示すグラフにおいて、横軸は、視差ｄを示し、縦軸は、視差ｄの頻度を示す。

撮影対象ＳＢにおいて、雲のような無限遠点に存在する物体の割合が大きい場合、図２１に示すように、無限遠点に対応する視差ｄにおいて、その頻度のピーク値Δｄが発生する。外部較正処理によりスケールが正解値に調整されているステレオカメラ３００で無限遠点の物体を撮影した場合、視差ｄの頻度は、視差ｄがゼロのときにピーク値Δｄを示す。一方、スケールが正解値でないステレオカメラ３００で無限遠点の物体を撮影した場合、視差ｄの頻度は、視差ｄがゼロでないときにピーク値Δｄを示す。

第２調整部１０４は、第１画像データＭＲにおける基準物体（本実施形態においては無限遠点）の画像データと第２画像データＭＬにおける基準物体の画像データとの視差ｄが、基準視差ｄ_０（本実施形態ではゼロ）と一致するように、外部パラメータのヨー角αを変更する。すなわち、第２調整部１０４は、無限遠点の基準物体を撮影したときの視差ｄのピークがゼロでない場合、その視差ｄのピークがゼロになるように、外部パラメータのヨー角αを変更する。

図２２は、本実施形態に係る第２調整部１０４によるスケールの調整方法の一例を説明するための模式図である。図２２に示すグラフにおいて、横軸は、外部パラメータのヨー角αを示し、縦軸は、無限遠点に存在する物体の計測点Ｐを撮影したときの視差ｄのピーク値Δｄ、すなわち図１７に示したような無限遠点に存在する物体が占める割合が大きい撮影対象ＳＢを撮影したときの視差ｄの頻度がピークを示している視差ｄのピーク値Δｄを示す。

第２調整部１０４は、基準ヨー角α_０を基準として、所定範囲において、ヨー角αを所定角度ずつ変更する。ステレオ計測部１０２は、変更されたヨー角αのそれぞれにおいて、視差ｄの頻度におけるピーク値を算出する。図２２に示すように、視差画像データＳＧの各画素における視差ｄは、ヨー角αの変更に基づいて変化する。第２調整部１０４は、所定範囲において基準視差ｄ_０と視差ｄとの差の最小値が得られたときのヨー角αを正解ヨー角αｒに決定する。すなわち、第２調整部１０４は、変更したヨー角αにおける視差ｄの頻度のピーク値が最もゼロに近いヨー角αを正解ヨー角αｒに決定する。なお、第２調整部１０４は、所定範囲において基準視差ｄ_０と視差ｄとの差が視差閾値以下になったときのヨー角αを正解ヨー角αｒに決定してもよい。視差閾値は、基準視差ｄ_０と視差ｄとの差について規定された閾値である。

以上説明したように、本実施形態においては、無限遠点に存在する物体を使って、３次元データＤＧのスケールを調整することができる。本実施形態においても、第１ステレオカメラ３０１の第１カメラ３０Ａと第２カメラ３０Ｂとの相対位置、及び第２ステレオカメラ３０２の第３カメラ３０Ｃと第４カメラ３０Ｄとの相対位置の両方が変動してしまった場合でも、第１ステレオカメラ３０１の自動較正処理、及び第２ステレオカメラ３０２の自動較正処理を個別に実施することができる。

なお、本実施形態に係る位置計測方法は、油圧ショベル１が第１ステレオカメラ３０１しか有していない場合にも適用できる。

なお、本実施形態において、第１調整部１０３を用いてステレオ率を向上する処理を実施せずに、本実施形態に係る位置計測方法を用いて、第２調整部１０４がスケールを調整する処理を実施してもよい。

なお、本実施形態においては、ステレオカメラ３００の代わりにレーザースキャナを使用する場合、スケールが不正確になった場合でもスケールを調整できる。

第４実施形態．
第４実施形態について説明する。以下の説明において、上述の実施形態と同一又は同等の構成要素については同一の符号を付し、その説明を簡略又は省略する。

上述の実施形態においては、第１調整部１０３及び第２調整部１０４によって変更されるパラメータが、第１カメラＭＲと第２カメラＭＬとの相対位置を規定する外部パラメータであることとした。

本実施形態においては、第１調整部１０３及び第２調整部１０４によって変更されるパラメータが、第１カメラ３０Ｒ及び第２カメラ３０Ｌそれぞれの内部パラメータである例について説明する。

内部パラメータは、第１カメラ３０Ｒ及び第２カメラ３０Ｌそれぞれの固有データを規定する。内部パラメータは、例えば、カメラ３０の光学系の焦点距離、及びカメラ３０の光学系の光学中心とイメージセンサの撮像面との交点と、イメージセンサにおける画像中心との位置ずれ量を含む。

本実施形態において、内部パラメータは、Ｘｓ軸及びＹｓ軸と平行な方向の第２カメラ３０Ｌの光学系の光軸に対する第２カメラ３０Ｌのイメージセンサ３１Ｌの位置を含む。すなわち、本実施形態において、内部パラメータは、Ｘｓ軸方向及びＹｓ軸方向における画像中心の位置を含む。

例えば外的要因により、カメラ３０の内部においてイメージセンサが動いてしまう可能性がある。例えば熱の影響により、イメージセンサの位置が変動する可能性がある。イメージセンサの位置が変動すると、カメラ３０の光学系の光軸とイメージセンサの撮像面との交点を示す画像中心の位置が変動する。画像中心の位置が変動すると、視差画像データＳＧのステレオ率が低下したり、３次元データＤＧのスケールとスケールの正解値との差が大きくなったりする可能性がある。

本実施形態において、第１調整部１０３は、ステレオ率を調整するために、内部パラメータとして、Ｘｓ軸方向のイメージセンサ３１Ｌの位置を変更して、Ｘｓ軸方向における画像中心の位置を調整する。

本実施形態において、第２調整部１０４は、スケールを調整するために、内部パラメータとして、Ｙｓ軸方向のイメージセンサ３１Ｌの位置を変更して、Ｙｓ軸方向における画像中心の位置を調整する。

カメラ３０の固有データは、上述の複数の内部パラメータを含む下記の（４）式で規定される。

（４）式において、（Ｘ，Ｙ，Ｚ）は、グローバル座標系における第２カメラ３０Ｌの画像中心の位置を示す。ｓは、カメラ座標系におけるＺｓ軸方向の距離又はスケールを示す。ｕは、カメラ座標系におけるＸｓ軸方向の位置を示す。ｆは、焦点距離を示す。ｓｕは、Ｘｓ軸方向の画素比率（Ｙｓ軸方向に対する比）を示す。ｓｖは、カメラ座標系におけるＹｓ軸方向の画素比率（Ｘｓ軸方向に対する比）を示す。ｕ_０は、カメラ座標系におけるＸｓ軸方向の第２カメラ３０Ｌの画像中心の位置を示す。ｖ_０は、カメラ座標系におけるＹｓ軸方向の第２カメラ３０Ｌの画像中心の位置を示す。

（４）式は、第２カメラ３０Ｌに関するヨー角α、ピッチ角β、ロール角γ、位置ｕ_０、及び位置ｖ_０によって規定されるカメラ内部行列と、第２カメラ３０Ｌの位置及び姿勢を示す複数のパラメータによって規定されるカメラ外部行列と、グローバル座標系における位置Ｘ、位置Ｙ、及び位置Ｚのそれぞれによって規定される平行移動ベクトルとを含む。（４）式は、第２カメラ３０Ｌのカメラ座標系におけるＸｓ軸方向の画像中心の位置ｕ_０及びＹｓ軸方向の画像中心の位置ｖ_０を第１カメラ３０Ｒのカメラ座標系におけるＸｓ軸方向の画像中心の位置ｕ及びＹｓ軸方向の画像中心の位置ｖに変換する変換式である。

第１調整部１０３は、ステレオ率が大きくなるように、位置ｕ_０を変更する。位置ｕ_０を変更することは、上述の実施形態で説明した、ピッチ角βを変更することと近似する。

本実施形態において、記憶部１０９は、Ｘｓ軸方向のイメージセンサ３１Ｌの基準位置を記憶する。Ｘｓ軸方向のイメージセンサ３１Ｌの基準位置は、Ｘｓ軸方向の第２カメラ３０Ｌの画像中心の基準位置を含む。Ｘｓ軸方向の基準位置は、工場出荷時の外部較正処理において、ステレオ率がステレオ閾値以上になるように調整された画像中心の位置である。第１調整部１０３は、Ｘｓ軸方向の基準位置を含む所定範囲においてイメージセンサ３１Ｌの位置を変更して、ステレオ率の最大値を算出する。

第２調整部１０４は、基準スケールと３次元データＤＧのスケールとの差が小さくなるように、位置ｖ_０を変更する。位置ｖ_０を変更することは、上述の実施形態で説明した、ヨー角αを変更することと近似する。

本実施形態において、記憶部１０９は、Ｙｓ軸方向のイメージセンサ３１Ｌの基準位置を記憶する。Ｙｓ軸方向のイメージセンサ３１Ｌの基準位置は、Ｙｓ軸方向の第２カメラ３０Ｌの画像中心の基準位置を含む。Ｙｓ軸方向の基準位置は、工場出荷時の外部較正処理において、基準スケールと３次元データＤＧのスケールとの差がスケール閾値以下になるように調整された画像中心の位置である。第２調整部１０４は、Ｙｓ軸方向の基準位置を含む所定範囲においてイメージセンサ３１Ｌの位置を変更して、基準スケールと３次元データＤＧのスケールとの差の最小値を算出する。

以上説明したように、第１調整部１０３及び第２調整部１０４は、カメラ３０の内部パラメータを調整することによって、視差画像データＳＧのステレオ率を向上させ、３次元データＤＧのスケールを正解値に近付けることができる。

第５実施形態．
第５実施形態について説明する。以下の説明において、上述の実施形態と同一又は同等の構成要素については同一の符号を付し、その説明を簡略又は省略する。

図２３は、本実施形態に係る位置計測システム５００の一例を模式的に示す図である。本実施形態において、油圧ショベル１の制御装置１００は、通信回線５０２を介して、油圧ショベル１の遠隔地に設けられているサーバ５０１及び端末装置５０３とデータ通信可能である。サーバ５０１は、コンピュータシステムを含む。端末装置５０３は、パーソナルコンピュータ又は携帯端末を含む。通信回線５０２は、インターネット（internet）、ローカルエリアネットワーク（ＬＡＮ：Local Area Network）、携帯電話通信網、及び衛星通信網の少なくとも一つを含む。

本実施形態において、サーバ５０１は、制御装置１００の一部又は全部の機能を有する。すなわち、サーバ５０１は、画像データ取得部１０１、ステレオ計測部１０２、第１調整部１０３、第２調整部１０４、位置データ算出部１０５、作業機位置データ算出部１０６、マップデータ作成部１０７、表示制御部１０８、記憶部１０９、及び入出力部１１０の少なくとも一つを有する。

油圧ショベル１のカメラ３０で撮像された画像データ、位置検出器２３で検出された旋回体３の位置データ、姿勢検出器２４で検出された旋回体３の姿勢データ、及び方位検出器２５で検出された旋回体３の方位データが、通信回線５０２を介して、サーバ５０１に供給される。

例えば、サーバ５０１が、ステレオ計測部１０２、第１調整部１０３、第２調整部１０４、及び記憶部１０９を有する場合、サーバ５０１は、カメラ３０で撮影され通信回線５０２を介して供給された画像データに基づいて、ステレオ計測を実施したり、ステレオ率が大きくなるようにパラメータを変更したり、スケールが正解値になるようにパラメータを変更したりすることができる。

なお、本実施形態において、端末装置５０３が、制御装置１００の一部または全部の機能を有してもよい。すなわち、端末装置５０３が、画像データ取得部１０１、ステレオ計測部１０２、第１調整部１０３、第２調整部１０４、位置データ算出部１０５、作業機位置データ算出部１０６、マップデータ作成部１０７、表示制御部１０８、記憶部１０９、及び入出力部１１０の少なくとも一つを有してもよい。

なお、本実施形態において、表示制御部１０８に制御される表示装置２６は、油圧ショベル１に設けられてもよいし、サーバ５０１に接続されてもよいし、端末装置５０３に接続されてもよい。

第６実施形態．
第６実施形態について説明する。以下の説明において、上述の実施形態と同一又は同等の構成要素については同一の符号を付し、その説明を簡略又は省略する。

上述の第２実施形態においては、ターゲットＴＧを用いて３次元データＤＧのスケールを調整することとした。本実施形態においては、基準スケールとして作業機２を用いて、３次元データＤＧのスケールを調整する例について説明する。なお、本実施形態においても、上述の実施形態に従って、ステレオ率を向上する処理が実施される。

図２４は、本実施形態に係る位置計測方法の一例を説明するための模式図である。本実施形態においては、基準物体であるバケット８の刃先８ＢＴがステレオカメラ３００の撮影領域に配置される。例えば、Ｘｍ軸方向におけるステレオカメラ３００と刃先８ＢＴとの距離が最も長くなるように作業機２の姿勢が調整された状態で、刃先８ＢＴがステレオカメラ３００の撮影領域に配置される。

作業機位置データ算出部１０６は、グローバル座標系におけるバケット８の刃先８ＢＴの位置を算出可能である。また、位置データ算出部１０５は、グローバル座標系におけるカメラ３０の位置データを算出可能である。

位置データ算出部１０５は、グローバル座標系におけるステレオカメラ３００と刃先８ＢＴとの相対位置データを算出する。

第２調整部１０４は、基準スケールと３次元データＤＧのスケールとの差が小さくなるように、外部パラメータのヨー角αを変更する。本実施形態において、基準スケールは、ステレオカメラ３００と刃先８ＢＴとの距離である基準距離ＺＦを含む。

以上説明したように、本実施形態においては、作業機２を使って、３次元データＤＧのスケールを調整することができる。

なお、本本実施形態において、油圧ショベル１とは別の油圧ショベルの作業機が基準スケールとして利用されてもよい。別の油圧ショベルが、本実施形態に係る油圧ショベル１と同様に、作業機角度検出器２２、位置検出器２３、姿勢検出器２４、方位検出器２５、及び制御装置１００を有していれば、別の油圧ショベルは、別の油圧ショベルの作業機のバケットの刃先の絶対位置を取得することができる。その別の油圧ショベルの刃先の絶対位置を示す絶対位置データが、通信装置を介して本実施形態に係る油圧ショベル１に送信されることにより、油圧ショベル１は、ステレオカメラ３００と別の油圧ショベルの刃先との相対位置データを取得することができる。

なお、上述の各実施形態においては、油圧ショベル１にカメラ３０が４つ、及びステレオカメラ３００が２つ設けられる例について説明した。カメラ３０は、油圧ショベル１に少なくとも２つ設けられていればよい。また、上述の第２実施形態及び第３実施形態においては、ステレオカメラ３００は少なくとも１つ設けられていればよい。

なお、上述の各実施形態においては、ステレオ率は、ステレオ計測される視差画像データＳＧの複数の画素のうちステレオ計測が成功した画素の割合を示すこととした。ステレオ率は、他の定義でもよい。例えば、ステレオ率は、ステレオ計測される視差画像データの複数の画素のうちステレオ計測が成功した画素の総数を示すこととしてもよい。

なお、上述の各実施形態においては、３次元データのスケール調整として、ヨー角などを調整、つまり視差ｄを調整することによりスケールを変更するようにしたが、その実施形態に限られず、その他の調整、例えば数１におけるｂ（カメラ間距離）やｆ（焦点距離）を調整することによりスケールを変更することとしてもよい。

なお、上述の各実施形態においては、作業機械１が油圧ショベルであることとした。作業機械１は、施工対象を施工可能な作業機械であればよく、施工対象を掘削可能な掘削機械及び土砂を運搬可能な運搬機械でもよい。作業機械１は、例えばホイールローダでもよいし、ブルドーザでもよいし、ダンプトラックでもよい。

１…油圧ショベル（作業機械）、１Ｂ…車体、２…作業機、３…旋回体、４…運転室、４Ｓ…運転席、５…走行体、５Ａ…履帯、５Ｂ…履帯、６…ブーム、７…アーム、８…バケット、８ＢＴ…刃先、１０…ブームシリンダ、１１…アームシリンダ、１２…バケットシリンダ、１３…ブームピン、１４…アームピン、１５…バケットピン、１６…ブームストロークセンサ、１７…アームストロークセンサ、１８…バケットストロークセンサ、２０…検出システム、２１…ＧＰＳアンテナ、２２…作業機角度検出器、２３…位置検出器、２４…姿勢検出器、２５…方位検出器、２６…表示装置、３０…カメラ、３０Ａ…第１カメラ、３０Ｂ…第２カメラ、３０Ｃ…第１カメラ、３０Ｄ…第２カメラ、３０Ｌ…第２カメラ、３０Ｒ…第１カメラ、３１Ｌ…イメージセンサ、３１Ｒ…イメージセンサ、３５…操作装置、３５Ｌ…左操作レバー、３５Ｒ…右操作レバー、５０…位置計測システム、１００…制御装置、１０１…画像データ取得部、１０２…ステレオ計測部、１０３…第１調整部、１０４…第２調整部、１０５…位置データ算出部、１０６…作業機位置データ算出部、１０７…マップデータ作成部、１０８…表示制御部、１０９…記憶部、１１０…入出力部、３００…ステレオカメラ、３０１…第１ステレオカメラ、３０２…第２ステレオカメラ、５００…位置計測システム、５０１…サーバ、５０２…通信回線、ＡＸ１…回転軸、ＡＸ２…回転軸、ＡＸ３…回転軸、ＤＧ…３次元データ、ＥＲ…点、ＥＬ…点、ＢＬ…ベースライン、ＭＲ…第１画像データ、ＭＬ…第２画像データ、ＯＲ…光学中心、ＯＬ…光学中心、Ｐ…計測点、ＳＢ…撮影対象、ＳＧ…視差画像データ、Ｚｒ…旋回軸。

Claims (11)

1. 作業機械に設けられた第１ステレオカメラの第１カメラで撮影された第１画像データ及び前記第１ステレオカメラの第２カメラで撮影された第２画像データを取得する画像データ取得部と、
   前記第１画像データと、前記第２画像データと、前記第１カメラ及び前記第２カメラに関するパラメータとに基づいて、ステレオ計測を実施するステレオ計測部と、
   前記パラメータの少なくとも一部を変更して、ステレオ計測された第１視差画像データのステレオ率を調整する第１調整部と、
   前記パラメータの少なくとも一部を変更して、前記第１視差画像データから求められる第１の３次元データのスケールを調整する第２調整部と、
   を備える位置計測システム。
2. 前記第２調整部は、前記第１視差画像データの視差を調整して、前記第１の３次元データのスケールを調整する、
   請求項１に記載の位置計測システム。
3. 前記パラメータは、前記第１カメラと前記第２カメラとの相対位置を規定する外部パラメータを含み、
   前記外部パラメータは、前記第２カメラのピッチ角を含み、
   前記第１調整部は、前記ピッチ角を変更する、
   請求項１又は請求項２に記載の位置計測システム。
4. 前記パラメータは、前記第１カメラと前記第２カメラとの相対位置を規定する外部パラメータを含み、
   前記外部パラメータは、前記第２カメラのヨー角を含み、
   前記第２調整部は、前記ヨー角を変更する、
   請求項１から請求項３のいずれか一項に記載の位置計測システム。
5. 第３カメラ及び第４カメラからなる第２ステレオカメラをさらに備え、
   前記第２調整部は、前記第３カメラから取得された第３画像データ及び前記第４カメラから取得された第４画像データに基づいて生成された第２の３次元データのスケールと前記第１の３次元データのスケールとの差が小さくなるように、前記第１の３次元データのスケールを調整する、
   請求項１から請求項４のいずれか一項に記載の位置計測システム。
6. 前記第２調整部は、前記第１ステレオカメラの位置データと基準物体の位置データとに基づいて算出される前記第１ステレオカメラと基準物体との基準距離と、前記第１の３次元データにおける前記第１ステレオカメラと前記基準物体との距離との差が小さくなるように、前記パラメータを変更する、
   請求項１から請求項４のいずれか一項に記載の位置計測システム。
7. 前記第１画像データ及び前記第２画像データのそれぞれは、前記第１ステレオカメラの無限遠点に配置された基準物体の画像データを含み、
   前記第２調整部は、前記第１画像データ及び前記第２画像データに基づき計測された第１視差画像データにおける視差のピークがゼロとなるように、前記パラメータを変更する、
   請求項１から請求項４のいずれか一項に記載の位置計測システム。
8. 作業機械に設けられたステレオカメラの第１カメラで撮影された第１画像データ及び前記ステレオカメラの第２カメラで撮影された第２画像データを取得する画像データ取得部と、
   前記第１画像データと、前記第２画像データと、前記第１カメラ及び前記第２カメラに関するパラメータとに基づいて、ステレオ計測を実施するステレオ計測部と、
   前記ステレオカメラの絶対位置データと基準物体の絶対位置データとに基づいて算出される前記ステレオカメラと前記基準物体との基準距離と、前記ステレオ計測部で生成された視差画像データにおける前記ステレオカメラと前記基準物体との距離との差が小さくなるように、前記パラメータの少なくとも一部を変更する第２調整部と、
   を備える位置計測システム。
9. 作業機械に設けられたステレオカメラの第１カメラで撮影された前記ステレオカメラの無限遠点に配置された基準物体を含む第１画像データ及び前記ステレオカメラの第２カメラで撮影された前記基準物体を含む第２画像データを取得する画像データ取得部と、
   前記第１画像データと、前記第２画像データと、前記第１カメラ及び前記第２カメラに関するパラメータとに基づいて、ステレオ計測を実施するステレオ計測部と、
   前記第１画像データ及び前記第２画像データに基づき計測された第１視差画像データにおける視差のピークがゼロとなるように、前記パラメータの少なくとも一部を変更する第２調整部と、
   を備える位置計測システム。
10. 請求項１から請求項９のいずれか一項に記載の位置計測システムを備える作業機械。
11. 作業機械に設けられた第１ステレオカメラの第１カメラで撮影された第１画像データ及び前記第１ステレオカメラの第２カメラで撮影された第２画像データを取得することと、
    前記第１画像データと、前記第２画像データと、前記第１カメラ及び前記第２カメラに関するパラメータとに基づいて、ステレオ計測を実施することと、
    前記パラメータの少なくとも一部を変更して、ステレオ計測された第１視差画像データのステレオ率を調整することと、
    前記パラメータの少なくとも一部を変更して、前記第１視差画像データから求められる３次元データのスケールを調整することと、
    を含む位置計測方法。