Mobile body, control unit, data generation unit, method for controlling the operation of a mobile body, and method for generating data

Abstract

The present invention achieves high-precision positioning in an environment heavily wooded with a plurality of trees. A moving body according to the present invention moves between rows of a plurality of trees. The moving body comprises: two or more LiDAR sensors that output two-dimensional or three-dimensional point group data indicating the distribution of objects in the environment surrounding the moving body; a storage device that stores environment map data indicating the distribution of trunks in the rows of a plurality of trees; 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. While the moving body is moving, the self-position estimation device estimates the position of the moving body by detecting, on the basis of the point group data repeatedly outputted from the two or more LiDAR sensors, trunks in the rows of trees in the environment surrounding the moving body, and matching the detected trunks in the rows of trees with the environment map data.

Description

Technical Field

[0001] The present disclosure relates to a moving body, a control unit, a data generation unit, a method for controlling the operation of a moving body, and a method for generating data.

Background Art

[0002] As next-generation agriculture, research and development of smart agriculture using ICT (Information and Communication Technology) and IoT (Internet of Things) has been promoted. Research and development for the automation and unmanning 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 moving 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 orchards such as vineyards or environments where trees are distributed at high density such as forests, the leaves that grow thickly at the upper part of the trees form a canopy, which acts as an obstacle or a multiple reflector for radio waves from satellites. In such an environment, it is difficult to accurately position using GNSS. In an environment where GNSS cannot be used, it is conceivable to use SLAM (Simultaneous Localization and Mapping) that simultaneously performs 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 in an environment where a large number of trees exist using SLAM. For example, since the distribution of the leaf parts of the trees changes greatly depending on the change of seasons, there is a problem that the map created in the past cannot be continuously used.

Means for Solving the Problems

[0006] A moving body according to an exemplary embodiment of the present disclosure moves between a plurality of tree rows. The moving body includes at least two LiDAR sensors that output two-dimensional or three-dimensional point cloud 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 the plurality of tree rows, a self-position estimation device, and a control device. The self-position estimation device detects the trunks of the tree rows in the environment around the moving body based on the point cloud data repeatedly output from the at least two LiDAR 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. The control device controls the movement of the moving body according to the estimated position of the moving body.

[0007] According to another exemplary embodiment of the present disclosure, a mobile body moves between a plurality of rows of trees. The mobile body includes at least two LiDAR sensors that output two-dimensional or three-dimensional point cloud data indicating the distribution of objects in the environment around the mobile body, and a data generation device. While estimating the position of the mobile body based on the point cloud data repeatedly output from the at least two LiDAR sensors while the mobile body is moving, the data generation device detects the trunks of the tree rows in the environment around the mobile body, and generates local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows, and records the local map data in a storage device.

[0008] 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 in one device, or may be separately arranged in two or more separate devices.

Advantages of the Invention

[0009] According to an embodiment of the present disclosure, automatic steering or autonomous movement of a mobile body can be realized even in an environment where GNSS-based positioning is difficult. 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

[0010]

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

Embodiments for Carrying Out the Invention

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

[0012] 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 (UAVs, so-called drones) such as multicopters. The mobile body can be unmanned or manned.

[0013] A "work vehicle" means a vehicle that can travel while performing specific work at a work site such as a farm field (e.g., 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, ride-on management machines, and ride-on lawn mowers, as well as vehicles used for non-agricultural purposes such as construction work vehicles and snow removal vehicles.

[0014] "SLAM" is a general term for technologies that simultaneously perform self-position estimation and map creation of a mobile body.

[0015] "Self-position estimation" is to estimate the position of the mobile body on the map (e.g., the center-of-gravity position of the mobile body). In self-position estimation by SLAM, usually, the pose of the mobile body is obtained.

[0016] "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, which is a world coordinate system fixed to the earth, and θ is an angle with respect to a 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, which is a world coordinate system, and (θ R , θ P , θ Y ) are the roll, pitch, and yaw angles with respect to their respective reference directions. The posture of the mobile body is (θ R, θ P , θ Y ) is represented by. Roll angle θ R represents the amount of rotation around the axis in the longitudinal direction of the moving body. Pitch angle θ P represents the amount of rotation around the axis in the lateral direction of the moving body. Yaw angle θ Y represents the amount of rotation around the axis in the vertical direction of the moving body. The attitude can also be defined by other angles such as Euler angles or quaternions.

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

[0018] "Autopilot" is to perform the steering of the moving body by the function of the control device without manual operation. Part or all of the control device may be outside the moving body. Communication such as control signals, commands, or data can be performed between the control device outside the moving body and the moving body. During autopilot, other operations such as speed control may be performed manually.

[0019] "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, and ascending, descending, and hovering during flight. Autonomous movement may include detection of obstacles and obstacle avoidance operations.

[0020] "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 temporarily move based on an operator's instruction.

[0021] "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.

[0022] "External sensor" is a sensor that senses the state outside a moving object. 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.

[0023] "Internal sensor" is a sensor that senses the state of a moving object. Internal sensors include a wheel encoder that measures the rotational speed of wheels, an acceleration sensor, and an angular acceleration sensor (e.g., gyroscope). An inertial measurement unit (IMU) includes an acceleration sensor and an angular acceleration sensor and can output signals indicating the movement amount and attitude of a moving object. Information indicating the amount of change in the pose of a moving object acquired by an internal sensor is referred to as "odometry information".

[0024] "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.

[0025] [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).

[0026] First, refer to FIG. 1. FIG. 1 is a diagram schematically showing an example of a moving body 100 moving 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 represented 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 Σ fixed to the moving body 100 V 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, i.e., as it rotates.

[0027] 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 taken as the reference direction of the angle, and the counterclockwise direction is taken as 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.

[0028] 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 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".

[0029] 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

[0030] 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, for example, 10 milliseconds, the distance Δd1 is sufficiently short and Equation 1 holds. By periodically updating the estimated values of Δd1 and Δφ1 as time progresses from t1, t2, t3, ···, 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.

[0031] Next, referring 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.

[0032] <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.

[0033] 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.

[0034] 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.

[0035] 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.

[0036] 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 description, 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 coordinate system by rotating one coordinate system by a known angle and translating it by a known distance.

[0037] 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, r2 can be estimated based on the observation values z1~z7, etc. 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.

[0038] Next, with reference to FIG. 3, a method for 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 transformation) 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

[0039] 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 content of the rotation matrix R and the position vector T is obtained from the pose of the moving object 100.

[0040] (u1’, v1’) obtained by coordinate transformation 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

[0041] 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.

[0042] In the equation of Equation (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 Equation (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 Equation (2) can be obtained. Therefore, by solving the simultaneous equations, the calculated values of the pose r2 (x2, y2, θ2) can be obtained. Since errors are added to actual observations, optimization using the least squares method or the like is performed using a system of simultaneous equations consisting of a large number of equations exceeding three.

[0043] 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.

[0044] 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, it is possible to calculate the position of the moving body 100 (more precisely, the position of the distance measuring sensor). Estimating the position based on such a principle is also included in "self-position estimation".

[0045] <When there is no environmental map data> When there is no environmental map data, based on the observed values obtained while the moving body 100 is moving 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 Σ of the landmarks m1, m2, m3, m4, ··· W The coordinates above constitute 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.

[0046] 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.

[0047] 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.

[0048] 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 that diffuses 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, and thus 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.

[0049] 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 coordinate system Σ V which is represented. The coordinates of the individual reflection points are in the mobile coordinate system Σ V and are converted from the mobile coordinate system Σ W to the world coordinate system Σ W to obtain the coordinates of the individual reflection points in the world coordinate system Σ V As a result, a point cloud map can be constructed. To convert from the mobile coordinate system Σ W to the world coordinate system Σ

[0050] In the SLAM technology, the pose estimation 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 using a satellite positioning system and an inertial guidance 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 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.

[0051] [3. Embodiment] Embodiments of the present disclosure will be described below. However, detailed descriptions may be omitted more than necessary. For example, detailed descriptions of well-known matters and redundant 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. Note that the inventors provide the accompanying drawings and the following description for those skilled in the art to fully understand the present disclosure, and do 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.

[0052] [3-1. Embodiment 1] A moving body according to a first embodiment of the present disclosure will be described.

[0053] The mobile body in this embodiment is used in an environment where a plurality of trees grow thickly and a plurality of tree rows are formed. The mobile body includes one or more sensors that output sensor data indicating the distribution of objects in the surrounding environment, a storage device, a self-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 the 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 the 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.

[0054] 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.

[0055] 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.

[0056] Hereinafter, taking as an example 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 type LiDAR sensor, the configuration and operation of the present embodiment will be described.

[0057] <Configuration> FIG. 6 is a perspective view showing the appearance of the tractor 100A in the present 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.

[0058] As shown in FIG. 6, the tractor 100A in the present embodiment includes a LiDAR sensor 110, a GNSS unit 120, and one or more obstacle sensors 130. Although one obstacle sensor 130 is shown in the example of FIG. 6, the obstacle sensors 130 can be provided at a plurality of locations on the tractor 100A.

[0059] 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.

[0060] 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 headlamp 109. The effects obtained by disposing the LiDAR sensor 110 at such a low position will be described later.

[0061] 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.

[0062] 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, and the cabin 105 of the vehicle body 101. 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 existence 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 the obstacles around the tractor 100A can be reduced. The LiDAR sensor 110 may function as an obstacle sensor.

[0063] 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 a 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.

[0064] 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.

[0065] 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.

[0066] 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 control the position or attitude of the work implement 300 by raising and lowering the three-point link device by, for example, a hydraulic device. 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.

[0067] The working machine 300 shown in FIG. 7 is a sprayer for spraying chemicals on crops, but the working machine 300 is not limited to a sprayer. For example, any working machine such as a mower, a seeder, a spreader, a rake, a baler, a harvester, a plow, a cultivator, or a rotary can be connected to the tractor 100A and used.

[0068] 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 to be 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.

[0069] FIG. 8 is a block diagram showing an example of the schematic configuration of the tractor 100A and the working machine 300. The tractor 100A in this example includes, in addition to the LiDAR sensor 110, the GNSS unit 120, the obstacle sensor 130, and the operation terminal 200, 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 working 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.

[0070] 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.

[0071] 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 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.

[0072] 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 the environmental map data. The storage device 150 also stores computer programs that cause the ECUs 160, 170, 180 to execute various operations described later. Such computer programs 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 computer programs may be sold as commercial software.

[0073] ECU 160 is an electronic circuit that executes processing based on the aforementioned SLAM technology. ECU 160 includes a self-position estimation module 162 and a map data generation module 164. ECU 160 is an example of the aforementioned self-position estimation device and data generation device. While the tractor 100A is running, 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. Also, during automatic driving after the environmental map data is generated, ECU 160 can 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.

[0074] The self-position estimation module 162 executes calculations for estimating the position and orientation of the tractor 100A, i.e., self-position estimation. The map data generation module 164 executes a process of 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 to construct 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 may be divided into multiple circuits.

[0075] 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 farm (for example, a vineyard) or for one section of the farm by stitching together the local map data. The environmental map data may be generated for each section of the farm. 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.

[0076] 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.

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

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

[0079] 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.

[0080] The operation terminal 200 is a terminal for the 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 switching 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.

[0081] The drive device 340 in the working machine 300 performs operations necessary for the working machine 300 to execute a predetermined work. The drive device 340 includes a device corresponding to the use of the working machine 300, such as a pump, a hydraulic device, or an electric motor.

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

[0083] 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 diagram when the LiDAR sensor 110 is viewed from the side direction of the tractor 100A. FIG. 9B is a schematic diagram when the LiDAR sensor 110 is 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, an optical 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.

[0084] 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 (the surface of the earth) is inclined with respect to the horizontal plane, the uv plane and the horizontal plane intersect. FIG. 9A shows a state in which N laser beams L 1 , ···, L N are 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".

[0085] 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°.

[0086] 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.

[0087] FIG. 10 is a block diagram showing an example of the 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.

[0088] 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 for the photodetector 113. For example, when silicon (Si) is used as the material of the photoelectric conversion element, a wavelength around 900 nm can 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 can be used. Note that the wavelength of the laser beam is not limited to the near-infrared wavelength range. In applications where the influence of ambient light is not a problem (for example, 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".

[0089] The photodetector 113 is a device that detects the 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 according to the received light amount.

[0090] 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.

[0091] 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.

[0092] 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, for example, by the Time of Flight (ToF) method. There are a direct ToF method and an indirect ToF method in the 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 in 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.

[0093] The control circuit 115 and the signal processing circuit 116 are separated into two circuits in the example of FIG. 10, but may be realized by one circuit.

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

[0095] 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 changing 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.

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

[0097] FIG. 11 is a diagram schematically showing an example of the environment in which tractor 100A travels. In this example, tractor 100A travels between a plurality of tree rows 20 in a vineyard while performing a predetermined operation (for example, mowing, control, etc.) using work implement 300. 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.

[0098] 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 appearance due to seasonal changes compared to the leaf part. By using the trunk as a landmark for SLAM, 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.

[0099] 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. The map data generation module 164 detects the trunks of the tree rows in the environment around the tractor 100A while estimating its own position based on the sensor data repeatedly output from the LiDAR sensor 110 while the tractor 100A is moving, and repeatedly performs 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 the present embodiment, the trunks of the tree rows 20 are used as landmarks for SLAM.

[0100] In an area where multiple rows of trees are arranged, one or more support columns may be provided. For example, in a vineyard, generally, multiple support columns for fence construction are provided near the trees. Since the support columns, like the tree trunks, 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 support columns in the environment around the tractor 100A based on the sensor data, and the map data generation module 164 may generate environmental map data showing the distribution of the support columns in addition to the distribution of the trunks.

[0101] When the environmental map data is generated, the tractor 100A can perform automatic or autonomous driving. The self-position estimation module 162 of the ECU 160 detects the trunks of the tree rows 20 in the surrounding environment based on the sensor data repeatedly output from the LiDAR sensor 110 during the driving of the tractor 100A, and estimates the position of the tractor 100A by performing matching between the detected trunks of the tree rows 20 and the environmental map data. When the distribution information of the support columns is included in the environmental map data, the self-position estimation module 162 may estimate the position of the tractor 100A by performing matching between the trunks and support columns of the tree rows 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 on not only the position but also the orientation of the tractor 100A.

[0102] The environmental map showing the distribution of the trunks of the tree row 20 can be generated at any time. For example, the environmental map can be generated in a season 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 trunks 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 trunks from the sensor data output from the LiDAR sensor 110.

[0103] The ECU 160 may generate data showing the distribution of the trunks 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 trunks detected from the newly acquired sensor data to the previously generated environmental map data. Alternatively, the data showing the distribution of the trunks 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 showing the distribution of the trunks (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 the growth of the tree or the like, a new environmental map reflecting the change can be newly created.

[0104] 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 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.

[0105] 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 the route 30 indicated by the arrow in FIG. 13A between a plurality of tree rows 20 in the vineyard. In the figure, the line segments included in the route 30 are shown 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 the end. In the example of FIG. 13A, the tractor 100A first travels between the first tree row 20A and the second tree row 20B, and after the completion of the travel, turns and travels in the reverse direction between the second tree row 20B and the third tree row 20C. After the completion of the travel between the second tree row 20B and the third tree row 20C, it further turns and travels between the third tree row 20C and the fourth tree row 20D. Thereafter, by repeating the same operation, it travels to the end of the route 30 between the last two tree rows. When the distance between adjacent tree rows is short, as shown in FIG. 13B, it may travel skipping one row. In this case, after the completion of the travel between the last two tree rows, 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 the 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.

[0106] <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.

[0107] FIG. 14A schematically shows a state in which the trees 21 and the ground are irradiated with laser beams emitted from the LiDAR sensor 110 of the tractor 100A. FIG. 14B schematically shows 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 a negative elevation angle are represented by thick dashed lines, and the laser beams emitted from the LiDAR sensor 110 in the direction of a positive elevation angle are represented by thin dashed lines. Among the plurality of laser beams emitted from the LiDAR sensor 110 at different elevation angles, the laser beams emitted in the direction of a negative elevation angle can be reflected by the ground if they do not hit the trees 21. On the other hand, the laser beams emitted in the direction of a positive elevation angle do not form reflection points if they do not hit the trees 21, and the reflection points when they hit the trees 21 are often located on the surfaces of the leaves and branches. As described above, the reflection points located on the surfaces of the leaves and branches are not suitable for matching with the environmental map data. For this reason, in the present embodiment, self-position estimation and construction of the environmental map are performed using the reflection on the trunks. 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. Also, 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.

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

[0109] 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 including 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.

[0110] 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 form distinguishable from other parts from the point cloud data. During the running of the tractor 100A, 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 during the running of the tractor 100A, 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.

[0111] 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. In addition, 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 trunk 22, the thickness of the trunk 22 can 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 the 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.

[0112] FIG. 16 is a diagram schematically showing an example of the point cloud of the trunk 22 obtained by one scan of the laser beams of a plurality of layers (L 1 , ···, L k+3 ). FIGS. 17A to 17C are diagrams schematically showing how the point cloud of the trunk 22 is obtained by scanning the laser beams 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 showing 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 relatively long distances are continuous and a region where reflection points with relatively short distances appear locally may be included.

[0113] FIG. 18 is a diagram showing a plurality of layers (L k , L k+1 , Lk+2 This 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 each layer, 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 and weeds on the ground, the measured distance values are not as simple as shown in the figure.

[0114] By repeating the above scan while the tractor 100A is moving, it is possible to obtain data of a high-density point cloud 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 obtain high-density point cloud data necessary for constructing an accurate environmental map showing the distribution of the trunks.

[0115] Note that it is also possible to detect the trunk from the relationship between the intensity of the reflected light and the azimuth angle of the reflection point 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.

[0116] 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. Also, 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.

[0117] 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.

[0118] 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.

[0119] 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.

[0120] By using the scanned data from which such trunks are extracted 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.

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

[0122] FIG. 21A shows an example in which the LiDAR sensor 110 is attached to the lower front of the vehicle body of the tractor 100A as in the present embodiment. FIG. 21B shows an example in which the LiDAR sensor 110 is attached to the upper front of the 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.

[0123] 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.

[0124] On the other hand, in the example of FIG. 21B, since the LiDAR sensor 110 is arranged at a high position (for example, 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, such an arrangement as in FIG. 21B may be adopted for the purpose of managing the growth status of the trees or determining a route that avoids the leaf part of the trees. Even in the arrangement as in FIG. 21B, by attaching the LiDAR sensor 110 downward, more laser beams can irradiate the trunk. Therefore, such an arrangement as shown in FIG. 21B may also be adopted for the purpose of acquiring distribution data of the trunk. The LiDAR sensor 110 may be arranged not only at the lower front part of the vehicle body and the upper front part of the cabin but also at other positions. For example, the LiDAR sensor 110 can be arranged at a position lower than the height of the cabin.

[0125] In this embodiment, one LiDAR sensor 110 is used, but 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 parts. Details of the embodiment of the mobile body equipped with a plurality of LiDAR sensors will be described later.

[0126] 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, the elevation angles of which are included in 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 in a specific range of elevation angles that are likely to irradiate the tree trunks among the laser pulses emitted from the LiDAR sensor 110, the tree trunks can be detected more efficiently. Also, 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.

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

[0128] 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.

[0129] 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.

[0130] (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 about once to twenty times per second, for example. This scan data may include the coordinates of a plurality of points expressed in the sensor coordinate system and time stamp information. When the scan data includes information on the distance and direction to each point but does not include coordinate information, the self-position estimation module 162 performs conversion from the distance and direction information to coordinate information.

[0131] (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 tree rows included in the environment in which the tractor 100A travels. The environmental map data includes data in any of the following formats (1) to (3), for example. (1) Data recorded in a format 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

[0132] 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 of the points determined not to be the trunk need to be deleted, and 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.

[0133] 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 that point reflects the surface shape of the trunk or the pillar (for example, 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 that are locally closer to the LiDAR sensor 110 compared to neighboring points can be extracted, and those points can be determined to be points representing the trunk or the pillar. The data of the point cloud distributed in a curve obtained in each scan can 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 can 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 close positions may be candidates for points representing the surface of the trunk. The LiDAR sensor 110 may emit laser beams of a plurality of different wavelengths, classify the point cloud based on the ratio of the reflection intensity for each wavelength, and detect the trunk.

[0134] 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 tree trunk can be detected from the point cloud data with high accuracy. When a machine learning algorithm is used, creation (i.e., learning) of a trained model for detecting the trunk from the point cloud data is performed in advance.

[0135] Not only tree trunks but also other objects may be recognized. For example, by recognizing the ground, weeds, 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 tree trunk 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 (e.g., 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 be determined from the point cloud indicating the ground.

[0136] 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, an 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, a pillar 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. 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.

[0137] (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.

[0138] 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 the information of the measurement time (timestamp).

[0139] The self-position estimation module 162 refers to the timestamp of the acquired scan data and acquires the IMU data generated during the corresponding time.

[0140] (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 judged not to correspond to the trunks of trees or the pillars of fences, such as the ground, weeds, leaves of trees, and obstacles, can be removed.

[0141] (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.

[0142] (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 is used for the control of the drive device 140.

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

[0144] 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 operation shown in FIG. 25 can be started, for example, by an operation using the operation terminal 200 by the user.

[0145] (Step S101) The self-position estimation module 162 reads the environmental map data from the storage device 150. When different environmental map data is recorded for each section of the vineyard to be traveled, the environmental map data corresponding to the current location is read. The environmental map data corresponding to the current location 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 may be specified based on the GNSS signal received by the GNSS unit 120, and the corresponding environmental map data may 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.

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

[0147] (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.

[0148] (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.

[0149] (Step S105) The self-position estimation module 162 sets the initial position of the matching based on the read IMU data. The initial position of the 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 of the 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.

[0150] (Step S106) The self-position estimation module 162 filters the acquired scan data and reduces the number of points used for the 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.

[0151] 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 accelerated. 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.

[0152] (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.

[0153] (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 , θ YIt can be the data of (). Note that the environmental map and scan data represent a point group in a two-dimensional space (plane), and when matching is performed in the two-dimensional space, the output pose data may include values of two-dimensional coordinates (x, y) and orientation (θ). Only the position can be estimated by matching, and the pose information indicated by the IMU data can be directly used for the pose. If the coordinate system of the tractor 100A coincides with the sensor coordinate system, step S108 can be omitted.

[0154] (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 executed for the next scan data. If the command to end the operation has been issued, the process ends.

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

[0156] FIG. 27 is a flowchart showing an example of the operation of the ECU 180 executed after the pose of the tractor 100A is estimated. When the pose of the tractor 100A is output in step S108 shown in FIG. 25, the ECU 180 acquires 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 also 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, a 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 a 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 a command to end the operation has been issued, the process ends.

[0157] 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.

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

[0159] 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.

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

[0161] As shown in FIG. 28B, when the position of the tractor 100A is 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 leftward. 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.

[0162] As shown in FIG. 28C, when the position of the tractor 100A is 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 rightward. 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.

[0163] As shown in FIG. 28D, when the position of the tractor 100A is not significantly deviated from the planned travel route P but the direction is different from the direction of the planned travel route P, the ECU 180 changes the steering angle so that the azimuth deviation Δθ becomes smaller. Also in this case, the speed may be changed in addition to 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 significantly 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.

[0164] 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.

[0165] 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.

[0166] <Example of method for determining planned travel route> Subsequently, an example of the method for determining the planned travel route of the tractor 100A by the ECU 170 will be described.

[0167] 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.

[0168] FIG. 29A is a diagram showing an example of two-dimensional data indicating the trunk distribution of a row of trees, 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 centers of individual trunks. 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 trunks 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 rows of trees, such as the distance between trees or the position coordinates of the trunks, using a human-machine interface (HMI) such as the operation terminal 200. The input information is recorded in the storage device 150 or other storage media.

[0169] FIG. 29B is a diagram for explaining an example of a method for determining the planned travel route 30 based on the trunk distribution data. The ECU 170 in this example determines a curve connecting the trunks 22 of each row of trees, and determines a curve or a broken line passing through the central part between two adjacent rows of trees as the planned travel route 30. It is not necessary for the center of the tractor 100A to always be located at the central part between two adjacent trees, and the planned travel route 30 may be shifted from the central part 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.

[0170] FIG. 30 is a diagram showing another example of trunk distribution data for route setting. In this example, the driving environment of the tractor 100A is divided into a plurality of cells and managed. Each cell can be, for example, a square having a side length close to the average value of the diameter of the trunk 22. The cells are classified into the following four types (1) to (4). (1) Cells containing the trunk 22 (2) Cells adjacent to the cells containing the trunk 22 (3) Cells located between two adjacent tree rows and not falling under (1) and (2) (4) Other cells

[0171] In FIG. 30, the cells (1) to (4) are represented in different shades. The range of the cells (2) adjacent to the cells containing the trunk 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 lattice 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 lattice data as shown in FIG. 30. For example, the ECU 170 sets the cells (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 center of the drivable area.

[0172] <Steering control during map data generation> As described above, when creating an environmental map by autonomous driving, the tractor 100A in this embodiment collects local map data indicating the distribution of the trunks of a tree row while performing self-position estimation and steering control. At this time, the ECU 160 may further detect the foliage of the tree row in the environment around the tractor 100A based on the sensor data repeatedly output from the LiDAR sensor 110 while the tractor 100A is moving, and further generate data indicating the distribution of the detected foliage of the tree row. 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 row. 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 row.

[0173] 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 row detected based on the sensor data from the LiDAR sensor 110. For example, a path passing through the center of the positions of a pair of trunks of two adjacent tree rows is determined. 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.

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

[0175] 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 the 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.

[0176] FIG. 31A is a diagram schematically showing an example of an environment where tree rows are arranged in a curved manner. The cylinders in the figure represent the tree trunks, the thick dashed lines represent the approximate arrangement of the tree rows, and the dotted lines schematically represent the scans by the laser beams of a plurality of layers 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.

[0177] 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 a relatively large number of 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 to estimate the positions of the hidden tree trunks.

[0178] 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 showing 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 that have not yet been 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.

[0179] 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. If 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.

[0180] FIG. 31D is a diagram showing an example of grid data showing a two-dimensional distribution of trunks when the tree row is arranged in a curved shape. 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.

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

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

[0183] 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 portion, or a trunk portion 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.

[0184] In the example shown in FIG. 32, the person 50 is present near the trunk portion 22 of the tree. When matching is performed including the leaf portion of the tree, the person 50 or other obstacles may be hidden in the leaf portion, making it difficult to distinguish between the leaf portion and other objects. In particular, when the person 50 stands in the leaves near the trunk portion 22, it is difficult to distinguish. In this embodiment, since matching is performed using the environmental map showing the distribution of the trunk portions 22, it is possible to easily distinguish between the trunk portion and other objects. Objects other than the trunk portion 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 locations of the tractor 100A, the obstacle sensor 130 may be omitted.

[0185] <Self-position estimation based on a combination of unique trunk intervals> In the above embodiments, it is assumed that the intervals between the tree trunks are approximately constant. However, in reality, the intervals between the tree trunks can vary depending on the location. Therefore, when performing self-position estimation, the ECU 160 detects the tree 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 tree trunks of the plurality of trees having the unique combination of trunk intervals with the tree 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.

[0186] FIG. 33A is a diagram showing an example of the arrangement of the tree 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 tree 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 interval between each of the detected tree trunks 22 from the acquired scan data, and determines the portion having the unique combination of trunk intervals. Then, the portion having the determined combination of trunk intervals is searched for and specified from the environmental map shown in FIG. 33B. The ECU 160 can estimate the current position and orientation of the tractor 100A by matching the 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.

[0187] 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 traveling 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 by a plurality of tractors or other moving bodies in a shared manner. In that case, a computer that generates 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.

[0188] 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 may be different for each moving body. Even for the same model, an error may occur in the conversion from the sensor coordinate system to the moving body coordinate system due to an error in the mounting position of the sensor. In order to reduce the influence of such an error, 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.

[0189] 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 in the following embodiments.

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

[0191] 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.

[0192] 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.

[0193] FIG. 34 is a schematic view of the tractor 100B in the present embodiment as seen from the side direction. This tractor 100B is provided with a first LiDAR sensor 110A at the lower front part of the vehicle body, and a second LiDAR sensor 110B at the upper front part of the cabin. Other points are the same as the configuration shown in FIG. 7. 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.

[0194] 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 laser beams 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.

[0195] 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).

[0196] 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, for example, within a range of ±20 degrees, and in one example, within a range of ±10 degrees.

[0197] 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 approximately 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, for example, within a range of 10 degrees or more and 45 degrees or less.

[0198] FIG. 35 is a block diagram showing an example of a 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.

[0199] 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.

[0200] 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 body, 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 run 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 connecting 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.

[0201] 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 runs 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 running 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.

[0202] Figures 36A to 36C are diagrams schematically showing the state in which the tractor 100B travels near the tree 21. In the order of FIGS. 36A, 36B, and 36C, the tractor 100B approaches the tree 21. In these figures, the broken line represents the laser beam 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 of the trunk 22 of the tree 21 irradiated by the laser beam emitted from the second LiDAR sensor 110B changes in the height direction. On the other hand, the change in the position of the irradiation point of the trunk 22 of the tree 21 irradiated by the laser beam emitted from the first LiDAR sensor 110A is relatively small. By combining the point cloud data 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.

[0203] In the present embodiment, the ECU 160 may accumulate scan data for a predetermined number of scans (for example, three or five 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.

[0204] 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 so as to pass through the 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.

[0205] The detection of 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 located 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.

[0206] 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 trees (for example, canopy management).

[0207] 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. If the obstacle sensor 130 is not provided at the upper front part 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.

[0208] 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 it 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.

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

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

[0211] 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 the "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 tractors 100C is arbitrary. Each drone 400 generates local map data for constructing environmental map data while flying in the environment where 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 in the orchard based on the distributed environmental map data.

[0212] 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.

[0213] 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.

[0214] Server 500 can be a computer installed at a location away from tractor 100C and drone 400, such as a cloud server or an edge server, for example. Server 500 includes a storage device 550, a processing device 560, and a communication IF 590. Communication IF 590 is a circuit for communicating with tractor 100C and drone 400 via network 60. The processing device 560 generates environmental map data by unifying and integrating local map data obtained 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 performing self-position estimation by matching the scan data output from the LiDAR sensor 410 with the environmental map data, and execute a predetermined operation.

[0215] FIG. 39 is a perspective view showing an example of the appearance of 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.

[0216] 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.

[0217] 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.

[0218] 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 moving body 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 that 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 the trunk based on the characteristics of the distribution of the point cloud.

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

[0220] 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 a row of trees may generate local map data by a user driving or operating it.

[0221] 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.

[0222] [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.

[0223] In the above embodiments, one or more sensors included 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.

[0224] An apparatus that executes the 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 performs self-position estimation based on the sensor data repeatedly output from one or more sensors while the moving body is moving, detects the trunks of the tree rows in the environment around the moving body, 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.

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

[0226] [Item 1] A moving body that moves between a plurality of tree rows, at least two LiDAR sensors that output two-dimensional or three-dimensional point cloud 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 the plurality of tree rows, Based on the point cloud data repeatedly output from the at least two LiDAR sensors while the moving object is moving, detect the trunks of the tree rows in the environment around the moving object, and perform matching between the detected trunks of the tree rows and the environmental map data to estimate the position of the moving object, a self-position estimation device; A control device that controls the movement of the moving object according to the estimated position of the moving object; A moving object comprising the above.

[0227] [Item 2] A moving object that moves between a plurality of tree rows, At least two LiDAR sensors that output two-dimensional or three-dimensional point cloud data indicating the distribution of objects in the environment around the moving object; Based on the point cloud data repeatedly output from the at least two LiDAR sensors while the moving object is moving, while estimating the position of the moving object, detect the trunks of the tree rows in the environment around the moving object, and generate local map data for generating environmental map data indicating the distribution of the detected trunks of the tree rows and record it in a storage device, a data generation device; A moving object comprising the above.

[0228] [Item 3] The moving object according to Item 2, further comprising a control device that controls the movement of the moving object according to the estimated position of the moving object.

[0229] [Item 4] The at least two LiDAR sensors are A first LiDAR sensor mounted at a position lower than the average height of the trunks of the tree rows, A second LiDAR sensor mounted at a position higher than the average height of the trunks of the tree rows, and the moving object according to any one of Items 1 to 3.

[0230] [Item 5] The moving object according to Item 4, wherein the first LiDAR sensor is disposed at a height of 15 cm or more and 100 cm or less from the ground.

[0231] [Item 6] The first LiDAR sensor is arranged to emit laser pulses forward of the moving body, and the second LiDAR sensor is arranged to emit laser pulses obliquely downward in front of the moving body. The moving body according to item 4 or 5.

[0232] [Item 7] The moving body according to any one of items 1 to 6, wherein each of the first LiDAR sensor and the second LiDAR sensor is a two-dimensional LiDAR sensor that outputs two-dimensional point cloud data.

[0233] [Item 8] The self-position estimation device acquires an input point cloud of a plurality of scans including the latest scan from the point cloud data repeatedly output from the at least two LiDAR sensors, and matches the input point cloud of the plurality of scans with the environmental map data, thereby estimating the position of the moving body. The moving body according to any one of items 1 to 7.

[0234] [Item 9] The control device determines whether there is an obstacle on the path of the moving body based on the point cloud data output from at least one of the at least two LiDAR sensors, and when it is determined that there is an obstacle, causes the moving body to execute an operation to avoid colliding with the obstacle. The moving body according to item 1 or 3.

[0235] [Item 10] The at least two LiDAR sensors include a first LiDAR sensor mounted at a position lower than the average height of the trunks of the tree row, and a second LiDAR sensor mounted at a position higher than the average height of the trunks of the tree row. The control device determines whether there is an obstacle on the path of the moving body based on the point cloud data output from the second LiDAR sensor, and when it is determined that there is an obstacle, causes the moving body to execute an operation to avoid colliding with the obstacle. The moving body according to item 1 or 3.

[0236] [Item 11] A control unit that controls the operation of a moving body that moves between a plurality of tree rows, At least two LiDAR sensors that are attached to and used by the moving body and output two-dimensional or three-dimensional point cloud 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 the plurality of tree rows, Based on the point cloud data repeatedly output from the at least two sensors while the moving body is moving, the trunks of the tree rows in the environment around the moving body are detected, and the detected trunks of the tree rows are matched with the environmental map data. A self-position estimation device that estimates the position of the moving body by performing the above, A control device that controls the movement of the moving body according to the estimated position of the moving body, A control unit comprising:

[0237] [Item 12] A data generation unit used in a moving body that moves between a plurality of tree rows, At least two LiDAR sensors that are attached to and used by the moving body and output two-dimensional or three-dimensional point cloud data indicating the distribution of objects in the environment around the moving body, While estimating the position of the moving body based on the point cloud data repeatedly output from the at least two sensors while the moving body is moving, the trunks of the tree rows in the environment around the moving body are detected, and the distribution of the detected trunks of the tree rows is detected. A data generation device that generates local map data for generating environmental map data to be shown and records it in a storage device, A data generation unit comprising:

[0238] [Item 13] A method for controlling the operation of a moving body that moves between a plurality of tree rows, acquiring environmental map data indicating the distribution of the trunks of the plurality of tree rows; while the moving body is moving, obtaining sensor data indicating the distribution of objects in the environment around the moving body from at least two LiDAR sensors mounted on the moving body that output two-dimensional or three-dimensional point cloud data; detecting the trunks of the tree rows in the environment around the moving body based on the obtained sensor data; estimating the position of the moving body by performing matching between the detected trunks of the tree rows and the environmental map data; controlling the movement of the moving body according to the estimated position of the moving body; A method including the above.

[0239] [Item 14] A method executed by a moving body that moves between a plurality of tree rows, while the moving body is moving, obtaining sensor data indicating the distribution of objects in the environment around the moving body from at least two LiDAR sensors mounted on the moving body that output two-dimensional or three-dimensional point cloud data; detecting the trunks of the tree rows in the environment around the moving body based on the obtained 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; A method including the above.

[0240] [Item 15] A computer program executed by a computer for controlling the operation of a moving body that moves between a plurality of tree rows, the computer, Obtaining environmental map data indicating the distribution of the trunks of the plurality of tree rows While the moving body is moving, obtaining sensor data indicating the distribution of objects in the environment around the moving body from at least two LiDAR sensors mounted on the moving body that output two-dimensional or three-dimensional point cloud data Detecting the trunks of the tree rows in the environment around the moving body based on the obtained sensor data Estimating the position of the moving body by performing matching between the detected trunks of the tree rows and the environmental map data Controlling the movement of the moving body according to the estimated position of the moving body A computer program for causing the above to be executed

[0241] [Item 16] A computer program executed by a computer in a moving body that moves between a plurality of tree rows, the computer program causing the computer to: While the moving body is moving, obtaining sensor data indicating the distribution of objects in the environment around the moving body from at least two LiDAR sensors mounted on the moving body that output two-dimensional or three-dimensional point cloud data While the moving body is moving, obtaining sensor data indicating the distribution of objects in the environment around the moving body from at least two LiDAR sensors mounted on the moving body that output two-dimensional or three-dimensional point cloud data Detecting the trunks of the tree rows in the environment around the moving body based on the obtained 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 A computer program for causing the above to be executed

[0242] [Item 17] Obtaining the local map data generated by one or more moving bodies, each of which is the moving body described in item 3 Generating the environmental map data by integrating the obtained local map data Recording the environmental map data in a storage device A method including

[0243] [Item 18] The method according to item 17, further including distributing the environmental map data to the one or more moving bodies or other moving bodies.

Industrial Applicability

[0244] 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 rows of trees, such as an orchard like a vineyard.

Explanation of Signs

[0245] 20 rows of trees Trunks of 22 trees 30 Travel route 40 Obstacles 50 People 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 Connecting 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, comprising: At least two LiDAR sensors that output two-dimensional or three-dimensional point cloud 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 the plurality of rows of trees; Based on the point cloud data repeatedly output from the at least two LiDAR sensors while the moving body is moving, a self-position estimation device that detects the trunks of the rows of trees in the environment around the moving body and performs matching between the detected trunks of the rows of trees and the environmental map data to estimate the position of the moving body; A control device that controls the movement of the moving body according to the estimated position of the moving body; Comprising: For one of two adjacent rows of trees among the plurality of rows of trees, when 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. A moving body that moves between a plurality of rows of trees, comprising: At least two LiDAR sensors that output two-dimensional or three-dimensional point cloud data indicating the distribution of objects in the environment around the moving body; A data generation device that generates local map data for generating environmental map data indicating the distribution of the trunks of the rows of trees in the environment around the moving body while estimating the position of the moving body based on the point cloud data repeatedly output from the at least two LiDAR sensors while the moving body is moving, and records the generated local map data in a storage device; A control device that controls the movement of the moving body according to the estimated position of the moving body; Comprising: For one of two adjacent rows of trees among the plurality of rows of trees, when 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.

3. The at least two LiDAR sensors include: A first LiDAR sensor mounted at a position lower than the average height of the trunks of the rows of trees; a second LiDAR sensor mounted at a position higher than the average height of the trunks of the tree rows; The self-position estimation device extracts point clouds whose height from the ground is within a predetermined range from the point cloud data output from the first LiDAR sensor and the point cloud data output from the second LiDAR sensor, and detects the trunks based on the extracted point clouds. The moving body according to claim 1.

4. The environmental map data further shows the distribution of a plurality of support columns located around the plurality of tree rows. The self-position estimation device detects the trunks and support columns of the tree rows in the environment around the moving body based on the point cloud data, and estimates the position of the moving body by performing matching between the detected trunks and support columns of the tree rows and the environmental map data. The moving body according to claim 1.

5. The data generation device detects the trunks and support columns of the tree rows in the environment around the moving body based on the point cloud data, and generates local map data for generating environmental map data showing the distribution of the detected trunks and support columns of the tree rows, and records the local map data in the storage device. The moving body according to claim 2.

6. The first LiDAR sensor is disposed at a height of 15 cm or more and 100 cm or less from the ground. The moving body according to claim 3.

7. The first LiDAR sensor is arranged to emit laser pulses forward of the moving body. The second LiDAR sensor is arranged to emit laser pulses obliquely downward in front of the moving body. The moving body according to claim 3 or 6.

8. Each of the first LiDAR sensor and the second LiDAR sensor is a two-dimensional LiDAR sensor that outputs two-dimensional point cloud data. The moving body according to any one of claims 3, 6, and 7.

9. The self-position estimation device acquires input point clouds of a plurality of scans including the latest scan from the point cloud data repeatedly output from the at least two LiDAR sensors, and estimates the position of the moving body by performing matching between the input point clouds of the plurality of scans and the environmental map data. The moving body according to claim 1 or 4.

10. The moving body is a work vehicle capable of autonomous movement. It further includes a headlight. The first LiDAR sensor is disposed at a position lower than the headlight. The mobile body according to any one of claims 3, 6 to 8.

11. Based on the point cloud data output from at least one of the at least two LiDAR sensors, the control device determines whether there is an obstacle on the path of the mobile body, and when it is determined that there is an obstacle, the control device causes the mobile body to execute an operation to avoid colliding with the obstacle. The mobile body according to claim 1 or 2.

12. Based on the point cloud data output from the second LiDAR sensor, the control device determines whether there is an obstacle on the path of the mobile body, and when it is determined that there is an obstacle, the control device causes the mobile body to execute an operation to avoid colliding with the obstacle. The mobile body according to any one of claims 3, 6 to 8, 10.

13. A control unit for controlling the operation of a mobile body moving between a plurality of tree rows, At least two LiDAR sensors that are attached to and used by the mobile body and output two-dimensional or three-dimensional point cloud data indicating the distribution of objects in the environment around the mobile body, A storage device that stores environmental map data indicating the distribution of the trunks of the plurality of tree rows, Based on the point cloud data repeatedly output from the at least two LiDAR sensors while the mobile body is moving, the trunks of the tree rows in the environment around the mobile body are detected, and by performing matching between the detected trunks of the tree rows and the environmental map data, a self-position estimation device for estimating the position of the mobile body, A control device that controls the movement of the mobile body according to the estimated position of the mobile body, Comprising, For one of two adjacent tree rows among the plurality of tree rows, when 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 mobile body based on the estimated distribution. Control unit.

14. A control unit used in a mobile body moving between a plurality of tree rows, At least two LiDAR sensors that are attached to and used by the mobile body and output two-dimensional or three-dimensional point cloud data indicating the distribution of objects in the environment around the mobile body, While the moving body is moving, based on the point cloud data repeatedly output from the at least two LiDAR sensors, while estimating the position of the moving body, 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 generated local map data in a storage device; A control device that controls the movement of the moving body according to the estimated position of the moving body; Comprising: For one of two adjacent tree rows among the plurality of tree rows, when 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. Control unit.

15. A method for controlling the operation of a moving body moving between a plurality of tree rows, comprising: Obtaining environmental map data indicating the distribution of the trunks of the plurality of tree rows; While the moving body is moving, obtaining sensor data indicating the distribution of objects in the environment around the moving body from at least two LiDAR sensors mounted on the moving body that output two-dimensional or three-dimensional point cloud data; Detecting the trunks of the tree rows in the environment around the moving body based on the obtained sensor data; Estimating the position of the moving body by matching the detected trunks of the tree rows with the environmental map data; Controlling the movement of the moving body according to the estimated position of the moving body; For one of two adjacent tree rows among the plurality of tree rows, when 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 the above.

16. A method executed by a moving body moving between a plurality of tree rows, comprising: While the moving body is moving, obtaining sensor data indicating the distribution of objects in the environment around the moving body from at least two LiDAR sensors mounted on the moving body that output two-dimensional or three-dimensional point cloud data; Based on the acquired sensor data, detecting the trunks of the tree rows in the environment around the moving body; 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 according to the estimated position of the moving body; For one of two adjacent tree rows among the plurality of tree rows, when 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 the above.

Images