A multi-mobile robot formation control method based on reliable communication edge keeping

Abstract

The application belongs to the technical field of robot control, and particularly relates to a multi-mobile robot formation control method based on reliable communication edge maintenance. The method comprises the following steps: constructing a multi-mobile robot formation operation environment, and generating a collision-free path; performing path tracking according to the collision-free path, and performing speed and formation adaptive adjustment until the formation task is completed. Thus, the problems of communication link reliability reduction, formation error increase, control constraint conflict and task stability deficiency of robots in the process of obstacle circumnavigation, turning and formation transformation are solved.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology, and in particular relates to a multi-mobile robot formation control method based on reliable communication edge maintenance. Background Technology

[0002] With the development of mobile robot technology, wireless communication technology, and autonomous navigation technology, multi-mobile robot systems are increasingly being applied to scenarios such as collaborative handling, environmental inspection, search and rescue, warehousing and transportation, and hazardous area operations. Compared to single-robot systems, multi-mobile robots can improve operational efficiency, expand the perception range, and enhance system fault tolerance through task division and cooperative movement. Formation control is one of the key technologies for achieving collaborative operation of multi-mobile robots. Its basic goal is to enable multiple robots to move according to a preset relative positional relationship and maintain a desired formation while tracking a leader robot or reference trajectory.

[0003] Existing multi-robot formation control methods typically require acquiring state information such as position, heading, and speed of neighboring robots via wireless communication and generating corresponding formation control commands based on the communication topology. Under ideal research conditions, the communication links between robots are usually assumed to be continuously available, or the ability to establish a connection is determined solely by the theoretical maximum communication distance of the wireless devices. However, in real-world robot systems, the reliability of communication links is affected by factors such as communication distance, obstacle obstruction, wireless signal attenuation, packet loss, transmission delay, and network load. Even if a robot remains within the maximum communication range of the wireless device, the transmission quality of its neighbor state information may no longer meet the real-time and stability requirements of formation control.

[0004] In static obstacle environments, the lead and follow robots need to navigate around, turn, and adjust local paths according to the obstacle distribution, causing the relative distance between robots to constantly change. Existing obstacle avoidance and formation control methods typically focus on ensuring safe distances between robots and obstacles, as well as collision avoidance distances between robots, but lack direct constraints on the reliability of critical communication links. When different robots navigate around obstacles separately, or when their formation expands and experiences significant lateral and longitudinal stretching during turns, the distances of some critical communication edges may approach or exceed the reliable communication range, leading to delays, loss, or fluctuations in neighbor state updates. This, in turn, increases formation errors, causes discontinuous control commands, and in severe cases, may lead to communication topology failure and formation task interruption.

[0005] Existing connectivity preservation methods typically aim for overall connectivity of the communication graph or for the distance between robots to remain within a given communication range. While these methods can prevent complete network disconnection to some extent, the communication range they employ is usually treated as a fixed physical communication boundary, failing to adequately distinguish between the maximum communication distance of wireless devices and the effective, reliable communication distance required for real-time formation control. Furthermore, some methods require continuous connectivity between a large number of robots; as the number of robots increases, the number of communication edges and control constraints also increases, easily raising the communication burden and control solution complexity. For critical communication edges that actually undertake state transfer tasks in different formation patterns, existing methods lack targeted dynamic selection and maintenance mechanisms.

[0006] On the other hand, when obstacle spacing is small or local passage space is limited, fixed deployment formations may struggle to simultaneously meet the requirements of obstacle avoidance, robot collision avoidance, and communication link maintenance. Forcibly shortening the distance between robots solely through control inputs may cause conflicts between obstacle avoidance and communication maintenance constraints, leading to sudden speed changes, local oscillations, stagnation, or obstacle avoidance failures. While some existing flexible formation and configuration switching methods can change formation shapes according to the environment, their adjustments are typically determined primarily by obstacle distances or passage widths, failing to consider the reliability of critical communication edges as a crucial basis for formation contraction, column reconfiguration, and formation restoration. Summary of the Invention

[0007] This invention aims to address the problems of insufficient effective and reliable communication boundary characterization, inadequate coordination in maintaining key communication edges, obstacle avoidance, and collision prevention among robots, and weak adaptability of fixed deployment formations to local confined spaces in existing multi-mobile robot formation control methods under static obstacle environments. These problems lead to decreased communication link reliability, increased formation errors, control constraint conflicts, and insufficient task stability during obstacle avoidance, turning, and formation changes. The invention provides a multi-mobile robot formation control method based on reliable communication edge maintenance under static obstacle environments. This method improves the dynamic determination and reliable maintenance of key activity communication edges under different formation modes, enhances the coordinated control performance between obstacle avoidance, collision prevention among robots, and communication maintenance, and improves the system's speed coordination, adaptive formation reconstruction, and stability recovery capabilities after passing through confined areas under communication risks and local space constraints.

[0008] This invention provides a multi-mobile robot formation control method based on reliable communication edge maintenance.

[0009] A multi-mobile robot formation consists of one lead robot and N follower robots, where N is an integer and N≥2;

[0010] Construct a multi-mobile robot formation operation environment and generate a collision-free path; perform path tracking based on the collision-free path and adaptively adjust speed and formation until the formation task is completed.

[0011] In one possible design, constructing a multi-mobile robot formation operating environment and generating collision-free paths includes:

[0012] Set the distribution of the navigating robot, the following robot, and static obstacles;

[0013] Set the initial position and the final position of each mobile robot;

[0014] Based on the initial position and final position of the navigator robot and the distribution of static obstacles, a collision-free path for the navigator robot is generated using the A* algorithm.

[0015] Based on the distance between the current position of the navigating robot and local path points in the collision-free path, as well as the heading deviation, the path point tracking controller generates the nominal linear velocity of the navigating robot. and the nominal angular velocity of the navigation robot .

[0016] In one possible design, path tracking is implemented based on a collision-free path, and adaptive adjustments to speed and formation are made, including:

[0017] Obtain the location information of each mobile robot and construct a set of communication edges;

[0018] When the navigation robot is at nominal linear velocity and nominal angular velocity When traveling along a collision-free path, the navigator robot determines whether to slow down based on the maximum distance between active communication edges in the active communication edge set; the formation pattern is determined and the formation scale is adjusted based on the safe distance between each mobile robot and a static obstacle.

[0019] In one possible design, obtaining the location information of each mobile robot and constructing the communication edge set includes:

[0020] Obtain the position information, heading angle, and speed of each mobile robot;

[0021] Set the reliable communication radius as ,and ,in, Let k be the distance between the i-th mobile robot and the j-th mobile robot during the k-th control cycle, where k is an integer and k≥1;

[0022] ,in, Let i be the position of the i-th mobile robot within the k-th control cycle. This represents the position of the j-th mobile robot within the k-th control cycle. , ,in, Let x be the x-axis coordinate of the i-th mobile robot within the k-th control cycle. Let be the y-axis coordinate of the i-th mobile robot within the k-th control cycle. Let x be the x-coordinate of the robot during the j-th movement in the k-th control cycle. Let y be the y-axis coordinate of the j-th mobile robot within the k-th control cycle. Indicates matrix transpose. This represents the calculation of the Euclidean norm. , , ;

[0023] Based on the location of the mobile robots, determine the active communication edge between the i-th and j-th mobile robots that need to maintain communication. Construct a set of active communication edges .

[0024] In one possible design, determining whether the navigating robot should decelerate based on the maximum distance between active communication edges in the set of active communication edges includes:

[0025] Calculate the maximum distance between active communication edges in the set of active communication edges. ; , This represents the communication edge connecting the i-th mobile robot and the j-th mobile robot;

[0026] Set the threshold for the starting deceleration distance of the navigator robot. and minimum speed ratio distance threshold And satisfy ;

[0027] When the maximum active communication edge distance reaches the threshold for starting deceleration. At this time, the navigator robot decelerates, and the linear velocity of the navigator robot during deceleration is... angular velocity for:

[0028] , ,

[0029] in, This refers to the speed adjustment coefficient of the navigation robot.

[0030] ,in, This represents the minimum speed ratio for the navigating robot. For smooth transition function, To normalize the communication risk coefficient;

[0031] ;in, This indicates that the calculation result is restricted to an interval. Inside.

[0032] In one possible design, determining the formation pattern and adjusting the formation scale based on the safe distance between each mobile robot and the static obstacle includes:

[0033] The desired formation scale is determined based on the safe distance between each mobile robot and the static obstacle;

[0034] Based on the desired formation scale, update the actual formation scale according to the preset formation contraction step size and formation recovery step size, determine whether the actual formation scale is less than the set column scale threshold, if so, select the target position of each follower robot according to the historical trajectory of the mobile robot, otherwise calculate the target position of each follower robot.

[0035] CBF-QP is used to perform safety filtering on the follower robot, and the safe plane velocity of each follower robot is calculated. Each follower robot moves from its current position to the corresponding target position according to the calculated safe plane velocity.

[0036] After all the follower robots have moved to their respective target positions, the position information, heading angle, and speed of each mobile robot are reacquired, and the next control cycle is entered until all mobile robots reach their respective final positions to complete the formation task.

[0037] In one possible design, determining the desired formation scale based on the safe distance between each mobile robot and the static obstacle includes:

[0038] Based on the current position, heading angle, and static obstacle positions of the navigating and following robots, the basic formation scale under the current environmental conditions is determined using a candidate formation safety assessment and threshold judgment method. The basic formation scale The dimensions of the formation to maximize the deployment to meet preset safety conditions;

[0039] Calculate the formation dimensions under communication constraints based on the maximum active communication edge distance. ;

[0040] ,in, For the minimum permissible non-column size, The communication distance at which the set formation begins to contract. To achieve the minimum non-column scale communication distance, and satisfy... ;

[0041] Calculate the desired formation scale ; ;

[0042] The method for updating the actual formation scale according to the desired formation scale and the preset formation contraction and recovery step sizes is as follows:

[0043] ;

[0044] in, Let the desired formation scale be the value for the (k-1)th control cycle. The actual formation scale for the k-th control cycle. The actual formation scale for the (k-1)th control cycle. To shrink the stride length of the formation, To restore the stride length to the formation;

[0045] The actual formation dimensions meet the following requirements: .

[0046] In one possible design, the method for selecting the target position of each following robot based on the historical trajectory of the mobile robot is as follows:

[0047] Select historical trajectory points in the navigator robot's historical trajectory that have accumulated a preset following distance from the navigator robot's current trajectory position as the target position of the first following robot in the current control cycle;

[0048] Select the first In the historical trajectory of the following robot, and the first Historical trajectory points where the cumulative trajectory distance between the current trajectory positions of each following robot reaches a preset following distance are used as the first... The follower robot is positioned at the target location in the current control cycle. .

[0049] In one possible design, the method for calculating the target position of each following robot is as follows:

[0050] ,in, For the k-th control cycle, the target position of the following robot is [position]. To determine the heading angle of the navigator robot in the kth control cycle Constructed two-dimensional rotation matrix, This is the current position of the navigating robot. This indicates that during the k-th control cycle, within a local coordinate system established with the navigator robot as the origin and the navigator robot's forward direction as the positive horizontal axis, the coordinates are determined according to the actual formation scale. The calculated expected relative position of the m-th following robot; ;

[0051] .

[0052] In one possible design, CBF-QP is used for safety filtering of the follower robot, and the safe planar velocity of each follower robot is calculated, including:

[0053] Based on the coordinate difference between the current position and the corresponding target position of each follower robot, a proportional target position tracking method is adopted, and the nominal planar velocity of each follower robot is generated through proportional control and speed limiting.

[0054] Construct collision avoidance constraints and active communication edge preservation constraints for each following robot;

[0055] Collision avoidance constraints are represented as:

[0056] , ;

[0057] in, Let m be the position of the following robot. For the position of the anti-collision object corresponding to the m-th following robot, when the anti-collision object is an obstacle, The point on the obstacle closest to the following robot is used when the collision avoidance target is another robot. To follow the robot's current position; Let be the planar velocity to be optimized for the m-th following robot, and be an unknown two-dimensional decision variable before optimization. To prevent collisions with the object's speed; For the set safety constraint gain, ; Let m be the safety barrier function for the m-th following robot relative to the l-th collision avoidance object. Let be the safe distance between the m-th following robot and its corresponding anti-collision object. When the anti-collision object is another robot... , and They are respectively the m-th following robot and the m-th... The equivalent envelope radius of a following robot For the pre-set additional safety margin, When the object to be avoided is an obstacle, , To provide an additional safety margin between the following robot and static obstacles. ; ;

[0058] The active communication edge maintenance constraint is represented as follows:

[0059] , ;

[0060] in, For the first The position of the robot. For the first The current planar velocity of the following robot, the first Each following robot is a communication neighbor of the m-th following robot; To maintain the set communication gain, ; For active communication edge The corresponding communication security function, , ; To maintain the radius for communication, , The set communication security margin;

[0061] The CBF-QP safety filtering problem is constructed based on collision avoidance constraints and active communication edge preservation constraints, and the safe plane velocity of each following robot is calculated.

[0062] ;in, Let m be the safe planar velocity of the following robot. Let m be the nominal planar velocity of the m-th following robot. and Let be the constraint coefficient vector and constraint boundary, respectively, obtained after linearization of the z-th constraint. For collision avoidance constraints, When the active communication edge maintains constraints; when hour, , ,when hour, , , This indicates that the condition is met;

[0063] All calculated safe planar velocities are limited and converted into safe linear and angular velocities that each following robot can execute;

[0064] Each follower robot moves from its current position to its corresponding target position based on its safe linear velocity and safe angular velocity.

[0065] The beneficial effects of this invention are:

[0066] This invention addresses the challenges of increased critical communication edge distances and decreased communication link reliability in multi-mobile robot formations during obstacle avoidance, turning, and formation changes in static obstacle environments. It also addresses the difficulty of simultaneously meeting obstacle avoidance, collision prevention, and communication maintenance requirements with fixed formations. The invention proposes a multi-mobile robot formation control method based on reliable communication edge maintenance. This method establishes effective and reliable communication boundaries, dynamically determines the active communication edges that need to be maintained under different formation modes, and combines communication maintenance constraints, lead robot speed adjustment, and formation adaptive contraction, column reconstruction, and recovery mechanisms to achieve coordinated control of obstacle avoidance, robot collision prevention, and critical communication link maintenance. This improves the motion stability, obstacle clearance capability, and communication reliability of multi-mobile robot formations in static obstacle environments.

[0067] This invention adaptively adjusts the formation scale and smoothly decelerates the lead robot based on the margin between the distance of the active communication edge and the reliable communication radius, thereby reducing the risk of the critical communication edge being overstretched during the robot's obstacle avoidance, turning and formation changes.

[0068] Based on changes in local obstacle space and path, this invention adaptively reconstructs between expanded formation, contracted triangular formation, and column formation. It also uses CBF-QP to uniformly handle obstacle avoidance, robot collision prevention, and communication constraints, enabling robot formations to safely pass through narrow areas and continuous turning scenarios.

[0069] This invention enables the robot to smoothly transition between unfolding formations, contracting triangular formations, and column formations by continuously adjusting the formation scale, limiting the amount of change of the target being followed within adjacent control cycles, and adopting a gradual recovery method. This reduces the sudden turns, oscillations, and sudden increases in formation errors caused by formation switching. Attached Figure Description

[0070] Figure 1 A flowchart of a multi-mobile robot formation control method based on reliable communication edge maintenance is provided in an embodiment of the present invention;

[0071] Figure 2 The following are schematic diagrams of three formation communication topologies for a multi-mobile robot formation control method based on reliable communication edge maintenance provided in this embodiment of the invention: (a) is a schematic diagram of normal triangular formation communication topology, (b) is a schematic diagram of shrinking triangular formation communication topology, and (c) is a schematic diagram of column formation communication topology.

[0072] Figure 3 The robot trajectory in a static obstacle environment provided in the embodiments of the present invention;

[0073] Figure 4 This is a curve showing the maximum active communication edge distance variation provided in an embodiment of the present invention;

[0074] Figure 5 The present invention provides curves showing the variation of active communication edge distance under different reliable communication radii in embodiments of the invention; wherein, (a) is the reliable communication radius. (a) Maximum active communication edge distance; (b) Reliable communication radius (c) is the maximum active communication edge distance; (d) is the reliable communication radius. Maximum active communication edge distance; (d) is the reliable communication radius. Maximum active communication edge distance;

[0075] Figure 6 The following are ablation comparison diagrams of different communication preservation mechanisms provided in the embodiments of the present invention; wherein, (a) is a comparison diagram of the maximum active communication edge distance; and (b) is a comparison diagram of the active communication edge over-limit rate. Detailed Implementation

[0076] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention. It should be noted that, unless otherwise specified, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0077] refer to Figure 1 This invention provides a multi-mobile robot formation control method based on reliable communication edge maintenance;

[0078] A multi-mobile robot formation consists of one lead robot and N follower robots, where N is an integer and N≥2;

[0079] Construct a multi-mobile robot formation operation environment and generate a collision-free path; perform path tracking based on the collision-free path and adaptively adjust speed and formation until the formation task is completed.

[0080] In one implementation, constructing a multi-mobile robot formation operating environment and generating collision-free paths includes:

[0081] Set the distribution of the navigating robot, the following robot, and static obstacles;

[0082] Set the initial position and the final position of each mobile robot;

[0083] Based on the initial position and final position of the navigator robot and the distribution of static obstacles, a collision-free path for the navigator robot is generated using the A* algorithm.

[0084] Based on the distance between the current position of the navigating robot and local path points in the collision-free path, as well as the heading deviation, the path point tracking controller generates the nominal linear velocity of the navigating robot. and the nominal angular velocity of the navigation robot .

[0085] Specifically, in the simulation environment, the distribution of the navigator robot, the follower robot, and static obstacles is set, and the initial position, target position, desired formation, and speed limit of each robot are set.

[0086] In one implementation, path tracking based on a collision-free path and adaptive adjustments to speed and formation include:

[0087] Obtain the location information of each mobile robot and construct a set of communication edges;

[0088] When the navigation robot is at nominal linear velocity and nominal angular velocity When traveling along a collision-free path, the navigator robot determines whether to slow down based on the maximum distance between active communication edges in the active communication edge set; the formation pattern is determined and the formation scale is adjusted based on the safe distance between each mobile robot and a static obstacle.

[0089] In one implementation, obtaining the location information of each mobile robot and constructing the communication edge set includes:

[0090] Obtain the position information, heading angle, and speed of each mobile robot;

[0091] The reliable communication radius is set to meet the real-time and stability requirements of formation control. For the currently active communication edge, its reliable communication condition is expressed as: ,in, Let k be the distance between the i-th mobile robot and the j-th mobile robot during the k-th control cycle, where k is an integer and k≥1;

[0092] Specifically, reliable communication radius This refers to the pre-defined reliable communication radius based on reliable communication requirements. During system initialization, it should be ensured that the selected initial active communication edge meets the distance requirements; subsequently, the active communication edge is maintained through formation shrinkage, deceleration of the lead robot, and communication-maintaining CBF constraints.

[0093] ,in, Let i be the position of the i-th mobile robot within the k-th control cycle. This represents the position of the j-th mobile robot within the k-th control cycle. , ,in, Let x be the x-axis coordinate of the i-th mobile robot within the k-th control cycle. Let be the y-axis coordinate of the i-th mobile robot within the k-th control cycle. Let x be the x-axis coordinate of the j-th mobile robot within the k-th control cycle. Let y be the y-axis coordinate of the j-th mobile robot within the k-th control cycle. Indicates matrix transpose. This represents the calculation of the Euclidean norm. , , ;

[0094] Based on the location of the mobile robots, determine the active communication edge between the i-th and j-th mobile robots that need to maintain communication. Construct a set of active communication edges .

[0095] In one implementation, determining whether the navigation robot should decelerate based on the maximum distance between active communication edges in the set of active communication edges includes:

[0096] As the navigating robot moves along the planned path, to measure how close the current active communication edge is to the reliable communication radius, the distance of the maximum active communication edge in the set of active communication edges is calculated. ; ;

[0097] Specifically, The closer to the reliable communication radius, the greater the stretching of the current active communication edge. The more the lead robot needs to reduce its speed, the more time the follower robot needs to catch up and adjust its formation.

[0098] Set the threshold for the starting deceleration distance of the navigator robot. and minimum speed ratio distance threshold And satisfy ;

[0099] When the maximum active communication edge distance reaches the threshold for starting deceleration. At this time, the navigator robot decelerates, and the linear velocity of the navigator robot during deceleration is... angular velocity for:

[0100] , ,

[0101] in, This refers to the speed adjustment coefficient of the navigation robot.

[0102] ,in, This represents the minimum speed ratio for the navigating robot. For smooth transition function, Used to reduce sudden changes in the speed of the navigating robot. To normalize the communication risk coefficient;

[0103] ;in, This indicates that the calculation result is restricted to an interval. Inside.

[0104] Specifically, when the maximum active communication edge distance is within a safe range The navigator robot moves at the nominal speed; as the active communication edge gradually approaches the reliable communication radius, the speed adjustment coefficient decreases smoothly, thereby reducing the speed at which the active communication edge continues to increase.

[0105] In one implementation, determining the formation pattern and adjusting the formation scale based on the safe distance between each mobile robot and a static obstacle includes:

[0106] The desired formation scale is determined based on the safe distance between each mobile robot and the static obstacle;

[0107] Based on the desired formation scale, update the actual formation scale according to the preset formation contraction step size and formation recovery step size, determine whether the actual formation scale is less than the set column scale threshold, if so, select the target position of each follower robot according to the historical trajectory of the mobile robot, otherwise calculate the target position of each follower robot.

[0108] CBF-QP is used to perform safety filtering on the follower robot, and the safe plane velocity of each follower robot is calculated. Each follower robot moves from its current position to the corresponding target position according to the calculated safe plane velocity.

[0109] After all the follower robots have moved to their respective target positions, the position information, heading angle, and speed of each mobile robot are reacquired, and the next control cycle is entered until all mobile robots reach their respective final positions to complete the formation task.

[0110] In one implementation, determining the desired formation scale based on the safe distance between each mobile robot and a static obstacle includes:

[0111] Based on the current position, heading angle, and static obstacle positions of the navigating and following robots, the basic formation scale under the current environmental conditions is determined using a candidate formation safety assessment and threshold judgment method. The basic formation scale The dimensions of the formation to maximize the deployment to meet preset safety conditions;

[0112] Specifically, the basic formation scale is determined through a candidate formation safety assessment and threshold judgment method. The formation scales corresponding to the normal triangular formation, the contracted triangular formation, and the column formation, the expected relative position of each follower robot under different formations, the obstacle safety distance, the obstacle distance threshold of the navigator robot, and the local path turning threshold are all preset values; the current position, heading angle, static obstacle position, and current local path point of the navigator robot and the follower robots are acquired in real time; based on the real-time acquired values, the target position of the follower robot under each candidate formation, the minimum safety margin of the candidate target position relative to the obstacle, the distance between the navigator robot and the nearest obstacle, and the degree of local path turning are calculated respectively, and the formation with the largest expansion degree is selected from the candidate formations that meet the preset safety conditions, and its corresponding formation scale is determined as the current basic formation scale;

[0113] A normal triangular formation corresponds to a larger formation scale, a contracting triangular formation corresponds to a smaller formation scale, and a column formation corresponds to the smallest formation scale. After the basic formation is determined, in order to avoid the active communication edge gradually approaching the reliable communication radius during the movement, the formation scale allowed by the communication conditions is further calculated based on the distance of the maximum active communication edge.

[0114] Calculate the formation dimensions under communication constraints based on the maximum active communication edge distance. ;

[0115] ,in, For the minimum permissible non-column size, The communication distance at which the set formation begins to contract. To achieve the minimum non-column scale communication distance, and satisfy... When the maximum active communication edge distance is small, As the active communication edge gradually approaches the reliable communication boundary, Smooth decrease.

[0116] Calculate the desired formation size by considering both obstacle environment and communication conditions. ; The actual formation size is gradually adjusted according to the principle of faster contraction and slower recovery in order to avoid sudden changes in formation.

[0117] The method for updating the actual formation scale according to the desired formation scale and the preset formation contraction and recovery step sizes is as follows:

[0118] ;

[0119] in, Let the desired formation scale be the value for the (k-1)th control cycle. The actual formation scale for the k-th control cycle. The actual formation scale for the (k-1)th control cycle. To shrink the stride length of the formation, To restore the stride length to the formation;

[0120] The actual formation dimensions meet the following requirements: .

[0121] In one implementation, the method for selecting the target position of each following robot based on the historical trajectory of the mobile robot is as follows:

[0122] Select historical trajectory points in the navigator robot's historical trajectory that have accumulated a preset following distance from the navigator robot's current trajectory position as the target position of the first following robot in the current control cycle;

[0123] Select the first In the historical trajectory of the following robot, and the first Historical trajectory points where the cumulative trajectory distance between the current trajectory positions of each following robot reaches a preset following distance are used as the first... The follower robot is positioned at the target location in the current control cycle. .

[0124] In one implementation, the method for calculating the target position of each following robot is as follows:

[0125] ,in, Let m be the target position of the following robot in the k-th control cycle. To determine the heading angle of the navigator robot in the kth control cycle Constructed two-dimensional rotation matrix, This is the current position of the navigating robot. This indicates that during the k-th control cycle, within a local coordinate system established with the navigator robot as the origin and the navigator robot's forward direction as the positive horizontal axis, the coordinates are determined according to the actual formation scale. The calculated expected relative position of the m-th following robot; ;

[0126] Specifically, for a three-robot formation, the expected relative positions of the first and second follower robots are as follows:

[0127] ,

[0128] in, For the following distance, This refers to the horizontal expansion distance;

[0129] ,in, Let be the sine value of the heading angle of the navigating robot in the k-th control cycle. Let be the cosine value of the heading angle of the navigating robot in the kth control cycle.

[0130] Specifically, when communication risks increase, the backward following distance should be reduced and the lateral deployment width should be appropriately reduced; when local space is limited, the lateral deployment width should be reduced primarily.

[0131] Taking a three-robot formation as an example, when the minimum safe distance between the target position of the following robot and surrounding obstacles in a contracting triangular formation is less than the column trigger safety distance threshold, the formation control node adjusts the target formation scale to the column scale, so that the actual formation scale smoothly decreases from the contracting triangular formation scale. When the actual formation scale further decreases to the column switching threshold, the formation switch to column is formally triggered and historical trajectory tracking is enabled. At this time, the target position of the first following robot is selected from the historical trajectory of the lead robot, and the target position of the second following robot is selected from the historical trajectory of the first following robot, so that the two following robots move sequentially along the path already traversed by the preceding robot. The target position is smoothly limited within adjacent control cycles to reduce the target abrupt change caused by formation switching. A nominal planar velocity is generated based on the error between the current position and the target position of the following robot, and it is used as the reference input for CBF-QP safety filtering in the next step. The nominal angular velocity is used to adjust the heading of the following robot.

[0132] In one implementation, CBF-QP is used for safety filtering of the following robots, and the calculation of the safe planar velocity for each following robot includes:

[0133] Based on the coordinate difference between the current position and the corresponding target position of each follower robot, a proportional target position tracking method is adopted. The nominal planar velocity of each follower robot is generated through proportional control and speed limiting. The nominal planar velocity of the follower robot includes the nominal velocity component in the x-direction and the nominal velocity component in the y-direction in the global coordinate system.

[0134] Construct collision avoidance constraints and active communication edge preservation constraints for each following robot;

[0135] Collision avoidance constraints are represented as:

[0136] , ;

[0137] in, Let m be the position of the following robot. For the position of the anti-collision object corresponding to the m-th following robot, when the anti-collision object is an obstacle, The point on the obstacle that is closest to the following robot is used when the collision avoidance target is another following robot. To follow the robot's current position; Let be the planar velocity to be optimized for the m-th following robot, which is an unknown two-dimensional decision variable before optimization. Its reference value is the nominal planar velocity of the m-th following robot. To prevent collisions with the speed of the object, when the object is an obstacle... ; For the set safety constraint gain, ; Let m be the safety barrier function for the m-th following robot relative to the l-th collision avoidance object. Let be the safe distance between the m-th following robot and its corresponding anti-collision object. When the anti-collision object is another following robot... , and They are respectively the m-th following robot and the m-th... The equivalent envelope radius of a following robot For the pre-set additional safety margin, When the object to be avoided is an obstacle, , To provide an additional safety margin between the following robot and static obstacles, ; ;

[0138] Specifically, when the object of the anti-collision constraint is an obstacle, the anti-collision constraint is an obstacle avoidance constraint; when the object of the anti-collision constraint is a mobile robot, the anti-collision constraint is an anti-collision constraint between mobile robots.

[0139] The active communication edge maintenance constraint is represented as follows:

[0140] , ;

[0141] in, For the first The position of the robot. For the first The current planar velocity of the following robot, the first Each following robot is a communication neighbor of the m-th following robot; To maintain the set communication gain, ; For active communication edge The corresponding communication security function, , ; To maintain the radius for communication, , For the set communication security margin, Used to keep the currently active communication edge within a valid and reliable communication range;

[0142] Specifically, When, it means that the m-th following robot is related to the m-th... The distance between the following robots did not exceed the communication maintenance radius. To facilitate the construction of CBF-QP safety constraints, the position layer motion of the m-th following robot is equivalently represented as a single integrator in the safety filtering layer. in, Let m be the planar position of the m-th following robot. This refers to the plane velocity to be optimized in CBF-QP. Within the current control cycle, and The active communication neighbors obtained by the formation control node are respectively The current position and current planar velocity; the active communication edge holding constraint is used to limit the m-th following robot from continuing to move away from its active communication neighbor, so that the active communication edge remains within the communication holding radius.

[0143] Specifically, the collision avoidance constraint and the active communication edge preservation constraint are uniformly written as a linear inequality. and the obtained nominal planar velocity of the following robot To optimize the reference, a CBF-QP security filtering problem was constructed;

[0144] The CBF-QP safety filtering problem is constructed based on collision avoidance constraints and active communication edge preservation constraints, and the safe plane velocity of each following robot is calculated.

[0145] ;in, Let m be the optimal safe plane velocity for the m-th following robot that satisfies all safety constraints and minimizes the deviation from the nominal plane velocity. Let be the nominal planar velocity of the m-th following robot, used as the reference input for CBF-QP; To optimize the decision variables in the process, and Let be the constraint coefficient vector and constraint boundary, respectively, obtained after linearization of the z-th constraint. For collision avoidance constraints, When the active communication edge maintains constraints; when hour, , ,when hour, , , This indicates that the condition is met;

[0146] Specifically, the z-th constraint is linearized to obtain the constraint coefficient vector. and constraint boundaries The linearization includes algebraic expansion and rearrangement; to facilitate the construction of CBF-QP, the planar positional motion following the robot is equivalently represented as a single integrator model in the safety filtering layer. Under this equivalent model, the CBF (control obstacle function) condition is related to the planar velocity to be optimized. The relationship is linear. Therefore, by simply performing an algebraic expansion and rearranging of the collision avoidance constraints and communication maintenance constraints according to the plane velocity to be optimized, the linear inequality used by the QP (quadratic programming) solver can be obtained. .

[0147] All calculated safe planar velocities are limited and converted into safe linear and angular velocities that each following robot can execute;

[0148] Specifically, after solving the CBF-QP optimization problem, the result is used as the safe planar velocity of the following robot, and further converted into linear velocity and angular velocity control commands that the following robot can execute;

[0149] Each follower robot moves from its current position to its corresponding target position based on its safe linear velocity and safe angular velocity.

[0150] Specifically, when following the robot in a column formation to track the historical trajectory, the historical trajectory point in the historical trajectory of the lead robot whose cumulative trajectory distance between the lead robot and the current trajectory position reaches a preset following distance is selected as the target position of the first follow robot in the current control cycle. The first follow robot continuously tracks the target position to achieve movement along the historical trajectory of the lead robot.

[0151] Select the first In the historical trajectory of the following robot, and the first Historical trajectory points where the cumulative trajectory distance between the current trajectory positions of each following robot reaches a preset following distance are used as the first... The following robot is at the target position in the current control cycle. A following robot continuously tracks the target position, achieving [following the target position along the [path]... The robot follows the historical trajectory of its movement.

[0152] In both normal and contracted triangular formations, the target position of the follower robot is recalculated each control cycle based on the latest position, heading angle, and current formation scale of the navigator robot. The follower robot continuously tracks the constantly updated position of the moving target, thus maintaining formation movement with the navigator robot.

[0153] After the follower robot moves to the corresponding target position, the system reacquires the states of all mobile robots, obstacle safety margins, and active communication edge distances, and then enters the next control cycle. When the formation passes through a locally restricted area, and the degree of turning on the local path decreases, the obstacle safety margin corresponding to the contracted triangular formation reaches the recovery threshold, and the active communication edge remains within the reliable communication range, the target formation scale is gradually increased from the column scale to the contracted triangular formation scale, so that the target position of the follower robot smoothly transitions from the historical trajectory point to the contracted triangular formation target position. When the obstacle safety margin corresponding to the contracted triangular formation further reaches the recovery threshold, and the reliable communication conditions are continuously met, the target formation scale is then gradually increased to the normal triangular formation scale. During the recovery process, the formation scale and target position are smoothed and limited; if the safety or communication conditions are no longer met, the deployment is paused or the formation is re-contracted. During the recovery process, the rate of change of the formation scale and the follower target position is limited, and the output control commands are smoothed to reduce the error surge caused by rapid formation deployment.

[0154] The formation task is complete when all mobile robots reach their respective final positions. After the formation task is completed, all mobile robots stop.

[0155] Example:

[0156] Combination Figures 2 to 6 According to the present invention, a multi-mobile robot formation control method based on reliable communication edge maintenance is used to perform formation control of three mobile robots;

[0157] Select one mobile robot as the lead robot, and the other two mobile robots are the first follower robot and the second follower robot, respectively;

[0158] For a three-mobile-robot system, the set of active communication edges is:

[0159] ;

[0160] In this designation, number 0 represents the lead robot, and numbers 1 and 2 represent the first and second follower robots, respectively. This indicates the current formation mode. In the column formation mode, direct communication between the lead robot and the second follower robot is not required. Instead, a chain-like active communication topology is adopted, in which the lead robot, the first follower robot, and the second follower robot are connected in sequence.

[0161] like Figure 2 As shown, when performing formation control on three mobile robots, the communication topology is fully connected when the formation is a normal triangular formation, adaptively shrinks according to the environmental margin and the reliable communication radius when the formation is a shrinking triangular formation, and the communication topology is chained when the formation is a vertical formation.

[0162] Constructing the operating environment for the mobile robot, the trajectories and routes of the navigator robot and the two follower robots are as follows: Figure 3 As shown;

[0163] When the reliable communication radius is set to 0.78m, the curves showing the change in the maximum active communication edge distance during the operation of the three mobile robots are as follows: Figure 4 As shown;

[0164] Different reliable communication radii result in different curves for the change of the maximum active communication edge distance during the operation of the mobile robot; Figure 5 (a) shows the reliable communication radius. The curve showing the change in the maximum active communication edge distance is shown in the figure. The maximum active communication edge distance is 0.766m, the number of points exceeding the boundary is 0, and the boundary rate is 0.00%. Figure 5 (b) shows the reliable communication radius. The curve showing the change in the maximum active communication edge distance is shown in the figure. The maximum active communication edge distance is 0.772m, the number of out-of-bounds points is 0, and the out-of-bounds rate is 0.00%. Figure 5 (c) shows the reliable communication radius. The curve showing the change in the maximum active communication edge distance is shown in the figure. The maximum active communication edge distance is 0.786m, the number of points exceeding the boundary is 0, and the boundary rate is 0.00%. Figure 5 (d) shows the reliable communication radius. The curve showing the change in the maximum active communication edge distance is shown in the figure. The maximum active communication edge distance is 0.800m, the number of out-of-bounds points is 0, and the out-of-bounds rate is 0.00%.

[0165] When reliable communication radius At the same time, the ablation comparison diagram of different communication-preservation mechanisms is as follows: Figure 6 As shown, where, Figure 6 (a) is the reliable communication radius (a) Comparison of maximum active communication edge distance; (b) Reliable communication radius Comparison chart of out-of-bounds rates for active communication edges; based on Figure 6 It can be seen that when the multi-mobile robot formation control method based on reliable communication edge maintenance proposed in this invention (i.e., the complete method in the figure) is adopted, the maximum active communication edge distance is maintained within the reliable communication radius. Within the range and with an out-of-bounds rate of 0 for active communication edges, this invention demonstrates that the proposed method for multi-mobile robot formation control based on reliable communication edge maintenance can effectively ensure the reliability of communication links during obstacle avoidance, turning, and formation changes, and improve the system's speed coordination, adaptive formation reconstruction, and stable recovery capabilities after passing through restricted areas under conditions of communication risk and local space constraints.

[0166] While the invention has been described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the invention. Therefore, it should be understood that many modifications can be made to the exemplary embodiments, and other arrangements can be designed without departing from the spirit and scope of the invention as defined by the appended claims. It should be understood that different dependent claims and features described herein can be combined in ways different from those described in the original claims. It is also understood that features described in conjunction with individual embodiments can be used in other described embodiments.

Claims

1. A multi-mobile robot formation control method based on reliable communication edge maintenance, characterized in that: A multi-mobile robot formation consists of one lead robot and N follower robots, where N is an integer and N≥2; Construct a multi-mobile robot formation operation environment and generate collision-free paths; Path tracking is implemented based on collision-free paths. By establishing reliable communication boundaries, the active communication edges that need to be maintained under different formation modes are dynamically determined, and speed and formation are adaptively adjusted until the formation task is completed.

2. The multi-mobile robot formation control method based on reliable communication edge maintenance according to claim 1, characterized in that, Constructing a multi-mobile robot formation operating environment and generating collision-free paths includes: Set the distribution of the navigating robot, the following robot, and static obstacles; Set the initial position and the final position of each mobile robot; Based on the initial position and final position of the navigator robot and the distribution of static obstacles, a collision-free path for the navigator robot is generated using the A* algorithm. Based on the distance between the current position of the navigating robot and local path points in the collision-free path, as well as the heading deviation, the path point tracking controller generates the nominal linear velocity of the navigating robot. and the nominal angular velocity of the navigation robot .

3. The multi-mobile robot formation control method based on reliable communication edge maintenance according to claim 2, characterized in that, Path tracking is performed based on a collision-free path, and adaptive adjustments to speed and formation are made, including: Obtain the location information of each mobile robot and construct a set of communication edges; When the navigation robot is at nominal linear velocity and nominal angular velocity When traveling along a collision-free path, the navigator robot determines whether to slow down based on the maximum distance between active communication edges in the active communication edge set; the formation pattern is determined and the formation scale is adjusted based on the safe distance between each mobile robot and a static obstacle.

4. The multi-mobile robot formation control method based on reliable communication edge maintenance according to claim 3, characterized in that, Obtain the location information of each mobile robot and construct the communication edge set, including: Obtain the position information, heading angle, and speed of each mobile robot; Set the reliable communication radius as ,and ,in, Let k be the distance between the i-th mobile robot and the j-th mobile robot during the k-th control cycle, where k is an integer and k≥1; ,in, Let i be the position of the i-th mobile robot within the k-th control cycle. This represents the position of the j-th mobile robot within the k-th control cycle. , ,in, Let x be the x-axis coordinate of the i-th mobile robot within the k-th control cycle. Let be the y-axis coordinate of the i-th mobile robot within the k-th control cycle. Let x be the x-axis coordinate of the j-th mobile robot within the k-th control cycle. Let y be the y-axis coordinate of the j-th mobile robot within the k-th control cycle. Indicates matrix transpose. This represents the calculation of the Euclidean norm. , , ; Based on the location of the mobile robots, determine the active communication edge between the i-th and j-th mobile robots that need to maintain communication. Construct a set of active communication edges .

5. The multi-mobile robot formation control method based on reliable communication edge maintenance according to claim 4, characterized in that, Determining whether the navigation robot should decelerate based on the maximum distance of the active communication edges in the set of active communication edges includes: Calculate the set of active communication edges Maximum active communication edge distance ; ; Set the threshold for the starting deceleration distance of the navigator robot. and minimum speed ratio distance threshold And satisfy ; When the maximum active communication edge distance reaches the threshold for starting deceleration. At this time, the navigator robot decelerates, and the linear velocity of the navigator robot during deceleration is... angular velocity for: ， ， in, This refers to the speed adjustment coefficient of the navigation robot. ,in, This represents the minimum speed ratio for the navigating robot. For smooth transition function, To normalize the communication risk coefficient; ;in, This indicates that the calculation result is restricted to an interval. Inside.

6. The multi-mobile robot formation control method based on reliable communication edge maintenance according to claim 5, characterized in that, Determining the formation pattern and adjusting the formation scale based on the safe distance between each mobile robot and static obstacles includes: The desired formation scale is determined based on the safe distance between each mobile robot and the static obstacle; Based on the desired formation scale, update the actual formation scale according to the preset formation contraction step size and formation recovery step size, determine whether the actual formation scale is less than the set column scale threshold, if so, select the target position of each follower robot according to the historical trajectory of the mobile robot, otherwise calculate the target position of each follower robot. CBF-QP is used to perform safety filtering on the follower robot, and the safe plane velocity of each follower robot is calculated. Each follower robot moves from its current position to the corresponding target position according to the calculated safe plane velocity. After all the follower robots have moved to their respective target positions, the position information, heading angle, and speed of each mobile robot are reacquired, and the next control cycle is entered until all mobile robots reach their respective final positions to complete the formation task.

7. The multi-mobile robot formation control method based on reliable communication edge maintenance according to claim 6, characterized in that: The desired formation scale is determined based on the safe distance between each mobile robot and the static obstacle, including: Based on the current position, heading angle, and static obstacle positions of the navigating and following robots, the basic formation scale under the current environmental conditions is determined using a candidate formation safety assessment and threshold judgment method. The basic formation scale The dimensions of the formation to maximize the deployment to meet preset safety conditions; Calculate the formation dimensions under communication constraints based on the maximum active communication edge distance. ; ,in, For the minimum permissible non-column size, The communication distance at which the set formation begins to contract. To achieve the minimum non-column scale communication distance, and satisfy... ; Calculate the desired formation scale ; ; The method for updating the actual formation scale according to the desired formation scale and the preset formation contraction and recovery step sizes is as follows: ； in, Let the desired formation scale be the value for the (k-1)th control cycle. The actual formation scale for the k-th control cycle. The actual formation scale for the (k-1)th control cycle. To shrink the stride length of the formation, To restore the stride length to the formation; The actual formation dimensions meet the following requirements: .

8. The multi-mobile robot formation control method based on reliable communication edge maintenance according to claim 7, characterized in that, The method for selecting the target position of each following robot based on the historical trajectory of the mobile robot is as follows: Select historical trajectory points in the navigator robot's historical trajectory that have accumulated a preset following distance from the navigator robot's current trajectory position as the target position of the first following robot in the current control cycle; Select the first In the historical trajectory of the following robot, and the first Historical trajectory points where the cumulative trajectory distance between the current trajectory positions of each following robot reaches a preset following distance are used as the first... The follower robot is positioned at the target location in the current control cycle. .

9. A multi-mobile robot formation control method based on reliable communication edge maintenance according to claim 8, characterized in that, The method for calculating the target position of each following robot is as follows: ,in, Let m be the target position of the following robot in the k-th control cycle. To determine the heading angle of the navigator robot in the kth control cycle Constructed two-dimensional rotation matrix, This is the current position of the navigating robot. This indicates that during the k-th control cycle, within a local coordinate system established with the navigator robot as the origin and the navigator robot's forward direction as the positive horizontal axis, the coordinates are determined according to the actual formation scale. The calculated expected relative position of the m-th following robot; ; 。 10. A multi-mobile robot formation control method based on reliable communication edge maintenance according to claim 8 or 9, characterized in that, CBF-QP is used for safety filtering of the follower robots, and the safe planar velocity of each follower robot is calculated, including: Based on the coordinate difference between the current position and the corresponding target position of each follower robot, a proportional target position tracking method is adopted, and the nominal planar velocity of each follower robot is generated through proportional control and speed limiting. Construct collision avoidance constraints and active communication edge preservation constraints for each following robot; Collision avoidance constraints are represented as: ， ； in, Let m be the position of the following robot. For the position of the anti-collision object corresponding to the m-th following robot, when the anti-collision object is an obstacle, The point on the obstacle that is closest to the following robot is used when the collision avoidance target is another following robot. To follow the robot's current position; Let m be the planar velocity to be optimized for the m-th following robot. To prevent collisions with the object's speed; For the set safety constraint gain, ; Let m be the safety barrier function for the m-th following robot relative to the l-th collision avoidance object. Let be the safe distance between the m-th following robot and its corresponding anti-collision object. When the anti-collision object is another following robot... , and They are respectively the m-th following robot and the m-th... The equivalent envelope radius of a following robot For the pre-set additional safety margin, When the object to be avoided is an obstacle, , To provide an additional safety margin between the following robot and static obstacles. ; ; The active communication edge maintenance constraint is represented as follows: ， ； in, For the first The position of the robot. For the first The current planar velocity of the following robot, the first Each following robot is a communication neighbor of the m-th following robot; To maintain the set communication gain, ; For active communication edge The corresponding communication security function, , ; To maintain the radius for communication, , The set communication security margin; The CBF-QP safety filtering problem is constructed based on collision avoidance constraints and active communication edge preservation constraints, and the safe plane velocity of each following robot is calculated. ;in, Let m be the safe planar velocity of the following robot. Let m be the nominal planar velocity of the m-th following robot. Let be the decision variable in the optimization process, representing the planar velocity to be optimized for the m-th following robot. For a two-dimensional set of real numbers, and Let be the constraint coefficient vector and constraint boundary, respectively, obtained after linearization of the z-th constraint. For collision avoidance constraints, When the active communication edge maintains constraints; when hour, , ,when hour, , , This indicates that the condition is met; All calculated safe planar velocities are limited and converted into safe linear and angular velocities that each following robot can execute; Each follower robot moves from its current position to its corresponding target position based on its safe linear velocity and safe angular velocity.

Images