CN112650214A - Formation control method for dynamic formation of cluster system - Google Patents
Formation control method for dynamic formation of cluster system Download PDFInfo
- Publication number
- CN112650214A CN112650214A CN202011307984.XA CN202011307984A CN112650214A CN 112650214 A CN112650214 A CN 112650214A CN 202011307984 A CN202011307984 A CN 202011307984A CN 112650214 A CN112650214 A CN 112650214A
- Authority
- CN
- China
- Prior art keywords
- path
- formation
- obstacle
- cluster
- cluster system
- 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.)
- Granted
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 102
- 238000000034 method Methods 0.000 title claims abstract description 52
- 238000004891 communication Methods 0.000 claims abstract description 21
- 230000008569 process Effects 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims abstract description 14
- 230000007613 environmental effect Effects 0.000 claims abstract description 13
- 230000001133 acceleration Effects 0.000 claims description 23
- 239000000178 monomer Substances 0.000 claims description 16
- 230000004888 barrier function Effects 0.000 claims description 15
- 238000012545 processing Methods 0.000 claims description 6
- 238000010586 diagram Methods 0.000 description 8
- 230000005855 radiation Effects 0.000 description 5
- 230000008859 change Effects 0.000 description 3
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000002829 reductive effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000002147 killing effect Effects 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 238000000465 moulding Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles
Abstract
The invention discloses a formation control method for dynamic formation of a cluster system, which comprises the following steps: performing optimal path search for a navigation host based on an environmental threat map, and comprehensively considering the shortest path and the minimum obstacle threat to obtain a comprehensive optimal path; parameterizing the optimal path and re-optimizing the parameterized path information; distributing following paths for the rest following slaves through formation information and parameterizing; according to the communication conditions, finishing formation coordination control for the formation path through the speed control quantity; obstacle avoidance control is completed on obstacles added later in the formation process; and updating the path information periodically, and updating and synchronizing the latest path parameterization matrix in the cluster system. The invention solves the problem that the prior control method for formation of the cluster system is difficult to complete the formation of complex curves under the condition of limited communication conditions and has low precision.
Description
Technical Field
The invention relates to the technical field of formation control of a cluster intelligent system, in particular to a formation control method for dynamic formation of a cluster system.
Background
The cluster system adopts a cluster mode to expand the capability of the monomer system, efficiently cooperates and coordinates to complete tasks, and the monomers are mutually functionally backed up, so that the task failure caused by the monomer failure can be avoided. The cluster system generally needs to complete the task by maintaining a certain formation form, and the accuracy of the formation form may directly affect whether the task is completed or not. The current cluster formation can be divided into static and dynamic. Static formation plans the track for each monomer in advance through molding and transform according to the formation at the ground station, and set up unified time stamp for each track point, the track that will plan is stored in the inside storage equipment of each monomer, send the start command by the ground station broadcast when the task of formation begins, follow-up every cluster monomer can follow-up predetermined route according to the synchronous tracking of time stamp, can obtain very high formation precision, and the motion process need not communication, and interference killing feature is strong, but can't change formation information at the formation in-process, can only be used for formation performance or fixed path to patrol and examine etc..
For dynamic formation, in most algorithm researches of the current cluster formation path, shortest path search from a starting point to an end point is adopted, generally, only the space position avoiding obstacles is considered, and the type judgment and pretreatment of the obstacles are lacked. Therefore, a suitable method is found for carrying out more reasonable classification judgment on the obstacle information, and the planned path can be more reasonable and safe by preprocessing the obstacle information before path planning. The high-precision formation required by dynamic formation is maintained and tracked, and the relative position is usually maintained after position information is acquired among all monomers in the cluster system, but the mode needs to acquire state information among other monomers in the cluster system all the time, and the formation and the precision are influenced by communication delay or short loss. Therefore, the method for acquiring formation information in dynamic formation is improved, and the improvement of the formation precision of complex curves is an urgent problem to be solved.
Disclosure of Invention
The invention aims to provide a formation control method for dynamic formation of a cluster system, which aims to solve the problem that the formation accuracy of cluster systems such as unmanned aerial vehicles, unmanned vehicles and unmanned ships is not high.
In order to solve the above problems, the present invention is realized by the following technical scheme:
a formation control method for dynamic formation of a cluster system comprises the following steps: and step S1, establishing an environmental obstacle threat map. And step S2, acquiring an optimal path for a pilot host in the cluster system according to the environmental obstacle threat map. And step S3, carrying out parameterization processing on the optimal path to obtain first path information. And step S4, optimizing the first path information to obtain second path information, wherein the second path information meets preset kinematic constraint conditions and obstacle avoidance requirements. And step S5, distributing an independent following path for each following slave in the cluster system according to the second path information, parameterizing the following paths, and then synchronizing in the cluster system through a communication data chain. Step S6, the pilot host computer is at constant speed VdFollowing the second path information, if communication is lost in the cluster system and the state information of the cluster system cannot be obtained, the following slave is also at the constant speed VdFollowing the path to maintain formation; if the communication in the cluster system is normal, the constant speed V isdBased on the speed of the slave, the coordinated speed V of the following slave is obtained by a speed coordination controllerr。
Preferably, the method further comprises the following steps: step S7, the cluster system detects the environmental information in real time in the moving process through the carried sensor, and sets the radius of influence r for the obstacle after detecting a new obstacleiTo enter this influence radius riThe cluster single body is controlled to avoid the obstacle at the constant speed VdOr the coordinated speed VrOn the basis, the obstacle avoidance acceleration a is increasedi。
Preferably, the method further comprises the following steps: and S8, in the process of formation, searching a new optimal path in a fixed period or a variable period according to the environment complexity and ground control input information, and repeating the steps S3-S7 according to the latest optimal path.
Preferably, the step S1 includes: dividing a square region from a starting point to an end point into n-x-n grids, wherein each grid is a node.
Determining an impassable area based on the obstacle type, setting different threat coefficients for each grid according to the distance and direction information of the obstacle type, and performing superposition processing on each threat coefficient to obtain an n-x-n obstacle threat coefficient matrix.
Preferably, a heuristic algorithm is used to obtain the optimal path: and taking the corresponding barrier threat coefficients in the barrier threat coefficient matrix and the distance function to the end point as heuristic values. The path length from the starting point to the current position according to the minimum heuristic value path is taken as a cost value. And taking the sum of the cost value and the heuristic value as the cost value of each node. And calculating cost values of nodes around the current node from the starting point, continuously searching from the minimum cost value points until reaching the target point, and connecting the minimum cost value points to obtain the optimal path.
Preferably, the first path information h (x) is calculated by using the following formula:
in the formula, xiAnd yiRespectively setting the x-axis coordinate and the y-axis coordinate of the ith original planning path point; q. q.siA derivative representing the original planned path point i; x is the number ofi+1And yi+1The x-axis coordinate and the y-axis coordinate of the (i + 1) th original planning path point are respectively; q. q.si+1The derivative of the i +1 th original planned path point is indicated.
Preferably, the step S4 includes:
and S4.1, adding the middle point of the straight line connecting line of the front and the back original path points of the path point at the next path point which is collided in the first path information.
And S4.2, searching a new path again to obtain the second path information.
And S4.3, repeating the step S4.1 to the step S4.2 until the second path information meets the requirements of kinematic constraint and obstacle avoidance.
Preferably, the step S5 includes: and superposing coordinate offset on the basis of the second path information of the pilot host by taking the mass center of the pilot host as an origin, the speed tangential direction as an x axis and the normal direction as a y axis to obtain the parameterized following path of each following slave.
The coordinate offset dijThe following formula is adopted for representation:
dij＝(xoff_ij,yoff_ij,zoff_ij)
in the formula, xoff_ijRepresenting the x-coordinate vector, y, representing the space between the clustered singlets i to joff_ijRepresenting a vector of y coordinates, z, representing the number of clustered singlets i to joff_ijA z-coordinate vector representing the cluster between singlets i through j is shown.
Preferably, the step S6 includes: coordinated speed V of the following slaverCalculated by the following formula:
in the formula, | | Vd||∈(vmin+Δv,vmax-Δv)，f(x)＝2/(1+e(-x)) -1, f (x) e (-1,1), the final coordinated velocity satisfying Vr∈(vmin,vmax)，vminRepresenting the minimum movement speed of the cluster single machine; v. ofmaxRepresenting the maximum movement speed of the cluster single machine; Δ v represents the magnitude of the adjustable velocity amount; e.g. of the type(-x)Represents the power of-x representing the natural index e.
Preferably, the step S7 includes: the obstacle avoidance acceleration aiCalculated by the following formula:
where b and c are control force coefficients, ρijThe distance between the cluster monomer i and the obstacle j; the obstacle avoidance acceleration aiIs directed by the obstacle point towards the cluster cell i.
When the detected new obstacle has a movement speed, the obstacle avoidance acceleration a needs to be adjustediIn the direction of (a).
The adjustment method is that the radius of influence r of the barrier is adjusted when the barrier entersiWhen the cluster monomer intersects the motion velocity vector of the obstacle: if the following path of the cluster single body is at the right of the affected cluster single body, the obstacle-avoiding acceleration a is observed from the speed direction of the affected cluster single bodyiIs rotated 90 degrees counter clockwise. Observing from the speed direction of the affected cluster single body, if the following path of the cluster single body is at the left of the affected cluster single body, avoiding the obstacle acceleration aiIs rotated 90 degrees clockwise.
The invention has at least one of the following advantages:
compared with the traditional formation control method, the method has the advantages that the barrier information is classified, and threat coefficient hypothesis is introduced, so that the planned path information is more reasonable and safer; the dynamic obstacles in the formation process are reasonably optimized and avoided according to the movement direction of the obstacles, so that the path is more optimal; the searched path information is described and periodically updated by segmented parameterized data, the formation shape can be guaranteed not to be influenced in a period of communication loss, the parameterized data can realize accurate description of the formation path, and the formation controller can achieve higher formation control accuracy. Therefore, the interference of communication delay and short loss on formation forms can be reduced, and the precision and the stability of a cluster system for tracking a complex curve path are greatly improved. The invention is applied to the high-precision formation control of cluster systems such as unmanned planes, unmanned vehicles, unmanned ships and the like.
Drawings
Fig. 1 is a flowchart of a method for controlling formation of a dynamic formation of a cluster system according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a common point derivative solution for piecewise curves provided in accordance with an embodiment of the present invention;
FIG. 3 is a diagram illustrating parameterized path optimization adjustment according to an embodiment of the present invention;
FIG. 4 is a schematic diagram of a cluster formation coordinate system according to an embodiment of the present invention;
fig. 5 is a schematic diagram of a direction of acceleration adjustment for dynamic obstacle avoidance according to an embodiment of the present invention.
Detailed Description
The following describes in detail a formation control method for dynamic formation of a cluster system according to the present invention with reference to the accompanying drawings and the detailed description. The advantages and features of the present invention will become more apparent from the following description. It is to be noted that the drawings are in a very simplified form and are all used in a non-precise scale for the purpose of facilitating and distinctly aiding in the description of the embodiments of the present invention. To make the objects, features and advantages of the present invention comprehensible, reference is made to the accompanying drawings. It should be understood that the structures, ratios, sizes, and the like shown in the drawings and described in the specification are only used for matching with the disclosure of the specification, so as to be understood and read by those skilled in the art, and are not used to limit the implementation conditions of the present invention, so that the present invention has no technical significance, and any structural modification, ratio relationship change or size adjustment should still fall within the scope of the present invention without affecting the efficacy and the achievable purpose of the present invention.
As shown in fig. 1, the method for controlling formation of a dynamic formation of a cluster system provided in this embodiment includes: and step S1, establishing an environmental obstacle threat map. Namely, an environmental obstacle threat map is established, and an obstacle threat coefficient matrix of n x n is obtained. The step S1 includes: the square region from the start point to the end point is divided into a grid of n x n. Each grid is a node; and the size of each grid is determined according to the physical size and the formation precision of the specific composition unit of the cluster system.
Determining an impassable region (for example, setting a physical space range of obstacles such as trees, buildings and the like as an impassable region) based on an obstacle type (an obstacle type), setting different threat coefficients for each grid according to distance and direction information of the obstacle type (of different obstacles), and performing superposition processing on each threat coefficient to obtain an n x n obstacle threat coefficient matrix (obtaining an n x n obstacle threat coefficient matrix after performing superposition processing on the threat coefficients distributed to the obstacles in each grid). The closer to the obstacle (obstacle) the larger the threat coefficient, the more the path planning algorithm should avoid the area. For example, for radiation source barriers, the physical space impassable area of each barrier is equal in size, but the radiation intensity is different, so by setting a larger threat factor for a radiation source barrier with a higher radiation intensity, a more rational path can be planned between two radiation sources with different intensities.
And step S2, obtaining an optimal path for the pilot host in the cluster system according to the environmental obstacle threat map (searching the optimal path for the pilot host based on the environmental obstacle threat map).
The step S2 includes: when the optimal path search is carried out, only the currently known environmental obstacle information is considered, and obstacles detected by sensors of subsequent cluster systems are not considered. There are multiple available paths to reach the end point avoiding these obstacles, but considering the performance constraints of the cluster system, an optimal path needs to be found.
Searching the optimal path by adopting a heuristic algorithm: taking as heuristic values h (n) a function of distances to end points on which the corresponding obstacle threat coefficients in the matrix of obstacle threat coefficients are superimposed:
wherein a is a weight coefficient, wiIs the threat coefficient of the ith obstacle, i is 1,2, …, k, dmaxIs the maximum possible distance between a cluster monomer (such as an unmanned aerial vehicle) and an obstacle center in the path searching process, d (p)i) Is a cluster monomerThe distance from the center of the ith obstacle, x and y are coordinates of the current search position, and xd，ydN represents the nth node as the end point coordinate.
And taking the path length from the starting point to the current position according to the minimum heuristic value h (n) as a cost value, wherein the sum of the cost value and the heuristic value is the cost value of each node. Calculating cost values of surrounding nodes (the surrounding nodes refer to 8 nodes in total, namely the upper, lower, left and right sides of the current node and 4 diagonal angles) from the starting point, continuing searching from the minimum cost value points until reaching the target point, and connecting the minimum cost value points to obtain the searched optimal path.
That is, in step S2, the optimal path search is performed for the pilot host based on the threat map, and the shortest path and the minimum obstacle threat are comprehensively considered to obtain a comprehensive optimal path.
Step S3, performing parameterization on the optimal path to obtain first path information (parameterized planned path information).
The obtained optimal path is discretized, each discrete path is composed of segmented broken lines, and the discrete paths can have large corners. Therefore, parameterization is realized after polynomial fitting is adopted for segmenting the optimal path, and in order to ensure that the cluster system tracks the planned path on the premise of meeting the kinematic constraint, the parameterized path needs to meet the following 2 points: the first point is a passing original path point; the second point is that the derivatives of the parameterized segmented paths at the original common path point are the same. As shown in fig. 2, the derivative can be determined from the fact that the tangent of the parameterized path at the original path point is perpendicular to the bisector of the angle of the broken line of the discrete path points. The path after the parameter (first path information) h (x) is obtained by calculating:
in the formula, xiAnd yiRespectively setting the x-axis coordinate and the y-axis coordinate of the ith original planning path point; q. q.siA derivative representing the original planned path point i; x is the number ofi+1And yi+1The x-axis coordinate and the y-axis coordinate of the (i + 1) th original planning path point are respectively; q. q.si+1The derivative of the i +1 th original planned path point is indicated.
That is, the step S3 parameterizes the path by polynomial fitting so as to propagate through the communication network.
And step S4, optimizing the first path information to obtain second path information (path information after parameterization is optimized), wherein the second path information meets preset kinematic constraint conditions and obstacle avoidance requirements.
Specifically, the kinematic constraint conditions are: a is less than or equal to amax,||v||≤vmaxV, a and λ are cluster stand-alone (e.g. drone) speed, acceleration and turning radius, respectively, vmax、amaxAnd λmaxMaximum speed, maximum acceleration and maximum turning radius, respectively. Obstacle avoidance requirements: d < Rmin. d and RminThe distance between the cluster single machine and the obstacle and the influence radius of the obstacle are respectively, and collision can occur when the cluster single machine enters the radius.
The step S4 includes: as shown in fig. 3, the parameterized path may have a possibility of colliding again with an obstacle. The following optimization scheme is adopted:
and S4.1, adding the middle point of a straight line connecting line of the front and rear original path points of the next path point which is collided in the first path (the first path information).
And S4.2, searching a new path again to obtain the second path (second path information).
The searching is to add two path points and then recalculate the path points according to the formula h (x) to obtain new second path information.
And S4.3, repeating the step S4.1 to the step S4.2 until the second path information meets the requirements of kinematic constraint and obstacle avoidance.
That is, in step S4, the parameterized path information is re-optimized until the optimized curve meets the requirements of kinematic constraint and obstacle avoidance.
Step S5, allocating an independent following path to each following slave in the cluster system according to the second path information, parameterizing the following path, and synchronizing in the cluster system through a communication data chain (queue formation allocation of the cluster system).
The step S5 includes: on the basis of the parameterized path searched and obtained for the pilot host, coordinate offset is increased, an independent following path is distributed for each following slave in the cluster system, and then the piecewise curve parameters of the following path are synchronized in the cluster system through a communication data link in a parameter matrix form.
As shown in fig. 4, with the centroid of the pilot host as the origin, the tangential direction of the velocity as the x-axis, and the normal direction as the y-axis, the coordinate offset d is superimposed on the second path information of the pilot hostij＝(xoff_ij,yoff_ij,zoff_ij) Obtaining the parameterized following path of each following slave (obtaining the paths of the rest following slaves);
the coordinate offset dijThe following formula is adopted for representation:
dij＝(xoff_ij,yoff_ij,zoff_ij) (3)
in the formula, xoff_ijRepresenting the x-coordinate vector, y, representing the space between the clustered singlets i to joff_ijRepresenting a vector of y coordinates, z, representing the number of clustered singlets i to joff_ijA z-coordinate vector representing the cluster between singlets i through j is shown.
That is, the step S5 assigns following paths to the remaining following slaves through the queuing information and parameterizes them. The formation information refers to formation information of the cluster systems, that is, according to what formation the cluster systems should be arranged, such as a straight line, a herringbone, a pentagram and the like. Formation information is obtained by coordinate offset dijTo be embodied.
Step S6, performing formation path coordination control, wherein the pilot host computer performs constant speed VdFollowing the formation path (second path information), if communication is lost in the cluster system and the state information of the cluster system cannot be obtained, the following slave is also at the constant speed VdFollowing the path to maintain formation; if the communication in the cluster system is normal, the constant speed V isdBased on the speed of the slave, the coordinated speed V of the following slave is obtained by a speed coordination controllerrTo further improve the formation accuracy. That is, in step S6, the formation cooperative control is completed for the formation path by the speed control amount according to the communication condition.
The state information of the cluster system refers to the attitude, position, speed, data link signal strength, electric quantity, task progress and other information of each single machine in the cluster.
Coordinated speed V of the following slaverCalculated by the following formula:
in the formula, | | Vd||∈(vmin+Δv,vmax-Δv)，f(x)＝2/(1+e(-x)) -1, f (x) e (-1,1), the final coordinated velocity satisfying Vr∈(vmin,vmax)，vminRepresenting the minimum movement speed of the cluster single machine; v. ofmaxRepresenting the maximum movement speed of the cluster single machine; Δ v represents the magnitude of the adjustable velocity amount; e.g. of the type(-x)Represents the power of-x representing the natural index e.
This embodiment still includes: step S7, obstacle avoidance control, namely, the cluster system detects environmental information in real time in the moving process through a carried sensor, and after a new obstacle is detected, an influence radius r is set for the obstacleiTo enter this influence radius riThe cluster single body is controlled to avoid the obstacle at the constant speed VdOr the coordinated speed VrOn the basis, the obstacle avoidance acceleration a is increasedi。
The obstacle avoidance acceleration aiCalculated by the following formula:
where b and c are control force coefficients, ρijThe distance between the cluster monomer i and the obstacle j; the obstacle avoidance acceleration aiIs directed by the obstacle point towards the cluster cell i.
As shown in fig. 5, the repulsive force is rotated 90 degrees counterclockwise in the case of fig. a, b; in the case of fig. c and d, the rotation is 90 degrees clockwise.
In particular, when the detected new obstacle has a movement speed, the obstacle avoidance acceleration a needs to be adjustediIn the direction of (a).
The adjustment method is that the radius of influence r of the barrier is adjusted when the barrier entersiWhen the cluster monomer intersects the motion velocity vector of the obstacle:
if the target path (the following path of the cluster single body) is at the right side of the affected cluster single body, viewed from the speed direction of the affected cluster single body, the obstacle avoidance acceleration a is obtainediRotates 90 degrees counterclockwise (the obstacle avoidance acceleration is multiplied by a 90-degree direction cosine matrix R (90)).
If the target path (the following path of the cluster single body) is at the left of the affected cluster single body, viewed from the speed direction of the affected cluster single body, the obstacle avoidance acceleration a is obtainediRotates 90 degrees clockwise (the obstacle avoidance acceleration is multiplied by a-90 degree direction cosine matrix R (-90)).
That is, step S7 is to complete obstacle avoidance control for the obstacle added later in the formation process.
This embodiment still includes: and S8, updating path information, namely searching a new optimal path in a fixed period or a variable period according to the environment complexity and ground control input information in the queuing process, and repeating the steps S3-S7 according to the latest optimal path (and updating and synchronizing the latest path parameterized matrix in the cluster system). The step S8 is to periodically update the path information and synchronize the latest path parameterization matrix in the cluster system.
The subjects studied in this example were based on dynamic formation. Dynamic formation can change formation form and track in real time according to ground control input and geographic environment information in the motion process through a data chain, so the formation is more flexible and has stronger applicability, but communication delay and loss, bandwidth and external interference all influence formation precision. Therefore, the high-precision formation control method is suitable for achieving complex curve path tracking of cluster systems such as unmanned aerial vehicles, unmanned vehicles and unmanned ships in complex spaces under the condition of avoiding obstacles.
In the embodiment, the barrier information is classified, and the threat coefficient hypothesis is introduced, so that the planned path information is more reasonable and safer; the dynamic obstacles in the formation process are reasonably optimized and avoided according to the movement direction of the obstacles, so that the path is more optimal; the searched path information is described and periodically updated by segmented parameterized data, the formation shape can be guaranteed not to be influenced in a period of communication loss, the parameterized data can realize accurate description of the formation path, and the formation controller can achieve higher formation control accuracy. Therefore, the interference of communication delay and short loss on formation forms can be reduced, and the precision and the stability of a cluster system for tracking a complex curve path are greatly improved.
It should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other identical elements in a process, method, article, or apparatus that comprises the element.
It should be noted that the apparatuses and methods disclosed in the embodiments herein can be implemented in other ways. The apparatus embodiments described above are merely illustrative, and for example, the flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments herein. In this regard, each block in the flowchart or block diagrams may represent a module, a program, or a portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems that perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
While the present invention has been described in detail with reference to the preferred embodiments, it should be understood that the above description should not be taken as limiting the invention. Various modifications and alterations to this invention will become apparent to those skilled in the art upon reading the foregoing description. Accordingly, the scope of the invention should be determined from the following claims.
Claims (10)
1. A formation control method for dynamic formation of a cluster system is characterized by comprising the following steps:
step S1, establishing an environmental barrier threat map;
step S2, obtaining an optimal path for a pilot host in the cluster system according to the environmental obstacle threat map;
step S3, carrying out parameterization processing on the optimal path to obtain first path information;
step S4, optimizing the first path information to obtain second path information, wherein the second path information meets preset kinematic constraint conditions and obstacle avoidance requirements;
step S5, distributing an independent following path for each following slave in the cluster system according to the second path information, parameterizing the following paths, and then synchronizing in the cluster system through a communication data chain;
step S6, the pilot host computer is at constant speed VdFollowing the second path information, if communication is lost in the cluster system and the state information of the cluster system cannot be obtained, the following slave is also at the constant speed VdFollowing the path to maintain formation; if the communication in the cluster system is normal, the constant speed V isdBased on the speed of the slave, the coordinated speed V of the following slave is obtained by a speed coordination controllerr。
2. The method for controlling the formation of the dynamic formation of the cluster system according to claim 1, further comprising: step S7, the cluster system detects the environmental information in real time in the moving process through the carried sensor, and sets the radius of influence r for the obstacle after detecting a new obstacleiTo enter this influence radius riThe cluster single body is controlled to avoid the obstacle at the constant speed VdOr the coordinated speed VrOn the basis, the obstacle avoidance acceleration a is increasedi。
3. The method for controlling the formation of the dynamic formation of the cluster system according to claim 2, further comprising: and S8, in the process of formation, searching a new optimal path in a fixed period or a variable period according to the environment complexity and ground control input information, and repeating the steps S3-S7 according to the latest optimal path.
4. The method for controlling the formation of the dynamic formation of the cluster system according to claim 3, wherein the step S1 comprises:
dividing a square region from a starting point to an end point into n-n grids, wherein each grid is a node;
determining an impassable area based on the obstacle type, setting different threat coefficients for each grid according to the distance and direction information of the obstacle type, and performing superposition processing on each threat coefficient to obtain an n-x-n obstacle threat coefficient matrix.
5. The method for controlling the formation of a dynamic formation of a cluster system according to claim 4,
obtaining the optimal path by adopting a heuristic algorithm: taking a distance function which is superposed with the corresponding obstacle threat coefficients in the obstacle threat coefficient matrix and the terminal point as a heuristic value;
taking the length of a path from a starting point to the current position according to the minimum heuristic value path as a cost value;
taking the sum of the consumption value and the heuristic value as a cost value of each node;
and calculating cost values of nodes around the current node from the starting point, continuously searching from the minimum cost value points until reaching the target point, and connecting the minimum cost value points to obtain the optimal path.
6. The method for controlling the formation of the dynamic formation of the cluster system according to claim 5, wherein the first path information h (x) is calculated by using the following formula:
in the formula, xiAnd yiRespectively setting the x-axis coordinate and the y-axis coordinate of the ith original planning path point; q. q.siA derivative representing the original planned path point i; x is the number ofi+1And yi+1The x-axis coordinate and the y-axis coordinate of the (i + 1) th original planning path point are respectively; q. q.si+1The derivative of the i +1 th original planned path point is indicated.
7. The method for controlling the formation of the dynamic formation of the cluster system according to claim 6, wherein the step S4 comprises:
s4.1, adding the middle point of a straight line connecting line of the front original path point and the rear original path point of the next path point which is collided in the first path information;
s4.2, searching a new path again to obtain the second path information;
and S4.3, repeating the step S4.1 to the step S4.2 until the second path information meets the requirements of kinematic constraint and obstacle avoidance.
8. The method for controlling formation of dynamic formation of cluster system according to claim 7, wherein said step S5 comprises:
superposing coordinate offset on the basis of the second path information of the pilot host by taking the mass center of the pilot host as an origin, the tangential direction of the speed as an x axis and the normal direction as a y axis to obtain the parameterized following path of each following slave;
the coordinate offset dijThe following formula is adopted for representation:
dij＝(xoff_ij,yoff_ij,zoff_ij)
in the formula, xoff_ijRepresenting the x-coordinate vector, y, representing the space between the clustered singlets i to joff_ijRepresenting a vector of y coordinates, z, representing the number of clustered singlets i to joff_ijA z-coordinate vector representing the cluster between singlets i through j is shown.
9. The method for controlling formation of dynamic formation of cluster system according to claim 8, wherein said step S6 comprises: coordinated speed V of the following slaverCalculated by the following formula:
in the formula, | | Vd||∈(vmin+Δv,vmax-Δv)，f(x)＝2/(1+e(-x)) -1, f (x) e (-1,1), the final coordinated velocity satisfying Vr∈(vmin,vmax)，vminRepresenting a clusterMinimum movement speed of the single machine; v. ofmaxRepresenting the maximum movement speed of the cluster single machine; Δ v represents the magnitude of the adjustable velocity amount; e.g. of the type(-x)Represents the power of-x representing the natural index e.
10. The method for controlling formation of dynamic formation of cluster system according to claim 1, wherein said step S7 comprises: the obstacle avoidance acceleration aiCalculated by the following formula:
where b and c are control force coefficients, ρijThe distance between the cluster monomer i and the obstacle j; the obstacle avoidance acceleration aiIs directed to the cluster monomer i by the barrier point;
when the detected new obstacle has a movement speed, the obstacle avoidance acceleration a needs to be adjustediThe direction of (a);
the adjustment method is that the radius of influence r of the barrier is adjusted when the barrier entersiWhen the cluster monomer intersects the motion velocity vector of the obstacle:
if the following path of the cluster single body is at the right of the affected cluster single body, the obstacle-avoiding acceleration a is observed from the speed direction of the affected cluster single bodyiIs rotated 90 degrees counterclockwise;
observing from the speed direction of the affected cluster single body, if the following path of the cluster single body is at the left of the affected cluster single body, avoiding the obstacle acceleration aiIs rotated 90 degrees clockwise.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011307984.XA CN112650214B (en) | 2020-11-20 | 2020-11-20 | Formation control method for dynamic formation of cluster system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011307984.XA CN112650214B (en) | 2020-11-20 | 2020-11-20 | Formation control method for dynamic formation of cluster system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112650214A true CN112650214A (en) | 2021-04-13 |
| CN112650214B CN112650214B (en) | 2022-12-23 |
Family
ID=75349539
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011307984.XA Active CN112650214B (en) | 2020-11-20 | 2020-11-20 | Formation control method for dynamic formation of cluster system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112650214B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113848957A (en) * | 2021-11-17 | 2021-12-28 | 中国人民解放军陆军工程大学 | Ground unmanned vehicle formation control device and method |
| CN114020036A (en) * | 2021-12-03 | 2022-02-08 | 南京大学 | Anti-collision method for formation array transformation of multiple unmanned aerial vehicles |
| CN114061573A (en) * | 2021-11-16 | 2022-02-18 | 中国人民解放军陆军工程大学 | Ground unmanned vehicle formation positioning device and method |
| CN114384929A (en) * | 2021-12-02 | 2022-04-22 | 上海航天控制技术研究所 | Unmanned cluster formation control method based on deviation optimization heuristic algorithm |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101685309A (en) * | 2008-09-24 | 2010-03-31 | 中国科学院自动化研究所 | Method for controlling multi-robot coordinated formation |
| CN103941728A (en) * | 2014-04-24 | 2014-07-23 | 北京航空航天大学 | Formation changing method for intensive autonomous formations of unmanned aerial vehicle |
| US20170329348A1 (en) * | 2016-05-13 | 2017-11-16 | Macau University Of Science And Technology | System and method for operating a follower vehicle in a vehicle platoon |
| CN108594845A (en) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | More AUV formation methods based on PREDICTIVE CONTROL under a kind of communication limitation |
| CN108594846A (en) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | More AUV flight patterns optimal control methods under a kind of obstacle environment |
| CN108663939A (en) * | 2018-06-12 | 2018-10-16 | 哈尔滨工程大学 | Consider the UUV level of constellation face path tracking control method of communication packet loss |
| CN111399537A (en) * | 2020-03-27 | 2020-07-10 | 西北工业大学 | Distributed unmanned aerial vehicle dynamic formation switching method based on waypoints |
-
2020
- 2020-11-20 CN CN202011307984.XA patent/CN112650214B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101685309A (en) * | 2008-09-24 | 2010-03-31 | 中国科学院自动化研究所 | Method for controlling multi-robot coordinated formation |
| CN103941728A (en) * | 2014-04-24 | 2014-07-23 | 北京航空航天大学 | Formation changing method for intensive autonomous formations of unmanned aerial vehicle |
| US20170329348A1 (en) * | 2016-05-13 | 2017-11-16 | Macau University Of Science And Technology | System and method for operating a follower vehicle in a vehicle platoon |
| CN108594845A (en) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | More AUV formation methods based on PREDICTIVE CONTROL under a kind of communication limitation |
| CN108594846A (en) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | More AUV flight patterns optimal control methods under a kind of obstacle environment |
| CN108663939A (en) * | 2018-06-12 | 2018-10-16 | 哈尔滨工程大学 | Consider the UUV level of constellation face path tracking control method of communication packet loss |
| CN111399537A (en) * | 2020-03-27 | 2020-07-10 | 西北工业大学 | Distributed unmanned aerial vehicle dynamic formation switching method based on waypoints |
Non-Patent Citations (3)
| Title |
|---|
| QASIM ALI: "Explicit Model Following Distributed Control Scheme for Formation Flying of Mini UAVs", 《IEEE ACCESS》 * |
| 刘宪力: "基于领航通信的多机器人队形控制的算法设计", 《企业导报》 * |
| 彭皓月: "多无人机航迹规划的自适应B样条算法", 《计算机工程与应用》 * |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114061573A (en) * | 2021-11-16 | 2022-02-18 | 中国人民解放军陆军工程大学 | Ground unmanned vehicle formation positioning device and method |
| CN114061573B (en) * | 2021-11-16 | 2024-03-22 | 中国人民解放军陆军工程大学 | Ground unmanned vehicle formation positioning device and method |
| CN113848957A (en) * | 2021-11-17 | 2021-12-28 | 中国人民解放军陆军工程大学 | Ground unmanned vehicle formation control device and method |
| CN113848957B (en) * | 2021-11-17 | 2023-11-10 | 中国人民解放军陆军工程大学 | Ground unmanned vehicle formation control device and method |
| CN114384929A (en) * | 2021-12-02 | 2022-04-22 | 上海航天控制技术研究所 | Unmanned cluster formation control method based on deviation optimization heuristic algorithm |
| CN114384929B (en) * | 2021-12-02 | 2023-09-12 | 上海航天控制技术研究所 | Unmanned cluster formation control method based on deviation optimization heuristic algorithm |
| CN114020036A (en) * | 2021-12-03 | 2022-02-08 | 南京大学 | Anti-collision method for formation array transformation of multiple unmanned aerial vehicles |
| CN114020036B (en) * | 2021-12-03 | 2023-12-12 | 南京大学 | Anti-collision method for multi-unmanned aerial vehicle formation matrix transformation |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112650214B (en) | 2022-12-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112650214B (en) | Formation control method for dynamic formation of cluster system | |
| Harmon et al. | Sensor data fusion through a distributed blackboard | |
| US20170043771A1 (en) | Device for signalling objects to a navigation module of a vehicle equipped with this device | |
| US20180364657A1 (en) | Method and system for determining optimal coefficients of controllers for autonomous driving vehicles | |
| CN110440811B (en) | Universal autonomous navigation control method, device and equipment terminal | |
| CN110580740B (en) | Multi-agent cooperative three-dimensional modeling method and device | |
| CN110490809B (en) | Multi-agent cooperative positioning and mapping method and device | |
| CN111208829B (en) | Multi-mobile-robot formation method based on distributed preset time state observer | |
| WO2024012212A1 (en) | Environmental perception method, domain controller, storage medium, and vehicle | |
| CN112406905B (en) | Data processing method and device based on vehicle, computer and storage medium | |
| CN112731929A (en) | Ackerman model-based mobile robot obstacle avoidance path planning method | |
| Bai et al. | Cooperative trajectory optimization for unmanned aerial vehicles in a combat environment | |
| Lv et al. | A SLAM algorithm based on edge-cloud collaborative computing | |
| Chen et al. | Provably safe and robust drone routing via sequential path planning: A case study in san francisco and the bay area | |
| CN114815899A (en) | Unmanned aerial vehicle three-dimensional space path planning method based on 3D laser radar sensor | |
| Zhang et al. | A modified FastSLAM for an autonomous mobile robot | |
| Lacroix et al. | Integration of concurrent localization algorithms for a planetary rover | |
| CN112747752A (en) | Vehicle positioning method, device, equipment and storage medium based on laser odometer | |
| CN112506203A (en) | Robot motion dynamic feedback method and system | |
| Wang et al. | A dynamics controller design method for car-like mobile robot formation control | |
| Yang et al. | Gps and lidar fusion positioning based on unscented kalman filtering and long short-term memory cosidering gps failure | |
| Liu et al. | Multirobot collaborative navigation algorithms based on odometer/vision information fusion | |
| Ge et al. | Event-Triggered Robust H_ ∞ H∞ Control for Networked Flight Control Systems with Actuator Failures | |
| CN113701742B (en) | Mobile robot SLAM method based on cloud and edge fusion calculation | |
| AU2022268367B2 (en) | Three-dimensional space formation control method for heterogeneous unmanned system |
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 |