A path planning method and system for a ball picking robot

Abstract

The present invention discloses a path planning method and system for a ball picking robot, and relates to the field of path planning technology. The path planning method comprises the following steps: obtaining data of a collection target and a ball picking robot; setting the coordinates of the ball to be collected as the target point, and the starting point and the end point of the path as the stop points; establishing a closed path model; setting the target point as a real point, and determining a point near the real point that is passed by the center line of the path as a virtual point; using a clustering algorithm to determine the point cluster, and using the Monte Carlo method to update the center point of each initial point cluster to generate a final point cluster; sorting the virtual points and the center point in the point cluster by a simulated annealing algorithm to form an initial path; fine-tuning the virtual points and the center point on the initial path, and outputting the final path planning. Using the path planning method of the present invention, the center line of the robot path does not need to directly pass through the point where the target ball is located, and only needs to pass near the ball to achieve storage.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a path planning method and system for a ball picking robot. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] As a new type of service robot, the ball-picking robot can automatically collect the target balls in the training venue during the multi-ball practice of tennis, table tennis and other athletes, which can save a lot of energy and time for manual ball picking. At present, relatively mature ball-picking robots have appeared in the domestic and foreign markets. They can realize robot positioning and navigation through vision, laser radar, ultrasonic and other sensors, automatically identify tennis balls and plan the path to pick up the balls.

[0004] In recent years, researchers have gradually begun to study ball-picking robots. Since most robots use computer vision to identify the target ball and locate and navigate the robot according to the relative position, one of the key technologies is the identification of the target ball. The main technologies used include geometric features and template matching, color recognition, contour feature recognition, edge detection, McCann-Frankle Retinex algorithm and other classic image processing algorithms. Most of these studies on the path planning of ball-picking robots compare this problem to the solution of the TSP problem. The TSP problem is a typical combinatorial optimization problem and has been widely used in logistics distribution, aircraft route scheduling, highway network planning and other fields. Heuristic algorithms are mainly used to solve large-scale TSP problems, such as simulated annealing algorithm (SA), genetic algorithm, ant colony algorithm and particle swarm algorithm. Among them, SA is widely used in solving TSP problems, and the algorithm principle is simple and the convergence speed of solving problems is fast.

[0005] However, the inventors found that the existing algorithms solve the path planning problem of a single target point. Most robots are designed with structures similar to "armspan" or "roller" to facilitate the contact between the robot and the ball, thereby improving the ball collection effect. Therefore, unlike the point-to-point relationship of the manipulator grabbing objects, the ball-picking robot and the ball form a line-to-point relationship. The robot's walking path is a strip plane with a fixed cross-sectional length, or it can be described as a path that takes width into consideration. If the ball is simplified to a point, the strip plane formed by the point and the path can be collected if it overlaps. However, the existing algorithm does not consider the problem of path width in the above case. The center line of the robot path needs to pass through the point where the target ball is located to realize the ball picking process, which greatly reduces the efficiency of picking up the ball. Summary of the invention

[0006] In view of the shortcomings of the prior art, the purpose of the present invention is to provide a path planning method and system for a ball picking robot. The present invention takes the path width into consideration and proposes a simulated annealing algorithm that integrates k-means clustering and Monte Carlo method. The center line of the robot path does not need to directly pass through the point where the target ball is located, but only needs to pass near the ball to achieve collection.

[0007] In order to achieve the above object, the present invention is implemented through the following technical solutions:

[0008] A first aspect of the present invention provides a path planning method for a ball picking robot, comprising the following steps:

[0009] Obtain the data of the collection target and the ball-picking robot; set the coordinates of the ball to be collected as the target point, and the starting point and the end point of the path as the stop points;

[0010] Establish a closed path model based on stop points, target points and path width;

[0011] The target point is set as a real point, and a point near the real point that is passed by the center line of the path is determined as a virtual point;

[0012] Clustering algorithm is used to determine point clusters, and the real points in the point clusters that can share a virtual point are named as center points. The Monte Carlo method is used to update the center points of each initial point cluster to generate the final point cluster.

[0013] The virtual points and central points in the point cluster are sorted by the simulated annealing algorithm to form an initial path;

[0014] Fine-tune the virtual points and center points on the initial path and output the final path planning.

[0015] Furthermore, multiple real points can correspond to the same virtual point, and the virtual point of the stop point is itself.

[0016] Furthermore, the virtual point is assumed to be a point in a circle corresponding to the real point, with the coordinates of the real point as the center and half the length of the ball receiving structure as the radius, and a closed path model is established with the shortest closed path passing through the virtual point as the goal.

[0017] Furthermore, the specific steps of using clustering algorithm to determine point clusters are:

[0018] An initial point cluster is established with each real point as the center point. The real points included in each initial point cluster are determined by comparing the distance between the center point and other real points with half the length of the ball collection structure.

[0019] Furthermore, if the distance from a solid point to other solid points is greater than half the length of the ball receiving structure, then the solid point is named an independent solid point.

[0020] Furthermore, the Monte Carlo method is used to update the center points of each initial point cluster. The specific steps to generate the final point cluster are:

[0021] Get the maximum and minimum values ​​of the horizontal and vertical coordinates of all real points in the point cluster corresponding to the center point to form a rectangular search box;

[0022] A point is randomly generated in the rectangular search box, and the real points contained in the circle with the random point as the center and half the length of the ball structure as the radius are calculated. If the number of real points is greater than the origin cluster, the real points of the current point cluster are updated, and the center point of the current point cluster is updated to the random point;

[0023] Eliminate the situation where the same real point in the initial point cluster belongs to different initial point clusters, determine the mapping relationship between all real points and point clusters, set all initial point clusters to an unprocessed state, and generate all point clusters.

[0024] Furthermore, the specific steps to generate all point clusters are as follows: Find the point cluster with the most real points in the unprocessed point clusters, assuming it is c k , marked as processed; delete other unprocessed point clusters and c k Overlapping real points; repeat the operation until all point clusters are processed.

[0025] Furthermore, the virtual points and the center points in the point cluster are sorted by the simulated annealing algorithm, and the specific steps of forming the initial path are as follows:

[0026] The objective function is set according to the coordinates of the virtual points corresponding to each independent real point and the order of all virtual points and center points on the center line of the path;

[0027] A greedy algorithm is used to initially sort all virtual points and center points to obtain an initial solution;

[0028] New solutions are generated by changing the order of virtual points and center points in the path and randomly changing the coordinates of virtual points corresponding to independent real points;

[0029] Determine whether to accept the new solution, determine the final solution, and get the initial path based on the final solution.

[0030] Furthermore, the specific steps of fine-tuning the points on the initial path and outputting the final path planning are as follows:

[0031] Starting from the stop point, each virtual point and the center point that the center line of the path passes through are fine-tuned one by one. The virtual points are fine-tuned using geometric calculation method, and the center point is fine-tuned using Monte Carlo method.

[0032] A second aspect of the present invention provides a path planning system for a ball picking robot, comprising:

[0033] The data acquisition module is configured to acquire data of the acquisition target and the ball-picking robot; the coordinates of the ball to be collected are set as the target point, and the starting point and the end point of the path are used as the stop points;

[0034] A model building module is configured to build a closed path model according to the stop points, the target points and the path width;

[0035] The point cluster determination module is configured to set the target point as a real point, determine a point near the real point that is passed by the path center line as a virtual point; use a clustering algorithm to determine the point cluster, name the real point in the point cluster that can share a virtual point as the center point, and use the Monte Carlo method to update the center point of each initial point cluster to generate the final point cluster;

[0036] The annealing algorithm optimization module is configured to sort the virtual points and the center points in the point cluster by using a simulated annealing algorithm to form an initial path;

[0037] The path planning module is configured to fine-tune the virtual points and the center point on the initial path to obtain the final path planning.

[0038] One or more of the above technical solutions have the following beneficial effects:

[0039] The present invention discloses a path planning method and system for a ball picking robot that takes path width into consideration. The point clusters to which each point belongs and the corresponding center points are determined by integrating the idea of ​​the k-means clustering algorithm and the Monte Carlo method, and then the improved simulated annealing algorithm is used to solve the initial route. Finally, the positions of each virtual point or the center point of the point cluster are adjusted by geometric calculation and the Monte Carlo method to obtain the final path. As long as the center line of the path passes through the center point of the point cluster, the path can cover all the real points in the point cluster, and there is no need to pass through each real point, which greatly reduces the path length. In the path planning method of the present invention, the center line of the robot path does not need to directly pass through the point where the target ball is located, and only needs to pass near the ball to achieve storage.

[0040] Advantages of additional aspects of the present invention will be given in part in the following description, and in part will become obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0042] Figure 1 This is a flow chart of a path planning method for a ball picking robot in Embodiment 1 of the present invention;

[0043] Figure 2This is a schematic diagram of the ball collecting structure of the "armspan" type ball picking robot in the first embodiment of the present invention;

[0044] Figure 3 This is a schematic diagram of the ball collecting structure of the "drum" type ball picking robot in the first embodiment of the present invention;

[0045] Figure 4 Schematic diagram of independent real points, virtual points, central points and point clusters in the first embodiment of the present invention;

[0046] Figure 5 This is a schematic diagram of the tangency between a circle and an ellipse in the first embodiment of the present invention;

[0047] Figure 6 A schematic diagram of fine-tuning the center point using the Monte Carlo method in Embodiment 1 of the present invention;

[0048] Figure 7 This is a comparison diagram of algorithm effects of different parameters during the simulation experiment in Example 1 of the present invention;

[0049] Figure 8 This is a Monte Carlo parameter analysis diagram during the simulation experiment in Example 1 of the present invention;

[0050] Fig. 9 It is a convergence curve diagram of the path planning method during the simulation experiment in the first embodiment of the present invention;

[0051] Fig.10 It is a comparison chart of the results of SA_KM in different objective environments during the simulation experiment in Example 1 of the present invention. DETAILED DESCRIPTION

[0052] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used in this embodiment have the same meanings as those commonly understood by ordinary technicians in the technical field to which the present application belongs.

[0053] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "include" and / or "include" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or their combinations;

[0054] Embodiment 1:

[0055] Classic robot path planning problems are mostly about finding a collision-free path from the starting point to the end point in a specified area. The problem of the present invention is to realize path planning under a known map, which belongs to the category of global path planning. Its algorithms are mainly divided into graph-based search algorithms, sampling-based algorithms and heuristic intelligent algorithms. Most of these studies on the path planning of ball-picking robots compare this problem to the TSP problem. However, if the ball-collecting structure of the robot is considered, it can be found that the problem of the present invention is different from the TSP problem. The traditional TSP problem does not consider the path width and needs to reach each target point. The present invention only needs to reach the vicinity of the target point. In order to solve the problem of path planning of the ball-picking robot considering the path width, this embodiment first makes the following assumptions: 1. The volume of the ball is not considered, and it is regarded as a point in the plane; 2. The robot cannot retreat, but can only turn and move forward in place; 3. The ball touched within the arm span of the robot can be collected, regardless of the change in the position of the ball after being collided with by the robot; 4. The robot starts from a fixed stop point and returns to the stop point after picking up the ball. Based on the above assumptions, this embodiment establishes a mathematical model for the problem, proposes the concepts of point clusters and center points, and determines the point clusters and corresponding center points of each point by integrating the idea of ​​k-means clustering algorithm and Monte Carlo method. Then, the improved simulated annealing algorithm is used to solve the initial route. Finally, the positions of each virtual point or the center point of the point cluster are adjusted by geometric calculation and Monte Carlo method to obtain the final path. The simulation experiment results show that the algorithm can greatly improve the ball picking efficiency of the ball picking robot. At the same time, the experiment has obtained the influence of the robot's working environment and arm length on the algorithm, and the influence of parameters on the algorithm effect. The problem of the present invention and the TSP problem are both path planning for multiple target points. If the points that the center line of the path must pass through can be determined, it can be approximately converted to the TSP problem.

[0056] Embodiment 1 of the present invention provides a path planning method for a ball picking robot, such as Figure 1 As shown, the following steps are included:

[0057] Step 1: Obtain the data of the collection target and the ball-picking robot; set the coordinates of the ball to be collected as the target point, and the starting point and the end point of the path as the stop points; establish a closed path model based on the stop points, the target point and the path width; set the target point as a real point, and determine a point near the real point that is passed by the center line of the path as a virtual point;

[0058] Step 2: Use clustering algorithm to determine point clusters, name the real points in the point clusters that can share a virtual point as center points, use Monte Carlo method to update the center points of each initial point cluster to generate the final point cluster;

[0059] Step 3: Sort the virtual points and center points in the point cluster by simulated annealing algorithm to form an initial path;

[0060] Step 4: Fine-tune the virtual points and center points on the initial path and output the final path planning.

[0061] In step 1, the problem of the present invention can be summarized as there are n target points and one stop point on the plane, and a closed path P with a starting point and an end point as the stop point and a width of 2r is to be found. The strip plane covered by the path must contain all the target points, and the length of the center line of the path must be the shortest. To eliminate ambiguity, the target point is called a real point, and the coordinates are p and i (x i ,y i ), i = 1, 2, 3...n, the coordinates of the stop point are p o (x o ,y o ). The set S represents the set of all point numbers, that is, S = {1, 2, 3...n, o}.

[0062] Considering the path width, all real points whose distance from the path centerline is less than or equal to r can be included in the path, so each real point can correspond to a nearby point as long as the path centerline passes through the point. Among them, r is half of the length of the ball receiving structure, such as Figure 2 and Figure 3 As shown, Figure 2 It is a robot with an "armspan" ball-collecting structure. Figure 3 For robots with a "drum" type ball receiving structure, the length of the ball receiving structure is 2r. In the present invention, the path width is determined according to the length of the ball receiving structure, so r can also be expressed as half of the path width. It should be noted that the ball receiving structure includes but is not limited to the "arm span" and "drum" types. Any similar ball receiving structure that allows multiple balls to enter the robot storage part side by side at the same time should be within the protection scope of the present invention.

[0063] Define the points near the real point as virtual points. And multiple real points can correspond to the same virtual point, and the virtual point of the stop point is itself. Therefore, this problem can be converted into an approximate TSP problem. Let the virtual point be a point in the circle corresponding to the real point with the real point coordinate as the center and half the length of the ball collection structure as the radius. The closed path model is established with the shortest closed path passing through all virtual points as the goal:

[0064] Assume that the virtual point p′ i (x′ i ,y′ i ) is a real point p i (x i ,y i ) corresponds to p i (x i ,y i ) is a point inside a circle with center r as radius, p′ o =p o,i∈S, find the i The shortest closed path. Set the decision variable to z i,j , z i,j =1 means the path centerline starts from p′ i Arrival p′ j , p′ j The coordinates are (x′ j ,y′ j ). Otherwise z i,j =0, the mathematical model is as follows:

[0065] min∑ i∈S ∑ j∈S z i,j d i,j (1)

[0066]

[0067]

[0068] (x′ o -x o ) 2 +(y′ o -y o ) 2 =0 (4)

[0069] ∑ i∈S z i,j =1,j∈S (5)

[0070] ∑ j∈S z i,j =1,i∈S (6)

[0071]

[0072] z i,j ∈{0,1},i,j∈S (8)

[0073] Formula (1) is the objective function, which finds the shortest distance of the closed path passing through all virtual points; Formula (2) calculates the distance between virtual points; Formula (3) requires that the virtual point corresponding to the real point is within a circle with the real point as the center and r as the radius; Formula (4) limits the virtual point corresponding to the stop point; Formulas (5) and (6) ensure that each virtual point can only enter and exit once; Formula (7) is a sub-loop elimination constraint. There are infinite possibilities for the coordinates of the virtual points corresponding to each real point in this problem, which increases the difficulty of solving the TSP problem based on virtual points. The traditional TSP problem has been proved to be an NP-hard problem, so the problem of the present invention is also an NP-hard problem and cannot be solved in polynomial time. Therefore, the present invention designs a simulated annealing algorithm that integrates k-means clustering and Monte Carlo method to solve it.

[0074] In step 2, since multiple real points can share one virtual point, the present invention proposes the concept of point clusters and improves the k-means clustering algorithm to generate point clusters.

[0075] The present invention refers to the set of real points that can be included in the path when the center line of the robot path passes through a certain point at any angle as a point cluster. Therefore, a point cluster is a set of several special real points, and the real points in the point cluster can share a virtual point, which is named the center point. The point cluster and the real points in the point cluster all correspond to a center point. The center point can be a real point or a virtual point. Let p″ k (k≤n) represents the center point, and the corresponding point cluster is c k , the symbol dis(α,β) represents the distance between points α and β, then c k ={p i |i∈S&dis(p i ,p″ k )≤r}. Figure 4 As shown, p 1 ,p 2 ,p 4 The center point of the point cluster is p″ 4 .

[0076] According to the definition of point clusters and center points, it can be found that as long as the center line of the path passes through the center point of the point cluster, the path can cover all the real points in the point cluster, and no longer needs to pass through each real point, which will greatly reduce the path length. Therefore, how to classify the real points into different point clusters will affect the path planning effect. The present invention designs a k-means clustering algorithm that integrates Monte Carlo to obtain each point cluster and its center point:

[0077] S1: Use clustering algorithm to determine point clusters:

[0078] Take each real point as the center point (cluster center) to establish the initial point cluster, and determine the real points included in each initial point cluster by comparing the distance between the center point and other real points with half the length of the ball collection structure r. If the distance between a real point and other real points is greater than half the length of the ball collection structure r, then the real point is named an independent real point.

[0079] S2: Use the Monte Carlo method to update the center points of each initial point cluster to generate the final point cluster, so that each point cluster can contain more real points. The number of experiments in the Monte Carlo method is expressed by the parameter L 1 For each center point p″ determined in S1 k , perform the following operations:

[0080] S2.1: Get the maximum and minimum values ​​of the horizontal and vertical coordinates of all real points in the point cluster corresponding to the center point, forming a rectangular search box: get p″ kThe corresponding point cluster c k The maximum and minimum horizontal and vertical coordinates of all real points in The horizontal axis is formed by arrive The vertical axis is arrive Rectangular search box. Repeat S2.1L 1 Second-rate..

[0081] S2.2: Generate a point randomly in the rectangular search box, calculate the real points contained in the circle with the random point as the center and half the length of the ball structure r as the radius. If the number of real points is greater than the origin cluster, update the real points of the current point cluster and update the center point of the current point cluster to the random point.

[0082] S3: Eliminate the situation where the same real point in the initial point cluster belongs to different initial point clusters, determine the mapping relationship between all real points and point clusters, set all initial point clusters to an unprocessed state, and generate all point clusters.

[0083] S3.1: The specific steps of generating all point clusters are as follows: Find the point cluster with the most real points among the unprocessed point clusters, assuming it is c k , marked as processed; delete other unprocessed point clusters and c k Overlapping real points; loop S3.1 until all point clusters are processed and the algorithm ends.

[0084] Figure 4 The generation process of point clusters is shown in Figure 2. 1 ,p 2 ) <r,dis(p 1 ,p 4 ) and is(p 2 ,p 4 ) is greater than r, so p 1 and p 2 The initial point clusters with the center point do not contain p 4 , use the Monte Carlo method to search for point p″ in the dashed box in the figure 4 , with p″ 4 The point cluster c is the center point 4 Can increase p 4 After that, it becomes a new point cluster. In addition, p 3 is an independent real point and does not belong to any point cluster. 3 Any point in a circle with r as the center and r as the radius can be used as p 3 A virtual point, such as p′ 3 .

[0085] In step 3, after clustering the real points in step 2, the real points are divided into two categories: independent real points and real points in point clusters. For independent real points, the algorithm needs to make the path center line pass through the corresponding virtual point, while for real points in point clusters, the path center line needs to pass through the center point of the point cluster. The present invention converts the problem into a TSP problem with vertices corresponding to the virtual points of independent real points and all center points, and improves the simulated annealing algorithm to solve it. The simulated annealing algorithm is derived from the simulation of the solid annealing process and belongs to the heuristic Monte Carlo method. From the initial solution X 0 and temperature T = T 0 At the beginning, the iteration of "generate new solution → calculate the objective function value → accept or discard" is repeated for the current solution, and the T value is gradually reduced. The current solution at the end of the algorithm is the approximate optimal solution. The annealing process is controlled by the cooling schedule, and the control parameters include the initial temperature T 0 , attenuation factor α, number of iterations L at each T value and stop condition, etc. The algorithm also determines the coordinates of the virtual points corresponding to each independent real point, and the order of all virtual points and center points on the center line of the path.

[0086] The main process of simulated annealing algorithm:

[0087] (1) Initialization: Set the initial temperature T 0 、Initial solution X 0 , number of iterations L, termination temperature T end , temperature attenuation coefficient α, let the number of iterations i = 0, X = X 0 ;

[0088] (2) If the temperature T <T end , terminate the algorithm, X is the final solution; otherwise, execute (3);

[0089] (3) Determine i. If i≤L, execute steps (4) to (7) in sequence; otherwise, set i=0, T=αT and return to step (2);

[0090] (4) Generate a new solution X new ;

[0091] (5) Calculate the increment ΔE = E(X new )-E(X);

[0092] (6) If ΔE<0, then accept X as the current new solution. Otherwise, accept X as the current new solution with probability exp(-ΔE / T). If the current new solution is finally accepted, let X = X new Let i=i+1 and return to step (3).

[0093] The specific steps are:

[0094] a. Set the objective function according to the coordinates of the virtual points corresponding to each independent real point and the order of all virtual points and center points on the center line of the path.

[0095] The starting and ending points of the route have been determined as stop points p o , the algorithm only needs to solve the order of the real points in the path, so the encoding does not include the stop points. The encoding of the solution consists of two parts. The first part is the ordered arrangement of the coordinates of the virtual points and the center point. As the solution changes, the order of this part will not change, but the coordinates of the virtual points may be adjusted. The encoding rule stipulates that this part arranges the virtual points first, followed by the center point. Assuming there are θ virtual points, the symbol of the first part is w = {p″ 1 , p″ 2 ,…,p″ θ , p″ θ+1 , …}, where p″ 1 , p″ 2 ,…,p″ θ is a virtual point, p″ θ+1 and the following point as the center point, w i represents the i-th point in w.

[0096] The second part is the order of the elements in the first part, expressed as X = {τ 1 ,τ 2 ,…},X records the sequence number of the elements in w, that is, X i represents the i-th element τ in X i , τ i Represents the index of w, through The i-th point on the center line of the path can be obtained. Let m represent the total number of virtual points and center points, m = |w| = |X| ≤ n, when w only contains virtual points, θ = m = n.

[0097] Since the route is based on the stop point p o is a closed loop with starting and ending points, so under the definition of this code, the solution formula of the objective function is:

[0098]

[0099] b. Use the greedy algorithm to initially sort all virtual points and center points to obtain the initial solution; in the initial state, the virtual points are independent real points themselves, and the order of all virtual points and center points in the path is generated by the greedy algorithm.

[0100] c. Generate a new solution by changing the order of virtual points and center points in the path and randomly changing the coordinates of virtual points corresponding to independent real points; the generation of a new solution is to change w and X based on the current solution.

[0101] The method of changing w is: generate a random integer d not greater than |w|, if d≤θ, then d The corresponding independent real point is the center of the circle, and a random point is generated in the circle with a radius of r to replace w d ; If d>θ, w remains unchanged.

[0102] The method to change X is to randomly select two elements in X and exchange their positions.

[0103] d. Take the difference between the total path length obtained by the new solution and the total path obtained by the current solution as the increment ΔE, judge whether to accept the new solution, determine the final solution, and get the initial path based on the final solution.

[0104] In step 4, after determining the order of each virtual point and the center point in the path, they can be adjusted according to the positions of their front and rear connection points to further shorten the path distance. The fine-tuning process of this embodiment is to fine-tune each virtual point and the center point through which the center line of the path passes one by one starting from the stop point. Different fine-tuning methods are adopted for virtual points and center points. The virtual points are fine-tuned by geometric calculation method, and the center point is fine-tuned by Monte Carlo method.

[0105] Fine-tune virtual points by geometric calculation method:

[0106] If the coordinates of the two points before and after the virtual point are known, the coordinates of the virtual point with the shortest sum of distances to the two points can be accurately calculated by geometric solution. Assume that the independent real point corresponding to the virtual point is p i , define circle O i It is p i is the center of the circle and r is the radius. The two points before and after the virtual point are p″ j , p″ k According to p″ j , p″ k Does the line connecting the two pass through circle O? i , divided into the following two situations.

[0107] 1. When p″ j , p″ k The line passes through circle O i When the virtual point is adjusted to p″ j , p″ k Connect with O i Any point in the intersection (one or two) of .

[0108] 2. When p″ j , p″ k The connecting line does not pass through circle O i According to Theorem 1, the virtual point should be adjusted to p″ j , p″ k The ellipse and circle O with foci i The tangent point.

[0109] Theorem 1: The point of tangency between a circle and an ellipse is the point on the circle whose sum of distances to the two foci of the ellipse is the shortest.

[0110] Proof: Use proof by contradiction. Make an ellipse with C and D as foci and tangent to the given circle, with the tangent point at B and the midpoint of CD at O. Assume that there is another point H on the circle that also satisfies the minimum distance to points C and D. Connect HO, and HO intersects the ellipse at K, such as Figure 5 As shown. It is easy to get HD+HC>KC+KD. Because B and K are points on the ellipse, KC+KD=BC+BD, and HD+HC>BC+BD, so point H does not satisfy the minimum distance to points C and D. Therefore, there is no point on the circle other than the tangent point B that satisfies the minimum distance to points C and D.

[0111] To calculate the tangent point coordinates, the coordinate system is first translated and rotated. After the transformation, the coordinate system is p″ j and p″ k The connecting line is the x-axis, the midpoint is the origin, and the coordinates of the desired tangent point are assumed to be (x, y). After conversion, it becomes (x R ,y R ). By solving the simultaneous equations (9), we can get In the equation is (x i ,y i ) in the new coordinate system, half the distance between the foci of the ellipse is c, and half the length of the major and minor axes are a and b respectively. c and is a known quantity. Solving for (x R ,y R ), (x,y) can be obtained through the inverse transformation of translation and rotation.

[0112]

[0113] Monte Carlo method to fine-tune the center point:

[0114] Similar to S2 for determining point clusters, this embodiment uses the Monte Carlo method to generate random points within a certain range, and searches for a point that can contain all the real points in the point cluster and can minimize the sum of the distances from the center point to the two points before and after the path, as the new center point. The number of trials in the Monte Carlo method is determined by the parameter L 2 Assume that the center point p″ k The two points before and after are and For the center point p″ k , do the following:

[0115] Step 1: Get p″ k The corresponding point cluster c kThe maximum and minimum horizontal and vertical coordinates of all real points in , that is The horizontal axis is formed by arrive The vertical axis is arrive Rectangular search box. Execute Step 2 and 3 with parameter L 2 Second-rate.

[0116] Step 2: Randomly generate a point p″ in the rectangular search box r , if c k All real points in the r If the circle is in a circle with center r and radius r, proceed to Step 3, otherwise repeat Step 2.

[0117] Step 3: If Then p″ k Adjust to p″ r .

[0118] Figure 6 This is an example of Monte Carlo method to fine-tune the center point. 1 ,p 2 ,...,p 6 Belongs to point cluster c 1 , the center point is p″ 1 . Point p″ on the path centerline 1 The two points before and after are and Using the Monte Carlo method, we can search for point p″ in the dashed box in the figure r , with p″ r The point cluster with the center as the circle can contain p 1 ,p 2 ,...,p 6 , and p″ r and The line segment and becomes shorter. Therefore, p″ r Can be used as p 1 ,p 2 ,...,p 6 The center point after fine-tuning.

[0119] In order to verify the effect of the path planning method of this embodiment, a simulation experiment was carried out:

[0120] This embodiment designs an experiment to measure the performance of SA_KM. First, the effects of different parameters on the algorithm are compared to determine the algorithm parameters. Then, the number and distribution of balls in a certain area and the length of the robot's arms are changed to analyze the effects of these objective factors on the algorithm effect. Finally, a comparative experiment is designed to compare the results of the full coverage method, the greedy algorithm, and a variety of simulated annealing algorithms to measure the effectiveness of SA_KM and analyze the differences between the effects of each algorithm and SA_KM under different objective environments. All simulation experiments were completed using Matlab 2019A on a computer with an Intel(R) Core(TM) i7-8550M CPU @1.80 GHz dual-core processor and 16.00GB of memory.

[0121] In order to measure the quality of solutions obtained by different algorithms, this embodiment calculates the relative percentage deviation (RPD) of each algorithm in solving each case, and the calculation formula is as follows:

[0122]

[0123] Among them, C best is the optimal objective function value of all algorithms to solve a case, C method The objective function value obtained when using the method algorithm for a case. RPD reflects the difference between the test result and the currently known optimal solution. The smaller the RPD value, the closer the result is to the optimal solution and the better the optimization effect. Using SA_KM to solve multiple times to obtain the RPD average can also reflect the stability of the algorithm in solving problems.

[0124] When exploring the impact of objective factors on the algorithm, this embodiment takes the use scenario of a tennis ball picking robot as an example, considering the actual situation of tennis multi-ball training, that is, athletes mostly hit from one side of the net to the other side, so most of the tennis balls land on one side of the court, and the closer to the net, the fewer balls. Therefore, this embodiment divides half of the court (1830cm×1830cm) into four areas parallel to the net, and the probability of tennis balls appearing in the four areas is the following four distribution combinations: [0.1, 0.2, 0.3, 0.4], [0.1, 0.2, 0.2, 0.5], [0.25, 0.25, 0.25, 0.25], [0.1, 0.1, 0.4, 0.4], represented by the numbers 1234, 1225, 2525, and 1144, respectively. The number of tennis balls tested is the following values, 50, 100, 150, and 200. In addition, this embodiment compares the effects of different robot arm span lengths on the algorithm, and the tested arm span lengths (cm) are 50, 100, 150, and 200 respectively.

[0125] In all experiments, the robot's stopping point was set at the lower left corner of half the court with coordinates (0, 0).

[0126] Parameter determination and impact analysis:

[0127] The parameters of SA_KM include two parts. The first part is the parameters of classic SA, T 0 , X 0 , L, T end and α; the second part is the number of trials of the Monte Carlo method, L 1 and L 2 The simulation experiment scenario is to randomly generate 300 balls in the form of 2525 distribution, with a wingspan of 100 cm, and a total of 10 experimental cases. The algorithm of each parameter combination is run 10 times in the experiments of different cases to take the RPD average.

[0128] Part I SA parameter combination [T 0 ,X 0 ,L,T end ,α] The following groups were selected for testing: [100,500,0.99, 0.001], [200, 500, 0.99, 0.001], [300, 500, 0.99, 0.001], [100, 200,0.99,0.001], [100, 800, 0.99, 0.001], [100, 1100, 0.99, 0.001], [100, 500, 0.93,0.001],

[0129] [100, 500, 0.96, 0.001], [100, 500, 0.90, 0.001], [100, 500, 0.99,0.01], [100, 500, 0.99,0.0001], respectively, and the parameter L 1 and L 2 Set to 110. Figure 7 It can be found that when the parameters are [100, 200, 0.99, 0.001], the algorithm effect is better and the time is shorter, so this embodiment uses this as the parameter value of all SA algorithms. The influence of SA parameters on the algorithm is consistent with the classic conclusion, and this embodiment will not be repeated. 1 and L 2 The values ​​are 10, 30, 70, 90, 110, 130, 170, 190 respectively. Figure 8 Summarizes different L 1 and L 2 The impact on the algorithm, while recording the changes in the results before and after the algorithm fine-tuning, can be obtained. 1and L 2 The degree of influence on the results. Figure 8 Part (a) shows the number of trials L for determining the point cluster using the Monte Carlo method. 1 The influence of L on the algorithm. This parameter affects the formation of point clusters, and thus affects the optimization results of SA. As can be seen from the figure, a smaller or larger L 1 All of them are not conducive to forming a suitable point cluster. 1 This will prevent some real points from being included in the point cluster, increasing the number of virtual points in the path. 1 Many fixed point clusters may be generated, which reduces randomness and may increase the impact of bad point clusters on the results in repeated experiments. 1 The optimal value of is 70. Figure 8 Part (b) shows that the number of experiments L for center point fine-tuning in Monte Carlo method is 2 Increasing can directly improve the performance of the algorithm. In the figure, the difference between the RPD curves before and after fine-tuning the center point gradually increases. 2 As the average RPD increases, the change range becomes smaller, and the final curve tends to be flat. 2 When it is greater than 70, the effect is basically unchanged, so L 2 The value is 70.

[0130] In addition, through Figure 8 Comparing the three broken lines in (a) or (b) in the figure, it can be concluded that the average RPD after fine-tuning the virtual point is significantly lower than that before fine-tuning. Figure 8 The experiments shown in (a) and (b) both reduce the average RPD by about 5.2%. Fine-tuning the center point can reduce the average RPD by 2% on average, where Figure 8 The experiment shown in part (b) of the experiment reduces the average by 1.72%. Figure 8 The data show that fine-tuning the scheme after SA sorting can further optimize the results, and fine-tuning the virtual points has a greater effect. Fig. 9 Convergence curves can also be found.

[0131] This embodiment tests 4 target ball distributions, 4 ball numbers, and 4 arm span lengths, for a total of 4×4×4=64 objective environments. Each objective environment generates 10 cases, and each algorithm solves each case 10 times to take the RPD mean. This embodiment uses the variance analysis method to measure whether different objective environments have a significant impact on the solution effect of SA_KM. Because the SA_KM effect is optimal under each objective environment, the RPD mean of SA_KM solution reflects the volatility of the results, so the RPD mean measures whether different objective factors affect the stability of SA_KM. The larger the RPD mean, the more unstable the solution. Fig.10The RPD mean of SA_KM under different objective environments is recorded. The figure contains the p-value of variance analysis of each objective factor. It can be seen that distribution, number of balls and arm length all have a significant impact on the stability of the algorithm. Fig.10 From part (a) of the figure, it can be found that the longer the robot arm is, the more stable the algorithm is. As the arm length increases, the number of real points assigned to the point cluster increases, the number of points that the path needs to reach decreases, the problem scale decreases, and the effect is more stable. However, when the arm span is 50 cm, the number of point clusters is too small, and the center line of the path almost needs to reach the vicinity of each real point, so the fine-tuning of the virtual points in the algorithm plays a greater role, and the fine-tuning effect of different cases is not much different, so the result is relatively stable. Fig.10 Part (b) shows the effect of the number of balls on the algorithm. The fewer the number of balls, the more stable the effect. This may be the same reason why the algorithm is more stable when the arm span is 50 cm. When the number of balls increases, without increasing the arm span, the generated point clusters are more random, affecting the stability of the algorithm. Fig.10 In part (c), we can find that the more evenly the balls are distributed, the worse the algorithm stability is. With the same number of balls, different distributions directly affect the formation of point clusters. A more even distribution leads to fewer center points and more virtual points, which increases the problem size and deteriorates the algorithm stability.

[0132] Comparison of different algorithms:

[0133] The experiment also compares the solution results of different algorithms under different objective environments. On the one hand, it proves the effectiveness of SA_KM in solving the problem; on the other hand, it analyzes the difference between the effect of each algorithm and SA_KM under different objective environments. The algorithms compared in this embodiment include all-cover method (ACM), greedy algorithm (Greedy), and multiple SA. According to whether SA refers to SA_KM to change the virtual point position (change w) when generating a new solution, it is divided into SA-N and SA-O, which represent the points that the path centerline can pass through the neighborhood (Neighborhood) and can only pass through the original real point (Origin point). According to whether the greedy algorithm is used as the initial solution of SA, SA is divided into SA-G and SA-R, which represent use and no use, respectively. Therefore, SA is divided into four types, SA-NG, SA-NR, SA-OG, and SA-OR, and the parameter combination uses the conclusions of the previous article. The objective environment of the experiment is the same as the previous article. Each objective environment generates 10 cases, and each algorithm solves each case 10 times to take the RPD average. The description of the greedy algorithm and the full coverage method is as follows.

[0134] (1) Greedy algorithm:

[0135] The greedy algorithm is the first algorithm considered in practice. First, find the point closest to the stop point as the end point of the current path, then find the point closest to the current end point among all the remaining real points as the new end point, and repeat this process until all real points are searched and the stop point is returned. All real points are on the center line of the path.

[0136] (2) Full coverage method:

[0137] The full coverage method refers to the idea of ​​path planning of the sweeping robot to achieve full coverage of the area where the target ball is located. This embodiment is based on the longitudinal (y-axis direction) path. First, find the leftmost ball that is not collected in the plane, and determine the left and right boundaries of the strip plane formed by the longitudinal path according to the horizontal coordinate of the ball and the length of the robot arm; then obtain the maximum and minimum vertical coordinates of all target balls between the left and right boundaries as the upper and lower boundaries of the path, and deduce the starting and ending points of the center line of the path; for all uncollected balls, repeat the above operation until all balls can be collected. The connection between the center lines of every two adjacent longitudinal paths adopts the principle of proximity, and the ends are connected in an "S" shape; finally, the stop point and the path are connected from beginning to end.

[0138] Table 1. Comparison of the effects of different algorithms under different objective conditions

[0139]

[0140] Table 1 shows the average RPD of different algorithms under different objective environments. The last two rows of data summarize the average RPD and average running time of different algorithms. It can be seen from the table that the solution effect of SA_KM is significantly better than SA-NG, SA-NR, SA-OG, SA-OR, greedy algorithm and ACM. The average RPD of SA_KM can be reduced by about 40% on average compared with other algorithms. Compared with SA with random initial solution, whether or not to consider changing the virtual point, SA-NR and SA-OR have unsatisfactory effects, and their average RPD is more than 50% greater than SA_KM. The traditional greedy algorithm has a certain impact on the results, and also improves the solution quality of SA-NG and SA-OG with greedy algorithm as the initial solution. However, the differences between these three methods are still large compared with SA_KM, and the average RPD is more than 33% greater than SA_KM. The difference between the two different SA-N and SA-O is not large, so the optimization method of randomly changing the position of the virtual point is not obvious. Therefore, the SA_KM proposed in this embodiment is effective, and the algorithm plays a good role in clustering real points into point clusters and fine-tuning the positions of points on the center lines of each path.

[0141] In terms of algorithm running time, the greedy algorithm and ACM require almost no running time, while SA-OG and SA-OR without considering virtual points take about 2.3 seconds less time than SA-NG and SA-NR with considering virtual points. SA_KM needs to determine the point cluster and adds a fine-tuning step, which takes the longest time, an average of 6.17 seconds, which is also within an acceptable range.

[0142] In addition, the effect of SA_KM under different objective environments has a greater advantage over other algorithms, but the size of the advantage varies under different environments. First, when the distribution is different, the solution effect of each algorithm is relatively stable, so the difference between other algorithms and SA_KM does not change much. When the number of target balls is different, the difference between the effect of ACM and SA_KM changes the most. As the number of tennis balls increases, the effect of ACM improves, and it can even exceed the SA algorithm with two random initial solutions, and the difference with SA_KM becomes smaller, which is also consistent with the actual situation. As the number of tennis balls increases, the scale of the problem increases, and the optimization effect of SA is greatly reduced, so the difference between the effect of the four different SA and SA_KM increases. The different number of tennis balls has little effect on the effect of the greedy algorithm, and the greedy algorithm as the initial solution reduces the degree of reduction in the solution effect of various SA. The influence trends of the change in arm span length and the change in the number of tennis balls on different algorithms are basically the same. However, the arm span length has a greater impact on the effect of the greedy algorithm. The larger the arm span, the greater the difference between the greedy algorithm and SA_KM. After all, the traditional greedy algorithm does not consider the path width, and the difference between the number of points that the greedy algorithm needs to reach and SA_KM increases.

[0143] This embodiment proposes a simulated annealing algorithm (SA_KM) that integrates k-means clustering and Monte Carlo method to solve the ball picking robot ball picking path planning problem considering the path width. The simulation experiment proves that SA_KM is effective in solving the problem and the effect is much better than other algorithms. The experiment also proves that the components of SA_KM, such as generating point clusters, fine-tuning the virtual points corresponding to each real point by geometric calculation method, and fine-tuning the center point corresponding to each point cluster by Monte Carlo method, all have obvious optimization effects.

[0144] The path planning method of this embodiment is an extension of the shortest path problem. The results can be applied not only to the path planning of the ball picking robot, but also to the path planning problems of other robots with a service radius. The experimental results can be used as a reference for both the supply and demand sides of the robot. The longer the robot's arm span and the more balls there are in the court, the more obvious the SA_KM solution advantage. Therefore, robot manufacturers can consider improving the mechanical structure, such as increasing the arm span. When tennis and table tennis players use more balls for training, they should consider using SA_KM to plan the ball picking path.

[0145] Embodiment 2:

[0146] Embodiment 2 of the present invention provides a path planning system for a ball picking robot, comprising:

[0147] The data acquisition module is configured to acquire data of the acquisition target and the ball-picking robot; the coordinates of the ball to be collected are set as the target point, and the starting point and the end point of the path are used as the stop points;

[0148] A model building module is configured to build a closed path model according to the stop points, the target points and the path width;

[0149] The point cluster determination module is configured to set the target point as a real point, determine a point near the real point that is passed by the path center line as a virtual point; use a clustering algorithm to determine the point cluster, name the real point in the point cluster that can share a virtual point as the center point, and use the Monte Carlo method to update the center point of each initial point cluster to generate the final point cluster;

[0150] The annealing algorithm optimization module is configured to sort the virtual points and the center points in the point cluster by using a simulated annealing algorithm to form an initial path;

[0151] The path planning module is configured to fine-tune the virtual points and the center point on the initial path to obtain the final path planning.

[0152] The steps involved in the above embodiment 2 correspond to the method embodiment 1, and the specific implementation method can refer to the relevant description part of embodiment 1. Those skilled in the art should understand that the modules or steps of the present invention can be implemented by a general computer device, and optionally, they can be implemented by a program code executable by a computing device, so that they can be stored in a storage device and executed by the computing device, or they can be made into individual integrated circuit modules, or multiple modules or steps therein can be made into a single integrated circuit module for implementation. The present invention is not limited to any specific combination of hardware and software.

[0153] Although the above describes the specific implementation mode of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without creative work are still within the scope of protection of the present invention.

Claims

1. A path planning method for a ball picking robot. It is characterized in that The following steps are involved: Obtain the data of the collection target and the ball-picking robot; set the coordinates of the ball to be collected as the target point, and the starting point and the end point of the path as the stop points; Establish a closed path model based on stop points, target points and path width; The target point is set as a real point, and a point near the real point that is passed by the center line of the path is determined as a virtual point; Clustering algorithm is used to determine point clusters, and the real points in the point clusters that can share a virtual point are named as center points. The Monte Carlo method is used to update the center points of each initial point cluster to generate the final point cluster. The virtual points and central points in the point cluster are sorted by the simulated annealing algorithm to form an initial path; Fine-tune the virtual points and center points on the initial path and output the final path planning.

2. The path planning method of the ball picking robot according to claim 1, It is characterized in that Multiple real points can correspond to the same virtual point, and the virtual point of the stop point is itself.

3. The path planning method of the ball picking robot according to claim 1, It is characterized in that Assume that the virtual point is a point inside a circle corresponding to the real point, with the coordinates of the real point as the center and half the length of the ball receiving structure as the radius, and establish a closed path model with the shortest closed path passing through the virtual point as the goal.

4. The path planning method of the ball picking robot according to claim 1, It is characterized in that The specific steps of using clustering algorithm to determine point clusters are: An initial point cluster is established with each real point as the center point. The real points included in each initial point cluster are determined by comparing the distance between the center point and other real points with half the length of the ball receiving structure.

5. The path planning method of the ball picking robot according to claim 4, It is characterized in that If the distance from a solid point to other solid points is greater than half the length of the ball receiving structure, then the solid point is named an independent solid point.

6. The path planning method of the ball picking robot according to claim 5, It is characterized in that The Monte Carlo method is used to update the center points of each initial point cluster. The specific steps to generate the final point cluster are: Get the maximum and minimum values ​​of the horizontal and vertical coordinates of all real points in the point cluster corresponding to the center point to form a rectangular search box; A point is randomly generated in the rectangular search box, and the real points contained in the circle with the random point as the center and half the length of the ball structure as the radius are calculated. If the number of real points is greater than the origin cluster, the real points of the current point cluster are updated, and the center point of the current point cluster is updated to the random point; Eliminate the situation where the same real point in the initial point cluster belongs to different initial point clusters, determine the mapping relationship between all real points and point clusters, set all initial point clusters to an unprocessed state, and generate all point clusters.

7. The path planning method of the ball picking robot according to claim 6, It is characterized in that The specific steps to generate all point clusters are: find the point cluster with the most real points in the unprocessed point clusters, assuming it is c k , marked as processed; delete other unprocessed point clusters and c k Overlapping real points; repeat the operation until all point clusters are processed.

8. The path planning method of the ball picking robot according to claim 7, It is characterized in that The specific steps of sorting the virtual points and center points in the point cluster by simulated annealing algorithm to form the initial path are as follows: The objective function is set according to the coordinates of the virtual points corresponding to each independent real point and the order of all virtual points and center points on the center line of the path; A greedy algorithm is used to initially sort all virtual points and center points to obtain an initial solution; New solutions are generated by changing the order of virtual points and center points in the path and randomly changing the coordinates of virtual points corresponding to independent real points; Determine whether to accept the new solution, determine the final solution, and get the initial path based on the final solution.

9. The path planning method of the ball picking robot according to claim 1, It is characterized in that The specific steps to fine-tune the points on the initial path and output the final path planning are as follows: Starting from the stop point, each virtual point and the center point that the center line of the path passes through are fine-tuned one by one. The virtual points are fine-tuned using geometric calculation method, and the center point is fine-tuned using Monte Carlo method.

10. A path planning system for a ball picking robot. It is characterized in that include: A data acquisition module is configured to acquire data of a collection target and a ball picking robot; Set the coordinates of the ball to be collected as the target point, and the starting and ending points of the path as the stop points; A model building module is configured to build a closed path model according to the stop points, the target points and the path width; The point cluster determination module is configured to set the target point as a real point, determine a point near the real point that is passed by the path center line as a virtual point; use a clustering algorithm to determine the point cluster, name the real point in the point cluster that can share a virtual point as the center point, and use the Monte Carlo method to update the center point of each initial point cluster to generate the final point cluster; The annealing algorithm optimization module is configured to sort the virtual points and the center points in the point cluster by using a simulated annealing algorithm to form an initial path; The path planning module is configured to fine-tune the virtual points and the center point on the initial path to obtain the final path planning.

Images