JP2006101737A - Automatic controller of inclination in working vehicle - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide an automatic controller of inclination in a working vehicle, making it sufficient that an arithmetic operation load and required memory capacity are small, when a reference value of an angular velocity sensor is corrected moment by moment, and capable of necessarily and sufficiently ensuring accuracy of correction, even when reading frequency of outputs of the angular velocity sensor is small. <P>SOLUTION: In this automatic controller, a finite difference datum is calculated by subtracting a reference datum at the present time from an output datum of the angular velocity sensor at regular intervals and the reference datum is corrected by conducting arithmetic operations as follows: (new reference datum)=(reference datum at the present time)+(correction quantity); and (correction quantity)=(finite difference datum)/(2<SP>n</SP>), so that the reference datum is induced toward a level corresponding to the new reference datum at every time of the arithmetic operations, when the reference value corresponding to an angular velocity O of the angular velocity sensor changes. The correction quantity is decreased by stepwise increasing a multiplier n, when the finite difference datum is small, but the correction quantity at one time is increased by stepwise decreasing the multiplier n, when the finite difference datum is large. <P>COPYRIGHT: (C)2006,JPO&NCIPI

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.

  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 angular velocity 0 is determined, and (sensor output−reference) Value) to determine the angular velocity of right / left rotation.

  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.

  Patent Document 1 discloses a working vehicle in which an inclination sensor that detects an inclination angle and an angular velocity sensor that detects an angular velocity of rolling are attached to a traveling machine body to measure the left and right inclination angles of a working unit. Here, when the angular velocity is not detected, the inclination angle is calculated based on the output of the inclination sensor, but when the angular velocity is detected, the inclination angle is calculated by integrating the output of the angular velocity sensor. In addition, after a predetermined time has elapsed since the last angular velocity detection, the calculation is returned to the calculation of the tilt angle based on the output of the tilt sensor.

  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.

  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.

  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.

JP-A-8-89011 JP-A-11-289808 JP 2000-354402 A JP 2002-253005 A

  In the automatic tilt control apparatus shown in Patent Documents 1 and 2, the tilt angle based on the output of the angular velocity sensor is merely replaced with the tilt angle based on the output of the tilt sensor, and the error of the angular velocity calculated from the output of the angular velocity sensor is a reference value. It depends on you. Therefore, if the reference value is deviated, there is a possibility that the tilt angle in which the angular velocity error is accumulated is calculated.

  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.

  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.

An automatic tilt control apparatus for a working vehicle according to claim 1 supports a working unit (30) on a traveling machine body (10) so as to be tiltable to the left and right, and detects a right and left tilt angle of the traveling machine body (10) or the working unit (30). A working vehicle including an automatic tilt control device (40) for controlling the working unit (30) to be held in a preset posture regardless of the left-right tilt of the traveling machine body (10). In addition, an inclination sensor (13) and an angular velocity sensor (14) are provided as means for detecting the left / right inclination angle, and the reference value of the angular velocity sensor (14) is expressed as "the reference value so far + (the detection value of the angular velocity sensor- calculated by the reference value) / 2 ⁿ "to date, the exponent n, is obtained by a modifiable integer during operation of the tilt automatic control.

  The automatic inclination control device for the working vehicle according to claim 2 increases the index n in the invention according to claim 1 when the difference between the (detection value of the angular velocity sensor−the reference value so far) is small. If the difference is large, the change is made to be small.

  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.

  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.

  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.

  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.

  The tilt angle of the tractor 10 is calculated every moment based on the outputs of the tilt sensor 13 and the angular velocity sensor 14, and the lift rod cylinder 25 is driven based on the calculation result, thereby automatically adjusting the left and right tilt angles of the work implement 30.

  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.

  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 rotary blade 31 is connected to the rear part of the lifting link mechanism 20. ing. The lifting / lowering link mechanism 20 is a three-point type including one upper link 21 and a pair of left and right lower links 22, and various work machines can be selected and mounted in addition to the work machine 30.

  The lower link 22 of the elevating link mechanism 20 is suspended and supported by a lift arm 23 via a lift rod 24, and the lift arm 23 is rotated up and down by an extension / contraction operation of a lift cylinder 27 disposed on the base side thereof. The Accordingly, the lift cylinder 27 can be hydraulically driven to move the working machine 30 up / down, or an arbitrary tilling depth can be set.

  The lift rod 24 on the left side in the direction of travel of the tractor 10 is supported by being suspended from the lift arm 23 via a lift rod cylinder 25, and the lift rod cylinder 25 is hydraulically driven to increase the length of the left and right lift rods 24. By making it different, the left-right inclination angle of the work machine 30 can be arbitrarily set.

  An operation panel 16 is disposed adjacent to the driver seat 15 of the tractor 10, and an automatic tilt control device 40 is disposed below the operation panel 16. The operation panel 16 is provided with a position lever 11 for manually moving the lift arm 23 up and down, and a number of switches and volumes for performing various settings and switching related to work.

  As shown in FIG. 2, the automatic tilt control device 40 is an electronic circuit configured using a microcomputer circuit (including an MPU, RAM, ROM, IO circuit, etc.) and has a height set by the position lever 11. Position control for automatically raising / lowering the work machine 30 as a target, automatic tilling depth control for automatically raising / lowering the work machine 30 with a set tilling depth not shown as a target, setting inclination of the inclination setting volume 43 Control such as automatic tilt control for automatically tilting the work implement 30 with the angle as a target is executed.

  The automatic tilt control device 40 includes an automatic switching volume 41, a work implement switching volume 42, a tilt setting volume 43, an operation switch 44, an engine rotation sensor 45, an axle rotation sensor 46, a remote control switch 47, a lift arm sensor 26, and a lift rod sensor. 28, a position lever sensor 29, an inclination sensor 13, an angular velocity sensor 14, a relay unit 57, solenoid valves 51, 52, and the like are connected.

  The automatic switching volume 41 is a variable resistor that is linked to an automatic switching knob disposed on the operation panel 16 and determines a plurality of fixed positions corresponding to ON / OFF of automatic tilt control. When the automatic changeover switch is set to OFF, automatic adjustment of the left / right tilt of the work machine 30 is not executed. However, when ON is set, a short changeover sound is output through the speaker 54, and automatic adjustment is started. The notification lamp 55 is turned on.

  The work machine switching volume 42 is a variable resistor that is linked to the work machine switching knob arranged on the operation panel 16 and discriminates a plurality of fixed positions corresponding to the type of work machine connected to the tractor 10.

  The tilt setting volume 43 is a variable resistor linked to the tilt angle setting knob disposed on the operation panel 16 and corresponds to the tilt angle of the work machine 30 set by rotating the tilt angle setting knob by the operator. Output analog voltage. The operation switch group 44 sets auxiliary functions for automatic tilt control, such as setting the operating sensitivity of tilt angle adjustment.

  The engine rotation sensor 45 is a pulse encoder attached to the engine output shaft of the tractor 10 and outputs a pulse voltage having a frequency corresponding to the engine rotation speed. The axle rotation sensor 46 is a pulse encoder attached to the rear wheel shaft of the tractor 10 and outputs a pulse voltage having a frequency corresponding to the ground speed (traveling speed) of the tractor 10. The remote control switch 47 is a switch for manually operating the lift rod cylinder 25 from the outside of the driver seat 15.

  As shown in FIG. 1, the lift arm sensor 26 is a variable resistor arranged at the base of the lift arm 23, and outputs a voltage signal corresponding to the rotation angle of the lift arm 23. The lift rod sensor 28 is a length measurement sensor disposed adjacent to the lift rod cylinder 25, and outputs a voltage signal corresponding to the draw length of the lift rod cylinder 25. The position lever sensor 29 is a variable resistor disposed at the base of the position lever 11 and outputs a voltage signal corresponding to the rotation angle of the position lever 11.

  The tilt sensor 13 is a gravity-type tilt angle sensor disposed in the vicinity of the rear wheel shaft of the tractor 10 and outputs a voltage signal corresponding to the rolling tilt angle of the vehicle body. The angular velocity sensor 14 is an angular velocity sensor stored in the box of the automatic tilt control device 40, and outputs a voltage signal corresponding to the rolling angular velocity of the vehicle body.

  The steering angle detection switch 56 is a limit switch arranged so as to be turned on when the steering 12 of the tractor 10 is rotated and the front wheel angle exceeds a certain left and right angle. The relay unit 57 outputs a constant voltage signal while the steering 12 is operated and any one of the steering angle detection switches 56 is ON.

  The solenoid valve 51 flows oil in the extending direction of the lift rod cylinder 25, and the solenoid valve 52 flows oil in the shortening direction of the lift rod cylinder 25. The priority valve 53 permits (or stops) oil supply to the solenoid valves 51 and 52.

  In the automatic tilt control apparatus 40 according to the first embodiment configured as described above, the automatic switching volume 41 is set to ON to select automatic adjustment of the left / right tilt angle, and the work machine switching volume 42 is set to the rotary work machine 30. Is set to At this time, the automatic tilt control device 40 calculates the stepwise tilt angle of the tractor 10 based on the outputs of the tilt sensor 13 and the angular velocity sensor 14, and is set in the tilt setting volume 43 while referring to the output of the lift rod sensor 28. The control amount is calculated every moment with the target inclination angle as a target. Then, the tilt automatic control device 40 operates the solenoid valves 51 and 52 based on this control amount, thereby extending / shortening the lift rod cylinder 25 and controlling the tilt angle of the work implement 30.

  As shown in FIG. 3, the automatic tilt control device 40 executes the automatic tilt angle control according to the main flow 200 configured by steps 210 to 370.

  In step 210, as shown in FIGS. 4 and 5, initial setting of various arithmetic processes is performed. At subsequent step 220, as shown in FIGS. 6 and 7, the connection determination of the angular velocity sensor is performed. In the subsequent step 250, as shown in FIG. 8, smoothing processing is performed to average the outputs of the inclination sensor 13 and create reference data. In the subsequent step 280, as shown in FIG. 9, a calculation dead zone used for determination when the output data of the angular velocity sensor 14 is integrated is set.

  In the subsequent step 310, as shown in FIGS. 10 and 11, switching control for selecting control based on the angular velocity sensor 14 and control based on the tilt sensor 13 in accordance with the situation of the tractor 10 is performed. In the following step 360, as shown in FIG. 12, an angle calculation for integrating the outputs of the angular velocity sensor 14 is performed. In the subsequent step 370, as shown in FIG. 13, tilt control is performed to control the output based on the calculated tilt angle.

  As shown in FIG. 4, in the initial setting flow (210), it is determined in step 211 whether or not initial setting is necessary, and if it is determined that the automatic changeover switch is turned on or the like, steps 212 to 216 are performed. As shown, various calculation processes are initially set. In step 212, the calculation start flag and the integration flag are reset.

  In step 213 of FIG. 4, as shown in FIG. 5, a smooth initial setting flow (240) is executed. That is, in step 241, the reference value of the inclination sensor 13 is adopted as smooth data, in step 242, the setting of smooth calculation data is started, and in step 244, the smooth flag is cleared.

  In step 214 of FIG. 4, as shown in FIG. 5, a reference initial setting flow (390) is executed. That is, the setting of the reference shift is started in step 391, the setting of the base is started in step 392, and the four control counters are cleared in step 393.

  Returning to FIG. 5, in step 215, the update confirmation timer, timeout, and start delay timer settings are started. In step 216, calculation dead zone setting is started.

  As shown in FIG. 6, in the connection determination flow (220), it is determined in step 221 whether the output value of the angular velocity sensor 14 is within the normal range. As shown in FIG. 21, the normal range is set to a range of 0.1 to 4.9 V in the range of 0 to 5 V of the angular velocity sensor output, and a hysteresis range is provided in a range of 0.2 V above and below the normal range. . Here, if it is determined that it is out of the normal range, the routine proceeds to step 224, where the out-of-range flag is turned ON, and further, the routine proceeds to step 225, where it is determined whether or not the sensor out timer has exceeded the limit time. As shown in FIG. 7, the sensor removal timer accumulates the ON elapsed time of the out-of-range flag as timer interruption processing, and when the ON state reaches the limit time, the process proceeds to step 226 and the sensor removal flag is turned ON.

  On the other hand, if the output value of the angular velocity sensor 14 is within the normal range, the routine proceeds to step 222, where it is determined whether or not it is within the hysteresis range. If it is not within the hysteresis range, the routine proceeds to step 227, where the out-of-range flag is turned off, and further proceeds to step 228, where it is determined whether the sensor connection timer has exceeded the limit time. As shown in FIG. 7, the sensor connection timer integrates the OFF elapsed time of the out-of-range flag as a timer interrupt process, and if the OFF state exceeds the limit time, the process proceeds to step 229 and the sensor out-of-range flag is ON. If it is determined that the sensor removal flag is ON, the process proceeds to step 231 where the sensor removal flag is turned OFF. Subsequently, in step 232, reset processing is executed, and in step 233, base initial setting is executed.

  If the hysteresis range is determined in step 222, the process proceeds to step 223 to determine whether the out-of-range flag is ON / OFF. If it is OFF, the process proceeds to step 227, but if it is ON, the hysteresis range is entered from outside the normal output range. If applicable, the process proceeds to step 224 to continue the processing corresponding to out of range. If the sensor disconnection timer has not expired in step 225, the sensor connection timer has not expired in step 228, and the sensor disconnection flag is OFF in step 229, each current flag state is continued.

  In this way, the output of the angular velocity sensor 14 is determined, and if the connection and function of the angular velocity sensor 14 are confirmed to be normal, the sensor detachment flag is turned OFF. If not confirmed, the sensor detachment flag is turned ON.

  As shown in FIG. 8, in the smoothing control flow (250), a sampling period for calculating the average value of the output of the tilt sensor 13 is set. In step 251, it is determined whether or not the smooth flag is cleared in the smooth initial setting flow (240), that is, whether or not the initial state is maintained. If it is in the initial state, the routine proceeds to step 253, where a normal sampling period of 200 msec is set.

  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 work machine 30 is lowered, and is reset after the work has started. When the flag is reversed from set to reset, it is determined that the work is started, and the process proceeds to step 254 to set a short sampling period of 100 msec. If it does not correspond to the start of work, a normal sampling period of 200 msec is set.

  In this way, when the work implement 30 is lowered and the tilt control is restarted, the output value of the tilt sensor 13 is also likely to fluctuate. Therefore, the sampling angle is shortened to try to calculate the correct tilt angle as much as possible. .

  The smoothing process of the output value of the tilt sensor 13 is executed in the timer interrupt process flow (260). If it is determined at step 261 that the sampling time is reached, the routine proceeds to step 262 where the output value of the tilt sensor 13 is added. In step 263, when it is confirmed that the addition has been performed a predetermined number of times (five times), the process proceeds to step 264 and the smooth flag is set.

  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.

  As shown in FIG. 9, in the dead zone setting flow (280), a dead zone corresponding to the ground speed of the tractor 10 is set. The dead zone is an output range that is set so that a small output change of the angular velocity sensor 14 is not reflected in the automatic control of the tilt angle. In step 280, the output of the axle rotation sensor 46 is referred to and it is determined whether or not the ground speed is less than 2.5 km / h. If the ground speed is 2.5 km / h or less, as shown in a diagram 290, the process proceeds to step 283 to set the dead zone to 1.5 degrees. If not, the process proceeds to step 282. It is determined whether or not the speed exceeds 6.0 km / h. If the ground speed exceeds 6.0 km / h, the process proceeds to step 284 and the dead zone is set to 2.25 degrees. If the ground speed is in the range of 2.5 to 6.0 km / h, As shown at 290, continuous values in the range of 1.5 degrees to 2.25 degrees are set.

  In this way, when the ground speed of the tractor 10 increases, the change in the output value of the angular velocity sensor 14 increases even on the same cultivation surface, but the number of control outputs becomes abnormal by setting a large dead zone corresponding to the ground speed. It avoids the situation of increasing the number of times or causing the instability by repeating the reversal of control.

  As shown in FIG. 10, in the switching control flow (310), control based on the output of the angular velocity sensor 14 is applied only when the output of the angular velocity sensor 14 exceeds the dead zone. In step 311, the output of the relay box 57 is referred to and it is determined whether or not the steering 12 has been operated beyond a predetermined angle. If it is not operated, the routine proceeds to step 312 where the output of the position lever sensor 29 is referred to and it is determined whether or not the position lever 11 is operated in the raised state. If the lift arm 23 is operated, the routine proceeds to step 313, where the output of the lift arm sensor 26 is referred to, and it is determined whether or not the lift arm 23 is in the lift state.

  If it is determined that the steering wheel 12 has been operated beyond a predetermined angle or the work machine 30 is in the raised state based on the output of the position lever sensor 29 or the lift arm sensor 26, the routine proceeds to step 154 and the start delay timer counts. If none of the above applies, the process proceeds to step 320 to determine whether or not the start delay timer has exceeded the limit time TA.

  The start delay timer counts up at step 321 and continues to pass through steps 311 to 313 with NO (the elapsed time since the last execution of step 154, in other words, from the end of steering or the completion of lowering of the work implement 30) Elapsed time) is measured, and if the start delay timer exceeds the limit time TA, the process proceeds from step 320 to step 331 and the work start flag is reset, but the start delay is reached in step 321 until the limit time TA is reached. The timer continues counting, the integration flag is reset at step 322, the smooth data is taken into the inclination reference data at step 323, the integration data of the output of the angular velocity sensor 14 is cleared at step 324, and the work start flag is set at step 325. Set timeout countdown at step 326 The continue to start.

  In step 332 following step 331, the timeout is counted down. In step 333, difference data obtained by subtracting the dead zone angular velocity from the absolute value obtained by subtracting the reference data from the output value of the angular velocity sensor 14 is calculated. In the following step 334, the difference data is determined, and if the difference data is greater than 0, that is, if the output of the angular velocity sensor 14 exceeds the dead zone, the process proceeds to step 335 to determine whether or not a timeout has been reached. If the timeout has not expired in step 335, the process proceeds to step 336, the calculation start flag is set, the update confirmation timer is cleared in step 337, and the integration flag is set in step 338.

  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 zone setting flow 280 of FIG. 9, and as shown in FIG. In order to avoid instability at the boundary, the inside / outside of the dead zone is determined by adding hysteresis.

  Here, the timeout is a timer that is set to remove the integration error when the output of the angular velocity sensor 14 has exceeded the dead zone for a long time. The time-out starts counting down at the time when the step time 326 is passed immediately before the limit time TA after the work machine 30 descends, or at the time when the output of the angular velocity sensor 14 is in the dead zone last. If the time-out is completed in step 335, the process proceeds to step 341, the integration flag is reset, smooth data is taken into the tilt reference data in step 342, and the integration data is cleared in step 343.

  On the other hand, if it is determined in step 334 that the difference data is 0 or less, that is, the output of the angular velocity sensor 14 is inside the dead zone, the process proceeds to a partial flow 330 in FIG. As shown in FIG. 11, in step 351, the calculation start flag is reset, and in step 352, it is confirmed whether or not the count of the update confirmation timer is completed.

  Here, the update confirmation timer measures the elapsed time after the output of the angular velocity sensor 14 has finally changed from outside the dead zone to inside the dead zone, and detects the passage of the limit time TB. Then, even if the output of the angular velocity sensor 14 changes into the dead zone, the calculation of the inclination angle based on the output of the angular velocity sensor 14 is continued for the limit time TB, and then the calculation of the inclination angle is started with the smooth data of the inclination sensor 13. Thus, it is possible to secure a time for the output of the tilt sensor 13 to follow the tilt and to realize stable switching.

  If it is determined in step 352 that the update confirmation timer has been completed, the process proceeds to step 355, the integration flag is reset, a timeout is set in the subsequent step 356, smooth data is taken in as the inclination reference data in the subsequent step 357, and the subsequent step 358 is performed. The accumulated data is cleared with. On the other hand, if it is determined in step 352 that the update confirmation timer has not expired, the process proceeds to step 353 where the update confirmation timer is counted, and in step 354, the integration flag is set.

  As shown in FIG. 12, in the angle calculation flow (360), the inclination angle of the tractor 10 is calculated by integrating the output of the angular velocity sensor 14 with the inclination reference data as an initial value.

  In step 361, the difference data of the angular velocity sensor 14 sampled at a constant period is added and divided by the number of integrations per second, and then multiplied by a constant ratio of the inclination data and the angular velocity data, and the output of the inclination sensor 13 is smoothed. Data is added.

  Here, when the difference data of the angular velocity sensor 14 is integrated for a predetermined time and divided by the number of integrations per second, the change in the inclination angle (integral value) for the predetermined time is calculated. For example, when the difference data 1 degree / second obtained by subtracting the reference data from the output value of the angular velocity sensor 14 sampled every 200 msec is added five times and divided by the number of integrations 5 per second, the inclination angle added per second is added. Once is calculated.

  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 angular velocity sensor 14 outputs 2V for the same inclination of 10 degrees, the integrated value of the angular velocity sensor is halved to match the level of the inclination sensor 13. Is.

  The actual integration process is executed by the timer interrupt process 365. In step 366, it is determined whether it is a sampling time. If it coincides with the sampling time, the process proceeds to step 367 and it is confirmed that the integration flag is ON. If the integration flag is ON, the process proceeds to step 368 and the angular velocity data is added, but if the integration flag is OFF, the process proceeds to step 369. The accumulated data is cleared with. The integration flag continues to be set as long as the output (difference data) of the angular velocity sensor 14 is outside the dead zone in step 338 of FIG.

  In this way, when the tractor 10 tilts left and right and the output of the angular velocity sensor 14 jumps out of the dead zone, the inclination angle obtained by integrating the change in the output of the angular velocity sensor 14 is continuously calculated until the next return to the dead zone.

  As shown in FIG. 13, in the tilt control flow (370), the solenoid valves 51 and 52 are operated based on the calculated tilt angle to control the tilt angle of the work implement 30. In step 371, the sensor connection is determined. If the sensor connection flag set in the connection determination flow 220 shown in FIG. 11 is ON, the process proceeds to step 372 to check whether the integration flag is set. If the integration flag is set in the switching control flow (310, 330) shown in FIGS. 10 and 11, the process proceeds to step 373 and the angular velocity integration data calculated in the angle calculation flow (360) shown in FIG. Is used as tilt data. In the subsequent step 374, a narrow dead zone of 1.5 to 2.25 degrees set in the dead zone setting flow (280) shown in FIG. 9 is adopted.

  On the other hand, if the sensor removal flag is ON in step 371, the process proceeds to step 377 and only the safe tilt sensor data 13 is adopted, and in step 378, a wider dead zone than in the case of step 374 is set. If the integration flag is in the reset state in step 372, the process proceeds to step 375 and the smoothed data of the tilt sensor is adopted, and a wider dead zone is set in step 376 than in step 374.

  In either case, the process proceeds to step 379, and the solenoid valves 51 and 52 are operated based on the calculated tilt data to control the tilt angle of the work implement 30.

  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 angular velocity sensor 14 every 200 msec is executed. The reference value is an origin value for discriminating whether the output of the angular velocity sensor 14 is clockwise or counterclockwise, that is, the output value of the angular velocity sensor 14 corresponding to the angular velocity 0. Further, as described with reference to FIG. 16, the angular velocity sensor 14 outputs 0 V at the maximum clockwise angular velocity and 5 V at the maximum counterclockwise angular velocity, and outputs an analog voltage of 0 to 5 V with respect to the angular velocity between the two. Output.

  Then, as shown in FIG. 23, this analog voltage is read by the inclination control device 40 as 8-bit 256-step output data, and a reference value (hereinafter referred to as reference data) is subtracted from the output data to calculate an angular velocity (difference). The Thereafter, the inclination angle is calculated by integrating the difference and dividing by the number of integrations per second, as described for the angle calculation flow (360) shown in FIG.

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.

  As shown in FIG. 14, in step 421, it is determined whether or not the timing of the reference data processing is met, and if so, the process proceeds to step 422 to refer to the output of the relay box 57, and the steering angle switch It is determined whether or not the vehicle 56 matches during turning. And if it does not agree during this turning, the reference data processing of step 423 and the subsequent steps is executed. During the turn, the output value of the angular velocity sensor 14 changes due to the influence of the ground axis sensitivity, so the reference data processing is not performed.

  In step 423, the current reference value (hereinafter referred to as reference data) is subtracted from the output data of the angular velocity sensor 14, and the difference is calculated. This difference corresponds to the current angular velocity based on the current reference data. In the following step 424, it is determined whether or not the difference is 0 or more. If it is greater than or equal to 0, the flow proceeds to a flow after step 425, but if it is less than 0, the flow proceeds to a flow after step 445 shown in FIG.

  In step 425, the subtraction up counter and the subtraction down counter that are not used in the following flow are cleared, and in the subsequent step 426, it is determined whether or not the difference is in the dead zone range. Here, the dead zone is a dead zone of a smaller range that is different from the dead zone set in the dead zone setting flow (280) shown in FIG. 14, and is defined within a range of ± 16 centering on the current reference data. . If it is determined in step 426 that the difference is within the dead zone, the process proceeds to step 427, the addition up counter is cleared, and the addition down counter is incremented.

  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. .

In step 428, it is determined whether or not the addition down counter has reached the set number of times, and if not, the process proceeds to step 431 and the same multiplier n,
Difference shift processing data = (output data−current reference data) / 2 n-th power calculation is executed. In the following step 432,
Calculation of new reference data = current reference data + difference shift processing data is executed.

  On the other hand, when the addition down counter reaches the set number of times in step 428, the process proceeds to step 429, where the addition down counter is cleared, and in step 430, one multiplier n (reference shift) is added.

  Here, as shown in FIG. 18, when the output data of the angular velocity sensor 14 is 130 and the reference data is 125, the difference is 130-125 = 5, and the process proceeds from step 424 to step 425. Thereafter, in step 426, it is determined that the difference 5 is in the range of dead zone ± 16, and the process proceeds to step 427. The above calculation is repeated every 200 msec, for example, with a multiplier of 8, until the addition down counter reaches the set value.

  Then, when the addition down counter reaches the set value, the routine proceeds from step 429 to step 430, where the multiplier n is incremented by 1, and the above calculation by the multiplier 9 is repeated until the addition down counter reaches the set value. As a result, the reference data is initially shifted gradually toward the correct reference data 130 gradually and converges smoothly to the correct reference data 130.

  By the way, if it is determined in step 426 that the difference is out of the dead zone, the process proceeds to step 436 to determine whether or not the difference is out of range. As shown in FIG. 18, the area outside this range is arranged above and below the dead zone at intervals. If the angular velocity level is large enough to reach a region outside this range, the flow proceeds to the flow after step 437 and the reference data processing for gradually reducing the multiplier n is executed. If the angular velocity level is not as large as the region outside the range, step 435 is executed. The process proceeds to step S4 to clear the addition up counter and the addition down counter, and executes the reference data processing that continues the current multiplier n.

  Accordingly, when the tractor 10 is shaken so as to reach a region outside the range, the correction amount per time is increased so that convergence to the correct reference data is not delayed. In this way, after the multiplier n is initially set to 8 in step 391 shown in FIG. 5, the multiplier n is incremented / decremented within the range of 2 to 16 according to the level of fluctuation, and the difference shift processing data is correspondingly corresponding thereto. Changes in the range of difference data / 4 to difference data / (2 to the 16th power).

  In step 437, the addition up counter is incremented to clear the addition down counter. In the subsequent step 438, it is determined whether or not the addition up counter has reached the set number of times. If it has been reached, the process proceeds to step 439 to clear the addition up counter, and in the subsequent step 440, the multiplier n (reference shift) is decremented. . If not, the process proceeds to step 231 to repeat the calculation of the same multiplier n until the addition up counter reaches the set number of times.

  On the other hand, when it is determined in step 424 that the difference is less than 0, that is, when the angular velocity is detected in the opposite direction to the case where the difference is determined to be 0 or more, as shown in FIG. The above-described flow is executed to execute reference data processing substantially the same as in step 425 and subsequent steps in the direction of lowering the reference data.

  In step 445, the addition up counter and the addition down counter that are not used in the following flow are cleared. In the following step 446, it is determined whether or not the difference is within the dead zone. If it is determined that the difference is within the dead zone, the process proceeds to step 477, where the addition up counter is cleared and the subtraction down counter is incremented. The subtraction up counter and the subtraction down counter also set the number of repetitions of the reference data processing with the same multiplier n.

In the following step 448, it is determined whether or not the subtraction down counter has reached the set number of times. If not, the process proceeds to step 451, where the same multiplier n is used.
Difference shift processing data = (output data−current reference data) / 2 n-th power calculation is executed. In the following step 452,
Calculation of new reference data = current reference data + difference shift processing data is executed.

  On the other hand, if the subtraction down counter has reached the set number in step 448, the process proceeds to step 449, where the subtraction down counter is cleared, and in step 450, one multiplier n (reference shift) is added.

  By the way, if it is determined in step 446 that the difference is out of the dead zone, the process proceeds to step 456 to determine whether or not the difference is out of range. If the value falls outside the range, the flow proceeds to step 457 and subsequent steps to execute the reference data processing for gradually reducing the multiplier n. If the value does not fall outside the range, the flow proceeds to step 455 to clear the subtraction up counter and the subtraction down counter. Then, the reference data processing is performed by continuing the current multiplier n.

  In step 457, the subtraction up counter is incremented to clear the subtraction down counter. In the following step 458, it is determined whether or not the subtraction up counter has reached the set number of times. If it has been reached, the process proceeds to step 459 to clear the subtraction up counter, and in the subsequent step 460, the multiplier n (reference shift) is decremented. . If not, the process proceeds to step 451, and the same multiplier n is repeated until the subtraction up counter reaches the set number of times.

  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 angular velocity 0 from the output data. In the inclination automatic control device to be calculated, the reference data is corrected every moment according to the calculation formula (new reference data ← old reference data + difference data × proportional constant). Therefore, as shown in FIG. The reference data gradually converge.

  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.

  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.

  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.

  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.

It is a side view of the tractor carrying the automatic inclination control device of Example 1. It is explanatory drawing of the connection state of an inclination automatic control apparatus. 4 is a flowchart of overall control in the automatic tilt control apparatus of the first embodiment. It is a flowchart of initial setting. It is a flowchart of smooth initial setting and reference | standard initial setting. It is a flowchart of connection judgment. It is a flowchart of the timer interruption process in connection determination. It is a flowchart of a smoothing process. It is a flowchart of calculation dead zone setting. It is a flowchart of switching control. It is a partial flowchart in switching control. It is a flowchart of angle calculation. It is a flowchart of inclination control. It is a flowchart of a reference data process. It is a partial flowchart in a reference | standard data process. It is a diagram of a hysteresis setting. It is a diagram of a dead zone setting in inclination control. It is a diagram of dead zone setting in standard data processing.

Explanation of symbols

10 Tractor (running vehicle)
11 Position lever 12 Steering 13 Inclination sensor 14 Angular velocity sensor 15 Driver's seat 16 Operation panel 20 Lifting link mechanism 21 Upper link 22 Lower link 23 Lift arm 24 Lift rod 25 Lift rod cylinder 26 Lift arm sensor 27 Lift cylinder 28 Lift rod sensor 29 Position Lever sensor 30 Working machine 31 Rotary blade 40 Automatic tilt control device 41 Automatic switching volume 42 Working machine switching volume 43 Tilt setting volume 44 Operation switch group 45 Engine rotation sensor 46 Axle rotation sensor 47 Remote control switch 51, 52 Solenoid valve 54 Speaker 55 Lamp 56 Steering angle detection switch

Claims (3)

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 ⁿ
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. 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. 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.

Images