Virtual navigator-driven multi-robot affine formation path optimization method

Abstract

The invention discloses a multi-robot affine formation path optimization method driven by a virtual navigator, and the method comprises the steps: firstly setting a point set composed of the virtual navigator and followers, enabling the virtual navigator to form a convex hull and meet an affine localization condition, so as to guarantee that the followers are always located in the convex hull; generating a collision-free reference trajectory of the mass center of the virtual navigator based on a fast random tree algorithm; constructing a nonlinear constraint optimization problem NCO which contains linear transformation constraint and meets smoothness, obstacle avoidance, speed and angular speed constraint; and establishing a communication graph of the virtual navigator and the follower, calculating balance stress, designing a smoother follower distributed differential speed control law, and realizing formation path optimization and obstacle avoidance control. Compared with an existing method, the formation path generation method has the advantages that the smooth and stable formation path fitting the reference trajectory can be generated while obstacle avoidance and physical feasibility are guaranteed, and the cooperative motion capability and the control performance of a multi-robot system in a complex environment are improved.

Description

Technical Field

[0001] This invention belongs to the field of multi-robot formation control and trajectory planning technology, specifically involving a virtual navigator-driven multi-robot affine formation path optimization method. Background Technology

[0002] Formation control, as an important research direction in multi-agent systems, has shown broad application prospects in various fields such as military, aerospace, industry, environmental monitoring, and autonomous driving. Multi-agent formation control refers to the coordinated movement of multiple agents during task execution through local interactions, forming a specific geometric configuration and maintaining a stable formation to adapt to complex environments and diverse task requirements. This technology not only ensures the successful completion of collective tasks but also significantly improves the robustness and flexibility of the system. Existing multi-agent formation control methods mainly include the leader-follower method, behavior-based methods, and virtual structure-based methods.

[0003] The leader-follower method is simple in structure and easy to implement, but it is sensitive to single points of failure and lacks robustness. Behavior-based methods have a high level of intelligence and can handle multiple tasks such as obstacle avoidance and collision avoidance, but the models are complex, lack real-time performance, and are difficult to guarantee with strict stability. Virtual structure-based methods have high formation accuracy and robustness, but they are heavily reliant on communication and computing resources, making it difficult to meet real-time requirements in complex environments. As application scenarios become more complex, traditional methods are gradually revealing their insufficient adaptability.

[0004] In recent years, formation control methods based on consensus theory have attracted widespread attention. These methods emphasize distributed information exchange, enabling multiple agents to gradually converge to a consistent formation structure even without global information. Research shows that the consensus framework can unify various existing formation control strategies, providing theoretical support for distributed control in complex scenarios. However, these methods still face challenges in practical applications, such as limited communication, dynamic environmental interference, and obstacle avoidance.

[0005] To overcome the aforementioned problems, affine transformation-based formation control methods have gradually become a new research hotspot. This method, based on consensus control theory, constructs a stress matrix to achieve various maneuvers such as scale adjustment, rotation, and translation of the formation, exhibiting good flexibility and scalability, and effectively coping with complex dynamic environments. Related research has proven the stability of affine formation systems from a graph theory perspective, laying the theoretical foundation for further promotion of this method. Therefore, a new virtual navigator-driven multi-robot affine formation path optimization method is urgently needed to improve the formation's obstacle avoidance ability and path planning performance while ensuring formation stability, thereby adapting to complex and ever-changing application environments. Summary of the Invention

[0006] The purpose of this invention is to overcome the problems of insufficient robustness, poor real-time performance, and limited obstacle avoidance capability of existing multi-agent formation control methods in complex environments. It proposes a multi-robot affine formation path optimization method based on virtual navigator driving to achieve global optimization of formation trajectory and real-time obstacle avoidance, thereby improving the mobility and stability of the formation.

[0007] To achieve the above objectives, the present invention proposes the following technical solution:

[0008] A virtual navigator-driven multi-robot affine formation path optimization method, characterized by the following steps:

[0009] Step 1: Set up a set of points consisting of a virtual navigator and followers. The virtual navigator forms a convex hull and satisfies the affine localizability condition.

[0010] Step 2: Generate a collision-free reference trajectory for the virtual navigator's centroid based on the fast random tree method;

[0011] Step 3: Construct a nonlinear constraint optimization problem containing linear transformation constraints, satisfying smoothness, obstacle avoidance, velocity and angular velocity constraints;

[0012] Step 4: Establish a communication graph between the virtual navigator and followers, calculate the equilibrium stress and update the follower speed using a distributed control law, design a smooth differential control law for nonholonomic constraints of the differential chassis, and realize formation path optimization and obstacle avoidance control.

[0013] Further, step 1 includes the following steps:

[0014] 1.1) Determine the number of virtual navigators and followers in the formation, respectively, n. l and n f The formation target configuration is denoted as in The formation target configuration satisfies that when a linear transformation is applied to r, r f Always in r l Inside the convex hull;

[0015] 1.2) The following limitations are imposed on the relevant parameters of the virtual navigator robot: the number of virtual navigator robots must satisfy n. l ≥3; its augmented matrix configuration The rank should satisfy This ensures that the virtual navigator can span the entire two-dimensional space, providing sufficient conditions for subsequent affine transformations and follower positioning.

[0016] Furthermore, step 2 specifically includes:

[0017] Step 2.1: First, set the target configuration r l center of mass As the root node, denoted as p start and store it in the node set. In the middle; then, a sampling point p is randomly generated in the environment. rand If p rand If not in free space, discard and resample; if p rand Within free space, from the set of nodes Find the nearest node p nearest Next, determine p. rand and p nearest If the line connecting the two points collides with an obstacle, and if it does, discard the resampled point and resample; if there is no collision, further determine the Euclidean distance between the two points. rand -p nearest If ||≤γ, connect the two nodes and let p new =p rand When ||p rand -p nearest If ||>γ, then update p according to the following formula. new :

[0018]

[0019] Where λ is the step size;

[0020] p new Store in node set middle: If ||p new -p goal If ||≤∈, then the target point is considered to have been reached. The algorithm terminates; otherwise, it continues to generate sampling points and repeats the above process.

[0021] Step 2.2: Find the backtracking path from the target point to the starting point in the random tree: Create a table In the table, [i, j] represents the i-th row and j-th column, initially stored at the starting point p. start That is, TREE[1,1]=p start The first sampling yields a point p. new1 , and p start Connect, and p new1 Stored in p start Below it, at index [2,1], indicates that its parent node is p. start The second sampling yielded p new2 It is stored in the second position of the child node row, i.e., with index [2,2], and its parent node is calculated to be p. new1 , will p new1Stored in [1,2], the above expansion process continues until the target point is reached, then backtracking the path; first find the target point p. goal parent node p new，n , will p new，n As a child node, find its parent node p. new,n-1 again with p new,n-1 Find the parent node for each child node until the parent node is p. start Thus, the source p was found. goal to p start The path is discretized into K parts, denoted as b. k , k = 1, 2, ..., K.

[0022] Furthermore, step 3 specifically includes:

[0023] Discretize the problem, taking discrete time intervals k = 1, 2, ..., K. At time k, the linear transformation matrix is... Centroid trajectory Use the straightening operator to straighten A k Write it as a column vector Establish the navigator's position to satisfy the affine transformation constraint:

[0024]

[0025] in The total decision variable z of the entire optimization problem can be represented as a sequence of elements of a linear transformation matrix. The optimization problem is structured as follows:

[0026]

[0027] Where w i i = 1, 2, 3 are the weights of the three objective functions, a k k = 1, 2, ..., K are the decision variable elements. For the centroid coordinates of the navigator robot, b k The centroid trajectory of the formation configuration, Δt is the control interval, and u i,k For the control input of robot i, o j Let J be the coordinates of the j-th obstacle. A collection of navigator robots. Let r be the set of obstacle coordinates. j Let d be the radius of the obstacle. min Indicates the minimum distance from the obstacle; v represents the minimum distance between robots. min v max ω min and ω maxThese represent the minimum and maximum linear velocities and the minimum and maximum angular velocities of the robot's movement, respectively, and ε is a numerical regularization term to prevent the angular velocity from diverging when the robot stops or slows down.

[0028] Furthermore, step 4 is specifically as follows:

[0029] Establish an undirected communication graph between virtual navigator and follower robots. in Let ε represent the set of robot nodes, and ε represent the set of communication edges between robots, describing the adjacency relationships between them; the graph is calculated in conjunction with the target configuration r. equilibrium stress vector in This represents the stress coefficient between node i and node j; the geometry of the entire formation remains unchanged when the stress vector satisfies the following equilibrium constraint:

[0030] Based on the movement trajectory of the virtual navigator, the following iterative control law is used to update the position of the follower robot in real time:

[0031]

[0032] In the above formula, α is the control gain parameter, used to adjust the convergence speed and system stability, and Δt is the control interval, representing the discrete time interval for robot state updates. For a group of follower robots;

[0033] According to the above formula, mobile robots The velocity vector at time k+1 is in in, and Let be the velocity vector of mobile robot i in the world coordinate system at time k+1.

[0034] Assume that at time τ, the yaw rate of the mobile robot is... According to its differential control law:

[0035]

[0036] Where v i，τ+1 and ω i，τ+1 Let be the linear velocity and angular velocity of the mobile robot i at time τ+1. According to formula (6), the velocity constraint condition can be obtained. Right now Therefore, the linear velocity update formula for the differential motion robot can be derived as follows: The explicit form of the differential control law can be obtained further:

[0037] Based on the control law of formula (7), the speed control quantity of the differential mobile robot after time τ is... The increment is linear, meaning that while maintaining a stable formation structure, it can smoothly follow the trajectory of the virtual navigator. Considering cosθ... i，τ This may cause the linear velocity value to be too high, requiring adjustment of the speed command. Apply amplitude limiting constraints:

[0038] By coupling a stress matrix-based distributed update mechanism with a differential control law, this method enables the follower robot to maintain its formation while smoothly and dynamically tracking the virtual navigator's trajectory under nonholonomic constraints. This method achieves a good balance between formation stability, path optimization, and obstacle avoidance control, exhibiting high real-time performance and scalability.

[0039] By coupling a stress matrix-based distributed update mechanism with a differential control law, this method enables the follower robot to maintain its formation while smoothly and dynamically tracking the virtual navigator's trajectory under nonholonomic constraints. This method achieves a good balance between formation stability, path optimization, and obstacle avoidance control, exhibiting high real-time performance and scalability.

[0040] The design concept of this invention is as follows:

[0041] This method first sets up a point set consisting of a virtual navigator and followers. The virtual navigator forms a convex hull and satisfies the affine localizability condition to ensure that the followers are always within the convex hull. Then, a collision-free reference trajectory of the virtual navigator's centroid is generated based on the Rapidly-Exploring Random Tree (RRT) algorithm. A nonlinear constrained optimization (NCO) problem with linear transformation constraints and satisfying smoothness, obstacle avoidance, velocity, and angular velocity constraints is constructed to establish an undirected communication graph between the virtual navigator and followers. The equilibrium stress is calculated and a smoother distributed differential speed control law for followers is designed to achieve formation path optimization and obstacle avoidance control.

[0042] The beneficial effects of this invention are as follows:

[0043] Compared to existing methods, this invention generates smooth, stable formation paths that conform to reference trajectories while ensuring obstacle avoidance and physical feasibility, thus improving the cooperative motion capability and control performance of multi-robot systems in complex environments. Furthermore, this invention combines distributed coordinated control and affine formation transformation mechanisms with global path planning, enabling the system to adjust its formation shape and spacing in real time according to environmental changes, achieving autonomous obstacle avoidance and adaptive cooperative control, resulting in greater flexibility and task adaptability. Attached Figure Description

[0044] Figure 1 This is a schematic diagram of the virtual navigator formation of the present invention;

[0045] Figure 2 This is a flowchart of the present invention;

[0046] Figure 3 This is a configuration diagram of an embodiment of the present invention. Detailed Implementation

[0047] The present invention will be further described below with reference to the accompanying drawings.

[0048] A virtual navigator-driven multi-robot affine formation path optimization method, the specific steps of which are as follows:

[0049] Step 1: Set up a set of points consisting of a virtual navigator and followers. The virtual navigator forms a convex hull and satisfies the affine localizability condition.

[0050] Step 2: Generate a collision-free reference trajectory for the virtual navigator's centroid based on the fast random tree method;

[0051] Step 3: Construct a nonlinear constraint optimization problem containing linear transformation constraints, satisfying smoothness, obstacle avoidance, velocity and angular velocity constraints;

[0052] Step 4: Establish a communication graph between the virtual navigator and followers, calculate the equilibrium stress and update the follower speed using a distributed control law, design a smooth differential control law, and realize formation path optimization and obstacle avoidance control.

[0053] Example:

[0054] First, the number of virtual navigator and followers in the formation is determined to be 4 and 4 respectively; the formation target configuration is designed as follows. in This configuration satisfies that when a linear transformation is applied to r, r f Always in r l Inside the convex hull;

[0055] Calculate the augmented matrix of configuration r rank:

[0056]

[0057] The above ensures that the virtual navigator can span the entire two-dimensional space, providing sufficient conditions for subsequent affine transformations and follower positioning.

[0058] Then the target configuration r l center of mass As the root node, denoted as p start and store it in the node set. middle;

[0059] A sampling point p is randomly generated in the environment. and If p and If a node is no longer in free space, it is discarded and resampled; if it is in free space, it is selected from the node set. Find the nearest node p nearest Next, determine p. rand and p nearest If the line connecting the two points collides with an obstacle, and if it does, discard the resampled point and resample; if there is no collision, further determine the Euclidean distance between the two points. rand -p nearest If ||≤γ, connect the two nodes and let p new =p rand ;

[0060] When ||p rand -p nearest If ||>γ, then update p according to the following formula. new ;

[0061]

[0062] Where λ is the step size. Let p new Store in node set middle:

[0063] If ||p new -p goal If ||≤∈, then the target point is considered to have been reached;

[0064] at this time The algorithm terminates; otherwise, it continues to generate sampling points and repeats the above process.

[0065] Furthermore, the program searches for a backtracking path from the target point to the starting point within a random tree. A table is created during program execution. In the table, [i, j] represents the i-th row and j-th column; the initial storage starting point is p. start TREE[1,1]=p start The first sampling yields a point p. new1, and p start Connect; connect p new1 Stored in p start Below it, at index [2,1], indicates that its parent node is p. start The second sampling yielded p new2 It is stored in the second position of the child node row, i.e., with index [2,2]; its parent node is calculated to be p. new1 , will p new1 Stored in [1,2]; the above expansion process continues until the target point is reached, after which the path is backtracked;

[0066] First, find the target point p. goal parent node p new,n , will p new,n As a child node, find its parent node p. new,n-1 ; again with p new,n-1 Find the parent node for each child node until the parent node is p. start Thus, the source p was found. goal to p start The path, denoted as b k Then, the problem is discretized, taking discrete time intervals k = 1, 2, ..., K. At time k, the linear transformation matrix is ​​taken as... Centroid trajectory

[0067] Use the straightening operator to straighten A k Write it as a column vector Establish the navigator's position to satisfy the affine transformation constraint: x i，k =T i a k +b k ;

[0068] in Therefore, the total decision variable z of the entire optimization problem can be represented as a sequence of elements of a linear transformation matrix. At the initial moment, the formation maintains its initial configuration, that is...

[0069] Based on this setting, the optimization problem can be constructed in the following form:

[0070]

[0071] st

[0072]

[0073] Where w i i = 1, 2, 3 are the weights of the three objective functions, a k k = 1, 2, ..., K are the decision variable elements. For the centroid coordinates of the navigator robot, b k Let f be the center-of-mass trajectory of the formation configuration, and f be the kinematic model of the mobile robot. In this example, Δt is the control interval, u i,k For the control input of robot i, o j Let J be the coordinates of the j-th obstacle. A collection of navigator robots. Let r be the set of obstacle coordinates. j Let d be the radius of the obstacle. min Indicates the minimum distance from the obstacle; v represents the minimum distance between robots. min v max ω min and ω max These represent the minimum and maximum linear velocities and the minimum and maximum angular velocities of the robot's movement, respectively, and ε is a numerical regularization term to prevent the angular velocity from diverging when the robot stops or slows down.

[0074] In the optimization framework constructed above, the objective function mainly consists of three key parts; the first is about the linear transformation matrix a. k The first is a smoothing term that constrains the variation amplitude of the virtual navigator formation transformation matrix between adjacent time points, ensuring a smooth transition of the entire formation trajectory over time; the second is to prevent linear transformation matrix a k The third part is the deviation minimization term between the virtual navigator's expected trajectory and the reference trajectory generated from the initial sampling, ensuring that the optimized trajectory can fit the reference path as closely as possible while taking into account the overall smoothness.

[0075] Regarding constraints, the kinematic constraints of the virtual navigator were first considered to ensure that the trajectory evolution conforms to the dynamic characteristics of the system; secondly, obstacle constraints were introduced to achieve obstacle avoidance by ensuring the minimum safe distance between the virtual navigator and obstacles in the environment; at the same time, mutual collision avoidance constraints between virtual navigators were added to avoid collisions during formation.

[0076] In addition, upper and lower bound constraints were set for the velocity and angular velocity during the motion process to ensure that the generated trajectory meets the physical feasibility constraints of the actual robot.

[0077] Through the combined effect of the objective function and constraints, the optimization algorithm can generate an optimal control trajectory that satisfies both obstacle avoidance and formation maintenance requirements, and is numerically stable, smooth, and closely follows the reference trajectory, thereby improving the robustness and performance of formation control.

[0078] Finally, an undirected communication graph between the virtual navigator and follower robots is constructed. in and These are the sets of leader and follower robot nodes, respectively. ε={e 12 e 23 e 34 e 41 e 15 e 26 e 37 e 48 e 78 e 85 e 56 e 67 e 57 e 68}. ε represents the set of communication edges between robots, describing the adjacency relationships between them; the graph is calculated in conjunction with the target configuration r. equilibrium stress matrix

[0079]

[0080] in This represents the stress coefficient between node i and node j. The geometry of the entire formation remains unchanged when this stress vector satisfies the following equilibrium constraint:

[0081]

[0082] Based on the movement trajectory of the virtual navigator, the following iterative control law is used to update the position of the follower robot in real time:

[0083]

[0084] In the above formula, α is the control gain parameter, used to adjust the convergence speed and system stability, and Δt is the control interval, representing the discrete time interval for robot state updates. For a group of follower robots. Based on the above formula, mobile robots... The velocity vector at time k+1 is in in, and Let be the velocity vector of mobile robot i in the world coordinate system at time k+1.

[0085] Assume that at time τ, the yaw rate of the mobile robot is... According to its differential control law:

[0086]

[0087] Where v i，τ+1and ω i，τ+1 Let be the linear velocity and angular velocity of the mobile robot i at time τ+1. According to formula (9), the velocity constraint condition can be obtained. Right now Therefore, the linear velocity update formula for the differential motion robot can be derived as follows: The explicit form of the differential control law can be obtained further:

[0088]

[0089] Based on the control law of formula (10), the speed control quantity of the differential mobile robot after time τ is... The increment is linear, allowing for smooth following of the virtual navigator's trajectory while maintaining a stable formation structure. Considering cosθ... i,τ This may cause the linear velocity value to be too high, requiring adjustment of the speed command. Apply amplitude limiting constraints:

[0090]

[0091] By coupling a stress matrix-based distributed update mechanism with a differential control law, this method enables the follower robot to maintain its formation while smoothly and dynamically tracking the virtual navigator's trajectory under nonholonomic constraints. This method achieves a good balance between formation stability, path optimization, and obstacle avoidance control, exhibiting high real-time performance and scalability.

[0092] The above embodiments are merely preferred embodiments of the present invention and are not intended to limit the technical solutions of the present invention. Any technical solution that can be implemented based on the above embodiments without creative effort should be considered to fall within the scope of protection of the patent of the present invention.

Claims

1. A method for optimizing the path of a multi-robot affine formation driven by a virtual navigator, characterized in that, Includes the following steps: Step 1: Set up a set of points consisting of a virtual navigator and followers. The virtual navigator forms a convex hull and satisfies the affine locatability condition to ensure that the followers are always inside the convex hull. Step 2: Generate a collision-free reference trajectory for the virtual navigator's centroid based on the fast random tree method; Step 3: Construct a nonlinear constraint optimization problem containing linear transformation constraints, satisfying smoothness, obstacle avoidance, velocity and angular velocity constraints; Step 4: Establish a communication graph between the virtual navigator and followers, calculate the equilibrium stress and update the follower speed using a distributed control law, design a smooth differential control law for nonholonomic constraints of the differential chassis, and realize formation path optimization and obstacle avoidance control.

2. The method for optimizing the path of a multi-robot affine formation driven by a virtual navigator according to claim 1, characterized in that, Step 1 includes the following steps: 1.1) Determine the number of virtual navigators and followers in the formation, respectively, n. l and n f The formation target configuration is denoted as r = [r l r f ] T ,in The formation target configuration satisfies that when a linear transformation is applied to r, r f Always in r l Inside the convex hull; 1.2) The following limitations are imposed on the relevant parameters of the virtual navigator robot: the number of virtual navigator robots must satisfy n. l ≥3; its augmented matrix configuration The rank should satisfy in This ensures that the virtual navigator can span the entire two-dimensional space, providing sufficient conditions for subsequent affine transformations and follower positioning.

3. The method for optimizing the path of a multi-robot affine formation driven by a virtual navigator according to claim 1, characterized in that, Step 2 specifically involves: Step 2.1: First, set the target configuration r l center of mass As the root node, denoted as p start and store it in the node set. In the middle; then, a sampling point p is randomly generated in the environment. rand If p rand If not in free space, discard and resample; if p rand Within free space, from the set of nodes Find the nearest node p nearest Next, determine P. rand and p nearest If the line connecting the two points collides with an obstacle, and if it does, discard the resampled point and resample; if there is no collision, further determine the Euclidean distance between the two points. rand- p nearest If ||≤γ, connect the two nodes and let p new =p rand When ||p rand -p nearest If ||>γ, then update p according to the following formula. new : Where λ is the step size; p new Store in node set middle: If ||p new -p goal If ||≤∈, then the target point is considered to have been reached. The algorithm terminates; otherwise, it continues to generate sampling points and repeats the above process. Step 2.2: Find the backtracking path from the target point to the starting point in the random tree: Create a table In the table, [i, j] represents the i-th row and j-th column, initially storing the starting point P. start That is, TREE[1,1]=p start The first sampling yields a point p. new1 , and p start Connect, and p new1 Stored in p start Below it, at index [2,1], indicates that its parent node is p. start The second sampling yielded p new2 It is stored in the second position of the child node row, i.e., with index [2,2], and its parent node is calculated to be p. new1 , will p new1 Stored in [1,2], the above expansion process continues until the target point is reached, then backtracking the path; first find the target point p. goal parent node p new，n , will p new，n As a child node, find its parent node p. new，n-1 again with p new，n-1 Find the parent node for each child node until the parent node is p. start Thus, the source p was found. goal to p start The path is discretized into K parts, denoted as b. k k = 1, 2, ..., K.

4. The method for optimizing the path of a multi-robot affine formation driven by a virtual navigator according to claim 1, characterized in that, Step 3 specifically involves: Discretize the problem, taking discrete time intervals k = 1, 2, ..., K. At time k, take the linear transformation matrix as... Centroid trajectory Use the straightening operator to straighten A k Write it as a column vector Establish the navigator's position to satisfy the affine transformation constraint: in The total decision variable z of the entire optimization problem can be represented as a sequence of elements of a linear transformation matrix. The optimization problem is structured as follows: Where w i i = 1, 2, 3 are the weights of the three objective functions, a k k = 1, 2, ..., K are the decision variable elements. For the centroid coordinates of the navigator robot, b k The centroid trajectory of the formation configuration, Δt is the control interval, and u i，k For the control input of robot i, o j Let J be the coordinates of the j-th obstacle. A collection of navigator robots. Let r be the set of obstacle coordinates. j Let d be the radius of the obstacle. min Indicates the minimum distance from the obstacle; v represents the minimum distance between robots. min v max ω min and ω max These represent the minimum and maximum linear velocities and the minimum and maximum angular velocities of the robot's movement, respectively, and ε is a numerical regularization term to prevent the angular velocity from diverging when the robot stops or slows down.

5. The method for optimizing the path of a multi-robot affine formation driven by a virtual navigator according to claim 1, characterized in that, Step 4 is as follows: Establish an undirected communication graph between virtual navigator and follower robots. in Let ε represent the set of robot nodes, and ε represent the set of communication edges between robots, describing the adjacency relationships between them; the graph is calculated in conjunction with the target configuration r. equilibrium stress matrix Where ω ij This represents the stress coefficient between node i and node j; the geometry of the entire formation remains unchanged when the stress vector satisfies the following equilibrium constraint: Based on the movement trajectory of the virtual navigator, the following iterative control law is used to update the position of the follower robot in real time: In the above formula, 'a' is the control gain parameter, used to adjust the convergence speed and system stability, and 'Δt' is the control interval, representing the discrete time interval for robot state updates. For a group of follower robots; According to the above formula, mobile robots The velocity vector at time k+1 is in in, and Let be the velocity vector of mobile robot i in the world coordinate system at time k+1; Assume that at time τ, the yaw rate of the mobile robot is... According to its differential control law: Where v i，τ+1 and ω i，τ+1 Let be the linear velocity and angular velocity of the mobile robot i at time τ+1. According to formula (6), the velocity constraint condition can be obtained. Right now Therefore, the linear velocity update formula for the differential motion robot can be derived as follows: The explicit form of the differential control law can be obtained further: Based on the control law of formula (7), the speed control quantity u = [v] of the differential motion robot after time τ. i，τ+1 ω i，τ+1 ] T The increment is linear, meaning that while maintaining a stable formation structure, it can smoothly follow the trajectory of the virtual navigator; considering cosθ i，τ This may cause the linear velocity value to be too large, requiring adjustment of the velocity command u. i，k =[v i，k ω i，k ] T Apply amplitude limiting constraints: By coupling the distributed update mechanism based on the stress matrix with the differential control law, the follower robot can maintain its formation structure and smoothly achieve dynamic tracking of the virtual navigator's trajectory while satisfying nonholonomic constraints.

Images