(57)【要約】  連続が前の剛体に結合された少なくとも１つの剛体を含む、連続として配置された結合された剛体間の相対角度および／または成分相対角度を決定するための方法。本方法は、前の剛体の重力加速度ベクトルおよび角速度と剛体の重力加速度および角速度とを取得することを含む。相対角度および／または成分相対角度は剛体の絶対角度を使用せずに決定される。【選択図】図５
2021512325.tif 000002
【特許請求の範囲】
【請求項１】
分離しているが結合されている剛体間の成分相対角度を決定するための方法であって、前記方法が、
前の剛体に取り付けられたＩＭＵの観測値から前記前の剛体の重力加速度ベクトルおよび角速度を取得することと、
剛体に取り付けられたＩＭＵの観測値から前記剛体の重力加速度ベクトルおよび角速度を取得することと、
前記前の剛体の前記重力加速度ベクトルおよび前記角速度と前記剛体の前記重力加速度ベクトルおよび前記角速度とに少なくとも一部は基づいて、前記前の剛体と前記剛体との間の第１の成分相対角度および第２の成分相対角度を推定することと
を含む、方法。
【請求項２】
前記前の剛体と前記剛体との間の前記第１の成分相対角度および前記第２の成分相対角度が、前記前の剛体の絶対角度または前記剛体の絶対角度を使用せずに決定される、請求項１に記載の方法。
【請求項３】
前記第１の成分相対角度が正弦成分角度であり、前記第２の成分相対角度が余弦成分角度である、請求項１に記載の方法。
【請求項４】
前記正弦成分角度および前記余弦成分角度を使用して前記前の剛体と前記剛体との間の相対角度を決定することをさらに含む、請求項３に記載の方法。
【請求項５】
前記第１の成分相対角度および前記第２の成分相対角度を推定することが、
前記剛体の第１の観測された成分相対角度および第２の観測された成分相対角度を決定することと、
前記前の剛体から前記剛体への回転行列を決定することと、
少なくとも１つの共分散行列を決定することと
を含む、請求項１に記載の方法。
【請求項６】
前記剛体の前記第１の観測された成分相対角度および前記第２の観測された成分相対角度が、前記前の剛体の前記重力加速度ベクトルおよび前記剛体の前記重力加速度ベクトルを使用して決定される、請求項５に記載の方法。
【請求項７】
慣性計測装置（ＩＭＵ）が各々配置されている結合された剛体間の成分相対角度を推定するための方法であって、前記方法が、
前の剛体に取り付けられたＩＭＵの観測値から決定された前記前の剛体の重力加速度ベクトルを受け取ることと、
前記前の剛体に取り付けられた前記ＩＭＵの観測値から前記前の剛体の角速度を受け取ることと、
剛体に取り付けられたＩＭＵの観測値から決定された前記剛体の重力加速度ベクトルを受け取ることと、
前記剛体に取り付けられた前記ＩＭＵの観測値から前記剛体の角速度を受け取ることと、
前記前の剛体の前記重力加速度ベクトルおよび前記角速度と前記剛体の前記重力加速度ベクトルおよび前記角速度とに少なくとも一部は基づいて、前記前の剛体と前記剛体との間の第１の成分相対角度および第２の成分相対角度を推定することと、
次の剛体に取り付けられたＩＭＵの観測値から決定された前記次の剛体の重力加速度ベクトルおよび角速度を受け取ることと、
前記剛体の前記重力加速度ベクトルおよび前記角速度と前記次の剛体の前記重力加速度ベクトルおよび前記角速度とに少なくとも一部は基づいて、前記剛体と前記次の剛体との間の第１の成分相対角度および第２の成分相対角度を推定することと
を含む、方法。
【請求項８】
前記前の剛体と前記剛体との間の前記第１の成分相対角度および前記第２の成分相対角度が、前記前の剛体の絶対角度または前記剛体の絶対角度を使用せずに決定される、請求項７に記載の方法。
【請求項９】
前記前の剛体と前記剛体との間の前記第１の成分相対角度が正弦成分角度であり、前記前の剛体と前記剛体との間の前記第２の成分相対角度が余弦成分角度である、請求項７に記載の方法。
【請求項１０】
前記正弦成分角度および前記余弦成分角度を使用して前記前の剛体と前記剛体との間の相対角度を決定することをさらに含む、請求項９に記載の方法。
【請求項１１】
前記剛体と前記次の剛体との間の前記第１の成分相対角度および前記第２の成分相対角度が、前記剛体の絶対角度または前記次の剛体の絶対角度を使用せずに決定される、請求項７に記載の方法。
【請求項１２】
前記剛体と前記次の剛体との間の前記第１の成分相対角度および前記第２の成分相対角度が、前記前の剛体の前記重力加速度ベクトルを使用せずに決定される、請求項７に記載の方法。
【請求項１３】
前記前の剛体と前記剛体との間の前記第１の成分相対角度および前記第２の成分相対角度を推定することが、
前記剛体の第１の観測された成分相対角度および第２の観測された成分相対角度を受け取ることと、
前記前の剛体から前記剛体への回転行列を受け取ることと、
少なくとも１つの共分散行列を受け取ることと
を含む、請求項７に記載の方法。
【請求項１４】
前記前の剛体と前記剛体との間の前記第１の観測された成分相対角度および前記第２の観測された成分相対角度が、前記前の剛体の前記重力加速度ベクトルおよび前記剛体の前記重力加速度ベクトルを使用して決定される、請求項１３に記載の方法。
【請求項１５】
前の剛体および剛体を含む、一連の分離しているが結合されている剛体間の成分相対角度を決定するための方法であって、前記方法が、
前記前の剛体の重力加速度ベクトルおよび前記剛体の重力加速度ベクトルを受け取ることと、
前記前の剛体と前記剛体との間の第１の観測された成分相対角度および第２の観測された成分相対角度を計算することと、
前記前の剛体の角速度、前記剛体の角速度、前記前の剛体から前記剛体への回転行列、および少なくとも１つの共分散行列を受け取ることと、
前記第１の観測された成分相対角度および前記第２の観測された成分相対角度と、前記前の剛体の前記角速度と、前記剛体の前記角速度と、前記前の剛体から前記剛体への前記回転行列と、前記少なくとも１つの共分散行列とに基づいて、前記前の剛体と前記剛体との間の第１の成分相対角度および第２の成分相対角度を計算することと
を含む、方法。
【請求項１６】
前記第１の成分相対角度および前記第２の成分相対角度が、前記前の剛体の絶対角度または前記剛体の絶対角度を使用せずに計算される、請求項１５に記載の方法。
【請求項１７】
前記少なくとも１つの共分散行列を受け取ることが、初期推定誤差共分散行列、プロセスノイズ共分散行列、または観測ノイズ共分散行列のうちの少なくとも１つを受け取ることを含む、請求項１５に記載の方法。
【請求項１８】
前記剛体と次の剛体との間の相対角度を、前記前の剛体と前記剛体との間の前記第１の成分相対角度および前記第２の成分相対角度を使用せずに決定することをさらに含む、請求項１５に記載の方法。
【請求項１９】
連続して配置された旋回可能に結合された剛体間の相対角度を決定するための方法であって、前記方法が、
前の剛体に取り付けられたＩＭＵの観測値から決定された前記前の剛体の重力加速度ベクトルおよび角速度を受け取ることと、
剛体に取り付けられたＩＭＵの観測値から決定された前記剛体の重力加速度ベクトルおよび角速度を受け取ることと、
前記前の剛体の前記重力加速度ベクトルおよび前記角速度と、前記剛体の前記重力加速度ベクトルおよび前記角速度とに基づいて前記前の剛体と前記剛体との間の相対角度を推定することであって、前記前の剛体と前記剛体との間の前記相対角度が、前記前の剛体の絶対角度または前記剛体の絶対角度を使用せずに決定されることと
を含む、方法。
【発明の詳細な説明】
【技術分野】
【０００１】
[0001]相互参照
本出願は、２０１８年２月２日に出願された「慣性計測装置を使用した相対角度推定」という名称の米国非仮特許出願第１５／８８７７１７号の利益と優先権を主張し、その全内容はあらゆる目的でその全体が参照により本明細書に組み込まれる。
【０００２】
[0002]本明細書に記載される実施形態は、一般に、慣性計測装置（ｉｎｅｒｔｉａｌ  ｍｅａｓｕｒｅｍｅｎｔ  ｕｎｉｔ（ＩＭＵ））を使用して結合された剛体間の相対角度を推定することに関する。
【背景技術】
【０００３】
[0003]ＩＭＵは、通常、加速度計およびジャイロスコープを使用して物体の直線運動および角運動を測定するセンサである。ＩＭＵは、物体の動きに関する情報が求められる多くの用途で一般的に使用される。例には、車両、航空機、および衛星が含まれる。ＩＭＵは、大きな物体の様々な部分の運動を測定するためにも一般的に使用される。
【０００４】
[0004]ＩＭＵを使用して結合された剛体間の相対角度を決定する改善された方法が求められている。
【発明の概要】
【課題を解決するための手段】
【０００５】
[0005]本明細書に記載される実施形態は、２つ以上の結合された剛体間の相対角度および／または成分相対角度ｓｉｎ（α）ｃｏｓ（α）を決定するための改善された方法を提供する。相対角度は、剛体に取り付けられたＩＭＵからの情報を使用して決定される。
【０００６】
[0006]一実施形態によれば、分離しているが結合されている剛体間の成分相対角度を決定するための方法は、前の剛体に取り付けられたＩＭＵの観測値から前の剛体の重力加速度ベクトルおよび角速度を取得することと、剛体に取り付けられたＩＭＵの観測値から剛体の重力加速度ベクトルおよび角速度を取得することと、を含む。前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度が、前の剛体の重力加速度ベクトルおよび角速度と、剛体の重力加速度ベクトルおよび角速度とに少なくとも一部は基づいて推定される。
【０００７】
[0007]一実施形態では、前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度は、前の剛体の絶対角度または剛体の絶対角度を使用せずに決定される。
【０００８】
[0008]別の実施形態では、第１の成分相対角度は正弦成分角度であり、第２の成分相対角度は余弦成分角度である。
【０００９】
[0009]別の実施形態では、本方法は、正弦成分角度および余弦成分角度を使用して前の剛体と剛体との間の相対角度を決定することも含む。
【００１０】
[0010]別の実施形態では、第１の成分相対角度および第２の成分相対角度を推定することが、剛体の第１の観測された成分相対角度および第２の観測された成分相対角度を決定することと、前の剛体から剛体への回転行列を決定することと、少なくとも１つの共分散行列を決定することと、を含む。
【００１１】
[0011]さらに別の実施形態では、剛体の第１の観測された成分相対角度および第２の観測された成分相対角度は、前の剛体の重力加速度ベクトルおよび剛体の重力加速度ベクトルを使用して決定される。
【００１２】
[0012]別の実施形態によれば、ＩＭＵが各々配置されている結合された剛体間の成分相対角度を推定するための方法は、前の剛体に取り付けられたＩＭＵの観測値から決定された前の剛体の重力加速度ベクトルを受け取ることと、前の剛体に取り付けられたＩＭＵの観測値から前の剛体の角速度を受け取ることと、剛体に取り付けられたＩＭＵの観測値から決定された剛体の重力加速度ベクトルを受け取ることと、剛体に取り付けられたＩＭＵの観測値から剛体の角速度を受け取ることと、を含む。前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度が、前の剛体の重力加速度ベクトルおよび角速度と、剛体の重力加速度ベクトルおよび角速度とに少なくとも一部は基づいて推定される。本方法は、次の剛体に取り付けられたＩＭＵの観測値から決定された次の剛体の重力加速度ベクトルおよび角速度を受け取ること、も含む。剛体と次の剛体との間の第１の成分相対角度および第２の成分相対角度が、剛体の重力加速度ベクトルおよび角速度と、次の剛体の重力加速度ベクトルおよび角速度とに少なくとも一部は基づいて推定される。
【００１３】
[0013]一実施形態では、剛体と次の剛体との間の第１の成分相対角度および第２の成分相対角度は、剛体の絶対角度または次の剛体の絶対角度を使用せずに決定される。
【００１４】
[0014]別の実施形態では、剛体と次の剛体との間の第１の成分相対角度および第２の成分相対角度は、前の剛体の重力加速度ベクトルを使用せずに決定される。
【００１５】
[0015]別の実施形態によれば、前の剛体および剛体を含む、一連の分離しているが結合されている剛体間の成分相対角度を決定するための方法は、前の剛体の重力加速度ベクトルおよび剛体の重力加速度ベクトルを受け取ることと、前の剛体と剛体との間の第１の観測された成分相対角度および第２の観測された成分相対角度を計算することと、を含む。本方法は、前の剛体の角速度、剛体の角速度、前の剛体から剛体への回転行列、および少なくとも１つの共分散行列を受け取ること、も含む。前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度が、第１の観測された成分相対角度および第２の観測された成分相対角度と、前の剛体の角速度と、剛体の角速度と、前の剛体から剛体への回転行列と、少なくとも１つの共分散行列とに基づいて計算される。
【００１６】
[0016]一実施形態では、少なくとも１つの共分散行列を受け取ることは、初期推定誤差共分散行列、プロセスノイズ共分散行列、または観測ノイズ共分散行列のうちの少なくとも１つを受け取ること、を含む。
【００１７】
[0017]別の実施形態では、本方法は、剛体と次の剛体との間の相対角度を、前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度を使用せずに決定すること、も含む。
【００１８】
[0018]さらに別の実施形態によれば、連続して配置された旋回可能に結合された剛体間の相対角度を決定するための方法は、前の剛体に取り付けられたＩＭＵの観測値から決定された前の剛体の重力加速度ベクトルおよび角速度を受け取ることと、剛体に取り付けられたＩＭＵの観測値から決定された剛体の重力加速度ベクトルおよび角速度を受け取ることと、を含む。前の剛体と剛体との間の相対角度が、前の剛体の重力加速度ベクトルおよび角速度と、剛体の重力加速度ベクトルおよび角速度とに基づいて推定される。前の剛体と剛体との間の相対角度は、前の剛体の絶対角度または剛体の絶対角度を使用せずに決定される。
【図面の簡単な説明】
【００１９】
【図１】一実施形態による、掘削機（一連の結合された剛体を有する物体の例）の簡略化された側面図である。
【図２】一実施形態による、剛体間の観測された成分相対角度を決定する方法を示すフローチャートである。
【図３】一実施形態による、剛体間の成分相対角度を決定する方法を示すフローチャートである。
【図４】いくつかの実施形態による、剛体間の成分相対角度を決定する方法を示すフローチャートである。
【図５】いくつかの実施形態による、一連の剛体間の成分相対角度を決定する方法を示すフローチャートである。
【図６】いくつかの実施形態による、制御システムの簡略化された図である。
【図７】一実施形態による、前の剛体と剛体との間の成分相対角度を決定する方法を示すフローチャートである。
【図８】一実施形態による、剛体と次の剛体との間の相対角度を決定する方法を示すフローチャートである。
【図９】一実施形態による、連続として配置された剛体間の成分相対角度を決定する方法を示すフローチャートである。
【図１０】一実施形態による、連続として配置された旋回可能に結合された剛体間の相対角度を決定する方法を示すフローチャートである。
【発明を実施するための形態】
【００２０】
[0029]本明細書に記載される実施形態は、結合された剛体間の相対角度および／または成分相対角度を決定する改善された方法を提供する。いくつかの実施形態では、例えば、成分相対角度を、連続における先の剛体からの情報を必要とせずに、剛体および連続における直前の剛体からの測定値および計算されたパラメータに基づいて決定することができる。
【００２１】
[0030]本出願全体を通して使用される一連の剛体の一例が、相互に旋回可能に結合されているいくつかの可動部品（または移動体）を含む掘削機である。掘削機は一例として使用されているにすぎず、本明細書に記載される実施形態は、連続して結合された剛体を含む任意の他の重機、車両、機械、または物体と共に使用され得ることを理解されたい。
【００２２】
[0031]図１は、プラットフォーム１１、ブーム１２、スティック１６、およびバケット２０を含む掘削機の簡略化された側面図である。ブーム１２は、旋回軸１４でプラットフォーム１１に旋回可能に結合されており、スティック１６は、旋回軸１８でブーム１２に旋回可能に結合されており、バケット２０は、旋回軸２２でスティック１６に旋回可能に結合されている。ブーム１２、スティック１６、およびバケット２０を動かすために、油圧装置２４、２６、２８が設けられている。バケット２０は、掘削の助けとなり得るトゥース３０を含む。プラットフォーム１１は、作業現場での掘削機の移動を容易にするために車輪または無限軌道を含み得る下部走行体３２上に支持されたキャブ３１を含む。プラットフォーム１１は、油圧モータ３３によっておおむね垂直な軸３５を中心に回転させることができる。この例は一体型ブームを含むが、本明細書に記載される実施形態を、可変角度ブームを有する掘削機で利用できることを理解されたい。さらに、掘削機を、バケット２０の他に、オーガ、トレンチャ、コンパクターなどの他の器具または工具と共に使用することができる。
【００２３】
[0032]掘削機は、一般に、様々なセンサを利用して、様々な機械要素の位置を監視し、かつ／または要素位置の表示をオペレータに提供する。一例として、プラットフォーム１１とブーム１２とスティック１６とバケット２０との間の相対角度を、エンコーダ、旋回軸１４、１８、２２と関連付けられたセンサ、油圧装置２４、２６、２８と関連付けられたストリングエンコーダ、および／またはその他のセンサを使用して決定することができる。あるいは、ＩＭＵなどの傾斜計を使用して、重力に対する剛体の角度を決定することもできる。図１の例では、掘削機は、プラットフォーム１１上のＩＭＵ４２、ブーム１２上のＩＭＵ４４、スティック１６上のＩＭＵ４６、およびバケット２０上のＩＭＵ４８を含む。これらのＩＭＵを使用して、例えば、剛体の重力加速度や角速度を決定することができる。ＩＭＵは１つまたは複数の軸で動作し得る。
【００２４】
[0033]この例の掘削機は、関連付けられたメモリを有するコントローラ５０を含む。コントローラ５０は、ＩＭＵに応答して、剛体の相対角度に基づいてバケット２０および／またはトゥース３０の位置を決定し得る。位置は、プラットフォーム１１もしくはプラットフォーム１１上の点に対して決定することができ、またはプラットフォーム１１は、別の座標フレームで位置が決定されることを可能にする、全地球航法衛星システム（ＧＮＳＳ）測位システム３４などの位置センサを含み得る。
【００２５】
[0034]コントローラ５０は、バケット２０の位置と、作業現場の掘削点における所望の高さとの間の差も決定し得る。所望の高さは、メモリに格納されている所望のトポロジーから決定され得る。オペレータがキャブ３１内のコントロールを使用して掘削機の運動を制御し、かつ／またはコントローラ５０が自動機械制御を提供し得る。キャブ３１内のディスプレイ７８は、バケット２０の現在位置および所望の高さを示すことによりオペレータを支援し得る。自動機械制御は、所望の高さに対するバケット２０および／またはトゥース３０の位置に基づいて部品または剛体を動かし得る。
【００２６】
[0035]図２は、一実施形態による、剛体ｎと前の剛体ｎ−１との間の観測された成分相対角度
【数１】
2021512325.tif 000003
および
【数２】
2021512325.tif 000004
を決定する方法を示すフローチャートである。この例の剛体ｎおよび前の剛体ｎ−１は、旋回可能に結合された剛体である。一実施形態では、観測された成分相対角度
【数３】
2021512325.tif 000005
および
【数４】
2021512325.tif 000006
は、相対角度観測モジュールで、前の剛体ｎ−１の重力加速度ベクトルＡｃｃＧｎ−１と剛体ｎの重力加速度ベクトルＡｃｃＧｎとに基づいて計算される。一例では、重力加速度ベクトルＡｃｃＧは、要素ｘ、要素ｙ、および要素ｚを有する３Ｄベクトルである。各要素は、通常、対応する剛体のｘ軸、ｙ軸、およびｚ軸に整列される。
【００２７】
[0036]一般に、重力加速度はＩＭＵによって直接測定されない。代わりに、重力加速度、または重力加速度ベクトルＡｃｃＧｎを、ＩＭＵ観測値から公知の技法を使用して決定することができる。
【００２８】
[0037]剛体ｎと前の剛体ｎ−１との間の観測された成分相対角度
【数５】
2021512325.tif 000007
および
【数６】
2021512325.tif 000008
は、図３に記載されるような相対角度推定モジュールにおいて推定成分相対角度
【数７】
2021512325.tif 000009
および
【数８】
2021512325.tif 000010
を決定するために使用され得る。推定成分相対角度は、観測された成分相対角度よりも正確であり得る。
【００２９】
[0038]特定の実施形態では、観測された成分相対角度
【数９】
2021512325.tif 000011
および
【数１０】
2021512325.tif 000012
は、剛体ｎに取り付けられたＩＭＵｎおよび前の剛体ｎ−１に取り付けられたＩＭＵｎ−１からの観測値と次の式とを使用して計算され得る。
【数１１】
2021512325.tif 000013
【００３０】
[0039]図３の相対角度推定モジュールでは、剛体ｎの角速度（ｇｎ）、前の剛体ｎ−１の角速度（ｇｎ−１）、前の剛体ｎ−１から現在の剛体ｎへの回転行列
【数１２】
2021512325.tif 000014
、カルマン・フィルタ・パラメータにおけるプロセスノイズ共分散行列、カルマン・フィルタ・パラメータにおける観測ノイズ共分散行列、およびカルマン・フィルタ・パラメータにおける初期推定誤差共分散行列（Ｑ、Ｒ、Ｐ）のそれぞれが、観測された成分相対角度
【数１３】
2021512325.tif 000015
および
【数１４】
2021512325.tif 000016
と共に成分相対角度推定値
【数１５】
2021512325.tif 000017
および
【数１６】
2021512325.tif 000018
を決定するために使用される。
【００３１】
[0040]特定の実施形態では、第１の相対角速度ωが、次式：
【数１７】
2021512325.tif 000019
を、プロセスモデル：
【数１８】
2021512325.tif 000020
観測モデル：
【数１９】
2021512325.tif 000021
ならびに、カルマン・フィルタ・パラメータに基づくプロセスノイズ共分散行列、観測ノイズ共分散行列、および初期推定誤差共分散行列としてのＱ、Ｒ、およびＰのそれぞれと共に使用して計算される。
【００３２】
[0041]カルマンフィルタについての次の状態ベクトル：
【数２０】
2021512325.tif 000022
および観測ベクトル：
【数２１】
2021512325.tif 000023
を定義する。
カルマンフィルタの時間更新は、
【数２２】
2021512325.tif 000024
であり、カルマンフィルタの測定の更新は、
【数２３】
2021512325.tif 000025
であり、式中、上付き文字「＋」および上付き文字「−」は、それぞれ事後推定値および事前推定値を表す。Ｋ（ｔ）はカルマンゲインであり、上付き文字「Ｔ」は行列転置演算子である。各エポックタイムにおいて、Ｘ＋は、
【数２４】
2021512325.tif 000026
および
【数２５】
2021512325.tif 000027
の推定ベクトルである。
【００３３】
[0042]図４は、いくつかの実施形態による、剛体ｎと前の剛体ｎ−１との間の成分相対角度
【数２６】
2021512325.tif 000028
および
【数２７】
2021512325.tif 000029
を決定する方法を示すフローチャートである。一例では、成分相対角度は、正弦成分角度および余弦成分角度である。このフローチャートには、入力ＡｃｃＧｎ−１、入力ＡｃｃＧｎ、入力ｇｎ−１、入力ｇｎ、入力
【数２８】
2021512325.tif 000030
、および入力Ｑ、Ｒ、Ｐを使用して、どのようにして剛体ｎにおける成分相対角度
【数２９】
2021512325.tif 000031
および
【数３０】
2021512325.tif 000032
を決定できるかが示されている。このフローチャートには、出力ＡｃｃＧｎおよび出力ｇｎをどのようにして次の剛体ｎ＋１に渡すことができるかも示されている。他の実施形態と同様に、この図には、剛体ｎにおける成分相対角度を、絶対角度を一切必要とせずに、剛体ｎおよび前の剛体ｎ−１からのＩＭＵ観測値に基づいて決定できることが示されている。
【００３４】
[0043]図５は、一実施形態による、剛体間の成分相対角度を決定する方法を示すフローチャートである。この図には、剛体１、剛体２、および剛体ｎにおける成分相対角度の計算を表す３つのボックスが示されている。具体的に図示されていないが、各剛体にはＩＭＵが取り付けられており、各剛体は図１に示される例と同様に連続して配置されている。
【００３５】
[0044]図５の各ボックスは、成分相対角度を計算するために使用されるいくつかの入力を有する。入力は、ボックスの内方に向かう矢印で表されている。各ボックスの左側に示されている入力は、連続における前の剛体と関連付けられている入力である。各ボックスの上側に示されている入力は、現在の剛体と関連付けられている入力である。
【００３６】
[0045]出力は、ボックスから外方を指し示す矢印で表されている。各ボックスの右側に示されている出力は、現在の剛体から連続における次の剛体に渡される値である。各ボックスの下側に示されている出力は、成分相対角度である。
【００３７】
[0046]（連続の最初の剛体または最初の剛体に結合されたＩＭＵを除く）剛体ごとに、前の剛体と関連付けられたＩＭＵの観測値から決定される入力は次のとおりである。
ｇｎ−１は、前の剛体の角速度ベクトルである。
ＡｃｃＧｎ−１は、前の剛体の重力加速度ベクトルである。
【００３８】
[0047]剛体ごとに、剛体と関連付けられたＩＭＵの観測値から決定される入力は次のとおりである。
ｇｎは、現在の剛体の角速度ベクトルである。
ＡｃｃＧｎは、現在の剛体の重力加速度ベクトルである。
【００３９】
[0048]現在の剛体から次の剛体に渡される出力は次のとおりである。
ｇｎは、現在の剛体の角速度ベクトルである。
ＡｃｃＧｎは、現在の剛体の重力加速度ベクトルである。
【００４０】
[0049]前の剛体は連続における直前の剛体であり、次の剛体は連続における直後の剛体である。いくつかの実施形態では、剛体（すなわち、剛体ｎ）と前の剛体（すなわち、剛体ｎ−１）との間の相対角度は、推定相対角度
【数３１】
2021512325.tif 000033
によって表され、さらに推定成分相対角度
【数３２】
2021512325.tif 000034
および
【数３３】
2021512325.tif 000035
によって表される。剛体ｎにおける重力加速度ベクトルおよび角速度を入力の一部として使用して、推定成分相対角度
【数３４】
2021512325.tif 000036
および
【数３５】
2021512325.tif 000037
を決定することができる。この例に示されるように、成分相対角度
【数３６】
2021512325.tif 000038
および
【数３７】
2021512325.tif 000039
を、連続における先の剛体からの情報を必要とせずに、前の剛体および剛体からの情報に基づいて決定することができる。この例は、推定成分相対角度
【数３８】
2021512325.tif 000040
および
【数３９】
2021512325.tif 000041
を決定するために絶対角度が不要であることも示している。
【００４１】
[0050]いくつかの実施形態では、剛体ｎと前の剛体ｎ−１との間の成分相対角度
【数４０】
2021512325.tif 000042
および
【数４１】
2021512325.tif 000043
は、（ｉ）観測された成分相対角度
【数４２】
2021512325.tif 000044
および
【数４３】
2021512325.tif 000045
と、（ｉｉ）剛体ｎの角速度（ｇｎ）と、（ｉｉｉ）前の剛体ｎ−１の角速度（ｇｎ−１）と、前の剛体から現在の剛体への回転行列
【数４４】
2021512325.tif 000046
と、カルマン・フィルタ・パラメータにおけるプロセスノイズ共分散行列、カルマン・フィルタ・パラメータにおける観測ノイズ共分散行列、およびカルマン・フィルタ・パラメータにおける初期推定誤差共分散行列（Ｑ、Ｒ、Ｐ）のそれぞれとに基づいて計算される。これは、例えば、図３に示されている。繰り返すが、成分相対角度は、連続における先の剛体からの情報または剛体の絶対角度を必要とせずに、剛体ｎおよび前の剛体ｎ−１からの測定値および計算されたパラメータに基づいて決定される。
【００４２】
[0051]図６は、いくつかの実施形態による、制御システムの簡略化された図である。システムは、例えば、図１に示される掘削機（例えば、コントローラ５０）と共に使用され得る状態推定器を含む。この図には、プラットフォーム、ブーム、スティック、およびバケットと関連付けられたＩＭＵからの出力が状態推定器にどのように渡されるが示されている。ＩＭＵは、例えば、測定された加速度および角速度を状態推定器に提供する。状態推定器は、それらの測定値を様々な実施形態に記載されるような他の入力と共に使用して、相対角度および／または成分相対角度を決定する。いくつかの実施形態では、状態推定器は、本明細書に記載される器具（例えば、バケットおよび／またはトゥース）の位置も決定し得る。これらの値および／または位置は、オペレータを支援するためにディスプレイ上に出力され得る。
【００４３】
[0052]状態推定器および／またはコントローラは、通常、良く知られたソフトウェアコンポーネントおよびハードウェアコンポーネントを含む。例えば、状態推定器および／またはコントローラは、１つまたは複数のオペレーティングシステム、プロセッサ、記憶のためのローカルメモリ、入出力装置、およびハードウェアコンポーネントを相互接続するシステムバスを含み得る。ＲＡＭおよびディスクドライブが、データおよびコンピュータープログラムの記憶のためのローカルメモリの例である。他のタイプのローカルメモリには、磁気記憶媒体、光記憶媒体、フラッシュメモリやソリッド・ステート・メモリ、ネットワーク接続された記憶装置などが含まれる。
【００４４】
[0053]図７は、いくつかの実施形態による、剛体間の相対角度を推定する方法を示すフローチャートである。剛体は、図１に示される掘削機の剛体と同様に連続として結合され得る。この方法は、前の剛体に取り付けられたＩＭＵの観測値から前の剛体の重力加速度ベクトルおよび角速度を取得すること７０１を含む。前の剛体は、剛体の連続における直前の剛体である。この方法は、現在の剛体に取り付けられたＩＭＵの観測値から現在の剛体の重力加速度ベクトルおよび角速度を取得すること７０２も含む。
【００４５】
[0054]この方法は、前の剛体と現在の剛体との間の第１の成分相対角度および第２の成分相対角度を、前の剛体の重力加速度ベクトルおよび角速度と、現在の剛体の重力加速度ベクトルおよび角速度とに少なくとも一部は基づいて推定すること７０３も含む。前述のように、成分相対角度を、前の剛体の重力加速度ベクトルと、現在の剛体の重力加速度ベクトルと、前の剛体の角速度と、現在の剛体の角速度とに基づいて決定することができる。前述のように、成分相対角度を推定することは、前の剛体から現在の剛体への回転行列、前の剛体と現在の剛体との間の中間の観測された第１の成分相対角度および第２の成分相対角度、カルマン・フィルタ・パラメータを用いたプロセスノイズ共分散、カルマン・フィルタ・パラメータを用いた観測ノイズ共分散、およびカルマン・フィルタ・パラメータを用いた初期推定誤差共分散も含むことができる。
【００４６】
[0055]図８は、いくつかの実施形態による、剛体間の相対角度を推定する方法を示すフローチャートである。剛体は、図１に示される掘削機の剛体と同様に連続として結合され得る。この方法は、剛体に取り付けられたＩＭＵの観測値から剛体の重力加速度ベクトルおよび角速度を受け取ること８０１を含む。この方法は、剛体に取り付けられたＩＭＵの観測値から次の剛体の重力加速度ベクトルおよび角速度を受け取ること８０２も含む。次の剛体は、剛体の連続におけるすぐ次の剛体である。
【００４７】
[0056]この方法は、剛体と次の剛体との間の第１の成分相対角度および第２の成分相対角度を、剛体の重力加速度ベクトルおよび角速度と、次の剛体の重力加速度ベクトルおよび角速度とに少なくとも一部は基づいて推定すること８０３も含む。前述のように、成分相対角度を、剛体の重力加速度ベクトルと、次の剛体の重力加速度ベクトルと、剛体の角速度と、次の剛体の角速度とに基づいて決定することができる。前述のように、成分相対角度を推定することは、剛体から次の剛体への回転行列、剛体と次の剛体との間の中間の観測された第１の成分相対角度および第２の成分相対角度、カルマン・フィルタ・パラメータを用いたプロセスノイズ共分散、カルマン・フィルタ・パラメータを用いた観測ノイズ共分散、およびカルマン・フィルタ・パラメータを用いた初期推定誤差共分散も含むことができる。
【００４８】
[0057]図９は、いくつかの実施形態による、剛体間（すなわち、前の剛体と剛体との間）の観測された成分相対角度
【数４５】
2021512325.tif 000047
および
【数４６】
2021512325.tif 000048
に基づいて剛体間の相対角度を推定する方法を示すフローチャートである。剛体は、図１に示される掘削機の剛体と同様に連続として結合され得る。この方法は、前の剛体の重力加速度ベクトルおよび剛体の重力加速度ベクトルを受け取ること９０１を含む。この方法は、剛体の第１の観測された成分相対角度および剛体の第２の観測された成分相対角度を決定すること９０２も含む。この方法は、前の剛体と剛体との間の第１の成分相対角度および第２の成分相対角度を、前の剛体の重力加速度ベクトルおよび角速度と、剛体の重力加速度ベクトルおよび角速度と、剛体の第１の観測された成分相対角度および剛体の第２の観測された成分相対角度とに少なくとも一部は基づいて推定すること９０３も含む。前述のように、観測された成分相対角度を、前の剛体の重力加速度ベクトルと剛体の重力加速度ベクトルとに基づいて決定することができる。観測された成分相対角度に基づいて成分相対角度を推定することは、前の剛体から剛体への回転行列、カルマン・フィルタ・パラメータを用いたプロセスノイズ共分散、カルマン・フィルタ・パラメータを用いた観測ノイズ共分散、およびカルマン・フィルタ・パラメータの初期推定誤差共分散行列を含み得る。
【００４９】
[0058]図１０は、いくつかの実施形態による、連続として配置された旋回可能に結合された剛体間の角度を決定する方法を示すフローチャートである。連続は、図１に示される掘削機の剛体のいくつかと同様に、前の剛体と次の剛体との間に配置された剛体を含む。この方法は、前の剛体に結合された剛体を含む連続として結合された剛体の配置に基づき、前の剛体と剛体との間の相対角度を、前の剛体の絶対角度または剛体の絶対角度を入力として使用せずに推定すること１００１を含む。
【００５０】
[0059]図７〜図１０に示される具体的なステップは、いくつかの実施形態による特定の方法を提供するものであることを理解されたい。代替の実施形態によれば、ステップの他の順序も実行され得る。例えば、代替の実施形態は、上記で概説されたステップを異なる順序で実行し得る。さらに、図７〜図１０に示される個々のステップは、様々な順序で実行され得る複数のサブステップを含み得る。さらに、個々の用途に応じて、追加ステップが追加されるか、または除去されてもよい。
【００５１】
[0060]いくつかの実施形態は、ハードウェア、ソフトウェア、ファームウェア、ミドルウェア、マイクロコード、ハードウェア記述言語、またはこれらの任意の組み合わせによって実施され得ることを理解されたい。ソフトウェア、ファームウェア、ミドルウェア、またはマイクロコードとして実施される場合、必要なタスクを実行するプログラムコードまたはコードセグメントは、記憶媒体などのコンピュータ可読媒体に格納され得る。プロセッサは、必要なタスクを実行するように適合され得る。「コンピュータ可読媒体」という用語には、可搬式または固定式の記憶装置、光記憶装置、無線チャネル、ＳＩＭカード、その他のスマートカード、および命令またはデータを格納、収容、または搬送することができる様々な他の非一時的な媒体が含まれるが、これらに限定されない。
【００５２】
[0061]本発明を特定の実施形態に関して説明してきたが、本発明の範囲が本明細書に記載される実施形態に限定されないことは当業者には明らかなはずである。例えば、本発明の１つまたは複数の実施形態の特徴は、本発明の範囲から逸脱することなく、他の実施形態の１つまたは複数の特徴と組み合わされ得る。したがって、本明細書および図面は、限定的意味ではなく例示として評価されるべきである。よって、本発明の範囲は、上記の説明を参照してではなく、添付の特許請求の範囲およびその均等物の全範囲を参照して決定されるべきである。
【図１】
2021512325.tif 000049
【図２】
2021512325.tif 000050
【図３】
2021512325.tif 000051
【図４】
2021512325.tif 000052
【図５】
2021512325.tif 000053
【図６】
2021512325.tif 000054
【図７】
2021512325.tif 000055
【図８】
2021512325.tif 000056
【図９】
2021512325.tif 000057
【図１０】
2021512325.tif 000058
【国際調査報告】
2021512325.tif 000059
2021512325.tif 000060