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
  • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
  • G01S17/88—Lidar systems specially adapted for specific applications
  • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
• • 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/001—Steering by means of optical assistance, e.g. television cameras
• • 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
• • 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
  • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
  • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
  • G01S17/06—Systems determining position data of a target
  • G01S17/08—Systems determining position data of a target for measuring distance only
  • G01S17/10—Systems determining position data of a target for measuring distance only using transmission of interrupted, pulse-modulated waves
• • 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
  • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
  • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
  • G01S17/06—Systems determining position data of a target
  • G01S17/42—Simultaneous measurement of distance and other co-ordinates
• • 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
  • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
  • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
• • 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
  • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
  • G01S17/87—Combinations of systems using electromagnetic waves other than radio waves
• • 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
  • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
  • G01S17/88—Lidar systems specially adapted for specific applications
  • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
  • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
• • 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/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
• • 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
• • 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
  • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
  • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
  • G01S7/481—Constructional features, e.g. arrangements of optical elements
  • G01S7/4817—Constructional features, e.g. arrangements of optical elements relating to scanning
• • 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/242—Means based on the reflection of waves generated by the vehicle
• • 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/246—Arrangements for determining position or orientation using environment maps, e.g. simultaneous localisation and mapping [SLAM]
  • G05D1/2464—Arrangements for determining position or orientation using environment maps, e.g. simultaneous localisation and mapping [SLAM] using an occupancy grid
• • 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/60—Intended control result
  • G05D1/646—Following a predefined trajectory, e.g. a line marked on the floor or a flight path
• • 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/60—Intended control result
  • G05D1/648—Performing a task within a working area or space, e.g. cleaning
• • G—PHYSICS
  • G05—CONTROLLING; REGULATING
  • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
  • G05D2101/00—Details of software or hardware architectures used for the control of position
  • G05D2101/20—Details of software or hardware architectures used for the control of position using external object recognition
• • G—PHYSICS
  • G05—CONTROLLING; REGULATING
  • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
  • G05D2105/00—Specific applications of the controlled vehicles
  • G05D2105/15—Specific applications of the controlled vehicles for harvesting, sowing or mowing in agriculture or forestry
• • G—PHYSICS
  • G05—CONTROLLING; REGULATING
  • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
  • G05D2107/00—Specific environments of the controlled vehicles
  • G05D2107/20—Land use
  • G05D2107/21—Farming, e.g. fields, pastures or barns
• • 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/10—Optical signals
  • G05D2111/17—Coherent light, e.g. laser signals
• • 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/50—Internal signals, i.e. from sensors located in the vehicle, e.g. from compasses or angular sensors
  • G05D2111/54—Internal signals, i.e. from sensors located in the vehicle, e.g. from compasses or angular sensors for measuring the travel distances, e.g. by counting the revolutions of wheels
• • 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/67—Sensor fusion

Definitions

• the present disclosure relates to a state estimation system and an agricultural machine equipped with the state estimation system.
• the present disclosure also relates to a state estimation system, a computer that executes state estimation, and a computer program.
• ICT Information and Communication Technology
• IoT Internet of Things
• work vehicles such as tractors used in farm fields.
• work vehicles that run with automatic steering using positioning systems such as GNSS (Global Navigation Satellite System), which allows precise positioning, have been put into practical use.
• GNSS Global Navigation Satellite System
• Patent Document 1 discloses an example of a work vehicle that uses LiDAR to automatically navigate between rows of crops in a field.
• SLAM Simultaneous Localization and Mapping
• a state estimation system includes a sensor mounted on a vehicle, which, during operation, scans a surrounding environment including a crop row and outputs sensor data including position information of an object present in the environment, and a processing device that detects an adjacent crop row located to the left or right of the vehicle based on the sensor data, and is configured to execute an estimation algorithm of a state space model based on the position information of the adjacent crop row to determine an estimate of the curvature ⁇ of the adjacent crop row, an orientation deviation ⁇ r of the vehicle relative to a center line of the adjacent crop row, and a lateral deviation y cr of the vehicle relative to the center line.
• the general or specific aspects of the present disclosure may be realized by an apparatus, a system, a method, an integrated circuit, a computer program, or a computer-readable non-transitory storage medium, or any combination thereof.
• the computer-readable storage medium may include a volatile storage medium or a non-volatile storage medium.
• the apparatus may be composed of multiple devices. When the apparatus is composed of two or more devices, the two or more devices may be arranged in one device, or may be arranged separately in two or more separate devices.
• FIG. 1 is a side view showing a schematic example of an agricultural machine having an implement connected thereto.
• FIG. 1 is a block diagram showing an example of the configuration of an agricultural machine.
• 1 is a schematic diagram of a LiDAR sensor viewed from the side. This is a schematic diagram of a LiDAR sensor viewed vertically from above.
• FIG. 2 is a block diagram showing an example configuration of a LiDAR sensor.
• 1 is a diagram illustrating an example of an environment (e.g., an orchard) in which an agricultural machine travels;
• FIG. 1 is a perspective view illustrating a schematic example of a surrounding environment of an agricultural machine.
• FIG. 2 is a diagram illustrating an example of a travel route of an agricultural machine.
• FIG. 11 is a diagram for explaining a travel control method for an agricultural machine in an inter-row travel mode.
• FIG. FIG. 1 is a diagram showing a schematic diagram of point cloud data obtained from a tree row.
• FIG. 1 illustrates an example of a method for detecting left and right rows of adjacent crop rows.
• FIG. 1 illustrates an example of a method for detecting left and right rows of adjacent crop rows in accordance with an embodiment of the present disclosure.
• FIG. 1 is a diagram illustrating an example of an object detection search according to an embodiment of the present disclosure.
• FIG. 2 is a plan view showing a schematic diagram of occupied cells (or point clouds) in the left and right columns detected by an object detection search.
• FIG. 11 is a plan view illustrating an example of a target route generated by a processing device based on a left-row approximation line and a right-row approximation line in an embodiment of the present disclosure.
• FIG. 1 is a flowchart illustrating a processing procedure executed by a processing device according to an embodiment of the present disclosure.
• FIG. 2 is a diagram for explaining state variables employed by the state estimation system according to the embodiment of the present disclosure.
• FIG. 2 is a plan view illustrating an example of feature points (observation values) according to an embodiment of the present disclosure.
• FIG. 2 is a diagram illustrating a state equation according to an embodiment of the present disclosure.
• FIG. 2 is a diagram illustrating an observation equation in an embodiment of the present disclosure.
• 1 is a functional block diagram illustrating an example of a state estimation system according to an embodiment of the present disclosure.
• FIG. 11 is a block diagram for explaining the operation of a second Kalman filter.
• an "agricultural machine” is a mobile machine that performs agricultural work in fields, forests, and the like.
• An example of such a mobile agricultural machine may be a work vehicle having a plurality of wheels as a propulsion device. At least one of the front and rear of the work vehicle may be configured to be capable of mounting an implement (also called a "work machine” or “working device”) according to the work content.
• the traveling of a work vehicle while performing work using an implement is sometimes referred to as "work traveling.”
• Automatic steering means steering the vehicle through the action of a processing device such as a computer (which can also function as a control device), without manual operation by the driver.
• a processing device such as a computer (which can also function as a control device), without manual operation by the driver.
• “Autonomous driving” means that the driving of a vehicle is controlled by the action of a processing device, not by manual operation by a driver.
• automatic driving not only the driving of the vehicle but also the operation of the work (for example, the operation of an implement) may be controlled automatically.
• the driving of a vehicle by automatic driving is called “automatic driving”.
• the processing device may control at least one of the steering, adjustment of the driving speed, and starting and stopping of driving required for the driving of the vehicle.
• the processing device may control operations such as raising and lowering the implement, and starting and stopping the operation of the implement.
• Driving by automatic driving may include not only driving of the vehicle toward a destination along a predetermined route, but also driving of following a tracking target.
• An automatic driving vehicle may drive partially based on the instructions of a user.
• an automatic driving vehicle may operate in a manual driving mode in which the vehicle drives by manual operation of the driver in addition to the automatic driving mode.
• a part or all of the processing device may be outside the vehicle. Communication of control signals, commands, data, etc. may be performed between the processing device outside the vehicle and the vehicle.
• a vehicle that performs autonomous driving may travel autonomously while sensing the surrounding environment, without a human being being involved in controlling the vehicle's travel.
• a vehicle that is capable of autonomous travel can travel unmanned. During autonomous travel, obstacles can be detected and obstacle avoidance operations can be performed.
• Sensors installed on agricultural machinery include “external sensors” and “internal sensors.”
• “External sensors” are sensors that sense the environment around the agricultural machinery. Examples of external sensors include LiDAR sensors, cameras (or image sensors), laser range finders (also called “range sensors”), ultrasonic sensors, millimeter wave radar, and magnetic sensors.
• “Internal sensors” are sensors that sense the state of the vehicle, and include vehicle speed sensors and orientation sensors such as gyroscopes.
• the "crop row detection system” and “state estimation system” disclosed herein include a sensor attached to the agricultural machinery vehicle, which during operation scans the surrounding environment including the crop rows and outputs sensor data including position information of objects present in the environment.
• the position information included in the sensor data may include information indicating the distance from the sensor to the object and the direction of the object from the sensor.
• a “crop row” is a row of crops, trees, or other plants that grow in a field such as an orchard or field, or in a forest.
• "crop row” is a concept that includes “tree row” and “ridge.” Even a “ridge” in which no crops are present is included in the "crop row” in this disclosure.
• a "map” is local map data that expresses the positions or areas of objects in the surrounding environment of the agricultural machine in a specified coordinate system.
• the coordinate system that defines the map may be, for example, a vehicle coordinate system fixed to the agricultural machine, or a world coordinate system (e.g., a geographic coordinate system) fixed relative to the Earth.
• the map may also include information other than the positions (e.g., attribute information such as height and reflectance) about objects in the surrounding environment of the agricultural machine.
• the map may be expressed in various formats, for example, an occupancy grid map or a point cloud map. Such a map may be called an "obstacle map".
• the agricultural machine is a tractor used for agricultural work in fields such as orchards.
• the technology disclosed herein is not limited to tractors, and can also be applied to other types of agricultural machines, such as combine harvesters, riding cultivators, and riding lawnmowers.
• [1. Configuration] 1 is a side view that shows a schematic example of an agricultural machine 100 and an implement 300 connected to the agricultural machine 100.
• the agricultural machine 100 in this embodiment can operate in both a manual driving mode and an automatic driving mode.
• the agricultural machine 100 can travel unmanned.
• the agricultural machine 100 performs automatic travel in an environment where multiple rows of crops (e.g., rows of trees) are planted, such as an orchard such as a vineyard, or a field.
• the agricultural machine 100 includes a vehicle body 101, a prime mover (engine) 102, and a transmission 103.
• the vehicle body 101 includes a traveling device including wheels with tires 104, and a cabin 105.
• the traveling device includes four wheels 104, an axle for rotating the four wheels, and a braking device (brake) for braking each axle.
• the wheels 104 include a pair of front wheels 104F and a pair of rear wheels 104R.
• a driver's seat 107, a steering device 106, an operation terminal 200, and a group of switches for operation are provided inside the cabin 105.
• One or both of the front wheels 104F and the rear wheels 104R may be replaced with a plurality of wheels (crawlers) equipped with tracks instead of wheels with tires.
• the agricultural machine 100 is equipped with multiple external sensors that sense the surroundings of the agricultural machine 100.
• the external sensors include multiple LiDAR sensors 140, multiple cameras 120, and multiple obstacle sensors 130.
• the cameras 120 may be installed, for example, on the front, rear, left and right sides of the agricultural machine 100.
• the cameras 120 capture images of the environment around the agricultural machine 100 and generate image data.
• the images captured by the cameras 120 may be transmitted, for example, to a terminal device for remote monitoring.
• the images may be used to monitor the agricultural machine 100 during unmanned operation.
• the cameras 120 may be installed as necessary, and the number of cameras 120 is arbitrary.
• the LiDAR sensor 140 is an example of an external sensor that outputs sensor data indicating the distribution of objects located in the surrounding environment of the agricultural machine 100.
• two LiDAR sensors 140 are arranged at the front and rear of the cabin 105.
• the LiDAR sensors 140 may also be provided in other positions (e.g., the lower front part of the vehicle body 101). While the agricultural machine 100 is traveling, each LiDAR sensor 140 repeatedly outputs sensor data indicating the distance and direction to each measurement point of an object in the surrounding environment, or the three-dimensional coordinate values of each measurement point.
• the number of LiDAR sensors 140 is not limited to two, and may be one or three or more.
• the LiDAR sensor 140 may be configured to output three-dimensional point cloud data as sensor data.
• point cloud data broadly means data indicating the distribution of multiple reflection points observed by the LiDAR sensor 140.
• the point cloud data may include, for example, coordinate values of each reflection point in three-dimensional space, or information indicating the distance and direction of each reflection point.
• the point cloud data may also include brightness information of each reflection point.
• the LiDAR sensor 140 may be configured to repeatedly output the point cloud data, for example, at a preset period. In this manner, the external sensor may include one or more LiDAR sensors 140 that output point cloud data as sensor data.
• the sensor data output from the LiDAR sensor 140 is processed by a processing device that controls the automatic driving of the agricultural machine 100. While the agricultural machine 100 is driving, the processing device can sequentially generate an obstacle map showing the distribution of objects present around the agricultural machine 100 based on the sensor data output from the LiDAR sensor 140.
• the obstacle sensors 130 shown in FIG. 1 are provided at the front and rear of the cabin 105.
• the obstacle sensors 130 may also be located at other locations.
• one or more obstacle sensors 130 may be provided at any position on the side, front, and rear of the vehicle body 101.
• the obstacle sensors 130 may include, for example, a laser scanner or ultrasonic sonar.
• the obstacle sensors 130 are used to detect surrounding obstacles during autonomous driving and to stop or detour the agricultural machine 100.
• the agricultural machine 100 in this embodiment further includes a GNSS unit 110.
• GNSS is a general term for satellite positioning systems such as GPS (Global Positioning System), QZSS (Quasi-Zenith Satellite System, e.g., Michibiki), GLONASS, Galileo, and BeiDou.
• the GNSS unit 110 receives satellite signals (also referred to as GNSS signals) transmitted from multiple GNSS satellites and performs positioning based on the satellite signals.
• the GNSS unit 110 is provided on the top of the cabin 105, but may be provided in another position.
• the GNSS unit 110 includes an antenna that receives signals from GNSS satellites, and a processing circuit.
• the agricultural machine 100 in this embodiment may be used in an environment where multiple trees grow and it is difficult to use GNSS, such as a vineyard.
• positioning is performed mainly using the LiDAR sensor 140.
• positioning may be performed using the GNSS unit 110.
• the GNSS unit 110 may include an inertial measurement unit (IMU). Signals from the IMU can be used to supplement the position data.
• the IMU can measure the tilt and minute movements of the agricultural machine 100. By using data acquired by the IMU to supplement the position data based on satellite signals, the positioning performance can be improved.
• the prime mover 102 may be, for example, a diesel engine.
• An electric motor may be used instead of a diesel engine.
• the transmission 103 can change the propulsive force and travel speed of the agricultural machine 100 by changing gears.
• the transmission 103 can also switch the agricultural machine 100 between forward and reverse.
• the steering device 106 includes a steering wheel, a steering shaft connected to the steering wheel, and a power steering device that assists steering by the steering wheel.
• the front wheels 104F are steered wheels, and the traveling direction of the agricultural machine 100 can be changed by changing the turning angle (also called the "steering angle").
• the steering angle of the front wheels 104F can be changed by operating the steering wheel.
• the power steering device includes a hydraulic device or an electric motor that supplies an auxiliary force to change the steering angle of the front wheels 104F. When automatic steering is performed, the steering angle is automatically adjusted by the force of the hydraulic device or electric motor under the control of a processing device arranged in the agricultural machine 100.
• a coupling device 108 is provided at the rear of the vehicle body 101.
• the coupling device 108 includes, for example, a three-point support device (also called a "three-point link” or “three-point hitch"), a PTO (Power Take Off) shaft, a universal joint, and a communication cable.
• the coupling device 108 can attach and detach the implement 300 to the agricultural machine 100.
• the coupling device 108 can change the position or posture of the implement 300 by raising and lowering the three-point link using, for example, a hydraulic device.
• power can be sent from the agricultural machine 100 to the implement 300 via the universal joint.
• the agricultural machine 100 can make the implement 300 perform a predetermined task while pulling the implement 300.
• the coupling device may be provided at the front of the vehicle body 101. In that case, the implement can be connected to the front of the agricultural machine 100.
• the implement 300 shown in FIG. 1 is a sprayer that sprays a chemical on crops, but the implement 300 is not limited to a sprayer.
• any working machine such as a mower, seeder, spreader, rake, baler, harvester, plow, harrow, or rotary, can be connected to the agricultural machine 100 and used.
• the agricultural machine 100 shown in FIG. 1 is capable of being operated by a human driver, but may also be capable of being operated only unmanned. In that case, the agricultural machine 100 may not be provided with components that are only required for manned operation, such as the cabin 105, steering device 106, and driver's seat 107.
• the unmanned agricultural machine 100 can run autonomously or by remote control by a user.
• FIG. 2 is a block diagram showing an example configuration of the agricultural machine 100 and the implement 300.
• the agricultural machine 100 and the implement 300 can communicate with each other via a communication cable included in the coupling device 108.
• the agricultural machine 100 can also communicate with a terminal device 400 for remote monitoring via the network 80.
• the terminal device 400 is any computer, such as a personal computer (PC), a laptop computer, a tablet computer, or a smartphone.
• the agricultural machine 100 includes a GNSS unit 110, a camera 120, an obstacle sensor 130, a LiDAR sensor 140, and an operation terminal 200, as well as a group of sensors 150 that detect the operating state of the agricultural machine 100, a driving control system 160, a communication device 190, a group of operation switches 210, and a drive device 240. These components are connected to each other via a bus so that they can communicate with each other.
• the GNSS unit 110 includes a GNSS receiver 111, an RTK receiver 112, an inertial measurement unit (IMU) 115, and a processing circuit 116.
• the sensor group 150 includes a steering wheel sensor 152, a turning angle sensor 154, and an axle sensor 156.
• the driving control system 160 includes a storage device 170 and a processing device 180.
• the processing device 180 includes a plurality of electronic control units (ECUs) 181 to 184.
• the implement 300 includes a drive device 340, a control device 380, and a communication device 390. Note that FIG. 2 shows components that are relatively highly related to the operation of the autonomous driving by the agricultural machine 100, and the illustration of other components is omitted.
• the GNSS receiver 111 in the GNSS unit 110 receives satellite signals transmitted from multiple GNSS satellites and generates GNSS data based on the satellite signals.
• the GNSS data is generated in a predetermined format, such as the NMEA-0183 format.
• the GNSS data may include, for example, values indicating the identification number, elevation angle, azimuth angle, and reception strength of each satellite from which the satellite signal is received.
• the GNSS unit 110 may perform positioning of the agricultural machine 100 using RTK (Real Time Kinematic)-GNSS.
• RTK Real Time Kinematic
• the reference station may be installed near the work site where the agricultural machine 100 performs work travel (for example, within 10 km from the agricultural machine 100).
• the reference station generates a correction signal, for example, in RTCM format, based on the satellite signals received from multiple GNSS satellites and transmits it to the GNSS unit 110.
• the RTK receiver 112 includes an antenna and a modem, and receives the correction signal transmitted from the reference station.
• the processing circuit 116 of the GNSS unit 110 corrects the positioning result by the GNSS receiver 111 based on the correction signal.
• RTK-GNSS By using RTK-GNSS, it is possible to perform positioning with an accuracy of, for example, an error of several centimeters.
• Position information including latitude, longitude, and altitude information is acquired by highly accurate positioning using RTK-GNSS.
• the GNSS unit 110 calculates the position of the agricultural machine 100 at a frequency of, for example, about 1 to 10 times per second.
• the positioning method is not limited to RTK-GNSS, and any positioning method (such as interferometric positioning or relative positioning) that can obtain position information with the required accuracy can be used.
• positioning may be performed using a Virtual Reference Station (VRS) or a Differential Global Positioning System (DGPS).
• VRS Virtual Reference Station
• DGPS Differential Global Positioning System
• the GNSS unit 110 in this embodiment further includes an IMU 115.
• the IMU 115 may include a three-axis acceleration sensor and a three-axis gyroscope.
• the IMU 115 may include an orientation sensor such as a three-axis geomagnetic sensor.
• the IMU 115 functions as a motion sensor and can output signals indicating various quantities such as the acceleration, speed, displacement, and attitude of the agricultural machine 100.
• the processing circuit 116 can estimate the position and orientation of the agricultural machine 100 with higher accuracy based on the signal output from the IMU 115 in addition to the satellite signal and the correction signal.
• the signal output from the IMU 115 can be used to correct or complement the position calculated based on the satellite signal and the correction signal.
• the IMU 115 outputs signals at a higher frequency than the GNSS receiver 111.
• the IMU 115 outputs signals at a frequency of about tens to thousands of times per second.
• the processing circuit 116 can measure the position and orientation of the agricultural machine 100 at a higher frequency (e.g., 10 Hz or more).
• a three-axis acceleration sensor and a three-axis gyroscope may be provided separately.
• the IMU 115 may be provided as a device separate from the GNSS unit 110.
• the camera 120 is an imaging device that captures the environment around the agricultural machine 100.
• the camera 120 includes an image sensor, such as a CCD (Charge Coupled Device) or a CMOS (Complementary Metal Oxide Semiconductor).
• the camera 120 may also include an optical system including one or more lenses and a signal processing circuit.
• the camera 120 captures the environment around the agricultural machine 100 while the agricultural machine 100 is traveling, and generates image (e.g., video) data.
• the camera 120 can capture video at a frame rate of, for example, 3 frames per second (fps) or more.
• the images generated by the camera 120 can be used, for example, when a remote monitor uses the terminal device 400 to check the environment around the agricultural machine 100.
• the images generated by the camera 120 may be used for positioning or obstacle detection.
• multiple cameras 120 may be provided at different positions on the agricultural machine 100, or a single camera may be provided.
• a visible camera that generates visible light images and an infrared camera that generates infrared images may be provided separately. Both a visible camera and an infrared camera may be provided as cameras that generate images for surveillance. The infrared camera may also be used to detect obstacles at night.
• the obstacle sensor 130 detects objects present around the agricultural machine 100.
• the obstacle sensor 130 may include, for example, a laser scanner or an ultrasonic sonar. When an object is present closer than a predetermined distance from the obstacle sensor 130, the obstacle sensor 130 outputs a signal indicating the presence of an obstacle.
• Multiple obstacle sensors 130 may be provided at different positions on the agricultural machine 100. For example, multiple laser scanners and multiple ultrasonic sonars may be disposed at different positions on the agricultural machine 100. By providing such a large number of obstacle sensors 130, blind spots in monitoring obstacles around the agricultural machine 100 can be reduced.
• the steering wheel sensor 152 measures the rotation angle of the steering wheel of the agricultural machine 100.
• the turning angle sensor 154 measures the turning angle of the front wheels 104F, which are the steered wheels.
• the measurement values from the steering wheel sensor 152 and the turning angle sensor 154 can be used for steering control by the processing device 180.
• the axle sensor 156 measures the rotational speed of the axle connected to the wheel 104, i.e., the number of rotations per unit time.
• the axle sensor 156 may be, for example, a sensor that uses a magnetoresistive element (MR), a Hall element, or an electromagnetic pickup.
• the axle sensor 156 outputs, for example, a numerical value indicating the number of rotations per minute (unit: rpm) of the axle.
• the axle sensor 156 is used to measure the speed of the agricultural machine 100.
• the measurement value by the axle sensor 156 may be used for speed control by the processing device 180.
• the drive unit 240 includes various devices necessary for the travel of the agricultural machine 100 and the driving of the implement 300, such as the prime mover 102, the transmission 103, the steering device 106, and the coupling device 108 described above.
• the prime mover 102 may be equipped with an internal combustion engine such as a diesel engine.
• the drive unit 240 may be equipped with an electric motor for traction instead of or in addition to the internal combustion engine.
• the storage device 170 includes one or more storage media such as a flash memory or a magnetic disk.
• the storage device 170 stores various data generated by the GNSS unit 110, the camera 120, the obstacle sensor 130, the LiDAR sensor 140, the sensor group 150, and the processing device 180.
• the data stored in the storage device 170 may include an environmental map of the environment in which the agricultural machine 100 travels, an obstacle map that is generated sequentially during travel, and route data for automatic driving.
• the storage device 170 also stores computer programs that cause each ECU in the processing device 180 to perform various operations described below.
• Such computer programs may be provided to the agricultural machine 100 via a storage medium (e.g., a semiconductor memory or an optical disk) or an electric communication line (e.g., the Internet).
• Such computer programs may be sold as commercial software.
• the processing device 180 includes multiple ECUs.
• the multiple ECUs include, for example, an ECU 181 for speed control, an ECU 182 for steering control, an ECU 183 for implement control, and an ECU 184 for automatic control that executes various calculations required for automatic steering or automatic driving.
• the ECU 181 controls the speed of the agricultural machine 100 by controlling the prime mover 102, the transmission 103, and the brakes included in the drive unit 240.
• the ECU 182 controls the steering of the agricultural machine 100 by controlling the hydraulic device or electric motor included in the steering device 106 based on the measurement value of the steering wheel sensor 152.
• the ECU 183 controls the operation of the three-point link and PTO shaft included in the coupling device 108 to cause the implement 300 to perform the desired operation.
• the ECU 183 also generates signals that control the operation of the implement 300 and transmits the signals from the communication device 190 to the implement 300.
• the ECU 184 performs calculations and control to achieve automatic steering or automatic driving based on the data output from the GNSS unit 110, the camera 120, the obstacle sensor 130, the LiDAR sensor 140, and the sensor group 150. For example, the ECU 184 estimates the position of the agricultural machine 100 based on the data output from at least one of the GNSS unit 110, the camera 120, and the LiDAR sensor 140. In a situation where the reception strength of the satellite signals from the GNSS satellites is sufficiently high, the ECU 184 may determine the position of the agricultural machine 100 based only on the data output from the GNSS unit 110.
• the ECU 184 estimates the position of the agricultural machine 100 using data output from the LiDAR sensor 140.
• the ECU 184 in the embodiment of the present disclosure also operates as a "processing device" that detects the left and right crop rows (adjacent crop rows) adjacent to the agricultural machine 100 when the agricultural machine 100 travels between crop rows in the orchard, and estimates the "state” defined by the agricultural machine 100 and the adjacent crop rows. Based on the estimated "state", it becomes possible to obtain an estimate of the position and orientation (pose) of the agricultural machine 100 between the adjacent crop rows. This point will be described later.
• the ECU 184 During automatic steering or automatic driving, the ECU 184 generates a target route based on the estimated position of the agricultural machine 100, and performs calculations necessary for the agricultural machine 100 to travel along the generated target route.
• the ECU 184 sends a command to change the steering angle to the ECU 182 based on the target route.
• the ECU 182 changes the steering angle by controlling the steering device 106 in response to the command to change the steering angle.
• the ECU 184 sends a command to change the speed to the ECU 181 based on the target route.
• the ECU 181 changes the speed of the agricultural machine 100 by controlling the prime mover 102, the transmission 103, or the brakes in response to the command to change the speed.
• the processing device 180 controls the drive device 240 based on the measured or estimated position of the agricultural machine 100 and the target route that is generated sequentially. This allows the processing device 180 to drive the agricultural machine 100 along the target route.
• the processing device 180 functions as an automatic steering device or an automatic driving device.
• the multiple ECUs included in the processing device 180 can communicate with each other according to a vehicle bus standard such as CAN (Controller Area Network).
• CAN Controller Area Network
• a faster communication method such as in-vehicle Ethernet (registered trademark) may be used instead of CAN.
• FIG. 2 each of the ECUs 181 to 184 is shown as an individual block, but the functions of each of these may be realized by multiple ECUs.
• An in-vehicle computer that integrates at least some of the functions of the ECUs 181 to 184 may be provided.
• the processing device 180 may include ECUs other than the ECUs 181 to 184, and any number of ECUs may be provided depending on the functions.
• Each ECU includes a processing circuit including one or more processors.
• the communication device 190 is a device including a circuit for communicating with the implement 300 and the terminal device 400.
• the communication device 190 includes a circuit for transmitting and receiving signals conforming to the ISOBUS standard, such as ISOBUS-TIM, between the communication device 390 of the implement 300. This allows the implement 300 to perform a desired operation and to obtain information from the implement 300.
• the communication device 190 may further include an antenna and a communication circuit for transmitting and receiving signals between the terminal device 400 via the network 80.
• the network 80 may include, for example, a cellular mobile communication network such as 3G, 4G, or 5G, and the Internet.
• the communication device 190 may have a function for communicating with a mobile terminal used by an observer located near the agricultural machine 100. Communication may be performed between such a mobile terminal in accordance with any wireless communication standard, such as Wi-Fi (registered trademark), cellular mobile communication such as 3G, 4G, or 5G, or Bluetooth (registered trademark).
• Wi-Fi registered trademark
• the operation terminal 200 is a terminal through which a user performs operations related to the traveling of the agricultural machine 100 and the operation of the implement 300, and is also called a virtual terminal (VT).
• the operation terminal 200 may include a display device such as a touch screen, and/or one or more buttons.
• the display device may be, for example, a liquid crystal or organic light-emitting diode (OLED) display.
• OLED organic light-emitting diode
• the operation terminal 200 may be configured to be detachable from the agricultural machine 100. A user who is located away from the agricultural machine 100 may control the operation of the agricultural machine 100 by operating the detached operation terminal 200.
• the drive unit 340 in the implement 300 shown in FIG. 2 performs the operations necessary for the implement 300 to perform a specified task.
• the drive unit 340 includes devices according to the application of the implement 300, such as a hydraulic device, an electric motor, or a pump.
• the control device 380 controls the operation of the drive unit 340.
• the control device 380 causes the drive unit 340 to perform various operations in response to signals transmitted from the agricultural machine 100 via the communication device 390.
• a signal according to the state of the implement 300 can also be transmitted from the communication device 390 to the agricultural machine 100.
• the LiDAR sensor 140 in this embodiment is a scanning type sensor that can acquire information on the distance distribution of objects in space by scanning a laser beam.
• Figure 3A is a schematic diagram of the LiDAR sensor 140 when viewed from the side of the agricultural machine 100.
• Figure 3B is a schematic diagram of the LiDAR sensor 140 when viewed vertically from above.
• Figures 3A and 3B show three mutually orthogonal axes u, v, and w in a sensor coordinate system fixed to the LiDAR sensor 140.
• the multiple straight lines extending radially in Figures 3A and 3B typically represent the central axis (or traveling direction) of the laser beam emitted from the LiDAR sensor 140.
• Each laser beam is collimated to a parallel light, but has a spread angle of several milliradians (e.g., 0.1 to 0.2 degrees). Therefore, the cross-sectional size (spot diameter) of each laser beam increases in proportion to the distance from the LiDAR sensor 140. For example, a light spot with a diameter of several centimeters may be formed 20 meters away from the LiDAR sensor 140.
• the diagram ignores the spread of the laser beams and only shows the central axes of the laser beams.
• the LiDAR sensor 140 in the example shown in FIG. 3A can scan the laser beam in different emission directions, for example, by a MEMS mirror.
• the elevation angle of the laser beam is defined by the angle with respect to the uv plane.
• FIG. 3A shows the directions of the laser beams L 1 , ..., L N corresponding to N scanning lines.
• N is an integer of 1 or more, for example, 64 or 100 or more.
• the elevation angle of the kth scanning direction from the bottom among the multiple scanning directions N is ⁇ k .
• FIG. 3A shows the elevation angle ⁇ N-1 of the N-1th laser beam as an example.
• the elevation angle of the laser beam facing upward from the uv plane is defined as a "positive elevation angle”
• the elevation angle of the laser beam facing downward from the uv plane is defined as a "negative elevation angle".
• the angle between the first scanning direction and the Nth scanning direction is called the "vertical viewing angle.”
• the vertical viewing angle can be set within the range of, for example, 0° to 60°.
• the LiDAR sensor 140 can perform scanning by changing the emission direction (e.g., azimuth angle) of the laser beam.
• FIG. 3B shows the emission direction of the laser beam rotating around a rotation axis parallel to the w axis.
• the range of the emission direction (azimuth angle) of the laser beam may be 360° or may be a range of angles smaller than 360° (e.g., 120° or 270°).
• the range of the azimuth angle of the emission direction of the laser beam is called the "horizontal field of view angle.”
• the horizontal field of view angle can be set, for example, within a range of about 90° to 360°.
• the LiDAR sensor 140 sequentially emits pulsed laser light (laser pulses) in different azimuth angles while rotating the emission direction of the laser beam around a rotation axis parallel to the w axis. In this way, it is possible to measure the distance to each reflection point by the pulsed laser light emitted at different elevation angles and different azimuth angles. Each reflection point corresponds to an individual point included in the point cloud data.
• the operation of measuring the distance to the reflection point while the laser beam scans the range defined by the vertical and horizontal viewing angles is called one scan.
• the LiDAR sensor 140 repeats the scanning operation at a frequency of, for example, 1 to 20 times per second to obtain sensor data from one scan. During one scan, for example, 100,000 or more pulses of laser light can be emitted in different directions.
• FIG. 4 is a block diagram showing an example of the configuration of the LiDAR sensor 140.
• the LiDAR sensor 140 shown in FIG. 4 includes one or more laser units 141, a scanning device 144, a control circuit 145, a signal processing circuit 146, and a memory 147.
• the laser unit 141 includes a laser light source 142 and a photodetector 143. Each laser unit 141 may include an optical system such as a lens and a mirror, but these are not shown in the figure.
• the scanning device 144 changes the direction of the laser beam emitted from each laser light source 142, for example, by rotating a mirror arranged on the optical path of the laser beam emitted from each laser light source 142.
• the laser light source 142 includes a laser diode and emits a pulsed laser beam of a predetermined wavelength in response to a command from the control circuit 145.
• the wavelength of the laser beam may be, for example, a wavelength in the near-infrared wavelength range (approximately 700 nm to 2.5 ⁇ m).
• the wavelength used depends on the material of the photoelectric conversion element used in the photodetector 143. For example, when silicon (Si) is used as the material of the photoelectric conversion element, a wavelength of about 900 nm may be mainly used.
• a wavelength of, for example, 1000 nm or more and 1650 nm or less may be used.
• the wavelength of the laser beam is not limited to the near-infrared wavelength range.
• a wavelength in the visible range approximately 400 nm to 700 nm
• radiation in the ultraviolet, visible, and infrared wavelength ranges is generally referred to as "light”.
• the photodetector 143 is a device that detects the laser pulse emitted from the laser light source 142 and reflected or scattered by an object.
• the photodetector 143 is equipped with a photoelectric conversion element such as an avalanche photodiode (APD).
• APD avalanche photodiode
• the scanning device 144 rotates or oscillates mirrors arranged on the optical path of the laser beam emitted from each laser light source 142 in response to a command from the control circuit 145. This realizes a scanning operation that changes the emission direction of the laser beam.
• the control circuit 145 controls the emission of laser pulses by the laser light source 142, the detection of reflected pulses by the photodetector 143, and the rotational operation by the scanning device 144.
• the control circuit 145 can be realized by a circuit including a processor, such as a microcontroller unit (MCU).
• MCU microcontroller unit
• the signal processing circuit 146 is a circuit that performs calculations based on the signal output from the photodetector 143.
• the signal processing circuit 146 calculates the distance to an object that has reflected the laser pulse emitted from each laser light source 142, for example, by the ToF (Time of Flight) method.
• ToF Time of Flight
• the distance to the reflection point is calculated by directly measuring the time from when the laser pulse is emitted from the laser light source 142 to when the reflected light is received by the photodetector 143.
• the distance to each reflection point is calculated based on the ratio of the amount of light detected in each exposure period.
• Either the direct ToF method or the indirect ToF method can be used.
• the signal processing circuit 146 generates and outputs sensor data that indicates, for example, the distance to each reflection point and the direction of the reflection point.
• the signal processing circuit 146 further calculates the coordinates (u, v) or (u, v, w) in the sensor coordinate system based on the distance to each reflection point and the direction of that reflection point, and outputs them as part of the sensor data.
• control circuit 145 and the signal processing circuit 146 are separated into two circuits, but they may also be realized by a single circuit.
• the memory 147 is a storage medium that stores data generated by the control circuit 145 and the signal processing circuit 146.
• the memory 147 stores, for example, data that associates the emission timing, emission direction, reflected light intensity, distance to the reflection point, and coordinates (u, v) or (u, v, w) in the sensor coordinate system of the laser pulse emitted from each laser unit 141. Such data is generated each time a laser pulse is emitted and recorded in the memory 147.
• the control circuit 145 outputs the data at a predetermined period (for example, the time required to emit a predetermined number of pulses, a half scan period, or one scan period). The output data is recorded in the storage device 170 of the agricultural machine 100.
• the LiDAR sensor 140 outputs sensor data, for example, at a frequency of about 1 to 20 times per second.
• This sensor data may include the coordinates of multiple points expressed in the sensor coordinate system and timestamp information. Note that there are also cases where the sensor data includes information on the distance and direction to each reflection point, but does not include coordinate information. In such cases, the processing device 180 converts the distance and direction information into coordinate information.
• the method of distance measurement is not limited to the ToF method, and other methods such as the FMCW (Frequency Modulated Continuous Wave) method may also be used.
• FMCW Frequency Modulated Continuous Wave
• the FMCW method light with a linearly changed frequency is emitted, and the distance is calculated based on the frequency of the beat generated by the interference between the emitted light and the reflected light.
• the LiDAR sensor 140 in this embodiment is a scan-type sensor that acquires information on the distance distribution of objects in the surrounding environment by scanning a laser beam.
• the LiDAR sensor 140 is not limited to a scan-type sensor.
• the LiDAR sensor 140 may be a flash-type sensor that acquires information on the distance distribution of objects in space by using light that diffuses over a wide range.
• a scan-type LiDAR sensor uses light of a higher intensity than a flash-type LiDAR sensor, and therefore can acquire distance information from greater distances.
• a flash-type LiDAR sensor has a simple structure and can be manufactured at low cost, making it suitable for applications that do not require strong light.
• FIG. 5 is a diagram showing an example of an environment in which the agricultural machine 100 travels.
• FIG. 6 is a perspective view showing an example of an environment around the agricultural machine 100.
• the agricultural machine 100 travels between a plurality of tree rows 20 (i.e., crop rows) in an orchard such as a vineyard, and performs a predetermined task (e.g., spraying pesticides, mowing, pest control, etc.) using the implement 300.
• a predetermined task e.g., spraying pesticides, mowing, pest control, etc.
• the sky is blocked by branches and leaves, making automatic travel using GNSS difficult.
• GNSS cannot be used, it is possible to travel while estimating the self-position by matching a pre-created environmental map with sensor data.
• the processing device 180 in this embodiment therefore detects two crop rows (adjacent crop rows) on either side of the agricultural machine 100 based on the sensor data output from the LiDAR sensor 140, and drives the agricultural machine 100 along the path between those two crop rows.
• FIG. 7 is a diagram showing a schematic example of a travel route 30 of the agricultural machine 100.
• the agricultural machine 100 travels between the rows of trees 20 on the route 30 as shown.
• the line segments included in the route 30 are drawn as straight lines, but the route on which the agricultural machine 100 actually travels may include curved or meandering portions.
• the multiple rows of trees 20 are ordered from the end as the first row of trees 20A, the second row of trees 20B, the third row of trees 20C, the fourth row of trees 20D, and so on.
• FIG. 7 is a diagram showing a schematic example of a travel route 30 of the agricultural machine 100.
• the agricultural machine 100 travels between the rows of trees 20 on the route 30 as shown.
• the line segments included in the route 30 are drawn as straight lines, but the route on which the agricultural machine 100 actually travels may include curved or meandering portions.
• the multiple rows of trees 20 are ordered from the end as the first row of trees 20A, the second row of trees 20B, the third row of trees 20
• the agricultural machine 100 first travels between the first row of trees 20A and the second row of trees 20B, and when that travel is completed, it turns right and travels in the opposite direction between the second row of trees 20B and the third row of trees 20C. After completing the journey between the second row of trees 20B and the third row of trees 20C, the vehicle turns further left and travels between the third row of trees 20C and the fourth row of trees 20D. By repeating the same operation, the vehicle travels to the end of the path 30 between the last two rows of trees.
• the agricultural machine 100 in an embodiment of the present disclosure can automatically steer between adjacent tree rows based on the sensor data output from the LiDAR sensor 140. This mode of automatically steered between adjacent crop rows is referred to as the "inter-row driving mode" in this disclosure.
• FIG. 8 is a diagram for explaining a driving control method for the agricultural machine 100 in the inter-row driving mode. While driving between two adjacent rows of trees, i.e., the left row 20L and the right row 20R, the agricultural machine 100 scans the surrounding environment with a laser beam using the LiDAR sensor 140. This allows data to be acquired that indicates the distribution of distances and orientations to objects present in the environment. Such data is converted, for example, into two-dimensional or three-dimensional point cloud data and output as sensor data.
• the processing device 180 sequentially generates an obstacle map 40 based on the sensor data output from the LiDAR sensor 140.
• the obstacle map 40 shows, for example, the distribution of objects in a vehicle coordinate system fixed to the agricultural machine 100.
• the obstacle map 40 in the example of FIG. 8 has a predetermined length Lh and width Lw.
• the length Lh is the size in the vertical direction corresponding to the traveling direction of the agricultural machine 100.
• the width Lw is the size in the horizontal direction perpendicular to both the traveling direction and the vertical direction of the agricultural machine 100.
• the processing device 180 detects the left row 20L and the right row 20R included in the two rows of trees (adjacent rows of trees) existing on both sides of the agricultural machine 100 based on the obstacle map 40. A specific method will be described later.
• the processing device 180 determines approximation lines 42R, 42L of the left row 20L and the right row 20R of the row of trees that extends in a straight line based on the obstacle map 40.
• the processing device 180 can set the target route 45 between the approximation lines 42R, 42L (e.g., the center).
• the processing device 180 can determine a curved approximation line instead of a straight line, and set the target route 45 between the approximation lines.
• the target route 45 may be set within a relatively short range (e.g., a range of about several meters) starting from the position of the agricultural machine 100.
• the target route 45 may be defined, for example, by a plurality of waypoints. Each waypoint may include information on the position and orientation (or speed) of a point through which the agricultural machine 100 must pass.
• the interval between waypoints may be set, for example, to a value of about several tens of centimeters (cm) to several meters (m).
• the processing device 180 causes the agricultural machine 100 to travel along the set target route 45. For example, the processing device 180 performs steering control of the agricultural machine 100 so as to minimize deviations in the position and orientation of the agricultural machine 100 relative to the target route 45. This allows the agricultural machine 100 to travel along the target route 45.
• a real tree row produces point cloud data that reflects the complex shapes of the trees. Trees are not necessarily arranged in a straight line as shown in Figure 8, and the tree row may include curved sections.
• FIG. 9 is a plan view showing point cloud data obtained from a non-linearly extending tree row.
• the black dots in the figure show the reflection points of the laser beam located on the object surface such as the trunk, branches, leaves, and fruits of the trees.
• the point cloud consisting of such reflection points may vary in a complex manner due to the growth of the tree branches and leaves.
• FIG. 9 shows an XYZ coordinate system (Cartesian coordinate system) for defining the position information of the reflection points in the obstacle map 40.
• the position of each point (reflection point) constituting the three-dimensional point cloud data can be expressed by coordinates (X, Y, Z) in the XYZ coordinate system.
• FIG. 9 shows a two-dimensional point cloud obtained by vertically projecting the three-dimensional point cloud onto the XZ plane, and corresponds to a bird's-eye view of the tree row seen directly below from the sky.
• the LiDAR sensor 140 can output sensor data at a predetermined cycle. For example, if one frame of point cloud data as shown in FIG. 9 is taken as one frame, the LiDAR sensor 140 can obtain multiple frames of point cloud data per second (e.g., 10 frames per second). In this case, for example, one obstacle map 40 may be created from one frame of point cloud data, or one obstacle map 40 may be created by superimposing multiple frames of point cloud data.
• the position and orientation of the LiDAR sensor 140 mounted on the agricultural machine 100 change while the LiDAR sensor 140 outputs multiple frames of point cloud data when the agricultural machine 100 is traveling. For this reason, when creating one obstacle map 40 from multiple frames of point cloud data, superimposition (registration) can be performed so that the point clouds of the multiple frames match.
• the processing device 180 performs an operation to detect the left row 20L and the right row 20R of adjacent crop rows, using the coordinates (position information) of the point cloud in a vehicle coordinate system fixed to the vehicle of the agricultural machine 100.
• the vehicle coordinate system is, for example, a coordinate system (XZ coordinate system) that defines a coordinate plane (XZ plane) that includes a first coordinate axis (Z axis) that extends from the origin in the longitudinal direction of the vehicle, and a second coordinate axis (X axis) that extends from the origin in the lateral direction of the vehicle.
• the origin is, for example, located at the center of the rear axle of the agricultural machine 100.
• the position information of the point cloud obtained from the three-dimensional LiDAR sensor is originally expressed by three-dimensional coordinates. Therefore, the coordinate (Y coordinate: height information) on the third coordinate axis (Y axis) perpendicular to the XZ plane can be recorded in a three-dimensional obstacle map along with the X and Z coordinates.
• the map in FIG. 10 can be formed based on a point cloud obtained by projecting a point cloud existing in a predetermined height range perpendicularly to the XZ plane.
• the height of the point cloud effective for detecting the presence of a tree is determined by the height of the tree.
• the obstacle map 40 can be created from a two-dimensional point cloud obtained by projecting a point cloud whose height from the ground is in a predetermined range from among the three-dimensional point clouds perpendicularly onto the XZ plane.
• the obstacle map 40 may be a three-dimensional occupancy grid map composed of three-dimensional cells (voxels) whose sides are, for example, in the range of 1 to 20 centimeters.
• a cell containing a point cloud with a density equal to or greater than a predetermined value is an "occupied cell” that indicates a cell in which an object (tree) exists.
• a cell in which the point cloud density is less than a predetermined value may be an "unoccupied cell (free space cell)" that indicates a cell in which an object is presumed not to exist, or an "unknown space cell” in which the probability of an object's existence cannot be determined.
• the three-dimensional occupancy grid map may be converted into a two-dimensional occupancy grid map on the XZ coordinate plane (a planar map equivalent to a bird's-eye view).
• the occupied cells of the two-dimensional occupancy grid map may be cells in which a point cloud exists within a predetermined height range. By limiting the predetermined height range to, for example, 0.3 to 1.5 meters, the spatial portion in which tree branches and leaves exist can be identified.
• Each cell of the two-dimensional occupancy grid map may have various information that can be used for object detection, classification, segmentation, and the like. For example, height information, reflectance information, point cloud density information, and the like of objects detected by a LiDAR sensor may be recorded on a cell-by-cell basis.
• the processing device 180 uses a map (obstacle map) based on the position information of such point clouds to start an object detection search parallel to the X-axis, which is the second coordinate axis, from the first coordinate point in the XZ coordinate system, and detects the left row 20L and right row 20R of the adjacent crop rows.
• a map obstacle map
• the processing device 180 sets first coordinate points P1 , P2 , ..., Pk , ..., Pn, Pn +1 arranged at a predetermined interval on the Z axis as starting points for starting an object detection search.
• n is an integer equal to or greater than 1
• k is an integer equal to or greater than 1 and equal to or less than n.
• the first coordinate points P1 , P2 , ..., Pk , ..., Pn , Pn +1 may be collectively referred to as "first coordinate points P.”
• the first coordinate points P1 , P2 , ..., Pk , ..., Pn , Pn +1 are set as starting points, and a search is performed to see whether there are points or occupied cells corresponding to a tree row on the left or right of each point.
• the processing device 180 searches in order through a number of cells arranged in a straight line to the right or left of the cell located at the first coordinate point P (starting cell) to find an occupied cell.
• the detected occupied cells are surrounded by dashed circles.
• the object detection search in this example may stop after passing through the left row 20L or the right row 20R of the adjacent crop row. For example, if the right row 20R curves to the left as shown in Figure 10, the first coordinate point Pn +1 enters the point cloud of the right row 20R. In that case, it becomes difficult to determine the distance from the first coordinate point Pn +1 to the right row 20R.
• scanning is performed in the horizontal direction while moving the scanning starting point along the row of trees. That is, the processing device 180 in this embodiment changes (updates) the position of the first coordinate point P k as k increases, as shown in Fig. 11.
• the processing device 180 when updating the first coordinate point P k to the first coordinate point P k+1 , the processing device 180 increases the first coordinate (Z coordinate) of the first coordinate point P k+1 on the first coordinate axis (Z axis) to be greater than the first coordinate (Z coordinate) of the first coordinate point P k on the first coordinate axis (Z axis), and makes the X coordinate (second coordinate) of the first coordinate point P k+1 coincide with the X coordinate of the center point of the crop row at which the distances to the left row 20L and the right row 20R of the adjacent crop rows are equal to each other.
• the distance in the Z-axis direction between the first coordinate point P k and the first coordinate point P k+1 can be set to, for example, an integer multiple of the size of one cell in the Z-axis direction.
• the X coordinate of the first coordinate point P can be appropriately shifted according to the curve. As a result, it becomes easy to detect the left row and the right row from the first coordinate point P.
• This method can also be applied when one of the left row 20L and right row 20R of the tree rows is curved, or when the direction, position, or degree of the curve differs from each other.
• FIG. 12 is a diagram showing an example of an object detection search performed using an occupancy grid map.
• a search is performed from the first coordinate points P 1 , P 2 , P 3 , P 4 , and P 5 to the left.
• the initial first coordinate point P 1 is placed on the Z axis.
• an occupancy grid map (obstacle map) in which a plurality of cells are two-dimensionally arranged in rows and columns, the cells hatched with diagonal lines are occupied cells.
• the occupied cells shown in FIG. 12 correspond to reflection points (point group) from the left row 20L of the adjacent tree row.
• a search from the first coordinate point P 1 is performed in order from the cell adjacent to the left of the cell containing the first coordinate point P 1 until an occupied cell is detected.
• one cell at a time is scanned horizontally from the scanning start point, and if there is a cell that satisfies the tree height condition (0.5 to 1.5 m), the cell is set as the position of the adjacent tree row, and the scanning ends.
• the thick arrow extending left from the first coordinate point P1 which is the scanning starting point, indicates the range of the left column search, and the cell at the tip of the arrow is the detected occupied cell.
• the detected occupied cell corresponds to the branch or leaf located at the innermost position (the side closer to the center line of the left column 20L and the right column 20R) of the adjacent tree column.
• no occupied cell was detected from the preset number of cells.
• the processing device 180 may be configured to select a specific area from the obstacle map 40 as a region of interest and detect the left row and the right row of the adjacent crop row from within the region of interest. For example, only cells within a predetermined distance range from each first coordinate point P k may be searched. The predetermined distance range may be determined based on the expected distance between the tree rows.
• FIG. 13 is a plan view showing a schematic diagram of occupied cells (or point cloud) 42L of the left row 20L and occupied cells (or point cloud) 42R of the right row 20R detected by an object detection search.
• the processing device 180 determines a left row approximation line 42L and a right row approximation line 42R based on the detected occupied cells (or point clouds) 42L, 42R.
• These approximation lines 42L, 42R can be obtained, for example, by the least squares method.
• the curve used for the approximation can be, for example, a quadratic curve, but may also be a cubic curve or other curve.
• the left row 20L and right row 20R of the adjacent tree rows can be appropriately approximated by a quadratic curve within the range of one frame acquired by the vehicle-mounted sensor.
• FIG. 14 is a plan view showing an example of a target route 45 generated by the processing device 180 based on the left row approximation line 42L and the right row approximation line 42R.
• the target route 45 is the center line of the detected adjacent crop row (approximation lines 42L, 42R).
• the processing device 180 can determine the azimuth deviation and lateral deviation of the vehicle with respect to the center line of the adjacent crop row (target route 45) on the map, and control the traveling of the vehicle so as to reduce the azimuth deviation and lateral deviation.
• the azimuth deviation and lateral deviation of the vehicle with respect to the target route 45 can be determined by various methods.
• the processing device 180 may match the sensor data obtained from the LiDAR sensor with the map to estimate its own position on the map.
• an estimation algorithm of the state space model is used based on the position information of the adjacent crop row to obtain estimated values of the curvature ⁇ of the adjacent crop row, the azimuth deviation ⁇ r of the vehicle with respect to the center line of the adjacent crop row, and the lateral deviation y cr of the vehicle with respect to the center line.
• the processing device 180 acquires sensor data from the sensor, including position information of objects present in the environment surrounding the vehicle.
• sensor data including position information of objects present in the environment surrounding the vehicle.
• point cloud data output by a LiDAR sensor was acquired, but image data output by a depth camera may also be acquired.
• image data may include object position information. For example, by using time-series image data consisting of multiple frames, it is possible to calculate position information of objects included in the image.
• step S12 the processing device 180 creates a map of the crop rows based on the sensor data.
• the position information in the map can be expressed as coordinates in various coordinate systems through coordinate transformation.
• the map may be defined by a coordinate system other than the vehicle coordinate system.
• the map dimensions are not limited to two dimensions.
• step S14 the processing device 180 starts an object detection search from the first coordinate point on the map parallel to the second coordinate axis to detect the left and right rows of adjacent crop rows.
• the second coordinate axis is parallel to the width of the vehicle.
• step S16 the processing device 180 sequentially updates the first coordinate point, and starts an object detection search parallel to the second coordinate axis from the updated first coordinate point to detect the left row and the right row of the adjacent crop row.
• the processing device 180 increases or decreases the first coordinate of the first coordinate point on the first coordinate axis, and matches the second coordinate of the first coordinate point on the second coordinate axis with the second coordinate of the crop row center point at which the distances to the left row and the right row of the adjacent crop row are equal to each other on the second coordinate axis.
• whether to increase or decrease the first coordinate of the first coordinate point can be determined based on the initial position of the first coordinate point and/or the traveling direction (forward or reverse) of the agricultural machine.
• the operations of the processing device 180 described above can be executed by one or more computers using a computer program.
• step S18 it is determined whether the first coordinate of the first coordinate point is within a predetermined range.
• This predetermined range can be set, for example, to a range of 0.5 meters to 30 meters ahead of the vehicle. If it is within the predetermined range (Yes), the process returns to step S14 and the sequential update of the first coordinate point continues. If it is not within the predetermined range (No), the process ends.
• the operations of the processing device 180 described above can be executed by one or more computers using a computer program.
• FIG. 16 is a plan view illustrating various variables that define the "state" to be estimated.
• a left-column approximation line 42L and a right-column approximation line 42R are shown.
• the left row 20L and the right row 20R of adjacent tree rows are generally planted in parallel, and the curvature ⁇ of the left row 20L and the right row 20R does not change sharply.
• the curvature ⁇ of each row is constant within a certain range (for example, within a range of several frames) in front of the agricultural machine 100.
• the left row approximation line 42L and the right row approximation line 42R are expressed by the following formulas (1) and (2), respectively.
• X and Z are components of the coordinate (X, Z) indicating the position of a point on the XZ coordinate plane.
• ⁇ is the curvature of the adjacent tree row
• W is the interval between the adjacent tree rows (left row approximation line 42L and right row approximation line 42R)
• ⁇ r is the azimuth deviation of the vehicle (agricultural machine 100) from the center line of the adjacent tree row
• ycr is the lateral deviation of the agricultural machine 100 from the center line.
• the coordinate (X, Z) that satisfies equation (1) is on the left row approximation line 42L.
• the center line of the adjacent tree row is indicated by a dotted line.
• Equations (1) and (2) are "crop row model curves" that define the positions of adjacent tree rows.
• the crop row model is represented by a quadratic curve (parabola).
• the state variables (state quantities) of the state space model in this embodiment include the curvature ⁇ of the adjacent crop rows, the spacing W of the adjacent crop rows, the azimuth deviation ⁇ r , and the lateral deviation y cr .
• equations (1) and (2) include position information (X, Z) of the tree rows that can be observed by the sensor.
• equations (1) and (2) that define the crop row model curve are used as observation equations.
• the estimated values of each state variable are updated based on the observed values of the position information of the adjacent crop rows, i.e., the observed values of the points (X, Z) on the left row approximation line 42L and the right row approximation line 42R.
• the curvature ⁇ , azimuth deviation ⁇ r , and lateral deviation y cr of the adjacent crop rows can be estimated.
• multiple feature points are extracted based on the position information of adjacent crop rows, and the position information of these multiple feature points is used as "observation values.”
• multiple feature points can be extracted from the left row approximation line 42L and the right row approximation line 42R in FIG. 16.
• FIG. 17 shows a schematic example of 10 feature points extracted in this way. In the example of FIG. 17, five feature points spaced 1 meter apart in the Z-axis direction are extracted from the left and right rows within a range of 4 to 8 meters in the forward direction of the agricultural machine 100.
• the above feature points are used as input for candidate points of adjacent tree rows, and a Kalman filter is used to estimate the vehicle's own position (lateral deviation and azimuth deviation) relative to the center line between the tree rows.
• the "Kalman filter” is an algorithm that uses a state space model to estimate state variables that cannot be directly observed, based on observed values obtained from sensors. By estimating the variance in addition to the state variables, it becomes possible to estimate the most statistically likely value when the next observed value is obtained, taking into account the variance. This makes it possible to make stable estimates even for observed values that contain errors.
• the tree row candidate points that serve as observation values contain a certain amount of error because they are based on distance measurement data of non-uniform tree rows. For this reason, when estimation is performed using only observed values, the results of self-location estimation are unstable due to this error.
• this embodiment by preparing the state space model described above and performing estimation that takes time changes into account, stable self-location estimation is achieved.
• the “state” at time k is represented by the vector ⁇ k in the following equation (3).
• ⁇ k (y ck , ⁇ k , ⁇ k , W k ) T equation (3)
• Fig. 18 is based on a "persistent prediction model" in which the change in the "state” is driven by process noise.
• v yc , v ⁇ , v ⁇ , and v W are the process noises of y ck , ⁇ k , ⁇ k , and W k , respectively.
• the process noise follows a normal distribution N(0,Q 2 ) with a mean of 0.
• Q 2 is the variance.
• F k and G k are identity matrices.
• Fig. 19 An equation that defines the relationship between the observation vector ⁇ k and the state vector ⁇ k , that is, the observation equation, is shown in Fig. 19.
• the observation noise follows a normal distribution N(0, R2 ) with mean 0, where R2 is the variance.
• an estimate of the state vector can be obtained at each time k from the following formula (7).
• k-1 is a priori estimate of the state vector at time k before an observation value is obtained.
• the priori estimate can be calculated based on the state equation.
• k1 is a posterior estimate of the state vector at time k after an observation value is obtained.
• the posterior estimate has a value obtained by updating the priori estimate based on the observation value.
• the estimate of the state vector has a variance defined by a covariance matrix.
• the processing device 180 can acquire sensor data at 10 frames per second, the observation vector ⁇ k can be acquired every 100 milliseconds.
• the processing device 180 may predict a plurality of feature points in advance using the above estimation algorithm to obtain a plurality of predicted points, and calculate the Mahalanobis distance from each predicted point to a corresponding feature point among the plurality of feature points.
• the processing device 180 may also exclude feature points (outliers) whose Mahalanobis distance is longer than a predetermined value from the observed values (Mahalanobis gate).
• the Mahalanobis gate is a method that uses the reliability (error covariance) of the estimation results in the Kalman filter to calculate the likelihood of the input observation values and removes outliers using statistical testing. By removing outliers, it is possible to suppress deterioration of estimation accuracy and divergence.
• the Kalman filter assumes that the error is normally distributed. For a certain level of steady-state error, it is possible to calculate an estimate by adjusting the parameters. However, if an error occurs that is much larger than the steady-state error, the estimation will no longer be performed correctly and may diverge. In orchards such as vineyards, not only will observation errors occur due to the various tree row shapes, but distance measurements may not be performed correctly due to factors such as the inclination of agricultural machinery and on-board sensors caused by uneven ground. If the estimated lateral and azimuth deviations fluctuate significantly due to observation errors, the steering amount will increase, increasing the possibility of colliding with trees.
• the possibility of colliding with trees can be further reduced by using a Mahalanobis gate to remove outliers from the observed data.
• the Mahalanobis gate identifies outliers using a statistical distance called the "Mahalanobis distance,” which is calculated by taking into account the correlation between multiple variables.
• the Kalman filter uses the error covariance of the predicted observed value derived in the estimation process, so it is possible to calculate the Mahalanobis distance of the observed value using that error covariance.
• the Mahalanobis distance can be calculated from the following equation (8).
• the Kalman filter assumes that errors have a normal distribution, so the Mahalanobis distance follows a chi-square distribution and outliers can be determined based on the chi-square test. In this way, each value required for the calculation is derived during the Kalman filter process, preventing an increase in calculation costs.
• FIG. 20 is a functional block diagram illustrating an example of a state estimation system 500 according to an embodiment of the present disclosure.
• the state estimation system 500 includes a Kalman filter 510A having a Mahalanobis gate 518, and a feature point extraction module 516.
• the feature point extraction module 516 includes a crop row detection module 517, which acquires sensor data from the LiDAR sensor 140.
• the crop row detection module 517 determines left and right row approximation lines of adjacent tree rows based on the sensor data using the method described above.
• the feature point extraction module 516 receives the Z coordinates 514 of multiple feature points, and determines the X coordinates of each feature point that correspond to the input Z coordinates 514 from the left and right approximation lines of the adjacent tree rows.
• the feature point extraction module 516 can acquire sensor data from the LiDAR sensor 140 at a predetermined time interval, for example, about 100 milliseconds, and input the coordinates of the feature points to the Kalman filter 510A.
• the Kalman filter 510A When the Kalman filter 510A starts operation, it first receives setting values 520 such as the initial values of the state variables and error variance, the covariance of the observation noise, and the covariance of the process noise. The Kalman filter 510A then calculates a priori estimates of the state variables based on the setting values 520. After that, when the Kalman filter 510A receives the coordinates (X, Y) of the feature point from the feature point extraction module 516 as observed values, it updates the priori estimates of the state variables to posterior estimates based on the observed values. The posterior estimates obtained in this manner are output from the Kalman filter 510A as state variables 530. The Kalman filter 510A can also update the error covariance based on the observed values, and output the posterior estimated error covariance 532.
• setting values 520 such as the initial values of the state variables and error variance, the covariance of the observation noise, and the covariance of the process noise.
• the Kalman filter 510A calculates a priori
• Such a state estimation system 500 can be realized by implementing a state estimation algorithm in the computer of the processing device 180. Therefore, the processing device 180 can determine the state variables 530 y cr , ⁇ , ⁇ r , and W every 100 milliseconds, for example, to obtain estimates of the azimuth deviation ⁇ r and the lateral deviation y cr (an estimate of the vehicle's position relative to adjacent tree rows).
• the processing device 180 can perform automatic steering or automatic driving based on the self-position estimate thus obtained. Since the state variables 530 include the curvature ⁇ and the width W between the rows of trees, the processing device 180 can also adjust the vehicle speed according to the curvature ⁇ or the width W between the rows of trees. For example, the vehicle speed may be reduced as the curvature ⁇ increases, or the vehicle speed may be reduced when the curvature ⁇ exceeds a predetermined level. In addition, when the width W between the rows of trees becomes smaller than a predetermined level close to the vehicle width of the agricultural machine 100, the vehicle speed may be reduced or stopped.
• the state equation of the above state space model a "sustained prediction model" is adopted.
• the state equation may include the vehicle's traveling speed and traveling direction as coefficients to define the time change of the state variable.
• the processing device 180 can obtain a predicted value of the state variable based on the measured or estimated value of the traveling speed and traveling direction of the agricultural machine vehicle. Then, by using such an estimated value of the state variable as an observed value, it is possible to correct the predicted value. More specifically, by using the movement amount of the agricultural machine, it is possible to correct the self-position estimation result by the Kalman filter and improve the estimation accuracy.
• the movement amount of the agricultural machine is "vehicle information" that can be acquired by an internal sensor.
• the Kalman filter can be applied again to the tracking process. We will explain the "state equation” and "observation equation” required for the Kalman filter for tracking.
• the model is defined by adding vehicle behavior to the state transitions of the azimuth error ⁇ r and the lateral error ycr of the vehicle relative to the center line.
• the time change of the curvature ⁇ is linear, and the time change of the width W between the trees is zero.
• the state model in continuous time can be approximated by the linear differential equation shown in the following equation 5.
• the output of the above-mentioned Kalman filter can be given to the lateral deviation y cr , azimuth deviation ⁇ r , tree line curvature ⁇ , and tree line spacing W.
• the lateral deviation y cr , azimuth deviation ⁇ r , tree line curvature ⁇ , and tree line spacing W are state variables that can be directly observed.
• the observation equation is obtained by adding observation noise to the following Equation 7.
• a 0 , a 1 , a 2 , and a 3 are observed values.
• the second Kalman filter 510B receives the lateral deviation y cr , azimuth deviation ⁇ r , tree line curvature ⁇ , and tree line spacing W as observed values from the above Kalman filter (first Kalman filter) 510A.
• the second Kalman filter 510B also receives the yaw rate (time rate of change of ⁇ abs ) from the IMU 114 and the vehicle speed V from the axle sensor 118.
• the second Kalman filter 510B calculates prior estimates of the state variables based on the state equation of Expression 6, and calculates posterior estimates of the state variables using the output of the first Kalman filter 510A.
• the posterior estimates of the state variables the lateral deviation yoff and the azimuth deviation ⁇ ref are provided to the ECU 182 for steering control.
• the sensor (ranging sensor or external sensor) used for map creation is a LiDAR sensor that outputs point cloud data as sensor data by scanning with a laser beam.
• a LiDAR sensor is not limited to a LiDAR sensor.
• a map of tree rows may be created using a stereo camera capable of measuring distances.
• the agricultural machine automatically travels between multiple tree rows in an orchard, but the agricultural machine may also be used to automatically travel between crop rows other than tree rows.
• the technology disclosed herein may be applied to agricultural machine such as a tractor that automatically travels between multiple crop rows in a field.
• the device that executes the processes required for automatic steering or automatic driving of the agricultural machine in the above embodiments can also be retrofitted to an agricultural machine that does not have these functions.
• a control unit that controls the operation of an agricultural machine that drives between multiple crop rows can be attached to the agricultural machine and used.
• the present disclosure includes the state estimation systems and agricultural machines, etc. described in the following items.
• a sensor mounted to the vehicle and configured, during operation, to scan a surrounding environment including the crop rows and output sensor data including location information of objects present in the environment;
• a processing device that detects an adjacent crop row located to the left or right of the vehicle based on the sensor data; Equipped with The processing device includes: determining an estimate of the curvature ⁇ of the adjacent crop row, an azimuth deviation ⁇ r of the vehicle relative to a center line of the adjacent crop row, and a lateral deviation y cr of the vehicle relative to the center line using a state space model estimation algorithm based on the position information of the adjacent crop row;
• a state estimation system configured to: [Item 2]
• the state variables of the state space model are the curvature ⁇ of the adjacent crop rows, the spacing W of the adjacent crop rows, the azimuth deviation ⁇ r , and the lateral deviation y cr ;
• the processing device includes: 2.
• the state estimation system according to item 1 , wherein an estimated value of the state variable is updated based on observed values of the position information of the adjacent crop row using a crop row model curve that defines the relationship between the curvature ⁇ , the spacing W, the azimuth deviation ⁇ r , the lateral deviation y cr , and the position information of the adjacent crop row as an observation equation.
• the processing device extracts a plurality of feature points based on the position information of the adjacent crop row, and uses the position information of the plurality of feature points as the observation value.
• the processing device includes: predicting the plurality of feature points in advance using the estimation algorithm to obtain a plurality of predicted points, and calculating a Mahalanobis distance from each predicted point to a corresponding feature point among the plurality of feature points; and 5.
• the state estimation system according to item 3 or 4 wherein feature points having the Mahalanobis distance longer than a predetermined value are excluded from the observed values.
• the processing device is configured to perform creating a map of the crop row based on the sensor data.
• the processing device includes: using the map to initiate an object detection search parallel to the second coordinate axis from a first coordinate point in the vehicle coordinate system to detect left and right rows of the adjacent crop rows; and updating the first coordinate point successively and starting an object detection search parallel to the second coordinate axis from the updated first coordinate point to detect the left row and the right row of the adjacent crop row; Run determining a curve or line segment that defines the left row based on position coordinates of a plurality of cells in the left row of the adjacent crop row detected by the object detection search; 8.
• a state equation of the state space model is an equation that defines a time change of the state variable and includes a traveling speed and a traveling direction of the vehicle as coefficients;
• the processing device includes: determining predicted values of the state variables based on measured or estimated values of a traveling speed and a traveling heading of the vehicle; and 10.
• the crop row is a tree row.
• the processing device generates a target route for the vehicle to travel based on a position of the detected adjacent crop row on the map.
• a state estimation system according to any one of items 1 to 9, A vehicle equipped with the state estimation system; and A propulsion device having a plurality of wheels including a steering wheel for propelling the vehicle; an automatic steering device for controlling the steering angle of the steering wheel; Equipped with The automatic steering device controls the steering angle based on the azimuth deviation ⁇ r and the lateral deviation y cr detected by the state estimation system.
• [Item 14] acquiring sensor data output from a sensor that is attached to the vehicle and outputs, during operation, the sensor data including position information of an object present in an environment surrounding the vehicle; determining an estimate of the curvature ⁇ of the adjacent crop row, an azimuth deviation ⁇ r of the vehicle relative to a center line of the adjacent crop row, and a lateral deviation y cr of the vehicle relative to the center line using a state space model estimation algorithm based on the position information of the adjacent crop row;
• a computer configured to run [Item 15]
• a computer program comprising: The computer acquiring sensor data output from a sensor that is attached to the vehicle and outputs, during operation, the sensor data including position information of objects present in an environment surrounding the vehicle; determining an estimate of the curvature ⁇ of the adjacent crop row, an azimuth deviation ⁇ r of the vehicle relative to a center line of the adjacent crop row, and a lateral deviation y cr of the vehicle relative to the center line using a state space model estimation algorithm based on the position information
• the technology disclosed herein can be applied to agricultural machinery such as tractors (work vehicles) that move through environments with multiple crop rows (e.g., tree rows), such as orchards, fields, or forests.
• agricultural machinery such as tractors (work vehicles) that move through environments with multiple crop rows (e.g., tree rows), such as orchards, fields, or forests.

Landscapes

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

Priority Applications (3)

Applications Claiming Priority (2)

Related Child Applications (1)

Publications (1)

Family

ID=91588261

Family Applications (1)

Country Status (4)

Cited By (1)

Citations (4)

Family Cites Families (2)

• 2023
  • 2023-09-15 JP JP2024565605A patent/JPWO2024135019A1/ja active Pending
  • 2023-09-15 EP EP23906394.4A patent/EP4620277A4/en active Pending
  • 2023-09-15 WO PCT/JP2023/033696 patent/WO2024135019A1/ja not_active Ceased
• 2025
  • 2025-06-17 US US19/240,012 patent/US20250314779A1/en active Pending

Patent Citations (5)

Non-Patent Citations (1)

Also Published As

Similar Documents

Legal Events