JP2020200597A - 建設機械 - Google Patents

Abstract

【課題】フロント作業機の上げ下げなどの建設機械の姿勢変化によらずに、GNSSを利用して正確な方位角を算出することができるようにする。【解決手段】コントローラ２０は、位置姿勢演算部２０ａにおいて、車体IMU１３で取得した上部旋回体２の姿勢角とブームIMU１４で取得したフロント作業機１の姿勢角の少なくとも一方がしきい値以上であるときにGNSS受信機１７ｃで算出した上部旋回体２の方位角の品質が低いと判断し、方位角の品質とGNSS受信機１７ｃで算出した上部旋回体２の方位角とに基づいてバイアス除去演算を実行し、GNSS受信機１７ｃで算出した上部旋回体２の方位角と、ジャイロバイアスを除去した上部旋回体２の角速度に基づいて上部旋回体２の修正方位角を算出し、この修正方位角を用いてフロント作業機１の３次元の位置及び姿勢を演算する。【選択図】 図４

Description

本発明は、上部旋回体が下部走行体に対して回転可能な油圧ショベル等の建設機械に関する。

情報化施工への対応に伴い、多関節型のフロント作業機の位置や姿勢をオペレータへ表示するマシンガイダンスや、フロント作業機の位置や姿勢を目標施工面に沿って動くように制御するマシンコントロールの機能を有する油圧ショベルが開発されている。

このような油圧ショベルにおいて、３次元情報化施工（３D情報化施工）に対応させるためには、施工現場における油圧ショベル自身の位置（座標）と方位（フロント作業機の向き）を提供する必要がある。これらの情報を提供するために、従来は、上部旋回体に２本のGNSS（Global Navigation Satellite System）アンテナが取り付けられ、GNSS測位を行う油圧ショベルが開発されている。

２本のGNSSアンテナを用いてGNSS測位を行う油圧ショベルにおいては、GNSSアンテナが必要な数の衛星信号を捕捉できない場合、GNSS測位が正常に実施されなくなる。このような状態では、位置や方位情報を提供できないため、マシンガイダンスやマシンコントロールの動作を停止せざるを得ず、作業効率が低下し得る。

特許文献１では、２本のGNSSアンテナによるGNSS測位に加えて、施工現場に用意された基準局との無線通信により送信される補正データを受信し、位置情報についてRTK（Real Time Kinematic）測位を実施するとともに、ジャイロなどのヨウ角計測手段を備えることで油圧ショベルの車体（上部旋回体）の方位角を算出し、RTK測位が異常なときにその方位角を用いて位置情報を計測できるようにした技術が示されている。

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

２本のGNSSアンテナを利用して車体（上部旋回体）の方位角を算出する場合、２本のGNSSアンテナのうち１つをメインアンテナ、もう１つをサブアンテナとして用い、これらメイン、サブアンテナ間でRTK測位を行うことが一般的に行われている。これにより施工現場に設置された基準局との通信の可否によらず、車体の方位角についてはRTK測位が実施できる。

ところで、油圧ショベルのフロント作業機は金属部材で構成されるため、GNSSの衛星信号がフロント作業機によって反射、もしくは、遮断されることが考えられる。

また、油圧ショベルの掘削動作においては、フロント作業機が上下動作を繰り返すため、衛星信号の反射や途絶が繰り返し発生するようなGNSSの使用環境になる。このような使用環境においては、衛星配置が著しく偏るため、車体の方位角の演算精度が低下する。

そして、油圧ショベルではフロント作業機の上げ下げ作業を頻繁に行うため、上部旋回体を旋回していないのにもかかわらず、GNSSによって算出された車体の方位角がばらついてしまう。この算出結果をマシンガイダンスに利用すると、上部旋回体がふらふらと動作しているように見えてしまう。

なお、RTK測位を実施するには最低４つ以上の衛星が補足できていればよい。このため、上記のように作業機を上げ下げしただけで、基準局からの補正データによるRTK測位自体が実施できなくなることは稀であり、GNSS受信機は正常な測位（RTK-Fix状態）を継続する。

このため、RTK測位が異常になったときに対策を実施する特許文献１の技術では、上述した課題を解決することはできない。つまり、方位角の算出に限れば、GNSS測位が異常となる場面が特許文献１とは異なり、特許文献１の技術では、フロント作業機の上げ下げなどの建設機械の姿勢変化による車体（上部旋回体）の方位角の演算精度の低下を防止することはできない。

なお、２本のGNSSアンテナを前後方向に直線状に並べて配置すれば、遮蔽領域が左右対称にならないため（後述する図１５Ｂ及び図１５Ｃ参照）、上記の現象は多少緩和されるが、遮蔽領域が生じる現象自体は解消されない。よって、上記課題の根本的な解決にはならない。

また、フロント作業機の最高地点よりもGNSSアンテナを高くすれば、フロント作業機による遮蔽の影響がなくなる。しかし、GNSSアンテナが長くなると構造的にGNSSアンテナが振動してしまい、かえって測位精度が低下するおそれがある。

本発明は、上述した課題を解決するためになされたものであり、フロント作業機の上げ下げなどフロント作業機の姿勢変化によらずに、GNSSを利用して正確な車体の方位角を算出することができる建設機械を提供することである。

本発明は、上記課題を解決するため、下部走行体と、下部走行体に対して旋回可能な上部旋回体と、上部旋回体に対して垂直方向に回転可能に取り付けられた多関節型のフロント作業機と、前記上部旋回体に取り付けられた２本のGNSSアンテナと、前記GNSSアンテナが受信した衛星信号に基づいて前記GNSSアンテナの３次元座標と前記上部旋回体の方位角を算出するGNSS受信機を有するGNSSシステムと、前記上部旋回体に取り付けられ、前記上部旋回体の角速度を取得する角速度取得装置と、前記上部旋回体に取り付けられ、前記上部旋回体の姿勢角を取得する車体姿勢角取得装置と、前記フロント作業機の姿勢角を取得するフロント姿勢角取得装置と、コントローラとを備え、前記コントローラは、前記GNSS受信機で算出された前記GNSSアンテナの３次元座標及び前記上部旋回体の方位角と、前記車体姿勢角取得装置で取得した前記上部旋回体の姿勢角、前記角速度取得装置で取得した前記上部旋回体の角速度及び前記フロント姿勢角取得装置で取得した前記フロント作業機の姿勢角に基づいて、前記フロント作業機の位置及び姿勢を演算し、前記フロント作業機の位置及び姿勢に基づいて前記フロント作業機の動作を制御する建設機械において、前記コントローラは、前記車体姿勢角取得装置で取得した前記上部旋回体の姿勢角と前記フロント姿勢角取得装置で取得した前記フロント作業機の姿勢角の少なくとも一方に基づいて前記GNSS受信機で算出された前記上部旋回体の方位角の品質を判断し、前記方位角の品質の判断結果と、前記GNSS受信機で算出された前記上部旋回体の方位角とに基づいて、前記角速度取得装置で取得した前記上部旋回体の角速度からジャイロバイアスを除去するバイアス除去演算を実行し、かつ前記ジャイロバイアスを除去した前記上部旋回体の角速度に基づいて前記上部旋回体の旋回動作の有無を判断し、前記GNSS受信機で算出された前記上部旋回体の方位角と、前記ジャイロバイアスを除去した前記上部旋回体の角速度と、前記上部旋回体の旋回動作の判断結果に基づいて前記上部旋回体の修正方位角を算出し、前記修正方位角を用いて前記フロント作業機の位置及び姿勢を演算するものとする。

本発明によれば、フロント作業機の上げ下げなどの建設機械の姿勢変化によらずに、GNSSを利用して正確な上部旋回体の方位角を算出し、上部旋回体の方位角を高精度、かつ、ロバストに取得することができる。

本発明の実施の形態に係る建設機械の一例である油圧ショベルの外観及び内部構造の一部を示す図である。 本発明の第１の実施形態における油圧ショベルに搭載されるコントローラの処理機能の一部を示すブロック図である。 第１の実施形態の特徴部分であるコントローラの位置姿勢演算部の処理機能を示すブロック図である。 修正方位角演算部の処理機能を示すブロック図である。 方位角品質判断部の品質判断処理の一例を示す図である。 方位角品質判断部の品質判断処理の他の一例を示す図である。 修正方位角演算部の演算手順を示すフローチャートである。 本発明の第２の実施形態における修正方位角演算部の処理機能を示すブロック図である。 本発明の第３の実施形態における油圧ショベルの外観とその周囲の状況を示す図である。 第３の実施形態におけるコントローラの処理機能を示すブロック図である。 第３の実施形態における修正方位角演算部の処理機能を示すブロック図である。 本発明の第４の実施形態におけるコントローラの処理機能を示すブロック図である。 第４の実施形態における修正方位角演算部の処理機能を示すブロック図である。 油圧ショベルの上面図である。 油圧ショベルのフロント作業機が下がっている状態での衛星信号の受信状況を示す図である。 油圧ショベルのフロント作業機が下がっている状態での衛星信号の受信状況を示す図である。 油圧ショベルのフロント作業機が下がっている状態での衛星信号の受信状況を示す図である。 油圧ショベルのフロント作業機が上がっている状態での衛星信号の受信状況を示す図である。 油圧ショベルのフロント作業機が上がっている状態での衛星信号の受信状況を示す図である。 油圧ショベルのフロント作業機が上がっている状態での衛星信号の受信状況を示す図である。 油圧ショベルのフロント作業機が上がっている状態での衛星信号の受信状況を示す図である。 油圧ショベルが傾斜面に配置されている状態での衛星信号の受信状況を示す図である。 油圧ショベルの背面に急峻な傾斜面がある場合の衛星信号の受信状況を示す図である。

以下、本発明の実施の形態を図面に従い説明する。

＜第１の実施の形態＞
図１は、本発明に係る建設機械の一例である油圧ショベルの外観及び内部構造の一部を示す図である。

図１において、油圧ショベル１００は、垂直方向にそれぞれ回動する複数の被駆動部材であるブーム４、アーム５、バケット（作業具）６を連結して構成された多関節型のフロント作業機１と、車体を構成する上部旋回体２及び下部走行体３とを備え、上部旋回体２は下部走行体３に対して旋回可能に搭載されている。また、フロント作業機１のブーム４の基端は上部旋回体２の前部に垂直方向に回動可能に支持され、アーム５の一端はブーム４の先端に垂直方向に回動可能に支持され、アーム５の他端にはバケット６が垂直方向に回動可能に支持されている。ブーム４、アーム５、バケット６、上部旋回体２、及び下部走行体３は、油圧アクチュエータであるブームシリンダ４ａ、アームシリンダ５ａ、バケットシリンダ６ａ、旋回モータ２ａ、及び左右の走行モータ３ａ，３ｂによりそれぞれ駆動される。

ブーム４、アーム５及びバケット６は、フロント作業機１を含む平面上で動作し、以下ではこの平面を動作平面と称することがある。つまり、動作平面とは、ブーム４、アーム５及びバケット６の回動軸に直交する平面であり、ブーム４、アーム５及びバケット６の幅方向の中心位置に設定することができる。

オペレータが搭乗するキャビン９内（運転室）には、油圧アクチュエータ２ａ，４ａ〜６ａを操作するための操作信号を生成する操作レバー装置９ａ，９ｂやモニタ１１などが設置されている。

操作レバー装置９ａ，９ｂはいわゆる電気レバー方式であり、それぞれ、前後左右に傾倒可能な操作レバーと、これら操作レバーの傾倒量であるレバー操作量を電気的に検知する図示しない検出装置とを含み、検出装置は検出したレバー操作量を電気的な操作信号として制御装置であるコントローラ２０に電気配線を介して出力する。また、操作レバー装置９ａ，９ｂの操作レバーの前後方向または左右方向に、油圧アクチュエータ２ａ，４ａ〜６ａの操作指令がそれぞれ割り当てられている。

旋回モータ２ａ、ブームシリンダ４ａ、アームシリンダ５ａ、バケットシリンダ６ａの動作制御は、エンジンや電動モータなどの原動機４０によって駆動される油圧ポンプ装置７から油圧アクチュエータ２ａ，４ａ〜６ａに供給される作動油の方向及び流量をコントロールバルブ８内のそれぞれのスプールを制御することにより行う。コントロールバルブ８内のスプールは、図示しないパイロットポンプから電磁比例弁１０ａ，１０ｂ・・・（図２参照）を介して出力される駆動信号（パイロット圧）により操作され。コントローラ２０は、操作レバー装置９ａ，９ｂから出力される操作信号に基づいて制御信号を生成し、その制御信号を電磁比例弁１０ａ，１０ｂ・・・に出力することで、電磁比例弁１０ａ，１０ｂ・・・から駆動信号（パイロット圧）をコントロールバルブ８内のそれぞれのスプールに出力し、各油圧アクチュエータ２ａ〜６ａの動作が制御される。

なお、操作レバー装置９ａ，９ｂは油圧パイロット方式であってもよい。この場合、操作レバー装置９ａ，９ｂは、それぞれ操作レバーの操作方向及び操作量に応じたパイロット圧を生成し、このパイロット圧がコントロールバルブ８に、直接、駆動信号として供給され、各油圧アクチュエータ２ａ〜６ａの動作が制御される。

上部旋回体２には、慣性計測装置（IMU: Inertial Measurement Unit）１３が配置されている。

慣性計測装置１３は、上部旋回体２の角速度及び加速度を計測するものであり、本発明において、慣性計測装置１３は、上部旋回体の角速度を取得する角速度取得装置を構成する。

慣性計測装置１３により計測される上部旋回体２の角速度は、上部旋回体２の前後方向の角速度と、左右方向の角速度と、旋回方向の角速度を含む。

また、計測した上部旋回体２の前後方向の角速度、左右方向の角速度、旋回方向の角速度をそれぞれ時間で積分することにより、上部旋回体２の前後方向の傾き（ピッチ角）と、左右方向の傾き（ロール角）と、旋回方向の傾き（ヨー角）をそれぞれ計測することができる。すなわち、本発明において、慣性計測装置１３は、角速度取得装置に加え、上部旋回体２の姿勢角を取得する車体姿勢角取得装置を構成する。

なお、姿勢角の取得に加速度を利用した方法を利用しても良い。具体的には、慣性計測装置１３が配置された上部旋回体２が静止している場合を考えると、慣性計測装置１３に設定された車体座標系における鉛直方向（つまり，重力加速度が作用する方向）と、慣性計測装置１３が実際に検出した加速度を比較することで、上部旋回体２の前後方向への傾き（ピッチ角）、左右方向への傾き（ロール角）をそれぞれ検出することができる。

図１において、Σmは車体座標系を示している。車体座標系Σmは上部旋回体２に設定された３次元の直交座標系である。その一例として、車体座標系Σmは、上部旋回体２の旋回中心の軸線Ｓ（図２参照）と上部旋回体２のベースフレーム（旋回フレーム）上面との交点を原点とし、旋回中心の軸線Ｓをｚ軸（図２参照）、ｚ軸に直交する前後方向の軸線をｘ軸、ｚ軸とｘ軸に直交する左右方向の軸線をｙ軸とした直交座標系である。

上述した上部旋回体２の前後方向の角速度は車体座標系Σmのｙ軸回りの角速度であり、左右方向の角速度は同ｘ軸回りの角速度であり、旋回方向の角速度は同ｚ軸回りの角速度である。また、上部旋回体２の前後方向の傾きは車体座標系Σmのｙ軸回りの傾き（ピッチ角）であり、左右方向の傾きは同ｘ軸回りの傾き（ロール角）であり、旋回方向の傾きは同ｚ軸回りの傾き（ヨー角）である。

なお、本実施形態では、これらの傾き（角度）の演算機能が慣性計測装置に実装されているセンサ利用を想定しているが、慣性計測装置１３に角度の算出機能が内蔵されていない場合には、コントローラ２０内に角度演算機能を用意すれば、本発明を実現できることは言うまでもない。

上部旋回体２には２本のGNSSアンテナ１７ａ，１７ｂが取り付けられている。GNSSアンテナ１７ａ，１７ｂのそれぞれで受信した衛星信号はGNSS受信機１７ｃ（図２及び図３等参照）において、GNSSアンテナ１７ａ，１７ｂの３次元座標の算出や上部旋回体２の方位角の算出などの各種測位演算が実行される。GNSS受信機１７ｃにおいて、GNSSアンテナ１７ａ，１７ｂの３次元座標はグローバル座標系の座標値として演算される。また、本実施形態では、２本のGNSSアンテナ１７ａ，１７ｂのうち１つをメインアンテナ、もう１つをサブアンテナとして用い、GNSS受信機１７ｃはこれらメイン、サブアンテナ間でRTK（Real Time Kinematic）測位を実施する。このため、施工現場に設置された基準局と無線通信で接続しなくても、或いは基準局との無線通信の可否によらず、上部旋回体２の方位角についてはRTK測位が実施できる。一方、３次元座標の位置情報については、施工現場に設置された基準局と無線通信で接続することで基準局から送信される補正情報、或いはネットワーク経由で配信される補正情報を利用したRTK測位を実施し、位置情報を取得する。

以下において、説明を簡略化するために、GNSSアンテナ１７ａ，１７ｂとGNSS受信機１７ｃをまとめてGNSSシステム１７と呼ぶことがある。

フロント作業機１の各構成要素であるブーム４、アーム５、バケット６には、それぞれの姿勢（姿勢角）を計測するために慣性計測装置１４，１５，１６が適切な位置に設置されている。慣性計測装置１４〜１６は、本発明において、フロント作業機１の姿勢角を取得するフロント姿勢角取得装置を構成する。

以下において、慣性計測装置１３，１４，１５，１６をそれぞれ区別するため、設置場所に応じて、適宜、車体IMU１３、ブームIMU１４、アームIMU１５、バケットIMU１６と呼ぶ。本発明において、車体IMU１３は、上部旋回体２に取り付けられた少なくとも１つの車体傾斜センサを構成し、ブームIMU１４はフロント作業機１に取り付けられた少なくとも１つのフロント傾斜センサを構成する。

図２は、本発明の第１の実施形態における油圧ショベル１００に搭載されるコントローラ２０の処理機能の一部を示すブロック図である。

図２において、コントローラ２０は、油圧ショベル１００の動作を制御するための種々の機能を有しており、その一部として、位置姿勢演算部２０ａ、施工目標面演算部２０ｂ、モニタ表示制御部２０ｃ、及び油圧システム制御部２０ｄの各機能を有している。位置姿勢演算部２０ａ、施工目標面演算部２０ｂ、モニタ表示制御部２０ｃ、及び油圧システム制御部２０ｄはＣＰＵによって構成されている。

また、コントローラ２０はデータベースとしての記憶装置２０ｅと、演算途中の値を記憶する記憶装置（例えばＲＡＭ）２０ｆと、プログラムや設定値等を記憶する図示しない記憶装置（例えばＲＯＭ）を有している。

位置姿勢演算部２０ａは、慣性計測装置１３〜１６からの検出結果（上部旋回体２の角速度及び姿勢角（ピッチ角）とフロント作業機１の姿勢角）と、GNSS受信機１７ｃからの演算結果（GNSSアンテナ１７ａ，１７ｂの３次元座標及び上部旋回体２の方位角）を入力し、それらの入力値に基づいて、例えば上部旋回体２に設定された車体座標系Σmにおけるフロント作業機１の３次元の位置と姿勢を演算する位置姿勢演算処理を行う。

施工目標面演算部２０ｂは、施工管理者によって記憶装置２０ｅに予め記憶されている３次元施工図面などの施工情報２１と、位置姿勢演算部２０ａで演算されたフロント作業機１の３次元の位置と姿勢とに基づいて、施工対象の目標形状を定義する施工目標面を演算する。

モニタ表示制御部２０ｃは、キャビン９内に設けられたモニタ１１の表示を制御するものであり、施工目標面演算部２０ｂで演算された施工目標面と、位置姿勢演算部２０ａで演算されたフロント作業機１の位置と姿勢とに基づいて、オペレータに対する操作支援の指示内容を演算し、キャビン９内のモニタ１１に表示する。すなわち、モニタ表示制御部２０ｃは、例えば、ブーム４、アーム５、バケット６などの被駆動部材を有するフロント作業機１の姿勢や、バケット６の先端位置と角度をモニタ１１に表示してオペレータの操作を支援するマシンガイダンスシステムとしての機能の一部を担っている。

油圧システム制御部２０ｄは、油圧ポンプ装置７やコントロールバルブ８、電磁比例弁１０ａ，１０ｂ・・・、油圧アクチュエータ２ａ〜６ａ等からなる油圧ショベル１００の油圧システムを制御するものであり、操作レバー装置９ａ，９ｂから出力される操作信号（電気信号）に基づいてフロント作業機１の被駆動部材の目標動作速度を演算し、その目標動作速度を実現するように油圧ショベル１００の油圧システムを制御する。また、油圧システム制御部２０ｄは、施工目標面演算部２０ｂで演算された施工目標面と、位置姿勢演算部２０ａで演算されたフロント作業機１の位置と姿勢とに基づいて、フロント作業機１の被駆動部材の目標動作速度を演算し、その目標動作速度を実現するように操作レバー装置９ａ，９ｂから出力された対応する操作信号を補正し、油圧ショベル１００の油圧システムを制御する。これにより油圧システム制御部２０ｄは、例えば、バケット６などの作業具の先端が目標施工面に一定以上近づかないように動作に制限をかけたり、作業具（例えば、バケット６の爪先）が目標施工面に沿って動くよう制御したりするマシンコントロールシステムとしての機能の一部を担っている。

このようにコントローラ２０は、GNSS受信機１７ｃで算出されたGNSSアンテナ１７ａ，１７ｂの３次元座標及び上部旋回体２の方位角と、車体IMU１３（車体姿勢角取得装置）で取得した上部旋回体２の姿勢角、車体IMU１３（角速度取得装置）で取得した上部旋回体２の角速度、及びブームIMU１４、アームIMU１５及びバケットIMU１６（フロント姿勢角取得装置）で取得したフロント作業機１の姿勢角に基づいてフロント作業機１の位置及び姿勢を演算し、フロント作業機１の位置及び姿勢に基づいてフロント作業機１の動作を制御する。

また、コントローラ２０は、本発明の特徴として、位置姿勢演算部２０ａにおいて以下の演算処理を行う。

まずコントローラ２０は、位置姿勢演算部２０ａにおいて、車体IMU１３（車体姿勢角取得装置）で取得した上部旋回体２の姿勢角とブームIMU１４（フロント姿勢角取得装置）で取得したフロント作業機１の姿勢角の少なくとも一方に基づいてGNSS受信機１７ｃで算出された上部旋回体２の方位角の品質を判断し、方位角の品質の判断結果と、GNSS受信機１７ｃで算出された上部旋回体２の方位角とに基づいて、車体IMU１３（角速度取得装置）で取得した上部旋回体２の角速度からジャイロバイアスを除去するバイアス除去演算を実行し、かつジャイロバイアスを除去した上部旋回体２の角速度に基づいて上部旋回体２の旋回動作の有無を判断し、GNSS受信機１７ｃで算出された上部旋回体２の方位角と、ジャイロバイアスを除去した上部旋回体２の角速度と、上部旋回体２の旋回動作の判断結果に基づいて上部旋回体２の修正方位角を算出し、この修正方位角を用いてフロント作業機１の位置及び姿勢を演算する。

また、コントローラ２０は、車体IMU１３（車体傾斜センサ）で検出した上部旋回体２の姿勢角とブームIMU１４（フロント傾斜センサ）で検出したフロント作業機１の姿勢角の少なくとも一方に基づいてGNSS受信機１７ｃで算出された上部旋回体２の方位角の品質を判断し、上部旋回体２の方位角の品質が低下していると判断したときにバイアス除去演算を実行する。なお、本明細書において「上部旋回体２の方位角の品質が低下している」とは、位置姿勢演算部２０ａ（後述する３次元位置姿勢演算部２０ａ−２）がGNSS受信機１７ｃで算出された上部旋回体２の方位角を用いてフロント作業機１の３次元の位置及び姿勢を演算するとき、その演算精度が油圧システム制御部２０ｄにおいてフロント作業機１の適切な動作制御を確保できなくなる程度に上部旋回体２の方位角の品質が劣化することを意味する。また、以下の説明では、「上部旋回体２の方位角の品質が低下している」を「上部旋回体２の方位角の品質が低い」、「上部旋回体２の方位角の品質が不良である」などとも言い、そうでない場合を「上部旋回体２の方位角の品質が高い」、「上部旋回体２の方位角の品質が良好である」などとも言う。

また、コントローラ２０は、位置姿勢演算部２０ａにおいて、上部旋回体２の方位角の品質が高いと判断したときの方位角を記憶する記憶装置２０ｆ（方位角記憶装置）を備え、上部旋回体２の方位角の品質が低いと判断し、上部旋回体２の旋回動作無しを判断したときは、上部旋回体２の方位角の品質が低いと判断する直前に記憶装置２０ｆ（方位角記憶装置）に記憶した方位角を修正方位角として用い、フロント作業機１の位置及び姿勢を演算する。

更に、コントローラ２０は、位置姿勢演算部２０ａにおいて、上部旋回体２の方位角の品質が低いと判断し、かつ上部旋回体２の旋回動作有りを判断した後、上部旋回体２の方位角の品質が低いまま所定の時間を経過したときは、上部旋回体２の方位角の精度が低い可能性があることをユーザに通知する。

以下に、上述した位置姿勢演算部２０ａの処理内容の詳細を図３に従って具体的に説明する。図３は、本実施形態の特徴部分であるコントローラ２０の位置姿勢演算部２０ａの処理機能を示すブロック図である。

図３において、位置姿勢演算部２０ａは、修正方位角演算部２０ａ−１と３次元位置姿勢演算部２０ａ−２を有している。

修正方位角演算部２０ａ−１には、車体IMU１３、ブームIMU１４、GNSSシステム１７で計測、もしくは、演算した値（信号）が入力される。より具体的には、GNSS受信機１７ｃで算出した上部旋回体２の方位角、車体IMU１３で計測した上部旋回体２の角速度と姿勢角、ブームIMU１４で算出したブーム４の姿勢角が入力される。

修正方位角演算部２０ａ−１はGNSS受信機１７ｃが算出した上部旋回体２の方位角に後述する補正演算を実施して、修正方位角を算出する。また、修正方位角が十分な精度で演算できない場合には、その状態をオペレータに通知するための低精度警告のフラグを算出する。そして、低精度警告フラグが有効になった場合は、モニタ表示制御部２０ｃを介して、モニタ１１上にメッセージを出すなどをしてオペレータに通知を行う。

３次元位置姿勢演算部２０ａ−２には、IMU１３〜１６で検出した姿勢角、GNSS受信機１７ｃで算出したGNSSアンテナ１７ａ，１７ｂの３次元座標、修正方位角演算部２０ａ−１で算出した修正方位角が入力される。

３次元位置姿勢演算部２０ａ−２は、GNSSシステム１７で取得したGNSSアンテナ１７ａ，１７ｂの３次元座標に基づいて、例えば車体座標系Σmにおける油圧ショベル１００の位置と姿勢を演算する。

３次元位置姿勢演算部２０ａ−２は、車体座標系Σmにおける油圧ショベル１００の位置と姿勢と、IMU１３〜１６で検出した姿勢角と、修正方位角演算部２０ａ−１で算出した修正方位角に基づいて、例えば車体座標系Σmにおけるフロント作業機１の３次元の位置及び姿勢を演算し、前述したように、この演算値が施工目標面演算部２０ｂ、モニタ表示制御部２０ｃ及び油圧システム制御部２０ｄに送られる。

３次元位置姿勢演算部２０ａ−２は、車体座標系Σmに代えて油圧ショベル１００の周囲に設定された現場座標系におけるフロント作業機１の３次元の位置及び姿勢を算出してもよいし、車体座標系Σmで算出したフロント作業機１の３次元の位置及び姿勢を現場座標系に変換して制御で用いてもよい。

本実施形態におけるGNSSシステム１７は、前述したとおり、２本のGNSSアンテナ１７ａ，１７ｂ間でRTK測位を実施することで、上部旋回体２の方位角を取得する。このため、油圧ショベル１００の姿勢変化が方位角の品質に影響する（後述）。

一方、３次元座標の位置情報については、施工現場に設置された基準局と無線通信で接続することで基準局から送信される補正情報、或いはネットワーク経由で配信される補正情報を利用したRTK測位を実施する。このため、油圧ショベル１００の姿勢変化が３次元位置情報に及ぼす影響は方位角に比して非常に小さい。また、３次元姿勢演算部２０ａ−２で利用される３次元座標は、２本のGNSSアンテナ１７ａ，１７ｂのうち、後述する図１５Ｂ及び図１５Ｃにおいて、DOP（(Dilution Of Precision)の悪化率が低いGNSSアンテナ１７ａ又は１７ｂを起点とすればよい。つまり、図１５Ｂ及び図１５Ｃのように、天頂方向の一部が隠された状態で、衛星の配置が特定の場所に偏らない側のGNSSアンテナを位置演算に利用すればよい。したがって、油圧ショベル１００の姿勢変化でGNSSシステム１７が取得する方位角の品質が低下した場合でも、GNSSシステム１７のRTK測位で取得した３次元座標の位置情報の精度の低下は少なく、３次元位置姿勢演算部２０ａ−２においてその３次元の位置情報を利用可能である。

このような考えに基づき本発明は、３次元姿勢演算部２０ａ−２において、油圧ショベル１００の上部旋回体２の方位角に、修正方位角演算部２０ａ−１で算出した修正方位角を用い、かつ、測位精度の低下が少ないGNSSアンテナ１７ａ，１７ｂの３次元座標を用いてフロント作業機１の３次元の位置及び姿勢を演算するものである。３次元姿勢演算部２０ａ−２におけるそれ以外の演算は、従来と特段に変わる点はないため、詳細な説明は省略する。

以下に、本発明の特徴となる修正方位角演算部２０ａ−１について、図４を用いて詳細に説明する。図４は、修正方位角演算部２０ａ−１の処理機能を示すブロック図である。

図４において、修正方位角演算部２０ａ−１は、方位角品質判断部MH01、バイアス除去実行部MH02、旋回動作判断部MH03、方位角統合演算部MH04の４つの演算機能を有している。

＜方位角品質判断部MH01＞
方位角品質判断部MH01は、油圧ショベル１００の姿勢情報を利用して、GNSSシステム１７が算出した上部旋回体２の方位角の品質を判断する。本実施形態では、油圧ショベル１００の姿勢情報として、車体IMU１３とブームIMU１４で取得した姿勢角情報を用いる。

ここで、まず、GNSSシステム１７が算出した上部旋回体２の方位角の品質を判断する必要性を説明する。

油圧ショベル１００のフロント作業機１は金属部材で構成されるため、GNSSの衛星信号がフロント作業機１によって反射、もしくは、遮断されることが考えられる。

また、油圧ショベル１００の掘削動作においては、フロント作業機１が上下動作を繰り返すため、衛星信号の反射や途絶が繰り返し発生するような環境になる。

このような使用環境において、GNSSアンテナ１７ａ，１７ｂを用いてGNSS測位を行う場合の課題を図１３〜図１６を用いて説明する。

図１３は、油圧ショベル１００の上面図である。図１４Ａ〜図１４Ｃは、フロント作業機１が下がっている状態での衛星信号の受信状況を示す図である。図１５Ａ〜図１５Ｄは、フロント作業機１が上がっている状態での衛星信号の受信状況を示す図である。図１６は、油圧ショベル１００が傾斜面に配置されている状態での衛星信号の受信状況を示す図である。

図１３に示すように、GNSSアンテナ１７ａ，１７ｂがフロント作業機１の中心線１ａの左右に配置されている。

図１４Ａに示すように、ブーム４が下げ姿勢にあり、フロント作業機１が下がっている状態では、GNSSアンテナ１７ａ，１７ｂよりもフロント作業機１は低い位置にあるため、GNSSアンテナ１７ａ，１７ｂの天頂方向の衛星信号受信可能領域２００に対する遮蔽物のイメージは図１４Ｂ及び図１４Ｃのようになる。図１４Ｂは図１３において左側に配置されたGNSSアンテナ１７ａ，図１４Ｃは図１３において右側に配置されたGNSSアンテナ１７ｂの上面図に対応する。図１４Ｂ及び図１４Ｃに示すように、それぞれのGNSSアンテナ１７ａ，１７ｂの上空に遮蔽物が一切ないため、衛星信号の遮断や反射は起きにくい状況である。

一方、図１５Ａに示すように、ブーム４が上げ姿勢にあり、フロント作業機１が上がっている状態では、GNSSアンテナ１７ａ，１７ｂよりもフロント作業機１が高い位置にある。このような状況では、GNSSアンテナ１７ａ，１７ｂの天頂方向の衛星信号受信可能領域２００がフロント作業機１により部分的に遮られ、フロント作業機１が存在する方向の衛星信号をGNSSアンテナ１７ａ，１７ｂが受信できなくなる。さらに、GNSSアンテナ１７ａ，１７ｂはフロント作業機１に対して左右に配置されるため、遮断される領域は図１５Ｂ及び図１５Ｃに示すように対称的な関係になることに注意されたい。図１５Ｂは図１３において左側に配置されたGNSSアンテナ１７ａ、図１５Ｃは図１３において右側に配置されたGNSSアンテナ１７ｂの上面図に対応する。

また、図１５Ｂ及び１５Ｃにおいて黒で塗りつぶしている箇所はある１時刻においてフロント作業機１が衛星信号を遮断する領域を示しており、灰色で塗りつぶしている箇所はフロント作業機１の連続的な姿勢変化によって衛星信号が遮蔽される領域を模式的に示したものである。

さて、上述の通り、２本のGNSSアンテナ１７ａ，１７ｂを用いて上部旋回体２の方位角を算出する場合は、GNSSアンテナ１７ａ，１７ｂの一方をメインアンテナ、他方をサブアンテナとしたRTK測位を実施する。このとき、２本のGNSSアンテナ１７ａ，１７ｂが共通して利用できる衛星のみが測位に利用されるため、図１５Ｄに示すように、それぞれのGNSSアンテナ１７ａ，１７ｂでの遮断領域を組み合わせた領域において、衛星信号が遮断される状況になる。つまり、フロント作業機１を上げるだけで、天頂方向の半分の衛星信号が測位に利用できなくなってしまうおそれがある。このような状況では衛星信号を利用できる衛星配置が著しく偏るため、方位角の演算精度が低下する。

また、油圧ショベル１００ではフロント作業機１の上げ下げを頻繁に行うため、上部旋回体２を旋回していないのにも係わらず、GNSS測位によって算出された方位角がばらついてしまう。この算出結果をマシンガイダンスに利用すると、上部旋回体２がふらふらと動作しているように見えてしまう。

更に、図１６に示すように、傾斜地に油圧ショベル１００が配置されている場合など、油圧ショベル１００の車体（上部旋回体２）が前後方向に傾きを持っている場合にも、ブーム下げ姿勢であっても、天頂方向が遮蔽される状況を生じ得る。このような状況でも衛星信号を利用できる衛星配置が偏るため、方位角の演算精度が低下する。

図４に戻り、方位角品質判断部MH01における方位角品質の判断条件を説明する。

図１４Ａに示したように、ブーム４が下げ姿勢にあるときは、GNSSアンテナ１７ａ，１７ｂの上空部に障害物が存在しないため、GNSSシステム１７が演算した上部旋回体２の方位角の品質は良いと判断する。一方、図１５Ａに示したように、ブーム４が上げ姿勢にあるときは、特定方向の衛星信号が遮断されるため、GNSSシステム１７が演算した上部旋回体２の方位角の品質は劣っていると判断する。方位角品質判断部MH01は、このような考え方に基づいて方位角品質を判断する。すなわち、ブームIMU１４で取得したブーム角をフロント作業機１の姿勢角として入力し、ブーム角（姿勢角）が所定値以上であるときは、上部旋回体２の方位角の品質は良好であり、所定値より小さければ方位角の品質は劣っていると判断する。ここで、所定値は、例えば、水平面を基準にして30degである。

また、図１６に示したように、傾斜地に油圧ショベル１００が配置されている場合にも、図１４Ａの状況に比べて、GNSSシステム１７が演算した方位角の品質は劣ると判断する。したがって、この場合も、方位角品質判断部MH01は、車体IMU１３で取得した車体傾斜角（上部旋回体２の前後方向（ｙ軸回り）の傾きであるピッチ角）を車体姿勢角として入力し、車体姿勢角（ピッチ角）が所定値以上であるときは、上部旋回体２の方位角の品質は良好であり、所定値より小さければ方位角の品質は劣っていると判断する。ここで、所定値は、例えば、水平面を基準にして15degである。

上記ブーム角と車体傾斜角の所定値は、上部旋回体２の方位角の品質を判断するための判定値として、コントローラ２０の記憶装置（例えばＲＯＭ）に記憶されている。

図５Ａは、方位角品質判断部MH01における品質判断処理の他の例を示す図である。

方位角品質判断部MH01は図５Ａに示すような方位角品質演算部MH01-Aを有していていもよい。方位角品質演算部MH01-Aは、上述した判断条件に基づいて、ブーム角度と車体傾斜角度（車体姿勢角、すなわち上部旋回体２の前後方向（ｙ軸回り）の傾きであるピッチ角）を入力とし、方位角品質を出力とする３次元テーブルとして構成されている。この３次元テーブルにおいて、方位角品質は０から１までの数値で表されており、方位角品質が低くなるほど方位角品質の数値は大きくなるように設定されている。すなわち、数値が０のときが最も方位角品質が良く、１のときが最も方位角品質が悪いことを意味する。

方位角品質演算部MH01-Aの３次元テーブルは以下のように構成されている。

まず、３次元テーブルは、例えば、ブーム角度が最小値（ブームが下がっている）であっても、車体傾斜角（ピッチ角）が大きければ方位角品質は１になるように設定されている。また、３次元テーブルは、車体傾斜角度（ピッチ角）が最小値（車体が水平を保っているとき）であっても、ブーム角度が大きければ（ブームが上がっていれば）、方位角品質は１になるように設定されている。更に、３次元テーブルは、例えば、車体傾斜角度（ピッチ角）が０度でブーム角度が１０度のときに算出される方位角品質の値（Ａ）よりも、車体傾斜角度（ピッチ角）が１０度でブーム角度が１０度のときに算出される方位角品質の値（Ｂ）の方が大きくなる（Ａ＜Ｂ）ように設定されている。

図５Ｂは、方位角品質判断部MH01における品質判断処理の更に他の一例を示す図である。

方位角品質判断部MH01は図５Bに示すような方位角品質演算部MH01-Bを有していてもよい。方位角品質演算部MH01-Bは、図５Ｂに示すように、ブーム角度が所定位置以上であるときに方位角品質を１となる２次元テーブルＴ１と、車体傾斜角度（ピッチ角）が所定値以上であるときに方位角品質が１となる２次元テーブルＴ２を有し、これらの２つの２次元テーブルＴ１，Ｔ２で算出された方位角品質の値を加算して、加算後の値を１で飽和させることで、方位角品質を算出する。

方位角品質判断部MH01は、方位角品質演算部MH01-A又はBMH01-Bにおいて算出された方位角品質を用い、その方位角品質が所定値、例えば０．８以上であれば方位角品質は良好であり、所定値より小さければ方位角品質は劣っていると判断する。

＜バイアス除去実行部MH02＞
バイアス除去実行部MH02は、車体IMU１３が検出した角速度（旋回方向（ｚ軸回り）の角速度）のバイアス、いわゆる、ジャイロバイアスの除去を実施する。

ジャイロバイアスの除去は、カルマンフィルタ（KF：Kalman Filter）やオブザーバを適用する手法が一般的である。本発明の実施形態では、カルマンフィルタを利用したジャイロバイアスの除去方法についてのみを説明する。カルマンフィルタの詳細については、例えば、「足立、丸田：カルマンフィルタの基礎、東京電機大学出版局、2012」を参照されたい。

ここで、上部旋回体２の方位角をθ、実際の上部旋回体の角速度をω、車体IMU１３が検出した角速度をω^mes、ジャイロバイアスをω^b、コントローラ２０の演算周期をΔtとすると、次の式(１）、(2)が成立する。

なお、下付きk，k-1は各演算における時刻を示す記号である。式(1)，(2)について、状態ベクトルxkを式(3)のように定義する。

さらに、GNSS受信機１７ｃが算出した方位角θを観測値ykとすると、次の式(4)，(5)が導出できる。

式(4)，(5)にKFを適用するため、式(4)，(5)の各行列を簡略化した表現にする。

ここで、新たに追加されたw_k-1，v_kはそれぞれプロセス雑音、観測雑音である。プロセス雑音、観測雑音ともに平均値が0の正規分布に従う雑音であり、w_kの分散はQ_k，v_kの分散はR_kである。これらQ_k，R_kともに設計値として扱われる。

状態ベクトルxの推定値をx^∧（^∧はxの上に付く）、状態ベクトルxの共分散行列をPとすると、カルマンフィルタの予測式(a3)〜(a6)、および、更新式(a7)〜(a9)を次式で与えることができる。

以上の演算により、方位角θkとジャイロバイアスω^bから構成される状態ベクトルx^∧（^∧はxの上に付く）を逐次推定することができる。

ただし、方位角品質判断部MH01で算出される方位角品質が低い場合には、観測値ykである方位角が真の方位角に対してバラつきをもっているため、ジャイロバイアスω^bの推定値に誤差を生じる可能性が高い。この現象を解決するためには、方位角品質が高い場合にのみ、ジャイロバイアスの推定が有効になるようにロジックを構築することが望ましい。

例えば、カルマンフィルタを用いる場合、方位角品質が低い場合には観測雑音の分散Rを非常に大きな値で設定する。観測雑音の分散Rを大きな値にすると、式(a5)のPxyが大きな値を取るようになる。よって、式(a7)の逆行列が0に近づくことで、カルマンゲインも0に近づく。

カルマンゲインKが0になったとき、カルマンフィルタによる更新式(a8)，(a9)は次のように簡略化することができる。

式(a10)、(a11)は予測式(a3)，(a4)がそのまま出力される式に他ならない。さらに、式(a3)は式(3)，(4)より

になるため、ジャイロバイアスの推定値の更新が停止することになる。

バイアス除去実行部MH02は、車体IMU１３が算出した角速度ωmesから、上述の演算で算出されたジャイロバイアスωbの推定値（上記式(a12)の値）を引いた値を、ジャイロバイアスを除去した角速度ωとして出力する。

＜旋回動作判断部MH03＞
旋回動作実行判断部MH03は、バイアス除去実行部MH02で算出したジャイロバイアスを除去した角速度ωを用いて、上部旋回体２の旋回動作の有無を判断する。具体的には角速度ωの絶対値が所定値（例えば、0.5deg/s）以上のときに上部旋回体２が旋回していると判断する。この所定値は、車体IMU１３の性能に応じて設計する必要があることは言うまでもない。

なお、角速度ωの絶対値を取る理由は、上部旋回体２は左回り（＋の角速度）、右回り（−の角速度）の回転を自在に行えるためである。

＜方位角統合演算部MH04＞
方位角演算統合演算部MH04は、方位角品質判断部MH01、バイアス除去演算実行部MH02、旋回動作実行判断部MH03のそれぞれの演算結果と、GNSSシステム１７から出力された上部旋回体２の方位角を用いて、修正方位角の算出を行う。具体的な処理内容を以下に示す。

まず、方位角品質判断部MH01で演算された方位角品質が良好である場合、旋回動作実行判断部MH03における旋回動作の有無によらず、GNSS受信機１７ｃからの算出値である上部旋回体２の方位角θをそのまま修正方位角θmとして出力する。つまり、θm＝θとなる。

ただし、GNSS受信機１７ｃのサンプリング周期が車体IMU１３のサンプリング周期よりも遅い場合には、各サンプリング周期間の方位角を、バイアス除去演算実行部MH02で算出した角速度ωによって補間してもよい。例えば、コントローラ２０の演算周期Δｔに従って、GNSSシステム１７が出力した方位角θに「Δt×ω」を逐次加算すればよい。

以下に、方位角品質判断部MH01で演算された方位角品質が不良である場合の方位角統合演算部MH04の演算内容を説明する。

まず、旋回動作実行判断部MH03で旋回動作が無いと判断された場合、方位角統合演算部MH04は、方位角品質が不良と判断される直前の方位角θ0を修正方位角θmとして出力する。

これは、旋回動作が実施されない限り、方位角に変化が起きないという事実を利用するものである。なお、コントローラ２０は、GNSS受信機１７ｃからの算出値である上部旋回体２の方位角θを、逐次、記憶装置２０ｆ（図２参照）に記憶し、方位角品質が不良と判断されたとき、その直前に記憶装置２０ｆに記憶された方位角θを方位角θ0として読み込むことで、方位角品質が不良と判断される直前の方位角θ0を修正方位角θmとして出力することができる。

次に、旋回動作実行判断部MH03で旋回動作があると判断された場合であって、かつ、方位角品質が不良と判断されてからの時間ｔが所定値Tmax未満のとき、方位角統合演算部MH04は、方位角品質が不良と判断される直前の方位角θ0を基準値とし、この方位角θ0にジャイロバイアス除去実行部MH02の出力ωの積算値を加算した値を、修正方位角θmとして出力する。つまり修正方位角θmは次式で与えられる。

さらに、旋回動作実行判断部MH03で旋回動作があると判断された場合であって、かつ、方位角品質が不良と判断されてからの時間ｔが所定値Tmax以上のとき、方位角統合演算部MH04は低精度警告フラグを有効にする。この状態は、バイアス除去実行部MH02で算出した角速度の積分値では、十分な精度の方位角を算出できないことを意味する。このため、修正方位角θmは式(7)の値で停止することが望ましい。なお、整数Nは所定時間までのサンプル数であり、「Tmax÷Δt」である。

これまでに説明した修正方位角演算部２０ａ−１の演算手順を図６のフローチャートに従って説明する。

ステップFC01にて、コントローラ２０の演算周期ごとに各種センサ（IMU１３〜１６、GNSSシステム１７）から情報取得を行う。

ステップFC02では、IMU１３〜１６から取得した各姿勢角が方位角品質を低下させ得る値になっていないかの確認を行う。例えば、ブーム角が所定値（例えば、水平面を基準にして30degなど）以上である場合は、Yes判定がなされステップFC03に遷移する。ステップFC03に遷移すると、方位角品質が良好であることが記憶装置２０ｆ（図２参照）に記憶される。一方で、No判定がなされた場合には、ステップFC04に遷移し、方位角品質が不良であると記憶装置２０ｆに記憶される。

なお、ステップFC02〜FC04は方位角品質判断部MH01の演算内容であるため、判断部を介さずに、図５のようにテーブル参照としてもよい。

ステップFC05では、ステップFC03、FC04で記憶装置２０ｆに記憶した方位角品質を参照し、この値が良好（Yes）である場合はステップFC06、不良（No）である場合にはステップFC07に遷移を行う判断をする。

ステップFC06に遷移した場合、GNSSシステム１７から取得した方位角の信頼性が高いため、観測雑音の分散Rを小さな値にする。一方、ステップFC07に遷移した場合、GNSSシステム１７から取得した方位角の信頼性が低いため、観測雑音の分散Rを大きな値にする。

ステップFC08では、ステップFC06、FC07で決定した分散Rを利用したカルマンフィルタ(KF)の演算を行い、ジャイロバイアスを推定する。

ステップFC08の処理が完了すると、ステップFC21に遷移する。ステップFC21では、車体IMU１３が取得した角速度ωmesからステップFC08で推定したジャイロバイアスωbを減算することでジャイロバイアス除去操作を実施する。

以上のステップFC05〜FC08の一連の演算は、ジャイロバイアス除去実行部MH02に相当する。

ステップFC09では、ジャイロバイアス除去実行部MH02で算出した角速度ωと所定値を比較する。角速度ωの絶対値が所定値未満（Yes）であれば、ステップFC10に遷移し、旋回動作が無いと判断する。一方、角速度ωの絶対値が所定値以上（No）であれば、ステップFC11に遷移し、旋回動作があると判断する。

以上のステップFC09〜FC11の一連の演算は、旋回動作実行判断部MH03に相当する。

ステップFC12では、再び、記憶装置２０ｆに記憶した方位角品質を参照し、方位角品質が良好（Yes）である場合はステップFC13へ、不良（No）である場合はステップFC15へそれぞれ遷移する。

ステップFC13に遷移した場合、方位角品質が良好であるので、修正方位角θmはGNSSシステム１７が出力した方位角θを修正方位角θmとして出力する。その後、ステップFC14に遷移し、低精度警告に関するフラグを無効にして、演算を終了する。

ステップFC15に遷移した場合、ステップFC10、FC11で決定した旋回動作の有無を参照し、旋回動作がない場合（Yesの場合）はステップFC16に遷移し、一方、旋回動作がある場合（Noの場合）にはステップFC17に遷移する。

ステップFC16に遷移した場合、方位角品質が不良であっても、旋回動作が実施されていないため、方位角品質が不良になる直前の方位角θ0を修正方位角θmとして出力する。その後、ステップFC14に遷移し、低精度警告に関するフラグを無効にして、演算を終了する。

ステップFC17に遷移した場合、方位角品質が不良になってからの経過時間tが所定時間Tmax未満であるかを確認する。経過時間tが所定時間未満（Yes）である場合はステップFC18に遷移する。一方、経過時間が所定値以上（No）である場合はステップFC19へと遷移する。

ステップFC18に遷移した場合、修正方位角θmは式(6)によって算出される。その後、ステップFC14に遷移し、低精度警告に関するフラグを無効にして、演算を終了する。

ステップFC19に遷移した場合、修正方位角θmは式(7)によって算出される。その後、ステップFC20に遷移し、低精度警告に関するフラグを有効にして、演算を終了する。

以上のように本実施の形態においては、コントローラ２０は、位置姿勢演算部２０ａにおいて、車体IMU１３（車体姿勢角取得装置）で取得した上部旋回体２の姿勢角とブームIMU１４（フロント姿勢角取得装置）で取得したフロント作業機１の姿勢角の少なくとも一方が所定値以上であるときにGNSS受信機１７ｃで算出された上部旋回体２の方位角の品質が低いと判断し、方位角の品質と、GNSS受信機１７ｃで算出された上部旋回体２の方位角とに基づいて、車体IMU１３（角速度取得装置）で取得した上部旋回体２の角速度からジャイロバイアスを除去するバイアス除去演算を実行し、かつジャイロバイアスを除去した上部旋回体２の角速度に基づいて上部旋回体２の旋回動作の有無を判断し、GNSS受信機１７ｃで算出された上部旋回体２の方位角と、ジャイロバイアスを除去した上部旋回体２の角速度と、上部旋回体２の旋回動作の判断結果に基づいて上部旋回体２の修正方位角を算出するため、フロント作業機１の上げ下げなどの油圧ショベル１００の姿勢変化によらずに、GNSSを利用して正確な上部旋回体２の方位角を算出し、上部旋回体２の方位角を高精度、かつ、ロバストに取得することができる。このためマシンガイダンスやマシンコントロールの動作を停止させる頻度を低減し、作業効率を向上することができる。

＜第２の実施の形態＞
第１の実施形態は、油圧ショベル１００に追加して取り付けられたセンサのみを用いる構成であった。

本発明の第２の実施形態は、油圧ショベル１００にもともと取り付けられているセンサを利用することで、演算内容の容易化、もしくは、ロバスト化を達成するものである。

まず、本実施形態の特徴を概略的に説明すれば以下のようである。

本実施の形態において、コントローラ２０は、GNSS受信機１７ｃで算出された上部旋回体２の方位角の品質の判断に際して、ブームIMU１４（フロント姿勢角取得装置）で取得したフロント作業機１の姿勢角のバックアップとしてブーム４の上げ動作（フロント作業機１の動作）を指示する操作レバー装置９ａ（第１操作レバー装置）の操作信号を入力し、この操作レバー装置９ａの操作信号に基づいて、操作レバー装置９ａのフロント作業機１の上げ方向操作が所定時間以上継続したときに、上部旋回体２の方位角の品質が低いと判断する。所定時間は、例えばフロント作業機１がブーム上げ開始時の標準的な姿勢にあるときに操作レバー装置９ａをブーム上げ方向にフル操作し、ブーム４の角度が前述した所定値に達するまでの経過時間であり、上部旋回体２の方位角の品質を判断するための判定値として、コントローラ２０の記憶装置（例えばＲＯＭ）に記憶されている。

また、コントローラ２０は、上部旋回体２の旋回動作の有無の判断に際して、ジャイロバイアスを除去した上部旋回体２の角速度のバックアップとして操作レバー装置９ｂ（第２操作レバー装置）の操作信号を入力し、この操作レバー装置９ｂの操作信号に基づいて、操作レバー装置９ｂの操作信号の大きさが所定値以上であるときに、上部旋回体２の旋回動作が有りと判断する。所定値は、操作信号を発生させたとき、旋回モータ２ａが回転し始める操作信号の値であり、上部旋回体２の旋回動作の有無の判定値として、コントローラ２０の記憶装置（例えばＲＯＭ）に記憶されている。

以下に、上述したコントローラ２０の処理内容の詳細を図７に基づいて説明する。図７は、本実施形態における修正方位角演算部２０ａ−１の処理機能を示すブロック図である。

本実施形態における修正方位角演算部２０ａ−１は、図４に示す方位角品質判断部MH01及び旋回動作実行判断部MH03に、操作レバー装置９ａ，９ｂの操作信号（電気信号）が追加信号として更に入力される。

方位角品質判断部MH01は、上述の車体IMU１３及びブームIMU１４から出力される姿勢角情報を用いた判断に加えて、操作レバー装置９ａから出力されるブーム上げの操作信号を利用することでも、方位角品質を判断する。例えば、操作レバー装置９ａから出力されるブーム上げの操作信号に基づくブーム上げの操作が所定時間続くと、ブームシリンダ４ａが徐々に伸びて、最終的に図１５Ａの姿勢に達する。つまり、ブームIMU１４の姿勢角を取得しなくとも、ブーム上げの操作信号とその経過時間を利用することでも、方位角品質の判断に必要なフロント作業機１の姿勢を推測することができる。

このように、操作レバー装置９ａの操作信号を利用すれば、万が一、故障などでブームIMU１４の信号が利用できない状況が発生した場合でも、方位角品質の判断を継続することができ、システムとしてのロバスト性の向上が実現できる。

また、本実施形態における旋回動作実行判断部MH03は、バイアス除去実行部MH02から出力される角速度ωを用いた判断に加えて、操作レバー装置９ｂから出力される旋回の操作信号を利用することで、旋回動作の有無を判断する。

旋回操作に関する操作信号の値が所定値未満（例えば、パイロット圧換算値で0.5MPa未満）である場合は、旋回モータ２ａに圧油が流入しないため、上部旋回体２は回転しないが、操作信号の値が旋回モータ２ａが回転する操作信号の下限値である所定値以上（例えば、パイロット圧換算値で1.0MPa以上）になると、旋回モータ２ａに圧油が流入して上部旋回体２が回転する。

このため、旋回動作に係る操作信号を確認すれば、車体IMU１３が角速度の変化を検出する前から旋回動作の有無を判断可能になる。このように旋回の操作信号を使った旋回動作の判定は、ジャイロバイアス除去実行部MH02でのジャイロバイアスの除去性能が不十分であり、図６のフローチャートにおけるステップFC09の旋回動作を判断するための角速度の閾値値を十分に下げられない場合に、特に有効である。

なお、第１及び第２の実施形態では、方位角品質判断演算部MH01に、ブームIMU１４の姿勢角のみを入力しているが、本発明はこれに限定されるものではない。フロント作業機１の姿勢をより正確に判断したい場合には、ブームIMU１４、アームIMU１５、バケットIMU１６の全ての姿勢角を入力するように変更すればよい。

また、第２の実施形態では、操作レバー装置９ａ，９ｂが電気レバー方式である場合について説明したが、操作レバー装置９ａ，９ｂが油圧パイロット方式である場合は、それぞれの操作レバーの操作方向及び操作量に応じたパイロット圧を検出する圧力センサを設け、その圧力センサの信号を入力するようにすればよい。

このように本実施の形態によれば、操作レバー装置９ａ，９ｂの操作信号をフロント作業機１の姿勢角或いは上部旋回体２の角速度のバックアップとして用いるため、システムとしてのロバスト性を更に向上させることができ、作業効率を向上することができる。

＜第３の実施の形態＞
第１及び第２の実施形態は、フロント作業機１にIMUに代表される傾斜角検出器を用いるものであったが、本発明は、フロント作業機１に傾斜角検出器を用いなくても実施可能である。

まず、本実施形態の特徴を概略的に説明すれば以下のようである。

本実施の形態において、油圧ショベル１００は、車体IMU13（車体姿勢角取得装置）及びブームIMU１４（フロント姿勢角取得装置）の少なくとも一方の姿勢角取得装置として画像認識装置３５（図８参照）を備え、画像認識装置３５は、少なくとも１台のカメラ３０又は３２又は３３（図８参照）から取得した画像情報に基づいて上部旋回体２の姿勢角とフロント作業機１の姿勢角の少なくとも一方を認識し、コントローラ２０は、画像認識装置３５で認識した上部旋回体２の姿勢角とフロント作業機１の姿勢角の少なくとも一方に基づいてGNSS受信機１７ｃで算出された上部旋回体２の方位角の品質を判断し、上部旋回体２の方位角の品質が低いと判断したときにバイアス除去演算を実行する。

以下、第３の実施形態の詳細を図８〜図１０を用いて説明する。

図８は、本実施形態における油圧ショベル１００の外観とその周囲の状況を示す図である。なお、説明を簡略化するため、図１と同一の構成要素を示す符号は図示を省略している。

本実施形態に関する油圧ショベル１００は、キャビン９の頂部前側に設置されたステレオカメラ３０と、キャビン９の頂部後側に設置された無線通信機３１ａとを備えている。無線通信機３１ａは作業現場内に備えられた別の無線通信機３１ｂと無線通信を行う。無線通信機３１ｂは作業現場内に備えられた管理用カメラ３２が撮影した画像情報を作業現場内に配信する。なお、管理用カメラ３２は作業現場内に固定されたカメラに限定されない。例えば、作業現場内を自由に飛行可能な無人航空機（ドローン）３４にカメラ３３と無線通信機３１ｃを搭載してもよい。

図９は、本実施形態におけるコントローラ２０の処理機能を示すブロック図である。なお、説明を簡単にするため、図３に示された施工目標面演算部２０ｂ及び油圧システム制御部２０ｄは図示を省略している。

本実施形態におけるコントローラ２０の処理機能は、図８に示す油圧ショベルに設置されたステレオカメラ３０及び無線通信機３１ｂの情報が入力されるように変更されている点を除いて、図３に示した第１の実施形態におけるコントローラ２０の処理機能と同じである。コントローラ２０は、その機能の変更部分として、取得した画像情報に基づいてフロント作業機の姿勢情報を算出する画像認識部２０ｇが追加されている。ステレオカメラ３０、管理用カメラ３２、画像認識部２０ｇをまとめたものが、前述した画像認識装置３５に相当する。

画像認識装置３５の画像認識部２０ｇによって算出される姿勢情報は、ステレオカメラ３０と管理用カメラ３２の性能（解像度など）に大きく依存する。本発明では、フロント作業機１が上げ姿勢（図１５Ａ）であるか、下げ姿勢（図１４Ａ）であるかを判断できる性能を有していれば十分である。

本実施形態では、上述の画像認識部２０ｇで演算した姿勢情報を修正方位角演算部２０ａ−１で利用する。

図１０は、本実施形態における修正方位角演算部２０ａ−１の処理機能を示すブロック図である。

図１０において、修正方位角演算部２０ａ−１には、図４に示す方位角品質判断部MH01及び旋回動作実行判断部MH03に、画像認識部２０ｇからの信号が追加信号として更に入力される。

方位角品質判定部MH01は、車体IMU１３で取得した車体傾斜角の情報に加えて、画像認識部２０ｇで演算したフロント作業機１の上げ姿勢、下げ姿勢情報を用いて方位角品質を判断する。つまり、画像認識部２０ｇで演算した姿勢情報において、フロント作業機１が上げ姿勢であれば、方位角品質が劣っていると判断する。

なお、ドローン３４に設定した管理用カメラ３３を利用する場合、油圧ショベル１００自身だけでなく油圧ショベル１００の周辺の画像情報の取得が可能になる。このような場合には、方位角品質演算部MH01は油圧ショベル１００自身の姿勢情報だけでなく、周辺障害物の情報も加味して方位角品質を算出してもよい。

また、画像認識部２０ｇにおいて、フロント作業機１の姿勢角だけでなく、上部旋回体２の姿勢角も演算し、方位角品質演算部MH0でその上部旋回体２の姿勢角を利用してもよい。ただし、その場合の姿勢角は画像処理による姿勢情報であるので、方位角品質の判断精度は車体IMU１３よりも劣るため、バックアップとしての利用にとどめることが望ましい。

旋回動作実行判断部MH03にも、画像認識部２０ｇで取得した上部旋回体２の姿勢情報を利用してもよい。ただし、この場合も、画像処理による姿勢情報に基づく旋回動作の有無の判断精度は車体IMU１３よりも劣るため、バックアップとしての利用にとどめることが望ましい。

なお、図８の油圧ショベル１００ではステレオカメラ３０と管理用カメラ３２の両方を用いた構成としているが、どちらか一方でも本発明を実行できることは言うまでもない。

このように本実施の形態によれば、ブームIMU１４或いは車体IMU１３を用いなくても、フロント作業機１の上げ下げなどの油圧ショベル１００の姿勢変化によらずに、GNSSを利用して正確な上部旋回体２の方位角を算出し、上部旋回体２の方位角を高精度、かつ、ロバストに取得することができる。

また、画像認識装置３５で取得した姿勢情報をブームIMU１４或いは車体IMU１３のバックアップとして用いる場合は、システムとしてのロバスト性を更に向上させ、作業効率を向上することができる。

＜第４の実施の形態＞
例えば、山間部の作業現場では、図１７に示すように油圧ショベル１００の背後に急峻な傾斜面がある場合がある。このような状況では、油圧ショベル１００の背面の衛星信号を受信することができないため、方位角の品質も当然低下する。

本発明の第４の実施形態は、そのような状況に鑑みてなされたものである。

まず、本実施形態の特徴を概略的に説明すれば以下のようである。

本実施の形態において、コントローラ２０は、油圧ショベル１００（建設機械）の周辺の地形情報を記憶している地形情報記憶装置２０ｇ（図１１参照）を備え、GNSS受信機１７ｃによって算出したGNSSアンテナ１７ａ，１７ｂの３次元座標に基づいて、地形情報記憶装置２０ｇに記憶した油圧ショベル１００（建設機械）の周辺の地形情報を読み込み、車体IMU１３（車体姿勢角取得装置）で取得した上部旋回体２の姿勢角とブームIMU１４（フロント姿勢角取得装置）で取得したフロント作業機１の姿勢角に基づく上部旋回体２の方位角の品質の判断に加え、或いはその判断に代え、油圧ショベル１００（建設機械）の周辺地形がGNSSアンテナ１７ａ，１７ｂの衛星信号受信可能領域２００を遮断する可能性があるとき、GNSS受信機１７ｃで算出された上部旋回体２の方位角の品質が低いと判断し、バイアス除去演算を実行する。

以下に、コントローラ２０の処理内容の詳細を図１１に基づいて説明する。図１１は、本実施形態におけるコントローラ２０の処理機能を示すブロック図である。

本実施形態において、コントローラ２０は、周辺地形参照部２０ｈを更に備えている。

周辺地形参照部２０ｈは、GNSSシステム１７で取得したGNSSアンテナ１７ａ，１７ｂの３次元座標情報を入力して、油圧ショベル周辺の地形情報を参照し、この参照した地形情報を修正方位角演算部２０ａ−１に出力する。

図１２は、本実施形態における修正方位角演算部２０ａ−１の処理機能を示すブロック図である。

本実施形態における修正方位角演算部２０ａ−１は、図１２に示す通り、油圧ショベル１００の姿勢情報（フロント作業機１の上げ下げなどの油圧ショベル１００の姿勢変化）のみでなく、周辺の地形情報からも方位角品質を決定できることに特徴がある。つまり、自車位置の周辺に衛星信号を遮断するような地形（図１７の急斜面など）がある場合、油圧ショベル１００の姿勢によらずに、方位角品質が低下していると判断する。

本実施の形態によれば、フロント作業機１の上げ下げなどの油圧ショベル１００の姿勢変化によらずに、かつ／又は油圧ショベル１００の周辺地形によらずに、上部旋回体２の方位角を高精度、かつ、ロバストに取得し、作業効率を向上することができる。

なお、以上において第１〜第４の実施形態を説明したが、これらの実施形態のいずれか２つ以上を組み合わせた構成にしてもよく、その構成も本発明の範囲に該当することは言及するまでもない。

１ フロント作業機
２ 上部旋回体
３ 下部走行体
４ ブーム
５ アーム
６ バケット
９ キャビン
９ａ，９ｂ 操作レバー装置
１１ モニタ
１３ 慣性計測装置（車体IMU）（角速度取得装置）（車体姿勢角取得装置）（車体傾斜センサ）
１４ 慣性計測装置（ブームIMU）（フロント姿勢角取得装置）（フロント傾斜センサ）
１５ 慣性計測装置（アームIMU）（フロント姿勢角取得装置）
１６ 慣性計測装置（バケットIMU）（フロント姿勢角取得装置）
１７ GNSSシステム
１７ａ，１７ｂ GNSSアンテナ
１７ｃ GNSS受信機
２０ コントローラ
２０ａ 位置姿勢演算部
２０ａ−１ 修正方位角演算部
２０ａ−２ ３次元位置姿勢演算部
２０ｂ 施工目標面演算部
２０ｃ モニタ表示制御部
２０ｄ 油圧システム制御部
２０ｅ 記憶装置
２０ｆ 記憶装置
２０ｇ 画像認識部
２０ｈ 周辺地形参照部
３０ ステレオカメラ
３１ａ 無線通信機
３１ｂ 無線通信機
３１ｃ 無線通信機
３２ 管理用カメラ
３３ カメラ
３４ 無人航空機（ドローン）
３５ 画像認識装置
１００ 油圧ショベル
２００ 衛星信号受信可能領域
Σm 車体座標系
MH01 方位角品質判断部
MH02 バイアス除去実行部
MH03 旋回動作判断部
MH04 方位角統合演算部

Claims (8)

1. 下部走行体と、
   下部走行体に対して旋回可能な上部旋回体と、
   上部旋回体に対して垂直方向に回転可能に取り付けられた多関節型のフロント作業機と、
   前記上部旋回体に取り付けられた２本のGNSSアンテナと、前記GNSSアンテナが受信した衛星信号に基づいて前記GNSSアンテナの３次元座標と前記上部旋回体の方位角を算出するGNSS受信機を有するGNSSシステムと、
   前記上部旋回体に取り付けられ、前記上部旋回体の角速度を取得する角速度取得装置と、
   前記上部旋回体に取り付けられ、前記上部旋回体の姿勢角を取得する車体姿勢角取得装置と、
   前記フロント作業機の姿勢角を取得するフロント姿勢角取得装置と、
   コントローラとを備え、
   前記コントローラは、前記GNSS受信機で算出された前記GNSSアンテナの３次元座標及び前記上部旋回体の方位角と、前記車体姿勢角取得装置で取得した前記上部旋回体の姿勢角、前記角速度取得装置で取得した前記上部旋回体の角速度及び前記フロント姿勢角取得装置で取得した前記フロント作業機の姿勢角に基づいて、前記フロント作業機の位置及び姿勢を演算し、前記フロント作業機の位置及び姿勢に基づいて前記フロント作業機の動作を制御する建設機械において、
   前記コントローラは、
   前記車体姿勢角取得装置で取得した前記上部旋回体の姿勢角と前記フロント姿勢角取得装置で取得した前記フロント作業機の姿勢角の少なくとも一方に基づいて前記GNSS受信機で算出された前記上部旋回体の方位角の品質を判断し、
   前記方位角の品質の判断結果と、前記GNSS受信機で算出された前記上部旋回体の方位角とに基づいて、前記角速度取得装置で取得した前記上部旋回体の角速度からジャイロバイアスを除去するバイアス除去演算を実行し、かつ前記ジャイロバイアスを除去した前記上部旋回体の角速度に基づいて前記上部旋回体の旋回動作の有無を判断し、
   前記GNSS受信機で算出された前記上部旋回体の方位角と、前記ジャイロバイアスを除去した前記上部旋回体の角速度と、前記上部旋回体の旋回動作の判断結果に基づいて前記上部旋回体の修正方位角を算出し、前記修正方位角を用いて前記フロント作業機の位置及び姿勢を演算することを特徴とする建設機械。
2. 請求項１記載の建設機械において、
   前記車体姿勢角取得装置は、前記上部旋回体に取り付けられた少なくとも１つの車体傾斜センサであり、
   前記フロント姿勢角取得装置は、前記フロント作業機に取り付けられた少なくとも１つのフロント傾斜センサであり、
   前記コントローラは、前記車体傾斜センサで検出した前記上部旋回体の姿勢角と前記フロント傾斜センサで検出した前記フロント作業機の姿勢角の少なくとも一方に基づいて前記GNSS受信機で算出された前記上部旋回体の方位角の品質を判断し、前記上部旋回体の方位角の品質が低下していると判断したときに前記バイアス除去演算を実行することを特徴とする建設機械。
3. 請求項１記載の建設機械において、
   前記車体姿勢角取得装置及び前記フロント姿勢角取得装置の少なくとも一方の姿勢角取得装置として画像認識装置を備え、
   前記画像認識装置は、少なくとも１台のカメラから取得した画像情報に基づいて前記上部旋回体の姿勢角と前記フロント作業機の姿勢角の少なくとも一方を認識し、
   前記コントローラは、前記画像認識装置で認識した前記上部旋回体の姿勢角と前記フロント作業機の姿勢角の少なくとも一方に基づいて前記GNSS受信機で算出された前記上部旋回体の方位角の品質を判断し、前記上部旋回体の方位角の品質が低下していると判断したときに前記バイアス除去演算を実行することを特徴とする建設機械。
4. 請求項１記載の建設機械において、
   前記コントローラは、
   前記建設機械の周辺の地形情報を記憶している地形情報記憶装置を備え、
   前記GNSS受信機によって算出した前記GNSSアンテナの３次元座標に基づいて、前記地形情報記憶装置に記憶した前記建設機械の周辺の地形情報を読み込み、
   前記車体姿勢角取得装置で取得した前記上部旋回体の姿勢角と前記フロント姿勢角取得装置で取得した前記フロント作業機の姿勢角に基づく前記上部旋回体の方位角の品質の判断に加え、或いはその判断に代え、前記建設機械の周辺地形が前記GNSSアンテナの衛星信号受信可能領域を遮断する可能性があるとき、前記GNSS受信機で算出された前記上部旋回体の方位角の品質が低いと判断し、前記バイアス除去演算を実行することを特徴とする建設機械。
5. 請求項１記載の建設機械において、
   前記コントローラは、
   前記上部旋回体の方位角の品質が高いと判断したときの方位角を記憶する方位角記憶装置を備え、
   前記上部旋回体の方位角の品質が低いと判断し、前記上部旋回体の旋回動作無しを判断したときは、前記上部旋回体の方位角の品質が低いと判断する直前に前記方位角記憶装置に記憶した方位角を修正方位角として用い、前記フロント作業機の位置及び姿勢を演算することを特徴とする建設機械。
6. 請求項１記載の建設機械において、
   前記コントローラは、
   前記上部旋回体の方位角の品質が低いと判断し、かつ前記上部旋回体の旋回動作有りを判断した後、前記上部旋回体の方位角の品質が低いまま所定の時間を経過したときは、前記上部旋回体の方位角の精度が低い可能性があることをユーザに通知することを特徴とする建設機械。
7. 請求項１記載の建設機械において、
   前記フロント作業機の動作を指示する第１操作レバー装置を更に備え、
   前記コントローラは、
   前記GNSS受信機で算出された前記上部旋回体の方位角の品質の判断に際して、前記フロント姿勢角取得装置で取得した前記フロント作業機の姿勢角のバックアップとして前記第１操作レバー装置の操作信号を入力し、この第１操作レバー装置の操作信号に基づいて、前記第１操作レバー装置の前記フロント作業機の上げ方向操作が所定時間以上継続したときに、前記上部旋回体の方位角の品質が低いと判断することを特徴とする建設機械。
8. 請求項１記載の建設機械において、
   前記上部旋回体の動作を指示する第２操作レバー装置を更に備え、
   前記コントローラは、
   前記上部旋回体の旋回動作の有無の判断に際して、前記ジャイロバイアスを除去した前記上部旋回体の角速度のバックアップとして前記第２操作レバー装置の操作信号を入力し、この第２操作レバー装置の操作信号に基づいて、前記第２操作レバー装置の操作信号の大きさが所定値以上であるときに、前記上部旋回体の旋回動作が有りと判断することを特徴とする建設機械。

Images