CN115235470A - A kind of agricultural robot positioning and navigation method and system based on visual detection - Google Patents

Abstract

The invention provides a method and system for positioning and navigating an agricultural robot based on visual detection, and belongs to the technical field of agricultural robots. The method of the invention includes: at the beginning stage of the crop growth cycle in the greenhouse environment, using the three-dimensional laser radar installed by the agricultural robot to establish an environmental map of the greenhouse environment, and calculating the visual labels between the ridges of the greenhouse environment based on the environmental map The pose of the code in the world coordinate system; based on the pose of the environment map and the visual label code in the world coordinate system, the pose of the agricultural robot is determined by visual detection method; the environment map is used as the A navigation map of the agricultural robot, and autonomous navigation of the agricultural robot based on the pose of the agricultural robot. The invention obtains a visual aided positioning method for agricultural robots suitable for fast-growing crops.

Description

A method and system for positioning and navigation of agricultural robot based on visual detection

technical field

The invention relates to the technical field of agricultural robots, in particular to a method and system for positioning and navigation of agricultural robots based on visual detection.

Background technique

The current indoor navigation system includes automatic line patrol, wireless positioning and laser SLAM positioning and other methods. However, in the agricultural greenhouse environment, especially in the environment where the road surface is not hardened, the automatic line patrol navigation system is inconvenient to lay, and the wireless positioning method will be interfered by the signal, and the laser SLAM positioning and navigation results largely depend on the laser scanning information and the map. Matching effect, in the agricultural environment, the environment of different growth cycles of crops changes greatly, and it is impossible to navigate through the map established in the previous stage.

SUMMARY OF THE INVENTION

In view of the above problems, the present invention proposes a positioning and navigation method for agricultural robots based on visual detection, including:

At the beginning of the crop growth cycle in the greenhouse environment, use the three-dimensional laser radar installed by the agricultural robot to establish an environmental map of the greenhouse environment, and calculate the visual label code between the ridges of the greenhouse environment based on the environmental map in the world coordinate system. pose;

Based on the pose of the environment map and the visual tag code in the world coordinate system, the pose of the agricultural robot is determined by a visual detection method;

The environment map is used as the navigation map of the agricultural robot, and the agricultural robot is autonomously navigated based on the pose of the agricultural robot.

Optionally, the agricultural robot uses crawler wheels to move, a three-dimensional laser radar is installed in the center of the top of the vehicle body of the agricultural robot, a camera is installed on the liftable pan/tilt on the top of the vehicle body, and an IMU attitude sensor is installed in the center of the chassis of the vehicle body. Encoders are installed on the wheels on both sides of the vehicle body.

Optionally, using the three-dimensional laser radar installed by the agricultural robot to establish an environmental map of the greenhouse environment, including:

The three-dimensional lidar emits multi-line laser light, and builds a 3D point cloud map of the greenhouse environment based on a three-dimensional mapping algorithm. 2D grid map for agricultural robot navigation;

The environment map includes: 3D point cloud map and 2D grid map.

Optionally, the method further includes: storing the pose of the visual label code in the world coordinate system into the label code array.

Optionally, based on the pose of the environment map and the visual label code in the world coordinate system, the pose of the agricultural robot is determined by a visual detection method, including:

The visual label code between the ridges in the greenhouse environment is recognized by the camera installed on the agricultural robot. If the camera recognizes the visual label code, the pose of the visual label code stored in the label code array in the world coordinate system is called, and the visual detection method is used to detect the visual label code. The pose of the agricultural robot is corrected to determine the pose of the agricultural robot.

Optionally, the visual label code between the ridges in the greenhouse environment is recognized by a camera installed on the agricultural robot, and if the camera does not recognize the visual label code, the agricultural robot is positioned by an encoder and an IMU attitude sensor.

Optionally, the environment map is used as the navigation map of the agricultural robot to navigate the agricultural robot, including:

Call the environment map, and use the 2D grid map in the environment map as the navigation map;

The agricultural robot is navigated based on a navigation map and a navigation function package that invokes ROS.

Optionally, the navigation function package includes: a pre-planned movement path of the agricultural robot between the ridges in the greenhouse environment.

On the other hand, the present invention also proposes an agricultural robot positioning and navigation system based on visual detection, including:

A data acquisition unit is used to establish an environmental map of the greenhouse environment by using the three-dimensional laser radar installed by the agricultural robot at the beginning of the crop growth cycle in the greenhouse environment, and calculate the visual labels between the ridges of the greenhouse environment based on the environmental map The pose of the code in the world coordinate system;

a positioning unit, based on the environment map and the pose of the visual tag code in the world coordinate system, to determine the pose of the agricultural robot through a visual detection method;

The data acquisition unit is used for taking the environment map as the navigation map of the agricultural robot, and autonomously navigating the agricultural robot based on the pose of the agricultural robot.

Compared with the prior art, the beneficial effects of the present invention are:

The present invention establishes an environmental map of the greenhouse environment by using an agricultural robot in the greenhouse environment through the three-dimensional laser radar installed in the initial stage of the crop growth cycle in the greenhouse environment, and calculates and obtains based on the environmental map, the greenhouse environment The pose of the visual label code between the ridges under the world coordinate system; based on the environment map and the pose of the visual label code relative to the world coordinate system, the positioning of the agricultural robot is visually aided; the environment map As a navigation map of the agricultural robot, the agricultural robot is navigated. A vision-assisted positioning method for agricultural robots suitable for fast-growing crops is obtained. Moreover, the present invention does not need to lay a navigation line and is basically not affected by the growth of crops and weeds, and can realize the autonomous positioning and navigation functions of the agricultural robot in the whole cycle of crop growth in a greenhouse environment.

Description of drawings

1 is a flow chart of a method according to an embodiment of the present invention;

2 is a schematic structural diagram of an agricultural robot according to an embodiment of the present invention;

3 is a schematic diagram of the arrangement position of visual label codes between ridges in a greenhouse environment according to an embodiment of the present invention;

4 is a flowchart of the specific implementation of step S1 in an embodiment of the present invention;

5 is a flowchart of the specific implementation of step S2 in an embodiment of the present invention;

FIG. 6 is a system structure diagram of an embodiment of the present invention.

Detailed ways

Exemplary embodiments of the present invention will now be described with reference to the accompanying drawings, however, the present invention may be embodied in many different forms and is not limited to the embodiments described herein, which are provided for the purpose of this thorough and complete disclosure invention, and fully convey the scope of the invention to those skilled in the art. The terms used in the exemplary embodiments shown in the drawings are not intended to limit the invention. In the drawings, the same elements/elements are given the same reference numerals.

Unless otherwise defined, terms (including scientific and technical terms) used herein have the commonly understood meanings to those skilled in the art. In addition, it is to be understood that terms defined in commonly used dictionaries should be construed as having meanings consistent with the context in the related art, and should not be construed as idealized or overly formal meanings.

Example 1:

The present invention proposes a positioning and navigation method for agricultural robots based on visual detection, as shown in Figure 1, including:

Step S1, in the initial stage of the crop growth cycle of the greenhouse environment, use the three-dimensional laser radar installed by the agricultural robot to establish an environmental map of the greenhouse environment, and calculate the visual label code between the greenhouse environment ridges based on the environmental map in the world. The pose in the coordinate system;

Step S2, determining the pose of the agricultural robot by a visual detection method based on the environment map and the pose of the visual tag code in the world coordinate system;

Step S3, using the environment map as a navigation map of the agricultural robot, and autonomously navigating the agricultural robot based on the pose of the agricultural robot.

Wherein, the agricultural robot used in step S1, as shown in FIG. 2, adopts crawler wheels to move, the center of the top of the vehicle body of the agricultural robot is installed with a three-dimensional laser radar, and a camera is installed on the liftable platform on the top of the vehicle body, An IMU attitude sensor is installed in the center of the chassis of the car body, and encoders are installed on the wheels on both sides of the car body.

In step S1, an environment map of the greenhouse environment is established with the three-dimensional laser radar installed by the agricultural robot, including:

The three-dimensional lidar emits multi-line laser light, and builds a 3D point cloud map of the greenhouse environment based on a three-dimensional mapping algorithm. 2D grid map for agricultural robot navigation;

The environment map includes: 3D point cloud map and 2D grid map.

The specific implementation steps of step S1, as shown in Figure 3, include:

Step SS21 In the initial construction of the map, it is only necessary to use the 3D lidar combined with the IMU and the encoder to perform the SLAM mapping work, and the robot (agricultural robot) needs to be manipulated to move between the ridges in the greenhouse environment until the construction of the entire greenhouse is completed. The 3D point cloud map is further converted into a 2D grid map, which is further provided for the subsequent navigation functions;

SS22: In the process of using multi-line lidar to build a map, turn on the camera to identify the visual label code in real time. Once the label code is identified, the relative positional relationship with the camera is calculated and recorded;

SS23: Use the laser SLAM algorithm to match the robot pose in real time and the relative position relationship between the label code and the camera obtained in step SS22, and calibrate the pose of the label code in the environment, that is, the robot's own coordinate system B released by the SLAM algorithm The TF to the world coordinate system W is converted into a homogeneous transformation matrix T _BW , and the relative positional relationship between the camera coordinate system C and the robot's own coordinate system B is actually measured, and the homogeneous transformation of the camera coordinate system C relative to the robot's own coordinate system B is obtained. The matrix T _CB , when the camera observes the label code, can calculate the homogeneous transformation matrix T _CM of the camera coordinate system C relative to the label code coordinate system M, and define the homogeneous transformation matrix of the label code coordinate system M relative to the world coordinate system W is T _MW , the calculation formula of T _MW is as follows;

T _MW = T _BW T _CB (T _CM ) ^-1

Extract the translation matrix and rotation matrix in the homogeneous transformation matrix T _BW to obtain the attitude P _M =(x _M , y _M , θ _M ) of the tag code in the world coordinate system;

SS24: Determine the number of label code data with the same ID, when it is greater than 100, execute SS25, otherwise jump to execute SS22;

SS25: This step calculates the mean value of the pose of the same ID tag code, and after the calculation, removes the value with a large difference and then obtains the mean value twice;

SS26: Save the average pose of the visual label code calculated above as the actual pose of the label code in the world coordinate system in the label code array.

In step S2, based on the environment map and the pose of the visual tag code in the world coordinate system, the positioning of the agricultural robot is visually aided, including:

The visual label code between the ridges in the greenhouse environment is recognized by the camera installed on the agricultural robot. The location of the visual label code in the greenhouse environment is shown in Figure 4. If the encoder recognizes the visual label code, the visual label stored in the label code array is called. The pose of the code in the world coordinate system is corrected, and the pose of the agricultural robot is corrected, that is, the positioning of the agricultural robot is visually aided.

In step S2, the visual label code between the ridges in the greenhouse environment is identified by the label code array installed by the agricultural robot. If the encoder does not recognize the visual label code, the agricultural robot is positioned by the encoder and the IMU attitude sensor.

The specific implementation of step S2 is shown in Figure 5, including:

SS31: Receive sensor data, including encoder, IMU and vision camera data;

SS32: Determine whether the data information of the visual label code is recognized, if not, execute SS33, if there is, execute SS34;

SS33: If the camera does not observe the label code at time t, calculate the difference Δθ between the attitude θ _k obtained by the visual label code and the attitude γ _k obtained by the IMU at time K (the moment when the visual label code was observed last time), at Δθ is added to the posture γ _t obtained by the IMU at time t as a correction to the posture of the IMU, and the posture of the robot at time t after the correction is θ _t =γ _t +Δθ. At the same time, record the left and right wheel speeds measured by the encoder at time t-1 as v _L and v _R , and calculate the speed v of the robot:

Further calculate the distance Δd moved by the robot from time t-1 to time t and the components Δx and Δy of Δd in the x and y directions:

Δd=v·Δt

Δx=Δd·cosθ _t-1

Δy=Δd·sinθ _t-1

Finally, calculate the position of the robot at time t x _t =x _t-1 +Δx, y _t =y _t-1 +Δy, then the posture of the robot at time t is P _t =(x _t ,y _t ,θ _t );

SS34: If the camera can observe the label code at time t, use the PnP algorithm to solve the rotation transformation matrix R and translation matrix t of the camera coordinate system C relative to the label code coordinate system M, and convert them into a homogeneous transformation matrix T _CM :

SS35: The homogeneous transformation matrix T _MW of the label code coordinate system M relative to the world coordinate system W can be obtained by using the calibration of the label code position in SS33. By actually measuring the relative positional relationship between the camera coordinate system C and the robot's own coordinate system B, obtain The homogeneous transformation matrix T _BC of the robot's own coordinate system B relative to the camera coordinate system C defines the homogeneous transformation matrix of the robot's own coordinate system relative to the world coordinate system as T _BW , then:

T _BW = T _MW T _CM T _BC

Finally, the translation matrix and rotation matrix in the homogeneous transformation matrix T _BW are extracted to obtain the pose P _t =(x _t , y _t , θ _t ) of the robot relative to the world at time t;

SS36: Publish the pose of the robot in the world coordinate system at time t.

In step S3, the environment map is used as the navigation map of the agricultural robot, and the agricultural robot is autonomously navigated based on the pose of the agricultural robot, including:

Call the environment map, and use the 2D grid map in the environment map as the navigation map;

The agricultural robot is navigated based on a navigation map and a navigation function package that invokes ROS.

Among them, the navigation function package includes: a pre-planned movement path of the agricultural robot between the ridges in the greenhouse environment.

Based on the same inventive concept, the present invention also proposes an agricultural robot positioning and navigation system 200 based on visual detection, as shown in FIG. 6 , including:

The data acquisition unit 201 is used for establishing an environmental map of the greenhouse environment by using the three-dimensional laser radar installed by the agricultural robot at the beginning of the crop growth cycle in the greenhouse environment, and calculating the vision between the ridges of the greenhouse environment based on the environmental map. The pose of the tag code in the world coordinate system;

The positioning unit 202 determines the pose of the agricultural robot through a visual detection method based on the environment map and the pose of the visual tag code in the world coordinate system;

The data collection unit 203 is configured to use the environment map as a navigation map of the agricultural robot, and autonomously navigate the agricultural robot based on the pose of the agricultural robot.

Compared with the prior art, the beneficial effects of the present invention are:

The present invention establishes an environmental map of the greenhouse environment by using an agricultural robot in the greenhouse environment through the three-dimensional laser radar installed in the initial stage of the crop growth cycle in the greenhouse environment, and calculates and obtains based on the environmental map, the greenhouse environment The pose of the visual label code between the ridges relative to the world coordinate system; based on the environment map and the pose of the visual label code in the world coordinate system, the positioning of the agricultural robot is visually aided; the environment map As a navigation map of the agricultural robot, the agricultural robot is navigated. A vision-assisted positioning method for agricultural robots suitable for fast-growing crops is obtained. Moreover, the present invention does not need to lay a navigation line and is basically not affected by the growth of crops and weeds, and can realize the autonomous positioning and navigation functions of the agricultural robot in the whole cycle of crop growth in a greenhouse environment.

As will be appreciated by one skilled in the art, embodiments of the present invention may be provided as a method, system, or computer program product. Accordingly, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) having computer-usable program code embodied therein.

The present invention is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each flow and/or block in the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to the processor of a general purpose computer, special purpose computer, embedded processor or other programmable data processing device to produce a machine such that the instructions executed by the processor of the computer or other programmable data processing device produce Means for implementing the functions specified in a flow or flow of a flowchart and/or a block or blocks of a block diagram.

These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory result in an article of manufacture comprising instruction means, the instructions The apparatus implements the functions specified in the flow or flow of the flowcharts and/or the block or blocks of the block diagrams.

These computer program instructions can also be loaded on a computer or other programmable data processing device to cause a series of operational steps to be performed on the computer or other programmable device to produce a computer-implemented process such that The instructions provide steps for implementing the functions specified in the flow or blocks of the flowcharts and/or the block or blocks of the block diagrams.

The above are only embodiments of the present invention, and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention are included in the invention pending approval. within the scope of the claims.

Claims (9)

1. An agricultural robot positioning and navigation method based on visual inspection is characterized by comprising the following steps:

at the beginning stage of a crop growth cycle of a greenhouse environment, establishing an environment map of the greenhouse environment by using a three-dimensional laser radar installed by the agricultural robot, and calculating the pose of a visual tag code among ridges of the greenhouse environment in a world coordinate system based on the environment map;

determining the pose of the agricultural robot through a visual detection method based on the environment map and the pose of the visual tag code in a world coordinate system;

and taking the environment map as a navigation map of the agricultural robot, and performing autonomous navigation on the agricultural robot based on the pose of the agricultural robot.

2. The positioning and navigation method according to claim 1, wherein the agricultural robot moves by using crawler wheels, a three-dimensional laser radar is installed in the center of the top of the vehicle body of the agricultural robot, a camera is installed on a lifting platform on the top of the vehicle body, an IMU attitude sensor is installed in the center of a chassis of the vehicle body, and encoders are installed on wheels on two sides of the vehicle body.

3. The positioning and navigation method according to claim 1, wherein the establishing of the environment map of the greenhouse environment by using the agricultural robot-mounted three-dimensional laser radar comprises:

emitting multi-line laser through the three-dimensional laser radar, constructing a 3D point cloud map of a greenhouse environment based on a three-dimensional mapping algorithm, projecting according to the 3D point cloud map and the height of ridges of the greenhouse environment, and determining a 2D grid map for navigation of the agricultural robot based on the projection;

the environment map comprises: a 3D point cloud map and a 2D grid map.

4. The method of claim 1, further comprising: and storing the pose of the visual tag code in the world coordinate system into the tag code array.

5. The positioning and navigation method according to claim 1, wherein the determining the pose of the agricultural robot by a visual detection method based on the pose of the environment map and the visual tag code in a world coordinate system comprises:

the method comprises the steps that a camera installed on the agricultural robot identifies visual label codes among ridges of a greenhouse environment, if the camera identifies the visual label codes, the position and pose of the visual label codes stored in a label code array under a world coordinate system are called, the position and pose of the agricultural robot are corrected through a visual detection method, and the position and pose of the agricultural robot are determined.

6. The positioning and navigation method according to claim 5, wherein the camera mounted by the agricultural robot recognizes a visual label code between ridges of the greenhouse environment, and if the camera does not recognize the visual label code, the agricultural robot is positioned by an encoder and an IMU attitude sensor.

7. The positioning and navigation method according to claim 1, wherein the navigating the agricultural robot by using the environment map as the navigation map of the agricultural robot comprises:

calling an environment map, and taking a 2D grid map in the environment map as a navigation map;

and navigating the agricultural robot based on the navigation map and the navigation function package calling the ROS.

8. The positioning and navigation method according to claim 7, wherein the navigation function package comprises: and (3) pre-planning the movement path of the agricultural robot among ridges of the greenhouse environment.

9. An agricultural robot positioning and navigation system based on visual inspection, characterized in that the system comprises:

the data acquisition unit is used for establishing an environment map of the greenhouse environment by using a three-dimensional laser radar installed by the agricultural robot at the beginning stage of the crop growth cycle of the greenhouse environment, and deducing the pose of the visual label codes among ridges of the greenhouse environment in a world coordinate system based on the environment map;

the positioning unit is used for determining the pose of the agricultural robot through a visual detection method based on the environment map and the pose of the visual tag code in a world coordinate system;

and the data acquisition unit is used for taking the environment map as a navigation map of the agricultural robot and carrying out autonomous navigation on the agricultural robot based on the pose of the agricultural robot.

Images