Method and system for generating picking operation waypoint of unmanned aerial vehicle

Abstract

The invention relates to the technical field of artificial intelligence, in particular to an unmanned aerial vehicle picking operation waypoint generation method and system, and the method comprises the steps: constructing a combined speedometer based on an FAST-LIO2 algorithm; constructing an information synchronization mechanism; acquiring a global picking point with a timestamp; constructing a global acquisition map; determining the position and orientation of a picking point; and determining a picking waypoint to guide the unmanned aerial vehicle to pick. In the dynamic picking operation of the unmanned aerial vehicle, the picking waypoint is provided for the unmanned aerial vehicle in advance, and the picking efficiency of the unmanned aerial vehicle is effectively improved.

Description

Technical Field

[0001] The present invention relates to the field of artificial intelligence technology, and in particular to a method and system for generating waypoints for unmanned aerial vehicle harvesting operations. Background Art

[0002] As the agricultural labor force population decreases, the use of agricultural robots to replace manual labor has become a trend, and drone-type harvesting robots have also been used and promoted in recent years. The advantage of drones is that they are not restricted by terrain and have a wide range of motion. In the long run, using drones for harvesting can help reduce labor costs.

[0003] When a drone dynamically picks fruits without a global view, the drone's onboard camera cannot cover all the fruits, and the drone needs to move before detecting and identifying the fruits. This wastes time and increases costs to a certain extent.

[0004] In the existing technology, visual mileage calculation method is often used through camera sensors to build data maps of agricultural scenes. However, the effect of visual mileage calculation method is greatly affected by changes in ambient light. In the harvesting operation scene, the camera is easily blocked by obstacles such as leaves, which will directly lead to the failure of visual mileage calculation method. Although drone harvesting technology has made significant progress, its recognition accuracy in terms of object recognition and spatial positioning still needs to be improved. Summary of the invention

[0005] 1. Technical issues to be resolved

[0006] The main purpose of the present invention is to provide a method and system for generating waypoints for drone picking operations to solve the above problems.

[0007] (II) Technical solution

[0008] To achieve the above object, the present invention provides a method for generating waypoints for a UAV picking operation, comprising the steps of:

[0009] S1, build a combined odometer based on the FAST-LIO2 algorithm, including:

[0010] S11, using the FAST-LIO2 algorithm to process the data of the laser radar sensor and the inertial measurement unit to obtain a point cloud map with a timestamp T based on the ROS time;

[0011] S12, obtaining image information collected by the camera and carrying a timestamp T based on the ROS time;

[0012] S2, constructing an information synchronization mechanism to realize external parameter calibration and information synchronization between the camera sensor and the lidar sensor, including:

[0013] S21, through calibration, obtain the transformation matrix from the camera coordinate system to the lidar coordinate system The image information is converted into image information based on a laser radar coordinate system;

[0014] S22, based on the ROS timestamp and using the buffer queue combined with the NLerp interpolation method, after time synchronization of the odometer information provided by the laser radar sensor and the image information collected by the camera, obtain the odometer information provided by the synchronized laser radar sensor. n Image information collected by the synchronized camera I n , and output to the synchronization queue;

[0015] S3, obtaining a global picking point with a timestamp, including: based on the image information obtained in step S12 and the union information in the synchronization queue obtained in step S2, identifying and detecting the picking point to be picked, so as to obtain the picking coordinate point p in the image coordinate system pixel and the corresponding timestamp ts; the picking coordinate point p in the image coordinate system pixel Transformed into the picking point p in the global coordinate system gobal and combined with the corresponding timestamp ts to output the global picking point set P ts ;

[0016] S4, construct a point cloud map based on the data structure of the kd tree with a flag and a global picking point set P ts , as a global acquisition map;

[0017] S5, determining the location and orientation of the picking point, including:

[0018] S51, performing credibility analysis on the picking point positions of the same object to be picked according to the global collection map, obtaining the position with the highest credibility, and generating the final picking point position of each object to be picked;

[0019] S52, determining the direction of the picking point according to the global collection map and the final picking point position;

[0020] S6, determining a picking waypoint according to the final picking point position and the direction of the picking point of each object to be picked.

[0021] Preferably, the step S51 includes:

[0022] S511, calculate the density estimate of the picking point p in its neighborhood:

[0023]

[0024] Among them, h is the average width of the object to be collected, Pts In the field of picking point p, is a sphere with the picking point p as the center and radius h, ts p is the corresponding timestamp, K(u) is the Gaussian kernel function:

[0025] S512, calculate the gradient function Determine the direction of movement:

[0026]

[0027] Among them, c k,3 represents the regularization parameter, K′(u) represents the derivative of the Gaussian kernel function;

[0028] Introducing the function g(s)=-K′(S), we get:

[0029]

[0030] S513, calculate the movement vector m h (p), determine the new position:

[0031]

[0032] S514, update the location and timestamp of the picking point. h (p) = 0 stop:

[0033]

[0034] Preferably, the step S52 includes:

[0035] S521, define collision area N D , based on the data stored in the data structure of the kd tree with flags, find the field of picking point p Point o i , yes i Decentralize to get o′ i , and construct the covariance matrix C:

[0036]

[0037] S522, using singular value decomposition to solve the minimum eigenvalue, the singular value decomposition solves the minimum eigenvalue of the covariance matrix C; the eigenvector corresponding to the minimum eigenvalue obtained by principal component analysis is the normal vector n of the picking point p o ;

[0038] S523, using the constraint point to determine the direction of the normal vector, the constraint point specifically refers to the odometer point of the timestamp corresponding to the picking point p, and searching for a point with the same timestamp in the odometer queue through the timestamp. p , and then use the following formula to determine whether the normal vector n needs to be flipped o :

[0039] O p =(x i ,y i ,z i ),n o =(x j ,y j ,z j )

[0040]

[0041] Among them, d(O p ,Dn o ) represents point O p To along n o The distance from the point in the direction D;

[0042] S524, calculating the yaw angle ψ based on the XY plane: the normal vector n o Projection to the XY plane gives the yaw angle ψ of the waypoint:

[0043]

[0044] Preferably, the data structure of the kd tree with flags includes: point type PointType point, used to store point coordinates, intensity and other information; pointer leftnode pointing to the left child node; pointer rightnode pointing to the right child node; integer variable partition_axis, used to represent the partition axis; floating-point variable timestamp, used to store the corresponding time information of the global sampling point; floating-point array range, used to store the bounding box vertices of the global sampling point and Boolean variable filter, used to filter the laser radar point cloud that is not necessarily processed, wherein flag has three status bits: obstacle point, picking point and traversed picking point.

[0045] Preferably, the step S1 further includes:

[0046] Step S13, using the ScanContext method to reduce the error of the point cloud map of the timestamp T based on the ROS time, and optimizing it through the ICP algorithm and the GTSAM library;

[0047] S131, point cloud data storage and matching, based on the current frame point cloud matrix I qAnd the historical frame point cloud matrix I c Compute the cosine distance between corresponding column vectors:

[0048]

[0049] Among them, I q Represents the current point cloud matrix, I c Represents a point cloud matrix in the history queue, N s represents the number of matrix columns, Represents the matrix I q The jth column of Represents the matrix I c The jth column of

[0050] Set the threshold τ, when d(I q ,I c )<τ, then the two matrices are judged to be similar; the current frame point cloud matrix I q The corresponding original point cloud matrix P k With the historical frame point cloud matrix I c The corresponding original point cloud matrix To match:

[0051]

[0052] S132, using the ICP algorithm to solve the pose constraints, based on step S131 to obtain the matched original point cloud P k and Solving pose constraints

[0053]

[0054] Optimization objective function:

[0055]

[0056] S133, optimizing the pose constraints, inputting the pose in the world coordinate system output by the FAST-LIO2 algorithm and the pose constraints calculated by the ICP into the GTSAM library for optimization, so as to update the map and trajectory.

[0057] Preferably, the step S21 comprises: converting the point in the camera coordinate system to the laser radar coordinate system:

[0058]

[0059] Among them, [X L ,Y L ,Z L ] T represents a point in the laser radar coordinate system, [XC ,Y C ,Z C ] T Represents a point in the camera coordinate system.

[0060] Preferably, the step S22 includes: storing the camera sensor image information in the image information queue, storing the odometer information in the odometer queue, and using the buffer queue in combination with the NLerp linear interpolation method to achieve time synchronization, including:

[0061] Take the earliest timestamp ts=n from the odometer queue 1 Odometer information O m , check the earliest image information I in the image information queue n , if O m The time is later than I n , then discard I n , if O m The time is equal to I n , then the combination O m and I n Two data are output to the synchronization queue; if O m Earlier than I n , then find the first time greater than I from the odometer queue n O n-1 , and the first time later than I n O n+1 , and then perform NLep interpolation to calculate O n , combination O n and I n Output to the synchronization queue.

[0062] Preferably, the identification and detection of the picking points of the objects to be collected include identification and detection of the picking points of the objects to be collected through a YOLO-v8 network.

[0063] Preferably, in step S3, the picking coordinate point p is obtained in the image coordinate system. pixel And the corresponding timestamp, it also includes:

[0064] S31, the p pixel Convert to the picking point p in the global coordinate system gobal ,include:

[0065] S311, conversion from pixel coordinates to camera coordinate system:

[0066]

[0067] in, represents pixel coordinates, u i Represents the horizontal coordinate of the image point, vi Represents the vertical coordinate of the image point;

[0068] S312, conversion from camera coordinate system to lidar coordinate system:

[0069]

[0070] Among them, f x ,f y represents the focal length of the camera, c x ,c y Represents the principal point coordinates of the camera, Z i The corresponding pixel The depth value of is the rotation matrix between the camera coordinate system and the lidar sensor coordinate system, is the translation vector between the camera coordinate system and the lidar sensor coordinate system;

[0071] S313, conversion from lidar coordinate system to global coordinate system:

[0072]

[0073] in, is the rotation matrix between the lidar coordinate system and the global coordinate system, is the translation vector between the lidar coordinate system and the global coordinate system;

[0074] S32, the picking point p in the global coordinate system gobal Combined with the corresponding timestamp to form new data Add to the global picking point set P with timestamp ts middle.

[0075] The present invention also provides a system for generating waypoints for drone picking operations, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the system implements the steps of the method for generating waypoints for drone picking operations as described in any one of the above items.

[0076] (III) Beneficial effects

[0077] The invention provides a method for generating waypoints for drone picking operations. By using the improved FAST-LIO2 algorithm and Yolo-v8 network, the global picking map in the drone coordinate system is constructed, and reasonable waypoints for drone flight are generated based on the map. The drone can realize dynamic and continuous picking operations, reduce the detection time caused by field of view limitations, and improve the picking efficiency of the drone.

[0078] The present invention provides a method for generating waypoints for drone picking operations, which is based on a fusion solution of multi-sensor information of laser radar, inertial measurement unit, and camera multi-sensors, and performs corresponding processing on different sensor data, thereby improving the accuracy of method positioning and the robustness of method effects, and better coping with complex real-world environments.

[0079] The present invention provides a method for generating waypoints for unmanned aerial vehicle picking operations, which uses a kd-tree data structure with a flag to store point cloud maps and global sampling point information with timestamps, thereby improving the efficiency of storing and processing multi-source data and facilitating the subsequent generation accuracy and efficiency of picking waypoints. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] Figure 1 A schematic diagram of a flow chart of a method for generating waypoints for a drone harvesting operation provided by an embodiment of the present invention;

[0081] Figure 2 A schematic diagram of a SLAM system flow based on FASTLIO2 and GTSAM provided in one embodiment of the present invention;

[0082] Figure 3 A schematic diagram of a flow chart of matching odometer information and image information provided by an embodiment of the present invention;

[0083] Figure 4 A schematic diagram of a pseudo-algorithm of a data structure of a kd-tree with a flag provided in one embodiment of the present invention;

[0084] Figure 5 A schematic diagram of the hardware structure of a method for generating waypoints for drone harvesting operations provided in one embodiment of the present invention. DETAILED DESCRIPTION

[0085] In order to better explain the present invention and facilitate understanding, the present invention is described in detail below through specific implementation modes in conjunction with the accompanying drawings.

[0086] It should be noted that all directional indications in the embodiments of the present invention (such as up, down, left, right, front, back, etc.) are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0087] In addition, in the present invention, descriptions such as "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" or "second" may explicitly or implicitly include at least one of the features. In the description of the present invention, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0088] In the present invention, unless otherwise clearly specified and limited, the terms "connection", "fixation", etc. should be understood in a broad sense. For example, "fixation" can be a fixed connection, a detachable connection, or an integral connection; "connection" can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, and it can be the internal connection of two elements or the interaction relationship between two elements, unless otherwise clearly defined. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0089] like Figure 1 As shown, in this embodiment, a method for generating waypoints for drone picking operations is provided. In this embodiment, the drone may be, for example, an unmanned aerial vehicle (UAV), which is an aircraft that can be remotely controlled or autonomously flown without a human on board. The waypoint is a specific point on the drone's flight route, which is used to define the flight path. The method for generating waypoints for drone picking operations includes the steps of:

[0090] S1, construct a combined odometer based on the FAST-LIO2 algorithm (Fast LiDAR-Inertial Odometry 2). Compared with the single visual odometer calculation method, this embodiment uses a laser radar sensor (LiDAR) to solve the problem that the positioning algorithm is affected by sensor occlusion, and can provide real-time posture for the camera; Figure 2 As shown, the SLAM system based on FASTLIO2 and GTSAM obtains positioning information, and the construction of a combined odometer based on the FAST-LIO2 algorithm includes:

[0091] S11, using the FAST-LIO2 algorithm to process the data of the laser radar sensor and the inertial measurement unit (IMU), and obtaining a point cloud map with a timestamp T based on the ROS (Robot Operating System) time; the FAST-LIO2 algorithm is a version of the simultaneous localization and mapping (SLAM) algorithm based on the fusion of laser radar sensor and inertial measurement unit data. It inherits and develops the idea of ​​FAST-LIO and aims to provide faster and more accurate positioning and mapping capabilities. FAST-LIO2 takes advantage of the advantages of both laser radar sensors and inertial measurement units. The laser radar sensor can provide high-precision distance measurement, while the inertial measurement unit can provide high-frequency attitude and acceleration information. The combination of the two can make up for the shortcomings of a single sensor. In this embodiment, the laser radar sensor and the inertial measurement unit can obtain a large amount of positioning-related data, including posture information.

[0092] S12, acquiring image information collected by the camera and carrying a timestamp T based on the ROS time; in this embodiment, the acquired image information can be selected according to actual conditions, and can be, for example, an RGB image and a grayscale image. Specifically, in this embodiment, the acquired image information is an RGB image;

[0093] S2, constructing an information synchronization mechanism to realize external parameter calibration and information synchronization between the camera sensor and the lidar sensor, including:

[0094] S21, through calibration, obtain the transformation matrix from the camera coordinate system to the lidar coordinate system To obtain the image information converted into image information based on the laser radar coordinate system; the external parameter calibration method can be set according to the actual situation, for example, a method based on a chessboard or a special calibration plate, a method based on natural feature points, and an automated online calibration method. Specifically, the external parameter calibration method in this embodiment is an open source calibration method named livox_camare_calib proposed by the University of Hong Kong based on the Livox series radar;

[0095] S22, based on the ROS timestamp and using the buffer queue combined with the NLerp interpolation method (Normalized Linear Interpolation), the odometer information provided by the laser radar sensor and the image information collected by the camera are time synchronized to obtain the odometer information provided by the synchronized laser radar sensor. n Image information collected by the synchronized camera I n, and output to the synchronization queue; wherein, the ROS system is a flexible framework for robot development. It contains a series of tools, libraries and conventions designed to simplify the development of robot applications; the NLerp interpolation method is a variant of linear interpolation, in which the weights in the interpolation process are normalized.

[0096] S3, obtaining a global picking point with a timestamp, including: based on the image information obtained in step S12 and the union information in the synchronization queue obtained in step S2, identifying and detecting the picking point to be picked, so as to obtain the picking coordinate point p in the image coordinate system pixel And the corresponding timestamp ts, the picking coordinate point p in the image coordinate system pixel Transformed into the picking point p in the global coordinate system gobal and combined with the corresponding timestamp ts to output the global picking point set P ts ; wherein the image information is the posture information provided by the odometer based on the step S1, the information of the union information in the synchronization queue is the sensor data after synchronization in the step S2, the identification and detection of the picking point of the object to be picked are performed using the YOLO (You Only Look Once) network, which is a popular target detection algorithm that can quickly and accurately identify the target object in the image, wherein the object to be picked can be set according to the actual situation, and can be, for example, fruit. Specifically, in the present embodiment, the object to be picked is an apple.

[0097] S4, construct a point cloud map based on the data structure of the kd tree with a flag and a global picking point set P ts , as a global acquisition map; using a kd tree data structure with a flag bit to store the map information obtained in step S1 and the sampling point information obtained in step S2 for subsequent access and management, wherein the information obtained in step S1 includes point cloud map information, and the information obtained in step S2 includes global sampling point information with a timestamp; the kd tree data structure with a flag bit is an optimized kd tree structure, also known as an Fkd-Tree data structure, which can process large-scale point cloud data and reduce the time required for searching.

[0098] S5, determining the position and orientation of the picking point, including:

[0099] S51, according to the global collection map, the credibility analysis of the picking point position of the same object to be collected is performed to obtain the position with the highest credibility, and the final picking point position of each object to be collected is generated; wherein, the credibility analysis includes: density analysis and optimization of the picking point position, based on the global collection map of step S4, the global picking point is processed by the SameWayMeanShift algorithm to find the optimal picking point position, wherein the SameWayMeanShift algorithm is an optimization of the traditional MeanShift algorithm (mean shift algorithm), and the SameWayMeanShift is a local processing, which does not process all data globally, but only targets other picking points in the neighborhood of a certain picking point in the current calculation. This reduces unnecessary calculations; when calculating the movement vector of a certain picking point, other picking points participating in the calculation in its neighborhood will be marked as "traversed" in the data structure of the kd tree with a flag bit. This mechanism ensures that each picking point will not repeatedly calculate its own movement vector, thereby saving computing resources. For the traversed picking points, their timestamps will be updated to the arithmetic mean of the timestamps of these points. This helps maintain the temporal consistency of the data, especially when dealing with dynamically changing environments. Even if some picking points have been marked as "traversed", they can still be traversed again when calculating the movement vectors of other picking points. This design ensures that the algorithm can make full use of all available information during the iteration process without missing important neighborhood relationships. The data structure of the kd tree with a flag bit is used as the underlying data structure to efficiently manage and query picking point information. This not only speeds up the neighborhood search process, but also supports fast insertion, deletion, and update operations, which is very suitable for dynamic update scenarios. The SameWayMeanShift algorithm includes:

[0100] Based on the location with the highest density, it has a higher degree of credibility and can determine the final picking point location for each object to be picked.

[0101] S52, determine the direction of the picking point according to the global collection map and the final picking point position; after determining the unique picking point position of each object to be picked, it is necessary to further determine the direction of the picking point to determine the picking waypoint of the drone. First, fit a plane with the picking point p as the center, then find the normal vector of the picking point p, then use the odometer point corresponding to the timestamp of the picking point p as the constraint point to determine the direction of the normal vector, and finally project it to the XY plane to obtain the yaw angle ψ of the waypoint; the determination of the direction of the picking point includes:

[0102] S6, determining a picking waypoint according to the final picking point position and the picking point orientation of each object to be picked. For any object to be picked, the corresponding picking waypoint is composed of the position obtained in step S51 and the yaw angle obtained in step S52. The waypoints generated by the drone according to the above method can achieve more efficient picking operations.

[0103] The direction of the picking point corresponds to the direction of the drone waypoint, which includes three dimensions, namely yaw, pitch, and roll. The yaw refers to the rotation of the drone around the vertical axis (Z axis), that is, turning left and right. This determines the direction in which the drone's nose points. The pitch refers to the rotation of the drone around the lateral axis (X axis), that is, the action of raising or lowering the head up and down. This affects the forward or backward angle of the drone. Considering the normal flight state of the drone, the pitch axis is autonomously stabilized by the drone flight controller. The roll refers to the rotation of the drone around the longitudinal axis (Y axis), that is, the roll action. This controls the drone to tilt to the left or right. For the roll axis, the waypoint does not need to contain this dimension information, and the autonomous stabilization of the roll axis is achieved by the drone's flight controller. Therefore, when planning the direction of the picking point, only the yaw angle needs to be considered because it directly affects the forward direction of the drone. The roll and pitch axes are automatically adjusted by the flight controller according to the actual situation to ensure the safety and stable flight of the drone.

[0104] Preferably, the step S51 comprises:

[0105] S511, calculate the density estimate of the picking point p in its neighborhood:

[0106]

[0107] Among them, h is the average width of the object to be collected, P ts In the field of picking point p, is a sphere with point p as the center and radius h, ts p is the corresponding timestamp. K(u) is the Gaussian kernel function:

[0108] S512, calculate the gradient function Determine the direction of movement:

[0109]

[0110] Among them, c k,3 represents the regularization parameter, K′(u) represents the derivative of the Gaussian kernel function, represents the gradient of the density estimate f(p);

[0111] The function g(s)=-K′(S) is introduced to obtain:

[0112]

[0113] S513, calculate the movement vector m h (p), determine the new position:

[0114]

[0115] Among them, m h (p) represents the motion vector, p i Neighborhood The i-th point in ;

[0116] S514, update the location and timestamp of the picking point, and update the picking point and the corresponding time until m h (p) = 0 stop:

[0117]

[0118] Specifically, the step S53 includes:

[0119] S521, define collision area N D , based on the data stored in the data structure of the kd tree with flags, find the field of picking point p Point o i , yes i Decentralize to get o′ i , and construct the covariance matrix C:

[0120]

[0121] Among them, i Representation field The i-th point in , p represents the center point, field the number of midpoints;

[0122]

[0123] Where q represents the reference point;

[0124] S522, using singular value decomposition to solve the minimum eigenvalue, the singular value decomposition solves the minimum eigenvalue of the covariance matrix C; the eigenvector corresponding to the minimum eigenvalue obtained by principal component analysis is the normal vector n of the picking point p o ;

[0125] S523, using the constraint point to determine the direction of the normal vector, the constraint point specifically refers to the odometer point of the timestamp corresponding to the picking point p, and searching for a point with the same timestamp in the odometer queue through the timestamp. p , and then use the following formula to determine whether the normal vector n needs to be flipped o :

[0126] O p =(x i ,y i ,z i ),n o =(x j ,y j ,z j )

[0127] Among them, O p represents the coordinates of the constraint point, n o Represents the coordinates of the normal vector;

[0128]

[0129] Among them, d(O p ,Dn o ) represents point O p To along n o The distance from the point in the direction D, d(O p ,-Dn o ) represents point O p To Along-n o The distance from the point in the direction D;

[0130]

[0131] Among them, d(O p ,Dn o ) represents point O p To along n o The distance from the point in the direction D;

[0132] S524, calculating the yaw angle ψ based on the XY plane: the normal vector n o Projection to the XY plane gives the yaw angle ψ of the waypoint:

[0133]

[0134] like Figure 4As shown, specifically, in this embodiment, the data structure of the kd tree with flags includes: point type PointType point, used to store information such as point coordinates and intensity; pointer leftnode pointing to the left child node; pointer rightnode pointing to the right child node; integer variable partition_axis, used to represent the partition axis; floating point variable timestamp, used to store the corresponding time information of the global sampling point; floating point array range, used to store the bounding box vertices of the global sampling point and Boolean variable filter, used to filter the laser radar point cloud that is not necessary to process, wherein flag has three status bits: obstacle point, picking point and traversed picking point. The data structure of the kd tree with flags can process dynamic data sets, that is, it allows new points to be added to the existing tree structure without reconstructing the entire tree, which makes it very suitable for real-time systems. For subsequent processing steps that rely on fast spatial queries (such as clustering, segmentation, matching, etc.), the pre-constructed data structure of the kd tree with flags can directly provide efficient index support, thereby speeding up the operation of these algorithms. Storing the timestamp together with the point cloud data in a data structure of a kd-tree with a flag not only preserves the time attribute of each sampling point but also facilitates selective queries based on the time range.

[0135] Furthermore, in this embodiment, the step S1 further includes:

[0136] In step S13, the ScanContext method is used to reduce the error of the point cloud map of the timestamp T based on the ROS time, and the optimization is performed by the ICP algorithm (Iterative Closest Point) and the GTSAM library (Georgia Tech Smoothing and Mapping). The ScanContext method is a descriptor for the point cloud of a lidar sensor, which captures the geometric features of the environment by converting a 3D point cloud into a 2D columnar histogram. The ICP is a commonly used point cloud registration algorithm that aims to minimize the distance difference between two sets of three-dimensional point sets. The GTSAM library is a probabilistic modeling library designed specifically for robotics and computer vision, and is particularly suitable for nonlinear optimization problems. First, each frame of point cloud data of the laser radar is stored as a matrix I in ScanContext format. The specific format is a two-dimensional matrix of Colum called Sector and Row called Ring. The matrix of each frame is matched with the historical matrix before storage by calculating the cosine distance between the corresponding column (Ring) vectors of the two matrices. The similarity of the two matrices is represented by the sum of the distances of each column. The step S13 includes:

[0137] S131, point cloud data storage and matching, based on the current frame point cloud matrix I q And the historical frame point cloud matrix I c Compute the cosine distance between corresponding column vectors:

[0138]

[0139] Among them, I q Represents the current point cloud matrix, I c Represents a point cloud matrix in the history queue, N s represents the number of matrix columns, Represents the matrix I q The jth column of Represents the matrix I c The jth column of

[0140] Set the threshold τ, when d(I q ,I c )<τ, then the two matrices are judged to be similar; the current frame point cloud matrix I q The corresponding original point cloud matrix P k With the historical frame point cloud matrix I c The corresponding original point cloud matrix To match:

[0141]

[0142] Among them, P k For I q The corresponding original point cloud, P k_loop For I c The corresponding original point cloud; the value of the set threshold τ can be set according to the actual situation, for example, it can be 0.10, 0.13 and 0.15. Specifically in this embodiment, the value of the threshold τ is 0.13.

[0143] S132, the ICP algorithm solves the pose constraints and obtains the matched original point cloud P based on step S131 k and P k and Input into ICP algorithm to solve pose constraints

[0144]

[0145] Optimization objective function:

[0146]

[0147] in, represents the optimal rotation matrix, represents the optimal translation vector, argmin represents finding the parameters that minimize the objective function, R represents the rotation matrix, t represents the translation vector, |P k Represents the point cloud P k The number of midpoints, P k represents the point cloud of the current frame, Represents the i-th point in the loop closed frame.

[0148] S133, optimizing the pose constraints, inputting the pose in the world coordinate system output by the FAST-LIO2 algorithm and the pose constraints calculated by the ICP into the GTSAM library for optimization to update the map and trajectory; in this embodiment, the pose in the world coordinate system output by the FAST-LIO2 algorithm is And the pose constraints calculated by ICP Input to GTSAM library to get the pose Use the optimized pose Updates the Map and Trajectory.

[0149] Optionally, the step S21 further includes: converting the point in the camera coordinate system to the laser radar coordinate system:

[0150]

[0151] Among them, [X L ,Y L ,Z L ] T represents the point in the laser radar coordinate system, [X C ,Y C ,Z C ] T represents a point in the camera coordinate system, represents the camera to lidar rotation matrix obtained after calibration, Represents the camera to lidar translation vector obtained after calibration.

[0152] like Figure 3 As shown, specifically, in this embodiment, step S22 includes: storing the camera sensor image information in the image information queue, storing the odometer information in the odometer queue, and using the buffer queue combined with the NLerp linear interpolation method to achieve time synchronization, including:

[0153] Take the earliest timestamp ts=n from the odometer queue 1 Odometer information O m , check the earliest image information I in the image information queue n , if Om The time is later than I n , then discard I n , if O m The time is equal to I n , then the combination O m and I n Two data are output to the synchronization queue; if O m Earlier than I n , then find the first time greater than I from the odometer queue n O n-1 , and the first time later than I n O n+1 , and then perform NLep interpolation to calculate O n , combination O n and I n Output to the synchronous queue. This processing can not only effectively solve the problem of asynchronous data flow, but also significantly improve the performance and reliability of the system.

[0154] Preferably, the NLerp linear interpolation method includes: odometer information interpolation:

[0155]

[0156] Among them, odom ts＝n represents the state of the odometer at timestamp ts=n, R ts＝n Indicates that at timestamp t ts＝n The rotation matrix, t ts＝n represents the translation vector at timestamp ts=n, where n represents the interpolation coefficient used for linear interpolation.

[0157] Rotation matrix R:

[0158]

[0159] Among them, q 0 ,q 1 ,q 2 ,q 3 The components of the quaternion represent the attitude; i, j, k are complex units.

[0160] Translation vector interpolation:

[0161]

[0162] in, Indicates that at timestamp ts=n 2 The interpolated translation vector of ; Indicates that at timestamp ts=n 1 The translation vector of Indicates that at timestamp ts=n3 The translation vector of 2 Represents the interpolation coefficient.

[0163] Quaternion interpolation:

[0164]

[0165] in, At timestamp ts=n 2 The interpolated quaternion of ; Indicates that at timestamp ts=n 1 The quaternion of Indicates that at timestamp ts=n 3 The quaternion of 2 Represents the interpolation coefficient.

[0166] Through these formulas, the time synchronization and interpolation of sensor data can be effectively achieved to ensure the consistency and accuracy of the data.

[0167] Specifically, the identification and detection of the picking points to be collected include the identification and detection of the picking points to be collected through the YOLO-v8 network. After being frameworkd, YOLO-v8 is an excellent choice for target detection tasks. YOLO-v8 builds on the success of previous YOLO versions and introduces new features and improvements to further improve the performance and flexibility of the network. In Backbone and Neck, YOLO-v8 uses a richer gradient flow C2f module to replace the original C3 module. Backbone also adjusts the number of channels for models of different scales, which greatly improves the performance of the model. In the Head part, a major change was made from Anchor-Based to Anchor-Free, and Decoupled-Head was adopted, which is very suitable for application engineering practice.

[0168] As a preferred embodiment of the present invention, step S3 obtains the union information from the image and odometer synchronization queue obtained in step S2 and uses it as the input of the YOLO-v8 network to obtain the picking coordinate point p in the image coordinate system. pixel And the corresponding timestamp (ts), and then p pixel Convert to the picking point p in the global coordinate system gobal In step S3, the picking coordinate point p is obtained in the image coordinate system. pixel And the corresponding timestamp, where:

[0169] S31, the p pixel Convert to the picking point p in the global coordinate system gobal ,include:

[0170] S311, conversion from pixel coordinates to camera coordinate system:

[0171]

[0172] in, represents pixel coordinates, u i Represents the horizontal coordinate of the image point, v i Represents the vertical coordinate of the image point;

[0173] S312, conversion from camera coordinate system to lidar coordinate system:

[0174]

[0175] Among them, f x ,f y represents the focal length of the camera, c x ,c y Represents the principal point coordinates of the camera, Z i The corresponding pixel The depth value of is the rotation matrix between the camera coordinate system and the lidar sensor coordinate system, is the translation vector between the camera coordinate system and the lidar sensor coordinate system;

[0176] S313, conversion from lidar coordinate system to global coordinate system:

[0177]

[0178] in, is the rotation matrix between the lidar coordinate system and the global coordinate system, is the translation vector between the lidar coordinate system and the global coordinate system;

[0179] S32, the picking point p in the global coordinate system gobal Combined with the corresponding timestamp to form new data Add to the global picking point set P with timestamp ts middle.

[0180] The present invention also provides a system for generating waypoints for drone picking operations, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the system implements the steps of the method for generating waypoints for drone picking operations as described in any one of the above items.

[0181] Figure 5 The hardware structure diagram of the method for generating waypoints for a drone harvesting operation provided by an embodiment of the present invention is shown in FIG. Figure 5As shown, the embodiment / computer 6 includes: a processor 60, a memory 61, and a computer program 62 stored in the memory 61 and executable on the processor 60, such as a program for a method for generating waypoints for drone picking operations. When the processor 60 executes the computer program 62, the steps in each of the above-mentioned embodiments of the method for generating waypoints for drone picking operations are implemented. Alternatively, when the processor 60 executes the computer program 62, the functions of each module / unit in the above-mentioned device embodiments are implemented. Exemplarily, the computer program 62 can be divided into one or more modules / units, and the one or more modules / units are stored in the memory 61 and executed by the processor 60 to complete the present invention. The one or more modules / units can be a series of computer program instruction segments that can perform specific functions, and the instruction segments are used to describe the execution process of the computer program 62 in the computer 6.

[0182] The computer 6 may be a computing device such as a desktop computer, a notebook, a PDA, or a cloud server. The computer 6 may include, but is not limited to, a processor 60 and a memory 61. Those skilled in the art will appreciate that Figure 5 It is only an example of computer 6 and does not constitute a limitation on computer 6. It may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the computer 6 may also include input and output devices, network access devices, buses, etc.

[0183] The processor 60 may be a central processing unit (CPU), other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc.

[0184] The memory 61 may be an internal storage unit of the computer 6, such as a hard disk or memory of the computer 6. The memory 61 may also be an external storage device of the computer 6, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the terminal device. Further, the memory 61 may also include both an internal storage unit of the computer 6 and an external storage device. The memory 61 is used to store the computer program and other programs and data required by the terminal device. The memory 61 may also be used to temporarily store data that has been output or is to be output.

[0185] The technicians in the relevant field can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In practical applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, which will not be repeated here.

[0186] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0187] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0188] In the embodiments provided by the present invention, it should be understood that the disclosed devices / terminal equipment and methods can be implemented in other ways. For example, the device / terminal equipment embodiments described above are only schematic. For example, the division of the modules or units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0189] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0190] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0191] If the integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present invention implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may include: any entity or device capable of carrying the computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electric carrier signal, telecommunication signal and software distribution medium. It should be noted that the content contained in the computer-readable medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media do not include electric carrier signals and telecommunication signals.

[0192] The embodiments described above are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. Such modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included in the protection scope of the present invention.

Claims

1. A method for generating waypoints for drone picking operations, characterized in that: Includes steps: S1, build a combined odometer based on the FAST-LIO2 algorithm, including: S11, using the FAST-LIO2 algorithm to process the data of the laser radar sensor and the inertial measurement unit to obtain a point cloud map with a timestamp T based on the ROS time; S12, obtaining image information collected by the camera and carrying a timestamp T based on the ROS time; S2, constructing an information synchronization mechanism to realize external parameter calibration and information synchronization between the camera sensor and the lidar sensor, including: S21, through calibration, obtain the transformation matrix from the camera coordinate system to the lidar coordinate system The image information is converted into image information based on a laser radar coordinate system; S22, based on the ROS timestamp and using the buffer queue combined with the NLerp interpolation method, after time synchronization of the odometer information provided by the laser radar sensor and the image information collected by the camera, obtain the odometer information provided by the synchronized laser radar sensor. n Image information collected by the synchronized camera I n , and output to the synchronization queue; S3, obtaining a global picking point with a timestamp, including: based on the image information obtained in step S12 and the union information in the synchronization queue obtained in step S2, identifying and detecting the picking point to be picked, so as to obtain the picking coordinate point p in the image coordinate system pixel and the corresponding timestamp ts; the picking coordinate point p in the image coordinate system pixel Transformed into the picking point p in the global coordinate system gobal and combined with the corresponding timestamp ts to output the global picking point set P ts ; S4, construct a point cloud map based on the data structure of the kd tree with a flag and a global picking point set P ts , as a global acquisition map; S5, determining the location and orientation of the picking point, including: S51, performing credibility analysis on the picking point positions of the same object to be picked according to the global collection map, obtaining the position with the highest credibility, and generating the final picking point position of each object to be picked; S52, determining the direction of the picking point according to the global collection map and the final picking point position; S6, determining a picking waypoint according to the final picking point position and the direction of the picking point of each object to be picked.

2. The method for generating waypoints for drone picking operations according to claim 1, characterized in that: The step S51 comprises: S511, calculate the density estimate of the picking point p in its neighborhood: Among them, h is the average width of the object to be collected, P ts In the field of picking point p, is a sphere with the picking point p as the center and radius h, ts p is the corresponding timestamp, K(u) is the Gaussian kernel function: S512, calculate the gradient function Determine the direction of movement: Among them, c k,3 represents the regularization parameter, K′(u) represents the derivative of the Gaussian kernel function; Introducing the function g(s)=-K′(S), we get: S513, calculate the movement vector m h (p), determine the new position: S514, update the location and timestamp of the picking point. h (p) = 0 stop:

3. The method for generating waypoints for drone picking operations according to claim 1, characterized in that: The step S52 comprises: S521, define collision area N D , based on the data stored in the data structure of the kd tree with flags, find the field of picking point p Point o i , yes i Decentralize to get o′ i , and construct the covariance matrix C: S522, using singular value decomposition to solve the minimum eigenvalue, the singular value decomposition solves the minimum eigenvalue of the covariance matrix C; the eigenvector corresponding to the minimum eigenvalue obtained by principal component analysis is the normal vector n of the picking point p o ; S523, using the constraint point to determine the direction of the normal vector, the constraint point specifically refers to the odometer point of the timestamp corresponding to the picking point p, and searching for a point with the same timestamp in the odometer queue through the timestamp. p , and then use the following formula to determine whether the normal vector n needs to be flipped o : O p =(x i ,y i ,z i ),n o =(x j ,y j ,z j ) Among them, d(O p ,Dn o ) represents point O p To along n o The distance from the point in the direction D; S524, calculating the yaw angle ψ based on the XY plane: the normal vector n o Projection to the XY plane gives the yaw angle ψ of the waypoint:

4. The method for generating waypoints for drone picking operations according to claim 1, characterized in that: The data structure of the kd tree with flags includes: point type PointType point, used to store point coordinates, intensity and other information; pointer leftnode pointing to the left child node; pointer rightnode pointing to the right child node; integer variable partition_axis, used to represent the partition axis; floating-point variable timestamp, used to store the corresponding time information of the global sampling point; floating-point array range, used to store the bounding box vertices of the global sampling point and Boolean variable filter, used to filter the laser radar point cloud that is not necessarily processed, where flag has three status bits: obstacle point, picking point and traversed picking point.

5. The method for generating waypoints for drone picking operations according to claim 1, characterized in that: The step S1 further includes: Step S13, using the ScanContext method to reduce the error of the point cloud map of the timestamp T based on the ROS time, and optimizing it through the ICP algorithm and the GTSAM library; S131, point cloud data storage and matching, based on the current frame point cloud matrix I q And the historical frame point cloud matrix I c Compute the cosine distance between corresponding column vectors: Among them, I q Represents the current point cloud matrix, I c Represents a point cloud matrix in the history queue, N s represents the number of matrix columns, Represents the matrix I q The jth column of Represents the matrix I c The jth column of Set the threshold τ, when d(I q ,I c )<τ, then the two matrices are judged to be similar; the current frame point cloud matrix I q The corresponding original point cloud matrix P k With the historical frame point cloud matrix I c The corresponding original point cloud matrix To match: S132, using the ICP algorithm to solve the pose constraints, based on step S131 to obtain the matched original point cloud P k and Solving pose constraints Optimization objective function: S133, optimizing the pose constraints, inputting the pose in the world coordinate system output by the FAST-LIO2 algorithm and the pose constraints calculated by the ICP into the GTSAM library for optimization, so as to update the map and trajectory.

6. The method for generating waypoints for drone picking operations according to claim 1, characterized in that: The step S21 comprises: converting the point in the camera coordinate system to the laser radar coordinate system: Among them, [X L ,Y L ,Z L ] T represents a point in the laser radar coordinate system, [X C ,Y C ,Z C ] T Represents a point in the camera coordinate system.

7. The method for generating waypoints for drone picking operations according to claim 1, characterized in that: The step S22 includes: storing the camera sensor image information in the image information queue, storing the odometer information in the odometer queue, and using the buffer queue in combination with the NLerp linear interpolation method to achieve time synchronization, including: Take out the earliest odometer information O with timestamp ts=n1 from the odometer queue m , check the earliest image information I in the image information queue n , if O m The time is later than I n , then discard I n , if O m The time is equal to I n , then the combination O m and I n Two data are output to the synchronization queue; if O m Earlier than I n , then find the first time greater than I from the odometer queue n O n-1 , and the first time later than I n O n+1 , and then perform NLep interpolation to calculate O n , combination O n and I n Output to the synchronization queue.

8. The method for generating waypoints for drone picking operations according to claim 1, characterized in that: The identification and detection of the picking points to be collected include the identification and detection of the picking points to be collected by using a YOLO-v8 network.

9. The method for generating waypoints for drone picking operations according to claim 1, characterized in that: In step S3, the picking coordinate point p is obtained in the image coordinate system. pixel And the corresponding timestamp, it also includes: S31, the p pixel Convert to the picking point p in the global coordinate system gobal ,include: S311, conversion from pixel coordinates to camera coordinate system: in, represents pixel coordinates, u i Represents the horizontal coordinate of the image point, v i Represents the vertical coordinate of the image point; S312, conversion from camera coordinate system to lidar coordinate system: Among them, f x ,f y represents the focal length of the camera, c x ,c y Represents the principal point coordinates of the camera, Z i The corresponding pixel The depth value of is the rotation matrix between the camera coordinate system and the lidar sensor coordinate system, is the translation vector between the camera coordinate system and the lidar sensor coordinate system; S313, conversion from lidar coordinate system to global coordinate system: in, is the rotation matrix between the lidar coordinate system and the global coordinate system, is the translation vector between the lidar coordinate system and the global coordinate system; S32, the picking point p in the global coordinate system gobal Combined with the corresponding timestamp to form new data Add to the global picking point set P with timestamp ts middle.

10. A system for generating waypoints for drone picking operations, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method for generating waypoints for drone picking operations as described in any one of claims 1 to 9 are implemented.

Images