WO2024090014A1 - スイング角度較正方法、姿勢検出方法、スイング角度較正システムおよび姿勢検出システム - Google Patents
スイング角度較正方法、姿勢検出方法、スイング角度較正システムおよび姿勢検出システム Download PDFInfo
- Publication number
- WO2024090014A1 WO2024090014A1 PCT/JP2023/031259 JP2023031259W WO2024090014A1 WO 2024090014 A1 WO2024090014 A1 WO 2024090014A1 JP 2023031259 W JP2023031259 W JP 2023031259W WO 2024090014 A1 WO2024090014 A1 WO 2024090014A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- swing
- swing angle
- angle
- work machine
- posture
- Prior art date
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 105
- 238000000034 method Methods 0.000 title claims abstract description 38
- 230000036544 posture Effects 0.000 claims description 125
- 238000004364 calculation method Methods 0.000 claims description 15
- 238000005259 measurement Methods 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 17
- 238000004891 communication Methods 0.000 description 8
- 230000007246 mechanism Effects 0.000 description 7
- 239000010720 hydraulic oil Substances 0.000 description 6
- 230000015654 memory Effects 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- 230000009466 transformation Effects 0.000 description 4
- 238000003384 imaging method Methods 0.000 description 3
- 230000007935 neutral effect Effects 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000002485 combustion reaction Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000010191 image analysis Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000009420 retrofitting Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/28—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets
- E02F3/36—Component parts
- E02F3/42—Drives for dippers, buckets, dipper-arms or bucket-arms
- E02F3/43—Control of dipper or bucket position; Control of sequence of drive operations
Definitions
- the present disclosure relates to a swing angle calibration method, a posture detection method, a swing angle calibration system, and a posture detection system.
- the hydraulic excavator shown in Patent Document 1 is equipped with a vehicle body and a working implement.
- the vehicle body is provided with, for example, a GNSS (Global Navigation Satellite System) antenna to detect the position of the vehicle body.
- the vehicle body is equipped with, for example, an IMU (Inertial Measurement Unit).
- the IMU detects the roll angle and pitch angle of the vehicle body.
- the working implement has a boom, an arm, a bucket, and a hydraulic cylinder that drives them.
- the hydraulic excavator controller calculates the position of the blade tip of the bucket based on parameters such as the positional relationship between the GNSS antenna and the boom pin, the respective lengths of the boom, arm, and bucket, and the respective rotation angles of the boom, arm, and bucket.
- Patent Document 1 proposes performing calibration using the pitch angle measured by the IMU of the vehicle body in order to suppress a decrease in accuracy of the cutting edge position in the height direction caused by the hydraulic excavator tilting in the fore-and-aft direction of the vehicle body due to the weight of the work machine.
- Patent Document 1 does not take into consideration cases where the angle sensor that detects the swing angle of the work machine has an error, which reduces the detection accuracy of the cutting edge position.
- the present disclosure aims to provide a swing angle calibration method, a posture detection method, a swing angle calibration system, and a posture detection system that are capable of calibrating errors that occur when measuring the swing angle of a work machine with an angle sensor and accurately detecting the swing angle. (Means for solving the problem)
- the swing angle calibration method of the first aspect is a swing angle calibration method for calibrating the swing angle of a working implement of a work machine, and includes a position information acquisition step, a first swing angle calculation step, a calibration second swing angle acquisition step, and a creation step.
- the position information acquisition step acquires position information of a predetermined position of the working implement using a measuring device in a plurality of swing postures in which the working implement is fixed in a predetermined posture and only swings left and right.
- the first swing angle calculation step calculates a first swing angle in each swing posture based on the position information.
- the calibration second swing angle acquisition step acquires a second swing angle in each swing posture based on detection values in the plurality of swing postures by an angle sensor mounted on the working machine that detects the swing angle of the working implement.
- the creation step creates a correlation table based on the first swing angle and the second swing angle in the plurality of swing postures.
- the second aspect of the attitude detection method is an attitude detection method for detecting the attitude of a working implement of a work machine, and includes a second swing angle acquisition step, a corrected second swing angle detection step, and an attitude detection step.
- the second swing angle acquisition step acquires a second swing angle based on a detection value of an angle sensor mounted on the work machine and detecting the swing angle of the working implement.
- the corrected second swing angle detection step detects a second swing angle after error correction based on the correlation table of the swing angle calibration method of the first aspect and the second swing angle.
- the attitude detection step detects the attitude of the working implement using the error-corrected second swing angle.
- the swing angle calibration system of the third aspect of the present disclosure is a swing angle calibration system that calibrates the swing angle of a working implement of a work machine, and includes a measurement device, an angle sensor, and a controller.
- the measurement device measures position information of a predetermined position of the working implement in multiple swing postures in which the working implement is fixed in a predetermined posture and only swings left and right.
- the angle sensor is mounted on the work machine and detects the swing angle of the working implement.
- the controller calculates a first swing angle in each swing posture based on the position information, obtains a second swing angle in each swing posture based on the detection values in the multiple swing postures by the angle sensor, and creates a correlation table based on the first swing angle and the second swing angle in the multiple swing postures.
- a posture detection system is a posture detection system for detecting the posture of a working implement of a work machine, and includes an angle sensor, a storage unit, and a controller.
- the angle sensor is mounted on the work machine and detects a swing angle of the working implement.
- the storage unit stores a correlation table of the swing angle calibration method of the first aspect.
- the controller corrects the swing angle detected by the angle sensor based on the correlation table, and detects the posture of the working implement using the corrected swing angle.
- the present disclosure provides a swing angle calibration method, a posture detection method, and a posture detection system that can calibrate errors that occur when measuring the swing angle of a work machine using an angle sensor, and can detect the swing angle with high accuracy.
- FIG. 1 illustrates a work machine system according to the present disclosure.
- FIG. 2 is a perspective view showing the vicinity of a swing bracket of the work machine of FIG. 1 .
- FIG. 3 is a top view showing the vicinity of the swing bracket of FIG. 2 .
- 1 is a block diagram showing the configuration of a drive system and a drive control system of a work machine. 2 is a block diagram showing a control configuration of the attitude detection system for the work machine.
- FIG. FIG. 1 is a side view showing a schematic configuration of a work machine.
- FIG. 13 is a diagram showing a guide screen displayed on the display input device.
- FIG. 1 is a flow diagram illustrating a swing angle calibration method according to the present disclosure.
- FIG. 1 is a flow diagram illustrating a swing angle calibration method according to the present disclosure.
- FIG. 1A is a front view showing the state in which the working machine is moved to multiple swing positions and the coordinates of the arm top are measured by a total station
- FIG. 1B is a plan view showing the state in which the working machine has been moved to multiple swing positions.
- FIG. 13 is a diagram showing a display screen of a display input device during swing angle calibration.
- FIG. 13 is a diagram showing a display screen of a display input device during swing angle calibration.
- 11A and 11B are diagrams for explaining a swing angle calibration method.
- 11A and 11B are diagrams for explaining a swing angle calibration method.
- 11A and 11B are diagrams for explaining a swing angle calibration method.
- 11A and 11B are diagrams for explaining a swing angle calibration method.
- FIG. 13 is a diagram showing a swing angle error table created by a swing angle calibration method.
- FIG. 1 is a flow diagram illustrating a posture detection method according to the present disclosure.
- FIG. 1 is a diagram showing a work machine 1 according to the present disclosure.
- An example of the work machine 1 is a hydraulic excavator.
- the work machine 1 has a vehicle body 2 (an example of a work machine body) and a work implement 3 .
- the vehicle body 2 has a rotating body 11, a lower structure 12, and a blade 13.
- the rotating body 11 is disposed above the lower structure 12.
- the rotating body 11 is supported so as to be rotatable relative to the lower structure 12.
- the rotating body 11 has a floor 14 on its upper surface, a driver's seat 15 disposed on the floor 14, and a roof 16 disposed above the driver's seat 15.
- the work machine 1 of this embodiment is a canopy type.
- the lower structure 12 includes tracks 12a, 12b.
- the work machine 1 travels by rotating the tracks 12a, 12b.
- the blade 13 is disposed in front of the lower structure 12.
- the blade 13 can be lifted by a hydraulic actuator.
- front and rear refer to the front and rear of the vehicle body 2. Additionally, in the following description, “right,” “left,” “up,” and “down” refer to directions based on the state of looking forward from the driver's seat 15. "Vehicle width direction” and “left-right direction” are synonymous. In the figure, the forward direction F, rear direction B, left direction L, and right direction R are shown.
- the work implement 3 is attached to the vehicle body 2.
- the work implement 3 includes a swing bracket 20, a boom 21, an arm 22, and a bucket 23.
- the swing bracket 20 is disposed on the front side of the rotating body 11.
- Fig. 2 is a perspective view showing the vicinity of the swing bracket of the work machine 1.
- Fig. 3 is a top view showing the vicinity of the swing bracket 20.
- the work implement 3 is omitted in Fig. 3.
- the swing bracket 20 shown in Figs. 2 and 3 is in a state where the work implement 3 is disposed along the fore-aft direction of the rotating body 11.
- the swing bracket 20 is disposed on the front side of the rotating body 11. As shown in FIG. 2, the rotating body 11 has a support bracket 18 at its front end. The support bracket 18 is disposed so as to protrude forward. As shown in FIG. 3, the swing bracket 20 is attached to the support bracket 18 by a vertical pin 19 so as to be rotatable clockwise and counterclockwise when viewed from above. In FIG. 3, the center of rotation of the pin 19 is shown as axis A1.
- the base end of the boom 21 is attached to the swing bracket 20 via a boom pin 24 so as to be rotatable in the vertical direction.
- the base end of the arm 22 is rotatably attached to the tip of the boom 21 via an arm pin 25.
- the bucket 23 is rotatably attached to the tip of the arm 22 via a bucket pin 26.
- the arm 22 and bucket 23 are connected by a link 27.
- the swing bracket 20 has a bracket body 32 and a lever portion 33.
- the bracket body 32 is rotatably supported by the support bracket 18 via a pin 19.
- the lever portion 33 protrudes from the bracket body 32 in the right direction R.
- the bracket body 32 has a work machine support part 34 and a link mechanism support part 35.
- the work machine support part 34 supports the work machine 3 so that it can rotate in the vertical direction.
- the work machine support part 34 has a left side part 34a and a right side part 34b.
- the left side part 34a and the right side part 34b are arranged to sandwich the base end of the boom 21 from the left and right.
- the link mechanism support part 35 and the work machine support part 34 are arranged to sandwich the axis A1.
- the link mechanism support part 35 supports the link mechanism 36 described below.
- the work machine 3 includes a boom cylinder 28, an arm cylinder 29, a bucket cylinder 30, and a swing cylinder 31 (see FIG. 2).
- the boom cylinder 28, the arm cylinder 29, the bucket cylinder 30, and the swing cylinder 31 are each a hydraulic cylinder.
- the bottom end of the boom cylinder 28 is rotatably attached to the swing bracket 20.
- the bottom end of the boom cylinder 28 is disposed between the left side portion 34a and the right side portion 34b shown in FIG. 3.
- the rod end of the boom cylinder 28 is rotatably attached to the boom 21.
- the bottom end of the arm cylinder 29 is rotatably attached to the boom 21.
- the rod end of the arm cylinder 29 is rotatably attached to the arm 22.
- the bottom end of the bucket cylinder 30 is rotatably attached to the arm 22.
- the rod end of the bucket cylinder 30 is rotatably attached to the bucket 23 via a link 27.
- the boom cylinder 28 extends and retracts, causing the boom 21 to rotate up and down.
- the arm cylinder 29 extends and retracts, causing the arm 22 to rotate relative to the boom 21.
- the bucket cylinder 30 extends and retracts, causing the bucket 23 to rotate relative to the arm 22.
- the swing cylinder 31 connects the rotating body 11 and the lever portion 33.
- the bottom end of the swing cylinder 31 is rotatably attached to the rotating body 11.
- the rod end of the swing cylinder 31 is rotatably attached to the lever portion 33.
- the work machine 1 has a drive system 41 and a drive control system 42.
- FIG. 4 is a block diagram showing the configurations of the drive system 41 and the drive control system 42 of the work machine 1.
- the drive system 41 includes a drive source 43 and a hydraulic pump 44.
- the drive source 43 is, for example, an internal combustion engine. However, the drive source 43 may be an electric motor or a hybrid mechanism of an engine and an electric motor.
- the hydraulic pump 44 is driven by the drive source 43 and discharges hydraulic oil.
- the hydraulic oil discharged from the hydraulic pump 44 is supplied to the boom cylinder 28, the arm cylinder 29, the bucket cylinder 30, and the swing cylinder 31.
- the work machine 1 includes a first traveling motor 45a, a second traveling motor 45b, and a swing motor 46.
- the first traveling motor 45a drives the crawler belt 12a.
- the second traveling motor 45b drives the crawler belt 12b.
- the swing motor 46 causes the swing body 11 to swing.
- the hydraulic oil discharged from the hydraulic pump 44 is supplied to the first travel motor 45a, the second travel motor 45b, and the swing motor 46.
- one hydraulic pump 44 is illustrated in Fig. 4, a plurality of hydraulic pumps may be provided.
- the drive control system 42 includes an operating device 47 and an input device 48.
- the operating device 47 and the input device 48 are arranged around the driver's seat 15.
- the operating device 47 accepts operations by the operator to drive the work machine 3, the rotating body 11, and the lower structure 12, and outputs an operation signal according to the operation.
- the operating device 47 includes, for example, a lever, a pedal, a switch, etc.
- the input device 48 accepts operations by the operator to set the control of the work machine 1, and outputs an operation signal corresponding to the operation.
- the input device 48 is, for example, a touch screen.
- the input device 48 may include a lever or a switch.
- the drive control system 42 includes a main body controller 51, a memory device 52, and a control valve 53.
- the main body controller 51 is programmed to control the work machine 1 based on the acquired operation signal.
- the main body controller 51 includes a processor such as a CPU (Central Processing Unit) and memories such as a RAM (Random Access Memory) and a ROM (Read Only Memory).
- the memory device 52 is an example of a recording medium readable by a non-transitory processor.
- the memory device 52 records computer instructions that are executable by the processor and for controlling the work machine 1.
- the main body controller 51 receives operation signals from the operation device 47 and the input device 48.
- the main body controller 51 controls the control valve 53 based on the operation signals.
- the control valve 53 may be a pressure proportional control valve.
- the control valve 53 may be an electromagnetic proportional control valve.
- the control valve 53 controls the flow rate of hydraulic oil supplied from the hydraulic pump 44 to the first travel motor 45a and the second travel motor 45b. This allows the work machine 1 to travel in response to the operation of the operation device 47.
- the control valve 53 controls the flow rate of hydraulic oil supplied from the hydraulic pump 44 to the boom cylinder 28, the arm cylinder 29, the bucket cylinder 30, and the swing cylinder 31.
- the main body controller 51 generates a command signal to the control valve 53 so that the boom 21, the arm 22, and the bucket 23 operate in response to the operation of the operation device 47.
- the control valve 53 controls the flow rate of hydraulic oil supplied from the hydraulic pump 44 to the swing motor 46.
- the main body controller 51 generates a command signal to the control valve 53 so that the rotating body 11 rotates in response to the operation of the operating device 47.
- the main body controller 51 generates a command signal to the control valve 53 so that the work machine 3 swings left and right relative to the vehicle body 2 in response to the operation of the operating device 47.
- the work machine 1 is equipped with an attitude detection system 60.
- Fig. 5 is a block diagram showing the configuration of the attitude detection system 60.
- the attitude detection system 60 includes a GNSS receiver 61, a pair of GNSS antennas 62a, 62b, a main body IMU 63, a boom IMU 64, an arm IMU 65, a bucket IMU 66, a swing angle sensor 67 (an example of an angle sensor), an attitude detection controller 68, a storage device 69, a display input device 70, and a communication device 71.
- the GNSS receiver 61 and a pair of GNSS antennas 62a, 62b are arranged on the vehicle body 2. As shown in FIG. 1, the GNSS antennas 62a, 62b are arranged at a predetermined interval in the left-right direction.
- the GNSS (Global Navigation Satellite System) receiver 61 is, for example, a receiver for the GPS (Global Positioning System).
- the GNSS receiver 61 receives positioning signals from satellites, and uses the positioning signals to calculate the positions of the GNSS antennas 62a, 62b to create vehicle body position data.
- the vehicle body position data includes position information in the global coordinate system by the GNSS and information on the orientation of the rotating body 11.
- the attitude detection controller 68 acquires the vehicle body position data from the GNSS receiver 61.
- the main body IMU 63 is disposed in the vehicle main body 2 as shown in FIG. 1.
- the main body IMU 63 acquires main body inclination angle data.
- the vehicle inclination angle data includes the angle (pitch angle) in the vehicle's fore-aft direction relative to the horizontal, and the angle (roll angle) in the vehicle's left-right direction relative to the horizontal.
- the main body IMU 63 outputs the vehicle inclination angle data to the attitude detection controller 68.
- the boom IMU 64 is disposed on the boom 21 as shown in FIG. 1.
- the boom IMU 64 acquires inclination angle data of the boom 21.
- the boom inclination angle data includes the inclination angle of the boom 21 in the up-down direction relative to the horizontal direction.
- the boom IMU 64 outputs the boom inclination angle data to the attitude detection controller 68.
- the arm IMU 65 is disposed on the arm 22 as shown in FIG. 1.
- the arm IMU 65 acquires inclination angle data of the arm 22.
- the arm inclination angle data includes the inclination angle of the arm 22 in the vertical direction relative to the horizontal direction.
- the arm IMU 65 outputs the arm inclination angle data to the posture detection controller 68.
- the bucket IMU 66 is disposed on the link 27 to which the bucket 23 is connected.
- the bucket IMU 66 acquires inclination angle data of the bucket 23.
- the bucket inclination angle data includes the inclination angle of the bucket 23 in the vertical direction relative to the horizontal direction.
- the bucket IMU 66 outputs the bucket inclination angle data to the attitude detection controller 68.
- the swing angle sensor 67 detects a detection value for determining the swing angle of the swing bracket 20 relative to the rotating body 11.
- the swing angle sensor 67 is disposed to the side of the swing bracket 20.
- the swing angle sensor 67 is fixed to the rotating body 11.
- the swing angle sensor 67 is connected to the swing bracket 20 by the link mechanism 36.
- the swing angle sensor 67 includes a rotation sensor (not shown), a magnet, and a pin 37 connected to the magnet.
- the rotation sensor detects the rotation angle of the magnet, and outputs the detected value, which is swing angle data, to the posture detection controller 68.
- the attitude detection controller 68 includes a processor such as a CPU (Central Processing Unit) and memories such as a RAM (Random Access Memory) and a ROM (Read Only Memory).
- the storage device 69 is an example of a recording medium that can be read by a non-transitory processor.
- the storage device 69 records computer instructions that can be executed by the processor and are used to control the work machine 1.
- FIG. 6 is a side view showing a schematic configuration of the work machine 1.
- the posture detection controller 68 calculates the boom angle ⁇ 1, the arm angle ⁇ 2, and the bucket angle ⁇ 3 based on the main body tilt angle data, the boom tilt angle data, the arm tilt angle data, and the bucket tilt angle data.
- the boom angle ⁇ 1 indicates the tilt angle of the boom 21 relative to the vehicle body 2.
- the arm angle ⁇ 2 indicates the tilt angle of the arm 22 relative to the boom 21.
- the bucket angle ⁇ 3 indicates the tilt angle of the bucket 23 relative to the arm 22.
- sensors may be provided to detect the stroke amounts of the boom cylinder 28, the arm cylinder 29, and the bucket cylinder 30, and the boom angle ⁇ 1, the arm angle ⁇ 2, and the bucket angle ⁇ 3 may be calculated from the detection values of the sensors.
- the posture detection controller 68 calculates the swing angle from the swing angle data, and calculates the calibrated swing angle based on the swing angle error table stored in the storage device 69.
- the swing angle error table and the calculation of the swing angle will be described in detail later.
- the storage device 69 stores shape data of the vehicle body 2 and the work machine 3.
- the shape data of the vehicle body 2 indicates the shape of the vehicle body 2.
- the shape data of the vehicle body 2 indicates the positional relationship between the GNSS antenna 62 and a reference position on the vehicle body 2.
- the shape data of the vehicle body 2 indicates the positional relationship between the reference position on the vehicle body 2 and the boom pin 24.
- the shape data of the work machine 3 indicates the shape of the work machine 3.
- the shape data includes boom length L1, arm length L2, and bucket length L3.
- Boom length L1 is the length from boom pin 24 to arm pin 25.
- Arm length L2 is the length from arm pin 25 to bucket pin 26.
- Bucket length L3 is the length from bucket pin 26 to blade tip position P10 of bucket 23.
- the attitude detection controller 68 detects bucket position data from the vehicle body position data acquired by the GNSS receiver 61, based on the vehicle inclination angle data, boom inclination angle data, arm inclination angle data, bucket inclination angle data, and swing angle data.
- the bucket position data indicates the blade tip position P10 of the bucket 23.
- the storage device 69 stores current terrain data and designed terrain data.
- the current terrain data indicates the current terrain of the work site.
- the designed terrain data indicates the target shape of the work site.
- the attitude detection controller 68 displays a guide screen 72 shown in FIG. 7 on the display input device 70 based on the current terrain data, designed terrain data, and shape data. As shown in FIG. 7, the guide screen 72 indicates the current terrain 73, designed terrain 74, and the positions of the work machine 1.
- the shape data includes data indicating the shape of the bucket 23.
- the attitude detection controller 68 displays the position of the bucket 23 relative to the current terrain 73 and designed terrain 74 on the guide screen 72 based on the shape data and bucket position data of the bucket 23.
- the display input device 70 displays the guide screen 72 based on the input instructions of the worker.
- the display input device 70 communicates with the posture detection controller 68 over the Internet via the communication device 71.
- the display input device 70 is, for example, a tablet terminal.
- the communication device 71 connects the display input device 70 to the Internet.
- the communication between the display input device 70 and the communication device 71 may be either wired or wireless.
- the communication device 71 is, for example, a wireless LAN router.
- FIGS. 8A and 8B are flow charts showing the swing angle calibration method.
- step S10 the coordinates of a specified point (swing point) of the work machine are measured in multiple swing postures, and each point is projected onto a specified swing plane to create multiple swing points on the swing plane.
- step S20 a swing coordinate system on the swing plane is created.
- step S30 the swing angles at the multiple swing points in the swing coordinate system are calculated.
- step S40 a swing angle error table is created.
- Figure 9(a) is a front view showing the state in which the work machine 3 has been moved to multiple swing positions.
- Figure 9(b) is a plan view showing the state in which the work machine 3 has been moved to multiple swing positions and the coordinates of the arm top are being measured by a total station TS (an example of a measuring device).
- a prism is affixed to the tip of the arm 22 (arm top).
- step S11 of step S10 the boom angle ⁇ 1, arm angle ⁇ 2, and bucket angle ⁇ 3 are fixed, and only the swing posture is changed, and the reflected waves from the prism attached to the arm top are received by the total station TS to measure the coordinates of the prism.
- Point p1 is the position of the arm top in swing posture K1 (right end), which is the mechanical limit when the working machine 3 is swung to the right relative to the rotating body 11.
- Point p3 is the position of the arm top in swing posture K3 (neutral posture) in which the working machine 3 is arranged parallel to the front-to-rear direction of the rotating body 11 in a plan view.
- Point p2 is the position of the arm top in swing posture K2 between swing posture K1 and swing posture K3. Swing posture K2 is set so as to be located approximately in the center between swing posture K1 and swing posture K3.
- Point p7 is the position of the arm top in swing posture K7 (left end), which is the mechanical limit when the working machine 3 is swung to the left relative to the rotating body 11.
- Point p4 is the position of the arm top in swing posture K4.
- Point p5 is the position of the arm top in swing posture K5.
- Point p6 is the position of the arm top in swing posture K6.
- Swing postures K4, K5, and K6 are set in order between swing postures K3 and K7. Swing postures K4, K5, and K6 are set so as to divide the gap between swing postures K3 and K7 into four equal parts. Note that swing postures K2 to K6 do not need to be set precisely and can be determined by the worker's eye.
- a swing angle calibration screen 80 shown in FIG. 10A is displayed on the display input device 70.
- the swing angle calibration screen 80 instructs the worker to adopt the swing posture displayed on the screen.
- the swing angle calibration screen 80 has a left screen 81, a right screen 82, an X coordinate input section 83, a Y coordinate input section 84, and a Z coordinate input section 85.
- the left screen 81 displays a side view of the work machine 1.
- the right screen 82 displays a plan view of the work machine 1.
- a prism 91 is shown on the left screen 81 and the right screen 82.
- a total station TS is shown on the right screen 82, and the XYZ coordinates of the total station TS are shown.
- the worker operates the operating device 47 to drive the swing cylinder 31 and rotate the work machine 3 relative to the rotating body 11 so that the swing posture K3 is reached by visual estimation according to the swing angle calibration screen 80 shown in FIG. 10A.
- the total station TS detects the position coordinates of the prism 91.
- the XYZ coordinates detected by the total station TS are input by the worker to the X coordinate input unit 83, the Y coordinate input unit 84, and the Z coordinate input unit 85.
- the input XYZ coordinates are output to the posture detection controller 68.
- the worker After inputting the XYZ coordinates, the worker presses the proceed button 86, and the swing angle calibration screen 80 shown in FIG. 10B is displayed. On the swing angle calibration screen 80 in FIG. 10B, the worker is instructed to set the work machine 3 to the swing position K1.
- the worker follows the swing angle calibration screen 80 and swings the work machine 3 to the right relative to the rotating body 11 until it reaches the mechanical limit, with the boom angle ⁇ 1, arm angle ⁇ 2, and bucket angle ⁇ 3 fixed. This causes the work machine 3 to assume a swing posture K1.
- the total station TS then detects the position coordinates of the prism 91.
- the XYZ coordinates detected by the total station TS are input by the worker to the X coordinate input unit 83, the Y coordinate input unit 84, and the Z coordinate input unit 85.
- the input XYZ coordinates are output to the posture detection controller 68.
- the swing angle calibration screen 80 displays an instruction to place the work machine 3 in swing position K2 with the boom angle ⁇ 1, arm angle ⁇ 2, and bucket angle ⁇ 3 fixed. Then, in the same manner as above, the XYZ coordinate values of point p2 detected by the total station TS in swing position K2 are input, and the input XYZ values are output to the position detection controller 68.
- the XYZ coordinate values of points p4 to p7 measured by the total station TS are output to the posture detection controller 68.
- the posture detection controller 68 obtains the XYZ coordinate values in the total station TS of points p1 to p7 of the arm top in swing postures K1 to K7.
- the posture detection controller 68 calculates a swing motion plane using the least squares method. Specifically, as shown in formulas (1) and (2), the XYZ coordinates of the total station TS at points p1 to p7 are pi, and the center of gravity vector is g. (Equation 1) (Equation 2) If the equation of the swing plane U that has the smallest distance from each point pi is the following equation (3), a, b, and c can be found using the least squares method from the following equation (4).
- equation 9) 11B the posture detection controller 68 calculates the unit vector sx of the p"1p"7 vector as the x-axis of the swing plane U. The unit vector sx is expressed by the following (Equation 10).
- step S23 the posture detection controller 68 sets the cross product sy of sx and n (the unit vector of the normal vector N in step S12) to the y-axis of the swing plane U as shown in the following (Equation 11). Also, the posture detection controller 68 sets n to the z-axis of the swing plane U as shown in the following (Equation 12). (Equation 11) (Equation 12) Next, in step S24, the attitude detection controller 68 calculates a matrix Q for transforming from the coordinate system of the total station TS to the coordinate system of the swing plane U. The matrix Q is expressed by the following (Equation 13).
- step S31 of step S30 the posture detection controller 68 converts the swing point pi'' into the swing coordinate system using the following (Equation 14), and sets the converted point as a swing transformation point psi.
- Equation 14 8B
- step S32 the posture detection controller 68 uses the least squares method to calculate a circle that minimizes the error with the swing transformation point psi on the swing plane U with sx as the x-axis and sy as the y-axis.
- the circle is expressed by the following (Equation 15).
- Equation 15 A, B, and C in this formula 15 can be obtained using the following formula 16.
- Equation 16 Here, as shown in FIG.
- step S33 the posture detection controller 68 calculates the swing angle ⁇ i (an example of a first swing angle) from the swing transformation point ps3 to the swing transformation points ps1, ps2, ps4, ps5, ps6, and ps7.
- the vector from the swing center so to psi is defined as qi.
- q3 is the neutral posture.
- step S34 the posture detection controller 68 acquires swing angle data which is a detection value by the swing angle sensor 67 in the swing postures K1 to K7.
- the swing angle data may be acquired by the swing angle sensor 67 when the work machine 3 is rotated relative to the revolving body 11 in step S11 to be placed in the swing postures K1 to K7.
- step S35 the posture detection controller 68 calculates the swing angle in each swing posture from the swing angle data in the swing postures K1 to K7.
- the swing angles ⁇ sw (an example of the second swing angle) calculated by the posture detection controller 68 are ⁇ sw1 , ⁇ sw2 , ..., ⁇ sw7 .
- step S36 the posture detection controller 68 calculates the angle error ei in each of the swing postures K1 to K7.
- the angle error ei is calculated by the following (Equation 21).
- Equation 22 Fig. 12 is a diagram showing a swing angle error table T. The horizontal axis of Fig.
- the 12 indicates the swing angle (deg) calculated from the swing angle sensor 67.
- the vertical axis indicates the swing angle error (deg).
- the swing angle to the right from the swing posture K3, which is the neutral posture is indicated by a negative value
- the swing angle to the left from the swing posture K3 is indicated by a positive value.
- the posture detection controller 68 uses the swing angle error table T to calibrate the swing angle ⁇ sw calculated from the swing angle data, which is the detection value of the swing angle sensor 67, as shown in the following formula (23), to calculate the swing angle ⁇ sw '.
- step S11 corresponds to an example of a position information acquisition step.
- Steps S12, S13, S21 to S24, and S31 to S33 correspond to an example of a first swing angle calculation step.
- Steps S34 and S35 correspond to an example of a calibration second swing angle acquisition step.
- Step S40 corresponds to an example of a creation step.
- Step S12 corresponds to an example of a plane calculation step.
- Steps S20 and S31 correspond to an example of a coordinate conversion step.
- Step S32 corresponds to an example of a circle calculation step.
- Step S33 corresponds to an example of an angle calculation step.
- Step S36 corresponds to an example of an error calculation step.
- the swing angle calibration system includes a total station TS, a swing angle sensor 67, and a posture detection controller 68.
- step S110 the attitude detection controller 68 acquires vehicle body position data from the GNSS receiver 61.
- step S120 the attitude detection controller 68 acquires body tilt angle data from the body IMU 63.
- step S130 the attitude detection controller 68 acquires boom tilt angle data from the boom IMU 64.
- step S140 the posture detection controller 68 acquires arm tilt angle data from the arm IMU 65.
- step S150 the attitude detection controller 68 acquires bucket tilt angle data from the bucket IMU 66.
- step S160 the posture detection controller 68 acquires swing angle data from the swing angle sensor 67, and calculates the swing angle ⁇ sw from the swing angle data.
- Step S160 corresponds to an example of a second swing angle acquisition step.
- step S170 the posture detection controller 68 calculates the boom angle ⁇ 1, arm angle ⁇ 2, and bucket angle ⁇ 3 from the main body tilt angle data, boom tilt angle data, arm tilt angle data, and bucket tilt angle data.
- step S180 the posture detection controller 68 calculates an error e in the swing angle ⁇ sw based on the swing angle error table T stored in the storage device 69.
- the posture detection controller 68 calculates an error-corrected swing angle ⁇ sw ' by subtracting the error e from the swing angle ⁇ sw .
- Step S180 corresponds to an example of a step of detecting a second swing angle after correction.
- step S190 the attitude detection controller 68 calculates the blade tip position P10 of the bucket 23 from the vehicle body position data, main body inclination angle data, boom angle ⁇ 1, arm angle ⁇ 2, bucket angle ⁇ 3, swing angle after error correction, and shape data of the vehicle body 2 and the work machine 3.
- Step S190 corresponds to an example of an attitude detection step.
- step S200 the attitude detection controller 68 displays the guide screen 72 shown in FIG. 7 on the display input device 70 based on the current terrain data, the designed terrain data, the shape data of the vehicle body 2 and the work machine 3, and the detected cutting edge position P10.
- a swing angle error table T is created based on the swing angle ⁇ i calculated from the position information measured by the total station TS and the swing angle ⁇ swi calculated from the detection value of the swing angle sensor 67.
- the swing angle ⁇ i which is the true value of the swing angle, with higher accuracy.
- the true value corresponds to the first swing angle.
- the angle sensor that detects the swing angle of the work machine 3 relative to the revolving body 11 is calibrated, but this is not limiting.
- the swing angle calibration method of the present disclosure may be applied to an angle sensor that detects the rotation angle of the revolving body 11 relative to the lower structure 12.
- the swing angle calibration method of the present disclosure may be applied to an angle sensor that detects the swing angle of an offset type shovel.
- the coordinates of the position of the arm top in the swing postures K1 to K7 are acquired using a total station as an example of a measuring device, but this is not limited thereto.
- a measuring device for example, the cutting edge of the bucket 23 may be placed on a plurality of reference points of known coordinates arranged at the work site to acquire coordinates.
- the posture of the work machine may be acquired by an imaging device mounted on an unmanned aerial vehicle (UAV) and the position coordinates of the boom top may be acquired by image analysis.
- the surrounding environment may be imaged by an imaging device mounted on the work machine, and the position coordinates of the boom top may be acquired based on the difference before and after the swing.
- the posture of the work machine may be acquired by an imaging device installed at the work site, or a GNSS antenna may be placed at the measurement target position of the work machine, and the posture of the work machine may be acquired based on the position information of the GNSS antenna.
- the position coordinates of the boom top are acquired, but this does not have to be limited to the boom top, and may be a specified position of the bucket 23, or any position of the work machine 3 that rotates in conjunction with the swing operation.
- a swing angle error table showing the relationship between the swing angle acquired from the swing angle sensor 67 and the error is stored in the storage device 69, but this is not limited to this.
- a correlation table showing the relationship between the swing angle acquired from the swing angle sensor 67 and the swing angle acquired from the total station TS may be stored in the storage device 69.
- the posture detection controller 68 detects the blade tip position of the bucket 23 and creates the swing angle error table T, but the creation of the swing angle error table T may be performed by a separate controller.
- the data acquisition steps of steps S11 and S34 may be performed by an on-board controller, and the acquired data may be transmitted to an off-vehicle controller, which may then process steps S12, S13, S20, S31 to S33, S36, and S40.
- the created swing angle error table T may be transmitted to the on-board controller.
- attitude detection controller 68 and the main body controller 51 are provided separately, but a single controller may serve both functions.
- the guide screen 72 is displayed on the display input device 70, but if the input device 48 of the work machine 1 is equipped with a display such as a touch panel, it may be displayed on the input device 48. In that case, the communication device 71 does not need to be provided.
- a hydraulic excavator has been used as the work machine 1, but it is not limited to a hydraulic excavator and may be other machines such as a mechanical shovel or a rope shovel.
- the work machine 1 according to the above embodiment is a so-called back-hoe type shovel, but it may also be a face shovel.
- the lower structure is not limited to tracks and may be wheels.
- the lower structure may be a ship's hull such as a barge.
- the work machine 1 may be a dredger equipped with a work implement including a boom.
- the present disclosure provides a swing angle calibration method, a posture detection method, and a posture detection system that can calibrate errors that occur when measuring the swing angle of a work machine using an angle sensor, and can detect the swing angle with high accuracy.
Landscapes
- Engineering & Computer Science (AREA)
- Mechanical Engineering (AREA)
- Mining & Mineral Resources (AREA)
- Civil Engineering (AREA)
- General Engineering & Computer Science (AREA)
- Structural Engineering (AREA)
- Operation Control Of Excavators (AREA)
Abstract
スイング角度較正方法のステップ(S11)は、作業機(3)を左右方向へのスイングのみを行った複数のスイング姿勢(K1~K7)でトータルステーション(TS)を用いて作業機(3)の所定位置の位置情報を取得する。ステップ(S12、S13、S21~S24、S31~S33)は、位置情報に基づいて各々のスイング姿勢におけるスイング角度(θi)を算出する。ステップ(S34,S35)は、スイング角度センサ(67)による複数のスイング姿勢(K1~K7)での検出値に基づいて各々のスイング姿勢(K1~K7)におけるスイング角度(θswi)を取得する。ステップ(S40)は、複数のスイング姿勢(K1~K7)におけるスイング角度(θi)とスイング角度(θswi)に基づいてスイング角度誤差テーブル(T)を作成する。
Description
本開示は、スイング角度較正方法、姿勢検出方法、スイング角度較正システムおよび姿勢検出システムに関する。
作業機を有する作業機械において、アタッチメントであるバケットの刃先の位置を算出する技術が、知られている。例えば、特許文献1に示す油圧ショベルは、車両本体と作業機とを備えている。車両本体には、車体の位置を検出するために、例えばGNSS(Global Navigation Satellite System)のアンテナが設けられている。また、車両本体には、例えばIMU(Inertial Measurement Unit)が配置されている。IMUは、車両本体のロール角、及び、ピッチ角などを検出する。作業機は、ブームと、アームと、バケットと、それらを駆動する油圧シリンダとを有している。油圧ショベルのコントローラは、GNSSアンテナとブームピンの位置関係、ブームとアームとバケットのそれぞれの長さ、ブームとアームとバケットのそれぞれの回転角度等のパラメータに基づいて、バケットの刃先の位置を演算している。
このように刃先位置を演算するために、初期設定時には、油圧ショベルが備える作業機のパラメータが較正される。例えば、外部計測装置を用いて作業機の所定位置を計測し、その計測値に基づいて作業機の寸法等に関するパラメータが較正される。特許文献1では、作業機の自重により油圧ショベルが車体前後方向に傾斜することによって生じる高さ方向における刃先の位置の精度低下を抑制するために、車両本体のIMUによるピッチ角を利用して較正を行うことが提案されている。
しかしながら、特許文献1では、作業機のスイング角度を検出する角度センサが誤差を持つ場合は考慮しておらず、その場合には刃先位置の検出精度が低下していた。
本開示は、作業機のスイング角度を検出する角度センサで測定する場合に生じる誤差を較正し、精度良くスイング角度を検出することが可能なスイング角度較正方法、姿勢検出方法、スイング角度較正システムおよび姿勢検出システムを提供することを目的とする。
(課題を解決するための手段)
(課題を解決するための手段)
本開示にかかる第1の態様のスイング角度較正方法は、作業機械の作業機のスイング角度を較正するスイング角度較正方法であって、位置情報取得ステップと、第1スイング角度算出ステップと、較正用第2スイング角度取得ステップと、作成ステップと、を備える。位置情報取得ステップは、作業機を所定の姿勢に固定した状態で左右方向へのスイングのみを行った複数のスイング姿勢で、計測装置を用いて作業機の所定位置の位置情報を取得する。第1スイング角度算出ステップは、位置情報に基づいて、各々のスイング姿勢における第1スイング角度を算出する。較正用第2スイング角度取得ステップは、作業機械に搭載され、作業機のスイング角度を検出する角度センサによる複数のスイング姿勢での検出値に基づいて、各々のスイング姿勢における第2スイング角度を取得する。作成ステップは、複数のスイング姿勢における第1スイング角度と第2スイング角度に基づいて相関テーブルを作成する。
本開示にかかる第2の態様の姿勢検出方法は、作業機械の作業機の姿勢を検出する姿勢検出方法であって、第2スイング角度取得ステップと、補正後第2スイング角度検出ステップと、姿勢検出ステップと、を備える。第2スイング角度取得ステップは、作業機械に搭載され、作業機のスイング角度を検出する角度センサの検出値に基づいて第2スイング角度を取得する。補正後第2スイング角度検出ステップは、第1の態様のスイング角度較正方法の相関テーブルと第2スイング角度に基づいて、誤差補正後の第2スイング角度を検出する。姿勢検出ステップは、誤差補正後の第2スイング角度を用いて作業機の姿勢を検出する。
本開示にかかる第3の態様のスイング角度較正システムは、作業機械の作業機のスイング角度を較正するスイング角度較正システムであって、計測装置と、角度センサと、コントローラと、を備える。計測装置は、作業機を所定の姿勢に固定した状態で左右方向へのスイングのみを行った複数のスイング姿勢で、作業機の所定位置の位置情報を計測する。角度センサは、作業機械に搭載され、作業機のスイング角度を検出する。コントローラは、位置情報に基づいて、各々のスイング姿勢における第1スイング角度を算出し、角度センサによる複数のスイング姿勢での検出値に基づいて、各々のスイング姿勢における第2スイング角度を取得し、複数のスイング姿勢における第1スイング角度と第2スイング角度に基づいて相関テーブルを作成する。
本開示にかかる第4の態様の姿勢検出システムは、作業機械の作業機の姿勢を検出する姿勢検出システムであって、角度センサと、記憶部と、コントローラと、を備える。角度センサは、作業機械に搭載され、作業機のスイング角度を検出する。記憶部は、第1の態様のスイング角度較正方法の相関テーブルを記憶する。コントローラは、角度センサで検出したスイング角度を相関テーブルに基づいて補正し、補正後のスイング角度を用いて、作業機の姿勢を検出する。
(発明の効果)
(発明の効果)
本開示によれば、作業機のスイング角度を検出する角度センサで測定する場合に生じる誤差を較正し、精度良くスイング角度を検出することが可能なスイング角度較正方法、姿勢検出方法および姿勢検出システムを提供することができる。
以下、図面を参照して、本開示の一実施形態に係るスイング角度較正方法、姿勢検出方法および姿勢検出システムについて説明する。
図1は、本開示にかかる作業機械1を示す図である。作業機械1は、例えば、油圧ショベルを挙げることができる。
(作業機械1の概要)
作業機械1は、車両本体2(作業機械本体の一例)と、作業機3を有する。
作業機械1は、車両本体2(作業機械本体の一例)と、作業機3を有する。
車両本体2は、旋回体11と、下部構造体12と、ブレード13と、を有する。旋回体11は、下部構造体12の上側に配置されている。旋回体11は、下部構造体12に対して旋回可能に支持されている。旋回体11は、上面のフロア14と、フロア14に配置された運転席15と、運転席15の上方に配置されたルーフ16とを有している。本実施形態の作業機械1は、キャノピー型である。下部構造体12は履帯12a、12bを含む。履帯12a、12bが回転することにより作業機械1が走行する。ブレード13は、下部構造体12の前方に配置されている。ブレード13は、油圧アクチュエータによってリフト動作可能である。
なお、以下の説明において、「前」および「後」とは車両本体2の前後を意味する。また、以下の説明における「右」、「左」、「上」、及び「下」とは運転席15から前方を見た状態を基準とする方向を示す。「車幅方向」と「左右方向」は同義である。図には、前方向F、後方向B、左方向Lおよび右方向Rが示されている。
(作業機3)
作業機3は、車両本体2に取り付けられている。作業機3は、スイングブラケット20と、ブーム21と、アーム22と、バケット23と、を含む。スイングブラケット20は、旋回体11の前側に配置されている。図2は、作業機械1のスイングブラケット近傍を示す斜視図である。図3は、スイングブラケット20の近傍を示す上面図である。図3では作業機3は省略されている。図2および図3に示すスイングブラケット20は、旋回体11の前後方向に沿って作業機3が配置されている状態である。
作業機3は、車両本体2に取り付けられている。作業機3は、スイングブラケット20と、ブーム21と、アーム22と、バケット23と、を含む。スイングブラケット20は、旋回体11の前側に配置されている。図2は、作業機械1のスイングブラケット近傍を示す斜視図である。図3は、スイングブラケット20の近傍を示す上面図である。図3では作業機3は省略されている。図2および図3に示すスイングブラケット20は、旋回体11の前後方向に沿って作業機3が配置されている状態である。
スイングブラケット20は、旋回体11の前側に配置されている。図2に示すように、旋回体11は、前端部に支持ブラケット18を有している。支持ブラケット18は、前方に向かって突出するように配置されている。図3に示すように、スイングブラケット20は、上下方向のピン19によって上面視において時計回りおよび反時計回りに回転可能に支持ブラケット18に装着されている。図3には、ピン19の回転中心が軸A1として示されている。
ブーム21の基端は、図1に示すように、ブームピン24を介してスイングブラケット20に上下方向に回転可能に取り付けられている。アーム22の基端は、アームピン25を介してブーム21の先端に回転可能に取り付けられている。バケット23は、バケットピン26を介してアーム22の先端に回転可能に取り付けられている。アーム22とバケット23の間は、リンク27によって接続されている。
スイングブラケット20は、図3に示すように、ブラケット本体32と、レバー部33と、を有する。ブラケット本体32は、図3に示すように、ピン19によって回転可能に支持ブラケット18に支持されている。レバー部33は、図2および図3に示すように、ブラケット本体32から右方向Rに突出している。
ブラケット本体32は、図3に示すように、作業機支持部34と、リンク機構支持部35と、を有する。作業機支持部34は、作業機3を上下方向に回転可能に支持する。作業機支持部34は、図3に示すように、左側部34aと右側部34bと、を有する。左側部34aと右側部34bは、ブーム21の基端を左右から挟むように配置されている。リンク機構支持部35と作業機支持部34は、軸A1を挟んで配置されている。リンク機構支持部35は、後述するリンク機構36を支持する。
作業機3は、図1に示すように、ブームシリンダ28と、アームシリンダ29と、バケットシリンダ30、スイングシリンダ31(図2参照)と、を含む。ブームシリンダ28と、アームシリンダ29と、バケットシリンダ30と、スイングシリンダ31は、それぞれ油圧シリンダである。
ブームシリンダ28のボトム側の端は、スイングブラケット20に回転可能に取り付けられている。ブームシリンダ28のボトム側の端は、図3に示す左側部34aと右側部34bの間に配置される。ブームシリンダ28のロッド側の端は、ブーム21に回転可能に取り付けられている。
アームシリンダ29のボトム側の端は、ブーム21に回転可能に取り付けられている。アームシリンダ29のロッド側の端は、アーム22に回転可能に取り付けられている。
バケットシリンダ30のボトム側の端は、アーム22に回転可能に取り付けられている。バケットシリンダ30のロッド側の端は、バケット23にリンク27を介して回転可能に取り付けられている。
ブームシリンダ28が伸縮することで、ブーム21が上下方向に回転する。アームシリンダ29が伸縮することで、ブーム21に対してアーム22が回転する。バケットシリンダ30が伸縮することで、アーム22に対してバケット23が回転する。
スイングシリンダ31は、旋回体11とレバー部33の間を接続する。スイングシリンダ31のボトム側の端は、旋回体11に回転可能に取り付けられている。スイングシリンダ31のロッド側の端は、レバー部33に回転可能に取り付けられている。スイングシリンダ31が伸縮することによって、スイングブラケット20を含む作業機3が旋回体11に対して左右方向にスイングする。
(駆動システム41および駆動制御システム42)
作業機械1は、駆動システム41と、駆動制御システム42と、を有する。図4は、作業機械1の駆動システム41と駆動制御システム42の構成を示すブロック図である。駆動システム41は、駆動源43と、油圧ポンプ44とを備えている。駆動源43は、例えば内燃エンジンである。ただし、駆動源43は、電動モータ、あるいはエンジンと電動モータのハイブリッド機構であってもよい。油圧ポンプ44は、駆動源43によって駆動され、作動油を吐出する。油圧ポンプ44から吐出された作動油は、ブームシリンダ28、アームシリンダ29、バケットシリンダ30,およびスイングシリンダ31に供給される。作業機械1は、第1走行モータ45aと、第2走行モータ45bと、旋回モータ46と、を備えている。第1走行モータ45aは、履帯12aを駆動する。第2走行モータ45bは、履帯12bを駆動する。旋回モータ46は、旋回体11を旋回させる。油圧ポンプ44から吐出された作動油は、第1走行モータ45aと、第2走行モータ45bと、旋回モータ46とに供給される。なお、図4では、1つの油圧ポンプ44が図示されているが、複数の油圧ポンプが設けられていてもよい。
作業機械1は、駆動システム41と、駆動制御システム42と、を有する。図4は、作業機械1の駆動システム41と駆動制御システム42の構成を示すブロック図である。駆動システム41は、駆動源43と、油圧ポンプ44とを備えている。駆動源43は、例えば内燃エンジンである。ただし、駆動源43は、電動モータ、あるいはエンジンと電動モータのハイブリッド機構であってもよい。油圧ポンプ44は、駆動源43によって駆動され、作動油を吐出する。油圧ポンプ44から吐出された作動油は、ブームシリンダ28、アームシリンダ29、バケットシリンダ30,およびスイングシリンダ31に供給される。作業機械1は、第1走行モータ45aと、第2走行モータ45bと、旋回モータ46と、を備えている。第1走行モータ45aは、履帯12aを駆動する。第2走行モータ45bは、履帯12bを駆動する。旋回モータ46は、旋回体11を旋回させる。油圧ポンプ44から吐出された作動油は、第1走行モータ45aと、第2走行モータ45bと、旋回モータ46とに供給される。なお、図4では、1つの油圧ポンプ44が図示されているが、複数の油圧ポンプが設けられていてもよい。
駆動制御システム42は、操作装置47と、入力装置48と、を備える。操作装置47および入力装置48は、運転席15の周囲に配置されている。操作装置47は、作業機3、旋回体11、および下部構造体12を駆動するためのオペレータによる操作を受け付け、操作に応じた操作信号を出力する。操作装置47は、例えば、レバー、ペダル、スイッチ等を含む。
入力装置48は、作業機械1の制御の設定を行うためのオペレータによる操作を受け付け、操作に応じた操作信号を出力する。入力装置48は、例えば、タッチスクリーンである。或いは、入力装置48は、レバー、あるいはスイッチを含んでもよい。
駆動制御システム42は、本体コントローラ51と、記憶装置52と、制御弁53と、を含む。本体コントローラ51は、取得した操作信号に基づいて作業機械1を制御するようにプログラムされている。本体コントローラ51は、CPU(Central Processing Unit)などのプロセッサと、RAM(Random Access Memory)、及びROM(Read Only Memory)などのメモリを含む。記憶装置52は、非一時的な(non-transitory)プロセッサで読み取り可能な記録媒体の一例である。記憶装置52は、プロセッサによって実行可能であり作業機械1を制御するためのコンピュータ指令を記録している。
本体コントローラ51は、操作装置47および入力装置48から操作信号を取得する。本体コントローラ51は、操作信号に基づいて、制御弁53を制御する。制御弁53は、圧力比例制御弁であってもよい。或いは、制御弁53は、電磁比例制御弁であってもよい。制御弁53は、油圧ポンプ44から第1走行モータ45aと第2走行モータ45bとに供給される作動油の流量を制御する。それにより、操作装置47の操作に応じて作業機械1が走行する。制御弁53は、油圧ポンプ44から、ブームシリンダ28、アームシリンダ29、バケットシリンダ30,およびスイングシリンダ31に供給される作動油の流量を制御する。本体コントローラ51は、操作装置47の操作に応じて、ブーム21、アーム22、およびバケット23が動作するように、制御弁53への指令信号を生成する。制御弁53は、油圧ポンプ44から旋回モータ46へ供給される作動油の流量を制御する。本体コントローラ51は、操作装置47の操作に応じて、旋回体11が旋回するように、制御弁53への指令信号を生成する。本体コントローラ51は、操作装置47の操作に応じて、作業機3が車両本体2に対して左右にスイングするように、制御弁53への指令信号を生成する。
(姿勢検出システム60)
作業機械1は、姿勢検出システム60を備える。図5は、姿勢検出システム60の構成を示すブロック図である。姿勢検出システム60は、GNSSレシーバ61と、一対のGNSSアンテナ62a、62bと、本体IMU63と、ブームIMU64と、アームIMU65と、バケットIMU66と、スイング角度センサ67(角度センサの一例)と、姿勢検出コントローラ68と、記憶装置69と、表示入力装置70と、通信装置71と、を備える。
作業機械1は、姿勢検出システム60を備える。図5は、姿勢検出システム60の構成を示すブロック図である。姿勢検出システム60は、GNSSレシーバ61と、一対のGNSSアンテナ62a、62bと、本体IMU63と、ブームIMU64と、アームIMU65と、バケットIMU66と、スイング角度センサ67(角度センサの一例)と、姿勢検出コントローラ68と、記憶装置69と、表示入力装置70と、通信装置71と、を備える。
GNSSレシーバ61および一対のGNSSアンテナ62a、62bは、車両本体2に配置されている。GNSSアンテナ62a、62bは、図1に示すように、左右方向において所定間隔を空けて配置されている。GNSS(Global Navigation Satellite System)レシーバ61は、例えばGPS(Global Positioning System)用の受信機である。GNSSレシーバ61は、衛星より測位信号を受信し、測位信号によりGNSSアンテナ62a、62bの位置を演算して車体位置データを作成する。車体位置データは、GNSSによるグローバル座標系における位置情報と、旋回体11の向きの情報を含む。姿勢検出コントローラ68は、GNSSレシーバ61から車体位置データを取得する。
本体IMU63は、図1に示すように、車両本体2に配置されている。本体IMU63は、本体傾斜角データを取得する。車両傾斜角データは、車両前後方向の水平に対する角度(ピッチ角)、および車両左右方向の水平に対する角度(ロール角)を含む。本体IMU63は、姿勢検出コントローラ68に車両傾斜角データを出力する。
ブームIMU64は、図1に示すように、ブーム21に配置されている。ブームIMU64は、ブーム21の傾斜角データを取得する。ブーム傾斜角データは、ブーム21の水平方向に対する上下方向への傾斜角を含む。ブームIMU64は、姿勢検出コントローラ68にブーム傾斜角データを出力する。
アームIMU65は、図1に示すように、アーム22に配置されている。アームIMU65は、アーム22の傾斜角データを取得する。アーム傾斜角データは、アーム22の水平方向に対する上下方向への傾斜角を含む。アームIMU65は、姿勢検出コントローラ68にアーム傾斜角データを出力する。
バケットIMU66は、図1に示すように、バケット23が接続されているリンク27に配置されている。バケットIMU66は、バケット23の傾斜角データを取得する。バケット傾斜角データは、バケット23の水平方向に対する上下方向への傾斜角を含む。バケットIMU66は、姿勢検出コントローラ68にバケット傾斜角データを出力する。
スイング角度センサ67は、スイングブラケット20の旋回体11に対するスイング角度を求めるための検出値を検出する。スイング角度センサ67は、スイングブラケット20の側方に配置されている。スイング角度センサ67は、旋回体11に固定されている。
スイング角度センサ67は、図3に示すように、リンク機構36によってスイングブラケット20に接続されている。スイング角度センサ67は、図示しない回転センサと、マグネットと、マグネットに接続されたピン37と、を含む。スイングブラケット20が回転すると、その回転がリンク機構36を介してピン37に伝達され、ピン37が回転軸A2を中心にして回転し、ピン37に固定されているマグネットが回転する。マグネットの回転角度を回転センサが検出し、その検出値であるスイング角度データを姿勢検出コントローラ68に出力する。
姿勢検出コントローラ68は、CPU(Central Processing Unit)などのプロセッサと、RAM(Random Access Memory)、及びROM(Read Only Memory)などのメモリを含む。記憶装置69は、非一時的な(non-transitory)プロセッサで読み取り可能な記録媒体の一例である。記憶装置69は、プロセッサによって実行可能であり作業機械1を制御するためのコンピュータ指令を記録している。
図6は、作業機械1の構成を模式的に示す側面図である。姿勢検出コントローラ68は、本体傾斜角データ、ブーム傾斜角データ、アーム傾斜角データ、およびバケット傾斜角データに基づいて、ブーム角α1、アーム角α2、およびバケット角α3を算出する。ブーム角α1は、車両本体2に対するブーム21の傾斜角を示す。アーム角α2は、ブーム21に対するアーム22の傾斜角を示す。バケット角α3は、アーム22に対するバケット23の傾斜角を示す。なお、ブームIMU64、アームIMU65、およびバケットIMU66の代わりに、ブームシリンダ28、アームシリンダ29、およびバケットシリンダ30の各々のストローク量を検出するセンサが設けられ、センサの検出値からブーム角α1、アーム角α2、およびバケット角α3を算出してもよい。
姿勢検出コントローラ68は、スイング角度データからスイング角度を算出し、記憶装置69に記憶されているスイング角度誤差テーブルに基づいて、較正後のスイング角度を算出する。スイング角度誤差テーブルおよびスイング角度の算出については後段にて詳述する。
記憶装置69は、車両本体2と作業機3との形状データとを記憶している。車両本体2の形状データは、車両本体2の形状を示す。車両本体2の形状データは、GNSSアンテナ62と車両本体2における基準位置との位置関係を示す。車両本体2の形状データは、車両本体2における基準位置と、ブームピン24との位置関係を示す。
作業機3の形状データは、作業機3の形状を示す。形状データは、ブーム長L1と、アーム長L2と、バケット長L3とを含む。ブーム長L1は、ブームピン24からアームピン25までの長さである。アーム長L2は、アームピン25からバケットピン26までの長さである。バケット長L3は、バケットピン26からバケット23の刃先位置P10までの長さである。姿勢検出コントローラ68は、車両傾斜角データ、ブーム傾斜角データ、アーム傾斜角データ、バケット傾斜角データ、およびスイング角度データに基づいて、GNSSレシーバ61が取得した車体位置データから、バケット位置データを検出する。バケット位置データは、バケット23の刃先位置P10を示す。
記憶装置69は、現況地形データと設計地形データとを記憶している。現況地形データは、作業現場の現況地形を示す。設計地形データは、作業現場の目標形状を示す。姿勢検出コントローラ68は、現況地形データと、設計地形データと、形状データとに基づいて、図7に示すガイド画面72を表示入力装置70に表示する。図7に示すように、ガイド画面72は、現況地形73と、設計地形74と、作業機械1との位置を示す。形状データは、バケット23の形状を示すデータを含む。姿勢検出コントローラ68は、バケット23の形状データとバケット位置データとに基づいて、現況地形73及び設計地形74に対するバケット23の位置をガイド画面72に示す。
表示入力装置70は、作業者の入力指示に基づいて、上記ガイド画面72を表示する。表示入力装置70は、通信装置71を介してインターネットを通じて姿勢検出コントローラ68と通信を行う。表示入力装置70は、例えば、タブレット端末である。通信装置71は、表示入力装置70をインターネットに接続する。表示入力装置70と通信装置71の間の通信は、有線または無線のいずれで行われてもよい。通信装置71は、例えば、無線LANルータである。
(スイング角度較正方法)
次に、記憶装置69に記憶されているスイング角度誤差テーブルを作成するスイング角度較正方法について説明するとともにスイング角度較正システムについても同時に述べる。図8Aおよび図8Bは、スイング角度較正方法を示すフロー図である。
次に、記憶装置69に記憶されているスイング角度誤差テーブルを作成するスイング角度較正方法について説明するとともにスイング角度較正システムについても同時に述べる。図8Aおよび図8Bは、スイング角度較正方法を示すフロー図である。
図8Aおよび図8Bに示すように、スイング角度較正方法では、ステップS10において、複数のスイング姿勢において作業機の所定の点(スイング点)の座標を計測し、それぞれの点を所定のスイング平面上に投影し、スイング平面上における複数のスイング点を作成する。次に、ステップS20において、スイング平面におけるスイング座標系を作成する。次に、ステップS30において、スイング座標系において複数のスイング点におけるスイング角度を計算する。次に、ステップS40において、スイング角度誤差テーブルを作成する。
以下、ステップS10~S40の各々のステップにおける詳細について説明する。
図9(a)は、作業機3を複数のスイング姿勢に移動させた状態を示す正面図である。図9(b)は、作業機3を複数のスイング姿勢に移動させてトータルステーションTS(計測装置の一例)でアームトップの座標を計測している状態を示す平面図である。例えばアーム22の先端(アームトップ)にプリズムが貼り付けられる。
ステップS10におけるステップS11において、ブーム角α1、アーム角α2、およびバケット角α3を固定した状態でスイング姿勢のみを変化させて、アームトップに貼り付けられているプリズムからの反射波をトータルステーションTSで受信してプリズムの座標を計測する。
例えば、図9(b)に示すように、複数のスイング姿勢K1~K7におけるアームトップの点p1~p7の座標が計測される。点p1は、旋回体11に対して作業機3を右方向にスイングさせた際の機構的限界となるスイング姿勢K1(右エンド)におけるアームトップの位置である。点p3は、平面視において作業機3が旋回体11の前後方向に平行に配置されたスイング姿勢K3(中立姿勢)におけるアームトップの位置である。点p2は、スイング姿勢K1とスイング姿勢K3の間のスイング姿勢K2におけるアームトップの位置である。スイング姿勢K2は、スイング姿勢K1とスイング姿勢K3の概ね中央に位置するように設定される。点p7は、旋回体11に対して作業機3を左方向にスイングさせた際の機構的限界となるスイング姿勢K7(左エンド)におけるアームトップの位置である。点p4は、スイング姿勢K4におけるアームトップの位置である。点p5は、スイング姿勢K5におけるアームトップの位置である。点p6は、スイング姿勢K6におけるアームトップの位置である。スイング姿勢K4、K5、K6は、スイング姿勢K3とスイング姿勢K7の間に順に設定されている。スイング姿勢K4、K5、K6は、概ねスイング姿勢K3とスイング姿勢K7の間を4等分するように設定されている。なお、スイング姿勢K2~K6は、厳密でなくてもよく、作業者による目測でよい。
より具体的には、ステップS11において、表示入力装置70に、図10Aに示すスイング角度較正画面80が表示される。スイング角度較正画面80は、画面に表示されているスイング姿勢にするように作業者に指示を行う。スイング角度較正画面80は、左画面81と、右画面82と、X座標入力部83と、Y座標入力部84と、Z座標入力部85と、を有する。左画面81は、作業機械1の側面図を表示する。右画面82は、作業機械1の平面図を表示する。左画面81および右画面82には、プリズム91が示されている。右画面82には、トータルステーションTSが示されており、トータルステーションTSにおけるXYZ座標が示されている。
作業者が、図10Aに示すスイング角度較正画面80に従って、目測でスイング姿勢K3になるように操作装置47を操作してスイングシリンダ31を駆動させ、作業機3を旋回体11に対して回転させる。そして、トータルステーションTSによって、プリズム91の位置座標が検出される。トータルステーションTSで検出されたXYZ座標は、作業者によってX座標入力部83と、Y座標入力部84と、Z座標入力部85に入力される。入力されたXYZ座標は、姿勢検出コントローラ68に出力される。
XYZ座標の入力後、作業者が進む釦86を押下すると、図10Bに示すスイング角度較正画面80が表示される。図10Bのスイング角度較正画面80では、作業機3をスイング姿勢K1にするように作業者に指示が行われる。
作業者が、スイング角度較正画面80に従って、ブーム角α1、アーム角α2、およびバケット角α3を固定した状態で旋回体11に対して作業機3を機構限界に達するまで右方向にスイングさせる。これによって作業機3がスイング姿勢K1となる。そして、トータルステーションTSによって、プリズム91の位置座標が検出される。トータルステーションTSで検出されたXYZ座標は、作業者によってX座標入力部83と、Y座標入力部84と、Z座標入力部85に入力される。入力されたXYZ座標は、姿勢検出コントローラ68に出力される。
次に、進む釦86を押下すると、スイング角度較正画面80には、ブーム角α1、アーム角α2、およびバケット角α3を固定した状態で作業機3をスイング姿勢K2に配置させるような指示が表示される。そして、上記と同様に、スイング姿勢K2におけるトータルステーションTSで検出された点p2のXYZ座標の値が入力され、入力されたXYZの値は、姿勢検出コントローラ68に出力される。
同様に、スイング姿勢K4~K7において、トータルステーションTSによる点p4~p7のXYZ座標の値が姿勢検出コントローラ68に出力される。
以上の動作によって、姿勢検出コントローラ68は、スイング姿勢K1~K7におけるアームトップの点p1~p7のトータルステーションTSにおけるXYZ座標の値を取得する。
次に、図8AのステップS12に示すように、姿勢検出コントローラ68は、最小二乗法を用いて、スイング動作平面を算出する。具体的には、式(1)および式(2)のように、点p1~p7におけるトータルステーションTSのXYZ座標をpi、その重心ベクトルをgとする。
(式1)
(式2)
点pi各点からの距離が最小となるスイング平面Uの方程式を以下の式(3)とすると、以下の式(4)より最小二乗法を用いてa, b, cが求められる。
(式3)
(式4)
スイング平面Uの法線ベクトルをNとし、原点からの距離をhとし、Nの単位ベクトルをnとすると、Nは以下の式(5)で示され、nは以下の式(6)で示され、hは以下の式(7)で示される。
(式5)
(式6)
(式7)
次に、ステップS13において、図11Aに示すように、姿勢検出コントローラ68は、点pi(i=1、2、3、4、5、6、7)をスイング平面Uに投影し、点p´i(i=1、2、3、4、5、6、7)を作成する。スイング平面Uに投影した点pi´は、以下の式(8)で表わすことができる。
(式8)
次に、ステップS20(スイング座標系の作成)のステップS21において、姿勢検出コントローラ68は、スイング点pi´(i=1、2、3、4、5、6、7)を平行移動し、スイング点p”i(i=1、2、3、4、5、6、7)を作成する。具体的には、姿勢検出コントローラ68は、以下の式(9)に示すように、スイング点p´iを-pi´平行移動した点をp”iとする。これは、スイング座標系においてp´iを原点とするためである。
(式9)
次に、ステップS22において、図11Bに示すように、姿勢検出コントローラ68は、p”1p”7ベクトルの単位ベクトルsxをスイング平面Uのx軸として算出する。単位ベクトルsxは、以下の(式10)で表わされる。
(式10)
次に、ステップS23において、図11Bに示すように、姿勢検出コントローラ68は、以下の(式11)に示すように、sxとn(ステップS12における法線ベクトルNの単位ベクトル)の外積syをスイング平面Uのy軸に設定する。また、姿勢検出コントローラ68は、以下の式(12)に示すように、nをスイング平面Uのz軸に設定する。
(式11)
(式12)
次に、ステップS24において、姿勢検出コントローラ68は、トータルステーションTSの座標系からスイング平面Uの座標系へと変換する行列Qを算出する。行列Qは、以下の(式13)で表わされる。
(式13)
次に、ステップS30(スイング角度計算)のステップS31において、姿勢検出コントローラ68は、以下の(式14)を用いてスイング点pi”をスイング座標系へと変換した点をスイング変換点psiとする。
(式14)
次に、図8Bに示すように、ステップS32において、姿勢検出コントローラ68は、最小二乗法を用いて、sxをx軸とし、syをy軸とするスイング平面U上においてスイング変換点psiとの誤差が最小となる円を算出する。円を以下の(式15)で表わす。
(式15)
この式15のA、BおよびCは、以下の(式16)を用いて求めることができる。
(式16)
ここで、図11Cに示すように、円の中心座標をso(a、b)とし、半径をrとすると、a、b、およびrと、A、BおよびCの関係は以下の(式17)~(式19)で表わすことができる。
(式17)
(式18)
(式19)
これにより、円の中心座標so(a、b)および半径rが求められる。
(式1)
(式2)
点pi各点からの距離が最小となるスイング平面Uの方程式を以下の式(3)とすると、以下の式(4)より最小二乗法を用いてa, b, cが求められる。
(式3)
(式4)
スイング平面Uの法線ベクトルをNとし、原点からの距離をhとし、Nの単位ベクトルをnとすると、Nは以下の式(5)で示され、nは以下の式(6)で示され、hは以下の式(7)で示される。
(式5)
(式6)
(式7)
次に、ステップS13において、図11Aに示すように、姿勢検出コントローラ68は、点pi(i=1、2、3、4、5、6、7)をスイング平面Uに投影し、点p´i(i=1、2、3、4、5、6、7)を作成する。スイング平面Uに投影した点pi´は、以下の式(8)で表わすことができる。
(式8)
次に、ステップS20(スイング座標系の作成)のステップS21において、姿勢検出コントローラ68は、スイング点pi´(i=1、2、3、4、5、6、7)を平行移動し、スイング点p”i(i=1、2、3、4、5、6、7)を作成する。具体的には、姿勢検出コントローラ68は、以下の式(9)に示すように、スイング点p´iを-pi´平行移動した点をp”iとする。これは、スイング座標系においてp´iを原点とするためである。
(式9)
次に、ステップS22において、図11Bに示すように、姿勢検出コントローラ68は、p”1p”7ベクトルの単位ベクトルsxをスイング平面Uのx軸として算出する。単位ベクトルsxは、以下の(式10)で表わされる。
(式10)
次に、ステップS23において、図11Bに示すように、姿勢検出コントローラ68は、以下の(式11)に示すように、sxとn(ステップS12における法線ベクトルNの単位ベクトル)の外積syをスイング平面Uのy軸に設定する。また、姿勢検出コントローラ68は、以下の式(12)に示すように、nをスイング平面Uのz軸に設定する。
(式11)
(式12)
次に、ステップS24において、姿勢検出コントローラ68は、トータルステーションTSの座標系からスイング平面Uの座標系へと変換する行列Qを算出する。行列Qは、以下の(式13)で表わされる。
(式13)
次に、ステップS30(スイング角度計算)のステップS31において、姿勢検出コントローラ68は、以下の(式14)を用いてスイング点pi”をスイング座標系へと変換した点をスイング変換点psiとする。
(式14)
次に、図8Bに示すように、ステップS32において、姿勢検出コントローラ68は、最小二乗法を用いて、sxをx軸とし、syをy軸とするスイング平面U上においてスイング変換点psiとの誤差が最小となる円を算出する。円を以下の(式15)で表わす。
(式15)
この式15のA、BおよびCは、以下の(式16)を用いて求めることができる。
(式16)
ここで、図11Cに示すように、円の中心座標をso(a、b)とし、半径をrとすると、a、b、およびrと、A、BおよびCの関係は以下の(式17)~(式19)で表わすことができる。
(式17)
(式18)
(式19)
これにより、円の中心座標so(a、b)および半径rが求められる。
次に、ステップS33において、姿勢検出コントローラ68は、スイング変換点ps3からスイング変換点ps1、ps2、ps4、ps5、ps6、ps7までのスイング角度θi(第1スイング角度の一例)を算出する。図11Dに示すように、スイング中心soからpsiへのベクトルをqiとする。q3が中立姿勢である。θi(i=1、2、4、5、6、7)は、以下の(式20)によって求めることができる。
(式20)
一方、ステップS34において、姿勢検出コントローラ68は、スイング姿勢K1~K7におけるスイング角度センサ67による検出値であるスイング角度データを取得する。これらのスイング角度データは、ステップS11において作業機3を旋回体11に対して旋回してスイング姿勢K1~K7にしたときに、スイング角度センサ67が取得すればよい。
(式20)
一方、ステップS34において、姿勢検出コントローラ68は、スイング姿勢K1~K7におけるスイング角度センサ67による検出値であるスイング角度データを取得する。これらのスイング角度データは、ステップS11において作業機3を旋回体11に対して旋回してスイング姿勢K1~K7にしたときに、スイング角度センサ67が取得すればよい。
次に、ステップS35において、姿勢検出コントローラ68は、スイング姿勢K1~K7におけるスイング角度データから、各々のスイング姿勢におけるスイング角度を算出する。ここで、姿勢検出コントローラ68が算出したスイング角度θsw(第2スイング角度の一例)をθsw1,θsw2,・・・θsw7とする。
次に、ステップS36において、姿勢検出コントローラ68は、スイング姿勢K1~K7の各々における角度誤差eiを算出する。角度誤差eiは、以下の(式21)によって算出される。
(式21)
次に、ステップS40において、姿勢検出コントローラ68は、スイング角度誤差テーブルT(相関テーブルの一例)を作成する。(θsw1,e1)、(θsw2,e2)、・・・(θsw7,e7)を結ぶ線分をy=ai*x+biとすると、aiとbiは以下の(式22)で表される。
(式22)
図12は、スイング角度誤差テーブルTを示す図である。図12の横軸は、スイング角度センサ67から算出したスイング角度(deg)を示す。縦軸は、スイング角度の誤差(deg)を示す。横軸に示すように、中立姿勢であるスイング姿勢K3から右方向へのスイング角度をマイナスで示し、スイング姿勢K3から左方向へのスイング角度をプラスで示している。
(式21)
次に、ステップS40において、姿勢検出コントローラ68は、スイング角度誤差テーブルT(相関テーブルの一例)を作成する。(θsw1,e1)、(θsw2,e2)、・・・(θsw7,e7)を結ぶ線分をy=ai*x+biとすると、aiとbiは以下の(式22)で表される。
(式22)
図12は、スイング角度誤差テーブルTを示す図である。図12の横軸は、スイング角度センサ67から算出したスイング角度(deg)を示す。縦軸は、スイング角度の誤差(deg)を示す。横軸に示すように、中立姿勢であるスイング姿勢K3から右方向へのスイング角度をマイナスで示し、スイング姿勢K3から左方向へのスイング角度をプラスで示している。
図12に示すスイング角度誤差テーブルTが記憶装置69に記憶される。姿勢検出コントローラ68は、スイング角度センサ67の検出値であるスイング角度データから算出したスイング角度θswに対してスイング角度誤差テーブルTを用いて、以下の式(23)に示すように較正を行ってスイング角度θsw´を算出する。
(式23)
例えば、スイング角度センサ67から得られたスイング角度θswがθsw1とθsw2の間の場合、y=a1*x+b1の直線M1(図12参照)を用いて誤差を算出し、得られたスイング角度から誤差を引くことによって、誤差補正を行った後のスイング角度θsw´を得ることができる。なお、ステップS11は、位置情報取得ステップの一例に対応する。ステップS12、S13、S21~S24、S31~S33は、第1スイング角度算出ステップの一例に対応する。ステップS34、S35は、較正用第2スイング角度取得ステップの一例に対応する。ステップS40は、作成ステップの一例に対応する。ステップS12は、平面算出ステップの一例に対応する。ステップS20、S31は、座標変換ステップの一例に対応する。ステップS32は、円算出ステップの一例に対応する。ステップS33は、角度算出ステップの一例に対応する。ステップS36は、誤差算出ステップの一例に対応する。なお、スイング角度較正システムは、トータルステーションTSと、スイング角度センサ67と、姿勢検出コントローラ68と、を含む。
(式23)
例えば、スイング角度センサ67から得られたスイング角度θswがθsw1とθsw2の間の場合、y=a1*x+b1の直線M1(図12参照)を用いて誤差を算出し、得られたスイング角度から誤差を引くことによって、誤差補正を行った後のスイング角度θsw´を得ることができる。なお、ステップS11は、位置情報取得ステップの一例に対応する。ステップS12、S13、S21~S24、S31~S33は、第1スイング角度算出ステップの一例に対応する。ステップS34、S35は、較正用第2スイング角度取得ステップの一例に対応する。ステップS40は、作成ステップの一例に対応する。ステップS12は、平面算出ステップの一例に対応する。ステップS20、S31は、座標変換ステップの一例に対応する。ステップS32は、円算出ステップの一例に対応する。ステップS33は、角度算出ステップの一例に対応する。ステップS36は、誤差算出ステップの一例に対応する。なお、スイング角度較正システムは、トータルステーションTSと、スイング角度センサ67と、姿勢検出コントローラ68と、を含む。
(姿勢検出方法)
次に、本実施形態の姿勢検出方法について説明する。図13は、本実施形態の姿勢検出方法を示すフロー図である。
次に、本実施形態の姿勢検出方法について説明する。図13は、本実施形態の姿勢検出方法を示すフロー図である。
はじめに、ステップS110において、姿勢検出コントローラ68は、GNSSレシーバ61から車体位置データを取得する。
次に、ステップS120において、姿勢検出コントローラ68は、本体IMU63から本体傾斜角データを取得する。
次に、ステップS130において、姿勢検出コントローラ68は、ブームIMU64からブーム傾斜角データを取得する。
次に、ステップS140において、姿勢検出コントローラ68は、アームIMU65からアーム傾斜角データを取得する。
次に、ステップS150において、姿勢検出コントローラ68は、バケットIMU66からバケット傾斜角データを取得する。
次に、ステップS160において、姿勢検出コントローラ68は、スイング角度センサ67からスイング角度データを取得し、スイング角度データからスイング角度θswを算出する。ステップS160は、第2スイング角度取得ステップの一例に対応する。
次に、ステップS170において、姿勢検出コントローラ68は、本体傾斜角データ、ブーム傾斜角データ、アーム傾斜角データ、およびバケット傾斜角データから、ブーム角α1、アーム角α2、およびバケット角α3を算出する。
次に、ステップS180において、姿勢検出コントローラ68は、記憶装置69に記憶されているスイング角度誤差テーブルTに基づいてスイング角度θswにおける誤差eを算出する。姿勢検出コントローラ68は、スイング角度θswから誤差eを差し引くことによって、誤差補正後のスイング角度θsw´を算出する。ステップS180は、補正後第2スイング角度検出ステップの一例に対応する。
次に、ステップS190において、姿勢検出コントローラ68は、車体位置データ、本体傾斜角データ、ブーム角α1、アーム角α2、バケット角α3、誤差補正後のスイング角度、および車両本体2と作業機3との形状データから、バケット23の刃先位置P10を算出する。ステップS190は、姿勢検出ステップの一例に対応する。
次に、ステップS200において、姿勢検出コントローラ68は、現況地形データと、設計地形データと、車両本体2と作業機3との形状データと、検出した刃先位置P10に基づいて、図7に示すガイド画面72を表示入力装置70に表示する。
(特徴等)
本実施形態のスイング角度較正方法では、トータルステーションTSで計測した位置情報から算出したスイング角度θiとスイング角度センサ67の検出値から算出したスイング角度θswiとに基づいてスイング角度誤差テーブルTを作成する。作成したスイング角度誤差テーブルTを用いることで、スイング角度センサの測定誤差や、車体の組立誤差等を較正できるので、スイング角度センサ67によって精度良くスイング角度を検出することができる。
本実施形態のスイング角度較正方法では、トータルステーションTSで計測した位置情報から算出したスイング角度θiとスイング角度センサ67の検出値から算出したスイング角度θswiとに基づいてスイング角度誤差テーブルTを作成する。作成したスイング角度誤差テーブルTを用いることで、スイング角度センサの測定誤差や、車体の組立誤差等を較正できるので、スイング角度センサ67によって精度良くスイング角度を検出することができる。
また、複数のスイング姿勢K1~K7において取得した点p1~p7をスイング平面Uに投影し、スイング座標系においてスイング角度θiを算出することによって、より精度良くスイング角度の真値であるスイング角度θiを算出することができる。真値は、第1スイング角度に対応する。
また、既存の作業機械1に姿勢検出システム60を後付けすることによって、既存の作業機械に対してICT(Information and Communication Technology)施工を可能にすることができる。
(他の実施形態)
以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。
以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。
(A)
上記実施形態では、作業機3の旋回体11に対するスイング角度を検出する角度センサについて較正を行っているが、これに限らなくてもよい。例えば、旋回体11の下部構造体12に対する旋回角度を検出する角度センサに対して本開示のスイング角度較正方法を適用してもよい。また、オフセット式ショベルのスイング角度を検出する角度センサに対して本開示のスイング角度較正方法を適用してもよい。
上記実施形態では、作業機3の旋回体11に対するスイング角度を検出する角度センサについて較正を行っているが、これに限らなくてもよい。例えば、旋回体11の下部構造体12に対する旋回角度を検出する角度センサに対して本開示のスイング角度較正方法を適用してもよい。また、オフセット式ショベルのスイング角度を検出する角度センサに対して本開示のスイング角度較正方法を適用してもよい。
(B)
上記実施形態では、計測装置の一例としてトータルステーションを用いてスイング姿勢K1~K7におけるアームトップの位置の座標を取得しているが、これに限らなくてもよい。計測装置として、例えば、作業現場に配置された複数の既知の座標の基準点にバケット23の刃先を当てて座標を取得してもよい。また、無人航空機(UAV(Unmanned Aerial Vehicle))に搭載した撮像装置で作業機械の姿勢を取得してブームトップの位置座標を画像解析によって取得してもよい。さらに、作業機械に搭載している撮像装置で周辺環境を撮像し、スイングさせた前後の差分に基づいてブームトップの位置座標を取得してもよい。現場に設置された撮像装置で作業機械の姿勢を取得してもよいし、作業機械の測定対象位置にGNSSアンテナを配置し、GNSSアンテナの位置情報に基づいて作業機械の姿勢を取得してもよい。
上記実施形態では、計測装置の一例としてトータルステーションを用いてスイング姿勢K1~K7におけるアームトップの位置の座標を取得しているが、これに限らなくてもよい。計測装置として、例えば、作業現場に配置された複数の既知の座標の基準点にバケット23の刃先を当てて座標を取得してもよい。また、無人航空機(UAV(Unmanned Aerial Vehicle))に搭載した撮像装置で作業機械の姿勢を取得してブームトップの位置座標を画像解析によって取得してもよい。さらに、作業機械に搭載している撮像装置で周辺環境を撮像し、スイングさせた前後の差分に基づいてブームトップの位置座標を取得してもよい。現場に設置された撮像装置で作業機械の姿勢を取得してもよいし、作業機械の測定対象位置にGNSSアンテナを配置し、GNSSアンテナの位置情報に基づいて作業機械の姿勢を取得してもよい。
(C)
上記実施形態では、7つのスイング姿勢におけるアームトップの位置座標を7点取得したが、7点に限らなくてもよく、6点以下、8点以上であってもよい。また、上記(B)で記載した方法によって、7点の位置座標に対して補充を行ってもよい。
上記実施形態では、7つのスイング姿勢におけるアームトップの位置座標を7点取得したが、7点に限らなくてもよく、6点以下、8点以上であってもよい。また、上記(B)で記載した方法によって、7点の位置座標に対して補充を行ってもよい。
(D)
上記実施形態ではブームトップの位置座標を取得しているが、ブームトップに限らなくてもよく、バケット23の所定位置等であってもよく、スイング動作に伴って回転する作業機3のいずれかの位置であればよい。
上記実施形態ではブームトップの位置座標を取得しているが、ブームトップに限らなくてもよく、バケット23の所定位置等であってもよく、スイング動作に伴って回転する作業機3のいずれかの位置であればよい。
(E)
上記実施形態では、スイング角度センサ67から取得したスイング角度と誤差の関係を示すスイング角度誤差テーブルが記憶装置69に記憶されているが、これに限らなくてもよく、例えば、スイング角度センサ67から取得したスイング角度と、トータルステーションTSから取得したスイング角度との関係を示す相関テーブルが記憶装置69に記憶されていてもよい。
上記実施形態では、スイング角度センサ67から取得したスイング角度と誤差の関係を示すスイング角度誤差テーブルが記憶装置69に記憶されているが、これに限らなくてもよく、例えば、スイング角度センサ67から取得したスイング角度と、トータルステーションTSから取得したスイング角度との関係を示す相関テーブルが記憶装置69に記憶されていてもよい。
(F)
上記実施形態では、姿勢検出コントローラ68が、バケット23の刃先位置を検出し、且つスイング角度誤差テーブルTを作成しているが、スイング角度誤差テーブルTの作成は別のコントローラによって行われてもよい。例えば、ステップS11とステップS34のデータの取得ステップは、車載コントローラで実施し、取得したデータを車外コントローラに送信して、車外コントローラでステップS12、S13、S20、S31~S33、S36、およびS40を処理することも可能である。その場合、作成したスイング角度誤差テーブルTを車載コントローラに送信してもよい。
上記実施形態では、姿勢検出コントローラ68が、バケット23の刃先位置を検出し、且つスイング角度誤差テーブルTを作成しているが、スイング角度誤差テーブルTの作成は別のコントローラによって行われてもよい。例えば、ステップS11とステップS34のデータの取得ステップは、車載コントローラで実施し、取得したデータを車外コントローラに送信して、車外コントローラでステップS12、S13、S20、S31~S33、S36、およびS40を処理することも可能である。その場合、作成したスイング角度誤差テーブルTを車載コントローラに送信してもよい。
(G)
上記実施形態では、姿勢検出コントローラ68と本体コントローラ51が分けて設けられているが、1つのコントローラが兼ねていてもよい。
上記実施形態では、姿勢検出コントローラ68と本体コントローラ51が分けて設けられているが、1つのコントローラが兼ねていてもよい。
(H)
上記実施形態では、ガイド画面72を表示入力装置70に表示させているが、作業機械1の入力装置48がタッチパネル等のディスプレイを備えている場合には、入力装置48に表示させてもよい。その場合、通信装置71は設けられていなくてもよい。
上記実施形態では、ガイド画面72を表示入力装置70に表示させているが、作業機械1の入力装置48がタッチパネル等のディスプレイを備えている場合には、入力装置48に表示させてもよい。その場合、通信装置71は設けられていなくてもよい。
(I)
上記実施形態における作業機械1として油圧ショベルを用いて説明したが、油圧ショベルに限らず、機械式ショベル、ロープショベル等の他の機械であってもよい。上記の実施形態に係る作業機械1は、いわゆるバックフォー型のショベルであるが、フェースショベルであってもよい。また、下部構造体は、履帯に限らず、ホイールであってもよい。また、下部構造体は、台船等の船体であってもよい。すなわち、作業機械1は、ブームを含む作業機を備える浚渫船であってもよい。
上記実施形態における作業機械1として油圧ショベルを用いて説明したが、油圧ショベルに限らず、機械式ショベル、ロープショベル等の他の機械であってもよい。上記の実施形態に係る作業機械1は、いわゆるバックフォー型のショベルであるが、フェースショベルであってもよい。また、下部構造体は、履帯に限らず、ホイールであってもよい。また、下部構造体は、台船等の船体であってもよい。すなわち、作業機械1は、ブームを含む作業機を備える浚渫船であってもよい。
本開示によれば、作業機のスイング角度を検出する角度センサで測定する場合に生じる誤差を較正し、精度良くスイング角度を検出することが可能なスイング角度較正方法、姿勢検出方法および姿勢検出システムを提供することができる。
1 :作業機械
2 :車両本体
3 :作業機
11 :旋回体
12 :下部構造体
67 :スイング角度センサ
2 :車両本体
3 :作業機
11 :旋回体
12 :下部構造体
67 :スイング角度センサ
Claims (11)
- 作業機械の作業機のスイング角度を較正するスイング角度較正方法であって、
前記作業機を所定の姿勢に固定した状態で左右方向へのスイングのみを行った複数のスイング姿勢で、計測装置を用いて前記作業機の所定位置の位置情報を取得する位置情報取得ステップと、
前記位置情報に基づいて、各々の前記スイング姿勢における第1スイング角度を算出する第1スイング角度算出ステップと、
前記作業機械に搭載され、前記作業機のスイング角度を検出する角度センサによる前記複数のスイング姿勢での検出値に基づいて、各々の前記スイング姿勢における第2スイング角度を取得する較正用第2スイング角度取得ステップと、
前記複数のスイング姿勢における前記第1スイング角度と前記第2スイング角度に基づいて相関テーブルを作成する作成ステップと、
を備えたスイング角度較正方法。 - 前記相関テーブルは、前記第1スイング角度と前記第2スイング角度の相関を示す、
請求項1に記載のスイング角度較正方法。 - 前記第1スイング角度と前記第2スイング角度に基づいて、前記角度センサの測定誤差を算出する誤差算出ステップを更に備え、
前記相関テーブルは、前記第2スイング角度と前記測定誤差の相関を示す、
請求項1に記載のスイング角度較正方法。 - 前記第1スイング角度算出ステップは、
前記複数のスイング姿勢における前記作業機の前記所定位置の位置情報に基づいて、複数の前記所定位置からの距離が最小となる平面を算出する平面算出ステップと、
前記複数の所定位置を前記平面上に投影した複数の投影位置の座標を前記計測装置の座標系から前記平面の座標系に変換する座標変換ステップと、
前記平面の座標系において、前記複数の投影位置との誤差が最小となる前記平面上の円を算出する円算出ステップと、
各々の前記投影位置の前記円における回転角度を算出し、前記回転角度を前記投影位置に対応する前記スイング姿勢における前記第1スイング角度とする角度算出ステップと、を有する、
請求項1に記載のスイング角度較正方法。 - 作業機械の作業機の姿勢を検出する姿勢検出方法であって、
前記作業機械に搭載され、前記作業機のスイング角度を検出する角度センサの検出値に基づいて第2スイング角度を取得する第2スイング角度取得ステップと、
請求項1~4のいずれかに記載のスイング角度較正方法の前記相関テーブルと前記第2スイング角度に基づいて、誤差補正後の第2スイング角度を検出する補正後第2スイング角度検出ステップと、
検出した前記誤差補正後の第2スイング角度を用いて前記作業機の姿勢を検出する姿勢検出ステップと、
を備えた、姿勢検出方法。 - 前記作業機械は、前記作業機が左右方向にスイング可能に取り付けられた作業機械本体を有し、
前記第2スイング角度取得ステップにおいて、前記角度センサは、前記作業機械本体に対する前記作業機のスイング角度を検出する、
請求項5に記載の姿勢検出方法。 - 前記作業機は、
前記作業機械本体に上下方向に回転可能に取り付けられたブームと、
前記ブームの先端に回転可能に取り付けられたアームと、
前記アームの先端に回転可能に取り付けられたバケットと、を有し、
前記姿勢検出ステップは、前記バケットの刃先位置を検出する、
請求項6に記載の姿勢検出方法。 - 作業機械の作業機のスイング角度を較正するスイング角度較正システムであって、
前記作業機を所定の姿勢に固定した状態で左右方向へのスイングのみを行った複数のスイング姿勢で、前記作業機の所定位置の位置情報を計測する計測装置と、
前記作業機械に搭載され、前記作業機のスイング角度を検出する角度センサと、
前記位置情報に基づいて、各々の前記スイング姿勢における第1スイング角度を算出し、前記角度センサによる前記複数のスイング姿勢での検出値に基づいて、各々の前記スイング姿勢における第2スイング角度を取得し、前記複数のスイング姿勢における前記第1スイング角度と前記第2スイング角度に基づいて相関テーブルを作成するコントローラと、を備えた、
スイング角度較正システム。 - 作業機械の作業機の姿勢を検出する姿勢検出システムであって、
前記作業機械に搭載され、前記作業機のスイング角度を検出する角度センサと、
請求項1~4のいずれかに記載のスイング角度較正方法の前記相関テーブルを記憶する記憶部と、
前記角度センサで検出した前記スイング角度を前記相関テーブルに基づいて補正し、補正後のスイング角度を用いて、前記作業機の姿勢を検出するコントローラと、を備えた、
姿勢検出システム。 - 前記作業機械は、前記作業機が左右方向にスイング可能に取り付けられた作業機械本体を有し、
前記角度センサは、前記作業機械本体に対する前記作業機のスイング角度を検出する、
請求項9に記載の姿勢検出システム。 - 前記作業機は、
前記作業機械本体に上下方向に回転可能に取り付けられたブームと、
前記ブームの先端に回転可能に取り付けられたアームと、
前記アームの先端に回転可能に取り付けられたバケットと、を有し、
前記コントローラは、前記バケットの刃先位置を検出する、
請求項10に記載の姿勢検出システム。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2022-170951 | 2022-10-25 | ||
JP2022170951A JP2024062841A (ja) | 2022-10-25 | 2022-10-25 | スイング角度較正方法、姿勢検出方法、スイング角度較正システムおよび姿勢検出システム |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2024090014A1 true WO2024090014A1 (ja) | 2024-05-02 |
Family
ID=90830611
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/JP2023/031259 WO2024090014A1 (ja) | 2022-10-25 | 2023-08-29 | スイング角度較正方法、姿勢検出方法、スイング角度較正システムおよび姿勢検出システム |
Country Status (2)
Country | Link |
---|---|
JP (1) | JP2024062841A (ja) |
WO (1) | WO2024090014A1 (ja) |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20110311342A1 (en) * | 2007-10-26 | 2011-12-22 | Deere And Company | Three dimensional feature location from an excavator |
WO2015173920A1 (ja) * | 2014-05-14 | 2015-11-19 | 株式会社小松製作所 | 油圧ショベルの較正システム及び較正方法 |
JP2022543745A (ja) * | 2019-08-13 | 2022-10-14 | ユニコントロール エーピーエス | 掘削機のバケットの位置を検出するための、位置検出デバイス及び方法 |
-
2022
- 2022-10-25 JP JP2022170951A patent/JP2024062841A/ja active Pending
-
2023
- 2023-08-29 WO PCT/JP2023/031259 patent/WO2024090014A1/ja unknown
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20110311342A1 (en) * | 2007-10-26 | 2011-12-22 | Deere And Company | Three dimensional feature location from an excavator |
WO2015173920A1 (ja) * | 2014-05-14 | 2015-11-19 | 株式会社小松製作所 | 油圧ショベルの較正システム及び較正方法 |
JP2022543745A (ja) * | 2019-08-13 | 2022-10-14 | ユニコントロール エーピーエス | 掘削機のバケットの位置を検出するための、位置検出デバイス及び方法 |
Also Published As
Publication number | Publication date |
---|---|
JP2024062841A (ja) | 2024-05-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5823046B1 (ja) | 油圧ショベルの較正システム及び較正方法 | |
CN110392754B (zh) | 工程机械 | |
US8838329B2 (en) | Hydraulic shovel calibration system and hydraulic shovel calibration method | |
JP6068730B2 (ja) | 作業機械、及び作業機械の作業機パラメータ補正方法 | |
CN103080427B (zh) | 液压挖掘机的校正装置及液压挖掘机的校正方法 | |
KR101495582B1 (ko) | 유압 셔블의 교정 장치 및 유압 셔블의 교정 방법 | |
WO2015137527A1 (ja) | 作業機械の較正装置、及び作業機械の作業機パラメータの較正方法 | |
JP5841300B1 (ja) | 作業機械の較正装置、及び作業機械の作業機パラメータの較正方法 | |
JP6845810B2 (ja) | 油圧ショベルおよび油圧ショベルの較正方法 | |
KR102479701B1 (ko) | 작업 기계 | |
JP7016297B2 (ja) | 作業機械 | |
WO2022107589A1 (ja) | 情報取得システムおよび情報取得方法 | |
JP2024096491A (ja) | 作業機モーメントを推定する方法 | |
WO2024090014A1 (ja) | スイング角度較正方法、姿勢検出方法、スイング角度較正システムおよび姿勢検出システム | |
JP7229109B2 (ja) | 作業機械および作業機械の制御方法 | |
WO2024057846A1 (ja) | 作業機械における車体座標系を設定するためのシステムおよび方法 | |
CN115867766A (zh) | 校正装置以及校正方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 23882228 Country of ref document: EP Kind code of ref document: A1 |