A method and system for generating a picking work point of a UAV

Abstract

The present application relates to the technical field of artificial intelligence, in particular to a method and system for generating picking operation waypoints of a UAV, comprising the steps of: constructing a combined odometer based on the FAST-LIO2 algorithm; constructing an information synchronization mechanism; obtaining a global picking point with a timestamp; constructing a global collection map; determining the position and orientation of the picking point; and determining the picking waypoint to guide the UAV picking. In the dynamic picking operation of the UAV, the picking waypoints are provided to the UAV in advance, effectively improving the picking efficiency of the UAV.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of artificial intelligence, and particularly relates to a method and system for generating a picking operation flight point of a UAV. BACKGROUND

[0002] With the decline of agricultural labor population, using agricultural robots to replace manual labor has become a trend, and picking robots in the form of UAVs have also been applied and popularized in recent years. The advantage of UAVs is that they are not limited by terrain and have a wide range of motion, and in the long term, using UAVs for picking helps to reduce labor costs.

[0003] When a UAV picks fruits dynamically without a global view, the on-board camera of the UAV cannot cover all the fruits, and the UAV needs to move and then detect and identify the fruits, which to some extent, wastes time and increases cost.

[0004] In the prior art, a data map of an agricultural scene is often constructed by using a visual odometry calculation method through a camera sensor, but the effect of the visual odometry calculation method is greatly affected by changes in ambient light, and in a picking operation scene, the camera is easily blocked by leaves and other obstacles, which will directly cause the visual odometry calculation method to fail. Although significant progress has been made in UAV picking technology, the recognition accuracy of the to-be-picked objects and spatial positioning still needs to be improved. SUMMARY

[0005] (I) Technical problem to be solved

[0006] The main purpose of the present application is to provide a method and system for generating a picking operation flight point of a UAV to solve the above problems.

[0007] (II) Technical solution

[0008] To achieve the above purpose, the present application provides a method for generating a picking operation flight point of a UAV, comprising the steps of:

[0009] S1, constructing a combined odometry based on the FAST-LIO2 algorithm, comprising:

[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 carrying a timestamp T based on ROS time;

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

[0012] S2, constructing an information synchronization mechanism to realize the extrinsic calibration and information synchronization of the camera sensor and the laser radar sensor, comprising:

[0013] S21, obtaining a transformation matrix from a camera coordinate system to a LiDAR coordinate system through calibration to obtain image information converted into image information based on the LiDAR coordinate system;

[0014] S22, time synchronizing the odometer information provided by the LiDAR sensor and the image information collected by the camera based on the ROS timestamp and using a buffer queue combined with the NLerp interpolation method, to obtain odometer information O n synchronized with the image information I n collected by the camera, and output to a synchronization queue;

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

[0016] S4, constructing a point cloud map based on a data structure of a k-d tree with a flag bit and the global picking point set P ts as a global collection map;

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

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

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

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

[0021] Preferably, the step S51 includes:

[0022] S511, calculating the density estimation of the picking point p in its neighborhood:

[0023]

[0024] where h is the average width of the object to be picked, 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) yields:

[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, when m 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 inside i , for o 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, determine the normal vector direction by using the constraint point, which is the odometer point corresponding to the timestamp of the picking point p, find the point O with the same timestamp in the odometer queue p , and then determine whether the normal vector n needs to be flipped by the following formula o :

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

[0040]

[0041] Wherein, d(O p ,Dn o ) represents the distance from point O p to the point along the direction of n o distance D;

[0042] S524, calculate the yaw angle ψ based on the X-Y plane: the normal vector n o projection to the X-Y plane to get the yaw angle ψ of the navigation point:

[0043]

[0044] Preferably, the data structure of the k-d tree with flag includes: a point type PointType point for storing point coordinates, intensity and other information; a pointer leftnode pointing to the left child node; a pointer rightnode pointing to the right child node; an integer variable partition_axis for indicating the partition axis; a floating point variable timestamp for storing the corresponding time information of the global sampling point; a floating point array range for storing the bounding box vertices of the global sampling point; and a Boolean variable filter for filtering unnecessary processing of the laser radar point cloud, wherein the flag has three state bits: obstacle point, picking point and traversed picking point.

[0045] Preferably, the step S1 further comprises:

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

[0047] S131, point cloud data storage and matching, based on the current frame point cloud matrix I qand a historical frame point cloud matrix I c The cosine distance between corresponding column vectors is calculated:

[0048]

[0049] where I q represents the current point cloud matrix, I c represents a point cloud matrix in the historical queue, N s represents the number of matrix columns, represents the jth column of matrix I q . represents the jth column of matrix I c .

[0050] A threshold τ is set, and when d(I q , I c ) < τ, it is determined that the two matrices are similar; the corresponding original point cloud matrix P q of the current frame point cloud matrix I k is matched with the corresponding original point cloud matrix P c of the historical frame point cloud matrix I k .

[0051]

[0052] S132, using the ICP algorithm to solve the pose constraint, based on step S131 to obtain the matched original point cloud P L and solving the pose constraint

[0053]

[0054] Optimization objective function:

[0055]

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

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

[0058]

[0059] where [X L , Y L , Z 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.

[0060] Preferably, the step S22 comprises: storing the camera sensor image information into an image information queue, storing the odometry information into an odometry queue, using a buffer queue to realize time synchronization in combination with the NLerp linear interpolation method, comprising:

[0061] taking out the earliest timestamp ts=n1 odometry information O m from the odometry queue, checking the image information I n with the earliest time in the image information queue, m if the time of O n is later than I n , discarding I m , if the time of O n is equal to I m , combining O n and I m and outputting to the synchronization queue; if the time of O n is earlier than I n , finding the first O n-1 with a time greater than I n and the first O n+1 with a time later than I n from the odometry queue, then performing NLerp interpolation to calculate O n , combining O n and I pixel and outputting to the synchronization queue.

[0062] Preferably, the identification and detection of the picking point of the to-be-picked object comprises identification and detection of the picking point of the to-be-picked object through a YOLO-v8 network.

[0063] Preferably, the step S3 further comprises, after obtaining the picking coordinate point p pixel in the image coordinate system and the corresponding timestamp:

[0064] S31, converting the p gobal into a picking point p i in the global coordinate system, comprising:

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

[0066]

[0067] wherein, represents pixel coordinates, u irepresents a vertical coordinate of an image point;

[0068] S312, conversion of the camera coordinate system to the laser radar coordinate system:

[0069]

[0070] wherein 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 is the depth value of the corresponding pixel point , is the rotation matrix between the camera coordinate system and the laser radar sensor coordinate system, is the translation vector between the camera coordinate system and the laser radar sensor coordinate system;

[0071] S313, conversion of the laser radar coordinate system to the global coordinate system:

[0072]

[0073] wherein, is the rotation matrix between the laser radar coordinate system and the global coordinate system, is the translation vector between the laser radar coordinate system and the global coordinate system;

[0074] S32, combining the picking point p gobal in the global coordinate system with the corresponding timestamp to form new data and adding it to the timestamped global picking point set P ts .

[0075] The application also provides a UAV picking operation waypoint generation system, which comprises a memory, a processor, a computer program stored in the memory and executable on the processor, and the processor implements the steps of the UAV picking operation waypoint generation method according to any one of the above.

[0076] (III) Beneficial effects

[0077] The UAV picking operation waypoint generation method provided by the application realizes the construction of a global picking map in the UAV coordinate system by using the improved FAST-LIO2 algorithm and Yolo-v8 network, and generates reasonable flight waypoints for the UAV based on the map. The method realizes dynamic and continuous picking operation of the UAV, reduces the detection time caused by the field of view limitation, and improves the picking efficiency of the UAV.

[0078] The unmanned aerial vehicle picking operation flight point generation method provided by the application improves the accuracy of method positioning and the robustness of method effect, and better responds to complex real environment.

[0079] The unmanned aerial vehicle picking operation flight point generation method provided by the application uses the data structure of the k-d tree with a flag bit to store the point cloud map and the global sampling point information with a time stamp, improves the efficiency of multi-source data storage and processing, and is beneficial to the generation accuracy and generation efficiency of subsequent picking flight points. BRIEF DESCRIPTION OF DRAWINGS

[0080] Figure 1 The flowchart of the unmanned aerial vehicle picking operation flight point generation method provided by an embodiment of the application is shown in the figure.

[0081] Figure 2 The flowchart of the SLAM system based on FASTLIO2 and GTSAM provided by an embodiment of the application is shown in the figure.

[0082] Figure 3 The flowchart of the odometer information and image information matching provided by an embodiment of the application is shown in the figure.

[0083] Figure 4 The pseudo algorithm of the data structure of the k-d tree with a flag bit provided by an embodiment of the application is shown in the figure.

[0084] Figure 5 The hardware structure diagram of the unmanned aerial vehicle picking operation flight point generation method provided by an embodiment of the application is shown in the figure. DETAILED DESCRIPTION

[0085] In order to better explain the application, so as to be understood, the application is described in detail below by specific embodiments in combination with the drawings.

[0086] It should be noted that all directional indications (such as up, down, left, right, front, back, etc.) in the embodiments of the application are only used to explain the relative positional relationship, movement condition, etc. between components in a certain posture (as shown in the drawings), and if the certain posture changes, the directional indications also change accordingly.

[0087] In addition, the descriptions such as "first", "second" and the like in the present application are only for the purpose of description, and cannot be understood as indicating or implying the relative importance of the technical features indicated, or implicitly indicating the number of technical features indicated. Therefore, the features defined as "first", "second" can explicitly or implicitly include at least one of the features. In the description of the present application, the meaning of "a plurality of" is at least two, for example, two, three, etc., unless otherwise explicitly specified and limited.

[0088] In the present application, unless otherwise explicitly specified and limited, the terms "connection", "fixing" and the like should be understood broadly, for example, "fixing" can be fixed connection, or detachable connection, or integral; "connection" can be mechanical connection, or electrical connection; can be directly connected, or indirectly connected through intermediate medium, can be internal communication of two elements or interaction relationship between two elements, unless otherwise explicitly limited. For those skilled in the art, the specific meaning of the above terms in the present application can be understood according to the specific circumstances.

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

[0090] S1, a combined odometry based on FAST-LIO2 algorithm (Fast LiDAR-Inertial Odometry 2) is constructed. Compared with a single visual odometry algorithm, the laser radar sensor (LiDAR) used in the present embodiment can well solve the problem that the positioning algorithm is affected by sensor occlusion, and can provide real-time pose for the camera; as shown in Figure 2 The SLAM system based on FASTLIO2 and GTSAM acquires positioning information, and the combined odometry based on FAST-LIO2 algorithm includes:

[0091] S11, data of a laser radar sensor and an inertial measurement unit (IMU) are processed by using the FAST-LIO2 algorithm to obtain a point cloud map carrying a timestamp T based on a ROS (Robot Operating System) time; the FAST-LIO2 algorithm is a version of a SLAM (Simultaneous Localization and Mapping) algorithm based on fusion of data of the laser radar sensor and the inertial measurement unit, which 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 both the laser radar sensor and the inertial measurement unit. The laser radar sensor can provide high-precision distance measurement, while the inertial measurement unit can provide high-frequency attitude and acceleration information, and 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 pose information.

[0092] S12, image information carrying a timestamp T based on a ROS time is obtained by a camera; in this embodiment, the obtained 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 obtained image information is an RGB image.

[0093] S2, an information synchronization mechanism is constructed to realize external parameter calibration and information synchronization of a camera sensor and the laser radar sensor, including:

[0094] S21, a transformation matrix from a camera coordinate system to a laser radar coordinate system is obtained through calibration to obtain the image information converted into image information based on the laser radar coordinate system; the method of external parameter calibration can be set according to actual conditions, for example, a method based on a checkerboard or a specially designed calibration board, a method based on natural feature points, and an automatic online calibration method. Specifically, in this embodiment, the method of external parameter calibration is an open-source calibration method named livox_camera_calib proposed by the University of Hong Kong based on Livox series radars;

[0095] S22, based on the timestamp of the ROS and using a 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 synchronized odometer information O n and synchronized image information I n, and outputs it to a synchronous queue. The ROS system is a flexible framework for robotics development. It includes a series of tools, libraries, and conventions designed to simplify the development of robotics 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, 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, the output is 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, and the identification and detection of the picking point of the object to be picked are performed using the YOLO (You Only Look Once) network. The YOLO network 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 this 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 bit 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 location and orientation of the picking point, including:

[0099] S51, according to the global collection map, the picking point position of the same to-be-picked object is analyzed for credibility, the position with the highest credibility is obtained, and the final picking point position of each to-be-picked object is generated; wherein the credibility analysis includes: density analysis and optimized picking point position, based on the global collection map of step S4, the optimal picking point position is found by processing the global picking point through the SameWayMeanShift algorithm, wherein the SameWayMeanShift algorithm is an optimization of the traditional MeanShift algorithm (MeanShift algorithm), the SameWayMeanShift is a local processing, and all data is not processed globally, but only for other picking points in the neighborhood of a picking point in the current calculation. This reduces unnecessary calculation; when calculating the movement vector of a picking point, other picking points in its neighborhood participating in the calculation will be marked as "traversed" in the data structure of the k-d tree with a flag. This mechanism ensures that each picking point will not repeat the calculation of its own movement vector, thereby saving computing resources. For the traversed picking points, their time stamps will be updated to the arithmetic mean of the time stamps of these points. This helps to maintain the temporal consistency of the data, especially when dealing with dynamic changing environments. Even if some picking points have been marked as "traversed", they can still be traversed again when calculating the movement vector of other picking points. This design ensures that the algorithm can fully utilize all available information during iteration without missing important neighborhood relationships. The data structure of the k-d tree with a flag 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 position with the maximum density, the credibility is higher, and the determination of the final picking point position of each to-be-picked object is realized.

[0101] S52, according to the global collection map and the final picking point position, the direction of the picking point is determined; after determining the unique picking point position of each to-be-picked object, the direction of the picking point needs to be further determined to determine the picking waypoint of the unmanned aerial vehicle. First, a plane is fitted with the picking point p as the center, then the normal vector of the picking point p is calculated, then the odometer point corresponding to the time stamp of the picking point p is used as a constraint point to determine the direction of the normal vector, and finally the yaw angle ψ of the waypoint is obtained by projecting to the XY plane; the determination of the direction of the picking point includes:

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

[0103] The orientation of the picking point corresponds to the direction of the UAV waypoint, which includes three dimensions, yaw (yaw), pitch (pitch) and roll (roll). The yaw refers to the rotation of the UAV around the vertical axis (Z axis), i.e. left and right rotation. This determines which direction the UAV head points to. The pitch refers to the rotation of the UAV around the horizontal axis (X axis), i.e. the action of lifting or lowering the head. This affects the forward or backward angle of the UAV, considering the normal flight state of the UAV, the pitch axis is realized by the UAV flight controller to realize autonomous stabilization. The roll refers to the rotation of the UAV around the longitudinal axis (Y axis), i.e. the action of tilting to the left or right. For the roll axis, the waypoint does not need to contain the dimension information, and the roll axis is realized by the flight controller of the UAV to realize autonomous stabilization. Therefore, when planning the orientation of the picking point, only the yaw angle needs to be considered, because it directly affects the forward direction of the UAV. The roll and pitch axes are automatically adjusted by the flight controller according to the actual situation to ensure the safe and stable flight of the UAV.

[0104] Preferably, the step S51 comprises:

[0105] S511, calculating the density estimation of the picking point p in its neighborhood:

[0106]

[0107] wherein h is the average width of the to-be-picked object, is the density estimation of the picking point p, ts the field of the picking point p, is a sphere with the point p as the center and the radius h, ts p is the corresponding time stamp. K(u) is a Gaussian kernel function:

[0108] S512, calculating the gradient function determining the moving direction:

[0109]

[0110] wherein c k,3 represents a regularization parameter, K'(u) represents the derivative of the Gaussian kernel function, represents the gradient of the density estimation f(p);

[0111] Introducing the function g(s) = -K'(s) gives:

[0112]

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

[0114]

[0115] where m h (p) represents the moving vector, p i neighborhood of the ith point in the neighborhood;

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

[0117]

[0118] Specifically, the step S53 comprises:

[0119] S521, define the collision field N D , find the point o in the field of the picking point p based on the data stored in the data structure of the k-d tree with the flag bit i , decentralize o i to obtain o' i , and construct the covariance matrix C:

[0120]

[0121] where o i represents the ith point in the field , p represents the center point, the number of points in the field ;

[0122]

[0123] where q represents the reference point;

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

[0125] ​S523, determine the normal vector direction by using the constraint point, which is the odometer point corresponding to the time stamp of the picking point p, find the point O with the same time stamp in the odometer queue through the time stamp p , and judge whether the normal vector n needs to be reversed by the following formula o :

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

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

[0128]

[0129] Wherein, d(O p ,Dn o ) represents the distance from point O p to the point along the direction of n o D, d(O p ,-Dn o ) represents the distance from point O p to the point along the direction of-n o D;

[0130]

[0131] Wherein, d(O p ,Dn o ) represents the distance from point O p to the point along the direction of n o D;

[0132] S524, calculate the yaw angle ψ based on the X-Y plane: the normal vector n o Projection to get the X-Y plane to get the yaw angle ψ of the navigation point:

[0133]

[0134] As Figure 4As shown, specifically, in the embodiment, the data structure of the k-d tree with flag includes: a point type PointType point for storing point coordinates, intensity, etc.; a pointer leftnode pointing to a left child node; a pointer rightnode pointing to a right child node; an integer variable partition_axis for indicating a partition axis; a floating point variable timestamp for storing corresponding time information of global sampling points; a floating point array range for storing bounding box vertices of global sampling points; and a Boolean variable filter for filtering unnecessary processed lidar point clouds. The flag has three state bits: obstacle points, picking points, and traversed picking points. The data structure of the k-d tree with flag can process dynamic data sets, that is, new points can be added to the existing tree structure without the need to reconstruct the entire tree, which makes it very suitable for real-time systems. For subsequent processing steps (such as clustering, segmentation, matching, etc.) that rely on fast spatial queries, the pre-built data structure of the k-d tree with flag can directly provide efficient index support, thereby speeding up the running speed of these algorithms. Storing timestamps together with point cloud data in the data structure of the k-d tree with flag not only retains the time attribute of each sampling point, but also facilitates selective queries based on time ranges.

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

[0136] The step S13 reduces the error of the point cloud map with ROS time-based timestamp T by using a ScanContext method, and optimizes it by an ICP algorithm (Iterative Closest Point) and a GTSAM library (Georgia Tech Smoothing and Mapping). The ScanContext method is a descriptor for lidar sensor point clouds that captures the geometric features of the environment by converting 3D point clouds into 2D column histograms. The ICP is a commonly used point cloud registration algorithm that aims to minimize the distance difference between two sets of three-dimensional points. The GTSAM library is a probabilistic modeling library specifically designed for robots and computer vision, particularly suitable for nonlinear optimization problems. First, each frame of lidar point cloud data is stored as a ScanContext format matrix I, which is a two-dimensional matrix of columns called Sector and rows called Ring. Each frame of matrix 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, ICP algorithm solves the pose constraints and obtains the matched original point cloud P based on step S131. k and P k and Input the ICP algorithm to solve the 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 represents the number of midpoints, P k represents the point cloud of the current frame, represents the i-th point in the loop closure frame.

[0148] S133, optimize the pose constraint, input the pose in the world coordinate system output by the FAST-LIO2 algorithm and the pose constraint calculated by ICP into the GTSAM library for optimization, to update the map and the trajectory; in this embodiment, the pose in the world coordinate system output by the FAST-LIO2 algorithm and the pose constraint calculated by ICP are input into the GTSAM library for processing to obtain the pose use the optimized pose to update the Map (map) and Trajectory (trajectory).

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

[0150]

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

[0152] As Figure 3 shown, specifically, in this embodiment, the step S22 includes: storing the camera sensor image information into an image information queue, storing the odometry information into an odometry queue, using a buffer queue to realize time synchronization in combination with the NLerp linear interpolation method, including:

[0153] taking out the odometry information O m with the earliest timestamp ts=n1 from the odometry queue, checking the image information I n with the earliest time in the image information queue, if O m ​​the time of I n is discarded n if O m the time of I n is combined m and I n two data and output to the synchronization queue; if O m the time of I n is found from the odometer queue first O n the time greater than I n-1 and the first O n the time later than I n+1 , then the NLerp interpolation calculation is performed O n , combined O n and I n output to the synchronization queue. This processing not only effectively solves the problem of asynchronous data flow, but also significantly improves the performance and reliability of the system.

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

[0155]

[0156] wherein odom ts＝n represents the odometer state at timestamp ts=n, R ts＝n represents the rotation matrix at timestamp t ts＝n , t ts＝n represents the translation vector at timestamp ts=n, n represents the interpolation coefficient for linear interpolation.

[0157] Rotation matrix R:

[0158]

[0159] wherein q0, q1, q2, q3 are the components of the quaternion, representing the attitude; i, j, k are complex units.

[0160] Translation vector interpolation:

[0161]

[0162] wherein, represents the interpolated translation vector at timestamp ts=n2; represents the translation vector at timestamp ts=n1; represents the translation vector at timestamp ts=n3; n2 represents the interpolation coefficient.

[0163] Quaternion interpolation:

[0164]

[0165] wherein, interpolated quaternion at timestamp ts=n2; denotes a quaternion at timestamp ts=n1; denotes a quaternion at timestamp ts=n3; n2 denotes an interpolation coefficient.

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

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

[0168] As a preferred embodiment of the present application, step S3 obtains the joint information from the image obtained in step S2 and the odometer synchronization queue, and uses the joint information as the input of the YOLO-v8 network to obtain the picking coordinate point p pixel and the corresponding timestamp (ts) in the image coordinate system, and then converts p pixel into the picking point p gobal in the global coordinate system. After obtaining the picking coordinate point p pixel and the corresponding timestamp in the image coordinate system, step S3 includes:

[0169] S31 converts the p pixel into the picking point p gobal in the global coordinate system, including:

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

[0171]

[0172] wherein, denotes pixel coordinates, u i denotes the horizontal coordinate of the image point, v i denotes 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 Indicates 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 Join 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 This is a schematic diagram of the hardware structure of a method for generating waypoints for a UAV harvesting operation provided by an embodiment of the present invention. 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, for example, a program running the method for generating unmanned aerial vehicle picking operation waypoints. The processor 60 implements the steps in each of the above-described various embodiments of the method for generating unmanned aerial vehicle picking operation waypoints when executing the computer program 62. Alternatively, the processor 60 implements the functions of each module / unit in each of the above-described various apparatus embodiments when executing the computer program 62. Illustratively, the computer program 62 can be divided into one or more modules / units, which are stored in the memory 61 and executed by the processor 60 to complete the present application. The one or more modules / units can be a series of computer program instruction segments capable of completing a specific function, which are used to describe the execution process of the computer program 62 in the computer 6.

[0182] The computer 6 can be a desktop computer, a notebook, a palm computer, and a cloud server, etc. The computer 6 device can include, but is not limited to, a processor 60, a memory 61. Those skilled in the art can understand that, Figure 5 The computer 6 is only an example and does not constitute a limitation on the computer 6, and can include more or fewer components than shown, or combine certain components, or different components, for example, the computer 6 can also include an input / output device, a network access device, a bus, etc.

[0183] The processor 60 can be a central processing unit (CPU), and can also be 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. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor.

[0184] The memory 61 can be an internal storage unit of the computer 6, such as a hard disk or a memory of the computer 6. The memory 61 can 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, and the like equipped on the terminal device. Further, the memory 61 can include both the internal storage unit and the external storage device of the computer 6. The memory 61 is used to store the computer program and other programs and data required by the terminal device. The memory 61 can also be used to temporarily store data that has been output or will be output.

[0185] Those skilled in the art can clearly understand that, for the convenience and brevity of description, only the division of the above functional units and modules is exemplified, and in actual application, the above functions can be completed by different functional units and modules according to needs, that is, the internal structure of the apparatus is divided into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment can be integrated in one processing unit, or each unit can exist physically, or two or more units can be integrated in one unit, and the integrated unit can be realized in the form of hardware or software. In addition, the specific names of each functional unit and module are only for easy distinction, and do not limit the protection scope of the application. The specific working process of the units and modules in the system can refer to the corresponding process in the foregoing method embodiments, which will not be described here.

[0186] In the above embodiments, the description of each embodiment has its own emphasis, and the parts not described or recorded in detail in a certain embodiment can refer to the relevant description of other embodiments.

[0187] Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in combination with the embodiments disclosed herein can be realized in electronic hardware or a combination of computer software and electronic hardware. Whether the functions are executed in hardware or software depends on the specific application and design constraints of the technical solution. A person skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the present application.

[0188] In the embodiments of the present application, it should be understood that the disclosed apparatus / terminal device and method can be implemented in other manners. For example, the embodiments of the apparatus / terminal device described above are merely schematic, and the division of the modules or units is merely logical function division, and there can be another division manner in actual implementation. For example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the displayed or discussed mutual couplings or direct couplings or communication connections between the units can be indirect couplings or communication connections through some interfaces, devices or units, and can be electrical, mechanical or other forms.

[0189] The units described as separate components can or can not be physically separate, and the components shown as units can or can not be physical units, i.e., can be located in one place, or can be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the embodiments.

[0190] In addition, each functional unit in the various embodiments of the present application can be integrated into a processing unit, or each unit can exist physically, or two or more units can be integrated into one unit. The integrated unit can be implemented in the form of hardware or in the form of a software functional unit.

[0191] The integrated module / unit, if implemented in the form of a software functional unit and sold or used as an independent product, can be stored in a computer readable storage medium. Based on such understanding, all or part of the flow of the above-mentioned embodiment methods can be completed by a computer program instructing related hardware, and the computer program can be stored in a computer readable storage medium. When the processor executes the computer program, the steps of each method embodiment described above can be implemented. The computer program includes computer program code, which can be in the form of source code, object code, executable file or some intermediate form. The computer readable medium can include any entity or device capable of carrying the computer program code, recording medium, U disk, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal and software distribution medium, etc. It should be noted that the computer readable medium can include appropriate contents according to the requirements of legislation and patent practice in the jurisdiction, for example, in some jurisdictions, according to legislation and patent practice, the computer readable medium does not include electrical carrier signals and telecommunication signals.

[0192] The above-described embodiments are only used to illustrate the technical solutions of the present application, but not limit the present application; although the present application has been described in detail with reference to the foregoing embodiments, those ordinarily skilled in the art should understand: the technical solutions recorded in the foregoing embodiments can be modified, or some technical features can be replaced equivalently; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application, and should be included in the protection scope of the present application.

Claims

1.A method for generating a UAV picking operation waypoint, characterized in that, The method comprises the steps of: S1, constructing a combined odometer based on the FAST-LIO2 algorithm, comprising: S11, processing data of a laser radar sensor and an inertial measurement unit by using the FAST-LIO2 algorithm to obtain a point cloud map carrying a timestamp T based on a ROS time; S12, obtaining image information collected by a camera sensor, the image information carrying a timestamp T based on a ROS time; S2, constructing an information synchronization mechanism to realize external parameter calibration and information synchronization of the camera sensor and the laser radar sensor, comprising: S21, through calibration, a transformation matrix of a camera coordinate system to a laser radar coordinate system is obtained to obtain the image information conversion into image information based on the laser radar coordinate system; S22, based on the time stamp of the ROS and using the buffer queue combined with the NLerp interpolation method, time synchronization is performed on the odometer information provided by the laser radar sensor and the image information collected by the camera to obtain odometer information O n synchronized with the camera collected image information 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, 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, the output is the global picking point set P ts ; S4, constructing a point cloud map based on a data structure of a k-d tree with a flag bit and a global picking point set P ts , as a global collection map; S5, determining a picking point position and an orientation, comprising: S51, performing credibility analysis on the picking point position of the same object to be picked according to the global collection map to obtain a position with the highest credibility, and generating a final picking point position of each object to be picked; S52, determining the orientation 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 of each object to be picked and the orientation of the picking point. 2.The method of claim 1, wherein, The step S51 comprises: S511, calculating a density estimation of the picking point p in its neighborhood: Among them, h is the average width of the objects 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, compute gradient function Determine movement direction: where c k,3 denotes a regularization parameter, K ' (u) denotes the derivative of the Gaussian kernel function; The function g(s) = -K is introduced ' (S) gives: S513, calculate the movement vector m h (p), determine the new position: S514, update the location and timestamp of the picking point, when m h (p) = 0 stop: 3.The method of claim 1, wherein, The step S52 comprises: S521, defining a collision field N D , finding the field of the picking point p based on the data stored in the data structure of the k-d tree with the flag bit The point o in the field i , decentralizing o i to obtain o ' i , and constructing the covariance matrix C: S522, solving the minimum eigenvalue using singular value decomposition, the singular value decomposition solving 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, determine the normal vector direction by using the constraint point, which is specifically the odometer point corresponding to the time stamp of the picking point p, find the point O with the same time stamp in the odometer queue p , and then determine whether the normal vector n needs to be flipped by the following formula o : where d(O p , Dn o ) denotes the distance from point O p to the point along the n o direction at a distance D. S524, calculate the yaw angle ψ based on the X-Y plane: the normal vector n o Projection to the X-Y plane to get the yaw angle ψ of the waypoint: 4.The method of claim 1, wherein, The data structure of the k-d tree with a flag includes: a point type PointType point for storing point coordinates and intensity information; a pointer leftnode pointing to a left child node; a pointer rightnode pointing to a right child node; an integer variable partition_axis for indicating a partition axis; a floating-point variable timestamp for storing corresponding time information of a global sampling point; a floating-point array range for storing a bounding box vertex of the global sampling point; and a Boolean variable filter for filtering unnecessary laser radar point clouds, wherein the flag has three state bits: an obstacle point, a picking point, and a traversed picking point. 5.The method of claim 1, wherein, The step S1 further comprises: Step S13, reducing an error of the point cloud map with the timestamp T based on the ROS time by using a ScanContext method, and optimizing by an ICP algorithm and a 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 Calculate the cosine distance between the 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 A threshold τ is set, when d(I q ,I c )<τ, it is judged that the two matrices are similar; the current frame point cloud matrix I q corresponding to the original point cloud matrix P k is matched with the historical frame point cloud matrix I c corresponding to the original point cloud matrix ​ S132, solve the pose constraint using the ICP algorithm, based on the matched original point cloud P obtained in step S131 k and Solve the pose constraint represents the optimal rotation matrix, represents the optimal translation vector; Optimization objective function: R represents a rotation matrix, t denotes a translation vector, denotes the i-th point in the closed loop frame; S133, optimizing a pose constraint, inputting a pose in a world coordinate system output by the FAST-LIO2 algorithm and a pose constraint calculated by the ICP into the GTSAM library for optimization, and updating a map and a trajectory. 6.The method of claim 1, wherein, The step S21 comprises: converting a point in a camera coordinate system to a laser radar coordinate system: Rcam2Lidar represents the camera to lidar rotation matrix obtained after calibration, tcam2Lidar represents the camera to lidar translation vector obtained after calibration, where [X L ,Y L ,Z L ] T denotes the point in the laser radar coordinate system, [X C ,Y C ,Z C ] T denotes the point in the camera coordinate system. 7.The method of claim 1, wherein, The step S22 comprises: storing camera sensor image information into an image information queue and storing odometer information into an odometer queue, and realizing time synchronization by using a buffer queue and a NLerp linear interpolation method, comprising: taking out the earliest timestamped odometry information O from the odometry queue m , checking the earliest timestamped image information I from the image queue n , if the time of O m is later than I n , discarding I n , if the time of O m is equal to I n , combining O m and I n and outputting to the synchronization queue; if the time of O m is earlier than I n , finding the first O n with time greater than I n-1 and the first O n with time later than I n+1 from the odometry queue, then performing NLerp interpolation to calculate O n , combining O n and I n and outputting to the synchronization queue. 8.The method of claim 1, wherein, The identification and detection of the picking point of the object to be picked include identification and detection of the picking point of the object to be picked by a YOLO-v8 network. 9.The method of claim 1, wherein, The step S3 obtains the picking coordinate point p in the image coordinate system after the picking coordinate point p in the world coordinate system is obtained pixel and the corresponding timestamp, further comprising: S31, converting the p pixel into a picking point p gobal in a global coordinate system, comprising: S311, pixel coordinate conversion to a camera coordinate system: wherein denotes the pixel coordinate, u i denotes the horizontal coordinate of the image point, v i denotes the vertical coordinate of the image point; S312, camera coordinate system conversion to a laser radar coordinate system: wherein f x y denotes the focal length of the camera, c x y denotes the principal point coordinates of the camera, Z i is the depth value of the corresponding pixel point 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, laser radar coordinate system conversion to a global coordinate system: wherein, is a rotation matrix between the LiDAR coordinate system and the global coordinate system, is a translation vector between the LiDAR coordinate system and the global coordinate system; S32, picking point p in the global coordinate system is combined with the corresponding timestamp into a new data gobal with the corresponding timestamp into a new data into the set of time-stamped global picking points P ts . 10.A system for generating a UAV picking operation waypoint, comprising a memory, a processor, a computer program stored in the memory and capable of running on the processor, characterized in that, The processor implements the steps of the unmanned aerial vehicle picking operation waypoint generation method according to any one of claims 1 to 9 when executing the computer program. The processor implements the steps of the unmanned aerial vehicle picking operation waypoint generation method according to any one of claims 1 to 9 when executing the computer program.

Images