Multi-robot formation tracking and obstacle avoidance control method under directed communication topology
By constructing a directed communication topology and a parameterized actuator reachable affine set, the problem of unified construction of formation and safety constraints in multi-robot systems is solved, achieving closed-loop input computation and task compatibility determination, reducing online computation and ensuring input realizability.
Patent Information
- Application Number
- CN202611311063.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-08-27
- Publication Date
- 2026-11-03
AI Technical Summary
Existing technologies struggle to uniformly construct directed graph formation rows and two types of safe rows in multi-robot systems, and lack methods for directly generating feasible inputs, resulting in large online computational loads and actuator reachability issues.
Construct a directed communication topology with the virtual navigator as the root node, combine natural constraints and input geometric parameterized actuator reachable affine sets of accelerations, construct formation, environmental obstacles and inter-robot safety constraints, and achieve task compatibility through closed-form least-weighted norm least squares input.
It enables direct computation of pseudo-inverse closed-form inputs within the reachable acceleration set of the actuator, reducing the amount of online computation, explicitly guaranteeing input realizability, and uniformly constructing formations and two types of safe tasks, providing task compatibility criteria.
Smart Images
Figure CN123029935A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of multi-robot system cooperative control technology, specifically relating to a multi-robot formation tracking and obstacle avoidance control method under a directed communication topology. Background Technology
[0002] When multi-robot systems perform tasks such as formation cruising, path tracking, environmental exploration, and cooperative transport, they need to track moving references, maintain preset geometric relationships, and keep safe distances from environmental obstacles and other robots. Sparse or directed communication, nonholonomic constraints, and underactuated inputs couple these objectives with actuator realizability.
[0003] Model predictive control, sequential quadratic programming, and quadratic programming-based safety filtering methods can explicitly handle state and input constraints, but they typically require solving an online optimization problem in each control cycle. The problem size increases with the number of robots and safety constraints.
[0004] Direct feedback and modular safety control can reduce online computation, and potential field methods are also easy to implement in engineering. However, these methods typically construct tasks in the kinematic layer or in independent safety modules, and whether the joint task is located within the actuator's reachable acceleration direction is not always explicit.
[0005] Graph theory-based formation control is suitable for describing local communication relationships. The Udwadia-Kalaba method can express motion requirements as acceleration layer constraints and give closed generalized constraint forces. Classical formulas do not directly incorporate the input matrix into the solution, and the resulting generalized forces may exceed the actuator's realizable force subspace on underactuated platforms.
[0006] Furthermore, both environmental obstacle avoidance and robot collision avoidance fall under state-dependent inequality safety requirements. Existing methods still lack a unified closed structure for jointly constructing directed graph formation rows and two types of safe rows, verifying their compatibility with actuator reachable acceleration sets, and directly generating realizable inputs. Summary of the Invention
[0007] To address the aforementioned issues, this invention parameterizes the actuator reachable affine acceleration set at the acceleration layer, determined jointly by natural constraints and input geometry. It constructs safety constraints for formation, environmental obstacles, and robot-to-robot interactions, and provides closed-form minimum weighted norm least-squares inputs on a normalized weighted task stack. Compatibility conditions are used to determine whether a task line can be accurately implemented. Safety conclusions within a fixed activation interval are predicated on initial safety values and accurate implementation of the safe line.
[0008] To achieve the aforementioned objectives, the present invention employs the following technical solution: a multi-robot formation tracking and obstacle avoidance control method under a directed communication topology, comprising the following steps: S1: Construct a directed communication topology with the virtual navigator as the root node, establish a dynamic model of the controlled mechanical system of each robot, and combine natural constraints with the input geometric parameterized actuator reachable affine set of acceleration. S2: Select the formation task output of each robot, and construct the formation tracking constraints of the acceleration layer based on the directed communication topology and the desired formation offset; S3: Based on the boundary distance between the robot center and environmental obstacles, construct the activation weighted logarithmic distance coordinates and their acceleration layer safety constraints; S4: Exclude the virtual navigator and construct collision avoidance constraints between robots based on the boundary spacing between the follower robot pairs; S5: Normalize the formation tracking constraints and the two types of safety constraints according to a fixed reference scale to form a weighted task stack with safety residual priority; S6: Calculate the closed-form least-weighted norm least-squares input within the affine set of the achievable acceleration of the actuator, and determine whether the current task stack can be accurately implemented by using compatibility conditions.
[0009] Furthermore, S1 specifically includes: Construct a directed communication topology with the virtual navigator as the root node:
[0010] Under the aforementioned topology, the Laplace matrix Divided into sections based on leaders and followers:
[0011] in, For a directed communication graph, For a set of nodes, Let be a set of directed edges. It is an adjacency matrix. For the topology matrix blocks from leader to follower, For the topological matrix block corresponding to the follower subgraph, the superscript is... Indicates matrix transpose; Establish a dynamic model of the controlled mechanical system of a single robot:
[0012] in, It is a symmetric positive definite mass matrix. For known generalized forces, For ideal natural constraints, Given a full column rank input matrix, To control the input, For generalized coordinates, For time, Denotes the first derivative. Represents the second derivative; Introduce mass scaling acceleration and write natural constraints into the scaling coordinates:
[0013] in, For mass scaling acceleration, Free acceleration scaled for mass. An acceleration layer representation of natural constraints. The acceleration coefficient matrix, Scaling the natural constraint matrix to quality, the natural constraints satisfy... Furthermore, the natural constraint force is an ideal constraint force, with the symbol... Represents the column space of a matrix; Define the natural constraint null space projection matrix and the baseline feasible acceleration without control input:
[0014] in, Project scaling acceleration onto , Let the free acceleration be the projection onto the naturally constrained affine set. The identity matrix, symbol This indicates the Moore-Penrose pseudo-inverse. Represents the null space; Selecting a positive definite input weight matrix And define the weighted input mapping and projected input matrix:
[0015] in, For the quality scaling weighted input matrix, The input direction matrix after satisfying the natural constraints; The affine set of all actuators in the system at the current state and time is:
[0016] in, For the actuator to reach scaled acceleration set, For any input parameter, Input dimension; Equivalently, with weighted input variables Represents reachable scaling acceleration:
[0017] in, For weighted input variables, This is the actual control input.
[0018] Furthermore, S2 specifically includes: Set up a robot The output of the formation mission is Its second derivative with respect to generalized acceleration is in affine form:
[0019] in, Output the mapping matrix of generalized acceleration for the task. For known items determined by state and time, For function mapping, For robots Generalized coordinates; Stack the outputs and expected offsets of each robot task as follows: and The graph-coupled formation condition is formed by using second-order stable reconstruction:
[0020] in, and For positive formation reconstruction gain, for 3D identity matrix, symbol Represents the Kronecker product. Output the dimension for the task; Retaining the follower equation, we obtain the follower formation constraints:
[0021]
[0022] in, For the generalized acceleration stacking vector of the followers, , For the follower acceleration coefficient matrix, To constrain the goals for followers, To reconstruct the follower block of the right-hand term in the graph coupling, Accelerate output for the navigator This is a stacked vector corresponding to the known terms.
[0023] Furthermore, S3 specifically includes: Follower robot With the An environmental obstacle is defined, specifying the distance from the robot's center to the obstacle's outer boundary:
[0024] in, The center position of the robot, The center of the obstacle. Let the radius of the circumcircle of the obstacle be . The distance from the center to the boundary of the obstacle; Given spacing scale Activation width and threshold Define the outer activation boundary:
[0025] exist Within the domain of definition, construct logarithmic interval coordinates, logical activation weights, and continuous truncation weights:
[0026] in, Logarithmic interval coordinates For logical activation weights, To continuously truncate the weights, , The minimum allowable distance threshold from the robot's center to the obstacle boundary, when hour, ; right A second-order stability equation is applied to the obstacle row, and it is written as a linear constraint on the robot's generalized acceleration:
[0027] in, and For positive security reconfiguration gain, and For the obstacle avoidance constraint coefficient row and the obstacle avoidance constraint objective term, by The second derivative, the relative position and velocity of the robot and the obstacle, and the acceleration of the obstacle are determined.
[0028] Furthermore, S4 specifically includes: excluding virtual navigators, defining the set of physical follower robot pairs and the robot pair boundary spacing:
[0029] in, For the physical follower robot pair set, and Let be the collision envelope radius of the two robots. For the robot's boundary spacing, the initial state satisfies , The rigid spacing between robots; Using the same logarithmic coordinates and continuous activation construction as the environmental obstacle constraints, linear constraints are formed for each activated robot pair to create an acceleration layer, and they are stacked in a fixed order:
[0030] in, and For robots The safety constraint row and the right-hand item, and To follow the stacked matrix and vector of the robot pair, the inactive robot pair contributes zero rows and zero right-hand terms.
[0031] Furthermore, S5 specifically includes: First, divide both ends of each constraint row by a fixed positive reference scale with the same residual units, and then construct the joint safety task stack:
[0032] in, and Given the safety constraint matrix and the safety constraint objective vector, and The stacking results of safe rows for environmental obstacles are used to adjust the relative weights of the two types of safe rows. This indicates that the blocks are stacked vertically by row. Combine the normalized formation line and the joint safety line into the current control task stack:
[0033] in, and For the current control task matrix and the right-hand side, Weighting for joint security actions.
[0034] Furthermore, the weighted least squares problem within the reachable set corresponding to S5 is: set up For the actuator to reach the generalized affine set of acceleration, For weight The optimal acceleration is:
[0035] in, To normalize the sum of squared residuals, It is the Euclidean norm; When joint security action When feasible, the acceleration for any safe passage. The safety residual satisfies:
[0036] This upper bound indicates that, under fixed states and a fixed set of active rows, the safety residual varies with... The increase approaches zero; When joint security action When internal infeasibility is not possible, define the minimum set of safe residuals:
[0037] when When it approaches infinity, For any finite cluster point, first minimize the joint security residual, and then minimize the formation residual in the solution set with the minimum security residual; Within a fixed active row interval, if the starting point of the interval satisfies the hard spacing and each active safe row is precisely implemented, then there is no finite-time boundary contact within the interval, and the corresponding conditional safety conclusion is:
[0038] limited If only residual weights are given, unconditional safety cannot be derived from the large weights themselves if the task stack and the executor reachable acceleration set are incompatible.
[0039] Furthermore, S6 specifically includes: Stack the blocks diagonally to follow the robot's dynamics, and set the current control task as follows: :
[0040] in, , , , and These are the stacking amounts of the follower mass matrix, known generalized forces, natural constraints, input matrix, and control input, respectively. Constructed by natural constraints of followers and baseline feasible scaling acceleration Then define the dimension reduction control matrix and the required row corrections:
[0041] in, This is the quality scaling control constraint matrix. To scale the input action matrix, As the input matrix for dimensionality reduction constraints, Adjust the vector for the required constraint row. The input weight matrix is positive definite. The input for the closed-form least-weighted norm least squares is:
[0042] The necessary and sufficient condition for the current task stack to be implemented precisely is: When this condition is met, closed-form input is the only minimum function that precisely implements the current task stack. Norm input; if this condition is not met, the closed-form input is the minimum residual solution of the current normalized task stack, and has the minimum among all minimum residual inputs. Norm.
[0043] The beneficial effects of this invention are: (1) Closed-loop input calculation. This invention directly calculates the pseudo-inverse closed-loop input within the reachable acceleration set of the actuator, without having to solve the online constraint optimization problem in each control cycle.
[0044] (2) Explicitly guaranteeing input realizability. This invention directly incorporates natural constraints and input geometry into acceleration parameterization, which can uniformly describe fully driven and underdriven platforms.
[0045] (3) Unified formation construction and two types of safety tasks. The present invention constructs environmental obstacle rows and robot safety rows separately, and stacks them together with the formation rows after normalization with a fixed reference scale.
[0046] (4) Provide task compatibility criteria. The compatibility criteria determine whether the current task stack can be accurately implemented by the existing executor. If incompatible, the controller returns the minimum residual input with the minimum weighted norm.
[0047] (5) Clarify the scope of application of the conclusions. The exponential convergence of the formation error requires the formation rows to be continuously and accurately realized. The safety conclusion within the fixed active row interval requires the safety initial value and the active safe row to be accurately realized. Attached Figure Description
[0048] Figure 1 This is a flowchart of a multi-robot formation tracking and obstacle avoidance control method under a directed communication topology.
[0049] Figure 2 This is a schematic diagram of the directed communication topology of a multi-robot system and the structure and coordinate system of the differential drive robot.
[0050] Figure 3 This is a diagram of the formation obstacle avoidance motion trajectory of a differential drive robot system.
[0051] Figure 4 Output error curves for planar tasks of the differential drive robot team. Detailed Implementation
[0052] The present invention will be further described below with reference to the accompanying drawings and specific embodiments.
[0053] like Figure 1 As shown, a method for multi-robot formation tracking and obstacle avoidance control under a directed communication topology includes the following steps.
[0054] S1: Construct a directed communication topology with the virtual navigator as the root node, establish a dynamic model of the controlled mechanical system of each robot, and combine natural constraints with the input geometric parameterized actuator reachable affine set of acceleration.
[0055] This embodiment includes a virtual navigator and A follower robot. The set of nodes is denoted as... Node 0 is the navigator. If following the robot... Receiving robot The information, then the neighboring rights The navigator does not receive information from followers.
[0056] Directed communication graph is represented as Divide the Laplace matrix into blocks based on leaders and followers:
[0057] in, For the topology matrix blocks from leader to follower, This represents the topology matrix block corresponding to the follower subgraph. The communication graph contains a directed spanning tree rooted at the leader node, therefore... It is a non-singular M matrix.
[0058] Establish a dynamic model of the controlled mechanical system of a single robot:
[0059] in, It is a symmetric positive definite mass matrix. For known generalized forces, For ideal natural constraints, Given a full column rank input matrix, To control the input, For generalized coordinates, For time, Denotes the first derivative. It represents the second derivative. The number of columns is less than When the dimension is , the system is an underactuated system.
[0060] Introduce a quality scaling variable:
[0061] The acceleration layer representation of natural constraints and its scaling form are as follows:
[0062] in, For mass scaling acceleration, Free acceleration scaled for mass. For mass scaling, naturally constrain acceleration. Acceleration is controlled by mass scaling. The acceleration coefficient matrix, Natural constraints satisfy the conditions of consistency and ideality.
[0063] Eliminating the ideal natural constraints yields the null space projection matrix and the baseline feasible acceleration without control input.
[0064] in, Project scaling acceleration onto , Let be the projection of free acceleration onto the naturally constrained affine set.
[0065] Select And define the quality scaling weighted input matrix and the projection input matrix:
[0066] in, For the quality scaling weighted input matrix, The input direction matrix after satisfying the natural constraints.
[0067] The affine set of all actuators in the system at the current state and time is:
[0068] in, For the actuator to reach scaled acceleration set, For any input parameter, The input dimension.
[0069] Weighted input variables Represents the total scaling acceleration:
[0070] in, This formula directly restricts the control input to the directions of acceleration that are allowed by natural constraints and that the actuator can produce. The above parameterization gives the affine set of reachable accelerations of the actuator in the current state, without requiring additional modeling constraints that are independent of the control task.
[0071] Assume the control task is... It consists of several acceleration layers, each with its own scaling coordinates:
[0072] in, For the task row matrix, This is the right-hand item of the task.
[0073] Substituting the parameterized reachable acceleration into the control task yields a dimension-reduced linear equation for the weighted input:
[0074] in, For the projection control gain matrix, The amount of correction required to achieve the task line.
[0075] Among all inputs that minimize row residuals, the one that is uniquely minimized is... Norm input is:
[0076] The necessary and sufficient condition for the precise realizability of the control task is:
[0077] When the compatibility condition is met, the closed-form input precisely implements the task row. When the condition is not met, the closed-form input minimizes the row residual of the current task stack and has the smallest residual among all inputs with the smallest residual. Norm.
[0078] S2: Select the formation task output of each robot, and construct the formation tracking constraints of the acceleration layer based on the directed communication topology and the desired formation offset.
[0079] Set up a robot The output of the formation mission is Its second derivative with respect to generalized acceleration is in affine form:
[0080] in, Output the mapping matrix of generalized acceleration for the task. For known items determined by state and time, For function mapping, For robots Generalized coordinates; set up and These are the stacking vectors for all task outputs and the desired formation offset, respectively. The formation condition based on local in-neighbor information is:
[0081] in, Output the identity matrix corresponding to the dimension of the task, with the symbol... Represents the Kronecker product. Output the dimension for the task. A second-order stable reconstruction is used to transform the formation conditions into an acceleration layer equation:
[0082] in, and A positive formation reconstruction gain.
[0083] Retaining the follower-side equations, we obtain the follower formation constraints:
[0084] , in, For the generalized acceleration stacking vector of the followers, , For the follower acceleration coefficient matrix, To constrain the goals for followers, To reconstruct the follower block of the right-hand term in the graph coupling, Accelerate output for the navigator This is a stacked vector corresponding to the known terms.
[0085] If the formation is realized precisely within a certain time interval, the graph coupling task error satisfies the second-order Hurwitz error equation. If the precise realization holds true for all subsequent time intervals, the task error exponentially converges to zero.
[0086] S3: Based on the boundary distance between the robot center and environmental obstacles, construct the activation weighted logarithmic distance coordinates and their acceleration layer safety constraints.
[0087] Let the first The center position of each obstacle is The circumcircle radius is For follower robots Define the distance from the center to the outer boundary of the obstacle:
[0088] in, This is the center position of the robot. (Specified) and take Not less than the collision envelope radius of any robot. Obstacle position, velocity, and acceleration are used for online calculation of safe movement.
[0089] Given , and Define the outer activation boundary:
[0090] exist Within the domain, define the logarithmic interval coordinates, the logical activation weights, and the continuity truncation weights:
[0091] in, Logarithmic interval coordinates For logical activation weights, To continuously truncate the weights, .
[0092] when hour, .when hour, and Apply a second-order stabilizing equation to the row of activated obstacles:
[0093] Taking the second derivative with respect to relative kinematics, we obtain the linear safety behavior for the robot's generalized acceleration:
[0094] in, To select the planar coordinates of the robot's center, and For the obstacle avoidance constraint coefficient row and the obstacle avoidance constraint objective term, , Calculated by relative position, relative velocity, obstacle acceleration, and gain. , calculate.
[0095] Define a follower robot Set of activated obstacles:
[0096] Assume that at any given time, each following robot corresponds to at most one active obstacle. When there is no active obstacle, the robot contributes zero rows and zero right-hand items.
[0097] Stack the environmental obstacles corresponding to the robot in a diagonal block manner:
[0098] in, and These are the stacking matrix and vector of the safe row of environmental obstacles, respectively.
[0099] Fix the time interval of a row containing a specific activated obstacle. If the starting point of the interval satisfies... And if the activation line is implemented precisely, then It remains greater than δ within this interval. This conclusion does not cover incompatible tasks, input saturation, model errors, or activation row switching.
[0100] S4: Exclude the virtual navigator and construct collision avoidance constraints between robots based on the boundary spacing between the follower robot pairs.
[0101] Define the physical follower robot pair set and the robot pair boundary spacing:
[0102] in, For the physical follower robot pair set, and Let be the collision envelope radius of the two robots. For the robot's boundary spacing, the initial state satisfies , This refers to the hard spacing between robots.
[0103] The desired formation offset satisfies the following formula, thus providing the required safe distance for the robot's center:
[0104] in, and For mission space robots and The desired offset planar component, and For task output and robots and A known upper bound on the distance between centers. The corresponding center position is output. .
[0105] For each robot pair, logarithmically spaced coordinates and continuous truncation weights are constructed using independent parameters, and second-order stable safe movement is applied. For relative positions... After taking the second derivative, we get Stack all physical follower robot pairs in a fixed order to obtain .in, and For robots The safety constraint row and the right-hand item, and To follow the stacked matrix and vector of the robot pair, the inactive robot pair contributes zero rows and zero right-hand terms.
[0106] S5: Normalize the formation tracking constraints and the two types of safety constraints according to a fixed reference scale to form a weighted task stack with safety residual priority.
[0107] First, divide both ends of each constraint row by a fixed positive reference scale with the same residual units, and then construct the joint safety task stack:
[0108] in, and Given the safety constraint matrix and the safety constraint objective vector, and The result of stacking for safe passage through environmental obstacles. Used to adjust the weight of safe movement between robots relative to safe movement against environmental obstacles. This indicates that the blocks are stacked vertically in rows.
[0109] Combine the normalized formation line and the joint safety line into the current control task stack:
[0110]
[0111] in, and For the current control task matrix and the right-hand side, To enhance joint security, Let A be a set formed by stacking blocks of affine sets of generalized accelerations attainable by the actuators of each following robot. For weight The optimal acceleration under these conditions. This problem is in... Minimize normalized formation residuals and The sum of squares of the safety residuals.
[0112] If joint security action is in If it is feasible internally, then for any safe and feasible acceleration... The safety residual satisfies:
[0113] This upper bound applies only to fixed states and fixed active row sets, indicating that the safety residual varies with... The increase approaches zero.
[0114] If joint security action is in If internally infeasible, define the minimum set of safe residuals: , when When it approaches infinity, Any finite cluster point first belongs to And then Minimize the formation residual.
[0115] Define a fixed time interval containing a set of active rows. If the starting point of the interval satisfies all hard spacings and each active-safe row is precisely implemented, then the conditional safety conclusion is:
[0116] The first set of inequalities corresponds to the robot and environmental obstacles, while the second set corresponds to the physical following robot pair.
[0117] If the formation is executed precisely within a certain time interval, the mission error satisfies the second-order Hurwitz error equation. If the precise execution holds true for all subsequent time intervals, the mission error converges exponentially.
[0118]
[0119] in, This represents task error.
[0120] limited Only change the relative weights of the normalized residuals.
[0121] S6: Calculate the closed-form least-weighted norm least-squares input within the affine set of the achievable acceleration of the actuator, and determine whether the current task stack can be accurately implemented by using compatibility conditions.
[0122] Let the current control task be And stacked diagonally in blocks to follow the robot's dynamics:
[0123] in, , , , and These represent the stacking amounts of the follower mass matrix, known generalized forces, natural constraints, input matrix, and control input, respectively.
[0124] Constructed by natural constraints of followers and baseline feasible scaling acceleration redefining and Closed-form input is:
[0125] in, This is the quality scaling control constraint matrix. To scale the input action matrix, As the input matrix for dimensionality reduction constraints, Adjust the vector for the required constraint row. The input weight matrix is positive definite.
[0126] The necessary and sufficient condition for the current task stack to be exactly realized is: When compatibility conditions are met, Accurately implement the current task stack. If this condition is not met, Minimize the row residuals of the current normalized task stack.
[0127] During online execution, robot state and obstacle motion information are read in each sampling cycle. Formation lines, environmental obstacle lines, and safe lines between robots are constructed, normalized according to a fixed reference scale, and formed into a weighted task stack. Natural constraint projections and closed inputs are calculated, and then the robot state is updated. This process does not require online solving of constraint optimization problems.
[0128] In one embodiment of the invention, validation was performed using an underactuated team consisting of four differential-driven follower robots and a virtual navigator.
[0129] like Figure 2 As shown, the communication graph contains a directed spanning tree rooted at node 0. When using the incoming neighbor convention, its Laplace matrix is:
[0130] The desired formation is a square with sides of 5 m, and the navigator's trajectory is... The four expected plane offsets of the following robot are taken from the four vertices of a square. The main scenario for differential drive adopts... , , , , m, m and Safety parameters between robots are adopted. m, , m, , and Translation, angle, and spacing are normalized according to a fixed reference scale.
[0131] The obstacle environment consists of one dynamic circular obstacle and two static polygonal obstacles. Each robot uses a circular collision envelope with a radius of 0.2 m, and all six physical follower robot pairs participate in the safe travel calculation. The simulation time domain is 100 s.
[0132] Differential drive robots use generalized coordinates Its dynamics are: , in, , Input matrix The two columns correspond to the torque of the left and right wheels, respectively, and .
[0133] Formation missions utilize the distance in front of the robot. For the planar task output of m, the safe distance is still calculated by the robot center. Figure 3 and Figure 4 Displays the motion trajectory and task output error over 100 seconds. In the main scene, the minimum center distance of dynamic obstacles is 3.539 m, which is higher than the 3.5 m hard threshold.
[0134] Those skilled in the art will recognize that the embodiments described herein are intended to help the reader understand the principles of the invention, and should be understood that the scope of protection of the invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific modifications and combinations based on the technical teachings disclosed in this invention without departing from the spirit of the invention, and these modifications and combinations are still within the scope of protection of the invention.
Claims
1. A method for multi-robot formation tracking and obstacle avoidance control under a directed communication topology, characterized in that, Includes the following steps: S1: Construct a directed communication topology with the virtual navigator as the root node, establish a dynamic model of the controlled mechanical system of each robot, and combine natural constraints with the input geometric parameterized actuator reachable affine set of acceleration. S2: Select the formation task output of each robot, and construct the formation tracking constraints of the acceleration layer based on the directed communication topology and the desired formation offset; S3: Based on the boundary distance between the robot center and environmental obstacles, construct the activation weighted logarithmic distance coordinates and their acceleration layer safety constraints; S4: Exclude the virtual navigator and construct collision avoidance constraints between robots based on the boundary spacing between the follower robot pairs; S5: Normalize the formation tracking constraints and the two types of safety constraints according to a fixed reference scale to form a weighted task stack with safety residual priority; S6: Calculate the closed-form least-weighted norm least-squares input within the affine set of the achievable acceleration of the actuator, and determine whether the current task stack can be accurately implemented by using compatibility conditions.
2. The multi-robot formation tracking and obstacle avoidance control method under directed communication topology according to claim 1, characterized in that, S1 specifically includes: Construct a directed communication topology with the virtual navigator as the root node: Under the aforementioned topology, the Laplace matrix Divided into sections based on leaders and followers: in, For a directed communication graph, For a set of nodes, Let be a set of directed edges. It is an adjacency matrix. For the topology matrix blocks from leader to follower, For the topological matrix block corresponding to the follower subgraph, the superscript is... Indicates matrix transpose; Establish a dynamic model of the controlled mechanical system of a single robot: in, It is a symmetric positive definite mass matrix. For known generalized forces, For ideal natural constraints, Given a full column rank input matrix, To control the input, For generalized coordinates, For time, Denotes the first derivative. Represents the second derivative; Introduce mass scaling acceleration and write natural constraints into the scaling coordinates: in, For mass scaling acceleration, Free acceleration scaled for mass. An acceleration layer representation of natural constraints. The acceleration coefficient matrix, Scaling the natural constraint matrix to quality, the natural constraints satisfy... Furthermore, the natural constraint force is an ideal constraint force, with the symbol... Represents the column space of a matrix; Define the natural constraint null space projection matrix and the baseline feasible acceleration without control input: in, Project scaling acceleration onto , Let the free acceleration be the projection onto the naturally constrained affine set. The identity matrix, symbol This indicates the Moore-Penrose pseudo-inverse. Represents the null space; Selecting a positive definite input weight matrix And define the weighted input mapping and projected input matrix: in, For the quality scaling weighted input matrix, The input direction matrix after satisfying the natural constraints; The affine set of all actuators in the system at the current state and time is: in, For the actuator to reach scaled acceleration set, For any input parameter, Input dimension; Equivalently, with weighted input variables Represents reachable scaling acceleration: in, For weighted input variables, This is the actual control input.
3. The multi-robot formation tracking and obstacle avoidance control method under directed communication topology according to claim 2, characterized in that, S2 specifically includes: Set up a robot The output of the formation mission is Its second derivative with respect to generalized acceleration is in affine form: in, Output the mapping matrix of generalized acceleration for the task. For known items determined by state and time, For function mapping, For robots Generalized coordinates; Stack the outputs and expected offsets of each robot task as follows: and The graph-coupled formation condition is formed by using second-order stable reconstruction: in, and For positive formation reconstruction gain, for 3D identity matrix, symbol Represents the Kronecker product. Output the dimension for the task; Retaining the follower equation, we obtain the follower formation constraints: in, For the generalized acceleration stacking vector of the followers, , For the follower acceleration coefficient matrix, To constrain the goals for followers, To reconstruct the follower block of the right-hand term in the graph coupling, Accelerate output for the navigator This is a stacked vector corresponding to the known terms.
4. The multi-robot formation tracking and obstacle avoidance control method under directed communication topology according to claim 3, characterized in that, S3 specifically includes: Follower robot With the An environmental obstacle is defined, specifying the distance from the robot's center to the obstacle's outer boundary: in, The center position of the robot, The center of the obstacle. Let the radius of the circumcircle of the obstacle be . The distance from the center to the boundary of the obstacle; Given spacing scale Activation width and threshold Define the outer activation boundary: exist Within the domain of definition, construct logarithmic interval coordinates, logical activation weights, and continuous truncation weights: in, Logarithmic interval coordinates For logical activation weights, To continuously truncate the weights, , The minimum allowable distance threshold from the robot's center to the obstacle boundary, when hour, ; right A second-order stability equation is applied to the obstacle row, and it is written as a linear constraint on the robot's generalized acceleration: in, and For positive security reconfiguration gain, and For the obstacle avoidance constraint coefficient row and the obstacle avoidance constraint objective term, by The second derivative, the relative position and velocity of the robot and the obstacle, and the acceleration of the obstacle are determined.
5. The multi-robot formation tracking and obstacle avoidance control method under directed communication topology according to claim 4, characterized in that, S4 specifically includes: excluding virtual navigators, defining the physical follower robot pair set and robot pair boundary spacing: in, For the physical follower robot pair set, and Let be the collision envelope radius of the two robots. For the robot's boundary spacing, the initial state satisfies , The rigid spacing between robots; Using the same logarithmic coordinates and continuous activation construction as the environmental obstacle constraints, linear constraints are formed for each activated robot pair to create an acceleration layer, and they are stacked in a fixed order: in, and For robots The safety constraint row and the right-hand item, and To follow the stacked matrix and vector of the robot pair, the inactive robot pair contributes zero rows and zero right-hand terms.
6. The multi-robot formation tracking and obstacle avoidance control method under directed communication topology according to claim 5, characterized in that, S5 specifically includes: First, divide both ends of each constraint row by a fixed positive reference scale with the same residual units, and then construct the joint safety task stack: in, and Given the safety constraint matrix and the safety constraint objective vector, and The result of stacking for safe passage through environmental obstacles. Used to adjust the relative weights of the two types of safe rows. This indicates that the blocks are stacked vertically by row. Combine the normalized formation line and the joint safety line into the current control task stack: in, and For the current control task matrix and the right-hand side, Weighting for joint security actions.
7. The multi-robot formation tracking and obstacle avoidance control method under directed communication topology according to claim 6, characterized in that, The weighted least squares problem within the reachable set corresponding to S5 is: set up For the actuator to reach the generalized affine set of acceleration, For weight The optimal acceleration is: in, To normalize the sum of squared residuals, It is the Euclidean norm; When joint security action When feasible, the acceleration for any safe passage. The safety residual satisfies: This upper bound indicates that, under fixed states and a fixed set of active rows, the safety residual varies with... The increase approaches zero; When joint security action When internal infeasibility is not possible, define the minimum set of safe residuals: when When it approaches infinity, For any finite cluster point, first minimize the joint security residual, and then minimize the formation residual in the solution set with the minimum security residual; Within a fixed active row interval, if the starting point of the interval satisfies the hard spacing and each active safe row is precisely implemented, then there is no finite-time boundary contact within the interval, and the corresponding conditional safety conclusion is: limited If only residual weights are given, unconditional safety cannot be derived from the large weights themselves if the task stack and the executor reachable acceleration set are incompatible.
8. The multi-robot formation tracking and obstacle avoidance control method under directed communication topology according to claim 7, characterized in that, S6 specifically includes: Stack the blocks diagonally to follow the robot's dynamics, and set the current control task as follows: : in, , , , and These are the stacking amounts of the follower mass matrix, known generalized forces, natural constraints, input matrix, and control input, respectively. Constructed by natural constraints of followers and baseline feasible scaling acceleration Then define the dimension reduction control matrix and the required row corrections: in, This is the quality scaling control constraint matrix. To scale the input action matrix, As the input matrix for dimensionality reduction constraints, Adjust the vector for the required constraint row. The input weight matrix is positive definite. The input for the closed-form least-weighted norm least squares is: The necessary and sufficient condition for the current task stack to be implemented precisely is: When this condition is met, closed-form input is the only minimum function that precisely implements the current task stack. Norm input; if this condition is not met, the closed-form input is the minimum residual solution of the current normalized task stack, and has the minimum among all minimum residual inputs. Norm.