work vehicles

Abstract

This invention provides a work vehicle capable of detecting antenna placement errors more flexibly than conventional devices. The work vehicle has a control device (150). The control device (150) has a detection function (F106), a calculation function (F104), a calculation function (F105), a calculation function (F107), and an estimation function (F110). The detection function (F106) detects normal driving based on the vehicle's speed, acceleration, and angular velocity. The calculation function (F104) calculates a first vehicle direction based on the placement information of the first and second antennas relative to the vehicle. The calculation function (F105), when normal driving is detected, calculates a second vehicle direction based on the time change of the position information of the first antenna. The calculation function (F107) calculates a direction correction parameter for correcting the first vehicle direction based on the second vehicle direction. The estimation function (F110) estimates the vehicle's position and attitude based on the direction correction parameter and the first vehicle direction.

Description

Technical Field

[0001] This invention relates to work vehicles. Background Technology

[0002] The inventions known from the prior art relate to a method for determining the integer value deviation of the carrier phase when performing carrier differential positioning by receiving radio waves from a transmitter that transmits positioning signals such as GPS satellites, and a method and apparatus for measuring the orientation and posture of objects and moving bodies (see Patent Document 1 below).

[0003] The azimuth and attitude measurement device described in Patent Document 1 comprises a carrier phase integer value deviation determination device and a mechanism for determining azimuth or attitude (claim 5, etc.). The carrier phase integer value deviation determination device includes a first and a second antenna, an antenna azimuth observation mechanism, and a mechanism for calculation, correction, and determination (claim 4, etc.). The antenna azimuth observation mechanism receives multiple positioning signals from a positioning transmitter via the first and second antennas, respectively, and determines the azimuth of the second antenna relative to the first antenna based on the carrier phase change of the positioning signals.

[0004] The mechanism for performing the above calculations, corrections, and determinations operates as follows: First, it calculates the calculated carrier phase difference, i.e., the calculated phase difference, based on the azimuth of the second antenna relative to the first antenna, the distance between the first and second antennas, and the position of the positioning transmitter. Then, it calculates the fractional part of the carrier phase difference of the positioning signal received by the first and second antennas, i.e., the observed phase difference, and corrects the calculated phase difference in such a way that the fractional part of the calculated phase difference matches the observed phase difference within a range of ±0.5 periods. Finally, it determines this corrected calculated phase difference as the integer deviation of the observed phase difference (claim 4, etc.).

[0005] The mechanism for determining the orientation or position determines the relative position of the second antenna relative to the first antenna based on the integer value deviation determined by the carrier phase integer value deviation determination device and the observed phase difference, and determines the orientation or position between the first and second antennas based on the relative position (claim 5, etc.).

[0006] Furthermore, the aforementioned azimuth and attitude measuring device places the first and second antennas on a moving body traveling in a forward-backward direction within a substantially horizontal plane. The antenna azimuth observation mechanism observes the traveling azimuth of the moving body based on the carrier phase change of the positioning signal generated by the movement of the first and second antennas accompanying the movement of the moving body. Moreover, the device considers this traveling azimuth as the forward azimuth of the moving body, and observes the azimuth of the second antenna relative to the first antenna based on the forward azimuth of the moving body and the installation position relationship of the first and second antennas relative to the moving body (claim 7, etc.).

[0007] Existing technical documents

[0008] Patent documents

[0009] Patent Document 1: Japanese Patent Application Publication No. 2005-043212 Summary of the Invention

[0010] For example, dump trucks and other work vehicles, in order to measure the vehicle's direction, have a first antenna and a second antenna, similar to the conventional orientation and attitude measuring devices described above, to receive radio waves from satellite positioning systems such as the Global Positioning Satellite System (GNSS). Such antennas are, for example, positioned at the front end of an antenna mast extending upwards from the vehicle body to avoid radio wave obstruction.

[0011] Furthermore, the operation of vehicles, such as those loading or unloading ore and sand, or traveling on uneven terrain, can sometimes subject the antenna to vibration and impact. Therefore, over time, errors may arise between the actual and preset antenna positions, and between the calculated vehicle direction based on the preset position and the actual vehicle direction.

[0012] In this context, the conventional azimuth and attitude measurement device observes the azimuth of the moving body based on the movement of the first and second antennas, which accompany the movement of the moving body traveling in a forward-backward direction within a generally horizontal plane. However, in ore mining and engineering sites where work vehicles operate, the opportunities for the work vehicles to travel in a forward-backward direction within a generally horizontal plane are limited. Therefore, it is required for work vehicles to be able to more flexibly detect errors in the antenna placement even under these limited conditions.

[0013] This invention provides a work vehicle that can more flexibly detect errors in the antenna's setting position compared to conventional devices.

[0014] One aspect of the present invention is a work vehicle comprising: a vehicle; a first antenna and a second antenna mounted on the vehicle and receiving radio waves from a satellite positioning system; a receiver outputting position information of the first antenna obtained based on the radio waves and a baseline direction between the first antenna and the second antenna; sensors for measuring the speed, acceleration, and angular velocity of the vehicle; and a control device for estimating the position and attitude of the vehicle, wherein the control device has the following functions: a detection function for detecting normal driving based on the speed, acceleration, and angular velocity; a function for calculating a first vehicle direction based on the setting information of the first antenna and the second antenna relative to the vehicle; a function for calculating a second vehicle direction based on the time change of the position information of the first antenna when normal driving is detected; a function for calculating a direction correction parameter for correcting the first vehicle direction based on the second vehicle direction; and a function for estimating the position and attitude of the vehicle based on the direction correction parameter and the first vehicle direction.

[0015] Invention Effects

[0016] According to the above-described manner of the present invention, a work vehicle can be provided that is more flexible in detecting the error in the antenna's installation position compared to conventional devices. Attached Figure Description

[0017] Figure 1 This is a side view showing Embodiment 1 of the work vehicle of the present invention.

[0018] Figure 2 yes Figure 1 Functional block diagram of the control device for the work vehicle.

[0019] Figure 3 It means based on Figure 1 A flowchart illustrating an example of the processing performed by the control device of a work vehicle.

[0020] Figure 4 This means to explain Figure 3 The sensor information is stored in a table for processing.

[0021] Figure 5 This means to explain Figure 3 The table stores the location and orientation of the data.

[0022] Figure 6 It is a test Figure 3 The flowchart for the routine driving process.

[0023] Figure 7 This is a schematic diagram showing the relationship between the baseline direction of the first antenna and the second antenna and the vehicle direction.

[0024] Figure 8 It is a calculation Figure 3 The flowchart for processing in the direction of the first vehicle.

[0025] Figure 9 This is an explanation of the reason. Figure 8 A schematic diagram of the direction correction parameters obtained through processing.

[0026] Figure 10 It is a calculation Figure 3 The flowchart for processing in the second vehicle direction.

[0027] Figure 11 This is a schematic diagram illustrating an example of a turn during normal driving.

[0028] Figure 12 This is a schematic diagram illustrating the center of rotation of a vehicle.

[0029] Figure 13 It is a calculation Figure 3 The flowchart shows the process for handling the direction correction parameters.

[0030] Figure 14 This is a schematic diagram illustrating the direction correction parameters.

[0031] Figure 15 This is a functional block diagram of the control device in Embodiment 2 of the work vehicle of the present invention.

[0032] Figure 16 It means based on Figure 15 A flowchart illustrating an example of the processing performed by the routine driving detection function of the control device.

[0033] Figure 17 It means based on Figure 15 A flowchart illustrating an example of the processing performed by the driving control function of the control device. Detailed Implementation

[0034] Hereinafter, embodiments of the work vehicle of the present invention will be described with reference to the accompanying drawings.

[0035] [Implementation Method 1]

[0036] Figure 1 This is a side view showing Embodiment 1 of the work vehicle of the present invention. Figure 2 yes Figure 1 Functional block diagram of the control device 150 of the work vehicle 100. The work vehicle 100 in this embodiment is, for example, a dump truck used at ore mining sites and engineering sites. The work vehicle 100 includes, for example, a vehicle 110, a positioning device 120, a sensor 130, a cargo bucket 140, and a control device 150.

[0037] Vehicle 110 includes, for example, a chassis 111, wheels 112, and a driver's cab 113. The chassis 111 is, for example, a ladder-shaped structure. The chassis 111 supports, for example, the left and right wheels 112 mounted on the axles via suspension. Furthermore, the chassis 111 supports, for example, an engine, generator, motor, power transmission mechanism, steering mechanism, hydraulic system, and actuators for vehicle control, etc. (not shown).

[0038] Wheels 112 are connected to a motor via a power transmission mechanism, for example, and the vehicle 110 is driven by the motor. The cab 113 is a compartment for the operator of the work vehicle 100. Inside the cab 113, for example, a steering wheel (not shown), operating pedals, operating levers, information devices, speakers, timers, indicator lights, etc. are provided.

[0039] The positioning device 120 may be configured as a satellite positioning system such as a Global Positioning Satellite System (GNSS). The positioning device 120 may include, for example, a first antenna 121, a second antenna 122, and a receiver 123. The first antenna 121 and the second antenna 122 may be mounted on a vehicle 110 to receive radio waves from a satellite positioning system such as GNSS.

[0040] The first antenna 121 and the second antenna 122 are provided, for example, spaced apart in the width direction of the vehicle 110, which is orthogonal to the longitudinal and vertical directions of the vehicle 110. The first antenna 121 and the second antenna 122 are mounted, for example, at the front end of the antenna mast fixed to the vehicle 110, and are positioned at a higher position than the conventional position of the front end 141a of the cargo bed body 141.

[0041] Receiver 123 is connected, for example, to the first antenna 121 and the second antenna 122 via a signal line. Receiver 123 outputs the position information of the first antenna 121 and the baseline direction between the first antenna 121 and the second antenna 122 based on the radio waves received by the first antenna 121 and the second antenna 122. Here, the baseline direction refers to, for example, the direction of the straight line connecting the installation positions of the first antenna 121 and the second antenna 122.

[0042] Sensor 130 includes, for example, a velocity sensor 131, an acceleration sensor 132, and an angular velocity sensor 133. Additionally, Figure 1 as well as Figure 2 In the example shown, sensor 130 includes, for example, a lift sensor 134. These sensors 130 are connected to the control unit 150 in a communicative manner, for example, via a control local area network (CAN). Furthermore, if the speed of vehicle 110 is calculated by the control unit 150 based on the position determined by the positioning device 120, the positioning device 120 can also be used as a speed sensor, omitting the speed sensor 131.

[0043] Speed ​​sensor 131 detects the speed of vehicle 110, for example, based on the rotational speed of wheel 112, and outputs this speed to control device 150. Accelerometer 132 detects the acceleration of vehicle 110, for example, excluding gravitational acceleration, and outputs this acceleration to control device 150. Angular velocity sensor 133 detects the angular velocity of vehicle 110, for example, and outputs this angular velocity to control device 150. Lift sensor 134 detects the lifting state of cargo box body 141, including the stroke of lifting hydraulic cylinder 142 that lifts cargo box body 141, and outputs this lifting state to control device 150.

[0044] The cargo bucket 140, for example, includes a cargo bucket body 141, a lifting hydraulic cylinder 142, and a rotating shaft 143. The cargo bucket body 141 is rotatably supported on the vehicle frame 111 about the rotating shaft 143 located at the rear of the bottom. The cargo bucket body 141 is, for example, the part of the work vehicle 100 used for loading and transporting materials such as ore, rock, gravel, and soil.

[0045] The lifting hydraulic cylinders 142 are, for example, a pair of hydraulic cylinders located on both sides of the vehicle 110 in the width direction. In the lifting hydraulic cylinders 142, the front end of the piston rod is connected to the front of the rotation axis 143 at the bottom of the cargo box body 141, and the end of the hydraulic cylinder pipe on the opposite side of the piston rod is connected to the lower part of the vehicle frame 111. The lifting hydraulic cylinders 142 constitute part of the hydraulic system controlled by the control device 150. By extending and retracting the piston rod, the lifting hydraulic cylinders 142 cause the cargo box body 141 to rotate about the rotation axis 143, thereby raising and lowering the front end 141a of the cargo box body 141.

[0046] The control device 150 is, for example, a computer system such as a microcontroller or firmware. The control device 150 comprises, for example, a central processing unit (CPU), storage devices such as RAM and ROM, programs and data stored in these storage devices, and input / output units for inputting and outputting time and signals. The control device 150 and the sensor 130 operate synchronously at fixed intervals, thereby preventing signal loss due to time discrepancies. Furthermore, the drive cycle of the control device 150 can, for example, be equal to the shortest output cycle of the sensor 130.

[0047] like Figure 2As shown, the control device 150 includes, for example, a function F100 for calculating the position and orientation of the work vehicle 100, and a function F200 for monitoring the state of the work vehicle 100. These functions of the control device 150 can be implemented, for example, by the CPU executing a program stored in the storage device of the control device 150. In the following description, the function F100 for calculating the position and orientation of the work vehicle 100 will sometimes be simply referred to as the calculation function F100, and the function F200 for monitoring the state of the work vehicle 100 will sometimes be simply referred to as the state monitoring function F200.

[0048] Figure 2 In the example shown, the calculation function F100 of the control device 150 includes, for example, a function F101 for storing antenna setting information, a function F102 for storing position and orientation information, a function F103 for storing sensor information, and a function F108 for storing orientation correction parameters. In the following description, these functions of the control device 150 will sometimes be referred to simply as storage functions F101, F102, F103, and F108, respectively.

[0049] in addition, Figure 2 In the example shown, the calculation function F100 of the control device 150 also includes a function F104 for calculating the direction of the first vehicle, a function F105 for calculating the direction of the second vehicle, a function F106 for detecting normal driving, and a function F107 for calculating direction correction parameters. In the following description, these functions of the control device 150 will sometimes be referred to simply as calculation function F104, F105, detection function F106, and calculation function F107, respectively.

[0050] in addition, Figure 2 In the example shown, the calculation function F100 of the control device 150 also has a function F109 to determine whether the direction correction parameters can be used, and a function F110 to estimate the position and posture of the work vehicle 100. In the following description, these functions of the control device 150 will sometimes be referred to simply as the judgment function F109 and the estimation function F110, respectively.

[0051] Figure 3 It means based on Figure 1 A flowchart illustrating an example of the processing performed by the control unit 150 of the work vehicle 100. The control unit 150 performs, for example, the calculation function F100. Figure 3 The process is shown. Specifically, when the control device 150 starts... Figure 3 After the processing shown, for example, the storage function F103 performs the storage sensor information processing P1. In this processing P1, the storage function F103 stores the sensor information input from the sensor 130 via the input / output section of the control device 150 in the storage device constituting the control device 150.

[0052] Figure 4 Table T1 represents an example of sensor information stored in P1 via storage function F103. The sensor information includes, for example, output values ​​from the velocity sensor 131, acceleration sensor 132, angular velocity sensor 133, and lift sensor 134 included in sensor 130, namely velocity v, acceleration a, angular velocity w, and lift state h. Additionally, the sensor information includes, for example, the time t, t-1, ..., t-m, ... when each output value is output. The storage device of control device 150 includes, for example, sensor information at the current time t, and sensor information at times prior to the current time t, t-1, ..., t-m, ..., storing sensor information within a fixed period.

[0053] The fixed period for storing sensor information in the storage device of the control device 150 is at least a period longer than the period during which the position information of the first antenna 121 and the baseline direction connecting the first antenna 121 and the second antenna 122 are output from the receiver 123. Furthermore, the velocity, acceleration, and angular velocity included in the sensor information can be the output values ​​of the velocity sensor 131, the acceleration sensor 132, and the angular velocity sensor 133, or values ​​converted from the output values ​​of each sensor to a three-dimensional representation using vehicle coordinates.

[0054] Next, the control device 150 executes, for example, process P2, which determines whether the position information of the first antenna 121 and the baseline direction connecting the first antenna 121 and the second antenna 122 have been jointly obtained from the receiver 123. Furthermore, the position information of the first antenna 121 is the position information of the first antenna 121 output from the receiver 123 as a result of inputting a signal from the first antenna 121 receiving GNSS radio waves to the receiver 123. Additionally, the baseline direction is, for example, the direction of the straight line connecting the first antenna 121 and the second antenna 122, output from the receiver 123 as a result of inputting a signal from the first antenna 121 receiving GNSS radio waves to the receiver 123.

[0055] The storage function F102 of the control device 150, for example, acquires the position information of the first antenna 121 and the baseline direction from the receiver 123 at fixed periods, and stores them in the storage device of the control device 150. Therefore, in process P2, the control device 150 determines, for example, whether the position information of the first antenna 121 and the baseline direction connecting the first antenna 121 and the second antenna 122 have been obtained jointly from the receiver 123.

[0056] Specifically, for example, suppose that the receiver 123 outputs the position information of the first antenna 121 at a period of 1 Hz, and outputs the baseline direction connecting the first antenna 121 and the second antenna 122 at a period of 10 Hz. In this case, in process P2, if the control device 150 determines that the position information of the first antenna 121 has not been obtained (No) when only the baseline direction is obtained from the receiver 123, it executes process P13 described later. On the other hand, if the control device 150 determines in process P2 that both the position information of the first antenna 121 and the baseline direction have been obtained (Yes), it executes the next process P3.

[0057] Figure 5 Table T2 shows an example of the position information p and baseline direction d of the first antenna 121 stored in the storage device of the control device 150 in process P3. In process P3, the control device 150, for example, stores the position information p and baseline direction d of the first antenna 121 obtained from the receiver 123 together with the obtained time t, t-m, ..., t-6m, ... in the storage device via the storage function F102.

[0058] Figure 5 This example illustrates the joint output of the position information p of the first antenna 121 and the baseline direction d over a period m. The position information p of the first antenna 121, for example, represents information about a point on a map, and is two-dimensional or three-dimensional position information. Furthermore, the baseline direction d connecting the first antenna 121 and the second antenna 122 is, for example, a two-dimensional or three-dimensional vector or azimuth. Figure 3 As shown, after processing P3 is completed, the control device 150 executes the next processing P4.

[0059] Figure 6 It is a test Figure 3 The flowchart for the normal driving process is shown in P4. Furthermore... Figure 6 The processes shown can be executed, for example, by the detection function F106 of the control device 150. When process P4 begins, the control device 150 first executes the process P401, which calculates the average speed. Specifically, in this process P401, the control device 150, for example, refers to a storage device and to the time when the position information p of the first antenna 121 was obtained from the positioning device 120 one cycle ago. Furthermore, the control device 150, for example, uses the speed v of the vehicle 110 from the time the position information p of the first antenna 121 was obtained one cycle ago until the present, to calculate the average speed of the vehicle 110.

[0060] Next, the control device 150 executes process P402, which determines whether the calculated average speed of the vehicle 110 is higher than a predetermined speed threshold Vth. In this process P402, for example, if the control device 150 determines that the calculated average speed of the vehicle 110 is lower than the predetermined speed threshold Vth (no), it executes the next process P408.

[0061] In process P408, control device 150 sets the normal driving indicator stored in the storage device to 0, and ends the process. Figure 6 The process is as shown. A normal driving status of 0 indicates that the work vehicle 100 is not performing normal driving. That is, normal driving includes the average speed of vehicle 110 being higher than the prescribed speed threshold Vth. Therefore, parking and low-speed driving below the speed threshold Vth are excluded from normal driving.

[0062] On the other hand, in process P402, if the control device 150 determines, for example, that the calculated average speed of the vehicle 110 is higher than a predetermined speed threshold Vth, it executes the next process P403. In process P403, the control device 150 calculates, for example, the height of the cargo bin 140. Specifically, the control device 150 calculates the height of the cargo bin 140 by referring to a storage device, based on the lifting state of the cargo bin 140 at the time when the output of the positioning device 120 one cycle ago was obtained. Here, the height of the cargo bin 140 is, for example, the height of the front end 141a of the cargo bin body 141.

[0063] For example, suppose the lifting state of the hopper 140 is stored in the storage device of the control device 150 as the angle of the hopper body 141. In this case, the control device 150 can, for example, use the sine of the angle of the hopper body 141 to determine the height of the front end 141a of the hopper body 141. Furthermore, the dimensional data of the hopper body 141 is, for example, pre-stored in the storage device of the control device 150. After processing P403 is completed, the control device 150 executes the next processing P404.

[0064] In process P404, the control device 150, for example, determines whether the height of the hopper 140 calculated by process P403 is lower than a predetermined height threshold Hth. More specifically, the control device 150 determines, for example, whether the height of the hopper 140 at the current time and at the time when the position information p was obtained from the previous positioning device 120 is lower than the height threshold Hth. Here, the height threshold Hth can be set, for example, to a height equal to the height of the first antenna 121 or the second antenna 122, or the lower of the first antenna 121 and the second antenna 122.

[0065] In process P404, assuming the control device 150 determines that, for example, at either the current time or the time when position information p was obtained from the previous positioning device 120, the height of the cargo bin 140 is above the height threshold Hth (No). In this case, the control device 150 executes process P408, which sets the aforementioned normal driving indicator to 0, and ends the process. Figure 6 The process shown is P4.

[0066] Additionally, in process P404, it is assumed that the control device 150 determines, for example, that the height of the cargo bin 140 is lower than the height threshold Hth at either the current time or the time when the position information p was obtained from the previous positioning device 120. In this case, the control device 150 performs, for example, process P405, which calculates the driving state of the work vehicle 100. More specifically, in process P405, the control device 150 calculates the angular velocity, angular acceleration, and their respective average values, as well as the change in direction of the vehicle 110.

[0067] Here, the angular velocity, angular acceleration, and average value of the acceleration of the work vehicle 100 are, for example, the average values ​​from the moment the position information p was obtained from the positioning device 120 to the present moment. The angular acceleration of the work vehicle 100 can be calculated, for example, based on the difference between the angular velocity of the work vehicle 100 at a certain moment and the angular velocity of the work vehicle 100 at a moment a period ago. The direction of the vehicle 110, that is, the change in the vehicle's direction, can be calculated, for example, as follows.

[0068] Figure 7 This is a schematic diagram showing the relationship between the baseline direction d connecting the first antenna 121 and the second antenna 122 and the vehicle direction Dv. Figure 7 In the example shown, during the period from the moment the work vehicle 100 obtained position information p from the previous positioning device 120 to the present moment, the vehicle 110 moves along a circle centered at point Ct, turning at an angle θ. In this case, the vehicle direction Dv changes at an angle equal to the turning angle θ of the vehicle 110. Similarly, the baseline direction d, that is, the baseline vector, also changes at an angle equal to the turning angle θ of the vehicle 110.

[0069] Thus, the angle θ of the change in vehicle direction Dv is equal to the angle θ of the change in baseline direction d. Therefore, by calculating the angle change in baseline direction d between the current time and the time when position information p was obtained from the previous positioning device 120, the angle change in vehicle direction Dv can be calculated. That is, in processing P405, the control device 150 calculates the angle change in vehicle direction Dv, for example, by calculating the angle change in baseline direction d. As described above, in processing P405, the control device 150 calculates, for example, the average values ​​of the angular velocity, angular acceleration, and acceleration of the working vehicle 100, and the angle change in vehicle direction Dv.

[0070] Next, the control device 150 performs, for example, a process P406 to determine whether the driving state of the work vehicle 100 is normal driving. Here, normal driving refers to, for example, the state in which the work vehicle 100 is driving in a predetermined motion state. More specifically, normal driving of the work vehicle 100 includes, for example, turning the work vehicle 100 along a circle with the same center and radius, and straight-line driving the work vehicle 100 along a straight line. In addition, normal driving is not limited to turning and straight-line driving, and may also include other driving states.

[0071] In process P406, control device 150 determines, for example, that the difference between the change in vehicle direction Dv calculated from the previous process P405 and the time integral of the angular velocity of vehicle 110 is below a threshold, that the work vehicle 100 is turning along a circle with the same center and radius. In this case, since the turning of work vehicle 100 is included in normal driving, control device 150 determines in process P406 that the driving state of work vehicle 100 is normal driving (yes) and executes the next process P407.

[0072] Furthermore, in process P406, if, for example, the angular acceleration and the average value of the acceleration of vehicle 110 calculated from the previous process P405 are below a threshold, the control device 150 determines that the work vehicle 100 is traveling in a straight line. In this case, since the straight-line travel of the work vehicle 100 is included in normal travel, the control device 150 determines in process P406 that the travel state of the work vehicle 100 is normal travel (yes) and executes the next process P407.

[0073] On the other hand, suppose the difference between the change in vehicle direction Dv calculated from the previous process P405 and the time integral of the angular velocity of vehicle 110 is greater than a threshold, or the angular acceleration of vehicle 110 and its average value are greater than a threshold. In this case, in process P406, the control device 150 determines that the driving state of the work vehicle 100 is not normal driving (no), executes process P408 which sets the normal driving flag to 0, and ends the process. Figure 6 The process shown is P4.

[0074] In process P407, the control device 150 determines whether the absolute value |αave| of the average angular velocity of the vehicle 110 calculated from process P405 is smaller than a predetermined angular velocity threshold αth. If the control device 150 determines in process P407 that the absolute value |αave| is smaller than the angular velocity threshold αth (yes), it executes process P409, which sets the normal driving indicator stored in the storage device to 1, and then ends the process. Figure 6 The process shown is P4. The normal driving status of 1 indicates that the working vehicle 100 is traveling in a straight line.

[0075] On the other hand, in process P407, if the control device 150 determines that the absolute value |αave| is greater than or equal to the angular velocity threshold αth (no), then process P410, which sets the normal driving indicator stored in the storage device to 2, is executed, and the process ends. Figure 6 The process P4 is shown. The normal driving state marked 2 indicates that the working vehicle 100 is turning along a circle with the same center and radius. After process P4 is completed, the control device 150 executes... Figure 3 The processing shown is P5.

[0076] In process P5, the control device 150 determines, for example, whether the driving state of the work vehicle 100 is normal driving through the detection function F106. Specifically, the control device 150 uses the detection function F106 to refer to the normal driving indicator stored in the storage device, which was set in the previous process P4. If the referenced normal driving indicator is 0, it means that the work vehicle 100 is not driving normally. Therefore, the control device 150 determines through the detection function F106 that it is not driving normally (no) and executes the process P13 of estimating position and posture.

[0077] When the work vehicle 100 is not performing regular driving, including turning and straight-line travel, the control device 150, in process P13, uses the direction correction parameters stored in the storage device from the previous processes P6 to P12 to estimate the position and orientation of the work vehicle 100. The process P13 for estimating the position and orientation of the work vehicle 100 will be described in detail after the description of processes P6 to P12.

[0078] In process P5, if the referenced normal driving indicator is 1 or 2, it indicates that the work vehicle 100 is performing normal driving. Therefore, the control device 150 determines that it is performing normal driving (yes) through the detection function F106 and executes process P6 to calculate the first vehicle direction. Here, the first vehicle direction refers to the direction of the vehicle 110 calculated based on the setting information of the first antenna 121 and the second antenna 122. This setting information includes, for example, the coordinates of the positions where the first antenna 121 and the second antenna 122 are set, through... Figure 2 The storage function F101 of the control device 150 shown is stored in the storage device of the control device 150.

[0079] Figure 8 This is a flowchart of the process P6 for calculating the direction of the first vehicle. For example, after the control device 150 starts calculating the direction of the first vehicle P6, it first executes the process P601 for obtaining direction correction parameters through the calculation function F104. In this process P601, the control device 150 obtains, for example, the latest direction correction parameters stored in the storage device through the storage function F101 through the calculation function F104.

[0080] Figure 9 This is an explanation Figure 8 The diagram shows the direction correction parameters obtained in processing P601. The first antenna 121 and the second antenna 122 are fixedly positioned on the vehicle 110. Based on the positions of the first antenna 121 and the second antenna 122 relative to the vehicle 110, the vector Av from the first antenna 121 to the second antenna 122 can be calculated. Furthermore, the direction of the centerline of the vehicle 110, which is parallel to the longitudinal direction of the vehicle 110, can be defined as the vehicle direction Dv.

[0081] In this case, the setting information of the first antenna 121 and the second antenna 122 includes the angle between the vehicle direction Dv and the vector Av, i.e., the antenna setting angle δ. Furthermore, the direction correction parameter is a parameter used to correct for changes in the antenna setting angle δ caused by some reason. The setting positions of the first antenna 121 and the second antenna 122 can be represented by coordinates fixed to the coordinate system of the vehicle 110, and the antenna setting angle δ and the direction correction parameter can also be calculated similarly by performing coordinate transformation.

[0082] The orientation correction parameters are calculated in process P8 (described later) by the calculation function F107 of the control device 150 and stored in the storage device by the storage function F108. Furthermore, the control device 150's storage device stores, for example, orientation correction parameters for a certain period of time along with the time interval. Additionally, if process P8 is not implemented and no orientation correction parameters are calculated, the control device 150's storage device stores 0 as an initial value. When the orientation correction parameters are 0, no correction is performed on the antenna setting angle δ.

[0083] Next, the control device 150 performs the processing P602, for example, by calculating the direction of the first vehicle via the calculation function F104. Specifically, for example, the control device 150 uses the calculation function F104 to calculate the direction of the first vehicle from... Figure 9 The values ​​obtained by subtracting the direction correction parameter from the antenna setting angle δ shown are: Figure 9 The vector Av shown is used to calculate the vehicle direction Dv, and then... Figure 8 The process P6 is shown. The vehicle direction Dv calculated by this process P6 is the first vehicle direction derived from the installation positions of the first antenna 121 and the second antenna 122. Next, the control device 150, for example, uses calculation function F105 as... Figure 3 The process P7, which calculates the direction of the second vehicle, is shown.

[0084] Figure 10 This is the flowchart for calculating the direction of the second vehicle, P7. Figure 10 The processes shown are executed, for example, by the calculation function F105 of the control device 150. If the control device 150 starts processing P7, it first executes the process P701, which calculates the difference in antenna positions. In process P701, the control device 150 obtains the position information p of the first antenna 121, which is input to the control device 150 from the receiver 123 of the positioning device 120 and stored in the storage device by the storage function F102 of the control device 150. Here, the control device 150 obtains the current position information p of the first antenna 121 and the position information p of the first antenna 121 one cycle ago.

[0085] Furthermore, in this process P701, the control device 150 calculates the direction of movement of the vehicle 110 based on the difference between the current position information p of the first antenna 121 and the position information p of the first antenna 121 one cycle ago. More specifically, the control device 150 calculates the movement vector of the vehicle 110 based on the difference between the current position information p of the first antenna 121 and the position information p of the first antenna 121 one cycle ago.

[0086] Next, the control device 150 executes process P702, which determines whether the normal driving indicator is 2. In process P702, it is assumed that the control device 150 determines that the normal driving indicator is not 2 (no), meaning the normal driving indicator is 1. In this case, the vehicle 110 is traveling in a straight line, and the vehicle direction Dv is consistent with the direction of travel of the vehicle 110. Therefore, the control device 150 executes process P706, which sets the movement vector of the vehicle 110 calculated by the previous process P701 as the second vehicle direction, and then ends the process. Figure 10 The processing shown is P7.

[0087] On the other hand, in process P702, it is assumed that the control device 150 determines that the normal driving indicator is 2 (yes). In this case, the driving state of the vehicle 110 is a turn along a circle with the same center and radius. Therefore, the control device 150 performs process P703 to calculate the position of the center of the turn of the vehicle 110.

[0088] Figure 11 This is a schematic diagram illustrating an example of a turn during the normal driving of the work vehicle 100. Figure 11 In the diagram, we show the current position of vehicle 110, which is turning left (counterclockwise) around center Ct, and its position one cycle ago. In processing P703, control device 150 first calculates the vehicle direction Dv of vehicle 110 one cycle ago based on the position information p of the first antenna 121 one cycle ago and the position information p of the first antenna 121 at the current time.

[0089] Here, vehicle 110 turns along a circle with the same radius as center Ct. Therefore, the rotation angle of vehicle 110 around center Ct from the time one cycle ago to the present time is equal to the angle θ between the vehicle direction Dv one cycle ago and the vehicle direction Dv at the present time. The turning center Ct of vehicle 110 lies on the bisecting line of the movement vector Mv calculated by the above-described process P701. In addition, the distance D from the movement vector Mv to the turning center Ct can be calculated by setting the length of the movement vector Mv as L using the following equation (1).

[0090] D=L / {2×tan(θ / 2)}···(1)

[0091] Therefore, in process P703, the control device 150 can, for example, calculate the normal relative to the movement vector Mv based on the average angular velocity of the vehicle 110, thereby calculating the center of rotation Ct. Next, the control device 150 executes process P704, which calculates the center of rotation of the vehicle 110.

[0092] In this process P704, the control device 150 first calculates the vector from the position of the first antenna 121 toward the rotation center of the vehicle 110. To this end, the control device 150 calculates the vector from the installation position of the first antenna 121 toward the installation position of the acceleration sensor 132, and the vector from the installation position of the acceleration sensor 132 toward the rotation center of the vehicle 110.

[0093] Figure 12 This is a schematic diagram illustrating the rotation center Ctv of vehicle 110. The rotation center Ctv of vehicle 110 is a point where vehicle 110 is considered a rigid body and the direction of its velocity vector Vv is equal to the vehicle's direction Dv. Furthermore, the direction of the velocity vector Vv of vehicle 110 is the normal direction of the line segment connecting the turning center Ct of vehicle 110 to the rotation center Ctv of vehicle 110. The vector from the location of the acceleration sensor 132 toward the rotation center Ctv of vehicle 110 can be calculated as follows.

[0094] The longitudinal component, ax, of the vehicle 110 is extracted from the average acceleration of the vehicle 110 calculated by the above-described process P4. Furthermore, the average angular velocity wz in the yaw direction and the deviation g from the vehicle direction Dv are used. Thus, the vector from the installation position of the acceleration sensor 132 toward the rotation center Ctv of the vehicle 110 can be calculated as (ax / (wz×wz), g).

[0095] Based on the above, the vector from the first antenna 121 toward the acceleration sensor 132 and the vector from the acceleration sensor 132 toward the rotation center Ctv of the vehicle 110 can be obtained. Furthermore, by adding these vectors together, the vector from the first antenna 121 toward the rotation center Ctv of the vehicle 110 can be obtained.

[0096] Furthermore, when the vehicle 110 is traveling at low speed, the rear axle center Cra can be defined as the rotation center of the vehicle 110 according to Ackermann geometry. In this case, the vector from the first antenna 121 to the rear axle center Cra can be pre-calculated based on the specifications of the vehicle 110 and stored in the storage device of the control device 150.

[0097] Furthermore, in processing P704, the control device 150 calculates the rotation center Ctv of the vehicle 110 using the vector from the first antenna 121 toward the rotation center Ctv of the vehicle 110 and the vector of the baseline direction connecting the first antenna 121 and the second antenna 122. Specifically, the vector from the first antenna 121 toward the rotation center Ctv of the vehicle frame 111 is rotated around the position of the first antenna 121 one cycle ago, which is stored in the storage device of the control device 150 through the storage function F102.

[0098] The amount of vector rotation at this point is determined, for example, by calculating the direction of the first vehicle. Here, the direction of the first vehicle can, for example, be determined using the same method as described in process P602 above, from... Figure 9 The value obtained by subtracting the direction correction parameter from the antenna setting angle δ shown, and the vector Av from the first antenna 121 towards the second antenna 122 one cycle ago, are used to calculate the rotation center Ctv of the vehicle 110 in processing P704.

[0099] Next, the control device 150 executes the process P705 for calculating the second vehicle direction. In this process P705, the control device 150 calculates the second vehicle direction based on the turning center Ct of the vehicle 110 calculated by the above-described process P703 and the rotation center Ctv of the vehicle 110 calculated by the above-described process P704, and then ends the process. Figure 10 The processing shown is P7.

[0100] The second vehicle direction calculated by the process P7 is derived from the information output from the receiver 123 of the positioning device 120. The second vehicle direction can be calculated as the normal direction obtained through a right-handed coordinate system, relative to the vector from the turning center Ct of the vehicle 110 towards the rotation center Ctv of the vehicle 110. Then, the control device 150 performs the calculation. Figure 3 Processing of direction correction parameters (P8).

[0101] Figure 13 This is the flowchart on page 8 for calculating the direction correction parameters. Figure 14 This is a schematic diagram illustrating the processing of P8. If the control device 150 begins processing P8, it first executes the process P801, which calculates the correction angle φ. The correction angle φ is, for example, the angle between the first vehicle direction Dv1 and the second vehicle direction Dv2. Here, the first vehicle direction Dv1 is the vehicle direction derived from the installation positions of the first antenna 121 and the second antenna 122, and the second vehicle direction Dv2 is the vehicle direction derived from the position information p of the first antenna 121 output from the receiver 123 of the positioning device 120.

[0102] In other words, the first vehicle direction Dv1 is affected by the positional errors of the first antenna 121 and the second antenna 122, while the second vehicle direction Dv2 is not affected by these positional errors. Therefore, the second vehicle direction Dv2 is considered the actual vehicle direction. The control device 150 calculates the difference between the first vehicle direction Dv1 and the second vehicle direction Dv2 using the calculation function F107, and calculates the correction angle φ used to make the first vehicle direction Dv1 and the second vehicle direction Dv2 equal.

[0103] Next, the control device 150 executes the process P802 of updating the orientation correction parameters. In this process P802, the control device 150, for example, uses the calculation function F107 to add the correction angle φ calculated by the previous process P801 to the orientation correction parameters stored in the storage device, thereby updating the orientation correction parameters. Next, the control device 150 executes the process P803 of storing the orientation correction parameters. In this process P803, the control device 150, for example, uses the storage function F108 to store the orientation correction parameters updated by the previous process P802 in the storage device, and then ends the process. Figure 13 The processing shown is P8.

[0104] Next, the control device 150, for example, through the judgment function F109, such as... Figure 3 The process shown on page P9 determines whether the direction correction parameters have converged. The second vehicle direction, Dv2, is also used... Figure 10 In the process P704, which calculates the rotation center Ctv of vehicle 110, an error arises in the second vehicle direction Dv2 where the previous direction correction parameters cannot be completely removed. To reduce this error, the control device 150 performs a convergence operation in process P9 using the latest direction correction parameters calculated from the previous process P8.

[0105] In processing P9, the control device 150, for example, is... Figure 13 If the absolute value of the correction angle φ calculated by process P801 is smaller than a preset threshold, it is determined that the direction correction parameter has converged (yes), and the next process P10 is executed. On the other hand, in process P9, if, for example, the absolute value of the correction angle φ is greater than a preset threshold, the control device 150 determines that the direction correction parameter has not converged (no), and processes P6 to P9 are executed repeatedly.

[0106] Next, the control device 150 performs a process P10, for example, by using a decision function F109, to calculate the average value of the direction correction parameters. The direction correction parameters are calculated for each sample period based on the position information p of the first antenna 121 input from the receiver 123 of the positioning device 120 to the control device 150. Therefore, errors also occur in the direction correction parameters due to position errors that occur when the receiver 123 performs positioning calculations based on the GNSS radio waves received by the first antenna 121.

[0107] To eliminate errors in the orientation correction parameter, the control device 150 performs successive averaging of the orientation correction parameter in processing P10. In the successive averaging of the orientation correction parameter, for example, an upper limit can be set for the number of calculations or a forgetting factor can be introduced to suppress excessive increases in the number of calculations. This prevents the inability to reflect changes in the orientation correction parameter.

[0108] Next, the control device 150 executes, for example, a process P11 that determines whether the direction correction parameter can be used via the judgment function F109. As described above, the direction correction parameter calculated at each moment includes the error, thereby calculating the average value of the direction correction parameter calculated over a specified period. Therefore, it is necessary to calculate a certain number of direction correction parameters and to prevent the direction correction parameter from varying too much in each cycle.

[0109] Therefore, in process P11, if the calculated value of the direction correction parameter is above a predetermined threshold and the difference between the latest average of the direction correction parameter and the average of the parameter one cycle ago is below a predetermined threshold, the control device 150 determines that the direction correction parameter can be used (yes). In this case, the control device 150 uses the direction correction parameter to perform process P12 to correct the vehicle direction, and based on the corrected vehicle direction, performs process P13 to estimate the position and attitude of the vehicle 110 through the estimation function F110.

[0110] On the other hand, in process P11, if the control device 150 determines that the direction correction parameters cannot be used (no), it does not use the direction correction parameters and executes process P13, which estimates the position and attitude of the vehicle 110 through the estimation function F110. Finally, the control device 150 executes process P14, which outputs the average value of the direction correction parameters calculated by process P10 and the position and attitude of the vehicle 110 estimated by process P13 to the status monitoring function F200 via CAN. Through the above, based on the calculation function F100 of the control device 150... Figure 3 All processes shown have ended.

[0111] Next, the status monitoring function F200 of the control device 150 detects errors in the setting positions of the first antenna 121 or the second antenna 122 based on the setting information of the first antenna 121 and the second antenna 122 and the direction correction parameters. Furthermore, the status monitoring function F200 of the control device 150 determines, for example, that the setting positions of the first antenna 121 and the second antenna 122 are abnormal if the direction correction parameters exceed a predetermined threshold.

[0112] In this case, the work vehicle 100 may have an information notification device for notifying the operator or user. Such an information notification device may include, for example, an LCD display, indicator lights, a speaker, or a buzzer. Thus, when the directional correction parameter exceeds a predetermined threshold, the status monitoring function F200 of the control device 150 outputs error information relative to the setting information of the first antenna 121 and the second antenna 122 to the information notification device, thereby notifying the operator or user of the work vehicle 100 of the anomaly.

[0113] As described above, the work vehicle 100 of this embodiment includes a vehicle 110, and a first antenna 121 and a second antenna 122 mounted on the vehicle 110 to receive radio waves from a satellite positioning system. Furthermore, the work vehicle 100 includes a receiver 123 that outputs the position information p of the first antenna 121 obtained based on the radio waves from the satellite positioning system, and the baseline direction d between the first antenna 121 and the second antenna 122. Additionally, the work vehicle 100 includes a sensor 130 that measures the speed, acceleration, and angular velocity of the vehicle 110, and a control device 150 that estimates the position and attitude of the vehicle 110. The control device 150 includes a detection function F106, a calculation function F104, a calculation function F105, a calculation function F107, and an estimation function F110. The detection function F106 is a function that detects normal driving based on the speed, acceleration, and angular velocity of the vehicle 110. Calculation function F104 calculates the first vehicle direction Dv1 based on the setting information of the first antenna 121 and the second antenna 122 relative to the vehicle 110. Calculation function F105 calculates the second vehicle direction Dv2 based on the time change of the position information p of the first antenna 121 when normal driving is detected. Calculation function F107 calculates the direction correction parameters for correcting the first vehicle direction Dv1 based on the second vehicle direction Dv2. Estimation function F110 estimates the position and attitude of the vehicle 110 based on the direction correction parameters and the first vehicle direction Dv1.

[0114] With this configuration, the work vehicle 100 according to this embodiment can detect errors in the installation positions of the first antenna 121 and the second antenna 122 more flexibly compared to conventional devices. More specifically, the first antenna 121 and the second antenna 122 are mounted, for example, at the front end of an antenna mast extending upward from the vehicle frame 111, in a manner that does not obstruct radio waves from the satellite positioning system by the vehicle 110 and cargo bed 140. Furthermore, the work vehicle 100 may load or unload cargo into or from the cargo bed 140, or travel on uneven terrain such as mines and construction sites. Therefore, vibrations and impacts may sometimes act on the first antenna 121 and the second antenna 122. Consequently, over time, errors may sometimes arise between the preset installation positions of the first antenna 121 and the second antenna 122 and their actual installation positions. In such cases, errors may sometimes arise between the first vehicle direction Dv1 calculated based on the preset installation positions and the actual vehicle direction.

[0115] Therefore, in this embodiment, when the detection function F106 of the control device 150 detects normal driving, the work vehicle 100 calculates the second vehicle direction Dv2 using the time change of the position information p of the first antenna 121 obtained based on the radio waves received by the satellite positioning system from the first antenna 121. Furthermore, based on the second vehicle direction Dv2, a direction correction parameter for correcting the first vehicle direction Dv1 is calculated, and based on this direction correction parameter and the first vehicle direction Dv1, the position and attitude of the vehicle 110 are estimated. In other words, the work vehicle 100 of this embodiment performs normal driving, which includes not only the strictly defined driving states as in conventional devices, but also a wider variety of driving states, thereby enabling more flexible correction of the error between the first vehicle direction Dv1 and the actual vehicle direction Dv. Additionally, there is no need for operations to detect errors arising from the installation positions of the first antenna 121 and the second antenna 122.

[0116] Furthermore, in the work vehicle 100 of this embodiment, the control device 150 also has a determination function F109 for determining whether the direction correction parameter can be used. Moreover, the estimation function F110 of the control device 150, when the direction correction parameter is usable, estimates the position and posture of the vehicle 110 based on the direction correction parameter and the first vehicle direction Dv1. With this configuration, for example, it is possible to prevent the first vehicle direction Dv1 from being corrected based on a direction correction parameter that includes a large error, and the first vehicle direction Dv1 can be corrected more accurately.

[0117] Furthermore, in the work vehicle 100 of this embodiment, regular driving includes turning, which involves traveling along a circle with the same center and radius, and straight-line driving, which involves traveling along a straight line. With this configuration, by performing regular driving, this regular driving not only includes the strictly defined driving state that allows the work vehicle 100 to travel in the forward and backward direction in a substantially horizontal plane, as in conventional devices, but also includes turning and straight-line driving. As a result, it is possible to more flexibly correct the error between the first vehicle direction Dv1 and the actual vehicle direction.

[0118] Furthermore, in the work vehicle 100 of this embodiment, normal driving includes situations where the average speed of the vehicle 110 is higher than a predetermined speed threshold Vth. With this configuration, situations such as the vehicle 110 stopping and situations such as the vehicle 110 slowly moving at a speed below the speed threshold Vth are excluded from normal driving, and the position and posture of the vehicle 110 can be estimated more accurately.

[0119] Furthermore, the work vehicle 100 of this embodiment includes a cargo bed 140 installed on the vehicle 110, a lifting hydraulic cylinder 142 serving as a lifting mechanism for raising and lowering the cargo bed 140, and a lifting sensor 134 for detecting the height of the cargo bed 140. Also, in the work vehicle 100 of this embodiment, normal driving includes situations where the height of the cargo bed 140 is lower than a predetermined height threshold Hth.

[0120] With this configuration, the work vehicle 100 of this embodiment does not calculate the second vehicle direction Dv2 when, for example, the front end 141a of the cargo bucket body 141 is above a height threshold Hth based on the height of the first antenna 121 and the second antenna 122. Therefore, it is possible to prevent the calculation of the second vehicle direction Dv2 when radio waves from the satellite positioning system to the first antenna 121 or the second antenna 122 are blocked by the cargo bucket 140, thus reducing the error in the second vehicle direction Dv2. Therefore, the work vehicle 100 according to this embodiment can more accurately estimate the position and orientation of the vehicle 110.

[0121] Furthermore, the work vehicle 100 of this embodiment can be equipped with an information notification device for notifying operators or users. In this case, if the direction correction parameter exceeds a predetermined threshold, the control device 150 can output error information regarding the setting information of the first antenna 121 and the second antenna 122 to the information notification device. With this configuration, the work vehicle 100 of this embodiment can notify operators or users of the situation where an error has occurred in the setting position of the first antenna 121 or the second antenna 122.

[0122] As described above, according to this embodiment, a work vehicle 100 can be provided that is more flexible in detecting the error in the setting position of the first antenna 121 or the second antenna 122 compared to conventional devices.

[0123] [Implementation Method 2]

[0124] The following uses some of the figures used in Implementation 1, and refers to... Figures 15 to 17 To illustrate Embodiment 2 of the work vehicle of the present invention. Figure 15 This is a functional block diagram illustrating the control device 150 of Embodiment 2 of the work vehicle of the present invention. The work vehicle 100 of this embodiment differs from the work vehicle 100 of Embodiment 1 described above in that the control device 150 has different functions. Other aspects of the work vehicle 100 of this embodiment are the same as those of the work vehicle 100 of Embodiment 1 described above, therefore, the same reference numerals are used for the same parts and descriptions are omitted.

[0125] In the work vehicle 100 of this embodiment, the control device 150 has a position and posture calculation function F100', which is the same as the calculation function F100 in Embodiment 1 described above. Furthermore, in the work vehicle 100 of this embodiment, the control device 150 replaces the status monitoring function F200 and has a driving control function F300. In the control device 150 of this embodiment, the calculation function F100' outputs to the driving control function F300 an estimation result of the vehicle 110's position and posture obtained based on the estimation function F110, and a detection result of normal driving obtained based on the detection function F106.

[0126] The driving control function F300 executes driving control of the vehicle 110 based on the estimation and detection results input from the calculation function F100'. More specifically, the driving control function F300 controls various actuators of the vehicle 110, automatically operating the accelerator pedal, brake pedal, transmission, steering wheel, etc., of the vehicle 110 to drive the vehicle 110. Furthermore, in the work vehicle 100 of this embodiment, the control device 150, for example, uses the detection function F106 instead of... Figure 3 as well as Figure 4 The process shown is P4, which is the normal driving detection process. Figure 16 The detection process shown is P4', which is a normal driving process.

[0127] Figure 16 This is a flowchart illustrating an example of the processing P4' performed by the detection function F106 of the control device 150 based on this embodiment. Figure 16 In the processing shown in P4', for the case of... Figure 6 The same processing method as shown in Implementation 1 (P4) is labeled with the same reference numerals and the description is omitted.

[0128] Figure 16 In process P402 of process P4' shown, if the control device 150 determines that the average speed of the vehicle 110 is below the predetermined speed threshold Vth (No), then, for example, it executes process P411, which sets the speed deficiency flag stored in the storage device to 1, through detection function F106. This speed deficiency flag setting to 1 indicates that the calculation of the direction correction parameters cannot be performed because the vehicle 110's speed is insufficient. After process P411 is completed, the control device 150, similarly to Embodiment 1 described above, executes process P408, which sets the normal driving flag to 0.

[0129] in addition, Figure 16In process P406 of process P4' shown, if the control device 150 determines that the driving state of the vehicle 110 is not normal driving (no), then, for example, through the detection function F106, it executes process P412, which sets the non-normal flag stored in the storage device to 1. The state where the non-normal flag is set to 1 indicates that the driving state of the vehicle 110 is not normal driving and the direction correction parameters cannot be calculated. After process P412 is completed, the control device 150 executes process P408, which sets the normal driving flag to 0, in the same way as in embodiment 1 described above.

[0130] Furthermore, the initial values ​​of the insufficient speed indicator and the abnormal behavior indicator are 0, and are initialized simultaneously with the start of each sample cycle of the control device 150. If the insufficient speed indicator or the abnormal behavior indicator is set to 1, the direction correction parameters cannot be calculated, and the estimation error of the posture of the work vehicle 100 becomes larger. Therefore, in this embodiment, the control device 150 performs driving control of the vehicle 110 by using the driving control function F300 to make the driving state of the vehicle 110 normal driving.

[0131] Figure 17 This is a flowchart illustrating an example of the processing P15 performed by the driving control function F300 of the control device 150 in the work vehicle 100 based on this embodiment. If the control device 150 starts... Figure 17 The process P15 shown first executes the process P1501, which retrieves the insufficient speed flag and the non-standard flag stored in the storage device.

[0132] Next, the control device 150 executes process P1502, which determines whether the insufficient speed indicator is 1. In process P1502, if the control device 150 determines that the insufficient speed indicator is 1 (yes), it can determine that the vehicle 110 is in a state where the speed is insufficient and the direction correction parameters cannot be calculated. In this case, the control device 150 executes process P1503, which sets the driving control parameters to accelerate the vehicle 110 to a speed exceeding the speed threshold Vth. After process P1503 is completed, the control device 150 executes process P1509, which determines whether there is any blocking as described later.

[0133] On the other hand, in process P1502, if the control device 150 determines that the speed insufficient indicator is not 1 (no), then process P1504, which determines whether the abnormal indicator is 1, is executed. In process P1504, if the control device 150 determines that the abnormal indicator is not 1 (no), then it can determine that the driving state of the vehicle 110 is in normal driving where the direction correction parameters can be calculated, and therefore process P1509, which determines whether there is an obstruction as described later, is executed.

[0134] On the other hand, in process P1504, if the control device 150 determines that the abnormal flag is 1 (yes), it executes process P1505, which determines whether the speed of vehicle 110 is higher than the prescribed speed threshold Vth'. In process P1505, if the control device 150 determines that the speed of vehicle 110 is higher than the speed threshold Vth' (yes), it executes process P1506, which sets the driving control parameters to decelerate vehicle 110 in a way that brings the speed of vehicle 110 into the normal driving speed range. After process P1506 is completed, the control device 150 executes process P1509, which determines whether there is a blocking mechanism described later.

[0135] On the other hand, in process P1505, if the control device 150 determines that the speed of the vehicle 110 is below the predetermined speed threshold Vth' (no), it can determine that there is no room for adjustment of the speed of the vehicle 110. In this case, the control device 150 executes process P1507, which determines whether the absolute value |α| of the angular velocity of the vehicle 110 is lower than the predetermined angular velocity threshold αth. In process P1507, if the control device 150 determines that the absolute value |α| of the angular velocity of the vehicle 110 is lower than the predetermined angular velocity threshold αth (yes), it can determine that the vehicle 110 is traveling in a straight line. In this case, the control device 150 executes process P1508, which sets the parameters for maintaining the steering wheel angle for driving control. After process P1508 is completed, the control device 150 executes process P1509, which determines whether there is any blocking as described later.

[0136] On the other hand, in process P1507, if the control device 150 determines that the absolute value |α| of the angular velocity of the vehicle 110 is greater than or equal to the predetermined angular velocity threshold αth (no), then it executes process P1509 to determine whether there is an obstruction. In this process P1509, the control device 150 determines whether an obstruction signal to stop control has been input from another control device, and whether there is an obstruction based on the operation of the vehicle 110 performed by the operator of the vehicle 110.

[0137] In process P1509, if the control device 150 determines that there is no obstruction (no), it executes process P1510, which controls the driving of the vehicle 110 based on the parameters set in processes P1503, P1506, or P1508, and then ends the process. Figure 17 The process P15 is shown. On the other hand, in process P1509, if the control device 150 determines that there is a blockage (yes), it executes process P1511, which stops the driving control of the vehicle 110 based on the parameters set in processes P1503, P1506, or P1508, and ends the process. Figure 17 The processing shown is P15.

[0138] As described above, in the work vehicle 100 of this embodiment, the control device 150 has a driving control function F300 for controlling the movement of the vehicle 110. This driving control function F300 controls the vehicle 110 to meet the conditions for normal driving when the detection function F106 does not detect normal driving of the vehicle 110. With this configuration, the work vehicle 100 according to this embodiment not only achieves the same effect as the work vehicle 100 of Embodiment 1, but also allows the control device 150 to control the driving state of the vehicle 110 to normal driving, and more reliably calculates the direction correction parameters.

[0139] The embodiments of the work vehicle of the present invention have been described in detail above with reference to the accompanying drawings. However, the specific configuration is not limited to these embodiments. Even design changes that do not depart from the spirit of the present invention are included in the present invention.

[0140] Explanation of reference numerals in the attached figures

[0141] 100 work vehicles

[0142] 110 vehicles

[0143] 121 First Antenna

[0144] 122 Antenna 2

[0145] 123 Receiver

[0146] 130 sensor

[0147] 134 Lifting Sensor

[0148] 140 cargo bays

[0149] 142 Lifting hydraulic cylinder (lifting mechanism)

[0150] 150 Control device

[0151] Ct Center

[0152] d Baseline direction

[0153] Dv1 First Vehicle Direction

[0154] Dv2 Second Vehicle Direction

[0155] F104 function to calculate the direction of the first vehicle

[0156] F105 calculates the direction of the second vehicle.

[0157] F106 Detection Function

[0158] F107 function to calculate direction correction parameters

[0159] F109 Function to determine if the direction correction parameter can be used.

[0160] F110 Presumed Function

[0161] F300 Driving Control Functions

[0162] Hth height threshold

[0163] P Location information.

Claims

1. A working vehicle, comprising: A vehicle; a first antenna and a second antenna installed on the vehicle to receive radio waves from a satellite positioning system; and a receiver that outputs the position information of the first antenna obtained based on the radio waves and the baseline direction between the first antenna and the second antenna. The vehicle is characterized by sensors for measuring the speed, acceleration, and angular velocity of the vehicle, and a control device for estimating the position and attitude of the vehicle. The control device has the following functions: The detection function detects normal driving based on the speed, acceleration, and angular velocity. Normal driving includes turning driving along a circle with the same center and radius, and straight driving along a straight line, and the average speed is higher than a specified speed threshold. The function of calculating the direction of the first vehicle based on the position information of the first antenna output from the receiver, the baseline direction, and the setting information of the first antenna and the second antenna relative to the vehicle; When straight-line driving is detected as normal driving, the movement vector calculated based on the difference between the current position information of the first antenna and the position information of the first antenna one cycle ago is set as the second vehicle direction; when turning driving is detected as normal driving, the turning center and rotation center of the vehicle in the turning driving are calculated based on the current position information of the first antenna and the position information of the first antenna one cycle ago, and the second vehicle direction is calculated based on the calculated turning center and rotation center of the vehicle. The function of calculating the direction correction parameters for correcting the calculated first vehicle direction based on the calculated second vehicle direction; and The function of estimating the position and attitude of the vehicle based on the direction correction parameters and the first vehicle direction.

2. The working vehicle according to claim 1, characterized in that, The control device also has the function of determining whether the direction correction parameters can be used. The estimation function, when the direction correction parameter is available, estimates the position and attitude of the vehicle based on the direction correction parameter and the first vehicle direction.

3. The operating vehicle according to claim 1, characterized in that, The vehicle includes a cargo bed installed on the vehicle, a lifting mechanism for raising and lowering the cargo bed, and a lifting sensor for detecting the height of the cargo bed. The term "normal driving" includes situations where the height of the cargo bed is lower than a specified height threshold.

4. The working vehicle according to claim 1, characterized in that, It has an information notification device for notifying operators. When the direction correction parameter exceeds a predetermined threshold, the control device outputs error information regarding the setting information of the first antenna and the second antenna to the information notification device.

5. The operating vehicle according to claim 1, characterized in that, The control device has a driving control function to control the movement of the vehicle. If the driving control function does not detect the normal driving conditions through the detection function, it controls the vehicle to meet the normal driving conditions.

Images