Mobile body, data generation unit, and method for generating data

Abstract

To determine with high accuracy a position in a heavily wooded environment. A moving body moves between a plurality of tree lines. The moving body comprises at least one sensor for outputting sensor data indicating the distribution of objects in an environment around the moving body, and a data generating device. The data generating device detects the trunk parts of the tree lines in the environment around the moving body while estimating the position of the moving body on the basis of the sensor data repeatedly outputted from the at least one sensor during the movement of the moving body, generates local map data for generating environment map data indicating the distribution of the detected trunk parts of the tree lines, and records the data in a storage device.

Description

Technical Field

[0001] The present disclosure relates to a mobile body, a data generation unit, and a method for generating data.

Background Art

[0002] As next-generation agriculture, research and development of smart agriculture utilizing ICT (Information and Communication Technology) and IoT (Internet of Things) has been promoted. Research and development for the automation and unmanned operation of work vehicles such as tractors used in fields has also been advanced. For example, work vehicles that travel with automatic steering using a positioning system such as GNSS (Global Navigation Satellite System) capable of 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] On the other hand, the development of mobile bodies that autonomously move using distance sensors such as LiDAR (Light Detection and Ranging) has also been advanced. For example, Patent Document 4 discloses an example of a work vehicle that automatically travels between crop rows in a field using LiDAR.

Prior Art Documents

Patent Documents

[0004]

Patent Document 1

Patent Document 2

Patent Document 3

Patent Document 4

Summary of the Invention

Problems to be Solved by the Invention

[0005] In environments such as orchards like vineyards or forests where trees are distributed at high density, the thick-growing leaves at the upper part of the trees form a canopy, acting as obstacles or multiple reflectors for radio waves from satellites. In such environments, it is difficult to perform accurate positioning using GNSS. In environments where GNSS cannot be used, it is conceivable to use SLAM (Simultaneous Localization and Mapping) that simultaneously executes position estimation and map creation. However, there are various problems in the practical application of a moving body that autonomously or automatically steers while moving within an environment where a large number of trees exist using SLAM. For example, since the distribution of the foliage of trees changes significantly depending on the season, there is a problem that the map created in the past cannot be continuously used.

Means for Solving the Problems

[0006] The moving body according to an exemplary embodiment of the present disclosure moves between a plurality of tree rows. The moving body 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 performs self-position estimation while detecting the trunks 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 generates local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows and records it in a storage device.

[0007] The comprehensive or specific aspects of the present disclosure can be implemented 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 within one device, or may be separately arranged within two or more separate devices.

Advantages of the Invention

[0008] According to an embodiment of the present disclosure, even in an environment where GNSS-based positioning is difficult, automatic steering or autonomous movement of a moving body can be realized. The distribution of the trunks of the tree rows has less seasonal variation compared to the distribution of the leaves. By creating an environmental map focusing on the trunks, it becomes possible to continuously use the same environmental map over a relatively long period.

Brief Description of the Drawings

[0009]

Figure 1

Figure 2A

Figure 2B

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Figure 8

Figure 9A

Figure 9B

Figure 10

Figure 11

Figure 12

Figure 13A

Figure 13B

Figure 14A

Figure 14B

Figure 15A

Figure 15B

Figure 16

Figure 17A

Figure 17B

Figure 17C

Figure 18

Figure 19

Figure 20A

Figure 20B

Figure 21A

Figure 21B

Figure 22

Figure 23

Figure 24

Figure 25

Figure 26

Figure 27

Figure 28A

Figure 28B

Figure 28C

Figure 28D

Figure 29A

Figure 29B

Figure 30

Figure 31A

Figure 31B

Figure 31C

Figure 31D

Figure 32

Figure 33A

Figure 33B

Figure 34

Figure 35

Figure 36A

Figure 36B

Figure 36C

Figure 37

Figure 38

Figure 39

Figure 40

Figure 41

Figure 42

Mode for Carrying Out the Invention

[0010] [1. Definition of Terms] The definitions of the main terms used in this specification are described below.

[0011] A "mobile body" is a device equipped with a driving device such as wheels, crawlers, bipedal or multi-legged walking devices, or propellers that generate a driving force (traction) for movement. The "mobile body" in the present disclosure includes work vehicles such as tractors, transport vehicles, mobile robots, and unmanned aerial vehicles (Unmanned aerial vehicle: UAV, so-called drones) such as multicopters. The mobile body may be unmanned or manned.

[0012] A "work vehicle" means a vehicle that can travel while performing a specific work at a work site such as a farm field (for example, an orchard, a field, a paddy field, or a pasture), a mountain forest, or a construction site. Examples of work vehicles include agricultural machines such as tractors, rice transplanters, combines, riding management machines, and riding lawn mowers, as well as vehicles used for non-agricultural purposes such as construction work vehicles and snow removal vehicles.

[0013] "SLAM" is a general term for technologies that simultaneously perform self-position estimation and map creation of a moving object.

[0014] "Self-position estimation" is to estimate the position of the moving object on the map (for example, the center-of-gravity position of the moving object). In self-position estimation by SLAM, usually, the pose of the moving object is obtained.

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

[0016] "Environmental map data" is data that represents the positions or areas of objects existing in the environment where a moving object moves in a predetermined coordinate system. The environmental map data may sometimes be simply referred to as "environmental map". Examples of the coordinate system that defines the environmental map include not only world coordinate systems such as the geographic coordinate system fixed to the Earth, but also odometry coordinate systems that display poses based on odometry information. The environmental map data may include information other than the position (for example, attribute information and other information) about the objects existing in the environment. The environmental map includes various forms of maps such as a point cloud map or a grid map. Hereinafter, the environmental map may be referred to as "map data" or simply "map". Also, the data of a local map or a partial map generated or processed in the process of constructing the environmental map may also be referred to as "map data" or simply "map".

[0017] "Automatic steering" is to steer a moving object by the function of a control device without manual operation. Part or all of the control device may be outside the moving object. Communication such as control signals, commands, or data can be performed between the control device outside 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 by the function of a control device while sensing the surrounding environment without human involvement in the control of the movement. Autonomous movement includes autonomous driving and autonomous flight. The control device can control operations necessary for the movement of the moving object, such as steering, speed control, start and stop of driving, ascent, descent, and hovering during flight. Autonomous movement may include detection of obstacles and obstacle avoidance operations.

[0019] "Automatic driving" includes autonomous driving by a control device provided in a moving object and driving based on commands from a computer in an operation management system. Autonomous driving includes not only the movement of a moving object toward a destination along a predetermined route, but also the movement following a following target. Also, it may move temporarily based on an operator's instruction.

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

[0021] The "external sensor" is a sensor that senses the state outside the moving body. Examples of external sensors include, for example, a laser rangefinder (also referred to as a "range sensor"), a camera (or image sensor), a LiDAR sensor, a millimeter-wave radar, and a magnetic sensor.

[0022] The "internal sensor" is a sensor that senses the state of the moving body. The internal sensors include 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 signals indicating the movement amount and posture of the moving body. Information indicating the amount of change in the pose of the moving body acquired by the internal sensor is referred to as "odometry information".

[0023] The "trunk of a tree" is the lignified stem of a woody plant and is the main axis part that stands upright on the ground and bears branches. It does not include the branches, leaves, and roots of the tree.

[0024] [2. Basic Principles of SLAM] Next, the basic principle of self-position estimation using the SLAM technology used in the embodiments of the present disclosure will be described. Here, for simplicity, it is assumed that the moving body moves within a two-dimensional space (i.e., a plane).

[0025] First, refer to FIG. 1. FIG. 1 is a diagram schematically showing an example of a moving body 100 that moves on a plane from time t1 to time t2. The moving body 100 in this example is a work vehicle (e.g., a tractor) that travels by 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. In FIG. 1, the position and orientation of the moving body 100 moving on the plane are schematically shown. The position coordinates of the moving body 100 are indicated by an XY coordinate system. The XY coordinate system is a world coordinate system Σ fixed to the earth w which is. In FIG. 1, a moving body coordinate system Σ V fixed to the moving body 100 is also shown. The moving body coordinate system Σ V in this example is a uv coordinate system in which the direction in which the front of 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. The position of the origin of the moving body coordinate system Σ V changes as the moving body 100 moves. The orientation of the moving body coordinate system Σ V changes as the orientation of the moving body 100 changes, that is, as it rotates.

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

[0027] In the example of FIG. 1, between time t1 and time t2, the position of the moving body 100 moves (translates) by Δd1 in the azimuth of the angle θ1, and the orientation of the moving body 100 rotates counterclockwise by Δφ1 (= θ2 - θ1). Thus, 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 traveling direction of the moving body 100 can be approximated to be parallel to the u-axis of the moving body coordinate system Σ V Therefore, the following Equation 1 holds.

Equation

[0029] When the moving body 100 is equipped with an internal sensor such as a wheel rotation speed sensor and / or an inertial measurement unit (IMU), the estimated values of Δd1 and Δφ1, that is, the odometry information, can be obtained from the internal sensor. When the time difference from time t1 to time t2 is short, such as 10 milliseconds, the distance Δd1 is sufficiently short and Equation 1 holds. As time progresses from t1, t2, t3, ···, by periodically updating the estimated values of Δd1 and Δφ1, it is possible to estimate the changes in the position and orientation (i.e., pose) of the moving body 100. In other words, when the initial pose, for example, (x1, y1, θ1) is known, the estimated value of the subsequent pose of the moving body 100 can be periodically updated based on the odometry information. However, there is a problem that errors accumulate in the pose estimation based on the odometry information. Therefore, in many cases, it is necessary to use a satellite positioning system or SLAM technology to obtain a highly accurate estimated value of the position of the moving body 100.

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

[0031] <When environmental map data exists> In the example shown in FIGS. 2A and 2B, in the environment where the moving body 100 moves from time t1 to time t2, there are landmarks m1, m2, m3, m4 with fixed positions. In this example, environmental map data indicating the positions of the landmarks already exists. Such environmental map data includes, for example, an identifier (e.g., landmark number) for identifying each landmark and the position coordinates of the landmark in the world coordinate system Σ w associated therewith. The position coordinates of the landmarks m1, m2, m3, m4 in the world coordinate system Σ w are respectively (x m1 , y m1 ), (x m2 , y m2 ), (x m3 , y m3 ), (x m4 , y m4 ). Individual points in the point cloud data acquired by LiDAR may function as landmarks. The environmental map constituted by the point cloud data is referred to as a point cloud map. The positions of the landmarks may be indicated by cells on a grid map.

[0032] In the example of FIG. 2A, let the observation values (or measurement values) of the landmarks m1, m2, m3, m4 acquired by the moving body 100 at the pose r1 be z1, z2, z3, z4 respectively. 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 the present disclosure.

[0033] As shown in FIG. 2B, as the moving body 100 moves, let the observation values of the landmarks m1, m2, m3 acquired by the moving body 100 at the pose r2 be z5, z6, z7 respectively.

[0034] Based on these observation values, the position coordinates of the landmarks on the moving body coordinate system Σ V can be obtained. As described above, the moving body coordinate system Σ VSince it is a coordinate system fixed to the moving body 100, the position coordinates of the same landmark (for example, landmark m1) in the moving body coordinate system Σ V will 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 the sensor coordinate system determined by the position and orientation of the external sensor. Strictly speaking, the sensor coordinate system may be different from the moving body coordinate system Σ V However, in the following explanation, it is assumed that the sensor coordinate system is the same as the moving body coordinate system Σ V Since the relationship between the sensor coordinate system and the moving body coordinate system Σ V is known, one coordinate system can be made to coincide with the other by rotating one coordinate system by a known angle and translating it by a known distance.

[0036] The moving body 100 moving in the environment observes a plurality of landmarks to obtain the position coordinates of each landmark in the moving body coordinate system Σ V Then, when the position coordinates of each landmark in the world coordinate system Σ W are included in the environmental map data, the poses r1 and r2 can be estimated based on the observation values z1 to z7 and the like. Such estimation can be achieved, for example, by matching the position coordinates of the landmark obtained from the observation value with the position coordinates of the landmark included in the environmental map data.

[0037] Next, with reference to FIG. 3, a method of converting the coordinates (u1, v1) on the moving body coordinate system Σ V to the coordinates (u1’, v1’) on the world coordinate system Σ W will be described. FIG. 3 shows the arrangement relationship between the moving body coordinate system Σ V and the world coordinate system Σ W at time t2. In the example of FIG. 3, for simplicity, the origin of the moving body coordinate system Σ V is made to coincide with the origin of the sensor coordinate system (sensor center), and it is assumed that both coordinate systems are the same. The moving body coordinate system Σ VThe upper coordinates (u1, v1) are calculated based on the observation value z5 of the landmark m1 from the moving object 100 at the pose r2 at time t2. If the observation value z5 is defined by the distance r m1 and the angle φ with respect to the u-axis m1 , then u1 and v1 are u1 = r m1 cos φ m1 , v1 = r m1 sin φ m1 and are calculated by. The conversion from the coordinates on the moving object coordinate system Σ V to the coordinates on the world coordinate system Σ W (coordinate conversion) is performed by rotating the moving object coordinate system Σ V clockwise by an angle θ2, and translating the origin of the moving object coordinate system Σ V to the origin of the world coordinate system Σ W . At this time, the following equations 2 and 3 hold.

Equation

Equation

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

[0039] The (u1’, v1’) obtained by coordinate conversion should match the coordinates of the landmark m1 in the world coordinate system Σ w (x m1 , y m1 ). However, if the estimated value of the pose of the moving object 100 deviates from the true value, an error (distance) will occur between (u1’, v1’) and (x m1 , y m1 ). Performing self-position estimation means that between (u1’, v1’) and (x m1 , y m1It is to determine the content of the rotation matrix R and the position vector T so that the error from

[0040] Next, an example of matching will be described in more detail with reference to FIGS. 4 and 5. FIG. 4 shows the coordinates of landmarks m1, m2, and m3 on the uv coordinate system which is the moving body coordinate system Σ at time t2. These coordinates can be calculated from the observation values z5, z6, and z7. In this example, let the coordinates calculated from the observation values z5, z6, and z7 be (u5, v5), (u6, v6), and (u7, v7), respectively. By performing coordinate transformation from the moving body coordinate system Σ V to the world coordinate system Σ V , (u5, v5), (u6, v6), and (u7, v7) on the moving body coordinate system Σ W are transformed into (u5’, v5’), (u6’, v6’), and (u7’, v7’) on the world coordinate system Σ V as shown in FIG. 5. FIG. 5 shows an example of a state where there are differences between the transformed coordinates (u5’, v5’), (u6’, v6’), (u7’, v7’) and the coordinates (x W , y m2 ), (x m2 , y m3 ), (x m3 , y m4 ) of the corresponding landmarks. The coordinate transformation is defined by rotation by the rotation matrix R and translation by the position vector T. On the world coordinate system Σ m4 , the content of the coordinate transformation (R, T) is determined so that (u5’, v5’), (u6’, v6’), and (u7’, v7’) approach (i.e., match) the coordinates (x W , y m2 ), (x m2 , y m3 ), (x m3 , y m4 ), (x m4 ) of the corresponding landmarks respectively.

[0041] In the equation of Formula 2 described above, the unknowns are x2, y2, and θ2. Since the number of unknowns is 3, if there are three or more equations corresponding to Formula 2, x2, y2, and θ2 can be obtained by calculation. As shown in FIG. 5, when three or more landmarks are observed from the moving body 100 in the same pose r2, three equations corresponding to Formula 2 can be obtained. Therefore, by solving the system of simultaneous equations, the calculated values of the pose r2 (x2, y2, θ2) can be obtained. Since errors are added to actual observations, optimization using a system of simultaneous equations consisting of a large number of equations exceeding three is performed by the least squares method or the like.

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

[0043] Note that when estimating the position of the moving body 100 from the observed values and the environmental map data, it is not always necessary to estimate the orientation of the moving body 100. For example, as shown in FIGS. 2A and 2B, when the distances to landmarks whose positions are known can be obtained from the observed values, based on the principle of triangulation, the position of the moving body 100 (accurately, the position of the distance measuring sensor) can be calculated. Estimating the position based on such a principle is also included in "self-position estimation".

[0044] <When there is no environmental map data> When there is no environmental map data, based on the observed values obtained by the moving body 100 during movement as shown in FIGS. 2A and 2B, while determining (i.e., estimating) the coordinates of the landmarks m1, m2, m3, m4, ···, it is necessary to perform a process of estimating the pose of the moving body 100. The world coordinate system Σ W of the coordinates of the landmarks m1, m2, m3, m4, ··· constitutes the environmental map data. Therefore, to construct a map, it is necessary to determine the position coordinates of the objects that function as landmarks in the world coordinate system Σ W above.

[0045] In the examples of FIGS. 2A and 2B, the moving object 100 that is in motion acquires a plurality of observation values from each of the landmarks m2, m3, and m4. By performing more observations, a system of simultaneous equations can be obtained that is greater than the total number of unknowns included in the pose of the moving object 100 and the coordinates of the landmarks. Thereby, it is possible to calculate estimated values of the pose of the moving object 100 and the coordinates of the landmarks.

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

[0047] Note that LiDAR sensors include a scanning type sensor that acquires information on the distance distribution of an object in space by scanning a laser beam, and a flash type sensor that acquires information on the distance distribution of an object in space by using light diffused over a wide range. Since the scanning type LiDAR sensor uses light of higher intensity than the flash type LiDAR sensor, it can acquire distance information for a farther distance. On the other hand, the flash type LiDAR sensor has a simple structure and can be manufactured at low cost, so it is suitable for applications that do not require strong light. In the present disclosure, an example in which mainly a scanning type LiDAR sensor is 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 with a typical scanning type LiDAR sensor, by emitting a pulsed laser beam (i.e., a laser pulse) and measuring the time until the laser pulse reflected by an object existing in the surrounding environment returns to the LiDAR sensor, the distance and direction to the reflection point located on the surface of the object can be known. If the distance and direction to the reflection point are known, the moving object coordinate system ΣV It is possible to obtain the coordinates of the "reflection points" in V . Scanning-type LiDAR sensors can be classified into two-dimensional LiDAR sensors and three-dimensional LiDAR sensors. According to a two-dimensional LiDAR sensor, the environment can be scanned so that the laser beam rotates within one plane. In contrast, in a three-dimensional LiDAR sensor, the environment can be scanned so that a plurality of laser beams rotate along different conical surfaces. The coordinates (two-dimensional or three-dimensional coordinate values) of the individual reflection points obtained by these LiDAR sensors are in the mobile body coordinate system Σ V represented. The coordinates of the individual reflection points are in the mobile body coordinate system Σ V from the world coordinate system Σ W By converting to, the coordinates of the individual reflection points in the world coordinate system Σ W can be obtained, and as a result, a point cloud map can be constructed. To convert from the mobile body coordinate system Σ V to the world coordinate system Σ W , as described above, information on the pose of the mobile body 100 is required.

[0049] In the SLAM technology, the estimation of the pose of the mobile body 100 and the construction of the environmental map can be executed simultaneously. However, when constructing the environmental map, the pose of the mobile body 100 may be estimated or measured using a technology other than the SLAM technology. This is because the pose of the mobile body 100 can also be determined by using a satellite positioning system and an inertial navigation device that measure the position of the mobile 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 mobile body 100 using the SLAM technology in order to construct the environmental map. Note that the position of the mobile body 100 may be estimated using the SLAM technology, and the orientation or posture of the mobile body 100 may be estimated by other sensors such as an inertial measurement device.

[0050] [3. Embodiment] Hereinafter, embodiments of the present disclosure will be described. However, detailed descriptions that are more than necessary may be omitted. For example, detailed descriptions of well-known matters and duplicate descriptions of substantially the same configurations may be omitted. This is to avoid making the following description unnecessarily redundant and to facilitate the understanding of those skilled in the art. The inventor provides the accompanying drawings and the following description to enable those skilled in the art to fully understand the present disclosure, and does not intend to limit the subject matter described in the claims thereby. In the following description, components having the same or similar functions are denoted by the same reference numerals.

[0051] [3-1. Embodiment 1] A moving body 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 are lush 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-position 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 the trunks of a plurality of tree rows. The self-position estimation device detects the trunks of the tree rows in the environment around the mobile body based on the sensor data repeatedly output from one or more sensors while the mobile body is moving, and estimates the position of the mobile body by performing matching between the detected trunks of the tree rows and 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 environmental map data. The data generation device detects the trunks of the tree rows in the environment around the mobile body while performing self-position estimation based on the 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 the detected trunks of the tree rows and records it in the storage device. The data generation device can also generate environmental map data by stitching together the repeatedly generated local map data and record it in the storage device. Thus, the mobile body in this embodiment has a function of creating an environmental map while moving between a plurality of tree rows, and a function of autonomously moving between the tree rows while estimating the position of the mobile body using the created environmental map.

[0053] The one or more sensors may include 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 the distribution of a plurality of reflection points observed by a LiDAR sensor. The point cloud data may include, for example, the coordinate values of each reflection point in a 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, for example, at a preset cycle. The data generation device can detect the trunk based on the position of each point in the point cloud data output during a period of one cycle or more, or the distance or angle from each point to the moving object.

[0055] Hereinafter, taking the case where the moving object is a work vehicle such as a tractor mainly used for work in an orchard such as a vineyard and the sensor is a scanning LiDAR sensor as an example, the configuration and operation of this embodiment will be described.

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

[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. In the example of FIG. 6, one obstacle sensor 130 is shown, but the obstacle sensors 130 can 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 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. Inside the cabin 105, a driver's seat 107, a steering device 106, and an operation terminal 200 are provided. One or both of the front wheels 104F and the rear wheels 104R may be crawlers instead of tires.

[0059] In this embodiment, the LiDAR sensor 110 is disposed at the lower front part of the vehicle body 101. The LiDAR sensor 110 can be disposed at a position lower than the average height of the trunks of the tree rows existing in the environment where the tractor 100A travels, for example, at a height of 15 cm or more and 100 cm or less from the ground. The LiDAR sensor 110 can be disposed, for example, at a position lower than half of the height of the tractor 100A. The height of the tractor 100A is the height from the ground to the uppermost part of the cabin 105 and can 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 wheel 104 and the headlight 109. The effects 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 existing 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 generation of environmental map data based on the sensor data and self-position estimation using the environmental map by using the above-described SLAM algorithm. Note that instead of completing the environmental map data, the control device may create local map data for generating the environmental map data. In that case, the process of integrating the local map data to construct the environmental map data may be executed by a computer outside the tractor 100A, for example, a cloud server.

[0061] In the example of FIG. 7, an obstacle sensor 130 is provided at the rear part of the vehicle body 101. The obstacle sensor 130 can also be arranged at a part other than the rear part of the vehicle body 101. For example, one or a plurality of obstacle sensors 130 can be provided at any location of the side part, the front part of the vehicle body 101, and the cabin 105. The obstacle sensor 130 detects an object existing relatively close to the tractor 100A. The obstacle sensor 130 can include, for example, a laser scanner or an ultrasonic sonar. When an object exists 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 vehicle body of the tractor 100A. For example, a plurality of laser scanners and a plurality of ultrasonic sonars may be arranged at different positions on the vehicle body. By providing such a large number of obstacle sensors 130, the 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, for example, GPS (Global Positioning System), GLONAS, Galileo, BeiDou, or Quasi-Zenith Satellite System (QZSS, for example, Michibiki), and performs positioning based on the signals. Since the tractor 100A in the present embodiment is mainly used in an environment where a plurality of trees grow thickly like a vineyard and it is difficult to use GNSS, positioning is performed using the LiDAR sensor 110. However, in an environment where GNSS signals can be received, positioning may be performed using the GNSS unit 120. By combining the positioning by the LiDAR sensor 110 and the 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 switch the driving force of the tractor 100A by shifting gears. The transmission 103 can also switch between the forward and reverse 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 steering wheels, and by changing their steering angles, the traveling direction of the tractor 100A can be changed. 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 work implement 300 can be attached to and detached from the tractor 100A by the coupling device 108. The coupling device 108 can, for example, raise and lower the three-point link device by a hydraulic device to control the position or attitude of the work implement 300. Also, power can be transmitted from the tractor 100A to the work implement 300 via the universal joint. The tractor 100A can make the work implement 300 perform a predetermined operation while pulling the work implement 300. The coupling device may be provided in front of the vehicle body 101. In that case, a work implement can be connected in front of the tractor 100A. When the LiDAR sensor 110 is used with the work implement connected in front of the tractor 100A, the LiDAR sensor 110 is arranged at a position where the laser beam is not blocked by the work implement.

[0066] The work machine 300 shown in Fig. 7 is a sprayer for spraying chemicals on crops, but the work machine 300 is not limited to a sprayer. For example, any work machine such as a mower, a seeder, a spreader, a rake, a baler, a harvester, a plow, a cultivator, 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. In the drawings, for clarity, the origin of the uvw coordinate system is shown as being located 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 the schematic configuration of the tractor 100A and the work machine 300. The tractor 100A in this example includes a LiDAR sensor 110, a GNSS unit 120, an obstacle sensor 130, an operation terminal 200, in addition to an inertial measurement unit (IMU) 125, a drive device 140, a storage device 150, a plurality of electronic control units (ECUs) 160, 170, 180, and a communication interface (IF) 190. The work machine 300 includes a drive device 340, a control device 380, and a communication interface 390. Note that Fig. 8 shows the 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 such signals, for example, at a frequency of 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 provided separately.

[0070] The drive device 140 includes various devices necessary for the running of the tractor 100A and the driving of the working machine 300, such as, for example, the aforementioned prime mover 102, transmission 103, wheels 104, steering device 106, and coupling device 108. The prime mover 102 includes, for example, an internal combustion engine such as a diesel engine. The drive device 140 may include a traction electric motor instead of or together with 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, 180 including environmental map data. The storage device 150 also stores a computer program that causes the ECUs 160, 170, 180 to execute various operations described later. Such a computer program can be provided to the tractor 100A via a storage medium (for example, a semiconductor memory or an optical disk, etc.) or a telecommunication line (for example, the Internet). Such a computer program may be sold as commercial software.

[0072] The ECU 160 is an electronic circuit that executes processing based on the aforementioned SLAM technology. The ECU 160 includes a self-position estimation module 162 and a map data generation module 164. The ECU 160 is an example of the aforementioned self-position 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 the data or signals repeatedly output from the LiDAR sensor 110 and the IMU 125 while the tractor 100A is running. 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 driving after the environmental map data is generated.

[0073] The self-position estimation module 162 executes operations for estimating the position and orientation of the tractor 100A, i.e., self-position estimation. The map data generation module 164 executes processing 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 it in the storage device 150. The self-position estimation module 162 and the map data generation module 164 may be realized in one circuit or divided into a plurality of circuits.

[0074] When the environmental map data has not been generated yet, while estimating its own position, the ECU 160 can detect the trunks of the tree rows in the environment around the tractor 100A, repeatedly generate local map data indicating the distribution of the detected trunks of the tree rows, and record it in the storage device 150. The ECU 160 can further generate environmental map data for the entire farmland (e.g., vineyard) or a section of the farmland by stitching together the local map data. The environmental map data may be generated for each section of the farmland. The ECU 160 may not detect the trunks of the tree rows at the stage of generating the local map data, but detect the trunks at the stage of generating the final environmental map data and record the trunks in a form that can be distinguished from other objects. During automatic 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. Note that the ECU 160 may determine only the position of the tractor 100A by matching and determine the orientation of the tractor 100A using the signal from the IMU 125.

[0075] The ECU 170 is a circuit that performs the process of determining the path of the tractor 100A. When the environmental map data has not been generated yet, the ECU 170 determines the path that the tractor 100A should travel based on the data or signals output from the LiDAR sensor 110 and the obstacle sensor 130. For example, a path 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 the target path. During automatic driving after the environmental map data has been generated, the ECU 170 determines the target path (hereinafter, also referred to as the "planned travel path") based on the environmental map data or based on an instruction from the user.

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

[0077] ECU 160, 170, and 180 can communicate with each other according to a vehicle bus standard such as CAN (Controller Area Network). In FIG. 8, each of ECU 160, 170, and 180 is shown as an individual block, but each of their functions may be realized by a plurality of ECUs. Alternatively, one in-vehicle computer integrating the functions of ECU 160, 170, and 180 may be provided. The tractor 100A may be provided with an ECU other than ECU 160, 170, and 180, and any number of ECUs may be provided according to the functions.

[0078] The communication IF 190 is a circuit that communicates with the communication IF 390 of the work implement 300. The communication IF 190 executes transmission and reception of signals compliant with a communication control standard such as ISOBUS based on ISO 11783 with the communication IF 390 of the work implement 300. Thereby, it is possible to cause the work implement 300 to execute a desired operation or acquire information from the work implement 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 a farm management system that manages the growth status of crops, the operation status of the tractor 100A, and work records.

[0079] The operation terminal 200 is a terminal for a user to perform 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, the user can perform various operations such as turning on / off the automatic driving mode or the automatic steering mode, setting the initial position of the tractor 100A, setting a route, recording or editing a surrounding map, and the like.

[0080] The drive device 340 in the working machine 300 performs operations necessary for the working machine 300 to perform a predetermined work. The drive device 340 includes a device according to the use of the working 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. The control device 380 causes the drive device 340 to perform various operations in response to a signal transmitted from the tractor 100A via the communication IF 390.

[0082] Next, with reference to FIGS. 9A and 9B, a configuration example of the LiDAR sensor 110 will be described. The LiDAR sensor 110 in the present embodiment is a scanning 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 view of the LiDAR sensor 110 as viewed from the side direction of the tractor 100A. FIG. 9B is a schematic view of the LiDAR sensor 110 as viewed from directly above. The plurality of straight lines extending radially in FIGS. 9A and 9B schematically represent the central axis (or traveling direction) of the laser beam emitted from the LiDAR sensor 110. Each laser beam is collimated into parallel light, but has a divergence angle of several milliradians (for example, 0.1 to 0.2 degrees). For this reason, 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 can be formed 20 meters ahead of the LiDAR sensor 110. In the figure, for simplicity, the divergence of the laser beam is ignored, and only the central axis of the laser beam is shown.

[0083] In the example shown in FIG. 9A, the LiDAR sensor 110 can emit laser beams at different elevation angles from a plurality of laser light sources arranged in the vertical direction. The elevation angle is defined by the angle with respect to the uv plane. In this example, the uv plane is substantially parallel to the horizontal plane. When the ground (ground surface) is inclined with respect to the horizontal plane, the uv plane and the horizontal plane intersect. FIG. 9A shows N laser beams L 1 , ···, L N being emitted. Here, "N" is an integer of 1 or more, for example, 10 or more, and in a high-performance model, it can be 64, or 100 or more. Among the plurality of laser beams, let the elevation angle of the k-th laser beam from the bottom be θ k . FIG. 9A shows, as an example, the elevation angle θ N-1 of the (N - 1)-th laser beam. The elevation angle of the laser beam directed upward from the uv plane is defined as a "positive elevation angle", and the elevation angle of the laser beam directed downward from the uv plane is defined as a "negative elevation angle".

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

[0085] As shown in Fig. 9B, the LiDAR sensor 110 can change the emission direction (azimuth angle) of the laser beam. Fig. 9B shows the emission directions of the plurality of laser beams shown in Fig. 9A rotating around a rotation axis parallel to the w-axis. The range of the emission direction (azimuth angle) of the laser beam may be 360°, or may be a range of an angle smaller than 360° (for example, 210° or 270°, etc.). The range of the azimuth angle of the emission direction of the laser beam is called the "horizontal field of view angle". The horizontal field of view angle can be set, for example, within a range of about 90° to 360°. The LiDAR sensor 110 sequentially emits pulsed laser light (laser pulses) in different azimuth directions 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. The operation of measuring the distance to the reflection point while the azimuth angle of the laser beam makes one rotation around the rotation axis is called one scan. The sensor data obtained by one scan includes the 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 for the same environment increases. The LiDAR sensor 110 repeats the scan operation, for example, at a frequency of about 1 to 20 times per second. During one scan operation, for example, more than 100,000 pulses of laser light can be emitted in different directions.

[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 their illustration is omitted. The motor 114 changes the direction of the laser beam emitted from each laser light source 112, for example, by rotating a mirror disposed 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 having 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 around 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 (such as for night 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, all radiation included in the ultraviolet, visible light, and infrared wavelength ranges is 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), for example. The photodetector 113 outputs an electrical signal corresponding to the amount of received light.

[0089] The motor 114 rotates a mirror disposed on the optical path of the laser beam emitted from each laser light source 112 in response to a command from the control circuit 115. Thereby, a scanning operation for changing the emission direction of the laser beam is realized.

[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 rotation operation by the motor 114. The control circuit 115 can be realized by a circuit including a processor, such as a microcontroller unit (MCU) for example.

[0091] The signal processing circuit 116 is a circuit that performs operations 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 by, for example, the Time of Flight (ToF) method. The ToF method includes 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 until the reflected light is received by the photodetector 113. In the indirect ToF method, a plurality of exposure periods are set for the photodetector 113, and the distance to each reflection point is calculated based on the ratio of the light amounts 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, for example, sensor data indicating the distance to each reflection point and the direction of that reflection point. The signal processing circuit 116 may further calculate the coordinates (u, v) or (u, v, w) in the sensor coordinate system based on the distance to each reflection point and the direction of that reflection point, and include them in the sensor data for output.

[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 one circuit.

[0093] Memory 117 is a storage medium that stores data generated by control circuit 115 and signal processing circuit 116. Memory 117 stores, for example, data associating the emission timing of laser pulses emitted from each laser unit 111, the emission direction, the reflected light intensity, the distance to the reflection point, and the coordinates (u, v) or (u, v, w) in the sensor coordinate system. Such data is generated each time a laser pulse is emitted and recorded in memory 117. Control circuit 115 outputs the data at a predetermined period (for example, the time required for a predetermined number of pulse emissions, a half-scan period, or a full-scan period, etc.). The output data is recorded in storage device 150 of tractor 100A.

[0094] Note that 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 varying 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 tractor 100A will be described.

[0096] FIG. 11 is a diagram schematically showing an example of the environment in which tractor 100A travels. In this example, tractor 100A performs a predetermined operation (for example, mowing, control, etc.) using work implement 300 while traveling between a plurality of tree rows 20 in a vineyard. In a vineyard, the sky is blocked by branches and leaves, making it difficult to perform autonomous driving using GNSS. Also, in a vineyard, the shape of the leaves of the trees or the outer shape of the hedges changes greatly depending on the season. Therefore, it is difficult to use the point cloud map created by the conventional SLAM technology throughout the year.

[0097] Therefore, in the embodiments 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, the trees in a vineyard are not replanted for a long time once planted, and the trunk has less change in its outer shape due to seasonal changes compared to the leaf part. By using the trunk as a SLAM landmark, an environmental map that can be used throughout the year can be created, and the automatic driving operation can be performed without creating the environmental map again throughout the year.

[0098] Hereinafter, the operation of the tractor 100A will be specifically described. The ECU 160 first generates environmental map data indicating the distribution of the trunks of the tree rows 20 in the whole vineyard or a section of the vineyard by using the sensor data output from the LiDAR sensor 110, and records it in the storage device 150. The generation of the environmental map data is executed by repeating the self-position estimation by the self-position estimation module 162 and the generation of the local map data by the map data generation module 164. While estimating its own position based on the sensor data repeatedly output from the LiDAR sensor 110 while the tractor 100A is moving, the map data generation module 164 detects the trunks of the tree rows in the environment around the tractor 100A, and repeats the operation of generating local map data indicating the distribution of the detected trunks of the tree rows and recording it in the storage device 150. The map data generation module 164 generates environmental map data by connecting the local map data generated while the tractor 100A travels through the whole vineyard or a section of the vineyard. This environmental map data is data that records the distribution of the trunks of the tree rows 20 in the environment in a form distinguishable from other objects. Thus, in this embodiment, the trunks of the tree rows 20 are used as SLAM landmarks.

[0099] One or more supports may be provided in an area where a plurality of rows of trees are arranged. For example, in a vineyard, generally, a plurality of supports for hedging are provided near the trees. Since the supports, like the trunks of the trees, have little change in their outer shape depending on the season, they are suitable as landmarks. Therefore, the self-position estimation module 162 may further detect the 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, automatic or autonomous driving by the tractor 100A becomes possible. The self-position estimation module 162 of the ECU 160 detects the trunks of the rows of trees 20 in the surrounding environment based on the sensor data repeatedly output from the LiDAR sensor 110 during the driving of the tractor 100A, and estimates the position of the tractor 100A by performing matching between the detected trunks of the rows of trees 20 and the environmental map data. When the environmental map data includes the distribution information of the supports, the self-position estimation module 162 may estimate the position of the tractor 100A by performing matching between the trunks and supports of the rows of trees detected based on the sensor data and the environmental map data. The ECU 180 for drive control 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 deviating from the planned travel route determined by the ECU 170, the ECU 180 performs control to approach the planned travel route by adjusting the steering of the tractor 100A. Such steering control may be performed based not only on the position but also on the orientation of the tractor 100A.

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

[0102] The ECU 160 may generate data indicating the distribution of the trunk parts of the tree row detected based on the sensor data repeatedly acquired during the travel of the tractor 100A, and update the environmental map data using the data. For example, the ECU 160 may update the environmental map data by adding the information on the trunk parts detected from the newly acquired sensor data to the previously generated environmental map data. Alternatively, the data indicating the distribution of the trunk parts of the detected tree row may be transmitted to an external device that updates the environmental map data. In that case, the ECU 160 outputs information indicating the estimated position of the tractor 100A included in the data indicating the distribution of the trunk parts (for example, local map data). The external device can 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 when the travel environment has changed since the environmental map was created due to, for example, the growth of the tree, a new environmental map reflecting the change can be newly created.

[0103] FIG. 12 is a perspective view schematically showing an example of the environment around the tractor 100A. The tractor 100A travels between two adjacent rows of trees in the vineyard. While traveling, the tractor 100A scans the surrounding environment with a laser beam using the LiDAR sensor 110. Thereby, data indicating the distance distribution of the objects existing in the environment is acquired. The data indicating 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 whole vineyard or in a 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 a plurality of reflection points acquired at different timings into coordinates in a world coordinate system fixed to the earth and connecting them. In the present embodiment, the ECU 160 of the tractor 100A generates the environmental map data, but a computer provided outside the tractor 100A may generate the environmental map data.

[0104] FIG. 13A is a diagram schematically showing an example of a travel route of the tractor 100A during environmental map generation and during autonomous driving. The tractor 100A travels, for example, along a route 30 indicated by an arrow in FIG. 13A between a plurality of tree rows 20 in a vineyard. In the figure, the line segments included in the route 30 are depicted as straight lines, but the actual route traveled by the tractor 100A may include a meandering portion. Here, the plurality of tree rows 20 are sequentially numbered as the first tree row 20A, the second tree row 20B, the third tree row 20C, the fourth tree row 20D, ··· from one end. In the example of FIG. 13A, the tractor 100A first travels between the first tree row 20A and the second tree row 20B. When this travel is completed, the tractor turns and travels in the reverse direction between the second tree row 20B and the third tree row 20C. When the travel between the second tree row 20B and the third tree row 20C is completed, the tractor turns further and travels between the third tree row 20C and the fourth tree row 20D. Thereafter, by repeating the same operation, the tractor travels to the end of the route 30 between the last two tree rows. Note that when the distance between adjacent tree rows is short, as shown in FIG. 13B, the tractor may travel skipping one row. In this case, after the travel between the last two tree rows is completed, an operation of traveling skipping one row between the tree rows that have not yet been traveled may be executed. Such travel is automatically performed while the tractor 100A estimates its own position using the LiDAR sensor 110. Note that positioning may be performed based on the GNSS signal at the timing when the GNSS unit 120 can receive the GNSS signal. For example, at the timing of changing direction in the route 30 shown in FIGS. 13A and 13B, positioning based on the GNSS signal is possible because there are no leaves blocking the GNSS signal.

[0105] <Example of sensor data acquisition> Next, with reference to FIGS. 14A to 17C, an example of acquiring sensor data by the tractor 100A will be described. The sensor data is used to estimate the position of the tractor 100A by matching it with the environmental map when the environmental map has already been constructed. On the other hand, when the 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 update of the environmental map do not need to be performed while the tractor 100A is moving, and can be executed by processing the sensor data acquired while the tractor 100A is moving by a computer inside or outside the tractor 100A.

[0106] FIG. 14A is a diagram schematically showing a state in which a tree 21 and the ground are irradiated with laser beams emitted from the LiDAR sensor 110 of the tractor 100A. FIG. 14B is a diagram schematically showing a state in which the ground is irradiated with laser beams emitted from the LiDAR sensor 110. In these figures, the laser beams emitted from the LiDAR sensor 110 in the direction of negative elevation angle are represented by thick broken lines, and the laser beams emitted from the LiDAR sensor 110 in the direction of positive elevation angle are represented by thin broken lines. Among the plurality of laser beams emitted from the LiDAR sensor 110 at different elevation angles, the laser beam emitted in the direction of negative elevation angle can be reflected by the ground if it does not hit the tree 21. On the other hand, the laser beam emitted in the direction of positive elevation angle does not form a reflection point if it does not hit the tree 21, and the reflection point when it hits the tree 21 is often located on the surface of leaves or branches. As described above, the reflection points located on the surface of leaves or branches are not suitable for matching with the environmental map data. Therefore, in the present embodiment, self-position estimation and construction of the environmental map are performed using the reflection on the trunk. Specifically, among the plurality of laser beams emitted from the LiDAR sensor 110 at different elevation angles, the trunk may be detected based on the reflection points of the laser pulses emitted from the laser light sources whose elevation angles are included in a predetermined range. Further, the trunk may be detected based on the reflection points whose heights are lower than the average height of the trunk. In these cases, after detecting the trunk, the reflection points located on the ground may be selectively cut from the point cloud of the sensor data.

[0107] Figures 15A and 15B are plan views schematically showing examples of point clouds of sensor data acquired by the LiDAR sensor 110 of the tractor 100A in one scan operation. Figure 15A schematically shows the tractor 100A at a certain point in time and an example of the distribution of reflection points of laser pulses emitted from the LiDAR sensor 110. 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 laser beams emitted in the negative elevation direction. In Figure 15A, reflection points are arranged on seven arcs (corresponding to seven layers) with different radii. Each layer is formed by a scan in which laser beams emitted at different negative elevation angles rotate in the azimuth direction. When this scan is being performed, a scan by the laser beam may be performed in the horizontal direction or in the positive elevation direction. Note that the reflection points arranged on the same layer are formed by laser beams emitted from the same laser light source at the same elevation angle and incident on the object surface in a pulsed manner while rotating. Figure 15B schematically shows the tractor 100A at a point in time slightly after the time point of Figure 15A and an example of the distribution of reflection points of laser pulses emitted from the LiDAR sensor 110.

[0108] A part of the laser pulses emitted from the LiDAR sensor 110 is reflected by the surface of the trunk 22 of the tree. A part of the laser pulses reflected by the ground, the trunk 22 of the tree, or other objects 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 measurable distance (for example, 50 m, 100 m, or 200 m, etc.). The LiDAR sensor 110 generates and outputs sensor data that associates, for example, 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 forming 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-position estimation 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 that includes information on the three-dimensional coordinates (u, v, w) of each reflection point expressed in the sensor coordinate system fixed to the LiDAR sensor 110. Note that when the LiDAR sensor 110 outputs after converting the distance and direction data of each reflection point into point cloud data, the conversion of the point cloud data by the self-position estimation module 162 is omitted.

[0109] When constructing the environmental map, while the tractor 100A is running, the map data generation module 164 generates local map data that records the trunk 22 in a distinguishable form from other parts from the point cloud data. While the tractor 100A is running, the map data generation module 164 repeats the operation of adding and updating the previously generated local map data with the local map data based on the newly acquired sensor data. Thereby, the final environmental map data can be generated. Note that the map data generation module 164 only performs the operation of generating and recording local map data while the tractor 100A is running, and the generation of the final environmental map data may be performed after the running is completed. In that 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 part of the vehicle body of the tractor 100A. Therefore, as shown in FIGS. 15A and 15B, the laser pulse is radiated forward of the tractor 100A. The LiDAR sensor 110 may also be attached to the rear part 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. Further, the accuracy of self-position estimation by matching in automatic driving after the environmental map is generated can be improved. Furthermore, since point cloud data can be obtained from a wide range on the surface of each individual trunk 22, the thickness of the trunk 22 can also be measured with high accuracy from the point cloud data. Thus, 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] FIG. 16 is a diagram schematically showing an example of a point cloud of the trunk 22 obtained by one scan of the laser beam of a plurality of layers (L 1 , ···, L k+3 ). FIGS. 17A to 17C are diagrams schematically showing how a point cloud of the trunk 22 is obtained by scanning the laser beam of three of the plurality of layers (L k , L k+1 , L k+2 ) shown in FIG. 16, respectively. As shown in this figure, for each scan, point cloud data indicating the distribution of the trunks 22 existing in the measurable range can be obtained. More specifically, when measuring the distance to each individual reflection point by one scan, in each layer, a region where reflection points with a relatively long distance are continuous and a region where reflection points with a relatively short distance appear locally may be included.

[0112] FIG. 18 is a diagram showing a plurality of layers (L k , L k+1 , Lk+2 It is a graph showing an example of the relationship between the distance to the reflection point measured in one scan by a laser beam (···) and the azimuth angle of the reflection point. The azimuth angle range shown in FIG. 18 is narrow, and reflections from two trunks are shown within this range. In this example, the main reflection points of each layer are located on the ground. Among the reflection points belonging to individual layers, the reflection points with relatively short distances are arranged on the curve corresponding to the surface of the trunk. Among the two trunks, the trunk at the relatively short distance position is irradiated with the laser beams of layer L k 、L k+1 、L k+2 、···. On the other hand, the trunk at the relatively long distance position is irradiated with fewer laser beams of layer L k+1 、L k+2 、···. Therefore, when the point cloud on the surface of the trunk in three-dimensional space is projected onto a plane parallel to the horizontal plane, the density of the point cloud of the trunk at the relatively short distance position is relatively high. Based on the measurement results shown in FIG. 18, the trunk can be detected. In reality, since there are undulations or weeds on the ground, the measured distance values are not as simple as shown in the figure.

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

[0114] Note that the trunk can also be detected from the relationship between the intensity of the reflected light and the azimuth angle of the reflection point instead of the distance to the reflection point. This is because the intensity of the reflected light by the laser beam of the same layer increases as the distance to the reflection point becomes shorter.

[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 the tree trunks may include a large number of reflection points other than the reflection points located on the surface of the tree trunks. Therefore, it is effective to select the reflection points that are likely to be located on the surface of the tree trunks from the acquired scan data. Further, the scan data may generate point cloud data with high density by integrating the reflection points obtained by a plurality of continuously acquired scans, not just the reflection points obtained by one scan.

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

[0117] FIG. 19 shows an example of a local map created by extracting the reflection points located on the surface of the tree trunk from the 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 tree trunk is created. In such a local map, the coordinates of the tree trunks located around the tractor 100A at that position can be expressed in the 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, an example of the arrangement of the LiDAR sensor 110 will be described with reference to FIGS. 21A and 21B.

[0121] FIG. 21A shows an example in which the LiDAR sensor 110 is attached to the lower front part 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 part of the cabin 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 a plurality of tree rows existing in the planned travel route of the tractor 100A. The height of the trunk of a tree is the distance from the ground to the portion where the lowest branch occurs in that tree. The LiDAR sensor 110 can be arranged, for example, at a height of 10 cm or more and 150 cm or less from the ground, and in one example, 15 cm or more and 100 cm or less. By arranging the LiDAR sensor 110 at such a low position, more laser beams can irradiate the trunk without being blocked by leaves or branches. Therefore, it becomes 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 about 2 m from the ground), many of the laser beams irradiate the leaf part rather than the trunk. Therefore, such an arrangement as in FIG. 21B is not suitable for the purpose of acquiring point cloud data of the trunk. However, such an arrangement is effective for the purpose of actively acquiring the distribution information of the leaf part of the tree row because it can efficiently acquire the distribution information of the leaf part of the tree row. For example, an arrangement as in FIG. 21B may be adopted for the purpose of managing the growth status of the tree or determining a route that avoids the leaf part of the tree. Even in the arrangement as in FIG. 21B, by attaching the LiDAR sensor 110 downward, more laser beams can irradiate the trunk. Therefore, the arrangement as shown in FIG. 21B may also be adopted for the purpose of acquiring distribution data of the trunk. The LiDAR sensor 110 is not limited to the lower front part of the vehicle body and the upper front part of the cabin, and may be arranged at other positions. For example, the LiDAR sensor 110 can be arranged at a position lower than the height of the cabin.

[0124] In addition, although one LiDAR sensor 110 is used in the present embodiment, the tractor 100A may be equipped with a plurality of LiDAR sensors. By combining the sensor data output from the plurality of LiDAR sensors, the distribution data of the tree trunks can be obtained more efficiently. The plurality of LiDAR sensors may be provided, for example, on the left and right sides of the tractor 100A, or may be provided at the front and rear portions. Details of an embodiment of a moving body equipped with a plurality of LiDAR sensors will be described later.

[0125] The self-position estimation module 162 in the ECU 160 may detect the tree trunks based on the reflection points of the laser pulses emitted from the laser light sources among the plurality of laser light sources in the LiDAR sensor 110 whose elevation angles are within a predetermined range. For example, the tree trunks may be detected based only on the reflection points of the laser pulses emitted in the direction of a negative elevation angle. The user may be able to set the range of the elevation angles of the laser pulses used for trunk detection. For example, the user may be able to set the range of the elevation angles of the laser pulses used for trunk detection by operating the operation terminal 200. By using only the laser pulses emitted within a specific range of elevation angles that are highly likely to irradiate the tree trunks among the laser pulses emitted from the LiDAR sensor 110, the tree trunks can be detected more efficiently. Further, when there are undulations on the ground and the uv plane in the sensor coordinate system is largely inclined from the horizontal plane, the elevation angle range of the laser beam may be adaptively selected to appropriately select the laser pulses reflected by the tree trunks according to the inclination angle of the sensor coordinate system, and the reflection points may be extracted. The inclination angle of the LiDAR sensor 110 can be obtained by using the signal from the IMU 125.

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

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

[0128] The self-position 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 / orientation determination 162f. The details of these processes will be described below.

[0129] (Scan Data Acquisition 162a) The self-position estimation module 162 acquires the scan data output from the LiDAR sensor 110. The LiDAR sensor 110 outputs scan data at a frequency of, for example, about once to 20 times per second. This scan data may include the coordinates of a plurality of points expressed in the sensor coordinate system and time stamp information. In addition, when the scan data includes information on the distance and direction to each point and does not include coordinate information, the self-position estimation module 162 performs conversion from the distance and direction information to 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 shows the distribution of the trunks of the tree rows included in the environment in which the tractor 100A travels. The environmental map data includes data in any of the following forms (1) to (3), for example. (1) Data recorded in a form in which trunks and objects other than trunks can be identified For example, data in which the numerical value "1" is assigned to a point determined to be a trunk and the numerical value "0" is assigned to a point determined to be an object other than a trunk corresponds. The trunk ID for identifying each trunk may be included in the environmental map data. (2) Data with relatively large weights assigned to the trunks and relatively small weights assigned to objects other than the trunks For example, data to which larger numerical values are assigned to points with a higher probability of being estimated as points on the surface of the trunk corresponds. (3) Data that includes information on the distribution of the detected trunks and does not include information on the distribution of some or all of the objects other than the trunks

[0131] For example, data obtained by deleting other points from a point cloud representing an environmental map and leaving only the points determined to be the surface of the trunk corresponds. Not all points determined not to be the trunk need to be deleted; some points may be left. For example, in a vineyard, generally, pillars for fencing are provided around the trunk, so information on the points representing such pillars may be included in the environmental map.

[0132] Whether a certain point in the point cloud corresponds to a trunk or a pillar can be determined, for example, based on whether the distribution of that point and a plurality of points around it reflects the surface shape of the trunk or pillar (e.g., a downwardly convex arc-shaped distribution, etc.). Alternatively, from the data of the point cloud distributed in a curve obtained in each scan, a collection of points with a locally shorter distance from the LiDAR sensor 110 compared to neighboring points may be extracted, and those points may be determined to be points representing the trunk or pillar. The data of the point cloud distributed in a curve obtained in each scan may be classified into a plurality of classes according to the distance from the LiDAR sensor 110, and whether it corresponds to a trunk or a pillar may be determined for each class. Also, the point cloud may be classified based not only on the distance but also on the information of the reflection intensity. 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 with the reflection intensity within a predetermined range and in close proximity may be candidates for points representing the surface of the trunk. Laser beams with a plurality of different wavelengths may be emitted from the LiDAR sensor 110, and the point cloud may be classified based on the ratio of the reflection intensity for each wavelength to detect the trunk.

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

[0134] Not only tree trunks but also other objects may be recognized. For example, by recognizing the ground, weeds, and tree leaves, etc. from the point cloud data by pattern recognition or machine learning and removing those points, point cloud data mainly including points corresponding to the trunk of a tree may be generated. Prior to the detection of the trunk, a process of extracting only the point cloud whose height from the ground is within a predetermined range (for example, from 0.5 m to 1.5 m, etc.) may be performed. By making only the point cloud within such a specific coordinate range the target of trunk detection, the time required for detection can be shortened. 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 can be determined, for example, by referring to a Digital Elevation Model (DEM). The Z coordinate of the ground may also be determined from the point cloud indicating the ground.

[0135] FIG. 23 is a diagram schematically showing an example of the format of environmental map data. The environmental map data in this example includes information on the number of points, and for each point, the ID, classification, trunk ID, X coordinate, Y coordinate, Z coordinate, and reflection intensity information. If the number of points is n, n sets of data combinations of 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 identifiers indicating, for example, the trunk of a tree, the support of a fence, the ground, weeds, the leaves of a tree, etc. The classification may be a binary numerical value indicating whether it is the trunk of a tree or a numerical value indicating the probability that it is the trunk of a tree. 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. Also, 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 represents the intensity of the reflected light from that point. The information on the reflection intensity may not be included in the environmental map data. The environmental map data may include information such as a header indicating the data format in addition to the information shown in FIG. 23. When the vineyard where the tractor 100A travels is large, environmental map data may be generated for each section of the vineyard (for example, for each 100 m × 100 m section). In that case, the environmental map data may also include ID information for identifying the section. Note that the environmental map data may record two-dimensional coordinates instead of recording the three-dimensional coordinates of each point. For example, data including the coordinates of a point group on a plane at a specific height parallel to the horizontal plane may be generated as environmental map data. In that case, the matching between the scan data and the environmental map data is performed with respect to the two-dimensional coordinates.

[0136] (IMU data acquisition 162c) The self-position estimation module 162 acquires the 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, for example, at a frequency of about several tens to several thousands of times per second. This output period is generally shorter than the output period 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, the IMU data is output at a period of 1 millisecond (ms), and the scan data is output at a period of 10 ms. Assuming that the time required for one scan is, for example, 100 ms, in the example of FIG. 24, the scan data is output every one-tenth of a scan. The scan data includes the distance and direction, or coordinates, of each measured point, and information on the measurement time (timestamp).

[0138] The self-position estimation module 162 acquires the IMU data generated during the corresponding time with reference to the timestamp of the acquired scan data.

[0139] (Scan data filtering 162d) The self-position estimation module 162 filters the acquired scan data and reduces the number of points used for matching. Further, it removes the parts unnecessary for matching. For example, points that are determined not to correspond to the trunks of trees or the supports of fences, such as the ground, weeds, leaves of trees, and obstacles, can be removed.

[0140] (Matching 162e) The self-position estimation module 162 performs matching between the filtered scan data and the map data. The matching can be executed using any matching algorithm such as NDT (Normal Distribution Transform) or ICP (Iterative Closest Point). By the matching, the position and orientation of the LiDAR sensor 110 are determined.

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

[0142] Next, while referring to FIG. 25, the self-position estimation operation in this embodiment will be described in more detail.

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

[0144] (Step S101) The self-position estimation module 162 reads the environmental map data from the storage device 150. If 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 can 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 can be identified based on the GNSS signal received by the GNSS unit 120, and the corresponding environmental map data can be selected and read. By reading only a part of the environmental map data corresponding to the position of the tractor 100A as in this example, the processing can be speeded up. Note that all the environmental map data may be read in a batch in this step.

[0145] (Step S102) The self-position estimation module 162 sets the starting point of the position estimation. The starting point of the position estimation is the current position of the tractor 100A at that time, and can be set, for example, by the user designating a specific point from the map displayed on the operation terminal 200. Alternatively, when a GNSS signal can be received at that location, the starting 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-position estimation module 162 reads the scan data output from the LiDAR sensor 110. The LiDAR sensor 110 outputs scan data at a predetermined period (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. The self-position estimation module 162 reads the scan data, for example, every time the scan data is output from the LiDAR sensor 110. Alternatively, the self-position estimation module 162 may read the scan data collectively every time the scan data is output a predetermined number of times.

[0147] (Step S104) The self-position estimation module 162 reads the IMU data corresponding to the scan data with reference to the time stamp included in the scan data.

[0148] (Step S105) The self-position estimation module 162 sets the 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 the matching from this initial position, the time until convergence can be shortened. Instead of using the IMU data, for example, the initial value for matching may be determined by linear interpolation based on the difference in the estimated values of the position and orientation for the past two scans.

[0149] (Step S106) The self-position estimation module 162 filters the acquired scan data and reduces the number of points used for matching. Further, by the method described above, the trunk is detected and at least a part of the unnecessary parts other than the trunk is removed from the point cloud.

[0150] Here, with reference to FIG. 26, an example of the operation of filtering scan data will be described. FIG. 26 schematically shows a part of the point cloud included in the scan data. In FIG. 26, the white circles are the points included in the point cloud. The self-position estimation module 162 divides the three-dimensional space in the sensor coordinate system into a plurality of voxels of a certain size. And when a plurality of points are included in an individual voxel, those points are replaced with one point located at the centroid. In FIG. 26, the replaced centroid points are represented by black circles. By performing such downsampling, the number of points in the scan data can be reduced, and the matching can be speeded up. In the present embodiment, after such downsampling is performed, the detection of the trunk is performed, but the detection of the trunk may be performed before downsampling. When the data size of the point cloud is not a problem, the downsampling process may be omitted.

[0151] (Step S107) The self-position estimation module 162 performs matching between the filtered scan data and the environmental map data, and estimates the pose of the LiDAR sensor 110. Specifically, the pose of the LiDAR sensor 110 is determined by determining the coordinate transformation from the sensor coordinate system to the world coordinate system by 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 the coordinates (x, y, z) and the attitude (θ R , θ P , θ Y) It can be the data of. Note that the environmental map and the scan data represent a point cloud in a two-dimensional space (plane), and when the matching is performed in the two-dimensional space, the output pose data may include the values of two-dimensional coordinates (x, y) and the orientation (θ). Only the position can be estimated by the matching, and the pose information indicated by the IMU data can be directly used for the pose. When the coordinate system of the tractor 100A coincides with the sensor coordinate system, step S108 can be omitted.

[0153] (Step S109) The self-position estimation module 162 determines whether a command to end the operation has been issued. The command to end the operation is issued, for example, when the user uses the operation terminal 200 to instruct the stop of the automatic driving mode 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 ECU 180 for drive control will be described.

[0155] FIG. 27 is a flowchart showing an example of the operation of the ECU 180 that is 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 the pose data (step S121). Next, the ECU 180 calculates the deviation between the position of the tractor 100A indicated by the pose data and the planned travel route determined in advance by the ECU 170 (step S122). The planned travel route can be set, for example, by the 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 or not the calculated position deviation exceeds a preset threshold value (step S123). If the deviation exceeds the threshold value, the ECU 180 changes the drive value (for example, the steering angle) of the drive device 140 so that the deviation becomes smaller. In addition to the steering angle, the speed may be changed. If the deviation does not exceed the threshold value in step S123, step S124 is omitted. In the subsequent step S125, the ECU 180 determines whether or not a command to end the operation has been received. As described above, the command to end the operation is issued, for example, when the user instructs to stop the automatic driving mode 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 S121, and the same operation is executed based on the pose of the tractor 100A estimated based on the next scan data. If the command to end the operation has been issued, the process ends.

[0156] In the example of FIG. 27, the ECU 180 controls the drive device 140 based only on the deviation between the estimated position and the planned travel route, but may also control it taking into account the deviation in azimuth. For example, when the azimuth deviation, which is the angular difference between the estimated direction of the tractor 100A indicated by the acquired pose data and the direction of the planned travel route, exceeds a preset threshold value, the ECU 180 may change the drive value (for example, the steering angle) of the drive device 140 according to the deviation.

[0157] Hereinafter, an example of the steering control by the ECU 180 will be described with reference to FIGS. 28A to 28D.

[0158] FIG. 28A is a diagram showing an example of the tractor 100A traveling along the planned travel route P. FIG. 28B is a diagram showing an example of the tractor 100A at a position shifted to the right from the planned travel route P. FIG. 28C is a diagram showing an example of the tractor 100A at a position shifted to the left from the planned travel route P. FIG. 28D is a diagram showing an example of the tractor 100A facing in a direction inclined 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 has shifted to the right from the planned travel route P, the ECU 180 changes the rotation angle of the steering motor included in the drive device 140 to change the steering angle so that the travel direction of the tractor 100A becomes more to the left. At this time, in addition to the steering angle, the speed may also be changed. The amount of change in the steering angle can be adjusted according to, for example, the magnitude of the position deviation Δx.

[0161] As shown in FIG. 28C, when the position of the tractor 100A has shifted 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 becomes more to the right. Also in this case, in addition to the steering angle, the speed may also be changed. The amount of change in the steering angle can be adjusted according to, for example, the magnitude of the position deviation Δx.

[0162] As shown in FIG. 28D, when the position of the tractor 100A is not significantly deviated from the planned travel route P but the orientation is different from the direction of the planned travel route P, the ECU 180 changes the steering angle so that the azimuth deviation Δθ becomes smaller. Also in this case, the speed may be changed together with the steering angle. The amount of change in the steering angle can be adjusted according to, for example, the magnitudes 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 corresponding to the azimuth deviation Δθ may be made. When the absolute value of the position deviation Δx is large, the steering angle will be changed greatly to return to the planned travel route P, so inevitably the absolute value of the azimuth deviation Δθ will become large. Conversely, when the absolute value of the position deviation Δx is small, since it is necessary to bring the azimuth deviation Δθ closer to zero, it is reasonable to relatively increase the weight of the azimuth deviation Δθ when determining the amount of change in the steering angle.

[0163] For the steering control and speed control of the tractor 100A, control techniques such as PID control or MPC control (model predictive control) can be applied. By applying these control techniques, the control to bring the tractor 100A closer to the planned travel route P can be made smoother.

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

[0165] <Example of Method for Determining Planned Travel Route> Subsequently, an example of a method for determining the 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 the planned travel route of 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 indicating the trunk distribution of a tree row, which is generated based on an environmental map. The map data generation module 164 in the ECU 160 may generate two-dimensional data that roughly shows the distribution of the trunks 22, as shown in FIG. 29A, separately from the environmental map for matching. In this two-dimensional data, the trunks 22 of individual trees are represented by circles of a certain size. Such two-dimensional data showing the trunk distribution may be referred to as "trunk distribution data". The trunk distribution data includes, for example, information on the position coordinates (x, y) of the center of each trunk. In addition to the position coordinates, the trunk distribution data may include information such as the identification number (trunk ID) of each trunk and the thickness of the trunk. By including such information, the growth status of the trunk can also be managed. The growth status of tree trunks or leaves, etc. can be managed, for example, by a computer in an agricultural management system. As in this example, the growth status of trees can be managed with data different from the environmental map. The user may be able to input information indicating the arrangement relationship of a plurality of tree rows, 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 input information is recorded in the storage device 150 or other storage medium.

[0168] FIG. 29B is a diagram for explaining an example of a method for determining a planned travel route 30 based on trunk distribution data. The ECU 170 in this example determines a curve connecting the trunks 22 of each tree row, and determines a curve or a broken line passing through the central part between two adjacent tree rows as the planned travel route 30. It is not necessary for the center of the tractor 100A to always be located at the center between two adjacent trees, and the planned travel route 30 may be shifted from the center between two adjacent trees. The ECU 180 for drive control 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 the backbone 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 can be, for example, a square having a side length close to the average value of the diameter of the backbone 22. The cells are classified into the following four types: (1) to (4). (1) Cells containing the backbone 22 (2) Cells adjacent to the cells containing the backbone 22 (3) Cells located between two adjacent tree rows and not corresponding to (1) and (2) (4) Other cells

[0170] In FIG. 30, the cells of (1) to (4) are represented in different shades. The range of the cells (2) adjacent to the cells containing the backbone 22 may be determined based on the point cloud data showing the foliage of the tree rows acquired by the LiDAR sensor 110. In that case, the map data generation module 164 of the ECU 160 generates grid data showing the two-dimensional distributions of the trunks and foliage of the tree rows 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 data as shown in FIG. 30. For example, the ECU 170 sets the cells of the above (3) located between two adjacent tree rows as a drivable area. The ECU 180 controls the driving of the tractor 100A so as to pass near the central part of the drivable area.

[0171] <Steering control during map data generation> As described above, when creating an environmental map by autonomous driving, the tractor 100A in the present embodiment collects local map data indicating the distribution of the trunks of tree rows while performing self-position estimation and steering control. At this time, the ECU 160 may further detect the foliage of the tree rows in the environment around the tractor 100A based on the sensor data repeatedly output from the LiDAR sensor 110, and further generate data indicating the distribution of the detected foliage of the tree rows. In that case, the ECU 180 can perform steering control of the tractor 100A based on the respective distributions of the detected trunks and foliage of the tree rows. The ECU 180 performs steering control of the tractor 100A, for example, 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 foliage of the tree rows.

[0172] In this operation, the ECU 170 determines a path passing 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, it determines a path passing through the center of the positions of the pair of trunks of two adjacent tree rows. The ECU 180 performs steering control of the tractor 100A so as to move the tractor 100A along the determined path. Thereby, autonomous driving can be performed while collecting local map data.

[0173] In the environment where the tractor 100A travels, the tree rows are not necessarily arranged linearly. There may be cases where the tree rows are arranged in a curve. In such a case, when performing autonomous driving while collecting map data, there may be cases where tree rows are detected only on one side, either left or right. In such a case, since the pair of left and right tree rows cannot be specified, it is difficult to continue autonomous driving by the above method.

[0174] Therefore, when the tree rows are arranged in a curved manner, the following control can be performed. When the tree rows are arranged in a curved manner, for one of two adjacent tree rows, a smaller number of tree trunks are detected than the other of the two tree rows. In such a case, the ECU 170 estimates the distribution of hidden tree trunks in one of the two tree rows based on the detected distribution of tree trunks, and determines the path 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 path.

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

[0176] In such a case, the ECU 170 estimates the positions of the tree trunks of the hidden trees on the left side from the arrangement of the tree rows on the right side where relatively more trees are detected, and determines the travel path based on the estimated positions of the tree trunks. For example, the relative positional relationship between two adjacent front tree trunks 22R1 and 22L1 whose positions are specified based on the scan data is applied to two adjacent rear tree trunks as well, thereby estimating the positions of the hidden tree trunks.

[0177] FIG. 31C is a diagram for explaining an 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 indicating a two-dimensional distribution of trunks of a tree row acquired based on scan data. In FIG. 31C, the positions of the trunks of the trees detected based on the scan data are indicated by solid circles, and the positions of the trunks of the trees not yet detected are indicated by broken circles. 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, starting from the position of the rear trunk 22R2, a position displaced by the amount of 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 path 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 such a manner, the rear trunk 22L2 that was hidden by the trunk 22L1 is eventually detected from the scan data. Assume that the further rear trunk 22L3 has not yet been detected. When the position of the trunk 22L2 is different from the estimated position (cross mark), the ECU 170 identifies the right-side trunk (trunk 22R3 in the example of FIG. 31C) paired with the trunk 22L2, and determines a vector V2 from the trunk 22R3 to the trunk 22L2. Next, starting from the position of the further rear trunk 22R4, a position displaced by the amount of the vector V2 from that position is determined as the position of the hidden trunk 22L3. By repeating the operations as described above, 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 indicating a two-dimensional distribution of trunks when the tree row is arranged in a curved manner. The ECU 160 may generate grid data as shown in FIG. 31D in the process of collecting data for constructing an environmental map. In that case, the ECU 170 may set a route based on such grid data. In this example, the position of the hidden trunk can be estimated in units of the cell size.

[0180] By operating as described above, relatively smooth steering control can be performed while creating an environmental map.

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

[0182] FIG. 32 is a diagram schematically showing an example of a situation where an obstacle 40 and a person 50 exist in the traveling environment of the tractor 100A. The tractor 100A is provided with one or more obstacle sensors 130. During traveling, when the obstacle sensor 130 detects an obstacle 40 (for example, a cage) or a person 50, the ECU 180 performs steering control to avoid the obstacle 40 or the person 50. If a collision with the obstacle 40, the person 50, a branch part, or a trunk part 22 cannot be avoided even after avoidance, the ECU 180 stops the tractor 100A. Note that the ECU 180 may stop the tractor 100A even when the obstacle can be avoided when the obstacle is detected. Similar detection may be performed by the LiDAR sensor 110 instead of the obstacle sensor 130.

[0183] In the example shown in FIG. 32, the person 50 exists near the trunk part 22 of the tree. When matching is performed including the leaf part of the tree, the person 50 or other obstacles may be hidden in the leaf part, making it difficult to distinguish between the leaf part and other objects. In particular, when the person 50 stands in the leaves near the trunk part 22, it is difficult to distinguish. In this embodiment, since matching is performed using the environmental map showing the distribution of the trunk parts 22, it is possible to easily distinguish between the trunk part and other objects. An object other than the trunk part detected by matching the scan data and the environmental map can be easily detected as a person or an obstacle. In this way, the LiDAR sensor 110 can also be used as an obstacle sensor. When the tractor 100A is provided with a plurality of LiDAR sensors 110 at different positions of the tractor 100A, the obstacle sensor 130 may be omitted.

[0184] <Self-position estimation based on a combination of unique trunk intervals> In the above embodiments, it is assumed that the intervals between the trunks of the trees are approximately constant. However, in reality, the intervals between the trunks of the trees may vary depending on the location. Therefore, when performing self-position estimation, the ECU 160 detects the trunks of a plurality of trees having a unique combination of trunk intervals from the sensor data, and estimates the 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. Hereinafter, this operation will be described with reference to FIGS. 33A and 33B.

[0185] FIG. 33A is a diagram showing an example of the arrangement of the trunks 22 of the tree row detected by the tractor 100A. FIG. 33B is a diagram showing an example of the trunk distribution indicated by the environmental map data. In this example, the three trunks surrounded by the dotted line frame have a unique combination of trunk intervals of 1.2 m and 1.5 m. In such a case, when performing self-position estimation, the ECU 160 calculates the intervals between the respective detected trunks 22 from the acquired scan data, and determines the portion having the unique combination of trunk intervals. Then, from the environmental map shown in FIG. 33B, the portion having the determined combination of trunk intervals is searched for and specified. The ECU 160 can estimate the current position and orientation of the tractor 100A by matching the portions having the unique trunk intervals. By such an operation, when the tree row includes a portion having a unique combination of trunk intervals, it is possible to speed up the matching and improve the accuracy of position estimation. Information indicating the combination of trunk intervals in the tree row may be recorded in a storage medium in advance. Such information may be automatically recorded by the ECU 160 based on the environmental map data, 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 also be provided to other tractors or moving bodies other than tractors that travel in the same environment. For example, in a large orchard, when work is performed by a plurality of moving bodies, it is sufficient for one moving body or computer to create an environmental map, and it is efficient to share the environmental map among the plurality of moving bodies. Local maps for constructing an environmental map within one environment may be generated in a shared manner by a plurality of tractors or other moving bodies. In that case, a computer for generating one environmental map by combining the local maps generated by the plurality of moving bodies is provided in the system. The computer may distribute the generated environmental map to the plurality of moving bodies via a wired or wireless network or a storage medium.

[0187] In a system in which a plurality of moving bodies move and work within the same environment, the positional relationship between the sensor coordinate system and the moving body coordinate system can be different for each moving body. Even for the same model, due to errors in the mounting position of the sensor, errors can occur in the conversion from the sensor coordinate system to the moving body coordinate system. In order to reduce the influence of such errors, each moving body executes calibration to determine parameters for coordinate conversion from the sensor coordinate system to the moving body coordinate system by a test run before starting normal operation.

[0188] The technology of this embodiment is not limited to tractors traveling in orchards such as vineyards, but can be applied to any moving body (for example, mobile robots, drones, etc.) used in an environment where there are a plurality of tree rows, such as forests. This is the same for the following embodiments.

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

[0190] The mobile object in this embodiment is equipped with at least two LiDAR sensors, and performs self-position estimation and generation of map data based on the sensor data output from these 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 object. Similar to Embodiment 1, the mobile object includes a storage device that stores environmental map data indicating the distribution of the trunks of a plurality of tree rows, a self-position estimation device, and a control device that controls the movement of the mobile object according to the position of the mobile object estimated by the self-position estimation device. The self-position estimation device detects the trunks of the tree rows in the environment around the mobile object based on the point cloud data repeatedly output from at least two sensors while the mobile object is moving, and estimates the position of the mobile object by performing matching between the detected trunks of the tree rows and the environmental map data. The mobile object further includes a data generation device that generates environmental map data, or local map data for constructing environmental map data. The data generation device detects the trunks of the tree rows in the environment around the mobile object while performing self-position estimation based on the point cloud data repeatedly output from at least two sensors while the mobile object is moving, and generates local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows, and records it in the storage device. The data generation device can also generate environmental map data in the world coordinate system by integrating the local map data repeatedly generated while the mobile object is moving, and record it in the storage device.

[0191] Hereinafter, taking the case where the mobile object is a tractor traveling in an orchard such as a vineyard as an example, the configuration and operation of this embodiment will be described. In the following description, the description will focus on the differences from Embodiment 1, and the description of overlapping matters will be omitted.

[0192] FIG. 34 is a schematic view of the tractor 100B in the present embodiment as seen from the side direction. This tractor 100B includes a first LiDAR sensor 110A at the lower front of the vehicle body and a second LiDAR sensor 110B at the upper front of the cab. Other points are the same as the configuration shown in FIG. 7. Although the connecting device 108 and the working machine 300 shown in FIG. 7 are not shown in FIG. 34, these can also be used in the present embodiment.

[0193] Each of the first LiDAR sensor 110A and the second LiDAR sensor 110B in the present 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 that emits a laser beam only in the direction of one elevation angle in the example shown in FIG. 9A when N = 1. 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 the two-dimensional coordinates of a plurality of reflection points expressed in the respective sensor coordinate systems of the LiDAR sensors 110A and 110B. By using two two-dimensional LiDARs, the cost 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 where 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 can be arranged, for example, at a height of 10 cm or more and 150 cm or less from the ground, and in one example, 15 cm or more and 100 cm or less. The second LiDAR sensor 110B can be arranged, for example, 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 arranged to emit laser pulses forward of the tractor 100B, similar to the LiDAR sensor 110 in Embodiment 1. The first LiDAR sensor 110A in the example of FIG. 34 emits laser pulses substantially parallel to the ground. When the tractor 100B is located on flat ground, the angle formed between the emission direction of the laser pulses 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 arranged to emit laser pulses obliquely downward in front of the tractor 100B. The second LiDAR sensor 110B in the example of FIG. 34 emits laser pulses in a direction inclined by about 25 degrees from the ground. When the tractor 100B is located on flat ground, the angle formed between the emission direction of the laser pulses emitted from the second LiDAR sensor 110B and the ground can be set within a range of, for example, 10 degrees or more and 45 degrees or less.

[0197] FIG. 35 is a block diagram showing an example of the schematic configuration of the tractor 100B in the present embodiment. In the configuration example shown in FIG. 35, the tractor 100B is different from the configuration shown in FIG. 8 in that it includes two LiDAR sensors 110A and 110B.

[0198] The ECU 160 functions as the above-described self-position estimation device and data generation device. The ECU 160 acquires input point clouds of a plurality of scans including the latest scan from the point cloud data repeatedly output from the two LiDAR sensors 110A and 110B, and performs matching between the input point clouds of the plurality of scans and the environmental map data. Thereby, the position and orientation of the traveling tractor 100B can be estimated. Note that the ECU 160 may estimate only the position of the tractor 100B by matching, and determine the orientation based on the signal output from the IMU 125.

[0199] In the process of constructing the environmental map, while the tractor 100B is moving, the ECU 160 performs self-position estimation based on the scan data repeatedly output from the two LiDAR sensors 110A and 110B, detects the trunks of the tree rows in the environment around the moving object, and generates local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows, and records it in the storage device 150. While this operation is being performed, the ECU 170 sets a target path passing between the trunks of the tree rows detected based on the scan data. The ECU 180 controls the drive device 140 according to the set target path to drive the tractor 100B along the target path. The ECU 170 may detect obstacles based on the signal output from the obstacle sensor 130 or the scan data output from one or both of the LiDAR sensors 110A and 110B, and set the target path so as to avoid contact with the obstacles. The ECU 160 generates environmental map data by stitching together the repeatedly generated local map data and records it in the storage device 150. Note that the detection of the trunks of the tree rows may be performed at the stage of generating the environmental map data instead of 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-position estimation by matching the detected trunks with the trunks indicated by the environmental map data. The ECU 180 drives the tractor 100B along the driving planned path determined in advance by the ECU 170. Also in this case, the ECU 170 may detect obstacles based on the signal output from the obstacle sensor 130 during the driving of the tractor 100B or the scan data output from one or both of the LiDAR sensors 110A and 110B, and change the driving planned path so as to avoid contact with the obstacles.

[0201] Figures 36A to 36C are diagrams schematically showing the state of the tractor 100B traveling near the tree 21. In the order of FIGS. 36A, 36B, and 36C, the tractor 100B is approaching the tree 21. In these figures, the broken lines represent the laser beams emitted from the LiDAR sensors 110A and 110B. As shown, depending on the distance between the tractor 100B and the tree 21, the position of the irradiation point on the trunk 22 of the tree 21 irradiated by the laser beam emitted from the second LiDAR sensor 110B changes in the height direction. On the other hand, the change in the position of the irradiation point on 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 obtained by multiple scans of these laser beams, it is possible to obtain point cloud data showing the three-dimensional distribution of the reflection points on the surface of the trunk 22, similar to the case of using three-dimensional LiDAR.

[0202] In this embodiment, the ECU 160 may accumulate scan data for a predetermined number of scans (for example, 3 or 5 times) output from each of the LiDAR sensors 110A and 110B, and perform self-position estimation by matching the accumulated scan data with the environmental map data. By doing so, it is possible to perform self-position estimation with high accuracy similar to the case of substantially using three-dimensional LiDAR.

[0203] While the tractor 100B is traveling to acquire map data, the ECU 170 determines a local target path of the tractor 100B based on the scan data for several scans output from one or both of the LiDAR sensors 110A and 110B. For example, from the arrangement of the point cloud shown by the scan data for several scans output from one or both of the LiDAR sensors 110A and 110B, the center position between two adjacent trees can be determined, and the local target path of the tractor 100B can be determined to pass through that center position. By setting such a path, the tractor 100B can autonomously travel while maintaining a state where the distances from the left and right tree rows are generally equal.

[0204] The detection of the tree trunks 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 at a relatively low position, often contains a lot of information on the point cloud representing the surface of the tree trunks. Therefore, even when using only the scan data from the first LiDAR sensor 110A, it is possible to obtain the distribution information of the tree trunks.

[0205] Since the second LiDAR sensor 110B in the present embodiment is arranged at a relatively high position, it is suitable for detecting the foliage of the tree row. Therefore, the ECU 160 may generate data indicating the distribution of the foliage of the tree row based on the point cloud data acquired by the second LiDAR sensor 110B and record it in the storage device 150 or another storage medium. Such data can be used for managing the growth status of the trees (for example, canopy management).

[0206] During the travel of the tractor 100B, based on the point cloud data acquired by the second LiDAR sensor 110B, obstacles existing at relatively high positions, such as the foliage of the tree row, may be detected, and operations such as avoidance or stop may be performed. When the obstacle sensor 130 is not provided at the upper front of the tractor 100B, if the second LiDAR sensor 110B is not provided, it is difficult to detect and avoid the leaves or vines of the trees protruding above the path between the tree rows. By providing the second LiDAR sensor 110B, it becomes easier to detect those leaves or vines.

[0207] According to the present embodiment, by using two 2D LiDARs, it is possible to realize the same operation as in Embodiment 1 at a low cost. Note that the number 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. Thus, various modifications are possible for the configuration of the present embodiment. Note that the various techniques described for Embodiment 1 can also be applied as they are in the present embodiment.

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

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

[0210] FIG. 37 is a diagram schematically showing a 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 communicably connected to 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 each of the drones 400 and the tractors 100C is arbitrary. Each drone 400 generates local map data for constructing environmental map data while flying within the environment in which each tractor 100C travels, 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 autonomously travels within the orchard based on the distributed environmental map data.

[0211] FIG. 38 is a block diagram showing a configuration example of this system. The tractor 100C in this example includes the same components as the tractor 100A of Embodiment 1 shown in FIG. 8. However, the ECU 160 in this embodiment does not include a 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 in FIG. 38, the illustration of the working machine connected to and used by the tractor 100C is omitted. Similar to Embodiment 1, the communication IF 190 may communicate with the working machine connected to the tractor 100C.

[0212] The drone 400 includes a data generation unit 450, a drive device 440, a control device 480, and a communication IF 490. The drive device 440 includes various devices necessary for the flight of the drone 400, such as, for example, an electric motor for driving and a plurality of propellers. The control device 480 controls the operations of the data generation unit 450 and the drive 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 the same function as the LiDAR sensor 110 in Embodiment 1. The data generation device 460 has the same function as the ECU 160 in Embodiment 1. 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 generation device 460 repeatedly generates local map data indicating the distribution of the trunks of a plurality of tree rows based on the scan data repeatedly output from the LiDAR sensor 410 during the flight of the drone 400 and accumulates it in the storage medium. The accumulated local map data is transmitted from the communication IF 490 to the server 500, for example, by an operation by the user.

[0213] The server 500 can 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, for example. 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 local map data acquired from a plurality of drones 400 in a unified coordinate system, and records it in the storage device 550. The processing device 560 distributes the generated environmental map data from the communication IF 590 to a plurality of 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 performs not only the collection of map data but also operations such as seeding, fertilizing, or control, the environmental map data may be distributed to the drone 400. In that case, the drone 400 can autonomously fly while estimating its own position by matching the scan data output from the LiDAR sensor 410 with the environmental map data, and execute a predetermined operation.

[0214] FIG. 39 is a perspective view showing an example of the appearance of the drone 400. This drone 400 includes a data generation unit 450 on the front surface 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 schematically showing an example of the state of data collection work by a plurality of drones 400. FIG. 40 illustrates two drones 400 flying between a plurality of tree rows 20. These drones 400 repeatedly perform an operation of generating local map data indicating the distribution of the trunks by scanning the trunks of the tree rows 20 with a laser beam while autonomously moving at low altitude between the tree rows 20. The method of generating the local map data is the same as the method in Embodiment 1.

[0216] FIG. 41 is a diagram schematically showing a state in which a tree 21 is irradiated with a laser beam emitted from a LiDAR sensor 410 mounted on a drone 400. The drone 400 in the present embodiment is programmed so that the LiDAR sensor 410 flies at an altitude that keeps it at 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 positioned at a height of, for example, 15 cm or more and 100 cm or less from the ground. Based on the sensor data repeatedly output from the LiDAR sensor 410 positioned at such a low altitude, the data generation device 460 detects the trunks of the tree row in the environment around the drone 400 while estimating its own position, and generates local map data showing the distribution of the detected trunks of the tree row and records it in a storage medium. The data generation device 460 acquires input point clouds of a plurality of scans from the sensor data repeatedly output from the LiDAR sensor 410, and performs self-position estimation by matching the input point cloud of at least one scan including the latest scan with the local map data generated before the previous time.

[0217] FIG. 42 is a diagram showing an example of the format of local map data output from each drone 400. The local map data in this example includes a mobile ID which is the identification number of the drone 400, the position (x, y, z) and orientation (θ R , θ P , θ Y ) of the drone 400 in the world coordinate system, and information recorded for each reflection point. The information recorded for each reflection point is the same as the information shown in FIG. 23, but the coordinates of each reflection point are the coordinates (u, v, w) in the sensor coordinate system fixed to the LiDAR sensor 410 of the drone 400. Such local map data can be transmitted from each drone 400 to the server 500. Note that information regarding whether or not a point is a trunk, such as classification and trunk ID, may not be included in the local map data. After the processing device 560 in the server 500 acquires local map data from a plurality of drones 400, it 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, for example, a large orchard.

[0219] In addition, 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. Further, the moving body that generates the local map data does not necessarily have to be a moving body that can move autonomously. For example, 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 the tree rows may generate local map data by a user driving or operating it.

[0220] Without providing the server 500, a moving body such as the drone 400 or the tractor 100C may generate the final environmental map data and provide the environmental map data to other moving bodies. In that case, the environmental map data can be directly transmitted and received by communication between the moving 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 driving, such as a cabin, a driver's seat, a steering wheel, and an operation terminal, do not have to be provided on the tractor. The unmanned tractor may perform operations similar to those in the above-described embodiments by autonomous driving or remote operation by the user of the tractor.

[0222] In the above embodiments, one or more sensors provided in the moving body 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, other types of sensors such as a flash-type LiDAR sensor or an image sensor may be used. Such other types of sensors may be used in combination with a scanning-type LiDAR sensor.

[0223] An apparatus that executes processes necessary for self-position estimation, autonomous movement (or automatic steering), or map data generation in the above-described embodiments can also be attached later to a moving body that does not have those functions. For example, a control unit that controls the operation of a moving body moving between a plurality of tree rows can be attached to and used with the moving body. 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 the trunks of a plurality of tree rows, a self-position 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-position estimation device. The self-position estimation device detects the trunks of the tree rows in the environment around the moving body based on the sensor data repeatedly output from one or more sensors while the moving body is moving, and estimates the position of the moving body by performing matching between the detected trunks of the tree rows and the environmental map data. Also, a data generation unit that generates map data can be attached to and used with a moving body moving between a plurality of tree rows. 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 the trunks of the tree rows in the environment around the moving body while performing self-position estimation based on the 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 indicating the distribution of the detected trunks of the tree rows and records it in the storage device.

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

[0225] [Item 1] A moving body that moves between a plurality of tree rows, one or more sensors that output sensor data indicating the distribution of objects in the environment around the moving body, While the mobile object is moving, based on the sensor data repeatedly output from the one or more sensors, while performing self-position estimation, detect the trunks of the tree rows in the environment around the mobile object, and generate local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows, and record the generated local map data in a storage device. A data generation device, A mobile object comprising

[0226] [Item 2] The environmental map data is Data in which the detected trunks and objects other than the trunks are recorded in a distinguishable format, Data in which a relatively large weight is assigned to the detected trunks and a relatively small weight is assigned to objects other than the trunks, and Data including information on the distribution of the detected trunks and not including information on the distribution of some or all of the objects other than the trunks Including any of The mobile object according to Item 1.

[0227] [Item 3] 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 mobile object according to Item 1 or 2.

[0228] [Item 4] The LiDAR sensor is mounted at a position lower than the average height of the trunks of the tree rows. The mobile object according to Item 3.

[0229] [Item 5] The LiDAR sensor is arranged at a height of 15 cm or more and 100 cm or less from the ground. The mobile object according to Item 4.

[0230] [Item 6] The mobile body according to item 4 or 5, wherein the data generation device obtains input point clouds of a plurality of 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 performs self-position estimation by matching the input point cloud of at least one scan including the latest scan with the local map data generated before the previous time.

[0231] [Item 7] The LiDAR sensor mounted at a position lower than the average height of the trunks of the tree row has a plurality of laser light sources arranged in the vertical direction, and each of the plurality of laser light sources emits laser pulses at different elevation angles. The mobile body according to any one of items 4 to 6, wherein the data generation device detects the trunk based on the reflection points of the laser pulses emitted from the laser light sources whose elevation angles are included in a predetermined range among the plurality of laser light sources.

[0232] [Item 8] The mobile body according to item 7, wherein the data generation device detects the trunk based on the change in the distance from the LiDAR sensor to the reflection point of the laser pulse during the scan by the LiDAR sensor.

[0233] [Item 9] 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 mobile body according to any one of items 3 to 8, wherein the data generation device detects the trunk based on the position of each point in the point cloud data output during a period of one cycle or more, or the distance or angle from each point to the mobile body.

[0234] [Item 10] The data generation device generates the environmental map data based on the local map data. The mobile body according to any one of Items 1 to 9, wherein the environmental map data is updated by adding the information on the trunk portion detected from the newly acquired sensor data to the environmental map data generated last time.

[0235] [Item 11] The mobile body according to any one of Items 1 to 10, wherein the data generation device detects a trunk portion of the tree row in the environment around the mobile body based on the sensor data repeatedly output from the one or more sensors while the mobile body is moving, performs matching between the detected trunk portion of the tree row and the environmental map data, thereby estimating the position of the mobile body, and outputs information indicating the estimated position of the mobile body included in the local map data.

[0236] [Item 12] further comprising an inertial measurement device, wherein the data generation device estimates the position of the mobile body using a signal output from the inertial measurement device, The mobile body according to any one of Items 1 to 11.

[0237] [Item 13] The mobile body according to any one of Items 1 to 12, further comprising a control device that controls the movement of the mobile body based on the distribution of the detected trunk portions of the tree rows.

[0238] [Item 14] The mobile body according to Item 13, wherein when trunk portions of two adjacent tree rows among the plurality of tree rows are detected, the control device performs steering control of the mobile body so as to move the mobile body along a path passing between the trunk portions of the two tree rows.

[0239] [Item 15] For one of two adjacent tree rows among the plurality of tree rows, if the number of tree trunks detected for the one of the two tree rows is less than that of the other of the two tree rows, the control device estimates the distribution of hidden tree trunks in the one of the two tree rows based on the detected distribution of the tree trunks, and performs steering control of the moving body based on the estimated distribution. The moving body according to item 13 or 14.

[0240] [Item 16] The data generation device further detects the foliage 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 further generates data indicating the distribution of the detected foliage of the tree rows. The moving body according to any one of items 1 to 15.

[0241] [Item 17] The data generation device further detects the foliage 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, Based on the respective distributions of the detected tree trunks and foliage of the tree rows, the control device performs steering control of the moving body so that the moving body moves along a path that passes between two adjacent tree rows among the plurality of tree rows and reduces contact with the foliage of the tree rows. The moving body according to any one of items 13 to 16.

[0242] [Item 18] The data generation device further generates grid data indicating the two-dimensional distributions of the tree trunks and foliage of the tree rows, Based on the grid data, the control device determines the path of the moving body. The moving body according to item 17.

[0243] [Item 19] The moving body is a work vehicle capable of autonomous movement. The moving body according to any one of items 1 to 18.

[0244] [Item 20] The mobile body is an unmanned aerial vehicle, The data generation device is the mobile body according to any one of items 1 to 17, which generates the local map data while the mobile body is flying at a position lower than the average height of the trunks of the tree rows.

[0245] [Item 21] The data generation device is the mobile body according to any one of items 1 to 20, which transmits the local map data to an external device that generates the environmental map data.

[0246] [Item 22] The data generation device is the mobile body according to any one of items 1 to 21, which 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.

[0247] [Item 23] One or more support columns are provided in the area where the plurality of tree rows are arranged, The data generation device further detects the support columns in the environment around the mobile body based on the sensor data repeatedly output from the one or more sensors while the mobile body is moving, and generates data indicating the distribution of the detected trunks of the tree rows and the support columns as the environmental map data and records it in the storage device. The mobile body according to any one of items 1 to 22.

[0248] [Item 24] The data generation device is the mobile body according to any one of items 1 to 23, which further generates data indicating the arrangement of the detected tree rows and records it in the storage device.

[0249] [Item 25] A data generation unit used in a mobile body that moves between a plurality of tree rows, One or more sensors that are attached to and used by the mobile body and output sensor data indicating the distribution of objects in the environment around the mobile body, While the mobile body is moving, based on the sensor data repeatedly output from the one or more sensors, while performing self-position estimation, detect the trunks of the tree rows in the environment around the mobile body, and generate local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows, and record the local map data in a storage device. A data generation device A data generation unit comprising

[0250] [Item 26] A method executed by a mobile body moving between a plurality of tree rows, comprising: While the mobile body is moving, obtain sensor data indicating the distribution of objects in the environment around the mobile body from one or more sensors mounted on the mobile body; Based on the obtained sensor data, detect the trunks of the tree rows in the environment around the mobile body; Generate local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows; Record the local map data in a storage device; A method including

[0251] [Item 27] A computer program executed by a computer in a mobile body moving between a plurality of tree rows, the computer program causing the computer to: While the mobile body is moving, obtain sensor data indicating the distribution of objects in the environment around the mobile body from one or more sensors mounted on the mobile body; Based on the obtained sensor data, detect the trunks of the tree rows in the environment around the mobile body; Generate local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows; Record the local map data in a storage device; A computer program causing the above to be executed

[0252] [Item 28] Obtaining the local map data generated by one or more moving bodies, each of which is a moving body according to any one of Items 1 to 24; Generating the environmental map data by integrating the obtained local map data; Recording the environmental map data in a storage device; A method including the above.

[0253] [Item 29] The method according to Item 28, further including distributing the environmental map data to the one or more moving bodies or other moving bodies.

Industrial Applicability

[0254] The technology of the present disclosure can be applied to moving bodies such as tractors, drones, and walking robots that move in an environment where there are multiple tree rows, such as an orchard like a vineyard.

Description of Signs

[0255] 20 Tree rows 22 Trunks of trees 30 Travel route 40 Obstacle 50 Person 60 Network 100 Moving body 100A, 100B Tractors 101 Vehicle body 102 Prime mover (engine) 103 Transmission 104 Wheels 105 Cabin 106 Steering device 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 unit 125 Inertial measurement unit (IMU) 130 Obstacle sensor 140 Driving device 150 Storage device 160 Electronic control unit (SLAM) 162 Self-position 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 Working machine (implement) 340 Driving device 380 Control device 390 Communication interface (IF) 400 Drone 410 LiDAR sensor 440 Driving device 450 Data generation unit 460 Data generation device 480 Control device 490 Communication IF 500 Server 550 Storage device 560 Processing device 590 Communication IF

Claims

1. A moving body that moves between a plurality of rows of trees, one or more sensors that output sensor data indicating the distribution of objects in the environment around the moving body, while performing self-position estimation based on the sensor data repeatedly output from the one or more sensors while the moving body is moving, detecting the trunks of the rows of trees in the environment around the moving body, and generating local map data for generating environmental map data indicating the distribution of the detected trunks of the rows of trees and recording it in a storage device, a data generation device, a control device that controls the movement of the moving body based on the distribution of the detected trunks of the rows of trees, comprising: For one of two adjacent rows of trees among the plurality of rows of trees, if a smaller number of trunks are detected than the other of the two rows of trees, the control device estimates the distribution of hidden trunks in the one of the two rows of trees based on the detected distribution of the trunks, and performs steering control of the moving body based on the estimated distribution. Moving body.

2. The one or more sensors include a LiDAR sensor that outputs three-dimensional point cloud data indicating the distribution of objects in the environment around the moving body as the sensor data, the LiDAR sensor is mounted at a position lower than the average height of the trunks of the rows of trees, the LiDAR sensor has a plurality of laser light sources, the plurality of laser light sources each emit laser pulses at different elevation angles, the data generation device detects the trunks based on the reflection points of the laser pulses emitted from a part of the plurality of laser light sources whose elevation angles are within a predetermined range. The moving body according to claim 1.

3. The environmental map data is data in which the detected trunks and objects other than the trunks are recorded in a distinguishable form, data in which a relatively large weight is assigned to the detected trunks and a relatively small weight is assigned to objects other than the trunks, and data including information on the distribution of the detected trunks and not including information on the distribution of some or all of the objects other than the trunks including any of The moving body according to claim 1 or 2.

4. The moving body according to claim 2, wherein the data generation device detects the trunks based on the reflection points among the reflection points of the laser pulses emitted from the plurality of laser light sources that are at positions lower than the average height of the trunks.

5. Some of the plurality of laser light sources emit the laser pulses downward with respect to the 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 data generation device detects the trunk based on the reflection points of the laser pulses from some of the laser light sources that emit the laser pulses downward with respect to the moving direction of the moving body among the plurality of laser light sources. The moving body according to claim 2 or 4.

6. The moving body according to any one of claims 2, 4, and 5, wherein the LiDAR sensor is disposed at a height of 15 cm or more and 100 cm or less from the ground.

7. The data generation device obtains input point clouds of a plurality of scans from the sensor data repeatedly output from the LiDAR sensor, and performs self-position estimation by performing matching between the input point clouds of the plurality of scans including the latest scan and the local map data generated before the previous time. The moving body according to any one of claims 2, 4 to 6.

8. The data generation device determines the predetermined range based on a setting operation performed by a user using a terminal device. The moving body according to any one of claims 2, 4 to 7.

9. The data generation device detects the trunk based on a change in the distance from the LiDAR sensor to the reflection point of the laser pulse during scanning by the LiDAR sensor. The moving body according to any one of claims 2, 4 to 8.

10. The LiDAR sensor repeatedly outputs the point cloud data at a preset period. The data generation device detects the trunk based on the position of each point in the point cloud data output during a period of one cycle or more, or the distance or angle of each point from the moving body. The moving body according to any one of claims 2, 4 to 9.

11. The data generation device generates the environmental map data based on the local map data, and updates the environmental map data by adding information on the trunk detected from the newly acquired sensor data to the environmental map data generated last time. The moving body according to any one of claims 1 to 10.

12. The data generation device detects the trunks of the tree rows in the environment around the mobile body based on the sensor data repeatedly output from the one or more sensors while the mobile body is moving, and performs matching between the detected trunks of the tree rows and the environmental map data, thereby estimating the position of the mobile body, and outputs the information indicating the estimated position of the mobile body by including it in the local map data. The mobile body according to any one of claims 1 to 11.

13. further comprising an inertial measurement device, The data generation device estimates the position of the mobile body using the signal output from the inertial measurement device. The mobile body according to any one of claims 1 to 12.

14. Further comprising an inertial measurement device, The data generation device, determines the tilt angle of the LiDAR sensor based on the signal output from the inertial measurement device, selects the predetermined range adaptively according to the tilt angle. The mobile body according to any one of claims 2, 4 to 10.

15. When the trunks of two adjacent tree rows among the plurality of tree rows are detected, the control device performs steering control of the mobile body so as to move the mobile body along a path passing between the trunks of the two tree rows. The mobile body according to any one of claims 1 to 14.

16. The data generation device further detects the foliage of the tree rows in the environment around the mobile body based on the sensor data repeatedly output from the one or more sensors while the mobile body is moving, and further generates data indicating the distribution of the detected foliage of the tree rows. The mobile body according to any one of claims 1 to 15.

17. The data generation device further detects the foliage of the tree rows in the environment around the mobile body based on the sensor data repeatedly output from the one or more sensors while the mobile body is moving, The control device performs steering control of the mobile body so as to move the mobile body along a path passing between two adjacent tree rows among the plurality of tree rows and reducing contact with the foliage of the tree rows based on the respective distributions of the detected trunks and foliage of the tree rows. The mobile body according to any one of claims 1 to 16.

18. The data generation device further generates grid data indicating the two-dimensional distributions of the trunks and foliage of the tree rows, The control device determines the path of the moving body based on the grid-like data. The moving body according to claim 17. **Claim 19** The moving body according to any one of claims 1 to 18, wherein the moving body is a work vehicle capable of autonomous movement. **Claim 20** The moving body is a work vehicle capable of autonomous movement, further includes a headlight, and the LiDAR sensor is disposed at a position lower than the headlight. The moving body according to any one of claims 2, 4 to 10, and 14. **Claim 21** The moving body is a drone, and the data generation device generates the local map data while the moving body is flying at a position lower than the average height of the trunks of the tree row. The moving body according to any one of claims 1 to 18. **Claim 22** The data generation device transmits the local map data to an external device that generates the environmental map data. The moving body according to any one of claims 1 to 21. **Claim 23** The data generation device 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 moving body according to any one of claims 1 to 22. **Claim 24** The data generation device further generates data indicating the arrangement of the detected tree row and records it in the storage device. The moving body according to any one of claims 1 to 23. **Claim 25** The data generation device selects from the point cloud indicated by the sensor data (a) a collection of points in a downwardly convex arc shape, or (b) a collection of points where the distance from the LiDAR sensor is locally closer than surrounding points, or (c) a collection of points where the reflection intensity is within a predetermined range and the positions are close, and detects the trunk by extraction. The moving body according to any one of claims 2, 4 to 10, 14, and 20. **Claim 26** The data generation device extracts only the point cloud included in a range having a height from the ground from the point cloud indicated by the sensor data, and detects the trunk based on the extracted point cloud. The moving body according to any one of claims 1 to 25. **Claim 27** The data generation device detects the trunks and struts of the tree row in the environment around the moving body based on the sensor data, and generates local map data for generating environmental map data indicating the distribution of the detected trunks and struts of the tree row and records it in the storage device. The moving body according to any one of claims 1 to 26. **Claim 28** A system used in a moving body that moves between a plurality of tree rows, one or more sensors attached to and used by the moving body, which output sensor data indicating the distribution of objects in the environment around the moving body; while performing self-position estimation based on the sensor data repeatedly output from the one or more sensors while the moving body is moving, detecting the trunks of the tree rows in the environment around the moving body, and generating local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows, and recording the local map data in a storage device; a control device that controls the movement of the moving body based on the distribution of the detected trunks of the tree rows; comprising: when, for one of two adjacent tree rows among the plurality of tree rows, a smaller number of trunks are detected than the other of the two tree rows, the control device estimates the distribution of hidden trunks in the one of the two tree rows based on the detected distribution of the trunks, and performs steering control of the moving body based on the estimated distribution; a system.

29. A method executed by a moving body that moves between a plurality of tree rows, acquiring sensor data indicating the distribution of objects in the environment around the moving body from one or more sensors mounted on the moving body while the moving body is moving; detecting the trunks of the tree rows in the environment around the moving body based on the acquired sensor data; generating local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows; recording the local map data in a storage device; controlling the movement of the moving body based on the distribution of the detected trunks of the tree rows; when, for one of two adjacent tree rows among the plurality of tree rows, a smaller number of trunks are detected than the other of the two tree rows, estimating the distribution of hidden trunks in the one of the two tree rows based on the detected distribution of the trunks, and performing steering control of the moving body based on the estimated distribution; a method including.

Images