JP2003092902A - Device for compensating sensor zero point in implement - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a device for compensating a sensor zero point in an implement capable of calculating accurate zero point even when time fluctuation of the zero point is large. SOLUTION: This device is equipped with a means for calculating the average of the fluctuation of the zero point based on signals from the angular velocity and a means for estimating changing tendency of average values and a time-lag of the average values to zero point from series of average values and predicting and calculating the present zero point.

Description

Description: BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a sensor which can be used for tractor rolling control and automatic tillage depth control, or rice transplanter rolling control and automatic elevating control. The present invention relates to a zero point compensator. 2. Description of the Related Art In a control using a sensor whose zero point fluctuates (drifts) due to a temperature change or the like, it is necessary to perform zero point compensation. A plurality of samplings are performed to calculate an average value, and the average value is calculated. Zero point compensating means for replacing with a new zero point is often employed. According to the zero point compensating means, if the average value is calculated at an appropriate timing and sampling number, the zero point in the section can be calculated quite accurately. Since the data to be sampled is past data, an accurate zero cannot be calculated if the time variation of the zero is large. For example, as shown in FIG. 5, when the actual fluctuation of the zero is A (n), the zero obtained by sampling and averaging becomes B (n). The present invention has been made in view of such a point, and it is an object of the present invention to provide a sensor zero point compensating apparatus which can calculate an accurate zero point even when the time variation of the zero point is large. I have. [0005] In order to achieve the above object, the present invention employs the following configuration. That is, according to the first aspect of the present invention, there is provided means for averaging and calculating the variation of a zero based on a signal from a sensor, a change tendency of the average from a sequence of the average, and a time for the zero of the average. The present invention is characterized in that a means is provided for estimating the delay and predicting and calculating the current zero point. According to the above configuration, for example, as shown in FIG. 5, a zero point B obtained by sampling average of the past 20 seconds with respect to the fluctuation of the actually detected zero point A (n) is obtained.
If the fluctuation of (n) is delayed by about half (10 seconds) of the sampling time, and the approximate zero at the present time is C (n),
The approximate zero point C (n) is given by: C (n) = B (n) + [change of B (n) per time] × [time delay of B (n)] = B (n) + ｛[B (n ) −B (n−1)] / 10 seconds｝ × 10 seconds = [B (n) × 2] −B (n−1), and the characteristic of the predicted zero point C (n) is
This is close to the characteristic of the actual zero A (n). Therefore, according to the present invention, an accurate zero point can be calculated even when the time variation of the zero point is large due to a temperature drift, and highly accurate control can be performed using a sensor which can be obtained at relatively low cost. It became so. Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 shows a rear part of an agricultural tractor as an example of a working machine. A transmission case 3 supporting two rear wheels is provided at a rear part of the tractor main body 1 as a traveling body. Link 4
a and a rotary tillage device (an example of a ground working device) 5 is connected via a three-point link mechanism 4 including a pair of left and right lower links 4b. At the upper part of the transmission case 3, there is provided a pair of lift arms 7 which are driven to swing up and down by a lift cylinder 6. These lift arms 7 and the lower link 4b are connected to the lift rod 8 and the double-acting hydraulic cylinder. Through a rolling cylinder 9. As shown in FIG. 2, the electromagnetic control valve 11 connected to the lift cylinder 6 is operated by the control device 10 to drive the rotary tillage device 5 up and down by the lift cylinder 6 and the lift arm 7. The electromagnetic control valve 12 connected to the rolling cylinder 9 is operated by the control device 10, and the expansion and contraction operation of the rolling cylinder 9 causes the rotary tilling device 5 to connect with the lower link 4 b opposite to the rolling cylinder 9. It is driven to roll around. [0011] The agricultural tractor is a rotary tilling device 5.
Control to maintain the tilling depth of the cultivator at a set value, position control to arbitrarily adjust the height of the rotary tilling device 5 with respect to the tractor main unit 1, and tilt angle of the rotary tilling device 5 with respect to the horizontal plane in the left-right direction to the set angle. It is possible to maintain rolling control. At the rear of the rotary tillage device 5, a rear cover 15 for suppressing the tillage marks is provided in a vertically swayable and downwardly biased state, and a tillage depth sensor for detecting the vertical swing angle of the rear cover 15 is provided. A detection signal is input to the control device 10. On the other hand, the control device 10 is connected with a tillage setting device 17 composed of a dial-operated potentiometer and an on / off switch 18 for turning on / off the automatic tillage depth control. The electromagnetic control valve 1 is controlled so that the detection value of the depth sensor 16 is balanced with the set value of the tillage depth setting device 17.
1 is operated, and the rotary tillage device 5 is automatically moved up and down by the lift cylinder 6, so that the actual tillage depth is stably maintained at the depth corresponding to the set value of the tillage depth setting device 17. ing. The control device 10 includes a lift arm 7
An angle sensor 19 for detecting the vertical angle of the vehicle and a position setting device 21 operated by a position lever 20 are connected. When the on / off switch 18 is turned off to stop the automatic plowing control, the position control is stopped. Is executed until the value detected by the angle sensor 19 is balanced with the value set by the position setting device 21.
Is operated to hold the lift cylinder 6 at that position. When the position lever 20 is largely moved in the upward direction during the automatic cultivation depth control with the ON / OFF switch 18 set to "ON", the detection value of the angle sensor 19 corresponding to the set cultivation depth of the cultivation depth setting device 17 is obtained. And position setting device 2
When the target value of the position setting device 21 is higher than the target value of 1, the position control is preferentially operated. Therefore, in the plowing operation by automatic plowing depth control, when turning the body direction at the ridge, the rotary tillage device 4 can be lifted to the ground by operating the position lever 20 to the upper limit, and after the body direction change. By operating the position lever 20 to the lower limit, the automatic plowing depth control at the plowing depth set by the plowing depth setting device 17 can be restarted. This agricultural tractor is provided with a rolling control means for performing a rolling drive so as to maintain the horizontal tilt angle of the rotary tillage device 5 with respect to the horizontal plane at a set angle, and a dial operation type connected to the control device 10. The set angle of the rotary tilling device 5 in the left-right direction can be arbitrarily changed by adjusting the tilt setting device 25 composed of a potentiometer. In this rolling control, in addition to the inclination setting device 25, a weight type inclination sensor 26 for detecting the left-right inclination angle of the tractor main body (traveling body) 1 and the left-right inclination direction of the tractor main body 1 A vibration gyro-type angular velocity sensor 27 that detects the angular velocity of the rolling cylinder 9 and a stroke sensor 28 that detects the operating length of the rolling cylinder 9 are used. That is, as shown in the block diagram of FIG.
Is calculated in the left-right direction of the tractor main body 1
Is at this inclination angle θ, the target cylinder length L0 of the rolling cylinder 9 necessary for setting the rotary tilling apparatus 5 to the set angle by the inclination setting device 25 is calculated, and the length L of the rolling cylinder 9 is set to the target cylinder length L0. Cylinder length L
Feedback control is performed so as to approach 0, and the electromagnetic control valve 12 is operated. FIG. 4 shows an inclination sensor 26 and an angular velocity sensor 2.
7 is a control block diagram for calculating the left-right inclination angle θ of the tractor main unit 1 based on the information from FIG. As can be seen from the figure, a form is employed in which the inclination angle is calculated by integrating the signal from the angular velocity sensor 27 and the error is corrected by the signal from the inclination sensor 26. That is, a sensor zero point correction process for updating and correcting the zero point of the angular velocity sensor 27 that drifts according to various conditions such as temperature with the passage of time is performed. In other words, a plurality of sampling output values detected by the angular velocity sensor 27 are stored, a predetermined plurality of stored sampling data are averaged, and a low-pass filter (L
A zero point is determined by performing a smoothing process using PF), and the difference between the zero point and the actual detected value is integrated by a gain K1 to calculate the inclination angle θ. Further, by feeding back a value obtained by multiplying the deviation between the calculated inclination angle θ calculated in this way and the detected inclination angle θr obtained from the inclination sensor 26 by the gain K2, the accumulation of errors due to the integration processing is eliminated. I have. In this case, the zero point of the angular velocity sensor 27 fluctuates due to the influence of temperature or the like. For example, as shown in the data charts of FIGS. 5 and 6, the past sampling data is averaged as described above. The zero point B (n) obtained by
The angular velocity sensor 27 has a delay with respect to the actual zero point A (n). In this example, the fluctuation of the zero point B (n) obtained by the sampling average of the past 20 seconds is delayed by about half (10 seconds) of the sampling time with respect to the fluctuation of the actual zero point A (n). Assuming that the approximate zero at the present time is C (n), the approximate zero is C (n), and C (n) = B (n) + [change of B (n) per time] × [B (n) Time delay] = B (n) + {[B (n)-B (n-1)] / 10 seconds} x 10 seconds = [B (n) x 2]-B (n-1) , And the predicted zero C (n) c approximates the actual zero A (n). As means for determining the zero point C (n), in addition to using the above equation, a polynomial approximation, a logarithmic approximation or the like may be selected in accordance with the drift characteristic of the zero point tested in advance. Further, in the case of drifting with some regularity, it is also possible to store and use the fluctuation pattern as map data or the like. In the above embodiment, the angular velocity sensor is described as an example of a sensor that drifts in temperature. However, the present invention can be applied to other sensors.

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a perspective view showing a rear part of an agricultural tractor. FIG. 2 is a block diagram showing a schematic configuration of a control device. FIG. 3 is a block diagram of a rolling control device. FIG. FIG. 5 is a diagram showing fluctuations of actual zeros, average zeros, and calculated and predicted zeros. FIG. 6 is a data chart of real zeros, average zeros, and calculated and predicted zeros. ] 27 sensor (angular velocity sensor)

Claims (1)

Claims: 1. A means for averaging and calculating the variation of a zero based on a signal from a sensor, a change tendency of the average from a sequence of the average, and a time delay with respect to the zero of the average. An apparatus for compensating for a sensor zero of a working machine, comprising means for estimating and predicting and calculating a current zero.