Method and device for planning gently harvesting trajectory of bunch fruit

Abstract

A method and device for planning a gently harvesting trajectory of bunch fruit, relating to the technical field of robots. The method comprises: determining a target space point set on a harvesting path of bunch fruit to be harvested, wherein the target space point set comprises starting and ending space points and at least one intermediate space point; using the target space point set to segment a reference trajectory of bunch fruit harvesting to obtain multiple segmented trajectories and reference trajectory segments corresponding to the segmented trajectories; and using the segmented trajectories to perform imitation learning calculation on the reference trajectory segments corresponding to the segmented trajectories, determining optimal learning trajectories corresponding to the segmented trajectories, and splicing the optimal learning trajectories to generate an optimal harvesting trajectory of said bunch fruit. The flexibility of a fruit harvesting robot for performing bunch fruit harvesting can be effectively improved, so that the robot has the capability of generating an optimal harvesting trajectory plan by learning manual skills, and the requirement of gently placing bunch fruit can be well met.

Description

Method and device for planning smooth collection trajectory of fruit bunches

[0001] This application claims priority to the Chinese patent application filed with the China Patent Office on September 28, 2023, with application number 202311270405.2 and invention name “Method and device for planning the smooth collection trajectory of fruit strings”, the entire contents of which are incorporated by reference into this application. Technical Field

[0002] The present invention relates to the field of robotics technology, and in particular to a method and device for planning a smooth collection trajectory for fruit bunches. Background Art

[0003] Fruit harvesting robots, with their unique advantage of replacing manual labor, have become a core element of smart agriculture. The development and application of fresh fruit picking robots can meet the production and application needs of standardized greenhouse and orchard cultivation scenarios. However, the development of harvesting robots often fails to consider the mechanical damage that can occur to the fresh fruit during the collection process. For example, when the actuator moves too quickly, the instantaneous rigid operation can cause the grapes to fall off. During the collection operation, the rigid operation can cause collisions with the surface on which the grapes are placed, damaging the grapes. Therefore, minimizing the damage rate during the harvesting process is essential for the harvesting of fresh grapes.

[0004] Rationally planning the collection trajectory of fruit bunches after picking to ensure smooth collection and prevent fruit drop, damage, and rot is a prerequisite for efficient, accurate, and non-destructive harvesting by harvesting robots, and has become a research hotspot in the field of agricultural robotics. However, existing imitation learning methods used by harvesting robots cannot meet the requirements for smooth placement of fruit bunches.

[0005] Summary of the Invention

[0006] The present invention provides a method and device for planning a trajectory for the smooth collection of bunches of fruit, which are used to solve the defect in the prior art that the imitation learning method performed by the existing harvesting robot cannot meet the demand for the smooth collection of bunches of fruit.

[0007] The present invention provides a method for planning a smooth collection trajectory of fruit bunches, comprising:

[0008] Determine a target spatial point set on a collection path of the bunches of fruit to be collected, wherein the target spatial point set includes a starting and ending spatial point and at least one intermediate spatial point;

[0009] Using the target space point set, the reference trajectory of the fruit string collection is segmented to obtain multiple segmented trajectories and reference trajectory segments corresponding to each segmented trajectory; the reference trajectory is determined based on multiple sets of manually taught trajectory information for collecting the fruit string;

[0010] Each segmented trajectory is used to perform imitation learning calculation on its corresponding reference trajectory segment, determine the optimal learning trajectory corresponding to each segmented trajectory, and splice each optimal learning trajectory to generate the optimal collection trajectory of the fruit bunch to be collected.

[0011] According to a method for planning a smooth collection trajectory of fruit bunches provided by the present invention, the method uses each segmented trajectory to perform imitation learning calculation on its corresponding reference trajectory segment to determine the optimal learning trajectory corresponding to each segmented trajectory, including:

[0012] Determining a learning trajectory for each segmented trajectory segment using a kernelized motion primitive algorithm based on a reference trajectory segment corresponding to each segmented trajectory segment;

[0013] A genetic algorithm is used to determine the optimal learning trajectory of each segmented trajectory for its corresponding reference trajectory segment, with the goal of minimizing the mean square error between the learning trajectory corresponding to each segmented trajectory and the reference trajectory segment corresponding to each segmented trajectory.

[0014] According to a method for planning a smooth collection trajectory for fruit bunches provided by the present invention, the method utilizes a kernelized motion primitive algorithm to determine a learning trajectory for each segmented trajectory according to a reference trajectory segment corresponding to each segmented trajectory, including:

[0015] Using the information divergence minimization method, the optimal solution of the mean and covariance between each segmented trajectory and its corresponding reference trajectory segment is calculated;

[0016] Based on the optimal solution of the mean and covariance, a learning trajectory of each segmented trajectory with respect to its corresponding reference trajectory segment is determined.

[0017] According to a method for planning a smooth collection trajectory of fruit bunches provided by the present invention, the method utilizes a genetic algorithm to determine the optimal learning trajectory for each segmented trajectory segment with respect to its corresponding reference trajectory segment, with the goal of minimizing the mean square error between the learning trajectory corresponding to each segmented trajectory segment and the reference trajectory segment corresponding to each segmented trajectory segment, including:

[0018] A two-layer encoding method is used to generate multiple chromosomes as an initial population according to the learning trajectory corresponding to each segmentation trajectory and the reference trajectory segment corresponding to each segmentation trajectory;

[0019] Determining the fitness function of the algorithm according to the mean square error between the learning trajectory corresponding to each segmented trajectory and the reference trajectory segment corresponding to each segmented trajectory;

[0020] Performing genetic operations based on the initial population to obtain an optimal chromosome when a preset termination condition is met;

[0021] According to the two-layer encoding method, the optimal chromosome is decoded to obtain the optimal learning trajectory of each segmented trajectory with respect to its corresponding reference trajectory segment.

[0022] According to a method for planning a trajectory for the flexible placement of fruit strings provided by the present invention, performing genetic operations based on the initial population to obtain an optimal chromosome when a preset termination condition is met includes:

[0023] Selecting multiple chromosomes with small fitness function values ​​from the initial population as multiple parent individuals;

[0024] Performing genetic operations on each of the parent individuals to generate a corresponding offspring population, and obtaining multiple new populations based on each of the parent individuals and the corresponding offspring population;

[0025] Retaining a plurality of target chromosomes with small fitness function values ​​from each of the new populations;

[0026] When a preset termination condition is met, a chromosome with the smallest fitness function value is determined from the multiple target chromosomes to obtain the optimal chromosome.

[0027] According to a method for planning a smooth collection trajectory of fruit bunches provided by the present invention, determining a target spatial point set on a collection path of the fruit bunches to be collected comprises:

[0028] Determining a starting spatial point on a collection path for the fruit bunches to be collected according to the initial hanging spatial position of the fruit bunches to be collected;

[0029] According to the size information of the fruit bunches to be collected and the size information and position information of the placement area, a terminal spatial point and a plurality of intermediate spatial points on the collection path of the fruit bunches to be collected are determined.

[0030] According to a method for planning a smooth collection trajectory for fruit bunches provided by the present invention, before segmenting the reference trajectory for fruit bunch collection using the target spatial point set to obtain multiple segmented trajectories and reference trajectory segments corresponding to each segmented trajectory, the method further includes:

[0031] Collecting multiple sets of artificial teaching trajectory information of the fruit strings as a training set;

[0032] The time series and displacement trajectory point sequence of each group of manual teaching trajectory information in the training set are used as teaching data. The Gaussian mixture model and Gaussian mixture regression algorithm are used to extract the trajectory distribution from the teaching data to generate a reference trajectory for fruit collection.

[0033] The present invention also provides a device for planning a smooth collection trajectory of fruit bunches, comprising:

[0034] a processing module, configured to determine a target spatial point set on a collection path of the bunches of fruit to be collected, wherein the target spatial point set includes a starting and ending spatial point and at least one intermediate spatial point;

[0035] a segmentation module for segmenting the reference trajectory of the fruit string collection using the target spatial point set to obtain multiple segmented trajectories and reference trajectory segments corresponding to each segmented trajectory; the reference trajectory is determined based on multiple sets of manually taught trajectory information for collecting the fruit string;

[0036] A generation module is used to use each segmented trajectory to perform imitation learning calculation on its corresponding reference trajectory segment, determine the optimal learning trajectory corresponding to each segmented trajectory, and splice each optimal learning trajectory to generate the optimal sh trajectory of the fruit to be collected.

[0037] The present invention provides a method and device for planning a trajectory for the smooth collection of fruit bunches. The method and device obtain a reference trajectory for fruit bunch collection by performing imitation learning calculations on multiple groups of manually taught trajectory information for collecting fruit bunches, so as to integrate human prior skills into the fruit bunch collection path planning. At the same time, the target space point set on the collection path of the fruit bunches to be collected is calculated, and the reference trajectory for fruit bunch collection is segmented to obtain multiple segmented trajectories and a reference trajectory segment corresponding to each segmented trajectory. At the same time, each segmented trajectory is subjected to imitation learning calculations on its corresponding reference trajectory segment to obtain the optimal learning trajectory corresponding to each segmented trajectory. Finally, the optimal collection trajectory for the fruit bunches to be collected is obtained by splicing. The segmented trajectory learning method is used to realize segmented adaptive trajectory planning, and a smooth collection trajectory that is closer to human skills is obtained. This can effectively improve the flexibility of the robot in executing fruit bunch collection, and enable the fruit harvesting robot to have the ability to generate the optimal collection trajectory planning by learning human skills, which can well meet the robot's demand for smooth collection of fruit bunches.

[0038] Figures in the specification

[0039] In order to more clearly illustrate the technical solutions in the present invention or the prior art, a brief introduction is given below to the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0040] FIG1 is a flow chart of a method for planning a smooth collection trajectory of fruit bunches provided by the present invention;

[0041] FIG2 is a schematic diagram of the trajectory of fruit bunch collection in the method for planning the smooth collection trajectory of fruit bunches provided by the present invention;

[0042] FIG3 is a schematic diagram of the spatial point states of the trajectory of the fruit bunch collection in the method for planning the smooth collection trajectory of the fruit bunch provided by the present invention;

[0043] FIG4 is a schematic structural diagram of the device for planning the smooth collection trajectory of fruit bunches provided by the present invention. DETAILED DESCRIPTION

[0044] To make the objectives, technical solutions, and advantages of the present invention more clear, the technical solutions of the present invention will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the embodiments described are only some of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts shall fall within the scope of protection of the present invention.

[0045] The following describes the method and device for planning the smooth collection trajectory of fruit bunches of the present invention in conjunction with Figures 1 to 4.

[0046] FIG1 is a flow chart of the method for planning the smooth collection trajectory of fruit bunches provided by the present invention. As shown in FIG1 , the method includes: step 110 , step 120 and step 130 .

[0047] Step 110, determining a target spatial point set on a collection path for placing the fruit bunches, the target spatial point set including a starting and ending spatial point and at least one intermediate spatial point;

[0048] Step 120 , segmenting the reference trajectory of the fruit string collection using the target space point set to obtain multiple segmented trajectories and reference trajectory segments corresponding to each segmented trajectory; the reference trajectory is determined based on multiple sets of manually taught trajectory information for collecting the fruit string;

[0049] Step 130 , using each segmented trajectory to perform imitation learning calculation on its corresponding reference trajectory segment, determining the optimal learning trajectory corresponding to each segmented trajectory, and splicing each optimal learning trajectory to generate the optimal collection trajectory for the fruit bunch to be collected.

[0050] Specifically, the fruit bunches to be collected described in the embodiment of the present invention refer to fruit bunches waiting for the harvesting robot to perform picking and collection trajectory planning, which may include bunches of fruits such as grapes, green grapes, loquats, mulberries, etc.

[0051] The artificial teaching trajectory information described in the embodiment of the present invention refers to the trajectory information of the fruit stringing process extracted from the video frame data captured by manually demonstrating the skill action of collecting fruit strings.

[0052] The reference trajectory described in the embodiment of the present invention refers to a set of reference trajectories generated by utilizing multiple sets of manual teaching trajectory information, learning the manual skills of fruit bunch collection from multiple manual demonstrations, and extracting the trajectory distribution characteristics of the fruit bunch collection process.

[0053] The target spatial point set described in the embodiment of the present invention refers to a set of multiple spatial points used to spatially segment the reference trajectory for collecting the bunches of fruit. These spatial points all come from the collection path trajectory of the bunches of fruit to be collected.

[0054] The multi-segment segmented trajectory described in the embodiment of the present invention refers to a multi-segment segmented trajectory obtained by replacing the spatial points at corresponding positions on the reference trajectory with the spatial points in the target spatial point set and then performing segmentation at the replacement points on the reference trajectory.

[0055] The reference trajectory segments described in the embodiment of the present invention refer to multiple segmented trajectories obtained by determining corresponding spatial points on the reference trajectory based on the positions of each spatial point in the target spatial point set, and then performing segmentation at these corresponding position points on the reference trajectory.

[0056] The optimal learning trajectory described in the embodiment of the present invention refers to a fitting trajectory with minimal information loss determined from the learned fitting trajectories by performing imitation learning calculation on each segmented trajectory segment corresponding to the reference trajectory segment.

[0057] The optimal collection trajectory described in the embodiment of the present invention refers to a complete trajectory for collecting fruit strings obtained by reconnecting the optimal learning trajectory learned from each segmented trajectory according to the previously segmented nodes.

[0058] In an embodiment of the present invention, in step 110, the spatial points on the collection path of the fruit bunch to be collected are calculated to obtain a target spatial point set. The target spatial point set includes the starting and ending spatial points and at least one intermediate spatial point. The starting and ending spatial points refer to the spatial points at the picking location of the fruit bunch to be collected and the spatial points after the fruit bunch to be collected has been placed steadily. For ease of description, the spatial points can refer to the endpoints on one side of the fruit bunch's stem. Intermediate spatial points refer to spatial points on the collection path of the fruit bunch to be collected other than the starting and ending spatial points. These intermediate spatial points can be selected from the spatial points at the inflection points on the collection path trajectory.

[0059] Based on the content of the above embodiment, as an optional embodiment, determining the target spatial point set on the collection path of the bunched fruits to be collected includes:

[0060] Determine the starting spatial point on the collection path of the fruit bunches to be collected according to the initial hanging spatial position of the fruit bunches to be collected;

[0061] According to the size information of the fruit bunches to be collected and the size information and position information of the placement area, the terminal space point and multiple intermediate space points on the collection path of the fruit bunches to be collected are determined.

[0062] Specifically, the size information of the fruit bunches to be collected described in the embodiment of the present invention includes the total length (including the stem) and width (which can be understood as the diameter of the circumscribed circle with the largest girth on the geometric appearance of the fruit bunches) of the fruit bunches when placed vertically.

[0063] The size information of the placement area described in the embodiment of the present invention includes the length and width of the placement area, and the position information of the placement area includes the height of the placement area above the ground.

[0064] FIG2 is a schematic diagram of the trajectory of fruit bunch collection in the method for planning the trajectory of smooth collection of fruit bunches provided by the present invention. As shown in FIG2 , is the starting point of the fruit collection trajectory, that is, the starting space point; is the insertion point, i.e. the middle space point, which can be understood as the time t that the fruit string is in the placement area v The state space position of the waypoints; It is the end point of the fruit collection trajectory, that is, the ending space point.

[0065] FIG3 is a schematic diagram of the spatial point state of the trajectory of the fruit bunch collection in the method for planning the smooth collection trajectory of the fruit bunch provided by the present invention. As shown in FIG3, in the embodiment of the present invention, the total length of the fruit bunch (including the fruit stem) is A1, the width is B, and t v The distance between the lowest point of the fruit string and the placement area is A2, and the height from the placement area to the ground is A f , the length of the placement area is W, the width is U, t n Indicates the end time. The end state is at the end space point The four vertices of the placement area can be expressed as E1(x1,y1), E2(x1,y2), E3(x2,y1), and E4(x2,y2). and end point The status is calculated as follows:

[0066] By using the coordinate system set in the placement area and the initial hanging space position of the fruit bunch to be collected, the starting space point on the collection path of the fruit bunch to be collected can be calculated.

[0067] Furthermore, the end spatial point and a plurality of intermediate spatial points on the collection path of the fruit bunches to be collected can be calculated based on the size information of the fruit bunches to be collected and the size information and position information of the placement area.

[0068] The method of the embodiment of the present invention, by determining each spatial point on the collection path of the fruit bunches to be collected, is conducive to the robot imitating the learned trajectory to be closer to the reference trajectory of human skills, and generating an adaptive operation trajectory passing through different nodes and endpoints to improve the flexibility of fruit bunch collection.

[0069] Based on the content of the above embodiment, as an optional embodiment, before segmenting the reference trajectory of the fruit collection using the target space point set to obtain multiple segmented trajectories and the reference trajectory segment corresponding to each segmented trajectory, the method further includes:

[0070] Collect multiple sets of manual teaching trajectory information of fruit strings as training sets;

[0071] The time series and displacement trajectory point sequence of each group of manual teaching trajectory information in the training set are used as teaching data. The Gaussian mixture model and Gaussian mixture regression algorithm are used to extract the trajectory distribution from the teaching data to generate the reference trajectory for fruit collection.

[0072] Specifically, the Gaussian mixture model (GMM) described in the embodiment of the present invention is used to estimate probability distribution and is a method for density estimation of data to achieve a balance between model complexity and the amount of training data.

[0073] The Gaussian mixture regression (GMR) algorithm described in the embodiment of the present invention is used to estimate the corresponding spatial value at each time t on the fruit bunch collection path through the distribution regression of the Gaussian mixture model to generate a set of Gaussian distributions.

[0074] In an embodiment of the present invention, after obtaining initial trajectory information from a manual demonstration of fruit string collection, the initial trajectory is aligned, segmented, and extended to generate a manual teaching trajectory. Based on the manual skill of fruit string collection trajectory planning learned from multiple human demonstrations, the trajectory distribution is encoded and extracted using the GMM and GMR algorithms to generate a set of reference trajectories for fruit string collection.

[0075] In an embodiment of the present invention, multiple sets of artificial teaching trajectory information of stringing fruits are collected as training sets, which can be expressed as It means that the skill is demonstrated M times, each demonstration has N data, N m Represents N data under the mth skill demonstration, time series t n,m ∈R L and displacement trajectory point sequence ξ n,m ∈R O Represents input and output respectively, where L and O represent input and output dimensions. d (d is the total dimension of input and output), then the time series t n,m ∈R, displacement trajectory point sequence ξ n,m ∈Rd-1 .

[0076] The time series t of each group of manual teaching trajectory information in the training set n,m , displacement trajectory point sequence ξ n,m As teaching data, GMM and GMR algorithms are used to extract trajectory distribution from the teaching data. In this embodiment, the number of Gaussian components is set to k, and a set of teaching data is decomposed into k groups of Gaussian distributions, each component corresponds to a sub-Gaussian distribution, μ k is the mean matrix of k groups of Gaussian distribution, Σ k is the covariance matrix of k Gaussian distributions, which describes the distribution of the data and the probability of each value. The parameters of the GMM model are iteratively obtained using the expectation maximization algorithm (EM). The probability distribution P(ξ|k) of the GMM output is expressed as follows:

[0077] Among them, k is the number of Gaussian components, decomposing a set of data into k Gaussian components, μ k Mean matrix, Σ k is the covariance matrix, which describes the distribution of data and the probability of occurrence of each value, t is the time series, and ξ is the d-1 dimensional trajectory.

[0078] In an embodiment of the present invention, P(ξ|k) represents a mixed probability model containing k sub-distributions in the overall distribution of teaching data. The data is clustered through continuous iteration to divide the overall teaching data into k clusters, which represents the probability distribution of the observed data in the population.

[0079] For the GMR algorithm, its input is the time series t n,m The output of the trained GMM is the robot's trajectory variables, such as the end position. Use regression to estimate the continuous time value of the query point ξ t Corresponding spatial values

[0080] For each Gaussian component k, the mean of the input and output μ k and covariance∑ k Decompose them separately and define them as follows: μ k ={μ t,k , μ ξ,k};

[0081] Given input t * , then the mean of Gaussian component k and covariance matrix It is expressed as follows:

[0082] Corresponding trajectory ξ OThe model estimates the distribution of is as follows:

[0083] in,

[0084] In this embodiment, the responsiveness of each Gaussian distribution to each data point is defined as h k (t * ), represents the data point t * The probability of k coming from a Gaussian distribution. Represents the probability density function of the multivariate Gaussian distribution.

[0085] In general, in this embodiment, by applying GMM to the teaching data, GMM can decompose the teaching data into k Gaussian distributions, each of which corresponds to a different state or behavior. The parameters of GMM include the mean, covariance matrix, and weight of each Gaussian distribution (the sum of the weights of these Gaussian distributions is 1). For each Gaussian distribution k, the mean μ of its output vector is calculated. k and the covariance matrix Σ k For a given input time vector t * , use the GMR algorithm to predict the output trajectory vector ξ O The GMR algorithm uses the input time vector t * The GMR algorithm calculates the conditional probability distribution P(ξ|k) of the output trajectory vector ξ with the trained GMM. The GMR algorithm calculates the conditional probability distribution P(ξ|k) of each Gaussian distribution k for a given input vector t * The responsivity h k (t * ), and then calculate the weighted average of the output vector Finally, the output vector sequence of GMR prediction is used to generate the reference trajectory for fruit collection.

[0086] The method of the embodiment of the present invention uses GMM to decompose the teaching data into multiple Gaussian distributions, and uses the GMR algorithm to predict the response of each Gaussian distribution to vectors at different times. It can effectively extract the reference trajectory sequence of the fruit string collection from the teaching data, provide accurate reference data for subsequent trajectory learning, and help improve the accuracy of the trajectory learning algorithm.

[0087] In an embodiment of the present invention, in step 120, the previously determined reference trajectory for collecting fruit bunches is segmented using each spatial point in the determined target spatial point set. The points at corresponding positions on the reference trajectory are replaced with each spatial point, and segmentation is then performed to obtain multiple segmented trajectories. Simultaneously, the reference trajectory is segmented according to the corresponding positions of each spatial point on the reference trajectory to obtain reference trajectory segments corresponding to each segmented trajectory.

[0088] Furthermore, in an embodiment of the present invention, in step 130, a robot arm trajectory planning algorithm can be used to perform imitation learning calculations on the corresponding reference trajectory segment for each segmented trajectory segment, thereby obtaining the optimal learned trajectory for each segmented trajectory segment. The optimal learned trajectories for each segmented trajectory segment are then sequentially connected end to end to generate the optimal placement trajectory for the fruit bunch to be collected. Finally, the robot can pick and place the fruit bunch to be collected according to the optimal collection trajectory, achieving a smooth collection effect for the fruit bunch.

[0089] The fruit bunch collection trajectory planning method of the embodiment of the present invention obtains a reference trajectory for fruit bunch collection by performing imitation learning calculation on multiple groups of manually taught trajectory information for collecting fruit bunches, so as to integrate human prior skills into the fruit bunch collection path planning. At the same time, by calculating the target space point set on the collection path of the fruit bunches to be collected, the reference trajectory for fruit bunch collection is segmented to obtain multiple segmented trajectories and a reference trajectory segment corresponding to each segmented trajectory. At the same time, each segmented trajectory is subjected to imitation learning calculation for its corresponding reference trajectory segment to obtain the optimal learning trajectory corresponding to each segmented trajectory. Finally, the optimal collection trajectory for the fruit bunches to be collected is obtained by splicing. The segmented trajectory imitation learning method is used to realize segmented adaptive trajectory planning and obtain a smooth collection trajectory that is closer to human skills. This can effectively improve the flexibility of the robot in executing fruit bunch collection, enable the fruit harvesting robot to have the ability to generate the optimal collection trajectory planning by learning human skills, and well meet the requirements for smooth placement of fruit bunches.

[0090] Based on the content of the above embodiment, as an optional embodiment, using each segmented trajectory segment to perform imitation learning calculation on its corresponding reference trajectory segment to determine the optimal learning trajectory corresponding to each segmented trajectory segment, including:

[0091] Using the kernelized motion primitive algorithm, the learning trajectory of each segmented trajectory is determined according to the reference trajectory segment corresponding to each segmented trajectory;

[0092] A genetic algorithm is used to determine the optimal learning trajectory for each segmented trajectory with respect to its corresponding reference trajectory segment, with the goal of minimizing the mean square error between the learning trajectory corresponding to each segmented trajectory and the reference trajectory segment corresponding to each segmented trajectory.

[0093] Specifically, the kernelized movement primitives (KMP) algorithm described in an embodiment of the present invention obtains a non-parametric skill learning model by minimizing the KL divergence (Kullback-Leibler divergence) between parameterized trajectories and sample trajectories and introducing kernel treatment.

[0094] The genetic algorithm described in the embodiments of this invention is a method that searches for optimal solutions by simulating the natural evolutionary process. This algorithm mathematically transforms the problem-solving process into processes similar to crossover and mutation of chromosomal genes in biological evolution. When solving complex combinatorial optimization problems, it can achieve better optimization results more quickly than conventional optimization algorithms.

[0095] In an embodiment of the present invention, the KMP algorithm may be used to learn the corresponding reference trajectory segment using each segmented trajectory to determine the learning trajectory of each segmented trajectory.

[0096] Based on the content of the above embodiment, as an optional embodiment, a kernelized motion primitive algorithm is used to determine a learning trajectory for each segmented trajectory according to a reference trajectory segment corresponding to each segmented trajectory, including:

[0097] Using the information divergence minimization method, the optimal solution of the mean and covariance between each segmented trajectory and its corresponding reference trajectory segment is calculated;

[0098] Based on the optimal solution of mean and covariance, the learning trajectory of each segmented trajectory for its corresponding reference trajectory segment is determined.

[0099] Specifically, assuming that the reference trajectory is represented by ξ, and there are three spatial points in the target spatial point set, represented as Represent the starting spatial point, the middle spatial point and the ending spatial point on the path of the fruit string to be placed. The reference trajectory is divided according to the spatial point nodes to obtain and Two segmented trajectories are calculated, and the partial trajectories are as follows The KMP algorithm is used to imitate and learn the corresponding reference trajectory segment to obtain the corresponding learning trajectory.

[0100] Specifically, in this embodiment, for each segmented trajectory segment, the KMP algorithm can be used to represent its motion pattern. The KMP algorithm is a method for modeling motion patterns, which is implemented by representing each segmented trajectory segment as a linear combination of a series of primitives. Each primitive is a function that describes a specific motion pattern and is represented by a center point μ. ω and a width ω parameterized. The parameters of the KMP algorithm include the center point and width of each primitive, which can be learned by minimizing the error.

[0101] Assuming ξ(t) is the learned trajectory, then: ξ(t)=Φ T (t)ω;

[0102] Where Φ(t)∈R BO×O , is the B-dimensional basis function, ω∈R BO is a normal distribution.

[0103] Among them, the average value μ ω and covariance∑ ω It is obtained by KL divergence, which ensures the minimum information loss in the imitation learning process. The process can be described by the following formula:

[0104] Where N is the total number of reference trajectories, and are the means of Gaussian components k respectively and covariance matrix and Input t n The probability distribution of parameter segmentation trajectory and t n The probability distribution of the corresponding reference trajectory is related, and KL(·||·) is the KL divergence between the two distributions.

[0105] Furthermore, kernel processing and vector and matrix derivation can be introduced, and the formula is as follows: k * =[k(t * ,t1)k(t * ,t2)…k(t * ,t N )];

[0106] Among them, k(t i ,t j ) is the kernel matrix, Ι o is the O-dimensional identity matrix, K is the evaluation matrix, which is the training input k(t i ,f j ), is a hyperparameter.

[0107] Furthermore, we can get the new input time t * The mean of the corresponding trajectory ξ(t) and variance The formula is as follows:

[0108] Where,

[0109] Among them, λ1 and λ2 are regularization terms used to constrain the mean and covariance of the prediction results.

[0110] Thus, the mean of the trajectory ξ(f) and variance The average value μ can be determined accordingly ω and covariance∑ ω , so that the normal distribution ω can be further obtained and the learned trajectory ξ(t) can be calculated.

[0111] For each segmented trajectory segment, the KMP algorithm can be used to represent its motion pattern. The KMP algorithm is a method for modeling motion patterns, which is implemented by representing each segmented trajectory segment as a linear combination of a series of primitives. Each primitive is a function that describes a specific motion pattern and is represented by a center point μ ω and a width ω parameterized. The parameters of the KMP algorithm include the center point and width of each primitive, which can be learned by minimizing the error.

[0112] In this embodiment, for each segmented trajectory segment and its corresponding reference trajectory segment, the error between the kernelized motion primitives between them is calculated (K is the evaluation matrix, which is the training input k(t i ,t j ) where the kernel matrix k(t i ,t j ) super parameters Used to control the scale of the trajectory. When is too large, the distance between the obtained data will increase, which may lead to underfitting of data features and poor learning effect. On the other hand, when If the value is too small, the distance between the obtained data will decrease, which will lead to overfitting and poor generalization effect, resulting in state deviation. The mean μ between each segmented trajectory segment and its corresponding reference trajectory segment can be calculated by minimizing the KL divergence. ω and covariance Σ ω Then, based on the optimal solution of the mean and covariance, the learning trajectory of each segmented trajectory for its corresponding reference trajectory segment is determined.

[0113] It should be noted that due to the hyperparameters The value is crucial to achieve the best performance in simulation, which requires adaptive value selection in combination with the balance between model complexity and generalization ability. The specific value of is not specifically limited. The method of the embodiment of the present invention performs trajectory learning by using the KMP algorithm to construct a motion pattern for each segmented trajectory corresponding to the reference trajectory segment, and combines it with the KL divergence minimization method to optimize the effect of control trajectory learning, thereby improving the reliability and efficiency of trajectory learning.

[0114] Furthermore, in an embodiment of the present invention, a genetic algorithm is used, and the mean square error (MSE) between the learning trajectory corresponding to each segmented trajectory and the reference trajectory segment corresponding to each segmented trajectory is set as the fitness function. With the goal of minimizing the MSE, the hyperparameters and trajectory of the iterative imitation learning are optimized, and the optimal learning trajectory for each segmented trajectory with respect to its corresponding reference trajectory segment can be obtained.

[0115] The method of the embodiment of the present invention uses the genetic algorithm to iteratively optimize the trajectory learned by the KMP algorithm and the corresponding reference trajectory, further controlling the minimum mean square error between the learned trajectory and the reference trajectory, ensuring that the optimal collection trajectory planning is learned by imitation, providing trajectory planning for the smooth operation of the harvesting robot, and improving the robot's ability to achieve smooth collection trajectory planning.

[0116] Based on the content of the above embodiment, as an optional embodiment, a genetic algorithm is used to determine the optimal learning trajectory for each segmented trajectory segment with respect to its corresponding reference trajectory segment, with the goal of minimizing the mean square error between the learning trajectory corresponding to each segmented trajectory segment and the reference trajectory segment corresponding to each segmented trajectory segment. The method includes:

[0117] A two-layer encoding method is used to generate multiple chromosomes as the initial population based on the learning trajectory corresponding to each segmentation trajectory and the reference trajectory segment corresponding to each segmentation trajectory;

[0118] The fitness function of the algorithm is determined based on the mean square error between the learning trajectory corresponding to each segmented trajectory and the reference trajectory corresponding to each segmented trajectory;

[0119] Perform genetic operations based on the initial population and obtain the optimal chromosome when the preset termination conditions are met;

[0120] According to the two-layer encoding method, the optimal chromosome is decoded to obtain the optimal learning trajectory of each segmented trajectory for its corresponding reference trajectory segment.

[0121] Specifically, in the genetic algorithm employed in this embodiment of the present invention, each solution can be defined by a chromosome. This embodiment employs a segmented encoding method to define chromosomes. A two-layer encoding method is employed for chromosomes. The first layer represents the sequence of learning trajectories corresponding to each segmented trajectory, which can be described as O; the second layer represents the sequence of reference trajectory segments corresponding to each segmented trajectory, which can be described as A.

[0122] In an embodiment of the present invention, parameters are first initialized, including initial population size, number of iterations, crossover probability, mutation probability, and other parameters. Furthermore, based on the above-described two-layer encoding method, multiple chromosomes are generated as the initial population. The number of chromosomes can be set to M, which yields M initial solutions. Each solution is a sequence of optimal learning trajectories corresponding to each segmented trajectory. These M solutions can all be considered the initial population. M is a preset value that can be set based on the control parameters of the genetic algorithm and can be set to 50, 100, 150, and so on.

[0123] Furthermore, in an embodiment of the present invention, the mean square error between the learning trajectory corresponding to each segmented trajectory and the reference trajectory corresponding to each segmented trajectory is used as the fitness function of the algorithm. Therefore, the fitness function fit can be expressed as:

[0124] Among them, ξ n represents the reference trajectory of the nth time series, It represents the learning trajectory of the nth time series obtained by the KMP algorithm, and M is the total time series value of the trajectory.

[0125] In this embodiment, genetic operations such as selection, crossover, and mutation are further performed based on the initial population, and when a preset termination condition is met, the optimal chromosome corresponding to the minimum of the final fitness function is obtained.

[0126] Furthermore, the obtained optimal chromosome is decoded according to the two-layer encoding method, thereby obtaining the optimal learning trajectory of each segmented trajectory for its corresponding reference trajectory segment.

[0127] The method of the embodiment of the present invention uses a two-layer encoding method to encode the learning trajectory corresponding to each segmented trajectory and the reference trajectory segment corresponding to each segmented trajectory, and searches for the optimal chromosome with the goal of minimizing the mean square error between the learning trajectory corresponding to each segmented trajectory and the reference trajectory segment corresponding to each segmented trajectory. This can ensure that the segmented trajectory learns the optimal trajectory and improve the ability of the fruit harvesting robot to plan the optimal collection trajectory.

[0128] Based on the content of the above embodiment, as an optional embodiment, performing genetic operations based on the initial population to obtain the optimal chromosome when a preset termination condition is met includes:

[0129] Select multiple chromosomes with small fitness function values ​​from the initial population as multiple parent individuals;

[0130] Perform genetic operations on each parent individual to generate the corresponding offspring population, and obtain multiple new populations based on each parent individual and the corresponding offspring population;

[0131] From each new population, multiple target chromosomes with small fitness function values ​​are retained;

[0132] When the preset termination conditions are met, the chromosome with the smallest fitness function value is determined from multiple target chromosomes to obtain the optimal chromosome.

[0133] Specifically, the process of genetic algorithm finding the optimal solution is the process of genetic operation, which mainly includes three genetic operations: selection operation, crossover operation and mutation operation.

[0134] For the selection operation, the roulette wheel selection method can be used to select a better solution. In the selection operation, multiple chromosomes with smaller fitness and ranked from small to high are selected from the N chromosomes of the initial population as multiple parent individuals.

[0135] For the crossover operation, the purpose of crossover is to generate new individuals after a certain combination of operations using the parent chromosome individuals. The effective inheritance of characteristics ensures that the information of the parent individuals is inherited to the offspring population.

[0136] In this embodiment of the present invention, a crossover operation is performed on each parent individual, exchanging randomly selected gene segments at the corresponding positions of O and A. However, due to the data characteristics of O in the chromosome, the crossover operation may cause data redundancy and loss of some service processing sequence information. In this case, to ensure the feasibility and effectiveness of the service processing sequence in the offspring chromosome, redundant gene positions can be checked and supplemented with missing gene data.

[0137] For the mutation operation, the purpose of mutation is to generate new individuals by randomly changing certain genes of the chromosomes and making minor disturbances to the chromosomes, thereby increasing population diversity.

[0138] Furthermore, through the three genetic operations described above, offspring populations are generated. The parent population and the corresponding offspring population are merged into a new population, and all chromosomes are sorted in descending order based on the calculated fitness. Based on the principles of natural evolution, chromosomes with poor fitness are eliminated, and only a certain number of chromosomes with good fitness are retained in the new population, thus obtaining multiple new populations.

[0139] Based on the above population evolution algorithm, for each new population, the above fitness calculation, selection operation, crossover operation and mutation operation are repeated until the preset termination condition is reached. When the preset termination condition is met, the chromosome with the smallest fitness function value, that is, the chromosome with the smallest mean square error, is determined from multiple target chromosomes, thereby obtaining the optimal chromosome.

[0140] The method of the embodiment of the present invention adopts a genetic algorithm to select multiple chromosomes with smaller fitness function values ​​from the initial population as multiple parent individuals, so as to facilitate the subsequent determination of the chromosome with the smallest fitness function value from multiple target chromosomes when the preset termination conditions are met, as the optimal chromosome, which is beneficial to improving the flexibility of the robot in placing fruits in a string.

[0141] The following describes the device for planning the smooth collection trajectory of fruit strings provided by the present invention. The device for planning the smooth collection trajectory of fruit strings described below and the method for planning the smooth collection trajectory of fruit strings described above can be referenced to each other.

[0142] FIG4 is a schematic diagram of the structure of the device for planning the smooth collection trajectory of fruit bunches provided by the present invention, as shown in FIG4 , comprising:

[0143] Processing module 410 is used to determine a target spatial point set on a collection path for placing fruit bunches, the target spatial point set including a starting and ending spatial point and at least one intermediate spatial point;

[0144] A segmentation module 420 is configured to segment the reference trajectory of the fruit string collection using the target spatial point set to obtain multiple segmented trajectories and reference trajectory segments corresponding to each segmented trajectory; the reference trajectory is determined based on multiple sets of manually taught trajectory information for collecting the fruit string;

[0145] The generation module 430 is used to use each segmented trajectory to perform imitation learning calculation on its corresponding reference trajectory segment, determine the optimal learning trajectory corresponding to each segmented trajectory, and splice each optimal learning trajectory to generate the optimal collection trajectory for the fruit bunch to be collected.

[0146] The device for planning the smooth collection trajectory of fruit bunches described in this embodiment can be used to execute the above-mentioned embodiment of the method for planning the smooth collection trajectory of fruit bunches. Its principles and technical effects are similar and will not be repeated here.

[0147] The device for planning a smooth collection trajectory of fruit bunches in an embodiment of the present invention obtains a reference trajectory for placing the fruit bunches by performing imitation learning calculations on multiple groups of manually taught trajectory information for placing the fruit bunches, so as to integrate human prior skills into the fruit bunch collection path planning. At the same time, by calculating the target spatial point set on the placement path of the fruit bunches to be collected, the reference trajectory for collecting the fruit bunches is segmented to obtain multiple segmented trajectories and a reference trajectory segment corresponding to each segmented trajectory. At the same time, each segmented trajectory is subjected to imitation learning calculations on its corresponding reference trajectory segment to obtain the optimal learning trajectory corresponding to each segmented trajectory. Finally, the optimal placement trajectory of the fruit bunches to be collected is obtained by splicing them together. The segmented trajectory learning method is used to realize segmented adaptive trajectory planning and obtain a smooth collection trajectory that is closer to human skills. This can effectively improve the flexibility of the robot in executing fruit bunch collection, enable the fruit harvesting robot to have the ability to generate the optimal collection trajectory planning by learning human skills, and well meet the needs of smooth placement of fruit bunches.

[0148] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, i.e., they may be located in one location or distributed across multiple network units. Some or all of the modules may be selected based on actual needs to achieve the objectives of the present embodiment. Persons of ordinary skill in the art will be able to understand and implement the present invention without inventive effort.

[0149] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus a necessary general hardware platform, or of course, by hardware. Based on this understanding, the essence of the above technical solution or the part that contributes to the existing technology can be embodied in the form of a software product. The computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or certain parts of the embodiments.

[0150] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A method for planning a smooth collection trajectory of fruit bunches, characterized in that: include: Determine a target spatial point set on a collection path of the bunches of fruit to be collected, wherein the target spatial point set includes a starting and an ending spatial point and at least one intermediate spatial point; Using the target space point set to segment the reference trajectory of the fruit collection, a plurality of segmented trajectories and a reference trajectory segment corresponding to each segmented trajectory are obtained; The reference trajectory is determined based on the collection of multiple sets of artificial teaching trajectory information of the stringed fruit; Each segmented trajectory is used to perform imitation learning calculation on the corresponding reference trajectory segment, determine the optimal learning trajectory corresponding to each segmented trajectory, and splice each optimal learning trajectory to generate the optimal collection trajectory of the bunch of fruits to be collected.

2. The method for planning the smooth collection trajectory of fruits in a bunch according to claim 1, characterized in that: The method of using each segmented trajectory to perform imitation learning calculation on the reference trajectory segment corresponding to the segmented trajectory to determine the optimal learning trajectory corresponding to each segmented trajectory includes: Determine the learning trajectory of each segmented trajectory according to the reference trajectory segment corresponding to each segmented trajectory by using a kernelized motion primitive algorithm; A genetic algorithm is used to determine the optimal learning trajectory of each segmented trajectory for its corresponding reference trajectory segment, with the goal of minimizing the mean square error between the learning trajectory corresponding to each segmented trajectory and the reference trajectory segment corresponding to each segmented trajectory.

3. The method for planning the smooth collection trajectory of fruits in clusters according to claim 2, characterized in that: The method of using the kernelized motion primitive algorithm to determine the learning trajectory of each segmented trajectory according to the reference trajectory segment corresponding to each segmented trajectory includes: Using the information divergence minimization method, the optimal solution of the mean and covariance between each segmented trajectory and its corresponding reference trajectory segment is calculated; Based on the optimal solution of the mean and the covariance, a learning trajectory of each segmented trajectory with respect to its corresponding reference trajectory segment is determined.

4. The method for planning the smooth collection trajectory of fruits in clusters according to claim 2, characterized in that: The method of using a genetic algorithm to minimize the mean square error between the learning trajectory corresponding to each segmented trajectory and the reference trajectory corresponding to each segmented trajectory, and determining the optimal learning trajectory for each segmented trajectory with respect to its corresponding reference trajectory segment, comprises: A two-layer encoding method is adopted to generate multiple chromosomes as an initial population according to the learning trajectory corresponding to each segmented trajectory and the reference trajectory segment corresponding to each segmented trajectory; Determining the fitness function of the algorithm according to the mean square error between the learning trajectory corresponding to each segmented trajectory and the reference trajectory corresponding to each segmented trajectory; Performing genetic operations based on the initial population to obtain an optimal chromosome when a preset termination condition is met; According to the two-layer encoding method, the optimal chromosome is decoded to obtain the optimal learning trajectory of each segmented trajectory for its corresponding reference trajectory segment.

5. The method for planning the smooth collection trajectory of fruits in a bunch according to claim 4, characterized in that: The performing of genetic operation based on the initial population to obtain the optimal chromosome when a preset termination condition is met includes: Selecting multiple chromosomes with small fitness function values ​​from the initial population as multiple parent individuals; Performing genetic operations on each of the parent individuals to generate a corresponding offspring population, and obtaining multiple new populations based on each of the parent individuals and the corresponding offspring population; From each of the new populations, retain multiple target chromosomes with small fitness function values; When a preset termination condition is met, a chromosome with the smallest fitness function value is determined from the multiple target chromosomes to obtain the optimal chromosome.

6. The method for planning the smooth collection trajectory of fruits in a bunch according to any one of claims 1 to 5, characterized in that: The step of determining a target spatial point set on a collection path of the bunches of fruits to be collected comprises: Determining a starting spatial point on a collection path of the fruit bunches to be collected according to the initial hanging spatial position of the fruit bunches to be collected; According to the size information of the fruit bunches to be collected and the size information and position information of the placement area, the terminal space point and multiple intermediate space points on the collection path of the fruit bunches to be collected are determined.

7. The method for planning the smooth collection trajectory of fruits in a bunch according to any one of claims 1 to 5, characterized in that: Before segmenting the reference trajectory of the fruit collection using the target space point set to obtain multiple segmented trajectories and reference trajectory segments corresponding to each segmented trajectory, the method further includes: Collecting multiple groups of artificial teaching trajectory information of fruit strings as training sets; The time series and displacement trajectory point sequence of each group of artificial teaching trajectory information in the training set are used as teaching data, and the trajectory distribution is extracted from the teaching data using a Gaussian mixture model and a Gaussian mixture regression algorithm to generate a reference trajectory for stringing fruit collection.

8. A device for planning the smooth collection trajectory of fruit bunches, characterized in that: include: A processing module, used to determine a target spatial point set on a collection path of the bunches of fruits to be collected, wherein the target spatial point set includes a starting and an ending spatial point and at least one intermediate spatial point; A segmentation module, used to segment the reference trajectory of the fruit collection using the target space point set, to obtain multiple segmented trajectories and a reference trajectory segment corresponding to each segmented trajectory; The reference trajectory is determined based on collecting multiple sets of artificial teaching trajectory information of the stringed fruit; The generation module is used to use each segmented trajectory to perform imitation learning calculation on the corresponding reference trajectory segment, determine the optimal learning trajectory corresponding to each segmented trajectory, and splice each optimal learning trajectory to generate the optimal collection trajectory of the bunch of fruits to be collected.

Images