▶ トプコン ポジショニング システムズ， インク．の特許一覧
特表2024-500130動力学的に機敏な整地機における誤り検出及び誤り回復のための入力整形
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公表特許公報(A)
(11)【公表番号】
(43)【公表日】2024-01-04
(54)【発明の名称】動力学的に機敏な整地機における誤り検出及び誤り回復のための入力整形
(51)【国際特許分類】
E02F 3/85 20060101AFI20231222BHJP
E02F 9/20 20060101ALI20231222BHJP
E02F 9/26 20060101ALI20231222BHJP
【ＦＩ】
E02F3/85 D
E02F9/20 M
E02F9/26 B
【審査請求】未請求
【予備審査請求】未請求
(21)【出願番号】P 2023537228
(86)(22)【出願日】2021-10-08
(85)【翻訳文提出日】2023-06-23
(86)【国際出願番号】 US2021071792
(87)【国際公開番号】W WO2022133363
(87)【国際公開日】2022-06-23
(31)【優先権主張番号】17/125,967
(32)【優先日】2020-12-17
(33)【優先権主張国・地域又は機関】US
(81)【指定国・地域】
(71)【出願人】
【識別番号】505315742
【氏名又は名称】トプコン ポジショニング システムズ， インク．
(74)【代理人】
【識別番号】100098796
【弁理士】
【氏名又は名称】新井 全
(72)【発明者】
【氏名】カランタル， シャハブ
(72)【発明者】
【氏名】ポプラブスキ， ディミトロ
【テーマコード（参考）】
2D003
2D015
【Ｆターム（参考）】
2D003AA02
2D003AB04
2D003BA02
2D003BA03
2D003BB10
2D003CA02
2D003DA02
2D003DA04
2D003DB04
2D003DB05
2D003FA02
2D015HA03
2D015HB04
2D015HB05
(57)【要約】
車両（１００）のボディ（１０２）上に取り付けられた用具（１０４）の高さを車両（１００）が地形（１０８）上を走行するにつれ調節するためのシステム及び方法が提供される。センサデータは車両上に配置された一組のセンサ（１２２，１２４）から受信される。車両に関連する軌道が、受信されたセンサデータに基づき判断される。地形のプロファイルが、車両に関連する判断された軌道に基づき推定される。地形内の溝が検出され、そして用具の高さを調節するための補償値が、検出された溝を補正するために地形の推定プロファイルに基づき判断される。判断された補償値に基づき用具の高さを調節するための１つ又は複数の制御信号が１つ又は複数のアクチュエータ（１１８）へ送信される。
【特許請求の範囲】
【請求項1】
車両のボディ上に取り付けられた用具の高さを前記車両が地形上を走行するにつれ調節する方法であって、前記車両上に配置された一組のセンサからセンサデータを受信すること；
前記車両に関連する軌道を前記受信されたセンサデータに基づき判断すること；
前記車両に関連する前記判断された軌道に基づき前記地形のプロファイルを推定すること；
前記地形内の溝を検出し、そして前記検出された溝を補正するために前記地形の前記推定プロファイルに基づき前記用具の前記高さを調節するための補償値を判断すること；及び
前記判断された補償値に基づき前記用具の前記高さを調節するための１つ又は複数の制御信号を１つ又は複数のアクチュエータへ送信することを含む方法。
【請求項2】
前記車両に関連する軌道を前記受信されたセンサデータに基づき判断することは：前記ボディの状態及び前記用具の状態を前記受信されたセンサデータに基づき判断すること；
前記車両の状態を判断するために前記ボディの前記状態及び前記用具の前記状態を一次元空間へマッピングすること；及び
前記車両に関連する前記軌道を前記車両の前記状態に基づき判断することを含む、請求項１に記載の方法。
【請求項3】
前記ボディの状態及び前記用具の状態を前記受信されたセンサデータに基づき判断することは、前記ボディ及び前記用具の前記位置及び前記配向と、前記ボディ及び前記用具の前記位置及び前記配向の各軸に関連する線速度及び角速度とを判断することを含む、請求項２に記載の方法。
【請求項4】
前記車両に関連する前記判断された軌道に基づき前記地形のプロファイルを推定することは前記ボディに関連するピッチを前記車両に関連する前記判断された軌道に基づき判断することを含む、請求項１に記載の方法。
【請求項5】
前記地形内の溝を検出し、そして前記検出された溝を補正するために前記地形の前記推定プロファイルに基づき前記用具の前記高さを調節するための補償値を判断することは、前記地形の前記推定プロファイルの一次導関数を計算すること；
現在点の前記地形の前記推定プロファイルの前記一次導関数が零交差であるということを判断すること；及び
前記現在点の前記地形の前記推定プロファイルの大きさと、零交差であると判断された最終点の前記地形の前記推定プロファイルの大きさとを比較することを含む、請求項１に記載の方法。
【請求項6】
前記地形内の溝を検出し、そして前記検出された溝を補正するために前記地形の前記推定プロファイルに基づき前記用具の前記高さを調節するための補償値を判断することはさらに、前記現在点と前記地形の前記推定プロファイルにおけるこぶであると判断された最終点との間の一連の点を判断すること、及び
前記一連の点内の各点の前記補償値を、形状特徴と、前記現在点の前記推定プロファイルとこぶであると判断された前記最終点における前記地形の前記推定プロファイルとの差分とに基づき判断することを含む、請求項５に記載の方法。
【請求項7】
前記形状特徴は段差形状特徴、対数形状特徴、二次形状特徴、傾斜形状特徴、指数形状特徴又はそれらの組合せを含む、請求項６に記載の方法。
【請求項8】
前記判断された補償値に基づき前記用具の前記高さを調節するための１つ又は複数の制御信号を１つ又は複数のアクチュエータへ送信することは、標的地形表面を実現するための初期誤差値と最終誤差値を生成するための前記判断された補償値とを組み合わせること；及び
前記最終誤差値に従って前記用具の前記高さを調節するための前記１つ又は複数の制御信号を生成することを含む、請求項１に記載の方法。
【請求項9】
前記車両はブルドーザを含み、前記車両の前記用具はブレードを含む、請求項１に記載の方法。
【請求項10】
車両のボディ上に取り付けられた用具の高さを前記車両が地形上を走行するにつれ調節するためのコンピュータプログラム指令を格納する非一時的コンピュータ読み取り可能な記録媒体であって、前記コンピュータプログラム指令はプロセッサにより実行されると前記プロセッサに：
前記車両上に配置された一組のセンサからセンサデータを受信すること；
前記車両に関連する軌道を前記受信されたセンサデータに基づき判断すること；
前記車両に関連する前記判断された軌道に基づき前記地形のプロファイルを推定すること；
前記地形内の溝を検出し、そして前記検出された溝を補正するために前記地形の前記推定プロファイルに基づき前記用具の前記高さを調節するための補償値を判断すること；及び
前記判断された補償値に基づき前記用具の前記高さを調節するための１つ又は複数の制御信号を１つ又は複数のアクチュエータへ送信することを含む動作を行なわせる、非一時的コンピュータ読み取り可能な記録媒体。
【請求項11】
前記車両に関連する軌道を前記受信されたセンサデータに基づき判断することは：前記ボディの状態及び前記用具の状態を前記受信されたセンサデータに基づき判断すること；
前記車両の状態を判断するために前記ボディの前記状態及び前記用具の前記状態を一次元空間へマッピングすること；及び
前記車両に関連する前記軌道を前記車両の前記状態に基づき判断することを含む、請求項１０に記載の非一時的コンピュータ読み取り可能な記録媒体。
【請求項12】
前記ボディの状態及び前記用具の状態を前記受信されたセンサデータに基づき判断することは、前記ボディ及び前記用具の前記位置及び前記配向と、前記ボディ及び前記用具の前記位置及び前記配向の各軸に関連する線速度及び角速度とを判断することを含む、請求項１１に記載の非一時的コンピュータ読み取り可能な記録媒体。
【請求項13】
前記車両に関連する前記判断された軌道に基づき前記地形のプロファイルを推定することは前記ボディに関連するピッチを前記車両に関連する前記判断された軌道に基づき判断することを含む、請求項１０に記載の非一時的コンピュータ読み取り可能な記録媒体。
【請求項14】
プロセッサ；及び車両のボディ上に取り付けられた用具の高さを前記車両が地形上を走行するにつれ調節するためのコンピュータプログラム指令を格納するためのメモリを含むコントローラであって、
前記コンピュータプログラム指令は前記プロセッサ上で実行されると前記プロセッサに：
前記車両上に配置された一組のセンサからセンサデータを受信すること；
前記車両に関連する軌道を前記受信されたセンサデータに基づき判断すること；
前記車両に関連する前記判断された軌道に基づき前記地形のプロファイルを推定すること；
前記地形内の溝を検出し、そして前記検出された溝を補正するために前記地形の前記推定プロファイルに基づき前記用具の前記高さを調節するための補償値を判断すること；及び
前記判断された補償値に基づき前記用具の前記高さを調節するための１つ又は複数の制御信号を１つ又は複数のアクチュエータへ送信することを含む動作を行なわせる、コントローラ。
【請求項15】
前記地形内の溝を検出し、そして前記検出された溝を補正するために前記地形の前記推定プロファイルに基づき前記用具の前記高さを調節するための補償値を判断することは、前記地形の前記推定プロファイルの一次導関数を計算すること；
現在点の前記地形の前記推定プロファイルの前記一次導関数が零交差であるということを判断すること；及び
前記現在点の前記地形の前記推定プロファイルの大きさと、零交差であると判断された最終点の前記地形の前記推定プロファイルの大きさとを比較することを含む、請求項１４に記載のコントローラ。
【請求項16】
前記地形内の溝を検出し、そして前記検出された溝を補正するために前記地形の前記推定プロファイルに基づき前記用具の前記高さを調節するための補償値を判断することはさらに、前記現在点と前記地形の前記推定プロファイルにおけるこぶであると判断された最終点との間の一連の点を判断すること、及び
前記一連の点内の各点の前記補償値を、形状特徴と、前記現在点の前記推定プロファイルとこぶであると判断された前記最終点における前記地形の前記推定プロファイルとの差分とに基づき判断することを含む、請求項１５に記載のコントローラ。
【請求項17】
前記形状特徴は、段差形状特徴、対数形状特徴、二次形状特徴、傾斜形状特徴、指数形状特徴又はそれらの組合せを含む、請求項１６に記載のコントローラ。
【請求項18】
前記判断された補償値に基づき前記用具の前記高さを調節するための１つ又は複数の制御信号を１つ又は複数のアクチュエータへ送信することは、標的地形表面を実現するための初期誤差値と最終誤差値を生成するために前記判断された補償値とを組み合わせること；及び
前記最終誤差値に従って前記用具の前記高さを調節するための前記１つ又は複数の制御信号を生成することを含む、請求項１４に記載のコントローラ。
【請求項19】
前記車両はブルドーザを含み、前記車両の前記用具はブレードを含む、請求項１４に記載のコントローラ。
【請求項20】
ボディ；前記ボディへ結合された用具；
前記ボディ及び前記用具へ結合された１つ又は複数のアクチュエータ；
前記車両が地形上を走行するにつれセンサデータを生成するための前記車両上に配置された一組のセンサ；及び
前記センサデータを受信し；
前記車両に関連する軌道を前記受信されたセンサデータに基づき判断し；
前記車両に関連する前記判断された軌道に基づき前記地形のプロファイルを推定し；
前記地形内の溝を検出し、そして前記検出された溝を補正するために前記地形の前記推定プロファイルに基づき前記用具の高さを調節するための補償値を判断し；そして
前記判断された補償値に基づき前記用具の前記高さを調節するための１つ又は複数の制御信号を１つ又は複数のアクチュエータへ送信するためのコントローラを含む車両。
【請求項21】
前記受信されたセンサデータに基づき前記車両に関連する軌道を判断することは、前記ボディの状態及び前記用具の状態を前記受信されたセンサデータに基づき判断すること；
前記車両の状態を判断するために前記ボディの前記状態及び前記用具の前記状態を一次元空間へマッピングすること；及び
前記車両に関連する前記軌道を前記車両の前記状態に基づき判断することを含む、請求項２０に記載の車両。
【請求項22】
前記ボディの状態及び前記用具の状態を前記受信されたセンサデータに基づき判断することは、前記ボディ及び前記用具の位置及び配向と、前記ボディ及び前記用具の前記位置及び前記配向の各軸に関連する線速度及び角速度とを判断することを含む、請求項２１に記載の車両。
【請求項23】
前記車両に関連する前記判断された軌道に基づき前記地形のプロファイルを推定することは前記ボディに関連するピッチを前記車両に関連する前記判断された軌道に基づき判断することを含む、請求項２０に記載の車両。
【請求項24】
前記地形内の溝を検出し、そして前記検出された溝を補正するために前記地形の前記推定プロファイルに基づき前記用具の高さを調節するための補償値を判断することは、前記地形の前記推定プロファイルの一次導関数を計算すること；
現在点の地形の前記推定プロファイルの前記一次導関数が零交差であるということを判断すること；及び
前記現在点の前記地形の前記推定プロファイルの大きさと、零交差であると判断された最終点の前記地形の前記推定プロファイルの大きさとを比較することを含む、請求項２０に記載の車両。
【請求項25】
前記地形内の溝を検出し、そして前記検出された溝を補正するために前記地形の前記推定プロファイルに基づき前記用具の高さを調節するための補償値を判断することはさらに、前記現在点と前記地形の前記推定プロファイルにおけるこぶであると判断された最終点との間の一連の点を判断すること、及び
前記一連の点内の各点の前記補償値を、形状特徴と、前記現在点の前記推定プロファイルとこぶであると判断された前記最終点における前記地形の前記推定プロファイルの差分とに基づき判断することを含む、請求項２４に記載の車両。
【請求項26】
前記形状特徴は、段差形状特徴、対数形状特徴、二次形状特徴、傾斜形状特徴、指数形状特徴又はそれらの組合せを含む、請求項２５に記載の車両。
【請求項27】
前記判断された補償値に基づき前記用具の前記高さを調節するための１つ又は複数の制御信号を１つ又は複数のアクチュエータへ送信することは、標的地形表面を実現するための初期誤差値と最終誤差値を生成するために前記判断された補償値とを組み合わせること；及び
前記最終誤差値に従って前記用具の前記高さを調節するための前記１つ又は複数の制御信号を生成することを含む、請求項２０に記載の車両。
【請求項28】
前記車両はブルドーザを含み、前記用具はブレードを含む、請求項２０に記載の車両。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、一般的には車両の用具の高さを調節するための制御システムに関し、具体的には動力学的に機敏な整地機（ｇｒａｄｉｎｇ ｍａｃｈｉｎｅ）における誤り検出及び誤り回復のための入力整形に関する。
【背景技術】
【0002】
整地は、標的地形表面を実現するために地形の表面を操作するプロセスである。整地は多くの異なるアプリケーションにおいて重要なプロセスである。例えば、建設では、整地はビルの基礎を設定するために行われ得る。農業では、整地は表面水流出を導くために行われ得る。ブルドーザは通常、地形を整地するために使用される。ブルドーザはボディ、及びボディへ結合されたブレードを含む。動作中、ブルドーザが地形上を走行するにつれ、ブレードの高さは標的地形表面を実現するために地形の表面を操作するように調節される。
【0003】
従来の制御システムは、ブルドーザが擾乱を受ける際にブルドーザに対する擾乱を補正するためにブレードの高さを自動的に調節するためにブルドーザ内に実装され得る。しかし、従来の制御システムは固有応答遅延時間（例えば約１００～３００ミリ秒）を有し、したがっていくつかの擾乱を補正するためにブレードの高さが適時に調節されるのを妨げる。例えば、地形は通常、溝などの多くの擾乱を含む。ブルドーザが地形上を走行するにつれ、溝がブルドーザを前方向にピッチさせ、これによりブレードを下げさせる。従来の制御システムはブレードの高さを適時に調節することができないので、下げられたブレードは別の溝の生成に到る。次に、ブルドーザは生成された溝上を走行し、これにより別のより大きな溝を生成することになる。この問題は、ブルドーザが地形上を走行し続けると永続することになり、整地された地形において不安定性及び振動を生じる。
【発明の概要】
【課題を解決するための手段】
【0004】
１つ又は複数の実施形態によると、車両が地形上を走行するにつれ、車両のボディ上に取り付けられた用具の高さを調節するためのシステム及び方法が提供される。センサデータは車両上に配置された一組のセンサから受信される。車両に関連する軌道が、受信されたセンサデータに基づき判断される。地形のプロファイルが、車両に関連する判断された軌道に基づき推定される。地形内の溝が検出され、そして用具の高さを調節するための補償値が、検出された溝を補正するために地形の推定プロファイルに基づき判断される。判断された補償値に基づき用具の高さを調節するための１つ又は複数の制御信号が１つ又は複数のアクチュエータへ送信される。車両はブルドーザであり得、車両の用具はブレードであり得る。
【0005】
一実施形態では、車両に関連する軌道は、ボディの状態及び用具の状態を受信されたセンサデータに基づき判断し、車両の状態を判断するためにボディの状態及び用具の状態を一次元空間へマッピングし、そして車両に関連する軌道を車両の状態に基づき判断することにより判断される。ボディの状態及び用具の状態は、ボディ及び用具の位置及び配向として、そしてボディ及び用具の位置及び配向の各軸に関連する線速度及び角速度として判断され得る。
【0006】
一実施形態では、地形のプロファイルは、ボディに関連するピッチを車両に関連する判断された軌道に基づき判断することにより推定される。
【0007】
一実施形態では、溝は、地形の推定プロファイルの一次導関数を計算し、現在点の地形の推定プロファイルの一次導関数が零交差であるということを判断し、そして現在点の地形の推定プロファイルの大きさと、零交差であると判断された最終点の地形の推定プロファイルの大きさとを比較することにより検出される。補償値は、現在点と地形の推定プロファイルにおけるこぶであると判断された最終点との間の一連の点を判断し、そして一連の点内の各点の補償値を形状特徴と、現在点の推定プロファイルとこぶであると判断された最終点における地形の推定プロファイルとの差分と、に基づき判断することにより判断され得る。形状特徴は、段差形状特徴、対数形状特徴、二次形状特徴、傾斜形状特徴、指数形状特徴又はそれらの組合せを含み得る。
【0008】
一実施形態では、１つ又は複数の制御信号は、標的地形表面を実現するための初期誤差値と最終誤差値を生成するために判断された補償値とを組み合わせ、そして最終誤差値に従って用具の高さを調節するための１つ又は複数の制御信号を生成することにより送信される。
【0009】
本発明のこれら及び他の利点は、以下の詳細な説明及び添付図面を参照することにより当業者に明らかになる。
【図面の簡単な説明】
【0010】
【図1】１つ又は複数の実施形態による例示的ブルドーザを示す。
【0011】
【図2】１つ又は複数の実施形態によるブルドーザが動作する例示的２Ｄ環境を示す。
【0012】
【図3】１つ又は複数の実施形態による２Ｄ環境のブルドーザのボディ上に取り付けられたブレードの高さを調節するための予測制御システムの概略図を示す。
【0013】
【図4】１つ又は複数の実施形態による２Ｄ環境のブルドーザのボディ上に取り付けられたブレードの高さを調節するためのコントローラシステムのブロック図を示す。
【0014】
【図5】１つ又は複数の実施形態による３Ｄ環境の車両のボディ上に取り付けられた用具の高さを調節するための予測制御システムを示す。
【0015】
【図6】１つ又は複数の実施形態による３Ｄ環境の車両のボディ上に取り付けられた用具の高さを調節するための方法を示す。
【0016】
【図7】１つ又は複数の実施形態による例示的ブルドーザを示す。
【0017】
【図8】１つ又は複数の実施形態による車両の剛体モデルを示す概略図を示す。
【0018】
【図9】１つ又は複数の実施形態による地形の推定プロファイルにおける溝の検出を示すグラフを示す。
【0019】
【図10】１つ又は複数の実施形態による様々な形状特徴の線図を示す。
【0020】
【図11】１つ又は複数の実施形態による溝の補償を描写するグラフを示す。
【0021】
【図12】１つ又は複数の実施形態による車両の例示的概略図を示す。
【0022】
【図13】１つ又は複数の実施形態による車両のボディ・ツー・ボディ相関及びボディ・ツー・用具相関を示すグラフを示す。
【0023】
【図14】１つ又は複数の実施形態による例示的スミス予測器型コントローラを示す。
【0024】
【図15】１つ又は複数の実施形態を実施するために使用され得るコンピュータのハイレベルブロック図を示す。
【発明を実施するための形態】
【0025】
図１を参照すると、１つ又は複数の実施形態による例示的ブルドーザ１００が示される。ブルドーザ１００は、ボディ１０２と、ブームアーム（示されない）を介しボディ１０２へ旋回自在に結合されたブレード１０４（又は他の好ましい用具）とを含む。ブルドーザ１００はまた、地形１０８上を走行するためのトラック（ｔｒａｃｋ）１０６を含んでおり、操作者により自律的に又は手動で操作され得る。一般的に、動作中、ブルドーザ１００は、ブレード１０４を介し地形１０８の表面上の土壌、砂、瓦礫などを操作し標的地形表面を実現するために建設環境、農業環境又は任意の他の環境内で利用され得る。地形１０８上を走行するにつれ、ブルドーザ１００は地形１０８において様々な擾乱に遭遇し得る。このような擾乱の一例は溝である。
【0026】
ブルドーザ１００は３Ｄ（３次元）環境内で動作し、そしてブレード１０４はボディに対する回転運動の３つの自由度を有する。したがって、ブルドーザ１００の状態はボディ１０２の位置（ナビゲーション系１１６内の）、ボディ１０２の配向（ナビゲーション系１１６内の）、そしてボディ１０２に対するブレード１０４の相対配位（ボディ系１１２内の）により定義され得る。一実施形態では、位置はデカルト座標における（Ｘ，Ｙ，Ｚ）で定義され、配向は（ヨー、ピッチ、回転）オイラー角で定義される。しかし、位置及び配向は任意の好適な形式で表され得るということを理解すべきである。例えば、位置は、地形１０８の表面上に在る２Ｄ（２次元）曲線（地形１０８の知識を必要とするだろう）として表され得、そして配向は回転行列、四元数などとして表され得る。
【0027】
ブルドーザはブレードの高さを調節するための従来の制御システムが実装され得る。しかし、従来の制御システムは、このような従来の制御システムが溝及び他のこのような擾乱を補正するためにブレードの高さを適時に調節するのを妨げる固有応答遅延時間を有する。
【0028】
本明細書において説明される実施形態は、地形１０８の擾乱に起因するボディ１０２に対する（例えば溝に起因する）擾乱をブレード１０４の運動に基づき予測するために予測制御システムを提供する。予測制御システムはコントローラ１２０、ブレード１０４上に配置された一組のセンサ１２２及び／又はボディ１０２上に配置された一組のセンサ１２４、並びにボディ１０２及びブレード１０４へ結合された１つ又は複数のアクチュエータ１１８を含む。コントローラ１２０は、ボディ１０２に対する外乱を予測するために一組のセンサ１２２及び／又はセンサ１２４からデータを受信し、そしてアクチュエータ１１８を制御しブレード１０４の高さを調節して予測外乱を補正するための制御信号を生成する。有利には、コントローラ１２０は、ボディ１０２上の外乱を予測し、これにより、溝及び他のこのような外乱の補正のためのブレード１０４を調節するための十分な時間を提供する。提示の簡単のために、地形１０８における擾乱に起因するボディ１０２に対する外乱の予測は、３Ｄ環境に関して説明される前に図２～４に関し２Ｄ環境に関して最初に説明されることになる。
【0029】
図２は、１つ又は複数の実施形態によるブルドーザ２０２が動作する例示的２Ｄ環境２００を示す。ブルドーザ２０２はボディ２０６及びブレード２０８を含む。一例では、ブルドーザ２０２は図１のブルドーザ１００であり得る。図２に示すように、ブルドーザ２０２は地形２０４の表面を操作するために２Ｄ環境２００内で動作する。ブルドーザ２０２のボディ２０６は、ボディ２０６の高さ及び水平に対するボディ２０６のピッチ角によりパラメータ化される。ブルドーザ２０２のブレード２０８はブレードピッチ角θ２１０によりパラメータ化される。ブルドーザ２０２の性能は地形２０４の実際の表面と地形２０４の標的表面との差を測定することにより評価される。ブルドーザ２０２は、１つ又は複数の実施形態によると１つ又は複数のセンサ、１つ又は複数のアクチュエータ及びブレード２０８の高さを自動的に調節するためのコントローラが実装される。
【0030】
図３は１つ又は複数の実施形態による２Ｄ環境のブルドーザのボディ上に取り付けられたブレードの高さを調節するための予測制御システムの概略図３００を示す。一例では、ブルドーザは図１のブルドーザ１００又は図２のブルドーザ２０２であり得る。
【0031】
ブロック３０４において、車両のボディ姿勢（位置）は、車両のボディ上に取り付けられた一組のセンサに基づき判断される。ボディ姿勢を判断するための一組のセンサは、車両のボディの（Ｘ，Ｙ）デカルト座標を判断するための位置センサを含み得る。ブロック３０２において、車両のボディ運動学は、車両のボディ上に取り付けられた一組のセンサ及びボディ姿勢（工程３０４において判断された）に基づき判断される。一例では、ボディ運動学を判断するための一組のセンサは角回転センサ及び加速度計を含み得る。ブロック３０８では、ブレード順運動学が、ボディ姿勢、ボディ運動学、及び車両のブレード上に取り付けられた一組のセンサに基づき判断される。ブロック３１０では、ブレード姿勢がブレード順運動学に基づき判断される。ブロック３０６では、ブレードの逆回転運動学がブレード姿勢及びボディ姿勢に基づき判断される。ブロック３１２では、経路及び軌道計画が表面エンジン３１４からの表面検索結果に基づき行われる。ブロック３１６では、ブレード逆運動学が経路及び軌道計画に基づき判断される。ブロック３１８では、関節・ツー・ラム（ｊｏｉｎｔ ｔｏ ｒａｍ）運動学がブレード逆運動学から判断される。ブロック３２０では、関節・ツー・ラム運動学は、ブレードの高さを調節するための指令を弁３２２へ出力するコントローラ（例えば比例積分微分コントローラ）へ入力される。
【0032】
図４は、１つ又は複数の実施形態による２Ｄ環境のブルドーザのボディ上に取り付けられたブレードの高さを調節するためのコントローラシステム４００のブロック図を示す。図４に示すように、コントローラシステム４００はコントローラ４０２及びプラント４０４を含む。コントローラ４０２は、ブルドーザのボディ及びブレード上に取り付けられたセンサ４０６からデータを受信し、そしてボディ及びブレードの状態を観測器４０８により推定する。観測器４０８は補償値（ブレードｚと呼ばれる）４２０を生成する。補償値４２０は所望表面４１０を生成するために初期誤差値と組み合わせられて誤差４１８を生成する。誤差４１８は低レベルコントローラ４１２へ入力され、低レベルコントローラ４１２は地面４１６を操作するためにブルドーザのブレードの高さを調節するための命令を油圧系遅延４１４へ送信する。
【0033】
図５は１つ又は複数の実施形態による３Ｄ環境の車両のボディ上に取り付けられた用具の高さを調節するための予測制御システムの概略図５００を示す。図６は１つ又は複数の実施形態による３Ｄ環境の車両のボディ上に取り付けられた用具の高さを調節するための方法６００を示す。図５及び図６は併せて説明されることになる。図６の方法６００の工程は、コントローラ（例えば図１のコントローラ１２０）又は任意の他の好ましいコンピューティングデバイス（例えば図１１のコンピュータ１１０２など）により行われ得る。例示的コントローラは、ＰＩＤ（比例積分微分）コントローラ又はＰＬＣ（プログラマブル論理コントローラ）などの単純コントローラ及びスミス予測器又はＭＰＣ（モデル予測制御）などのより洗練されたコントローラを含む。
【0034】
車両は標的地形表面を実現するために用具を介し地形を操作するために地形上を当初走行し得る。車両の用具の高さは標的地形表面を実現するために初期誤差値に従って調節される。しかし、用具の高さを初期誤差値に従って調節することはいくつかの擾乱（例えば地形内の溝など）を補償することができない可能性がある。方法５００によると、補償値が、地形内の溝及び他の擾乱を補償するために判断される。補償値は、地形内の溝及び他の擾乱も補正する一方で標的地形表面を実現するために用具の高さを調節するための最終誤差値を生成するために初期誤差値と組み合わせられ得る。
【0035】
工程６０２では、センサデータは車両上に配置された一組のセンサから受信される。車両はボディへ結合された用具を有する任意の車両（例えば建設車両（例えばボディへ結合されたブレードを有するブルドーザ又はボディへ結合されたブレードを有する小型トラックローダ）又は農業車両（例えばボディへ結合されたヘッダーを有するコンバイン）など）であり得る。一例では、車両はボディ１０２へ結合されたブレード１０４及びセンサ１２２、１２４を有する図１のブルドーザ１００である。図５に示すように、車両はブルドーザ５０２であり得、一組のセンサはセンサ５０４であり得る。
【0036】
一組のセンサは、車両のボディ及び用具の位置及び配向だけでなく軸毎の線形速度及び角速度も判断するための任意数の好ましいセンサを含み得る。一実施形態では、位置はデカルト座標（Ｘ，Ｙ，Ｚ）で定義され、配向はオイラー角（ヨー、ピッチ、回転）で定義される。しかし、位置及び配向は任意の好適な形式で表され得るということを理解すべきである。例示的センサは、ＩＭＵ（慣性測定ユニット：ｉｎｅｒｔｉａｌ ｍｅａｓｕｒｅｍｅｎｔ ｕｎｉｔ）、ＧＰＳ（全地球測位システム：ｇｌｏｂａｌ ｐｏｓｉｔｉｏｎｉｎｇ ｓｙｓｔｅｍ）センサ、ＬＰＳ（局所測位システム：ｌｏｃａｌ ｐｏｓｉｔｉｏｎｉｎｇ ｓｙｓｔｅｍ）センサ、音響距離計、レーザ距離計、符号器、ラム内（ｉｎ－ｒａｍ）圧力センサ、オドメータ、又は任意の他の好ましいセンサを含み得る。
【0037】
一組のセンサは、ボディ及び用具の位置及び配向だけでなく軸毎の線形速度及び角速度も判断するための任意の好適な場所において車両上に配置され得る。例えば、一組のセンサは、用具上に配置された１つ又は複数のセンサ及び／又はボディ上に配置された１つ又は複数のセンサを含み得る。一実施形態では、一組のセンサはマスト無し（ｍａｓｔｌｅｓｓ）構成で車両上に配置され、ここでは、２つのＧＰＳセンサがボディ上に取り付けられ、ＩＭＵはボディ上に取り付けられ、そしてＩＭＵは用具上に取り付けられる。ボディ上に取り付けられたＧＰＳセンサは、主補助対（ｍａｉｎ－ａｕｘｉｌｉａｒｙ ｐａｉｒ）を形成し、ＲＴＫ（リアルタイム運動学）アルゴリズムを実行する。任意選択的に、車両が押し棒を備えたブルドーザである場合、追加ＩＭＵが押し棒上に取り付けられ得る。別の実施形態では、一組のセンサはマスト有り（ｍａｓｔｅｄ）構成で車両上に配置され、ここでは、１つ又は２つのＧＰＳセンサが用具上に取り付けられ、ＩＭＵは用具上に取り付けられ、そして任意選択的にＩＭＵがボディ上に取り付けられる。
【0038】
工程６０４では、車両に関連する軌道が、受信されたセンサデータに基づき判断される。軌道は、車両が地形上を走行する際の車両に関連する点の場所を表す。軌道の判断は図５を引き続き参照して説明されることになる。
【0039】
軌道を判断するために、車両のボディの状態及び車両の用具の状態が、受信されたセンサデータに基づき最初に判断される。図５に示すように、観測（推定）ブロック５０６が、ボディ５０８の観測された状態及び用具５１０の観測された状態を推定するためにセンサ５０４からデータを受信する。ボディの状態及び用具の状態は、ボディ及び用具の位置（例えばＸ，Ｙ，Ｚデカルト座標における）及び配向（例えばヨー、ピッチ、ロールオイラー角における）だけでなく軸毎の線形速度及び角速度にも対応する２４個のパラメータという観点で定義される。
【0040】
図１にしばらく戻って参照すると、ブルドーザ１００は様々な基準系（例えばブルドーザ１００のボディ１０２のボディ系１１４、ブレード１０４のブレード系１１２、地形表面１０８の曲面パッチ系１１０、及びナビゲーションのナビゲーション系１１６など）を有する３次元環境内で動作する。ボディ１０２の状態は、ボディ系１１４におけるその絶対位置及び配向（或る基準点に対する）という観点で定義される。ブレード１０４の状態は、ブレード系１０４におけるその相対位置及び配向（ボディ系１１４に対する）という観点で定義される。ブレード１０４の絶対位置及び配向（基準点に対するボディ系１１４における）はその相対位置及び配向から判断され得る。
【0041】
ボディの状態及び用具の状態は以下の部品を使用することにより判断される：
（１）ａ）ボディ上に取り付けられたＩＭＵからの加速度計出力、ｂ）ボディ上に取り付けられた主ＧＰＳからの位置出力、及びｃ）ボディ配向（（２）における拡張カルマンフィルタからの出力）に基づきボディの位置並びに線形速度及び角速度を判断するための拡張カルマンフィルタ；
（２）ａ）ボディ上に取り付けられたＩＭＵからのジャイロスコープ出力、ｂ）ボディ上に取り付けられた補助ＧＰＳからの基準（補助ＧＰＳから主ＧＰＳへのベクトル）出力、及びｃ）重力ベクトル（（３）におけるボディ加速度コンピュータにより推定される）に基づきボディの配向を判断するための拡張カルマンフィルタ；
（３）ボディ上に取り付けられたＩＭＵの場所における重力ベクトルをａ）ボディ速度（（１）における拡張カルマンフィルタから出力された）及びｂ）ボディ上に取り付けられたＩＭＵからのジャイロスコープ出力に基づき推定するためのボディ加速度コンピュータ；
（４）ａ）用具上に取り付けられたＩＭＵからのジャイロスコープ出力及びｂ）重力ベクトル（（５）における用具加速度コンピュータにより推定された）に基づき用具の配向を判断するための拡張カルマンフィルタ；
（５）用具上に取り付けられたＩＭＵの場所における重力ベクトルを（（６）におけるブレード運動学モジュールにより計算された）ブレード速度に基づき推定するための用具加速度コンピュータ；及び
（６）ボディに対する用具の位置及び配向、用具の線形速度及び角速度、運動学的構造の関節の角度及び角速度を、ａ）ボディの位置及び線形速度及び角速度（（１）における拡張カルマンフィルタにより判断された）及びボディの配向（（２）における拡張カルマンフィルタにより判断された）、ｂ）用具の配向及び角速度（（４）における拡張カルマンフィルタにより判断された）及びｃ）車両の運動学的構造（例えば車両の骨格又は青写真）に基づき計算するためのブレード運動学モジュール。車両の運動学的構造は、車両の関節の数及び関節が互いに対して位置決めされるやり方を指す。車両の運動学的構造は以下の図１２に関してさらに説明される。
（１）ａ）ボディ上に取り付けられたＩＭＵからの加速度計出力、ｂ）ボディ上に取り付けられた主ＧＰＳからの位置出力、及びｃ）ボディ配向（（２）における拡張カルマンフィルタからの出力）に基づきボディの位置並びに線形速度及び角速度を判断するための拡張カルマンフィルタ；
（２）ａ）ボディ上に取り付けられたＩＭＵからのジャイロスコープ出力、ｂ）ボディ上に取り付けられた補助ＧＰＳからの基準（補助ＧＰＳから主ＧＰＳへのベクトル）出力、及びｃ）重力ベクトル（（３）におけるボディ加速度コンピュータにより推定される）に基づきボディの配向を判断するための拡張カルマンフィルタ；
（３）ボディ上に取り付けられたＩＭＵの場所における重力ベクトルをａ）ボディ速度（（１）における拡張カルマンフィルタから出力された）及びｂ）ボディ上に取り付けられたＩＭＵからのジャイロスコープ出力に基づき推定するためのボディ加速度コンピュータ；
（４）ａ）用具上に取り付けられたＩＭＵからのジャイロスコープ出力及びｂ）重力ベクトル（（５）における用具加速度コンピュータにより推定された）に基づき用具の配向を判断するための拡張カルマンフィルタ；
（５）用具上に取り付けられたＩＭＵの場所における重力ベクトルを（（６）におけるブレード運動学モジュールにより計算された）ブレード速度に基づき推定するための用具加速度コンピュータ；及び
（６）ボディに対する用具の位置及び配向、用具の線形速度及び角速度、運動学的構造の関節の角度及び角速度を、ａ）ボディの位置及び線形速度及び角速度（（１）における拡張カルマンフィルタにより判断された）及びボディの配向（（２）における拡張カルマンフィルタにより判断された）、ｂ）用具の配向及び角速度（（４）における拡張カルマンフィルタにより判断された）及びｃ）車両の運動学的構造（例えば車両の骨格又は青写真）に基づき計算するためのブレード運動学モジュール。車両の運動学的構造は、車両の関節の数及び関節が互いに対して位置決めされるやり方を指す。車両の運動学的構造は以下の図１２に関してさらに説明される。
【0042】
次に、ボディの状態及び用具の状態は、標的地形表面と共に、車両全体の一次元状態を判断するために一次元空間へマッピングされる。図５に示すように、ボディ５０８の状態及び用具５１０の状態だけでなく標的地形表面５１２も、車両５１６の状態を判断するために展直面ブロック５１４へマッピングすることにより展直面へマッピングされる。図７をしばらく参照すると、１つ又は複数の実施形態による地形上のブルドーザ経路７０８に沿って走行しているボディ７０２及びブレード７０４を含むブルドーザ７００が示される。ボディ７０２及びブレード７０４の状態（位置、配向、及び線形速度及び角速度における）はブルドーザ経路７０８に対して垂直である展直面７０６へマッピングされる。
【0043】
ボディ７０２及びブレード７０４の状態を一次元展直面７０６へマッピングするために、以下のブレード７０４上の様々な関心点が識別される：
（１）ブレード７０４の両端からほぼ中間に位置するとして定義されるブレード７０４の中点。ブレード中点は通常、ブレードが直角である場合に所望表面に対するブレード７０４の仰角を判断する点（零ヨー）である；
（２）ブレード７０４の両端点。例えば右端はブルドーザを計測し、そしてセンサの場所を調査するための基準として使用され得る。このような端は点（０，０，０）として指定され得る；
（３）ブレード７０４の両端間内のユーザ選択点（ブレード７０４の両端を含む）として定義されるポイントフィット（ｐｏｉｎｔ－ｆｉｔ）関心点（ＰＯＩ：ｐｏｉｎｔ ｏｆ ｉｎｔｅｒｅｓｔ）。０～１の比を定義することを介し特定点を選択することにより（０は一端を指示し１は他端を指示する）、ユーザはブレード７０４の所望表面の特定領域に関する自身の嗜好を指示する。換言すれば、ＰＯＩ直下の曲面パッチはメインフォール（ｍａｉｎｆａｌｌ）誤差及び横断勾配（ｃｒｏｓｓ ｓｌｏｐｅ）誤差を計算する基礎として使用されることになる。一実施形態では、この比のデフォルト値は中点を表す０．５である；及び
（４）ベストフィット関心点（制御システムにより判断されるということを除きポイントフィットＰＯＩと同じである）。ベストフィット動作モードでは、選択される曲面パッチは、曲面パッチのうちのどの曲面パッチが端との最大重なりを有するか又はどの曲面パッチがブレード端に最も近いかを含む様々な要因に基づく。通常、選択されたベストフィットＰＯＩは両端点のいずれかである。
（１）ブレード７０４の両端からほぼ中間に位置するとして定義されるブレード７０４の中点。ブレード中点は通常、ブレードが直角である場合に所望表面に対するブレード７０４の仰角を判断する点（零ヨー）である；
（２）ブレード７０４の両端点。例えば右端はブルドーザを計測し、そしてセンサの場所を調査するための基準として使用され得る。このような端は点（０，０，０）として指定され得る；
（３）ブレード７０４の両端間内のユーザ選択点（ブレード７０４の両端を含む）として定義されるポイントフィット（ｐｏｉｎｔ－ｆｉｔ）関心点（ＰＯＩ：ｐｏｉｎｔ ｏｆ ｉｎｔｅｒｅｓｔ）。０～１の比を定義することを介し特定点を選択することにより（０は一端を指示し１は他端を指示する）、ユーザはブレード７０４の所望表面の特定領域に関する自身の嗜好を指示する。換言すれば、ＰＯＩ直下の曲面パッチはメインフォール（ｍａｉｎｆａｌｌ）誤差及び横断勾配（ｃｒｏｓｓ ｓｌｏｐｅ）誤差を計算する基礎として使用されることになる。一実施形態では、この比のデフォルト値は中点を表す０．５である；及び
（４）ベストフィット関心点（制御システムにより判断されるということを除きポイントフィットＰＯＩと同じである）。ベストフィット動作モードでは、選択される曲面パッチは、曲面パッチのうちのどの曲面パッチが端との最大重なりを有するか又はどの曲面パッチがブレード端に最も近いかを含む様々な要因に基づく。通常、選択されたベストフィットＰＯＩは両端点のいずれかである。
【0044】
１つ又は複数のこのような関心点が、表面交差と呼ばれる点において交差する１Ｄ面へ投影される。制御システムのゴールは、関心点（時間に応じた）が一定時間内に表面交差へ収束する（時間に応じて）ようにブレード７０４を動かすことである。より一般的には、ブレード７０４の端は、ＰＯＩから延長された垂直光線と交差する曲面パッチ（空間面である）に交差しそしてその上に在らねばならない。
【0045】
本明細書において説明される実施形態は、３Ｄへ拡張され得るが、問題が２Ｄ面（３Ｄ空間内に埋め込まれた）上で定義された問題として投げかければ、大いに単純化され、そしてより大きな成果を得る。ブレード７０４の中点は、ブレードが傾斜及び／又は回転するにつれ常に変動するので面を選択するための基準であってはならない。さらに、軌道はボディが移動すると生成される。結局、この面は、ブルドーザを２つの部分（例えば左部分及び右部分）に分割するサジタル展直面７０６として選択される。これは一般性を損なわないということが示され得る。換言すれば、この面上でシリンダを制御することで、問題が３Ｄ空間においても解決されるということを確実にする。
【0046】
正式に、ナビゲーション座標系は
で表される。ナビゲーション座標系の中心は重要ではない。ブレード７０４の中点は３Ｄ空間内のｍ（ｘ，ｙ，ｚ）で表され、ナビゲーション系に対して表現される。ブレード７０４の端の座標系（ｍを中心とする）は
で表され、ここで、ユニット方向
はブレード７０４の端に沿った方向（右側に向かって正）を指し、
はブレード７０４が零位置（校正時に判断される）に在る場合の垂直に沿った下方向を指し、
は
及び
に対し直交する前方向を指す。関心点はｐ（ｘ，ｙ，ｚ）で表される。表面交差点はｓ（ｘ，ｙ，ｚ）で表される。ｐを中心とする表面系は
で表される。サジタル展直面７０６は
で表され、ここで、
はブルドーザの前部を指す面上に在り、
は同面に対して直交し、そして
は局所重力ベクトルに沿う。
【数1】
【数2】
【数3】
【数4】
【数5】
【数6】
【数7】
【数8】
【数9】
【数10】
【数11】
【数12】
【0047】
ボディ７０２及びブレード７０４の状態は次のように展直面７０６へマッピングされ得る。第１に、関心点（ポイントフィット関心点又はベストフィット関心点）は比αを変更することにより選択される。運動学を使用することにより、関心点はｐ（ｘ，ｙ，ｚ）を得るためにナビゲーション系で表される。ｍ及びＭは運動学により計算され、次式を生じる：
第２に、ＰＯＩは、所望表面の曲面パッチと交差するために垂直方向
に投影される。第３に、ブレード７０４の端を表す線分Ｌが曲面パッチ上へ投影される。定義により、点ｓはこの投影図上に既に在るはずである。ユニットベクトル
は線分Ｌ上に在ると仮定される。第４に、ｍは
に沿ってＰへ投影され、そして投影点はｍＰで表される。第５に、ｓは
に沿ってＰへ投影され、投影点はｓＰで表される。最後に、第６に、ｍＰ及びｓＰがそれぞれ軌道に対するブレード点及び標的表面点として追加される。両方の点の系は、同じ面上に在るのでＰと同じになる。ブレード７０４のヨー及び回転はｍＰの位置を変更することになるが、この変更はｍの変更より非常に小さく、この系の安定性を助けるということに留意すべきである。
【数13】
【数14】
【数15】
【数16】
【数17】
【0048】
図６の工程６０４を参照すると、車両に関連する軌道が車両の状態に基づき判断される。図５に示すように、車両５１６の状態は軌道を判断するために追跡点判断ブロック５１８により使用される。用具が地形の表面上を走行するにつれ、軌道が、点を地形の表面へ加えることにより形成される。軌道の前部（出発点）は用具の位置を表す点であり、そして軌道の後尾（端）は前部から所定距離まで後方へ伸びる。各点は車両に関連する任意の位置に在り得る。例えば、各点はボディの位置及び／又は用具の位置に基づき判断される位置に在り得る。
【0049】
一実施形態では、軌道の各点は、地面の形状に関する情報を引き出すためのその適切性に基づき判断される。例えば、軌道のいくつかの点が用具の先端として取られれば、用具は地形の表面に必ずしも接触していないということが仮定されなければならなく、これは必ずしも真でない可能性がある。用具がピッチアップすれば、用具は、地形の表面との接触を失うことになり、そして偽のこぶ及び溝に至る。別の実施形態では、軌道の各点はボディ上の仮想点に基づき判断される。このような仮想点は、車両の物理的ボディから遠くに在るブレード先端の下に位置する。仮想点は、ボディが十分に長ければボディ上への用具の先端の下方向垂直投影と考えられ得る。別の実施形態では、軌道の各点は、ブレード先端の代わりに地形の表面を表す点であり、これはボディがピッチアップ／ダウンだけする場合は良い選択であり得る。別の実施形態では、軌道の各点は、用具の先端の垂直高さとボディ上の仮想点の垂直高さとを比較しそして最低垂直高さを有する点を選択することにより判断され得る。用具が地形の表面との接触を失ったとしても、軌道は依然として、どこに地形の表面が在るかの合理的な推定となるだろう。別の実施形態では、軌道の各点は、用具が標的表面に侵入する量に基づき地形の表面を表すように判断され得る。これは、ブルドーザが標的表面に十分に近ければ（換言すれば地形の実際の表面が所望表面にほぼ等しい場合）良い選択だろう。しかし、計算目的のために、小さなディザーが当該点へ加えられなければならない。そうでなければ、点の傾斜は変曲点において零ではなく一定値となる。
【0050】
一実施形態では、軌道は一時的モードで生成される。一時的モードでは、点はいかなる制限も無く軌道へ加えられる。したがって、時間が進むにつれて（車両が移動しているか否かにかかわらず）、点が推定されそして加えられる。システムの速度に依存して、点は例えば約１０ミリ秒毎に加えられ得る。したがって、点同士は常に１０ｍｓ時間離れている。しかし、車両が静止していれば、点は加えられた点の平均を中心に点集団を形成することになる。点集団の幅はシステム内の一組のフィルタの分散により判断される。しかし、軌道は最大容量を有する。最大容量を越えると、軌道の端において加えられた最も古い点は、新しい点のためのスペースを空けるために除去されることになる。車両が時間と共に移動し始めれば、点集団は痕跡（軌道履歴と呼ばれる）を形成する。どれくらい速く車両が移動しているかに依存して、新しい点同士は車両の速度に基づき離間されることになる。例えば、車両が毎秒１メートルで走行していれば、１０ｍｓ＝０．０１秒の割合で、約１００点が互いに１０ｍｍ離間されて軌道へ加えられる。したがって、軌道は車両の速度に応じた可変長のものになる。別の実施形態では、軌道は保持モードで生成される。保持モードでは、軌道は所定最大長を有する。点が最前部において軌道へ加えられ軌道が所定最大長に達すると、点は所定最大長を維持するために後尾において軌道から除外される。点は所定離間距離に従って離間されて軌道へ加えられる。一時的モード及び保持モードの両方に関し、時間遅延限界が、一時的モード及び保持モードの両方のために２つの連続点間に定義され得る。２つの連続点間の遅延が時間遅延限度を越えれば、軌道はリセットされる。
【0051】
工程６０６では、地形のプロファイルが、車両に関連する判断された軌道に基づき推定される。車両に関連する軌道は、地形のプロファイルをほぼ近似するが、車両のボディのピッチングの振る舞いの非常に粗い指標である。例えば、軌道は、車両が狭い溝に遭遇する場合、用具が地形に接触していない場合、又は用具の振動が存在する場合、地形のプロファイルの正確な表現ではないかもしれない。したがって、地形のプロファイルは、車両が地形上を走行するにつれてボディのピッチをシミュレートすることにより推定される（軌道により近似される）。ボディのピッチは、車両の軌道と比較して地形のより正確なプロファイルを提供する。図５に示すように、軌道は、ボディのピッチをシミュレートすることにより地形のプロファイルを予測するためにボディ状態予測（シミュレーション）ブロック５２０により使用される。
【0052】
一実施形態では、ボディの運動は、車両が地形上を走行するにつれてボディのピッチを判断するために剛体手法を使用することによりシミュレートされる。剛体手法では、車両は、（関節幾何学形状（ａｒｔｉｃｕｌａｔｉｏｎ ｇｅｏｍｅｔｒｙ）の変化と共に変化する）質量及び質量慣性モーメントにより特徴付けられた剛体としてモデル化される。車両の様々なサブシステム（例えばボディ、用具、トラック、車輪など）は車両の剛体モデルでモデル化され得る。いくつかの実施形態では、車両の剛体モデルでモデル化されたサブシステムはまた、車両のサスペンションシステム、トラックシュー又は任意の他のサブシステムを含み得る。モデル化を単純にするために、トラックと地形との相互作用が、上に湾曲しそして地形と相互作用しないトラックの前部及び後部における部分を考慮すること無くモデル化される。
【0053】
図８は１つ又は複数の実施形態による車両８０２の剛体モデルを示す概略図８００を示す。車両８０２は、ボディ８０４、用具８０６及びトラック８１２を含み、そして重心８１０を有する。トラック８１２は、ゴムで作製され、したがって、地形８０８との相互作用だけに起因して歪み得る弾性媒体としてモデル化され得る。簡単のために、車両８０４のロードホイールはモデル化されない。図８に示すように、トラック８１２は、トラック８１２が地形８０８に接触する場所におけるゴムバンドの上側境界及び下側境界間に仮想バネ／緩衝器対８１４を使用することによりゴムバンドとしてモデル化される。バネ／緩衝器対８１４はそれぞれバネｋ及び緩衝器ｂを含む。トラック８１２は任意数のバネ／緩衝器対８１４を使用することによりモデル化され得る。トラック８１２をモデル化するために使用されるバネ／緩衝器対８１４の数が多ければ多いほどモデルはより正確になる。バネ／緩衝器対８１４は地形８０８（トラック８１２が地形８０８と接触する場所における）からの力及びボディ８０４からの力下で歪む。次に、バネ／緩衝器対８１４は力をボディ８０４に掛け、重心８１０を中心とする線形力及び回転トルクを生じる。これらの結果力及びトルクは、車両８０２のバウンス及びピッチ動力学を計算するためにニュートン力学方程式を使用することにより計算され得る線形加速度及び角加速度を生成する。線形加速度と角加速度との合成が車両８０２の垂直速度及びピッチ角速度を生じる。車両８０２の速度は知られているので、地形８０８の引張力をモデル化する必要性（これは固体剪断応力及び変形の知識を必要としていただろう）がない。トラック８１２の一部分は地形８０８との接触を失い得、したがってトラック８１２のそれらの部分を表すバネ／緩衝器対８１４はいかなる垂直力も生成しなくなる。いくつかの実施形態では、用具８０６と地形８０８との相互作用もまた、より正確且つ現実的結果を提供するためにモデル化され得る。
【0054】
１つの挑戦は、バネ／緩衝器対８１４におけるバネｋ及び緩衝器ｂの剛性パラメータ及びダンピングパラメータをそれぞれ判断することである。バネｋは、ゴムトラック８１２の弾性振る舞い及び地形８０８の圧力沈み込み振る舞いをモデル化する非線形バネである。ベッカー（Ｂｅｋｋｅｒ）の式に従って、圧力沈み込み関係は次式により与えられる：
ｐ＝（ｋｃ／ｂ＋ｋφ）ｙｎ
ここで、ｐは圧力であり、ｙは沈み込みの量であり、ｂは地形８０８の接触領域の半径であり、そしてｋｃ／ｂ、ｋφ及びｎは地形８０８の実験定数パラメータである。計算された沈み込みｙは地形８０８により生成される（及びバネｋへ印可される）力を表す。ｙの指数はバネｋをおそらく非線形にし得る。
ｐ＝（ｋｃ／ｂ＋ｋφ）ｙｎ
ここで、ｐは圧力であり、ｙは沈み込みの量であり、ｂは地形８０８の接触領域の半径であり、そしてｋｃ／ｂ、ｋφ及びｎは地形８０８の実験定数パラメータである。計算された沈み込みｙは地形８０８により生成される（及びバネｋへ印可される）力を表す。ｙの指数はバネｋをおそらく非線形にし得る。
【0055】
図６に戻ると、工程６０８では、地形内の溝が検出される。そして検出された溝を補正するために地形の推定プロファイルに基づき用具の高さを調節するための補償値が判断される。図５に示すように、溝検出アルゴリズムブロック５２２は地形のプロファイルに関連する形状プロファイル（地形内で検出された溝の補償値を定義する）を判断する。
【0056】
地形のプロファイルは複数の点を含む。溝は、地形の推定プロファイル内の現在点を解析することにより地形内で最初に検出される。地形の推定プロファイル内の現在点は地形の推定プロファイルの最前部からの所定距離における点である。車両が地形上を走行するにつれて、追加点が地形の推定プロファイルへ加えられ、現在点を前方へ進ませる。地形の推定プロファイル内の各点は地形の形状特徴を表す形状プロファイルに関連する。現在点の解析に基づき、現在点（そして、おそらく他の点）に関連する形状プロファイルは形状特徴を定義するために更新され得る。
【0057】
図９は１つ又は複数の実施形態による地形の推定プロファイル内の溝の検出を示すグラフ９００を示す。グラフ９００では、信号９０２は地形の推定プロファイルｂｚ（ｘ）を表す。溝の検出は、推定プロファイルｂｚ（ｘ）内の現在点を解析することにより行われる。溝を検出するために、推定プロファイルｂｚ（ｘ）の傾斜
（一次導関数）が最初に計算されそして平滑化される。推定プロファイルｂｚ（ｘ）の傾斜
はグラフ９００内の信号９０４として示される。次に、推定プロファイルｂｚ（ｘ）の第２の傾斜
が計算されそして平滑化される。推定プロファイルｂｚ（ｘ）の第２の傾斜
はグラフ９００内の信号９０６として示される。次に、推定プロファイルｂｚ（ｘ）の現在点の傾斜
が零交差であるかどうかが判断される。グラフ９００に示すように、点９０８及び点９１０の傾斜
は零交差する。推定プロファイルｂｚ（ｘ）の現在点の傾斜
が零交差でなければ、現在点は地形内の溝又はこぶではなく、そして現在点は廃棄される。推定プロファイルｂｚ（ｘ）の現在点の傾斜
が零交差であれば、現在点における推定プロファイルｂｚ（ｘ）の大きさと傾斜
が零交差である最終点における推定プロファイルｂｚ（ｘ）の大きさとを比較することにより推定プロファイルｂｚ（ｘ）内の現在点が地形内の溝又はこぶであるかが判断される。例えば、点９０８の大きさは、点９０８が溝であるということを判断するために点９１０の大きさと比較される。溝は、負のＺ値を有する推定プロファイルｂｚ（ｘ）の第２の傾斜
の２つの連続零交差間の領域である。こぶは、負のＺ値を有する推定プロファイルｂｚ（ｘ）の第２の傾斜
の２つの連続零交差間の領域である。
【数18】
【数19】
【数20】
【数21】
【数22】
【数23】
【数24】
【数25】
【数26】
【数27】
【数28】
【0058】
次に、用具の高さを調節するための補償値は、検出された溝を補正するために地形の推定プロファイルに基づき判断される。特に、現在点が溝であると検出されれば、推定プロファイルｂｚ（ｘ）はこぶであると判断された最終点まで（又はこぶであると判断された最終点が存在しなければ推定プロファイルｂｚ（ｘ）の端まで）横断して戻される。こぶであると判断された現在点における推定プロファイルｂｚ（ｘ）と最終点における推定プロファイルｂｚ（ｘ）との差分が、計算され、そして現在点と最終点との間の点に関連するプロファイルを更新するために使用される。一実施形態では、用具の高さを調節するための補償値は、検出されたこぶを補正するために地形の推定プロファイルに基づき判断され得る。現在点がこぶであると検出されれば、推定プロファイルｂｚ（ｘ）は溝であると判断された最終点まで（又は溝であると判断された最終点が存在しなければ推定プロファイルｂｚ（ｘ）の端まで）横断して戻される。溝であると判断された現在点における推定プロファイルｂｚ（ｘ）と最終点における推定プロファイルｂｚ（ｘ）との差分が、計算され、そして現在点と最終点との間の点に関連するプロファイルを更新するために使用される。
【0059】
更新されたプロファイルに関連する推定プロファイルｂｚ（ｘ）内の各点は、調節されるものとして識別される。車両のボディが余りに速く走行している又は溝が広過ぎる場合には未調節点が発生する。これらの場合、用具の高さはユーザ定義される。
【0060】
溝であるとして検出された推定プロファイルｂｚ（ｘ）内の点に関連する形状プロファイル（すなわちこぶであると判断された現在点と最終点との間の一連の点）は地形の形状特徴を選択することにより更新され得る。形状特徴は任意の好適な形状を表し得る。図１０は１つ又は複数の実施形態による地形１０１０のプロファイルを補償するための様々な形状特徴の線図１０００を示す。地形１０１０のプロファイルは多くの溝を含む。様々な形状特徴１００２～１００８は地形１０１０のプロファイル内のこのような溝を補正するために選択され得る。線図１０００に示すように、形状特徴は段差形状特徴１００２、対数形状特徴１００４、二次形状特徴１００６及び傾斜特徴１００８を含み得る。形状特徴１００２～１００８は地形１０１０のプロファイル内の溝を補正するために様々な差分に従って線図１０００内に示される。例えば指数形状特徴、２つ以上の形状特徴を連結する複雑な形状特徴などの他の形状特徴も考慮される。形状特徴１００２～１００８を適用した後、地形１０１０のプロファイルはほぼ平坦に見えるだろう。
【0061】
一連の点内の各点の補償値は形状特徴と大きさとを組み合わせることにより判断される。一実施形態では、形状特徴は、０～１の範囲の高さを有し、そして一連の点の上にオーバーレイされ得る。各点の補償値は、形状特徴の高さに差分を乗算することにより判断され得る。
【0062】
図１１は１つ又は複数の実施形態による溝の補償を描写するグラフ１１００を示す。車両１１０２はボディ１１０４及び用具１１０６を含む。ボディ１１０４は重心１１０８を有し、用具１１０６は先端１１１０を有する。車両１１０２が地形の表面上を走行するにつれて、地形の表面の推定プロファイルを表す軌道１１１２が生成される。信号１１１４は、車両１１０２が地形の表面上を走行する際の車両１１０２の真のボディピッチを示す。様々な形状特徴を含む補正プロファイル１１１６が、軌道１１１２において検出された溝を補正するために適用される。
【0063】
図６に戻ると、工程６１０では、判断された補償値に基づき用具の高さを調節するための１つ又は複数の制御信号が１つ又は複数のアクチュエータへ送信される。一例では、アクチュエータは図１のアクチュエータ１１８であり得る。アクチュエータは油圧シリンダ若しくは油圧ドライブ又は任意の他の好ましいアクチュエータであり得る。図５では、関連形状プロファイルを有する地形のプロファイルが、１つ又は複数の制御信号をブルドーザ５０２の油圧系５３０へ送信するために低レベルコントローラ５２６により使用される。制御信号は、標的地形表面を実現する一方で地形内の溝及び他の擾乱も補正するために用具の高さを調節するための指令を含む。一実施形態では、標的地形表面を実現するための初期誤差値が、最終誤差値を生成するために補償値と組み合わられる（例えば、加算される）。制御信号は、最終誤差値に従って用具の高さを調節するための指令を含むように生成される。
【0064】
いくつかの実施形態では、方法６００を行う単純コントローラ（例えばＰＩＤコントローラ）はアクチュエータ遅延に起因して劣悪に動作し得る。予測コントローラ（例えばスミス予測器、ＭＰＣ）及び他のより洗練化されたコントローラが方法６００を好適に行うことができるかもしれないが、車両の振る舞いの数学的モデル化を必要とし得る。一実施形態によると、車両は数学的にモデル化され得る。図５では、車両はブレード状態予測器ブロック５２８により数学的にモデル化される。
【0065】
図１２は１つ又は複数の実施形態による車両の例示的概略図１２００を示す。図１２に示すように、車両１２０２は重心１２０６が（ｘ１，ｙ１）に位置するボディ１２０４を含む。ボディ１２０４の重心１２０６は速度ｖ１でもって地形の表面１２０８上（ボディ軌道として表される）を走行する。ボディ１２０４は、重心１２０６を中心に角速度ω１だけピッチしボディピッチθ１を生成する。ロッド１２１０がボディ１２０４から延伸し、そしてピボット１２１２を介しブーム１２１４へリンクされる。ブーム１２１４はピボット１２１２を中心に角速度ω２だけピッチしボディピッチθ２を生成する。ブーム１２１４の端点１２１６はブレードへ結合されるように構成される。端点１２１６は位置（ｘ２，ｙ２）を有する。ブレードは、基本運動学にとって重要ではなく、したがって点で構成されると仮定される。
【0066】
概略図１２００の運動学モデルは次のように与えられる：
概略図１２００の状態空間モデルは次のように与えられる：
ここで、状態は
であり、入力は
であり、基準入力は
であり、そしてｘｄ＝ｘ２＝ｙ２＝０は基準軌道を与える。ω１は唯一の被制御変数である。しかし、ω１の制御は直接的なものではなくアクチュエータの出力である。一実施形態では、アクチュエータモデルは時間遅延を有する第１次又は第２次遅延ステップ応答を有するものとして識別され得る。このようなモデルはまた、状態空間において表現され得、そしてボディ及び用具の差分運動学モデルと共に本システムの完全モデルを形成し得る。
【数29】
【数30】
【数31】
【数32】
【数33】
【数34】
【数35】
【数36】
【数37】
【数38】
【数39】
【数40】
【数41】
【0067】
典型的ブルドーザの線形状態空間モデル（ＣＴＬを含む）が次に導出される。このモデルは車両の運動学的構造に基づく。車両は、１自由度関節を介し互いに接続される（そして移動する）剛性リンクで構成されてモデル化されることになる。リンクは、任意形状を有し得、そして基準座標系に対するそれらの場所及び配向により特徴付けられ得る。各リンクには座標系が取り付けられる。各関節には、それに沿った又はそれを中心とする運動を指示するユニットベクトルが関節のタイプに依存して取り付けられる。ブルドーザのセンサ位置及び他の重要点は或るリンクの一部であると考えられる。これらの点は、取り付けられるリンクに関して表現される。リンク及び関節は併せて開放（又は閉鎖）チェーン又はツリーを形成する。このような剛体モデルは非常に詳しく述べられ得るが、十分に詳細なモデルだけが本明細書において論述されることになる。
【0068】
線形状態空間モデルは以下のリンク及び関節を順番に有することになる：
（１）車体はチェーン内の第１のリンクである。ＩＭＵ及び２本のＧＰＳアンテナがそれへ堅固に取り付けられる。車体は地面に接触しており、そしてその位置及び配向は土壌との複雑な相互作用を介し判断される。このようなリンクは可動性であると呼ばれ、その運動は関節の運動により判断されない（少なくとも直接判断されない）。
（２）ブーム（押しアーム）リンクがピボット関節を介しボディへ取り付けられる。このリンクの運動はピボット関節により制約される。ＩＭＵがこのリンク上に設置され得る。これはブレードの仰角を変更するための主機構である。
（３）ブレードは球状関節（ブレード回転中心と呼ばれる）を介しブームへ取り付けられる。この関節は、同じ位置を占有する３つの１自由度関節から構成されるとしてモデル化される。３つの（仮想）関節を接続する最初の２つのリンクは零長を有するものとしてモデル化される（したがって、これらは単に仮想リンクである）。第３のリンクはブレード自体である。例えばＩＭＵ、１つ又は２つのＧＰＳアンテナ、音響センサなどを含む多くのセンサがブレードへ取り付けられ得る。さらに、ブレードリンク上の最も重要なエンティティは中点及びその座標系である。ブレードは回転中心を中心にヨー、ピッチ、及びロール運動を経験し得る。
（１）車体はチェーン内の第１のリンクである。ＩＭＵ及び２本のＧＰＳアンテナがそれへ堅固に取り付けられる。車体は地面に接触しており、そしてその位置及び配向は土壌との複雑な相互作用を介し判断される。このようなリンクは可動性であると呼ばれ、その運動は関節の運動により判断されない（少なくとも直接判断されない）。
（２）ブーム（押しアーム）リンクがピボット関節を介しボディへ取り付けられる。このリンクの運動はピボット関節により制約される。ＩＭＵがこのリンク上に設置され得る。これはブレードの仰角を変更するための主機構である。
（３）ブレードは球状関節（ブレード回転中心と呼ばれる）を介しブームへ取り付けられる。この関節は、同じ位置を占有する３つの１自由度関節から構成されるとしてモデル化される。３つの（仮想）関節を接続する最初の２つのリンクは零長を有するものとしてモデル化される（したがって、これらは単に仮想リンクである）。第３のリンクはブレード自体である。例えばＩＭＵ、１つ又は２つのＧＰＳアンテナ、音響センサなどを含む多くのセンサがブレードへ取り付けられ得る。さらに、ブレードリンク上の最も重要なエンティティは中点及びその座標系である。ブレードは回転中心を中心にヨー、ピッチ、及びロール運動を経験し得る。
【0069】
提案システム内の運動学－動力学モジュールは以下の責務を有する：
（１）ボディ動力学：トラック・土壌相互作用の動力学的モデルを使用することによりボディの位置、配向、線形速度及び角速度を判断する。この情報はボディ上のＧＰＳ及び慣性センサを使用することにより観測器モジュールにより既に推定されているということに留意されたい。このモデリングは予測のために使用される。
（２）ブレード逆運動学：ボディの所与の場所、配向及び角速度だけでなくブレードの配向及び角速度も、ボディをブレードへ接続するすべての関節の値及び速度を判断する。
（３）ブレード順運動学：ボディの所与位置、配向、線形速度及び角速度だけでなく関節値及び速度もブレード場所、線速度及び加速度を判断する。
（４）ブレード動力学：ボディ動力学と共に、ブレードをボディに対して動かす弁・シリンダのアクチュエータモデルを使用することによりブレード運動をシミュレート（予測）する。
（１）ボディ動力学：トラック・土壌相互作用の動力学的モデルを使用することによりボディの位置、配向、線形速度及び角速度を判断する。この情報はボディ上のＧＰＳ及び慣性センサを使用することにより観測器モジュールにより既に推定されているということに留意されたい。このモデリングは予測のために使用される。
（２）ブレード逆運動学：ボディの所与の場所、配向及び角速度だけでなくブレードの配向及び角速度も、ボディをブレードへ接続するすべての関節の値及び速度を判断する。
（３）ブレード順運動学：ボディの所与位置、配向、線形速度及び角速度だけでなく関節値及び速度もブレード場所、線速度及び加速度を判断する。
（４）ブレード動力学：ボディ動力学と共に、ブレードをボディに対して動かす弁・シリンダのアクチュエータモデルを使用することによりブレード運動をシミュレート（予測）する。
【0070】
公式には、
はナビゲーション系で表現されたブレードリンク上の基準点（０，０，０）の位置を表す。ブレード自身の系において表現されたこの同じ点は
又は単純にｐＢｌａｄｅ（ｘ，ｙ，ｚ）により表されることになる。ブレード上の他の点はこの点（ｍ（ｘ，ｙ，ｚ）：ブレード中点など）に対するものである。ｐＢｌａｄｅの１つの選択肢はｍ自体である。
は、ナビゲーション系におけるブレード系の回転行列を表す。
はボディ系からブレード系への回転行列を表す。
はブレードからナビゲーション系への回転行列を表す。
はボディ上の任意の点を表すが、通常は重心に取られる。
はボディからナビゲーション系への回転行列を表す。
【数42】
【数43】
【数44】
【数45】
【数46】
【数47】
【数48】
【0071】
上に指摘したように、運動学的構造は４つの関節により接続された５つのリンクを有する。すべての関節は回転式である。関節角はΘ（ｔ）＝（θ１，θ２，θ３，θ４，θ５）として表される。各関節の説明は以下のとおりである：
（１）θ１（ｔ）は当初基準に対するブームの角度（すなわちピッチ）（通常は、ブームが平坦面（ブレード端が表面の上に載っているときにボディが位置する）となす角度）である。このピッチングはボディ系の
方向を中心に起こる。
（２）θ２（ｔ）はブームに対し垂直なベクトルを中心とするブレードの回転（すなわちヨー）である。
（３）θ３（ｔ）は、ヨーイングブレード（ｙａｗｅｄ ｂｌａｄｅ）の前方向
を中心とするブレードの傾き（すなわちローリング）である。
（４）θ４（ｔ）は、ヨーイング／ローリングブレード（ｙａｗｅｄ ａｎｄ ｒｏｌｌｅｄ ｂｌａｄｅ）の
方向を中心とする追加ピッチである。
（１）θ１（ｔ）は当初基準に対するブームの角度（すなわちピッチ）（通常は、ブームが平坦面（ブレード端が表面の上に載っているときにボディが位置する）となす角度）である。このピッチングはボディ系の
【数49】
（２）θ２（ｔ）はブームに対し垂直なベクトルを中心とするブレードの回転（すなわちヨー）である。
（３）θ３（ｔ）は、ヨーイングブレード（ｙａｗｅｄ ｂｌａｄｅ）の前方向
【数50】
（４）θ４（ｔ）は、ヨーイング／ローリングブレード（ｙａｗｅｄ ａｎｄ ｒｏｌｌｅｄ ｂｌａｄｅ）の
【数51】
【0072】
用具の実際の機構に起因してθ２（ｔ）、θ３（ｔ）及びθ４（ｔ）は互いに独立ではないということに留意すべきである。θ３（ｔ）はθ２（ｔ）が変化すれば変化することになり、そしてθ４（ｔ）について同じことが言える。θ３（ｔ）の変化はブレードを傾けることにより補償され得るが、θ４（ｔ）は車両の特徴であり、アクチュエータにより操作され得ない。
【0073】
Θ（ｔ）は用具の関節空間の要素である。ｍＢｏｄｙ（ｘ，ｙ，ｚ）（ボディ系におけるブレード中点）の各値は一意的Θ（ｔ）に対応する（一般的には真でないが、我々の特別運動学的構造においては真である）。これらの２つは次式により関係付けられ、
ここで、Ｋｐ（Θ（ｔ））は運動学的マッピングである。このマッピングはロボットアルゴリズムを使用することにより構築され得る。手短に言えば、このマッピングは、特別の４×４行列を構築すること（同次変換）Ａｉ（θｉ）、リンク－関節毎に、それらを掛け合わせること、及び次に位置部分を抽出することであって：
Ｋｐ（Θ）＝Ｋ（Θ）＊（０００１）Ｔ
ここで、
Ｋ（Θ）＝Ａ１（θ１）＊Ａ２（θ２）＊Ａ３（θ３）＊Ａ４（θ４）＊Ａ５（θ５）
は
となるように同次運動学行列である、抽出することに関与する。
また、
ここで、Ｊｐ（Θ（ｔ））＝∂Ｋｐ（Θ）／∂Θはヤコビ行列であり、そして
は関節値の変化率である。より一般的には
である。
【数52】
Ｋｐ（Θ）＝Ｋ（Θ）＊（０００１）Ｔ
ここで、
Ｋ（Θ）＝Ａ１（θ１）＊Ａ２（θ２）＊Ａ３（θ３）＊Ａ４（θ４）＊Ａ５（θ５）
は
【数53】
また、
【数54】
【数55】
【数56】
【0074】
順運動学モデルは次の状態であり得る：
ここで、ωＢｏｄｙはボディの軸を中心とするボディの回転の角速度である。
【数57】
【0075】
ブレード逆運動学に関して、Θ（ｔ）は
及び
を所与として発見される必要がある。これらの行列は観測器モジュールにより独立に推定される。これを行うために、ブルドーザ／ブレード機構はＴａｉｔ－Ｂｒｙａｎ角度系列Ｙ１－Ｚ２－Ｘ３として構築された回転行列によりモデル化され得るということに注意すべきである：
これは展開すると次のようになる：
したがって、この角度は以下のように容易に計算される：
θ２＝ａｓｉｎ（Ｒ２，１）
【数58】
【数59】
【数60】
【数61】
θ２＝ａｓｉｎ（Ｒ２，１）
【数62】
【数63】
【0076】
この方法は較正局面により補完される。この理由は、ブレードがローリングすれば計算ブームピッチは誤ることになるためである。したがって、ブルドーザを校正している間、傾くことに起因する明白な観測されたブームピッチはモデル化されなければならなく、そしてここで計算された値から減じられなければならない。二次曲線はこの依存性をモデル化するのに十分である。
【0077】
追加ＩＭＵがブーム上に設置される場合、θ１は、推定される必要がなく、そして傾斜計として追加のＩＭＵを使用することにより容易に判断され得る。他方で、θ４のヨーへの依存性は常に校正されなければならない。再び、二次曲線がこの依存性を十分な精度でモデル化し得る。
【0078】
角速度を計算するために、我々は次式を使用し得る：
関節速度を計算するための速度はボディ、ブーム及びブレード上のＩＭＵにより測定される。しかし、上記モデルが予測制御のために使用される場合、これらの速度はアクチュエータの動力学的モデルの出力である。完全油圧モデルは、必要なセンサが設置されていなければ（これは実際には通常はそうである）余りに厄介でありまた不正確である。しかし、油圧弁の動力学のための単純化モデルは容易に構築され得る。このようなモデルは入力として弁命令を受信するだろう、そして伸縮の結果速度を出力することになる。ブルドーザでは、様々なシリンダの長さの変化は最終的には関節を中心とするリンクの回転運動に至る。したがって、油圧モデルは弁命令と関節角速度との間のマッピングと考えられ得る。ブームピッチ及びブレード傾斜は次式をもたらす唯一の制御可能関節である：
ここで、Ｔ（）は伝達関数を表し、ｃは弁命令である。伝達関数は実験的に識別され得る。遅延を有する二次伝達関数
は弁へ送信される命令と測定された角速度との関係を説明するために十分過ぎることが示された。測定は搭載ＩＭＵを使用することにより行われ得る。
【数64】
【数65】
【数66】
【0079】
最後に、動力学的モデルは、地面と接触しそして相互作用する際のボディのボディ角速度を計算するために使用される。地面は、本明細書において説明される実施形態に従ってボディが移動することになる地形の表面の推定を指す。この地面は２Ｄ空間における曲線（プロファイル）として表される。地面は、相互作用を現実的なものにするために土質を備え持つ必要がある。既に論述されたように、リアルタイム剛体手法が使用され得る。質量及び慣性モーメントを有するボディとトラックへ取り付けられそしてそれに沿って分散された一連の離散的バネ／緩衝器要素とを含む集中モデルが使用され得るということが仮定される。相互作用はトラックの底へさらに制限される。ここで、ブレードの端と地面との相互作用は、力センサを必要とするだろうからモデル化されない。ロードホイールとボディとの間にサスペンションは無いということも想定される（タンクのような機械の場合はそのような安全な仮定は無い）。個々の要素の剛性Ｋ及びダンピングＤのために選択されるパラメータはゴムだけでなく土壌の特性も考慮に入れそして表さなければならない。
【0080】
特定土壌タイプに関して、ベッカーの式（ｎ＝１）は土壌内への侵入の量と生成された力との間の関係を与える。しかし、これは正確に線形バネの定義である。土壌の一覧化パラメータがばねの剛性係数の推定を計算するために使用され得る。ダンピングはモデルを補完するために加えられる。その値は実験を介し推定される。
【0081】
問題が２Ｄ問題へ帰着されたということを所与に、走行がｘに沿って発生しそして点は座標（ｘ，ｚ）を有するということを仮定する。ボディの重心の高さをｚｃ（ｔ）により表す。Ｍ及びＩはそれぞれボディの質量及び慣性モーメントを表す。慣性モーメントは一般的に、ボディに対する用具の位置の変化が著しければ可変であり得る。
【0082】
前進速度は原理的にトラックと地面との間の引張力をモデル化することにより見出され得るが、この場合、前進速度は、予測の開始時に知られ（観測され）、そして予測範囲中一定のままであると考えられ得る。したがって、
である。
【数67】
【0083】
ボディのバウンス動力学方程式は次のように表される：
ここで、
ｚｉ（ｔ）＝ｚｃ（ｔ）－ｌｉ（ｃｏｓ（φｉ）ｓｉｎ（θ（ｔ））＋ｓｉｎ（φｉ）ｃｏｓ（θ（ｔ）））
ここで、θ（ｔ）はボディのピッチである。
【数68】
【数69】
【数70】
【数71】
【0084】
同様に、ピッチ動力学に関し：
ここで、
τｉ（ｔ）＝ｌｉｆｉｃｏｓ（φｉ）ｃｏｓ（θ）
である。
【数72】
τｉ（ｔ）＝ｌｉｆｉｃｏｓ（φｉ）ｃｏｓ（θ）
である。
【0085】
一実施形態では、方法６００による制御システムの性能が評価され得る。制御システムを評価するために、予測ボディピッチ（工程６０６において判断された）は、用具が位置する点の上をボディが通った後に実際のボディピッチと相関付けられる。図５では、ボディ相関ブロック５２４が予測ボディピッチと実際のボディピッチとを相関付ける。評価は補償の妥当性の信頼性を調節するために使用され得る。評価は、振る舞い及びパラメータを自動的に調節するために監視制御ボックスへ、又は振る舞い及びパラメータの手動調整のためにユーザへ出力され得る。次の２つの相関に関心がある：１）ボディ・ツー・ボディ相関、及び２）ボディ・ツー・用具相関。
【0086】
ボディ・ツー・ボディ相関はトラックと地形との相互作用の予測可能性を測定する。ボディ・ツー・ボディ相関を計算するために、ボディピッチは、ボディの重心から一定距離にある仮想ボディ点のＺ成分と比較されることになる。一定距離は、ボディの重心における点と用具の端における点との間の距離と同じ距離である。仮想点の高さがボディの重心における点の高さに対応すれば（相関すれば）（ボディのピッチパターンを表せば）、ボディが仮想点の場所に到達すると期待通りに振る舞うことになるというより高い可能性がある。より高い相関は判断された補償値のより高い信頼性を指示する。
【0087】
ボディ・ツー・用具相関は、用具の運動がボディの運動を予測し得る量を測定する。この相関は、ボディのピッチと用具上の点のＺ成分とを比較することにより計算される。より高いボディ・ツー・用具相関は用具の振動的振る舞いを指示する。したがって、制御システムはボディ・ツー・用具相関を低減すべきである。
【0088】
一例では、車両は完全に正弦曲線地形上を走行する。図１３は、１つ又は複数の実施形態による車両が正弦曲線地形上を走行するにつれての車両のボディ・ツー・ボディ相関及びボディ・ツー・用具相関を示すグラフ１３００を示す。信号１３０２は、正弦曲線地形に起因して正弦曲線であるボディのピッチを表す。信号１３０６は、本明細書において説明される実施形態に従って判断された用具の高さの補正を表す。補正は段差形状特徴として適用された。信号１３０４は、補正に従って調整された用具の位置を表す。グラフ１３００に示すように、用具の位置（信号１３０４）は補正（信号１３０６）をかなりうまく追跡する。したがって、ボディ・ボディ相関（信号１３１０）は高くそしてボディ・ツー・用具相関（信号１３０８）は低く、これは良い性能を指示する。うまく動作するシステムに関して、用具はボディのピッチとの同期が完全に外れる。
【0089】
相関の信頼性の欠如は次のように計算され得る：
ここで、σｕｂ及びσｂｂはそれぞれボディ・ボディ相関及びボディ・ツー・用具相関を表し、σｓｏｉｌは地形の柔らかさを表す。地形が滑らかになればなるほど、ボディが期待振る舞いを呈示するという可能性は低くなる。換言すれば、より滑らかな地形は、地形がその形状を変更する及び／又は用具の位置が地形のプロファイルを表さないという結果を生じ得る。このような状態では、補正の強さは低減されるべきである。
【数73】
【0090】
方法６００は任意の好適なコントローラにより行われ得る。一実施形態では、コントローラは低レベルコントローラである。低レベルコントローラは、用具の端が標的表面に従って地形の表面を追跡するということを保証する。この問題は、別のｐ（ｘ，ｚ）による（標的レベル）平面ｓ（ｘ，ｚ）上の１つの点の収束及び追跡（サジタルボディ上へのブレード端の投影）の問題に帰した。これはブルドーザの仰角（メインフォール）制御と呼ばれる。同時に、コントローラはまた、端全体が所望横断勾配（表面により支配される）上にあるということを確実にしなければならない。メインフォール及び横断勾配誤差はｅｍａｉｎｆａｌｌ及びｅｃｒｏｓｓ－ｓｌｏｐｅでそれぞれ表される。横断勾配制御は、メインフォール制御に直交するものと考えられ得、そして他の制御から独立に行われ得る。これらの２つの制御を行うアクチュエータは別々に作動され得るが、油圧系に起因してそれらの間の結合が存在する。円滑な追跡を実現しそして設定点追跡を回避するためにそれらを結合することは有益である。例えば、
などの滑らかな切替え（重み付け）機構は、十分な力による仰角（ブームピッチ）調節を試みる前に所望横断勾配が適切に実現されるということを確実にし得る。ここで、Ｋｅｌｅｖａｔｉｏｎは仰角コントローラの利得である。
【数74】
【0091】
アクチュエータにおける遅延のおかげで、コントローラはプラントにおける出力遅延に対処することができるべきである。１つ好ましいコントローラは特別に調節されたＰＩＤコントローラである。しかし、弁モデルは測定を介した十分な精度で知られているので、より良い解決策はスミス予測器型コントローラになるだろう。例示的スミス予測器型コントローラ１４００が図１４に示される。スミス予測器型コントローラ１４００は仰角誤差をピッチ誤差に変換する。代替案として、ＭＰＣ（モデル予測制御：ｍｏｄｅｌ ｐｒｅｄｉｃｔｉｖｅ ｃｏｎｔｒｏｌ）コントローラが利用され得る。上に導出された動力学的モデルはＭＰＣコントローラのバックボーンを形成することになる。ＭＰＣコントローラは、有界水平線内でシステム全体の将来振る舞いを予測するためにモデルを使用する。
【0092】
本明細書において説明される実施形態の範囲は、より一般的であるということと、そして地形の表面のプロファイルを判断する他の手段を備えたシステムへ容易に適用され得るということとを理解すべきである。例えば、地形の表面のプロファイルは、地形の表面の方向へ向けられた（おそらくブーム上に取り付けられた）光センサ（例えばレーザセンサ）を使用することにより追加的に又は代替的に判断され得る。上述のように軌道の点を判断する代わりにブーム上に取り付けられた光センサを使用することによれば、点はボディ位置及び配向、ブームピッチ角、ブーム上の光センサの場所、及び光センサにより測定された距離に基づく運動学方程式を使用することにより計算され得る。
【0093】
本明細書において説明されたシステム、装置及び方法は、ディジタル回路構成を使用することにより、又は周知のコンピュータプロセッサ、メモリユニット、ストレージデバイス、コンピュータソフトウェア及び他の部品を使用する１つ又は複数のコンピュータを使用することにより実装され得る。通常、コンピュータは、指令を実行するためのプロセッサ及び指令及びデータを格納するための１つ又は複数のメモリを含む。コンピュータはまた、１つ又は複数のマスストレージデバイス（１つ又は複数の磁気ディスク、内部ハードディスク及び着脱可能ディスク、光磁気ディスク、光ディスクなど）を含んでもよいしそれへ結合されてもよい。
【0094】
本明細書において説明されたシステム、装置及び方法は、プログラム可能プロセッサによる実行のために情報キャリア内に（例えば非一時的機械読み取り可能なストレージデバイス内に）有形に具現化されたコンピュータプログラム製品を使用することにより実装され得；本明細書において説明された方法及びワークフロー工程（図３及び図５～６の１つ又は複数の工程又は機能を含む）は、このようなプロセッサにより実行可能である１つ又は複数のコンピュータプログラムを使用することにより実施され得る。コンピュータプログラムは、或る活動を行う又はいくつかの結果を引き起こすコンピュータ内の直接又は間接的に使用され得る一組のコンピュータプログラム指令である。コンピュータプログラムは、任意の形式のプログラミング言語（コンパイル又はインタープリタ型言語を含む）で書かれ得、そして任意の形式で（例えば、スタンドアロンプログラムとして、又はモジュールとして、部品として、サブルーチンとして、又はコンピューティング環境内の使用に好適な他のユニットとして）配備され得る。
【0095】
本明細書において説明されたシステム、装置及び方法を実施するために使用され得る例示的コンピュータ１５０２のハイレベルブロック図が図１５に描写される。本明細書で論述されたシステム及び装置（図１のコントローラ１２０、図４のコントローラ４００及び図１４のコントローラ１４００を含む）のうちの任意のもの又はすべては、コンピュータ１５０２などの１つ又は複数のコンピュータを使用することにより実装され得る。コンピュータ１５０２はデータストレージデバイス１５１２及びメモリ１５１０へ作動可能に結合されたプロセッサ１５０４を含む。プロセッサ１５０４は、このような動作を定義するコンピュータプログラム指令を実行することによりコンピュータ１５０２の動作全体を制御する。コンピュータプログラム指令は、データストレージデバイス１５１２又は他のコンピュータ読み取り可能な記録媒体内に格納され得、そしてコンピュータプログラム指令の実行が望まれるとメモリ１５１０内へロードされ得る。したがって、図３及び図５～６の方法及びワークフローの工程又は機能は、メモリ１５１０及び／又はデータストレージデバイス１５１２内に格納されたコンピュータプログラム指令により定義され得、そしてコンピュータプログラム指令を実行するプロセッサ１５０４により制御され得る。例えば、コンピュータプログラム指令は、図３及び図５～６の方法及びワークフロー工程又は機能を行うために当業者によりプログラムされたコンピュータ実行可能コードとして実装され得る。したがって、コンピュータプログラム指令を実行することにより、プロセッサ１５０４は図３及び図５～６の方法及びワークフローの工程又は機能を実行する。コンピュータ１５０４はまた、ネットワークを介し他のデバイスと通信するための１つ又は複数のネットワークインターフェース１５０６を含み得る。コンピュータ１５０２はまた、コンピュータ１５０２とのユーザ相互作用を可能にする１つ又は複数の入力／出力デバイス１５０８（例えばディスプレイ、キーボード、マウス、スピーカー、ボタンなど）を含み得る。
【0096】
プロセッサ１５０４は、汎用マイクロプロセッサ及び専用マイクロプロセッサの両方を含み得、そしてコンピュータ１５０２の単独プロセッサ又は複数のプロセッサの内の１つであり得る。例えば、プロセッサ１５０４は１つ又は複数の中央処理ユニット（ＣＰＵ）を含み得る。プロセッサ１５０４、データストレージデバイス１５１２、及び／又はメモリ１５１０は、１つ又は複数の特定用途集積回路（ＡＳＩＣ： ａｐｐｌｉｃａｔｉｏｎ－ｓｐｅｃｉｆｉｃ ｉｎｔｅｇｒａｔｅｄ ｃｉｒｃｕｉｔ）及び／又は１つ又は複数のフィールドプログラマブルゲートアレイ（ＦＰＧＡ）を含み得る、又はそれらにより補完され得る、又はそれらの中に取り込まれ得る。
【0097】
データストレージデバイス１５１２及びメモリ１５１０はそれぞれ有形非一時的コンピュータ読み取り可能なストレージ媒体を含む。データストレージデバイス１５１２及びメモリ１５１０はそれぞれ、ダイナミックランダムアクセスメモリ（ＤＲＡＭ）、スタティックランダムアクセスメモリ（ＳＲＡＭ）、ダブルデータレート同期ダイナミックランダムアクセスメモリ（ＤＤＲ ＲＡＭ）、又は他のランダムアクセス固体メモリデバイスなどの高速ランダムアクセスメモリを含み得る。データストレージデバイス１５１２及びメモリ１５１０は、不揮発性メモリを含み得、不揮発性メモリは：内部ハードディスク及び着脱可能ディスク、光磁気ディスクストレージデバイス、光ディスクストレージデバイスなどの１つ又は複数の磁気ディスクストレージデバイス；フラッシュメモリデバイス；消去可能プログラマブル読み出し専用メモリ（ＥＰＲＯＭ）、電気的消去可能プログラマブル読み出し専用メモリ（ＥＥＰＲＯＭ）などの半導体メモリデバイス；コンパクトディスク読み出し専用メモリ（ＣＤ－ＲＯＭ）；デジタルバーサタイルディスク読み出し専用メモリ（ＤＶＤ－ＲＯＭ）ディスク；又は他の不揮発性固体ストレージデバイスなどを含み得る。
【0098】
入力／出力デバイス１５０８は、プリンタ、スキャナ、ディスプレイ画面などの周辺機器を含み得る。例えば、入力／出力デバイス１５０８は：情報をユーザへ表示するための陰極線管（ＣＲＴ）、又は液晶ディスプレイ（ＬＣＤ）モニタなどの表示デバイス；キーボード；及びマウス又はユーザが入力をコンピュータ１５０２へ提供し得るトラックボールなどのポインティングデバイスを含み得る。
【0099】
当業者は、実際のコンピュータ又はコンピュータシステムの実装が他の構造を有し得、そして他の部品も同様に含み得るということと、図１５が例示目的のためのこのようなコンピュータの部品のうちのいくつかの部品の高レベル表現であるということとを認識することになる。
【0100】
これまでの詳細な説明はあらゆる点で例示的であるが限定的でないと理解されるべきであり、本明細書において開示された発明の範囲は、詳細な説明からではなく、むしろ特許法により許容される範囲全体に従って解釈される特許請求の範囲から判断されるべきである。本明細書に示され説明された実施形態は本発明の原理の例示に過ぎないということと様々な修正が本発明の範囲及び精神から逸脱することなく当業者により実施され得るということとを理解すべきである。当業者は本発明の範囲及び精神から逸脱することなく様々な他の特徴の組み合わせを実現する可能性がある。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【国際調査報告】