| 【発明の名称】 |
移動農機 |
| 【発明者】 |
【氏名】田中 武二
【住所又は居所】島根県八束郡東出雲町大字揖屋町６６７番地１ 三菱農機株式会社内
|
| 【要約】 |
【課題】角速度センサの積算データを用いて作業機の傾斜制御を行う移動農機において、誤差の蓄積を抑制し、傾斜制御の精度を向上させる。
【解決手段】作業機３の左右傾斜を、傾斜センサ７及び角速度センサ８の検出値に基づいて自動的に制御するトラクタにおいて、角速度センサ８の検出値が変動状態のときは、角速度センサ８の検出値を積算して傾斜データとすると共に、角速度センサ８の検出値が安定状態のときは、傾斜センサ７の検出値を傾斜データとして作業機３の左右傾斜角を判断し、更に、角速度センサ８の検出値を傾斜データとする状態が所定時間以上継続したとき、角速度センサ８の積算データをクリアする。 |
【特許請求の範囲】
【請求項１】
作業機の左右傾斜を、傾斜センサ及び角速度センサの検出値に基づいて自動的に制御する移動農機において、
前記角速度センサの検出値が変動状態のときは、角速度センサの検出値を積算して傾斜データとすると共に、角速度センサの検出値が安定状態のときは、傾斜センサの検出値を傾斜データとして作業機の左右傾斜角を判断し、更に、角速度センサの検出値を傾斜データとする状態が所定時間以上継続したとき、角速度センサの積算データをクリアすることを特徴とする移動農機。
|
【発明の詳細な説明】【技術分野】
【０００１】
本発明は、作業機の左右傾斜を自動制御するトラクタなどの移動農機に関する。
【背景技術】
【０００２】
作業機の左右傾斜を自動制御する移動農機が知られている。この種の移動農機は、通常、作業機又は走行機体に傾斜センサを備え、該傾斜センサの検出値に基づいて作業機の左右傾斜角を判断し、作業機の左右傾斜制御を行うが、走行機体の横揺れ時、急発進時、クラッチ接続時などにおいては、作業機や走行機体の左右傾斜とは無関係に傾斜センサ値が変動するため、作業機の左右傾斜角を誤認し、制御精度が低下する可能性がある。
【０００３】
そこで、傾斜センサ及び角速度センサの検出値に基づいて作業機の左右傾斜角を判断することが提案される（特許文献１参照）。このような構成によれば、角速度センサの検出値が変動状態のときは、角速度センサの検出値を積算して傾斜データとし、角速度センサの検出値が安定状態のときは、傾斜センサの検出値を傾斜データとして作業機の左右傾斜角を判断することにより、傾斜制御の精度を高めることができる。
【特許文献１】特開平１１−２８９８０８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
ところで、角速度センサの積算データにも誤差は含まれており、この誤差は積算を繰り返すことにより蓄積される。通常、この蓄積誤差は、角速度センサの検出値が変動状態となったとき、すなわち、傾斜データを傾斜センサ値に切換えるときにクリアされるため、問題とはならないが、角速度センサの検出値がいつまでも安定しない状況が続いた場合には、大きな制御誤差を発生させる可能性がある。
【課題を解決するための手段】
【０００５】
本発明は、上記の如き実情に鑑みこれらの課題を解決することを目的として創作されたものであって、作業機の左右傾斜を、傾斜センサ及び角速度センサの検出値に基づいて自動的に制御する移動農機において、前記角速度センサの検出値が変動状態のときは、角速度センサの検出値を積算して傾斜データとすると共に、角速度センサの検出値が安定状態のときは、傾斜センサの検出値を傾斜データとして作業機の左右傾斜角を判断し、更に、角速度センサの検出値を傾斜データとする状態が所定時間以上継続したとき、角速度センサの積算データをクリアすることを特徴とする。
このように構成すれば、角速度センサの検出値がいつまでも安定しない状況が続いた場合、角速度センサの積算データがクリアされるので、誤差の蓄積を抑制し、傾斜制御の精度を向上させることができる。
【発明を実施するための最良の形態】
【０００６】
次に、本発明の実施形態について、図面に基づいて説明する。図面において、１はトラクタの走行機体であって、該走行機体１の後部には、昇降リンク機構２を介してロータリ等の作業機３が昇降自在に連結されている。昇降リンク機構２は、左右一対のリフトロッド４を介してリフトアーム５で吊持されており、リフトアーム５の油圧動作に応じて作業機３を昇降させると共に、左右何れかのリフトロッド４に介設されるリフトロッドシリンダ６の油圧動作に応じて作業機３を左右に傾斜させる。
【０００７】
走行機体１（又は作業機３）の所定位置には、走行機体１（又は作業機３）の左右傾斜角を検出する傾斜センサ７（静電容量方式等）と、左右傾斜変化時の角速度を検出する角速度センサ８（水晶音叉方式等）とが組み込まれている。即ち、従来から採用されていた傾斜センサ７の出力信号は、走行機体１の横揺れ時、急発進時、クラッチ接続時等において、作業機３や走行機体１の左右傾斜とは無関係に変動するだけでなく、傾斜変化に対する応答性能が低いため、角速度センサ８の検出値を加味して作業機３の左右傾斜角を判断する。具体的には、角速度センサ８の検出値が変動状態のときは、角速度センサ８の検出値を積算して傾斜データとすると共に、角速度センサ８の検出値が安定状態のときは、傾斜センサ７の検出値を傾斜データとして作業機３の左右傾斜角を判断し、作業機３の左右傾斜を自動制御することが可能になる。
【０００８】
走行機体１には、マイクロコンピュータ（ＣＰＵ、ＲＯＭ、ＲＡＭ等を含む）を用いて構成される制御部９が設けられている。制御部９には、傾斜制御に係る入力機器として、傾斜制御をＯＮ／ＯＦＦ操作する傾斜制御スイッチ１０と、傾斜制御の目標傾斜を設定する傾斜設定ボリューム１１と、設定画面操作を行うモニタ操作スイッチ１２と、リフトアーム５の位置を検出するリフトアームセンサ１３と、ポジションレバー１４の操作位置を検出するポジションセンサ１５と、リフトロッドシリンダ６のシリンダ長を検出するリフトロッドセンサ１６と、ステアリングハンドル１７の旋回操作を検出するステアリング切れ角検出スイッチ１８と、前述した傾斜センサ７及び角速度センサ８とが接続される一方、傾斜制御に係る出力機器としては、リフトロッド用電磁切換バルブ１９の伸長用ソレノイド１９ａと、縮小用ソレノイド１９ｂと、設定画面を表示するモニタ２０とが接続されている。
【０００９】
そして、制御部９では、傾斜設定データ及び傾斜判断データにもとづいて傾斜目標値（リフトロッド目標値）をセットし、該目標値に対するリフトロッドセンサデータの偏差に応じてリフトロッドシリンダ６を伸縮制御するが、本実施形態の制御部９は、このような傾斜制御を実行するにあたり、傾斜制御ルーチン（傾斜演算処理ルーチンを含む）の他に、傾斜制御の不感帯設定を行う傾き感度操作ルーチン、各種の処理ルーチンを含む角速度処理メインルーチン、タイマ割込みで実行される基準データ処理ルーチン、積算処理ルーチン、平滑処理ルーチン等を備えている。
【００１０】
基準データ処理ルーチンは、角速度センサ値の基準データを演算するための処理であり、ここで演算された基準データをもとに角速度センサ値の安定判断等が行われる。積算処理ルーチンは、角速度センサ値を積算（積分）して角度変化量（積算データ）を得るための処理であり、積算フラグのＯＮに応じて積算処理を実行する。平滑処理ルーチンは、直近の傾斜センサ値を所定周期で所定個数サンプリングし、その平均値（平滑データ）を演算するための処理であり、ここで演算された平滑データは、角度演算中の角度基準データとして使用される。
【００１１】
次に、角速度処理メインルーチンについて、図３及び図４を参照して説明する。図３に示すように、角速度処理メインルーチンには、初期設定ルーチン（Ｓ１０１）、接続判断ルーチン（Ｓ１０２）、平滑制御ルーチン（Ｓ１０３）、演算不感帯設定ルーチン（Ｓ１０４）、切替制御ルーチン（Ｓ１０５）、角度演算ルーチン（Ｓ１０６）等のサブルーチンが含まれている。
【００１２】
初期設定ルーチンは、各種フラグ、各種タイマ、各種データ等の初期設定を行う。接続判断ルーチンは、角速度センサ８が制御部９に正しく接続されているか否かを判断するための処理であり、例えば、角速度センサ８からの入力電圧が予め設定された範囲内であるか否かによって角速度センサ８の接続判断を行っている。平滑制御ルーチンは、前述した平滑処理ルーチンにおける傾斜センサ値のサンプリング周期を設定するための処理であり、例えば、作業開始直後などの如く、傾斜センサ値が変動し易いとき、少しでも正確な傾斜データが得られるように、傾斜センサ値のサンプリング周期を短くする。演算不感帯設定ルーチンは、角速度センサ値の演算に用いる不感帯を設定するための処理であり、例えば、車速に応じて演算不感帯データを変更する。ここで設定される演算不感帯データは、角速度センサ値の安定判断等で利用される。
【００１３】
角度演算ルーチンは、傾斜センサ値及び角速度センサ値にもとづいて走行機体１又は作業機３の傾斜角度を演算する処理であり、例えば、傾斜センサ値の平滑データからなる傾斜基準データと、角速度センサ値を積算してなる積算データとを加算して求められる。ただし、この演算処理では、両センサの出力レベルを合わせるために、所定の定数を用いて積算データの補正が行われることが好ましい。切替制御ルーチンは、傾斜制御で用いる傾斜判断データを、角速度センサ値の安定判断等に基づいて切換えるための処理である。以下、本実施形態の切替制御ルーチンについて、図４を参照して説明する。
【００１４】
図４に示すように、切替制御ルーチンでは、まず、ステアリング切れ角検出スイッチ１８の検出信号に基づく旋回判断（Ｓ２０１）と、ポジションセンサ１５の検出信号に基づくポジション上昇操作中判断（Ｓ２０２）と、リフトアームセンサ１３の検出信号に基づくリフトアーム上昇中判断（Ｓ２０３）とを行う。ここで、いずれかの判断がＹＥＳの場合は、角速度センサ値による傾斜制御の開始を遅延させるために開始遅延タイマ（図５のＴＡに相当）の設定を行う（Ｓ２０４）。次に、開始遅延タイマに基づいて開始遅延時間の未経過を判断し（Ｓ２０５）、この判断がＹＥＳの間は、開始遅延タイマのカウント（Ｓ２０６）と、積算フラグのリセット（Ｓ２０７）と、傾斜基準データに対する平滑データのセット（Ｓ２０８）と、積算データのクリア（Ｓ２０９）と、作業開始フラグのセット（Ｓ２１０）と、タイムアウトの設定（Ｓ２１１）とを行う。
【００１５】
一方、開始遅延時間が経過したら、作業開始フラグのリセット（Ｓ２１２）と、タイムアウト（角速度センサ値による傾斜制御の継続タイマ）のカウント（Ｓ２１３）を行うと共に、差分データを演算する（Ｓ２１４）。ここで演算する差分データは、前述した基準データに対する角速度センサ値の差分であり、具体的には、その絶対値から演算不感帯データを減算して求められる。次に、差分データにより演算判断を行う（Ｓ２１５）。この判断は、前述した角速度センサ値の安定判断に相当するものであり、角速度センサ値が不安定なときは、ＹＥＳと判断され、安定しているときは、ＮＯと判断される。
【００１６】
上記判断がＮＯの場合は、傾斜センサ値による傾斜制御を行うために、演算開始フラグのリセット（Ｓ２１６）と、積算フラグのリセット（Ｓ２１７）と、タイムアウトの設定（Ｓ２１８）と、傾斜基準データに対する平滑データのセット（Ｓ２１９）と、積算データのクリア（Ｓ２２０）を行う。ただし、角速度センサ値が安定状態になった直後は、傾斜センサ値の応答遅れ（図５のｔｌに相当）を考慮し、所定時間（図５のＴＢに相当）だけ角速度センサ値による傾斜制御を維持するようになっている。具体的には、角速度センサ値が安定したら、更新確認タイマ（ＴＢ）の経過を判断しつつ（Ｓ２２１）、更新確認タイマのカウント（Ｓ２２２）及び積算フラグのセット（Ｓ２２３）を行い、更新確認タイマが経過したら、Ｓ２１７〜Ｓ２２０を実行する。
【００１７】
一方、角速度センサ値が不安定な状態では、演算開始フラグのセット（Ｓ２２４）と、更新確認タイマのクリア（Ｓ２２５）と、積算フラグのセット（Ｓ２２６）を行い、角速度センサ値による傾斜制御を可能にするが、この状態では、常にタイムアウト時間の経過を判断する（Ｓ２２７）。そして、タイムアウト時間が経過した場合は、積算データの累積誤差を一旦解消するために、積算フラグのリセット（Ｓ２２８）と、傾斜基準データに対する平滑データのセット（Ｓ２２９）と、積算データのクリア（Ｓ２３０）を行う。つまり、角速度センサ値がいつまでも安定しない状況が続いた場合には、角速度センサ８の積算データを強制的にクリアすることにより、累積誤差による制御精度の低下を抑制するようにしてある。
【００１８】
次に、傾斜制御ルーチン（傾斜演算処理ルーチンを含む）及び傾き感度操作ルーチンについて、図６〜図８を参照して説明する。図６に示すように、傾斜制御ルーチンでは、まず、角速度センサ制御であるか否かを判断する（Ｓ３０１）。この判断は、例えば、前述した接続判断ルーチン（Ｓ１０２）において、角速度センサ８が正しく接続されていると判断した場合にＹＥＳとなる。そして、この判断がＮＯの場合は、傾斜データ（傾斜制御が傾斜判断に用いるデータ）に傾斜センサ値をセットすると共に（Ｓ３０２）、広い不感帯を設定し（Ｓ３０３）、傾斜センサ値による傾斜制御を実行する。
【００１９】
一方、角速度センサ８が正しく接続されている場合は、積算データを演算中であるか否かを判断する（Ｓ３０４）。この判断は、例えば、前述した積算フラグの状態を参照し、積算フラグがセットならばＹＥＳ（切替制御ルーチンでＳ２２４〜Ｓ２２６、又はＳ２２２〜Ｓ２２３が実行されている状態）、リセットならばＮＯ（切替制御ルーチンでＳ２０６〜Ｓ２１１、Ｓ２２８〜Ｓ２３０、又はＳ２１７〜Ｓ２２０が実行されている状態）と判断することができる。そして、この判断がＹＥＳの場合は、角速度センサ値からなる演算データ（平滑データ＋積算データ）を傾斜データにセットし（Ｓ３０５）、ＮＯの場合は、傾斜センサ値からなる演算データ（平滑データ）を傾斜データにセットする（Ｓ３０６）。また、それと共に不感帯の設定を行うが、角速度センサ値による傾斜制御の場合は狭い不感帯を設定し（Ｓ３０７）、傾斜センサ値による傾斜制御の場合は広い不感帯を設定した後（Ｓ３０８）、傾斜演算処理ルーチン（Ｓ３０９）を実行する。つまり、応答性能の高い角速度センサ８の検出値に基づいて傾斜制御する場合は、その性能を生かすために狭い不感帯を適用する。これにより、角速度センサ値による傾斜制御時の不感帯を適正化し、角速度センサ８の性能を生かした高精度な傾斜制御を行うことができる。
【００２０】
図７に示すように、傾斜演算処理ルーチンでは、まず、傾斜データをリフトロッド位置に変換し、リフトロッド目標値を設定する（Ｓ４０１）。次に、リフトロッド目標値に対するリフトロッドセンサ値の差分を求めると共に（Ｓ４０２）、その差分が縮み側であるか否かを判断する（Ｓ４０３）。そして、差分が縮み側で、かつ、差分が不感帯外の場合には（Ｓ４０４）、リフトロッドシリンダ６を縮小させ（Ｓ４０５）、差分が伸び側で、かつ、差分が不感帯外の場合には（Ｓ４０６）、リフトロッドシリンダ６を伸長させ（Ｓ４０７）、更に、差分が不感帯内の場合は、リフトロッドシリンダ６を停止させる（Ｓ４０８）。
【００２１】
図８に示すように、傾き感度操作ルーチンでは、まず、傾き感度処理であるか否かを判断する（Ｓ５０１）。この判断は、例えば、メニュー画面で傾き感度設定の項目が選択操作された場合にＹＥＳとなる。上記判断がＹＥＳになると、モニタ操作スイッチ１２の操作を判断する。例えば、データＳＷアップの場合は（Ｓ５０２）、感度データをアップ処理し（Ｓ５０３）、データＳＷダウンの場合は（Ｓ５０４）、感度データをダウン処理し（Ｓ５０５）、更に、決定ＳＷダウンの場合は（Ｓ５０６）、感度データを確定する（Ｓ５０７）。ただし、感度データは、予め決められた範囲（０〜１０）から逸脱しないように、データリミット処理が行われる（Ｓ５０８）。そして、これらの処理（Ｓ５０１〜Ｓ５０８）を、感度データをモニタ表示する表示処理ルーチン（Ｓ５０９）と共に実行することにより、モニタ画面での傾き感度設定が可能になる。
【００２２】
上記の傾き感度操作によれば、傾斜制御の不感帯を図９のように変更することができる。ここで、広いテーブル不感帯データは、前述した傾斜制御ルーチンのＳ３０３、Ｓ３０８で設定される不感帯データであり、狭いテーブル不感帯データは、Ｓ３０７で設定される不感帯データである。そして、角速度センサ値による傾斜制御時に適用される狭い不感帯は、傾斜センサ値による傾斜制御時に適用される広い不感帯よりも常に小さくなるように設定されると共に、傾き感度操作に応じて任意に変更することができる。これにより、角速度センサ値による傾斜制御時に適用される不感帯を設定により最適化し、更なる精度の向上を図ることが可能になる。
【００２３】
叙述の如く構成された本実施形態のトラクタは、作業機３の左右傾斜を、傾斜センサ７及び角速度センサ８の検出値に基づいて自動的に制御するにあたり、角速度センサ８の検出値が変動状態のときは、角速度センサ８の検出値を積算して傾斜データとすると共に、角速度センサ８の検出値が安定状態のときは、傾斜センサ７の検出値を傾斜データとして作業機３の左右傾斜角を判断し、更に、角速度センサ８の検出値を傾斜データとする状態が所定時間以上継続したとき、角速度センサ８の積算データをクリアするので、積算データの累積誤差を抑制し、傾斜制御の精度を向上させることができる。
【００２４】
また、角速度センサ８に基づく傾斜データを用いる場合は、傾斜センサ７の傾斜データを用いる場合よりも狭い不感帯を適用して作業機３の傾斜制御を行うので、角速度センサ８による傾斜制御時の不感帯を適正化し、角速度センサ８の性能を生かした高精度な傾斜制御を行うことができる。さらに、本実施形態では、上記の狭い不感帯を更に任意に変更できるようにしたので、不感帯を最適化して更なる精度の向上が図れる。
【図面の簡単な説明】
【００２５】
【図１】トラクタの側面図である。
【図２】制御部の入出力を示すブロック図である。
【図３】角速度処理メインルーチンのフローチャートである。
【図４】切替制御ルーチンのフローチャートである。
【図５】作用を示すタイミングチャートである。
【図６】傾斜制御ルーチンのフローチャートである。
【図７】傾斜演算処理ルーチンのフローチャートである。
【図８】傾き感度操作ルーチンのフローチャートである。
【図９】傾き感度操作ルーチンの作用説明図である。
【符号の説明】
【００２６】
１ 走行機体
３ 作業機
７ 傾斜センサ
８ 角速度センサ
９ 制御部
|
| 【出願人】 |
【識別番号】０００００１８７８
【氏名又は名称】三菱農機株式会社
【住所又は居所】島根県八束郡東出雲町大字揖屋町６６７番地１
|
| 【出願日】 |
平成１６年１２月１３日（２００４．１２．１３） |
| 【代理人】 |
【識別番号】１０００８５３９４
【弁理士】
【氏名又は名称】廣瀬 哲夫
|
| 【公開番号】 |
特開２００６−１６６７１３（Ｐ２００６−１６６７１３Ａ） |
| 【公開日】 |
平成１８年６月２９日（２００６．６．２９） |
| 【出願番号】 |
特願２００４−３５９５７１（Ｐ２００４−３５９５７１） |
|