JP2019174300A - Detection device for tilt angle of work vehicle - Google Patents

Abstract

To provide a detection device for a tilt angle of a work vehicle which allows for improvement in calculation accuracy for the tilt angle.SOLUTION: A work vehicle is provided that comprises a controller (50) which receives, as input, each signal from an acceleration sensor (32), an angular velocity sensor (33), and a velocity sensor (31), and which calculates the tilt angle in a pitch direction of a vehicle body. In the work vehicle, the controller differentiates (51) a speed signal detected with a velocity sensor to obtain an acceleration component, removes the obtained acceleration component from the acceleration signal detected with the acceleration sensor, calculates (52) the tilt angle, and sets (53) an observation error in such a manner that if an observation error is large, the speed of a vehicle body is large, and if the speed is small, the observation error is small, and performs Kalman filter processing to update (60) the tilt angle of the vehicle body and offset of the angular velocity sensor based on the calculated tilt angle, the set observation error, and the angular velocity signal.SELECTED DRAWING: Figure 3

Description

  The present invention relates to a work vehicle tilt angle detection device used in a work vehicle such as a wheel loader.

  As background art of this technical field, for example, Patent Document 1 includes an acceleration sensor and a speed sensor mounted on a vehicle body, and includes an observation error of the acceleration sensor, acceleration detected by the acceleration sensor, and speed detected by the speed sensor. Based on this, a control technique for updating the tilt angle (also referred to as attitude angle) of the vehicle body and the offset of the acceleration sensor by executing a known Kalman filter process is disclosed.

JP 2011-102784 A

  However, in the prior art described in Patent Document 1, the tilt angle at which the vehicle body tilts can be detected and corrected with a certain degree of accuracy. However, since the angular velocity is not taken into account, there is still room to detect the tilt angle with a higher degree of accuracy. It is left. In addition, if the prior art described in Patent Document 1 is applied to a work vehicle that mainly travels on rough terrain such as a wheel loader, it is affected by vibration, and the tilt angle of the vehicle body can be obtained with high accuracy. It may not be possible.

  An object of the present invention is to provide a tilt angle detection device for a work vehicle that can improve the accuracy of calculating the tilt angle even when traveling on rough terrain where vehicle body vibration is likely to occur.

  In order to achieve the above object, a representative invention is mounted on a vehicle body, detects an acceleration in the longitudinal direction of the vehicle body, and outputs the detected acceleration as an acceleration signal, and is mounted on the vehicle body. An angular velocity sensor for detecting an angular velocity in the pitch direction of the vehicle body and outputting the detected angular velocity as an angular velocity signal; and for detecting the velocity of the vehicle body and outputting the detected velocity as a velocity signal. And a controller that inputs signals from the acceleration sensor, the angular velocity sensor, and the speed sensor, and calculates a tilt angle in the pitch direction of the vehicle body. An acceleration component is obtained by differentiating the speed signal detected by the speed sensor, and the acceleration component is converted into the acceleration sensor. The inclination angle is calculated by removing from the acceleration signal detected at, and according to the speed signal detected by the speed sensor, the observation error increases as the vehicle speed increases, and the speed is The observation error is set to be smaller as the observation error is smaller, and based on the calculated tilt angle, the set observation error, and the angular velocity signal detected by the angular velocity sensor, a Kalman filter A process is executed to update the tilt angle of the vehicle body and the offset of the angular velocity sensor.

  According to the tilt angle detection device for a work vehicle according to the present invention, it is possible to improve the accuracy of calculating the tilt angle even when traveling on rough terrain where vibration of the vehicle body is likely to occur. Problems, configurations, and effects other than those described above will be clarified by the following description of embodiments.

It is a side view of the wheel loader concerning the embodiment of the present invention. It is a block diagram which shows typically the hardware constitutions of a controller. It is a block diagram which shows the function structure of a controller. It is a figure which shows the coordinate system used for the calculation by a controller. It is a flowchart which shows the estimation procedure of an inclination angle. It is a flowchart which shows the procedure of a Kalman filter process. It is a figure for demonstrating the effect of this embodiment. It is a figure for demonstrating the effect of a modification.

  Hereinafter, a wheel loader to which a tilt angle detection device for a work vehicle according to the present invention is applied will be described as an example with reference to the drawings of the embodiments.

  FIG. 1 is a side view of a wheel loader 1 according to an embodiment of the present invention. As shown in FIG. 1, the wheel loader 1 includes a front frame (vehicle body) 5 having a pair of booms 2, a bucket 3, a pair of front wheels 4, a cab 6, an engine compartment 7, a pair of rear wheels 8, and the like. The rear frame (vehicle body) 9 is provided. An engine 25 is mounted in the engine chamber 7, and a counterweight 10 is attached to the rear of the rear frame 9.

  The pair of booms 2 is turned up and down (up and down) by driving the pair of boom cylinders 11, and the bucket 3 is turned up and down (clouding or dumping) by driving the bucket cylinder 12. A link mechanism including a bell crank 13 is interposed between the bucket cylinder 12 and the bucket 3, and the bucket cylinder 12 rotates the bucket 3 via this link mechanism. The pair of booms 2, the bucket 3, the pair of boom cylinders 11, the bucket cylinder 12, the bell crank 13 and the like constitute a work implement 14.

  The front frame 5 and the rear frame 9 are rotatably connected to each other by a center pin 15, and the front frame 5 is refracted right and left with respect to the rear frame 9 by expansion and contraction of a steering cylinder (not shown). A driver's cab 6 mounted on the front portion of the rear frame 9 includes a driver's seat where an operator sits, a steering wheel that controls the steering angle of the wheel loader 1, a key switch that starts and stops the wheel loader 1, and information to the operator. A display device (none of which is shown) is provided.

  Further, the wheel loader 1 includes a speed sensor 31 for detecting a speed (traveling speed) corresponding to the moving speed of the wheel loader 1, an acceleration sensor 32 for detecting acceleration corresponding to the front-rear direction of the wheel loader 1, An angular velocity sensor 33 for detecting an angular velocity corresponding to the pitch direction of the wheel loader 1 is provided. Each signal from each of the sensors 31, 32, 33 is input to the controller 50. The controller 50 calculates a vehicle body inclination angle θ (see FIG. 4B), which will be described later, based on each input signal. In place of the acceleration sensor 32 and the angular velocity sensor 33, an IMU (Internal Measurement Unit / Inertial Measurement Device) that detects the vehicle acceleration and the vehicle angular velocity may be used.

  FIG. 2 is a block diagram schematically showing the hardware configuration of the controller 50. As shown in FIG. 2, the controller 50 includes a CPU (Central Processing Unit) 50A that performs various calculations for controlling the overall operation of the vehicle body, and a ROM (Read Only Memory) that stores a program for executing calculations by the CPU 50A. ) 50B storage device, RAM (Random Access Memory) 50C, which is a work area when the CPU 50A executes a program, and an input / output interface 50D for inputting / outputting various information and signals to / from an external device It is composed of hardware including

  In such a hardware configuration, the program stored in the ROM 50B is read out to the RAM 50C and operated according to the control of the CPU 50A, whereby the program (software) and the hardware cooperate to realize the function of the controller 50. Functional blocks are configured.

FIG. 3 is a block diagram showing a functional configuration of the controller 50. As shown in FIG. 3, the speed signal V output from the speed sensor 31, the acceleration signal a _x output from the acceleration sensor 32, and the angular velocity signal θ dot output from the angular velocity sensor 33 are input to the controller 50. The vehicle body inclination angle θ is finally output by the calculation described below. The controller 50 includes a translational acceleration removing unit 51, an inclination angle calculating unit 52, an observation error setting unit 53, and a state estimating unit 60 including a Kalman filter. The state estimation unit 60 includes an estimated value calculation unit 61, an observation residual calculation unit 62, a Kalman gain calculation unit 63, a state variable update unit 64, and a state variable storage unit 65.

  Details of the functions of the respective units will be described below. The following description follows the coordinate system shown in FIG.

The estimated value calculator 61 receives the angular velocity signal θ dot from the angular velocity sensor 33. Here, the angular velocity signal at time k is denoted as θ _k dots, and the state variable x _k at time _k is defined. The state variable x _k includes an inclination angle θ and an angular velocity offset θ _b dot, and is represented by the following Equation 1.

The estimated value calculation unit 61 calculates an estimated value x _k hat at time k according to the state equation shown in Expression 2 based on the input u _k and the state variable x _k−1 one time before. Here, in Equation 2, the input u _k is θ _k dots. The state variable x _k−1 is input from the state variable storage unit 65.

When Δt is a sampling interval, the system matrix A in Expression 2 can be expressed as Expression 3, and the input matrix B can be expressed as Expression 4.

The observation residual calculator 62 receives the observed value z _k of the tilt angle θ from the tilt angle calculator 52 and the estimated value x _k hat from the estimated value calculator 61, and the observed value z _k and the estimated value x according to Equation 5: The observation residual y _k with _k hat is calculated.

Here, the output matrix H is expressed as Equation 6.

Further, the observed value Z _k can be calculated by Equation 7. Here, in Expression 7, a _xk is an acceleration signal of the acceleration sensor, V _k−1 and V _k are speed signals of the speed sensor, and g is a gravitational acceleration.

Translational acceleration removing unit 51 inputs the acceleration signal a _x from the speed signal V and the acceleration sensor 32 from the speed sensor 31, from the acceleration signal a _x, removes the acceleration component obtained by differentiating the velocity signal V . The tilt angle calculation unit 52 calculates the observation value Z _{k according} to Equation 7 described above based on the acceleration signal from which the acceleration component has been removed by the translational acceleration removal unit 51.

The observation error setting unit 53 inputs the speed signal V from the speed sensor 31 and sets the observation error δθ _k of the observation value Z _k at time k. Observation error .delta..theta _k observations Z _k from can vary from scene such as when moving or when the vehicle is stopped, the observation error setting unit 53, observed by Equation 8 as defined velocity signal V a (vehicle speed) as a component error .delta..theta _k Is calculated. As shown below, this equation 8 is defined such that it decreases as the speed signal V decreases and increases as the speed signal increases. Here, in Equation 8, α and β are coefficients.

In addition to calculating the observation error δθ _k using Equation 8 above, for example, a table in which the correspondence relationship between the speed signal V _k and the observation error δθ _k of the observation value Z _k is defined in advance in the ROM 50B or the like. stored advance, observation error setting unit 53 may determine the observation error .delta..theta _k with reference to this table.

なお、共分散行列Ｒ_ｋ等からカルマンゲインＫ_ｋを算出する方法は、公知であるため、ここでの説明は省略する。 The Kalman gain calculation unit 63 receives the estimated value x _k hat from the estimated value calculation unit 61 and the observation error δθ _k from the observation error setting unit 53, and based on the system error Q, the covariance matrix R _{k, and the} like. Then, the Kalman gain K _k of the Kalman filter 60 is calculated. Observation error .delta..theta _k observations Z _k is as representing the standard deviation, defines the covariance matrix R _k as in Equation 9.

Note that a method for calculating the Kalman gain K _k from the covariance matrix R _k or the like is well known, and thus the description thereof is omitted here.

The state variable update unit 64 inputs the observation residual y _k from the observation residual calculation unit 62, inputs the estimated value x _k hat from the estimated value calculation unit 61, and inputs the Kalman gain K _k from the Kalman gain calculation unit 63. To do. The state variable updating unit 64 updates the state variable x _k by calculating Equation 10 based on the estimated value x _k hat, the gain K _k , and the observation residual y _k . That is, the state variable update unit 64 updates the inclination angle θ of the wheel loader 1 and the offset of the angular velocity sensor 33.

Then, the state variable updating unit 64 outputs the inclination angle θ of the state variable x _k which updated. The updated state variable x _k is stored in the state variable storage unit 65 and used for the estimated value x _{k + 1} hat after the next discrete time.

Next, a procedure for calculating the tilt angle θ by the controller 50 will be described. FIG. 5 is a flowchart showing a procedure for estimating the inclination angle θ. As shown in FIG. 5, the controller 50 acquires the velocity signal V from the velocity sensor 31 (S10), acquires the acceleration signal a _x from the acceleration sensor 32 (S12), and acquires the angular velocity signal θ dot from the angular velocity sensor 33. (S13). The controller 50 differentiates the velocity signal V removes acceleration components (S4), calculates the inclination angle on the basis of the acceleration signal acceleration components are removed obtained by differentiating the velocity signal V from the acceleration signal a _x (S5). Next, the controller 50 sets the observation error δθ _k (S6), executes the Kalman filter process (S7), and outputs the tilt angle θ (S8). If the process is continued (S9 / Yes), the process returns to S1. If the process is not continued (S9 / No), the process ends.

Next, details of the Kalman filter process executed in S7 will be described. FIG. 6 is a flowchart showing the procedure of the Kalman filter process. As shown in FIG. 6, the estimated value calculation unit 61 acquires the previous state variable x _k (S11), calculates the estimated value x _k hat (S12), and then the observation residual calculation unit 62 The difference y _k is calculated (S13). Then, the Kalman gain computation unit 63 computes the Kalman gain _{K k} (S14). Next, the state variable update unit 64 updates the state variable (S15), and the state variable storage unit 65 stores the updated state variable (S16).

(effect)
According to this embodiment, if the Kalman gain K _k is small, the weight of the estimated value x _k hat is large, and if the Kalman gain K _k is large, the weight of the observation residual y _k is large. The magnitude of the Kalman gain K _k depends on the set value of the observation error δθ _k by the observation error setting unit 53. If the set value of the observation error δθ _k is small, the Kalman gain K _k is increased and the setting of the observation error δθ _k is set. If the value is large, the Kalman gain _Kk becomes small. In the present embodiment, the observation error δθ _k is set to a larger value when the speed signal (vehicle speed) V is in the high speed region than when the speed signal (vehicle speed) V is in the low speed region. That is, when the speed signal (vehicle speed) V is in the high speed region, the weight of the estimated value x _k hat increases.

  The inclination angle calculation by the acceleration sensor 32 has high detection accuracy when the wheel loader 1 is stopped, but is affected by translational motion and vibration during traveling, and the detection accuracy is reduced (angular velocity angle conversion in FIG. 7b). On the other hand, the angular velocity sensor 33 can be detected without being affected by the translational movement even while the wheel loader 1 is traveling. However, since the sensor output value includes an offset, an error accumulates when integrating for calculating the inclination angle ( Angular velocity integration in FIG. 7b). In the present embodiment, the observation error is set based on the speed signal (vehicle speed) V, and the inclination angle θ and the offset of the angular velocity sensor 33 are updated based on the observation error. Therefore, the acceleration sensor that changes according to the speed signal (vehicle speed) The offset of the tilt angle θ and the angular velocity sensor 33 reflecting the change in the detection accuracy of 32 can be obtained.

  This will be described with reference to FIG. When the observation error is a constant value with a low value (R fixed 1 in FIG. 7c), the tilt angle can be estimated satisfactorily while the vehicle speed is low (1 to 2 seconds of the vehicle speed in FIG. 7a) (FIG. 7d). If the vehicle speed increases (2 to 10 seconds in FIG. 7a), the accuracy of the inclination angle calculation by the acceleration sensor 32 decreases (2 to 10 seconds in FIG. 7b), and the inclination angle θ Is also affected and the accuracy deteriorates (2 to 10 seconds of angle estimation (R fixed 1) in FIG. 7d). On the other hand, when the observation error is set to a constant value with a high value (R fixed 2 in FIG. 7c), the offset estimation accuracy of the angular velocity sensor 33 deteriorates, and the inclination angle estimation error accumulates over time ( Angle estimation (R fixed 2) in FIG. 7d for 1-2 seconds). When the observation error is changed according to the vehicle speed (R variable in FIG. 7c), the change in the accuracy of the acceleration sensor 32 (acceleration angle conversion in FIG. 7b) that changes with the vehicle speed is reflected in the inclination angle θ and the offset of the angular velocity sensor 33. It can be seen that the tilt angle can be estimated well (angle estimation (R variable) in FIG. 7d). Therefore, by setting the observation error based on the speed signal (vehicle speed), the inclination angle θ of the wheel loader 1 can be detected with high accuracy even when traveling on rough terrain where vehicle body vibration is likely to occur. (See FIG. 7).

  As described above, based on the inclination angle θ obtained in the present embodiment, for example, by changing the transmission schedule of the transmission, it is possible to carry out an optimal shift in a scene on an uphill road and reduce the shift frequency. It becomes possible.

(Modification)
In the present embodiment, by performing a filtering process on the set value of the observation error .delta..theta _k, at the rise of the set value of the observation error .delta..theta _k is fast, may be set to be slower when the fall (in Figure 8c R variable). Thus, immediately after stopping from the time of vehicle movement can also prevent the observation error .delta..theta _k is immediately reduced. Immediately after the vehicle is moved and immediately after stopping, there is a vibration of the vehicle body that occurs when the vehicle is stopped, and the detection accuracy of the acceleration sensor 32 is low. That is, the observation error is large during vibration. By adopting the configuration as in the modified example, it is possible to appropriately set the observation error, and it is possible to prevent the accuracy of the inclination angle estimation from deteriorating (see FIG. 8).

  The embodiments described above are examples for explaining the present invention, and are not intended to limit the scope of the present invention only to those embodiments. Those skilled in the art can implement the present invention in various other modes without departing from the gist of the present invention.

1 Wheel loader (work vehicle)
5 Front frame (car body)
9 Rear frame (car body)
31 Speed sensor 32 Acceleration sensor 33 Angular speed sensor 50 Controller

Claims (2)

An acceleration sensor mounted on a vehicle body for detecting acceleration in the longitudinal direction of the vehicle body and outputting the detected acceleration as an acceleration signal;
An angular velocity sensor mounted on the vehicle body for detecting an angular velocity in the pitch direction of the vehicle body and outputting the detected angular velocity as an angular velocity signal;
A speed sensor mounted on the vehicle body for detecting the speed of the vehicle body and outputting the detected speed as a speed signal;
In a tilt angle detection device for a work vehicle, comprising: a controller that inputs signals from the acceleration sensor, the angular velocity sensor, and the velocity sensor, and calculates a tilt angle in the pitch direction of the vehicle body.
The controller is
Differentiating the speed signal detected by the speed sensor to obtain an acceleration component, removing the acceleration component from the acceleration signal detected by the acceleration sensor, calculating the tilt angle,
In accordance with the speed signal detected by the speed sensor, the observation error is set so that the observation error increases as the speed of the vehicle body increases, and the observation error decreases as the speed decreases.
Based on the calculated tilt angle, the set observation error, and the angular velocity signal detected by the angular velocity sensor, a Kalman filter process is executed to perform an inclination angle of the vehicle body and an offset of the angular velocity sensor. An inclination angle detecting device for a work vehicle, characterized in that: The apparatus for detecting a tilt angle of a work vehicle according to claim 1,
The controller is
Performing filtering on the observation error;
An apparatus for detecting a tilt angle of a work vehicle, wherein the observation error is set so as to be different when the observation error increases and to be different when the observation error decreases.

Images