A court perception and efficient path planning method for an adaptive tennis ball picking robot

Abstract

A court perception and efficient path planning method of an adaptive tennis ball picking robot belongs to the technical field of robot motion planning, and mainly comprises the following steps: through double-view point cloud collection of the robot in situ, extraction of the court boundary and grid division of the working area are completed; detection of tennis balls and obstacles is completed based on two-dimensional voxel neighborhood features; all tennis balls detected in the current unit are clustered and merged; the optimal path is solved by a two-step solving strategy of generating an initial path through a greedy algorithm and optimizing the path through a 2-opt iteration algorithm, so that the global ball picking sequence is planned; a plurality of groups of candidate motion trajectories are sampled, and the optimal trajectory is selected by combining collision detection and cost screening; after completing the ball picking task of all internal areas, the robot automatically enters the boundary supplementary picking. The method can effectively improve the ball picking efficiency and robustness of the ball picking robot in unknown courts.

Description

Technical Field

[0001] This invention relates to the field of robot motion planning technology, and in particular to a method for site perception and efficient path planning of an adaptive tennis ball retrieval robot. Background Technology

[0002] During tennis training, the manual collection of scattered tennis balls is inefficient and time-consuming, making automated tennis ball-retrieving robots a hot topic in the industry. Existing ball-retrieving robots mainly rely on visual sensors or single LiDAR for perception and planning, but these have many limitations in practical applications:

[0003] 1. Site perception level: Most existing solutions rely on pre-built field maps or global mapping through SLAM technology. This process requires the robot to traverse the entire site, which takes several minutes and cannot adapt to temporary sites or rapid adaptation after site changes.

[0004] 2. Target detection level: Vision-based detection solutions are easily affected by lighting, shadows, and ground stains, and cannot work in rainy weather or strong light; while traditional point cloud detection only distinguishes targets by a single height threshold, which cannot effectively distinguish tennis balls from small obstacles such as pebbles and bottle caps, resulting in a high false detection rate.

[0005] 3. Path planning level: Most existing solutions use greedy algorithms or basic TSP algorithms for path sorting, without considering the characteristic that close-range tennis balls can be picked up in one go. For tennis balls that are very close, the path is still planned one by one, resulting in repeated movements and wasting time. Moreover, traditional path planning algorithms are slow to respond in dynamic environments and cannot take into account both the smoothness of movement and the obstacle avoidance requirements.

[0006] 4. Boundary Retrieval Level: Due to the limited field of view of the robot's sensors, tennis balls in the boundary area are easily missed when detecting the inner area. However, the existing technology lacks effective handling for the missed tennis balls in the boundary area after completing the retrieval of the inner area, resulting in missed retrieval and easy collision when moving along the wall. Summary of the Invention

[0007] To address the aforementioned problems of existing tennis ball-retrieving robots, including low efficiency in initial mapping in unknown fields, susceptibility of tennis ball detection to environmental interference, low efficiency in multi-target ball-retrieving path planning, and missed detection in boundary areas, this disclosure provides a complete adaptive field perception and efficient ball-retrieving path solution. Its main purpose is to:

[0008] ① A rapid site modeling method based on dual static point cloud acquisition is constructed. Without the need for robot movement and traversal, the site boundary extraction and working area division can be completed quickly by only two static acquisitions in place, and the site initialization can be completed within 30 seconds.

[0009] ② Construct a target detection method based on the height difference of two-dimensional voxel neighborhood. By fusing multiple frame point clouds, the height difference features of voxels and their 8 neighborhoods are used to distinguish tennis balls, obstacles and the ground, which greatly improves the detection robustness.

[0010] ③ Construct a global path planning method for nearest-neighbor tennis ball clustering and improved TSP. Cluster and merge near-distance tennis balls that can be picked up in one go to reduce path nodes. At the same time, combine robot kinematic constraints to optimize the path and improve ball picking efficiency.

[0011] ④ Construct an adaptive edge-traversal boundary replenishment strategy. After completing the ball picking in the internal area, automatically traverse and replenish the ball along the boundary of the field to solve the problem of missing the ball at the boundary. At the same time, dynamic trajectory sampling ensures the collision-free movement along the edge.

[0012] Specifically, the adaptive field perception and efficient ball retrieval path planning method provided in this disclosure mainly includes the following steps:

[0013] S1 uses a robot to collect point cloud data from two perspectives in situ, thereby extracting the site boundaries and dividing the work area into grids.

[0014] S2, after the robot moves to the center point of the unit, it collects the lidar point cloud of the current area, performs multi-frame fusion, and completes the detection of tennis balls and obstacles through two-dimensional voxel neighborhood features;

[0015] S3 clusters and merges all tennis balls detected in the current unit; with the minimum total motion time of the robot as the core optimization objective, the cost function is constructed by incorporating rotation time into the distance; the optimal path is solved by a two-step solution strategy of generating the initial path with a greedy algorithm and optimizing the path with a 2-opt iterative algorithm, so as to realize the planning of the global ball picking order.

[0016] Furthermore, step S1 specifically includes:

[0017] S11: The robot collects point clouds from two perspectives at any point on the court, transforms them to the global map coordinate system, and performs a union and fusion to obtain the global court point cloud set. ;

[0018] S12, for the global point cloud set The wall point cloud is extracted based on a set height threshold; the minimum bounding rectangle of the wall point cloud is calculated as the working area for robot movement and operation. ;

[0019] S13, work area It is divided into multiple regional units for the robot to traverse and pick up balls in different regions.

[0020] Furthermore, step S2 specifically includes:

[0021] S21. After the robot moves to the center point of the unit, it performs noise reduction and fusion on the multi-frame LiDAR point cloud collected in the current area to obtain a high-density point cloud set of the current area.

[0022] Construct a two-dimensional voxel grid: Divide the point cloud of the current region into several square grids along the xy plane, with the grid side length slightly smaller than the diameter of a tennis ball; For each voxel corresponding to a grid, calculate the height range of all point clouds within it, and use it as the height range feature of each voxel.

[0023] S22: For each voxel, extract the extreme height values ​​of itself and its surrounding adjacent voxels, calculate the difference between the maximum height value and the minimum height value of all voxels, and distinguish between tennis balls, obstacles, and the ground based on the height difference.

[0024] S23, For the neighborhood area determined to be the tennis ball, the center coordinates of the tennis ball are accurately located by traversing through a sliding window;

[0025] S24. For areas identified as obstacles, convert them into obstacle grids in the grid map, expand the obstacle grids so that the robot can avoid the expanded areas during subsequent path planning, and set the robot's ball picking threshold based on the distance relative to the obstacle grids to avoid collisions with obstacles during the picking process.

[0026] Furthermore, the specific method of step S22 includes:

[0027] (1) Extract the current voxel The set of adjacent voxels, Two-dimensional integer indices for voxels: defined as all indices ( )satisfy and The voxels are denoted as:

[0028]

[0029] Includes the current voxel and a ring of adjacent voxels;

[0030] (2) Calculate the overall height difference within the neighborhood formed by the current voxel and its surrounding adjacent voxels:

[0031] Take the difference between the maximum height value and the minimum height value of all voxels in the neighborhood, that is:

[0032]

[0033]

[0034] ;

[0035] (3) Classify targets according to the range of height difference:

[0036] like If the target in that neighborhood is a tennis ball;

[0037] like If the target in that neighborhood is an obstacle, then the target in that neighborhood is an obstacle.

[0038] The rest, i.e. or If the area is flat and there are no valid targets, then the area is flat ground.

[0039] Furthermore, step S3 specifically includes:

[0040] S31, Nearest Neighbor Tennis Clustering and Merging: Merge several nearby tennis balls that are less than a set threshold into a single target point;

[0041] S32 defines the transfer cost of the robot from its current position to the target point to be visited as the sum of the movement time and the rotation time, where the rotation time is the time it takes for the robot to adjust from the current heading angle to the target heading angle;

[0042] S33, for the set of target points to be visited Starting from the center point of the cell where the robot is currently located, a complete initial traversal path is quickly generated using a nearest neighbor greedy strategy. ;

[0043] S34, the initial path generated by the greedy algorithm is iteratively optimized using the 2-opt local search algorithm to obtain the resulting path. This is the optimal path for picking up the ball;

[0044] S35, Output the optimal path The order in which the target points are visited is the robot's global ball-picking order. The robot moves to each target point in sequence according to this order to efficiently pick up all the tennis balls in the current unit.

[0045] Furthermore, step S31 specifically includes:

[0046] For the detected tennis ball set ,in For the first The two-dimensional coordinates of a tennis ball are used for distance threshold clustering. The steps include:

[0047] Initialize cluster collection ;

[0048] Traverse each tennis ball If clustering exists , making arrive If the distance between the centers is less than 0.12m, then... Add to this cluster;

[0049] Distance determination:

[0050]

[0051] Otherwise, create a new cluster and... Add to this cluster;

[0052] For each cluster The target point is the center point of all tennis balls within the cluster, and the calculation formula is:

[0053] Let clustering Include tennis Then the cluster center coordinates are:

[0054]

[0055] Cluster target points are denoted as:

[0056]

[0057] This clustering method merges multiple close-range tennis balls into a single target point.

[0058] Furthermore, step S32 specifically includes:

[0059] Define the robot from its current position. , The robot's current heading angle is the angle between the robot's forward direction and the x-axis, and the distance to the target point is... The transfer cost is the sum of the movement time and the rotation time, that is:

[0060]

[0061] The calculations for each part are as follows:

[0062] (1)

[0063] In the formula, Here, represents the robot's maximum linear velocity, and represents a fixed hardware parameter.

[0064] (2) Rotation time: The time it takes for the robot to adjust from the current heading angle to the target heading angle, where the target heading angle is the time it takes for the robot to rotate from point A. Point of view The direction angle, specifically calculated using the following methods:

[0065] ① Calculate the target heading angle: ;

[0066] ② Calculate the heading angle difference: ;

[0067] ③ Normalize the angle difference to Range, avoid invalid rotations: ;

[0068] ④ Calculate the rotation time: In the formula represents the robot's maximum angular velocity, and represents a fixed hardware parameter.

[0069] Furthermore, the method also includes the following steps:

[0070] S4 samples multiple candidate motion trajectories, combines collision detection and cost filtering to select the optimal trajectory, which serves as the real-time control input for the robot, achieving the dual goals of dynamic obstacle avoidance and smooth motion;

[0071] Specifically, it includes:

[0072] S41, uniformly sample within the effective range of all feasible turning angular velocities of the robot to obtain the complete candidate trajectory corresponding to each sampled angular velocity. All candidate trajectories constitute a trajectory set. ,in angular velocity The corresponding trajectory;

[0073] S42, for each candidate trajectory Collision detection at key points: The validity of a trajectory is determined by examining the distribution of obstacles at the midpoint and endpoint of the trajectory within each predicted time period. Collision-free feasible trajectories are selected, and all valid trajectories constitute a set of feasible trajectories. ;

[0074] S43, for the set of feasible trajectories The trajectory with the smallest angular velocity is selected first as the optimal motion trajectory for the current robot, and the sampling angular velocity corresponding to this trajectory is used. and constant linear velocity As the real-time control input for the robot.

[0075] Furthermore, the method also includes the following steps:

[0076] S5. After the robot completes the ball-collecting task in all internal areas, it enters the boundary replenishment mode. In this mode, the robot performs a continuous, smooth, and collision-free traversal movement along the boundary of the field to pick up the tennis balls left in the boundary area.

[0077] Furthermore, in step S5, the robot does not use global path planning in the boundary replenishment mode, but achieves natural edge contact and dynamic obstacle avoidance through real-time sampling, real-time collision detection, and real-time optimal trajectory selection.

[0078] Specifically, it includes:

[0079] S51, edge-fitting motion control based on dynamic trajectory sampling:

[0080] Set the angular velocity sampling range and sampling interval;

[0081] For each sampled angular velocity Predict the trajectory of movement within a set prediction period in the future;

[0082] Edge-priority trajectory selection strategy: When the robot moves along the boundary, it always prioritizes the effective trajectory with the smallest angular velocity, so that the robot naturally moves to the boundary of the site, realizing pure perception edge-fitting motion without instructions or a global path.

[0083] S52: For each sampled trajectory, the midpoint and endpoint are detected within the predicted time to determine whether it is safe to pass. Only trajectories that simultaneously satisfy the safety of the midpoint and endpoint will enter the optimal trajectory selection process.

[0084] S53, during the boundary traversal, the robot automatically switches its movement speed according to the distribution of obstacles ahead.

[0085] Compared with the prior art, the beneficial effects of this disclosure are:

[0086] 1. Significantly improved efficiency in court initialization: It solves the problems of not being able to quickly complete the global working area modeling for unknown tennis courts, the time-consuming nature of traditional SLAM mapping, and the reliance on prior maps, and can quickly construct the court boundaries and working area;

[0087] 2. Enhanced robustness in target detection: Robust target classification based on voxel neighborhood height difference solves the problem that traditional vision or single-frame point cloud tennis ball detection is susceptible to interference from lighting, ground texture, and small obstacles, achieving a tennis ball detection accuracy of 99.2%;

[0088] 3. Significantly improved ball retrieval efficiency: By using nearest neighbor clustering and improved TSP sorting, the problem of path planning getting trapped in local optima and repeated movement of close-range tennis balls under multiple tennis ball targets is solved, reducing the global ball retrieval time by more than 35%;

[0089] 4. No missing items: An adaptive edge traversal strategy is proposed to solve the problem that traditional path planning cannot effectively pick up edge tennis balls and easily collided walls in the boundary area, so as to achieve no missing items and collision-free traversal in the boundary area;

[0090] 5. Enhanced motion safety: The dynamic trajectory planning method can avoid obstacles in real time and will not collide with walls when moving along the edge, greatly improving the robot's motion safety. Attached Figure Description

[0091] The above and other objects, features and advantages of this disclosure will become more apparent from the more detailed description of exemplary embodiments of this disclosure taken in conjunction with the accompanying drawings, in which the same reference numerals generally represent the same components.

[0092] Figure 1 This diagram shows an overall flowchart of an exemplary embodiment of the present disclosure; Figure 2 A schematic diagram illustrating the process from court division to tennis ball extraction and identification. Detailed Implementation

[0093] Preferred embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While preferred embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that the present disclosure will be thorough and complete, and will fully convey the scope of the present disclosure to those skilled in the art.

[0094] This disclosure addresses the problems of low initial mapping efficiency in unknown fields, susceptibility of tennis ball detection to environmental interference, low efficiency in multi-target ball retrieval path planning, and missed detections in boundary areas in existing tennis ball retrieval robots. It provides an adaptive tennis ball retrieval robot for field perception and efficient path planning. The flowchart of an exemplary embodiment of this disclosure is attached. Figure 1 As shown, the specific steps include:

[0095] 1. Rapid site modeling and work area division

[0096] In this stage, the robot collects point clouds from two perspectives in situ, quickly extracting the site boundaries and dividing the work area into grids, without requiring the robot to move and traverse the entire area.

[0097] 1.1 Dual-view point cloud acquisition and coordinate transformation

[0098] First, control the robot to remain stationary at any initial point on the field for 3 seconds, and collect 30 frames of LiDAR point cloud data, which is recorded as the initial viewpoint point cloud set:

[0099]

[0100] in This represents the number of point clouds in a single frame.

[0101] The robot was then controlled to rotate 180° in place, remain stationary for 3 seconds, and 30 frames of point cloud data from another perspective were collected, denoted as:

[0102]

[0103] All point clouds are uniformly transformed to the global map coordinate system using the robot pose transformation matrix. The transformation method is as follows:

[0104] Point coordinates of point cloud in local coordinate system Convert to coordinates in the global map coordinate system The three-dimensional rigid body transformation formula is:

[0105]

[0106] Wherein, rotation matrix : Describes the robot's posture, rotation, and translation vector. : Describes the robot's position in the map coordinate system.

[0107] Expand into component form:

[0108]

[0109] This transformation unifies all point clouds from both perspectives to the global coordinate system, resulting in a global set of point clouds for the entire field:

[0110] ,

[0111] By **union-fusion** the point clouds obtained from the two perspective transformations, a global field point cloud set is obtained:

[0112]

[0113] 1.2 Wall Extraction and Work Area Delineation

[0114] For global point cloud collection The wall point cloud is extracted based on a height threshold: Since the wall height of a tennis court is usually greater than 1.5m, point clouds with a height greater than 1.5m are selected as wall point clouds, denoted as:

[0115]

[0116] The minimum bounding rectangle of the wall point cloud is calculated to obtain the boundary of the working area. The specific calculation process is as follows:

[0117] (1) Extract the extreme values ​​of the x and y coordinates of the wall point cloud: traverse the wall point cloud Given the plane coordinates of all points, find the minimum value in the x-direction. Maximum value Minimum value in the y direction Maximum value ,Right now:

[0118]

[0119]

[0120] (2) This gives us the bounding rectangle of the work area. Its four vertices are:

[0121] The shape represents the robot's global work area, covering the entire usable area of ​​the tennis court. During movement and operation, the robot will use this rectangular boundary as a constraint to limit its own range of movement and avoid exceeding the effective area of ​​the tennis court.

[0122] 1.3 Grid Region Unit Division

[0123] To achieve traversal ball picking by region, the working area is... Divided into multiple Regional units, as shown in the appendix Figure 2 As shown. The division rules are as follows:

[0124] First, based on the bounding rectangle of the work area Boundary extrema Calculate the number of elements in the x and y directions:

[0125] Assume that the side length of the region unit in the x and y directions is 1. Then the number of units in the x-direction Number of units in the y-direction All calculations are rounded up to the nearest integer to ensure complete coverage of the entire work area. The formula is as follows:

[0126]

[0127] in This is the floor operator. All are positive integers.

[0128] For the line, number Column area cells ( Rows and columns are counted starting from the bottom left corner of the work area, and their boundaries are:

[0129]

[0130]

[0131] The rectangular boundary of this unit can be represented as The z-direction is uniformly defined as the effective height range of the tennis court surface. .

[0132] The coordinates of the center point of this unit are:

[0133]

[0134] The robot will move to the center point of each cell in a row-to-row, column-to-column traversal order from left to right and bottom to top. The system detects and picks up all tennis balls in the current unit at the central point, and then moves to the next unit after completing one unit, achieving precise traversal of different areas and ensuring that there are no blind spots in ball picking within the tennis court's working area.

[0135] 2. Tennis ball and obstacle detection based on voxel neighborhood features

[0136] Once the robot moves to the center point of the unit, it collects 30 frames of LiDAR point cloud data of the current area. After multi-frame fusion, it uses two-dimensional voxel neighborhood features to detect tennis balls and obstacles.

[0137] 2.1 Multi-frame point cloud fusion and 2D voxel grid construction

[0138] First, distortion correction and fusion are performed on 30 frames of point clouds to eliminate noise in individual frames, resulting in a high-density point cloud set for the current region. ,in The three-dimensional coordinates of the point cloud, This represents the total number of points in the merged point cloud.

[0139] Then, a two-dimensional voxel grid is constructed, dividing the current region's xy plane into sections with side lengths of... Square voxels, ignoring the z-axis coordinate, are divided into planar grids for each point cloud. The index of the corresponding voxel is:

[0140]

[0141] in This is the floor operator. A two-dimensional integer index for a voxel, uniquely identifying a voxel raster. .

[0142] For each voxel Count the extreme heights of all point clouds within the voxel, and denote the set of point clouds within the voxel as . Then the extreme value of height is:

[0143]

[0144]

[0145] This yields the height range characteristics of each voxel. .

[0146] 2.2 Target classification based on neighborhood height difference features

[0147] For each voxel, extract the extreme height values ​​of itself and all voxels within its 8 neighboring voxels, and calculate the maximum height difference within the neighborhood to distinguish tennis balls, obstacles, and the ground.

[0148] (1) First, extract the current voxel. The set of 8 adjacent voxels is defined as all indices satisfying and The voxels are denoted as:

[0149]

[0150] It contains the current voxel and its 8 surrounding adjacent voxels, for a total of 9 voxel grids.

[0151] (2) Calculate the overall height difference within the neighborhood (the neighborhood refers to the area formed by a voxel and its surrounding adjacent voxels), and take the difference between the maximum and minimum height values ​​of all voxels within the neighborhood, i.e.:

[0152]

[0153]

[0154]

[0155] (3) Classify targets according to the range of height difference:

[0156] Tennis goal: If If the target in that neighborhood is a tennis ball, then the target in that neighborhood is a tennis ball. This is because the diameter of a standard tennis ball is about 6.7 cm, and the height difference of its point cloud just fits within that range, and is much smaller than the height difference of obstacles.

[0157] Obstacle target: If If the target in that neighborhood is an obstacle, such as a chair, water bottle, or other debris in the area, then the target in that neighborhood is an obstacle.

[0158] Ground: Other cases ( or The area is flat ground with no effective targets.

[0159] 2.3 Precise positioning of tennis balls

[0160] For the neighborhood area identified as the tennis ball, the coordinates of the tennis ball are further precisely located using a sliding window:

[0161] Traverse the neighborhood area using a 6cm×6cm sliding window, find the highest point within the window, and the x and y coordinates of that point are the center coordinates of the tennis ball.

[0162] Specifically, for the set of neighboring point clouds corresponding to the tennis ball Iterate through all points and find the point with the largest z-coordinate. The three-dimensional coordinates of the tennis ball are:

[0163]

[0164] The z-coordinate is set to 0 to match the court's ground coordinate system. This method can effectively eliminate the interference of ground point clouds and accurately locate the center position of the tennis ball with a positioning error of less than 2cm.

[0165] 2.4 Obstacle Grid Expansion Processing

[0166] For areas identified as obstacles, they are converted into obstacle grids in a grid map. To prevent the robot from colliding with obstacles, the obstacle grids are expanded with an expansion radius of 5cm.

[0167] The mathematical description of dilation processing is: for obstacle grid The expanded region consists of all graticules that satisfy the condition (Euclidean distance between graticule centers ≤ expansion radius + half-side length of voxel). ,Right now:

[0168]

[0169] in, Let the voxel side length be , , The coordinates of the center plane of the voxel raster are given.

[0170] After expansion, the robot will avoid the expanded area during path planning. At the same time, a pickup threshold is set: tennis balls that are less than 0.1m away from the center of the obstacle grid are judged as unpickable balls and are skipped directly to avoid collisions between the robot and obstacles during the pickup process.

[0171] 3. Target Clustering and Global Task Ranking

[0172] After detecting all tennis balls in the current cell, the tennis balls are clustered and merged, and the optimal ball-picking order is planned to improve ball-picking efficiency.

[0173] 3.1 Nearest Neighbor Tennis Cluster Merging

[0174] Since the robot's scanning mechanism is approximately 0.2m wide, it can scan two tennis balls less than 0.12m apart in a single movement without needing to plan separate paths. Therefore, we perform nearest-neighbor clustering on the detected tennis balls:

[0175] For the detected tennis ball set ,in For the first The two-dimensional coordinates of a tennis ball are used for distance threshold clustering, and the process is as follows:

[0176] ① Initialize the cluster set ;

[0177] ② Iterate through each tennis ball If clustering exists , making arrive If the distance between the centers is less than 0.12m, then... Add to this cluster;

[0178] Distance determination formula:

[0179]

[0180] ③ Otherwise, create a new cluster and... Add to this cluster;

[0181] ④ For each cluster The target point is the center point of all tennis balls within the cluster, and the calculation formula is:

[0182] Let clustering Include tennis Then the cluster center coordinates are:

[0183]

[0184] Cluster target points are denoted as:

[0185]

[0186] This clustering method merges multiple close-range tennis balls into a single target point, significantly reducing the number of nodes in path planning and avoiding repetitive movements.

[0187] 3.2 Improved TSP Global Access Sequence Planning

[0188] For the clustered set of target points to be visited Traditional Traveling Salesman Problem (TSP) only considers the shortest Euclidean distance as the optimization objective, neglecting the rotation time of the robot's motion, which can easily lead to an increase in the actual total motion time. Therefore, this solution adopts an improved TSP algorithm, with minimizing the total robot motion time as the core optimization objective. It incorporates rotation time into the cost function based on distance and uses a two-step solution strategy of generating the initial path with a greedy algorithm and optimizing the path with a 2-opt iterative algorithm to find the optimal path, thus achieving accurate planning of the global ball-picking order.

[0189] 3.2.1 Construction of the transfer cost function

[0190] The core of path planning is defining the transfer cost between two target points, which directly reflects the actual time taken for the robot to move from one point to another.

[0191] For the robot's current position ( (This refers to the robot's current heading angle, i.e., the angle between the robot's forward direction and the x-axis, and the target point to be visited.) The transfer cost is the sum of the movement time and the rotation time, that is:

[0192]

[0193] The calculations for each part are as follows:

[0194] (1) Movement time: the time taken for the robot to move from point Time The time taken for linear motion is obtained from the ratio of the Euclidean distance to the maximum linear velocity:

[0195]

[0196] In the formula is the robot's maximum linear velocity, and is a fixed hardware parameter.

[0197] (2) Rotation time: The time it takes for the robot to adjust from the current heading angle to the target heading angle, where the target heading angle is the time it takes for the robot to rotate from point A. Point of view The direction angle is calculated as follows:

[0198] ① Calculate the target heading angle: ;

[0199] ② Calculate the heading angle difference: ;

[0200] ③ Normalize the angle difference to Range, avoid invalid rotations: ;

[0201] ④ Calculate the rotation time: In the formula represents the robot's maximum angular velocity, and represents a fixed hardware parameter.

[0202] 3.2.2 Improved Two-Step Solution Process of the TSP Algorithm

[0203] The improved TSP algorithm aims to minimize the total transfer cost by finding the optimal access order for the robot to traverse all cluster target points. It employs a combined strategy of "generious initial path generation using a greedy algorithm + path optimization using a 2-opt iterative algorithm," which quickly obtains feasible initial paths and approximates the global optimum through iterative optimization. The specific process is as follows:

[0204] (1) Greedy algorithm to generate initial feasible path

[0205] Starting from the center point of the cell where the robot is currently located, a complete initial traversal path is quickly generated using a nearest neighbor greedy strategy. This provides a high-quality starting point for subsequent iterations and optimizations, avoiding inefficient optimization caused by random initial paths. The specific steps are as follows:

[0206] ① Initialize the set of visited points Current point As the center point of the unit, the initial path ;

[0207] ② From the set of target points to be visited In the middle, select the current point The point with the lowest transfer cost ,Right now ;

[0208] ③ Add to visited collection and initial path and update the current point to ;

[0209] ④ Repeat steps ②-③ until... This completes the generation of the initial feasible path.

[0210] (2) The 2-opt iterative algorithm is optimized to the optimal path.

[0211] Based on the initial path generated by the greedy algorithm, the 2-opt local search algorithm is used for iterative optimization. This algorithm continuously reduces the total transition cost by flipping the sub-paths between any two nodes in the path, gradually approaching the globally optimal path. It has the characteristics of simple implementation and high optimization efficiency. The specific optimization steps are as follows:

[0212] ① Let the initial optimization path be Calculate its total transfer cost ( (This is the number of target points in the path; no backtracking to the starting point is required, only the cost of traversing all target points is calculated).

[0213] ② Traverse all pairwise node combinations in the path ( ), flip the node To the node The sub-paths between them are used to obtain the new path. ;

[0214] ③ Calculate the total transfer cost of the new path. ,like Then update and optimize the path. And update the total cost simultaneously. ;

[0215] ④ Repeat steps ②-③ until, after traversing all node combinations, the total cost no longer decreases. The resulting path is... This is the optimal path for picking up the ball.

[0216] (3) Output the optimal ball picking order

[0217] Extract the optimal path The order in which cluster target points are accessed is the robot's global ball-picking order. The robot will move to each cluster target point in sequence according to this order to efficiently pick up all the tennis balls in the current cell.

[0218] 4. Improved Dynamic Trajectory Prediction Path Planning

[0219] After obtaining the access order of the target points, traditional path tracking algorithms can only move along a preset path, failing to respond to sudden obstacles in the field in real time, and the movement trajectory has poor smoothness. This invention proposes a sampling-based dynamic trajectory planning method, which samples multiple sets of candidate motion trajectories and combines collision detection and cost filtering to select the optimal trajectory as the robot's real-time control input, achieving the dual goals of dynamic obstacle avoidance and smooth movement.

[0220] 4.1 Kinematic trajectory sampling

[0221] Trajectory sampling is based on a differential kinematics model of the robot. This model is adapted to the motion characteristics of wheeled mobile robots, considering only planar motion (z-axis is the ground, no vertical motion). With the robot's current position as the origin and the current heading as the positive x-axis, the robot's kinematic model is as follows:

[0222]

[0223] in, The robot has a constant linear velocity (which remains unchanged during movement). For the robot's angular velocity, These represent the linear velocity along the x-axis and y-axis, and the angular velocity along the heading angle, respectively. Provides the robot's real-time heading angle.

[0224] (1) To cover all feasible turning motions of the robot, uniform sampling is performed within the effective range of angular velocity, and the angular velocity range is set as follows: The sampling step size is The sampled angular velocities are obtained as follows:

[0225]

[0226] in The total number of sampled angular velocities in this scheme It covers all motion scenarios including left turn, straight line, and right turn.

[0227] (2) For each sampled angular velocity The robot's trajectory is pre-calculated within the next 1.2 seconds (1.2 seconds is the trajectory prediction time, balancing real-time performance and obstacle avoidance effectiveness). The robot's initial state is assumed to be... Position of time Heading angle For any time Coordinates of points on the trajectory Solve for two motion states:

[0228] ① Linear motion: when When the angular velocity approaches 0, it is considered to be without steering. The robot moves in uniform linear motion, and the trajectory formula is:

[0229]

[0230] ② Circular motion: when At that time, the robot performs uniform circular motion, and the turning radius is... The trajectory formula is:

[0231]

[0232] Using the above formula, the complete candidate trajectory corresponding to each sampled angular velocity can be obtained, and all candidate trajectories constitute a trajectory set. ,in angular velocity The corresponding trajectory.

[0233] 4.2 Trajectory Validity Collision Detection

[0234] To quickly filter out collision-free feasible trajectories, for each candidate trajectory Perform collision detection at key points (avoiding traversing all points on the trajectory, balancing detection efficiency and accuracy). The validity of the trajectory is determined by checking the obstacle distribution at the midpoint and endpoint. The specific detection process is as follows:

[0235] (1) Extract key detection points: for candidate trajectories Extract the midpoint within the prediction time ( The corresponding spatial point and the endpoint ( The corresponding spatial point ;

[0236] (2) Obstacle neighborhood determination: based on and Centered on the target, a circular detection area with a radius of 0.3m is constructed, and the number of obstacle point clouds in each area is counted. If the number of obstacle point clouds in a certain detection area is less than 3, the point is considered to have no collision (excluding the interference of isolated noise points).

[0237] (3) Trajectory validity determination: If and If there is no collision at either of the two key detection points, the candidate trajectory is determined to be a valid trajectory; if there is a collision at either detection point, it is determined to be an invalid trajectory and is discarded directly.

[0238] All valid trajectories constitute the set of feasible trajectories. This provides a foundation for subsequent optimal trajectory selection.

[0239] 4.3 Cost-Weighted Optimal Trajectory Selection

[0240] For the set of feasible trajectories This solution adheres to the core principles of "closeness to the target direction + priority smooth motion," employing an angular velocity priority filtering strategy to select the optimal trajectory. It also incorporates a dynamic speed mode switching mechanism to improve motion efficiency in open areas. The specific implementation method is as follows:

[0241] Optimal trajectory selection rules:

[0242] Traverse all candidate trajectories in ascending order of angular velocity (i.e., according to...) (Sequence), find the first valid trajectory as the optimal motion trajectory for the current robot, and the sampled angular velocity corresponding to this trajectory. and constant linear velocity This refers to the real-time control input for the robot.

[0243] The core advantage of this rule is that it prioritizes the trajectory with the lowest angular velocity, ensuring that the robot moves as smoothly as possible in the direction of the target, reducing unnecessary turning, and maximizing motion efficiency while avoiding obstacles.

[0244] 5. Adaptive edge-traversal strategy for boundary completion

[0245] After the robot completes its ball-collecting task in all internal areas, it automatically enters the boundary replenishment mode, performing continuous, smooth, and collision-free traversal movement along the court boundaries to pick up any tennis balls left in the boundary areas. This strategy does not rely on a global map; it achieves adaptive edge-hugging and automatic obstacle avoidance solely through real-time LiDAR perception and trajectory selection, ensuring that the robot always moves smoothly along the court boundaries.

[0246] 5.1 Edge-fitting Motion Control Based on Dynamic Trajectory Sampling

[0247] In boundary clearance mode, the robot does not rely on global path planning. Instead, it achieves natural edge contact and dynamic obstacle avoidance through real-time sampling, real-time collision detection, and real-time optimal trajectory selection.

[0248] (1) Angular velocity sampling rules

[0249] Maintain angular velocity sampling range

[0250]

[0251] Sampling step size The candidate angular velocity set is obtained.

[0252]

[0253] (2) Trajectory prediction rules

[0254] For each sampled angular velocity Predicting the future The trajectory of movement within.

[0255] - when At that time, the robot moves in a straight line, and its trajectory is as follows:

[0256]

[0257] - when At that time, the robot moves in a circular arc, with a turning radius of... The trajectory is:

[0258]

[0259] (3) Edge-priority trajectory selection strategy

[0260] When the robot moves along the boundary, it always selects the effective trajectory with the smallest angular velocity (far right), so that the robot naturally moves to the boundary of the site, achieving pure perception-based edge-fitting motion without instructions or a global path.

[0261] The selection rule is:

[0262]

[0263] That is, traverse all candidate trajectories from right to left and select the first valid trajectory as the current control output.

[0264] This method requires no boundary model or global path; it enables the robot to autonomously and smoothly navigate along the inner boundary of the site solely through the natural behavior of "prioritizing left turns and collision avoidance."

[0265] 5.2 Trajectory Validity Collision Detection

[0266] For each sampling trajectory, a dual-point detection method (midpoint + endpoint) is used to determine whether it is safe to pass:

[0267] (1) Take the midpoint of the trajectory and the finish line As a testing point ;

[0268] (2) Establish a radius with the detection point as the center. The safe zone;

[0269] (3) Count the number of obstacle point clouds within the statistical area ;

[0270] (4) If satisfied

[0271]

[0272] If the trajectory is valid and there is no collision, then the trajectory is determined to be valid and there is no collision.

[0273] Only trajectories that simultaneously ensure the safety of both the midpoint and the endpoint can enter the optimal trajectory selection process.

[0274] 5.3 Adaptive adjustment of velocity for boundary motion

[0275] During the boundary traversal, the robot automatically switches its movement speed based on the distribution of obstacles ahead:

[0276] (1) High-speed mode ( )

[0277] When all trajectories are valid for 5 consecutive seconds, the robot enters high-speed mode and moves forward quickly to find the boundary.

[0278] (2) Conventional mode ( )

[0279] When the trajectory detects obstacles, boundary walls, or near corners, the robot automatically switches to normal speed to ensure smooth and safe movement along the edge.

[0280] (3) Mechanism for switching back to normal mode from high-speed mode

[0281] In high-speed mode, if an invalid trajectory exists for 0.5 seconds consecutively, the system returns to normal mode for judgment. The judgment method is to traverse from the invalid trajectory to the trajectory with a larger angular velocity. When the first valid trajectory is found, its velocity is taken as the normal velocity. This process continues from... The trajectory is traversed to the left to find a valid trajectory as the running trajectory, until... Until the trajectory is a valid trajectory, switch back to fully normal mode, i.e., from... The trajectory begins to be traversed.

[0282] This mechanism ensures that no ball is missed or left behind in the boundary area, achieving an integrated movement of "sticking to the ball while walking, picking up the ball, and avoiding obstacles."

[0283] As can be seen, the main ideas of the technical solution described in this embodiment include:

[0284] 1. A rapid site modeling method based on dual static point cloud acquisition: By acquiring point clouds twice in place, the wall point cloud is extracted to quickly divide the working area without the need for moving and traversing to build the map.

[0285] 2. A tennis ball detection method based on the height difference of 8 neighborhoods of two-dimensional voxels: This method distinguishes tennis balls, obstacles and the ground by the height difference of the voxel neighborhoods, thereby improving detection robustness.

[0286] 3. The nearest-neighbor tennis ball clustering method merges tennis balls less than 0.12m apart into a single target point, adapting to the characteristics of the scanning mechanism and reducing path nodes;

[0287] 4. A dynamic path planning method based on kinematic trajectory sampling selects the optimal effective trajectory by sampling candidate trajectories with different angular velocities, thereby achieving real-time obstacle avoidance and smooth motion.

[0288] 5. Boundary adaptive edge-following picking strategy: After picking up balls in the internal area, the system traverses along the field boundary to pick up balls again, thus solving the problem of missing balls at the boundary.

[0289] Its main technical effects are:

[0290] 1. Significantly improved site initialization efficiency: The rapid mapping method only requires 6 seconds of on-site data acquisition time to complete the modeling of the entire site. Compared with the several minutes of traditional SLAM mapping, the efficiency is improved by more than 90%, enabling rapid startup of unknown sites;

[0291] 2. Stronger object detection robustness: The provided voxel neighborhood detection method is unaffected by lighting conditions. Compared with visual detection, it can work normally in strong light, low light, and rainy weather, and the false detection rate is reduced to below 0.8%.

[0292] 3. Significantly improved ball retrieval efficiency: Through nearest neighbor clustering and improved TSP planning, the global ball retrieval time is reduced by 35% compared to traditional methods, especially in scenarios with dense tennis balls, where the efficiency improvement is even more obvious;

[0293] 4. Zero Missed Pickup Coverage: Through the boundary replenishment strategy, the missed pickup rate in the boundary area is reduced from 15% in the existing technology to 0, achieving 100% coverage of the entire court, and all tennis balls can be picked up;

[0294] 5. Enhanced motion safety: The dynamic trajectory planning method can avoid obstacles in real time and will not collide with walls when moving along the edge, greatly improving the robot's motion safety.

[0295] The above technical solutions are merely exemplary embodiments of the present invention. For those skilled in the art, based on the application methods and principles disclosed in the present invention, it is easy to make various types of improvements or modifications, and not limited to the methods described in the specific embodiments of the present invention. Therefore, the methods described above are merely preferred and not restrictive.

Claims

1. A method for site perception and efficient path planning of an adaptive tennis ball-retrieval robot, characterized in that, Includes the following steps: S1 uses a robot to collect point cloud data from two perspectives in situ, thereby extracting the site boundaries and dividing the work area into grids. S2, after the robot moves to the center point of the unit, it collects the lidar point cloud of the current area, performs multi-frame fusion, and completes the detection of tennis balls and obstacles through two-dimensional voxel neighborhood features; S3 clusters and merges all tennis balls detected in the current unit; with the minimum total motion time of the robot as the core optimization objective, the cost function is constructed by incorporating rotation time into the distance; the optimal path is solved by a two-step solution strategy of generating the initial path with a greedy algorithm and optimizing the path with a 2-opt iterative algorithm, so as to realize the planning of the global ball picking order.

2. The method according to claim 1, characterized in that, Step S1 specifically includes: S11: The robot collects point clouds from two perspectives at any point on the court, transforms them to the global map coordinate system, and performs a union and fusion to obtain the global court point cloud set. ; S12, for the global point cloud set The wall point cloud is extracted based on a set height threshold; the minimum bounding rectangle of the wall point cloud is calculated as the working area for robot movement and operation. ; S13, work area It is divided into multiple regional units for the robot to traverse and pick up balls in different regions.

3. The method according to claim 1, characterized in that, Step S2 specifically includes: S21. After the robot moves to the center point of the unit, it performs noise reduction and fusion on the multi-frame LiDAR point cloud collected in the current area to obtain a high-density point cloud set of the current area. Construct a two-dimensional voxel grid: Divide the point cloud of the current region into several square grids along the xy plane, with the grid side length slightly smaller than the diameter of a tennis ball; For each voxel corresponding to a grid, calculate the height range of all point clouds within it, and use it as the height range feature of each voxel. S22: For each voxel, extract the extreme height values ​​of itself and its surrounding adjacent voxels, calculate the difference between the maximum height value and the minimum height value of all voxels, and distinguish between tennis balls, obstacles, and the ground based on the height difference. S23, For the neighborhood area determined to be the tennis ball, the center coordinates of the tennis ball are accurately located by traversing through a sliding window; S24. For areas identified as obstacles, convert them into obstacle grids in the grid map, expand the obstacle grids so that the robot can avoid the expanded areas during subsequent path planning, and set the robot's ball picking threshold based on the distance relative to the obstacle grids to avoid collisions with obstacles during the picking process.

4. The method according to claim 3, characterized in that, The specific method of step S22 includes: (1) Extract the current voxel The set of adjacent voxels, Two-dimensional integer indices for voxels: defined as all indices ( )satisfy and The voxels are denoted as: Includes the current voxel and a ring of adjacent voxels; (2) Calculate the overall height difference within the neighborhood formed by the current voxel and its surrounding adjacent voxels: Take the difference between the maximum height value and the minimum height value of all voxels in the neighborhood, that is: ； (3) Classify targets according to the range of height difference: like If the target in that neighborhood is a tennis ball; like If the target in that neighborhood is an obstacle, then the target in that neighborhood is an obstacle. The rest, i.e. or If the area is flat and there are no valid targets, then the area is flat ground.

5. The method according to claim 1, characterized in that, Step S3 specifically includes: S31, Nearest Neighbor Tennis Clustering and Merging: Merge several nearby tennis balls that are less than a set threshold into a single target point; S32 defines the transfer cost of the robot from its current position to the target point to be visited as the sum of the movement time and the rotation time, where the rotation time is the time it takes for the robot to adjust from the current heading angle to the target heading angle; S33, for the set of target points to be visited Starting from the center point of the cell where the robot is currently located, a complete initial traversal path is quickly generated using a nearest neighbor greedy strategy. ; S34, the initial path generated by the greedy algorithm is iteratively optimized using the 2-opt local search algorithm to obtain the resulting path. This is the optimal path for picking up the ball; S35, Output the optimal path The order in which the target points are visited is the robot's global ball-picking order. The robot moves to each target point in sequence according to this order to efficiently pick up all the tennis balls in the current unit.

6. The method according to claim 5, characterized in that, Step S31 specifically includes: For the detected tennis ball set ,in For the first The two-dimensional coordinates of a tennis ball are used for distance threshold clustering. The steps include: Initialize cluster set ; Traverse each tennis ball If clustering exists , making arrive If the distance between the centers is less than 0.12m, then... Add to this cluster; Distance determination: Otherwise, create a new cluster and... Add to this cluster; For each cluster The target point is the center point of all tennis balls within the cluster, and the calculation formula is: Let clustering Include tennis Then the cluster center coordinates are: Cluster target points are denoted as: This clustering method merges multiple close-range tennis balls into a single target point.

7. The method according to claim 5, characterized in that, Step S32 specifically includes: Define the robot from its current position. , The robot's current heading angle is the angle between the robot's forward direction and the x-axis, and the distance to the target point is... The transfer cost is the sum of the movement time and the rotation time, that is: The calculations for each part are as follows: （1） In the formula, Here, represents the robot's maximum linear velocity, and represents a fixed hardware parameter. (2) Rotation time: The time it takes for the robot to adjust from the current heading angle to the target heading angle, where the target heading angle is the time it takes for the robot to rotate from point A. Point of view The direction angle, specifically calculated using the following methods: ① Calculate the target heading angle: ; ② Calculate the heading angle difference: ; ③ Normalize the angle difference to Range, avoid invalid rotations: ; ④ Calculate the rotation time: In the formula represents the robot's maximum angular velocity, and represents a fixed hardware parameter.

8. The method according to claim 1, characterized in that, It also includes the following steps: S4 samples multiple candidate motion trajectories, combines collision detection and cost filtering to select the optimal trajectory, which serves as the real-time control input for the robot, achieving the dual goals of dynamic obstacle avoidance and smooth motion; Specifically, it includes: S41, uniformly sample within the effective range of all feasible turning angular velocities of the robot to obtain the complete candidate trajectory corresponding to each sampled angular velocity. All candidate trajectories constitute a trajectory set. ,in angular velocity The corresponding trajectory; S42, for each candidate trajectory Collision detection at key points: The validity of a trajectory is determined by examining the distribution of obstacles at the midpoint and endpoint of the trajectory within each predicted time period. Collision-free feasible trajectories are selected, and all valid trajectories constitute a set of feasible trajectories. ; S43, for the set of feasible trajectories The trajectory with the smallest angular velocity is selected first as the optimal motion trajectory for the current robot, and the sampling angular velocity corresponding to this trajectory is used. and constant linear velocity As the real-time control input for the robot.

9. The method according to claim 1, characterized in that, It also includes the following steps: S5. After the robot completes the ball-collecting task in all internal areas, it enters the boundary replenishment mode. In this mode, the robot performs a continuous, smooth, and collision-free traversal movement along the boundary of the field to pick up the tennis balls left in the boundary area.

10. The method according to claim 9, characterized in that, In step S5, the robot does not use global path planning in the boundary replenishment mode, but achieves natural edge contact and dynamic obstacle avoidance through real-time sampling, real-time collision detection, and real-time optimal trajectory selection. Specifically, it includes: S51, edge-fitting motion control based on dynamic trajectory sampling: Set the angular velocity sampling range and sampling interval; For each sampled angular velocity Predict the trajectory of movement within a set prediction period in the future; Edge-priority trajectory selection strategy: When the robot moves along the boundary, it always prioritizes the effective trajectory with the smallest angular velocity, so that the robot naturally moves to the boundary of the site, realizing pure perception edge-fitting motion without instructions or a global path. S52: For each sampled trajectory, the midpoint and endpoint are detected within the predicted time to determine whether it is safe to pass. Only trajectories that simultaneously satisfy the safety of the midpoint and endpoint will enter the optimal trajectory selection process. S53, during the boundary traversal, the robot automatically switches its movement speed according to the distribution of obstacles ahead.

Images