Work machine system, control method, and controller

Abstract

The processor of a work machine system acquires an inclination angle of a work machine with respect to a traveling direction of the work machine, the inclination angle being calculated on the basis of a normal positioning signal which is a position of the work machine at a first time point, and determines the position of the work machine on the basis of the inclination angle and a vehicle speed detected by a vehicle speed sensor at a second time point.

Description

Machinery system, control method, and controller

[0001] This disclosure relates to a work machine system, a control method, and a controller.

[0002] Japanese Patent Publication No. 2019-173371 discloses a control system that enables a work vehicle to perform work efficiently and with high-quality finishes through automatic control. The controller of the control system disclosed in Patent Document 1 acquires current terrain data indicating the current terrain of the work site, determines a target depth, acquires the positions of multiple section points located on the current terrain based on the current terrain data, determines multiple reference points by displacing each of the multiple section points vertically by the target depth, determines a target design terrain based on the multiple reference points, and generates a command signal to operate the work machine according to the target design terrain.

[0003] Incidentally, a work machine such as the one disclosed in Japanese Patent Publication No. 2019-173371 may be equipped with a sensor consisting of a GNSS (Global Navigation Satellite System) receiver and antenna (hereinafter simply referred to as "GNSS sensor") and other sensors. In such a case, the position of the work machine, etc. (for example, the position, orientation, and speed of the work machine) is determined based on the sensor values ​​acquired by each of these sensors.

[0004] However, there may be cases where the position of the work machine cannot be determined by the GNSS sensor, or where the accuracy of the position of the work machine obtained by the GNSS sensor is low. In such cases, it is conceivable that the position of the work machine may be estimated based on sensor values ​​(e.g., acceleration and angular velocity).

[0005] However, if the position of the work machine is continuously estimated based on sensor values, errors may accumulate in the estimated position. Therefore, if the position of the work machine is continuously estimated using sensor values, the accuracy of the estimation may decrease. For example, the work machine may travel while tilted relative to the direction of travel. In such cases, the position of the work machine estimated using sensor values ​​does not take into account the tilt of the work machine relative to the direction of travel, which may reduce the accuracy of the estimation of the work machine's position.

[0006] This disclosure has been made in view of the above points, and aims to provide a work machine system, control method, and controller that can accurately estimate the position of a work machine even when the work machine is traveling while tilted relative to the direction of travel.

[0007] To achieve the above objective, a first aspect of the present disclosure is a work machine system comprising: a work machine including a work implement and a travel device; a vehicle speed sensor that acquires the vehicle speed based on the drive of the travel device; a positioning sensor that acquires a positioning signal which is the position of the work machine; and a controller including a processor, wherein the processor acquires the inclination angle of the work machine with respect to the direction of travel of the work machine, calculated based on a normal positioning signal at a first time point, and determines the position of the work machine based on the inclination angle and the vehicle speed detected by the vehicle speed sensor at a second time point.

[0008] According to the work machine system, control method, and controller described herein, the position of the work machine can be accurately estimated even when the work machine is traveling while tilted relative to the direction of travel.

[0009] This is a schematic diagram of the work machine according to the embodiment. This is a schematic diagram of the work machine system according to the embodiment. This is a block diagram showing the hardware configuration of the controller. This is a diagram for explaining the automated construction of this embodiment. This is a diagram for explaining an environment in which radio waves from satellites are blocked. This is a diagram for explaining mode transitions related to the position estimation of the work machine. This is a diagram for explaining the case in which the vehicle body is traveling while tilted with respect to the direction of travel. This is a diagram for explaining the method for calculating the tilt angle of the vehicle body with respect to the direction of travel. This is a flowchart showing the flow of the tilt angle calculation process. This is a flowchart showing the flow of the control process.

[0010] Hereinafter, an example of an embodiment of this disclosure will be described with reference to the drawings. In each drawing, identical or equivalent components and parts are given the same reference numerals. Also, the dimensions and proportions in the drawings are exaggerated for illustrative purposes and may differ from actual proportions.

[0011] <Embodiment> Figure 1 is a schematic diagram of the work machine 1 according to this embodiment. The work machine 1 according to this embodiment is a bulldozer. As shown in Figure 1, the work machine 1 comprises a vehicle body 14, a work machine 16, and a travel device 18. In this embodiment, the case in which the work machine 1 is a bulldozer is used as an example, but it is not limited to this. For example, the work machine 1 may be a hydraulic excavator, a wheel loader, or a motor grader, etc.

[0012] The vehicle body 14 has an engine compartment 14A and a driver's cab 14B. The engine compartment 14A is located in front of the driver's cab 14B. A driver's seat (not shown) is located in the driver's cab 14B. The running gear 18 is attached to the lower part of the vehicle body 14. The running gear 18 has a pair of left and right tracks 18B and a pair of left and right sprockets 18A. Note that in Figure 1, only the left track 18B and the left sprocket 18A of the vehicle body 14 are shown. The rotation of the sprocket 18A causes the track 18B to rotate, and the work machine 1 moves.

[0013] The implement 16 is attached to the vehicle body 14. The implement 16 has a lift frame 16A, a blade 16B, and a lift cylinder 16C. The lift frame 16A is attached to the vehicle body 14 so as to be movable up and down. The lift frame 16A supports the blade 16B. The blade 16B included in the implement 16 may be other attachments, and the attachment may be a ripper or the like, which is provided not only at the front of the machine but also at the rear.

[0014] The blade 16B is positioned at the front of the vehicle body 14. The blade 16B moves up and down in conjunction with the vertical movement of the lift frame 16A. The lift frame 16A may be attached to the traveling device 18. The lift cylinder 16C is connected to the vehicle body 14 and the lift frame 16A. The lift frame 16A operates as the lift cylinder 16C extends and retracts.

[0015] Figure 2 is a block diagram showing the configuration of the work machine system 10 according to this embodiment. As shown in Figure 2, the work machine system 10 comprises a work machine 1, a vehicle body 14, a work machine 16, and a traveling device 18.

[0016] The work machine system 10 is equipped with a drive system (not shown) for driving the work machine 1. The drive system (not shown) includes, for example, an engine (not shown), a hydraulic pump (not shown), and a power transmission device (not shown). The hydraulic pump (not shown) is driven by an engine (not shown), which is an example of a prime mover, and discharges hydraulic fluid. The prime mover may be, for example, an electric motor. The hydraulic fluid discharged from the hydraulic pump (not shown) is supplied to each lift cylinder (not shown) via a control valve (not shown). Based on the operation command for the work machine 16, the opening of the control valve (not shown) is adjusted, and the operating speed of each lift cylinder (not shown) is adjusted. The power transmission device (not shown) transmits the driving force of the engine (not shown) to the travel device (not shown). The power transmission device (not shown) may be, for example, an HST (Hydro Static Transmission). Alternatively, the power transmission device (not shown) may be, for example, a torque converter or a transmission with multiple gears. The power transmission device (not shown) may also be, for example, a diesel-electric system. Furthermore, the driving force of the work implement 16 may be electric rather than hydraulic.

[0017] As shown in Figure 2, the control system 12 that can be mounted on the vehicle body 14 includes an operating device 20A, an input device 20B, a GNSS sensor 22 which is an example of a positioning sensor, an IMU 24, a vehicle speed sensor 26, a blade tip position detection sensor 28, and a controller 30.

[0018] The operating device 20A and the input device 20B are located in the driver's cab 14B. The operating device 20A is a device for operating the work implement 16 and the travel device 18. The operating device 20A is located in the driver's cab 14B. The operating device 20A receives operations from the operator to drive the work implement 16 and the travel device 18 and outputs an operation signal corresponding to the operation. The operating device 20A includes, for example, an operating lever, a pedal, and a switch. The operating device 20A and the input device 20B do not necessarily have to be located in the driver's cab 14B. If the operating device 20A and the input device 20B are not located in the driver's cab 14B, the work implement 16 and the travel device 18 are controlled by remote control or autonomous control.

[0019] The input device 20B is a device for setting the automatic control of the work machine 1, which will be described later. The input device 20B receives operations from the operator and outputs operation signals corresponding to the operations. The operation signals from the input device 20B are output to the controller 30. The input device 20B includes, for example, a touch panel display. However, the input device 20B is not limited to a touch panel and may also include hardware keys. For example, the touch panel display may show information about the work site based on the location information of the work machine 1, information about the terrain of the construction target at the work site, information about the height relationship of the blade between the work machine 1 and the construction target, and information about the current control mode of the work machine 1.

[0020] The GNSS sensor 22 is composed of, for example, a GPS (Global Positioning System) receiver (not shown), a main first GNSS antenna 22A, and a sub second GNSS antenna 22B. As shown in Figure 1, for example, the first GNSS antenna 22A and the second GNSS antenna 22B are arranged on the driver's cab 14B. The receiver (not shown) of the GNSS sensor 22 receives positioning signals from satellites, calculates the position and speed of the first GNSS antenna 22A based on the positioning signals to acquire position and speed data of the vehicle body 14, and acquires azimuth data of the vehicle body 14 based on the positioning signals of the first GNSS antenna 22A and the second GNSS antenna 22B. The GNSS sensor 22 is an example of a positioning sensor.

[0021] The controller 30, described later, acquires position data, speed data, and orientation data of the vehicle body 14 obtained by the GNSS sensor 22. The controller 30 also acquires roll angle, pitch angle, and yaw angle, which represent the attitude of the vehicle body 14, based on the position data, speed data, and orientation data of the vehicle body 14.

[0022] The IMU 24 acquires the acceleration and angular velocity of the vehicle body 14 of the work machine 1. Based on the acceleration and angular velocity of the vehicle body 14 acquired by the IMU 24, the controller 30 acquires the roll angle, pitch angle, and yaw angle, which represent the attitude of the vehicle body 14. The IMU 24 is an example of an attitude sensor.

[0023] The vehicle speed sensor 26 acquires the vehicle speed of the vehicle body 14 based on the drive of the running gear 18. Specifically, the vehicle speed sensor 26 is a sensor that detects the rotational speed of the sprocket 18A. Therefore, the vehicle speed of the vehicle body 14 corresponding to the rotational speed of the track 18B in accordance with the rotation of the sprocket 18A is acquired. The vehicle speed sensor 26 is installed inside the left and right sprockets 18A. In this embodiment, the speed obtained from the average of the rotational speeds of the left and right tracks 18B is used as the vehicle speed of the vehicle body 14.

[0024] The blade tip position detection sensor 28 is a sensor for detecting the position of the blade tip of the blade 16B. For example, the blade tip position detection sensor 28 is installed on the lift cylinder 16C and detects the position of the blade tip of the blade 16B by detecting the degree of extension and retraction of the lift cylinder 16C. Alternatively, the blade tip position detection sensor 28 is a sensor that detects the position of the blade tip of the blade 16B based on sensor values ​​acquired by an IMU installed on the blade 16B. Alternatively, the blade tip position detection sensor 28 may be a sensor that detects the position of the blade tip of the blade 16B based on sensor values ​​acquired by an optical sensor (e.g., a Lidar) provided on the work machine 1.

[0025] The controller 30 is programmed to control the work machine 1 based on the acquired data. The controller 30 includes a processor, such as a CPU (Central Processing Unit), as will be described later. The controller 30 acquires operation signals from the operating device 20A and the input device 20B. Note that the controller 30 is not limited to a single unit, but may be divided into multiple controllers. If the controller 30 is divided into multiple controllers, some of its functions may be distributed and located outside the driver's cab 14B. The controller 30 moves the vehicle body 14 by controlling the running device 18 or the power transmission device (not shown). The controller 30 moves the blade 16B up and down by controlling the control valve (not shown).

[0026] Figure 3 is a block diagram showing the hardware configuration of the controller 30. As shown in Figure 3, the controller 30 includes a CPU (Central Processing Unit) 101, a memory 102, a storage device 103, an input / output interface (I / F) 104, a storage medium reader 105, and a communication interface 106. Each component is connected to the others via a bus 107 so that they can communicate with each other.

[0027] The storage device 103 stores a control program for executing control processing, which will be described later. The CPU 101 is a central processing unit that executes various programs and controls each component. Specifically, the CPU 101 reads a program from the storage device 103 and executes the program using memory 102 as a workspace. The CPU 101 controls each component and performs various calculations according to the program stored in the storage device 103.

[0028] Memory 102 is composed of RAM (Random Access Memory) and temporarily stores programs and data as a working area. Storage device 103 is composed of ROM (Read Only Memory), HDD (Hard Disk Drive), SSD (Solid State Drive), etc., and stores various programs including the operating system and various data.

[0029] The input / output interface 104 is an interface for inputting and outputting data to and from external devices. In this embodiment, the following external devices are connected to the controller 30 via the input / output interface 104: the operating device 20A, the input device 20B, the GNSS sensor 22, the IMU 24, the vehicle speed sensor 26, and the blade tip position detection sensor 28.

[0030] The storage medium reader 105 reads data stored on various storage media such as CD (Compact Disc)-ROM, DVD (Digital Versatile Disc)-ROM, Blu-ray disc, and USB (Universal Serial Bus) memory, and writes data to the storage media.

[0031] The communication interface 106 is an interface for communicating with other devices, and standards such as Ethernet®, FDDI, or Wi-Fi® can be used. The above-mentioned external devices may also be connected to the controller 30 via the communication interface 106.

[0032] The controller 30 automatically controls the work machine 1 based on current terrain data representing the current terrain of the construction site, target design terrain data representing the target terrain of the construction site, and blade tip position data representing the position of the blade tip of the blade 16B. The automatic control of the work machine 1 may be semi-automatic control performed in conjunction with manual operation by an operator. Alternatively, the automatic control of the work machine 1 may be fully automatic control performed without manual operation by an operator. The movement of the work machine 1 may be automatically controlled by the controller 30. For example, the movement control of the work machine 1 may be fully automatic control performed without manual operation by an operator. Alternatively, the movement control may be semi-automatic control performed in conjunction with manual operation by an operator. Alternatively, the movement of the work machine 1 may be performed by manual operation by an operator.

[0033] Furthermore, the controller 30 calculates the position, speed, and attitude of the vehicle body 14 of the work machine 1 using a known Kalman filter and a known inertial navigation calculation. The calculated position, speed, and attitude of the vehicle body 14 are used to control the work machine 1. When the automatic construction mode is set, the controller 30 performs various controls.

[0034] [1. Automatic Construction by Work Machine 1] Figure 4 is a diagram illustrating the automatic construction of this embodiment. As shown in Figure 4, for example, the controller 30 controls the cutting edge of the blade 16B of the work machine 1a to make contact with the design surface T. The controller 30 then controls the work machine 1b to perform excavation and leveling work. For example, if the load on the blade 16B of the work machine 1c in Figure 4 increases, the controller performs control such as controlling the position of the blade 16B. This ensures that stable automatic excavation and leveling continue, as shown by the work machine 1d in Figure 4. As shown in Figure 4, in order to construct the existing terrain into a terrain like the design surface T, the existing terrain is repeatedly constructed into a smooth surface N as shown in Figure 4, until it approaches the final design surface T.

[0035] Furthermore, when performing automated construction as shown in Figure 4, it is necessary to accurately estimate the position of the work machine 1. However, in environments where radio waves from satellites are blocked, the accuracy of the position estimation of the work machine 1 obtained by the GNSS sensor 22 deteriorates, which may make it difficult to continue automated construction.

[0036] Figure 5 illustrates an environment in which radio waves from a satellite are blocked. As shown in Figure 5, consider a case where the work machine 1 performs automatic construction based on its position acquired by the GNSS sensor 22. In such a case, if the positioning signal from satellite St is blocked by an obstruction SH as shown in Figure 5, the accuracy of estimating the position and attitude of the work machine 1 deteriorates. Then, as shown in Figure 5, when the positioning signal from satellite St becomes receivable, the work machine 1 performs automatic construction based on its position acquired by the GNSS sensor 22.

[0037] Therefore, in this embodiment, if the data acquired by the GNSS sensor 22 (hereinafter also referred to as "GNSS data") deteriorates, the GNSS data is discarded. The GNSS data includes the position data, speed data, and orientation data of the vehicle body 14.

[0038] For example, a determination is made as to whether the GNSS data is degraded based on the difference between the altitude of the GNSS antenna 22A calculated from each data acquired by the IMU 24 and the altitude of the GNSS antenna 22A obtained from the position data included in the GNSS data. The altitude of the GNSS antenna 22A is in the field coordinate system O Ｇ This represents the position coordinates in the Z direction.

[0039] For example, the controller 30 calculates the difference between the altitude calculated by the controller 30 and the altitude calculated by the GNSS sensor 22 at each time point within a predetermined time interval. If the variance of this difference is greater than a predetermined threshold, the controller determines that the GNSS data is degraded and discards the GNSS data. After discarding the GNSS data, the controller 30 estimates the position of the work machine 1 using the vehicle speed mode described later. As described later, in the vehicle speed mode, the position, speed, and attitude (hereinafter sometimes simply referred to as "position, etc.") of the work machine 1 are estimated based on the vehicle speed of the work machine 1 acquired by the vehicle speed sensor 26 and the acceleration and angular velocity of the work machine 1 acquired by the IMU 24.

[0040] [2. Modes related to position estimation of the work machine 1] Figure 6 is a diagram illustrating the mode transitions related to position estimation of the work machine 1 in this embodiment. As shown in Figure 6, in this embodiment, the position of the work machine 1 during automated construction is estimated by switching between initialization mode M0, GNSS_FIX mode M1-1, GNSS_NON-FIX mode M1-2, and vehicle speed mode M2.

[0041] Initialization mode M0 is the mode set when the work machine 1 starts driving. GNSS_FIX mode M1-1 is a mode in which GNSS data can be acquired by the GNSS sensor 22, and the GNSS data is highly accurate. GNSS_NON-FIX mode M1-2 is a mode in which GNSS data can be acquired by the GNSS sensor 22, but the GNSS data is less accurate than the GNSS data acquired in GNSS_FIX mode M1-1, or does not output a valid position. Vehicle speed mode M2 ​​is a mode in which the position of the vehicle body 14 is estimated based on the vehicle speed of the vehicle body 14 acquired by the vehicle speed sensor 26, and the acceleration and angular velocity of the vehicle body 14 acquired by the IMU 24.

[0042] As shown in Figure 6, for example, when the mode of the work machine 1 is in initialization mode M0, if the initialization process after the work machine 1 starts driving is completed, the mode of the work machine 1 transitions from initialization mode M0 to GNSS_FIX mode M1-1. Also, as shown in Figure 6, for example, when the mode of the work machine 1 is in GNSS_FIX mode M1-1, if an instruction signal is issued indicating that the estimation of the position of the work machine 1 has ended, the mode of the work machine 1 transitions from GNSS_FIX mode M1-1 to initialization mode M0.

[0043] Furthermore, as shown in Figure 6, for example, when the mode of the work machine 1 is GNSS_FIX mode M1-1, if the valid conditions for the vehicle speed mode are met and the invalid conditions for GNSS data are met, the mode of the work machine 1 transitions from GNSS_FIX mode M1-1 to vehicle speed mode M2. Also, as shown in Figure 6, for example, when the mode of the work machine 1 is GNSS_FIX mode M1-1, if the valid conditions for the vehicle speed mode are not met and the invalid conditions for GNSS data are met, the mode of the work machine 1 transitions from GNSS_FIX mode M1-1 to GNSS_NON-FIX mode M1-2.

[0044] Furthermore, as shown in Figure 6, for example, when the mode of the work machine 1 is in vehicle speed mode M2, if the conditions related to GNSS data are met, the mode of the work machine 1 transitions from vehicle speed mode M2 ​​to GNSS_FIX mode M1-1. Also, as shown in Figure 6, for example, when the mode of the work machine 1 is in vehicle speed mode M2, if the conditions related to the vehicle speed mode are no longer met, the mode of the work machine 1 transitions from vehicle speed mode M0 to GNSS_NON-FIX mode M1-2. Also, as shown in Figure 6, for example, when the mode of the work machine 1 is in vehicle speed mode M2, if an instruction signal is issued to terminate the estimation of the position and attitude of the work machine 1, the mode of the work machine 1 transitions from vehicle speed mode M2 ​​to initialization mode M0.

[0045] Furthermore, as shown in Figure 6, for example, when the mode of the work machine 1 is GNSS_NON-FIX mode M1-2, if the conditions related to GNSS data are met, the mode of the work machine 1 transitions from GNSS_NON-FIX mode M1-2 to GNSS_FIX mode M1-1. Also, as shown in Figure 6, for example, when the mode of the work machine 1 is GNSS_NON-FIX mode M1-2, if an instruction signal is issued to terminate the estimation of the position and orientation of the work machine 1, the mode of the work machine 1 transitions from GNSS_NON-FIX mode M1-2 to initialization mode M0.

[0046] For example, if the vehicle speed mode conditions representing conditions 1 to 6 below are met, and the specific conditions set for each other mode are also met, the mode of the work machine 1 will transition from the other mode to vehicle speed mode M2.

[0047] 1. The detected value obtained by the IMU 24 and the detected value obtained by the vehicle speed sensor 26 are consistent. 2. The work is low-load work (for example, the rotation of the work machine 1 is less than a predetermined value). 3. The driving conditions are appropriate.

[0048] The vehicle speed mode M2 ​​includes a first vehicle speed mode and a second vehicle speed mode. The first vehicle speed mode is a temporary mode that immediately transitions to the GNSS_FIX mode when the degradation of the GNSS data is restored. For example, if the surrounding environment of the location where the work machine 1 is located is under a bridge or under a highway, the degradation of the GNSS data is temporary. Therefore, anticipating the degradation of the GNSS data in such an environment, in the first vehicle speed mode, when the degradation of the GNSS data is restored, it immediately transitions to the GNSS_FIX mode.

[0049] On the other hand, the second vehicle speed mode is a vehicle mode that continues the vehicle speed mode even if the degradation of GNSS data recovers. For example, if the surrounding environment of the location where the work machine 1 is located is near a wall or a tree, the degradation of GNSS data will continue. In such an environment, even if the degradation of GNSS data recovers, there is a possibility that the GNSS data will degrade again. Therefore, anticipating the degradation of GNSS data in such an environment, the second vehicle speed mode is a mode that continues the vehicle speed mode without immediately transitioning to GNSS_FIX mode even if the degradation of GNSS data recovers.

[0050] The mode to which the vehicle speed mode transitions from another mode to either the first or second vehicle speed mode is set, for example, by the operator of the work machine 1. Alternatively, the mode to which the vehicle speed mode transitions from another mode to either the first or second vehicle speed mode is determined based on the degree of degradation of the GNSS data. For example, as described above, if the variance of the difference between the altitude of the GNSS antenna 22A calculated from each data acquired by the IMU 24 and the altitude of the GNSS antenna 22A obtained from the position data included in the GNSS data is greater than the first threshold, the mode of the work machine 1 transitions to the first vehicle speed mode. On the other hand, for example, if the variance of the difference is greater than a predetermined second threshold, the mode of the work machine 1 transitions to the second vehicle speed mode. The first threshold is greater than the second threshold.

[0051] [3. Inclination Angle of the Work Machine 1 with Respect to the Direction of Travel] Figures 7(A) and 7(B) illustrate the case where the work machine 1 is traveling while inclined with respect to the direction of travel. As shown in Figures 7(A) and 7(B), the actual work machine 1 may be traveling with the front part of the vehicle body 14 lifted off the ground. The state in which the front part of the vehicle body 14 is lifted off the ground can also be described as the vehicle body 14 being wheelie. In addition, although not shown in Figures 7(A) and 7(B), the actual work machine 1 may be traveling with the rear part of the vehicle body 14 lifted off the ground. Under such circumstances, if the GNSS data deteriorates, the mode of the work machine 1 transitions from GNSS_FIX mode to vehicle speed mode, and the position of the work machine 1 is estimated in this vehicle speed mode. However, even if the position of the vehicle body 14 is estimated based on the vehicle speed acquired by the vehicle speed sensor 26 while the vehicle body 14 is traveling while inclined, errors may occur.

[0052] In this embodiment, the position of the work machine 1 is estimated using a known Kalman filter and a known inertial navigation calculation. Therefore, when estimating the position of the vehicle body 14, it is necessary to input a velocity vector in the three-dimensional direction on the vehicle body coordinate system, with the vehicle body 14 as the reference, to the Kalman filter.

[0053] However, the vehicle speed V of the work machine 1 according to the present embodiment is a scalar value corresponding to the rotation of the sprocket 18A. Therefore, when the vehicle body 14 is traveling in the traveling direction D1 as shown in FIG. 7(A), the vehicle body coordinate system O Ｌ , the vehicle speed V is set with respect to the X-axis direction of the vehicle body coordinate system O Ｌ and the speed in the Y-axis direction of the vehicle body coordinate system O Ｌ is set to 0, whereby the position and the like of the vehicle body 14 are calculated by the Kalman filter.

[0054] However, as shown in FIG. 7(A), when the actual vehicle body 14 is traveling while being inclined with respect to the traveling direction D1 of the vehicle body 14, the actual vehicle speed V of the work machine 1 is in the vehicle body coordinate system O Ｌ of X Ｌ axis direction, but in the X Ｇ axis direction of the on-site coordinate system O Ｇ . Therefore, when the vehicle speed V of the vehicle body 14 is set as the speed generated in the X axis direction of the vehicle body coordinate system O Ｌ of X Ｌ axis direction to estimate the position and the like of the vehicle body 14, the estimation accuracy deteriorates. Specifically, when estimating the position of the vehicle body 14 using the Kalman filter, if it is assumed that the vehicle speed V of the vehicle body 14 is generated in the X axis direction of the vehicle body coordinate system in the figure, the position of the vehicle body 14 estimated by the Kalman filter will be the position in the P1 direction in FIG. 7(A), and a position different from the actual position of the vehicle body 14 will be estimated. For this reason, the position of the vehicle body 14 estimated by the Kalman filter will gradually deviate from the true position.

[0055] Therefore, in the present embodiment, the inclination angle θ of the vehicle body 14 (specifically, the X Ｌ axis direction of the vehicle body coordinate system) with respect to the traveling direction D1 of the vehicle body 14 ｗ is calculated. Then, the vehicle speed v acquired by the vehicle speed sensor 26 is vector-decomposed according to the inclination angle θ ｗ of the vehicle body 14 with respect to the traveling direction D1 of the working implement 16. Specifically, as shown in FIG. 7(B), the vehicle speed v acquired by the vehicle speed sensor 26 is converted into the vehicle body coordinate system O Ｌ of X Ｌ axis direction velocity vector v​​ｘ 'and vehicle coordinate system O Ｌ Z Ｌ Axial velocity vector v ｚ It is decomposed into '.Then the controller 30 obtains the velocity vector v by vector decomposition of the vehicle speed v. Ｘ ',v Ｚ By inputting this into the Kalman filter, the position of the work implement 16 is estimated. As a result, the position of the vehicle body 14 estimated by the Kalman filter changes in the direction of P2 in Figure 7(B), and the actual position of the vehicle body 14 is estimated. Therefore, even when the vehicle body 14 is traveling while tilted relative to the direction of travel, the position of the vehicle body 14 can be estimated with high accuracy.

[0056] In this embodiment, under conditions where high-precision estimation of position, etc., is possible by the GNSS sensor 22, the inclination angle θ of the vehicle body 14 with respect to the direction of travel is determined. ｗ The controller 30 calculates the inclination angle θ of the vehicle body 14 with respect to the direction of travel when the mode of the work machine 1 is GNSS_FIX mode. ｗ The controller calculates the inclination angle θ calculated while the GNSS_FIX mode was running. Then, when the mode of the work machine 1 transitions from GNSS_FIX mode to vehicle speed mode, the controller 30 calculates the inclination angle θ calculated while the GNSS_FIX mode was running. ｗ Based on this, the vehicle speed v obtained by the vehicle speed sensor 26 is assigned to the vehicle body coordinate system O Ｌ X Ｌ Axial velocity vector v Ｘ 'and vehicle coordinate system O Ｌ Z Ｌ Directional velocity vector v Ｚ It is decomposed into '. Then, the controller 30 controls the velocity vector v Ｘ ',v Ｚ By inputting ' into the Kalman filter, the position of the work machine 16 is estimated. As mentioned above, the work machine 1 is in a forward-moving state, and the average slewing angular velocity of the work machine 1 is the first set value TH related to the slewing angular velocity. Ｒ，１ If it is less than, the vehicle transitions from other modes to vehicle speed mode, so the vehicle coordinate system O Ｌ Y Ｌ Axial velocity vector v Ｙ' is considered zero.

[0057] Note that the inclination angle θ of this embodiment ｗ This is the angle representing the inclination of the vehicle body 14 with respect to the direction of travel of the vehicle body 14, and is in the vehicle body coordinate system O Ｌ This is different from the pitch angle that represents the inclination of the vehicle body 14.

[0058] [4. Calculation conditions for the inclination angle of the work machine 1 with respect to the direction of travel] The following conditions are the calculation conditions for the inclination angle of the vehicle body 14 with respect to the direction of travel (hereinafter simply referred to as "inclination angle calculation conditions"). The inclination angle θ is determined when the inclination angle calculation conditions representing the following conditions 1 and 2 are met. ｗ The calculation begins.

[0059] 1. The aforementioned vehicle speed mode conditions are met. 2. The work is a low-load operation.

[0060] Note that condition 2 of the inclination angle calculation conditions relates to the turning of the work machine 1 and is the same as condition 2 of the vehicle speed mode conditions. However, condition 2 of the inclination angle calculation conditions determines more strictly than condition 2 of the vehicle speed mode conditions whether or not turning motion is occurring.

[0061] [5. Method for calculating the inclination angle of the work machine 1 with respect to the direction of travel] (5-1. Inclination angle calculation) Figure 8 is a diagram illustrating the method for calculating the inclination angle of the vehicle body 14 with respect to the direction of travel. Consider the case where the mode of the work machine 1 is GNSS_FIX mode and automatic leveling work between leveling areas is started by the work machine 1. In this case, as shown in Figure 8, the inclination angle θ ｗ This is calculated based on the altitude H1 and altitude H2 of the work machine 1 when the work machine 1 has traveled a distance L from the starting point S for leveling.

[0062] The altitude H1 of the work machine 1 is in the field coordinate system O Ｇ The vertical direction (Z) of the vehicle speed v in the Z region. Ｇ Velocity component v in the axial direction ｚ The field coordinate system O obtained by integrating Ｇ The vertical direction (Z) of the work machine 1 in ＧThis is the position in the axial direction. Therefore, the altitude H1 of the work machine 1 is the altitude based on the vehicle speed v obtained by the vehicle speed sensor 26.

[0063] On the other hand, the altitude H2 of the work machine 1 is obtained based on data obtained by the GNSS sensor 22 in GNSS_FIX mode, in the field coordinate system O Ｇ The vertical direction (Z) of the work machine 1 in Ｇ This is the position in the axial direction. Therefore, the altitude H2 of the work machine 1 is the altitude based on the data acquired by the GNSS sensor 22.

[0064] Altitude H1, Altitude H2, Error, and Inclination Angle θ ｗ The relationship is as shown in Figure 8. As shown in Figure 8, the work machine 1 is traveling while tilted with respect to the direction of travel, with an inclination angle θ. ｗ If this occurs, the vertical velocity component v of the vehicle speed v ｚ Errors accumulate in the altitude H1 obtained by integrating. Therefore, as the travel distance of the work machine 1 increases, the vertical velocity component v of the vehicle speed v ｚ The difference between the altitude H1 obtained by integrating and the altitude H2 calculated from the GNSS data acquired by the GNSS sensor 22 increases.

[0065] Specifically, as shown in Figure 8, the vertical velocity component v of the vehicle speed v when the work machine 1 travels a distance L1. ｚ The difference between the altitude H1-1 obtained by integrating and the altitude H2-1 calculated from the GNSS data acquired by the GNSS sensor 22 is a1. On the other hand, the difference between altitude H1-2 and altitude 2-2 when the work machine 1 travels a distance L2 which is longer than distance L1 is a2 (> a1). Note that in GNSS_FIX mode, the GNSS data acquired by the GNSS sensor 22 is highly accurate, so the position data among the GNSS data obtained at that time can be said to be the true value of the work machine 1.

[0066] In this embodiment, this point is utilized, and the inclination angle θ is calculated by the arc tangent of the error between the travel distance of the work machine 1 and the altitude. ｗThis calculates the inclination angle θ of this embodiment. Specifically, the inclination angle θ of this embodiment. ｗ It is calculated according to the following formula.

[0067]

[0068] Here, v in the above equation ｘ The field coordinate system O Ｇ X of vehicle speed v in Ｇ This is the component in the axial direction, v ｙ The field coordinate system O Ｇ Y of vehicle speed v in Ｇ This is the component in the axial direction, v ｚ The field coordinate system O Ｇ Vehicle speed v in Z Ｇ This is the component in the axial direction. C ｂ ｌ This is a rotation matrix for transforming from the vehicle coordinate system to the field coordinate system. α is the roll angle of the vehicle body 14, β is the pitch angle of the vehicle body 14, and γ is the yaw angle of the vehicle body 14. Note that the roll angle α, pitch angle β, and yaw angle γ are calculated from GNSS data obtained by the GNSS sensor 22 in GNSS_FIX mode. Also, Xt, Yt, and Zt are calculated from GNSS data obtained by the GNSS sensor 22 in GNSS_FIX mode in the field coordinate system O Ｇ This is the three-dimensional position at time t.

[0069] Therefore, in this embodiment, when the above-described slope angle calculation conditions are met, the travel distance L of the work machine 1 and the vertical component v of the vehicle speed are calculated starting from the leveling start point S. ｚ Based on the change in altitude H1 obtained by integrating (calculated value including error) and the change in altitude H2 of the work machine 1 calculated when the mode of the work machine 1 is GNSS_FIX mode (reference value), the inclination angle θ ｗ The following is calculated. The travel distance L of the work machine 1 is given by the Σ in the above formula. ｔ ΔL ｔ This corresponds to the vertical component of the vehicle speed v. ｚ The altitude H1 obtained by integrating is the Σ in the above equation. ｔ v ｚ This corresponds to Δt. Also, the altitude H2 of the work machine 1 calculated when the mode of the work machine 1 is GNSS_FIX mode is Σ in the above formula.ｔ (Z ｔ -Z ｔ－１ ) corresponds to.

[0070] It should be noted that the inclination angle θ ｗ is calculated for each leveling section ｗ where the calculation conditions for the inclination angle θ are satisfied. When the calculation conditions for the inclination angle θ ｗ are no longer satisfied, the calculation of the inclination angle θ ｗ in the corresponding leveling section is stopped.

[0071] (5-2. Confirmation of Travel Distance After Leveling) The change in altitude H2 calculated in the GNSS_FIX mode may also contain certain noise. For this reason, by causing the work machine 1 to travel a certain travel distance, it becomes possible to reduce errors occurring in the calculated value of the inclination angle θ ｗ with respect to the noise. Therefore, in the present embodiment, when the calculation conditions for the inclination angle θ ｗ are satisfied, the calculation of the inclination angle θ ｗ is started after the work machine 1 has traveled a predetermined travel distance.

[0072] (5-3. Limit Processing) After the inclination angle θ ｗ is calculated, limit processing is executed. Specifically, when the inclination angle θ calculated at each time ｗ is not a value within a preset range, the inclination angle θ ｗ is rejected. Specifically, an upper limit value and a lower limit value for the inclination angle θ ｗ are preset, and when the calculated inclination angle θ ｗ is not within the range between the upper limit value and the lower limit value, the inclination angle θ ｗ is rejected.

[0073] (5-4. Smoothing Processing) After the inclination angle θ ｗ is calculated, smoothing processing is executed. Specifically, smoothing of the inclination angle θ ｗ is performed by, for example, simply averaging the inclination angles θ calculated at respective times ｗ .

[0074] Hereinafter, the calculation of the inclination angle θ by the controller 30 based on the above and the inclination angle θ ｗ ​ｗ The use thereof will be described.

[0075] The controller 30 acquires an inclination angle of the vehicle body 14 relative to the traveling direction of the vehicle body 14, which is calculated based on a normal positioning signal at a first time point (for example, a time point within a time period in which the GNSS_FIX mode continues), and determines the position of the vehicle body 14 based on the inclination angle and a vehicle speed detected by the vehicle speed sensor 26 at a second time point (for example, a time point within a time period in which the vehicle speed mode continues). In other words, the controller 30 corrects the vehicle speed v detected by the vehicle speed sensor 26 based on the vehicle speed v detected by the vehicle speed sensor 26 and the inclination angle θ of the vehicle body 14 relative to the traveling direction of the vehicle body 14 calculated based on a positioning signal from a satellite ｗ . Specifically, as shown in FIG. 7B, the controller 30 corrects the vehicle speed v detected by the vehicle speed sensor 26 into a corrected vehicle speed v ｘ ', v ｚ ' by decomposing the vehicle speed v. Then, the controller 30 estimates the position and the like of the vehicle body 14 using a known Kalman filter based on the corrected vehicle speeds v ｘ ', v ｚ ' and the acceleration and angular velocity of the vehicle body 14 acquired by the IMU 24.

[0076] As described above, the calculation of the inclination angle θ ｗ is executed when the mode of the working machine 1 is the GNSS_FIX mode indicating that the accuracy of a positioning signal acquired by the GNSS sensor 22 is equal to or higher than a predetermined accuracy. The GNSS_FIX mode is an example of the first mode of the present disclosure. Specifically, the controller 30 calculates the inclination angle θ of the vehicle body 14 relative to the traveling direction of the vehicle body 14 based on the vehicle speed v detected by the vehicle speed sensor 26 and the position of the vehicle body 14 estimated from the positioning signal acquired by the GNSS sensor 22 ｗ .

[0077] Then, when the mode of the working machine 1 is the vehicle speed mode indicating that the accuracy of a signal acquired by the GNSS sensor 22 is less than a predetermined accuracy, the controller 30 uses the inclination angle θ calculated while the GNSS_FIX mode continues ｗBased on this, the vehicle speed v detected by the vehicle speed sensor is corrected. The vehicle speed mode is an example of the second mode of this disclosure.

[0078] As described above, the inclination angle θ of this embodiment ｗ This is the tilt angle calculated based on the difference between altitude H2, which is an example of the distance traveled by the vehicle body 14 estimated from the positioning signal acquired by the GNSS sensor 22, and altitude H1, which is an example of the distance traveled by the vehicle body 14 estimated from the vehicle speed acquired by the vehicle speed sensor 26.

[0079] Note that the inclination angle θ ｗ This is the tilt angle calculated based on the vehicle speed v acquired by the vehicle speed sensor 26 when the vehicle body 14 is in a predetermined state. Specifically, when the vehicle body 14 is in a state that satisfies the above-mentioned vehicle speed mode transition conditions and tilt angle calculation conditions, the controller 30 calculates the tilt angle θ based on the vehicle speed v acquired by the vehicle speed sensor 26. ｗ The inclination angle θ is calculated. Alternatively, for example, when the working machine 1 is in a low-slip state, or when the driving state is different from the driving state included in each of the above conditions, ｗ You may also perform the calculation.

[0080] Also, the inclination angle θ ｗ This is the inclination angle calculated based on the vehicle speed v acquired by the vehicle speed sensor 26 after the vehicle body 14 is in a predetermined state and has traveled a predetermined distance or more. Specifically, as described above, the controller 30 calculates the inclination angle θ based on the vehicle speed v acquired by the vehicle speed sensor 26 after the work machine 1 has started leveling the ground and has traveled a predetermined distance. ｗ Calculate (see (5-2. Confirmation of distance traveled after ground leveling) above).

[0081] More specifically, the controller 30 controls the inclination angle θ according to the above formula. ｗSpecifically, when the mode of the work machine 1 is GNSS_FIX mode, the controller 30 calculates the inclination angle θ of the vehicle body 14 relative to the direction of travel of the vehicle body 14, based on the vehicle speed v detected by the vehicle speed sensor 26, the position of the vehicle body (Xt, Yt, Zt) estimated from the positioning signal acquired by the GNSS sensor 22, and the attitude angles of the vehicle body 14 (roll angle α, pitch angle β, yaw angle γ) estimated from the positioning signal acquired by the GNSS sensor 22, according to the above formula. ｗ Calculate.

[0082] In this case, first, the controller 30 calculates a rotation matrix C from the positioning signal acquired by the GNSS sensor 22 and the attitude angles of the vehicle body 14 (roll angle α, pitch angle β, yaw angle γ) estimated from the IMU 24. ｂ ｌ Based on the vehicle speed v obtained by the vehicle speed sensor 26, the field coordinate system O Ｇ The vertical velocity component v ｚ Calculate.

[0083] Next, the controller 30 processes the speed component v of the vehicle speed v ｚ Based on this, the field coordinate system O Ｇ The altitude H1, which is an example of the first vertical travel distance of the vehicle body 14, is calculated.

[0084] Furthermore, the controller 30 uses the positioning signal acquired by the GNSS sensor 22 and the position (Xt, Yt, Zt) of the vehicle body 14 estimated from the IMU 24 to determine the field coordinate system O Ｇ The controller 30 calculates the altitude H2, which is an example of the second vertical travel distance of the vehicle body 14. Then, based on altitudes H1 and H2, the controller 30 calculates the inclination angle θ of the vehicle body 14 with respect to the direction of travel according to the above formula. ｗ Calculate.

[0085] The inclination angle θ of the vehicle body 14 with respect to the direction of travel ｗ After the calculation is performed, when the mode of the work machine 1 switches from GNSS_FIX mode to vehicle speed mode, the controller 30 calculates the inclination angle θ that was calculated in GNSS_FIX mode. ｗBased on this, the vehicle speed v detected by the vehicle speed sensor 26 is corrected. The scenario in which switching from GNSS_FIX mode to vehicle speed mode is assumed to occur when the work machine 1 is temporarily unable to acquire a high-precision positioning signal while leveling the ground surface. In this scenario, the work machine 1 is leveling the ground surface, and the inclination of the vehicle body 14 in vehicle speed mode is assumed to be the same as the inclination of the vehicle body 14 in the preceding GNSS_FIX mode. Therefore, in this embodiment, the inclination angle θ of the vehicle body 14 calculated in GNSS_FIX mode ｗ This is used to correct the speed v of the work machine 1 performing leveling work in vehicle mode.

[0086] Specifically, the controller 30 receives the vehicle speed v detected by the vehicle speed sensor 26 and the tilt angle θ. ｗ Based on this, the vehicle speed v detected by the vehicle speed sensor 26 is assigned to the vehicle body coordinate system O Ｌ Velocity vectors v in each direction at ｘ ',v ｚ By applying a correction to decompose into ', the vehicle coordinate system O Ｌ Velocity vectors v in each direction at ｘ ',v ｚ Obtain '. Note that in this embodiment, the vehicle body coordinate system O Ｌ Y in Ｌ Axial velocity vector v ｙ ' is set to 0.

[0087] The controller 30 then controls the velocity vectors v in each direction of the vehicle body 14. ｘ ',v ｚ Based on this, the position of the vehicle body 14 is estimated. Specifically, the controller 30 estimates the velocity vector v in each direction of the vehicle body 14. ｘ ',v ｚ By inputting this into a known Kalman filter, the position, speed, and attitude of the vehicle body 14 are estimated.

[0088] Next, the operation of the work machine system 10 according to the embodiment will be described. When the work machine 1 is performing automatic construction and the mode of the work machine 1 is GNSS_FIX mode, the inclination angle calculation processing routine shown in Figure 9 is repeatedly executed. The inclination angle calculation processing routine shown in Figure 9 is repeatedly executed, for example, at a period of 100 Hz.

[0089] In step S100, the CPU 101 of the controller 30 determines whether the above-mentioned inclination angle calculation conditions are met. If the inclination angle calculation conditions are met, the process proceeds to step S102. If the inclination angle calculation conditions are not met, the process terminates.

[0090] In step S102, the CPU 101 of the controller 30 performs the above-described process, thereby changing the inclination angle θ of the vehicle body 14. ｗ Calculate.

[0091] In step S104, the CPU 101 of the controller 30 determines whether the distance traveled by the work machine 1 after it started leveling work is greater than a predetermined distance. If the distance traveled by the work machine 1 after it started leveling work is greater than the predetermined distance, the process proceeds to step S106. If the distance traveled by the work machine 1 after it started leveling work is less than or equal to the predetermined distance, the process ends.

[0092] In step S106, the CPU 101 of the controller 30 calculates the inclination angle θ calculated in step S102. ｗ It is determined whether the value is within a predetermined range. ｗ If the value is within a predetermined range, the process proceeds to step S108. Meanwhile, the inclination angle θ ｗ If the value is not within the specified range, the process will terminate.

[0093] In step S108, the CPU 101 of the controller 30 calculates the inclination angle θ obtained in the previous calculation. ｗ The data is read from the storage device 103. Then, the CPU 101 of the controller 30 reads the tilt angle θ obtained from the previous calculation. ｗ And the inclination angle θ obtained in step S102 of this calculation. ｗBy performing a smoothing process based on this (e.g., simple averaging), the new slope angle θ after smoothing is obtained. ｗ This is obtained. Then, the CPU 101 of the controller 30 determines the new inclination angle θ after smoothing. ｗ The data is stored in the memory device 103.

[0094] When the mode of the work machine 1 switches from GNSS_FIX mode to vehicle speed mode, the control processing routine shown in Figure 10 is repeatedly executed.

[0095] In step S200, the CPU 101 of the controller 30 calculates the inclination angle θ when the mode of the work machine 1 is GNSS_FIX mode. ｗ Read the data from the storage device 103.

[0096] In step S202, the CPU 101 of the controller 30 uses the tilt angle θ obtained in step S200. ｗ Based on this, the vehicle speed v acquired by the vehicle speed sensor 26 is corrected. Specifically, the CPU 101 of the controller 30 adjusts the tilt angle θ ｗ Based on this, the vehicle speed v is the velocity vector v in the vehicle coordinate system. ｘ ',v ｚ A correction is made that breaks it down into '.

[0097] In step S204, the CPU 101 of the controller 30 processes the velocity vector v obtained by correction in step S202. ｘ ',v ｚ Based on this, the position, velocity, and attitude of the vehicle body 14 are estimated using a known Kalman filter and a known inertial navigation calculation.

[0098] In step S206, the CPU 101 of the controller 30 outputs the position, speed, and attitude of the vehicle body 14 obtained in step S204 as results.

[0099] The position, speed, and attitude of the vehicle body 14 output in step S206 are used to control the work machine 1.

[0100] As described above, the work machine system according to the embodiment includes a work machine including a work machine and a vehicle body, a vehicle speed sensor that acquires the vehicle speed of the vehicle body, an attitude sensor that acquires the acceleration and angular velocity of the vehicle body, and a controller including a processor. A CPU, which is an example of a processor in the work machine system, acquires the inclination angle of the vehicle body 14 with respect to the direction of travel of the vehicle body 14, calculated based on a normal positioning signal at a first time point (for example, a time point within a time interval in which the GNSS_FIX mode is in effect), and determines the position of the vehicle body 14 based on the inclination angle and the vehicle speed detected by the vehicle speed sensor 26 at a second time point (for example, a time point within a time interval in which the vehicle speed mode is in effect). A normal positioning signal is a high-precision positioning state or a positioning state equivalent thereto that can guarantee the position accuracy of the vehicle body 14, and is, for example, a positioning signal obtained within a time interval in which the GNSS_FIX mode of this embodiment is in effect. In other words, a CPU, which is an example of a processor in a work machine system, corrects the vehicle speed detected by the vehicle speed sensor based on the vehicle speed detected by the vehicle speed sensor and the vehicle's tilt angle relative to the direction of travel, which is calculated based on positioning signals from satellites. Then, the CPU of the work machine system estimates the vehicle's position and attitude based on the corrected vehicle speed and the acceleration and angular velocity acquired by an IMU, which is an example of an attitude sensor. This allows for accurate estimation of the work machine's position even when the work machine is traveling while tilted relative to the direction of travel.

[0101] Furthermore, the inclination angle calculated in this embodiment is the inclination angle calculated based on the vehicle speed acquired by the vehicle speed sensor when the vehicle's driving state is in a predetermined state. By calculating the inclination angle when the vehicle's driving state is stable (for example, when the above-mentioned vehicle speed mode conditions and inclination angle calculation conditions are met), an accurate inclination angle can be obtained under stable conditions.

[0102] Furthermore, the tilt angle calculated in this embodiment is the tilt angle calculated based on the vehicle speed acquired by the vehicle speed sensor after the vehicle's driving state is in a predetermined state and the vehicle's travel distance has exceeded a predetermined distance. By calculating the tilt angle when the vehicle's travel distance exceeds a predetermined distance, a highly accurate (reduced noise) tilt angle can be obtained. Specifically, when the vehicle's travel distance is short, various pieces of information contain noise; therefore, by calculating the tilt angle after the vehicle has traveled a predetermined distance or more, the influence of noise can be reduced.

[0103] Furthermore, the tilt angle is calculated based on the difference between the vehicle's travel distance estimated from the positioning signal acquired by the GNSS sensor and the vehicle's travel distance estimated from the vehicle speed acquired by the vehicle speed sensor. Specifically, the controller calculates the vertical velocity component v of the vehicle speed v acquired by the vehicle speed sensor in the field coordinate system, which is a coordinate system based on the ground surface. ｚ Based on this, H1, an example of the first vertical travel distance of the vehicle body in the field coordinate system, is calculated. Based on the vehicle body position estimated from the GNSS sensor and IMU, altitude H2, an example of the second vertical travel distance of the vehicle body in the field coordinate system, is calculated. Based on altitude H1 and altitude H2, the inclination angle of the vehicle body relative to the direction of travel is calculated. Since altitude H1, calculated from data obtained by the vehicle speed sensor, and altitude H2, calculated from GNSS data obtained by the GNSS sensor and acceleration and angular velocity acquired by the IMU, contain the information necessary to calculate the inclination angle of the vehicle body, it is possible to calculate the inclination angle of the vehicle body based on these two pieces of information.

[0104] Furthermore, in the GNSS_FIX mode, which indicates that the accuracy of the signal acquired by the GNSS sensor is above a predetermined accuracy, the CPU of the work machine system calculates the vehicle's tilt angle relative to the direction of travel based on the vehicle speed detected by the vehicle speed sensor and the vehicle's position estimated from the positioning signal acquired by the GNSS sensor. In addition, in the vehicle speed mode, where the accuracy of the signal acquired by the GNSS sensor is below a predetermined accuracy, the processor of the work machine system corrects the vehicle speed detected by the vehicle speed sensor based on the tilt angle calculated while the GNSS mode is in effect. By using the tilt angle calculated while the GNSS_FIX mode, which provides accurate position information, is in effect, the vehicle speed of the sprocket detected by the vehicle speed sensor can be corrected with high accuracy.

[0105] Furthermore, the CPU of the work machine system calculates the tilt angle of the vehicle body relative to the direction of travel based on the vehicle speed detected by the vehicle speed sensor and the position and attitude of the vehicle body estimated from the positioning signal acquired by the GNSS sensor and the IMU. The processor of the work machine system also calculates the velocity vectors in each direction in the vehicle body coordinate system by correcting the vehicle speed detected by the vehicle speed sensor to decompose it into velocity vectors in each direction in the vehicle body coordinate system, which is a coordinate system based on the vehicle body, based on the vehicle speed and tilt angle detected by the vehicle speed sensor, and estimates the position of the vehicle body based on the velocity vectors in each direction of the vehicle body. This makes it possible to obtain velocity vectors in each direction in the vehicle body coordinate system that can be used when estimating the position of the vehicle body, etc., using known Kalman filters and known inertial navigation calculations.

[0106] As described above, the speed sensor used in this embodiment is a sensor that detects the rotation of the sprocket. Therefore, the speed v obtained by the vehicle speed sensor is a scalar value that measures the speed only in the direction of travel of the vehicle body, and is not three-dimensional vehicle body speed information. In this case, the working machine is moving in the front-rear direction (X in the field coordinate system). Ｇ When traveling in the axial direction, the vehicle speed v obtained by the vehicle speed sensor is used in the X coordinate system of the vehicle body coordinate system. Ｌ Set as the axis, Y coordinate system of the vehicle body coordinate system ＬAxis and Z coordinate system of the vehicle body Ｌ By assuming that no axis velocity is generated (for example, the velocity is set to 0), the vehicle speed v can be used as three-dimensional velocity information in the controller's calculations of position, etc. However, as mentioned above, the vehicle may travel while tilted relative to the ground due to the vehicle's center of gravity or the load on the work equipment. In this case, as mentioned above, the vehicle speed v is used as the X coordinate system of the vehicle. Ｌ Set as the axis, Y coordinate system of the vehicle body coordinate system Ｌ Axis and Z coordinate system of the vehicle body Ｌ If the axis velocity is set to 0, the calculated position of the vehicle will diverge from the ground, mainly increasing in the vertical direction. Therefore, the vehicle speed v is set to the vehicle axis (X in the vehicle coordinate system). Ｌ Axis and Z Ｌ In order to correctly decompose the vehicle speed in the direction of the vehicle axis (X coordinate system), Ｌ It is necessary to estimate the inclination angle, which is the inclination information between the axis and the running surface.

[0107] Therefore, in this embodiment, the position of the vehicle body calculated by the controller (or position data included in the GNSS data) and the X coordinate system of the vehicle body coordinate system are used. Ｌ The change in position is calculated by integrating the vertical velocity of the vehicle body using the velocity vector obtained by vector decomposition of the vehicle speed (scalar) assumed to be occurring on the axis. The distance traveled is then calculated using the position calculated by the controller. The inclination angle of the vehicle body relative to the direction of travel is then calculated using the difference between the two types of position changes and the distance traveled, with respect to the direction of travel calculated using the inverse trigonometric function of the tangent. In this embodiment, in order to calculate the inclination angle accurately, the vehicle speed state or straight-line state is checked, and the process of calculating the inclination angle is executed only when an accurate inclination angle can be obtained. Also, since the denominator of the inverse trigonometric function is the distance traveled, errors occur when the distance traveled is short. For this reason, in order to improve the accuracy of the inclination angle, a process is provided to adopt the value when the vehicle body has traveled a certain distance. As a result, according to this embodiment, the position of the work machine can be estimated accurately even when the work machine is traveling while tilted relative to the direction of travel.

[0108] This disclosure is not limited to the embodiments and examples described above, and various modifications and applications are possible without departing from the gist of this disclosure.

[0109] For example, the program processing flow described in the above embodiment is just one example, and unnecessary steps may be deleted, new steps added, or the processing order rearranged, as long as it does not deviate from the main purpose.

[0110] Furthermore, although the above embodiment was described using the example of a case where the controller 30 is mounted on the vehicle body 14, it is not limited to this. For example, the controller 30 may not be mounted on the vehicle body 14, and the vehicle body 14 may be controlled based on signals output from a controller 30 located at a different location from the vehicle body 14.

[0111] Furthermore, in the above embodiment, the inclination angle θ ｗ In calculating the roll angle α, pitch angle β, and yaw angle γ, which are the attitude angles of the vehicle body 14, were explained as being calculated by combining GNSS data obtained from the GNSS sensor 22 and data obtained from the IMU 24 in GNSS_FIX mode, but this is not the only way. For example, the roll angle α and pitch angle β may be calculated based on the acceleration and angular velocity acquired by the IMU 24. Alternatively, a method may be used to detect the attitude angle using a tilt angle sensor or the like, without using the acceleration and angular velocity acquired by the IMU 24.

[0112] Furthermore, in the above embodiment, when the mode of the work machine 1 is GNSS_FIX mode, the inclination angle θ ｗ The example given is the calculation of the inclination angle θ. ｗ You could also calculate it this way.

[0113] Furthermore, the control processing that the CPU reads and executes in each of the above embodiments may be executed by various processors other than the CPU. Examples of such processors include PLDs (Programmable Logic Devices) such as FPGAs (Field-Programmable Gate Arrays) whose circuit configuration can be changed after manufacturing, and dedicated electrical circuits that are processors with circuit configurations specifically designed to execute specific processing, such as ASICs (Application Specific Integrated Circuits). The control processing may be executed by one of these various processors, or by a combination of two or more processors of the same or different types (for example, multiple FPGAs, and a combination of a CPU and an FPGA). More specifically, the hardware structure of these various processors is an electrical circuit that combines circuit elements such as semiconductor elements.

[0114] Furthermore, while the above embodiments describe a configuration in which the program is pre-stored (installed) on a storage device, the invention is not limited to this. The program may be provided in a form recorded on a recording medium such as a CD-ROM, DVD-ROM (Digital Versatile Disc Read Only Memory), or USB (Universal Serial Bus) memory. Alternatively, the program may be provided in a form that can be downloaded from an external device via a network.

[0115] The disclosure of Japanese Patent Application No. 2025-057132, filed on 28 March 2025, is incorporated herein by reference in its entirety. All documents, patent applications, and technical standards described herein are incorporated herein by reference to the same extent as if each individual document, patent application, and technical standard were specifically and individually noted to be incorporated by reference.

Claims

1. A work machine system comprising: a work machine including a work implement and a travel device; a vehicle speed sensor that acquires the vehicle speed based on the drive of the travel device; a positioning sensor that acquires a positioning signal which is the position of the work machine; and a controller including a processor, wherein the processor acquires the inclination angle of the work machine with respect to the direction of travel of the work machine, calculated based on a normal positioning signal at a first time point, and determines the position of the work machine based on the inclination angle and the vehicle speed detected by the vehicle speed sensor at a second time point.

2. The work machine system according to claim 1, wherein the inclination angle is calculated based on the vehicle speed acquired by the vehicle speed sensor when the working machine is in a predetermined state.

3. The work machine system according to claim 1 or 2, wherein the inclination angle is calculated based on the vehicle speed acquired by the vehicle speed sensor after the working machine is in a predetermined state and has traveled a predetermined distance or more.

4. The work machine system according to claim 1 or 2, wherein the inclination angle is calculated based on the difference between the travel distance of the work machine estimated from the positioning signal acquired by the positioning sensor and the travel distance of the work machine estimated from the vehicle speed acquired by the vehicle speed sensor.

5. The work machine system according to claim 4, wherein the processor calculates the inclination angle of the work machine with respect to the direction of travel of the work machine based on the vehicle speed detected by the vehicle speed sensor and the position of the work machine estimated from the positioning signal acquired by the positioning sensor when in a first mode indicating that the accuracy of the signal acquired by the positioning sensor is equal to or greater than a predetermined accuracy, and corrects the vehicle speed detected by the vehicle speed sensor based on the inclination angle calculated while the first mode is continuing when in a second mode indicating that the accuracy of the signal acquired by the positioning sensor is less than a predetermined accuracy.

6. The processor calculates the inclination angle of the work machine with respect to the direction of travel of the work machine based on the vehicle speed detected by the vehicle speed sensor, the position of the work machine estimated from the positioning signal acquired by the positioning sensor, and the attitude of the work machine estimated from the positioning signal acquired by the positioning sensor or the attitude of the work machine obtained by an attitude sensor provided on the work machine; calculates the velocity vectors in each direction in the vehicle body coordinate system by correcting the vehicle speed detected by the vehicle speed sensor to decompose it into velocity vectors in each direction in the vehicle body coordinate system, which is a coordinate system based on the work machine, based on the vehicle speed detected by the vehicle speed sensor and the inclination angle; and estimates the position of the work machine based on the velocity vectors of the work machine in each direction, according to claim 1 or claim 2.

7. The work machine system according to claim 1 or 2, wherein the processor calculates a first vertical travel distance of the work machine in the field coordinate system, which is a coordinate system based on the ground surface, based on the vertical velocity component of the vehicle speed obtained by the vehicle speed sensor; calculates a second vertical travel distance of the work machine in the field coordinate system based on the position of the work machine estimated from the signal obtained by the positioning sensor; and calculates the inclination angle of the work machine with respect to the direction of travel based on the first travel distance and the second travel distance.

8. The work machine system according to claim 1 or claim 2, wherein the controller is mounted on the work machine.

9. A method for controlling a work machine in a work machine system comprising: a work machine including a work implement and a travel device; a vehicle speed sensor that acquires the vehicle speed based on the drive of the travel device; a positioning sensor that acquires a positioning signal which is the position of the work machine; and a controller including a processor, wherein the processor acquires the inclination angle of the work machine with respect to the direction of travel of the work machine, calculated based on a normal positioning signal at a first time point, and determines the position of the work machine based on the inclination angle and the vehicle speed detected by the vehicle speed sensor at a second time point.

10. A controller for a work machine in a work machine system comprising: a work machine including a work implement and a travel device; a vehicle speed sensor that acquires the vehicle speed based on the drive of the travel device; a positioning sensor that acquires a positioning signal which is the position of the work machine; and a controller including a processor, the controller acquiring the inclination angle of the work machine with respect to the direction of travel of the work machine calculated based on a normal positioning signal at a first time point, and determining the position of the work machine based on the inclination angle and the vehicle speed detected by the vehicle speed sensor at a second time point.

Images