Picking robot positioning method, device and equipment, medium and product

Abstract

The invention discloses a picking robot positioning method, device and equipment, a medium and a product, and relates to the field of agriculture, and the method comprises the steps: obtaining the pose of Apriltag at each moment relative to a monocular camera according to an environment image, obtained by the monocular camera at each moment, of the position where a picking robot is located; obtaining relative poses of the picking robot at adjacent moments according to the environment point cloud data of the position of the picking robot obtained by the laser radar at each moment and the accelerated speed and the angular speed of the picking robot obtained by the IMU at each moment; constructing a factor graph; nodes of the factor graph comprise the global pose of the picking robot at each moment and the global pose of the Apriltag; the edge of the factor graph comprises the pose of the Apriltag at each moment relative to the monocular camera and the relative pose of the picking robot at each adjacent moment in the target time period; and performing graph optimization on the factor graph to obtain the global pose of the picking robot at the current moment. The method can improve the accuracy of a positioning result.

Description

Technical Field

[0001] The present application relates to the field of agriculture, and in particular to a method, device, equipment, medium and product for positioning a harvesting robot. Background Art

[0002] With the development of intelligent agriculture, higher requirements are placed on automated harvesting equipment, especially in farmland environments, where harvesting robots need to have high-precision, stable positioning and navigation functions. However, in related technologies, a single visual or lidar positioning solution is difficult to meet the complex requirements of farmland environments. Visual positioning is easily affected by occlusion, while lidar positioning accuracy is low and cannot provide stable global pose information, resulting in inaccurate final positioning results of existing harvesting robot positioning methods. Summary of the invention

[0003] The purpose of this application is to provide a picking robot positioning method, device, equipment, medium and product, which can improve the accuracy of positioning results.

[0004] To achieve the above objectives, this application provides the following solutions.

[0005] In the first aspect, the present application provides a picking robot positioning method, including: obtaining the position of Apriltag relative to the monocular camera at each moment in the target time period based on the environmental image of the picking robot's position acquired by the monocular camera at each moment in the target time period; the last moment in the target time period is the current moment.

[0006] The relative position and posture of the picking robot at each adjacent moment in the target time period are obtained based on the environmental point cloud data of the picking robot's position obtained by the lidar at each moment in the target time period and the acceleration and angular velocity of the picking robot obtained by the IMU at each moment in the target time period.

[0007] Construct a factor graph; the nodes of the factor graph include the global pose of the picking robot and the global pose of Apriltag at each moment in the target time period; the edges of the factor graph include: the pose of Apriltag relative to the monocular camera at each moment in the target time period and the relative pose of the picking robot at each adjacent moment in the target time period.

[0008] The factor graph is optimized to obtain the global pose of the picking robot at the current moment, thereby realizing the positioning of the picking robot.

[0009] In the second aspect, the present application provides a picking robot positioning device, including: a data processing module, used to obtain the position and posture of Apriltag relative to the monocular camera at each moment in the target time period according to the environmental image of the picking robot's position acquired by the monocular camera at each moment in the target time period; the last moment of the target time period is the current moment; the relative position and posture of the picking robot at each adjacent moment in the target time period is obtained according to the environmental point cloud data of the picking robot's position acquired by the lidar at each moment in the target time period and the acceleration and angular velocity of the picking robot acquired by the IMU at each moment in the target time period.

[0010] A graph optimization module is used to construct a factor graph; the nodes of the factor graph include the global pose of the picking robot at each moment in the target time period and the global pose of Apriltag; the edges of the factor graph include: the pose of Apriltag relative to the monocular camera at each moment in the target time period and the relative pose of the picking robot at each adjacent moment in the target time period; the factor graph is optimized to obtain the global pose of the picking robot at the current moment, so as to realize the positioning of the picking robot.

[0011] In a third aspect, the present application provides a computer device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement any of the above-described picking robot positioning methods.

[0012] In a fourth aspect, the present application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements any of the above-described picking robot positioning methods.

[0013] In a fifth aspect, the present application provides a computer program product, including a computer program, which, when executed by a processor, implements any of the above-mentioned picking robot positioning methods.

[0014] According to the specific embodiments provided in the present application, the present application has the following technical effects: the present application provides a method, device, equipment, medium and product for positioning a picking robot. A single visual or lidar positioning solution is difficult to meet the complex requirements of the farmland environment. Visual positioning is easily affected by occlusion, and the lidar positioning accuracy is low and cannot provide stable global pose information, resulting in inaccurate final positioning results of existing picking robot positioning methods. The present application is based on multi-sensor data fusion and effectively solves the technical problem of unstable positioning of a single sensor by using a combination of visual tags Apriltag, lidar and IMU, thereby improving the accuracy of the positioning results of the picking robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0016] Figure 1 A schematic flow chart of a picking robot positioning method provided in one embodiment of the present application.

[0017] Figure 2 A schematic diagram of the functional modules of a picking robot positioning device provided in one embodiment of the present application.

[0018] Figure 3 A schematic diagram of the structure of a computer device provided in one embodiment of the present application. DETAILED DESCRIPTION

[0019] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0020] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0021] In an exemplary embodiment, Figure 1 As shown, a picking robot positioning method is provided, including the following steps.

[0022] Step 201: Obtain the position of Apriltag relative to the monocular camera at each moment in the target time period according to the environment image of the picking robot's position acquired by the monocular camera at each moment in the target time period; the last moment in the target time period is the current moment.

[0023] Step 202: Obtain the relative position and posture of the picking robot at each adjacent moment in the target time period based on the environmental point cloud data of the picking robot's position acquired by the lidar at each moment in the target time period and the acceleration and angular velocity of the picking robot acquired by the IMU (Inertial Measurement Unit) at each moment in the target time period.

[0024] Step 203: construct a factor graph; the nodes of the factor graph include the global pose of the picking robot at each moment in the target time period (to be solved) and the global pose of Apriltag (representing the fixed pose of Apriltag in the global coordinate system, which can be measured by external tools such as laser rangefinders when the environment is arranged. The pose of Apriltag in the environment is fixed, and its global pose does not change with time, so it is known prior data); the edges of the factor graph include: the pose of Apriltag relative to the monocular camera at each moment in the target time period and the relative pose of the picking robot at each adjacent moment in the target time period. In graph optimization, nodes can be considered as variables, so this application uses the global pose of the robot and the global pose of Apriltag as nodes in the factor graph.

[0025] Step 204: Optimize the factor graph to obtain the global pose of the picking robot at the current moment, and realize the positioning of the picking robot. Through graph optimization, the pose information of the robot relative to the global coordinate system is obtained for further navigation and path planning.

[0026] Implementing the above steps 201 to 204 can improve the accuracy of the positioning result.

[0027] In another exemplary embodiment of the present application, the executor of the present application is the robot system controller jetsontx2.

[0028] In another exemplary embodiment of the present application, the position and posture of Apriltag relative to the monocular camera at each moment in the target time period is obtained based on the environmental image of the picking robot's position acquired by the monocular camera at each moment in the target time period, specifically including: for the t-th moment in the target time period, the Apriltag algorithm is used to process the environmental image of the picking robot's position acquired by the monocular camera at the t-th moment to obtain the position and posture of Apriltag relative to the monocular camera at the t-th moment.

[0029] In another exemplary embodiment of the present application, the relative pose of each adjacent moment in the target time period of the picking robot is obtained according to the environmental point cloud data of the position of the picking robot obtained by the laser radar at each moment in the target time period and the acceleration and angular velocity of the picking robot obtained by the IMU at each moment in the target time period, specifically including: for the t-th moment in the target time period, the FastLIO2 (Fast LiDAR-Inertial Odometry 2) algorithm is used to process the environmental point cloud data of the position of the picking robot obtained by the laser radar at the t-th moment and the acceleration and angular velocity of the picking robot obtained by the IMU at the t-th moment, and obtain the relative pose of the picking robot at the t-th moment and the t-1-th moment. FastLIO2 is an open source LiDAR-Inertial Odometry (LIO) algorithm with the characteristics of real-time, high precision and lightweight.

[0030] In another exemplary embodiment of the present application, the factor graph is optimized to obtain the global pose of the picking robot at the current moment, specifically including: using the ISAM2 algorithm to optimize the factor graph to obtain the global pose of the picking robot at the current moment.

[0031] Graph optimization is a nonlinear optimization method based on factor graphs, which is widely used in robot positioning and navigation. By modeling the pose information and observation information as nodes and edges in the graph, and using the optimization algorithm to minimize the edge error, the global pose result is finally optimized. In the factor graph, the edge is used to describe the constraint relationship between two nodes. Therefore, this application uses the global pose of the robot with unknown nodes to optimize the pose of the robot. And the global pose of the known node Apriltag Create two constraint relationships.

[0032] One is the odometer pose edge, using the constraint equation Indicates. Among them, Represents the Lie group Convert to Lie algebra The corresponding six-dimensional vector function is Representation calculation The transpose of Indicates that Lie Group Convert to Lie algebra The corresponding six-dimensional vector is, Representation calculation The inverse matrix of Representation calculation The covariance matrix of represents the odometer pose edge at the i-th moment, Representation calculation The inverse matrix of is the global pose of the picking robot at the i-th moment, is the global position of the picking robot at the i-1th moment, representing the variables of the two moment nodes, It is the odometer pose, i.e., the algorithm output of Fastlio2, which indicates the relative pose of the picking robot at the i-1th moment and the ith moment.

[0033] The other is the Apriltag relative robot pose edge, using the constraint equation It indicates that, represents the position edge of Apriltag relative to the robot at the i-th moment, Representation calculation The transpose of Indicates that Lie Group Convert to Lie algebra The corresponding six-dimensional vector function, Representation calculation The inverse matrix of Representation calculation The covariance matrix of represents the global pose of Apriltag, Representation calculation The inverse matrix of represents the position of Apriltag relative to the picking robot coordinate system at the i-th moment, , is the position of Apriltag relative to the monocular camera at the i-th moment, Represents the pose from the camera to the picking robot coordinate system. This is the external parameter of the monocular camera and does not change over time. The specific parameters are determined by the mechanical structure before the algorithm runs. The odometer pose edge provides motion constraints between consecutive moments of the robot to ensure the continuity and real-time update of the trajectory; the Apriltag relative robot pose edge provides the relative pose constraints between the robot and the global reference Apriltag, which helps to correct the drift of the odometer estimate. Pose optimization: Establish a nonlinear least squares objective function to optimize the global pose of the picking robot at all times: in, It represents the global pose of the picking robot at each moment in the target time period, which is the set of the global poses of the k picking robots from the initial moment to the current moment. , . Since the optimization needs to be performed in real time, and as time goes by, the number of edges that need to be optimized increases, that is, k increases, and ordinary optimization algorithms are difficult to optimize in real time. Since the ISAM2 algorithm can achieve real-time graph optimization based on the incremental update characteristics of the Bayesian tree, the ISAM2 algorithm in the GTSAM library is selected for real-time graph optimization calculation. The input of the ISAM2 algorithm is the nodes and edges of the graph, and the above nonlinear least squares problem is optimized. The output is the global position of the picking robot at all times. , which includes the global position of the picking robot at the current moment That is, in another exemplary embodiment of the present application, the ISAM2 algorithm is used to optimize the factor graph to obtain the global posture of the picking robot at the current moment, specifically: the ISAM2 algorithm is used to optimize the factor graph to obtain the global posture of the picking robot at the current moment. The global position of the picking robot at the current moment is obtained by optimization and solution, where k represents the current moment.

[0034] In another exemplary embodiment of the present application, the monocular camera and the laser radar are both arranged on the picking robot, and the laser radar has the IMU built in. Specifically, the laser radar shown is a MID360 laser radar, which obtains point cloud data of the surrounding environment, and the MID360 laser radar has a built-in IMU to obtain its own motion information (acceleration of the picking robot). and angular velocity ).

[0035] This application combines the environmental information obtained by the monocular camera and the lidar (with built-in IMU), uses the global positioning of the visual tag Apriltag and the relative positioning of the FastLIO2 odometer fused with the lidar and IMU to achieve real-time high-precision global positioning of the robot in the agricultural environment. This method is mainly used in the navigation and positioning system of automated harvesting equipment.

[0036] Compared with the existing single-sensor positioning method, this application achieves the following advantages through the fusion positioning method of vision and lidar sensors.

[0037] 1. High-precision positioning: The robot uses the Apriltag visual recognition method to calculate the pose and combines it with LiDAR + IMU to maintain high positioning accuracy in complex farmland environments.

[0038] 2. Strong real-time performance: Using ISAM2 graph optimization algorithm for real-time calculation, the robot can instantly update its global position during navigation.

[0039] 3. Good stability: Even when the field of vision is easily blocked, the robot position can still be calculated through the point cloud data of the lidar and the IMU's own motion data (the acceleration and angular velocity of the harvesting robot), making the overall positioning solution more environmentally adaptable.

[0040] Based on the same inventive concept, the embodiment of the present application also provides a picking robot positioning device for implementing the picking robot positioning method involved above. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in one or more picking robot positioning device embodiments provided below can refer to the limitations of the picking robot positioning method above, and will not be repeated here.

[0041] In an exemplary embodiment, Figure 2 As shown, a picking robot positioning device is provided, including: a data processing module, used to obtain the position and posture of Apriltag relative to the monocular camera at each moment in the target time period according to the environmental image of the picking robot's position obtained by the monocular camera at each moment in the target time period; the last moment of the target time period is the current moment; the relative position and posture of the picking robot at each adjacent moment in the target time period is obtained according to the environmental point cloud data of the picking robot's position obtained by the lidar at each moment in the target time period and the acceleration and angular velocity of the picking robot obtained by the IMU at each moment in the target time period.

[0042] A graph optimization module is used to construct a factor graph; the nodes of the factor graph include the global pose of the picking robot at each moment in the target time period and the global pose of Apriltag; the edges of the factor graph include: the pose of Apriltag relative to the monocular camera at each moment in the target time period and the relative pose of the picking robot at each adjacent moment in the target time period; the factor graph is optimized to obtain the global pose of the picking robot at the current moment, so as to realize the positioning of the picking robot.

[0043] In an exemplary embodiment, the picking robot positioning device also includes: a data acquisition module, the data acquisition module includes a monocular camera, a MID360 laser radar and a built-in IMU.

[0044] In an exemplary embodiment, the data processing module includes an Apriltag recognition and posture solution module and a FastLIO2 mileage calculation module.

[0045] The Apriltag identification and pose solving module is used to obtain the pose of the Apriltag relative to the monocular camera at each moment in the target time period according to the environmental image of the picking robot's position obtained by the monocular camera at each moment in the target time period.

[0046] The FastLIO2 mileage calculation module is used to obtain the relative posture of the picking robot at each adjacent moment within the target time period based on the environmental point cloud data of the picking robot's position obtained by the lidar at each moment within the target time period and the acceleration and angular velocity of the picking robot obtained by the IMU at each moment within the target time period.

[0047] In an exemplary embodiment, the graph optimization module performs graph optimization on the factor graph based on the ISAM2 algorithm in the GTSAM library to obtain the global pose of the picking robot at the current moment.

[0048] In an exemplary embodiment, a computer device is provided. The computer device may be a server or a terminal. The internal structure diagram thereof may be as follows: Figure 3 As shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, referred to as I / O) and a communication interface. The processor, the memory and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store the positioning data of the picking robot. The input / output interface of the computer device is used to exchange information between the processor and an external device. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a picking robot positioning method is implemented.

[0049] Those skilled in the art will understand that Figure 3 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components. In an exemplary embodiment, a computer device is provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor implements the above-mentioned method embodiments when executing the computer program.

[0050] In an exemplary embodiment, a computer-readable storage medium is provided, storing a computer program, and when the computer program is executed by a processor, the above-mentioned method embodiments are implemented.

[0051] In an exemplary embodiment, a computer program product is provided, including a computer program, and when the computer program is executed by a processor, the above-mentioned method embodiments are implemented.

[0052] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with relevant regulations.

[0053] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM may be in various forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM).

[0054] The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. The non-relational database may include a distributed database based on blockchain, etc., but is not limited thereto. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but is not limited thereto.

[0055] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0056] This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core ideas of this application. At the same time, for those skilled in the art, according to the ideas of this application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A picking robot positioning method, characterized in that: The picking robot positioning method comprises: The position and posture of Apriltag relative to the monocular camera at each moment in the target time period are obtained according to the environmental image of the picking robot's position obtained by the monocular camera at each moment in the target time period; the last moment in the target time period is the current moment; The relative position and posture of the picking robot at each adjacent moment in the target time period are obtained based on the environmental point cloud data of the picking robot's position obtained by the laser radar at each moment in the target time period and the acceleration and angular velocity of the picking robot obtained by the IMU at each moment in the target time period; Construct a factor graph; the nodes of the factor graph include the global pose of the picking robot and the global pose of Apriltag at each moment in the target time period; the edges of the factor graph include: the pose of Apriltag relative to the monocular camera at each moment in the target time period and the relative pose of the picking robot at each adjacent moment in the target time period; The factor graph is optimized to obtain the global pose of the picking robot at the current moment, thereby realizing the positioning of the picking robot.

2. The picking robot positioning method according to claim 1, characterized in that: According to the environmental image of the picking robot's location acquired by the monocular camera at each moment in the target time period, the position and posture of Apriltag relative to the monocular camera at each moment in the target time period are obtained, specifically including: For the tth moment in the target time period, the Apriltag algorithm is used to process the environmental image of the picking robot's location obtained by the monocular camera at the tth moment to obtain the position and pose of Apriltag relative to the monocular camera at the tth moment.

3. The picking robot positioning method according to claim 1, characterized in that: The relative position and posture of the picking robot at each adjacent moment in the target time period are obtained based on the environmental point cloud data of the picking robot's position obtained by the lidar at each moment in the target time period and the acceleration and angular velocity of the picking robot obtained by the IMU at each moment in the target time period, including: For the tth moment in the target time period, the FastLIO2 algorithm is used to process the environmental point cloud data of the picking robot's position obtained by the lidar at the tth moment and the acceleration and angular velocity of the picking robot obtained by the IMU at the tth moment to obtain the relative pose of the picking robot at the tth moment and the t-1th moment.

4. The picking robot positioning method according to claim 1, characterized in that: The factor graph is optimized to obtain the global posture of the picking robot at the current moment, specifically including: The ISAM2 algorithm is used to optimize the factor graph to obtain the global pose of the picking robot at the current moment.

5. The picking robot positioning method according to claim 4, characterized in that: The ISAM2 algorithm is used to optimize the factor graph to obtain the global pose of the picking robot at the current moment, specifically: Using ISAM2 algorithm The global position of the picking robot at the current moment is obtained by optimization, where: represents the global position of the picking robot at each moment in the target time period, k represents the current moment, Representation calculation The transpose of Indicates that Lie Group Convert to Lie algebra The corresponding six-dimensional vector function is represents the global pose of Apriltag, Representation calculation The inverse matrix of represents the global pose of the picking robot at the i-th moment, represents the position of Apriltag relative to the picking robot coordinate system at the i-th moment, , represents the external parameters of the monocular camera, represents the position of Apriltag relative to the monocular camera at the i-th moment, Representation calculation The inverse matrix of represents the global position of the picking robot at the i-1th moment, represents the relative position of the picking robot at the i-th moment and the i-1-th moment, Representation calculation The inverse matrix of Representation calculation The covariance matrix of Representation calculation The transpose of Indicates that Lie Group Convert to Lie algebra The corresponding six-dimensional vector function is Representation calculation The inverse matrix of Representation calculation The covariance matrix of .

6. The picking robot positioning method according to claim 1, characterized in that: The monocular camera and the laser radar are both arranged on the picking robot, and the laser radar has the IMU built in.

7. A picking robot positioning device, characterized in that: The picking robot positioning device comprises: The data processing module is used to obtain the position and posture of Apriltag relative to the monocular camera at each moment in the target time period according to the environmental image of the picking robot's position obtained by the monocular camera at each moment in the target time period; the last moment in the target time period is the current moment; the relative position and posture of the picking robot at each adjacent moment in the target time period is obtained according to the environmental point cloud data of the picking robot's position obtained by the lidar at each moment in the target time period and the acceleration and angular velocity of the picking robot obtained by the IMU at each moment in the target time period; A graph optimization module is used to construct a factor graph; the nodes of the factor graph include the global pose of the picking robot at each moment in the target time period and the global pose of Apriltag; the edges of the factor graph include: the pose of Apriltag relative to the monocular camera at each moment in the target time period and the relative pose of the picking robot at each adjacent moment in the target time period; the factor graph is optimized to obtain the global pose of the picking robot at the current moment, so as to realize the positioning of the picking robot.

8. A computer device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the picking robot positioning method according to any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the picking robot positioning method described in any one of claims 1 to 6 is implemented.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the picking robot positioning method described in any one of claims 1 to 6 is implemented.

Images