Heterogeneous UAV / USV collision avoidance formation control method with input quantization
Through unified model and quantitative technology, the problems of model differences and insufficient communication in the UAV and UAV fleets are solved, effective collision avoidance and coordinated control are achieved, and the trajectory tracking and stability of the formation are ensured.
Patent Information
- Application Number
- CN202411121724.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-15
- Publication Date
- 2024-11-12
AI Technical Summary
Due to the differences in model, external interference and insufficient communication bandwidth, drone and unmanned boat fleets are difficult to achieve effective collision avoidance and coordinated control, especially in maritime practice, there are collision risks and information synchronization difficulties.
The unified model is used to construct heterogeneous agents, define the unmanned ship model through the manual method, design kinematics and dynamic subsystems in a layered manner, introduce artificial potential fields and expansion state observers, and use quantitative technology to reduce the burden of signal transmission, and design Adaptive controller to ensure formation trajectory tracking and stability.
It realizes effective collision avoidance and coordinated control between drones and unmanned boats, reduces collision risks, ensures the synchronization and stability of formation information, and is suitable for the control rules of multi-level servo systems.
Smart Images

Figure CN118938927A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of artificial intelligence technology, and in particular to a heterogeneous UAV / USV collision avoidance formation control method with input quantization. Background Art
[0002] With the progress and development of scientific and technological capabilities, in future development plans, unmanned system equipment will be our goal of vigorous development to ensure that the industrialization of unmanned systems can be rapidly developed. In the national "new generation of artificial intelligence" development plan released in 2017, unmanned systems have been identified as one of the four major research directions in the future. The role of unmanned system formations in engineering practice is becoming increasingly prominent, especially the research on unmanned aerial vehicle (UAV) system and unmanned surface vehicle (USV) system formations has become an important topic.
[0003] However, the formation of drones and unmanned boats belongs to the heterogeneous multi-agent formation problem and will face many challenges, as follows:
[0004] 1. There are great differences between the UAV model and the unmanned boat model in terms of system structure, internal parameters, kinematic and dynamic state dimensions, etc.
[0005] 2. The influence of external interference caused by unknown information and environmental changes will pose challenges to the control task, and due to insufficient channel bandwidth and low information transmission efficiency during the communication process, formation information is difficult to synchronize.
[0006] 3. During the formation process of UAVs, they may encounter various obstacles, causing the risk of collision, making the formation mission difficult.
[0007] How to solve the above problems has always been the focus and hotspot of research. In view of the existing related research, the bandwidth limitation problem in maritime practice, the impact of external environmental factors and the evaluation of unknown quantities, and how to carry out reasonable collision avoidance and obstacle avoidance actions considering the actual situation of navigation practice need to be solved urgently. Considering the collision avoidance formation control of heterogeneous UAV / USV with input quantization is more meaningful for navigation practice. Summary of the invention
[0008] According to the above-mentioned relevant problems in navigation practice, the present invention provides a heterogeneous UAV / USV collision avoidance formation control method with input quantization for the problem of heterogeneous formation control of unmanned ships and unmanned aerial vehicles with input quantization. For heterogeneous intelligent agents, the idea of unified model is adopted, and the hand-point method is used to construct a unified intelligent agent model to solve the problem of inconsistency of unmanned ship and unmanned aerial vehicle models; referring to the missile design idea, the controller is hierarchically designed into a kinematic subsystem and a dynamic subsystem, and the tracking error is considered in the kinematic subsystem, and an artificial potential field is introduced to design the kinematic guidance law. In the dynamic subsystem, quantization technology is used to reduce the signal transmission burden and reduce the execution frequency of the actuator, which is more in line with the control law of multi-level servo systems in practice. The design is based on an extended state observer to observe the unknown and uncertain terms of the kinematic guidance law and the adaptive dynamics controller to ensure that the unmanned ship and the unmanned aerial vehicle can track the desired trajectory, can move forward along the desired trajectory and maintain a fixed formation with each other.
[0009] The technical means adopted by the present invention are as follows:
[0010] A heterogeneous UAV / USV collision avoidance formation control method with input quantization, characterized in that it comprises the following steps:
[0011] S1. To address the inconsistency of mathematical models between UAV and USV, consider using mathematical methods to reorganize and design a consistent mathematical model of heterogeneous agents;
[0012] S2. Based on the target trajectory, a distributed heterogeneous multi-UAV unmanned ship time-varying formation error is constructed, an extended state observer is used to observe the information of neighboring agents contained in the controller, and an improved artificial potential field is introduced to design the kinematic guidance law;
[0013] S3. Take the guidance signal as the input of the dynamics subsystem, design the dynamics underlying controller and adaptive law that considers input quantization, and use a uniform quantizer for the quantization process. Design the altitude controller for the altitude tracking process in the vertical plane to ensure that the formation trajectory tracking goal is achieved;
[0014] S4. Design a Lyapunov function and prove the stability of the closed-loop system based on the input-to-state stability theory.
[0015] Furthermore, step S1 specifically includes:
[0016] S11. Construct the mathematical model of UAV control as follows:
[0017]
[0018]
[0019] In the formula, p ax,p ay ,p az represents the spatial position information of the UAV, φ, θ, ψ represent the angle information of the UAV; u p represents the control thrust of the UAV, τ φ ,τ θ ,τ ψ Represents the three control torques of the UAV, m a represents the mass of the UAV, g represents the acceleration due to gravity, express
[0020] Overall residual rotor angle, d x ,d y ,d z , d θ ,d ψ is the translational drag coefficient, J ax ,J ay ,J az represents the moment of inertia, J ar represents the rotor's moment of inertia, Δ ax ,Δ ay ,Δ az Indicates the external interference encountered by the UAV;
[0021] The UAV control model in the horizontal plane is as follows:
[0022]
[0023] p a =[p ax ,p ay ] T , g a =diag{1 / m a ,1 / m a}, Δ a =[Δ ax ,Δ ay ] T ,u a =[u ax ,u ay ] T , and there are
[0024] S12. Construct a mathematical model of the unmanned ship as follows:
[0025]
[0026]
[0027]
[0028] In the formula, x si ,y si represents the position information of the i-th USV; ψ si represents the yaw angle of the i-th USV; α i =[μ si ,v si ,r si ] T Represents surge, sway and yaw speed, m μsi ,m vsi ,m rsi They represent inertial mass respectively; f μsi (α i ),f vsi (α i ),f rsi (α i ) represent nonlinear unknown functions, including unmodeled fluid mechanics and Coriolis force; and Respectively represent surge force and yaw moment; w μsi ,w vsi ,w rsi They represent the bounded disturbances caused by waves respectively;
[0029] S13. Define the ship model using the hand position method. The process is as follows:
[0030] (p six ,p siy ) as the hand point, defined as:
[0031]
[0032] Where, L si Indicates the actual position (x si ,y si ) and the newly defined hand point (p six ,p siy ) between the two sides;
[0033] Taking the second derivative of the defined hand point and substituting the USV mathematical model into it, we get:
[0034]
[0035] in: β=[μ si ,v si ,r si ,ψ si ]; then the mathematical model of the USV in the horizontal plane is written as:
[0036]
[0037] In the formula, p si =[p six ,p siy ] T , f sixy =[f six ,f siy ] T ,u si =[τ μ ,τ r ] T , w dixy =[w dix ,w diy ] T ,Ω si (ψ si )=[cosψ si ,-sinψ si ;sinψ si ,cosψ si ],ω si =diag{1 / m μsi ,L si / m rsi};
[0038] In order to ensure the unification of the USV model and the UAV model, the above formula is expanded and further converted into:
[0039]
[0040] S14. Build a unified model of USV and UAV. The construction process is as follows:
[0041] Combining step S12 and step S13, a unified mathematical model of heterogeneous agents in the horizontal plane is obtained as follows:
[0042]
[0043] When the model represents a USV model, represents the position and velocity of the USV, where u xi =[τ μi ,τ ri ] T ,
[0044] Δ xi =[w dix ,w diy ] T ;
[0045] When the model represents a UAV model, represents the position and velocity of the UAV, where u xi =[u ax ,u ay ] T , Δ xi =[Δ ax ,Δ ay ] T ;
[0046] S15. Construct the Z-axis model of the UAV as follows:
[0047]
[0048] S16, using the mean quantizer, the control input u xi Convert to Q(u xi ), the process is as follows:
[0049] Considering the quantized control input, the unified model of UAV and USV is:
[0050]
[0051] The model is further transformed to obtain the final heterogeneous agent dynamics model in the XY plane:
[0052]
[0053] Among them, F i =[F iU ,F iV ] T , Q(τ i )=[Q(τ iU ),Q(τ iV )] T , G i =diag[G iU ,G iV ], Δ i =[Δ iU ,Δ iV ] T , τ i =[τ iU ,τ iV ] T =[u xi1 ,u xi2 ] T ;
[0054] The quantization process is described as:
[0055]
[0056]
[0057] Among them, k iU , k iV For quantitative indicators;
[0058] S17. Assuming that the following-agents in the formation have the same potential energy, the potential function of the collision avoidance repulsion field between the following unmanned ships in the formation and the obstacle avoidance potential function between each unmanned ship and the obstacle are expressed as:
[0059]
[0060] Among them, i,j=1...n,i≠j means there are n agents, b=1,...,m means there are m obstacles, and the Euclidean distance between any two agents in the formation is given by The Euclidean distance between the agent and the obstacle is represented by Indicates that, [x i ,y i ] T and [x j ,y j ] T is the position coordinates of the ith agent and the jth agent, [x k ,y k ] T is the position coordinate of the kth obstacle, R >0, are the upper and lower bounds of the collision avoidance zone;
[0061] By finding the negative gradient of the collision avoidance repulsive force field potential function, we can obtain the corresponding repulsive force function as follows:
[0062]
[0063]
[0064] S18. Consider a time-varying reference trajectory p0(t) = [x0(t), y0(t)] T , where the trajectory parameter is a continuous differentiable vector. By introducing graph theory, Represents the communication topology diagram of multiple unmanned systems, where V = {n0,n1,...,n N} is the set consisting of all nodes, representing the N ships in the fleet; E = {(i, j)∈V×V} is the set consisting of all edges, representing the information transmission between adjacent ships, (n i ,n j )∈E represents the information exchange between nodes, otherwise it is not true. is the adjacency matrix of the formation system, when (n i ,n j )∈Eij =1, otherwise a ij =0.
[0065] Further, step S2 specifically includes:
[0066] S21. Define the time-varying formation error of distributed heterogeneous agents as:
[0067]
[0068] in, is the distance of the ith unmanned vehicle relative to the parameterized trajectory, is the relative position deviation between the unmanned vehicles;
[0069] S22. Derivative and simplify the defined distributed heterogeneous agent time-varying formation error to obtain:
[0070]
[0071] in,
[0072] S23, use ESO extended state observer to observe the kinematic unknowns, and design the extended state observer as follows:
[0073]
[0074] in, The observation error is: Further converted to: in, in represents the potential function of the defined artificial potential field;
[0075] S24. According to steps S21, S22 and S23, a kinematic guidance law for the heterogeneous agent system is designed as follows:
[0076]
[0077] Among them, K i =diag{k1,k2}, k1, k2 are positive constants, and ε i is a positive constant;
[0078] S25. Based on the designed kinematic guidance law of the heterogeneous intelligent system, the kinematic trajectory tracking error considering the artificial potential field is designed as follows:
[0079]
[0080] Further, step S3 specifically includes:
[0081] S31, a linear model is used to describe the quantization process, and the linear quantization model is described as Q(τ iU )=q 1iU (t)τ iU +q 2iU (t),
[0082] Q(τ iV )=q 1iV (t)τ iV +q 2iV (t) and there is:
[0083]
[0084] Among them, q 1iU (t), q 1iV (t) is unknown, because the positive and negative signals of are not affected by the quantization process, and we get q 1iU (t)>0,q 1iV (t)>0; if |τ iU (t)|<a,|τ iV (t)|<a, then Q(τ iU (t))、Q(τ iV (t)) is bounded and q 2iU (t), q 2iV (t) is bounded,
[0085] S32. Design the sliding surface as follows:
[0086]
[0087] S33. Derivative the designed sliding surface to obtain:
[0088]
[0089]
[0090] S34. Definition d iU (X) = Δ iU +F iU , d iV (X) = Δ iV +F iV , the derivative in step S34 is considered as:
[0091]
[0092] S35, using ESO extended state observer to calculate the unknown quantity d in the dynamic model iU (X), d iV (X) Conduct observation and design the observer function model as follows:
[0093]
[0094] in, The observed error is:
[0095]
[0096] The observer function is further transformed into: in
[0097] S36. Definition From steps S35 and S36, we get:
[0098]
[0099] in, Similarly, there are:
[0100]
[0101]
[0102] S37, due to q 1iU and q 1iV is time-varying and unknown, so adaptive estimation is used, using q 1iU and q 1iV The lower bound of is estimated, and a time-varying gain is defined in, Q 1iU ,q 1iV The lower bound of , the dynamic controller and adaptive law are designed as follows:
[0103]
[0104]
[0105] Therefore, the dynamic control error is obtained as:
[0106]
[0107] S38. Define the UAV tracking error in the Z-axis direction as follows:
[0108]
[0109] Among them, p azd is the expected position of UAV, v azd is the expected speed of the UAV;
[0110] S39. Design the tracking controller as follows:
[0111]
[0112] in,
[0113] Further, step S4 specifically includes:
[0114] S41. Perform stability analysis on the extended state observer designed for the kinematic subsystem;
[0115] S42. Perform stability analysis on the extended state observer designed for the dynamics subsystem;
[0116] S43, performing stability analysis on the Z-axis tracking controller of the UAV;
[0117] S44. Perform stability analysis on the dynamic closed-loop system;
[0118] S45. Perform stability analysis on the closed-loop system.
[0119] Further, step S41 specifically includes:
[0120] S411. Define the Lyapunov function as follows:
[0121]
[0122] Among them, P i is a positive definite matrix that satisfies
[0123] S412. Deriving the defined Lyapunov function, we obtain:
[0124]
[0125] S413, due to Make So the system is stable and has:
[0126]
[0127] in,
[0128] Further, step S42 specifically includes:
[0129] S421. Define the Lyapunov function as follows:
[0130]
[0131] Among them, Q i is a positive definite matrix that satisfies
[0132] S422. Derivative the defined Lyapunov function to obtain:
[0133]
[0134] S423, due to Make So the system is stable and has:
[0135]
[0136] in,
[0137] Further, step S43 specifically includes:
[0138] S431, deriving the UAV tracking error in the Z-axis direction defined in step S38, and obtaining:
[0139]
[0140] S432, e azv As a virtual control input, design a virtual signal ζ z =-k ζ e azp To ensure the stability of the tracker;
[0141] S433. Define the Lyapunov function as follows:
[0142]
[0143] S434. Deriving the defined Lyapunov function, we obtain:
[0144]
[0145] S435. Define a new error e ζ =e azv -ζ z , combining the new error with the UAV tracking error in the Z-axis direction defined in step S38 and taking the derivative, we get:
[0146]
[0147] S436. Define a new Lyapunov function And derive it to get:
[0148]
[0149] S437: For the unknown quantity F in step S436az , using the RBFNN system to calculate the unknown quantity F az Approximation is performed, where the basic expression of the RBFNN system is written as in is the ideal weight of RBFNN, ε a Represents the RBFNN estimation error, h(X) represents the Gaussian function value, from which we can notice that:
[0150]
[0151] in,
[0152] S438. According to step S437, the Lyapunov function inequality defined in step S436 is converted to:
[0153]
[0154] in, From this we can see that Therefore, the UAV altitude tracking controller is stable and the tracking error is consistently and ultimately bounded.
[0155] Further, step S44 specifically includes:
[0156] S441. Define the Lyapunov function as follows:
[0157]
[0158] Derivative the defined Lyapunov function, we get:
[0159]
[0160] S442, Definition Substituting the formulas in step S36 and step S37 into the Lyapunov function derived in step S441, we obtain:
[0161]
[0162] S443, by Knowable So we get:
[0163]
[0164] S444, Definition Then there is
[0165] S445. Definition but The inequality can be expressed as:
[0166]
[0167] Substituting the kinematic trajectory tracking error formula designed in step S25 into the above formula, we obtain:
[0168]
[0169] in,
[0170] S446, substituting the dynamic control error formula obtained in step S37 into the formula to obtain:
[0171]
[0172] S447, Definition s U =[s 1U ,s 2U ,...,s NU ] T ,
[0173] s V =[s 1V ,s 2V ,...,s NV ] T ,
[0174] get:
[0175]
[0176] S448, Definition Make in So the error system is stable; and:
[0177]
[0178]
[0179] in,
[0180] Further, step S45 specifically includes:
[0181] S451. Based on the cascade system stability theory, the cascade system composed of the kinematic subsystem and the dynamic subsystem is input-state stable, and ||E3(t)|| is bounded, that is:
[0182]
[0183] When the agent is outside the collision avoidance range, due to therefore In the above stability proof process The size of the term is 0, and the same conclusion is obtained using the same argumentation process. The closed-loop system still satisfies the input-to-state stability.
[0184] Compared with the prior art, the present invention has the following advantages:
[0185] 1. The present invention provides a heterogeneous UAV / USV collision avoidance formation control method with input quantization. It considers the problem of heterogeneous UAV and unmanned ship formation with input quantization for the first time. The tracking error is considered in the kinematic subsystem, and an artificial potential field is introduced to design the kinematic guidance law. The quantization technology is used in the dynamic subsystem to reduce the burden of signal transmission. The quantization process can reduce the execution frequency of the actuator. The quantization process is described by a linear time-varying model and does not require quantization parameter information. ESO is used to estimate the interference of the external environment to achieve air-sea coordinated control.
[0186] 2. The present invention provides a UAV and unmanned ship heterogeneous formation control method with input quantization, proposes an extended state observer stability analysis and a closed-loop system stability determination method, and strictly proves the boundedness of the quantization error and the closed-loop stability of the control system.
[0187] Based on the above reasons, the present invention can be widely promoted in fields such as artificial intelligence. BRIEF DESCRIPTION OF THE DRAWINGS
[0188] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.
[0189] Figure 1 The figure is a flow chart of the method of the present invention.
[0190] Figure 2 A simplified block diagram of the proposed heterogeneous UAV / USV collision avoidance formation control method with input quantization.
[0191] Figure 3 This is a diagram of the collision avoidance trajectory tracking result of the unmanned ship and unmanned aerial vehicle according to an embodiment of the present invention.
[0192] Figure 4 This is a comparison diagram of the longitudinal control input before and after quantization according to an embodiment of the present invention.
[0193] Figure 5This is a comparison diagram of lateral control input before and after quantization according to an embodiment of the present invention.
[0194] Figure 6 This is a quantified longitudinal speed curve of the unmanned aerial vehicle (UAV) and unmanned boat according to an embodiment of the present invention.
[0195] Figure 7 This is a quantified lateral velocity curve of the unmanned aerial vehicle (UAV) and unmanned boat according to an embodiment of the present invention.
[0196] Figure 8 This is a quantified longitudinal tracking error curve of the unmanned aerial vehicle (UAV) and unmanned ship according to an embodiment of the present invention.
[0197] Fig. 9 This is a quantified lateral tracking error curve of the unmanned aerial vehicle and unmanned ship according to an embodiment of the present invention. DETAILED DESCRIPTION
[0198] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.
[0199] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.
[0200] like Figure 1 As shown, the present invention provides a heterogeneous UAV / USV collision avoidance formation control method with input quantization, comprising the following steps:
[0201] S1. To address the inconsistency of mathematical models between UAV and USV, consider using mathematical methods to reorganize and design a consistent mathematical model of heterogeneous agents;
[0202] S2. Based on the target trajectory, a distributed heterogeneous multi-UAV unmanned ship time-varying formation error is constructed, an extended state observer is used to observe the information of neighboring agents contained in the controller, and an improved artificial potential field is introduced to design the kinematic guidance law;
[0203] S3. Take the guidance signal as the input of the dynamics subsystem, design the dynamics underlying controller and adaptive law that considers input quantization, and use a uniform quantizer for the quantization process. Design the altitude controller for the altitude tracking process in the vertical plane to ensure that the formation trajectory tracking goal is achieved;
[0204] S4. Design a Lyapunov function and prove the stability of the closed-loop system based on the input-to-state stability theory.
[0205] In specific implementation, as a preferred embodiment of the present invention, step S1 specifically includes:
[0206] S11. Construct the mathematical model of UAV control as follows:
[0207]
[0208]
[0209] In the formula, p ax ,p ay ,p az represents the spatial position information of the UAV, φ, θ, ψ represent the angle information of the UAV; u p represents the control thrust of the UAV, τ φ ,τ θ ,τ ψ Represents the three control torques of the UAV, m a represents the mass of the UAV, g represents the acceleration due to gravity, represents the overall residual rotor angle, d x ,d y ,d z ,d φ ,d θ ,d ψ is the translational drag coefficient, J ax ,J ay ,J az represents the moment of inertia, J ar represents the inertia moment of the rotor, Δ ax ,Δ ay ,Δ az Indicates the external interference encountered by the UAV;
[0210] The UAV control model in the horizontal plane is as follows:
[0211]
[0212] p a =[p ax ,p ay ] T , g a =diag{1 / m a ,1 / m a}, Δ a =[Δ ax ,Δ ay ] T ,u a =[u ax ,u ay ] T , and there are
[0213] S12. Construct a mathematical model of the unmanned ship as follows:
[0214]
[0215]
[0216]
[0217] In the formula, x si ,y si represents the position information of the i-th USV; ψ si represents the yaw angle of the i-th USV; α i =[μ si ,v si ,r si ] T Represents surge, sway and yaw speed, m μsi ,m vsi ,m rsi They represent inertial mass respectively; f μsi (α i ),f vsi (α i ),f rsi (α i ) represent nonlinear unknown functions, including unmodeled fluid mechanics and Coriolis force; and Respectively represent surge force and yaw moment; w μsi ,w vsi ,w rsi They represent the bounded disturbances caused by waves respectively;
[0218] S13. Define the ship model using the hand position method. The process is as follows:
[0219] In order to deal with the underactuated USV motion model, we consider the idea of hand position method. six ,p siy ) as the hand point, defined as:
[0220]
[0221] Where, L si Indicates the actual position (x si ,y si ) and the newly defined hand point (p six ,p siy ) between the two sides;
[0222] Taking the second derivative of the defined hand point and substituting the USV mathematical model into it, we get:
[0223]
[0224] in: β=[μ si ,v si ,r si ,ψ si ]; then the mathematical model of the USV in the horizontal plane is written as:
[0225]
[0226] In the formula, p si =[p six ,p siy ] T , f sixy =[f six ,f siy ] T ,u si =[τ μ ,τ r ] T , w dixy =[w dix ,w diy ] T ,Ω si (ψ si )=[cosψ si ,-sinψ si ;sinψ si ,cosψ si ],ω si =diag{1 / m μsi ,L si / m rsi};
[0227] In order to ensure the unification of the USV model and the UAV model, the above formula is expanded and further converted into:
[0228]
[0229] S14. Build a unified model of USV and UAV. The construction process is as follows:
[0230] Combining step S12 and step S13, a unified mathematical model of heterogeneous agents in the horizontal plane is obtained as follows:
[0231]
[0232] When the model represents a USV model, represents the position and velocity of the USV, where u xi =[τ μi ,τ ri ] T ,
[0233] Δ xi =[w dix ,w diy ] T ;
[0234] When the model represents a UAV model, represents the position and velocity of the UAV, where u xi =[u ax ,u ay ] T , Δ xi =[Δ ax ,Δ ay ] T ;
[0235] S15. Construct the Z-axis model of the UAV as follows:
[0236]
[0237] S16, quantization technology can convert the initial continuous signal into a segmented, constant signal. The mean quantizer is easy to implement, so the present invention uses the mean quantizer to control the input u xi Convert to Q(u xi ), the process is as follows:
[0238] Considering the quantized control input, the unified model of UAV and USV is:
[0239]
[0240] The model is further transformed to obtain the final heterogeneous agent dynamics model in the XY plane:
[0241]
[0242] Among them, F i =[F iU ,F iV ] T , Q(τ i )=[Q(τ iU ),Q(τ iV )] T , G i =diag[G iU ,G iV ], Δ i =[Δ iU ,Δ iV ] T , τ i =[τ iU ,τ iV ] T =[u xi1 ,u xi2 ] T ;
[0243] The quantization process is described as:
[0244]
[0245]
[0246] Among them, k iU , k iV In the process of quantification, quantization error will be generated, which will reduce the performance of the control system. In order to ensure that the dynamic subsystem of the unmanned system can effectively track the guidance input, a quantitative control scheme with sufficient accuracy and low communication rate must be designed.
[0247] S17. Assuming that the following-agents in the formation have the same potential energy, the potential function of the collision avoidance repulsion field between the following unmanned ships in the formation and the obstacle avoidance potential function between each unmanned ship and the obstacle are expressed as:
[0248]
[0249] Among them, i,j=1...n,i≠j means there are n agents, b=1,...,m means there are m obstacles, and the Euclidean distance between any two agents in the formation is given by The Euclidean distance between the agent and the obstacle is represented by Indicates that, [x i ,y i ]T and [x j ,y j ] T is the position coordinates of the ith agent and the jth agent, [x k ,y k ] T is the position coordinate of the kth obstacle, are the upper and lower bounds of the collision avoidance area; the repulsive field potential function is 0 outside the upper bound of the detection area, and the repulsive field potential function is infinite at the lower bound of the detection area. If the distance between the two agents satisfies Collision avoidance function is greater than zero and the potential function is valid in the additional control input. The distance between the two agents satisfies Collision avoidance function is greater than zero and the potential function is valid in the additional control input.
[0250] By finding the negative gradient of the collision avoidance repulsive force field potential function, we can obtain the corresponding repulsive force function as follows:
[0251]
[0252]
[0253] S18. Consider a time-varying reference trajectory p0(t) = [x0(t), y0(t)] T , where the trajectory parameter is a continuous and differentiable vector. Since the present invention considers multiple agents, graph theory is introduced here to Represents the communication topology diagram of multiple unmanned systems, where V = {n0,n1,...,n N} is the set consisting of all nodes, representing the N ships in the fleet; E = {(i, j)∈V×V} is the set consisting of all edges, representing the information transmission between adjacent ships, (n i ,n j )∈E represents the information exchange between nodes, otherwise it is not true. is the adjacency matrix of the formation system, when (n i ,n j )∈E ij =1, otherwise a ij =0.
[0254] In specific implementation, as a preferred embodiment of the present invention, step S2 specifically includes:
[0255] S21. Define the time-varying formation error of distributed heterogeneous agents as:
[0256]
[0257] in, is the distance of the ith unmanned vehicle relative to the parameterized trajectory, is the relative position deviation between the unmanned vehicles;
[0258] S22. Derivative and simplify the defined distributed heterogeneous agent time-varying formation error to obtain:
[0259]
[0260] in,
[0261] S23, use ESO extended state observer to observe the kinematic unknowns, and design the extended state observer as follows:
[0262]
[0263] in, The observation error is: Further converted to: in, definition in represents the potential function of the defined artificial potential field;
[0264] S24. According to steps S21, S22 and S23, a kinematic guidance law for the heterogeneous agent system is designed as follows:
[0265]
[0266] Among them, K i =diag{k1,k2}, k1, k2 are positive constants, and ε i is a positive constant;
[0267] S25. Based on the designed kinematic guidance law of the heterogeneous intelligent system, the kinematic trajectory tracking error considering the artificial potential field is designed as follows:
[0268]
[0269] In specific implementation, as a preferred embodiment of the present invention, step S3 specifically includes:
[0270] S31, a linear model is used to describe the quantization process, and the linear quantization model is described as Q(τ iU )=q 1iU (t)τ iU +q 2iU (t),
[0271] Q(τ iV )=q 1iV(t)τ iV +q 2iV (t) and there is:
[0272]
[0273] Among them, q 1iU (t), q 1iV (t) is unknown, because the positive and negative signals of are not affected by the quantization process, and we get q 1iU (t)>0,q 1iV (t)>0; if |τ iU (t)|<a,|τ iV (t)|<a, then Q(τ iU (t))、Q(τ iV (t)) is bounded and q 2iU (t), q 2iV (t) is bounded,
[0274] S32. Design the sliding surface as follows:
[0275]
[0276] S33. Derivative the designed sliding surface to obtain:
[0277]
[0278]
[0279] S34. Definition d iU (X) = Δ iU +F iU , d iV (X) = Δ iV +F iV , the derivative in step S34 is considered as:
[0280]
[0281] Similarly, for the sliding surface s iV The same inequality exists;
[0282] S35, using ESO extended state observer to calculate the unknown quantity d in the dynamic model iU (X), d iV (X) Conduct observation and design the observer function model as follows:
[0283]
[0284] in, The observation error is obtained as:
[0285]
[0286] The observer function is further transformed into: in
[0287] S36. Definition From steps S35 and S36, we get:
[0288]
[0289] in, Similarly, there are:
[0290]
[0291]
[0292] S37, due to q 1iU and q 1iV is time-varying and unknown, so adaptive estimation is used, using q 1iU and q 1iV To estimate the lower bound of , we need to avoid singular problems when the estimate is zero. Define a time-varying gain in, Q 1iU ,q 1iV The lower bound of , the dynamic controller and adaptive law are designed as follows:
[0293]
[0294]
[0295] Therefore, the dynamic control error is obtained as:
[0296]
[0297] S38. Define the UAV tracking error in the Z-axis direction as follows:
[0298]
[0299] Among them, p azd is the expected position of UAV, v azd is the expected speed of the UAV;
[0300] S39. Design the tracking controller as follows:
[0301]
[0302] in,
[0303] In specific implementation, as a preferred embodiment of the present invention, step S4 specifically includes:
[0304] S41. Perform stability analysis on the extended state observer designed for the kinematic subsystem;
[0305] S42. Perform stability analysis on the extended state observer designed for the dynamics subsystem;
[0306] S43, performing stability analysis on the Z-axis tracking controller of the UAV;
[0307] S44. Perform stability analysis on the dynamic closed-loop system;
[0308] S45. Perform stability analysis on the closed-loop system.
[0309] In specific implementation, as a preferred embodiment of the present invention, step S41 specifically includes:
[0310] S411. Define the Lyapunov function as follows:
[0311]
[0312] Among them, P i is a positive definite matrix that satisfies
[0313] S412. Deriving the defined Lyapunov function, we obtain:
[0314]
[0315] S413, due to Make So the system is stable and has:
[0316]
[0317] in
[0318] In specific implementation, as a preferred embodiment of the present invention, step S42 specifically includes:
[0319] S421. Define the Lyapunov function as follows:
[0320]
[0321] Among them, Q i is a positive definite matrix that satisfies
[0322] S422. Derivative the defined Lyapunov function to obtain:
[0323]
[0324] S423, due to Make So the system is stable and has:
[0325]
[0326] in,
[0327] In specific implementation, as a preferred embodiment of the present invention, step S43 specifically includes:
[0328] S431, deriving the UAV tracking error in the Z-axis direction defined in step S38, and obtaining:
[0329]
[0330] S432, e azv As a virtual control input, design a virtual signal ζ z =-k ζ e azp To ensure the stability of the tracker;
[0331] S433. Define the Lyapunov function as follows:
[0332]
[0333] S434. Deriving the defined Lyapunov function, we obtain:
[0334]
[0335] S435. Define a new error e ζ =e azv -ζ z , combining the new error with the UAV tracking error in the Z-axis direction defined in step S38 and taking the derivative, we get:
[0336]
[0337] S436. Define a new Lyapunov function And derive it to get:
[0338]
[0339] S437: For the unknown quantity F in step S436 az, using the RBFNN system to calculate the unknown quantity F az Approximation is performed, where the basic expression of the RBFNN system is written as in is the ideal weight of RBFNN, ε a Represents the RBFNN estimation error, h(X) represents the Gaussian function value, from which we can notice that:
[0340]
[0341] in,
[0342] S438. According to step S437, the Lyapunov function inequality defined in step S436 is converted to:
[0343]
[0344] in, From this we can see that Therefore, the UAV altitude tracking controller is stable and the tracking error is consistently and ultimately bounded.
[0345] In specific implementation, as a preferred embodiment of the present invention, step S44 specifically includes:
[0346] S441. Define the Lyapunov function as follows:
[0347]
[0348] Derivative the defined Lyapunov function, we get:
[0349]
[0350] S442, Definition Substituting the formulas in step S36 and step S37 into the Lyapunov function derived in step S441, we obtain:
[0351]
[0352] S443, by Knowable So we get:
[0353]
[0354] S444, Definition Then there is
[0355] S445. Definition but The inequality can be expressed as:
[0356]
[0357] Substituting the kinematic trajectory tracking error formula designed in step S25 into the above formula, we obtain:
[0358]
[0359] in,
[0360] S446, substituting the dynamic control error formula obtained in step S37 into the formula to obtain:
[0361]
[0362] S447. The following stability analysis is divided into two steps. First, we analyze the stability of the closed-loop system within the collision avoidance region. The second step gives the stability analysis outside the collision avoidance region. Definition s U =[s 1U ,s 2U ,...,s NU ] T ,s V =[s 1V ,s 2V ,...,s NV ] T ,
[0363] get:
[0364]
[0365] S448, Definition Make in So the error system is stable; and:
[0366]
[0367]
[0368] in,
[0369] In specific implementation, as a preferred embodiment of the present invention, step S45 specifically includes:
[0370] S451. Based on the cascade system stability theory, the cascade system composed of the kinematic subsystem and the dynamic subsystem is input-state stable, and ||E3(t)|| is bounded, that is:
[0371]
[0372] When the agent is outside the collision avoidance range, due to therefore In the above stability proof process The size of the term is 0, and the same conclusion is obtained using the same argumentation process. The closed-loop system still satisfies the input-to-state stability.
[0373] Example
[0374] In order to verify the effectiveness of the solution of the present invention, this embodiment uses MATLAB to perform heading tracking control simulation, and sets the initial position of each unmanned system to: p1 = [0, 0, 0] T ,p2=[-7,7,0] T ,p3=[-7,-7,8] T ,p4=[-14,14,0] T ,p5=[-14,-14,0] T The required formation of the unmanned systems in the XY plane is set to p 1d =[0,0] T , p 2d =[-7,7] T , p 3d =[-7,-7] T , p 4d =[-14,14] T , p 5d =[-14,-14] T .
[0375] To further discuss the effectiveness of the proposed controller, let the leader UAV follow the time-varying reference trajectory p0 = [0.2t, 2sin(0.1t)-3] T Navigation. From the simulation results, we can see that Figure 3 It shows the formation trajectory of four unmanned ships and one drone. It can be clearly seen from the figure that under the influence of external interference and internal system errors, the unmanned ships and drones will follow the leader from the initial position and move along the expected trajectory. When encountering obstacles, they will perform autonomous collision avoidance. It can be seen from the figure that each USV can successfully complete the collision avoidance and the tracking task of the leader's trajectory can also be completed. Figure 4 and Figure 5The longitudinal and lateral speeds of four USVs and one UAV in the XY plane are described. As shown in the figure, when the formation is sailing normally, the speed change is small, and when collision avoidance operations are required during the formation, the speed change will be increased. Figure 6 Describes the longitudinal control input with and without input quantization, Figure 7 The lateral control inputs with and without input quantization are described respectively. It can be seen that when the formation performs collision avoidance maneuvers, the controller will provide huge thrust to the relevant collision avoidance ships to achieve the collision avoidance goal, and the two are compared. The quantization process can ensure that the control signal maintains the quality of the control scheme without losing control performance. Figure 8 and Fig. 9 The formation tracking error of four USVs and one UAV in the XY plane is described. When performing collision avoidance operations, the tracking error will increase due to the need to change direction, but when the collision avoidance phase is over, the formation tracking error will decrease. Obviously, they can complete the formation task with a smaller error under the controller designed by the present invention.
Claims
1. A heterogeneous UAV / USV collision avoidance formation control method with input quantization, characterized in that: The steps include: S1. To address the inconsistency of mathematical models between UAV and USV, consider using mathematical methods to reorganize and design a consistent mathematical model of heterogeneous agents; S2. Based on the target trajectory, a distributed heterogeneous multi-UAV unmanned ship time-varying formation error is constructed, an extended state observer is used to observe the information of neighboring agents contained in the controller, and an improved artificial potential field is introduced to design the kinematic guidance law; S3. Take the guidance signal as the input of the dynamics subsystem, design the dynamics underlying controller and adaptive law that considers input quantization, and use a uniform quantizer for the quantization process. Design the altitude controller for the altitude tracking process in the vertical plane to ensure that the formation trajectory tracking goal is achieved; S4. Design a Lyapunov function and prove the stability of the closed-loop system based on the input-to-state stability theory.
2. The heterogeneous UAV / USV collision avoidance formation control method with input quantization according to claim 1, characterized in that: Step S1 specifically includes: S11. Construct the mathematical model of UAV control as follows: In the formula, p ax ,p ay ,p az represents the spatial position information of the UAV, φ, θ, ψ represent the angle information of the UAV; u p represents the control thrust of the UAV, τ φ ,τ θ ,τ ψ Represents the three control torques of the UAV, m a represents the mass of the UAV, g represents the acceleration due to gravity, represents the overall residual rotor angle, d x ,d y ,d z ,d φ ,d θ ,d ψ is the translational drag coefficient, J ax ,J ay ,J az represents the moment of inertia, J ar represents the inertia moment of the rotor, Δ ax ,Δ ay ,Δ az Indicates the external interference encountered by the UAV; The UAV control model in the horizontal plane is as follows: p a =[p ax ,p ay ] T , g a =diag{1 / m a ,1 / m a }, Δ a = [Δ ax ,Δ ay ] T , u a =[u ax ,u ay ] T , there is S12. Construct a mathematical model of the unmanned ship as follows: In the formula, x si ,y si represents the position information of the i-th USV; ψ si represents the yaw angle of the i-th USV; α i =[μ si ,v si ,r si ] T Represents surge, sway and yaw speed, m μsi ,m vsi ,m rsi They represent inertial mass respectively; f μsi (α i ),f vsi (α i ),f rsi (α i ) represent nonlinear unknown functions, including unmodeled fluid mechanics and Coriolis force; and Respectively represent surge force and yaw moment; w μsi ,w vsi ,w rsi They represent the bounded disturbances caused by waves respectively; S13. Define the ship model using the hand position method. The process is as follows: (p six ,p siy ) as the hand point, defined as: Where, L si Indicates the actual position (x si ,y si ) and the newly defined hand point (p six ,p siy ) between the two sides; Taking the second derivative of the defined hand point and substituting the USV mathematical model into it, we get: in: β=[μ si ,v si ,r si ,ψ si ]; then the mathematical model of the USV in the horizontal plane is written as: where p si = [p six , p siy T , f sixy = [f six , f siy T , u si = [τ μ , τ r T , w dixy = [w dix , w diy T , Ω si (ψ si ) = [cosψ si , -sinψ si ; sinψ si , cosψ si , ω si = diag{1 / m μsi , L si / m rsi}; In order to ensure the unification of the USV model and the UAV model, the above formula is expanded and further converted into: S14. Build a unified model of USV and UAV. The construction process is as follows: Combining step S12 and step S13, a unified mathematical model of heterogeneous agents in the horizontal plane is obtained as follows: When the model represents a USV model, represents the position and velocity of the USV, where u xi =[τ μi ,τ ri ] T , Δ xi =[w dix ,w diy ] T ; When the model represents a UAV model, represents the position and velocity of the UAV, where u xi =[u ax ,u ay ] T , Δ xi =[Δ ax ,Δ ay ] T ; S15. Construct the Z-axis model of the UAV as follows: S16, using the mean quantizer, the control input u xi Convert to Q(u xi ), the process is as follows: Considering the quantized control input, the unified model of UAV and USV is: The model is further transformed to obtain the final heterogeneous agent dynamics model in the XY plane: Among them, F i =[F iU ,F iV ] T ,Q(τ i )=[Q(τ iU ),Q(τ iV )] T ,G i =diag[G iU ,G iV ],D i =[D iU ,D iV ] T ,t i =[τ iU ,t iV ] T =[u xi1 ,u xi2 ] T ; The quantization process is described as: Among them, k iU , k iV For quantitative indicators; S17. Assuming that the following-agents in the formation have the same potential energy, the potential function of the collision avoidance repulsion field between the following unmanned ships in the formation and the obstacle avoidance potential function between each unmanned ship and the obstacle are expressed as: Among them, i,j=1...n,i≠j means there are n agents, b=1,...,m means there are m obstacles, and the Euclidean distance between any two agents in the formation is given by The Euclidean distance between the agent and the obstacle is represented by Indicates that, [x i ,y i ] T and [x j ,y j ] T is the position coordinates of the ith agent and the jth agent, [x k ,y k ] T is the position coordinate of the kth obstacle, are the upper and lower bounds of the collision avoidance zone; By finding the negative gradient of the collision avoidance repulsive force field potential function, we can obtain the corresponding repulsive force function as follows: S18. Consider a time-varying reference trajectory p0(t) = [x0(t), y0(t)] T , where the trajectory parameter is a continuous differentiable vector. By introducing graph theory, Represents the communication topology diagram of multiple unmanned systems, where V = {n0,n1,...,n N } is the set consisting of all nodes, representing the N ships in the fleet; E = {(i, j)∈V×V} is the set consisting of all edges, representing the information transmission between adjacent ships, (n i ,n j )∈E represents the information exchange between nodes, otherwise it is not true. is the adjacency matrix of the formation system, when (n i ,n j )∈E ij =1, otherwise a ij =0.
3. A heterogeneous UAV / USV collision avoidance formation control method with input quantization according to claim 1, characterized in that: Step S2 specifically includes: S21. Define the time-varying formation error of distributed heterogeneous agents as: in, is the distance of the ith unmanned vehicle relative to the parameterized trajectory, is the relative position deviation between the unmanned vehicles; S22. Derivative and simplify the defined distributed heterogeneous agent time-varying formation error to obtain: in, S23, use ESO extended state observer to observe the kinematic unknowns, and design the extended state observer as follows: in, The observation error is: Further converted to: in, definition in represents the potential function of the defined artificial potential field; S24. According to steps S21, S22 and S23, a kinematic guidance law for the heterogeneous agent system is designed as follows: Among them, K i =diag{k1,k2}, k1, k2 are positive constants, and ε i is a positive constant; S25. Based on the designed kinematic guidance law of the heterogeneous intelligent system, the kinematic trajectory tracking error considering the artificial potential field is designed as follows:
4. A heterogeneous UAV / USV collision avoidance formation control method with input quantization according to claim 1, characterized in that: Step S3 specifically includes: S31, a linear model is used to describe the quantization process, and the linear quantization model is described as Q(τ iU )=q 1iU (t)τ iU +q 2iU (t), Q(τ iV )=q 1iV (t)τ iV +q 2iV (t) and there is: Among them, q 1iU (t), q 1iV (t) is unknown, because the positive and negative signals of are not affected by the quantization process, and we get q 1iU (t)>0,q 1iV (t)>0; if |τ iU (t)|<a,|τ iV (t)|<a, then Q(τ iU (t))、Q(τ iV (t)) is bounded and q 2iU (t), q 2iV (t) is bounded, S32. Design the sliding surface as follows: S33. Derivative the designed sliding surface to obtain: S34. Definition d iU (X) = Δ iU +F iU , d iV (X) = Δ iV +F iV , the derivative in step S34 is considered as: S35, using ESO extended state observer to calculate the unknown quantity d in the dynamic model iU (X), d iV (X) Conduct observation and design the observer function model as follows: in, The observation error is obtained as: The observer function is further transformed into: in S36. Definition From steps S35 and S36, we get: in, Similarly, there are: S37, due to q 1iU and q 1iV is time-varying and unknown, so adaptive estimation is used, using q 1iU and q 1iV The lower bound of is estimated, and a time-varying gain is defined in, for q 1iU ,q 1iV The lower bound of , the dynamic controller and adaptive law are designed as follows: Therefore, the dynamic control error is obtained as: S38. Define the UAV tracking error in the Z-axis direction as follows: Among them, p azd is the expected position of UAV, v azd is the expected speed of the UAV; S39. Design the tracking controller as follows: in, 5. A heterogeneous UAV / USV collision avoidance formation control method with input quantization according to claim 1, characterized in that: Step S4 specifically includes: S41. Perform stability analysis on the extended state observer designed for the kinematic subsystem; S42. Perform stability analysis on the extended state observer designed for the dynamics subsystem; S43, performing stability analysis on the Z-axis tracking controller of the UAV; S44. Perform stability analysis on the dynamic closed-loop system; S45. Perform stability analysis on the closed-loop system.
6. A heterogeneous UAV / USV collision avoidance formation control method with input quantization according to claim 1, characterized in that: Step S41 specifically includes: S411. Define the Lyapunov function as follows: Among them, P i is a positive definite matrix that satisfies S412. Deriving the defined Lyapunov function, we obtain: S413, due to Make So the system is stable and has: in, 7. A heterogeneous UAV / USV collision avoidance formation control method with input quantization according to claim 1, characterized in that: Step S42 specifically includes: S421. Define the Lyapunov function as follows: Among them, Q i is a positive definite matrix that satisfies S422. Derivative the defined Lyapunov function to obtain: S423, due to Make So the system is stable and has: in, 8. A heterogeneous UAV / USV collision avoidance formation control method with input quantization according to claim 1, characterized in that: Step S43 specifically includes: S431, deriving the UAV tracking error in the Z-axis direction defined in step S38, and obtaining: S432, e azv As a virtual control input, design a virtual signal ζ z =-k ζ e azp To ensure the stability of the tracker; S433. Define the Lyapunov function as follows: S434. Deriving the defined Lyapunov function, we obtain: S435. Define a new error e ζ =e azv -ζ z , combining the new error with the UAV tracking error in the Z-axis direction defined in step S38 and taking the derivative, we get: S436. Define a new Lyapunov function And derive it to get: S437: For the unknown quantity F in step S436 az , using the RBFNN system to calculate the unknown quantity F az Approximation is performed, where the basic expression of the RBFNN system is written as in is the ideal weight of RBFNN, ε a Represents the RBFNN estimation error, h(X) represents the Gaussian function value, from which we can notice that: in, S438. According to step S437, the Lyapunov function inequality defined in step S436 is converted to: Among them, θ z =min{2k ζ ,2σ z ,ι 11 k 11 ,ι 12 k 12 ,ι 13 k 13 }>0, From this we can see that Therefore, the UAV altitude tracking controller is stable and the tracking error is consistently and ultimately bounded.
9. A heterogeneous UAV / USV collision avoidance formation control method with input quantization according to claim 1, characterized in that: Step S44 specifically includes: S441. Define the Lyapunov function as follows: Derivative the defined Lyapunov function, we get: S442, Definition Substituting the formulas in step S36 and step S37 into the Lyapunov function derived in step S441, we obtain: S443, by Knowable So we get: S444, Definition Then there is S445. Definition but The inequality can be expressed as: Substituting the kinematic trajectory tracking error formula designed in step S25 into the above formula, we obtain: in, S446, substituting the dynamic control error formula obtained in step S37 into the formula to obtain: S447, Definition get: S448, Definition Make in So the error system is stable; and: in, 10. A heterogeneous UAV / USV collision avoidance formation control method with input quantization according to claim 1, characterized in that: Step S45 specifically includes: S451. Based on the cascade system stability theory, the cascade system composed of the kinematic subsystem and the dynamic subsystem is input-state stable, and ||E3(t)|| is bounded, that is: When the agent is outside the collision avoidance range, due to therefore In the above stability proof process The size of the term is 0, and the same conclusion is obtained using the same argumentation process. The closed-loop system still satisfies the input-to-state stability.
Citation Information
Cited By
Bipartite sliding mode formation control method for dynamically observing heterogeneous USV-UAV system based on preset time
CN120386354A
Method for dynamic observation of heterogeneous usv-uav system bipartite sliding mode formation control based on preset time
CN120386354B
