JP3875397B2 - Tractor - Google Patents

Description

[0001]
BACKGROUND OF THE INVENTION
The present invention belongs to the technical field of a tractor having an automatic tilt control function.
[0002]
[Prior art]
In general, some tractors of this type include an automatic tilt control means for automatically controlling the tilt of a working unit connected to a traveling machine body so as to be tiltable to the left and right with the tilt setter set as a target.
[0003]
[Problems to be solved by the invention]
Conventionally, the tilt angle sensor detects the right and left tilt angle of the traveling machine body or working part.When the traveling machine body rolls, suddenly starts, or when the clutch is connected, the traveling machine body or working part is tilted to the left or right. Since the inclination sensor value fluctuates greatly regardless of the angle, the accuracy of the automatic inclination control may be reduced based on the misidentification of the left and right inclination angles.
[0004]
[Means for Solving the Problems]
The present invention has been created in view of the above circumstances and has been created for the purpose of solving these problems.The invention of claim 1 includes a working unit coupled to a traveling machine body so as to be tiltable in the left-right direction, The work unit is automatically set based on the tilt setting device that sets the left / right tilt of the working unit, the left / right tilt judging means for judging the left / right tilt angle of the traveling machine or the working unit, and the input data including the tilt setting data and the left / right tilt judging data in tractor and a tilt automatic control means for controllably tilting, the angular velocity sensor for detecting the lateral inclination determining means, and the detection value of the tilt sensor for detecting the lateral inclination angle of the traveling machine body or working portion, the angular velocity of the lateral inclination Upon configured to determine lateral inclination angle of the traveling machine body or the working unit based detection value and the right and left slope determination means, the angular velocity sensor data and tilt sensor The first state in which the angular velocity sensor data is fluctuating, the second state in which the angular velocity sensor data is stable and the tilt sensor data is fluctuating, and both data are stable. In the first state, the tilt sensor data at the time of stability is used as tilt angle reference data, and the calculated angle data obtained by adding or subtracting the integration data of the angular velocity sensor data to the tilt angle reference data is the angle. In the second state, as in the first state, the calculated angle data is set in the angle data, but the correction data calculated in the correction data processing that gradually brings the calculated angle data closer to the tilt sensor data. are those correcting the operation angle data is performed based on, in the third state, characterized in that to set the tilt sensor data to the angle data It is a tractor that.
The invention according to claim 2 sets the tilt sensor data in the angle data when the left / right tilt determination means determines that the tilt sensor data is changing at a low frequency in the first state. The tractor according to claim 1.
[0005]
DETAILED DESCRIPTION OF THE INVENTION
Next, one embodiment of the present invention will be described with reference to the drawings. In the drawings, reference numeral 1 denotes a traveling machine body of a tractor, and a working machine (cultivation part) 3 such as a rotary is connected to a rear part of the traveling machine body 1 via a lifting link mechanism 2 so as to be lifted and lowered. The working machine 3 moves up and down as the lift arm 5 swings up and down via the lift rod 4 and lifts interposed between the left and right lift rods 4. As the rod cylinder 6 expands and contracts, it tilts left and right, but these basic configurations are all conventional.
[0006]
Reference numeral 7 denotes a lift cylinder that moves the lift arm 5 up and down. The lift cylinder 7 expands and contracts with the valve switching operation of the lift arm electromagnetic switching valve 8, but switches the lift arm electromagnetic switching valve 8. Since the extension solenoid 8a and the reduction solenoid 8b can be intermittently driven based on the inching pulse, the operating speed of the lift cylinder 7 can be controlled based on the duty setting (ON time / cycle) of the inching pulse. It has become.
[0007]
Reference numeral 9 denotes a lift rod electromagnetic switching valve for expanding and contracting the lift rod cylinder 6. The extension solenoid 9a and the reduction solenoid 9b for switching the lift rod electromagnetic switching valve 9 are intermittently driven based on an inching pulse. Therefore, the operating speed of the lift rod cylinder 6 can be controlled based on the duty setting of the inching pulse.
[0008]
Reference numeral 10 denotes a rolling sensor unit provided at a predetermined position of the traveling machine body 1. The rolling sensor unit 10 includes an inclination sensor 11 (capacitance type or the like) for detecting the left and right inclination angle of the traveling machine body 1, An angular velocity sensor 12 (rotor method, piezo electric method, etc.) for detecting the angular velocity of the inclination is incorporated. In other words, the output signal of the inclination sensor 11 that has been conventionally used follows a change in inclination at a low frequency, but in the followability, response, noise generation rate, etc. with respect to an inclination change other than the low frequency, the angular velocity sensor 12 outputs. Since the direction is superior, the left and right inclination angles of the traveling machine body 1 are determined in consideration of the output signal of the angular velocity sensor 12 as will be described later.
[0009]
A control unit 13 is configured by using a microcomputer (including a CPU, a ROM, a RAM, and the like). On the input side of the control unit 13, the tilt sensor 11, the angular velocity sensor 12, and the lift arm 5 described above are provided. A lift arm sensor 14 for detecting a position, a position sensor 16 for detecting an operation position of a position lever (work machine-to-machine height setting device) 15, a pitching sensor 17 for detecting a front / rear inclination angle of the traveling machine body 1, and a lift rod cylinder 6 Lift rod sensor 18 for detecting the cylinder length, automatic tilling depth switch 19 for turning ON / OFF the automatic tilling depth control (pitching sensor method), tilling depth setting volume for setting the target tilling depth for automatic tilling depth control 20) Height setting unit) 20, automatic inclination switch 21 for turning on / off automatic inclination control, and target inclination for automatic inclination control An inclination setting volume 22 and the like are connected via a predetermined input interface circuit, and on the output side, the above-mentioned lift arm electromagnetic switching valve 8 and expansion solenoids 8a and 8b, lift rod electromagnetic switching valve are provided. Nine expansion / reduction solenoids 9a, 9b and the like are connected via a predetermined output interface circuit. That is, the control unit 13 sets a position target value (lift arm target value) based on the position sensor data, and controls the lift arm 5 to move up and down according to the deviation of the lift arm sensor data from the target value. ”, The plowing depth target value (lift arm target value) is set based on the plowing depth setting volume data and the pitching sensor data, and the lift arm 5 is controlled to move up and down according to the deviation of the lift arm sensor data from the target value. The target tilt value (lift rod target value) is set on the basis of “automatic depth control”, tilt setting volume data, and left / right tilt judgment data, and the lift rod cylinder 6 is expanded and contracted according to the deviation of the lift rod sensor data from the target value. It is configured to execute `` Automatic tilt control '' etc. ® emission control ", the basic control procedure of" tilling depth automatic control "and" tilt automatic control "because it is conventional, flowcharts and detailed description thereof will be omitted.
[0010]
Now, in executing the “inclination automatic control”, in this embodiment, “angle data processing” (main routine) for setting angle data (right / left inclination determination data) necessary for calculating the inclination target value, angular velocity sensor data In addition, processing such as “sensor input processing” (A / D multiple interruption) for inputting tilt sensor data and “correction data processing” (timer interruption) for calculating correction data of calculation angle data described later is executed. Hereinafter, the procedure of each process will be described with reference to a flowchart. However, in the “initial set” of the main routine, the following initial settings are performed.
Angular velocity reference data ← Current angular velocity sensor data Inclination angle reference data ← Current inclination sensor data Angular velocity prohibition flag ← Set [0011]
In the “sensor input process”, angular velocity sensor data is input, and the angular velocity sensor data is integrated and added to the integral data. That is, the angular velocity data is converted into the angle change amount data using the following arithmetic expression.
Current integration data ← (previous angular velocity data + current angular velocity data) / 2
Current addition data ← Current integration data-Angular velocity reference data Integration data ← Integration data + Current addition data After calculating the integration data, multiply the predetermined angle conversion coefficient to obtain the angle deviation data that matches the level of the tilt sensor data. After conversion, the tilt sensor data is input and the interruption is terminated.
[0012]
In the “correction data processing”, first, it is determined whether or not the correction flag is set in “angle data processing”. If the determination is YES, the calculation angle data (calculation angle data = tilt angle reference data ± The correction data (angle deviation data ± correction data) is calculated using the following formula.
Adjustment data ← (| Tilt sensor data-Calculated angle data | x Correction coefficient) ²
Correction data ← correction data + addition / subtraction data In other words, “correction data processing” is processing for calculating correction data for gradually bringing the calculated angle data closer to the tilt sensor data, and the actual correction processing is “angle data processing”. To be done.
[0013]
In the “angle data processing”, first, it is determined whether or not the control is permitted using the angular velocity sensor data (inclination automatic ON state). If the determination is NO, the inclination angle reference data is updated. After executing the process (setting the current tilt sensor data), the integral data (angle deviation data) clearing process, the correction data clearing process, and the correction flag resetting process, the tilt sensor data is set in the angle data. It is like that.
[0014]
On the other hand, when it is determined that the control is permitted, the stable state of the angular velocity sensor data and the tilt sensor data is determined. Here, the first state where the angular velocity sensor data is fluctuating and the angular velocity sensor data are stabilized. In addition, the second state in which the tilt sensor data is fluctuating and the third state in which both data are stable are separated, and the process is branched according to this state determination. In the third state where both data are stable, in principle, the tilt sensor data is set in the angle data, while in other states, in principle, the calculated angle data is set in the angle data. Since the angular velocity prohibition flag described above is set at the start of control that tends to become stable, the tilt sensor data is preferentially set in the angle data until the angular velocity prohibition flag is reset based on the stability determination of both sensor data. It is supposed to be.
[0015]
In the first state, after the correction flag reset process, the calculation angle data setting process, and the calculation flag setting process are executed, the calculation angle data is set in the angle data. In other words, in a situation where the angular velocity sensor data is fluctuating , the tilt sensor data at the time of stabilization is used as the tilt angle reference data, and the integration data (angle deviation data) of the angular velocity sensor data is added (or subtracted) to the tilt angle reference data. The calculated angle data is set in the angle data, but even if the angular velocity sensor data is changing, if it is determined that the tilt sensor data fluctuates at a low frequency, the tilt sensor data is added to the angle data. It is supposed to be set.
[0016]
In the second state, the correction flag is set, the calculation angle data is set, and then the calculation angle data is set in the angle data. That is, in the situation where the angular velocity sensor data is stable and the tilt sensor data fluctuates , the calculated angle data is set in the angle data as in the first state, but in the second state, the “correction data processing” The calculation angle data is corrected based on the correction data calculated in step (b).
[0017]
In the third state, the calculation flag is set and the presence / absence of a deviation (deviation between the tilt sensor data and the calculation angle data) is determined. If any of the determinations is NO, the angular velocity reference data is determined. After the inclination angle reference data update processing, integration data and correction data clear processing, and correction flag, calculation flag, and angular velocity prohibition flag reset processing are executed, the inclination sensor data is set in the angle data. ing. That is, in a situation where both sensor data are stable, various data necessary for calculation of the calculation angle data are updated, and the consistency between the actual inclination angle and the calculation angle data is maintained.
[0018]
On the other hand, when the calculation flag is set and the deviation is large, the angle data changes abruptly when the angle data is switched from the calculation angle data to the tilt sensor data. The calculation angle data is set to the angle data while performing the correction. In this state, the calculation angle data is clearly erroneous calculation data (in the case where the deviation is remarkably large in the same phase, or in the opposite phase). If the determination is YES and the determination is YES, after setting the angular velocity prohibition flag, tilt sensor data is set in the angle data.
[0019]
In the configuration as described above, the tilt target value is set based on the tilt setting volume data and the angle data in the left-right direction of the machine, and the lift rod cylinder 6 is controlled to expand and contract according to the deviation of the lift rod sensor data from the target value. In executing the automatic tilt control, the angle data is determined based on the detection data of the tilt sensor 11 that detects the left-right tilt angle of the traveling body 1 and the detection data of the angular velocity sensor 12 that detects the angular velocity of the left-right tilt. Will be. That is, in order to determine the left / right inclination angle of the traveling aircraft 1 in consideration of angular velocity sensor data that does not vary greatly even when rolling, etc., the inclination determination is made in comparison with the conventional case where the aircraft left / right inclination angle is determined only by the inclination sensor data. The accuracy can be improved, and as a result, the disadvantage that the accuracy of the automatic tilt control is reduced based on the tilt determination error can be prevented as much as possible.
[0020]
The calculated angle data calculated based on the angular velocity sensor data is calculated by using the tilt sensor data at the time of stability as the tilt angle reference data, and adding the integral data of the angular velocity sensor data to the tilt angle reference data. Therefore, it is possible to avoid the disadvantage that the integration error accumulates and the accuracy of the calculation angle data decreases.
[0021]
Also, in the situation where the angular velocity sensor data with excellent responsiveness is fluctuating , the calculation angle data calculated based on the angular velocity sensor data is preferentially used, so the inclination judgment with excellent responsiveness and high accuracy must be performed. Can do.
[0022]
Even if the angular velocity sensor data is fluctuating , if the inclination sensor data is a low-frequency inclination change that follows, the inclination sensor data is used, so the angular velocity sensor data at the time of the low-frequency inclination change is used. The inclination determination accuracy can be improved as compared with the case of integrating a minute change of.
[0023]
In addition, when the angular velocity sensor data is stable, the calculation angle data is corrected according to the deviation from the tilt sensor data, so that the integration error of the angular velocity sensor data is accumulated and the accuracy of the calculation angle data is reduced. can do.
[0024]
In addition, since the tilt sensor data is used from the start of control until the angular velocity sensor data is stabilized, it is possible to prevent the inconvenience that the accuracy of the tilt determination is lowered based on the error of the angular velocity sensor data.
[0025]
Of course, the present invention is not limited to the above-described embodiment. For example, when updating the reference data, the working machine tilt operation state and the work machine lifting operation state are determined, and the operation stop state is determined. The reference data may be updated only in step S2, and in this case, the sensor noise accompanying the operation can be eliminated to improve the accuracy of the reference data. In the above embodiment, the use of the calculated angle data based on the angular velocity sensor data is prohibited in the initial stage of control, but the start of the automatic tilt control (valve output, automatic ramp) until the tilt sensor data and the angular velocity sensor data become stable. Lighting) may be prohibited. In the above embodiment, the angular velocity prohibition flag prohibiting the use of the calculated angle data is set in the initial set of the main routine. However, the angular velocity prohibition flag is set every time the automatic inclination control is restarted, and others. It may be set according to a predetermined event.
[Brief description of the drawings]
FIG. 1 is a side view of a tractor.
FIG. 2 is a graph showing basic sensor characteristics of an inclination sensor and an angular velocity sensor.
FIG. 3 is a graph showing sensor characteristics in a gentle inclination change state and a rapid inclination change state.
FIG. 4 is a graph showing sensor characteristics in a rolling state.
FIG. 5 is a block diagram showing input / output of a control unit.
FIG. 6 is a flowchart showing a main routine.
FIG. 7 is a flowchart showing “angle data processing”;
FIG. 8 is a flowchart showing “sensor input processing”;
FIG. 9 is a flowchart showing “correction data processing”;
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 Traveling machine body 2 Lifting link mechanism 3 Working machine 10 Rolling sensor unit 11 Inclination sensor 12 Angular velocity sensor 13 Control part 22 Inclination setting volume

Claims (2)

A working unit coupled to the traveling machine body so as to be tiltable to the left and right; an inclination setting unit for setting the right and left inclination of the working part; In a tractor including an automatic inclination control unit that automatically controls the inclination of the working unit based on input data including the right / left inclination determination data, the right / left inclination determination unit is configured to detect the right / left inclination angle of the traveling machine body or the working unit. In determining to determine the left / right inclination angle of the traveling machine body or the working unit based on the detection value of the sensor and the detection value of the angular velocity sensor that detects the angular velocity of the left / right inclination, the left / right inclination determination means includes angular velocity sensor data and The stable state of the tilt sensor data is judged, the first state where the angular velocity sensor data is fluctuating, the angular velocity sensor data is stable, and the tilt sensor data is The second state where the data is fluctuating and the third state where both data are stable are separated. In the first state, the tilt sensor data at the time of stability is used as the tilt angle reference data, The calculation angle data obtained by adding or subtracting the integral data of the angular velocity sensor data is set in the angle data. In the second state, the calculation angle data is set in the angle data as in the first state. The correction angle data is corrected based on the correction data calculated in the correction data processing for gradually approaching the inclination sensor data. In the third state, the inclination sensor data is set in the angle data. A tractor characterized by that. The left / right inclination determination means sets the inclination sensor data in the angle data when it is determined that the inclination sensor data changes at a low frequency in the first state. The tractor according to 1.

Images