WO2021193269A1 - 作業車両 - Google Patents

作業車両 Download PDF

Info

Publication number
WO2021193269A1
WO2021193269A1 PCT/JP2021/010748 JP2021010748W WO2021193269A1 WO 2021193269 A1 WO2021193269 A1 WO 2021193269A1 JP 2021010748 W JP2021010748 W JP 2021010748W WO 2021193269 A1 WO2021193269 A1 WO 2021193269A1
Authority
WO
WIPO (PCT)
Prior art keywords
vehicle
antenna
control device
function
work vehicle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/JP2021/010748
Other languages
English (en)
French (fr)
Inventor
幹雄 板東
信一 魚津
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hitachi Construction Machinery Co Ltd
Original Assignee
Hitachi Construction Machinery Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hitachi Construction Machinery Co Ltd filed Critical Hitachi Construction Machinery Co Ltd
Priority to US17/782,036 priority Critical patent/US12105523B2/en
Priority to EP21775765.7A priority patent/EP4130801B1/en
Priority to CN202180006899.6A priority patent/CN114787661B/zh
Publication of WO2021193269A1 publication Critical patent/WO2021193269A1/ja
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/23—Testing, monitoring, correcting or calibrating of receiver elements
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/52—Determining velocity
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/20—Control system inputs
    • G05D1/24—Arrangements for determining position or orientation
    • G05D1/245—Arrangements for determining position or orientation using dead reckoning
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/20—Control system inputs
    • G05D1/24—Arrangements for determining position or orientation
    • G05D1/247—Arrangements for determining position or orientation using signals provided by artificial sources external to the vehicle, e.g. navigation beacons
    • G05D1/248—Arrangements for determining position or orientation using signals provided by artificial sources external to the vehicle, e.g. navigation beacons generated by satellites, e.g. GPS
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2105/00—Specific applications of the controlled vehicles
    • G05D2105/20—Specific applications of the controlled vehicles for transportation
    • G05D2105/28—Specific applications of the controlled vehicles for transportation of freight
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2107/00—Specific environments of the controlled vehicles
    • G05D2107/70—Industrial sites, e.g. warehouses or factories
    • G05D2107/73—Mining
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2109/00—Types of controlled vehicles
    • G05D2109/10—Land vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/50—Internal signals, i.e. from sensors located in the vehicle, e.g. from compasses or angular sensors
    • G05D2111/52—Internal signals, i.e. from sensors located in the vehicle, e.g. from compasses or angular sensors generated by inertial navigation means, e.g. gyroscopes or accelerometers
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/60—Combination of two or more signals
    • G05D2111/63—Combination of two or more signals of the same type, e.g. stereovision or optical flow
    • G05D2111/65—Combination of two or more signals of the same type, e.g. stereovision or optical flow taken successively, e.g. visual odometry or optical flow

Definitions

  • This disclosure relates to work vehicles.
  • the directional / posture measuring device described in Patent Document 1 comprises a carrier phase integer value bias determining device and a means for obtaining the directional or posture (claim 5, etc.).
  • the carrier phase integer value bias determining device includes first and second antennas, antenna orientation observing means, and means for calculating, correcting, and determining (claim 4 and the like).
  • the antenna orientation observing means receives positioning signals from a plurality of positioning transmitters by the first and second antennas, respectively, and is a second antenna with respect to the first antenna based on the amount of carrier phase change of the positioning signals. Find the orientation of the antenna.
  • the means for making the above calculations, corrections, and decisions operates as follows. First, the calculated phase difference, which is the calculated carrier phase difference obtained from the orientation of the second antenna with respect to the first antenna, the distance between the first and second antennas, and the position of the positioning transmitter, is calculated. Further, the observation phase difference, which is a fractional part of the carrier phase difference of the positioning signal received by the first and second antennas, is obtained, and the fractional part of the calculated phase difference is the observation position within a range of ⁇ 0.5 cycles. The calculated phase difference is modified to match the phase difference. Then, the corrected calculated phase difference is determined as an integer bias of the observed phase difference (claim 4 and the like).
  • the means for obtaining the orientation or attitude is to obtain the relative position of the second antenna with respect to the first antenna from the integer value bias obtained by the carrier phase integer value bias determining device and the observed phase difference, and from the relative position.
  • the orientation or orientation between the first and second antennas is obtained (claim 5 and the like).
  • the first and second antennas are provided on a moving body traveling in a substantially horizontal plane in the front-rear direction, and the antenna directional observation means is accompanied by the movement of the moving body.
  • the traveling direction of the moving body is observed based on the carrier phase change amount of the positioning signal due to the movement of the second antenna.
  • this device considers the traveling direction as the front direction of the moving body, and considers the front direction of the moving body and the mounting positional relationship of the first and second antennas with respect to the moving body, so that the second antenna with respect to the first antenna (Claim 7 etc.).
  • a work vehicle such as a dump truck receives radio waves of a satellite positioning system such as a global positioning satellite system (GNSS) in order to measure the direction of the vehicle, similarly to the above-mentioned conventional orientation / attitude measuring device.
  • GNSS global positioning satellite system
  • a second antenna is installed, for example, at the tip of a pole extending upward from the vehicle body in order to avoid blocking radio waves by the vehicle body.
  • the work vehicle may vibrate or shock the antenna by loading ore, earth and sand, etc., unloading the load, or running on uneven ground. Therefore, with the passage of time, an error occurs between the actual installation position of the antenna and the preset installation position, and the direction of the vehicle calculated based on the set installation position and the actual installation position. There may be an error with the direction of the vehicle.
  • the conventional orientation / attitude measuring device observes the traveling direction of the moving body based on the movement of the first and second antennas accompanying the movement of the moving body traveling in the substantially horizontal plane in the front-rear direction. ..
  • the opportunity for the work vehicle to travel in the substantially horizontal plane in the front-rear direction is limited. Therefore, in the work vehicle, it is required to detect the error of the antenna installation position more flexibly instead of such a limited condition.
  • the present disclosure provides a work vehicle capable of detecting an error in the installation position of an antenna more flexibly than a conventional device.
  • One aspect of the present disclosure includes a vehicle, a first antenna and a second antenna attached to the vehicle to receive radio waves of a satellite positioning system, position information of the first antenna based on the radio waves, and the first antenna.
  • a work vehicle including a receiver that outputs a baseline direction connecting the second antenna, a sensor that measures the speed, acceleration, and angular speed of the vehicle, and a control device that estimates the position and orientation of the vehicle. Therefore, the control device has a detection function for detecting steady running based on the speed, the acceleration, and the angular speed, and the first vehicle based on the installation information of the first antenna and the second antenna with respect to the vehicle.
  • a work vehicle having a function of calculating a direction correction parameter for correcting the first vehicle direction, and an estimation function of estimating the position and the attitude of the vehicle based on the direction correction parameter and the first vehicle direction. Is.
  • FIG. 5 is a flow chart showing an example of processing by the control device of the work vehicle of FIG.
  • FIG. 3 is a flow chart of a process for detecting steady running in FIG.
  • the schematic diagram which shows the relationship between the baseline direction and the vehicle direction between the 1st and 2nd antennas.
  • FIG. 3 is a flow chart of a process for calculating the first vehicle direction in FIG.
  • FIG. 3 is a flow chart of a process for calculating the second vehicle direction in FIG.
  • the schematic diagram which shows an example of the turning included in the steady running.
  • the schematic diagram explaining the rotation center of a vehicle.
  • the flow chart of the process of calculating the direction correction parameter of FIG. The schematic diagram explaining the direction correction parameter.
  • the functional block diagram of the control device in Embodiment 2 of the work vehicle which concerns on this disclosure.
  • FIG. 5 is a flow chart showing an example of processing by the steady running detection function of the control device of FIG.
  • FIG. 5 is a flow chart showing an example of processing by the traveling control function of the control device of FIG.
  • FIG. 1 is a side view showing the first embodiment of the work vehicle according to the present disclosure.
  • FIG. 2 is a functional block diagram of the control device 150 of the work vehicle 100 of FIG.
  • the work vehicle 100 of the present embodiment is, for example, a dump truck used at an ore mining site or a construction site.
  • the work vehicle 100 includes, for example, a vehicle 110, a positioning device 120, a sensor 130, a vessel 140, and a control device 150.
  • the vehicle 110 includes, for example, a body frame 111, wheels 112, and a cabin 113.
  • the body frame 111 is, for example, a ladder-shaped structure.
  • the body frame 111 supports, for example, the left and right wheels 112 attached to the axle via suspension. Further, the vehicle body frame 111 supports, for example, an engine, a generator, a motor, a power transmission mechanism, a steering mechanism, a hydraulic device, an actuator for vehicle control, and the like (not shown).
  • the wheels 112 are connected to a motor via, for example, a power transmission mechanism, and are driven by the motor to drive the vehicle 110.
  • the cabin 113 is a passenger compartment for the operator of the work vehicle 100 to board. Inside the cabin 113, for example, a steering wheel (not shown), an operation pedal, an operation lever, an information device, a speaker, an instrument, an indicator lamp, and the like are installed.
  • the positioning device 120 can be configured by, for example, a satellite positioning system such as a global positioning satellite system (GNSS).
  • the positioning device 120 includes, for example, a first antenna 121, a second antenna 122, and a receiver 123.
  • the first antenna 121 and the second antenna 122 are attached to the vehicle 110, for example, and receive radio waves of a satellite positioning system such as GNSS.
  • the first antenna 121 and the second antenna 122 are installed apart from each other in the width direction of the vehicle 110, which is orthogonal to the front-rear direction and the height direction of the vehicle 110, for example.
  • the first antenna 121 and the second antenna 122 are attached to, for example, the tip of a pole fixed to the vehicle 110, and are installed at a position higher than the steady position of the front end portion 141a of the vessel main body 141.
  • the receiver 123 is connected to, for example, the first antenna 121 and the second antenna 122 via a signal cable.
  • the 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.
  • the baseline direction is, for example, the direction of a straight line connecting the installation position of the first antenna 121 and the installation position of the second antenna 122.
  • the sensor 130 includes, for example, a speed sensor 131, an acceleration sensor 132, and an angular velocity sensor 133. Further, in the examples shown in FIGS. 1 and 2, the sensor 130 includes, for example, an elevating sensor 134. These sensors 130 are communicably connected to the control device 150, for example, via a control area network (CAN). When the speed of the vehicle 110 is calculated by the control device 150 based on the position measured by the positioning device 120, the positioning device 120 may be used as the speed sensor and the speed sensor 131 may be omitted.
  • CAN control area network
  • the speed sensor 131 detects the speed of the vehicle 110 based on, for example, the rotation speed of the wheels 112, and outputs the speed to the control device 150.
  • the acceleration sensor 132 detects, for example, the acceleration of the vehicle 110 excluding the gravitational acceleration, and outputs the acceleration to the control device 150.
  • the angular velocity sensor 133 detects, for example, the angular velocity of the vehicle 110 and outputs the angular velocity to the control device 150.
  • the elevating sensor 134 detects, for example, the elevating state of the vessel main body 141 including the stroke of the elevating cylinder 142 that elevates and elevates the vessel main body 141, and outputs the elevating state to the control device 150.
  • the vessel 140 has, for example, a vessel main body 141, an elevating cylinder 142, and a rotating shaft 143.
  • the vessel body 141 is rotatably supported on the vehicle body frame 111 around a rotation shaft 143 provided on the rear side of the bottom, for example.
  • the vessel main body 141 is a part for loading and transporting a load such as ore, rock, gravel, and earth and sand in the work vehicle 100, for example.
  • the elevating cylinder 142 is, for example, a pair of hydraulic cylinders provided on both sides of the vehicle 110 in the width direction.
  • the tip of the piston rod is connected to the front of the rotating shaft 143 at the bottom of the vessel body 141, and the end of the cylinder tube on the opposite side of the piston rod is connected to the lower part of the vehicle body frame 111.
  • the elevating cylinder 142 constitutes a part of the hydraulic system controlled by the control device 150.
  • the elevating cylinder 142 rotates the vessel main body 141 around the rotation shaft 143 by expanding and contracting the piston rod, and raises and lowers the front end portion 141a of the vessel main body 141.
  • the control device 150 is, for example, a computer system such as a microcontroller or firmware.
  • the control device 150 is composed of, for example, a central processing unit (CPU), a storage device such as a RAM or ROM, a program and data stored in the storage device, a timer, and an input / output unit for inputting / outputting signals. .. Since the control device 150 and the sensor 130 are time-synchronized with each other and operate at a fixed cycle, for example, signal loss due to a time lag is prevented. Further, the drive cycle of the control device 150 can be made equal to, for example, the shortest output cycle of the sensor 130.
  • the control device 150 has, for example, a function F100 for calculating the position and posture of the work vehicle 100 and a function F200 for monitoring the state of the work vehicle 100 as shown in FIG. These functions of the control device 150 can be realized, for example, by executing the program stored in the storage device of the control device 150 by the CPU.
  • the function F100 for calculating the position and posture of the work vehicle 100 may be abbreviated as the calculation function F100
  • the function F200 for monitoring the state of the work vehicle 100 may be abbreviated as the state monitoring function F200.
  • the calculation function F100 of the control device 150 includes, for example, a function F101 for storing antenna installation information, a function F102 for storing position and direction information, a function F103 for storing sensor information, and directions. It has a function F108 for storing correction parameters.
  • these functions of the control device 150 may be abbreviated as storage functions F101, F102, F103, and F108, respectively.
  • the calculation function F100 of the control device 150 further includes a function F104 for calculating the first vehicle direction, a function F105 for calculating the second vehicle direction, and a function F106 for detecting steady running. , Has a function F107 for calculating the direction correction parameter.
  • these functions of the control device 150 may be abbreviated as calculation functions F104 and F105, detection function F106, and calculation function F107, respectively.
  • the calculation function F100 of the control device 150 further has a function F109 for determining whether or not the direction correction parameter can be used, and a function F110 for estimating the position and posture of the work vehicle 100.
  • these functions of the control device 150 may be abbreviated as a determination function F109 and an estimation function F110, respectively.
  • FIG. 3 is a flow chart showing an example of processing by the control device 150 of the work vehicle 100 of FIG.
  • the control device 150 executes the process shown in FIG. 3 by, for example, the calculation function F100.
  • the storage function F103 executes the process P1 for storing the sensor information.
  • the storage function F103 stores the sensor information input from the sensor 130 via the input / output unit of the control device 150 in the storage device constituting the control device 150.
  • FIG. 4 is a table T1 showing an example of sensor information stored by the storage function F103 in the process P1.
  • the sensor information includes, for example, a velocity v, an acceleration a, an angular velocity w, and an ascending / descending state h, which are output values from each of the velocity sensor 131, the acceleration sensor 132, the angular velocity sensor 133, and the elevating sensor 134 included in the sensor 130. ..
  • the sensor information includes, for example, the time t, t-1, ..., Tm, ... When each output value is output.
  • the storage device of the control device 150 includes, for example, sensor information at the current time t and sensor information at times t-1, ..., Tm, ... Before the current time t, and sensor information for a certain period of time. I remember.
  • the position information of the first antenna 121 and the baseline direction connecting the first antenna 121 and the second antenna 122 are from the receiver 123.
  • the period is longer than the output cycle.
  • the speed, acceleration, and angular velocity included in the sensor information may be the output values of the speed sensor 131, the acceleration sensor 132, and the angular velocity sensor 133, and the output values of each sensor are converted into three-dimensional notation in vehicle coordinates. It may be the value obtained.
  • the control device 150 determines whether, for example, the position information of the first antenna 121 and the baseline direction connecting the first antenna 121 and the second antenna 122 are both acquired from the receiver 123.
  • Process P2 is executed.
  • the position information of the first antenna 121 is the position information of the first antenna 121 that is output from the receiver 123 as a result of the signal being input from the first antenna 121 that has received the GNSS radio wave to the receiver 123. ..
  • the baseline direction for example, signals are input from the first antenna 121 and the first antenna 121 that have received the GNSS radio wave to the receiver 123, and as a result, the first antenna 121 and the second antenna are output from the receiver 123. This is the direction of the straight line connecting the antenna 122.
  • the storage function F102 of the control device 150 acquires, for example, the position information of the first antenna 121 and the baseline direction from the receiver 123 at regular intervals and stores them in the storage device of the control device 150. .. Therefore, in the process P2, for example, whether or not the position information of the first antenna 121 and the baseline direction connecting the first antenna 121 and the second antenna 122 are both acquired from the receiver 123 in the control device 150. To judge.
  • the position information of the first antenna 121 is output from the receiver 123 in a cycle of 1 [Hz], and the baseline direction connecting the first antenna 121 and the second antenna 122 is 10 [Hz]. It is assumed that the output is periodic.
  • the control device 150 determines in the process P2 that the position information of the first antenna 121 was not acquired (NO) at the time when only the baseline direction was acquired from the receiver 123, and the process P13 described later. To execute.
  • the control device 150 determines in the process P2 that both the position information of the first antenna 121 and the baseline direction are acquired (YES)
  • the control device 150 executes the next process P3.
  • FIG. 5 is a table T2 showing an example of the position information p and the baseline direction d of the first antenna 121 stored in the storage device of the control device 150 in the process P3.
  • the control device 150 obtains the position information p of the first antenna 121 acquired from the receiver 123 and the baseline direction d by the storage function F102 at the acquired times t, tm, ..., T-. It is stored in the storage device together with 6 m, ....
  • FIG. 5 shows an example in which the position information p of the first antenna 121 and the baseline direction d are output together for each period m.
  • the position information p of the first antenna 121 is, for example, information indicating a point on a map, and is two-dimensional or three-dimensional position information.
  • 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 an orientation.
  • the control device 150 executes the next process P4.
  • FIG. 6 is a flow chart of the process P4 for detecting the steady running of FIG.
  • Each process shown in FIG. 6 can be executed by, for example, the detection function F106 of the control device 150.
  • the control device 150 starts the process P4, the control device 150 first executes the process P401 for calculating the average speed.
  • the control device 150 refers to, for example, the storage device, and refers to the time when the position information p of the first antenna 121 one cycle before is acquired from the positioning device 120.
  • the control device 150 calculates the average speed of the vehicle 110 by using, for example, the speed v of the vehicle 110 from the time when the position information p of the first antenna 121 one cycle ago was acquired to the present.
  • the control device 150 executes the process P402 for determining whether or not the calculated average speed of the vehicle 110 is higher than the predetermined speed threshold value Vth.
  • the control device 150 determines that the calculated average speed of the vehicle 110 is equal to or less than a predetermined speed threshold value Vth (NO)
  • the control device 150 executes the next process P408.
  • the control device 150 sets the steady running flag stored in the storage device to 0, and ends the process shown in FIG.
  • the state in which the steady-state running flag is 0 indicates that the work vehicle 100 is not in steady-state running. That is, the steady running includes that the average value of the speeds of the vehicles 110 is higher than the predetermined speed threshold value Vth. As a result, while the vehicle 110 is stopped and low-speed traveling below the speed threshold value Vth is excluded from steady traveling.
  • the control device 150 determines, for example, that the calculated average speed of the vehicle 110 is higher than the predetermined speed threshold value Vth (YES)
  • the control device 150 executes the next process P403.
  • the control device 150 calculates, for example, the height of the vessel 140.
  • the control device 150 refers to, for example, a storage device, and the height of the vessel 140 is based on the current time and the ascending / descending state of the vessel 140 at the time when the output of the positioning device 120 one cycle before is acquired. Calculate the time.
  • the height of the vessel 140 is, for example, the height of the front end portion 141a of the vessel main body 141.
  • the control device 150 can obtain the height of the front end portion 141a of the vessel main body 141 by using, for example, the sine of the angle of the vessel main body 141.
  • the dimensional data of the vessel main body 141 is stored in advance in the storage device of the control device 150, for example. After the end of the process P403, the control device 150 executes the next process P404.
  • the control device 150 determines, for example, whether or not the height of the vessel 140 calculated in the process P403 is lower than the predetermined height threshold value Hth. More specifically, in the control device 150, for example, whether or not the height of each vessel 140 at the current time and the time when the position information p is acquired from the previous positioning device 120 is lower than the height threshold value Hth. Is determined.
  • the height threshold value Hth is set to, for example, a height equal to the height of the first antenna 121 or the second antenna 122, or a lower height of the first antenna 121 and the second antenna 122. be able to.
  • the control device 150 has, for example, the current time and the time when the position information p is acquired from the previous positioning device 120, or one of the heights of the vessel 140 is equal to or higher than the height threshold value Hth (). It is assumed that it is determined as NO). In this case, the control device 150 executes the process P408 for setting the steady running flag to 0, and ends the process P4 shown in FIG.
  • the height of the vessel 140 is lower than the height threshold value Hth of the control device 150, for example, at both or one of the current time and the time when the position information p is acquired from the previous positioning device 120. It is assumed that (YES) is determined.
  • the control device 150 executes, for example, the process P405 for calculating the traveling state of the work vehicle 100. More specifically, in the process P405, the control device 150 calculates the average values of the angular velocity, the angular acceleration, and the acceleration of the work vehicle 100 and the change in the direction of the vehicle 110.
  • the average values of the angular velocity, the angular acceleration, and the acceleration of the work vehicle 100 are, for example, the average values from the time when the position information p was acquired from the previous positioning device 120 to the current time.
  • the angular acceleration of the work vehicle 100 can be calculated, for example, from the difference between the angular velocity of the work vehicle 100 at a certain time and the angular velocity of the work vehicle 100 at the time one cycle before the angular velocity.
  • the direction of the vehicle 110 that is, the change in the vehicle direction can be calculated as follows, for example.
  • FIG. 7 is a schematic diagram showing the relationship between the baseline direction d connecting the first antenna 121 and the first antenna 121 and the vehicle direction Dv.
  • the work vehicle 100 has an angle ⁇ along the circumference of the vehicle 110 centered on the center Ct between the time when the position information p was acquired from the previous positioning device 120 and the current time. Only turning and moving.
  • the vehicle direction Dv changes at an angle equal to the angle ⁇ at which the vehicle 110 turns.
  • the baseline direction d that is, the baseline vector, also changes at an angle equal to the angle ⁇ at which the vehicle 110 turns.
  • the angle change of the vehicle direction Dv can be calculated by calculating the angle change of the baseline direction d between the current time and the time when the position information p is acquired from the previous positioning device 120. That is, in the process P405, the control device 150 calculates the angle change in the vehicle direction Dv by, for example, calculating the angle change in the baseline direction d. As described above, in the process P405, the control device 150 calculates, for example, the average values of the angular velocity, the angular acceleration, and the acceleration of the work vehicle 100, and the angular change of the vehicle direction Dv.
  • the control device 150 executes, for example, the process P406 for determining whether or not the traveling state of the work vehicle 100 is steady traveling.
  • the steady running is, for example, a state in which the work vehicle 100 is running in a constant motion state. More specifically, the steady running of the work vehicle 100 includes, for example, turning in which the work vehicle 100 travels along a circumference having the same center and radius, and straight travel in which the work vehicle 100 travels in a straight line.
  • the steady running is not limited to turning and going straight, and may include other running states.
  • the control device 150 is centered on the work vehicle 100 when, for example, the difference between the change in the vehicle direction Dv calculated in the previous process P405 and the time-minute integration of the angular velocity of the vehicle 110 is equal to or less than the threshold value. It is determined that the vehicle is turning along the same circumference. In this case, since the turning of the work vehicle 100 is included in the steady running, the control device 150 determines in the process P406 that the running state of the work vehicle 100 is the steady running (YES), and executes the next process P407. do.
  • the control device 150 is traveling straight in which the work vehicle 100 travels in a straight line when, for example, the average value of the angular acceleration and the acceleration of the vehicle 110 calculated in the previous process P405 is equal to or less than the threshold value. Is determined. In this case, since the straight running of the work vehicle 100 is included in the steady running, the control device 150 determines in the process P406 that the running state of the work vehicle 100 is the steady running (YES), and executes the next process P407. do.
  • the control device 150 determines that the running state of the work vehicle 100 is not steady running (NO), executes the process P408 for setting the above-mentioned steady running flag to 0, and is shown in FIG. The process P4 shown is terminated.
  • the control device 150 determines whether or not the absolute value
  • the control device 150 executes the process P409 for setting the steady running flag stored in the storage device to 1. , The process P4 shown in FIG. 6 is terminated.
  • the state in which the steady running flag is 1 indicates that the running state of the work vehicle 100 is traveling straight.
  • the control device 150 determines that the absolute value
  • the control device 150 executes the process P410 for setting the steady running flag stored in the storage device to 2.
  • the process P4 shown in FIG. 6 is terminated.
  • the state where the steady running flag is 2 indicates that the running state of the work vehicle 100 is a turn that runs along a circumference having the same center and radius.
  • the control device 150 executes the process P5 shown in FIG.
  • the control device 150 determines, for example, whether or not the traveling state of the work vehicle 100 is steady traveling by the detection function F106. Specifically, the control device 150 refers to the steady-state traveling flag set in the previous process P4 and stored in the storage device by the detection function F106. When the referenced steady-state travel flag is 0, it indicates that the work vehicle 100 is not in steady-state travel. Therefore, the control device 150 determines that the work vehicle 100 is not in steady-state travel (NO) by the detection function F106, and determines that the position is not steady-state. And the process P13 for estimating the posture is executed.
  • the control device 150 uses the direction correction parameter stored in the storage device through the previous processes P6 to P12 in the process P13.
  • the position and posture of the work vehicle 100 are estimated.
  • the process P13 for estimating the position and posture of the work vehicle 100 will be described in detail below after the processes P6 to P12 are described.
  • the process P5 when the referenced steady-state running flag is 1 or 2, it indicates that the work vehicle 100 is in steady-state running, so that the control device 150 is in steady-state running by the detection function F106 (YES). ), And the process P6 for calculating the first vehicle direction is executed.
  • the first vehicle direction is the direction of the vehicle 110 calculated based on the installation information of the first antenna 121 and the second antenna 122.
  • This installation information includes, for example, the coordinates of the positions where the first antenna 121 and the second antenna 122 are installed, and is stored in the storage device of the control device 150 by the storage function F101 of the control device 150 shown in FIG. ..
  • FIG. 8 is a flow chart of the process P6 for calculating the first vehicle direction.
  • the control device 150 starts the process P6 for calculating the first vehicle direction
  • the control device 150 first executes the process P601 for acquiring the direction correction parameter by the calculation function F104.
  • the control device 150 acquires, for example, the latest direction correction parameter stored in the storage device by the storage function F101 by the calculation function F104.
  • FIG. 9 is a schematic diagram illustrating the direction correction parameters acquired in the process P601 shown in FIG.
  • the first antenna 121 and the second antenna 122 are installed in the vehicle 110 and their positions are fixed. Based on the installation position of the first antenna 121 and the installation position of the second antenna 122 with respect to the vehicle 110, the vector Av from the first antenna 121 to the second antenna 122 can be obtained. Further, the direction of the center line of the vehicle 110 parallel to the front-rear direction of the vehicle 110 is defined as the vehicle direction Dv.
  • the installation information of the first antenna 121 and the second antenna 122 includes the antenna installation angle ⁇ , which is the angle formed by the vehicle direction Dv and the vector Av.
  • the direction correction parameter is a parameter for correcting the change when the antenna installation angle ⁇ changes for some reason.
  • the antenna installation angle ⁇ and the direction correction parameter are calculated in the same manner even if the installation positions of the first antenna 121 and the second antenna 122 are represented by the coordinates of the coordinate system fixed to the vehicle 110, for example, by performing coordinate conversion. It is possible.
  • the direction correction parameter is calculated by the calculation function F107 of the control device 150 and stored in the storage device by the storage function F108 in the process P8 described later.
  • the direction correction parameters for a certain period of time are stored together with the time.
  • 0 as an initial value is stored in the storage device of the control device 150.
  • the antenna installation angle ⁇ is not corrected.
  • the control device 150 executes the process P602 for calculating the first vehicle direction by, for example, the calculation function F104. Specifically, for example, the control device 150 uses the calculation function F104 to set the vehicle direction Dv by using the value obtained by subtracting the direction correction parameter from the antenna installation angle ⁇ shown in FIG. 9 and the vector Av shown in FIG. The calculation is performed, and the process P6 shown in FIG. 8 is terminated.
  • 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.
  • the control device 150 executes the process P7 for calculating the second vehicle direction by, for example, the calculation function F105, as shown in FIG.
  • FIG. 10 is a flow chart of the process P7 for calculating the second vehicle direction. Each process shown in FIG. 10 is executed by, for example, the calculation function F105 of the control device 150.
  • the control device 150 first executes the process P701 for calculating the difference between the antenna positions.
  • the control device 150 acquires the position information p of the first antenna 121 that is input from the receiver 123 of the positioning device 120 to the control device 150 and stored in the storage device by the storage function F102 of the control device 150.
  • the control device 150 acquires the current position information p of the first antenna 121 and the position information p of the first antenna 121 one cycle before.
  • the control device 150 calculates the moving direction 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 before. do. 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 before.
  • the control device 150 executes the process P702 for determining whether or not the steady running flag is 2. It is assumed that in the process P702, the control device 150 determines that the steady-state running flag is not 2 (NO), that is, the steady-state running flag is 1. In this case, the traveling state of the vehicle 110 is a straight-ahead traveling in a straight line, and the vehicle direction Dv and the traveling direction of the vehicle 110 coincide with each other. Therefore, the control device 150 executes the process P706 for setting the movement vector of the vehicle 110 calculated in the previous process P701 as the second vehicle direction, and ends the process P7 shown in FIG.
  • the control device 150 determines that the steady running flag is 2 (YES). In this case, the traveling state of the vehicle 110 is a turn traveling along a circle having the same center and radius. Therefore, the control device 150 executes the process P703 for calculating the position of the center of turning of the vehicle 110.
  • FIG. 11 is a schematic view showing an example of turning included in the steady running of the work vehicle 100.
  • FIG. 11 shows the position of the vehicle 110 that turns counterclockwise around the center Ct at the current time and the position at the time one cycle before.
  • the control device 150 first determines the vehicle at the time one cycle before, based on the position information p of the first antenna 121 at the time one cycle before and the position information p of the first antenna 121 at the current time.
  • the vehicle direction Dv of 110 is calculated.
  • the vehicle 110 is turning along a circle having the same center Ct and radius. Therefore, the rotation angle of the vehicle 110 around the center Ct between the time one cycle before and the current time is equal to the angle ⁇ between the vehicle direction Dv at the time one cycle before and the vehicle direction Dv at the current time. ..
  • the center Ct of the turn of the vehicle 110 is on the bisector of the movement vector Mv calculated by the above-mentioned process P701. Further, the distance D from the movement vector Mv to the center Ct of the turn can be obtained by the following equation (1), where L is the length of the movement vector Mv.
  • the control device 150 can calculate the center Ct of the turn by, for example, calculating the normal with respect to the movement vector Mv from the average of the angular velocities of the vehicle 110.
  • the control device 150 executes the process P704 for calculating the rotation center of the vehicle 110.
  • the control device 150 first calculates a vector from the position of the first antenna 121 toward the rotation center of the vehicle 110. Therefore, the control device 150 calculates a vector from the installation position of the first antenna 121 to the installation position of the acceleration sensor 132 and a vector from the installation position of the acceleration sensor 132 toward the rotation center of the vehicle 110.
  • FIG. 12 is a schematic view illustrating the rotation center Ctv of the vehicle 110.
  • the rotation center Ctv of the vehicle 110 is a point where the vehicle 110 is regarded as one rigid body and the direction of the speed vector Vv of the vehicle 110 and the vehicle direction Dv are equal to each other.
  • the direction of the speed vector Vv of the vehicle 110 is the normal direction of the line segment connecting the center Ct of the turning of the vehicle 110 and the center Ctv of the rotation of the vehicle 110.
  • the vector from the installation position of the acceleration sensor 132 toward the rotation center Ctv of the vehicle 110 can be calculated as follows.
  • the ax which is a component in the front-rear direction of the vehicle 110, is extracted from the average value of the acceleration of the vehicle 110 calculated in the above-mentioned process P4. Further, the average value wz of the angular velocity in the yaw direction and the deviation g from the vehicle direction Dv are used. As a result, the vector from the installation position of the acceleration sensor 132 toward the rotation center Ctv of the vehicle 110 can be obtained as (ax / (wz ⁇ wz), g).
  • a vector from the first antenna 121 toward the acceleration sensor 132 and a vector from the acceleration sensor 132 toward the rotation center Ctv of the vehicle 110 are obtained. Further, by adding these vectors, it is possible to obtain a vector from the first antenna 121 toward the rotation center Ctv of the vehicle 110.
  • the rear wheel axle center Cra can be defined as the rotation center of the vehicle 110 according to the Ackermann geometry.
  • the vector from the first antenna 121 to the rear wheel axle center Cra can be calculated in advance based on the specifications of the vehicle 110 and stored in the storage device of the control device 150.
  • the control device 150 uses the vector from the first antenna 121 toward the rotation center Ctv of the vehicle 110 and the vector in the baseline direction connecting the first antenna 121 and the second antenna 122 to the vehicle 110.
  • the rotation center Ctv of is calculated. Specifically, the vector from the first antenna 121 toward the rotation center Ctv of the vehicle body frame 111 is rotated around the position of the first antenna 121 one cycle before stored in the storage device of the control device 150 by the storage function F102. Let me.
  • the amount of rotation of the vector at this time is determined, for example, by calculating the direction of the first vehicle.
  • the first vehicle direction is, for example, the value obtained by subtracting the direction correction parameter from the antenna installation angle ⁇ shown in FIG. 9 and the first antenna 121 to the second antenna 122 one cycle before, as in the above-described processing P602. It can be obtained by using the vector Av toward. From the above, the control device 150 can calculate the rotation center Ctv of the vehicle 110 in the process P704.
  • control device 150 executes the process P705 for calculating the second vehicle direction.
  • the control device 150 calculates the second vehicle direction based on the turning center Ct of the vehicle 110 calculated in the above process P703 and the rotation center Ctv of the vehicle 110 calculated in the above process P704. Then, the process P7 shown in FIG. 10 is terminated.
  • the second vehicle direction calculated by this process P7 is the vehicle direction derived from the information output from the positioning device 120 receiver 123.
  • the second vehicle direction can be calculated as the normal direction obtained by the right-handed system with respect to the vector from the turning center Ct of the vehicle 110 to the rotation center Ctv of the vehicle 110.
  • the control device 150 executes the process P8 for calculating the direction correction parameter shown in FIG.
  • FIG. 13 is a flow chart of the process P8 for calculating the direction correction parameter.
  • FIG. 14 is a schematic diagram illustrating the process P8.
  • the correction angle ⁇ is, for example, the angle formed by the first vehicle direction Dv1 and the second vehicle direction Dv2.
  • the first vehicle direction Dv1 is the vehicle direction derived from the installation positions of the first antenna 121 and the second antenna 122
  • the second vehicle direction Dv2 is the first output from the receiver 123 of the positioning device 120. This is the vehicle direction derived from the position information p of the antenna 121.
  • the control device 150 calculates the difference between the first vehicle direction Dv1 and the second vehicle direction Dv2 by the calculation function F107, and the first vehicle direction Dv1. Is calculated to be equal to the second vehicle direction Dv2.
  • the control device 150 executes the process P802 for updating the direction correction parameter.
  • the control device 150 updates the direction correction parameter by adding the correction angle ⁇ calculated in the previous process P801 to the direction correction parameter stored in the storage device by the calculation function F107. do.
  • the control device 150 executes the process P803 for storing the direction correction parameter.
  • the control device 150 stores the direction correction parameter updated in the previous process P802 in the storage device by the storage function F108, and ends the process P8 shown in FIG.
  • the control device 150 executes, for example, the process P9 for determining whether or not the direction correction parameters have converged, as shown in FIG. 3, by the determination function F109.
  • the second vehicle direction Dv2 is also used in the process P704 for calculating the rotation center Ctv of the vehicle 110 shown in FIG. Therefore, the second vehicle direction Dv2 also has an error that cannot be completely removed by the previous direction correction parameter.
  • the control device 150 performs the convergence calculation in the process P9 by performing the calculation again using the latest direction correction parameters calculated in the previous process P8.
  • the control device 150 determines that the direction correction parameters have converged (YES), for example, when the absolute value of the correction angle ⁇ calculated in the process P801 shown in FIG. 13 is smaller than the preset threshold value. Then, the next process P10 is executed. On the other hand, in the process P9, when the absolute value of the correction angle ⁇ is, for example, equal to or greater than a preset threshold value, the control device 150 determines that the direction correction parameters have not converged (NO), and processes from the process P6. Repeat up to P9.
  • the control device 150 executes the process P10 for calculating the average value of the direction correction parameters by, for example, the determination function F109.
  • the direction correction parameter is calculated 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 for each sampling cycle. Therefore, an error also occurs in the direction correction parameter due to the position error that occurs when the receiver 123 performs the positioning calculation based on the GNSS radio wave received by the first antenna 121.
  • the control device 150 executes the sequential averaging of the direction correction parameter in the process P10.
  • the sequential averaging of the direction correction parameters for example, an upper limit of the number of integrations may be set or an oblivion coefficient may be introduced to suppress an excessive increase in the number of integrations. As a result, it is possible to prevent the change in the direction correction parameter from being reflected.
  • the control device 150 executes, for example, the process P11 for determining whether or not the direction correction parameter can be used by the determination function F109.
  • the direction correction parameters calculated at each time include an error, the average value of the direction correction parameters calculated in a predetermined period is calculated. Therefore, it is necessary that a certain number or more of the direction correction parameters are calculated and that the fluctuation of the direction correction parameters for each cycle is not excessive.
  • the control device 150 executes the process P12 for correcting the vehicle direction using the direction correction parameter, and the process P13 for estimating the position and attitude of the vehicle 110 by the estimation function F110 based on the corrected vehicle direction. Run.
  • the control device 150 determines that the direction correction parameter cannot be used (NO)
  • the control device 150 executes the process P13 for estimating the position and attitude of the vehicle 110 by the estimation function F110 without using the direction correction parameter. do.
  • the control device 150 outputs the average value of the direction correction parameters calculated in the process P10 and the position and attitude of the vehicle 110 estimated in the process P13 to the state monitoring function F200 via CAN in the process P14. Run. As a result, each process shown in FIG. 3 by the calculation function F100 of the control device 150 is completed.
  • condition monitoring function F200 of the control device 150 has an error in the installation position of the first antenna 121 or the second antenna 122 based on the installation information of the first antenna 121 and the second antenna 122 and the direction correction parameter. Detect what is happening. Further, the condition monitoring function F200 of the control device 150 determines, for example, an abnormality in the installation positions of the first antenna 121 and the second antenna 122 when the direction correction parameter exceeds a predetermined threshold value.
  • the work vehicle 100 may be provided with an information notification device for notifying the operator or the user of the information.
  • an information notification device for example, a liquid crystal display device, an indicator lamp, a speaker, a buzzer, or the like can be used.
  • the condition monitoring function F200 of the control device 150 outputs error information with respect to the installation information of the first antenna 121 and the second antenna 122 to the information notification device when the direction correction parameter exceeds a predetermined threshold value.
  • the operator or user of the work vehicle 100 can be notified of the abnormality.
  • the work vehicle 100 of the present embodiment includes a vehicle 110 and a first antenna 121 and a second antenna 122 that are attached to the vehicle 110 and receive radio waves of the satellite positioning system. Further, the work vehicle 100 includes a receiver 123 that outputs the position information p of the first antenna 121 based on the radio waves of the satellite positioning system and the baseline direction d between the first antenna 121 and the second antenna 122. There is. Further, the work vehicle 100 includes a sensor 130 for measuring the speed, acceleration, and angular velocity of the vehicle 110, and a control device 150 for estimating the position and posture of the vehicle 110.
  • 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 is a function of detecting steady running based on the speed, acceleration, and angular velocity of the vehicle 110.
  • the calculation function F104 is a function of calculating the first vehicle direction Dv1 based on the installation information of the first antenna 121 and the second antenna 122 with respect to the vehicle 110.
  • the calculation function F105 is a function of calculating the second vehicle direction Dv2 based on the time change of the position information p of the first antenna 121 when steady running is detected.
  • the calculation function F107 is a function of calculating a direction correction parameter for correcting the first vehicle direction Dv1 based on the second vehicle direction Dv2.
  • the estimation function F110 is a function of estimating the position and attitude of the vehicle 110 based on the direction correction parameter and the first vehicle direction Dv1.
  • the work vehicle 100 of the present embodiment it is possible to detect an error in the installation positions of the first antenna 121 and the second antenna 122 more flexibly than the conventional device. More specifically, the first antenna 121 and the second antenna 122 are attached to, for example, the tips of poles extending upward from the vehicle body frame 111 so that radio waves from the satellite positioning system are not blocked by the vehicle 110, vessel 140, or the like. .. Further, the work vehicle 100, for example, loads ore, earth and sand, etc. on the vessel 140, unloads the load from the vessel 140, and travels on uneven ground such as a mine or a construction site. Therefore, vibration or impact may act on the first antenna 121 and the second antenna 122.
  • an error may occur between the preset installation positions of the first antenna 121 and the second antenna 122 and the actual installation positions.
  • an error may occur between the first vehicle direction Dv1 calculated based on the preset installation position and the actual vehicle direction.
  • the work vehicle 100 of the present embodiment has a first antenna based on the radio waves of the satellite positioning system received by the first antenna 121 when steady running is detected by the detection function F106 of the control device 150.
  • the second vehicle direction Dv2 is calculated using the time change of the position information p of 121.
  • a direction correction parameter for correcting the first vehicle direction Dv1 is calculated based on the second vehicle direction Dv2, and the position and attitude of the vehicle 110 are estimated based on the direction correction parameter and the first vehicle direction Dv1. That is, the work vehicle 100 of the present embodiment has a first vehicle direction Dv1 and an actual vehicle by performing steady running including not only extremely limited running states as in the conventional device but also more various running states.
  • the error between the direction Dv and the direction Dv can be corrected more flexibly. Further, the work for detecting the error generated at the installation position of the first antenna 121 and the second antenna 122 becomes unnecessary.
  • the control device 150 further has a determination function F109 for determining whether or not the direction correction parameter can be used. Then, the estimation function F110 of the control device 150 estimates the position and attitude of the vehicle 110 based on the direction correction parameter and the first vehicle direction Dv1 when the direction correction parameter is available. With this configuration, for example, it is possible to prevent the first vehicle direction Dv1 from being corrected based on the direction correction parameter including a large error, and it is possible to correct the first vehicle direction Dv1 more accurately.
  • the steady traveling includes a turning traveling along a circumference having the same center and a radius, and a straight traveling traveling in a straight line.
  • the steady running includes that the average value of the speeds of the vehicles 110 is higher than the predetermined speed threshold value Vth.
  • the work vehicle 100 of the present embodiment includes a vessel 140 attached to the vehicle 110, an elevating cylinder 142 which is an elevating mechanism for elevating and elevating the vessel 140, and an elevating sensor 134 for detecting the height of the vessel 140. ing. Then, in the work vehicle 100 of the present embodiment, the steady running includes that the height of the vessel 140 is lower than the predetermined height threshold value Hth.
  • the work vehicle 100 of the present embodiment is the second vehicle when, for example, the front end portion 141a of the vessel main body 141 is equal to or higher than the height threshold value Hth based on the heights of the first antenna 121 and the second antenna 122.
  • Direction Dv2 is not calculated. Therefore, it is possible to prevent the calculation of the second vehicle direction Dv2 in a state where the radio wave from the satellite positioning system to the first antenna 121 or the second antenna 122 is blocked by the vessel 140, and the error of the second vehicle direction Dv2 is reduced. .. Therefore, according to the work vehicle 100 of the present embodiment, the position and posture of the vehicle 110 can be estimated more accurately.
  • the work vehicle 100 of the present embodiment can be provided with an information notification device for notifying an operator or a user of information.
  • the control device 150 can output error information with respect to the installation information of the first antenna 121 and the second antenna 122 to the information notification device when the direction correction parameter exceeds a predetermined threshold value.
  • the work vehicle 100 of the present embodiment can notify the operator or the user that an error in the installation position of the first antenna 121 or the second antenna 122 has occurred.
  • the work vehicle 100 capable of detecting an error in the installation position of the first antenna 121 or the second antenna 122 more flexibly than the conventional device. ..
  • FIG. 15 is a functional block diagram of the control device 150 showing the second embodiment of the work vehicle according to the present disclosure.
  • the function of the work vehicle 100 of the present embodiment is different from that of the work vehicle 100 of the above-described first embodiment in the function provided by the control device 150. Since the other points of the work vehicle 100 of the present embodiment are the same as those of the work vehicle 100 of the first embodiment, the same reference numerals are given to the same parts and the description thereof will be omitted.
  • the control device 150 has a position and posture calculation function F100'similar to the calculation function F100 of the first embodiment described above. Further, in the work vehicle 100 of the present embodiment, the control device 150 has a travel control function F300 instead of the condition monitoring function F200. In the control device 150 of the present embodiment, the calculation function F100'outputs the estimation result of the position and posture of the vehicle 110 by the estimation function F110 and the detection result of steady running by the detection function F106 to the travel control function F300.
  • the travel control function F300 executes the travel control of the vehicle 110 based on the estimation result and the detection result input from the calculation function F100'. More specifically, the travel control function F300 controls various actuators of the vehicle 110 to automatically operate the accelerator pedal, brake pedal, transmission, steering wheel, etc. of the vehicle 110 to drive the vehicle 110. Let me. Further, in the work vehicle 100 of the present embodiment, for example, the control device 150 detects the steady running shown in FIG. 16 by the detection function F106 instead of the process P4 for detecting the steady running shown in FIGS. 3 and 4. Process P4'is executed.
  • FIG. 16 is a flow chart showing an example of processing P4'by the detection function F106 of the control device 150 of the present embodiment.
  • the same processes as the process P4 of the first embodiment shown in FIG. 6 are designated by the same reference numerals, and the description thereof will be omitted.
  • the detection function F106 stores the unsteady state in the storage device.
  • the process P412 for setting the flag to 1 is executed.
  • the state in which the non-steady flag is set to 1 indicates that the traveling state of the vehicle 110 is not steady traveling and the direction correction parameter cannot be calculated.
  • the control device 150 executes the process P408 for setting the steady running flag to 0, as in the first embodiment.
  • the initial values of the speed shortage flag and the non-stationary flag are 0, and they are initialized at the same time when the control device 150 is started for each sampling cycle.
  • the control device 150 of the present embodiment executes the traveling control of the vehicle 110 by the traveling control function F300 so that the traveling state of the vehicle 110 becomes a steady traveling state.
  • FIG. 17 is a flow chart showing an example of processing P15 by the traveling control function F300 of the control device 150 in the work vehicle 100 of the present embodiment.
  • the control device 150 starts the process P15 shown in FIG. 17, the control device 150 first executes the process P1501 for acquiring the speed shortage flag and the unsteady flag stored in the storage device.
  • the control device 150 executes the process P1502 for determining whether or not the speed shortage flag is 1.
  • the control device 150 determines that the speed shortage flag is 1 (YES)
  • the control device 150 executes the process P1503 for setting the driving control parameters for accelerating the vehicle 110 to a predetermined speed exceeding the speed threshold value Vth.
  • the control device 150 executes the process P1509 for determining the presence / absence of an interrupt, which will be described later.
  • the control device 150 determines that the speed shortage flag is not 1 (NO)
  • the control device 150 executes the process P1504 for determining whether or not the non-stationary flag is 1.
  • the control device 150 determines that the non-stationary flag is not 1 (NO)
  • it can determine that the traveling state of the vehicle 110 is steady traveling in which the direction correction parameter can be calculated.
  • the process P1509 for determining the presence / absence is executed.
  • the control device 150 determines that the unsteady flag is 1 (YES)
  • the control device 150 executes the process P1505 for determining whether or not the speed of the vehicle 110 is higher than the predetermined speed threshold value Vth'. ..
  • the control device 150 decelerates the vehicle 110 so that the speed of the vehicle 110 falls within the speed range of steady running.
  • the process P1506 for setting the control parameters is executed. After the end of this process P1506, the control device 150 executes the process P1509 for determining the presence / absence of an interrupt, which will be described later.
  • the control device 150 determines that the speed of the vehicle 110 is equal to or less than the predetermined speed threshold value Vth'(NO), it can be determined that the speed of the vehicle 110 cannot be adjusted.
  • the control device 150 executes the process P1507 for determining whether or not the absolute value
  • the control device 150 determines that the absolute value
  • control device 150 executes the process P1508 for setting the travel control parameters for maintaining the steering angle of the steering wheel. After the end of this process P1508, the control device 150 executes the process P1509 for determining the presence / absence of an interrupt, which will be described later.
  • the control device 150 determines that the absolute value
  • the control device 150 executes the process P1509 for determining the presence / absence of an interrupt.
  • the control device 150 determines whether or not an interrupt signal for stopping control is input from another control device and whether or not there is an interrupt for the operation of the vehicle 110 by the operator of the vehicle 110.
  • the control device 150 determines that there is no interrupt (NO)
  • the control device 150 executes the process P1510 that controls the traveling of the vehicle 110 based on the parameters set in the above-mentioned processes P1503, P1506, or P1508.
  • the process P15 shown in FIG. 17 is terminated.
  • the control device 150 determines that there is an interrupt (YES)
  • the control device 150 executes the process P1511 for stopping the travel control of the vehicle 110 based on the parameters set in the above-mentioned processes P1503, P1506, or P1508.
  • the process P15 shown in FIG. 17 is terminated.
  • the control device 150 has a travel control function F300 that controls the travel of the vehicle 110.
  • the travel control function F300 controls the vehicle 110 so as to satisfy the steady travel condition when the detection function F106 does not detect the steady travel of the vehicle 110.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本開示は、従来の装置よりも柔軟にアンテナの設置位置の誤差を検出することが可能な作業車両を提供する。作業車両は制御装置１５０を備える。制御装置１５０は、検出機能Ｆ１０６と、算出機能Ｆ１０４と、算出機能Ｆ１０５と、算出機能Ｆ１０７と、推定機能Ｆ１１０と、を有する。検出機能Ｆ１０６は、車両の速度、加速度、および角速度に基づいて定常走行を検出する。算出機能Ｆ１０４は、車両に対する第１アンテナおよび第２アンテナの設置情報に基づいて第１車両方向を算出する。算出機能Ｆ１０５は、定常走行が検出された場合に、第１アンテナの位置情報の時間変化に基づいて第２車両方向を算出する。算出機能Ｆ１０７は、第２車両方向に基づいて第１車両方向を修正するための方向修正パラメータを算出する。推定機能Ｆ１１０は、方向修正パラメータと第１車両方向に基づいて車両の位置および姿勢を推定する。

Description

作業車両
　本開示は、作業車両に関する。
　従来からＧＰＳ衛星等の測位用信号を送信する送信機からの電波を受信してキャリアディファレンシャル測位を行う際のキャリア位相整数値バイアスを決定する方法、物体や移動体の方位・姿勢を計測する方法および装置に関する発明が知られている（下記特許文献１を参照）。
　特許文献１に記載された方位・姿勢計測装置は、キャリア位相整数値バイアス決定装置と、方位または姿勢を求める手段と、から成る（請求項５等）。前記キャリア位相整数値バイアス決定装置は、第１・第２のアンテナと、アンテナ方位観測手段と、算出、修正、および決定を行う手段と、を備える（請求項４等）。上記アンテナ方位観測手段は、複数の測位用送信機からの測位用信号を、第１・第２のアンテナでそれぞれ受信し、測位用信号のキャリア位相変化量に基づいて第１のアンテナに対する第２のアンテナの方位を求める。
　上記算出、修正、および決定を行う手段は、次のように動作する。まず、第１のアンテナに対する第２のアンテナの方位、第１・第２のアンテナ間の距離、および測位用送信機の位置から求まる計算上のキャリア位相差である計算位相差を算出する。また、第１・第２のアンテナで受信した前記測位用信号のキャリア位相差の小数部である観測位相差を求め、前記計算位相差の小数部が±０．５サイクルの範囲で前記観測位相差と一致するように前記計算位相差を修正する。そして、該修正後の計算位相差を前記観測位相差の整数値バイアスとして決定する（請求項４等）。
　上記方位または姿勢を求める手段は、上記キャリア位相整数値バイアス決定装置で求められた整数値バイアスと前記観測位相差とから第１のアンテナに対する第２のアンテナの相対位置を求め、該相対位置から第１・第２のアンテナ間の方位または姿勢を求める（請求項５等）。
　また、上記方位・姿勢計測装置は、前記第１・第２のアンテナを、略水平面を前後方向に進行する移動体に設け、前記アンテナ方位観測手段が、前記移動体の移動にともなう第１・第２のアンテナの移動による前記測位用信号のキャリア位相変化量に基づいて前記移動体の進行方位を観測する。さらに、この装置は、該進行方位を移動体の前方方位と見なして、該移動体の前方方位と移動体に対する第１・第２のアンテナの取り付け位置関係から第１のアンテナに対する第２のアンテナの方位を観測する（請求項７等）。
特開２００５‐０４３２１２号公報
　たとえば、ダンプトラックなどの作業車両は、車両の方向を計測するために、上記従来の方位・姿勢計測装置と同様に、全球測位衛星システム（ＧＮＳＳ）などの衛星測位システムの電波を受信する第１のアンテナと第２のアンテナを備える。このようなアンテナは、たとえば、車体によって電波が遮蔽されるのを回避するために、車体から上方へ延びるポールの先端に設置される。
　また、作業車両は、たとえば、鉱石や土砂などを積載したり、その積載物を降ろしたり、凹凸の多い地面の上を走行したりすることで、アンテナに振動や衝撃が作用することがある。そのため、時間の経過にともなって、アンテナの実際の設置位置と、あらかじめ設定された設置位置との間に誤差が生じ、その設定された設置位置に基づいて算出される車両の方向と、実際の車両の方向との間に誤差が生じることがある。
　このような場合、前記従来の方位・姿勢計測装置は、略水平面を前後方向に進行する移動体の移動にともなう第１・第２のアンテナの移動に基づいて、移動体の進行方位を観測する。しかし、作業車両が走行する鉱石の採掘現場や建設現場では、作業車両が略水平面を前後方向に進行する機会は限定されている。そのため、作業車両においては、このような限定された条件ではなく、より柔軟にアンテナの設置位置の誤差を検出することが求められている。
　本開示は、従来の装置よりも柔軟にアンテナの設置位置の誤差を検出することが可能な作業車両を提供する。
　本開示の一態様は、車両と、前記車両に取り付けられて衛星測位システムの電波を受信する第１アンテナおよび第２アンテナと、前記電波に基づく前記第１アンテナの位置情報および前記第１アンテナと前記第２アンテナとを結ぶ基線方向を出力する受信機と、前記車両の速度、加速度、および角速度を計測するセンサと、前記車両の位置および姿勢を推定する制御装置と、を備えた作業車両であって、前記制御装置は、前記速度、前記加速度、および前記角速度に基づいて定常走行を検出する検出機能と、前記車両に対する前記第１アンテナおよび前記第２アンテナの設置情報に基づいて第１車両方向を算出する機能と、前記定常走行が検出された場合に、前記第１アンテナの前記位置情報の時間変化に基づいて第２車両方向を算出する機能と、前記第２車両方向に基づいて前記第１車両方向を修正するための方向修正パラメータを算出する機能と、前記方向修正パラメータと前記第１車両方向に基づいて前記車両の前記位置および前記姿勢を推定する推定機能と、を有する作業車両である。
　本開示の上記一態様によれば、従来の装置よりも柔軟にアンテナの設置位置の誤差を検出することが可能な作業車両を提供することができる。
本開示に係る作業車両の実施形態１を示す側面図。 図１の作業車両の制御装置の機能ブロック図。 図１の作業車両の制御装置による処理の一例を示すフロー図。 図３のセンサ情報を記憶する処理を説明する表。 図３の位置および方向を記憶する処理を説明する表。 図３の定常走行を検出する処理のフロー図。 第１および第２アンテナ間の基線方向と車両方向との関係を示す模式図。 図３の第１車両方向を算出する処理のフロー図。 図８の処理で取得される方向修正パラメータを説明する模式図。 図３の第２車両方向を算出する処理のフロー図。 定常走行に含まれる旋回の一例を示す模式図。 車両の回転中心を説明する模式図。 図３の方向修正パラメータを算出する処理のフロー図。 方向修正パラメータを説明する模式図。 本開示に係る作業車両の実施形態２における制御装置の機能ブロック図。 図１５の制御装置の定常走行検出機能による処理の一例を示すフロー図。 図１５の制御装置の走行制御機能による処理の一例を示すフロー図。
　以下、図面を参照して本開示に係る作業車両の実施形態を説明する。
［実施形態１］
　図１は、本開示に係る作業車両の実施形態１を示す側面図である。図２は、図１の作業車両１００の制御装置１５０の機能ブロック図である。本実施形態の作業車両１００は、たとえば、鉱石の採掘現場や建設現場で使用されるダンプトラックである。作業車両１００は、たとえば、車両１１０と、測位装置１２０と、センサ１３０と、ベッセル１４０と、制御装置１５０とを備えている。
　車両１１０は、たとえば、車体フレーム１１１と、車輪１１２と、キャビン１１３とを備えている。車体フレーム１１１は、たとえば、ラダー状の構造体である。車体フレーム１１１は、たとえば、車軸に取り付けられた左右の車輪１１２を、サスペンションを介して支持している。また、車体フレーム１１１は、たとえば、図示を省略するエンジン、発電機、モータ、動力伝達機構、操舵機構、油圧装置、および車両制御用のアクチュエータなどを支持している。
　車輪１１２は、たとえば、動力伝達機構を介してモータに接続され、モータによって駆動されて車両１１０を走行させる。キャビン１１３は、作業車両１００のオペレータが搭乗するための車室である。キャビン１１３の内部には、たとえば、図示を省略するステアリングホイール、操作ペダル、操作レバー、情報装置、スピーカ、計器、表示ランプなどが設置されている。
　測位装置１２０は、たとえば、全球測位衛星システム（ＧＮＳＳ）などの衛星測位システムによって構成することができる。測位装置１２０は、たとえば、第１アンテナ１２１と、第２アンテナ１２２と、受信機１２３とを備えている。第１アンテナ１２１および第２アンテナ１２２は、たとえば、車両１１０に取り付けられ、ＧＮＳＳなどの衛星測位システムの電波を受信する。
　第１アンテナ１２１と第２アンテナ１２２は、たとえば車両１１０の前後方向および高さ方向に直交する車両１１０の幅方向に離隔して設置される。第１アンテナ１２１および第２アンテナ１２２は、たとえば、車両１１０に固定されたポールの先端に取り付けられ、ベッセル本体１４１の前端部１４１ａの定常位置よりも高い位置に設置される。
　受信機１２３は、たとえば、第１アンテナ１２１および第２アンテナ１２２に対して信号ケーブルを介して接続されている。受信機１２３は、第１アンテナ１２１および第２アンテナ１２２が受信した電波に基づいて、第１アンテナ１２１の位置情報、および、第１アンテナ１２１と第２アンテナ１２２との間の基線方向を出力する。ここで、基線方向とは、たとえば、第１アンテナ１２１の設置位置と、第２アンテナ１２２の設置位置とを結ぶ直線の方向である。
　センサ１３０は、たとえば、速度センサ１３１と、加速度センサ１３２と、角速度センサ１３３と、を含む。また、図１および図２に示す例において、センサ１３０は、たとえば、昇降センサ１３４を含んでいる。これらのセンサ１３０は、たとえば、コントロールエリアネットワーク（ＣＡＮ）を介して制御装置１５０と通信可能に接続されている。なお、測位装置１２０によって測定された位置に基づいて、制御装置１５０によって車両１１０の速度を算出する場合は、測位装置１２０を速度センサとして用い、速度センサ１３１を省略してもよい。
　速度センサ１３１は、たとえば、車輪１１２の回転速度に基づいて車両１１０の速度を検知し、その速度を制御装置１５０へ出力する。加速度センサ１３２は、たとえば、重力加速度を除いた車両１１０の加速度を検知し、その加速度を制御装置１５０へ出力する。角速度センサ１３３は、たとえば、車両１１０の角速度を検知し、その角速度を制御装置１５０へ出力する。昇降センサ１３４は、たとえば、ベッセル本体１４１を昇降させる昇降シリンダ１４２のストロークを含むベッセル本体１４１の昇降状態を検知し、その昇降状態を制御装置１５０へ出力する。
　ベッセル１４０は、たとえば、ベッセル本体１４１と、昇降シリンダ１４２と、回転軸１４３とを有している。ベッセル本体１４１は、たとえば、底部の後方側に設けられた回転軸１４３を中心に回転可能に車体フレーム１１１の上に支持されている。ベッセル本体１４１は、たとえば、作業車両１００において、鉱石、岩石、砂利、土砂などの積載物を積載して運搬するための部分である。
　昇降シリンダ１４２は、たとえば、車両１１０の幅方向の両側に設けられた一対の油圧シリンダである。昇降シリンダ１４２は、ピストンロッドの先端がベッセル本体１４１の底部の回転軸１４３よりも前方に連結され、ピストンロッドと反対側のシリンダチューブの端部が車体フレーム１１１の下部に連結されている。昇降シリンダ１４２は、制御装置１５０によって制御される油圧装置の一部を構成する。昇降シリンダ１４２は、ピストンロッドを伸縮させることで、回転軸１４３を中心としてベッセル本体１４１を回動させ、ベッセル本体１４１の前端部１４１ａを昇降させる。
　制御装置１５０は、たとえば、マイクロコントローラまたはファームウェアなどのコンピュータシステムである。制御装置１５０は、たとえば、中央処理装置（ＣＰＵ）、ＲＡＭやＲＯＭなどの記憶装置、その記憶装置に記憶されたプログラムおよびデータ、タイマーならびに信号の入出力を行う入出力部などによって構成されている。制御装置１５０とセンサ１３０は、たとえば、それぞれ時刻同期されて一定の周期で動作しているため、時刻のずれによる信号の取り零しが防止されている。また、制御装置１５０の駆動周期は、たとえば、センサ１３０の最も短い出力周期と等しくすることができる。
　制御装置１５０は、たとえば、図２に示すように作業車両１００の位置および姿勢を算出する機能Ｆ１００と、作業車両１００の状態を監視する機能Ｆ２００とを有している。制御装置１５０のこれらの機能は、たとえば、制御装置１５０の記憶装置に記憶されたプログラムを、ＣＰＵによって実行することによって実現することができる。以下の説明では、作業車両１００の位置および姿勢を算出する機能Ｆ１００を、算出機能Ｆ１００と略称し、作業車両１００の状態を監視する機能Ｆ２００を、状態監視機能Ｆ２００と略称する場合がある。
　図２に示す例において、制御装置１５０の算出機能Ｆ１００は、たとえば、アンテナ設置情報を記憶する機能Ｆ１０１と、位置および方向の情報を記憶する機能Ｆ１０２と、センサ情報を記憶する機能Ｆ１０３と、方向修正パラメータを記憶する機能Ｆ１０８とを有している。以下の説明では、制御装置１５０のこれらの機能を、それぞれ、記憶機能Ｆ１０１，Ｆ１０２，Ｆ１０３，Ｆ１０８と略称する場合がある。
　また、図２に示す例において、制御装置１５０の算出機能Ｆ１００は、さらに、第１車両方向を算出する機能Ｆ１０４と、第２車両方向を算出する機能Ｆ１０５と、定常走行を検出する機能Ｆ１０６と、方向修正パラメータを算出する機能Ｆ１０７とを有している。以下の説明では、制御装置１５０のこれらの機能を、それぞれ、算出機能Ｆ１０４，Ｆ１０５、検出機能Ｆ１０６、算出機能Ｆ１０７と略称する場合がある。
　また、図２に示す例において、制御装置１５０の算出機能Ｆ１００は、さらに、方向修正パラメータの使用可否を判定する機能Ｆ１０９と、作業車両１００の位置および姿勢を推定する機能Ｆ１１０とを有している。以下の説明では、制御装置１５０のこれらの機能を、それぞれ、判定機能Ｆ１０９、推定機能Ｆ１１０と略称する場合がある。
　図３は、図１の作業車両１００の制御装置１５０による処理の一例を示すフロー図である。制御装置１５０は、たとえば、算出機能Ｆ１００によって、図３に示す処理を実行する。具体的には、制御装置１５０は、図３に示す処理を開始すると、たとえば、記憶機能Ｆ１０３によって、センサ情報を記憶する処理Ｐ１を実行する。この処理Ｐ１において、記憶機能Ｆ１０３は、制御装置１５０の入出力部を介してセンサ１３０から入力されたセンサ情報を、制御装置１５０を構成する記憶装置に記憶させる。
　図４は、処理Ｐ１において記憶機能Ｆ１０３によって記憶されるセンサ情報の一例を示す表Ｔ１である。センサ情報は、たとえば、センサ１３０に含まれる速度センサ１３１、加速度センサ１３２、角速度センサ１３３、および昇降センサ１３４のそれぞれからの出力値である速度ｖ、加速度ａ、角速度ｗ、および昇降状態ｈを含む。また、センサ情報は、たとえば、各出力値が出力された時刻ｔ，ｔ－１，…，ｔ－ｍ，…を含む。制御装置１５０の記憶装置は、たとえば、現在時刻ｔのセンサ情報と、現在時刻ｔよりも前の時刻ｔ－１，…，ｔ－ｍ，…のセンサ情報とを含む、一定の期間のセンサ情報を記憶している。
　制御装置１５０の記憶装置にセンサ情報が記憶される一定の期間は、少なくとも、第１アンテナ１２１の位置情報と、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向とが、受信機１２３から共に出力される周期以上の期間である。また、センサ情報に含まれる速度、加速度、および角速度は、速度センサ１３１、加速度センサ１３２、および角速度センサ１３３の出力値であってもよく、各センサの出力値を車両座標で三次元表記に変換した値であってもよい。
　次に、制御装置１５０は、たとえば、第１アンテナ１２１の位置情報と、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向とが、共に受信機１２３から取得されているか否かを判定する処理Ｐ２を実行する。なお、第１アンテナ１２１の位置情報は、ＧＮＳＳの電波を受信した第１アンテナ１２１から受信機１２３へ信号が入力され、その結果、受信機１２３から出力される第１アンテナ１２１の位置情報である。また、基線方向は、たとえば、ＧＮＳＳの電波を受信した第１アンテナ１２１と第１アンテナ１２１から受信機１２３へ信号が入力され、その結果、受信機１２３から出力される第１アンテナ１２１と第２アンテナ１２２とを結ぶ直線の方向である。
　制御装置１５０の記憶機能Ｆ１０２は、たとえば、受信機１２３から、それぞれ一定の周期で、第１アンテナ１２１の位置情報と、基線方向とを取得して、制御装置１５０の記憶装置に記憶させている。したがって、処理Ｐ２において、制御装置１５０は、たとえば、第１アンテナ１２１の位置情報と、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向とが、共に受信機１２３から取得されているか否かを判定する。
　具体的には、たとえば、受信機１２３から、第１アンテナ１２１の位置情報が１［Ｈｚ］の周期で出力され、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向が１０［Ｈｚ］の周期で出力されているとする。この場合、制御装置１５０は、処理Ｐ２において、受信機１２３から基線方向のみが取得された時刻においては、第１アンテナ１２１の位置情報が取得されなかった（ＮＯ）と判定し、後述する処理Ｐ１３を実行する。一方、制御装置１５０は、処理Ｐ２において、第１アンテナ１２１の位置情報と基線方向とが共に取得された（ＹＥＳ）と判定すると、次の処理Ｐ３を実行する。
　図５は、処理Ｐ３において制御装置１５０の記憶装置に記憶される第１アンテナ１２１の位置情報ｐと基線方向ｄの一例を示す表Ｔ２である。処理Ｐ３において、制御装置１５０は、たとえば、記憶機能Ｆ１０２により、受信機１２３から取得した第１アンテナ１２１の位置情報ｐと基線方向ｄとを、取得した時刻ｔ，ｔ－ｍ，…，ｔ－６ｍ，…とともに記憶装置に記憶させる。
　図５は、第１アンテナ１２１の位置情報ｐと、基線方向ｄとが、周期ｍごとに共に出力される例を示している。第１アンテナ１２１の位置情報ｐは、たとえば、地図上の点を示す情報であり、２次元または３次元の位置情報である。また、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向ｄは、たとえば、２次元もしくは３次元のベクトル、または方位である。図３に示すように、処理Ｐ３の終了後、制御装置１５０は、次の処理Ｐ４を実行する。
　図６は、図３の定常走行を検出する処理Ｐ４のフロー図である。なお、図６に示す各処理は、たとえば、制御装置１５０の検出機能Ｆ１０６によって実行することができる。制御装置１５０は、処理Ｐ４を開始すると、まず、平均速度を算出する処理Ｐ４０１を実行する。具体的には、この処理Ｐ４０１において、制御装置１５０は、たとえば、記憶装置を参照し、測位装置１２０から一周期前の第１アンテナ１２１の位置情報ｐが取得された時刻を参照する。さらに、制御装置１５０は、たとえば、一周期前の第１アンテナ１２１の位置情報ｐが取得された時刻から現在までの車両１１０の速度ｖを用いて、車両１１０の平均速度を算出する。
　次に、制御装置１５０は、算出した車両１１０の平均速度が、所定の速度閾値Ｖｔｈよりも高いか否かを判定する処理Ｐ４０２を実行する。この処理Ｐ４０２において、制御装置１５０は、たとえば、算出した車両１１０の平均速度が所定の速度閾値Ｖｔｈ以下（ＮＯ）と判定すると、次の処理Ｐ４０８を実行する。
　処理Ｐ４０８において、制御装置１５０は、記憶装置に記憶された定常走行フラグを０に設定し、図６に示す処理を終了する。この定常走行フラグが０の状態は、作業車両１００が定常走行をしていないことを示している。すなわち、定常走行は、車両１１０の速度の平均値が、所定の速度閾値Ｖｔｈよりも高いことを含む。これにより、車両１１０の停車中や速度閾値Ｖｔｈ以下の低速走行が定常走行から除外される。
　一方、処理Ｐ４０２において、制御装置１５０は、たとえば、算出した車両１１０の平均速度が所定の速度閾値Ｖｔｈよりも高い（ＹＥＳ）と判定すると、次の処理Ｐ４０３を実行する。処理Ｐ４０３において、制御装置１５０は、たとえば、ベッセル１４０の高さを算出する。具体的には、制御装置１５０は、たとえば、記憶装置を参照し、現在時刻と、一周期前の測位装置１２０の出力が取得された時刻のベッセル１４０の昇降状態に基づいて、ベッセル１４０の高さを算出する。ここで、ベッセル１４０の高さは、たとえば、ベッセル本体１４１の前端部１４１ａの高さである。
　たとえば、ベッセル１４０の昇降状態が、制御装置１５０の記憶装置にベッセル本体１４１の角度として記憶されているとする。この場合、制御装置１５０は、たとえば、ベッセル本体１４１の角度の正弦を用いて、ベッセル本体１４１の前端部１４１ａの高さを求めることができる。なお、ベッセル本体１４１の寸法データは、たとえば、あらかじめ制御装置１５０の記憶装置に記憶されている。処理Ｐ４０３の終了後、制御装置１５０は、次の処理Ｐ４０４を実行する。
　処理Ｐ４０４において、制御装置１５０は、たとえば、処理Ｐ４０３で算出したベッセル１４０の高さが、所定の高さ閾値Ｈｔｈよりも低いか否かを判定する。より詳細には、制御装置１５０は、たとえば、現在時刻と、前回の測位装置１２０から位置情報ｐが取得された時刻とのそれぞれのベッセル１４０の高さが、高さ閾値Ｈｔｈよりも低いか否かを判定する。ここで、高さ閾値Ｈｔｈは、たとえば、第１アンテナ１２１もしくは第２アンテナ１２２の高さと等しい高さ、または、第１アンテナ１２１と第２アンテナ１２２のうち、より低い方の高さに設定することができる。
　処理Ｐ４０４において、制御装置１５０が、たとえば、現在時刻と、前回の測位装置１２０から位置情報ｐが取得された時刻との両方または一方で、ベッセル１４０の高さが高さ閾値Ｈｔｈ以上である（ＮＯ）と判定したとする。この場合、制御装置１５０は、前述の定常走行フラグを０に設定する処理Ｐ４０８を実行し、図６に示す処理Ｐ４を終了する。
　また、処理Ｐ４０４において、制御装置１５０が、たとえば、現在時刻と、前回の測位装置１２０から位置情報ｐが取得された時刻との両方または一方で、ベッセル１４０の高さが高さ閾値Ｈｔｈより低い（ＹＥＳ）と判定したとする。この場合、制御装置１５０は、たとえば、作業車両１００の走行状態を算出する処理Ｐ４０５を実行する。より具体的には、制御装置１５０は、処理Ｐ４０５において、作業車両１００の角速度、角加速度、および加速度のそれぞれの平均値と、車両１１０の方向の変化を算出する。
　ここで、作業車両１００の角速度、角加速度、および加速度のそれぞれの平均値は、たとえば、前回の測位装置１２０から位置情報ｐが取得された時刻から現在時刻までの平均値である。作業車両１００の角加速度は、たとえば、ある時刻における作業車両１００の角速度と、その一周期前の時刻における作業車両１００の角速度との差分から算出することができる。車両１１０の方向、すなわち車両方向の変化は、たとえば、以下のように算出することができる。
　図７は、第１アンテナ１２１と第１アンテナ１２１とを結ぶ基線方向ｄと、車両方向Ｄｖとの関係を示す模式図である。図７に示す例において、作業車両１００は、前回の測位装置１２０から位置情報ｐが取得された時刻から現在時刻までの間に、車両１１０が中心Ｃｔを中心とする円周に沿って角度θだけ旋回して移動している。この場合、車両方向Ｄｖは、車両１１０が旋回した角度θと等しい角度で変化する。同様に、基線方向ｄすなわち基線ベクトルも、車両１１０が旋回した角度θと等しい角度で変化する。
　このように、車両方向Ｄｖが変化する角度θと、基線方向ｄが変化する角度θは等しい。そのため、現在時刻と、前回の測位装置１２０から位置情報ｐが取得された時刻との間の基線方向ｄの角度変化を算出することで、車両方向Ｄｖの角度変化を算出することができる。すなわち、制御装置１５０は、処理Ｐ４０５において、たとえば、基線方向ｄの角度変化を算出することで、車両方向Ｄｖの角度変化を算出する。以上のように、処理Ｐ４０５において、制御装置１５０は、たとえば作業車両１００の角速度、角加速度、および加速度のそれぞれの平均値と、車両方向Ｄｖの角度変化とを算出する。
　次に、制御装置１５０は、たとえば、作業車両１００の走行状態が、定常走行であるか否かを判定する処理Ｐ４０６を実行する。ここで、定常走行とは、たとえば、作業車両１００が一定の運動状態で走行している状態である。より具体的には、作業車両１００の定常走行は、たとえば、中心および半径が同一の円周に沿って作業車両１００が走行する旋回と、作業車両１００が一直線に走行する直進とを含む。なお、定常走行は、旋回および直進に限定されず、他の走行状態を含んでもよい。
　処理Ｐ４０６において、制御装置１５０は、たとえば、前の処理Ｐ４０５で算出した車両方向Ｄｖの変化と、車両１１０の角速度の時間分積分との差が閾値以下である場合に、作業車両１００が中心および半径が同一の円周に沿って走行する旋回中と判定する。この場合、作業車両１００の旋回が定常走行に含まれることから、制御装置１５０は、処理Ｐ４０６において、作業車両１００の走行状態が定常走行である（ＹＥＳ）と判定し、次の処理Ｐ４０７を実行する。
　また、処理Ｐ４０６において、制御装置１５０は、たとえば、前の処理Ｐ４０５で算出した車両１１０の角加速度および加速度の平均値が閾値以下である場合に、作業車両１００が一直線に走行する直進中であると判定する。この場合、作業車両１００の直進が定常走行に含まれることから、制御装置１５０は、処理Ｐ４０６において、作業車両１００の走行状態が定常走行である（ＹＥＳ）と判定し、次の処理Ｐ４０７を実行する。
　一方、前の処理Ｐ４０５で算出した車両方向Ｄｖの変化と、車両１１０の角速度の時間分積分との差が閾値より大であるか、または、車両１１０の角加速度および加速度の平均値が閾値より大であったとする。この場合、処理Ｐ４０６において、制御装置１５０は、作業車両１００の走行状態が定常走行ではない（ＮＯ）と判定し、前述の定常走行フラグを０に設定する処理Ｐ４０８を実行して、図６に示す処理Ｐ４を終了する。
　処理Ｐ４０７において、制御装置１５０は、前述の処理Ｐ４０５で算出した車両１１０の角速度の平均値の絶対値｜αａｖｅ｜が、所定の角速度閾値αｔｈより小であるか否かを判定する。この処理Ｐ４０７において、制御装置１５０は、絶対値｜αａｖｅ｜が角速度閾値αｔｈより小である（ＹＥＳ）と判定すると、記憶装置に記憶された定常走行フラグを１に設定する処理Ｐ４０９を実行して、図６に示す処理Ｐ４を終了する。この定常走行フラグが１の状態は、作業車両１００の走行状態が直進であることを示している。
　一方、処理Ｐ４０７において、制御装置１５０は、絶対値｜αａｖｅ｜が角速度閾値αｔｈ以上である（ＮＯ）と判定すると、記憶装置に記憶された定常走行フラグを２に設定する処理Ｐ４１０を実行して、図６に示す処理Ｐ４を終了する。この定常走行フラグが２の状態は、作業車両１００の走行状態が、中心および半径が同一の円周に沿って走行する旋回であることを示している。処理Ｐ４の終了後、制御装置１５０は、図３に示す処理Ｐ５を実行する。
　処理Ｐ５において、制御装置１５０は、たとえば、検出機能Ｆ１０６により、作業車両１００の走行状態が、定常走行であるか否かを判定する。具体的には、制御装置１５０は、検出機能Ｆ１０６により、前の処理Ｐ４で設定されて記憶装置に記憶された定常走行フラグを参照する。参照した定常走行フラグが０である場合、作業車両１００が定常走行をしていないことを示しているので、制御装置１５０は、検出機能Ｆ１０６により、定常走行ではない（ＮＯ）と判定し、位置および姿勢を推定する処理Ｐ１３を実行する。
　作業車両１００が特定の旋回や直進を含む定常走行をしていない場合、制御装置１５０は、処理Ｐ１３において、前回までの処理Ｐ６から処理Ｐ１２までを経て記憶装置に記憶された方向修正パラメータを用いて、作業車両１００の位置および姿勢を推定する。この作業車両１００の位置および姿勢を推定する処理Ｐ１３については、以下で処理Ｐ６から処理Ｐ１２までを説明した後に詳述する。
　処理Ｐ５において、参照した定常走行フラグが１または２である場合、作業車両１００が定常走行をしていることを示しているので、制御装置１５０は、検出機能Ｆ１０６により、定常走行である（ＹＥＳ）と判定し、第１車両方向を算出する処理Ｐ６を実行する。ここで、第１車両方向とは、第１アンテナ１２１および第２アンテナ１２２の設置情報に基づいて算出される車両１１０の方向である。この設置情報は、たとえば、第１アンテナ１２１および第２アンテナ１２２が設置された位置の座標を含み、図２に示す制御装置１５０の記憶機能Ｆ１０１により、制御装置１５０の記憶装置に記憶されている。
　図８は、第１車両方向を算出する処理Ｐ６のフロー図である。制御装置１５０は、たとえば、第１車両方向を算出する処理Ｐ６を開始すると、まず、算出機能Ｆ１０４により、方向修正パラメータを取得する処理Ｐ６０１を実行する。この処理Ｐ６０１において、制御装置１５０は、たとえば、記憶機能Ｆ１０１によって記憶装置に記憶された最新の方向修正パラメータを、算出機能Ｆ１０４によって取得する。
　図９は、図８に示す処理Ｐ６０１において取得される方向修正パラメータを説明する模式図である。第１アンテナ１２１および第２アンテナ１２２は、車両１１０に設置されて位置が固定される。この車両１１０に対する第１アンテナ１２１の設置位置と第２アンテナ１２２の設置位置に基づいて、第１アンテナ１２１から第２アンテナ１２２へ向かうベクトルＡｖを求めることができる。また、車両１１０の前後方向に平行な車両１１０の中心線の方向を車両方向Ｄｖとする。
　この場合、第１アンテナ１２１および第２アンテナ１２２の設置情報は、車両方向ＤｖとベクトルＡｖとのなす角であるアンテナ設置角δを含む。そして、方向修正パラメータは、何らかの原因によりアンテナ設置角δに変化が生じた場合に、その変化を補正するためのパラメータである。アンテナ設置角δおよび方向修正パラメータは、第１アンテナ１２１および第２アンテナ１２２の設置位置を車両１１０に固定された座標系の座標で表しても、たとえば座標変換を行うことで、同様に算出することが可能である。
　方向修正パラメータは、後述する処理Ｐ８において、制御装置１５０の算出機能Ｆ１０７により算出されて、記憶機能Ｆ１０８により記憶装置に記憶される。なお、制御装置１５０の記憶装置には、たとえば、一定期間分の方向修正パラメータが、それぞれ、時刻とともに記憶されている。また、処理Ｐ８が未実施で方向修正パラメータが算出されていない場合には、制御装置１５０の記憶装置には、初期値としての０が記憶されている。方向修正パラメータが０の場合は、アンテナ設置角δの補正は行われない。
　次に、制御装置１５０は、たとえば、算出機能Ｆ１０４により、第１車両方向を算出する処理Ｐ６０２を実行する。具体的には、たとえば、制御装置１５０は、算出機能Ｆ１０４により、図９に示すアンテナ設置角δから方向修正パラメータを差し引いた値と、図９に示すベクトルＡｖとを用いて、車両方向Ｄｖを算出し、図８に示す処理Ｐ６を終了する。この処理Ｐ６で算出される車両方向Ｄｖが、第１アンテナ１２１および第２アンテナ１２２の設置位置に由来する、第１車両方向である。次に、制御装置１５０は、たとえば算出機能Ｆ１０５により、図３に示すように、第２車両方向を算出する処理Ｐ７を実行する。
　図１０は、第２車両方向を算出する処理Ｐ７のフロー図である。図１０に示す各処理は、たとえば、制御装置１５０の算出機能Ｆ１０５によって実行される。制御装置１５０は、処理Ｐ７を開始すると、まず、アンテナ位置の差分を算出する処理Ｐ７０１を実行する。処理Ｐ７０１において、制御装置１５０は、測位装置１２０の受信機１２３から制御装置１５０へ入力され、制御装置１５０の記憶機能Ｆ１０２によって記憶装置に記憶された第１アンテナ１２１の位置情報ｐを取得する。ここで、制御装置１５０は、現在の第１アンテナ１２１の位置情報ｐと、一周期前の第１アンテナ１２１の位置情報ｐとを取得する。
　さらに、この処理Ｐ７０１において、制御装置１５０は、現在の第１アンテナ１２１の位置情報ｐと、一周期前の第１アンテナ１２１の位置情報ｐとの差分に基づいて、車両１１０の移動方向を算出する。より詳細には、制御装置１５０は、現在の第１アンテナ１２１の位置情報ｐと、一周期前の第１アンテナ１２１の位置情報ｐとの差分に基づいて、車両１１０の移動ベクトルを算出する。
　次に、制御装置１５０は、定常走行フラグが２であるか否かを判定する処理Ｐ７０２を実行する。処理Ｐ７０２において、制御装置１５０が、定常走行フラグは２ではない（ＮＯ）、すなわち、定常走行フラグが１であると判定したとする。この場合、車両１１０の走行状態は一直線に走行する直進であり、車両方向Ｄｖと車両１１０の進行方向とが一致する。そのため、制御装置１５０は、前の処理Ｐ７０１で算出した車両１１０の移動ベクトルを第２車両方向として設定する処理Ｐ７０６を実行し、図１０に示す処理Ｐ７を終了する。
　一方、処理Ｐ７０２において、制御装置１５０が、定常走行フラグは２である（ＹＥＳ）と判定したとする。この場合、車両１１０の走行状態は、中心および半径が同一の円に沿って走行する旋回である。そのため、制御装置１５０は、車両１１０の旋回の中心の位置を算出する処理Ｐ７０３を実行する。
　図１１は、作業車両１００の定常走行に含まれる旋回の一例を示す模式図である。図１１では、中心Ｃｔを左回り（反時計回り）に旋回する車両１１０の現在時刻における位置と、一周期前の時刻における位置とを示している。制御装置１５０は、処理Ｐ７０３において、まず、一周期前の時刻の第１アンテナ１２１の位置情報ｐと、現在時刻の第１アンテナ１２１の位置情報ｐとに基づいて、一周期前の時刻における車両１１０の車両方向Ｄｖを算出する。
　ここで、車両１１０は、中心Ｃｔおよび半径が同一の円に沿って旋回している。そのため、一周期前の時刻から現在時刻までの間の中心Ｃｔの周りの車両１１０の回転角度は、一周期前の時刻の車両方向Ｄｖと現在時刻の車両方向Ｄｖとの間の角度θに等しい。車両１１０の旋回の中心Ｃｔは、前述の処理Ｐ７０１で算出した移動ベクトルＭｖの二等分線上にある。また、移動ベクトルＭｖから旋回の中心Ｃｔまでの距離Ｄは、移動ベクトルＭｖの長さをＬとして、次の式（１）によって求めることができる。
　　　　　　　Ｄ＝Ｌ／｛２×ｔａｎ（θ／２）｝　・・・（１）
　したがって、処理Ｐ７０３において、制御装置１５０は、たとえば、車両１１０の角速度の平均から移動ベクトルＭｖに対する法線を算出することで、旋回の中心Ｃｔを算出することができる。次に、制御装置１５０は、車両１１０の回転中心を算出する処理Ｐ７０４を実行する。
　この処理Ｐ７０４において、制御装置１５０は、まず、第１アンテナ１２１の位置から車両１１０の回転中心へ向かうベクトルを算出する。そのために、制御装置１５０は、第１アンテナ１２１の設置位置から加速度センサ１３２の設置位置へ向かうベクトルと、加速度センサ１３２の設置位置から車両１１０の回転中心へ向かうベクトルとを算出する。
　図１２は、車両１１０の回転中心Ｃｔｖを説明する模式図である。車両１１０の回転中心Ｃｔｖは、車両１１０を一つの剛体と考え、車両１１０の速度ベクトルＶｖの方向と車両方向Ｄｖとが等しくなる点である。なお、車両１１０の速度ベクトルＶｖの方向は、車両１１０の旋回の中心Ｃｔと車両１１０の回転中心Ｃｔｖとを結ぶ線分の法線方向である。加速度センサ１３２の設置位置から車両１１０の回転中心Ｃｔｖへ向かうベクトルは、次のように算出することができる。
　前述の処理Ｐ４で算出された車両１１０の加速度の平均値の中から車両１１０の前後方向の成分であるａｘを抽出する。さらに、ヨー方向の角速度の平均値ｗｚと、車両方向Ｄｖからのずれｇを用いる。これにより、加速度センサ１３２の設置位置から車両１１０の回転中心Ｃｔｖへ向かうベクトルを（ａｘ／（ｗｚ×ｗｚ），ｇ）のように求めることができる。
　以上により、第１アンテナ１２１から加速度センサ１３２へ向かうベクトルと、加速度センサ１３２から車両１１０の回転中心Ｃｔｖへ向かうベクトルとが求められる。さらに、これらのベクトルを加算することで、第１アンテナ１２１から車両１１０の回転中心Ｃｔｖへ向かうベクトルを求めることができる。
　また、車両１１０が低速で走行している場合には、アッカーマンジオメトリに従って後輪軸中心Ｃｒａを車両１１０の回転中心に規定することもできる。この場合は、車両１１０の仕様に基づいて、第１アンテナ１２１から後輪軸中心Ｃｒａまでのベクトルを予め算出して、制御装置１５０の記憶装置に記憶させておくことができる。
　さらに、処理Ｐ７０４において、制御装置１５０は、第１アンテナ１２１から車両１１０の回転中心Ｃｔｖへ向かうベクトルと、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向のベクトルとを用いて、車両１１０の回転中心Ｃｔｖを算出する。具体的には、記憶機能Ｆ１０２によって制御装置１５０の記憶装置に記憶された一周期前の第１アンテナ１２１の位置を中心として、第１アンテナ１２１から車体フレーム１１１の回転中心Ｃｔｖへ向かうベクトルを回転させる。
　このときのベクトルの回転量は、たとえば、第１車両方向を算出することで決定される。ここで、第１車両方向は、たとえば、前述の処理Ｐ６０２と同様に、図９に示すアンテナ設置角δから方向修正パラメータを差し引いた値と、一周期前の第１アンテナ１２１から第２アンテナ１２２へ向かうベクトルＡｖとを用いて求めることができる。以上により、制御装置１５０は、処理Ｐ７０４において、車両１１０の回転中心Ｃｔｖを算出することができる。
　次に、制御装置１５０は、第２車両方向を算出する処理Ｐ７０５を実行する。この処理Ｐ７０５において、制御装置１５０は、前述の処理Ｐ７０３で算出した車両１１０の旋回の中心Ｃｔと、前述の処理Ｐ７０４で算出した車両１１０の回転中心Ｃｔｖとに基づいて、第２車両方向を算出して、図１０に示す処理Ｐ７を終了する。
　この処理Ｐ７で算出される第２車両方向は、測位装置１２０受信機１２３から出力された情報に由来する車両方向である。第２車両方向は、車両１１０の旋回の中心Ｃｔから車両１１０の回転中心Ｃｔｖへ向かうベクトルに対して右手系で求めた法線方向として、算出することができる。次に、制御装置１５０は、図３の方向修正パラメータを算出する処理Ｐ８を実行する。
　図１３は、方向修正パラメータを算出する処理Ｐ８のフロー図である。図１４は、処理Ｐ８を説明する模式図である。制御装置１５０は、処理Ｐ８を開始すると、まず、修正角φを算出する処理Ｐ８０１を実行する。修正角φは、たとえば、第１車両方向Ｄｖ１と第２車両方向Ｄｖ２とのなす角である。ここで、第１車両方向Ｄｖ１は、第１アンテナ１２１と第２アンテナ１２２の設置位置に由来する車両方向であり、第２車両方向Ｄｖ２は、測位装置１２０の受信機１２３から出力された第１アンテナ１２１の位置情報ｐに由来する車両方向である。
　すなわち、第１車両方向Ｄｖ１は、第１アンテナ１２１および第２アンテナ１２２の設置位置の誤差の影響を受けるのに対し、第２車両方向Ｄｖ２は、第１アンテナ１２１および第２アンテナ１２２の設置位置の誤差の影響を受けない。したがって、第２車両方向Ｄｖ２が実際の車両方向であると考え、制御装置１５０は、算出機能Ｆ１０７により、第１車両方向Ｄｖ１と第２車両方向Ｄｖ２との差分を算出し、第１車両方向Ｄｖ１を第２車両方向Ｄｖ２と等しくするための修正角φを算出する。
　次に、制御装置１５０は、方向修正パラメータを更新する処理Ｐ８０２を実行する。この処理Ｐ８０２において、制御装置１５０は、たとえば、算出機能Ｆ１０７により、記憶装置に記憶された方向修正パラメータに、前の処理Ｐ８０１で算出された修正角φを加算することで、方向修正パラメータを更新する。次に、制御装置１５０は、方向修正パラメータを記憶する処理Ｐ８０３を実行する。この処理Ｐ８０３において、制御装置１５０は、たとえば、記憶機能Ｆ１０８により、前の処理Ｐ８０２で更新された方向修正パラメータを記憶装置に記憶させ、図１３に示す処理Ｐ８を終了する。
　次に、制御装置１５０は、たとえば、判定機能Ｆ１０９により、図３に示すように、方向修正パラメータが収束したか否かを判定する処理Ｐ９を実行する。第２車両方向Ｄｖ２は、図１０に示す車両１１０の回転中心Ｃｔｖを算出する処理Ｐ７０４においても用いられている。そのため、第２車両方向Ｄｖ２にも、前回の方向修正パラメータでは除去しきれない誤差が生じている。この誤差を減少させるため、制御装置１５０は、処理Ｐ９において、前の処理Ｐ８で算出した最新の方向修正パラメータを用いて再度計算を行うことで、収束演算を実施する。
　処理Ｐ９において、制御装置１５０は、たとえば、図１３に示す処理Ｐ８０１で算出された修正角φの絶対値が、あらかじめ設定した閾値よりも小さい場合に、方向修正パラメータが収束した（ＹＥＳ）と判定し、次の処理Ｐ１０を実行する。一方、処理Ｐ９において、制御装置１５０は、たとえば、修正角φの絶対値が、あらかじめ設定した閾値以上である場合に、方向修正パラメータが収束していない（ＮＯ）と判定し、処理Ｐ６から処理Ｐ９までを繰り返し実行する。
　次に、制御装置１５０は、たとえば、判定機能Ｆ１０９により、方向修正パラメータの平均値を算出する処理Ｐ１０を実行する。方向修正パラメータは、サンプリング周期ごとに測位装置１２０の受信機１２３から制御装置１５０に入力される第１アンテナ１２１の位置情報ｐに基づいて算出される。そのため、第１アンテナ１２１が受信したＧＮＳＳの電波に基づいて受信機１２３が測位計算を行う際に発生する位置誤差によって、方向修正パラメータにも誤差が生じる。
　この方向修正パラメータの誤差を除去するために、制御装置１５０は、処理Ｐ１０において、方向修正パラメータの逐次平均を実行する。方向修正パラメータの逐次平均においては、たとえば、積算回数の上限を設定したり、忘却係数を導入したりすることで、積算回数の過度の増加を抑制してもよい。これにより、方向修正パラメータの変化が反映されなくなるのを防止できる。
　次に、制御装置１５０は、たとえば、判定機能Ｆ１０９により、方向修正パラメータの使用可否を判定する処理Ｐ１１を実行する。前述のように、各時刻において算出された方向修正パラメータは誤差を含むため、所定の期間に算出された方向修正パラメータの平均値を算出する。そのため、一定数以上の方向修正パラメータが算出されていることと、周期毎の方向修正パラメータの変動が過大にならないことが必要である。
　したがって、処理Ｐ１１において、制御装置１５０は、方向修正パラメータの算出数が所定の閾値以上であり、かつ、方向修正パラメータの最新の平均と一周期前の平均との差が所定の閾値以下である場合に、方向修正パラメータを使用可（ＹＥＳ）と判定する。この場合、制御装置１５０は、方向修正パラメータを用いて車両方向を補正する処理Ｐ１２を実行し、補正した車両方向に基づいて、推定機能Ｆ１１０により、車両１１０の位置および姿勢を推定する処理Ｐ１３を実行する。
　一方、処理Ｐ１１において、制御装置１５０は、方向修正パラメータを使用不可（ＮＯ）と判定すると、方向修正パラメータを使用せずに、推定機能Ｆ１１０により車両１１０の位置および姿勢を推定する処理Ｐ１３を実行する。最後に、制御装置１５０は、処理Ｐ１０で算出された方向修正パラメータの平均値と、処理Ｐ１３で推定された車両１１０の位置および姿勢を、状態監視機能Ｆ２００へＣＡＮを介して出力する処理Ｐ１４を実行する。以上により、制御装置１５０の算出機能Ｆ１００による図３に示す各処理が終了する。
　次に、制御装置１５０の状態監視機能Ｆ２００は、第１アンテナ１２１および第２アンテナ１２２の設置情報と、方向修正パラメータとに基づいて、第１アンテナ１２１または第２アンテナ１２２の設置位置に誤差が生じていることを検出する。また、制御装置１５０の状態監視機能Ｆ２００は、たとえば、方向修正パラメータが所定の閾値を超えた場合に、第１アンテナ１２１および第２アンテナ１２２の設置位置の異常を判定する。
　この場合、作業車両１００は、オペレータまたはユーザへ情報を通知するための情報通知装置を備えていてもよい。情報通知装置としては、たとえば、液晶表示装置、表示ランプ、スピーカ、ブザーなどを使用することができる。これにより、制御装置１５０の状態監視機能Ｆ２００は、方向修正パラメータが所定の閾値を超えた場合に、第１アンテナ１２１および第２アンテナ１２２の設置情報に対する誤差情報を情報通知装置へ出力して、作業車両１００のオペレータまたはユーザに異常を通知することができる。
　以上のように、本実施形態の作業車両１００は、車両１１０と、その車両１１０に取り付けられて衛星測位システムの電波を受信する第１アンテナ１２１および第２アンテナ１２２とを備えている。また、作業車両１００は、衛星測位システムの電波に基づく第１アンテナ１２１の位置情報ｐ、および、第１アンテナ１２１と第２アンテナ１２２との間の基線方向ｄを出力する受信機１２３を備えている。また、作業車両１００は、車両１１０の速度、加速度、および角速度を計測するセンサ１３０と、車両１１０の位置および姿勢を推定する制御装置１５０と、を備えている。そして、制御装置１５０は、検出機能Ｆ１０６と、算出機能Ｆ１０４と、算出機能Ｆ１０５と、算出機能Ｆ１０７と、推定機能Ｆ１１０と、を有する。検出機能Ｆ１０６は、車両１１０の速度、加速度、および角速度に基づいて定常走行を検出する機能である。算出機能Ｆ１０４は、車両１１０に対する第１アンテナ１２１および第２アンテナ１２２の設置情報に基づいて第１車両方向Ｄｖ１を算出する機能である。算出機能Ｆ１０５は、定常走行が検出された場合に、第１アンテナ１２１の位置情報ｐの時間変化に基づいて第２車両方向Ｄｖ２を算出する機能である。算出機能Ｆ１０７は、第２車両方向Ｄｖ２に基づいて第１車両方向Ｄｖ１を修正するための方向修正パラメータを算出する機能である。推定機能Ｆ１１０は、方向修正パラメータと第１車両方向Ｄｖ１に基づいて車両１１０の位置および姿勢を推定する機能である。
　このような構成により、本実施形態の作業車両１００によれば、従来の装置よりも柔軟に第１アンテナ１２１および第２アンテナ１２２の設置位置の誤差を検出することが可能になる。より詳細には、第１アンテナ１２１および第２アンテナ１２２は、衛星測位システムからの電波が車両１１０やベッセル１４０などによって遮蔽されないように、たとえば、車体フレーム１１１から上方に延びるポールの先端に取り付けられる。また、作業車両１００は、たとえば、ベッセル１４０に鉱石や土砂などを積載したり、ベッセル１４０から積載物を降ろしたり、鉱山や建設現場などの凹凸の多い地面の上を走行したりする。そのため、第１アンテナ１２１および第２アンテナ１２２には、振動や衝撃が作用することがある。すると、時間の経過にともなって、第１アンテナ１２１および第２アンテナ１２２のあらかじめ設定された設置位置と、実際の設置位置との間に誤差が生じることがある。このような場合、あらかじめ設定された設置位置に基づいて算出される第１車両方向Ｄｖ１と、実際の車両方向との間に誤差が生じることがある。
　そこで、本実施形態の作業車両１００は、前述のように、制御装置１５０の検出機能Ｆ１０６によって定常走行が検出された場合に、第１アンテナ１２１が受信した衛星測位システムの電波に基づく第１アンテナ１２１の位置情報ｐの時間変化を用いて第２車両方向Ｄｖ２を算出する。そして、第２車両方向Ｄｖ２に基づいて第１車両方向Ｄｖ１を修正するための方向修正パラメータを算出し、その方向修正パラメータと第１車両方向Ｄｖ１に基づいて車両１１０の位置および姿勢を推定する。すなわち、本実施形態の作業車両１００は、従来の装置のような極めて限定された走行状態だけではなく、より多様な走行状態を含む定常走行を行うことで、第１車両方向Ｄｖ１と実際の車両方向Ｄｖとの間の誤差をより柔軟に補正することができる。また、第１アンテナ１２１および第２アンテナ１２２の設置位置に生じた誤差を検出するための作業が不要となる。
　また、本実施形態の作業車両１００において、制御装置１５０は、方向修正パラメータの使用可否を判定する判定機能Ｆ１０９をさらに有している。そして、制御装置１５０の推定機能Ｆ１１０は、方向修正パラメータが使用可能である場合に、方向修正パラメータと第１車両方向Ｄｖ１に基づいて、車両１１０の位置および姿勢を推定する。この構成により、たとえば、大きな誤差を含む方向修正パラメータに基づいて第１車両方向Ｄｖ１が補正されることが防止され、第１車両方向Ｄｖ１をより正確に補正することが可能になる。
　また、本実施形態の作業車両１００において、定常走行は、中心および半径が同一の円周に沿って走行する旋回と、一直線に走行する直進とを含む。この構成により、従来の装置のような作業車両１００が略水平面を前後方向に進行する極めて限定された走行状態だけではなく、旋回と直進を含む定常走行を行うことで、第１車両方向Ｄｖ１と実際の車両方向との間の誤差をより柔軟に補正することができる。
　また、本実施形態の作業車両１００において、定常走行は、車両１１０の速度の平均値が所定の速度閾値Ｖｔｈよりも高いことを含む。この構成により、車両１１０が停止しているような場合や、車両１１０が速度閾値Ｖｔｈ以下の速度で徐行しているような場合を、定常走行から除外して、車両１１０の位置および姿勢をより正確に推定することが可能になる。
　また、本実施形態の作業車両１００は、車両１１０に取り付けられたベッセル１４０と、ベッセル１４０を昇降させる昇降機構である昇降シリンダ１４２と、ベッセル１４０の高さを検知する昇降センサ１３４と、を備えている。そして、本実施形態の作業車両１００において、定常走行は、ベッセル１４０の高さが、所定の高さ閾値Ｈｔｈよりも低いことを含む。
　この構成により、本実施形態の作業車両１００は、たとえばベッセル本体１４１の前端部１４１ａが、第１アンテナ１２１および第２アンテナ１２２の高さに基づく高さ閾値Ｈｔｈ以上である場合に、第２車両方向Ｄｖ２が算出されない。そのため、衛星測位システムから第１アンテナ１２１または第２アンテナ１２２への電波がベッセル１４０によって遮られた状態で第２車両方向Ｄｖ２を算出することが防止され、第２車両方向Ｄｖ２の誤差が減少する。したがって、本実施形態の作業車両１００によれば、車両１１０の位置および姿勢をより正確に推定することが可能になる。
　また、本実施形態の作業車両１００は、オペレータまたはユーザへ情報を通知するための情報通知装置を備えることができる。この場合、制御装置１５０は、方向修正パラメータが所定の閾値を超えた場合に、第１アンテナ１２１および第２アンテナ１２２の設置情報に対する誤差情報を、情報通知装置へ出力することができる。この構成により、本実施形態の作業車両１００は、オペレータまたはユーザへ第１アンテナ１２１または第２アンテナ１２２の設置位置の誤差が生じていることを通知できる。
　以上説明したように、本実施形態によれば、従来の装置よりも柔軟に第１アンテナ１２１または第２アンテナ１２２の設置位置の誤差を検出することが可能な作業車両１００を提供することができる。
［実施形態２］
　以下、実施形態１で用いた一部の図を援用し、さらに図１５から図１７を参照して、本開示に係る作業車両の実施形態２を説明する。図１５は、本開示に係る作業車両の実施形態２を示す制御装置１５０の機能ブロック図である。本実施形態の作業車両１００は、制御装置１５０が備える機能が、前述の実施形態１の作業車両１００と異なっている。本実施形態の作業車両１００のその他の点は、前述の実施形態１の作業車両１００と同様であるため、同様の部分には同一の符号を付して説明を省略する。
　本実施形態の作業車両１００において、制御装置１５０は、前述の実施形態１の算出機能Ｆ１００と同様の位置および姿勢の算出機能Ｆ１００’を有している。また、本実施形態の作業車両１００において、制御装置１５０は、状態監視機能Ｆ２００に代えて、走行制御機能Ｆ３００を有している。本実施形態の制御装置１５０において、算出機能Ｆ１００’は、推定機能Ｆ１１０による車両１１０の位置および姿勢の推定結果と、検出機能Ｆ１０６による定常走行の検知結果を、走行制御機能Ｆ３００へ出力する。
　走行制御機能Ｆ３００は、算出機能Ｆ１００’から入力された推定結果と検知結果に基づいて、車両１１０の走行制御を実行する。より具体的には、走行制御機能Ｆ３００は、車両１１０の各種のアクチュエータを制御して、車両１１０のアクセルペダル、ブレーキペダル、変速機、ステアリングホイールなどを自動的に操作して、車両１１０を走行させる。また、本実施形態の作業車両１００において、制御装置１５０は、たとえば、検出機能Ｆ１０６により、図３および図４に示す定常走行を検出する処理Ｐ４に代えて、図１６に示す定常走行を検出する処理Ｐ４’を実行する。
　図１６は、本実施形態の制御装置１５０の検出機能Ｆ１０６による処理Ｐ４’の一例を示すフロー図である。図１６に示す処理Ｐ４’において、図６に示す実施形態１の処理Ｐ４と同様の処理には、同一の符号を付して説明を省略する。
　図１６に示す処理Ｐ４’の処理Ｐ４０２において、制御装置１５０は、車両１１０の平均速度が所定の速度閾値Ｖｔｈ以下（ＮＯ）と判定すると、たとえば検出機能Ｆ１０６により、記憶装置に記憶された速度不足フラグを１に設定する処理Ｐ４１１を実行する。この速度不足フラグが１に設定された状態は、車両１１０の速度が不足しているために方向修正パラメータの算出ができない状態であることを示している。処理Ｐ４１１の終了後、制御装置１５０は、前述の実施形態１と同様に、定常走行フラグを０に設定する処理Ｐ４０８を実行する。
　また、図１６に示す処理Ｐ４’の処理Ｐ４０６において、制御装置１５０は、車両１１０の走行状態が定常走行ではない（ＮＯ）と判定すると、たとえば検出機能Ｆ１０６により、記憶装置に記憶された非定常フラグを１に設定する処理Ｐ４１２を実行する。この非定常フラグが１に設定された状態は、車両１１０の走行状態が定常走行ではなく、方向修正パラメータの算出ができない状態であることを示している。処理Ｐ４１２の終了後、制御装置１５０は、前述の実施形態１と同様に、定常走行フラグを０に設定する処理Ｐ４０８を実行する。
　なお、速度不足フラグおよび非定常フラグの初期値は０であり、制御装置１５０のサンプリング周期毎の起動と同時に初期化される。速度不足フラグまたは非定常フラグが１に設定されると、方向修正パラメータは算出されず、作業車両１００の姿勢の推定誤差が大きくなる。そこで、本実施形態の制御装置１５０は、走行制御機能Ｆ３００により、車両１１０の走行状態が定常走行となるように、車両１１０の走行制御を実行する。
　図１７は、本実施形態の作業車両１００における制御装置１５０の走行制御機能Ｆ３００による処理Ｐ１５の一例を示すフロー図である。制御装置１５０は、図１７に示す処理Ｐ１５を開始すると、まず、記憶装置に記憶された速度不足フラグおよび非定常フラグを取得する処理Ｐ１５０１を実行する。
　次に、制御装置１５０は、速度不足フラグが１であるか否かを判定する処理Ｐ１５０２を実行する。この処理Ｐ１５０２において、制御装置１５０は、速度不足フラグが１である（ＹＥＳ）と判定すると、車両１１０の速度が不足して方向修正パラメータが算出できない状態であることが判断できる。この場合、制御装置１５０は、車両１１０を速度閾値Ｖｔｈを超える所定の速度まで加速させる走行制御のパラメータを設定する処理Ｐ１５０３を実行する。この処理Ｐ１５０３の終了後、制御装置１５０は、後述する割り込みの有無を判定する処理Ｐ１５０９を実行する。
　一方、処理Ｐ１５０２において、制御装置１５０は、速度不足フラグが１ではない（ＮＯ）と判定すると、非定常フラグが１であるか否かを判定する処理Ｐ１５０４を実行する。この処理Ｐ１５０４において、制御装置１５０は、非定常フラグが１ではない（ＮＯ）と判定すると、車両１１０の走行状態は方向修正パラメータを算出可能な定常走行であると判断できることから、後述する割り込みの有無を判定する処理Ｐ１５０９を実行する。
　一方、処理Ｐ１５０４において、制御装置１５０は、非定常フラグが１である（ＹＥＳ）と判定すると、車両１１０の速度が所定の速度閾値Ｖｔｈ’よりも高いか否かを判定する処理Ｐ１５０５を実行する。この処理Ｐ１５０５において、制御装置１５０は、車両１１０の速度が速度閾値Ｖｔｈ’よりも高い（ＹＥＳ）と判定すると、車両１１０の速度が定常走行の速度範囲に入るように、車両１１０を減速させる走行制御のパラメータを設定する処理Ｐ１５０６を実行する。この処理Ｐ１５０６の終了後、制御装置１５０は、後述する割り込みの有無を判定する処理Ｐ１５０９を実行する。
　一方、処理Ｐ１５０５において、制御装置１５０は、車両１１０の速度が所定の速度閾値Ｖｔｈ’以下である（ＮＯ）と判定すると、車両１１０の速度に調整の余裕がないと判断できる。この場合、制御装置１５０は、車両１１０の角速度の絶対値｜α｜が所定の角速度閾値αｔｈよりも低いか否かを判定する処理Ｐ１５０７を実行する。この処理Ｐ１５０７において、制御装置１５０は、車両１１０の角速度の絶対値｜α｜が所定の角速度閾値αｔｈよりも低い（ＹＥＳ）と判定すると、車両１１０が直進していると判断できる。この場合、制御装置１５０は、ステアリングホイールの操舵角を維持する走行制御のパラメータを設定する処理Ｐ１５０８を実行する。この処理Ｐ１５０８の終了後、制御装置１５０は、後述する割り込みの有無を判定する処理Ｐ１５０９を実行する。
　一方、処理Ｐ１５０７において、制御装置１５０は、車両１１０の角速度の絶対値｜α｜が所定の角速度閾値αｔｈ以上である（ＮＯ）と判定すると、割り込みの有無を判定する処理Ｐ１５０９を実行する。この処理Ｐ１５０９において、制御装置１５０は、他の制御装置から制御を停止させる割り込み信号が入力されたか否かと、車両１１０のオペレータによる車両１１０の操作の割り込みがあるか否かを判定する。
　この処理Ｐ１５０９において、制御装置１５０は、割り込みがない（ＮＯ）と判定すると、前述の処理Ｐ１５０３、Ｐ１５０６、またはＰ１５０８において設定されたパラメータに基づいて車両１１０の走行制御を行う処理Ｐ１５１０を実行し、図１７に示す処理Ｐ１５を終了する。一方、処理Ｐ１５０９において、制御装置１５０は、割り込みがある（ＹＥＳ）と判定すると、前述の処理Ｐ１５０３、Ｐ１５０６、またはＰ１５０８において設定されたパラメータに基づく車両１１０の走行制御を中止する処理Ｐ１５１１を実行し、図１７に示す処理Ｐ１５を終了する。
　以上のように、本実施形態の作業車両１００において、制御装置１５０は、車両１１０の走行を制御する走行制御機能Ｆ３００を有している。この走行制御機能Ｆ３００は、検出機能Ｆ１０６によって車両１１０の定常走行が検出されなかった場合に、定常走行の条件を満たすように車両１１０を制御する。このような構成により、本実施形態の作業車両１００によれば、実施形態１の作業車両１００と同様の効果を奏することができるだけでなく、制御装置１５０によって車両１１０の走行状態を定常走行に制御して、より確実に方向修正パラメータを算出することが可能になる。
　以上、図面を用いて本開示に係る作業車両の実施形態を詳述してきたが、具体的な構成はこの実施形態に限定されるものではなく、本開示の要旨を逸脱しない範囲における設計変更等があっても、それらは本開示に含まれるものである。
１００　　作業車両
１１０　　車両
１２１　　第１アンテナ
１２２　　第２アンテナ
１２３　　受信機
１３０　　センサ
１３４　　昇降センサ
１４０　　ベッセル
１４２　　昇降シリンダ（昇降機構）
１５０　　制御装置
Ｃｔ　　　中心
ｄ　　　　基線方向
Ｄｖ１　　第１車両方向
Ｄｖ２　　第２車両方向
Ｆ１０４　第１車両方向を算出する機能
Ｆ１０５　第２車両方向を算出する機能
Ｆ１０６　検出機能
Ｆ１０７　方向修正パラメータを算出する機能
Ｆ１０９　方向修正パラメータの使用可否を判定する機能
Ｆ１１０　推定機能
Ｆ３００　走行制御機能
Ｈｔｈ　　高さ閾値
ｐ　　　　位置情報

Claims (7)

  1. 　車両と、前記車両に取り付けられて衛星測位システムの電波を受信する第１アンテナおよび第２アンテナと、前記電波に基づく前記第１アンテナの位置情報および前記第１アンテナと前記第２アンテナとの間の基線方向を出力する受信機と、前記車両の速度、加速度、および角速度を計測するセンサと、前記車両の位置および姿勢を推定する制御装置と、を備えた作業車両であって、
    　前記制御装置は、
    　前記速度、前記加速度、および前記角速度に基づいて定常走行を検出する検出機能と、　前記車両に対する前記第１アンテナおよび前記第２アンテナの設置情報に基づいて第１車両方向を算出する機能と、
    　前記定常走行が検出された場合に、前記第１アンテナの前記位置情報の時間変化に基づいて第２車両方向を算出する機能と、
    　前記第２車両方向に基づいて前記第１車両方向を修正するための方向修正パラメータを算出する機能と、
    　前記方向修正パラメータと前記第１車両方向に基づいて前記車両の前記位置および前記姿勢を推定する推定機能と、を有する作業車両。
  2. 　前記制御装置は、前記方向修正パラメータの使用可否を判定する機能をさらに有し、
    　前記推定機能は、前記方向修正パラメータが使用可能である場合に、前記方向修正パラメータと前記第１車両方向に基づいて前記車両の前記位置および前記姿勢を推定する、請求項１に記載の作業車両。
  3. 　前記定常走行は、中心および半径が同一の円周に沿って走行する旋回と、一直線に走行する直進とを含む、請求項１に記載の作業車両。
  4. 　前記定常走行は、前記速度の平均値が所定の速度閾値よりも高いことを含む、請求項１に記載の作業車両。
  5. 　前記車両に取り付けられたベッセルと、前記ベッセルを昇降させる昇降機構と、前記ベッセルの高さを検知する昇降センサと、を備え、
    　前記定常走行は、前記ベッセルの高さが、所定の高さ閾値よりも低いことを含む、請求項１に記載の作業車両。
  6. 　オペレータへ情報を通知するための情報通知装置を備え、
    　前記制御装置は、前記方向修正パラメータが所定の閾値を超えた場合に、前記第１アンテナおよび前記第２アンテナの前記設置情報に対する誤差情報を、前記情報通知装置へ出力する、請求項１に記載の作業車両。
  7. 　前記制御装置は、前記車両の走行を制御する走行制御機能を有し、
    　前記走行制御機能は、前記検出機能によって前記定常走行が検出されなかった場合に、前記定常走行の条件を満たすように前記車両を制御する、請求項１に記載の作業車両。
PCT/JP2021/010748 2020-03-26 2021-03-17 作業車両 Ceased WO2021193269A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
US17/782,036 US12105523B2 (en) 2020-03-26 2021-03-17 Work vehicle
EP21775765.7A EP4130801B1 (en) 2020-03-26 2021-03-17 Work vehicle
CN202180006899.6A CN114787661B (zh) 2020-03-26 2021-03-17 作业车辆

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2020055684A JP6951494B2 (ja) 2020-03-26 2020-03-26 作業車両
JP2020-055684 2020-03-26

Publications (1)

Publication Number Publication Date
WO2021193269A1 true WO2021193269A1 (ja) 2021-09-30

Family

ID=77891827

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2021/010748 Ceased WO2021193269A1 (ja) 2020-03-26 2021-03-17 作業車両

Country Status (5)

Country Link
US (1) US12105523B2 (ja)
EP (1) EP4130801B1 (ja)
JP (1) JP6951494B2 (ja)
CN (1) CN114787661B (ja)
WO (1) WO2021193269A1 (ja)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20220019559A (ko) * 2020-08-10 2022-02-17 삼성전자주식회사 통신 제어 장치 및 이에 의한 통신 링크의 수립 방법
US12325982B2 (en) * 2022-11-02 2025-06-10 Trimble Inc. Construction site orchestration using dynamic computer vision

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04359113A (ja) * 1991-06-06 1992-12-11 Matsushita Electric Ind Co Ltd 移動体方位算出装置
JP2002358122A (ja) * 2001-05-31 2002-12-13 Yanmar Agricult Equip Co Ltd 農業用作業車
JP2005043212A (ja) 2003-07-22 2005-02-17 Furuno Electric Co Ltd キャリア位相整数値バイアス決定方法、方位・姿勢計測方法および装置
WO2006067968A1 (ja) * 2004-12-20 2006-06-29 Matsushita Electric Industrial Co., Ltd. 進行方向測定装置
WO2007066695A1 (ja) * 2005-12-08 2007-06-14 Pioneer Corporation 方位算出装置、方位算出方法、方位算出プログラム、および記録媒体
JP2013169931A (ja) * 2012-02-22 2013-09-02 Hitachi Constr Mach Co Ltd 運搬車両
JP2013170903A (ja) * 2012-02-20 2013-09-02 Ono Sokki Co Ltd 計測装置及び計測方法
JP2013228318A (ja) * 2012-04-26 2013-11-07 Ono Sokki Co Ltd キャリブレーション良否判定装置及び方法

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8140223B2 (en) * 2003-03-20 2012-03-20 Hemisphere Gps Llc Multiple-antenna GNSS control system and method
US8190337B2 (en) * 2003-03-20 2012-05-29 Hemisphere GPS, LLC Satellite based vehicle guidance control in straight and contour modes
JP4082388B2 (ja) * 2004-06-01 2008-04-30 トヨタ自動車株式会社 走行制御装置
GB2450143A (en) * 2007-06-13 2008-12-17 Andreas Zachariah Mode of transport determination
US8224525B1 (en) * 2008-02-29 2012-07-17 Javad Gnss, Inc. Indication of attraction domains for controlled planar motion of a ground vehicle
US9494430B2 (en) * 2014-10-27 2016-11-15 Caterpillar Inc. Positioning system implementing multi-sensor pose solution
US9617717B2 (en) * 2015-06-29 2017-04-11 Komatsu Ltd. Construction machine control system and construction machine control method
US10241215B2 (en) 2015-11-19 2019-03-26 Agjunction Llc Sensor alignment calibration
US9821847B2 (en) * 2015-11-24 2017-11-21 Deere & Company Method for guiding an off-road vehicle along a curved path
US10661830B1 (en) * 2016-07-05 2020-05-26 Ag Leader Technology, Inc. Yaw orientation error detection
JP6493364B2 (ja) * 2016-11-18 2019-04-03 トヨタ自動車株式会社 運転支援装置
EP4276645A3 (en) * 2016-12-15 2024-01-10 Positec Power Tools (Suzhou) Co., Ltd. State detection method for an automatic working system and mobile station
JP6695297B2 (ja) * 2017-03-24 2020-05-20 ヤンマー株式会社 自律走行システム
JP7016297B2 (ja) * 2018-06-29 2022-02-04 日立建機株式会社 作業機械
JP6987711B2 (ja) * 2018-07-13 2022-01-05 株式会社クボタ 作業機
EP3894899B1 (en) * 2018-12-12 2025-02-12 Volvo Truck Corporation A method for estimating the attitude of a vehicle
US11178805B2 (en) * 2019-07-05 2021-11-23 Deere & Company Apparatus and methods for vehicle steering to follow a curved path

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04359113A (ja) * 1991-06-06 1992-12-11 Matsushita Electric Ind Co Ltd 移動体方位算出装置
JP2002358122A (ja) * 2001-05-31 2002-12-13 Yanmar Agricult Equip Co Ltd 農業用作業車
JP2005043212A (ja) 2003-07-22 2005-02-17 Furuno Electric Co Ltd キャリア位相整数値バイアス決定方法、方位・姿勢計測方法および装置
WO2006067968A1 (ja) * 2004-12-20 2006-06-29 Matsushita Electric Industrial Co., Ltd. 進行方向測定装置
WO2007066695A1 (ja) * 2005-12-08 2007-06-14 Pioneer Corporation 方位算出装置、方位算出方法、方位算出プログラム、および記録媒体
JP2013170903A (ja) * 2012-02-20 2013-09-02 Ono Sokki Co Ltd 計測装置及び計測方法
JP2013169931A (ja) * 2012-02-22 2013-09-02 Hitachi Constr Mach Co Ltd 運搬車両
JP2013228318A (ja) * 2012-04-26 2013-11-07 Ono Sokki Co Ltd キャリブレーション良否判定装置及び方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP4130801A4

Also Published As

Publication number Publication date
JP2021156678A (ja) 2021-10-07
JP6951494B2 (ja) 2021-10-20
CN114787661B (zh) 2025-10-31
US12105523B2 (en) 2024-10-01
US20230004171A1 (en) 2023-01-05
EP4130801A1 (en) 2023-02-08
EP4130801A4 (en) 2024-02-21
EP4130801B1 (en) 2025-05-07
CN114787661A (zh) 2022-07-22

Similar Documents

Publication Publication Date Title
JP5953426B2 (ja) 作業用車両の自動操舵システム
US9630672B2 (en) Roll angle estimation device and transport equipment
US10343717B2 (en) Travel vehicle and method for controlling travel vehicle
JP7133298B2 (ja) 運搬車両の管制システム及び運搬車両の管理方法
JP6951494B2 (ja) 作業車両
JP6148403B2 (ja) 作業機械の制御システム、作業機械、作業機械の管理システム及び作業機械の制御方法
JP4854778B2 (ja) 車両用推定航法装置、車両用推定航法及び車両用推定航法のプログラム
KR102939508B1 (ko) 전자 제어 장치
US11835643B2 (en) Work machine control system, work machine, and work machine control method
JP6614112B2 (ja) 前照灯制御装置
JP5398603B2 (ja) 左右重心位置推定装置、左右重心位置推定方法、および牽引車
JP2016024097A (ja) 運搬車両
JP6900341B2 (ja) 位置算出装置及びダンプトラック
JP7012001B2 (ja) 車両の姿勢推定装置
JP5906802B2 (ja) 舵角検出装置、舵角検出方法、及びプログラム
JP2019014446A (ja) 制御装置
JP2021142969A (ja) センサ誤差補正装置
JP2015040784A (ja) 車体傾斜角検出装置
JP2025187422A (ja) 搬送車両
KR20220155266A (ko) 오차 보정 장치
JP2019219211A (ja) 傾斜角計測装置および傾斜角計測方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21775765

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

ENP Entry into the national phase

Ref document number: 2021775765

Country of ref document: EP

Effective date: 20221026

WWG Wipo information: grant in national office

Ref document number: 2021775765

Country of ref document: EP

WWG Wipo information: grant in national office

Ref document number: 202180006899.6

Country of ref document: CN