A perception and planning method for automatic ball picking of a tennis ball picking robot

Abstract

A perception and planning method for automatic ball picking of a tennis ball picking robot, mainly including four parts of visual perception, map state updating, path planning and robot motion planning; the method is based on the camera carried by the ball picking robot, uses computer vision and artificial intelligence technology to identify and locate the coordinates of the tennis ball in the local field of view; combines the historical local field of view perception information, performs space-time fusion and task planning, without the need for prior global scanning mapping or relying on global positioning sensors, realizes the improvement of the ball picking efficiency without using more sensors, and does not miss the ball picking area.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of artificial intelligence, in particular to a perception and planning method for a tennis ball picking robot to automatically pick up balls. BACKGROUND

[0002] The existing ball picking schemes mainly include:

[0003] Scheme 1: manually operated hand-pushed ball picking device or hand-pushed ball picking vehicle, which picks up balls on the tennis court through a hand-pushed mechanism;

[0004] Scheme 2: using a camera carried by a ball picking robot to identify and locate a tennis ball in a local field of view, and picking up the ball after identifying the ball;

[0005] Scheme 3: using a camera carried by a ball picking robot, cooperating with more sensors to locate a global tennis ball and a robot, or performing global scanning and mapping in a tennis court in advance, so as to plan a ball picking path based on global information.

[0006] The main disadvantages of the above methods are:

[0007] Scheme 1: manual operation, time-consuming and laborious, efficiency depends on the walking path of the person, and is not automated.

[0008] Scheme 2: due to the large size of the tennis court, the local field of view of the camera carried by the ball picking robot is limited and cannot cover the entire court, when there is no ball in the local field of view, the unknown area cannot be effectively explored or only limited exploration is performed through the way of turning around in place, and there is a blind area for picking up balls in the court; even if all the areas with balls can be seen, the ball picking robot will have low efficiency of the planned path due to the limitation of the field of view.

[0009] Scheme 3: needs to cooperate with more sensors to locate a global tennis ball and a robot, or perform global scanning and mapping in a tennis court in advance, so as to plan a ball picking path based on global information; the technical route is more complex, and the introduction of sensors leads to higher system cost. SUMMARY

[0010] The present disclosure provides a perception and planning method for a tennis ball picking robot to automatically pick up balls, which uses computer vision and artificial intelligence technology in a local field of view based on a camera carried by a ball picking robot to identify and locate the coordinates of a tennis ball in the local field of view; without the need for prior global scanning and mapping or relying on global positioning sensors, the method combines historical local field of view perception information for spatio-temporal fusion and planning, realizes the improvement of ball picking efficiency without using more sensors, and does not miss the ball picking area.

[0011] The perception and planning method for a tennis ball picking robot to automatically pick up balls provided by the present disclosure includes the following steps:

[0012] S1, visual perception: taking pictures of the scene by the camera mounted on the tennis ball picking robot moving with the robot, identifying several tennis balls in the current field of view from the pictures, and then converting the identified tennis balls from the image coordinate system to the tennis ball picking robot coordinate system using the pre-calibrated camera internal and external parameters;

[0013] S2, map state update: converting the tennis ball coordinates in the picking robot coordinate system at the time of taking pictures to the tennis ball coordinates in the picking robot coordinate system at the current time, and then to the map global coordinates; based on the map global coordinates, tracking and fusing the current detection results of the tennis balls in the current field of view with the historical detection results; based on the fusion results, updating the map ball cluster state; based on the current position and field of view of the robot, updating the map exploration state;

[0014] S3, path planning: based on the current fusion of the tennis ball detection results and the current map exploration state, selecting the task with less time consumption as the path planning target from the exploration of the unexplored area or the picking of the tennis ball;

[0015] S4, robot motion planning: driving the robot to complete the task specified by the path planning according to the planning target.

[0016] Further, the step S2 specifically comprises:

[0017] S21, global map gridding, state initialization as unexplored state;

[0018] S22, start receiving visual perception data, i.e. tennis ball detection results in the picking robot coordinate system at the time of taking pictures;

[0019] S23, obtain the tennis ball picking robot odometer information, convert the tennis ball coordinates in the picking robot coordinate system at the time of taking pictures to the tennis ball coordinates in the picking robot coordinate system at the current time;

[0020] S24, update the map grid exploration state in the field of view at the time of taking pictures;

[0021] S25, convert the tennis ball coordinates in the picking robot coordinate system in the current field of view at the current time to the map global coordinates;

[0022] S26, tracking and fusing the current detection results with the historical detection results of the area in the current field of view at the current time, updating the historical detection results of the balls in the map;

[0023] S27, based on the update results of step S26, updating the ball cluster state in the map, the ball cluster refers to a region containing multiple balls, and the diameter of the minimum circumscribed circle of all balls in the region is less than the ball cluster diameter threshold;

[0024] S28, whether all the grids of the global map are explored, yes, reset the map state to the initialization state;

[0025] S29, wait for receiving the next visual perception result, i.e. repeat steps S22-S29.

[0026] Further, the step S21 includes the following specific methods:

[0027] The global map is uniformly divided into m*n large grids, and the division of the large grids needs to meet the requirement that the robot can turn around at the center of the large grid and the field of view can cover the entire large grid;

[0028] Then, each large grid is uniformly divided into k*k small grids, and when the exploration states of all small grids in a large grid are "explored", the exploration state of the large grid is set to "explored".

[0029] The state initialization will delete all recorded tennis ball coordinates in the global map, and set the exploration states of all large grids and small grids to unexplored.

[0030] Further, the step S24 includes the following specific methods:

[0031] According to the position of the tennis ball picking robot in the global map at the current time and the pose matrix H_global, combined with the camera field of view, the small grids near the position of the robot are calculated, and the exploration state of the small grid completely covered by the current camera field of view is set to "explored".

[0032] When the exploration states of all small grids in a large grid are "explored", the exploration state of the large grid is set to "explored".

[0033] Further, the step S26 includes the following specific methods:

[0034] Extract the historical detection results of the tennis balls in the area covered by the current field of view;

[0035] In the global coordinate system, match the historical detection results covered by the current field of view with the n tennis ball detection results in the current field of view in position;

[0036] For the matched tennis balls, update their historical detection results to the global coordinates at the current time.

[0037] Delete all historical detection results that are covered by the field of view for three times and are not successfully matched from the historical detection results, and then add all current detection results that are not successfully matched to the historical detection results as new results.

[0038] Further, the step S3 specifically comprises:

[0039] S31, first determine whether there is a cluster or ball in the map, if not:

[0040] (1) determine whether there is a larger grid of unfinished exploration in the map, if not, do not plan; if there is, select the larger grid with the optimal efficiency weight from the larger grid of unfinished exploration as the tentative exploration target under the current state;

[0041] (2) determine whether the goal of the last path planning is to explore:

[0042] If not, directly take the tentative exploration target under the current state as the exploration target;

[0043] If yes, compare the robot motion theoretical time consumption to the exploration target under the current state + exploration time threshold, and the robot motion theoretical time consumption corresponding to the exploration target of the last path planning, select the smaller one as the exploration target;

[0044] (3) then determine whether the robot's current position is in the center of the larger grid as the exploration target:

[0045] If yes, define the goal of path planning as the robot's self-rotation for one round, and send the path planning result to the robot motion planning module;

[0046] If not, directly take the exploration to the exploration target as the path planning goal, and send the path planning result to the robot motion planning module;

[0047] S32, when the first step of path planning determines that there is a cluster or ball in the map:

[0048] (1) according to the designed motion planning algorithm, select the cluster or ball with the optimal time consumption as the tentative ball picking target under the current state;

[0049] Then, determine whether the goal of the last path planning is to pick up the ball:

[0050] If not, directly take the tentative ball picking target under the current state as the ball picking target;

[0051] If yes, compare the robot motion theoretical time consumption corresponding to the tentative ball picking target under the current state + ball picking time threshold, and the robot motion theoretical time consumption corresponding to the ball picking target of the last path planning, select the smaller one as the ball picking target;

[0052] (2) after determining the ball picking target, further determine whether there is a larger grid of unfinished exploration in the map:

[0053] If not, go to the ball picking target to pick up the ball as the path planning target, and send the path planning result to the robot motion planning module;

[0054] If yes, calculate the efficiency weight of the uncompleted exploration large grid, and select the one with the optimal efficiency weight as the exploration target;

[0055] (3) After completing the selection of the exploration target, determine whether the current position of the robot is at the center of the large grid as the exploration target:

[0056] If yes, define the path planning target as the robot rotating in place for one round, and send the path planning result to the robot motion planning module;

[0057] If not, compare the motion planning theoretical time consumption of the robot from the current position to the exploration target + the exploration threshold value, and the motion planning theoretical time consumption of the robot from the current position to the ball picking target, and select the task with smaller time consumption as the path planning target, and send the path planning result to the robot motion planning module.

[0058] Further, the method for selecting the large grid with the optimal efficiency weight from the uncompleted exploration large grids in step S3 comprises:

[0059] Let the efficiency weight of the uncompleted exploration large grid be Efficiency(k), where k = 1, 2, …, n_big_block, and n_big_block is the total number of uncompleted exploration large grids;

[0060] The calculation method of Efficiency(k) comprises: taking the distance from the current position of the robot to the center of the uncompleted exploration large grid as D_explore(k), and taking the number of uncompleted exploration small grids contained in the uncompleted exploration large grid as N_block_explore(k), then:

[0061]

[0062] Therefore, under the current state, the large grid with the optimal efficiency weight is the grid with the maximum Efficiency value.

[0063] Further, in step S3,

[0064] The exploration time threshold value and the ball picking time threshold value are both taken as 1 second;

[0065] The value of the exploration threshold value can be greater than 0 or less than 0, depending on whether exploration is prioritized or ball picking is prioritized.

[0066] Compared with the prior art, the beneficial effects of the present disclosure are:

[0067] (1) Using computer vision and artificial intelligence technology to realize perception and planning algorithm, which is used for the ball picking robot to automatically pick up balls on the tennis court, avoiding the consumption of players' physical strength due to manual ball picking, and maximizing the players' training concentration to improve the training effect;

[0068] (2) Based on the camera carried by the ball picking robot, the computer vision and artificial intelligence technology are used in the local field of view to identify and locate the tennis coordinates in the local field of view, and the historical local field of view perception information is combined for spatio-temporal fusion and planning, without the need for prior global scanning mapping or relying on global positioning sensors, the ball picking efficiency is improved without using more sensors, and the ball picking area is not missed;

[0069] (3) In high frame rate planning, the problem of path planning target switching between two theoretically similar targets due to robot position micro-motion in two planning processes is largely avoided when the theoretical time consumption of two targets is similar, and the target is more inclined to continue the last path planning when the theoretical time is similar; At the same time, by setting a threshold, the robot can be controlled to prefer to pick up balls or explore;

[0070] (4) While the camera is taking pictures and performing perception calculation, the robot can keep moving on the original planning path until the perception calculation result is obtained to update the map state, and then a new round of path planning is performed to realize high frame rate dynamic correction of path planning target, without stopping the robot to wait for the results of photographing and perception calculation. BRIEF DESCRIPTION OF DRAWINGS

[0071] The above and other objects, features and advantages of the present disclosure will become more apparent from the following detailed description of exemplary embodiments of the present disclosure taken in conjunction with the accompanying drawings, in which like reference characters refer to the like parts throughout the different views.

[0072] Figure 1 The overall control strategy for the tennis ball picking robot according to the present disclosure to complete the automatic ball picking task;

[0073] Figure 2 The image coordinate system and the tennis ball picking robot coordinate system are defined in the exemplary embodiment;

[0074] Figure 3 The map state updating process of the exemplary embodiment;

[0075] Figure 4 The tracking fusion method flowchart of the current detection result and the historical detection result;

[0076] Figure 5 The exemplary path planning flowchart. DETAILED DESCRIPTION

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

[0078] The present disclosure provides a perception and planning method for a tennis ball picking robot to automatically pick up balls. According to an exemplary embodiment of the present disclosure, the overall control strategy for the tennis ball picking robot to complete the automatic ball picking task mainly includes four parts: visual perception, map state update, path planning, and robot motion planning (as shown in FIG. Figure 1

[0079] I. Visual Perception

[0080] The visual perception module captures a scene image Image through a camera installed on the tennis ball picking robot and moving with the robot. The n tennis balls Balls {b(0), b(1), …, b(n)} in the current field of view are identified using an artificial intelligence algorithm model (Yolov5 model is used in this embodiment) on Image, where n ≥ 0 and is an integer. Then, the n tennis balls Balls are converted from the image coordinate system coordinates Balls_Image {b_image(0), b_image(1), …, b_image(n)} to the tennis ball picking robot coordinate system coordinates Balls_Robot {b_robot(0), b_robot(1), …, b_robot(n)} using the camera internal parameters M and external parameters [R|T] calibrated in advance.

[0081] Wherein, the image coordinate system coordinates b_image(i) of the i^(i = 0, 1, 2, …, n) tennis ball can be represented as {u(i), v(i)}, and the corresponding tennis ball picking robot coordinate system coordinates b_robot(i) can be represented as {x(i), y(i), z(i)}.

[0082] Wherein, the image coordinate system and the tennis ball picking robot coordinate system are defined with reference to Figure 2 The origin of the image coordinate system is at the top left corner of the picture, and the width and height directions of the picture are the x and y directions of the image coordinate system, respectively. The tennis ball picking robot coordinate system can be defined as needed. In this embodiment, the vertical projection point of the center of the driving wheel axle of the tennis ball picking robot on the ground plane is taken as the coordinate origin, the positive direction of the y-axis is in front of the robot advancing in the vertical direction to the driving wheel axle, the positive direction of the z-axis is vertically upward, and the positive direction of the x-axis is to the right of the driving wheel axle when looking in the forward direction of the robot advancing.

[0083] ​The specific method of converting the recognized n tennis balls Balls from the image coordinate system coordinates Balls_Image{b_image(0), b_image(1), …, b_image(n)} to the tennis ball picking robot coordinate system coordinates Balls_Robot{b_robot(0), b_robot(1), …, b_robot(n)} is as follows.

[0084] It is known that {u(i), v(i)}, {x(i), y(i), z(i)}, M and [R|T] satisfy the following relationship:

[0085]

[0086] where the unknown z c is solved as follows. Take the radius of the tennis ball as a fixed z(i), and in this embodiment z(i) = 34 mm.

[0087] The above formula is transformed as follows:

[0088]

[0089] Let H2 = R -1 ·T, we get

[0090] From the third row of the formula, we get z c = (z(i) + H2[2]) / H1[2], where H1[2] and H2[2] are the third rows of H1 and H2 respectively.

[0091] After obtaining z c , {x(i), y(i), z(i)} can be obtained through {u(i), v(i)}:

[0092]

[0093] The method of the camera carried by the ball picking robot for perceiving and positioning the tennis ball in the local field of view can also use the traditional ball edge detection algorithm in computer vision, other target detection algorithms based on deep learning, such as SSD, YOLO other series models, Faster-RCNN, CenterNet, detection models based on Transformer, etc.

[0094] II. Map state update

[0095] In this embodiment, the flow of the map state update module is as shown in Figure 3 .

[0096] (1) First, the global map is rasterized, and the state is initialized.

[0097] Specifically, the global map is uniformly divided into m*n large square grids, and the side length of the large square grid is block_w. In this embodiment, block_w = 5m is taken as an example for illustration. Then each large square grid is uniformly divided into k*k small square grids. In this embodiment, k = 10 is taken as an example for illustration, and the side length of each small square grid is block_w / k, which is 5 / 10 = 0.5m in this embodiment. The division of the large square grid needs to meet the requirement that the robot can turn around at the center of the large square grid, and the field of view can cover the entire large square grid.

[0098] The global map is rasterized to facilitate the representation of the explored and unexplored areas in the global map, and to facilitate efficient calculation of the exploration efficiency of different areas of the map. State initialization will delete all recorded tennis coordinates in the global map, and set the exploration state of all large square grids and small square grids to "unexplored".

[0099] Of course, other shapes of grids can also be used, such as hexagonal grids.

[0100] (2) After completing the global map rasterization and state initialization, data can be continuously received from the visual perception module. The data is n tennis detection results in the robot coordinate system at the camera shooting time, i.e. Balls_Robot{b_robot(0), b_robot(1), …, b_robot(n)}, n ≥ 0 and is an integer.

[0101] (3) After obtaining Balls_Robot, the tennis robot odometer information is immediately obtained. The odometer information includes: the current position and pose matrix H_global of the tennis robot in the global map, and the relative motion matrix H_relative of the tennis robot from the camera shooting time corresponding to Balls_Robot to the current time. Wherein, H_global and H_relative are both 3*3 rotation and translation matrices in the two-dimensional plane coordinate system.

[0102] Then, the tennis ball coordinates Balls_Robot{b_robot(0), b_robot(1), …, b_robot(n)} in the pick-up robot coordinate system at the time of shooting are converted into the tennis ball coordinates Balls_Robot_current{b_robot_current(0), b_robot_current(1), …, b_robot_current(n)} in the pick-up robot coordinate system at the current time by H_relative. Wherein, the tennis ball coordinates b_robot(i) of the i(th) (i = 0, 1, 2, …, n) tennis ball in the pick-up robot coordinate system at the time of shooting can be represented as {x(i), y(i), z(i)}, and the corresponding tennis ball pick-up robot coordinate system coordinates b_robot_current(i) at the current time can be represented as {x_c(i), y_c(i), z_c(i)}. The specific conversion method is as follows:

[0103]

[0104] There are many ways to obtain the pick-up robot odometry information, such as wheel odometry, IMU odometry, visual odometry, laser radar-based odometry, and odometry based on fusion of different sensors (IMU, vision, laser radar).

[0105] (4) Next, update the exploration state of the map grid in the field of view at the time of shooting. The specific method is:

[0106] According to the position and pose matrix H_global of the tennis ball pick-up robot in the global map at the current time, combined with the camera field of view, the small square grids near the position of the robot are calculated, and the exploration state of the small square grid completely covered by the current camera field of view is set to “explored”.

[0107] When the exploration state of all small square grids in a large square grid is “explored”, the exploration state of the large square grid is set to “explored”.

[0108] (5) Next, the current time in the field of view of the robot coordinates under the coordinates of the tennis balls Balls_Robot_current {b_robot_current (0), b_robot_current (1), …, b_robot_current (n)} into the global coordinate map Balls_Global {b_global (0), b_global (1), …, b_global (n)}. Where the i (i = 0, 1, 2, …, n)th ball in the current time in the field of view of the robot coordinates under the coordinates of the tennis balls b_robot_current (i) can be represented as {x_c (i), y_c (i), z_c (i)}, its corresponding global coordinate map b_global (i) can be represented as {x_g (i), y_g (i), z_g (i)}. The specific conversion method is as follows:

[0109]

[0110] (6) Next, the current time in the field of view of the region of the current detection result of the global coordinate map Balls_Global and the historical detection result tracking fusion. The specific method is described in Figure 4 .

[0111] First, find the current time in the field of view of the historical detection result, recorded as Balls_Globa_Old {b_global_old (0), b_global_old (1), …, b_global_old (n_old)}, n_old ≥ 0 and is an integer.

[0112] Then traverse the n_old results in Balls_Globa_Old, for each traversed result b_global_old (j), j = 0, 1, 2, …, n_old, traverse the n results in Balls_Global, for each traversed result b_global (i), calculate the distance D (i, j) between b_global (i) and b_global_old (j) and record;

[0113] For each traversed result b global old (j), after traversing n results in Balls Global, select the b global (i_dmin) closest to b global old (j), judge whether the distance D(i_dmin,j) is less than the threshold D_thresh, if less than the threshold, match b global (i_dmin) with b global old (j), and update the historical detection result b global old (j) to b global (i_dmin);

[0114] The successfully matched results no longer participate in the matching in the subsequent traversal; after traversing n_old results in Balls_Globa_Old, all historical detection results covered by the field of view for three times and not successfully matched are deleted from the historical detection results, and then all current detection results not successfully matched are added to the historical detection results as new results.

[0115] Wherein, b global (i) can be represented as {x_g(i),y_g(i),z_g(i)}, b global old (i) can be represented as {x_g_old(i),y_g_old(i),z_g_old(i)}, and the distance D(i,j) between b global (i) and b global old (j) is calculated as follows:

[0116]

[0117] Wherein, the selection method of the closest b global (i_dmin) is as follows:

[0118]

[0119] Wherein, in this embodiment, D_thresh=0.1m is taken as an example for calculation.

[0120] (7) After tracking and fusing the current detection results in the current field of view region and the historical detection results, update the state of the ball cluster in the map. The ball cluster is defined as follows: if a region contains multiple balls (for example, at least 5 balls), and the diameter of the minimum circumscribed circle of all balls in this region is less than the ball cluster diameter threshold Cluster_thresh, then the balls in this region form a ball cluster. The calculation of the ball cluster is to facilitate the subsequent path planning algorithm to improve the efficiency of picking up balls.

[0121] Update the state of all ball clusters in the map through the historical detection result ball coordinates in the updated map in the last step. This embodiment takes Cluster_thresh=1m as an example for illustration.

[0122] (8) Finally, when all the small square grids in the global map are marked as "explored", reset the map state to the initialization state (including deleting all the recorded tennis ball coordinates in the global map, and setting the exploration state of all the large square grids and small square grids to "unexplored"), and then wait for the next detection result received from the perception module;

[0123] Otherwise, directly wait for the next detection result received from the perception module.

[0124] III. Path planning

[0125] The flow of the path planning module is shown in Figure 5

[0126] (1) First, determine whether there is a ball cluster or a ball in the map. If there is no ball cluster or ball:

[0127] 1) Then determine whether there is an unfinished exploration large square grid (i.e., a large square grid with an exploration state of "unexplored") in the map. If there is no unfinished exploration large square grid, no planning is performed.

[0128] 2) If there is an unfinished exploration large square grid, first calculate the efficiency weight Efficiency(k) when selecting different unfinished exploration large square grids, k = 1, 2, …, n_big_block, n_big_block is the total number of unfinished exploration large square grids, and then select the unfinished exploration large square grid with the optimal efficiency weight as the exploration target Target_explore.

[0129] The calculation method of the efficiency weight Efficiency(k) of the unfinished exploration large square grid is as follows. Let D_explore(k) be the distance from the current position of the robot to the center of the unfinished exploration large square grid, and N_block_explore(k) be the number of unfinished exploration small square grids contained in the unfinished exploration large square grid. Then:

[0130]

[0131] The selection method of the exploration target Target_explore is as follows:

[0132] First, calculate the exploration target Target_explore_current with the optimal efficiency weight in the current state:

[0133]

[0134] ​If the goal of the last path planning is not to explore, directly take the exploration goal with the optimal efficiency weight in the current state Target_explore_current as the exploration goal Target_explore, that is, exploration goal Target_explore = Target_explore_current;

[0135] Otherwise, if the goal of the last path planning is to explore, assuming that the exploration goal of the last path planning is Target_explore_last, compare the theoretical time consumption T(Target_explore_current) of the robot motion to the exploration goal with the optimal efficiency weight in the current state plus the exploration time threshold thresh_time_explore with the theoretical time consumption T(Target_explore_last) of the robot motion to the exploration goal Target_explore_last of the last path planning, and select the smaller one as the exploration goal Target_explore, that is:

[0136]

[0137] In this embodiment, thresh_time_explore = 1 second.

[0138] The purpose of setting this threshold is to avoid the path planning goal switching between two robot motion theoretical time consumptions when the robot position is slightly changed in the two planning processes, and to prefer to continue the goal of the last path planning when the theoretical time is similar.

[0139] 3) Then judge whether the current position of the robot is in the center of the large square grid that has not been explored:

[0140] If yes, define the goal of path planning as the robot rotating one circle in place, and send the path planning result to the robot motion planning module;

[0141] If the current position of the robot is not in the center of the large square grid that has not been explored, since there is no ball cluster or ball, it is not necessary to judge whether picking up the ball is better or exploring is better, directly take exploring the exploration goal as the path planning goal, and send the path planning result to the robot motion planning module.

[0142] In this embodiment, the exploration goal is defined as the center position of the large square grid where the exploration goal is located.

[0143] (2) When the first step of path planning judges that there is a ball cluster or ball in the map:

[0144] 1) According to the designed motion planning algorithm, the theoretical time consumption of robot motion when selecting different ball clusters or balls is calculated, wherein the theoretical time consumption when selecting a ball cluster is calculated by taking the minimum circumscribed circle center position of all balls in the selected ball cluster as the target position of the robot. Then, the ball cluster or ball with the optimal theoretical time consumption is selected as the ball picking target Target_ball, and the specific method is as follows:

[0145] First, the ball cluster or ball with the shortest theoretical time consumption is selected as the ball picking target Target_ball_current in the current state;

[0146] If the target of the last path planning is not to pick up the ball, the ball picking target Target_ball_current in the current state is directly taken as the ball picking target Target_ball, i.e., the ball picking target Target_ball = Target_ball_current;

[0147] Otherwise, if the target of the last path planning is to pick up the ball, assuming that the ball picking target of the last path planning is Target_ball_last, the theoretical time consumption T(Target_ball_current) of robot motion corresponding to the ball picking target Target_ball_current in the current state is compared with the theoretical time consumption T(Target_ball_last) of robot motion corresponding to the ball picking target Target_ball_last of the last path planning, and the smaller one is selected as the ball picking target Target_ball, i.e.:

[0148]

[0149] In this embodiment, thresh_time_ball = 1 second.

[0150] The purpose of setting the ball picking time threshold is to avoid the switching of the path planning target between two theoretical time consumptions similar to each other due to the slight movement of the robot position in the two planning processes, and to prefer to continue the target of the last path planning when the theoretical time is similar.

[0151] 2) After determining the ball picking target, it is further determined whether there is a large square grid that has not been explored (i.e., a large square grid with an exploration state of “unexplored”) in the map:

[0152] If there is no unfinished exploration big square grid, the robot will go to the ball picking target to pick up the ball as the path planning target, and send the path planning result to the robot motion planning module (in this embodiment, if the ball picking target is a ball cluster, the definition of going to the ball picking target is to go to the center position of the minimum circumscribed circle of all balls in the ball cluster) ;

[0153] If there is an unfinished exploration big square grid, first calculate the efficiency weight Efficiency(k) when selecting different unfinished exploration big square grids, k = 1, 2, …, n_big_block, n_big_block is the total number of unfinished exploration big square grids, and then select the unfinished exploration big square grid with the optimal efficiency weight as the exploration target Target_explore (when there is an unfinished exploration big square grid, the calculation method of each variable is the same as the foregoing) ;

[0154] 3) After the selection of the exploration target Target_explore is completed, it is judged whether the current position of the robot is in the center of the unfinished exploration big square grid:

[0155] If yes, the target of path planning is defined as the robot rotating one circle in place, and the path planning result is sent to the robot motion planning module;

[0156] If the current position of the robot is not in the center of the unfinished exploration big square grid, it is judged whether the motion planning time T_target_explore from the current position of the robot to the exploration target plus the exploration threshold Thresh_explore is greater than or equal to the motion planning time T_target_ball from the current position of the robot to the ball picking target, if T_target_explore + Thresh_explore ≥ T_target_ball, the robot will go to the ball picking target to pick up the ball as the path planning target, and send the path planning result to the robot motion planning module; otherwise, if T_target_explore + Thresh_explore < T_target_ball, the robot will go to the exploration target to explore as the path planning target, and send the path planning result to the robot motion planning module.

[0157] In this embodiment, Thresh_explore = 2 seconds is taken. The size of this variable can control whether the path planning strategy is more inclined to pick up the ball first or explore first, and the value can be greater than 0 or less than 0.

[0158] Four, robot motion planning

[0159] The robot motion planning module receives the planning target from the path planning module to drive the robot main wheel to complete the task specified by the path planning. In this process, real-time obstacle detection is performed using sensors, such as ultrasonic radar. In the case of encountering obstacles on the path to the specified target, the path planning target is maintained while the obstacles are avoided. There are many obstacle avoidance methods, such as using other sensors or fusing multiple sensors, including laser radar, millimeter wave radar, visual camera, proximity sensor, etc.

[0160] While the camera is taking pictures and performing perception calculations, the robot can maintain the original planned path movement until the perception calculation result is obtained to update the map state, and then a new round of path planning is performed to achieve high-frame-rate dynamic correction of the path planning target without stopping the robot to wait for the picture and perception calculation result.

[0161] The above technical solutions are only exemplary embodiments of the present application. For those skilled in the art, based on the application of the disclosed methods and principles, various types of improvements or modifications can be easily made, and are not limited to the methods described in the above specific embodiments of the present application. Therefore, the above-described methods are only preferred and do not have a limiting meaning.

Claims

1. A perception and planning method for a tennis ball-picking robot to automatically pick up a ball, comprising the following steps: S1, visual perception: A camera mounted on the tennis ball retrieval robot moves with the robot to capture a scene image, identify several tennis balls in the current field of view from the image, and then use pre-calibrated camera intrinsic and extrinsic parameters to transform the identified tennis balls from the image coordinate system to the tennis ball retrieval robot coordinate system; S2, map status update: Convert the tennis ball's coordinates in the robot's coordinate system at the time of the photo capture to the current robot's coordinate system, and then convert them to global map coordinates; based on the global map coordinates, track and fuse the current detection results of the tennis ball in the field of view at the current moment with the historical detection results; based on the fusion results, update the map ball cluster status; Update the map exploration status based on the robot's current position and field of view; S3, path planning: Based on the current fused tennis ball detection results and the current map exploration status, the path planning goal is to choose the less time-consuming task of exploring unexplored areas or retrieving the ball. S4, robot motion planning: according to the planning goals, drive the robot to complete the tasks specified by the path planning.

2. The method according to claim 1, characterized in that In step S1, the specific method of converting the identified tennis ball from the image coordinate system to the tennis ball picking robot coordinate system includes: Assume that n tennis balls are identified, n ≥ 0, where the image coordinates of the i-th (i = 0, 1, 2, ..., n) tennis ball b_image(i) are {u(i), v(i)}, and the corresponding tennis ball picking robot coordinates b_robot(i) are {x(i), y(i), z(i)}; the camera intrinsic parameters M, extrinsic parameters [R|T]; Then {u(i), v(i)}, {x(i), y(i), z(i)}, M and [R|T] satisfy the following relationship: Among them, the unknown z c The solution is as follows: take the radius of the tennis ball as a fixed z(i), Transform the above formula as follows: make H2=R -1 T, available From the equality of the left and right sides of the third row of the formula, we can get z c =(z(i)+H2[2]) / H1[2], where H1[2] and H2[2] are the third rows of H1 and H2 respectively; Get z c After that, {x(i),y(i),z(i)} can be obtained through {u(i),v(i)}:

3. The method according to claim 1, characterized in that The step S2 specifically includes: S21, global map rasterization, the state is initialized to the unexplored state; S22, starting to receive visual perception data, i.e., the tennis ball detection result in the coordinate system of the ball-picking robot at the time of taking the photo; S23, obtaining odometer information of the tennis ball picking robot, and converting the coordinates of the tennis ball in the coordinate system of the tennis ball picking robot at the time of taking the photo into the coordinates of the tennis ball in the coordinate system of the tennis ball picking robot at the current time; S24, updating the map grid exploration status within the field of view at the time of taking the photo; S25, converting the coordinates of the tennis ball in the coordinate system of the ball-picking robot within the current field of view into the global coordinates of the map; S26, tracking and fusing the current detection result of the area within the field of view at the current moment with the historical detection results, and updating the spherical coordinates of the historical detection results in the map; S27, based on the update result of step S26, updating the ball cluster status in the map, wherein the ball cluster refers to an area containing multiple balls, and the diameter of the minimum circumscribed circle of all balls in the area is less than the ball cluster diameter threshold; S28, whether all grids of the global map have been explored, if so, reset the map state to the initialization state; S29, waiting to receive the next visual perception result, that is, repeating steps S22-S29.

4. The method according to claim 3, characterized in that The specific method of step S21 includes: The global map is evenly divided into m*n larger grids. The division of the larger grid must meet the following requirements: the robot rotates in a circle at the center of the larger grid, and its field of view can cover the entire larger grid; Then, each larger grid is evenly divided into k*k smaller grids. When the exploration status of all smaller grids within a larger grid is "explored", the exploration status of the larger grid is set to "explored"; State initialization will delete all recorded tennis ball coordinates in the global map and set the exploration status of all larger and smaller grids to unexplored.

5. The method according to claim 3, characterized in that The specific method of step S23 includes: After receiving the n tennis ball detection results in the ball picking robot coordinate system at the moment of the camera photo from the visual perception module, i.e., Balls_Robot{b_robot(0), b_robot(1), ..., b_robot(n)}, the odometry information of the tennis ball picking robot is immediately obtained. The odometry information includes the current position and pose matrix H_global of the tennis ball picking robot in the global map, as well as the relative motion matrix H_relative of the tennis ball picking robot from the moment the camera corresponding to Balls_Robot takes the photo to the current moment; where H_global and H_relative are both 3*3 rotation and translation matrices in the two-dimensional plane coordinate system; Then, through H_relative, the tennis ball coordinates Balls_Robot{b_robot(0),b_robot(1),……,b_robot(n)} in the ball-picking robot coordinate system at the moment of taking the photo are converted into the tennis ball coordinates Balls_Robot_current{b_robot_current(0),b_robot_current(1),……,b_robot_current(n)} in the ball-picking robot coordinate system at the current moment. Among them, the tennis ball coordinates b_robot(i) in the ball-picking robot coordinate system at the moment of taking the photo of the i-th tennis ball (i=0,1,2,……,n) are expressed as {x(i),y(i),z(i)}, and the corresponding tennis ball coordinates b_robot_current(i) in the ball-picking robot coordinate system at the current moment are expressed as {x_c(i),y_c(i),z_c(i)}. The specific conversion method is as follows:

6. The method according to claim 3, characterized in that The specific method of step S24 includes: Based on the current position of the tennis ball retrieval robot in the global map and its pose matrix H_global, combined with the camera's field of view, the robot traverses and calculates the smaller grids near the robot's location. The exploration status of the smaller grids that are completely covered by the current camera's field of view is set to "explored". When the exploration status of all smaller grids within a larger grid is "explored", the exploration status of the larger grid is set to "explored".

7. The method according to claim 3, characterized in that The specific method of step S26 includes: Extract the historical detection results of tennis balls in the area covered by the current field of view; In the global coordinate system, the historical detection results covered by the current field of view are matched with the n tennis ball detection results in the current field of view; For the matched tennis balls, update their historical detection results to the global coordinates at the current moment; All historical detection results that are covered by the field of view three times in a row and are not successfully matched are deleted from the historical detection results, and then all current moment detection results that are not successfully matched are added as new results to the historical detection results.

8. The method according to claim 4, characterized in that The step S3 specifically includes: S31, first determine whether there is a ball cluster or a ball in the map, if there is no ball cluster or ball: (1) Determine whether there are any large grids that have not been explored in the map. If not, no planning is performed. If so, the larger grid with the best efficiency weight is selected from the large grids that have not been explored and used as the tentative exploration target in the current state. (2) Determine whether the goal of the last path planning was to conduct exploration: If not, directly use the tentative exploration target in the current state as the exploration target; If so, compare the robot motion theoretical time required to reach the tentative exploration target in the current state + the exploration time threshold with the robot motion theoretical time required to reach the exploration target in the last path planning, and select the smaller one as the exploration target; (3) Then determine whether the robot's current position is at the center of the larger grid that is the exploration target: If so, define the path planning goal as the robot rotating in place for one circle, and send the path planning result to the robot motion planning module; If not, the path planning goal is to go directly to the exploration target and send the path planning result to the robot motion planning module; S32, when the first step of path planning determines that there is a ball cluster or a ball in the map: (1) According to the designed motion planning algorithm, the ball cluster or ball with the best time consumption is selected as the temporary ball picking target in the current state; Then, determine whether the goal of the last path planning was to pick up the ball: If not, directly use the tentative ball picking target in the current state as the ball picking target; If yes, then compare the robot motion theoretical time corresponding to the tentative ball picking target in the current state plus the ball picking time threshold with the robot motion theoretical time corresponding to the ball picking target in the last path planning, and select the smaller one as the ball picking target; (2) After determining the target for picking up the ball, further determine whether there are any large grids in the map that have not been explored: If it does not exist, the robot will go to the ball picking target to pick up the ball as the path planning goal, and send the path planning result to the robot motion planning module; If it exists, the one with the best efficiency weight is selected from the larger grid that has not been explored as the exploration target; (3) After completing the selection of the exploration target, determine whether the robot's current position is at the center of the larger grid that serves as the exploration target: If so, the path planning goal is defined as the robot rotating in place for one circle, and the path planning result is sent to the robot motion planning module; If not, compare the theoretical motion planning time from the robot's current position to the exploration target + the exploration threshold with the theoretical motion planning time from the robot's current position to the ball-picking target, select the task with the shorter time as the path planning target, and send the path planning result to the robot motion planning module.

9. The method according to claim 8, characterized in that In step S3, the method of selecting the larger grid with the best efficiency weight from the larger grids that have not been explored includes: Let the efficiency weight of the unexplored large grid be Efficiency(k), where k = 1, 2, ..., n_big_block, n_big_block is the total number of unexplored large grids; Efficiency(k) is calculated by denoting the distance from the robot's current position to the center of the unexplored larger grid as D_explore(k), and the number of unexplored smaller grids contained in the unexplored larger grid as N_block_explore(k). Then: In the current state, the larger grid with the best efficiency weight is the grid with the largest Efficiency value.

10. The method according to claim 8, characterized in that In the step S3, The exploration time threshold and the ball picking time threshold are both set to 1 second; The exploration threshold can be set to a value greater than or less than 0, depending on whether exploration or ball collection is prioritized.

Images