A UAV sliding mode formation control system and method based on laser radar relative positioning

Abstract

The present invention proposes a UAV sliding mode formation control system and method for relative positioning of laser radar. The present invention constructs a directed graph of the communication topology of the follower UAV; uses the hector_mapping algorithm to perform horizontal mapping and positioning, obtains the two-dimensional coordinates of each follower UAV in the corresponding two-dimensional map coordinate system, and converts them into coordinates in the inertial coordinate system; constructs a motion model of each follower UAV with internal parameter uncertainty and external disturbance; establishes a formation error model containing disturbance terms; combines the relationship between the uncertain disturbance terms and the formation error in the formation error mathematical model, and sets the interference observer update strategy; designs a distributed collaborative sliding mode formation controller through Lyapunov stability analysis, calculates the speed of each follower UAV in the three-dimensional axis and the control input of the yaw angular velocity, adjusts the flight speed of the follower UAV, and realizes a stable formation of multiple UAVs under uncertain interference. The present invention has excellent anti-interference ability and formation maintenance ability.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned aerial vehicle formation, and in particular relates to a control system and method for an unmanned aerial vehicle sliding mode formation with relative positioning of laser radars. Background Art

[0002] Drones are now widely used in industry, agriculture and military fields. With the diversification of application scenarios, the limitations of individual drone functions are increasing. Drone formations have stronger environmental perception capabilities and greater fault tolerance, and have significant advantages over single drones when performing complex tasks. At present, drone formation research usually requires UWB positioning systems or GPS modules, but these two methods are only applicable to indoors and outdoors, respectively. The UWB system is complex to build and can only be used in fixed experimental sites, lacking versatility and flexibility, while the GPS positioning system needs to be used in open outdoor areas. The pilot-follower method is the most commonly used method in the sliding mode formation control of multiple drones, but due to the limitations of drone size and load, the communication modules carried are limited and it is difficult to support large-scale formations. In order to make up for the lack of drone communication capabilities, researchers have further studied distributed pilot-follower formations. However, the traditional distributed pilot-follower formation control method has the disadvantages of step-by-step accumulation of formation errors and poor robustness, especially when the internal parameters are uncertain and there is external interference, it is difficult for drone formations to achieve good results. Therefore, the traditional drone sliding mode formation control scheme is difficult to achieve universal and stable formations indoors and outdoors.

[0003] Based on the above technical problems, the present invention provides a UAV sliding mode formation control system with laser radar relative positioning. The system can provide accurate position information for indoor and outdoor UAV formations through laser radar mapping positioning and coordinate conversion, and uses a distributed adaptive sliding mode controller to improve the robustness of the leader-follower formation control and reduce formation errors. Summary of the invention

[0004] In order to solve the above technical problems, the present invention proposes a UAV sliding mode formation control system and method with laser radar relative positioning.

[0005] The technical solution of the system of the present invention is a UAV sliding mode formation control system with laser radar relative positioning, comprising: a pilot UAV and multiple follower UAVs; the pilot UAV communicates with one of the multiple follower UAVs;

[0006] Construct a directed graph of follow-up drone communication topology;

[0007] Use the hector_mapping algorithm to build a horizontal map and locate the drone, obtain the two-dimensional coordinates of each drone in the corresponding two-dimensional map coordinate system, and convert them into the coordinates of each drone in the inertial coordinate system.

[0008] Construct the motion model of each follower UAV with internal parameter uncertainty and external disturbance;

[0009] Establish a relationship model of the relative distance and angle between any two communicating neighbor following drones on the X, Y, and Z axes in the body coordinate system, and construct a formation error model with disturbance terms;

[0010] Combined with the relationship between the uncertain disturbance term and the formation error in the formation error mathematical model, a disturbance observer update strategy containing the formation error and the sliding mode term is set;

[0011] Through Lyapunov stability analysis, a distributed cooperative sliding mode formation controller is designed. The distributed cooperative controller calculates the control input of the speed of each follower UAV in the X, Y, and Z axes and the yaw angular velocity, adjusts the flight speed of the follower UAV, and realizes a stable formation of multiple UAVs under uncertain interference.

[0012] The technical solution of the method of the present invention is a sliding mode formation control method for unmanned aerial vehicles with relative positioning of laser radar, which is specifically as follows:

[0013] Step 1: Construct a directed graph of the follow-up drone communication topology;

[0014] Step 2: Each follower drone is combined with the laser radar and the initial position of each follower drone is used to perform horizontal mapping and positioning using the hector_mapping algorithm to obtain the two-dimensional map coordinate system of each follower drone and the two-dimensional coordinates of each follower drone in the corresponding two-dimensional map coordinate system. The two-dimensional coordinates of each follower drone in the corresponding two-dimensional map coordinate system are converted into the coordinates of each follower drone in the inertial coordinate system;

[0015] Step 3: Construct the body coordinate system, combine the motion relationship of each follower drone in the body coordinate system and the inertial coordinate system, and construct the motion model of each follower drone with internal parameter uncertainty and external disturbance;

[0016] Step 4: According to the motion model of each follower drone, a relationship model of the relative distance and angle between any two communicating neighbor follower drones on the X, Y, and Z axes in the body coordinate system is established, and a formation error model containing a disturbance term is constructed in combination with the difference in the expected formation distance;

[0017] Step 5: Use the formation error model with disturbance term to construct the sliding mode functions of any two communication neighbor following UAVs and the sliding mode functions of the communication neighbor following UAV and the pilot UAV, and combine the relationship between the uncertain disturbance term and the formation error in the formation error mathematical model to set the interference observer update strategy containing the formation error and the sliding mode term;

[0018] Step 6: Based on the sliding mode function and disturbance observer constructed in step 5, a distributed cooperative sliding mode formation controller is designed through Lyapunov stability analysis. The distributed cooperative controller calculates the control inputs of the speed of each follower UAV in the X, Y, and Z axes and the yaw angular velocity, adjusts the flight speed of the follower UAV, and realizes a stable formation of multiple UAVs under uncertain interference.

[0019] Preferably, step 1 constructs a directed graph of the follow-up drone communication topology, as follows:

[0020] Define the set of follow-up drones as:

[0021] Γ={1,…,n}

[0022] Where n is the number of following drones;

[0023] Define the pilot drone number as 0;

[0024] Each follower drone in the follower drone set is regarded as a node in the follower drone communication topology directed graph;

[0025] The following drone communication topology directed graph is defined as follows:

[0026]

[0027] in, represents a set of vertices, represents the set of edges, (i,j) represents the communication relationship between the i-th following drone and the j-th following drone, represents the adjacency matrix. If the i-th follower drone can receive the information of the j-th follower drone, then the j-th follower drone is called the communication neighbor of the i-th follower drone and a ij > 0, otherwise a ij =0, represents the set of the communication neighbors of the i-th following drone, and n represents the number of following drones;

[0028] The communication topology directed graph of the follow-up drone is a distributed topology directed graph;

[0029] The motion trajectory of the pilot drone is generated by the system, and only one follower drone in the set of follower drones communicates with the pilot drone and obtains the motion trajectory parameters that need to be tracked;

[0030] The remaining following drones in the following drone set only exchange information with each neighbor following drone in the set of neighbor following drones, and determine their own control input according to the motion state of each neighbor following drone in the set of neighbor following drones;

[0031] Preferably, in step 2, the two-dimensional map coordinate system of each unmanned aircraft following the aircraft takes the initial position of each unmanned aircraft following the aircraft as the origin of the two-dimensional map coordinate system;

[0032] Each follower drone collects the vertical height of each follower drone through a laser ranging module;

[0033] Step 2 converts the two-dimensional coordinates of each follower drone in the two-dimensional map coordinate system into the coordinates of each follower drone in the inertial coordinate system, as follows:

[0034]

[0035] in, are the coordinates of the X-axis and Y-axis of the i-th following drone in the inertial coordinate system when it is at the initial position, are the coordinates of the X-axis and Y-axis of the i-th following drone in the corresponding two-dimensional map coordinate system, and the coordinates of the X-axis and Y-axis of the i-th following drone in the inertial coordinate system, respectively. n represents the number of following drones.

[0036] Preferably, the body coordinate system is constructed in step 3 as follows:

[0037] Define the origin of the body coordinate system at the center of gravity of the drone, the X-axis of the body coordinate system points to the direction of the drone's nose, the Y-axis of the body coordinate system points from the origin to the right side of the drone, and the Z-axis direction of the body coordinate system passes through X, and Y is determined by the right-hand rule;

[0038] The motion model of each following drone described in step 3 is specifically defined as follows:

[0039]

[0040] in, are the speed of the i-th following drone in the X-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Y-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Z-axis direction in the inertial coordinate system, and the angular velocity of the i-th following drone in the inertial coordinate system, ω i is the angular velocity of the i-th following UAV in the body coordinate system, They represent the disturbances caused by internal parameter uncertainty and external disturbances on the linear velocity and yaw angular velocity of the X, Y, and Z axes of the i-th following UAV in the inertial coordinate system. are the linear velocity of the i-th following drone in the X-axis direction of the body coordinate system, the linear velocity of the i-th following drone in the Y-axis direction of the body coordinate system, and the linear velocity of the i-th following drone in the Z-axis direction of the body coordinate system;

[0041] Preferably, the relationship model of the relative distance and angle between any two communication neighbors following the drone on the X, Y, and Z axes in the body coordinate system in step 4 is defined as follows:

[0042]

[0043] in, They respectively represent the relative distance of the i-th following drone in the X direction in the body coordinate system of the j-th following drone, the relative distance of the i-th following drone in the Y direction in the body coordinate system of the j-th following drone, and the relative distance of the i-th following drone in the Z direction in the body coordinate system of the j-th following drone. is the yaw angle difference of the i-th following UAV in the body coordinate system of the j-th following UAV;

[0044] If j=0, the jth following drone represents the virtual pilot node;

[0045] If j∈N i When , the jth following drone represents the communication neighbor node of the i-th following drone;

[0046] N i represents the set of communication neighbor following drones of the i-th following drone;

[0047] x i ,y i ,z i ,ψ i are the speed of the i-th following drone in the X-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Y-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Z-axis direction in the inertial coordinate system, and the angular velocity of the i-th following drone in the inertial coordinate system; x j ,y j ,z j ,ψ j They are respectively the speed of the jth following drone in the X-axis direction in the inertial coordinate system, the speed of the jth following drone in the Y-axis direction in the inertial coordinate system, the speed of the jth following drone in the Z-axis direction in the inertial coordinate system, and the angular velocity of the jth following drone in the inertial coordinate system;

[0048] The formation error model containing the disturbance term is constructed in step 4, as follows:

[0049]

[0050] in, is the set of formation errors and yaw angle errors of the i-th communication neighbor following UAV and the j-th communication neighbor following UAV in the X, Y, and Z axis directions of the body coordinate system, YesEij The rate of change, is the set of velocity and angular velocity of the X, Y, and Z axes of the i-th UAV body coordinate system, H ij is the coefficient matrix, F ij is the formation error matrix between the i-th UAV and the j-th UAV, D ij is the set of disturbance terms of the i-th UAV and the j-th UAV formation.

[0051] Preferably, the sliding mode function of any two communicating neighbors following the UAV in step 5 is specifically defined as follows:

[0052]

[0053]

[0054] Among them, S ij is the sliding mode function of the i-th communication neighbor following the UAV and the j-th communication neighbor following the UAV, E ij is the set of formation errors and yaw angle errors of the i-th communication neighbor following UAV and the j-th communication neighbor following UAV in the X, Y, and Z axis directions of the body coordinate system, sgn is the sign function, and the integral term Indicates that for the time interval [0, t] Integrate the value of , gain α1∈(0,1), control gain k1, k2>0, exponent γ1∈(0,1);

[0055] The sliding mode function S of the i-th UAV and the virtual pilot UAV i0 , specifically defined as follows:

[0056]

[0057]

[0058] Among them, S i0 is the sliding mode function of the i-th follower UAV and the pilot UAV, with gain α2∈(0,1), b i0 is the communication weight between UAV i and the virtual navigator, control gain k1, k2>0, exponential term γ1∈(0,1), sgn is the sign function, E i0 is the formation error between the i-th UAV and the pilot UAV, the integral term Indicates that for the time interval [0, t] Integrate the value of .

[0059] The uncertain disturbance described in step 5 will be indirectly reflected in the formation error and the value of the sliding mode function. Therefore, the update strategy of the two interference observers of the i-th UAV and the j-th UAV is set by using the formation error and the sliding mode function:

[0060]

[0061] in, is the interference observer between the i-th UAV and the j-th UAV, is the interference observer 2 between the i-th UAV and the j-th UAV, Representation Observer The rate of change of the sliding mode function is calculated using the absolute value of |S ij |Observe, coefficient h 1,ij >0, Representation Observer The rate of change of the formation error is calculated using the absolute value of the formation error |E ij | and the absolute value of the sliding mode function |S ij | is observed by multiplying 2,ij >0.

[0062] And the two interference observer update strategies of the i-th UAV and the pilot UAV are

[0063]

[0064] in, is the interference observer between the i-th UAV and the pilot UAV, is the interference observer 2 between the i-th UAV and the pilot UAV, for The rate of change of the sliding mode function is calculated using the absolute value of |S i0 |Observe, coefficient h 1,i0 >0, for The rate of change of the formation error is calculated using the absolute value of the formation error |E i0 | and the absolute value of the sliding mode function |S i0 | is observed by multiplying 2,i0 >0.

[0065] Preferably, the distributed cooperative sliding mode formation controller described in step 6 is defined as follows:

[0066]

[0067] in, ω i are the speed and angular velocity of the X, Y, and Z axes of the i-th UAV’s body coordinate system, is the set of speed control inputs, H ij is the coefficient matrix, F ij is the formation error matrix between the i-th UAV and the j-th UAV, is the matrix Hij The inverse matrix, H i0 is the coefficient matrix, F i0 is the formation error matrix between the i-th UAV and the pilot UAV, is the matrix H i0 The inverse matrix of , control gain b1>0, control gain b2>0, index γ2∈(0,1).

[0068] Each following UAV adjusts its own flight speed through the distributed sliding mode formation controller, which can achieve a stable formation of multiple UAVs under uncertain interference.

[0069] The advantage of the present invention is that the UAV sliding mode formation control system with relative positioning of laser radar proposed in this paper has excellent anti-interference ability and formation maintenance ability under the conditions of inaccurate internal model and uncertain external interference, and can realize the formation control of UAVs without a UWB positioning system. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 : Schematic diagram of the communication topology structure of the UAV formation according to an embodiment of the present invention.

[0071] Figure 2 : Flow chart of the method of an embodiment of the present invention.

[0072] Figure 3 : Schematic diagram of the coordinate transformation relationship of the drone according to an embodiment of the present invention.

[0073] Figure 4 : Schematic diagram of a leader-follower formation according to an embodiment of the present invention.

[0074] Figure 5 : UAV formation simulation trajectory diagram of an embodiment of the present invention.

[0075] Figure 6 : Error diagram of the UAV formation simulation according to an embodiment of the present invention.

[0076] Figure 7 : Diagram of the multi-UAV formation experiment process of an embodiment of the present invention.

[0077] Figure 8 : Trajectory and error diagram of a multi-UAV formation according to an embodiment of the present invention. DETAILED DESCRIPTION

[0078] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0079] In specific implementation, the method proposed in the technical solution of the present invention can be implemented by technical personnel in this field using computer software technology to realize automatic operation process. System devices for implementing the method, such as computer-readable storage media storing the corresponding computer program of the technical solution of the present invention and computer equipment running the corresponding computer program, should also be within the scope of protection of the present invention.

[0080] Combine the following Figures 1 to 8 The specific implementation modes of the present invention are introduced.

[0081] The technical solution of the system of the embodiment of the present invention is a UAV sliding mode formation control system with relative positioning of laser radar, which is as follows:

[0082] In order to verify the effect of the invented strategy in practical application, a communication topology was built. Figure 1 The three quad-rotor drone platforms shown in the figure are as follows Figure 2 The flowchart of the method shown in the figure verifies the effectiveness of the proposed strategy. Node UAV0 represents the virtual navigator. Among the following drones, only UAV1 can communicate with the navigator to obtain the motion trajectory parameters that need to be tracked. UAV2 communicates with UAV1, and UAV3 communicates with UAV2. Set the initial position of the first following drone Initial position of the second follower drone The initial position of the third follower drone Formation control gain k1 = 0.1, k2 = 0.1, exponent γ1 = 0.6, γ2 = 0.9, adaptive observer control gain b1 = 0.5, b2 = 0.8, adaptive observer coefficient h 1,i0 =0.01,h 2,i0 =0.5,h 1,ij =0.01,h 2,ij = 0.5, the expected distance between the first UAV and the virtual pilot UAV is set to The expected distance between the second drone and the first drone is set to The expected distance between the third drone and the first drone is set to

[0083] The technical solution of the method of the embodiment of the present invention is a sliding mode formation control method of unmanned aerial vehicles with relative positioning of laser radar, which is specifically as follows:

[0084] Step 1: Construct a directed graph of the follow-up drone communication topology;

[0085] Step 1 constructs a directed graph of the follow-up drone communication topology, as follows:

[0086] Define the set of follow-up drones as:

[0087] Γ={1,…,n}

[0088] Among them, n = 3 is the number of following drones;

[0089] Define the pilot drone number as 0;

[0090] Each follower drone in the follower drone set is regarded as a node in the follower drone communication topology directed graph;

[0091] The following drone communication topology directed graph is defined as follows:

[0092]

[0093] in, represents a set of vertices, represents the set of edges, (i,j) represents the communication relationship between the i-th following drone and the j-th following drone, represents the adjacency matrix. If the i-th follower drone can receive the information of the j-th follower drone, then the j-th follower drone is called the communication neighbor of the i-th follower drone and a ij > 0, otherwise a ij =0, represents the set of the communication neighbors of the i-th following drone, and n = 3 represents the number of following drones;

[0094] The communication topology directed graph of the follow-up drone is a distributed topology directed graph;

[0095] The motion trajectory of the pilot drone is generated by the system, and only one follower drone in the set of follower drones communicates with the pilot drone and obtains the motion trajectory parameters that need to be tracked;

[0096] The remaining following drones in the following drone set only exchange information with each neighbor following drone in the set of neighbor following drones, and determine their own control input according to the motion state of each neighbor following drone in the set of neighbor following drones;

[0097] Step 2: Each follower drone is combined with the laser radar and the initial position of each follower drone is used to perform horizontal mapping and positioning using the hector_mapping algorithm to obtain the two-dimensional map coordinate system of each follower drone and the two-dimensional coordinates of each follower drone in the corresponding two-dimensional map coordinate system. The two-dimensional coordinates of each follower drone in the corresponding two-dimensional map coordinate system are converted into the coordinates of each follower drone in the inertial coordinate system;

[0098] In step 2, the two-dimensional map coordinate system of each unmanned aircraft follows the initial position of each unmanned aircraft as the origin of the two-dimensional map coordinate system;

[0099] Each follower drone collects the vertical height of each follower drone through a laser ranging module;

[0100] The coordinate transformation relationship described in step 2 is as follows Figure 3 As shown, the two-dimensional coordinates of each follower drone in the two-dimensional map coordinate system are converted into the coordinates of each follower drone in the inertial coordinate system, as follows:

[0101]

[0102] in, are the coordinates of the X-axis and Y-axis of the i-th following drone in the inertial coordinate system when it is at the initial position, are the coordinates of the X-axis and Y-axis of the ith following drone in the corresponding two-dimensional map coordinate system, and the coordinates of the X-axis and Y-axis of the ith following drone in the inertial coordinate system, respectively. n=3 indicates the number of following drones.

[0103] Step 3: Construct the body coordinate system, combine the motion relationship of each follower drone in the body coordinate system and the inertial coordinate system, and construct the motion model of each follower drone with internal parameter uncertainty and external disturbance;

[0104] Step 3 constructs the body coordinate system as follows:

[0105] Define the origin of the body coordinate system at the center of gravity of the drone, the X-axis of the body coordinate system points to the direction of the drone's nose, the Y-axis of the body coordinate system points from the origin to the right side of the drone, and the Z-axis direction of the body coordinate system passes through X, and Y is determined by the right-hand rule;

[0106] The motion model of each following drone described in step 3 is specifically defined as follows:

[0107]

[0108] in, are the speed of the i-th following drone in the X-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Y-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Z-axis direction in the inertial coordinate system, and the angular velocity of the i-th following drone in the inertial coordinate system, ω i is the angular velocity of the i-th following UAV in the body coordinate system, They represent the disturbances caused by internal parameter uncertainty and external disturbances on the linear velocity and yaw angular velocity of the X, Y, and Z axes of the i-th following UAV in the inertial coordinate system. are the linear velocity of the i-th following drone in the X-axis direction of the body coordinate system, the linear velocity of the i-th following drone in the Y-axis direction of the body coordinate system, and the linear velocity of the i-th following drone in the Z-axis direction of the body coordinate system;

[0109] Step 4: According to the motion model of each follower drone, a relationship model of the relative distance and angle between any two communicating neighbor follower drones on the X, Y, and Z axes in the body coordinate system is established. Combined with the difference in the expected formation distance, a formation error model with a disturbance term is constructed. The pilot-follower formation is as follows: Figure 4 As shown;

[0110] The relationship model of the relative distance and angle between any two communication neighbors following the drone on the X, Y, and Z axes in the body coordinate system described in step 4 is defined as follows:

[0111]

[0112] in, They respectively represent the relative distance of the i-th following drone in the X direction in the body coordinate system of the j-th following drone, the relative distance of the i-th following drone in the Y direction in the body coordinate system of the j-th following drone, and the relative distance of the i-th following drone in the Z direction in the body coordinate system of the j-th following drone. is the yaw angle difference of the i-th following UAV in the body coordinate system of the j-th following UAV;

[0113] If j=0, the jth following drone represents the virtual pilot node;

[0114] If j∈N i When , the jth following drone represents the communication neighbor node of the i-th following drone;

[0115] N i represents the set of communication neighbor following drones of the i-th following drone;

[0116] x i ,y i ,z i ,ψ i are the speed of the i-th following drone in the X-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Y-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Z-axis direction in the inertial coordinate system, and the angular velocity of the i-th following drone in the inertial coordinate system; x j ,y j ,z j ,ψ jThey are respectively the speed of the jth following drone in the X-axis direction in the inertial coordinate system, the speed of the jth following drone in the Y-axis direction in the inertial coordinate system, the speed of the jth following drone in the Z-axis direction in the inertial coordinate system, and the angular velocity of the jth following drone in the inertial coordinate system;

[0117] The formation error model containing the disturbance term is constructed in step 4, as follows:

[0118]

[0119] in, is the set of formation errors and yaw angle errors of the i-th communication neighbor following UAV and the j-th communication neighbor following UAV in the X, Y, and Z axis directions of the body coordinate system, YesE ij The rate of change, is the set of velocity and angular velocity of the X, Y, and Z axes of the i-th UAV body coordinate system, H ij is the coefficient matrix, F ij is the formation error matrix between the i-th UAV and the j-th UAV, D ij is the set of disturbance terms of the i-th UAV and the j-th UAV formation.

[0120] Step 5: Use the formation error model with disturbance term to construct the sliding mode functions of any two communication neighbor following UAVs and the sliding mode functions of the communication neighbor following UAV and the pilot UAV, and combine the relationship between the uncertain disturbance term and the formation error in the formation error mathematical model to set the interference observer update strategy containing the formation error and the sliding mode term;

[0121] The sliding mode function of any two communicating neighbors following the UAV in step 5 is specifically defined as follows:

[0122]

[0123] Among them, S ij is the sliding mode function of the i-th communication neighbor following the UAV and the j-th communication neighbor following the UAV, E ij is the set of formation errors and yaw angle errors of the i-th communication neighbor following UAV and the j-th communication neighbor following UAV in the X, Y, and Z axis directions of the body coordinate system, sgn is the sign function, and the integral term Indicates that for the time interval [0, t] Integrate the value of , gain α1∈(0,1), control gain k1, k2>0, exponent γ1∈(0,1);

[0124] The sliding mode function S of the i-th UAV and the virtual pilot UAV i0 , specifically defined as follows:

[0125]

[0126]

[0127] Among them, S i0 is the sliding mode function of the i-th follower UAV and the pilot UAV, with gain α2∈(0,1), b i0 is the communication weight between UAV i and the virtual navigator, control gain k1, k2>0, exponential term γ1∈(0,1), sgn is the sign function, E i0 is the formation error between the i-th UAV and the pilot UAV, the integral term Indicates that for the time interval [0, t] Integrate the value of .

[0128] The uncertain disturbance described in step 5 will be indirectly reflected in the formation error and the value of the sliding mode function. Therefore, the update strategy of the two interference observers of the i-th UAV and the j-th UAV is set by using the formation error and the sliding mode function:

[0129]

[0130] in, is the interference observer between the i-th UAV and the j-th UAV, is the interference observer 2 between the i-th UAV and the j-th UAV, Representation Observer The rate of change of the sliding mode function is calculated using the absolute value of |S ij |Observe, coefficient h 1,ij >0, Representation Observer The rate of change of the formation error is calculated using the absolute value of the formation error |E ij | and the absolute value of the sliding mode function |S ij | is observed by multiplying 2,ij >0.

[0131] And the two interference observer update strategies of the i-th UAV and the pilot UAV are

[0132]

[0133] in, is the interference observer between the i-th UAV and the pilot UAV, is the interference observer 2 between the i-th UAV and the pilot UAV, for The rate of change of the sliding mode function is calculated using the absolute value of |S i0 |Observe, coefficient h 1,i0 >0, for The rate of change of the formation error is calculated using the absolute value of the formation error |E i0 | and the absolute value of the sliding mode function |S i0 | is observed by multiplying 2,i0 >0.

[0134] Step 6: Based on the sliding mode function and disturbance observer constructed in step 5, a distributed cooperative sliding mode formation controller is designed through Lyapunov stability analysis. The distributed cooperative controller calculates the control inputs of the speed of each follower UAV in the X, Y, and Z axes and the yaw angular velocity, adjusts the flight speed of the follower UAV, and realizes a stable formation of multiple UAVs under uncertain interference.

[0135] The distributed cooperative sliding mode formation controller described in step 6 is defined as follows:

[0136]

[0137] in, ω i are the speed and angular velocity of the X, Y, and Z axes of the i-th UAV’s body coordinate system, is the set of speed control inputs, H ij is the coefficient matrix, F ij is the formation error matrix between the i-th UAV and the j-th UAV, is the matrix H ij The inverse matrix, H i0 is the coefficient matrix, F i0 is the formation error matrix between the i-th UAV and the pilot UAV, is the matrix H i0 The inverse matrix of , control gain b1>0, control gain b2>0, index γ2∈(0,1).

[0138] Figure 5 is the simulation trajectory of the formation control method proposed in the present invention, Figure 6 is the simulation formation error, indicating that when the UAV formation moves in a curve, since step 4 establishes the relative distance and angle relationship between any two communication neighbors following the UAV in the body coordinate system, the formation trajectory is kept good and more regular than the formation trajectory of the existing method, which has greater application value in the collaborative task planning of multi-aircraft formations. Figure 7 This is a real-life formation experiment of three drones, Figure 7 (a) is a line formation. Figure 7 (b) is a triangular formation. Figure 8 (a) is the displacement curve of the UAV in the experiment. Figure 8(a) is the formation error curve. Using the lidar positioning and coordinate transformation method in step 2, the formation control of the UAV can be realized without a UWB positioning system. The experimental results show that the three quadrotor UAVs have an initial formation error. Under the formation control strategy of the present invention, the formation error can converge to within 0.1m. Due to the accurate estimation of uncertain interference by the adaptive observer constructed in step 5, the formation error can still converge in the presence of external interference. The distributed collaborative controller set in step 6 can utilize limited communication resources to increase the scale of the UAV formation. It can be seen that the UAV sliding mode formation control system with relative lidar positioning proposed in this paper has excellent anti-interference ability and formation maintenance ability.

[0139] It should be understood that parts not elaborated in detail in this specification belong to the prior art.

[0140] It should be understood that the above description of the preferred embodiment is relatively detailed and cannot be regarded as limiting the scope of patent protection of the present invention. Under the enlightenment of the present invention, ordinary technicians in this field can also make substitutions or modifications without departing from the scope of protection of the claims of the present invention, which all fall within the scope of protection of the present invention. The scope of protection requested for the present invention shall be based on the attached claims.

Claims

1. A UAV sliding mode formation control method based on laser radar relative positioning, applied to a UAV sliding mode formation control system, characterized in that: The UAV sliding mode formation control system includes: a pilot UAV and multiple follower UAVs; the pilot UAV communicates with one of the multiple follower UAVs; The UAV sliding mode formation control method comprises the following steps: Step 1: Construct a directed graph of the follow-up drone communication topology; Step 2: Each follower drone is combined with the laser radar and the initial position of each follower drone is used to perform horizontal mapping and positioning using the hector_mapping algorithm to obtain the two-dimensional map coordinate system of each follower drone and the two-dimensional coordinates of each follower drone in the corresponding two-dimensional map coordinate system. The two-dimensional coordinates of each follower drone in the corresponding two-dimensional map coordinate system are converted into the coordinates of each follower drone in the inertial coordinate system; Step 3: Construct the body coordinate system, combine the motion relationship of each follower drone in the body coordinate system and the inertial coordinate system, and construct the motion model of each follower drone with internal parameter uncertainty and external disturbance; Step 4: According to the motion model of each follower drone, a relationship model of the relative distance and angle between any two communicating neighbor follower drones on the X, Y, and Z axes in the body coordinate system is established, and a formation error model containing a disturbance term is constructed in combination with the difference in the expected formation distance; Step 5: Use the formation error model with disturbance term to construct the sliding mode functions of any two communication neighbor following UAVs and the sliding mode functions of the communication neighbor following UAV and the pilot UAV, and combine the relationship between the uncertain disturbance term and the formation error in the formation error mathematical model to set the interference observer update strategy containing the formation error and the sliding mode term; Step 6: Based on the sliding mode function and disturbance observer constructed in step 5, a distributed cooperative sliding mode formation controller is designed through Lyapunov stability analysis. The distributed cooperative controller calculates the speed of each follower UAV in the X, Y, and Z axes and the control input of the yaw angular velocity, adjusts the flight speed of the follower UAV, and realizes a stable formation of multiple UAVs under uncertain interference; The sliding mode function of any two communicating neighbors following the UAV in step 5 is specifically defined as follows: Among them, S ij is the sliding mode function of the i-th communication neighbor following the UAV and the j-th communication neighbor following the UAV, E ij is the set of formation errors and yaw angle errors of the i-th communication neighbor following UAV and the j-th communication neighbor following UAV in the X, Y, and Z axis directions of the body coordinate system, sgn is the sign function, and the integral term Indicates that in the time interval [0, t] Integrate the value of , gain α1∈(0,1), control gain k1, k2>0, exponent γ1∈(0,1); The sliding mode function S of the i-th UAV and the virtual pilot UAV i0 , specifically defined as follows: Among them, S i0 is the sliding mode function of the i-th follower UAV and the pilot UAV, with gain α2∈(0,1), b i0 is the communication weight between UAV i and the virtual navigator, control gain k1, k2>0, exponential term γ1∈(0,1), sgn is the sign function, E i0 is the formation error between the i-th UAV and the pilot UAV, and the integral term Indicates that in the time interval [0, t] Integrate the value of ; The uncertain disturbance described in step 5 will be indirectly reflected in the formation error and the value of the sliding mode function. Therefore, the update strategy of the two interference observers of the i-th UAV and the j-th UAV is set by using the formation error and the sliding mode function: in, is the interference observer between the i-th UAV and the j-th UAV, is the interference observer 2 between the i-th UAV and the j-th UAV, Representation Observer The rate of change of the sliding mode function is calculated using the absolute value of |S ij |Observe, coefficient h 1,ij >0, Representation Observer The rate of change of the formation error is calculated using the absolute value of the formation error |E ij | and the absolute value of the sliding mode function |S ij | is observed by multiplying the coefficient h 2,ij >0; And the two interference observer update strategies of the i-th UAV and the pilot UAV are in, is the interference observer between the i-th UAV and the pilot UAV, is the interference observer 2 between the i-th UAV and the pilot UAV, for The rate of change of the sliding mode function is calculated using the absolute value of |S i0 |Observe, coefficient h 1,i0 >0, for The rate of change of the formation error is calculated using the absolute value of the formation error |E i0 | and the absolute value of the sliding mode function |S i0 | is observed by multiplying the coefficient h 2,i0 >0; The distributed cooperative sliding mode formation controller described in step 6 is defined as follows: in, ω i are the speed and angular velocity of the X, Y, and Z axes of the i-th UAV’s body coordinate system, is the set of speed control inputs, H ij is the coefficient matrix, F ij is the formation error matrix between the i-th UAV and the j-th UAV, is the matrix H ij The inverse matrix, H i0 is the coefficient matrix, F i0 is the formation error matrix between the i-th UAV and the pilot UAV, is the matrix H i0 The inverse matrix of , control gain b1>0, control gain b2>0, index γ2∈(0,1); Each following UAV adjusts its own flight speed through the distributed sliding mode formation controller, which can achieve a stable formation of multiple UAVs under uncertain interference.

2. The UAV sliding mode formation control method according to claim 1, characterized in that: Step 1 constructs a directed graph of the follow-up drone communication topology, as follows: Define the set of follow-up drones as: Γ={1,...,n} in, n is the number of following drones; Define the pilot drone number as 0; Each follower drone in the follower drone set is regarded as a node in the follower drone communication topology directed graph; The following drone communication topology directed graph is defined as follows: in, represents a set of vertices, represents the set of edges, (i,j) represents the communication relationship between the i-th following drone and the j-th following drone, represents the adjacency matrix. If the i-th follower drone can receive the information of the j-th follower drone, then the j-th follower drone is called the communication neighbor of the i-th follower drone and a ij > 0, otherwise a ij =0, represents the set of communication neighbor following drones of the i-th following drone, and n represents the number of following drones.

3. The UAV sliding mode formation control method according to claim 2, characterized in that: The communication topology directed graph of the follow-up drone is a distributed topology directed graph; The motion trajectory of the pilot drone is generated by the system, and only one follower drone in the set of follower drones communicates with the pilot drone and obtains the motion trajectory parameters that need to be tracked; The remaining follower drones in the follower drone set only exchange information with each neighbor follower drone in the set of neighbor follower drones, and determine their own control input according to the motion state of each neighbor follower drone in the set of neighbor follower drones.

4. The UAV sliding mode formation control method according to claim 3, characterized in that: In step 2, the two-dimensional map coordinate system of each unmanned aircraft follows the initial position of each unmanned aircraft as the origin of the two-dimensional map coordinate system; Each follower drone collects the vertical height of each follower drone through a laser ranging module; Step 2 converts the two-dimensional coordinates of each follower drone in the two-dimensional map coordinate system into the coordinates of each follower drone in the inertial coordinate system, as follows: in, are the coordinates of the X-axis and Y-axis of the i-th following drone in the inertial coordinate system when it is at the initial position, are the coordinates of the X-axis and Y-axis of the i-th following drone in the corresponding two-dimensional map coordinate system, and the coordinates of the X-axis and Y-axis of the i-th following drone in the inertial coordinate system, and n represents the number of following drones.

5. The UAV sliding mode formation control method according to claim 4, characterized in that: Step 3 constructs the body coordinate system as follows: Define the origin of the body coordinate system at the center of gravity of the drone, the X-axis of the body coordinate system points to the direction of the drone's nose, the Y-axis of the body coordinate system points from the origin to the right side of the drone, and the Z-axis direction of the body coordinate system passes through X, and Y is determined by the right-hand rule; The motion model of each following drone described in step 3 is specifically defined as follows: in, are the speed of the i-th following drone in the X-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Y-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Z-axis direction in the inertial coordinate system, and the angular velocity of the i-th following drone in the inertial coordinate system, ω i is the angular velocity of the i-th following UAV in the body coordinate system, They represent the disturbances caused by internal parameter uncertainty and external disturbances on the linear velocity and yaw angular velocity of the X, Y, and Z axes of the i-th following UAV in the inertial coordinate system. They are respectively the linear velocity of the i-th following drone in the X-axis direction of the body coordinate system, the linear velocity of the i-th following drone in the Y-axis direction of the body coordinate system, and the linear velocity of the i-th following drone in the Z-axis direction of the body coordinate system.

6. The UAV sliding mode formation control method according to claim 5, characterized in that: The relationship model of the relative distance and angle between any two communication neighbors following the drone on the X, Y, and Z axes in the body coordinate system described in step 4 is defined as follows: in, They respectively represent the relative distance of the i-th following drone in the X direction in the body coordinate system of the j-th following drone, the relative distance of the i-th following drone in the Y direction in the body coordinate system of the j-th following drone, and the relative distance of the i-th following drone in the Z direction in the body coordinate system of the j-th following drone. is the yaw angle difference of the i-th following UAV in the body coordinate system of the j-th following UAV; If j=0, the jth following drone represents the virtual pilot node; If j∈N i When , the jth following drone represents the communication neighbor node of the i-th following drone; N i represents the set of communication neighbor following drones of the i-th following drone; x i ,y i ,z i ,ψ i are the speed of the i-th following drone in the X-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Y-axis direction in the inertial coordinate system, the speed of the i-th following drone in the Z-axis direction in the inertial coordinate system, and the angular velocity of the i-th following drone in the inertial coordinate system; x j ,y j ,z j ,ψ j They are respectively the speed of the j-th following drone in the X-axis direction in the inertial coordinate system, the speed of the j-th following drone in the Y-axis direction in the inertial coordinate system, the speed of the j-th following drone in the Z-axis direction in the inertial coordinate system, and the angular velocity of the j-th following drone in the inertial coordinate system.

7. The UAV sliding mode formation control method according to claim 6, characterized in that: The formation error model containing the disturbance term is constructed as described in step 4, as follows: in, is the set of formation errors and yaw angle errors of the i-th communication neighbor following UAV and the j-th communication neighbor following UAV in the X, Y, and Z axis directions of the body coordinate system, YesE ij The rate of change, is the set of velocity and angular velocity of the X, Y, and Z axes of the i-th UAV body coordinate system, H ij is the coefficient matrix, F ij is the formation error matrix between the i-th UAV and the j-th UAV, D ij is the set of disturbance terms of the i-th UAV and the j-th UAV formation.

Images