特許 7307736 慣性計測装置を使用した相対角度推定

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-07-04

(45)【発行日】2023-07-12

(54)【発明の名称】慣性計測装置を使用した相対角度推定

(51)【国際特許分類】

   G01B 21/22 20060101AFI20230705BHJP

   E02F 9/26 20060101ALI20230705BHJP

【ＦＩ】

G01B21/22

E02F9/26 B

【請求項の数】 16

(21)【出願番号】P 2020542111

(86)(22)【出願日】2019-01-30

(65)【公表番号】

(43)【公表日】2021-05-13

(86)【国際出願番号】 US2019015817

(87)【国際公開番号】W WO2019152494

(87)【国際公開日】2019-08-08

【審査請求日】2022-01-18

(31)【優先権主張番号】15/887,717

(32)【優先日】2018-02-02

(33)【優先権主張国・地域又は機関】US

(73)【特許権者】

【識別番号】506196063

【氏名又は名称】キャタピラー トリンブル コントロール テクノロジーズ、 エルエルシー

(74)【代理人】

【識別番号】100137969

【弁理士】

【氏名又は名称】岡部 憲昭

(74)【代理人】

【識別番号】100104824

【弁理士】

【氏名又は名称】穐場 仁

(74)【代理人】

【識別番号】100121463

【弁理士】

【氏名又は名称】矢口 哲也

(72)【発明者】

【氏名】アラム，ニマ

【審査官】眞岩 久恵

(56)【参考文献】

【文献】米国特許出願公開第２０１６／０１６０４７２（ＵＳ，Ａ１）

【文献】国際公開第２０１６／１６４９７５（ＷＯ，Ａ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｂ ２１／００－２１／３２

Ｅ０２Ｆ ９／２６

(57)【特許請求の範囲】

【請求項1】

掘削機のバケットとスティックとの間の相対角度の三角関数を使用して前記掘削機のプラットフォームに対する前記バケットの位置を決定するための方法であって、前記方法が、
前記スティックに取り付けられた第１の慣性計測装置（ＩＭＵ）を使用して前記スティックの角速度および重力加速度ベクトルを測定することと、
前記バケットに取り付けられたＩＭＵを使用して前記バケットの角速度および重力加速度ベクトルを測定することと、
前記スティックと前記バケットとの間の前記相対角度の正弦および前記スティックと前記バケットとの間の前記相対角度の余弦を推定することであって、前記正弦および前記余弦が、前記スティックの前記重力加速度ベクトルおよび前記角速度と、前記バケットの前記重力加速度ベクトルおよび前記角速度と、前記スティックから前記バケットへの回転行列と、少なくとも１つの共分散行列とに基づいて推定されることと、
少なくとも前記正弦および前記余弦を使用して前記掘削機の前記プラットフォームに対する前記バケットの前記位置を決定することと
を含み、
前記少なくとも１つの共分散行列が、初期推定誤差共分散行列、プロセスノイズ共分散行列、または観測ノイズ共分散行列のうちの少なくとも１つを含む、方法。

【請求項2】

前記スティックと前記バケットとの間の前記相対角度の前記正弦および前記余弦が、前記スティックの絶対角度または前記バケットの絶対角度を使用せずに推定される、請求項１に記載の方法。

【請求項3】

前記スティックと前記バケットとの間の前記相対角度の前記正弦および前記余弦を推定することが、
前記スティックと前記バケットとの間の前記相対角度の観測された正弦を決定することと、
前記スティックと前記バケットとの間の前記相対角度の観測された余弦を決定することと、
前記スティックから前記バケットへの前記回転行列を決定することと、
前記少なくとも１つの共分散行列を決定することと
を含む、請求項１に記載の方法。

【請求項4】

前記相対角度の前記観測された正弦および前記相対角度の前記観測された余弦が、前記スティックの前記重力加速度ベクトルおよび前記バケットの前記重力加速度ベクトルを使用して決定される、請求項３に記載の方法。

【請求項5】

慣性計測装置（ＩＭＵ）が各々配置されている結合された剛体間の相対角度の三角関数を推定するための方法であって、前記方法が、
前の剛体に結合された第１のＩＭＵを使用して前記前の剛体の角速度および重力加速度ベクトルを測定することと、
剛体に結合された第２のＩＭＵを使用して前記剛体の角速度および重力加速度ベクトルを測定することであって、前記剛体が前記前の剛体に旋回可能に結合されていることと、
前記前の剛体と前記剛体との間の第１の相対角度の第１の観測された三角関数および第２の観測された三角関数を決定することと、
前記前の剛体の前記重力加速度ベクトルおよび前記角速度と、前記剛体の前記重力加速度ベクトルおよび前記角速度と、前記第１の観測された三角関数および前記第２の観測された三角関数とに基づいて、前記前の剛体と前記剛体との間の前記第１の相対角度の第１の三角関数および第２の三角関数を推定することと、
次の剛体に結合された第３のＩＭＵを使用して前記次の剛体の角速度および重力加速度ベクトルを測定することであって、前記次の剛体が前記剛体に旋回可能に結合されていることと、
前記剛体の前記重力加速度ベクトルおよび前記角速度と、前記次の剛体の前記重力加速度ベクトルおよび前記角速度とに基づいて、前記剛体と前記次の剛体との間の第２の相対角度の第１の三角関数および第２の三角関数を推定することと
を含む、方法。

【請求項6】

前記前の剛体と前記剛体との間の前記第１の相対角度の前記第１の三角関数および前記第２の三角関数が、前記前の剛体の絶対角度または前記剛体の絶対角度を使用せずに推定される、請求項５に記載の方法。

【請求項7】

前記第１の相対角度の前記第１の三角関数が、前記前の剛体と前記剛体との間の前記第１の相対角度の正弦であり、前記第１の相対角度の前記第２の三角関数が、前記前の剛体と前記剛体との間の前記第１の相対角度の余弦である、請求項５に記載の方法。

【請求項8】

前記正弦および前記余弦を使用して前記前の剛体と前記剛体との間の前記第１の相対角度を決定することをさらに含む、請求項７に記載の方法。

【請求項9】

前記剛体と前記次の剛体との間の前記第２の相対角度の前記第１の三角関数および前記第２の三角関数が、前記剛体の絶対角度または前記次の剛体の絶対角度を使用せずに推定される、請求項５に記載の方法。

【請求項10】

前記剛体と前記次の剛体との間の前記第２の相対角度の前記第１の三角関数および前記第２の三角関数が、前記前の剛体の前記重力加速度ベクトルを使用せずに決定される、請求項５に記載の方法。

【請求項11】

前記前の剛体と前記剛体との間の前記第１の相対角度の前記第１の三角関数および前記第２の三角関数を推定することが、
前記前の剛体から前記剛体への回転行列を決定することと、
少なくとも１つの共分散行列を決定することと
を含み、
前記少なくとも１つの共分散行列を決定することが、初期推定誤差共分散行列、プロセスノイズ共分散行列、または観測ノイズ共分散行列のうちの少なくとも１つを決定することを含む、請求項５に記載の方法。

【請求項12】

前記前の剛体と前記剛体との間の前記第１の相対角度の前記第１の観測された三角関数および前記第２の観測された三角関数が、前記前の剛体の前記重力加速度ベクトルおよび前記剛体の前記重力加速度ベクトルを使用して決定される、請求項１１に記載の方法。

【請求項13】

前の剛体および剛体を含む、一連の分離しているが結合されている剛体間の相対角度の三角関数を決定するための方法であって、前記方法が、
前記前の剛体に結合された第１のＩＭＵを使用して前記前の剛体の角速度および重力加速度ベクトルを測定することと、
前記剛体に結合された第２のＩＭＵを使用して前記剛体の角速度および重力加速度ベクトルを測定することと、
前記前の剛体と前記剛体との間の相対角度の第１の観測された三角関数および第２の観測された三角関数を推定することと、
前記前の剛体から前記剛体への回転行列および少なくとも１つの共分散行列を決定することと、
前記第１の観測された三角関数および前記第２の観測された三角関数と、前記前の剛体の前記角速度と、前記剛体の前記角速度と、前記前の剛体から前記剛体への前記回転行列と、前記少なくとも１つの共分散行列とに基づいて、前記前の剛体と前記剛体との間の前記相対角度の第１の三角関数および第２の三角関数を推定することと
を含み、
前記少なくとも１つの共分散行列を決定することが、初期推定誤差共分散行列、プロセスノイズ共分散行列、または観測ノイズ共分散行列のうちの少なくとも１つを決定することを含む、方法。

【請求項14】

前記第１の三角関数および前記第２の三角関数が、前記前の剛体の絶対角度または前記剛体の絶対角度を使用せずに計算される、請求項１３に記載の方法。

【請求項15】

前記剛体と次の剛体との間の相対角度を、前記前の剛体と前記剛体との間の前記第１の三角関数および前記第２の三角関数を使用せずに決定することをさらに含む、請求項１３に記載の方法。

【請求項16】

連続して配置された旋回可能に結合された剛体間の相対角度を決定するための方法であって、前記方法が、
前の剛体に結合された第１の慣性計測装置（ＩＭＵ）を使用して前記前の剛体の角速度および重力加速度ベクトルを測定することと、
剛体に結合された第２のＩＭＵを使用して前記剛体の角速度および重力加速度ベクトルを測定することであって、前記剛体が前記前の剛体に旋回可能に結合されていることと、
前記前の剛体の前記重力加速度ベクトルおよび前記角速度と、前記剛体の前記重力加速度ベクトルおよび前記角速度と、前記前の剛体と前記剛体との間の前記相対角度の第１の観測された三角関数および第２の観測された三角関数と、前記前の剛体から前記剛体への回転行列と、少なくとも１つの共分散行列とに基づいて、前記前の剛体と前記剛体との間の相対角度の第１の三角関数および第２の三角関数を推定することと、
前記第１の三角関数および前記第２の三角関数を使用して前記前の剛体と前記剛体との間の前記相対角度を推定することであって、前記前の剛体と前記剛体との間の前記相対角度が、前記前の剛体の絶対角度または前記剛体の絶対角度を使用せずに決定されることと
を含み、
前記少なくとも１つの共分散行列が、初期推定誤差共分散行列、プロセスノイズ共分散行列、または観測ノイズ共分散行列のうちの少なくとも１つを含む、方法。

【発明の詳細な説明】

【技術分野】

【0001】

[0001]相互参照
本出願は、２０１８年２月２日に出願された「慣性計測装置を使用した相対角度推定」という名称の米国非仮特許出願第１５／８８７７１７号の利益と優先権を主張し、その全内容はあらゆる目的でその全体が参照により本明細書に組み込まれる。

【0002】

[0002]本明細書に記載される実施形態は、一般に、慣性計測装置（ｉｎｅｒｔｉａｌ ｍｅａｓｕｒｅｍｅｎｔ ｕｎｉｔ（ＩＭＵ））を使用して結合された剛体間の相対角度を推定することに関する。

【背景技術】

【0003】

[0003]ＩＭＵは、通常、加速度計およびジャイロスコープを使用して物体の直線運動および角運動を測定するセンサである。ＩＭＵは、物体の動きに関する情報が求められる多くの用途で一般的に使用される。例には、車両、航空機、および衛星が含まれる。ＩＭＵは、大きな物体の様々な部分の運動を測定するためにも一般的に使用される。

【0004】

[0004]ＩＭＵを使用して結合された剛体間の相対角度を決定する改善された方法が求められている。

【発明の概要】

【課題を解決するための手段】

【0005】

[0005]本明細書に記載される実施形態は、２つ以上の結合された剛体間の相対角度および／または成分相対角度ｓｉｎ（α）ｃｏｓ（α）を決定するための改善された方法を提供する。相対角度は、剛体に取り付けられたＩＭＵからの情報を使用して決定される。

【0006】

[0006]一実施形態によれば、分離しているが結合されている剛体間の成分相対角度を決定するための方法は、前の剛体に取り付けられたＩＭＵの観測値から前の剛体の重力加速度ベクトルおよび角速度を取得することと、剛体に取り付けられたＩＭＵの観測値から剛体の重力加速度ベクトルおよび角速度を取得することと、を含む。前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度が、前の剛体の重力加速度ベクトルおよび角速度と、剛体の重力加速度ベクトルおよび角速度とに少なくとも一部は基づいて推定される。

【0007】

[0007]一実施形態では、前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度は、前の剛体の絶対角度または剛体の絶対角度を使用せずに決定される。

【0008】

[0008]別の実施形態では、第１の成分相対角度は正弦成分角度であり、第２の成分相対角度は余弦成分角度である。

【0009】

[0009]別の実施形態では、本方法は、正弦成分角度および余弦成分角度を使用して前の剛体と剛体との間の相対角度を決定することも含む。

【0010】

[0010]別の実施形態では、第１の成分相対角度および第２の成分相対角度を推定することが、剛体の第１の観測された成分相対角度および第２の観測された成分相対角度を決定することと、前の剛体から剛体への回転行列を決定することと、少なくとも１つの共分散行列を決定することと、を含む。

【0011】

[0011]さらに別の実施形態では、剛体の第１の観測された成分相対角度および第２の観測された成分相対角度は、前の剛体の重力加速度ベクトルおよび剛体の重力加速度ベクトルを使用して決定される。

【0012】

[0012]別の実施形態によれば、ＩＭＵが各々配置されている結合された剛体間の成分相対角度を推定するための方法は、前の剛体に取り付けられたＩＭＵの観測値から決定された前の剛体の重力加速度ベクトルを受け取ることと、前の剛体に取り付けられたＩＭＵの観測値から前の剛体の角速度を受け取ることと、剛体に取り付けられたＩＭＵの観測値から決定された剛体の重力加速度ベクトルを受け取ることと、剛体に取り付けられたＩＭＵの観測値から剛体の角速度を受け取ることと、を含む。前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度が、前の剛体の重力加速度ベクトルおよび角速度と、剛体の重力加速度ベクトルおよび角速度とに少なくとも一部は基づいて推定される。本方法は、次の剛体に取り付けられたＩＭＵの観測値から決定された次の剛体の重力加速度ベクトルおよび角速度を受け取ること、も含む。剛体と次の剛体との間の第１の成分相対角度および第２の成分相対角度が、剛体の重力加速度ベクトルおよび角速度と、次の剛体の重力加速度ベクトルおよび角速度とに少なくとも一部は基づいて推定される。

【0013】

[0013]一実施形態では、剛体と次の剛体との間の第１の成分相対角度および第２の成分相対角度は、剛体の絶対角度または次の剛体の絶対角度を使用せずに決定される。

【0014】

[0014]別の実施形態では、剛体と次の剛体との間の第１の成分相対角度および第２の成分相対角度は、前の剛体の重力加速度ベクトルを使用せずに決定される。

【0015】

[0015]別の実施形態によれば、前の剛体および剛体を含む、一連の分離しているが結合されている剛体間の成分相対角度を決定するための方法は、前の剛体の重力加速度ベクトルおよび剛体の重力加速度ベクトルを受け取ることと、前の剛体と剛体との間の第１の観測された成分相対角度および第２の観測された成分相対角度を計算することと、を含む。本方法は、前の剛体の角速度、剛体の角速度、前の剛体から剛体への回転行列、および少なくとも１つの共分散行列を受け取ること、も含む。前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度が、第１の観測された成分相対角度および第２の観測された成分相対角度と、前の剛体の角速度と、剛体の角速度と、前の剛体から剛体への回転行列と、少なくとも１つの共分散行列とに基づいて計算される。

【0016】

[0016]一実施形態では、少なくとも１つの共分散行列を受け取ることは、初期推定誤差共分散行列、プロセスノイズ共分散行列、または観測ノイズ共分散行列のうちの少なくとも１つを受け取ること、を含む。

【0017】

[0017]別の実施形態では、本方法は、剛体と次の剛体との間の相対角度を、前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度を使用せずに決定すること、も含む。

【0018】

[0018]さらに別の実施形態によれば、連続して配置された旋回可能に結合された剛体間の相対角度を決定するための方法は、前の剛体に取り付けられたＩＭＵの観測値から決定された前の剛体の重力加速度ベクトルおよび角速度を受け取ることと、剛体に取り付けられたＩＭＵの観測値から決定された剛体の重力加速度ベクトルおよび角速度を受け取ることと、を含む。前の剛体と剛体との間の相対角度が、前の剛体の重力加速度ベクトルおよび角速度と、剛体の重力加速度ベクトルおよび角速度とに基づいて推定される。前の剛体と剛体との間の相対角度は、前の剛体の絶対角度または剛体の絶対角度を使用せずに決定される。

【図面の簡単な説明】

【0019】

【図1】一実施形態による、掘削機（一連の結合された剛体を有する物体の例）の簡略化された側面図である。

【図2】一実施形態による、剛体間の観測された成分相対角度を決定する方法を示すフローチャートである。

【図3】一実施形態による、剛体間の成分相対角度を決定する方法を示すフローチャートである。

【図4】いくつかの実施形態による、剛体間の成分相対角度を決定する方法を示すフローチャートである。

【図5】いくつかの実施形態による、一連の剛体間の成分相対角度を決定する方法を示すフローチャートである。

【図6】いくつかの実施形態による、制御システムの簡略化された図である。

【図7】一実施形態による、前の剛体と剛体との間の成分相対角度を決定する方法を示すフローチャートである。

【図8】一実施形態による、剛体と次の剛体との間の相対角度を決定する方法を示すフローチャートである。

【図9】一実施形態による、連続として配置された剛体間の成分相対角度を決定する方法を示すフローチャートである。

【図10】一実施形態による、連続として配置された旋回可能に結合された剛体間の相対角度を決定する方法を示すフローチャートである。

【発明を実施するための形態】

【0020】

[0029]本明細書に記載される実施形態は、結合された剛体間の相対角度および／または成分相対角度を決定する改善された方法を提供する。いくつかの実施形態では、例えば、成分相対角度を、連続における先の剛体からの情報を必要とせずに、剛体および連続における直前の剛体からの測定値および計算されたパラメータに基づいて決定することができる。

【0021】

[0030]本出願全体を通して使用される一連の剛体の一例が、相互に旋回可能に結合されているいくつかの可動部品（または移動体）を含む掘削機である。掘削機は一例として使用されているにすぎず、本明細書に記載される実施形態は、連続して結合された剛体を含む任意の他の重機、車両、機械、または物体と共に使用され得ることを理解されたい。

【0022】

[0031]図１は、プラットフォーム１１、ブーム１２、スティック１６、およびバケット２０を含む掘削機の簡略化された側面図である。ブーム１２は、旋回軸１４でプラットフォーム１１に旋回可能に結合されており、スティック１６は、旋回軸１８でブーム１２に旋回可能に結合されており、バケット２０は、旋回軸２２でスティック１６に旋回可能に結合されている。ブーム１２、スティック１６、およびバケット２０を動かすために、油圧装置２４、２６、２８が設けられている。バケット２０は、掘削の助けとなり得るトゥース３０を含む。プラットフォーム１１は、作業現場での掘削機の移動を容易にするために車輪または無限軌道を含み得る下部走行体３２上に支持されたキャブ３１を含む。プラットフォーム１１は、油圧モータ３３によっておおむね垂直な軸３５を中心に回転させることができる。この例は一体型ブームを含むが、本明細書に記載される実施形態を、可変角度ブームを有する掘削機で利用できることを理解されたい。さらに、掘削機を、バケット２０の他に、オーガ、トレンチャ、コンパクターなどの他の器具または工具と共に使用することができる。

【0023】

[0032]掘削機は、一般に、様々なセンサを利用して、様々な機械要素の位置を監視し、かつ／または要素位置の表示をオペレータに提供する。一例として、プラットフォーム１１とブーム１２とスティック１６とバケット２０との間の相対角度を、エンコーダ、旋回軸１４、１８、２２と関連付けられたセンサ、油圧装置２４、２６、２８と関連付けられたストリングエンコーダ、および／またはその他のセンサを使用して決定することができる。あるいは、ＩＭＵなどの傾斜計を使用して、重力に対する剛体の角度を決定することもできる。図１の例では、掘削機は、プラットフォーム１１上のＩＭＵ４２、ブーム１２上のＩＭＵ４４、スティック１６上のＩＭＵ４６、およびバケット２０上のＩＭＵ４８を含む。これらのＩＭＵを使用して、例えば、剛体の重力加速度や角速度を決定することができる。ＩＭＵは１つまたは複数の軸で動作し得る。

【0024】

[0033]この例の掘削機は、関連付けられたメモリを有するコントローラ５０を含む。コントローラ５０は、ＩＭＵに応答して、剛体の相対角度に基づいてバケット２０および／またはトゥース３０の位置を決定し得る。位置は、プラットフォーム１１もしくはプラットフォーム１１上の点に対して決定することができ、またはプラットフォーム１１は、別の座標フレームで位置が決定されることを可能にする、全地球航法衛星システム（ＧＮＳＳ）測位システム３４などの位置センサを含み得る。

【0025】

[0034]コントローラ５０は、バケット２０の位置と、作業現場の掘削点における所望の高さとの間の差も決定し得る。所望の高さは、メモリに格納されている所望のトポロジーから決定され得る。オペレータがキャブ３１内のコントロールを使用して掘削機の運動を制御し、かつ／またはコントローラ５０が自動機械制御を提供し得る。キャブ３１内のディスプレイ７８は、バケット２０の現在位置および所望の高さを示すことによりオペレータを支援し得る。自動機械制御は、所望の高さに対するバケット２０および／またはトゥース３０の位置に基づいて部品または剛体を動かし得る。

【0026】

[0035]図２は、一実施形態による、剛体ｎと前の剛体ｎ－１との間の観測された成分相対角度

【数1】

および

【数2】

を決定する方法を示すフローチャートである。この例の剛体ｎおよび前の剛体ｎ－１は、旋回可能に結合された剛体である。一実施形態では、観測された成分相対角度

【数3】

および

【数4】

は、相対角度観測モジュールで、前の剛体ｎ－１の重力加速度ベクトルＡｃｃＧ_ｎ－１と剛体ｎの重力加速度ベクトルＡｃｃＧ_ｎとに基づいて計算される。一例では、重力加速度ベクトルＡｃｃＧは、要素ｘ、要素ｙ、および要素ｚを有する３Ｄベクトルである。各要素は、通常、対応する剛体のｘ軸、ｙ軸、およびｚ軸に整列される。

【0027】

[0036]一般に、重力加速度はＩＭＵによって直接測定されない。代わりに、重力加速度、または重力加速度ベクトルＡｃｃＧ_ｎを、ＩＭＵ観測値から公知の技法を使用して決定することができる。

【0028】

[0037]剛体ｎと前の剛体ｎ－１との間の観測された成分相対角度

【数5】

および

【数6】

は、図３に記載されるような相対角度推定モジュールにおいて推定成分相対角度

【数7】

および

【数8】

を決定するために使用され得る。推定成分相対角度は、観測された成分相対角度よりも正確であり得る。

【0029】

[0038]特定の実施形態では、観測された成分相対角度

【数9】

および

【数10】

は、剛体ｎに取り付けられたＩＭＵ_ｎおよび前の剛体ｎ－１に取り付けられたＩＭＵ_ｎ－１からの観測値と次の式とを使用して計算され得る。

【数11】

【0030】

[0039]図３の相対角度推定モジュールでは、剛体ｎの角速度（ｇ_ｎ）、前の剛体ｎ－１の角速度（ｇ_ｎ－１）、前の剛体ｎ－１から現在の剛体ｎへの回転行列

【数12】

、カルマン・フィルタ・パラメータにおけるプロセスノイズ共分散行列、カルマン・フィルタ・パラメータにおける観測ノイズ共分散行列、およびカルマン・フィルタ・パラメータにおける初期推定誤差共分散行列（Ｑ、Ｒ、Ｐ）のそれぞれが、観測された成分相対角度

【数13】

および

【数14】

と共に成分相対角度推定値

【数15】

および

【数16】

を決定するために使用される。

【0031】

[0040]特定の実施形態では、第１の相対角速度ωが、次式：

【数17】

を、プロセスモデル：

【数18】

観測モデル：

【数19】

ならびに、カルマン・フィルタ・パラメータに基づくプロセスノイズ共分散行列、観測ノイズ共分散行列、および初期推定誤差共分散行列としてのＱ、Ｒ、およびＰのそれぞれと共に使用して計算される。

【0032】

[0041]カルマンフィルタについての次の状態ベクトル：

【数20】

および観測ベクトル：

【数21】

を定義する。
カルマンフィルタの時間更新は、

【数22】

であり、カルマンフィルタの測定の更新は、

【数23】

であり、式中、上付き文字「＋」および上付き文字「－」は、それぞれ事後推定値および事前推定値を表す。Ｋ（ｔ）はカルマンゲインであり、上付き文字「Ｔ」は行列転置演算子である。各エポックタイムにおいて、Ｘ^＋は、

【数24】

および

【数25】

の推定ベクトルである。

【0033】

[0042]図４は、いくつかの実施形態による、剛体ｎと前の剛体ｎ－１との間の成分相対角度

【数26】

および

【数27】

を決定する方法を示すフローチャートである。一例では、成分相対角度は、正弦成分角度および余弦成分角度である。このフローチャートには、入力ＡｃｃＧ_ｎ－１、入力ＡｃｃＧ_ｎ、入力ｇ_ｎ－１、入力ｇ_ｎ、入力

【数28】

、および入力Ｑ、Ｒ、Ｐを使用して、どのようにして剛体ｎにおける成分相対角度

【数29】

および

【数30】

を決定できるかが示されている。このフローチャートには、出力ＡｃｃＧ_ｎおよび出力ｇ_ｎをどのようにして次の剛体ｎ＋１に渡すことができるかも示されている。他の実施形態と同様に、この図には、剛体ｎにおける成分相対角度を、絶対角度を一切必要とせずに、剛体ｎおよび前の剛体ｎ－１からのＩＭＵ観測値に基づいて決定できることが示されている。

【0034】

[0043]図５は、一実施形態による、剛体間の成分相対角度を決定する方法を示すフローチャートである。この図には、剛体_１、剛体_２、および剛体_ｎにおける成分相対角度の計算を表す３つのボックスが示されている。具体的に図示されていないが、各剛体にはＩＭＵが取り付けられており、各剛体は図１に示される例と同様に連続して配置されている。

【0035】

[0044]図５の各ボックスは、成分相対角度を計算するために使用されるいくつかの入力を有する。入力は、ボックスの内方に向かう矢印で表されている。各ボックスの左側に示されている入力は、連続における前の剛体と関連付けられている入力である。各ボックスの上側に示されている入力は、現在の剛体と関連付けられている入力である。

【0036】

[0045]出力は、ボックスから外方を指し示す矢印で表されている。各ボックスの右側に示されている出力は、現在の剛体から連続における次の剛体に渡される値である。各ボックスの下側に示されている出力は、成分相対角度である。

【0037】

[0046]（連続の最初の剛体または最初の剛体に結合されたＩＭＵを除く）剛体ごとに、前の剛体と関連付けられたＩＭＵの観測値から決定される入力は次のとおりである。
ｇ_ｎ－１は、前の剛体の角速度ベクトルである。
ＡｃｃＧ_ｎ－１は、前の剛体の重力加速度ベクトルである。

【0038】

[0047]剛体ごとに、剛体と関連付けられたＩＭＵの観測値から決定される入力は次のとおりである。
ｇ_ｎは、現在の剛体の角速度ベクトルである。
ＡｃｃＧ_ｎは、現在の剛体の重力加速度ベクトルである。

【0039】

[0048]現在の剛体から次の剛体に渡される出力は次のとおりである。
ｇ_ｎは、現在の剛体の角速度ベクトルである。
ＡｃｃＧ_ｎは、現在の剛体の重力加速度ベクトルである。

【0040】

[0049]前の剛体は連続における直前の剛体であり、次の剛体は連続における直後の剛体である。いくつかの実施形態では、剛体（すなわち、剛体ｎ）と前の剛体（すなわち、剛体ｎ－１）との間の相対角度は、推定相対角度

【数31】

によって表され、さらに推定成分相対角度

【数32】

および

【数33】

によって表される。剛体ｎにおける重力加速度ベクトルおよび角速度を入力の一部として使用して、推定成分相対角度

【数34】

および

【数35】

を決定することができる。この例に示されるように、成分相対角度

【数36】

および

【数37】

を、連続における先の剛体からの情報を必要とせずに、前の剛体および剛体からの情報に基づいて決定することができる。この例は、推定成分相対角度

【数38】

および

【数39】

を決定するために絶対角度が不要であることも示している。

【0041】

[0050]いくつかの実施形態では、剛体ｎと前の剛体ｎ－１との間の成分相対角度

【数40】

および

【数41】

は、（ｉ）観測された成分相対角度

【数42】

および

【数43】

と、（ｉｉ）剛体ｎの角速度（ｇ_ｎ）と、（ｉｉｉ）前の剛体ｎ－１の角速度（ｇ_ｎ－１）と、前の剛体から現在の剛体への回転行列

【数44】

と、カルマン・フィルタ・パラメータにおけるプロセスノイズ共分散行列、カルマン・フィルタ・パラメータにおける観測ノイズ共分散行列、およびカルマン・フィルタ・パラメータにおける初期推定誤差共分散行列（Ｑ、Ｒ、Ｐ）のそれぞれとに基づいて計算される。これは、例えば、図３に示されている。繰り返すが、成分相対角度は、連続における先の剛体からの情報または剛体の絶対角度を必要とせずに、剛体ｎおよび前の剛体ｎ－１からの測定値および計算されたパラメータに基づいて決定される。

【0042】

[0051]図６は、いくつかの実施形態による、制御システムの簡略化された図である。システムは、例えば、図１に示される掘削機（例えば、コントローラ５０）と共に使用され得る状態推定器を含む。この図には、プラットフォーム、ブーム、スティック、およびバケットと関連付けられたＩＭＵからの出力が状態推定器にどのように渡されるが示されている。ＩＭＵは、例えば、測定された加速度および角速度を状態推定器に提供する。状態推定器は、それらの測定値を様々な実施形態に記載されるような他の入力と共に使用して、相対角度および／または成分相対角度を決定する。いくつかの実施形態では、状態推定器は、本明細書に記載される器具（例えば、バケットおよび／またはトゥース）の位置も決定し得る。これらの値および／または位置は、オペレータを支援するためにディスプレイ上に出力され得る。

【0043】

[0052]状態推定器および／またはコントローラは、通常、良く知られたソフトウェアコンポーネントおよびハードウェアコンポーネントを含む。例えば、状態推定器および／またはコントローラは、１つまたは複数のオペレーティングシステム、プロセッサ、記憶のためのローカルメモリ、入出力装置、およびハードウェアコンポーネントを相互接続するシステムバスを含み得る。ＲＡＭおよびディスクドライブが、データおよびコンピュータープログラムの記憶のためのローカルメモリの例である。他のタイプのローカルメモリには、磁気記憶媒体、光記憶媒体、フラッシュメモリやソリッド・ステート・メモリ、ネットワーク接続された記憶装置などが含まれる。

【0044】

[0053]図７は、いくつかの実施形態による、剛体間の相対角度を推定する方法を示すフローチャートである。剛体は、図１に示される掘削機の剛体と同様に連続として結合され得る。この方法は、前の剛体に取り付けられたＩＭＵの観測値から前の剛体の重力加速度ベクトルおよび角速度を取得すること７０１を含む。前の剛体は、剛体の連続における直前の剛体である。この方法は、現在の剛体に取り付けられたＩＭＵの観測値から現在の剛体の重力加速度ベクトルおよび角速度を取得すること７０２も含む。

【0045】

[0054]この方法は、前の剛体と現在の剛体との間の第１の成分相対角度および第２の成分相対角度を、前の剛体の重力加速度ベクトルおよび角速度と、現在の剛体の重力加速度ベクトルおよび角速度とに少なくとも一部は基づいて推定すること７０３も含む。前述のように、成分相対角度を、前の剛体の重力加速度ベクトルと、現在の剛体の重力加速度ベクトルと、前の剛体の角速度と、現在の剛体の角速度とに基づいて決定することができる。前述のように、成分相対角度を推定することは、前の剛体から現在の剛体への回転行列、前の剛体と現在の剛体との間の中間の観測された第１の成分相対角度および第２の成分相対角度、カルマン・フィルタ・パラメータを用いたプロセスノイズ共分散、カルマン・フィルタ・パラメータを用いた観測ノイズ共分散、およびカルマン・フィルタ・パラメータを用いた初期推定誤差共分散も含むことができる。

【0046】

[0055]図８は、いくつかの実施形態による、剛体間の相対角度を推定する方法を示すフローチャートである。剛体は、図１に示される掘削機の剛体と同様に連続として結合され得る。この方法は、剛体に取り付けられたＩＭＵの観測値から剛体の重力加速度ベクトルおよび角速度を受け取ること８０１を含む。この方法は、剛体に取り付けられたＩＭＵの観測値から次の剛体の重力加速度ベクトルおよび角速度を受け取ること８０２も含む。次の剛体は、剛体の連続におけるすぐ次の剛体である。

【0047】

[0056]この方法は、剛体と次の剛体との間の第１の成分相対角度および第２の成分相対角度を、剛体の重力加速度ベクトルおよび角速度と、次の剛体の重力加速度ベクトルおよび角速度とに少なくとも一部は基づいて推定すること８０３も含む。前述のように、成分相対角度を、剛体の重力加速度ベクトルと、次の剛体の重力加速度ベクトルと、剛体の角速度と、次の剛体の角速度とに基づいて決定することができる。前述のように、成分相対角度を推定することは、剛体から次の剛体への回転行列、剛体と次の剛体との間の中間の観測された第１の成分相対角度および第２の成分相対角度、カルマン・フィルタ・パラメータを用いたプロセスノイズ共分散、カルマン・フィルタ・パラメータを用いた観測ノイズ共分散、およびカルマン・フィルタ・パラメータを用いた初期推定誤差共分散も含むことができる。

【0048】

[0057]図９は、いくつかの実施形態による、剛体間（すなわち、前の剛体と剛体との間）の観測された成分相対角度

【数45】

および

【数46】

に基づいて剛体間の相対角度を推定する方法を示すフローチャートである。剛体は、図１に示される掘削機の剛体と同様に連続として結合され得る。この方法は、前の剛体の重力加速度ベクトルおよび剛体の重力加速度ベクトルを受け取ること９０１を含む。この方法は、剛体の第１の観測された成分相対角度および剛体の第２の観測された成分相対角度を決定すること９０２も含む。この方法は、前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度を、前の剛体の重力加速度ベクトルおよび角速度と、剛体の重力加速度ベクトルおよび角速度と、剛体の第１の観測された成分相対角度および剛体の第２の観測された成分相対角度とに少なくとも一部は基づいて推定すること９０３も含む。前述のように、観測された成分相対角度を、前の剛体の重力加速度ベクトルと剛体の重力加速度ベクトルとに基づいて決定することができる。観測された成分相対角度に基づいて成分相対角度を推定することは、前の剛体から剛体への回転行列、カルマン・フィルタ・パラメータを用いたプロセスノイズ共分散、カルマン・フィルタ・パラメータを用いた観測ノイズ共分散、およびカルマン・フィルタ・パラメータの初期推定誤差共分散行列を含み得る。

【0049】

[0058]図１０は、いくつかの実施形態による、連続として配置された旋回可能に結合された剛体間の角度を決定する方法を示すフローチャートである。連続は、図１に示される掘削機の剛体のいくつかと同様に、前の剛体と次の剛体との間に配置された剛体を含む。この方法は、前の剛体に結合された剛体を含む連続として結合された剛体の配置に基づき、前の剛体と剛体との間の相対角度を、前の剛体の絶対角度または剛体の絶対角度を入力として使用せずに推定すること１００１を含む。

【0050】

[0059]図７～図１０に示される具体的なステップは、いくつかの実施形態による特定の方法を提供するものであることを理解されたい。代替の実施形態によれば、ステップの他の順序も実行され得る。例えば、代替の実施形態は、上記で概説されたステップを異なる順序で実行し得る。さらに、図７～図１０に示される個々のステップは、様々な順序で実行され得る複数のサブステップを含み得る。さらに、個々の用途に応じて、追加ステップが追加されるか、または除去されてもよい。

【0051】

[0060]いくつかの実施形態は、ハードウェア、ソフトウェア、ファームウェア、ミドルウェア、マイクロコード、ハードウェア記述言語、またはこれらの任意の組み合わせによって実施され得ることを理解されたい。ソフトウェア、ファームウェア、ミドルウェア、またはマイクロコードとして実施される場合、必要なタスクを実行するプログラムコードまたはコードセグメントは、記憶媒体などのコンピュータ可読媒体に格納され得る。プロセッサは、必要なタスクを実行するように適合され得る。「コンピュータ可読媒体」という用語には、可搬式または固定式の記憶装置、光記憶装置、無線チャネル、ＳＩＭカード、その他のスマートカード、および命令またはデータを格納、収容、または搬送することができる様々な他の非一時的な媒体が含まれるが、これらに限定されない。

【0052】

[0061]本発明を特定の実施形態に関して説明してきたが、本発明の範囲が本明細書に記載される実施形態に限定されないことは当業者には明らかなはずである。例えば、本発明の１つまたは複数の実施形態の特徴は、本発明の範囲から逸脱することなく、他の実施形態の１つまたは複数の特徴と組み合わされ得る。したがって、本明細書および図面は、限定的意味ではなく例示として評価されるべきである。よって、本発明の範囲は、上記の説明を参照してではなく、添付の特許請求の範囲およびその均等物の全範囲を参照して決定されるべきである。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】