JP3568597B2

JP3568597B2 - Mobile farm machine tilt detection device

Info

Publication number: JP3568597B2
Application number: JP25873194A
Authority: JP
Inventors: 弘章山崎
Original assignee: MITSUBISHI NOUKI KABUSHIKI KAISHA
Current assignee: MITSUBISHI NOUKI KABUSHIKI KAISHA
Priority date: 1994-09-28
Filing date: 1994-09-28
Publication date: 2004-09-22
Anticipated expiration: 2019-09-22
Also published as: JPH0889011A

Description

【０００１】
【産業上の利用分野】
本発明は、トラクタ、移植機、管理機、収穫機等の移動農機の傾斜検出装置に関するものである。
【０００２】
【従来技術及び発明が解決しようとする課題】
一般に、この種移動農機のなかには、走行機体もしくは作業部の傾斜角検出に基づいて傾斜姿勢制御を行うものがあるが、従来、傾斜角検出用センサとしては振子センサ等の重力式傾斜センサが用いられていた。しかるに、重力式傾斜センサにおいては、振子等の検出体に慣性力が作用するため、急激な傾斜変動に追随することができず、応答性に劣るのが実状であった。そこで、高速応答性を具備するジャイロを用いて傾斜角検出を行うことが提案されるが、ジャイロにおいては、アンプ等を構成する電子部品の温度変化等に伴って特性変化が生じる許りか、ジャイロ出力から検出角を演算する際にも誤差が生じるため、長時間に亘って傾斜角検出を継続した場合には、誤差が累積されて検出精度が低下するという不都合がある。
【０００３】
【課題を解決するための手段】
本発明は、上記の如き実情に鑑みこれらの欠点を一掃することができる移動農機の傾斜検出装置を提供することを目的として創作されたものであって、走行機体もしくは作業部の傾斜角検出に基づいて傾斜姿勢制御を行う移動農機であって、該移動農機に、走行機体もしくは作業部の絶対傾斜角を検出する重力式傾斜センサと、走行機体もしくは作業部の相対傾斜角を検出する角速度または角加速度出力タイプのジャイロと、ジャイロが出力した角速度または角加速度が安定状態である場合に、重力式傾斜センサの検出角を最終的な検出角とし、かつ重力式傾斜センサの検出角を基準値として格納する安定時検出手段と、ジャイロが出力した角速度または角加速度が変動状態である場合に、前記基準値にジャイロが出力する角速度または角加速度に基づいて演算される相対傾斜角を加算して最終的な検出角とする変動時検出手段とを設けたことを特徴とする移動農機の傾斜検出装置である。
そして本発明は、この構成によって、傾斜角検出の応答性および検出精度を向上させることができるようにしたものである。
【０００４】
【実施例】
次に、本発明の一実施例を図面に基づいて説明する。図面において、１はトラクタの走行機体であって、該走行機体１の後部には、昇降リンク機構２を介してロータリ等の作業部３が昇降自在に連結されている。そして作業部３は、リフトシリンダ４の伸縮に伴うリフトアーム５の上下揺動に連動して昇降する一方、リフトロッドシリンダ６の伸縮に基づいて左右傾斜するが、これらの基本構成は何れも従来通りである。
【０００５】
７は前記走行機体１に設けられる重力式傾斜センサであって、該重力式傾斜センサ７は、機体左右方向（ローリング方向）の絶対傾斜角を検出すべく取付けられているが、さらに走行機体１には、重力式傾斜センサ７と同一方向の相対傾斜角を検出するジャイロ８（角速度出力タイプ）が併設されている。
【０００６】
さらに、前記走行機体１には、マイクロコンピュータ等を用いて構成される制御部９が設けられるが、該制御部９は、前述した重力式傾斜センサ７およびジャイロ８に加え、傾斜自動制御をＯＮ−ＯＦＦするための傾斜自動スイッチ１０、傾斜自動制御の目標傾斜を設定するための傾斜設定ボリューム１１、リフトロッドシリンダ６の作動長を検出するためのロッド長センサ１２、走行状態を検出するための車軸回転センサ１３（走行クラッチの断続検出スイッチでも可）等から信号を入力する一方、リフトロッドシリンダ６を作動させる電磁バルブの伸長用ソレノイド１４、縮小用ソレノイド１５等に対して作動指令を出力すべく構成されている。即ち、制御部９は、前記重力式傾斜センサ７およびジャイロ８の出力に基づいて走行機体１の左右傾斜角を検出する傾斜角検出制御、前記検出角と目標傾斜角との比較に基づいて作業部３を自動的に傾斜制御する傾斜自動制御等の各種制御を行うが、以下、本発明が要旨とする傾斜角検出制御をフローチャートに基づいて詳述する。
【０００７】
さて、傾斜角検出制御においては、まず、所定時間毎にサンプリングされる重力式傾斜センサ７の検出データに基づいて傾斜角（絶対傾斜角）Ｓを演算した後、走行状態を判断するが、走行停止中である場合には、ジャイロ８の中立値（角速度が「０」のときのジャイロデータ）を格納すると共に、最終的な検出傾斜角（傾斜自動制御の参照データ）を格納する「傾斜角」変数に前記傾斜角Ｓを格納するようになっている。
【０００８】
一方、機体が走行中である場合には、ジャイロ８が出力した角速度が略「０」であるか否かを判断し、該判断がＹＥＳの場合には、所定のタイマ時間（重力式傾斜センサ７の応答時間に略対応）が経過したか否かを判断する。つまり、角速度が略「０」である状態が所定時間継続したか否かを判断するが、該判断がＹＥＳの場合には、前記傾斜角Ｓを「基準値」変数および「傾斜角」変数に格納するようになっている。
【０００９】
また、角速度が略「０」の状態でない場合、もしくは略「０」の状態が所定時間未満である場合には、ジャイロ８が出力する角速度データに基づいて相対傾斜角Ｇを演算する。即ち、ジャイロ８の検出データから前記中立値を減算すると共に、その差分データをサンプリング時間毎に積算して前記相対傾斜角Ｇを得るが、相対傾斜角Ｇを演算した後は、相対傾斜角Ｇに前記基準値を加算して「傾斜角」変数に格納するようになっている。
【００１０】
叙述の如く構成された本発明の実施例において、走行機体１の傾斜角を重力式傾斜センサ７およびジャイロ８を用いて検出し、検出した傾斜角に基づいて作業部３を自動的に傾斜制御することになるが、ジャイロ８が検出した角速度が略「０」の状態では、重力式傾斜センサ７の検出データに基づいて演算される傾斜角Ｓを適用するため、誤差の小さい絶対傾斜角が得られることになる。一方、前記角速度が略「０」でない状態では、ジャイロ８が出力する角速度データに基づいて相対傾斜角Ｇを演算すると共に、演算した相対傾斜角Ｇに基準値を加算して傾斜角を得るため、応答性に優れた傾斜角検出を行うことが可能になり、しかも、前記基準値は、角速度が略「０」になる毎に格納される絶対傾斜角（傾斜角Ｓ）であるため、特性変化による誤差や演算による誤差が累積される不都合も解消することができる。従って、重力式傾斜センサ７およびジャイロ８の長所を有効利用することが可能になり、その結果、傾斜角検出の応答性および検出精度を同時に向上させることができる。
【００１１】
尚、本発明は、前記実施例に限定されないものであることは勿論であって、例えば前記実施例では、角速度出力タイプのジャイロを用い、角速度が略「０」である状態を基準として検出角の演算方式を切換えているが、角加速度出力タイプのジャイロを用い、角加速度が略「０」である状態を基準として検出角の演算方式を切換えることも可能である。
【００１２】
【作用効果】
以上要するに、本発明は叙述の如く構成されたものであるから、走行機体もしくは作業部の傾斜角検出に基づいて傾斜姿勢制御を行うものでありながら、重力式傾斜センサおよびジャイロを備えており、そして、ジャイロの検出角が安定状態である場合には、重力式傾斜センサの検出角を最終的な検出角とする一方、ジャイロから出力される角速度または角加速度が変動状態である場合には、ジャイロ安定時に格納した重力式傾斜センサの検出角（基準値）にジャイロから出力される角速度または角加速度データに基づいて演算される相対傾斜角に前記格納した重力式傾斜センサの検出角を加算して最終的な検出角とするため、それぞれの長所を生かした傾斜角検出が可能になる。即ち、ジャイロ安定時には、誤差の少ない絶対傾斜角を検出できる一方、ジャイロ変動時には、応答性に優れた傾斜角検出を行うことができ、しかも、ジャイロの基準値は、ジャイロの検出角が安定する毎に更新されるため、特性変化による誤差や演算による誤差が累積される不都合も解消することができる。
【図面の簡単な説明】
【図１】トラクタの側面図である。
【図２】制御部の入出力を示すブロック図である。
【図３】傾斜角検出制御のフローチャートである。
【符号の説明】
１走行機体
３作業部
７重力式傾斜センサ
８ジャイロ
９制御部[0001]
[Industrial applications]
The present invention relates to a tilt detection device for a mobile farm machine such as a tractor, a transplanter, a management machine, and a harvester.
[0002]
2. Prior Art and Problems to be Solved by the Invention
In general, among this kind of mobile farming machine, there is a type that performs a tilt posture control based on detection of a tilt angle of a traveling machine body or a working unit, but conventionally, a gravity type tilt sensor such as a pendulum sensor is used as a tilt angle detection sensor. Had been. However, in a gravity type tilt sensor, since inertial force acts on a detection body such as a pendulum, it is impossible to follow a rapid change in tilt and the response is inferior. Therefore, it has been proposed to perform inclination angle detection using a gyro having a high-speed response. In the gyro, is it possible to change the characteristics of the electronic components constituting the amplifier and the like due to a temperature change or the like? Since an error also occurs when calculating the detection angle from the output, if the inclination angle detection is continued for a long time, there is a disadvantage that the error is accumulated and the detection accuracy is reduced.
[0003]
[Means for Solving the Problems]
SUMMARY OF THE INVENTION The present invention has been made in view of the above-described circumstances, and has been created with the object of providing a tilt detection device for a mobile agricultural machine that can eliminate these disadvantages. A moving agricultural machine that performs tilt posture control based on the moving agricultural machine, a gravitational inclination sensor that detects an absolute inclination angle of the traveling body or the working unit, and an angular velocity or an angular velocity that detects a relative inclination angle of the traveling body or the working unit. When the angular acceleration output type gyro and the angular velocity or angular acceleration output by the gyro are in a stable state, the detection angle of the gravity type inclination sensor is used as the final detection angle, and the detection angle of the gravity type inclination sensor is used as the reference value. and stability when detecting means for storing as, when the gyro is an angular velocity or angular acceleration and output is variation state, the angular velocity or angular pressurized gyro outputs to the reference value An inclination detecting device of the mobile agricultural machine is characterized in that by adding the relative inclination angle that is calculated is provided a variation time detection means for the final detection angle based on time.
According to the present invention, the responsiveness and the detection accuracy of the tilt angle detection can be improved by this configuration.
[0004]
【Example】
Next, an embodiment of the present invention will be described with reference to the drawings. In the drawings, reference numeral 1 denotes a traveling body of a tractor, and a working unit 3 such as a rotary is connected to a rear portion of the traveling body 1 via a lifting link mechanism 2 so as to be able to move up and down. The working unit 3 moves up and down in conjunction with the vertical swing of the lift arm 5 accompanying the expansion and contraction of the lift cylinder 4, and tilts left and right based on the expansion and contraction of the lift rod cylinder 6. It is on the street.
[0005]
Numeral 7 denotes a gravity type inclination sensor provided on the traveling body 1. The gravity type inclination sensor 7 is attached to detect an absolute inclination angle in the lateral direction (rolling direction) of the aircraft. Is provided with a gyro 8 (angular velocity output type) for detecting a relative inclination angle in the same direction as the gravity type inclination sensor 7.
[0006]
Further, the traveling body 1 is provided with a control unit 9 configured using a microcomputer or the like. The control unit 9 turns on the automatic inclination control in addition to the gravity type inclination sensor 7 and the gyro 8 described above. A tilt automatic switch 10 for turning off, a tilt setting volume 11 for setting a target tilt for automatic tilt control, a rod length sensor 12 for detecting an operation length of the lift rod cylinder 6, and a travel state for detecting a running state. While a signal is input from an axle rotation sensor 13 (an on / off detection switch of the traveling clutch is also possible), an operation command is output to an extension solenoid 14 and a reduction solenoid 15 of an electromagnetic valve for operating the lift rod cylinder 6. It is configured as follows. That is, the control unit 9 performs tilt angle detection control for detecting the left and right tilt angles of the traveling body 1 based on the outputs of the gravitational tilt sensor 7 and the gyro 8, and performs operation based on a comparison between the detected angle and the target tilt angle. Various controls such as automatic tilt control for automatically controlling the tilt of the section 3 are performed. Hereinafter, the tilt angle detection control, which is the gist of the present invention, will be described in detail based on a flowchart.
[0007]
In the inclination angle detection control, first, the inclination state (absolute inclination angle) S is calculated based on the detection data of the gravity type inclination sensor 7 sampled every predetermined time, and then the traveling state is determined. When the gyro 8 is stopped, the neutral value (gyro data when the angular velocity is “0”) is stored and the final detected tilt angle (reference data for automatic tilt control) is stored. The inclination angle S is stored in a variable.
[0008]
On the other hand, when the aircraft is traveling, it is determined whether or not the angular velocity output by the gyro 8 is substantially “0”. If the determination is YES, a predetermined timer time (gravity-type tilt sensor 7 (substantially corresponding to the response time 7). That is, it is determined whether or not the state where the angular velocity is substantially “0” has continued for a predetermined time. If the determination is YES, the inclination angle S is set to the “reference value” variable and the “inclination angle” variable. It is designed to be stored.
[0009]
If the angular velocity is not substantially “0”, or if the state of substantially “0” is less than the predetermined time, the relative inclination angle G is calculated based on the angular velocity data output by the gyro 8. That is, the subtracting the neutral value from the detection data of the gyro 8, wherein at obtaining a relative inclination angle G by multiplying the difference data for each sampling time, after the calculating of the relative inclination angle G, the relative inclination angle G Is added to the reference value and stored in a “tilt angle” variable.
[0010]
In the embodiment of the present invention configured as described above, the inclination angle of the traveling body 1 is detected by using the gravity type inclination sensor 7 and the gyro 8, and the working unit 3 is automatically inclined based on the detected inclination angle. However, in the state where the angular velocity detected by the gyro 8 is substantially “0”, since the inclination angle S calculated based on the detection data of the gravity type inclination sensor 7 is applied, the absolute inclination angle with a small error is Will be obtained. On the other hand, when the angular velocity is not substantially "0", while calculates the relative tilt angle G on the basis of the angular velocity data gyro 8 outputs, to obtain the inclination angle by adding the reference value of the calculated relative angle of inclination G Therefore, it is possible to perform the inclination angle detection with excellent responsiveness, and the reference value is an absolute inclination angle (inclination angle S) stored every time the angular velocity becomes substantially “0”. The inconvenience of accumulating errors due to characteristic changes and errors due to calculations can be eliminated. Therefore, the advantages of the gravity type tilt sensor 7 and the gyro 8 can be effectively used, and as a result, the response and the detection accuracy of the tilt angle detection can be improved at the same time.
[0011]
The present invention is, of course, not limited to the above-described embodiment. For example, in the above-described embodiment, an angular velocity output type gyro is used, and a detection angle is set based on a state where the angular velocity is substantially “0”. However, it is also possible to use a gyro of an angular acceleration output type and switch the calculation method of the detected angle based on the state where the angular acceleration is substantially "0".
[0012]
[Effects]
In short, since the present invention is configured as described above, the present invention includes a gravity type tilt sensor and a gyro while performing tilt attitude control based on detection of the tilt angle of the traveling body or the working unit, Then, when the detection angle of the gyro is in a stable state, the detection angle of the gravity-type inclination sensor is used as the final detection angle, while when the angular velocity or angular acceleration output from the gyro is in a fluctuating state, The detected angle of the stored gravity type tilt sensor is added to the relative tilt angle calculated based on the angular velocity or angular acceleration data output from the gyro to the detected angle (reference value) of the gravity type tilt sensor stored when the gyro is stable. In this case, the inclination angle can be detected by making use of the respective advantages. In other words, when the gyro is stable, an absolute inclination angle with a small error can be detected. On the other hand, when the gyro fluctuates, an inclination angle detection with excellent responsiveness can be performed. In addition, the gyro reference value is such that the gyro detection angle is stable. Since it is updated every time, the inconvenience of accumulating errors due to characteristic changes and errors due to calculations can be eliminated.
[Brief description of the drawings]
FIG. 1 is a side view of a tractor.
FIG. 2 is a block diagram showing input and output of a control unit.
FIG. 3 is a flowchart of tilt angle detection control.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 Traveling body 3 Working part 7 Gravity type inclination sensor 8 Gyro 9 Control part

Claims

走行機体もしくは作業部の傾斜角検出に基づいて傾斜姿勢制御を行う移動農機であって、該移動農機に、走行機体もしくは作業部の絶対傾斜角を検出する重力式傾斜センサと、走行機体もしくは作業部の相対傾斜角を検出する角速度または角加速度出力タイプのジャイロと、ジャイロが出力した角速度または角加速度が安定状態である場合に、重力式傾斜センサの検出角を最終的な検出角とし、かつ重力式傾斜センサの検出角を基準値として格納する安定時検出手段と、ジャイロが出力した角速度または角加速度が変動状態である場合に、前記基準値にジャイロが出力する角速度または角加速度に基づいて演算される相対傾斜角を加算して最終的な検出角とする変動時検出手段とを設けたことを特徴とする移動農機の傾斜検出装置。A moving agricultural machine that performs inclination posture control based on detection of an inclination angle of a traveling machine or a working unit, wherein the moving agricultural machine includes a gravity type inclination sensor that detects an absolute inclination angle of the traveling machine or the operating unit, A gyro of an angular velocity or angular acceleration output type for detecting the relative inclination angle of the section, and when the angular velocity or angular acceleration output by the gyro is in a stable state, the detection angle of the gravity type inclination sensor is the final detection angle; and and stability when detecting means for storing the detection angle of the gravity gradient sensor as a reference value, when the gyro is an angular velocity or angular acceleration and output is variation state, on the basis of the angular velocity or angular acceleration gyro outputs to the reference value A change detecting means for adding a calculated relative tilt angle to obtain a final detected angle;