MOBILE OBJECT, CONTROL UNIT, AND METHOD FOR CONTROLLING OPERATION OF MOBILE OBJECT

Abstract

This invention achieves highly accurate positioning in an environment overgrown with multiple trees. A moving body moves between a plurality of rows of trees. The moving body comprises: one or more sensors that output sensor data indicating the distribution of objects in the environment around the moving body; a storage device that stores environment map data indicating the distribution of trunk parts of the plurality of rows of trees; a self-position estimation device; and a control device that controls the movement of the moving body in accordance with the position of the moving body estimated by the self-position estimation device. The self-position estimation device: detects the trunk parts of the rows of trees in the environment around the moving body on the basis of the sensor data repeatedly output from the one or more sensors while the moving body is moving; and estimates the position of the moving body by matching the detected trunk parts of the rows of trees and the environment map data.

Description

[Technical field]

[0001] The present disclosure relates to a moving object, a control unit, and a method for controlling the operation of a moving object. [Background technology]

[0002] As the next generation of agriculture, research and development is being conducted on smart agriculture that utilizes ICT (Information and Communication Technology) and IoT (Internet of Things). Research and development is also being conducted on the automation and unmanned operation of work vehicles such as tractors used in farm fields. For example, work vehicles that run with automatic steering using positioning systems such as GNSS (Global Navigation Satellite System) that enable precise positioning have been put into practical use. Patent Documents 1 to 3 disclose examples of work vehicles that perform automatic steering based on the results of positioning performed using GNSS.

[0003] Meanwhile, autonomous moving objects that use distance sensors such as LiDAR (Light Detection and Ranging) are also being developed. For example, Patent Document 4 discloses an example of a work vehicle that uses LiDAR to automatically navigate between rows of crops in a farm field. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] International Publication No. 2017 / 208306 [Patent Document 2] JP 2020-104617 A [Patent Document 3] JP 2020-12680 A [Patent Document 4] JP 2019-154379 A Summary of the Invention [Problem to be solved by the invention]

[0005] In environments where trees are densely distributed, such as orchards such as vineyards or forests, the leaves growing at the top of the trees form a canopy that acts as an obstacle or multiple reflector for radio waves from satellites. In such environments, accurate positioning using GNSS is difficult. In environments where GNSS cannot be used, it is possible to use SLAM (Simultaneous Localization and Mapping), which performs position estimation and map creation simultaneously. However, there are various challenges to be overcome in practical use of a mobile object that moves autonomously or with automatic steering in an environment with many trees using SLAM. For example, there is a challenge that maps created in the past cannot be used continuously because the distribution of tree leaves changes significantly with the change of seasons. [Means for solving the problem]

[0006] A moving body according to an exemplary embodiment of the present disclosure moves among a plurality of tree rows. The moving body includes one or more sensors that output sensor data indicating a distribution of objects in an environment around the moving body, a storage device that stores environmental map data indicating a distribution of stems of the plurality of tree rows, a self-location estimation device, and a control device that controls the movement of the moving body according to the position of the moving body estimated by the self-location estimation device. The self-location estimation device detects the stems of the tree rows in the environment around the moving body based on the sensor data repeatedly output from the one or more sensors while the moving body is moving, and estimates the position of the moving body by matching the detected stems of the tree rows with the environmental map data.

[0007] A general or specific aspect of the present disclosure may be realized by an apparatus, a system, a method, an integrated circuit, a computer program, or a computer-readable recording medium, or any combination thereof. The computer-readable recording medium may include a volatile recording medium or a non-volatile recording medium. The apparatus may be composed of a plurality of apparatuses. When the apparatus is composed of two or more apparatuses, the two or more apparatuses may be arranged in one device, or may be arranged separately in two or more separate devices. Effect of the Invention

[0008] According to an embodiment of the present disclosure, it is possible to realize automatic steering or autonomous movement of a moving object even in an environment where GNSS-based positioning is difficult. The distribution of the trunks of a tree row changes less with the seasons than the distribution of the leaves. By creating an environmental map with a focus on the trunks, it becomes possible to continuously use the same environmental map for a relatively long period of time. [Brief description of the drawings]

[0009] [Figure 1] 1 is a diagram showing a schematic example of a moving object moving on a plane between time t1 and time t2. FIG. [Figure 2A] FIG. 2 is a diagram showing a moving object and a plurality of landmarks at time t1. [Figure 2B] FIG. 13 is a diagram showing a moving object and a plurality of landmarks at time t2. [Diagram 3] 1 is a diagram showing the positional relationship between a moving body coordinate system ΣV and a world coordinate system ΣW. [Figure 4] FIG. 11 is a diagram for explaining an example of matching. [Diagram 5] FIG. 11 is a diagram for explaining an example of matching. [Figure 6] FIG. 1 is a perspective view showing the appearance of a tractor in a first embodiment. [Figure 7] FIG. 2 is a schematic diagram of the tractor as viewed from the side. [Figure 8]FIG. 2 is a block diagram showing an example of a schematic configuration of a tractor and a work machine. [Figure 9A] FIG. 2 is a schematic diagram of a LiDAR sensor viewed from the side of a tractor. [Figure 9B] FIG. 1 is a schematic diagram of a LiDAR sensor viewed vertically from above. [Figure 10] FIG. 1 is a block diagram illustrating an example of a schematic configuration of a LiDAR sensor. [Figure 11] FIG. 1 is a diagram illustrating an example of an environment in which a tractor travels. [Figure 12] FIG. 2 is a perspective view showing a schematic example of the environment around the tractor; [Figure 13A] FIG. 2 is a diagram illustrating an example of a travel route of a tractor. [Figure 13B] FIG. 11 is a diagram showing another example of a travel route of the tractor. [Figure 14A] FIG. 2 is a diagram illustrating a schematic diagram of trees and the ground being irradiated with a laser beam emitted from a LiDAR sensor. [Figure 14B] FIG. 2 is a diagram illustrating a schematic diagram of the ground being irradiated with a laser beam emitted from a LiDAR sensor. [Figure 15A] FIG. 1 is a diagram showing a schematic example of a tractor at a certain point in time and a distribution of reflection points of a laser pulse emitted from a LiDAR sensor. [Figure 15B] FIG. 15B is a diagram showing a schematic example of a tractor and a distribution of reflection points of a laser pulse emitted from a LiDAR sensor at a point in time slightly after the point in time of FIG. 15A. [Figure 16] FIG. 13 is a diagram showing a schematic example of a point cloud of a trunk obtained by one scan of a laser beam on multiple layers. [Figure 17A] 13 is a diagram showing a schematic diagram of how a point cloud of the trunk is obtained by scanning a layer Lk with a laser beam. FIG. [Figure 17B] FIG. 13 is a diagram showing a schematic diagram of how a point cloud of the trunk is obtained by scanning a laser beam of layer Lk+1. [Figure 17C]FIG. 13 is a diagram showing a schematic diagram of how a point cloud of the trunk is obtained by scanning a laser beam of layer Lk+2. [Figure 18] 13 is a graph showing an example of the relationship between the distance to a reflection point and the azimuth angle of the reflection point measured in one scan using laser beams of multiple layers. [Figure 19] FIG. 13 is a diagram showing an example of a local map created by extracting reflection points located on the surface of the trunk from scan data acquired by a tractor at a certain position. [Figure 20A] FIG. 1 is a plan view that illustrates a process for estimating the position of a moving object by matching a local map with a constructed environmental map. [Figure 20B] FIG. 1 is a plan view that illustrates a process for estimating the position of a moving object by matching a local map with a constructed environmental map. [Figure 21A] FIG. 1 is a diagram showing an example in which a LiDAR sensor is attached to the front lower part of a vehicle body of a tractor. [Figure 21B] FIG. 1 is a diagram showing an example in which a LiDAR sensor is attached to the upper front part of a tractor cabin. [Figure 22] FIG. 2 is a diagram illustrating an example of a functional configuration of a self-location estimation module. [Diagram 23] FIG. 2 is a diagram illustrating an example of a format of environment map data. [Figure 24] FIG. 1 is a diagram illustrating an example of a temporal relationship between IMU data and scan data. [Diagram 25] 10 is a flowchart showing a self-position estimation operation. [Figure 26] FIG. 11 is a diagram for explaining a filtering process of scan data. [Figure 27] 6 is a flowchart showing an example of control executed after the pose of the tractor is estimated. [Figure 28A] 1 is a diagram showing an example of a tractor traveling along a planned travel route P. FIG. [Figure 28B] FIG. 13 is a diagram showing an example of a tractor in a position shifted to the right from a planned travel route P. [Figure 28C]FIG. 13 is a diagram showing an example of a tractor in a position shifted to the left from a planned travel route P. [Figure 28D] 1 is a diagram showing an example of a tractor facing in a direction inclined with respect to a planned travel route P. FIG. [Figure 29A] FIG. 13 is a diagram showing an example of two-dimensional data showing the distribution of trunks of a tree row, generated based on an environmental map. [Figure 29B] 10 is a diagram for explaining an example of a method for determining a planned traveling route based on trunk distribution data. FIG. [Diagram 30] FIG. 13 is a diagram showing another example of trunk distribution data for route setting. [Figure 31A] FIG. 1 is a schematic diagram illustrating an example of an environment in which a row of trees is arranged in a curvilinear manner. [Figure 31B] FIG. 31B is a diagram showing a schematic diagram of a point cloud that can be observed in one scan in the environment shown in FIG. 31A. [Figure 31C] 13 is a diagram for explaining an operation of determining a travel route by estimating the position of a hidden trunk. FIG. [Figure 31D] FIG. 13 is a diagram showing an example of grid data indicating a two-dimensional distribution of trunks when a row of trees is arranged in a curved line. [Diagram 32] 1 is a diagram showing a schematic example of a situation in which an obstacle and a person are present in a travel environment of a tractor; [Figure 33A] FIG. 13 is a diagram showing an example of the arrangement of tree trunks in a row detected by a tractor. [Figure 33B] FIG. 13 is a diagram showing an example of stem distribution indicated by environmental map data. [Diagram 34] FIG. 11 is a schematic diagram showing a tractor in a second embodiment as viewed from the side. [Diagram 35] FIG. 11 is a block diagram showing an example of a schematic configuration of a tractor in a second embodiment. [Figure 36A] FIG. 1 is a diagram illustrating a schematic view of a tractor traveling near a tree. [Figure 36B] FIG. 11 is another diagram showing a schematic view of the tractor traveling near a tree. [Figure 36C]FIG. 11 is yet another diagram showing a schematic view of the tractor traveling near a tree. [Figure 37] FIG. 11 is a diagram illustrating a configuration example of a system according to a third embodiment. [Figure 38] FIG. 11 is a block diagram showing an example of the configuration of a system according to a third embodiment. [Figure 39] FIG. 1 is a perspective view showing an example of the appearance of a drone. [Diagram 40] FIG. 1 is a perspective view showing a schematic example of a data collection operation performed by multiple drones. [Diagram 41] FIG. 1 is a schematic diagram showing a tree being illuminated by a laser beam emitted from a LiDAR sensor mounted on a drone. [Diagram 42] FIG. 11 is a diagram illustrating an example of a format of local map data output from a drone. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0010] [1. Definition of terms] The following are definitions of key terms used in this specification.

[0011] A "mobile body" is a device equipped with a drive device such as wheels, crawlers, bipedal or multipedal walking devices, or propellers that generate a driving force (traction) for movement. In this disclosure, a "mobile body" includes work vehicles such as tractors, carrier vehicles, mobile robots, and unmanned aerial vehicles (UAVs, so-called drones) such as multicopters. Mobile bodies may be unmanned or manned.

[0012] "Work vehicle" refers to a vehicle capable of traveling while performing a specific task in a work area such as a farm field (e.g., an orchard, a field, a rice paddy, or a pasture), a forest, or a construction site. Examples of work vehicles include agricultural machines such as tractors, rice transplanters, combine harvesters, riding cultivators, and riding mowers, as well as vehicles used for purposes other than agriculture, such as construction vehicles and snowplows.

[0013] "SLAM" is a general term for technology that simultaneously estimates the self-location of a moving object and creates a map.

[0014] "Self-localization" is the process of estimating the position of a moving object on a map (e.g., the center of gravity of the moving object). In self-localization using SLAM, the pose of the moving object is usually obtained.

[0015] A "pose" is the "position and orientation" of an object. A pose in two-dimensional space is defined by three coordinate values, for example (x, y, θ). Here, (x, y) are coordinate values ​​in the XY coordinate system, which is a world coordinate system fixed to the Earth, and θ is the angle with respect to a reference direction. A pose in three-dimensional space is defined by three coordinate values, for example (x, y, z, θ). R ,θ P ,θ Y Here, (x, y, z) are coordinate values ​​in the XYZ coordinate system, which is the world coordinate system, and (θ R ,θ P ,θ Y ) are the roll, pitch, and yaw angles relative to the respective reference directions. The attitude of the moving body is given by (θ R ,θ P ,θ Y ) The roll angle θ R represents the amount of rotation around the axis in the forward / rearward direction of the moving body. P represents the amount of rotation of the moving object around the axis in the left-right direction. Y represents the amount of rotation around the up-down axis of the moving object. The orientation can also be specified by other angles, such as Euler angles, or by quaternions.

[0016] "Environmental map data" is data that expresses the positions or areas of objects in the environment in which the mobile object moves using a specified coordinate system. Environmental map data may be simply referred to as an "environmental map." Examples of coordinate systems that define an environmental map include not only world coordinate systems such as a geographic coordinate system fixed relative to the Earth, but also odometry coordinate systems that display poses based on odometry information. Environmental map data may include information other than positions (e.g., attribute information and other information) about objects in the environment. Environmental maps include maps in various formats, such as point cloud maps and grid maps. Hereinafter, environmental maps may be referred to as "map data" or simply as "maps." Data of local or partial maps that are generated or processed in the process of constructing an environmental map may also be referred to as "map data" or simply as "maps."

[0017] "Automatic steering" refers to steering a moving object by the action of a control device, without manual intervention. A part or all of the control device may be external to the moving object. Communication of control signals, commands, data, etc. may be performed between the control device external to the moving object and the moving object. During automatic steering, other operations, such as speed control, may be performed manually.

[0018] "Autonomous movement" means that a moving object moves under the control of a control device while sensing the surrounding environment, without human involvement in controlling the movement. Autonomous movement includes autonomous driving and autonomous flight. The control device may control the operations required for the movement of a moving object, such as steering, speed control, starting and stopping of travel, ascent, descent, and hovering during flight. Autonomous movement may include detection of obstacles and obstacle avoidance operations.

[0019] "Autonomous driving" includes autonomous driving by a control device equipped in a moving body, and driving based on commands from a computer in a traffic management system. Autonomous driving includes not only the movement of a moving body toward a destination along a predetermined route, but also the movement of a moving body following a tracking target. In addition, the moving body may move temporarily based on the instructions of an operator.

[0020] A "self-location estimation device" is a device that estimates its own location on an environmental map based on sensor data acquired by an external sensor such as a LiDAR (Light Detection and Ranging) sensor.

[0021] An "external sensor" is a sensor that senses the external state of a moving object. Examples of the external sensor include a laser range finder (also called a "range sensor"), a camera (or an image sensor), a LiDAR sensor, a millimeter wave radar, and a magnetic sensor.

[0022] An "internal sensor" is a sensor that senses the state of a moving object. The internal sensor includes a wheel encoder that measures the rotational speed of the wheels, an acceleration sensor, and an angular acceleration sensor (e.g., a gyroscope). An inertial measurement unit (IMU) includes an acceleration sensor and an angular acceleration sensor, and can output a signal indicating the amount of movement and the attitude of the moving object. Information indicating the amount of change in pose of the moving object obtained by the internal sensor is called "odometry information."

[0023] "Tree trunk" means the lignified stem of a woody plant, the main axis that stands upright above ground and gives rise to branches. It does not include the branches, leaves, or roots of the tree.

[0024] [2. Basic principles of SLAM] Next, the basic principle of self-localization estimation using the SLAM technique used in the embodiment of the present disclosure will be described. For simplicity, it is assumed here that a moving object moves in a two-dimensional space (i.e., a plane).

[0025] First, refer to FIG. 1. FIG. 1 is a diagram that shows a schematic example of a moving body 100 moving on a plane between time t1 and time t2. In this example, the moving body 100 is a work vehicle (e.g., a tractor) that runs on wheels. The moving body 100 may be a vehicle other than a tractor, or another type of moving body such as a walking robot or a drone. FIG. 1 shows a schematic position and orientation of the moving body 100 moving on a plane. The position coordinates of the moving body 100 are indicated by an XY coordinate system. The XY coordinate system is a coordinate system that is fixed to the earth and is expressed as a coordinate system of a world coordinate system Σ w FIG. 1 shows a moving body coordinate system Σ V The moving body coordinate system Σ V is a uv coordinate system in which the direction in which the moving body 100 faces is the u-axis direction, and the direction obtained by rotating the u-axis 90 degrees counterclockwise is the v-axis direction. V The position of the origin of the moving body coordinate system Σ changes as the moving body 100 moves. V The direction of the light changes with the change in the direction of the moving body 100, that is, with the rotation.

[0026] The pose (i.e., position and orientation) of the moving body 100 at time t1 is defined as r1, and the pose of the moving body 100 at time t2 is defined as r2. Pose r1 is defined, for example, by a position indicated by coordinates (x1, y1) and an orientation indicated by an angle θ1. Here, the orientation of the moving body 100 is defined as the direction in which the front of the moving body 100 faces. In addition, the positive direction of the X-axis is defined as the reference direction of the angle, and the counterclockwise direction is defined as the positive direction of the angle. Pose r2 is defined by a position indicated by coordinates (x2, y2) and an orientation indicated by an angle θ2.

[0027] 1, between time t1 and time t2, the position of the moving body 100 moves (translates) by Δd1 in the direction of angle θ1, and the orientation of the moving body 100 rotates counterclockwise by Δφ1 (=θ2-θ1). In this way, the motion of the moving body 100 is a combination of "translation" and "rotation."

[0028] In this example, when the distance Δd1 is sufficiently short, the moving direction of the moving body 100 is the moving body coordinate system Σ V Therefore, the following equation 1 holds:

number

[0029] If the moving body 100 is equipped with an internal sensor such as a wheel speed sensor and / or an inertial measurement unit (IMU), the estimated values ​​of Δd1 and Δφ1, i.e., odometry information, can be obtained from the internal sensor. If the time difference between time t1 and time t2 is short, such as 10 milliseconds, the distance Δd1 is short enough that Equation 1 holds. By periodically updating the estimated values ​​of Δd1 and Δφ1 as time progresses from t1 to t2 to t3, . . ., it is possible to estimate changes in the position and orientation (i.e., pose) of the moving body 100. In other words, if an initial pose, for example (x1, y1, θ1), is known, the estimated value of the pose of the moving body 100 thereafter can be periodically updated based on the odometry information. However, estimating the pose based on the odometry information has a problem in that errors accumulate. For this reason, it is often necessary to use a satellite positioning system or SLAM technology to obtain a highly accurate estimate of the position of the moving body 100.

[0030] Next, self-location estimation performed using an external sensor such as LiDAR will be described with reference to Figures 2A to 5. Below, examples of self-location estimation operation will be described for both the case where environmental map data exists and the case where it does not exist.

[0031] <If environmental map data exists> In the example shown in Figures 2A and 2B, the environment in which the moving object 100 moves between time t1 and time t2 includes landmarks m1, m2, m3, and m4 whose positions are fixed. In this example, environmental map data indicating the positions of the landmarks already exists. Such environmental map data includes, for example, identifiers (e.g., landmark numbers) that identify the individual landmarks and a world coordinate system Σ of the landmarks associated with each identifier. w The world coordinate system Σ w The coordinates of the landmarks m1, m2, m3, and m4 in are (x m1 ,y m1 ), (x m2 ,y m2 ), (x m3 ,y m3 ), (x m4 ,y m4 ) Each point in the point cloud data acquired by the LiDAR may function as a landmark. An environmental map composed of point cloud data is called a point cloud map. The positions of the landmarks may be displayed by cells on a grid map.

[0032] In the example of Fig. 2A, the observation values ​​(or measurement values) of landmarks m1, m2, m3, and m4 acquired by the moving body 100 in pose r1 are respectively designated as z1, z2, z3, and z4. The observation values ​​are, for example, values ​​indicating the distance and direction to each landmark measured by an external sensor. Since the observation values ​​are data acquired by an external sensor, they may be referred to as "sensor data" in this disclosure.

[0033] As shown in FIG. 2B, the moving body 100 moves, and the observed values ​​of the landmarks m1, m2, and m3 acquired by the moving body 100 at a pose r2 are denoted as z5, z6, and z7, respectively.

[0034] Based on these observations, the mobile coordinate system Σ V The position coordinates of the landmarks on the moving object coordinate system Σ Vis a coordinate system fixed to the moving body 100, so the moving body coordinate system Σ V The position coordinates on the screen change according to the change in the pose of the moving body 100.

[0035] The position of the landmark obtained based on the observation value has coordinates in a sensor coordinate system determined by the position and orientation of the external sensor. Strictly speaking, the sensor coordinate system is a moving body coordinate system Σ V However, in the following explanation, the sensor coordinate system and the moving body coordinate system Σ V The sensor coordinate system and the moving object coordinate system Σ V Since the relationship between these two coordinate systems is known, one coordinate system can be made to coincide with the other coordinate system by rotating it by a known angle and translating it by a known distance.

[0036] A moving object 100 moving within an environment observes multiple landmarks and is then calculated based on the moving object coordinate system Σ V Then, obtain the position coordinates of each landmark in the world coordinate system Σ W When the position coordinates of each landmark in are included in the environmental map data, the poses r1 and r2 can be estimated based on the observed values ​​z1 to z7, etc. Such estimation is possible, for example, by matching the position coordinates of the landmarks obtained from the observed values ​​with the position coordinates of the landmarks included in the environmental map data.

[0037] Next, referring to Figure 3, the moving body coordinate system Σ V The coordinates (u1,v1) above are in the world coordinate system Σ W We will explain how to convert the coordinates (u1', v1') to the coordinates (u1', v1') above. Figure 3 shows the moving body coordinate system Σ V and the world coordinate system Σ W In the example of Figure 3, for simplicity, the moving body coordinate system Σ V The origin of the sensor coordinate system (center of the sensor) is set to coincide with the origin of the sensor coordinate system, and both coordinate systems are considered to be the same. VThe coordinates (u1, v1) on the left are calculated based on the observation value z5 of the landmark m1 from the moving object 100 at the pose r2 at the time t2. m1 and the angle φ with respect to the u axis m1 Then, u1 and v1 are defined as u1=r m1 cosφ m1 , v1=r m1 sinφ m1 It is calculated by the moving body coordinate system Σ V From the coordinates above to the world coordinate system Σ W The transformation to the above coordinates (coordinate transformation) is done in the moving body coordinate system Σ V is rotated clockwise by an angle of θ2, and the moving body coordinate system Σ V The origin of the world coordinate system Σ W This is done by translating the object to the origin of the equation. At this time, the following equations 2 and 3 hold.

number

number

[0038] Here, R is a rotation matrix determined by the orientation of the moving body 100, and T is the rotation matrix of the world coordinate system Σ w The contents of the rotation matrix R and the position vector T are calculated from the pose of the moving body 100.

[0039] The (u1', v1') obtained by the coordinate transformation is in the world coordinate system Σ w The coordinates of the landmark m1 in (x m1 ,y m1 However, if the estimated pose of the moving object 100 deviates from the true value, (u1', v1') and (x m1 ,y m1 ) will cause an error (distance) between (u1', v1') and (x m1 ,y m1The aim of the present invention is to determine the contents of the rotation matrix R and the position vector T so that the error between the two vectors is small.

[0040] Next, an example of matching will be described in more detail with reference to Fig. 4 and Fig. 5. Fig. 4 shows a moving body coordinate system Σ V The coordinates of landmarks m1, m2, and m3 on the uv coordinate system are shown in schematic form. These coordinates can be calculated from the observed values ​​z5, z6, and z7. In this example, the coordinates calculated from the observed values ​​z5, z6, and z7 are (u5, v5), (u6, v6), and (u7, v7), respectively. The moving body coordinate system Σ V From the world coordinate system Σ W By performing coordinate transformation to the moving body coordinate system Σ V The above (u5,v5), (u6,v6), and (u7,v7) are respectively in the world coordinate system Σ as shown in Figure 5. W The coordinates are transformed to the coordinates (u5',v5'), (u6',v6'), and (u7',v7') shown above. Figure 5 shows the transformed coordinates (u5',v5'), (u6',v6'), and (u7',v7') and the corresponding landmark coordinates (x m2 ,y m2 ), (x m3 ,y m3 ), (x m4 ,y m4 ) is shown. The coordinate transformation is defined by the rotation by the rotation matrix R and the translation by the position vector T. The world coordinate system Σ W In the above, (u5',v5'), (u6',v6'), and (u7',v7') are the coordinates of the corresponding landmarks, respectively (x m2 ,y m2 ), (x m3 ,y m3 ), (x m4 ,y m4 The contents of the coordinate transformation (R, T) are determined so as to approximate (i.e. match)

[0041] In the equation of Equation 2 described above, the unknowns are x2, y2, and θ2. Since the number of unknowns is three, if there are three or more equations equivalent to Equation 2, x2, y2, and θ2 can be found by calculation. As shown in FIG. 5, when three or more landmarks are observed from the moving object 100 at the same pose r2, three equations equivalent to Equation 2 are obtained, and the calculated value of pose r2 (x2, y2, θ2) can be obtained by solving the simultaneous equations. Since errors are added to real observations, a simultaneous equation consisting of more than three equations is used, and optimization is performed by the least squares method or the like.

[0042] Examples of algorithms for estimating the self-location by performing such matching-based optimization include an ICP matching method and an NDT matching method. Any of these matching methods or other methods may be used.

[0043] When estimating the position of the moving body 100 from the observation values ​​and environmental map data, it is not necessary to estimate the orientation of the moving body 100. For example, as shown in Figures 2A and 2B, if the distance to a landmark whose position is known can be obtained from the observation values, it is possible to calculate the position of the moving body 100 (more precisely, the position of the distance measuring sensor) based on the principle of triangulation. Estimating the position based on such a principle is also included in "self-location estimation."

[0044] <If there is no environmental map data> In the absence of environmental map data, a process is required to estimate the pose of the moving body 100 while determining (i.e., estimating) the coordinates of the landmarks m1, m2, m3, m4, based on the observation values ​​acquired while the moving body 100 is moving, as shown in Figures 2A and 2B. W The coordinates in the above form the environmental map data. Therefore, to construct the map, we define the world coordinate system Σ W It is necessary to determine the position coordinates on the

[0045] 2A and 2B, the moving object 100 acquires multiple observations from each of the landmarks m2, m3, and m4 while moving. By making more observations, it is possible to obtain a number of simultaneous equations that are greater than the total number of unknowns included in the pose of the moving object 100 and the coordinates of the landmarks. This makes it possible to calculate estimates of the pose of the moving object 100 and the coordinates of the landmarks.

[0046] There are many types of algorithms for performing self-localization and creating an environmental map by SLAM. Examples of SLAM algorithms include not only algorithms using LiDAR sensors, but also algorithms using other external sensors such as cameras. In addition, a Bayes filter such as a particle filter may be used for self-localization, or the accuracy of pose estimation may be improved by a graph-based method. In the embodiment of the present disclosure, the type of SLAM algorithm is not particularly limited.

[0047] In addition, LiDAR sensors include a scan type sensor that acquires information on the distance distribution of objects in space by scanning a laser beam, and a flash type sensor that acquires information on the distance distribution of objects in space by using light that diffuses over a wide range. The scan type LiDAR sensor uses light with a higher intensity than the flash type LiDAR sensor, so it can acquire distance information from a longer distance. On the other hand, the flash type LiDAR sensor has a simple structure and can be manufactured at a low cost, so it is suitable for applications that do not require strong light. In this disclosure, an example in which a scan type LiDAR sensor is mainly used will be described, but a flash type LiDAR sensor may also be used depending on the application.

[0048] When observing an object in the environment using a typical scanning LiDAR sensor, a pulsed laser beam (i.e., a laser pulse) is emitted and the time it takes for the laser pulse reflected by an object in the surrounding environment to return to the LiDAR sensor is measured, allowing the distance and direction to the reflection point located on the surface of the object to be known. If the distance and direction to the reflection point are known, the moving body coordinate system ΣV The coordinates of the "reflection point" in the moving object coordinate system can be obtained. Scanning LiDAR sensors can be classified into two-dimensional LiDAR sensors and three-dimensional LiDAR sensors. With a two-dimensional LiDAR sensor, the environment can be scanned by rotating a laser beam in a single plane. In contrast, with a three-dimensional LiDAR sensor, the environment can be scanned by rotating multiple laser beams along different conical surfaces. The coordinates (two-dimensional or three-dimensional coordinate values) of each reflection point obtained by these LiDAR sensors are expressed as a coordinate system in the moving object coordinate system Σ V The coordinates of each reflection point are expressed as the moving body coordinate system Σ V From the world coordinate system Σ W By transforming each reflection point into the world coordinate system Σ W The coordinates on the moving object coordinate system Σ can be obtained, and as a result, a point cloud map can be constructed. V From the world coordinate system Σ W As described above, information on the pose of the moving body 100 is required to convert the pose into

[0049] In the SLAM technique, the pose of the moving body 100 can be estimated and the environmental map can be constructed at the same time. However, when constructing the environmental map, the pose of the moving body 100 may be estimated or measured using a technique other than the SLAM technique. This is because the pose of the moving body 100 can also be determined using a satellite positioning system and an inertial guidance unit that measure the position of the moving body 100. However, in a situation where a positioning system such as a satellite positioning system cannot be used, it is necessary to estimate the pose of the moving body 100 using the SLAM technique in order to construct the environmental map. Note that the position of the moving body 100 may be estimated using the SLAM technique, and the orientation or attitude of the moving body 100 may be estimated by other sensors such as an inertial measurement unit.

[0050] [3. Embodiment] Hereinafter, an embodiment of the present disclosure will be described. However, more detailed explanation than necessary may be omitted. For example, detailed explanation of already well-known matters and duplicate explanation of substantially the same configuration may be omitted. This is to avoid the following explanation becoming unnecessarily redundant and to facilitate understanding by those skilled in the art. Note that the inventor provides the accompanying drawings and the following explanation so that those skilled in the art can fully understand the present disclosure, and does not intend to limit the subject matter described in the claims by them. In the following description, components having the same or similar functions are given the same reference numerals.

[0051] [3-1. First embodiment] A moving object according to a first embodiment of the present disclosure will be described.

[0052] The mobile body in this embodiment is used in an environment where a plurality of trees grow thickly and a plurality of tree rows are formed. The mobile body includes one or more sensors that output sensor data indicating the distribution of objects in the surrounding environment, a storage device, a self-location estimation device, and a control device that controls the movement of the mobile body. The storage device stores environmental map data indicating the distribution of trunks of a plurality of tree rows. The self-location estimation device detects trunks of tree rows in the environment surrounding the mobile body based on sensor data repeatedly output from one or more sensors while the mobile body is moving, and estimates the position of the mobile body by matching the detected trunks of the tree rows with the environmental map data. The control device controls the movement of the mobile body according to the estimated position of the mobile body. The mobile body in this embodiment further includes a data generation device that generates environmental map data or local map data for generating the environmental map data. The data generation device detects trunks of tree rows in the environment surrounding the mobile body while performing self-location estimation based on sensor data repeatedly output from one or more sensors while the mobile body is moving, and generates local map data for generating environmental map data indicating the distribution of trunks of the detected tree rows, and records the local map data in the storage device. The data generating device can also generate environmental map data by connecting together the repeatedly generated local map data and record the data in the storage device. In this manner, the mobile body in this embodiment has a function of generating an environmental map while moving among a plurality of tree rows, and a function of autonomously moving among the tree rows while estimating the position of the mobile body by using the generated environmental map.

[0053] The one or more sensors may include, for example, at least one LiDAR sensor that outputs two-dimensional or three-dimensional point cloud data as sensor data. In this specification, "point cloud data" broadly means data indicating a distribution of a plurality of reflection points observed by a LiDAR sensor. The point cloud data may include, for example, coordinate values ​​of each reflection point in two-dimensional or three-dimensional space, and / or information indicating the distance and direction of each reflection point.

[0054] The LiDAR sensor repeatedly outputs point cloud data at a preset cycle, for example. The data generating device can detect the trunk based on the position of each point in the point cloud data output during one or more cycles, or the distance or angle of each point from the moving object.

[0055] Below, the configuration and operation of this embodiment will be described using as an example a case in which the moving body is a work vehicle such as a tractor that is mainly used for work in orchards such as vineyards, and the sensor is a scanning type LiDAR sensor.

[0056] <Configuration> Fig. 6 is a perspective view showing the appearance of the tractor 100A in this embodiment. Fig. 7 is a schematic diagram showing the tractor 100A as viewed from the side. Fig. 7 also shows an implement 300 that is connected to the tractor 100A when used.

[0057] As shown in Fig. 6, the tractor 100A in this embodiment includes a LiDAR sensor 110, a GNSS unit 120, and one or more obstacle sensors 130. Although one obstacle sensor 130 is shown in the example of Fig. 6, the obstacle sensor 130 may be provided at multiple locations on the tractor 100A.

[0058] As shown in Fig. 7, the tractor 100A includes a vehicle body 101, a prime mover (engine) 102, and a speed changer (transmission) 103. The vehicle body 101 is provided with wheels (tires) 104 and a cabin 105. The wheels 104 include front wheels 104F and rear wheels 104R. A driver's seat 107, a steering device 106, and an operation terminal 200 are provided inside the cabin 105. One or both of the front wheels 104F and the rear wheels 104R may be crawlers instead of tires.

[0059] The LiDAR sensor 110 in this embodiment is disposed at the front lower part of the vehicle body 101. The LiDAR sensor 110 may be disposed at a position lower than the average height of the trunks of the tree rows present in the environment in which the tractor 100A travels, for example, at a height of 15 cm to 100 cm from the ground. The LiDAR sensor 110 may be disposed at a position lower than half the height of the tractor 100A, for example. The height of the tractor 100A is the height from the ground to the top of the cabin 105, and may be, for example, 2 meters or more. The LiDAR sensor 110 may be disposed at a position lower than 1 / 3 of the height of the tractor 100A. In the example of FIG. 7, the LiDAR sensor 110 is disposed at a position lower than the upper part of the wheels 104 and the headlights 109. The effect obtained by disposing the LiDAR sensor 110 at such a low position will be described later.

[0060] While the tractor 100A is moving, the LiDAR sensor 110 repeatedly outputs sensor data indicating the distance and direction of an object in the surrounding environment, or two-dimensional or three-dimensional coordinate values. The sensor data output from the LiDAR sensor 110 is processed by a control device such as an ECU (Electronic Control Unit) provided in the tractor 100A. The control device can execute processes such as generating environmental map data based on the sensor data and estimating the self-position using the environmental map by using the above-mentioned SLAM algorithm. Note that the control device may create local map data for generating the environmental map data instead of completing the environmental map data. In that case, the process of constructing the environmental map data by integrating the local map data may be executed by a computer outside the tractor 100A, for example, a cloud server.

[0061] In the example of FIG. 7, the obstacle sensor 130 is provided at the rear of the vehicle body 101. The obstacle sensor 130 may be provided at a location other than the rear of the vehicle body 101. For example, one or more obstacle sensors 130 may be provided at any of the side, front, and cabin 105 of the vehicle body 101. The obstacle sensor 130 detects an object that is relatively close to the tractor 100A. 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. A plurality of obstacle sensors 130 may be provided at different positions on the body of the tractor 100A. For example, a plurality of laser scanners and a plurality of ultrasonic sonars may be provided at different positions on the body. By providing such a large number of obstacle sensors 130, blind spots in monitoring obstacles around the tractor 100A can be reduced. The LiDAR sensor 110 may function as an obstacle sensor.

[0062] The GNSS unit 120 is provided on the upper part of the cabin 105. The GNSS unit 120 is a GNSS receiver including an antenna for receiving signals from GNSS satellites and a processing circuit. The GNSS unit 120 receives GNSS signals transmitted from GNSS satellites such as Global Positioning System (GPS), GLONAS, Galileo, BeiDou, or Quasi-Zenith Satellite System (QZSS, for example, Michibiki), and performs positioning based on the signals. The tractor 100A in this embodiment is mainly used in an environment where multiple trees grow and it is difficult to use GNSS, such as a vineyard, so positioning is performed using the LiDAR sensor 110. However, in an environment where a GNSS signal can be received, positioning may be performed using the GNSS unit 120. By combining positioning by the LiDAR sensor 110 and positioning by the GNSS unit 120, the stability or accuracy of positioning can be improved.

[0063] The prime mover 102 is, for example, a diesel engine. An electric motor may be used instead of the diesel engine. The transmission 103 can change the propulsive force of the tractor 100A by changing the speed. The transmission 103 can also switch between forward and reverse travel of the tractor 100A.

[0064] 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 power steering device includes a hydraulic device or an electric motor that supplies an auxiliary force for rotating the steering shaft. The front wheels 104F are steered wheels, and the traveling direction of the tractor 100A can be changed by changing the steering angle. The steering angle of the front wheels 104F can be changed by operating the steering wheel. When automatic steering is performed, the steering angle is automatically adjusted by the force of the hydraulic device or the electric motor under the control of an electronic control unit (ECU) arranged in the tractor 100A.

[0065] A coupling device 108 is provided at the rear of the vehicle body 101. The coupling device 108 includes, for example, a three-point link (hitch) device, a PTO (Power Take Off) shaft, and a universal joint. The working machine 300 can be attached to and detached from the tractor 100A by the coupling device 108. The coupling device 108 can raise and lower the three-point link device by, for example, a hydraulic device, and control the position or attitude of the working machine 300. In addition, power can be sent from the tractor 100A to the working machine 300 via the universal joint. The tractor 100A can make the working machine 300 perform a predetermined task while pulling the working machine 300. The coupling device may be provided at the front of the vehicle body 101. In that case, the working machine can be connected to the front of the tractor 100A. Note that, when the LiDAR sensor 110 is used with the working machine connected to the front of the tractor 100A, the LiDAR sensor 110 is disposed at a position where the laser beam is not blocked by the working machine.

[0066] 7 is a sprayer that sprays a chemical on crops, but the working machine 300 is not limited to a sprayer. For example, any working machine such as a mower, a seeder, a spreader, a rake, a baler, a harvester, a plow, a harrow, or a rotary can be connected to the tractor 100A and used.

[0067] In the following description, the uvw coordinate system shown in FIG. 7 is used. The uvw coordinate system is a coordinate system fixed to the tractor 100A and the LiDAR sensor 110. The traveling direction of the tractor 100A is the u-axis direction, the left direction is the v-axis direction, and the upward direction is the w-axis direction. For ease of understanding, the drawings are illustrated as if the origin of the uvw coordinate system exists at a position away from the tractor 100A, but in reality, the origin of the uvw coordinate system coincides with the center of the LiDAR sensor 110 provided on the tractor 100A.

[0068] FIG. 8 is a block diagram showing an example of a schematic configuration of the tractor 100A and the work machine 300. In this example, the tractor 100A includes an inertial measurement unit (IMU) 125, a drive unit 140, a storage device 150, a plurality of electronic control units (ECUs) 160, 170, 180, and a communication interface (IF) 190 in addition to the LiDAR sensor 110, the GNSS unit 120, the obstacle sensor 130, and the operation terminal 200. The work machine 300 includes a drive unit 340, a control device 380, and a communication interface 390. Note that FIG. 8 shows components that are relatively highly related to the self-position estimation operation, the automatic driving operation, and the map generation operation in this embodiment, and the illustration of other components is omitted.

[0069] The IMU 125 in this embodiment includes a three-axis acceleration sensor and a three-axis gyroscope. The IMU 125 functions as a motion sensor and can output signals indicating various quantities such as the acceleration, speed, displacement, and attitude of the tractor 100A. The IMU 125 outputs the signals at a frequency of, for example, about several tens to several thousands of times per second. Instead of the IMU 125, a three-axis acceleration sensor and a three-axis gyroscope may be separately provided.

[0070] The drive device 140 includes various devices necessary for the travel of the tractor 100A and the driving of the work implement 300, such as the above-mentioned prime mover 102, transmission 103, wheels 104, steering device 106, and coupling device 108. The prime mover 102 is equipped with an internal combustion engine such as a diesel engine. The drive device 140 may be equipped with an electric motor for traction instead of or in addition to the internal combustion engine.

[0071] The storage device 150 includes a storage medium such as a flash memory or a magnetic disk, and stores various data generated by the ECUs 160, 170, and 180, including environmental map data. The storage device 150 also stores computer programs that cause the ECUs 160, 170, and 180 to execute various operations, which will be described later. Such computer programs may be provided to the tractor 100A 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.

[0072] The ECU 160 is an electronic circuit that executes processing based on the above-mentioned SLAM technology. The ECU 160 includes a self-location estimation module 162 and a map data generation module 164. The ECU 160 is an example of the above-mentioned self-location estimation device and data generation device. The ECU 160 can generate map data while estimating the position and orientation (i.e., pose) of the tractor 100A based on data or signals repeatedly output from the LiDAR sensor 110 and the IMU 125 while the tractor 100A is traveling. The ECU 160 can also estimate the position and orientation of the tractor 100A by matching the sensor data output from the LiDAR sensor 110 with the environmental map data during automatic traveling after the environmental map data is generated.

[0073] The self-location estimation module 162 executes a calculation for estimating the position and orientation of the tractor 100A, that is, for estimating the self-location. The map data generation module 164 executes a process for generating map data. The "map data" generated by the map data generation module 164 includes environmental map data used for matching during automatic driving, and local map data, which is partial data generated for constructing the environmental map data. The map data generation module 164 can generate environmental map data by connecting the repeatedly generated local map data, and record the environmental map data in the storage device 150. The self-location estimation module 162 and the map data generation module 164 may be realized by one circuit, or may be separated into multiple circuits.

[0074] At a stage where the environmental map data has not yet been generated, the ECU 160 can detect the trunks of the tree rows in the environment around the tractor 100A while performing self-location estimation, repeatedly generate local map data indicating the distribution of the detected trunks of the tree rows, and record the local map data in the storage device 150. The ECU 160 can further generate environmental map data for the entire farm field (e.g., a vineyard) or for a section of the farm field by connecting the local map data. The environmental map data may be generated for each section of the farm field. The ECU 160 may not detect the trunks of the tree rows at the stage of generating the local map data, but may detect the trunks at the stage of generating the final environmental map data, and record the trunks in a format that allows them to be distinguished from other objects. During autonomous driving after the environmental map data has been generated, the ECU 160 estimates the position and orientation of the tractor 100A by matching the sensor data output from the LiDAR sensor 110 with the environmental map data. It is to be noted that the ECU 160 may determine only the position of the tractor 100A by matching, and determine the orientation of the tractor 100A by using a signal from the IMU 125.

[0075] The ECU 170 is a circuit that performs a process of determining a route for the tractor 100A. At a stage where the environmental map data has not yet been generated, the ECU 170 determines a route for the tractor 100A to travel based on data or signals output from the LiDAR sensor 110 and the obstacle sensor 130. For example, a route that passes between a plurality of tree rows detected based on the sensor data output from the LiDAR sensor 110 and avoids obstacles is determined as a target route. During automatic travel after the environmental map data has been generated, the ECU 170 determines a target route (hereinafter also referred to as a "planned travel route") based on the environmental map data or an instruction from a user.

[0076] The ECU 180 is a circuit that controls the driving device 140. The ECU 180 controls the driving device 140 based on the position and orientation of the tractor 100A estimated by the ECU 160 and the planned travel route determined by the ECU 170. The ECU 180 also generates a signal that controls the operation of the work implement 300 and transmits the signal from the communication IF 190 to the work implement 300.

[0077] The ECUs 160, 170, and 180 can communicate with each other according to a vehicle bus standard such as CAN (Controller Area Network). In FIG. 8, the ECUs 160, 170, and 180 are shown as individual blocks, but the functions of each of these may be realized by multiple ECUs. Alternatively, a single on-board computer that integrates the functions of the ECUs 160, 170, and 180 may be provided. The tractor 100A may be provided with ECUs other than the ECUs 160, 170, and 180, and any number of ECUs may be provided depending on the functions.

[0078] The communication IF 190 is a circuit that communicates with the communication IF 390 of the working machine 300. The communication IF 190 transmits and receives signals conforming to a communication control standard, such as ISOBUS based on ISO 11783, between the communication IF 390 of the working machine 300. This makes it possible to cause the working machine 300 to perform a desired operation and to obtain information from the working machine 300. The communication IF 190 can also communicate with an external computer via a wired or wireless network. For example, it can communicate with a computer such as a server in an agricultural management system that manages the growth status of crops, the operating status of the tractor 100A, and work records.

[0079] The operation terminal 200 is a terminal through which a user performs operations related to the automatic driving or automatic steering of the tractor 100A, and may also be referred to as a virtual terminal (VT). The operation terminal 200 may include a display device such as a touch screen, and / or one or more buttons. By operating the operation terminal 200, a user can perform various operations such as switching the automatic driving mode or automatic steering mode on / off, setting the initial position of the tractor 100A, setting a route, and recording or editing an environmental map.

[0080] The drive device 340 in the work machine 300 performs operations necessary for the work machine 300 to perform a predetermined task. The drive device 340 includes devices according to the application of the work machine 300, such as a pump, a hydraulic device, or an electric motor.

[0081] The control device 380 controls the operation of the drive device 340. In response to a signal transmitted from the tractor 100A via the communication IF 390, the control device 380 causes the drive device 340 to perform various operations.

[0082] Next, a configuration example of the LiDAR sensor 110 will be described with reference to FIG. 9A and FIG. 9B. The LiDAR sensor 110 in this embodiment is a scan-type sensor capable of acquiring information on the distance distribution of an object in space by scanning a laser beam. FIG. 9A is a schematic diagram of the LiDAR sensor 110 when viewed from the side of the tractor 100A. FIG. 9B is a schematic diagram of the LiDAR sensor 110 when viewed vertically from above. A plurality of straight lines extending radially in FIG. 9A and FIG. 9B typically represent the central axis (or traveling direction) of the laser beam emitted from the LiDAR sensor 110. Each laser beam is collimated into a parallel beam, but has a spread angle of several milliradians (for example, 0.1 to 0.2 degrees). Therefore, the cross-sectional size (spot diameter) of each laser beam expands in proportion to the distance from the LiDAR sensor 110. For example, a light spot with a diameter of several centimeters may be formed 20 meters away from the LiDAR sensor 110. For simplicity, the divergence of the laser beam is ignored in the figure, and only the central axis of the laser beam is shown.

[0083] The LiDAR sensor 110 in the example shown in FIG. 9A can emit laser beams at different elevation angles from multiple laser light sources arranged vertically. The elevation angle is defined by the angle with respect to the uv plane. In this example, the uv plane is almost parallel to the horizontal plane. Note that if the ground (surface) is inclined with respect to the horizontal plane, the uv plane intersects with the horizontal plane. In FIG. 9A, N laser beams L1, . . . , L N Here, "N" is an integer equal to or greater than 1, and may be, for example, 10 or more, or in high-performance models, 64, or even 100 or more. The elevation angle of the k-th laser beam from the bottom among the multiple laser beams is defined as θ k FIG. 9A shows an example of an elevation angle θ of the (N-1)th laser beam. N-1 The elevation angle of the laser beam directed upward from the uv plane is called a "positive elevation angle," and the elevation angle of the laser beam directed downward from the uv plane is called a "negative elevation angle."

[0084] A LiDAR sensor where N is 1 may be called a "2D LiDAR", and a LiDAR sensor where N is 2 or more may be called a "3D LiDAR". When N is 2 or more, the angle between the first laser beam and the Nth laser beam is called the "vertical viewing angle". The vertical viewing angle may be set within a range of, for example, about 20° to 60°.

[0085] The LiDAR sensor 110 can change the emission direction (azimuth angle) of the laser beam as shown in FIG. 9B. FIG. 9B shows how the emission directions of the multiple laser beams shown in FIG. 9A rotate 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., 210° or 270°). The range of the azimuth angle of the emission direction of the laser beam is called the "horizontal viewing angle." The horizontal viewing angle may be set within a range of, for example, about 90° to 360°. The LiDAR sensor 110 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 becomes 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. An operation of measuring the distance to the reflection point while the azimuth angle of the laser beam rotates once around the rotation axis is called one scan. The sensor data obtained by one scan includes data measured for each layer associated with a specific elevation angle shown in FIG. 9A. Therefore, as the number of layers increases, the number of points in the point cloud obtained by one scan of the same environment increases. The LiDAR sensor 110 repeats the scanning operation at a frequency of, for example, about 1 to 20 times per second. For example, 100,000 or more pulses of laser light can be emitted in different directions during one scan operation.

[0086] FIG. 10 is a block diagram showing an example of a schematic configuration of the LiDAR sensor 110. As shown in FIG. 10, the LiDAR sensor 110 includes a plurality of laser units 111, an electric motor 114, a control circuit 115, a signal processing circuit 116, and a memory 117. Each laser unit 111 includes a laser light source 112 and a photodetector 113. Each laser unit 111 may include an optical system such as a lens and a mirror, but these are not shown. The motor 114 changes the direction of the laser beam emitted from each laser light source 112, for example, by rotating a mirror arranged on the optical path of the laser beam emitted from each laser light source 112.

[0087] The laser light source 112 includes a laser diode, and emits a pulsed laser beam of a predetermined wavelength in response to a command from the control circuit 115. The wavelength of the laser beam may be, for example, a wavelength included 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 113. 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. When indium gallium arsenide (InGaAs) is used as the material of the photoelectric conversion element, for example, a wavelength of 1000 nm or more and 1650 nm or less may be used. Note that the wavelength of the laser beam is not limited to the near-infrared wavelength range. In applications where the influence of ambient light is not a problem (for example, nighttime use, etc.), a wavelength included in the visible range (approximately 400 nm to 700 nm) may be used. Depending on the application, it is also possible to use the ultraviolet wavelength range. In this specification, radiation included in the ultraviolet, visible, and infrared wavelength ranges is generally referred to as "light."

[0088] The photodetector 113 is a device that detects a laser pulse emitted from the laser light source 112 and reflected or scattered by an object. The photodetector 113 includes a photoelectric conversion element such as an avalanche photodiode (APD). The photodetector 113 outputs an electrical signal according to the amount of received light.

[0089] In response to a command from the control circuit 115, the motor 114 rotates a mirror disposed on the optical path of the laser beam emitted from each laser light source 112. This realizes a scanning operation that changes the emission direction of the laser beam.

[0090] The control circuit 115 controls the emission of laser pulses by the laser light source 112, the detection of reflected pulses by the photodetector 113, and the rotational operation by the motor 114. The control circuit 115 can be realized by a circuit including a processor, such as a microcontroller unit (MCU).

[0091] The signal processing circuit 116 is a circuit that performs calculations based on the signal output from the photodetector 113. The signal processing circuit 116 calculates the distance to an object that has reflected the laser pulse emitted from each laser light source 112, for example, by a ToF (Time of Flight) method. There are two types of ToF methods: a direct ToF method and an indirect ToF method. In the direct ToF method, 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 112 to when the reflected light is received by the photodetector 113. In the indirect ToF method, a plurality of exposure periods are set in the photodetector 113, and 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 116 generates and outputs sensor data indicating, for example, the distance to each reflection point and the direction of the reflection point. The signal processing circuit 116 may further calculate coordinates (u, v) or (u, v, w) in the sensor coordinate system based on the distance to each reflection point and the direction of the reflection point, and output the calculated coordinates as part of the sensor data.

[0092] Although the control circuit 115 and the signal processing circuit 116 are separated into two circuits in the example of FIG. 10, they may be realized by a single circuit.

[0093] The memory 117 is a storage medium that stores data generated by the control circuit 115 and the signal processing circuit 116. The memory 117 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 111. Such data is generated every time a laser pulse is emitted and recorded in the memory 117. The control circuit 115 outputs the data at a predetermined period (for example, a time required for emitting a predetermined number of pulses, a half scan period, or one scan period, etc.). The output data is recorded in the storage device 150 of the tractor 100A.

[0094] 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 be used. In 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.

[0095] <Operation> Next, the operation of the tractor 100A will be described.

[0096] FIG. 11 is a diagram showing a schematic example of an environment in which a tractor 100A travels. In this example, the tractor 100A uses a work machine 300 to perform a predetermined task (e.g., mowing, pest control, etc.) while traveling among a plurality of tree rows 20 in a vineyard. In a vineyard, the sky is blocked by branches and leaves, making automatic travel using GNSS difficult. Also, in a vineyard, the outer shapes of the leaves of trees or hedges change significantly depending on the season. For this reason, it is difficult to use a point cloud map created by conventional SLAM technology throughout the year.

[0097] Therefore, in the embodiment of the present disclosure, an environmental map suitable for an orchard such as a vineyard is prepared, and the tractor 100A is automatically driven based on the environmental map. For example, trees in a vineyard are not replanted for a long period of time once planted, and the trunks of the trees change less in external shape with seasonal changes than the leaves. By using the trunks as SLAM landmarks, an environmental map that can be used throughout the year can be created, and automatic driving operations can be performed throughout the year without recreating the environmental map.

[0098] The operation of the tractor 100A will be specifically described below. The ECU 160 first generates environmental map data showing the distribution of the stems of the tree rows 20 in the entire vineyard or a section of the vineyard using the sensor data output from the LiDAR sensor 110, and records the data in the storage device 150. The generation of the environmental map data is performed by repeating the self-location estimation by the self-location estimation module 162 and the generation of local map data by the map data generation module 164. The map data generation module 164 detects the stems of the tree rows in the environment around the tractor 100A while performing self-location estimation based on the sensor data repeatedly output from the LiDAR sensor 110 while the tractor 100A is moving, and repeats the operation of generating local map data showing the distribution of the detected stems of the tree rows and recording the local map data in the storage device 150. The map data generation module 164 generates the environmental map data by piecing together the local map data generated while the tractor 100A travels through the entire vineyard or a section of the vineyard. This environmental map data is data that records the distribution of trunks of tree rows 20 in the environment in a format that allows them to be distinguished from other objects. Thus, in this embodiment, the trunks of tree rows 20 are used as landmarks for SLAM.

[0099] In an area where multiple tree rows are arranged, one or more supports may be provided. For example, in a vineyard, multiple supports for hedge training are generally provided near the trees. Supports are suitable as landmarks because, like tree trunks, their external shapes do not change much with the seasons. Therefore, the self-location estimation module 162 may further detect supports in the environment around the tractor 100A based on the sensor data, and the map data generation module 164 may generate environmental map data indicating the distribution of the supports in addition to the distribution of the trunks.

[0100] When the environmental map data is generated, the tractor 100A can travel automatically or autonomously. The self-position estimation module 162 of the ECU 160 detects the trunks of the tree rows 20 in the surrounding environment based on the sensor data repeatedly output from the LiDAR sensor 110 while the tractor 100A is traveling, and estimates the position of the tractor 100A by matching the detected trunks of the tree rows 20 with the environmental map data. If the environmental map data includes distribution information of posts, the self-position estimation module 162 may estimate the position of the tractor 100A by matching the trunks and posts of the tree rows detected based on the sensor data with the environmental map data. The drive control ECU 180 is a control device that controls the movement of the tractor 100A according to the position of the tractor 100A estimated by the ECU 160. For example, when the tractor 100A deviates from the planned travel route determined by the ECU 170, the ECU 180 performs control to bring the tractor 100A closer to the planned travel route by adjusting the steering of the tractor 100A. Such steering control may be based on the orientation as well as the position of the tractor 100A.

[0101] The environmental map showing the distribution of the trunks of the tree row 20 can be generated at any time. For example, the environmental map can be generated in a season such as winter when the trees have few leaves. In this case, it is easier to generate an environmental map that more accurately reflects the distribution of the trunks compared to a case where the environmental map is generated in a season such as summer when the trees have many leaves. When the environmental map is generated in winter, the ECU 160 may perform self-location estimation based on the environmental map not only in winter but also in other seasons. In this case, the ECU 160 performs self-location estimation by matching the environmental map data generated during the winter with data obtained by removing parts other than the trunks from the sensor data output from the LiDAR sensor 110.

[0102] The ECU 160 may generate data indicating the distribution of trunks of a row of trees detected based on sensor data repeatedly acquired while the tractor 100A is traveling, and may update the environmental map data using the data. For example, the ECU 160 may update the environmental map data by adding information on trunks detected from newly acquired sensor data to the previously generated environmental map data. Alternatively, the ECU 160 may transmit the data indicating the distribution of the detected trunks of a row of trees to an external device that updates the environmental map data. In this case, the ECU 160 outputs information indicating the estimated position of the tractor 100A by including it in data indicating the distribution of the trunks (for example, local map data). The external device may update the environmental map data using the acquired data, and transmit the updated environmental map data to the tractor 100A. By such an operation, even if the traveling environment has changed since the time the environmental map was created due to the growth of trees, etc., it is possible to create a new environmental map that reflects the change.

[0103] FIG. 12 is a perspective view showing a schematic example of an environment around the tractor 100A. The tractor 100A travels between two adjacent rows of trees in a vineyard. While traveling, the tractor 100A scans the surrounding environment with a laser beam using the LiDAR sensor 110. This acquires data showing the distance distribution of objects present in the environment. The data showing the distance distribution is converted into two-dimensional or three-dimensional point cloud data and recorded. The tractor 100A generates environmental map data showing the distribution of the trunks of the rows of trees in the entire vineyard or in one section of the vineyard by integrating the point cloud data repeatedly acquired while traveling in the vineyard. The integration of the point cloud data includes a process of converting the coordinates of multiple reflection points acquired at different times into coordinates in a world coordinate system fixed to the earth and connecting them together. In this embodiment, the ECU 160 of the tractor 100A generates the environmental map data, but the environmental map data may also be generated by a computer provided outside the tractor 100A.

[0104] FIG. 13A is a diagram showing an example of a travel path of the tractor 100A during the generation of an environmental map and during automatic travel. The tractor 100A travels, for example, between a plurality of tree rows 20 in a vineyard along a path 30 shown by an arrow in FIG. 13A. In the figure, the line segments included in the path 30 are drawn as straight lines, but the path on which the tractor 100A actually travels may include a meandering portion. Here, the plurality of tree rows 20 are ordered from the end as a first tree row 20A, a second tree row 20B, a third tree row 20C, a fourth tree row 20D, and so on. In the example of FIG. 13A, the tractor 100A first travels between the first tree row 20A and the second tree row 20B, and when the travel is completed, it turns and travels between the second tree row 20B and the third tree row 20C in the opposite direction. When the tractor 100A completes traveling between the second row of trees 20B and the third row of trees 20C, it turns further and travels between the third row of trees 20C and the fourth row of trees 20D. Thereafter, the tractor 100A repeats the same operation to travel to the end of the path 30 between the last two rows of trees. If the distance between adjacent rows of trees is short, the tractor 100A may travel between every other row of trees that have not yet been traveled after completing the traveling between the last two rows of trees. This traveling is performed automatically while the tractor 100A estimates its own position using the LiDAR sensor 110. At the timing when the GNSS unit 120 can receive a GNSS signal, the tractor 100A may perform positioning based on the GNSS signal. For example, at the timing when the tractor 100A changes direction on the path 30 shown in FIG. 13A and FIG. 13B, there are no leaves that block the GNSS signal, so positioning based on the GNSS signal is possible.

[0105] <Example of sensor data acquisition> Next, an example of sensor data acquisition by the tractor 100A will be described with reference to Figs. 14A to 17C. When an environmental map has already been constructed, the sensor data is used to estimate the position of the tractor 100A by matching with the environmental map. On the other hand, when an environmental map has not been constructed or when at least a part of the environmental map is to be updated, the environmental map is constructed or updated based on the sensor data. The construction and updating of the environmental map does not need to be performed while the tractor 100A is moving, and can be performed by processing the sensor data acquired by the tractor 100A while it is moving by a computer inside or outside the tractor 100A.

[0106] FIG. 14A is a diagram showing a schematic diagram of a state where a tree 21 and the ground are irradiated with a laser beam emitted from the LiDAR sensor 110 of the tractor 100A. FIG. 14B is a diagram showing a schematic diagram of a state where a laser beam emitted from the LiDAR sensor 110 is irradiated with a laser beam. In these diagrams, a laser beam emitted from the LiDAR sensor 110 in a negative elevation angle direction is represented by a thick dashed line, and a laser beam emitted from the LiDAR sensor 110 in a positive elevation angle direction is represented by a thin dashed line. Of the multiple laser beams emitted from the LiDAR sensor 110 at different elevation angles, a laser beam emitted in a negative elevation angle direction may be reflected by the ground if it does not hit the tree 21. On the other hand, a laser beam emitted in a positive elevation angle direction does not form a reflection point if it does not hit the tree 21, and the reflection point if it hits the tree 21 is often located on the surface of a leaf or branch. As described above, reflection points located on the surface of leaves and branches are not suitable for matching with environmental map data. For this reason, in this embodiment, self-location estimation and construction of an environmental map are performed using reflections from the trunk. Specifically, the trunk may be detected based on the reflection points of laser pulses emitted from a laser light source whose elevation angle is included in a predetermined range among a plurality of laser beams emitted from the LiDAR sensor 110 at different elevation angles. Also, the trunk may be detected based on reflection points whose height is lower than the average height of the trunk. In these cases, after the detection of the trunk, the reflection points located on the ground may be selectively cut from the point cloud of the sensor data.

[0107] 15A and 15B are plan views showing an example of a point cloud of sensor data acquired by one scanning operation by the LiDAR sensor 110 of the tractor 100A. FIG. 15A shows an example of the distribution of reflection points of the laser pulse emitted from the tractor 100A and the LiDAR sensor 110 at a certain time. In this example, it is assumed that the ground is a flat surface. The laser beam reflected by the ground forms reflection points on an arc on the ground. Such reflection points on the ground are formed by a laser beam emitted in a negative elevation angle direction. In FIG. 15A, reflection points are arranged on seven arcs (corresponding to seven layers) with different radii. Each layer is formed by a scan in which a laser beam emitted at a different negative elevation angle rotates in the azimuth angle direction. When this scan is performed, a scan by a laser beam may be performed in the horizontal direction or in the positive elevation angle direction. Note that reflection points arranged on the same layer are formed by a laser beam emitted at the same elevation angle from the same laser light source incident in a pulsed manner on the object surface while rotating. FIG. 15B illustrates a schematic diagram of the tractor 100A and an example of a distribution of reflection points of laser pulses emitted from the LiDAR sensor 110 at a point in time slightly after the point in time of FIG. 15A.

[0108] A part of the laser pulse emitted from the LiDAR sensor 110 is reflected by the surface of the trunk 22 of the tree. A part of the laser pulse reflected by the ground, the trunk 22 of the tree, or another object is detected by the LiDAR sensor 110, and the distance to the reflection point is measured, except when the reflection point is located far away beyond the measurement distance (for example, 50 m, 100 m, or 200 m, etc.). The LiDAR sensor 110 generates and outputs sensor data that associates, for each reflection point, the distance to the reflection point, the direction of the reflection point, the reflected light intensity, and the identification number of the laser light source that emitted the laser beam that formed the reflection point. This sensor data may also include information on the measurement time. The information on the measurement time may be recorded, for example, for each reflection point or for each group of reflection points measured within a certain period of time. The self-localization module 162 of the ECU 160 converts the sensor data output from the LiDAR sensor 110 into point cloud data. The point cloud data is data including information on the three-dimensional coordinates (u, v, w) of each reflection point expressed in a sensor coordinate system fixed to the LiDAR sensor 110. When the LiDAR sensor 110 converts the data of the distance and direction of each reflection point into point cloud data and then outputs the data, the conversion into point cloud data by the self-location estimation module 162 is omitted.

[0109] When constructing the environmental map, while the tractor 100A is traveling, the map data generation module 164 generates local map data from the point cloud data, in which the trunk 22 is recorded in a format that allows the trunk 22 to be distinguished from other parts. While the tractor 100A is traveling, the map data generation module 164 repeats an operation of adding and updating the local map data based on newly acquired sensor data to the local map data generated up to that point. This makes it possible to generate the final environmental map data. Note that the map data generation module 164 may only perform an operation of generating and recording the local map data while the tractor 100A is traveling, and may generate the final environmental map data after the tractor 100A has completed traveling. In this case, the final environmental map data may be generated by an external computer.

[0110] In this embodiment, the LiDAR sensor 110 is attached to the front of the body of the tractor 100A. Therefore, as shown in FIG. 15A and FIG. 15B, the laser pulse is emitted toward the front of the tractor 100A. The LiDAR sensor 110 may also be attached to the rear of the tractor 100A. In that case, since the point cloud data of both the front and rear of the tractor 100A can be acquired at once, map data with a higher point cloud density can be generated. In addition, the accuracy of self-position estimation by matching in automatic driving after the environmental map is generated can be improved. Furthermore, since the point cloud data is obtained from a wide range on the surface of each trunk 22, the thickness of the trunk 22 can be measured with high accuracy from the point cloud data. In this way, the ECU 160 also functions as a recording device that measures the thickness of each trunk of the detected tree row based on the sensor data and records the thickness of each trunk in a storage medium. The storage medium may be a storage medium in the storage device 150 or another storage medium. The thickness of the trunk may be added to the environmental map data as one of the attribute information of each landmark, or may be recorded in other vector format map data.

[0111] Figure 16 shows a diagram of multiple layers (L1, . . . , L k+3 17A to 17C are diagrams each showing an example of a point cloud of the trunk 22 obtained by one scan of a laser beam of 160 nm. k , L k+1 , L k+2 1 is a schematic diagram showing how a point cloud of the trunk 22 is obtained by scanning with a laser beam of 1000 nm. As shown in this figure, point cloud data showing the distribution of the trunk 22 existing in the measurable range can be obtained for each scan. More specifically, when measuring the distance to each reflection point by one scan, each layer may include an area where reflection points with relatively long distances are continuous and an area where reflection points with relatively short distances appear locally.

[0112] Figure 18 shows the structure of multiple layers (L k , L k+1 , L k+218 is a graph showing an example of the relationship between the distance to a reflection point measured in one scan by a laser beam (1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 109, 108, 109, 109, 110, 111, 112, 113, 114, 115, 116, 117 k , L k+1 , L k+2 , . . . The stem, which is located at a relatively long distance, is irradiated with fewer layers L k+1 , L k+2 ,..., laser beams are irradiated onto the stem surface. Therefore, when the point cloud on the stem surface in three-dimensional space is projected onto a plane parallel to the horizontal plane, the density of the point cloud of the stem at a position where the distance is relatively short is relatively high. The stem can be detected based on the measurement results shown in Figure 18. In reality, the ground may be undulating or have weeds, so the distance measurement value is not as simple as shown in the figure.

[0113] By repeating the above scan while the tractor 100A is moving, it is possible to obtain high-density point cloud data from each trunk located around the tractor 100A. Therefore, by utilizing the large amount of sensor data obtained by the moving tractor 100A, it is possible to obtain high-density point cloud data necessary for constructing a highly accurate environmental map showing the distribution of trunks.

[0114] It is also possible to detect the trunk from the relationship between the intensity of the reflected light and the azimuth angle of the reflection point, rather than the distance to the reflection point. The intensity of the reflected light from a laser beam of the same layer increases as the distance to the reflection point decreases.

[0115] As described above, the sensor data (hereinafter also referred to as "scan data") to be compared with the environmental map showing the distribution of trunks may contain many reflection points other than those located on the trunk surface. For this reason, it is effective to select reflection points that are likely to be located on the trunk surface from the acquired scan data. In addition, the scan data may generate dense point cloud data by integrating not only reflection points obtained by one scan but also reflection points obtained by multiple scans obtained consecutively.

[0116] 17A to 17C, for example, a laser beam emitted at a positive elevation angle can irradiate leaves and branches other than the trunks to form a large number of reflection points. The ECU 160 in this embodiment generates data showing the distribution of the trunks 22 by extracting some points having characteristics of the trunks 22 from the point cloud data generated based on the scan data.

[0117] Figure 19 shows an example of a local map created by extracting reflection points located on the surface of the stem from scan data acquired by the tractor 100A at a certain position. In this example, a two-dimensional local map showing the two-dimensional distribution of the stems is created. In such a local map, the coordinates of the stems located around the tractor 100A at that position can be expressed in a uv coordinate system fixed to the tractor 100A.

[0118] Figures 20A and 20B are plan views schematically showing a process of estimating the position of the tractor 100A by matching the local map of FIG. 19 with the constructed environmental map. Each coordinate in the local map is expressed in the sensor coordinate system, and each coordinate in the environmental map is expressed in the world coordinate system. The matching is to optimize the coordinate transformation from the sensor coordinate system to the world coordinate system. By such optimization of the coordinate transformation, the position coordinates and orientation of the sensor in the world coordinate system are determined. In FIGS. 20A and 20B, the trunks in the environmental map are shown in black. FIG. 20A shows a state during the matching. In this state, there is a displacement (error) between the trunk on the local map and the corresponding trunk on the environmental map. FIG. 20B shows a state where the matching is completed. In this state, the displacement (error) between the trunk on the local map and the corresponding trunk on the environmental map is minimized.

[0119] By using the scanned data from which the trunks are extracted in this way and the environmental map showing the distribution of the trunks, highly accurate position estimation is achieved. In the above example, the matching is performed based on a two-dimensional environmental map, but the matching may also be performed based on a three-dimensional environmental map.

[0120] <Example of LiDAR Sensor Arrangement> Next, with reference to FIGS. 21A and 21B, an example of the arrangement of the LiDAR sensor 110 will be described.

[0121] FIG. 21A shows an example in which the LiDAR sensor 110 is attached to the lower front of the vehicle body of the tractor 100A as in the present embodiment. FIG. 21B shows an example in which the LiDAR sensor 110 is attached to the upper front of the cab of the tractor 100A. In these figures, the thick dashed lines indicate the laser beams irradiated on the trunks of the trees 21, and the thin dashed lines indicate the laser beams emitted toward the foliage of the trees or the sky above them.

[0122] In the example of FIG. 21A, the LiDAR sensor 110 is mounted at a position lower than the average height of the trunks of the tree rows. The average height of the trunks of the tree rows is the average value of the heights of the trunks of the trees included in the multiple tree rows that exist on the planned travel route of the tractor 100A. The height of the trunk of the tree is the distance from the ground to the part of the tree that produces the lowest branch. The LiDAR sensor 110 can be disposed at a height of, for example, 10 cm to 150 cm, and in one example, 15 cm to 100 cm, from the ground. By disposing the LiDAR sensor 110 at such a low position, the trunks can be irradiated with more laser beams without being blocked by leaves or branches. This makes it possible to determine the position of the trunk from the sensor data with high accuracy.

[0123] On the other hand, in the example of FIG. 21B, since the LiDAR sensor 110 is arranged at a high position (for example, at a position of about 2 m above the ground), most of the laser beams irradiate the leaves rather than the trunks. For this reason, the arrangement as shown in FIG. 21B is not suitable for the purpose of acquiring point cloud data of the trunks. However, such an arrangement can efficiently acquire distribution information of the leaves of the tree rows, so it is effective for the purpose of actively acquiring distribution information of the leaves. For example, the arrangement as shown in FIG. 21B may be adopted for the purpose of managing the growth status of the trees or determining a route that avoids the leaves of the trees. Note that even in the arrangement as shown in FIG. 21B, the LiDAR sensor 110 can be attached facing downward to irradiate the trunks with more laser beams. For this reason, the arrangement as shown in FIG. 21B may also be adopted for the purpose of acquiring distribution data of the trunks. The LiDAR sensor 110 may be arranged at other positions, not limited to the front lower part of the vehicle body and the front upper part of the cabin. For example, the LiDAR sensor 110 may be arranged at a position lower than the height of the cabin.

[0124] In this embodiment, one LiDAR sensor 110 is used, but the tractor 100A may be equipped with multiple LiDAR sensors. By combining sensor data output from multiple LiDAR sensors, distribution data of the trunks can be obtained more efficiently. For example, the multiple LiDAR sensors may be provided on the left and right sides of the tractor 100A, or may be provided on the front and rear parts. Details of an embodiment of a moving body equipped with multiple LiDAR sensors will be described later.

[0125] The self-position estimation module 162 in the ECU 160 may detect the trunk based on the reflection point of the laser pulse emitted from the laser light source whose elevation angle is included in a predetermined range among the multiple laser light sources in the LiDAR sensor 110. For example, the trunk may be detected based only on the reflection point of the laser pulse emitted in the direction of a negative elevation angle. The elevation angle range of the laser pulse used for trunk detection may be set by the user. For example, the elevation angle range of the laser pulse used for trunk detection may be set by the user by operating the operation terminal 200. By using only the laser pulse emitted in a specific elevation angle range that is likely to irradiate the trunk among the laser pulses emitted from the LiDAR sensor 110, the trunk can be detected more efficiently. In addition, when the ground is undulating and the uv plane in the sensor coordinate system is greatly inclined from the horizontal plane, the elevation angle range of the laser beam may be adaptively selected to extract the reflection point so as to appropriately select the laser pulse reflected by the trunk according to the inclination angle of the sensor coordinate system. The inclination angle of the LiDAR sensor 110 can be obtained by using a signal from the IMU 125.

[0126] <Example of automatic driving operation after constructing an environmental map> Next, an example of an automatic driving operation after the environmental map is created will be described.

[0127] 22 is a diagram showing an example of a functional configuration of the self-position estimation module 162 in the ECU 160. The self-position estimation module 162 executes a computer program (software) stored in the storage device 150 to match the scan data output from the LiDAR sensor 110 with map data and estimate the position and orientation of the tractor 100A.

[0128] The self-location estimation module 162 executes each process of scan data acquisition 162a, map data acquisition 162b, IMU data acquisition 162c, scan data filtering 162d, matching 162e, and vehicle position and orientation determination 162f. The details of these processes will be described below.

[0129] (Scan data acquisition 162a) The self-location estimation module 162 acquires scan data output from the LiDAR sensor 110. The LiDAR sensor 110 outputs scan data at a frequency of, for example, about 1 to 20 times per second. This scan data may include coordinates of multiple points expressed in a sensor coordinate system and time stamp information. Note that, if the scan data includes information on the distance and direction to each point but does not include coordinate information, the self-location estimation module 162 converts the distance and direction information into coordinate information.

[0130] (Map data acquisition 162b) The self-position estimation module 162 acquires the environmental map data stored in the storage device 150. The environmental map data indicates the distribution of trunks of tree rows contained in the environment in which the tractor 100A travels. The environmental map data includes, for example, data in any one of the following formats (1) to (3). (1) Data recorded in a format that allows executives and non-executives to be identified For example, the data corresponds to a point determined to be a leader being assigned the value "1" and a point determined to be an object other than a leader being assigned the value "0." A leader ID for identifying each leader may be included in the environmental map data. (2) Data in which a relatively large weight is assigned to the executives and a relatively small weight is assigned to objects other than the executives For example, data to which a larger value is assigned corresponds to data that is more likely to be estimated as a point on the surface of the trunk. (3) Data that includes information on the distribution of detected stems, but does not include information on the distribution of any or all of the objects other than the stems.

[0131] For example, this data corresponds to data obtained by deleting all points from a point cloud representing an environmental map, leaving only points determined to be the surface of the trunk. All points determined not to be trunks may be left as they are, and some points may be left as they are. For example, in vineyards, posts for hedge training are generally provided around the trunks, and information on points representing such posts may be included in the environmental map.

[0132] Whether or not a point in a point cloud corresponds to a trunk or a pillar can be determined based on, for example, whether or not the distribution of the point and a plurality of points around the point reflects the surface shape of the trunk or pillar (for example, a downwardly convex arc-shaped distribution, etc.). Alternatively, a collection of points that are locally closer to the LiDAR sensor 110 than neighboring points may be extracted from the data of the curved point cloud obtained by each scan, and these points may be determined to represent the trunk or pillar. The data of the curved point cloud obtained by each scan may be classified into a plurality of classes according to the distance from the LiDAR sensor 110, and whether or not each class corresponds to a trunk or a pillar may be determined. In addition, the point cloud may be classified based on not only the distance but also the reflection intensity information. Since the reflection intensity is clearly different between the trunk of a tree and other parts around it, it is effective to classify the point cloud based on the similarity of the reflection intensity and the similarity of the position. For example, a plurality of points whose reflection intensity is within a predetermined range and whose positions are close to each other may be set as candidates for points representing the surface of the trunk. The LiDAR sensor 110 may emit laser beams of multiple different wavelengths, classify the point cloud based on the ratio of reflection intensities for each wavelength, and detect the trunk.

[0133] Machine learning may be used to detect trunks. By using a machine learning algorithm based on a neural network such as deep learning, points corresponding to the surface of tree trunks can be detected from point cloud data with high accuracy. When a machine learning algorithm is used, a trained model for detecting trunks from point cloud data is created (i.e., trained) in advance.

[0134] The recognition may be limited to tree trunks, and may be performed to recognize other objects. For example, the ground, weeds, and tree leaves may be recognized from the point cloud data by pattern recognition or machine learning, and these points may be removed to generate point cloud data mainly including points corresponding to tree trunks. Prior to detecting the trunks, a process may be performed to extract only point clouds whose height from the ground is within a predetermined range (for example, 0.5 m to 1.5 m). By detecting only point clouds that are within such a specific coordinate range, the time required for detection can be reduced. The height from the ground is calculated by subtracting the Z coordinate of the ground from the Z coordinate of each point. The Z coordinate of the ground may be determined, for example, by referring to a digital elevation model (DEM). The Z coordinate of the ground may be determined from a point cloud indicating the ground.

[0135] FIG. 23 is a diagram showing a schematic example of the format of the environmental map data. The environmental map data in this example includes information on the number of points, and information on the ID, classification, trunk ID, X coordinate, Y coordinate, Z coordinate, and reflection intensity for each point. If the number of points is n, n sets of data on the point ID, classification, trunk ID, X coordinate, Y coordinate, Z coordinate, and reflection intensity are recorded. The classification indicates what the point represents, and includes an identifier indicating, for example, a tree trunk, a hedge post, the ground, weeds, or a tree leaf. The classification may be a binary number indicating whether or not the point is a tree trunk, or a number indicating the probability that the point is a tree trunk. The trunk ID is a number for identifying the trunk when the point represents a trunk. The trunk ID may not be included in the environmental map data. In addition to the trunk ID, identifiers of other objects (for example, leaves, branches, the ground, weeds, etc.) may be included. The X coordinate, Y coordinate, and Z coordinate are the coordinates of the point in the world coordinate system. The reflection intensity indicates the reflected light intensity from the point. The information on the reflection intensity may not be included in the environment map data. The environment map data may include information such as a header indicating a data format in addition to the information shown in FIG. 23. If the vineyard in which the tractor 100A travels is large, environment map data may be generated for each section of the vineyard (for example, for each section of 100 m×100 m). In that case, the environment map data may also include ID information for identifying the section. Note that the environment map data may record two-dimensional coordinates instead of recording three-dimensional coordinates of each point. For example, data including coordinates of a group of points on a plane at a specific height parallel to the horizontal plane may be generated as the environment map data. In that case, matching between the scan data and the environment map data is performed with respect to the two-dimensional coordinates.

[0136] (IMU data acquisition 162c) The self-position estimation module 162 acquires IMU data output from the IMU 125. The IMU data may include information such as the acceleration, speed, displacement, attitude, and measurement time (timestamp) of the tractor 100A. The IMU data is output at a frequency of, for example, about several tens to several thousands of times per second. This output cycle is generally shorter than the output cycle of the scan data by the LiDAR sensor 110.

[0137] Fig. 24 is a diagram showing an example of the temporal relationship between IMU data and scan data. In this example, IMU data is output at a cycle of 1 millisecond (ms), and scan data is output at a cycle of 10 ms. If the time required for one scan is, for example, 100 ms, in the example of Fig. 24, scan data is output for every 1 / 10th of a scan. The scan data includes information on the distance and direction or coordinates of each measured point and the measurement time (timestamp).

[0138] The self-localization module 162 references the timestamp of the acquired scan data to obtain the IMU data generated during the corresponding time.

[0139] (Scan Data Filtering 162d) The self-localization module 162 filters the acquired scan data to reduce the number of points used for matching, and further removes parts that are not necessary for matching. For example, points that are determined not to correspond to tree trunks or hedge posts, such as ground, weeds, tree leaves, and obstacles, can be removed.

[0140] (Matching 162e) The self-localization module 162 matches the filtered scan data with the map data. The matching can be performed using any matching algorithm, such as Normal Distribution Transform (NDT) or Iterative Closest Point (ICP). The matching determines the position and orientation of the LiDAR sensor 110.

[0141] (Vehicle position and orientation determination 162f) The self-position estimation module 162 determines the position and orientation of the tractor 100A based on the matching result, and outputs data indicating the position and orientation. The data is sent to the ECU 180 for drive control, and is used to control the drive unit 140.

[0142] Next, the self-location estimation operation in this embodiment will be described in more detail with reference to FIG.

[0143] Fig. 25 is a flowchart showing the self-location estimation operation by the self-location estimation module 162. The self-location estimation module 162 estimates the position and orientation, i.e., the pose, of the tractor 100A by executing the operations of steps S101 to S109 shown in Fig. 25. The operation of each step will be described below. The operation shown in Fig. 25 can be started by, for example, a user's operation using the operation terminal 200.

[0144] (Step S101) The self-position estimation module 162 reads the environmental map data from the storage device 150. When different environmental map data is recorded for each section of the vineyard to be traveled, the environmental map data corresponding to the current location is read. The environmental map data corresponding to the current location may be specified, for example, by the user operating the operation terminal 200. Alternatively, when a GNSS signal can be received at that location, the current location may be identified based on the GNSS signal received by the GNSS unit 120, and the corresponding environmental map data may be selected and read. As in this example, the processing can be speeded up by reading only a portion of the environmental map data corresponding to the position of the tractor 100A. Note that all the environmental map data may be read at once in this step.

[0145] (Step S102) The self-position estimation module 162 sets a start point of the position estimation. The start point of the position estimation is the current position of the tractor 100A at that time, and can be set by, for example, the user specifying a specific point from a map displayed on the operation terminal 200. Alternatively, if a GNSS signal can be received at that point, the start point may be set based on the GNSS signal received by the GNSS unit 120.

[0146] (Step S103) When the start position is set, the operation of the LiDAR sensor 110 is started. The self-location estimation module 162 reads scan data output from the LiDAR sensor 110. The LiDAR sensor 110 outputs scan data at a predetermined cycle (for example, 5 milliseconds or more and 1 second or less). The scan data includes point cloud data within a range of, for example, several degrees to 360 degrees for each layer. For example, the self-location estimation module 162 reads scan data every time the scan data is output from the LiDAR sensor 110. Alternatively, the self-location estimation module 162 may read the scan data collectively every time a predetermined number of scan data are output.

[0147] (Step S104) The self-location estimation module 162 refers to the timestamp included in the scan data and reads the IMU data corresponding to that scan data.

[0148] (Step S105) The self-position estimation module 162 sets an initial position for matching based on the read IMU data. The initial position for matching is the estimated position of the tractor 100A at the current time indicated by the IMU data. By starting matching from this initial position, the time until convergence can be shortened. Instead of using the IMU data, the initial value for matching may be determined by linear interpolation based on the difference between the estimated values ​​of the position and orientation for the past two scans, for example.

[0149] (Step S106) The self-localization module 162 filters the acquired scan data to reduce the number of points used for matching, and further detects the trunk by the above-mentioned method, and removes at least a part of unnecessary parts other than the trunk from the point cloud.

[0150] Here, an example of the operation of filtering scan data will be described with reference to FIG. 26. FIG. 26 shows a schematic diagram of a part of a point cloud included in scan data. In FIG. 26, white circles are points included in the point cloud. The self-localization module 162 divides the three-dimensional space in the sensor coordinate system into a plurality of voxels of a certain size. Then, when each voxel includes a plurality of points, these points are replaced with one point located at the center of gravity. In FIG. 26, the replaced center of gravity point is represented by a black circle. By performing such downsampling, the number of points of the scan data can be reduced, and matching can be accelerated. In this embodiment, after such downsampling, the detection of the trunk is performed, but the detection of the trunk may be performed before the downsampling. If the data size of the point cloud is not a problem, the downsampling process may be omitted.

[0151] (Step S107) The self-localization module 162 matches the filtered scan data with the environmental map data to estimate the pose of the LiDAR sensor 110. Specifically, the pose of the LiDAR sensor 110 is determined by determining a coordinate transformation from the sensor coordinate system to the world coordinate system using a method such as the NDT method or the ICP method.

[0152] (Step S108) The self-position estimation module 162 calculates the pose of the tractor 100A based on the pose of the LiDAR sensor 110, and outputs the result. The pose of the tractor 100A is expressed in the world coordinate system by the coordinates (x, y, z) of the representative point (origin) of the tractor 100A and the attitude (θ R ,θ P ,θ Y) data. In addition, when the environmental map and the scan data represent a point group in a two-dimensional space (plane) and matching is performed in the two-dimensional space, the output pose data may include values ​​of two-dimensional coordinates (x, y) and orientation (θ). Only the position may be estimated by matching, and the attitude information indicated by the IMU data may be used as is for the attitude. If the coordinate system of the tractor 100A coincides with the sensor coordinate system, step S108 may be omitted.

[0153] (Step S109) The self-position estimation module 162 judges whether or not a command to end the operation has been issued. The command to end the operation is issued, for example, when the user instructs the automatic driving mode to be stopped using the operation terminal 200, or when the tractor 100A reaches the destination. If the command to end the operation has not been issued, the process returns to step S103, and the same operation is performed for the next scan data. If the command to end the operation has been issued, the process ends.

[0154] Next, an example of the operation of the drive control ECU 180 will be described.

[0155] FIG. 27 is a flowchart showing an example of the operation of the ECU 180 executed after the pose of the tractor 100A is estimated. When the pose of the tractor 100A is output in step S108 shown in FIG. 25, the ECU 180 acquires data of the pose (step S121). Next, the ECU 180 calculates the deviation between the position of the tractor 100A indicated by the pose data and a planned travel route previously determined by the ECU 170 (step S122). The planned travel route can be set, for example, by a user operating the operation terminal 200. The deviation represents the distance between the estimated position of the tractor 100A at that time and the planned travel route. The ECU 180 determines whether the deviation of the calculated position exceeds a preset threshold value (step S123). If the deviation exceeds the threshold value, the ECU 180 changes the drive value (for example, steering angle) of the drive unit 140 so as to reduce the deviation. In addition to the steering angle, the speed may be changed. If the deviation does not exceed the threshold in step S123, step S124 is omitted. In the following step S125, the ECU 180 judges whether or not an operation end command has been received. As described above, an operation end command is issued, for example, when the user instructs the automatic driving mode to be stopped using the operation terminal 200 or when the tractor 100A reaches the destination. If an operation end command has not been issued, the process returns to step S121, and the same operation is executed based on the pose of the tractor 100A estimated based on the next scan data. If an operation end command has been issued, the process ends.

[0156] 27, ECU 180 controls drive unit 140 based only on the deviation between the estimated position and the planned travel route, but ECU 180 may also control drive unit 140 by further considering the deviation in orientation. For example, when an orientation deviation, which is the angular difference between the estimated orientation of tractor 100A indicated by the acquired pose data and the direction of the planned travel route, exceeds a preset threshold, ECU 180 may change the drive value (e.g., steering angle) of drive unit 140 according to the deviation.

[0157] An example of steering control by the ECU 180 will be described below with reference to Figs. 28A to 28D.

[0158] Fig. 28A is a diagram showing an example of a tractor 100A traveling along a planned travel route P. Fig. 28B is a diagram showing an example of a tractor 100A in a position shifted to the right from the planned travel route P. Fig. 28C is a diagram showing an example of a tractor 100A in a position shifted to the left from the planned travel route P. Fig. 28D is a diagram showing an example of a tractor 100A facing in an inclined direction with respect to the planned travel route P.

[0159] As shown in FIG. 28A, when the position and orientation of the tractor 100A do not deviate from the planned travel route P, the ECU 180 maintains the steering angle and speed of the tractor 100A without changing them.

[0160] As shown in Fig. 28B, when the position of the tractor 100A shifts to the right from the planned travel route P, the ECU 180 changes the steering angle by changing the rotation angle of the steering motor included in the drive unit 140 so that the travel direction of the tractor 100A shifts to the left. At this time, the speed may be changed in addition to the steering angle. The amount of change in the steering angle may be adjusted according to the magnitude of the position deviation Δx, for example.

[0161] As shown in Fig. 28C, when the position of the tractor 100A shifts to the left from the planned travel route P, the ECU 180 changes the rotation angle of the steering motor to change the steering angle so that the travel direction of the tractor 100A shifts to the right. In this case, the speed may also be changed in addition to the steering angle. The amount of change in the steering angle may be adjusted according to the magnitude of the position deviation Δx, for example.

[0162] As shown in FIG. 28D, when the position of the tractor 100A is not far from the planned travel route P but the direction is different from the direction of the planned travel route P, the ECU 180 changes the steering angle so that the azimuth deviation Δθ is reduced. In this case, the speed may also be changed in addition to the steering angle. The amount of change in the steering angle may be adjusted, for example, according to the magnitude of the position deviation Δx and the azimuth deviation Δθ. For example, the smaller the absolute value of the position deviation Δx, the larger the amount of change in the steering angle according to the azimuth deviation Δθ may be. When the absolute value of the position deviation Δx is large, the steering angle is changed significantly to return to the planned travel route P, so that the absolute value of the azimuth deviation Δθ inevitably becomes large. Conversely, when the absolute value of the position deviation Δx is small, it is necessary to bring the azimuth deviation Δθ closer to zero, so it is appropriate to relatively increase the weight of the azimuth deviation Δθ when determining the amount of change in the steering angle.

[0163] A control technique such as PID control or MPC (model predictive control) control may be applied to the steering control and speed control of the tractor 100A. By applying these control techniques, the control for moving the tractor 100A closer to the planned travel path P can be made smooth.

[0164] If an obstacle is detected by one or more obstacle sensors 130 while traveling, the ECU 180 controls the drive unit 140 to avoid the obstacle. If the obstacle cannot be avoided, the ECU 180 stops the tractor 100A. If an obstacle is detected, the ECU 180 may stop the tractor 100A regardless of whether the obstacle can be avoided or not.

[0165] <Example of how to determine the planned driving route> Next, an example of a method for determining a planned travel route of the tractor 100A by the ECU 170 will be described.

[0166] After the environmental map is created, the ECU 170 determines a planned travel route for the tractor 100A. The planned travel route may be automatically determined by the ECU 170 based on the environmental map, or may be set by the user operating the operation terminal 200.

[0167] FIG. 29A is a diagram showing an example of two-dimensional data showing the distribution of trunks in a row of trees, generated based on an environmental map. The map data generation module 164 in the ECU 160 may generate two-dimensional data roughly showing the distribution of trunks 22 as shown in FIG. 29A, in addition to the environmental map for matching. In this two-dimensional data, the trunks 22 of each tree are represented by circles of a certain size. Such two-dimensional data showing the distribution of trunks may be referred to as "stem distribution data". The trunk distribution data includes, for example, information on the position coordinates (x, y) of the center of each trunk. The trunk distribution data may include information such as an identification number (stem ID) and the thickness of each trunk in addition to the position coordinates. By including such information, the growth status of the trunks can also be managed. The growth status of the trunks or leaves of the trees can be managed by, for example, a computer in the agricultural management system. As in this example, the growth status of the trees can be managed by data different from the environmental map. A user may be allowed to input information showing the arrangement relationship of a plurality of rows of trees, such as the distance between trees or the position coordinates of the trunks, using a human-machine interface (HMI) such as the operation terminal 200. The entered information is recorded in storage device 150 or other storage medium.

[0168] 29B is a diagram for explaining an example of a method for determining the planned travel route 30 based on trunk distribution data. In this example, the ECU 170 determines a curve connecting the trunks 22 of each tree row, and determines a curve or broken line passing through the center of two adjacent tree rows as the planned travel route 30. The center of the tractor 100A does not necessarily have to be located at the center of the two adjacent trees, and the planned travel route 30 may be shifted from the center between the two adjacent trees. The drive control ECU 180 performs steering control of the tractor 100A based on the determined planned travel route 30.

[0169] Fig. 30 is a diagram showing another example of trunk distribution data for route setting. In this example, the driving environment of the tractor 100A is divided into a plurality of cells and managed. Each cell may be, for example, a square with a side length close to the average diameter of the trunks 22. The cells are classified into the following four types (1) to (4). (1) Cell containing Executive 22 (2) A cell adjacent to the cell containing Executive 22 (3) A cell located between two adjacent tree rows that does not fall under (1) or (2). (4) Other cells

[0170] In FIG. 30, cells (1) to (4) are expressed with different shading. The range of cell (2) adjacent to the cell including the trunk 22 may be determined based on point cloud data indicating the leaves of the tree row acquired by the LiDAR sensor 110. In this case, the map data generation module 164 of the ECU 160 generates grid-like data indicating the two-dimensional distribution of the trunks and leaves of the tree row separately from the environmental map data for matching. The ECU 170 in this example determines the route of the tractor 100A based on the grid-like data as shown in FIG. 30. For example, the ECU 170 sets the above-mentioned cell (3) located between two adjacent tree rows as the drivable area. The ECU 180 controls the travel of the tractor 100A so that the tractor 100A passes through the vicinity of the center of the drivable area.

[0171] <Steering control when generating map data> As described above, when the tractor 100A in this embodiment creates an environmental map by automatic traveling, it collects local map data indicating the distribution of the trunks of the tree rows while performing self-location estimation and steering control. At this time, the ECU 160 may further detect the leaves of the tree rows in the environment around the tractor 100A based on the sensor data repeatedly output from the LiDAR sensor 110 while the tractor 100A is moving, and further generate data indicating the distribution of the leaves of the detected tree rows. In this case, the ECU 180 can perform steering control of the tractor 100A based on the respective distributions of the trunks and leaves of the detected tree rows. For example, the ECU 180 performs steering control of the tractor 100A so as to move the tractor 100A along a path that passes between two adjacent tree rows among the detected tree rows and reduces contact with the leaves of the tree rows.

[0172] In this operation, the ECU 170 determines a route that passes between the trunks of two adjacent tree rows from the distribution of the trunks of the tree rows detected based on the sensor data from the LiDAR sensor 110. For example, the ECU 170 determines a route that passes through the center of the positions of the pairs of trunks of the two adjacent tree rows. The ECU 180 performs steering control of the tractor 100A so as to move the tractor 100A along the determined route. This allows automatic driving to be performed while collecting local map data.

[0173] In the environment in which the tractor 100A travels, tree rows are not necessarily arranged in a straight line. There are also cases where tree rows are arranged in a curved line. In such cases, when the tractor 100A travels automatically while collecting map data, tree rows may be detected only on one side, either the left or right. In such cases, since the pair of left and right tree rows cannot be identified, it is difficult to continue automatic travel using the above method.

[0174] Therefore, when the rows of trees are arranged in a curved line, the following control may be performed. When the rows of trees are arranged in a curved line, a smaller number of trunks are detected in one of two adjacent rows of trees than in the other of the two rows of trees. In such a case, the ECU 170 estimates the distribution of hidden trunks in one of the two rows of trees based on the distribution of the detected trunks, and determines the route of the tractor 100A based on the estimated distribution. The ECU 180 performs steering control of the tractor 100A so that the tractor 100A travels along the determined route.

[0175] FIG. 31A is a diagram showing an example of an environment in which tree rows are arranged in a curved line. The cylinders in the diagram represent the trunks of the trees, the thick dashed lines represent the general arrangement of the tree rows, and the dotted lines represent the scanning of multiple layers of laser beams emitted from the LiDAR sensor 110. FIG. 31B is a diagram showing a point cloud that can be observed in one scan in the environment shown in FIG. 31A. In this example, of two adjacent tree rows, three trunks 22R1, 22R2, and 22R3 are detected for the right tree row, and only one trunk 22L1 is detected for the left tree row. For the left tree row, the laser beam is blocked by the trunk 22L1 of the tree in front, and the trunk of the tree behind cannot be detected.

[0176] In such a case, the ECU 170 estimates the position of the hidden tree trunk on the left side from the arrangement of the tree row on the right side where a relatively large number of trees are detected, and determines the driving route based on the estimated trunk position. For example, the position of the hidden trunk is estimated by applying the relative positional relationship of the two adjacent trunks 22R1 and 22L1 in the front, the positions of which are identified based on the scan data, to the two adjacent trunks in the rear.

[0177] FIG. 31C is a diagram for explaining the operation of determining a travel route by estimating the position of a hidden trunk. In this example, the position of the hidden trunk is estimated based on trunk distribution data showing a two-dimensional distribution of trunks of a tree row acquired based on scan data. In FIG. 31C, the position of the trunk of the tree detected based on the scan data is indicated by a solid circle, and the position of the trunk of the tree that has not yet been detected is indicated by a dashed circle. In this example, the ECU 170 first determines a vector V1 from the position of the trunk 22R1 to the position of the trunk 22L1. Next, the position of the rear trunk 22R2 is set as the starting point, and a position displaced by the vector V1 from that position is determined as the estimated position of the hidden trunk. In FIG. 31C, the estimated position of the hidden trunk is indicated by a cross mark. The ECU 170 determines, for example, a route passing through the midpoint C1 between the position and the position of the trunk 22R2 as the travel route of the tractor 100A.

[0178] When the tractor 100A travels along the route determined in this manner, the rear trunk 22L2 hidden by the trunk 22L1 is eventually detected from the scan data. Assume that the further rear trunk 22L3 has not yet been detected. If the position of the trunk 22L2 is different from the estimated position (marked with an x), the ECU 170 identifies the right trunk (trunk 22R3 in the example of FIG. 31C) that pairs with the trunk 22L2, and determines a vector V2 from the trunk 22R3 to the trunk 22L2. Next, the position of the further rear trunk 22R4 is set as the starting point, and the position displaced by the vector V2 from that position is determined as the position of the hidden trunk 22L3. By repeating the above operations, the position of the hidden trunk can be estimated, an appropriate route can be set, and automatic steering can be performed along the route.

[0179] Fig. 31D is a diagram showing an example of grid data showing a two-dimensional distribution of trunks when a row of trees is arranged in a curved line. ECU 160 may generate grid data as shown in Fig. 31D in the process of collecting data for constructing an environmental map. In this case, ECU 170 may set a route based on such grid data. In this example, the position of the hidden trunks may be estimated in units of the size of a cell.

[0180] By performing the above-described operations, it is possible to perform relatively smooth steering control while creating an environmental map.

[0181] <Obstacle avoidance action> Next, the obstacle avoidance operation in this embodiment will be described.

[0182] FIG. 32 is a diagram showing a schematic example of a situation in which an obstacle 40 and a person 50 are present in the travel environment of the tractor 100A. The tractor 100A is equipped with one or more obstacle sensors 130. When the obstacle sensor 130 detects an obstacle 40 (e.g., a basket) or a person 50 during travel, the ECU 180 performs steering control to avoid the obstacle 40 or the person 50. When collision with the obstacle 40, the person 50, the branch, the trunk 22, or the like cannot be avoided even if the obstacle is avoided, the ECU 180 stops the tractor 100A. Note that when an obstacle is detected, the ECU 180 may stop the tractor 100A even if the obstacle can be avoided. Similar detection may be performed by the LiDAR sensor 110 instead of the obstacle sensor 130.

[0183] In the example shown in FIG. 32, a person 50 is present near the trunk 22 of a tree. When matching is performed including the leaves of a tree, the person 50 or other obstacles may be hidden by the leaves, making it difficult to distinguish between the leaves and other objects. In particular, when the person 50 is standing in the leaves near the trunk 22, it is difficult to distinguish. In this embodiment, matching is performed using an environmental map showing the distribution of the trunks 22, so that it is easy to distinguish between the trunks and other objects. Objects other than the trunks detected by matching the scan data with the environmental map can be easily detected as people or obstacles. In this way, the LiDAR sensor 110 can also be used as an obstacle sensor. When the tractor 100A is equipped with multiple LiDAR sensors 110 at different locations on the tractor 100A, the obstacle sensor 130 may be omitted.

[0184] <Self-location estimation based on unique combinations of trunk intervals> In the above embodiment, the interval between the trunks of trees is assumed to be approximately constant, but in reality, the interval between the trunks of trees may vary depending on the location. Therefore, when ECU 160 estimates its own position, it may detect the trunks of multiple trees having a unique combination of trunk intervals from the sensor data, and estimate the position of the moving body by matching the trunks of multiple trees having the unique combination of trunk intervals with the trunks of multiple trees having the unique combination of trunk intervals extracted from the environmental map data. This operation will be described below with reference to Figures 33A and 33B.

[0185] FIG. 33A is a diagram showing an example of the arrangement of the trunks 22 of a row of trees detected by the tractor 100A. FIG. 33B is a diagram showing an example of the trunk distribution shown in the environmental map data. In this example, the three trunks surrounded by the dotted frame have a unique combination of trunk intervals of 1.2 m and 1.5 m. In such a case, when the ECU 160 performs self-location estimation, it calculates the intervals between the trunks 22 detected from the acquired scan data and determines the part having the unique combination of trunk intervals. Then, it searches and identifies the part having the determined combination of trunk intervals from the environmental map shown in FIG. 33B. The ECU 160 can estimate the current position and orientation of the tractor 100A by matching the parts having the unique trunk intervals. By such an operation, when the row of trees includes a part having a unique combination of trunk intervals, it is possible to speed up matching and improve the accuracy of position estimation. Information indicating the combination of trunk intervals in the row of trees may be recorded in advance in a storage medium. Such information may be automatically recorded by the ECU 160 based on the environmental map data, for example, or may be recorded by the user operating the operation terminal 200.

[0186] In this embodiment, the environmental map data generated by the tractor 100A can be provided to other tractors or other mobile bodies traveling in the same environment. For example, in a large orchard, when multiple mobile bodies are working, it is sufficient for one mobile body or computer to create an environmental map, and it is efficient to share the environmental map among multiple mobile bodies. Local maps for constructing an environmental map in one environment may be generated by multiple tractors or other mobile bodies. In this case, a computer is provided in the system that compiles the local maps generated by the multiple mobile bodies to generate one environmental map. The computer may distribute the generated environmental map to multiple mobile bodies via a wired or wireless network or a storage medium.

[0187] In a system in which multiple mobile objects move and work within the same environment, the positional relationship between the sensor coordinate system and the mobile object coordinate system may differ for each mobile object. Even for the same model, an error may occur in the conversion from the sensor coordinate system to the mobile object coordinate system due to an error in the mounting position of the sensor. In order to reduce the effect of such an error, each mobile object performs a calibration to determine parameters for the coordinate conversion from the sensor coordinate system to the mobile object coordinate system by a test run before starting normal operation.

[0188] The technology of this embodiment is not limited to tractors that run through orchards such as vineyards, but can be applied to any moving object (e.g., a moving robot, a drone, etc.) that is used in an environment where multiple rows of trees exist, such as a forest. This point is the same for the following embodiments.

[0189] [3-2.Embodiment 2] Next, a moving body according to a second embodiment of the present disclosure will be described.

[0190] The mobile body in this embodiment includes at least two LiDAR sensors, and performs self-location estimation and map data generation based on sensor data output from the LiDAR sensors. Each of the at least two LiDAR sensors outputs two-dimensional or three-dimensional point cloud data or distance distribution data indicating the distribution of objects in the environment around the mobile body. As in the first embodiment, the mobile body includes a storage device that stores environmental map data indicating the distribution of trunks of multiple tree rows, a self-location estimation device, and a control device that controls the movement of the mobile body according to the position of the mobile body estimated by the self-location estimation device. The self-location estimation device detects trunks of tree rows in the environment around the mobile body based on point cloud data repeatedly output from at least two sensors while the mobile body is moving, and estimates the position of the mobile body by matching the detected trunks of the tree rows with the environmental map data. The mobile body further includes a data generation device that generates environmental map data or local map data for constructing the environmental map data. The data generating device detects trunks of tree rows in the environment surrounding the mobile body while performing self-location estimation based on point cloud data repeatedly output from at least two sensors while the mobile body is moving, generates local map data for generating environmental map data indicating the distribution of the trunks of the detected tree rows, and records the local map data in the storage device. The data generating device can also generate environmental map data in a world coordinate system and record the environmental map data in the storage device by integrating the local map data repeatedly generated while the mobile body is moving.

[0191] The configuration and operation of this embodiment will be described below using as an example a case where the moving object is a tractor traveling in an orchard such as a vineyard. In the following description, differences from the first embodiment will be mainly described, and descriptions of overlapping points will be omitted.

[0192] Fig. 34 is a schematic diagram of the tractor 100B in this embodiment as viewed from the side. This tractor 100B is equipped with a first LiDAR sensor 110A at the front lower part of the vehicle body, and a second LiDAR sensor 110B at the upper front part of the cabin. Other points are the same as the configuration shown in Fig. 7. Fig. 34 does not show the coupling device 108 and the work implement 300 shown in Fig. 7, but these can also be used in this embodiment.

[0193] Each of the first LiDAR sensor 110A and the second LiDAR sensor 110B in this embodiment is a two-dimensional LiDAR sensor. In other words, each of the LiDAR sensors 110A and 110B is equivalent to the LiDAR sensor 110 in the example shown in FIG. 9A when N=1, that is, emitting a laser beam only in the direction of one elevation angle. Each of the LiDAR sensors 110A and 110B outputs data indicating the distance and direction to each reflection point, or two-dimensional point cloud data. The two-dimensional point cloud data includes information on two-dimensional coordinates of a plurality of reflection points expressed in the sensor coordinate system of each of the LiDAR sensors 110A and 110B. By using two two-dimensional LiDARs, costs can be reduced compared to the case of using a three-dimensional LiDAR.

[0194] The first LiDAR sensor 110A is mounted at a position lower than the average height of the trunks of the tree rows in the environment in which the tractor 100B travels. The second LiDAR sensor 110B is mounted at a position higher than the average height of the trunks of the tree rows. The first LiDAR sensor 110A may be located at a height of, for example, 10 cm to 150 cm, and in one example, 15 cm to 100 cm, from the ground. The second LiDAR sensor 110B may be located at a position higher than 150 cm from the ground (for example, about 2 m from the ground).

[0195] The first LiDAR sensor 110A is disposed to emit a laser pulse toward the front of the tractor 100B, similar to the LiDAR sensor 110 in the first embodiment. The first LiDAR sensor 110A in the example of FIG. 34 emits a laser pulse substantially parallel to the ground. When the tractor 100B is located on flat ground, the angle between the emission direction of the laser pulse emitted from the first LiDAR sensor 110A and the ground can be set within a range of, for example, ±20 degrees, and in one example, within a range of ±10 degrees.

[0196] On the other hand, the second LiDAR sensor 110B is disposed so as to emit a laser pulse diagonally downward in front of the tractor 100B. In the example of Fig. 34, the second LiDAR sensor 110B emits a laser pulse in a direction inclined at about 25 degrees from the ground. When the tractor 100B is located on flat ground, the angle between the emission direction of the laser pulse emitted from the second LiDAR sensor 110B and the ground can be set, for example, in the range of 10 degrees to 45 degrees.

[0197] Fig. 35 is a block diagram showing an example of a schematic configuration of a tractor 100B in this embodiment. The configuration example shown in Fig. 35 differs from the configuration shown in Fig. 8 in that the tractor 100B includes two LiDAR sensors 110A and 110B.

[0198] The ECU 160 functions as the self-position estimation device and data generation device described above. The ECU 160 acquires an input point cloud of multiple scans including the latest scan from the point cloud data repeatedly output from the two LiDAR sensors 110A and 110B, and matches the input point cloud of the multiple scans with the environmental map data. This makes it possible to estimate the position and orientation of the traveling tractor 100B. Note that the ECU 160 may estimate only the position of the tractor 100B by matching, and determine the orientation based on a signal output from the IMU 125.

[0199] In the process of constructing the environmental map, the ECU 160 detects trunks of tree rows in the environment around the moving body while performing self-location estimation based on the scan data repeatedly output from the two LiDAR sensors 110A and 110B while the tractor 100B is moving, generates local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows, and records the local map data in the storage device 150. While this operation is being performed, the ECU 170 sets a target route that passes between the trunks of the tree rows detected based on the scan data. The ECU 180 causes the tractor 100B to travel along the target route by controlling the drive device 140 according to the set target route. The ECU 170 may detect an obstacle based on a signal output from the obstacle sensor 130 or scan data output from one or both of the LiDAR sensors 110A and 110B, and set a target route to avoid contact with the obstacle. The ECU 160 generates environmental map data by piecing together the repeatedly generated local map data, and records the environmental map data in the storage device 150. The detection of tree trunks may be performed at the stage of generating the environmental map data, rather than at the stage of generating the local map data. Also, the process of generating the environmental map data based on the local map data may be executed by an external computer.

[0200] During the operation of automatic driving or automatic steering after the environmental map is constructed, the ECU 160 detects the trunks of the tree rows based on the scan data repeatedly output from the LiDAR sensors 110A and 110B, and performs self-location estimation by matching the detected trunks with the trunks indicated by the environmental map data. The ECU 180 causes the tractor 100B to travel along a planned travel route determined in advance by the ECU 170. In this case, too, the ECU 170 may detect an obstacle based on a signal output from the obstacle sensor 130 or scan data output from one or both of the LiDAR sensors 110A and 110B while the tractor 100B is traveling, and change the planned travel route to avoid contact with the obstacle.

[0201] 36A to 36C are diagrams that show a schematic view of a tractor 100B traveling near a tree 21. The tractor 100B approaches the tree 21 in the order of FIG. 36A, FIG. 36B, and FIG. 36C. In these diagrams, the broken lines represent the laser beams emitted from the LiDAR sensors 110A and 110B. As shown in the figures, the position of the irradiation point of the trunk 22 of the tree 21 irradiated by the laser beam emitted from the second LiDAR sensor 110B changes in the height direction depending on the distance between the tractor 100B and the tree 21. On the other hand, the change in the position of the irradiation point of the trunk 22 of the tree 21 irradiated by the laser beam emitted from the first LiDAR sensor 110A is relatively small. By combining the point cloud data acquired by multiple scans of these laser beams, point cloud data showing the three-dimensional distribution of reflection points on the surface of the trunk 22 can be acquired, similar to the case of using a three-dimensional LiDAR.

[0202] The ECU 160 in this embodiment may accumulate scan data for a predetermined number of scans (e.g., three or five scans) output from each of the LiDAR sensors 110A and 110B, and perform self-location estimation by matching the accumulated scan data with environmental map data. In this way, self-location estimation with high accuracy similar to that achieved by using a three-dimensional LiDAR can be achieved.

[0203] While the tractor 100B is traveling to acquire map data, the ECU 170 determines a local target route for the tractor 100B based on several scans of scan data output from one or both of the LiDAR sensors 110A and 110B. For example, the ECU 170 can determine the center position between two adjacent trees from the arrangement of point clouds indicated by several scans of scan data output from one or both of the LiDAR sensors 110A and 110B, and determine the local target route for the tractor 100B so as to pass through the center position. By setting the route in this way, the tractor 100B can travel autonomously while maintaining a roughly equal distance between the left and right rows of trees.

[0204] The detection of the trunk may be performed based only on the scan data output from the first LiDAR sensor 110A. The scan data output from the first LiDAR sensor 110A, which is located at a relatively low position, often contains a lot of point cloud information representing the trunk's surface. Therefore, even if only the scan data from the first LiDAR sensor 110A is used, the distribution information of the trunk can be obtained.

[0205] The second LiDAR sensor 110B in this embodiment is disposed at a relatively high position, and is therefore suitable for detecting leaves in a row of trees. Therefore, the ECU 160 may generate data indicating the distribution of leaves in a row of trees based on the point cloud data acquired by the second LiDAR sensor 110B, and record the data in the storage device 150 or another storage medium. Such data may be used for managing the growth conditions of the trees (e.g., canopy management).

[0206] While the tractor 100B is traveling, it may detect obstacles that are located at a relatively high position, such as the leaves of a row of trees, based on the point cloud data acquired by the second LiDAR sensor 110B, and perform an operation to avoid or stop the tractor 100B. If the obstacle sensor 130 is not provided at the front upper part of the tractor 100B, it would be difficult to detect and avoid the leaves or vines of trees that extend above the path between the rows of trees unless the second LiDAR sensor 110B is provided. By providing the second LiDAR sensor 110B, it becomes easier to detect those leaves or vines.

[0207] According to this embodiment, by using two 2D LiDARs, it is possible to realize the same operation as in the first embodiment at a low cost. It is to be noted that the number of 2D LiDARs is not limited to two, and three or more 2D LiDARs may be provided at different positions. Also, one or more 2D LiDARs and one or more 3D LiDARs may be used in combination. In this way, various modifications are possible to the configuration of this embodiment. It is to be noted that the various techniques described in the first embodiment can be applied to this embodiment as they are.

[0208] [3-3. Embodiment 3] Next, a third embodiment of the present disclosure will be described.

[0209] This embodiment relates to a system that uses one or more moving bodies (e.g., drones) to collect local map data for constructing environmental map data, and generates environmental map data based on the collected local map data.

[0210] FIG. 37 is a diagram showing a schematic configuration example of the system of this embodiment. This system includes one or more drones 400, one or more tractors 100C, and a server computer 500 (hereinafter referred to as "server 500"). These components are connected to each other so that they can communicate with each other via a wired or wireless network 60. Although two drones 400 and three tractors 100C are shown in FIG. 37, the number of drones 400 and tractors 100C is arbitrary. Each drone 400 generates local map data for constructing environmental map data while flying in the environment in which each tractor 100C runs, and transmits it to the server 500. The server 500 generates environmental map data expressed in a unified world coordinate system based on the local map data received from one or more drones 400, and distributes it to each tractor 100C. Each tractor 100C runs autonomously in the orchard based on the distributed environmental map data.

[0211] FIG. 38 is a block diagram showing a configuration example of the present system. The tractor 100C in this example has the same components as the tractor 100A in the first embodiment shown in FIG. 8. However, the ECU 160 in this embodiment does not have the map data generation module 164. The communication IF 190 communicates with the server 500 via the network 60. The storage device 150 stores the environmental map data distributed from the server 500. Note that FIG. 38 omits the illustration of the work machine that is connected to the tractor 100C for use. The communication IF 190 may communicate with the work machine connected to the tractor 100C as in the first embodiment.

[0212] The drone 400 includes a data generation unit 450, a driving device 440, a control device 480, and a communication IF 490. The driving device 440 includes various devices necessary for the flight of the drone 400, such as an electric motor for driving and a plurality of propellers. The control device 480 controls the operation of the data generation unit 450 and the driving device 440. The communication IF 490 is a circuit for communicating with the server 500 via the network 60. The data generation unit 450 includes a LiDAR sensor 410 and a data generation device 460. The LiDAR sensor 410 has a function similar to that of the LiDAR sensor 110 in the first embodiment. The data generation device 460 has a function similar to that of the ECU 160 in the first embodiment. That is, the data generation device 460 has a function of simultaneously performing self-position estimation and map generation. However, the data generation device 460 does not generate the final environmental map data. The data generation device 460 includes a processor and a storage medium such as a memory. The data generating device 460 repeatedly generates local map data indicating the distribution of trunks of multiple tree rows based on the scan data repeatedly output from the LiDAR sensor 410 while the drone 400 is flying, and accumulates the data in a storage medium. The accumulated local map data is transmitted from the communication IF 490 to the server 500, for example, by a user operation.

[0213] The server 500 may be a computer installed at a location remote from the tractor 100C and the drone 400, such as a cloud server or an edge server. The server 500 includes a storage device 550, a processing device 560, and a communication IF 590. The communication IF 590 is a circuit for communicating with the tractor 100C and the drone 400 via the network 60. The processing device 560 generates environmental map data by unifying and integrating the local map data acquired from the multiple drones 400 in a coordinate system, and records the environmental map data in the storage device 550. The processing device 560 distributes the generated environmental map data from the communication IF 590 to the multiple tractors 100C. The processing device 560 may distribute the environmental map data to a moving body other than the tractor 100C. For example, when the drone 400 not only collects map data but also performs tasks such as sowing, fertilization, or pest control, the environmental map data may be distributed to the drone 400. In this case, the drone 400 can fly autonomously while estimating its own position by matching the scan data output from the LiDAR sensor 410 with the environmental map data, and perform predetermined tasks.

[0214] 39 is a perspective view showing an example of the appearance of a drone 400. This drone 400 includes a data generation unit 450 on the front side of the main body. The data generation unit 450 includes a LiDAR sensor 410 and can scan the surrounding environment with a laser beam while flying.

[0215] FIG. 40 is a perspective view that shows a schematic example of a data collection operation by a plurality of drones 400. FIG. 40 shows two drones 400 flying between a plurality of tree rows 20. These drones 400 autonomously fly at low altitude between the tree rows 20, and repeat the operation of generating local map data showing the distribution of the trunks by scanning the trunks of the tree rows 20 with a laser beam. The method of generating the local map data is the same as the method in the first embodiment.

[0216] FIG. 41 is a diagram showing a tree 21 being irradiated by a laser beam emitted from a LiDAR sensor 410 mounted on a drone 400. The drone 400 in this embodiment is programmed so that the LiDAR sensor 410 flies at an altitude that maintains a position lower than the average height of the trunks of the tree row. The altitude of the drone 400 is controlled so that the LiDAR sensor 410 is located at a height of, for example, 15 cm to 100 cm above the ground. The data generating device 460 detects the trunks of the tree row in the environment surrounding the drone 400 while performing self-location estimation based on the sensor data repeatedly output from the LiDAR sensor 410 located at such a low altitude, generates local map data showing the distribution of the detected trunks of the tree row, and records it in a storage medium. The data generating device 460 obtains input point clouds of multiple scans from the sensor data repeatedly output from the LiDAR sensor 410, and performs self-location estimation by matching the input point cloud of at least one scan including the latest scan with the local map data generated before the last time.

[0217] 42 is a diagram showing an example of the format of local map data output from each drone 400. In this example, the local map data includes a moving object ID, which is an identification number of the drone 400, and the position (x, y, z) and attitude (θ R ,θ P ,θ Y ) and information recorded for each reflection point. The information recorded for each reflection point is similar to the information shown in FIG. 23, but the coordinates of each reflection point are coordinates (u, v, w) in a sensor coordinate system fixed to the LiDAR sensor 410 of the drone 400. Such local map data may be transmitted from each drone 400 to the server 500. Note that information on whether the point is a trunk, such as classification and trunk ID, may not be included in the local map data. After acquiring local map data from multiple drones 400, the processing device 560 in the server 500 may detect a point cloud corresponding to a trunk based on the characteristics of the distribution of the point cloud.

[0218] With the above configuration, data for constructing an environmental map can be efficiently collected even in a large orchard, for example.

[0219] In this embodiment, not only the drone 400 but also other moving bodies such as the tractor 100C may perform an operation of acquiring local map data for constructing environmental map data. In addition, the moving body that generates the local map data does not necessarily have to be a moving body that can move autonomously. For example, the local map data may be generated by a user driving or operating a moving body such as a tractor or a drone equipped with one or more LiDAR sensors mounted at a position lower than the average height of the trunks of a tree row.

[0220] A mobile body such as the drone 400 or the tractor 100C may generate final environmental map data and provide the environmental map data to other mobile bodies without providing the server 500. In this case, the environmental map data may be directly transmitted and received by communication between the mobile bodies.

[0221] [3-4. Other embodiments] In the above embodiments, each tractor may be an unmanned tractor. In that case, components necessary only for manned operation, such as a cabin, a driver's seat, a steering wheel, and an operation terminal, may not be provided in the tractor. The unmanned tractor may perform the same operations as those in the above embodiments by autonomous driving or remote control by the user of the tractor.

[0222] In the above embodiments, one or more sensors provided on the moving object are LiDAR sensors that output two-dimensional or three-dimensional point cloud data or distance distribution data as sensor data by scanning a laser beam. However, the sensor is not limited to such a LiDAR sensor. For example, a flash-type LiDAR sensor or other types of sensors such as an image sensor may be used. Such other types of sensors may be used in combination with a scanning LiDAR sensor.

[0223] A device that executes the processes required for self-location estimation and autonomous movement (or automatic steering) or map data generation in the above embodiments can be attached to a moving body that does not have these functions. For example, a control unit that controls the operation of a moving body moving between multiple tree rows can be attached to the moving body and used. Such a control unit includes one or more sensors that output sensor data indicating the distribution of objects in the environment around the moving body, a storage device that stores environmental map data indicating the distribution of trunks of multiple tree rows, a self-location estimation device, and a control device that controls the movement of the moving body according to the position of the moving body estimated by the self-location estimation device. The self-location estimation device detects trunks of tree rows in the environment around the moving body based on sensor data repeatedly output from one or more sensors while the moving body is moving, and estimates the position of the moving body by matching the detected trunks of the tree rows with the environmental map data. Also, a data generation unit that generates map data can be attached to a moving body moving between multiple tree rows and used. Such a data generation unit includes one or more sensors that output sensor data indicating the distribution of objects in the environment around the moving body, and a data generation device. The data generation device detects trunks of tree rows in the environment surrounding the moving body while performing self-location estimation based on sensor data repeatedly output from one or more sensors while the moving body is moving, and generates local map data for generating environmental map data showing the distribution of the trunks of the detected tree rows and records the local map data in a storage device.

[0224] As described above, the present disclosure includes the moving object, the control unit, the method, and the computer program described in the following items.

[0225] [Item 1] A moving object that moves among a plurality of tree rows, one or more sensors that output sensor data indicative of a distribution of objects in an environment surrounding the moving object; a storage device that stores environmental map data indicating a distribution of trunks of the plurality of tree rows; a self-location estimation device that detects trunks of the tree rows in the environment surrounding the moving body based on the sensor data repeatedly output from the one or more sensors while the moving body is moving, and estimates a position of the moving body by matching the detected trunks of the tree rows with the environmental map data; A control device that controls the movement of the moving object in accordance with the estimated position of the moving object; A moving body comprising:

[0226] [Item 2] The moving body described in item 1, wherein the one or more sensors include at least one LiDAR sensor that outputs two-dimensional or three-dimensional point cloud data as the sensor data.

[0227] [Item 3] (LiDAR sensor position) 3. The vehicle according to item 2, wherein the LiDAR sensor is mounted at a position lower than the average height of the trunks of the tree row.

[0228] [Item 4] 4. The moving body according to item 3, wherein the LiDAR sensor is disposed at a height of 15 cm to 100 cm above the ground.

[0229] [Item 5] The moving body according to item 3 or 4, wherein the self-location estimation device obtains an input point cloud of multiple scans from sensor data repeatedly output from the LiDAR sensor mounted at a position lower than the average height of the trunks of the tree row, and estimates the position of the moving body by matching the input point cloud of at least one scan including the latest scan with the environmental map data.

[0230] [Item 6] The LiDAR sensor is mounted at a position lower than an average height of the trunk of the tree row and has a plurality of laser light sources, each of the plurality of laser light sources emitting a laser pulse at a different elevation angle; The moving body according to any one of items 3 to 5, wherein the self-position estimation device detects the trunk based on a reflection point of the laser pulse emitted from a laser light source, among the plurality of laser light sources, whose elevation angle is within a predetermined range.

[0231] [Item 7] The moving body described in item 6, wherein the self-location estimation device detects the trunk based on a change in the distance from the LiDAR sensor to a reflection point of the laser pulse during scanning by the LiDAR sensor.

[0232] [Item 8] The one or more sensors include at least one LiDAR sensor that outputs two-dimensional or three-dimensional point cloud data as the sensor data; The LiDAR sensor repeatedly outputs the point cloud data at a preset period; The moving body described in item 1, wherein the self-location estimation device extracts points representing the surface of the trunk based on the position of each point in the point cloud data output during one or more periods, or the distance or angle of each point from the moving body, and estimates the position of the moving body by matching the extracted points with the environmental map data.

[0233] [Item 9] Further comprising an inertial measurement unit; The self-position estimation device estimates the position of the moving object by using a signal output from the inertial measurement unit. 9. A mobile object according to any one of items 1 to 8.

[0234] [Item 10] the self-location estimation device detects trunks of a plurality of trees having a unique combination of trunk intervals from the sensor data, and estimates a position of the moving body by matching the trunks of the plurality of trees having the unique combination of trunk intervals with the trunks of the plurality of trees having the unique combination of trunk intervals extracted from the environmental map data; 10. A mobile object according to any one of items 1 to 9.

[0235] [Item 11] 11. The moving body described in any one of items 1 to 10, wherein the control device determines whether or not an obstacle exists on the path of the moving body based on the environmental map data and the sensor data, and when it determines that the obstacle exists, causes the moving body to perform an operation to avoid colliding with the obstacle.

[0236] [Item 12] Further comprising an obstacle sensor; 12. The moving body according to any one of items 1 to 11, wherein the control device causes the moving body to perform an operation to avoid colliding with the obstacle when the obstacle sensor detects an obstacle on a path of the moving body.

[0237] [Item 13] 13. The moving body according to any one of items 1 to 12, further comprising a recording device that measures the thickness of each trunk of the detected tree row based on the sensor data and records the thickness of each trunk in a storage medium.

[0238] [Item 14] One or more supports are provided in the area in which the plurality of tree rows are arranged; The environmental map data indicates the distribution of the pillars in addition to the distribution of the trunks; the self-location estimation device further detects the pillars in the environment surrounding the moving body based on the sensor data, and estimates the position of the moving body by matching the detected trunks of the tree rows and the pillars with the environmental map data; 14. A mobile object according to any one of items 1 to 13.

[0239] [Item 15] A moving body described in any one of items 1 to 14, wherein the self-location estimation device detects the trunks of the tree rows in the environment surrounding the moving body based on the sensor data and information indicating the arrangement relationship of the multiple tree rows recorded in a storage medium.

[0240] [Item 16] Item 16. The moving body according to item 15, wherein information indicating the arrangement relationship of the plurality of tree rows is recorded in the storage medium by being input via an HMI (Human Machine Interface).

[0241] [Item 17] 17. The moving body according to any one of items 1 to 16, wherein the moving body is an autonomously movable work vehicle.

[0242] [Item 18] The self-location estimation device includes: generating data indicative of a distribution of trunks of the tree row detected based on the sensor data; updating the environmental map data using the data indicating the distribution of the stems, or transmitting the data indicating the distribution of the stems to an external device that updates the environmental map data; 18. A mobile object according to any one of items 1 to 17.

[0243] [Item 19] A control unit for controlling an operation of a moving object that moves among a plurality of tree rows, One or more sensors attached to the mobile body for use in outputting sensor data indicative of a distribution of objects in an environment surrounding the mobile body; a storage device that stores environmental map data indicating a distribution of trunks of the plurality of tree rows; a self-location estimation device that detects trunks of the tree rows in the environment surrounding the moving body based on the sensor data repeatedly output from the one or more sensors while the moving body is moving, and estimates a position of the moving body by matching the detected trunks of the tree rows with the environmental map data; A control device that controls the movement of the moving object in accordance with the estimated position of the moving object; A control unit comprising:

[0244] [Item 20] 1. A method for controlling the movement of a vehicle moving among a plurality of rows of trees, comprising: acquiring environmental map data indicative of a distribution of trunks of the plurality of tree rows; acquiring sensor data indicative of a distribution of objects in an environment surrounding the moving body from one or more sensors mounted on the moving body while the moving body is moving; Detecting trunks of the tree rows in an environment surrounding the moving object based on the acquired sensor data; estimating a position of the moving object by matching the detected trunks of the tree rows with the environmental map data; Controlling the movement of the moving body in response to the estimated position of the moving body; The method includes:

[0245] [Item 21] A computer program executed by a computer to control the operation of a moving object that moves among a plurality of tree rows, The computer includes: acquiring environmental map data indicative of a distribution of trunks of the plurality of tree rows; acquiring sensor data indicative of a distribution of objects in an environment surrounding the moving body from one or more sensors mounted on the moving body while the moving body is moving; Detecting trunks of the tree rows in an environment surrounding the moving object based on the acquired sensor data; estimating a position of the moving object by matching the detected trunks of the tree rows with the environmental map data; Controlling the movement of the moving body in response to the estimated position of the moving body; A computer program that executes the following: [Industrial Applicability]

[0246] The techniques disclosed herein can be applied to mobile objects such as tractors, drones, walking robots, and the like that move through environments with multiple rows of trees, such as orchards such as vineyards. [Explanation of symbols]

[0247] 20 tree rows 22 Tree trunk 30 Travel route 40 Obstacles 50 people 60 Network 100 Mobile 100A, 100B Tractor 101 Vehicle body 102 Engine 103 Transmission 104 Wheels Cabin 105 106 Steering Gear 107 Driver's seat 108 Coupling device 110, 110A, 110B LiDAR Sensors 111 Laser unit 112 Laser light source 113 Photodetector 114 Motor 115 Control circuit 116 Signal Processing Circuit 117 Memory 120 GNSS units 125 Inertial Measurement Unit (IMU) 130 Obstacle Sensor 140 Drive unit 150 Storage device 160 Electronic Control Unit (SLAM) 162 Self-location estimation module 164 Map Data Generation Module 170 Electronic control unit (route determination) 180 Electronic control unit (drive control) 190 Communication Interface (IF) 200 Operation terminal 300 Implements 340 Drive Unit 380 Control Device 390 Communication Interface (IF) 400 Drone 410 LiDAR Sensor 440 Drive Unit 450 Data Generation Unit 460 Data Generator 480 Control Device 490 Communication Interface 500 servers 550 Storage device 560 Processing Equipment 590 Communication Interface

Claims

1. A moving object that moves among a plurality of tree rows, A LiDAR sensor that outputs sensor data that is three-dimensional point cloud data indicating a distribution of objects in an environment surrounding the moving body; a storage device that stores environmental map data indicating a distribution of trunks of the plurality of tree rows; a self-location estimation device that detects trunks of the tree rows in the environment surrounding the moving body based on the sensor data repeatedly output from the LiDAR sensor while the moving body is moving, and estimates a position of the moving body by matching the detected trunks of the tree rows with the environmental map data; A control device that controls the movement of the moving object in accordance with the estimated position of the moving object; Equipped with when a smaller number of trunks are detected in one of two adjacent tree rows among the plurality of tree rows than in the other of the two tree rows, the control device estimates a distribution of hidden trunks in the one of the two tree rows based on a distribution of the detected trunks, and performs steering control of the moving body based on the estimated distribution. Mobile body.

2. The LiDAR sensor is mounted at a position lower than the average height of the trunks of the tree row, The LiDAR sensor has a plurality of laser light sources, each of the plurality of laser light sources emits a laser pulse at a different elevation angle; the self-position estimation device detects the trunk based on a reflection point of the laser pulse emitted from a part of the laser light sources, the elevation angle of which is included in a predetermined range, among the plurality of laser light sources; The moving body according to claim 1 .

3. the self-position estimation device detects the trunk based on reflection points of the laser pulses emitted from the plurality of laser light sources that are located at a position lower than an average height of the trunk; The moving body according to claim 2.

4. some of the plurality of laser light sources emit the laser pulses downward with respect to a moving direction of the moving body; The rest of the plurality of laser light sources emit the laser pulses upward with respect to the moving direction of the moving body, the self-position estimation device detects the trunk based on a reflection point of the laser pulse from a part of the laser light sources that emits the laser pulse downward with respect to the moving direction of the moving body, among the plurality of laser light sources; A moving body according to claim 2 or 3.

5. The moving body according to claim 1 , wherein the LiDAR sensor is disposed at a height of 15 cm to 100 cm above the ground.

6. A moving body as described in any one of claims 1 to 5, wherein the self-location estimation device obtains an input point cloud of multiple scans from sensor data repeatedly output from the LiDAR sensor, and estimates the position of the moving body by matching the input point cloud of multiple scans including the latest scan with the environmental map data.

7. The moving body according to claim 2 , wherein the self-location estimation device determines the predetermined range based on a setting operation performed by a user using a terminal device.

8. A moving body as described in any one of claims 2 to 4 and 7, wherein the self-location estimation device detects the trunk based on a change in the distance from the LiDAR sensor to a reflection point of the laser pulse during scanning by the LiDAR sensor.

9. The LiDAR sensor repeatedly outputs the point cloud data at a preset period; A moving body as described in any one of claims 1 to 8, wherein the self-location estimation device extracts points representing the surface of the trunk based on the position of each point in the point cloud data output during one or more periods, or the distance or angle of each point from the moving body, and estimates the position of the moving body by matching the extracted points with the environmental map data.

10. Further comprising an inertial measurement unit; The self-position estimation device estimates the position of the moving object by using a signal output from the inertial measurement unit. A moving body according to any one of claims 1 to 9.

11. The self-location estimation device includes: determining a tilt angle of the LiDAR sensor based on the signal output from the inertial measurement unit; adaptively selecting the predetermined range depending on the tilt angle; The moving body according to claim 10.

12. the self-location estimation device detects trunks of a plurality of trees having a unique combination of trunk intervals from the sensor data, and estimates a position of the moving body by matching the trunks of the plurality of trees having the unique combination of trunk intervals with the trunks of the plurality of trees having the unique combination of trunk intervals extracted from the environmental map data; A moving body according to any one of claims 1 to 11.

13. The mobile body according to any one of claims 1 to 12, wherein the control device determines whether or not an obstacle is present on a path of the mobile body based on the environmental map data and the sensor data, and when it determines that the obstacle is present, causes the mobile body to perform an action to avoid colliding with the obstacle.

14. Further comprising an obstacle sensor; 14. The mobile body according to claim 1, wherein the control device causes the mobile body to perform an operation to avoid colliding with the obstacle when the obstacle sensor detects an obstacle on a path of the mobile body.

15. 15. The moving body according to claim 1, further comprising a recording device that measures a thickness of each trunk of a detected row of trees based on the sensor data and records the thickness of each trunk in a storage medium.

16. A moving body as described in any one of claims 1 to 15, wherein the self-location estimation device detects the trunks of the tree rows in the environment surrounding the moving body based on the sensor data and information indicating the arrangement relationship of the multiple tree rows recorded in a storage medium.

17. The moving body according to claim 1 , wherein the moving body is a work vehicle capable of moving autonomously.

18. Further equipped with headlights, The LiDAR sensor is disposed at a position lower than the headlight. A moving body according to any one of claims 1 to 17.

19. The self-location estimation device includes: generating data indicative of a distribution of trunks of the tree row detected based on the sensor data; updating the environmental map data using the data indicating the distribution of the stems, or transmitting the data indicating the distribution of the stems to an external device that updates the environmental map data; A moving body according to any one of claims 1 to 18.

20. The self-location estimation device, from the point cloud indicated by the sensor data, (a) A downward convex arc-shaped collection of points, or (b) a collection of points that are locally closer to the LiDAR sensor than surrounding points; or (c) a group of points whose reflection intensities are within a predetermined range and whose positions are close to each other; The stem is detected by extracting A moving body according to any one of claims 1 to 19.

21. The moving body according to any one of claims 1 to 20, wherein the self-location estimation device extracts only a point cloud that is within a certain range of height from the ground from the point cloud indicated by the sensor data, and detects the trunk based on the extracted point cloud.

22. The environmental map data further indicates a distribution of a plurality of posts located around the plurality of tree rows; A moving body as described in any one of claims 1 to 21, wherein the self-location estimation device detects trunks and supports of the tree row in the environment surrounding the moving body based on the sensor data, and estimates the position of the moving body by matching the detected trunks and supports of the tree row with the environmental map data.

23. A control unit for controlling an operation of a moving object that moves among a plurality of tree rows, A LiDAR sensor that is attached to the moving body and used to output sensor data that is three-dimensional point cloud data indicating a distribution of objects in an environment surrounding the moving body; a storage device that stores environmental map data indicating a distribution of trunks of the plurality of tree rows; a self-location estimation device that detects trunks of the tree rows in the environment surrounding the moving body based on the sensor data repeatedly output from the one or more LiDAR sensors while the moving body is moving, and estimates the position of the moving body by matching the detected trunks of the tree rows with the environmental map data; A control device that controls the movement of the moving object in accordance with the estimated position of the moving object; Equipped with when a smaller number of trunks are detected in one of two adjacent tree rows among the plurality of tree rows than in the other of the two tree rows, the control device estimates a distribution of hidden trunks in the one of the two tree rows based on a distribution of the detected trunks, and performs steering control of the moving body based on the estimated distribution. Control unit.

24. 1. A method for controlling the operation of a vehicle moving among a plurality of rows of trees, comprising: acquiring environmental map data indicative of a distribution of trunks of the plurality of tree rows; Acquiring sensor data, which is three-dimensional point cloud data indicating a distribution of objects in an environment surrounding the moving body, from a LiDAR sensor mounted on the moving body while the moving body is moving; Detecting trunks of the tree rows in an environment surrounding the moving object based on the acquired sensor data; estimating a position of the moving object by matching the detected trunks of the tree rows with the environmental map data; Controlling the movement of the moving body in response to the estimated position of the moving body; When a smaller number of stems are detected in one of two adjacent tree rows among the plurality of tree rows than in the other of the two tree rows, estimating a distribution of hidden stems in the one of the two tree rows based on a distribution of the detected stems; performing steering control of the moving body based on the estimated distribution; and The method includes:

Images