A flexible dynamic robust formation cooperative control method based on control obstacle function

Abstract

The present invention provides a method for coordinated control of an elastic dynamic robust formation based on a control obstacle function, and belongs to the technical field of unmanned system control. The method comprises the following steps: Step S1, establishing a motion dynamics model for each unmanned system, and determining the position information and motion speed of each unmanned system; Step S2, using a control obstacle function to ensure a collision-free safety set between the unmanned system and static obstacles and dynamic unmanned systems, and solving the input safety set of the control system based on forward invariance; Step S3, maintaining the formation based on the control obstacle function; Step S4, performing coordinated control of the formation based on the control obstacle function. The present invention adopts the above-mentioned method for coordinated control of an elastic dynamic robust formation based on a control obstacle function, which can effectively deal with uncertainties and random obstacles in unknown dynamic environments, and improve the coordinated control performance of multiple unmanned systems.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned system control, and in particular to a flexible dynamic robust formation collaborative control method based on a control obstacle function. Background Art

[0002] In recent years, multi-unmanned systems have many advantages over single unmanned systems, including stronger robustness in the face of failures and higher flexibility in completing tasks. Therefore, the coordinated control of multi-unmanned systems has attracted widespread attention. In the task of coordinated control of multi-unmanned systems, the control goal is to achieve and maintain the specified distance constraints between the unmanned systems. In this case, it is usually necessary to assign a specific tolerance range to the specified distance between the unmanned systems, thereby obtaining a fixed value of the minimum tolerance range and an elastic value of the larger tolerance range.

[0003] In order to ensure the scalability of multiple unmanned systems and the robustness of single unmanned systems, it is necessary to design distributed controllers to meet potential mission requirements. First, the unmanned systems need to be as close to the specified distance as possible, and under necessary conditions, the distance between unmanned systems needs to be within the specified tolerance range. In addition, in order to ensure the safety of unmanned system navigation in a dynamically changing environment, the measurement factors of onboard sensors need to be considered.

[0004] At present, scholars have proposed a variety of research methods to solve the problem of dynamic robust formation cooperative control, including the use of stress matrix, complex Laplace operator, etc., to guide the unmanned system to complete the continuous adjustment of the formation control process. In addition, some scholars have also studied the formation cooperative control problem in a constraint-based collaborative framework, in which the formation maintenance is achieved by converting the control barrier function (CBF) into the constraints of the optimization problem. However, the above methods often face the following problems:

[0005] (1) During the formation maneuver control process: methods such as stress matrix and complex Laplace operator do not clearly determine whether the distance of the unmanned system during the maneuver is within a specific tolerance range. The obstacles during the maneuver are also predetermined, which cannot meet the autonomous and dynamic real-time adaptation to the environment, and it is difficult to complete tasks such as avoiding collisions with detected obstacles.

[0006] (2) In the process of collaborative control of formations: Methods such as the collaborative formation control framework based on CBF constraints usually assume that the obstacles encountered in the process are known. Therefore, such methods are difficult to deal with unknown obstacles in dynamic and complex environments, and have problems in completing flexible changes in formations.

[0007] In summary, many current technical means have considered the problem of dynamic robust formation collaborative control of unmanned systems, but their ability to handle the uncertainty and random obstacles faced in unknown dynamic environments is limited. Summary of the invention

[0008] The purpose of the present invention is to provide a flexible dynamic robust formation collaborative control method based on a control obstacle function, which can effectively cope with the uncertainty and random obstacles in an unknown dynamic environment and improve the collaborative control performance of multiple unmanned systems.

[0009] To achieve the above object, the present invention provides a flexible dynamic robust formation collaborative control method based on a control obstacle function, comprising the following steps:

[0010] Step S1, establishing a motion dynamics model for each unmanned system, and determining the position information and motion speed of each unmanned system;

[0011] Step S2: using the control obstacle function to ensure the collision-free safety set between the unmanned system and the static obstacles and the dynamic unmanned system, and solving the input safety set of the control system based on forward invariance;

[0012] Step S3, by introducing dynamic tolerance and defining related control obstacle functions, establishing an upper level set to maintain the desired distance between unmanned systems and the maximum dynamic tolerance range;

[0013] Step S4: A quadratic programming problem is used to solve the control input of the formation control. The constraints include the upper level set and the forward invariance of the control obstacle function. A weight factor is introduced to adjust the weights of the control proximity and the specified proximity between the unmanned systems to achieve the desired distance between the unmanned systems.

[0014] Preferably, in step S1, a motion dynamics model is established for each unmanned system, and the position information and motion speed of each unmanned system are determined. The specific operations are as follows:

[0015] The position information of each unmanned system is expressed as , No. The motion dynamics model of an unmanned system is modeled as:

[0016] ;

[0017] in, Indicates the location information of unmanned systems Find the first-order derivative, represents the speed of movement of the unmanned system involved, Represents the number of unmanned systems.

[0018] Preferably, in step S2, the control obstacle function is used to ensure the collision-free safety set between the unmanned system and the static obstacles and the dynamic unmanned system, and the input safety set of the control system is solved based on forward invariance. The specific operations are as follows:

[0019] Step S21: Establish a collision-free safety set:

[0020] ;

[0021] in, represents the established collision-free safety set, Indicates the distance information between the unmanned system and static obstacles and dynamic unmanned systems. Indicates the location information of static obstacles and other dynamic unmanned systems. represents the collision-free radius parameter determined by the dimension of the unmanned system under the conditions of sensor error and discretization error;

[0022] Step S22: Design control system input :

[0023] ;

[0024] in, express The first derivative of Indicates that the control barrier function satisfies the establishment conditions Class functions; represents transpose;

[0025] Step S23: solving the collision-free safety set of the unmanned system for each test point The upper level set obtained by the intersection of The forward invariance of can realize collision-free control of the unmanned system and the entire point set:

[0026] ;

[0027] in, Indicates A collision-free safety set for each test point Achieve collision-free control system input, i.e., single obstacle collision avoidance safety set, It represents the control system input to achieve collision-free operation of the entire unmanned system, i.e., the multi-obstacle collision avoidance safety set. Indicates The number of control inputs that an unmanned system can achieve without collision.

[0028] Preferably, in step S3, by introducing dynamic tolerance and defining related control obstacle functions, an upper level set is established to maintain the desired distance between unmanned systems and the maximum dynamic tolerance range. The specific operations are as follows:

[0029] Step S31, introducing dynamic tolerance;

[0030] The formation constraint is represented as a fully connected undirected graph , represents the set of points in an undirected graph, represents the set of edges in an undirected graph, where the undirected edges , describing the unmanned system The expected distance between and the maximum dynamic tolerance range ;

[0031] For unmanned systems and unmanned systems Introducing new dynamic tolerances , the dynamic tolerance form is as follows:

[0032] ;

[0033] in, , Both indicate unmanned systems and unmanned systems The first derivative of the dynamic tolerance between;

[0034] Step S32, defining a control barrier function for each relevant undirected edge;

[0035] Defining Unmanned Systems The state is represented by , Unmanned system The dynamic tolerance for each relevant undirected edge Define the following control barrier function:

[0036] ;

[0037] in, , , Unmanned system The expected distance between Indicates The state of an unmanned system, , , , Unmanned system , There are four types of control barrier functions between Unmanned system The maximum dynamic tolerance range between Indicates The location information of unmanned systems;

[0038] Step S33, establishing an upper level set to maintain the desired distance between the unmanned systems and the maximum dynamic tolerance range;

[0039] Unmanned Systems and unmanned systems The distance between the upper level set In connection, it is expressed as follows:

[0040] ;

[0041] in, Unmanned system and unmanned systems The upper level set between ;

[0042] The maintenance of the unmanned system formation configuration is equivalent to: , Satisfies forward invariance, that is, the upper level set .

[0043] Preferably, in step S4, a quadratic programming problem is used to solve the control input of the formation control, the constraints include the upper level set and the forward invariance of the control obstacle function, and a weight factor is introduced to adjust the weights of the control proximity and the prescribed proximity between the unmanned systems to achieve the desired distance between the unmanned systems. The specific operations are as follows:

[0044] ;

[0045] in, represents the control input obtained by quadratic optimization, Indicates the main control object; represents the weight parameter; Indicates control input.

[0046] Therefore, the present invention adopts the above-mentioned elastic dynamic robust formation collaborative control method based on the control obstacle function, and the beneficial technical effects are as follows:

[0047] (1) Better system robustness: Considering that obstacles during the maneuvering process of the unmanned system are dynamically changing, dynamic robust collaborative control of multiple unmanned systems is realized, thereby completing tasks such as collision avoidance with detected obstacles, meeting the needs of autonomous real-time adaptation to dynamic unknown environments and ensuring the safety of the system.

[0048] (2) Higher dynamic real-time performance: Based on the forward invariance of the control obstacle function, a dynamic tolerance range is introduced, which allows the unmanned system to dynamically adjust the unmanned system collaborative formation configuration in real time within the maximum allowable tolerance range, making it closer to the expected effect of the formation and having better formation control flexibility. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 The present invention is a flow chart of a flexible dynamic robust formation collaborative control method based on a control obstacle function. DETAILED DESCRIPTION

[0050] The technical solution of the present invention is further described below through the accompanying drawings and embodiments.

[0051] Unless otherwise defined, technical or scientific terms used in the present invention shall have the common meanings understood by one having ordinary skills in the field to which the present invention belongs.

[0052] Embodiment 1

[0053] like Figure 1 As shown, it is a flow chart of a flexible dynamic robust formation collaborative control method based on a control obstacle function of the present invention, which specifically includes the following steps:

[0054] 1. Unmanned system motion modeling.

[0055] Consider having The scene in which an unmanned system moves in a working area includes several static obstacles. The position information of each unmanned system is represented as , No. The motion dynamics model of an unmanned system can be modeled as:

[0056] ;

[0057] in, Indicates the location information of unmanned systems Find the first-order derivative, Indicates the speed of movement of the unmanned system involved.

[0058] In addition, in order to ensure the safety of the navigation process, that is, to avoid collision, each unmanned system is equipped with sensors such as lidar to detect static obstacles and other dynamic unmanned systems. A relative distance.

[0059] 2. Avoid collisions between obstacles and unmanned systems.

[0060] 201. First, ensure that the unmanned system and the measured point set There is no collision at a certain point.

[0061] The collision-free safe set can be expressed as:

[0062] ;

[0063] in, represents the established collision-free safety set, Indicates the distance information between the unmanned system and static obstacles and dynamic unmanned systems. Indicates the location information of static obstacles and other dynamic unmanned systems. It represents the collision-free radius parameter determined by the dimension of the unmanned system under the conditions of sensor error and discretization error. The input of the control system Should meet:

[0064] ;

[0065] in, express The first derivative of Indicates that the control barrier function satisfies the establishment conditions Class functions; Indicates transpose.

[0066] 202. Further, consider the unmanned system and the entire point set No collision.

[0067] The input safety set of this part of the control system can be determined by the unmanned system for each test point safety set The upper level set obtained by the intersection of The forward invariance of is solved, which is expressed as:

[0068] ;

[0069] in, Indicates A collision-free safety set for each test point Achieve collision-free control system input, i.e., single obstacle collision avoidance safety set, It represents the control system input to achieve collision-free operation of the entire unmanned system, i.e., the multi-obstacle collision avoidance safety set. Indicates The number of control inputs that an unmanned system can achieve without collision.

[0070] 3. Maintenance of formation based on CBF.

[0071] The formation constraint is represented as a fully connected undirected graph , represents the set of points in an undirected graph, represents the set of edges in an undirected graph, where the undirected edges , describing the unmanned system The expected distance between and the maximum dynamic tolerance range .

[0072] 301. Introduce new dynamic tolerance.

[0073] Introducing unmanned systems and unmanned systems Introducing new dynamic tolerances These dynamic tolerances help the dynamic adjustment of the formation, making the distance between the two unmanned systems closer to the expected value and allowing the formation to change within the maximum tolerance range. Specifically, the dynamic tolerance form is as follows:

[0074] ;

[0075] 302. Define CBF for the relevant edges.

[0076] Define the state of the unmanned system as , Unmanned system Next, for each relevant edge Define the following control barrier function:

[0077] ;

[0078] in, , , Unmanned system The expected distance between Indicates The state of an unmanned system, , , , Unmanned system , There are four types of control barrier functions between Unmanned system The maximum dynamic tolerance range between Indicates The location information of an unmanned system.

[0079] 303. Establishment of upper level set.

[0080] Unmanned Systems and unmanned systems The distance between the upper level set In connection, it is expressed as follows:

[0081] ;

[0082] Furthermore, the maintenance of the unmanned system formation configuration is equivalent to: , Satisfies forward invariance, that is, the upper level set .

[0083] 4. Formation collaborative control based on CBF.

[0084] use To represent the main control object, the solution of the formation control in the invention is given by the solution of the following quadratic programming problem:

[0085] ;

[0086] The constraint condition is the upper level set and To satisfy the forward invariance of the CBF function, we introduce To adjust the weights of control proximity and prescribed proximity between unmanned systems.

[0087] The design for dynamic tolerances is as follows:

[0088] ;

[0089] Among them, select the appropriate control input To reduce the dynamic error to 0, so that the distance between unmanned systems reaches the expected distance. represents the controller gain.

[0090] It is worth noting that the contents not elaborated in detail in the present invention are all prior art and are well known to those skilled in the art.

[0091] Therefore, the present invention adopts the above-mentioned elastic dynamic robust formation collaborative control method based on the control obstacle function, which can effectively deal with the uncertainty and random obstacles in unknown dynamic environments and improve the collaborative control performance of multi-unmanned systems.

[0092] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A flexible dynamic robust formation cooperative control method based on control obstacle function, characterized in that: The following steps are involved: Step S1, establishing a motion dynamics model for each unmanned system, and determining the position information and motion speed of each unmanned system; Step S2: using the control obstacle function to ensure the collision-free safety set between the unmanned system and the static obstacles and the dynamic unmanned system, and solving the input safety set of the control system based on forward invariance; Step S3, by introducing dynamic tolerance and defining related control obstacle functions, establishing an upper level set to maintain the desired distance between unmanned systems and the maximum dynamic tolerance range; Step S4: A quadratic programming problem is used to solve the control input of the formation control. The constraints include the upper level set and the forward invariance of the control obstacle function. A weight factor is introduced to adjust the weights of the control proximity and the specified proximity between the unmanned systems to achieve the desired distance between the unmanned systems.

2. The elastic dynamic robust formation collaborative control method based on control obstacle function according to claim 1 is characterized in that: In step S1, a motion dynamics model is established for each unmanned system, and the position information and motion speed of each unmanned system are determined. The specific operations are as follows: The position information of each unmanned system is expressed as , No. The motion dynamics model of an unmanned system is modeled as: ； in, Indicates the location information of unmanned systems Find the first-order derivative, represents the speed of movement of the unmanned system involved, Represents the number of unmanned systems.

3. The elastic dynamic robust formation collaborative control method based on control obstacle function according to claim 2 is characterized in that: In step S2, the control obstacle function is used to ensure the collision-free safety set between the unmanned system and the static obstacles and the dynamic unmanned system, and the input safety set of the control system is solved based on the forward invariance. The specific operations are as follows: Step S21: Establish a collision-free safety set: ； in, represents the established collision-free safety set, Indicates the distance information between the unmanned system and static obstacles and dynamic unmanned systems. Indicates the location information of static obstacles and other dynamic unmanned systems. represents the collision-free radius parameter determined by the dimension of the unmanned system under the conditions of sensor error and discretization error; Step S22: Design control system input : ； in, express The first derivative of Indicates that the control barrier function satisfies the establishment conditions Class functions; represents transpose; Step S23: solving the collision-free safety set of the unmanned system for each test point The upper level set obtained by the intersection of The forward invariance of can realize collision-free control of the unmanned system and the entire point set: ； in, Indicates A collision-free safety set for each test point Achieve collision-free control system input, i.e., single obstacle collision avoidance safety set, It represents the control system input to achieve collision-free operation of the entire unmanned system, i.e., the multi-obstacle collision avoidance safety set. Indicates The number of control inputs that an unmanned system can achieve without collision.

4. The elastic dynamic robust formation collaborative control method based on control obstacle function according to claim 3 is characterized in that: In step S3, by introducing dynamic tolerance and defining related control obstacle functions, an upper level set is established to maintain the desired distance between unmanned systems and the maximum dynamic tolerance range. The specific operations are as follows: Step S31, introducing dynamic tolerance; The formation constraint is represented as a fully connected undirected graph , represents the set of points in an undirected graph, represents the set of edges in an undirected graph, where the undirected edges , describing the unmanned system The expected distance between and the maximum dynamic tolerance range ; For unmanned systems and unmanned systems Introducing new dynamic tolerances , the dynamic tolerance form is as follows: ； in, , Both indicate unmanned systems and unmanned systems The first derivative of the dynamic tolerance between; Step S32, defining a control barrier function for each relevant undirected edge; Defining Unmanned Systems The state is represented by , Unmanned system The dynamic tolerance for each relevant undirected edge Define the following control barrier function: ； in, , , Unmanned system The expected distance between Indicates The state of an unmanned system, , , , Unmanned system , There are four types of control barrier functions between Unmanned system The maximum dynamic tolerance range between Indicates The location information of unmanned systems; Step S33, establishing an upper level set to maintain the desired distance between the unmanned systems and the maximum dynamic tolerance range; Unmanned Systems and unmanned systems The distance between the upper level set In connection, it is expressed as follows: ； in, Unmanned system and unmanned systems The upper level set between ; The maintenance of the unmanned system formation configuration is equivalent to: , Satisfies forward invariance, that is, the upper level set .

5. The elastic dynamic robust formation cooperative control method based on control obstacle function according to claim 4 is characterized in that: In step S4, a quadratic programming problem is used to solve the control input of the formation control. The constraints include the upper level set and the forward invariance of the control obstacle function. A weight factor is introduced to adjust the weights of the control proximity and the prescribed proximity between the unmanned systems to achieve the desired distance between the unmanned systems. The specific operations are as follows: ； in, represents the control input obtained by quadratic optimization, Indicates the main control object; represents the weight parameter; Indicates control input.

Images