JP2024524546A - スイングブーム掘削機の鉛直軸ジョイント角推定に関するｉｍｕベースのシステム - Google Patents

Abstract

車両のスイングブームのスイング角を判断するためのシステム及び方法が提供される。車両のスイングブーム及び車体に配置されたセンサからセンサデータが受信される。センサデータに基づいて、スイングブームが車体に対して静止しているか動作しているかが判断される。スイングブームが静止していると判断した場合、受信したセンサデータが観測スイング角に基づいて補正され、補正されたセンサデータに基づいて推定スイング角が算出される。スイングブームが動作していると判断した場合、受信したセンサデータに基づいて推定スイング角が算出される。推定スイング角は出力される。

Description

本発明は、一般的にスイングブーム掘削機の鉛直軸ジョイント角推定に関するものであり、特にスイングブーム掘削機の鉛直軸ジョイント角推定のためのＩＭＵ（inertial measurement unit）ベースのシステムに関するものである。

建設、鉱業及び農業の機械において、ガイダンスシステム及び自動位置制御システムがますます普及してきている。例えば、スイングブーム掘削機では、ガイダンスシステムはオペレーターの気づきを向上させることがあり、自動位置制御システムは掘削機の操縦及びスイングブームの位置決めにおけるいくつかの複雑さを緩和することがある。このようなガイダンスシステム及び自動位置制御システムを実施するためには、スイングブームの各ジョイント間のジョイント角を正確に推定することが重要である。

従来の方法の一つとして、ジャイロスコープは、動作を測定し、ブームのジョイント間の角度を判断するために使用されている。しかし、このような従来のジャイロスコープには、ジャイロスコープバイアスに起因するドリフトの問題がある。このようなドリフトを補正するために加速度計が使用されることがある。しかし、加速度計は、鉛直又はほぼ鉛直なジョイントに対しては、ジャイロスコープバイアスを十分に補正することができない。なぜなら、十分に大きい水平軸に投影される軸とは異なり、加速度計による出力はジョイント角が変化しても同じままだからである。

一又は複数の実施形態において、車両のスイングブームのスイング角を判断するためのシステム及び方法が提供される。車両の車体とスイングブームとに配置されたセンサからセンサデータが受信される。前記センサデータに基づいて、前記スイングブームが前記車体に対して静止しているか動作しているかが判断される。前記スイングブームが静止していると判断された場合、受信された前記センサデータが観測スイング角に基づいて補正され、補正された前記センサデータに基づいて推定スイング角が算出される。前記スイングブームが動作していると判断された場合、前記推定スイング角は、受信された前記センサデータに基づいて算出される。前記推定スイング角は出力される。

一実施形態では、前記センサから受信した信号のエネルギーを算出し、算出された前記エネルギーと一又は複数の閾値との比較を行い、前記比較に基づいて前記スイングブームが静止しているか動作しているかを判断することにより、前記スイングブームが前記車体に対して静止しているか動作しているかが判断される。

一実施形態では、受信された前記センサデータを補正することは、前記観測スイング角と直近の推定スイング角との比較を行うことと、前記比較に基づいて前記センサデータからバイアスを除去することとを備える。

一実施形態では、前記推定スイング角が前記車両のスイング制限を超えること、及び、前記推定スイング角を算出するために使用されるカルマンフィルタがリセットされること、について判断される。

一実施形態では、前記観測スイング角は、前記センサの回転軸を判断し、判断された前記回転軸に基づいて前記観測スイング角を算出することによって算出される。別の実施形態では、前記観測スイング角は、転送整列によってスイング角誤差を判断し、前記スイング角誤差に基づいて前記観測スイング角を算出することによって算出される。別の実施形態では、前記観測スイング角は、前記車両が傾斜閾値を満たす傾斜に位置すると判断された場合、前記車両のロール及びピッチに基づいて前記観測スイング角を算出することによって算出される。

一実施形態では、前記センサはＩＭＵ（inertial measurement unit）である。一実施形態では、前記車両は掘削機である。

本発明のこれら及び他の利点は、以下の詳細な説明及び添付の図面を参照することにより通常の技術者には明らかである。

図１は、本明細書で説明する一又は複数の実施形態を実施可能な、典型的な掘削機を示している。 図２は、一又は複数の実施形態における掘削機に対する上面視を示している。 図３は、一又は複数の実施形態における掘削機に配置されたＩＭＵのキャリブレーションに関するワークフローを示している。 図４は、一又は複数の実施形態における掘削機のスイング角を推定するためのスイング角推定システムの概略図を示している。 図５は、一又は複数の実施形態における運動学的角度推定モジュールの概略図を示している。 図６は、一又は複数の実施形態における掘削機のスイング角を推定する方法を示している。 図７は、一又は複数の実施形態における、スイングブームが静止しているか動作しているかを判断するためのヒステリシス型の閾値を示している。 図８は、一又は複数の実施形態における、車両のスイングブームのスイング角を判断するための状態機械を示している。 図９は、一又は複数の実施形態における典型的なユーザインタフェースを示している。 図１０は、一又は複数の実施形態における掘削機のモデルを示している。 図１１は、一又は複数の実施形態における掘削機で記録された生のジャイロスコープ信号のグラフを示している。 図１２は、一又は複数の実施形態における、生のジャイロスコープ信号の別のグラフを示している。 図１３は、一又は複数の実施形態における試験装置のモデルを示している。 図１４は、一又は複数の実施形態における、減衰振動の回転軸推定アルゴリズムの出力例であるグラフを示している。 図１５は、一又は複数の実施形態における、スイングＩＭＵの回転に伴う回転軸の変化を表すグラフを示している。 図１６は、一又は複数の実施形態における、回転軸を算出するための信号のプロットを示している。 図１７は、一又は複数の実施形態における、関数fに対するsの適用例であるグラフを示している。 図１８は、一又は複数の実施形態における誤差推定モジュールの概略図を示している。 図１９は、一又は複数の実施形態における、転送整列の結果を表すグラフを示している。 図２０は、一又は複数の実施形態を実施するために使用可能なコンピュータの高レベルのブロックダイアグラムを示している。

発明の詳細な説明

本明細書で説明されている実施形態は、ＩＭＵ（inertial measurement unit）ベースのスイング角推定システムを提供する。車両に配置されたＩＭＵのセンサデータのみを使用し、車両の車体に対するスイングブームのスイング角を推定するために、複数のアルゴリズムを結合するデータ融合スイング角システムが提供される。具体的には、上記アルゴリズムには、運動学的角度推定（静的動的のエポックを判断するために仮説試験が使用される）、回転軸の算出、転送整列（機械の残留振動を使用する）、そしてゼロでない傾斜を利用するものが含まれる。また、スイングリンクにおけるＩＭＵの追加を不要にする、ブームに配置された既存のセンサを使用した一実施形態も開示されている。推定スイング角は、掘削機のガイダンスシステム及び／又は自動制御システムで使用可能である。この角度を知ることは、オペレータが変更すべき角度の変更を許可されている場合、この追加の特徴を備えた掘削機を最大限に活用するために重要である。上記車両は、例えば、建設機械、採掘機械、農業機械等であってもよい。一実施形態において、上記車両は、図１に示されるように掘削機である。

図１は、本明細書で説明されている一又は複数の実施形態を実施可能な、典型的な掘削機１００を示している。図１の掘削機１００は、コンパクト掘削機又はミニ掘削機として示されているが、任意の適切な形態で実施することができる。掘削機１００は、オペレータと他の構成要素とを収容するための車体（すなわちハウス）１０４と、掘削機１００及び支持体１０４を案内する通路を備えた車台１０２と、土壌又は他の材料を扱うためにバケット１０８に結合されたブームスイング１０６とを備えている。掘削機１００は、追加又は代替で、押土作業を実行するためのブレード等の他の構成要素を備えてもよい。操作時、掘削機１００は、土壌等を掘る(digging)、放つ(dumping)、扱う(manipulating)などの様々なタスクを実行するために、一又は複数のジョイント１０８－Ａ、１０８－Ｂ、１０８－Ｃ、１０８－Ｄ、及び１０８－Ｅ（総称して「ジョイント１０８」という。）において回転する。

図１に示されるように、複数のＩＭＵは、バケット１０８に配置されたＩＭＵ１１０－Ａ、ブームスイング１０６に配置されたＩＭＵ１１０－Ｂ、１１０－Ｃ、及び１１０－Ｄ、及び車体１０４に配置されたＩＭＵ１１０－Ｅ（これらを総称して「ＩＭＵ１１０」という。）で構成されている。一実施形態では、それぞれのＩＭＵ１１０は、３軸ジャイロスコープ及び３軸加速度計を備えている。しかし、それぞれのＩＭＵ１１０は、角速度及び加速度を測定するための任意の適当な一又は複数のセンサとして実施されてもよい。例えば、ＩＭＵ１１０は、代わりに別々のジャイロスコープ及び別々の加速度計として実施されてもよい。

本明細書に記載の実施形態において、掘削機１００のジョイント１０８－Ｄにおけるスイング角は、ＩＭＵ１１０－Ｄ及びＩＭＵ１１０－Ｅからの情報のみに基づいて判断される。スイング角は、掘削機１００を上から見たときにおける、ある基準面に対するスイングブーム１０６の角度であり、図１においてはジョイント１０８－Ｄの角度として示されている。スイング角は、以下で詳細に説明するが図２においても示されている。他のジョイントの角度（例えば、ジョイント１０８－Ｅ）も、本明細書に記載の実施形態に従って判断されてもよい。ジョイント１１０の角度は、例えば、掘削機１００のオペレータへのガイダンスの提供、及び／又は、掘削機１００の自動制御等に使用可能である。

図１に示される掘削機１００におけるＩＭＵ１１０の数と配置とは、例示的なものであり、その用途に基づいて変化してもよいことが理解されるべきである。例えば、一つの応用では、本明細書に記載の実施形態は、上述のように、ジョイント１０８－Ｄにおけるスイング角を判断するために、車体１０４に配置されたＩＭＵ１１０－Ｅとブームスイング１０６に配置されたＩＭＵ１１０－Ｄとを使用する、スタンドアローンデュアルＩＭＵスイング角ガイダンスシステムとして実施されてもよい。別の例示的な用途では、本明細書に記載の実施形態は、仮想センサとして機能する既存の制御システム及び／又はガイダンスシステムに統合されてもよい。また、別の応用では、本明細書に記載の実施形態は、コストを減らすためにブームスイング１０６の既存のＩＭＵを利用してもよい。別の例示的な用途では、本明細書に記載の実施形態は、掘削機１００が斜面で動作するときに、パフォーマンスを向上させるため、車体１０４の旋回回転角（ＧＰＳベースの姿勢推定サブシステムを使用して導出可能）を入力として受信してもよい。

掘削機１００の一般的な運用シナリオは以下の通りである：１) ブームスイング１０６を第一の方向に合わせる；２) ブームスイング１０６を、例えば「掘ること」のために操作する；３)ブームスイング１０６を第二の方向に合わせる；４) ブームスイング１０６を、例えば「放つこと」のために操作する；５) ステップ１に戻り、このサイクルを繰り返す。この運用シナリオは、１)ブームスイング１０６を特定のアーム角度に振り出し、スイングを一定に維持しつつ第一の方向と第二の方向との間において旋回する；２)特定のハウス角度に旋回し、旋回を一定に維持しつつ第一の方向と第二の方向との間においてブームスイング１０６をsingingする；または、３)１及び２の組み合わせによって達成されてもよい。デュアルＩＭＵシステムを使用する場合、目的は、ＩＭＵ１１０－Ｄ及び１１０－Ｅからの情報（すなわち、それらの３軸の周りの回転速度、及び、それらの軸に沿った特定の力）だけを用いて、ジョイント１０８－Ｄにおけるスイング角度を推定することである。

図２は、一又は複数の実施形態における掘削機２００の上面図である。一例では、掘削機２００は図１の掘削機１００である。掘削機２００は、車体２０２とスイングブーム２０４とを備えている。掘削機２００のスイング角は、掘削機２００の上面視において、スイングブーム２０４が基準面２０６に対して形成する角度である。基準面２０６は、図２において掘削機２００の中央面として示されているが、他の適切な基準面であってもよい。図２では、掘削機２００は、リセット又はゼロの姿勢になっており、スイングブーム２０４は、車体２０２の前方に沿って（すなわち、基準面２０６に沿って）整列しており、したがってスイング角はゼロ度になっている。スイングブーム２０４は、基準面２０６に対して、例えば－４０°と＋４０°との間のスイング制限内にて、時計回り及び反時計回りに回転することができる。スイング制限は、任意の適切な角度であってもよく、機械的な停止によって実施されてもよい。

図３は、一又は複数の実施形態における掘削機３０２に配置されたＩＭＵ３０８、３１０をキャリブレーションするためのワークフロー３００を示している。一例では、掘削機３０２は図１の掘削機１００であってもよい。図３に示されているように、スイングＩＭＵ３０８は、掘削機３０２のブームスイング３０４に配置され、車体ＩＭＵ３１０は、掘削機３０２の車体３０６に配置されている。ＩＭＵ３０８、３１０は、非線形性、スケールファクタ、ジャイロスコープバイアス、軸ずれ、及び温度変動を補償するためにキャリブレーションされる。さらに、ＩＭＵ３０８、３１０が幾何学的にキャリブレーションされることにより、掘削機３０２の幾何属性とＩＭＵ３０８、３１０の配置とが正確に測定されるようになる。

次の測定は重要である：１）スイングブーム３０４を車体３０６の前方に沿って（すなわち、リセット又はゼロの姿勢で）整列させるための位置測定、２）例えば機械的な停止によって実施されるスイングブーム３０４の左右のスイング制限の測定、３）ＩＭＵ３０８、３１０の間における整列変換。測定１は測定３の必要条件である。いったんスイングブーム３０４がリセット姿勢に配置されると、ＩＭＵ３１０に対してＩＭＵ３０８を整列させる回転行列が正確に推定可能となる。

ワークフロー３００に示されているように、ＩＭＵ３０８の初期軸３１２が校正軸３１６へとキャリブレーションされ、ＩＭＵ３１０の初期軸３１４が校正軸３１８へとキャリブレーションされることにより、ＩＭＵ３０８、３１０のＸ軸が掘削機３０２の長手軸（すなわち前後軸）に沿って整列し、掘削機３０８、３１０のＹ軸が掘削機３０２の横軸（すなわち左右軸）に沿って整列し、ＩＭＵ３０８、３１０のＺ軸が掘削機３０２の鉛直軸（すなわち上下軸）に沿って整列する。初期軸３１２、３１４のアライメントに関わらず、キャリブレーション後においては、軸３１２、３１４のアライメントは同一になっている。スイングブーム３０４のスイング動作は、スイングＩＭＵ３０８をＺ軸の周りに回転させるものと考えられ、このＺ軸は、スイングブーム３０４と車体３０６とを接続するジョイントの単一自由度の軸に対応している。形式的には、スイングＩＭＵ３０８の座標枠

で表されるベクトル

は、以下の式に従って車体ＩＭＵ３１０の座標枠

で表すことができる。

ここで、

は以下の式に等しい。

ここで、θはスイング角である。実施形態によっては、回転行列

は、以下のように機械公差を誤差回転によって説明することができる。

誤差行列を構成する角度は、小さいので、微小回転の性質をもたらしている。これは、追加的なノイズとして扱うことができる。

図４は、一又は複数の実施形態における掘削機のスイング角を推定するためのスイング角推定システム４００の概略図を示している。スイング角推定システム４００は、図１の掘削機１００を引き続き参照しつつ説明される。一例では、スイング角推定システム４００は、スイングブーム１０６のスイング角を推定するために、図１に示される掘削機１００において実施される。しかし、スイング角推定システム４００は、建設機械、鉱山機械、農業機械等、他の適切な車両において実施されてもよい。スイング角推定システム４００は、一若しくは複数のコントローラ、又は、図２０のコンピュータ２００２等の他の適切な一若しくは複数の処理装置によって実施されてもよい。

スイング角推定システム４００は、掘削機の車体に配置された車体ＩＭＵ４０４（例えば図１のＩＭＵ１１０－Ｅ）によって測定された掘削機の車体の角速度

及び加速度

と、掘削機のスイングブームに配置されたスイングＩＭＵ４０２（例えば図１のＩＭＵ１１０－Ｄ）によって測定された掘削機のスイングブームの角速度

及び加速度

とのみに基づいて、推定スイング角

を予測する。スイング角推定システム４００の核心は、観測スイング角θ_oに基づいて、スイングＩＭＵ４０２及び車体ＩＭＵ４０４から受信したセンサデータを補正する、運動学的角度推定モジュール４０６である。観測スイング角θ_oは、傾斜（スロープ）推定モジュール４０８、回転軸推定モジュール４１２、及び／又は、誤差推定モジュール４１４によって算出されてもよい。誤差推定モジュール４１４は、角速度

、

、加速度

、及び推定スイング角

に基づいて観測スイング角θ_oを推定するための、転送整列を実施する。回転軸推定モジュール４１２は、回転軸を判断し、適切な条件（例えば、大振動、低ノイズ）の下で、回転軸に基づいて観測スイング角θ_oを算出する。傾斜推定モジュール４０８は、掘削機が、十分に大きい傾斜の表面に位置し、十分に長い期間において静止を維持しているときに、角速度

と加速度

とに基づいて、傾斜（インクリネーション）推定モジュール４１０を使用して観測スイング角θ_oを算出する。スイング角推定システム４００のモジュールは、以下においてさらに詳細に説明する。

運動学的角度推定

図５は、一又は複数の実施形態における運動学的角度推定モジュール５００の概略図を示している。図６は、一又は複数の実施形態における掘削機のスイング角を推定するための方法６００を示している。図５及び図６は共に説明される。一例では、運動学的角度推定モジュール５００は、図４の運動学的角度推定モジュール４０６であり、方法６００の工程は、図４の運動学的角度推定モジュール４０６によって実施される。

図６の工程６０２では、車両のスイングブーム及び車体に配置されたセンサ群からセンサデータが受信される。この車両は、例えば、建設機械、鉱山機械、農業機械、又はスイングブームと車体とを備える他の適切な車両であってもよい。一実施形態では、この車両は、図１の掘削機１００である。センサデータは、車両の車体に配置された一又は複数のセンサにより測定される、車体の角速度

、線形加速度

と、車両のスイングブームに配置される一又は複数のセンサによって測定される、スイングブームの角速度

、線形加速度

とを備えている。センサデータは、測定元であるセンサのローカル座標枠で表現される。

センサ群は、角速度と線形加速度とを測定するためのものであれば任意の適切なセンサを備えることができる。一実施形態では、センサ群は、ジャイロスコープと加速度計とを備えるＩＭＵである。別実施形態では、センサ群は、別々のジャイロスコープと加速度計とを備えている。一例では、図５に示されているように、センサ群は、車両の車体に配置された車体ＩＭＵ５０２、及び、車両のスイングブームに配置されたスイングＩＭＵ５０４であってもよい。車体ＩＭＵ５０２及びスイングＩＭＵ５０４は、例えば、図３のワークフロー３００に従って、オフセットキャリブレータ５０６によって決定されるオフセットを伴うようにキャリブレーションされてもよい。

図６の工程６０４では、センサデータに基づいてスイングブームが車体に対して静止しているか動作しているかが判断される。例えば、図５に示されているように、ＺＭＤ（zero movement detection）モジュール５１０は、合算部５０８の出力に基づいて、スイングブームが静止しているか動作しているかを判断する。車体に結合されたスイングジョイントの周りをスイングブームが回転しているとき、スイングブームが車体に対して動作しているためスイング角が変化している。

スイングブームが静止しているか動作しているかの判断は、次の２つの仮説のうち１つを選択しなければならない仮説試験の問題とすることができる。
(１)帰無仮説Ｈ₀: スイングブームは動作している。
(２)対立仮説Ｈ₁: スイングブームは静止している。

ネイマン・ピアソンの定理は、尤度比試験（ＬＲＴ）の閾値を提供するために適用することができる。尤度比は、Ｈ₀とＨ₁との尤度の比較を示す指標である。一連の観測値ｚに対して、尤度比は次のように定義される。

p(z|H₁)は、対応する仮説の確率密度関数である。ネイマン・ピアソンの定理によれば、検出確率Pr｛H₁|H₁｝を最大化するためには、誤報の確率Pr(H₁|H₀)がαに等しいことを考慮すると、L(z)>γであればH₁が選択されなければならない。

であるので、γは、Pr｛H₁|H₁｝＝αから決定される閾値である。

スイングブームの静止は、全ての動作を含んでいるわけではない。例えば、本適用例では、Ｚ軸を中心とするブームスイングの回転速度

と、平面（ただし、当該平面の法線はＺ軸である。）に投影された線形加速度

とのみが対象となっており、ここで

は、スイングジョイントの中心からスイングブームのセンサへ向かうベクトルである。さらに、ジャイロスコープの信号は通常、質及びノイズ特性の点で、加速度計の信号よりも桁違いに優れている。したがって、動作を検知するためには速度データ（ジャイロスコープセンサからのデータ）のみが使用される。よって、H₁は次のように選択される。

ここで、Ｎはサンプルの数である。ジャイロスコープのノイズ分散σ_ωは、この特殊なケースでは単なるスケールファクタであって無視できる。つまり、ジャイロスコープ信号のエネルギーが算出され、算出されたジャイロスコープ信号のエネルギーが閾値以下である場合、スイングジョイントが静止しているか動作しているかが判断される。σ_ωが記式において無視される場合、不等式の左辺は、ジャイロスコープ信号における不連続時間のエネルギーである（この場合ω_ｚ,swing）。

閾値の決定には、信号のエネルギーを学習するための既知の方法が使用されてもよい。このような方法によって、ジャイロスコープ信号の現在の（静止）エネルギーγ_nowは、サイズＮのバッチを使用して算出される。その後、閾値γ_newは、次のように反復的に更新されてもよい。

γ_new＝（１－ｐ)γ_old+ｐ・γ_now

ここで、０＜ｐ＜１が一次フィルタのパラメータである。パラメータpを決定するために直近のＭバッチが履歴に保持される。その後、新しいバッチがＭバッチに追加され、同時に最初のバッチが忘れられて新しい履歴が形成される。その後、古い分散σ_oldと新しい分散σ_newとの履歴が比較される。最も一般的な形態では、単調なマッピングf:σ_new／σ_old→ｐを適用することができる。

実際は、信号の閾値は理論的なものではない。当該閾値は、ヒステリシス型の閾値に置き換わると、より確かなものになる。図７は、一又は複数の実施形態において、３つの軸に沿って提示されたヒステリシスの挙動であるグラフ７００を示している。スイングブームが動作しているというH₀の決定は、信号エネルギー７０４が閾値７０２の特定の予め定められたファクタを満たす場合（例えば、超える、又は、未満となる）にのみ行われ、スイングブームが静止しているというH₁の決定は、信号エネルギー７０４が閾値７０４の予め定められたファクタを満たさない場合（例えば、超えない、又は、未満とならない）にのみ行われる。ファクタβ_dynamic及びβ_staticは、上限閾値及び下限閾値（β_dynamicγ及びβ_staticγ）とは異なっていてもよい。さらに、信号が特定の予め定められた時間だけ閾値のファクタを下回った場合、もっぱら信号エネルギーが下限閾値よりも低いものと考えられる。グラフ７００は、閾値７０２を下回る信号の部分７０６を示しており、スイングブームは、予め定められた時間の後に静止していると判断される。この挙動は、誤報の数を減少させつつ、特定の大きさの動作に対するシステムの感度を高めることを意図したものである。

一実施形態では、信号エネルギーをヒステリシス型の閾値と比較するだけでなく、信号エネルギーが動作ではなくノイズの増加に起因して増加している状況を処理するために、サイズＮのバッチに対するゼロクロスレートZ_ωが以下のように算出される。

Z_ωは、その後、２つの閾値T_ZCR,lowとT_ZCR,highとに対して比較される。信号は、Z_ω＞T_ZCR,highであれば動的と判断され、Z_ω＜T_ZCR,lowであれば静的と判断される。これらの２つの判断は、組み合わせられて、（予め定められた期間において）エネルギーが低いことと、ゼロクロスが閾値未満であることとの両方の組み合わせとして、最終判断H₀又はH₁を形成する。

スイングブームが静止しているか（すなわち、スイング角が変化していない）動作しているか（すなわち、スイング角が変化している）を判断することで、カルマンフィルタは次のいずれかによってスイングブームの推定スイング角を算出することができる。１）スイングブームが静止していると判断された場合（工程６０６）、受信したセンサデータを観測スイング角に基づいて補正し、補正したセンサデータに基づいて推定スイング角を算出する。２）スイングブームが動作していると判断された場合（工程６０８）、受信したセンサデータに基づいて推定スイング角を算出する。スイングブームが静止しているか動作しているかの判断に基づく推定スイング角の算出は、図８の状態機械８００によって示されている。カルマンフィルタは、Ｚ軸周りにおける車体とスイングジャイロスコープとの速度の差異のバイアスに対する、単なる一次元のオブザーバである。（唯一の誤差源が相対バイアスランダムウォークであると仮定している）。

,

補正は、図８に示されている２つの状態の状態機械に従って作用する。状態#１の場合、スイングジョイントの角度が変化しており、フィルタはジャイロスコープの入力を継続して積分する。状態#２の場合、角度の観測値θ_oが使用され、バイアスが除去されて推定値が補正される。

,

.

図８は、一又は複数の実施形態における車両のスイングブームのスイング角を判断するための状態機械８００を示している。状態機械８００は、車両の車体に対して動作しているスイングブームに対応する状態#１と、車体に対して静止しているスイングジョイントに対応する状態#２とを備えている。状態機械８００が状態#１にあり、かつ、スイングブームが静止していると判断されたとき（すなわち、ゼロモーション検知がなされたとき）、状態機械８００は状態#１から状態#２に遷移する。状態機械８００が状態#２にあり、かつ、スイングブームが動作していると判断されたとき（すなわち、モーション検知がなされたとき）、状態機械８００は状態#２から状態#１に遷移する。状態#１では、受信したセンサデータに基づいて推定スイング角

が算出される。状態#２では、受信したセンサデータが観測スイング角θ_oで補正され、補正されたセンサデータに基づいて推定スイング角

が算出される。

図６の工程６０６では、スイングブームが静止していると判断された場合、受信したセンサデータが観測スイング角に基づいて補正され、補正されたセンサデータに基づいて推定スイング角が算出される。図４に示されているように、観測スイング角θ_oは、傾斜推定モジュール４０８、回転軸推定モジュール４１２、及び／又は誤差推定モジュール４１４から受信されてもよい。受信されたセンサデータは、観測スイング角θ_oと直近の推定スイング角θ_sとを比較することにより、補正されてバイアスを除去される。次に、補正されたセンサデータに基づいて、例えばスイングブームと車体との角速度の差異を積分することにより、カルマンフィルタを使用して推定スイング角

が算出される。

一例では、図５に示されているように、スイングジョイントが静止しているとＺＭＤ５１２が判断したとき、ゲート５１６は、始動し、直近の推定スイング角θ_s５１８を１Ｄ角度カルマンフィルタ（ＫＦ）５１０に入力し、それは、直近の推定スイング角θ_s５１８と観測スイング角θ_oとを比較することによりセンサデータを補正し、補正されたセンサデータに基づいて推定スイング角

を算出する。スイングブームが動作状態から静止状態へ遷移したことをＺＭＤ５１２が判断したとき、ゲート５１４は、始動し、直近の推定スイング角θ_s５１８を推定スイング角

で更新する。カルマンフィルタの文脈では、観測スイング角θ_oの信頼性は通常低くなっている。ジャイロスコープは、積分されて推定スイング角

を算出するが、ノイズがあるため、適宜ドライブの観測が必要となる。したがって、観測スイング角θ_oは、推定スイング角

を算出するために直近の推定スイング角θ_s５１８と組み合わされる。

図６の工程６０８では、スイングジョイントが動作していると判断される場合、受信したセンサデータに基づいて推定スイング角が算出される。推定スイング角

は、例えばスイングブームと車体との角速度の差分を積分することにより、受信したセンサデータに基づいてカルマンフィルタを使用して算出される。一実施形態では、図５に示されているように、スイングジョイントが静止しているとＺＭＤ５１２が判断するとき、角度ＫＦ５１０は、例えば合算部５０８によって出力されるスイングブームの角速度とスイングの角速度との差分を積分することにより、推定スイング角

を算出する。

実施形態によっては、推定スイング角

が車両のスイング制限を超え、カルマンフィルタがリセットされる場合がある。例えば、図５に示されているように、推定スイング角

が車両のスイング制限を超えたと制限検知部５２０が判断したとき、角度ＫＦ５１０はリセットされる。この制限は、例えば図３のワークフロー３００に従って、リミットキャリブレータ５２２によって設定される。カルマンフィルタをリセットするとは、カルマンフィルタの状態の値を手動で設定することである。スイング制限に達したとき、何が正しい状態であるかが既知であるため、上記状態を直接更新することができる。

図６の工程６１０では、推定スイング角が出力される。例えば、推定スイング角は、コンピュータシステムの表示装置に推定スイング角を表示したり、コンピュータシステムのメモリ又はストレージに推定スイング角を記憶したり、推定スイング角をリモートコンピュータシステムに送信したりすることにより、出力することができる。一例として、推定スイング角は、図９に示すユーザインタフェース９００に出力される。別例として、図４に示されているように、推定スイング角

は、観測スイング角θ_oを生成するために、運動学的角度推定モジュール４０６により誤差推定モジュール４１４に出力されてもよい。

図９は、一又は複数の実施形態における模範的なユーザインタフェース９００を示している。ユーザインタフェース９００は、例えば、掘削機（又は他の適切な車両）のオペレータのようなユーザに提示することができる。図９に示されているように、ユーザインタフェース９００は、ゼロ姿勢における掘削機のスイングブームのレイアウト９０２を示すことにより、スイング制限の全範囲を視覚的に等分して初期化を容易にする。スイングブームのスイング制限９０４は、±４０°として示されている。ユーザインタフェース９００は、ユーザによって定義されたラインによって示される作業方向を示してもよく、これにより、ある方向に沿ってスイングブームを整列させるためにスイングブームを旋回及び揺動させることが容易になる。

ユーザインタフェース９００には、現在の推定スイング角に基づいて判断されたスイングブームの現在推定位置９０６と、現在の観測スイング角に基づいて判断されたスイングブームの現在観測位置９０８とがさらに表示される。様々なパラメータ９１４（例えば、デカルトのバケット位置）は、推定スイング角に応じて更新されてもよい。スイングブームの現在観測位置９０８は、アウトラインとして表示されている。現在観測位置９０８は、システムの認識によるスイングブームの位置を示している。図９に示されているように、現在観測位置９０８は、スイングブームの現在推定位置９０６に非常に近く、同じでなくてもよい。これは、推定スイング角は、観測スイング角に対応しているのではないが、観測スイング角は、カルマンフィルタのパラメータに従ってジャイロスコープバイアスを補正するために使用されるためである。初期化時には、現在観測位置９０８と現在推定位置９０６とが非常に離れていてもよい。オペレータが車両を操作するとき、現在観測位置９０８と現在推定位置９０６とが一点に収束する。等級バンド９１０が示され、現在推定位置９０６と現在観測位置９０８との間の誤差が描写されてもよい。等級バンド９１０は、等級ごとにカラーコード化（例えば、緑、黄、赤）されてもよく、それぞれの等級の範囲は、ユーザによって定義されてもよい。円形の分散指標９１２が示され、指標９１２の大きさが分散を示し、分散によって現在観測位置９０８が変化するようにしてもよい。したがって、指標９１２が大きいほど、分散が大きくなり、現在観測位置９０８の不確実性が大きくなり、その結果として現在推定位置９０６の不確実性が大きくなる。ユーザインタフェース９００は、掘削機の状態をオペレータに通知する指標を示してもよい。この状態は、通常状況と不完全状況との指標を超えており、オンライン状態のサブ状態を含んでいる（例えばラッチ又はチェイシング）。つまり、ユーザインタフェース９００は、走行時に発生するおそれがある確度の低下をオペレータに通知されることを、維持するように設計されている。過去の代替手段は、低い確度又は高い確度のオンオフ指標である。

回転軸の推定

運転中、車両の車体は、ブームスイングが動作しているとき、例えばブーム、スティック、又は、バケットピッチ及びバケットチルトの作動の結果として、わずかな振動を受ける。このような振動は、観測スイング角の観測値θ_oを判断するのに十分である。 回転軸推定アルゴリズムは、観測スイング角の観測値θ_oを判断するために、図４の回転軸推定モジュール４１２によって実行されてもよい。

上述した前提によれば、任意の時点、一定のスイング角において、スイングＩＭＵ及び車体ＩＭＵは、同一の剛体に取り付けられるが、一方が他方に対して回転していると考えることができる。基本的なアイデアは、当該剛体が動作しているとき、それらの両方によって感知される単一の本体（entity）の測定であっても、それらの出力において観測可能な差異（上記角度に比例する。）を伴うことである。その動作は、回転軸に直交する多くの要素を有している。しかし、剛体（すなわち、掘削機のキャビン）に確かな動作はない。つまり、運転中、掘削機は一定の場所にとどまり、そのアームのみが動作する。キャビンとスイングとの動作は回転軸に平行である。したがって、デザインド・マニューバ（designed maneuvers）には信頼性がない。

図１０は、一又は複数の実施形態にける掘削機の簡略モデル１０００を示している。掘削機のモデル１０００は、車体リンク１００２を運動学的連鎖の最初のリンクとして始め、スイングリンク１００４に続き、ブームリンク１００６において終了する。車体リンク１００２は、ひとまとまりのスプリングダンパエレメント１００８を介して地面に接続された剛体によって、掘削機のシャシとトラックとを形成する。ジョイントにおけるトルクの変化は、車体の減衰振動を引き起こす。これは、油圧ピストン工程の終了時（油圧バルブが閉じ、機構が突然止まるとき）に顕著になり、その後の状況は、ブレーキ後の車のシャシの自由振動に類似している。この効果は、オペレータの操作レバーにおいて、ジャブ（jab）によって容易に認識（及び感知）することができる。この現象は、インパルス荷重に続く振動構造の減衰自由振動としてモデル化することができる。

説明用として、図１１は、一又は複数の実施形態における、実際の機械運転から記録された生のジャイロスコープ信号のグラフ１１００を示している。信号には多くのノイズがあるにもかかわらず、図１１に示すように、明確に振動に対応しているスポットがある。これらのセグメントでは、ＳＮＲ（signal-to-noise ratio）が急激に変化して十分な観測源となり、一方で残りの信号は、非常に質が悪く、ノイズとほぼ区別がつかない状態となる。図１２は、一又は複数の実施形態における試験装置にて実施される、一方が他方に対して回転している同一の固定プラットフォームに取り付けられた２つのＩＭＵ、によって取り込まれたジャイロスコープ信号のグラフ１２００を示している。図１３は、一又は複数の実施形態における試験装置のモデル１３００を示している。試験装置のモデル１３００は、プラットフォーム１３０２に配置されたＩＭＵ１３０８、１３１０を備えており、ＩＭＵ１３０８はＩＭＵ１３１０に対して回転している。プラットフォーム１３０２は、その一端においてロッド１３０４によって支持されており、その他端においてチルトジョイント、スプリング１３０６を介して上記一端に接続されている。

試験装置の設置理由は、プラットフォーム１３０２の動作が固定された回転軸の周りでのみ発生することを確認することである。その際、複数の信号間での類似性があれば、アームの操作により、掘削機の車体は、事実上固定された回転軸を中心に振動していることも確認される。その際、ＩＭＵ１３０８、１３１０は、この軸に関する情報を提供していると結論づけることができる。しかし、回転していないＩＭＵ１３１０は、そのＹ軸（プラットフォームの回転軸）を中心とした動作を記録するが、回転しているＩＭＵ１３０８は、その全ての軸を中心とした動作（それ自身のローカル座標枠において表現される同一の回転軸を中心とする振動）を認識する。

生のジャイロスコープ測定から一定の回転軸を特定するために、簡単で効果的な回転軸推定アルゴリズムが実施される。ジャイロスコープの速度

に関するＮ個の測定についての１つのバッチに関して、回転軸は次のように与えられる。

ここで

である。十分に高速な速度を必要とする平均化であっても適切に作用する。しかし、ゼロに近い速度又はクロスゼロであるときには上記アルゴリズムは作用しない。メジアン操作が提示され、球面座標系において正規化サンプルが最初に示される。

ここで

その後、回転軸は次のように算出される。

ここで

と

とは中央値を示す。

図１４は、一又は複数の実施形態における、減衰振動をもとにした回転軸推定アルゴリズムの出力例であるグラフ１４００を示している。図１５は、一又は複数の実施形態における、スイングＩＭＵの回転に伴う回転軸の変化を表すグラフ１５００を示している。グラフ１５００を生成するために、角度を徐々に変化させていき装置を振動させる。これにより、減衰振動を精密にモデル化することができる明瞭な信号が生成され、振動部分が互いに分離される。図１６は、一又は複数の実施形態における、回転軸を算出するための信号のプロット１６００を示している。

回転軸推定アルゴリズムは、ある十分に長い間隔にわたって回転軸が一定であると仮定している。試験装置は、慎重に計画された励振により、これらを容易に生成することができるが、実際の機械では、信号対雑音比が低く、振動が回転軸において無秩序なノイズ変化と重複する。時間を通じて振動を特定するための方法（ストラテジ）は多数存在する。

信号が振動に関するものであるか否かを特定し、それらの振動の振幅と周波数とを算出するための多数の方法が存在する。ここでの対象は、振動(バイブレーション)そのものにあるのではなく、車体の動作を最もよく表している振動（オシレーション）にあるのである。その他の時間帯では常に、車体の動作はノイズのレベルにある。フーリエ解析は、原理的には、一般的な信号の成分を推定するために使用することができる。しかし、曲線適合方法は、単一の正弦波成分の特性（例えば、振幅、周波数、位相、減衰）を推定するのに、より適している。このような方法は、与えられた信号（セグメント）に最もよく一致する次の形態の関数のパラメータを見つけようとする。

式(１)

ここで、α、λ、ω、β、及びbは、振幅、減衰率、周波数、位相、及びオフセットをそれぞれ示している。この関数は、チャープ（chirp）効果をモデル化しておらず、より多くの高調波を追加することで、信号をよりよく説明できる可能性がある。集中質量－ばね－ダンパ物理系（又は、そのモデル）のパラメータ（例えば、剛性ｋ、減衰ｃ、質量ｍ）を特定するための他の方法としては、次のように振動を生成する。

この方程式は、１ＤＯＦシステムを表現している。さらに現実的なモデルを挙げるとすれば、これらのセットである。しかし、ここでの目的は、忠実にモデル化することではなく、挙動のパターンを特定することである。使用する方法に関わらず、それは、信号を前処理することを容易にし、さらなる特定を実現しそうな部分を抽出することを容易にする。次の前処理工程が実行されてもよい。
１）直近のＮ個のサンプルをバッファに記憶する。それを

によって表示する。
２）それ（ｓ）をデトレンド（detrend）する。
３）max(s)及びmin(s)を使用して、それを正規化する。
４）ピークを見つける。
５）最大のピークを検索する。
６）このピークより前のサンプルを除去する。
７）最初のサンプルから始め、より大きなピークが見つかるまでバッファの最後に向けて進める。
８）その最後において残りのサンプルを除去する。バッファ

をリインデックス（reindex）する。
９）ゼロクロスを見つける。
１０）２つの連続するゼロクロス間の平均距離の２倍として、振動の周期ｐを推定する。
１１）最適化装置を使用し、式（１）に定義されている関数ｆにsを適用する。α、λ、ω、β、及びbの初期値は、それぞれ、|max(s)-min(s)|、0.1、2π/p、及び

（バッファの平均）、のように選択されてもよい。図１７は、一又は複数の実施形態における、ｓを関数fに適用する一例であるグラフ１７００を示している。

車両の車体とスイングブームとの回転軸の推定が与えられると、観測スイング角は以下のように算出される。

上述した回転軸推定アルゴリズムは、ノイズに対して非常に敏感である。実験では、特定の値（例えば２５°）未満の角度は、１軸におけるジャイロスコープ速度成分がノイズ自体と同程度に小さくなるため、この方法によって扱うことができないことが分かった。したがって、実施形態によっては、角度の観測値を判断するための他の方法が実施される場合がある。

誤差推定

図１８は、一又は複数の実施形態における誤差推定モジュール１８００の概略図を示している。一例では、誤差推定モジュール１８００は、図４の誤差推定モジュール４１４である。

誤差の推定は、転送整列によって実施される。転送整列は、２つのセンサ（例えばＩＭＵ）を整列させるための一連の方法であり、これら２つのセンサのうち、１つはマスターセンサ、もう１つはスレイブセンサである。転送整列は、例えば、マスタースレイブユニットによって測定された、角速度、速度、加速度等を比較することによって機能する。転送整列による誤差推定は、観測スイング角θ_oを算出するために適用され、この観測スイング角においては、１）角度の誤差は、大きく（例えば、最大で４０°）、通常、誤整列とは考えられないものであり、２）車両は、いかなるデザインドマニューバ（designed maneuvers）も実行することができない。

車体に配置された車体センサはマスターであり、スイングブームに配置されたスイングセンサはスレイブである。整列誤差は、車体に対する、スイングブームのピッチ、ロール及びヨーについて定義される。

は、次の微分方程式によって展開する。

ここで、

であり、

は、車体に対するブームスイングのジャイロスコープ速度から構成される交代行列である。

転送整列は、小さな角度の誤整列を仮定している。

(式２)

ここで、Iは単位行列、

は、非対角要素が姿勢誤差になっている交代行列である。

誤差は、次の微分方程式によって展開（伝播）することが示される。

ここで、ｘはベクトルのクロス乗積操作を示し、

は誤差ベクトルであり、ジャイロスコープの誤差は次のように与えられる。

(式３)

加速度計の読み取りを使用して同様の結果が抽出される。

ここで、

は車両（すなわち、トラック）の直線速度ベクトルであり、

はスイングブームに配置されたＩＭＵによって測定された特定の力（加速度計の出力）であり、

は車体フレームの内部における重力ベクトルである。以下の微分方程式が伝播を決定することが示される。

ここで、

は速度算出の誤差を表し、

は車両の車体に配置されたＩＭＵによって測定された車両の加速度であり、

は真の加速度と測定された加速度との差分である。

この公式化は、カルマンフィルタの実施につながっている。この目的のために次の仮定がなされる。１)車体速度の誤差はゼロであると仮定される（すなわち、

)、２)スイング速度の誤差は、ガウスホワイトノイズとしてモデル化される（すなわち、

）、３)スイング加速度計の誤差は、ガウスホワイトノイズとしてモデル化される（すなわち、

）。

カルマンフィルタによって使用される状態空間モデルは次の通りである。

ここで、

は状態ベクトルであり、

はジャイロスコープと加速度計とのノイズ成分であり、

である。

上記を離散化すると以下のようになる。

カルマンフィルタは、状態（速度及び回転の誤差）の観測に基づいて機能する。

z_k=H_kx_k＋v_k

ここで、v_kはノイズの測定値である。入手可能な観測値に基づいて、H_kは、速度の一致、速度比の一致を含む様々な形態をとることができる。

速度の一致では、

の直接的な観測値が次のように与えられる。

ここで、

は、車体ＩＭＵからスイングＩＭＵへのベクトルの推定値である。これは、推定値θと、固定された機械パラメータ（例えば、車体ＩＭＵからスイングジョイントへの距離、及び、スイングジョイントからスイングＩＭＵへの距離）と、の関数である。よって、

である。

速度比の一致では、車両（車体）の速度間の差分の観測値は、車体ＩＭＵによって算出され、スイングＩＭＵによって推定されているので、

となる。
式２、３を使用すると、それは以下のように示される。

をガウスホワイトノイズとしてモデル化すると、それは

とすることができる。この場合、v_kにはフレキシャも含まれる。掘削機の文脈では、フレキシャは、スイングジョイントの機械的なスロップ（slop）に起因する、未知であってモデル化されていない不確実性である。

転送整列は、姿勢と加速度との一致に基づいて実行することもできる。掘削機のスイング角の誤差推定の適用では、多数の簡略化を達成することができる。第一に、掘削機はめったに動作せず、動作しても、その動作はスイングブームが動作しているときのものではないから、速度比の一致が好ましい。第二に、カルマンフィルタは、スイングブームがスイングしておらず、ω_z,swingがほぼゼロであると仮定可能な時間に、実行されるのである。

誤差の推定を利用可能な時間であれば、現在の推定スイング角θがＺ軸における誤差推定を追加するため、運動学的フィルタの観測値θ_oが生成される。

図１９は、一又は複数の実施形態における転送整列の結果を示すグラフ１９００を示している。プロット１９０２はスイングジョイントの角度を示し、プロット１９０４はスイング角の誤差を示し、プロット１９０６は補正されたスイングジョイントの角度を示し、プロット１９０８は補正されたスイング角の誤差を示している。グラフ１９００は、スレイブ（スイング）ＩＭＵを２０°回転させることにより生成される。グラフ１９００から自明なように、転送整列フィルタは、真の角度に収束している。その後の振動は、所望の値を中心として状態を変動させている。

信号の質は、理想的なセットアップではない。さらに、振動は、よく分離されたチャンク（chunks）としては生じない。転送整列は、ノイズに対して非常に強く、好適な信号（例えば、回転軸を算出するために検討される信号）を与えられる必要はないが、注意が必要である。例えば、仮定であるが、非常に小さな振幅の信号、又は、非常に短い信号に対する転送整列の結果は、十分なものではなく破棄されるべきである。これらの仮定が適切に適用されても、異常値が存在してしまうことは不可避である。また、誤差が分散すると、推定値が必要以上に引き出されてしまうおそれがある。

注意すべきは、補正の基本的理由は、ジャイロスコープの推定がドリフトしないようにすることである。したがって、より円滑な推定値を生成するために、異常値を除去し、補正を適用する分散を動的に改変する方法（ストラテジ）が考案されてもよい。

強さ（Robustness）

本セクションは、より円滑、安定的で強いシステムの実行を容易にするための方法を明確に扱うものである。

当然ながら、観測スイング角θ_oには不確実性がある。この不確実性は、運動学的角度フィルタを補正する際に使用されるＲＭＳ（root mean square）値

に反映されている。ＲＭＳ値は、観測値が収束する速さを決定するものである。一実施形態では、収束速度は、観測値を中心としてエンベロープを作成することで規定可能である。実際の観測信号は以下の通りである。

ここで

は、不確実性のエンベロープの幅である。一実施形態において、

は２°である。

振動の初期振幅と持続時間とは、低質で潜在的に危険な誤差推定を回避するための効果的な方法（ストラテジ）である。持続時間の影響度は、振幅の影響度よりも低いことが判明している。これは、急速に減衰している振動の末尾がノイズ自体のレベルにあるためである。しかし、振動は、少なくとも最初の数周期を含む程度には長くなければならない。これらのパラメータの最小値は、生の信号を解析することにより取得することができる。

回転軸の推定に関して説明されたように、減衰振動のモデルに適合する信号区間の抽出は、その信号に作用するアルゴリズムの成功を保証するものである。そのモデルを信号ストリームに結び付け、それが好適であるか否かを判断しようとすることは容易であるが、そのモデルに十分に類似している部分に関して効果的なアルゴリズムを設計することは些細なことではない。さらに、ＭＳＥのような適合測定の好適な基本的性質は、過度に保守的であり、必要以上に廃棄してしまう可能性が生ずる。

システムにおける１つの重要な特徴は、状態を動作と静止とに分離することである。最初は、システムは、スイング角の真値を認識していない。この場合、システムは、誤差推定を適用することにより（どれほど大きくても）、スイング角の真値を追求する。システムは、入力誤差推定の統計（例えば平均及び分散）を算出する。平均がゼロになり、分散が十分に小さくなると、推定値が真のスイング角に接着する確率が高くなる。このためには、アームを動作させることにより、車体を十分な時間だけ振動させる必要がある。代わりの方法は、掘削機のオペレータがスイング制限をヒットすることである。いずれの方法が速いかは、統計処理のために選択されたウィンドウサイズに依存する。この状態に移行する利点は、ゲート観測における多くの制限が誤差推定に基づいて提供され得ることである。その理論的根拠は、ジャイロスコープ信号の特性を考慮し、ジョイント機構が単一軸回転ジョイントに十分に近い状態で動作すると仮定する場合、いったん真の角度と同一又は近似になると、偏向を引き起こす主要な要因は、ジャイロランダムウォークに起因する遅いドリフトである、ということである。特に、このモードでは、次の安全策によって円滑な推定を実現することができる。

ほとんどの場合、観測スイング角θ_oは、現在の推定スイング角

を中心として変動する。推定スイング角

が観測スイング角θ_oに近いほど、観測スイング角θ_oが推定スイング角

の正確性を実際に裏付けている確率が大きくなる。この直感に基づいて、システムでは、現在の推定値が

の範囲内になる場合、補正分散が適応的に変化する。

ここで、mは振幅である。不確実性エンベロープの幅の半分は、正規分布の２σに等しい。

統計的な異常値の分析は、分布が既知の場合における誤った補正の拒絶を容易にすることができる。推定誤差の平均

及び分散

が十分に小さい場合、正規ＰＤＦ（probability density function）は、真の誤差分布を反映させていると合理的に仮定することができる。そのため、異常値を拒絶するためにＺ－試験を使用することができる。より形式的には、

の場合、推定誤差

は拒絶される。ただし以下の通りである。

もう１つの、さらに単純な方法（ストラテジ）は、誤差の大きさを一定の範囲に規制することである。選択された範囲が不確実性の範囲における幅と同一であれば、これは、事実上、推定値を中心に配置するために誤差のサインを使用することと同等である。

類似の方法（ストラテジ）は、不確実なエンベロープサイズを伴う誤差不感帯を使用することである。角度推定が不感帯の範囲内にある限り、角度推定フィルタには補正が適用されない。

システムは、連続的な励振の原理に作用する。よって、所与の誤差の推定は、それが一定時間にわたって適用された後のステイル（stale）であると考えることができる。これにより、特にオペレータが操作を停止し、特定の補正が偶然最後になるとき、的外れの誤差推定が過剰に長期にわたって推定値を引き出すことを防止することができる。同様に、スイング角が変化するとき、現在の補正（動作前に推定されたもの）は、停止し、続く観測値を待たなければならない。

初期化段階（すなわち、物理的なスイングが生じていなくても推定値が変動する可能性がある段階）の後、誤差推定の大きさは、スイング量に対して相応でなければならない。すなわち、次のように予測される。

ここで、t₀及びt₁は、誤差推定が到着した２つの連続している時間を表している。

が異常値、

であると仮定する。積分は、

によって表される。 この事例を扱うために、システムは、不一致

に応じて補正分散を増加させる。ここで、

である。

傾斜(スロープ)推定

車両が十分な傾斜に位置している場合、観測スイング角θ_oは、より単純な方法で算出することができる。観測スイング角θ_oは、図４の傾斜推定モジュール４０８による傾斜推定に基づいて、かつ、傾斜推定モジュール４１０による傾斜判断に基づいて、算出することができる。一実施形態では、車両が十分な傾斜に位置していると考えられる条件は、シャシのロール及びピッチの最小角度が、少なくとも６°以上（すなわち、１２％の傾斜閾値）であって運転中に一定のままである、というものである。これは、たいていの適用例おいて所望の傾斜よりも大きく、シャシの傾斜が一定であるという仮定は、急な傾斜においては容易に破られ得る。さらに、トラックに対するキャビンの回転が既知でなければならない。通常であればシャシは取り付けられていないので、これは、傾斜が変化するたびにキャリブレーション段階を必要とし、これは、既存のＧＰＳベース（又は同等の光学）のガイダンスシステム及び／又は自動制御システムがキャビンの進行方向を提供するために存在する場合に、行われるに過ぎない。しかし、上記の仮定が有効である場合、その利点は明らかである。

最初に、回転行列

は、スイングの回転軸が明示されるように表現される。そのためにRodriguesの公式の行列形式が使用される。

,

ここで、

はスイングジョイント軸であり、θはスイングジョイントの回転角であり、

は交代行列である。

重力ベクトルを

とし、ここで、(

)とすると、（静止時における）加速度計の出力は以下のようになる。

上述した表記は、（任意の方向における）測定値の最初の２つの要素が回転角度を算出するのに十分であることを示すために、鉛直軸(

）を中心に回転する一般的な方向センサの場合にまで拡大されてもよく、この回転角度は、[－π,π)の範囲内特有のものである。同様の論理が、任意の回転（スイング）軸と固定された方向（重力）とに対しても同一の結果を得るために使用される。

上記の式は以下のように展開される（されてもよい）。

.

ここで、左辺は加速度計の出力である。上述した表記を特殊な（さらに、処理しやすい）場合に簡略化するために、基礎の変更が行われ、

を

に対応させる。すなわち、このような座標変化は、３Ｄ空間を、回転平面（その法線ベクトルが

である）として認識される２Ｄの次元部分空間に対応させる。このような変換は、

を

のように対応させる。ここで、

は適切な線形演算子（３×３行列）である。これにより以下のようになる。

上記の式は、複素数として簡潔に表現することができる。

これは、

を意味し、θを加速度計データの位相と回転前の初期位相（すなわち、自身の軸を中心とするキャビンの回転量）とに関連付ける。一般的な場合におけるスイング回転の非可観測性を説明するための有用な公式ではあるが、生の加速度計データを使用する動作は困難であり（ノイズ及び線形加速度のため）、上記の補正は、推定重力が安定している特定の時間において適用可能であるにすぎない。重力の推定のための努力は、代わりに、ジャイロスコープの挙動を活用しつつ、スイングＩＭＵのピッチ及びロールを推定するために費やすことができる。そのＩＭＵのピッチ及びロールが入手可能であると仮定すると、次の公式

が傾斜における方向、すなわちスイング角を提供する。スイング角からキャビンの回転を引くことで、観測スイング角θ_０が得られる。その傾斜は以下のように与えられる。

しかし、（スイング）角度の方向が観測されるためには、ピッチ若しくはロール、又は、その両方の大きさが閾値を超える必要がある。上記の測定は、ピッチとロールとが等しい場合、その大きさに関わらず同一の値を出力する。回転軸が鉛直に近づくにつれて、可観測性は消失していき分散が無限になる。

スイングブームセンサの使用

実施形態によっては、車両には、既存のセンサ（例えばＩＭＵ）が既に取り付けられていてもよい。これらの実施形態では、運動学的角度推定モジュールは、既存のセンサからセンサデータを受信し、これによりコストを削減することができる。誤差推定の公式は変わらない。しかし、複雑さが増加し、推定の質の低下が生じ得る。これは、スイングブームにおける既存のセンサが２つの動作、すなわち、車体の動作と車体に対するスイングブームの動作とを測定するためである。よって、車体に対するスイングの回転行列の推定は、もはや十分ではなく、ピッチの要素を含める必要がある。その課題は、既存のセンサが同一のピッチの動作を受ける共通の動作を抽出することである。残りのピッチの要素は誤差に追加される。

まず考慮されるべきは、その装置（特に車体、スイングリンク及びブーム）の対象箇所に関する傾斜（ピッチ及びロール）の算出である。姿勢推定は、回転のユニットクォータニオン表記を使用し、観測者としてカルマンタイプのフィルタを採用し、実行されてもよい。他の表記（例えば、ＤＣＭ（direction cosine matrix）及びオイラー角）、他の観測者（例えば、補完フィルタ及び非線形観測者）を採用してもよい。

ｑ＝（q_w,q_x,q_y,q_z）で示されるユニットクォータニオンは、キャビン、スイング又はブームのようなＩＭＵの姿勢を符号化しており、その時間発展は、

によって決定される。ここで、

,

,

は、ジャイロスコープの出力

,

であり、ｂ＝（b_x b_y b_z）はジャイロバイアスであり、

は、クォータニオンの乗算と、

と、直近の式における等式

と、を示している。フィルタの状態が

であり、フィルタがバイアスを推定する。拡張カルマンフィルタの離散時間状態の展開式を導出するために、ゼロ次の前進積分を使用することができる。

,

状態遷移行列及びノイズ行列は、

及び

によって与えられる。

ヨーの角度は対象外であるため、ヨーの角度の観測値は提供されないことに留意すべきである。ピッチとロールとを補正するためには、ＩＭＵローカルフレームにて表記された重力ベクトル

の推定値が入手可能であると、一時的に仮定する。イノベーションは以下のように算出することができる。

ここで、

は、クォータニオンの姿勢の関数としての回転行列表記である。よって、観測行列は、

によって与えられる。

重力ベクトルを推定するための２つの主要な方法として「静的」及び「動的」がある。

静的：加速度計の出力がＩＭＵローカルフレームにて重力に等しい時間帯にのみフィルタを補正する。これは、加速度計の出力の分散が十分に小さく、その大きさがｇに十分に近いケースである。次に、エネルギーベースの方法（ストラテジ）を使用して、静止仮説（H₁）は、次の関係の両方が真である場合に決定することができる。

ここで、

は、この特殊なケースでのスケールファクタである。この方法のデメリットは、装置が十分な頻度及び期間で安定しない場合、補正の頻度が十分でないおそれがある、ということである。この制約を改善するためには、補正の誤差分散と共にγを増加させることができる。この方法の主要なメリットは、簡易性に加えて、装置によって所有される運動学に関する情報から上記方法が独立している、ということである。

動的：線形加速度を補償して常時フィルタを補正する。この方法は、非常に動的な環境、及び／又は、ノイズの多い環境において好適である。補償は、観測共分散行列を最適に変化させることにより実行されてもよい。これは、特に加速度を十分な確度で算出可能な場合、頑強なカルマンフィルタを使用して実行されてもよい。

一般的に、p_imu(t)がグローバル空間におけるＩＭＵの位置を示す場合、これは、p_base(t)（例えば車体の中心）に位置する基礎を有するシリアルメカニズムの終端点として考慮されると、

Ｐ_imu(t)＝P_base(t)＋Ｋ_DH(θ)

という結果になる。ここで、Ｋ_DH(θ)はデナビット・ハーテンバーグ規約を使用して算出された運動学的マッピングを示し、θはジョイントの角度ベクトルである。同様に、

である。ここで、

はシリアルリンクメカニズムのヤコビアンであり、

は対応するジョイントの回転速度である。車体が動作中に静止していると仮定すると（時々の滑りを除くと合理的な仮定である）、センサによって感知される線形加速度は、ＩＭＵをローカルセンサフレームの内部空間にて移動させる加速度の表記である。

よって、姿勢フィルタを補正するためのローカル重力は、

のように推定される。上記は、車体、スイング及びブームのＩＭＵに当てはまり、そのため、θは、それぞれ（θ_body）、（θ_body,θ_swing）及び（θ_body,θ_swing,φ_boom）になる。同様に

は、（ω_z,body）、（ω_z,body,ω_z,swing－ω_z,body)及び（ω_z,body,ω_z,swing－ω_z,body,ω_y,boom－ω_y,swing）になる。

運動学的マッピング及びそのヤコビアンもまた、通常、装置キャリブレーションの一部として測定される特定の長さの関数である。これらは、基部と車体ＩＭＵとの水平距離、基部とスイングジョイントとの水平距離、スイングジョイントとスイングＩＭＵとの距離、スイングジョイントとブームジョイントとの距離、及び、ブームジョイントとブームＩＭＵとの距離、である。

最後に、（このシステムのように）線形加速度の推定に基づくスキームは、特に、車体が滑る時又は動作を開始する時であって速度情報を入手不可能な時、不確かなケースをカバーするために、何らかの適応性のある分散が適用されると、一般的にさらに頑強になる。

本明細書にて説明されたシステム、装置、及び方法は、デジタル回路を使用するか、又は、既知のコンピュータプロセッサ、メモリユニット、ストレージデバイス、コンピュータソフトウェア及び他の要素を使用している一若しくは複数のコンピュータを使用するか、により実施されてもよい。典型的には、コンピュータは、命令を実行するためのプロセッサと、命令及びデータを記憶するための一又は複数のメモリとを含んでいる。コンピュータは、一又は複数の大容量記憶装置、例えば一又は複数の磁気ディスク、内部ハードディスク、リムーバルディスク、磁気光学ディスク及び光ディスク等、を含んでいてもよく、又は、に結合されていてもよい。

本明細書にて説明されているシステム、装置及び方法は、クライアントサーバリレーションシップにて動作するコンピュータを使用して実施されてもよい。通常、このようなシステムでは、クライアントコンピュータは、サーバコンピュータから遠隔の位置にあり、ネットワークを介して対話する。クライアントサーバリレーションシップは、それぞれのクライアントコンピュータ及びサーバコンピュータにて実行されるコンピュータプログラムによって定義及び制御されてもよい。

本明細書にて説明されているシステム、装置及び方法は、ネットワークベースのクラウドコンピューティングシステムの範囲内において実施されてもよい。このようなネットワークベースのクラウドコンピューティングシステムでは、ネットワークに接続されたサーバ又は他のプロセッサがネットワークを介して一又は複数のクライアントコンピュータと通信する。例えば、クライアントコンピュータは、クライアントコンピュータに存在して動作するネットワークブラウザアプリケーションを介してサーバと通信してもよい。クライアントコンピュータは、データをサーバに記憶し、ネットワークを介してデータにアクセスしてもよい。クライアントコンピュータは、ネットワークを介して、サーバに対して、データのリクエスト又はオンラインサービスのリクエストを送信してもよい。サーバは、リクエストされたサービスを実行し、データをクライアントコンピュータに提供してもよい。サーバは、例えば、演算を実行するために、スクリーンに特定のデータを表示するために、クライアントコンピュータに特定の機能を実行させるように適応されたデータを送信してもよい。例えば、サーバは、クライアントコンピュータに、本明細書にて説明されている方法及びワークフローにおける一又は複数の工程又は機能（図４－６及び１８における一又は複数の工程及び機能を含む）を実行させるように適応されたリクエストを送信してもよい。本明細書にて説明されている方法及びワークフローの特定の工程又は機能（図４－６及び１８における一又は複数の工程又は機能を含む）は、ネットワークベースのクラウドコンピューティングシステムの内部において、サーバ又は他のプロセッサによって実行されてもよい。本明細書にて説明されている方法及びワークフローにおける特定の工程又は機能（図４－６及び１８における一又は複数の工程を含む）は、ネットワークベースのクラウドコンピューティングシステムの内部において、クライアントコンピュータによって実行されてもよい。本明細書にて説明されている方法及びワークフローの工程又は機能（図４－６及び１８における一又は複数の工程を含む）は、ネットワークベースのクラウドコンピューティングシステムの内部において、サーバ及び／又はクライアントコンピュータによって、あらゆる組み合わせで実行されてもよい。

本明細書にて説明されているシステム、装置及び方法は、プログラマブルプロセッサによって実行するための情報媒体（例えば、非一時的の機械可読記憶装置）において、明白に具現化されたコンピュータプログラム製品を使用して実施されてもよく、本明細書にて説明されている方法及びワークフローの工程（図４－６、１８における一又は複数の工程又は機能を含む）は、このようなプロセッサによって実行可能な一又は複数のコンピュータプログラムを使用して実施されてもよい。コンピュータプログラムは、特定のアクティビティを実行するために、又は、特定の結果をもたらすために、コンピュータにおいて直接的又は間接的に使用可能な一連のコンピュータプログラム命令である。コンピュータプログラムは、任意の形式のプログラム言語（コンパイル型言語又はインタプリタ形式言語を含む）で記述可能であり、任意の形式（スタンドアローンプログラムとして、又はモジュール、コンポーネント、サブルーチン若しくは演算環境において使用するための他の適切なユニットとして）において展開可能である。

図２０には、本明細書にて説明されているシステム、装置及び方法を実施するために使用可能な一例のコンピュータ２００２の高レベルブロックダイアグラムが示されている。コンピュータ２００２は、データストレージデバイス２０１２及びメモリ２０１０に動作的に結合されたプロセッサ２００４を含んでいる。プロセッサ２００４は、コンピュータ２００２の全体的な動作を制御するために、その動作を定義するコンピュータプログラム命令を実行する。コンピュータプログラム命令は、データストレージデバイス２０１２又は他のコンピュータ可読媒体において記憶され、コンピュータプログラム命令の実行が要求される時にはメモリ２０１０にロードされてもよい。よって、図４－６及び１８の方法及びワークフローの工程又は機能は、メモリ２０１０及び／又はデータストレージデバイス２０１２に記憶されたコンピュータプログラム命令によって定義することができ、コンピュータプログラム命令を実行するプロセッサ２００４によって制御することができる。例えば、コンピュータプログラム命令は、図４－６及び１８における方法及びワークフローの工程又は機能を実行するように、本分野の技術者によりプログラムされたコンピュータ実行可能コードとして実施することができる。したがって、コンピュータプログラム命令を実行することにより、プロセッサ２００４は、図４－６及び１８における方法及びワークフローの工程又は機能を実行する。コンピュータ２００２は、ネットワークを介して他の装置と通信するための一又は複数のネットワークインタフェース２００６を含んでいてもよい。コンピュータ２００２は、コンピュータ２００２とのユーザインタラクションを可能にする一又は複数の入力／出力装置２００８（例えば、ディスプレイ、キーボード、マウス、スピーカ、ボタン等）を含んでいてもよい。

プロセッサ２００４は、一般の用途及び特殊な用途の両方のマイクロプロセッサを含んでいてもよく、コンピュータ２００２における単独のプロセッサ、又は複数のプロセッサの１つであってもよい。プロセッサ２００４は、例えば、一又は複数の中央処理ユニット（ＣＰＵ）を含んでいてもよい。プロセッサ２００４、データストレージデバイス２０１２、及び／又はメモリ２０１０は、一又は複数の特定用途向け集積回路（ＡＳＩＣｓ）、及び/又は、一又は複数のフィールドプログラマブルゲートアレイ（ＦＰＧＡｓ）、を含んでいてもよく、又は、を補充されてもよく、又は、において組み込まれていてもよい。

データストレージデバイス２０１２及びメモリ２０１０は、それぞれ有形の非一時的コンピュータ可読記憶媒体を含んでいる。データストレージデバイス２０１２及びメモリ２０１０は、それぞれ、ダイナミックランダムアクセスメモリ（ＤＲＡＭ）、スタティックランダムアクセスメモリ（ＳＲＡＭ）、ダブルデータレート同期ダイナミックランダムアクセスメモリ（ＤＤＲ ＲＡＭ）、又は他のランダムアクセス固体記憶装置のような高速ランダムアクセスメモリを含んでいてもよく、内部ハードディスク及びリムーバルディスクのような一又は複数の磁気ディスクストレージデバイス、磁気光学ディスクストレージデバイス、光ディスクストレージデバイス、フラッシュメモリデバイス、消去可能なプログラマブル読取専用メモリ（ＥＰＲＯＭ）、電気的に消去可能なプログラマブル読取専用メモリ（ＥＥＰＲＯＭ）のような半導体メモリデバイス、コンパクトディスク読取専用メモリ（ＣＤ－ＲＯＭ）、デジタルバーサタイルディスク読取専用メモリ（ＤＶＤ－ＲＯＭ）、又は、他の不揮発性固体ストレージデバイスのような不揮発性記憶装置を含んでいてもよい。

入力／出力装置２００８は、プリンタ、スキャナ、ディスプレイ画面等の周辺機器を含んでいてもよい。例えば、入力／出力装置２００８は、ユーザに情報を表示するためのブラウン管（ＣＲＴ）、又は液晶表示（ＬＣＤ）モニタのような表示装置、ユーザがコンピュータ２００２に入力するためのマウス又はトラックボールのようなポインティングデバイス、及びキーボードを含んでいてもよい。

本明細書にて説明されたシステム及び装置の一部又は全部は、コンピュータ２００２のような一又は複数のコンピュータを使用して実施されてもよい。

本分野の技術者であれば、実際のコンピュータ又はコンピュータシステムの実施が他の構造及び要素を有していてもよく、図２０がこのようなコンピュータのいくつかの要素を説明するための高レベル表記である、ということを理解する。

上述した「詳細な説明」は、あらゆる面において説明及び例示として理解されるべきであり、限定的なものではなく、本明細書にて開示された発明の範囲は、上記「詳細な説明」からではなく、特許法により許容される全域に従って解釈される請求項から判断されるべきである。理解されるべきは、本明細書において示され説明されている実施形態が現在の発明の原理の例示に過ぎず、発明の範囲及び意図から逸脱することなく本分野の技術者が様々な修正を実施してもよい。本分野の技術者であれば、発明の範囲と意図とを逸脱せずに様々な他の特徴の組み合わせを実施することができる。

Claims (20)

1. コンピュータにより実施される方法であって、
   車両のスイングブーム及び車体に配置されたセンサからのセンサデータを受信する工程と、
   前記センサデータに基づいて、前記スイングブームが前記車体に対して静止しているか動作しているかを判断する工程と、
   前記スイングブームが静止していると判断した場合、受信した前記センサデータを観測スイング角に基づいて補正し、補正した前記センサデータに基づいて推定スイング角を算出する工程と、
   前記スイングブームが動作していると判断した場合、受信した前記センサデータに基づいて前記推定スイング角を算出する工程と、
   前記推定スイング角を出力する工程と、
   を備えるコンピュータにより実施される方法。
2. 前記センサデータに基づいて、前記スイングブームが前記車体に対して静止しているか動作しているかを判断する工程は、
   前記センサから受信した信号のエネルギーを算出する工程と、
   算出した前記エネルギーと一又は複数の閾値との比較を行う工程と、
   前記比較に基づいて、前記スイングブームが静止しているか動作しているかを判断する工程と、
   を備える、請求項１に記載のコンピュータにより実施される方法。
3. 受信した前記センサデータを観測スイング角に基づいて補正し、補正された前記センサデータに基づいて推定スイング角を算出する工程は、
   前記観測スイング角と直近の推定スイング角との比較を行う工程と、
   前記比較に基づいて前記センサデータからバイアスを除去する工程と、
   を備える、請求項１に記載のコンピュータにより実施される方法。
4. 前記推定スイング角が前記車両のスイング制限を超えていることを判断する工程と、
   前記推定スイング角を算出するために使用されるカルマンフィルタをリセットする工程と、
   をさらに備える、請求項１に記載のコンピュータにより実施される方法。
5. 前記センサの回転軸を判断する工程と、
   判断した前記回転軸に基づいて前記観測スイング角を算出する工程と、
   をさらに備える、請求項１に記載のコンピュータにより実施される方法。
6. 転送整列によってスイング角誤差を判断する工程と、
   前記スイング角誤差に基づいて前記観測スイング角を算出する工程と、
   をさらに備える、請求項１に記載のコンピュータにより実施される方法。
7. 傾斜閾値を満たす傾斜に前記車両が位置することを判断した場合、前記車両のロール及びピッチに基づいて前記観測スイング角を算出する工程をさらに備える、
   請求項１に記載のコンピュータにより実施される方法。
8. 前記センサはＩＭＵ（inertial measurement unit）である、
   請求項１に記載のコンピュータにより実施される方法。
9. 前記車両は掘削機である、
   請求項１に記載のコンピュータにより実施される方法。
10. コンピュータプログラム命令を記憶するメモリと、
    前記コンピュータプログラム命令を処理するように構成された少なくとも１つのプロセッサと、を備え、
    前記コンピュータプログラム命令は、前記少なくとも１つのプロセッサに対し、
    車両のスイングブーム及び車体に配置されたセンサからのセンサデータを受信する工程と、
    前記センサデータに基づいて、前記スイングブームが前記車体に対して静止しているか動作しているかを判断する工程と、
    前記スイングブームが静止していると判断した場合、受信した前記センサデータを観測スイング角に基づいて補正し、補正した前記センサデータに基づいて推定スイング角を算出する工程と、
    前記スイングブームが動作していると判断した場合、受信した前記センサデータに基づいて前記推定スイング角を算出する工程と、
    前記推定スイング角を出力する工程と、
    を備える動作を実行させるように構成されている、システム。
11. 前記センサデータに基づいて、前記スイングブームが前記車体に対して静止しているか動作しているかを判断する工程は、
    前記センサから受信した信号のエネルギーを算出する工程と、
    算出した前記エネルギーと一又は複数の閾値との比較を行う工程と、
    前記比較に基づいて、前記スイングブームが静止しているか動作しているかを判断する工程と、
    を備える、請求項１０に記載のシステム。
12. 受信した前記センサデータを観測スイング角に基づいて補正し、補正した前記センサデータに基づいて推定スイング角を算出する工程は、
    前記観測スイング角と直近の推定スイング角との比較を行う工程と、
    前記比較に基づいて前記センサデータからバイアスを除去する工程と、
    を備える、請求項１０に記載のシステム。
13. 前記動作は、
    前記推定スイング角が前記車両のスイング制限を超えていることを判断する工程と、
    前記推定スイング角を算出するために使用されるカルマンフィルタをリセットする工程と、
    をさらに備える、請求項１０に記載のシステム。
14. 前記動作は、
    前記センサの回転軸を判断する工程と、
    判断した前記回転軸に基づいて前記観測スイング角を算出する工程と、
    をさらに備える、請求項１０に記載のシステム。
15. コンピュータプログラム命令を記憶する非一時的なコンピュータ可読媒体であって、
    前記コンピュータプログラム命令は、プロセッサによって実行されると、前記プロセッサに対して、
    車両のスイングブーム及び車体に配置されたセンサからのセンサデータを受信する工程と、
    前記センサデータに基づいて、前記スイングブームが前記車体に対して静止しているか動作しているかを判断する工程と、
    前記スイングブームが静止していると判断した場合、受信した前記センサデータを観測スイング角に基づいて補正し、補正した前記センサデータに基づいて推定スイング角を算出する工程と、
    前記スイングブームが動作していると判断した場合、受信した前記センサデータに基づいて前記推定スイング角を算出する工程と、
    前記推定スイング角を出力する工程と、
    を備える動作を実行させる、非一時的なコンピュータ可読媒体。
16. 前記動作は、
    転送整列によってスイング角誤差を判断する工程と、
    前記スイング角誤差に基づいて前記観測スイング角を算出する工程と、
    をさらに備える、請求項１５に記載の非一時的なコンピュータ可読媒体。
17. 前記動作は、
    傾斜閾値を満たす傾斜に前記車両が位置すると判断した場合、前記車両のロール及びピッチに基づいて前記観測スイング角を算出する工程をさらに備える、
    請求項１５に記載の非一時的なコンピュータ可読媒体。
18. 前記センサはＩＭＵ（inertial measurement unit）である、
    請求項１５に記載の非一時的なコンピュータ可読媒体。
19. 前記車両は掘削機である、
    請求項１５に記載の非一時的なコンピュータ可読媒体。
20. 掘削機であって、
    前記掘削機のスイングブームに配置された第１のセンサと、
    前記掘削機の車体に配置された第２のセンサと、
    コンピュータプログラム命令を記憶するメモリと、
    前記コンピュータプログラム命令を処理するように構成された少なくとも１つのプロセッサと、を備え、
    前記コンピュータプログラム命令は、前記少なくとも１つのプロセッサに対して、
    前記第１のセンサ及び前記第２のセンサからのセンサデータを受信する工程と、
    前記センサデータに基づいて、前記スイングブームが前記車体に対して静止しているか動作しているかを判断する工程と、
    前記スイングブームが静止していると判断した場合、受信した前記センサデータを観測スイング角に基づいて補正し、補正した前記センサデータに基づいて推定スイング角を算出する工程と、
    前記スイングブームが動作していると判断した場合、受信した前記センサデータに基づいて前記推定スイング角を算出する工程と、
    前記推定スイング角を出力する工程と、
    を備える動作を実行させるように構成されている、掘削機。
    

Images