Patent Application
20230295902
The present invention relates to a system and a method for calibrating the bearing of a work machine.
A work machine is provided with a plurality of positional sensors. A controller of the work machine calculates the bearing of the work machine based on the position of each of the positional sensors. For example, Japanese Laid-open Patent 2003-239328 discloses a bulldozer provided with a plurality of GPS antennas. The controller of the bulldozer acquires the position of each antenna with GPS signals from the GPS antennas. The controller calculates the bearing of the bulldozer from the positional relationships among the plurality of GPS antennas.
An error in the detection value of a positional sensor in a work machine may occur. For example, an error in the detection value of a positional sensor occurs if there is an error in the attachment position of a positional sensor or if the detection values of the positional sensor fluctuate. In this case, an error will arise between the calculated bearing of the work machine and the actual bearing of the work machine in the above work machine. Errors between the calculated bearing and the actual bearing can be calibrated with measurements using an external measurement apparatus, such as a total station. However, the work for calibrating is complicated in this case. An object of the present disclosure is to easily and accurately calibrate an error between a calculated bearing and the actual bearing of a work machine.
A system according to a first aspect of the present disclosure is a system for calibrating the bearing of a work machine calculated based on the positions of a plurality of positional sensors mounted to the work machine. The system according to the present aspect includes a first positional sensor, a second positional sensor, and a controller. The first positional sensor and the second positional sensor are mounted to the work machine. The controller communicates with the first positional sensor and the second positional sensor. The controller acquires first positional data that indicates the position of the first positional sensor and second positional data that indicates the position of the second positional sensor. The controller calculates a first bearing that indicates the bearing of the work machine based on the first positional data and the second positional data. The controller calculates the position of the work machine based on at least one of the first positional data and the second positional data. When a determination condition is satisfied within a predetermined zone, the controller calculates a second bearing that indicates the bearing of the work machine based on a change in the position of the work machine in the predetermined zone. The determination condition includes a travel condition that indicates that the work machine is traveling in a straight line. The controller calculates a correction value of the bearing of the work machine within the predetermined zone based on the difference between the first bearing and the second bearing. The controller corrects the first bearing based on the correction value.
A method according to a second aspect of the present disclosure is a method for calibrating the bearing of the work machine calculated based on the positions of a plurality of positional sensors mounted to the work machine. The plurality of positional sensors includes a first positional sensor and a second positional sensor. The method according to the present aspect includes the following processes. A first process is acquiring first positional data that indicates the position of the first positional sensor, and second positional data that indicates the position of the second positional sensor. A second process is calculating a first bearing that indicates the bearing of the work machine based on the first positional data and the second positional data. A third process is calculating the position of the work machine based on at least one of the first positional data and the second positional data. A fourth process is calculating a second bearing that indicates the bearing of the work machine based on a change in the position of the work machine in a predetermined zone when a determination condition within the predetermined zone is satisfied. The determination condition includes a travel condition that indicates that the work machine is traveling in a straight line. A fifth process is calculating a correction value of the bearing of the work machine based on the difference between the first bearing and the second bearing within the predetermined zone. A sixth process is correcting the first bearing based on the correction value. The order of the execution of the above processes is not limited to the above-mentioned order and may be changed.
According to the system and method as in the present disclosure, the first bearing is calibrated by using a second bearing calculated based on a change in the position of the work machine. The second bearing is acquired without the use an external measurement apparatus while the work machine is traveling. As a result, an error between the calculated bearing and the actual bearing of the work machine can be calibrated easily. In addition, the determination condition includes a travel condition that indicates that the work machine is traveling in a straight line. As a result, the calibration can be performed accurately.
A work machine 1 according to an embodiment is discussed hereinbelow with reference to the drawings.
The vehicle body 11 includes an operating cabin 14 and an engine compartment 15. An operator's seat that is not illustrated is disposed in the operating cabin 14. The engine compartment 15 is disposed in front of the operating cabin 14. The travel device 12 is attached to a bottom part of the vehicle body 11. The travel device 12 includes a left crawler belt 16a and a right crawler belt 16b. The work machine 1 travels due to the rotation of the crawler belts 16a and 16b. The travel of the work machine 1 may be in the form of automated travel, semi-automated travel, or travel based on operations by an operator.
The work implement 13 is attached to the vehicle body 11. The work implement 13 includes a lift frame 17, a blade 18, and a lift cylinder 19. The lift frame 17 is attached to the vehicle body 11 movably up and down about an axis that extends in the vehicle width direction. The lift frame 17 supports the blade 18. The blade 18 is disposed in front of the vehicle body 11. The blade 18 moves up and down accompanying the up and down movements of the lift frame 17. The lift cylinder 19 is coupled to the vehicle body 11 and the lift frame 17. The lift frame 17 moves up and down due to the extension and contraction of the lift cylinder 19.
The power transmission device 24 transmits driving power from the engine 22 to the travel device 12. The power transmission device 24, for example, may be a hydrostatic transmission (HST). Alternatively, the power transmission device 24, for example, may be a transmission including a torque converter or a plurality of speed change gears.
The control system 3 includes a first operating device 25a, a second operating device 25b, an input device 25c, a controller 26, and a control valve 27. The first operating device 25a is a device for operating the travel device 12. The first operating device 25a is disposed in the operating cabin 14. The first operating device 25a receives an operation from the operator for driving the travel device 12 and outputs an operation signal corresponding to the operation. The first operating device 25a includes, for example, an operating lever, a pedal, or a switch and the like.
The first operating device 25a is operable between a forward movement position, a reverse movement position, and a neutral position. The first operating device 25a is operable between a left turn position and a right turn position. An operation signal indicating the position of the first operating device 25a is output to the controller 26. The controller 26 controls the travel device 12 or the power transmission device 24 so that the work machine 1 moves forward when the operating position of the first operating device 25a is the forward movement position. The controller 26 controls the travel device 12 or the power transmission device 24 so that the work machine 1 moves in reverse when the operating position of the first operating device 25a is the reverse movement position.
The controller 26 controls the travel device 12 or the power transmission device 24 so that the work machine 1 turns to the left when the operating position of the first operating device 25a is the left turn position. The controller 26 controls the travel device 12 or the power transmission device 24 so that the work machine 1 turns to the right when the operating position of the first operating device 25a is the right turn position.
The second operating device 25b is a device for operating the work implement 13. The second operating device 25b is disposed in the operating cabin 14. The second operating device 25b receives an operation from the operator for driving the work implement 13 and outputs an operation signal corresponding to the operation. The second operating device 25b includes, for example, an operating lever, a pedal, or a switch and the like.
For example, the second operating device 25b is operable between a raising position, a lowering position, and a neutral position. An operation signal indicating the position of the second operating device 25b is output to the controller 26. The controller 26 controls the control valve 27 to raise the work implement 13 when the operating position of the second operating device 25b is the raising position. The controller 26 controls the control valve 27 to lower the work implement 13 when the operating position of the second operating device 25b is the lowering position.
The input device 25c is, for example, touch screen-type input device. The input device 25 may be an input device such as a switch. The operator can use the input device 25c to input a setting for controlling the work machine 1.
The controller 26 is programmed to control the work machine 1 based on acquired data. The controller 26 includes a storage device 28 and a processor 30. The processor 30 includes, for example, a CPU. The storage device 28 may include a memory, such as a RAM or a ROM, for example. The storage device 28 may include an auxiliary recording medium, such as a semiconductor memory or a hard disk and the like. The storage device 28 is an example of a non-transitory computer-readable recording medium. The storage device 28 records computer commands that are executable by the processor 30 and that are for controlling the work machine 1.
The controller 26 acquires operation signals from the first operating device 25a and the second operating device 25b. The controller 26 controls the control valve 27 based on the operation signals. The controller 26 is not limited to one component and may be divided into a plurality of controllers.
The control valve 27 is a proportional control valve and is controlled with command signals from the controller 26. The control valve 27 is disposed between the hydraulic pump 23 and hydraulic actuators, such as the lift cylinder 19. The control valve 27 controls the flow rate of the hydraulic fluid supplied from the hydraulic pump 23 to the lift cylinder 19. The controller 26 generates a command signal for the control valve 27 so that the blade 18 moves in accordance with the above-mentioned operation of the second operating device 25b. As a result, the lift cylinder 19 is controlled in response to the operation amount of the second operating device 25b. The control valve 27 may also be a pressure proportional control valve. Alternatively, the control valve 27 may be an electromagnetic proportional control valve.
The control system 3 includes a lift angle sensor 29. The lift angle sensor 29 detects a lift angle of the blade 18. The lift angle is the angle of the work implement 13 from the origin position of the work implement 13. The origin position of the work implement 13 is, for example, the position of the blade 18 while the blade tip of the blade 18 is in contact with the ground surface on a horizontal ground surface. The lift angle sensor 29 may detect the stroke length of the lift cylinder 19. The controller 26 may calculate the lift angle of the blade 18 based on the lift cylinder length. Alternatively, the lift angle sensor 29 may detect the lift angle of the blade 18 directly.
As illustrated in
The first positional sensor 31 includes a first receiver 34 and a first antenna 35. The first receiver 34 receives a positioning signal from a satellite, computes the position of the first antenna 35 from the positioning signal, and generates the first positional data. The position of the first positional sensor 31 signifies the position of the first antenna 35. The second positional sensor 32 includes a second receiver 36 and a second antenna 37. The second receiver 36 receives a positioning signal from a satellite, computes the position of the second antenna 37 from the positioning signal, and generates the second positional data. The position of the second positional sensor 32 signifies the position of the second antenna 37.
The controller 26 acquires the first positional data from the first positional sensor 31. The controller 26 acquires the second positional data from the second positional sensor 32. The first positional data indicates the position of the first positional sensor 31 in a global coordinate system. The second positional data indicates the position of the second positional sensor 32 in the global coordinate system.
The attitude sensor 33 is, for example, an inertial measurement unit (IMU). The attitude sensor 33 outputs inclination angle data of the vehicle body 11. The inclination angle data of the vehicle body 11 includes the pitch angle and the roll angle of the vehicle body 11. That is, the attitude sensor 33 is a pitch angle sensor for detecting the pitch angle of the vehicle body 11 and a roll angle sensor for detecting the roll angle of the vehicle body 11. The pitch angle is an angle in the front-back direction of the vehicle body 11 with respect to the horizontal direction. The roll angle is an angle in the lateral direction of the vehicle body 11 with respect to the horizontal direction. The controller 26 acquires the inclination angle data from the attitude sensor 33.
The controller 26 computes a blade tip position P0 of the blade 18 from the first positional data, the inclination angle data, and the lift angle of the blade 18. The controller 26 calculates local coordinates of the blade tip position P0 based on the lift angle and vehicle body dimension data. The local coordinate system is a coordinate system based on the vehicle body 11. The global coordinate system is an external coordinate system of the vehicle body 11. The vehicle body dimension data indicates a positional relationship between a point of origin position of the vehicle body 11 and the blade tip position P0 in the local coordinate system. The vehicle body dimension data indicates a positional relationship between the point of origin position of the vehicle body 11 and the first positional sensor 31 in the local coordinate system. The vehicle body dimension data indicates a positional relationship between the point of origin position of the vehicle body 11 and the second positional sensor 32 in the local coordinate system. The vehicle body dimension data indicates a positional relationship between the first positional sensor 31 and the second positional sensor 32 in the local coordinate system. The vehicle body dimension data is stored in the storage device 28.
The controller 26 calculates the global coordinates of the blade tip position P0 based on the global coordinates of the first positional sensor 31, the local coordinates of the blade tip position P0, the inclination angle data, and the vehicle body dimension data. The controller 26 calculates a first bearing that indicates the bearing of the vehicle body 11 in the global coordinates, from the global coordinates of the first positional sensor 31 and the global coordinates of the second positional sensor 32. Specifically, the controller 26 determines the direction from the position of the second positional sensor 32 toward the position of the first positional sensor 31 as the first bearing.
The storage device 28 stores work site data. The work site data indicates the actual topography of the work site. The work site data is, for example, a topographical survey map of the work site in a three-dimensional data format.
The controller 26 acquires actual topography data. The actual topography data indicates the actual topography 50 of the work site.
The actual topography data is information that indicates the topography located in the traveling direction of the work machine 1. The actual topography data is acquired by means of calculation by the controller 26 from the work site data, the current position of the work machine 12, and the first bearing. The actual topography data indicates the actual topography 50 from the current position of the work machine 1 as far as a predetermined distance dn in the first bearing. Specifically, the actual topography data includes heights Z0 to Zn of the actual topography 50 at a plurality of reference points from the current position of the work machine 1 as far as a predetermined distance dn in the first bearing. In the present embodiment, the current position is a position defined based on the current blade tip position Pb of the work machine 1. However, the current position may also be defined based on the current position of another portion of the work machine 1. The plurality of reference points are aligned with a predetermined interval, for example 1 m, between each point.
The controller 26 automatically controls the work implement 13 based on the actual topography data, the blade tip position P0, and the first bearing. The automatic control of the work implement 13 may be a semi-automatic control that is performed in accompaniment to manual operations by the operator. Alternatively, the automatic control of the work implement 13 may be a fully automatic control that is performed without manual operations by an operator.
The controller 26 determines a target design topography 70 based on the actual topography 50. For example, as illustrated in
Next, processing for calibrating the first bearing calculated based on the position of the first positional sensor 31 and the position of the second positional sensor 32 will be explained.
In step S101, the controller 26 acquires machine state data. The machine state data includes the travel velocity, the roll angle, the pitch angle, a turning operating state, the work implement position, and the work implement operating state of the work machine 1. The travel velocity is calculated from a change in the positions of the first positional sensor 31 and/or the second positional sensor 32. Alternatively, the travel velocity may be calculated from the rotation speed of the power transmission device 24 or the travel device 12. The roll angle and the pitch angle are acquired from the above-mentioned inclination angle data of the vehicle body 11.
The turning operating state indicates the presence of a turning operation by the first operating device 25a. The turning operating state is acquired based on an operation signal from the first operating device 25a. Alternatively, the turning operating state may be acquired based on a signal from a sensor, such as an IMU, that detects the attitude of the work machine 1. The work implement position indicates the height position of the blade. The work implement position may be acquired based on a signal from the lift angle sensor 29. The work implement operating state indicates whether the work implement 13 is being operated. The work implement operating state is acquired based on an operation signal from the second operating device 25b. The controller 26 repeatedly acquires and records the machine state data over a predetermined sample cycle.
In step S102, the controller 26 acquires machine position and bearing data. The machine position and bearing data includes the position and the first bearing of the work machine 1. The position of the work machine 1 is acquired based on the position of the first positional sensor 31 and/or the position of the second positional sensor 32. The position of the work machine 1 is a position defined from at least one of the first positional sensor 31 and the second positional sensor 32. For example, the position of the work machine 1 may be the position of the first positional sensor 31. Alternatively, the position of the work machine 1 may be the position of the second positional sensor 32. Alternatively, the position of the work machine 1 may be a position between the first positional sensor 31 and the second positional sensor 32. The first bearing is calculated based on the position of the first positional sensor 31 and the position of the second positional sensor 32. The controller 26 repeatedly acquires and records the machine position and bearing data over a predetermined sample cycle.
In step S103, the controller 26 determines whether the determination conditions are continuously satisfied within a predetermined zone. The predetermined zone may be defined by the movement distance of the work machine 1. Alternatively, the predetermined zone may be defined by the movement time of the work machine 1. The determination conditions include travel conditions and non-working conditions. The travel conditions signify that the work machine 1 is traveling in a straight line on flat ground. The non-working conditions signify that the work machine 1 is not performing work with the work implement.
The controller 26 determines whether the travel conditions are satisfied from the machine state data and the machine position and bearing data. The controller 26 determines whether the non-working conditions are satisfied from the machine state data. The controller 26 determines that the determination conditions are satisfied when both the travel conditions and the non-working conditions are satisfied.
Specifically, the travel conditions include the following first to seventh travel conditions. The first travel condition is that the travel velocity of the work machine 1 is greater than or equal to a predetermined velocity threshold. The second travel condition is that the size of change of the first bearing is less than or equal to a first threshold. The third travel condition is that the work machine 1 is not turning. The fourth travel condition is that the size of a change of the roll angle of the work machine 1 is less than or equal to a second threshold.
The fifth travel condition is that the size of a change of the pitch angle of the work machine 1 is less than or equal to a third threshold. The sixth travel condition is that the roll angle of the work machine 1 is less than or equal to a fourth threshold. The seventh travel condition is that the pitch angle of the work machine 1 is less than or equal to a fifth threshold. The controller 26 determines that the travel conditions are satisfied when all of the first to seventh travel conditions are continuously satisfied within the predetermined zone. The afore-mentioned thresholds are set to values appropriate for calibrating the first bearing with accuracy.
The non-working conditions include a first non-working condition and a second non-working condition as indicated below. The first non-working condition is that the second operating device 25b is in a non-operating state. The second non-working condition is that the height position of the work implement is at least a predetermined height or higher. The controller 26 determines that the non-working conditions are satisfied when all of the first to second non-working conditions are continuously satisfied within the predetermined zone.
When the controller 26 has determined that the determination conditions are continuously satisfied within the predetermined zone, the processing advances to step S104. In step S104, the controller 26 calculates the first bearing and a second bearing within the predetermined zone.
The controller 26 calculates the second bearing that indicates the bearing of the work machine 1 based on a change in the position of the work machine 1 within the predetermined zone. Specifically, the controller 26 calculates the bearing from a starting position P1 to an ending position P2 within the predetermined zone as the second bearing. The controller 26 calculates a second bearing Wpos using the following equation (1).
Wpos=a tan 2(Eg−Es,Ng−Ns) (1)
The global coordinates of the starting position P1 are (Es, Ns, Zs). The global coordinates of the ending position P2 are (Eg, Ng, Zg). The starting position P1 may be an average value of the position of the work machine 1 from the starting time t-T until after a predetermined time period. The ending position P2 may be an average value of the position of the work machine 1 from the ending time t until before the predetermined time period.
In step S105, the controller 26 calculates a correction value of the bearing of the work machine 1. The controller 26 saves the correction value in the storage device 28. The controller calculates the correction value of the bearing of the work machine 1 based on the difference between the first bearing Wnav and the second bearing Wpos within the predetermined zone. Specifically, the controller 26 calculates the correction value Dwl using the following equation (2).
Dwl=Wnav−Wpos
In step S106, the controller 26 calculates an average value of the correction values. The controller 26 repeatedly executes the processing from step S101 to step S107 illustrated in
In step S107, the controller 26 updates and saves the average value of the correction values as correction values for correcting the first bearing. Thereafter, when determining the first bearing, the controller 26 uses the updated correction value to correct the first bearing calculated based on the position of the first positional sensor 31 and the position of the second positional sensor 32.
According to the work machine 1 as in the present embodiment as discussed above, the first bearing is calibrated by using a second bearing calculated based on a change in the position of the work machine 1. The second bearing is acquired without the use of an external measurement apparatus while the work machine is traveling. As a result, an error between the calculated bearing and the actual bearing of the work machine 1 can be calibrated easily. In addition, the determination conditions include travel conditions that indicate that the work machine 1 is traveling in a straight line. As a result, the calibration can be performed accurately. Furthermore, by using an average value of the correction values, errors due to fluctuation over time of the detection values of the sensors 31 and 32 can be accurately calibrated.
Although one embodiment of the present invention has been described so far, the present invention is not limited to the above embodiment and various modifications may be made within the scope of the invention. The work machine 1 is not limited to a bulldozer, and may be another type of machine, such as a wheel loader, a motor grader, a dump truck, or the like.
The work machine 1 may be a vehicle that can be remotely operated. In this case, a portion of the control system 3 may be disposed outside of the work machine 1. For example, the controller 26 may be disposed outside the work machine 1. The controller 26 may be disposed inside a control center separated from the work site.
The first operating device 25a, the second operating device 25b, and the input device 25c may be disposed outside of the work machine 1. In this case, the operating cabin may be omitted from the work machine 1. The first operating device 25a, the second operating device 25b, and the input device 25c may be omitted from the work machine 1. The work machine 1 may be operated with only the automatic control by the controller 26 without operations via the first operating device 25a or the second operating device 25b.
The number of sensors is not limited to two and may be three or more. The dispositions of the positional sensors are not limited to those of the above embodiment and may be changed. For example, the first positional sensor 31 and the second positional sensor 32 may be aligned in the width direction of the work machine 1. The positional sensors are not limited to sensors of a positioning system based on the Earth. The positional sensors may be sensors of a positioning system based on a specific region, such as a work site.
The order of the processing by the controller 26 is not limited to that of the above embodiment and may be changed. The processing by the controller 26 is not limited to the above embodiment and may be changed. The processing for performing automatic control of the work implement 13 in accordance with the target design topography 70 may be omitted. The controller 26 may cause the work machine 1 to travel automatically to a destination based on the position and the first bearing of the work machine 1.
The above-mentioned processing for calculating the correction values may be executed constantly without specific instructions from an operator during the normal work of the work machine 1. Alternatively, the processing for calculating the correction values may be executed when a calibration instruction is inputted by an operator using a device such as the input device 25c. The processing for calculating the correction values may be executed when the positions of the positional sensors are determined to be correct. The processing for calculating the correction values may be executed when the machine state data and the machine position and bearing data are determined to be suitable.
The determination conditions are not limited to those of the above embodiment and may be added to, omitted, or changed. For example, the travel conditions are not limited to those of the above embodiment and may be added to, omitted, or changed. The non-working conditions are not limited to those of the above embodiment and may be added to, omitted, or changed.
According to the present disclosure, calibrating an error between a calculated bearing and the actual bearing of a work machine can be performed easily and accurately.
| Number | Date | Country | Kind |
|---|---|---|---|
| 2020-109977 | Jun 2020 | JP | national |
This application is a U.S. National stage application of International Application No. PCT/JP2021/019774, filed on May 25, 2021. This U.S. National stage application claims priority under 35 U.S.C. § 119(a) to Japanese Patent Application No. 2020-109977, filed in Japan on Jun. 25, 2020, the entire contents of which are hereby incorporated herein by reference.
| Filing Document | Filing Date | Country | Kind |
|---|---|---|---|
| PCT/JP2021/019774 | 5/25/2021 | WO |
