JPWO2015177843A1 - 作業機械の姿勢演算装置、油圧ショベルの姿勢演算装置及び作業機械 - Google Patents

Abstract

作業機械の姿勢演算装置は、作業機械に備えられて、角速度及び加速度を検出する計測装置と、前記計測装置によって検出された角速度及び加速度から前記作業機械の姿勢角を求める姿勢角演算部と、第１の遮断周波数が設定され、前記姿勢角演算部によって求められた前記姿勢角に含まれる雑音を低減して第１姿勢角を出力する第１相補フィルタと、前記第１の遮断周波数とは異なる第２の遮断周波数が設定され、前記姿勢角演算部によって求められた前記姿勢角に含まれる雑音を低減して第２姿勢角を出力する第２相補フィルタと、前記計測装置の状態に応じて、前記第１姿勢角又は前記第２姿勢角を切り替えて出力する切替部と、を含む。

Description

本発明は、作業機械の姿勢演算装置、油圧ショベルの姿勢演算装置及び作業機械に関する。

近年、油圧ショベル又はブルドーザ等の作業機械において、作業機械の位置を検出して作業管理をしたり、作業機械の位置情報と施工現場の地形を示す施工情報とに基づいて施工管理をしたりするものがある（例えば、特許文献１）。

特開２００５−６１０２４号公報

作業機械が備える作業機の位置及び姿勢を求める場合、作業機械の傾きに関する情報を正確に求める必要がある。例えば、ＩＭＵ（Inertial Measurement Unit：慣性計測装置）を作業機械に搭載し、ＩＭＵの検出値からロール角及びピッチ角といった姿勢角を作業機械の傾きに関する情報として得ることができる。ＩＭＵの検出値から作業機械の姿勢角を求める場合、姿勢角の精度が、例えば、求められる作業機等の位置及び姿勢の精度に影響を与える。

本発明は、作業機械の姿勢角を求めるにあたって、姿勢角の精度低下を抑制することを目的とする。

本発明は、作業機械に備えられて、角速度及び加速度を検出する計測装置と、前記計測装置によって検出された角速度及び加速度から前記作業機械の姿勢角を求める姿勢角演算部と、第１の遮断周波数が設定され、前記姿勢角演算部によって求められた前記姿勢角に含まれる雑音を低減して第１姿勢角を出力する第１相補フィルタと、前記第１の遮断周波数とは異なる第２の遮断周波数が設定され、前記姿勢角演算部によって求められた前記姿勢角に含まれる雑音を低減して第２姿勢角を出力する第２相補フィルタと、前記作業機械の状態に応じて、前記第１姿勢角又は前記第２姿勢角を切り替えて出力する切替部と、を含む、作業機械の姿勢演算装置である。

前記第１の遮断周波数は、前記第２の遮断周波数よりも高いことが好ましい。

前記切替部は、前記作業機械が準静止状態であると判定した場合には前記第１姿勢角を出力し、前記作業機械が動的状態であると判定した場合には前記第２姿勢角を出力することが好ましい。

前記切替部は、前記第１姿勢角を前記第２姿勢角に切り替えた後は、切り替え時での前記第１姿勢角から前記第２姿勢角を減算した値を補正値として用いて、前記第２姿勢角を補正することが好ましい。

前記切替部は、前記第２姿勢角を前記第１姿勢角に切り替えた後は、切り替え時での前記第２姿勢角の誤差に０よりも大きく１よりも小さい係数を乗じた値を補正値として用いて、前記第１姿勢角を補正することが好ましい。

本発明は、油圧ショベルの上部旋回体に備えられて、角速度及び加速度を検出する計測装置と、前記計測装置が検出した前記角速度及び前記加速度から前記油圧ショベルの姿勢角を求める姿勢角演算部と、第１の遮断周波数が設定され、前記姿勢角演算部によって求められた前記姿勢角に含まれる雑音を低減して第１姿勢角を出力する第１相補フィルタと、前記第１の遮断周波数よりも低い第２の遮断周波数が設定され、前記姿勢角演算部によって求められた前記姿勢角に含まれる雑音を低減して第２姿勢角を出力する第２相補フィルタと、前記油圧ショベルが準静止状態であると判定した場合には前記第１姿勢角を出力し、前記油圧ショベルが動的状態であると判定した場合には前記第２姿勢角出力する切替部と、を含む、油圧ショベルの姿勢演算装置である。

本発明は、前述した作業機械の姿勢演算装置と、前記作業機械の姿勢演算装置から出力された前記第１姿勢角又は前記第２姿勢角を用いて、前記作業機械に取り付けられた作業機の位置を求める制御部と、を備えた作業機械である。

本発明は、作業機械の姿勢角を求めるにあたって、姿勢角の精度低下を抑制することができる。

図１は、本実施形態に係る作業機械の斜視図である。 図２は、本実施形態に係る作業機械の制御系を示す図である。 図３は、ＩＭＵの一例を示すブロック図である。 図４は、センサ制御装置を示すブロック図である。 図５は、相補フィルタの特性を示す図である。 図６は、誤差の周波数特性を示す図である。 図７は、第１相補フィルタのゲイン及び第２相補フィルタのゲインと周波数との関係を示す図である。 図８は、本実施形態の変形例における第１姿勢角と第２姿勢角との切り替えに用いるテーブルの一例を示す図である。 図９は、センサ制御装置の切替部が出力する油圧ショベルの姿勢角と、第１姿勢角と、第２姿勢角との時間変化の一例を示す図である。 図１０は、姿勢角を求める処理の一例を示すフローチャートである。 図１１は、ＩＭＵの状態に応じて姿勢角を切り替えた結果の一例を示す図である。

本発明を実施するための形態（実施形態）につき、図面を参照しつつ詳細に説明する。

＜作業機械の全体構成＞
図１は、本実施形態に係る作業機械の斜視図である。図２は、本実施形態に係る作業機械の制御系を示す図である。作業機械としての油圧ショベル１００は、車両本体１と作業機２とを有する。車両本体１は、旋回体としての上部旋回体３と走行体としての走行装置５とを有する。上部旋回体３は、機関室３ＥＧの内部に、図２に示す動力発生装置としてのエンジン３６及び油圧ポンプ３７等の装置を収容している。機関室３ＥＧは、上部旋回体３の後端側に配置されている。本実施形態を説明するにあたり、定義する方向については後述する。

本実施形態において、油圧ショベル１００は、動力発生装置としてのエンジン３６に、例えばディーゼルエンジン等の内燃機関が用いられるが、動力発生装置はこのようなものに限定されない。油圧ショベル１００の動力発生装置は、例えば、内燃機関と発電電動機と蓄電装置とを組み合わせた、いわゆるハイブリッド方式の装置であってもよい。

上部旋回体３は、運転室４を有する。運転室４は、上部旋回体３の前端側に設置されている。すなわち、運転室４は、機関室３ＥＧが配置されている側とは反対側に設置されている。運転室４内には、図２に示す、第１表示装置２８及び操作装置３０が配置される。これらについては後述する。上部旋回体３の上方には、手すり１９及びアンテナ２０、２１が取り付けられている。アンテナ２０、２１についての詳細は後述する。

走行装置５は、履帯５ａ、５ｂを有している。走行装置５は、左右に設けられた油圧モータ５ｃの一方又は両方が駆動し、履帯５ａ、５ｂが回転することにより、油圧ショベル１００を前後に走行又は旋回させる。作業機２は、上部旋回体３の運転室４の側方側に取り付けられている。

油圧ショベル１００は、履帯５ａ、５ｂの代わりにタイヤを備え、図２に示すエンジン３６の駆動力を、トランスミッションを介してタイヤへ伝達して走行が可能な走行装置を備えたものであってもよい。このような形態の油圧ショベル１００としては、例えば、ホイール式油圧ショベルがある。また、油圧ショベル１００は、このようなタイヤを有した走行装置を備え、さらに車両本体に作業機が取り付けられ、図１に示すような上部旋回体３及びその旋回機構を備えていない構造を有する、例えばバックホウローダであってもよい。すなわち、バックホウローダは、車両本体に作業機が取り付けられ、車両本体の一部を構成する走行装置を備えたものである。

上部旋回体３は、作業機２及び運転室４が配置されている側が前であり、機関室３ＥＧが配置されている側が後である。前に向かって左側が上部旋回体３の左であり、前に向かって右側が上部旋回体３の右である。また、油圧ショベル１００又は車両本体１は、上部旋回体３を基準として走行装置５側が下であり、走行装置５を基準として上部旋回体３側が上である。油圧ショベル１００が水平面に設置されている場合、下は鉛直方向、すなわち重力の作用方向側であり、上は鉛直方向とは反対側である。

作業機２は、ブーム６とアーム７とバケット８とブームシリンダ１０とアームシリンダ１１とバケットシリンダ１２とを有する。ブーム６の基端部は、ブームピン１３を介して上部旋回体３の前部に揺動可能に取り付けられている。アーム７の基端部は、アームピン１４を介してブーム６の先端部に揺動可能に取り付けられている。アーム７の先端部には、バケットピン１５を介してバケット８が取り付けられている。バケット８は、バケットピン１５を中心として揺動する。バケット８は、バケットピン１５とは反対側に複数の刃８Ｂが取り付けられている。刃先８Ｔは、刃８Ｂの先端である。

バケット８は、複数の刃８Ｂを有していなくてもよい。つまり、図１に示すような刃８Ｂを有しておらず、刃先が鋼板によってストレート形状に形成されたようなバケットであってもよい。作業機２は、例えば、単数の刃を有するチルトバケットを備えていてもよい。チルトバケットとは、バケットチルトシリンダを備え、バケットが左右にチルト傾斜することで油圧ショベルが傾斜地にあっても、斜面、平地を自由な形に成形、整地をすることができ、底板プレートによる転圧作業もできるバケットである。この他にも、作業機２は、バケット８の代わりに、法面バケット又は削岩用のチップを備えた削岩用のアタッチメント等を備えていてもよい。

図１に示すブームシリンダ１０とアームシリンダ１１とバケットシリンダ１２とは、それぞれ作動油の圧力（以下、適宜油圧という）によって駆動される油圧シリンダである。ブームシリンダ１０はブーム６を駆動して、これを昇降させる。アームシリンダ１１は、アーム７を駆動して、アームピン１４の周りを回動させる。バケットシリンダ１２は、バケット８を駆動して、バケットピン１５の周りを回動させる。

ブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２等の油圧シリンダと図２に示す油圧ポンプ３７との間には、図２に示す油圧制御弁３８が設けられている。油圧制御弁３８は、油圧モータ５ｃを駆動するための走行用制御弁と、ブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２並びに上部旋回体３を旋回させる旋回モータを制御するための作業機用制御弁とを含む。図２に示す作業機制御装置２５が、油圧制御弁３８を制御することにより、ブームシリンダ１０、アームシリンダ１１、バケットシリンダ１２、旋回モータ又は油圧モータ５ｃに供給される作動油の流量が制御される。その結果、ブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２等の動作が制御される。

上部旋回体３の上部には、アンテナ２０、２１が取り付けられている。アンテナ２０、２１は、油圧ショベル１００の現在位置を検出するために用いられる。アンテナ２０、２１は、図２に示す、油圧ショベル１００の現在位置を検出するための位置検出装置２３と電気的に接続されている。位置検出装置２３は、ＲＴＫ−ＧＮＳＳ（Real Time Kinematic - Global Navigation Satellite SＹstems、ＧＮＳＳは全地球航法衛星システムをいう）を利用して油圧ショベル１００の現在位置を検出する。以下の説明において、アンテナ２０、２１を、適宜ＧＮＳＳアンテナ２０、２１という。

ＧＮＳＳアンテナ２０、２１が受信したＧＮＳＳ電波に応じた信号は、位置検出装置２３に入力される。位置検出装置２３は、ＧＮＳＳアンテナ２０、２１の設置位置を検出する。

ＧＮＳＳアンテナ２０、２１は、図１に示すように、上部旋回体３の上であって、油圧ショベル１００の左右方向に離れた両端位置に設置されることが好ましい。ＧＮＳＳアンテナ２０、２１は、上部旋回体３の上であって、図１に示すように、手すり１９に取り付けられていてもよい。また、ＧＮＳＳアンテナ２０、２１は、上部旋回体３の後端に設けられたカウンタウエイト３ＣＷの上部又は運転室４の後方であって機械室３ＥＧの上に設置されてもよい。いずれにしても、ＧＮＳＳアンテナ２０、２１は、互いに可能な限り離れた位置に設置される方が、油圧ショベル１００の現在位置の検出精度は向上する。また、ＧＮＳＳアンテナ２０、２１は、オペレータの視界を極力妨げない位置に設置されることが好ましい。

（油圧ショベルの制御系）
図２を用いて、油圧ショベル１００の制御系について説明する。油圧ショベル１００は、制御系として、作業機械の姿勢演算装置としてのセンサ制御装置２４と、作業機制御装置２５と、エンジン制御装置２６と、ポンプ制御装置２７と、第１表示装置２８と、角速度及び加速度を検出する慣性計測装置としてのＩＭＵ２９と、第２表示装置３９とを含む。これらは、上部旋回体３に設置される。例えば、ＩＭＵ２９は運転室４の下部に設置されている。第１表示装置２８及び第２表示装置３９は、運転室４の内部の運転席近傍に設置される。

センサ制御装置２４と、作業機制御装置２５と、エンジン制御装置２６と、ポンプ制御装置２７と、第１表示装置２８と、位置検出装置２３とは、油圧ショベル１００内に設置された車内信号線４１と電気的に接続されている。センサ制御装置２４と、作業機制御装置２５と、エンジン制御装置２６と、ポンプ制御装置２７と、第１表示装置２８と、位置検出装置２３とは、車内信号線４１を介して互いに通信できる。センサ制御装置２４とＩＭＵ２９と第２表示装置３９とは、車内信号線４１とは異なる車内信号線４２と電気的に接続されている。センサ制御装置２４とＩＭＵ２９と第２表示装置３９は、車内信号線４２を介して互いに通信できる。ＩＭＵ２９は、車内信号線４２ではなく、車内信号線４１と電気的に接続されていてもよい。

センサ制御装置２４は、図１に示すブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２のストロークを検出するセンサ、ブーム６の角度及びアーム７の角度を検出するセンサ並びに上部旋回体３の旋回角度を検出するセンサ等といった各種のセンサ類３５が電気的に接続される。ブーム６の角度、アーム７の角度及び旋回角度を検出するセンサは、必ずしも必要ではない。センサ制御装置２４は、各種のセンサ類３５が検出した信号に、フィルタ処理又はＡ／Ｄ（Analog／Digital）変換等の各種の信号処理を施した後、車内信号線４１に出力する。

ブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２のストロークを検出するセンサとして、各油圧シリンダの伸縮量を検知する近接センサやロータリエンコーダなどを用いることができる。また、ブーム６の基端部及びアーム７の基端部に、図示しないエンコーダを備え、そのエンコーダによって各油圧シリンダの伸縮量を検知する近接センサ又はロータリエンコーダの検出値をリセットすることができるようにしてもよい。

センサ制御装置２４は、車内信号線４２から、ＩＭＵ２９が検出した信号、具体的には加速度及び角速度を取得する。センサ制御装置２４は、ＩＭＵ２９から取得した加速度及び角速度を用いて作業機械の姿勢角を求め、求めた姿勢角に本実施形態に係るフィルタ処理を施してノイズを除去してから、車内信号線４１に出力する。姿勢角は、油圧ショベル１００の傾きに関する情報である。姿勢角の精度は、第２表示装置３９による刃先８Ｔの位置情報の表示精度及び油圧ショベル１００の作業機制御の精度に影響を与える。本実施形態に係るフィルタ処理を実行することで、姿勢角の精度を確保する。本実施形態に係るフィルタ処理については後述する。

作業機制御装置２５は、操作装置３０からの入力に基づき、図１に示す作業機２の動作、上部旋回体３の動作及び走行装置５の動作を制御する。操作装置３０は、操作部としての作業機操作部材３１Ｌ、３１Ｒ及び走行操作部材３３Ｌ、３３Ｒと、作業機操作検出部３２Ｌ、３２Ｒ及び走行操作検出部３４Ｌ、３４Ｒとを有する。本実施形態において、作業機操作部材３１Ｌ、３１Ｒ及び走行操作部材３３Ｌ、３３Ｒは、パイロット圧方式のレバーであるが、これに限定されない。作業機操作部材３１Ｌ、３１Ｒ及び走行操作部材３３Ｌ、３３Ｒは、例えば、ポテンショメータを用いて各操作部材（レバー）の操作量を検出可能な電気方式のレバーであってもよい。

作業機操作部材３１Ｌ、３１Ｒは、運転席の左右に設置され、油圧ショベル１００のオペレータが作業機２を操作するための部材であり、例えば、ジョイスティックのような握り部分と棒材とを備えた操作レバーである。このような構造の作業機操作部材３１Ｌ、３１Ｒは、握り部を握って前後左右に傾倒させることが可能である。例えば左に設置された作業機操作部材３１Ｌを操作することで、アーム７及び上部旋回体３を動作させることができ、右に設置された作業機操作部材３１Ｒを操作することで、バケット８及びブーム６を動作させることができる。

作業機操作検出部３２Ｌ、３２Ｒは、作業機操作部材３１Ｌ、３１Ｒに対する入力、すなわち操作内容に応じてパイロット圧を発生させ、油圧制御弁３８が備える作業用制御弁に、発生した作動油のパイロット圧を供給する。このパイロット圧の大きさに応じて、作業用制御弁が動作して、図１に示すブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２等に、油圧ポンプ３７から作動油が供給される。作業機操作部材３１Ｌ、３１Ｒが電気方式のレバーである場合、作業機操作検出部３２Ｌ、３２Ｒは、作業機操作部材３１Ｌ、３１Ｒに対する入力、すなわち操作内容を、例えばポテンショメータ等を用いて検出し、入力を電気信号（検出信号）に変換して作業機制御装置２５へ送る。作業機制御装置２５は、この検出信号に基づいて、作業用制御弁を制御する。

走行操作部材３３Ｌ、３３Ｒは、運転席の前方に設置され、オペレータが油圧ショベル１００の走行装置５を走行又は旋回させる際に操作するための部材である。走行操作部材３３Ｌ、３３Ｒは、例えば、握り部と棒材とを備えた操作レバー（以下、適宜走行レバーと称呼する）である。このような走行操作部材３３Ｌ、３３Ｒは、オペレータが握り部を握って前後に傾倒させることが可能である。走行操作部材３３Ｌ、３３Ｒは、２つの操作レバーが同時に前に傾倒されれば油圧ショベル１００が前進し、後ろに傾倒されれば油圧ショベル１００は後進する。

走行操作部材３３Ｌ、３３Ｒは、オペレータが足で踏むことで操作が可能な図示しないペダルでもよく、例えばシーソー式のペダルである。ペダルの前側又は後側のいずれかを踏むことで前述した操作レバーと同様にパイロット圧が発生し、走行用制御弁が制御され、油圧モータ５ｃが駆動し油圧ショベル１００を前進又は後進させることができる。２つのペダルを同時に、かつ前側を踏めば油圧ショベル１００は前進し、後側を踏めば油圧ショベル１００は後進する。片方のペダルの前側又は後側を踏めば、履帯５ａ、５ｂの片側のみが回転し、油圧ショベル１００の走行装置５を旋回させることができる。

このように、オペレータは、油圧ショベル１００を走行させたい場合、手で操作レバーを前後に傾倒させるか又は足でペダルの前側又は後側を踏むかのうちいずれか一方を実行すれば、走行装置５の油圧モータ５ｃを駆動させることができる。図２に示すように、走行操作部材３３Ｌ、３３Ｒ及び走行操作検出部３４Ｌ、３４Ｒは、２組存在する。左側の走行操作部材３３Ｌを操作することで、左側の油圧モータ５ｃを駆動させて左側の履帯５ｂを動作させることができる。右側の走行操作部材３３Ｒを操作することで、右側の油圧モータ５ｃを駆動させて右側の履帯５ａを動作させることができる。

走行操作検出部３４Ｌ、３４Ｒは、走行操作部材３３Ｌ、３３Ｒに対する入力、すなわち操作内容に応じてパイロット圧を発生させ、油圧制御弁３８が備える走行用制御弁に、発生したパイロット圧を供給する。このパイロット圧の大きさに応じて、走行用制御弁が動作して、走行用の油圧モータ５ｃに作動油が供給される。走行操作部材３３Ｌ、３３Ｒが電気方式のレバーである場合、走行操作検出部３４Ｌ、３４Ｒは、走行操作部材３３Ｌ、３３Ｒに対する入力、すなわち操作内容を、例えばポテンショメータ等を用いて検出し、入力を電気信号（検出信号）に変換して作業機制御装置２５へ送る。作業機制御装置２５は、この検出信号に基づいて、走行用制御弁を制御する。

エンジン制御装置２６は、エンジン３６へ供給する燃料の量を調整することにより、エンジン３６の出力を制御する。エンジン３６は、出力シャフトに機械的に連結された油圧ポンプ３７を駆動する。エンジン３６に連結された油圧ポンプ３７は、油圧ショベル１００が備えるブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２等の油圧機器に作動油を供給する。エンジン制御装置２６には回転速度検出センサ３６Ｒ及び燃料調整ダイヤル２６Ｄが電気的に接続されている。エンジン制御装置２６は、回転速度検出センサ３６Ｒが検出したエンジン３６のクランクシャフトの回転速度及び燃料調整ダイヤル２６Ｄの設定等に基づいて、エンジン３６に供給する燃料の量を制御する。このようにして、エンジン制御装置２６は、エンジン３６の出力を制御する。

ポンプ制御装置２７は、油圧ショベル１００が備える油圧ポンプ３７を制御する。油圧ポンプ３７は、例えば、斜板の傾転角を変更することにより作動油の吐出量等を変更する斜板式の油圧ポンプである。ポンプ制御装置２７は、例えば、車内信号線４１を介して作業機制御装置２５から、油圧制御弁３８の油圧センサ３８Ｃが検出したパイロット圧を取得する。ポンプ制御装置２７は、取得したパイロット圧に基づいて油圧ポンプ３７の斜板の傾転角を制御することにより、油圧ポンプ３７から吐出される作動油の流量を制御する。油圧ポンプ３７から吐出された作動油は、油圧制御弁３８が備える作業用制御弁又は走行用制御弁を介して、ブームシリンダ１０、アームシリンダ１１、バケットシリンダ１２及び油圧モータ５ｃ及び上部旋回体３を旋回させるための旋回モータの少なくとも１つに供給され、これらの少なくとも１つを駆動する。

第１表示装置２８は、画像を表示する装置である。第１表示装置２８は、表示部２８Ｍと、制御部２８Ｃとを含む。第１表示装置２８は、図１に示す、油圧ショベル１００の運転室４内であって、運転席近傍に設置される。本実施形態において、第１表示装置２８は、例えば、油圧ショベル１００の稼働情報を表示部２８Ｍに表示する。稼働情報は、例えば、油圧ショベル１００の累積稼働時間、燃料の残量又はエンジン３６の冷却水温度等である。油圧ショベル１００が周辺監視用又はバックモニタ用のカメラ等を備える場合、第１表示装置２８は、このカメラが撮像した画像を表示してもよい。

本実施形態において、第１表示装置２８は、各種の画像を表示部２８Ｍに表示する他に、入力装置としても機能する。このために、第１表示装置２８は、表示部２８Ｍの下方に、入力装置２８Ｉを備える。本実施形態において、入力装置２８Ｉは、複数の押しボタン式のスイッチが表示部２８Ｍの横方向に対して平行に配列されている。入力装置２８Ｉが操作されることにより、表示部２８Ｍに表示される画像を切り替えたり、油圧ショベル１００の動作に関する各種の設定を実行したりすることができる。なお、入力装置２８Ｉを表示部２８Ｍに組み込んだタッチパネルによって第１表示装置２８を構成するようにしてもよい。また、入力装置２８Ｉは、第１表示装置２８とは別体として、運転席近傍のコンソールに設置するようにしてもよい。

第２表示装置３９は、画像を表示する装置である。第２表示装置３９は、表示部３９Ｍと、制御部３９Ｃとを含む。第２表示装置３９は、図１に示す、油圧ショベル１００の運転室４内の運転席近傍に設置される。本実施形態において、第２表示装置３９は、例えば、油圧ショベル１００が備えるバケット８の刃先８Ｔの、施工現場の地形に対する位置情報を、画像として表示部２８Ｍに表示する。このとき、第２表示装置３９は、刃先８Ｔが掘削しようとしている施工現場の地形に関する情報を、刃先８Ｔの位置情報とともに表示してもよい。

本実施形態において、第２表示装置３９の表示部３９Ｍは、例えば、液晶表示装置であるが、これに限定されるものではない。制御部３９Ｃは、表示部３９Ｍの動作を制御したり、刃先８Ｔの位置情報を求めたりする。また、制御部３９Ｃは、刃先８Ｔの位置と施工現場の地形との相対位置関係を示すガイダンス画像を表示部３９Ｍに表示させたりする。このために、制御部３９Ｃは、施工現場の地形についてのグローバル座標位置情報を記憶している。

本実施形態において、第２表示装置３９は、表示部３９Ｍの下方に入力装置３９Ｉを備える。本実施形態において、入力装置３９Ｉは、複数の押しボタン式のスイッチが表示部３９Ｍの横方向に対して平行に配列されている。入力装置３９Ｉが操作されることにより、表示部３９Ｍに表示されるガイダンス画像を切り替えたり、ガイダンスの内容を変更したりすることができる。本実施形態において、第２表示装置３９の機能は、第１表示装置２８が実現してもよい。入力装置３９Ｉを表示部３９Ｍに組み込んだタッチパネルによって第２表示装置３９を構成するようにしてもよい。また、第１表示装置２８と第２表示装置３９とを一つの筐体に収めた表示装置としたものを用いてもよい。

ＩＭＵ２９は、油圧ショベル１００の角速度及び加速度を検出する。油圧ショベル１００の動作にともない、走行時に発生する加速度、旋回時に発生する角加速度及び重力加速度といった様々な加速度が生じるが、ＩＭＵ２９は少なくとも重力加速度を含む加速度を検出し、各加速度の種類を区別することなく検出した加速度を出力する。ＩＭＵ２９は、詳細については後述するが、より高い精度で加速度を検出するために、例えば、油圧ショベル１００の上部旋回体３の旋回中心軸上に設けられることが望ましいが、前述のようにＩＭＵ２９は運転室４の下部に設置されてもよい。

ＩＭＵ２９は、図１に示す座標系（Ｘ、Ｙ、Ｚ）において、Ｘ軸方向、Ｙ軸方向及びＺ軸方向の加速度並びにＸ軸、Ｙ軸及びＺ軸周りの角速度（回転角速度）を検出する。図１に示す例において、Ｘ軸は油圧ショベル１００の前後方向と平行な軸であり、Ｙ軸は油圧ショベル１００の幅方向と平行な軸であり、Ｚ軸はＸ軸及びＹ軸の両方に直交する軸である。座標系（Ｘ、Ｙ、Ｚ）は、例えば車体座標系とすることができる。

図３は、ＩＭＵ２９の一例を示すブロック図である。ＩＭＵ２９は、ジャイロ２９Ｖと、加速度センサ２９Ａと、ＡＤ変換部２９ＡＤと、物理量変換部２９ＰＴとを含む。ジャイロ２９Ｖは、油圧ショベル１００の角速度を検出する。加速度センサ２９Ａは、油圧ショベルの加速度を検出する。ジャイロ２９Ｖによって検出された角速度及び加速度センサ２９Ａによって検出された加速度はいずれもアナログ量である。ＡＤ変換部２９ＡＤは、これらのアナログ量をディジタル量に変換する。物理量変換部２９ＰＴは、ＡＤ変換部２９ＡＤの出力を、物理量に変換する。具体的には、物理量変換部２９ＰＴは、ジャイロ２９Ｖの検出値に対応するＡＤ変換部２９ＡＤの出力を角速度ωに変換し、加速度センサ２９Ａの検出値に対応するＡＤ変換部２９ＡＤの出力を加速度Ａｃに変換する。物理量変換部２９ＰＴは、角速度ω及び加速度Ａｃを車内信号線４２に出力する。

油圧ショベル１００の傾きは、ピッチ角、ロール角及びヨー角で表すことができる。ピッチ角は、Ｙ軸周りに油圧ショベル１００が傾斜したときの角度であり、ロール角はＸ軸周りに油圧ショベル１００が傾斜したときの角度であり、ヨー角は、Ｚ軸周りに油圧ショベル１００が傾斜したときの角度である。本実施形態において、ピッチ角及びロール角を、油圧ショベル１００の姿勢角という。本実施形態において、センサ制御装置２４は、ＩＭＵ２９が検出した、油圧ショベル１００の角速度及び加速度を、車内信号線４２を介して取得する。センサ制御装置２４は、取得した油圧ショベル１００の角速度及び加速度から、姿勢角を求める。以下において、姿勢角は、適宜符号θを用いて表す。

図４は、センサ制御装置２４を示すブロック図である。センサ制御装置２４は、姿勢角演算部としての第１姿勢角演算部５１と、同じく姿勢角演算部としての第２姿勢角演算部５２と、第１相補フィルタ５３と、第２相補フィルタ５４と、切替部５５とを含む。第１姿勢角演算部５１及び第２姿勢角演算部５２は、油圧ショベル１００の角速度ω及び加速度Ａｃから油圧ショベル１００の姿勢角θを求める。本実施形態において、第１姿勢角演算部５１は、ＩＭＵ２９が検出した油圧ショベル１００の加速度Ａｃから姿勢角θを求める。より具体的には、第１姿勢角演算部５１は、重力加速度の方向から姿勢角θを求める。第２姿勢角演算部５２は、ＩＭＵ２９が検出した油圧ショベル１００の角速度ωから姿勢角θを求める。より具体的には、第２姿勢角演算部５２は、角速度ωを積分して姿勢角θを求める。

第１相補フィルタ５３は、第１の遮断周波数が設定され、第１姿勢角演算部５１及び第２姿勢角演算部５２によって求められた姿勢角θに含まれる雑音を低減して、第１姿勢角θ１を出力する。第２相補フィルタ５４は、第１の遮断周波数とは異なる第２の遮断周波数が設定され、第１姿勢角演算部５１及び第２姿勢角演算部５２によって求められた姿勢角θに含まれる雑音を低減して第２姿勢角θ２を出力する。第１相補フィルタ５３と第２相補フィルタ５４は、遮断周波数（カットオフ周波数）のみが異なる。

第１相補フィルタ５３は、フィルタ部５３Ｆと、加算部５３ＡＤとを有する。フィルタ部５３Ｆは、第１ＬＰＦ（Low Pass Filter）ａと第１ＨＰＦ（High Pass Filter）ａとを有する。加算部５３ＡＤは、第１ＬＰＦａの出力と第１ＨＰＦａの出力とを加算して出力する。加算部５３ＡＤの出力は、第１相補フィルタ５３の出力である。第１相補フィルタ５３の出力を、適宜第１姿勢角θ１と称する。

第２相補フィルタ５４は、フィルタ部５４Ｆと、加算部５４ＡＤとを有する。フィルタ部５４Ｆは、第２ＬＰＦ（Low Pass Filter）ｂと第２ＨＰＦ（High Pass Filter）ｂとを有する。加算部５４ＡＤは、第２ＬＰＦｂの出力と第２ＨＰＦｂの出力とを加算して出力する。加算部５４ＡＤの出力は、第２相補フィルタ５４の出力である。第２相補フィルタ５４の出力を、第２姿勢角θ２と称する。

切替部５５は、処理部５５Ｃと、切替器５５Ｓとを有する。切替部５５は、油圧ショベル１００の動作状態に応じて、第１姿勢角θ１又は第２姿勢角θ２を切り替えて出力する。次に、この出力について説明する。切替部５５の処理部５５Ｃは、油圧ショベル１００の状態、例えば、油圧ショベル１００が動いているか静止しているかに応じて、第１姿勢角θ１又は第２姿勢角θ２のいずれを出力するかを判定する。処理部５５Ｃの判定結果は、判定結果出力線５５ａを介して切替器５５Ｓに出力され、切替器５５Ｓは、処理部５５Ｃの判定結果に応じて、第１姿勢角θ１又は第２姿勢角θ２のいずれか一方を、センサ制御装置２４によって求められた、油圧ショベル１００の姿勢角θｏとして、姿勢角出力線５５ｂを介して車内信号線４１に出力する。

図５は、相補フィルタの特性を示す図である。図５の縦軸はゲインＧＮ、横軸は周波数ｆである。図５の曲線（ＬＰＦとＨＰＦ）は、相補フィルタの周波数特性を示す。相補フィルタは、ＬＰＦ（Low Pass Filter）と、ＨＰＦ（High Pass Filter）とを備えており、図５から分かるように、ＬＰＦのゲインＧＮとＨＰＦのゲインＧＮとの和が１となるフィルタである。例えば、相補フィルタに姿勢角θを入力すると、ＬＰＦの出力ＬＰＦ（θ）とＨＰＦの出力ＨＰＦ（θ）との和は１となる。すなわち、ＬＰＦ（θ）＋ＨＰＦ（θ）＝θとなる。ＬＰＦのゲインＧＮとＨＰＦのゲインＧＮとがいずれも０．５になるときの周波数を、遮断周波数ｆｃという。センサ制御装置２４が有する第１相補フィルタ５３及び第２相補フィルタ５４は、前述したように、遮断周波数ｆｃのみが異なる。

図４に示す第１姿勢角演算部５１が重力加速度の方向から求めた姿勢角θは、真の姿勢角θｔｒと、誤差θａｎとの和で求められる。誤差θａｎは、例えば、衝撃加速度等のような重力加速度以外の加速度によって生じる。誤差θａｎは、高周波成分が主体の雑音である。図４に示す第２姿勢角演算部５２が角速度ωを積分して求めた姿勢角θは、真の姿勢角θｔｒと、誤差θｗｎとの和で求められる。誤差θｗｎは、積分によって累積するドリフトによって生じる。誤差θｗｎは、低周波成分が主体の雑音である。

このように、第１姿勢角演算部５１が重力加速度の方向から求めた姿勢角θは、高周波成分が主体の誤差θａｎを含むため、第１相補フィルタ５３の第１ＬＰＦａ及び第２相補フィルタ５４の第２ＬＰＦｂに入力される。第２姿勢角演算部５２が角速度ωを積分して求めた姿勢角θは、低周波成分が主体の誤差θｗｎを含むため、第１相補フィルタ５３の第１ＨＰＦａ及び第２相補フィルタ５４の第２ＨＰＦｂに入力される。

第１ＬＰＦａの出力はＬＰＦａ（θｔｒ＋θａｎ）、第１ＨＰＦａの出力はＨＰＦａ（θｔｒ＋θｗｎ）となる。第２ＬＰＦｂの出力はＬＰＦｂ（θｔｒ＋θａｎ）、第２ＨＰＦｂの出力はＨＰＦｂ（θｔｒ＋θｗｎ）となる。ＬＰＦａ（θｔｒ＋θａｎ）、ＨＰＦａ（θｔｒ＋θｗｎ）、ＬＰＦｂ（θｔｒ＋θａｎ）及びＨＰＦｂ（θｔｒ＋θｗｎ）は、いずれも線形性を有するとすると、式（１）から式（４）が成立する。
ＬＰＦａ（θｔｒ＋θａｎ）＝ＬＰＦａ（θｔｒ）＋ＬＰＦａ（θａｎ）・・（１）
ＨＰＦａ（θｔｒ＋θｗｎ）＝ＨＰＦａ（θｔｒ）＋ＨＰＦａ（θｗｎ）・・（２）
ＬＰＦｂ（θｔｒ＋θａｎ）＝ＬＰＦｂ（θｔｒ）＋ＬＰＦｂ（θａｎ）・・（３）
ＨＰＦｂ（θｔｒ＋θｗｎ）＝ＨＰＦｂ（θｔｒ）＋ＨＰＦｂ（θｗｎ）・・（４）

前述した相補フィルタの特性から、ＬＰＦａ（θ）＋ＨＰＦａ（θ）＝θ及びＬＰＦｂ（θ）＋ＨＰＦｂ（θ）＝θが成立する。第１相補フィルタ５３において、フィルタ部５３Ｆの出力、すなわち第１ＬＰＦａの出力と第１ＨＰＦａの出力とは、加算部５３ＡＤによって加算される。加算部５３ＡＤの出力、すなわち第１姿勢角θ１は、θｔｒ＋ＬＰＦａ（θａｎ）＋ＨＰＦａ（θｗｎ）となる。第２相補フィルタ５４において、フィルタ部５４Ｆの出力、すなわち第２ＬＰＦｂの出力と第２ＨＰＦｂの出力とは、加算部５４ＡＤによって加算される。加算部５４ＡＤの出力、すなわち第２姿勢角θ２は、θｔｒ＋ＬＰＦｂ（θａｎ）＋ＨＰＦｂ（θｗｎ）となる。

誤差θａｎは高周波成分が主体なので、第１ＬＰＦａ及び第２ＬＰＦｂによって低減される。このため、ＬＰＦａ（θａｎ）及びＬＰＦｂ（θａｎ）の値は小さくなる。誤差θｗｎは低周波成分が主体なので、第１ＨＰＦａ及び第２ＨＰＦｂによって低減される。このため、ＬＰＦａ（θａｎ）及びＨＰＦａ（θｗｎ）並びにＬＰＦｂ（θａｎ）及びＨＰＦｂ（θｗｎ）の値は小さくなり、加算部５３ＡＤの出力である第１姿勢角θ１及び加算部５４ＡＤの出力である第２姿勢角θ２は、真の姿勢角θｔｒに近い値となる。

図６は、誤差θａｎ及び誤差θｗｎの周波数特性を示す図である。図６の縦軸は、誤差θａｎ及び誤差θｗｎのスペクトル、横軸は周波数ｆである。仮に、ＩＭＵ２９の性能が高いものを用いることができた場合、ＩＭＵ２９が検出する角速度ω及び加速度Ａｃの精度も高いため、図４に示すセンサ制御装置２４が有する第１姿勢角演算部５１が求める姿勢角θの誤差θａｎ及び第２姿勢角演算部５２が求める姿勢角θの誤差θｗｎは小さくなる。ＩＭＵ２９の性能が低い場合、ＩＭＵ２９が検出する角速度ω及び加速度Ａｃの精度が低くなるため、図４に示すセンサ制御装置２４が有する第１姿勢角演算部５１が求める姿勢角θの誤差θａｎ及び第２姿勢角演算部５２が求める姿勢角θの誤差θｗｎは大きくなる。その結果として、図６に示すように、誤差θｗｎと誤差θａｎとが、それぞれ相補フィルタの遮断周波数ｆｃを超えても存在してしまい、遮断周波数ｆｃを含む所定の周波数ｆの範囲で重なり合う。誤差θｗｎは、遮断周波数ｆｃよりも大きな周波数であっても存在してしまい、誤差θａｎは遮断周波数ｆｃよりも小さな周波数であっても存在してしまう。

したがって、ＩＭＵ２９の性能が低い場合、一つの相補フィルタでは、雑音である誤差θｗｎ及び誤差θａｎを十分に除去できず、姿勢角θの精度低下を招く可能性がある。これは、第２表示装置３９による刃先８Ｔの位置情報の表示精度及び油圧ショベル１００の作業機制御の精度に影響を与える可能性がある。高性能のＩＭＵ２９は価格も高いので、油圧ショベル１００の製造コストの上昇を招く。つまり、性能の低いＩＭＵ２９を油圧ショベル１００に適用するために、図６に示した特性を考慮する必要がある。そこで、センサ制御装置２４は、比較的性能が低いＩＭＵ２９を用いた場合でも、姿勢角θの精度低下を抑制できるように、遮断周波数ｆｃが異なる第１相補フィルタ５３と第２相補フィルタ５４とを用いている。

図７は、第１相補フィルタ５３のゲインＧＮ及び第２相補フィルタ５４のゲインＧＮと周波数ｆとの関係を示す図である。図７の縦軸はゲインＧＮ、横軸は周波数ｆである。周波数ｆｃｈは第１相補フィルタ５３の第１の遮断周波数であり、周波数ｆｃｌは第２相補フィルタ５４の第２の遮断周波数である。本実施形態において、第１の遮断周波数ｆｃｈは、第２の遮断周波数ｆｃｌよりも高くなっている。すなわち、第２の遮断周波数ｆｃｌは、第１の遮断周波数ｆｃｈよりも低くなっている。

第１相補フィルタ５３の第１の遮断周波数ｆｃｈは、角速度ωの積分誤差、すなわち誤差θｗｎを十分に低減できる周波数に設定される。第２相補フィルタ５４の第２の遮断周波数ｆｃｌは、重力加速度以外の加速度による誤差θａｎを十分に低減できる周波数に設定される。

第１相補フィルタ５３は、第１ＨＰＦａによって角速度ωの積分による誤差θｗｎを効果的に低減することができるが、重力加速度以外の加速度に起因する誤差θａｎを効果的に低減することは難しい。このため、第１相補フィルタ５３は、油圧ショベル１００が静止している状態又は静止している状態に近い状態、すなわち静止していると見なされる状態（適宜、準静止状態という）である場合においては姿勢角θを精度よく求めることができるが、油圧ショベル１００が準静止状態でない動的状態である場合においては姿勢角θの精度が低下する。本実施形態において、動的状態とは、油圧ショベル１００が動いていると見なされる状態である。

第２相補フィルタ５４は、第２ＬＰＦａによって重力加速度以外の加速度による誤差θａｎを効果的に低減することができるが、角速度ωの積分による誤差θｗｎを効果的に低減することは難しい。このため、第２相補フィルタ５４は、油圧ショベル１００が動的状態である場合においては姿勢角θを精度よく求めることができるが、油圧ショベル１００が準静止状態である場合においては、第１相補フィルタ５３が算出する姿勢角θと比較して姿勢角θの精度が低下する。すなわち、第２相補フィルタ５４は、短時間の動特性には優れるが、準静止状態においては動的状態と同様に、角速度ωの積分による誤差θｗｎが存在する。

図４に示すセンサ制御装置２４が備える切替部５５は、油圧ショベル１００の動作状態が準静止状態であるか動的状態であるかに応じて、第１姿勢角θ１又は第２姿勢角θ２を切り替えて出力する。例えば、切替部５５は、油圧ショベル１００が準静止状態である場合には、油圧ショベル１００の姿勢角θｏとして、第１相補フィルタ５３が出力する第１姿勢角θ１を車内信号線４１に出力する。油圧ショベル１００が動的状態である場合、切替部５５は、油圧ショベル１００の姿勢角θｏとして、第２相補フィルタ５４が出力する第２姿勢角θ２を車内信号線４１に出力する。

このように、センサ制御装置２４は、油圧ショベル１００が準静止状態である場合には第１相補フィルタ５３の第１姿勢角θ１を油圧ショベル１００の姿勢角θｏとするので、準静止状態において姿勢角θｏの精度低下を抑制することができる。油圧ショベル１００が動的状態である場合、センサ制御装置２４は、第２相補フィルタ５４の第２姿勢角θ２を油圧ショベル１００の姿勢角θｏとするので、動的状態においても姿勢角θｏの精度低下を抑制することができる。結果として、センサ制御装置２４は、油圧ショベル１００が準静止状態及び動的状態のいずれにおいても、油圧ショベル１００の姿勢角θｏの精度低下を抑制できる。

油圧ショベル１００が動いているときには第２相補フィルタ５４が出力する第２姿勢角θ２が用いられて、例えば図１に示すバケット８の刃先８Ｔの位置が求められる。また、油圧ショベル１００が静止しているときには第１相補フィルタ５３が出力する第１姿勢角θ１によって、バケット８の刃先８Ｔの位置が求められる。このため、図２に示す第２表示装置３９が、バケット８の刃先８Ｔの位置に代表される作業機２の位置又は油圧ショベル１００の車両本体１の位置等を求める際の精度低下が抑制される。

切替部５５の処理部５５Ｃは、準静止状態と動的状態とを、例えば、次の条件Ａと条件Ｂとを用いて判定し、その判定結果に基づいて切替器５５Ｓを制御する。
条件Ａ：切り替えの判定をする時点よりも前の所定期間において、第１姿勢角θ１の標準偏差が予め設定された閾値よりも小さい。
条件Ｂ：重力加速度以外の加速度の大きさが予め設定された閾値よりも小さい。
第１姿勢角θ１はＩＭＵ２９が検出した角速度ω又は加速度Ａｃから求められ、重力加速度を含む加速度は、ＩＭＵ２９によって検出される。すなわち、処理部５５Ｃは、油圧ショベル１００が備えるＩＭＵ２９の状態に基づき、準静止状態と動的状態とを判定する。

前述の条件Ｂについて説明する。ＩＭＵ２９は、前述のように、少なくとも重力加速度を含む加速度を検出し、検出されたそれぞれの加速度の種類を区別することなく検出した加速度を出力する。重力加速度は既知である。そこで、処理部５５Ｃは、ＩＭＵ２９が出力した加速度から、Ｘ軸方向又はＹ軸方向の加速度を演算する。処理部５５Ｃは、求められたＸ軸方向の加速度から重力加速度のＸ軸方向相当の重力加速度を減算すると、重力加速度以外の加速度の大きさを求めることができる。処理部５５Ｃは、重力加速度以外の加速度の大きさと予め設定された閾値とを比較する。なお、処理部５５Ｃは、求められたＹ軸方向の加速度から重力加速度のＹ軸方向相当の重力加速度を減算して、重力加速度以外の加速度の大きさを求め、予め設定された閾値と比較して条件Ｂが成立するかどうかを判定してもよい。

処理部５５Ｃは、ＩＭＵ２９から取得した加速度Ａｃ及び第１相補フィルタ５３の出力である第１姿勢角θ１を、図４に示す加速度送信線Ｌ１又は第１姿勢角送信線Ｌ２を介して取得し、条件Ａ及び条件Ｂが同時に成立するか否かを判定する。条件Ａと条件Ｂとの両方が成立した場合には、準静止状態と見なすことができる。本実施形態において、準静止状態とは、油圧ショベル１００が、走行、上部旋回体３の旋回及び作業機３の動作のいずれも行わずに完全に静止している状態又は走行と油圧ショベル１００の上部旋回体３の旋回が行われずに作業機３だけが動作している状態である。この場合、処理部５５Ｃは、切替器５５Ｓが第１相補フィルタ５３の加算部５３ＡＤと接続するように切換器５５Ｓを動作させる。切替器５５Ｓは、油圧ショベル１００の姿勢角θｏとして、第１相補フィルタ５３が出力する第１姿勢角θ１を車内信号線４１に出力する。

条件Ａと条件Ｂとが成立しない場合、すなわち、条件Ａ及び条件Ｂの少なくとも一方が成立しない場合には、動的状態、すなわち油圧ショベル１００が動いていると見なすことができる。この場合、処理部５５Ｃは、切替器５５Ｓが第２相補フィルタ５４の加算部５４ＡＤと接続するように切換器５５Ｓを動作させる。切替器５５Ｓは、油圧ショベル１００の姿勢角θｏとして、第２相補フィルタ５４が出力する第２姿勢角θ２を車内信号線４１に出力する。条件Ａ及び条件Ｂを用いて切替部５５が第１姿勢角θ１と第２姿勢角θ２とを切り替えるようにすれば、ＩＭＵ２９の検出値のみで前述した切り替えが実現できる。

本実施形態において、条件Ａの所定期間は、例えば１秒と設定されたものであるが、これに限定されるものではない。条件Ａの標準偏差と比較する閾値は限定されるものではないが、例えば、０．１度とすることができる。条件Ｂは、重力加速度以外の加速度が予め設定された閾値よりも小さい場合に成立し、予め設定された閾値以上の重力加速度以外の加速度が検出された場合には成立しない。条件Ｂの閾値は限定されるものではないが、例えば、重力加速度の０．１倍以上の範囲で適宜設定することができる。

（準静止状態か動的状態かを判定する変形例）
本実施形態において、図４に示す切替部５５の処理部５５Ｃは、図２に示すＩＭＵ２９の検出値に基づき、第１姿勢角θ１又は第２姿勢角θ２を切り替えて、油圧ショベル１００の姿勢角θｏとして出力した。第１姿勢角θ１又は第２姿勢角θ２の選択は、このようなものに限定されず、処理部５５Ｃは、例えば、油圧ショベル１００の動作に関する情報（以下、適宜動作情報という）を用いて第１姿勢角θ１又は第２姿勢角θ２を切り替えてもよい。

本実施形態において、動作情報は、油圧ショベル１００に何らかの動きが発生したことに関する情報である。例えば、動作情報は、図１に示す上部旋回体３が旋回しているか否かの情報、走行装置５が動作しているか否かの情報又は作業機２が動作しているか否かの情報等がある。動作情報は、例えば、上部旋回体３が旋回していることを検出するセンサから出力される検出値、上部旋回体３を旋回させるための旋回モータにレゾルバ等の旋回角度センサを設け、そのような角度検出器若しくは回転センサから出力される検出値、又は図２に示す作業機操作検出部３２Ｌ、３２Ｒにより生成されるパイロット圧を検知する油圧センサから出力される検出値等が用いられる。すなわち、動作情報は、例えば、上部旋回体３又は作業機２等が実際に動作しているか否かの情報であってもよいし、上部旋回体３又は作業機２等を動作させるための操作部材に対する操作の情報であってもよい。

図８は、本実施形態の変形例における第１姿勢角θ１と第２姿勢角θ２との切り替えに用いるテーブルＴＢの一例を示す図である。本変形例において、切替部５５の処理部５５Ｃは、ＩＭＵ２９の検出値に基づく準静止状態か動的状態かの判定と、上部旋回体３が旋回中であるか否かの判定とに基づき、第１姿勢角θ１又は第２姿勢角θ２を切り替える。テーブルＴＢは、上部旋回体３の状態と、ＩＭＵ２９の検出値に基づく条件Ａ及び条件Ｂとに対して、油圧ショベル１００の姿勢角θｏとして出力すべき姿勢角が記述されている。上部旋回体３の状態は、ＯＮ又はＯＦＦで表され、ＯＮであるときが旋回中であり、ＯＦＦであるときが停止中である。条件Ａ及び条件Ｂは、Ａ＆Ｂ又はＮＯＴ（Ａ＆Ｂ）で表され、Ａ＆Ｂが準静止状態であり、ＮＯＴ（Ａ＆Ｂ）は動的状態である。

ＩＭＵ２９の検出値に基づく判定結果が準静止状態、かつ動作情報から上部旋回体３が旋回中（ＯＮ）であるとする。この場合、切替部５５は、第２姿勢角θ２を油圧ショベル１００の姿勢角θｏとして出力する。上部旋回体３が実際に動いていることから、第２姿勢角θ２を油圧ショベル１００の姿勢角θｏとして用いることにより、姿勢角θｏの精度を確保できる。

ＩＭＵ２９の検出値に基づく判定結果が準静止状態、かつ動作情報から上部旋回体３が停止中（ＯＦＦ）であるとする。この場合、切替部５５は、第１姿勢角θ１を油圧ショベル１００の姿勢角θｏとして出力する。準静止状態であり、かつ上部旋回体３が実際には停止していることから、第１姿勢角θ１を油圧ショベル１００の姿勢角θｏとして用いることにより、角速度ωの積分による誤差を低減できる。

ＩＭＵ２９の検出値に基づく判定結果が動的状態、かつ動作情報から上部旋回体３が旋回中（ＯＮ）であるとする。この場合、切替部５５は、第２姿勢角θ２を油圧ショベル１００の姿勢角θｏとして出力する。動的状態であり、かつ上部旋回体３が実際に動いていることから、第２姿勢角θ２を油圧ショベル１００の姿勢角θｏとして用いることにより、姿勢角θｏの精度を確保できる。

ＩＭＵ２９の検出値に基づく判定結果が動的状態、かつ動作情報から上部旋回体３が停止中（ＯＦＦ）であるとする。この場合、切替部５５は、第１姿勢角θ１又は第２姿勢角θ２のいずれを油圧ショベル１００の姿勢角θｏとして出力してもよいが、本変形例では第２姿勢角θ２を出力する。

本変形例では、切替部５５は、ＩＭＵ２９の検出値に基づく準静止状態か動的状態かの判定と、上部旋回体３が旋回中であるか否かの判定とに基づき、第１姿勢角θ１又は第２姿勢角θ２を切り替えた。このようにすることで、切替部５５は、油圧ショベル１００の状態をより精度よく判定して、適切な姿勢角を選択することができる。本変形例において、前述した処理に限定されず、切替部５５は、ＩＭＵ２９の検出値に基づく判定結果を用いずに上部旋回体３が旋回中であるか否かの判定に基づき、第１姿勢角θ１又は第２姿勢角θ２を切り替えてもよい。例えば、上部旋回体３が旋回中であるには第２姿勢角θ２を油圧ショベル１００の姿勢角θｏとし、上部旋回体３が停止中であるには第１姿勢角θ１を油圧ショベル１００の姿勢角θｏとしてもよい。

図９は、センサ制御装置２４の切替部５５が出力する油圧ショベル１００の姿勢角θｏと、第１姿勢角θ１と、第２姿勢角θ２との時間変化の一例を示す図である。図９に示す太い実線は、後述する緩和項ｄｉｆの時間変化の一例を示している。図９の縦軸は姿勢角θであり、横軸は時間ｔである。図９のＳｓｔで示す区間は準静止状態であり、第１姿勢角θ１が油圧ショベル１００の姿勢角θｏとして出力される。図９のＳｄｍで示す区間は動的状態であり、第２姿勢角θ２が油圧ショベル１００の姿勢角θｏとして出力される。図９に示す例において、時間ｔ１から時間ｔ２及び時間ｔ３以降は準静止状態Ｓｓｔであり、時間ｔ２から時間ｔ３は動的状態Ｓｄｍである。

油圧ショベル１００の姿勢角θｏは、時間ｔ２で第１姿勢角θ１から第２姿勢角θ２に切り替わり、時間ｔ３で第２姿勢角θ２から第１姿勢角θ１に切り替わっている。第２姿勢角θ２は、角速度ωを積分することによる誤差θｗｎが蓄積するので、時間ｔ２において、第１姿勢角θ１と第２姿勢角θ２とは異なる値になる。同様に、時間ｔ３において、第２姿勢角θ２と第１姿勢角θ１とは異なる値になる。

センサ制御装置２４から出力される姿勢角θｏを切替部５５が第１姿勢角θ１から第２姿勢角θ２に切り替えるとき又は第２姿勢角θ２から第１姿勢角θ１に切り替えるときには、そのまま切り替えると、切替時において油圧ショベル１００の姿勢角θｏが不連続になる可能性がある。また、前述したように、第２姿勢角θ２は、角速度ωを積分することによる誤差θｗｎが蓄積するので、第２姿勢角θ２を油圧ショベル１００の姿勢角θｏとして用いる場合には、積分による誤差θｗｎを低減する必要がある。

姿勢角θｏの切り替え時に発生する姿勢角θｏの不連続及び積分による誤差θｗｎを低減するため、本実施形態において、切替部５５の処理部５５Ｃは、式（５）から式（１０）を用いて姿勢角θｏを求め、出力する。
θｏ＝θ１＋ｄｉｆ・・（５）
θｏ＝θ２＋ｄｉｆ・・・（６）
ｄｉｆ＝Ｆｔｒ×ｄｉｆ＿ｐｒｅｖ・・・（７）
ｄｉｆ＝ｄｉｆ＿ｐｒｅｖ・・・（８）
ｄｉｆ＝ｄｉｆ＿ｐｒｅｖ＋θ１−θ２・・・（９）
ｄｉｆ＝ｄｉｆ＿ｐｒｅｖ＋θ２−θ１・・・（１０）

式（５）は準静止状態で姿勢角θｏを求める場合に用いられ、式（６）は動的状態で姿勢角θｏを求める場合に用いられる。式（５）及び式（６）のｄｉｆは、緩和項である。式（７）の緩和項ｄｉｆは、準静止状態で用いられ、式（８）の緩和項ｄｉｆは動的状態で用いられる。式（７）のＦｔｒは緩和係数である。緩和係数Ｆｔｒは、０よりも大きく１よりも小さい（０＜Ｆｔｒ＜１）。式（９）の緩和項ｄｉｆは、準静止状態から動的状態に移行するタイミングで用いられる。式（１０）の緩和項ｄｉｆは、動的状態から準静止状態に移行するタイミングで用いられる。式（８）から式（１０）のｄｉｆ＿ｐｒｅｖは、直前のＩＭＵ２９の状態（準静止状態Ｓｓｔ又は動的状態Ｓｄｍ）での緩和項ｄｉｆである。ｄｉｆ＿ｐｒｅｖの初期値は０である。

図９に示すように、第１姿勢角θ１は、準静止状態Ｓｓｔでは高精度を保っているが、動的状態Ｓｄｍでは大きな誤差が生じている。第２姿勢角θ２は準静止状態Ｓｓｔ及び動的状態Ｓｄｍのいずれにおいても積分の累積による誤差が生じている。ｄｉｆ＿ｐｒｅｖの初期値は０なので、時間ｔ１から時間ｔ２の準静止状態Ｓｓｔでは緩和項ｄｉｆ＝０になる。その結果、式（５）から、準静止状態Ｓｓｔにおける油圧ショベル１００の姿勢角θｏは第１姿勢角θ１になる。

準静止状態Ｓｓｔから動的状態Ｓｄｍに切り替わるとき、すなわち時間ｔ＝ｔ２のとき、処理部５５Ｃは式（９）を用いて緩和項ｄｉｆを求める。前述したように、時間ｔ＝ｔ２ときの緩和項ｄｉｆは０なので、緩和項ｄｉｆは、第１姿勢角θ１と第２姿勢角θ２との差である、θ１−θ２の値となる。この場合の緩和項ｄｉｆは、図９に示すように、負の値である。時間ｔ２のとき、式（５）による姿勢角θｏはθ１であり、式（６）の緩和項ｄｉｆに入力されるものがθ１−θ２の値であるから、式（６）による姿勢角θｏもθ１となる。このため、準静止状態Ｓｓｔから動的状態Ｓｄｍに切り替わるときは、油圧ショベル１００の姿勢角θｏは連続して変化する。

時間ｔ２から時間ｔ３の動的状態Ｓｄｍでは、緩和項ｄｉｆの値は、切り替わり時、すなわち時間ｔ２で得られたθ１−θ２の値がそのまま保持される。動的状態Ｓｄｍにおける油圧ショベル１００の姿勢角θｏは、式（６）から、動的状態Ｓｄｍにおける第２姿勢角θ２から時間ｔ２で得られて保持されている緩和項ｄｉｆ＝θ１−θ２の値を加算して求められる。このときに用いられる緩和項ｄｉｆは、式（８）からｄｉｆ＿ｐｒｅｖとなるので、動的状態Ｓｄｍで用いられる緩和項ｄｉｆは、時間ｔ２で得られて保持されている緩和項ｄｉｆ＝θ１−θ２の値が用いられる。このように、切替部５５の処理部５５Ｃは、第１姿勢角θ１を第２姿勢角θ２に切り替えた後は、切り替え時での第１姿勢角θ１から第２姿勢角θ２を減算した値、すなわち切り替え時における緩和項ｄｉｆを補正値として用いて、求められた第２姿勢角θ２を補正して、姿勢角θｏを取得する。このようにすることで、動的状態Ｓｄｍへの切り替え以前に生じた第２姿勢角θ２の積分の蓄積による誤差θｗｎが、油圧ショベル１００の姿勢角θｏに与える影響を低減できる。

動的状態Ｓｄｍから再び準静止状態Ｓｓｔに切り替わるとき、すなわち時間ｔ３において、処理部５５Ｃは式（１０）を用いて緩和項ｄｉｆを求める。式（１０のｄｉｆ＿ｐｒｅｖは、既に得られて保持されている緩和項ｄｉｆである。つまり、式（１０）のｄｉｆ＿ｐｒｅｖは、時間ｔ２での緩和項ｄｉｆ、すなわち時間ｔ２でのθ１−θ２の値である。式（１０）から、時間ｔ３における緩和項ｄｉｆは、時間ｔ２で得られて保持されている、θ１−θ２の値と、時間ｔ３で得られたθ２−θ１の値とを加算した値になる。式（１０）を用いることにより、動的状態Ｓｄｍから準静止状態Ｓｓｔに切り替わるときは、油圧ショベル１００の姿勢角θｏは連続して変化する。

時間ｔ３以降の準静止状態Ｓｓｔにおいて、処理部５５Ｃは、式（５）を用いて油圧ショベル１００の姿勢角θｏを求める。このときの緩和項ｄｉｆは、式（７）によって決定される。式（７）中のｄｉｆ＿ｐｒｅｖは、動的状態Ｓｄｍから準静止状態Ｓｓｔに再び切り替わるタイミング、すなわち時間ｔ３における緩和項ｄｉｆである。時間ｔ３以降の準静止状態Ｓｓｔでは、緩和係数Ｆｔｒの効果で緩和項ｄｉｆの値は徐々に小さくなり、０に収束する。すなわち、時間ｔ３以降の準静止状態Ｓｓｔにおいて、油圧ショベル１００の姿勢角θｏは、第１姿勢角θ１に収束する。このように、切替部５５の処理部５５Ｃは、第２姿勢角θ２を第１姿勢角θ１に切り替えた後は、切り替え時での第２姿勢角の誤差、すなわち切り替え時での緩和項ｄｉｆに、０よりも大きく１よりも小さい係数としての緩和係数Ｆｔｒを乗じた値を補正値として用いて、第１姿勢角θ１を補正する。このようにすることで、動的状態Ｓｄｍから準静止状態Ｓｓｔに切り替わった後は、油圧ショベル１００の姿勢角θｏは連続して変化する。

（姿勢角θｏを求める処理の一例）
図１０は、姿勢角θｏを求める処理の一例を示すフローチャートである。油圧ショベル１００の姿勢角θｏを求めるにあたって、図２及び図４に示すセンサ制御装置２４は、ステップＳ１０１において、車内通信線４２を介してＩＭＵ２９による角速度ω及び加速度Ａｃの検出値を取得する。ステップＳ１０２において、図２に示す第１姿勢角演算部５１は、ＩＭＵ２９が検出した加速度Ａｃから姿勢角θを求める。ステップＳ１０３において、図２に示す第２姿勢角演算部５２は、ＩＭＵ２９が検出した角速度ωから姿勢角θを求める。ステップＳ１０２とステップＳ１０３との順序は問わない。

ステップＳ１０４において、図４に示す第１相補フィルタ５３の第１ＬＰＦａは、加速度Ａｃから得られた姿勢角θにフィルタ処理を施す。ステップＳ１０５において、図４に示す第２相補フィルタ５４の第２ＬＰＦｂは、加速度Ａｃから得られた姿勢角θにフィルタ処理を施す。ステップＳ１０６において、図４に示す第１相補フィルタ５３の第１ＨＰＦａは、角速度ωから得られた姿勢角θにフィルタ処理を施す。ステップＳ１０７において、図４に示す第２相補フィルタ５４の第２ＨＰＦｂは、角速度ωから得られた姿勢角θにフィルタ処理を施す。ステップＳ１０４とステップＳ１０５とステップＳ１０６とステップＳ１０７の順序は問わない。

次に、ステップＳ１０８に進み、第１相補フィルタ５３は、第１姿勢角θ１を求める。具体的には、加算部５３ＡＤが、第１ＬＰＦａの出力と第１ＨＰＦａの出力とを加算することにより、第１姿勢角θ１が求められる。ステップＳ１０９において、第２相補フィルタ５４は、第２姿勢角θ２を求める。具体的には、加算部５４ＡＤが、第２ＬＰＦｂの出力と第２ＨＰＦｂの出力とを加算することにより、第２姿勢角θ２が求められる。ステップＳ１０８とステップＳ１０９との順序は問わない。

ステップＳ１１０に進み、図４に示す切替部５５の処理部５５Ｃは、油圧ショベル１００が準静止状態である場合（ステップＳ１１０、Ｙｅｓ）、ステップＳ１１１に処理を進める。ステップＳ１１１において、処理部５５Ｃは、センサ制御装置２４が第１姿勢角θ１を油圧ショベル１００の姿勢角θｏとして出力するように、切替器５５Ｓを制御する。油圧ショベル１００が動的状態である場合（ステップＳ１１０、Ｎｏ）、ステップＳ１１２において、処理部５５Ｃは、センサ制御装置２４が第２姿勢角θ２を油圧ショベル１００の姿勢角θｏとして出力するように、切替器５５Ｓを制御する。

（評価例）
図１１は、油圧ショベル１００の動作状態に応じて出力される姿勢角θｏを切り替えた結果の一例を示す図である。図１１の縦軸は、姿勢角θｏの１つであるピッチ角θｐであり、横軸は時間ｔである。図１１中の実線が油圧ショベル１００のピッチ角θｐであり、１点鎖線が第１姿勢角θ１であり、２点鎖線が第２姿勢角θ２である。図１１に示す例は、図１に示す油圧ショベル１００のブーム６を持ち上げた状態から一気に下方に向かって下ろして静止させたときの、油圧ショベル１００のピッチ角θｐの時間変化を示している。ブーム６を下げて止めたときには、バケット８は接地していない。

時間ｔ１でブーム６が停止して、油圧ショベル１００のＹ軸周りにおける運動（ピッチング）、すなわち揺れが発生している。油圧ショベル１００にピッチングが発生していない場合（時間ｔ１以前）は第１姿勢角θ１が姿勢角θｏとして出力され、油圧ショベル１００にピッチングが発生している期間（時間ｔ１以降の所定期間）は第２姿勢角θ２が姿勢角θｏとして出力されていることが分かる。

時間ｔ２からｔ３までは、油圧ショベル１００に揺れ戻しが発生している。ピッチングが収まったために準静止状態と判定された後（時間ｔ２以降）は、姿勢角θｏが徐々に第１姿勢角θ１に近づき、時間ｔ３で姿勢角θｏが第１姿勢角θ１にほぼ一致することが分かる。準静止状態と判定された後は、積分による誤差の蓄積により、第２姿勢角θ２が第１姿勢角θ１とは異なる値となる。本実施形態においては、ピッチングが収まった後は、第１姿勢角θ１を油圧ショベル１００の姿勢角θｏとするので、積分誤差の影響を抑制することができる。

本実施形態は、第１の遮断周波数が設定された第１相補フィルタ５３と、第１の遮断周波数とは異なる第２の遮断周波数が設定された第２相補フィルタ５４とを用いる。第１相補フィルタ５３は、角速度ωを積分することによって累積する誤差（雑音）を低減し、第２相補フィルタ５４は、重力加速度以外の加速度による加速度による誤差（雑音）を低減する。本実施形態では、油圧ショベル１００の動作状態に応じて、第１相補フィルタ５３が出力する姿勢角と、第２相補フィルタ５４が出力する姿勢角とが切り替えられる。その結果、油圧ショベル１００の動作状態に応じた適切な相補フィルタによって油圧ショベル１００の姿勢角θｏが求められるので、動的状態及び準静止状態のいずれにおいても姿勢角θｏの精度低下が抑制される。

精度の高いＩＭＵ２９は高価であり、安価なＩＭＵ２９は精度が相対的に低い。本実施形態は、精度の低いＩＭＵ２９を用いた場合でも、動的状態及び準静止状態のいずれにおいても姿勢角θｏの精度低下を抑制することができる。このため、姿勢角θｏの精度低下を抑えつつ、油圧ショベル１００の製造コストを低減することができる。

以上、本実施形態及びその変形例を説明したが、前述した内容により本実施形態及びその変形例が限定されるものではない。また、前述した構成要素には、当業者が容易に想定できるもの、実質的に同一のもの、いわゆる均等の範囲のものが含まれる。さらに、前述した構成要素は適宜組み合わせることが可能である。さらに、本実施形態及びその変形例の要旨を逸脱しない範囲で構成要素の種々の省略、置換及び変更のうち少なくとも１つを行うことができる。例えば、作業機２は、ブーム６、アーム７、バケット８を有しているが、作業機２に装着されるアタッチメントはこれに限られず、バケット８には限定されない。作業機械は油圧ショベル１００に限定されない。例えば、本実施形態に係る姿勢演算装置をブルドーザ、モータグレーダ又はダンプトラック等に適用し、作業機制御又は走行制御等を実行することができる。

本実施形態では、第１相補フィルタ５３及び第２相補フィルタ５４を用いたが、さらに第１の遮断周波数及び第２の遮断周波数とは異なる第３の遮断周波数が設定された第３相補フィルタが追加されたり、第１の遮断周波数、第２の遮断周波数及び第３の遮断周波数とは異なる第４の遮断周波数が設定された第４相補フィルタが追加されたりしてもよい。すなわち、遮断周波数が異なる相補フィルタの数は２個に限定されない。

１ 車両本体
２ 作業機
３ 上部旋回体
４ 運転室
５ 走行装置
６ ブーム
７ アーム
８ バケット
８Ｔ 刃先
２０、２１ アンテナ
２３ 位置検出装置
２４ センサ制御装置
２５ 作業機制御装置
２６ エンジン制御装置
２７ ポンプ制御装置
２８ 第１表示装置
２８Ｍ 表示部
２９ ＩＭＵ
２９Ａ 加速度センサ
２９Ｖ ジャイロ
３０ 操作装置
３５ センサ類
３６ エンジン
３７ 油圧ポンプ
３８ 油圧制御弁
３９ 第２表示装置
４１、４２ 車内信号線
５１ 第１姿勢角演算部
５２ 第２姿勢角演算部
５３ 第１相補フィルタ
５３Ｆ フィルタ部
５３ＡＤ 加算部
５４Ｆ フィルタ部
５４ＡＤ 加算部
５４ 第２相補フィルタ
５５ 切替部
５５Ｃ 処理部
５５Ｓ 切替器
１００ 油圧ショベル
ＴＢ テーブル

Claims (7)

1. 作業機械に備えられて、角速度及び加速度を検出する計測装置と、
   前記計測装置によって検出された角速度及び加速度から前記作業機械の姿勢角を求める姿勢角演算部と、
   第１の遮断周波数が設定され、前記姿勢角演算部によって求められた前記姿勢角に含まれる雑音を低減して第１姿勢角を出力する第１相補フィルタと、
   前記第１の遮断周波数とは異なる第２の遮断周波数が設定され、前記姿勢角演算部によって求められた前記姿勢角に含まれる雑音を低減して第２姿勢角を出力する第２相補フィルタと、
   前記作業機械の状態に応じて、前記第１姿勢角又は前記第２姿勢角を切り替えて出力する切替部と、
   を含む、作業機械の姿勢演算装置。
2. 前記第１の遮断周波数は、前記第２の遮断周波数よりも高い、請求項１に記載の作業機械の姿勢演算装置。
3. 前記切替部は、
   前記作業機械が準静止状態であると判定した場合には前記第１姿勢角を出力し、
   前記作業機械が動的状態であると判定した場合には前記第２姿勢角を出力する、請求項２に記載の作業機械の姿勢演算装置。
4. 前記切替部は、
   前記第１姿勢角を前記第２姿勢角に切り替えた後は、切り替え時での前記第１姿勢角から前記第２姿勢角を減算した値を補正値として用いて、前記第２姿勢角を補正する、請求項１から請求項３のいずれか１項に記載の作業機械の姿勢演算装置。
5. 前記切替部は、前記第２姿勢角を前記第１姿勢角に切り替えた後は、切り替え時での前記第２姿勢角の誤差に０よりも大きく１よりも小さい係数を乗じた値を補正値として用いて、前記第１姿勢角を補正する、請求項４に記載の作業機械の姿勢演算装置。
6. 油圧ショベルの上部旋回体に備えられて、角速度及び加速度を検出する計測装置と、
   前記計測装置が検出した前記角速度及び前記加速度から前記油圧ショベルの姿勢角を求める姿勢角演算部と、
   第１の遮断周波数が設定され、前記姿勢角演算部によって求められた前記姿勢角に含まれる雑音を低減して第１姿勢角を出力する第１相補フィルタと、
   前記第１の遮断周波数よりも低い第２の遮断周波数が設定され、前記姿勢角演算部によって求められた前記姿勢角に含まれる雑音を低減して第２姿勢角を出力する第２相補フィルタと、
   前記油圧ショベルが準静止状態であると判定した場合には前記第１姿勢角を出力し、前記油圧ショベルが動的状態であると判定した場合には前記第２姿勢角出力する切替部と、を含む、油圧ショベルの姿勢演算装置。
7. 請求項１から請求項５のいずれか１項に記載の作業機械の姿勢演算装置と、
   前記作業機械の姿勢演算装置から出力された前記第１姿勢角又は前記第２姿勢角を用いて、前記作業機械に取り付けられた作業機の位置を求める制御部と、
   を備えた作業機械。

Images