(57)【特許請求の範囲】
【請求項１】
走行装置と、作業具を有する作業機と、前記作業機が取り付けられ、かつ前記走行装置に取り付けられて旋回する旋回体と、を備える作業機械を制御するシステムであって、
前記作業機械の一部の位置である第１位置を検出し、前記第１位置の情報として出力する位置検出装置と、
前記作業機械の動作を示す動作情報を検出して出力する状態検出装置と、
前記第１位置及び前記動作情報から得られる情報である、前記旋回体の回転中心軸と前記走行装置が接地する面に対応する面との交点である特定点の位置に平滑化処理を施し、前記平滑化処理後の前記特定点の位置を用いて、前記一部の位置に相当し、かつ前記第１位置の測位結果のばらつきによる影響を低減した第２位置を求め、前記第２位置の情報を用いて、前記作業機の少なくとも一部の位置を求める処理装置と、
を含む、作業機械の制御システム。
【請求項２】
走行装置と、作業具を有する作業機と、前記作業機が取り付けられ、かつ前記走行装置に取り付けられて旋回する旋回体と、を備える作業機械を制御するシステムであって、
前記作業機械の一部の位置である第１位置を検出し、前記第１位置の情報として出力する位置検出装置と、
前記作業機械の動作を示す動作情報を検出して出力する状態検出装置と、
前記位置検出装置に接続され、前記作業機械の位置を検出するためのアンテナの速度を前記動作情報に基づいて求め、前記アンテナの速度を用いて前記第１位置に平滑化処理を施して、前記一部の位置に相当し、かつ前記第１位置の測位結果のばらつきによる影響を低減した第２位置を求め、前記第２位置の情報を用いて、前記作業機の少なくとも一部の位置を求める処理装置と、
を含む、作業機械の制御システム。
【請求項３】
前記処理装置は、
前記位置検出装置による前記作業機械の位置の検出が正常、かつ前記作業機械の走行が停止、かつ前記旋回体が旋回していないときに、前記第２位置の情報を用いて、前記作業機の少なくとも一部の位置を求める、請求項１または２に記載の作業機械の制御システム。
【請求項４】
走行装置と、作業具を有する作業機と、前記作業機が取り付けられ、かつ前記走行装置に取り付けられて旋回する旋回体と、を備える作業機械を制御するにあたり、
前記作業機械が備える位置検出装置が検出した前記作業機械の一部の位置である第１位置と、前記作業機械が備える状態検出装置が検出した前記作業機械の動作情報とを用いて、前記旋回体の回転中心軸と前記走行装置が接地する面に対応する面との交点である特定点の位置を求め、
前記特定点の位置に平滑化処理を施し、前記平滑化処理後の前記特定点の位置を用いて、前記一部の位置に相当し、かつ前記第１位置の測位結果のばらつきによる影響を低減した前記作業機械の第２位置を求め、
前記第２位置を用いて、前記作業機の少なくとも一部の位置を求める、
作業機械の制御方法。
【請求項５】
走行装置と、作業具を有する作業機と、前記作業機が取り付けられ、かつ前記走行装置に取り付けられて旋回する旋回体と、を備える作業機械を制御するにあたり、
前記作業機械が備える位置検出装置が検出した前記作業機械の一部の位置である第１位置に、前記作業機械が備える状態検出装置が検出した前記作業機械の動作情報に基づいて求めた、前記作業機械の位置を検出するためのアンテナの速度を用いて平滑化処理を施して、前記一部の位置に相当し、かつ前記第１位置の測位結果のばらつきによる影響を低減した前記作業機械の第２位置を求め、
前記第２位置を用いて、前記作業機の少なくとも一部の位置を求める、
作業機械の制御方法。
【請求項６】
前記処理装置が求めた前記作業機の一部の位置に基づき、前記作業機の動作を制御、もしくは、前記作業機械に設置された表示装置に前記作業機の一部の位置を表示する、請求項１から３のいずれか１項に記載の作業機械の制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、作業機を備えた作業機械に用いられる作業機械の制御システム及び作業機械に関する。
【背景技術】
【０００２】
ＧＰＳ（Global  Positioning  System）等を利用して作業機械の３次元位置を測位し、得られた作業機械の位置情報を用いて作業機械を管理したり、作業機械による施工状態を管理したり、作業機械を制御したりする技術が知られている（例えば、特許文献１）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−１４７５８８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
作業機械の位置を測位する機器を備える作業機械としては、その機器が検出した作業機械の位置の情報を用いて、作業機械の運転室内に設置された表示装置に作業のガイダンス画面を表示させたり、作業機の動作を制御したりするものがある。このような作業機械による施工は、情報化施工と呼ばれる。【０００５】
情報化施工による建設工事が行われることがある。情報化施工を行うために、ＧＰＳアンテナ等を搭載した油圧ショベルにより、法面成形といった施工が行われ、工期短縮又は熟練オペレータ不足の問題を解消することが期待されている。【０００６】
ＧＰＳの測位衛星の位置、電離層、対流圏又はＧＰＳアンテナ周辺の地形の影響により、測位結果にばらつきが発生することがある。情報化施工においては、測位結果に基づきバケットの刃先位置を求め、作業機の制御及びガイダンス画面の表示を行うが、測位結果のばらつきの影響により、施工面が波打ったり、ガイダンス画面に表示されるバケットの刃先が揺らいだりする可能性がある。結果として、施工面の仕上がりが滑らかにならなかったり、施工中におけるガイダンスの画面の視認性が低下したりする可能性がある。【０００７】
本発明は、作業機械の位置を測位した結果に基づいて情報化施工を行う作業機械において、測位結果のばらつきが情報化施工に与える影響を低減することを目的とする。
【課題を解決するための手段】
【０００８】
本発明は、走行装置と、作業具を有する作業機と、前記作業機が取り付けられ、かつ前記走行装置に取り付けられて旋回する旋回体と、を備える作業機械を制御するシステムであって、前記作業機械の一部の位置である第１位置を検出し、前記第１位置の情報として出力する位置検出装置と、前記作業機械の動作を示す動作情報を検出して出力する状態検出装置と、前記第１位置の情報及び前記動作情報を用いて、前記一部の位置に相当する第２位置を求め、前記第２位置の情報を用いて、前記作業機の少なくとも一部の位置を求める処理装置と、を含む、作業機械の制御システムである。【０００９】
前記処理装置は、前記第１位置及び前記動作情報から得られる情報である、前記旋回体の回転中心軸と前記走行装置が接地する面に対応する面との交点である特定点の位置を用いて、前記第２位置を求めることが好ましい。【００１０】
前記処理装置は、前記特定点の位置に平滑化処理を施し、前記平滑化処理後の前記特定点の位置を用いて、前記第２位置の情報を求めることが好ましい。【００１１】
前記処理装置は、前記動作情報を用いて前記第１位置に平滑化処理を施して前記第２位置の情報を求めることが好ましい。【００１２】
前記処理装置は、前記位置検出装置による前記作業機械の位置の検出が正常、かつ前記作業機械の走行が停止、かつ前記旋回体が旋回していないときに、前記第２位置の情報を用いて、前記作業機の少なくとも一部の位置を求めることが好ましい。【００１３】
前記処理装置は、前記作業機械の走行が停止、かつ前記旋回体が旋回しているときは、前記第２位置の情報を求める処理を中断することが好ましい。【００１４】
前記処理装置は、前記旋回体の旋回が停止したときは、前記第２位置を求める処理を中断する前に得られた前記第２位置の情報を用いて前記作業機の少なくとも一部の位置を求めることが好ましい。【００１５】
前記処理装置は、前記第２位置を求める処理を中断しているときに、前記作業機械が走行を開始した場合、前記第２位置の情報を求める処理を停止することが好ましい。【００１６】
前記処理装置は、前記位置検出装置が前記作業機械の位置を正常に検出し、かつ前記作業機械の走行が停止している場合、前記第２位置を求める処理を停止することが好ましい。【００１７】
前記処理装置は、前記動作情報を用いて前記作業機械の位置を推定し、推定により得られた前記作業機械の推定位置を補正して第２位置として出力する位置推定部と、前記第１位置の情報及び前記動作情報の少なくとも一方を用いて、前記推定位置に含まれる誤差を求め、前記位置推定部に出力する誤差演算部と、を有し、前記位置推定部は、前記誤差演算部が出力した前記誤差を用いて前記推定位置を補正することが好ましい。【００１８】
前記処理装置は、前記位置検出装置による前記作業機械の位置の検出の状態と、前記作業機械の動作状態とを用いて、前記誤差演算部に入力される情報を選択することが好ましい。【００１９】
本発明は、前述した作業機械の制御システムを備える作業機械である。【００２０】
本発明は、走行装置と、作業具を有する作業機と、前記作業機が取り付けられ、かつ前記走行装置に取り付けられて旋回する旋回体と、を備える作業機械を制御するにあたり、前記作業機械が備える位置検出装置が検出した前記作業機械の一部の位置である第１位置と、前記作業機械が備える状態検出装置が検出した前記作業機械の動作情報とを用いて、前記一部の位置に相当する前記作業機械の第２位置を求め、前記第２位置を用いて、前記作業機の少なくとも一部の位置を求める、作業機械の制御方法である。【００２１】
前記第１位置及び前記動作情報から得られる情報である、前記旋回体の回転中心軸と前記走行装置が接地する面に対応する面との交点である特定点の位置を用いて、前記第２位置を求めることが好ましい。【００２２】
前記特定点の位置に平滑化処理を施し、前記平滑化処理後の前記特定点の位置を用いて、前記第２位置を求めることが好ましい。【００２３】
前記動作情報を用いて前記第１位置に平滑化処理を施して前記第２位置を求めることが好ましい。【００２４】
前記第２位置を求める場合、前記動作情報を用いて前記作業機械の位置を推定することにより推定位置を求め、前記第１位置及び前記動作情報の少なくとも一方を用いて前記推定位置に含まれる誤差を計算し、前記誤差演算部が出力した前記誤差を用いて前記推定位置を補正することが好ましい。【００２５】
本発明は、作業機械の位置を測位した結果に基づいて情報化施工を行う作業機械において、測位結果のばらつきが情報化施工に与える影響を低減することができる。
【図面の簡単な説明】
【００２６】
【図１】図１は、実施形態１に係る作業機械の斜視図である。
【図２】図２は、制御システム及び油圧システムの構成を示すブロック図である。
【図３】図３は、油圧ショベルの側面図である。
【図４】図４は、油圧ショベルの背面図である。
【図５】図５は、実施形態１に係る制御システムの制御ブロック図である。
【図６】図６は、油圧ショベルの姿勢を示す平面図である。
【図７】図７は、実施形態１に係る装置コントローラが有する位置情報演算部を示す図である。
【図８】図８は、実施形態１に係る制御システムの処理の一例を示すフローチャートである。
【図９】図９は、平滑化処理の状態の遷移を説明するための図である。
【図１０】図１０は、装置コントローラが平滑化処理の状態を遷移させる処理のフローチャートであって、特に平滑化処理の中断に関する処理を示すものである。
【図１１】図１１は、装置コントローラが平滑化処理の状態を遷移させる処理のフローチャートであって、特に平滑化処理のリセットに関する処理を示すものである。
【図１２】図１２は、実施形態２に係る制御システムの制御ブロック図である。
【図１３】図１３は、実施形態２に係る装置コントローラが有する位置情報演算部を示す図である。
【図１４】図１４は、実施形態２に係る制御システムの処理の一例を示すフローチャートである。
【図１５】図１５は、実施形態３に係る制御システムの制御ブロック図である。
【図１６】図１６は、実施形態３に係る装置コントローラが有する位置・姿勢情報演算部を示す図である。
【図１７】図１７は、実施形態３に係る装置コントローラが有する位置・姿勢情報演算部の制御ブロック図である。
【図１８】図１８は、誤差演算部が使用する観測方程式を選択する際に用いられる情報が記述されたテーブルの一例を示す図である。
【図１９】図１９は、実施形態３に係る制御システムの処理の一例を示すフローチャートである。
【発明を実施するための形態】
【００２７】
本発明を実施するための形態（本実施形態）につき、図面を参照しつつ詳細に説明する。
【００２８】
実施形態１．
＜作業機械の全体構成＞
図１は、実施形態１に係る作業機械の斜視図である。図２は、制御システム２００及び油圧システム３００の構成を示すブロック図である。作業機械としての油圧ショベル１００は、本体部としての車両本体１と作業機２とを有する。車両本体１は、旋回体である上部旋回体３と走行体としての走行装置５とを有する。上部旋回体３は、機械室３ＥＧの内部に、動力発生装置であるエンジン及び油圧ポンプ等の装置を収容している。
【００２９】
本実施形態において、油圧ショベル１００は、動力発生装置であるエンジンに、例えばディーゼルエンジン等の内燃機関が用いられるが、動力発生装置は内燃機関に限定されない。油圧ショベル１００の動力発生装置は、例えば、内燃機関と発電電動機と蓄電装置とを組み合わせた、いわゆるハイブリッド方式の装置であってもよい。また、油圧ショベル１００の動力発生装置は、内燃機関を有さず、蓄電装置と発電電動機とを組み合わせた装置であってもよい。
【００３０】
上部旋回体３は、運転室４を有する。運転室４は、上部旋回体３の他端側に設置されている。すなわち、運転室４は、機械室３ＥＧが配置されている側とは反対側に設置されている。運転室４内には、図２に示す、表示部２９及び操作装置２５が配置される。上部旋回体３の上方には、手すり９が取り付けられている。
【００３１】
走行装置５の上には、上部旋回体３が搭載されている。走行装置５は、履帯５ａ、５ｂを有している。走行装置５は、左右に設けられた油圧モータ５ｃの一方又は両方によって駆動される。走行装置５の履帯５ａ、５ｂが回転することにより、油圧ショベル１００を走行させる。作業機２は、上部旋回体３の運転室４の側方側に取り付けられている。
【００３２】
油圧ショベル１００は、履帯５ａ、５ｂの代わりにタイヤを備え、エンジンの駆動力を、トランスミッションを介してタイヤへ伝達して走行が可能な走行装置を備えたものであってもよい。このような形態の油圧ショベル１００としては、例えば、ホイール式油圧ショベルがある。
【００３３】
上部旋回体３は、作業機２及び運転室４が配置されている側が前であり、機械室３ＥＧが配置されている側が後である。上部旋回体３の前後方向がｘ方向である。前に向かって左側が上部旋回体３の左であり、前に向かって右側が上部旋回体３の右である。上部旋回体３の左右方向は、幅方向又はｙ方向ともいう。油圧ショベル１００又は車両本体１は、上部旋回体３を基準として走行装置５側が下であり、走行装置５を基準として上部旋回体３側が上である。上部旋回体３の上下方向がｚ方向である。油圧ショベル１００が水平面に設置されている場合、下は鉛直方向、すなわち重力の作用方向側であり、上は鉛直方向とは反対側である。
【００３４】
作業機２は、ブーム６とアーム７と作業具であるバケット８とブームシリンダ１０とアームシリンダ１１とバケットシリンダ１２とを有する。ブーム６の基端部は、ブームピン１３を介して車両本体１の前部に回動可能に取り付けられている。アーム７の基端部は、アームピン１４を介してブーム６の先端部に回動可能に取り付けられている。アーム７の先端部には、バケットピン１５を介してバケット８が取り付けられている。バケット８は、バケットピン１５を中心として回動する。バケット８は、バケットピン１５とは反対側に複数の刃８Ｂが取り付けられている。刃先８Ｔは、刃８Ｂの先端である。
【００３５】
バケット８は、複数の刃８Ｂを有していなくてもよい。つまり、図１に示すような刃８Ｂを有しておらず、刃先が鋼板によってストレート形状に形成されたようなバケットであってもよい。作業機２は、例えば、単数の刃を有するチルトバケットを備えていてもよい。チルトバケットとは、バケットチルトシリンダを備え、バケットが左右にチルト傾斜することで油圧ショベル１００が傾斜地にあっても、斜面、平地を自由な形に成形したり、整地したりすることができ、底板プレートによる転圧作業もできるバケットである。この他にも、作業機２は、バケット８の代わりに、法面バケット又は削岩用のチップを備えた削岩用のアタッチメント等を作業具として備えていてもよい。
【００３６】
図１に示すブームシリンダ１０とアームシリンダ１１とバケットシリンダ１２とは、それぞれ作動油の圧力によって駆動される油圧シリンダである。以下において、作動油の圧力を、適宜油圧と称する。ブームシリンダ１０はブーム６を駆動して、昇降させる。アームシリンダ１１は、アーム７を駆動して、アームピン１４の周りを回動させる。バケットシリンダ１２は、バケット８を駆動して、バケットピン１５の周りを回動させる。
【００３７】
ブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２等の油圧シリンダと図２に示される油圧ポンプ３６、３７との間には、図２に示される方向制御弁６４が設けられている。方向制御弁６４は、油圧ポンプ３６、３７からブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２等に供給される作動油の流量を制御するとともに、作動油が流れる方向を切り替える。方向制御弁６４は、油圧モータ５ｃを駆動するための走行用方向制御弁と、ブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２並びに上部旋回体３を旋回させる旋回モータ３８を制御するための作業機用方向制御弁とを含む。
【００３８】
操作装置２５から供給される、所定のパイロット圧力に調整された作動油が方向制御弁６４のスプールを動作させると、方向制御弁６４から流出する作動油の流量が調整されて、油圧ポンプ３６、３７からブームシリンダ１０、アームシリンダ１１、バケットシリンダ１２、旋回モータ３８又は油圧モータ５ｃに供給される作動油の流量が制御される。その結果、ブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２等の動作が制御される。
【００３９】
また、図２に示される装置コントローラ３９が、図２に示される制御弁２７を制御することにより、操作装置２５から方向制御弁６４に供給される作動油のパイロット圧が制御されるので、方向制御弁６４からブームシリンダ１０、アームシリンダ１１、バケットシリンダ１２又は旋回モータ３８に供給される作動油の流量が制御される。その結果、装置コントローラ３９は、ブームシリンダ１０、アームシリンダ１１、バケットシリンダ１２及び上部旋回体３の動作を制御することができる。
【００４０】
上部旋回体３の上部には、アンテナ２１、２２が取り付けられている。アンテナ２１、２２は、油圧ショベル１００の現在位置を検出するために用いられる。アンテナ２１、２２は、図２に示されるグローバル座標演算装置２３と電気的に接続されている。グローバル座標演算装置２３は、油圧ショベル１００の位置を検出する位置検出装置である。グローバル座標演算装置２３は、ＲＴＫ−ＧＮＳＳ（Real  Time  Kinematic  -  Global  Navigation  Satellite  Systems、ＧＮＳＳは全地球航法衛星システムをいう）を利用して油圧ショベル１００の現在位置、より具体的には油圧ショベル１００の一部の現在位置を検出する。以下の説明において、アンテナ２１、２２を、適宜ＧＮＳＳアンテナ２１、２２と称する。本実施形態において、グローバル座標演算装置２３は、油圧ショベル１００の一部の現在位置として、ＧＮＳＳアンテナ２１、２２の少なくとも１つの位置を検出する。ＧＮＳＳアンテナ２１、２２が受信したＧＮＳＳ電波に応じた信号は、グローバル座標演算装置２３に入力される。グローバル座標演算装置２３は、グローバル座標系におけるＧＮＳＳアンテナ２１、２２の設置位置を求める。全地球航法衛星システムの一例としては、ＧＰＳ（Global  Positioning  System）が挙げられるが、全地球航法衛星システムは、これに限定されるものではない。
【００４１】
ＲＴＫ−ＧＮＳＳでは、測位衛星の配置、電離層、対流圏又はＧＮＳＳアンテナ周辺の地形の影響で測位の状態が変化する。この測位の状態には、例えば、Ｆｉｘ（精度±１ｃｍから２ｃｍ程度）、Ｆｌｏａｔ（精度±１０ｃｍから数ｍ程度）、単独測位（精度±数ｍ程度）、非測位（測位計算不能）等がある。以下において、測位の状態がＦｉｘである場合を正常と称し、Ｆｉｘ以外の状態である場合を異常と称する。
【００４２】
ＧＮＳＳアンテナ２１、２２は、図１に示すように、上部旋回体３の上であって、油圧ショベル１００の左右方向、すなわち幅方向に離れた両端位置に設置されることが好ましい。本実施形態において、ＧＮＳＳアンテナ２１、２２は、上部旋回体３の幅方向両側にそれぞれ取り付けられた手すり９に取り付けられる。ＧＮＳＳアンテナ２１、２２が上部旋回体３に取り付けられる位置は手すり９に限定されるものではないが、ＧＮＳＳアンテナ２１、２２は、可能な限り離れた位置に設置される方が、油圧ショベル１００の現在位置の検出精度は向上するので好ましい。また、ＧＮＳＳアンテナ２１、２２は、オペレータの視界を極力妨げない位置に設置されることが好ましい。例えば、ＧＮＳＳアンテナ２１、２２は、機械室３ＥＧの後方に配置されたカウンタウェイトの上に配置されてもよい。
【００４３】
図２に示すように、油圧ショベル１００の油圧システム３００は、エンジン３５と、油圧ポンプ３６、３７とを備える。油圧ポンプ３６、３７は、エンジン３５によって駆動され、作動油を吐出する。油圧ポンプ３６、３７から吐出された作動油は、ブームシリンダ１０とアームシリンダ１１とバケットシリンダ１２とに供給される。また、油圧ショベル１００は、旋回モータ３８を備える。旋回モータ３８は、油圧モータであり、油圧ポンプ３６、３７から吐出された作動油によって駆動される。旋回モータ３８は、上部旋回体３を旋回させる。図２では、２つの油圧ポンプ３６、３７が図示されているが、油圧ポンプは１つでもよい。旋回モータ３８は、油圧モータに限らず、電気モータであってもよい。
【００４４】
作業機械の制御システムである制御システム２００は、グローバル座標演算装置２３と、角速度及び加速度を検出する状態検出装置であるＩＭＵ（Inertial  Measurement  Unit：慣性計測装置）２４と、操作装置２５と、処理装置としての装置コントローラ３９と、処理装置としての表示コントローラ２８と、表示部２９とを含む。操作装置２５は、図１に示す作業機２、上部旋回体３及び走行装置５の少なくとも１つを操作するための装置である。操作装置２５は、作業機２等を駆動させるためにオペレータによる操作を受け付けて、操作量に応じたパイロット油圧を出力する。
【００４５】
操作装置２５は、オペレータの左側に設置される左操作レバー２５Ｌと、オペレータの右側に配置される右操作レバー２５Ｒと、を有する。左操作レバー２５Ｌ及び右操作レバー２５Ｒは、前後左右の動作が２軸の動作に対応されている。例えば、右操作レバー２５Ｒの前後方向の操作は、ブーム６の操作に対応されている。例えば、右操作レバー２５Ｒの左右方向の操作は、バケット８の操作に対応されている。例えば、左操作レバー２５Ｌの前後方向の操作は、アーム７の操作に対応している。例えば、左操作レバー２５Ｌの左右方向の操作は、上部旋回体３の旋回に対応している。
【００４６】
本実施形態において、操作装置２５は、パイロット油圧方式が用いられる。操作装置２５には、油圧ポンプ３６から、図示しない減圧弁によって所定のパイロット圧力に減圧された作動油がブーム操作、バケット操作、アーム操作、旋回操作及び走行操作に基づいて供給される。
【００４７】
右操作レバー２５Ｒの前後方向の操作に応じて、パイロット油路４５０へパイロット油圧が供給可能とされて、オペレータによるブーム６の操作が受け付けられる。右操作レバー２５Ｒの操作量に応じて右操作レバー２５Ｒが備える弁装置が開き、パイロット油路４５０へ作動油が供給される。また、圧力センサ６６は、そのときのパイロット油路４５０内における作動油の圧力をパイロット圧として検出する。圧力センサ６６は、検出したパイロット圧を、ブーム操作信号ＭＢとして装置コントローラ３９へ送信する。
【００４８】
操作装置２５とブームシリンダ１０との間のパイロット油路４５０には、圧力センサ６８、制御弁（以下、適宜介入弁と称する）２７Ｃ及びシャトル弁５１が設けられる。右操作レバー２５Ｒの左右方向の操作に応じて、パイロット油路４５０へパイロット油圧が供給可能とされて、オペレータによるバケット８の操作が受け付けられる。右操作レバー２５Ｒの操作量に応じて右操作レバー２５Ｒが備える弁装置が開き、パイロット油路４５０に作動油が供給される。また、圧力センサ６６は、そのときのパイロット油路４５０内における作動油の圧力をパイロット圧として検出する。圧力センサ６６は、検出したパイロット圧を、バケット操作信号ＭＴとして装置コントローラ３９へ送信する。
【００４９】
左操作レバー２５Ｌの前後方向の操作に応じて、パイロット油路４５０へパイロット油圧が供給可能とされて、オペレータによるアーム７の操作が受け付けられる。左操作レバー２５Ｌの操作量に応じて左操作レバー２５Ｌが備える弁装置が開き、パイロット油路４５０へ作動油が供給される。また、圧力センサ６６は、そのときのパイロット油路４５０内における作動油の圧力をパイロット圧として検出する。圧力センサ６６は、検出したパイロット圧を、アーム操作信号ＭＡとして装置コントローラ３９へ送信する。
【００５０】
左操作レバー２５Ｌの左右方向の操作に応じて、パイロット油路４５０へパイロット油圧が供給可能とされて、オペレータによる上部旋回体３の旋回操作が受け付けられる。左操作レバー２５Ｌの操作量に応じて左操作レバー２５Ｌが備える弁装置が開き、パイロット油路４５０へ作動油が供給される。また、圧力センサ６６は、そのときのパイロット油路４５０内における作動油の圧力をパイロット圧として検出する。圧力センサ６６は、検出したパイロット圧を、旋回操作信号ＭＲとして装置コントローラ３９へ送信する。
【００５１】
右操作レバー２５Ｒが操作されることにより、操作装置２５は、右操作レバー２５Ｒの操作量に応じた大きさのパイロット油圧を方向制御弁６４に供給する。左操作レバー２５Ｌが操作されることにより、操作装置２５は、左操作レバー２５Ｌの操作量に応じた大きさのパイロット油圧を制御弁２７に供給する。このパイロット油圧によって、方向制御弁６４のスプールが移動する。
【００５２】
パイロット油路４５０には、制御弁２７が設けられている。右操作レバー２５Ｒ及び左操作レバー２５Ｌの操作量は、パイロット油路４５０に設置される圧力センサ６６によって検出される。圧力センサ６６が検出したパイロット油圧は、装置コントローラ３９に入力される。装置コントローラ３９は、入力されたパイロット油圧に応じた、パイロット油路４５０の制御信号Ｎを制御弁２７に出力して、パイロット油路４５０を開閉する。右操作レバー２５Ｒ又は左操作レバー２５Ｌの操作方向と操作対象（バケット８、アーム７、ブーム６、上部旋回体３）との関係は、上記に限定されるものではなく、異なる関係であってもよい。
【００５３】
操作装置２５は、走行用レバー２５ＦＬ、２５ＦＲを有する。本実施形態において、操作装置２５は、パイロット油圧方式が用いられるので、油圧ポンプ３６から、減圧された作動油が方向制御弁６４に供給され、パイロット油路４５０内の作動油の圧力に基づき方向制御弁６４のスプールが駆動される。すると、油圧ショベル１００の走行装置５が備える油圧モータ５ｃ、５ｃに、油圧ポンプ３６、３７から作動油が供給され、走行可能となる。パイロット油路４５０内の作動油の圧力、すなわちパイロット圧は、圧力センサ２７ＰＣによって検出される。
【００５４】
油圧ショベル１００のオペレータが走行装置５を動作させる場合、オペレータは走行用レバー２５ＦＬ、２５ＦＲを操作する。オペレータによる走行用レバー２５ＦＬ、２５ＦＲの操作量は圧力センサ２７ＰＣで検出されて、装置コントローラ３９へ操作信号ＭＤとして出力される。
【００５５】
左操作レバー２５Ｌ及び右操作レバー２５Ｒの操作量が、例えば、ポテンショメータ及びホールＩＣ等によって検出され、装置コントローラ３９は、これらの検出値に基づいて方向制御弁６４及び制御弁２７を制御することによって、作業機２を制御してもよい。このように、左操作レバー２５Ｌ及び右操作レバー２５Ｒは、電気方式であってもよい。
【００５６】
制御システム２００は、第１ストロークセンサ１６と第２ストロークセンサ１７と第３ストロークセンサ１８とを有する。例えば、第１ストロークセンサ１６はブームシリンダ１０に、第２ストロークセンサ１７はアームシリンダ１１に、第３ストロークセンサ１８はバケットシリンダ１２に、それぞれ設けられる。第１ストロークセンサ１６は、ブームシリンダ１０の伸長に対応する変位量を検出して、装置コントローラ３９に出力する。第２ストロークセンサ１７は、アームシリンダ１１の伸長に対応する変位量を検出して、装置コントローラ３９に出力する。第３ストロークセンサ１８は、バケットシリンダ１２の伸長に対応する変位量を検出して、装置コントローラ３９に出力する。
【００５７】
装置コントローラ３９は、ＣＰＵ（Central  Processing  Unit）等のプロセッサである処理部３９Ｐと、ＲＡＭ（Random  Access  Memory）及びＲＯＭ（Read  Only  Memory）等の記憶装置である記憶部３９Ｍとを有する。装置コントローラ３９には、グローバル座標演算装置２３の検出値、ＩＭＵ２４の検出値、圧力センサ２７ＰＣ、６６、６８の検出値、第１ストロークセンサ１６の検出値、第２ストロークセンサ１７の検出値及び第３ストロークセンサ１８の検出値が入力される。装置コントローラ３９は、グローバル座標演算装置２３の検出値及びＩＭＵ２４の検出値から、油圧ショベル１００の位置に関連する位置情報ＩＰＬを求めて表示コントローラ２８に出力する。装置コントローラ３９は、図２に示される圧力センサ６６の検出値に基づいて、制御弁２７及び介入弁２７Ｃを制御する。
【００５８】
図２に示される方向制御弁６４は、例えば比例制御弁であり、操作装置２５から供給される作動油によって制御される。方向制御弁６４は、ブームシリンダ１０、アームシリンダ１１、バケットシリンダ１２及び旋回モータ３８等の油圧アクチュエータと、油圧ポンプ３６、３７との間に配置される。方向制御弁６４は、油圧ポンプ３６、３７からブームシリンダ１０、アームシリンダ１１、バケットシリンダ１２及び旋回モータ３８に供給される作動油の流量を制御する。
【００５９】
グローバル座標演算装置２３は、図２に示される補正データ受信装置２６から、補正データＣ１を受信する。補正データ受信装置２６は、グローバル座標演算装置２３に接続される。補正データＣ１は、油圧ショベル１００の外部に設置されたＧＮＳＳ受信機で生成された、ＲＴＫ−ＧＮＳＳで利用可能な情報であり、補正データ受信装置２６と共通規格の通信機能を有する装置から送信された情報である。また、補正データ受信装置２６を電話回線用のモデムとし、補正データ配信サービスを利用して外部から補正データＣ１を得るようにしてもよい。補正データ受信装置２６は補正データＣ１をグローバル座標演算装置２３に出力する。ＧＮＳＳアンテナ２１、ＧＮＳＳアンテナ２２は、複数の測位衛星から信号を受信し、グローバル座標演算装置２３に出力する。
【００６０】
グローバル座標演算装置２３は、ＧＮＳＳアンテナ２１、ＧＮＳＳアンテナ２２から入力された測位衛星の信号と、補正データ受信装置２６から受信した補正データＣ１とに基づいて、ＧＮＳＳアンテナ２１の位置である基準位置データＰ１とＧＮＳＳアンテナ２２の位置である基準位置データＰ２を測位する。グローバル座標演算装置２３は、ＧＮＳＳアンテナ２１の位置である基準位置データＰ１とＧＮＳＳアンテナ２２の位置である基準位置データＰ２との相対位置から旋回体方位データＱを算出する。ＧＮＳＳアンテナ２１、２２及びグローバル座標演算装置２３によりＧＰＳコンパスを構成し、旋回体方位データＱを得るようにしてもよい。
【００６１】
ＧＮＳＳアンテナ２１、２２が設置される部分は、油圧ショベル１００の一部である。したがって、基準位置データＰ１、Ｐ２は、油圧ショベル１００の一部、具体的にはＧＮＳＳアンテナ２１、２２が設置される部分の位置を示す情報である。以下において、ＧＮＳＳアンテナ２１、２２が設置される部分の位置を、適宜、第１位置と称する。基準位置データＰ１、Ｐ２は、第１位置の情報である。
【００６２】
本実施形態において、旋回体方位データＱは、ＧＮＳＳアンテナ２１、２２が取得した基準位置データＰ、すなわち基準位置データＰ１及び基準位置データＰ２の少なくとも一方から決定される方位が、グローバル座標の基準方位（例えば北）に対してなす角、すなわち方位角である。方位角は、油圧ショベル１００のヨー角でもある。旋回体方位データＱは、上部旋回体３、すなわち作業機２が向いている方位を示している。
【００６３】
グローバル座標演算装置２３は、ＣＰＵ等のプロセッサである処理部と、ＲＡＭ及びＲＯＭ等の記憶装置である記憶部とを有する。グローバル座標演算装置２３は、測位した２つの基準位置データＰ１、Ｐ２、すなわち基準位置データＰと旋回体方位データＱを、装置コントローラ３９に出力する。
【００６４】
表示コントローラ２８は、ＣＰＵ等のプロセッサである処理部２８Ｐと、ＲＡＭ及びＲＯＭ等の記憶装置である記憶部２８Ｍとを有する。表示コントローラ２８は、表示部２９に、例えば、後述するガイダンス画面等の画像を表示する他、装置コントローラ３９から得られる油圧ショベル１００の位置情報ＩＰＬを用いて、バケット８の刃先８Ｔの３次元位置である刃先位置を示すバケット刃先位置データＳを生成する。表示部２９は、例えば、液晶表示装置等であるが、これに限定されるものではない。表示部２９は、例えば入力部と表示部を一体化したタッチパネルを用いることができる。本実施形態においては、表示部２９に隣接して、スイッチ２９Ｓが設置されている。スイッチ２９Ｓは、後述する掘削制御を実行させたり、実行中の掘削制御を停止させたりするための入力装置である。表示部２９にタッチパネルを用いる場合、スイッチ２９Ｓはタッチパネルの入力部に組み込まれてもよい。
【００６５】
表示コントローラ２８は、作業機２が掘削する対象の目標施工面の画像と、バケット刃先位置データＳを用いて生成したバケット８の画像とをガイダンス画面として表示部２９に表示させることができる。表示コントローラ２８は、ガイダンス画面により、目標施工面とバケット８との位置関係を油圧ショベル１００のオペレータに認識させ、情報化施工を行う際のオペレータの負担を軽減することができる。
【００６６】
ＩＭＵ２４は、油圧ショベル１００の動作を示す動作情報ＭＩを検出する状態検出装置である。油圧ショベル１００の動作は、上部旋回体３の動作及び走行装置５の動作の少なくとも一方を含む。本実施形態において、動作情報ＭＩは、油圧ショベル１００の姿勢を示す情報を含んでいてもよい。油圧ショベル１００の姿勢を示す情報は、油圧ショベル１００のロール角、ピッチ角及び方位角が例示される。
【００６７】
本実施形態において、ＩＭＵ２４は、油圧ショベル１００の角速度及び加速度を検出する。油圧ショベル１００の動作にともない、油圧ショベル１００には、走行時に発生する加速度、旋回時に発生する角加速度及び重力加速度といった様々な加速度が生じるが、ＩＭＵ２４は少なくとも重力加速度を含む加速度を検出し、各加速度の種類を区別することなく検出した加速度を出力する。重力加速度は、重力に対応した加速度である。ＩＭＵ２４は、図１に示される車体座標系（ｘ、ｙ、ｚ）において、ｘ軸方向、ｙ軸方向及びｚ軸方向の加速度ａと、ｘ軸、ｙ軸及びｚ軸周りの角速度（回転角速度）ωとを検出する。これらが動作情報ＭＩとなる。車体座標系とは、油圧ショベル１００を基準とした、（ｘ、ｙ、ｚ）で示される３次元座標系である。
【００６８】
ＩＭＵ２４が検出する動作情報ＭＩには、上部旋回体３の回転中心軸となるｚ軸を中心として上部旋回体３が旋回する際の角速度ωが含まれる。旋回時の角速度ωは、ＧＮＳＳアンテナ２１、２２の位置を示す情報から取得された上部旋回体３の旋回角度を時間で微分することにより求められてもよい。旋回時の角速度ωを時間で積分することにより、旋回角度を求めることができる。
【００６９】
ＩＭＵ２４は、上部旋回体３に取り付けられている。ＩＭＵ２４は、より高い精度で加速度等を検出するために、例えば、油圧ショベル１００の上部旋回体３の旋回中心軸上に設けられることが望ましいが、ＩＭＵ２４は運転室４の下部に設置されてもよい。
【００７０】
図３は、油圧ショベル１００の側面図である。図４は、油圧ショベル１００の背面図である。車両本体１の左右方向、すなわち幅方向に対する傾斜角θ４は油圧ショベル１００のロール角であり、車両本体１の前後方向に対する傾斜角θ５は油圧ショベル１００のピッチ角であり、ｚ軸周りにおける上部旋回体３の角度は油圧ショベル１００の方位角である。ロール角はＩＭＵ２４によって検出されたｘ軸周りの角速度を時間で積分することにより、ピッチ角はＩＭＵ２４によって検出されたｙ軸周りの角速度を時間で積分することにより、方位角はＩＭＵ２４によって検出されたｚ軸周りの角速度を時間で積分することにより求められる。ｚ軸周りの角速度は、油圧ショベル１００の旋回時の角速度ωである。すなわち、旋回時の角速度ωを時間で積分することにより油圧ショベル１００、より具体的には上部旋回体３の方位角が得られる。
【００７１】
ＩＭＵ２４は、所定の周期で油圧ショベル１００の加速度及び角速度を更新する。ＩＭＵ２４の更新周期は、グローバル座標演算装置２３における更新周期よりも短いことが好ましい。ＩＭＵ２４が検出した加速度及び角速度は、動作情報ＭＩとして装置コントローラ３９に出力される。装置コントローラ３９は、ＩＭＵ２４から取得した動作情報ＭＩにフィルタ処理及び積分といった処理を施して、ロール角である傾斜角θ４、ピッチ角である傾斜角θ５及び方位角を求める。装置コントローラ３９は、求めた傾斜角θ４、傾斜角θ５及び方位角を、油圧ショベル１００の位置に関連する位置情報ＩＰＬとして、表示コントローラ２８に出力する。
【００７２】
表示コントローラ２８は、グローバル座標演算装置２３から基準位置データＰ及び旋回体方位データＱを取得する。旋回体方位データＱは、油圧ショベル１００の方位を示す情報であり、本実施形態においては、上部旋回体３の方位を示す情報である。具体的には、旋回体方位データＱは、上部旋回体３の方位角である。本実施形態において、表示コントローラ２８は、作業機位置データとして、バケット刃先位置データＳを生成する。バケット刃先位置データＳは、装置コントローラ３９によって生成されてもよい。そして、表示コントローラ２８は、バケット刃先位置データＳと、目標施工情報Ｔとを用いて、掘削対象の目標形状を示す目標掘削地形データＵを生成する。目標施工情報Ｔは、表示コントローラ２８の記憶部２８Ｍ（目標施工情報格納部２８Ｃ）に記憶されている。目標施工情報Ｔは、油圧ショベル１００が備える作業機２の掘削対象の掘削後における仕上がりの目標となる情報であり、設計データから得られる目標施工面の情報を含む。目標掘削地形データＵは、車体座標系において刃先８Ｔの現時点における刃先位置を通る垂線と、目標施工面との交点を掘削対象位置としたとき、掘削対象位置の前後における単数又は複数の変曲点の位置を示す情報とその前後の線の角度情報である。
【００７３】
装置コントローラ３９は、第１ストロークセンサ１６が検出したブームシリンダ長から、車体座標系における水平面と直交する方向（ｚ軸方向）に対するブーム６の傾斜角θ１（図３参照）を算出する。装置コントローラ３９は、第２ストロークセンサ１７が検出したアームシリンダ長から、ブーム６に対するに対するアーム７の傾斜角θ２（図３参照）を算出する。装置コントローラ３９は、第３ストロークセンサ１８が検出したバケットシリンダ長から、アーム７に対するバケット８の傾斜角θ３を算出する。ＩＭＵ２４は、旋回時の角速度ωを装置コントローラ３９に出力する。
【００７４】
装置コントローラ３９は、前述したように、図１に示されるｚ軸を中心として上部旋回体３が旋回するときにおける上部旋回体３の旋回時の角速度ωを、ＩＭＵ２４から取得する。また、装置コントローラ３９は、圧力センサ６６からブーム操作信号ＭＢ、バケット操作信号ＭＴ、アーム操作信号ＭＡ及び旋回操作信号ＭＲを取得する。
【００７５】
装置コントローラ３９は、表示コントローラ２８から、目標掘削地形データＵを取得する。装置コントローラ３９は、自身が求めた作業機２の角度（θ１、θ２、θ３）から、バケット８の刃先８Ｔの位置（以下、適宜刃先位置と称する）を求める。装置コントローラ３９の記憶部３９Ｍは、作業機２のデータ（以下、適宜作業機データという）を記憶している。作業機データは、ブーム６の長さＬ１、アーム７の長さＬ２及びバケット８の長さＬ３といった設計寸法を含む。図３に示されるように、ブーム６の長さＬ１は、ブームピン１３からアームピン１４までの長さに相当する。アーム７の長さＬ２は、アームピン１４からバケットピン１５までの長さに相当する。バケット８の長さＬ３は、バケットピン１５からバケット８の刃先８Ｔまでの長さに相当する。刃先８Ｔは、図１に示す刃８Ｂの先端である。また、作業機データは、車体座標系の位置ＰＬに対するブームピン１３までの位置の情報を含む。装置コントローラ３９は、長さＬ１、Ｌ２、Ｌ３、傾斜角θ１、θ２、θ３及び位置ＰＬを用いて、位置ＰＬに対する刃先位置を求めることができる。
【００７６】
装置コントローラ３９は、目標掘削地形データＵに沿ってバケット８の刃先８Ｔが移動するように、操作装置２５から入力されたブーム操作信号ＭＢ、バケット操作信号ＭＴ及びアーム操作信号ＭＡを、目標掘削地形データＵとバケット８の刃先８Ｔとの距離及びバケット８の刃先８Ｔの速度に基づき調整する。装置コントローラ３９は、目標掘削地形データＵに沿ってバケット８の刃先８Ｔが移動するように作業機２を制御するための制御信号Ｎを生成して、図２に示される制御弁２７に出力する。このような処理により、作業機２が目標掘削地形データＵに近づく速度は、目標掘削地形データＵに対する距離に応じて制限される。
【００７７】
装置コントローラ３９からの制御信号Ｎに応じて、ブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２のそれぞれに対して２個ずつ設けられた制御弁２７が開閉する。左操作レバー２５Ｌ又は右操作レバー２５Ｒの操作と制御弁２７の開閉指令とに基づき、方向制御弁６４のスプールが動作して、ブームシリンダ１０、アームシリンダ１１及びバケットシリンダ１２へ作動油が供給される。
【００７８】
グローバル座標演算装置２３は、グローバル座標系におけるＧＮＳＳアンテナ２１、２２の基準位置データＰ１、Ｐ２を検出する。本実施形態において、グローバル座標系とは、例えば、ＧＮＳＳにおける座標系である。図３では、グローバル座標系は、（Ｘｇ、Ｙｇ、Ｚｇ）で示される３次元座標系である。現場座標系は、油圧ショベル１００の作業エリアＧＡに設置された基準となる、例えば基準杭６０の位置ＰＧを基準とした、（Ｘ、Ｙ、Ｚ）で示される３次元座標系である。図３に示されるように、位置ＰＧは、例えば、作業エリアＧＡに設置された基準杭６０の先端６０Ｔに位置する。グローバル座標系（Ｘｇ、Ｙｇ、Ｚｇ）と現場座標系（Ｘ、Ｙ、Ｚ）とは、互いに変換できる。
【００７９】
図２に示される表示コントローラ２８は、グローバル座標演算装置２３による検出結果に基づいて、グローバル座標系での車体座標系の位置を求める。本実施形態において、例えば、車体座標系の位置ＰＬは、旋回体の回転中心軸であるｚ軸と走行装置５が接地する面に相当する面との交点である。本実施形態において、位置ＰＬの座標は、車体座標系において、（０、０、０）となる。走行装置５が接地する面は、履帯５ａ、５ｂが接する作業エリアＧＡの表面ＧＤである。走行装置５が接地する面に相当する面は、作業エリアＧＡの表面ＧＤであってもよいし、履帯５ａ、５ｂが接地する部分によって規定される平面ＣＰであってもよい。履帯５ａ、５ｂが接地する部分によって規定される平面ＣＰは、車体座標系（ｘ、ｙ、ｚ）において、油圧ショベル１００の設計寸法から一義的に決定される。
【００８０】
位置ＰＬは、ｚ軸と平面ＣＰとの交点に限定されるものではない。本実施形態において、後述する疑似不動点の位置は、位置ＰＬと一致していてもよいし、一致していなくてもよい。車体座標系の位置ＰＬは、他の場所であってもよく、例えば、ブームピン１３の軸方向の長さの中心点を位置ＰＬとしてもよい。位置ＰＬは、ｚ軸上、かつ上部旋回体３が旋回するためのスイングサークル上に位置していてもよい。前述したように、装置コントローラ３９は、位置ＰＬに対する刃先位置、すなわち車体座標系での刃先位置を求めるので、グローバル座標系での位置ＰＬの座標が得られれば、車体座標系での刃先位置の座標を、グローバル座標系での刃先位置の座標に変換することができる。
【００８１】
装置コントローラ３９は、バケット８が目標掘削地形を侵食することを抑制するために、作業機２が掘削対象に接近する方向の速度が制限速度以下になるように制御する。この制御を、適宜掘削制御という。掘削制御は、表示コントローラ２８から取得された目標掘削地形データＵ及びバケット刃先位置データＳとに基づいて、作業機２と掘削対象との相対位置を演算しながら作業機２が掘削対象に接近する方向の速度を制限速度以下になるようにする制御である。このような制御を実行することで、掘削対象を目標形状（目標施工情報Ｔが示す形状）に施工することができる。次に、制御システム２００について、より詳細に説明する。
【００８２】
＜制御システム２００＞
図５は、実施形態１に係る制御システム２００の制御ブロック図である。本実施形態において、制御システム２００の装置コントローラ３９と表示コントローラ２８とは、信号線を介して互いに情報をやり取りすることができる。また、装置コントローラ３９は、グローバル座標演算装置２３から信号線を介して情報を取得することができる。制御システム２００内で情報を伝達する信号線は、ＣＡＮ（Controller  Area  Network）のような車内信号線が例示される。本実施形態において、制御システム２００は、装置コントローラ３９と、表示コントローラ２８とが別個の装置であるが、両者は１つの装置で実現されてもよい。
【００８３】
表示コントローラ２８は、刃先位置算出部２８Ａと、目標掘削地形データ生成部２８Ｂと、目標施工情報格納部２８Ｃとを有する。刃先位置算出部２８Ａ及び目標掘削地形データ生成部２８Ｂは、記憶部２８Ｍに記憶されたコンピュータプログラムを処理部２８Ｐが実行することにより実現される。目標施工情報格納部２８Ｃは、記憶部２８Ｍの記憶領域の一部によって実現される。
【００８４】
刃先位置算出部２８Ａは、装置コントローラ３９から取得する位置情報ＩＰＬに基づいて、上部旋回体３の旋回中心軸となるｚ軸を通る、油圧ショベル１００の旋回中心の位置を示す旋回中心位置データＸＲを生成する。刃先位置算出部２８Ａが装置コントローラ３９から取得する位置情報ＩＰＬは、基準位置データＰ１、Ｐ２に基づく基準位置データＰ１ｃ、Ｐ２ｃ及び油圧ショベル１００の姿勢角を含む。姿勢角は、ロール角θ４、ピッチ角θ５及び方位角θｄｃである。
【００８５】
刃先位置算出部２８Ａは、旋回中心位置データＸＲと作業機２の傾斜角θ１、θ２、θ３と、ブーム６の長さＬ１、アーム７の長さＬ２及びバケット８の長さＬ３とに基づいて、バケット８の刃先８Ｔの現在位置を示すバケット刃先位置データＳを生成し、目標掘削地形データ生成部２８Ｂに出力する。バケット刃先位置データＳは、作業機２の位置を示す情報である。本実施形態において、作業機２の位置は、刃先位置、すなわちバケット８の刃先８Ｔの３次元位置に限定されるものではなく、作業機２の特定の部分の位置であればよい。例えば、作業機２の位置は、バケット８の尻の部分の位置であってもよいし、法面バケットの底の部分の位置であってもよいし、作業機２のアタッチメントを取り付ける部分の位置であってもよい。
【００８６】
目標掘削地形データ生成部２８Ｂは、目標施工情報格納部２８Ｃに格納された目標施工情報Ｔと、刃先位置算出部２８Ａからのバケット刃先位置データＳと、を取得する。目標掘削地形データ生成部２８Ｂは、車体座標系において刃先８Ｔの現時点における刃先位置を通る垂線と目標施工面との交点を掘削対象位置として設定する。目標掘削地形データ生成部２８Ｂは、目標施工情報Ｔとバケット刃先位置データＳとに基づいて、目標掘削地形データＵを生成し、後述する装置コントローラ３９の処理部３９Ｐが有する作業機制御部３９Ｃに出力する。
【００８７】
装置コントローラ３９の処理部３９Ｐは、姿勢角演算部３９Ａと、位置情報演算部３９Ｂと、作業機制御部３９Ｃとを有する。姿勢角演算部３９Ａ、位置情報演算部３９Ｂ及び作業機制御部３９Ｃは、記憶部３９Ｍに記憶されたコンピュータプログラムを処理部３９Ｐが実行することにより実現される。本実施形態において、作業機制御部３９Ｃは、装置コントローラ３９とは別個の制御装置であってもよい。
【００８８】
姿勢角演算部３９Ａには、ＩＭＵ２４の検出値である加速度ａ（ａｘ、ａｙ、ａｚ）及び角速度ω（ωｘ、ωｙ、ωｚ）、すなわち動作情報ＭＩと、グローバル座標演算装置２３の検出値である旋回体方位データＱ（方位角θｄａ）とが入力される。また、処理部３９Ｐの姿勢角演算部３９Ａ及び位置情報演算部３９Ｂには、圧力センサ６６、２７ＰＣの検出値ＳＴｒ、ＳＴｄが入力される。
【００８９】
グローバル座標演算装置２３は、電波の受信状態又は装置コントローラ３９との通信の状態を示す情報である状態情報ＳＲを生成して、装置コントローラ３９の処理部３９Ｐ及び表示コントローラ２８の処理部２８Ｐに出力する。状態情報ＳＲは、グローバル座標演算装置２３が電波を受信できなくなった場合、電波の受信状態が低下した場合又はグローバル座標演算装置２３と装置コントローラ３９との通信に不良が発生した場合等に、それぞれの場合の受信状態又は通信状態を示す情報を有する。受信状態又は通信状態を示す情報は、グローバル座標演算装置２３による測位の状態を示す。測位の状態には、前述したように、測位の精度がよい状態（Ｆｉｘ）、測位不可能の状態（非測位）、測位はできるが情報が少ない状態及び測位の精度が悪い状態（Ｆｌｏａｔ、単独測位）等が挙げられる。このように、グローバル座標演算装置２３は、ＲＴＫ−ＧＮＳＳによる測位に不具合が発生したか否かを判定する測位状態判断装置である。
【００９０】
本実施形態において、測位状態判断装置、すなわちグローバル座標演算装置２３は、測位の精度が良い状態（Ｆｉｘ）を測位の状態が正常であると判定する。また、グローバル座標演算装置２３は、測位不可能の状態（非測位）、測位はできるが情報が少ない状態及び測位の精度が悪い状態（Ｆｌｏａｔ、単独測位）を、測位の状態が異常であると判定する。すなわち、グローバル座標演算装置２３は、測位の状態がＦｉｘである場合を正常として判定し、Ｆｉｘ以外の状態である場合を異常として判定する。
【００９１】
表示コントローラ２８は、状態情報ＳＲを取得すると、測位の状態に応じた情報を、図２に示される表示部２９に表示する。状態情報ＳＲが測位不可能の状態を示す場合、表示コントローラ２８は、図２に示される表示部２９に、ＲＴＫ−ＧＮＳＳによる測位に異常が発生したことを表示する。
【００９２】
姿勢角演算部３９Ａは、ＩＭＵ２４の検出値から、油圧ショベル１００のロール角である傾斜角θ４と、油圧ショベル１００のピッチ角である傾斜角θ５とを求めて、位置情報演算部３９Ｂ及び表示コントローラ２８の刃先位置算出部２８Ａに出力する。姿勢角演算部３９Ａは、ＩＭＵ２４が検出したｚ軸周りの角速度ωを積分して方位角θｄｉを求めることができる。ロール角θ４、ピッチ角θ５及び方位角θｄｉが姿勢角である。
【００９３】
姿勢角演算部３９Ａは、位置検出装置であるグローバル座標演算装置２３の状態に応じて、自身が求めた方位角θｄｉ又はグローバル座標演算装置２３から取得した方位角θｄａを切り替えて、表示コントローラ２８の刃先位置算出部２８Ａ又は位置情報演算部３９Ｂに方位角θｄｃとして出力する。つまり、ＲＴＫ−ＧＮＳＳによる測位が正常である場合には、グローバル座標演算装置２３から取得した方位角θｄａを用いてバケット刃先位置データＳが求められ、ＲＴＫ−ＧＮＳＳによる測位が異常である場合には、ＩＭＵ２４が検出したｚ軸周りの角速度ωを積分して求めた方位角θｄｉを用いてバケット刃先位置データＳが求められる。また、姿勢角演算部３９Ａから表示コントローラ２８に送られる傾斜角θ４、傾斜角θ５及び方位角θｄｃは、油圧ショベル１００の位置に関連する位置情報ＩＰＬである。以下において、傾斜角θ４を適宜ロール角θ４と称し、傾斜角θ５を適宜ピッチ角θ５と称する。
【００９４】
本実施形態において、位置情報ＩＰＬは、前述したように、作業機械である油圧ショベル１００の位置に関連する情報である。位置情報ＩＰＬには、油圧ショベル１００の位置そのものの情報の他、油圧ショベル１００の位置を求めるために必要な情報も含む。油圧ショベル１００の位置そのものの情報は、基準位置データＰ１、Ｐ２及びバケット刃先位置データＳが例示され、油圧ショベル１００の位置を求めるために必要な情報は、傾斜角θ４、傾斜角θ５及び方位角（θｄａ、θｄｉ又はθｄｃ）が例示される。
【００９５】
位置情報演算部３９Ｂは、グローバル座標演算装置２３から取得した基準位置データＰ１、Ｐ２と、ＩＭＵ２４から取得した動作情報ＭＩとを用いて、基準位置データＰ１、Ｐ２が示す位置に相当する位置を求める。基準位置データＰ１、Ｐ２は、第１位置の情報である。以下において、位置情報演算部３９Ｂが基準位置データＰ１、Ｐ２及び動作情報ＭＩから求めた位置を、適宜第２位置と称する。第２位置の情報は、基準位置データＰ１ｉ、Ｐ２ｉである。基準位置データＰ１ｉ、Ｐ２ｉは、位置情報演算部３９Ｂが生成する。以下において、グローバル座標演算装置２３が出力した基準位置データＰ１、Ｐ２を適宜、第１基準位置データＰ１、Ｐ２と称し、位置情報演算部３９Ｂによって生成された基準位置データＰ１ｉ、Ｐ２ｉを適宜、第２基準位置データＰ１ｉ、Ｐ２ｉと称する。
【００９６】
第２基準位置データＰ１ｉ、Ｐ２ｉは、ＩＭＵ２４の検出値である加速度ａ（ａｘ、ａｙ、ａｚ）及び角速度ω（ωｘ、ωｙ、ωｚ）から姿勢角演算部３９Ａが求めたロール角θ４及びピッチ角θ５と、姿勢角演算部３９Ａから出力される方位角θｄｃとによって求められる。この方位角θｄｃは、姿勢角演算部３９Ａがグローバル座標演算装置２３から取得した方位角θｄａ又は方位角θｄａに旋回時の角速度ωの積分によって得られた角度を加算した方位角である。この場合、グローバル座標演算装置２３は、第１基準位置データＰ１、Ｐ２から方位角θｄａを求めて、姿勢角演算部３９Ａに出力する。旋回時の角速度ωが０である場合、姿勢角演算部３９Ａから出力される方位角θｄｃは、グローバル座標演算装置２３から取得した方位角θｄａと等しい。このように、位置情報演算部３９Ｂは、第１基準位置データＰ１、Ｐ２及び動作情報ＭＩを用いて第２基準位置データＰ１ｉ、Ｐ２ｉを生成する。本実施形態において、姿勢角演算部３９Ａは、グローバル座標演算装置２３から第１基準位置データＰ１、Ｐ２を取得し、これを用いて方位角θｄａを求めてもよい。
【００９７】
測位衛星の配置、電離層、対流圏又はＧＮＳＳアンテナ周辺の地形の影響により、グローバル座標演算装置２３による測位結果がばらつくことがある。測位結果がばらつくと、掘削制御においては施工面が波打って設計面の通りに施工されない可能性がある。また、測位結果がばらつくと、ガイダンス画面に表示されるバケット８の刃先が揺らぎ、オペレータの視認性が低下する可能性がある。図２に示される操作装置２５がパイロット圧方式である場合、左操作レバー２５Ｌ又は右操作レバー２５Ｒに油激が発生して、オペレータが違和感を覚える可能性もある。
【００９８】
グローバル座標演算装置２３から出力される第１基準位置データＰ１、Ｐ２に、ローパスフィルタ処理又は移動平均のような平滑化処理を施すことにより、グローバル座標演算装置２３の測位結果のばらつきによる影響を低減することが考えられる。油圧ショベル１００は、掘削時における姿勢角の変動によってＧＮＳＳアンテナ２１、２２の位置も変動する。このため、第１基準位置データＰ１、Ｐ２が直接平滑化処理されると、平滑化処理後におけるＧＮＳＳアンテナ２１、２２の位置の算出において平滑化処理をすることによる時間遅れが発生する。その結果、平滑化処理後におけるＧＮＳＳアンテナ２１、２２の位置が、現実のＧＮＳＳアンテナ２１、２２の位置とは異なる可能性がある。
【００９９】
油圧ショベル１００が作業のために稼働している時間において絶対位置が変化しない不動点が油圧ショベル１００に存在し、ＧＮＳＳアンテナ２１、２２の位置と不動点との相対関係が判れば、制御システム２００の装置コントローラ３９は不動点の絶対位置をＧＮＳＳアンテナ２１、２２の位置から算出できる。装置コントローラ３９は、不動点の絶対位置に平滑化処理を適用し、ばらつきが低減された不動点を得る。装置コントローラ３９は、平滑化処理を適用した後の不動点の絶対位置からＧＮＳＳアンテナ２１、２２の位置を計算すれば、平滑化処理の時間遅れが影響することなくＲＴＫ−ＧＮＳＳによる測位のばらつきを低減できる。
【０１００】
実際には油圧ショベル１００のエンジン３５が稼働している限り、作業機２の動作等により振動が発生するため、不動点と見なせる近似的な位置が疑似不動点として選定される。制御システム２００の装置コントローラ３９は、選定された疑似不動点を、前述した不動点と同様に取り扱うことで、疑似不動点を用いてＲＴＫ−ＧＮＳＳによる測位をバックアップすることができる。疑似不動点を不動点と見なすことができるのは、油圧ショベル１００が移動していない場合、すなわち図１に示される履帯５ａ、５ｂが動いていない場合である。
【０１０１】
本実施形態において、制御システム２００、より具体的には制御システム２００の装置コントローラ３９は、前述した疑似不動点に平滑化処理を施し、平滑化処理を施した疑似不動点を用いて、ＧＮＳＳアンテナ２１、２２の位置、すなわち第２位置を求める。疑似不動点は、後述するように、油圧ショベル１００が作業のために稼働している時間において、時間の経過によって絶対位置が変化しないと見なすことができるので、平滑化処理による遅れの影響は無視できる。その結果、装置コントローラ３９は、平滑化処理後におけるＧＮＳＳアンテナ２１、２２の位置と、現実のＧＮＳＳアンテナ２１、２２の位置とを一致させることができる。このように、装置コントローラ３９は、疑似不動点に平滑化処理を施すことにより、グローバル座標演算装置２３の測位結果のばらつきによる影響を低減することができる。その結果、装置コントローラ３９は、掘削制御を用いた施工時の精度低下及びガイダンス画面の視認性の低下を抑制できる。
【０１０２】
位置情報演算部３９Ｂは、図２に示される左操作レバー２５Ｌのパイロット圧を検出する圧力センサ６６の検出値ＳＴｒ（前述した旋回操作信号ＭＲに対応）及び走行用レバー２５ＦＬ及び走行用レバー２５ＦＲのパイロット圧を検出する圧力センサ２７ＰＣの検出値ＳＴｄ（前述した操作信号ＭＤに対応）を取得する。位置情報演算部３９Ｂは、取得した検出値ＳＴｒ、Ｓｔｄに基づいて、平滑化処理を実行するか否かを始めとした各種の判定を行う。
【０１０３】
次に、疑似不動点について説明する。本実施形態において、疑似不動点は、図３及び図４に示される油圧ショベル１００の位置ＰＬである。なお、前述のように車体座標系の原点として位置ＰＬを定めているが、他の位置に車体座標系の原点を定めてもよい。したがって、疑似不動点は、以下の説明において特定点と称することもある。作業中の油圧ショベル１００に回転（以下、旋回と称することもある）が生ずるような動きがあった場合、その回転の支点は動かないので、疑似不動点がその支点にあれば、制御システム２００によって求められる位置、例えば、作業機２の特定の部分の位置又はバケット８の刃先８Ｔの位置を含む作業機２の位置の誤差が最も小さくなる。疑似不動点を回転の支点とすることができない場合でも、疑似不動点をできるだけ支点の近くに設定すれば、制御システム２００によって求められる位置（作業機２の位置）の誤差を小さくできる。
【０１０４】
上部旋回体３が旋回する際の支点は旋回中心軸、すなわちｚ軸であるので、疑似不動点をｚ軸上にする。ロール角θ４の方向及びピッチ角θ５の方向における回転の支点は一定点ではないが、必ず油圧ショベル１００が接地する面上にあると考えられる。本実施形態において、前述したように、位置ＰＬは、旋回体の回転中心軸であるｚ軸と走行装置５が接地する面に相当する面との交点である。本実施形態では、疑似不動点を、油圧ショベル１００が接地する面上とすることで、作業中の油圧ショベル１００に回転が生ずるような動きがあった場合でも、疑似不動点は不動であると考えられる。このため、ＲＴＫ−ＧＮＳＳによる測位のばらつきが発生した際において、制御システム２００によって求められる位置、より具体的にはＧＮＳＳアンテナ２１、２２の絶対位置のばらつきを低減できる。
【０１０５】
油圧ショベル１００は様々な作業を行うことができるが、前述のように油圧ショベル１００に回転が生じても疑似不動点は不動であると考えられる場合として、例えば、法面の施工等の作業がある。この場合、走行装置５は停止したまま、作業機２又は上部旋回体３の操作だけで掘削又は均しが行われることがある。情報化施工を可能とする油圧ショベル１００を用いて、このような法面施工等の施工を行う場合、制御システム２００は、疑似不動点及びＲＴＫ−ＧＮＳＳによる測位結果を用いて油圧ショベル１００の第２位置、具体的にはＧＮＳＳアンテナ２１、２２の位置を求める。このようにすることで、制御システム２００は、掘削制御の精度低下及びガイダンス画面の視認性の低下を抑制できる。
【０１０６】
＜疑似不動点の求め方＞
油圧ショベル１００の制御システム２００、具体的には装置コントローラ３９が、ＧＮＳＳアンテナ２１、２２の絶対位置から疑似不動点を求める方法、及び疑似不動点からＧＮＳＳアンテナ２１、２２の絶対位置を求め直す方法を説明する。
【０１０７】
式（１）は、車体座標系における位置ＰＬの位置ベクトルとＧＮＳＳアンテナ２１、２２の位置ベクトルとの差分を、図３に示される現場座標系（Ｘ、Ｙ、Ｚ）における位置ＰＬの位置ベクトルとＧＮＳＳアンテナ２１、２２の位置ベクトルとの差分に変換する式である。式（２）は、現場座標系におけるＧＮＳＳアンテナ２１、２２の位置ベクトルの測定値Ｒａｌから現場座標系における位置ＰＬの位置ベクトルＲｆｌを計算する式であり、式（１）の変形である。式（３）は、現場座標系における位置ＰＬの位置ベクトルＲｆｌから現場座標系におけるＧＮＳＳアンテナ２１、２２の位置ベクトルの測定値Ｒａｌを、現場座標系におけるＧＮＳＳアンテナ２１、２２の位置ベクトルの計算値Ｒａｌｃを求める式として表したものである。
【０１０８】
Ｒｆｌ−Ｒａｌ＝Ｃｌｂ（Ｒｆｂ−Ｒａｂ）・・・（１）
Ｒｆｌ＝Ｃｌｂ（Ｒｆｂ−Ｒａｂ）＋Ｒａｌ・・・（２）
Ｒａｌｃ＝Ｃｌｂ（Ｒａｂ−Ｒｆｂ）＋Ｒｆｌ・・・（３）
ここで、
Ｒｆｂ：車体座標系における位置ＰＬの位置ベクトルの校正値
Ｒａｂ：車体座標系におけるＧＮＳＳアンテナ２１、２２の位置ベクトルの校正値
Ｒｆｌ：現場座標系における位置ＰＬの位置ベクトルの計算値
Ｒａｌ：現場座標系におけるＧＮＳＳアンテナ２１、２２の位置ベクトルの測定値
Ｒａｌｃ：現場座標系におけるＧＮＳＳアンテナ２１、２２の位置ベクトルの計算値
Ｃｌｂ：車体座標系から現場座標系への座標回転行列
【０１０９】
校正値とは、油圧ショベル１００の各位置及び寸法を計測することによって得られた、位置ＰＬ及びＧＮＳＳアンテナ２１、２２の位置の値であり、装置コントローラ３９の記憶部３９Ｍ及び表示コントローラ２８の記憶部２８Ｍの少なくとも一方に記憶されている。校正値は、油圧ショベル１００の設計寸法に基づくものでもよいが、設計寸法は、油圧ショベル１００毎にばらつきが生じる可能性がある。このため、校正値は、計測（キャリブレーション）に基づいて得られたものが好ましい。
【０１１０】
座標回転行列Ｃｌｂは、ロール角θ４、ピッチ角θ５及びヨー角、すなわち方位角θｄを用いて式（４）のように表現される。ロール角θ４、ピッチ角θ５及び方位角θｄは、ＩＭＵ２４によって検出されたｘ軸周りの角速度ωｘ、ｙ軸周りの角速度ωｙ及びｚ軸周りの角速度ωｚを姿勢角演算部３９Ａが時間で積分されることによって求められる。式（４）中のｓｘはｓｉｎθ４、ｓｙはｓｉｎθ５、ｓｚはｓｉｎθｄ、ｃｘはｃｏｓθ４、ｃｙはｃｏｓθ５、ｃｚはｃｏｓθｄである。
【数１】
0005909029.tif 000002
【０１１１】
制御システム２００は、式（２）を用いることにより、疑似不動点である特定点（本実施形態では位置ＰＬ）の位置を求めることができる。また、制御システム２００は、式（３）を用いることにより、疑似不動点である特定点の位置を用いて、ＧＮＳＳアンテナ２１、２２の絶対位置、すなわち現場座標系における位置又はグローバル座標系における位置を求めることができる。制御システム２００は、式（２）及び式（３）を用いることによって、ＧＮＳＳアンテナ２１、２２の絶対位置を求めることができる。
【０１１２】
＜平滑化処理＞
本実施形態において、装置コントローラ３９は、疑似不動点である特定点の位置に平滑化処理を施す。本実施形態において、平滑化処理には、例えば、式（５）で示されるローパスフィルタが用いられる。
Ｒｆｔ＝｛（Ｍ−１）×Ｒｆｔｐｒ＋Ｒｆｌ｝／Ｍ・・・（５）
【０１１３】
式（５）中のＲｆｔは、今回の制御周期におけるローパスフィルタの出力であり、Ｒｆｔｐｒは前回の制御周期におけるローパスフィルタ（以下、適宜フィルタと称する）の出力である。これらは、いずれも特定点の位置ベクトルである。Ｍは、平均化定数である。本実施の形態において、平均化定数Ｍの初期値は１であり、Ｍの値が設定値Ｍｍａｘに到達するまで、１回の制御周期が終了する毎に、Ｍは１ずつ増加する。
【０１１４】
本実施形態において、装置コントローラ３９は、平滑化処理を開始すると、前回の制御周期におけるフィルタの出力Ｒｆｔｐｒを記憶部３９Ｍに一時的に記憶させる。記憶部３９Ｍは、次の制御周期のフィルタ処置が実行されるまで又は実行中の平滑化処理がリセットされるまで、前回の制御周期におけるフィルタの出力Ｒｆｔｐｒを記憶する。
【０１１５】
装置コントローラ３９は、式（２）を用いて、特定点の位置を示す位置ベクトルＲｆｌを求め、得られた位置ベクトルＲｆｌを式（５）に与える。装置コントローラ３９は、式（５）により、制御の１周期毎に、特定点の位置ベクトルＲｆｌに平滑化処理、具体的にはローパスフィルタ処理を施す。装置コントローラ３９は、ローパスフィルタ処理の後、平滑化処理後における特定点の位置ベクトルとして、今回の制御周期におけるローパスフィルタの出力Ｒｆｔを出力する。平滑化処理後における特定点の位置ベクトルを、以下においては適宜位置ベクトルＲｆｔと称する。位置ベクトルＲｆｔは、第２基準位置データＰ１ｉ、Ｐ２ｉである。第２基準位置データＰ１ｉ、Ｐ２ｉは、平滑化処理によって得られた情報である。このように、装置コントローラ３９は、式（５）で示されるローパスフィルタの機能を実現することにより、特定点の位置に平滑化処理を施し、平滑化処理後の特定点の位置を用いて、第２位置を求める。
【０１１６】
装置コントローラ３９は、初回の平滑化処理又は平滑化処理をリセットした後において、前回の制御周期におけるフィルタの出力Ｒｆｔｐｒを式（５）のＲｆｌとし、平均化定数Ｍを１とする。初回の平滑化処理とは、装置コントローラ３９が平滑化処理を開始する場合に、前回の制御周期におけるフィルタの出力Ｒｆｔｐｒを有していない場合である。
【０１１７】
装置コントローラ３９は、何らかの理由で平滑化処理を中断した場合、今回の制御周期におけるフィルタの出力Ｒｆｔを出力せず、前回の制御周期におけるフィルタの出力Ｒｆｔｐｒ及び前回の制御周期における平均化定数Ｍを保持する。この場合、装置コントローラ３９は、記憶部３９Ｍに、前回の制御周期におけるフィルタの出力Ｒｆｔｐｒに加えて、前回の制御周期における平均化定数Ｍを一時的に記憶させる。装置コントローラ３９は、中断していた平滑化処理を再開する場合、現在の制御周期における特定点の位置ベクトルＲｆｌと、記憶部３９Ｍに記憶していた、中断前におけるフィルタの出力Ｒｆｔｐｒ及び平均化定数Ｍとを式（５）に与える。この処理により、装置コントローラ３９は、特定点の位置ベクトルＲｆｌを平滑化する。
【０１１８】
装置コントローラ３９は、グローバル座標演算装置２３の測位結果の異常値を回避するため、平滑化処理を開始した場合（初回の平滑化処理の開始を除く）又は平滑化処理の中断から復帰する場合に、判定処理を実行する。判定処理を実行するにあたり、装置コントローラ３９は、式（６）を用いて差分ΔＲｆｌを求める。式（６）中、Ｒｆｌは、今回の制御周期における特定点の位置ベクトルであり、Ｒｆｔｐｒは、平滑化処理の開始前又は平滑化処理の中断から復帰、すなわち再開する時点において記憶部３９Ｍに記憶されているフィルタの出力である。
ΔＲｆｌ＝｜Ｒｆｌ−Ｒｆｔｐｒ｜・・・（６）
【０１１９】
判定処理において、装置コントローラ３９は、差分ΔＲｆｌが予め定められた閾値未満であれば、今回の制御周期における特定点の位置ベクトルＲｆｌは正常であると判定して、今回の制御周期における特定点の位置ベクトルＲｆｌを用いて平滑化処理を実行する。装置コントローラ３９は、差分ΔＲｆｌが予め定められた閾値以上であれば、今回の制御周期における特定点の位置ベクトルＲｆｌは異常であると判定する。このように、異常と判定された場合、装置コントローラ３９は、今回の制御周期における特定点の位置ベクトルＲｆｌの代わりに、記憶部３９Ｍに記憶されているフィルタの出力Ｒｆｔｐｒを用いて、式（５）のフィルタの出力Ｒｆｔを求める。このような処理により、装置コントローラ３９は、グローバル座標演算装置２３の測位結果に異常値が発生した場合には、異常値に起因したバケット８の刃先位置の変動を抑制できる。差分ΔＲｆｌが予め定められた閾値以上となる状態が、予め定められた設定値Ｎｔ秒継続した場合、装置コントローラ３９は、タイムアウト処理を実行する。具体的には、装置コントローラ３９は、平滑化処理をリセットする。
【０１２０】
グローバル座標演算装置２３の測位結果に異常値が発生する場合には、グローバル座標演算装置２３が出力する第１基準位置データＰ１、Ｐ２の座標値が異常値を示す場合、グローバル座標演算装置２３と装置コントローラ３９との間で通信エラーが発生した場合及びＲＴＫ−ＧＮＳＳによる測位に不具合が発生した場合のいずれか一つが発生した場合、又は複数が同時に発生した場合が含まれる。ＧＮＳＳアンテナ２１、２２が測位衛星からの電波を受信できなくなる又は電波を受信し難くなることにより、ＲＴＫ−ＧＮＳＳによる測位に不具合が発生する。
【０１２１】
本実施形態において、ローパスフィルタは、式（５）に示されるものに限定されない。平滑化処理は、ローパスフィルタには限定されず、例えば、特定点の位置を移動平均する処理であってもよい。
【０１２２】
本実施形態において、装置コントローラ３９は、ＲＴＫ−ＧＮＳＳによる測位が正常である場合に平滑化処理を実行する。装置コントローラ３９が特定点に平滑化処理を施すにあたり、装置コントローラ３９が有する姿勢角演算部３９Ａは、ロール角θ４、ピッチ角θ５及び方位角θｄｃを求め、位置情報演算部３９Ｂ及び表示コントローラ２８の刃先位置算出部２８Ａに出力する。グローバル座標演算装置２３が受信した基準位置データＰ１、Ｐ２によって方位角θｄｃ、すなわち旋回体方位データＱが得られるため、車体座標系について求められた作業機２の位置を、現場座標系における作業機２の位置として求めることができる。
【０１２３】
図６は、油圧ショベルの姿勢を示す平面図である。姿勢角演算部３９Ａによって求められた方位角θｄｃは、現場座標系（Ｘ、Ｙ、Ｚ）のＹ軸に対する上部旋回体３の前後軸であるｘ軸の傾きを表す。方位角θｄｃによって油圧ショベル１００の方位Ｄ１が定まる。
【０１２４】
位置情報演算部３９Ｂは、姿勢角演算部３９Ａによって求められたロール角θ４、ピッチ角θ５及び方位角θｄｃから座標回転行列Ｃｌｂを求める。この場合、位置情報演算部３９Ｂは、姿勢角演算部３９Ａによって求められた方位角θｄｃを式（４）のθｄに与えて、座標回転行列Ｃｌｂを求める。また、位置情報演算部３９Ｂは、ＲＴＫ−ＧＮＳＳによる測位が正常である状態でグローバル座標演算装置２３から取得した基準位置データＰ１、Ｐ２から現場座標系におけるＧＮＳＳアンテナ２１、２２の位置ベクトルの測定値Ｒａｌを求める。そして、位置情報演算部３９Ｂは、求めた座標回転行列Ｃｌｂと位置ベクトルの測定値Ｒａｌとを式（２）に与えて、現場座標系における位置ＰＬの位置ベクトルＲｆｌを求める。位置ベクトルＲｆｌは計算値である。
【０１２５】
位置ベクトルＲｆｌが得られたら、位置情報演算部３９Ｂは、式（５）に位置ベクトルＲｆｌを与えることにより、位置ベクトルＲｆｌに平滑化処理を施す。位置情報演算部３９Ｂは、平滑化処理後における位置ベクトルＲｆｌ、すなわちローパスフィルタの出力Ｒｆｔを式（３）のＲｆｌとして与えて、現場座標系におけるＧＮＳＳアンテナ２１、２２の位置ベクトル、すなわち第２基準位置データＰ１ｉ、Ｐ２ｉを求める。現場座標系におけるＧＮＳＳアンテナ２１、２２の位置ベクトルは、式（３）に示される計算値Ｒａｌｃである。位置情報演算部３９Ｂは、第２基準位置データＰ１ｉ、Ｐ２ｉを、基準位置データＰ１ｃ、Ｐ２ｃとして表示コントローラ２８の刃先位置算出部２８Ａに出力する。
【０１２６】
次に、装置コントローラ３９の処理部３９Ｐが有する作業機制御部３９Ｃについて説明する。作業機制御部３９Ｃは、表示コントローラ２８から取得した目標掘削地形データＵに基づいて、作業機２が目標掘削地形データＵに近づく速度を制御するための制御信号Ｎを生成する。作業機制御部３９Ｃは、生成した制御信号Ｎを制御弁２７に与え、制御弁２７を開閉することにより、作業機２が目標掘削地形データＵに近づく速度を制御する。
【０１２７】
図７は、実施形態１に係る装置コントローラ３９が有する位置情報演算部３９Ｂを示す図である。位置情報演算部３９Ｂは、判定部４０Ａと、特定点演算部４０Ｂと、平滑化処理部４０Ｃと、位置計算部４０Ｄとを含む。判定部４０Ａは、装置コントローラ３９が平滑化処理を実行するか停止させるか、実行中の平滑化を中断するか否か、中断中の平滑化処理を再開させるか否か、及び平滑化処理をリセットするかについて判定する。これらの判定は、圧力センサ６６の検出値ＳＴｒ及び圧力センサ２７ＰＣの検出値ＳＴｄに基づいて行われる。
【０１２８】
特定点演算部４０Ｂは、式（２）を用いて特定点の位置ベクトルＲｆｌを求める。平滑化処理部４０Ｃは、式（５）を用いて、特定点演算部４０Ｂによって求められた特定点の位置ベクトルＲｆｌに平滑化処理を施す。位置計算部４０Ｄは、平滑化処理後における位置ベクトルＲｆｔを式（３）のＲｆｌに与えて第２基準位置データＰ１ｉ、Ｐ２ｉを求め、これらを基準位置データＰ１ｃ、Ｐ２ｃとして表示コントローラ２８に出力する。次に、本実施形態に係る制御システム２００が平滑化処理を実行して、バケット８の刃先位置を求める処理の一例を説明する。
【０１２９】
＜制御システム２００の処理の一例＞
図８は、実施形態１に係る制御システム２００の処理の一例を示すフローチャートである。ステップＳ１０１において、制御システム２００の装置コントローラ３９が有する位置情報演算部３９Ｂの判定部４０Ａは、装置コントローラ３９が平滑化処理を実行するために必要な実行条件が成立したか否かを判定する。実行条件は、ＲＴＫ−ＧＮＳＳによる測位が正常、かつ油圧ショベル１００が非走行、かつ上部旋回体３が非旋回である場合に成立する。
【０１３０】
開始条件が成立した場合（ステップＳ１０１、Ｙｅｓ）、装置コントローラ３９は、ステップＳ１０２において特定点を求める。具体的には、装置コントローラ３９が有する位置情報演算部３９Ｂの特定点演算部４０Ｂが、特定点、具体的には特定点の位置ベクトルＲｆｌを求める。ステップＳ１０３において、装置コントローラ３９は、特定点演算部４０Ｂによって求められた特定点の位置ベクトルＲｆｌに平滑化処理を施す。ステップＳ１０４において、装置コントローラ３９が有する位置情報演算部３９Ｂの位置計算部４０Ｄは、平滑化処理後における位置ベクトルＲｆｌである位置ベクトルＲｆｔを用いて第２基準位置データＰ１ｉ、Ｐ２ｉを求める。そして、位置計算部４０Ｄは、求めた第２基準位置データＰ１ｉ、Ｐ２ｉを基準位置データＰ１ｃ、Ｐ２ｃとして表示コントローラ２８に出力する。
【０１３１】
ステップＳ１０５において、表示コントローラ２８の処理部２８Ｐは、装置コントローラ３９から取得した油圧ショベル１００の位置情報ＩＰＬを用いて、バケット８の刃先８Ｔの３次元位置である刃先位置を求める。具体的には、処理部２８Ｐは、刃先位置を示すバケット刃先位置データＳを生成する。位置情報ＩＰＬは、基準位置データＰ１ｃ、Ｐ２ｃ、ロール角θ４、ピッチ角θ５及び方位角θｄｃである。次に、ステップＳ１０１に戻って説明する。ステップＳ１０１において、開始条件が成立しなかった場合（ステップＳ１０１、Ｎｏ）、装置コントローラ３９は処理を終了する。つまり、判定部４０Ａは、ＲＴＫ−ＧＮＳＳによる測位が正常であること、油圧ショベル１００が非走行であること、及び上部旋回体３が非旋回であることのうち、いずれか一つが成立していない場合、実行条件が成立していないと判定する。次に、平滑化処理の状態の遷移について説明する。
【０１３２】
＜平滑化処理の状態の遷移＞
図９は、平滑化処理の状態の遷移を説明するための図である。本実施形態において、平滑化処理は、状態１（ＯＮ、平滑化処理の実行）、状態２（ＯＦＦ、平滑化処理の停止）、状態３（中断、実行中の平滑化処理の中断）及び状態４（リセット、平滑化処理のリセット）の４つの状態の間を遷移する。
【０１３３】
装置コントローラ３９は、ＲＴＫ−ＧＮＳＳによる測位が正常、かつ油圧ショベル１００の走行が停止（非走行）、かつ上部旋回体３が非旋回である場合に、平滑化処理の状態を状態１に遷移させる。すなわち、状態１は、前述した実行条件が成立した場合における平滑化処理の状態である。状態１において、装置コントローラ３９は、第２位置の情報である第２基準位置データＰ１ｉ、Ｐ２ｉを用いて刃先位置を求める。
【０１３４】
装置コントローラ３９は、実行条件が成立しなかった場合、より具体的には油圧ショベルが走行している場合に、平滑化処理を状態１から状態２に遷移させる（I）。すなわち、装置コントローラ３９は、油圧ショベル１００が走行しているときは、第２位置を求める処理、すなわち平滑化処理を停止する。装置コントローラ３９は、状態２においてＲＴＫ−ＧＮＳＳによる測位が正常、かつ油圧ショベル１００が非走行、かつ上部旋回体３が非旋回である場合に、平滑化処理を状態２から状態１に遷移させる（I）。
【０１３５】
装置コントローラ３９は、状態１において、実行条件が成立しなくなった場合、より具体的にはＲＴＫ−ＧＮＳＳによる測位が異常になった場合及び上部旋回体３が旋回している場合の少なくとも一方が成立した場合に、平滑化処理を状態１から状態３に遷移させる（II）。状態３において、装置コントローラ３９は、第２位置の情報である第２基準位置データＰ１ｉ、Ｐ２ｉを求める処理、すなわち平滑化処理を中断する。装置コントローラ３９は、状態３においてＲＴＫ−ＧＮＳＳによる測位が正常、かつ油圧ショベル１００が非走行、かつ上部旋回体３が停止、すなわち非旋回である場合に、平滑化処理を状態３から状態１に遷移させる（II）。この場合、装置コントローラ３９は、中断していた平滑化処理を再開する。装置コントローラ３９は、中断していた平滑化処理を再開する場合、平滑化処理を中断する前に得られた第２基準位置データＰ１ｉ、Ｐ２ｉを用いて刃先位置を求める。
【０１３６】
装置コントローラ３９は、グローバル座標演算装置２３の測位結果の異常値を回避する処理又はタイムアウト処理を実行する場合に、平滑化処理を状態１から状態４に遷移させる（III）。装置コントローラ３９は、状態４においてＲＴＫ−ＧＮＳＳによる測位が正常、かつ油圧ショベル１００が非走行、かつ上部旋回体３が非旋回、かつ平滑化処理のリセットが完了した場合に、平滑化処理を状態４から状態１に遷移させる（III）。
【０１３７】
装置コントローラ３９は、平滑化処理の中断中に油圧ショベル１００が走行を開始した場合に、平滑化処理を状態３から状態２に遷移させる（IV）。すなわち、装置コントローラ３９は、第２位置の情報を求める処理である平滑化処理を停止する。装置コントローラ３９は、状態４において油圧ショベル１００が走行、又は上部旋回体３が旋回、又はＲＴＫ−ＧＮＳＳによる測位が異常である場合に、平滑化処理を状態４から状態２に遷移させる（V）。
【０１３８】
＜油圧ショベル１００の状態及びＲＴＫ−ＧＮＳＳによる測位の状態の判定＞
位置情報演算部３９Ｂが平滑化処理の状態を遷移させるにあたって、図７に示される位置情報演算部３９Ｂの判定部４０Ａは、油圧ショベル１００の状態及びＲＴＫ−ＧＮＳＳによる測位の状態を判定する。判定部４０Ａは、圧力センサ２７ＰＣが、走行用レバー２５ＦＬ及び走行用レバー２５ＦＲの少なくとも一方のパイロット圧を検出した場合、油圧ショベル１００は走行していると判定する。上部旋回体３を旋回操作するための操作レバーである左操作レバー２５Ｌが左右いずれかの方向に操作され、圧力センサ６６がパイロット圧を検出した場合、判定部４０Ａは、上部旋回体３は旋回していると判定する。判定部４０Ａは、グローバル座標演算装置２３が生成した状態情報ＳＲが、ＲＴＫ−ＧＮＳＳによる測位の状態が異常であることを示す場合、測位の状態が異常であると判定する。
【０１３９】
＜平滑化処理の状態を遷移させる処理＞
図１０は、装置コントローラ３９が平滑化処理の状態を遷移させる処理のフローチャートであって、特に平滑化処理の中断に関する処理を示すものである。ステップＳ２０１において、装置コントローラ３９が平滑化処理を実行している場合、装置コントローラ３９が有する位置情報演算部３９Ｂの判定部４０Ａは、平滑化処理を中断する条件が成立したか否かを判定する。平滑化処理を中断する条件は、ＲＴＫ−ＧＮＳＳによる測位が異常になった場合及び上部旋回体３が旋回している場合の少なくとも一方が成立した場合である。平滑化処理を中断する条件が成立したと判定部４０Ａが判定した場合（ステップＳ２０１、Ｙｅｓ）、ステップＳ２０２において、装置コントローラ３９の位置情報演算部３９Ｂは平滑化処理を中断する（II）。
【０１４０】
ステップＳ２０３において、判定部４０Ａは、油圧ショベル１００が走行しているか否かを判定する。油圧ショベル１００が走行していると判定部４０Ａが判定した場合（ステップＳ２０３、Ｙｅｓ）、ステップＳ２０４において、位置情報演算部３９Ｂは、中断していた平滑化処理を停止させる（IV）。次に、ステップＳ２０１に戻って説明する。平滑化処理を中断する条件が成立しないと判定部４０Ａが判定した場合（ステップＳ２０１、Ｎｏ）、装置コントローラ３９は処理を終了する。
【０１４１】
次に、ステップＳ２０３に戻って説明する。油圧ショベル１００が走行していないと判定部４０Ａが判定した場合（ステップＳ２０３、Ｎｏ）、ステップＳ２０５において、判定部４０Ａは、実行条件が成立したか否かを判定する。実行条件が成立したと判定部４０Ａが判定した場合（ステップＳ２０５、Ｙｅｓ）、ステップＳ２０６において、位置情報演算部３９Ｂは、平滑化処理を中断したときの情報を用いて、平滑化処理を実行する（II）。平滑化処理を中断したときの情報は、記憶部３９Ｍに記憶していた中断前におけるフィルタの出力Ｒｆｔｐｒ及び平均化定数Ｍである。実行条件が成立しないと判定部４０Ａが判定した場合（ステップＳ２０５、Ｎｏ）、位置情報演算部３９Ｂは、ステップＳ２０２に戻ってステップＳ２０２以降の処理を実行する。
【０１４２】
図１１は、装置コントローラ３９が平滑化処理の状態を遷移させる処理のフローチャートであって、特に平滑化処理のリセットに関する処理を示すものである。ステップＳ３０１において、装置コントローラ３９が平滑化処理を実行している場合、判定部４０Ａは、平滑化処理をリセットする条件が成立したか否かを判定する。平滑化処理をリセットする条件は、グローバル座標演算装置２３の測位結果の異常値を回避する処理として、異常値が発生した状態が、予め定められた時間（設定値Ｎｔ秒）継続した際に、タイムアウト処理が実行される場合である。平滑化処理をリセットする条件が成立したと判定部４０Ａが判定した場合（ステップＳ３０１、Ｙｅｓ）、ステップＳ３０２において、装置コントローラ３９の位置情報演算部３９Ｂは平滑化処理をリセットする（III）。
【０１４３】
ステップＳ３０３において、判定部４０Ａは、平滑化処理の復帰条件が成立したか否かを判定する。平滑化処理の復帰条件は、状態４においてＲＴＫ−ＧＮＳＳによる測位が正常、かつ油圧ショベル１００が非走行、かつ上部旋回体３が非旋回、かつ平滑化処理のリセットが完了した場合である。平滑化処理の復帰条件が成立したと判定部４０Ａが判定した場合（ステップＳ３０３、Ｙｅｓ）、ステップＳ３０４において、位置情報演算部３９Ｂは、平滑化処理を実行する（III）。
【０１４４】
次に、ステップＳ３０１に戻って説明する。平滑化処理をリセットする条件が成立しないと判定部４０Ａが判定した場合（ステップＳ３０１、Ｎｏ）、位置情報演算部３９Ｂは、ステップＳ３０５において実行中の平滑化処理を継続する。次に、ステップＳ３０３に戻って説明する。平滑化処理の復帰条件が成立していないと判定部４０Ａが判定した場合（ステップＳ３０３、Ｎｏ）、ステップＳ３０６において平滑化処理を停止する（V）。
【０１４５】
本実施形態は、グローバル座標演算装置２３からの第１位置の情報、すなわち第１基準位置データＰ１、Ｐ２及びＩＭＵ２４からの動作情報ＭＩを用いて、油圧ショベル１００一部の位置に相当する第２位置を求め、得られた第２位置の情報を用いて、作業機２の少なくとも一部の位置を求める。本実施形態では、第１基準位置データＰ１、Ｐ２及びＩＭＵ２４から得られる特定点、すなわち上部旋回体３の回転中心軸であるｚ軸と走行装置５が接地する面に対応する面との交点の情報を用いて、第２位置が求められる。特定点は、油圧ショベル１００の作業中において、時間の経過によって絶対位置が変化しないと見なすことができるので、装置コントローラ３９は、特定点の位置に平滑化処理を施し、平滑化処理後の特定点の位置を用いて第２位置を求めても、平滑化処理による遅れの影響は無視できる。その結果、本実施形態は、第２位置と、油圧ショベル１００一部の位置とを一致させることができるので、作業機械の位置を測位した結果に基づいて情報化施工を行う作業機械において、測位結果のばらつきが情報化施工に与える影響を低減できる。一例としては、掘削制御を用いた施工時の精度低下及びガイダンス画面の視認性の低下が抑制される。
【０１４６】
以上、実施形態１について説明したが、実施形態１の構成は、以下の実施形態においても適宜適用できる。
【０１４７】
実施形態２．
図１２は、実施形態２に係る制御システム２００ａの制御ブロック図である。図１３は、実施形態２に係る装置コントローラ３９ａが有する位置情報演算部３９Ｂａを示す図である。制御システム２００ａは、実施形態１の制御システム２００と同様であるが、ＩＭＵ２４の検出値である動作情報ＭＩが装置コントローラ３９ａの処理部３９Ｐａが有する位置情報演算部３９Ｂａに入力される点、及び位置情報演算部３９Ｂａの構成が異なる。装置コントローラ３９ａは、実施形態１と同様に、ＣＰＵ等のプロセッサ及びＲＡＭ及びＲＯＭ等の記憶装置により実現される。装置コントローラ３９ａの処理部３９Ｐａの機能は、その機能を実現するためのコンピュータプログラムを記憶部３９Ｍから処理部３９Ｐａが読み込んで実行することにより、実現される。
【０１４８】
位置情報演算部３９Ｂａは、判定部４０Ａと、速度計算部４０Ｅと、平滑化処理部４０Ｂａとを有する。判定部４０Ａは、実施形態１に係る装置コントローラ３９の判定部４０Ａと同一なので説明を省略する。速度計算部４０Ｅは、ＩＭＵ２４から取得した動作情報ＭＩである角速度ω及びＩＭＵ２４とＧＮＳＳアンテナ２１、２２との相対位置関係からＧＮＳＳアンテナ２１、２２に発生する速度ｖを求める。つまり、ある角速度ωが生じているということは、車両本体１に動きが生じており、ＩＭＵ２４と同じ車両本体１上に設置されているＧＮＳＳアンテナ２１、２２が動いているということになる。ＩＭＵ２４とＧＮＳＳアンテナ２１、２２との相対位置関係（例えば設計寸法）は既知である。このため、角速度ω及び相対位置関係から、ＧＮＳＳアンテナ２１、２２の動き（移動距離）が求められるので、結果としてＧＮＳＳアンテナ２１、２２が所定時間に移動した距離、すなわち速度ｖが求められる。時間ｄｔは、制御の１周期である。
【０１４９】
＜平滑化処理＞
本実施形態において、装置コントローラ３９ａ、より具体的には平滑化処理部４０Ｂａは、速度ｖを用いて第１位置、より具体的には第１位置の情報である基準位置データＰ１、Ｐ２に平滑化処理を施す。本実施形態において、平滑化処理には、式（７）で示されるローパスフィルタが用いられる。
Ｐｉ＝｛Ｐ＋（Ｍ−１）×（Ｐｉ−１＋ｖｄｔ）｝／Ｍ・・・（７）
【０１５０】
式（７）中のＰは、今回の制御周期における第１位置の情報である第１基準位置データＰ１、Ｐ２である。Ｐｉ−1は、前回の制御周期におけるローパスフィルタの出力、すなわち前回の制御周期において平滑化処理を施された第１位置の情報である第１基準位置データＰ１、Ｐ２である。第１基準位置データＰ１、Ｐ２は、グローバル座標演算装置２３が出力する。式（７）中のＰｉは、今回の制御周期におけるローパスフィルタの出力であり、第２位置の情報である第２基準位置データＰ１ｉ、Ｐ２ｉである。式（７）中のｖは、ＩＭＵ２４が検出した角速度ω及びＩＭＵ２４とＧＮＳＳアンテナ２１、２２との相対位置関係から速度計算部４０Ｅによって求められたＧＮＳＳアンテナ２１、２２の速度である。式（７）中のｄｔは、装置コントローラ３９ａによる制御の１周期である。ｖｄｔは、装置コントローラ３９ａ制御の１周期で油圧ショベル１００が移動した距離である。Ｍは、平均化定数である。平均化定数Ｍは、実施形態１と同様である。装置コントローラ３９ａの平滑化処理部４０Ｂａは、式（７）で示されるローパスフィルタの機能を実現することにより、動作情報ＭＩを用いて第１位置に平滑化処理を施して第２位置を求める。
【０１５１】
本実施形態において、装置コントローラ３９ａの位置情報演算部３９Ｂａが有する速度計算部４０Ｅは、制御の１周期毎に速度ｖを求め、平滑化処理部４０Ｂａは、速度ｖを用いて制御の１周期毎に第１基準位置データＰ１、Ｐ２に平滑化処理を施す。速度ｖは、ＩＭＵ２４の検出値である角速度ω及びＩＭＵ２４とＧＮＳＳアンテナ２１、２２との相対位置関係から求められる。装置コントローラ３９ａの位置情報演算部３９Ｂａは、グローバル座標演算装置２３から出力された第１基準位置データＰ１、Ｐ２に、ＩＭＵ２４の検出値を用いて平滑化処理を施す。このように位置情報演算部３９Ｂａは、ＩＭＵ２４の検出値を用いて平滑化処理を施す。このため、位置情報演算部３９Ｂａは、掘削時における油圧ショベル１００の姿勢変化がＧＮＳＳアンテナ２１、２２の位置の変化に与える影響を、ＩＭＵ２４の検出値によって反映させて、第２基準位置データＰ１ｉ、Ｐ２ｉを求めることができる。その結果、装置コントローラ３９ａは、グローバル座標演算装置２３の測位結果のばらつきによる影響を低減することができるので、掘削制御を用いた施工時の精度低下及びガイダンス画面の視認性の低下を抑制できる。
【０１５２】
＜制御システム２００ａの処理の一例＞
図１４は、実施形態２に係る制御システム２００ａの処理の一例を示すフローチャートである。ステップＳ４０１において、制御システム２００ａの装置コントローラ３９ａが有する位置情報演算部３９Ｂａの判定部４０Ａは、装置コントローラ３９ａが平滑化処理を実行するために必要な実行条件が成立したか否かを判定する。実行条件は、実施形態１で説明した通りである。
【０１５３】
開始条件が成立した場合（ステップＳ４０１、Ｙｅｓ）、装置コントローラ３９ａは、ステップＳ４０２においてＩＭＵ２４から角速度ωを取得し、グローバル座標演算装置２３から第１基準位置データＰ１、Ｐ２を取得する。ステップＳ４０３において、装置コントローラ３９ａの位置情報演算部３９Ｂａが有する平滑化処理部４０Ｂａは、速度ｖを用いて第１基準位置データＰ１、Ｐ２に平滑化処理を施す。速度ｖは、速度計算部４０Ｅによって、角速度ω及びＩＭＵ２４とＧＮＳＳアンテナ２１、２２との相対位置関係から求められる。ＩＭＵ２４とＧＮＳＳアンテナ２１、２２との相対位置関係は、校正値として計測（キャリブレーション）に基づいて得られたものが好ましい。
【０１５４】
ステップＳ４０４において、装置コントローラ３９ａが有する平滑化処理部４０Ｂａは、フィルタ出力、すなわち第２基準位置データＰ１ｉ、Ｐ２ｉを基準位置データＰ１ｃ、Ｐ２ｃとして表示コントローラ２８に出力する。ステップＳ４０５において、表示コントローラ２８の処理部２８Ｐは、装置コントローラ３９から取得した油圧ショベル１００の位置情報ＩＰＬを用いて、バケット８の刃先８Ｔの３次元位置である刃先位置を求める。次に、ステップＳ４０１に戻って説明する。ステップＳ４０１において、開始条件が成立しなかった場合（ステップＳ４０１、Ｎｏ）、装置コントローラ３９ａは処理を終了する。本実施形態において、平滑化処理の状態を遷移させる処理については、実施形態１と同様である。
【０１５５】
本実施形態は、第１基準位置データＰ１、Ｐ２及びＩＭＵ２４からの動作情報ＭＩを用いて第２位置を求め、得られた第２位置の情報を用いて、作業機２の少なくとも一部の位置を求める。実施形態１では、第１基準位置データＰ１、Ｐ２及びＩＭＵ２４から得られる特定点に情報を用いて第２位置が求められたが、本実施形態は、ＩＭＵ２４によって検出された動作情報ＭＩ、より具体的には角速度等から速度を求め、得られた速度を用いて第１位置の情報である第１基準位置データＰ１、Ｐ２に平滑化処理を施して、第２位置を求める。本実施形態は、掘削時における油圧ショベル１００の姿勢変化がＧＮＳＳアンテナ２１、２２の位置の変化に与える影響を、ＩＭＵ２４の検出値、具体的には角速度によって反映させて、第２位置を求めることができる。その結果、本実施形態は、グローバル座標演算装置２３の測位結果のばらつきが情報化施工に与える影響を低減できる。
【０１５６】
以上、実施形態２について説明したが、実施形態２の構成は、以下の実施形態においても適宜適用できる。
【０１５７】
実施形態３．
図１５は、実施形態３に係る制御システム２００ｂの制御ブロック図である。図１６は、実施形態３に係る装置コントローラ３９ｂが有する位置・姿勢情報演算部３９Ｂｂを示す図である。実施形態３は、位置姿勢演算手法にカルマンフィルタを用いる。制御システム２００ｂは、実施形態１の制御システム２００と同様であるが、位置・姿勢情報演算部３９Ｂｂを有する点、及びＩＭＵ２４の検出値である動作情報ＭＩが装置コントローラ３９ｂの処理部３９Ｐｂが有する位置・姿勢情報演算部３９Ｂｂに入力される点が異なる。装置コントローラ３９ｂは、実施形態１と同様に、ＣＰＵ等のプロセッサ及びＲＡＭ及びＲＯＭ等の記憶装置により実現される。装置コントローラ３９ｂの処理部３９Ｐｂの機能は、その機能を実現するためのコンピュータプログラムを記憶部３９Ｍから処理部３９Ｐｂが読み込んで実行することにより、実現される。
【０１５８】
位置・姿勢情報演算部３９Ｂｂは、位置推定部４０Ｆと、誤差演算部４０Ｂｂと、選択部４０Ａｂと、特定点演算部４０Ｂとを有する。位置推定部４０Ｆは、ＩＭＵ２４によって検出された動作情報ＭＩを用いて油圧ショベル１００の位置、速度、方位角及び姿勢角といった位置姿勢情推定値を推定する。油圧ショベル１００の位置は、ＧＮＳＳアンテナ２１、２２の位置である。本実施形態において、位置推定部４０Ｆは、油圧ショベル１００の位置、速度、方位角及び姿勢角といった位置姿勢値を推定して位置姿勢推定値を求めるにあたり、慣性航法を用いる。位置推定部４０Ｆは、推定により得られた油圧ショベル１００の位置を第２位置、具体的には第２基準位置データＰ１ｉ、Ｐ２ｉとして出力する。また、位置推定部４０Ｆは、誤差演算部４０Ｂｂが出力した誤差を用いて第２位置を補正する。
【０１５９】
誤差演算部４０Ｂｂは、第１基準位置データＰ１、Ｐ２、油圧ショベル１００の速度Ｖ、方位角θｄａ、特定点（本実施形態では位置ＰＬ）及び静止時の角速度ω＝０のうち少なくとも１つを観測値として使用して、位置推定部４０Ｆによって推定された油圧ショベル１００の位置、速度、方位角及び姿勢角又はそれらが有する誤差を求め、位置推定部４０Ｆに出力する。すなわち、誤差演算部４０Ｂｂは、位置姿勢推定値を補正するための情報を位置推定部４０Ｆに送信する。位置推定部４０Ｆは、位置姿勢推定値を補正するための情報を用いて、先に求められていた位置姿勢推定値の誤差を補正する。その後、位置推定部４０Ｆは、補正された位置姿勢推定値から第２位置データを算出する。誤差演算部４０Ｂｂが使用する観測値のうち、第１基準位置データＰ１、Ｐ２、油圧ショベル１００の速度Ｖ、方位角θｄａは、グローバル座標演算装置２３から得られる。誤差演算部４０Ｂｂは、グローバル座標演算装置２３から得たグローバル座標系の第１基準位置データＰ１、Ｐ２及び速度Ｖを、現場座標系に変換する。特定点、本実施形態では位置ＰＬ及び特定点の位置ベクトルＲｆｌは、特定点演算部４０Ｂが求める。本実施形態において、誤差演算部４０Ｂｂは、カルマンフィルタを含む。
【０１６０】
選択部４０Ａｂは、油圧ショベル１００の状態に応じて、誤差演算部４０Ｂｂが使用する観測値を選択する。油圧ショベル１００の状態には、静定状態、非静定状態、上部旋回体３が旋回している状態、油圧ショベル１００が走行している状態が含まれる。
【０１６１】
図１７は、実施形態３に係る装置コントローラ３９ｂが有する位置・姿勢情報演算部３９Ｂｂの制御ブロック図である。位置推定部４０Ｆは、ＩＭＵ２４によって測定された角速度を積分して車体の姿勢角の推定値及び方位角の推定を算出する。位置推定部４０Ｆは、ＩＭＵ２４によって測定された加速度を積分して、油圧ショベル１００の推定速度および推定位置を算出する。
【０１６２】
選択部４０Ａｂは、挙動検出部４２ａと、判定器４２ｂと、選択器４２ｃとを含む。挙動検出部４２ａには、車体情報ＩＦｂと、ＩＭＵ２４の検出値である角速度ω及び加速度ａとが入力される。本実施形態において、車体情報ＩＦｂは、図２に示される左操作レバー２５Ｌ及び右操作レバー２５Ｒのパイロット圧を検出する圧力センサ６６の検出値ＳＴｒ及び走行用レバー２５ＦＬ及び走行用レバー２５ＦＲのパイロット圧を検出する圧力センサ２７ＰＣの検出値ＳＴｄである。挙動検出部４２ａは、角速度ω、加速度ａ及び車体情報ＩＦｂを用いて、油圧ショベル１００の状態を検出し、検出結果に応じた信号を、判定器４２ｂに出力する。
【０１６３】
判定器４２ｂには、挙動検出部４２ａからの信号、車体情報ＩＦｂ及びグローバル座標演算装置２３が出力した状態情報ＳＲが入力される。判定器４２ｂは、入力された情報に基づいて選択器４２ｃを動作させて、誤差演算部４０Ｂｂに入力する観測値を選択する。選択器４２ｃには、観測値、すなわちグローバル座標演算装置２３が受信した第１基準位置データＰ１、Ｐ２、油圧ショベル１００の速度Ｖｃ、方位角θｄａ、特定点演算部４０Ｂが求めた特定点の位置ベクトルＲｆｌ及び油圧ショベル１００が旋回していないときの角速度ω＝０が入力される。グローバル座標演算装置２３は、測位衛星からの電波（信号）を用いて、第１基準位置データＰ１、Ｐ２を求めると同時に、油圧ショベル１００の速度Ｖｃを求める。方位角θｄａは、グローバル座標演算装置２３が第１基準位置データＰ１、Ｐ２から求める。
【０１６４】
誤差演算部４０Ｂｂは、選択部４０Ａｂの選択器４２ｃから、油圧ショベル１００の状態に応じた観測値が入力される。誤差演算部４０Ｂｂは、カルマンフィルタを含む。誤差演算部４０Ｂｂは、観測ベクトルを取得し、状態方程式で事前に予測した状態ベクトルを補正し、事後の推定値を求める。この処理を繰り返すことでより確からしい推定値を求める。式（８）はカルマンフィルタの計算式である。Ｘｋ｜ｋ（Ｘは太字）は事後推定により得られた状態ベクトル、Ｘｋ｜ｋ−1（Ｘは太字）は事前推定により得られた状態ベクトル、Ｋ（Ｋは太字）はカルマンゲイン、ｚｋ（ｚは太字）は観測ベクトル、Ｈｋ（Ｈは太字）は観測行列である。誤差演算部４０Ｂｂは、式（８）を用いて事後推定により得られた状態ベクトルを求める。
【０１６５】
【数２】
0005909029.tif 000003
【０１６６】
カルマンゲインＫ（Ｋは太字）は、式（９）で求められる。Ｐｋ｜ｋ−1（Ｐは太字）は推定誤差の共分散、Ｒｋ（Ｒは太字）は観測誤差の共分散である。推定誤差の共分散Ｐｋ｜ｋ−1及び観測誤差の共分散Ｒｋの設定によって、状態ベクトルＸｋ｜ｋ（Ｘは太字）及び観測ベクトルｚｋ（ｚは太字）の重みが決定される。
【０１６７】
【数３】
0005909029.tif 000004
【０１６８】
状態ベクトルについて説明する。予測値を通常字体で表し、補正値をイタリック体で表すと、エラー状態ベクトルは式（１０）から式（１４）で定義される。ここで、
δΨｎｎｂ（Ψは太字）：航法座標系における油圧ショベル１００の角度誤差ベクトル［ｒａｄ］
δωｂ（ωｂは太字）：ＩＭＵ２４の角速度バイアス誤差ベクトル［ｒａｄ／ｓ］
δＰｌｌｂ（Ｐは太字）：現場座標系における現場座標系を基準とした車体座標原点の位置誤差ベクトル［ｍ］
δＶｎｅｂ（Ｖは太字）：現場座標系におけるＥＣＥＦ（Earth  Centered  Earth  Fixed：地球中心地球固定）座標系を基準とした車体座標原点の速度誤差ベクトル［ｍ／ｓ］
δＡｂ（Ａｂは太字）：ＩＭＵ２４の加速度バイアス誤差ベクトル［ｍ／ｓ２］
Ｃｂｎ（Ｃは太字）：車体座標系から航法座標系への座標回転行列
Ψｎｎｂ（Ψは太字）：航法座標系における油圧ショベル１００の角度ベクトル［ｒａｄ］
ωｂ（ωｂは太字）：ＩＭＵ２４の角速度ベクトル［ｒａｄ／ｓ］
Ａｂ（Ａｂは太字）：ＩＭＵ２４の加速度ベクトル［ｍ／ｓ２］
Ｐｌｌｂ（Ｐは太字）：現場座標系における現場座標系を基準とした車体座標原点の位置ベクトル［ｍ］
Ｖｎｅｂ（Ｖは太字）：現場座標系におけるＥＣＥＦ座標系を基準とした車体座標原点の速度ベクトル［ｍ／ｓ］
Ｉ：単位行列
【０１６９】
【数４】
0005909029.tif 000005
【０１７０】
【数５】
0005909029.tif 000006
【０１７１】
【数６】
0005909029.tif 000007
【０１７２】
【数７】
0005909029.tif 000008
【０１７３】
【数８】
0005909029.tif 000009
【０１７４】
状態方程式について説明する。式（１１）から式（１９）は、エラー状態モデルに基づく状態方程式である。雑音項は省略してある。ここで、
ωｎｉｅ（ωは太字）：航法座標系における地球自転速度ベクトル［ｒａｄ／ｓ］
Ａｎｉｂ（Ａは太字）：航法座標系における慣性座標系を基準とした車体座原点の加速度ベクトル［ｍ／ｓ２］
【０１７５】
【数９】
0005909029.tif 000010
【０１７６】
【数１０】
0005909029.tif 000011
【０１７７】
【数１１】
0005909029.tif 000012
【０１７８】
【数１２】
0005909029.tif 000013
【０１７９】
【数１３】
0005909029.tif 000014
【０１８０】
イタリック体で表示した観測値についての観測方程式を式（２０）から式（２４）に示す。雑音項は省略してある。式（２０）はＧＮＳＳアンテナ２１、２２の位置の観測方程式であり、式（２１）はＧＮＳＳアンテナ２１、２２の速度の観測方程式である。式（２２）は、特定点の速度の観測方程式である。静止時及び旋回時に用いられる。式（２３）は、静止時における油圧ショベル１００の加速度の観測方程式である。式（２３）は、油圧ショベル１００の非旋回時におけるＧＰＳコンパスによる方位角の観測方程式である。ここで、
Ｐｌｌａ（斜体）：現場座標系における現場座標系を基準としたＧＮＳＳアンテナ２１、２２の位置［ｍ］
Ｖｎｅａ（斜体）：航法座標系におけるＥＣＥＦ座標系を基準としたＧＮＳＳアンテナ２１、２２の速度［ｍ／ｓ］
Ｖｎｅｑ（斜体）：航法座標系におけるＥＣＥＦ座標系を基準とした特定点の速度［ｍ／ｓ］
Ψｚ（斜体）：ＧＰＳコンパスによる油圧ショベル１００の方位角測定値［ｒａｄ］
δＣｂｎＴ：姿勢角誤差の回転行列（δＣｂｎＴ＝Ｉ−［δΨｎｎｂ^］）
δΨｚ：油圧ショベル１００の方位角の誤差（δΨｎｎｂのＺ成分）［ｒａｄ］
Ｒｂｂａ：車体座標系における車体座標系を基準としたＧＮＳＳアンテナ２１、２２の位置［ｍ］
Ｒｂｂｑ：車体座標系における車体座標系を基準とした特定点の位置［ｍ］
ωｂｎｂ：車体座標系における航法座標系を基準とした車体座標系の角速度ベクトル［ｒａｄ／ｓ］
Ψｚ：慣性航法演算による油圧ショベル１００の方位角［ｒａｄ］
【０１８１】
【数１４】
0005909029.tif 000015
【０１８２】
【数１５】
0005909029.tif 000016
【０１８３】
【数１６】
0005909029.tif 000017
【０１８４】
【数１７】
0005909029.tif 000018
【０１８５】
【数１８】
0005909029.tif 000019
【０１８６】
前述した式（１０）中の［δΨｎｎｂ＾］（Ψは太字）と、式（１５）及び式（１８）中の［ωｎｉｅ＾］（ωは太字）と、式（１８）中の［Ａｎｉｂ］（Ａは太字）とについて説明する。δΨｎｎｂ（Ψは太字）、ωｎｉｅ（ωは太字）及びＡｎｉｂ（Ａは太字）が、（α、β、γ）の３次元座標系におけるベクトル又はロール方向、ピッチ方向、ヨー方向のベクトルであるとする。ロール方向はα軸周りの方向、ピッチ方向はβ軸周りの方向、ヨー方向はγ軸周りの方向である。この場合、［δΨｎｎｂ＾］（Ψは太字）は式（２５）で、［ωｎｉｅ＾］（ωは太字）は式（２６）で、［Ａｎｉｂ］（Ａは太字）は式（２７）で表される。δΨα、δΨβ、δΨγは、この順に、α軸周り、β軸周り、γ軸周りにおける油圧ショベル１００の角度誤差である。ωｉα、ωｉβ、ωｉγは、この順に、α軸周り、β軸周り、γ軸周りにおける地球自転速度である。Ａｉα、Ａｉβ、Ａｉγは、この順に、α軸周り、β軸周り、γ軸周りにおける油圧ショベル１００の車体座原点の加速度である。
【０１８７】
【数１９】
0005909029.tif 000020
【０１８８】
【数２０】
0005909029.tif 000021
【０１８９】
【数２１】
0005909029.tif 000022
【０１９０】
誤差演算部４０Ｂｂは、事前推定において、式（１５）から式（１９）に示される状態方程式を解くことにより、式（１０）から式（１４）に示される状態ベクトルの事前推定値、すなわち状態ベクトルＸｋ｜ｋ−1を求めることができる。本実施形態において、状態ベクトルは、航法座標系における油圧ショベル１００の角度ベクトルΨｎｎｂ（Ψは太字）、ＩＭＵ２４の角速度ベクトルωｂ（ωｂは太字）、ＩＭＵ２４の加速度ベクトルＡｂ（Ａｂは太字）、現場座標系における現場座標系を基準とした車体座標原点の位置ベクトルＰｌｌｂ（Ｐは太字）及び現場座標系におけるＥＣＥＦ座標系を基準とした車体座標原点の速度ベクトルδＶｌｅｂ（Ｖは太字）である。誤差演算部４０Ｂｂは、事前推定により状態ベクトルＸｋ｜ｋ−1を求める場合、位置推定部４０Ｆが求めた姿勢角（ロール角θ４、ピッチ角θ５、方位角θｄｃ）、第２基準位置データＰ１ｉ、Ｐ２ｉ及び速度Ｖを取得し、位置、速度及び姿勢の予測値として用いる。
【０１９１】
観測行列は、観測方程式のヤコビアンによって得られる。誤差演算部４０Ｂｂは、式（２０）から式（２４）を用いて観測ベクトルｚｋ（ｚは太字）を求め、また式（９）からカルマンゲインＫ（Ｋは太字）を求める。そして、誤差演算部４０Ｂｂは、式（８）に、事前推定による状態ベクトルＸｋ｜ｋ−1、観測ベクトルｚｋ（ｚは太字）及びカルマンゲインＫ（Ｋは太字）を与えて解くことにより、事後の推定値である状態ベクトルＸｋ｜ｋを求めることができる。
【０１９２】
角速度バイアス誤差ＦＢａのベクトルは、ＩＭＵ２４の角速度バイアス誤差ベクトルδωｂ（ωｂは太字）である。車体角度誤差ＦＢｂのベクトルは、航法座標系における油圧ショベル１００の角度誤差ベクトルδΨｎｎｂ（Ψは太字）である。車体速度誤差ＦＢｃのベクトルは、現場座標系におけるＥＣＥＦ座標系を基準とした車体座標原点の速度誤差ベクトルδＶｌｅｂ（Ｖは太字）である。車体位置誤差ＦＢｄのベクトルは、現場座標系における現場座標系を基準とした車体座標原点の位置誤差ベクトルδＰｌｌｂ（Ｐは太字）である。加速度バイアス誤差ＦＢｅのベクトルは、ＩＭＵ２４の加速度バイアス誤差ベクトルδＡｂ（Ａｂは太字）である。
【０１９３】
事後の推定により得られた状態ベクトルＸｋ｜ｋは、前述したように、角速度バイアス誤差ＦＢａ、車体角度誤差ＦＢｂ、車体速度誤差ＦＢｃ、車体位置誤差ＦＢｄ及び加速度バイアス誤差ＦＢｅに対応する。誤差演算部４０Ｂｂは、事後の推定により求めた状態ベクトルＸｋ｜ｋを位置推定部４０Ｆに与える。位置推定部４０Ｆは、誤差演算部４０Ｂｂから取得した状態ベクトルＸｋ｜ｋを用いて、位置姿勢推定値を補正する。より具体的には、位置推定部４０Ｆは、状態ベクトルＸｋ｜ｋを用いて、位置姿勢推定値に含まれる誤差を補正（位置姿勢推定値を補正）する。位置推定部４０Ｆが状態ベクトルＸｋ｜ｋを用いて位置姿勢推定値（推定位置）を補正した位置が、油圧ショベル１００の第２位置となる。このように、位置推定部４０Ｆは、動作情報を用いて油圧ショベルの位置を推定し、得られた推定位置を状態ベクトルＸｋ｜ｋを用いて補正することにより、油圧ショベルの第２位置を求める。
【０１９４】
図１８は、誤差演算部４０Ｂｂが使用する観測方程式を選択する際に用いられる情報が記述されたテーブル４４の一例を示す図である。テーブル４４は、図１５に示される装置コントローラ３９ｂの記憶部３９Ｍに記憶されている。本実施形態において、誤差演算部４０Ｂｂが状態ベクトルＸｋ｜ｋを推定する際に使用する観測値は、油圧ショベル１００の状態に応じて選択される。このため、誤差演算部４０Ｂｂによって使用される観測値によって、誤差演算部４０Ｂｂによって使用される観測方程式が異なる。誤差演算部４０Ｂｂは、状態ベクトルＸｋ｜ｋを推定する際に、油圧ショベル１００の状態に応じて選択された観測値に対応した観測方程式を、式（２０）から式（２４）の中から選択する。
【０１９５】
油圧ショベル１００の状態は、図１８に示されるように、ＲＴＫ−ＧＮＳＳによる測位状態を示す状態Ａ、状態Ｂ及び状態Ｃと、油圧ショベル１００の動作状態、すなわち車体状態を示す車体静定１、車体静定２及び車体走行との組合せによって決定される。ＲＴＫ−ＧＮＳＳによる測位状態は、グローバル座標演算装置２３による油圧ショベル１００の位置の検出の状態である。本実施形態では、測位状態が３個、車体状態が３個なので、油圧ショベル１００の状態は、合計９個となる。測位状態及び動作状態の内容の詳細を下記に示す。本実施形態において、測位状態及び動作状態の数及び内容は限定されない。
状態Ａ：測位状態がＦｉｘ
状態Ｂ：測位状態がＦｉｘ、非測位以外の状態
状態Ｃ：測位状態が非測位
車体静定１：油圧ショベル１００が停止し、かつ上部旋回体３も停止している
車体静定２：油圧ショベル１００が停止し、かつ上部旋回体３が旋回している
車体走行：油圧ショベルが走行している
【０１９６】
選択部４０Ａｂの判定器４２ｂは、挙動検出部４２ａからの信号及び車体情報ＩＦｂから動作状態を判定し、グローバル座標演算装置２３が出力した状態情報ＳＲから測位状態を判定する。判定器４２ｂは、記憶部３９Ｍに記憶されているテーブル４４に判定した動作条件及び測位条件に基づき、誤差演算部４０Ｂｂによって使用される観測方程式から、誤差演算部４０Ｂｂに入力する観測値を決定する。そして、決定された観測値が誤差演算部４０Ｂｂに入力されるように、選択器４２ｃを動作させる。
【０１９７】
判定器４２ｂは、式（２０）の観測方程式が使用される場合、グローバル座標演算装置２３が受信したＧＮＳＳアンテナ２１、２２の位置に対応した第１基準位置データＰ１、Ｐ２を、誤差演算部４０Ｂｂに入力する観測値とする。判定器４２ｂは、式（２１）の観測方程式が使用される場合、グローバル座標演算装置２３が受信したＧＮＳＳアンテナ２１、２２の位置に対応した第１基準位置データＰ１、Ｐ２及びグローバル座標演算装置２３が求めたＧＮＳＳアンテナ２１、２２の速度Ｖａを現場座標系に変換し、誤差演算部４０Ｂｂに入力する観測値とする。判定器４２ｂは、式（２２）の観測方程式が使用される場合、図１６に示される特定点演算部４０Ｂが求めた特定点の位置ベクトルＲｆｌを、誤差演算部４０Ｂｂに入力する観測値とする。判定器４２ｂは、式（２３）の観測方程式が使用される場合、静止時の角速度、すなわち油圧ショベル１００が静止しているときの角速度ω＝０を、誤差演算部４０Ｂｂに入力する観測値とする。判定器４２ｂは、式（２４）の観測方程式が使用される場合、グローバル座標演算装置２３が求めた、ＧＰＳコンパスによる油圧ショベル１００の方位角θｄａを、誤差演算部４０Ｂｂに入力する観測値とする。
【０１９８】
誤差演算部４０Ｂｂは、選択器４２ｃから入力された観測値を用いて、入力された観測値に対応する観測方程式を用いて観測ベクトルｚｋ（ｚは太字）を求める。このように、誤差演算部４０Ｂｂは、油圧ショベル１００の状態、すなわち測位状態及び車体状態に応じて観測ベクトルｚｋ（ｚは太字）を求める際に用いる観測方程式を変更するので、油圧ショベル１００の状態によっては不要な観測方程式を使用しないようにすることができる。その結果、誤差演算部４０Ｂｂは、演算の負荷を軽減することができる。
【０１９９】
油圧ショベル１００が停止し、かつ上部旋回体３が旋回しない場合、誤差演算部４０Ｂｂは、油圧ショベル１００の特定点の速度が０、かつ角速度ω＝０という観測値を用いて観測ベクトルｚｋ（ｚは太字）を求めることができる。その結果、グローバル座標演算装置２３の測位結果のばらつきによる影響を低減できる。
【０２００】
図１９は、実施形態３に係る制御システム２００ｂの処理の一例を示すフローチャートである。ステップＳ５０１において、装置コントローラ３９ｂの位置・姿勢情報演算部３９Ｂｂは、次の時刻、本実施形態では次の制御周期における油圧ショベル１００の状態ベクトルを推定し、また、観測値を取得する。
【０２０１】
ステップＳ５０２において、位置・姿勢情報演算部３９Ｂｂの誤差演算部４０Ｂｂは、測位状態及び車体状態に応じて観測ベクトルｚｋ（ｚは太字）を求める際に用いる観測方程式を選択する。ステップＳ５０３において、誤差演算部４０Ｂｂは、事後の推定値である状態ベクトルＸｋ｜ｋを求め、これに対応する角速度バイアス誤差ＦＢａ、車体角度誤差ＦＢｂ、車体速度誤差ＦＢｃ、車体位置誤差ＦＢｄ及び加速度バイアス誤差ＦＢｅを、位置推定部４０Ｆに与える。位置推定部４０Ｆは、誤差演算部４０Ｂｂから取得した角速度バイアス誤差ＦＢａ、車体角度誤差ＦＢｂ、車体速度誤差ＦＢｃ、車体位置誤差ＦＢｄ及び加速度バイアス誤差ＦＢｅを用いて、ＩＭＵ２４が検出した角速度ω及び加速度ａと、位置推定部４０Ｆが角速度ωから求めた角度と、位置推定部４０Ｆが加速度ａから求めた速度及び位置とを補正する。
【０２０２】
ステップＳ５０４において、位置・姿勢情報演算部３９Ｂｂは、前述した補正によって得られた第２基準位置データＰ１ｉ、Ｐ２ｉを、基準位置データＰ１ｃ、Ｐ２ｃとして表示コントローラ２８の刃先位置算出部２８Ａに出力する。ステップＳ５０５において、表示コントローラ２８の処理部２８Ｐは、装置コントローラ３９から取得した油圧ショベル１００の位置情報ＩＰＬを用いて、バケット８の刃先８Ｔの３次元位置である刃先位置を求める。
【０２０３】
本実施形態は、第１位置の情報である第１基準位置データＰ１、Ｐ２及びＩＭＵ２４からの動作情報ＭＩを用いて第２位置を求め、得られた第２位置の情報を用いて、作業機２の少なくとも一部の位置を求める。実施形態１では、第１基準位置データＰ１、Ｐ２及びＩＭＵ２４から得られる特定点の情報を用いて第２位置が求められたが、本実施形態においても、特定点（疑似不動点）が静止しているという条件を観測方程式に追加することによって、実施形態１と同様に第２位置を求めることができる。
【０２０４】
本実施形態において、油圧ショベル１００の位置は慣性航法により推定され、油圧ショベル１００の位置姿勢誤差に含まれる誤差及びＩＭＵ２４の誤差等はカルマンフィルタによって求められる。本実施形態は、慣性航法により次の時刻の油圧ショベル１００の位置を推定し、第１位置の情報及び動作情報ＭＩを用いてカルマンフィルタが求めた誤差により、推定された油圧ショベル１００の位置を補正する。実施形態１及び実施形態２では、グローバル座標演算装置２３によって得られた位置の情報を平滑化処理するが、本実施形態では、慣性航法により予め推定された位置をカルマンフィルタによって求められた誤差を用いて補正するか、又は慣性航法により予め推定された状態ベクトルをカルマンフィルタによって求められた状態ベクトル用いて補正する。このため、本実施形態は、平滑化処理の遅れの影響を排除できるので、作業機械の位置を測位した結果に基づいて情報化施工を行う作業機械において、測位結果のばらつきが情報化施工に与える影響をより確実に低減できる。
【０２０５】
以上、実施形態１から実施形態３を説明したが、前述した内容により実施形態１から実施形態３が限定されるものではない。また、前述した構成要素には、当業者が容易に想定できるもの、実質的に同一のもの、いわゆる均等の範囲のものが含まれる。さらに、前述した構成要素は適宜組み合わせることが可能である。
【０２０６】
さらに、実施形態１から実施形態３の要旨を逸脱しない範囲で構成要素の種々の省略、置換及び変更のうち少なくとも１つを行うことができる。例えば、装置コントローラ３９が実行する各処理は、装置コントローラ３９、表示コントローラ２８、ポンプコントローラ又はこれら以外のコントローラが実行してもよい。作業機械は油圧ショベル１００に限定されず、ホイールローダー又はブルドーザのような他の作業機械であってもよい。図５に示される姿勢角演算部３９Ａ及び位置・姿勢情報演算部３９Ｂは、装置コントローラ３９に備えられるが、いずれか一方又は両方が表示コントローラ２８に備えられてもよいし、表示コントローラ２８以外のコントローラに備えられてもよい。
【符号の説明】
【０２０７】
１  車両本体
２  作業機
３  上部旋回体
５  走行装置
８  バケット
８Ｂ  刃
８Ｔ  刃先
２１、２２  アンテナ（ＧＮＳＳアンテナ）
２３  グローバル座標演算装置
２５  操作装置
２８  表示コントローラ
２８Ａ  刃先位置算出部
２８Ｂ  目標掘削地形データ生成部
２８Ｃ  目標施工情報格納部
２８Ｍ  記憶部
２８Ｐ  処理部
３９、３９ａ、３９ｂ  装置コントローラ
３９Ａ  姿勢角演算部
３９Ｂ、３９Ｂａ  位置情報演算部
３９Ｂｂ  位置・姿勢情報演算部
３９Ｃ  作業機制御部
３９Ｍ  記憶部
３９Ｐ、３９Ｐａ、３９Ｐｂ  処理部
４０Ａ  判定部
４０Ａｂ  選択部
４０Ｂ  特定点演算部
４０Ｂａ、４０Ｃ  平滑化処理部
４０Ｄ  位置計算部
４０Ｅ  速度計算部
４０Ｆ  位置推定部
４０Ｂｂ  誤差演算部
４１ａ、４１ｇ、４１ｐ  車体座標系変換部
４１ｂ、４１ｃ、４１ｈ、４１ｊ  加減算器
４１ｄ、４１ｆ  更新部
４１ｉ  現場座標系変換部
４１ｋ  速度補正部
４１ｍ  積分器
４１ｎ  位置補正部
４２ａ  挙動検出部
４２ｂ  判定器
４２ｃ  選択器
４４  テーブル
６０  基準杭
１００  油圧ショベル
２００、２００ａ、２００ｂ  制御システム
ＦＢａ  角速度バイアス誤差
ＦＢｂ  車体角度誤差
ＦＢｃ  車体速度誤差
ＦＢｄ  車体位置誤差
ＦＢｅ  加速度バイアス誤差
Ｋ  カルマンゲイン
Ｍ  平均化定数
ＭＩ  動作情報
Ｐ、Ｐ１ｃ、Ｐ２ｃ  基準位置データ
Ｐ１、Ｐ２  第１基準位置データ
Ｐ１ｉ、Ｐ２ｉ  第２基準位置データ
ＳＲ  状態情報
ａ  加速度
ｇ  重力加速度
ω  角速度
θ４  ロール角
θ５  ピッチ角
θｄａ、θｄｃ、θｄｉ  方位角
【要約】
作業機械の制御システムは、走行装置と、作業具を有する作業機と、前記作業機が取り付けられ、かつ前記走行装置に取り付けられて、前記走行装置に対して旋回する旋回体と、を備える作業機械を制御する。作業機械の制御システムは、前記作業機械の一部の位置である第１位置を検出し、前記第１位置の情報として出力する位置検出装置と、前記作業機械の動作を示す動作情報を検出して出力する状態検出装置と、前記第１位置の情報及び前記動作情報を用いて、前記一部の位置に相当する第２位置を求め、前記第２位置の情報を用いて、前記作業機の少なくとも一部の位置を求める処理装置と、を含む。【図１】
0005909029.tif 000023
【図２】
0005909029.tif 000024
【図３】
0005909029.tif 000025
【図４】
0005909029.tif 000026
【図５】
0005909029.tif 000027
【図６】
0005909029.tif 000028
【図７】
0005909029.tif 000029
【図８】
0005909029.tif 000030
【図９】
0005909029.tif 000031
【図１０】
0005909029.tif 000032
【図１１】
0005909029.tif 000033
【図１２】
0005909029.tif 000034
【図１３】
0005909029.tif 000035
【図１４】
0005909029.tif 000036
【図１５】
0005909029.tif 000037
【図１６】
0005909029.tif 000038
【図１７】
0005909029.tif 000039
【図１８】
0005909029.tif 000040
【図１９】
0005909029.tif 000041