CN116880193B - Control method of multi-unmanned vehicle cooperative system, control device, terminal and medium thereof - Google Patents
Control method of multi-unmanned vehicle cooperative system, control device, terminal and medium thereof Download PDFInfo
- Publication number
- CN116880193B CN116880193B CN202310888848.1A CN202310888848A CN116880193B CN 116880193 B CN116880193 B CN 116880193B CN 202310888848 A CN202310888848 A CN 202310888848A CN 116880193 B CN116880193 B CN 116880193B
- Authority
- CN
- China
- Prior art keywords
- unmanned vehicle
- vehicle
- constraint
- order
- ith
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 106
- 230000015572 biosynthetic process Effects 0.000 claims abstract description 57
- 230000008569 process Effects 0.000 claims abstract description 21
- 239000011159 matrix material Substances 0.000 claims description 37
- 238000013461 design Methods 0.000 claims description 19
- 238000004590 computer program Methods 0.000 claims description 14
- 230000001133 acceleration Effects 0.000 claims 1
- 238000012545 processing Methods 0.000 abstract description 5
- 238000010606 normalization Methods 0.000 abstract description 4
- 238000005755 formation reaction Methods 0.000 description 43
- 230000008859 change Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 6
- 230000002776 aggregation Effects 0.000 description 2
- 238000004220 aggregation Methods 0.000 description 2
- 230000006399 behavior Effects 0.000 description 2
- 239000003795 chemical substances by application Substances 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000001747 exhibiting effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 230000002195 synergetic effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
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
Technical Field
The invention relates to a control method in the field of multi-unmanned vehicle cooperative control, in particular to a control method of a navigation-following multi-unmanned vehicle cooperative system based on a black-bone model, a control device corresponding to the control method, the navigation-following multi-unmanned vehicle cooperative system based on the black-bone model adopting the control method, a computer terminal executing computer program instructions for realizing the control method, and a readable storage medium storing the computer program instructions for realizing the control method.
Background
The multi-unmanned vehicle system is a branch of a multi-agent system, and formation tracking control research mainly comprises a pilot-following method, a behavior-based method, a virtual structure method and the like. The pilot-following method is to set different position relations between pilot intelligent bodies (pilot vehicles) and following intelligent bodies (following vehicles), and the follower tracks the positions and directions of the pilot intelligent bodies with set parameters such as distance, speed and the like to form different formation formations. The method is characterized in that the collaboration among members is realized through sharing the state information of the pilot vehicle.
Although there are many efforts currently being made to the formation tracking control of pilot-following multi-unmanned vehicle systems, there are few studies on the establishment of a formation, tracking constraint analytical model. In addition, no normalization treatment is carried out on collision avoidance constraint and pilot track constraint of an unmanned workshop in the current research, the existing dynamics model is inaccurate, and the provided control method is too ideal.
Disclosure of Invention
In order to solve the problems of inaccurate dynamic modeling, low control precision and the like in the formation tracking control of the existing multi-unmanned vehicle system, the invention provides a control method of a piloting-following multi-unmanned vehicle cooperative system based on a black-bone model, a control device corresponding to the control method, the piloting-following multi-unmanned vehicle cooperative system based on the black-bone model adopting the control method, a computer terminal executing computer program instructions for realizing the control method, and a readable storage medium storing the computer program instructions for realizing the control method, which can meet the global collision avoidance of members in the movement process of the multi-unmanned vehicle and realize the track tracking control of the multi-unmanned vehicle system.
The invention is realized by adopting the following technical scheme: a control method of a navigation-following multi-unmanned vehicle cooperative system based on a black-card model comprises the steps of selecting any one unmanned vehicle from N unmanned vehicle cooperative systems as a navigation vehicle, belonging to a set L, and selecting the rest N-1 unmanned vehicles as following vehicles, belonging to a set F; the control method comprises the following steps:
step S1, designing a control input model of a controller of each vehicle based on a mechanical system dynamics model;
the mechanical system dynamics model is as follows:
wherein i epsilon {1,2, …, N } represents the serial number of the unmanned vehicle, M i Representing a mass matrix, q, of an ith unmanned vehicle in a collaborative system i ＝[x i ，y i ] T Represents generalized coordinates, x of the ith unmanned vehicle i Is the abscissa, y of the ith unmanned vehicle i Is the ordinate of the ith unmanned vehicle,indicating the Coriolis force, F, of the ith drone i ＝[F ix ，F iy ] T Represents the sum of the external forces to which the ith unmanned vehicle is subjected, τ i (t)＝[τ ix (t)，τ iy (t)] T A control input representing an ith drone over time t;
the control input model is:
wherein B is i ＝A i M i -1/2 ，b i Representing the second order constraint vector of the ith unmanned vehicle, A i Constraint matrix, τ, representing the ith drone i2 (t) a feedback control section for controlling the input;
step S2, controlling a pilot vehicle to be mainly responsible for track tracking tasks according to a control input model of each vehicle and designing each parameter of the control input model, wherein a following vehicle is responsible for keeping a desired formation with the pilot vehicle, so that overall track tracking is realized;
wherein: the design method of each parameter of the control input model is that the following conditions are satisfied:
(1)、τ i2 (t)＝-γ i M i 1/2 B i + η i
wherein, gamma i Constant control parameter for ith unmanned vehicle, gamma i ＞0，η i Following the error for the constraint of the ith drone,c i a first order constraint vector representing an ith drone;
(2)、
in the method, in the process of the invention,
track tracking constraint matrix of ith unmanned vehicleFormation constraint matrix of ith unmanned vehicle
First-order track tracking constraint vector of ith unmanned vehicle
First order formation constraint vector for ith unmanned vehicle
Second-order track tracking constraint vector of ith unmanned vehicle
Second order formation constraint vector of ith unmanned vehicle
For any i ε {1,2, …, N }, j ε {1,2, …, N }, i+.j, r i Represents a safe radius with the i-th unmanned vehicle mass center as the center of a circle, r j Represented by the j th unmanned vehicleThe center of mass is the safe radius of the circle center,
collision avoidance constraint matrix between ith unmanned vehicle and jth unmanned vehicleIn order to achieve this, the first and second,
first order collision avoidance constraint vector between ith unmanned vehicle and jth unmanned vehicleIn order to achieve this, the first and second,
second order collision avoidance constraint vector between ith unmanned vehicle and jth unmanned vehicleIn order to achieve this, the first and second,
after arrangement, the ith unmanned vehicle needs to meet collision avoidance constraint matrixFirst order collision avoidance constraint vector->Second order collision avoidance constraint vector->The generalization is as follows:
when i=1, the number of the cells,
when i=2, 3, …, N-1,
when i=n, the number of the cells,
wherein, I d Representing normal constant parameters, x d Target ground abscissa, y for the ith unmanned vehicle d Is the target ground ordinate, l of the ith unmanned vehicle il Representing constant parameters, l il ＞0，Indicating the relative position of the follower and pilot vehicle, < >>Representing a collision avoidance constraint matrix between the ith unmanned vehicle and the nth unmanned vehicle, +.>Representing a first order collision avoidance constraint vector between an ith unmanned vehicle and an nth unmanned vehicle,/->Representing a second order collision avoidance constraint vector between the ith unmanned vehicle and the nth unmanned vehicle,>representing a collision avoidance constraint matrix between the ith unmanned vehicle and the 1 st unmanned vehicle,>representing a first order collision avoidance constraint vector between the ith unmanned vehicle and the 1 st unmanned vehicle,/>Represent the firstAnd i, a second-order collision avoidance constraint vector between the unmanned vehicle and the 1 st unmanned vehicle.
The invention also provides a control device of the navigation-following multi-unmanned vehicle cooperative system based on the black-bone model, which applies the control method of the navigation-following multi-unmanned vehicle cooperative system based on the Wu Ka model, and the control device comprises the following components: a control input model setting module for designing a control input model of a controller of each vehicle based on the mechanical system dynamics model; and the parameter design module is used for designing each parameter of the control input model according to the control input model of each vehicle.
The invention also provides a computer terminal which comprises a memory, a processor and a computer program stored in the memory and capable of running on the processor, wherein the processor realizes the steps of the control method of the piloting-following multi-unmanned vehicle cooperative system based on the Wu Ka model when executing the program.
The invention also provides a readable storage medium, wherein the readable storage medium stores computer program instructions, and when the computer program instructions are read and run by a processor, the steps of the pilot-following multi-unmanned vehicle cooperative system control method based on the Wu Ka model are realized.
According to the invention, through carrying out normalization processing on collision avoidance constraint, formation constraint and track tracking constraint, an analysis dynamics model of the multi-unmanned vehicle system is established by adopting the black-bone method, and track tracking control of the multi-unmanned vehicle system based on constraint following is provided, so that the analysis dynamics model of the multi-unmanned vehicle system is established, the modeling process is clear and simple, the method is also suitable for dynamics analysis modeling of any number of multi-unmanned vehicle systems, and the model precision and the control precision are higher.
Drawings
Fig. 1 is a flowchart of a control method of a navigation-following multi-unmanned vehicle cooperative system based on a black-card model of the present invention.
Fig. 2 is a schematic diagram of a pilot vehicle track following situation using the control method of fig. 1.
Fig. 3 is a schematic diagram of a pilot X, Y direction tracking error using the control method of fig. 1.
FIG. 4 is a schematic diagram of tracking of various vehicle trajectories using the control method of FIG. 1.
Fig. 5 is a schematic view of the distance between a follower and a pilot vehicle using the control method of fig. 1.
Fig. 6 is a schematic view of the distance between adjacent follower vehicles using the control method of fig. 1.
FIG. 7 is a schematic view of the distance between non-adjacent follower vehicles using the control method of FIG. 1.
FIG. 8 is a schematic diagram of a distance error between each follower and pilot using the control method of FIG. 1.
Fig. 9 is a schematic diagram of control inputs in the X direction for each vehicle employing the control method of fig. 1.
Fig. 10 is a schematic diagram of control inputs in the Y direction for each vehicle employing the control method of fig. 1.
Detailed Description
The present invention will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present invention more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
Please refer to fig. 1, which is a flowchart of a control method of a navigation-following multi-unmanned vehicle cooperative system based on a black-card model. The control method needs to select any unmanned vehicle as a pilot vehicle in the N unmanned vehicle cooperative systems, belongs to a set L, and the other N-1 unmanned vehicles as following vehicles, and belong to a set F.
The control method mainly comprises the following steps: step S1, designing a control input model of a controller of each vehicle based on a mechanical system dynamics model; and S2, controlling the pilot vehicle to be mainly responsible for track tracking tasks and the following vehicle to be responsible for maintaining a desired formation with the pilot vehicle according to the control input model of each vehicle by designing each parameter of the control input model, so that overall track tracking is realized.
The design key point of the invention is how to realize the global collision avoidance of members in the movement process of the multi-unmanned vehicle and the track tracking control of the multi-unmanned vehicle system by designing each parameter of the control input model.
In step S1, the mechanical system dynamics model is:
wherein i epsilon {1,2, …, N } represents the serial number of the unmanned vehicle, M i Representing a mass matrix, q, of an ith unmanned vehicle in a collaborative system i ＝[x i ，y i ] T Represents generalized coordinates, x of the ith unmanned vehicle i Is the abscissa, y of the ith unmanned vehicle i Is the ordinate of the ith unmanned vehicle,indicating the Coriolis force, F, of the ith drone i ＝[F ix ，F iy ] T Represents the sum of the external forces to which the ith unmanned vehicle is subjected, τ i (t)＝[τ ix (t)，τ iy (t)] T The control input of the ith drone over time t is represented.
The control input model is:
wherein B is i ＝A i M i -1/2 ，b i Representing the second order constraint vector of the ith unmanned vehicle, A i Constraint matrix, τ, representing the ith drone i2 And (t) represents a feedback control section that controls the input.
These two models belong to the classical model and are not described in detail here.
The design key point of the invention is to design each parameter of the control input model, and the design method is to meet the following two conditions.
(1)、τ i2 (t)＝-γ i M i 1/2 B i + η i
Wherein, gamma i Constant control parameter for ith unmanned vehicle, gamma i ＞0，η i Following the error for the constraint of the ith drone,c i representing a first order constraint vector for the ith drone.
(2)、
In the method, in the process of the invention,
track tracking constraint matrix of ith unmanned vehicleFormation constraint matrix of ith unmanned vehicle
First-order track tracking constraint vector of ith unmanned vehicle
First order formation constraint vector for ith unmanned vehicle
Second-order track tracking constraint vector of ith unmanned vehicle
Second order formation constraint vector of ith unmanned vehicle
For any i ε {1,2, …, N }, j ε {1,2, …, N }, i+.j, r i Represents a safe radius with the i-th unmanned vehicle mass center as the center of a circle, r j Represents the safety radius taking the mass center of the jth unmanned vehicle as the center of circle,
collision avoidance constraint matrix between ith unmanned vehicle and jth unmanned vehicleIn order to achieve this, the first and second,
first order collision avoidance constraint vector between ith unmanned vehicle and jth unmanned vehicleIn order to achieve this, the first and second,
second order collision avoidance constraint vector between ith unmanned vehicle and jth unmanned vehicleIn order to achieve this, the first and second,
after arrangement, the ith unmanned vehicle needs to meet collision avoidance constraint matrixFirst order collision avoidance constraint vector->Second order collision avoidance constraint vector->The generalization is as follows:
when i=1, the number of the cells,
when i=2, 3, …, N-1,
when i=n, the number of the cells,
wherein, I d Representing normal constant parameters, x d Target ground abscissa, y for the ith unmanned vehicle d Is the target ground ordinate, l of the ith unmanned vehicle il Representing constant parameters, l il ＞0，Indicating the relative position of the follower and pilot vehicle, < >>Representing a collision avoidance constraint matrix between the ith unmanned vehicle and the nth unmanned vehicle, +.>Representing a first order collision avoidance constraint vector between an ith unmanned vehicle and an nth unmanned vehicle,/->Representing a second order collision avoidance constraint vector between the ith unmanned vehicle and the nth unmanned vehicle,>representing a collision avoidance constraint matrix between the ith unmanned vehicle and the 1 st unmanned vehicle,>representing a first order collision avoidance constraint vector between the ith unmanned vehicle and the 1 st unmanned vehicle,/>And the second order collision avoidance constraint vector between the ith unmanned vehicle and the 1 st unmanned vehicle is represented.
Compared with the prior art, the invention adopts collision avoidance constraint, formation constraint and track tracking constraint to carry out normalization treatment, adopts the Wuka method to establish the analysis dynamics model of the multi-unmanned vehicle system, has clear and simple modeling process, is suitable for the dynamics analysis modeling of any number of multi-unmanned vehicle systems, and has higher model precision and control precision.
In this embodiment, the control method further includes the following three major steps.
(1) A method of reselecting a pilot vehicle.
The method for reselecting the pilot vehicle comprises the following steps:
step S3, during the movement process of N unmanned vehicles, when the pilot vehicle is reselected, aiming at the selected pilot vehicle i (i E L), the control signals required to be received comprise: track tracking constraint matrix of pilot vehicle iFirst-order track tracking constraint vector of pilot vehicle i>Second-order track tracking constraint vector of pilot vehicle i>Collision avoidance constraint matrix for pilot i and any unmanned vehicles j (j e {1,2, …, N }, j not equal i) left in balance>First-order collision avoidance constraint vector of pilot vehicle i and any unmanned vehicle j left behind>Second-order collision avoidance constraint vector +.>
Step S4, for the recovered following vehicle i (i epsilon F), the control signal to be received comprises: formation constraint matrix of follower iFirst order formation constraint vector of follower i>Second order formation constraint vector of follower i>Collision avoidance constraint matrix for follower vehicle i and any remaining unmanned vehicles j (j e {1,2, …, N }, j not equal i)>First-order collision avoidance constraint vector +.>Second order collision avoidance constraint vector of the following vehicle i and any remaining unmanned vehicle j>
(2) The method for exiting the unmanned vehicles in the movement process of N unmanned vehicles.
The method for exiting the unmanned vehicle in the movement process of N unmanned vehicles comprises the following steps:
if the withdrawn unmanned vehicle is a pilot vehicle, the step S3 is required to be executed firstly, the step S5 is executed secondly, and otherwise, the step S5 is directly executed;
step S5, for the unmanned vehicle i needing to exit the motion process, the control signal needing to be received includes: unmanned vehicle i and any remaining unmanned vehicles j (j ε {1,2, …, N }, j not equal to i)) Collision avoidance constraint matrix of (2)First-order collision avoidance constraint vector +_for unmanned vehicle i and any other unmanned vehicle j>Second order collision avoidance constraint vector +_for unmanned vehicle i and any remaining unmanned vehicle j>
(3) A method for changing the target position.
The method for changing the target position comprises the following steps:
step S6, changing the target position, wherein the control signal received by each unmanned vehicle is as follows: updating x d 、y d 。
In a specific implementation process, the control method of the navigation-following multi-unmanned vehicle cooperative system based on the black-bone model can be set into a software form in application, such as an APP designed to be independent or embedded software which can be called at any time, and is applied to a computer terminal (such as a remote controller of a controller multi-unmanned vehicle cooperative system). The computer terminal includes a memory, a processor, and a computer program stored on the memory and executable on the processor. The computer terminal may also be a smart phone, a tablet computer, a notebook computer, etc. capable of executing a program. The processor may be a central processing unit (Central Processing Unit, CPU), controller, microcontroller, microprocessor, or other data processing chip in some embodiments. The processor is typically used to control the overall operation of the computer device. In this embodiment, the processor is configured to execute the program code stored in the memory or process the data. The steps of the control method of the navigation-following multi-unmanned vehicle cooperative system based on the black-bone model can be realized when the processor executes the program.
In the application of the control method of the navigation-following multi-unmanned vehicle cooperative system based on the black-bone model, the control method can be designed to be capable of realizing the steps of the control method of the navigation-following multi-unmanned vehicle cooperative system based on the black-bone model when the computer program instructions are read and run by a processor, and the computer program instructions are stored in a readable storage medium, such as a U shield and various drivers stored in the U shield. When the remote controller is made into a U-shield form and is plugged onto a data interface of a traditional remote controller in an electronic plugging manner, a CPU of the remote controller can read and execute computer program instructions in the U-shield, so that the problems of inaccurate dynamic modeling, low control precision and the like in the conventional multi-unmanned vehicle system formation tracking control can be solved, and the sensitivity and accuracy of the remote controller are improved. Therefore, the invention can also very conveniently upgrade the software of the existing remote controller, thereby being beneficial to popularization and application of the invention.
Both non-embedded and embedded control devices can be summarized as corresponding control devices of the navigation-following multi-unmanned vehicle cooperative system based on the black-card model. The control device of the pilot-following multi-unmanned vehicle cooperative system based on the Wu Ka model comprises a control input model setting module for designing a control input model of a controller of each vehicle based on a mechanical system dynamics model; and the parameter design module is used for designing each parameter of the control input model according to the control input model of each vehicle. The control input model setting module and the parameter design module execute the two steps respectively.
In order to demonstrate the feasibility of the technical solution of the invention, detailed theoretical deductions and experimental simulations are performed in this example.
(1) Theoretical derivation
First, mechanical system dynamics model setup
And selecting a cooperative system consisting of N unmanned vehicles to carry out target track tracking control design.
The kinetic equation is as follows
Where i ε {1,2, …, N } represents the vehicle number, M i Representing the quality of the ith unmanned vehicle in the cooperative systemQuantity matrix, q i ＝[x i ,y i ] T Represents the generalized coordinates of the ith unmanned vehicle,indicating the Coriolis force, F, of the ith drone i ＝[F ix ,F iy ] T Represents the sum of other external forces to which the ith unmanned vehicle is subjected, τ i ＝[τ ix ,τ iy ] T Representing a control input for the ith drone.
All vehicles are divided into 1 pilot vehicle and (N-1) follower vehicles, wherein the pilot vehicle belongs to the collection L and the follower vehicles belong to the collection F. All vehicles do not collide with each other, the pilot vehicle is mainly responsible for track tracking tasks, and the following vehicle is responsible for keeping an expected formation with the pilot vehicle, so that overall track tracking is realized.
Second, track following constraint design for pilot vehicles
Assume that the target track of a given pilot vehicle is q d (t)＝[x d (t),y d (t)] T Assuming that the ith vehicle is selected as the pilot vehicle, i epsilon L, and the coordinates of the ith vehicle are q i (t)＝[x i (t),y i (t)] T 。
Then define the track following error e d (t) is e d (t)＝q i (t)-q d (t) defining a first order trajectory tracking constraint asWherein l d And > 0 is a constant, and substitution finishing can be achieved:
wherein->Constraint matrix for track tracking->Tracking a constraint vector for the first-order track;
the second-order track tracking constraint is thatSubstitution arrangement can be obtained:
wherein->The constraint vector is tracked for the second order trajectory.
In particular, the method comprises the steps of,
third, the desired formation constraint design for the aggregation formation of follower vehicles towards pilot vehicles
Assuming that the remaining N-1 vehicles are used as the following vehicles, for any j E F, the position coordinate of the jth following vehicle is q j (t)＝[x j (t),y j (t)] T I.e., j e F. The desired formation constraints when considering the aggregation formation of the remaining follower vehicles towards the pilot vehicle are:
wherein q is j (t) and q l (t) represents the positions of the following unmanned vehicle and the piloting unmanned vehicle respectively, q j (t)＝[x j (t),y j (t)] T q l (t)＝[x l (t),y l (t)] T ，Indicating the relative position of the following unmanned vehicle and the pilot vehicle,/->Andrepresenting the ideal formation distance in the x, y directions, respectively. The method can be regarded as a zero-order form of formation constraint, and the expected ideal formation can be formed by setting the relative positions of each follower vehicle and the pilot vehicle.
Using constraint form of formation error, the desired formation error can be defined by the desired formation constraint as follows:
assume that for any initial state q j (t 0 ) Are all present for a time T j < ++T for all T j All have:
wherein->Is an arbitrarily small constant. Will make formation error->At a finite time T j Inner access to any small range->Is referred to as approximate formation.
The following constraint forms are set for achieving approximate formation:
wherein l jl > 0 is a constant, the constraint comprising a formation error e jl One-order derivative of the first order derivativeAnd when t → infinity, a formation error e is expected jl (t) will converge to0。
Opposite typeDeriving to obtain a second-order constraint form:
first and second derivatives of the expected formation error are obtained:
the first-order constraint form and the second-order constraint form are obtained by combining the above-mentioned steps as follows:
the first and second forms are written as follows:
wherein,
fourth, the collision avoidance constraint design is followed between any two vehicles.
In order to ensure that no collision of any vehicles occurs, collision avoidance constraint needs to be followed between any two vehicles. The specific collision avoidance design is as follows:
let the distance between any two vehicles be delta S ij (t)，
ΔS ij (t)＝q i (t)-q j (t)-(r i +r j )
Wherein q i And q j Representing the coordinates of the centroids of any two vehicles, where i, j ε {1,2, …, N }, i noteq, j, r i Representing a safe radius centered on the i-th vehicle centroid. When DeltaS ij (t) < 0, collision may occur between vehicles. Further defined for computational convenience is:
S ij (t)＝||q i (t)-q j (t)|| 2 -(r i +r j ) 2
will S ij The following factorization was performed:
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 is ij When (t) < 0, the actual distance DeltaS between any two vehicles ij (t) < 0; when S is ij When (t) > 0, the actual distance DeltaS between any two vehicles ij (t) > 0, S ij The sign of (t) is used as a basis for judging whether a collision occurs.
Arbitrary safe initial state S ij (t 0 ) > 0, if S ij > 0 for any t.gtoreq.t 0 The multi-agent system is said to have global collision avoidance. Is true toThe global collision avoidance of the existing multi-unmanned vehicle cooperative system is constructed as follows:
the collision avoidance function is related to S ij A logarithmic function of (t), the function having implicit constraints due to the domain constraints of the logarithmic function: ΔS ij (t) > 0. First and second derivatives are calculated for the function:
thereby constructing a first order collision avoidance constraint as
Namely:
the second order collision avoidance constraint is:
namely:
for any two vehicles i, j e {1,2,..N }, i not equal j in the cooperative system, the first-order collision avoidance constraint and the second-order collision avoidance constraint of the cooperative system can be written as follows:
wherein,
for a synergistic system consisting of N unmanned vehicles, N (N-1)/2 collision avoidance constraints are applied altogether. As the action object of the collision avoidance constraint is two corresponding unmanned vehicles, for the same collision avoidance constraint, any one of the two vehicles is selected as the object to which the collision avoidance constraint is applied. In order to prevent the situation of excessive restraint applied to two vehicles simultaneously by collision avoidance restraint, a total of N (N-1)/2 collision avoidance restraints are distributed and then are summarized as follows:
when i=1, the number of the cells,
when i=2, 3, …, N-1,
when i=n, the number of the cells,
fifth, unmanned vehicle cooperative system track tracking controller design
All constraint designs of the piloting unmanned vehicle and the following unmanned vehicle are completed, all constraint allocation is now generalized, and for any vehicle i in the cooperative system, the constraint is:
wherein,
based on the Udwadia-Kalaba equation, the ideal constraint force of the unmanned vehicle i constrained by the above is:
wherein B is i ＝A i M i -1/2 The method comprises the steps of carrying out a first treatment on the surface of the The above equation can be used as a control input for each drone in an ideal state (i.e., no uncertainty and no initial error). Ideal condition control of the ith vehicle
Considering that the unmanned vehicles are scattered and have no formation relation when the cooperative system is at the initial moment, the following control inputs are added to inhibit initial errors:
τ i2 ＝-γ i M i 1/2 B i + η i
wherein eta i In order to constrain the following error,
γ i > 0 is a constant control parameter, gamma i The larger, η i The faster the convergence speed.
The track tracking controller of the multi-unmanned vehicle cooperative system is designed as follows:
τ i (t)＝τ i1 +τ i2
(2) Test simulation: the effectiveness of the control method is verified through experiments.
Fig. 2 reflects the tracking situation of the target track of the pilot vehicle, and it can be seen that the actual track of the pilot vehicle substantially coincides with the target track, and only slight deviation exists at the initial time, and the deviation is about 0.1. FIG. 3 reflects the track tracking error of the pilot vehicle in the X, Y axis direction, with the error varying significantly over the 0-5 second and 20-30 second time periods, due to the effects of formation adjustment in 0-5 seconds, and the effects of the pilot vehicle in the middle of the track, with the track curve varying significantly over 20-30 seconds, respectively. During the whole tracking process, the tracking error in the X direction is smaller than 0.1m, and the error in the Y direction is only smaller than 0.04m. By combining the figures 4 and 5, the track tracking condition of the pilot vehicle in the cooperative system is good, and an important foundation is laid for other following vehicles to realize good tracking conditions.
Fig. 6-7 respectively show the change of the distance between vehicles in the track tracking process of the multi-unmanned vehicle cooperative system. It can be seen that each car distance tended to stabilize after 10 seconds. The vehicle distances shown in fig. 5 and 6 are each stable around 6m, which correspond to the designed regular hexagonal formation side length and the apex-to-center distance, respectively. The distance of each vehicle slightly fluctuates in 20-30 seconds because the middle change position of the track curve is larger, but the fluctuation amplitude is smaller, and the fluctuation peak value is only about 0.1 m. Fig. 7 shows that the distances between non-adjacent vehicles in the formation are respectively stabilized at about 10.4m and 12m, corresponding to different diagonal distances of the regular hexagon. In addition, it can be seen that the distances between vehicles are greater than the safe distance in the whole process, which means that no vehicle collision occurs in the whole process. The results shown in fig. 5-7 collectively demonstrate that the designed controller can better meet the desired formation constraints.
Fig. 8 shows the change of the distance error between each follower and the pilot, and it can be seen that the distance error approaches 0 after the 10 th second. The vehicle distance error fluctuates slightly around 25 seconds because each vehicle is at the middle of the track curve at 25 seconds, the speed change is large, but the error remains around 0.1 m.
Fig. 9 and 10 show control inputs in the direction of each drone X, Y, respectively. It can be seen that the control inputs in the direction X, Y are large at the beginning and the maximum amplitude is around 400N, because the initial team formation errors are large and high control inputs are required to achieve the team behaviour. After 5 seconds the control input becomes significantly smaller and the rate of change starts to flatten, after which the control input in the X direction remains stable, while the control input in the Y direction remains substantially stable between 8-20 seconds, after 20 seconds a significant change occurs, exhibiting a waveform change similar to a sine function, because a speed change occurs in the middle of each track curve, after which the control input also returns to the previous state. In addition, the steady values of the control inputs are different for each vehicle in the direction X, Y due to the different mass, external forces, and other parameters of each unmanned vehicle.
The foregoing description of the preferred embodiments of the invention is not intended to be limiting, but rather is intended to cover all modifications, equivalents, and alternatives falling within the spirit and principles of the invention.
Claims (10)
1. A control method of a navigation-following multi-unmanned vehicle cooperative system based on a black-card model comprises the steps of selecting any one unmanned vehicle from N unmanned vehicle cooperative systems as a navigation vehicle, belonging to a set L, and selecting the rest N-1 unmanned vehicles as following vehicles, belonging to a set F; the control method comprises the following steps:
step S1, designing a control input model of a controller of each vehicle based on a mechanical system dynamics model;
the mechanical system dynamics model is as follows:
wherein i epsilon {1,2, …, N } represents the serial number of the unmanned vehicle, M i Representing a mass matrix, q, of an ith unmanned vehicle in a collaborative system i ＝[x i ，y i ] T Representing the ith unmanned aerial vehicleGeneralized coordinates, x of vehicle i Is the abscissa, y of the ith unmanned vehicle i Is the ordinate of the ith unmanned vehicle,indicating the Coriolis force, F, of the ith drone i ＝[F ix ，F iy ] T Represents the sum of the external forces to which the ith unmanned vehicle is subjected, τ i (t)＝[τ ix (t)，τ iy (t)] T A control input representing an ith drone over time t;
the control input model is:
wherein B is i ＝A i M i -1/2 ，b i Representing the second order constraint vector of the ith unmanned vehicle, A i Constraint matrix, τ, representing the ith drone i2 (t) a feedback control section for controlling the input;
step S2, controlling a pilot vehicle to be mainly responsible for track tracking tasks according to a control input model of each vehicle and designing each parameter of the control input model, wherein a following vehicle is responsible for keeping a desired formation with the pilot vehicle, so that overall track tracking is realized;
the method is characterized in that: the design method of each parameter of the control input model is that the following conditions are satisfied:
(1)τ i2 (t)＝-γ i M i 1/2 B i + η i
wherein, gamma i Constant control parameter for ith unmanned vehicle, gamma i ＞0，η i Following the error for the constraint of the ith drone,c i a first order constraint vector representing an ith drone;
(2)
in the method, in the process of the invention,
track tracking constraint matrix of ith unmanned vehicleFormation constraint matrix of ith unmanned vehicle
First-order track tracking constraint vector of ith unmanned vehicle
First order formation constraint vector for ith unmanned vehicle
Second-order track tracking constraint vector of ith unmanned vehicle
Second order formation constraint vector of ith unmanned vehicle
For any i ε {1,2, …, N }, j ε {1,2, …, N }, i+.j, r i Represents a safe radius with the i-th unmanned vehicle mass center as the center of a circle, r j Represents the safety radius taking the mass center of the jth unmanned vehicle as the center of circle,
collision avoidance constraint matrix between ith unmanned vehicle and jth unmanned vehicleIn order to achieve this, the first and second,
first order collision avoidance constraint vector between ith unmanned vehicle and jth unmanned vehicleIn order to achieve this, the first and second,
second order collision avoidance constraint vector between ith unmanned vehicle and jth unmanned vehicleIn order to achieve this, the first and second,
collision avoidance constraint matrix required to be met by ith unmanned vehicleFirst order collision avoidance constraint vector->Second order collision avoidance constraint vectorThe generalization is as follows:
when i=1, the number of the cells,
when i=2, 3, …, N-1,
when i=n, the number of the cells,
wherein, I d Representing normal constant parameters, x d Target ground abscissa, y for the ith unmanned vehicle d Is the target ground ordinate, l of the ith unmanned vehicle il Representing constant parameters, l il ＞0，Indicating the relative position of the follower and pilot vehicle, < >>Representing a collision avoidance constraint matrix between the ith unmanned vehicle and the nth unmanned vehicle, +.>Representing a first order collision avoidance constraint vector between an ith unmanned vehicle and an nth unmanned vehicle,/->Representing a second order collision avoidance constraint vector between the ith unmanned vehicle and the nth unmanned vehicle,>representing a collision avoidance constraint matrix between the ith unmanned vehicle and the 1 st unmanned vehicle,>representing a first order collision avoidance constraint vector between the ith unmanned vehicle and the 1 st unmanned vehicle,/>Representing a second order collision avoidance constraint direction between an ith unmanned vehicle and a 1 st unmanned vehicleAmount of the components.
2. The method for controlling a navigation-following multi-unmanned vehicle cooperative system based on the black-card model according to claim 1,the design method of (2) is as follows: for a cooperative system consisting of N unmanned vehicles, N (N-1)/2 collision avoidance constraints are applied, as the collision avoidance constraint is applied to two corresponding unmanned vehicles, for the same collision avoidance constraint, any one of the two unmanned vehicles is selected as the collision avoidance constraint application object, and for preventing the situation of excessive constraint of the collision avoidance constraint applied to the two vehicles at the same time, the total N (N-1)/2 collision avoidance constraints are distributed and then summarized.
3. The control method of a combined navigation-following multi-unmanned vehicle cooperative system based on the black-card model according to claim 2, wherein for any j e F, the formation constraint matrixFirst order formation constraint vector->Second order formation constraint vector->The design method of the three components comprises the following steps:
to achieve the target formation, i.e. the desired formation, the following constraint forms are set:
in the method, in the process of the invention,is a formation error, and when t → infinity, a formation error e is expected jl (t) will converge to 0;
opposite typeDeriving to obtain a second-order constraint form:
first and second derivatives of the expected formation error are obtained:
wherein q is l ，Respectively representing the position, the speed and the acceleration of the pilot vehicle;
the first-order constraint form and the second-order constraint form are obtained by combining the above-mentioned steps as follows:
the first and second forms are written as follows:
the method can obtain:
4. the control method of a navigation-following multi-unmanned vehicle cooperative system based on a black-card model according to claim 1, wherein the control method further comprises the steps of:
step S3, during the movement process of N unmanned vehicles, when the pilot vehicle is reselected, aiming at the selected pilot vehicle i (i E L), the control signals required to be received comprise: track tracking constraint matrix of pilot vehicle iFirst-order track tracking constraint vector of pilot vehicle i>Second-order track tracking constraint vector of pilot vehicle i>Collision avoidance constraint matrix for pilot i and any unmanned vehicles j (j e {1,2, …, N }, j not equal i) left in balance>First-order collision avoidance constraint vector of pilot vehicle i and any unmanned vehicle j left behind>Second-order collision avoidance constraint vector +.>
Step S4, for the recovered following vehicle i (i epsilon F), the control signal to be received comprises: formation constraint matrix of follower iFirst order formation constraint vector of follower i>Second order formation constraint vector of follower i>Collision avoidance constraint matrix for follower vehicle i and any remaining unmanned vehicles j (j e {1,2, …, N }, j not equal i)>First-order collision avoidance constraint vector +.>Second order collision avoidance constraint vector of the following vehicle i and any remaining unmanned vehicle j>
5. The control method of a navigation-following multi-unmanned vehicle cooperative system based on a black-card model according to claim 4, wherein the control method further comprises a method for exiting the unmanned vehicle during the movement of N unmanned vehicles, the method for exiting comprising the steps of:
if the withdrawn unmanned vehicle is a pilot vehicle, the step S3 is required to be executed firstly, the step S5 is executed secondly, and otherwise, the step S5 is directly executed;
step S5, for the unmanned vehicle i needing to exit the motion process, the control signal needing to be received includes: unmanned aerial vehicleCollision avoidance constraint matrix for vehicle i and any remaining unmanned vehicles j (j e {1,2, …, N }, j not equal i)First-order collision avoidance constraint vector +_for unmanned vehicle i and any other unmanned vehicle j>Second order collision avoidance constraint vector +_for unmanned vehicle i and any remaining unmanned vehicle j>
6. The control method of a navigation-following multi-unmanned vehicle cooperative system based on a black-card model according to claim 4, wherein the control method further comprises the steps of:
step S6, changing the target position, wherein the control signal received by each unmanned vehicle is as follows: updating x d 、y d 。
7. The navigation-following multi-unmanned vehicle cooperative system based on the black-bone model is characterized in that the control method of the navigation-following multi-unmanned vehicle cooperative system based on the black-bone model is adopted according to any one of claims 1 to 6.
8. A control device of a navigation-following multi-unmanned vehicle cooperative system based on a black-card model, to which the control method of the navigation-following multi-unmanned vehicle cooperative system based on a black-card model according to any one of claims 1 to 6 is applied, characterized in that the control device comprises:
a control input model setting module for designing a control input model of a controller of each vehicle based on the mechanical system dynamics model;
and the parameter design module is used for designing each parameter of the control input model according to the control input model of each vehicle.
9. A computer terminal comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor, when executing the program, realizes the steps of the control method of the combined navigation-following multi-unmanned vehicle system based on the black-card model as claimed in any one of claims 1 to 6.
10. A readable storage medium, wherein computer program instructions are stored in the readable storage medium, and when the computer program instructions are read and executed by a processor, the steps of the control method of the combined navigation-following multi-unmanned vehicle system based on the black-card model according to any one of claims 1 to 6 are realized.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310888848.1A CN116880193B (en) | 2023-07-19 | 2023-07-19 | Control method of multi-unmanned vehicle cooperative system, control device, terminal and medium thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310888848.1A CN116880193B (en) | 2023-07-19 | 2023-07-19 | Control method of multi-unmanned vehicle cooperative system, control device, terminal and medium thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116880193A CN116880193A (en) | 2023-10-13 |
| CN116880193B true CN116880193B (en) | 2024-02-23 |
Family
ID=88256489
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310888848.1A Active CN116880193B (en) | 2023-07-19 | 2023-07-19 | Control method of multi-unmanned vehicle cooperative system, control device, terminal and medium thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116880193B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118226761B (en) * | 2024-05-24 | 2024-08-13 | 合肥工业大学 | Control of hybrid vehicle trains and modeling method and apparatus thereof |
| CN118244646B (en) * | 2024-05-27 | 2024-08-23 | 合肥工业大学 | Formation control of unmanned vehicle team and modeling method and device thereof |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108459605A (en) * | 2018-03-22 | 2018-08-28 | 合肥工业大学 | Trajectory Tracking Control method based on AGV system |
| CN114594756A (en) * | 2020-11-30 | 2022-06-07 | 上海交通大学 | Multi-vehicle cooperative formation control method, terminal and medium in dynamic obstacle environment |
| CN115268464A (en) * | 2022-08-25 | 2022-11-01 | 山东科技大学 | Automatic driving automobile track tracking control method and device based on hierarchical constraint |
| CN115328144A (en) * | 2022-08-29 | 2022-11-11 | 江西科骏实业有限公司 | Trajectory tracking control method, system and medium for multi-unmanned vehicle cooperative carrying system |
| CN116009536A (en) * | 2022-12-09 | 2023-04-25 | 清华大学 | Hierarchical modeling and constraint following control method for multi-vehicle cooperative transportation system |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018189324A1 (en) * | 2017-04-12 | 2018-10-18 | Norwegian University Of Science And Technology (Ntnu) | Recovery system for uav |
-
2023
- 2023-07-19 CN CN202310888848.1A patent/CN116880193B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108459605A (en) * | 2018-03-22 | 2018-08-28 | 合肥工业大学 | Trajectory Tracking Control method based on AGV system |
| CN114594756A (en) * | 2020-11-30 | 2022-06-07 | 上海交通大学 | Multi-vehicle cooperative formation control method, terminal and medium in dynamic obstacle environment |
| CN115268464A (en) * | 2022-08-25 | 2022-11-01 | 山东科技大学 | Automatic driving automobile track tracking control method and device based on hierarchical constraint |
| CN115328144A (en) * | 2022-08-29 | 2022-11-11 | 江西科骏实业有限公司 | Trajectory tracking control method, system and medium for multi-unmanned vehicle cooperative carrying system |
| CN116009536A (en) * | 2022-12-09 | 2023-04-25 | 清华大学 | Hierarchical modeling and constraint following control method for multi-vehicle cooperative transportation system |
Non-Patent Citations (2)
| Title |
|---|
| Robust Approximate Constraint-following Control for Autonomous Vehicle Platoon Systems;Zhao Xiaomin, et al.;ASIAN Journal of Control;第4卷(第20期);第1611-1623页 * |
| 三轮全向移动平台的轨迹跟踪控制与多参数优化研究;金栋;中国优秀硕士学位论文全文数据库 工程科技II辑(第6期);第C029-60页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116880193A (en) | 2023-10-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116880193B (en) | Control method of multi-unmanned vehicle cooperative system, control device, terminal and medium thereof | |
| Chai et al. | Design and implementation of deep neural network-based control for automatic parking maneuver process | |
| CN109808707B (en) | Automobile steering control method and controller based on stochastic model prediction | |
| CN107015477A (en) | Vehicle route tracking H ∞ control methods based on feedback of status | |
| CN116449820A (en) | Unmanned tracked vehicle track tracking control method based on constraint following | |
| CN112180925B (en) | AGV track following method and device and processing equipment | |
| CN114348026B (en) | Vehicle control method, device, equipment and storage medium | |
| CN110687797B (en) | Self-adaptive MPC parking transverse control method based on position and posture | |
| CN114771570A (en) | Control method and device for automatic driving vehicle | |
| CN113759707A (en) | Predictive control optimization method and device for autonomous vehicle | |
| Zhang et al. | Stochastic cooperative game-based assistant fault-tolerant control considering driver behavior uncertainty | |
| Wang et al. | Path tracking control of an autonomous vehicle with model-free adaptive dynamic programming and RBF neural network disturbance compensation | |
| CN117922221A (en) | Vehicle body posture prediction method, device, equipment and medium | |
| CN113848899A (en) | Vehicle transverse control method, device, equipment and storage medium | |
| CN111301411B (en) | Vehicle travel control method and device | |
| WO2024046789A1 (en) | Trajectory planning method and apparatus, device and storage medium | |
| CN112034869A (en) | Design method and application of variable parameter neurodynamics controller of unmanned aerial vehicle | |
| CN116300419A (en) | Distributed formation control method based on group function | |
| CN112685835B (en) | Elastic event trigger control method and system for autonomous driving of vehicle | |
| US11834066B2 (en) | Vehicle control using neural network controller in combination with model-based controller | |
| CN116279410A (en) | Heading error compensation method, system, device and storage medium | |
| Wang et al. | Application of stochastic model predictive control to modeling driver steering skills | |
| CN114620074A (en) | Vehicle control method, device, electronic device and storage medium | |
| CN113778082A (en) | Unmanned vehicle track tracking control method and system based on self-triggering mechanism | |
| CN118210314B (en) | Automatic driving vehicle track planning and dynamic obstacle avoidance method, equipment and medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |