本発明を実施するための形態(本実施形態)につき、図面を参照しつつ詳細に説明する。
実施形態1.
<作業機械の全体構成>
図1は、実施形態1に係る作業機械の斜視図である。図2は、制御システム200及び油圧システム300の構成を示すブロック図である。作業機械としての油圧ショベル100は、本体部としての車両本体1と作業機2とを有する。車両本体1は、旋回体である上部旋回体3と走行体としての走行装置5とを有する。上部旋回体3は、機械室3EGの内部に、動力発生装置であるエンジン及び油圧ポンプ等の装置を収容している。
本実施形態において、油圧ショベル100は、動力発生装置であるエンジンに、例えばディーゼルエンジン等の内燃機関が用いられるが、動力発生装置は内燃機関に限定されない。油圧ショベル100の動力発生装置は、例えば、内燃機関と発電電動機と蓄電装置とを組み合わせた、いわゆるハイブリッド方式の装置であってもよい。また、油圧ショベル100の動力発生装置は、内燃機関を有さず、蓄電装置と発電電動機とを組み合わせた装置であってもよい。
上部旋回体3は、運転室4を有する。運転室4は、上部旋回体3の他端側に設置されている。すなわち、運転室4は、機械室3EGが配置されている側とは反対側に設置されている。運転室4内には、図2に示す、表示部29及び操作装置25が配置される。上部旋回体3の上方には、手すり9が取り付けられている。
走行装置5の上には、上部旋回体3が搭載されている。走行装置5は、履帯5a、5bを有している。走行装置5は、左右に設けられた油圧モータ5cの一方又は両方によって駆動される。走行装置5の履帯5a、5bが回転することにより、油圧ショベル100を走行させる。作業機2は、上部旋回体3の運転室4の側方側に取り付けられている。
油圧ショベル100は、履帯5a、5bの代わりにタイヤを備え、エンジンの駆動力を、トランスミッションを介してタイヤへ伝達して走行が可能な走行装置を備えたものであってもよい。このような形態の油圧ショベル100としては、例えば、ホイール式油圧ショベルがある。
上部旋回体3は、作業機2及び運転室4が配置されている側が前であり、機械室3EGが配置されている側が後である。上部旋回体3の前後方向がx方向である。前に向かって左側が上部旋回体3の左であり、前に向かって右側が上部旋回体3の右である。上部旋回体3の左右方向は、幅方向又はy方向ともいう。油圧ショベル100又は車両本体1は、上部旋回体3を基準として走行装置5側が下であり、走行装置5を基準として上部旋回体3側が上である。上部旋回体3の上下方向がz方向である。油圧ショベル100が水平面に設置されている場合、下は鉛直方向、すなわち重力の作用方向側であり、上は鉛直方向とは反対側である。
作業機2は、ブーム6とアーム7と作業具であるバケット8とブームシリンダ10とアームシリンダ11とバケットシリンダ12とを有する。ブーム6の基端部は、ブームピン13を介して車両本体1の前部に回動可能に取り付けられている。アーム7の基端部は、アームピン14を介してブーム6の先端部に回動可能に取り付けられている。アーム7の先端部には、バケットピン15を介してバケット8が取り付けられている。バケット8は、バケットピン15を中心として回動する。バケット8は、バケットピン15とは反対側に複数の刃8Bが取り付けられている。刃先8Tは、刃8Bの先端である。
バケット8は、複数の刃8Bを有していなくてもよい。つまり、図1に示すような刃8Bを有しておらず、刃先が鋼板によってストレート形状に形成されたようなバケットであってもよい。作業機2は、例えば、単数の刃を有するチルトバケットを備えていてもよい。チルトバケットとは、バケットチルトシリンダを備え、バケットが左右にチルト傾斜することで油圧ショベル100が傾斜地にあっても、斜面、平地を自由な形に成形したり、整地したりすることができ、底板プレートによる転圧作業もできるバケットである。この他にも、作業機2は、バケット8の代わりに、法面バケット又は削岩用のチップを備えた削岩用のアタッチメント等を作業具として備えていてもよい。
図1に示すブームシリンダ10とアームシリンダ11とバケットシリンダ12とは、それぞれ作動油の圧力によって駆動される油圧シリンダである。以下において、作動油の圧力を、適宜油圧と称する。ブームシリンダ10はブーム6を駆動して、昇降させる。アームシリンダ11は、アーム7を駆動して、アームピン14の周りを回動させる。バケットシリンダ12は、バケット8を駆動して、バケットピン15の周りを回動させる。
ブームシリンダ10、アームシリンダ11及びバケットシリンダ12等の油圧シリンダと図2に示される油圧ポンプ36、37との間には、図2に示される方向制御弁64が設けられている。方向制御弁64は、油圧ポンプ36、37からブームシリンダ10、アームシリンダ11及びバケットシリンダ12等に供給される作動油の流量を制御するとともに、作動油が流れる方向を切り替える。方向制御弁64は、油圧モータ5cを駆動するための走行用方向制御弁と、ブームシリンダ10、アームシリンダ11及びバケットシリンダ12並びに上部旋回体3を旋回させる旋回モータ38を制御するための作業機用方向制御弁とを含む。
操作装置25から供給される、所定のパイロット圧力に調整された作動油が方向制御弁64のスプールを動作させると、方向制御弁64から流出する作動油の流量が調整されて、油圧ポンプ36、37からブームシリンダ10、アームシリンダ11、バケットシリンダ12、旋回モータ38又は油圧モータ5cに供給される作動油の流量が制御される。その結果、ブームシリンダ10、アームシリンダ11及びバケットシリンダ12等の動作が制御される。
また、図2に示される装置コントローラ39が、図2に示される制御弁27を制御することにより、操作装置25から方向制御弁64に供給される作動油のパイロット圧が制御されるので、方向制御弁64からブームシリンダ10、アームシリンダ11、バケットシリンダ12又は旋回モータ38に供給される作動油の流量が制御される。その結果、装置コントローラ39は、ブームシリンダ10、アームシリンダ11、バケットシリンダ12及び上部旋回体3の動作を制御することができる。
上部旋回体3の上部には、アンテナ21、22が取り付けられている。アンテナ21、22は、油圧ショベル100の現在位置を検出するために用いられる。アンテナ21、22は、図2に示されるグローバル座標演算装置23と電気的に接続されている。グローバル座標演算装置23は、油圧ショベル100の位置を検出する位置検出装置である。グローバル座標演算装置23は、RTK−GNSS(Real Time Kinematic - Global Navigation Satellite Systems、GNSSは全地球航法衛星システムをいう)を利用して油圧ショベル100の現在位置、より具体的には油圧ショベル100の一部の現在位置を検出する。以下の説明において、アンテナ21、22を、適宜GNSSアンテナ21、22と称する。本実施形態において、グローバル座標演算装置23は、油圧ショベル100の一部の現在位置として、GNSSアンテナ21、22の少なくとも1つの位置を検出する。GNSSアンテナ21、22が受信したGNSS電波に応じた信号は、グローバル座標演算装置23に入力される。グローバル座標演算装置23は、グローバル座標系におけるGNSSアンテナ21、22の設置位置を求める。全地球航法衛星システムの一例としては、GPS(Global Positioning System)が挙げられるが、全地球航法衛星システムは、これに限定されるものではない。
RTK−GNSSでは、測位衛星の配置、電離層、対流圏又はGNSSアンテナ周辺の地形の影響で測位の状態が変化する。この測位の状態には、例えば、Fix(精度±1cmから2cm程度)、Float(精度±10cmから数m程度)、単独測位(精度±数m程度)、非測位(測位計算不能)等がある。以下において、測位の状態がFixである場合を正常と称し、Fix以外の状態である場合を異常と称する。
GNSSアンテナ21、22は、図1に示すように、上部旋回体3の上であって、油圧ショベル100の左右方向、すなわち幅方向に離れた両端位置に設置されることが好ましい。本実施形態において、GNSSアンテナ21、22は、上部旋回体3の幅方向両側にそれぞれ取り付けられた手すり9に取り付けられる。GNSSアンテナ21、22が上部旋回体3に取り付けられる位置は手すり9に限定されるものではないが、GNSSアンテナ21、22は、可能な限り離れた位置に設置される方が、油圧ショベル100の現在位置の検出精度は向上するので好ましい。また、GNSSアンテナ21、22は、オペレータの視界を極力妨げない位置に設置されることが好ましい。例えば、GNSSアンテナ21、22は、機械室3EGの後方に配置されたカウンタウェイトの上に配置されてもよい。
図2に示すように、油圧ショベル100の油圧システム300は、エンジン35と、油圧ポンプ36、37とを備える。油圧ポンプ36、37は、エンジン35によって駆動され、作動油を吐出する。油圧ポンプ36、37から吐出された作動油は、ブームシリンダ10とアームシリンダ11とバケットシリンダ12とに供給される。また、油圧ショベル100は、旋回モータ38を備える。旋回モータ38は、油圧モータであり、油圧ポンプ36、37から吐出された作動油によって駆動される。旋回モータ38は、上部旋回体3を旋回させる。図2では、2つの油圧ポンプ36、37が図示されているが、油圧ポンプは1つでもよい。旋回モータ38は、油圧モータに限らず、電気モータであってもよい。
作業機械の制御システムである制御システム200は、グローバル座標演算装置23と、角速度及び加速度を検出する状態検出装置であるIMU(Inertial Measurement Unit:慣性計測装置)24と、操作装置25と、処理装置としての装置コントローラ39と、処理装置としての表示コントローラ28と、表示部29とを含む。操作装置25は、図1に示す作業機2、上部旋回体3及び走行装置5の少なくとも1つを操作するための装置である。操作装置25は、作業機2等を駆動させるためにオペレータによる操作を受け付けて、操作量に応じたパイロット油圧を出力する。
操作装置25は、オペレータの左側に設置される左操作レバー25Lと、オペレータの右側に配置される右操作レバー25Rと、を有する。左操作レバー25L及び右操作レバー25Rは、前後左右の動作が2軸の動作に対応されている。例えば、右操作レバー25Rの前後方向の操作は、ブーム6の操作に対応されている。例えば、右操作レバー25Rの左右方向の操作は、バケット8の操作に対応されている。例えば、左操作レバー25Lの前後方向の操作は、アーム7の操作に対応している。例えば、左操作レバー25Lの左右方向の操作は、上部旋回体3の旋回に対応している。
本実施形態において、操作装置25は、パイロット油圧方式が用いられる。操作装置25には、油圧ポンプ36から、図示しない減圧弁によって所定のパイロット圧力に減圧された作動油がブーム操作、バケット操作、アーム操作、旋回操作及び走行操作に基づいて供給される。
右操作レバー25Rの前後方向の操作に応じて、パイロット油路450へパイロット油圧が供給可能とされて、オペレータによるブーム6の操作が受け付けられる。右操作レバー25Rの操作量に応じて右操作レバー25Rが備える弁装置が開き、パイロット油路450へ作動油が供給される。また、圧力センサ66は、そのときのパイロット油路450内における作動油の圧力をパイロット圧として検出する。圧力センサ66は、検出したパイロット圧を、ブーム操作信号MBとして装置コントローラ39へ送信する。
操作装置25とブームシリンダ10との間のパイロット油路450には、圧力センサ68、制御弁(以下、適宜介入弁と称する)27C及びシャトル弁51が設けられる。右操作レバー25Rの左右方向の操作に応じて、パイロット油路450へパイロット油圧が供給可能とされて、オペレータによるバケット8の操作が受け付けられる。右操作レバー25Rの操作量に応じて右操作レバー25Rが備える弁装置が開き、パイロット油路450に作動油が供給される。また、圧力センサ66は、そのときのパイロット油路450内における作動油の圧力をパイロット圧として検出する。圧力センサ66は、検出したパイロット圧を、バケット操作信号MTとして装置コントローラ39へ送信する。
左操作レバー25Lの前後方向の操作に応じて、パイロット油路450へパイロット油圧が供給可能とされて、オペレータによるアーム7の操作が受け付けられる。左操作レバー25Lの操作量に応じて左操作レバー25Lが備える弁装置が開き、パイロット油路450へ作動油が供給される。また、圧力センサ66は、そのときのパイロット油路450内における作動油の圧力をパイロット圧として検出する。圧力センサ66は、検出したパイロット圧を、アーム操作信号MAとして装置コントローラ39へ送信する。
左操作レバー25Lの左右方向の操作に応じて、パイロット油路450へパイロット油圧が供給可能とされて、オペレータによる上部旋回体3の旋回操作が受け付けられる。左操作レバー25Lの操作量に応じて左操作レバー25Lが備える弁装置が開き、パイロット油路450へ作動油が供給される。また、圧力センサ66は、そのときのパイロット油路450内における作動油の圧力をパイロット圧として検出する。圧力センサ66は、検出したパイロット圧を、旋回操作信号MRとして装置コントローラ39へ送信する。
右操作レバー25Rが操作されることにより、操作装置25は、右操作レバー25Rの操作量に応じた大きさのパイロット油圧を方向制御弁64に供給する。左操作レバー25Lが操作されることにより、操作装置25は、左操作レバー25Lの操作量に応じた大きさのパイロット油圧を制御弁27に供給する。このパイロット油圧によって、方向制御弁64のスプールが移動する。
パイロット油路450には、制御弁27が設けられている。右操作レバー25R及び左操作レバー25Lの操作量は、パイロット油路450に設置される圧力センサ66によって検出される。圧力センサ66が検出したパイロット油圧は、装置コントローラ39に入力される。装置コントローラ39は、入力されたパイロット油圧に応じた、パイロット油路450の制御信号Nを制御弁27に出力して、パイロット油路450を開閉する。右操作レバー25R又は左操作レバー25Lの操作方向と操作対象(バケット8、アーム7、ブーム6、上部旋回体3)との関係は、上記に限定されるものではなく、異なる関係であってもよい。
操作装置25は、走行用レバー25FL、25FRを有する。本実施形態において、操作装置25は、パイロット油圧方式が用いられるので、油圧ポンプ36から、減圧された作動油が方向制御弁64に供給され、パイロット油路450内の作動油の圧力に基づき方向制御弁64のスプールが駆動される。すると、油圧ショベル100の走行装置5が備える油圧モータ5c、5cに、油圧ポンプ36、37から作動油が供給され、走行可能となる。パイロット油路450内の作動油の圧力、すなわちパイロット圧は、圧力センサ27PCによって検出される。
油圧ショベル100のオペレータが走行装置5を動作させる場合、オペレータは走行用レバー25FL、25FRを操作する。オペレータによる走行用レバー25FL、25FRの操作量は圧力センサ27PCで検出されて、装置コントローラ39へ操作信号MDとして出力される。
左操作レバー25L及び右操作レバー25Rの操作量が、例えば、ポテンショメータ及びホールIC等によって検出され、装置コントローラ39は、これらの検出値に基づいて方向制御弁64及び制御弁27を制御することによって、作業機2を制御してもよい。このように、左操作レバー25L及び右操作レバー25Rは、電気方式であってもよい。
制御システム200は、第1ストロークセンサ16と第2ストロークセンサ17と第3ストロークセンサ18とを有する。例えば、第1ストロークセンサ16はブームシリンダ10に、第2ストロークセンサ17はアームシリンダ11に、第3ストロークセンサ18はバケットシリンダ12に、それぞれ設けられる。第1ストロークセンサ16は、ブームシリンダ10の伸長に対応する変位量を検出して、装置コントローラ39に出力する。第2ストロークセンサ17は、アームシリンダ11の伸長に対応する変位量を検出して、装置コントローラ39に出力する。第3ストロークセンサ18は、バケットシリンダ12の伸長に対応する変位量を検出して、装置コントローラ39に出力する。
装置コントローラ39は、CPU(Central Processing Unit)等のプロセッサである処理部39Pと、RAM(Random Access Memory)及びROM(Read Only Memory)等の記憶装置である記憶部39Mとを有する。装置コントローラ39には、グローバル座標演算装置23の検出値、IMU24の検出値、圧力センサ27PC、66、68の検出値、第1ストロークセンサ16の検出値、第2ストロークセンサ17の検出値及び第3ストロークセンサ18の検出値が入力される。装置コントローラ39は、グローバル座標演算装置23の検出値及びIMU24の検出値から、油圧ショベル100の位置に関連する位置情報IPLを求めて表示コントローラ28に出力する。装置コントローラ39は、図2に示される圧力センサ66の検出値に基づいて、制御弁27及び介入弁27Cを制御する。
図2に示される方向制御弁64は、例えば比例制御弁であり、操作装置25から供給される作動油によって制御される。方向制御弁64は、ブームシリンダ10、アームシリンダ11、バケットシリンダ12及び旋回モータ38等の油圧アクチュエータと、油圧ポンプ36、37との間に配置される。方向制御弁64は、油圧ポンプ36、37からブームシリンダ10、アームシリンダ11、バケットシリンダ12及び旋回モータ38に供給される作動油の流量を制御する。
グローバル座標演算装置23は、図2に示される補正データ受信装置26から、補正データC1を受信する。補正データ受信装置26は、グローバル座標演算装置23に接続される。補正データC1は、油圧ショベル100の外部に設置されたGNSS受信機で生成された、RTK−GNSSで利用可能な情報であり、補正データ受信装置26と共通規格の通信機能を有する装置から送信された情報である。また、補正データ受信装置26を電話回線用のモデムとし、補正データ配信サービスを利用して外部から補正データC1を得るようにしてもよい。補正データ受信装置26は補正データC1をグローバル座標演算装置23に出力する。GNSSアンテナ21、GNSSアンテナ22は、複数の測位衛星から信号を受信し、グローバル座標演算装置23に出力する。
グローバル座標演算装置23は、GNSSアンテナ21、GNSSアンテナ22から入力された測位衛星の信号と、補正データ受信装置26から受信した補正データC1とに基づいて、GNSSアンテナ21の位置である基準位置データP1とGNSSアンテナ22の位置である基準位置データP2を測位する。グローバル座標演算装置23は、GNSSアンテナ21の位置である基準位置データP1とGNSSアンテナ22の位置である基準位置データP2との相対位置から旋回体方位データQを算出する。GNSSアンテナ21、22及びグローバル座標演算装置23によりGPSコンパスを構成し、旋回体方位データQを得るようにしてもよい。
GNSSアンテナ21、22が設置される部分は、油圧ショベル100の一部である。したがって、基準位置データP1、P2は、油圧ショベル100の一部、具体的にはGNSSアンテナ21、22が設置される部分の位置を示す情報である。以下において、GNSSアンテナ21、22が設置される部分の位置を、適宜、第1位置と称する。基準位置データP1、P2は、第1位置の情報である。
本実施形態において、旋回体方位データQは、GNSSアンテナ21、22が取得した基準位置データP、すなわち基準位置データP1及び基準位置データP2の少なくとも一方から決定される方位が、グローバル座標の基準方位(例えば北)に対してなす角、すなわち方位角である。方位角は、油圧ショベル100のヨー角でもある。旋回体方位データQは、上部旋回体3、すなわち作業機2が向いている方位を示している。
グローバル座標演算装置23は、CPU等のプロセッサである処理部と、RAM及びROM等の記憶装置である記憶部とを有する。グローバル座標演算装置23は、測位した2つの基準位置データP1、P2、すなわち基準位置データPと旋回体方位データQを、装置コントローラ39に出力する。
表示コントローラ28は、CPU等のプロセッサである処理部28Pと、RAM及びROM等の記憶装置である記憶部28Mとを有する。表示コントローラ28は、表示部29に、例えば、後述するガイダンス画面等の画像を表示する他、装置コントローラ39から得られる油圧ショベル100の位置情報IPLを用いて、バケット8の刃先8Tの3次元位置である刃先位置を示すバケット刃先位置データSを生成する。表示部29は、例えば、液晶表示装置等であるが、これに限定されるものではない。表示部29は、例えば入力部と表示部を一体化したタッチパネルを用いることができる。本実施形態においては、表示部29に隣接して、スイッチ29Sが設置されている。スイッチ29Sは、後述する掘削制御を実行させたり、実行中の掘削制御を停止させたりするための入力装置である。表示部29にタッチパネルを用いる場合、スイッチ29Sはタッチパネルの入力部に組み込まれてもよい。
表示コントローラ28は、作業機2が掘削する対象の目標施工面の画像と、バケット刃先位置データSを用いて生成したバケット8の画像とをガイダンス画面として表示部29に表示させることができる。表示コントローラ28は、ガイダンス画面により、目標施工面とバケット8との位置関係を油圧ショベル100のオペレータに認識させ、情報化施工を行う際のオペレータの負担を軽減することができる。
IMU24は、油圧ショベル100の動作を示す動作情報MIを検出する状態検出装置である。油圧ショベル100の動作は、上部旋回体3の動作及び走行装置5の動作の少なくとも一方を含む。本実施形態において、動作情報MIは、油圧ショベル100の姿勢を示す情報を含んでいてもよい。油圧ショベル100の姿勢を示す情報は、油圧ショベル100のロール角、ピッチ角及び方位角が例示される。
本実施形態において、IMU24は、油圧ショベル100の角速度及び加速度を検出する。油圧ショベル100の動作にともない、油圧ショベル100には、走行時に発生する加速度、旋回時に発生する角加速度及び重力加速度といった様々な加速度が生じるが、IMU24は少なくとも重力加速度を含む加速度を検出し、各加速度の種類を区別することなく検出した加速度を出力する。重力加速度は、重力に対応した加速度である。IMU24は、図1に示される車体座標系(x、y、z)において、x軸方向、y軸方向及びz軸方向の加速度aと、x軸、y軸及びz軸周りの角速度(回転角速度)ωとを検出する。これらが動作情報MIとなる。車体座標系とは、油圧ショベル100を基準とした、(x、y、z)で示される3次元座標系である。
IMU24が検出する動作情報MIには、上部旋回体3の回転中心軸となるz軸を中心として上部旋回体3が旋回する際の角速度ωが含まれる。旋回時の角速度ωは、GNSSアンテナ21、22の位置を示す情報から取得された上部旋回体3の旋回角度を時間で微分することにより求められてもよい。旋回時の角速度ωを時間で積分することにより、旋回角度を求めることができる。
IMU24は、上部旋回体3に取り付けられている。IMU24は、より高い精度で加速度等を検出するために、例えば、油圧ショベル100の上部旋回体3の旋回中心軸上に設けられることが望ましいが、IMU24は運転室4の下部に設置されてもよい。
図3は、油圧ショベル100の側面図である。図4は、油圧ショベル100の背面図である。車両本体1の左右方向、すなわち幅方向に対する傾斜角θ4は油圧ショベル100のロール角であり、車両本体1の前後方向に対する傾斜角θ5は油圧ショベル100のピッチ角であり、z軸周りにおける上部旋回体3の角度は油圧ショベル100の方位角である。ロール角はIMU24によって検出されたx軸周りの角速度を時間で積分することにより、ピッチ角はIMU24によって検出されたy軸周りの角速度を時間で積分することにより、方位角はIMU24によって検出されたz軸周りの角速度を時間で積分することにより求められる。z軸周りの角速度は、油圧ショベル100の旋回時の角速度ωである。すなわち、旋回時の角速度ωを時間で積分することにより油圧ショベル100、より具体的には上部旋回体3の方位角が得られる。
IMU24は、所定の周期で油圧ショベル100の加速度及び角速度を更新する。IMU24の更新周期は、グローバル座標演算装置23における更新周期よりも短いことが好ましい。IMU24が検出した加速度及び角速度は、動作情報MIとして装置コントローラ39に出力される。装置コントローラ39は、IMU24から取得した動作情報MIにフィルタ処理及び積分といった処理を施して、ロール角である傾斜角θ4、ピッチ角である傾斜角θ5及び方位角を求める。装置コントローラ39は、求めた傾斜角θ4、傾斜角θ5及び方位角を、油圧ショベル100の位置に関連する位置情報IPLとして、表示コントローラ28に出力する。
表示コントローラ28は、グローバル座標演算装置23から基準位置データP及び旋回体方位データQを取得する。旋回体方位データQは、油圧ショベル100の方位を示す情報であり、本実施形態においては、上部旋回体3の方位を示す情報である。具体的には、旋回体方位データQは、上部旋回体3の方位角である。本実施形態において、表示コントローラ28は、作業機位置データとして、バケット刃先位置データSを生成する。バケット刃先位置データSは、装置コントローラ39によって生成されてもよい。そして、表示コントローラ28は、バケット刃先位置データSと、目標施工情報Tとを用いて、掘削対象の目標形状を示す目標掘削地形データUを生成する。目標施工情報Tは、表示コントローラ28の記憶部28M(目標施工情報格納部28C)に記憶されている。目標施工情報Tは、油圧ショベル100が備える作業機2の掘削対象の掘削後における仕上がりの目標となる情報であり、設計データから得られる目標施工面の情報を含む。目標掘削地形データUは、車体座標系において刃先8Tの現時点における刃先位置を通る垂線と、目標施工面との交点を掘削対象位置としたとき、掘削対象位置の前後における単数又は複数の変曲点の位置を示す情報とその前後の線の角度情報である。
装置コントローラ39は、第1ストロークセンサ16が検出したブームシリンダ長から、車体座標系における水平面と直交する方向(z軸方向)に対するブーム6の傾斜角θ1(図3参照)を算出する。装置コントローラ39は、第2ストロークセンサ17が検出したアームシリンダ長から、ブーム6に対するに対するアーム7の傾斜角θ2(図3参照)を算出する。装置コントローラ39は、第3ストロークセンサ18が検出したバケットシリンダ長から、アーム7に対するバケット8の傾斜角θ3を算出する。IMU24は、旋回時の角速度ωを装置コントローラ39に出力する。
装置コントローラ39は、前述したように、図1に示されるz軸を中心として上部旋回体3が旋回するときにおける上部旋回体3の旋回時の角速度ωを、IMU24から取得する。また、装置コントローラ39は、圧力センサ66からブーム操作信号MB、バケット操作信号MT、アーム操作信号MA及び旋回操作信号MRを取得する。
装置コントローラ39は、表示コントローラ28から、目標掘削地形データUを取得する。装置コントローラ39は、自身が求めた作業機2の角度(θ1、θ2、θ3)から、バケット8の刃先8Tの位置(以下、適宜刃先位置と称する)を求める。装置コントローラ39の記憶部39Mは、作業機2のデータ(以下、適宜作業機データという)を記憶している。作業機データは、ブーム6の長さL1、アーム7の長さL2及びバケット8の長さL3といった設計寸法を含む。図3に示されるように、ブーム6の長さL1は、ブームピン13からアームピン14までの長さに相当する。アーム7の長さL2は、アームピン14からバケットピン15までの長さに相当する。バケット8の長さL3は、バケットピン15からバケット8の刃先8Tまでの長さに相当する。刃先8Tは、図1に示す刃8Bの先端である。また、作業機データは、車体座標系の位置PLに対するブームピン13までの位置の情報を含む。装置コントローラ39は、長さL1、L2、L3、傾斜角θ1、θ2、θ3及び位置PLを用いて、位置PLに対する刃先位置を求めることができる。
装置コントローラ39は、目標掘削地形データUに沿ってバケット8の刃先8Tが移動するように、操作装置25から入力されたブーム操作信号MB、バケット操作信号MT及びアーム操作信号MAを、目標掘削地形データUとバケット8の刃先8Tとの距離及びバケット8の刃先8Tの速度に基づき調整する。装置コントローラ39は、目標掘削地形データUに沿ってバケット8の刃先8Tが移動するように作業機2を制御するための制御信号Nを生成して、図2に示される制御弁27に出力する。このような処理により、作業機2が目標掘削地形データUに近づく速度は、目標掘削地形データUに対する距離に応じて制限される。
装置コントローラ39からの制御信号Nに応じて、ブームシリンダ10、アームシリンダ11及びバケットシリンダ12のそれぞれに対して2個ずつ設けられた制御弁27が開閉する。左操作レバー25L又は右操作レバー25Rの操作と制御弁27の開閉指令とに基づき、方向制御弁64のスプールが動作して、ブームシリンダ10、アームシリンダ11及びバケットシリンダ12へ作動油が供給される。
グローバル座標演算装置23は、グローバル座標系におけるGNSSアンテナ21、22の基準位置データP1、P2を検出する。本実施形態において、グローバル座標系とは、例えば、GNSSにおける座標系である。図3では、グローバル座標系は、(Xg、Yg、Zg)で示される3次元座標系である。現場座標系は、油圧ショベル100の作業エリアGAに設置された基準となる、例えば基準杭60の位置PGを基準とした、(X、Y、Z)で示される3次元座標系である。図3に示されるように、位置PGは、例えば、作業エリアGAに設置された基準杭60の先端60Tに位置する。グローバル座標系(Xg、Yg、Zg)と現場座標系(X、Y、Z)とは、互いに変換できる。
図2に示される表示コントローラ28は、グローバル座標演算装置23による検出結果に基づいて、グローバル座標系での車体座標系の位置を求める。本実施形態において、例えば、車体座標系の位置PLは、旋回体の回転中心軸であるz軸と走行装置5が接地する面に相当する面との交点である。本実施形態において、位置PLの座標は、車体座標系において、(0、0、0)となる。走行装置5が接地する面は、履帯5a、5bが接する作業エリアGAの表面GDである。走行装置5が接地する面に相当する面は、作業エリアGAの表面GDであってもよいし、履帯5a、5bが接地する部分によって規定される平面CPであってもよい。履帯5a、5bが接地する部分によって規定される平面CPは、車体座標系(x、y、z)において、油圧ショベル100の設計寸法から一義的に決定される。
位置PLは、z軸と平面CPとの交点に限定されるものではない。本実施形態において、後述する疑似不動点の位置は、位置PLと一致していてもよいし、一致していなくてもよい。車体座標系の位置PLは、他の場所であってもよく、例えば、ブームピン13の軸方向の長さの中心点を位置PLとしてもよい。位置PLは、z軸上、かつ上部旋回体3が旋回するためのスイングサークル上に位置していてもよい。前述したように、装置コントローラ39は、位置PLに対する刃先位置、すなわち車体座標系での刃先位置を求めるので、グローバル座標系での位置PLの座標が得られれば、車体座標系での刃先位置の座標を、グローバル座標系での刃先位置の座標に変換することができる。
装置コントローラ39は、バケット8が目標掘削地形を侵食することを抑制するために、作業機2が掘削対象に接近する方向の速度が制限速度以下になるように制御する。この制御を、適宜掘削制御という。掘削制御は、表示コントローラ28から取得された目標掘削地形データU及びバケット刃先位置データSとに基づいて、作業機2と掘削対象との相対位置を演算しながら作業機2が掘削対象に接近する方向の速度を制限速度以下になるようにする制御である。このような制御を実行することで、掘削対象を目標形状(目標施工情報Tが示す形状)に施工することができる。次に、制御システム200について、より詳細に説明する。
<制御システム200>
図5は、実施形態1に係る制御システム200の制御ブロック図である。本実施形態において、制御システム200の装置コントローラ39と表示コントローラ28とは、信号線を介して互いに情報をやり取りすることができる。また、装置コントローラ39は、グローバル座標演算装置23から信号線を介して情報を取得することができる。制御システム200内で情報を伝達する信号線は、CAN(Controller Area Network)のような車内信号線が例示される。本実施形態において、制御システム200は、装置コントローラ39と、表示コントローラ28とが別個の装置であるが、両者は1つの装置で実現されてもよい。
表示コントローラ28は、刃先位置算出部28Aと、目標掘削地形データ生成部28Bと、目標施工情報格納部28Cとを有する。刃先位置算出部28A及び目標掘削地形データ生成部28Bは、記憶部28Mに記憶されたコンピュータプログラムを処理部28Pが実行することにより実現される。目標施工情報格納部28Cは、記憶部28Mの記憶領域の一部によって実現される。
刃先位置算出部28Aは、装置コントローラ39から取得する位置情報IPLに基づいて、上部旋回体3の旋回中心軸となるz軸を通る、油圧ショベル100の旋回中心の位置を示す旋回中心位置データXRを生成する。刃先位置算出部28Aが装置コントローラ39から取得する位置情報IPLは、基準位置データP1、P2に基づく基準位置データP1c、P2c及び油圧ショベル100の姿勢角を含む。姿勢角は、ロール角θ4、ピッチ角θ5及び方位角θdcである。
刃先位置算出部28Aは、旋回中心位置データXRと作業機2の傾斜角θ1、θ2、θ3と、ブーム6の長さL1、アーム7の長さL2及びバケット8の長さL3とに基づいて、バケット8の刃先8Tの現在位置を示すバケット刃先位置データSを生成し、目標掘削地形データ生成部28Bに出力する。バケット刃先位置データSは、作業機2の位置を示す情報である。本実施形態において、作業機2の位置は、刃先位置、すなわちバケット8の刃先8Tの3次元位置に限定されるものではなく、作業機2の特定の部分の位置であればよい。例えば、作業機2の位置は、バケット8の尻の部分の位置であってもよいし、法面バケットの底の部分の位置であってもよいし、作業機2のアタッチメントを取り付ける部分の位置であってもよい。
目標掘削地形データ生成部28Bは、目標施工情報格納部28Cに格納された目標施工情報Tと、刃先位置算出部28Aからのバケット刃先位置データSと、を取得する。目標掘削地形データ生成部28Bは、車体座標系において刃先8Tの現時点における刃先位置を通る垂線と目標施工面との交点を掘削対象位置として設定する。目標掘削地形データ生成部28Bは、目標施工情報Tとバケット刃先位置データSとに基づいて、目標掘削地形データUを生成し、後述する装置コントローラ39の処理部39Pが有する作業機制御部39Cに出力する。
装置コントローラ39の処理部39Pは、姿勢角演算部39Aと、位置情報演算部39Bと、作業機制御部39Cとを有する。姿勢角演算部39A、位置情報演算部39B及び作業機制御部39Cは、記憶部39Mに記憶されたコンピュータプログラムを処理部39Pが実行することにより実現される。本実施形態において、作業機制御部39Cは、装置コントローラ39とは別個の制御装置であってもよい。
姿勢角演算部39Aには、IMU24の検出値である加速度a(ax、ay、az)及び角速度ω(ωx、ωy、ωz)、すなわち動作情報MIと、グローバル座標演算装置23の検出値である旋回体方位データQ(方位角θda)とが入力される。また、処理部39Pの姿勢角演算部39A及び位置情報演算部39Bには、圧力センサ66、27PCの検出値STr、STdが入力される。
グローバル座標演算装置23は、電波の受信状態又は装置コントローラ39との通信の状態を示す情報である状態情報SRを生成して、装置コントローラ39の処理部39P及び表示コントローラ28の処理部28Pに出力する。状態情報SRは、グローバル座標演算装置23が電波を受信できなくなった場合、電波の受信状態が低下した場合又はグローバル座標演算装置23と装置コントローラ39との通信に不良が発生した場合等に、それぞれの場合の受信状態又は通信状態を示す情報を有する。受信状態又は通信状態を示す情報は、グローバル座標演算装置23による測位の状態を示す。測位の状態には、前述したように、測位の精度がよい状態(Fix)、測位不可能の状態(非測位)、測位はできるが情報が少ない状態及び測位の精度が悪い状態(Float、単独測位)等が挙げられる。このように、グローバル座標演算装置23は、RTK−GNSSによる測位に不具合が発生したか否かを判定する測位状態判断装置である。
本実施形態において、測位状態判断装置、すなわちグローバル座標演算装置23は、測位の精度が良い状態(Fix)を測位の状態が正常であると判定する。また、グローバル座標演算装置23は、測位不可能の状態(非測位)、測位はできるが情報が少ない状態及び測位の精度が悪い状態(Float、単独測位)を、測位の状態が異常であると判定する。すなわち、グローバル座標演算装置23は、測位の状態がFixである場合を正常として判定し、Fix以外の状態である場合を異常として判定する。
表示コントローラ28は、状態情報SRを取得すると、測位の状態に応じた情報を、図2に示される表示部29に表示する。状態情報SRが測位不可能の状態を示す場合、表示コントローラ28は、図2に示される表示部29に、RTK−GNSSによる測位に異常が発生したことを表示する。
姿勢角演算部39Aは、IMU24の検出値から、油圧ショベル100のロール角である傾斜角θ4と、油圧ショベル100のピッチ角である傾斜角θ5とを求めて、位置情報演算部39B及び表示コントローラ28の刃先位置算出部28Aに出力する。姿勢角演算部39Aは、IMU24が検出したz軸周りの角速度ωを積分して方位角θdiを求めることができる。ロール角θ4、ピッチ角θ5及び方位角θdiが姿勢角である。
姿勢角演算部39Aは、位置検出装置であるグローバル座標演算装置23の状態に応じて、自身が求めた方位角θdi又はグローバル座標演算装置23から取得した方位角θdaを切り替えて、表示コントローラ28の刃先位置算出部28A又は位置情報演算部39Bに方位角θdcとして出力する。つまり、RTK−GNSSによる測位が正常である場合には、グローバル座標演算装置23から取得した方位角θdaを用いてバケット刃先位置データSが求められ、RTK−GNSSによる測位が異常である場合には、IMU24が検出したz軸周りの角速度ωを積分して求めた方位角θdiを用いてバケット刃先位置データSが求められる。また、姿勢角演算部39Aから表示コントローラ28に送られる傾斜角θ4、傾斜角θ5及び方位角θdcは、油圧ショベル100の位置に関連する位置情報IPLである。以下において、傾斜角θ4を適宜ロール角θ4と称し、傾斜角θ5を適宜ピッチ角θ5と称する。
本実施形態において、位置情報IPLは、前述したように、作業機械である油圧ショベル100の位置に関連する情報である。位置情報IPLには、油圧ショベル100の位置そのものの情報の他、油圧ショベル100の位置を求めるために必要な情報も含む。油圧ショベル100の位置そのものの情報は、基準位置データP1、P2及びバケット刃先位置データSが例示され、油圧ショベル100の位置を求めるために必要な情報は、傾斜角θ4、傾斜角θ5及び方位角(θda、θdi又はθdc)が例示される。
位置情報演算部39Bは、グローバル座標演算装置23から取得した基準位置データP1、P2と、IMU24から取得した動作情報MIとを用いて、基準位置データP1、P2が示す位置に相当する位置を求める。基準位置データP1、P2は、第1位置の情報である。以下において、位置情報演算部39Bが基準位置データP1、P2及び動作情報MIから求めた位置を、適宜第2位置と称する。第2位置の情報は、基準位置データP1i、P2iである。基準位置データP1i、P2iは、位置情報演算部39Bが生成する。以下において、グローバル座標演算装置23が出力した基準位置データP1、P2を適宜、第1基準位置データP1、P2と称し、位置情報演算部39Bによって生成された基準位置データP1i、P2iを適宜、第2基準位置データP1i、P2iと称する。
第2基準位置データP1i、P2iは、IMU24の検出値である加速度a(ax、ay、az)及び角速度ω(ωx、ωy、ωz)から姿勢角演算部39Aが求めたロール角θ4及びピッチ角θ5と、姿勢角演算部39Aから出力される方位角θdcとによって求められる。この方位角θdcは、姿勢角演算部39Aがグローバル座標演算装置23から取得した方位角θda又は方位角θdaに旋回時の角速度ωの積分によって得られた角度を加算した方位角である。この場合、グローバル座標演算装置23は、第1基準位置データP1、P2から方位角θdaを求めて、姿勢角演算部39Aに出力する。旋回時の角速度ωが0である場合、姿勢角演算部39Aから出力される方位角θdcは、グローバル座標演算装置23から取得した方位角θdaと等しい。このように、位置情報演算部39Bは、第1基準位置データP1、P2及び動作情報MIを用いて第2基準位置データP1i、P2iを生成する。本実施形態において、姿勢角演算部39Aは、グローバル座標演算装置23から第1基準位置データP1、P2を取得し、これを用いて方位角θdaを求めてもよい。
測位衛星の配置、電離層、対流圏又はGNSSアンテナ周辺の地形の影響により、グローバル座標演算装置23による測位結果がばらつくことがある。測位結果がばらつくと、掘削制御においては施工面が波打って設計面の通りに施工されない可能性がある。また、測位結果がばらつくと、ガイダンス画面に表示されるバケット8の刃先が揺らぎ、オペレータの視認性が低下する可能性がある。図2に示される操作装置25がパイロット圧方式である場合、左操作レバー25L又は右操作レバー25Rに油激が発生して、オペレータが違和感を覚える可能性もある。
グローバル座標演算装置23から出力される第1基準位置データP1、P2に、ローパスフィルタ処理又は移動平均のような平滑化処理を施すことにより、グローバル座標演算装置23の測位結果のばらつきによる影響を低減することが考えられる。油圧ショベル100は、掘削時における姿勢角の変動によってGNSSアンテナ21、22の位置も変動する。このため、第1基準位置データP1、P2が直接平滑化処理されると、平滑化処理後におけるGNSSアンテナ21、22の位置の算出において平滑化処理をすることによる時間遅れが発生する。その結果、平滑化処理後におけるGNSSアンテナ21、22の位置が、現実のGNSSアンテナ21、22の位置とは異なる可能性がある。
油圧ショベル100が作業のために稼働している時間において絶対位置が変化しない不動点が油圧ショベル100に存在し、GNSSアンテナ21、22の位置と不動点との相対関係が判れば、制御システム200の装置コントローラ39は不動点の絶対位置をGNSSアンテナ21、22の位置から算出できる。装置コントローラ39は、不動点の絶対位置に平滑化処理を適用し、ばらつきが低減された不動点を得る。装置コントローラ39は、平滑化処理を適用した後の不動点の絶対位置からGNSSアンテナ21、22の位置を計算すれば、平滑化処理の時間遅れが影響することなくRTK−GNSSによる測位のばらつきを低減できる。
実際には油圧ショベル100のエンジン35が稼働している限り、作業機2の動作等により振動が発生するため、不動点と見なせる近似的な位置が疑似不動点として選定される。制御システム200の装置コントローラ39は、選定された疑似不動点を、前述した不動点と同様に取り扱うことで、疑似不動点を用いてRTK−GNSSによる測位をバックアップすることができる。疑似不動点を不動点と見なすことができるのは、油圧ショベル100が移動していない場合、すなわち図1に示される履帯5a、5bが動いていない場合である。
本実施形態において、制御システム200、より具体的には制御システム200の装置コントローラ39は、前述した疑似不動点に平滑化処理を施し、平滑化処理を施した疑似不動点を用いて、GNSSアンテナ21、22の位置、すなわち第2位置を求める。疑似不動点は、後述するように、油圧ショベル100が作業のために稼働している時間において、時間の経過によって絶対位置が変化しないと見なすことができるので、平滑化処理による遅れの影響は無視できる。その結果、装置コントローラ39は、平滑化処理後におけるGNSSアンテナ21、22の位置と、現実のGNSSアンテナ21、22の位置とを一致させることができる。このように、装置コントローラ39は、疑似不動点に平滑化処理を施すことにより、グローバル座標演算装置23の測位結果のばらつきによる影響を低減することができる。その結果、装置コントローラ39は、掘削制御を用いた施工時の精度低下及びガイダンス画面の視認性の低下を抑制できる。
位置情報演算部39Bは、図2に示される左操作レバー25Lのパイロット圧を検出する圧力センサ66の検出値STr(前述した旋回操作信号MRに対応)及び走行用レバー25FL及び走行用レバー25FRのパイロット圧を検出する圧力センサ27PCの検出値STd(前述した操作信号MDに対応)を取得する。位置情報演算部39Bは、取得した検出値STr、Stdに基づいて、平滑化処理を実行するか否かを始めとした各種の判定を行う。
次に、疑似不動点について説明する。本実施形態において、疑似不動点は、図3及び図4に示される油圧ショベル100の位置PLである。なお、前述のように車体座標系の原点として位置PLを定めているが、他の位置に車体座標系の原点を定めてもよい。したがって、疑似不動点は、以下の説明において特定点と称することもある。作業中の油圧ショベル100に回転(以下、旋回と称することもある)が生ずるような動きがあった場合、その回転の支点は動かないので、疑似不動点がその支点にあれば、制御システム200によって求められる位置、例えば、作業機2の特定の部分の位置又はバケット8の刃先8Tの位置を含む作業機2の位置の誤差が最も小さくなる。疑似不動点を回転の支点とすることができない場合でも、疑似不動点をできるだけ支点の近くに設定すれば、制御システム200によって求められる位置(作業機2の位置)の誤差を小さくできる。
上部旋回体3が旋回する際の支点は旋回中心軸、すなわちz軸であるので、疑似不動点をz軸上にする。ロール角θ4の方向及びピッチ角θ5の方向における回転の支点は一定点ではないが、必ず油圧ショベル100が接地する面上にあると考えられる。本実施形態において、前述したように、位置PLは、旋回体の回転中心軸であるz軸と走行装置5が接地する面に相当する面との交点である。本実施形態では、疑似不動点を、油圧ショベル100が接地する面上とすることで、作業中の油圧ショベル100に回転が生ずるような動きがあった場合でも、疑似不動点は不動であると考えられる。このため、RTK−GNSSによる測位のばらつきが発生した際において、制御システム200によって求められる位置、より具体的にはGNSSアンテナ21、22の絶対位置のばらつきを低減できる。
油圧ショベル100は様々な作業を行うことができるが、前述のように油圧ショベル100に回転が生じても疑似不動点は不動であると考えられる場合として、例えば、法面の施工等の作業がある。この場合、走行装置5は停止したまま、作業機2又は上部旋回体3の操作だけで掘削又は均しが行われることがある。情報化施工を可能とする油圧ショベル100を用いて、このような法面施工等の施工を行う場合、制御システム200は、疑似不動点及びRTK−GNSSによる測位結果を用いて油圧ショベル100の第2位置、具体的にはGNSSアンテナ21、22の位置を求める。このようにすることで、制御システム200は、掘削制御の精度低下及びガイダンス画面の視認性の低下を抑制できる。
<疑似不動点の求め方>
油圧ショベル100の制御システム200、具体的には装置コントローラ39が、GNSSアンテナ21、22の絶対位置から疑似不動点を求める方法、及び疑似不動点からGNSSアンテナ21、22の絶対位置を求め直す方法を説明する。
式(1)は、車体座標系における位置PLの位置ベクトルとGNSSアンテナ21、22の位置ベクトルとの差分を、図3に示される現場座標系(X、Y、Z)における位置PLの位置ベクトルとGNSSアンテナ21、22の位置ベクトルとの差分に変換する式である。式(2)は、現場座標系におけるGNSSアンテナ21、22の位置ベクトルの測定値Ralから現場座標系における位置PLの位置ベクトルRflを計算する式であり、式(1)の変形である。式(3)は、現場座標系における位置PLの位置ベクトルRflから現場座標系におけるGNSSアンテナ21、22の位置ベクトルの測定値Ralを、現場座標系におけるGNSSアンテナ21、22の位置ベクトルの計算値Ralcを求める式として表したものである。
Rfl−Ral=Clb(Rfb−Rab)・・・(1)
Rfl=Clb(Rfb−Rab)+Ral・・・(2)
Ralc=Clb(Rab−Rfb)+Rfl・・・(3)
ここで、
Rfb:車体座標系における位置PLの位置ベクトルの校正値
Rab:車体座標系におけるGNSSアンテナ21、22の位置ベクトルの校正値
Rfl:現場座標系における位置PLの位置ベクトルの計算値
Ral:現場座標系におけるGNSSアンテナ21、22の位置ベクトルの測定値
Ralc:現場座標系におけるGNSSアンテナ21、22の位置ベクトルの計算値
Clb:車体座標系から現場座標系への座標回転行列
校正値とは、油圧ショベル100の各位置及び寸法を計測することによって得られた、位置PL及びGNSSアンテナ21、22の位置の値であり、装置コントローラ39の記憶部39M及び表示コントローラ28の記憶部28Mの少なくとも一方に記憶されている。校正値は、油圧ショベル100の設計寸法に基づくものでもよいが、設計寸法は、油圧ショベル100毎にばらつきが生じる可能性がある。このため、校正値は、計測(キャリブレーション)に基づいて得られたものが好ましい。
座標回転行列Clbは、ロール角θ4、ピッチ角θ5及びヨー角、すなわち方位角θdを用いて式(4)のように表現される。ロール角θ4、ピッチ角θ5及び方位角θdは、IMU24によって検出されたx軸周りの角速度ωx、y軸周りの角速度ωy及びz軸周りの角速度ωzを姿勢角演算部39Aが時間で積分されることによって求められる。式(4)中のsxはsinθ4、syはsinθ5、szはsinθd、cxはcosθ4、cyはcosθ5、czはcosθdである。
制御システム200は、式(2)を用いることにより、疑似不動点である特定点(本実施形態では位置PL)の位置を求めることができる。また、制御システム200は、式(3)を用いることにより、疑似不動点である特定点の位置を用いて、GNSSアンテナ21、22の絶対位置、すなわち現場座標系における位置又はグローバル座標系における位置を求めることができる。制御システム200は、式(2)及び式(3)を用いることによって、GNSSアンテナ21、22の絶対位置を求めることができる。
<平滑化処理>
本実施形態において、装置コントローラ39は、疑似不動点である特定点の位置に平滑化処理を施す。本実施形態において、平滑化処理には、例えば、式(5)で示されるローパスフィルタが用いられる。
Rft={(M−1)×Rftpr+Rfl}/M・・・(5)
式(5)中のRftは、今回の制御周期におけるローパスフィルタの出力であり、Rftprは前回の制御周期におけるローパスフィルタ(以下、適宜フィルタと称する)の出力である。これらは、いずれも特定点の位置ベクトルである。Mは、平均化定数である。本実施の形態において、平均化定数Mの初期値は1であり、Mの値が設定値Mmaxに到達するまで、1回の制御周期が終了する毎に、Mは1ずつ増加する。
本実施形態において、装置コントローラ39は、平滑化処理を開始すると、前回の制御周期におけるフィルタの出力Rftprを記憶部39Mに一時的に記憶させる。記憶部39Mは、次の制御周期のフィルタ処置が実行されるまで又は実行中の平滑化処理がリセットされるまで、前回の制御周期におけるフィルタの出力Rftprを記憶する。
装置コントローラ39は、式(2)を用いて、特定点の位置を示す位置ベクトルRflを求め、得られた位置ベクトルRflを式(5)に与える。装置コントローラ39は、式(5)により、制御の1周期毎に、特定点の位置ベクトルRflに平滑化処理、具体的にはローパスフィルタ処理を施す。装置コントローラ39は、ローパスフィルタ処理の後、平滑化処理後における特定点の位置ベクトルとして、今回の制御周期におけるローパスフィルタの出力Rftを出力する。平滑化処理後における特定点の位置ベクトルを、以下においては適宜位置ベクトルRftと称する。位置ベクトルRftは、第2基準位置データP1i、P2iである。第2基準位置データP1i、P2iは、平滑化処理によって得られた情報である。このように、装置コントローラ39は、式(5)で示されるローパスフィルタの機能を実現することにより、特定点の位置に平滑化処理を施し、平滑化処理後の特定点の位置を用いて、第2位置を求める。
装置コントローラ39は、初回の平滑化処理又は平滑化処理をリセットした後において、前回の制御周期におけるフィルタの出力Rftprを式(5)のRflとし、平均化定数Mを1とする。初回の平滑化処理とは、装置コントローラ39が平滑化処理を開始する場合に、前回の制御周期におけるフィルタの出力Rftprを有していない場合である。
装置コントローラ39は、何らかの理由で平滑化処理を中断した場合、今回の制御周期におけるフィルタの出力Rftを出力せず、前回の制御周期におけるフィルタの出力Rftpr及び前回の制御周期における平均化定数Mを保持する。この場合、装置コントローラ39は、記憶部39Mに、前回の制御周期におけるフィルタの出力Rftprに加えて、前回の制御周期における平均化定数Mを一時的に記憶させる。装置コントローラ39は、中断していた平滑化処理を再開する場合、現在の制御周期における特定点の位置ベクトルRflと、記憶部39Mに記憶していた、中断前におけるフィルタの出力Rftpr及び平均化定数Mとを式(5)に与える。この処理により、装置コントローラ39は、特定点の位置ベクトルRflを平滑化する。
装置コントローラ39は、グローバル座標演算装置23の測位結果の異常値を回避するため、平滑化処理を開始した場合(初回の平滑化処理の開始を除く)又は平滑化処理の中断から復帰する場合に、判定処理を実行する。判定処理を実行するにあたり、装置コントローラ39は、式(6)を用いて差分ΔRflを求める。式(6)中、Rflは、今回の制御周期における特定点の位置ベクトルであり、Rftprは、平滑化処理の開始前又は平滑化処理の中断から復帰、すなわち再開する時点において記憶部39Mに記憶されているフィルタの出力である。
ΔRfl=|Rfl−Rftpr|・・・(6)
判定処理において、装置コントローラ39は、差分ΔRflが予め定められた閾値未満であれば、今回の制御周期における特定点の位置ベクトルRflは正常であると判定して、今回の制御周期における特定点の位置ベクトルRflを用いて平滑化処理を実行する。装置コントローラ39は、差分ΔRflが予め定められた閾値以上であれば、今回の制御周期における特定点の位置ベクトルRflは異常であると判定する。このように、異常と判定された場合、装置コントローラ39は、今回の制御周期における特定点の位置ベクトルRflの代わりに、記憶部39Mに記憶されているフィルタの出力Rftprを用いて、式(5)のフィルタの出力Rftを求める。このような処理により、装置コントローラ39は、グローバル座標演算装置23の測位結果に異常値が発生した場合には、異常値に起因したバケット8の刃先位置の変動を抑制できる。差分ΔRflが予め定められた閾値以上となる状態が、予め定められた設定値Nt秒継続した場合、装置コントローラ39は、タイムアウト処理を実行する。具体的には、装置コントローラ39は、平滑化処理をリセットする。
グローバル座標演算装置23の測位結果に異常値が発生する場合には、グローバル座標演算装置23が出力する第1基準位置データP1、P2の座標値が異常値を示す場合、グローバル座標演算装置23と装置コントローラ39との間で通信エラーが発生した場合及びRTK−GNSSによる測位に不具合が発生した場合のいずれか一つが発生した場合、又は複数が同時に発生した場合が含まれる。GNSSアンテナ21、22が測位衛星からの電波を受信できなくなる又は電波を受信し難くなることにより、RTK−GNSSによる測位に不具合が発生する。
本実施形態において、ローパスフィルタは、式(5)に示されるものに限定されない。平滑化処理は、ローパスフィルタには限定されず、例えば、特定点の位置を移動平均する処理であってもよい。
本実施形態において、装置コントローラ39は、RTK−GNSSによる測位が正常である場合に平滑化処理を実行する。装置コントローラ39が特定点に平滑化処理を施すにあたり、装置コントローラ39が有する姿勢角演算部39Aは、ロール角θ4、ピッチ角θ5及び方位角θdcを求め、位置情報演算部39B及び表示コントローラ28の刃先位置算出部28Aに出力する。グローバル座標演算装置23が受信した基準位置データP1、P2によって方位角θdc、すなわち旋回体方位データQが得られるため、車体座標系について求められた作業機2の位置を、現場座標系における作業機2の位置として求めることができる。
図6は、油圧ショベルの姿勢を示す平面図である。姿勢角演算部39Aによって求められた方位角θdcは、現場座標系(X、Y、Z)のY軸に対する上部旋回体3の前後軸であるx軸の傾きを表す。方位角θdcによって油圧ショベル100の方位D1が定まる。
位置情報演算部39Bは、姿勢角演算部39Aによって求められたロール角θ4、ピッチ角θ5及び方位角θdcから座標回転行列Clbを求める。この場合、位置情報演算部39Bは、姿勢角演算部39Aによって求められた方位角θdcを式(4)のθdに与えて、座標回転行列Clbを求める。また、位置情報演算部39Bは、RTK−GNSSによる測位が正常である状態でグローバル座標演算装置23から取得した基準位置データP1、P2から現場座標系におけるGNSSアンテナ21、22の位置ベクトルの測定値Ralを求める。そして、位置情報演算部39Bは、求めた座標回転行列Clbと位置ベクトルの測定値Ralとを式(2)に与えて、現場座標系における位置PLの位置ベクトルRflを求める。位置ベクトルRflは計算値である。
位置ベクトルRflが得られたら、位置情報演算部39Bは、式(5)に位置ベクトルRflを与えることにより、位置ベクトルRflに平滑化処理を施す。位置情報演算部39Bは、平滑化処理後における位置ベクトルRfl、すなわちローパスフィルタの出力Rftを式(3)のRflとして与えて、現場座標系におけるGNSSアンテナ21、22の位置ベクトル、すなわち第2基準位置データP1i、P2iを求める。現場座標系におけるGNSSアンテナ21、22の位置ベクトルは、式(3)に示される計算値Ralcである。位置情報演算部39Bは、第2基準位置データP1i、P2iを、基準位置データP1c、P2cとして表示コントローラ28の刃先位置算出部28Aに出力する。
次に、装置コントローラ39の処理部39Pが有する作業機制御部39Cについて説明する。作業機制御部39Cは、表示コントローラ28から取得した目標掘削地形データUに基づいて、作業機2が目標掘削地形データUに近づく速度を制御するための制御信号Nを生成する。作業機制御部39Cは、生成した制御信号Nを制御弁27に与え、制御弁27を開閉することにより、作業機2が目標掘削地形データUに近づく速度を制御する。
図7は、実施形態1に係る装置コントローラ39が有する位置情報演算部39Bを示す図である。位置情報演算部39Bは、判定部40Aと、特定点演算部40Bと、平滑化処理部40Cと、位置計算部40Dとを含む。判定部40Aは、装置コントローラ39が平滑化処理を実行するか停止させるか、実行中の平滑化を中断するか否か、中断中の平滑化処理を再開させるか否か、及び平滑化処理をリセットするかについて判定する。これらの判定は、圧力センサ66の検出値STr及び圧力センサ27PCの検出値STdに基づいて行われる。
特定点演算部40Bは、式(2)を用いて特定点の位置ベクトルRflを求める。平滑化処理部40Cは、式(5)を用いて、特定点演算部40Bによって求められた特定点の位置ベクトルRflに平滑化処理を施す。位置計算部40Dは、平滑化処理後における位置ベクトルRftを式(3)のRflに与えて第2基準位置データP1i、P2iを求め、これらを基準位置データP1c、P2cとして表示コントローラ28に出力する。次に、本実施形態に係る制御システム200が平滑化処理を実行して、バケット8の刃先位置を求める処理の一例を説明する。
<制御システム200の処理の一例>
図8は、実施形態1に係る制御システム200の処理の一例を示すフローチャートである。ステップS101において、制御システム200の装置コントローラ39が有する位置情報演算部39Bの判定部40Aは、装置コントローラ39が平滑化処理を実行するために必要な実行条件が成立したか否かを判定する。実行条件は、RTK−GNSSによる測位が正常、かつ油圧ショベル100が非走行、かつ上部旋回体3が非旋回である場合に成立する。
開始条件が成立した場合(ステップS101、Yes)、装置コントローラ39は、ステップS102において特定点を求める。具体的には、装置コントローラ39が有する位置情報演算部39Bの特定点演算部40Bが、特定点、具体的には特定点の位置ベクトルRflを求める。ステップS103において、装置コントローラ39は、特定点演算部40Bによって求められた特定点の位置ベクトルRflに平滑化処理を施す。ステップS104において、装置コントローラ39が有する位置情報演算部39Bの位置計算部40Dは、平滑化処理後における位置ベクトルRflである位置ベクトルRftを用いて第2基準位置データP1i、P2iを求める。そして、位置計算部40Dは、求めた第2基準位置データP1i、P2iを基準位置データP1c、P2cとして表示コントローラ28に出力する。
ステップS105において、表示コントローラ28の処理部28Pは、装置コントローラ39から取得した油圧ショベル100の位置情報IPLを用いて、バケット8の刃先8Tの3次元位置である刃先位置を求める。具体的には、処理部28Pは、刃先位置を示すバケット刃先位置データSを生成する。位置情報IPLは、基準位置データP1c、P2c、ロール角θ4、ピッチ角θ5及び方位角θdcである。次に、ステップS101に戻って説明する。ステップS101において、開始条件が成立しなかった場合(ステップS101、No)、装置コントローラ39は処理を終了する。つまり、判定部40Aは、RTK−GNSSによる測位が正常であること、油圧ショベル100が非走行であること、及び上部旋回体3が非旋回であることのうち、いずれか一つが成立していない場合、実行条件が成立していないと判定する。次に、平滑化処理の状態の遷移について説明する。
<平滑化処理の状態の遷移>
図9は、平滑化処理の状態の遷移を説明するための図である。本実施形態において、平滑化処理は、状態1(ON、平滑化処理の実行)、状態2(OFF、平滑化処理の停止)、状態3(中断、実行中の平滑化処理の中断)及び状態4(リセット、平滑化処理のリセット)の4つの状態の間を遷移する。
装置コントローラ39は、RTK−GNSSによる測位が正常、かつ油圧ショベル100の走行が停止(非走行)、かつ上部旋回体3が非旋回である場合に、平滑化処理の状態を状態1に遷移させる。すなわち、状態1は、前述した実行条件が成立した場合における平滑化処理の状態である。状態1において、装置コントローラ39は、第2位置の情報である第2基準位置データP1i、P2iを用いて刃先位置を求める。
装置コントローラ39は、実行条件が成立しなかった場合、より具体的には油圧ショベルが走行している場合に、平滑化処理を状態1から状態2に遷移させる(I)。すなわち、装置コントローラ39は、油圧ショベル100が走行しているときは、第2位置を求める処理、すなわち平滑化処理を停止する。装置コントローラ39は、状態2においてRTK−GNSSによる測位が正常、かつ油圧ショベル100が非走行、かつ上部旋回体3が非旋回である場合に、平滑化処理を状態2から状態1に遷移させる(I)。
装置コントローラ39は、状態1において、実行条件が成立しなくなった場合、より具体的にはRTK−GNSSによる測位が異常になった場合及び上部旋回体3が旋回している場合の少なくとも一方が成立した場合に、平滑化処理を状態1から状態3に遷移させる(II)。状態3において、装置コントローラ39は、第2位置の情報である第2基準位置データP1i、P2iを求める処理、すなわち平滑化処理を中断する。装置コントローラ39は、状態3においてRTK−GNSSによる測位が正常、かつ油圧ショベル100が非走行、かつ上部旋回体3が停止、すなわち非旋回である場合に、平滑化処理を状態3から状態1に遷移させる(II)。この場合、装置コントローラ39は、中断していた平滑化処理を再開する。装置コントローラ39は、中断していた平滑化処理を再開する場合、平滑化処理を中断する前に得られた第2基準位置データP1i、P2iを用いて刃先位置を求める。
装置コントローラ39は、グローバル座標演算装置23の測位結果の異常値を回避する処理又はタイムアウト処理を実行する場合に、平滑化処理を状態1から状態4に遷移させる(III)。装置コントローラ39は、状態4においてRTK−GNSSによる測位が正常、かつ油圧ショベル100が非走行、かつ上部旋回体3が非旋回、かつ平滑化処理のリセットが完了した場合に、平滑化処理を状態4から状態1に遷移させる(III)。
装置コントローラ39は、平滑化処理の中断中に油圧ショベル100が走行を開始した場合に、平滑化処理を状態3から状態2に遷移させる(IV)。すなわち、装置コントローラ39は、第2位置の情報を求める処理である平滑化処理を停止する。装置コントローラ39は、状態4において油圧ショベル100が走行、又は上部旋回体3が旋回、又はRTK−GNSSによる測位が異常である場合に、平滑化処理を状態4から状態2に遷移させる(V)。
<油圧ショベル100の状態及びRTK−GNSSによる測位の状態の判定>
位置情報演算部39Bが平滑化処理の状態を遷移させるにあたって、図7に示される位置情報演算部39Bの判定部40Aは、油圧ショベル100の状態及びRTK−GNSSによる測位の状態を判定する。判定部40Aは、圧力センサ27PCが、走行用レバー25FL及び走行用レバー25FRの少なくとも一方のパイロット圧を検出した場合、油圧ショベル100は走行していると判定する。上部旋回体3を旋回操作するための操作レバーである左操作レバー25Lが左右いずれかの方向に操作され、圧力センサ66がパイロット圧を検出した場合、判定部40Aは、上部旋回体3は旋回していると判定する。判定部40Aは、グローバル座標演算装置23が生成した状態情報SRが、RTK−GNSSによる測位の状態が異常であることを示す場合、測位の状態が異常であると判定する。
<平滑化処理の状態を遷移させる処理>
図10は、装置コントローラ39が平滑化処理の状態を遷移させる処理のフローチャートであって、特に平滑化処理の中断に関する処理を示すものである。ステップS201において、装置コントローラ39が平滑化処理を実行している場合、装置コントローラ39が有する位置情報演算部39Bの判定部40Aは、平滑化処理を中断する条件が成立したか否かを判定する。平滑化処理を中断する条件は、RTK−GNSSによる測位が異常になった場合及び上部旋回体3が旋回している場合の少なくとも一方が成立した場合である。平滑化処理を中断する条件が成立したと判定部40Aが判定した場合(ステップS201、Yes)、ステップS202において、装置コントローラ39の位置情報演算部39Bは平滑化処理を中断する(II)。
ステップS203において、判定部40Aは、油圧ショベル100が走行しているか否かを判定する。油圧ショベル100が走行していると判定部40Aが判定した場合(ステップS203、Yes)、ステップS204において、位置情報演算部39Bは、中断していた平滑化処理を停止させる(IV)。次に、ステップS201に戻って説明する。平滑化処理を中断する条件が成立しないと判定部40Aが判定した場合(ステップS201、No)、装置コントローラ39は処理を終了する。
次に、ステップS203に戻って説明する。油圧ショベル100が走行していないと判定部40Aが判定した場合(ステップS203、No)、ステップS205において、判定部40Aは、実行条件が成立したか否かを判定する。実行条件が成立したと判定部40Aが判定した場合(ステップS205、Yes)、ステップS206において、位置情報演算部39Bは、平滑化処理を中断したときの情報を用いて、平滑化処理を実行する(II)。平滑化処理を中断したときの情報は、記憶部39Mに記憶していた中断前におけるフィルタの出力Rftpr及び平均化定数Mである。実行条件が成立しないと判定部40Aが判定した場合(ステップS205、No)、位置情報演算部39Bは、ステップS202に戻ってステップS202以降の処理を実行する。
図11は、装置コントローラ39が平滑化処理の状態を遷移させる処理のフローチャートであって、特に平滑化処理のリセットに関する処理を示すものである。ステップS301において、装置コントローラ39が平滑化処理を実行している場合、判定部40Aは、平滑化処理をリセットする条件が成立したか否かを判定する。平滑化処理をリセットする条件は、グローバル座標演算装置23の測位結果の異常値を回避する処理として、異常値が発生した状態が、予め定められた時間(設定値Nt秒)継続した際に、タイムアウト処理が実行される場合である。平滑化処理をリセットする条件が成立したと判定部40Aが判定した場合(ステップS301、Yes)、ステップS302において、装置コントローラ39の位置情報演算部39Bは平滑化処理をリセットする(III)。
ステップS303において、判定部40Aは、平滑化処理の復帰条件が成立したか否かを判定する。平滑化処理の復帰条件は、状態4においてRTK−GNSSによる測位が正常、かつ油圧ショベル100が非走行、かつ上部旋回体3が非旋回、かつ平滑化処理のリセットが完了した場合である。平滑化処理の復帰条件が成立したと判定部40Aが判定した場合(ステップS303、Yes)、ステップS304において、位置情報演算部39Bは、平滑化処理を実行する(III)。
次に、ステップS301に戻って説明する。平滑化処理をリセットする条件が成立しないと判定部40Aが判定した場合(ステップS301、No)、位置情報演算部39Bは、ステップS305において実行中の平滑化処理を継続する。次に、ステップS303に戻って説明する。平滑化処理の復帰条件が成立していないと判定部40Aが判定した場合(ステップS303、No)、ステップS306において平滑化処理を停止する(V)。
本実施形態は、グローバル座標演算装置23からの第1位置の情報、すなわち第1基準位置データP1、P2及びIMU24からの動作情報MIを用いて、油圧ショベル100一部の位置に相当する第2位置を求め、得られた第2位置の情報を用いて、作業機2の少なくとも一部の位置を求める。本実施形態では、第1基準位置データP1、P2及びIMU24から得られる特定点、すなわち上部旋回体3の回転中心軸であるz軸と走行装置5が接地する面に対応する面との交点の情報を用いて、第2位置が求められる。特定点は、油圧ショベル100の作業中において、時間の経過によって絶対位置が変化しないと見なすことができるので、装置コントローラ39は、特定点の位置に平滑化処理を施し、平滑化処理後の特定点の位置を用いて第2位置を求めても、平滑化処理による遅れの影響は無視できる。その結果、本実施形態は、第2位置と、油圧ショベル100一部の位置とを一致させることができるので、作業機械の位置を測位した結果に基づいて情報化施工を行う作業機械において、測位結果のばらつきが情報化施工に与える影響を低減できる。一例としては、掘削制御を用いた施工時の精度低下及びガイダンス画面の視認性の低下が抑制される。
以上、実施形態1について説明したが、実施形態1の構成は、以下の実施形態においても適宜適用できる。
実施形態2.
図12は、実施形態2に係る制御システム200aの制御ブロック図である。図13は、実施形態2に係る装置コントローラ39aが有する位置情報演算部39Baを示す図である。制御システム200aは、実施形態1の制御システム200と同様であるが、IMU24の検出値である動作情報MIが装置コントローラ39aの処理部39Paが有する位置情報演算部39Baに入力される点、及び位置情報演算部39Baの構成が異なる。装置コントローラ39aは、実施形態1と同様に、CPU等のプロセッサ及びRAM及びROM等の記憶装置により実現される。装置コントローラ39aの処理部39Paの機能は、その機能を実現するためのコンピュータプログラムを記憶部39Mから処理部39Paが読み込んで実行することにより、実現される。
位置情報演算部39Baは、判定部40Aと、速度計算部40Eと、平滑化処理部40Baとを有する。判定部40Aは、実施形態1に係る装置コントローラ39の判定部40Aと同一なので説明を省略する。速度計算部40Eは、IMU24から取得した動作情報MIである角速度ω及びIMU24とGNSSアンテナ21、22との相対位置関係からGNSSアンテナ21、22に発生する速度vを求める。つまり、ある角速度ωが生じているということは、車両本体1に動きが生じており、IMU24と同じ車両本体1上に設置されているGNSSアンテナ21、22が動いているということになる。IMU24とGNSSアンテナ21、22との相対位置関係(例えば設計寸法)は既知である。このため、角速度ω及び相対位置関係から、GNSSアンテナ21、22の動き(移動距離)が求められるので、結果としてGNSSアンテナ21、22が所定時間に移動した距離、すなわち速度vが求められる。時間dtは、制御の1周期である。
<平滑化処理>
本実施形態において、装置コントローラ39a、より具体的には平滑化処理部40Baは、速度vを用いて第1位置、より具体的には第1位置の情報である基準位置データP1、P2に平滑化処理を施す。本実施形態において、平滑化処理には、式(7)で示されるローパスフィルタが用いられる。
Pi={P+(M−1)×(Pi−1+vdt)}/M・・・(7)
式(7)中のPは、今回の制御周期における第1位置の情報である第1基準位置データP1、P2である。Pi−1は、前回の制御周期におけるローパスフィルタの出力、すなわち前回の制御周期において平滑化処理を施された第1位置の情報である第1基準位置データP1、P2である。第1基準位置データP1、P2は、グローバル座標演算装置23が出力する。式(7)中のPiは、今回の制御周期におけるローパスフィルタの出力であり、第2位置の情報である第2基準位置データP1i、P2iである。式(7)中のvは、IMU24が検出した角速度ω及びIMU24とGNSSアンテナ21、22との相対位置関係から速度計算部40Eによって求められたGNSSアンテナ21、22の速度である。式(7)中のdtは、装置コントローラ39aによる制御の1周期である。vdtは、装置コントローラ39a制御の1周期で油圧ショベル100が移動した距離である。Mは、平均化定数である。平均化定数Mは、実施形態1と同様である。装置コントローラ39aの平滑化処理部40Baは、式(7)で示されるローパスフィルタの機能を実現することにより、動作情報MIを用いて第1位置に平滑化処理を施して第2位置を求める。
本実施形態において、装置コントローラ39aの位置情報演算部39Baが有する速度計算部40Eは、制御の1周期毎に速度vを求め、平滑化処理部40Baは、速度vを用いて制御の1周期毎に第1基準位置データP1、P2に平滑化処理を施す。速度vは、IMU24の検出値である角速度ω及びIMU24とGNSSアンテナ21、22との相対位置関係から求められる。装置コントローラ39aの位置情報演算部39Baは、グローバル座標演算装置23から出力された第1基準位置データP1、P2に、IMU24の検出値を用いて平滑化処理を施す。このように位置情報演算部39Baは、IMU24の検出値を用いて平滑化処理を施す。このため、位置情報演算部39Baは、掘削時における油圧ショベル100の姿勢変化がGNSSアンテナ21、22の位置の変化に与える影響を、IMU24の検出値によって反映させて、第2基準位置データP1i、P2iを求めることができる。その結果、装置コントローラ39aは、グローバル座標演算装置23の測位結果のばらつきによる影響を低減することができるので、掘削制御を用いた施工時の精度低下及びガイダンス画面の視認性の低下を抑制できる。
<制御システム200aの処理の一例>
図14は、実施形態2に係る制御システム200aの処理の一例を示すフローチャートである。ステップS401において、制御システム200aの装置コントローラ39aが有する位置情報演算部39Baの判定部40Aは、装置コントローラ39aが平滑化処理を実行するために必要な実行条件が成立したか否かを判定する。実行条件は、実施形態1で説明した通りである。
開始条件が成立した場合(ステップS401、Yes)、装置コントローラ39aは、ステップS402においてIMU24から角速度ωを取得し、グローバル座標演算装置23から第1基準位置データP1、P2を取得する。ステップS403において、装置コントローラ39aの位置情報演算部39Baが有する平滑化処理部40Baは、速度vを用いて第1基準位置データP1、P2に平滑化処理を施す。速度vは、速度計算部40Eによって、角速度ω及びIMU24とGNSSアンテナ21、22との相対位置関係から求められる。IMU24とGNSSアンテナ21、22との相対位置関係は、校正値として計測(キャリブレーション)に基づいて得られたものが好ましい。
ステップS404において、装置コントローラ39aが有する平滑化処理部40Baは、フィルタ出力、すなわち第2基準位置データP1i、P2iを基準位置データP1c、P2cとして表示コントローラ28に出力する。ステップS405において、表示コントローラ28の処理部28Pは、装置コントローラ39から取得した油圧ショベル100の位置情報IPLを用いて、バケット8の刃先8Tの3次元位置である刃先位置を求める。次に、ステップS401に戻って説明する。ステップS401において、開始条件が成立しなかった場合(ステップS401、No)、装置コントローラ39aは処理を終了する。本実施形態において、平滑化処理の状態を遷移させる処理については、実施形態1と同様である。
本実施形態は、第1基準位置データP1、P2及びIMU24からの動作情報MIを用いて第2位置を求め、得られた第2位置の情報を用いて、作業機2の少なくとも一部の位置を求める。実施形態1では、第1基準位置データP1、P2及びIMU24から得られる特定点に情報を用いて第2位置が求められたが、本実施形態は、IMU24によって検出された動作情報MI、より具体的には角速度等から速度を求め、得られた速度を用いて第1位置の情報である第1基準位置データP1、P2に平滑化処理を施して、第2位置を求める。本実施形態は、掘削時における油圧ショベル100の姿勢変化がGNSSアンテナ21、22の位置の変化に与える影響を、IMU24の検出値、具体的には角速度によって反映させて、第2位置を求めることができる。その結果、本実施形態は、グローバル座標演算装置23の測位結果のばらつきが情報化施工に与える影響を低減できる。
以上、実施形態2について説明したが、実施形態2の構成は、以下の実施形態においても適宜適用できる。
実施形態3.
図15は、実施形態3に係る制御システム200bの制御ブロック図である。図16は、実施形態3に係る装置コントローラ39bが有する位置・姿勢情報演算部39Bbを示す図である。実施形態3は、位置姿勢演算手法にカルマンフィルタを用いる。制御システム200bは、実施形態1の制御システム200と同様であるが、位置・姿勢情報演算部39Bbを有する点、及びIMU24の検出値である動作情報MIが装置コントローラ39bの処理部39Pbが有する位置・姿勢情報演算部39Bbに入力される点が異なる。装置コントローラ39bは、実施形態1と同様に、CPU等のプロセッサ及びRAM及びROM等の記憶装置により実現される。装置コントローラ39bの処理部39Pbの機能は、その機能を実現するためのコンピュータプログラムを記憶部39Mから処理部39Pbが読み込んで実行することにより、実現される。
位置・姿勢情報演算部39Bbは、位置推定部40Fと、誤差演算部40Bbと、選択部40Abと、特定点演算部40Bとを有する。位置推定部40Fは、IMU24によって検出された動作情報MIを用いて油圧ショベル100の位置、速度、方位角及び姿勢角といった位置姿勢情推定値を推定する。油圧ショベル100の位置は、GNSSアンテナ21、22の位置である。本実施形態において、位置推定部40Fは、油圧ショベル100の位置、速度、方位角及び姿勢角といった位置姿勢値を推定して位置姿勢推定値を求めるにあたり、慣性航法を用いる。位置推定部40Fは、推定により得られた油圧ショベル100の位置を第2位置、具体的には第2基準位置データP1i、P2iとして出力する。また、位置推定部40Fは、誤差演算部40Bbが出力した誤差を用いて第2位置を補正する。
誤差演算部40Bbは、第1基準位置データP1、P2、油圧ショベル100の速度V、方位角θda、特定点(本実施形態では位置PL)及び静止時の角速度ω=0のうち少なくとも1つを観測値として使用して、位置推定部40Fによって推定された油圧ショベル100の位置、速度、方位角及び姿勢角又はそれらが有する誤差を求め、位置推定部40Fに出力する。すなわち、誤差演算部40Bbは、位置姿勢推定値を補正するための情報を位置推定部40Fに送信する。位置推定部40Fは、位置姿勢推定値を補正するための情報を用いて、先に求められていた位置姿勢推定値の誤差を補正する。その後、位置推定部40Fは、補正された位置姿勢推定値から第2位置データを算出する。誤差演算部40Bbが使用する観測値のうち、第1基準位置データP1、P2、油圧ショベル100の速度V、方位角θdaは、グローバル座標演算装置23から得られる。誤差演算部40Bbは、グローバル座標演算装置23から得たグローバル座標系の第1基準位置データP1、P2及び速度Vを、現場座標系に変換する。特定点、本実施形態では位置PL及び特定点の位置ベクトルRflは、特定点演算部40Bが求める。本実施形態において、誤差演算部40Bbは、カルマンフィルタを含む。
選択部40Abは、油圧ショベル100の状態に応じて、誤差演算部40Bbが使用する観測値を選択する。油圧ショベル100の状態には、静定状態、非静定状態、上部旋回体3が旋回している状態、油圧ショベル100が走行している状態が含まれる。
図17は、実施形態3に係る装置コントローラ39bが有する位置・姿勢情報演算部39Bbの制御ブロック図である。位置推定部40Fは、IMU24によって測定された角速度を積分して車体の姿勢角の推定値及び方位角の推定を算出する。位置推定部40Fは、IMU24によって測定された加速度を積分して、油圧ショベル100の推定速度および推定位置を算出する。
選択部40Abは、挙動検出部42aと、判定器42bと、選択器42cとを含む。挙動検出部42aには、車体情報IFbと、IMU24の検出値である角速度ω及び加速度aとが入力される。本実施形態において、車体情報IFbは、図2に示される左操作レバー25L及び右操作レバー25Rのパイロット圧を検出する圧力センサ66の検出値STr及び走行用レバー25FL及び走行用レバー25FRのパイロット圧を検出する圧力センサ27PCの検出値STdである。挙動検出部42aは、角速度ω、加速度a及び車体情報IFbを用いて、油圧ショベル100の状態を検出し、検出結果に応じた信号を、判定器42bに出力する。
判定器42bには、挙動検出部42aからの信号、車体情報IFb及びグローバル座標演算装置23が出力した状態情報SRが入力される。判定器42bは、入力された情報に基づいて選択器42cを動作させて、誤差演算部40Bbに入力する観測値を選択する。選択器42cには、観測値、すなわちグローバル座標演算装置23が受信した第1基準位置データP1、P2、油圧ショベル100の速度Vc、方位角θda、特定点演算部40Bが求めた特定点の位置ベクトルRfl及び油圧ショベル100が旋回していないときの角速度ω=0が入力される。グローバル座標演算装置23は、測位衛星からの電波(信号)を用いて、第1基準位置データP1、P2を求めると同時に、油圧ショベル100の速度Vcを求める。方位角θdaは、グローバル座標演算装置23が第1基準位置データP1、P2から求める。
誤差演算部40Bbは、選択部40Abの選択器42cから、油圧ショベル100の状態に応じた観測値が入力される。誤差演算部40Bbは、カルマンフィルタを含む。誤差演算部40Bbは、観測ベクトルを取得し、状態方程式で事前に予測した状態ベクトルを補正し、事後の推定値を求める。この処理を繰り返すことでより確からしい推定値を求める。式(8)はカルマンフィルタの計算式である。Xk|k(Xは太字)は事後推定により得られた状態ベクトル、Xk|k−1(Xは太字)は事前推定により得られた状態ベクトル、K(Kは太字)はカルマンゲイン、zk(zは太字)は観測ベクトル、Hk(Hは太字)は観測行列である。誤差演算部40Bbは、式(8)を用いて事後推定により得られた状態ベクトルを求める。
カルマンゲインK(Kは太字)は、式(9)で求められる。Pk|k−1(Pは太字)は推定誤差の共分散、Rk(Rは太字)は観測誤差の共分散である。推定誤差の共分散Pk|k−1及び観測誤差の共分散Rkの設定によって、状態ベクトルXk|k(Xは太字)及び観測ベクトルzk(zは太字)の重みが決定される。
状態ベクトルについて説明する。予測値を通常字体で表し、補正値をイタリック体で表すと、エラー状態ベクトルは式(10)から式(14)で定義される。ここで、
δΨn nb(Ψは太字):航法座標系における油圧ショベル100の角度誤差ベクトル[rad]
δωb(ωbは太字):IMU24の角速度バイアス誤差ベクトル[rad/s]
δPl lb(Pは太字):現場座標系における現場座標系を基準とした車体座標原点の位置誤差ベクトル[m]
δVn eb(Vは太字):現場座標系におけるECEF(Earth Centered Earth Fixed:地球中心地球固定)座標系を基準とした車体座標原点の速度誤差ベクトル[m/s]
δAb(Abは太字):IMU24の加速度バイアス誤差ベクトル[m/s2]
Cb n(Cは太字):車体座標系から航法座標系への座標回転行列
Ψn nb(Ψは太字):航法座標系における油圧ショベル100の角度ベクトル[rad]
ωb(ωbは太字):IMU24の角速度ベクトル[rad/s]
Ab(Abは太字):IMU24の加速度ベクトル[m/s2]
Pl lb(Pは太字):現場座標系における現場座標系を基準とした車体座標原点の位置ベクトル[m]
Vn eb(Vは太字):現場座標系におけるECEF座標系を基準とした車体座標原点の速度ベクトル[m/s]
I:単位行列
状態方程式について説明する。式(11)から式(19)は、エラー状態モデルに基づく状態方程式である。雑音項は省略してある。ここで、
ωn ie(ωは太字):航法座標系における地球自転速度ベクトル[rad/s]
An ib(Aは太字):航法座標系における慣性座標系を基準とした車体座原点の加速度ベクトル[m/s2]
イタリック体で表示した観測値についての観測方程式を式(20)から式(24)に示す。雑音項は省略してある。式(20)はGNSSアンテナ21、22の位置の観測方程式であり、式(21)はGNSSアンテナ21、22の速度の観測方程式である。式(22)は、特定点の速度の観測方程式である。静止時及び旋回時に用いられる。式(23)は、静止時における油圧ショベル100の加速度の観測方程式である。式(23)は、油圧ショベル100の非旋回時におけるGPSコンパスによる方位角の観測方程式である。ここで、
Pl la(斜体):現場座標系における現場座標系を基準としたGNSSアンテナ21、22の位置[m]
Vn ea(斜体):航法座標系におけるECEF座標系を基準としたGNSSアンテナ21、22の速度[m/s]
Vn eq(斜体):航法座標系におけるECEF座標系を基準とした特定点の速度[m/s]
Ψz(斜体):GPSコンパスによる油圧ショベル100の方位角測定値[rad]
δCb nT:姿勢角誤差の回転行列(δCb nT=I−[δΨn nb^])
δΨz:油圧ショベル100の方位角の誤差(δΨn nbのZ成分)[rad]
Rb ba:車体座標系における車体座標系を基準としたGNSSアンテナ21、22の位置[m]
Rb bq:車体座標系における車体座標系を基準とした特定点の位置[m]
ωb nb:車体座標系における航法座標系を基準とした車体座標系の角速度ベクトル[rad/s]
Ψz:慣性航法演算による油圧ショベル100の方位角[rad]
前述した式(10)中の[δΨn nb^](Ψは太字)と、式(15)及び式(18)中の[ωn ie^](ωは太字)と、式(18)中の[An ib](Aは太字)とについて説明する。δΨn nb(Ψは太字)、ωn ie(ωは太字)及びAn ib(Aは太字)が、(α、β、γ)の3次元座標系におけるベクトル又はロール方向、ピッチ方向、ヨー方向のベクトルであるとする。ロール方向はα軸周りの方向、ピッチ方向はβ軸周りの方向、ヨー方向はγ軸周りの方向である。この場合、[δΨn nb^](Ψは太字)は式(25)で、[ωn ie^](ωは太字)は式(26)で、[An ib](Aは太字)は式(27)で表される。δΨα、δΨβ、δΨγは、この順に、α軸周り、β軸周り、γ軸周りにおける油圧ショベル100の角度誤差である。ωiα、ωiβ、ωiγは、この順に、α軸周り、β軸周り、γ軸周りにおける地球自転速度である。Aiα、Aiβ、Aiγは、この順に、α軸周り、β軸周り、γ軸周りにおける油圧ショベル100の車体座原点の加速度である。
誤差演算部40Bbは、事前推定において、式(15)から式(19)に示される状態方程式を解くことにより、式(10)から式(14)に示される状態ベクトルの事前推定値、すなわち状態ベクトルXk|k−1を求めることができる。本実施形態において、状態ベクトルは、航法座標系における油圧ショベル100の角度ベクトルΨn nb(Ψは太字)、IMU24の角速度ベクトルωb(ωbは太字)、IMU24の加速度ベクトルAb(Abは太字)、現場座標系における現場座標系を基準とした車体座標原点の位置ベクトルPl lb(Pは太字)及び現場座標系におけるECEF座標系を基準とした車体座標原点の速度ベクトルδVl eb(Vは太字)である。誤差演算部40Bbは、事前推定により状態ベクトルXk|k−1を求める場合、位置推定部40Fが求めた姿勢角(ロール角θ4、ピッチ角θ5、方位角θdc)、第2基準位置データP1i、P2i及び速度Vを取得し、位置、速度及び姿勢の予測値として用いる。
観測行列は、観測方程式のヤコビアンによって得られる。誤差演算部40Bbは、式(20)から式(24)を用いて観測ベクトルzk(zは太字)を求め、また式(9)からカルマンゲインK(Kは太字)を求める。そして、誤差演算部40Bbは、式(8)に、事前推定による状態ベクトルXk|k−1、観測ベクトルzk(zは太字)及びカルマンゲインK(Kは太字)を与えて解くことにより、事後の推定値である状態ベクトルXk|kを求めることができる。
角速度バイアス誤差FBaのベクトルは、IMU24の角速度バイアス誤差ベクトルδωb(ωbは太字)である。車体角度誤差FBbのベクトルは、航法座標系における油圧ショベル100の角度誤差ベクトルδΨn nb(Ψは太字)である。車体速度誤差FBcのベクトルは、現場座標系におけるECEF座標系を基準とした車体座標原点の速度誤差ベクトルδVl eb(Vは太字)である。車体位置誤差FBdのベクトルは、現場座標系における現場座標系を基準とした車体座標原点の位置誤差ベクトルδPl lb(Pは太字)である。加速度バイアス誤差FBeのベクトルは、IMU24の加速度バイアス誤差ベクトルδAb(Abは太字)である。
事後の推定により得られた状態ベクトルXk|kは、前述したように、角速度バイアス誤差FBa、車体角度誤差FBb、車体速度誤差FBc、車体位置誤差FBd及び加速度バイアス誤差FBeに対応する。誤差演算部40Bbは、事後の推定により求めた状態ベクトルXk|kを位置推定部40Fに与える。位置推定部40Fは、誤差演算部40Bbから取得した状態ベクトルXk|kを用いて、位置姿勢推定値を補正する。より具体的には、位置推定部40Fは、状態ベクトルXk|kを用いて、位置姿勢推定値に含まれる誤差を補正(位置姿勢推定値を補正)する。位置推定部40Fが状態ベクトルXk|kを用いて位置姿勢推定値(推定位置)を補正した位置が、油圧ショベル100の第2位置となる。このように、位置推定部40Fは、動作情報を用いて油圧ショベルの位置を推定し、得られた推定位置を状態ベクトルXk|kを用いて補正することにより、油圧ショベルの第2位置を求める。
図18は、誤差演算部40Bbが使用する観測方程式を選択する際に用いられる情報が記述されたテーブル44の一例を示す図である。テーブル44は、図15に示される装置コントローラ39bの記憶部39Mに記憶されている。本実施形態において、誤差演算部40Bbが状態ベクトルXk|kを推定する際に使用する観測値は、油圧ショベル100の状態に応じて選択される。このため、誤差演算部40Bbによって使用される観測値によって、誤差演算部40Bbによって使用される観測方程式が異なる。誤差演算部40Bbは、状態ベクトルXk|kを推定する際に、油圧ショベル100の状態に応じて選択された観測値に対応した観測方程式を、式(20)から式(24)の中から選択する。
油圧ショベル100の状態は、図18に示されるように、RTK−GNSSによる測位状態を示す状態A、状態B及び状態Cと、油圧ショベル100の動作状態、すなわち車体状態を示す車体静定1、車体静定2及び車体走行との組合せによって決定される。RTK−GNSSによる測位状態は、グローバル座標演算装置23による油圧ショベル100の位置の検出の状態である。本実施形態では、測位状態が3個、車体状態が3個なので、油圧ショベル100の状態は、合計9個となる。測位状態及び動作状態の内容の詳細を下記に示す。本実施形態において、測位状態及び動作状態の数及び内容は限定されない。
状態A:測位状態がFix
状態B:測位状態がFix、非測位以外の状態
状態C:測位状態が非測位
車体静定1:油圧ショベル100が停止し、かつ上部旋回体3も停止している
車体静定2:油圧ショベル100が停止し、かつ上部旋回体3が旋回している
車体走行:油圧ショベルが走行している
選択部40Abの判定器42bは、挙動検出部42aからの信号及び車体情報IFbから動作状態を判定し、グローバル座標演算装置23が出力した状態情報SRから測位状態を判定する。判定器42bは、記憶部39Mに記憶されているテーブル44に判定した動作条件及び測位条件に基づき、誤差演算部40Bbによって使用される観測方程式から、誤差演算部40Bbに入力する観測値を決定する。そして、決定された観測値が誤差演算部40Bbに入力されるように、選択器42cを動作させる。
判定器42bは、式(20)の観測方程式が使用される場合、グローバル座標演算装置23が受信したGNSSアンテナ21、22の位置に対応した第1基準位置データP1、P2を、誤差演算部40Bbに入力する観測値とする。判定器42bは、式(21)の観測方程式が使用される場合、グローバル座標演算装置23が受信したGNSSアンテナ21、22の位置に対応した第1基準位置データP1、P2及びグローバル座標演算装置23が求めたGNSSアンテナ21、22の速度Vaを現場座標系に変換し、誤差演算部40Bbに入力する観測値とする。判定器42bは、式(22)の観測方程式が使用される場合、図16に示される特定点演算部40Bが求めた特定点の位置ベクトルRflを、誤差演算部40Bbに入力する観測値とする。判定器42bは、式(23)の観測方程式が使用される場合、静止時の角速度、すなわち油圧ショベル100が静止しているときの角速度ω=0を、誤差演算部40Bbに入力する観測値とする。判定器42bは、式(24)の観測方程式が使用される場合、グローバル座標演算装置23が求めた、GPSコンパスによる油圧ショベル100の方位角θdaを、誤差演算部40Bbに入力する観測値とする。
誤差演算部40Bbは、選択器42cから入力された観測値を用いて、入力された観測値に対応する観測方程式を用いて観測ベクトルzk(zは太字)を求める。このように、誤差演算部40Bbは、油圧ショベル100の状態、すなわち測位状態及び車体状態に応じて観測ベクトルzk(zは太字)を求める際に用いる観測方程式を変更するので、油圧ショベル100の状態によっては不要な観測方程式を使用しないようにすることができる。その結果、誤差演算部40Bbは、演算の負荷を軽減することができる。
油圧ショベル100が停止し、かつ上部旋回体3が旋回しない場合、誤差演算部40Bbは、油圧ショベル100の特定点の速度が0、かつ角速度ω=0という観測値を用いて観測ベクトルzk(zは太字)を求めることができる。その結果、グローバル座標演算装置23の測位結果のばらつきによる影響を低減できる。
図19は、実施形態3に係る制御システム200bの処理の一例を示すフローチャートである。ステップS501において、装置コントローラ39bの位置・姿勢情報演算部39Bbは、次の時刻、本実施形態では次の制御周期における油圧ショベル100の状態ベクトルを推定し、また、観測値を取得する。
ステップS502において、位置・姿勢情報演算部39Bbの誤差演算部40Bbは、測位状態及び車体状態に応じて観測ベクトルzk(zは太字)を求める際に用いる観測方程式を選択する。ステップS503において、誤差演算部40Bbは、事後の推定値である状態ベクトルXk|kを求め、これに対応する角速度バイアス誤差FBa、車体角度誤差FBb、車体速度誤差FBc、車体位置誤差FBd及び加速度バイアス誤差FBeを、位置推定部40Fに与える。位置推定部40Fは、誤差演算部40Bbから取得した角速度バイアス誤差FBa、車体角度誤差FBb、車体速度誤差FBc、車体位置誤差FBd及び加速度バイアス誤差FBeを用いて、IMU24が検出した角速度ω及び加速度aと、位置推定部40Fが角速度ωから求めた角度と、位置推定部40Fが加速度aから求めた速度及び位置とを補正する。
ステップS504において、位置・姿勢情報演算部39Bbは、前述した補正によって得られた第2基準位置データP1i、P2iを、基準位置データP1c、P2cとして表示コントローラ28の刃先位置算出部28Aに出力する。ステップS505において、表示コントローラ28の処理部28Pは、装置コントローラ39から取得した油圧ショベル100の位置情報IPLを用いて、バケット8の刃先8Tの3次元位置である刃先位置を求める。
本実施形態は、第1位置の情報である第1基準位置データP1、P2及びIMU24からの動作情報MIを用いて第2位置を求め、得られた第2位置の情報を用いて、作業機2の少なくとも一部の位置を求める。実施形態1では、第1基準位置データP1、P2及びIMU24から得られる特定点の情報を用いて第2位置が求められたが、本実施形態においても、特定点(疑似不動点)が静止しているという条件を観測方程式に追加することによって、実施形態1と同様に第2位置を求めることができる。
本実施形態において、油圧ショベル100の位置は慣性航法により推定され、油圧ショベル100の位置姿勢誤差に含まれる誤差及びIMU24の誤差等はカルマンフィルタによって求められる。本実施形態は、慣性航法により次の時刻の油圧ショベル100の位置を推定し、第1位置の情報及び動作情報MIを用いてカルマンフィルタが求めた誤差により、推定された油圧ショベル100の位置を補正する。実施形態1及び実施形態2では、グローバル座標演算装置23によって得られた位置の情報を平滑化処理するが、本実施形態では、慣性航法により予め推定された位置をカルマンフィルタによって求められた誤差を用いて補正するか、又は慣性航法により予め推定された状態ベクトルをカルマンフィルタによって求められた状態ベクトル用いて補正する。このため、本実施形態は、平滑化処理の遅れの影響を排除できるので、作業機械の位置を測位した結果に基づいて情報化施工を行う作業機械において、測位結果のばらつきが情報化施工に与える影響をより確実に低減できる。
以上、実施形態1から実施形態3を説明したが、前述した内容により実施形態1から実施形態3が限定されるものではない。また、前述した構成要素には、当業者が容易に想定できるもの、実質的に同一のもの、いわゆる均等の範囲のものが含まれる。さらに、前述した構成要素は適宜組み合わせることが可能である。
さらに、実施形態1から実施形態3の要旨を逸脱しない範囲で構成要素の種々の省略、置換及び変更のうち少なくとも1つを行うことができる。例えば、装置コントローラ39が実行する各処理は、装置コントローラ39、表示コントローラ28、ポンプコントローラ又はこれら以外のコントローラが実行してもよい。作業機械は油圧ショベル100に限定されず、ホイールローダー又はブルドーザのような他の作業機械であってもよい。図5に示される姿勢角演算部39A及び位置・姿勢情報演算部39Bは、装置コントローラ39に備えられるが、いずれか一方又は両方が表示コントローラ28に備えられてもよいし、表示コントローラ28以外のコントローラに備えられてもよい。