JP2006101737A - Automatic controller of inclination in working vehicle - Google Patents
Automatic controller of inclination in working vehicle Download PDFInfo
- Publication number
- JP2006101737A JP2006101737A JP2004290804A JP2004290804A JP2006101737A JP 2006101737 A JP2006101737 A JP 2006101737A JP 2004290804 A JP2004290804 A JP 2004290804A JP 2004290804 A JP2004290804 A JP 2004290804A JP 2006101737 A JP2006101737 A JP 2006101737A
- Authority
- JP
- Japan
- Prior art keywords
- angular velocity
- velocity sensor
- sensor
- output
- tilt
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Lifting Devices For Agricultural Implements (AREA)
Abstract
Description
本発明は、走行機体または作業部の刻々の傾斜角を傾斜センサと角速度センサで検知して作業部の左右傾斜角を自動制御する作業車輌における傾斜自動制御装置に関する。 The present invention relates to an automatic tilt control apparatus in a working vehicle that automatically controls left and right tilt angles of a working unit by detecting a tilt angle of a traveling machine body or a working unit with a tilt sensor and an angular velocity sensor.
トラクタ等の走行機体にロータリ耕耘機等の作業部を連結して整地、耕耘等を行う作業車輌では、走行機体の傾きとは無関係に、作業部を水平に維持したり、地面の傾きと一致した左右傾斜角を作業部に設定したりすることが望まれる。このような要求に応えて、走行機体に傾斜センサと角速度センサを配置して走行機体の刻々の左右傾斜角を演算し、当該傾斜角に基づいて作業部の左右傾斜角を自動調整するようにした作業車輌が実用化されている。 For work vehicles that perform leveling, tilling, etc. by connecting a working unit such as a rotary cultivator to a traveling machine such as a tractor, the working part is kept horizontal or coincides with the inclination of the ground regardless of the inclination of the traveling machine. It is desirable to set the left / right inclination angle in the working unit. In response to such a request, an inclination sensor and an angular velocity sensor are arranged on the traveling machine body to calculate the horizontal inclination angle of the traveling machine every moment, and the left and right inclination angle of the working unit is automatically adjusted based on the inclination angle. The working vehicle is put into practical use.
ここでは、走行機体と作業部とが左右傾斜角度を変更可能な形式で連結され、連結部に油圧シリンダ等を配置して作業部の左右傾斜角を任意に設定できるようにし、傾斜センサと角速度センサの出力に基づいて演算された刻々の傾斜角が油圧シリンダ等の駆動にフィードバックされる。 Here, the traveling machine body and the working part are connected in a form that can change the right and left inclination angle, and a hydraulic cylinder or the like is arranged in the connecting part so that the right and left inclination angle of the working part can be arbitrarily set. The moment of inclination calculated based on the output of the sensor is fed back to the drive of the hydraulic cylinder or the like.
このような作業車輌における傾斜自動制御装置では、絶対傾斜角度を検出できる傾斜センサの出力と応答性の高い角速度センサの出力とを組み合わせて傾斜角が演算される。角速度センサは走行機体が傾斜する際の角速度に応じた出力を出力するから、所定時間積分すれば当該時間における走行機体の傾斜角度変化量が演算される。 In such an automatic tilt control apparatus for a working vehicle, the tilt angle is calculated by combining the output of the tilt sensor capable of detecting the absolute tilt angle and the output of the angular velocity sensor with high responsiveness. Since the angular velocity sensor outputs an output corresponding to the angular velocity when the traveling machine body tilts, if the integration is performed for a predetermined time, the amount of change in the inclination angle of the traveling machine body at that time is calculated.
ここで、例えば、0〜5Vの範囲で±最大検出角速度の出力を行うような角速度センサを使用する場合、角速度0に対応する基準値(例えば2.5V)を定めて、(センサ出力−基準値)で右回転/左回転の角速度を判別する必要がある。
Here, for example, when using an angular velocity sensor that outputs ± maximum detected angular velocity in a range of 0 to 5 V, a reference value (for example, 2.5 V) corresponding to the
しかし、この基準値は、角速度センサの温度変化、計時変化、振動や運転状態等によって変化する可能性がある。基準値が変化すると、左回転の角速度を右回転の角速度と誤認したり、演算された角速度の大きさが変化したりする。そして、このような角速度の誤差が積分によって累積されると実際の傾斜角とはかけ離れた傾斜角が演算されてしまう。 However, this reference value may change due to temperature change of the angular velocity sensor, change in timekeeping, vibration, operating state, and the like. When the reference value changes, the angular velocity of the left rotation is mistaken as the angular velocity of the right rotation, or the calculated angular velocity changes. When such angular velocity errors are accumulated by integration, an inclination angle far from the actual inclination angle is calculated.
そこで、所定のタイミングで角速度センサの出力に基づく傾斜角を傾斜センサの出力に基づく傾斜角に置き換えたり、角速度センサの基準値を随時更新したりして、制御に用いられる傾斜角の誤差を小さくする技術が提案されている。 Therefore, the tilt angle based on the output of the angular velocity sensor is replaced with the tilt angle based on the output of the tilt sensor at a predetermined timing, or the reference value of the angular velocity sensor is updated as needed to reduce the tilt angle error used for control. Techniques to do this have been proposed.
特許文献1には、傾き角度を検知する傾斜センサとローリングの角速度を検知する角速度センサとを走行機体に取り付けて作業部の左右傾斜角度を計測するようにした作業車輌が示される。ここでは、角速度非検出時には傾斜センサの出力に基づいて傾斜角を演算するが、角速度検出時には角速度センサの出力を積分して傾斜角を演算している。また、最後の角速度検出から一定時間が経過した後に、傾斜センサの出力に基づく傾斜角の演算に復帰させている。
特許文献2には、傾斜センサの出力状態と角速度センサの出力状態との組み合わせから走行機体の走行状態をきめ細かく判別して傾斜角度の演算方法を切り替えるようにした傾斜自動制御装置が示される。また、角速度センサの出力に基づく演算から傾斜センサの出力に基づく演算へ切り替える際に、演算された傾斜角の格差を次第に減少させて傾斜角の急変を回避している。 Patent Document 2 discloses an automatic inclination control apparatus that switches the calculation method of the inclination angle by finely determining the traveling state of the traveling machine body from the combination of the output state of the inclination sensor and the output state of the angular velocity sensor. Further, when switching from the calculation based on the output of the angular velocity sensor to the calculation based on the output of the tilt sensor, the difference in the calculated tilt angle is gradually reduced to avoid a sudden change in the tilt angle.
特許文献3には、角速度センサの出力の移動平均値で角速度演算の基準値を刻々と更新する傾斜自動制御装置が示される。ここでは、角速度センサの安定出力における上のピーク値の平均に対して不感帯の上限を設定し、下のピーク値の平均に対して不感帯の下限を設定し、両者の間の不感帯内に角速度センサの出力があれば傾斜センサの出力に基づく傾斜角度制御を行う一方、不感帯を超えたときに角速度センサの出力に基づく傾斜角度制御へ移行させている。 Patent Document 3 discloses an automatic tilt control apparatus that constantly updates a reference value for angular velocity calculation with a moving average value of an output of an angular velocity sensor. Here, the upper limit of the dead band is set for the average of the upper peak values in the stable output of the angular velocity sensor, the lower limit of the dead band is set for the average of the lower peak values, and the angular velocity sensor is within the dead band between the two. If there is an output, the tilt angle control based on the output of the tilt sensor is performed, and when the dead zone is exceeded, the control is shifted to the tilt angle control based on the output of the angular velocity sensor.
特許文献4には、角速度センサの出力に基づく傾斜角度の演算値に傾斜センサの出力をフィードバックして実際の傾斜角度に演算値を近付ける傾斜自動制御装置が示される。ここでは、角速度センサの出力値の平均値と基準値を重み付け加算平均して基準値を刻々更新している。 Patent Document 4 discloses an automatic tilt control apparatus that feeds back an output of a tilt sensor to a calculated value of an inclination angle based on an output of an angular velocity sensor and brings the calculated value close to an actual tilt angle. Here, the average value of the output value of the angular velocity sensor and the reference value are weighted and averaged, and the reference value is updated every moment.
特許文献1、2に示される傾斜自動制御装置では、角速度センサの出力に基づく傾斜角と傾斜センサの出力に基づく傾斜角を置き換えるにとどまり、角速度センサの出力から演算された角速度の誤差は基準値次第である。従って、基準値がずれると角速度の誤差が累積した傾斜角が演算される可能性がある。
In the automatic tilt control apparatus shown in
特許文献3に示される傾斜自動制御装置では、角速度センサの出力の移動平均を求めて基準値を刻々と更新するから、角速度センサの基準値がずれた際には速やかに正しい基準値に誘導されて角速度の誤差は維持される。しかし、角速度センサの出力のピークを検出するために高速でサンプリングを行う必要があり、過去の角速度センサの出力を多数記憶しておく必要もある。従って、演算負荷や記憶容量が膨大になって演算装置の大型化と消費電力増を招く。 In the automatic tilt control apparatus disclosed in Patent Document 3, the moving average of the output of the angular velocity sensor is obtained and the reference value is updated every moment. Therefore, when the reference value of the angular velocity sensor is shifted, it is promptly guided to the correct reference value. Thus, the error in angular velocity is maintained. However, in order to detect the peak of the output of the angular velocity sensor, it is necessary to perform sampling at a high speed, and it is also necessary to store many past angular velocity sensor outputs. Therefore, the calculation load and the storage capacity become enormous, leading to an increase in the size of the calculation device and an increase in power consumption.
特許文献4に示される傾斜自動制御装置では、角速度センサの出力に基づく傾斜角度の演算値に傾斜センサの出力をフィードバックして演算値を刻々補正するから、いつまでも演算値が実際の傾斜角に追従する。しかし、加算とローパスフィルタ演算を正確に行うには、kHzレベルの高速サンプリングが必要であり、演算負荷や記憶容量が膨大になって演算装置の大型化と消費電力増を招く。 In the automatic tilt control apparatus disclosed in Patent Document 4, since the calculated value is corrected by feeding back the output of the tilt sensor to the calculated value of the tilt angle based on the output of the angular velocity sensor, the calculated value always follows the actual tilt angle. To do. However, in order to accurately perform addition and low-pass filter calculation, high-speed sampling at the kHz level is required, and the calculation load and storage capacity become enormous, resulting in an increase in the size of the arithmetic device and an increase in power consumption.
本発明は、角速度センサの基準値を刻々と補正する際の演算負荷や必要なメモリ容量が小さく済み、しかも、角速度センサ出力の読み込み頻度が少なくても補正の精度が必要十分に確保され、従って、低速でメモリ容量の少ないマイコン素子等を採用して小型安価低消費電力を実現可能な作業車輌における傾斜自動制御装置を提供することを目的としている。 According to the present invention, the calculation load and the required memory capacity for correcting the reference value of the angular velocity sensor every moment are small, and the correction accuracy is sufficiently and sufficiently ensured even when the angular velocity sensor output is read less frequently. An object of the present invention is to provide an automatic tilt control apparatus in a working vehicle that can realize a small size, low cost and low power consumption by adopting a microcomputer element having a low memory capacity at a low speed.
請求項1の作業車輌における傾斜自動制御装置は、走行機体(10)に作業部(30)を左右傾斜自在に支持し、前記走行機体(10)又は作業部(30)の左右傾斜角を検出して、前記走行機体(10)の左右傾斜に拘らず、前記作業部(30)が予め設定された姿勢に保持されるように制御する傾斜自動制御装置(40)を備えた、作業車輌において、前記左右傾斜角を検出する手段として傾斜センサ(13)と角速度センサ(14)とを備え、前記角速度センサ(14)の基準値を、「今までの基準値+(角速度センサの検出値−今までの基準値)/2n」により算出し、上記指数nを、傾斜自動制御の作動中に変更可能な整数としたものである。
An automatic tilt control apparatus for a working vehicle according to
請求項2の作業車輌における傾斜自動制御装置は、請求項1の発明における前記指数nを、前記(角速度センサの検出値−今までの基準値)の差分が小さい状態にあっては、大きくなるように変更し、前記差分が大きい状態にあっては、小さくなるように変更するものである。
The automatic inclination control device for the working vehicle according to claim 2 increases the index n in the invention according to
請求項3の作業車輌における傾斜自動制御装置は、請求項1の発明における(前記角速度センサの検出値−前記基準値)の差分データが、不感帯を越えることに基づき、該差分データの積分値による傾斜角により傾斜自動制御し、かつ前記不感帯が、車速により変化するものである。 According to a third aspect of the present invention, there is provided an automatic tilt control apparatus for a working vehicle, wherein the difference data of (the detected value of the angular velocity sensor−the reference value) in the first aspect of the invention exceeds the dead zone, Inclination is automatically controlled by the inclination angle, and the dead zone changes depending on the vehicle speed.
なお、上述した括弧内の符号は図面を参照するために示すものであって、本発明を何ら限定するものではない。 In addition, the code | symbol in the parenthesis mentioned above is shown in order to refer drawings, Comprising: This invention is not limited at all.
請求項1に係る本発明によると、移動平均値による基準値の設定のように、多くのデータを蓄積する必要がなく、バッファやメモリを多用することなく、演算を高速化できると共に、安価なマイクロコンピュータを用いることが可能となる。 According to the first aspect of the present invention, unlike the setting of the reference value based on the moving average value, it is not necessary to store a large amount of data, the calculation can be speeded up without using a lot of buffers and memories, and the cost is low. A microcomputer can be used.
請求項2に係る本発明によると、(角速度センサの検出値−今までの基準値)の差分が小さい場合は、指数を大きくして基準値の安定化を図り、上記差分が大きい場合、指数を小さくして基準値を角速度の検出値に速く近づけるようにして、角速度センサによる傾斜自動制御を高い応答性能でかつ安定して行うことができる。 According to the second aspect of the present invention, when the difference between (the detected value of the angular velocity sensor−the reference value so far) is small, the index is increased to stabilize the reference value, and when the difference is large, the index is By making the reference value close to the detected value of the angular velocity quickly, automatic tilt control by the angular velocity sensor can be performed stably with high response performance.
請求項3に係る本発明によると、上記基準値に基づく角速度センサの差分データの不感帯が、車速により変化するので、車速が高い場合、不感帯を広くして安定した制御を行うことができると共に、車速が低い場合は不感帯を狭くして、高い応答性能により正確に制御することができる。 According to the third aspect of the present invention, since the dead zone of the difference data of the angular velocity sensor based on the reference value varies depending on the vehicle speed, when the vehicle speed is high, the dead zone can be widened and stable control can be performed. When the vehicle speed is low, the dead zone can be narrowed and accurately controlled with high response performance.
傾斜センサ13と角速度センサ14の出力に基づいてトラクタ10の傾斜角を刻々と演算し、演算結果に基づいてリフトロッドシリンダ25を駆動することにより、作業機30の左右傾斜角を自動調整する。
The tilt angle of the
図1は実施例1の傾斜自動制御装置を搭載したトラクタの側面図、図2は傾斜自動制御装置の接続状態の説明図、図3は実施例1の傾斜自動制御装置における制御全体のフローチャート、図4は初期設定のフローチャート、図5は、平滑初期設定と基準初期設定のフローチャート、図6は接続判断のフローチャート、図7は接続判断におけるタイマ割込処理のフローチャート、図8は平滑処理のフローチャート、図9は演算不感帯設定のフローチャート、図10は切替制御のフローチャート、図11は切替制御における部分的なフローチャート、図12は角度演算のフローチャート、図13は傾斜制御のフローチャート、図14は基準データ処理のフローチャート、図15は基準データ処理における部分的なフローチャート、図16はヒステリシス設定の線図、図17は傾斜制御における不感帯設定の線図、図18は基準データ処理における不感帯設定の線図である。 FIG. 1 is a side view of a tractor equipped with an automatic tilt control apparatus according to a first embodiment, FIG. 2 is an explanatory diagram of a connection state of the automatic tilt control apparatus, and FIG. 3 is a flowchart of overall control in the automatic tilt control apparatus according to the first embodiment. 4 is a flowchart of initial setting, FIG. 5 is a flowchart of smoothing initial setting and reference initial setting, FIG. 6 is a flowchart of connection determination, FIG. 7 is a flowchart of timer interruption processing in connection determination, and FIG. 9 is a flowchart of calculation dead zone setting, FIG. 10 is a flowchart of switching control, FIG. 11 is a partial flowchart of switching control, FIG. 12 is a flowchart of angle calculation, FIG. 13 is a flowchart of tilt control, and FIG. FIG. 15 is a partial flowchart of the reference data processing, and FIG. Diagram of cis configuration 17 is diagrammatic view of the dead zone set in the tilt control, FIG. 18 is a diagram of the dead band setting in the reference data processing.
図1に示すように、走行機体(トラクタ10)の後部には昇降リンク機構20が接続され、昇降リンク機構20の後部には、回転刃31を有する作業機(ロータリ耕耘ユニット)30が連結されている。昇降リンク機構20は、1個のアッパリンク21と左右一対のロアリンク22を備える三点式のもので、作業機30以外にも種々の作業機を選択して装着可能である。
As shown in FIG. 1, a lifting link mechanism 20 is connected to the rear part of the traveling machine body (tractor 10), and a working machine (rotary tillage unit) 30 having a
昇降リンク機構20のロアリンク22は、リフトロッド24を介してリフトアーム23に吊り下げ支持され、リフトアーム23は、その根元側に配置したリフトシリンダ27の伸長/収縮動作によって上下に回動される。したがって、リフトシリンダ27を油圧駆動して作業機30を上昇/下降動作させたり、任意の耕耘深さを設定したりすることができる。
The
また、トラクタ10の進行方向左側のリフトロッド24は、リフトロッドシリンダ25を介してリフトアーム23に吊り下げ支持されており、リフトロッドシリンダ25を油圧駆動して左右のリフトロッド24の長さを異ならせることによって、作業機30の左右傾き角度を任意に設定可能である。
The
トラクタ10の運転席15に隣接させて操作パネル16が配置され、操作パネル16の下方に傾斜自動制御装置40が配置されている。操作パネル16には、リフトアーム23を手動で昇降動作させるポジションレバー11と、作業に関する各種設定や切替えを行うためのスイッチやボリュームが多数配置されている。
An
図2に示すように、傾斜自動制御装置40は、マイクロコンピュータ回路(MPU、RAM、ROM、IO回路等を含む)を用いて構成される電子回路であって、ポジションレバー11による設定高さを目標として作業機30を自動的に昇降制御するポジション制御、図示しない耕深設定ボリュームの設定耕深を目標として作業機30を自動的に昇降制御する耕深自動制御、傾斜設定ボリューム43の設定傾斜角度を目標として作業機30を自動的に傾斜制御する傾斜自動制御等の制御を実行する。
As shown in FIG. 2, the automatic
傾斜自動制御装置40には、自動切替ボリューム41、作業機切替ボリューム42、傾き設定ボリューム43、操作スイッチ44、エンジン回転センサ45、車軸回転センサ46、リモコンスイッチ47、リフトアームセンサ26、リフトロッドセンサ28、ポジションレバーセンサ29、傾斜センサ13、角速度センサ14、リレーユニット57、ソレノイドバルブ51、52等が接続されている。
The automatic
自動切替ボリューム41は、操作パネル16に配置された自動切替ツマミに連動させた可変抵抗器であって、傾斜自動制御のON/OFF等に対応させた複数の固定ポジションを判別させる。自動切替スイッチでOFFが設定されると作業機30の左右傾斜の自動調整は実行されないが、ONが設定されるとスピーカー54を通じて短い切替音が出力されて自動調整が開始され、自動調整状態を通知するランプ55が点灯される。
The
作業機切替ボリューム42は、操作パネル16に配置された作業機切替ツマミに連動させた可変抵抗器であって、トラクタ10に連結する作業機の種類に対応させた複数の固定ポジションを判別させる。
The work
傾き設定ボリューム43は、操作パネル16に配置された傾斜角度設定ツマミに連動させた可変抵抗器であって、作業者が傾斜角度設定ツマミを回転させて設定した作業機30の傾斜角度に対応するアナログ電圧を出力する。操作スイッチ群44は、傾斜角度調整の作動感度を設定する等、傾斜自動制御の付属機能を設定するものである。
The
エンジン回転センサ45は、トラクタ10のエンジン出力軸に取り付けられたパルスエンコーダであって、エンジン回転速度に応じた周波数のパルス電圧を出力する。車軸回転センサ46は、トラクタ10の後輪軸に取り付けられたパルスエンコーダであって、トラクタ10の対地速度(走行速度)に応じた周波数のパルス電圧を出力する。リモコンスイッチ47は、リフトロッドシリンダ25を運転席15の外から手動で動作させるスイッチである。
The
リフトアームセンサ26は、図1にも示すように、リフトアーム23の根元に配置された可変抵抗器であって、リフトアーム23の回動角に応じた電圧信号を出力する。リフトロッドセンサ28は、リフトロッドシリンダ25に隣接配置された測長センサであって、リフトロッドシリンダ25の引出し長さに応じた電圧信号を出力する。ポジションレバーセンサ29は、ポジションレバー11の根元に配置された可変抵抗器であって、ポジションレバー11の回動角に対応した電圧信号を出力する。
As shown in FIG. 1, the
傾斜センサ13は、トラクタ10の後輪軸の近傍に配置された重力式の傾斜角度センサであって、車体のローリング傾斜角度に対応した電圧信号を出力する。角速度センサ14は、傾斜自動制御装置40の箱内に格納された角速度センサであって、車体のローリング角速度に応じた電圧信号を出力する。
The
ステアリング切れ角検出スイッチ56は、トラクタ10のステアリング12が回転されて前輪切れ角が左右一定角度以上になるとONするように配置されたリミットスイッチである。リレーユニット57は、ステアリング12が操作されてステアリング切れ角検出スイッチ56のいずれかがONしている間、一定の電圧信号を出力する。
The steering
ソレノイドバルブ51は、リフトロッドシリンダ25の伸長方向にオイルを流し、ソレノイドバルブ52は、リフトロッドシリンダ25の短縮方向にオイルを流す。プライオリティバルブ53は、ソレノイドバルブ51、52へのオイル供給を許可(または停止)する。
The
以上のように構成された実施例1の傾斜自動制御装置40では、自動切替ボリューム41がONに設定されて左右傾斜角度の自動調整が選択され、作業機切替ボリューム42はロータリ式の作業機30に設定されている。このとき、傾斜自動制御装置40は、傾斜センサ13および角速度センサ14の出力に基づいてトラクタ10の刻々の傾斜角を演算し、リフトロッドセンサ28の出力を参照しつつ傾き設定ボリューム43に設定された傾斜角度を目標にして刻々の制御量を演算する。そして、傾斜自動制御装置40は、この制御量に基づいてソレノイドバルブ51、52を作動させることにより、リフトロッドシリンダ25を伸長/短縮して作業機30の傾斜角を制御する。
In the automatic
図3に示すように、傾斜自動制御装置40は、ステップ210〜370で構成されるメインフロー200に従って傾斜角自動制御を実行する。
As shown in FIG. 3, the automatic
ステップ210では、図4、図5に示すように各種演算処理の初期設定を行う。続くステップ220では、図6、図7に示すように、角速度センサの接続判断を行う。続くステップ250では、図8に示すように、傾斜センサ13の出力を平均して基準データを作成する平滑処理を行う。続くステップ280では、図9に示すように、角速度センサ14の出力データを積算する際の判断に使用される演算不感帯が設定される。
In
続くステップ310では、図10、図11に示すように、トラクタ10の状況に応じて角速度センサ14に基づく制御と傾斜センサ13に基づく制御とを選択する切替制御を行う。続くステップ360では、図12に示すように、角速度センサ14の出力を積算する角度演算を行う。続くステップ370では、図13に示すように、演算された傾斜角度に基づいて出力を制御する傾斜制御を行う。
In the
図4に示すように、初期設定フロー(210)では、ステップ211で初期設定が必要か否かを判定し、自動切替スイッチがONされる等して必要と判断されればステップ212〜216に示されるように、各種演算処理の初期設定を行う。ステップ212では演算開始フラグと積算フラグがリセットされる。
As shown in FIG. 4, in the initial setting flow (210), it is determined in
図4のステップ213では、図5に示すように、平滑初期設定フロー(240)が実行される。すなわち、ステップ241では、平滑データとして傾斜センサ13の基準値が採用され、ステップ242では、平滑演算データの設定が開始され、ステップ244では平滑フラグがクリアされる。
In
図4のステップ214では、図5に示すように、基準初期設定フロー(390)が実行される。すなわち、ステップ391では基準シフトの設定が開始され、ステップ392ではベースの設定が開始され、ステップ393では4つの制御カウンタがクリアされる。
In
図5に戻って、ステップ215では、更新確認タイマ、タイムアウト、開始遅延タイマの各設定が開始される。ステップ216では、演算不感帯の設定が開始される。
Returning to FIG. 5, in
図6に示すように、接続判断フロー(220)では、ステップ221で角速度センサ14の出力値が正常範囲か否かが判定される。正常範囲は、図21に示すように、角速度センサの出力0〜5Vの範囲中0.1〜4.9Vの範囲に定められ、正常範囲の上下0.2Vの範囲にヒステリシス範囲を設けている。ここで、正常範囲外と判断されればステップ224へ進んで範囲外フラグがONされ、さらに、ステップ225へ進んでセンサ外れタイマが限界時間を越えたか否かが判定される。センサ外れタイマは、図7に示すように、タイマ割込み処理として範囲外フラグのON経過時間を積算しており、ON状態が限界時間に達するとステップ226へ進んでセンサ外れフラグがONされる。
As shown in FIG. 6, in the connection determination flow (220), it is determined in
一方、角速度センサ14の出力値が正常範囲内であれば、ステップ222へ進んでヒステリシス範囲か否かが判定される。そして、ヒステリシス範囲でなければ、ステップ227へ進んで範囲外フラグがOFFされ、さらに、ステップ228へ進んでセンサ接続タイマが限界時間を越えたか否かが判定される。センサ接続タイマは、図7に示すように、タイマ割込み処理として範囲外フラグのOFF経過時間を積算しており、OFF状態が限界時間を越えていればステップ229へ進んでセンサ外れフラグがONか否かが判定され、センサ外れフラグがONであれば、ステップ231へ進んでセンサ外れフラグがOFFされる。続いて、ステップ232ではリセット処理が実行され、ステップ233ではベース初期設定が実行される。
On the other hand, if the output value of the
また、ステップ222でヒステリシス範囲の場合、ステップ223へ進んで範囲外フラグのON/OFFが判定され、OFFの場合はステップ227へ進むが、ONの場合(正常出力範囲外からヒステリシス範囲へ入った場合に該当)はステップ224へ進んで範囲外に対応する処理を継続する。そして、ステップ225でセンサ外れタイマ未了、ステップ228でセンサ接続タイマ未了、ステップ229でセンサ外れフラグOFFのそれぞれの場合は、現状の各フラグ状態が継続される。
If the hysteresis range is determined in
このようにして、角速度センサ14の出力を判定し、角速度センサ14の接続と機能の正常が確認されるとセンサ外れフラグはOFFとなる一方、確認されないとセンサ外れフラグはONとなる。
In this way, the output of the
図8に示すように、平滑制御フロー(250)では、傾斜センサ13の出力の平均値を演算するためのサンプリング周期を設定する。ステップ251では、平滑フラグが平滑初期設定フロー(240)でクリアされたまま、すなわち初期状態のままか否かが判定される。そして、初期状態であればステップ253へ進んで通常の200msecのサンプリング周期が設定される。
As shown in FIG. 8, in the smoothing control flow (250), a sampling period for calculating the average value of the output of the
一方、初期状態でなければ、ステップ252へ進んで作業開始フラグがセットからリセットへ反転したか否かが確認される。作業開始フラグは、図10、図11に示す切替制御フロー(310)で説明するように、作業機30が下降されて所定時間が経過するまでセットされ続け、経過後にリセットされるので、作業開始フラグがセットからリセットへ反転すると、作業開始と判定してステップ254へ進み、短い100msecのサンプリング周期を設定する。作業開始に該当しなければ通常の200msecのサンプリング周期が設定される。
On the other hand, if not the initial state, the routine proceeds to step 252 where it is confirmed whether or not the work start flag has been reversed from set to reset. As described in the switching control flow (310) shown in FIGS. 10 and 11, the work start flag is continuously set until the predetermined time has elapsed after the
このようにして、作業機30を下降させて傾斜制御を再開する際には、傾斜センサ13の出力値も変動し易いため、サンプリング周期を短くして少しでも正確な傾斜角を演算しようとしている。
In this way, when the work implement 30 is lowered and the tilt control is restarted, the output value of the
傾斜センサ13の出力値の平滑処理は、タイマ割り込み処理フロー(260)で実行される。ステップ261でサンプリング時刻と判定されると、ステップ262へ進んで傾斜センサ13出力値が加算される。続くステップ263で所定回数(5回)の加算が確認されると、ステップ264へ進んで平滑フラグがセットされる。
The smoothing process of the output value of the
このようにして、作業開始時以外は、ほぼ200msec間隔で、最新の5個のサンプリング出力値が加算平均される。 In this way, the latest five sampling output values are added and averaged at intervals of approximately 200 msec except when the work is started.
図9に示すように、不感帯設定フロー(280)では、トラクタ10の対地速度に応じた不感帯を設定する。不感帯は、角速度センサ14の小さな出力変化を傾斜角の自動制御に反映させないために設定される出力範囲である。ステップ280では、車軸回転センサ46の出力が参照され、対地速度2.5km/h未満か否かが判定される。そして、対地速度2.5km/h以下に該当すれば、線図290に示すように、ステップ283へ進んで不感帯を1.5度に設定するが、該当しなければ、ステップ282へ進んで対地速度6.0km/hを越えるか否かが判定される。そして、対地速度が6.0km/hを越えていればステップ284へ進んで不感帯を2.25度に設定するが、対地速度2.5〜6.0km/hの範囲であれば、線図290に示すように、1.5度から2.25度の範囲の連続した値を設定する。
As shown in FIG. 9, in the dead zone setting flow (280), a dead zone corresponding to the ground speed of the
このようにして、トラクタ10の対地速度が増すと同じ耕作面でも角速度センサ14の出力値の変化が大きくなるが、対地速度に対応させて不感帯を大きく設定することによって、制御出力の回数が異常に増えたり、制御の反転を繰り返して不安定を引き起こしたりする事態を回避している。
In this way, when the ground speed of the
図10に示すように、切替制御フロー(310)では、角速度センサ14の出力が不感帯を越えた場合にだけ角速度センサ14の出力に基づく制御を適用している。ステップ311ではリレーボックス57の出力が参照され、ステアリング12が所定角度を越えて操作されたか否かが判定される。そして、操作されていなければステップ312へ進んでポジションレバーセンサ29の出力が参照され、ポジションレバー11が上昇状態に操作されているか否かが判定される。そして、上昇状態に操作されていれば、ステップ313へ進んでリフトアームセンサ26の出力が参照され、リフトアーム23が上昇状態か否かが判定される。
As shown in FIG. 10, in the switching control flow (310), control based on the output of the
そして、ステアリング12が所定角度を越えて操作されたか、ポジションレバーセンサ29またはリフトアームセンサ26の出力によって作業機30が上昇状態と判定される場合には、ステップ154へ進んで開始遅延タイマのカウントを開始させるが、いずれにも該当しなければ、ステップ320へ進んで開始遅延タイマが限界時間TAを越えたか否かを判定する。
If it is determined that the
開始遅延タイマは、ステップ321でカウントアップされてステップ311〜313をNOで通過し続ける時間(最後にステップ154を実行してからの経過時間、言い換えればステアリング終了または作業機30の下降完了からの経過時間)を計測しており、開始遅延タイマが限界時間TAを越えるとステップ320からステップ331へ移行して、作業開始フラグをリセットするが、限界時間TAに達するまでは、ステップ321で開始遅延タイマのカウントを継続し、ステップ322で積算フラグをリセットし、ステップ323で傾斜基準データに平滑データを取り込み、ステップ324で角速度センサ14の出力の積算データをクリアし、ステップ325で作業開始フラグをセットし、ステップ326でタイムアウトのカウントダウンを開始させ続ける。
The start delay timer counts up at
ステップ331に続くステップ332ではタイムアウトをカウントダウンし、ステップ333では、角速度センサ14の出力値から基準データを差し引いた絶対値から、不感帯の角速度を差し引いた差分データが演算される。続くステップ334では、差分データが判定され、差分データが0より大、すなわち角速度センサ14の出力が不感帯を越える場合にはステップ335へ進んでタイムアウトに達したか否かが判定される。ステップ335でタイムアウト未了の場合はステップ336へ進んで演算開始フラグがセットされ、ステップ337で更新確認タイマがクリアされ、ステップ338で積算フラグがセットされる。
In step 332 following
なお、このときの不感帯は、先に図9の不感帯設定フロー280で対地速度に応じて1.5〜2.25度の範囲に定めたものであって、図17に示されるように、実際には境界での不安定を回避すべく、ヒステリシスを加味して不感帯の内外を判定される。
Note that the dead zone at this time was previously determined in the range of 1.5 to 2.25 degrees according to the ground speed in the dead
ここで、タイムアウトは角速度センサ14の出力が長時間に渡って不感帯を越えている場合にその積算誤差を除去するために設定されるタイマである。タイムアウトは、作業機30の下降後限界時間TAが経過する直前にステップ326を通過した時点、または角速度センサ14の出力が最後に不感帯にあったステップ356でカウントダウン開始されている。ステップ335でタイムアウト完了の場合は、ステップ341へ進んで積算フラグがリセットされ、ステップ342で傾斜基準データに平滑データが取り込まれ、ステップ343で積算データがクリアされる。
Here, the timeout is a timer that is set to remove the integration error when the output of the
一方、ステップ334で差分データが0以下、すなわち角速度センサ14の出力が不感帯の内側と判定された場合、図11の部分的なフロー330へ移行する。図11に示すように、ステップ351では演算開始フラグがリセットされ、ステップ352では更新確認タイマのカウント完了か否かが確認される。
On the other hand, if it is determined in
ここで、更新確認タイマは、角速度センサ14の出力が最後に不感帯外から不感帯内へ変化して以降の経過時間を計測し、限界時間TBの経過を検知する。そして、角速度センサ14の出力が不感帯内へ変化しても角速度センサ14の出力に基づく傾斜角の演算を限界時間TBだけ継続させ、その後に傾斜センサ13の平滑データで傾斜角を演算開始させることにより、傾斜センサ13の出力が傾斜に追従する時間を確保して、安定した切替えが実現される。
Here, the update confirmation timer measures the elapsed time after the output of the
ステップ352で更新確認タイマ完了と判定された場合、ステップ355へ進んで積算フラグがリセットされ、続くステップ356でタイムアウトが設定され、続くステップ357で傾斜基準データとして平滑データが取り込まれ、続くステップ358で積算データがクリアされる。一方、ステップ352で更新確認タイマ未了と判定された場合、ステップ353へ進んで更新確認タイマのカウントが継続され、ステップ354で積算フラグがセットされる。
If it is determined in
図12に示すように、角度演算フロー(360)では、傾斜基準データを初期値にして角速度センサ14の出力を積分することにより、トラクタ10の刻々の傾斜角を演算する。
As shown in FIG. 12, in the angle calculation flow (360), the inclination angle of the
ステップ361では、一定周期でサンプリングされる角速度センサ14の差分データが加算されて毎秒積算回数で除算された後に、傾斜データと角速度データの定数比が乗算され、これに傾斜センサ13の出力の平滑データが加算される。
In
ここで、角速度センサ14の差分データを所定時間分積算して毎秒積算回数で除算すると、当該所定時間の傾斜角変化(積分値)が演算される。例えば、200msecごとにサンプリングされる角速度センサ14の出力値から基準データを差し引いた差分データ1度/秒が5回分加算されて毎秒積算回数5で除算されると、1秒間に追加された傾斜角1度が演算される。
Here, when the difference data of the
また、傾斜データと角速度データの定数比とは、出力レベルの異なる角速度センサの出力を傾斜センサの出力に適合させて加算減算を可能にするための定数であって、例えば、傾斜センサ13が10度の傾斜に対して1Vを出力し、角速度センサ14が同じ10度の傾斜に対して2Vを出力するとすれば、角速度センサの積分値を傾斜センサ13のレベルに合わせるために1/2とするものである。
The constant ratio between the tilt data and the angular velocity data is a constant for enabling addition / subtraction by adapting the output of the angular velocity sensor having different output levels to the output of the tilt sensor. If 1V is output for the inclination of the degree and the
実際の積算処理は、タイマ割込処理365によって実行される。ステップ366ではサンプリング時刻か否かが判定される。サンプリング時刻に合致すればステップ367へ進んで積算フラグのONが確認され、積算フラグがONであればステップ368へ進んで角速度データが加算されるが、積算フラグがOFFの場合はステップ369へ進んで積算データがクリアされる。なお、積算フラグは、図10のステップ338において、角速度センサ14の出力(差分データ)が不感帯の外にある限りセットされ続ける。
The actual integration process is executed by the timer interrupt process 365. In
このようにして、トラクタ10が左右に傾斜して角速度センサ14の出力が不感帯から飛び出すと、次回に不感帯へ復帰するまで角速度センサ14の出力変化を積分した傾斜角が演算され続ける。
In this way, when the
図13に示すように、傾斜制御フロー(370)では、演算された傾斜角に基づいてソレノイドバルブ51、52を作動させて作業機30の傾斜角制御を行う。ステップ371ではセンサ接続が判定され、図11に示す接続判断フロー220でセットされたセンサ接続フラグがONの場合は、ステップ372へ進んで積算フラグがセットされているか否かが確認される。そして、図10、図11に示す切替制御フロー(310、330)で積算フラグがセットされていれば、ステップ373へ進んで、図12に示す角度演算フロー(360)で演算された角速度積算データを傾斜データとして採用する。そして、続くステップ374では、図9に示す不感帯設定フロー(280)で設定した1.5〜2.25度の狭い不感帯を採用する。
As shown in FIG. 13, in the tilt control flow (370), the
一方、ステップ371でセンサ外れフラグがONの場合は、ステップ377へ進んで無事な傾斜センサデータ13だけを採用し、続くステップ378では、ステップ374の場合よりも広い不感帯を設定する。また、ステップ372で積算フラグがリセット状態の場合は、ステップ375へ進んで傾斜センサの平滑データを採用し、ステップ376でステップ374の場合よりも広い不感帯を設定する。
On the other hand, if the sensor removal flag is ON in step 371, the process proceeds to step 377 and only the safe
そして、いずれの場合もステップ379へ進み、演算された傾斜データに基づいてソレノイドバルブ51、52を作動させて作業機30の傾斜角制御を行う。
In either case, the process proceeds to step 379, and the
さて、実施例1の傾斜角自動制御装置では、角速度センサ14の出力の基準値を200msecおきに定期的に更新する基準データ処理を実行する。基準値とは、角速度センサ14の出力の右回りと左回りを判別する原点値、すなわち角速度0に対応する角速度センサ14の出力値である。また、角速度センサ14は、図16を参照して説明したように、右回り最大角速度で0V、左回り最大角速度で5Vを出力し、両者の間の角速度に対して0〜5Vのアナログ電圧を出力する。
In the tilt angle automatic control apparatus according to the first embodiment, reference data processing for periodically updating the reference value of the output of the
そして、このアナログ電圧は、図23に示すように、8ビット256段階の出力データとして傾斜制御装置40に読み取られ、出力データから基準値(以下基準データ)を差し引いて角速度(差分)が演算される。その後、差分を積算して毎秒積算回数で除することによって傾斜角が演算されることは、図17に示す角度演算フロー(360)について説明したとおりである。
Then, as shown in FIG. 23, this analog voltage is read by the
そして、基準データの更新は、
新しい基準データ=現在の基準データ+(出力データ−現在の基準データ)/2のn乗
という演算によって実行され、乗数nは、角速度レベルが大きいと次第に減らされるが、角速度レベルが小さいと次第に増加される。
And the update of the reference data
New reference data = current reference data + (output data−current reference data) / 2 is executed by an operation of n-th power. The multiplier n is gradually decreased when the angular velocity level is large, but gradually increases when the angular velocity level is small. Is done.
図14に示すように、ステップ421では、基準データ処理のタイミングに合致するか否かが判定され、合致していればステップ422へ進んでリレーボックス57の出力が参照されて、ステアリング切れ角スイッチ56がONする旋回中に合致するか否かが判定される。そして、この旋回中に合致しなければ、ステップ423以下の基準データ処理が実行される。旋回中は地軸感度の影響で角速度センサ14の出力値が変化するので、基準データ処理は行わない。
As shown in FIG. 14, in
ステップ423では、角速度センサ14の出力データから現在の基準値(以下基準データ)が差し引かれて差分が演算される。この差分は、現在の基準データに基づく現在の角速度に対応している。続くステップ424では、差分が0以上か否かが判定される。0以上の場合は、ステップ425以下のフローへ進むが、0未満の場合は図20に示すステップ445以下のフローへ進む。
In
ステップ425では、以下のフローでは使用されない減算アップカウンタおよび減算ダウンカウンタがクリアされ、続くステップ426では差分が不感帯の範囲か否かが判定される。ここで、不感帯は、図14に示す不感帯設定フロー(280)で設定した不感帯とは異なる、さらに小さな範囲の不感帯であって、現在の基準データを中心とする±16の範囲に定められている。ステップ426で差分が不感帯内と判定されるとステップ427へ進んで加算アップカウンタがクリアされて、加算ダウンカウンタがインクリメントされる。
In
加算アップカウンタ、加算ダウンカウンタは、同じ乗数nによる基準データ処理の繰り返し回数を設定するもので、例えば2と設定していれば、2回おきに乗数nが1つ増加または1つ減少される。 The addition up counter and the addition down counter are for setting the number of times the reference data processing is repeated with the same multiplier n. For example, if 2 is set, the multiplier n is incremented or decremented by 1 every two times. .
ステップ428では、加算ダウンカウンタが設定回数に達したか否かが判定され、達していなければ、ステップ431へ進んで同じ乗数nで、
差分シフト処理データ=(出力データ−現在の基準データ)/2のn乗
の演算を実行する。続くステップ432では、
新しい基準データ=現在の基準データ+差分シフト処理データ
の演算を実行する。
In
Difference shift processing data = (output data−current reference data) / 2 n-th power calculation is executed. In the
Calculation of new reference data = current reference data + difference shift processing data is executed.
一方、ステップ428で加算ダウンカウンタが設定回数に達していると、ステップ429へ進んで加算ダウンカウンタがクリアされ、続くステップ430では乗数n(基準シフト)が1つ加算される。
On the other hand, when the addition down counter reaches the set number of times in
ここで、図18に示すように、角速度センサ14の出力データが130のとき、基準データが125とすると、差分は130−125=5となって、ステップ424からステップ425へ進む。その後、ステップ426では差分5が不感帯±16の範囲と判断されてステップ427へ進み、加算ダウンカウンタが設定値に達するまで、200msecごとに、例えば乗数8で上記演算を繰り返す。
Here, as shown in FIG. 18, when the output data of the
そして、加算ダウンカウンタが設定値に達すると、ステップ429からステップ430へ進んで乗数nが1つ増やされ、次に加算ダウンカウンタが設定値に達するまで乗数9による上記演算が繰り返される。これにより、基準データは、最初は大きく次第にゆっくりと正しい基準データ130へ向かってシフトされ、なめらかに正しい基準データ130へ収束する。
Then, when the addition down counter reaches the set value, the routine proceeds from
ところで、ステップ426で差分が不感帯外と判定されるとステップ436へ進んで差分が範囲外か否かが判定される。この範囲外の領域は、図18に示すように、不感帯の外側に間隔を置いて上下に配置されている。この範囲外の領域に達するほど角速度レベルが大きい場合には、ステップ437以下のフローへ進んで乗数nを次第に減らす基準データ処理を実行するが、範囲外の領域ほど大きくない場合には、ステップ435へ進んで加算アップカウンタおよび加算ダウンカウンタをクリアし、現在の乗数nを継続した基準データ処理を実行する。
By the way, if it is determined in
従って、トラクタ10に範囲外の領域に達するほどの大きな揺れが発生している場合には1回あたりの補正量を大きくし、正しい基準データへの収束が遅れないようにしている。このようにして、乗数nは、図5に示すステップ391で8と初期設定された後は揺れのレベルに応じて2〜16の範囲でインクリメント/デクリメントされ、これに対応して差分シフト処理データは、差分データ/4から差分データ/(2の16乗)の範囲で変化する。
Accordingly, when the
ステップ437では、加算アップカウンタをインクリメントして加算ダウンカウンタをクリアする。続くステップ438では加算アップカウンタが設定回数に達したか否かが判定され、達していればステップ439へ進んで加算アップカウンタをクリアし、続くステップ440で乗数n(基準シフト)がデクリメントされる。達していなければ、ステップ231へ進んで、加算アップカウンタが設定回数に達するまで同じ乗数nの演算を繰り返す。
In
一方、ステップ424で差分が0未満と判定された場合、すなわち差分が0以上と判定された場合とは逆方向に角速度を検知しているような場合、図20に示すように、ステップ445以下のフローを実行して、基準データを引き下げる方向で、ステップ425以下とほぼ同様な基準データ処理を実行する。
On the other hand, when it is determined in
ステップ445では、以下のフローで使用しない加算アップカウンタおよび加算ダウンカウンタをクリアする。続くステップ446では差分が不感帯の範囲か否かが判定され、不感帯内と判定されるとステップ477へ進んで加算アップカウンタがクリアされて、減算ダウンカウンタがインクリメントされる。減算アップカウンタ、減算ダウンカウンタもまた、同じ乗数nによる基準データ処理の繰り返し回数を設定するものである。
In
続くステップ448では、減算ダウンカウンタが設定回数に達したか否かが判定され、達していなければ、ステップ451へ進んで同じ乗数nで、
差分シフト処理データ=(出力データ−現在の基準データ)/2のn乗
の演算を実行する。続くステップ452では、
新しい基準データ=現在の基準データ+差分シフト処理データ
の演算を実行する。
In the
Difference shift processing data = (output data−current reference data) / 2 n-th power calculation is executed. In the
Calculation of new reference data = current reference data + difference shift processing data is executed.
一方、ステップ448で減算ダウンカウンタが設定回数に達していると、ステップ449へ進んで減算ダウンカウンタがクリアされ、続くステップ450では乗数n(基準シフト)が1つ加算される。
On the other hand, if the subtraction down counter has reached the set number in
ところで、ステップ446で差分が不感帯外と判定されるとステップ456へ進んで差分が範囲外か否かが判定される。そして、範囲外に該当すればステップ457以下のフローへ進んで乗数nを次第に減らす基準データ処理を実行するが、範囲外に該当しなければステップ455へ進んで減算アップカウンタおよび減算ダウンカウンタをクリアし、現在の乗数nを継続した基準データ処理を実行する。
By the way, if it is determined in
ステップ457では、減算アップカウンタをインクリメントして減算ダウンカウンタをクリアする。続くステップ458では減算アップカウンタが設定回数に達したか否かが判定され、達していればステップ459へ進んで減算アップカウンタをクリアし、続くステップ460で乗数n(基準シフト)がデクリメントされる。達していなければ、ステップ451へ進んで、減算アップカウンタが設定回数に達するまで同じ乗数nの演算を繰り返す。
In
以上のように構成された実施例の傾斜角自動制御装置では、角速度センサの出力信号を出力データに変換し、角速度0に対応する基準データを出力データから差し引いた差分データに基づいて傾斜角を演算する傾斜自動制御装置において、基準データを(新しい基準データ←古い基準データ+差分データ×比例定数)という演算式によって刻々と修正するから、図18に示すように、正しい基準データに向かって現在の基準データが次第に収束する。
In the tilt angle automatic control device of the embodiment configured as described above, the output signal of the angular velocity sensor is converted into output data, and the tilt angle is determined based on the difference data obtained by subtracting the reference data corresponding to the
そして、基準データ、出力データ、比例定数という3つの数値データだけで修正演算が完了するから、出力データの移動平均値で基準データを更新する場合に比較して過去の出力データが不要な分、演算負荷が軽減される。そして、出力データの移動平均値を求める場合に比較して、ピーク検出の必要が無い分、出力データのサンプリング間隔を拡大しても修正精度がさほど低下しないから、サンプリング回数を減らして演算負荷を大幅に削減できる。 And since the correction calculation is completed with only three numerical data such as reference data, output data, and proportionality constant, the past output data is unnecessary compared with the case where the reference data is updated with the moving average value of the output data, Calculation load is reduced. Compared with the case where the moving average value of the output data is obtained, the correction accuracy does not decrease much even if the sampling interval of the output data is expanded because the peak detection is not required. It can be greatly reduced.
従って、角速度センサの基準値を刻々と補正する際の演算負荷や必要なメモリ容量が小さく済み、しかも、角速度センサ出力の読み込み頻度が少なくても補正の精度が必要十分に確保される。これにより、低速でメモリ容量の少ないマイコン素子等を採用して小型安価低消費電力を実現でき、例えば、メモリやADコンバータをCPUとともに1チップ化した8ビットマイコン素子でも、基準データの修正を含む傾斜角自動制御全体の演算処理を賄うことができる。 Therefore, the calculation load and the necessary memory capacity for correcting the reference value of the angular velocity sensor every moment are small, and the correction accuracy is sufficiently and sufficiently ensured even if the angular velocity sensor output is read less frequently. As a result, it is possible to realize a small size, low cost and low power consumption by adopting a microcomputer element or the like having a low memory capacity at a low speed. Computation processing of the entire tilt angle automatic control can be provided.
また、その比例定数は、差分データの絶対値が小さい領域(不感帯)では、次第に小さく置き換えられ、一方、差分データの絶対値が大きい領域(範囲外)では、次第に大きく置き換えられるから、角度センサの基準値が大きく変化した場合でも、基準データが速やかに正しい基準値に相当する基準データへ収束する。 In addition, the proportionality constant is gradually replaced in the area where the absolute value of the difference data is small (dead zone), while it is gradually replaced in the area where the absolute value of the difference data is large (outside the range). Even when the reference value changes greatly, the reference data quickly converges to the reference data corresponding to the correct reference value.
また、比例定数は、1/(2のn乗)と定めて、乗数nを段階的に増減することにより比例定数を変化させるから、比例定数の演算と、差分データとの乗算とをデジタル回路で簡単に演算でき、演算負荷はさらに軽減される。 Further, the proportionality constant is set to 1 / (2 to the nth power), and the proportionality constant is changed by increasing or decreasing the multiplier n stepwise. Therefore, the calculation of the proportionality constant and the multiplication with the difference data are performed in the digital circuit. Can be easily calculated, and the calculation load is further reduced.
また、角速度センサの出力データに不感帯を設定し、差分データが不感帯内であれば傾斜センサの出力データの移動平均値を傾斜角データとし、差分データが不感帯の外側であれば角速度センサの出力データから基準データを差し引いた差分データを積算して傾斜角データを演算する傾斜自動制御装置において、不感帯を走行機体の対地速度に応じて変化させて対地速度が高い場合は低い場合よりも広い不感帯を設定し、かつ、基準データを(新しい基準データ←古い基準データ+差分データ×比例定数)という演算式によって刻々と修正する構成を採用したので、不感帯の外側では、常に基準データが更新された高精度の差分データを積算して正確な傾斜角の演算ができるとともに、基準データの更新を含む角速度演算に使用する合計のデータ数が少なくて済み、演算もデジタル処理が容易な数式によってなされ、しかも、演算頻度を下げても傾斜角制御の制度が十分確保できるようになった。従って、低速でメモリ容量の少ないマイコン素子等を採用して小型安価低消費電力を実現でき、例えば、メモリやADコンバータをCPUとともに1チップ化した8ビットマイコン素子でも、基準データの修正を含む傾斜角自動制御全体の演算処理を賄うことができる。 In addition, a dead zone is set in the output data of the angular velocity sensor, and if the difference data is within the dead zone, the moving average value of the output data of the tilt sensor is the tilt angle data, and if the difference data is outside the dead zone, the output data of the angular velocity sensor In the automatic tilt control device that calculates the tilt angle data by summing the difference data obtained by subtracting the reference data from the vehicle, the dead zone is changed according to the ground speed of the vehicle and the dead zone is wider when the ground speed is high than when the ground speed is low. Because the configuration is set and the reference data is corrected every moment by the formula (new reference data ← old reference data + difference data × proportional constant), the reference data is constantly updated outside the dead zone. Accurate difference data can be accumulated to calculate the correct tilt angle, and the total data used for the angular velocity calculation including the update of the reference data Fewer data number, operation also made by easy formulas digital processing, moreover, system tilt angle control even by lowering the operation frequency can now be sufficiently ensured. Therefore, it is possible to realize a small size, low cost and low power consumption by adopting a microcomputer element with a low memory capacity at a low speed. Computation processing of the entire angle automatic control can be covered.
本発明は、角速度センサの基準値を刻々と修正する際に用いられる演算式に特徴を有するから、ロータリ式耕耘ユニットとトラクタの組み合わせに限定されず、走行機体に作業部を連結して作業部の左右傾斜角度を自動調整する作業車輌に広く利用可能である。 Since the present invention has a feature in the arithmetic expression used when the reference value of the angular velocity sensor is corrected every moment, it is not limited to the combination of the rotary type tillage unit and the tractor, and the working unit is connected to the traveling machine body. It can be widely used for a working vehicle that automatically adjusts the right / left inclination angle.
10 トラクタ(走行機体)
11 ポジションレバー
12 ステアリング
13 傾斜センサ
14 角速度センサ
15 運転席
16 操作パネル
20 昇降リンク機構
21 アッパリンク
22 ロアリンク
23 リフトアーム
24 リフトロッド
25 リフトロッドシリンダ
26 リフトアームセンサ
27 リフトシリンダ
28 リフトロッドセンサ
29 ポジションレバーセンサ
30 作業機
31 回転刃
40 傾斜自動制御装置
41 自動切替ボリューム
42 作業機切替ボリューム
43 傾き設定ボリューム
44 操作スイッチ群
45 エンジン回転センサ
46 車軸回転センサ
47 リモコンスイッチ
51、52 ソレノイドバルブ
54 スピーカー
55 ランプ
56 ステアリング切れ角検出スイッチ
10 Tractor (running vehicle)
11
Claims (3)
前記左右傾斜角を検出する手段として傾斜センサと角速度センサとを備え、
前記角速度センサの基準値を、
今までの基準値+(角速度センサの検出値−今までの基準値)/2n
により算出し、
上記指数nは、傾斜自動制御の作動中に変更可能な整数である、
ことを特徴とする作業車輌における傾斜自動制御装置。 The working unit is supported on the traveling machine body so as to be tiltable left and right, and the right and left tilt angles of the traveling machine body or the working unit are detected, and the working unit is held in a preset posture regardless of the left and right tilting of the traveling machine body. In a working vehicle equipped with an automatic tilt control device for controlling
As a means for detecting the left and right tilt angle, comprising a tilt sensor and an angular velocity sensor,
The reference value of the angular velocity sensor is
Conventional reference value + (detection value of angular velocity sensor-conventional reference value) / 2 n
Calculated by
The index n is an integer that can be changed during operation of the automatic tilt control.
An automatic tilt control apparatus for a working vehicle.
ことを特徴とする請求項1記載の作業車輌における傾斜自動制御装置。 The index n is changed so as to increase when the difference (detection value of the angular velocity sensor−the reference value so far) is small, and decreases when the difference is large. change,
The automatic tilt control apparatus for a working vehicle according to claim 1.
かつ前記不感帯が、車速により変化する、
ことを特徴とする請求項1記載の作業車輌における傾斜自動制御装置。
Based on the fact that the difference data of (the detected value of the angular velocity sensor−the reference value) exceeds the dead zone, the inclination is automatically controlled by the inclination angle based on the integral value of the difference data,
And the dead zone changes depending on the vehicle speed,
The automatic tilt control apparatus for a working vehicle according to claim 1.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2004290804A JP4373315B2 (en) | 2004-10-01 | 2004-10-01 | Inclination automatic control device for work vehicle |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2004290804A JP4373315B2 (en) | 2004-10-01 | 2004-10-01 | Inclination automatic control device for work vehicle |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2006101737A true JP2006101737A (en) | 2006-04-20 |
JP4373315B2 JP4373315B2 (en) | 2009-11-25 |
Family
ID=36372146
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2004290804A Expired - Fee Related JP4373315B2 (en) | 2004-10-01 | 2004-10-01 | Inclination automatic control device for work vehicle |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP4373315B2 (en) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2014112090A (en) * | 2013-12-17 | 2014-06-19 | Seiko Epson Corp | Physical quantity detection device, abnormality diagnostic system for physical quantity detection device, and abnormality diagnostic method for physical quantity detection device |
CN113739794A (en) * | 2020-05-28 | 2021-12-03 | 精工爱普生株式会社 | Attitude estimation device and method, sensor module, measurement system, and moving object |
-
2004
- 2004-10-01 JP JP2004290804A patent/JP4373315B2/en not_active Expired - Fee Related
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2014112090A (en) * | 2013-12-17 | 2014-06-19 | Seiko Epson Corp | Physical quantity detection device, abnormality diagnostic system for physical quantity detection device, and abnormality diagnostic method for physical quantity detection device |
CN113739794A (en) * | 2020-05-28 | 2021-12-03 | 精工爱普生株式会社 | Attitude estimation device and method, sensor module, measurement system, and moving object |
CN113739794B (en) * | 2020-05-28 | 2024-01-12 | 精工爱普生株式会社 | Posture estimating device and method, sensor module, measuring system, and moving object |
Also Published As
Publication number | Publication date |
---|---|
JP4373315B2 (en) | 2009-11-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP4373315B2 (en) | Inclination automatic control device for work vehicle | |
JP4179935B2 (en) | Signal discrimination method for display device | |
JP4500644B2 (en) | Inclination automatic control device for work vehicle | |
JP5802026B2 (en) | Work vehicle | |
JP3356943B2 (en) | Riding rice transplanter | |
KR100436641B1 (en) | Rolling controller of working vehicle | |
JP3717411B2 (en) | Sensor zero correction device for work equipment | |
JP3274352B2 (en) | Paddy field machine | |
JP3870611B2 (en) | Horizontal control device | |
JP3648749B2 (en) | Tractor attitude control device | |
JP2022006511A (en) | combine | |
JPH07274631A (en) | Controlling device for rice planter | |
JP3418036B2 (en) | Rice transplanter | |
JP3688211B2 (en) | Rolling control device for work equipment | |
JP4500659B2 (en) | Mobile farm machine | |
JP3916555B2 (en) | Agricultural machine rolling control device | |
JP4233902B2 (en) | Controller that controls the drive of multiple actuators | |
JP4500660B2 (en) | Mobile farm machine | |
JP4594922B2 (en) | Agricultural machine rolling control device | |
JPH11289810A (en) | Tractor | |
JP2022006512A (en) | combine | |
JP2000324906A (en) | Horizontal control device | |
JP3082326B2 (en) | Hydraulic controller for power farm machine | |
JPH0585121B2 (en) | ||
JP2007116979A (en) | Agricultural working vehicle |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20070330 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20081006 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20081111 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20081222 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20090825 |
|
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20090903 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120911 Year of fee payment: 3 |
|
R151 | Written notification of patent or utility model registration |
Ref document number: 4373315 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120911 Year of fee payment: 3 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120911 Year of fee payment: 3 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130911 Year of fee payment: 4 |
|
S533 | Written request for registration of change of name |
Free format text: JAPANESE INTERMEDIATE CODE: R313533 |
|
R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
LAPS | Cancellation because of no payment of annual fees |