知財求人 - 知財ポータルサイト「IP Force」
特開2023-10052作業機械の制御システム、作業機械、及び作業機械の制御方法
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2023010052
(43)【公開日】2023-01-20
(54)【発明の名称】作業機械の制御システム、作業機械、及び作業機械の制御方法
(51)【国際特許分類】
E02F 9/26 20060101AFI20230113BHJP
G01C 11/02 20060101ALI20230113BHJP
G01C 15/06 20060101ALI20230113BHJP
G01C 15/00 20060101ALI20230113BHJP
【ＦＩ】
E02F9/26 A
G01C11/02
G01C15/06 T
G01C15/00 104D
【審査請求】未請求
【請求項の数】20
【出願形態】ＯＬ
(21)【出願番号】P 2021113849
(22)【出願日】2021-07-08
(71)【出願人】
【識別番号】000001236
【氏名又は名称】株式会社小松製作所
(74)【代理人】
【識別番号】110002147
【氏名又は名称】弁理士法人酒井国際特許事務所
(72)【発明者】
【氏名】厚見 彰吾
(72)【発明者】
【氏名】園山 昌司
(72)【発明者】
【氏名】峯後 俊秀
(72)【発明者】
【氏名】菅原 大樹
(72)【発明者】
【氏名】松田 豊久
【テーマコード（参考）】
2D015
【Ｆターム（参考）】
2D015HA03
(57)【要約】
【課題】ＧＮＳＳの測位不良が発生した場合において、作業機械の位置及び方位角を算出すること。
【解決手段】走行体及び旋回体を備える作業機械の制御システムは、作業機械の外部に設置された複数のターゲットの画像と、旋回体の傾斜角とに基づいて、旋回体の位置及び方位角を算出する位置方位算出部を備える。
【選択図】図６
【解決手段】走行体及び旋回体を備える作業機械の制御システムは、作業機械の外部に設置された複数のターゲットの画像と、旋回体の傾斜角とに基づいて、旋回体の位置及び方位角を算出する位置方位算出部を備える。
【選択図】図６
【特許請求の範囲】
【請求項1】
走行体及び旋回体を備える作業機械の制御システムであって、
前記作業機械の外部に設置された複数のターゲットの画像と、前記旋回体の傾斜角とに基づいて、前記旋回体の位置及び方位角を算出する位置方位算出部を備える、
作業機械の制御システム。
前記作業機械の外部に設置された複数のターゲットの画像と、前記旋回体の傾斜角とに基づいて、前記旋回体の位置及び方位角を算出する位置方位算出部を備える、
作業機械の制御システム。
【請求項2】
複数の前記ターゲットのそれぞれの３次元位置を記憶する記憶部を備え、
前記位置方位算出部は、前記ターゲットの３次元位置と、前記画像における前記ターゲットの２次元位置と、前記旋回体の傾斜角とに基づいて、前記旋回体の位置及び方位角を算出する、
請求項１に記載の作業機械の制御システム。
前記位置方位算出部は、前記ターゲットの３次元位置と、前記画像における前記ターゲットの２次元位置と、前記旋回体の傾斜角とに基づいて、前記旋回体の位置及び方位角を算出する、
請求項１に記載の作業機械の制御システム。
【請求項3】
前記旋回体に配置され前記ターゲットを撮像する撮像装置を備え、
前記位置方位算出部は、前記撮像装置から前記画像を取得する、
請求項２に記載の作業機械の制御システム。
前記位置方位算出部は、前記撮像装置から前記画像を取得する、
請求項２に記載の作業機械の制御システム。
【請求項4】
前記位置方位算出部は、前記ターゲットの３次元位置と、前記ターゲットの２次元位置と、前記旋回体の傾斜角とに基づいて、現場座標系における前記撮像装置の位置及び方位角を算出し、前記撮像装置の位置及び方位角に基づいて、前記旋回体の位置及び方位角を算出する、
請求項３に記載の作業機械の制御システム。
請求項３に記載の作業機械の制御システム。
【請求項5】
前記ターゲットの３次元位置及び２次元位置のそれぞれは、前記ターゲットに規定された基準点の３次元位置及び２次元位置を含む、
請求項２から請求項４のいずれか一項に記載の作業機械の制御システム。
請求項２から請求項４のいずれか一項に記載の作業機械の制御システム。
【請求項6】
前記ターゲットは、前記ターゲットの基準点から放射方向に伸びる放射マークを含み、
前記位置方位算出部は、前記放射マークに基づいて、前記基準点の２次元位置を算出する、
請求項５に記載の作業機械の制御システム。
前記位置方位算出部は、前記放射マークに基づいて、前記基準点の２次元位置を算出する、
請求項５に記載の作業機械の制御システム。
【請求項7】
前記ターゲットは、識別マークを含み、
前記記憶部は、前記識別マークにより規定される識別データと前記ターゲットの３次元位置との関係を示す相関データを記憶し、
前記位置方位算出部は、前記画像における前記識別マークに基づいて、前記記憶部から前記ターゲットの３次元位置を取得する、
請求項２から請求項６のいずれか一項に記載の作業機械の制御システム。
前記記憶部は、前記識別マークにより規定される識別データと前記ターゲットの３次元位置との関係を示す相関データを記憶し、
前記位置方位算出部は、前記画像における前記識別マークに基づいて、前記記憶部から前記ターゲットの３次元位置を取得する、
請求項２から請求項６のいずれか一項に記載の作業機械の制御システム。
【請求項8】
前記旋回体に配置された傾斜センサと、
前記傾斜センサの検出データに基づいて前記旋回体の傾斜角を算出する傾斜角算出部と、を備え、
前記位置方位算出部は、前記傾斜角算出部から前記旋回体の傾斜角を取得する、
請求項１から請求項７のいずれか一項に記載の作業機械の制御システム。
前記傾斜センサの検出データに基づいて前記旋回体の傾斜角を算出する傾斜角算出部と、を備え、
前記位置方位算出部は、前記傾斜角算出部から前記旋回体の傾斜角を取得する、
請求項１から請求項７のいずれか一項に記載の作業機械の制御システム。
【請求項9】
前記位置方位算出部は、前記旋回体の位置及び方位角を算出した後において、前記旋回体が旋回動作した場合、少なくとも１つのターゲットの画像に基づいて、前記旋回体の位置及び方位角を算出する、
請求項１から請求項８のいずれか一項に記載の作業機械の制御システム。
請求項１から請求項８のいずれか一項に記載の作業機械の制御システム。
【請求項10】
前記位置方位算出部は、前記旋回体の位置及び方位角を算出した後において、前記旋回体が旋回動作した場合、前記旋回体の旋回を検出する旋回センサの検出データに基づいて、前記旋回体の位置及び方位角を算出する、
請求項１から請求項８のいずれか一項に記載の作業機械の制御システム。
請求項１から請求項８のいずれか一項に記載の作業機械の制御システム。
【請求項11】
請求項１から請求項１０のいずれか一項に記載の作業機械の制御システムを備える、
作業機械。
作業機械。
【請求項12】
走行体及び旋回体を備える作業機械の制御方法であって、
前記作業機械の外部に設置された複数のターゲットの画像を取得することと、
前記旋回体の傾斜角を取得することと、
複数の前記ターゲットの画像と前記旋回体の傾斜角とに基づいて、前記旋回体の位置及び方位角を算出することと、を含む、
作業機械の制御方法。
前記作業機械の外部に設置された複数のターゲットの画像を取得することと、
前記旋回体の傾斜角を取得することと、
複数の前記ターゲットの画像と前記旋回体の傾斜角とに基づいて、前記旋回体の位置及び方位角を算出することと、を含む、
作業機械の制御方法。
【請求項13】
前記ターゲットの３次元位置を取得することと、
前記画像に基づいて前記画像における前記ターゲットの２次元位置を算出することと、を含み、
前記旋回体の位置及び方位角は、前記ターゲットの３次元位置と、前記ターゲットの２次元位置と、前記旋回体の傾斜角とに基づいて、算出される、
請求項１２に記載の作業機械の制御方法。
前記画像に基づいて前記画像における前記ターゲットの２次元位置を算出することと、を含み、
前記旋回体の位置及び方位角は、前記ターゲットの３次元位置と、前記ターゲットの２次元位置と、前記旋回体の傾斜角とに基づいて、算出される、
請求項１２に記載の作業機械の制御方法。
【請求項14】
複数の前記ターゲットの画像は、前記旋回体に配置された撮像装置により取得され、
前記ターゲットの３次元位置と、前記ターゲットの２次元位置と、前記旋回体の傾斜角とに基づいて、現場座標系における前記撮像装置の位置及び方位角を算出することを含み、
前記旋回体の位置及び方位角は、前記撮像装置の位置及び方位角に基づいて、算出される、
請求項１３に記載の作業機械の制御方法。
前記ターゲットの３次元位置と、前記ターゲットの２次元位置と、前記旋回体の傾斜角とに基づいて、現場座標系における前記撮像装置の位置及び方位角を算出することを含み、
前記旋回体の位置及び方位角は、前記撮像装置の位置及び方位角に基づいて、算出される、
請求項１３に記載の作業機械の制御方法。
【請求項15】
前記ターゲットの３次元位置及び２次元位置のそれぞれは、前記ターゲットに規定された基準点の３次元位置及び２次元位置を含む、
請求項１２から請求項１４のいずれか一項に記載の作業機械の制御方法。
請求項１２から請求項１４のいずれか一項に記載の作業機械の制御方法。
【請求項16】
前記ターゲットは、前記ターゲットの基準点から放射方向に伸びる放射マークを含み、
前記放射マークに基づいて、前記基準点の２次元位置が算出される、
請求項１５に記載の作業機械の制御方法。
前記放射マークに基づいて、前記基準点の２次元位置が算出される、
請求項１５に記載の作業機械の制御方法。
【請求項17】
前記ターゲットは、識別マークを含み、
前記画像における前記識別マークと、前記識別マークにより規定される識別データと前記ターゲットの３次元位置との関係を示す相関データとに基づいて、前記ターゲットの３次元位置が取得される、
請求項１２から請求項１６のいずれか一項に記載の作業機械の制御方法。
前記画像における前記識別マークと、前記識別マークにより規定される識別データと前記ターゲットの３次元位置との関係を示す相関データとに基づいて、前記ターゲットの３次元位置が取得される、
請求項１２から請求項１６のいずれか一項に記載の作業機械の制御方法。
【請求項18】
前記旋回体の傾斜角は、前記旋回体に配置された傾斜センサの検出データに基づいて算出される、
請求項１２から請求項１６のいずれか一項に記載の作業機械の制御方法。
請求項１２から請求項１６のいずれか一項に記載の作業機械の制御方法。
【請求項19】
前記旋回体の位置及び方位角を算出した後において、前記旋回体が旋回動作した場合、少なくとも１つのターゲットの画像に基づいて、前記旋回体の位置及び方位角が算出される、
請求項１２から請求項１８のいずれか一項に記載の作業機械の制御方法。
請求項１２から請求項１８のいずれか一項に記載の作業機械の制御方法。
【請求項20】
前記旋回体の位置及び方位角を算出した後において、前記旋回体が旋回動作した場合、前記旋回体の旋回を検出する旋回センサの検出データに基づいて、前記旋回体の位置及び方位角が算出される、
請求項１２から請求項１８のいずれか一項に記載の作業機械の制御方法。
請求項１２から請求項１８のいずれか一項に記載の作業機械の制御方法。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、作業機械の制御システム、作業機械、及び作業機械の制御方法に関する。
【背景技術】
【0002】
作業機械に係る技術分野において、特許文献１に開示されているような、目標施工面に基づいて掘削対象を掘削する技術が知られている。目標施工面に基づいて掘削対象を掘削する技術として、目標施工面と作業機との相対位置を示すガイダンス画像を作業機械のオペレータに提示するマシンガイダンス技術と、目標施工面に従って作業機が動作するようにオペレータの操作をアシスト制御するマシンコントロール技術とが知られている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】国際公開第２０１５／１６７０２２号
【発明の概要】
【発明が解決しようとする課題】
【0004】
目標施工面に基づいて掘削対象を掘削する場合、作業機械の位置及び方位角を算出する必要がある。作業機械の位置及び方位角は、全球測位衛星システム（ＧＮＳＳ：Global Navigation Satellite System）を利用して算出される。ＧＮＳＳの測位不良が発生すると、作業機械の位置及び方位角を算出することが困難となる。
【0005】
本開示は、ＧＮＳＳの測位不良が発生した場合において、作業機械の位置及び方位角を算出することを目的とする。
【課題を解決するための手段】
【0006】
本開示に従えば、走行体及び旋回体を備える作業機械の制御システムであって、作業機械の外部に設置された複数のターゲットの画像と、旋回体の傾斜角とに基づいて、旋回体の位置及び方位角を算出する位置方位算出部を備える、作業機械の制御システムが提供される。
【発明の効果】
【0007】
本開示によれば、ＧＮＳＳの測位不良が発生した場合において、作業機械の位置及び方位角が算出される。
【図面の簡単な説明】
【0008】
【発明を実施するための形態】
【0009】
以下、本開示に係る実施形態について図面を参照しながら説明するが、本開示は実施形態に限定されない。以下で説明する実施形態の構成要素は適宜組み合わせることができる。また、一部の構成要素を用いない場合もある。
【0010】
【0011】
作業機械１は、作業現場において稼働する。実施形態において、作業機械１は、油圧ショベルである。以下の説明において、作業機械１を適宜、油圧ショベル１、と称する。
【0012】
油圧ショベル１は、走行体３と、旋回体４と、作業機５と、油圧シリンダ６と、操作装置７と、車載モニタ８と、位置センサ９と、傾斜センサ１０と、撮像装置１１と、制御装置１２とを備える。
【0013】
図２に示すように、作業現場に３次元の現場座標系（Ｘｇ，Ｙｇ，Ｚｇ）が規定される。旋回体４に３次元の車体座標系（Ｘｍ，Ｙｍ，Ｚｍ）が規定される。撮像装置１１に３次元のカメラ座標系（Ｘｃ，Ｙｃ，Ｚｃ）が規定される。
【0014】
現場座標系は、作業現場に規定された現場基準点Ｏｇから南北に伸びるＸｇ軸、現場基準点Ｏｇから東西に伸びるＹｇ軸、及び現場基準点Ｏｇから上下に伸びるＺｇ軸により構成される。
【0015】
車体座標系は、旋回体４に規定された代表点Ｏｍから旋回体４の前後方向に伸びるＸｍ軸、代表点Ｏｍから旋回体４の左右方向に伸びるＹｍ軸、及び代表点Ｏｍから旋回体４の上下方向に伸びるＺｍ軸により構成される。旋回体４の代表点Ｏｍを基準として、＋Ｘｍ方向は旋回体４の前方であり、－Ｘｍ方向は旋回体４の後方であり、＋Ｙｍ方向は旋回体４の左方であり、－Ｙｍ方向は旋回体４の右方であり、＋Ｚｍ方向は旋回体４の上方であり、－Ｚｍ方向は旋回体４の下方である。
【0016】
カメラ座標系は、撮像装置１１を構成する１つのカメラ１３の光学中心Ｏｃからカメラ１３の幅方向に伸びるＸｃ軸、光学中心Ｏｃからカメラ１３の上下方向に伸びるＹｃ軸、及び光学中心Ｏｃからカメラ１３の光学系の光軸に平行な方向に伸びるＺｃ軸により構成される。
【0017】
走行体３は、旋回体４を支持した状態で走行する。走行体３は、一対の履帯３Ａを有する。履帯３Ａの回転により、走行体３は、走行動作する。走行体３の走行動作は、前進動作及び後進動作を含む。油圧ショベル１は、走行体３により作業現場を移動することができる。
【0018】
旋回体４は、走行体３に支持される。旋回体４は、走行体３よりも上方に配置される。旋回体４は、走行体３に支持された状態で旋回軸ＲＸを中心に旋回動作する。旋回軸ＲＸは、Ｚｍ軸に平行である。旋回体４の旋回動作は、左旋回動作及び右旋回動作を含む。運転室２は、旋回体４に設けられる。
【0019】
作業機５は、旋回体４に支持される。作業機５は、作業を実施する。実施形態において、作業機５により実施される作業は、掘削対象を掘削する掘削作業及び掘削物を積込対象に積み込む積込作業を含む。
【0020】
作業機５は、ブーム５Ａと、アーム５Ｂと、バケット５Ｃとを含む。ブーム５Ａの基端部は、旋回体４の前部に回動可能に連結される。アーム５Ｂの基端部は、ブーム５Ａの先端部に回動可能に連結される。バケット５Ｃの基端部は、アーム５Ｂの先端部に回動可能に連結される。
【0021】
油圧シリンダ６は、作業機５を動作させる。油圧シリンダ６は、ブームシリンダ６Ａと、アームシリンダ６Ｂと、バケットシリンダ６Ｃとを含む。ブームシリンダ６Ａは、ブーム５Ａを上げ動作及び下げ動作させる。アームシリンダ６Ｂは、アーム５Ｂを掘削動作及びダンプ動作させる。バケットシリンダ６Ｃは、バケット５Ｃを掘削動作及びダンプ動作させる。ブームシリンダ６Ａの基端部は、旋回体４に連結される。ブームシリンダ６Ａの先端部は、ブーム５Ａに連結される。アームシリンダ６Ｂの基端部は、ブーム５Ａに連結される。アームシリンダ６Ｂの先端部は、アーム５Ｂに連結される。バケットシリンダ６Ｃの基端部は、アーム５Ｂに連結される。バケットシリンダ６Ｃの先端部は、バケット５Ｃに連結される。
【0022】
図３に示すように、操作装置７は、運転室２に配置される。操作装置７は、走行体３、旋回体４、及び作業機５の少なくとも一つを動作させるために操作される。操作装置７は、運転室２に搭乗したオペレータに操作される。オペレータは、運転室２に配置された運転シート１４に着座した状態で、操作装置７を操作することができる。
【0023】
操作装置７は、旋回体４及び作業機５の動作のために操作される左作業レバー７Ａ及び右作業レバー７Ｂと、走行体３の動作のために操作される左走行レバー７Ｃ及び右走行レバー７Ｄと、左フットペダル７Ｅ及び右フットペダル７Ｆとを含む。
【0024】
左作業レバー７Ａが前後方向に操作されることにより、アーム５Ｂがダンプ動作又は掘削動作する。左作業レバー７Ａが左右方向に操作されることにより、旋回体４が左旋回動作又は右旋回操作する。右作業レバー７Ｂが左右方向に操作されることにより、バケット５Ｃが掘削動作又はダンプ動作する。右作業レバー７Ｂが前後方向に操作されることにより、ブーム５Ａが下げ動作又は上げ動作する。なお、左作業レバー７Ａが前後方向に操作されたときに旋回体４が右旋回動作又は左旋回動作し、左作業レバー７Ａが左右方向に操作されたときにアーム５Ｂがダンプ動作又は掘削動作してもよい。
【0025】
左走行レバー７Ｃが前後方向に操作されることにより、走行体３の左側の履帯３Ａが前進動作又は後進動作する。右走行レバー７Ｄが前後方向に操作されることにより、走行体３の右側の履帯３Ａが前進動作又は後進動作する。
【0026】
左フットペダル７Ｅは、左走行レバー７Ｃと連動する。右フットペダル７Ｆは、右走行レバー７Ｄ連動する。左フットペダル７Ｅ及び右フットペダル７Ｆが操作されることにより、走行体３が前進動作又は後進動作されてもよい。
【0027】
車載モニタ８は、運転室２に配置される。車載モニタ８は、運転シート１４の右前方に配置される。車載モニタ８は、表示装置８Ａと、入力装置８Ｂとを有する。
【0028】
表示装置８Ａは、規定の表示データを表示する。表示装置８Ａとして、液晶ディスプレイ（ＬＣＤ：Liquid Crystal Display）又は有機ＥＬディスプレイ（ＯＥＬＤ：Organic Electroluminescence Display）のようなフラットパネルディスプレイが例示される。
【0029】
入力装置８Ｂは、オペレータに操作されることにより入力データを生成する。入力装置８Ｂとして、ボタンスイッチ、コンピュータ用キーボード、及びタッチパネルが例示される。
【0030】
位置センサ９は、現場座標系における位置を検出する。位置センサ９は、全球測位衛星システム（ＧＮＳＳ：Global Navigation Satellite System）を利用して現場座標系における位置を検出する。全地球航法衛星システムは、全地球測位システム（ＧＰＳ：Global Positioning System）を含む。全地球航法衛星システムは、緯度、経度、及び高度の座標データで規定される位置を検出する。位置センサ９は、ＧＮＳＳ衛星からＧＮＳＳ電波を受信するＧＮＳＳ受信機を含む。位置センサ９は、旋回体４に配置される。実施形態において、位置センサ９は、旋回体４のカウンタウエイトに配置される。
【0031】
位置センサ９は、第１位置センサ９Ａと、第２位置センサ９Ｂとを含む。第１位置センサ９Ａと第２位置センサ９Ｂとは、旋回体４の異なる位置に配置される。実施形態において、第１位置センサ９Ａと第２位置センサ９Ｂとは、旋回体４において左右方向に間隔をあけて配置される。第１位置センサ９Ａは、第１位置センサ９Ａが配置されている位置を示す第１測位位置を検出する。第２位置センサ９Ｂは、第２位置センサ９Ｂが配置されている位置を示す第２測位位置を検出する。
【0032】
傾斜センサ１０は、旋回体４の加速度及び角速度を検出する。傾斜センサ１０は、慣性計測装置（ＩＭＵ：Inertial Measurement Unit）を含む。傾斜センサ１０は、旋回体４に配置される。実施形態において、傾斜センサ１０は、運転室２の下方に設置される。
【0033】
撮像装置１１は、旋回体４の前方を撮像する。撮像装置１１は、旋回体４に配置される。実施形態において、撮像装置１１は、運転室２の上部に配置される。撮像装置１１は、複数のカメラ１３を含む。カメラ１３は、光学系と、光学系を介して光を受光するイメージセンサとを含む。イメージセンサとして、ＣＣＤ（Charge Coupled Device）センサ又はＣＭＯＳ（Complementary Metal Oxide Semiconductor）センサが例示される。
【0034】
実施形態において、カメラ１３は、４つ設けられる。カメラ１３は、カメラ１３Ａと、カメラ１３Ｂと、カメラ１３Ｃと、カメラ１３Ｄとを含む。１組のカメラ１３によりステレオカメラ１５が構成される。実施形態において、１組のカメラ１３Ａ，１３Ｃにより第１のステレオカメラ１５Ａが構成される。１組のカメラ１３Ｂ，１３Ｄにより第２のステレオカメラ１５Ｂが構成される。
【0035】
ステレオカメラ１５Ａのカメラ１３Ａとカメラ１３Ｃとは、旋回体４の左右方向に間隔をあけて配置される。ステレオカメラ１５Ｂのカメラ１３Ｂとカメラ１３Ｄとは、旋回体４の左右方向に間隔をあけて配置される。カメラ１３Ａ，１３Ｃの光学系の光軸は、Ｘｇ軸に実質的に平行である。カメラ１３Ｂ，１３Ｄの光学系の光軸は、旋回体４の前方に向かって下方に傾斜する。
【0036】
［制御システム］
図４は、実施形態に係る作業機械１の制御システム３０を示すブロック図である。油圧ショベル１は、制御システム３０を備える。制御システム３０は、車載モニタ８と、位置センサ９と、傾斜センサ１０と、撮像装置１１と、制御装置１２とを有する。制御装置１２は、油圧ショベル１を制御する。制御装置１２は、コンピュータシステムを含む。
図４は、実施形態に係る作業機械１の制御システム３０を示すブロック図である。油圧ショベル１は、制御システム３０を備える。制御システム３０は、車載モニタ８と、位置センサ９と、傾斜センサ１０と、撮像装置１１と、制御装置１２とを有する。制御装置１２は、油圧ショベル１を制御する。制御装置１２は、コンピュータシステムを含む。
【0037】
制御装置１２は、記憶部１６と、第１位置方位算出部１７と、第２位置方位算出部１８と、傾斜角算出部１９と、切換部２０と、３次元データ算出部２１と、表示制御部２２と、補正部２３とを有する。
【0038】
記憶部１６は、規定の記憶データを記憶する。記憶部１６は、後述するターゲット２４に係るターゲットデータを記憶する。ターゲット２４は、油圧ショベル１の外部に複数設置される。ターゲットデータは、複数のターゲット２４のそれぞれの３次元位置を含む。ターゲットデータは、ターゲット２４の識別マーク２７により規定される識別データとターゲット２４の３次元位置との関係を示す相関データを含む。
【0039】
第１位置方位算出部１７は、位置センサ９の検出データに基づいて、現場座標系における旋回体４の位置及び方位角を算出する。上述のように、位置センサ９は、ＧＮＳＳ電波を受信するＧＮＳＳ受信機を含む。第１位置方位算出部１７は、ＧＮＳＳ電波に基づいて、旋回体４の位置及び方位角を算出する。旋回体４の方位角は、例えばＸｇ軸を基準とする旋回体４の方位角である。
【0040】
第１位置方位算出部１７は、第１位置センサ９Ａにより検出される第１測位位置及び第２位置センサ９Ｂにより検出される第２測位位置の少なくとも一方に基づいて、旋回体４の位置を算出する。第１位置方位算出部１７は、第１位置センサ９Ａにより検出される第１測位位置と第２位置センサ９Ｂにより検出される第２測位位置との相対位置に基づいて、旋回体４の方位角を算出する。
【0041】
第２位置方位算出部１８は、撮像装置１１により取得された画像に基づいて、現場座標系における旋回体４の位置及び方位角を算出する。上述のように、油圧ショベル１の外部に複数のターゲット２４が設置される。撮像装置１１は、ターゲット２４を撮像する。第２位置方位算出部１８は、撮像装置１１から複数のターゲット２４の画像を取得する。第２位置方位算出部１８は、油圧ショベル１の外部に設置された複数のターゲット２４の画像に基づいて、旋回体４の位置及び方位角を算出する。
【0042】
傾斜角算出部１９は、傾斜センサ１０の検出データに基づいて、旋回体４の傾斜角を算出する。旋回体４の傾斜角は、旋回体４のロール角及びピッチ角を含む。ロール角とは、Ｘｇ軸を中心とする傾斜方向における旋回体４の傾斜角をいう。ピッチ角とは、Ｙｇ軸を中心とする傾斜方向における旋回体４の傾斜角をいう。傾斜角算出部１９は、傾斜センサ１０の検出データに基づいて、旋回体４のロール角及びピッチ角を算出する。
【0043】
切換部２０は、第１位置方位算出部１７により旋回体４の位置及び方位角を算出する第１算出モードと、第２位置方位算出部１８により旋回体４の位置及び方位角を算出する第２算出モードとを切り換える。
【0044】
３次元データ算出部２１は、ステレオカメラ１５により撮像された１組の画像に基づいて、ステレオカメラ１５と撮像対象との距離を算出する。撮像対象として、作業機５により掘削される掘削対象が例示される。３次元データ算出部２１は、ステレオカメラ１５の１組のカメラ１３が撮像した同一の撮像対象の画像をステレオ処理することにより、撮像対象の３次元データを算出する。３次元データ算出部２１は、カメラ座標系における３次元データを算出する。
【0045】
表示制御部２２は、車載モニタ８の表示装置８Ａを制御する。表示制御部２２は、規定の表示データを表示装置８Ａに表示させる。
【0046】
補正部２３は、傾斜センサ１０の誤差を補正する。
【0047】
［算出モード］
図５は、実施形態に係る旋回体４の位置及び方位角の算出モードを説明するための模式図である。実施形態において、旋回体４の位置及び方位角は、第１算出モード及び第２算出モードの少なくとも一方により算出される。旋回体４の位置は、現場座標系における旋回体４の代表点Ｏｍの位置を含む。旋回体４の方位角は、現場座標系における旋回体４の代表点Ｏｍを基準とする車体座標系の方位角を含む。
図５は、実施形態に係る旋回体４の位置及び方位角の算出モードを説明するための模式図である。実施形態において、旋回体４の位置及び方位角は、第１算出モード及び第２算出モードの少なくとも一方により算出される。旋回体４の位置は、現場座標系における旋回体４の代表点Ｏｍの位置を含む。旋回体４の方位角は、現場座標系における旋回体４の代表点Ｏｍを基準とする車体座標系の方位角を含む。
【0048】
第１算出モードは、ＧＮＳＳ電波に基づいて、旋回体４の位置及び方位角を算出する算出モードである。第１算出モードにおいて、第１位置方位算出部１７は、位置センサ９の検出データに基づいて、旋回体４の位置及び方位角を算出する。
【0049】
第２算出モードは、複数のターゲット２４の画像に基づいて、旋回体４の位置及び方位角を算出する算出モードである。第２算出モードにおいて、第２位置方位算出部１８は、撮像装置１１により撮像されたターゲット２４の画像に基づいて、旋回体４の位置及び方位角を算出する。
【0050】
ＧＮＳＳの測位不良が発生すると、第１位置方位算出部１７による旋回体４の位置及び方位角の算出が困難になる可能性がある。ＧＮＳＳの測位不良は、ＧＮＳＳの測位精度の低下及び測位不能を含む。ＧＮＳＳの測位不良として、位置センサ９が受信するＧＮＳＳ電波の強度不足又はＧＮＳＳ電波のマルチパスが例示される。ＧＮＳＳ電波のマルチパスとは、ＧＮＳＳ衛星から送信されたＧＮＳＳ電波が、地面や建造物などで反射したり、電離層において反射又は屈折したりして、位置センサ９が複数の伝送経路からＧＮＳＳ電波を受信することにより、検出される位置に誤差が発生する現象をいう。
【0051】
ＧＮＳＳの測位不良が発生していない場合、旋回体４の位置及び方位角は、第１算出モードで算出される。ＧＮＳＳの測位不良が発生した場合、旋回体４の位置及び方位角は、第２算出モードで算出される。
【0052】
切換部２０は、ＧＮＳＳ電波の受信状況に基づいて、第１算出モードと第２算出モードとを切り換える。第１位置方位算出部１７は、ＧＮＳＳ電波の受信状況の良否を判定することができる。第１位置方位算出部１７は、例えばＧＮＳＳ電波の強度を判定することができる。切換部２０は、位置センサ９によるＧＮＳＳ電波の受信状況に基づいて、第１算出モードと第２算出モードとを切り換える。また、切換部２０は、第１位置方位算出部１７による旋回体４の位置及び方位角の算出の可否に基づいて、第１算出モードと第２算出モードとを切り換える。例えばＧＮＳＳ電波の強度が不足し、ＧＮＳＳ電波の受信状況が不良である場合、第１位置方位算出部１７は、旋回体４の位置及び方位角を算出不可能な状態になる可能性が高い。一方、ＧＮＳＳ電波の強度が充足し、ＧＮＳＳ電波の受信状況が良好である場合、第１位置方位算出部１７は、旋回体４の位置及び方位角を算出可能な状態になる可能性が高い。
【0053】
切換部２０は、ＧＮＳＳ電波の受信状況が良好な状態から不良な状態に変化した場合、第１算出モードから第２算出モードに切り換える。また、切換部２０は、第１位置方位算出部１７が旋回体４の位置及び方位角を算出可能な状態から算出不可能な状態に変化した場合、第１算出モードから第２算出モードに切り換える。
【0054】
切換部２０は、ＧＮＳＳ電波の受信状況が不良な状態から良好な状態に変化した場合、第２算出モードから第１算出モードに切り換える。また、切換部２０は、第１位置方位算出部１７が旋回体４の位置及び方位角を算出不可能な状態から算出可能な状態に変化した場合、第２算出モードから第１算出モードに切り換える。
【0055】
実施形態において、表示制御部２２は、ＧＮＳＳ電波の受信状況を表示装置８Ａに表示させる。図５に示すように、ＧＮＳＳ電波の受信状況が良好な状態から不良な状態に変化した場合、表示制御部２２は、ＧＮＳＳ電波の受信状況が不良であることを表示装置８Ａに表示させてもよい。オペレータは、表示装置８Ａに表示された表示データに基づいて、ＧＮＳＳ電波の受信状況が不良であることを認識することができる。実施形態において、第１算出モードから第２算出モードへの切り換えは、オペレータによる入力装置８Ｂの操作に基づいて実施されてもよい。ＧＮＳＳ電波の受信状況が不良であることを認識したオペレータは、入力装置８Ｂを操作して、第１算出モードから第２算出モードへの切り換えを実施するための入力データを生成する。切換部２０は、入力装置８Ｂからの入力データに基づいて、第１算出モードから第２算出モードに切り換える。
【0056】
第１算出モードから第２算出モードに切り換えられた場合、表示制御部２２は、第１算出モードから第２算出モードに切り換えられたことを表示装置８Ａに表示させてもよい。これにより、オペレータは、第１算出モードから第２算出モードに切り換えられたことを認識することができる。
【0057】
一方、ＧＮＳＳ電波の受信状況が不良な状態から良好な状態に変化した場合、表示制御部２２は、ＧＮＳＳ電波の受信状況が良好であることを表示装置８Ａに表示させる。オペレータは、表示装置８Ａに表示された表示データに基づいて、ＧＮＳＳ電波の受信状況が良好であることを認識することができる。第２算出モードから第１算出モードへの切り換えは、オペレータによる入力装置８Ｂの操作に基づいて実施されてもよい。ＧＮＳＳ電波の受信状況が良好であることを認識したオペレータは、入力装置８Ｂを操作して、第２算出モードから第１算出モードへの切り換えを実施するための入力データを生成する。切換部２０は、入力装置８Ｂからの入力データに基づいて、第２算出モードから第１算出モードに切り換える。
【0058】
第２算出モードから第１算出モードに切り換えられた場合、表示制御部２２は、第２算出モードから第１算出モードに切り換えられたことを表示装置８Ａに表示させてもよい。これにより、オペレータは、第２算出モードから第１算出モードに切り換えられたことを認識することができる。
【0059】
［ターゲット］
図６は、実施形態に係る作業現場に設置されたターゲット２４を示す図である。図６に示すように、ターゲット２４は、作業現場において油圧ショベル１の外部に配置される。ターゲット２４は、作業現場において油圧ショベル１の周囲に複数配置される。ターゲット２４は、表示板２５に描かれたマークを含む。実施形態において、表示板２５の下端部に接地板２６が固定される。表示板２５は、接地板２６を介して作業現場の地面に置かれる。なお、表示板２５は、作業現場に固定されていればよい。ターゲット２４は、例えば作業現場の構造物に貼り付けられてもよい。ターゲット２４は、杭のような部材を用いて作業現場に立てられてもよい。
図６は、実施形態に係る作業現場に設置されたターゲット２４を示す図である。図６に示すように、ターゲット２４は、作業現場において油圧ショベル１の外部に配置される。ターゲット２４は、作業現場において油圧ショベル１の周囲に複数配置される。ターゲット２４は、表示板２５に描かれたマークを含む。実施形態において、表示板２５の下端部に接地板２６が固定される。表示板２５は、接地板２６を介して作業現場の地面に置かれる。なお、表示板２５は、作業現場に固定されていればよい。ターゲット２４は、例えば作業現場の構造物に貼り付けられてもよい。ターゲット２４は、杭のような部材を用いて作業現場に立てられてもよい。
【0060】
図７は、実施形態に係るターゲット２４を示す図である。ターゲット２４は、識別マーク２７と、識別マーク２７の周囲に配置される放射マーク２８とを含む。識別マーク２７は、ターゲット２４を識別するための識別データを含む。実施形態において、識別マーク２７は、ターゲット２４を識別する２次元バーコードを含む。ターゲット２４に基準点Ｏｔが規定される。放射マーク２８は、ターゲット２４の基準点Ｏｔから放射方向に伸びる。放射マーク２８は、ターゲット２４の基準点Ｏｔから放射方向に伸びる複数のライン２８Ａを有する。ライン２８Ａは、放射マーク２８のエッジを含む。複数のライン２８Ａの交点にターゲット２４の基準点Ｏｔが規定される。
【0061】
ターゲット２４が作業現場に設置された後、ターゲット２４の位置が測量機により測量される。測量機は、現場座標系におけるターゲット２４の３次元位置を測定する。ターゲット２４の３次元位置は、基準点Ｏｔの３次元位置を含む。測量機は、基準点Ｏｔの３次元位置を測定する。測量機により測定された複数のターゲット２４のそれぞれの３次元位置は、記憶部１６に記憶される。記憶部１６は、識別マーク２７により規定されるターゲット２４の識別データと、測量機により測定されたターゲット２４の３次元位置との関係を示す相関データを記憶する。識別マーク２７に基づいてターゲット２４が特定されることにより、特定されたターゲット２４の３次元位置が特定される。
【0062】
［第２算出モード］
次に、第２算出モードによる旋回体４の位置及び方位角の算出方法について説明する。図８は、実施形態に係る旋回体４の位置及び方位角の算出方法を示すフローチャートである。図９は、実施形態に係る旋回体４の位置及び方位角の算出方法を説明するための模式図である。
次に、第２算出モードによる旋回体４の位置及び方位角の算出方法について説明する。図８は、実施形態に係る旋回体４の位置及び方位角の算出方法を示すフローチャートである。図９は、実施形態に係る旋回体４の位置及び方位角の算出方法を説明するための模式図である。
【0063】
旋回体４の位置及び方位角を第１算出モードで算出できなくなった場合、旋回体４の位置及び方位角は、第２算出モードで算出される。実施形態において、第２位置方位算出部１８は、複数のターゲット２４の画像と、旋回体４の傾斜角とに基づいて、旋回体４の位置及び方位角を算出する。第２位置方位算出部１８は、撮像装置１１から複数のターゲット２４の画像を取得する。第２位置方位算出部１８は、傾斜角算出部１９から旋回体４の傾斜角を取得する。上述のように、旋回体４の傾斜角は、旋回体４のロール角及びピッチ角を含む。
【0064】
複数のターゲット２４が撮像装置１１により撮像される。撮像装置１１は、複数のターゲット２４を同時に撮像する。図９に示すように、３つのターゲット２４が撮像装置１１により同時に撮像される。第２位置方位算出部１８は、撮像装置１１により撮像された３つのターゲット２４の画像２９を取得する（ステップＳＡ１）。
【0065】
図９に示すように、１つの画像２９に３つのターゲット２４が配置される。
【0066】
第２位置方位算出部１８は、ターゲット２４の識別マーク２７により規定される識別データに基づいて、ターゲット２４を識別する（ステップＳＡ２）。
【0067】
第２位置方位算出部１８は、画像２９における識別マーク２７に基づいて、ターゲット２４を特定する。第２位置方位算出部１８は、画像２９における識別マーク２７と、記憶部１６に記憶されている相関データとに基づいて、記憶部１６からターゲット２４の３次元位置を取得する（ステップＳＡ３）。
【0068】
上述のように、ターゲット２４の３次元位置は、測量機により事前に測定され、記憶部１６に記憶されている。また、記憶部１６には、ターゲット２４の識別マーク２７により規定される識別データとターゲット２４の３次元位置との関係を示す相関データが予め記憶されている。したがって、第２位置方位算出部１８は、画像２９における識別マーク２７と、記憶部１６に記憶されている相関データとに基づいて、ターゲット２４の３次元位置を取得することができる。
【0069】
第２位置方位算出部１８は、画像２９におけるターゲット２４の２次元位置を取得する（ステップＳＡ４）。
【0070】
画像２９におけるターゲット２４の２次元位置は、ターゲット２４に規定された基準点Ｏｔの２次元位置を含む。上述のように、ターゲット２４は、ライン２８Ａを含む放射マーク２８を有する。第２位置方位算出部１８は、画像２９における放射マーク２８に基づいて、画像２９における基準点Ｏｔの２次元位置を算出する。第２位置方位算出部１８は、ターゲット２４の画像２９を画像処理することにより、放射マーク２８に基づいて、画像２９における基準点Ｏｔの２次元位置を高精度に算出することができる。以下の説明において、画像２９における基準点Ｏｔを適宜、基準点Ｏｔｉ、と称する。
【0071】
傾斜角算出部１９は、ターゲット２４が撮像されているときの傾斜センサ１０の検出データを取得して、ターゲット２４が撮像されているときの旋回体４のピッチ角及びロール角を算出する。第２位置方位算出部１８は、ターゲット２４が撮像されているときの旋回体４のロール角及びピッチ角を傾斜角算出部１９から取得する（ステップＳＡ５）。
【0072】
第２位置方位算出部１８は、ステップＳＡ３において取得した３つのターゲット２４の３次元位置と、ステップＳＡ４において取得した画像２９におけるターゲット２４の２次元位置と、ステップＳＡ５において取得した旋回体４のロール角及びピッチ角とに基づいて、現場座標系におけるカメラ１３の位置及び方位角を算出する（ステップＳＡ６）。
【0073】
実施形態において、第２位置方位算出部１８は、空中三角測量におけるブロック調整法の一種であるバンドル法に基づいて、現場座標系におけるカメラ１３の位置及び方位角を算出する。空中三角測量（Aerial Triangulation）とは、光の直進性を示す共線条件及び空中写真の幾何学的性質及びを利用して、既知の基準点Ｏｔの座標に基づいて、複数のカメラ１３による複数の画像２９のそれぞれの撮像位置及び撮像方向を算出する方法をいう。
【0074】
第２位置方位算出部１８は、バンドル法に基づいてカメラ１３の位置及び方位角を算出するために、３つの基準点Ｏｔの３次元位置と、画像２９における基準点Ｏｔｉの２次元位置と、旋回体４のロール角及びピッチ角とを取得する。基準点Ｏｔの３次元位置は、現場座標系における３次元位置である。基準点Ｏｔｉの２次元位置は、画像２９に規定される画像座標系における２次元位置である。画像座標系は、画像２９の左上のコーナーを原点とし横方向をｕ軸とし縦方向をｖ軸とするｕｖ座標系で表される。基準点Ｏｔｉの２次元位置は、複数の画像２９の重複部分を結合するためのパスポイントとして機能する。
【0075】
例えば、現場座標系における基準点Ｏｔの３次元位置をＰ（Ｘ，Ｙ，Ｚ）、カメラ座標系における基準点Ｏｔの３次元位置をＰｃ（Ｘｃ，Ｙｃ，Ｚｃ）、画像座標系における基準点Ｏｔｉの２次元位置をｐ（ｘ、ｙ）、現場座標系における光学中心Ｏｃの位置をＯ（Ｘｏ，Ｙｏ，Ｚｏ）、現場座標系におけるカメラ１３の姿勢を示す回転行列をＲ、内部パラメータ行列をｋとした場合、以下の（１）式、２（式）、及び（３）式の条件が成立する。
【0076】
ｐ ＝ ｋ・Ｐｃ …（１）
Ｐ ＝ Ｒ・ＰＣ＋Ｏ …（２）
Ｐ ＝ Ｒ・（ｋ－１・ｐ） …（３）
Ｐ ＝ Ｒ・ＰＣ＋Ｏ …（２）
Ｐ ＝ Ｒ・（ｋ－１・ｐ） …（３）
【0077】
第２位置方位算出部１８は、バンドル法に基づいて、３つの基準点Ｏｔの３次元位置と、画像２９における基準点Ｏｔｉの２次元位置と、旋回体４のロール角及びピッチ角とを収束計算することにより、現場座標系におけるカメラ１３の位置及び方位角を算出することができる。
【0078】
第２位置方位算出部１８は、ステップＳＡ６において算出したカメラ１３の位置及び方位角に基づいて、現場座標系における旋回体４の位置及び方位角を算出する（ステップＳＡ７）。
【0079】
カメラ１３の光学中心Ｏｃと旋回体４の代表点Ｏｍとの相対位置は、既知である。また、旋回体４に規定された代表点Ｏｍを基準とする車体座標系とカメラ１３の光学中心Ｏｃを基準とするカメラ座標系とを変換する変換行列は、既知である。したがって、第２位置方位算出部１８は、ターゲット２４を撮像した画像２９を用いてバンドル法に基づいて現場座標系におけるカメラ１３の位置及び方位角を算出し、変換行列に基づいてカメラ１３の位置及び方位角を座標変換することにより、現場座標系における旋回体４の位置及び方位角を算出することができる。
【0080】
上述のステップＳＡ１からステップＳＡ７の処理は、ターゲット２４が撮像された場合に実施される。走行体３が走行動作した後に、旋回体４の位置及び方位角を算出する場合、上述のステップＳＡ１からステップＳＡ７の処理が再度実行される。
【0081】
なお、上述のステップＳＡ１からステップＳＡ７の処理において、３つのターゲット２４が撮像されなくてもよく、少なくとも２つのターゲット２４が撮像されればよい。
【0082】
［旋回動作後の位置及び方位の算出］
旋回体４の位置及び方位角が算出された後、走行体３が走行動作した場合、旋回体４の位置及び方位角を算出するために、ターゲット２４が撮像される。ターゲット２４が撮像された場合、上述のステップＳＡ１からステップＳＡ７の処理が再度実行される。
旋回体４の位置及び方位角が算出された後、走行体３が走行動作した場合、旋回体４の位置及び方位角を算出するために、ターゲット２４が撮像される。ターゲット２４が撮像された場合、上述のステップＳＡ１からステップＳＡ７の処理が再度実行される。
【0083】
一方、上述のステップＳＡ１からステップＳＡ７の処理により旋回体４の位置及び方位角を算出した後において、走行体３が走行動作せずに旋回体４が旋回動作した場合、第２位置方位算出部１８は、少なくとも２つのターゲット２４を用いること無く、少なくとも１つのターゲット２４の画像２９に基づいて、旋回体４の位置及び方位角を算出することができる。
【0084】
図１０は、実施形態に係る旋回体４の位置及び方位角の算出方法を説明するための模式図である。旋回体４が第１方向Ｄ１を向いている状態において、少なくとも２つのターゲット２４が撮像されることにより、上述のステップＳＡ１からステップＳＡ７の処理に従って、旋回体４の位置及び方位が算出される。
【0085】
旋回体４の位置及び方位角が算出された後、旋回体４が第１方向Ｄ１から第２方向Ｄ２を向くように旋回して、撮像装置１１により少なくとも１つのターゲット２４が撮像された場合、旋回体４が第２方向Ｄ２を向いているときの旋回体４の方位角は、少なくとも１つのターゲット２４の画像２９に基づいて算出される。第２位置方位算出部１８は、少なくとも２つのターゲット２４を用いて旋回体４の位置及び方位角を算出した後において、旋回体４が第１方向Ｄ１から第２方向Ｄ２を向くように旋回軸ＲＸを中心に旋回動作した場合、撮像装置１１により撮像された少なくとも１つのターゲット２４の画像２９に基づいて、旋回体４の位置及び方位角を算出することができる。
【0086】
すなわち、第２位置方位算出部１８は、第１方向Ｄ１に存在する少なくとも２つのターゲット２４を用いて算出した旋回動作する前の旋回体４の方位角と、第２方向Ｄ２に存在する１つのターゲット２４の画像２９と、旋回動作する前の旋回体４のロール角及びピッチ角と、旋回動作した後の旋回体４のロール角及びピッチ角とに基づいて、旋回角θを算出する。第２位置方位算出部１８は、旋回角θを算出することにより、少なくとも２つのターゲット２４を用いて算出した旋回体４の方位角と旋回角θとに基づいて、旋回動作した後の旋回体４の方位角を算出することができる。また、走行体３が走行動作していない場合、旋回軸ＲＸの位置は変化しないので、第２位置方位算出部１８は、算出した旋回角θに基づいて、旋回体４の位置を算出することができる。
【0087】
また、第２位置方位算出部１８は、旋回体４が旋回動作する前に撮像装置１１により撮像された少なくとも２つのターゲット２４の画像２９と、旋回体４が旋回動作する前の旋回体４のロール角及びピッチ角と、旋回体４が旋回動作した後に撮像装置１１により撮像された少なくとも１つのターゲット２４の画像２９と、旋回体４が旋回動作した後の旋回体４のロール角及びピッチ角とに基づいて、旋回軸ＲＸの位置と、旋回体４が旋回動作する前の旋回体４の方位角と、旋回体４が旋回動作した後の旋回体４の方位角とを、同時に算出してもよい。
【0088】
なお、第２位置方位算出部１８は、傾斜センサ１０の検出データに基づいて、旋回角θを算出することができる。上述のように、傾斜センサ１０は、慣性計測装置（ＩＭＵ）を含む。慣性計測装置（ＩＭＵ）は、旋回体４の旋回を検出する旋回センサとして機能する。第２位置方位算出部１８は、慣性計測装置（ＩＭＵ）の検出データに基づいて、旋回角θを算出することができる。したがって、第２位置方位算出部１８は、３つのターゲット２４を用いて旋回体４の位置及び方位角を算出した後において、走行体３が走行動作せずに旋回体４が旋回動作した場合、旋回体４の旋回を検出する傾斜センサ１０の検出データに基づいて、旋回動作した後の旋回体４の位置及び方位角を算出することができる。
【0089】
図１１は、実施形態に係る旋回体４が旋回動作した後の旋回体４の位置及び方位角の算出方法を示すフローチャートである。旋回体４が旋回動作した後、第２位置方位算出部１８は、撮像装置１１がターゲット２４を撮像できたか否かを判定する。すなわち、第２位置方位算出部１８は、旋回体４が旋回動作した後に、少なくとも１つのターゲット２４の画像を取得できたか否かを判定する（ステップＳＢ１）。
【0090】
ステップＳＢ１において、少なくとも１つのターゲット２４の画像を取得できたと判定した場合（ステップＳＢ１：Ｙｅｓ）、第２位置方位算出部１８は、少なくとも１つのターゲット２４の画像２９と、旋回動作する前の旋回体４のロール角及びピッチ角と、旋回動作した後の旋回体４のロール角及びピッチ角とに基づいて、旋回動作した後の旋回体４の方位角を算出する（ステップＳＢ２）。
【0091】
ステップＳＢ１において、少なくとも１つのターゲット２４の画像を取得できないと判定した場合（ステップＳＢ１：Ｎｏ）、第２位置方位算出部１８は、旋回体４の旋回を検出する傾斜センサ１０の検出データに基づいて、旋回動作した後の旋回体４の位置及び方位角を算出する（ステップＳＢ３）。
【0092】
このように、第１位置方位算出部１７が旋回体４の位置及び方位角を算出不可能な状態、且つ、旋回体４が旋回動作する前に撮像装置１１が少なくとも２つのターゲット２４を撮像した場合において、第２位置方位算出部１８は、少なくとも２つのターゲット２４の画像２９と旋回体４の傾斜角とに基づいて、旋回体４の位置及び方位角を算出する。第１位置方位算出部１７が旋回体４の位置及び方位角を算出不可能な状態、且つ、走行体３が走行動作せずに旋回体４が旋回動作した場合において、第２位置方位算出部１８は、旋回体４が旋回動作した後に撮像装置１１により取得された少なくとも１つのターゲット２４の画像２９又は旋回体４が旋回動作した後の傾斜センサ１０の検出データに基づいて、旋回体４の位置及び方位角を算出することができる。
【0093】
［補正部の処理］
次に、補正部２３の処理について説明する。補正部２３は、傾斜センサ１０の誤差を補正する。上述のように、走行体３が走行動作せずに旋回体４が旋回動作した後において、位置センサ９の検出データに基づいて旋回体４の位置及び方位角を算出できず、且つ、少なくとも１つのターゲット２４の画像２９に基づいて旋回体４の位置及び方位角を算出できない場合、第２位置方位算出部１８は、ＩＭＵを含む傾斜センサ１０の検出データに基づいて、旋回動作した後の旋回体４の位置及び方位角を算出することができる。傾斜センサ１０の検出データを用いて旋回動作した後の旋回体４の位置及び方位角を算出する場合、傾斜センサ１０により検出された加速度が時間で２重積分されることにより、旋回体４の位置が算出され、傾斜センサ１０により検出された角速度が時間で積分されることにより、旋回体４の方位角が算出される。傾斜センサ１０の検出データが積分されると、積分加算により旋回体４の位置及び方位角の算出結果に累積誤差が発生する可能性がある。すなわち、加速度又は角速度の積分による誤差が蓄積して、旋回体４の位置及び方位角の算出精度が低下する可能性がある。
次に、補正部２３の処理について説明する。補正部２３は、傾斜センサ１０の誤差を補正する。上述のように、走行体３が走行動作せずに旋回体４が旋回動作した後において、位置センサ９の検出データに基づいて旋回体４の位置及び方位角を算出できず、且つ、少なくとも１つのターゲット２４の画像２９に基づいて旋回体４の位置及び方位角を算出できない場合、第２位置方位算出部１８は、ＩＭＵを含む傾斜センサ１０の検出データに基づいて、旋回動作した後の旋回体４の位置及び方位角を算出することができる。傾斜センサ１０の検出データを用いて旋回動作した後の旋回体４の位置及び方位角を算出する場合、傾斜センサ１０により検出された加速度が時間で２重積分されることにより、旋回体４の位置が算出され、傾斜センサ１０により検出された角速度が時間で積分されることにより、旋回体４の方位角が算出される。傾斜センサ１０の検出データが積分されると、積分加算により旋回体４の位置及び方位角の算出結果に累積誤差が発生する可能性がある。すなわち、加速度又は角速度の積分による誤差が蓄積して、旋回体４の位置及び方位角の算出精度が低下する可能性がある。
【0094】
ＧＮＳＳ電波の受信状況が良好であり、第１位置方位算出部１７が旋回体４の位置及び方位角を算出可能な状態において、補正部２３は、第１位置方位算出部１７の算出結果に基づいて、旋回体４の位置及び方位角の誤差を補正することができる。
【0095】
一方、ＧＮＳＳ電波の受信状況が不良であり、第１位置方位算出部１７が旋回体４の位置及び方位角を算出不可能な状態において、補正部２３は、第２位置方位算出部１８の算出結果に基づいて、旋回体４の位置及び方位角の誤差を補正することができる。
【0096】
図１２は、実施形態に係る旋回体４の位置及び方位角の算出結果の補正方法を示すフローチャートである。切換部２０は、第１位置方位算出部１７が旋回体４の方位角を算出可能な状態か否かを判定する（ステップＳＣ１）。
【0097】
ステップＳＣ１において、第１位置方位算出部１７が旋回体４の方位角を算出可能な状態であると判定された場合（ステップＳＣ１：Ｙｅｓ）、補正部２３は、第１位置方位算出部１７により算出された旋回体４の方位角に基づいて、旋回体４の位置及び方位角の誤差を補正する（ステップＳＣ２）。
【0098】
ステップＳＣ１において、第１位置方位算出部１７が旋回体４の方位角を算出不可能な状態であると判定された場合（ステップＳＣ１：Ｎｏ）、補正部２３は、第２位置方位算出部１８により算出された旋回体４の方位角に基づいて、旋回体４の位置及び方位角の誤差を補正する（ステップＳＣ３）。
【0099】
［コンピュータシステム］
図１３は、実施形態に係るコンピュータシステム１０００を示すブロック図である。上述の制御装置１２は、コンピュータシステム１０００を含む。コンピュータシステム１０００は、ＣＰＵ（Central Processing Unit）のようなプロセッサ１００１と、ＲＯＭ（Read Only Memory）のような不揮発性メモリ及びＲＡＭ（Random Access Memory）のような揮発性メモリを含むメインメモリ１００２と、ストレージ１００３と、入出力回路を含むインターフェース１００４とを有する。上述の制御装置１２の機能は、コンピュータプログラムとしてストレージ１００３に記憶されている。プロセッサ１００１は、コンピュータプログラムをストレージ１００３から読み出してメインメモリ１００２に展開し、プログラムに従って上述の処理を実行する。なお、コンピュータプログラムは、ネットワークを介してコンピュータシステム１０００に配信されてもよい。
図１３は、実施形態に係るコンピュータシステム１０００を示すブロック図である。上述の制御装置１２は、コンピュータシステム１０００を含む。コンピュータシステム１０００は、ＣＰＵ（Central Processing Unit）のようなプロセッサ１００１と、ＲＯＭ（Read Only Memory）のような不揮発性メモリ及びＲＡＭ（Random Access Memory）のような揮発性メモリを含むメインメモリ１００２と、ストレージ１００３と、入出力回路を含むインターフェース１００４とを有する。上述の制御装置１２の機能は、コンピュータプログラムとしてストレージ１００３に記憶されている。プロセッサ１００１は、コンピュータプログラムをストレージ１００３から読み出してメインメモリ１００２に展開し、プログラムに従って上述の処理を実行する。なお、コンピュータプログラムは、ネットワークを介してコンピュータシステム１０００に配信されてもよい。
【0100】
コンピュータプログラム又はコンピュータシステム１０００は、上述の実施形態に従って、走行体３及び旋回体４を備える油圧ショベル１の外部に設置された複数のターゲット２４の画像を取得することと、旋回体４の傾斜角を取得することと、複数のターゲット２４の画像と旋回体４の傾斜角とに基づいて、旋回体４の位置及び方位角を算出することと、を実行することができる。
【0101】
［効果］
以上説明したように、実施形態によれば、油圧ショベル１の外部に設置された複数のターゲット２４の画像が取得される。また、旋回体４の傾斜角であるロール角及びピッチ角が取得される。複数のターゲット２４の画像と旋回体４の傾斜角とに基づいて、旋回体４の位置及び方位角が算出される。ＧＮＳＳの測位不良が発生した場合においても、ターゲット２４の画像に基づいて、油圧ショベル１の位置及び方位角が算出される。したがって、ＧＮＳＳの測位不良が発生した場合においても、油圧ショベル１は、マシンガイダンス技術又はマシンコントロール技術に基づいて、作業を実施することができる。
以上説明したように、実施形態によれば、油圧ショベル１の外部に設置された複数のターゲット２４の画像が取得される。また、旋回体４の傾斜角であるロール角及びピッチ角が取得される。複数のターゲット２４の画像と旋回体４の傾斜角とに基づいて、旋回体４の位置及び方位角が算出される。ＧＮＳＳの測位不良が発生した場合においても、ターゲット２４の画像に基づいて、油圧ショベル１の位置及び方位角が算出される。したがって、ＧＮＳＳの測位不良が発生した場合においても、油圧ショベル１は、マシンガイダンス技術又はマシンコントロール技術に基づいて、作業を実施することができる。
【0102】
ターゲット２４の３次元位置が測量機により取得され、記憶部１６に予め記憶される。ターゲット２４の画像２９に基づいて、画像２９におけるターゲット２４の２次元位置が第２位置方位算出部１８により算出される。これにより、第２位置方位算出部１８は、ターゲット２４の３次元位置と、ターゲット２４の２次元位置と、旋回体４の傾斜角とに基づいて、旋回体４の位置及び方位角を算出することができる。
【0103】
実施形態において、ターゲット２４の３次元位置と、ターゲット２４の２次元位置と、旋回体４の傾斜角とに基づいて、現場座標系におけるカメラ１３の位置及び方位角が算出される。現場座標系におけるカメラ１３の位置及び方位角が算出された後、カメラ１３の位置及び方位角が座標変換されることにより、旋回体４の位置及び方位角が算出される。これにより、第２位置方位算出部１８は、カメラ１３の位置及び方位角が算出された後、カメラ１３の位置及び方位角に基づいて、旋回体４の位置及び方位角を適正に算出することができる。
【0104】
ターゲット２４の３次元位置は、ターゲット２４に規定された基準点Ｏｔの３次元位置である。画像２９におけるターゲット２４の２次元位置は、ターゲット２４に規定された画像２９における基準点Ｏｔｉの２次元位置である。基準点Ｏｔ及び基準点Ｏｔｉが用いられることにより、旋回体４の位置及び方位角の算出精度の低下が抑制される。
【0105】
ターゲット２４は、ターゲット２４の基準点Ｏｔから放射方向に伸びる放射マーク２８を含む。放射マーク２８により、基準点Ｏｔｉの２次元位置の算出精度の低下が抑制される。
【0106】
ターゲット２４は、ターゲット２４を識別する識別マーク２７を含む。識別マーク２７により規定されるターゲット２４の識別データと測量機により測定されたターゲット２４の３次元位置との関係を示す相関データが記憶部１６に予め記憶される。これにより、第２位置方位算出部１８は、画像２９における識別マーク２７に基づいて、記憶部１６に記憶されている相関データを参照することにより、画像２９に写っているターゲット２４の３次元位置を取得することができる。
【0107】
旋回体４の傾斜角は、旋回体４に配置された傾斜センサ１０の検出データに基づいて算出される。これにより、旋回体４の傾斜角が高精度に算出される。
【0108】
３つのターゲット２４を用いて第２位置方位算出部１８により旋回体４の位置及び方位角が算出された後、走行体３が走行動作せずに旋回体４が旋回動作した場合、少なくとも１つのターゲット２４の画像を取得すれば、第２位置方位算出部１８は、旋回体４の位置及び方位角を効率良く算出することができる。
【0109】
３つのターゲット２４を用いて第２位置方位算出部１８により旋回体４の位置及び方位角が算出された後、走行体３が走行動作せずに旋回体４が旋回動作した場合、第２位置方位算出部１８は、ターゲット２４を用いずに、旋回角θを検出可能な傾斜センサ１０の検出データに基づいて、旋回体４の位置及び方位角を効率良く算出することができる。
【0110】
［その他の実施形態］
上述の実施形態において、第２位置方位算出部１８は、３つの基準点Ｏｔに基づいて、現場座標系におけるカメラ１３の位置及び方位角を算出することとした。第２位置方位算出部１８は、少なくとも２つの基準点Ｏｔに基づいて、現場座標系におけるカメラ１３の位置及び方位角を算出してもよい。すなわち、第２位置方位算出部１８は、少なくとも２つの基準点Ｏｔの３次元位置と、画像２９における基準点Ｏｔｉの２次元位置と、旋回体４のロール角及びピッチ角とを収束計算することにより、現場座標系におけるカメラ１３の位置及び方位角を算出してもよい。
上述の実施形態において、第２位置方位算出部１８は、３つの基準点Ｏｔに基づいて、現場座標系におけるカメラ１３の位置及び方位角を算出することとした。第２位置方位算出部１８は、少なくとも２つの基準点Ｏｔに基づいて、現場座標系におけるカメラ１３の位置及び方位角を算出してもよい。すなわち、第２位置方位算出部１８は、少なくとも２つの基準点Ｏｔの３次元位置と、画像２９における基準点Ｏｔｉの２次元位置と、旋回体４のロール角及びピッチ角とを収束計算することにより、現場座標系におけるカメラ１３の位置及び方位角を算出してもよい。
【0111】
上述の実施形態において、第２位置方位算出部１８は、現場座標系におけるカメラ１３の位置及び方位角を算出し、現場座標系における旋回体４の位置及び方位角を算出することとした。第２位置方位算出部１８は、車体座標系におけるカメラ１３の位置及び方位角を算出してもよいし、カメラ座標系におけるカメラ１３の位置及び方位角を算出してもよい。また、第２位置方位算出部１８は、車体座標系における旋回体４の位置及び方位角を算出してもよいし、カメラ座標系における旋回体４の位置及び方位角を算出してもよい。
【0112】
上述の実施形態において、ターゲット２４は、ステレオカメラ１５により撮像されることとした。ターゲット２４は、単眼カメラにより撮像されてもよい。
【0113】
上述の実施形態において、車載モニタ８が表示装置８Ａ及び入力装置８Ｂを有することとした。例えばタブレット端末が表示装置８Ａ及び入力装置８Ｂを有してもよい。すなわち、表示装置８Ａ及び入力装置８Ｂは、油圧ショベル１から分離されてもよい。また、上述の実施形態において、表示装置８Ａ及び入力装置８Ｂは、運転室２に配置されることとした。表示装置８Ａ及び入力装置８Ｂの一方又は両方が運転室２の外部に配置されてもよい。
【0114】
上述の実施形態において、ＧＮＳＳ電波の受信状況が表示装置８Ａに表示されることとした。表示制御部２２は、例えば第１算出モードと第２算出モードとの切り換えを推奨するリコメンド表示データを表示装置８Ａに表示させてもよい。例えば、ＧＮＳＳ電波の受信状況が良好な状態から不良な状態に変化した場合、表示制御部２２は、例えば「第１算出モードから第２算出モードに切り換えることを推奨します」のような文字データを表示装置８Ａに表示させてもよい。ＧＮＳＳ電波の受信状況が不良な状態から良好な状態に変化した場合、表示制御部２２は、例えば「第２算出モードから第１算出モードに切り換えることを推奨します」のような文字データを表示装置８Ａに表示させてもよい。
【0115】
上述の実施形態において、第１算出モードと第２算出モードとの切換が、オペレータによる入力装置８Ｂの操作に基づいて実施されることとした。ＧＮＳＳ電波の受信状況が表示装置８Ａに表示されなくてもよい。また、第１算出モードと第２算出モードとの切換は、制御装置１２により自動的に実施されてもよい。例えば、ＧＮＳＳ電波の受信状況が良好な状態から不良な状態に変化した場合、切換部２０は、入力装置８Ｂからの入力データによらずに、第１算出モードから第２算出モードに自動的に切り換えてもよい。また、ＧＮＳＳ電波の受信状況が不良な状態から良好な状態に変化した場合、切換部２０は、入力装置８Ｂからの入力データによらずに、第２算出モードから第１算出モードに自動的に切り換えてもよい。第１算出モードと第２算出モードとが自動的に切り換えられた場合、表示制御部２２は、第１算出モードと第２算出モードとが切り換えられたことを表示装置８Ａに表示させてもよい。
【0116】
上述の実施形態において、記憶部１６、第１位置方位算出部１７、第２位置方位算出部１８、傾斜角算出部１９、切換部２０、３次元データ算出部２１、表示制御部２２、及び補正部２３のそれぞれが、別々のハードウエアにより構成されてもよい。
【0117】
上述の実施形態において、作業機械１が走行体３及び旋回体４を有する油圧ショベルであることとした。作業機械１は走行体３及び旋回体４を有しなくてもよい。作業機械１は、作業機を有していればよく、例えばブルドーザでもよいしホイールローダでもよい。
【符号の説明】
【0118】
１…油圧ショベル（作業機械）、２…運転室、３…走行体、３Ａ…履帯、４…旋回体、５…作業機、５Ａ…ブーム、５Ｂ…アーム、５Ｃ…バケット、６…油圧シリンダ、６Ａ…ブームシリンダ、６Ｂ…アームシリンダ、６Ｃ…バケットシリンダ、７…操作装置、７Ａ…左作業レバー、７Ｂ…右作業レバー、７Ｃ…左走行レバー、７Ｄ…右走行レバー、７Ｅ…左フットペダル、７Ｆ…右フットペダル、８…車載モニタ、８Ａ…表示装置、８Ｂ…入力装置、９…位置センサ、９Ａ…第１位置センサ、９Ｂ…第２位置センサ、１０…傾斜センサ、１１…撮像装置、１２…制御装置、１３…カメラ、１３Ａ…カメラ、１３Ｂ…カメラ、１３Ｃ…カメラ、１３Ｄ…カメラ、１４…運転シート、１５…ステレオカメラ、１５Ａ…ステレオカメラ、１５Ｂ…ステレオカメラ、１６…記憶部、１７…第１位置方位算出部、１８…第２位置方位算出部、１９…傾斜角算出部、２０…切換部、２１…３次元データ算出部、２２…表示制御部、２３…補正部、２４…ターゲット、２５…表示板、２６…接地板、２７…識別マーク、２８…放射マーク、２８Ａ…ライン、２９…画像、３０…制御システム、１０００…コンピュータシステム、１００１…プロセッサ、１００２…メインメモリ、１００３…ストレージ、１００４…インターフェース、Ｄ１…第１方向、Ｄ２…第２方向、Ｏｃ…光学中心、Ｏｔ…基準点、Ｏｇ…現場基準点、Ｏｍ…代表点、Ｏｔｉ…基準点、ＲＸ…旋回軸、θ…旋回角。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】