WO2022107588A1 - 移動体、制御ユニット、データ生成ユニット、移動体の動作を制御する方法、およびデータを生成する方法 - Google Patents
移動体、制御ユニット、データ生成ユニット、移動体の動作を制御する方法、およびデータを生成する方法 Download PDFInfo
- Publication number
- WO2022107588A1 WO2022107588A1 PCT/JP2021/040300 JP2021040300W WO2022107588A1 WO 2022107588 A1 WO2022107588 A1 WO 2022107588A1 JP 2021040300 W JP2021040300 W JP 2021040300W WO 2022107588 A1 WO2022107588 A1 WO 2022107588A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- moving body
- data
- trunk
- map data
- tractor
- Prior art date
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 39
- 238000000034 method Methods 0.000 title claims description 75
- 238000009826 distribution Methods 0.000 claims abstract description 123
- 238000003860 storage Methods 0.000 claims abstract description 59
- 230000007613 environmental effect Effects 0.000 claims description 194
- 238000010586 diagram Methods 0.000 description 41
- 238000004891 communication Methods 0.000 description 25
- 230000008569 process Effects 0.000 description 22
- 238000012545 processing Methods 0.000 description 20
- 230000008859 change Effects 0.000 description 18
- 230000006870 function Effects 0.000 description 16
- 238000001514 detection method Methods 0.000 description 14
- 238000006243 chemical reaction Methods 0.000 description 11
- 238000005259 measurement Methods 0.000 description 11
- 238000004422 calculation algorithm Methods 0.000 description 10
- 238000004590 computer program Methods 0.000 description 10
- 239000002420 orchard Substances 0.000 description 9
- 230000008878 coupling Effects 0.000 description 8
- 238000010168 coupling process Methods 0.000 description 8
- 238000005859 coupling reaction Methods 0.000 description 8
- 230000005540 biological transmission Effects 0.000 description 7
- 241000196324 Embryophyta Species 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 6
- 238000005304 joining Methods 0.000 description 5
- 230000009466 transformation Effects 0.000 description 5
- 238000010276 construction Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 238000001914 filtration Methods 0.000 description 4
- 238000010801 machine learning Methods 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000013459 approach Methods 0.000 description 3
- 230000005484 gravity Effects 0.000 description 3
- 239000000463 material Substances 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 229910000530 Gallium indium arsenide Inorganic materials 0.000 description 2
- KXNLCSXBJCPWGL-UHFFFAOYSA-N [Ga].[As].[In] Chemical compound [Ga].[As].[In] KXNLCSXBJCPWGL-UHFFFAOYSA-N 0.000 description 2
- 238000002485 combustion reaction Methods 0.000 description 2
- 238000013480 data collection Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000012827 research and development Methods 0.000 description 2
- 230000001932 seasonal effect Effects 0.000 description 2
- 230000002123 temporal effect Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 244000025254 Cannabis sativa Species 0.000 description 1
- 241001124569 Lycaenidae Species 0.000 description 1
- 240000007594 Oryza sativa Species 0.000 description 1
- 235000007164 Oryza sativa Nutrition 0.000 description 1
- XUIMIQQOPSSXEZ-UHFFFAOYSA-N Silicon Chemical compound [Si] XUIMIQQOPSSXEZ-UHFFFAOYSA-N 0.000 description 1
- 241000607479 Yersinia pestis Species 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000001174 ascending effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 235000013399 edible fruits Nutrition 0.000 description 1
- 238000009313 farming Methods 0.000 description 1
- 230000004720 fertilization Effects 0.000 description 1
- 125000001475 halogen functional group Chemical group 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000003909 pattern recognition Methods 0.000 description 1
- 230000001141 propulsive effect Effects 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 235000009566 rice Nutrition 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 229910052710 silicon Inorganic materials 0.000 description 1
- 239000010703 silicon Substances 0.000 description 1
- 238000009331 sowing Methods 0.000 description 1
- 239000007921 spray Substances 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 230000008685 targeting Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
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/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/007—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow
- A01B69/008—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow automatic
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/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/89—Lidar systems specially adapted for specific applications for mapping or imaging
- G01S17/894—3D imaging with simultaneous measurement of time-of-flight at a 2D array of receiver pixels, e.g. time-of-flight cameras or flash lidar
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/483—Details of pulse systems
- G01S7/484—Transmitters
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/483—Details of pulse systems
- G01S7/486—Receivers
- G01S7/4861—Circuits for detection, sampling, integration or read-out
-
- 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
Definitions
- the present disclosure relates to a moving body, a control unit, a data generation unit, a method of controlling the operation of the moving body, and a method of generating data.
- 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 at least two LiDAR sensors that output two-dimensional or three-dimensional point group data showing the distribution of objects in the environment around the moving body, and an environmental map showing the distribution of the trunks of the plurality of tree rows. It includes a storage device for storing data, a self-position estimation device, and a control device.
- the self-position estimation device determines the trunk of the tree row in the environment around the moving body based on the point cloud data repeatedly output from the at least two LiDAR sensors while the moving body is moving.
- the position of the moving body is estimated by detecting and matching the detected trunk of the tree row 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 objects move between a plurality of tree rows.
- the moving body includes at least two LiDAR sensors that output two-dimensional or three-dimensional point group data indicating the distribution of objects in the environment around the moving body, and a data generator.
- the data generator estimates the position of the moving body based on the point cloud data repeatedly output from the at least two LiDAR sensors while the moving body is moving, while surrounding the moving body.
- the trunk of the tree row is detected in the environment of the above, and local map data for generating environmental map data showing the distribution of the detected trunk of the tree row is generated and recorded in the storage device.
- 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 light detector 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 trunk of the tree row while performing self-position estimation and steering control when creating an environmental map by automatic driving.
- 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 uses the scan data for several scans output from one or both of the LiDAR sensors 110A and 110B to determine the local target route of the tractor 100B.
- 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 moving body, the control unit, the data generation unit, the method, and the computer program described in the following items.
- a mobile body that moves between multiple rows of trees At least two LiDAR sensors that output two-dimensional or three-dimensional point cloud data showing 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 point cloud data repeatedly output from the at least two LiDAR 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 is detected.
- a self-position estimation device that estimates the position of the moving object by matching the column executives with the environmental map data, and 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.
- a mobile body that moves between multiple rows of trees At least two LiDAR sensors that output two-dimensional or three-dimensional point cloud data showing the distribution of objects in the environment around the moving object.
- a data generator that detects the trunk of the tree and generates local map data for generating environmental map data showing the distribution of the detected trunk of the tree row and records it in the storage device.
- the at least two LiDAR sensors The first LiDAR sensor mounted at a position lower than the average height of the trunk of the tree row, The moving object according to any one of items 1 to 3, comprising a second LiDAR sensor mounted at a position higher than the average height of the trunk of the tree row.
- Item 4 The moving body according to item 4, wherein the first LiDAR sensor is arranged at a height of 15 cm or more and 100 cm or less from the ground.
- the first LiDAR sensor is arranged so as to emit a laser pulse toward the front of the moving body.
- the second LiDAR sensor is arranged so as to emit a laser pulse toward the front diagonally downward of the moving body.
- the moving object according to item 4 or 5.
- each of the first LiDAR sensor and the second LiDAR sensor is a two-dimensional LiDAR sensor that outputs two-dimensional point cloud data.
- the self-position estimation device acquires an input point cloud of a plurality of scans including the latest scan from the point cloud data repeatedly output from the at least two LiDAR sensors, and the input point cloud of the plurality of scans and the environment map data.
- the moving body according to any one of items 1 to 7, wherein the position of the moving body is estimated by matching with.
- the control device determines whether or not an obstacle exists on the path of the moving body based on the point cloud data output from at least one of the at least two LiDAR sensors, and the obstacle causes the obstacle.
- the at least two LiDAR sensors The first LiDAR sensor mounted at a position lower than the average height of the trunk of the tree row, It includes a second LiDAR sensor mounted at a position higher than the average height of the trunk of the tree row.
- the control device determines whether or not an obstacle exists on the path of the moving body based on the point cloud data output from the second LiDAR sensor, and determines that the obstacle exists.
- the moving body according to item 1 or 3, wherein the moving body is made to perform an operation of avoiding collision with the obstacle.
- a control unit that controls the movement of moving objects that move between multiple rows of trees.
- At least two LiDAR sensors that are attached to the moving object and used to output two-dimensional or three-dimensional point group data indicating the distribution of objects in the environment surrounding 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 point cloud data repeatedly output from the at least two 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.
- a data generation unit used in mobiles that move between multiple rows of trees At least two LiDAR sensors that are attached to the moving object and used to output two-dimensional or three-dimensional point group data indicating the distribution of objects in the environment surrounding the moving object. Based on the point cloud data repeatedly output from the at least two sensors while the moving body is moving, the position of the moving body is estimated, and the tree row in the environment around the moving body is used.
- a data generation device that detects an executive and generates local map data for generating environmental map data showing the distribution of the detected trunk of the tree row and records it in a storage device.
- a data generation unit that comprises.
- 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, at least two sensor data showing the distribution of objects in the environment around the moving body are output, and two-dimensional or three-dimensional point group data mounted on the moving body is output. Obtaining from two LiDAR sensors and 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, at least two sensor data showing the distribution of objects in the environment around the moving body are output, and two-dimensional or three-dimensional point group data mounted on the moving body is output.
- To the computer While the moving body is moving at least two sensor data showing the distribution of objects in the environment around the moving body are output, and two-dimensional or three-dimensional point group data mounted on the moving body is output.
- 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)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Life Sciences & Earth Sciences (AREA)
- Mechanical Engineering (AREA)
- Soil Sciences (AREA)
- Environmental Sciences (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Traffic Control Systems (AREA)
Abstract
複数の樹木が生い茂る環境において、高精度の測位を実現する。移動体は、複数の樹木列の間を移動する。前記移動体は、前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、自己位置推定装置と、前記自己位置推定装置によって推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、を備える。前記自己位置推定装置は、前記移動体が移動している間に前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する。
Description
本開示は、移動体、制御ユニット、データ生成ユニット、移動体の動作を制御する方法、およびデータを生成する方法に関する。
次世代農業として、ICT(Information and Communication Technology)およびIoT(Internet of Things)を活用したスマート農業の研究開発が進められている。圃場で使用されるトラクタなどの作業車両の自動化および無人化に向けた研究開発も進められている。例えば、精密な測位が可能なGNSS(Global Navigation Satellite System)などの測位システムを利用して自動操舵で走行する作業車両が実用化されてきた。特許文献1から3は、GNSSを利用して行った測位の結果に基づいて自動操舵を行う作業車両の例を開示している。
一方、LiDAR(Light Detection and Ranging)などの距離センサを利用して自律的に移動する移動体の開発も進められている。例えば特許文献4は、LiDARを利用して圃場における作物列の間を自動走行する作業車両の例を開示している。
ぶどう園(vineyard)などの果樹園、または森林などの、樹木が高い密度で分布する環境では、樹木の上部で生い茂る葉が樹冠(canopy)を形成し、衛星からの電波にとって障害物または多重反射体として働く。このような環境では、GNSSによる正確な測位が困難である。GNSSを利用できない環境においては、位置推定と地図作成とを同時に実行するSLAM(Simultaneous Localization and Mapping)を利用することが考えられる。しかし、SLAMを利用して多数の樹木が存在する環境内を自律的に、あるいは自動操舵で移動する移動体の実用化には様々な課題が存在する。例えば、季節の変化によって樹木の葉部の分布が大きく変化するため、過去に作成された地図を継続して使用することができないといった課題がある。
本開示の例示的な実施形態による移動体は、複数の樹木列の間を移動する。前記移動体は、前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、自己位置推定装置と、制御装置とを備える。前記自己位置推定装置は、前記移動体が移動している間に前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する。前記制御装置は、推定された前記移動体の位置に応じて前記移動体の移動を制御する。
本開示の例示的な他の実施形態による移動体は、複数の樹木列の間を移動する。前記移動体は、前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、データ生成装置とを備える。前記データ生成装置は、前記移動体が移動している間に前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データに基づいて、前記移動体の位置を推定しながら、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録する。
本開示の包括的または具体的な態様は、装置、システム、方法、集積回路、コンピュータプログラム、もしくはコンピュータが読み取り可能な記録媒体、またはこれらの任意の組み合わせによって実現され得る。コンピュータが読み取り可能な記録媒体は、揮発性の記録媒体を含んでいてもよいし、不揮発性の記録媒体を含んでいてもよい。装置は、複数の装置で構成されていてもよい。装置が2つ以上の装置で構成される場合、当該2つ以上の装置は、1つの機器内に配置されてもよいし、分離した2つ以上の機器内に分かれて配置されていてもよい。
本開示のある実施形態によれば、GNSSによる測位が困難な環境においても、移動体の自動操舵あるいは自律移動を実現することができる。樹木列の幹部の分布は、葉部の分布と比較して季節による変化が少ない。幹部に着目して環境地図を作成することにより、比較的長い期間にわたって同一の環境地図を継続して使用することが可能になる。
[1.用語の定義]
本明細書において用いられる主な用語の定義を以下に記載する。
本明細書において用いられる主な用語の定義を以下に記載する。
「移動体」は、移動のための駆動力(traction)を発生させる車輪、クローラ、二足または多足歩行装置、またはプロペラなどの駆動装置を備える装置である。本開示における「移動体」は、トラクタなどの作業車両、搬送車、モバイルロボット、およびマルチコプタなどの無人航空機(Unmanned aerial vehicle:UAV、いわゆるドローン)を含む。移動体は、無人・有人の別を問わない。
「作業車両」は、圃場(例えば、果樹園、畑、水田、または牧草地など)、山林、または建設現場などの作業地において特定の作業を行いながら走行することが可能な車両を意味する。作業車両の例は、トラクタ、田植機、コンバイン、乗用管理機、および乗用草刈機などの農業機械に加えて、建設作業車および除雪車などの、農業以外の用途で使用される車両も含む。
「SLAM」は、移動体の自己位置推定と地図作成とを同時に行う技術の総称である。
「自己位置推定」は、地図上における移動体の位置(例えば移動体の重心位置)を推定することである。SLAMによる自己位置推定では、通常、移動体のポーズ(pose)が求められる。
「ポーズ」は、物(オブジェクト)の「位置および向き」である。2次元空間におけるポーズは、例えば(x,y,θ)の3個の座標値によって規定される。ここで、(x,y)は、地球に固定されたワールド座標系であるXY座標系における座標値であり、θは基準方向に対する角度である。3次元空間におけるポーズは、例えば(x,y,z,θR,θP,θY)の6個の座標値によって規定される。ここで(x,y,z)は、ワールド座標系であるXYZ座標系における座標値であり、(θR,θP,θY)は、それぞれの基準方向に対するロール、ピッチ、およびヨーの角度である。移動体の姿勢は、(θR,θP,θY)によって表される。ロール角θRは、移動体の前後方向の軸の周りの回転量を表す。ピッチ角θPは、移動体の左右方向の軸の周りの回転量を表す。ヨー角θYは、移動体の上下方向の軸の周りの回転量を表す。姿勢は、オイラー角などの他の角度、またはクォータニオン(四元数)によっても規定され得る。
「環境地図データ」は、移動体が移動する環境に存在する物の位置または領域を所定の座標系によって表現したデータである。環境地図データを単に「環境地図」と称することがある。環境地図を規定する座標系の例は、地球に対して固定された地理座標系などのワールド座標系だけでなく、オドメトリ情報に基づくポーズを表示するオドメトリ座標系などを含む。環境地図データは、環境に存在する物について、位置以外の情報(例えば、属性情報その他の情報)を含んでいてもよい。環境地図は、点群地図または格子地図など、さまざまな形式の地図を含む。以下、環境地図を「地図データ」と呼んだり、単に「地図」と呼んだりすることがある。また、環境地図を構築する過程で生成または処理される局所地図または部分地図のデータについても、「地図データ」または単に「地図」と呼ぶことがある。
「自動操舵」は、手動によらず、制御装置の働きによって移動体の操舵を行うことである。制御装置の一部または全部が移動体の外部にあってもよい。移動体の外部にある制御装置と移動体との間では、制御信号、コマンド、またはデータなどの通信が行われ得る。自動操舵中において、速度制御など、その他の動作は手動で行われてもよい。
「自律移動」は、人が移動の制御に関与することなく、周囲の環境をセンシングしながら、制御装置の働きによって移動体の移動を行うことを意味する。自律移動は、自律走行および自律飛行を含む。制御装置は、移動体の移動に必要な操舵、速度制御、走行の開始および停止、飛行中の上昇・降下・ホバリングなどの動作を制御し得る。自律移動は、障害物の検知および障害物の回避動作を含み得る。
「自動走行」は、移動体が備える制御装置による自律走行、および、運行管理システムにおけるコンピュータからの指令に基づく走行を含む。自律走行には、移動体が所定の経路に沿って目的地に向かう移動のみならず、追尾目標に追従する移動も含まれる。また、一時的に作業者の指示に基づいて移動してもよい。
「自己位置推定装置」は、LiDAR(Light Detection and Ranging)センサなどの外界センサによって取得されたセンサデータに基づいて環境地図上における自己位置を推定する装置である。
「外界センサ」は、移動体の外部の状態をセンシングするセンサである。外界センサの例は、例えば、レーザレンジファインダ(「測域センサ」ともいう)、カメラ(またはイメージセンサ)、LiDARセンサ、ミリ波レーダ、および磁気センサを含む。
「内界センサ」は、移動体の状態をセンシングするセンサである。内界センサは、車輪の回転速度を計測するホイールエンコーダ、加速度センサ、および角加速度センサ(例えばジャイロスコープ)を含む。慣性計測装置(IMU)は、加速度センサおよび角加速度センサを備え、移動体の移動量および姿勢を示す信号を出力することができる。内界センサによって取得される移動体のポーズ変化量を示す情報を「オドメトリ情報」と称する。
「樹木の幹部」は、木本植物の木質化した茎であり、地上に直立して枝を生ずる主軸部分である。樹木の枝、葉および根は含まない。
[2.SLAMの基本原理]
次に、本開示の実施形態において用いられるSLAM技術を利用した自己位置推定の基本原理を説明する。ここでは、簡単のため、移動体が2次元空間(すなわち平面)内を移動するものとする。
次に、本開示の実施形態において用いられるSLAM技術を利用した自己位置推定の基本原理を説明する。ここでは、簡単のため、移動体が2次元空間(すなわち平面)内を移動するものとする。
まず、図1を参照する。図1は、時刻t1から時刻t2までの間に平面上を移動する移動体100の例を模式的に示す図である。この例における移動体100は、車輪によって走行する作業車両(例えばトラクタ)である。移動体100は、トラクタ以外の車両、または歩行ロボットもしくはドローンなどの他の種類の移動体であってもよい。図1には、平面上を移動する移動体100の位置および向きが模式的に示されている。移動体100の位置座標は、XY座標系によって示される。XY座標系は、地球に固定されたワールド座標系Σwである。図1には、移動体100に固定された移動体座標系ΣVも示されている。この例における移動体座標系ΣVは、移動体100の正面が向く方向をu軸方向、u軸を反時計回りに90度回転させた方向をv軸方向とするuv座標系である。移動体座標系ΣVの原点の位置は、移動体100の移動に伴って変化する。移動体座標系ΣVの向きは、移動体100の向きの変化、すなわち回転に伴って変化する。
時刻t1における移動体100のポーズ(すなわち位置および向き)をr1、時刻t2における移動体100のポーズをr2とする。ポーズr1は、例えば、座標(x1,y1)で示される位置と、角度θ1で示される向きとによって規定される。ここでは、移動体100の向きは移動体100の正面が向く方向であると定義する。また、X軸正方向を角度の基準方向とし、反時計回りを角度の正方向とする。ポーズr2は、座標(x2,y2)で示される位置と、角度θ2で示される向きとによって規定される。
図1の例において、時刻t1から時刻t2までの間に、移動体100の位置は、角度θ1の方位にΔd1だけ移動(並進)し、移動体100の向きは、Δφ1(=θ2-θ1)だけ反時計回りに回転している。このように、移動体100の運動は、「並進」と「回転」の合成である。
移動体100が車輪の回転数センサおよび/または慣性計測装置(IMU)などの内界センサを備えている場合、その内界センサから、Δd1およびΔφ1の推定値、すなわちオドメトリ情報を取得することができる。時刻t1から時刻t2までの時間差が例えば10ミリ秒のように短い場合、距離Δd1は十分に短く、式1が成立する。時刻がt1、t2、t3、・・・と進む過程で、Δd1およびΔφ1の推定値を周期的に更新していくことにより、移動体100の位置および向き(すなわちポーズ)の変化を推定することが可能である。言い換えると、初期のポーズ、例えば(x1,y1,θ1)が既知である場合、その後の移動体100のポーズの推定値をオドメトリ情報に基づいて周期的に更新することができる。しかし、オドメトリ情報に基づくポーズの推定には、誤差が累積するという問題がある。このため、多くの場合、衛星測位システムまたはSLAM技術を利用して、移動体100の位置の正確度の高い推定値を取得することが必要である。
次に図2Aから図5を参照して、LiDARなどの外界センサを用いて行われる自己位置推定を説明する。以下、環境地図データが存在する場合と存在しない場合のそれぞれについて、自己位置推定動作の例を説明する。
<環境地図データが存在する場合>
図2Aおよび図2Bに示される例において、時刻t1から時刻t2までの間に移動体100が移動する環境には、位置が固定されたランドマークm1、m2、m3、m4が存在する。この例では、ランドマークの位置を示す環境地図データがすでに存在する。このような環境地図データは、例えば、個々のランドマークを特定する識別子(例えばランドマーク番号)と、各識別子に対応付けられたランドマークのワールド座標系Σwにおける位置座標とを含む。ワールド座標系Σwにおけるランドマークm1、m2、m3、m4の座標を、それぞれ、(xm1,ym1)、(xm2,ym2)、(xm3,ym3)、(xm4,ym4)とする。LiDARによって取得される点群データにおける個々の点がランドマークとして機能してもよい。点群データによって構成される環境地図を、点群地図と称する。ランドマークの位置は格子地図上のセルによって表示されていてもよい。
図2Aおよび図2Bに示される例において、時刻t1から時刻t2までの間に移動体100が移動する環境には、位置が固定されたランドマークm1、m2、m3、m4が存在する。この例では、ランドマークの位置を示す環境地図データがすでに存在する。このような環境地図データは、例えば、個々のランドマークを特定する識別子(例えばランドマーク番号)と、各識別子に対応付けられたランドマークのワールド座標系Σwにおける位置座標とを含む。ワールド座標系Σwにおけるランドマークm1、m2、m3、m4の座標を、それぞれ、(xm1,ym1)、(xm2,ym2)、(xm3,ym3)、(xm4,ym4)とする。LiDARによって取得される点群データにおける個々の点がランドマークとして機能してもよい。点群データによって構成される環境地図を、点群地図と称する。ランドマークの位置は格子地図上のセルによって表示されていてもよい。
図2Aの例において、ポーズr1における移動体100が取得したランドマークm1、m2、m3、m4の観測値(または計測値)を、それぞれ、z1、z2、z3、z4とする。観測値は、例えば、外界センサによって計測される各ランドマークまでの距離および方向のそれぞれを示す値である。観測値は、外界センサによって取得されるデータであるため、本開示において「センサデータ」と称することがある。
図2Bに示されるように、移動体100が移動し、ポーズr2における移動体100が取得したランドマークm1、m2、m3の観測値を、それぞれ、z5、z6、z7とする。
これらの観測値に基づいて、移動体座標系ΣV上のランドマークの位置座標を取得することができる。前述したように、移動体座標系ΣVは移動体100に固定された座標系であるため、同一のランドマーク(例えばランドマークm1)の移動体座標系ΣV上における位置座標は、移動体100のポーズの変化に応じて変化していく。
観測値に基づいて取得されるランドマークの位置は、外界センサの位置および向きによって定まるセンサ座標系における座標を有する。厳密には、センサ座標系は移動体座標系ΣVとは異なり得るが、以下の説明では、センサ座標系と移動体座標系ΣVとは同一であるものとする。センサ座標系と移動体座標系ΣVとの関係は既知であるため、一方の座標系を既知の角度だけ回転させ、かつ、既知の距離だけ並進させることにより、他方の座標系に一致させることができる。
環境内を移動する移動体100は、複数のランドマークを観測することにより、移動体座標系ΣVにおける各ランドマークの位置座標を取得する。そして、ワールド座標系ΣWにおける各ランドマークの位置座標が環境地図データに含まれている場合、観測値z1~z7などに基づいてポーズr1、r2を推定することができる。このような推定は、例えば、観測値から求められるランドマークの位置座標と、環境地図データに含まれるランドマークの位置座標とのマッチングを行うことにより可能になる。
次に、図3を参照して、移動体座標系ΣV上の座標(u1,v1)をワールド座標系ΣW上の座標(u1’、v1’)に変換する方法を説明する。図3は、時刻t2における移動体座標系ΣVとワールド座標系ΣWとの配置関係を示している。図3の例では、簡単のため、移動体座標系ΣVの原点をセンサ座標系の原点(センサ中心)と一致させ、両座標系が同一であるものとする。移動体座標系ΣV上の座標(u1,v1)は、時刻t2のポーズr2における移動体100からのランドマークm1に対する観測値z5に基づいて算出される。観測値z5が、距離rm1とu軸に対する角度φm1とによって規定されるとすると、u1およびv1は、u1=rm1cosφm1、v1=rm1sinφm1によって計算される。移動体座標系ΣV上の座標からワールド座標系ΣW上の座標への変換(座標変換)は、移動体座標系ΣVを時計回りに角度θ2だけ回転させること、および、移動体座標系ΣVの原点をワールド座標系ΣWの原点まで並進させることによって行われる。このとき、以下の式2および式3が成立する。
ここで、Rは、移動体100の向きによって決まる回転行列であり、Tはワールド座標系Σwにおける移動体100の位置ベクトルである。回転行列Rおよび位置ベクトルTの中身は、移動体100のポーズから求められる。
座標変換によって得られる(u1’、v1’)は、ワールド座標系Σwにおけるランドマークm1の座標である(xm1,ym1)に一致すべきである。しかし、移動体100のポーズの推定値が真値からずれていると、(u1’,v1’)と(xm1,ym1)との間に誤差(距離)が生じる。自己位置推定を行うことは、(u1’,v1’)と(xm1,ym1)との間の誤差が小さくなるように回転行列Rおよび位置ベクトルTの中身を決定することである。
次に、図4および図5を参照してマッチングの一例をより詳細に説明する。図4は、時刻t2における移動体座標系ΣVであるuv座標系上のランドマークm1、m2、m3の座標を模式的に示している。これらの座標は、観測値z5、z6、z7から算出され得る。この例において、観測値z5、z6、z7から算出される座標を、それぞれ、(u5,v5)、(u6,v6)、(u7,v7)とする。移動体座標系ΣVからワールド座標系ΣWへの座標変換を行うことにより、移動体座標系ΣV上の(u5,v5)、(u6,v6)、(u7,v7)は、それぞれ、図5に示されるように、ワールド座標系ΣW上の(u5’,v5’)、(u6’,v6’)、(u7’,v7’)に変換される。図5は、変換された座標(u5’,v5’)、(u6’,v6’)、(u7’,v7’)と、対応するランドマークの座標(xm2,ym2)、(xm3,ym3)、(xm4,ym4)との間に差異が生じている状態の例を示している。座標変換は、回転行列Rによる回転、および位置ベクトルTによる並進によって規定される。ワールド座標系ΣW上において、(u5’,v5’)、(u6’,v6’)、(u7’,v7’)が、それぞれ、対応するランドマークの座標である(xm2,ym2)、(xm3,ym3)、(xm4,ym4)に近づくように(すなわちマッチするように)座標変換(R、T)の中身が決定される。
前述した式2の等式において、未知数はx2、y2、θ2である。未知数の個数は3であるため、式2に相当する等式が3個以上あれば、計算によって、x2、y2、θ2を求めることができる。図5に示されるように同一のポーズr2における移動体100から3個以上のランドマークが観測される場合、式2に相当する等式が3個得られるため、連立方程式を解くことにより、ポーズr2(x2、y2、θ2)の計算値を取得できる。現実の観測には誤差が加わるため、3個を超える多数の式からなる連立方程式を用い、最小二乗法などによる最適化が行われる。
このようなマッチングによる最適化を行って自己位置を推定するアルゴリズムの例は、ICPマッチング法およびNDTマッチング法を含む。これらのいずれのマッチング法、または他の方法を用いてもよい。
なお、観測値と環境地図データから移動体100の位置を推定するとき、必ずしも移動体100の向きを推定する必要はない。例えば図2Aおよび図2Bに示されるように、位置が既知であるランドマークまでの距離が観測値から取得できる場合、三角測量の原理に基づいて、移動体100の位置(正確には測距センサの位置)を算出することが可能である。このような原理に基づいて位置を推定することも、「自己位置推定」に含まれる。
<環境地図データが存在しない場合>
環境地図データが存在しない場合、図2Aおよび図2Bに示されるように移動体100が移動中に取得した観測値に基づいて、ランドマークm1、m2、m3、m4、・・・の座標を決定(すなわち推定)しながら、移動体100のポーズを推定する処理が必要になる。ランドマークm1、m2、m3、m4、・・・のワールド座標系ΣW上における座標は、環境地図データを構成する。したがって、地図を構築するには、ランドマークとして機能する物体のワールド座標系ΣW上における位置座標を決定する必要がある。
環境地図データが存在しない場合、図2Aおよび図2Bに示されるように移動体100が移動中に取得した観測値に基づいて、ランドマークm1、m2、m3、m4、・・・の座標を決定(すなわち推定)しながら、移動体100のポーズを推定する処理が必要になる。ランドマークm1、m2、m3、m4、・・・のワールド座標系ΣW上における座標は、環境地図データを構成する。したがって、地図を構築するには、ランドマークとして機能する物体のワールド座標系ΣW上における位置座標を決定する必要がある。
図2Aおよび図2Bの例では、移動しつつある移動体100がランドマークm2、m3、m4のそれぞれから複数の観測値を取得している。より多くの観測を行うことにより、移動体100のポーズおよびランドマークの座標に含まれる未知数の総数よりも多くの連立方程式を得ることができる。それにより、移動体100のポーズおよびランドマークの座標の推定値を算出することが可能である。
SLAMによって自己位置推定と環境地図の作成を行うためのアルゴリズムには、多数の種類が存在する。SLAMアルゴリズムの例は、LiDARセンサを利用するアルゴリズムだけではなく、カメラなどの他の外界センサを利用するアルゴリズムも含む。また、自己位置推定にパーティクルフィルタなどのベイズフィルタを利用したり、グラフベース手法によってポーズ推定の精度を高めたりしてもよい。本開示の実施形態では、SLAMアルゴリズムの種類は特に限定されない。
なお、LiDARセンサには、レーザビームのスキャンによって空間中の物体の距離分布の情報を取得するスキャン型のセンサと、広範囲に拡散する光を利用して空間中の物体の距離分布の情報を取得するフラッシュ型のセンサとがある。スキャン型のLiDARセンサは、フラッシュ型のLiDARセンサよりも高い強度の光を使用するため、より遠方の距離情報を取得することができる。一方、フラッシュ型のLiDARセンサは、構造が簡単であり、低いコストで製造できるため、強い光を必要としない用途に適している。本開示では、主にスキャン型のLiDARセンサが用いられる例を説明するが、用途によってはフラッシュ型のLiDARセンサも使用され得る。
スキャン型の典型的なLiDARセンサによって環境内の物体を観測する場合、パルス状のレーザビーム(すなわちレーザパルス)を出射し、周囲の環境に存在する物体によって反射されたレーザパルスがLiDARセンサに帰還するまでの時間を計測することにより、物体の表面に位置する反射点までの距離と方向とを知ることができる。反射点までの距離および方向がわかれば、移動体座標系ΣVにおける「反射点」の座標を取得することができる。スキャン型のLiDARセンサは、2次元LiDARセンサと、3次元LiDARセンサとに分類することができる。2次元LiDARセンサによれば、レーザビームが1つの平面内を回転するように環境のスキャンが行われ得る。これに対して、3次元LiDARセンサでは、複数のレーザビームがそれぞれ異なる円錐面に沿って回転するように環境のスキャンが行われ得る。これらのLiDARセンサによって取得される個々の反射点の座標(2次元または3次元の座標値)は、移動体座標系ΣVで表される。個々の反射点の座標を移動体座標系ΣVからワールド座標系ΣWに変換することにより、個々の反射点のワールド座標系ΣW上における座標を得ることができ、その結果、点群地図を構築することができる。移動体座標系ΣVからワールド座標系ΣWに変換するためには、上記のように、移動体100のポーズの情報が必要である。
SLAM技術では、移動体100のポーズの推定と、環境地図の構築とを同時に実行することができる。しかしながら、環境地図を構築するときにSLAM技術以外の技術を利用して移動体100のポーズを推定または測定してもよい。移動体100のポーズは、移動体100の位置を測定する衛星測位システムおよび慣性誘導装置を利用しても決定することができるからである。しかしながら、衛星測位システムのような測位システムを利用できない状況では、環境地図を構築するためにSLAM技術を利用して移動体100のポーズを推定することが必要になる。なお、移動体100の位置についてはSLAM技術を利用して推定し、移動体100の向きまたは姿勢については、慣性計測装置などの他のセンサによって推定してもよい。
[3.実施形態]
以下、本開示の実施形態を説明する。ただし、必要以上に詳細な説明は省略する場合がある。例えば、既によく知られた事項の詳細説明および実質的に同一の構成に対する重複説明を省略する場合がある。これは、以下の説明が不必要に冗長になることを避け、当業者の理解を容易にするためである。なお、発明者は、当業者が本開示を十分に理解するために添付図面および以下の説明を提供するのであって、これらによって特許請求の範囲に記載の主題を限定することを意図するものではない。以下の説明において、同一または類似の機能を有する構成要素については、同じ参照符号を付している。
以下、本開示の実施形態を説明する。ただし、必要以上に詳細な説明は省略する場合がある。例えば、既によく知られた事項の詳細説明および実質的に同一の構成に対する重複説明を省略する場合がある。これは、以下の説明が不必要に冗長になることを避け、当業者の理解を容易にするためである。なお、発明者は、当業者が本開示を十分に理解するために添付図面および以下の説明を提供するのであって、これらによって特許請求の範囲に記載の主題を限定することを意図するものではない。以下の説明において、同一または類似の機能を有する構成要素については、同じ参照符号を付している。
[3-1.実施形態1]
本開示の第1の実施形態による移動体を説明する。
本開示の第1の実施形態による移動体を説明する。
本実施形態における移動体は、複数の樹木が生い茂り、複数の樹木列が形成されている環境で使用される。移動体は、周囲の環境における物体の分布を示すセンサデータを出力する1つ以上のセンサと、記憶装置と、自己位置推定装置と、移動体の移動を制御する制御装置とを備える。記憶装置は、複数の樹木列の幹部の分布を示す環境地図データを記憶する。自己位置推定装置は、移動体が移動している間に1つ以上のセンサから繰り返し出力されるセンサデータに基づいて、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部と環境地図データとのマッチングを行うことにより、移動体の位置を推定する。制御装置は、推定された移動体の位置に応じて移動体の移動を制御する。本実施形態における移動体は、環境地図データ、または環境地図データを生成するための局所地図データを生成するデータ生成装置をさらに備えている。データ生成装置は、移動体が移動している間に1つ以上のセンサから繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録する。データ生成装置は、繰り返し生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成し、記憶装置に記録することもできる。このように、本実施形態における移動体は、複数の樹木列の間を移動しながら環境地図を作成する機能と、作成した環境地図を利用して移動体の位置を推定しながら樹木列間を自律的に移動する機能とを備える。
1つ以上のセンサは、例えば2次元または3次元の点群データをセンサデータとして出力する少なくとも1つのLiDARセンサを含み得る。本明細書において「点群データ」は、LiDARセンサによって観測される複数の反射点の分布を示すデータを広く意味する。点群データは、例えば、2次元空間または3次元空間における各反射点の座標値、および/または各反射点の距離および方向を示す情報を含み得る。
LiDARセンサは、例えば予め設定された周期で点群データを繰り返し出力する。データ生成装置は、1周期以上の期間中に出力された点群データにおける各点の位置、または各点の移動体からの距離もしくは角度に基づいて、幹部を検出することができる。
以下、移動体が、主にぶどう園などの果樹園における作業に用いられるトラクタなどの作業車両であり、センサがスキャン型のLiDARセンサである場合を例に、本実施形態の構成および動作を説明する。
<構成>
図6は、本実施形態におけるトラクタ100Aの外観を示す斜視図である。図7は、トラクタ100Aを側面方向から見た模式図である。図7には、トラクタ100Aに連結されて使用される作業機(implement)300も示されている。
図6は、本実施形態におけるトラクタ100Aの外観を示す斜視図である。図7は、トラクタ100Aを側面方向から見た模式図である。図7には、トラクタ100Aに連結されて使用される作業機(implement)300も示されている。
図6に示すように、本実施形態におけるトラクタ100Aは、LiDARセンサ110と、GNSSユニット120と、1つ以上の障害物センサ130とを備える。図6の例では1つの障害物センサ130が示されているが、障害物センサ130は、トラクタ100Aの複数の箇所に設けられ得る。
図7に示すように、トラクタ100Aは、車両本体101と、原動機(エンジン)102と、変速装置(トランスミッション)103とを備える。車両本体101には、車輪(タイヤ)104と、キャビン105とが設けられている。車輪104は、前輪104Fと後輪104Rとを含む。キャビン105の内部に運転席107、操舵装置106、および操作端末200が設けられている。前輪104Fおよび後輪104Rの一方または両方は、タイヤではなくクローラであってもよい。
本実施形態におけるLiDARセンサ110は、車両本体101の前面下部に配置されている。LiDARセンサ110は、トラクタ100Aが走行する環境に存在する樹木列の幹部の平均高さよりも低い位置、例えば、地上から15cm以上100cm以下の高さに配置され得る。LiDARセンサ110は、例えば、トラクタ100Aの高さの半分よりも低い位置に配置され得る。トラクタ100Aの高さは、地面からキャビン105の最上部までの高さであり、例えば2メートル以上であり得る。LiDARセンサ110は、トラクタ100Aの高さの1/3よりも低い位置に配置されていてもよい。図7の例では、LiDARセンサ110は、車輪104の上部および前照灯109よりも低い位置に配置されている。LiDARセンサ110をこのような低い位置に配置することによって得られる効果については後述する。
LiDARセンサ110は、トラクタ100Aが移動している間、周囲の環境に存在する物体の距離および方向、または2次元もしくは3次元の座標値を示すセンサデータを繰り返し出力する。LiDARセンサ110から出力されたセンサデータは、トラクタ100Aが備えるECU(Electronic Control Unit)などの制御装置によって処理される。制御装置は、前述のSLAMアルゴリズムを利用して、センサデータに基づく環境地図データの生成、および環境地図を用いた自己位置推定などの処理を実行することができる。なお、制御装置は、環境地図データを完成する代わりに、環境地図データを生成するための局所地図データを作成してもよい。その場合、局所地図データを統合して環境地図データを構築する処理は、トラクタ100Aの外部にあるコンピュータ、例えばクラウドサーバで実行されてもよい。
図7の例では、車両本体101の後部に障害物センサ130が設けられている。障害物センサ130は、車両本体101の後部以外の部位にも配置され得る。例えば、車両本体101の側部、前部、およびキャビン105のいずれかの箇所に、1つまたは複数の障害物センサ130が設けられ得る。障害物センサ130は、トラクタ100Aの比較的近くに存在する物体を検出する。障害物センサ130は、例えばレーザスキャナまたは超音波ソナーを備え得る。障害物センサ130は、障害物センサ130から所定の距離よりも近くに物体が存在する場合に、障害物が存在することを示す信号を出力する。複数の障害物センサ130がトラクタ100Aの車体の異なる位置に設けられていてもよい。例えば、複数のレーザスキャナと、複数の超音波ソナーとが、車体の異なる位置に配置されていてもよい。そのような多くの障害物センサ130を備えることにより、トラクタ100Aの周囲の障害物の監視における死角を減らすことができる。LiDARセンサ110が障害物センサとして機能してもよい。
GNSSユニット120は、キャビン105の上部に設けられている。GNSSユニット120は、GNSS衛星からの信号を受信するアンテナと、処理回路とを備えるGNSS受信機である。GNSSユニット120は、例えばGPS(Global Positioning System)、GLONAS、Galileo、BeiDou、または準天頂衛星(Quasi-Zenith Satellite System:QZSS、例えばみちびき)などのGNSS衛星から送信されるGNSS信号を受信し、当該信号に基づいて測位を行う。本実施形態におけるトラクタ100Aは、主にぶどう園のように、複数の樹木が生い茂り、GNSSを利用しにくい環境で使用されるため、LiDARセンサ110を利用して測位が行われる。しかし、GNSS信号を受信可能な環境ではGNSSユニット120を利用して測位を行ってもよい。LiDARセンサ110による測位とGNSSユニット120による測位とを組み合わせることにより、測位の安定性または精度を向上させることができる。
原動機102は、例えばディーゼルエンジンである。ディーゼルエンジンに代えて電動モータが使用されてもよい。変速装置103は、変速によってトラクタ100Aの推進力を切り換えることができる。変速装置103は、トラクタ100Aの前進と後進とを切り換えることもできる。
操舵装置106は、ステアリングホイールと、ステアリングホイールに接続された操舵軸と、ステアリングホイールによる操舵を補助するパワーステアリング装置とを含む。パワーステアリング装置は、操舵軸を回転させるための補助力を供給する油圧装置または電動モータを含む。前輪104Fは操舵輪であり、その操舵角を変化させることにより、トラクタ100Aの走行方向を変化させることができる。前輪104Fの操舵角は、ステアリングホイールを操作することによって変化させることができる。自動操舵が行われるときには、トラクタ100A内に配置された電子制御ユニット(ECU)からの制御により、油圧装置または電動モータの力によって操舵角が自動で調整される。
車両本体101の後部には、連結装置108が設けられている。連結装置108は、例えば3点リンク(ヒッチ)装置、PTO(Power Take Off)軸、およびユニバーサルジョイントを含む。連結装置108によって作業機300をトラクタ100Aに着脱することができる。連結装置108は、例えば油圧装置によって3点リンク装置を昇降させ、作業機300の位置または姿勢を制御することができる。また、ユニバーサルジョイントを介してトラクタ100Aから作業機300に動力を送ることができる。トラクタ100Aは、作業機300を引きながら、作業機300に所定の作業を実行させることができる。連結装置は、車両本体101の前方に設けられていてもよい。その場合、トラクタ100Aの前方に作業機を接続することができる。なお、作業機がトラクタ100Aの前方に接続された状態でLiDARセンサ110が使用される場合、LiDARセンサ110は、レーザビームが作業機によって遮られない位置に配置される。
図7に示す作業機300は、作物に薬剤を噴霧するスプレイヤであるが、作業機300はスプレイヤに限定されない。例えば、モーア(草刈機)、シーダ(播種機)、スプレッダ(施肥機)、レーキ、ベーラ(集草機)、ハーベスタ(収穫機)、プラウ、ハロー、またはロータリなどの、任意の作業機をトラクタ100Aに接続して使用することができる。
以下の説明において、図7に示すuvw座標系を用いる。uvw座標系は、トラクタ100AおよびLiDARセンサ110に固定された座標系である。トラクタ100Aの進行方向をu軸方向、左方向をv軸方向、上方向をw軸方向とする。図面では、わかりやすさのため、uvw座標系の原点がトラクタ100Aから離れた位置に存在するように記載されているが、現実には、トラクタ100Aに設けられるLiDARセンサ110の中心にuvw座標系の原点が一致している。
図8は、トラクタ100Aおよび作業機300の概略的な構成の例を示すブロック図である。この例におけるトラクタ100Aは、LiDARセンサ110、GNSSユニット120、障害物センサ130、操作端末200に加え、慣性計測装置(IMU)125、駆動装置140、記憶装置150、複数の電子制御ユニット(ECU)160、170、180、および通信インタフェース(IF)190を備える。作業機300は、駆動装置340と、制御装置380と、通信インタフェース390とを備える。なお、図8には、本実施形態における自己位置推定動作、自動走行動作、および地図生成動作との関連性が相対的に高い構成要素が示されており、それ以外の構成要素の図示は省略されている。
本実施形態におけるIMU125は、3軸加速度センサおよび3軸ジャイロスコープを備える。IMU125は、モーションセンサとして機能し、トラクタ100Aの加速度、速度、変位、および姿勢などの諸量を示す信号を出力することができる。IMU125は、例えば1秒間に数十回から数千回程度の頻度で、当該信号を出力する。IMU125に代えて、3軸加速度センサおよび3軸ジャイロスコープを別々に設けてもよい。
駆動装置140は、例えば前述の原動機102、変速装置103、車輪104、操舵装置106、および連結装置108などの、トラクタ100Aの走行および作業機300の駆動に必要な各種の装置を含む。原動機102は、例えばディーゼル機関などの内燃機関を備える。駆動装置140は、内燃機関に代えて、あるいは内燃機関とともに、トラクション用の電動モータを備えていてもよい。
記憶装置150は、フラッシュメモリまたは磁気ディスクなどの記憶媒体を含み、環境地図データを初めとするECU160、170、180が生成する各種のデータを記憶する。記憶装置150は、ECU160、170、180に、後述する各種の動作を実行させるコンピュータプログラムも記憶する。そのようなコンピュータプログラムは、記憶媒体(例えば半導体メモリまたは光ディスク等)または電気通信回線(例えばインターネット)を介してトラクタ100Aに提供され得る。そのようなコンピュータプログラムが、商用ソフトウェアとして販売されてもよい。
ECU160は、前述のSLAM技術に基づく処理を実行する電子回路である。ECU160は、自己位置推定モジュール162および地図データ生成モジュール164を含む。ECU160は、前述の自己位置推定装置およびデータ生成装置の一例である。ECU160は、トラクタ100Aが走行している間にLiDARセンサ110およびIMU125から繰り返し出力されたデータまたは信号に基づき、トラクタ100Aの位置および向き(すなわちポーズ)の推定を行いながら地図データを生成することができる。ECU160はまた、環境地図データが生成された後の自動走行時において、LiDARセンサ110から出力されたセンサデータと環境地図データとのマッチングを行うことにより、トラクタ100Aの位置および向きを推定することができる。
自己位置推定モジュール162は、トラクタ100Aの位置および向きの推定、すなわち自己位置推定を行うための演算を実行する。地図データ生成モジュール164は、地図データを生成する処理を実行する。地図データ生成モジュール164が生成する「地図データ」には、自動走行時のマッチングに使用される環境地図データと、環境地図データを構築するために生成される部分的なデータである局所地図データとが含まれる。地図データ生成モジュール164は、繰り返し生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成し、記憶装置150に記録することができる。自己位置推定モジュール162および地図データ生成モジュール164は、1つの回路で実現されていてもよいし、複数の回路に分かれていてもよい。
環境地図データがまだ生成されていない段階において、ECU160は、自己位置推定を行いながら、トラクタ100Aの周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す局所地図データを繰り返し生成して記憶装置150に記録することができる。ECU160は、さらに、局所地図データを繋ぎ合わせることにより、圃場(例えばぶどう園)の全体、または圃場の一区画についての環境地図データを生成することができる。圃場の区画ごとに環境地図データが生成されてもよい。ECU160は、局所地図データを生成する段階では樹木列の幹部を検出せず、最終的な環境地図データを生成する段階で幹部を検出し、幹部を他の物体と区別できる形式で記録してもよい。環境地図データが生成された後の自動走行時において、ECU160は、LiDARセンサ110から出力されるセンサデータと環境地図データとのマッチングを行うことにより、トラクタ100Aの位置および向きを推定する。なお、ECU160は、トラクタ100Aの位置のみをマッチングで決定し、トラクタ100Aの向きはIMU125からの信号を利用して決定してもよい。
ECU170は、トラクタ100Aの経路を決定する処理を行う回路である。環境地図データがまだ生成されていない段階において、ECU170は、LiDARセンサ110および障害物センサ130から出力されたデータまたは信号に基づいて、トラクタ100Aが走行すべき経路を決定する。例えば、LiDARセンサ110から出力されるセンサデータに基づいて検出される複数の樹木列の間を通り、かつ障害物を避ける経路が目標経路として決定される。環境地図データが生成された後の自動走行時において、ECU170は、環境地図データに基づいて、またはユーザからの指示に基づいて、目標経路(以下、「走行予定経路」とも称する。)を決定する。
ECU180は、駆動装置140を制御する回路である。ECU180は、ECU160によって推定されたトラクタ100Aの位置および向きと、ECU170によって決定された走行予定経路とに基づいて、駆動装置140を制御する。ECU180はまた、作業機300の動作を制御する信号を生成し、その信号を通信IF190から作業機300に送信する動作も実行する。
ECU160、170、180は、例えばCAN(Controller Area Network)などのビークルバス規格に従って、相互に通信することができる。図8において、ECU160、170、180のそれぞれは、個別のブロックとして示されているが、これらのそれぞれの機能が、複数のECUによって実現されていてもよい。あるいは、ECU160、170、180の機能を統合した1つの車載コンピュータが設けられていてもよい。トラクタ100Aは、ECU160、170、180以外のECUを備えていてもよく、機能に応じて任意の個数のECUが設けられ得る。
通信IF190は、作業機300の通信IF390との間で通信を行う回路である。通信IF190は、例えばISO 11783に基づくISOBUSなどの通信制御規格に準拠した信号の送受信を、作業機300の通信IF390との間で実行する。これにより、作業機300に所望の動作を実行させたり、作業機300から情報を取得したりすることができる。通信IF190はまた、有線または無線のネットワークを介して外部のコンピュータと通信することもできる。例えば、作物の生育状況、トラクタ100Aの稼働状況、および作業記録等を管理する営農管理システムにおけるサーバなどのコンピュータと通信することができる。
操作端末200は、トラクタ100Aの自動走行または自動操舵に関する操作をユーザが実行するための端末であり、バーチャルターミナル(VT)と称することもある。操作端末200は、タッチスクリーンなどの表示装置、および/または1つ以上のボタンを備え得る。ユーザは、操作端末200を操作することにより、例えば自動走行モードまたは自動操舵モードのオン/オフの切り替え、トラクタ100Aの初期位置の設定、経路の設定、環境地図の記録または編集などの種々の操作を実行することができる。
作業機300における駆動装置340は、作業機300が所定の作業を実行するために必要な動作を行う。駆動装置340は、例えばポンプ、油圧装置、または電気モータなどの、作業機300の用途に応じた装置を含む。
制御装置380は、駆動装置340の動作を制御する。制御装置380は、通信IF390を介してトラクタ100Aから送信された信号に応答して、駆動装置340に各種の動作を実行させる。
次に、図9Aおよび図9Bを参照して、LiDARセンサ110の構成例を説明する。本実施形態におけるLiDARセンサ110は、レーザビームのスキャンによって空間中の物体の距離分布の情報を取得可能なスキャン型のセンサである。図9Aは、LiDARセンサ110をトラクタ100Aの側面方向から見た場合の模式図である。図9Bは、LiDARセンサ110を鉛直上方から見た場合の模式図である。図9Aおよび図9Bにおいて放射状に延びる複数の直線は、LiDARセンサ110から出射されるレーザビームの中心軸(あるいは進行方向)を模式的に表している。各レーザビームは、平行光にコリメートされているが、数ミリラジアン(例えば0.1~0.2度)の拡がり角度を有している。このため、各レーザビームの断面サイズ(スポット径)は、LiDARセンサ110からの距離に比例して拡大する。例えば、LiDARセンサ110から20メートル先で直径が数センチメートルの光スポットが形成され得る。図では、簡単のため、レーザビームの広がりは無視し、レーザビームの中心軸のみが記載されている。
図9Aに示す例におけるLiDARセンサ110は、縦方向に配列された複数のレーザ光源から、それぞれ、異なる仰角でレーザビームを出射することができる。仰角は、uv面に対する角度によって定義される。この例において、uv面は水平面にほぼ平行である。なお、地面(地表)が水平面に対して傾斜している場合は、uv面と水平面とが交差する。図9Aには、N本のレーザビームL1、・・・、LNが出射されている様子が示されている。ここで「N」は、1以上の整数であり、例えば10以上、高性能な機種では64、あるいは100以上であり得る。複数のレーザビームのうち、下からk番目のレーザビームの仰角をθkとする。図9Aには、一例として、N-1番目のレーザビームの仰角θN-1が示されている。uv面よりも上方に向かうレーザビームの仰角を「正の仰角」とし、uv面よりも下方に向かうレーザビームの仰角を「負の仰角」とする。
Nが1であるLiDARセンサを「2次元LiDAR」と称し、Nが2以上であるLiDARセンサを「3次元LiDAR」と称する場合がある。Nが2以上の場合、1番目のレーザビームとN番目のレーザビームとのなす角度を、「垂直視野角」と称する。垂直視野角は、例えば20°から60°程度の範囲内に設定され得る。
LiDARセンサ110は、図9Bに示すように、レーザビームの出射方向(方位角)を変化させることができる。図9Bには、図9Aに示す複数のレーザビームの出射方向が、w軸に平行な回転軸の周りに回転している様子が示されている。レーザビームの出射方向(方位角)の範囲は360°であってもよいし、360°よりも小さい角度の範囲(例えば210°または270°等)であってもよい。レーザビームの出射方向の方位角の範囲を「水平視野角」と称する。水平視野角は、例えば90°から360°程度の範囲内に設定され得る。LiDARセンサ110は、レーザビームの出射方向をw軸に平行な回転軸の周りに回転させながら、異なる方位角の方向にパルス状のレーザ光(レーザパルス)を順次出射する。こうして、異なる仰角および異なる方位角に出射されるパルス状のレーザ光により、各反射点までの距離を計測することが可能になる。各反射点は、点群データに含まれる個々の点に相当する。レーザビームの方位角が回転軸の周りに一回転する間に反射点までの距離を測定する動作を1スキャンと称する。1スキャンによって得られるセンサデータは、図9Aに示す特定の仰角に対応付けられたレイヤごとに測定されるデータを含む。従って、レイヤの数が増加するほど、同一環境に対する1スキャンによって得られる点群中の点の個数は増加する。LiDARセンサ110は、例えば1秒あたり1回から20回程度の頻度でスキャン動作を繰り返す。1スキャンの動作中に例えば10万パルス以上のレーザ光が異なる方向に出射され得る。
図10は、LiDARセンサ110の概略的な構成の例を示すブロック図である。図10に示すように、LiDARセンサ110は、複数のレーザユニット111と、電気モータ114と、制御回路115と、信号処理回路116と、メモリ117とを備える。各レーザユニット111は、レーザ光源112と、光検出器113とを備える。各レーザユニット111は、レンズおよびミラーなどの光学系を含み得るが、それらの図示は省略されている。モータ114は、例えば、各レーザ光源112から出射されるレーザビームの光路上に配置されたミラーを回転させることにより、各レーザ光源112から出射されるレーザビームの方向を変化させる。
レーザ光源112は、レーザダイオードを含み、制御回路115からの指令に応答して所定の波長のパルス状のレーザビームを出射する。レーザビームの波長は、例えば近赤外線の波長域(およそ700nmから2.5μm)に含まれる波長であり得る。使用される波長は、光検出器113に使用される光電変換素子の材料に依存する。例えばシリコン(Si)を光電変換素子の材料として用いる場合、900nm前後の波長が主に使用され得る。インジウムガリウム砒素(InGaAs)を光電変換素子の材料として用いる場合、例えば1000nm以上1650nm以下の波長が使用され得る。なお、レーザビームの波長は、近赤外線の波長域に限定されない。環境光の影響が問題にならない用途(例えば夜間用等)においては、可視域(およそ400nmから700nm)に含まれる波長を使用してもよい。用途によっては紫外線の波長域を使用することも可能である。本明細書において、紫外線、可視光、および赤外線の波長域に含まれる放射全般を「光」と称する。
光検出器113は、レーザ光源112から出射され、物体で反射または散乱されたレーザパルスを検出する装置である。光検出器113は、例えばアバランシェフォトダイオード(APD)などの光電変換素子を備える。光検出器113は、受光量に応じた電気信号を出力する。
モータ114は、制御回路115からの指令に応答して各レーザ光源112から出射されるレーザビームの光路上に配置されたミラーを回転させる。これにより、レーザビームの出射方向を変化させるスキャン動作が実現される。
制御回路115は、レーザ光源112によるレーザパルスの出射、光検出器113による反射パルスの検出、およびモータ114による回転動作を制御する。制御回路115は、例えばマイクロコントローラユニット(MCU)などの、プロセッサを備える回路によって実現され得る。
信号処理回路116は、光検出器113から出力された信号に基づく演算を行う回路である。信号処理回路116は、例えばToF(Time of Flight)法によって各レーザ光源112から出射されたレーザパルスを反射した物体までの距離を計算する。ToF法には、直接ToF法と間接ToF法とがある。直接ToF法においては、レーザ光源112からレーザパルスが出射されてから光検出器113によって反射光が受光されるまでの時間を直接計測することにより、反射点までの距離を計算する。間接ToF法においては、光検出器113に複数の露光期間が設定され、各露光期間で検出された光量の比に基づいて各反射点までの距離が計算される。直接ToF法および間接ToF法のいずれの方法も用いられ得る。信号処理回路116は、例えば、各反射点までの距離と、その反射点の方向とを示すセンサデータを生成して出力する。信号処理回路116は、さらに、各反射点までの距離と、その反射点の方向とに基づいて、センサ座標系における座標(u,v)または(u,v,w)を計算し、センサデータに含めて出力してもよい。
制御回路115および信号処理回路116は、図10の例では2つの回路に分かれているが、1つの回路によって実現されていてもよい。
メモリ117は、制御回路115および信号処理回路116によって生成されるデータを記憶する記憶媒体である。メモリ117は、例えば、各レーザユニット111から出射されるレーザパルスの出射タイミングと、出射方向と、反射光強度と、反射点までの距離と、センサ座標系における座標(u,v)または(u,v,w)とを関連付けたデータを記憶する。そのようなデータが、レーザパルスが出射される度に生成され、メモリ117に記録される。制御回路115は、所定の周期(例えば、所定回数のパルス出射に要する時間、半スキャン周期、または1スキャン周期等)で当該データを出力する。出力されたデータは、トラクタ100Aの記憶装置150に記録される。
なお、距離計測の方法はToF法に限らず、FMCW(Frequency Modulated Continuous Wave)法などの他の方法を用いてもよい。FMCW法においては、周波数を線形的に変化させた光が出射され、出射光と反射光との干渉で生じるビートの周波数に基づいて距離が計算される。
<動作>
次に、トラクタ100Aの動作を説明する。
次に、トラクタ100Aの動作を説明する。
図11は、トラクタ100Aが走行する環境の一例を模式的に示す図である。この例では、トラクタ100Aは、ぶどう園における複数の樹木列20の間を走行しながら、作業機300を用いて所定の作業(例えば、草刈り、防除、その他)を行う。ぶどう園では、上空が枝および葉で遮られ、GNSSを用いた自動走行が難しい。また、ぶどう園では、樹木の葉または垣根の外形が季節によって大きく変化する。そのため、従来のSLAM技術によって作成された点群地図を通期で使用することが難しい。
そこで、本開示の実施形態では、ぶどう園などの果樹園に適した環境地図を用意し、その環境地図に基づいてトラクタ100Aの自動走行を行う。例えばぶどう園における樹木は、一度植えられると長期間植え替えられることがなく、幹部は葉部に比べて季節変化による外形の変化が少ない。幹部をSLAMのランドマークとすることで、通期で使用可能な環境地図を作成し、年間を通じて環境地図を作成し直すことなく自動走行作業を行うことができる。
以下、トラクタ100Aの動作を具体的に説明する。ECU160は、LiDARセンサ110から出力されるセンサデータを利用して、ぶどう園全体またはぶどう園の一区画における樹木列20の幹部の分布を示す環境地図データをまず生成し、記憶装置150に記録する。環境地図データの生成は、自己位置推定モジュール162による自己位置推定と、地図データ生成モジュール164による局所地図データの生成とを繰り返すことによって実行される。地図データ生成モジュール164は、トラクタ100Aが移動している間にLiDARセンサ110から繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、トラクタ100Aの周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す局所地図データを生成して記憶装置150に記録する動作を繰り返す。地図データ生成モジュール164は、トラクタ100Aがぶどう園全体またはぶどう園の一区画を走行する間に生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成する。この環境地図データは、環境中の樹木列20の幹部の分布を、他の物体と区別可能な形式で記録したデータである。このように、本実施形態では、樹木列20の幹部がSLAMのランドマークとして使用される。
複数の樹木列が配置されたエリアには1つ以上の支柱が設けられることがある。例えば、ぶどう園においては、一般に、垣根仕立てのための複数の支柱が樹木の近傍に設けられる。支柱は、樹木の幹部と同様、季節による外形の変化が少ないため、ランドマークとして適している。そこで、自己位置推定モジュール162は、センサデータに基づいて、トラクタ100Aの周囲の環境における支柱をさらに検出し、地図データ生成モジュール164は、幹部の分布に加えて支柱の分布も示す環境地図データを生成してもよい。
環境地図データが生成されると、トラクタ100Aによる自動走行または自律走行が可能になる。ECU160の自己位置推定モジュール162は、トラクタ100Aの走行中、LiDARセンサ110から繰り返し出力されるセンサデータに基づいて、周囲の環境における樹木列20の幹部を検出し、検出した樹木列20の幹部と環境地図データとのマッチングを行うことにより、トラクタ100Aの位置を推定する。環境地図データに支柱の分布情報が含まれる場合は、自己位置推定モジュール162は、センサデータに基づいて検出した樹木列の幹部および支柱と、環境地図データとのマッチングを行うことにより、トラクタ100Aの位置を推定してもよい。駆動制御用のECU180は、ECU160によって推定されたトラクタ100Aの位置に応じてトラクタ100Aの移動を制御する制御装置である。例えば、ECU180は、ECU170によって決定された走行予定経路から逸脱している場合に、トラクタ100Aのステアリングを調整することにより、走行予定経路に近付ける制御を行う。このような操舵制御は、トラクタ100Aの位置だけでなく向きに基づいて行われてもよい。
樹木列20の幹部の分布を示す環境地図は、任意の時期に生成することができる。例えば、冬などの樹木の葉部が少ない季節に環境地図を生成することができる。その場合、夏などの樹木の葉部が多い季節に環境地図を生成する場合と比較して、幹部の分布をより正確に反映した環境地図を生成し易い。冬に環境地図が生成される場合、冬だけでなくそれ以外の季節においても、ECU160は、その環境地図に基づいて自己位置推定を行ってもよい。その場合、ECU160は、冬の間に生成された環境地図データと、LiDARセンサ110から出力されたセンサデータから幹部以外の部分を除去したデータとのマッチングを行うことにより、自己位置推定を行う。
ECU160は、トラクタ100Aの走行中に繰り返し取得されたセンサデータに基づいて検出した樹木列の幹部の分布を示すデータを生成し、当該データを用いて環境地図データを更新してもよい。例えば、ECU160は、新たに取得されたセンサデータから検出された幹部の情報を前回生成した環境地図データに加えることによって環境地図データを更新してもよい。あるいは、検出した樹木列の幹部の分布を示すデータを、環境地図データを更新する外部の装置に送信してもよい。その場合、ECU160は、推定されたトラクタ100Aの位置を示す情報を当該幹部の分布を示すデータ(例えば局所地図データ)に含めて出力する。外部の装置は、取得したデータを用いて環境地図データを更新し、更新後の環境地図データをトラクタ100Aに送信することができる。そのような動作により、樹木の生長などに起因して、環境地図を作成した時点から走行環境が変化した場合であっても、その変化を反映した環境地図を新たに作り直すことができる。
図12は、トラクタ100Aの周囲の環境の例を模式的に示す斜視図である。トラクタ100Aは、ぶどう園内の隣り合う2つの樹木列の間を走行する。トラクタ100Aは、走行しながら、LiDARセンサ110を用いて周囲の環境をレーザビームでスキャンする。これにより、環境内に存在する物体の距離分布を示すデータを取得する。距離分布を示すデータは、2次元または3次元の点群データに変換され、記録される。トラクタ100Aは、ぶどう園内を走行しながら繰り返し取得した点群データを統合することにより、ぶどう園全体、またはぶどう園の一区画における樹木列の幹部の分布を示す環境地図データを生成する。点群データの統合は、異なるタイミングで取得された複数の反射点の座標を、地球に固定されたワールド座標系における座標に変換し、繋ぎ合わせる処理を含む。本実施形態では、トラクタ100AのECU160が環境地図データを生成するが、トラクタ100Aの外部に設けられたコンピュータが環境地図データを生成してもよい。
図13Aは、環境地図生成時、および自動走行時のトラクタ100Aの走行経路の一例を模式的に示す図である。トラクタ100Aは、例えば図13Aに矢印で示す経路30に沿ってぶどう園内の複数の樹木列20の間を走行する。図では、経路30に含まれる線分が直線状に記載されているが、トラクタ100Aが実際に走行する経路は、蛇行部分を含んでいてもよい。ここで、複数の樹木列20を、端から順に第1の樹木列20A、第2の樹木列20B、第3の樹木列20C、第4の樹木列20D、・・・と順序付けする。図13Aの例では、トラクタ100Aは、第1の樹木列20Aと第2の樹木列20Bとの間をまず走行し、その走行が完了すると、旋回して第2の樹木列20Bと第3の樹木列20Cとの間を逆向きに走行する。第2の樹木列20Bと第3の樹木列20Cとの間の走行が完了すると、さらに旋回して第3の樹木列20Cと第4の樹木列20Dとの間を走行する。以後、同様の動作を繰り返すことにより、最後の2つの樹木列の間の経路30の終端まで走行する。なお、隣り合う樹木列の間の距離が短い場合は、図13Bに示すように、1列おきに走行してもよい。この場合、最後の2つの樹木列の間の走行が完了した後、まだ走行していない樹木列間を1列おきに走行する動作が実行され得る。このような走行は、トラクタ100AがLiDARセンサ110を利用して自己位置推定を行いながら、自動で行う。なお、GNSSユニット120がGNSS信号を受信できるタイミングにおいては、GNSS信号に基づいて測位を行ってもよい。例えば、図13Aおよび図13Bに示す経路30において方向転換するタイミングでは、GNSS信号を遮る葉が存在しないことから、GNSS信号に基づく測位が可能である。
<センサデータ取得の例>
次に、図14Aから図17Cを参照して、トラクタ100Aによるセンサデータの取得の例を説明する。センサデータは、環境地図が既に構築済みの場合は、環境地図とのマッチングを行うことによってトラクタ100Aの位置を推定するために利用される。一方、環境地図が構築されていない場合、あるいは環境地図の少なくとも一部を更新する場合には、センサデータに基づいて環境地図の構築または更新が行われる。環境地図の構築および更新は、トラクタ100Aの移動中に行われる必要はなく、トラクタ100Aが移動中に取得したセンサデータをトラクタ100Aの内部または外部のコンピュータによって処理することで実行され得る。
次に、図14Aから図17Cを参照して、トラクタ100Aによるセンサデータの取得の例を説明する。センサデータは、環境地図が既に構築済みの場合は、環境地図とのマッチングを行うことによってトラクタ100Aの位置を推定するために利用される。一方、環境地図が構築されていない場合、あるいは環境地図の少なくとも一部を更新する場合には、センサデータに基づいて環境地図の構築または更新が行われる。環境地図の構築および更新は、トラクタ100Aの移動中に行われる必要はなく、トラクタ100Aが移動中に取得したセンサデータをトラクタ100Aの内部または外部のコンピュータによって処理することで実行され得る。
図14Aは、トラクタ100AのLiDARセンサ110から出射されたレーザビームで樹木21および地面が照射される様子を模式的に示す図である。図14Bは、LiDARセンサ110から出射されたレーザビームで地面が照射される様子を模式的に示す図である。これらの図において、LiDARセンサ110から負の仰角の方向に出射されたレーザビームは太い破線で表され、LiDARセンサ110から正の仰角の方向に出射されたレーザビームは細い破線で表されている。LiDARセンサ110から異なる仰角で出射される複数のレーザビームのうち、負の仰角の方向に出射されたレーザビームは、樹木21に当たらない場合、地面で反射され得る。一方、正の仰角の方向に出射されたレーザビームは、樹木21に当たらない場合、反射点を形成せず、樹木21に当たった場合の反射点は、葉や枝の表面に位置することが多い。前述したように、葉や枝の表面に位置する反射点は、環境地図データとのマッチングに適さない。このため、本実施形態では、幹部での反射を利用して自己位置推定および環境地図の構築を行う。具体的には、LiDARセンサ110から異なる仰角で出射される複数のレーザビームのうち、仰角が所定の範囲に含まれるレーザ光源から出射されたレーザパルスの反射点に基づいて幹部を検出してもよい。また、反射点の高さが幹部の平均的な高さよりも低い反射点に基づいて幹部を検出してもよい。これらの場合、幹部の検出後に、地面に位置する反射点をセンサデータの点群から選択的にカットしてもよい。
図15Aおよび図15Bは、トラクタ100AのLiDARセンサ110による1スキャン動作で取得されるセンサデータの点群の例を模式的に示す平面図である。図15Aは、ある時点におけるトラクタ100Aと、LiDARセンサ110から放射されたレーザパルスの反射点の分布の例を模式的に示している。この例では、地面は平坦な面であると仮定している。地面で反射されたレーザビームは、地面で円弧上に反射点を形成する。このような地面上の反射点は、仰角が負の方向に出射されたレーザビームによって形成される。図15Aにおいて、半径が異なる7本の円弧(7つのレイヤに相当)上に反射点が並んでいる。それぞれのレイヤは、異なる負の仰角で出射されたレーザビームが方位角方向に回転するスキャンによって形成される。このスキャンが行われているとき、水平方向または正の仰角の方向にレーザビームによるスキャンが行われていてもよい。なお、同一レイヤ上に並ぶ反射点は、同一のレーザ光源から同一の仰角で出射されたレーザビームが回転しながら物体表面にパルス状に入射して形成される。図15Bは、図15Aの時点からわずかに時間が経過した時点におけるトラクタ100Aと、LiDARセンサ110から放射されたレーザパルスの反射点の分布の例を模式的に示している。
LiDARセンサ110から放射されたレーザパルスの一部は、樹木の幹部22の表面で反射される。地面、樹木の幹部22、または他の物体で反射されたレーザパルスの一部は、反射点が測距可能距離(例えば50m、100m、または200m等)を超える遠方に位置する場合を除き、LiDARセンサ110によって検出され、反射点までの距離が計測される。LiDARセンサ110は、例えば、反射点ごとに、反射点までの距離と、反射点の方向と、反射光強度と、反射点を形成したレーザビームを出射したレーザ光源の識別番号とを関連付けたセンサデータを生成して出力する。このセンサデータには、計測時刻の情報も含まれ得る。計測時刻の情報は、例えば反射点ごと、または一定の時間内に計測された反射点のまとまりごとに記録され得る。ECU160の自己位置推定モジュール162は、LiDARセンサ110から出力されたセンサデータを点群データに変換する。点群データは、LiDARセンサ110に固定されたセンサ座標系で表現された各反射点の3次元座標(u,v,w)の情報を含むデータである。なお、LiDARセンサ110が各反射点の距離および方向のデータを点群データに変換した上で出力する場合には、自己位置推定モジュール162による点群データへの変換は省略される。
環境地図の構築時において、トラクタ100Aが走行している間、地図データ生成モジュール164は、点群データから、幹部22を他の部分と区別可能な形式で記録した局所地図データを生成する。トラクタ100Aの走行中、地図データ生成モジュール164は、それまでに生成した局所地図データに、新たに取得したセンサデータに基づく局所地図データを追加して更新する動作を繰り返す。これにより、最終的な環境地図データを生成することができる。なお、地図データ生成モジュール164は、トラクタ100Aの走行中には局所地図データを生成して記録する動作のみを行い、最終的な環境地図データの生成は、走行完了後に行ってもよい。その場合、最終的な環境地図データは、外部のコンピュータが生成してもよい。
本実施形態では、LiDARセンサ110がトラクタ100Aの車体の前部に取り付けられている。したがって、図15Aおよび図15Bに示すように、レーザパルスは、トラクタ100Aの前方に向けて放射される。トラクタ100Aの後部にもLiDARセンサ110が取り付けられていてもよい。その場合には、トラクタ100Aの前方と後方の両方の点群データを一度に取得できるため、より点群密度の高い地図データを生成することができる。また、環境地図が生成された後の自動走行におけるマッチングによる自己位置推定の精度を向上させることができる。さらに、個々の幹部22の表面における広い範囲から点群データが得られるため、その点群データから幹部22の太さを高い精度で計測することもできる。このように、ECU160は、センサデータに基づいて、検出された樹木列の各幹部の太さを計測し、各幹部の太さを記憶媒体に記録する記録装置としても機能する。記憶媒体は、記憶装置150内の記憶媒体でもよいし、他の記憶媒体でもよい。幹部の太さは、各ランドマークの属性情報のひとつとして環境地図データに加えられてもよいし、他のベクタ形式の地図データに記録されてもよい。
図16は、複数のレイヤ(L1、・・・、Lk+3)のレーザビームの1スキャンによって得られる幹部22の点群の例を模式的に示す図である。図17Aから図17Cは、それぞれ、図16に示す複数のレイヤのうちの3つのレイヤ(Lk、Lk+1、Lk+2)のレーザビームのスキャンによって幹部22の点群が得られる様子を模式的に示す図である。この図に示すように、スキャン毎に、計測可能な範囲に存在する幹部22の分布を示す点群データを得ることができる。より詳細には、1スキャンによって個々の反射点までの距離を計測するとき、それぞれのレイヤにおいて、距離が相対的に長い反射点が連続する領域と、距離が相対的に短い反射点が局所的に表れる領域とが含まれ得る。
図18は、複数のレイヤ(Lk、Lk+1、Lk+2、・・・)のレーザビームによって1スキャンで計測される反射点までの距離と反射点の方位角との関係の例を示すグラフである。図18に示される方位角の範囲は狭く、この範囲では2本の幹部からの反射が示されている。この例において、各レイヤの主たる反射点は地面に位置している。個々のレイヤに属する反射点のうち、相対的に距離が短い反射点は、幹部の表面に対応する曲線上に並んでいる。2つの幹部のうち、距離が相対的に短い位置にある幹部は、レイヤLk、Lk+1、Lk+2、・・・のレーザビームで照射される。これに対し、距離が相対的に長い位置にある幹部は、より少ないレイヤLk+1、Lk+2、・・・のレーザビームで照射される。このため、3次元空間中の幹部の表面における点群を水平面に平行な平面に投影した場合、距離が相対的に短い位置にある幹部の点群の密度が相対的に高い。図18に示される計測結果に基づいて、幹部を検出することができる。実際には地面には起伏が存在したり、雑草が存在したりするため、距離の計測値は図示されるほど単純ではない。
トラクタ100Aが移動しながら上記のスキャンを繰り返すことにより、トラクタ100Aの周囲に位置する各幹部から密度の高い点群のデータを取得することが可能である。したがって、移動するトラクタ100Aが取得する大量のセンサデータを利用することにより、幹部の分布を示す精度の高い環境地図の構築に必要な高密度の点群データを取得することができる。
なお、反射点までの距離ではなく、反射光の強度と反射点の方位角との関係から幹部を検出することもできる。同一レイヤのレーザビームによる反射光の強度は、反射点までの距離が短いほど大きくなるからである。
上述したように、幹部の分布を示す環境地図と比較されるべきセンサデータ(以下、「スキャンデータ」とも称する。)には、幹部の表面に位置する反射点以外の多数の反射点が含まれ得る。このため、取得されたスキャンデータから、幹部の表面に位置する可能性が高い反射点を選択することが有効である。また、スキャンデータは、1スキャンによって得られた反射点だけではなく、連続的に取得された複数のスキャンによって得られた反射点を統合して密度の高い点群データを生成してもよい。
なお、図17Aから図17Cでは省略されているが、例えば正の仰角で出射されるレーザビームは、幹部以外の葉部や枝部を照射して多数の反射点を形成し得る。本実施形態におけるECU160は、スキャンデータに基づいて生成した点群データから、幹部22の特徴を有する一部の点を抽出することにより、幹部22の分布を示すデータを生成する。
図19は、ある位置におけるトラクタ100Aで取得されたスキャンデータから、幹部の表面に位置する反射点を抽出して作成した局所地図の例を示している。この例では、幹部の2次元分布を示す2次元の局所地図が作成されている。このような局所地図では、その位置におけるトラクタ100Aの周囲に位置する幹部の座標が、トラクタ100Aに固定されたuv座標系で表現され得る。
図20Aおよび図20Bは、図19の局所地図を、構築済みの環境地図とマッチングさせることにより、トラクタ100Aの位置を推定するプロセスを模式的に示す平面図である。局所地図における各座標はセンサ座標系で表現され、環境地図における各座標はワールド座標系で表現されている。マッチングは、センサ座標系からワールド座標系への座標変換を最適化することである。そのような座標変換の最適化により、ワールド座標系におけるセンサの位置座標および向きが決定される。図20Aおよび図20Bにおいて、環境地図における幹部は黒塗りで表現されている。図20Aは、マッチングの途中の状態を示している。この状態では、局所地図上にある幹部と環境地図上にある対応する幹部との間には位置ずれ(誤差)が存在する。図20Bは、マッチングが完了した状態を示している。この状態では、局所地図上にある幹部と環境地図上にある対応する幹部との間の位置ずれ(誤差)は最小化されている。
このように幹部を抽出したスキャンデータと幹部の分布を示す環境地図を利用することにより、精度の高い位置推定が実現する。なお、上記の例では、2次元の環境地図に基づくマッチングが行われるが、3次元の環境地図に基づいてマッチングを行ってもよい。
<LiDARセンサの配置の例>
次に、図21Aおよび図21Bを参照して、LiDARセンサ110の配置の例を説明する。
次に、図21Aおよび図21Bを参照して、LiDARセンサ110の配置の例を説明する。
図21Aは、本実施形態のように、トラクタ100Aの車両本体の前方下部にLiDARセンサ110が取り付けられた例を示している。図21Bは、トラクタ100Aのキャビンの前方上部にLiDARセンサ110が取り付けられた例を示している。これらの図において、太い破線は樹木21の幹部に照射されるレーザビームを示しており、細い破線は樹木の葉部またはその上空に向けて出射されるレーザビームを示している。
図21Aの例において、LiDARセンサ110は、樹木列の幹部の平均高さよりも低い位置に搭載されている。樹木列の幹部の平均高さは、トラクタ100Aの走行予定経路に存在する複数の樹木列に含まれる樹木の幹部の高さの平均値である。樹木の幹部の高さは、地面から、その樹木において最も低い枝を生ずる部分までの距離である。LiDARセンサ110は、例えば地上から10cm以上150cm以下、ある例では15cm以上100cm以下の高さに配置され得る。このような低い位置にLiDARセンサ110を配置することにより、葉や枝によって遮られることなく、より多くのレーザビームで幹部を照射することができる。このため、センサデータから幹部の位置を高い精度で決定することが可能になる。
一方、図21Bの例では、LiDARセンサ110が高い位置(例えば地上から約2mの位置)に配置されていることから、レーザビームの多くは、幹部ではなく葉部を照射する。このため、幹部の点群データを取得する用途においては、図21Bのような配置は適さない。しかし、このような配置は、樹木列の葉部の分布情報を効率的に取得できるため、葉部の分布情報を積極的に取得する用途においては有効である。例えば、樹木の生育状況を管理したり、樹木の葉部を避ける経路を決定したりする目的のために図21Bのような配置が採用されてもよい。なお、図21Bのような配置においても、LiDARセンサ110を下向きに取り付けることにより、より多くのレーザビームで幹部を照射することができる。このため、幹部の分布データを取得する用途においても図21Bに示すような配置が採用される場合もある。LiDARセンサ110は、車体の前方下部およびキャビンの前方上部に限らず、他の位置に配置されていてもよい。例えば、LiDARセンサ110は、キャビンの高さよりも低い位置に配置され得る。
なお、本実施形態では1つのLiDARセンサ110が使用されるが、トラクタ100Aは複数のLiDARセンサを備えていてもよい。複数のLiDARセンサから出力されるセンサデータを組み合わせることにより、さらに効率的に幹部の分布データを取得することができる。複数のLiDARセンサは、例えばトラクタ100Aの左右に設けられていてもよいし、前部および後部に設けられていてもよい。複数のLiDARセンサを備えた移動体の実施形態の詳細については後述する。
ECU160における自己位置推定モジュール162は、LiDARセンサ110における複数のレーザ光源のうち、仰角が所定の範囲に含まれるレーザ光源から出射されたレーザパルスの反射点に基づいて幹部を検出してもよい。例えば、負の仰角の方向に出射されるレーザパルスの反射点のみに基づいて幹部を検出してもよい。幹検出に利用されるレーザパルスの仰角の範囲をユーザが設定できるようにしてもよい。例えば、ユーザが操作端末200を操作することによって幹検出に利用されるレーザパルスの仰角の範囲を設定できるようにしてもよい。LiDARセンサ110から出射されるレーザパルスのうち、幹部を照射する可能性の高い特定の仰角の範囲に出射されるレーザパルスのみを利用することにより、幹部をより効率的に検出することができる。また、地面に起伏があり、センサ座標系におけるuv面が水平面から大きく傾斜するときは、センサ座標系の傾斜角に応じて、幹部で反射されたレーザパルスを適切に選択するように、レーザビームの仰角範囲を適応的に選択して反射点を抽出してもよい。LiDARセンサ110の傾斜角は、IMU125からの信号を利用して求めることができる。
<環境地図構築後の自動走行動作の例>
次に、環境地図が作成された後の自動走行動作の例を説明する。
次に、環境地図が作成された後の自動走行動作の例を説明する。
図22は、ECU160における自己位置推定モジュール162の機能構成の例を示す図である。自己位置推定モジュール162は、記憶装置150に格納されたコンピュータプログラム(ソフトウェア)を実行することにより、LiDARセンサ110から出力されたスキャンデータと地図データとのマッチングを行い、トラクタ100Aの位置および向きを推定する。
自己位置推定モジュール162は、スキャンデータ取得162a、地図データ取得162b、IMUデータ取得162c、スキャンデータフィルタリング162d、マッチング162e、および車両位置・向き決定162fの各処理を実行する。以下、これらの処理の詳細を説明する。
(スキャンデータ取得162a)
自己位置推定モジュール162は、LiDARセンサ110から出力されたスキャンデータを取得する。LiDARセンサ110は、例えば毎秒1回から20回程度の頻度で、スキャンデータを出力する。このスキャンデータは、センサ座標系で表現された複数の点の座標と、タイムスタンプの情報を含み得る。なお、スキャンデータが各点までの距離および方向の情報を含み、座標情報を含まない場合は、自己位置推定モジュール162が距離および方向の情報から座標情報への変換を行う。
自己位置推定モジュール162は、LiDARセンサ110から出力されたスキャンデータを取得する。LiDARセンサ110は、例えば毎秒1回から20回程度の頻度で、スキャンデータを出力する。このスキャンデータは、センサ座標系で表現された複数の点の座標と、タイムスタンプの情報を含み得る。なお、スキャンデータが各点までの距離および方向の情報を含み、座標情報を含まない場合は、自己位置推定モジュール162が距離および方向の情報から座標情報への変換を行う。
(地図データ取得162b)
自己位置推定モジュール162は、記憶装置150に格納された環境地図データを取得する。環境地図データは、トラクタ100Aが走行する環境内に含まれる樹木列の幹部の分布を示す。環境地図データは、例えば以下の(1)~(3)のいずれかの形式のデータを含む。
(1)幹部と幹部以外の物体とが識別可能な形式で記録されたデータ
例えば、幹部であると判断された点に数値「1」が付与され、幹部以外の物体であると判断された点に数値「0」が付与されたデータが該当する。個々の幹部を識別するための幹IDが環境地図データに含まれていてもよい。
(2)幹部に相対的に大きい重みが付与され、幹部以外の物体に相対的に小さい重みが付与されたデータ
例えば、幹部の表面上の点であると推定される確率が高い点ほど大きい数値が付与されるデータが該当する。
(3)検出された幹部の分布の情報を含み、幹部以外の物体の一部または全部の分布の情報を含まないデータ
自己位置推定モジュール162は、記憶装置150に格納された環境地図データを取得する。環境地図データは、トラクタ100Aが走行する環境内に含まれる樹木列の幹部の分布を示す。環境地図データは、例えば以下の(1)~(3)のいずれかの形式のデータを含む。
(1)幹部と幹部以外の物体とが識別可能な形式で記録されたデータ
例えば、幹部であると判断された点に数値「1」が付与され、幹部以外の物体であると判断された点に数値「0」が付与されたデータが該当する。個々の幹部を識別するための幹IDが環境地図データに含まれていてもよい。
(2)幹部に相対的に大きい重みが付与され、幹部以外の物体に相対的に小さい重みが付与されたデータ
例えば、幹部の表面上の点であると推定される確率が高い点ほど大きい数値が付与されるデータが該当する。
(3)検出された幹部の分布の情報を含み、幹部以外の物体の一部または全部の分布の情報を含まないデータ
例えば、環境地図を表す点群から、幹部の表面であると判断された点のみを残して他の点を削除したデータが該当する。幹部ではないと判断された点の全てが削除されるのではなく、一部の点が残されていてもよい。例えば、ぶどう園では一般に、垣根仕立てのための支柱が幹部の周囲に設けられるため、そのような支柱を表す点の情報が環境地図に含まれていてもよい。
点群中のある点が幹部または支柱に該当するか否かは、例えば、その点およびその点の周囲の複数の点の分布が、幹部または支柱の表面形状を反映した分布(例えば下に凸の円弧状の分布等)であるか否かに基づいて判断され得る。あるいは、各回のスキャンで取得された曲線的に分布する点群のデータから、近傍の点に比べてLiDARセンサ110からの距離が局所的に近い点の集まりを抽出し、それらの点を幹部または支柱を表す点であると判断してもよい。各回のスキャンで取得された曲線的に分布する点群のデータを、LiDARセンサ110からの距離に応じて複数のクラスに分類し、クラスごとに幹部または支柱に該当するか否かを判断してもよい。また、距離だけでなく反射強度の情報にも基づいて点群を分類してもよい。樹木の幹とその周囲の他の部分とでは、反射強度が明確に異なるため、反射強度の類似性と位置の類似性とに基づいて点群を分類することが有効である。例えば、反射強度が所定の範囲内にあり、かつ位置が近接する複数の点を幹部の表面を表す点の候補としてもよい。LiDARセンサ110から複数の異なる波長のレーザビームを出射し、波長ごとの反射強度の比に基づいて点群を分類し、幹部を検出してもよい。
幹部の検出には、機械学習が利用されてもよい。深層学習などのニューラルネットワークに基づく機械学習アルゴリズムを利用することにより、点群データから樹木の幹の表面に該当する点を高い精度で検出することができる。機械学習アルゴリズムが用いられる場合、点群データから幹部を検出するための学習済みモデルの作成(すなわち学習)が事前に行われる。
樹木の幹部に限らず、それ以外の物体を認識してもよい。例えばパターン認識または機械学習によって点群データから地面、雑草、および樹木の葉部等を認識してそれらの点を除去することにより、樹木の幹部に該当する点を主に含む点群データを生成してもよい。幹部の検出に先立って、地面からの高さが所定の範囲(例えば、0.5mから1.5m等)に含まれる点群のみを抽出する処理を行ってもよい。そのような特定の座標範囲に含まれる点群のみを幹部検出の対象とすることにより、検出に要する時間を短縮することができる。地面からの高さは、各点のZ座標から地面のZ座標を減じることによって計算される。地面のZ座標は、例えば、数値標高モデル(Digital Elevation Model:DEM)を参照して決定され得る。地面のZ座標は、地面を示す点群から決定されてもよい。
図23は、環境地図データの形式の一例を模式的に示す図である。この例における環境地図データは、点の個数の情報と、点ごとのID、分類、幹ID、X座標、Y座標、Z座標、および反射強度の情報とを含む。点の個数をnとすると、点ID、分類、幹ID、X座標、Y座標、Z座標、および反射強度のデータの組がn組分記録される。分類は、その点が何を表すかを示し、例えば樹木の幹、垣根の支柱、地面、雑草、樹木の葉などを示す識別子を含む。分類は、樹木の幹であるか否かを示す2値の数値、または、樹木の幹である確率を示す数値であってもよい。幹IDは、その点が幹を表す場合に、その幹を識別するための番号である。幹IDは環境地図データに含まれていなくてもよい。また、幹IDに加えて、他の物体(例えば、葉、枝、地面、雑草等)の識別子が含まれていてもよい。X座標、Y座標、およびZ座標は、ワールド座標系におけるその点の座標である。反射強度は、その点からの反射光強度を表す。反射強度の情報は環境地図データに含まれていなくてもよい。環境地図データは、図23に示す情報以外にも、データ形式を示すヘッダなどの情報を含み得る。トラクタ100Aが走行するぶどう園が広い場合には、ぶどう園の区画ごと(例えば、100m×100mの区画ごと)に環境地図データが生成され得る。その場合には、環境地図データは、区画を識別するためのID情報も含み得る。なお、環境地図データは、各点の3次元座標を記録する代わりに、2次元座標を記録してもよい。例えば、水平面に平行な特定の高さの平面上の点群の座標を含むデータが環境地図データとして生成されてもよい。その場合、スキャンデータと環境地図データとのマッチングは、その2次元座標に関して行われる。
(IMUデータ取得162c)
自己位置推定モジュール162は、IMU125から出力されたIMUデータを取得する。IMUデータは、トラクタ100Aの加速度、速度、変位、姿勢、計測時刻(タイムスタンプ)などの情報を含み得る。IMUデータは、例えば毎秒数十回から数千回程度の頻度で出力される。この出力周期は、一般に、LiDARセンサ110によるスキャンデータの出力周期よりも短い。
自己位置推定モジュール162は、IMU125から出力されたIMUデータを取得する。IMUデータは、トラクタ100Aの加速度、速度、変位、姿勢、計測時刻(タイムスタンプ)などの情報を含み得る。IMUデータは、例えば毎秒数十回から数千回程度の頻度で出力される。この出力周期は、一般に、LiDARセンサ110によるスキャンデータの出力周期よりも短い。
図24は、IMUデータとスキャンデータの時間的な関係の一例を示す図である。この例では、IMUデータは1ミリ秒(ms)の周期で出力され、スキャンデータは10msの周期で出力される。1スキャンに要する時間が例えば100msであるとすると、図24の例では、10分の1スキャンごとにスキャンデータが出力される。スキャンデータには、計測された各点の距離および方向、または座標と、計測時刻(タイムスタンプ)の情報が含まれる。
自己位置推定モジュール162は、取得したスキャンデータのタイムスタンプを参照して、対応する時間中に生成されたIMUデータを取得する。
(スキャンデータフィルタリング162d)
自己位置推定モジュール162は、取得したスキャンデータをフィルタリングし、マッチングに用いる点の個数を削減する。さらに、マッチングに不要な部分を除去する。例えば、地面、雑草、樹木の葉部、および障害物などの、樹木の幹部または垣根の支柱に該当しないと判断される点を除去することができる。
自己位置推定モジュール162は、取得したスキャンデータをフィルタリングし、マッチングに用いる点の個数を削減する。さらに、マッチングに不要な部分を除去する。例えば、地面、雑草、樹木の葉部、および障害物などの、樹木の幹部または垣根の支柱に該当しないと判断される点を除去することができる。
(マッチング162e)
自己位置推定モジュール162は、フィルタされたスキャンデータと地図データとのマッチングを行う。マッチングは、例えばNDT(Normal Distribution Transform)またはICP(Iterative Closest Point)などの任意のマッチングアルゴリズムを用いて実行され得る。マッチングにより、LiDARセンサ110の位置および向きが決定される。
自己位置推定モジュール162は、フィルタされたスキャンデータと地図データとのマッチングを行う。マッチングは、例えばNDT(Normal Distribution Transform)またはICP(Iterative Closest Point)などの任意のマッチングアルゴリズムを用いて実行され得る。マッチングにより、LiDARセンサ110の位置および向きが決定される。
(車両位置・向き決定162f)
自己位置推定モジュール162は、マッチングの結果に基づき、トラクタ100Aの位置および向きを決定し、その位置および向きを示すデータを出力する。当該データは、駆動制御用のECU180に送られ、駆動装置140の制御に利用される。
自己位置推定モジュール162は、マッチングの結果に基づき、トラクタ100Aの位置および向きを決定し、その位置および向きを示すデータを出力する。当該データは、駆動制御用のECU180に送られ、駆動装置140の制御に利用される。
次に、図25を参照しながら、本実施形態における自己位置推定動作をより詳細に説明する。
図25は、自己位置推定モジュール162による自己位置推定動作を示すフローチャートである。自己位置推定モジュール162は、図25に示すステップS101からS109の動作を実行することにより、トラクタ100Aの位置および向き、すなわちポーズを推定する。以下、各ステップの動作を説明する。図25に示す動作は、例えばユーザによる操作端末200を用いた操作によって開始され得る。
(ステップS101)
自己位置推定モジュール162は、記憶装置150から環境地図データを読み込む。走行対象のぶどう園の区画ごとに異なる環境地図データが記録されている場合は、現在の地点に対応する環境地図データを読み込む。現在の地点に対応する環境地図データは、例えばユーザが操作端末200を操作することによって指定され得る。あるいは、その地点でGNSS信号を受信できる場合には、GNSSユニット120が受信したGNSS信号に基づいて現在の地点を特定し、対応する環境地図データを選択して読み込んでもよい。この例のように、トラクタ100Aの位置に対応する一部の環境地図データのみを読み込むことにより、処理を高速化することができる。なお、本ステップにおいて全ての環境地図データを一括して読み込んでもよい。
自己位置推定モジュール162は、記憶装置150から環境地図データを読み込む。走行対象のぶどう園の区画ごとに異なる環境地図データが記録されている場合は、現在の地点に対応する環境地図データを読み込む。現在の地点に対応する環境地図データは、例えばユーザが操作端末200を操作することによって指定され得る。あるいは、その地点でGNSS信号を受信できる場合には、GNSSユニット120が受信したGNSS信号に基づいて現在の地点を特定し、対応する環境地図データを選択して読み込んでもよい。この例のように、トラクタ100Aの位置に対応する一部の環境地図データのみを読み込むことにより、処理を高速化することができる。なお、本ステップにおいて全ての環境地図データを一括して読み込んでもよい。
(ステップS102)
自己位置推定モジュール162は、位置推定の開始地点を設定する。位置推定の開始地点は、その時点でのトラクタ100Aの現在位置であり、例えばユーザが操作端末200に表示された地図から特定の地点を指定することによって設定され得る。あるいは、その地点でGNSS信号を受信できる場合には、GNSSユニット120が受信したGNSS信号に基づいて開始地点を設定してもよい。
自己位置推定モジュール162は、位置推定の開始地点を設定する。位置推定の開始地点は、その時点でのトラクタ100Aの現在位置であり、例えばユーザが操作端末200に表示された地図から特定の地点を指定することによって設定され得る。あるいは、その地点でGNSS信号を受信できる場合には、GNSSユニット120が受信したGNSS信号に基づいて開始地点を設定してもよい。
(ステップS103)
開始位置が設定されると、LiDARセンサ110の動作が開始される。自己位置推定モジュール162は、LiDARセンサ110から出力されたスキャンデータを読み込む。LiDARセンサ110は、所定の周期(例えば5ミリ秒以上1秒以下)でスキャンデータを出力する。スキャンデータには、レイヤごとに、例えば数度から360度の範囲内の点群データが含まれる。自己位置推定モジュール162は、例えばLiDARセンサ110からスキャンデータが出力される度に、そのスキャンデータを読み込む。あるいは、自己位置推定モジュール162は、スキャンデータが所定回数分出力される度に、それらのスキャンデータをまとめて読み込んでもよい。
開始位置が設定されると、LiDARセンサ110の動作が開始される。自己位置推定モジュール162は、LiDARセンサ110から出力されたスキャンデータを読み込む。LiDARセンサ110は、所定の周期(例えば5ミリ秒以上1秒以下)でスキャンデータを出力する。スキャンデータには、レイヤごとに、例えば数度から360度の範囲内の点群データが含まれる。自己位置推定モジュール162は、例えばLiDARセンサ110からスキャンデータが出力される度に、そのスキャンデータを読み込む。あるいは、自己位置推定モジュール162は、スキャンデータが所定回数分出力される度に、それらのスキャンデータをまとめて読み込んでもよい。
(ステップS104)
自己位置推定モジュール162は、スキャンデータに含まれるタイムスタンプを参照して、そのスキャンデータに対応するIMUデータを読み込む。
自己位置推定モジュール162は、スキャンデータに含まれるタイムスタンプを参照して、そのスキャンデータに対応するIMUデータを読み込む。
(ステップS105)
自己位置推定モジュール162は、読み込んだIMUデータに基づき、マッチングの初期位置を設定する。マッチングの初期位置は、IMUデータが示す現時点でのトラクタ100Aの推定位置である。この初期位置からマッチングを開始することにより、収束までの時間を短くすることができる。IMUデータを用いる代わりに、例えば、過去2スキャン分の位置および向きの推定値の差分に基づいて、線形補間によってマッチングの初期値を決定してもよい。
自己位置推定モジュール162は、読み込んだIMUデータに基づき、マッチングの初期位置を設定する。マッチングの初期位置は、IMUデータが示す現時点でのトラクタ100Aの推定位置である。この初期位置からマッチングを開始することにより、収束までの時間を短くすることができる。IMUデータを用いる代わりに、例えば、過去2スキャン分の位置および向きの推定値の差分に基づいて、線形補間によってマッチングの初期値を決定してもよい。
(ステップS106)
自己位置推定モジュール162は、取得したスキャンデータをフィルタリングし、マッチングに用いる点の個数を削減する。さらに、前述の方法により、幹部を検出して、幹部以外の不要な部分の少なくとも一部を点群から除去する。
自己位置推定モジュール162は、取得したスキャンデータをフィルタリングし、マッチングに用いる点の個数を削減する。さらに、前述の方法により、幹部を検出して、幹部以外の不要な部分の少なくとも一部を点群から除去する。
ここで、図26を参照して、スキャンデータのフィルタリングの動作の例を説明する。図26は、スキャンデータに含まれる点群の一部を模式的に示している。図26において、白丸が点群に含まれる点である。自己位置推定モジュール162は、センサ座標系における3次元空間を、一定の大きさの複数のボクセルに分割する。そして、個々のボクセルに複数の点が含まれる場合、それらの点を、重心に位置する1つの点に置き換える。図26において、置き換えられた重心の点が黒丸で表現されている。このようなダウンサンプリングを行うことにより、スキャンデータの点の個数を削減して、マッチングを高速化することができる。本実施形態では、このようなダウンサンプリングが行われた後、幹部の検出が行われるが、ダウンサンプリングの前に幹部の検出を行ってもよい。点群のデータサイズが問題にならない場合には、ダウンサンプリング処理を省略してもよい。
(ステップS107)
自己位置推定モジュール162は、フィルタリングされたスキャンデータと、環境地図データとのマッチングを行い、LiDARセンサ110のポーズを推定する。具体的には、NDT法またはICP法などの方法で、センサ座標系からワールド座標系への座標変換を決定することにより、LiDARセンサ110のポーズを決定する。
自己位置推定モジュール162は、フィルタリングされたスキャンデータと、環境地図データとのマッチングを行い、LiDARセンサ110のポーズを推定する。具体的には、NDT法またはICP法などの方法で、センサ座標系からワールド座標系への座標変換を決定することにより、LiDARセンサ110のポーズを決定する。
(ステップS108)
自己位置推定モジュール162は、LiDARセンサ110のポーズに基づき、トラクタ100Aのポーズを計算し、結果を出力する。トラクタ100Aのポーズは、ワールド座標系で表現されたトラクタ100Aの代表点(原点)の座標(x,y,z)および姿勢(θR,θP,θY)のデータであり得る。なお、環境地図およびスキャンデータが2次元空間(平面)における点群を表し、その2次元空間においてマッチングが行われる場合には、出力されるポーズデータは、2次元座標(x,y)および向き(θ)の値を含み得る。位置のみをマッチングで推定し、姿勢についてはIMUデータが示す姿勢の情報をそのまま利用してもよい。トラクタ100Aの座標系がセンサ座標系と一致している場合は、ステップS108は省略され得る。
自己位置推定モジュール162は、LiDARセンサ110のポーズに基づき、トラクタ100Aのポーズを計算し、結果を出力する。トラクタ100Aのポーズは、ワールド座標系で表現されたトラクタ100Aの代表点(原点)の座標(x,y,z)および姿勢(θR,θP,θY)のデータであり得る。なお、環境地図およびスキャンデータが2次元空間(平面)における点群を表し、その2次元空間においてマッチングが行われる場合には、出力されるポーズデータは、2次元座標(x,y)および向き(θ)の値を含み得る。位置のみをマッチングで推定し、姿勢についてはIMUデータが示す姿勢の情報をそのまま利用してもよい。トラクタ100Aの座標系がセンサ座標系と一致している場合は、ステップS108は省略され得る。
(ステップS109)
自己位置推定モジュール162は、動作終了の指令が出されているか否かを判断する。動作終了の指令は、例えばユーザが操作端末200を用いて自動走行モードの停止を指示したり、トラクタ100Aが目的地に到達したりした場合に出される。動作終了の指令が出されていない場合、ステップS103に戻り、次のスキャンデータについて同様の動作を実行する。動作終了の指令が出されている場合、処理を終了する。
自己位置推定モジュール162は、動作終了の指令が出されているか否かを判断する。動作終了の指令は、例えばユーザが操作端末200を用いて自動走行モードの停止を指示したり、トラクタ100Aが目的地に到達したりした場合に出される。動作終了の指令が出されていない場合、ステップS103に戻り、次のスキャンデータについて同様の動作を実行する。動作終了の指令が出されている場合、処理を終了する。
次に、駆動制御用のECU180の動作の例を説明する。
図27は、トラクタ100Aのポーズが推定された後に実行されるECU180の動作の例を示すフローチャートである。図25に示すステップS108においてトラクタ100Aのポーズが出力されると、ECU180は、そのポーズのデータを取得する(ステップS121)。次に、ECU180は、当該ポーズデータが示すトラクタ100Aの位置と、ECU170によって予め決定された走行予定経路との偏差を算出する(ステップS122)。走行予定経路は、例えばユーザが操作端末200を操作することによって設定され得る。偏差は、その時点におけるトラクタ100Aの推定された位置と、走行予定経路との距離を表す。ECU180は、算出した位置の偏差が予め設定された閾値を超えるか否かを判断する(ステップS123)。偏差が閾値を超える場合、ECU180は、偏差が小さくなるように、駆動装置140の駆動値(例えば操舵角)を変更する。操舵角に加えて速度を変更してもよい。ステップS123において偏差が閾値を超えない場合、ステップS124は省略される。続くステップS125において、ECU180は、動作終了の指令を受けたか否かを判断する。動作終了の指令は、前述のように、例えばユーザが操作端末200を用いて自動走行モードの停止を指示したり、トラクタ100Aが目的地に到達したりした場合に出される。動作終了の指令が出されていない場合、ステップS121に戻り、次のスキャンデータに基づいて推定されたトラクタ100Aのポーズに基づいて、同様の動作を実行する。動作終了の指令が出されている場合、処理を終了する。
図27の例では、ECU180は、推定された位置と走行予定経路との偏差のみに基づいて駆動装置140を制御するが、方位の偏差もさらに考慮して制御してもよい。例えば、ECU180は、取得したポーズデータが示すトラクタ100Aの推定された向きと、走行予定経路の方向との角度差である方位偏差が、予め設定された閾値を超える場合に、その偏差に応じて駆動装置140の駆動値(例えば操舵角)を変更してもよい。
以下、図28Aから図28Dを参照しながら、ECU180による操舵制御の例を説明する。
図28Aは、走行予定経路Pに沿って走行するトラクタ100Aの例を示す図である。図28Bは、走行予定経路Pから右にシフトした位置にあるトラクタ100Aの例を示す図である。図28Cは、走行予定経路Pから左にシフトした位置にあるトラクタ100Aの例を示す図である。図28Dは、走行予定経路Pに対して傾斜した方向を向いているトラクタ100Aの例を示す図である。
図28Aに示すように、トラクタ100Aの位置および向きが走行予定経路Pから外れていない場合には、ECU180は、トラクタ100Aの操舵角および速度を変更せずに維持する。
図28Bに示すように、トラクタ100Aの位置が走行予定経路Pから右側にシフトしている場合には、ECU180は、トラクタ100Aの走行方向が左寄りになるように、駆動装置140に含まれるステアリングモータの回転角を変更して操舵角を変更する。このとき、操舵角に加えて速度も併せて変更してもよい。操舵角の変化量は、例えば位置偏差Δxの大きさに応じて調整され得る。
図28Cに示すように、トラクタ100Aの位置が走行予定経路Pから左側にシフトしている場合には、ECU180は、トラクタ100Aの走行方向が右寄りになるように、ステアリングモータの回転角を変更して操舵角を変更する。この場合も、操舵角に加えて速度も併せて変更してもよい。操舵角の変化量は、例えば位置偏差Δxの大きさに応じて調整され得る。
図28Dに示すように、トラクタ100Aの位置は走行予定経路Pから大きく外れていないが、向きが走行予定経路Pの方向とは異なる場合は、ECU180は、方位偏差Δθが小さくなるように操舵角を変更する。この場合も、操舵角に加えて速度も併せて変更してもよい。操舵角の変化量は、例えば位置偏差Δxおよび方位偏差Δθの大きさに応じて調整され得る。例えば、位置偏差Δxの絶対値が小さいほど方位偏差Δθに応じた操舵角の変化量を大きくしてもよい。位置偏差Δxの絶対値が大きい場合には、走行予定経路Pに戻るために操舵角を大きく変化させることになるため、必然的に方位偏差Δθの絶対値が大きくなる。逆に、位置偏差Δxの絶対値が小さい場合には、方位偏差Δθをゼロに近づけることが必要であるため、操舵角の変化量を決定する際の方位偏差Δθの重みを相対的に大きくすることが妥当である。
トラクタ100Aの操舵制御および速度制御には、PID制御またはMPC制御(モデル予測制御)などの制御技術が適用され得る。これらの制御技術を適用することにより、トラクタ100Aを走行予定経路Pに近付ける制御を滑らかにすることができる。
なお、走行中に1つ以上の障害物センサ130によって障害物が検出された場合には、ECU180は、障害物を回避するように駆動装置140を制御する。障害物を回避できない場合には、ECU180は、トラクタ100Aを停止させる。なお、ECU180は、障害物が検出された場合に、障害物を回避できるか否かに関わらず、トラクタ100Aを停止させてもよい。
<走行予定経路の決定方法の例>
続いて、ECU170によるトラクタ100Aの走行予定経路の決定方法の例を説明する。
続いて、ECU170によるトラクタ100Aの走行予定経路の決定方法の例を説明する。
環境地図が作成された後、ECU170は、トラクタ100Aの走行予定経路を決定する。走行予定経路は、ECU170が環境地図に基づいて自動で決定してもよいし、ユーザが操作端末200を操作して設定してもよい。
図29Aは、環境地図に基づいて生成される、樹木列の幹分布を示す2次元データの例を示す図である。ECU160における地図データ生成モジュール164は、マッチング用の環境地図とは別に、図29Aに示すような、幹部22の分布を大まかに示す2次元データを生成してもよい。この2次元データは、個々の樹木の幹部22が一定の大きさの円で表現されている。このような幹分布を示す2次元データを「幹分布データ」と称することがある。幹分布データは、例えば個々の幹の中心の位置座標(x,y)の情報を含む。幹分布データは、位置座標に加えて、個々の幹の識別番号(幹ID)および幹の太さなどの情報を含んでいてもよい。そのような情報を含めることにより、幹の生育状況を管理することもできる。樹木の幹または葉などの生育状況は、例えば営農管理システム内のコンピュータによって管理され得る。この例のように、樹木の生育状況は、環境地図とは異なるデータで管理され得る。ユーザが操作端末200などのヒューマンマシンインタフェース(HMI)を用いて、樹間距離または幹部の位置座標などの、複数の樹木列の配置関係を示す情報を入力できるようにしてもよい。入力された情報は、記憶装置150または他の記憶媒体に記録される。
図29Bは、幹分布データに基づく走行予定経路30の決定方法の一例を説明するための図である。この例におけるECU170は、各樹木列の幹部22を繋いだ曲線を決定し、隣り合う2つの樹木列の中央部を通る曲線または折れ線を走行予定経路30として決定する。隣り合う2つの樹木の中央部をトラクタ100Aの中心が必ず位置する必要はなく、走行予定経路30は、隣り合う2つの樹木間の中央部からシフトしていてもよい。駆動制御用のECU180は、決定された走行予定経路30を基準に、トラクタ100Aの操舵制御を行う。
図30は、経路設定用の幹分布データの他の例を示す図である。この例では、トラクタ100Aの走行環境が複数のセルに分割されて管理される。各セルは、例えば幹部22の直径の平均値に近い辺の長さをもつ正方形であり得る。セルは、以下の(1)~(4)の4種類に分類される。
(1)幹部22が含まれるセル
(2)幹部22が含まれるセルに隣接するセル
(3)隣り合う2つの樹木列の間に位置し、(1)および(2)に該当しないセル
(4)それ以外のセル
(1)幹部22が含まれるセル
(2)幹部22が含まれるセルに隣接するセル
(3)隣り合う2つの樹木列の間に位置し、(1)および(2)に該当しないセル
(4)それ以外のセル
図30において、(1)~(4)のセルが、異なる濃さで表現されている。幹部22が含まれるセルに隣接するセル(2)の範囲は、LiDARセンサ110によって取得された樹木列の葉部を示す点群データに基づいて決定されてもよい。その場合、ECU160の地図データ生成モジュール164は、マッチング用の環境地図データとは別に、樹木列の幹部および葉部のそれぞれの2次元分布を示す格子状データを生成する。この例におけるECU170は、図30に示すような格子状データに基づいてトラクタ100Aの経路を決定する。例えば、ECU170は、隣り合う2つの樹木列の間に位置する上記(3)のセルを走行可能領域として設定する。ECU180は、走行可能領域の中央部付近を通るようにトラクタ100Aの走行を制御する。
<地図データ生成時の操舵制御>
前述のように、本実施形態におけるトラクタ100Aは、自動走行によって環境地図を作成するとき、自己位置推定と操舵制御とを行いながら、樹木列の幹部の分布を示す局所地図データを収集する。このとき、ECU160は、トラクタ100Aが移動している間にLiDARセンサ110から繰り返し出力されるセンサデータに基づいて、トラクタ100Aの周囲の環境における樹木列の葉部をさらに検出し、検出した樹木列の葉部の分布を示すデータをさらに生成してもよい。その場合、ECU180は、検出された樹木列の幹部および葉部のそれぞれの分布に基づいて、トラクタ100Aの操舵制御を行うことができる。ECU180は、例えば、検出された樹木列のうちの隣り合う2つの樹木列の間を通り、かつ樹木列の葉部との接触が低減される経路に沿ってトラクタ100Aを移動させるようにトラクタ100Aの操舵制御を行う。
前述のように、本実施形態におけるトラクタ100Aは、自動走行によって環境地図を作成するとき、自己位置推定と操舵制御とを行いながら、樹木列の幹部の分布を示す局所地図データを収集する。このとき、ECU160は、トラクタ100Aが移動している間にLiDARセンサ110から繰り返し出力されるセンサデータに基づいて、トラクタ100Aの周囲の環境における樹木列の葉部をさらに検出し、検出した樹木列の葉部の分布を示すデータをさらに生成してもよい。その場合、ECU180は、検出された樹木列の幹部および葉部のそれぞれの分布に基づいて、トラクタ100Aの操舵制御を行うことができる。ECU180は、例えば、検出された樹木列のうちの隣り合う2つの樹木列の間を通り、かつ樹木列の葉部との接触が低減される経路に沿ってトラクタ100Aを移動させるようにトラクタ100Aの操舵制御を行う。
この動作において、ECU170は、LiDARセンサ110からのセンサデータに基づいて検出された樹木列の幹部の分布から、隣り合う2つの樹木列の幹部の間を通る経路を決定する。例えば、隣り合う2つの樹木列の幹部の対の位置の中央部を通る経路を決定する。ECU180は、決定された経路に沿ってトラクタ100Aを移動させるように、トラクタ100Aの操舵制御を行う。これにより、局所地図データを収集しながら、自動走行を行うことができる。
トラクタ100Aが走行する環境において、樹木列は必ずしも直線的に配置されているとは限らない。樹木列が曲線的に配置される場合もある。そのような場合、地図データを収集しながら自動走行するときに、左右の片側にしか樹木列が検出されない場合がある。そのような場合には、左右の樹木列の対を特定できないため、上記の方法では自動走行を継続することが難しい。
そこで、樹木列が曲線的に配置されている場合には、以下の制御が行われ得る。樹木列が曲線的に配置される場合、隣り合う2つの樹木列の一方について、2つの樹木列の他方よりも少ない数の幹部が検出される。そのような場合には、ECU170は、検出された幹部の分布に基づいて、当該2つの樹木列の一方における隠れた幹部の分布を推定し、推定した分布に基づいて、トラクタ100Aの経路を決定する。ECU180は、決定された経路に沿ってトラクタ100Aが走行するようにトラクタ100Aの操舵制御を行う。
図31Aは、樹木列が曲線的に配置されている環境の一例を模式的に示す図である。図中の円柱は樹木の幹部を表し、太い破線は樹木列の概略的な配列を表し、点線はLiDARセンサ110から出射される複数のレイヤのレーザビームによるスキャンを模式的に表している。図31Bは、図31Aに示す環境において1回のスキャンで観測され得る点群を模式的に示す図である。この例においては、隣り合う2つの樹木列のうち、右側の樹木列については3本の幹部22R1、22R2、22R3が検出され、左側の樹木列については1本の幹部22L1のみが検出されている。左側の樹木列については、前方の樹木の幹部22L1によってレーザビームが遮られ、後方の樹木の幹部を検出することができない。
このような場合、ECU170は、相対的に多くの樹木が検出される右側の樹木列の配列から左側の隠れた樹木の幹部の位置を推定し、推定した幹部の位置に基づいて走行経路を決定する。例えば、スキャンデータに基づいて位置が特定される前方の隣り合う2つの幹部22R1および22L1の相対的な位置関係を、後方の隣り合う2つの幹部にも適用することにより、隠れた幹部の位置を推定する。
図31Cは、隠れた幹部の位置を推定することによって走行経路を決定する動作を説明するための図である。この例では、スキャンデータに基づいて取得された樹木列の幹部の2次元分布を示す幹分布データに基づいて、隠れた幹部の位置が推定される。図31Cには、スキャンデータに基づいて検出された樹木の幹部の位置が実線の円で示され、まだ検出されていない樹木の幹部の位置が破線の円で示されている。この例において、ECU170は、まず幹部22R1の位置から幹部22L1の位置までのベクトルV1を決定する。次に、後方の幹部22R2の位置を起点として、その位置からベクトルV1の分だけ変位した位置を、隠れた幹部の推定位置として決定する。図31Cにおいて、推定された隠れた幹部の位置が×印で示されている。ECU170は、例えば、当該位置と幹部22R2の位置との中点C1を通る経路を、トラクタ100Aの走行経路として決定する。
このような方法で決定された経路に沿ってトラクタ100Aが走行すると、やがて幹部22L1によって隠れていた後方の幹部22L2がスキャンデータから検出される。さらに後方の幹部22L3はまだ検出されていないとする。幹部22L2の位置が推定位置(×印)とは異なる場合、ECU170は、幹部22L2と対を成す右側の幹部(図31Cの例では幹部22R3)を特定し、その幹部22R3から幹部22L2までのベクトルV2を決定する。次に、さらに後方の幹部22R4の位置を起点として、その位置からベクトルV2の分だけ変位した位置を、隠れた幹部22L3の位置として決定する。以上のような動作を繰り返すことにより、隠れた幹部の位置を推定して適切な経路を設定し、その経路に沿って自動操舵を行うことができる。
図31Dは、樹木列が曲線的に配置されている場合における幹部の2次元分布を示す格子状データの一例を示す図である。ECU160は、環境地図を構築するためのデータを収集する過程で、図31Dに示すような格子状データを生成してもよい。その場合、ECU170は、このような格子状データに基づいて経路設定を行ってもよい。この例において、隠れた幹の位置は、セルの大きさの単位で推定され得る。
以上のような動作により、環境地図を作成しながら、比較的なめらかな操舵制御を行うことができる。
<障害物回避動作>
次に、本実施形態における障害物回避動作を説明する。
次に、本実施形態における障害物回避動作を説明する。
図32は、トラクタ100Aの走行環境に障害物40および人50が存在する状況の例を模式的に示す図である。トラクタ100Aは、1つ以上の障害物センサ130を備えている。走行中、障害物センサ130によって障害物40(例えばカゴ)、または人50を検出した場合、ECU180は、障害物40または人50を回避するように操舵制御を行う。回避しても障害物40、人50、枝部、または幹部22等との衝突を避けられない場合には、ECU180は、トラクタ100Aを停止させる。なお、ECU180は、障害物が検出された場合に、障害物を回避できる場合であってもトラクタ100Aを停止させてもよい。障害物センサ130によらず、LiDARセンサ110によって同様の検出を行ってもよい。
図32に示す例では、人50が樹木の幹部22の近くに存在する。樹木の葉部も含めてマッチングが行われる場合には、葉部に人50またはその他の障害物が隠れてしまい、葉部か他の物体かの判別が難しいことがある。特に、人50が幹部22の近くの葉の中に立っている場合、判別が難しい。本実施形態では、幹部22の分布を示す環境地図を用いてマッチングが行われるので、幹部と他の物体との判別を容易にすることができる。スキャンデータと環境地図とのマッチングによって検出された幹部以外の物体を、人または障害物として容易に検出することができる。このように、LiDARセンサ110を障害物センサとして利用することもできる。トラクタ100Aが複数のLiDARセンサ110を、トラクタ100Aの異なる箇所に備えている場合、障害物センサ130を省略してもよい。
<固有の幹間隔の組み合わせに基づく自己位置推定>
以上の実施形態では、樹木の幹部の間隔がおおよそ一定であるものとしているが、現実には、樹木の幹部の間隔は場所によって異なり得る。そこで、ECU160は、自己位置推定を行うとき、センサデータから、固有の幹間隔の組み合わせを有する複数の樹木の幹部を検出し、当該固有の幹間隔の組み合わせを有する複数の樹木の幹部と、環境地図データから抽出した当該固有の幹間隔の組み合わせを有する複数の樹木の幹部とのマッチングにより、移動体の位置を推定してもよい。以下、図33Aおよび図33Bを参照して、この動作を説明する。
以上の実施形態では、樹木の幹部の間隔がおおよそ一定であるものとしているが、現実には、樹木の幹部の間隔は場所によって異なり得る。そこで、ECU160は、自己位置推定を行うとき、センサデータから、固有の幹間隔の組み合わせを有する複数の樹木の幹部を検出し、当該固有の幹間隔の組み合わせを有する複数の樹木の幹部と、環境地図データから抽出した当該固有の幹間隔の組み合わせを有する複数の樹木の幹部とのマッチングにより、移動体の位置を推定してもよい。以下、図33Aおよび図33Bを参照して、この動作を説明する。
図33Aは、トラクタ100Aによって検出される樹木列の幹部22の配置の例を示す図である。図33Bは、環境地図データが示す幹分布の例を示す図である。この例において、点線枠で囲まれた3つの幹部は、1.2mと1.5mという固有の幹間隔の組み合わせを有する。このような場合、ECU160は、自己位置推定を行うとき、取得したスキャンデータから検出された幹部22のそれぞれの間隔を計算し、固有の幹間隔の組み合わせを有する部分を決定する。そして、図33Bに示す環境地図から、決定した幹間隔の組み合わせを有する部分を検索して特定する。ECU160は、その固有の幹間隔を有する部分同士のマッチングにより、トラクタ100Aの現在の位置および向きを推定することができる。このような動作により、樹木列が固有の幹間隔の組み合わせを有する部分を含む場合に、マッチングの高速化および位置推定の精度の向上を図ることができる。樹木列における幹間隔の組み合わせを示す情報が予め記憶媒体に記録されていてもよい。そのような情報は、例えばECU160が環境地図データに基づいて自動で記録してもよいし、ユーザが操作端末200を操作することによって記録してもよい。
本実施形態において、トラクタ100Aが生成した環境地図データは、同一の環境内を走行する他のトラクタまたはトラクタ以外の移動体に提供することもできる。例えば広い果樹園において、複数の移動体によって作業を行う場合、環境地図を一台の移動体またはコンピュータが作成すれば十分であり、環境地図を複数の移動体で共有することが効率的である。1つの環境内の環境地図を構築するための局所地図を、複数のトラクタまたはその他の移動体によって分担して生成してもよい。その場合、複数の移動体によって生成された局所地図をまとめて1つの環境地図を生成するコンピュータがシステム内に設けられる。当該コンピュータは、生成した環境地図を、複数の移動体に、有線もしくは無線のネットワーク、または記憶媒体を介して配信してもよい。
同一の環境内を複数の移動体が移動しながら作業するシステムにおいては、移動体ごとにセンサ座標系と移動体座標系との位置関係が異なり得る。同一の機種であっても、センサの取り付け位置の誤差に起因して、センサ座標系から移動体座標系への変換に誤差が生じ得る。そのような誤差の影響を低減するために、各移動体は、通常の運行を開始する前に、試運転によってセンサ座標系から移動体座標系への座標変換のためのパラメータを決定するキャリブレーションを実行する。
本実施形態の技術は、ぶどう園などの果樹園を走行するトラクタに限らず、森林などの、複数の樹木列が存在する環境で使用される任意の移動体(例えば、移動ロボット、ドローン等)に適用することができる。この点については、以降の実施形態でも同様である。
[3-2.実施形態2]
次に、本開示の第2の実施形態による移動体を説明する。
次に、本開示の第2の実施形態による移動体を説明する。
本実施形態における移動体は、少なくとも2つのLiDARセンサを備え、それらのLiDARセンサから出力されるセンサデータに基づいて、自己位置推定および地図データの生成を行う。少なくとも2つのLiDARセンサの各々は、移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データ、または距離分布データを出力する。移動体は、実施形態1と同様、複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、自己位置推定装置と、自己位置推定装置によって推定された移動体の位置に応じて移動体の移動を制御する制御装置とを備える。自己位置推定装置は、移動体が移動している間に少なくとも2つのセンサから繰り返し出力される点群データに基づいて、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部と環境地図データとのマッチングを行うことにより、移動体の位置を推定する。移動体はさらに、環境地図データ、または環境地図データを構築するための局所地図データを生成するデータ生成装置も備える。データ生成装置は、移動体が移動している間に少なくとも2つのセンサから繰り返し出力される点群データに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録する。データ生成装置は、移動体が移動している間に繰り返し生成した局所地図データを統合することにより、ワールド座標系における環境地図データを生成して記憶装置に記録することもできる。
以下、移動体がぶどう園などの果樹園内を走行するトラクタである場合を例に、本実施形態の構成および動作を説明する。以下の説明において、実施形態1とは異なる点を中心に説明し、重複する事項についての説明は省略する。
図34は、本実施形態におけるトラクタ100Bを側面方向から見た模式図である。このトラクタ100Bは、車体前方下部に第1のLiDARセンサ110Aを備え、キャビンの上部前方に第2のLiDARセンサ110Bを備えている。それ以外の点は、図7に示す構成と同様である。図34には、図7に示されている連結装置108、および作業機300が示されていないが、本実施形態においてもこれらは使用され得る。
本実施形態における第1のLiDARセンサ110Aおよび第2のLiDARセンサ110Bの各々は、2次元LiDARセンサである。言い換えれば、LiDARセンサ110A、110Bの各々は、図9Aに示す例において、N=1の場合、すなわち1つの仰角の方向にのみレーザビームを出射するLiDARセンサ110と同等である。LiDARセンサ110A、110Bの各々は、各反射点までの距離と方向とを示すデータ、または、2次元の点群データを出力する。2次元の点群データは、LiDARセンサ110A、110Bのそれぞれのセンサ座標系で表現された複数の反射点の2次元座標の情報を含む。2つの2次元LiDARを用いることにより、3次元LiDARを用いる場合よりもコストを低減させることができる。
第1のLiDARセンサ110Aは、トラクタ100Bが走行する環境における樹木列の幹部の平均高さよりも低い位置に搭載されている。第2のLiDARセンサ110Bは、当該樹木列の幹部の平均高さよりも高い位置に搭載されている。第1のLiDARセンサ110Aは、例えば地上から10cm以上150cm以下、ある例では15cm以上100cm以下の高さに配置され得る。第2のLiDARセンサ110Bは、例えば地上から150cmよりも高い位置(例えば地上から約2m)に配置され得る。
第1のLiDARセンサ110Aは、実施形態1におけるLiDARセンサ110と同様、トラクタ100Bの前方に向けてレーザパルスを出射するように配置される。図34の例における第1のLiDARセンサ110Aは、地面にほぼ平行にレーザパルスを出射する。トラクタ100Bが平坦な地面に位置しているとき、第1のLiDARセンサ110Aから出射されるレーザパルスの出射方向と地面とのなす角度は、例えば±20度の範囲内、ある例では±10度の範囲内に設定され得る。
一方、第2のLiDARセンサ110Bは、トラクタ100Bの前方斜め下に向けてレーザパルスを出射するように配置される。図34の例における第2のLiDARセンサ110Bは、地面から約25度傾いた方向にレーザパルスを出射する。トラクタ100Bが平坦な地面に位置しているとき、第2のLiDARセンサ110Bから出射されるレーザパルスの出射方向と地面とのなす角度は、例えば10度以上45度以下の範囲に設定され得る。
図35は、本実施形態におけるトラクタ100Bの概略的な構成の例を示すブロック図である。図35に示す構成例では、トラクタ100Bが2つのLiDARセンサ110A、110Bを備えている点で、図8に示す構成とは異なる。
ECU160は、上記の自己位置推定装置およびデータ生成装置として機能する。ECU160は、2つのLiDARセンサ110A、110Bから繰り返し出力される点群データから、最新スキャンを含む複数スキャンの入力点群を取得し、複数スキャンの入力点群と環境地図データとのマッチングを行う。これにより、走行中のトラクタ100Bの位置および向きを推定することができる。なお、ECU160は、マッチングによってトラクタ100Bの位置のみを推定し、向きについてはIMU125から出力された信号に基づいて決定してもよい。
環境地図を構築する過程において、ECU160は、トラクタ100Bが移動している間に2つのLiDARセンサ110A、110Bから繰り返し出力されるスキャンデータに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置150に記録する。この動作が行われている間、ECU170は、スキャンデータに基づいて検出された樹木列の幹部の間を通る目標経路を設定する。ECU180は、設定された目標経路に従って駆動装置140を制御することにより、トラクタ100Bを目標経路に沿って走行させる。ECU170は、障害物センサ130から出力される信号、またはLiDARセンサ110A、110Bの一方または両方から出力されるスキャンデータに基づいて障害物を検出し、障害物との接触を回避するように目標経路を設定してもよい。ECU160は、繰り返し生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成して記憶装置150に記録する。なお、樹木列の幹部の検出は、局所地図データを生成する段階ではなく、環境地図データを生成する段階で行ってもよい。また、局所地図データに基づいて環境地図データを生成する処理は、外部のコンピュータが実行してもよい。
環境地図が構築された後の自動走行または自動操舵の動作中、ECU160は、LiDARセンサ110A、110Bから繰り返し出力されるスキャンデータに基づいて樹木列の幹部を検出し、検出した幹部と環境地図データが示す幹部とのマッチングを行うことにより、自己位置推定を行う。ECU180は、ECU170によって予め決定された走行予定経路に沿ってトラクタ100Bを走行させる。この場合も、ECU170は、トラクタ100Bの走行中に障害物センサ130から出力される信号、またはLiDARセンサ110A、110Bの一方または両方から出力されるスキャンデータに基づいて障害物を検出し、障害物との接触を回避するように走行予定経路を変更してもよい。
図36Aから図36Cは、トラクタ100Bが樹木21の近くを走行する様子を模式的に示す図である。図36A、図36B、図36Cの順に、トラクタ100Bが樹木21に近付いている。これらの図において、破線は、LiDARセンサ110A、110Bから出射されるレーザビームを表している。図示されるように、トラクタ100Bと樹木21との距離に応じて、第2のLiDARセンサ110Bから出射されるレーザビームによって照射される樹木21の幹部22の照射点の位置が、高さ方向に変化する。一方、第1のLiDARセンサ110Aから出射されるレーザビームによって照射される樹木21の幹部22の照射点の位置の変化は相対的に小さい。これらのレーザビームの複数回のスキャンによって取得される点群データを組み合わせることにより、3次元LiDARを用いた場合と同様に、幹部22の表面における反射点の3次元分布を示す点群データを取得することができる。
本実施形態におけるECU160は、LiDARセンサ110A、110Bのそれぞれから出力された所定のスキャン回数分(例えば3回分または5回分等)のスキャンデータを蓄積し、蓄積されたスキャンデータと、環境地図データとのマッチングを行うことにより、自己位置推定を行ってもよい。このようにすることで、実質的に3次元LiDARを用いた場合と同様の高い精度の自己位置推定が可能になる。
地図データを取得するためにトラクタ100Bが走行している間、ECU170は、LiDARセンサ110A、110Bの一方または両方から出力された数スキャン分のスキャンデータに基づいて、トラクタ100Bの局所的な目標経路を決定する。例えば、LiDARセンサ110A、110Bの一方または両方から出力された数スキャン分のスキャンデータが示す点群の配置から、隣り合う2つの樹木間の中心位置を決定し、その中心位置を通るようにトラクタ100Bの局所的な目標経路を決定することができる。このような経路設定により、トラクタ100Bは、左右の樹木列との距離が概ね等しい状態を保ちながら自律的に走行することができる。
幹部の検出は、第1のLiDARセンサ110Aから出力されるスキャンデータのみに基づいて行われてもよい。相対的に低い位置にある第1のLiDARセンサ110Aから出力されるスキャンデータは、幹部の表面を表す点群の情報を多く含むことが多い。このため、第1のLiDARセンサ110Aからのスキャンデータのみを用いた場合でも幹部の分布情報を取得することができる。
本実施形態における第2のLiDARセンサ110Bは、相対的に高い位置に配置されているため、樹木列の葉部の検出に適している。そこで、ECU160は、第2のLiDARセンサ110Bによって取得された点群データに基づいて、樹木列の葉部の分布を示すデータを生成し、記憶装置150または他の記憶媒体に記録してもよい。そのようなデータは、樹木の生育状況の管理(例えば、キャノピーマネジメント)のために使用され得る。
トラクタ100Bの走行中に、第2のLiDARセンサ110Bによって取得された点群データに基づいて、樹木列の葉部などの、比較的高い位置に存在する障害物を検出し、回避または停止する動作を行ってもよい。トラクタ100Bの前方上部に障害物センサ130が設けられていない場合、第2のLiDARセンサ110Bが設けられていなければ、樹木列間の経路の上方にはみだした樹木の葉または蔓を検出して避けることが難しい。第2のLiDARセンサ110Bを設けることにより、それらの葉または蔓を検出することが容易になる。
本実施形態によれば、2つの2次元LiDARを用いることにより、低いコストで実施形態1と同様の動作を実現することができる。なお、2つに限らず、3つ以上の2次元LiDARを異なる位置に設けてもよい。また、1つ以上の2次元LiDARと1つ以上の3次元LiDARとを組み合わせて使用してもよい。このように、本実施形態の構成には多様な変形が可能である。なお、実施形態1について説明した種々の技術は、本実施形態においてもそのまま適用できる。
[3-3.実施形態3]
次に、本開示の第3の実施形態を説明する。
次に、本開示の第3の実施形態を説明する。
本実施形態は、1台または複数台の移動体(例えばドローン)を用いて、環境地図データを構築するための局所地図データを収集し、収集された局所地図データに基づいて環境地図データを生成するシステムに関する。
図37は、本実施形態のシステムの構成例を模式的に示す図である。本システムは、1台以上のドローン400と、1台以上のトラクタ100Cと、サーバコンピュータ500(以下、「サーバ500」と称する。)とを備える。これらの構成要素は、有線または無線のネットワーク60を介して、互いに通信可能に接続されている。図37には、2台のドローン400および3台のトラクタ100Cが示されているが、ドローン400およびトラクタ100Cのそれぞれの台数は任意である。各ドローン400は、各トラクタ100Cが走行する環境内を飛行しながら環境地図データを構築するための局所地図データを生成し、サーバ500に送信する。サーバ500は、1台以上のドローン400から受信した局所地図データに基づいて、統一されたワールド座標系で表現された環境地図データを生成し、各トラクタ100Cに配信する。各トラクタ100Cは、配信された環境地図データに基づいて果樹園内を自律的に走行する。
図38は、本システムの構成例を示すブロック図である。この例におけるトラクタ100Cは、図8に示す実施形態1のトラクタ100Aと同様の構成要素を備える。ただし、本実施形態におけるECU160は、地図データ生成モジュール164を備えていない。通信IF190は、ネットワーク60を介してサーバ500と通信を行う。記憶装置150は、サーバ500から配信された環境地図データを記憶する。なお、図38では、トラクタ100Cに連結されて使用される作業機の図示は省略されている。通信IF190は、実施形態1と同様、トラクタ100Cに接続された作業機との通信を行ってもよい。
ドローン400は、データ生成ユニット450と、駆動装置440と、制御装置480と、通信IF490とを備える。駆動装置440は、例えば駆動用の電気モータ、および複数のプロペラなどの、ドローン400の飛行に必要な種々の装置を含む。制御装置480は、データ生成ユニット450および駆動装置440の動作を制御する。通信IF490は、ネットワーク60を介してサーバ500と通信するための回路である。データ生成ユニット450は、LiDARセンサ410と、データ生成装置460とを備える。LiDARセンサ410は、実施形態1におけるLiDARセンサ110と同様の機能を備える。データ生成装置460は、実施形態1におけるECU160と同様の機能を備える。すなわち、データ生成装置460は、自己位置推定と地図生成とを同時に行う機能を備える。ただし、データ生成装置460は、最終的な環境地図データの生成を行わない。データ生成装置460は、プロセッサとメモリ等の記憶媒体とを備える。データ生成装置460は、ドローン400の飛行中にLiDARセンサ410から繰り返し出力されたスキャンデータに基づいて、複数の樹木列の幹部の分布を示す局所地図データを繰り返し生成して記憶媒体に蓄積する。蓄積された局所地図データは、例えばユーザによる操作により、通信IF490からサーバ500に送信される。
サーバ500は、例えばクラウドサーバまたはエッジサーバなどの、トラクタ100Cおよびドローン400から離れた場所に設置されたコンピュータであり得る。サーバ500は、記憶装置550と、処理装置560と、通信IF590とを備える。通信IF590は、ネットワーク60を介してトラクタ100Cおよびドローン400と通信するための回路である。処理装置560は、複数のドローン400から取得した局所地図データを、座標系を統一して統合することにより、環境地図データを生成し、記憶装置550に記録する。処理装置560は、生成した環境地図データを、通信IF590から複数のトラクタ100Cに配信する。処理装置560は、環境地図データをトラクタ100C以外の移動体に配信してもよい。例えば、ドローン400が、地図データの収集だけでなく、播種、施肥、または防除などの作業も実行する場合には、ドローン400に環境地図データが配信されてもよい。その場合、ドローン400は、LiDARセンサ410から出力されたスキャンデータと、環境地図データとのマッチングによって自己位置推定を行いながら自律的に飛行し、所定の作業を実行することができる。
図39は、ドローン400の外観の例を示す斜視図である。このドローン400は、本体の前面にデータ生成ユニット450を備える。データ生成ユニット450は、LiDARセンサ410を備えており、飛行しながらレーザビームで周囲の環境をスキャンすることができる。
図40は、複数のドローン400によるデータ収集作業の様子の例を模式的に示す斜視図である。図40には、複数の樹木列20の間を飛行する2台のドローン400が例示されている。これらのドローン400は、樹木列20の間を低空飛行で自律的に移動しながら、樹木列20の幹部をレーザビームでスキャンすることにより、幹部の分布を示す局所地図データを生成する動作を繰り返す。局所地図データの生成方法は、実施形態1における方法と同様である。
図41は、ドローン400に搭載されたLiDARセンサ410から出射されたレーザビームによって樹木21が照射されている様子を模式的に示す図である。本実施形態におけるドローン400は、LiDARセンサ410が、樹木列の幹部の平均高さよりも低い位置を保つ高度で飛行するようにプログラミングされている。LiDARセンサ410が、例えば地上から15cm以上100cm以下の高さに位置するように、ドローン400の高度が制御される。データ生成装置460は、このような低い高度に位置するLiDARセンサ410から繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、ドローン400の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す局所地図データを生成して記憶媒体に記録する。データ生成装置460は、LiDARセンサ410から繰り返し出力されるセンサデータから複数スキャンの入力点群を取得し、最新スキャンを含む少なくとも1つのスキャンの入力点群と、前回以前に生成された局所地図データとのマッチングを行うことにより、自己位置推定を行う。
図42は、各ドローン400から出力される局所地図データの形式の一例を示す図である。この例における局所地図データは、ドローン400の識別番号である移動体IDと、ワールド座標系におけるドローン400の位置(x,y,z)および姿勢(θR,θP,θY)と、反射点ごとに記録される情報とを含む。反射点ごとに記録される情報は、図23に示す情報と同様であるが、各反射点の座標は、ドローン400のLiDARセンサ410に固定されたセンサ座標系における座標(u,v,w)である。このような局所地図データが、各ドローン400からサーバ500に送信され得る。なお、分類および幹IDなどの、その点が幹であるか否かに関する情報は、局所地図データに含まれていなくてもよい。サーバ500における処理装置560が、複数のドローン400から局所地図データを取得した後、点群の分布の特徴に基づいて幹部に該当する点群を検出してもよい。
以上の構成により、例えば広い果樹園においても、環境地図を構築するためのデータを効率的に収集することができる。
なお、本実施形態では、ドローン400だけでなく、トラクタ100Cなどの他の移動体も、環境地図データを構築するための局所地図データを取得する動作を行ってもよい。また、局所地図データを生成する移動体は、必ずしも自律的に移動可能な移動体でなくてもよい。例えば、樹木列の幹部の平均高さよりも低い位置に搭載された1つ以上のLiDARセンサを備えたトラクタまたはドローンなどの移動体を、ユーザが運転または操縦することによって局所地図データを生成してもよい。
サーバ500を設けることなく、ドローン400またはトラクタ100Cなどの移動体が最終的な環境地図データを生成し、他の移動体に環境地図データを提供してもよい。その場合には、移動体間の通信によって環境地図データが直接的に送受信され得る。
[3-4.他の実施形態]
以上の実施形態において、各トラクタは、無人トラクタであってもよい。その場合には、キャビン、運転席、ステアリングホイール、操作端末などの、有人運転にのみ必要な構成要素は、トラクタに設けられていなくてもよい。無人トラクタは、自律走行、またはトラクタのユーザによる遠隔操作によって、前述の各実施形態における動作と同様の動作を実行してもよい。
以上の実施形態において、各トラクタは、無人トラクタであってもよい。その場合には、キャビン、運転席、ステアリングホイール、操作端末などの、有人運転にのみ必要な構成要素は、トラクタに設けられていなくてもよい。無人トラクタは、自律走行、またはトラクタのユーザによる遠隔操作によって、前述の各実施形態における動作と同様の動作を実行してもよい。
以上の実施形態において、移動体が備える1つ以上のセンサは、レーザビームのスキャンによって2次元または3次元の点群データまたは距離分布データをセンサデータとして出力するLiDARセンサである。しかし、センサはそのようなLiDARセンサに限定されない。例えば、フラッシュ型のLiDARセンサ、またはイメージセンサなどの他の種類のセンサを利用してもよい。そのような他の種類のセンサを、スキャン型のLiDARセンサと組み合せて利用してもよい。
以上の実施形態における自己位置推定および自律移動(もしくは自動操舵)、または地図データ生成に必要な処理を実行する装置を、それらの機能を有しない移動体に後から取り付けることもできる。例えば、複数の樹木列の間を移動する移動体の動作を制御する制御ユニットを、当該移動体に取り付けて使用することができる。そのような制御ユニットは、移動体の周囲の環境における物体の分布を示すセンサデータを出力する1つ以上のセンサと、複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、自己位置推定装置と、自己位置推定装置によって推定された移動体の位置に応じて移動体の移動を制御する制御装置とを備える。自己位置推定装置は、移動体が移動している間に1つ以上のセンサから繰り返し出力されるセンサデータに基づいて、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部と環境地図データとのマッチングを行うことにより、移動体の位置を推定する。また、地図データを生成するデータ生成ユニットを、複数の樹木列の間を移動する移動体に取り付けて使用することもできる。そのようなデータ生成ユニットは、移動体の周囲の環境における物体の分布を示すセンサデータを出力する1つ以上のセンサと、データ生成装置とを備える。データ生成装置は、移動体が移動している間に1つ以上のセンサから繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録する。
以上のように、本開示は、以下の項目に記載の移動体、制御ユニット、データ生成ユニット、方法、およびコンピュータプログラムを含む。
[項目1]
複数の樹木列の間を移動する移動体であって、
前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える移動体。
複数の樹木列の間を移動する移動体であって、
前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える移動体。
[項目2]
複数の樹木列の間を移動する移動体であって、
前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記移動体が移動している間に前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データに基づいて、前記移動体の位置を推定しながら、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録するデータ生成装置と、
を備える移動体。
複数の樹木列の間を移動する移動体であって、
前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記移動体が移動している間に前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データに基づいて、前記移動体の位置を推定しながら、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録するデータ生成装置と、
を備える移動体。
[項目3]
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置をさらに備える、項目2に記載の移動体。
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置をさらに備える、項目2に記載の移動体。
[項目4]
前記少なくとも2つのLiDARセンサは、
前記樹木列の幹部の平均高さよりも低い位置に搭載された第1のLiDARセンサと、
前記樹木列の幹部の平均高さよりも高い位置に搭載された第2のLiDARセンサと、を含む、項目1から3のいずれかに記載の移動体。
前記少なくとも2つのLiDARセンサは、
前記樹木列の幹部の平均高さよりも低い位置に搭載された第1のLiDARセンサと、
前記樹木列の幹部の平均高さよりも高い位置に搭載された第2のLiDARセンサと、を含む、項目1から3のいずれかに記載の移動体。
[項目5]
前記第1のLiDARセンサは、地上から15cm以上100cm以下の高さに配置されている、項目4に記載の移動体。
前記第1のLiDARセンサは、地上から15cm以上100cm以下の高さに配置されている、項目4に記載の移動体。
[項目6]
前記第1のLiDARセンサは、前記移動体の前方に向けてレーザパルスを出射するように配置され、
前記第2のLiDARセンサは、前記移動体の前方斜め下に向けてレーザパルスを出射するように配置されている、
項目4または5に記載の移動体。
前記第1のLiDARセンサは、前記移動体の前方に向けてレーザパルスを出射するように配置され、
前記第2のLiDARセンサは、前記移動体の前方斜め下に向けてレーザパルスを出射するように配置されている、
項目4または5に記載の移動体。
[項目7]
前記第1のLiDARセンサおよび前記第2のLiDARセンサの各々は、2次元点群データを出力する2次元LiDARセンサである、項目1から6のいずれかに記載の移動体。
前記第1のLiDARセンサおよび前記第2のLiDARセンサの各々は、2次元点群データを出力する2次元LiDARセンサである、項目1から6のいずれかに記載の移動体。
[項目8]
前記自己位置推定装置は、前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データから、最新スキャンを含む複数スキャンの入力点群を取得し、前記複数スキャンの入力点群と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、項目1から7のいずれかに記載の移動体。
前記自己位置推定装置は、前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データから、最新スキャンを含む複数スキャンの入力点群を取得し、前記複数スキャンの入力点群と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、項目1から7のいずれかに記載の移動体。
[項目9]
前記制御装置は、前記少なくとも2つのLiDARセンサの少なくとも1つから出力された前記点群データに基づいて、前記移動体の経路上に障害物が存在するか否かを判断し、前記障害物が存在すると判断したとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、項目1または3に記載の移動体。
前記制御装置は、前記少なくとも2つのLiDARセンサの少なくとも1つから出力された前記点群データに基づいて、前記移動体の経路上に障害物が存在するか否かを判断し、前記障害物が存在すると判断したとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、項目1または3に記載の移動体。
[項目10]
前記少なくとも2つのLiDARセンサは、
前記樹木列の幹部の平均高さよりも低い位置に搭載された第1のLiDARセンサと、
前記樹木列の幹部の平均高さよりも高い位置に搭載された第2のLiDARセンサと、を含み、
前記制御装置は、前記第2のLiDARセンサから出力された前記点群データに基づいて、前記移動体の経路上に障害物が存在するか否かを判断し、前記障害物が存在すると判断したとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、項目1または3に記載の移動体。
前記少なくとも2つのLiDARセンサは、
前記樹木列の幹部の平均高さよりも低い位置に搭載された第1のLiDARセンサと、
前記樹木列の幹部の平均高さよりも高い位置に搭載された第2のLiDARセンサと、を含み、
前記制御装置は、前記第2のLiDARセンサから出力された前記点群データに基づいて、前記移動体の経路上に障害物が存在するか否かを判断し、前記障害物が存在すると判断したとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、項目1または3に記載の移動体。
[項目11]
複数の樹木列の間を移動する移動体の動作を制御する制御ユニットであって、
前記移動体に取り付けられて使用され、前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記少なくとも2つのセンサから繰り返し出力される前記点群データに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える制御ユニット。
複数の樹木列の間を移動する移動体の動作を制御する制御ユニットであって、
前記移動体に取り付けられて使用され、前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記少なくとも2つのセンサから繰り返し出力される前記点群データに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える制御ユニット。
[項目12]
複数の樹木列の間を移動する移動体において使用されるデータ生成ユニットであって、
前記移動体に取り付けられて使用され、前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記移動体が移動している間に前記少なくとも2つのセンサから繰り返し出力される前記点群データに基づいて、前記移動体の位置を推定しながら、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録するデータ生成装置と、
を備えるデータ生成ユニット。
複数の樹木列の間を移動する移動体において使用されるデータ生成ユニットであって、
前記移動体に取り付けられて使用され、前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記移動体が移動している間に前記少なくとも2つのセンサから繰り返し出力される前記点群データに基づいて、前記移動体の位置を推定しながら、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録するデータ生成装置と、
を備えるデータ生成ユニット。
[項目13]
複数の樹木列の間を移動する移動体の動作を制御する方法であって、
前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された、2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
を含む方法。
複数の樹木列の間を移動する移動体の動作を制御する方法であって、
前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された、2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
を含む方法。
[項目14]
複数の樹木列の間を移動する移動体によって実行される方法であって、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された、2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成することと、
前記局所地図データを記憶装置に記録することと、
を含む方法。
複数の樹木列の間を移動する移動体によって実行される方法であって、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された、2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成することと、
前記局所地図データを記憶装置に記録することと、
を含む方法。
[項目15]
複数の樹木列の間を移動する移動体の動作を制御するコンピュータによって実行されるコンピュータプログラムであって、
前記コンピュータに、
前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された、2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
を実行させるコンピュータプログラム。
複数の樹木列の間を移動する移動体の動作を制御するコンピュータによって実行されるコンピュータプログラムであって、
前記コンピュータに、
前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された、2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
を実行させるコンピュータプログラム。
[項目16]
複数の樹木列の間を移動する移動体におけるコンピュータによって実行されるコンピュータプログラムであって、
前記コンピュータに、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された、2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成することと、
前記局所地図データを記憶装置に記録することと、
を実行させるコンピュータプログラム。
複数の樹木列の間を移動する移動体におけるコンピュータによって実行されるコンピュータプログラムであって、
前記コンピュータに、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された、2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成することと、
前記局所地図データを記憶装置に記録することと、
を実行させるコンピュータプログラム。
[項目17]
各々が項目3に記載の移動体である1台以上の移動体によって生成された前記局所地図データを取得することと、
取得した前記局所地図データを統合することにより、前記環境地図データを生成することと、
前記環境地図データを記憶装置に記録することと、
を含む方法。
各々が項目3に記載の移動体である1台以上の移動体によって生成された前記局所地図データを取得することと、
取得した前記局所地図データを統合することにより、前記環境地図データを生成することと、
前記環境地図データを記憶装置に記録することと、
を含む方法。
[項目18]
前記環境地図データを、前記1台以上の移動体、または他の移動体に配信することをさらに含む、項目17に記載の方法。
前記環境地図データを、前記1台以上の移動体、または他の移動体に配信することをさらに含む、項目17に記載の方法。
本開示の技術は、例えばぶどう園などの果樹園のように、複数の樹木列が存在する環境を移動するトラクタ、ドローン、歩行ロボットなどの移動体に適用することができる。
20 樹木列
22 樹木の幹部
30 走行経路
40 障害物
50 人
60 ネットワーク
100 移動体
100A、100B トラクタ
101 車両本体
102 原動機(エンジン)
103 変速装置(トランスミッション)
104 車輪
105 キャビン
106 操舵装置
107 運転席
108 連結装置
110、110A、110B LiDARセンサ
111 レーザユニット
112 レーザ光源
113 光検出器
114 モータ
115 制御回路
116 信号処理回路
117 メモリ
120 GNSSユニット
125 慣性計測装置(IMU)
130 障害物センサ
140 駆動装置
150 記憶装置
160 電子制御ユニット(SLAM)
162 自己位置推定モジュール
164 地図データ生成モジュール
170 電子制御ユニット(経路決定)
180 電子制御ユニット(駆動制御)
190 通信インタフェース(IF)
200 操作端末
300 作業機(インプルメント)
340 駆動装置
380 制御装置
390 通信インタフェース(IF)
400 ドローン
410 LiDARセンサ
440 駆動装置
450 データ生成ユニット
460 データ生成装置
480 制御装置
490 通信IF
500 サーバ
550 記憶装置
560 処理装置
590 通信IF
22 樹木の幹部
30 走行経路
40 障害物
50 人
60 ネットワーク
100 移動体
100A、100B トラクタ
101 車両本体
102 原動機(エンジン)
103 変速装置(トランスミッション)
104 車輪
105 キャビン
106 操舵装置
107 運転席
108 連結装置
110、110A、110B LiDARセンサ
111 レーザユニット
112 レーザ光源
113 光検出器
114 モータ
115 制御回路
116 信号処理回路
117 メモリ
120 GNSSユニット
125 慣性計測装置(IMU)
130 障害物センサ
140 駆動装置
150 記憶装置
160 電子制御ユニット(SLAM)
162 自己位置推定モジュール
164 地図データ生成モジュール
170 電子制御ユニット(経路決定)
180 電子制御ユニット(駆動制御)
190 通信インタフェース(IF)
200 操作端末
300 作業機(インプルメント)
340 駆動装置
380 制御装置
390 通信インタフェース(IF)
400 ドローン
410 LiDARセンサ
440 駆動装置
450 データ生成ユニット
460 データ生成装置
480 制御装置
490 通信IF
500 サーバ
550 記憶装置
560 処理装置
590 通信IF
Claims (14)
- 複数の樹木列の間を移動する移動体であって、
前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える移動体。 - 複数の樹木列の間を移動する移動体であって、
前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記移動体が移動している間に前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データに基づいて、前記移動体の位置を推定しながら、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録するデータ生成装置と、
を備える移動体。 - 推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置をさらに備える、請求項2に記載の移動体。
- 前記少なくとも2つのLiDARセンサは、
前記樹木列の幹部の平均高さよりも低い位置に搭載された第1のLiDARセンサと、
前記樹木列の幹部の平均高さよりも高い位置に搭載された第2のLiDARセンサと、を含む、請求項1から3のいずれかに記載の移動体。 - 前記第1のLiDARセンサは、地上から15cm以上100cm以下の高さに配置されている、請求項4に記載の移動体。
- 前記第1のLiDARセンサは、前記移動体の前方に向けてレーザパルスを出射するように配置され、
前記第2のLiDARセンサは、前記移動体の前方斜め下に向けてレーザパルスを出射するように配置されている、
請求項4または5に記載の移動体。 - 前記第1のLiDARセンサおよび前記第2のLiDARセンサの各々は、2次元点群データを出力する2次元LiDARセンサである、請求項4から6のいずれかに記載の移動体。
- 前記自己位置推定装置は、前記少なくとも2つのLiDARセンサから繰り返し出力される前記点群データから、最新スキャンを含む複数スキャンの入力点群を取得し、前記複数スキャンの入力点群と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、請求項1から7のいずれかに記載の移動体。
- 前記制御装置は、前記少なくとも2つのLiDARセンサの少なくとも1つから出力された前記点群データに基づいて、前記移動体の経路上に障害物が存在するか否かを判断し、前記障害物が存在すると判断したとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、請求項1または3に記載の移動体。
- 前記少なくとも2つのLiDARセンサは、
前記樹木列の幹部の平均高さよりも低い位置に搭載された第1のLiDARセンサと、
前記樹木列の幹部の平均高さよりも高い位置に搭載された第2のLiDARセンサと、を含み、
前記制御装置は、前記第2のLiDARセンサから出力された前記点群データに基づいて、前記移動体の経路上に障害物が存在するか否かを判断し、前記障害物が存在すると判断したとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、請求項1または3に記載の移動体。 - 複数の樹木列の間を移動する移動体の動作を制御する制御ユニットであって、
前記移動体に取り付けられて使用され、前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記少なくとも2つのセンサから繰り返し出力される前記点群データに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える制御ユニット。 - 複数の樹木列の間を移動する移動体において使用されるデータ生成ユニットであって、
前記移動体に取り付けられて使用され、前記移動体の周囲の環境における物体の分布を示す2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサと、
前記移動体が移動している間に前記少なくとも2つのセンサから繰り返し出力される前記点群データに基づいて、前記移動体の位置を推定しながら、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録するデータ生成装置と、
を備えるデータ生成ユニット。 - 複数の樹木列の間を移動する移動体の動作を制御する方法であって、
前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された、2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
を含む方法。 - 複数の樹木列の間を移動する移動体によって実行される方法であって、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された、2次元または3次元の点群データを出力する少なくとも2つのLiDARセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の
幹部を検出することと、
検出した前記樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成することと、
前記局所地図データを記憶装置に記録することと、
を含む方法。
Priority Applications (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
EP21894462.7A EP4250046A4 (en) | 2020-11-18 | 2021-11-01 | MOVING BODY, CONTROL UNIT, DATA GENERATING UNIT, METHOD FOR CONTROLLING THE MOVEMENT OF A MOVING BODY AND METHOD FOR GENERATING DATA |
JP2022563679A JPWO2022107588A1 (ja) | 2020-11-18 | 2021-11-01 | |
US18/198,758 US20230284550A1 (en) | 2020-11-18 | 2023-05-17 | Mobile machine, control unit, data generation unit, method of controlling operation of a mobile machine, and method of generating data |
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2020-192015 | 2020-11-18 | ||
JP2020192015 | 2020-11-18 |
Related Child Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
US18/198,758 Continuation US20230284550A1 (en) | 2020-11-18 | 2023-05-17 | Mobile machine, control unit, data generation unit, method of controlling operation of a mobile machine, and method of generating data |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2022107588A1 true WO2022107588A1 (ja) | 2022-05-27 |
Family
ID=81708782
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/JP2021/040300 WO2022107588A1 (ja) | 2020-11-18 | 2021-11-01 | 移動体、制御ユニット、データ生成ユニット、移動体の動作を制御する方法、およびデータを生成する方法 |
Country Status (4)
Country | Link |
---|---|
US (1) | US20230284550A1 (ja) |
EP (1) | EP4250046A4 (ja) |
JP (1) | JPWO2022107588A1 (ja) |
WO (1) | WO2022107588A1 (ja) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2023037225A1 (de) * | 2021-09-07 | 2023-03-16 | Premosys Gmbh | Land- oder forstwirtschaftliches gerät, sensorvorrichtung und verfahren zum steuern des geräts |
US20230196611A1 (en) * | 2021-12-22 | 2023-06-22 | Aptiv Technologies Limited | Feature describing the shape of spatially distributed data set |
WO2024004575A1 (ja) * | 2022-06-28 | 2024-01-04 | 株式会社クボタ | 作業車両、および作業車両の制御方法 |
WO2024135019A1 (ja) * | 2022-12-21 | 2024-06-27 | 株式会社クボタ | 状態推定システムおよび農業機械 |
Citations (14)
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 | 株式会社クボタ | 作業車両の制御装置、作業車両、及び作業車両の制御方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11277956B2 (en) * | 2018-07-26 | 2022-03-22 | Bear Flag Robotics, Inc. | Vehicle controllers for agricultural and industrial applications |
JP7044694B2 (ja) * | 2018-12-27 | 2022-03-30 | ヤンマーパワーテクノロジー株式会社 | 作業車両用の障害物検知システム |
-
2021
- 2021-11-01 JP JP2022563679A patent/JPWO2022107588A1/ja active Pending
- 2021-11-01 WO PCT/JP2021/040300 patent/WO2022107588A1/ja unknown
- 2021-11-01 EP EP21894462.7A patent/EP4250046A4/en active Pending
-
2023
- 2023-05-17 US US18/198,758 patent/US20230284550A1/en active Pending
Patent Citations (14)
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 | 株式会社クボタ | 作業車両の制御装置、作業車両、及び作業車両の制御方法 |
Non-Patent Citations (1)
Title |
---|
See also references of EP4250046A4 |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2023037225A1 (de) * | 2021-09-07 | 2023-03-16 | Premosys Gmbh | Land- oder forstwirtschaftliches gerät, sensorvorrichtung und verfahren zum steuern des geräts |
US20230196611A1 (en) * | 2021-12-22 | 2023-06-22 | Aptiv Technologies Limited | Feature describing the shape of spatially distributed data set |
US11699245B1 (en) * | 2021-12-22 | 2023-07-11 | Aptiv Technologies Limited | Feature describing the shape of spatially distributed data set |
WO2024004575A1 (ja) * | 2022-06-28 | 2024-01-04 | 株式会社クボタ | 作業車両、および作業車両の制御方法 |
WO2024135019A1 (ja) * | 2022-12-21 | 2024-06-27 | 株式会社クボタ | 状態推定システムおよび農業機械 |
Also Published As
Publication number | Publication date |
---|---|
EP4250046A1 (en) | 2023-09-27 |
JPWO2022107588A1 (ja) | 2022-05-27 |
EP4250046A4 (en) | 2024-07-24 |
US20230284550A1 (en) | 2023-09-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2022107588A1 (ja) | 移動体、制御ユニット、データ生成ユニット、移動体の動作を制御する方法、およびデータを生成する方法 | |
US11778934B2 (en) | Agricultural lane following | |
US11789459B2 (en) | Vehicle controllers for agricultural and industrial applications | |
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 | |
Inoue et al. | The development of autonomous navigation and obstacle avoidance for a robotic mower using machine vision technique | |
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) | 作業車両、および作業車両の制御方法 | |
Vacho et al. | Identification of Agricultural Plant Row Using the Clustering Algorithm in the Navigation of Mobile Robot | |
WO2024004575A1 (ja) | 作業車両、および作業車両の制御方法 | |
WO2023112515A1 (ja) | 地図生成システムおよび地図生成方法 | |
WO2023234255A1 (ja) | センシングシステム、農業機械、およびセンシング装置 | |
WO2024004486A1 (ja) | 作業車両、制御方法および制御システム | |
Escolà et al. | Agricultural Robotic Sensors for Crop and Environmental Modelling | |
WO2023127557A1 (ja) | 農業機械、農業機械に用いるセンシングシステムおよびセンシング方法 | |
WO2023106158A1 (ja) | 自動運転を行う農業機械のための経路計画システム | |
WO2023248909A1 (ja) | 走行制御システム、農業機械および走行制御方法 | |
WO2023127556A1 (ja) | 農業機械、農業機械に用いるセンシングシステムおよびセンシング方法 |
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: 21894462 Country of ref document: EP Kind code of ref document: A1 |
|
ENP | Entry into the national phase |
Ref document number: 2022563679 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: 2021894462 Country of ref document: EP Effective date: 20230619 |