Method for cooperatively and actively chasing target by multiple spherical robots

Abstract

The invention discloses a multi-spherical robot cooperative active target chasing method, which comprises the following steps of: firstly, constructing a multi-spherical robot cooperative system comprising a square flexible net and four single spherical robots respectively pulling one corner of the flexible net, and defining the single spherical robots as accompanying robot; a forerunner robot is virtualized in the center of the flexible net, repulsive force exists between the accompanying robot, and the accompanying robot is subjected to traction force of the forerunner robot; constructing a chasing task including tracking, capturing and terminating; constructing a gravitational potential field and a repulsive potential field among the antecedent robot, the accompanying robot, the target point and the obstacle; the system executes a chasing task, the task type is firstly judged according to the distance between the precedent robot and the target point, if the task is a tracking or capturing task, the positions and the speeds of the precedent robot and the accompanying robot are subjected to updating control, and task type judgment is carried out again until the task is a termination task. According to the invention, active pursuit of a moving target point can be realized in a complex environment.

Description

Technical Field

[0001] The present invention relates to a robot target pursuit technology, in particular to a method for multiple spherical robots to collaboratively and actively pursue a target. Background Art

[0002] Unmanned aerial vehicles (UAVs) play an important role in numerous fields, including aerial photography, disaster relief, remote sensing mapping, environmental monitoring, geological exploration, and agriculture. With technological advancements, the coordinated control of multiple UAVs has become a hot topic of research. Due to their adaptability to complex environments, high mission flexibility, and powerful ability to execute complex tasks, they offer broad application prospects.

[0003] However, the phenomenon of illegal drone operations is becoming increasingly serious. Net-and-bullet interception is widely used as a common countermeasure. This technology fires a net-and-bullet at a target. Upon approaching the target, the net is released, and the net's elasticity expands, contracts, and wraps around the target, capturing it. This passive capture technology offers advantages such as low cost and suitability for slow-moving targets.

[0004] Despite this, net-and-bullet interception technology also has significant shortcomings. Because the net-and-bullet's posture is fixed after launch, it cannot adjust in real time to the target's maneuvers and environmental changes. This results in poor adaptability when performing missions in complex terrain, making efficient capture difficult. Therefore, to address these limitations, further research and optimization of drone countermeasures are needed to achieve active target capture. Summary of the Invention

[0005] Purpose of the invention: The purpose of the present invention is to provide a method for multiple spherical robots to collaborate and actively pursue targets, so as to realize the active pursuit of moving target points in complex environments.

[0006] Technical Solution: To achieve the above objectives, the present invention provides a method for multiple spherical robots to collaboratively and actively pursue a target, comprising the following steps:

[0007] S1. Construct a multi-spherical robot collaborative system, including a square flexible net and four single spherical robots, each holding a corner of the flexible net, to form a square formation;

[0008] S2. Formation control of the multi-spherical robot collaborative system is performed based on the pilot-formation method. A single spherical robot is defined as a companion robot. A virtual forerunner robot is created at the center of the flexible net, and the companion robot follows the trajectory and speed of the forerunner robot.

[0009] S3. Constructing the pursuit task of the multi-spherical robot collaborative system, including tracking task, capture task, and termination task;

[0010] S4. The multi-spherical robot collaborative system performs the pursuit mission and finds an optimal collision-free path to successfully capture the target point, including:

[0011] S401: including periodically collecting the position and speed information of the forerunner robot and the companion robot, the target point and the obstacle position information, and determining the task type based on the distance between the forerunner robot and the target point after the kth round of sampling;

[0012] S402: If the task is a tracking or capturing task, the attraction and repulsion of the target point and obstacles on the forerunner robot or the companion robot are calculated based on the artificial potential field method. At the same time, a regular plane model of the environment around the target point is constructed to calculate the distance between the forerunner robot and the companion robot and a specific plane around the target point.

[0013] S403: Based on the gravitational force and repulsive force and the distance to the specific plane around the target point, the position and speed of the forerunner robot and the companion robot are controlled, and the next round of information collection and task judgment is carried out until the task type is terminated.

[0014] The multi-spherical robot collaborative system described in S2 performs formation control, including:

[0015] (1) Define the orientation of the formation plane as the normal direction of the plane, and the angle between the normal direction and the velocity direction of the forerunner robot is an acute angle; define the coordinate system WP as the world coordinate system of the multi-spherical robot collaborative system, define the coordinate system P as the body coordinate system of the forerunner robot, and define the orientation of the formation plane as the z-axis direction in the forerunner robot body coordinate system upward. In the initial stage, the coordinate axis of the coordinate system P is parallel to the coordinate axis of the coordinate system WP;

[0016] (2) The position coordinates of the forerunner robot and the companion robot defined in the coordinate system WP are and i=0,1,2,3 are the numbers of the companion robots. In the initial stage,

[0017] (3) Define the expected coordinate matrix of the companion robot and the forerunner robot relative to the forerunner robot in the coordinate system WP as WPM, represents the expected coordinate matrix of the companion robot i relative to the forerunner robot in the coordinate system WP, A virtual coordinate matrix defined on the coordinate system P is PM, PM=(p0'p1'p2'p3'p'), p i',i=0,1,2,3 represents the expected coordinate matrix of companion robot i in coordinate system P, p' represents the expected coordinate matrix of the forerunner robot in coordinate system P, In the initial stage, WPM=PM;

[0018] (4) Define that there is a repulsive force between the companion robots, and the companion robot is subjected to the traction force of the forerunner robot. The traction force is: Among them, γ is the coefficient of controlling traction, w il A variable representing the difference between the relative distance between companion robot i and the predecessor robot and the expected relative distance in the world coordinate system WP, Where C is a constant, Δx il =(xx i )-(w l,x -w i,x ),Δy il =(yy i )-(w l,y -w i,y ),Δz il =(zz i )-(w l,z -w i,z ).

[0019] Among them, the pursuit task set S={A,B,C} described in S3 defines the task currently executed by the forerunner robot as s, s∈S, A represents the pursuit task. When the forerunner robot receives the task, it will notify the four companion robots to jointly execute the task of tracking the target point; B represents the capture task. When the forerunner robot receives the task, it will select a capture strategy and notify the four companion robots to capture the target; C represents the termination task, which terminates the movement of the forerunner robot and the companion robots;

[0020] Two distance thresholds L and M are defined, where L>>M, and M is the safety distance for the multi-spherical robot collaborative system to ensure its own safety; when the distance between the forerunner robot and the target point is greater than L, the multi-spherical robot collaborative system performs the tracking task; when the distance between the forerunner robot and the target point is greater than M and less than L, the multi-spherical robot collaborative system performs the capturing task; when the distance between the forerunner robot and the target point is less than M, the task ends, and the movement of the forerunner robot and the companion robot is terminated.

[0021] Among them, in the pursuit mission, the multi-spherical robot cooperation system uses the binocular vision method to obtain the distances between the pioneer robot, the target point, and the obstacles. First, it acquires two left and right images of the same scene through the cameras arranged on the left and right sides of the pioneer's forward direction. Then, it calculates the disparity value based on the corresponding pixel pairs in the left and right images to obtain a disparity map. After that, it performs back-projection on the disparity map to reconstruct the coordinates of each pixel point in the three-dimensional space of the scene, and can generate dense or sparse point cloud data. Finally, it calculates the distances between the pioneer robot, the target point, and the obstacles based on the coordinates.

[0022] Among them, the periodic acquisition of the position and speed information of the pioneer robot and the follower robots, as well as the position information of the target point and the obstacles in S401, includes the position coordinates p ik and the speed v ik of the follower robot i, the Euclidean distance d ik from the follower robot i to the pioneer robot, the Euclidean distance d ijk from the follower robot i to the follower robot j, and the position coordinates goal k to the target point, the position coordinates p k and v lk of the pioneer robot, the Euclidean distance d k from the pioneer robot to the target point. Here, d k =|p k -goal k |, the distance d' k between the pioneer robot and the obstacle. Among them, v lk represents the velocity matrix of the pioneer robot in the x-y-z three directions in the k-th round, and v ik represents the velocity matrix of the follower robot i in the x-y-z three directions in the k-th round;

[0023] The judgment principle of the task type is as follows: If d k <M, it is a termination task. If d k ≥L, it is a tracking task. If M≤d k <L, it is a capture task. L and M are distance thresholds, L>>M, and M is the safety distance for the multi-spherical robot cooperation system to ensure its own safety.

[0024] Among them, the control method for the position and speed of the pioneer robot and the follower robots in S403 is as follows:

[0025] (1) Regarding position control: The position control of the pioneer robot is p k+1 =p k +v lk+1 *dt, and the position control of the follower robot is p ik+1 =p ik +vik+1 *dt, where dt is the time step constant;

[0026] (2) About speed control: The speed control of the Forerunner robot is v lk+1 =v lk +G k +F ok According to the definition of artificial potential field method, the forerunner robot is subject to the gravitational force G of the target point. k =F a (p k ,goal k ,2), the pioneer robot is subject to the repulsive force of surrounding obstacles z is the amount of repulsive force, where d' k is the distance between the forerunner robot and the obstacle; when the obstacle is a plane extracted from the target point's surrounding environment features based on the target point's surrounding environment regular plane model, d' k =d wtck , d wtck represents the distance from the forerunner robot to the extracted plane in round k of acquisition, at which point z = n;

[0027] The speed of companion robot i is controlled to be v ik+1 =v ik +F pik +F iok According to the definition of the leader-follower method, the traction force F of the forerunner robot on the companion robot i is pik =F pi (p k ,p ik ,WPM k ), according to the definition of artificial potential field method, the companion robot i is subject to the repulsive force of the surrounding obstacles z is the amount of repulsive force, where d' k is the distance between the forerunner robot and the obstacle, and the companion robot i will also regard other companion robots as obstacles; when the obstacle is a plane extracted from the target point's surrounding environment features based on the target point's surrounding environment regular plane model, d' k =d witck , at this time z=n; WPM k is the expected coordinate matrix of the companion robot and the forerunner robot relative to the forerunner robot in the coordinate system WP.

[0028] The method for calculating the gravitational force and repulsive force exerted on the forerunner robot or the companion robot by the target point and the obstacle is as follows: establishing the gravitational potential field and the repulsive potential field between the forerunner robot or the companion robot and the target point and the obstacle based on the artificial potential field method, and then further calculating the gravitational force and repulsive force exerted on the robot;

[0029] The gravitational potential field between the robot and the target point is: U a (p,goal,m)=a o |p-goal| m , where p and goal represent the positions of the robot and the target point at a certain moment, respectively, |p-goal| represents the straight-line distance between the robot and the target point at a certain moment, and a o and m are adjustable parameters,

[0030] The gravitational force on the robot is:

[0031] The repulsive potential field between the robot and the obstacle is: Among them, k o is an adjustable parameter in the repulsive potential field, d' is the Euclidean distance between the robot and the obstacle, d0 is the maximum range of the repulsive force exerted by the obstacle on the robot,

[0032] The repulsive force on the robot is:

[0033] The method for calculating the distances between the forerunner robot and the companion robot and a specific plane around the target point by constructing a regular plane model of the target point's surrounding environment is as follows: first, the environment around the target point is identified and divided into multiple planes, the distances between the target point and the multiple planes are calculated respectively, a specific plane whose distance is less than a constant ρ is extracted, and then the distances between the forerunner robot and the companion robot and the specific plane are calculated, which are expressed as d wtc d witc , c = 1, 2, 3…n, n is the number of specific planes.

[0034] Among them, the expected coordinate matrix WPM k The update method is: WPM k =Rotate(t k ,t gk ,WPM k-1 ), t k is the orientation vector of the formation plane, and the orientation vector of the formation plane after rotation is t gk ;

[0035] If d k ≥L, the companion robot performs the tracking task, and the formation plane does not change its direction, then t gk =tk-1 , t k No update occurs, so WPM k = WPM k-1 ;

[0036] If M ≤ d k <L, the accompanying robot executes the capture mission, and the following four capture strategies adapted to the environment are defined to update t gk :

[0037] If the leading robot identifies that there is no specific plane adjacent to the target point, a vertical capture strategy from top to bottom or from bottom to top is selected according to the relative position of the leading robot and the target point. If the target point is above the leading robot, then t gk is updated to (0 0 1), and if the target point is below the leading robot, then t gk is updated to (0 0 -1);

[0038] If the leading robot identifies that there is a specific plane adjacent to the target point, then t gk is updated to the direction parallel to the normal of the plane and close to the plane;

[0039] If the leading robot identifies that there are two specific planes adjacent to the target point, find a point on the intersection line of the two planes that is connected to the target point and perpendicular to the intersection line, denote this point as Point, and update t gk to the direction from the target point to Point;

[0040] If the leading robot identifies that there are three or more specific planes adjacent to the target point, if these planes have a common intersection point O, then t gk is updated to the direction from the target point to O. If these planes do not have a common intersection point, then t gk is updated to the direction perpendicular to the intersection line of the plane closest to the target point and passing through the target point.

[0041] Among them, the method of rotating the said t k to t gk is: Define an interpolation method to gradually rotate t k to t gk , set a counter c x , with an initial value of 1, calculate the angle α between t gk and t k . In the rounds when t gk does not change, α remains unchanged. When the loop round increases, c x = c x + 1; When the scene changes or the target point moves, if t gk changes, then recalculate t gkWith t k The angle α between them is c x Set to 1;

[0042] Assume t k For uniform rotation, the angular velocity is ω (ω<α / 2), then

[0043]

[0044] Beneficial effects: The present invention has the following advantages: The present invention provides a method for multiple spherical robots to collaborate and actively pursue targets. Compared with conventional multi-robot collaborative pursuit methods, the present invention can perceive the complex environment around the moving target point and autonomously select a suitable capture strategy to capture the target point based on environmental information. This method can improve the flexibility and autonomy of capturing target points in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 Schematic diagram of the process of this method;

[0046] Figure 2 This is the structural diagram of the multi-spherical robot collaborative system;

[0047] Figure 3 Carry out pursuit mission process for multi-spherical robot collaborative system. DETAILED DESCRIPTION

[0048] The technical solution of the present invention is described in detail below with reference to the embodiments and drawings.

[0049] like Figures 1 to 3 As shown, the method for multi-spherical robots to actively pursue a target in collaboration with each other in the present invention includes the following contents:

[0050] 1. Build a multi-spherical robot collaborative system, including four single spherical robots and a square flexible net. The four single spherical robots each hold a corner of the flexible net to form a square formation.

[0051] 2. Formation control of multi-spherical robot collaborative system.

[0052] The present invention utilizes a pilot-formation method to perform formation control on a multi-spherical robot collaborative system. The basic idea of the pilot-follower method is that all formation members are designated as either pilots or followers. The pilot controls the movement trend of the entire formation by navigating along a predetermined or temporarily set path, and the followers follow the pilot based on the distance and orientation information relative to the pilot to achieve formation control. The algorithm establishes a virtual follower around the pilot based on the expected target formation, constructs the positional relationship between the virtual follower and the pilot and the positional relationship between the actual follower and the pilot in the world coordinate system, calculates the relative coordinate difference matrix A between the virtual follower and the actual follower, and then transforms the matrix A in the world coordinate system into the coordinate difference matrix A in the actual follower coordinate system through the coordinate conversion matrix. f At this point, the formation control problem is transformed into the problem of the actual follower tracking the trajectory of the virtual follower. Then, the controller of the follower is designed so that the actual follower gradually reaches the motion state of the virtual follower. The specific process is as follows:

[0053] (1) Define the system formation structure: define four single spherical robots as four companion robots, and create a virtual forerunner robot at the center of the flexible net. There is a repulsive force between the companion robots, and the companion robot is subject to the traction force of the forerunner robot.

[0054] (2) Define the system coordinate system: define coordinate system WP as the world coordinate system of the multi-spherical robot collaborative system; define coordinate system P as the body coordinate system of the forerunner robot, which conforms to the right-hand rule, with the origin at the center of gravity of the forerunner robot, the x-axis pointing in the forward direction of the forerunner robot, the y-axis pointing from the origin to the right side of the forerunner robot, and the z-axis direction determined by the right-hand rule through x and y; define the orientation of the formation plane formed by the four companion robots as the normal direction of the plane, and the angle between the normal direction and the direction of the z-axis in the forerunner robot body coordinate system is an acute angle. In the initial stage of the multi-spherical robot collaborative system, the coordinate axes of coordinate system P are parallel to the coordinate axes of coordinate system WP. In this embodiment, when the coordinate system of the physical quantity is not specifically indicated, it is the coordinate system WP.

[0055] The position coordinates of the forerunner robot and the companion robot defined in the coordinate system WP are: and When the system is in the initial stage, since the forerunner robot is at the center of the flexible net, Define the expected coordinate matrix of the companion robot and the forerunner robot relative to the forerunner robot on the coordinate system WP as WPM, represents the expected coordinate matrix of the companion robot i relative to the forerunner robot in the coordinate system WP, A virtual coordinate matrix defined on the coordinate system P is PM, PM = (p0'p1'p2'p3'p'). This matrix is determined at the beginning of the system. i '(i=0,1,2,3) represents the expected coordinate matrix of companion robot i on coordinate system P, p' represents the expected coordinate matrix of the forerunner robot on coordinate system P, Since the coordinate axes of the coordinate system P are parallel to the coordinate axes of the coordinate system WP in the initial stage of the system, WPM=PM in the initial stage.

[0056] (3) Define the traction force within the system: The traction force exerted on the companion robot i by the forerunner robot is defined as Among them, γ is the coefficient of controlling traction, w il A variable representing the difference between the relative distance between companion robot i and the predecessor robot and the expected relative distance in the world coordinate system WP, Where C is a constant, Δx il =(xx i )-(w l,x -w i,x ),Δy il =(yy i )-(w l,y -w i,y ),Δz il =(zz i )-(w l,z -w i,z ).

[0057] (4) Define the update method of the system's expected coordinate matrix WPM: define the orientation vector of the formation plane as t, and the orientation vector after rotation as t g , after rotation, WPM is updated, and its update function is: WPM * =Rotate(t,t g ,WPM)=R*WPM, where WPM * is the updated expectation matrix, R is calculated according to the Rodriguez formula to rotate the direction of the formation plane from t to t g The required rotation matrix, Where θ is the value of t and t g axis is the unit normal vector perpendicular to these two vectors.

[0058] 3. Establish the pursuit mission of the multi-spherical robot collaborative system, including tracking mission, capture mission and termination mission.

[0059] Define a set of tasks S, S = {A, B, C}, and define the task currently being performed by the Forerunner robot as s, s∈S. A represents a tracking task. Upon receiving this task, the Forerunner robot notifies the four companion robots to jointly perform the task of tracking the target point. B represents a capture task. Upon receiving this task, the Forerunner robot selects a capture strategy and notifies the four companion robots to capture the target. C represents a termination task, which terminates the movement of the Forerunner and companion robots.

[0060] Define two distance thresholds L and M, where L>>M and M is the safe distance for the multi-spherical robot collaborative system to ensure its own safety;

[0061] When the distance between the forerunner robot and the target point is greater than L, the multi-spherical robot collaborative system performs the tracking task; when the distance between the forerunner robot and the target point is greater than M and less than L, the multi-spherical robot collaborative system performs the capture task; when the distance between the forerunner robot and the target point is less than M, the task ends and the movement of the forerunner robot and the companion robot is terminated.

[0062] In the pursuit mission, the multi-spherical robot collaborative system uses binocular vision to obtain the distance between the forerunner robot and the target point and obstacles.

[0063] Binocular vision is a computer vision technology that uses dual cameras to obtain scene depth information. It simulates the way humans perceive the three-dimensional world with both eyes. Specifically, it obtains images of the same scene from two cameras, one on the left and one on the right, with a known baseline length (i.e., the relative position and posture between the two cameras). Using an image matching algorithm, it finds corresponding pairs of pixels in the left and right images to calculate the disparity value. Disparity refers to the difference in pixel position between the left and right images for the same three-dimensional point, and the disparity value is inversely proportional to the depth of the point from the camera. By back-projecting the disparity map, the coordinates of each pixel in the scene in three-dimensional space can be reconstructed, enabling the generation of dense or sparse point cloud data.

[0064] The binocular vision algorithm of this invention takes two calibrated and aligned left and right view images as input and outputs depth information or 3D coordinates corresponding to each pixel. This method achieves high-resolution and long-range depth estimation without the need for active light sources, and has wide applications in robotic navigation, 3D reconstruction, autonomous driving, and other fields.

[0065] The specific application of the binocular vision method in a multi-spherical robot collaborative system is as follows: In coordinate system P, a counterclockwise loop is defined, starting with the companion robot at the upper right corner of the Forerunner robot's direction of travel, and labeling the companion robots with numbers i = 0, 1, 2, and 3. Each companion robot is equipped with a depth camera to determine the distance between the companion robot and any object. Using the depth cameras on companion robots 0 and 1, the three-dimensional coordinates of any object can be obtained using binocular vision. Because the Forerunner robot is fictitious and only its position coordinates are known, the distances to other objects cannot be directly determined. Therefore, binocular vision is used to obtain the coordinates of the objects, and then the Euclidean distance between the Forerunner robot and the object is calculated, thereby obtaining the distance information between the Forerunner robot and all objects during the pursuit process.

[0066] 4. Based on the artificial potential field method, a gravitational potential field is constructed between the forerunner robot and the target point, as well as a repulsive potential field is constructed between the forerunner robot, companion robot and obstacles, to ensure that the multi-spherical robot collaborative system can find an optimal collision-free path in the pursuit mission.

[0067] The functions of the artificial potential field method include the gravitational function and the repulsive function. It is defined that there is a gravitational potential field between the robot and the target point, and its expression is: U a (p,goal,m)=a o |p-goal| m , where p and goal represent the positions of the robot and the target point at a certain moment, respectively, |p-goal| represents the straight-line distance between the robot and the target point at a certain moment, and a o and m are adjustable parameters. Accordingly, the negative gradient of the gravitational potential field function is used to represent the gravitational force on the robot:

[0068] A repulsive potential field is defined between the obstacle and the robot. The closer the distance between the two, the stronger the repulsion. The principle of this potential field is similar to that of the electric potential field. The repulsive potential field is defined as:

[0069]

[0070] Among them, k o is an adjustable parameter in the repulsive potential field, d' is the Euclidean distance between the robot and the obstacle, d0 is the maximum range of the obstacle's repulsive force on the robot, and is also the safe distance at which the robot will not have a collision risk. The repulsive force on the robot is:

[0071] Based on the artificial potential field method, the net force on the robot is: F = F a (p,goal,m)+F o(d'), by constructing the above-mentioned gravitational potential field and repulsive potential field to control the movement of the forerunner robot and the companion robot, it can ensure that the multi-spherical robot collaborative system can effectively avoid obstacles and find an optimal pursuit path without collision.

[0072] 5. Construct a regular plane model of the environment around the target point. The plane model is easier to perform mathematical calculations to realize the interaction ability of the multi-spherical robot collaborative system with the environment and the function of autonomously selecting the capture strategy. Specifically, it includes identifying the environment around the target point and dividing it into multiple planes, calculating the distance between the target point and the surrounding planes, extracting specific planes with a distance less than a constant ρ, and further calculating the distances of the forerunner robot and the companion robot to the specific planes, which are d wtc d witc , c = 1, 2, 3…n, n is the number of specific planes.

[0073] The present invention further utilizes the principal component analysis method (PCA) to process point cloud data. PCA is a classic linear dimensionality reduction and feature extraction method used to analyze the most important direction of change in high-dimensional data. In point cloud processing, PCA usually takes a neighborhood point set of a certain point as input, that is, collects m neighboring points around a given point. After centering the coordinates of these neighborhood points, its covariance matrix is calculated and eigenvalue decomposition (or singular value decomposition) is performed to obtain three eigenvectors and corresponding eigenvalues, which represent the spatial distribution of the neighborhood in the three main axis directions respectively. The output includes: three eigenvalues λ0, λ1, λ2 (λ0≤λ1≤λ2) and corresponding eigenvectors v0, v1, v2. Among them, v0 is usually used as the estimated direction of the normal vector, and λ0 reflects the degree of discreteness along the normal direction, which can be used to further calculate geometric features such as local curvature.

[0074] This paper uses RANSAC (Random Sample Consensus) to extract regular planes. RANSAC is a robust model fitting algorithm commonly used when data contains a large number of outliers. In point cloud processing, RANSAC can be used to estimate geometric models (such as planes, cylinders, or spheres) from three-dimensional point sets and identify the set of points that belong to the model. Its basic process involves first randomly selecting a minimum set of sample points (e.g., three points are required to fit a plane) from the original data to estimate the initial model parameters. This model is then applied to the entire dataset, and the number of "inliers" that are consistent with the model within an error threshold is counted. The current model and its corresponding number of inliers are recorded. This process is repeated several times, and the model with the most inliers is selected as the final output. RANSAC inputs include the original dataset, the minimum number of sample points, the error tolerance threshold, and the maximum number of iterations. Its output is the optimal model parameter estimate and a set of data points considered "inliers." This method is particularly suitable for scenarios with high noise levels and data mixed with multiple models or interfering points, effectively preventing outliers from dominating the fitting results.

[0075] The specific process of constructing the regular plane model of the target point's surrounding environment in the present invention is as follows:

[0076] Obtain point cloud data near the target point through the depth camera Among them, q v is the three-dimensional position coordinate of each point, and Q k According to the method of allocating θ point data to each continuous area, multiple continuous point cloud areas are divided, and the a-th time (a=0,1,2…) is defined as the number of points Q selected. k All the point data in a certain point cloud area are analyzed by PCA on the three-dimensional coordinates of these points, and the three largest eigenvalues λ are decomposed. 0a ,λ 1a ,λ 2a (λ 0a ≤λ 1a ≤λ 2a ), then the curvature of the surface corresponding to the θ point data is Remove this point cloud area from the point cloud and repeat the above operation to obtain the a+1th curvature curv a+1 , if |curv a+1 -curv a | is greater than a certain threshold, then it is determined that the point cloud area selected for the a+1th time and the point cloud area selected for the ath time are on two surfaces with a large difference in curvature. If |curv a+1 -curv a| is less than the threshold, the two point cloud areas are considered to be on the same surface; repeat the above operation until all point cloud areas are selected and multiple surfaces with very different curvatures are identified. Use the RANSAC method to fit multiple regular plane models near the target point to the above multiple surfaces. Calculate the distance between the target point and the surrounding planes. The Forerunner robot receives the distances less than ρ (ρ is a certain constant) and the corresponding planes; calculate the distance d from the Forerunner robot to these planes with distances less than ρ. wtc , where c = 1, 2, 3…n, where n is the number of planes; calculate the distance d from the companion robot to these planes whose distance is less than ρ witc , where c = 1, 2, 3…n, and n is the number of planes.

[0077] 6. Multi-spherical robot collaborative system performs pursuit missions.

[0078] During the execution of the task, the multi-spherical robot collaborative system performs periodic sampling. In the kth round of sampling, the position coordinates p of the companion robot i are ik With speed v ik , the Euclidean distance d from the companion robot i to the forerunner robot ik , the Euclidean distance d from companion robot i to companion robot j ijk And the location coordinates of the target point goal k , the position coordinates p of the forerunner robot k and v lk , the Euclidean distance d from the forerunner robot to the target point k , d k =|p k -goal k |, the distance d' between the forerunner robot and the obstacle k Among them, v lk represents the velocity matrix of the forerunner robot in the xyz directions in the kth round, v ik Represents the velocity matrix of companion robot i in the x, y, and z directions in the kth round.

[0079] According to the updated distance d between the forerunner robot and the target point k , to judge the task type, the judgment principle is:

[0080] If s=C, the pursuit mission ends. If s≠C, the position and speed of the forerunner robot and the companion robot are controlled, and the system state and the target point state are updated. The distance d between the forerunner robot and the target point after the update is calculated. k , determine the mission type until the pursuit mission is completed.

[0081] Among them, (1) Regarding position control: the position control of the forerunner robot is p k+1 =p k +v lk+1 *dt, the position control of the companion robot is p ik+1 =p ik +v ik+1 *dt, where dt is the time step constant.

[0082] (2) Regarding speed control: Since the capture operation is actually performed by the companion robot, the speed control method of the forerunner robot is the same regardless of s = A or s = B. To ensure that the multi-spherical robot collaborative system finds an optimal collision-free path in the pursuit task, the gravitational potential field between the system and the target point, the repulsive potential field between the system and the obstacle, and the traction force within the system must be taken into account when updating the speed.

[0083] Based on the gravitational potential field between the forerunner robot and the target point, and the repulsive potential field between the forerunner robot and the obstacle, in the kth round of sampling, the gravitational force exerted on the forerunner robot by the target point is defined as G k , the repulsive force from the surrounding obstacles is F ok The speed control formula of the forerunner robot is v lk+1 =v lk +G k +F ok According to the definition of artificial potential field method above, we know that G k =F a (p k ,goal k ,2), z is the amount of repulsive force, where d' k is the distance between the forerunner robot and the obstacle; when the obstacle is a plane extracted from the surrounding environment features of the target point, d' k =d wtck , d wtck It represents the distance from the forerunner robot to the extracted plane in the k-th round of acquisition, at which time z = n.

[0084] Based on the traction force of the forerunner robot on the companion robot and the repulsive potential field between the companion robot and the obstacle, in the k-th round of update, the traction force of the companion robot i on the forerunner robot is defined as F pik , the repulsive force from the surrounding obstacles is F iok , the speed control formula of companion robot i is v ik+1 =v ik +F pik +Fiok .

[0085] According to the leader-follower method, the traction force F pik = F pi (p k , p ik , WPM k ).

[0086] According to the artificial potential field method, it can be known that z is the number of repulsive forces. Among them, when the obstacle is an obstacle that needs to be bypassed and avoided during the movement of multiple robots, d' k is the distance between the leader robot and the obstacle, and the companion robot i will also regard other companion robots except itself as obstacles; when the obstacle is a plane extracted from the environmental characteristics around the target point, d' k = d witch , and at this time z = n.

[0087] (3) Based on the definition of the update method of the expected coordinate matrix WPM above, the update process of WPM k is as follows:

[0088] If d k ≥ L, that is, s = A, the companion robot performs the tracking task and will not change its orientation, then t gk = t k-1 , t k will not be updated, so WPM k = WPM k-1 .

[0089] If M ≤ d k < L, that is, s = B, the companion robot performs the capture task, and the following four capture strategies adapted to the environment are defined to update t gk , which are listed as follows:

[0090] If the leader robot identifies that there is no plane adjacent to the target point, a vertical capture strategy from top to bottom or from bottom to top is selected according to the relative position between the leader robot and the target point. If the target point is above the leader robot, then t gk is updated to (0 0 1); if the target point is below the leader robot, then t gk is updated to (0 0 -1);

[0091] If it is identified that there is a plane adjacent to the target point, then t gk is updated to the direction parallel to the normal of the plane and close to the plane;

[0092] If two planes are identified to be adjacent to the target point, a point that is perpendicular to the target point is found at the intersection of the two planes and is recorded as Point. gk Update to the direction from the target point to Point;

[0093] If three or more planes are identified to be adjacent to the target point, and if these planes have a common intersection point O, then t gk Update to the direction from the target point to O. If these planes do not have a common intersection, then change t gk Updated to a direction that is perpendicular to the intersection line of the plane closest to the target point and passes through the target point.

[0094] Define an interpolation method to gradually convert t k Rotate to t gk . Set a counter c x , initialize it to 1, calculate t gk With t k The angle α between them. At t gk In the rounds that do not change, α remains unchanged. When the cycle rounds increase, c x =c x +1; when the environment changes or the target point moves, if t gk If it changes, recalculate t gk With t k The angle α between them is c x Set to 1.

[0095] Assume t k Uniform rotation, angular velocity is ω (ω<α / 2),

[0096] but

[0097] According to the above updated t k ,t gk , you can get, WPM k =Rotate(t k ,t gk ,WPM k-1 ).

Claims

1. A method for multiple spherical robots to collaboratively and actively pursue a target, characterized in that: The following steps are involved: S1. Construct a multi-spherical robot collaborative system, including a square flexible net and four single spherical robots, each holding a corner of the flexible net, to form a square formation; S2. Formation control of the multi-spherical robot collaborative system is performed based on the pilot-formation method. A single spherical robot is defined as a companion robot. A virtual forerunner robot is created at the center of the flexible net, and the companion robot follows the trajectory and speed of the forerunner robot. S3. Constructing the pursuit task of the multi-spherical robot collaborative system, including tracking task, capture task, and termination task; S4. The multi-spherical robot collaborative system performs the pursuit mission and finds an optimal collision-free path to successfully capture the target point, including: S401: including periodically collecting the position and speed information of the forerunner robot and the companion robot, the target point and the obstacle position information, and determining the task type based on the distance between the forerunner robot and the target point after the kth round of sampling; S402: If the task is a tracking or capturing task, the attraction and repulsion of the target point and obstacles on the forerunner robot or the companion robot are calculated based on the artificial potential field method. At the same time, a regular plane model of the environment around the target point is constructed to calculate the distance between the forerunner robot and the companion robot and a specific plane around the target point. S403: Based on the gravitational force and repulsive force and the distance to the specific plane around the target point, the position and speed of the forerunner robot and the companion robot are controlled, and the next round of information collection and task judgment is carried out until the task type is terminated.

2. The method for actively pursuing a target by coordinating multiple spherical robots according to claim 1, characterized in that: The multi-spherical robot collaborative system described in S2 performs formation control, including: (1) Define the orientation of the formation plane as the normal direction of the plane, and the angle between the normal direction and the velocity direction of the forerunner robot is an acute angle; define the coordinate system WP as the world coordinate system of the multi-spherical robot collaborative system, define the coordinate system P as the body coordinate system of the forerunner robot, and define the orientation of the formation plane as the z-axis direction in the forerunner robot body coordinate system upward. In the initial stage, the coordinate axis of the coordinate system P is parallel to the coordinate axis of the coordinate system WP; (2) The position coordinates of the forerunner robot and the companion robot defined in the coordinate system WP are and The number of the companion robot. In the initial stage, (3) Define the expected coordinate matrix of the companion robot and the forerunner robot relative to the forerunner robot in the coordinate system WP as WPM, represents the expected coordinate matrix of the companion robot i relative to the forerunner robot in the coordinate system WP, A virtual coordinate matrix defined on the coordinate system P is PM, PM=(p0' p1' p2' p3' p'), p i ',i=0,1,2,3 represents the expected coordinate matrix of companion robot i in coordinate system P, p' represents the expected coordinate matrix of the forerunner robot in coordinate system P, In the initial stage, WPM=PM; (4) Define that there is a repulsive force between the companion robots, and the companion robot is subjected to the traction force of the forerunner robot. The traction force is: Among them, γ is the coefficient of controlling traction, w il A variable representing the difference between the relative distance between companion robot i and the predecessor robot and the expected relative distance in the world coordinate system WP, Where C is a constant, Δx il =(xx i )-(w l,x -w i,x ),Δy il =(yy i )-(w l,y -w i,y ),Δz il =(zz i )-(w l,z -w i,z ).

3. The method for cooperatively and actively pursuing a target by multiple spherical robots according to claim 1, characterized in that: The pursuit task set S = {A, B, C} described in S3 defines the task currently executed by the forerunner robot as s, s∈S, where A represents the pursuit task. Upon receiving the task, the forerunner robot will notify the four companion robots to jointly execute the task of tracking the target point; B represents the capture task. Upon receiving the task, the forerunner robot will select a capture strategy and notify the four companion robots to capture the target; C represents the termination task, which terminates the movement of the forerunner robot and the companion robots; Two distance thresholds L and M are defined, where L>>M, and M is the safety distance for the multi-spherical robot collaborative system to ensure its own safety; when the distance between the forerunner robot and the target point is greater than L, the multi-spherical robot collaborative system performs the tracking task; when the distance between the forerunner robot and the target point is greater than M and less than L, the multi-spherical robot collaborative system performs the capturing task; when the distance between the forerunner robot and the target point is less than M, the task ends, and the movement of the forerunner robot and the companion robot is terminated.

4. The method for cooperatively and actively pursuing a target by multiple spherical robots according to claim 3, characterized in that: In the pursuit mission, the multi-spherical robot collaborative system uses binocular vision to obtain the distance between the forerunner robot and the target point and obstacles, including first obtaining two left and right images of the same scene through cameras arranged on the left and right sides of the forerunner's forward direction, calculating the disparity value based on the corresponding pixel pairs in the left and right images, obtaining a disparity map, and back-projecting the disparity map to reconstruct the coordinates of each pixel point in the scene in three-dimensional space. It can also realize the generation of dense or sparse point cloud data, and obtain the distance between the forerunner robot and the target point and obstacles based on the coordinate calculation.

5. The method for cooperatively and actively pursuing a target by multiple spherical robots according to claim 1, characterized in that: The position and speed information of the forerunner robot and the companion robot, the target point and the obstacle position information are periodically collected in S401, including the position coordinates p of the companion robot i. ik With speed v ik , the Euclidean distance d from the companion robot i to the forerunner robot ik , the Euclidean distance d from companion robot i to companion robot j ijk And the location coordinates of the target point goal k , the position coordinates p of the forerunner robot k and v lk , the Euclidean distance d from the forerunner robot to the target point k , d k =|p k -goal k |, the distance d' between the forerunner robot and the obstacle k , where v lk represents the velocity matrix of the forerunner robot in the xyz directions in the kth round, v ik represents the velocity matrix of the companion robot i in the x, y, and z directions in the kth round; The judgment principle of the task type is as follows: If d k < M, it is a termination task. If d k ≥ L, it is a tracking task. If M ≤ d k < L, it is a capture task. L and M are distance thresholds, L >> M, and M is the safety distance for the multi-spherical robot cooperation system to ensure its own safety.

6. The method for actively pursuing a target by coordinating multiple spherical robots according to claim 5, characterized in that: The method for controlling the position and speed of the forerunner robot and the companion robot in S403 is: (1) Regarding position control: The position control of the Forerunner robot is p k+1 =p k +v lk+1 *dt, the position control of the companion robot is p ik+1 =p ik +v ik+1 *dt, where dt is the time step constant; (2) About speed control: The speed control of the Forerunner robot is v lk+1 =v lk +G k +F ok According to the definition of artificial potential field method, the forerunner robot is subject to the gravitational force G of the target point. k =F a (p k ,goal k ,2), the pioneer robot is subject to the repulsive force of surrounding obstacles z is the amount of repulsive force, where d' k is the distance between the forerunner robot and the obstacle; when the obstacle is a plane extracted from the target point's surrounding environment features based on the target point's surrounding environment regular plane model, d' k =d wtck , d wtck represents the distance from the forerunner robot to the extracted plane in round k of acquisition, at which point z = n; The speed of companion robot i is controlled to be v ik+1 =v ik +F pik +F iok According to the definition of the leader-follower method, the traction force F of the forerunner robot on the companion robot i is pik =F pi (p k ,p ik ,WPM k ), according to the definition of artificial potential field method, the companion robot i is subject to the repulsive force of the surrounding obstacles z is the amount of repulsive force, where d' k is the distance between the forerunner robot and the obstacle, and the companion robot i will also regard other companion robots as obstacles; when the obstacle is a plane extracted from the target point's surrounding environment features based on the target point's surrounding environment regular plane model, d' k =d witck , at this time z=n; WPM k is the expected coordinate matrix of the companion robot and the forerunner robot relative to the forerunner robot in the coordinate system WP.

7. The method for actively pursuing a target by coordinating multiple spherical robots according to claim 6, wherein: The method for calculating the gravitational force and repulsive force exerted on the forerunner robot or the companion robot by the target point and the obstacle is as follows: establishing the gravitational potential field and the repulsive potential field between the forerunner robot or the companion robot and the target point and the obstacle based on the artificial potential field method, and then further calculating the gravitational force and repulsive force exerted on the robot; The gravitational potential field between the robot and the target point is: U a (p,goal,m)=a o |p-goal| m , where p and goal represent the positions of the robot and the target point at a certain moment, respectively, |p-goal| represents the straight-line distance between the robot and the target point at a certain moment, and a o and m are adjustable parameters, The gravitational force on the robot is: The repulsive potential field between the robot and the obstacle is: Among them, k o is an adjustable parameter in the repulsive potential field, d' is the Euclidean distance between the robot and the obstacle, d0 is the maximum range of the repulsive force exerted by the obstacle on the robot, The repulsive force on the robot is:

8. The method for cooperatively and actively pursuing a target by multiple spherical robots according to claim 6, characterized in that: The method for calculating the distances of the forerunner robot and the companion robot to a specific plane around the target point by constructing a regular plane model of the target point's surrounding environment is as follows: first, the environment around the target point is identified and divided into multiple planes, the distances between the target point and the multiple planes are calculated respectively, a specific plane whose distance is less than a constant ρ is extracted, and then the distances of the forerunner robot and the companion robot to the specific plane are calculated, which are expressed as d wtc d witc , c = 1, 2, 3…n, n is the number of specific planes.

9. The method for actively pursuing a target by coordinating multiple spherical robots according to claim 6, wherein: The expected coordinate matrix WPM k The update method is: WPM k =Rotate(t k ,t gk ,WPM k-1 ), t k is the orientation vector of the formation plane, and the orientation vector of the formation plane after rotation is t gk ; If d k ≥L, the companion robot performs the tracking task, and the formation plane does not change its direction, then t gk =t k-1 , t k No updates occur, so WPM k =WPM k-1 ; If M ≤ d k <When L, the accompanying robot executes the capture mission, and the following four capture strategies adapted to the environment are defined to update t gk : If the forerunner robot recognizes that there is no specific plane adjacent to the target point, it selects a top-down or bottom-up vertical capture strategy based on the relative position of the forerunner robot and the target point. If the target point is above the forerunner robot, it will capture the target point from the top down or from the bottom up. gk Update to (0 0 1), if the target point is below the forerunner robot, then t gk Update to (0 0 -1); If the forerunner robot recognizes that there is a specific plane adjacent to the target point, it will gk Update to a direction parallel to the plane's normal and close to the plane; If the forerunner robot recognizes that there are two specific planes adjacent to the target point, it will find a point at the intersection of the two planes that is connected to the target point and perpendicular to the intersection line. This point is recorded as Point, and t gk Update to the direction from the target point to Point; If the forerunner robot recognizes that there are three or more specific planes adjacent to the target point, and if these planes have a common intersection O, then t gk Update to the direction from the target point to O. If these planes do not have a common intersection, then t gk Updated to a direction that is perpendicular to the intersection line of the plane closest to the target point and passes through the target point.

10. The method for actively pursuing a target by coordinating multiple spherical robots according to claim 9, characterized in that: The t k Rotate to t gk The method is: define an interpolation method to gradually change t k Rotate to t gk , set a counter c x , the initial value is 1, calculate t gk With t k The angle α between them is gk In the rounds that do not change, α remains unchanged. When the cycle rounds increase, c x =c x +1; When the scene changes or the target point moves, if t gk If it changes, recalculate t gk With t k The angle α between them is c x Set to 1; Assume t k For uniform rotation, the angular velocity is ω (ω<α / 2), then

Images