JP2006166714A - Mobile agricultural machine - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To rationalize a dead zone during tilt control with an angular velocity sensor and to carry out the tilt control with high accuracy utilizing performances of the angular velocity sensor. <P>SOLUTION: The right and left tilts of an implement 3 are automatically controlled on the basis of values detected with a tilt sensor 7 and the angular velocity sensor 8. In the process, values detected with the angular velocity sensor 8 are integrated to provide tilt data when the values detected with the angular velocity sensor 8 are in a variable state. When the values detected with the angular velocity sensor 8 are in a stable state, values detected with the tilt sensor 7 are used as the tilt data to judge the right and left tilt angles of the implement 3. Furthermore, when the tilt data on the basis of the angular velocity sensor 8 are used, a dead zone narrower than the case in which the tilt data of the tilt sensor 7 are used is applied to carry out the tilt control of the implement 3. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

  The present invention relates to a mobile agricultural machine such as a tractor that automatically controls the right / left inclination of a work machine.

  A mobile agricultural machine that automatically controls the right and left tilt of the work machine is known. This type of mobile agricultural machine is usually provided with a tilt sensor in a work machine or a traveling machine body, and determines the left / right tilt angle of the work machine based on the detection value of the tilt sensor, and performs left / right tilt control of the work machine. When the machine rolls, suddenly starts, or when the clutch is engaged, the tilt sensor value fluctuates regardless of the left / right tilt of the work machine or traveling machine. May be reduced.

Therefore, it is proposed to determine the left and right tilt angles of the work implement based on the detection values of the tilt sensor and the angular velocity sensor (see Patent Document 1). According to such a configuration, when the detected value of the angular velocity sensor is in a fluctuating state, the detected value of the angular velocity sensor is integrated to obtain inclination data, and when the detected value of the angular velocity sensor is in a stable state, the detected value of the inclined sensor. By determining the left and right tilt angles of the work implement using the tilt data as the tilt data, the accuracy of tilt control can be increased.
JP-A-11-289808

  However, in the past, even when using tilt data based on the angular velocity sensor, the same dead zone as when using the tilt data of the tilt sensor is applied, so the performance of the angular velocity sensor cannot be fully utilized. There was a problem that it did not lead to further improvement in accuracy.

The present invention was created to solve these problems in view of the above circumstances, and automatically controls the right / left tilt of the work machine based on the detection values of the tilt sensor and the angular velocity sensor. In the mobile agricultural machine, when the detected value of the angular velocity sensor is in a fluctuation state, the detected value of the angular velocity sensor is mainly integrated to obtain inclination data, and when the detected value of the angular velocity sensor is in a stable state, the inclination sensor When the inclination data based on the angular velocity sensor is used, the narrower dead zone is applied than when the inclination data of the inclination sensor is used. Inclination control is performed. If comprised in this way, the dead zone at the time of the inclination control by an angular velocity sensor can be optimized, and highly accurate inclination control using the performance of an angular velocity sensor can be performed.
Further, the dead zone can be arbitrarily set. If comprised in this way, a dead zone is optimized by a setting and the improvement of the further precision can be aimed at.

  Next, embodiments 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 3 such as a rotary is connected to a rear portion of the traveling machine body 1 via a lifting link mechanism 2 so as to be movable up and down. The lifting / lowering link mechanism 2 is suspended by a lift arm 5 via a pair of left and right lift rods 4, and lifts and lowers the work implement 3 according to the hydraulic operation of the lift arm 5. The work implement 3 is tilted left and right in accordance with the hydraulic operation of the lift rod cylinder 6 interposed.

  At a predetermined position of the traveling machine body 1 (or work machine 3), an inclination sensor 7 (capacitance method or the like) that detects the left and right inclination angle of the traveling machine body 1 (or work machine 3), and an angular velocity when the left and right inclination changes are displayed. An angular velocity sensor 8 to detect (quartz tuning fork method or the like) is incorporated. In other words, the output signal of the inclination sensor 7 that has been conventionally employed fluctuates irrespective of the left-right inclination of the work machine 3 or the traveling machine body 1 when the traveling machine body 1 rolls, suddenly starts, or when the clutch is connected. In addition, since the response performance with respect to the inclination change is low, the right and left inclination angles of the work machine 3 are determined in consideration of the detection value of the angular velocity sensor 8. Specifically, when the detected value of the angular velocity sensor 8 is in a fluctuating state, the detected values of the angular velocity sensor 8 are integrated to obtain inclination data, and when the detected value of the angular velocity sensor 8 is in a stable state, the inclined sensor 7 It is possible to determine the left / right inclination angle of the work implement 3 using the detected value of the inclination data as inclination data, and to automatically control the left / right inclination of the work implement 3.

  The traveling machine body 1 is provided with a control unit 9 configured using a microcomputer (including a CPU, a ROM, a RAM, and the like). The control unit 9 includes, as input devices for tilt control, a tilt control switch 10 for turning on / off tilt control, a tilt setting volume 11 for setting a target tilt for tilt control, and a monitor operation switch for performing setting screen operations. 12, a lift arm sensor 13 for detecting the position of the lift arm 5, a position sensor 15 for detecting the operation position of the position lever 14, a lift rod sensor 16 for detecting the cylinder length of the lift rod cylinder 6, and a steering handle 17 The steering angle detection switch 18 for detecting the turning operation of the vehicle, and the tilt sensor 7 and the angular velocity sensor 8 described above are connected. As an output device for tilt control, an extension solenoid of the lift rod electromagnetic switching valve 19 is used. 19a, a reduction solenoid 19b, and a monitor for displaying a setting screen 20 and are connected.

  Then, the control unit 9 sets an inclination target value (lift rod target value) based on the inclination setting data and inclination determination data, and controls the expansion and contraction of the lift rod cylinder 6 according to the deviation of the lift rod sensor data with respect to the target value. However, the control unit 9 of the present embodiment, in executing such tilt control, in addition to the tilt control routine (including the tilt calculation processing routine), a tilt sensitivity operation routine for setting a dead zone for tilt control, An angular velocity processing main routine including the above processing routine, a reference data processing routine executed by timer interruption, an integration processing routine, a smoothing processing routine, and the like.

  The reference data processing routine is a process for calculating the reference data of the angular velocity sensor value, and the stability determination of the angular velocity sensor value is performed based on the reference data calculated here. The integration process routine is a process for integrating (integrating) the angular velocity sensor values to obtain an angle change amount (integration data), and executes the integration process in response to the ON of the integration flag. The smoothing processing routine is a process for sampling a predetermined number of the latest tilt sensor values in a predetermined cycle and calculating an average value (smooth data). The smoothed data calculated here is an angle reference during angle calculation. Used as data.

  Next, the angular velocity processing main routine will be described with reference to FIGS. As shown in FIG. 3, the angular velocity processing main routine includes an initial setting routine (S101), a connection determination routine (S102), a smoothing control routine (S103), a calculation dead zone setting routine (S104), a switching control routine (S105), A subroutine such as an angle calculation routine (S106) is included.

  The initial setting routine performs initial setting of various flags, various timers, various data, and the like. The connection determination routine is a process for determining whether or not the angular velocity sensor 8 is correctly connected to the control unit 9. For example, whether or not the input voltage from the angular velocity sensor 8 is within a preset range. Thus, the connection determination of the angular velocity sensor 8 is performed. The smoothing control routine is a process for setting the sampling period of the inclination sensor value in the above-described smoothing processing routine. For example, when the inclination sensor value is likely to fluctuate, such as immediately after the start of work, the inclination data is accurate even a little. So that the sampling period of the inclination sensor value is shortened. The calculation dead zone setting routine is a process for setting a dead zone used for calculation of the angular velocity sensor value. For example, the calculation dead zone data is changed according to the vehicle speed. The calculation dead zone data set here is used for determining the stability of the angular velocity sensor value.

  The angle calculation routine is a process of calculating the tilt angle of the traveling machine body 1 or the work machine 3 based on the tilt sensor value and the angular velocity sensor value. For example, the tilt reference data including smooth data of the tilt sensor value and the angular velocity sensor value are calculated. It is obtained by adding up integrated data obtained by integrating. However, in this calculation process, it is preferable that the accumulated data is corrected using a predetermined constant in order to match the output levels of both sensors. The switching control routine is a process for switching the tilt determination data used in the tilt control based on the determination of the stability of the angular velocity sensor value. Hereinafter, the switching control routine of this embodiment will be described with reference to FIG.

  As shown in FIG. 4, in the switching control routine, first, a turning determination based on the detection signal of the steering angle detection switch 18 (S201), a determination during a position raising operation based on the detection signal of the position sensor 15 (S202), Based on the detection signal of the lift arm sensor 13, a determination is made that the lift arm is being lifted (S203). Here, if any one of the determinations is YES, a start delay timer (corresponding to TA in FIG. 5) is set in order to delay the start of the tilt control by the angular velocity sensor value (S204). Next, it is determined based on the start delay timer that the start delay time has not elapsed (S205). While this determination is YES, the start delay timer count (S206), the integration flag reset (S207), and the inclination Smooth data is set for the reference data (S208), accumulated data is cleared (S209), work start flag is set (S210), and timeout is set (S211).

  On the other hand, when the start delay time elapses, the work start flag is reset (S212), the timeout (inclination control continuation timer based on the angular velocity sensor value) is counted (S213), and the difference data is calculated (S214). The difference data calculated here is a difference of the angular velocity sensor value with respect to the reference data described above, and specifically, is obtained by subtracting the calculation dead zone data from the absolute value. Next, a calculation determination is made based on the difference data (S215). This determination corresponds to the above-described determination of the stability of the angular velocity sensor value. When the angular velocity sensor value is unstable, it is determined as YES, and when it is stable, it is determined as NO.

  If the determination is NO, in order to perform tilt control using the tilt sensor value, the calculation start flag is reset (S216), the integration flag is reset (S217), the timeout is set (S218), and the tilt reference data is set. Smooth data is set (S219) and accumulated data is cleared (S220). However, immediately after the angular velocity sensor value becomes stable, the inclination control by the angular velocity sensor value is performed for a predetermined time (equivalent to TB in FIG. 5) in consideration of the response delay of the inclination sensor value (corresponding to tl in FIG. 5). To maintain. Specifically, when the angular velocity sensor value is stabilized, the update confirmation timer (TB) is judged to have elapsed (S221), the update confirmation timer is counted (S222), and the integration flag is set (S223). When elapses, S217 to S220 are executed.

  On the other hand, when the angular velocity sensor value is unstable, the calculation start flag is set (S224), the update confirmation timer is cleared (S225), and the integration flag is set (S226), so that the inclination control by the angular velocity sensor value is possible. However, in this state, the elapse of the timeout time is always determined (S227). When the time-out period has elapsed, in order to temporarily eliminate the accumulated error of the accumulated data, the accumulated flag is reset (S228), the smooth data is set for the inclination reference data (S229), and the accumulated data is cleared (S230). )I do. That is, when a situation where the angular velocity sensor value is not stable indefinitely continues, the accumulated data of the angular velocity sensor 8 is forcibly cleared to suppress a decrease in control accuracy due to an accumulated error.

  Next, an inclination control routine (including an inclination calculation processing routine) and an inclination sensitivity operation routine will be described with reference to FIGS. As shown in FIG. 6, in the tilt control routine, first, it is determined whether or not the angular velocity sensor control is performed (S301). This determination is YES when, for example, it is determined that the angular velocity sensor 8 is correctly connected in the connection determination routine (S102) described above. If this determination is NO, the inclination sensor value is set in the inclination data (data used for inclination determination by the inclination control) (S302), a wide dead zone is set (S303), and the inclination control based on the inclination sensor value is performed. Execute.

  On the other hand, if the angular velocity sensor 8 is correctly connected, it is determined whether or not integrated data is being calculated (S304). This determination is made, for example, by referring to the state of the integration flag described above. If the integration flag is set, YES (a state where S224 to S226 or S222 to S223 are executed in the switching control routine), and if a reset, NO (switching) In the control routine, it can be determined that S206 to S211, S228 to S230, or S217 to S220 are being executed. If this determination is YES, the calculation data (smooth data + integrated data) consisting of the angular velocity sensor value is set in the inclination data (S305). If NO, the calculation data consisting of the inclination sensor value (smooth data). Is set to the inclination data (S306). At the same time, the dead zone is set. In the case of tilt control by the angular velocity sensor value, a narrow dead zone is set (S307), and in the case of tilt control by the tilt sensor value, a wide dead zone is set (S308), and then the tilt calculation is performed. A processing routine (S309) is executed. That is, when tilt control is performed based on the detection value of the angular velocity sensor 8 having high response performance, a narrow dead zone is applied in order to make use of the performance. Thereby, the dead zone at the time of the tilt control by the angular velocity sensor value can be optimized, and the highly accurate tilt control utilizing the performance of the angular velocity sensor 8 can be performed.

  As shown in FIG. 7, in the tilt calculation processing routine, first, tilt data is converted into a lift rod position, and a lift rod target value is set (S401). Next, a difference between the lift rod sensor value and the lift rod target value is obtained (S402), and it is determined whether the difference is on the contraction side (S403). If the difference is on the contraction side and the difference is outside the dead zone (S404), the lift rod cylinder 6 is reduced (S405). If the difference is on the extension side and the difference is outside the dead zone (S405) In step S406, the lift rod cylinder 6 is extended (S407). If the difference is within the dead zone, the lift rod cylinder 6 is stopped (S408).

  As shown in FIG. 8, in the tilt sensitivity operation routine, it is first determined whether or not tilt sensitivity processing is being performed (S501). This determination is YES when, for example, an item for setting the tilt sensitivity is selected on the menu screen. When the determination is YES, the operation of the monitor operation switch 12 is determined. For example, when the data SW is up (S502), the sensitivity data is up-processed (S503), when the data SW is down (S504), the sensitivity data is down-processed (S505), and when the decision SW is down (S506), sensitivity data is determined (S507). However, the sensitivity data is subjected to data limit processing so as not to deviate from a predetermined range (0 to 10) (S508). Then, by executing these processes (S501 to S508) together with a display processing routine (S509) for displaying the sensitivity data on a monitor, the tilt sensitivity can be set on the monitor screen.

  According to the tilt sensitivity operation described above, the dead zone for tilt control can be changed as shown in FIG. Here, the wide table dead zone data is the dead zone data set in S303 and S308 of the tilt control routine described above, and the narrow table dead zone data is the dead zone data set in S307. The narrow dead zone applied during tilt control using the angular velocity sensor value is set to be always smaller than the wide dead zone applied during tilt control using the tilt sensor value, and is arbitrarily changed according to the tilt sensitivity operation. be able to. As a result, it is possible to optimize the dead zone applied at the time of tilt control by the angular velocity sensor value by setting and further improve the accuracy.

  The tractor according to the present embodiment configured as described above is in a state in which the detection value of the angular velocity sensor 8 varies when the left and right inclination of the work machine 3 is automatically controlled based on the detection values of the inclination sensor 7 and the angular velocity sensor 8. In this case, the detected values of the angular velocity sensor 8 are integrated to obtain inclination data, and when the detected value of the angular velocity sensor 8 is in a stable state, the left and right inclination angles of the work implement 3 are set using the detected value of the inclination sensor 7 as inclination data. Further, when the state in which the detected value of the angular velocity sensor 8 is set to the inclination data continues for a predetermined time or longer, the accumulated data of the angular velocity sensor 8 is cleared, so that the accumulated error of the accumulated data is suppressed and the accuracy of the inclination control is reduced. Can be improved.

  In addition, when using the tilt data based on the angular velocity sensor 8, the dead zone of the work implement 3 is controlled by applying a narrower dead zone than when the tilt data of the tilt sensor 7 is used. Can be optimized, and highly accurate tilt control utilizing the performance of the angular velocity sensor 8 can be performed. Furthermore, in the present embodiment, since the narrow dead zone can be changed arbitrarily, the dead zone can be optimized to further improve accuracy.

It is a side view of a tractor. It is a block diagram which shows the input / output of a control part. It is a flowchart of an angular velocity processing main routine. It is a flowchart of a switching control routine. It is a timing chart which shows an effect | action. It is a flowchart of an inclination control routine. It is a flowchart of an inclination calculation processing routine. It is a flowchart of an inclination sensitivity operation routine. It is operation | movement explanatory drawing of an inclination sensitivity operation routine.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Traveling machine body 3 Working machine 7 Inclination sensor 8 Angular velocity sensor 9 Control part

Claims (2)

In a mobile agricultural machine that automatically controls the left and right inclination of the work machine based on the detection values of the inclination sensor and the angular velocity sensor,
When the detection value of the angular velocity sensor is in a fluctuating state, the detection value of the inclination sensor is mainly inclined when the detection value of the angular velocity sensor is stable. The left and right tilt angle of the work implement is determined as data, and when using the tilt data based on the angular velocity sensor, the left and right tilt control of the work implement is performed by applying a narrower dead band than when using the tilt data of the tilt sensor. A mobile agricultural machine characterized by   The mobile agricultural machine according to claim 1, wherein the dead zone can be arbitrarily set.

Images