CN116880193B - Control method of multi-unmanned vehicle cooperative system, control device, terminal and medium thereof - Google Patents

Abstract

The invention discloses a control method of a multi-unmanned vehicle cooperative system, a control device, a terminal and a medium thereof, comprising the following steps: designing a control input model of a controller of each vehicle based on the mechanical system dynamics model; according to the control input model of each vehicle, the pilot vehicle is controlled to be mainly responsible for track tracking tasks by designing each parameter of the control input model, and the following vehicle is responsible for keeping a desired formation with the pilot vehicle, so that overall track tracking is realized. According to the invention, through carrying out normalization processing on collision avoidance constraint, formation constraint and track tracking constraint, an analytical dynamics model of the multi-unmanned vehicle system is established by adopting a black-bone method, and track tracking control of the multi-unmanned vehicle system based on constraint following is provided. The modeling process is clear, simple and convenient, has strong expansibility, is suitable for dynamic analytic modeling of any number of multi-unmanned vehicle systems, and has higher model precision and control precision.

Description

Control method and control device, terminal and medium of multi-unmanned vehicle collaborative system

Technical field

The present invention relates to a control method in the field of collaborative control of multiple unmanned vehicles, specifically a control method of a pilot-following multiple unmanned vehicle collaborative system based on the Uka model, and a control device corresponding to the control method. A Uka model-based pilot-following multi-unmanned vehicle collaborative system using the control method, a computer terminal executing computer program instructions for implementing the control method, and a readable storage storing computer program instructions for implementing the control method medium.

Background technique

Multi-unmanned vehicle systems are a branch of multi-agent systems. Research on formation tracking control mainly includes leader-follower methods, behavior-based methods, virtual structure methods, etc. The leader-follower method sets different positional relationships between the leader agent (pilot car) and the follower agent (follower car), and the follower tracks the position and direction of the leader agent with set distance, speed and other parameters. Form different formations. The outstanding feature of this method is that collaboration among members is achieved through status information sharing of the pilot vehicle.

Although there have been many achievements in research on formation tracking control of leader-follower multi-unmanned vehicle systems, there are few studies on the establishment of analytical models of formation and tracking constraints. Moreover, the collision avoidance constraints and pilot trajectory constraints of unmanned workshops are not normalized in the current research. The existing dynamics model is not accurate and the proposed control method is too ideal.

Contents of the invention

In order to solve the problems of inaccurate dynamic modeling and low control accuracy in the existing multi-unmanned vehicle system formation tracking control, the present invention proposes a control method for the pilot-following multi-unmanned vehicle collaborative system based on the Uka model. A control device corresponding to the control method, a pilot-following multi-unmanned vehicle collaborative system based on the Uka model using the control method, a computer terminal that executes computer program instructions for implementing the control method, and a computer terminal that stores the instructions for implementing the control method. The readable storage medium of computer program instructions of the control method can satisfy the global collision avoidance of members during the movement of multiple unmanned vehicles and realize the trajectory tracking control of the multiple unmanned vehicle system.

The present invention adopts the following technical solution to realize: a control method of a pilot-following multi-unmanned vehicle collaborative system based on the Uka model. In the N unmanned vehicle collaborative system, any one unmanned vehicle is selected as the pilot vehicle and belongs to the set L , the remaining N-1 unmanned vehicles are follower vehicles and belong to set F; the control method includes the following steps:

Step S1, based on the mechanical system dynamics model, design the control input model of the controller of each vehicle;

The mechanical system dynamics model is:

In the formula, i∈{1,2,…,N} represents the serial number of the unmanned vehicle, M _i represents the mass matrix of the i-th unmanned vehicle in the collaborative system, q _i =[x _i , y _i ] ^T represents the The generalized coordinates of i unmanned vehicle, x _i is the abscissa coordinate of i-th unmanned vehicle, y _i is the ordinate coordinate of i-th unmanned vehicle, represents the Coriolis force of the i-th unmanned vehicle, F _i = [F _ix , F _iy ] ^T represents the sum of external forces on the i-th unmanned vehicle, τ _i (t) = [τ _ix ( t), τ _iy (t)] ^T represents the control input of the i-th unmanned vehicle over time t;

The control input model is:

In the formula, B _i =A _i M _i ^-1/2 , b _i represents the second-order constraint vector of the i-th unmanned vehicle, A _i represents the constraint matrix of the i-th unmanned vehicle, and τ _i2 (t) represents the control Feedback control part of the input;

Step S2, according to the control input model of each vehicle, by designing various parameters of the control input model, the control lead vehicle is mainly responsible for the trajectory tracking task, and the following vehicle is responsible for maintaining the desired formation with the lead vehicle, thereby achieving overall trajectory tracking;

Among them: the design method of each parameter of the control input model is to meet the following conditions:

(1), τ _i2 (t)=-γ _i M _i ^1/2 B _i ⁺ η _i

In the formula, γ _i is the constant control parameter of the i-th unmanned vehicle, γ _i > 0, η _i is the constrained following error of the i-th unmanned vehicle, c _i represents the first-order constraint vector of the i-th unmanned vehicle;

(2),

In the formula,

The trajectory tracking constraint matrix of the i-th unmanned vehicle The formation constraint matrix of the i-th unmanned vehicle

The first-order trajectory tracking constraint vector of the i-th unmanned vehicle

The first-order formation constraint vector of the i-th unmanned vehicle

The second-order trajectory tracking constraint vector of the i-th unmanned vehicle

The second-order formation constraint vector of the i-th unmanned vehicle

For any i∈{1,2,…,N}, j∈{1,2,…,N},i≠j,r _i represents the safety radius with the center of mass of the i-th unmanned vehicle as the center of the circle, r _j Represents the safety radius with the center of mass of the jth unmanned vehicle as the center of the circle,

Then the collision avoidance constraint matrix between the i-th unmanned vehicle and the j-th unmanned vehicle is for,

The first-order collision avoidance constraint vector between the i-th unmanned vehicle and the j-th unmanned vehicle for,

The second-order collision avoidance constraint vector between the i-th unmanned vehicle and the j-th unmanned vehicle for,

After sorting, the collision avoidance constraint matrix that the i-th unmanned vehicle needs to satisfy First-order collision avoidance constraint vector/> Second-order collision avoidance constraint vector/> It can be summarized as follows:

When i=1,

When i=2,3,…,N-1,

When i=N,

In the formula, l _d represents a positive constant parameter, x _d is the abscissa of the destination of the i-th unmanned vehicle, y _d is the ordinate of the destination of the i-th unmanned vehicle, l _il represents a constant parameter, l _il > 0, Indicates the relative position of the following vehicle and the leading vehicle,/> Represents the collision avoidance constraint matrix between the i-th unmanned vehicle and the N-th unmanned vehicle, /> Represents the first-order collision avoidance constraint vector between the i-th unmanned vehicle and the N-th unmanned vehicle,/> Represents the second-order collision avoidance constraint vector between the i-th unmanned vehicle and the N-th unmanned vehicle,/> Represents the collision avoidance constraint matrix between the i-th unmanned vehicle and the first unmanned vehicle, /> Represents the first-order collision avoidance constraint vector between the i-th unmanned vehicle and the first unmanned vehicle,/> Represents the second-order collision avoidance constraint vector between the i-th unmanned vehicle and the first unmanned vehicle.

The present invention also provides a control device for a pilot-following multi-unmanned vehicle collaborative system based on the Ukka model, which applies the above-mentioned control method for a pilot-following multiple unmanned vehicle collaborative system based on the Ukka model. The control device The device includes: a control input model setting module, which is used to design the control input model of the controller of each vehicle based on the mechanical system dynamics model; a parameter design module, which is used to design the control based on the control input model of each vehicle. Enter various parameters of the model.

The present invention also provides a computer terminal, which includes a memory, a processor and a computer program stored on the memory and executable on the processor, wherein when the processor executes the program, the above-mentioned The steps of the control method of the pilot-follower multi-unmanned vehicle collaborative system based on the Uka model.

The present invention also provides a readable storage medium. Computer program instructions are stored in the readable storage medium. When the computer program instructions are read and run by a processor, the above-mentioned pilot-following multi-step based on Uka model is realized. The steps of the control method of the human-vehicle collaborative system.

The present invention normalizes collision avoidance constraints, formation constraints and trajectory tracking constraints, uses the Uka method to establish an analytical dynamics model of a multi-unmanned vehicle system, and proposes a multi-unmanned vehicle system based on constraint following. Trajectory tracking control is used to establish an analytical dynamics model of multiple unmanned vehicle systems. Not only is the modeling process clear and simple, it is also suitable for analytical modeling of the dynamics of any number of multiple unmanned vehicle systems, with higher model accuracy and control. Accuracy.

Description of drawings

Figure 1 is a flow chart of the control method of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model of the present invention.

Figure 2 is a schematic diagram of the pilot vehicle trajectory tracking using the control method in Figure 1.

Figure 3 is a schematic diagram of the tracking error in the X and Y directions of the pilot vehicle using the control method in Figure 1.

Figure 4 is a schematic diagram of the trajectory tracking of each vehicle using the control method in Figure 1.

Figure 5 is a schematic diagram of the distance between the following vehicle and the leading vehicle using the control method in Figure 1.

Figure 6 is a schematic diagram of the distance between adjacent following vehicles using the control method in Figure 1.

Figure 7 is a schematic diagram of the distance between non-adjacent following vehicles using the control method in Figure 1.

Figure 8 is a schematic diagram of the distance error between each following vehicle and the leading vehicle using the control method in Figure 1.

Figure 9 is a schematic diagram of the control input of each vehicle in the X direction using the control method in Figure 1.

Figure 10 is a schematic diagram of the control input of each vehicle in the Y direction using the control method in Figure 1.

Detailed ways

In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described here are only used to explain the present invention and are not intended to limit the present invention.

Please refer to Figure 1, which is a flow chart of the control method of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model of the present invention. The control method requires selecting any one unmanned vehicle in the N unmanned vehicle collaborative system as the pilot vehicle, which belongs to the set L, and the remaining N-1 unmanned vehicles are follower vehicles, which belongs to the set F. The present invention can satisfy the needs of many unmanned vehicles. The global collision avoidance of members during the movement of people and vehicles and the trajectory tracking control of multi-unmanned vehicle systems.

The control method mainly includes the following steps: Step S1, based on the mechanical system dynamics model, design the control input model of the controller of each vehicle; Step S2, based on the control input model of each vehicle, by designing each of the control input models Parameters, controlling the leading vehicle is mainly responsible for the trajectory tracking task, while the following vehicle is responsible for maintaining the desired formation with the leading vehicle, thereby achieving overall trajectory tracking.

The design key point of the present invention lies in how to realize the global collision avoidance of members during the movement of multiple unmanned vehicles and the trajectory tracking control of the multi-unmanned vehicle system by designing various parameters of the control input model.

In step S1, the mechanical system dynamics model is:

In the formula, i∈{1,2,…,N} represents the serial number of the unmanned vehicle, M _i represents the mass matrix of the i-th unmanned vehicle in the collaborative system, q _i =[x _i , y _i ] ^T represents the The generalized coordinates of i unmanned vehicle, x _i is the abscissa coordinate of i-th unmanned vehicle, y _i is the ordinate coordinate of i-th unmanned vehicle, represents the Coriolis force of the i-th unmanned vehicle, F _i = [F _ix , F _iy ] ^T represents the sum of external forces on the i-th unmanned vehicle, τ _i (t) = [τ _ix ( t), τ _iy (t)] ^T represents the control input of the i-th unmanned vehicle over time t.

The control input model is:

In the formula, B _i =A _i M _i ^-1/2 , b _i represents the second-order constraint vector of the i-th unmanned vehicle, A _i represents the constraint matrix of the i-th unmanned vehicle, and τ _i2 (t) represents the control Feedback control section of the input.

These two models are classic models and will not be described in detail here.

The design key point of the present invention lies in the design of each parameter of the control input model, and the design method is to satisfy the following two conditions.

(1), τ _i2 (t)=-γ _i M _i ^1/2 B _i ⁺ η _i

In the formula, γ _i is the constant control parameter of the i-th unmanned vehicle, γ _i > 0, η _i is the constrained following error of the i-th unmanned vehicle, c _i represents the first-order constraint vector of the i-th unmanned vehicle.

(2),

In the formula,

The trajectory tracking constraint matrix of the i-th unmanned vehicle The formation constraint matrix of the i-th unmanned vehicle

The first-order trajectory tracking constraint vector of the i-th unmanned vehicle

The first-order formation constraint vector of the i-th unmanned vehicle

The second-order trajectory tracking constraint vector of the i-th unmanned vehicle

The second-order formation constraint vector of the i-th unmanned vehicle

For any i∈{1,2,…,N}, j∈{1,2,…,N},i≠j,r _i represents the safety radius with the center of mass of the i-th unmanned vehicle as the center of the circle, r _j Represents the safety radius with the center of mass of the jth unmanned vehicle as the center of the circle,

Then the collision avoidance constraint matrix between the i-th unmanned vehicle and the j-th unmanned vehicle is for,

The first-order collision avoidance constraint vector between the i-th unmanned vehicle and the j-th unmanned vehicle for,

The second-order collision avoidance constraint vector between the i-th unmanned vehicle and the j-th unmanned vehicle for,

After sorting, the collision avoidance constraint matrix that the i-th unmanned vehicle needs to satisfy First-order collision avoidance constraint vector/> Second-order collision avoidance constraint vector/> It can be summarized as follows:

When i=1,

When i=2,3,…,N-1,

When i=N,

In the formula, l _d represents a positive constant parameter, x _d is the abscissa of the destination of the i-th unmanned vehicle, y _d is the ordinate of the destination of the i-th unmanned vehicle, l _il represents a constant parameter, l _il > 0, Indicates the relative position of the following vehicle and the leading vehicle,/> Represents the collision avoidance constraint matrix between the i-th unmanned vehicle and the N-th unmanned vehicle, /> Represents the first-order collision avoidance constraint vector between the i-th unmanned vehicle and the N-th unmanned vehicle,/> Represents the second-order collision avoidance constraint vector between the i-th unmanned vehicle and the N-th unmanned vehicle,/> Represents the collision avoidance constraint matrix between the i-th unmanned vehicle and the first unmanned vehicle, /> Represents the first-order collision avoidance constraint vector between the i-th unmanned vehicle and the first unmanned vehicle,/> Represents the second-order collision avoidance constraint vector between the i-th unmanned vehicle and the first unmanned vehicle.

Compared with the existing technology, the present invention uses collision avoidance constraints, formation constraints and trajectory tracking constraints for normalization processing, and uses the Uka method to establish an analytical dynamics model of a multi-unmanned vehicle system. The modeling process is clear and simple. It is suitable for dynamic analytical modeling of any number of multi-unmanned vehicle systems, with higher model accuracy and control accuracy.

In this embodiment, the control method further includes the following three steps.

(1) Method to reselect the pilot car.

The method of reselecting the pilot car includes the following steps:

Step S3: During the movement of N unmanned vehicles, when the pilot vehicle is re-selected, the control signals that need to be received for the selected pilot vehicle i (i∈L) include: the trajectory tracking constraint matrix of the pilot vehicle i The first-order trajectory tracking constraint vector of pilot vehicle i/> Second-order trajectory tracking constraint vector of pilot vehicle i/> The collision avoidance constraint matrix of the pilot vehicle i and any other unmanned vehicle j (j∈{1,2,…,N}, j≠i)/> The first-order collision avoidance constraint vector of the pilot vehicle i and any other unmanned vehicle j/> The second-order collision avoidance constraint vector of the pilot vehicle i and any other unmanned vehicle j/>

Step S4, for the restored following vehicle i (i∈F), the control signals that need to be received include: the formation constraint matrix of following vehicle i The first-order formation constraint vector of following vehicle i/> Second-order formation constraint vector following vehicle i/> Collision avoidance constraint matrix of following vehicle i and any other unmanned vehicle j (j∈{1,2,…,N}, j≠i)/> The first-order collision avoidance constraint vector of following vehicle i and any other unmanned vehicle j/> The second-order collision avoidance constraint vector of following vehicle i and any other unmanned vehicle j/>

(2) The exit method of unmanned vehicles during the movement of N unmanned vehicles.

The exit method of an unmanned vehicle during the movement of N unmanned vehicles includes the following steps:

If the exiting unmanned vehicle is the pilot vehicle, step S3 needs to be executed first, and then step S5 is executed; otherwise, step S5 is executed directly;

Step S5, for the unmanned vehicle i that needs to exit the movement process, the control signals that need to be received include: the relationship between the unmanned vehicle i and any other unmanned vehicle j (j∈{1,2,…,N}, j≠i) Collision avoidance constraint matrix The first-order collision avoidance constraint vector of unmanned vehicle i and any other unmanned vehicle j/> The second-order collision avoidance constraint vector of unmanned vehicle i and any other unmanned vehicle j/>

(3) How to change the target position.

How to change the target location involves the following steps:

Step S6, change the target position, and the control signal received by each unmanned vehicle is: update x _d , y _d .

In the specific implementation process, the control method of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model of the present invention can be set in the form of software, such as designed as an independent APP, or can be called at any time The embedded software is used in computer terminals (such as remote controllers for multi-unmanned vehicle coordination systems). The computer terminal includes a memory, a processor, and a computer program stored on the memory and executable on the processor. The computer terminal can also be a smartphone, tablet computer, notebook computer, etc. that can execute programs. In some embodiments, the processor may be a central processing unit (CPU), a controller, a microcontroller, a microprocessor, or other data processing chips. The processor is typically used to control the overall operation of a computer device. In this embodiment, the processor is used to run program codes stored in the memory or process data. When the processor executes the program, the steps of the control method of the pilot-following multi-unmanned vehicle collaborative system based on the Ukka model of the present invention can be implemented.

When the control method of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model of the present invention is applied, it can also be designed to store computer program instructions in a readable storage medium, such as a U-shield, and various types of data stored in the U-shield. When the computer program instructions are read and run by a processor, the steps of the control method of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model of the present invention can be realized. In the form of a U-shield, when it can be plugged into the data interface of a traditional remote control through electronic plugging, the CPU of the remote control can read and execute the computer program instructions in the U-shield, thus solving the problem. There are problems such as inaccurate dynamic modeling and low control accuracy in the formation tracking control of multi-unmanned vehicle systems. It is necessary to improve the sensitivity and accuracy of the remote control. Therefore, the present invention can also very conveniently upgrade the software of the existing remote control, thereby facilitating the promotion and application of the present invention.

Whether it is non-embedded or embedded, it can be summarized as the corresponding control device of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model. The control device of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model includes a control input model setting module for designing the control input model of the controller of each vehicle based on the mechanical system dynamics model; The control input model of each vehicle is used to design a parameter design module for each parameter of the control input model. The control input model setting module and parameter design module perform the above two steps respectively.

In order to demonstrate the feasibility of the technical solution of the present invention, detailed theoretical derivation and experimental simulation are carried out in this embodiment.

(1)Theoretical derivation

First, mechanical system dynamics model setting

A collaborative system composed of N unmanned vehicles is selected for target trajectory tracking control design.

Its kinetic equation is as follows

Where i∈{1,2,…,N} represents the vehicle number, M _i represents the mass matrix of the i-th unmanned vehicle in the collaborative system, q _i =[x _i ,y _i ] ^T represents the i-th unmanned vehicle The generalized coordinates of represents the Coriolis force of the i-th unmanned vehicle, F _i =[F _ix , F _iy ] ^T represents the sum of other external forces on the i-th unmanned vehicle, τ _i =[τ _ix ,τ _iy ] ^T represents the control input of the i-th unmanned vehicle.

All vehicles are divided into 1 leading vehicle and (N-1) following vehicles, where the leading vehicle belongs to the set L and the following vehicles belong to the set F. All vehicles do not collide with each other. The leading vehicle is mainly responsible for the trajectory tracking task, and the following vehicle is responsible for maintaining the desired formation with the leading vehicle, thereby achieving overall trajectory tracking.

Second, the trajectory tracking constraint design of the pilot vehicle

Assume that the target trajectory of the given pilot vehicle is q _d (t) = [x _d (t), y _d (t)] ^T , and assume that the i-th vehicle is selected as the pilot vehicle, i∈L, and its coordinates are q _i ( t)=[x _i (t),y _i (t)] ^T .

Then define the trajectory tracking error _ed (t) as _ed (t) = q _i (t)-q _d (t), and define the first-order trajectory tracking constraint as Among them, l _d > 0 is a constant, which can be obtained by substituting into:

Among them/> is the trajectory tracking constraint matrix, /> is the first-order trajectory tracking constraint vector;

The second-order trajectory tracking constraint is Substitute and organize to get:

Among them/> is the second-order trajectory tracking constraint vector.

specifically,

Third, the desired formation constraint design when the following vehicle gathers the formation towards the lead vehicle

Assume that the remaining N-1 vehicles serve as following vehicles. For any j∈F, the position coordinates of the jth following vehicle are q _j (t) = [x _j (t), y _j (t)] ^T , that is, j∈ F. The desired formation constraint when considering the remaining following vehicles gathering towards the lead vehicle is:

Among them, q _j (t) and q _l (t) represent the positions of the following unmanned vehicle and the leading unmanned vehicle respectively, q _j (t) = [x _j (t), y _j (t)] ^T q _l (t )＝[x _l (t),y _l (t)] ^T , Indicates the relative position of the following unmanned vehicle and the pilot vehicle,/> and Represents the ideal formation distance in the x and y directions respectively. The above formula can be regarded as the zero-order form of formation constraints. By setting the relative positions of each following vehicle and the leading vehicle, the desired ideal formation can be formed.

Using the constraint form of formation error, the desired formation error can be defined from the above desired formation constraints as follows:

Assume that for any initial state q _j (t ₀ ), there is a time T _j <∞, and for all t ≥ T _j :

Among them/> is an arbitrarily small constant. Will make the formation error/> Enter any small range within a limited time T _j /> The formation method is called approximate formation.

The following constraint form is set to achieve approximate formation:

Among them, l _jl > 0 is a constant. This constraint includes the formation error e _jl and its first derivative. And when t→∞, the expected formation error e _jl (t) will converge to 0.

pairs The derivative is used to obtain the second-order constraint form:

Calculating the first and second derivatives of the desired formation error:

Based on the above formula, the first-order constraint form and the second-order constraint are as follows:

Write the first-order form and the second-order form as follows:

in,

Fourth, the collision avoidance constraint design is followed between any two vehicles.

To ensure that no vehicle collision occurs, collision avoidance constraints need to be followed between any two vehicles. The specific collision avoidance design is as follows:

Suppose the distance between any two vehicles is ΔS _ij (t),

ΔS _ij (t)=q _i (t)-q _j (t)-(r _i +r _j )

Among them, q _i and q _j represent the coordinates of the center of mass of any two vehicles, where i, j∈{1,2,...,N}, i≠j, and r _i represents the safety radius with the center of mass of the i-th vehicle as the center of the circle. When ΔS _ij (t)<0, collisions between vehicles may occur. Further definition for calculation convenience:

S _ij (t)＝||q _i (t)-q _j (t)|| ² -(r _i +r _j ) ²

Factor S _ij as follows:

S _ij (t)＝[||q _i (t)-q _j (t)||-(r _i +r _j )][||q _i (t)-q _j (t)||+(r _i +r _j )]

When S _ij (t)＜0, the actual distance ΔS _ij (t)＜0 of any two vehicles; when S _ij (t)＞0, the actual distance ΔS _ij (t)＞0 of any two vehicles, set S The sign of _ij (t) is used as the basis for judging whether a collision occurs.

Any safe initial state S _ij (t ₀ )＞0. If S _ij ＞0 holds for any t≥t ₀ , the multi-agent system is said to have global collision avoidance. In order to achieve global collision avoidance of the multi-unmanned vehicle collaborative system, the following collision avoidance function is constructed:

The collision avoidance function is a logarithmic function about S _ij (t). Since the logarithmic function has domain constraints, the function has an implicit constraint: ΔS _ij (t)>0. Find the first and second derivatives of this function:

From this, the first-order collision avoidance constraint is constructed as

Right now:

The second-order collision avoidance constraints are:

Right now:

For any two vehicles i,j∈{1,2,...,N},i≠j in the collaborative system, their first-order collision avoidance constraints and second-order collision avoidance constraints can be written in the following form:

in,

For a collaborative system composed of N unmanned vehicles, a total of N(N-1)/2 collision avoidance constraints need to be imposed. Since the collision avoidance constraints are applied to the corresponding two unmanned vehicles, for the same collision avoidance constraint, you only need to select any one of the two vehicles as the object to which the collision avoidance constraints are applied. In order to prevent excessive constraints when collision avoidance constraints are applied to two vehicles at the same time, a total of N(N-1)/2 collision avoidance constraints are distributed and summarized into the following form:

When i=1,

When i=2,3,…,N-1,

When i=N,

Fifth, the design of the trajectory tracking controller of the unmanned vehicle collaborative system

All the constraints of the leading unmanned vehicle and the following unmanned vehicle have been designed. Now all the constraints are allocated and summarized. For any vehicle i in the collaborative system, the constraints it is subject to are:

in,

Based on the Udwadia-Kalaba equation, the ideal binding force of the unmanned vehicle i subject to the above constraints is:

Among them, B _i =A _i M _i ^-1/2 ; the above formula can be used as the control input of each unmanned vehicle under an ideal state (ie, no uncertainty and no initial error). Control of the i-th vehicle under ideal conditions

Considering that at the initial moment of the collaborative system, each unmanned vehicle is scattered and has no formation relationship, so the following control input needs to be added to suppress the initial error:

τ _i2 =-γ _i M _i ^1/2 B _i ⁺ η _i

Among them, η _i is the constrained following error,

γ _i >0 is a constant control parameter. The larger γ _i is, the faster the convergence speed of eta _i will be.

The trajectory tracking controller of the multi-unmanned vehicle collaborative system is designed as:

τ _i (t)=τ _i1 +τ _i2

(2) Experimental simulation: Verify the effectiveness of the control method through experiments.

Figure 2 reflects the tracking of the target trajectory of the pilot vehicle. It can be seen that the actual trajectory of the pilot vehicle basically coincides with the target trajectory, with only a slight deviation at the initial moment, and the deviation is about 0.1. Figure 3 reflects the trajectory tracking error of the pilot vehicle in the X and Y axis directions. The error changes greatly in the time periods of 0-5 seconds and 20-30 seconds. The reasons are respectively caused by the formation adjustment within 0-5 seconds. The impact is also due to the fact that the pilot car is in the middle part of the trajectory within 20-30 seconds and the trajectory curve changes significantly. During the entire tracking process, the tracking error in the X direction is less than 0.1m, and the error in the Y direction is only less than 0.04m. Based on Figure 4 and Figure 5, the trajectory tracking of the pilot vehicle in the collaborative system is good, which lays an important foundation for other following vehicles to achieve good tracking.

Figures 6 to 7 respectively show the changes in the distance between the vehicles during the trajectory tracking process of the multi-unmanned vehicle collaborative system. It can be seen that the distances of each vehicle tend to stabilize after the 10th second. The vehicle distance shown in Figures 5 and 6 is stable at about 6m. These distances respectively correspond to the side lengths of the designed regular hexagonal formation and the distance from each vertex to the center. The distance between each vehicle fluctuates slightly in the 20th to 30th second. The reason is that the change in the middle of the trajectory curve is large, but the fluctuation amplitude is small, and the peak value of the fluctuation is only about 0.1m. Figure 7 shows the distance between non-adjacent vehicles in this formation, which is stable at around 10.4m and 12m respectively, corresponding to different diagonal distances of a regular hexagon. In addition, it can be seen that the distance between each vehicle is greater than the safe distance throughout the entire process, indicating that no vehicle collision occurred during the entire process. The results shown in Figures 5 to 7 jointly verify that the designed controller can better meet the desired formation constraints.

Figure 8 shows the changes in the distance error between each following vehicle and the leading vehicle. It can be seen that the distance error approaches 0 after the 10th second. Around the 25th second, the vehicle distance error fluctuates slightly. The reason is that at the 25th second, each vehicle is in the middle of the trajectory curve and the speed changes greatly, but the error still remains at about 0.1m.

Figures 9 and 10 respectively show the control inputs in the X and Y directions of each unmanned vehicle. It can be seen that the control inputs in the X and Y directions are large at the beginning, with the maximum amplitude around 400N. The reason is that the formation error is large at the beginning, and higher control input is required to achieve formation behavior. After the 5th second, the control input becomes significantly smaller and the rate of change begins to slow down. The control input in the X direction remains stable after that, while the control input in the Y direction remains basically stable between 8 and 20 seconds. An obvious change occurred after 20 seconds, showing a change similar to the sinusoidal function waveform. The reason was that the speed changed in the middle of each vehicle's trajectory curve, and the control input after that also returned to the previous state. In addition, the stable values of the control inputs of each vehicle in the X and Y directions are different. This is due to the differences in the mass, external force and other parameters of each unmanned vehicle.

The above descriptions are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention shall be included in the protection of the present invention. within the range.

Claims (10)

1. A control method for a pilot-follower multi-unmanned vehicle collaborative system based on the Uka model. Select any unmanned vehicle in the N unmanned vehicle collaborative system as the pilot vehicle, which belongs to the set L, and the remaining N-1 vehicles The unmanned vehicle is a follower vehicle and belongs to the set F; the control method includes the following steps: Step S1, based on the mechanical system dynamics model, design the control input model of the controller of each vehicle; The mechanical system dynamics model is: In the formula, i∈{1,2,…,N} represents the serial number of the unmanned vehicle, M _i represents the mass matrix of the i-th unmanned vehicle in the collaborative system, q _i =[x _i , y _i ] ^T represents the The generalized coordinates of i unmanned vehicle, x _i is the abscissa coordinate of i-th unmanned vehicle, y _i is the ordinate coordinate of i-th unmanned vehicle, represents the Coriolis force of the i-th unmanned vehicle, F _i = [F _ix , F _iy ] ^T represents the sum of external forces on the i-th unmanned vehicle, τ _i (t) = [τ _ix ( t), τ _iy (t)] ^T represents the control input of the i-th unmanned vehicle over time t; The control input model is: In the formula, B _i =A _i M _i ^-1/2 , b _i represents the second-order constraint vector of the i-th unmanned vehicle, A _i represents the constraint matrix of the i-th unmanned vehicle, and τ _i2 (t) represents the control Feedback control part of the input; Step S2, according to the control input model of each vehicle, by designing various parameters of the control input model, the control lead vehicle is mainly responsible for the trajectory tracking task, and the following vehicle is responsible for maintaining the desired formation with the lead vehicle, thereby achieving overall trajectory tracking; It is characterized in that: each parameter design method of the control input model satisfies the following conditions: (1)τ _i2 (t)＝-γ _i M _i ^1/2 B _i ⁺ η _i In the formula, γ _i is the constant control parameter of the i-th unmanned vehicle, γ _i > 0, η _i is the constrained following error of the i-th unmanned vehicle, c _i represents the first-order constraint vector of the i-th unmanned vehicle; (2) In the formula, The trajectory tracking constraint matrix of the i-th unmanned vehicle The formation constraint matrix of the i-th unmanned vehicle The first-order trajectory tracking constraint vector of the i-th unmanned vehicle The first-order formation constraint vector of the i-th unmanned vehicle The second-order trajectory tracking constraint vector of the i-th unmanned vehicle The second-order formation constraint vector of the i-th unmanned vehicle For any i∈{1,2,…,N}, j∈{1,2,…,N},i≠j,r _i represents the safety radius with the center of mass of the i-th unmanned vehicle as the center of the circle, r _j Represents the safety radius with the center of mass of the jth unmanned vehicle as the center of the circle, Then the collision avoidance constraint matrix between the i-th unmanned vehicle and the j-th unmanned vehicle is for, The first-order collision avoidance constraint vector between the i-th unmanned vehicle and the j-th unmanned vehicle for, The second-order collision avoidance constraint vector between the i-th unmanned vehicle and the j-th unmanned vehicle for, The collision avoidance constraint matrix that the i-th unmanned vehicle needs to satisfy First-order collision avoidance constraint vector/> Second-order collision avoidance constraint vector It can be summarized as follows: When i=1, When i=2,3,…,N-1, When i=N, In the formula, l _d represents a positive constant parameter, x _d is the abscissa of the destination of the i-th unmanned vehicle, y _d is the ordinate of the destination of the i-th unmanned vehicle, l _il represents a constant parameter, l _il > 0, Indicates the relative position of the following vehicle and the leading vehicle,/> Represents the collision avoidance constraint matrix between the i-th unmanned vehicle and the N-th unmanned vehicle, /> Represents the first-order collision avoidance constraint vector between the i-th unmanned vehicle and the N-th unmanned vehicle,/> Represents the second-order collision avoidance constraint vector between the i-th unmanned vehicle and the N-th unmanned vehicle,/> Represents the collision avoidance constraint matrix between the i-th unmanned vehicle and the first unmanned vehicle, /> Represents the first-order collision avoidance constraint vector between the i-th unmanned vehicle and the first unmanned vehicle,/> Represents the second-order collision avoidance constraint vector between the i-th unmanned vehicle and the first unmanned vehicle. 2. The control method of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model as claimed in claim 1, characterized in that, The design method of For the same collision avoidance constraint, you only need to select any one of the two unmanned vehicles as the object to which the collision avoidance constraints are applied. In order to prevent over-constraints in which the collision avoidance constraints are applied to both vehicles at the same time, A total of N(N-1)/2 collision avoidance constraints are allocated and summarized. 3. The control method of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model as claimed in claim 2, characterized in that, for any j∈F, the formation constraint matrix First-order formation constraint vector/> Second-order formation constraint vector/> The three design methods include the following steps: In order to achieve the target formation, that is, the desired formation, the following constraint form is set: In the formula, is the formation error, and when t→∞, the expected formation error e _jl (t) will converge to 0; pairs The derivative is used to obtain the second-order constraint form: Calculating the first and second derivatives of the desired formation error: In the formula, q _l , Represents the position, speed and acceleration of the pilot vehicle respectively; Based on the above formula, the first-order constraint form and the second-order constraint are as follows: Write the first-order form and the second-order form as follows: You can get: 4. The control method of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model as claimed in claim 1, characterized in that the control method further includes the following steps: Step S3: During the movement of N unmanned vehicles, when the pilot vehicle is re-selected, the control signals that need to be received for the selected pilot vehicle i (i∈L) include: the trajectory tracking constraint matrix of the pilot vehicle i The first-order trajectory tracking constraint vector of pilot vehicle i/> Second-order trajectory tracking constraint vector of pilot vehicle i/> The collision avoidance constraint matrix of the pilot vehicle i and any other unmanned vehicle j (j∈{1,2,…,N}, j≠i)/> The first-order collision avoidance constraint vector of the pilot vehicle i and any other unmanned vehicle j/> The second-order collision avoidance constraint vector of the pilot vehicle i and any other unmanned vehicle j/> Step S4, for the restored following vehicle i (i∈F), the control signals that need to be received include: the formation constraint matrix of following vehicle i The first-order formation constraint vector of following vehicle i/> Second-order formation constraint vector following vehicle i/> Collision avoidance constraint matrix of following vehicle i and any other unmanned vehicle j (j∈{1,2,…,N}, j≠i)/> The first-order collision avoidance constraint vector of following vehicle i and any other unmanned vehicle j/> The second-order collision avoidance constraint vector of following vehicle i and any other unmanned vehicle j/> 5. The control method of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model as claimed in claim 4, characterized in that the control method also includes the control of the unmanned vehicle during the movement of N unmanned vehicles. Exit method, which includes the following steps: If the exiting unmanned vehicle is the pilot vehicle, step S3 needs to be executed first, and then step S5 is executed; otherwise, step S5 is executed directly; Step S5, for the unmanned vehicle i that needs to exit the movement process, the control signals that need to be received include: the relationship between the unmanned vehicle i and any other unmanned vehicle j (j∈{1,2,…,N}, j≠i) Collision avoidance constraint matrix The first-order collision avoidance constraint vector of unmanned vehicle i and any other unmanned vehicle j/> The second-order collision avoidance constraint vector of unmanned vehicle i and any other unmanned vehicle j/> 6. The control method of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model as claimed in claim 4, characterized in that the control method further includes the following steps: Step S6, change the target position, and the control signal received by each unmanned vehicle is: update x _d , y _d . 7. The pilot-following multi-unmanned vehicle collaboration system based on the Ukka model, characterized in that it adopts the pilot-following multi-unmanned vehicle collaboration based on the Ukka model as described in any one of claims 1 to 6 System control methods. 8. A control device for a pilot-following multi-unmanned vehicle collaboration system based on the Ukka model, which applies the pilot-following multiple unmanned vehicles based on the Ukka model as claimed in any one of claims 1 to 6 The control method of a collaborative system is characterized in that the control device includes: The control input model setting module is used to design the control input model of the controller of each vehicle based on the mechanical system dynamics model; A parameter design module is used to design each parameter of the control input model according to the control input model of each vehicle. 9. A computer terminal, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that when the processor executes the program, it implements the following: The steps of the control method for the pilot-following multi-unmanned vehicle collaborative system based on the Uka model described in any one of claims 1 to 6. 10. A readable storage medium, characterized in that computer program instructions are stored in the readable storage medium. When the computer program instructions are read and run by a processor, any one of claims 1 to 6 is implemented. The steps of the control method of the pilot-following multi-unmanned vehicle collaborative system based on the Uka model.