JP7221152B2 - Turning angle detector - Google Patents

Description

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a turning angle detection device applied to heavy equipment having a turning mechanism.

Some construction machines, such as backhoes and cranes, have an upper mechanism that swings with respect to a lower mechanism. In order to perform unmanned construction using a heavy machine having such a turning mechanism, it is necessary to detect the turning angle of the upper mechanism in order to grasp the correct position and orientation of the heavy machine. The turning angle of the upper mechanism can be measured by a gyro sensor provided in the upper mechanism, but an inertial sensor such as a gyro sensor causes a drift error, resulting in a decrease in accuracy over time.
Other methods of detecting the turning angle of the upper mechanism include a method of measuring the range of multiple prisms attached to the heavy machinery using a total station, and a method of detecting the position using multiple positioning sensors (e.g. GPS receivers) installed on the heavy machinery. There are ways to do so.
However, in the method using a total station, it is necessary to secure a place for installing the total station, and the movement range of the heavy machinery is limited to the range in which the total station can measure the distance. In addition, the method using positioning sensors requires installation of a large number of positioning sensors in order to improve accuracy.
Further, Patent Literature 1 discloses a turning angle detection device that detects a turning angle of an upper mechanism by an encoder installed on a turning shaft of heavy machinery. However, the installation work of the encoder is costly and time-consuming.

JP 2015-160676 A

SUMMARY OF THE INVENTION An object of the present invention is to provide a turning angle detection device that can be easily installed in a heavy machine having a turning mechanism and that can accurately detect the turning angle of the upper mechanism with respect to the lower mechanism.

In order to solve the above problems, the present invention is a turning angle detection device for detecting the turning angle of an upper mechanism that turns on a lower mechanism.
The turning angle detection device of the first invention comprises an inertial sensor for detecting the angular velocity or angular acceleration of the upper mechanism, and an angle calculator for calculating the turning angle of the upper mechanism based on the angular velocity or angular acceleration detected by the inertial sensor. means, a detection body fixed to the lower mechanism, a detection sensor fixed to the upper mechanism, and angle correction means for correcting the turning angle calculated by the angle calculation means, wherein the detection body is the origin is installed at a position rotated by a predetermined angle (including 0°) around the turning center line from the corrects the turning angle calculated by the angle calculating means on the basis of the predetermined angle when the detection signal is output from .

The turning angle detection device of the second invention comprises an inertial sensor for detecting the angular velocity or angular acceleration of the upper mechanism, and an angle for calculating the turning angle of the upper mechanism based on the angular velocity or angular acceleration detected by the inertial sensor. Calculation means, a detection sensor fixed to the lower mechanism, a detection body fixed to the upper mechanism, and angle correction means for correcting the turning angle calculated by the angle calculation means, wherein the detection sensor is It is installed at a position rotated by a predetermined angle (including 0°) around the turning center line from the origin, the detection sensor outputs a detection signal when it passes the detection object, and the angle correction means outputs the detection signal. When the detection signal is output from the sensor, the turning angle calculated by the angle calculating means is corrected based on the predetermined angle.

According to such a turning angle detection device, since the detection sensor detects the detection object, the turning angle of the upper mechanism (rotation by a predetermined angle around the turning center line from the origin) can be grasped. Correction (correction for replacing the detected turning angle with a predetermined angle detected by the detection sensor) can be performed. Therefore, the turning angle of the upper mechanism can be accurately grasped. In addition, even when retrofitting an existing heavy machine, there is no need to modify the heavy machine on a large scale, so the installation work of the turning angle detection device is less costly and labor intensive.

In the case where a plurality of the detection bodies are provided, the detection sensor is provided with a plurality of proximity sensors arranged in a straight line, and each of the detection bodies is at least one arranged at a position capable of facing the proximity sensor. If two identifiers are provided and the positions and numbers of the identifiers are different for each of the detected objects, the detected objects can be identified by the positions and numbers of the identifiers of the detected object detected by the detection sensor. If a plurality of detection objects can be identified, the number of machines capable of correcting the turning angle will increase, so the turning angle can be accurately grasped.
Further, in the case where a plurality of the detection bodies are provided, the detection sensor is provided with a plurality of proximity sensors arranged in a straight line, and each of the detection bodies is arranged at a position that can face the proximity sensor. If an identifier for synchronization and at least one identifier mounting portion are provided, and the presence or absence of the identifier differs for each of the detection objects, the detection object can be identified by the presence or absence, arrangement, etc. of the identifier detected by the detection sensor. At this time, if the width dimension of the identifier for synchronization is smaller than the width dimension of the identifier attached to the identifier attachment portion, it is possible to prevent misidentification when the identifier for synchronization is detected and to confirm the presence or absence of the identifier. so that the detected object can be identified. If the size of the identifier for synchronization is the same as that of the identifier, the identifier arranged adjacent to the identifier for synchronization may not be detected when the identifier for synchronization is detected. intentionally reduce the size of the identifier for synchronization.

Advantageous Effects of Invention According to the turning angle detection device of the present invention, it is easier to install in a heavy machine than when an encoder is installed, and it is possible to detect the turning angle of the upper mechanism with respect to the lower mechanism with high accuracy.

1 is a perspective view of a backhoe provided with a turning angle detection device according to this embodiment; FIG. It is a schematic diagram which shows a turning angle detection apparatus. FIG. 4 is a schematic diagram showing how a detection sensor and a detection body are attached; FIG. 4 is a side view showing an outline of a detection sensor and a detection body; (a) is a front view showing the base of the detection body, and (b) to (e) are side views showing the detection body. FIG. 2 is a schematic diagram of a test stand for experiments conducted to confirm the ability of a turning angle detection device to detect a turning angle. 7 is a graph showing measurement results obtained by an encoder during an experiment; (a) is a graph showing a turning angle calculated based on the measurement result of an inertial sensor during an experiment, and (b) is a graph showing an error in the measurement result of (a). (a) is a graph showing the corrected turning angle calculated based on the measurement result of the inertial sensor, and (b) is a graph showing the error when corrected using the identifier.

In this embodiment, in a backhoe 1 having a lower mechanism (lower traveling body) 11 and an upper mechanism (upper rotating body) 12 shown in FIG. An angle detection device will be described. The lower mechanism 11 constitutes the traveling body of the backhoe 1 . The upper mechanism 12 is pivotable with respect to the lower mechanism 11 via a pivot mechanism (eg, pivot bearing) 13 . The upper mechanism 12 turns around the center line of turning (the center of the turning mechanism 13). A gap is formed between the lower mechanism 11 and the upper mechanism 12 by the turning mechanism 13 . The upper mechanism 12 includes an arm 14 provided in front of the upper mechanism 12, an attachment (for example, a bucket) 15 attached to the tip of the arm 14, a driver's seat 16 for operating the backhoe 1, and a power source for the backhoe 1. a source (eg, an engine);

The turning angle detection device 2 includes an inertial sensor 3, an angle calculation means 4, a detection body 5, a detection sensor 6, and an angle correction means 7, as shown in FIG.
The inertial sensor 3 is attached to the upper mechanism 12 and rotates around the turning center line together with the upper mechanism 12 to detect the angular velocity or angular acceleration of the upper mechanism 12 (see FIG. 1). The inertial sensor 3 is connected to the angle calculating means 4 via a wired or wireless communication line, and the detected value of the inertial sensor 3 is transmitted to the angle calculating means 4 . In this embodiment , a so-called 6-axis sensor equipped with a 3-axis acceleration sensor and a 3-axis gyro sensor is used as the inertial sensor 3 .

The angle calculator 4 calculates the turning angle of the upper mechanism 12 by integrating the angular velocity or angular acceleration detected by the inertial sensor 3 over time. The angle calculator 4 calculates the turning angle θ _i by integrating the detection result (for example, the angular velocity ω _i ) output from the inertial sensor 3 over time, as shown in Equation (1). The turning angle θ _i calculated by the angle calculation means 4 is stored in the storage means 41 .
_θi = _ωi ·dt+θi _-1 Equation 1
θ _i : turning angle (deg)
ω _i : angular velocity (deg/s)
dt: Measurement interval (s)
θ _i-1 : Past turning angle (deg)

The detection sensor 6 is fixed to the upper mechanism 12 side of the turning mechanism 13, as shown in FIG. The detection sensor 6 may be directly fixed to the upper mechanism 12 or may be fixed to the upper mechanism 12 via a pedestal or the like. The detection sensor 6 is fixed to the lower surface of the upper mechanism 12 outside the turning mechanism 13 so as to protrude downward. The detection sensor 6 is composed of a so-called proximity sensor 61 that outputs a detection signal when it passes the detection object 5 (when facing it). The detection sensor 6 is connected to the angle correction means 7 via a wired or wireless communication line as shown in FIG. As shown in FIG. 4, the detection sensor 6 of this embodiment includes three proximity sensors 61 vertically spaced apart. The centers of the three proximity sensors 61 are positioned on an imaginary straight line (a straight line parallel to the turning axis) extending in the vertical direction. That is, the detection sensor 6 has a plurality of proximity sensors 61 arranged in a straight line. The detection sensor 6 of the present embodiment is arranged on a straight line extending from the pivot shaft toward the traveling direction of the lower mechanism 11 when the pivot angle of the upper mechanism 12 is 0°.

The detection body 5 is fixed on the side of the lower mechanism 11 as shown in FIG. The detection body 5 may be directly fixed to the lower mechanism 11 or may be indirectly fixed via a pedestal or the like. The detection body 5 is fixed to the upper surface of the lower mechanism 11 outside the turning mechanism 13 so as to protrude upward so as to be able to face the detection sensor 6 . In this embodiment, four (only two are shown in FIG. 3) detection bodies 5 are provided at intervals in the turning direction. Each detection body 5 is installed at a position rotated by a predetermined angle around the turning center line from the origin. In other words, the detection body 5 is provided at a position where the upper mechanism 12 is turned by a known angle with the orientation that coincides with the traveling direction of the lower mechanism 11 as the origin.

As shown in FIGS. 4 and 5, the detection body 5 has a base 51 made of a metal plate. As shown in FIG. 5A, the base 51 has three through-holes (upper through-hole 52a, middle through-hole 52b, lower through-hole 52c) arranged on the same straight line. . The upper through-hole 52a, the middle through-hole 52b, and the lower through-hole 52c are arranged at positions facing the proximity sensor 61 of the detection sensor 6 (see FIG. 4). A synchronization identifier 53 is attached to the upper through-hole 52a, as shown in FIGS. 5(b) to 5(e). The synchronization identifier 53 of this embodiment is a bolt. Identifiers 54 can be attached to the middle through-holes 52b and the lower through-holes 52c. For the identifier 54, a bolt having a larger bolt diameter (width dimension) than the bolt diameter (width dimension) of the synchronization identifier 53 is used. Whether or not the identifier 54 is attached to the middle through-hole 52b and the lower through-hole 52c differs for each detection body 5 . For example, in the first detection body 5a, the identifier 54 is not attached to both the middle through-hole 52b and the lower through-hole 52c (see FIG. 5(b)), and in the second detection body 5b only the middle through-hole 52b (see FIG. 5B). 5(c)), both the middle through-hole 52b and the lower through-hole 52c in the third detection body 5c (see FIG. 5(d)), and only the lower through-hole 52c in the fourth detection body 5d (see FIG. 5(e) ), the detection object 5 can be identified by the presence or absence (arrangement) of the identifier 54 . More specifically, when the synchronization identifier 53 is detected by the corresponding proximity sensor 61, it is determined whether or not the identifier 54 is detected by another proximity sensor 61. Identify.

The angle correction means 7 is connected to the storage means 41 of the angle calculation means 4 as shown in FIG. 2 and corrects the turning angle calculated by the angle calculation means 4 .
The angle correction means 7 corrects the turning angle calculated by the angle calculation means 4 by replacing it with an angle corresponding to the position of the detection body 5 when the detection signal is output from the detection sensor 6 . That is, the past turning angle θ _i−1 in Equation 1 is replaced with the turning angle θ _F corresponding to the position of the detection body 5 (see Equation 2). Since the drift error is reset by the correction by the angle correction means 7, accumulation of the drift error is prevented. That is, the angle calculation means 4 calculates the current turning angle θ _i of the upper mechanism 12 using the measurement result (angular velocity ω _i ) of the inertial sensor 3 and the corrected turning angle θ _F . The corrected turning angle θ _i is stored in the storage means 41 and used to calculate the next turning angle.
_θi = _ωi ·dt+θi _-1 Equation 1
_θi = _ωi ·dt+ _θF Equation 2
θ _i : turning angle (deg)
ω _i : angular velocity (deg/s)
dt: Measurement interval (s)
θ _i-1 : Past turning angle (deg)
θ _F : Turning angle (deg) according to the position of the detected object

When the backhoe 1 is driven, the inertial sensor 3 measures the angular velocity ω _i (or the angular acceleration) of the upper mechanism 12 during the turning at predetermined time intervals (measurement intervals dt). A measurement result obtained by the inertial sensor 3 is output to the angle calculation means 4 . The angle calculation means 4 calculates the turning angle _θi of the upper mechanism 12 by Equation 1 based on the measurement result of the inertial sensor 3 and stores it in the storage means 41 . When the detection sensor 6 approaches the detection body 5 due to the rotation of the upper mechanism 12 , the detection sensor 6 detects the detection body 5 and transmits the detection result to the angle correction means 7 . The angle correction means 7 identifies which detection object 5 is based on the detection result transmitted from the detection sensor 6, and stores a preset angle (known angle) _θF of the detection object 5. It replaces the previous turning angle θ _i−1 stored in means 41 . This resets the accumulated drift error of the inertial sensor 3, so that the turning angle _θi can be calculated more accurately.

According to the turning angle detection device 2 of the present embodiment, the detection sensor 6 detects the detection body 5, so that the turning angle of the upper mechanism 12 (rotation from the origin about the turning center line by a predetermined angle) can be grasped. Then, when the turning angle _θi is calculated using the angular velocity _ωi obtained by the inertial sensor 3, the past (previous) turning angle θi _-1 is corrected by a known turning angle _θF . , the accumulation of drift errors associated with the use of the inertial sensor 3 can be suppressed. As a result, the turning angle _θi of the upper mechanism 12 can be grasped with high accuracy. If such a turning angle detection device 2 is used for unmanned construction, the direction of the heavy equipment can be accurately grasped, so construction can be performed with high precision.

In addition, since the work of installing the detection sensor 6 and the detection body 5 on the backhoe 1 is simple, the backhoe 1 does not need to be modified on a large scale, and installation of the turning angle detection device 2 is less costly and laborious. The detection sensor 6 and the detection body 5 can be attached from the outside of the backhoe 1 because they can be attached at positions where detection by the detection sensor 6 is possible. Therefore, even when existing heavy machinery is used for unmanned construction, it is possible to easily make improvements necessary for automatic operation.
Since the detector 5 is provided with the identifier 54 in a different arrangement for each installation location, the detection sensor 6 can appropriately detect the turning angle. Further, since the identifier 53 for synchronization whose width dimension is smaller than that of the identifier 54 is disposed on the detection body 5, if the presence or absence of the identifier 54 is determined when the identifier 53 for synchronization is detected, the detection body 5 can be detected. 5 can be identified.

Next, the results of an experiment conducted to confirm the turning angle detection capability of the turning angle detection device 2 will be described.
In this experiment, as shown in FIG. 6, a test table 110 having a pedestal 111 and a swivel table 112 was used. The swivel table 112 can swivel with respect to the pedestal 111 . Between the pedestal 111 and the swivel table 112, an encoder 108 for detecting the rotational position is provided on the swivel bearing. In addition, an inertial sensor (6-axis sensor: 3-axis gyro sensor + 3-axis acceleration sensor) 103 and a detection sensor 106 are installed on the turning table 112 . On the other hand, on the pedestal 111, three detection bodies 105 were installed at intervals. The detection body 105 was installed at a position rotated by a predetermined angle around the center line of rotation from the origin. The detection body 105 was provided on the pedestal 111 so as to face the detection sensor 106 . In this experiment, the turning angle of the turning table 112 was calculated based on the measured value of the inertial sensor 103, and the result of correcting the calculated value of the turning angle using the detection result of the detection sensor 106 was detected by the encoder 108. Compare with the turning angle. The turning table 112 rotates forward and backward at an arbitrary number of revolutions.

Comparing FIG. 8A showing the turning angle calculated based on the measurement value of the inertial sensor 103 and FIG. It was confirmed that A occurs. When checking the error of the measurement result of the inertial sensor 103 with respect to the measurement result of the encoder 108, it can be seen that the drift error is accumulated as shown in FIG. 8(b).
Next, the result of correcting the turning angle calculated from the measurement value of the inertial sensor 103 based on the detection result of the detection sensor 106 is compared with the measurement value of the encoder 108 . As shown in FIG. 9A, the post-correction turning angle exhibits a linear shape substantially similar to the measurement result of the encoder 108 shown in FIG. 7, confirming that the accuracy is improved. Moreover, the error is within ±4° without accumulating, as shown in FIG. 9(b).
Thus, it was confirmed that performing the output filtering process of the inertial sensor 103 contributed to the improvement of accuracy.

Although the embodiments according to the present invention have been described above, the present invention is not limited to the above-described embodiments, and the constituent elements described above can be changed as appropriate without departing from the scope of the present invention.
For example, a heavy machine that can use the turning angle detection device 2 of the present embodiment is not limited to the backhoe 1 as long as the upper mechanism 12 turns on the lower mechanism 11. For example, it may be a crane or the like. good.
Further, in the above-described embodiment, the detection body 5 is installed in the lower mechanism 11 and the detection sensor 6 is installed in the upper mechanism 12, but the detection sensor 6 is installed in the lower mechanism 11 and the upper mechanism 12 detects. A body 5 may be placed.
Further, in the above embodiment, a six-axis sensor is used as the inertial sensor 3, but the inertial sensor 3 does not necessarily have to be a six-axis sensor, and may be, for example, a one-axis sensor.

Further, in the above embodiment, a case where a plurality of detection bodies 5 are detected by one detection sensor 6 has been described, but a plurality of detection sensors 6 may be used to detect one detection body 5 . For example, a plurality of detection sensors 6 are arranged at positions where the position (angle with respect to the origin) of the lower mechanism 11 is known, one detection body 5 is provided in the upper mechanism 12, and the detection body 5 rotating together with the upper mechanism 12 is Correction may be made based on the position of the detection sensor 6 when passing in front of the detection sensor 6 .
Moreover, the number of the detection bodies 5 when detecting a plurality of detection bodies 5 by one detection sensor 6 is not limited as long as it is two or more. Further, the number of identifiers 54 provided on the detection body 5 is not limited to two, and may be one or three or more. In the above embodiment, the identifiers 54 are arranged in rows in the vertical direction, but the arrangement of the identifiers 54 is not limited. good too. Identifiers 54 are not limited to bolts. Also, the synchronization identifier 53 may be provided as required.

Also, the number of proximity sensors 61 of the detection sensor 6 may be appropriately determined according to the presence or absence of the synchronization identifier 53 of the detection body 5 and the number of identifiers 54 . Also, the arrangement of the proximity sensors is not limited to the case of arranging them in a row in the vertical direction. Moreover, the arrangement of the detection sensor 6 is not limited, and may be, for example, a position rotated by a predetermined angle around the turning center line from the origin.
Also, in the above embodiment, the proximity sensor 61 is used as the detection sensor 6, but the type of the detection sensor 6 is not limited, and may be, for example, an optical sensor or a magnetic sensor.

1 Backhoe 11 Lower Mechanism 12 Upper Mechanism 2 Turning Angle Detector 3 Inertial Sensor 4 Angle Calculating Means 5 Detector 53 Identifier for Synchronization 54 Identifier 6 Detection Sensor 7 Angle Correcting Means

Claims (4)

A turning angle detection device for detecting a turning angle of an upper mechanism turning on a lower mechanism,
an inertial sensor that detects the angular velocity or angular acceleration of the upper mechanism;
angle calculation means for calculating a turning angle of the upper mechanism based on the angular velocity or angular acceleration detected by the inertial sensor;
a plurality of detection bodies fixed to the lower mechanism;
a detection sensor fixed to the upper mechanism , comprising a plurality of proximity sensors arranged in a straight line ;
an angle correction means for correcting the turning angle calculated by the angle calculation means;
The detection body is installed at a position rotated by a predetermined angle around the turning center line from the origin,
each of the detection bodies includes at least one identifier disposed at a position that can face the proximity sensor;
The position and number of the identifiers are different for each of the detection objects,
The detection sensor outputs a detection signal when it passes the detection object,
The turning angle detecting device, wherein the angle correcting means corrects the turning angle calculated by the angle calculating means based on the predetermined angle when the detection signal is output from the detecting sensor. A turning angle detection device for detecting a turning angle of an upper mechanism turning on a lower mechanism,
an inertial sensor that detects the angular velocity or angular acceleration of the upper mechanism;
angle calculation means for calculating a turning angle of the upper mechanism based on the angular velocity or angular acceleration detected by the inertial sensor;
a detection sensor fixed to the lower mechanism, comprising a plurality of proximity sensors arranged on a straight line ;
a plurality of detection bodies fixed to the upper mechanism;
an angle correction means for correcting the turning angle calculated by the angle calculation means;
The detection sensor is installed at a position rotated by a predetermined angle around the turning center line from the origin,
each of the detection bodies includes at least one identifier disposed at a position that can face the proximity sensor;
The position and number of the identifiers are different for each of the detection objects,
The detection sensor outputs a detection signal when it passes the detection object,
The turning angle detecting device, wherein the angle correcting means corrects the turning angle calculated by the angle calculating means based on the predetermined angle when the detection signal is output from the detecting sensor. A turning angle detection device for detecting a turning angle of an upper mechanism turning on a lower mechanism,
an inertial sensor that detects the angular velocity or angular acceleration of the upper mechanism;
angle calculation means for calculating a turning angle of the upper mechanism based on the angular velocity or angular acceleration detected by the inertial sensor;
a plurality of detection bodies fixed to the lower mechanism;
a detection sensor fixed to the upper mechanism, comprising a plurality of proximity sensors arranged in a straight line ;
an angle correction means for correcting the turning angle calculated by the angle calculation means;
The detection body is installed at a position rotated by a predetermined angle around the turning center line from the origin,
each of the detection bodies includes a synchronization identifier and at least one identifier mounting portion arranged at a position that can face the proximity sensor;
The width dimension of the identifier for synchronization is smaller than the width dimension of the identifier attached to the identifier attachment portion,
The presence or absence of the identifier is different for each of the detection objects,
The detection sensor outputs a detection signal when it passes the detection object,
The turning angle detecting device, wherein the angle correcting means corrects the turning angle calculated by the angle calculating means based on the predetermined angle when the detection signal is output from the detecting sensor. A turning angle detection device for detecting a turning angle of an upper mechanism turning on a lower mechanism,
an inertial sensor that detects the angular velocity or angular acceleration of the upper mechanism;
angle calculation means for calculating a turning angle of the upper mechanism based on the angular velocity or angular acceleration detected by the inertial sensor;
a detection sensor having a plurality of linearly arranged proximity sensors and fixed to the lower mechanism;
a plurality of detection bodies fixed to the upper mechanism;
an angle correction means for correcting the turning angle calculated by the angle calculation means;
The detection sensor is installed at a position rotated by a predetermined angle around the turning center line from the origin,
each of the detection bodies includes a synchronization identifier and at least one identifier mounting portion arranged at a position that can face the proximity sensor;
The width dimension of the identifier for synchronization is smaller than the width dimension of the identifier attached to the identifier attachment portion,
The presence or absence of the identifier is different for each of the detection objects,
The detection sensor outputs a detection signal when it passes the detection object,
The turning angle detecting device, wherein the angle correcting means corrects the turning angle calculated by the angle calculating means based on the predetermined angle when the detection signal is output from the detecting sensor.

Images