WO2022107586A1 - 移動体、制御ユニット、および移動体の動作を制御する方法 - Google Patents

移動体、制御ユニット、および移動体の動作を制御する方法 Download PDF

Info

Publication number
WO2022107586A1
WO2022107586A1 PCT/JP2021/040298 JP2021040298W WO2022107586A1 WO 2022107586 A1 WO2022107586 A1 WO 2022107586A1 JP 2021040298 W JP2021040298 W JP 2021040298W WO 2022107586 A1 WO2022107586 A1 WO 2022107586A1
Authority
WO
WIPO (PCT)
Prior art keywords
moving body
data
trunk
tractor
sensor
Prior art date
Application number
PCT/JP2021/040298
Other languages
English (en)
French (fr)
Inventor
純一 湯浅
隼輔 宮下
拓也 玉井
俊介 江戸
諒 倉田
Original Assignee
株式会社クボタ
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 株式会社クボタ filed Critical 株式会社クボタ
Priority to EP21894460.1A priority Critical patent/EP4250044A1/en
Priority to JP2022563677A priority patent/JPWO2022107586A1/ja
Publication of WO2022107586A1 publication Critical patent/WO2022107586A1/ja
Priority to US18/198,730 priority patent/US20230288936A1/en

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
    • A01B69/001—Steering by means of optical assistance, e.g. television cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
    • A01B69/007—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow
    • A01B69/008—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow automatic
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector

Definitions

  • the present disclosure relates to a moving body, a control unit, and a method of controlling the operation of the moving body.
  • Patent Documents 1 to 3 disclose an example of a work vehicle that automatically steers based on the result of positioning performed using GNSS.
  • Patent Document 4 discloses an example of a work vehicle that automatically travels between crop rows in a field using LiDAR.
  • SLAM Simultaneous Localization and Mapping
  • the moving object moves between a plurality of tree rows.
  • the moving body includes one or more sensors that output sensor data showing the distribution of objects in the environment around the moving body, and a storage device that stores environmental map data showing the distribution of the trunks of the plurality of tree rows.
  • the self-position estimation device detects the trunk of the tree row in the environment around the moving body based on the sensor data repeatedly output from the one or more sensors while the moving body is moving. Then, the position of the moving body is estimated by matching the detected trunk of the tree row with the environmental map data.
  • the computer-readable recording medium may include a volatile recording medium or a non-volatile recording medium.
  • the device may be composed of a plurality of devices. When the device is composed of two or more devices, the two or more devices may be arranged in one device or may be separately arranged in two or more separated devices. ..
  • the present disclosure it is possible to realize automatic steering or autonomous movement of a moving body even in an environment where positioning by GNSS is difficult.
  • the distribution of the trunk of the tree row has less seasonal change than the distribution of the leaves.
  • FIG. 1 It is a figure which shows the example of the moving body which moves on a plane between time t1 and time t2 schematically. It is a figure which shows the moving body and a plurality of landmarks at time t1. It is a figure which shows the moving body and a plurality of landmarks at time t2. It is a figure which shows the arrangement relation between the moving body coordinate system ⁇ V and the world coordinate system ⁇ W. It is a figure for demonstrating an example of matching. It is a figure for demonstrating an example of matching. It is a perspective view which shows the appearance of the tractor in Embodiment 1. FIG. It is a schematic diagram which looked at the tractor from the side view.
  • FIG. 15 is a diagram schematically showing an example of distribution of a tractor and a reflection point of a laser pulse emitted from a LiDAR sensor at a time slightly elapsed from the time point of FIG. 15A.
  • FIG. 3 Yet another diagram schematically showing how a tractor travels near a tree. It is a figure which shows typically the configuration example of the system of Embodiment 3. It is a block diagram which shows the configuration example of the system of Embodiment 3. It is a perspective view which shows the example of the appearance of a drone. It is a perspective view which shows typically the example of the state of the data collection work by a plurality of drones. It is a figure which shows typically the state that the tree is irradiated by the laser beam emitted from the LiDAR sensor mounted on the drone. It is a figure which shows an example of the format of the local map data output from a drone.
  • a “moving body” is a device equipped with a driving device such as a wheel, a crawler, a bipedal or multi-legged walking device, or a propeller that generates a driving force (traction) for movement.
  • a driving device such as a wheel, a crawler, a bipedal or multi-legged walking device, or a propeller that generates a driving force (traction) for movement.
  • “Mobile” 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. Mobiles can be unmanned or manned.
  • Work vehicle means a vehicle capable of traveling while performing specific work in a work area such as a field (for example, an orchard, a field, a paddy field, or a pasture), a forest, or a construction site.
  • work vehicles include agricultural machinery such as tractors, rice transplanters, combines, passenger management machines, and riding grass mowers, as well as vehicles used in non-agricultural applications such as construction work vehicles and snow removal vehicles.
  • SAM is a general term for technologies that simultaneously estimate the self-position of a moving object and create a map.
  • Self-position estimation is to estimate the position of the moving body on the map (for example, the position of the center of gravity of the moving body).
  • the pose of a moving body is usually obtained.
  • a “pose” is the "position and orientation" of an object.
  • the pose in the two-dimensional space is defined by, for example, three coordinate values (x, y, ⁇ ).
  • (x, y) is a coordinate value in the XY coordinate system, which is a world coordinate system fixed to the earth, and ⁇ is an angle with respect to the reference direction.
  • the pose in the three-dimensional space is defined by, for example, six coordinate values (x, y, z, ⁇ R , ⁇ P , ⁇ Y ).
  • (x, y, z) are coordinate values in the XYZ coordinate system, which is the world coordinate system
  • ( ⁇ R , ⁇ P , ⁇ Y ) are the angles of roll, pitch, and yaw with respect to the respective reference directions.
  • the posture of the moving body is represented by ( ⁇ R , ⁇ P , ⁇ Y ).
  • the roll angle ⁇ R represents the amount of rotation of the moving body around the axis in the front-rear direction.
  • the pitch angle ⁇ P represents the amount of rotation of the moving body around the axis in the left-right direction.
  • the yaw angle ⁇ Y represents the amount of rotation of the moving body around the vertical axis.
  • Posture can also be defined by other angles, such as Euler angles, or by quaternions.
  • Environmental map data is data that expresses the position or area of an object existing in the environment in which a moving object moves by a predetermined coordinate system.
  • Environmental map data may be simply referred to as "environmental map”.
  • Examples of coordinate systems that define an environmental map include not only world coordinate systems such as geographic coordinate systems that are fixed to the earth, but also odometry coordinate systems that display poses based on odometry information.
  • the environmental map data may include information other than the position (for example, attribute information and other information) about an object existing in the environment.
  • Environmental maps include various types of maps, such as point cloud maps or grid maps.
  • the environmental map may be referred to as "map data” or simply “map”.
  • local map or partial map data generated or processed in the process of constructing an environmental map may also be referred to as "map data” or simply "map”.
  • Automatic steering is to steer a moving object by the action of a control device, not manually. Part or all of the control unit may be outside the moving object. Communication such as control signals, commands, or data may be performed between the control device outside the mobile body and the mobile body. Other operations, such as speed control, may be performed manually during automatic steering.
  • Autonomous movement means that a person moves a moving body by the action of a control device while sensing the surrounding environment without being involved in the control of movement.
  • Autonomous movement includes autonomous driving and autonomous flight.
  • the control device can control operations such as steering, speed control, start and stop of running, ascending / descending / hovering during flight, which are necessary for the movement of the moving body.
  • Autonomous movement may include obstacle detection and obstacle avoidance actions.
  • Autonomous driving includes autonomous driving by a control device provided in a moving body and driving based on a command from a computer in an operation management system.
  • Autonomous travel includes not only the movement of the moving object toward the destination along a predetermined route, but also the movement of following the tracking target. Further, it may be temporarily moved based on the instruction of the worker.
  • the "self-position estimation device” is a device that estimates the self-position on the environmental map based on the sensor data acquired by an external sensor such as a LiDAR (Light Detection and Ringing) sensor.
  • an external sensor such as a LiDAR (Light Detection and Ringing) sensor.
  • the “outside world sensor” is a sensor that senses the external state of a moving object.
  • external sensors include, for example, a laser range finder (also referred to as a “range sensor”), a camera (or image sensor), a LiDAR sensor, a millimeter wave radar, and a magnetic sensor.
  • the “inner world sensor” is a sensor that senses the state of a moving object.
  • Internal sensor includes a wheel encoder, an acceleration sensor, and an angular acceleration sensor (eg, a gyroscope) that measures the rotational speed of the wheel.
  • the inertial measurement unit (IMU) includes an acceleration sensor and an angular acceleration sensor, and can output a signal indicating the amount of movement and the attitude of the moving body.
  • the information indicating the amount of change in the pose of the moving body acquired by the internal sensor is called "odometry information”.
  • the "tree trunk” is the woody stem of a woody plant, and is the main shaft part that stands upright on the ground and produces branches. Does not include tree branches, leaves and roots.
  • FIG. 1 is a diagram schematically showing an example of a moving body 100 that moves on a plane between time t1 and time t2.
  • the moving body 100 in this example is a work vehicle (for example, a tractor) traveling 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.
  • FIG. 1 schematically shows the position and orientation of the moving body 100 moving on a plane.
  • the position coordinates of the moving body 100 are indicated by the XY coordinate system.
  • the XY coordinate system is the world coordinate system ⁇ w fixed to the earth.
  • FIG. 1 also shows a mobile coordinate system ⁇ V fixed to the mobile 100.
  • the moving body coordinate system ⁇ V in this example is an uv coordinate system in which the direction in which the front surface of the moving body 100 faces is the u-axis direction, and the direction in which the u-axis is rotated 90 degrees counterclockwise is the v-axis direction.
  • the position of the origin of the moving body coordinate system ⁇ V changes with the movement of the moving body 100.
  • the orientation of the moving body coordinate system ⁇ V changes with the change in the orientation of the moving body 100, that is, with the rotation.
  • r1 be the pose (that is, position and orientation) of the moving body 100 at time t1
  • r2 be the pose of the moving body 100 at time t2.
  • the pose r1 is defined by, for example, a position indicated by coordinates (x1, y1) and a direction indicated by an angle ⁇ 1.
  • the direction of the moving body 100 is defined as the direction in which the front surface of the moving body 100 faces.
  • the positive direction of the X-axis is defined as the reference direction of the angle
  • the counterclockwise direction is defined as the positive direction of the angle.
  • the pose r2 is defined by the position indicated by the coordinates (x2, y2) and the orientation indicated by the angle ⁇ 2.
  • the moving body 100 When the moving body 100 is provided with an internal sensor such as a wheel rotation speed sensor and / or an inertial measurement unit (IMU), it is possible to obtain estimated values of ⁇ d1 and ⁇ 1, that is, odometry information from the internal sensor. can.
  • an internal sensor such as a wheel rotation speed sensor and / or an inertial measurement unit (IMU)
  • IMU inertial measurement unit
  • the estimated value of the subsequent pose of the moving body 100 can be periodically updated based on the odometry information.
  • the initial pose for example (x1, y1, ⁇ 1)
  • the estimated value of the subsequent pose of the moving body 100 can be periodically updated based on the odometry information.
  • errors are accumulated in the estimation of the pose based on the odometry information. For this reason, it is often necessary to use satellite positioning systems or SLAM techniques to obtain highly accurate estimates of the position of the moving body 100.
  • environmental map data indicating the location of the landmark already exists.
  • Such environmental map data includes, for example, an identifier that identifies an individual landmark (for example, a landmark number) and the position coordinates of the landmark associated with each identifier in the world coordinate system ⁇ w .
  • the coordinates of the landmarks m1, m2, m3, and m4 in the world coordinate system ⁇ w are (x m1 , y m1 ), (x m2 , y m2 ), (x m3 , y m3 ), (x m4 , y, respectively). It shall be m4 ).
  • Individual points in the point cloud data acquired by LiDAR may serve as landmarks.
  • An environmental map composed of point cloud data is called a point cloud map.
  • the location of the landmark may be indicated by a cell on the grid map.
  • the observed values (or measured values) of the landmarks m1, m2, m3, and m4 acquired by the moving body 100 in the pose r1 are z1, z2, z3, and z4, respectively.
  • the observed value is, for example, a value indicating the distance and direction to each landmark measured by the external sensor. Since the observed value is data acquired by an external sensor, it may be referred to as "sensor data" in the present disclosure.
  • the observed values of the landmarks m1, m2, and m3 acquired by the moving body 100 in the pose r2 after the moving body 100 moves are set to z5, z6, and z7, respectively.
  • the position coordinates of the landmarks on the moving body coordinate system ⁇ V can be obtained.
  • the moving body coordinate system ⁇ V is a coordinate system fixed to the moving body 100
  • the position coordinates of the same landmark (for example, the landmark m1) on the moving body coordinate system ⁇ V are the moving body. It changes according to the change of 100 poses.
  • the position of the landmark acquired based on the observed value has coordinates in the sensor coordinate system determined by the position and orientation of the external sensor.
  • the sensor coordinate system may be different from the moving body coordinate system ⁇ V , but in the following description, it is assumed that the sensor coordinate system and the moving body coordinate system ⁇ V are the same. Since the relationship between the sensor coordinate system and the moving body coordinate system ⁇ V is known, one coordinate system should be rotated by a known angle and translated by a known distance to match the other coordinate system. Can be done.
  • the moving body 100 moving in the environment acquires the position coordinates of each landmark in the moving body coordinate system ⁇ V by observing a plurality of landmarks. Then, when the position coordinates of each landmark in the world coordinate system ⁇ W are included in the environmental map data, the poses r1 and r2 can be estimated based on the observed values z1 to z7 and the like. Such estimation is possible, for example, by matching the position coordinates of the landmarks obtained from the observed values with the position coordinates of the landmarks included in the environmental map data.
  • FIG. 3 shows the arrangement relationship between the moving body coordinate system ⁇ V and the world coordinate system ⁇ W at time t2.
  • the origin of the moving body coordinate system ⁇ V is made to coincide with the origin of the sensor coordinate system (sensor center), and both coordinate systems are assumed to be the same.
  • the coordinates (u1, v1) on the moving body coordinate system ⁇ V are calculated based on the observed value z5 for the landmark m1 from the moving body 100 in the pose r2 at time t2.
  • the conversion (coordinate conversion) from the coordinates on the moving body coordinate system ⁇ V to the coordinates on the world coordinate system ⁇ W is to rotate the moving body coordinate system ⁇ V clockwise by the angle ⁇ 2, and the moving body coordinate system. This is done by translating the origin of ⁇ V to the origin of the world coordinate system ⁇ W. At this time, the following equations 2 and 3 are established.
  • R is a rotation matrix determined by the direction of the moving body 100
  • T is a position vector of the moving body 100 in the world coordinate system ⁇ w .
  • the contents of the rotation matrix R and the position vector T are obtained from the pose of the moving body 100.
  • the (u1', v1') obtained by the 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 body 100 deviates from the true value, an error (distance) occurs between (u1', v1') and (x m1 , y m1 ). Performing self-position estimation is to determine the contents of the rotation matrix R and the position vector T so that the error between (u1', v1') and (x m1 , y m1 ) is small.
  • FIG. 4 schematically shows the coordinates of the landmarks m1, m2, and m3 on the uv coordinate system, which is the moving body coordinate system ⁇ V at time t2.
  • These coordinates can be calculated from the observed values z5, z6, z7.
  • the coordinates calculated from the observed values z5, z6, and z7 are (u5, v5), (u6, v6), and (u7, v7), respectively.
  • By performing coordinate conversion from the moving body coordinate system ⁇ V to the world coordinate system ⁇ W , (u5, v5), (u6, v6), and (u7, v7) on the moving body coordinate system ⁇ V are respectively.
  • FIG. 5 shows the converted coordinates (u5', v5'), (u6', v6'), (u7', v7') and the corresponding landmark coordinates (x m2 , y m2 ), (x m3 ). , Y m3 ), (x m4 , y m4 ) shows an example of a state in which there is a difference.
  • the coordinate transformation is defined by rotation by rotation matrix R and translation by position vector T.
  • (u5', v5'), (u6', v6'), (u7', v7') are the coordinates of the corresponding landmarks (x m2 , y m2 ), respectively.
  • (X m3 , y m3 ), (x m4 , y m4 ), the contents of the coordinate transformation (R, T) are determined so as to approach (that is, match).
  • the unknowns are x2, y2, and ⁇ 2. Since the number of unknowns is 3, if there are 3 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 the equation 2 are obtained. Therefore, the pose is obtained by solving the simultaneous equations. The calculated value of r2 (x2, y2, ⁇ 2) can be acquired. Since errors are added to actual observations, optimization is performed by the least squares method, etc., using simultaneous equations consisting of a large number of equations exceeding three.
  • Examples of algorithms that estimate self-position by performing optimization by such matching include the ICP matching method and the NDT matching method. Any of these matching methods, or any other method, may be used.
  • the coordinates of the landmarks m1, m2, m3, m4, ... are determined based on the observations acquired by the moving body 100 during movement as shown in FIGS. 2A and 2B. While (that is, estimation), a process of estimating the pose of the moving body 100 is required.
  • the coordinates of the landmarks m1, m2, m3, m4, ... On the world coordinate system ⁇ W constitute environmental map data. Therefore, in order to construct a map, it is necessary to determine the position coordinates of the object that functions as a landmark on the world coordinate system ⁇ W.
  • the moving moving body 100 acquires a plurality of observed values from each of the landmarks m2, m3, and m4. By making more observations, more simultaneous equations can be obtained than the total number of unknowns contained in the pose and landmark coordinates of the moving object 100. Thereby, it is possible to calculate the estimated value of the coordinates of the pose and the landmark of the moving body 100.
  • SLAM algorithms include not only algorithms that utilize LiDAR sensors, but also algorithms that utilize other external sensors such as cameras.
  • 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.
  • the type of SLAM algorithm is not particularly limited.
  • the LiDAR sensor has a scan-type sensor that acquires information on the distance distribution of objects in space by scanning a laser beam, and a scan-type sensor that acquires information on the distance distribution of objects in space using light that diffuses over a wide area.
  • a flash type sensor Since the scan-type LiDAR sensor uses higher intensity light than the flash-type LiDAR sensor, it is possible to acquire distance information at a greater distance.
  • the flash type LiDAR sensor has a simple structure and can be manufactured at low cost, so that it is suitable for applications that do not require strong light. In the present disclosure, an example in which a scan-type LiDAR sensor is mainly used will be described, but a flash-type LiDAR sensor may also be used depending on the application.
  • a pulsed laser beam that is, a laser pulse
  • 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 coordinates of the "reflection point" in the moving body coordinate system ⁇ V can be obtained.
  • Scan-type LiDAR sensors can be classified into two-dimensional LiDAR sensors and three-dimensional LiDAR sensors. According to the two-dimensional LiDAR sensor, the environment can be scanned so that the laser beam rotates in one plane.
  • 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 acquired by these LiDAR sensors are represented by the moving body coordinate system ⁇ V .
  • the coordinates of the individual reflection points on the world coordinate system ⁇ W can be obtained, and as a result, the point cloud map can be obtained.
  • the pose information of the moving body 100 is required as described above.
  • the pose of the moving body 100 may be estimated or measured by using a technique other than the SLAM technique. This is because the pose of the moving body 100 can also be determined by using a satellite positioning system and an inertial guidance device that measure the position of the moving body 100. However, in situations where a positioning system such as a satellite positioning system cannot be used, it is necessary to estimate the pose of the moving object 100 using SLAM technology in order to construct an environmental map.
  • the position of the moving body 100 may be estimated by using SLAM technique, and the direction or posture of the moving body 100 may be estimated by another sensor such as an inertial measurement unit.
  • the moving body in this embodiment is used in an environment in which a plurality of trees are overgrown and a plurality of tree rows are formed.
  • the moving 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 moving body.
  • the storage device stores environmental map data showing the distribution of the trunks of a plurality of tree rows.
  • the self-position estimation device detects the trunk of the tree row 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 the detected tree row.
  • the position of the moving object is estimated by matching the executives with the environmental map data.
  • the control device controls the movement of the moving body according to the estimated position of the moving body.
  • the moving body in the present embodiment further includes an environmental map data or a data generation device for generating local map data for generating the environmental map data.
  • the data generator detects the trunk of a tree row in the environment around the moving object while performing self-position estimation based on sensor data repeatedly output from one or more sensors while the moving object is moving. Then, local map data for generating environmental map data showing the distribution of the detected tree row trunks is generated and recorded in the storage device.
  • the data generation device can also generate environmental map data and record it in the storage device by joining the repeatedly generated local map data.
  • the moving body in the present embodiment has a function of creating an environmental map while moving between a plurality of tree rows, and a function of creating an environmental map while estimating the position of the moving body using the created environmental map. It has a function to move autonomously.
  • One or more sensors may include, for example, at least one LiDAR sensor that outputs two-dimensional or three-dimensional point group data as sensor data.
  • point cloud data broadly means data showing the distribution of a plurality of reflection points observed by a LiDAR sensor.
  • the point group data may include, for example, a coordinate value of each reflection point in a two-dimensional space or a three-dimensional space, and / or information indicating a distance and a direction of each reflection point.
  • the LiDAR sensor repeatedly outputs point cloud data, for example, at a preset cycle.
  • the data generator can detect the trunk based on the position of each point in the point cloud data output during the period of one cycle or more, or the distance or angle of each point from the moving body.
  • the moving body is a work vehicle such as a tractor mainly used for work in an orchard such as a vineyard and the sensor is a scan type LiDAR sensor. do.
  • 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 as viewed from the side surface.
  • FIG. 7 also shows an implementation 300 used in connection with the tractor 100A.
  • 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 sensor 130 may be provided at a plurality of positions of the tractor 100A.
  • the tractor 100A includes a vehicle main body 101, a prime mover (engine) 102, and a transmission (transmission) 103.
  • the vehicle body 101 is provided with wheels (tires) 104 and a cabin 105.
  • the wheel 104 includes a front wheel 104F and a rear wheel 104R.
  • a driver's seat 107, a steering device 106, and an operation terminal 200 are provided inside the cabin 105.
  • One or both of the front wheels 104F and the rear wheels 104R may be crawlers instead of tires.
  • the LiDAR sensor 110 in this embodiment is arranged at the lower part of the front surface of the vehicle body 101.
  • the LiDAR sensor 110 may be arranged at a position lower than the average height of the trunk of the tree row existing in the environment in which 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 may be located, for example, at a position lower than half the height of the tractor 100A.
  • the height of the tractor 100A is the height from the ground to the top of the cabin 105, which can be, for example, 2 meters or more.
  • the LiDAR sensor 110 may be located 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 located above the wheels 104 and below the headlights 109. The effect obtained by arranging the LiDAR sensor 110 at such a low position will be described later.
  • 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 while the tractor 100A is moving.
  • the sensor data output from the LiDAR sensor 110 is processed by a control device such as an ECU (Electronic Control Unit) included in the tractor 100A.
  • the control device can perform processing such as generation of environmental map data based on sensor data and self-position estimation using the environmental map by using the SLAM algorithm described above.
  • the control device may create local map data for generating the environmental map data instead of completing the environmental map data. In that case, the process of integrating the local map data and constructing the environmental map data may be executed by a computer outside the tractor 100A, for example, a cloud server.
  • an obstacle sensor 130 is provided at the rear of the vehicle body 101.
  • the obstacle sensor 130 may be arranged at a portion other than the rear portion of the vehicle body 101.
  • one or more obstacle sensors 130 may be provided at any of the sides, front, and cabin 105 of the vehicle body 101.
  • the obstacle sensor 130 detects an object that is relatively close to the tractor 100A.
  • the obstacle sensor 130 may include, for example, a laser scanner or ultrasonic sonar.
  • the obstacle sensor 130 outputs a signal indicating the existence of an obstacle when an object is present closer than a predetermined distance from the obstacle sensor 130.
  • a plurality of obstacle sensors 130 may be provided at different positions on the vehicle body of the tractor 100A.
  • a plurality of laser scanners and a plurality of ultrasonic sonars may be arranged at different positions on the vehicle body.
  • the LiDAR sensor 110 may function as an obstacle sensor.
  • the GNSS unit 120 is provided in the upper part of the cabin 105.
  • the GNSS unit 120 is a GNSS receiver including an antenna for receiving a signal from a GNSS satellite and a processing circuit.
  • the GNSS unit 120 receives a GNSS signal transmitted from a GNSS satellite such as GPS (Global Positioning System), GLONAS, Galileo, BeiDou, or a quasi-Zenith Satellite System (QZSS, for example, Michibiki). Positioning is performed based on the signal. Since the tractor 100A in the present embodiment is mainly used in an environment where a plurality of trees are overgrown and GNSS is difficult to use, such as a vineyard, positioning is performed using the LiDAR sensor 110. However, in an environment where the GNSS signal 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 the positioning can be improved.
  • GPS Global Positioning System
  • 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 propulsive force of the tractor 100A by shifting.
  • the transmission 103 can also switch between forward and reverse of the tractor 100A.
  • 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 wheel 104F is a steering wheel, and the traveling direction of the tractor 100A can be changed by changing the steering angle thereof.
  • the steering angle of the front wheel 104F can be changed by operating the steering wheel.
  • the steering angle is automatically adjusted by the force of the hydraulic device or the electric motor under the control from the electronic control unit (ECU) arranged in the tractor 100A.
  • ECU electronice control unit
  • 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) axis, and a universal joint.
  • the working machine 300 can be attached to and detached from the tractor 100A by the coupling device 108.
  • the coupling device 108 can control the position or posture of the working machine 300 by raising and lowering the three-point link device by, for example, a hydraulic device. Further, power can be sent from the tractor 100A to the working machine 300 via the universal joint.
  • the tractor 100A can make the working machine 300 perform a predetermined work while pulling the working machine 300.
  • the coupling device may be provided in front of the vehicle body 101.
  • the working machine can be connected in front of the tractor 100A.
  • the LiDAR sensor 110 is used with the working machine connected to the front of the tractor 100A, the LiDAR sensor 110 is arranged at a position where the laser beam is not blocked by the working machine.
  • the working machine 300 shown in FIG. 7 is a sprayer that sprays chemicals on crops, but the working machine 300 is not limited to the sprayer. Tractor any work equipment such as mowers, seeders, spreaders, rakes, balers, harvesters, plows, halos, or rotary. It can be used by connecting to 100A.
  • 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
  • the upward direction is the w-axis direction.
  • the origin of the uvw coordinate system is described as being located at a position away from the tractor 100A, but in reality, the origin of the uvw coordinate system is located at the center of the LiDAR sensor 110 provided in the tractor 100A.
  • FIG. 8 is a block diagram showing an example of a schematic configuration of the tractor 100A and the working machine 300.
  • the tractor 100A in this example includes a LiDAR sensor 110, a GNSS unit 120, an obstacle sensor 130, an operation terminal 200, an inertial measurement unit (IMU) 125, a drive device 140, a storage device 150, and a plurality of electronic control units (ECUs). It comprises 160, 170, 180, and a communication interface (IF) 190.
  • the working machine 300 includes a driving device 340, a control device 380, and a communication interface 390. Note that FIG. 8 shows the components having a relatively high relationship with the self-position estimation operation, the automatic traveling operation, and the map generation operation in the present embodiment, and the illustration of other components is omitted. Has been done.
  • the IMU 125 in this embodiment includes a 3-axis accelerometer and a 3-axis gyroscope.
  • the IMU 125 functions as a motion sensor and can output signals indicating various quantities such as acceleration, velocity, displacement, and attitude of the tractor 100A.
  • the IMU 125 outputs the signal at a frequency of, for example, several tens to several thousand times per second.
  • a 3-axis accelerometer and a 3-axis gyroscope may be provided separately.
  • the drive device 140 includes various devices necessary for traveling the tractor 100A and driving the working machine 300, such as the above-mentioned prime mover 102, transmission 103, wheels 104, steering device 106, and coupling device 108.
  • the prime mover 102 includes an internal combustion engine such as a diesel engine.
  • the drive device 140 may include an electric motor for traction instead of or in combination with the internal combustion engine.
  • the storage device 150 includes a storage medium such as a flash memory or a magnetic disk, and stores various data generated by the ECUs 160, 170, and 180, including environmental map data.
  • the storage device 150 also stores a computer program that causes the ECUs 160, 170, and 180 to execute various operations described later.
  • Such computer programs may be provided to the tractor 100A via a storage medium (eg, semiconductor memory or optical disk, etc.) or a telecommunication line (eg, the Internet).
  • a storage medium eg, semiconductor memory or optical disk, etc.
  • a telecommunication line eg, the Internet
  • the ECU 160 is an electronic circuit that executes processing based on the SLAM technique described above.
  • the ECU 160 includes a self-position estimation module 162 and a map data generation module 164.
  • the ECU 160 is an example of the above-mentioned self-position estimation device and data generation device.
  • the ECU 160 can generate map data while estimating the position and orientation (that is, pose) of the tractor 100A based on the data or signals repeatedly output from the LiDAR sensor 110 and the IMU 125 while the tractor 100A is traveling. can.
  • the ECU 160 can also estimate the position and orientation of the tractor 100A by matching the sensor data output from the LiDAR sensor 110 with the environmental map data during automatic driving after the environmental map data is generated. can.
  • the self-position estimation module 162 executes an operation for estimating the position and orientation of the tractor 100A, that is, 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 for constructing environmental map data. Is included.
  • the map data generation module 164 can generate environmental map data and record it in the storage device 150 by joining the repeatedly generated local map data.
  • the self-position estimation module 162 and the map data generation module 164 may be realized by one circuit or may be divided into a plurality of circuits.
  • the ECU 160 detects the trunk of the tree row in the environment around the tractor 100A while performing self-position estimation, and the local map data showing the distribution of the detected trunk of the tree row. Can be repeatedly generated and recorded in the storage device 150. Further, the ECU 160 can generate environmental map data for the entire field (for example, a vineyard) or a section of the field by connecting the local map data. Environmental map data may be generated for each plot of the field. Even if the ECU 160 does not detect the trunk of the tree row at the stage of generating local map data, but detects the trunk at the stage of generating the final environmental map data, and records the trunk in a format that can be distinguished from other objects. good.
  • 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.
  • the ECU 160 may determine only the position of the tractor 100A by matching, and the direction of the tractor 100A may be determined by using the signal from the IMU 125.
  • the ECU 170 is a circuit that performs a process of determining the route of the tractor 100A.
  • the ECU 170 determines the route on which 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 route that passes between a plurality of tree rows detected based on sensor data output from the LiDAR sensor 110 and avoids obstacles is determined as a target route.
  • the ECU 170 determines a target route (hereinafter, also referred to as "planned travel route”) based on the environment map data or an instruction from the user. ..
  • the 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 working machine 300, and also executes an operation of transmitting the signal from the communication IF 190 to the working machine 300.
  • the ECUs 160, 170, and 180 can communicate with each other according to a vehicle bus standard such as CAN (Controller Area Network).
  • vehicle bus standard such as CAN (Controller Area Network).
  • each of the ECUs 160, 170, and 180 is shown as a separate block, but each of these functions may be realized by a plurality of ECUs.
  • one in-vehicle computer that integrates the functions of the ECUs 160, 170, and 180 may be provided.
  • the tractor 100A may include ECUs other than the ECUs 160, 170, and 180, and any number of ECUs may be provided depending on the function.
  • the communication IF 190 is a circuit that communicates with the communication IF 390 of the working machine 300.
  • the communication IF 190 executes transmission / reception of a signal conforming to a communication control standard such as ISOBUS based on ISO 11783 with the communication IF 390 of the working machine 300.
  • a communication control standard such as ISOBUS based on ISO 11783
  • the communication IF 190 can also communicate with an external computer via a wired or wireless network.
  • a computer such as a server in a farming management system that manages the growth status of crops, the operating status of the tractor 100A, work records, and the like.
  • the operation terminal 200 is a terminal for the user to execute an operation related to automatic traveling or automatic steering of the tractor 100A, and may be referred to as a virtual terminal (VT).
  • the operating terminal 200 may include a display device such as a touch screen and / or one or more buttons.
  • the user can perform various operations such as switching on / off of the automatic driving mode or the automatic steering mode, setting the initial position of the tractor 100A, setting the route, recording or editing the environmental map, and the like. Can be executed.
  • the drive device 340 in the work machine 300 performs an operation necessary for the work machine 300 to perform a predetermined work.
  • the drive device 340 includes a device according to the application of the working machine 300, such as a pump, a hydraulic device, or an electric motor.
  • the control device 380 controls the operation of the drive device 340.
  • the control device 380 causes the drive device 340 to perform various operations in response to the signal transmitted from the tractor 100A via the communication IF 390.
  • the LiDAR sensor 110 in the present embodiment is a scan-type sensor capable of acquiring information on the distance distribution of an object in space by scanning a laser beam.
  • FIG. 9A is a schematic view of the LiDAR sensor 110 when viewed from the side surface of the tractor 100A.
  • FIG. 9B is a schematic view of the LiDAR sensor 110 when viewed from above vertically.
  • 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 to parallel light but has a spread angle of a few milliradians (eg 0.1-0.2 degrees). Therefore, the cross-sectional size (spot diameter) of each laser beam increases in proportion to the distance from the LiDAR sensor 110. For example, a light spot with a diameter of several centimeters can be formed 20 meters away from the LiDAR sensor 110. In the figure, for the sake of simplicity, the spread of the laser beam is ignored and only the central axis of the laser beam is shown.
  • the LiDAR sensor 110 in the example shown in FIG. 9A can emit a laser beam from a plurality of laser light sources arranged in the vertical direction at different elevation angles.
  • the elevation angle is defined by the angle with respect to the UV plane.
  • the uv plane is approximately parallel to the horizontal plane.
  • the ground (ground surface) is inclined with respect to the horizontal plane, the uv plane and the horizontal plane intersect.
  • FIG. 9A shows how N laser beams L 1 , ..., L N are emitted.
  • N is an integer of 1 or more, and may be, for example, 10 or more, and 64 or 100 or more in a high-performance model.
  • the elevation angle of the kth laser beam from the bottom is ⁇ k .
  • FIG. 9A shows, as an example, the elevation angle ⁇ N-1 of the N-1st laser beam.
  • the elevation angle of the laser beam directed above the uv surface is defined as a "positive elevation angle”
  • the elevation angle of the laser beam directed below the uv surface is defined as a "negative elevation angle”.
  • a LiDAR sensor having an N of 1 may be referred to as a "two-dimensional LiDAR", and a LiDAR sensor having an N of 2 or more may be referred to as a "three-dimensional LiDAR".
  • N the angle formed by the first laser beam and the Nth laser beam is referred to as a "vertical viewing angle".
  • the vertical viewing angle can be set, for example, in the range of about 20 ° to 60 °.
  • the LiDAR sensor 110 can change the emission direction (azimuth) of the laser beam.
  • FIG. 9B shows how the emission directions of the plurality of laser beams shown in FIG. 9A rotate around a rotation axis parallel to the w-axis.
  • the range of the emission direction (azimuth) of the laser beam may be 360 °, or may be a range of angles smaller than 360 ° (for example, 210 ° or 270 °).
  • the range of the azimuth angle in the emission direction of the laser beam is referred to as "horizontal viewing angle".
  • the horizontal viewing angle can be set, for example, in the range of about 90 ° to 360 °.
  • the LiDAR sensor 110 sequentially emits pulsed laser light (laser pulse) 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 beam 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 the 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 scanning operation at a frequency of, for example, about 1 to 20 times per second. For example, 100,000 pulses or more of laser light may be emitted in different directions during the operation of one scan.
  • FIG. 10 is a block diagram showing an example of a schematic configuration of the LiDAR sensor 110.
  • 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 optical systems such as lenses and mirrors, 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 arranged on the optical path of the laser beam emitted from each laser light source 112.
  • the laser light source 112 includes a laser diode and emits a pulsed laser beam having a predetermined wavelength in response to a command from the control circuit 115.
  • the wavelength of the laser beam may be, for example, a wavelength included in the near infrared wavelength range (approximately 700 nm to 2.5 ⁇ m).
  • the wavelength used depends on the material of the photoelectric conversion element used in the photodetector 113. For example, when silicon (Si) is used as a material for a photoelectric conversion element, a wavelength of around 900 nm can be mainly used.
  • InGaAs indium gallium arsenide
  • a wavelength of 1000 nm or more and 1650 nm or less can be used.
  • the wavelength of the laser beam is not limited to the wavelength range of near infrared rays.
  • wavelengths included in the visible region approximately 400 nm to 700 nm
  • radiation generally included in the wavelength range of ultraviolet rays, visible light, and infrared rays is referred to as "light”.
  • the photodetector 113 is a device that detects a laser pulse emitted from a 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).
  • APD avalanche photodiode
  • the motor 114 rotates a mirror arranged 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. As a result, a scanning operation that changes the emission direction of the laser beam is realized.
  • the control circuit 115 controls the emission of the laser pulse by the laser light source 112, the detection of the reflected pulse 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, for example, a microcontroller unit (MCU).
  • MCU microcontroller unit
  • the signal processing circuit 116 is a circuit that performs an operation based on the signal output from the photodetector 113.
  • the signal processing circuit 116 calculates the distance to the object that reflects the laser pulse emitted from each laser light source 112 by, for example, the ToF (Time of Flight) method.
  • the ToF method includes a direct ToF method and an indirect ToF method. In the direct ToF method, the distance to the reflection point is calculated by directly measuring the time from the emission of the laser pulse from the laser light source 112 to the reception of the reflected light by the photodetector 113.
  • the distance to each reflection point is calculated based on the ratio of the amount of light detected in each exposure period.
  • Both the direct ToF method and the indirect ToF method can be used.
  • the signal processing circuit 116 generates and outputs sensor data indicating, for example, the distance to each reflection point and the direction of the reflection point.
  • the signal processing circuit 116 further calculates the coordinates (u, v) or (u, v, w) in the sensor coordinate system based on the distance to each reflection point and the direction of the reflection point, and the sensor data. It may be included in and output.
  • control circuit 115 and the signal processing circuit 116 are divided into two circuits in the example of FIG. 10, they may be realized by one circuit.
  • the memory 117 is a storage medium for storing data generated by the control circuit 115 and the signal processing circuit 116.
  • the memory 117 is, for example, the emission timing of the laser pulse 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) in the sensor coordinate system. , V, w) and the associated data are stored. Such data is generated each time a laser pulse is emitted and recorded in memory 117.
  • the control circuit 115 outputs the data in a predetermined cycle (for example, a time required for emitting a predetermined number of pulses, a half scan cycle, a one scan cycle, or the like). The output data is recorded in the storage device 150 of the tractor 100A.
  • the distance measurement method is not limited to the ToF method, and other methods such as the FMCW (Frequency Modulated Continuous Wave) method may be used.
  • FMCW Frequency Modulated Continuous Wave
  • the FMCW method light whose frequency is linearly changed 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.
  • FIG. 11 is a diagram schematically showing an example of an environment in which the tractor 100A travels.
  • the tractor 100A performs predetermined work (for example, mowing, pest control, etc.) using the working machine 300 while traveling between a plurality of tree rows 20 in the vineyard.
  • predetermined work for example, mowing, pest control, etc.
  • the sky is blocked by branches and leaves, making automatic driving using GNSS difficult.
  • the outer shape of the leaves or hedges of trees changes greatly depending on the season. Therefore, it is difficult to use the point cloud map created by the conventional SLAM technique for the whole year.
  • 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.
  • trees in a vineyard, once planted, are not replanted for a long period of time, and the trunk has less change in outer shape due to seasonal changes than the leaves.
  • executives a landmark of SLAM it is possible to create an environmental map that can be used throughout the year, and to perform autonomous driving work without having to recreate the environmental map throughout the year.
  • the ECU 160 uses the sensor data output from the LiDAR sensor 110 to generate environmental map data showing the distribution of the trunks of the tree row 20 in the entire vineyard or one section of the vineyard, 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 trunk of a tree row in the environment around the tractor 100A while performing self-position estimation based on the sensor data repeatedly output from the LiDAR sensor 110 while the tractor 100A is moving.
  • the map data generation module 164 generates environmental map data by joining the local map data generated while the tractor 100A travels through the entire vineyard or a section of the vineyard.
  • This environmental map data is data in which the distribution of the trunks of the tree row 20 in the environment is recorded in a format that can be distinguished from other objects.
  • the executives of the tree row 20 are used as SLAM landmarks.
  • One or more columns may be provided in the area where multiple rows of trees are arranged.
  • a plurality of stanchions for making hedges are generally provided in the vicinity of trees.
  • the stanchions are suitable as landmarks because their outer shape does not change much with the seasons. Therefore, the self-position estimation module 162 further detects the pillars in the environment around the tractor 100A based on the sensor data, and the map data generation module 164 outputs the environmental map data showing the distribution of the pillars in addition to the distribution of the trunks. May be generated.
  • the self-position estimation module 162 of the ECU 160 detects the trunk of the tree row 20 in the surrounding environment based on the sensor data repeatedly output from the LiDAR sensor 110 while the tractor 100A is traveling, and together with the detected trunk of the tree row 20.
  • the position of the tractor 100A is estimated by matching with the environmental map data.
  • the self-position estimation module 162 performs matching between the trunks and columns of the tree row detected based on the sensor data and the environmental map data, thereby performing the tractor 100A.
  • the position may be estimated.
  • the drive control ECU 180 is a control device that controls the movement of the tractor 100A according to the position of the tractor 100A estimated by the ECU 160. For example, when the ECU 180 deviates from the planned travel route determined by the ECU 170, the ECU 180 controls the steering of the tractor 100A to approach the planned travel route by adjusting the steering. Such steering control may be performed based not only on the position of the tractor 100A but also on the orientation.
  • An environmental map showing the distribution of the trunks of the tree row 20 can be generated at any time. For example, it is possible to generate an environmental map in a season such as winter when there are few leaves of trees. In that case, it is easier to generate an environmental map that more accurately reflects the distribution of the trunks, as compared with the case of generating an environmental map in a season with many leaves such as summer.
  • 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 the winter with the data obtained by removing the portion other than the trunk from the sensor data output from the LiDAR sensor 110.
  • the ECU 160 may generate data indicating the distribution of the trunk of the tree row detected based on the sensor data repeatedly acquired while the tractor 100A is traveling, and may update the environmental map data using the data. For example, the ECU 160 may update the environmental map data by adding the executive information detected from the newly acquired sensor data to the previously generated environmental map data.
  • data indicating the distribution of the detected tree row trunks may be transmitted to an external device that updates the environmental map data.
  • the ECU 160 includes the information indicating the estimated position of the tractor 100A in the data indicating the distribution of the trunk (for example, local map data) and outputs the information.
  • the external device can update the environmental map data using the acquired data and transmit the updated environmental map data to the tractor 100A.
  • FIG. 12 is a perspective view schematically showing an example of the environment around the tractor 100A.
  • the tractor 100A runs between two adjacent tree rows in the vineyard. While traveling, the tractor 100A scans the surrounding environment with a laser beam using the LiDAR sensor 110. As a result, data showing the distance distribution of objects existing in the environment is acquired. The data showing the distance distribution is converted into two-dimensional or three-dimensional point cloud data and recorded.
  • the tractor 100A generates environmental map data showing the distribution of tree row trunks in the entire vineyard or in one section of the vineyard by integrating the point cloud data repeatedly acquired while traveling in the vineyard.
  • the integration of point cloud data includes the process of converting the coordinates of a plurality of reflection points acquired at different timings into the coordinates in the world coordinate system fixed to the earth and joining them together.
  • 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.
  • FIG. 13A is a diagram schematically showing an example of a traveling route of the tractor 100A at the time of generating an environmental map and at the time of automatic traveling.
  • the tractor 100A travels between a plurality of tree rows 20 in the vineyard, for example, along the path 30 indicated by the arrow in FIG. 13A.
  • the line segment included in the route 30 is described in a straight line, but the route actually traveled by the tractor 100A may include a meandering portion.
  • the plurality of tree rows 20 are ordered from the end to the first tree row 20A, the second tree row 20B, the third tree row 20C, the fourth tree row 20D, and so on.
  • FIG. 13A is a diagram schematically showing an example of a traveling route of the tractor 100A at the time of generating an environmental map and at the time of automatic traveling.
  • the tractor 100A travels between a plurality of tree rows 20 in the vineyard, for example, along the path 30 indicated by the arrow in FIG. 13A.
  • the line segment included in the route 30
  • the tractor 100A first travels between the first row of trees 20A and the second row of trees 20B, and when the travel is completed, it turns and turns to the second row of trees 20B and the third row of trees 20B. It runs in the opposite direction to the tree row 20C. When the running between the second tree row 20B and the third tree row 20C is completed, the vehicle further turns and runs between the third tree row 20C and the fourth tree row 20D. After that, by repeating the same operation, the vehicle travels to the end of the route 30 between the last two tree rows. If the distance between adjacent tree rows is short, the trees may run every other row as shown in FIG. 13B.
  • the operation of running every other row between the rows of trees that have not yet been run may be executed.
  • Such traveling is automatically performed while the tractor 100A estimates its own position using the LiDAR sensor 110.
  • the GNSS unit 120 can receive the GNSS signal, positioning may be performed based on the GNSS signal. For example, at the timing of turning in the path 30 shown in FIGS. 13A and 13B, since there is no leaf that blocks the GNSS signal, positioning based on the GNSS signal is possible.
  • Example of sensor data acquisition> an example of acquiring sensor data by the tractor 100A will be described with reference to FIGS. 14A to 17C.
  • the sensor data is used to estimate the position of the tractor 100A by matching with the environment map.
  • the environmental map is constructed or updated based on the sensor data.
  • the construction and updating of the environmental map does not need to be performed during the movement of the tractor 100A, but may be performed by processing the sensor data acquired by the tractor 100A during the movement by a computer inside or outside the tractor 100A.
  • FIG. 14A is a diagram schematically showing how the tree 21 and the ground are irradiated with the laser beam emitted from the LiDAR sensor 110 of the tractor 100A.
  • FIG. 14B is a diagram schematically showing how the ground is irradiated with the laser beam emitted from the LiDAR sensor 110.
  • the laser beam emitted from the LiDAR sensor 110 in the negative elevation direction is represented by a thick dashed line
  • the laser beam emitted from the LiDAR sensor 110 in the positive elevation angle direction is represented by a thin dashed line. ..
  • the laser beam emitted in the direction of the negative elevation angle may be reflected on the ground if it does not hit the tree 21.
  • the laser beam emitted in the direction of the positive elevation angle does not form a reflection point when it does not hit the tree 21, and the reflection point when it hits the tree 21 may be located on the surface of a leaf or a branch.
  • reflection points located on the surface of leaves and branches are not suitable for matching with environmental map data. Therefore, in the present embodiment, the self-position estimation and the construction of the environmental map are performed by utilizing the reflection in the executive.
  • the trunk may be detected based on the reflection point whose height of the reflection point is lower than the average height of the trunk.
  • the reflection points located on the ground may be selectively cut from the point cloud of the sensor data after the detection of the trunk.
  • FIG. 15A and 15B are plan views schematically showing an example of a point cloud of sensor data acquired by one scan operation by the LiDAR sensor 110 of the tractor 100A.
  • FIG. 15A schematically shows an example of the distribution of the reflection points of the tractor 100A and the laser pulse emitted from the LiDAR sensor 110 at a certain time point.
  • the ground is assumed to be a flat surface.
  • the laser beam reflected on the ground forms a point of reflection on an arc on the ground.
  • Such reflection points on the ground are formed by a laser beam emitted in a direction in which the elevation angle is negative.
  • reflection points are arranged on seven arcs (corresponding to seven layers) having different radii.
  • Each layer is formed by a scan in which laser beams emitted at different negative elevation angles rotate in the azimuth direction.
  • a laser beam scan may be performed in the horizontal or positive elevation direction.
  • the reflection points arranged on the same layer are formed by the laser beams emitted from the same laser light source at the same elevation angle rotating and incident on the surface of the object in a pulse shape.
  • FIG. 15B schematically shows an example of the distribution of the reflection points of the tractor 100A and the laser pulse emitted from the LiDAR sensor 110 at a time slightly elapsed from the time point of FIG. 15A.
  • a part of the laser pulse emitted from the LiDAR sensor 110 is reflected on the surface of the tree trunk 22.
  • a portion of the laser pulse reflected by the ground, tree trunk 22, or other object is LiDAR, unless the point of reflection is located far beyond the distance range (eg, 50m, 100m, or 200m, etc.). It is detected by the sensor 110 and the distance to the reflection point is measured.
  • the LiDAR sensor 110 is, for example, a sensor in which the distance to the reflection point, the direction of the reflection point, the intensity of the reflected light, and the identification number of the laser light source that emits the laser beam forming the reflection point are associated with each reflection point.
  • the measurement time information can 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 three-dimensional coordinates (u, v, w) of each reflection point represented by a sensor coordinate system fixed to the LiDAR sensor 110.
  • the LiDAR sensor 110 converts the distance and direction data of each reflection point into point cloud data and then outputs the data, the conversion to the point cloud data by the self-position estimation module 162 is omitted.
  • the map data generation module 164 At the time of constructing the environmental map, while the tractor 100A is running, the map data generation module 164 generates local map data in which the executive 22 is recorded in a format distinguishable from other parts from the point cloud data. While the tractor 100A is traveling, the map data generation module 164 repeats an operation of adding and updating the local map data based on the newly acquired sensor data to the local map data generated so far. This makes it possible to generate final environmental map data.
  • the map data generation module 164 only performs an operation of generating and recording local map data while the tractor 100A is traveling, and the final environmental map data may be generated after the traveling is completed. In that case, the final environmental map data may be generated by an external computer.
  • 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 in front of the tractor 100A.
  • a LiDAR sensor 110 may also be attached to the rear of the tractor 100A.
  • map data having a higher point cloud density can be generated.
  • the 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.
  • 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 the storage medium.
  • the storage medium may be the storage medium in the storage device 150 or another storage medium.
  • the thickness of the executive may be added to the environmental map data as one of the attribute information of each landmark, or may be recorded in the map data in another vector format.
  • FIG. 16 is a diagram schematically showing an example of a point cloud of the trunk 22 obtained by one scan of laser beams of a plurality of layers (L 1 , ..., L k + 3 ).
  • 17A to 17C schematically show a point cloud of the trunk 22 obtained by scanning the laser beam of three layers (L k , L k + 1 , L k + 2 ) among the plurality of layers shown in FIG. 16, respectively. It is a figure shown in.
  • point cloud data showing the distribution of the trunk 22 existing in the measurable range can be obtained for each scan. More specifically, when measuring the distance to each reflection point by one scan, in each layer, the region where the reflection points with a relatively long distance are continuous and the reflection point with a relatively short distance are local. Can include areas that appear in.
  • FIG. 18 is a graph showing an example of the relationship between the distance to the reflection point measured in one scan by the laser beams of a plurality of layers (L k , L k + 1 , L k + 2 , ...) And the azimuth angle of the reflection point. Is.
  • the range of azimuths shown in FIG. 18 is narrow, and reflections from the two trunks are shown in this range.
  • the main reflection point of each layer is located on the ground.
  • the reflection points having a relatively short distance are arranged on the curve corresponding to the surface of the trunk.
  • the trunk at a position where the distance is relatively short is irradiated with the laser beam of the layers L k , L k + 1 , L k + 2 , ....
  • the trunk at a position where the distance is relatively long is irradiated with a laser beam of a smaller number of layers L k + 1 , L k + 2 , .... Therefore, when the point cloud on the surface of the trunk in the three-dimensional space is projected onto a plane parallel to the horizontal plane, the density of the point cloud of the trunk at a position where the distance is relatively short is relatively high.
  • the trunk can be detected based on the measurement result shown in FIG. In reality, distance measurements are not as simple as shown because of the presence of undulations and weeds on the ground.
  • the sensor data (hereinafter, also referred to as “scan data”) to be compared with the environmental map showing the distribution of the trunk includes a large number of reflection points other than the reflection points located on the surface of the trunk. obtain. Therefore, it is effective to select a reflection point that is likely to be located on the surface of the trunk from the acquired scan data.
  • the scan data may be obtained by integrating not only the reflection points obtained by one scan but also the reflection points obtained by a plurality of continuously acquired scans to generate high-density point cloud data.
  • a laser beam emitted at a positive elevation angle can irradiate leaves and branches other than the trunk to form a large number of reflection points.
  • the ECU 160 in the present embodiment generates data showing the distribution of the trunk 22 by extracting some points having the characteristics of the trunk 22 from the point cloud data generated based on the scan data.
  • FIG. 19 shows an example of a local map created by extracting reflection points located on the surface of the trunk from scan data acquired by the tractor 100A at a certain position.
  • a two-dimensional local map showing the two-dimensional distribution of the trunk is created.
  • the coordinates of the trunk located around the tractor 100A at that position can be represented by the UV coordinate system fixed to the tractor 100A.
  • FIGS. 20A and 20B are plan views schematically showing the process of estimating the position of the tractor 100A by matching the local map of FIG. 19 with the constructed environment map.
  • Each coordinate in the local map is represented by the sensor coordinate system, and each coordinate in the environment map is represented by the world coordinate system.
  • Matching is optimizing the coordinate transformation from the sensor coordinate system to the world coordinate system. Optimization of such a coordinate transformation determines the position coordinates and orientation of the sensor in the world coordinate system.
  • the executives in the environmental map are shown in black.
  • FIG. 20A shows a state in the middle of matching. In this state, there is a misalignment (error) between the executive on the local map and the corresponding executive on the environmental map.
  • FIG. 20B shows a state in which matching is completed. In this state, the misalignment (error) between the executive on the local map and the corresponding executive on the environmental map is minimized.
  • FIG. 21A shows an example in which the LiDAR sensor 110 is attached to the lower front part of the vehicle body of the tractor 100A as in the present embodiment.
  • FIG. 21B shows an example in which the LiDAR sensor 110 is attached to the front upper part of the cabin of the tractor 100A.
  • the thick dashed line indicates the laser beam emitted to the trunk of the tree 21
  • the thin dashed line indicates the laser beam emitted toward the leaves of the tree or the sky above it.
  • the LiDAR sensor 110 is mounted at a position lower than the average height of the trunk of the tree row.
  • the average height of the trunks of the tree rows is the average value of the heights of the trunks of the trees included in the plurality of tree rows existing in the planned travel path of the tractor 100A.
  • the height of the trunk of a tree is the distance from the ground to the part of the tree that produces the lowest branches.
  • the LiDAR sensor 110 may be arranged, for example, at a height of 10 cm or more and 150 cm or less, and in some cases, 15 cm or more and 100 cm or less from the ground.
  • the arrangement as shown in FIG. 21B is not suitable for the purpose of acquiring the point cloud data of the executive.
  • such an arrangement is effective in the application of positively acquiring the distribution information of the leaves because the distribution information of the leaves of the tree row can be efficiently acquired.
  • the arrangement as shown in FIG. 21B may be adopted for the purpose of controlling the growth condition of the tree and determining the route for avoiding the leaves of the tree. Even in the arrangement as shown in FIG.
  • the LiDAR sensor 110 is not limited to the lower front portion of the vehicle body and the upper front portion of the cabin, and may be arranged at other positions. For example, the LiDAR sensor 110 may be located below the height of the cabin.
  • the tractor 100A may include a plurality of LiDAR sensors. By combining the sensor data output from a plurality of LiDAR sensors, it is possible to acquire the distribution data of the executives more efficiently.
  • the plurality of LiDAR sensors may be provided on the left and right sides of the tractor 100A, for example, or may be provided on the front portion and the rear portion. The details of the embodiment of the moving body including the plurality of LiDAR sensors will be described later.
  • the self-position estimation module 162 in the ECU 160 may detect the trunk based on the reflection point of the laser pulse emitted from the laser light source whose elevation angle is within a predetermined range among the plurality of laser light sources in the LiDAR sensor 110. For example, the trunk may be detected based only on the reflection point of the laser pulse emitted in the direction of the negative elevation angle.
  • the user may be able to set the range of elevation of the laser pulse used for stem detection. For example, the user may be able to set the range of the elevation angle of the laser pulse used for the trunk detection by operating the operation terminal 200.
  • the trunk can be detected more efficiently.
  • the laser beam is used to appropriately select the laser pulse reflected by the trunk according to the tilt angle of the sensor coordinate system.
  • the elevation range may be adaptively selected to extract reflection points.
  • the tilt angle of the LiDAR sensor 110 can be obtained by using the signal from the IMU 125.
  • 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 determines the position and orientation of the tractor 100A. presume.
  • 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.
  • the self-position estimation module 162 acquires the scan data output from the LiDAR sensor 110.
  • the LiDAR sensor 110 outputs scan data at a frequency of, for example, about 1 to 20 times per second.
  • This scan data may include coordinates of a plurality of points represented in the sensor coordinate system and time stamp information. If the scan data includes the distance and direction information to each point and does not include the coordinate information, the self-position estimation module 162 converts the distance and direction information into the coordinate information.
  • the self-position estimation module 162 acquires the environment map data stored in the storage device 150.
  • the environmental map data shows the distribution of the trunks of the tree rows included in the environment in which the tractor 100A travels.
  • the environmental map data includes, for example, data in any of the following formats (1) to (3).
  • the stem ID for identifying individual executives may be included in the environmental map data.
  • the data in which only the points judged to be the surface of the executive are left and the other points are deleted is applicable. Instead of deleting all points that are determined to be non-executive, some points may remain. For example, in vineyards, stanchions for hedge tailoring are generally provided around the cadres, so information on points representing such stanchions may be included in the environmental map.
  • a point in a point cloud corresponds to a trunk or strut is, for example, a distribution in which the distribution of the point and a plurality of points around the point reflects the surface shape of the trunk or strut (for example, downward convex). It can be judged based on whether or not the distribution is in the shape of an arc.
  • a group of points that are locally closer to the LiDAR sensor 110 than the neighboring points is extracted, and these points are used as the executive or the trunk. It may be judged that it is a point representing a support.
  • the curvilinearly distributed point cloud data acquired in each scan is classified into a plurality of classes according to the distance from the LiDAR sensor 110, and it is determined for each class whether or not it corresponds to an executive or a column. May be good. Further, 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 the tree and other parts around it, it is effective to classify the point cloud based on the similarity of the reflection intensity and the similarity of the position. For example, a plurality of points whose reflection intensities are within a predetermined range and whose positions are close to each other may be candidates for points representing the surface of the trunk. A plurality of laser beams having different wavelengths may be emitted from the LiDAR sensor 110, the point cloud may be classified based on the ratio of the reflection intensity for each wavelength, and the trunk may be detected.
  • Machine learning may be used to detect executives. By using a machine learning algorithm based on a neural network such as deep learning, it is possible to detect a point corresponding to the surface of a tree trunk from point cloud data with high accuracy. When a machine learning algorithm is used, a trained model for detecting an executive from point cloud data is created (that is, training) in advance.
  • a process of extracting only a point cloud whose height from the ground is within a predetermined range for example, 0.5 m to 1.5 m, etc.
  • a predetermined range for example, 0.5 m to 1.5 m, etc.
  • 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, with reference to a digital elevation model (DEM).
  • DEM digital elevation model
  • the Z coordinate of the ground may be determined from a point cloud indicating the ground.
  • FIG. 23 is a diagram schematically showing an example of the format of the environmental map data.
  • the environmental map data in this example includes information on the number of points, ID for each point, classification, trunk ID, X coordinate, Y coordinate, Z coordinate, and reflection intensity information. Assuming that the number of points is n, n sets of data sets of point ID, classification, trunk ID, X coordinate, Y coordinate, Z coordinate, and reflection intensity are recorded.
  • Classification indicates what the point represents, including identifiers indicating, for example, tree trunks, hedge stanchions, ground, weeds, tree leaves, and the like.
  • the classification may be a binary numerical value indicating whether or not it is a tree trunk, or a numerical value indicating the probability of being a tree trunk.
  • the stem ID is a number for identifying the stem when the point represents the stem.
  • the trunk ID does not have to be included in the environment map data. Further, in addition to the trunk ID, identifiers of other objects (for example, leaves, branches, ground, weeds, etc.) may be included.
  • the X, Y, and Z coordinates are the coordinates of that point in the world coordinate system.
  • the reflection intensity represents the intensity of the reflected light from that point.
  • the reflection intensity information does not have to be included in the environmental map data.
  • the environmental map data may include information such as a header indicating a data format.
  • environmental map data can be generated for each section of the vineyard (for example, for each section of 100 m ⁇ 100 m).
  • the environmental map data may also include ID information for identifying the section.
  • the environment 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 cloud 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 environment map data is performed with respect to the two-dimensional coordinates.
  • the self-position estimation module 162 acquires the IMU data output from the IMU 125.
  • the IMU data may include information such as acceleration, velocity, displacement, attitude, and measurement time (time stamp) of the tractor 100A.
  • the IMU data is output at a frequency of, for example, several tens to several thousand times per second. This output cycle is generally shorter than the output cycle of the scan data by the LiDAR sensor 110.
  • FIG. 24 is a diagram showing an example of the temporal relationship between IMU data and scan data.
  • the IMU data is output in a cycle of 1 millisecond (ms), and the scan data is output in a cycle of 10 ms.
  • scan data is output every one tenth scan.
  • the scan data includes information on the distance and direction or coordinates of each measured point and the measurement time (time stamp).
  • the self-position estimation module 162 refers to the time stamp of the acquired scan data and acquires the IMU data generated during the corresponding time.
  • the self-position estimation module 162 filters the acquired scan data and reduces the number of points used for matching. Furthermore, the part unnecessary for matching is removed. For example, it is possible to remove points such as the ground, weeds, leaves of trees, and obstacles that are not considered to correspond to the trunks of trees or the stanchions of hedges.
  • the self-position estimation module 162 matches the filtered scan data with the map data. Matching can be performed using any matching algorithm such as NDT (Normal Distribution Transform) or ICP (Iterative Closest Point). Matching determines the position and orientation of the LiDAR sensor 110.
  • NDT Normal Distribution Transform
  • ICP Iterative Closest Point
  • the self-position estimation module 162 determines the position and orientation of the tractor 100A based on the matching result, and outputs data indicating the position and orientation. The data is sent to the drive control ECU 180 and used for controlling the drive device 140.
  • 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. The operation of each step will be described below.
  • the operation shown in FIG. 25 can be started, for example, by an operation by a user using the operation terminal 200.
  • the self-position estimation module 162 reads the environment map data from the storage device 150. If different environmental map data is recorded for each section of the vineyard to be traveled, the environmental map data corresponding to the current location is read.
  • the environment map data corresponding to the current point can be specified, for example, by the user operating the operation terminal 200. Alternatively, if the GNSS signal can be received at that point, the current point 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.
  • the processing can be speeded up by reading only a part of the environmental map data corresponding to the position of the tractor 100A. In this step, all the environmental map data may be read at once.
  • 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 by, for example, the user designating a specific point from the map displayed on the operation terminal 200. Alternatively, if the GNSS signal can be received at that point, the starting point may be set based on the GNSS signal received by the GNSS unit 120.
  • 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 cycle (for example, 5 milliseconds or more and 1 second or less).
  • the scan data includes point cloud data in the range of, for example, several degrees to 360 degrees for each layer.
  • the self-position estimation module 162 reads the scan data every time the scan data is output from, for example, the LiDAR sensor 110. Alternatively, the self-position estimation module 162 may collectively read the scan data each time the scan data is output for a predetermined number of times.
  • the self-position estimation module 162 refers to the time stamp included in the scan data and reads the IMU data corresponding to the scan data.
  • Step S105 The self-position estimation module 162 sets the initial position of matching based on the read IMU data.
  • the initial position of matching is the estimated position of the tractor 100A at the present time indicated by the IMU data. By starting matching from this initial position, the time until convergence can be shortened.
  • the initial value of matching may be determined by linear interpolation, for example, based on the difference between the estimated values of the positions and orientations of the past two scans.
  • Step S106 The self-position estimation module 162 filters the acquired scan data and reduces the number of points used for matching. Further, by the above-mentioned method, the trunk is detected and at least a part of unnecessary parts other than the trunk is removed from the point cloud.
  • FIG. 26 schematically shows a part of the point cloud included in the scan data.
  • a white circle is a point 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. Then, when each voxel contains a plurality of points, those points are replaced with one point located at the center of gravity. In FIG. 26, the replaced points of the center of gravity are represented by black circles.
  • the trunk is detected after such downsampling is performed, but the trunk may be detected before the downsampling. If the data size of the point cloud does not matter, the downsampling process may be omitted.
  • the self-position estimation module 162 matches the filtered scan data with the environment 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 conversion from the sensor coordinate system to the world coordinate system by a method such as the NDT method or the ICP method.
  • 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 can be the data of the coordinates (x, y, z) and the posture ( ⁇ R , ⁇ P , ⁇ Y ) of the representative point (origin) of the tractor 100A expressed in the world coordinate system.
  • the output pose data has two-dimensional coordinates (x, y) and directions. It may include the value of ( ⁇ ). Only the position may be estimated by matching, and the posture information indicated by the IMU data may be used as it is for the posture. If the coordinate system of the tractor 100A matches the sensor coordinate system, step S108 may be omitted.
  • Step S109 The self-position estimation module 162 determines whether or not a command to end the operation has been issued.
  • the operation end command is issued, for example, when the user instructs the operation terminal 200 to stop the automatic traveling mode, or when the tractor 100A reaches the destination. If the operation end command has not been issued, the process returns to step S103, and the same operation is executed for the next scan data. If a command to end the operation is issued, the process ends.
  • 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.
  • the ECU 180 acquires the data of the pose (step S121).
  • 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 may 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).
  • the ECU 180 When the deviation exceeds the threshold value, the ECU 180 changes the drive value (for example, steering angle) of the drive device 140 so that the deviation becomes small. The speed may be changed in addition to the steering angle. If the deviation does not exceed the threshold in step S123, step S124 is omitted. In the following step S125, the ECU 180 determines whether or not a command to end the operation has been received. As described above, the operation end command is issued when, for example, the user instructs the operation terminal 200 to stop the automatic traveling mode, or the tractor 100A reaches the destination. If the operation end command is not 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 is issued, the process ends.
  • the operation end command is issued when, for example, the user instructs the operation terminal 200 to stop the automatic traveling mode, or the tractor 100A reaches the destination. If the operation end command is not issued, the process returns to step S121, and the
  • the ECU 180 controls the drive device 140 based only on the deviation between the estimated position and the planned travel route, but may be controlled in consideration of the deviation in the direction. For example, when the directional deviation, which is the angle 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 responds to the deviation.
  • the drive value (for example, steering angle) of the drive device 140 may be changed.
  • FIG. 28A is a diagram showing an example of a tractor 100A traveling along a planned travel route P.
  • FIG. 28B is a diagram showing an example of the tractor 100A at a position shifted to the right from the planned travel path P.
  • FIG. 28C is a diagram showing an example of the tractor 100A at a position shifted to the left from the planned travel path P.
  • FIG. 28D is a diagram showing an example of the tractor 100A facing a direction inclined with respect to the planned travel path P.
  • the ECU 180 maintains the steering angle and speed of the tractor 100A unchanged.
  • the ECU 180 is included in the steering motor 140 so that the travel direction of the tractor 100A is to the left.
  • the steering angle is changed by changing the rotation angle of.
  • 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 magnitude of the position deviation ⁇ x.
  • the ECU 180 changes the rotation angle of the steering motor so that the travel direction of the tractor 100A is to the right.
  • 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 magnitude of the position deviation ⁇ x.
  • the ECU 180 steers the steering angle so that the directional deviation ⁇ becomes small.
  • 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 according to the azimuth deviation ⁇ may be.
  • Control techniques such as PID control or MPC control (model prediction control) can be applied to the steering control and speed control of the tractor 100A. By applying these control techniques, it is possible to smooth the control of bringing the tractor 100A closer to the planned travel path P.
  • the ECU 180 controls the drive device 140 so as to avoid the obstacle. If the obstacle cannot be avoided, the ECU 180 stops the tractor 100A. The ECU 180 may stop the tractor 100A when an obstacle is detected, regardless of whether or not the obstacle can be avoided.
  • 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 environment map, or may be set by the user by operating the operation terminal 200.
  • FIG. 29A is a diagram showing an example of two-dimensional data showing the trunk distribution of a tree row, which is generated based on an environmental map.
  • the map data generation module 164 in the ECU 160 may generate two-dimensional data roughly showing the distribution of the trunk 22 as shown in FIG. 29A, separately from the environment map for matching.
  • the trunk 22 of each tree is represented by a circle of a certain size.
  • Two-dimensional data showing such a stem distribution may be referred to as "stem distribution data".
  • the stem distribution data includes, for example, information on the position coordinates (x, y) of the center of each stem.
  • the stem distribution data may include information such as the identification number (stem ID) of each stem and the thickness of the stem.
  • HMI human-machine interface
  • FIG. 29B is a diagram for explaining an example of a method of 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 tree row, and determines a curve or a polygonal line passing through the central portion of two adjacent tree rows as the planned travel route 30.
  • the center of the tractor 100A does not necessarily have to be located at the center of two adjacent trees, and the planned travel route 30 may be shifted from the center between the two adjacent trees.
  • the drive control ECU 180 controls the steering of the tractor 100A with reference to the determined travel schedule route 30.
  • FIG. 30 is a diagram showing another example of stem distribution data for route setting.
  • the traveling environment of the tractor 100A is divided into a plurality of cells and managed.
  • Each cell can be, for example, a square with a side length close to the average diameter of the trunk 22.
  • the cells are classified into the following four types (1) to (4). (1) Cell containing cadre 22 (2) Cell adjacent to cell containing cadre 22 (3) Cell located between two adjacent tree rows and not corresponding to (1) and (2) (4) ) Other cells
  • the cells (1) to (4) are represented by different densities.
  • the range of the cell (2) adjacent to the cell containing the trunk 22 may be determined based on the point cloud data indicating the leaves of the tree row acquired by the LiDAR sensor 110.
  • the map data generation module 164 of the ECU 160 generates grid-like data showing the two-dimensional distributions of the trunk and leaves of the tree row, in addition to the environment map data for matching.
  • the ECU 170 in this example determines the path of the tractor 100A based on the grid data as shown in FIG. For example, the ECU 170 sets the cell (3) located between two adjacent tree rows as a travelable area.
  • the ECU 180 controls the traveling of the tractor 100A so as to pass near the central portion of the travelable area.
  • the tractor 100A in the present embodiment collects local map data showing the distribution of the trunks of the tree row while performing self-position estimation and steering control when creating an environmental map by automatic traveling.
  • the ECU 160 further detects the leaves 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 the detected tree row. Further data showing the distribution of leaves may be generated.
  • the ECU 180 can control the steering of the tractor 100A based on the respective distributions of the trunk and leaves of the detected tree row.
  • the ECU 180 for example, moves the tractor 100A so as to pass between two adjacent tree rows of the detected tree rows and along a path that reduces contact with the leaves of the tree rows. Steering control is performed.
  • the ECU 170 determines a route 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, determine the path through the center of the paired positions of the trunks of two adjacent tree rows.
  • the ECU 180 controls the steering of the tractor 100A so as to move the tractor 100A along the determined path. As a result, automatic driving can be performed while collecting local map data.
  • Tree rows are not always arranged linearly. Tree rows may be arranged in a curved line. In such a case, when the vehicle automatically travels while collecting map data, a row of trees may be detected only on one side of the left and right sides. In such a case, it is difficult to continue the automatic driving by the above method because the pair of the left and right tree rows cannot be specified.
  • the ECU 170 estimates the distribution of the hidden trunk in one of the two tree rows based on the detected trunk distribution, and determines the path of the tractor 100A based on the estimated distribution. do.
  • the ECU 180 controls the steering of the tractor 100A so that the tractor 100A travels along the determined path.
  • FIG. 31A is a diagram schematically showing an example of an environment in which tree rows are arranged in a curved line.
  • the cylinders in the figure represent the trunk of the tree
  • the thick dashed lines represent the schematic arrangement of the tree rows
  • the dotted lines schematically represent the scanning by the laser beams of the plurality of layers emitted from the LiDAR sensor 110.
  • FIG. 31B is a diagram schematically showing a point cloud that can be observed in one scan in the environment shown in FIG. 31A.
  • three trunks 22R1, 22R2, 22R3 are detected in the tree row on the right side, and only one trunk 22L1 is detected in the tree row on the left side. ..
  • the laser beam is blocked by the tree trunk 22L1 in the front, and the trunk of the tree in the rear cannot be detected.
  • the ECU 170 estimates the position of the trunk of the hidden tree on the left side from the arrangement of the tree row on the right side where a relatively large number of trees are detected, and determines the traveling route based on the estimated position of the trunk. do. For example, by applying the relative positional relationship between the two adjacent cadres 22R1 and 22L1 in the front, whose positions are specified based on the scan data, to the two adjacent cadres in the rear, the position of the hidden cadre can be determined. presume.
  • FIG. 31C is a diagram for explaining an operation of determining a traveling route by estimating the position of a hidden executive.
  • the position of the hidden trunk is estimated based on the trunk distribution data showing the two-dimensional distribution of the trunk of the tree row acquired based on the scan data.
  • the position of the trunk of the tree detected based on the scan data is shown by a solid line circle, and the position of the trunk of the tree not yet detected is shown by a broken line circle.
  • the ECU 170 first determines the 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, the position displaced by the vector V1 from that position is determined as the estimated position of the hidden trunk.
  • the estimated hidden cadre position is indicated by a cross.
  • 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 traveling path of the tractor 100A.
  • the rear trunk 22L2 hidden by the trunk 22L1 is detected from the scan data. Further rearward cadre 22L3 is not yet detected.
  • the ECU 170 identifies the cadre on the right side (the cadre 22R3 in the example of FIG. 31C) paired with the cadre 22L2, and the vector from the cadre 22R3 to the cadre 22L2. Determine V2.
  • the position displaced by the vector V2 from that position is determined as the position of the hidden trunk 22L3.
  • FIG. 31D is a diagram showing an example of grid-like data showing the two-dimensional distribution of the trunk when the tree rows are arranged in a curve.
  • the ECU 160 may generate grid-like 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 the route based on such grid data.
  • the position of the hidden stem can be estimated in units of cell size.
  • FIG. 32 is a diagram schematically showing an example of a situation in which an obstacle 40 and a person 50 exist in the traveling environment of the tractor 100A.
  • the tractor 100A includes one or more obstacle sensors 130.
  • the ECU 180 performs steering control so as to avoid the obstacle 40 or the person 50. If the collision with the obstacle 40, the person 50, the branch portion, the trunk portion 22, or the like cannot be avoided even if the avoidance is avoided, the ECU 180 stops the tractor 100A.
  • the ECU 180 may stop the tractor 100A when an obstacle is detected, even if the obstacle can be avoided. Similar detection may be performed by the LiDAR sensor 110 regardless of the obstacle sensor 130.
  • the person 50 is near the trunk 22 of the tree.
  • the person 50 or other obstacles may be hidden in the leaves, and it may be difficult to distinguish between the leaves and other objects.
  • matching is performed using an environmental map showing the distribution of the trunk 22, so that it is possible to easily distinguish between the trunk and other objects.
  • Non-executive objects detected by matching scan data with environmental maps can be easily detected as humans or obstacles.
  • the LiDAR sensor 110 can also be used as an obstacle sensor. If the tractor 100A includes a plurality of LiDAR sensors 110 at different locations on the tractor 100A, the obstacle sensor 130 may be omitted.
  • FIG. 33A is a diagram showing an example of the arrangement of the trunk 22 of the tree row detected by the tractor 100A.
  • FIG. 33B is a diagram showing an example of the trunk distribution shown by the environmental map data.
  • the three stems surrounded by a dotted frame have a unique combination of stem spacings of 1.2 m and 1.5 m.
  • the ECU 160 calculates the interval of each of the trunks 22 detected from the acquired scan data, and determines the portion having a unique combination of stem spacings. Then, from the environment map shown in FIG. 33B, a portion having the determined combination of trunk spacing is searched and specified.
  • the ECU 160 can estimate the current position and orientation of the tractor 100A by matching the portions having their own stem spacing. By such an operation, when the tree row includes a portion having a unique combination of stem spacing, it is possible to speed up the matching and improve the accuracy of the position estimation.
  • Information indicating the combination of stem spacing in the tree row may be recorded in advance in the storage medium. Such information may be automatically recorded by, for example, the ECU 160 based on the environment map data, or may be recorded by the user operating the operation terminal 200.
  • the environmental map data generated by the tractor 100A can be provided to another tractor traveling in the same environment or a moving body other than the tractor.
  • a local map for constructing an environmental map in one environment may be shared and generated by a plurality of tractors or other moving objects.
  • a computer is provided in the system to generate one environmental map by collecting local maps generated by a plurality of moving objects.
  • the computer may distribute the generated environment map to a plurality of mobile objects via a wired or wireless network or a storage medium.
  • each moving object should be calibrated to determine the parameters for coordinate conversion from the sensor coordinate system to the moving object coordinate system by test run before starting normal operation. Run.
  • the technique of this embodiment is not limited to a tractor traveling in an orchard such as a vineyard, but is an arbitrary moving object (for example, a mobile robot, a drone, etc.) used in an environment where a plurality of tree rows exist, such as a forest. Can be applied to. This point is the same in the subsequent embodiments.
  • the moving body in the present embodiment includes at least two LiDAR sensors, and performs self-position estimation and map data generation based on the sensor data output from those LiDAR sensors.
  • Each of the at least two LiDAR sensors outputs two-dimensional or three-dimensional point group data or distance distribution data showing the distribution of objects in the environment around the moving object.
  • the moving body corresponds to the position of the moving body estimated by the storage device for storing the environmental map data showing the distribution of the trunks of a plurality of tree rows, the self-position estimation device, and the self-position estimation device. It is provided with a control device for controlling the movement of the moving body.
  • the self-position estimation device detects the trunk of the tree row in the environment around the moving body based on the point cloud data repeatedly output from at least two sensors while the moving body is moving, and the detected tree row.
  • the position of the moving object is estimated by matching the executives with the environmental map data.
  • the moving object also comprises a data generator that generates environmental map data, or local map data for constructing the environmental map data.
  • the data generator detects the trunk of the tree row in the environment around the moving object while performing self-position estimation based on the point cloud data repeatedly output from at least two sensors while the moving object is moving. Then, local map data for generating environmental map data showing the distribution of the detected tree row trunks is generated and recorded in the storage device.
  • the data generator can also generate environmental map data in the world coordinate system and record it in the storage device by integrating the local map data repeatedly generated while the moving object is moving.
  • the configuration and operation of the present embodiment will be described by taking as an example the case where the moving body is a tractor traveling in a fruit garden such as a vineyard.
  • the points different from those of the first embodiment will be mainly described, and the description of overlapping matters will be omitted.
  • FIG. 34 is a schematic view of the tractor 100B in the present embodiment as viewed from the side surface.
  • the tractor 100B is provided with a first LiDAR sensor 110A in the lower front portion of the vehicle body and a second LiDAR sensor 110B in the upper front portion of the cabin.
  • the configuration is the same as that shown in FIG.
  • the coupling device 108 and the working machine 300 shown in FIG. 7 are not shown in FIG. 34, they can also be used in this embodiment.
  • Each of the first LiDAR sensor 110A and the second LiDAR sensor 110B in the present embodiment is a two-dimensional LiDAR sensor.
  • 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 represented by the respective sensor coordinate systems of the LiDAR sensors 110A and 110B.
  • the first LiDAR sensor 110A is mounted at a position lower than the average height of the trunk of the tree row in the environment in which the tractor 100B travels.
  • the second LiDAR sensor 110B is mounted at a position higher than the average height of the trunk of the tree row.
  • the first LiDAR sensor 110A may be arranged, for example, at a height of 10 cm or more and 150 cm or less, and in some cases, 15 cm or more and 100 cm or less from the ground.
  • the second LiDAR sensor 110B may be located, for example, at a position higher than 150 cm above the ground (eg, about 2 m above the ground).
  • the first LiDAR sensor 110A is arranged so as to emit a laser pulse toward the front of the tractor 100B.
  • the first LiDAR sensor 110A in the example of FIG. 34 emits a laser pulse substantially parallel to the ground.
  • the angle between the emission direction of the laser pulse emitted from the first LiDAR sensor 110A and the ground is, for example, within the range of ⁇ 20 degrees, in some cases ⁇ 10 degrees. Can be set within the range of.
  • the second LiDAR sensor 110B is arranged so as to emit a laser pulse toward the diagonally lower front of the tractor 100B.
  • the second LiDAR sensor 110B in the example of FIG. 34 emits a laser pulse in a direction inclined by about 25 degrees from the ground.
  • the angle formed by the emission direction of the laser pulse emitted from the second LiDAR sensor 110B and the ground can be set in the range of, for example, 10 degrees or more and 45 degrees or less.
  • FIG. 35 is a block diagram showing an example of a schematic configuration of the tractor 100B in the present embodiment.
  • the configuration example shown in FIG. 35 is different from the configuration shown in FIG. 8 in that the tractor 100B includes two LiDAR sensors 110A and 110B.
  • the ECU 160 functions as the self-position estimation device and the data generation device described above.
  • the ECU 160 acquires an input point cloud for a plurality of scans including the latest scan from the point cloud data repeatedly output from the two LiDAR sensors 110A and 110B, and matches the input point cloud for the plurality of scans with the environment map data. This makes it possible to estimate the position and orientation of the traveling tractor 100B.
  • the ECU 160 may estimate only the position of the tractor 100B by matching, and determine the direction based on the signal output from the IMU 125.
  • the ECU 160 estimates the self-position based on the scan data repeatedly output from the two LiDAR sensors 110A and 110B while the tractor 100B is moving, while performing the self-position estimation around the moving body.
  • the trunk of the tree row in the environment is detected, and local map data for generating environmental map data showing the distribution of the trunk of the detected tree row is generated and recorded in the storage device 150.
  • the ECU 170 sets a target path through between the trunks of the tree row detected based on the scan data.
  • the ECU 180 controls the drive device 140 according to the set target route to cause the tractor 100B to travel along the target route.
  • the ECU 170 detects an obstacle 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 the target route is to avoid contact with the obstacle. May be set.
  • the ECU 160 generates environmental map data by joining the repeatedly generated local map data and records it in the storage device 150. It should be noted that the detection of the trunk of the tree row may be performed at the stage of generating the environmental map data, not at the stage of generating the local map data. Further, the process of generating the environmental map data based on the local map data may be executed by an external computer.
  • the ECU 160 detects the executives of the tree row based on the scan data repeatedly output from the LiDAR sensors 110A and 110B, and the detected executives and the environmental map data. Self-position estimation is performed by matching with the executives indicated by.
  • the ECU 180 drives the tractor 100B along a planned travel route determined in advance by the ECU 170. In this case as well, the ECU 170 detects the obstacle based on the signal output from the obstacle sensor 130 while the tractor 100B is traveling, or the scan data output from one or both of the LiDAR sensors 110A and 110B, and detects the obstacle.
  • the planned travel route may be changed so as to avoid contact with the vehicle.
  • FIGS. 36A to 36C are diagrams schematically showing how the tractor 100B runs near the tree 21.
  • the tractor 100B approaches the tree 21 in the order of FIG. 36A, FIG. 36B, and FIG. 36C.
  • the dashed line represents the laser beam emitted from the LiDAR sensors 110A, 110B.
  • 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 according to the distance between the tractor 100B and the tree 21 is in the height direction. Change.
  • 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.
  • the point cloud data showing the three-dimensional distribution of the reflection points on the surface of the trunk 22 is acquired as in the case of using the three-dimensional LiDAR. can do.
  • the ECU 160 in the present embodiment accumulates 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 the accumulated scan data and the environment map data are combined. You may perform self-position estimation by performing matching of. By doing so, it is possible to estimate the self-position with high accuracy substantially as in the case of using the three-dimensional LiDAR.
  • the ECU 170 determines the local target route of the tractor 100B based on the scan data for several scans output from one or both of the LiDAR sensors 110A and 110B. To determine. For example, the center position between two adjacent trees is determined from the arrangement of the point cloud indicated by the scan data for several scans output from one or both of the LiDAR sensors 110A and 110B, and the tractor passes through the center position. A local target route of 100B can be determined. With such a route setting, the tractor 100B can travel autonomously while maintaining a state in which the distances from the left and right tree rows are substantially the same.
  • the detection of the executive 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 a point cloud representing the surface of the trunk. Therefore, even when only the scan data from the first LiDAR sensor 110A is used, the distribution information of the trunk can be acquired.
  • the ECU 160 may generate data indicating the distribution of the leaves of the tree row based on the point cloud data acquired by the second LiDAR sensor 110B and record the data in the storage device 150 or another storage medium. .. Such data can be used for tree growth management (eg, canopy management).
  • the same operation as that of the first embodiment can be realized at a low cost.
  • two, three or more two-dimensional LiDAR may be provided at different positions.
  • one or more two-dimensional LiDAR and one or more three-dimensional LiDAR may be used in combination.
  • the configuration of the present embodiment can be variously modified. The various techniques described in the first embodiment can be applied as they are in the present embodiment.
  • one or more moving objects are used to collect local map data for constructing environmental map data, and environmental map data is generated based on the collected local map data.
  • moving objects for example, a drone
  • environmental map data is generated based on the collected local map data.
  • FIG. 37 is a diagram schematically showing a configuration example of the system of the present embodiment.
  • This system includes one or more drones 400, one or more tractors 100C, and a server computer 500 (hereinafter referred to as "server 500"). These components are communicably connected to each other via a wired or wireless network 60.
  • FIG. 37 shows two drones 400 and three tractor 100Cs, but the number of each of the drones 400 and the tractor 100C is arbitrary.
  • Each drone 400 generates local map data for constructing environmental map data while flying in the environment in which each tractor 100C travels, and transmits the local map data to the server 500.
  • the server 500 generates environment 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.
  • 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 the first embodiment shown in FIG. However, the ECU 160 in this embodiment does not include the map data generation module 164.
  • the communication IF 190 communicates with the server 500 via the network 60.
  • the storage device 150 stores the environment map data distributed from the server 500.
  • the illustration of the working machine connected to the tractor 100C and used is omitted.
  • the communication IF 190 may communicate with the working machine connected to the tractor 100C as in the first embodiment.
  • 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 an electric motor for driving and a plurality of propellers.
  • the control device 480 controls the operation of the data generation unit 450 and the 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 the first embodiment.
  • the data generation device 460 has the same functions as the ECU 160 in the first embodiment.
  • 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 showing 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 stores the data 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.
  • the server 500 may be a computer installed at a location away from the tractor 100C and the drone 400, such as a cloud server or an edge server.
  • the server 500 includes a storage device 550, a processing device 560, and a communication IF 590.
  • the communication IF 590 is a circuit for communicating with the tractor 100C and the drone 400 via the network 60.
  • the processing device 560 generates environmental map data by unifying and integrating the local map data acquired from the plurality of drones 400 in a coordinate system, and records the data in the storage device 550.
  • the processing device 560 distributes the generated environment map data from the communication IF 590 to the plurality of tractors 100C.
  • the processing device 560 may distribute the environmental map data to a moving body other than the tractor 100C.
  • the environmental map data may be delivered to the drone 400.
  • the drone 400 can fly autonomously while performing self-position estimation by matching the scan data output from the LiDAR sensor 410 with the environmental map data, and can perform a predetermined operation.
  • FIG. 39 is a perspective view showing an example of the appearance of the drone 400.
  • the 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.
  • FIG. 40 is a perspective view schematically showing an example of a 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 repeat the operation of generating local map data showing the distribution of the trunks by scanning the trunks of the rows 20 with a laser beam while autonomously moving between the rows of trees 20 at low altitude. ..
  • the method of generating the local map data is the same as the method in the first embodiment.
  • FIG. 41 is a diagram schematically showing how the tree 21 is irradiated by the laser beam emitted from the LiDAR sensor 410 mounted on the drone 400.
  • the drone 400 in this embodiment is programmed so that the LiDAR sensor 410 flies at an altitude that keeps it below the average height of the tree row cadres.
  • the altitude of the drone 400 is controlled so that the LiDAR sensor 410 is located, for example, at a height of 15 cm or more and 100 cm or less from the ground.
  • the data generator 460 detects the trunk of the tree row in the environment around the drone 400 while performing self-position estimation based on the sensor data repeatedly output from the LiDAR sensor 410 located at such a low altitude.
  • Local map data showing the distribution of the detected tree row trunks is generated and recorded in a storage medium.
  • the data generator 460 acquires the input point cloud of a plurality of scans from the sensor data repeatedly output from the LiDAR sensor 410, the input point cloud of at least one scan including the latest scan, and the local map data generated before the previous time. Self-position estimation is performed by matching with.
  • 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 the moving object ID, which is the identification number of the drone 400, the position (x, y, z) and attitude ( ⁇ R , ⁇ P , ⁇ Y ) of the drone 400 in the world coordinate system, and the reflection. Includes information recorded pointwise.
  • 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. Is.
  • Such local map data may be transmitted from each drone 400 to the server 500.
  • Information regarding whether or not the point is a trunk may not be included in the local map data.
  • the processing device 560 in the server 500 acquires local map data from a plurality of drones 400, the point cloud corresponding to the executive may be detected based on the characteristics of the distribution of the point cloud.
  • the moving body that generates the local map data does not necessarily have to be a moving body that can move autonomously.
  • a user can drive or steer a moving object, such as a tractor or drone, with one or more LiDAR sensors mounted below the average height of a row of trees to generate local map data. May be good.
  • a moving object such as a drone 400 or a tractor 100C may generate final environmental map data and provide environmental map data to other moving objects without providing a server 500.
  • environmental map data can be directly transmitted and received by communication between mobiles.
  • each tractor may be an unmanned tractor.
  • the tractor does not have to have components necessary only for manned operation, such as a cabin, a driver's seat, a steering wheel, and an operation terminal.
  • the unmanned tractor may perform the same operation as the operation in each of the above-described embodiments by autonomous driving or remote control by the user of the tractor.
  • the one or more sensors included in the moving body are LiDAR sensors that output two-dimensional or three-dimensional point group data or distance distribution data as sensor data by scanning a laser beam.
  • the sensor is not limited to such a LiDAR sensor.
  • other types of sensors such as flash type LiDAR sensors or image sensors may be used.
  • Such other types of sensors may be used in combination with scan-type LiDAR sensors.
  • a device that executes self-position estimation and autonomous movement (or automatic steering) in the above embodiments, or processing necessary for map data generation can be attached to the moving body and used.
  • control units include one or more sensors that output sensor data showing the distribution of objects in the environment around the moving object, and storage devices that store environmental map data showing the distribution of trunks of multiple tree rows.
  • the self-position estimation device detects the trunk of the tree row 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 the detected tree row.
  • the position of the moving object is estimated by matching the executives with the environmental map data.
  • a data generation unit that generates map data by attaching it to a moving body that moves between a plurality of tree rows.
  • Such a data generation unit comprises one or more sensors that output sensor data indicating the distribution of objects in the environment surrounding the moving object, and a data generation device.
  • the data generator detects the trunk of a tree row in the environment around the moving object while performing self-position estimation based on sensor data repeatedly output from one or more sensors while the moving object is moving. Then, local map data for generating environmental map data showing the distribution of the detected tree row trunks is generated and recorded in the storage device.
  • the present disclosure includes the mobile body, control unit, method, and computer program described in the following items.
  • a mobile body that moves between multiple rows of trees
  • One or more sensors that output sensor data indicating the distribution of objects in the environment around the moving object.
  • a storage device that stores environmental map data showing the distribution of the trunks of the plurality of tree rows, and Based on the sensor data repeatedly output from the one or more sensors while the moving body is moving, the trunk of the tree row in the environment around the moving body is detected, and the detected tree row is detected.
  • a self-position estimation device that estimates the position of the moving object by matching the executives of the vehicle with the environmental map data.
  • a control device that controls the movement of the moving body according to the estimated position of the moving body, and A mobile body equipped with.
  • Item 3 The moving body according to item 3, wherein the LiDAR sensor is arranged at a height of 15 cm or more and 100 cm or less from the ground.
  • the self-position estimation device acquires a group of input points for a plurality of scans from sensor data repeatedly output from the LiDAR sensor mounted at a position lower than the average height of the trunk of the tree row, and at least includes the latest scan.
  • the LiDAR sensor mounted at a position lower than the average height of the trunk of the tree row has a plurality of laser light sources, and the plurality of laser light sources each emit laser pulses at different elevation angles.
  • the self-position estimation device detects the trunk of the plurality of laser light sources based on the reflection point of the laser pulse emitted from the laser light source whose elevation angle is included in a predetermined range. A moving object described in either.
  • the one or more sensors include at least one LiDAR sensor that outputs two-dimensional or three-dimensional point group data as the sensor data.
  • the LiDAR sensor repeatedly outputs the point cloud data at a preset cycle, and outputs the point cloud data repeatedly.
  • the self-position estimation device represents a point representing the surface of the trunk based on the position of each point in the point cloud data output during a period of one cycle or more, or the distance or angle of each point from the moving body.
  • the moving body according to item 1, wherein the position of the moving body is estimated by extracting the above point and matching the extracted points with the environmental map data.
  • the self-position estimation device estimates the position of the moving body by using a signal output from the inertial measurement unit.
  • the moving body according to any one of items 1 to 8.
  • the self-position estimation device detects the trunks of a plurality of trees having a unique combination of trunk spacings from the sensor data, and the trunks of the plurality of trees having the unique combinations of trunk spacings and the environmental map data.
  • the position of the moving body is estimated by matching with the trunks of the plurality of trees having the unique combination of stem spacings extracted from.
  • the moving object according to any one of items 1 to 9.
  • the control device determines whether or not an obstacle exists on the path of the moving body based on the environment map data and the sensor data, and when it is determined that the obstacle exists, the moving body has an obstacle.
  • the moving body according to any one of items 1 to 10, wherein an operation for avoiding collision with the obstacle is executed.
  • the environmental map data also shows the distribution of the columns in addition to the distribution of the trunks.
  • the self-position estimation device further detects the pillar in the environment around the moving body based on the sensor data, and matches the detected trunk of the tree row and the pillar with the environment map data. By doing so, the position of the moving body is estimated.
  • the moving body according to any one of items 1 to 13.
  • the self-position estimation device detects the trunk of the tree row in the environment around the moving body based on the sensor data and the information indicating the arrangement relationship of the plurality of tree rows recorded in the storage medium. , The moving body according to any one of items 1 to 14.
  • the self-position estimation device is Data showing the distribution of the trunk of the tree row detected based on the sensor data was generated.
  • the environmental map data is updated using the data indicating the distribution of the executives, or the data indicating the distribution of the executives is transmitted to an external device for updating the environmental map data.
  • the moving body according to any one of items 1 to 17.
  • a control unit that controls the movement of moving objects that move between multiple rows of trees.
  • One or more sensors that are attached to and used by the moving body and output sensor data indicating the distribution of objects in the environment surrounding the moving body.
  • a storage device that stores environmental map data showing the distribution of the trunks of the plurality of tree rows, and Based on the sensor data repeatedly output from the one or more sensors while the moving body is moving, the trunk of the tree row in the environment around the moving body is detected, and the detected tree row is detected.
  • a self-position estimation device that estimates the position of the moving object by matching the executives of the vehicle with the environmental map data.
  • a control device that controls the movement of the moving body according to the estimated position of the moving body, and A control unit equipped with.
  • [Item 20] A method of controlling the movement of a moving object that moves between multiple rows of trees. Acquiring environmental map data showing the distribution of the trunks of the plurality of tree rows, and While the moving body is moving, sensor data showing the distribution of objects in the environment around the moving body is acquired from one or more sensors mounted on the moving body. Based on the acquired sensor data, detecting the trunk of the tree row in the environment around the moving body and By matching the detected trunk of the tree row with the environmental map data, the position of the moving body can be estimated. Controlling the movement of the moving body according to the estimated position of the moving body, and How to include.
  • a computer program run by a computer that controls the movement of a moving object that moves between multiple rows of trees.
  • To the computer Acquiring environmental map data showing the distribution of the trunks of the plurality of tree rows, and While the moving body is moving, sensor data showing the distribution of objects in the environment around the moving body is acquired from one or more sensors mounted on the moving body. Based on the acquired sensor data, detecting the trunk of the tree row in the environment around the moving body and By matching the detected trunk of the tree row with the environmental map data, the position of the moving body can be estimated. Controlling the movement of the moving body according to the estimated position of the moving body, and A computer program that runs a computer program.
  • the technique of the present disclosure can be applied to moving objects such as tractors, drones, and walking robots that move in an environment where a plurality of tree rows exist, such as an orchard such as a vineyard.

Landscapes

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

Abstract

複数の樹木が生い茂る環境において、高精度の測位を実現する。移動体は、複数の樹木列の間を移動する。前記移動体は、前記移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、自己位置推定装置と、前記自己位置推定装置によって推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、を備える。前記自己位置推定装置は、前記移動体が移動している間に前記１つ以上のセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する。

Description

移動体、制御ユニット、および移動体の動作を制御する方法
　本開示は、移動体、制御ユニット、および移動体の動作を制御する方法に関する。
　次世代農業として、ＩＣＴ（Information and Communication Technology）およびＩｏＴ（Internet of Things）を活用したスマート農業の研究開発が進められている。圃場で使用されるトラクタなどの作業車両の自動化および無人化に向けた研究開発も進められている。例えば、精密な測位が可能なＧＮＳＳ（Global Navigation Satellite System）などの測位システムを利用して自動操舵で走行する作業車両が実用化されてきた。特許文献１から３は、ＧＮＳＳを利用して行った測位の結果に基づいて自動操舵を行う作業車両の例を開示している。
　一方、ＬｉＤＡＲ（Light Detection and Ranging）などの距離センサを利用して自律的に移動する移動体の開発も進められている。例えば特許文献４は、ＬｉＤＡＲを利用して圃場における作物列の間を自動走行する作業車両の例を開示している。
国際公開第２０１７／２０８３０６号 特開２０２０－１０４６１７号公報 特開２０２０－１２６８０号公報 特開２０１９－１５４３７９号公報
　ぶどう園（vineyard）などの果樹園、または森林などの、樹木が高い密度で分布する環境では、樹木の上部で生い茂る葉が樹冠（canopy）を形成し、衛星からの電波にとって障害物または多重反射体として働く。このような環境では、ＧＮＳＳによる正確な測位が困難である。ＧＮＳＳを利用できない環境においては、位置推定と地図作成とを同時に実行するＳＬＡＭ（Simultaneous Localization and Mapping）を利用することが考えられる。しかし、ＳＬＡＭを利用して多数の樹木が存在する環境内を自律的に、あるいは自動操舵で移動する移動体の実用化には様々な課題が存在する。例えば、季節の変化によって樹木の葉部の分布が大きく変化するため、過去に作成された地図を継続して使用することができないといった課題がある。
　本開示の例示的な実施形態による移動体は、複数の樹木列の間を移動する。前記移動体は、前記移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、自己位置推定装置と、前記自己位置推定装置によって推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、を備える。前記自己位置推定装置は、前記移動体が移動している間に前記１つ以上のセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する。
　本開示の包括的または具体的な態様は、装置、システム、方法、集積回路、コンピュータプログラム、もしくはコンピュータが読み取り可能な記録媒体、またはこれらの任意の組み合わせによって実現され得る。コンピュータが読み取り可能な記録媒体は、揮発性の記録媒体を含んでいてもよいし、不揮発性の記録媒体を含んでいてもよい。装置は、複数の装置で構成されていてもよい。装置が２つ以上の装置で構成される場合、当該２つ以上の装置は、１つの機器内に配置されてもよいし、分離した２つ以上の機器内に分かれて配置されていてもよい。
　本開示のある実施形態によれば、ＧＮＳＳによる測位が困難な環境においても、移動体の自動操舵あるいは自律移動を実現することができる。樹木列の幹部の分布は、葉部の分布と比較して季節による変化が少ない。幹部に着目して環境地図を作成することにより、比較的長い期間にわたって同一の環境地図を継続して使用することが可能になる。
時刻ｔ１から時刻ｔ２までの間に平面上を移動する移動体の例を模式的に示す図である。 時刻ｔ１における移動体と複数のランドマークとを示す図である。 時刻ｔ２における移動体と複数のランドマークとを示す図である。 移動体座標系ΣＶとワールド座標系ΣＷとの配置関係を示す図である。 マッチングの一例を説明するための図である。 マッチングの一例を説明するための図である。 実施形態１におけるトラクタの外観を示す斜視図である。 トラクタを側面方向から見た模式図である。 トラクタおよび作業機の概略的な構成の例を示すブロック図である。 ＬｉＤＡＲセンサをトラクタの側面方向から見た場合の模式図である。 ＬｉＤＡＲセンサを鉛直上方から見た場合の模式図である。 ＬｉＤＡＲセンサの概略的な構成の例を示すブロック図である。 トラクタが走行する環境の一例を模式的に示す図である。 トラクタの周囲の環境の例を模式的に示す斜視図である。 トラクタの走行経路の一例を模式的に示す図である。 トラクタの走行経路の他の例を模式的に示す図である。 ＬｉＤＡＲセンサから出射されたレーザビームで樹木および地面が照射される様子を模式的に示す図である。 ＬｉＤＡＲセンサから出射されたレーザビームで地面が照射される様子を模式的に示す図である。 ある時点におけるトラクタと、ＬｉＤＡＲセンサから放射されたレーザパルスの反射点の分布の例を模式的に示す図である。 図１５Ａの時点からわずかに時間が経過した時点におけるトラクタと、ＬｉＤＡＲセンサから放射されたレーザパルスの反射点の分布の例を模式的に示す図である。 複数のレイヤのレーザビームの１スキャンによって得られる幹部の点群の例を模式的に示す図である。 レイヤＬｋのレーザビームのスキャンによって幹部の点群が得られる様子を模式的に示す図である。 レイヤＬｋ＋１のレーザビームのスキャンによって幹部の点群が得られる様子を模式的に示す図である。 レイヤＬｋ＋２のレーザビームのスキャンによって幹部の点群が得られる様子を模式的に示す図である。 複数のレイヤのレーザビームによって１スキャンで計測される反射点までの距離と反射点の方位角との関係の例を示すグラフである。 ある位置におけるトラクタで取得されたスキャンデータから、幹部の表面に位置する反射点を抽出して作成した局所地図の例を示す図である。 局所地図を、構築済みの環境地図とマッチングさせることにより、移動体の位置を推定するプロセスを模式的に示す平面図である。 局所地図を、構築済みの環境地図とマッチングさせることにより、移動体の位置を推定するプロセスを模式的に示す平面図である。 トラクタの車両本体の前方下部にＬｉＤＡＲセンサが取り付けられた例を示す図である。 トラクタのキャビンの前方上部にＬｉＤＡＲセンサが取り付けられた例を示す図である。 自己位置推定モジュールの機能構成の例を示す図である。 環境地図データの形式の一例を模式的に示す図である。 ＩＭＵデータとスキャンデータの時間的な関係の一例を示す図である。 自己位置推定動作を示すフローチャートである。 スキャンデータのフィルタリング処理を説明するための図である。 トラクタのポーズが推定された後に実行される制御の例を示すフローチャートである。 走行予定経路Ｐに沿って走行するトラクタの例を示す図である。 走行予定経路Ｐから右にシフトした位置にあるトラクタの例を示す図である。 走行予定経路Ｐから左にシフトした位置にあるトラクタの例を示す図である。 走行予定経路Ｐに対して傾斜した方向を向いているトラクタの例を示す図である。 環境地図に基づいて生成される、樹木列の幹分布を示す２次元データの例を示す図である。 幹分布データに基づく走行予定経路の決定方法の一例を説明するための図である。 経路設定用の幹分布データの他の例を示す図である。 樹木列が曲線的に配置されている環境の一例を模式的に示す図である。 図３１Ａに示す環境において１回のスキャンで観測され得る点群を模式的に示す図である。 隠れた幹部の位置を推定することによって走行経路を決定する動作を説明するための図である。 樹木列が曲線的に配置されている場合における幹部の２次元分布を示す格子状データの一例を示す図である。 トラクタの走行環境に障害物および人が存在する状況の例を模式的に示す図である。 トラクタによって検出される樹木列の幹部の配置の例を示す図である。 環境地図データが示す幹分布の例を示す図である。 実施形態２におけるトラクタを側面方向から見た模式図である。 実施形態２におけるトラクタの概略的な構成の例を示すブロック図である。 トラクタが樹木の近くを走行する様子を模式的に示す図である。 トラクタが樹木の近くを走行する様子を模式的に示す他の図である。 トラクタが樹木の近くを走行する様子を模式的に示すさらに他の図である。 実施形態３のシステムの構成例を模式的に示す図である。 実施形態３のシステムの構成例を示すブロック図である。 ドローンの外観の例を示す斜視図である。 複数のドローンによるデータ収集作業の様子の例を模式的に示す斜視図である。 ドローンに搭載されたＬｉＤＡＲセンサから出射されたレーザビームによって樹木が照射されている様子を模式的に示す図である。 ドローンから出力される局所地図データの形式の一例を示す図である。
　［１．用語の定義］
　本明細書において用いられる主な用語の定義を以下に記載する。
　「移動体」は、移動のための駆動力（traction）を発生させる車輪、クローラ、二足または多足歩行装置、またはプロペラなどの駆動装置を備える装置である。本開示における「移動体」は、トラクタなどの作業車両、搬送車、モバイルロボット、およびマルチコプタなどの無人航空機（Unmanned aerial vehicle：ＵＡＶ、いわゆるドローン）を含む。移動体は、無人・有人の別を問わない。
　「作業車両」は、圃場（例えば、果樹園、畑、水田、または牧草地など）、山林、または建設現場などの作業地において特定の作業を行いながら走行することが可能な車両を意味する。作業車両の例は、トラクタ、田植機、コンバイン、乗用管理機、および乗用草刈機などの農業機械に加えて、建設作業車および除雪車などの、農業以外の用途で使用される車両も含む。
　「ＳＬＡＭ」は、移動体の自己位置推定と地図作成とを同時に行う技術の総称である。
　「自己位置推定」は、地図上における移動体の位置（例えば移動体の重心位置）を推定することである。ＳＬＡＭによる自己位置推定では、通常、移動体のポーズ（pose）が求められる。
　「ポーズ」は、物（オブジェクト）の「位置および向き」である。２次元空間におけるポーズは、例えば（ｘ，ｙ，θ）の３個の座標値によって規定される。ここで、（ｘ，ｙ）は、地球に固定されたワールド座標系であるＸＹ座標系における座標値であり、θは基準方向に対する角度である。３次元空間におけるポーズは、例えば（ｘ，ｙ，ｚ，θＲ，θＰ，θＹ）の６個の座標値によって規定される。ここで（ｘ，ｙ，ｚ）は、ワールド座標系であるＸＹＺ座標系における座標値であり、（θＲ，θＰ，θＹ）は、それぞれの基準方向に対するロール、ピッチ、およびヨーの角度である。移動体の姿勢は、（θＲ，θＰ，θＹ）によって表される。ロール角θＲは、移動体の前後方向の軸の周りの回転量を表す。ピッチ角θＰは、移動体の左右方向の軸の周りの回転量を表す。ヨー角θＹは、移動体の上下方向の軸の周りの回転量を表す。姿勢は、オイラー角などの他の角度、またはクォータニオン（四元数）によっても規定され得る。
　「環境地図データ」は、移動体が移動する環境に存在する物の位置または領域を所定の座標系によって表現したデータである。環境地図データを単に「環境地図」と称することがある。環境地図を規定する座標系の例は、地球に対して固定された地理座標系などのワールド座標系だけでなく、オドメトリ情報に基づくポーズを表示するオドメトリ座標系などを含む。環境地図データは、環境に存在する物について、位置以外の情報（例えば、属性情報その他の情報）を含んでいてもよい。環境地図は、点群地図または格子地図など、さまざまな形式の地図を含む。以下、環境地図を「地図データ」と呼んだり、単に「地図」と呼んだりすることがある。また、環境地図を構築する過程で生成または処理される局所地図または部分地図のデータについても、「地図データ」または単に「地図」と呼ぶことがある。
　「自動操舵」は、手動によらず、制御装置の働きによって移動体の操舵を行うことである。制御装置の一部または全部が移動体の外部にあってもよい。移動体の外部にある制御装置と移動体との間では、制御信号、コマンド、またはデータなどの通信が行われ得る。自動操舵中において、速度制御など、その他の動作は手動で行われてもよい。
　「自律移動」は、人が移動の制御に関与することなく、周囲の環境をセンシングしながら、制御装置の働きによって移動体の移動を行うことを意味する。自律移動は、自律走行および自律飛行を含む。制御装置は、移動体の移動に必要な操舵、速度制御、走行の開始および停止、飛行中の上昇・降下・ホバリングなどの動作を制御し得る。自律移動は、障害物の検知および障害物の回避動作を含み得る。
　「自動走行」は、移動体が備える制御装置による自律走行、および、運行管理システムにおけるコンピュータからの指令に基づく走行を含む。自律走行には、移動体が所定の経路に沿って目的地に向かう移動のみならず、追尾目標に追従する移動も含まれる。また、一時的に作業者の指示に基づいて移動してもよい。
　「自己位置推定装置」は、ＬｉＤＡＲ（Light Detection and Ranging）センサなどの外界センサによって取得されたセンサデータに基づいて環境地図上における自己位置を推定する装置である。
　「外界センサ」は、移動体の外部の状態をセンシングするセンサである。外界センサの例は、例えば、レーザレンジファインダ（「測域センサ」ともいう）、カメラ（またはイメージセンサ）、ＬｉＤＡＲセンサ、ミリ波レーダ、および磁気センサを含む。
　「内界センサ」は、移動体の状態をセンシングするセンサである。内界センサは、車輪の回転速度を計測するホイールエンコーダ、加速度センサ、および角加速度センサ（例えばジャイロスコープ）を含む。慣性計測装置（ＩＭＵ）は、加速度センサおよび角加速度センサを備え、移動体の移動量および姿勢を示す信号を出力することができる。内界センサによって取得される移動体のポーズ変化量を示す情報を「オドメトリ情報」と称する。
　「樹木の幹部」は、木本植物の木質化した茎であり、地上に直立して枝を生ずる主軸部分である。樹木の枝、葉および根は含まない。
　［２．ＳＬＡＭの基本原理］
　次に、本開示の実施形態において用いられるＳＬＡＭ技術を利用した自己位置推定の基本原理を説明する。ここでは、簡単のため、移動体が２次元空間（すなわち平面）内を移動するものとする。
　まず、図１を参照する。図１は、時刻ｔ１から時刻ｔ２までの間に平面上を移動する移動体１００の例を模式的に示す図である。この例における移動体１００は、車輪によって走行する作業車両（例えばトラクタ）である。移動体１００は、トラクタ以外の車両、または歩行ロボットもしくはドローンなどの他の種類の移動体であってもよい。図１には、平面上を移動する移動体１００の位置および向きが模式的に示されている。移動体１００の位置座標は、ＸＹ座標系によって示される。ＸＹ座標系は、地球に固定されたワールド座標系Σｗである。図１には、移動体１００に固定された移動体座標系ΣＶも示されている。この例における移動体座標系ΣＶは、移動体１００の正面が向く方向をｕ軸方向、ｕ軸を反時計回りに９０度回転させた方向をｖ軸方向とするｕｖ座標系である。移動体座標系ΣＶの原点の位置は、移動体１００の移動に伴って変化する。移動体座標系ΣＶの向きは、移動体１００の向きの変化、すなわち回転に伴って変化する。
　時刻ｔ１における移動体１００のポーズ（すなわち位置および向き）をｒ１、時刻ｔ２における移動体１００のポーズをｒ２とする。ポーズｒ１は、例えば、座標（ｘ１，ｙ１）で示される位置と、角度θ１で示される向きとによって規定される。ここでは、移動体１００の向きは移動体１００の正面が向く方向であると定義する。また、Ｘ軸正方向を角度の基準方向とし、反時計回りを角度の正方向とする。ポーズｒ２は、座標（ｘ２，ｙ２）で示される位置と、角度θ２で示される向きとによって規定される。
　図１の例において、時刻ｔ１から時刻ｔ２までの間に、移動体１００の位置は、角度θ１の方位にΔｄ１だけ移動（並進）し、移動体１００の向きは、Δφ１（＝θ２－θ１）だけ反時計回りに回転している。このように、移動体１００の運動は、「並進」と「回転」の合成である。
　この例において、距離Δｄ１が十分に短いとき、移動体１００の進行方向は移動体座標系ΣＶのｕ軸に平行であると近似できる。このため、以下の式１が成立する。
Figure JPOXMLDOC01-appb-M000001
　移動体１００が車輪の回転数センサおよび／または慣性計測装置（ＩＭＵ）などの内界センサを備えている場合、その内界センサから、Δｄ１およびΔφ１の推定値、すなわちオドメトリ情報を取得することができる。時刻ｔ１から時刻ｔ２までの時間差が例えば１０ミリ秒のように短い場合、距離Δｄ１は十分に短く、式１が成立する。時刻がｔ１、t２、ｔ３、・・・と進む過程で、Δｄ１およびΔφ１の推定値を周期的に更新していくことにより、移動体１００の位置および向き（すなわちポーズ）の変化を推定することが可能である。言い換えると、初期のポーズ、例えば（ｘ１，ｙ１，θ１）が既知である場合、その後の移動体１００のポーズの推定値をオドメトリ情報に基づいて周期的に更新することができる。しかし、オドメトリ情報に基づくポーズの推定には、誤差が累積するという問題がある。このため、多くの場合、衛星測位システムまたはＳＬＡＭ技術を利用して、移動体１００の位置の正確度の高い推定値を取得することが必要である。
　次に図２Ａから図５を参照して、ＬｉＤＡＲなどの外界センサを用いて行われる自己位置推定を説明する。以下、環境地図データが存在する場合と存在しない場合のそれぞれについて、自己位置推定動作の例を説明する。
　＜環境地図データが存在する場合＞
　図２Ａおよび図２Ｂに示される例において、時刻ｔ１から時刻ｔ２までの間に移動体１００が移動する環境には、位置が固定されたランドマークｍ１、ｍ２、ｍ３、ｍ４が存在する。この例では、ランドマークの位置を示す環境地図データがすでに存在する。このような環境地図データは、例えば、個々のランドマークを特定する識別子（例えばランドマーク番号）と、各識別子に対応付けられたランドマークのワールド座標系Σｗにおける位置座標とを含む。ワールド座標系Σｗにおけるランドマークｍ１、ｍ２、ｍ３、ｍ４の座標を、それぞれ、（ｘｍ１，ｙｍ１）、（ｘｍ２，ｙｍ２）、（ｘｍ３，ｙｍ３）、（ｘｍ４，ｙｍ４）とする。ＬｉＤＡＲによって取得される点群データにおける個々の点がランドマークとして機能してもよい。点群データによって構成される環境地図を、点群地図と称する。ランドマークの位置は格子地図上のセルによって表示されていてもよい。
　図２Ａの例において、ポーズｒ１における移動体１００が取得したランドマークｍ１、ｍ２、ｍ３、ｍ４の観測値（または計測値）を、それぞれ、ｚ１、ｚ２、ｚ３、ｚ４とする。観測値は、例えば、外界センサによって計測される各ランドマークまでの距離および方向のそれぞれを示す値である。観測値は、外界センサによって取得されるデータであるため、本開示において「センサデータ」と称することがある。
　図２Ｂに示されるように、移動体１００が移動し、ポーズｒ２における移動体１００が取得したランドマークｍ１、ｍ２、ｍ３の観測値を、それぞれ、ｚ５、ｚ６、ｚ７とする。
　これらの観測値に基づいて、移動体座標系ΣＶ上のランドマークの位置座標を取得することができる。前述したように、移動体座標系ΣＶは移動体１００に固定された座標系であるため、同一のランドマーク（例えばランドマークｍ１）の移動体座標系ΣＶ上における位置座標は、移動体１００のポーズの変化に応じて変化していく。
　観測値に基づいて取得されるランドマークの位置は、外界センサの位置および向きによって定まるセンサ座標系における座標を有する。厳密には、センサ座標系は移動体座標系ΣＶとは異なり得るが、以下の説明では、センサ座標系と移動体座標系ΣＶとは同一であるものとする。センサ座標系と移動体座標系ΣＶとの関係は既知であるため、一方の座標系を既知の角度だけ回転させ、かつ、既知の距離だけ並進させることにより、他方の座標系に一致させることができる。
　環境内を移動する移動体１００は、複数のランドマークを観測することにより、移動体座標系ΣＶにおける各ランドマークの位置座標を取得する。そして、ワールド座標系ΣＷにおける各ランドマークの位置座標が環境地図データに含まれている場合、観測値ｚ１～ｚ７などに基づいてポーズｒ１、ｒ２を推定することができる。このような推定は、例えば、観測値から求められるランドマークの位置座標と、環境地図データに含まれるランドマークの位置座標とのマッチングを行うことにより可能になる。
　次に、図３を参照して、移動体座標系ΣＶ上の座標（ｕ１，ｖ１）をワールド座標系ΣＷ上の座標（ｕ１’、ｖ１’）に変換する方法を説明する。図３は、時刻ｔ２における移動体座標系ΣＶとワールド座標系ΣＷとの配置関係を示している。図３の例では、簡単のため、移動体座標系ΣＶの原点をセンサ座標系の原点（センサ中心）と一致させ、両座標系が同一であるものとする。移動体座標系ΣＶ上の座標（ｕ１，ｖ１）は、時刻ｔ２のポーズｒ２における移動体１００からのランドマークｍ１に対する観測値ｚ５に基づいて算出される。観測値ｚ５が、距離ｒｍ１とｕ軸に対する角度φｍ１とによって規定されるとすると、ｕ１およびｖ１は、ｕ１＝ｒｍ１ｃｏｓφｍ１、ｖ１＝ｒｍ１ｓｉｎφｍ１によって計算される。移動体座標系ΣＶ上の座標からワールド座標系ΣＷ上の座標への変換（座標変換）は、移動体座標系ΣＶを時計回りに角度θ２だけ回転させること、および、移動体座標系ΣＶの原点をワールド座標系ΣＷの原点まで並進させることによって行われる。このとき、以下の式２および式３が成立する。
Figure JPOXMLDOC01-appb-M000002
Figure JPOXMLDOC01-appb-M000003
　ここで、Ｒは、移動体１００の向きによって決まる回転行列であり、Ｔはワールド座標系Σｗにおける移動体１００の位置ベクトルである。回転行列Ｒおよび位置ベクトルＴの中身は、移動体１００のポーズから求められる。
　座標変換によって得られる（ｕ１’、ｖ１’）は、ワールド座標系Σｗにおけるランドマークｍ１の座標である（ｘｍ１，ｙｍ１）に一致すべきである。しかし、移動体１００のポーズの推定値が真値からずれていると、（ｕ１’，ｖ１’）と（ｘｍ１，ｙｍ１）との間に誤差（距離）が生じる。自己位置推定を行うことは、（ｕ１’，ｖ１’）と（ｘｍ１，ｙｍ１）との間の誤差が小さくなるように回転行列Ｒおよび位置ベクトルＴの中身を決定することである。
　次に、図４および図５を参照してマッチングの一例をより詳細に説明する。図４は、時刻ｔ２における移動体座標系ΣＶであるｕｖ座標系上のランドマークｍ１、ｍ２、ｍ３の座標を模式的に示している。これらの座標は、観測値ｚ５、ｚ６、ｚ７から算出され得る。この例において、観測値ｚ５、ｚ６、ｚ７から算出される座標を、それぞれ、（ｕ５，ｖ５）、（ｕ６，ｖ６）、（ｕ７，ｖ７）とする。移動体座標系ΣＶからワールド座標系ΣＷへの座標変換を行うことにより、移動体座標系ΣＶ上の（ｕ５，ｖ５）、（ｕ６，ｖ６）、（ｕ７，ｖ７）は、それぞれ、図５に示されるように、ワールド座標系ΣＷ上の（ｕ５’，ｖ５’）、（ｕ６’，ｖ６’）、（ｕ７’，ｖ７’）に変換される。図５は、変換された座標（ｕ５’，ｖ５’）、（ｕ６’，ｖ６’）、（ｕ７’，ｖ７’）と、対応するランドマークの座標（ｘｍ２，ｙｍ２）、（ｘｍ３，ｙｍ３）、（ｘｍ４，ｙｍ４）との間に差異が生じている状態の例を示している。座標変換は、回転行列Ｒによる回転、および位置ベクトルＴによる並進によって規定される。ワールド座標系ΣＷ上において、（ｕ５’，ｖ５’）、（ｕ６’，ｖ６’）、（ｕ７’，ｖ７’）が、それぞれ、対応するランドマークの座標である（ｘｍ２，ｙｍ２）、（ｘｍ３，ｙｍ３）、（ｘｍ４，ｙｍ４）に近づくように（すなわちマッチするように）座標変換（Ｒ、Ｔ）の中身が決定される。
　前述した式２の等式において、未知数はｘ２、ｙ２、θ２である。未知数の個数は３であるため、式２に相当する等式が３個以上あれば、計算によって、ｘ２、ｙ２、θ２を求めることができる。図５に示されるように同一のポーズｒ２における移動体１００から３個以上のランドマークが観測される場合、式２に相当する等式が３個得られるため、連立方程式を解くことにより、ポーズｒ２（ｘ２、ｙ２、θ２）の計算値を取得できる。現実の観測には誤差が加わるため、３個を超える多数の式からなる連立方程式を用い、最小二乗法などによる最適化が行われる。
　このようなマッチングによる最適化を行って自己位置を推定するアルゴリズムの例は、ＩＣＰマッチング法およびＮＤＴマッチング法を含む。これらのいずれのマッチング法、または他の方法を用いてもよい。
　なお、観測値と環境地図データから移動体１００の位置を推定するとき、必ずしも移動体１００の向きを推定する必要はない。例えば図２Ａおよび図２Ｂに示されるように、位置が既知であるランドマークまでの距離が観測値から取得できる場合、三角測量の原理に基づいて、移動体１００の位置（正確には測距センサの位置）を算出することが可能である。このような原理に基づいて位置を推定することも、「自己位置推定」に含まれる。
　＜環境地図データが存在しない場合＞
　環境地図データが存在しない場合、図２Ａおよび図２Ｂに示されるように移動体１００が移動中に取得した観測値に基づいて、ランドマークｍ１、ｍ２、ｍ３、ｍ４、・・・の座標を決定（すなわち推定）しながら、移動体１００のポーズを推定する処理が必要になる。ランドマークｍ１、ｍ２、ｍ３、ｍ４、・・・のワールド座標系ΣＷ上における座標は、環境地図データを構成する。したがって、地図を構築するには、ランドマークとして機能する物体のワールド座標系ΣＷ上における位置座標を決定する必要がある。
　図２Ａおよび図２Ｂの例では、移動しつつある移動体１００がランドマークｍ２、ｍ３、ｍ４のそれぞれから複数の観測値を取得している。より多くの観測を行うことにより、移動体１００のポーズおよびランドマークの座標に含まれる未知数の総数よりも多くの連立方程式を得ることができる。それにより、移動体１００のポーズおよびランドマークの座標の推定値を算出することが可能である。
　ＳＬＡＭによって自己位置推定と環境地図の作成を行うためのアルゴリズムには、多数の種類が存在する。ＳＬＡＭアルゴリズムの例は、ＬｉＤＡＲセンサを利用するアルゴリズムだけではなく、カメラなどの他の外界センサを利用するアルゴリズムも含む。また、自己位置推定にパーティクルフィルタなどのベイズフィルタを利用したり、グラフベース手法によってポーズ推定の精度を高めたりしてもよい。本開示の実施形態では、ＳＬＡＭアルゴリズムの種類は特に限定されない。
　なお、ＬｉＤＡＲセンサには、レーザビームのスキャンによって空間中の物体の距離分布の情報を取得するスキャン型のセンサと、広範囲に拡散する光を利用して空間中の物体の距離分布の情報を取得するフラッシュ型のセンサとがある。スキャン型のＬｉＤＡＲセンサは、フラッシュ型のＬｉＤＡＲセンサよりも高い強度の光を使用するため、より遠方の距離情報を取得することができる。一方、フラッシュ型のＬｉＤＡＲセンサは、構造が簡単であり、低いコストで製造できるため、強い光を必要としない用途に適している。本開示では、主にスキャン型のＬｉＤＡＲセンサが用いられる例を説明するが、用途によってはフラッシュ型のＬｉＤＡＲセンサも使用され得る。
　スキャン型の典型的なＬｉＤＡＲセンサによって環境内の物体を観測する場合、パルス状のレーザビーム（すなわちレーザパルス）を出射し、周囲の環境に存在する物体によって反射されたレーザパルスがＬｉＤＡＲセンサに帰還するまでの時間を計測することにより、物体の表面に位置する反射点までの距離と方向とを知ることができる。反射点までの距離および方向がわかれば、移動体座標系ΣＶにおける「反射点」の座標を取得することができる。スキャン型のＬｉＤＡＲセンサは、２次元ＬｉＤＡＲセンサと、３次元ＬｉＤＡＲセンサとに分類することができる。２次元ＬｉＤＡＲセンサによれば、レーザビームが１つの平面内を回転するように環境のスキャンが行われ得る。これに対して、３次元ＬｉＤＡＲセンサでは、複数のレーザビームがそれぞれ異なる円錐面に沿って回転するように環境のスキャンが行われ得る。これらのＬｉＤＡＲセンサによって取得される個々の反射点の座標（２次元または３次元の座標値）は、移動体座標系ΣＶで表される。個々の反射点の座標を移動体座標系ΣＶからワールド座標系ΣＷに変換することにより、個々の反射点のワールド座標系ΣＷ上における座標を得ることができ、その結果、点群地図を構築することができる。移動体座標系ΣＶからワールド座標系ΣＷに変換するためには、上記のように、移動体１００のポーズの情報が必要である。
　ＳＬＡＭ技術では、移動体１００のポーズの推定と、環境地図の構築とを同時に実行することができる。しかしながら、環境地図を構築するときにＳＬＡＭ技術以外の技術を利用して移動体１００のポーズを推定または測定してもよい。移動体１００のポーズは、移動体１００の位置を測定する衛星測位システムおよび慣性誘導装置を利用しても決定することができるからである。しかしながら、衛星測位システムのような測位システムを利用できない状況では、環境地図を構築するためにＳＬＡＭ技術を利用して移動体１００のポーズを推定することが必要になる。なお、移動体１００の位置についてはＳＬＡＭ技術を利用して推定し、移動体１００の向きまたは姿勢については、慣性計測装置などの他のセンサによって推定してもよい。
　［３．実施形態］
　以下、本開示の実施形態を説明する。ただし、必要以上に詳細な説明は省略する場合がある。例えば、既によく知られた事項の詳細説明および実質的に同一の構成に対する重複説明を省略する場合がある。これは、以下の説明が不必要に冗長になることを避け、当業者の理解を容易にするためである。なお、発明者は、当業者が本開示を十分に理解するために添付図面および以下の説明を提供するのであって、これらによって特許請求の範囲に記載の主題を限定することを意図するものではない。以下の説明において、同一または類似の機能を有する構成要素については、同じ参照符号を付している。
　［３－１．実施形態１］
　本開示の第１の実施形態による移動体を説明する。
　本実施形態における移動体は、複数の樹木が生い茂り、複数の樹木列が形成されている環境で使用される。移動体は、周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、記憶装置と、自己位置推定装置と、移動体の移動を制御する制御装置とを備える。記憶装置は、複数の樹木列の幹部の分布を示す環境地図データを記憶する。自己位置推定装置は、移動体が移動している間に１つ以上のセンサから繰り返し出力されるセンサデータに基づいて、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部と環境地図データとのマッチングを行うことにより、移動体の位置を推定する。制御装置は、推定された移動体の位置に応じて移動体の移動を制御する。本実施形態における移動体は、環境地図データ、または環境地図データを生成するための局所地図データを生成するデータ生成装置をさらに備えている。データ生成装置は、移動体が移動している間に１つ以上のセンサから繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録する。データ生成装置は、繰り返し生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成し、記憶装置に記録することもできる。このように、本実施形態における移動体は、複数の樹木列の間を移動しながら環境地図を作成する機能と、作成した環境地図を利用して移動体の位置を推定しながら樹木列間を自律的に移動する機能とを備える。
　１つ以上のセンサは、例えば２次元または３次元の点群データをセンサデータとして出力する少なくとも１つのＬｉＤＡＲセンサを含み得る。本明細書において「点群データ」は、ＬｉＤＡＲセンサによって観測される複数の反射点の分布を示すデータを広く意味する。点群データは、例えば、２次元空間または３次元空間における各反射点の座標値、および／または各反射点の距離および方向を示す情報を含み得る。
　ＬｉＤＡＲセンサは、例えば予め設定された周期で点群データを繰り返し出力する。データ生成装置は、１周期以上の期間中に出力された点群データにおける各点の位置、または各点の移動体からの距離もしくは角度に基づいて、幹部を検出することができる。
　以下、移動体が、主にぶどう園などの果樹園における作業に用いられるトラクタなどの作業車両であり、センサがスキャン型のＬｉＤＡＲセンサである場合を例に、本実施形態の構成および動作を説明する。
　＜構成＞
　図６は、本実施形態におけるトラクタ１００Ａの外観を示す斜視図である。図７は、トラクタ１００Ａを側面方向から見た模式図である。図７には、トラクタ１００Ａに連結されて使用される作業機（implement）３００も示されている。
　図６に示すように、本実施形態におけるトラクタ１００Ａは、ＬｉＤＡＲセンサ１１０と、ＧＮＳＳユニット１２０と、１つ以上の障害物センサ１３０とを備える。図６の例では１つの障害物センサ１３０が示されているが、障害物センサ１３０は、トラクタ１００Ａの複数の箇所に設けられ得る。
　図７に示すように、トラクタ１００Ａは、車両本体１０１と、原動機（エンジン）１０２と、変速装置（トランスミッション）１０３とを備える。車両本体１０１には、車輪（タイヤ）１０４と、キャビン１０５とが設けられている。車輪１０４は、前輪１０４Ｆと後輪１０４Ｒとを含む。キャビン１０５の内部に運転席１０７、操舵装置１０６、および操作端末２００が設けられている。前輪１０４Ｆおよび後輪１０４Ｒの一方または両方は、タイヤではなくクローラであってもよい。
　本実施形態におけるＬｉＤＡＲセンサ１１０は、車両本体１０１の前面下部に配置されている。ＬｉＤＡＲセンサ１１０は、トラクタ１００Ａが走行する環境に存在する樹木列の幹部の平均高さよりも低い位置、例えば、地上から１５ｃｍ以上１００ｃｍ以下の高さに配置され得る。ＬｉＤＡＲセンサ１１０は、例えば、トラクタ１００Ａの高さの半分よりも低い位置に配置され得る。トラクタ１００Ａの高さは、地面からキャビン１０５の最上部までの高さであり、例えば２メートル以上であり得る。ＬｉＤＡＲセンサ１１０は、トラクタ１００Ａの高さの１／３よりも低い位置に配置されていてもよい。図７の例では、ＬｉＤＡＲセンサ１１０は、車輪１０４の上部および前照灯１０９よりも低い位置に配置されている。ＬｉＤＡＲセンサ１１０をこのような低い位置に配置することによって得られる効果については後述する。
　ＬｉＤＡＲセンサ１１０は、トラクタ１００Ａが移動している間、周囲の環境に存在する物体の距離および方向、または２次元もしくは３次元の座標値を示すセンサデータを繰り返し出力する。ＬｉＤＡＲセンサ１１０から出力されたセンサデータは、トラクタ１００Ａが備えるＥＣＵ（Electronic Control Unit）などの制御装置によって処理される。制御装置は、前述のＳＬＡＭアルゴリズムを利用して、センサデータに基づく環境地図データの生成、および環境地図を用いた自己位置推定などの処理を実行することができる。なお、制御装置は、環境地図データを完成する代わりに、環境地図データを生成するための局所地図データを作成してもよい。その場合、局所地図データを統合して環境地図データを構築する処理は、トラクタ１００Ａの外部にあるコンピュータ、例えばクラウドサーバで実行されてもよい。
　図７の例では、車両本体１０１の後部に障害物センサ１３０が設けられている。障害物センサ１３０は、車両本体１０１の後部以外の部位にも配置され得る。例えば、車両本体１０１の側部、前部、およびキャビン１０５のいずれかの箇所に、１つまたは複数の障害物センサ１３０が設けられ得る。障害物センサ１３０は、トラクタ１００Ａの比較的近くに存在する物体を検出する。障害物センサ１３０は、例えばレーザスキャナまたは超音波ソナーを備え得る。障害物センサ１３０は、障害物センサ１３０から所定の距離よりも近くに物体が存在する場合に、障害物が存在することを示す信号を出力する。複数の障害物センサ１３０がトラクタ１００Ａの車体の異なる位置に設けられていてもよい。例えば、複数のレーザスキャナと、複数の超音波ソナーとが、車体の異なる位置に配置されていてもよい。そのような多くの障害物センサ１３０を備えることにより、トラクタ１００Ａの周囲の障害物の監視における死角を減らすことができる。ＬｉＤＡＲセンサ１１０が障害物センサとして機能してもよい。
　ＧＮＳＳユニット１２０は、キャビン１０５の上部に設けられている。ＧＮＳＳユニット１２０は、ＧＮＳＳ衛星からの信号を受信するアンテナと、処理回路とを備えるＧＮＳＳ受信機である。ＧＮＳＳユニット１２０は、例えばＧＰＳ（Global Positioning System）、ＧＬＯＮＡＳ、Ｇａｌｉｌｅｏ、ＢｅｉＤｏｕ、または準天頂衛星（Quasi-Zenith Satellite System：ＱＺＳＳ、例えばみちびき）などのＧＮＳＳ衛星から送信されるＧＮＳＳ信号を受信し、当該信号に基づいて測位を行う。本実施形態におけるトラクタ１００Ａは、主にぶどう園のように、複数の樹木が生い茂り、ＧＮＳＳを利用しにくい環境で使用されるため、ＬｉＤＡＲセンサ１１０を利用して測位が行われる。しかし、ＧＮＳＳ信号を受信可能な環境ではＧＮＳＳユニット１２０を利用して測位を行ってもよい。ＬｉＤＡＲセンサ１１０による測位とＧＮＳＳユニット１２０による測位とを組み合わせることにより、測位の安定性または精度を向上させることができる。
　原動機１０２は、例えばディーゼルエンジンである。ディーゼルエンジンに代えて電動モータが使用されてもよい。変速装置１０３は、変速によってトラクタ１００Ａの推進力を切り換えることができる。変速装置１０３は、トラクタ１００Ａの前進と後進とを切り換えることもできる。
　操舵装置１０６は、ステアリングホイールと、ステアリングホイールに接続された操舵軸と、ステアリングホイールによる操舵を補助するパワーステアリング装置とを含む。パワーステアリング装置は、操舵軸を回転させるための補助力を供給する油圧装置または電動モータを含む。前輪１０４Ｆは操舵輪であり、その操舵角を変化させることにより、トラクタ１００Ａの走行方向を変化させることができる。前輪１０４Ｆの操舵角は、ステアリングホイールを操作することによって変化させることができる。自動操舵が行われるときには、トラクタ１００Ａ内に配置された電子制御ユニット（ＥＣＵ）からの制御により、油圧装置または電動モータの力によって操舵角が自動で調整される。
　車両本体１０１の後部には、連結装置１０８が設けられている。連結装置１０８は、例えば３点リンク（ヒッチ）装置、ＰＴＯ（Power Take Off）軸、およびユニバーサルジョイントを含む。連結装置１０８によって作業機３００をトラクタ１００Ａに着脱することができる。連結装置１０８は、例えば油圧装置によって３点リンク装置を昇降させ、作業機３００の位置または姿勢を制御することができる。また、ユニバーサルジョイントを介してトラクタ１００Ａから作業機３００に動力を送ることができる。トラクタ１００Ａは、作業機３００を引きながら、作業機３００に所定の作業を実行させることができる。連結装置は、車両本体１０１の前方に設けられていてもよい。その場合、トラクタ１００Ａの前方に作業機を接続することができる。なお、作業機がトラクタ１００Ａの前方に接続された状態でＬｉＤＡＲセンサ１１０が使用される場合、ＬｉＤＡＲセンサ１１０は、レーザビームが作業機によって遮られない位置に配置される。
　図７に示す作業機３００は、作物に薬剤を噴霧するスプレイヤであるが、作業機３００はスプレイヤに限定されない。例えば、モーア（草刈機）、シーダ（播種機）、スプレッダ（施肥機）、レーキ、ベーラ（集草機）、ハーベスタ（収穫機）、プラウ、ハロー、またはロータリなどの、任意の作業機をトラクタ１００Ａに接続して使用することができる。
　以下の説明において、図７に示すｕｖｗ座標系を用いる。ｕｖｗ座標系は、トラクタ１００ＡおよびＬｉＤＡＲセンサ１１０に固定された座標系である。トラクタ１００Ａの進行方向をｕ軸方向、左方向をｖ軸方向、上方向をｗ軸方向とする。図面では、わかりやすさのため、ｕｖｗ座標系の原点がトラクタ１００Ａから離れた位置に存在するように記載されているが、現実には、トラクタ１００Ａに設けられるＬｉＤＡＲセンサ１１０の中心にｕｖｗ座標系の原点が一致している。
　図８は、トラクタ１００Ａおよび作業機３００の概略的な構成の例を示すブロック図である。この例におけるトラクタ１００Ａは、ＬｉＤＡＲセンサ１１０、ＧＮＳＳユニット１２０、障害物センサ１３０、操作端末２００に加え、慣性計測装置（ＩＭＵ）１２５、駆動装置１４０、記憶装置１５０、複数の電子制御ユニット（ＥＣＵ）１６０、１７０、１８０、および通信インタフェース（ＩＦ）１９０を備える。作業機３００は、駆動装置３４０と、制御装置３８０と、通信インタフェース３９０とを備える。なお、図８には、本実施形態における自己位置推定動作、自動走行動作、および地図生成動作との関連性が相対的に高い構成要素が示されており、それ以外の構成要素の図示は省略されている。
　本実施形態におけるＩＭＵ１２５は、３軸加速度センサおよび３軸ジャイロスコープを備える。ＩＭＵ１２５は、モーションセンサとして機能し、トラクタ１００Ａの加速度、速度、変位、および姿勢などの諸量を示す信号を出力することができる。ＩＭＵ１２５は、例えば１秒間に数十回から数千回程度の頻度で、当該信号を出力する。ＩＭＵ１２５に代えて、３軸加速度センサおよび３軸ジャイロスコープを別々に設けてもよい。
　駆動装置１４０は、例えば前述の原動機１０２、変速装置１０３、車輪１０４、操舵装置１０６、および連結装置１０８などの、トラクタ１００Ａの走行および作業機３００の駆動に必要な各種の装置を含む。原動機１０２は、例えばディーゼル機関などの内燃機関を備える。駆動装置１４０は、内燃機関に代えて、あるいは内燃機関とともに、トラクション用の電動モータを備えていてもよい。
　記憶装置１５０は、フラッシュメモリまたは磁気ディスクなどの記憶媒体を含み、環境地図データを初めとするＥＣＵ１６０、１７０、１８０が生成する各種のデータを記憶する。記憶装置１５０は、ＥＣＵ１６０、１７０、１８０に、後述する各種の動作を実行させるコンピュータプログラムも記憶する。そのようなコンピュータプログラムは、記憶媒体（例えば半導体メモリまたは光ディスク等）または電気通信回線（例えばインターネット）を介してトラクタ１００Ａに提供され得る。そのようなコンピュータプログラムが、商用ソフトウェアとして販売されてもよい。
　ＥＣＵ１６０は、前述のＳＬＡＭ技術に基づく処理を実行する電子回路である。ＥＣＵ１６０は、自己位置推定モジュール１６２および地図データ生成モジュール１６４を含む。ＥＣＵ１６０は、前述の自己位置推定装置およびデータ生成装置の一例である。ＥＣＵ１６０は、トラクタ１００Ａが走行している間にＬｉＤＡＲセンサ１１０およびＩＭＵ１２５から繰り返し出力されたデータまたは信号に基づき、トラクタ１００Ａの位置および向き（すなわちポーズ）の推定を行いながら地図データを生成することができる。ＥＣＵ１６０はまた、環境地図データが生成された後の自動走行時において、ＬｉＤＡＲセンサ１１０から出力されたセンサデータと環境地図データとのマッチングを行うことにより、トラクタ１００Ａの位置および向きを推定することができる。
　自己位置推定モジュール１６２は、トラクタ１００Ａの位置および向きの推定、すなわち自己位置推定を行うための演算を実行する。地図データ生成モジュール１６４は、地図データを生成する処理を実行する。地図データ生成モジュール１６４が生成する「地図データ」には、自動走行時のマッチングに使用される環境地図データと、環境地図データを構築するために生成される部分的なデータである局所地図データとが含まれる。地図データ生成モジュール１６４は、繰り返し生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成し、記憶装置１５０に記録することができる。自己位置推定モジュール１６２および地図データ生成モジュール１６４は、１つの回路で実現されていてもよいし、複数の回路に分かれていてもよい。
　環境地図データがまだ生成されていない段階において、ＥＣＵ１６０は、自己位置推定を行いながら、トラクタ１００Ａの周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す局所地図データを繰り返し生成して記憶装置１５０に記録することができる。ＥＣＵ１６０は、さらに、局所地図データを繋ぎ合わせることにより、圃場（例えばぶどう園）の全体、または圃場の一区画についての環境地図データを生成することができる。圃場の区画ごとに環境地図データが生成されてもよい。ＥＣＵ１６０は、局所地図データを生成する段階では樹木列の幹部を検出せず、最終的な環境地図データを生成する段階で幹部を検出し、幹部を他の物体と区別できる形式で記録してもよい。環境地図データが生成された後の自動走行時において、ＥＣＵ１６０は、ＬｉＤＡＲセンサ１１０から出力されるセンサデータと環境地図データとのマッチングを行うことにより、トラクタ１００Ａの位置および向きを推定する。なお、ＥＣＵ１６０は、トラクタ１００Ａの位置のみをマッチングで決定し、トラクタ１００Ａの向きはＩＭＵ１２５からの信号を利用して決定してもよい。
　ＥＣＵ１７０は、トラクタ１００Ａの経路を決定する処理を行う回路である。環境地図データがまだ生成されていない段階において、ＥＣＵ１７０は、ＬｉＤＡＲセンサ１１０および障害物センサ１３０から出力されたデータまたは信号に基づいて、トラクタ１００Ａが走行すべき経路を決定する。例えば、ＬｉＤＡＲセンサ１１０から出力されるセンサデータに基づいて検出される複数の樹木列の間を通り、かつ障害物を避ける経路が目標経路として決定される。環境地図データが生成された後の自動走行時において、ＥＣＵ１７０は、環境地図データに基づいて、またはユーザからの指示に基づいて、目標経路（以下、「走行予定経路」とも称する。）を決定する。
　ＥＣＵ１８０は、駆動装置１４０を制御する回路である。ＥＣＵ１８０は、ＥＣＵ１６０によって推定されたトラクタ１００Ａの位置および向きと、ＥＣＵ１７０によって決定された走行予定経路とに基づいて、駆動装置１４０を制御する。ＥＣＵ１８０はまた、作業機３００の動作を制御する信号を生成し、その信号を通信ＩＦ１９０から作業機３００に送信する動作も実行する。
　ＥＣＵ１６０、１７０、１８０は、例えばＣＡＮ（Controller Area Network）などのビークルバス規格に従って、相互に通信することができる。図８において、ＥＣＵ１６０、１７０、１８０のそれぞれは、個別のブロックとして示されているが、これらのそれぞれの機能が、複数のＥＣＵによって実現されていてもよい。あるいは、ＥＣＵ１６０、１７０、１８０の機能を統合した１つの車載コンピュータが設けられていてもよい。トラクタ１００Ａは、ＥＣＵ１６０、１７０、１８０以外のＥＣＵを備えていてもよく、機能に応じて任意の個数のＥＣＵが設けられ得る。
　通信ＩＦ１９０は、作業機３００の通信ＩＦ３９０との間で通信を行う回路である。通信ＩＦ１９０は、例えばＩＳＯ　１１７８３に基づくＩＳＯＢＵＳなどの通信制御規格に準拠した信号の送受信を、作業機３００の通信ＩＦ３９０との間で実行する。これにより、作業機３００に所望の動作を実行させたり、作業機３００から情報を取得したりすることができる。通信ＩＦ１９０はまた、有線または無線のネットワークを介して外部のコンピュータと通信することもできる。例えば、作物の生育状況、トラクタ１００Ａの稼働状況、および作業記録等を管理する営農管理システムにおけるサーバなどのコンピュータと通信することができる。
　操作端末２００は、トラクタ１００Ａの自動走行または自動操舵に関する操作をユーザが実行するための端末であり、バーチャルターミナル（ＶＴ）と称することもある。操作端末２００は、タッチスクリーンなどの表示装置、および／または１つ以上のボタンを備え得る。ユーザは、操作端末２００を操作することにより、例えば自動走行モードまたは自動操舵モードのオン／オフの切り替え、トラクタ１００Ａの初期位置の設定、経路の設定、環境地図の記録または編集などの種々の操作を実行することができる。
　作業機３００における駆動装置３４０は、作業機３００が所定の作業を実行するために必要な動作を行う。駆動装置３４０は、例えばポンプ、油圧装置、または電気モータなどの、作業機３００の用途に応じた装置を含む。
　制御装置３８０は、駆動装置３４０の動作を制御する。制御装置３８０は、通信ＩＦ３９０を介してトラクタ１００Ａから送信された信号に応答して、駆動装置３４０に各種の動作を実行させる。
　次に、図９Ａおよび図９Ｂを参照して、ＬｉＤＡＲセンサ１１０の構成例を説明する。本実施形態におけるＬｉＤＡＲセンサ１１０は、レーザビームのスキャンによって空間中の物体の距離分布の情報を取得可能なスキャン型のセンサである。図９Ａは、ＬｉＤＡＲセンサ１１０をトラクタ１００Ａの側面方向から見た場合の模式図である。図９Ｂは、ＬｉＤＡＲセンサ１１０を鉛直上方から見た場合の模式図である。図９Ａおよび図９Ｂにおいて放射状に延びる複数の直線は、ＬｉＤＡＲセンサ１１０から出射されるレーザビームの中心軸（あるいは進行方向）を模式的に表している。各レーザビームは、平行光にコリメートされているが、数ミリラジアン（例えば０．１～０．２度）の拡がり角度を有している。このため、各レーザビームの断面サイズ（スポット径）は、ＬｉＤＡＲセンサ１１０からの距離に比例して拡大する。例えば、ＬｉＤＡＲセンサ１１０から２０メートル先で直径が数センチメートルの光スポットが形成され得る。図では、簡単のため、レーザビームの広がりは無視し、レーザビームの中心軸のみが記載されている。
　図９Ａに示す例におけるＬｉＤＡＲセンサ１１０は、縦方向に配列された複数のレーザ光源から、それぞれ、異なる仰角でレーザビームを出射することができる。仰角は、ｕｖ面に対する角度によって定義される。この例において、ｕｖ面は水平面にほぼ平行である。なお、地面（地表）が水平面に対して傾斜している場合は、ｕｖ面と水平面とが交差する。図９Ａには、Ｎ本のレーザビームＬ１、・・・、ＬＮが出射されている様子が示されている。ここで「Ｎ」は、１以上の整数であり、例えば１０以上、高性能な機種では６４、あるいは１００以上であり得る。複数のレーザビームのうち、下からｋ番目のレーザビームの仰角をθｋとする。図９Ａには、一例として、Ｎ－１番目のレーザビームの仰角θＮ－１が示されている。ｕｖ面よりも上方に向かうレーザビームの仰角を「正の仰角」とし、ｕｖ面よりも下方に向かうレーザビームの仰角を「負の仰角」とする。
　Ｎが１であるＬｉＤＡＲセンサを「２次元ＬｉＤＡＲ」と称し、Ｎが２以上であるＬｉＤＡＲセンサを「３次元ＬｉＤＡＲ」と称する場合がある。Ｎが２以上の場合、１番目のレーザビームとＮ番目のレーザビームとのなす角度を、「垂直視野角」と称する。垂直視野角は、例えば２０°から６０°程度の範囲内に設定され得る。
　ＬｉＤＡＲセンサ１１０は、図９Ｂに示すように、レーザビームの出射方向（方位角）を変化させることができる。図９Ｂには、図９Ａに示す複数のレーザビームの出射方向が、ｗ軸に平行な回転軸の周りに回転している様子が示されている。レーザビームの出射方向（方位角）の範囲は３６０°であってもよいし、３６０°よりも小さい角度の範囲（例えば２１０°または２７０°等）であってもよい。レーザビームの出射方向の方位角の範囲を「水平視野角」と称する。水平視野角は、例えば９０°から３６０°程度の範囲内に設定され得る。ＬｉＤＡＲセンサ１１０は、レーザビームの出射方向をｗ軸に平行な回転軸の周りに回転させながら、異なる方位角の方向にパルス状のレーザ光（レーザパルス）を順次出射する。こうして、異なる仰角および異なる方位角に出射されるパルス状のレーザ光により、各反射点までの距離を計測することが可能になる。各反射点は、点群データに含まれる個々の点に相当する。レーザビームの方位角が回転軸の周りに一回転する間に反射点までの距離を測定する動作を１スキャンと称する。１スキャンによって得られるセンサデータは、図９Ａに示す特定の仰角に対応付けられたレイヤごとに測定されるデータを含む。従って、レイヤの数が増加するほど、同一環境に対する１スキャンによって得られる点群中の点の個数は増加する。ＬｉＤＡＲセンサ１１０は、例えば１秒あたり１回から２０回程度の頻度でスキャン動作を繰り返す。１スキャンの動作中に例えば１０万パルス以上のレーザ光が異なる方向に出射され得る。
　図１０は、ＬｉＤＡＲセンサ１１０の概略的な構成の例を示すブロック図である。図１０に示すように、ＬｉＤＡＲセンサ１１０は、複数のレーザユニット１１１と、電気モータ１１４と、制御回路１１５と、信号処理回路１１６と、メモリ１１７とを備える。各レーザユニット１１１は、レーザ光源１１２と、光検出器１１３とを備える。各レーザユニット１１１は、レンズおよびミラーなどの光学系を含み得るが、それらの図示は省略されている。モータ１１４は、例えば、各レーザ光源１１２から出射されるレーザビームの光路上に配置されたミラーを回転させることにより、各レーザ光源１１２から出射されるレーザビームの方向を変化させる。
　レーザ光源１１２は、レーザダイオードを含み、制御回路１１５からの指令に応答して所定の波長のパルス状のレーザビームを出射する。レーザビームの波長は、例えば近赤外線の波長域（およそ７００ｎｍから２．５μｍ）に含まれる波長であり得る。使用される波長は、光検出器１１３に使用される光電変換素子の材料に依存する。例えばシリコン（Ｓｉ）を光電変換素子の材料として用いる場合、９００ｎｍ前後の波長が主に使用され得る。インジウムガリウム砒素（ＩｎＧａＡｓ）を光電変換素子の材料として用いる場合、例えば１０００ｎｍ以上１６５０ｎｍ以下の波長が使用され得る。なお、レーザビームの波長は、近赤外線の波長域に限定されない。環境光の影響が問題にならない用途（例えば夜間用等）においては、可視域（およそ４００ｎｍから７００ｎｍ）に含まれる波長を使用してもよい。用途によっては紫外線の波長域を使用することも可能である。本明細書において、紫外線、可視光、および赤外線の波長域に含まれる放射全般を「光」と称する。
　光検出器１１３は、レーザ光源１１２から出射され、物体で反射または散乱されたレーザパルスを検出する装置である。光検出器１１３は、例えばアバランシェフォトダイオード（ＡＰＤ）などの光電変換素子を備える。光検出器１１３は、受光量に応じた電気信号を出力する。
　モータ１１４は、制御回路１１５からの指令に応答して各レーザ光源１１２から出射されるレーザビームの光路上に配置されたミラーを回転させる。これにより、レーザビームの出射方向を変化させるスキャン動作が実現される。
　制御回路１１５は、レーザ光源１１２によるレーザパルスの出射、光検出器１１３による反射パルスの検出、およびモータ１１４による回転動作を制御する。制御回路１１５は、例えばマイクロコントローラユニット（ＭＣＵ）などの、プロセッサを備える回路によって実現され得る。
　信号処理回路１１６は、光検出器１１３から出力された信号に基づく演算を行う回路である。信号処理回路１１６は、例えばＴｏＦ（Time of Flight）法によって各レーザ光源１１２から出射されたレーザパルスを反射した物体までの距離を計算する。ＴｏＦ法には、直接ＴｏＦ法と間接ＴｏＦ法とがある。直接ＴｏＦ法においては、レーザ光源１１２からレーザパルスが出射されてから光検出器１１３によって反射光が受光されるまでの時間を直接計測することにより、反射点までの距離を計算する。間接ＴｏＦ法においては、光検出器１１３に複数の露光期間が設定され、各露光期間で検出された光量の比に基づいて各反射点までの距離が計算される。直接ＴｏＦ法および間接ＴｏＦ法のいずれの方法も用いられ得る。信号処理回路１１６は、例えば、各反射点までの距離と、その反射点の方向とを示すセンサデータを生成して出力する。信号処理回路１１６は、さらに、各反射点までの距離と、その反射点の方向とに基づいて、センサ座標系における座標（ｕ，ｖ）または（ｕ，ｖ，ｗ）を計算し、センサデータに含めて出力してもよい。
　制御回路１１５および信号処理回路１１６は、図１０の例では２つの回路に分かれているが、１つの回路によって実現されていてもよい。
　メモリ１１７は、制御回路１１５および信号処理回路１１６によって生成されるデータを記憶する記憶媒体である。メモリ１１７は、例えば、各レーザユニット１１１から出射されるレーザパルスの出射タイミングと、出射方向と、反射光強度と、反射点までの距離と、センサ座標系における座標（ｕ，ｖ）または（ｕ，ｖ，ｗ）とを関連付けたデータを記憶する。そのようなデータが、レーザパルスが出射される度に生成され、メモリ１１７に記録される。制御回路１１５は、所定の周期（例えば、所定回数のパルス出射に要する時間、半スキャン周期、または１スキャン周期等）で当該データを出力する。出力されたデータは、トラクタ１００Ａの記憶装置１５０に記録される。
　なお、距離計測の方法はＴｏＦ法に限らず、ＦＭＣＷ（Frequency Modulated Continuous Wave）法などの他の方法を用いてもよい。ＦＭＣＷ法においては、周波数を線形的に変化させた光が出射され、出射光と反射光との干渉で生じるビートの周波数に基づいて距離が計算される。
　＜動作＞
　次に、トラクタ１００Ａの動作を説明する。
　図１１は、トラクタ１００Ａが走行する環境の一例を模式的に示す図である。この例では、トラクタ１００Ａは、ぶどう園における複数の樹木列２０の間を走行しながら、作業機３００を用いて所定の作業（例えば、草刈り、防除、その他）を行う。ぶどう園では、上空が枝および葉で遮られ、ＧＮＳＳを用いた自動走行が難しい。また、ぶどう園では、樹木の葉または垣根の外形が季節によって大きく変化する。そのため、従来のＳＬＡＭ技術によって作成された点群地図を通期で使用することが難しい。
　そこで、本開示の実施形態では、ぶどう園などの果樹園に適した環境地図を用意し、その環境地図に基づいてトラクタ１００Ａの自動走行を行う。例えばぶどう園における樹木は、一度植えられると長期間植え替えられることがなく、幹部は葉部に比べて季節変化による外形の変化が少ない。幹部をＳＬＡＭのランドマークとすることで、通期で使用可能な環境地図を作成し、年間を通じて環境地図を作成し直すことなく自動走行作業を行うことができる。
　以下、トラクタ１００Ａの動作を具体的に説明する。ＥＣＵ１６０は、ＬｉＤＡＲセンサ１１０から出力されるセンサデータを利用して、ぶどう園全体またはぶどう園の一区画における樹木列２０の幹部の分布を示す環境地図データをまず生成し、記憶装置１５０に記録する。環境地図データの生成は、自己位置推定モジュール１６２による自己位置推定と、地図データ生成モジュール１６４による局所地図データの生成とを繰り返すことによって実行される。地図データ生成モジュール１６４は、トラクタ１００Ａが移動している間にＬｉＤＡＲセンサ１１０から繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、トラクタ１００Ａの周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す局所地図データを生成して記憶装置１５０に記録する動作を繰り返す。地図データ生成モジュール１６４は、トラクタ１００Ａがぶどう園全体またはぶどう園の一区画を走行する間に生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成する。この環境地図データは、環境中の樹木列２０の幹部の分布を、他の物体と区別可能な形式で記録したデータである。このように、本実施形態では、樹木列２０の幹部がＳＬＡＭのランドマークとして使用される。
　複数の樹木列が配置されたエリアには１つ以上の支柱が設けられることがある。例えば、ぶどう園においては、一般に、垣根仕立てのための複数の支柱が樹木の近傍に設けられる。支柱は、樹木の幹部と同様、季節による外形の変化が少ないため、ランドマークとして適している。そこで、自己位置推定モジュール１６２は、センサデータに基づいて、トラクタ１００Ａの周囲の環境における支柱をさらに検出し、地図データ生成モジュール１６４は、幹部の分布に加えて支柱の分布も示す環境地図データを生成してもよい。
　環境地図データが生成されると、トラクタ１００Ａによる自動走行または自律走行が可能になる。ＥＣＵ１６０の自己位置推定モジュール１６２は、トラクタ１００Ａの走行中、ＬｉＤＡＲセンサ１１０から繰り返し出力されるセンサデータに基づいて、周囲の環境における樹木列２０の幹部を検出し、検出した樹木列２０の幹部と環境地図データとのマッチングを行うことにより、トラクタ１００Ａの位置を推定する。環境地図データに支柱の分布情報が含まれる場合は、自己位置推定モジュール１６２は、センサデータに基づいて検出した樹木列の幹部および支柱と、環境地図データとのマッチングを行うことにより、トラクタ１００Ａの位置を推定してもよい。駆動制御用のＥＣＵ１８０は、ＥＣＵ１６０によって推定されたトラクタ１００Ａの位置に応じてトラクタ１００Ａの移動を制御する制御装置である。例えば、ＥＣＵ１８０は、ＥＣＵ１７０によって決定された走行予定経路から逸脱している場合に、トラクタ１００Ａのステアリングを調整することにより、走行予定経路に近付ける制御を行う。このような操舵制御は、トラクタ１００Ａの位置だけでなく向きに基づいて行われてもよい。
　樹木列２０の幹部の分布を示す環境地図は、任意の時期に生成することができる。例えば、冬などの樹木の葉部が少ない季節に環境地図を生成することができる。その場合、夏などの樹木の葉部が多い季節に環境地図を生成する場合と比較して、幹部の分布をより正確に反映した環境地図を生成し易い。冬に環境地図が生成される場合、冬だけでなくそれ以外の季節においても、ＥＣＵ１６０は、その環境地図に基づいて自己位置推定を行ってもよい。その場合、ＥＣＵ１６０は、冬の間に生成された環境地図データと、ＬｉＤＡＲセンサ１１０から出力されたセンサデータから幹部以外の部分を除去したデータとのマッチングを行うことにより、自己位置推定を行う。
　ＥＣＵ１６０は、トラクタ１００Ａの走行中に繰り返し取得されたセンサデータに基づいて検出した樹木列の幹部の分布を示すデータを生成し、当該データを用いて環境地図データを更新してもよい。例えば、ＥＣＵ１６０は、新たに取得されたセンサデータから検出された幹部の情報を前回生成した環境地図データに加えることによって環境地図データを更新してもよい。あるいは、検出した樹木列の幹部の分布を示すデータを、環境地図データを更新する外部の装置に送信してもよい。その場合、ＥＣＵ１６０は、推定されたトラクタ１００Ａの位置を示す情報を当該幹部の分布を示すデータ（例えば局所地図データ）に含めて出力する。外部の装置は、取得したデータを用いて環境地図データを更新し、更新後の環境地図データをトラクタ１００Ａに送信することができる。そのような動作により、樹木の生長などに起因して、環境地図を作成した時点から走行環境が変化した場合であっても、その変化を反映した環境地図を新たに作り直すことができる。
　図１２は、トラクタ１００Ａの周囲の環境の例を模式的に示す斜視図である。トラクタ１００Ａは、ぶどう園内の隣り合う２つの樹木列の間を走行する。トラクタ１００Ａは、走行しながら、ＬｉＤＡＲセンサ１１０を用いて周囲の環境をレーザビームでスキャンする。これにより、環境内に存在する物体の距離分布を示すデータを取得する。距離分布を示すデータは、２次元または３次元の点群データに変換され、記録される。トラクタ１００Ａは、ぶどう園内を走行しながら繰り返し取得した点群データを統合することにより、ぶどう園全体、またはぶどう園の一区画における樹木列の幹部の分布を示す環境地図データを生成する。点群データの統合は、異なるタイミングで取得された複数の反射点の座標を、地球に固定されたワールド座標系における座標に変換し、繋ぎ合わせる処理を含む。本実施形態では、トラクタ１００ＡのＥＣＵ１６０が環境地図データを生成するが、トラクタ１００Ａの外部に設けられたコンピュータが環境地図データを生成してもよい。
　図１３Ａは、環境地図生成時、および自動走行時のトラクタ１００Ａの走行経路の一例を模式的に示す図である。トラクタ１００Ａは、例えば図１３Ａに矢印で示す経路３０に沿ってぶどう園内の複数の樹木列２０の間を走行する。図では、経路３０に含まれる線分が直線状に記載されているが、トラクタ１００Ａが実際に走行する経路は、蛇行部分を含んでいてもよい。ここで、複数の樹木列２０を、端から順に第１の樹木列２０Ａ、第２の樹木列２０Ｂ、第３の樹木列２０Ｃ、第４の樹木列２０Ｄ、・・・と順序付けする。図１３Ａの例では、トラクタ１００Ａは、第１の樹木列２０Ａと第２の樹木列２０Ｂとの間をまず走行し、その走行が完了すると、旋回して第２の樹木列２０Ｂと第３の樹木列２０Ｃとの間を逆向きに走行する。第２の樹木列２０Ｂと第３の樹木列２０Ｃとの間の走行が完了すると、さらに旋回して第３の樹木列２０Ｃと第４の樹木列２０Ｄとの間を走行する。以後、同様の動作を繰り返すことにより、最後の２つの樹木列の間の経路３０の終端まで走行する。なお、隣り合う樹木列の間の距離が短い場合は、図１３Ｂに示すように、１列おきに走行してもよい。この場合、最後の２つの樹木列の間の走行が完了した後、まだ走行していない樹木列間を１列おきに走行する動作が実行され得る。このような走行は、トラクタ１００ＡがＬｉＤＡＲセンサ１１０を利用して自己位置推定を行いながら、自動で行う。なお、ＧＮＳＳユニット１２０がＧＮＳＳ信号を受信できるタイミングにおいては、ＧＮＳＳ信号に基づいて測位を行ってもよい。例えば、図１３Ａおよび図１３Ｂに示す経路３０において方向転換するタイミングでは、ＧＮＳＳ信号を遮る葉が存在しないことから、ＧＮＳＳ信号に基づく測位が可能である。
　＜センサデータ取得の例＞
　次に、図１４Ａから図１７Ｃを参照して、トラクタ１００Ａによるセンサデータの取得の例を説明する。センサデータは、環境地図が既に構築済みの場合は、環境地図とのマッチングを行うことによってトラクタ１００Ａの位置を推定するために利用される。一方、環境地図が構築されていない場合、あるいは環境地図の少なくとも一部を更新する場合には、センサデータに基づいて環境地図の構築または更新が行われる。環境地図の構築および更新は、トラクタ１００Ａの移動中に行われる必要はなく、トラクタ１００Ａが移動中に取得したセンサデータをトラクタ１００Ａの内部または外部のコンピュータによって処理することで実行され得る。
　図１４Ａは、トラクタ１００ＡのＬｉＤＡＲセンサ１１０から出射されたレーザビームで樹木２１および地面が照射される様子を模式的に示す図である。図１４Ｂは、ＬｉＤＡＲセンサ１１０から出射されたレーザビームで地面が照射される様子を模式的に示す図である。これらの図において、ＬｉＤＡＲセンサ１１０から負の仰角の方向に出射されたレーザビームは太い破線で表され、ＬｉＤＡＲセンサ１１０から正の仰角の方向に出射されたレーザビームは細い破線で表されている。ＬｉＤＡＲセンサ１１０から異なる仰角で出射される複数のレーザビームのうち、負の仰角の方向に出射されたレーザビームは、樹木２１に当たらない場合、地面で反射され得る。一方、正の仰角の方向に出射されたレーザビームは、樹木２１に当たらない場合、反射点を形成せず、樹木２１に当たった場合の反射点は、葉や枝の表面に位置することが多い。前述したように、葉や枝の表面に位置する反射点は、環境地図データとのマッチングに適さない。このため、本実施形態では、幹部での反射を利用して自己位置推定および環境地図の構築を行う。具体的には、ＬｉＤＡＲセンサ１１０から異なる仰角で出射される複数のレーザビームのうち、仰角が所定の範囲に含まれるレーザ光源から出射されたレーザパルスの反射点に基づいて幹部を検出してもよい。また、反射点の高さが幹部の平均的な高さよりも低い反射点に基づいて幹部を検出してもよい。これらの場合、幹部の検出後に、地面に位置する反射点をセンサデータの点群から選択的にカットしてもよい。
　図１５Ａおよび図１５Ｂは、トラクタ１００ＡのＬｉＤＡＲセンサ１１０による１スキャン動作で取得されるセンサデータの点群の例を模式的に示す平面図である。図１５Ａは、ある時点におけるトラクタ１００Ａと、ＬｉＤＡＲセンサ１１０から放射されたレーザパルスの反射点の分布の例を模式的に示している。この例では、地面は平坦な面であると仮定している。地面で反射されたレーザビームは、地面で円弧上に反射点を形成する。このような地面上の反射点は、仰角が負の方向に出射されたレーザビームによって形成される。図１５Ａにおいて、半径が異なる７本の円弧（７つのレイヤに相当）上に反射点が並んでいる。それぞれのレイヤは、異なる負の仰角で出射されたレーザビームが方位角方向に回転するスキャンによって形成される。このスキャンが行われているとき、水平方向または正の仰角の方向にレーザビームによるスキャンが行われていてもよい。なお、同一レイヤ上に並ぶ反射点は、同一のレーザ光源から同一の仰角で出射されたレーザビームが回転しながら物体表面にパルス状に入射して形成される。図１５Ｂは、図１５Ａの時点からわずかに時間が経過した時点におけるトラクタ１００Ａと、ＬｉＤＡＲセンサ１１０から放射されたレーザパルスの反射点の分布の例を模式的に示している。
　ＬｉＤＡＲセンサ１１０から放射されたレーザパルスの一部は、樹木の幹部２２の表面で反射される。地面、樹木の幹部２２、または他の物体で反射されたレーザパルスの一部は、反射点が測距可能距離（例えば５０ｍ、１００ｍ、または２００ｍ等）を超える遠方に位置する場合を除き、ＬｉＤＡＲセンサ１１０によって検出され、反射点までの距離が計測される。ＬｉＤＡＲセンサ１１０は、例えば、反射点ごとに、反射点までの距離と、反射点の方向と、反射光強度と、反射点を形成したレーザビームを出射したレーザ光源の識別番号とを関連付けたセンサデータを生成して出力する。このセンサデータには、計測時刻の情報も含まれ得る。計測時刻の情報は、例えば反射点ごと、または一定の時間内に計測された反射点のまとまりごとに記録され得る。ＥＣＵ１６０の自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０から出力されたセンサデータを点群データに変換する。点群データは、ＬｉＤＡＲセンサ１１０に固定されたセンサ座標系で表現された各反射点の３次元座標（ｕ，ｖ，ｗ）の情報を含むデータである。なお、ＬｉＤＡＲセンサ１１０が各反射点の距離および方向のデータを点群データに変換した上で出力する場合には、自己位置推定モジュール１６２による点群データへの変換は省略される。
　環境地図の構築時において、トラクタ１００Ａが走行している間、地図データ生成モジュール１６４は、点群データから、幹部２２を他の部分と区別可能な形式で記録した局所地図データを生成する。トラクタ１００Ａの走行中、地図データ生成モジュール１６４は、それまでに生成した局所地図データに、新たに取得したセンサデータに基づく局所地図データを追加して更新する動作を繰り返す。これにより、最終的な環境地図データを生成することができる。なお、地図データ生成モジュール１６４は、トラクタ１００Ａの走行中には局所地図データを生成して記録する動作のみを行い、最終的な環境地図データの生成は、走行完了後に行ってもよい。その場合、最終的な環境地図データは、外部のコンピュータが生成してもよい。
　本実施形態では、ＬｉＤＡＲセンサ１１０がトラクタ１００Ａの車体の前部に取り付けられている。したがって、図１５Ａおよび図１５Ｂに示すように、レーザパルスは、トラクタ１００Ａの前方に向けて放射される。トラクタ１００Ａの後部にもＬｉＤＡＲセンサ１１０が取り付けられていてもよい。その場合には、トラクタ１００Ａの前方と後方の両方の点群データを一度に取得できるため、より点群密度の高い地図データを生成することができる。また、環境地図が生成された後の自動走行におけるマッチングによる自己位置推定の精度を向上させることができる。さらに、個々の幹部２２の表面における広い範囲から点群データが得られるため、その点群データから幹部２２の太さを高い精度で計測することもできる。このように、ＥＣＵ１６０は、センサデータに基づいて、検出された樹木列の各幹部の太さを計測し、各幹部の太さを記憶媒体に記録する記録装置としても機能する。記憶媒体は、記憶装置１５０内の記憶媒体でもよいし、他の記憶媒体でもよい。幹部の太さは、各ランドマークの属性情報のひとつとして環境地図データに加えられてもよいし、他のベクタ形式の地図データに記録されてもよい。
　図１６は、複数のレイヤ（Ｌ１、・・・、Ｌｋ＋３）のレーザビームの１スキャンによって得られる幹部２２の点群の例を模式的に示す図である。図１７Ａから図１７Ｃは、それぞれ、図１６に示す複数のレイヤのうちの３つのレイヤ（Ｌｋ、Ｌｋ＋１、Ｌｋ＋２）のレーザビームのスキャンによって幹部２２の点群が得られる様子を模式的に示す図である。この図に示すように、スキャン毎に、計測可能な範囲に存在する幹部２２の分布を示す点群データを得ることができる。より詳細には、１スキャンによって個々の反射点までの距離を計測するとき、それぞれのレイヤにおいて、距離が相対的に長い反射点が連続する領域と、距離が相対的に短い反射点が局所的に表れる領域とが含まれ得る。
　図１８は、複数のレイヤ（Ｌｋ、Ｌｋ＋１、Ｌｋ＋２、・・・）のレーザビームによって１スキャンで計測される反射点までの距離と反射点の方位角との関係の例を示すグラフである。図１８に示される方位角の範囲は狭く、この範囲では２本の幹部からの反射が示されている。この例において、各レイヤの主たる反射点は地面に位置している。個々のレイヤに属する反射点のうち、相対的に距離が短い反射点は、幹部の表面に対応する曲線上に並んでいる。２つの幹部のうち、距離が相対的に短い位置にある幹部は、レイヤＬｋ、Ｌｋ＋１、Ｌｋ＋２、・・・のレーザビームで照射される。これに対し、距離が相対的に長い位置にある幹部は、より少ないレイヤＬｋ＋１、Ｌｋ＋２、・・・のレーザビームで照射される。このため、３次元空間中の幹部の表面における点群を水平面に平行な平面に投影した場合、距離が相対的に短い位置にある幹部の点群の密度が相対的に高い。図１８に示される計測結果に基づいて、幹部を検出することができる。実際には地面には起伏が存在したり、雑草が存在したりするため、距離の計測値は図示されるほど単純ではない。
　トラクタ１００Ａが移動しながら上記のスキャンを繰り返すことにより、トラクタ１００Ａの周囲に位置する各幹部から密度の高い点群のデータを取得することが可能である。したがって、移動するトラクタ１００Ａが取得する大量のセンサデータを利用することにより、幹部の分布を示す精度の高い環境地図の構築に必要な高密度の点群データを取得することができる。
　なお、反射点までの距離ではなく、反射光の強度と反射点の方位角との関係から幹部を検出することもできる。同一レイヤのレーザビームによる反射光の強度は、反射点までの距離が短いほど大きくなるからである。
　上述したように、幹部の分布を示す環境地図と比較されるべきセンサデータ（以下、「スキャンデータ」とも称する。）には、幹部の表面に位置する反射点以外の多数の反射点が含まれ得る。このため、取得されたスキャンデータから、幹部の表面に位置する可能性が高い反射点を選択することが有効である。また、スキャンデータは、１スキャンによって得られた反射点だけではなく、連続的に取得された複数のスキャンによって得られた反射点を統合して密度の高い点群データを生成してもよい。
　なお、図１７Ａから図１７Ｃでは省略されているが、例えば正の仰角で出射されるレーザビームは、幹部以外の葉部や枝部を照射して多数の反射点を形成し得る。本実施形態におけるＥＣＵ１６０は、スキャンデータに基づいて生成した点群データから、幹部２２の特徴を有する一部の点を抽出することにより、幹部２２の分布を示すデータを生成する。
　図１９は、ある位置におけるトラクタ１００Ａで取得されたスキャンデータから、幹部の表面に位置する反射点を抽出して作成した局所地図の例を示している。この例では、幹部の２次元分布を示す２次元の局所地図が作成されている。このような局所地図では、その位置におけるトラクタ１００Ａの周囲に位置する幹部の座標が、トラクタ１００Ａに固定されたｕｖ座標系で表現され得る。
　図２０Ａおよび図２０Ｂは、図１９の局所地図を、構築済みの環境地図とマッチングさせることにより、トラクタ１００Ａの位置を推定するプロセスを模式的に示す平面図である。局所地図における各座標はセンサ座標系で表現され、環境地図における各座標はワールド座標系で表現されている。マッチングは、センサ座標系からワールド座標系への座標変換を最適化することである。そのような座標変換の最適化により、ワールド座標系におけるセンサの位置座標および向きが決定される。図２０Ａおよび図２０Ｂにおいて、環境地図における幹部は黒塗りで表現されている。図２０Ａは、マッチングの途中の状態を示している。この状態では、局所地図上にある幹部と環境地図上にある対応する幹部との間には位置ずれ（誤差）が存在する。図２０Ｂは、マッチングが完了した状態を示している。この状態では、局所地図上にある幹部と環境地図上にある対応する幹部との間の位置ずれ（誤差）は最小化されている。
　このように幹部を抽出したスキャンデータと幹部の分布を示す環境地図を利用することにより、精度の高い位置推定が実現する。なお、上記の例では、２次元の環境地図に基づくマッチングが行われるが、３次元の環境地図に基づいてマッチングを行ってもよい。
　＜ＬｉＤＡＲセンサの配置の例＞
　次に、図２１Ａおよび図２１Ｂを参照して、ＬｉＤＡＲセンサ１１０の配置の例を説明する。
　図２１Ａは、本実施形態のように、トラクタ１００Ａの車両本体の前方下部にＬｉＤＡＲセンサ１１０が取り付けられた例を示している。図２１Ｂは、トラクタ１００Ａのキャビンの前方上部にＬｉＤＡＲセンサ１１０が取り付けられた例を示している。これらの図において、太い破線は樹木２１の幹部に照射されるレーザビームを示しており、細い破線は樹木の葉部またはその上空に向けて出射されるレーザビームを示している。
　図２１Ａの例において、ＬｉＤＡＲセンサ１１０は、樹木列の幹部の平均高さよりも低い位置に搭載されている。樹木列の幹部の平均高さは、トラクタ１００Ａの走行予定経路に存在する複数の樹木列に含まれる樹木の幹部の高さの平均値である。樹木の幹部の高さは、地面から、その樹木において最も低い枝を生ずる部分までの距離である。ＬｉＤＡＲセンサ１１０は、例えば地上から１０ｃｍ以上１５０ｃｍ以下、ある例では１５ｃｍ以上１００ｃｍ以下の高さに配置され得る。このような低い位置にＬｉＤＡＲセンサ１１０を配置することにより、葉や枝によって遮られることなく、より多くのレーザビームで幹部を照射することができる。このため、センサデータから幹部の位置を高い精度で決定することが可能になる。
　一方、図２１Ｂの例では、ＬｉＤＡＲセンサ１１０が高い位置（例えば地上から約２ｍの位置）に配置されていることから、レーザビームの多くは、幹部ではなく葉部を照射する。このため、幹部の点群データを取得する用途においては、図２１Ｂのような配置は適さない。しかし、このような配置は、樹木列の葉部の分布情報を効率的に取得できるため、葉部の分布情報を積極的に取得する用途においては有効である。例えば、樹木の生育状況を管理したり、樹木の葉部を避ける経路を決定したりする目的のために図２１Ｂのような配置が採用されてもよい。なお、図２１Ｂのような配置においても、ＬｉＤＡＲセンサ１１０を下向きに取り付けることにより、より多くのレーザビームで幹部を照射することができる。このため、幹部の分布データを取得する用途においても図２１Ｂに示すような配置が採用される場合もある。ＬｉＤＡＲセンサ１１０は、車体の前方下部およびキャビンの前方上部に限らず、他の位置に配置されていてもよい。例えば、ＬｉＤＡＲセンサ１１０は、キャビンの高さよりも低い位置に配置され得る。
　なお、本実施形態では１つのＬｉＤＡＲセンサ１１０が使用されるが、トラクタ１００Ａは複数のＬｉＤＡＲセンサを備えていてもよい。複数のＬｉＤＡＲセンサから出力されるセンサデータを組み合わせることにより、さらに効率的に幹部の分布データを取得することができる。複数のＬｉＤＡＲセンサは、例えばトラクタ１００Ａの左右に設けられていてもよいし、前部および後部に設けられていてもよい。複数のＬｉＤＡＲセンサを備えた移動体の実施形態の詳細については後述する。
　ＥＣＵ１６０における自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０における複数のレーザ光源のうち、仰角が所定の範囲に含まれるレーザ光源から出射されたレーザパルスの反射点に基づいて幹部を検出してもよい。例えば、負の仰角の方向に出射されるレーザパルスの反射点のみに基づいて幹部を検出してもよい。幹検出に利用されるレーザパルスの仰角の範囲をユーザが設定できるようにしてもよい。例えば、ユーザが操作端末２００を操作することによって幹検出に利用されるレーザパルスの仰角の範囲を設定できるようにしてもよい。ＬｉＤＡＲセンサ１１０から出射されるレーザパルスのうち、幹部を照射する可能性の高い特定の仰角の範囲に出射されるレーザパルスのみを利用することにより、幹部をより効率的に検出することができる。また、地面に起伏があり、センサ座標系におけるｕｖ面が水平面から大きく傾斜するときは、センサ座標系の傾斜角に応じて、幹部で反射されたレーザパルスを適切に選択するように、レーザビームの仰角範囲を適応的に選択して反射点を抽出してもよい。ＬｉＤＡＲセンサ１１０の傾斜角は、ＩＭＵ１２５からの信号を利用して求めることができる。
　＜環境地図構築後の自動走行動作の例＞
　次に、環境地図が作成された後の自動走行動作の例を説明する。
　図２２は、ＥＣＵ１６０における自己位置推定モジュール１６２の機能構成の例を示す図である。自己位置推定モジュール１６２は、記憶装置１５０に格納されたコンピュータプログラム（ソフトウェア）を実行することにより、ＬｉＤＡＲセンサ１１０から出力されたスキャンデータと地図データとのマッチングを行い、トラクタ１００Ａの位置および向きを推定する。
　自己位置推定モジュール１６２は、スキャンデータ取得１６２ａ、地図データ取得１６２ｂ、ＩＭＵデータ取得１６２ｃ、スキャンデータフィルタリング１６２ｄ、マッチング１６２ｅ、および車両位置・向き決定１６２ｆの各処理を実行する。以下、これらの処理の詳細を説明する。
　（スキャンデータ取得１６２ａ）
　自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０から出力されたスキャンデータを取得する。ＬｉＤＡＲセンサ１１０は、例えば毎秒１回から２０回程度の頻度で、スキャンデータを出力する。このスキャンデータは、センサ座標系で表現された複数の点の座標と、タイムスタンプの情報を含み得る。なお、スキャンデータが各点までの距離および方向の情報を含み、座標情報を含まない場合は、自己位置推定モジュール１６２が距離および方向の情報から座標情報への変換を行う。
　（地図データ取得１６２ｂ）
　自己位置推定モジュール１６２は、記憶装置１５０に格納された環境地図データを取得する。環境地図データは、トラクタ１００Ａが走行する環境内に含まれる樹木列の幹部の分布を示す。環境地図データは、例えば以下の（１）～（３）のいずれかの形式のデータを含む。
（１）幹部と幹部以外の物体とが識別可能な形式で記録されたデータ
　例えば、幹部であると判断された点に数値「１」が付与され、幹部以外の物体であると判断された点に数値「０」が付与されたデータが該当する。個々の幹部を識別するための幹ＩＤが環境地図データに含まれていてもよい。
（２）幹部に相対的に大きい重みが付与され、幹部以外の物体に相対的に小さい重みが付与されたデータ
　例えば、幹部の表面上の点であると推定される確率が高い点ほど大きい数値が付与されるデータが該当する。
（３）検出された幹部の分布の情報を含み、幹部以外の物体の一部または全部の分布の情報を含まないデータ
　例えば、環境地図を表す点群から、幹部の表面であると判断された点のみを残して他の点を削除したデータが該当する。幹部ではないと判断された点の全てが削除されるのではなく、一部の点が残されていてもよい。例えば、ぶどう園では一般に、垣根仕立てのための支柱が幹部の周囲に設けられるため、そのような支柱を表す点の情報が環境地図に含まれていてもよい。
　点群中のある点が幹部または支柱に該当するか否かは、例えば、その点およびその点の周囲の複数の点の分布が、幹部または支柱の表面形状を反映した分布（例えば下に凸の円弧状の分布等）であるか否かに基づいて判断され得る。あるいは、各回のスキャンで取得された曲線的に分布する点群のデータから、近傍の点に比べてＬｉＤＡＲセンサ１１０からの距離が局所的に近い点の集まりを抽出し、それらの点を幹部または支柱を表す点であると判断してもよい。各回のスキャンで取得された曲線的に分布する点群のデータを、ＬｉＤＡＲセンサ１１０からの距離に応じて複数のクラスに分類し、クラスごとに幹部または支柱に該当するか否かを判断してもよい。また、距離だけでなく反射強度の情報にも基づいて点群を分類してもよい。樹木の幹とその周囲の他の部分とでは、反射強度が明確に異なるため、反射強度の類似性と位置の類似性とに基づいて点群を分類することが有効である。例えば、反射強度が所定の範囲内にあり、かつ位置が近接する複数の点を幹部の表面を表す点の候補としてもよい。ＬｉＤＡＲセンサ１１０から複数の異なる波長のレーザビームを出射し、波長ごとの反射強度の比に基づいて点群を分類し、幹部を検出してもよい。
　幹部の検出には、機械学習が利用されてもよい。深層学習などのニューラルネットワークに基づく機械学習アルゴリズムを利用することにより、点群データから樹木の幹の表面に該当する点を高い精度で検出することができる。機械学習アルゴリズムが用いられる場合、点群データから幹部を検出するための学習済みモデルの作成（すなわち学習）が事前に行われる。
　樹木の幹部に限らず、それ以外の物体を認識してもよい。例えばパターン認識または機械学習によって点群データから地面、雑草、および樹木の葉部等を認識してそれらの点を除去することにより、樹木の幹部に該当する点を主に含む点群データを生成してもよい。幹部の検出に先立って、地面からの高さが所定の範囲（例えば、０．５ｍから１．５ｍ等）に含まれる点群のみを抽出する処理を行ってもよい。そのような特定の座標範囲に含まれる点群のみを幹部検出の対象とすることにより、検出に要する時間を短縮することができる。地面からの高さは、各点のＺ座標から地面のＺ座標を減じることによって計算される。地面のＺ座標は、例えば、数値標高モデル（Digital Elevation Model：ＤＥＭ）を参照して決定され得る。地面のＺ座標は、地面を示す点群から決定されてもよい。
　図２３は、環境地図データの形式の一例を模式的に示す図である。この例における環境地図データは、点の個数の情報と、点ごとのＩＤ、分類、幹ＩＤ、Ｘ座標、Ｙ座標、Ｚ座標、および反射強度の情報とを含む。点の個数をｎとすると、点ＩＤ、分類、幹ＩＤ、Ｘ座標、Ｙ座標、Ｚ座標、および反射強度のデータの組がｎ組分記録される。分類は、その点が何を表すかを示し、例えば樹木の幹、垣根の支柱、地面、雑草、樹木の葉などを示す識別子を含む。分類は、樹木の幹であるか否かを示す２値の数値、または、樹木の幹である確率を示す数値であってもよい。幹ＩＤは、その点が幹を表す場合に、その幹を識別するための番号である。幹ＩＤは環境地図データに含まれていなくてもよい。また、幹ＩＤに加えて、他の物体（例えば、葉、枝、地面、雑草等）の識別子が含まれていてもよい。Ｘ座標、Ｙ座標、およびＺ座標は、ワールド座標系におけるその点の座標である。反射強度は、その点からの反射光強度を表す。反射強度の情報は環境地図データに含まれていなくてもよい。環境地図データは、図２３に示す情報以外にも、データ形式を示すヘッダなどの情報を含み得る。トラクタ１００Ａが走行するぶどう園が広い場合には、ぶどう園の区画ごと（例えば、１００ｍ×１００ｍの区画ごと）に環境地図データが生成され得る。その場合には、環境地図データは、区画を識別するためのＩＤ情報も含み得る。なお、環境地図データは、各点の３次元座標を記録する代わりに、２次元座標を記録してもよい。例えば、水平面に平行な特定の高さの平面上の点群の座標を含むデータが環境地図データとして生成されてもよい。その場合、スキャンデータと環境地図データとのマッチングは、その２次元座標に関して行われる。
　（ＩＭＵデータ取得１６２ｃ）
　自己位置推定モジュール１６２は、ＩＭＵ１２５から出力されたＩＭＵデータを取得する。ＩＭＵデータは、トラクタ１００Ａの加速度、速度、変位、姿勢、計測時刻（タイムスタンプ）などの情報を含み得る。ＩＭＵデータは、例えば毎秒数十回から数千回程度の頻度で出力される。この出力周期は、一般に、ＬｉＤＡＲセンサ１１０によるスキャンデータの出力周期よりも短い。
　図２４は、ＩＭＵデータとスキャンデータの時間的な関係の一例を示す図である。この例では、ＩＭＵデータは１ミリ秒（ｍｓ）の周期で出力され、スキャンデータは１０ｍｓの周期で出力される。１スキャンに要する時間が例えば１００ｍｓであるとすると、図２４の例では、１０分の１スキャンごとにスキャンデータが出力される。スキャンデータには、計測された各点の距離および方向、または座標と、計測時刻（タイムスタンプ）の情報が含まれる。
　自己位置推定モジュール１６２は、取得したスキャンデータのタイムスタンプを参照して、対応する時間中に生成されたＩＭＵデータを取得する。
　（スキャンデータフィルタリング１６２ｄ）
　自己位置推定モジュール１６２は、取得したスキャンデータをフィルタリングし、マッチングに用いる点の個数を削減する。さらに、マッチングに不要な部分を除去する。例えば、地面、雑草、樹木の葉部、および障害物などの、樹木の幹部または垣根の支柱に該当しないと判断される点を除去することができる。
　（マッチング１６２ｅ）
　自己位置推定モジュール１６２は、フィルタされたスキャンデータと地図データとのマッチングを行う。マッチングは、例えばＮＤＴ（Normal Distribution Transform）またはＩＣＰ（Iterative Closest Point）などの任意のマッチングアルゴリズムを用いて実行され得る。マッチングにより、ＬｉＤＡＲセンサ１１０の位置および向きが決定される。
　（車両位置・向き決定１６２ｆ）
　自己位置推定モジュール１６２は、マッチングの結果に基づき、トラクタ１００Ａの位置および向きを決定し、その位置および向きを示すデータを出力する。当該データは、駆動制御用のＥＣＵ１８０に送られ、駆動装置１４０の制御に利用される。
　次に、図２５を参照しながら、本実施形態における自己位置推定動作をより詳細に説明する。
　図２５は、自己位置推定モジュール１６２による自己位置推定動作を示すフローチャートである。自己位置推定モジュール１６２は、図２５に示すステップＳ１０１からＳ１０９の動作を実行することにより、トラクタ１００Ａの位置および向き、すなわちポーズを推定する。以下、各ステップの動作を説明する。図２５に示す動作は、例えばユーザによる操作端末２００を用いた操作によって開始され得る。
　（ステップＳ１０１）
　自己位置推定モジュール１６２は、記憶装置１５０から環境地図データを読み込む。走行対象のぶどう園の区画ごとに異なる環境地図データが記録されている場合は、現在の地点に対応する環境地図データを読み込む。現在の地点に対応する環境地図データは、例えばユーザが操作端末２００を操作することによって指定され得る。あるいは、その地点でＧＮＳＳ信号を受信できる場合には、ＧＮＳＳユニット１２０が受信したＧＮＳＳ信号に基づいて現在の地点を特定し、対応する環境地図データを選択して読み込んでもよい。この例のように、トラクタ１００Ａの位置に対応する一部の環境地図データのみを読み込むことにより、処理を高速化することができる。なお、本ステップにおいて全ての環境地図データを一括して読み込んでもよい。
　（ステップＳ１０２）
　自己位置推定モジュール１６２は、位置推定の開始地点を設定する。位置推定の開始地点は、その時点でのトラクタ１００Ａの現在位置であり、例えばユーザが操作端末２００に表示された地図から特定の地点を指定することによって設定され得る。あるいは、その地点でＧＮＳＳ信号を受信できる場合には、ＧＮＳＳユニット１２０が受信したＧＮＳＳ信号に基づいて開始地点を設定してもよい。
　（ステップＳ１０３）
　開始位置が設定されると、ＬｉＤＡＲセンサ１１０の動作が開始される。自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０から出力されたスキャンデータを読み込む。ＬｉＤＡＲセンサ１１０は、所定の周期（例えば５ミリ秒以上１秒以下）でスキャンデータを出力する。スキャンデータには、レイヤごとに、例えば数度から３６０度の範囲内の点群データが含まれる。自己位置推定モジュール１６２は、例えばＬｉＤＡＲセンサ１１０からスキャンデータが出力される度に、そのスキャンデータを読み込む。あるいは、自己位置推定モジュール１６２は、スキャンデータが所定回数分出力される度に、それらのスキャンデータをまとめて読み込んでもよい。
　（ステップＳ１０４）
　自己位置推定モジュール１６２は、スキャンデータに含まれるタイムスタンプを参照して、そのスキャンデータに対応するＩＭＵデータを読み込む。
　（ステップＳ１０５）
　自己位置推定モジュール１６２は、読み込んだＩＭＵデータに基づき、マッチングの初期位置を設定する。マッチングの初期位置は、ＩＭＵデータが示す現時点でのトラクタ１００Ａの推定位置である。この初期位置からマッチングを開始することにより、収束までの時間を短くすることができる。ＩＭＵデータを用いる代わりに、例えば、過去２スキャン分の位置および向きの推定値の差分に基づいて、線形補間によってマッチングの初期値を決定してもよい。
　（ステップＳ１０６）
　自己位置推定モジュール１６２は、取得したスキャンデータをフィルタリングし、マッチングに用いる点の個数を削減する。さらに、前述の方法により、幹部を検出して、幹部以外の不要な部分の少なくとも一部を点群から除去する。
　ここで、図２６を参照して、スキャンデータのフィルタリングの動作の例を説明する。図２６は、スキャンデータに含まれる点群の一部を模式的に示している。図２６において、白丸が点群に含まれる点である。自己位置推定モジュール１６２は、センサ座標系における３次元空間を、一定の大きさの複数のボクセルに分割する。そして、個々のボクセルに複数の点が含まれる場合、それらの点を、重心に位置する１つの点に置き換える。図２６において、置き換えられた重心の点が黒丸で表現されている。このようなダウンサンプリングを行うことにより、スキャンデータの点の個数を削減して、マッチングを高速化することができる。本実施形態では、このようなダウンサンプリングが行われた後、幹部の検出が行われるが、ダウンサンプリングの前に幹部の検出を行ってもよい。点群のデータサイズが問題にならない場合には、ダウンサンプリング処理を省略してもよい。
　（ステップＳ１０７）
　自己位置推定モジュール１６２は、フィルタリングされたスキャンデータと、環境地図データとのマッチングを行い、ＬｉＤＡＲセンサ１１０のポーズを推定する。具体的には、ＮＤＴ法またはＩＣＰ法などの方法で、センサ座標系からワールド座標系への座標変換を決定することにより、ＬｉＤＡＲセンサ１１０のポーズを決定する。
　（ステップＳ１０８）
　自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０のポーズに基づき、トラクタ１００Ａのポーズを計算し、結果を出力する。トラクタ１００Ａのポーズは、ワールド座標系で表現されたトラクタ１００Ａの代表点（原点）の座標（ｘ，ｙ，ｚ）および姿勢（θＲ，θＰ，θＹ）のデータであり得る。なお、環境地図およびスキャンデータが２次元空間（平面）における点群を表し、その２次元空間においてマッチングが行われる場合には、出力されるポーズデータは、２次元座標（ｘ，ｙ）および向き（θ）の値を含み得る。位置のみをマッチングで推定し、姿勢についてはＩＭＵデータが示す姿勢の情報をそのまま利用してもよい。トラクタ１００Ａの座標系がセンサ座標系と一致している場合は、ステップＳ１０８は省略され得る。
　（ステップＳ１０９）
　自己位置推定モジュール１６２は、動作終了の指令が出されているか否かを判断する。動作終了の指令は、例えばユーザが操作端末２００を用いて自動走行モードの停止を指示したり、トラクタ１００Ａが目的地に到達したりした場合に出される。動作終了の指令が出されていない場合、ステップＳ１０３に戻り、次のスキャンデータについて同様の動作を実行する。動作終了の指令が出されている場合、処理を終了する。
　次に、駆動制御用のＥＣＵ１８０の動作の例を説明する。
　図２７は、トラクタ１００Ａのポーズが推定された後に実行されるＥＣＵ１８０の動作の例を示すフローチャートである。図２５に示すステップＳ１０８においてトラクタ１００Ａのポーズが出力されると、ＥＣＵ１８０は、そのポーズのデータを取得する（ステップＳ１２１）。次に、ＥＣＵ１８０は、当該ポーズデータが示すトラクタ１００Ａの位置と、ＥＣＵ１７０によって予め決定された走行予定経路との偏差を算出する（ステップＳ１２２）。走行予定経路は、例えばユーザが操作端末２００を操作することによって設定され得る。偏差は、その時点におけるトラクタ１００Ａの推定された位置と、走行予定経路との距離を表す。ＥＣＵ１８０は、算出した位置の偏差が予め設定された閾値を超えるか否かを判断する（ステップＳ１２３）。偏差が閾値を超える場合、ＥＣＵ１８０は、偏差が小さくなるように、駆動装置１４０の駆動値（例えば操舵角）を変更する。操舵角に加えて速度を変更してもよい。ステップＳ１２３において偏差が閾値を超えない場合、ステップＳ１２４は省略される。続くステップＳ１２５において、ＥＣＵ１８０は、動作終了の指令を受けたか否かを判断する。動作終了の指令は、前述のように、例えばユーザが操作端末２００を用いて自動走行モードの停止を指示したり、トラクタ１００Ａが目的地に到達したりした場合に出される。動作終了の指令が出されていない場合、ステップＳ１２１に戻り、次のスキャンデータに基づいて推定されたトラクタ１００Ａのポーズに基づいて、同様の動作を実行する。動作終了の指令が出されている場合、処理を終了する。
　図２７の例では、ＥＣＵ１８０は、推定された位置と走行予定経路との偏差のみに基づいて駆動装置１４０を制御するが、方位の偏差もさらに考慮して制御してもよい。例えば、ＥＣＵ１８０は、取得したポーズデータが示すトラクタ１００Ａの推定された向きと、走行予定経路の方向との角度差である方位偏差が、予め設定された閾値を超える場合に、その偏差に応じて駆動装置１４０の駆動値（例えば操舵角）を変更してもよい。
　以下、図２８Ａから図２８Ｄを参照しながら、ＥＣＵ１８０による操舵制御の例を説明する。
　図２８Ａは、走行予定経路Ｐに沿って走行するトラクタ１００Ａの例を示す図である。図２８Ｂは、走行予定経路Ｐから右にシフトした位置にあるトラクタ１００Ａの例を示す図である。図２８Ｃは、走行予定経路Ｐから左にシフトした位置にあるトラクタ１００Ａの例を示す図である。図２８Ｄは、走行予定経路Ｐに対して傾斜した方向を向いているトラクタ１００Ａの例を示す図である。
　図２８Ａに示すように、トラクタ１００Ａの位置および向きが走行予定経路Ｐから外れていない場合には、ＥＣＵ１８０は、トラクタ１００Ａの操舵角および速度を変更せずに維持する。
　図２８Ｂに示すように、トラクタ１００Ａの位置が走行予定経路Ｐから右側にシフトしている場合には、ＥＣＵ１８０は、トラクタ１００Ａの走行方向が左寄りになるように、駆動装置１４０に含まれるステアリングモータの回転角を変更して操舵角を変更する。このとき、操舵角に加えて速度も併せて変更してもよい。操舵角の変化量は、例えば位置偏差Δｘの大きさに応じて調整され得る。
　図２８Ｃに示すように、トラクタ１００Ａの位置が走行予定経路Ｐから左側にシフトしている場合には、ＥＣＵ１８０は、トラクタ１００Ａの走行方向が右寄りになるように、ステアリングモータの回転角を変更して操舵角を変更する。この場合も、操舵角に加えて速度も併せて変更してもよい。操舵角の変化量は、例えば位置偏差Δｘの大きさに応じて調整され得る。
　図２８Ｄに示すように、トラクタ１００Ａの位置は走行予定経路Ｐから大きく外れていないが、向きが走行予定経路Ｐの方向とは異なる場合は、ＥＣＵ１８０は、方位偏差Δθが小さくなるように操舵角を変更する。この場合も、操舵角に加えて速度も併せて変更してもよい。操舵角の変化量は、例えば位置偏差Δｘおよび方位偏差Δθの大きさに応じて調整され得る。例えば、位置偏差Δｘの絶対値が小さいほど方位偏差Δθに応じた操舵角の変化量を大きくしてもよい。位置偏差Δｘの絶対値が大きい場合には、走行予定経路Ｐに戻るために操舵角を大きく変化させることになるため、必然的に方位偏差Δθの絶対値が大きくなる。逆に、位置偏差Δｘの絶対値が小さい場合には、方位偏差Δθをゼロに近づけることが必要であるため、操舵角の変化量を決定する際の方位偏差Δθの重みを相対的に大きくすることが妥当である。
　トラクタ１００Ａの操舵制御および速度制御には、ＰＩＤ制御またはＭＰＣ制御（モデル予測制御）などの制御技術が適用され得る。これらの制御技術を適用することにより、トラクタ１００Ａを走行予定経路Ｐに近付ける制御を滑らかにすることができる。
　なお、走行中に１つ以上の障害物センサ１３０によって障害物が検出された場合には、ＥＣＵ１８０は、障害物を回避するように駆動装置１４０を制御する。障害物を回避できない場合には、ＥＣＵ１８０は、トラクタ１００Ａを停止させる。なお、ＥＣＵ１８０は、障害物が検出された場合に、障害物を回避できるか否かに関わらず、トラクタ１００Ａを停止させてもよい。
　＜走行予定経路の決定方法の例＞
　続いて、ＥＣＵ１７０によるトラクタ１００Ａの走行予定経路の決定方法の例を説明する。
　環境地図が作成された後、ＥＣＵ１７０は、トラクタ１００Ａの走行予定経路を決定する。走行予定経路は、ＥＣＵ１７０が環境地図に基づいて自動で決定してもよいし、ユーザが操作端末２００を操作して設定してもよい。
　図２９Ａは、環境地図に基づいて生成される、樹木列の幹分布を示す２次元データの例を示す図である。ＥＣＵ１６０における地図データ生成モジュール１６４は、マッチング用の環境地図とは別に、図２９Ａに示すような、幹部２２の分布を大まかに示す２次元データを生成してもよい。この２次元データは、個々の樹木の幹部２２が一定の大きさの円で表現されている。このような幹分布を示す２次元データを「幹分布データ」と称することがある。幹分布データは、例えば個々の幹の中心の位置座標（ｘ，ｙ）の情報を含む。幹分布データは、位置座標に加えて、個々の幹の識別番号（幹ＩＤ）および幹の太さなどの情報を含んでいてもよい。そのような情報を含めることにより、幹の生育状況を管理することもできる。樹木の幹または葉などの生育状況は、例えば営農管理システム内のコンピュータによって管理され得る。この例のように、樹木の生育状況は、環境地図とは異なるデータで管理され得る。ユーザが操作端末２００などのヒューマンマシンインタフェース（ＨＭＩ）を用いて、樹間距離または幹部の位置座標などの、複数の樹木列の配置関係を示す情報を入力できるようにしてもよい。入力された情報は、記憶装置１５０または他の記憶媒体に記録される。
　図２９Ｂは、幹分布データに基づく走行予定経路３０の決定方法の一例を説明するための図である。この例におけるＥＣＵ１７０は、各樹木列の幹部２２を繋いだ曲線を決定し、隣り合う２つの樹木列の中央部を通る曲線または折れ線を走行予定経路３０として決定する。隣り合う２つの樹木の中央部をトラクタ１００Ａの中心が必ず位置する必要はなく、走行予定経路３０は、隣り合う２つの樹木間の中央部からシフトしていてもよい。駆動制御用のＥＣＵ１８０は、決定された走行予定経路３０を基準に、トラクタ１００Ａの操舵制御を行う。
　図３０は、経路設定用の幹分布データの他の例を示す図である。この例では、トラクタ１００Ａの走行環境が複数のセルに分割されて管理される。各セルは、例えば幹部２２の直径の平均値に近い辺の長さをもつ正方形であり得る。セルは、以下の（１）～（４）の４種類に分類される。
（１）幹部２２が含まれるセル
（２）幹部２２が含まれるセルに隣接するセル
（３）隣り合う２つの樹木列の間に位置し、（１）および（２）に該当しないセル
（４）それ以外のセル
　図３０において、（１）～（４）のセルが、異なる濃さで表現されている。幹部２２が含まれるセルに隣接するセル（２）の範囲は、ＬｉＤＡＲセンサ１１０によって取得された樹木列の葉部を示す点群データに基づいて決定されてもよい。その場合、ＥＣＵ１６０の地図データ生成モジュール１６４は、マッチング用の環境地図データとは別に、樹木列の幹部および葉部のそれぞれの２次元分布を示す格子状データを生成する。この例におけるＥＣＵ１７０は、図３０に示すような格子状データに基づいてトラクタ１００Ａの経路を決定する。例えば、ＥＣＵ１７０は、隣り合う２つの樹木列の間に位置する上記（３）のセルを走行可能領域として設定する。ＥＣＵ１８０は、走行可能領域の中央部付近を通るようにトラクタ１００Ａの走行を制御する。
　＜地図データ生成時の操舵制御＞
　前述のように、本実施形態におけるトラクタ１００Ａは、自動走行によって環境地図を作成するとき、自己位置推定と操舵制御とを行いながら、樹木列の幹部の分布を示す局所地図データを収集する。このとき、ＥＣＵ１６０は、トラクタ１００Ａが移動している間にＬｉＤＡＲセンサ１１０から繰り返し出力されるセンサデータに基づいて、トラクタ１００Ａの周囲の環境における樹木列の葉部をさらに検出し、検出した樹木列の葉部の分布を示すデータをさらに生成してもよい。その場合、ＥＣＵ１８０は、検出された樹木列の幹部および葉部のそれぞれの分布に基づいて、トラクタ１００Ａの操舵制御を行うことができる。ＥＣＵ１８０は、例えば、検出された樹木列のうちの隣り合う２つの樹木列の間を通り、かつ樹木列の葉部との接触が低減される経路に沿ってトラクタ１００Ａを移動させるようにトラクタ１００Ａの操舵制御を行う。
　この動作において、ＥＣＵ１７０は、ＬｉＤＡＲセンサ１１０からのセンサデータに基づいて検出された樹木列の幹部の分布から、隣り合う２つの樹木列の幹部の間を通る経路を決定する。例えば、隣り合う２つの樹木列の幹部の対の位置の中央部を通る経路を決定する。ＥＣＵ１８０は、決定された経路に沿ってトラクタ１００Ａを移動させるように、トラクタ１００Ａの操舵制御を行う。これにより、局所地図データを収集しながら、自動走行を行うことができる。
　トラクタ１００Ａが走行する環境において、樹木列は必ずしも直線的に配置されているとは限らない。樹木列が曲線的に配置される場合もある。そのような場合、地図データを収集しながら自動走行するときに、左右の片側にしか樹木列が検出されない場合がある。そのような場合には、左右の樹木列の対を特定できないため、上記の方法では自動走行を継続することが難しい。
　そこで、樹木列が曲線的に配置されている場合には、以下の制御が行われ得る。樹木列が曲線的に配置される場合、隣り合う２つの樹木列の一方について、２つの樹木列の他方よりも少ない数の幹部が検出される。そのような場合には、ＥＣＵ１７０は、検出された幹部の分布に基づいて、当該２つの樹木列の一方における隠れた幹部の分布を推定し、推定した分布に基づいて、トラクタ１００Ａの経路を決定する。ＥＣＵ１８０は、決定された経路に沿ってトラクタ１００Ａが走行するようにトラクタ１００Ａの操舵制御を行う。
　図３１Ａは、樹木列が曲線的に配置されている環境の一例を模式的に示す図である。図中の円柱は樹木の幹部を表し、太い破線は樹木列の概略的な配列を表し、点線はＬｉＤＡＲセンサ１１０から出射される複数のレイヤのレーザビームによるスキャンを模式的に表している。図３１Ｂは、図３１Ａに示す環境において１回のスキャンで観測され得る点群を模式的に示す図である。この例においては、隣り合う２つの樹木列のうち、右側の樹木列については３本の幹部２２Ｒ１、２２Ｒ２、２２Ｒ３が検出され、左側の樹木列については１本の幹部２２Ｌ１のみが検出されている。左側の樹木列については、前方の樹木の幹部２２Ｌ１によってレーザビームが遮られ、後方の樹木の幹部を検出することができない。
　このような場合、ＥＣＵ１７０は、相対的に多くの樹木が検出される右側の樹木列の配列から左側の隠れた樹木の幹部の位置を推定し、推定した幹部の位置に基づいて走行経路を決定する。例えば、スキャンデータに基づいて位置が特定される前方の隣り合う２つの幹部２２Ｒ１および２２Ｌ１の相対的な位置関係を、後方の隣り合う２つの幹部にも適用することにより、隠れた幹部の位置を推定する。
　図３１Ｃは、隠れた幹部の位置を推定することによって走行経路を決定する動作を説明するための図である。この例では、スキャンデータに基づいて取得された樹木列の幹部の２次元分布を示す幹分布データに基づいて、隠れた幹部の位置が推定される。図３１Ｃには、スキャンデータに基づいて検出された樹木の幹部の位置が実線の円で示され、まだ検出されていない樹木の幹部の位置が破線の円で示されている。この例において、ＥＣＵ１７０は、まず幹部２２Ｒ１の位置から幹部２２Ｌ１の位置までのベクトルＶ１を決定する。次に、後方の幹部２２Ｒ２の位置を起点として、その位置からベクトルＶ１の分だけ変位した位置を、隠れた幹部の推定位置として決定する。図３１Ｃにおいて、推定された隠れた幹部の位置が×印で示されている。ＥＣＵ１７０は、例えば、当該位置と幹部２２Ｒ２の位置との中点Ｃ１を通る経路を、トラクタ１００Ａの走行経路として決定する。
　このような方法で決定された経路に沿ってトラクタ１００Ａが走行すると、やがて幹部２２Ｌ１によって隠れていた後方の幹部２２Ｌ２がスキャンデータから検出される。さらに後方の幹部２２Ｌ３はまだ検出されていないとする。幹部２２Ｌ２の位置が推定位置（×印）とは異なる場合、ＥＣＵ１７０は、幹部２２Ｌ２と対を成す右側の幹部（図３１Ｃの例では幹部２２Ｒ３）を特定し、その幹部２２Ｒ３から幹部２２Ｌ２までのベクトルＶ２を決定する。次に、さらに後方の幹部２２Ｒ４の位置を起点として、その位置からベクトルＶ２の分だけ変位した位置を、隠れた幹部２２Ｌ３の位置として決定する。以上のような動作を繰り返すことにより、隠れた幹部の位置を推定して適切な経路を設定し、その経路に沿って自動操舵を行うことができる。
　図３１Ｄは、樹木列が曲線的に配置されている場合における幹部の２次元分布を示す格子状データの一例を示す図である。ＥＣＵ１６０は、環境地図を構築するためのデータを収集する過程で、図３１Ｄに示すような格子状データを生成してもよい。その場合、ＥＣＵ１７０は、このような格子状データに基づいて経路設定を行ってもよい。この例において、隠れた幹の位置は、セルの大きさの単位で推定され得る。
　以上のような動作により、環境地図を作成しながら、比較的なめらかな操舵制御を行うことができる。
　＜障害物回避動作＞
　次に、本実施形態における障害物回避動作を説明する。
　図３２は、トラクタ１００Ａの走行環境に障害物４０および人５０が存在する状況の例を模式的に示す図である。トラクタ１００Ａは、１つ以上の障害物センサ１３０を備えている。走行中、障害物センサ１３０によって障害物４０（例えばカゴ）、または人５０を検出した場合、ＥＣＵ１８０は、障害物４０または人５０を回避するように操舵制御を行う。回避しても障害物４０、人５０、枝部、または幹部２２等との衝突を避けられない場合には、ＥＣＵ１８０は、トラクタ１００Ａを停止させる。なお、ＥＣＵ１８０は、障害物が検出された場合に、障害物を回避できる場合であってもトラクタ１００Ａを停止させてもよい。障害物センサ１３０によらず、ＬｉＤＡＲセンサ１１０によって同様の検出を行ってもよい。
　図３２に示す例では、人５０が樹木の幹部２２の近くに存在する。樹木の葉部も含めてマッチングが行われる場合には、葉部に人５０またはその他の障害物が隠れてしまい、葉部か他の物体かの判別が難しいことがある。特に、人５０が幹部２２の近くの葉の中に立っている場合、判別が難しい。本実施形態では、幹部２２の分布を示す環境地図を用いてマッチングが行われるので、幹部と他の物体との判別を容易にすることができる。スキャンデータと環境地図とのマッチングによって検出された幹部以外の物体を、人または障害物として容易に検出することができる。このように、ＬｉＤＡＲセンサ１１０を障害物センサとして利用することもできる。トラクタ１００Ａが複数のＬｉＤＡＲセンサ１１０を、トラクタ１００Ａの異なる箇所に備えている場合、障害物センサ１３０を省略してもよい。
　＜固有の幹間隔の組み合わせに基づく自己位置推定＞
　以上の実施形態では、樹木の幹部の間隔がおおよそ一定であるものとしているが、現実には、樹木の幹部の間隔は場所によって異なり得る。そこで、ＥＣＵ１６０は、自己位置推定を行うとき、センサデータから、固有の幹間隔の組み合わせを有する複数の樹木の幹部を検出し、当該固有の幹間隔の組み合わせを有する複数の樹木の幹部と、環境地図データから抽出した当該固有の幹間隔の組み合わせを有する複数の樹木の幹部とのマッチングにより、移動体の位置を推定してもよい。以下、図３３Ａおよび図３３Ｂを参照して、この動作を説明する。
　図３３Ａは、トラクタ１００Ａによって検出される樹木列の幹部２２の配置の例を示す図である。図３３Ｂは、環境地図データが示す幹分布の例を示す図である。この例において、点線枠で囲まれた３つの幹部は、１．２ｍと１．５ｍという固有の幹間隔の組み合わせを有する。このような場合、ＥＣＵ１６０は、自己位置推定を行うとき、取得したスキャンデータから検出された幹部２２のそれぞれの間隔を計算し、固有の幹間隔の組み合わせを有する部分を決定する。そして、図３３Ｂに示す環境地図から、決定した幹間隔の組み合わせを有する部分を検索して特定する。ＥＣＵ１６０は、その固有の幹間隔を有する部分同士のマッチングにより、トラクタ１００Ａの現在の位置および向きを推定することができる。このような動作により、樹木列が固有の幹間隔の組み合わせを有する部分を含む場合に、マッチングの高速化および位置推定の精度の向上を図ることができる。樹木列における幹間隔の組み合わせを示す情報が予め記憶媒体に記録されていてもよい。そのような情報は、例えばＥＣＵ１６０が環境地図データに基づいて自動で記録してもよいし、ユーザが操作端末２００を操作することによって記録してもよい。
　本実施形態において、トラクタ１００Ａが生成した環境地図データは、同一の環境内を走行する他のトラクタまたはトラクタ以外の移動体に提供することもできる。例えば広い果樹園において、複数の移動体によって作業を行う場合、環境地図を一台の移動体またはコンピュータが作成すれば十分であり、環境地図を複数の移動体で共有することが効率的である。１つの環境内の環境地図を構築するための局所地図を、複数のトラクタまたはその他の移動体によって分担して生成してもよい。その場合、複数の移動体によって生成された局所地図をまとめて１つの環境地図を生成するコンピュータがシステム内に設けられる。当該コンピュータは、生成した環境地図を、複数の移動体に、有線もしくは無線のネットワーク、または記憶媒体を介して配信してもよい。
　同一の環境内を複数の移動体が移動しながら作業するシステムにおいては、移動体ごとにセンサ座標系と移動体座標系との位置関係が異なり得る。同一の機種であっても、センサの取り付け位置の誤差に起因して、センサ座標系から移動体座標系への変換に誤差が生じ得る。そのような誤差の影響を低減するために、各移動体は、通常の運行を開始する前に、試運転によってセンサ座標系から移動体座標系への座標変換のためのパラメータを決定するキャリブレーションを実行する。
　本実施形態の技術は、ぶどう園などの果樹園を走行するトラクタに限らず、森林などの、複数の樹木列が存在する環境で使用される任意の移動体（例えば、移動ロボット、ドローン等）に適用することができる。この点については、以降の実施形態でも同様である。
　［３－２．実施形態２］
　次に、本開示の第２の実施形態による移動体を説明する。
　本実施形態における移動体は、少なくとも２つのＬｉＤＡＲセンサを備え、それらのＬｉＤＡＲセンサから出力されるセンサデータに基づいて、自己位置推定および地図データの生成を行う。少なくとも２つのＬｉＤＡＲセンサの各々は、移動体の周囲の環境における物体の分布を示す２次元または３次元の点群データ、または距離分布データを出力する。移動体は、実施形態１と同様、複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、自己位置推定装置と、自己位置推定装置によって推定された移動体の位置に応じて移動体の移動を制御する制御装置とを備える。自己位置推定装置は、移動体が移動している間に少なくとも２つのセンサから繰り返し出力される点群データに基づいて、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部と環境地図データとのマッチングを行うことにより、移動体の位置を推定する。移動体はさらに、環境地図データ、または環境地図データを構築するための局所地図データを生成するデータ生成装置も備える。データ生成装置は、移動体が移動している間に少なくとも２つのセンサから繰り返し出力される点群データに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録する。データ生成装置は、移動体が移動している間に繰り返し生成した局所地図データを統合することにより、ワールド座標系における環境地図データを生成して記憶装置に記録することもできる。
　以下、移動体がぶどう園などの果樹園内を走行するトラクタである場合を例に、本実施形態の構成および動作を説明する。以下の説明において、実施形態１とは異なる点を中心に説明し、重複する事項についての説明は省略する。
　図３４は、本実施形態におけるトラクタ１００Ｂを側面方向から見た模式図である。このトラクタ１００Ｂは、車体前方下部に第１のＬｉＤＡＲセンサ１１０Ａを備え、キャビンの上部前方に第２のＬｉＤＡＲセンサ１１０Ｂを備えている。それ以外の点は、図７に示す構成と同様である。図３４には、図７に示されている連結装置１０８、および作業機３００が示されていないが、本実施形態においてもこれらは使用され得る。
　本実施形態における第１のＬｉＤＡＲセンサ１１０Ａおよび第２のＬｉＤＡＲセンサ１１０Ｂの各々は、２次元ＬｉＤＡＲセンサである。言い換えれば、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの各々は、図９Ａに示す例において、Ｎ＝１の場合、すなわち１つの仰角の方向にのみレーザビームを出射するＬｉＤＡＲセンサ１１０と同等である。ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの各々は、各反射点までの距離と方向とを示すデータ、または、２次元の点群データを出力する。２次元の点群データは、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂのそれぞれのセンサ座標系で表現された複数の反射点の２次元座標の情報を含む。２つの２次元ＬｉＤＡＲを用いることにより、３次元ＬｉＤＡＲを用いる場合よりもコストを低減させることができる。
　第１のＬｉＤＡＲセンサ１１０Ａは、トラクタ１００Ｂが走行する環境における樹木列の幹部の平均高さよりも低い位置に搭載されている。第２のＬｉＤＡＲセンサ１１０Ｂは、当該樹木列の幹部の平均高さよりも高い位置に搭載されている。第１のＬｉＤＡＲセンサ１１０Ａは、例えば地上から１０ｃｍ以上１５０ｃｍ以下、ある例では１５ｃｍ以上１００ｃｍ以下の高さに配置され得る。第２のＬｉＤＡＲセンサ１１０Ｂは、例えば地上から１５０ｃｍよりも高い位置（例えば地上から約２ｍ）に配置され得る。
　第１のＬｉＤＡＲセンサ１１０Ａは、実施形態１におけるＬｉＤＡＲセンサ１１０と同様、トラクタ１００Ｂの前方に向けてレーザパルスを出射するように配置される。図３４の例における第１のＬｉＤＡＲセンサ１１０Ａは、地面にほぼ平行にレーザパルスを出射する。トラクタ１００Ｂが平坦な地面に位置しているとき、第１のＬｉＤＡＲセンサ１１０Ａから出射されるレーザパルスの出射方向と地面とのなす角度は、例えば±２０度の範囲内、ある例では±１０度の範囲内に設定され得る。
　一方、第２のＬｉＤＡＲセンサ１１０Ｂは、トラクタ１００Ｂの前方斜め下に向けてレーザパルスを出射するように配置される。図３４の例における第２のＬｉＤＡＲセンサ１１０Ｂは、地面から約２５度傾いた方向にレーザパルスを出射する。トラクタ１００Ｂが平坦な地面に位置しているとき、第２のＬｉＤＡＲセンサ１１０Ｂから出射されるレーザパルスの出射方向と地面とのなす角度は、例えば１０度以上４５度以下の範囲に設定され得る。
　図３５は、本実施形態におけるトラクタ１００Ｂの概略的な構成の例を示すブロック図である。図３５に示す構成例では、トラクタ１００Ｂが２つのＬｉＤＡＲセンサ１１０Ａ、１１０Ｂを備えている点で、図８に示す構成とは異なる。
　ＥＣＵ１６０は、上記の自己位置推定装置およびデータ生成装置として機能する。ＥＣＵ１６０は、２つのＬｉＤＡＲセンサ１１０Ａ、１１０Ｂから繰り返し出力される点群データから、最新スキャンを含む複数スキャンの入力点群を取得し、複数スキャンの入力点群と環境地図データとのマッチングを行う。これにより、走行中のトラクタ１００Ｂの位置および向きを推定することができる。なお、ＥＣＵ１６０は、マッチングによってトラクタ１００Ｂの位置のみを推定し、向きについてはＩＭＵ１２５から出力された信号に基づいて決定してもよい。
　環境地図を構築する過程において、ＥＣＵ１６０は、トラクタ１００Ｂが移動している間に２つのＬｉＤＡＲセンサ１１０Ａ、１１０Ｂから繰り返し出力されるスキャンデータに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置１５０に記録する。この動作が行われている間、ＥＣＵ１７０は、スキャンデータに基づいて検出された樹木列の幹部の間を通る目標経路を設定する。ＥＣＵ１８０は、設定された目標経路に従って駆動装置１４０を制御することにより、トラクタ１００Ｂを目標経路に沿って走行させる。ＥＣＵ１７０は、障害物センサ１３０から出力される信号、またはＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの一方または両方から出力されるスキャンデータに基づいて障害物を検出し、障害物との接触を回避するように目標経路を設定してもよい。ＥＣＵ１６０は、繰り返し生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成して記憶装置１５０に記録する。なお、樹木列の幹部の検出は、局所地図データを生成する段階ではなく、環境地図データを生成する段階で行ってもよい。また、局所地図データに基づいて環境地図データを生成する処理は、外部のコンピュータが実行してもよい。
　環境地図が構築された後の自動走行または自動操舵の動作中、ＥＣＵ１６０は、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂから繰り返し出力されるスキャンデータに基づいて樹木列の幹部を検出し、検出した幹部と環境地図データが示す幹部とのマッチングを行うことにより、自己位置推定を行う。ＥＣＵ１８０は、ＥＣＵ１７０によって予め決定された走行予定経路に沿ってトラクタ１００Ｂを走行させる。この場合も、ＥＣＵ１７０は、トラクタ１００Ｂの走行中に障害物センサ１３０から出力される信号、またはＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの一方または両方から出力されるスキャンデータに基づいて障害物を検出し、障害物との接触を回避するように走行予定経路を変更してもよい。
　図３６Ａから図３６Ｃは、トラクタ１００Ｂが樹木２１の近くを走行する様子を模式的に示す図である。図３６Ａ、図３６Ｂ、図３６Ｃの順に、トラクタ１００Ｂが樹木２１に近付いている。これらの図において、破線は、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂから出射されるレーザビームを表している。図示されるように、トラクタ１００Ｂと樹木２１との距離に応じて、第２のＬｉＤＡＲセンサ１１０Ｂから出射されるレーザビームによって照射される樹木２１の幹部２２の照射点の位置が、高さ方向に変化する。一方、第１のＬｉＤＡＲセンサ１１０Ａから出射されるレーザビームによって照射される樹木２１の幹部２２の照射点の位置の変化は相対的に小さい。これらのレーザビームの複数回のスキャンによって取得される点群データを組み合わせることにより、３次元ＬｉＤＡＲを用いた場合と同様に、幹部２２の表面における反射点の３次元分布を示す点群データを取得することができる。
　本実施形態におけるＥＣＵ１６０は、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂのそれぞれから出力された所定のスキャン回数分（例えば３回分または５回分等）のスキャンデータを蓄積し、蓄積されたスキャンデータと、環境地図データとのマッチングを行うことにより、自己位置推定を行ってもよい。このようにすることで、実質的に３次元ＬｉＤＡＲを用いた場合と同様の高い精度の自己位置推定が可能になる。
　地図データを取得するためにトラクタ１００Ｂが走行している間、ＥＣＵ１７０は、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの一方または両方から出力された数スキャン分のスキャンデータに基づいて、トラクタ１００Ｂの局所的な目標経路を決定する。例えば、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの一方または両方から出力された数スキャン分のスキャンデータが示す点群の配置から、隣り合う２つの樹木間の中心位置を決定し、その中心位置を通るようにトラクタ１００Ｂの局所的な目標経路を決定することができる。このような経路設定により、トラクタ１００Ｂは、左右の樹木列との距離が概ね等しい状態を保ちながら自律的に走行することができる。
　幹部の検出は、第１のＬｉＤＡＲセンサ１１０Ａから出力されるスキャンデータのみに基づいて行われてもよい。相対的に低い位置にある第１のＬｉＤＡＲセンサ１１０Ａから出力されるスキャンデータは、幹部の表面を表す点群の情報を多く含むことが多い。このため、第１のＬｉＤＡＲセンサ１１０Ａからのスキャンデータのみを用いた場合でも幹部の分布情報を取得することができる。
　本実施形態における第２のＬｉＤＡＲセンサ１１０Ｂは、相対的に高い位置に配置されているため、樹木列の葉部の検出に適している。そこで、ＥＣＵ１６０は、第２のＬｉＤＡＲセンサ１１０Ｂによって取得された点群データに基づいて、樹木列の葉部の分布を示すデータを生成し、記憶装置１５０または他の記憶媒体に記録してもよい。そのようなデータは、樹木の生育状況の管理（例えば、キャノピーマネジメント）のために使用され得る。
　トラクタ１００Ｂの走行中に、第２のＬｉＤＡＲセンサ１１０Ｂによって取得された点群データに基づいて、樹木列の葉部などの、比較的高い位置に存在する障害物を検出し、回避または停止する動作を行ってもよい。トラクタ１００Ｂの前方上部に障害物センサ１３０が設けられていない場合、第２のＬｉＤＡＲセンサ１１０Ｂが設けられていなければ、樹木列間の経路の上方にはみだした樹木の葉または蔓を検出して避けることが難しい。第２のＬｉＤＡＲセンサ１１０Ｂを設けることにより、それらの葉または蔓を検出することが容易になる。
　本実施形態によれば、２つの２次元ＬｉＤＡＲを用いることにより、低いコストで実施形態１と同様の動作を実現することができる。なお、２つに限らず、３つ以上の２次元ＬｉＤＡＲを異なる位置に設けてもよい。また、１つ以上の２次元ＬｉＤＡＲと１つ以上の３次元ＬｉＤＡＲとを組み合わせて使用してもよい。このように、本実施形態の構成には多様な変形が可能である。なお、実施形態１について説明した種々の技術は、本実施形態においてもそのまま適用できる。
　［３－３．実施形態３］
　次に、本開示の第３の実施形態を説明する。
　本実施形態は、１台または複数台の移動体（例えばドローン）を用いて、環境地図データを構築するための局所地図データを収集し、収集された局所地図データに基づいて環境地図データを生成するシステムに関する。
　図３７は、本実施形態のシステムの構成例を模式的に示す図である。本システムは、１台以上のドローン４００と、１台以上のトラクタ１００Ｃと、サーバコンピュータ５００（以下、「サーバ５００」と称する。）とを備える。これらの構成要素は、有線または無線のネットワーク６０を介して、互いに通信可能に接続されている。図３７には、２台のドローン４００および３台のトラクタ１００Ｃが示されているが、ドローン４００およびトラクタ１００Ｃのそれぞれの台数は任意である。各ドローン４００は、各トラクタ１００Ｃが走行する環境内を飛行しながら環境地図データを構築するための局所地図データを生成し、サーバ５００に送信する。サーバ５００は、１台以上のドローン４００から受信した局所地図データに基づいて、統一されたワールド座標系で表現された環境地図データを生成し、各トラクタ１００Ｃに配信する。各トラクタ１００Ｃは、配信された環境地図データに基づいて果樹園内を自律的に走行する。
　図３８は、本システムの構成例を示すブロック図である。この例におけるトラクタ１００Ｃは、図８に示す実施形態１のトラクタ１００Ａと同様の構成要素を備える。ただし、本実施形態におけるＥＣＵ１６０は、地図データ生成モジュール１６４を備えていない。通信ＩＦ１９０は、ネットワーク６０を介してサーバ５００と通信を行う。記憶装置１５０は、サーバ５００から配信された環境地図データを記憶する。なお、図３８では、トラクタ１００Ｃに連結されて使用される作業機の図示は省略されている。通信ＩＦ１９０は、実施形態１と同様、トラクタ１００Ｃに接続された作業機との通信を行ってもよい。
　ドローン４００は、データ生成ユニット４５０と、駆動装置４４０と、制御装置４８０と、通信ＩＦ４９０とを備える。駆動装置４４０は、例えば駆動用の電気モータ、および複数のプロペラなどの、ドローン４００の飛行に必要な種々の装置を含む。制御装置４８０は、データ生成ユニット４５０および駆動装置４４０の動作を制御する。通信ＩＦ４９０は、ネットワーク６０を介してサーバ５００と通信するための回路である。データ生成ユニット４５０は、ＬｉＤＡＲセンサ４１０と、データ生成装置４６０とを備える。ＬｉＤＡＲセンサ４１０は、実施形態１におけるＬｉＤＡＲセンサ１１０と同様の機能を備える。データ生成装置４６０は、実施形態１におけるＥＣＵ１６０と同様の機能を備える。すなわち、データ生成装置４６０は、自己位置推定と地図生成とを同時に行う機能を備える。ただし、データ生成装置４６０は、最終的な環境地図データの生成を行わない。データ生成装置４６０は、プロセッサとメモリ等の記憶媒体とを備える。データ生成装置４６０は、ドローン４００の飛行中にＬｉＤＡＲセンサ４１０から繰り返し出力されたスキャンデータに基づいて、複数の樹木列の幹部の分布を示す局所地図データを繰り返し生成して記憶媒体に蓄積する。蓄積された局所地図データは、例えばユーザによる操作により、通信ＩＦ４９０からサーバ５００に送信される。
　サーバ５００は、例えばクラウドサーバまたはエッジサーバなどの、トラクタ１００Ｃおよびドローン４００から離れた場所に設置されたコンピュータであり得る。サーバ５００は、記憶装置５５０と、処理装置５６０と、通信ＩＦ５９０とを備える。通信ＩＦ５９０は、ネットワーク６０を介してトラクタ１００Ｃおよびドローン４００と通信するための回路である。処理装置５６０は、複数のドローン４００から取得した局所地図データを、座標系を統一して統合することにより、環境地図データを生成し、記憶装置５５０に記録する。処理装置５６０は、生成した環境地図データを、通信ＩＦ５９０から複数のトラクタ１００Ｃに配信する。処理装置５６０は、環境地図データをトラクタ１００Ｃ以外の移動体に配信してもよい。例えば、ドローン４００が、地図データの収集だけでなく、播種、施肥、または防除などの作業も実行する場合には、ドローン４００に環境地図データが配信されてもよい。その場合、ドローン４００は、ＬｉＤＡＲセンサ４１０から出力されたスキャンデータと、環境地図データとのマッチングによって自己位置推定を行いながら自律的に飛行し、所定の作業を実行することができる。
　図３９は、ドローン４００の外観の例を示す斜視図である。このドローン４００は、本体の前面にデータ生成ユニット４５０を備える。データ生成ユニット４５０は、ＬｉＤＡＲセンサ４１０を備えており、飛行しながらレーザビームで周囲の環境をスキャンすることができる。
　図４０は、複数のドローン４００によるデータ収集作業の様子の例を模式的に示す斜視図である。図４０には、複数の樹木列２０の間を飛行する２台のドローン４００が例示されている。これらのドローン４００は、樹木列２０の間を低空飛行で自律的に移動しながら、樹木列２０の幹部をレーザビームでスキャンすることにより、幹部の分布を示す局所地図データを生成する動作を繰り返す。局所地図データの生成方法は、実施形態１における方法と同様である。
　図４１は、ドローン４００に搭載されたＬｉＤＡＲセンサ４１０から出射されたレーザビームによって樹木２１が照射されている様子を模式的に示す図である。本実施形態におけるドローン４００は、ＬｉＤＡＲセンサ４１０が、樹木列の幹部の平均高さよりも低い位置を保つ高度で飛行するようにプログラミングされている。ＬｉＤＡＲセンサ４１０が、例えば地上から１５ｃｍ以上１００ｃｍ以下の高さに位置するように、ドローン４００の高度が制御される。データ生成装置４６０は、このような低い高度に位置するＬｉＤＡＲセンサ４１０から繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、ドローン４００の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す局所地図データを生成して記憶媒体に記録する。データ生成装置４６０は、ＬｉＤＡＲセンサ４１０から繰り返し出力されるセンサデータから複数スキャンの入力点群を取得し、最新スキャンを含む少なくとも１つのスキャンの入力点群と、前回以前に生成された局所地図データとのマッチングを行うことにより、自己位置推定を行う。
　図４２は、各ドローン４００から出力される局所地図データの形式の一例を示す図である。この例における局所地図データは、ドローン４００の識別番号である移動体ＩＤと、ワールド座標系におけるドローン４００の位置（ｘ，ｙ，ｚ）および姿勢（θＲ，θＰ，θＹ）と、反射点ごとに記録される情報とを含む。反射点ごとに記録される情報は、図２３に示す情報と同様であるが、各反射点の座標は、ドローン４００のＬｉＤＡＲセンサ４１０に固定されたセンサ座標系における座標（ｕ，ｖ，ｗ）である。このような局所地図データが、各ドローン４００からサーバ５００に送信され得る。なお、分類および幹ＩＤなどの、その点が幹であるか否かに関する情報は、局所地図データに含まれていなくてもよい。サーバ５００における処理装置５６０が、複数のドローン４００から局所地図データを取得した後、点群の分布の特徴に基づいて幹部に該当する点群を検出してもよい。
　以上の構成により、例えば広い果樹園においても、環境地図を構築するためのデータを効率的に収集することができる。
　なお、本実施形態では、ドローン４００だけでなく、トラクタ１００Ｃなどの他の移動体も、環境地図データを構築するための局所地図データを取得する動作を行ってもよい。また、局所地図データを生成する移動体は、必ずしも自律的に移動可能な移動体でなくてもよい。例えば、樹木列の幹部の平均高さよりも低い位置に搭載された１つ以上のＬｉＤＡＲセンサを備えたトラクタまたはドローンなどの移動体を、ユーザが運転または操縦することによって局所地図データを生成してもよい。
　サーバ５００を設けることなく、ドローン４００またはトラクタ１００Ｃなどの移動体が最終的な環境地図データを生成し、他の移動体に環境地図データを提供してもよい。その場合には、移動体間の通信によって環境地図データが直接的に送受信され得る。
　［３－４．他の実施形態］
　以上の実施形態において、各トラクタは、無人トラクタであってもよい。その場合には、キャビン、運転席、ステアリングホイール、操作端末などの、有人運転にのみ必要な構成要素は、トラクタに設けられていなくてもよい。無人トラクタは、自律走行、またはトラクタのユーザによる遠隔操作によって、前述の各実施形態における動作と同様の動作を実行してもよい。
　以上の実施形態において、移動体が備える１つ以上のセンサは、レーザビームのスキャンによって２次元または３次元の点群データまたは距離分布データをセンサデータとして出力するＬｉＤＡＲセンサである。しかし、センサはそのようなＬｉＤＡＲセンサに限定されない。例えば、フラッシュ型のＬｉＤＡＲセンサ、またはイメージセンサなどの他の種類のセンサを利用してもよい。そのような他の種類のセンサを、スキャン型のＬｉＤＡＲセンサと組み合せて利用してもよい。
　以上の実施形態における自己位置推定および自律移動（もしくは自動操舵）、または地図データ生成に必要な処理を実行する装置を、それらの機能を有しない移動体に後から取り付けることもできる。例えば、複数の樹木列の間を移動する移動体の動作を制御する制御ユニットを、当該移動体に取り付けて使用することができる。そのような制御ユニットは、移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、自己位置推定装置と、自己位置推定装置によって推定された移動体の位置に応じて移動体の移動を制御する制御装置とを備える。自己位置推定装置は、移動体が移動している間に１つ以上のセンサから繰り返し出力されるセンサデータに基づいて、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部と環境地図データとのマッチングを行うことにより、移動体の位置を推定する。また、地図データを生成するデータ生成ユニットを、複数の樹木列の間を移動する移動体に取り付けて使用することもできる。そのようなデータ生成ユニットは、移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、データ生成装置とを備える。データ生成装置は、移動体が移動している間に１つ以上のセンサから繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録する。
　以上のように、本開示は、以下の項目に記載の移動体、制御ユニット、方法、およびコンピュータプログラムを含む。
　［項目１］
　複数の樹木列の間を移動する移動体であって、
　前記移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、
　前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
　前記移動体が移動している間に前記１つ以上のセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
　推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える移動体。
　［項目２］
　前記１つ以上のセンサは、２次元または３次元の点群データを前記センサデータとして出力する少なくとも１つのＬｉＤＡＲセンサを含む、項目１に記載の移動体。
　［項目３］（ＬｉＤＡＲセンサの位置）
　前記ＬｉＤＡＲセンサは、前記樹木列の幹部の平均高さよりも低い位置に搭載されている、項目２に記載の移動体。
　［項目４］
　前記ＬｉＤＡＲセンサは、地上から１５ｃｍ以上１００ｃｍ以下の高さに配置されている、項目３に記載の移動体。
　［項目５］
　前記自己位置推定装置は、前記樹木列の幹部の平均高さよりも低い位置に搭載されている前記ＬｉＤＡＲセンサから繰り返し出力されるセンサデータから複数スキャンの入力点群を取得し、最新スキャンを含む少なくとも１つのスキャンの入力点群と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、項目３または４に記載の移動体。
　［項目６］
　前記樹木列の幹部の平均高さよりも低い位置に搭載されている前記ＬｉＤＡＲセンサは、複数のレーザ光源を有しており、前記複数のレーザ光源は、それぞれ、異なる仰角でレーザパルスを出射し、
　前記自己位置推定装置は、前記複数のレーザ光源のうち、前記仰角が所定の範囲に含まれるレーザ光源から出射された前記レーザパルスの反射点に基づいて前記幹部を検出する、項目３から５のいずれかに記載の移動体。
　［項目７］
　前記自己位置推定装置は、前記ＬｉＤＡＲセンサから前記レーザパルスの反射点までの距離の、前記ＬｉＤＡＲセンサによるスキャン中における変化に基づいて前記幹部を検出する、項目６に記載の移動体。
　［項目８］
　前記１つ以上のセンサは、２次元または３次元の点群データを前記センサデータとして出力する少なくとも１つのＬｉＤＡＲセンサを含み、
　前記ＬｉＤＡＲセンサは、予め設定された周期で前記点群データを繰り返し出力し、
　前記自己位置推定装置は、１周期以上の期間中に出力された前記点群データにおける各点の位置、または各点の前記移動体からの距離もしくは角度に基づいて、前記幹部の表面を表す点を抽出し、抽出した前記点と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、項目１に記載の移動体。
　［項目９］
　慣性計測装置をさらに備え、
　前記自己位置推定装置は、前記慣性計測装置から出力された信号を利用して前記移動体の位置を推定する、
項目１から８のいずれかに記載の移動体。
　［項目１０］
　前記自己位置推定装置は、前記センサデータから、固有の幹間隔の組み合わせを有する複数の樹木の幹部を検出し、前記固有の幹間隔の組み合わせを有する前記複数の樹木の幹部と、前記環境地図データから抽出した前記固有の幹間隔の組み合わせを有する前記複数の樹木の幹部とのマッチングにより、前記移動体の位置を推定する、
項目１から９のいずれかに記載の移動体。
　［項目１１］
　前記制御装置は、前記環境地図データおよび前記センサデータに基づいて、前記移動体の経路上に障害物が存在するか否かを判断し、前記障害物が存在すると判断したとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、項目１から１０のいずれかに記載の移動体。
　［項目１２］
　障害物センサをさらに備え、
　前記制御装置は、前記障害物センサによって前記移動体の経路上に障害物が検出されたとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、項目１から１１のいずれかに記載の移動体。
　［項目１３］
　前記センサデータに基づいて、検出された樹木列の各幹部の太さを計測し、前記各幹の太さを記憶媒体に記録する記録装置をさらに備える、項目１から１２のいずれかに記載の移動体。
　［項目１４］
　前記複数の樹木列が配置されたエリアには１つ以上の支柱が設けられ、
　前記環境地図データは、前記幹部の分布に加えて前記支柱の分布も示し、
　前記自己位置推定装置は、前記センサデータに基づいて、前記移動体の周囲の環境における前記支柱をさらに検出し、検出した前記樹木列の幹部および前記支柱と、前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、
項目１から１３のいずれかに記載の移動体。
　［項目１５］
　前記自己位置推定装置は、前記センサデータと、記憶媒体に記録された前記複数の樹木列の配置関係を示す情報とに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出する、項目１から１４のいずれかに記載の移動体。
　［項目１６］
　前記複数の樹木列の配置関係を示す情報は、ＨＭＩ（Human Machine Interface）を介して入力されることにより、前記記憶媒体に記録される、項目１５に記載の移動体。
　［項目１７］
　前記移動体は自律的に移動可能な作業車両である、項目１から１６のいずれかに記載の移動体。
　［項目１８］
　前記自己位置推定装置は、
　前記センサデータに基づいて検出した前記樹木列の幹部の分布を示すデータを生成し、
　前記幹部の分布を示すデータを用いて前記環境地図データを更新する、または、前記幹部の分布を示すデータを、前記環境地図データを更新する外部の装置に送信する、
項目１から１７のいずれかに記載の移動体。
　［項目１９］
　複数の樹木列の間を移動する移動体の動作を制御する制御ユニットであって、
　前記移動体に取り付けられて使用され、前記移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、
　前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
　前記移動体が移動している間に前記１つ以上のセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
　推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える制御ユニット。
　［項目２０］
　複数の樹木列の間を移動する移動体の動作を制御する方法であって、
　前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
　前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された１つ以上のセンサから取得することと、
　取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
　検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
　推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
を含む方法。
　［項目２１］
　複数の樹木列の間を移動する移動体の動作を制御するコンピュータによって実行されるコンピュータプログラムであって、
　前記コンピュータに、
　前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
　前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された１つ以上のセンサから取得することと、
　取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
　検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
　推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
を実行させるコンピュータプログラム。
　本開示の技術は、例えばぶどう園などの果樹園のように、複数の樹木列が存在する環境を移動するトラクタ、ドローン、歩行ロボットなどの移動体に適用することができる。
　２０　　樹木列
　２２　　樹木の幹部
　３０　　走行経路
　４０　　障害物
　５０　　人
　６０　　ネットワーク
　１００　移動体
　１００Ａ、１００Ｂ　トラクタ
　１０１　車両本体
　１０２　原動機（エンジン）
　１０３　変速装置（トランスミッション）
　１０４　車輪
　１０５　キャビン
　１０６　操舵装置
　１０７　運転席
　１０８　連結装置
　１１０、１１０Ａ、１１０Ｂ　ＬｉＤＡＲセンサ
　１１１　レーザユニット
　１１２　レーザ光源
　１１３　光検出器
　１１４　モータ
　１１５　制御回路
　１１６　信号処理回路
　１１７　メモリ
　１２０　ＧＮＳＳユニット
　１２５　慣性計測装置（ＩＭＵ）
　１３０　障害物センサ
　１４０　駆動装置
　１５０　記憶装置
　１６０　電子制御ユニット（ＳＬＡＭ）
　１６２　自己位置推定モジュール
　１６４　地図データ生成モジュール
　１７０　電子制御ユニット（経路決定）
　１８０　電子制御ユニット（駆動制御）
　１９０　通信インタフェース（ＩＦ）
　２００　操作端末
　３００　作業機（インプルメント）
　３４０　駆動装置
　３８０　制御装置
　３９０　通信インタフェース（ＩＦ）
　４００　ドローン
　４１０　ＬｉＤＡＲセンサ
　４４０　駆動装置
　４５０　データ生成ユニット
　４６０　データ生成装置
　４８０　制御装置
　４９０　通信ＩＦ
　５００　サーバ
　５５０　記憶装置
　５６０　処理装置
　５９０　通信ＩＦ

Claims (18)

  1. 　複数の樹木列の間を移動する移動体であって、
    　前記移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、
    　前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
    　前記移動体が移動している間に前記１つ以上のセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
    　推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
    を備える移動体。
  2. 　前記１つ以上のセンサは、２次元または３次元の点群データを前記センサデータとして出力する少なくとも１つのＬｉＤＡＲセンサを含む、請求項１に記載の移動体。
  3. 　前記ＬｉＤＡＲセンサは、前記樹木列の幹部の平均高さよりも低い位置に搭載されている、請求項２に記載の移動体。
  4. 　前記ＬｉＤＡＲセンサは、地上から１５ｃｍ以上１００ｃｍ以下の高さに配置されている、請求項３に記載の移動体。
  5. 　前記自己位置推定装置は、前記樹木列の幹部の平均高さよりも低い位置に搭載されている前記ＬｉＤＡＲセンサから繰り返し出力されるセンサデータから複数スキャンの入力点群を取得し、最新スキャンを含む少なくとも１つのスキャンの入力点群と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、請求項３または４に記載の移動体。
  6. 　前記樹木列の幹部の平均高さよりも低い位置に搭載されている前記ＬｉＤＡＲセンサは、複数のレーザ光源を有しており、前記複数のレーザ光源は、それぞれ、異なる仰角でレーザパルスを出射し、
    　前記自己位置推定装置は、前記複数のレーザ光源のうち、前記仰角が所定の範囲に含まれるレーザ光源から出射された前記レーザパルスの反射点に基づいて前記幹部を検出する、請求項３から５のいずれかに記載の移動体。
  7. 　前記自己位置推定装置は、前記ＬｉＤＡＲセンサから前記レーザパルスの反射点までの距離の、前記ＬｉＤＡＲセンサによるスキャン中における変化に基づいて前記幹部を検出する、請求項６に記載の移動体。
  8. 　前記１つ以上のセンサは、２次元または３次元の点群データを前記センサデータとして出力する少なくとも１つのＬｉＤＡＲセンサを含み、
    　前記ＬｉＤＡＲセンサは、予め設定された周期で前記点群データを繰り返し出力し、
    　前記自己位置推定装置は、１周期以上の期間中に出力された前記点群データにおける各点の位置、または各点の前記移動体からの距離もしくは角度に基づいて、前記幹部の表面を表す点を抽出し、抽出した前記点と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、請求項１に記載の移動体。
  9. 　慣性計測装置をさらに備え、
    　前記自己位置推定装置は、前記慣性計測装置から出力された信号を利用して前記移動体の位置を推定する、
    請求項１から８のいずれかに記載の移動体。
  10. 　前記自己位置推定装置は、前記センサデータから、固有の幹間隔の組み合わせを有する複数の樹木の幹部を検出し、前記固有の幹間隔の組み合わせを有する前記複数の樹木の幹部と、前記環境地図データから抽出した前記固有の幹間隔の組み合わせを有する前記複数の樹木の幹部とのマッチングにより、前記移動体の位置を推定する、
    請求項１から９のいずれかに記載の移動体。
  11. 　前記制御装置は、前記環境地図データおよび前記センサデータに基づいて、前記移動体の経路上に障害物が存在するか否かを判断し、前記障害物が存在すると判断したとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、請求項１から１０のいずれかに記載の移動体。
  12. 　障害物センサをさらに備え、
    　前記制御装置は、前記障害物センサによって前記移動体の経路上に障害物が検出されたとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、請求項１から１１のいずれかに記載の移動体。
  13. 　前記センサデータに基づいて、検出された樹木列の各幹部の太さを計測し、前記各幹の太さを記憶媒体に記録する記録装置をさらに備える、請求項１から１２のいずれかに記載の移動体。
  14. 　前記自己位置推定装置は、前記センサデータと、記憶媒体に記録された前記複数の樹木列の配置関係を示す情報とに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出する、請求項１から１３のいずれかに記載の移動体。
  15. 　前記移動体は自律的に移動可能な作業車両である、請求項１から１４のいずれかに記載の移動体。
  16. 　前記自己位置推定装置は、
    　前記センサデータに基づいて検出した前記樹木列の幹部の分布を示すデータを生成し、
    　前記幹部の分布を示すデータを用いて前記環境地図データを更新する、または、前記幹部の分布を示すデータを、前記環境地図データを更新する外部の装置に送信する、
    請求項１から１５のいずれかに記載の移動体。
  17. 　複数の樹木列の間を移動する移動体の動作を制御する制御ユニットであって、
    　前記移動体に取り付けられて使用され、前記移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、
    　前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
    　前記移動体が移動している間に前記１つ以上のセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
    　推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
    を備える制御ユニット。
  18. 　複数の樹木列の間を移動する移動体の動作を制御する方法であって、
    　前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
    　前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された１つ以上のセンサから取得することと、
    　取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
    　検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
    　推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
    を含む方法。
PCT/JP2021/040298 2020-11-18 2021-11-01 移動体、制御ユニット、および移動体の動作を制御する方法 WO2022107586A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
EP21894460.1A EP4250044A1 (en) 2020-11-18 2021-11-01 Moving body, control unit, and method for controlling operation of moving body
JP2022563677A JPWO2022107586A1 (ja) 2020-11-18 2021-11-01
US18/198,730 US20230288936A1 (en) 2020-11-18 2023-05-17 Mobile machine, control unit, and method of controlling operation of a mobile machine

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2020-192013 2020-11-18
JP2020192013 2020-11-18

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US18/198,730 Continuation US20230288936A1 (en) 2020-11-18 2023-05-17 Mobile machine, control unit, and method of controlling operation of a mobile machine

Publications (1)

Publication Number Publication Date
WO2022107586A1 true WO2022107586A1 (ja) 2022-05-27

Family

ID=81708785

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2021/040298 WO2022107586A1 (ja) 2020-11-18 2021-11-01 移動体、制御ユニット、および移動体の動作を制御する方法

Country Status (4)

Country Link
US (1) US20230288936A1 (ja)
EP (1) EP4250044A1 (ja)
JP (1) JPWO2022107586A1 (ja)
WO (1) WO2022107586A1 (ja)

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0443916U (ja) * 1990-08-20 1992-04-14
JPH04176786A (ja) * 1989-11-21 1992-06-24 Quest Technol Inc 階段登はん車両用電子式制御システム
JP2010072973A (ja) * 2008-09-19 2010-04-02 Hitachi Automotive Systems Ltd 走行環境認識装置
JP2010096752A (ja) * 2008-09-16 2010-04-30 Adoin Kenkyusho:Kk 樹木情報計測方法、樹木情報計測装置、プログラム
JP2016206876A (ja) * 2015-04-21 2016-12-08 Cyberdyne株式会社 自律移動体の走行経路教示システムおよび走行経路教示方法
WO2017208306A1 (ja) 2016-05-30 2017-12-07 株式会社クボタ 自動走行作業車両
JP2018505426A (ja) * 2014-12-26 2018-02-22 ヘーレ グローバル ベスローテン フェンノートシャップ マルチラテレーションを用いた装置の位置特定
JP2019106940A (ja) * 2017-12-19 2019-07-04 株式会社クボタ 自動走行草刈機
JP2019152924A (ja) * 2018-02-28 2019-09-12 学校法人立命館 自己位置同定システム、車両、及び処理装置
JP2019154379A (ja) 2018-03-15 2019-09-19 ヤンマー株式会社 作業車両および作物列認識プログラム
JP2019154378A (ja) * 2018-03-15 2019-09-19 ヤンマー株式会社 作業車両および作物列認識プログラム
JP2019170312A (ja) * 2018-03-29 2019-10-10 ヤンマー株式会社 作業車両用の自動走行システム
JP2020012680A (ja) 2018-07-13 2020-01-23 株式会社クボタ 作業機
JP2020104617A (ja) 2018-12-26 2020-07-09 株式会社クボタ 作業車両の制御装置、作業車両、及び作業車両の制御方法

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04176786A (ja) * 1989-11-21 1992-06-24 Quest Technol Inc 階段登はん車両用電子式制御システム
JPH0443916U (ja) * 1990-08-20 1992-04-14
JP2010096752A (ja) * 2008-09-16 2010-04-30 Adoin Kenkyusho:Kk 樹木情報計測方法、樹木情報計測装置、プログラム
JP2010072973A (ja) * 2008-09-19 2010-04-02 Hitachi Automotive Systems Ltd 走行環境認識装置
JP2018505426A (ja) * 2014-12-26 2018-02-22 ヘーレ グローバル ベスローテン フェンノートシャップ マルチラテレーションを用いた装置の位置特定
JP2016206876A (ja) * 2015-04-21 2016-12-08 Cyberdyne株式会社 自律移動体の走行経路教示システムおよび走行経路教示方法
WO2017208306A1 (ja) 2016-05-30 2017-12-07 株式会社クボタ 自動走行作業車両
JP2019106940A (ja) * 2017-12-19 2019-07-04 株式会社クボタ 自動走行草刈機
JP2019152924A (ja) * 2018-02-28 2019-09-12 学校法人立命館 自己位置同定システム、車両、及び処理装置
JP2019154379A (ja) 2018-03-15 2019-09-19 ヤンマー株式会社 作業車両および作物列認識プログラム
JP2019154378A (ja) * 2018-03-15 2019-09-19 ヤンマー株式会社 作業車両および作物列認識プログラム
JP2019170312A (ja) * 2018-03-29 2019-10-10 ヤンマー株式会社 作業車両用の自動走行システム
JP2020012680A (ja) 2018-07-13 2020-01-23 株式会社クボタ 作業機
JP2020104617A (ja) 2018-12-26 2020-07-09 株式会社クボタ 作業車両の制御装置、作業車両、及び作業車両の制御方法

Also Published As

Publication number Publication date
JPWO2022107586A1 (ja) 2022-05-27
US20230288936A1 (en) 2023-09-14
EP4250044A1 (en) 2023-09-27

Similar Documents

Publication Publication Date Title
WO2022107588A1 (ja) 移動体、制御ユニット、データ生成ユニット、移動体の動作を制御する方法、およびデータを生成する方法
US11789459B2 (en) Vehicle controllers for agricultural and industrial applications
US11778934B2 (en) Agricultural lane following
Gai et al. Using a depth camera for crop row detection and mapping for under-canopy navigation of agricultural robotic vehicle
US6199000B1 (en) Methods and apparatus for precision agriculture operations utilizing real time kinematic global positioning system systems
US20230288942A1 (en) Mobile machine, data generation unit, and method of generating data
AU2011235939A1 (en) Method and apparatus for machine coordination which maintains line-of-site contact
Fasiolo et al. Towards autonomous mapping in agriculture: A review of supportive technologies for ground robotics
Hutsol et al. Robotic technologies in horticulture: analysis and implementation prospects
WO2022107586A1 (ja) 移動体、制御ユニット、および移動体の動作を制御する方法
WO2024135018A1 (ja) 作物列検出システムおよび農業機械
WO2024135019A1 (ja) 状態推定システムおよび農業機械
WO2024004574A1 (ja) 作業車両、制御方法およびコンピュータプログラム
WO2023243514A1 (ja) 作業車両、および作業車両の制御方法
WO2024004575A1 (ja) 作業車両、および作業車両の制御方法
Vacho et al. Identification of Agricultural Plant Row Using the Clustering Algorithm in the Navigation of Mobile Robot
WO2023112515A1 (ja) 地図生成システムおよび地図生成方法
WO2023234255A1 (ja) センシングシステム、農業機械、およびセンシング装置
WO2024004486A1 (ja) 作業車両、制御方法および制御システム
WO2023127557A1 (ja) 農業機械、農業機械に用いるセンシングシステムおよびセンシング方法
WO2023106158A1 (ja) 自動運転を行う農業機械のための経路計画システム
WO2023248909A1 (ja) 走行制御システム、農業機械および走行制御方法
WO2023119996A1 (ja) 障害物検出システム、農業機械および障害物検出方法
WO2023127556A1 (ja) 農業機械、農業機械に用いるセンシングシステムおよびセンシング方法
Escolà et al. Agricultural Robotic Sensors for Crop and Environmental Modelling

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21894460

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2022563677

Country of ref document: JP

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE

ENP Entry into the national phase

Ref document number: 2021894460

Country of ref document: EP

Effective date: 20230619