JP5420061B2 - 作業アームの位置制御装置を備えた移動作業機械および移動作業機械の作業アームを位置制御する方法 - Google Patents

Description

本発明は少なくとも１つの作業アームを備えた移動作業機械、例えばバックホー、上部構造付きの貨物自動車または農業機械および林業機械に関する。この種の作業機械の作業アームはたいてい関節で相互に連結された複数のセグメントを有しており、柄とも呼ばれるセグメントには、シャベル、クラムシェルまたはハンマーのようなツールが取り付けられている。

たいていこの種の作業機械では、作業アームの現在の位置と姿勢が、特にツールの位置と姿勢がディスプレイ上でユーザに示される。これによりユーザは厳密に設定された計画に従って作業を遂行することが可能になり、例えば動かしたバラ荷または土壌または形成すべきもしくは既に形成された地盤の到達した高さ、長さ、深さまたは傾きに関する直接的なフィードバックが得られる。

この種のユーザディスプレイは例えばDE 201 16 666 U1とUS 5,854,988 Aから公知である。

良好にレトロフィットできるように、ツールの位置と姿勢は、作業アームの個々のセグメントに取り付けられた傾きセンサによって求められる。そして、アームの各セグメントの傾きから、例えば調節ブーム、ブーム、アームおよびツールからなる運動連鎖の既知の幾何学的関係を介してツールの位置と姿勢が計算される。

しかし、傾きセンサとして、例えば重力検知用の振り子のような慣性測定原理に基づいたセンサが使用されるため、傾きセンサはこの種の作業アームを使用する際に避けられない振動による加速度も検知してしまう。運動に起因するこの種の加速度はツールの位置と姿勢の測定を著しく妨害したり、一時的に不可能にしたりしかねない。これらの妨害を抑制するための公知の措置は、センサ信号をローパスフィルタリングして、有効周波数帯域の外では運動に起因する加速度を抑制することを主な内容としている。

しかしこれは複数の欠点をもたらす。一方では、有効周波数帯域内の加速度はこのようにしては抑圧できず、他方では、ローパスフィルタリングは数百ミリ秒までの時間遅延を引き起こす。しかし、この時間遅延は十分に正確な位置表示を可能にし、また作業アームの手動操作を妨げないので受け入れられる。

他の技術分野からは、例えばロボット、飛行体または車両の位置制御のために、加速度に基づいた傾きセンサと回転速度センサとの組合せを用いることが公知である。WO 01/57474 A1には、このような方法として、位置を計算するためにクォータ二オン表示を利用する方法が開示されている。

本発明の課題は、遅延時間が十分に短いため、遅延時間を位置表示のためだけでなく、作業機械の作業アームの位置制御にも使用できる、位置算出のための装置を備えた移動作業機械を提供することである。しかしその際、作業アームの動特性が制限されてはならない。本発明の別の課題はこのような位置制御のための方法を提供することである。

本発明によれば、この課題は独立請求項の対象によって解決される。本発明の有利な実施形態は従属請求項の対象である。

本発明による、作業アームの位置制御装置を備えた移動作業機械は、関節により第１端が作業機械の上部旋回体に取り付けられた作業アームを有している。作業アームの第２端にはツールが可動に取り付けられている。

作業機械はさらに複数のセンサ、それも上部旋回体に取り付けられた少なくとも１つの傾きセンサと、作業アームに取り付けられた少なくとも１つの別の傾きセンサと、作業アームに取り付けられた少なくとも１つの回転速度センサとを有している。作業機械はさらに、少なくとも１つの傾きセンサと少なくとも１つの別の傾きセンサと少なくとも１つの回転速度センサの信号を処理するための計算ユニットを有している。計算ユニットはもちろんアームに取り付けられたセンサモジュールの中に収納してもよい。

本発明の基本的アイデアによれば、リアルタイム制御のためには、最長でも約２５０ｍｓの遅延時間が達成されなければならない。しかし、この値はまた使用される作業アームハードウェアと使用目的とによっても変わりうる。それゆえ、測定値のローパスフィルタリングとそれに伴う信号遅延は断念しなければならない。その一方で、以前と同じように傾きセンサは使用すべきである。傾きセンサは比較的低コストで頑丈であり、そのうえ容易にレトロフィットできるので、移動作業機械での使用に特に適しているからである。傾きの測定を既に説明した運動に起因する加速度による妨害に対してよりロバストにするために、さらに回転速度センサが使用される。回転速度センサは高い動的正確さを有しているが、オフセットや雑音といった問題を抱えている。確かに、航空ナビゲーションで使用される回転速度センサで、サニャック効果に基づく、例えば光ファイバジャイロスコープを使用した回転速度センサは非常に正確である。しかし、マイクロメカニカル回転速度センサは明らかにより低コストであり、何よりもロバストであるから、移動作業機械での使用により適している。この種の回転速度センサには上記したドリフト問題が存在するため、本発明では回転速度センサは傾きセンサと組み合わされる。

この解決手段は、まさに移動作業機械での使用に向いているという利点を有している。リアルタイム制御は、非常に良い静的正確さを有する傾きセンサと、非常に良い動的正確さを有する回転速度センサと、データ融合または評価アルゴリズムとの組合せによって可能になる。

作業アームは関節によって相互に連結された複数のセグメントを有していてよい。ツールは例えば最後のセグメントの端に取り付けられている。いずれの場合においても、作業アームは少なくとも１つのセグメントを有している。

１つの実施形態では、各セグメントに１つの別の傾きセンサと１つの回転速度センサが配置される。

代替的な実施形態では、各セグメントに２つの別の傾きセンサと１つの回転速度センサが配置される。この実施形態では、加速度の１つの空間成分（例えばｘ方向の）だけでなく、同時に２つの空間成分（例えばｘ方向とｚ方向の）を測定することができ、これにより、所定の傾斜角範囲において測定の感度が改善される。

傾きセンサは様々な測定原理に基づくものであってよく、例えば振り子体および／または屈折性を有する液体鏡を有していてよい。傾きセンサはまた容量性または伝導性傾きセンサとして形成されていてもよいが、有利にはマイクロメカニカル加速度センサとして形成されている。

回転速度センサは特にマイクロメカニカルセンサとして形成されている。

位置制御装置は、例えばバックホー、テレハンドラー、ローダーバケット付きバックホー、ホイールローダー、ローダークレーンまたは林業機械のような移動作業機械での使用に適している。

本発明の別の側面では、移動作業機械の作業アームの位置を制御する方法が提供される。この方法において、位置算出は次のステップを有している。作業アームに作用する加速度

を求める。これは例えば３つの空間方向すべてにおいて行われる。その際、３つの成分ａ_x、ａ_y、ａ_zが３つの加速度センサによって測定される。しかし、ｘ方向とｚ方向の加速度だけを測定するだけでも十分である。作業アームが静止していると仮定して、作業アームに作用する加速度

から傾斜角θの第１の値θ_sを計算する。なおここで、傾斜角θはｙ軸回りの作業アームの傾きの角度として定義されている。

傾斜角θを求めるために、作業アームのセグメントまたはセンサの静止座標系から上部旋回体の静止座標系への座標変換が行われるので、作業アームに取り付けられている加速度センサに関して、次の関係が成り立つ。

ここで、ａは物体固定座標軸ｘ、ｙ、ｚの方向におけるセンサの測定値を、添字Ｓはその時その時のセンサを表している。ｖは作業アームの速度を、ωは作業アームの角速度を、φはいわゆるオイラー角のロール角を、θは傾斜角を表している。ｇは重力加速度を表している。

（１）の各式の第１項は作業機械全体または作業アームないしセグメントの並進から生じる加速度を記述しており、真ん中の各項は作業機械全体または作業アームないしセグメントの回転から生じる加速度を記述している。したがって、これらの項は最後の項とは違って「妨害」を記述している。なぜならば、これらの項は地球の重力場の影響から生じたのではない加速度に関するものであり、したがって重力場中でのセンサの傾きを描写していないからである。

（１）の第２行から明らかなように、ｙ方向において測定する加速度センサは、ロール角φが極小さい場合、上で述べた妨害しか測定しないので、傾斜角θの第１の値θ_sを計算するために静的なケース（静止した作業アーム）を仮定するのであれば、ｙ方向において測定する加速度センサは必要に応じて省いてもよい。（１）の第１行と第３行によって記述される加速度信号は主としてｘ方向とｚ方向とにおける直線運動と、ｙ軸回りの回転（向心力項とコリオリ力項）とによって妨害される。

加速度センサの測定値から傾斜角θの第１の値θ_sを計算するために、例えば以下のように進む。

作業アームが静止していると仮定すれば、つまり、静的ケースを仮定すれば、（１）から次の式が得られる。

これらの式から、求める傾斜角θの第１の値θ_sが

によって得られる。

また、（２）から導かれるように、静止したアームについて、ただ１つのセンサだけを用いてθ_sを

によって求めることも可能である。しかし、これはθ_sの値の範囲が区間−９０°〜９０°に制限されるという欠点を有している。現実的な状況を記述するためには、少なくとも作業機械のアーム、つまり、ツールが取り付けられているアーム部分と、ツールそれ自体とにとって、この値範囲は小さい過ぎる。その上、この場合、−９０°と９０°の近傍では正弦関数の勾配が小さいため、測定感度が低い。その代わりに、−１８０°から１８０°までの値の範囲を有する２変数の逆正接関数（"atan2"）を（２）に適用してもよい。

傾斜角θの第１の値θ_sを求めるために、ここでは作業アームは静止していると仮定している。作業アームが動いている場合および作業機械の上部旋回体が旋回している場合には、（１）の第１項と真ん中の項とによる妨害が急激に大きくなり、（３）に基づく傾斜角の算出は十分に正確な結果をもはやもたらさない。それゆえ、このやり方は作業アームの位置のリアルタイム制御には不十分である。

したがって、さらに作業アームの角速度ω、それも少なくともｙ軸回りの回転の成分ω_yが測定される。さらに、剛体の運動学から、回転速度ないし角速度ωをオイラー角θ、φおよびψ並びにその時間微分と関係づける以下の方程式系が知られている。

この方程式系から、ロール角φが極小さい場合には、

が得られる。
それゆえ、動的ケースにおける、傾斜角θの第２の値θ_dは角速度ω_yの期間ｔにわたる積分によって得られる。

しかしこれでは、例えばオフセット誤差とセンサ雑音も連続的に積分されてしまい、そのため比較的短い時間が経過しただけでも真の傾斜角θからの大きな偏差が生じ、その結果、測定の正確さがそれに課された要求を満たさないという問題が生じる。少なくとも、マイクロメカニカル回転速度センサを使用した場合はそうである。例えば光ファイバジャイロセンサのような他のセンサであれば、多くの場合において十分な、より高い正確さが得られるであろう。しかし、このようなセンサは高コストで比較的ロバストネスが低いという欠点を有しているため、移動作業機械での使用にはあまり適していない。

静的ケースにおいて良好な描写をする傾斜角センサと、動的ケースにおいて良好な描写をするが、比較的長い期間にわたってドリフト問題を有する回転速度センサとを用いて、傾斜角θの十分正確な値を得るために、２つの値θ_sとθ_dとから真の傾斜角θの推定値

を求める推定アルゴリズムが使用される。ｘ方向とｚ方向とにおいて測定された加速度ａ_x,Sおよびａ_z,Sから、（３）に応じたａｔａｎ２関数を介して静的ケースにおける角度θ_sが計算され、その後、回転速度ω_yの積分のための基点として使用される。さらに、積分結果と基点値との差が推定アルゴリズムにフィードバックされる。このようにして誤差のある初期値θ₀も廃棄される。

このためには、それ自体公知の推定アルゴリズム、例えばカルマンフィルタや、制御技術から公知の状態観測法を使用することができ、場合によってはさらに回転速度センサのオフセット誤差を推定してもよい。

その結果が真の傾斜角θの推定値

である。

本発明の方法は、３つのセンサ、すなわち２つの加速度センサと１つの回転速度センサしか用いずに、リアルタイム制御を可能にする正確さで作業アームまたは作業アームの特定の点、例えばツールの懸架点、の位置算出を実現するという利点を有している。

１つの実施形態では、作業アームは関節によって相互に連結された複数のセグメントを有しており、傾斜角の推定値

の算出は各セグメントにつき個々に行われる。なお、添字ｉは相応するセグメントを表している。

１つの実施形態では、

から、移動作業機械の作業アームに取り付けられたツールの位置および姿勢が計算される。これらの位置および姿勢は必要に応じて制御してもよい。

しかしまた、

から、移動作業機械の作業アームに配設された少なくとも１つの油圧シリンダの変位を計算し、必要に応じて制御してもよい。

本発明の１つの実施形態によるバックホーとして形成された移動作業機械を概略的に示す。 図１の作業機械の作業アームにおける幾何学的関係の概略図を示す。 本発明による位置制御の実施形態における上位の信号の流れを示す。 運動によって妨害された傾き信号のグラフを示す。 ローパスフィルタリングによる傾斜角算出のグラフを示す。 回転速度センサを用いたドリフト有りの傾斜角算出のグラフを示す。 本発明による傾斜角推定のブロック回路図を概略的に示す。

本発明の実施例を添付図面に即して以下で詳しく説明する。

すべての図において、同一の部分には同じ参照符号が付されている。

図１には、バックホーとして形成された移動作業機械１が概略的に示されている。この実施形態では、作業機械１は、地盤５の上に静止している、または地盤５の上を走っている下部構造２と、垂直軸４回りに旋回可能な上部旋回体３を有している。

上部旋回体３には関節によって作業アーム６が連結されており、作業アーム６は同様に関節によって相互に連結された第１セグメント７と第２セグメント８を含んでいる。第２セグメント８にはツール９が連結されている。ツール９の取付点はツールセンタポイント（ＴＣＰ）とも呼ばれる。上部旋回体と第１セグメント７との間、セグメント７と８の間、および第２セグメント８とツール９との間には油圧シリンダ１１が配置されており、それぞれ角度θ_iを成している。

上部旋回体３にはセンサユニット１３が、第１セグメント７にはセンサユニット１４が、第２セグメント８にはセンサユニット１５が、ツール９にはセンサユニット１６が配置されている。この実施形態では、各センサユニット１３、１４、１５および１６は２つの傾きセンサと１つの回転速度センサを含んでいる。ただし、２つの傾きセンサはｘ方向とｚ方向とにおける加速度を測定し、回転速度センサはｙ軸回りの回転の角速度を測定する。

図示されていない１つの実施形態では、センサユニットごとに１つの傾きセンサを省いてもよく、また上部旋回体の運動による妨害が比較的少ないと予想される場合には、上部旋回体３に割り当てられたセンサユニットは１つの傾きセンサだけを有し、回転速度センサは有さないようにしてもよい。

センサユニット１３、１４、１５および１６の測定データは移動作業機械１の計算ユニット１７に供給され、計算ユニット１７はこれらの測定データから特にＴＣＰの位置算出および位置制御、並びにツール９の姿勢の算出および制御を行う。しかし、位置を算出する計算ユニットはアームに取り付けられたセンサモジュールの中にあってもよい。

図２には、図１の作業機械の作業アームにおける幾何学的関係の概略図が示されている。

上部旋回体３は角度θ₁だけ、第１セグメントは角度θ₂だけ、第２セグメントは角度θ₃だけ、ツール９は角度θ₄だけ重力加速度ｇの方向に対する垂線に対して傾いている。第１セグメント７の長さである第１の回転中心１８と第２の回転中心１９との間の距離ｌ₂と、第２セグメント８の長さである第２の回転中心１９とＴＣＰ１０との間の距離ｌ₃が既知であるから、傾斜角θとＴＣＰ１０の位置とツール９の姿勢は計算することができる。つまり、アームセグメントの幾何学的（構造）データは前もって既知である。

図３には、本発明による位置制御の実施形態における上位の信号の流れが示されている。

第１ステップでは、運動連鎖のすべての構成要素において、つまり上部旋回体３、第１セグメント７、第２セグメント８およびツール９において、加速度の測定と回転速度の測定が行われる。これらの測定値から、傾斜角θ_iの動的な算出が行われる。

最後に、図２に関連して説明したように、傾斜角θ_iからツール９の位置と姿勢を求めてもよい。しかし、代替的または付加的に、傾斜角θ_iから油圧シリンダ１１の変位の計算を行ってもよい。

図４には、傾斜角の計算が方程式（３）だけから、すなわち傾きセンサだけを用いて行われる場合に得られる、運動によって妨害された傾き信号のグラフが示されている。この場合、運動に起因する加速度のせいで、計算された傾斜角と真の傾斜角との間に部分的に著しい偏差が生じる。

図５には、ローパスフィルタリング有りで方程式（３）から傾斜角を求めるグラフが示されている。ただし、ローパスフィルタリングに起因する時間遅延が明らかである。

図６には、回転速度センサを用いた、つまり傾きセンサを用いない、方程式（７）によるドリフト有りの傾斜角算出のグラフが示されている。

図７には、個々のセンサユニットの傾斜角θの本発明による推定のブロック回路図が概略的に示されている。まず、センサユニットの傾きセンサの測定値ａ_x,Sおよびａ_z,Sが静的傾斜角θ_sを計算するために用いられ、同じセンサユニットの回転速度センサの測定値ω_yが動的傾斜角θ_dを計算するために積分される。続いて、傾斜角の推定値

を得るために、静的傾斜角と動的傾斜角との差が評価アルゴリズムにフィードバックされる。

評価アルゴリズムとしては、例えばカルマンフィルタや状態観測のようなそれ自体公知の方法またはこれらから派生もしくは改良された方法を使用することができる。

１ 作業機械
２ 下部構造
３ 上部旋回体
４ 軸
５ 地盤
６ 作業アーム
７ 第１セグメント
８ 第２セグメント
９ ツール
１０ ツールセンタポイント
１１ 油圧シリンダ
１３ センサユニット
１４ センサユニット
１５ センサユニット
１６ センサユニット
１７ 計算ユニット
１８ 第１の回転中心
１９ 第２の回転中心

Claims (21)

1. 作業アーム（６）の位置制御装置を備えた移動作業機械（１）であって、
   第１端において関節により前記作業機械（１）の上部旋回体（３）に取り付けられた作業アーム（６）と
   前記作業アーム（６）の第２端に可動に取り付けられたツール（９）と、
   前記上部旋回体（３）に取り付けられた少なくとも１つの傾きセンサと、
   前記作業アーム（６）に取り付けられた少なくとも１つの別の傾きセンサと、
   前記作業アーム（６）に取り付けられた少なくとも１つの回転速度センサと、
   前記少なくとも１つの傾きセンサの信号、前記少なくとも１つの別の傾きセンサの信号および前記少なくとも１つの回転速度センサの信号を処理するための計算ユニット（１７）と
   を有していることを特徴とする、移動作業機械（１）。
2. 前記作業アーム（６）は関節により相互に連結された複数のセグメント（７，８）を有している、請求項１記載の移動作業機械（１）。
3. 前記各セグメント（７，８）に、別の１つの傾きセンサと１つの回転速度センサが配置されている、請求項２記載の移動作業機械（１）。
4. 前記各セグメント（７，８）に、別の２つの傾きセンサと１つの回転速度センサが配置されている、請求項２記載の移動作業機械（１）。
5. 前記傾きセンサは振り子体を有している、請求項１から４のいずれか１項記載の移動作業機械（１）。
6. 前記傾きセンサは屈折性のある液体鏡を有している、請求項１から５のいずれか１項記載の移動作業機械（１）。
7. 前記傾きセンサは容量性傾きセンサとして形成されている、請求項１から６のいずれか１項記載の移動作業機械（１）。
8. 前記傾きセンサはマイクロメカニカルセンサとして形成されている、請求項１から７のいずれか１項記載の移動作業機械（１）。
9. 前記傾きセンサは伝導性傾きセンサとして形成されている、請求項１から８のいずれか１項記載の移動作業機械（１）。
10. 前記回転速度センサはマイクロメカニカルセンサとして形成されている、請求項１から９のいずれか１項記載の移動作業機械（１）。
11. バックホーとして形成されている請求項１から１０のいずれか１項記載の移動作業機械（１）。
12. テレハンドラーとして形成されている請求項１から１０のいずれか１項記載の移動作業機械（１）。
13. ローダーバケット付きバックホーとして形成されている請求項１から１０のいずれか１項記載の移動作業機械（１）。
14. ホイールローダーとして形成されている請求項１から１０のいずれか１項記載の移動作業機械（１）。
15. 林業機械として形成されている請求項１から１０のいずれか１項記載の移動作業機械（１）。
16. ローダークレーンとして形成されている請求項１から１０のいずれか１項記載の移動作業機械（１）。
17. 前記計算ユニット（１７）は、次のステップ、すなわち、
    前記作業アーム（６）に作用する加速度
    

    

    を求めるステップと、
    前記作業アーム（６）が静止していると仮定して、前記作業アーム（６）に作用する加速度
    

    

    から、前記作業アーム（６）の、ｙ軸回りの傾きの角度として定義された傾斜角θの第１の値θ_sを計算するステップと、
    前記作業アーム（６）の、ｙ軸回りの角速度ω_yを測定するステップと、
    前記傾斜角θの第２の値θ_dを得るために前記角速度ω_yを期間ｔにわたって積分するステップと、
    推定アルゴリズムを使用して、前記第１の値θ_sと前記第２の値θ_dとから傾斜角θの推定値
    

    

    を得るステップとを実行する
    ことを特徴とする、請求項１記載の移動作業機械（１）。
18. 前記作業アーム（６）は関節により相互に連結された複数のセグメント（７，８）を有しており、前記各セグメント（７，８）について個々に傾斜角θ_iの推定値
    

    

    の算出が実行される、請求項１７記載の移動作業機械（１）。
19. 前記移動作業機械（１）の作業アーム（６）に取り付けられたツール（９）の位置および姿勢が
    

    

    から計算される、請求項１７または１８記載の移動作業機械（１）。
20. 前記移動作業機械（１）の作業アーム（６）に配設された少なくとも１つの油圧シリンダ（１１）の変位が
    

    

    から計算される、請求項１７または１８記載の移動作業機械（１）。
21. 傾斜角θを求めるために、前記作業アーム（６）のセグメント（７，８）の静止座標系から前記移動作業機械（１）の上部旋回体（３）の静止座標系への座標変換が行われる、請求項１７から２０のいずれか１項記載の移動作業機械（１）。

Images