Sensing and planning method for automatic ball picking of tennis ball picking robot

Abstract

The invention discloses a sensing and planning method for automatic ball picking of a tennis ball picking robot. The sensing and planning method mainly comprises the four parts of visual sensing, map state updating, path planning and robot motion planning. According to the method, based on a camera carried by a ball picking robot, computer vision and artificial intelligence technologies are utilized to perceive, recognize and position tennis coordinates in a local visual field; according to the method, time-space fusion and task planning are carried out in combination with historical local view perception information, global scanning mapping in advance or sensors depending on global positioning are not needed, the ball picking efficiency is improved on the premise that more sensors are not used, and a ball picking area is not missed.

Description

Technical Field

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

[0002] The ball picking scheme in the prior art mainly includes:

[0003] Solution 1: A manually operated hand-pushed ball picking device or hand-pushed ball picking cart, which picks up balls from the tennis court through a hand-pushing mechanism;

[0004] Solution 2: Use the camera on the ball-picking robot to identify and locate the tennis ball in the local field of view, and pick up the ball after identifying it;

[0005] Solution 3: Use the camera on the ball-picking robot in conjunction with more sensors to globally locate the tennis ball and robot, or perform a global scan and map of the tennis court in advance, and then plan the ball-picking path based on global information.

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

[0007] Solution 1: Manual operation is time-consuming and labor-intensive, and its efficiency depends on the person's walking path. It is not automated.

[0008] Solution 2: Since the tennis court is large, 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, it cannot effectively explore the unknown area or can only conduct limited exploration by turning in circles on the spot. There are blind spots for picking up balls in the venue. Even if the ball-picking robot can see all the areas with balls, the ball-picking robot will have low efficiency in planning the path due to the limitations of its field of view.

[0009] Solution 3: More sensors are needed to globally locate the tennis ball and the robot, or a global scan and map of the tennis court is performed in advance, so as to plan the ball picking path based on global information; the technical route is more complicated, and the introduction of sensors leads to higher system costs. Summary of the invention

[0010] The present disclosure provides a perception and planning method for a tennis ball picking robot to automatically pick up balls. Based on a camera carried by the ball picking robot, computer vision and artificial intelligence technologies are used in the local field of view to perceive, identify and locate the coordinates of the tennis ball in the local field of view. There is no need for prior global scanning and mapping or reliance on global positioning sensors. Instead, historical local field of view perception information is combined for spatiotemporal fusion and planning, which improves the ball picking efficiency without using more sensors and ensures that the ball picking area is not missed.

[0011] The present disclosure provides a perception and planning method for a tennis ball picking robot to automatically pick up a ball, comprising the following steps:

[0012] S1, visual perception: a camera device installed on the tennis ball picking robot moves with the robot to take pictures of the scene, identify several tennis balls in the current field of view from the pictures, and then use the pre-calibrated camera intrinsic and extrinsic parameters to convert the identified tennis balls from the image coordinate system to the tennis ball picking robot coordinate system;

[0013] S2, map status update: convert the tennis ball coordinates in the ball picking robot coordinate system at the time of taking the photo into the tennis ball coordinates in the ball picking robot coordinate system at the current moment, and then convert them into the global coordinates of the map; based on the global coordinates of the map, 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 state; based on the current position and field of view of the robot, update the map exploration state;

[0014] S3, path planning: Based on the fused tennis ball detection results at the current moment and the current map exploration status, choose the less time-consuming task from exploring the unexplored area or picking up the ball as the path planning target;

[0015] S4, robot motion planning: according to the planning objectives, drive the robot to complete the tasks specified by the path planning.

[0016] Furthermore, the step S2 specifically includes:

[0017] S21, global map rasterization, the state is initialized to the unexplored state;

[0018] S22, starting to receive visual perception data, that is, the tennis ball detection result in the coordinate system of the ball picking robot at the time of taking the photo;

[0019] 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;

[0020] S24, updating the map grid exploration status within the field of view at the time of taking the photo;

[0021] 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;

[0022] S26, tracking and fusing the current detection result of the area within the field of view at the current moment with the historical detection result, and updating the spherical coordinates of the historical detection result in the map;

[0023] S27, based on the update result of step S26, updating the ball cluster state 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;

[0024] S28, whether all grids in the global map have been explored, if yes, reset the map state to the initialization state;

[0025] S29, waiting to receive the next visual perception result, that is, repeating steps S22-S29.

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

[0027] 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 one circle at the center of the larger grid, and the field of view can cover the entire larger grid.

[0028] Then, each larger grid is evenly divided into k*k smaller grids. When the exploration status of all smaller grids in a larger grid is "explored", the exploration status of the larger grid is set to "explored";

[0029] 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.

[0030] Furthermore, the specific method of step S24 includes:

[0031] According to the current position of the tennis ball picking robot in the global map and the pose matrix H_global, combined with the camera field of view, the smaller grids near the robot's position are traversed and calculated, and the exploration status of the smaller grids completely covered by the current camera field of view is set to "explored";

[0032] 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".

[0033] Furthermore, the specific method of step S26 includes:

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

[0035] 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;

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

[0037] All historical detection results that are covered by the field of view three times in a row and have not been successfully matched are deleted from the historical detection results, and then all current moment detection results that have not been successfully matched are added as new results to the historical detection results.

[0038] Furthermore, the step S3 specifically includes:

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

[0040] (1) Determine whether there is a large grid that has not been explored in the map. If not, no planning is performed; if so, select the large grid with the best efficiency weight from the large grids that have not been explored as the provisional exploration target in the current state;

[0041] (2) Determine whether the goal of the last path planning was to conduct exploration:

[0042] If not, directly use the tentative exploration target in the current state as the exploration target;

[0043] 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 of the last path planning, and select the smaller one as the exploration target;

[0044] (3) Then determine whether the robot's current position is at the center of the larger grid that is the exploration target:

[0045] 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;

[0046] If not, the path planning target is directly taken as the exploration target, and the path planning result is sent to the robot motion planning module;

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

[0048] (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;

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

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

[0051] 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;

[0052] (2) After determining the ball picking target, further determine whether there are large grids in the map that have not been explored:

[0053] If it does not exist, the path planning goal is to go to the ball picking target to pick up the ball, and the path planning result is sent to the robot motion planning module;

[0054] 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;

[0055] (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:

[0056] 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;

[0057] 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.

[0058] Furthermore, in step S3, the method of selecting the larger grid with the best efficiency weight from the larger grids that have not been explored completely includes:

[0059] Let the efficiency weight of the unexplored larger grid be Efficiency(k), where k = 1, 2, ..., n_big_block, n_big_block is the total number of unexplored larger grids;

[0060] The calculation method of Efficiency(k) includes: recording the distance from the current position of the robot to the center of the unexplored larger grid as D_explore(k), and recording the number of unexplored smaller grids contained in the unexplored larger grid as N_block_explore(k), then:

[0061]

[0062] In the current state, the larger grid with the best efficiency weight is the grid with the largest Efficiency value.

[0063] Furthermore, in step S3,

[0064] The exploration time threshold and the ball picking time threshold are both set to 1 second;

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

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

[0067] (1) Computer vision and artificial intelligence technologies are used to implement perception and planning algorithms, which are used by ball-picking robots to automatically pick up balls on the tennis court, thus avoiding the physical exhaustion of players due to manual ball picking, ensuring the players' concentration during training to the greatest extent and thus improving the training effect;

[0068] (2) Based on the camera carried by the ball-picking robot, computer vision and artificial intelligence technologies are used in the local field of view to perceive, identify and locate the coordinates of the tennis ball in the local field of view, and the historical local field of view perception information is combined for spatiotemporal fusion and planning. There is no need for global scanning and mapping in advance 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 switching between two targets with similar theoretical motion times due to micro-movement of the robot position during the two planning processes is largely avoided. When the theoretical times are similar, the robot is more inclined to continue with the last path planning target. At the same time, by setting the threshold, the robot can be controlled to prioritize picking up the ball or exploring.

[0070] (4) While the camera is taking pictures and performing perception calculations, the robot can maintain the original planned path until the perception calculation results are obtained to update the map status, and then perform a new round of path planning to achieve the goal of dynamically correcting the path planning at a high frame rate without the need for the robot to stop waiting for the picture taking and perception calculation results. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] The above and other objects, features and advantages of the present disclosure will become more apparent through a more detailed description of exemplary embodiments of the present disclosure in conjunction with the accompanying drawings, wherein like reference numerals generally represent like components throughout the exemplary embodiments of the present disclosure.

[0072] Figure 1 An 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 retrieval robot coordinate system are defined in the exemplary embodiment;

[0074] Figure 3 An exemplary embodiment of a map status update process;

[0075] Figure 4 It is a flow chart of the tracking and fusion method of the current detection results and the historical detection results;

[0076] Figure 5 An exemplary path planning flowchart is shown below. DETAILED DESCRIPTION

[0077] The preferred embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although the preferred embodiments of the present disclosure are shown in the accompanying 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. On the contrary, these embodiments are provided to make the present disclosure 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 a ball. According to an exemplary embodiment of the present disclosure, the overall control strategy of 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 (see attached figure). Figure 1 shown).

[0079] 1. Visual Perception

[0080] The visual perception module uses a camera installed on the tennis ball picking robot and moving with the robot to capture a scene image Image, and uses an artificial intelligence algorithm model (the Yolov5 model is used in this embodiment) to identify the n tennis balls {b(0), b(1), ..., b(n)} in the current field of view, where n ≥ 0 and is an integer. Then, using the pre-calibrated camera intrinsic parameters M and extrinsic parameters [R|T], the identified n tennis 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)}.

[0081] Among them, the image coordinate system coordinate b_image(i) of the i-th (i=0,1,2,...,n) tennis ball can be expressed as {u(i),v(i)}, and its corresponding tennis ball picking robot coordinate system coordinate b_robot(i) can be expressed as {x(i),y(i),z(i)}.

[0082] The image coordinate system and the tennis ball picking robot coordinate system are defined as follows: Figure 2 The origin of the image coordinate system is in the upper left corner of the image, and the width and height directions of the image are the x and y directions of the image coordinate system respectively; the coordinate system of the tennis ball picking robot 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 origin of the coordinate system, the positive direction of the y axis is perpendicular to the driving wheel axle, the positive direction of the z axis is vertically upward, and looking in the positive direction of the robot, the positive direction of the x axis is along the right side of the driving wheel axle.

[0083] The following describes a specific method for converting the n identified tennis 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)}.

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

[0085]

[0086] Among them, the unknown number z c The solution is as follows: Take the radius of the tennis ball as a fixed z(i), in this embodiment z(i) = 34 mm.

[0087] Transform the above formula as follows:

[0088]

[0089] make H 2 =R -1 ·T, can be obtained

[0090] From the equality of the left and right sides of the third line of the formula, we can get z c =(z(i)+H 2 [2]) / H 1 [2], where H 1 [2] and H 2 [2] respectively 1 and H 2 The third line of .

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

[0092]

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

[0094] 2. Map status update

[0095] In this embodiment, the process of the map status update module is as follows: Figure 3 shown.

[0096] (1) First, rasterize the global map and initialize the state.

[0097] Specifically, the global map is evenly divided into m*n large square grids, the side length of the large square grid is block_w, and this embodiment is described by taking block_w=5m as an example; then each large square grid is evenly divided into k*k small square grids, and this embodiment is described by taking k=10 as an example, 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 must meet the requirements-the robot rotates in place 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 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 status of all large square grids and small square grids to "unexplored".

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

[0100] (2) After the global map is rasterized and the state is initialized, data can be continuously received from the visual perception module. The data is the detection results of n tennis balls in the coordinate system of the ball picking robot at the moment the camera takes the photo, that is, Balls_Robot{b_robot(0),b_robot(1),…,b_robot(n)}, where n≥0 is an integer.

[0101] (3) After obtaining Balls_Robot, immediately obtain the odometer information of the tennis ball picking robot, which includes: the current position and pose matrix H_global of the tennis ball picking robot in the global map, and the relative motion matrix H_relative of the tennis ball picking robot from the moment when the camera corresponding to Balls_Robot takes a photo to the current moment. Among them, H_global and H_relative are both 3*3 rotation and translation matrices in a two-dimensional plane coordinate system.

[0102] Then, H_relative is used to convert the tennis coordinates Balls_Robot{b_robot(0), b_robot(1), ..., b_robot(n)} in the ball picking robot coordinate system at the time of taking the photo into the tennis coordinates Balls_Robot_current{b_robot_current(0), b_robot_current(1), ..., b_robot_current(n)} in the current time of taking the photo. Among them, the tennis coordinates b_robot(i) in the ball picking robot coordinate system at the time of taking the photo of the i-th (i = 0, 1, 2, ..., n) tennis ball can be expressed as {x(i), y(i), z(i)}, and its corresponding tennis coordinates b_robot_current(i) in the current time of taking the ball picking robot coordinate system can be expressed 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 odometer information of the ball picking robot, such as wheel odometer, IMU odometer, visual odometer, odometer based on lidar, odometer based on the fusion of different sensors (IMU, vision, lidar), etc.

[0105] (4) Next, update the exploration status of the map grid within the field of view at the time of taking the photo. The specific method is:

[0106] According to the current position of the tennis ball picking robot in the global map and the pose matrix H_global, combined with the camera field of view, the small square grids near the robot's position are traversed and calculated, and the exploration status of the small square grids completely covered by the current camera field of view is set to "explored".

[0107] When the exploration status of all small square grids within a large square grid is "explored", the exploration status of the large square grid is set to "explored".

[0108] (5) Next, convert the coordinates of the tennis ball in the current field of view of the ball-picking robot coordinate system Balls_Robot_current {b_robot_current(0), b_robot_current(1), ..., b_robot_current(n)} into the global coordinates of the map Balls_Global {b_global(0), b_global(1), ..., b_global(n)}. Among them, the coordinates of the tennis ball b_robot_current(i) in the current field of view of the ball-picking robot coordinate system of the i-th tennis ball (i = 0, 1, 2, ..., n) can be expressed as {x_c(i), y_c(i), z_c(i)}, and its corresponding global coordinates of the map b_global(i) can be expressed as {x_g(i), y_g(i), z_g(i)}. The specific conversion method is as follows:

[0109]

[0110] (6) Next, the global coordinates of the map Balls_Global of the current detection result of the area within the field of view at the current moment are tracked and fused with the historical detection results. For specific methods, refer to Figure 4 .

[0111] First, find the historical detection results covered by the current field of view, recorded as Balls_Globa_Old{b_global_old(0),b_global_old(1),…,b_global_old(n_old)}, where 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 it;

[0113] For each traversed result b_global_old(j), after traversing the n results in Balls_Global, select b_global(i_dmin) that is closest to b_global_old(j), and determine whether the distance D(i_dmin,j) is less than the threshold D_thresh. If it is 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] Successfully matched results will no longer participate in the matching in subsequent traversals; after traversing n_old results in Balls_Globa_Old, all historical detection results that have been covered by the field of view for three consecutive times and have not been successfully matched will be deleted from the historical detection results, and then all current-time detection results that have not been successfully matched will be added as new results to the historical detection results.

[0115] Where b_global(i) can be expressed as {x_g(i), y_g(i), z_g(i)}, b_global_old(i) can be expressed 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] Among them, the selection method of the closest b_global(i_dmin) is as follows:

[0118]

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

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

[0121] By calculating the spherical coordinates of the historical detection results in the updated map in the previous step, the states of all spherical clusters in the map are updated. This embodiment is described by taking Cluster_thresh=1m as an example.

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

[0123] Otherwise, directly wait to receive the next detection result from the perception module.

[0124] 3. Path Planning

[0125] The process of the path planning module is as follows Figure 5 shown.

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

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

[0128] 2) If there are large square grids that have not been explored yet, first calculate the efficiency weights Efficiency(k) when selecting different large square grids that have not been explored yet, k = 1, 2, ..., n_big_block, n_big_block is the total number of large square grids that have not been explored yet, and then select the large square grid that has not been explored yet with the best efficiency weight as the exploration target Target_explore.

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

[0130]

[0131] The method for selecting the exploration target Target_explore is as follows:

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

[0133]

[0134] If the goal of the last path planning is not to explore, then the exploration target Target_explore_current with the best efficiency weight in the current state is directly used as the exploration target Target_explore, that is, the exploration target 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 T(Target_explore_current) of the robot movement corresponding to the exploration goal with the best efficiency weight in the current state plus the exploration time threshold thresh_time_explore with the theoretical time T(Target_explore_last) of the robot movement corresponding 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 to a large extent the situation where the path planning target switches back and forth between the two targets with similar theoretical robot motion times due to slight movement of the robot position during the two planning processes when the theoretical robot motion times of the two targets are similar. When the theoretical times are similar, it is more inclined to continue the target of the previous path planning.

[0139] 3) Then determine whether the robot's current position is at the center of the large orthogonal grid that has not been explored:

[0140] 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;

[0141] If the robot's current position is not at the center of the large orthogonal grid that has not been explored, since there is no ball cluster or ball, there is no need to judge whether picking up the ball is better or exploring is better. The path planning goal is directly set to explore the exploration target, and the path planning result is sent to the robot motion planning module.

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

[0143] (2) When the first step of path planning determines that there are ball clusters or balls in the map:

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

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

[0146] If the goal of the last path planning is not to pick up the ball, then the current ball picking target Target_ball_current is directly used as the ball picking target Target_ball, that is, the ball picking target Target_ball = Target_ball_current;

[0147] Otherwise, if the goal of the last path planning is to pick up the ball, assuming that the goal of the last path planning is Target_ball_last, compare the theoretical time T(Target_ball_current) of the robot motion corresponding to the current state of the ball picking target Target_ball_current plus the ball picking time threshold thresh_time_ball with the theoretical time T(Target_ball_last) of the robot motion corresponding to the last path planning of the ball picking target Target_ball_last, and select the smaller one as the ball picking target Target_ball, that is:

[0148]

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

[0150] The purpose of setting the ball picking time threshold is to avoid to a large extent that when the theoretical robot motion time of two targets is similar, the robot position moves slightly during the two planning processes, causing the path planning target to switch back and forth between two targets with similar theoretical motion time. When the theoretical time is similar, it is more inclined to continue the target of the previous path planning.

[0151] 2) After determining the ball picking target, further determine whether there are any large square grids in the map that have not been explored (i.e., large square grids with an exploration status of "unexplored"):

[0152] If there is no large square grid that has not been explored, then going to the ball-picking target to pick up the ball is taken as the path planning target, and the path planning result is sent to the robot motion planning module (in this embodiment, if the ball-picking target is a ball cluster, then going to the ball-picking target is defined as going to the position of the center of the minimum circumscribed circle of all the balls in the ball cluster);

[0153] If there is a large square grid that has not been explored, then first calculate the efficiency weight Efficiency(k) when selecting different large square grids that have not been explored, where k = 1, 2, ……, n_big_block, and n_big_block is the total number of large square grids that have not been explored. Then select the large square grid with the optimal efficiency weight among the large square grids that have not been explored as the exploration target Target_explore (when there is a large square grid that has not been explored, the calculation methods of the variables shown are the same as those described above);

[0154] 3) After selecting the exploration target Target_explore, determine whether the current position of the robot is at the center of the large square grid that has not been explored:

[0155] If so, define the target of path planning as the robot rotating in place for one week, and send the path planning result to the robot motion planning module;

[0156] If the current position of the robot is not at the center of the large square grid that has not been explored, then determine 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, then going to the ball-picking target to pick up the ball is taken as the path planning target, and the path planning result is sent to the robot motion planning module; otherwise, if T_target_explore + Thresh_explore < T_target_ball, then going to the exploration target for exploration is taken as the path planning target, and the path planning result is sent to the robot motion planning module.

[0157] Among them, 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] IV. Robot Motion Planning

[0159] The robot motion planning module receives the planning target from the path planning module and drives the robot's active wheels to complete the task specified by the path planning. In this process, sensors are used in real time to detect obstacles, such as ultrasonic radar. When obstacles are encountered on the path to the specified target, the path planning target is maintained while avoiding the obstacles. There are many ways to avoid obstacles, such as using other sensors or multiple sensor fusion methods, including lidar, 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 until the perception calculation results are obtained to update the map status, and then perform a new round of path planning to achieve the goal of dynamically correcting the path planning at a high frame rate, without the need for the robot to stop waiting for the picture taking and perception calculation results.

[0161] The above technical scheme is only an exemplary embodiment of the present invention. For those skilled in the art, it is easy to make various types of improvements or modifications based on the application methods and principles disclosed in the present invention, and it is not limited to the method described in the above specific embodiment of the present invention. Therefore, the method described above is only preferred and does not have a restrictive 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 device installed on the tennis ball picking robot moves with the robot to take pictures of the scene, identify several tennis balls in the current field of view from the pictures, and then use the pre-calibrated camera intrinsic and extrinsic parameters to convert the identified tennis balls from the image coordinate system to the tennis ball picking robot coordinate system; S2, map status update: convert the coordinates of the tennis ball in the ball picking robot coordinate system at the time of taking the photo into the coordinates of the tennis ball in the current time of the ball picking robot coordinate system, and then convert them into the global coordinates of the map; based on the global coordinates of the map, 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 state; Update the map exploration status based on the robot’s current position and field of view; S3, path planning: Based on the fused tennis ball detection results at the current moment and the current map exploration status, choose the less time-consuming task from exploring the unexplored area or picking up the ball as the path planning target; S4, robot motion planning: according to the planning objectives, 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 coordinate system coordinates b_image(i) of the i-th (i = 0, 1, 2, ..., n) tennis ball are {u(i), v(i)}, and the corresponding tennis ball picking robot coordinate system coordinates b_robot(i) are {x(i), y(i), z(i)}; 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 number 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, can be obtained From the equality of the left and right sides of the third line 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, that is, 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 result, and updating the spherical coordinates of the historical detection result in the map; S27, based on the update result of step S26, updating the ball cluster state 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 in the global map have been explored, if yes, 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 one circle at the center of the larger grid, and the 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 in 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 n tennis ball detection results in the ball picking robot coordinate system at the time of camera shooting from the visual perception module, i.e., Balls_Robot{b_robot(0), b_robot(1), ..., b_robot(n)}, the odometer information of the tennis ball picking robot is immediately obtained. The odometer information includes the current position and pose matrix H_global of the tennis ball picking robot in the global map, and the relative motion matrix H_relative of the tennis ball picking robot from the camera shooting moment corresponding to Balls_Robot to the current moment; wherein 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 coordinates Balls_Robot{b_robot(0),b_robot(1),...,b_robot(n)} in the ball picking robot coordinate system at the time of taking the photo are converted into the tennis 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, where the tennis coordinates b_robot(i) in the ball picking robot coordinate system at the time 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 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: According to the current position of the tennis ball picking robot in the global map and the pose matrix H_global, combined with the camera field of view, the smaller grids near the robot's position are traversed and calculated, and the exploration status of the smaller grids completely covered by the current camera 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 have not been successfully matched are deleted from the historical detection results, and then all current moment detection results that have not been 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 is a large grid that has not been explored in the map. If not, no planning is performed; if so, select the large grid with the best efficiency weight from the large grids that have not been explored as the provisional 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 of 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 target is directly taken as the exploration target, and the path planning result is sent 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 ball picking target, further determine whether there are large grids in the map that have not been explored: If it does not exist, the path planning goal is to go to the ball picking target to pick up the ball, and the path planning result is sent 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 larger grid be Efficiency(k), where k = 1, 2, ..., n_big_block, n_big_block is the total number of unexplored larger grids; The calculation method of Efficiency(k) includes: recording the distance from the current position of the robot to the center of the unexplored larger grid as D_explore(k), and recording 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 greater than 0 or less than 0, depending on whether exploration or ball collection is prioritized.

Images