WO2021187594A1 - 誤差補正装置 - Google Patents

誤差補正装置 Download PDF

Info

Publication number
WO2021187594A1
WO2021187594A1 PCT/JP2021/011182 JP2021011182W WO2021187594A1 WO 2021187594 A1 WO2021187594 A1 WO 2021187594A1 JP 2021011182 W JP2021011182 W JP 2021011182W WO 2021187594 A1 WO2021187594 A1 WO 2021187594A1
Authority
WO
WIPO (PCT)
Prior art keywords
work vehicle
sensor
vector
set position
error correction
Prior art date
Application number
PCT/JP2021/011182
Other languages
English (en)
French (fr)
Inventor
宏臣 荒金
豊織 吉田
正行 船山
Original Assignee
東京計器株式会社
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 東京計器株式会社 filed Critical 東京計器株式会社
Priority to CN202180019638.8A priority Critical patent/CN115244486A/zh
Priority to KR1020227028529A priority patent/KR20220155266A/ko
Priority to JP2022508440A priority patent/JPWO2021187594A1/ja
Priority to EP21771083.9A priority patent/EP4123408A4/en
Publication of WO2021187594A1 publication Critical patent/WO2021187594A1/ja

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
    • A01B69/007—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow
    • A01B69/008—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow automatic
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D6/00—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • 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/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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D15/00—Steering not otherwise provided for
    • B62D15/02—Steering position indicators ; Steering position determination; Steering aids
    • B62D15/025—Active steering aids, e.g. helping the driver by actively influencing the steering system after environment evaluation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D6/00—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits
    • B62D6/002—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits computing target steering angles for front or rear wheels
    • 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/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Definitions

  • An embodiment of the present invention relates to correction of measured values by a sensor device provided in a work vehicle.
  • a satellite positioning module that outputs position information based on satellite information from a satellite and a forward reference position that is a reference position for steering control of a work vehicle in forward travel are based on the position information.
  • a forward reference position calculation unit that calculates the forward reference position
  • a reverse reference position calculation unit that calculates the reverse reference position that is the reference position for steering control of the work vehicle in reverse travel, and a travel route and forward reference position during forward travel.
  • a steering control unit that outputs a forward steering control signal calculated based on the deviation from the above, and outputs a reverse steering control signal calculated based on the deviation between the travel path and the reverse reference position during reverse travel.
  • An automatic steering system including a steering mechanism for steering the work vehicle based on a forward steering control signal and a reverse steering control signal is known (see, for example, Patent Document 1).
  • a speed vector measured by a sensor device may be used, but depending on the running state of the work vehicle, the speed vector may deviate significantly from the vehicle body orientation of the work vehicle, causing an error. There is a problem.
  • the present invention has been made to solve the above-mentioned problems, and an object of the present invention is to provide a technique capable of reducing an error of a velocity vector.
  • the error correction device is an error correction device provided in the work vehicle to correct the error of the measured value by the sensor device provided with the GNSS sensor and the gyro sensor.
  • the first acquisition unit that acquires the speed vector of the work vehicle in the local coordinate system measured by the GNSS sensor and the angular speed in the sensor coordinate system having three axes based on the sensor device measured by the gyro sensor.
  • a second acquisition unit that acquires the steering amount of the work vehicle, and a setting position determination unit that determines a set position set at a position different from that of the sensor device in the sensor coordinate system based on the acquired steering amount.
  • the speed vector of the work vehicle at the determined set position is calculated, and the work measured by the GNSS sensor. It is provided with a speed vector correction unit that corrects the speed vector of the vehicle.
  • the error of the velocity vector can be reduced.
  • FIG. 1 is a schematic side view showing the configuration of an agricultural tractor according to an embodiment.
  • the vehicle to be steered by the automatic steering system is a work vehicle, specifically, an agricultural tractor 1 as shown in FIG.
  • the tractor 1 includes a vehicle body 10, two front wheels 11, and two rear wheels 12, and is a four-wheel vehicle that travels by rear-wheel drive. Any vehicle that can be steered is not limited to a work vehicle. It may be such a vehicle.
  • the tractor 1 includes a seat 13 for a driver to sit on, a steering column 15, a steering handle 16, a steering drive device 17, pedals 18 including an accelerator, a brake, etc., four support frames 19a to 19d, and a roof portion 19r. Be prepared. Further, it is assumed that the rotary tiller 41 as a work machine provided in the work vehicle is connected to the vehicle body 10 of the tractor 1 by a link mechanism.
  • a steering system for steering the tractor 1 by giving an deviation to the front wheels 11 is provided, and an input shaft 151 for inputting the steering wheel angle by the steering handle 16 or the steering drive device 17 to the steering system is provided.
  • a steering amount based on the rotation of the input shaft 151 is given to the front wheels 11.
  • the steering drive device 17 is a device that is not provided with a configuration for automatic steering control and is retrofitted to automatically steer the tractor 1 on the premise of manual steering, and the steering handle 16 is attached above.
  • the upper end portion of the input shaft 151 of the steering system is fitted below.
  • Four support frames 19a to 19d, each of which is formed long, are provided on the vehicle body 10 at different positions in front, back, left, and right in an upright state. The roof portion 19r formed on the flat plate is placed.
  • a sensor device 30 is provided on the upper surface of the roof portion 19r with the roof portion 19r as a pedestal. Since this sensor device 30 includes a GNSS sensor as described later, it is desirable to attach the sensor device 30 to the uppermost portion of the tractor 1 such as the roof portion 19r in order to receive the GNSS signal transmitted from the artificial satellite.
  • the sensor device 30 is provided with any of the four support frames 19a to 19d supporting the roof portion 19r as a pedestal.
  • the automatic steering control device 21 is provided on any of the support frames 19a and 19b located in the front. The automatic steering control device 21 controls the automatic steering of the tractor 1.
  • FIG. 2 is a block diagram showing the overall configuration of the automatic steering control system according to the embodiment.
  • the automatic steering control system provided in the tractor 1 includes a steering drive device 17 for driving an input shaft 151 in the steering system, a sensor device 30, an automatic steering control device 21, and FIG. It is composed of a handle angle control device 22 which is not used.
  • the steering drive device 17 is a transmission shaft 171 that transmits a driving force to the input shaft 151, a motor 172 that is an electric motor that drives the transmission shaft 171 and an encoder 173 that is a rotary encoder that detects the amount of rotation and the rotation position of the transmission shaft 171.
  • the motor 172 may be any type of motor as long as it can output a sufficient torque to rotate the input shaft 151.
  • the automatic steering control device 21 outputs a steering amount based on the value measured by the sensor device 30, and the handle angle control device 22 feedback-controls the steering drive device 17 based on the steering amount instructed by the automatic steering control device 21. do.
  • the steering drive device 17 controls the motor 172 so that the rotation position detected by the encoder 173 becomes a desired rotation position to drive the transmission shaft 171.
  • the tractor 1 configured to be manually steered by retrofitting the steering drive device 17, the sensor device 30, the automatic steering control device 21 and the steering wheel angle control device 22 to the tractor 1. It is possible to realize automatic steering.
  • the sensor device 30 includes a gyro sensor 31, an acceleration sensor 32, a GNSS sensor 33, and an error correction device 34, which are housed in the same housing.
  • the gyro sensor 31 detects the angular velocities around the three axes and measures the azimuth angle ⁇ , the pitch angle ⁇ , and the roll angle ⁇ of the tractor 1.
  • the acceleration sensor 32 measures the acceleration of three axes with respect to the tractor 1.
  • the GNSS sensor 33 measures the vehicle position, which is the position of the tractor 1, and the speed vector of the tractor 1.
  • the error correction device 34 corrects the error of the measured value by the gyro sensor 31, the acceleration sensor 32, and the GNSS sensor 33.
  • the automatic steering control device 21 advances the tractor 1 so as to match the vehicle position with the target traveling path based on the vehicle position which is the position of the tractor 1 detected by the GNSS sensor 33 and the set target traveling path.
  • the target traveling direction which is the direction, is calculated, and the steering amount is calculated based on the traveling direction deviation, which is the deviation between the target traveling direction and the traveling direction of the tractor 1 based on the speed vector by the GNSS sensor 33, and the steering wheel angle is controlled.
  • Output to device 22 is controlled.
  • the error correction device 34 includes a CPU (Central Processing Unit) 91, a RAM (Random Access Memory) 92, a storage device 93, and an external I / F (Interface) 94 as hardware.
  • a CPU Central Processing Unit
  • RAM Random Access Memory
  • storage device 93 storage device
  • I / F Interface
  • the CPU 91 and the RAM 92 cooperate with each other to execute various functions described later, and the storage device 93 stores various data used for processing executed by the various functions.
  • the external I / F 94 inputs / outputs data to / from the gyro sensor 31, the acceleration sensor 32, the GNSS sensor 33, and the automatic steering control device 21.
  • the automatic steering control device 21 may also include a CPU, RAM, a storage device, and an external I / F, and may execute various functions described later in place of the error correction device 34.
  • the error correction device 34 functions as a low-pass filter 342 having a first acquisition unit 341, a first low-pass filter 342a and a second low-pass filter 342b, a second acquisition unit 343, and a set position determination.
  • a unit 344, a coordinate conversion unit 345, a vehicle position correction unit 346, a speed vector correction unit 347, a third acquisition unit 348, and a vehicle body orientation calculation unit 349 are provided.
  • the first acquisition unit 341 acquires the measured values measured by each of the gyro sensor 31, the acceleration sensor 32, and the GNSS sensor 33.
  • the first low-pass filter 342a gradually reduces high-frequency signals higher than the preset first cutoff frequency with respect to the time-series signal of the value measured by the gyro sensor 31 acquired by the first acquisition unit 341.
  • the second low-pass filter 342b gradually reduces the high frequency signal higher than the preset second cutoff frequency with respect to the time-series signal of the value measured by the acceleration sensor 32 acquired by the first acquisition unit 341.
  • the sensor device 30 since the sensor device 30 includes the GNSS sensor 33, in order to receive the GNSS signal, the sensor is attached to any of the four support frames 19a to 19d or the roof portion 19r supported by the support frames 19a to 19d. It is desirable to attach the device 30.
  • the members supporting the roof corresponding to the four support frames 19a to 19d in the present embodiment are not sufficiently rigid, and therefore, especially when the work vehicle travels on a rough road surface.
  • a high frequency amplitude is applied to the sensor device 30. Such a high-frequency amplitude causes an error in the measured values of the gyro sensor 31 and the acceleration sensor 32, and the low-pass filter 342 can reduce such an error.
  • the second acquisition unit 343 acquires the steering amount output by the automatic steering control device 21.
  • the set position determination unit 344 determines the set position based on the steering amount acquired by the second acquisition unit 343.
  • the set position is a position where the measurement position by the sensor device 30 is projected onto the traveling surface of the tractor 1, that is, the surface of the field, and is set to the center position in the width direction of the tractor 1 with respect to the position in the front-rear direction of the tractor 1. Is determined based on the steering amount.
  • the coordinate conversion unit 345 converts each of the relative position vector and the angular velocity vector in the sensor coordinate system into a vector in the local coordinate system.
  • the sensor coordinate system is a coordinate system having three axes of x, y, and z orthogonal to each other with the measurement position, which is the position where the sensor device 30 is attached and the position where the measurement by the sensor device 30 is performed, as the origin.
  • the x-axis is directed to the front side in the straight-ahead direction of the tractor 1
  • the y-axis is directed to the right side in the width direction of the tractor 1
  • the z-axis is directed to the lower side in the vertical direction of the tractor 1.
  • the local coordinate system is a coordinate system having three axes pointing north, east, and downward. The relative position vector and the angular velocity vector will be described in detail later.
  • the vehicle position correction unit 346 detects the vehicle position of the tractor 1 in the local coordinate system measured by the GNSS sensor 33 at the measurement position of the sensor device 30 attached to the roof portion 19r in the present embodiment, above the tractor 1. It is corrected to the vehicle position of the tractor 1 at the preset position set in the coordinate system.
  • the velocity vector correction unit 347 corrects the velocity vector of the tractor 1 in the local coordinate system by the GNSS sensor 33 at the measurement position of the sensor device 30 to the velocity vector at the set position.
  • the third acquisition unit 348 acquires whether the tractor 1 is in the forward state or the reverse state as the traveling direction.
  • the vehicle body direction calculation unit 349 calculates the direction of the vehicle body 10 of the tractor 1 as the direction based on the traveling state acquired by the third acquisition unit 348 and the speed vector corrected by the speed vector correction unit 347.
  • FIG. 5 is a flowchart showing the operation of the error correction device.
  • 6 and 7 are a schematic front view and a schematic plan view showing relative position vectors, respectively.
  • FIG. 8 is a schematic front view showing the velocity vector. The operation shown in FIG. 5 is assumed to be executed at predetermined intervals.
  • the first acquisition unit 341 acquires the vehicle position and the speed vector in the local coordinate system measured by the GNSS sensor 33 (S101), and the tractor 1 measured by the gyro sensor 31.
  • the angular velocity and the acceleration of the tractor 1 measured by the acceleration sensor 32 are acquired (S102).
  • the low-pass filter 342 gradually reduces high-frequency signals higher than the preset first cutoff frequency and second cutoff frequency for each of the angular velocity and acceleration acquired by the first acquisition unit 341 (S103).
  • C indicates a coordinate transformation matrix for performing coordinate transformation from the sensor coordinate system to the local coordinate system.
  • each element of the coordinate transformation matrix C is defined as follows, where the azimuth angle is ⁇ , the pitch angle is ⁇ , and the roll angle is ⁇ .
  • an initialization process generally called alignment is performed.
  • the initial orientation (azimuth angle) and initial posture (pitch angle and roll angle) of the tractor 1 are determined, and in the coordinate conversion by the coordinate conversion unit 345, the initial orientation and the initial posture are the gyro sensor 31.
  • the azimuth angle ⁇ , the pitch angle ⁇ , and the roll angle ⁇ updated by the angular velocity measured by are given to the coordinate conversion matrix C.
  • the gravitational acceleration measured by the acceleration sensor 32 when the tractor 1 is stationary is g
  • the accelerations of the three axes are a x , a y , and a z, respectively. It is calculated from the formula of.
  • the azimuth angle ⁇ as the initial direction is set based on the velocity vector measured by the GNSS sensor 33 while the tractor 1 is traveling.
  • preset values may be used for the initial posture and the initial orientation.
  • the coordinates are converted to ⁇ (S107).
  • the error correction device 34 corrects each of the vehicle position and the speed vector measured at the measurement position s to the vehicle position and the speed vector at the set position a. According to the corrected vehicle position and speed vector, a more accurate vehicle position and traveling direction that reduce the error of the measured value due to the runout of the sensor device 30 that occurs when the tractor 1 travels in a particularly uneven field. And the traveling speed can be obtained by the automatic steering control device 21. As a result, accurate automatic steering of the tractor 1 by the automatic steering control device 21 is achieved.
  • FIG. 9 is a flowchart showing the operation of the set position correction process according to the present embodiment.
  • 10 and 11 are schematic plan views showing a tractor during a forward course change and a forward turn by steering the front wheels, respectively.
  • the second acquisition unit 343 acquires the steering amount output by the automatic steering control device 21 (S201), and the set position determination unit 344 sets the acquired steering amount in advance. It is determined whether or not the threshold value is equal to or higher than the threshold value (S202).
  • the threshold value is set to a steering amount such that the tractor 1 turns.
  • the set position determining unit 344 sets the first set position a1 which is the vehicle center of gravity or the vehicle motion center of the tractor 1 to the set position a as shown in FIG. Determine (S203).
  • the first set position a1 may be positioned at least between the axle center f of the front wheels 11 and the axle center r of the rear wheels 12 in the front-rear direction of the tractor 1.
  • the set position determining unit 344 sets the second set position a2 located at the axle center r of the rear wheel 12 to the set position a, as shown in FIG. (S204).
  • the second set position a2 may be positioned at least on the rear wheel 12 side of the first set position a1 in the front-rear direction of the tractor 1, in other words, on the non-steering wheel side.
  • FIG. 12 is a flowchart showing the operation of the vehicle body direction calculation process.
  • FIG. 13 is a schematic plan view showing a tractor during reverse turning by steering the front wheels.
  • the third acquisition unit 348 acquires whether the tractor 1 is in the forward state or the reverse state as the traveling direction (S301).
  • the third acquisition unit 348 acquires the state of the lever for switching forward / reverse, the switching state of the forward gear / reverse gear in the transmission for transmitting the power of the prime mover, and the like as the traveling direction.
  • the vehicle body direction calculation unit 349 determines whether or not the traveling direction acquired by the third acquisition unit 348 is reverse (S302).
  • the vehicle body orientation calculation unit 349 determines whether the traveling direction is backward (S302, YES). If the traveling direction is backward (S302, YES), the vehicle body orientation calculation unit 349, as shown in FIG. 13, by inverting the speed vector V a x-direction, i.e. in the longitudinal direction of the tractor 1 (S303), the inverted calculate the direction of the velocity vector V a as the vehicle azimuth (S304).
  • the vehicle body orientation calculation unit 349 calculates the direction of the velocity vector V a as the vehicle azimuth (S304).
  • the tractor is rear-wheel drive, and accordingly, the set position determined by the set position determination process in the error correction device is different from that of the first embodiment.
  • 14 and 15 are schematic plan views showing tractors during forward course change and forward turning by steering the rear wheels, respectively.
  • the tractor 1a is configured as a so-called rear-wheel drive vehicle including front wheels 11a which are non-steering wheels and rear wheels 12a which are steering wheels.
  • the set position when the steering amount is less than the threshold value, that is, the first set position is the axle center f of the front wheels 11a and the rear in the front-rear direction of the tractor 1a, as in the first embodiment.
  • the axle center r of the wheel 12a preferably the center of gravity of the vehicle or the center of motion of the vehicle.
  • the set position when the steering amount is equal to or greater than the threshold value, that is, the second set position a2 is the front wheel 11a located on the non-steering wheel side of the first set position a1 in the front-rear direction of the tractor 1a. It becomes the axle center f of.
  • the vehicle position and the speed vector at the measurement position s are corrected to the vehicle position and the speed vector at the set position a, but the set positions a of the vehicle position and the speed vector may be different. ..
  • the vehicle position is always corrected to the vehicle position at the first set position a1, and only the speed vector is the speed vector at either the first set position a1 or the second set position a2 according to the steering amount. It may be corrected to.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Mechanical Engineering (AREA)
  • Environmental Sciences (AREA)
  • Soil Sciences (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Transportation (AREA)
  • Guiding Agricultural Machines (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

作業車両に設けられてＧＮＳＳセンサとジャイロセンサとを備えたセンサ装置による測定値の誤差を補正する誤差補正装置３４であって、ＧＮＳＳセンサにより測定されたローカル座標系における作業車両の速度ベクトルと、ジャイロセンサにより測定されたセンサ装置を基準とした３軸を有するセンサ座標系における角速度とを取得する第１取得部３４１と、作業車両における操舵量を取得する第２取得部３４３と、センサ座標系においてセンサ装置と異なる位置に設定された設定位置を取得された操舵量に基づいて決定する設定位置決定部３４４と、測定位置における作業車両の速度ベクトルと、取得された角速度とに基づいて、決定された設定位置における作業車両の速度ベクトルを算出し、ＧＮＳＳセンサにより測定された作業車両の速度ベクトルを補正する速度ベクトル補正部３４７とを備えた。

Description

誤差補正装置
　本発明の実施形態は、作業車両に設けられたセンサ装置による測定値の補正に関する。
　従来、農作業の効率化を目的として、トラクタや田植機などの作業車両を指定した走行経路に追従させる自動操舵制御が行われている。この自動操舵制御によれば、目標とする走行経路上の位置と作業車両の位置との離間距離に基づいて、作業車両の位置を走行経路上に位置付けるように作業車両の操舵が制御される。
　また、作業車両の自動操舵に関する技術として、衛星からの衛星情報に基づいて位置情報を出力する衛星測位モジュールと、前進走行における作業車の操舵制御の基準位置となる前進基準位置を位置情報に基づいて算出する前進基準位置算出部と、後進走行における作業車の操舵制御の基準位置となる後進基準位置を位置情報に基づいて算出する後進基準位置算出部と、前進走行時に走行経路と前進基準位置との偏差に基づいて算出された前進用操舵制御信号を出力するとともに、後進走行時に走行経路と後進基準位置との偏差に基づいて算出された後進用操舵制御信号を出力する操舵制御部と、前進用操舵制御信号及び後進用操舵制御信号に基づいて前記作業車の操舵を行う操舵機構とを備えた自動操舵システム、が知られている（例えば、特許文献１参照）。
特開２０１８－１２０３６４号公報
　作業車両の自動操舵制御においては、センサ装置により測定される速度ベクトルが用いられることがあるが、この速度ベクトルが作業車両の走行状態によっては、作業車両の車体方位と大きくずれてしまい誤差を生じるという問題がある。
　本発明は、上述した問題点を解決するためになされたものであり、速度ベクトルの誤差を低減することができる技術を提供することを目的とする。
　上述した課題を解決するため、本実施形態に係る誤差補正装置は、作業車両に設けられてＧＮＳＳセンサとジャイロセンサとを備えたセンサ装置による測定値の誤差を補正する誤差補正装置であって、前記ＧＮＳＳセンサにより測定されたローカル座標系における前記作業車両の速度ベクトルと、前記ジャイロセンサにより測定された前記センサ装置を基準とした３軸を有するセンサ座標系における角速度とを取得する第１取得部と、前記作業車両における操舵量を取得する第２取得部と、前記センサ座標系において前記センサ装置と異なる位置に設定された設定位置を前記取得された操舵量に基づいて決定する設定位置決定部と、前記測定位置における前記作業車両の速度ベクトルと、前記取得された角速度とに基づいて、前記決定された設定位置における前記作業車両の速度ベクトルを算出し、前記ＧＮＳＳセンサにより測定された前記作業車両の速度ベクトルを補正する速度ベクトル補正部とを備える。
　本発明によれば、速度ベクトルの誤差を低減することができる。
第１の実施形態に係る農業用トラクタの構成を示す概略側面図である。 第１の実施形態に係る自動操舵制御システムの全体構成を示すブロック図である。 誤差補正装置のハードウェア構成を示すブロック図である。 誤差補正装置の機能構成を示すブロック図である。 誤差補正装置の動作を示すフローチャートである。 相対位置ベクトルを示す概略正面図である。 相対位置ベクトルを示す概略平面図である。 速度ベクトルを示す概略正面図である。 設定位置補正処理の動作を示すフローチャートである。 前輪操舵による前進進路変更中のトラクタを示す概略平面図である。 前輪操舵による前進旋回中のトラクタを示す概略平面図である。 車体方向算出処理の動作を示すフローチャートである。 前輪操舵による後進旋回中のトラクタを示す概略平面図である。 後輪操舵による前進進路変更中中のトラクタを示す概略平面図である。 後輪操舵による前進旋回中のトラクタを示す概略平面図である。
　以下、図面を参照しながら、本発明の実施形態について説明する。
＜第１の実施形態＞
（作業車両の構成）
　まず、本実施形態に係る誤差補正装置を備える作業車両について説明する。図１は、実施形態に係る農業用トラクタの構成を示す概略側面図である。
　本実施形態に係る自動操舵システムの操舵対象としての車両は、作業車両であり、具体的には、図１に示すような農業用のトラクタ１とする。このトラクタ１は、車体１０と、２つの前輪１１と２つの後輪１２とを備え、後輪駆動により走行する四輪車両とするが、操舵可能な車両であれば、作業車両に限らずどのような車両であっても良い。また、トラクタ１は、運転者が座るための座席１３、ステアリングコラム１５、ステアリングハンドル１６、操舵駆動装置１７、アクセル、ブレーキ等を含むペダル類１８、４つの支持フレーム１９ａ～１９ｄ、屋根部１９ｒを備える。また、トラクタ１の車体１０には、作業車両に設けられる作業機としてのロータリ耕耘機４１がリンク機構により連結されているものとする。
　ステアリングコラム１５内には、前輪１１に偏角を与えてトラクタ１を操舵する操舵系が備えられるとともに、ステアリングハンドル１６または操舵駆動装置１７によるハンドル角を操舵系に入力するための入力軸１５１が内蔵され、この入力軸１５１の回転に基づく操舵量が前輪１１に与えられる。操舵駆動装置１７は、自動操舵制御をするための構成が備えられていない、手動操舵を前提としたトラクタ１を自動操舵するために後付けされる装置であり、上方においては、ステアリングハンドル１６が取り付けられるとともに、下方において、操舵系の入力軸１５１の上端部が嵌合される。車体１０上には、それぞれが長尺に形成された４つの支持フレーム１９ａ～１９ｄが起立した状態で前後左右の異なる位置に設けられ、これら４つの支持フレーム１９ａ～１９ｄ上には、全体として略平板上に形成された屋根部１９ｒが載置される。
　屋根部１９ｒの上面には、この屋根部１９ｒを架台としてセンサ装置３０が設けられる。このセンサ装置３０は、後述するようにＧＮＳＳセンサを含むことから、人工衛星から送信されるＧＮＳＳ信号を受信するために、屋根部１９ｒのような、トラクタ１における最上部に取り付けることが望ましく、トラクタ１が屋根部１９ｒを備えない場合は、屋根部１９ｒを支持する４つの支持フレーム１９ａ～１９ｄのいずれかを架台としてセンサ装置３０が設けられるものとする。また、前方に位置する支持フレーム１９ａ，１９ｂのいずれかには、自動操舵制御装置２１が設けられるものとする。この自動操舵制御装置２１は、トラクタ１の自動操舵を制御するものである。
（自動操舵制御システムの構成）
　図２は、実施形態に係る自動操舵制御システムの全体構成を示すブロック図である。
　図２に示すように、トラクタ１に備えられた自動操舵制御システムは、操舵系における入力軸１５１を駆動する操舵駆動装置１７と、センサ装置３０と、自動操舵制御装置２１と、図１に図示されないハンドル角制御装置２２とにより構成される。
　操舵駆動装置１７は、入力軸１５１に駆動力を伝達する伝達軸１７１、伝達軸１７１を駆動する電動モータであるモータ１７２、伝達軸１７１の回転量及び回転位置を検知するロータリエンコーダであるエンコーダ１７３を備える。なお、モータ１７２は、入力軸１５１を回転させるのに十分なトルクを出力可能なモータであれば、どのような種類のモータであっても良い。
　自動操舵制御装置２１は、センサ装置３０による測定値に基づいて操舵量を出力し、ハンドル角制御装置２２は、自動操舵制御装置２１により指示された操舵量に基づいて操舵駆動装置１７をフィードバック制御する。ここで、操舵駆動装置１７は、エンコーダ１７３により検知された回転位置が所望の回転位置となるようにモータ１７２を制御して伝達軸１７１を駆動させる。
　このように、トラクタ１に対して、操舵駆動装置１７、センサ装置３０、自動操舵制御装置２１及びハンドル角制御装置２２を後付けで設置することによって、手動操舵されるように構成されたトラクタ１において自動操舵を実現することが可能となる。
　センサ装置３０は、ジャイロセンサ３１、加速度センサ３２、ＧＮＳＳセンサ３３、誤差補正装置３４を備え、これらは同一の筐体内に収容される。このことは、ジャイロセンサ３１、加速度センサ３２、ＧＮＳＳセンサ３３が、トラクタ１において、略同一箇所に設けられることを意味する。ジャイロセンサ３１は、３軸周りの角速度を検出してトラクタ１の方位角ξ、ピッチ角θ、ロール角φを測定する。加速度センサ３２は、トラクタ１について３軸の加速度を測定する。ＧＮＳＳセンサ３３は、トラクタ１の位置である車両位置と、トラクタ１の速度ベクトルを測定する。誤差補正装置３４は、ジャイロセンサ３１、加速度センサ３２、ＧＮＳＳセンサ３３による測定値の誤差を補正する。
　自動操舵制御装置２１は、ＧＮＳＳセンサ３３により検出されたトラクタ１の位置である車両位置と、設定された目標走行経路とに基づいて、車両位置を目標走行経路に一致させるようなトラクタ１の進行方位である目標進行方位を演算し、この目標進行方位と、ＧＮＳＳセンサ３３による速度ベクトルに基づくトラクタ１の進行方位との偏差である進行方位偏差に基づいて、操舵量を演算してハンドル角制御装置２２へ出力する。
（誤差補正装置の構成）
　誤差補正装置のハードウェア構成及び機能構成について説明する。図３、図４は、それぞれ、誤差補正装置のハードウェア構成、機能構成を示すブロック図である。
　誤差補正装置３４は、ハードウェアとして、図３に示すように、ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）９１、ＲＡＭ（Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）９２、記憶装置９３、外部Ｉ／Ｆ（Ｉｎｔｅｒｆａｃｅ）９４を備える。
　ＣＰＵ９１及びＲＡＭ９２は協働して後述する各種機能を実行し、記憶装置９３は各種機能により実行される処理に用いられる各種データを記憶する。外部Ｉ／Ｆ９４は、ジャイロセンサ３１、加速度センサ３２、ＧＮＳＳセンサ３３、自動操舵制御装置２１とのデータの入出力を行う。なお、自動操舵制御装置２１もＣＰＵ、ＲＡＭ、記憶装置、外部Ｉ／Ｆを備えるものとし、誤差補正装置３４に代わって後述する各種機能を実行するようにしても良い。
　また、誤差補正装置３４は、機能として、図４に示すように、第１取得部３４１、第１ローパスフィルタ３４２ａ及び第２ローパスフィルタ３４２ｂを有するローパスフィルタ３４２、第２取得部３４３、設定位置決定部３４４、座標変換部３４５、車両位置補正部３４６、速度ベクトル補正部３４７、第３取得部３４８、車体方位算出部３４９を備える。
　第１取得部３４１は、ジャイロセンサ３１、加速度センサ３２、ＧＮＳＳセンサ３３のそれぞれにより測定された測定値を取得する。第１ローパスフィルタ３４２ａは、第１取得部３４１により取得されたジャイロセンサ３１による測定値の時系列信号に対して、予め設定された第１遮断周波数より高い高周波信号を逓減させる。第２ローパスフィルタ３４２ｂは、第１取得部３４１により取得された加速度センサ３２による測定値の時系列信号に対して、予め設定された第２遮断周波数より高い高周波信号を逓減させる。
　上述したように、センサ装置３０にＧＮＳＳセンサ３３が含まれていることから、ＧＮＳＳ信号を受信するには、４つの支持フレーム１９ａ～１９ｄのいずれか、またはこれらに支持される屋根部１９ｒにセンサ装置３０を取り付けることが望ましい。トラクタ１などの作業車両においては、本実施形態における４つの支持フレーム１９ａ～１９ｄに相当するような屋根を支持する部材は剛性が十分でないため、特に作業車両が凹凸の激しい路面を走行した際などにセンサ装置３０に高周波の振幅が加わることになる。このような高周波の振幅は、特にジャイロセンサ３１、加速度センサ３２による測定値に誤差を生じさせることとなるが、ローパスフィルタ３４２によれば、このような誤差を低減させることができる。
　第２取得部３４３は、自動操舵制御装置２１により出力される操舵量を取得する。設定位置決定部３４４は、第２取得部３４３により取得された操舵量に基づいて設定位置を決定する。ここで、設定位置は、センサ装置３０による測定位置をトラクタ１の走行面、即ち圃場表面に投影した位置であり、トラクタ１の幅方向における中心位置に設定され、トラクタ１の前後方向の位置については操舵量に基づいて決定される。
　座標変換部３４５は、センサ座標系における相対位置ベクトル及び角速度ベクトルのそれぞれをローカル座標系におけるベクトルに座標変換する。ここで、センサ座標系は、センサ装置３０が取り付けられた位置であり、センサ装置３０による測定がなされる位置である測定位置を原点として互いに直交するｘ，ｙ，ｚの３軸を有する座標系であり、ｘ軸はトラクタ１の直進方向前側に向き、ｙ軸はトラクタ１の幅方向右側に向き、ｚ軸はトラクタ１の上下方向下側に向くものとする。ローカル座標系は、北、東、下を向く３軸を有する座標系である。なお、相対位置ベクトル、角速度ベクトルについては後に詳述する。
　車両位置補正部３４６は、トラクタ１における上方、本実施形態においては屋根部１９ｒに取り付けられたセンサ装置３０の測定位置においてＧＮＳＳセンサ３３により測定されたローカル座標系におけるトラクタ１の車両位置を、センサ座標系において予め設定された設定位置におけるトラクタ１の車両位置に補正する。速度ベクトル補正部３４７は、センサ装置３０の測定位置においてＧＮＳＳセンサ３３によるローカル座標系におけるトラクタ１の速度ベクトルを、設定位置における速度ベクトルに補正する。
　第３取得部３４８は、トラクタ１が前進状態または後進状態のいずれにあるのかを走行方向として取得する。車体方位算出部３４９は、第３取得部３４８により取得された走行状態と、速度ベクトル補正部３４７により補正された速度ベクトルとに基づいて、トラクタ１の車体１０の向きを方位として算出する。
（誤差補正装置の動作）
　誤差補正装置の動作について説明する。図５は、誤差補正装置の動作を示すフローチャートである。図６、図７は、それぞれ、相対位置ベクトルを示す概略正面図、概略平面図である。図８は、速度ベクトルを示す概略正面図である。なお、図５に示す動作は、所定の周期毎に実行されるものとする。
　図５に示すように、まず、第１取得部３４１は、ＧＮＳＳセンサ３３により測定されたローカル座標系における車両位置と速度ベクトルとを取得し（Ｓ１０１）、ジャイロセンサ３１により測定されたトラクタ１の角速度と、加速度センサ３２により測定されたトラクタ１の加速度とを取得する（Ｓ１０２）
　次に、ローパスフィルタ３４２は、第１取得部３４１により取得された角速度及び加速度のそれぞれについて、予め設定された第１遮断周波数、第２遮断周波数より高い高周波信号を逓減させる（Ｓ１０３）。
　次に、後述する設定位置決定処理が実行される（Ｓ１０４）。
　設定位置決定処理の実行後、座標変換部３４５は、図６及び図７に示すような、センサ座標系において設定位置ａを始点とする測定位置ｓの相対位置ベクトルＬｓを、ジャイロセンサ３１により測定されるトラクタ１の方位角、ピッチ角、ロール角に基づいて、Ｌ＝ＣＬｓの式によりローカル座標系における相対位置ベクトルＬに座標変換する（Ｓ１０５）。ここで、Ｃは、センサ座標系からローカル座標系への座標変換を行うための座標変換行列を示し、
Figure JPOXMLDOC01-appb-M000001
 である。ここで、座標変換行列Ｃの各要素は、方位角をξ、ピッチ角をθ、ロール角をφとして、以下のように定められる。
Figure JPOXMLDOC01-appb-M000002
　また、座標変換に先立って、一般的にアライメントと呼ばれる初期化処理がなされる。この初期化処理によれば、トラクタ１の初期方位（方位角）と初期姿勢（ピッチ角及びロール角）が決定され、座標変換部３４５による座標変換において、この初期方位及び初期姿勢がジャイロセンサ３１により計測される角速度により更新された方位角ξ、ピッチ角θ、ロール角φが座標変換行列Ｃに与えられる。
　初期姿勢におけるピッチ角θとロール角φは、トラクタ１が静止した状態において加速度センサ３２により測定される重力加速度をｇとし、３軸の加速度のそれぞれをａｘ，ａｙ，ａｚとして、以下の式から求められる。
Figure JPOXMLDOC01-appb-M000003
　また、初期方位としての方位角ξは、トラクタ１の走行中においてＧＮＳＳセンサ３３により測定される速度ベクトルに基づいて設定される。なお、初期姿勢及び初期方位は、予め設定された値を用いても良い。
　座標変換後、車両位置補正部３４６は、このローカル座標系の原点Ｏを始点とした設定位置ａの位置ベクトルをＰａ、測定位置ｓの位置ベクトルをＰｓとして、Ｐａ＝Ｐｓ－Ｌの式により、位置ベクトルＰａを算出することにより、測定位置ｓにおいて測定された車両位置を、設定位置補正処理により決定された設定位置ａにおける車両位置に補正する（Ｓ１０６）。
　次に、座標変換部３４５は、ジャイロセンサ３１により測定されるセンサ座標系におけるトラクタ１の角速度ベクトルωｓを、座標変換行列Ｃを用いて、ω＝Ｃωｓの式によりローカル座標系における角速度ベクトルωに座標変換する（Ｓ１０７）。
　座標変換後、速度ベクトル補正部３４７は、ＧＮＳＳセンサ３３により測定された測定位置ｓにおけるトラクタ１の速度ベクトルをＶｓとし、設定位置ａにおける速度ベクトルをＶａとして、Ｖａ＝Ｖｓ－ω×Ｌの式により、速度ベクトルＶａを算出することにより、速度ベクトルを補正する（Ｓ１０８）。
　速度ベクトルの補正後、後述する車体方位算出処理が実行される（Ｓ１０９）。
　このように、誤差補正装置３４は、測定位置ｓにおいて測定された車両位置、速度ベクトルのそれぞれを、設定位置ａにおける車両位置、速度ベクトルに補正する。補正された車両位置及び速度ベクトルによれば、特に凹凸がある圃場をトラクタ１が走行した場合に生じるセンサ装置３０の振れに起因する測定値の誤差を低減した、より正確な車両位置、進行方位及び進行速度を自動操舵制御装置２１が得ることができる。これによって、自動操舵制御装置２１による正確なトラクタ１の自動操舵が達成される。
（設定位置補正処理）
　設定位置補正処理の動作について説明する。図９は、本実施形態に係る設定位置補正処理の動作を示すフローチャートである。図１０、図１１は、それぞれ、前輪操舵による前進進路変更中、前進旋回中のトラクタを示す概略平面図である。
　図９に示すように、まず、第２取得部３４３が、自動操舵制御装置２１により出力される操舵量を取得し（Ｓ２０１）、設定位置決定部３４４が、取得された操舵量が予め設定された閾値以上であるか否かを判定する（Ｓ２０２）。ここで、閾値は、トラクタ１が旋回する程度の操舵量に設定される。
　操舵量が閾値以上ではない場合（Ｓ２０２，ＮＯ）、設定位置決定部３４４は、図１０に示すように、トラクタ１の車両重心または車両運動中心である第１の設定位置ａ１を設定位置ａに決定する（Ｓ２０３）。なお、第１の設定位置ａ１は、少なくとも、トラクタ１の前後方向において前輪１１の車軸中心ｆと後輪１２の車軸中心ｒとの間に位置付けられれば良い。
　一方、操舵量が閾値以上である場合（Ｓ２０２，ＹＥＳ）、設定位置決定部３４４は、図１１に示すように、後輪１２の車軸中心ｒに位置する第２の設定位置ａ２を設定位置ａに決定する（Ｓ２０４）。なお、第２の設定位置ａ２は、少なくとも、トラクタ１の前後方向において第１の設定位置ａ１よりも後輪１２側、換言すれば、非操舵輪側に位置付けられれば良い。
　このように、操舵量に基づいて設定位置を補正することによって、特にトラクタ１が旋回する際において、設定位置ａにおける速度ベクトルＶａの向きをトラクタ１の車体１０の向きにより近似したものとすることができる。
（車体方位算出処理）
　車体方位算出処理について説明する。図１２は、車体方向算出処理の動作を示すフローチャートである。図１３は、前輪操舵による後進旋回中のトラクタを示す概略平面図である。
　図１２に示すように、まず、第３取得部３４８は、トラクタ１が前進状態または後進状態のいずれにあるのかを走行方向として取得する（Ｓ３０１）。ここで、第３取得部３４８は、前進／後進を切り替えるレバーの状態や、原動機の動力を伝達するトランスミッションにおける前進ギア／後進ギアの切り替え状態などを走行方向として取得する。
　次に、車体方位算出部３４９は、第３取得部３４８により取得された走行方向が後進であるか否かを判定する（Ｓ３０２）。
　走行方向が後進である場合（Ｓ３０２，ＹＥＳ）、車体方位算出部３４９は、図１３に示すように、速度ベクトルＶａをｘ方向、即ちトラクタ１の前後方向に反転し（Ｓ３０３）、反転した速度ベクトルＶａの向きを車体方位として算出する（Ｓ３０４）。
　一方、走行方向が後進ではなく前進である場合（Ｓ３０２，ＮＯ）、車体方位算出部３４９は、速度ベクトルＶａの向きを車体方位として算出する（Ｓ３０４）。
　このように、トラクタ１の走行方向が後進である場合には反転させた速度ベクトルＶａの向きを車体方位とすることによって、速度ベクトルＶａから車体方位を算出することができる。
＜第２の実施形態＞
　本実施形態は、トラクタが後輪駆動であり、これに伴って、誤差補正装置における設定位置決定処理により決定される設定位置が第１の実施形態とは異なる。図１４、図１５は、それぞれ、後輪操舵による前進進路変更中、前進旋回中のトラクタを示す概略平面図である。
　図１４、図１５に示すように、本実施形態に係るトラクタ１ａは、非操舵輪である前輪１１ａと操舵輪である後輪１２ａを備えた所謂後輪駆動車として構成される。
　図１４に示すように、操舵量が閾値未満である場合の設定位置、即ち第１の設定位置は、第１の実施形態と同様に、トラクタ１ａの前後方向において前輪１１ａの車軸中心ｆと後輪１２ａの車軸中心ｒとの間、望ましくは、車両重心または車両運動中心である。
　図１５に示すように、操舵量が閾値以上である場合の設定位置、即ち第２の設定位置ａ２は、トラクタ１ａの前後方向において第１の設定位置ａ１よりも非操舵輪側にある前輪１１ａの車軸中心ｆとなる。
　このように、第２の設定位置ａ２を第１の設定位置ａ１よりも非操舵輪側に位置付けることによって、前輪操舵の車両だけでなく、後輪操舵の車両にも対応することができる。
　上述した実施形態において、測定位置ｓにおける車両位置、速度ベクトルを設定位置ａにおける車両位置、速度ベクトルに補正するものとしたが、車両位置、速度ベクトルそれぞれの設定位置ａを異なったものとしても良い。例えば、車両位置については、常時、第１の設定位置ａ１における車両位置に補正し、速度ベクトルのみを操舵量に応じて第１の設定位置ａ１または第２の設定位置ａ２のいずれかにおける速度ベクトルに補正するようにしても良い。
　本発明の実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
３４　誤差補正装置
３４１　第１取得部
３４３　第２取得部
３４４　設定位置決定部
３４７　速度ベクトル補正部

Claims (8)

  1. 　作業車両に設けられてＧＮＳＳセンサとジャイロセンサとを備えたセンサ装置による測定値の誤差を補正する誤差補正装置であって、
    　前記ＧＮＳＳセンサにより測定されたローカル座標系における前記作業車両の速度ベクトルと、前記ジャイロセンサにより測定された前記センサ装置を基準とした３軸を有するセンサ座標系における角速度とを取得する第１取得部と、
    　前記作業車両における操舵量を取得する第２取得部と、
    　前記センサ座標系において前記センサ装置と異なる位置に設定された設定位置を前記取得された操舵量に基づいて決定する設定位置決定部と、
    　前記測定位置における前記作業車両の速度ベクトルと、前記取得された角速度とに基づいて、前記決定された設定位置における前記作業車両の速度ベクトルを算出し、前記ＧＮＳＳセンサにより測定された前記作業車両の速度ベクトルを補正する速度ベクトル補正部と
    　を備える誤差補正装置。
  2. 　前記設定位置決定部は、前記取得された操舵量が予め設定された閾値未満である場合には第１の設定位置に前記設定位置を決定し、前記取得された操舵量が前記閾値以上である場合には前記第１の設定位置とは異なる第２の設定位置に前記設定位置を決定することを特徴とする請求項１に記載の誤差補正装置。
  3. 　前記作業車両は前輪または後輪のいずれかのみが操舵輪であり、
    　前記第１の設定位置は前記作業車両の前後方向において前記作業車両の前輪と後輪との間に位置し、
    　前記第２の設定位置は前記第１の設定位置に対して前後方向において非操舵輪側に位置することを特徴とする、請求項２に記載の誤差補正装置。
  4. 　前記補正された速度ベクトルの向きを前記作業車両の車体方位として算出する車体方位算出部を更に備えることを特徴とする請求項１～請求項３のいずれか一項に記載の誤差補正装置。
  5. 　前記作業車両が前進または後進のいずれの状態にあるのかを示す走行方向を取得する第３取得部を更に備え、
    　前記車体方位算出部は、前記取得された走行方向が後進である場合、前記補正された速度ベクトルを反転させた向きを前記作業車両の車体方位として算出することを特徴とする請求項４に記載の誤差補正装置。
  6. 　前記センサ座標系において前記センサ装置と異なる位置に設定された設定位置を始点とした前記センサ装置が設けられた測定位置の相対位置ベクトルを、前記取得された角速度に基づいて前記ローカル座標系における相対位置ベクトルに座標変換するとともに、前記取得された角速度に基づく前記センサ座標系における前記作業車両の角速度ベクトルを、前記取得された角速度に基づいて前記ローカル座標系における角速度ベクトルに座標変換する座標変換部を更に備え、
    　前記速度ベクトル補正部は、前記測定位置における前記作業車両の速度ベクトルと、前記座標変換された角速度ベクトルと、前記座標変換された相対位置ベクトルとに基づいて、前記決定された設定位置における前記作業車両の速度ベクトルを算出することを特徴とする請求項１～請求項５のいずれか一項に記載の誤差補正装置。
  7. 　前記速度ベクトル補正部は、前記測定位置における前記作業車両の速度ベクトルをＶｓ、前記設定位置における前記作業車両の速度ベクトルをＶａ、前記座標変換された角速度ベクトルをω、前記座標変換された相対位置ベクトルをＬとして、Ｖａ＝Ｖｓ－ω×Ｌの式によって前記決定された設定位置における前記作業車両の速度ベクトルを算出することを特徴とする請求項６に記載の誤差補正装置。
  8. 　前記ジャイロセンサにより測定された角速度の時系列信号に対して、予め設定された第１遮断周波数より高い高周波信号を逓減させるローパスフィルタを更に備え、
    　前記座標変換部は、前記高周波信号が逓減された角速度に基づいて前記相対位置ベクトルを座標変換することを特徴とする請求項６または請求項７に記載の誤差補正装置。
PCT/JP2021/011182 2020-03-18 2021-03-18 誤差補正装置 WO2021187594A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
CN202180019638.8A CN115244486A (zh) 2020-03-18 2021-03-18 误差修正装置
KR1020227028529A KR20220155266A (ko) 2020-03-18 2021-03-18 오차 보정 장치
JP2022508440A JPWO2021187594A1 (ja) 2020-03-18 2021-03-18
EP21771083.9A EP4123408A4 (en) 2020-03-18 2021-03-18 ERROR CORRECTION DEVICE

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2020048228 2020-03-18
JP2020-048228 2020-03-18

Publications (1)

Publication Number Publication Date
WO2021187594A1 true WO2021187594A1 (ja) 2021-09-23

Family

ID=77768460

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2021/011182 WO2021187594A1 (ja) 2020-03-18 2021-03-18 誤差補正装置

Country Status (5)

Country Link
EP (1) EP4123408A4 (ja)
JP (1) JPWO2021187594A1 (ja)
KR (1) KR20220155266A (ja)
CN (1) CN115244486A (ja)
WO (1) WO2021187594A1 (ja)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2010045350A1 (en) * 2008-10-14 2010-04-22 Hemisphere Gps Llc Satellite based vehicle guidance control in straight and contour modes
JP2011059065A (ja) * 2009-09-14 2011-03-24 Sony Corp 速度算出装置、速度算出方法及びナビゲーション装置
JP2016053553A (ja) * 2014-09-04 2016-04-14 日立建機株式会社 位置算出装置及び運搬車両
WO2016060283A1 (ja) * 2015-11-30 2016-04-21 株式会社小松製作所 作業機械の制御システム、作業機械、作業機械の管理システム及び作業機械の制御方法
JP2018120364A (ja) 2017-01-24 2018-08-02 株式会社クボタ 自動操舵システム

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2010045350A1 (en) * 2008-10-14 2010-04-22 Hemisphere Gps Llc Satellite based vehicle guidance control in straight and contour modes
JP2011059065A (ja) * 2009-09-14 2011-03-24 Sony Corp 速度算出装置、速度算出方法及びナビゲーション装置
JP2016053553A (ja) * 2014-09-04 2016-04-14 日立建機株式会社 位置算出装置及び運搬車両
WO2016060283A1 (ja) * 2015-11-30 2016-04-21 株式会社小松製作所 作業機械の制御システム、作業機械、作業機械の管理システム及び作業機械の制御方法
JP2018120364A (ja) 2017-01-24 2018-08-02 株式会社クボタ 自動操舵システム

Non-Patent Citations (1)

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

Also Published As

Publication number Publication date
EP4123408A1 (en) 2023-01-25
CN115244486A (zh) 2022-10-25
KR20220155266A (ko) 2022-11-22
EP4123408A4 (en) 2023-09-27
JPWO2021187594A1 (ja) 2021-09-23

Similar Documents

Publication Publication Date Title
US10696326B2 (en) Vehicle
CN103569194B (zh) 车辆的动力转向控制装置
JP4816732B2 (ja) 運転支援システムおよび運転支援方法
CN107685767B (zh) 多轴轮毂电机驱动车辆后轮线控转向驱动装置及转向方法
JP4433060B2 (ja) 駐車支援装置
JP6860387B2 (ja) 作業車
CN104742959A (zh) 车辆的车道保持控制装置
JP2001010518A (ja) 車両用操舵制御装置
JP2007296947A (ja) 車両の操舵装置
JP6811655B2 (ja) 作業車
JPH11198844A (ja) 操舵力制御装置
JP3707379B2 (ja) 車両の前後輪舵角制御装置
JP5338968B2 (ja) 操舵制御装置
JP7269255B2 (ja) 誤差補正装置
JP6871831B2 (ja) 作業車両用の自律走行システム
WO2021187594A1 (ja) 誤差補正装置
JP2009018756A (ja) 旋回挙動制御装置、自動車、及び旋回挙動制御方法
CN114872792A (zh) 一种车辆转向控制方法、装置、电子设备和存储介质
JP2023530249A (ja) 全方向移動車両
US11952010B1 (en) Automatically adjustable steering geometry for autonomous vehicle
JP2020080743A (ja) 車両の姿勢推定装置
JP5088008B2 (ja) 車体スリップ角制御装置及びプログラム
JP2007186099A (ja) 車両の操舵装置
JP2023068400A (ja) ステアリング制御装置、およびステアリング制御方法
WO2020090864A1 (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: 21771083

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2022508440

Country of ref document: JP

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE

ENP Entry into the national phase

Ref document number: 2021771083

Country of ref document: EP

Effective date: 20221018