JP6155873B2 - Construction machinery - Google Patents

Description

  The present invention relates to a construction machine including a lower swing body and an upper swing body that is turnable on the lower swing body.

  A hydraulic excavator will be described as an example.

  The hydraulic excavator includes a lower traveling body, an upper revolving body that is turnable on the lower traveling body, a boom that is undulated on the upper revolving body, an arm that is provided at a tip of the boom, an arm And a bucket provided at the tip of the.

  As this type of hydraulic excavator, one having a sensor capable of detecting a boom angle, an arm angle, a bucket angle, a turning angle, and the like in order to specify the working state (working posture) is known (for example, Patent Document 1).

  There is also known a construction machine having a sensor for detecting the angular velocity of the turning motion of the upper turning body relative to the lower traveling body.

  Here, as a sensor for detecting the angular velocity, for example, an encoder, a resolver, or a gyro sensor can be used.

  However, since the encoder and the resolver detect the angular velocity based on the rotational position of the turning shaft of the upper turning body, it is necessary to mechanically attach the encoder and the resolver to the turning shaft.

  On the other hand, the gyro sensor detects the angular velocity based on the Coriolis force generated according to the turning motion of the upper swing body with respect to the detection unit, and therefore can be attached to a position other than the swing axis in the upper swing body. it can.

  Therefore, the gyro sensor is advantageous in terms of attachment to the upper swing body compared to the encoder and the resolver.

JP 2011-58269 A

  Here, the gyro sensor can detect the angular velocity around the specified detection axis, but if the parallelism between this detection axis and the turning axis of the upper turning body is not maintained, the angular velocity of the turning motion is accurately detected. Can not do it.

  Therefore, in order to accurately detect the angular velocity, it is necessary to attach the gyro sensor to the upper swing body with the detection shaft of the gyro sensor and the swing shaft accurately positioned, and time and effort for adjusting the mounting position are required. There is a problem of being big.

  An object of the present invention is to provide a construction machine that can omit the positioning of the detection shaft of the gyro sensor and the swing shaft of the upper swing body.

  In order to solve the above-described problems, the present invention provides a construction machine including a lower traveling body and an upper swing body provided on the lower traveling body so as to be capable of swiveling around a preset swing axis. An X-axis gyro sensor for detecting an angular velocity around a preset X detection axis, a Y-axis gyro sensor for detecting an angular velocity around a Y detection axis orthogonal to the X detection axis, the X detection axis, and the A Z-axis gyro sensor that detects an angular velocity around a Z detection axis that is orthogonal to the Y detection axis, and an angular velocity calculation means that calculates an angular velocity of the upper swing body around the swing axis based on a detection result by each gyro sensor. The angular velocity calculation means includes a three-dimensional coordinate system for detection defined by the X detection axis, the Y detection axis, and the Z detection axis, and a plane perpendicular to the Z reference axis that coincides with the turning axis. On each other The Z reference is corrected by correcting the deviation between the detection three-dimensional coordinate system and the reference three-dimensional coordinate system by performing coordinate transformation to a reference three-dimensional coordinate system defined by an X reference axis and a Y reference axis orthogonal to In order to calculate the angular velocity around the axis, (A) the Z detection axis rotates around the X reference axis by a first angle with respect to the Z reference axis, and rotates around the Y reference axis by a second angle. The X reference among the three relational expressions representing the angular velocities around the three axes in the reference three-dimensional coordinate system using the detection results by the respective gyro sensors, the first angle, and the second angle. The first angle and the second angle are calculated based on these two relational expressions by using the fact that the angular velocities represented by the two relational expressions related to the axis and the Y reference axis are 0, respectively (B ) Of the above three relational expressions A relationship for the serial Z reference axis, and calculates the Z reference axis of the angular velocity on the basis of the calculated first angle and the second angle, provides a construction machine.

  During the turning operation of the upper turning body, the upper turning body operates around the Z reference axis (turning axis) with respect to the lower traveling body, while around the X reference axis and the Y reference axis perpendicular to the Z reference axis. Do not work.

  In other words, among the three relational expressions representing the angular velocities around the three axes in the reference three-dimensional coordinate system, the angular velocities represented by the two relational expressions relating to the X reference axis and the Y reference axis are zero.

  In the present invention, this point can be used to specify the deviation (first angle and second angle) of the detection three-dimensional coordinate system from the reference three-dimensional coordinate system based on the two relational expressions.

  Then, based on the first angle and the second angle and the remaining one relational expression, it is possible to calculate an angular velocity around the Z reference axis in consideration of the correction amount of the shift of each coordinate system.

  Therefore, the angular velocity around the Z reference axis (swivel axis) can be calculated by correcting the shift of each coordinate system by the angular velocity calculation means without finely adjusting the position of each gyro sensor with respect to the upper swing body.

  Therefore, according to the present invention, positioning of the detection axis of each gyro sensor and the turning axis (Z reference axis) of the upper turning body can be omitted.

  In the construction machine, the X-axis gyro sensor, the Y-axis gyro sensor, and the Z-axis gyro sensor are integrated in advance with the X detection axis, the Y detection axis, and the Z detection axis orthogonal to each other. It is preferable that

  According to this aspect, it is possible to further reduce the trouble of positioning the respective gyro sensors so that the respective detection axes are orthogonal to each other.

  According to the present invention, positioning of the detection shaft of the gyro sensor and the turning shaft of the upper turning body can be omitted.

It is a right view which shows the whole structure of the hydraulic shovel which concerns on embodiment of this invention. FIG. 2 is a block diagram showing an electrical configuration of the hydraulic excavator in FIG. 1. It is an XY plan view showing a deviation between a reference three-dimensional coordinate system and a detection three-dimensional coordinate system. It is a YZ plan view showing a deviation between a reference three-dimensional coordinate system and a detection three-dimensional coordinate system. It is a flowchart which shows the process performed by the angular velocity calculation means of FIG.

  Embodiments of the present invention will be described below with reference to the accompanying drawings. The following embodiments are examples embodying the present invention, and are not of a nature that limits the technical scope of the present invention.

  A hydraulic excavator 1 as an example of a construction machine according to an embodiment of the present invention will be described with reference to FIGS. 1 and 2.

  The hydraulic excavator 1 is attached to a lower traveling body 2 having a crawler 2a, an upper revolving body 3 provided on the lower traveling body 2 so as to be rotatable about a Z reference axis Zj (swivel axis), and the upper revolving body 3. The work attachment 4 and the angular velocity sensor 11 and the angular velocity calculation means 12 provided on the upper swing body 3 are provided.

  The work attachment 4 is provided with a boom 5 that can be raised and lowered with respect to the upper swing body 3, an arm 6 that is rotatably attached to the distal end portion of the boom 5, and a pivotable portion at the distal end portion of the arm 6. And a bucket 7 attached to.

  The work attachment 4 also includes a boom cylinder 8 that raises and lowers the boom 5 relative to the upper swing body 3, an arm cylinder 9 that rotates the arm 6 relative to the boom 5, and a bucket 7 that rotates relative to the arm 6. And a bucket cylinder 10 to be operated.

  2 to 4, the angular velocity sensor 11 includes an X-axis gyro sensor 11a that detects an angular velocity around a preset X detection axis Xb, and an angular velocity around a Y detection axis Yb that is orthogonal to the X detection axis Xb. And a Z-axis gyro sensor 11c for detecting an angular velocity around the Z detection axis Zb orthogonal to the X detection axis Xb and the Y detection axis Yb.

  Here, in the angular velocity sensor 11, the gyro sensors 11a to 11c are integrated in advance in a state where the X detection axis Xb, the Y detection axis Yb, and the Z detection axis Zb are orthogonal to each other.

  In the following description, the three-dimensional coordinate system defined by the X detection axis Xb, the Y detection axis Yb, and the Z detection axis Zb is a detection three-dimensional coordinate system (indicated by a two-dot chain line in FIGS. 3 and 4). That's it. Further, a three-dimensional coordinate system defined by a Z reference axis Zj (swivel axis) and an X reference axis Xj and a Y reference axis Yj orthogonal to each other on a plane orthogonal to the Z reference axis Zj is referred to as a reference three-dimensional coordinate system (FIGS. 3 and (Indicated by a solid line in 4).

  The angular velocity calculation means 12 calculates the angular velocity of the upper swing body 3 around the Z reference axis Zj (swivel axis) based on the detection results by the gyro sensors 11a to 11c.

  Specifically, the angular velocity calculation unit 12 includes an angular velocity detection unit 12a that reads detection results from the respective gyro sensors 11a to 11c, and a three-dimensional coordinate for detection by converting the three-dimensional coordinate system for detection into a reference three-dimensional coordinate system. And an angular velocity correcting unit 12b that corrects a deviation between the system and the reference three-dimensional coordinate system to calculate an angular velocity about the Z reference axis Zj.

  Hereinafter, as shown in FIGS. 3 and 4, the Z detection axis Zb rotates about the X reference axis Xj by the first angle θ with respect to the Z reference axis Zj, and rotates by the second angle α about the Y reference axis Yj. In this case, a method in which the angular velocity correcting means 12b calculates the angular velocity around the Z reference axis Zj will be described.

  In the state shown in FIGS. 3 and 4, a coordinate transformation matrix for coordinate transformation from the detection three-dimensional coordinate system to the reference three-dimensional coordinate system is represented by Equation 1.

  That is, Equation 1 is for rotating the three-dimensional coordinate system about the X reference axis Zj by the first angle θ and rotating the three-dimensional coordinate system about the Y reference axis Yj by the second angle α. The fourth row and the fourth column in Equation 1 indicate that the translation of the three-dimensional coordinate system is not performed by Equation 1.

  Using this equation 1, the angular velocity ωxb around the X detection axis Xb, the angular velocity ωyb around the Y detection axis Yb, and the angular velocity ωzb around the Z detection axis Zb detected by each gyro sensor 11a-11c, The angular velocity ωxj around Xj, the angular velocity ωyj around the Y reference axis Yj, and the angular velocity ωxj around the Z reference axis Zj can be expressed as in Equation 2.

  By expanding the equation 2, as shown in the following equations 3 to 5, the angular velocities ωxj, ωyj, and ωzj are converted into the detected angular velocities ωxb, ωyb, ωzb, the first angle θ, and the second angle α. Can be used.

  Here, since the upper swing body 3 does not move around the X reference axis Xj during the turning operation of the upper swing body 3, the value of the angular velocity ωxj around the X reference axis Xj is 0 as shown in Equation 6 below. .

  Similarly, since the upper swing body 3 does not move around the Y reference axis Yj during the swing operation of the upper swing body 3, the value of the angular velocity ωyj around the Y reference axis Yj is also 0 as shown in the following Expression 7. .

  This equation 7 can be transformed into the following equation 8, and based on this equation 8, as shown in equation 9, the first angle θ is changed only to the detected values ωyb and ωzb by the respective gyro sensors 11b and 11c. Can be expressed as Therefore, the first angle θ can be obtained based on Equation 9.

  On the other hand, the equation 6 can be transformed into the following equation 10, and based on the equation 10, the second angle α is expressed by the angular velocities ωxb, ωyb, ωzb and the first angle θ as shown in the equation 11. Can be represented. Therefore, the second angle α can be obtained based on Equation 11 using the first angle θ obtained as described above.

  Then, the angular velocity ωzj around the Z reference axis Zj (swivel axis) can be obtained by substituting the first angle θ and the second angle α thus obtained into the equation 5.

  Hereinafter, the process executed by the angular velocity calculating unit 12 will be described with reference to FIGS.

  First, angular velocities ωxb, ωyb, and ωzb detected by the respective gyro sensors 11a to 11c are detected by the angular velocity detecting means 12a (step S1).

  Next, the first angle θ and the second angle α are calculated based on the angular velocities ωxb, ωyb, ωzb detected in Step S1 and Equations 9 and 11 (Step S2).

  Then, the angular velocity ωzj around the Z reference axis Zj (swivel axis) is calculated based on the first angle θ and the second angle α calculated in step S2 and the equation 5, and the process returns.

  As described above, during the turning motion of the upper swing body 3, the angular velocities represented by the two equations 4 and 5 relating to the X reference axis Xj and the Y reference axis Yj among the three relational expressions of equations 3 to 5 are as follows. The first angle θ and the second angle α can be specified using a point that becomes zero.

  Then, based on the first angle θ and the second angle α and Equation 5, the angular velocity ωzj around the Z reference axis Zj to which the correction amount of the deviation between the detection three-dimensional coordinate system and the reference three-dimensional coordinate system is added is calculated. Can be calculated.

  Therefore, even if the position of each gyro sensor 11a-11c with respect to the upper turning body 3 is not adjusted finely, the angular velocity calculation means corrects the deviation of each coordinate system, and calculates the angular velocity around the Z reference axis Zj (swivel axis). be able to.

  Therefore, positioning of the detection axes Xb, Yb, Zb of the respective gyro sensors 11a to 11c and the turning axis (Z reference axis Zj) of the upper turning body 3 can be omitted.

  In the embodiment, the gyro sensors 11a to 11c are integrated in advance so that the X detection axis Xb, the Y detection axis Yb, and the Z detection axis Zb are orthogonal to each other. It is possible to further reduce the time and labor for positioning.

  In addition, although the said embodiment demonstrated the angular velocity sensor 11 with which each gyro sensor 11a-11c was integrated previously, each gyro sensor 11a-11c does not need to be integrated.

  Also in this case, the gyro sensors 11a to 11c are positioned so that the X detection axis Xb, the Y detection axis Yb, and the Z detection axis Zb are orthogonal to each other, and in this state, the gyro sensors 11a to 11c are moved to the upper swing body. 3, the positioning of the detection axes Xb, Yb, Zb and the Z reference axis Zj (swivel axis) of each of the gyro sensors 11a to 11c can be omitted.

Xb X detection axis (X axis defining three-dimensional coordinates for detection)
Yb Y detection axis (Y axis defining three-dimensional coordinates for detection)
Zb Z detection axis (Z axis defining 3D coordinates for detection)
Xj X reference axis (X axis defining reference three-dimensional coordinates)
Yj Y reference axis (Y axis that defines reference three-dimensional coordinates)
Zj Z reference axis (Z axis that defines reference three-dimensional coordinates: swivel axis)
θ 1st angle α 2nd angle ωxb X angular velocity around the detection axis ωyb Y angular velocity around the detection axis ωzb Z angular velocity around the detection axis ωxj X angular velocity around the reference axis (angular velocity of 0)
ωyj Angular velocity around Y reference axis (angular velocity that becomes 0)
ωzj Angular velocity around the Z reference axis 1 Hydraulic excavator (an example of construction machinery)
2 Lower traveling body 3 Upper turning body 11a X-axis gyro sensor 11b Y-axis gyro sensor 11c Z-axis gyro sensor 12 Angular velocity calculation means

Claims (2)

A construction machine comprising a lower traveling body and an upper swing body provided on the lower traveling body so as to be capable of swinging around a preset swing axis,
An X axis gyro sensor for detecting an angular velocity around a preset X detection axis;
A Y-axis gyro sensor for detecting an angular velocity around a Y detection axis orthogonal to the X detection axis;
A Z-axis gyro sensor that detects an angular velocity around a Z detection axis orthogonal to the X detection axis and the Y detection axis;
Angular velocity calculation means for calculating an angular velocity of the upper swing body around the swing axis based on a detection result by each gyro sensor;
The angular velocity calculation means sets a three-dimensional coordinate system for detection defined by the X detection axis, the Y detection axis, and the Z detection axis on a plane perpendicular to the Z reference axis that coincides with the turning axis. The Z reference is corrected by correcting the deviation between the detection three-dimensional coordinate system and the reference three-dimensional coordinate system by performing coordinate transformation to a reference three-dimensional coordinate system defined by an X reference axis and a Y reference axis orthogonal to each other. In order to calculate the angular velocity around the axis,
(A) When the Z detection axis is rotated by the first angle around the X reference axis with respect to the Z reference axis and is rotated by the second angle around the Y reference axis, the respective gyro sensors Of the three relational expressions representing the angular velocities around the three axes in the reference three-dimensional coordinate system using the detection result, the first angle, and the second angle, two relations regarding the X reference axis and the Y reference axis Using the fact that the angular velocities represented by the equations are each 0, the first angle and the second angle are calculated based on these two relational expressions,
(B) A construction machine that calculates an angular velocity about the Z reference axis based on the relational expression related to the Z reference axis among the three relational expressions and the calculated first angle and second angle.   The X-axis gyro sensor, the Y-axis gyro sensor, and the Z-axis gyro sensor are integrated in advance with the X detection axis, the Y detection axis, and the Z detection axis orthogonal to each other. Item 2. The construction machine according to Item 1.

Images