JP2024041326A

JP2024041326A - 作業機械における車体座標系を設定するためのシステムおよび方法

Info

Publication number: JP2024041326A
Application number: JP2022146072A
Authority: JP
Inventors: 崇幸篠田; 光内田; 隆之片岡
Original assignee: Komatsu Ltd
Current assignee: Komatsu Ltd
Priority date: 2022-09-14
Filing date: 2022-09-14
Publication date: 2024-03-27
Also published as: WO2024057846A1

Abstract

【課題】作業機械において、第１車体を旋回させることなく、精度よく車体座標系を設定する。【解決手段】システムは、姿勢センサと、位置計測装置と、コントローラとを備える。姿勢センサは、第１車体の姿勢を検出する。位置計測装置は、作業機に含まれる第１ターゲット部の位置を計測する。コントローラは、姿勢センサによって検出された第１車体を第２車体に対して静止させた状態での第１車体の姿勢を取得する。コントローラは、第１車体の姿勢と第１ターゲット部の位置とに基づいて、車体座標系を設定する。【選択図】図６

Description

本発明は、作業機械における車体座標系を設定するためのシステムおよび方法に関する。

作業機械には、第１車体と、第１車体と旋回可能に接続された第２車体と、第１車体に対して動作可能に取り付けられた作業機とを備えるものがある。第１車体が第２車体に対して旋回したり、作業機が動作したりすることで、作業機械は、掘削などの施工作業を行う。従来、作業機の位置を検出する技術が知られている。例えば、特許文献１では、作業機械は、例えばＧＮＳＳ（Global Navigation Satellite System）などの位置センサを備えている。位置センサは、作業機械において位置センサが取り付けられたグローバル座標系における位置を検出する。グローバル座標系における位置は、ＧＮＳＳによって計測される座標系であり、地球に固定された原点を基準とした座標系である。

また、作業機械のコントローラは、位置センサの位置から、車体座標系における作業機の位置を算出する。車体座標系は、作業機械を基準とする座標系である。例えば、コントローラは、位置センサに対する作業機の位置を、作業機械の車体の各構成部品の寸法、及び、作業機の角度などの機械パラメータに基づいて算出する。

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

上記のように作業機の位置を精度よく算出するためには、作業機械における車体座標系を精度よく設定することが求められる。作業機械における車体座標系の設定方法として例えば以下のものがある。

まず、第１車体に第１ターゲットプリズムが取り付けられる。そして、第１車体を旋回させることで、第１ターゲットプリズムの複数の位置が、外部の位置計測装置によって計測される。これらの第１ターゲットプリズムの複数の位置は、コントローラに入力され、コントローラは、第１ターゲットプリズムの複数の位置に基づいて、第１車体の旋回平面を算出する。

次に、作業機に第２ターゲットプリズムが取り付けられる。そして、作業機を第１車体に対して上下に動作させることで、第２ターゲットプリズムの複数の位置が、外部の位置計測装置によって計測される。これらの第２ターゲットプリズムの複数の位置は、コントローラに入力され、コントローラは、第２ターゲットプリズムの複数の位置に基づいて、作業機が動作する作業機平面を算出する。そして、コントローラは、旋回平面と作業機平面とに基づいて、第１車体に仮想的に設定される原点を基準とした車体座標系を算出する。

しかしながら、上述した作業機械における車体座標系の設定方法では、第１車体の旋回時の計測誤差により、車体座標系に誤差が生じる。第１車体を旋回させるためには、大きな作業エリアが必要となる。また、第１車体を旋回させるために、作業の工数が増大してしまう。本発明の目的は、作業機械において、第１車体を旋回させることなく、精度よく車体座標系を設定することにある。

本発明の一態様に係るシステムは、作業機械において車体座標系を設定するためのシステムである。作業機械は、第１車体と、第１車体と旋回可能に接続された第２車体と、第１車体に対して動作可能に取り付けられた作業機と、を備える。車体座標系は、第１車体を基準とする座標系である。当該システムは、姿勢センサと、位置計測装置と、コントローラとを備える。姿勢センサは、第１車体の姿勢を検出する。位置計測装置は、作業機に含まれる第１ターゲット部の位置を計測する。コントローラは、第１車体を第２車体に対して静止させた状態での第１車体の姿勢を、姿勢センサから取得する。コントローラは、第１車体の姿勢と第１ターゲット部の位置とに基づいて、車体座標系を設定する。

本発明の他の態様に係る方法は、作業機械において車体座標系を設定するための方法である。作業機械は、第１車体と、第１車体と旋回可能に接続された第２車体と、第１車体に対して動作可能に取り付けられた作業機と、を備える。車体座標系は、第１車体を基準とする座標系である。当該方法は、第１車体の姿勢を検出する姿勢センサにより、第１車体を第２車体に対して静止させた状態での第１車体の姿勢を取得することと、作業機に含まれる第１ターゲット部の位置を計測することと、第１車体の姿勢と第１ターゲット部の位置とに基づいて、車体座標系を設定すること、を備える。

本発明によれば、第１車体の旋回平面を求める代わりに、姿勢センサが検出した第１車体の姿勢に基づいて、車体座標系が設定される。そのため、作業機械において、第１車体を旋回させることなく、精度よく車体座標系を設定することができる。

作業機械の側面図である。作業機械およびその制御システムの構成を示すブロック図である。作業機械と外部座標系及び車体座標系を示す模式的な側面図である。作業機械と外部座標系及び車体座標系を示す模式的な背面図である。作業機械と外部座標系及び車体座標系を示す模式的な上面図である。機械パラメータを較正するための処理を示すフローチャートである。第１ターゲット部の一例を示す図である。第２ターゲット部の一例を示す図である。第２ターゲット部の一例を示す図である。第２ターゲット部の一例を示す図である。較正の精度を評価する方法を示す図である。検出した刃先位置を利用した作業機械の自動制御の一例を示す図である。検出した刃先位置を利用した作業機械のアシスタント画面の一例を示す図である。

以下、実施形態に係る作業機械１の制御システムについて、図面を参照しながら説明する。図１は、作業機械１の側面図である。本実施形態において、作業機械１は油圧ショベルなどの掘削機（excavator）である。

図１に示すように、作業機械１は、本体２と作業機３とを含む。作業機３は、本体２の前部に取り付けられている。本体２は、第１車体４と第２車体５とを含む。第１車体４は、第２車体５に対して旋回可能に接続されている。第１車体４には、キャブ６が配置されている。第２車体５は、履帯７を含む。なお、図１では、左右の履帯７のうち一方のみが図示されている。履帯７が駆動されることで、作業機械１は走行する。

作業機３は、第１車体４に対して、上下に動作可能に取り付けられる。作業機３は、ブーム１１と、アーム１２と、バケット１３とを含む。ブーム１１は、第１車体４に対して、ブームピン２８回りに回転可能に取り付けられている。アーム１２は、ブーム１１に対して、アームピン２９回りに回転可能に取り付けられている。バケット１３は、アーム１２に対して、バケットピン３０回りに回転可能に取り付けられている。

作業機３は、ブームシリンダ１４と、アームシリンダ１５と、バケットシリンダ１６とを含む。ブームシリンダ１４と、アームシリンダ１５と、バケットシリンダ１６とは、例えば油圧シリンダである。ブームシリンダ１４と、アームシリンダ１５と、バケットシリンダ１６とは、後述する油圧ポンプ２２からの作動油によって、駆動される。ブームシリンダ１４は、ブーム１１を動作させる。アームシリンダ１５は、アーム１２を動作させる。バケットシリンダ１６は、バケット１３を動作させる。

図２は、作業機械１およびその制御システムの構成を示すブロック図である。図２に示すように、作業機械１は、駆動源２１と、油圧ポンプ２２と、動力伝達装置２３と、コントローラ２４とを含む。駆動源２１は、コントローラ２４からの指令信号により制御される。駆動源２１は、例えば内燃エンジンである。或いは、駆動源は、電動モータ、或いは水素エンジンなどの駆動源を含んでもよい。油圧ポンプ２２は、駆動源２１によって駆動され、作動油を吐出する。油圧ポンプ２２から吐出された作動油は、ブームシリンダ１４と、アームシリンダ１５と、バケットシリンダ１６とに供給される。

作業機械１は、旋回モータ２５を含む。旋回モータ２５は、例えば油圧モータである。旋回モータ２５は、油圧ポンプ２２からの作動油によって駆動される。或いは、旋回モータ２５は、電動モータであってもよい。旋回モータ２５は、第１車体４を旋回させる。なお、図２では、１つの油圧ポンプが図示されているが、複数の油圧ポンプが設けられてもよい。

油圧ポンプ２２は可変容量ポンプである。油圧ポンプ２２にはポンプ制御装置２６が接続されている。ポンプ制御装置２６は、油圧ポンプ２２の傾転角を制御する。ポンプ制御装置２６は、例えば電磁弁を含み、コントローラ２４からの指令信号により制御される。コントローラ２４は、ポンプ制御装置２６を制御することで、油圧ポンプ２２の容量を制御する。

作業機械１は、制御弁２７を含む。油圧ポンプ２２とシリンダ１４－１６と旋回モータ２５とは、制御弁２７を介して油圧回路によって接続されている。制御弁２７は、コントローラ２４からの指令信号によって制御される。制御弁２７は、油圧ポンプ２２からシリンダ１４－１６及び旋回モータ２５に供給される作動油の流量を制御する。コントローラ２４は、制御弁２７を制御することで、作業機３の動作を制御する。コントローラ２４は、制御弁２７を制御することで、第１車体４の旋回を制御する。なお、シリンダ１４－１６は、油圧シリンダに限らず、電動モータによって駆動されるメカニカルシリンダであってもよい。

動力伝達装置２３は、駆動源２１の駆動力を第２車体５に伝達する。履帯７は、動力伝達装置２３からの駆動力によって駆動されて、作業機械１を走行させる。動力伝達装置２３は、例えば、トルクコンバーター、或いは複数の変速ギアを有するトランスミッションであってもよい。或いは、動力伝達装置２３は、ＨＳＴ（Hydro Static Transmission）、或いはＨＭＴ（Hydraulic Mechanical Transmission）などの他の形式のトランスミッションであってもよい。

コントローラ２４は、ＣＰＵなどのプロセッサ３１を含む。プロセッサ３１は、作業機械１の制御のための処理を行う。コントローラ２４は、記憶装置３２を含む。記憶装置３２は、ＲＡＭ或いはＲＯＭなどのメモリ、及び、ＨＤＤ（Hard Disk Drive）或いはＳＳＤ（Solid State Drive）などの補助記憶装置を含む。記憶装置３２は、作業機械１の制御のためのデータ及びプログラムを記憶している。

制御システムは、操作装置３３を含む。操作装置３３は、オペレータによって操作可能である。操作装置３３は、例えば、レバー、ペダル、或いはスイッチを含む。操作装置３３は、オペレータによる操作に応じた操作信号をコントローラ２４に出力する。コントローラ２４は、オペレータによる操作装置３３の操作に応じて、作業機３を動作させるように、制御弁２７を制御する。コントローラ２４は、オペレータによる操作装置３３の操作に応じて、第１車体４を旋回させるように、制御弁２７を制御する。コントローラ２４は、オペレータによる操作装置３３の操作に応じて、作業機械１を走行させるように、駆動源２１及び動力伝達装置２３を制御する。

制御システムは、入力装置３４とディスプレイ３５とを含む。入力装置３４は、オペレータによって操作可能である。入力装置３４は、例えばタッチスクリーンである。ただし、入力装置３４は、ハードウェアキーを含んでもよい。オペレータは、入力装置３４を操作することで、作業機械１に関する各種の設定を入力する。入力装置３４は、オペレータの操作に応じた入力信号を出力する。ディスプレイ３５は、例えばＬＣＤ、ＯＥＬＤ、或いは他の種類のディスプレイである。ディスプレイ３５は、コントローラ２４からの表示信号に応じた画面を表示する。

制御システムは、位置センサ３６と、姿勢センサ３７と、作業機センサ３８とを含む。位置センサ３６は、本体２の位置を検出する。本体２の位置は、外部座標系で示される。外部座標系は、作業機械１の外部を基準とする座標系である。外部座標系は、例えばＧＮＳＳ(Global Navigation Satellite System)によるグローバル座標系である。或いは、外部座標系は、作業機械１が使われる作業現場内での現場座標系であってもよい。

図３は、作業機械１と外部座標系Ｘ１－Ｙ１－Ｚ１及び車体座標系Ｘ２－Ｙ２－Ｚ２を示す模式的な側面図である。図４は、作業機械１と外部座標系Ｘ１－Ｙ１－Ｚ１及び車体座標系Ｘ２－Ｙ２－Ｚ２を示す模式的な背面図である。図５は、作業機械１と外部座標系Ｘ１－Ｙ１－Ｚ１及び車体座標系Ｘ２－Ｙ２－Ｚ２を示す模式的な上面図である。

図３に示すように、位置センサ３６は、アンテナ４１と受信機４２とを含む。アンテナ４１は、本体２に取り付けられている。受信機４２は、外部座標系Ｘ１－Ｙ１－Ｚ１におけるアンテナ４１の位置（以下、「アンテナ位置」と呼ぶ）Ｐ１を検出する。受信機４２は、外部座標系Ｘ１－Ｙ１－Ｚ１におけるアンテナ位置Ｐ１を示すアンテナ位置データを出力する。なお、アンテナ位置は、複数であってもよい。

姿勢センサ３７は、第１車体４に取り付けられている。姿勢センサ３７は、第１車体４の姿勢を検出する。第１車体４の姿勢は、第１車体４のヨー角θ_ｙ１と、ピッチ角θ_ｐ１と、ロール角θ_ｒ１とを含む。図３に示すように、第１車体４のピッチ角θ_ｐ１は、第１車体４の前後方向の傾斜角度である。図４に示すように、第１車体４のロール角θ_ｒ１は、第１車体４の左右方向の傾斜角度である。図５に示すように、第１車体４のヨー角θ_ｙ１は、第１車体４の前後方向の方位である。姿勢センサ３７は、例えばＩＭＵ（Inertial Measurement Unit）である。姿勢センサ３７は、第１車体４の姿勢を示す第１姿勢データを出力する。

作業機センサ３８は、作業機３の姿勢を検出する。作業機３は、図４及び図５に示す作業機平面５０内で動作可能に、第１車体４に取り付けられている。作業機平面５０は、作業機３が動作する際に通過する平面である。例えば、作業機平面５０は、作業機３の左右方向における中心を通り、第１車体４の上下方向及び前後方向に延びる仮想的な平面である。なお、作業機平面５０は、作業機３の左右方向における中心に限らず、中心から左右にオフセットした位置を通ってもよい。ブーム１１と、アーム１２と、バケット１３とのそれぞれがこの作業機平面５０内で回転することで、作業機３の姿勢が変化する。

図３に示すように、作業機３の姿勢は、ブーム角θ１と、アーム角θ２と、バケット角θ３とを含む。作業機センサ３８は、ブーム角θ１と、アーム角θ２と、バケット角θ３とを示す第２姿勢データを出力する。ブーム角θ１は、本体２の車体座標系の上下方向に対するブーム１１の角度である。アーム角θ２は、ブーム１１に対するアーム１２の角度である。バケット角θ３は、アーム１２に対するバケット１３の角度である。

詳細には、作業機センサ３８は、図３に示すブーム角センサ３８Ａと、アーム角センサ３８Ｂと、バケット角センサ３８Ｃとを含む。ブーム角センサ３８Ａは、ブーム角θ１を検出する。アーム角センサ３８Ｂは、アーム角θ２を検出する。バケット角センサ３８Ｃは、バケット角θ３を検出する。

詳細には、ブーム角センサ３８Ａは、ブームシリンダ１４のストローク長を検出する。アーム角センサ３８Ｂは、アームシリンダ１５のストローク長を検出する。バケット角センサ３８Ｃは、バケットシリンダ１６のストローク長を検出する。各シリンダ１４－１６のストローク長から、ブーム１１とアーム１２とバケット１３との各回転角度θ１～θ３が算出される。或いは、ブーム角センサ３８Ａと、アーム角センサ３８Ｂと、バケット角センサ３８Ｃとは、それぞれブーム１１とアーム１２とバケット１３との回転角度θ１～θ３を直接的に検出するセンサであってもよい。或いは、ブーム角センサ３８Ａと、アーム角センサ３８Ｂと、バケット角センサ３８Ｃとは、ＩＭＵであってもよい。

コントローラ２４は、操作装置３３から操作信号を受信する。コントローラ２４は、入力装置３４から入力信号を受信する。コントローラ２４は、ディスプレイ３５に表示信号を出力する。コントローラ２４は、位置センサ３６からアンテナ位置データを受信する。コントローラ２４は、姿勢センサ３７から第１姿勢データを受信する。コントローラ２４は、作業機センサ３８から第２姿勢データを受信する。

コントローラ２４は、受信したデータと機械パラメータとに基づいて、作業機３の位置を算出する。詳細には、コントローラ２４は、受信したデータと機械パラメータとに基づいて、バケット１３の刃先の位置（以下、「刃先位置」と呼ぶ）Ｐ２を算出する。コントローラ２４は、上述した外部座標系Ｘ１－Ｙ１－Ｚ１における刃先位置Ｐ２を算出する。

機械パラメータは、記憶装置３２に記憶されている。機械パラメータは、車体座標系Ｘ２－Ｙ２－Ｚ２におけるアンテナ４１と作業機３の刃先位置Ｐ２との位置関係を規定する。図３から図５に示すように、車体座標系Ｘ２－Ｙ２－Ｚ２は、第１車体４に設けられた仮想の原点を基準とする座標系である。車体座標系Ｘ２－Ｙ２－Ｚ２のＸ２軸は、第１車体４の前後方向を示す。車体座標系Ｘ２－Ｙ２－Ｚ２のＹ２軸は、第１車体４の左右方向を示す。車体座標系Ｘ２－Ｙ２－Ｚ２のＺ２軸は、第１車体４の上下方向を示す。

機械パラメータは、アンテナパラメータと作業機パラメータとを含む。アンテナパラメータは、車体座標系Ｘ２－Ｙ２－Ｚ２におけるブームピン２８などの基準位置に対するアンテナ位置Ｐ１の相対位置を示す。基準位置は、ブームピン２８に限らず、他の位置であってもよい。図３及び図４に示すように、アンテナパラメータは、アンテナ位置Ｐ１とブームピン２８との間の車体座標系Ｘ２－Ｙ２－Ｚ２のＸ２軸方向における距離Ｌｘと、Ｙ２軸方向における距離Ｌｙと、Ｚ２軸方向における距離Ｌｚとを含む。

図３に示すように、作業機パラメータは、ブーム１１の長さＬ１と、アーム１２の長さＬ２と、バケット１３の長さＬ３を含む。詳細には、ブーム１１の長さＬ１は、ブームピン２８とアームピン２９との間の距離である。アーム１２の長さＬ２は、アームピン２９とバケットピン３０との間の距離である。バケット１３の長さは、バケットピン３０とバケット１３の刃先との間の距離である。

コントローラ２４は、外部座標系Ｘ１－Ｙ１－Ｚ１におけるアンテナ位置Ｐ１と、機械パラメータと、第１姿勢データと、第２姿勢データとに基づいて、外部座標系Ｘ１－Ｙ１－Ｚ１における刃先位置Ｐ２を算出する。例えば、コントローラ２４は、機械パラメータと第２姿勢データとに基づいて、車体座標系Ｘ２－Ｙ２－Ｚ２におけるアンテナ位置Ｐ１と刃先位置Ｐ２との位置関係を算出する。コントローラ２４は、第１姿勢データから、外部座標系Ｘ１－Ｙ１－Ｚ１と車体座標系Ｘ２－Ｙ２－Ｚ２との位置関係を算出する。そして、コントローラ２４は、車体座標系Ｘ２－Ｙ２－Ｚ２におけるアンテナ位置Ｐ１と刃先位置Ｐ２との位置関係と、外部座標系Ｘ１－Ｙ１－Ｚ１と車体座標系Ｘ２－Ｙ２－Ｚ２との位置関係とから、車体座標系Ｘ２－Ｙ２－Ｚ２における刃先位置Ｐ２を、外部座標系Ｘ１－Ｙ１－Ｚ１における刃先位置Ｐ２に換算する。

以上のように、コントローラ２４は、位置センサ３６が検出した外部座標系Ｘ１－Ｙ１－Ｚ１におけるアンテナ位置Ｐ１から、外部座標系Ｘ１－Ｙ１－Ｚ１における刃先位置Ｐ２を算出する。

次に、刃先位置Ｐ２を算出するために用いられる機械パラメータを較正する処理について説明する。図６は、機械パラメータを較正するための処理を示すフローチャートである。較正の対象となるパラメータは、上述した機械パラメータの全て、或いは一部であってもよい。

まず、ステップＳ１０１～Ｓ１０４において、コントローラ２４は、作業機械１における車体座標系Ｘ２－Ｙ２－Ｚ２を設定する。図６に示すように、ステップＳ１０１において、コントローラ２４は、第１ターゲット部の位置を取得する。第１ターゲット部は、車体座標系Ｘ２－Ｙ２－Ｚ２を設定するために計測される部分であり、作業機３に含まれる。図７に示すように、第１ターゲット部は、例えば、バケットピン３０である。ただし、第１ターゲット部は、アームピン２９であってもよい。或いは、第１ターゲット部は、ブーム１１、アーム１２、或いはバケット１３に含まれる特定の部分であってもよい。コントローラ２４は、第１ターゲット部として、バケットピン３０の互いに異なる少なくとも３つの位置を取得する。

第１ターゲット部の位置は、外部の位置計測装置４５によって計測される。位置計測装置４５は、計測対象の外部座標系Ｘ１－Ｙ１－Ｚ１における位置を計測する。位置計測装置４５は、例えばレーザートラッカーである。或いは、位置計測装置４５は、トータルステーション、ステレオカメラなどの他の位置計測装置であってもよい。作業機械１のオペレータは、第１車体４を第２車体５に対して旋回させずに静止させた状態で、作業機３を第１車体４に対して、作業機平面５０内で動作させる。このとき、位置計測装置４５によって、第１ターゲット部の互いに異なる複数の位置が計測される。

計測された第１ターゲット部の複数の位置は、コントローラ２４に入力される。位置計測装置４５は、コントローラ２４と通信可能であり、コントローラ２４に、第１ターゲット部の複数の位置を示すデータを送信する。或いは、第１ターゲット部の複数の位置を示すデータは、入力装置３４を介して、手入力によって、コントローラ２４に入力されてもよい。

ステップＳ１０２では、コントローラ２４は、第１姿勢データを取得する。第１姿勢データは、上述したように、姿勢センサ３７が検出した第１車体４のピッチ角θ_ｐ１を含む。

ステップＳ１０３では、コントローラ２４は、作業機平面５０を算出する。作業機平面５０は、第１ターゲット部の複数の位置を含む。なお、第１ターゲット部の複数の位置が４点以上の場合は、作業機平面５０は、複数の位置から算出される最小二乗平面であってもよい。作業機平面５０は以下の式（１）で示される。以下の式（１）において、ａ_１，ｂ_１，ｃ_１は作業機平面５０の法線ベクトルを示す。コントローラ２４は、第１ターゲット部の複数の位置に基づいて、作業機平面５０の法線ベクトルを算出する。

ステップＳ１０４では、コントローラ２４は、車体座標系Ｘ２－Ｙ２－Ｚ２を設定する。コントローラ２４は、作業機平面５０の法線ベクトルに基づいて、車体座標系Ｘ２－Ｙ２－Ｚ２のＹ２軸の方向を決定する。コントローラ２４は、姿勢センサ３７が検出した第１車体４のピッチ角θ_ｐ１に基づいて、車体座標系Ｘ２－Ｙ２－Ｚ２のＸ２軸の方向を決定する。そして、コントローラ２４は、Ｙ２軸の方向とＸ２軸の方向とに基づいて、車体座標系Ｘ２－Ｙ２－Ｚ２のＺ２軸の方向を決定する。詳細には、コントローラ２４は、以下のように、車体座標系Ｘ２－Ｙ２－Ｚ２の外部座標系Ｘ１－Ｙ１－Ｚ１に対する回転行列Ｒを算出することで、車体座標系Ｘ２－Ｙ２－Ｚ２を設定する。

車体座標系Ｘ２－Ｙ２－Ｚ２の回転行列Ｒは、車体座標系Ｘ２－Ｙ２－Ｚ２の回転姿勢を表すオイラー角として、ヨー角θ_Ｙ２、ピッチ角θ_ｐ２、ロール角θ_ｒ２を用いて、以下の式（２）で示される。

車体座標系Ｘ２－Ｙ２－Ｚ２のＹ２軸方向ベクトルは、上述した作業機平面５０の法線ベクトルと一致する。従って、式（２）のＹ２軸方向ベクトルと作業機平面５０の法線ベクトルとから、以下の式（３）が成り立つ。

車体座標系Ｘ２－Ｙ２－Ｚ２のピッチ角θ_ｐ２は、姿勢センサ３７によって検出されるピッチ角θ_ｐ1と一致する。従って、姿勢センサ３７によって検出されるピッチ角θ_ｐ1を式（３）のピッチ角θ_ｐ２に代入して式（３）を展開することで、以下の式（４）、（５）、（６）に示すように、車体座標系Ｘ２－Ｙ２－Ｚ２の回転姿勢を示すオイラー角として、ピッチ角θ_ｐ２、ロール角θ_ｒ２、ヨー角θ_ｙ２がそれぞれ求まる。

そして、式（４）、（５）、（６）のヨー角θ_ｙ２、ピッチ角θ_ｐ２、ロール角θ_ｒ２を上記の式（２）に代入することで、車体座標系Ｘ２－Ｙ２－Ｚ２の回転行列Ｒが求まる。

次に、図６に示すように、ステップＳ１０５，Ｓ１０６において、コントローラ２４は、設定された車体座標系Ｘ２－Ｙ２－Ｚ２を用いて、機械パラメータの較正を行う。ステップＳ１０５では、コントローラ２４は、第２ターゲット部の位置を取得する。第２ターゲット部は、機械パラメータの較正を行うために計測される部分である。

図８に示すように、第２ターゲット部は、ブームピン２８とアンテナ位置Ｐ１とを含む。コントローラ２４は、ブームピン２８とアンテナ位置Ｐ１とを取得する。図９に示すように、第２ターゲット部は、バケットピン３０を含む。オペレータは、作業機３を動作させることで、互いに異なる複数のバケットピン３０の位置を取得する。図１０に示すように、第２ターゲット部は、刃先位置Ｐ２を含む。オペレータは、作業機３を動作させることで、互いに異なる複数の刃先位置Ｐ２を取得する。ただし、第２ターゲット部は、上述した部分以外の部分であってもよい。例えば、第２ターゲット部は、アームピン２９を含んでもよい。なお、コントローラ２４は、作業機３の１つの姿勢での複数の第２ターゲット部の位置を取得してもよい。

第２ターゲット部の位置は、外部の位置計測装置４５によって計測される。位置計測装置４５は、カメラ４６を備えており、カメラ４６が捉えた計測対象の位置をロックオンして計測する。コントローラ２４は、車体座標系Ｘ２－Ｙ２－Ｚ２における第２ターゲット部の位置を算出し、設定された車体座標系Ｘ２－Ｙ２－Ｚ２を用いて、車体座標系Ｘ２－Ｙ２－Ｚ２における第２ターゲット部の位置を外部座標系Ｘ１－Ｙ１－Ｚ１における第２ターゲット部の位置に変換して、位置計測装置４５に送信する。位置計測装置４５は、コントローラ２４から受信した第２ターゲット部の位置にカメラ４６を向けることで、第２ターゲット部の位置をロックオンして計測する。位置計測装置４５は、計測した第２ターゲット部の位置（以下、「外部計測値」と呼ぶ）をコントローラ２４に送信する。

上述したコントローラ２４から位置計測装置４５に送信される第２ターゲット部の位置は、較正前の機械パラメータを用いて算出されるため、精度の高くない概算位置である。しかし、位置計測装置４５は、第２ターゲット部の概算位置に基づいて、第２ターゲットが移動しても、カメラ４６により自動的に第２ターゲット部をロックオンすることができる。それにより、位置計測装置４５は、ロックオンする第２ターゲット部を自動で切り替えることができる。例えば、位置計測装置４５は、ロックオンする第２ターゲット部を、アームピン２９からバケットピン３０に自動で切り替えることができる。

ステップＳ１０６では、コントローラ２４は、機械パラメータの較正を実行する。コントローラ２４は、第１姿勢データと、第２姿勢データと、機械パラメータとに基づいて、外部座標系Ｘ１－Ｙ１－Ｚ１における第２ターゲット部の位置（以下、「演算値」と呼ぶ）を算出する。コントローラ２４は、第２ターゲット部の位置の外部計測値と演算値との誤差に基づいて、機械パラメータの較正を行う。或いは、コントローラ２４は、第２ターゲット部の位置の外部計測値に基づいて、機械パラメータの較正を行ってもよい。機械パラメータの具体的な較正方法については、公知の較正方法が用いられてもよい。例えば、アームピン２９とバケットピン３０の位置が計測され、計測されたアームピン２９とバケットピン３０の位置から算出された距離によって、アーム１２の長さＬ２が較正されてもよい。或いは、作業機３の複数の姿勢でのアームピン２９の位置の計測値と演算値との誤差評価関数が最小となるように、アーム１２の長さが較正されてもよい。

そして、ステップＳ１０７において、コントローラ２４は、較正の精度を評価する。図１１に示すように、位置計測装置４５は、刃先位置Ｐ２とアンテナ位置Ｐ１とを計測し、コントローラ２４に送信する。コントローラ２４は、位置計測装置４５から受信した刃先位置Ｐ２とアンテナ位置Ｐ１とに基づいて、アンテナ位置Ｐ１から刃先位置Ｐ２に向かうベクトル（以下、「外部計測値」と呼ぶ）を算出する。

一方、コントローラ２４は、車体座標系Ｘ２－Ｙ２－Ｚ２を用いて、アンテナ位置Ｐ１から刃先位置Ｐ２に向かうベクトルを算出する。コントローラ２４は、位置センサ３６が検出した位置データを用いずに、第１姿勢データと第２姿勢データと機械パラメータとによって、アンテナ位置Ｐ１から刃先位置Ｐ２に向かうベクトル（以下、「演算値」と呼ぶ）を算出する。コントローラ２４は、アンテナ位置Ｐ１から刃先位置Ｐ２に向かうベクトルの外部計測値と演算値とを比較することで、誤差を算出する。

コントローラ２４は、アンテナ位置Ｐ１から刃先位置Ｐ２に向かうベクトルの外部計測値と演算値との誤差に基づいて、較正の精度を評価する。例えば、コントローラ２４は、誤差が閾値以下であるときに、較正が成功したことを示すメッセージ、或いは画像を、ディスプレイに表示してもよい。コントローラ２４は、誤差が閾値より大きいときに、較正のやり直しを促すメッセージ、或いは画像を、ディスプレイに表示してもよい。

以上説明した本実施形態に係る作業機械１の制御システムでは、第１車体４の旋回平面を求める代わりに、姿勢センサ３７が検出した第１車体４の姿勢に基づいて、車体座標系Ｘ２－Ｙ２－Ｚ２が設定される。そのため、作業機械１において、第１車体４を旋回させることなく、精度よく車体座標系Ｘ２－Ｙ２－Ｚ２を設定することができる。

以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。

作業機械１は、油圧ショベルに限らず、クレーン、ホイールローダ、或いはモータグレーダなど第１車体と、第１車体に旋回可能に接続された第２車体とを含む他の種類の作業機械であってもよい。作業機械１の構成は上述したものに限らず、他のアタッチメントに変更されてもよい。

コントローラ２４は、複数のプロセッサを備えてもよい。複数のプロセッサによって上記の処理が分散して実行されてもよい。コントローラ２４は１台に限らず、複数のコントローラによって、上記の処理が分散して実行されてもよい。コントローラ２４は、作業機械１の外部に配置されてもよい。機械パラメータは、上記の実施形態のものに限らず、変更、省略、或いは追加されてもよい。

作業機械１は、遠隔から操作可能であってもよい。その場合、操作装置３３は、作業機械１の外部に配置されてもよい。作業機械１は、自動的に制御されてもよい。その場合、操作装置３３は省略されてもよい。

コントローラ２４は、算出された刃先位置Ｐ２を用いて、作業機械１の自動制御を行ってもよい。例えば、図１２に示すように、コントローラ２４は、施工対象の現況地形５１を示す現況地形データを取得する。コントローラ２４は、掘削などの施工作業を行うためのバケット１３の刃先の目標軌跡５２を取得する。コントローラ２４は、算出された刃先位置Ｐ２が目標軌跡５２に沿って移動するように、作業機３を自動的に制御する。

或いは、コントローラ２４は、算出された刃先位置Ｐ２を用いて、アシスタント画面をディスプレイに表示してもよい。例えば、図１３に示すように、アシスタント画面５３は、現況地形５４を示す画像と目標地形５５を示す画像とを含む。コントローラ２４は、算出した刃先位置Ｐ２に基づいて、現況地形５４と目標地形５５とに対応する位置に、作業機械１と刃先位置Ｐ２とを示す画像を表示する。

本発明によれば、作業機械において、第１車体を旋回させることなく、精度よく車体座標系を設定することができる。

３・・・作業機
４・・・第１車体
５・・・第２車体
２８・・・ブームピン（第２ターゲット部）
３０・・・バケットピン（第１ターゲット部、第２ターゲット部）
３６・・・位置センサ
３７・・・姿勢センサ
４５・・・位置計測装置
５２・・・目標軌跡
Ｐ１・・・アンテナ位置（第２ターゲット部）
Ｐ２・・・刃先位置（第２ターゲット部）
Ｘ１－Ｙ１－Ｚ１・・・外部座標系
Ｘ２－Ｙ２－Ｚ２・・・車体座標系

Claims

第１車体と、前記第１車体と旋回可能に接続された第２車体と、前記第１車体に対して動作可能に取り付けられた作業機と、を備える作業機械において、前記第１車体を基準とする車体座標系を設定するためのシステムであって、
前記第１車体の姿勢を検出する姿勢センサと、
前記作業機に含まれる第１ターゲット部の位置を計測する位置計測装置と、
コントローラと、
を備え、
前記コントローラは、
前記第１車体を前記第２車体に対して静止させた状態での前記第１車体の姿勢を、前記姿勢センサから取得し、
前記第１車体の姿勢と前記第１ターゲット部の位置とに基づいて、前記車体座標系を設定する、
システム。
前記コントローラは、
前記第１車体を前記第２車体に対して静止させた状態で前記作業機を前記第１車体に対して動作させることで、前記第１ターゲット部の複数の位置を取得し、
前記第１車体の姿勢と前記第１ターゲット部の複数の位置とに基づいて、前記車体座標系を設定する、
請求項１に記載のシステム。
前記作業機は、前記第１車体の上下方向及び前後方向に延びる作業機平面内で動作可能に、前記第１車体に取り付けられており、
前記コントローラは、
前記第１車体を前記第２車体に対して静止させた状態で、前記作業機を動作させることで、前記作業機平面内における前記第１ターゲット部の複数の位置を取得し、
前記第１ターゲット部の複数の位置に基づいて、前記作業機平面の法線ベクトルを算出し、
前記作業機平面の法線ベクトルに基づいて、前記第１車体の左右方向を示す前記車体座標系のｙ軸の方向を決定する、
請求項１に記載のシステム。
前記第１車体の姿勢は、前記第１車体のピッチ角を含み、
前記コントローラは、前記第１車体のピッチ角に基づいて、前記第１車体の前後方向を示す前記車体座標系のｘ軸の方向を決定する、
請求項１に記載のシステム。
前記作業機は、前記第１車体の上下方向及び前後方向に延びる作業機平面内で動作可能に、前記第１車体に取り付けられており、
前記第１車体の姿勢は、前記第１車体のピッチ角を含み、
前記コントローラは、
前記第１車体を前記第２車体に対して静止させた状態で、前記作業機を動作させることで、前記作業機平面内における前記第１ターゲット部の複数の位置を取得し、
前記第１ターゲット部の複数の位置に基づいて、前記作業機平面の法線ベクトルを算出し、
前記作業機平面の法線ベクトルに基づいて、前記第１車体の左右方向を示す前記車体座標系のｙ軸の方向を決定し、
前記第１車体のピッチ角に基づいて、前記第１車体の前後方向を示す前記車体座標系のｘ軸の方向を決定し、
前記ｙ軸の方向と前記ｘ軸の方向とに基づいて、前記第１車体の上下方向を示す前記車体座標系のｚ軸の方向を決定する、
請求項１に記載のシステム。
前記作業機械は、位置センサをさらに備え、
前記位置センサは、前記作業機械の外部を基準とする外部座標系における前記位置センサの位置を検出し、
前記コントローラは、
前記車体座標系における前記位置センサと前記作業機の刃先との位置関係を規定する機械パラメータを記憶しており、
前記外部座標系における前記位置センサの位置と、前記機械パラメータとに基づいて、前記外部座標系における前記刃先の位置を算出する、
請求項１に記載のシステム。
前記コントローラは、
所定の施工作業を行うための前記刃先の目標軌跡を取得し、
前記外部座標系における前記刃先の位置に基づいて、前記刃先が前記目標軌跡に従って移動するように、前記作業機を制御する、
請求項６に記載のシステム。
前記位置計測装置は、前記外部座標系における前記作業機に含まれる第２ターゲット部の位置を計測し、
前記コントローラは、
前記作業機の寸法を規定する機械パラメータを記憶しており、
前記計測された第２ターゲット部の位置に基づいて、前記機械パラメータの較正を行う、
請求項１に記載のシステム。
前記コントローラは、前記外部座標系における前記第２ターゲット部の位置を前記位置計測装置に送信し、
前記位置計測装置は、前記コントローラから受信した前記第２ターゲット部の位置に基づいて、前記第２ターゲット部の複数の位置の計測を自動的に切り替える、
請求項８に記載のシステム。
前記位置計測装置は、前記外部座標系における前記作業機の刃先の位置と、前記外部座標系における前記位置センサの位置とを計測し、
前記コントローラは、
前記計測された刃先の位置と、前記計測された位置センサの位置とに基づいて、前記位置センサに対する前記刃先の位置を示す外部計測値を生成し、
前記較正された機械パラメータに基づいて、前記位置センサに対する前記刃先の位置を算出することで、前記位置センサに対する前記刃先の位置を示す演算値を生成し、
前記外部計測値と、前記演算値とを比較することで、前記機械パラメータの較正の精度を評価する、
請求項８に記載のシステム。
第１車体と、前記第１車体と旋回可能に接続された第２車体と、前記第１車体に対して動作可能に取り付けられた作業機と、を備える作業機械において、前記第１車体を基準とする車体座標系を設定するための方法であって、
前記第１車体の姿勢を検出する姿勢センサにより、前記第１車体を前記第２車体に対して静止させた状態での前記第１車体の姿勢を取得することと、
前記作業機に含まれる第１ターゲット部の位置を計測することと、
前記第１車体の姿勢と前記第１ターゲット部の位置とに基づいて、前記車体座標系を設定すること、
を備える方法。
前記第１車体を前記第２車体に対して静止させた状態で前記作業機を前記第１車体に対して動作させることで、前記第１ターゲット部の複数の位置を取得することと、
前記第１車体の姿勢と前記第１ターゲット部の複数の位置とに基づいて、前記車体座標系を設定すること、
をさらに備える請求項１１に記載の方法。
前記作業機は、前記第１車体の上下方向及び前後方向に延びる作業機平面内で動作可能に、前記第１車体に取り付けられており、
前記第１車体を前記第２車体に対して静止させた状態で、前記作業機を動作させることで、前記作業機平面内における前記第１ターゲット部の複数の位置を取得することと、
前記第１ターゲット部の複数の位置に基づいて、前記作業機平面の法線ベクトルを算出することと、
前記作業機平面の法線ベクトルに基づいて、前記第１車体の左右方向を示す前記車体座標系のｙ軸の方向を決定すること、
をさらに備える請求項１１に記載の方法。
前記第１車体の姿勢は、前記第１車体のピッチ角を含み、
前記第１車体のピッチ角に基づいて、前記第１車体の前後方向を示す前記車体座標系のｘ軸の方向を決定すること、
をさらに備える請求項１１に記載の方法。
前記作業機は、前記第１車体の上下方向及び前後方向に延びる作業機平面内で動作可能に、前記第１車体に取り付けられており、
前記第１車体の姿勢は、前記第１車体のピッチ角を含み、
前記第１車体を前記第２車体に対して静止させた状態で、前記作業機を動作させることで、前記作業機平面内における前記第１ターゲット部の複数の位置を取得することと、
前記第１ターゲット部の複数の位置に基づいて、前記作業機平面の法線ベクトルを算出することと、
前記作業機平面の法線ベクトルに基づいて、前記第１車体の左右方向を示す前記車体座標系のｙ軸の方向を決定することと、
前記第１車体のピッチ角に基づいて、前記第１車体の前後方向を示す前記車体座標系のｘ軸の方向を決定することと、
前記ｙ軸の方向と前記ｘ軸の方向とに基づいて、前記第１車体の上下方向を示す前記車体座標系のｚ軸の方向を決定すること、
をさらに備える請求項１１に記載の方法。
前記作業機械は、位置センサをさらに備え、
前記位置センサは、前記作業機械の外部を基準とする外部座標系における前記位置センサの位置を検出し、
前記車体座標系における前記位置センサと前記作業機の刃先との位置関係を規定する機械パラメータと、前記外部座標系における前記位置センサの位置とに基づいて、前記外部座標系における前記刃先の位置を算出すること、
をさらに備える請求項１１に記載の方法。
所定の施工作業を行うための前記刃先の目標軌跡を取得することと、
前記外部座標系における前記刃先の位置に基づいて、前記刃先が前記目標軌跡に従って移動するように、前記作業機を制御すること、
をさらに備える請求項１６に記載の方法。
位置計測装置によって計測された、前記外部座標系における前記作業機に含まれる第２ターゲット部の位置を取得することと、
前記作業機の寸法を規定する機械パラメータを、前記計測された第２ターゲット部の位置に基づいて較正すること、
をさらに備える請求項１１に記載の方法。
前記外部座標系における前記第２ターゲット部の位置を前記位置計測装置に送信することと、
前記位置計測装置が、受信した前記第２ターゲット部の位置に基づいて、前記第２ターゲット部の複数の位置の計測を切り替えること、
をさらに備える請求項１８に記載の方法。
前記位置計測装置によって計測された、前記外部座標系における前記作業機の刃先の位置と、前記外部座標系における前記位置センサの位置とを取得することと、
前記計測された刃先の位置と、前記計測された位置センサの位置とに基づいて、前記位置センサに対する前記刃先の位置を示す外部計測値を生成することと、
前記較正された機械パラメータに基づいて、前記位置センサに対する前記刃先の位置を算出することで、前記位置センサに対する前記刃先の位置を示す演算値を生成することと、
前記外部計測値と、前記演算値とを比較することで、前記機械パラメータの較正の精度を評価すること、
をさらに備える請求項１８に記載の方法。