JP3717411B2

JP3717411B2 - Sensor zero correction device for work equipment

Info

Publication number: JP3717411B2
Application number: JP2001038332A
Authority: JP
Inventors: 享梅本; 俊也福本; 富美男高田
Original assignee: Kubota Corp
Current assignee: Kubota Corp
Priority date: 2001-02-15
Filing date: 2001-02-15
Publication date: 2005-11-16
Anticipated expiration: 2021-02-15
Also published as: JP2002243449A; KR100453266B1; KR20020067642A

Description

【０００１】
【発明の属する技術分野】
本発明は、トラクタ、田植機、直藩機といった作業機のセンサ零点補正装置に係り、詳しくは、温度やその他の諸条件変化によってドリフトする角速度センサの零点を、作業機の作動中においても正確に求めることができる技術に関するものである。
【０００２】
【従来の技術】
トラクタ等の作業機における対地作業装置のローリング制御においては、作業装置もしくは機体の変位検出手段として傾斜センサと角速度センサとの双方のセンサを用いることにより、応答性が良く、誤作動も先ず無い正確で精度の良い制御作動を行えることが知られている。
【０００３】
即ち、特開平２−２１６４１２号公報にて示されたように、重錘と、この重錘の揺動量を検出するポテンショメータ等から成る傾斜センサ（低速度反応センサ）、及び光式ジャイロ等で成る角速度センサ（高速度反応センサ）の双方のセンサを用いてローリング制御装置を構成したものである。これにより、慣性の影響を受けず、応答性に優れる角速度センサと、検出時点での絶対傾斜角は検出できない角速度センサの欠点を補う傾斜センサとを組み合わせて、ダンパーやフィルターを設けること無く正確迅速にローリング制御が行え、対地作業精度の向上を図ることができる。
【０００４】
【発明が解決しようとする課題】
即ち、振動式等のジャイロセンサによる角速度出力ｄθｊ／ｄｔを積分することにより、センサ筐体の横揺れの影響を受けない傾斜角度変化θｊを得ることができる。但し、ジャイロセンサは角度変化しか検知できないので、絶対角度の検知には傾斜センサが必要である。
【０００５】
ところが、上記のように２種のセンサによるローリング制御においても、依然として解決すべき問題があった。それは、角速度センサは温度変化等によって零点（基準電圧等）が容易にドリフトすることであり、そうなると正確なローリング制御が行えなくなる。例えば、ジャイロセンサは角速度を基準電圧からの偏差として出力するが、基準電圧には固体差や温度変化があるため、前述の角速度出力ｄθｊ／ｄｔは常に大きな誤差を含んでおり、傾斜角度変化θｊを得るための積分処理によって誤差が蓄積し、実際の傾斜角θが時間と共にずれて行くのである（図３参照）。
【０００６】
基準電圧を固体差に対応させるには、機体静止時の電圧を平均して基準電圧に置き換える処理を行えば良く、加えてその処理を定期的に行えば温度変化にも対応させることができる。しかしながら、トラクタのような作業機は連続作業が多くて静止時間を確保できないので、前述のような処理が不可能であった。
【０００７】
本発明の目的は、トラクタのように常に作動している状態の作業機でも、温度変化等によってドリフトする角速度センサの零点補正を精度良く行えるようにして、角速度センサと傾斜センサとを用いた応答性及び精度の良いローリング制御を実質的に実現できるようにする点にある。
【０００８】
【課題を解決するための手段】
〔構成〕
請求項１の構成は、対地作業装置をローリング自在に走行機体に連結し、対地作業装置を走行機体に対してローリング駆動するアクチュエータと、走行機体又は対地作業装置の左右傾斜角度を検出する傾斜センサと、走行機体又は対地作業装置の左右傾斜方向の角速度を検出する角速度センサとを備えるとともに、傾斜センサと角速度センサとの双方の検出値に基づいて、対地作業装置の左右方向姿勢が設定角度に維持されるようにアクチュエータを作動させるローリング制御手段を設けてある作業機のセンサ零点補正装置において、
角速度センサによって検出されるサンプリング出力値の複数を記憶する記憶手段と、記憶された複数のサンプリング出力値に基づいて演算処理された値を零点とする零点制御手段と、走行機体の姿勢安定度の高い低いを判別する姿勢安定度判別手段とを備え、
走行機体の姿勢安定度が高いほど演算処理に用いるサンプリング出力値の数を少なく設定し、走行機体の姿勢安定度が低いほど演算処理に用いるサンプリング出力値の数を多く設定する補正手段を設けてあることを特徴とする。
【０００９】
前述したように、トラクタ等の作業機は基本的に不整地を走行するものであるから、静止時のサンプリング出力値を平均化して精度の良い零点を求めることは先ず不可能である。しかしながら、走行機体が常に左右にローリングしているとは言え、一方向に回転（即ち横転）するということは無く、常に水平姿勢に向けて復帰しつつ左右傾斜しているから、作業中であっても、圃場の起伏や凹凸の周期よりも十分に長い時間に亘ってサンプリングして平均すれば、左右のローリングによる出力値が相殺され、ほぼ正確な零点を求めることは可能である、ということが新たに知見された。但し、サンプリング時間を長くし過ぎると、零点変動に対応できなくなるので、徒にサンプリング数を多くすることは得策ではない。
【００１０】
以上のような考えに基づいて想起された請求項１の構成によれば、補正手段の機能により、左右傾斜が比較的穏やかでその傾斜角度自体も小さいといった具合に、走行機体の姿勢安定度が高いほど演算処理に用いるサンプリング出力値の数が少なく設定されるから、姿勢変化条件が緩いときには平均化による角速度センサの零点を迅速に求めることが可能になる。そして、左右傾斜が比較的顕著でその傾斜角度自体も大きいといった具合に、走行機体の姿勢安定度が低いほど演算処理に用いるサンプリング出力値の数が多く設定されるから、姿勢変化条件が厳しいときでも正確に零点を求めることができるようになる。
【００１１】
請求項２の構成は、請求項１の構成において、姿勢安定度判別手段が、走行機体の走行速度を検出する車速センサであり、補正手段は、走行速度が遅いほど演算処理に用いるサンプリング出力値の数を少なく設定し、走行速度が速いほど演算処理に用いるサンプリング出力値の数を多く設定するものに構成されていることを特徴とするものである。
【００１２】
請求項２の構成によれば、走行機体の姿勢安定度の高い低いを判別する姿勢安定度判別手段が車速センサであり、走行速度が遅いほど横揺れ等が小さい又は生じ難いという状態、即ち姿勢安定度が高いから、この場合には演算処理に用いるサンプリング出力値の数を少なくして、平均化による角速度センサの零点を迅速に求めることが可能になる。そして、走行速度が速いほど横揺れ等が大きい又は生じ易いという状態、即ち姿勢安定度が低いから、この場合には演算処理に用いるサンプリング出力値の数を多くして、平均化による角速度センサの零点を正確に求めることが可能になる。
【００１３】
請求項３の構成は、請求項１の構成において、姿勢安定度判別手段が角速度センサであり、補正手段は、角速度センサの出力値が小さいほど演算処理に用いるサンプリング出力値の数を少なく設定し、角速度センサの出力値が大きいほど演算処理に用いるサンプリング出力値の数を多く設定するものに構成されていることを特徴とするものである。
【００１４】
請求項３の構成によれば、姿勢安定度判別手段が角速度センサ自体であり、角速度センサの出力値が小さいほど横揺れ等が小さい又は生じ難いという状態、即ち姿勢安定度が高いから、この場合には演算処理に用いるサンプリング出力値の数を少なくして、平均化による角速度センサの零点を迅速に求めることが可能になる。そして、角速度センサの出力値が大きいほど横揺れ等が大きい又は生じ易いという状態、即ち姿勢安定度が低いから、この場合には演算処理に用いるサンプリング出力値の数を多くして、平均化による角速度センサの零点を正確に求めることが可能になる。
【００１５】
請求項４の構成は、請求項１の構成において、姿勢安定度判別手段が傾斜センサであり、補正手段は、傾斜センサの出力値が小さいほど演算処理に用いるサンプリング出力値の数を少なく設定し、傾斜センサの出力値が大きいほど演算処理に用いるサンプリング出力値の数を多く設定するものに構成されていることを特徴とする。
【００１６】
請求項４の構成によれば、姿勢安定度判別手段が傾斜センサであり、傾斜センサの出力値が小さいほど横揺れ等が小さい又は生じ難いという状態、即ち姿勢安定度が高いから、この場合には演算処理に用いるサンプリング出力値の数を少なくして、平均化による角速度センサの零点を迅速に求めることが可能になる。そして、傾斜センサの出力値が大きいほど横揺れ等が大きい又は生じ易いという状態、即ち姿勢安定度が低いから、この場合には演算処理に用いるサンプリング出力値の数を多くして、平均化による角速度センサの零点を正確に求めることが可能になる。
【００１７】
請求項５の構成は、請求項１の構成において、姿勢安定度判別手段が、操向輪の切れ角を検出する切れ角センサであり、補正手段は、操向輪の切れ角が小さいほど演算処理に用いるサンプリング出力値の数を少なく設定し、操向輪の切れ角が大きいほど演算処理に用いるサンプリング出力値の数を多く設定するものに構成されていることを特徴とするものである。
【００１８】
請求項５の構成によれば、姿勢安定度判別判別手段が、操向輪の切れ角を検出する切れ角センサであり、切れ角センサの出力値が小さいほど横揺れ等が小さい又は生じ難いという状態、即ち姿勢安定度が高いから、この場合には演算処理に用いるサンプリング出力値の数を少なくして、平均化による角速度センサの零点を迅速に求めることが可能になる。そして、切れ角センサの出力値が大きいほど横揺れ等が大きい又は生じ易いという状態、即ち姿勢安定度が低いから、この場合には演算処理に用いるサンプリング出力値の数を多くして、平均化による角速度センサの零点を正確に求めることが可能になる。
【００２１】
請求項６の構成は、対地作業装置をローリング自在に走行機体に連結し、対地作業装置を走行機体に対してローリング駆動するアクチュエータと、対地作業装置の左右傾斜角度を検出する傾斜センサと、対地作業装置の左右傾斜方向の角速度を検出する角速度センサとを備えるとともに、傾斜センサと角速度センサとの双方の検出値に基づいて、対地作業装置の左右方向姿勢が設定角度に維持されるようにアクチュエータを作動させるローリング制御手段を設けてある作業機のセンサ零点補正装置であって、
角速度センサによって検出されるサンプリング出力値の複数を記憶する記憶手段と、記憶された複数のサンプリング出力値に基づいて演算処理された平均値を零点とする零点制御手段と、走行機体の状態安定度の高い低いを判別する状態安定度判別手段とを備え、
走行機体の状態安定度が高いほど演算に用いるサンプリング出力値の数を多く設定し、走行機体の状態安定度が低いほど演算に用いるサンプリング出力値の数を少なく設定する補正手段を設けてあることを特徴とする。
【００２２】
請求項６の構成によれば、走行機体の状態安定度が高いほど演算に用いるサンプリング出力値の数を多く設定し、走行機体の状態安定度が低いほど演算に用いるサンプリング出力値の数を少なく設定するものであるから、零点ドリフトが安定しているときには、多くのサンプリング出力値によって精度良く零点を求めることができるとともに、零点ドリフト変化が大きいときには、サンプリング出力値を少なくすることで変化に追従できるようになる。
【００２３】
請求項７の構成は、請求項６の構成において、状態安定度判別手段が、角速度センサの温度を検出する温度センサであり、角速度センサの温度変化が小さいほど演算に用いるサンプリング出力値の数を多く設定し、角速度センサの温度変化が大きいほど演算に用いるサンプリング出力値の数を少なく設定する補正手段を設けてあることを特徴とするものである。
【００２４】
請求項７の構成によれば、角速度センサの温度を検出する温度センサで状態安定度判別手段とするものである。作業機ではエンジン、ミッションケース等の温度変化を伴う部分の横傍等に角速度センサを配置することが多く、連続作業中ではエンジンやミッションケースは一定の温度に維持される安定状態を示し、エンジン始動直後や、作業負荷に軽重があるときには温度が変化する不安定状態を示すから、角速度センサの温度変化の有無を検出する温度センサに基づいて補正手段を実行させることにより、請求項６の構成による前記作用を得ることができる。
【００２５】
請求項８の構成は、請求項６の構成において、状態安定度判別手段が、メインスイッチがＯＮされたときからの経過時間を検出する時間計測手段であり、メインスイッチＯＮからの時間が長いほど演算に用いるサンプリング出力値の数を多く設定し、メインスイッチＯＮからの時間が短いほど演算に用いるサンプリング出力値の数を少なく設定する補正手段を設けてあることを特徴とするものである。
【００２６】
請求項８の構成によれば、メインスイッチがＯＮされたときからの経過時間を検出する時間計測手段で判別手段とするものである。即ち、エンジン始動直後から暫くの間はエンジン（ミッションケース）温度が上昇し続ける不安定状態であり、或る程度時間が経過すればエンジン温度が所定の値に落ち着く安定状態となるから、メインスイッチＯＮからの時間が長いほど演算に用いるサンプリング出力値の数を多くし、メインスイッチＯＮからの時間が短いほど演算に用いるサンプリング出力値の数を少なくすることにより、請求項６の構成による前記作用を得ることができる。
【００２７】
請求項９の構成は、対地作業装置をローリング自在に走行機体に連結し、前記対地作業装置を前記走行機体に対してローリング駆動するアクチュエータと、前記走行機体又は前記対地作業装置の左右傾斜角度を検出する傾斜センサと、前記走行機体又は前記対地作業装置の左右傾斜方向の角速度を検出する角速度センサとを備えるとともに、前記傾斜センサと前記角速度センサとの双方の検出値に基づいて、前記対地作業装置の左右方向姿勢が設定角度に維持されるように前記アクチュエータを作動させるローリング制御手段を設けてある作業機のセンサ零点補正装置であって、前記角速度センサによって検出されるサンプリング出力値の複数を記憶する記憶手段と、
記憶された複数のサンプリング出力値に基づいて演算処理された平均値を零点とする制御手段と、前記角速度センサによって検出されるサンプリング出力値の複数にローパスフィルタ処理を行って零点とする制御手段とを併備した零点制御手段と、
前記走行機体の姿勢安定度の高い低いを判別する姿勢安定度判別手段とを備え、
前記走行機体の姿勢安定度が高いほど演算処理に用いるサンプリング出力値の数を少なく設定し、前記走行機体の姿勢安定度が低いほど演算処理に用いるサンプリング出力値の数を多く設定する補正手段を設けてある。
【００２８】
【発明の実施の形態】
以下、本発明の実施の形態を図面に基づいて説明する。
図１は作業機の一例である農用トラクタの後部を示しており、ミッションケース３に、上下揺動自在なトップリンク１と左右一対のロアリンク２を介して、走行機体２１に対してローリング自在にロータリ耕耘装置（対地作業装置の一例）４を連結してある。ミッションケース３の上部に、油圧シリンダ５により上下に揺動駆動される一対のリフトアーム６が備えられ、一対のリフトアーム６とロアリンク２とがリフトロッド７、及び複動型の油圧シリンダ８を介して連結されている。１９は左右一対の駆動後輪である。
【００２９】
図２に示すように、油圧シリンダ５に対する３位置切換式の制御弁１６が制御装置２２により操作されて、油圧シリンダ５及びリフトアーム６によりロータリ耕耘装置４が昇降駆動される。油圧シリンダ８に対する３位置切換式の制御弁１７が制御装置２２により操作されて、油圧シリンダ８の伸縮作動によりロータリ耕耘装置４が、油圧シリンダ８とは反対側のロアリンク２との連結点周りにローリング駆動される。
【００３０】
この農用トラクタは、ロータリ耕耘装置４を地面から設定高さに維持し耕耘深さを設定値に維持する昇降制御手段２９、走行機体２１に対するロータリ耕耘装置４の高さを設定位置に維持するポジション制御手段３０、並びに、水平面に対するロータリ耕耘装置４の左右方向の傾斜角度を設定角度に維持するローリング制御機能が、制御装置２２に備えられている。
【００３１】
図２及び図１に示すように、ロータリ耕耘装置４に上下揺動自在に後部カバー９が備えられ、バネ１８により後部カバー９が下方側に付勢されて、ロータリ耕耘装置４に対する後部カバー９の上下揺動角度を検出する耕深センサ１０が備えられており、耕深センサ１０の検出値が制御装置２２に入力されている。これにより昇降制御手段２９によって、耕深センサ１０の検出値が、走行機体２１に設けられたダイヤル操作式でポテンショメータ型式の耕深設定器１１の設定耕耘深さとなるように、制御弁１６が操作されて、油圧シリンダ５によりロータリ耕耘装置４が自動的に昇降駆動される。
【００３２】
図２及び図１に示すように、走行機体２１に対するリフトアーム６の上下角度を検出する角度センサ１３が、リフトアーム６の基部に備えられており、角度センサ１３の検出値が制御装置２２に入力されている。これによりポジション制御手段３０によって、角度センサ１３の検出値が走行機体２１に設けられたレバー操作式のポジション設定器１２の目標値となるように、制御弁１６が操作されて、油圧シリンダ５によりリフトアーム６が上下に揺動駆動される。
【００３３】
前述の昇降制御手段２９及びポジション制御手段３０において、耕深設定器１１の設定耕耘深さに対応する角度センサ１３の検出値と、ポジション設定器１２の目標値とが比較されて、ポジション設定器１２の目標値の方が高い場合、昇降制御手段２９及び後述するローリング制御機能が停止して（油圧シリンダ８が停止した状態）、ポジション制御手段３０が作動する。これにより、ポジション設定器１２の目標値に角度センサ１３の検出値が一致するように、制御弁１６が操作されて、油圧シリンダ５によりロータリ耕耘装置４が昇降駆動される。従って、ポジション設定器１２を操作することにより、耕深設定器１１の設定耕耘深さに対応する角度センサ１３の検出値よりも高い範囲で、ロータリ耕耘装置４を走行機体２１に対して任意の高さに昇降駆動し停止させることができる。
【００３４】
次にポジション設定器１２を下降側に操作して、ポジション設定器１２の目標値が、耕深設定器１１の設定耕耘深さに対応する角度センサ１３の検出値に一致すると（又は低くなると）、ポジション制御手段３０が停止し、昇降制御手段２９及びローリング制御機能が作動する。これにより、昇降制御手段によって耕深センサ１０の検出値が耕深設定器１１の設定耕耘深さとなるように、制御弁１６が操作されて、油圧シリンダ５によりロータリ耕耘装置４が自動的に昇降駆動される。後述するようにローリング制御機能によって、水平面に対して左右方向に傾斜（又は水平面に平行）した設定角度に、水平面に対するロータリ耕耘装置４の左（右方向の傾斜角度が維持されるように、制御弁１７が操作されて、油圧シリンダ８によりロータリ耕耘装置４がローリング駆動される。
【００３５】
図１、図２に示すように、この農用トラクタでは、水平面に対して左右方向に傾斜（又は水平面に平行）した設定角度に、水平面に対するロータリ耕耘装置４の左右方向の傾斜角度が維持されるように、ロータリ耕耘装置４をローリング駆動するローリング制御手段２４を備えてある。ロータリ耕耘装置４の左右方向の設定角度を設定するダイヤル式の傾斜設定器２０を備えてあり、これは水平位置から右下り側及び左下り側に、任意に且つ連続的に設定角度を設定及び変更することができるように構成されている。
【００３６】
即ち、走行機体２１の左右傾斜角度を検出する重錘式の傾斜センサ１５と、走行機体２１の左右傾斜方向の角速度を検出する振動ジャイロ式の角速度センサ２３と、油圧シリンダ８の作動位置を検出するストロークセンサ１４とを備えてあり、ローリングシリンダ８の作動位置によって機体２１に対するロータリ耕耘装置４の左右傾斜角度が検出できるので、傾斜センサ１５と角速度センサ２３との双方の検出値に基づいて、ロータリ耕耘装置４の左右方向姿勢が傾斜設定器２０による設定角度に維持されるように油圧シリンダ８を作動させるローリング制御手段２４を制御装置２２に設けてある。
【００３７】
そして、温度等の諸条件によってドリフトする角速度センサ２３の基準電圧を時間経過に伴って更新して補正するセンサ零点補正装置Ｚを設けてある。即ち、角速度センサ２３によって検出されるサンプリング出力値の複数を記憶する記憶手段２５と、記憶された複数のサンプリング出力値に基づいて演算処理された平均値を基準電圧（零点）とする零点制御手段２６と、走行機体２１の姿勢安定度の高い低いを判別する姿勢安定度判別手段Ｈとを備え、走行機体２１の姿勢安定度が高いほど演算処理に用いるサンプリング出力値の数を少なく設定し、走行機体２１の姿勢安定度が低いほど演算処理に用いるサンプリング出力値の数を多く設定する補正手段２８を設けてセンサ零点補正装置Ｚが構成されている。
【００３８】
図２に示すように、後輪１９（図１参照）と前輪３１への伝動軸３２の回転数を検出して、走行機体２１の走行速度を検出する車速センサ２７を備えてあり、この車速センサ２７と、これの検出情報を処理する判別回路４１によって姿勢安定度判別手段Ｈが構成されている。即ち、走行速度が速いほど走行機体２１の姿勢安定度が低く、走行速度が遅いほど走行機体２１の姿勢安定度が高いと判断されるようになっている。
【００３９】
図３に、傾斜センサ１５と角速度センサ２３を利用した傾斜角度検出系のブロック図が示されている。図３において、第１誤差補償Ｇ１は、前記零点制御手段２６を構成するものであり、平均間隔内で発生するノイズを完全に除去するべく一定間隔平均処理と、適応ＬＰＦ（ローパスフィルタ）処理とを行う。
【００４０】
ここで、一定間隔平均処理は、１０００Ｈｚでサンプリングした所定時間分の所定個数のデータを足し算し、データ列ａとする。そして、単位時間ごとにデータ列ａの平均を計算し、データ列ｂとする。
【００４１】
適応ＬＰＦ（ローパスフィルタ）処理は、上記平均処理の度に以下の処理で基準電圧を更新し、
新基準電圧＝（１秒間の平均値×α＋旧基準電圧×β）÷（α＋β）
十分に平滑化した基準電圧を零点として出力する。但し、α、βは条件によって可変にするものであり、例えば、メインキーＯＮ直後や温度上昇時等、基準電圧が変動し易いときはαを大きくして変動への追従性を重視し、変動が安定する条件ではαを小さくして計算の正確さを重視する。又、旋回中や大きな傾斜変化時には処理を中断し、誤った基準電圧を計算しないようにする。
【００４２】
第１誤差補償Ｇ１で全ての誤差を除去することはできないので、さらにセンサの直線性にも誤差があり、積分処理による誤差の蓄積をなくすことはできない。そこで、一旦蓄積された誤差を取り除くため、傾斜設定器２０で設定された目標傾斜角θに傾斜センサ１５による検出傾斜角θｒとの偏差をフィードバックする第２誤差補償Ｇ２を行う。ここで、フィードバック係数Ｋ２を十分に小さく設定することができれば、機体停止時にθ＝θｒとなって傾斜センサ１５と同等の絶対精度が確保され、傾斜変化時には横揺れの影響を受けない応答性の良い傾斜角度を出力できる。逆にＫ２が大きいと、θｒの補償が効き過ぎて傾斜センサ１５の出力値と変わらなくなってしまう。
【００４３】
ここでフィードバック係数Ｋ２は蓄積される誤差に応じて設定する必要があるので、精度の良いジャイロセンサを用いるほど、第１誤差補償Ｇ１を工夫するほど小さくできる。前述の処理では、0.5%程度で誤差を除去することができ、必要性能を満たすに十分小さい値である。しかし、温度変化の激しい条件や、より廉価なジャイロセンサを用いる場合、さらに大きくする必要があり、それによる性能劣化がどの程度になるか評価する必要がある。
【００４４】
又、フィードバック係数Ｋ２を可変にすることも望ましい。例えば、停止中や傾斜変化の少ないときはＫ２を大きくして目標傾斜角θの誤差を速やかに除去し、逆に作業中や傾斜変化の激しいときはフィードバック係数Ｋ２を小さくして横揺れの影響を抑えて応答性を上げる。さらに、始動直後や温度変化の大きい等ジャイロセンサの誤差が大きくなる条件ではフィードバック係数Ｋ２を大きくして誤差の除去を重視し、温度が安定したときにはフィードバック係数Ｋ２を小さくして応答性を重視する。
【００４５】
参考として、図１０〜図１２に、トラクタの傾斜が変化するときの各センサ１５，２３の検出作動テスト結果を示す。
【００４６】
図１０によると、走行機体の傾斜変化に対して、角速度センサ２３の演算による出力は殆ど時間遅れなく変化しているのに対して、傾斜センサ１５は、 0.5 秒ほどの時間遅れを伴って変化しており、かつ、傾斜が正の方向に変化を開始する時、および負の方向に変化を開始するときには慣性によって一時的に逆方向の動きとして検出していることが理解できる。尚、図１０〜図１１において、ｖは走行機体２１の左右傾斜角、ｙは角速度センサの演算による出力、ｚは傾斜センサ出力である。
【００４７】
図１１は、図１０の場合よりも傾斜変化が速い例であり、 0.5 秒間で約２度変化する時の傾斜センサ１５と角速度センサ２３の演算による出力の変化特性を示したものである。これによると、傾斜センサ１５は、下降並びに上昇開始時に約１度逆方向に出力してしまっているとともに、昇降停止時にもオーバーシュートしている。これに対して角速度センサ２３の演算による出力は、やはり走行機体の傾斜変化に追従していることが分かる。このように、傾斜センサの出力を基にするよりも角速度センサの出力を基に制御するほうが応答性でも精度でも優れていることは明らかである。
【００４８】
〔別実施形態〕
センサ零点補正装置Ｚは、以下《１》〜《７》に記載した構成のものでも良い。
【００４９】
《１》図４に示すように、姿勢安定度判別手段Ｈが角速度センサ２３であり、補正手段２８を、角速度センサ２３の出力値が小さいほど角速度センサ２３によるサンプリング出力値の数を少なく設定し、角速度センサ２３の出力値が大きいほどサンプリング出力値の数を多く設定するものに構成する。
【００５０】
《２》図５に示すように、姿勢安定度判別手段Ｈが傾斜センサ１５であり、補正手段２８は、傾斜センサ１５の出力値が小さいほど角速度センサ２３によるサンプリング出力値の数を少なく設定し、傾斜センサ１５の出力値が大きいほどサンプリング出力値の数を多く設定するものに構成されている。
【００５１】
《３》図６に示すように、姿勢安定度判別手段Ｈが、操向前輪３１の切れ角を検出するポテンショメータ式の切れ角センサ３３であり、補正手段２８は、操向前輪３１の切れ角が小さいほど角速度センサ２３によるサンプリング出力値の数を少なく設定し、操向前輪３１の切れ角が大きいほどサンプリング出力値の数を多く設定するものに構成する。
【００５２】
《４》図７に示すように、角速度センサ２３によって検出されるサンプリング出力値の複数にローパスフィルタ（ＬＰＦ）３４により上記したローパスフィルタ処理を行って零点とする零点制御手段３５と、走行機体２１の状態安定度の高い低いを判別する状態安定度判別手段Ｊとを備え、走行機体２１の状態安定度が高いほどローパスフィルタ３４のカットオフ周波数を低く設定し、走行機体２１の状態安定度が低いほどカットオフ周波数を高く設定する補正手段３６を設けてある。
【００５３】
《５》図８、図９に示すように、角速度センサ２３によって検出されるサンプリング出力値の複数を記憶する記憶手段２５と、記憶された複数のサンプリング出力値に基づいて演算処理された平均値を零点とする零点制御手段２６と、走行機体２１の状態安定度の高い低いを判別する状態安定度判別手段Ｊとを備え、走行機体２１の状態安定度が高いほどサンプリング出力値の数を多く設定し、走行機体２１の状態安定度が低いほどサンプリング出力値の数を少なく設定する補正手段３７を設けてある。
【００５４】
《６》図８に示すように、状態安定度判別手段Ｊが、角速度センサ２３の温度を検出する温度センサ３８であり、角速度センサ２３の温度変化が小さいほど角速度センサ２３によるサンプリング出力値の数を多く設定し、角速度センサ２３の温度変化が大きいほどサンプリング出力値の数を少なく設定する補正手段３７を設けてある。
【００５５】
角速度センサ２３はミッションケース３から距離をおいて配置されているが、ミッションケース３の温度変化による雰囲気温度の影響を受ける。ミッションケース３の温度はエンジン温度に左右されるから、エンジン始動直後とエンジンが十分に暖気運転された後とでは、角速度センサ２３の温度が明確に異なっており、それによって零点がドリフトする。又、エンジン以外にも、季節、天候、地域、地理等の種々の条件により、角速度センサ２３の雰囲気温度（走行機体２１の状態安定度の一例）が異なるので、その変化に伴ってサンプリング出力値の数を変更する手段である。
【００５６】
《７》図９に示すように、状態安定度判別手段Ｊが、メインスイッチ３９がＯＮされたときからの経過時間を検出するタイマー（時間計測手段の一例）４０であり、メインスイッチ３９がＯＮされてからの時間が長いほど角速度センサ２３によるサンプリング出力値の数を多く設定し、メインスイッチ３９がＯＮされてからの時間が短いほどサンプリング出力値の数を少なく設定する補正手段３７を設けてある。
【００５７】
前述したように、エンジン温度、即ち角速度センサ２３の雰囲気温度は、エンジン始動直後と暖気後で全く異なるということは、換言すれば、メインキー３９のＯＮ操作（即ちエンジン始動時）からの経過時間異に伴って変化するものである。つまり、メインキー３９のＯＮ操作後の経過時間が短いほど角速度センサ２３の雰囲気温度は低く、経過時間が長いほど角速度センサ２３の雰囲気温度は高くなるから、前述の《６》の場合と同様に、メインキー３９のＯＮ操作後の経過時間の長短に伴ってサンプリング出力値の数を変更する手段である。
【００５８】
《その他》対地作業装置４又は走行機体２１の姿勢安定度の高い低いを判別する姿勢安定度判別手段Ｈの他の例としては、対地作業装置４又は機体２１の前後傾斜を検出するピッチングセンサや、そのピッチングに関する角速度センサ、対地作業装置４又は機体２１の上下方向に関する角速度センサ、アクセルレバー又はペダルの操作量、変速段数（位置）の高低等、種々の変更が可能である。
【００５９】
対地作業装置４又は走行機体２１の状態安定度の高い低いを判別する状態安定度判別手段Ｊの他の例としては、温度計、湿度計、高度計等、種々の変更が可能である。又、本実施形態において、対地作業装置４側に、傾斜センサ１５と角速度センサ２３とを配置するようにしても良い。対地作業装置４としては、耕耘装置の他、苗植付装置、直播装置、畦作成装置、整地装置等、種々の変更が可能である。
【００６０】
【発明の効果】
請求項１に記載の作業機のセンサ零点補正装置では、走行機体の姿勢安定度が高いほど角速度センサによる演算処理用のサンプリング出力値の数を少なく設定し、走行機体の姿勢安定度が低いほど演算処理用のサンプリング出力値の数を多く設定する補正手段を設ける工夫により、零点の補正が小さくて済む姿勢安定度の高い状況では、零点補正が迅速に行われて追従性に優れるローリング制御が実現でき、零点の補正が大きくなる姿勢安定度の低い状況でも、正確な零点補正が可能であって精度に優れるローリング制御が実現できるようになった。
【００６１】
請求項２に記載の作業機のセンサ零点補正装置では、走行速度を検出する車速センサで姿勢安定度の判別を行うことにより、走行速度が遅いときには追従性に優れるローリング制御が実現でき、走行速度が速いときでも精度に優れるローリング制御が実現できるようになった。
【００６２】
請求項３に記載の作業機のセンサ零点補正装置では、対地作業装置又は走行機体のローリング角速度を検出する角速度センサで姿勢安定度の判別を行うことにより、補正のための専用のセンサが不要で経済的なものとしながら、ローリング動の遅いときには追従性に優れるローリング制御が実現でき、ローリング動の速いときでも精度に優れるローリング制御が実現できる利点がある。
【００６３】
請求項４に記載の作業機のセンサ零点補正装置では、対地作業装置又は走行機体の絶対ローリング角を検出する傾斜センサで姿勢安定度の判別を行うことにより、補正のための専用のセンサが不要で経済的なものとしながら、絶対ローリング量の少ないときには追従性に優れるローリング制御が実現でき、絶対ローリング量の大きいときでも精度に優れるローリング制御が実現できる利点がある。
【００６４】
請求項５に記載の作業機のセンサ零点補正装置では、操向輪の切れ角を検出する切れ角センサ姿勢安定度の判別を行うことにより、切れ角が小さいときには追従性に優れるローリング制御が実現でき、切れ角が大きいときでも精度に優れるローリング制御が実現できるようになった。
【００６５】
請求項６に記載の作業機のセンサ零点補正装置では、走行機体の状態安定度が高いほどローパスフィルタのカットオフ周波数を低く設定して、角速度センサの零点を迅速に求めることができ、走行機体の状態安定度が低いほどカットオフ周波数を高く設定して、角速度センサの零点を正確に求めることが可能なローリング制御を実現することができた。
【００６６】
請求項７に記載の作業機のセンサ零点補正装置では、走行機体の状態安定度が高いほど演算処理用のサンプリング出力値の数を多く設定し、走行機体の状態安定度が低いほど演算処理用のサンプリング出力値の数を少なく設定することにより、零点ドリフトが安定しているときには精度に優れた零点を求めることができ、零点ドリフト変化が大きいときにはそのドリフト変化への追従性良く零点補正が行えるようになった。
【００６７】
請求項８に記載の作業機のセンサ零点補正装置では、角速度センサの温度変化の有無を検出する温度センサに基づく補正手段により、請求項７の構成による前記効果を奏することができる利点がある。
【００６８】
請求項９に記載の作業機のセンサ零点補正装置では、メインスイッチＯＮからの時間の長短に基づく補正手段により、請求項７の構成による前記効果を奏することができる利点がある。
【図面の簡単な説明】
【図１】農用トラクタ後部の構造を示す斜視図
【図２】ローリング制御の概略構造を示す機能系統図
【図３】補正制御手段の概念を示すブロック図
【図４】角速度センサに基づいて補正するローリング制御の機能系統図
【図５】傾斜センサに基づいて補正するローリング制御の機能系統図
【図６】切れ角センサに基づいて補正するローリング制御の機能系統図
【図７】ローパスフィルタ処理に基づいて補正するローリング制御の機能系統図
【図８】角速度センサの温度に基づいて補正するローリング制御の機能系統図
【図９】メインキーＯＮ後の経過時間に基づいて補正するローリング制御の機能系統図
【図１０】傾斜センサと角速度センサの出力特性グラフを示す図
【図１１】傾斜センサと角速度センサの出力特性グラフを示す図
【符号の説明】
４対地作業装置
８アクチュエータ
１５傾斜センサ
２１走行機体
２３角速度センサ
２４ローリング制御手段
２５記憶手段
２６零点制御手段
２７車速センサ
２８補正手段
３１操向輪
３３切れ角センサ
３５制御手段
３６補正手段
３７補正手段
３８温度センサ
３９メインスイッチ
４０時間計測手段
Ｈ姿勢安定度判別手段
Ｊ状態安定度判別手段 [0001]
BACKGROUND OF THE INVENTION
The present invention relates to a sensor zero point correction device for a work machine such as a tractor, a rice transplanter, or a straight-line machine. Specifically, the zero point of an angular velocity sensor that drifts due to changes in temperature and other conditions can be accurately detected even during operation of the work machine. It is related to the technology that can be requested.
[0002]
[Prior art]
In rolling control of ground work equipment on work machines such as tractors, work equipmentOr the aircraftIt is known that by using both an inclination sensor and an angular velocity sensor as the displacement detection means, it is possible to perform an accurate and accurate control operation with good responsiveness and no malfunction.
[0003]
That is, as disclosed in Japanese Patent Laid-Open No. 216164/1990, it is composed of a weight, a tilt sensor (low-speed reaction sensor) composed of a potentiometer for detecting the swing amount of the weight, an optical gyro, and the like. A rolling control device is configured by using both sensors of an angular velocity sensor (high-speed reaction sensor). As a result, an angular velocity sensor that is not affected by inertia and has excellent responsiveness, and an inclination sensor that compensates for the drawbacks of angular velocity sensors that cannot detect the absolute inclination angle at the time of detection, can be accurately and quickly installed without a damper or filter. In addition, the rolling control can be performed, and the ground work accuracy can be improved.
[0004]
[Problems to be solved by the invention]
That is, by integrating the angular velocity output dθj / dt by a vibration type gyro sensor or the like, it is possible to obtain a tilt angle change θj that is not affected by the roll of the sensor housing. However, since the gyro sensor can only detect a change in angle, an inclination sensor is required to detect the absolute angle.
[0005]
However, there is still a problem to be solved in the rolling control by the two types of sensors as described above. That is, the angular velocity sensor easily drifts at a zero point (reference voltage or the like) due to a temperature change or the like, and accurate rolling control cannot be performed. For example, the gyro sensor outputs the angular velocity as a deviation from the reference voltage. However, since the reference voltage includes individual differences and temperature changes, the angular velocity output dθj / dt described above always includes a large error, and the inclination angle change θj. The error is accumulated by the integration processing for obtaining the actual inclination angle, and the actual inclination angle θ shifts with time (see FIG. 3).
[0006]
In order to make the reference voltage correspond to the individual difference, a process of averaging the voltage when the aircraft is stationary and replacing it with the reference voltage may be performed. In addition, if the process is periodically performed, it is possible to cope with a temperature change. However, a work machine such as a tractor has many continuous operations and cannot secure a stationary time, and thus the processing as described above is impossible.
[0007]
The object of the present invention is to make it possible to accurately correct the zero point of an angular velocity sensor that drifts due to a temperature change or the like even in a working machine that is always in operation like a tractor, and to respond using an angular velocity sensor and an inclination sensor. It is in the point which makes it possible to substantially realize rolling control with good performance and accuracy.
[0008]
[Means for Solving the Problems]
〔Constitution〕
According to the first aspect of the present invention, the ground work device is connected to the traveling machine body so as to be able to roll freely, the actuator for rolling the ground work device with respect to the traveling machine body, and the inclination sensor for detecting the right and left inclination angle of the traveling machine body or the ground work device And an angular velocity sensor for detecting the angular velocity in the left-right tilt direction of the traveling machine body or the ground work device, and based on the detection values of both the tilt sensor and the angular speed sensor, the horizontal posture of the ground work device is set to the set angle. In the sensor zero correction device for a working machine provided with rolling control means for operating the actuator to be maintained,
Storage means for storing a plurality of sampling output values detected by the angular velocity sensor, and based on the stored sampling output valuesCalculated valueThe zero point control means with zero as the zero point and the high and low attitude stability of the traveling aircraftPosture stability determination meansAnd
The higher the attitude stability of the aircraftUsed for arithmetic processingSet a smaller number of sampling output values, the lower the attitude stability of the vehicleUsed for arithmetic processingCorrection means for setting a large number of sampling output values is provided.
[0009]
As described above, since work machines such as tractors basically travel on rough terrain, it is first impossible to average the sampling output values when stationary and obtain a precise zero point. However, although the traveling aircraft always rolls to the left and right, it does not rotate in one direction (that is, rolls over), and it always tilts back and forth while returning to a horizontal posture. However, if sampling and averaging over a time sufficiently longer than the field undulations and irregularities cycle, the output values from the left and right rolling are offset, and it is possible to obtain an almost accurate zero Was newly discovered. However, if the sampling time is too long, it becomes impossible to cope with zero point fluctuations, so it is not a good idea to increase the number of samplings.
[0010]
According to the configuration of claim 1, which has been recalled based on the above-described idea, the posture stability of the traveling machine body is improved by the function of the correction means such that the right-and-left inclination is relatively gentle and the inclination angle itself is small. The higher theUsed for arithmetic processingSince the number of sampling output values is set to be small, it is possible to quickly obtain the zero point of the angular velocity sensor by averaging when the posture change condition is loose. And the lower the attitude stability of the traveling aircraft, such as the relatively large left-right inclination and the large inclination angle itself,Used for arithmetic processingSince a large number of sampling output values are set, the zero point can be accurately obtained even when the posture change condition is severe.
[0011]
The structure of claim 2 is the structure of claim 1,Posture stability determination meansIs a vehicle speed sensor that detects the traveling speed of the traveling vehicle body, and the correction means increases the slower the traveling speed.Sampling output value used for arithmetic processingSet the number ofSampling used for arithmetic processingIt is characterized by being configured to set a large number of output values.
[0012]
According to the configuration of claim 2, it is determined whether the traveling stability of the traveling vehicle body is high or low.Posture stability determination meansIs a vehicle speed sensor, and the lower the running speed, the smaller or less likely to roll, that is, the posture stability is high.Used for arithmetic processingBy reducing the number of sampling output values, the zero point of the angular velocity sensor by averaging can be quickly obtained. And the higher the running speed, the greater or more likely to roll, that is, the posture stability is low.Used for arithmetic processingBy increasing the number of sampling output values, it is possible to accurately obtain the zero point of the angular velocity sensor by averaging.
[0013]
The structure of claim 3 is the structure of claim 1,Posture stability determination meansIs the angular velocity sensor, and the correction means is as the output value of the angular velocity sensor is smaller.Used for arithmetic processingDecrease the number of sampling output values and increase the output value of the angular velocity sensor.Used for arithmetic processingIt is characterized by being configured to set a large number of sampling output values.
[0014]
According to the configuration of claim 3,Posture stability determination meansIs the angular velocity sensor itself, and the smaller the output value of the angular velocity sensor, the smaller or less likely to roll, that is, the posture stability is higher.Used for arithmetic processingBy reducing the number of sampling output values, the zero point of the angular velocity sensor by averaging can be quickly obtained. In this case, the larger the output value of the angular velocity sensor, the larger or more likely to roll, that is, the posture stability is low.Used for arithmetic processingBy increasing the number of sampling output values, it is possible to accurately obtain the zero point of the angular velocity sensor by averaging.
[0015]
The structure of claim 4 is the structure of claim 1,Posture stability determination meansIs a tilt sensor, and the correction meansUsed for arithmetic processingSet a smaller number of sampling output values and the higher the output value of the tilt sensor,Used for arithmetic processingIt is characterized by being configured to set a large number of sampling output values.
[0016]
According to the configuration of claim 4,Posture stability determination meansIs a tilt sensor, and the smaller the tilt sensor output value, the smaller or less likely to roll, that is, the posture stability is higher.Used for arithmetic processingBy reducing the number of sampling output values, the zero point of the angular velocity sensor by averaging can be quickly obtained. And the larger the output value of the tilt sensor, the larger or more likely to roll, that is, the posture stability is low.Used for arithmetic processingBy increasing the number of sampling output values, it is possible to accurately obtain the zero point of the angular velocity sensor by averaging.
[0017]
The structure of claim 5 is the structure of claim 1,Posture stability determination meansIs a turning angle sensor for detecting the turning angle of the steered wheel, and the correction means is such that the smaller the turning angle of the steered wheelUsed for arithmetic processingDecrease the number of sampling output values and increase the steering wheel turning angle.Used for arithmetic processingIt is characterized by being configured to set a large number of sampling output values.
[0018]
According to the configuration of claim 5,Attitude stability discrimination / discrimination meansIs a turning angle sensor that detects the turning angle of the steered wheel, and the smaller the output value of the turning angle sensor, the smaller or less likely to roll, that is, the posture stability is higher.Used for arithmetic processingBy reducing the number of sampling output values, the zero point of the angular velocity sensor by averaging can be quickly obtained. And the larger the output value of the turning angle sensor, the larger or more likely to roll, that is, the posture stability is low.Used for arithmetic processingBy increasing the number of sampling output values, it is possible to accurately obtain the zero point of the angular velocity sensor by averaging.
[0021]
  Claim 6The ground work device is connected to the traveling machine body in a freely rolling manner, the actuator for rolling the ground work device with respect to the traveling machine body, the inclination sensor for detecting the right and left inclination angle of the ground work device, and the ground work device An angular velocity sensor that detects an angular velocity in the left-right inclination direction, and operates the actuator based on the detection values of both the inclination sensor and the angular velocity sensor so that the horizontal posture of the ground work device is maintained at the set angle. A sensor zero correction device for a working machine provided with a rolling control means,
  Storage means for storing a plurality of sampling output values detected by the angular velocity sensor, zero-point control means for setting an average value calculated based on the plurality of stored sampling output values to zero, and state stability of the traveling machine body A state stability determining means for determining high and low of
  A correction means is provided to set a larger number of sampling output values used for the calculation as the state stability of the traveling aircraft is higher, and to set a smaller number of sampling output values used for the operation as the state stability of the traveling aircraft is lower. It is characterized by.
[0022]
  Claim 6According to the configuration, the higher the state stability of the traveling aircraft, the larger the number of sampling output values used for the calculation, and the lower the traveling aircraft state stability, the smaller the number of sampling output values used for the operation. Therefore, when the zero point drift is stable, the zero point can be obtained with high accuracy by using many sampling output values, and when the zero point drift change is large, the sampling output value can be decreased to follow the change. Become.
[0023]
  Claim 7The configuration ofClaim 6In this configuration, the state stability determination means is a temperature sensor that detects the temperature of the angular velocity sensor, and the smaller the temperature change of the angular velocity sensor, the larger the number of sampling output values used for the calculation, and the temperature change of the angular velocity sensor A correction means for setting a smaller number of sampling output values used for calculation as the value increases is provided.
[0024]
  Claim 7According to the configuration, the temperature sensor for detecting the temperature of the angular velocity sensor is used as the state stability determination means. In work machines, angular velocity sensors are often placed beside the parts of the engine, mission case, etc. that are subject to temperature changes. During continuous work, the engine and mission case show a stable state that maintains a constant temperature. Immediately after starting or when the work load is light, it shows an unstable state in which the temperature changes, so by executing the correction means based on the temperature sensor that detects the presence or absence of the temperature change of the angular velocity sensor,Claim 6The said effect | action by the structure of can be acquired.
[0025]
  Claim 8The configuration ofClaim 6In this configuration, the state stability determining means is a time measuring means for detecting an elapsed time from when the main switch is turned on, and the longer the time from the main switch being turned on, the larger the number of sampling output values used for calculation. A correction means is provided for setting and setting a smaller number of sampling output values used for calculation as the time from the main switch ON is shorter.
[0026]
  Claim 8According to the configuration, the time measuring means for detecting the elapsed time from when the main switch is turned on serves as the discriminating means. That is, the engine (mission case) temperature is in an unstable state that continues to rise for a while immediately after the engine is started, and after a certain amount of time has elapsed, the engine temperature becomes a stable state that settles to a predetermined value. By increasing the number of sampling output values used for calculation as the time from ON becomes longer, and by decreasing the number of sampling output values used for calculation as the time from ON of the main switch becomes shorter,Claim 6The said effect | action by the structure of can be acquired.
[0027]
  Claim 9The structure of the present invention comprises a ground work device connected to a traveling machine body so as to be able to roll, an actuator for rolling the ground work device with respect to the traveling machine body, and an inclination for detecting a right and left inclination angle of the traveling machine body or the ground work device. A sensor and an angular velocity sensor that detects an angular velocity in a lateral tilt direction of the traveling machine body or the ground work device, and based on detection values of both the tilt sensor and the angular speed sensor, the left and right sides of the ground work device A working machine sensor zero correction device provided with a rolling control means for operating the actuator so that the direction and orientation are maintained at a set angle, and storing a plurality of sampling output values detected by the angular velocity sensor Means,
  Control means for setting an average value calculated based on a plurality of stored sampling output values to zero, and control means for performing low pass filter processing on a plurality of sampling output values detected by the angular velocity sensor to set to zero Zero control means equipped with
  Posture stability determining means for determining whether the posture stability of the traveling machine body is high or low,
  Correction means for setting a smaller number of sampling output values used for calculation processing as the attitude stability of the traveling aircraft is higher, and setting a larger number of sampling output values used for arithmetic processing as the posture stability of the traveling aircraft is lower. Providedis there.
[0028]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
FIG. 1 shows the rear part of an agricultural tractor which is an example of a work machine. The mission case 3 can be rolled with respect to a traveling machine body 21 via a top link 1 which can swing up and down and a pair of left and right lower links 2. A rotary tilling device (an example of a ground work device) 4 is connected to the head. A pair of lift arms 6 that are driven to swing up and down by a hydraulic cylinder 5 are provided on the top of the transmission case 3, and the pair of lift arms 6 and the lower link 2 are a lift rod 7 and a double-acting hydraulic cylinder 8. It is connected through. Reference numeral 19 denotes a pair of left and right driving rear wheels.
[0029]
As shown in FIG. 2, the three-position switching type control valve 16 for the hydraulic cylinder 5 is operated by the control device 22, and the rotary tiller 4 is driven up and down by the hydraulic cylinder 5 and the lift arm 6. A three-position switching type control valve 17 for the hydraulic cylinder 8 is operated by the control device 22, and the rotary tiller 4 is rotated around the connection point with the lower link 2 opposite to the hydraulic cylinder 8 by the expansion and contraction operation of the hydraulic cylinder 8. It is driven by rolling.
[0030]
This agricultural tractor is a position for maintaining the height of the rotary tiller 4 with respect to the traveling machine body 21 at a set position, ascending / descending control means 29 for maintaining the rotary tiller 4 at a set height from the ground and maintaining the tillage depth at a set value. The control device 30 is provided with a rolling control function for maintaining the control unit 30 and the horizontal tilt angle of the rotary tiller 4 with respect to the horizontal plane at a set angle.
[0031]
As shown in FIGS. 2 and 1, the rotary tiller 4 is provided with a rear cover 9 that can swing up and down. The rear cover 9 is urged downward by a spring 18, and the rear cover 9 against the rotary tiller 4 is provided. The tilling depth sensor 10 for detecting the vertical swing angle is provided, and the detection value of the tilling depth sensor 10 is input to the control device 22. As a result, the control valve 16 is operated by the lift control means 29 so that the detected value of the tilling depth sensor 10 becomes the set tilling depth of the dial operating type potentiometer type tilling depth setting device 11 provided in the traveling machine body 21. The rotary tiller 4 is automatically driven up and down by the hydraulic cylinder 5.
[0032]
As shown in FIGS. 2 and 1, an angle sensor 13 that detects the vertical angle of the lift arm 6 with respect to the traveling machine body 21 is provided at the base of the lift arm 6, and the detected value of the angle sensor 13 is transmitted to the control device 22. Have been entered. As a result, the control valve 16 is operated by the position control means 30 so that the detected value of the angle sensor 13 becomes the target value of the lever-operated position setting device 12 provided in the traveling machine body 21, and the hydraulic cylinder 5 The lift arm 6 is driven to swing up and down.
[0033]
In the above-described lift control means 29 and position control means 30, the detected value of the angle sensor 13 corresponding to the set tilling depth of the tilling depth setting device 11 and the target value of the position setting device 12 are compared, and the position setting device. When the target value of 12 is higher, the elevation control means 29 and the rolling control function described later are stopped (the hydraulic cylinder 8 is stopped), and the position control means 30 is activated. Accordingly, the control valve 16 is operated so that the detected value of the angle sensor 13 matches the target value of the position setting device 12, and the rotary tiller 4 is driven up and down by the hydraulic cylinder 5. Therefore, by operating the position setter 12, the rotary tiller 4 can be arbitrarily set with respect to the traveling machine body 21 within a range higher than the detected value of the angle sensor 13 corresponding to the set tiller depth of the tiller depth setter 11. It can be driven up and down to the height and stopped.
[0034]
Next, when the position setter 12 is operated downward, and the target value of the position setter 12 matches (or becomes lower) the detected value of the angle sensor 13 corresponding to the set tilling depth of the tilling depth setter 11. The position control means 30 is stopped, and the elevation control means 29 and the rolling control function are activated. Thereby, the control valve 16 is operated so that the detected value of the tilling depth sensor 10 becomes the set tilling depth of the tilling depth setting device 11 by the lifting control means, and the rotary tilling device 4 is automatically raised and lowered by the hydraulic cylinder 5. Driven. As will be described later, with the rolling control function, control is performed so that the left (rightward inclination angle of the rotary tiller 4 with respect to the horizontal plane is maintained at a set angle inclined in the left-right direction (or parallel to the horizontal plane) with respect to the horizontal plane. The valve 17 is operated, and the rotary tiller 4 is driven to roll by the hydraulic cylinder 8.
[0035]
As shown in FIGS. 1 and 2, in this agricultural tractor, the inclination angle of the rotary tiller 4 in the horizontal direction with respect to the horizontal plane is maintained at a set angle that is inclined in the horizontal direction (or parallel to the horizontal plane) with respect to the horizontal plane. As described above, a rolling control unit 24 for rolling the rotary tiller 4 is provided. There is provided a dial type inclination setting device 20 for setting a setting angle in the left-right direction of the rotary tiller 4, which sets the setting angle arbitrarily and continuously from the horizontal position to the right down side and the left down side. It is configured so that it can be changed.
[0036]
That is, a weight type inclination sensor 15 that detects the right and left inclination angle of the traveling machine body 21, a vibration gyro type angular velocity sensor 23 that detects an angular speed of the traveling machine body 21 in the right and left inclination direction, and an operating position of the hydraulic cylinder 8 are detected. Since the horizontal tilt angle of the rotary tiller 4 with respect to the airframe 21 can be detected by the operating position of the rolling cylinder 8, based on the detection values of both the tilt sensor 15 and the angular velocity sensor 23, Rolling control means 24 for operating the hydraulic cylinder 8 is provided in the control device 22 so that the horizontal posture of the rotary tiller 4 is maintained at a set angle by the tilt setting device 20.
[0037]
And the sensor zero point correction apparatus Z which updates and correct | amends the reference voltage of the angular velocity sensor 23 which drifts with various conditions, such as temperature, with time progress is provided. That is, a storage unit 25 that stores a plurality of sampling output values detected by the angular velocity sensor 23, and a zero point control unit that uses an average value calculated based on the stored sampling output values as a reference voltage (zero point). 26 and the high and low attitude stability of the traveling machine body 21 are discriminated.Posture stability determination means HAnd the higher the posture stability of the traveling machine body 21,Used for arithmetic processingThe smaller the number of sampling output values, the lower the attitude stability of the traveling aircraft 21Used for arithmetic processingThe sensor zero point correction device Z is configured by providing correction means 28 for setting a large number of sampling output values.
[0038]
As shown in FIG. 2, a vehicle speed sensor 27 for detecting the traveling speed of the traveling machine body 21 by detecting the rotational speed of the transmission shaft 32 to the rear wheel 19 (see FIG. 1) and the front wheel 31 is provided. By the sensor 27 and the discrimination circuit 41 that processes the detection information thereofPosture stability determination means HIs configured. That is, the higher the traveling speed, the lower the posture stability of the traveling machine body 21, and the slower the traveling speed, the higher the posture stability of the traveling machine body 21.
[0039]
FIG. 3 shows a block diagram of a tilt angle detection system using the tilt sensor 15 and the angular velocity sensor 23.In FIG. 3, the first error compensation G1 is the zero point control means. 26,A constant interval averaging process and an adaptive LPF (low pass filter) process are performed in order to completely remove noise generated within the average interval.
[0040]
  Here, the regular interval averaging process was sampled at 1000 Hz.A predetermined number of data for a predetermined timeIs added to obtain a data string a. AndUnit timeEach time, the average of the data string a is calculated to be the data string b.
[0041]
  The adaptive LPF (low pass filter) process updates the reference voltage by the following process every time the above average process is performed:
  New reference voltage = (average value for 1 second × α + old reference voltage × β) ÷ (α + β)
Output sufficiently smoothed reference voltage as zeroDo. However, α and β are variable depending on conditions. For example, when the reference voltage is likely to fluctuate, such as immediately after the main key is turned on or when the temperature rises, α is increased so that followability to the fluctuation is emphasized. In the condition where is stable, α is made small and importance is given to the accuracy of calculation. Also, the process is interrupted during turning or when there is a large change in inclination so that an incorrect reference voltage is not calculated.
[0042]
Since all errors cannot be removed by the first error compensation G1, there is also an error in the linearity of the sensor, and accumulation of errors due to integration processing cannot be eliminated. Therefore, in order to remove the accumulated error, the second error compensation G2 is performed to feed back the deviation between the target inclination angle θ set by the inclination setting device 20 and the detected inclination angle θr by the inclination sensor 15. Here, if the feedback coefficient K2 can be set sufficiently small, θ = θr is obtained when the aircraft is stopped, and absolute accuracy equivalent to that of the tilt sensor 15 is ensured. A good tilt angle can be output. Conversely, if K2 is large, the compensation of θr is too effective, and the output value of the tilt sensor 15 remains unchanged.
[0043]
hereFeedback factorSince K2 needs to be set according to the accumulated error, the more accurate the gyro sensor is used, the smaller the first error compensation G1 is devised. In the above-described processing, the error can be removed by about 0.5%, and the value is sufficiently small to satisfy the required performance. However, when the temperature changes drastically or when a cheaper gyro sensor is used, it is necessary to further increase the size, and it is necessary to evaluate how much the performance deterioration is caused by it.
[0044]
or,Feedback factorIt is also desirable to make K2 variable. For example, when stopping or when there is little change in inclination, K2 is increased to quickly remove the error in the target inclination angle θ.Feedback factorDecrease K2 to suppress the influence of roll and increase responsiveness. In addition, under conditions where the error of the gyro sensor becomes large, such as immediately after start-up or a large temperature change.Feedback factorWhen K2 is increased, emphasis is placed on removing errors, and when the temperature stabilizesFeedback factorEmphasize responsiveness by reducing K2.
[0045]
As reference,10 to 12 show the detection operation test results of the sensors 15 and 23 when the inclination of the tractor changes.
[0046]
According to FIG. 10, the output from the calculation of the angular velocity sensor 23 changes almost without time delay with respect to the change in the inclination of the traveling machine body, whereas the inclination sensor 15 0.5 When it starts to change in the positive direction and when it starts to change in the negative direction, it is detected as a movement in the reverse direction temporarily due to inertia. I can understand. 10 to 11., V is a left-right inclination angle of the traveling machine body 21, and y is an angular velocity sensor.CalculatingThe output, z, is the tilt sensor output.
[0047]
11 is more than the case of FIG.This is an example of a rapid change in slope. 0.5 The change characteristic of the output by the calculation of the inclination sensor 15 and the angular velocity sensor 23 when changing about 2 degrees per second is shown.According to this, the inclination sensor 15 outputs in the reverse direction about once when starting to descend and rise, and also overshoots when stopping raising and lowering. In contrast, the angular velocity sensor 23It can be seen that the output obtained by the calculation follows the inclination change of the traveling aircraft. In this way, based on the output of the tilt sensor It is clear that the control based on the output of the angular velocity sensor is superior in both responsiveness and accuracy than the above.
[0048]
[Another embodiment]
The sensor zero point correcting device Z may be configured as described in << 1 >> to << 7 >> below.
[0049]
<< 1 >> As shown in FIG.Posture stability determination means HIs the angular velocity sensor 23, and the correction means 28 sets the number of sampling output values by the angular velocity sensor 23 to be smaller as the output value of the angular velocity sensor 23 is smaller, and sets the number of sampling output values as the output value of the angular velocity sensor 23 is larger. Configure as many as you want.
[0050]
<< 2 >> As shown in FIG.Posture stability determination means HIs the inclination sensor 15, and the correction means 28 sets the number of sampling output values by the angular velocity sensor 23 to be smaller as the output value of the inclination sensor 15 is smaller, and sets the number of sampling output values as the output value of the inclination sensor 15 is larger. It is configured to set a lot.
[0051]
<3> As shown in FIG.Posture stability determination means HIs a potentiometer-type turning angle sensor 33 that detects the turning angle of the front steering wheel 31, and the correction means 28 sets the number of sampling output values by the angular velocity sensor 23 to be smaller as the turning angle of the front steering wheel 31 is smaller. The number of sampling output values is set larger as the turning angle of the steering front wheel 31 is larger.
[0052]
<< 4 >> As shown in FIG. 7, a plurality of sampling output values detected by the angular velocity sensor 23The low pass filter (LPF) 34 described aboveIt is determined whether the zero point control means 35 which performs low pass filter processing and sets the zero point and the state stability of the traveling machine body 21 is high or low.State stability determination means JAnd a correction means 36 is provided for setting the cutoff frequency of the low-pass filter 34 to be lower as the state stability of the traveling machine body 21 is higher, and setting the cutoff frequency to be higher as the state stability of the traveling machine body 21 is lower. .
[0053]
<< 5 >> As shown in FIGS. 8 and 9, storage means 25 for storing a plurality of sampling output values detected by the angular velocity sensor 23, and an average value calculated based on the stored sampling output values It is determined whether the zero point control means 26 having zero as a zero point and the state stability of the traveling machine body 21 is high or low.State stability determination means JAnd a correction means 37 is provided which sets a larger number of sampling output values as the state stability of the traveling machine body 21 is higher, and sets a smaller number of sampling output values as the state stability of the traveling machine body 21 is lower. .
[0054]
<< 6 >> As shown in FIG.State stability determination means JIs a temperature sensor 38 for detecting the temperature of the angular velocity sensor 23. The smaller the temperature change of the angular velocity sensor 23, the larger the number of sampling output values by the angular velocity sensor 23, and the greater the temperature change of the angular velocity sensor 23, the more the sampling output. Correction means 37 for setting the number of values small is provided.
[0055]
The angular velocity sensor 23 is disposed at a distance from the mission case 3, but is affected by the ambient temperature due to the temperature change of the mission case 3. Since the temperature of the mission case 3 depends on the engine temperature, the temperature of the angular velocity sensor 23 is clearly different between immediately after the engine is started and after the engine is sufficiently warmed up, thereby causing the zero point to drift. In addition to the engine, the ambient temperature of the angular velocity sensor 23 (an example of the state stability of the traveling machine body 21) varies depending on various conditions such as season, weather, region, and geography. It is a means to change the number of.
[0056]
<< 7 >> As shown in FIG.State stability determination means JIs a timer (an example of a time measuring means) 40 that detects an elapsed time from when the main switch 39 is turned on. The longer the time from when the main switch 39 is turned on, the longer the sampling output value from the angular velocity sensor 23 becomes. A correction means 37 is provided that sets a large number and sets the number of sampling output values to be smaller as the time from when the main switch 39 is turned on is shorter.
[0057]
As described above, the engine temperature, that is, the atmospheric temperature of the angular velocity sensor 23 is completely different from immediately after the engine is started and after the engine is warmed up. In other words, the elapsed time from the ON operation of the main key 39 (that is, when the engine is started). It changes with the difference. That is, as the elapsed time after the ON operation of the main key 39 is shorter, the ambient temperature of the angular velocity sensor 23 is lower, and as the elapsed time is longer, the ambient temperature of the angular velocity sensor 23 is higher. The means for changing the number of sampling output values according to the length of time elapsed after the main key 39 is turned on.
[0058]
<< Others >> It is determined whether the attitude stability of the ground work device 4 or the traveling machine body 21 is high or low.Posture stability determination means HAs other examples, a pitching sensor for detecting the front / rear inclination of the ground work device 4 or the airframe 21, an angular velocity sensor related to the pitching, an angular velocity sensor related to the vertical direction of the ground work device 4 or the airframe 21, an operation of an accelerator lever or a pedal Various changes can be made, such as the amount and the number of shift stages (positions).
[0059]
It is determined whether the state stability of the ground work device 4 or the traveling machine body 21 is high or low.State stability determination means JAs other examples, various modifications such as a thermometer, a hygrometer, and an altimeter are possible. In the present embodiment, the inclination sensor 15 and the angular velocity sensor 23 may be arranged on the ground work device 4 side. The ground work device 4 can be variously modified such as a seedling planting device, a direct sowing device, a cocoon creation device, and a ground leveling device in addition to a tilling device.
[0060]
【The invention's effect】
In the sensor zero correction device for a working machine according to claim 1, the higher the attitude stability of the traveling machine body, the higher the angular velocity sensor.For arithmetic processingSet a smaller number of sampling output values, the lower the attitude stability of the vehicleFor arithmetic processingBy devising a correction means to set a large number of sampling output values, in a high posture stability situation where zero correction is small, zero correction can be performed quickly and rolling control with excellent followability can be realized. Even in situations where posture correction is low and posture stability is low, accurate zero-point correction is possible and rolling control with excellent accuracy can be realized.
[0061]
In the sensor zero correction device for a work machine according to claim 2, by determining the posture stability with a vehicle speed sensor that detects a traveling speed, rolling control with excellent followability can be realized when the traveling speed is slow, and the traveling speed is Even when the speed is fast, it is possible to realize rolling control with excellent accuracy.
[0062]
In the sensor zero-point correction device for a work machine according to claim 3, the posture stability is determined by the angular velocity sensor that detects the rolling angular velocity of the ground work device or the traveling machine body, so that a dedicated sensor for correction is unnecessary. While being economical, there is an advantage that rolling control with excellent followability can be realized when the rolling motion is slow, and rolling control with excellent accuracy can be realized even when the rolling motion is fast.
[0063]
In the working machine sensor zero point correction device according to claim 4, the posture stability is determined by the inclination sensor that detects the absolute rolling angle of the ground working device or the traveling machine body, so that a dedicated sensor for correction is unnecessary. In addition, the rolling control with excellent followability can be realized when the absolute rolling amount is small, and the rolling control with excellent accuracy can be realized even when the absolute rolling amount is large.
[0064]
In the sensor zero-point correction device for a work machine according to claim 5, rolling control with excellent followability is realized when the turning angle is small by determining the turning angle sensor posture stability for detecting the turning angle of the steering wheel. It is possible to achieve rolling control with excellent accuracy even when the cutting angle is large.
[0065]
In the sensor zero correction device for a work machine according to claim 6, the higher the state stability of the traveling machine body, the lower the cutoff frequency of the low-pass filter can be set, and the zero point of the angular velocity sensor can be obtained quickly. The lower the state stability, the higher the cutoff frequency, and the rolling control that can accurately determine the zero point of the angular velocity sensor could be realized.
[0066]
In the sensor zero correction device for a working machine according to claim 7, the higher the state stability of the traveling machine body,For arithmetic processingSet a larger number of sampling output values, the lower the state stability of the vehicleFor arithmetic processingBy setting the number of sampling output values to be small, it is possible to obtain an accurate zero point when the zero point drift is stable, and to correct the zero point with good follow-up to the drift change when the zero point drift change is large. Became.
[0067]
According to the eighth aspect of the present invention, there is an advantage that the effect of the configuration according to the seventh aspect can be achieved by the correction means based on the temperature sensor that detects the presence or absence of the temperature change of the angular velocity sensor.
[0068]
In the sensor zero-point correcting device for a work machine according to a ninth aspect, there is an advantage that the effect of the configuration according to the seventh aspect can be achieved by the correcting means based on the length of time from the main switch ON.
[Brief description of the drawings]
FIG. 1 is a perspective view showing a structure of a rear portion of an agricultural tractor.
FIG. 2 is a functional system diagram showing a schematic structure of rolling control.
FIG. 3 is a block diagram showing the concept of correction control means.
FIG. 4 is a functional system diagram of rolling control that is corrected based on an angular velocity sensor.
FIG. 5 is a functional system diagram of rolling control that is corrected based on an inclination sensor.
FIG. 6 is a functional system diagram of rolling control that is corrected based on a cutting angle sensor.
FIG. 7 is a functional system diagram of rolling control for correction based on low-pass filter processing.
FIG. 8 is a functional system diagram of rolling control for correcting based on the temperature of the angular velocity sensor.
FIG. 9 is a functional system diagram of rolling control for correcting based on the elapsed time after the main key is turned on.
FIG. 10 is a graph showing output characteristic graphs of an inclination sensor and an angular velocity sensor.
FIG. 11 is a graph showing output characteristic graphs of an inclination sensor and an angular velocity sensor.
[Explanation of symbols]
4 Ground working equipment
8 Actuator
15 Tilt sensor
21 Traveling aircraft
23 Angular velocity sensor
24 Rolling control means
25 Memory means
26 Zero point control means
27 Vehicle speed sensor
28 Correction means
31 Steering wheel
33 Cutting angle sensor
35 Control means
36 Correction means
37 Correction means
38 Temperature sensor
39 Main switch
40 hour measuring means
HPosture stability determination means
JState stability determination means

Claims

An actuator for connecting the ground work device to a traveling machine body so as to be able to roll freely, the actuator for rolling the ground work device with respect to the traveling machine body, an inclination sensor for detecting a right and left inclination angle of the traveling machine body or the ground work device, An angular velocity sensor that detects an angular velocity in a lateral tilt direction of the traveling machine body or the ground work device, and a lateral orientation of the ground work device is set based on detection values of both the tilt sensor and the angular velocity sensor. A sensor zero correction device for a working machine provided with a rolling control means for operating the actuator so as to be maintained at an angle,
Storage means for storing a plurality of sampling output values detected by the angular velocity sensor, zero point control means for setting an average value calculated based on the stored sampling output values to zero, and the attitude of the traveling machine body Posture stability determining means for determining high and low stability,
Correction means for setting a smaller number of sampling output values used for calculation processing as the attitude stability of the traveling aircraft is higher, and setting a larger number of sampling output values used for arithmetic processing as the posture stability of the traveling aircraft is lower. A sensor zero correction device for a working machine provided.

The posture stability determination means is a vehicle speed sensor that detects the traveling speed of the traveling vehicle body. The slower the detected traveling speed, the smaller the number of sampling output values used for the arithmetic processing, and the faster the traveling speed, The sensor zero-point correction apparatus for a working machine according to claim 1, wherein the sensor zero-point correction apparatus is configured to set a large number of sampling output values used for arithmetic processing.

The posture stability determining means is the angular velocity sensor, and the smaller the output value of the angular velocity sensor, the smaller the number of sampling output values used for the arithmetic processing, and the larger the output value of the angular velocity sensor, the sampling used for the arithmetic processing. The sensor zero correction device for a work machine according to claim 1, wherein the device is configured to set a large number of output values.

The posture stability determination means is the tilt sensor, and the smaller the output value of the tilt sensor, the smaller the number of sampling output values used for the calculation process, and the larger the output value of the tilt sensor, the sampling used for the calculation process. The sensor zero correction device for a work machine according to claim 1, wherein the device is configured to set a large number of output values.

The posture stability determination means is a turning angle sensor that detects a turning angle of a steered wheel, and sets a smaller number of the sampling output values used for calculation processing as the detected turning angle of the steered wheel is smaller, The sensor zero correction device for a working machine according to claim 1, wherein the sensor zero point correction device is configured to set a larger number of the sampling output values used for the arithmetic processing as the turning angle of the steering wheel is larger.

An actuator for connecting the ground work device to a traveling machine body so as to be able to roll freely, the actuator for rolling the ground work device with respect to the traveling machine body, an inclination sensor for detecting a right and left inclination angle of the traveling machine body or the ground work device, An angular velocity sensor that detects an angular velocity in a lateral tilt direction of the traveling machine body or the ground work device, and a lateral orientation of the ground work device is set based on detection values of both the tilt sensor and the angular velocity sensor. A sensor zero correction device for a working machine provided with a rolling control means for operating the actuator so as to be maintained at an angle,
Storage means for storing a plurality of sampling output values detected by the angular velocity sensor, zero point control means for setting an average value calculated based on the stored sampling output values to zero, and the state of the traveling machine body A state stability determining means for determining high and low stability,
A correction for setting the number of sampling output values used for calculation processing to be higher as the state stability of the traveling aircraft is higher, and setting a smaller number of sampling output values to be used for calculation processing as the state stability of the traveling aircraft is lower. A sensor zero correction device for a working machine provided with means.

The state stability determination means is a temperature sensor that detects the temperature of the angular velocity sensor, and the smaller the temperature change of the angular velocity sensor, the larger the number of sampling output values used for the arithmetic processing, and the temperature of the angular velocity sensor 7. The sensor zero correction device for a working machine according to claim 6, further comprising correction means for setting a smaller number of sampling output values used for arithmetic processing as the change is larger.

The state stability determining means is a time measuring means for detecting an elapsed time from when the main switch is turned on, and the longer the time from the main switch being turned on, the larger the number of sampling output values used for the arithmetic processing is set. 7. A sensor zero correction device for a working machine according to claim 6, further comprising correction means for setting a smaller number of sampling output values used for calculation processing as the time from the main switch ON is shorter.

An actuator for connecting the ground work device to a traveling machine body so as to be able to roll freely, the actuator for rolling the ground work device with respect to the traveling machine body, an inclination sensor for detecting a right and left inclination angle of the traveling machine body or the ground work device, An angular velocity sensor that detects an angular velocity in a lateral tilt direction of the traveling machine body or the ground work device, and a lateral orientation of the ground work device is set based on detection values of both the tilt sensor and the angular velocity sensor. A sensor zero correction device for a working machine provided with a rolling control means for operating the actuator so as to be maintained at an angle, and storing means for storing a plurality of sampling output values detected by the angular velocity sensor;
Control means for setting an average value calculated based on a plurality of stored sampling output values to zero, and control means for performing low pass filter processing on a plurality of sampling output values detected by the angular velocity sensor to set to zero Zero control means equipped with
A discriminating means for discriminating whether the attitude stability of the traveling machine body is high or low,
Correction means for setting a smaller number of sampling output values used for calculation processing as the attitude stability of the traveling aircraft is higher, and setting a larger number of sampling output values used for arithmetic processing as the posture stability of the traveling aircraft is lower. A sensor zero correction device for a working machine provided.