Multi-vehicle cooperative platoon control method, terminal and medium under dynamic obstacle environment

Abstract

The present invention provides a multi-vehicle cooperative formation control method in a dynamic obstacle environment, wherein the unmanned vehicle detects obstacle information and determines the formation distance between the unmanned vehicles; according to the obstacle information and the formation distance parameter, the local target point of the leader is determined by an optimization algorithm, and the local target point is tracked by a model predictive control method to obtain the motion trajectory and corresponding control amount of the leader in the prediction time domain; the motion trajectory and the formation distance parameter are passed to the follower, and the reference trajectory of the leader in the prediction time domain is calculated, and the reference trajectory is tracked by a model predictive control method to obtain the control amount in the prediction time domain; the control amount of the leader at the current moment is applied to the leader, and the control amount of the follower at the current moment is applied to the follower until the leader reaches the global target point. The present invention establishes dynamic obstacle avoidance constraints under the rolling optimization framework, and realizes the adaptability of the multi-unmanned vehicle system to the dynamic obstacle environment through the model predictive control method.

Description

Technical Field

[0001] The present invention relates to a formation control technology in the field of unmanned systems, and in particular to a multi-vehicle cooperative formation control method, terminal and medium in a dynamic obstacle environment. Background Art

[0002] Multi-vehicle formation control is one of the core research contents of multi-unmanned vehicle system collaboration. The task of formation control is to control multiple unmanned vehicles to maintain a certain formation while moving towards the target point, while ensuring that the unmanned vehicles do not collide with obstacles. In addition, it is necessary to consider the collision avoidance problem of unmanned vehicles and the switching and reconstruction of formations. Maintaining a certain formation of multiple vehicles can improve the efficiency of task execution and increase the robustness and reliability of the system. In various mission scenarios, there are often various obstacles in the environment, and unmanned vehicles are required to avoid obstacles while maintaining the formation. The obstacle environment currently studied is relatively simple, mostly static obstacle environments with known global information, but the actual mission scenarios are more complex, and global environmental information is often difficult to obtain and there are dynamic obstacles. For example, in military reconnaissance missions, the environmental information in the reconnaissance area is unknown, and there are dynamic obstacles such as enemy vehicles. In a dynamic obstacle environment, the unmanned system can only obtain limited local environmental information and the environmental information changes dynamically, and it is necessary to respond quickly to dynamic obstacles, which brings great challenges to the formation control of the unmanned system. Therefore, studying the problem of multi-vehicle collaborative formation control in a dynamic obstacle environment has important practical significance and application value.

[0003] The current formation control algorithm often uses artificial potential field method and optimization-based method to deal with static obstacle avoidance problems. The idea of ​​artificial potential field method is to abstract the environment into a virtual force field. Under the joint action of the gravitational field of the target point and the repulsive field of the obstacle, the unmanned vehicle avoids obstacles and reaches the target point. The optimization-based method is to establish an optimization problem based on the path planning problem of the unmanned vehicle, establish obstacle avoidance constraints for obstacles, and realize path planning and obstacle avoidance by solving the optimization problem. For dynamic obstacles, there is little work at present. The main obstacle avoidance algorithm is an improved algorithm based on static obstacle avoidance, such as introducing the speed and acceleration factors of the obstacle into the artificial potential field method to improve the algorithm's ability to handle dynamic obstacles. However, the improved artificial potential field algorithm has poor obstacle avoidance effect in a dynamic multi-obstacle environment. The kinematic model of the unmanned vehicle is not considered, and there are problems such as local extreme values ​​and unreachable target points. The dynamic obstacle avoidance algorithm based on optimization expands the obstacles according to the obstacle's moving speed information, and sets an avoidance area centered on the obstacle and larger than the obstacle. By avoiding this area, it avoids collision with dynamic obstacles. However, this expansion process increases the range of obstacle areas that the unmanned vehicle needs to avoid, and the passable area of ​​the unmanned vehicle is further compressed in an environment with dense obstacles. Therefore, this algorithm has poor adaptability to complex environments with dense obstacles.

[0004] Therefore, designing a multi-unmanned vehicle system cooperative formation control method that can effectively handle dynamic obstacle avoidance constraints and can combine the unmanned vehicle kinematic model to ensure the formation and control performance is of great significance for the realization of the multi-unmanned vehicle cooperative system. At present, no description or report of similar technology to the present invention has been found, and similar information at home and abroad has not been collected. Summary of the invention

[0005] In view of the above-mentioned deficiencies in the prior art, the present invention provides a multi-vehicle cooperative formation control method, terminal and medium in a dynamic obstacle environment.

[0006] According to one aspect of the present invention, a method for controlling a multi-vehicle cooperative formation in a dynamic obstacle environment is provided, comprising:

[0007] S1, all unmanned vehicles in the unmanned vehicle formation detect obstacle information around them and transmit it to the navigator, where the obstacle information includes obstacle position information and speed information. The navigator determines the formation distance between the unmanned vehicles based on the obstacle information;

[0008] S2, the navigator determines the local target point of the navigator through the optimization algorithm according to the obstacle information and the formation distance parameters; the optimization target design is as follows:

[0009]

[0010] Where:

[0011] t is the current time, is the coordinate of the local target point of the navigator in the Cartesian system, is the global target point coordinate, Q local is the weight matrix of the cost function in the local target point optimization problem of the navigator, a l,n , b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint;

[0012] After solving the optimization objective, a local target point that satisfies the formation obstacle avoidance constraint and is closest to the global target point within the leader's detection range is obtained;

[0013] S3, after the navigator obtains the local target point through S2, it uses the model predictive control method to track the local target point to obtain the control amount and corresponding motion trajectory of the navigator in the prediction time domain; the model predictive control method adopts an online rolling optimization algorithm, and the optimization target design in each control cycle is as follows:

[0014]

[0015] in,

[0016]

[0017]

[0018] Where:

[0019] is the pilot control quantity, a l,n 、b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the navigator in the Cartesian system, that is, the corresponding motion trajectory of the navigator, which contains the position and velocity information of the navigator at each moment in the prediction time domain. is the expected state of the navigator when it reaches the local target point, is the weight matrix in the cost function and penalty function;

[0020] After solving the optimization objective, the control amount and the corresponding motion trajectory of the navigator in the prediction time domain are obtained;

[0021] S4, after the leader obtains the motion trajectory in the prediction time domain through S3, it transmits the leader's motion trajectory and the formation distance parameter determined in S1 to the follower; the follower first calculates its own reference trajectory in the prediction time domain according to the leader's motion trajectory and the formation distance parameter, and then uses the model predictive control method to track the reference trajectory and obtain the control quantity in the prediction time domain; the model predictive control method adopts an optimization algorithm, and the optimization target is designed as follows:

[0022]

[0023] Among them, the expressions of cost function and penalty function are:

[0024]

[0025]

[0026] Where:

[0027] Control volume for followers, is the hyperplane parameter in the follower hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the follower in the Cartesian system, that is, the motion trajectory of the follower, is the reference trajectory of the follower, is the weight matrix in the cost function and penalty function;

[0028] After solving the optimization objective, the control amount of the follower in the prediction time domain is obtained;

[0029] S5, apply the control amount of the navigator at the current moment determined in S3 to the navigator, and apply the control amount of the follower at the current moment determined in S4 to the follower. If the navigator has not reached the global target point, iterate S1 to S5 until the navigator reaches the global target point.

[0030] The multi-vehicle cooperative formation control method in a dynamic obstacle environment provided by the present invention adopts a leader-follower formation strategy to solve the multi-vehicle formation problem in a dynamic obstacle environment. The leader collects all obstacle information detected by the unmanned vehicle and gives a planned path to guide the unmanned vehicle to avoid obstacles and approach the global target point. The follower is responsible for tracking the movement trajectory of the leader to form a formation. The leader determines the current formation parameters according to the density of obstacles, optimizes a local target point within the detection range according to the formation obstacle avoidance constraints, and performs path planning based on the model predictive control method to control the leader to approach the local target point, and use the local target point to guide the leader to avoid obstacles in advance. The follower calculates its own reference path based on the leader's path and formation parameters, and tracks the reference path through the model predictive control method to achieve overall formation control of multiple vehicles.

[0031] According to another aspect of the present invention, a terminal is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor may be used to execute any of the above-mentioned methods when executing the program.

[0032] According to a third aspect of the present invention, there is provided a computer-readable storage medium having a computer program stored thereon, which can be used to execute any of the above-mentioned methods when the program is executed by a processor.

[0033] Due to the adoption of the above technical solution, the present invention has any of the following beneficial effects compared with the prior art:

[0034] 1. The multi-vehicle collaborative formation control method, terminal and medium provided by the present invention in a dynamic obstacle environment adopt a model predictive control algorithm to deal with the formation control problem. Its rolling optimization feature can adapt to the dynamic constraints brought by dynamic obstacles and respond quickly to dynamic obstacles. It adopts a leading and following formation strategy and designs a formation parameter adjustment strategy, which can maintain the formation well in an environment with sparse obstacles. At the same time, in an environment with dense obstacles, the adaptability can be improved by adjusting the formation parameters.

[0035] 2. The multi-vehicle collaborative formation control method, terminal and medium in a dynamic obstacle environment provided by the present invention can adapt to the kinematic model and control quantity constraints of the unmanned vehicle, can plan a reasonable motion path for the unmanned vehicle and obtain the corresponding control quantity, and has excellent control performance.

[0036] 3. The multi-vehicle collaborative formation control method, terminal and medium in a dynamic obstacle environment provided by the present invention establish obstacle avoidance constraints for dynamic obstacles under the rolling optimization framework, and realize the adaptability of the multi-unmanned vehicle system to the dynamic obstacle environment through the model predictive control method. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0038] Figure 1 It is a flow chart of a multi-vehicle cooperative formation control method in a dynamic obstacle environment in one embodiment of the present invention.

[0039] Figure 2 It is a flow chart of a multi-vehicle cooperative formation control method in a dynamic obstacle environment in a preferred embodiment of the present invention.

[0040] Figure 3 It is a schematic diagram of the status of an unmanned vehicle at various times in a preferred embodiment of the present invention.

[0041] Figure 4 It is a distance diagram between the follower and the leader of the unmanned vehicle at each moment in a preferred embodiment of the present invention.

[0042] Figure 5 It is a control quantity diagram of the unmanned vehicle at each moment in a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0043] The following is a detailed description of the embodiments of the present invention: This embodiment is implemented on the premise of the technical solution of the present invention, and a detailed implementation method and a specific operation process are given. It should be pointed out that for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention.

[0044] Figure 1 This is a flow chart of a multi-vehicle cooperative formation control method in a dynamic obstacle environment in one embodiment of the present invention.

[0045] like Figure 1 As shown, the multi-vehicle cooperative formation control method in a dynamic obstacle environment provided by this embodiment may include the following steps:

[0046] S1, all unmanned vehicles in the unmanned vehicle formation detect obstacle information around them and transmit it to the navigator, where the obstacle information includes obstacle position information and speed information. The navigator determines the formation distance between the unmanned vehicles based on the obstacle information;

[0047] S2, the navigator determines the local target point of the navigator through the optimization algorithm according to the obstacle information and the formation distance parameters; the optimization target design is as follows:

[0048]

[0049] Where:

[0050] t is the current time, is the coordinate of the local target point of the navigator in the Cartesian system, is the global target point coordinate, Q local is the weight matrix of the cost function in the local target point optimization problem of the navigator, a l,n 、b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint;

[0051] After solving the optimization goal, obtain the local target point that satisfies the formation obstacle avoidance constraint and is closest to the global target point within the leader's detection range;

[0052] S3, after the navigator obtains the local target point through S2, it uses the model predictive control method to track the local target point to obtain the control amount and corresponding motion trajectory of the navigator in the prediction time domain; the model predictive control method adopts the online rolling optimization algorithm, and the optimization target design in each control cycle is as follows:

[0053]

[0054] in,

[0055]

[0056]

[0057] Where:

[0058] is the pilot control quantity, a l,n 、b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the navigator in the Cartesian system, that is, the corresponding motion trajectory of the navigator, which contains the position and velocity information of the navigator at each moment in the prediction time domain. is the expected state of the navigator when it reaches the local target point, is the weight matrix in the cost function and penalty function;

[0059] After solving the optimization target, the control amount and corresponding motion trajectory of the navigator in the prediction time domain are obtained;

[0060] S4, after the leader obtains the motion trajectory in the prediction time domain through S3, it passes the leader's motion trajectory and the formation distance parameters determined in S1 to the follower; the follower first calculates its own reference trajectory in the prediction time domain based on the leader's motion trajectory and the formation distance parameters, and then uses the model predictive control method to track the reference trajectory and obtain the control quantity in the prediction time domain; the model predictive control method adopts an optimization algorithm, and the optimization target is designed as follows:

[0061]

[0062] Among them, the expressions of cost function and penalty function are:

[0063]

[0064]

[0065] Where:

[0066] Control volume for followers, is the hyperplane parameter in the follower hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the follower in the Cartesian system, that is, the motion trajectory of the follower, is the reference trajectory of the follower, is the weight matrix in the cost function and penalty function;

[0067] After solving the optimization objective, the control amount of the follower in the prediction time domain is obtained;

[0068] S5, apply the control amount of the navigator at the current moment determined in S3 to the navigator, and apply the control amount of the follower at the current moment determined in S4 to the follower. If the navigator has not reached the global target point, iterate S1 to S5 until the navigator reaches the global target point.

[0069] The multi-vehicle cooperative formation control method in a dynamic obstacle environment provided by an embodiment of the present invention is implemented for the following formation control problems and based on the following principles.

[0070] The problem of multi-vehicle platooning cooperative control in a dynamic obstacle environment can be described as:

[0071] The position of the global target point is known, and there are obstacles with convex polygonal structures in the environment. The obstacles move at a constant speed or remain stationary. n unmanned vehicles need to start from the initial position, form a formation and reach the global target point, and avoid collision with obstacles during the process of moving forward. The unmanned vehicle can detect obstacle information within a certain range around itself. The detection window is a circular window with a radius of R, and each unmanned vehicle can communicate with each other.

[0072] Consider obstacles in an unknown environment that are all convex polygonal structures. The obstacles only move in a uniform straight line or remain stationary, and the movement speed does not exceed the maximum linear speed of the unmanned vehicle. The unmanned vehicle can detect obstacle information in a circular area with a radius of R around itself. Both the navigator and the follower need to detect obstacle point information around themselves, and the follower needs to provide the detected obstacle point information to the navigator.

[0073] Kinematic model of unmanned vehicle:

[0074] Consider a two-wheel differential drive unmanned vehicle. This type of unmanned vehicle is driven forward by two drive wheels installed on a common axle. These two drive wheels can move back and forth independently, thereby driving the unmanned vehicle to rotate.

[0075] Now define a generalized coordinate z i (t) is used to describe the state of the unmanned vehicle in the Cartesian system:

[0076]

[0077] Among them, x i (t) is the horizontal coordinate of the autonomous vehicle i in the Cartesian system at time t, y i (t) is the ordinate of the autonomous vehicle i in the Cartesian system at time t, θ i (t) is the heading angle of the unmanned vehicle i in the Cartesian system at time t. The state of the unmanned vehicle i at time t is z i (t), control quantity u i (t) is the linear velocity and angular velocity of unmanned vehicle i, and we have:

[0078]

[0079] Due to the limitations of the unmanned vehicle driving system, the linear velocity and angular velocity of the unmanned vehicle i must meet certain constraints:

[0080]

[0081]

[0082] in, is the maximum angular velocity and maximum linear velocity of unmanned vehicle i, s i The kinematic model of the unmanned vehicle can be obtained by constraining the control quantity of the unmanned vehicle i:

[0083]

[0084] The collaborative control method of multi-vehicle formation in a dynamic obstacle environment provided by an embodiment of the present invention is conceived as follows: adopting a leader-follower formation strategy, the leader collects obstacle information detected by all unmanned vehicles and gives a planned path to guide the unmanned vehicles to avoid obstacles and approach the global target point, and the follower is responsible for tracking the movement trajectory of the leader to form a formation. The leader determines the current formation parameters according to the density of obstacles, optimizes a local target point within the detection range according to the formation obstacle avoidance constraints, and performs path planning based on the model predictive control method to control the leader to approach the local target point, and use the local target point to guide the leader to avoid obstacles in advance. The follower calculates its own reference path based on the leader's path and formation parameters, and tracks the reference path through the model predictive control method to achieve overall formation control of multiple vehicles.

[0085] Leading and following formation strategy:

[0086] One of the unmanned vehicles in the formation is designated as the leader to move towards the target point, and the other unmanned vehicles are designated as followers to follow the leader. The formation parameter is the distance l between each follower and the leader. i And the relative angle By controlling i , The expected distance between follower i and the leader Angle relative to expectation Realize formation control, namely:

[0087]

[0088]

[0089] Based on the above planning problems and principles, a preferred embodiment of the present invention provides a multi-vehicle platooning cooperative control method in a dynamic obstacle environment, such as Figure 2 As shown, the specific technical solution steps are as follows.

[0090] S1: All unmanned vehicles collect obstacle point information and pass it to the navigator. All unmanned vehicles detect obstacle information around them and pass it to the navigator, where the obstacle information includes obstacle location information and speed information. The navigator determines the formation distance between unmanned vehicles based on the obstacle information.

[0091] S2: The navigator optimizes the local target point. The navigator determines the local target point of the navigator through the optimization algorithm based on the obstacle information and the formation distance parameters. The optimization target design is as follows:

[0092]

[0093] In the above formulas:

[0094] t is the current time, is the coordinate of the local target point of the navigator in the Cartesian system, is the global target point coordinate, Q local is the weight matrix of the cost function in the local target point optimization problem of the navigator, a l,n 、b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint.

[0095] This optimization problem is a quadratic programming problem and can be solved directly by the solver in MATLAB. After solving the optimization problem, the local target point that satisfies the formation obstacle avoidance constraint and is closest to the global target point within the detection range of the leader can be obtained.

[0096] S3: Navigator path planning. After the navigator obtains the local target point through S2, it uses the model predictive control method to track the local target point, obtains the control amount of the navigator in the prediction time domain, and calculates the corresponding motion trajectory based on the unmanned vehicle kinematic model. The model predictive control method is an optimization algorithm, and the optimization target is designed as follows:

[0097]

[0098] Among them, the expressions of cost function and penalty function are:

[0099]

[0100]

[0101] In the above formulas:

[0102] is the pilot control quantity, a l,n 、b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the navigator in the Cartesian system, that is, the corresponding motion trajectory of the navigator, which contains the position and velocity information of the navigator at each moment in the prediction time domain. is the expected state of the navigator when it reaches the local target point, is the weight matrix in the cost function and penalty function.

[0103] The optimization goal is to minimize the difference between the position of the navigator and the position of the local target point in the prediction time domain, so that the navigator can quickly approach the local target point. This optimization problem is a quadratic programming problem and can be solved directly by the MATLAB solver. Solving this optimization problem can obtain the control amount and corresponding motion trajectory of the navigator in the prediction time domain.

[0104] S4: Follower path tracking. After the leader obtains the motion trajectory in the prediction time domain through S3, it passes the leader's motion trajectory and the formation distance parameter determined in S1 to the follower. The follower first calculates its own reference trajectory in the prediction time domain based on the leader's motion trajectory and the formation distance parameter, and then uses the model predictive control method to track the reference trajectory and obtain the control amount in the prediction time domain. The model predictive control method is an optimization algorithm, and the optimization target is designed as follows:

[0105]

[0106] Among them, the expressions of cost function and penalty function are:

[0107]

[0108]

[0109] In the above formulas:

[0110] Control volume for followers, is the hyperplane parameter in the follower hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the follower in the Cartesian system, that is, the motion trajectory of the follower, is the reference trajectory of the follower, is the weight matrix in the cost function and penalty function.

[0111] The optimization goal is to minimize the difference between the follower trajectory and the reference trajectory in the prediction time domain, so that the follower can accurately track the reference trajectory and maintain the formation. This optimization problem is a quadratic programming problem and can be solved directly by the MATLAB solver. Solving this optimization problem can obtain the control amount of the follower in the prediction time domain.

[0112] S5: Determine whether the navigator has reached the global target point. Apply the control amount of the navigator at the current moment determined in S3 to the navigator, and apply the control amount of the follower at the current moment determined in S4 to the follower. If the navigator has not reached the global target point, iterate S1-S5 until the navigator reaches the global target point.

[0113] As a preferred embodiment, in S1, the unmanned vehicle detects obstacle information around itself, including:

[0114] The detection range is a circular window with the unmanned vehicle itself as the center, where the obstacle is a convex polygon structure; the obstacle position information is the obstacle point information, including the obstacle vertex within the detection range of the unmanned vehicle and the coordinates of the intersection of the obstacle and the detection window; the unmanned vehicle calculates the obstacle's movement speed information by comparing the obstacle point positions at previous and subsequent times: when the unmanned vehicle detects the obstacle vertex, the obstacle speed information is the obstacle vertex speed; when the unmanned vehicle only detects the obstacle edge, the obstacle speed information is the obstacle's movement speed in the direction perpendicular to the edge.

[0115] As a preferred embodiment, in S1, the method for determining the formation distance between unmanned vehicles includes:

[0116] The formation distance is the maximum distance that the unmanned vehicle formation passes between adjacent obstacles along the direction of the leader's speed. If a collision occurs within the unmanned vehicle formation at this formation distance, it means that the current obstacle distribution is too dense. The formation distance takes the set initial value, which is set by the user. It is necessary to ensure that the unmanned vehicles do not collide with each other.

[0117] As a preferred embodiment, in S2, formation obstacle avoidance constraints are adopted, including:

[0118]

[0119]

[0120]

[0121]

[0122]

[0123]

[0124]

[0125] ||a l,n (t)|| 2 ≤1

[0126] n≤N i (t)

[0127] Where:

[0128] t is the current time, is the coordinate of the local target point of the navigator in the Cartesian system, a l,n 、b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint, r safe The safe distance for unmanned vehicles, is the jth obstacle point among the nth obstacles detected by unmanned vehicle i, is the set of all obstacle points of the nth obstacle detected by unmanned vehicle i, N i (t) is the number of obstacles detected by unmanned vehicle i at time t, Obstacle point The speed at time t, T l For the prediction time domain;

[0129] The formation obstacle avoidance constraint constrains the current position of the unmanned vehicle, the local target point, and the corresponding formation position to one side of the hyperplane, and the obstacle point and the motion T l The obstacle point after time is constrained to the other side of the hyperplane, ensuring that the unmanned vehicle reaches the local target point from the current position while maintaining the formation and does not collide with dynamic obstacles in the predicted time domain.

[0130] As a preferred embodiment, the desired direction for the navigator to reach the local target point is the direction of the line from the navigator's current position to the local target point, which is used to guide the navigator to turn in advance.

[0131] As a preferred embodiment, in S3, a model predictive control method is adopted to optimize the motion trajectory of the navigator and the corresponding control amount in the prediction time domain in a rolling manner during the iterative process, and the control amount at the current moment is applied to the navigator.

[0132] As a preferred embodiment, in S3, a formation hyperplane obstacle avoidance constraint is adopted, including:

[0133] τ∈(t,t+T p ),

[0134]

[0135]

[0136]

[0137]

[0138]

[0139]

[0140] ||a l,n (τ|t)|| 2 ≤1,n≤N i (t);

[0141] Where:

[0142] is the pilot control quantity, a l,n, b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the navigator in the Cartesian system, where are the horizontal and vertical coordinates of the pilot Cartesian system, Towards the leader, is the jth obstacle point among the nth obstacles detected by unmanned vehicle i, is the set of all obstacle points of the nth obstacle detected by unmanned vehicle i, N i (t) is the number of obstacles detected by unmanned vehicle i at time t, Obstacle point The speed at time t, r safe The safe distance for unmanned vehicles, is the expected state of the navigator when it reaches the local target point:

[0143]

[0144] The formation hyperplane obstacle avoidance constraint constrains the position of the leader at each moment in the prediction to one side of the hyperplane, and constrains the obstacles at the corresponding moment to the other side of the hyperplane, ensuring that the leader will not collide with dynamic obstacles while tracking the local target point in the prediction time domain.

[0145] As a preferred embodiment, in S4, a model predictive control method is adopted to optimize the control amount in the prediction time domain in a rolling manner during the iterative process, and the control amount at the current moment is applied to the follower.

[0146] As a preferred embodiment, in S4, the follower calculates its own reference trajectory in the prediction time domain according to the leader's motion trajectory and the formation distance parameter, including:

[0147] According to the movement trajectory of the leader in the predicted time domain and the formation distance parameters, the calculation is as follows:

[0148]

[0149] Where: For followers i The reference trajectory of is the movement trajectory of the navigator in the prediction time domain, is the coordinate of the navigator’s motion trajectory, is the direction of the leader's trajectory, i.e. the follower's f i The direction of For followers i The formation distance parameters and formation angle parameters.

[0150] As a preferred embodiment, in S4, a follower hyperplane obstacle avoidance constraint is adopted, including:

[0151] τ∈(t,t+T p ),

[0152]

[0153]

[0154]

[0155]

[0156]

[0157]

[0158]

[0159] Where:

[0160] Control volume for followers, is the hyperplane parameter in the follower hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the follower in the Cartesian system, is the horizontal and vertical coordinates of the follower Cartesian system, Towards the followers, is the jth obstacle point among the nth obstacles detected by unmanned vehicle i, is the set of all obstacle points of the nth obstacle detected by unmanned vehicle i, N i (t) is the number of obstacles detected by unmanned vehicle i at time t, Obstacle point The speed at time t, r safe Safe distance for unmanned vehicles;

[0161] The follower hyperplane obstacle avoidance constraint constrains the follower position at each moment in the prediction to one side of the hyperplane, and constrains the obstacle at the corresponding moment to the other side of the hyperplane, ensuring that the follower will not collide with dynamic obstacles while tracking the reference trajectory in the prediction time domain.

[0162] The multi-vehicle cooperative formation control method in a dynamic obstacle environment provided by this preferred embodiment can form and maintain a formation in a dynamic obstacle environment, avoid dynamic obstacles and reach the global target point.

[0163] The technical solution provided by the embodiment of the present invention is further described below with reference to the accompanying drawings and through simulation examples.

[0164] See also Figure 2 The multi-vehicle cooperative formation control method in a dynamic obstacle environment of this preferred embodiment is based on rolling optimization. Through the steps of leader optimization of local target points, leader path planning, follower path tracking and iteration, a formation is formed and maintained in a dynamic obstacle environment, dynamic obstacles are avoided and the global target point is reached.

[0165] See also Figure 3 , consider a leader f l , two followers f 1 、f 2 , a total of three unmanned vehicle formation control problems. The two triangles in the figure are obstacles, of which the lower obstacle is a static obstacle and the upper obstacle is a dynamic obstacle. The small circle in the figure represents the safety radius of the unmanned vehicle, and the large circle represents the detection window radius of the corresponding unmanned vehicle. The larger five-pointed star in the upper right corner is the global target point, and the smaller five-pointed star is the local target point of the leader and the corresponding expected position of the follower.

[0166] The parameters are set as follows: Unmanned vehicle safety radius r safe =0.05m, detection window radius R = 0.65m. The linear velocity constraint of the navigator is The angular velocity constraint is The follower linear velocity constraint is The angular velocity constraint is The expected formation parameters are

[0167] The control cycle of the unmanned vehicle is 0.1s. In the local target point optimization problem of the navigator, the parameter matrix Q local =diag(60,60), the parameter λ in the range constraint of the target point selection is 0.7, and the predicted obstacle movement time is T l = 0.3s. Prediction time domain T of the local target point tracking problem of the navigator p = 0.3s, i.e. the next three control cycles. The parameter matrices are In the follower trajectory tracking problem, the prediction time domain is the same as the prediction time domain in the leader local target point tracking algorithm, T p =0.3s, the parameter matrices are

[0168] At t=0s, the coordinates of the three unmanned vehicles are It does not meet the formation requirements. There are two obstacles in the environment, the first obstacle is a static obstacle, and the coordinates of the obstacle points are (0.15, 0), (0.2, 0.2), (0.3, 0); the second obstacle is a dynamic obstacle, and the initial coordinates of the obstacle points are (0.25, 1.05), (0.35, 0.85), (0.45, 1.05). From t = 0s to t = 10s, the obstacle will move uniformly to the lower right at a speed of (0.07 / s, -0.035 / s). The unmanned vehicle needs to approach the global target point and adjust the formation while avoiding obstacles. The dynamic obstacle in the upper right is initially above the line connecting the unmanned vehicle and the global target point, and will gradually move below the line. The unmanned vehicle needs to avoid dynamic obstacles and reach the global target point while considering the obstacle motion information.

[0169] The following will illustrate the iterative process at time t = 7s. Figure 2 In this preferred embodiment, the multi-vehicle platoon collaborative control method under a dynamic obstacle environment includes the following steps:

[0170] S1, all unmanned vehicles detect obstacle information around themselves and transmit it to the navigator, where the obstacle information includes obstacle location information and speed information. The navigator determines the formation distance between the unmanned vehicles based on the obstacle information.

[0171] At t=7s, the obstacle point detected by the leader is the obstacle point of the second obstacle: (0.7330, 0.8085), (0.8330, 0.6085), (0.9330, 0.8085); the follower f 1 The obstacle points of the first obstacle detected are: (0.2, 0.2), (0.1894, 0.1575), (0.2276, 0.1447), and the obstacle points of the second obstacle are: (0.7330, 0.8085), (0.8330, 0.6085), (0.9330, 0.8085); follower f 2 The obstacle points of the first obstacle detected are: (0.2, 0.2), (0.1736, 0.0945), (0.2879, 0.0241), and the obstacle points of the second obstacle are: (0.7330, 0.8085), (0.8330, 0.6085), (0.9330, 0.8085).

[0172] Since both obstacles have detected the obstacle vertices, the obstacle vertex velocity is the obstacle velocity. Therefore, the obstacle velocity can be calculated based on the coordinates of the obstacle vertex in the current and previous sampling cycles:

[0173]

[0174] Among them, T is the detection cycle of the unmanned vehicle, is the obstacle point ε j,n (t) is the moving speed, is the speed of the nth obstacle. For the first obstacle, at t = 7s, the coordinates of the obstacle vertex are (0.2, 0.2), and at t = 6.9s, the coordinates of the obstacle vertex are (0.2, 0.2), so the speed of the first obstacle is 0. For the second obstacle, at t = 7s, the three obstacle points detected are all obstacle vertices. Take the first vertex, whose coordinates are (0.7330, 0.8085), and the coordinates of this vertex at t = 6.9s are (0.7260, 0.8120). According to the above formula, the speed of the second obstacle can be calculated as (0.07 / s, -0.035 / s).

[0175] At t=7s, the coordinates of the navigator are (0.5592, 0.7475, 0.7252). The navigator calculates the distance between obstacles along the navigator's forward direction. Since only the second obstacle is in front of the navigator, the relative distance from the obstacle point to the straight line in the navigator's forward direction can be calculated by the following formula:

[0176]

[0177] Among them, ε(t) is the coordinate of the obstacle point, p l (t) is the navigator coordinate, θ l is the direction of the navigator, ε(t)=(0.7330,0.8085), p l (t)=(0.5592,0.7475),θ l =0.7252. Substituting the data into the calculation, the relative distance is d ref = 0.0696, since there is no obstacle on the other side of the unmanned vehicle, the detection range of the unmanned vehicle is R = 0.65m, so the unmanned vehicle formation distance parameter should allow the unmanned vehicle to pass through the width d along the direction of the leader. ref +R=0.7196, since the expected formation distance parameter is This can meet the requirements, so the navigator determines the current formation distance parameter to be 0.18m.

[0178] S2, the navigator determines the local target point of the navigator through the optimization algorithm based on the obstacle information and the formation distance parameters. The optimization target design is as follows:

[0179]

[0180] So that,

[0181]

[0182]

[0183]

[0184]

[0185]

[0186]

[0187]

[0188] ||a l,n (t)|| 2 ≤1

[0189] n≤N i (t).

[0190] In the above formulas:

[0191] t is the current time, is the coordinate of the local target point of the navigator in the Cartesian system, is the global target point coordinate, Q local is the weight matrix of the cost function in the local target point optimization problem of the navigator, a l,n , b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint, R sight is the obstacle detection window radius of the unmanned vehicle, λ is the optimization range parameter of the local target point, r safe The safe distance for unmanned vehicles, is the jth obstacle point among the nth obstacles detected by unmanned vehicle i, is the set of all obstacle points of the nth obstacle detected by unmanned vehicle i, N i (t) is the number of obstacles detected by unmanned vehicle i at time t, Obstacle point The speed at time t, T l For the prediction time domain.

[0192] This optimization problem is a quadratic programming problem and can be solved directly by the solver in MATLAB. After solving this optimization problem, the local target point that satisfies the formation obstacle avoidance constraint and is closest to the global target point within the detection range of the leader can be obtained. Substituting the data into the solution, the coordinates of the local target point of the leader at time t = 7s are:

[0193] S3, after the navigator obtains the local target point, it uses the model predictive control method to track the local target point and obtain the motion trajectory and corresponding control quantity of the navigator in the prediction time domain. The model predictive control method is an optimization algorithm, and the optimization target is designed as follows:

[0194]

[0195] So that τ∈(t,t+T p ),

[0196]

[0197]

[0198]

[0199]

[0200]

[0201]

[0202] ||a l,n (τ|t)|| 2 ≤1,n≤N i (t).

[0203] Among them, the expressions of cost function and penalty function are:

[0204]

[0205]

[0206] In the above formulas:

[0207] is the pilot control quantity, a l,n 、b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the navigator in the Cartesian system, are the horizontal and vertical coordinates of the pilot Cartesian system, Towards the leader, is the jth obstacle point among the nth obstacles detected by unmanned vehicle i, is the set of all obstacle points of the nth obstacle detected by unmanned vehicle i, N i (t) is the number of obstacles detected by unmanned vehicle i at time t, Obstacle point The speed at time t, r safe The safe distance for unmanned vehicles, is the expected state of the navigator when it reaches the local target point, is the weight matrix in the cost function and penalty function.

[0208] The optimization goal is to minimize the difference between the position of the navigator and the position of the local target point in the prediction time domain, so that the navigator can quickly approach the local target point. This optimization problem is a quadratic programming problem and can be solved directly by the MATLAB solver. Solving this optimization problem can obtain the motion trajectory of the navigator in the prediction time domain and the corresponding control quantity. Substituting the data into the solution, the trajectory of the navigator in the prediction time domain can be obtained: (0.5690, 0.7561, 0.8752), (0.5773, 0.7661, 0.9468), (0.5849, 0.7767, 0.9469), and the corresponding control quantity: (0.1300, 1.5000), (0.1300, 0.7158), (0.1300, 0.0014).

[0209] S4, after the leader obtains the motion trajectory in the predicted time domain through S3, it passes the leader's motion trajectory and the formation distance parameter determined in S1 to the follower. The follower first calculates its own reference trajectory in the predicted time domain based on the leader's motion trajectory and the formation distance parameter:

[0210]

[0211] Where: For followers i The reference trajectory of is the movement trajectory of the navigator in the prediction time domain, is the coordinate of the navigator’s motion trajectory, is the direction of the leader's trajectory, i.e. the follower's f i The direction of For followers i The formation distance parameter and formation angle parameter of the follower can be calculated by substituting the data into 1 The reference trajectory is: (0.3874, 0.7081, 0.8238), (0.3934, 0.7146, 0.9265), (0.3976, 0.7201, 0.9469); the reference f 2 The reference trajectories are: (0.5482, 0.5705, 0.8329), (0.5590, 0.5823, 0.9506), (0.5683, 0.5954, 0.9469).

[0212] The follower then uses the model predictive control method to track the reference trajectory and obtain the control quantity in the prediction time domain. The model predictive control method is an optimization algorithm, and the optimization objective is designed as follows:

[0213]

[0214] So that τ∈(t,t+T p ),

[0215]

[0216]

[0217]

[0218]

[0219]

[0220]

[0221]

[0222] Among them, the expressions of cost function and penalty function are:

[0223]

[0224]

[0225] In the above formulas:

[0226] is the follower control quantity, is the hyperplane parameter in the follower hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the follower in the Cartesian system, is the horizontal and vertical coordinates of the follower Cartesian system, Towards the followers, is the jth obstacle point among the nth obstacles detected by unmanned vehicle i, is the set of all obstacle points of the nth obstacle detected by unmanned vehicle i, N i (t) is the number of obstacles detected by unmanned vehicle i at time t, Obstacle point The speed at time t, r safe The safe distance for unmanned vehicles, is the weight matrix in the cost function and penalty function.

[0227] The optimization goal is to minimize the difference between the follower trajectory and the reference trajectory in the prediction time domain, so that the follower can accurately track the reference trajectory and maintain the formation. This optimization problem is a quadratic programming problem and can be solved directly by the MATLAB solver. Solving this optimization problem can obtain the control amount of the follower in the prediction time domain. Substituting the data into the solution, we can get the follower f 1 Control amount in the prediction time domain: (-0.1600, -0.0861), (0.0877, 1.0269), (0.0698, 0.2039); follower f 2 The control amount in the prediction time domain: (0.0589, 2.0000), (0.1600, 1.1774), (0.1600, -0.0372).

[0228] S5, the control amount of the navigator at the current moment determined in S3 is applied to the navigator, and the control amount of the follower at the current moment determined in S4 is applied to the follower. After the control amount is applied, the coordinates of the navigator at t = 7.1s are: (0.5690, 0.7561, 0.8752); the follower f 1 The coordinates are: (0.3874, 0.7081, 0.8238); follower f 2 The coordinates are: (0.5482, 0.5705, 0.8329).

[0229] Since the navigator has not reached the global target point, iterate S1 to S5 until the navigator reaches the global target point.

[0230] The following is an introduction to this example from the overall process. Figure 3 At t = 0s, the initial position of the unmanned vehicle does not meet the formation requirements, and there is an obstacle on the right. The optimized local target point is located in the upper right corner of the unmanned vehicle, guiding the unmanned vehicle to avoid obstacles. The unmanned vehicle will gradually adjust its formation while tracking the local target point. At t = 2.5s, the unmanned vehicle has avoided the obstacle below, and the formation has been adjusted to be close to the formation formation. After detecting the obstacle, the optimized local target point is below the dynamic obstacle, guiding the unmanned vehicle to avoid obstacles from below the obstacle. At t = 4s, the unmanned vehicle moves toward the global target point and continuously adjusts its formation for dynamic obstacles. At t = 7s, the unmanned vehicle is close to the dynamic obstacle. The obstacle is below the line connecting the unmanned vehicle and the global target point. The optimized local target point is above the obstacle, guiding the unmanned vehicle to avoid obstacles from above the obstacle. At t = 8.5s, the unmanned vehicle has avoided the obstacle, and the local target point is on the line connecting the unmanned vehicle and the global target point, guiding the unmanned vehicle to approach the global target point. At t=16s, the unmanned vehicle maintained its formation and reached the global target point.

[0231] Figure 4Shows the distance between the follower and the leader at each moment, Indicates the expected formation distance between the follower and the leader. Since the initial position of the unmanned vehicle does not meet the formation requirements and there are obstacles near the initial position of the unmanned vehicle, the unmanned vehicle needs to adjust the formation while avoiding obstacles in the early stage. It can be seen that the distance between the follower and the leader gradually approaches the expected formation distance. From t = 4s to t = 8.5s, in order to avoid dynamic obstacles, the unmanned vehicle continuously adjusts the formation. After t = 8.5s, after the unmanned vehicle avoids the dynamic obstacles, it continuously adjusts the formation to the expected value while approaching the global target point.

[0232] Figure 5 The control amount of the unmanned vehicle at each moment in the process of multi-vehicle formation under dynamic obstacles is shown, where (a) is the linear speed of each vehicle at each moment, and (b) is the angular speed of each vehicle at each moment. It can be seen that the linear speed of the leader is basically within the upper limit of the linear speed constraint, maintaining high speed. Except for the initial adjustment of the formation, the followers also maintain a linear speed close to the upper limit of the constraint. The control amount of the leader and the follower meets the constraints throughout the process.

[0233] The multi-vehicle cooperative formation control method under a dynamic obstacle environment provided by the above embodiment of the present invention considers the multi-vehicle formation obstacle avoidance problem under a convex polygon obstacle environment with uniform motion, and adopts a formation strategy of pilot following and a model predictive control method to realize formation control. For the navigator, all unmanned vehicles collect obstacle information around themselves and transmit it to the navigator, and the navigator determines the formation distance between the unmanned vehicles according to the obstacle distribution; then optimizes the local target point within the navigator's environmental detection range to guide the formation to avoid obstacles in advance; after determining the local target point, the navigator uses the model predictive control algorithm to track the local target point to obtain the motion trajectory and corresponding control amount of the navigator in the prediction time domain. For the follower, the navigator transmits the current formation distance and the motion trajectory of the navigator in the prediction time domain to the follower; the follower calculates its own reference trajectory in the prediction time domain, and then tracks the reference trajectory through the model predictive control algorithm. The multi-vehicle cooperative formation control method under a dynamic obstacle environment provided by the above embodiment of the present invention realizes that multiple vehicles in a dynamic environment can maintain the formation formation as much as possible while avoiding obstacles through the formation strategy of pilot following and the control method based on model predictive control.

[0234] Another embodiment of the present invention provides a terminal, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it can be used to execute any one of the methods in the above embodiments of the present invention.

[0235] Optionally, the memory is used to store programs; the memory may include volatile memory (English: volatile memory), such as random-access memory (English: random-access memory, abbreviated: RAM), such as static random-access memory (English: static random-access memory, abbreviated: SRAM), double data rate synchronous dynamic random access memory (English: Double Data Rate Synchronous Dynamic Random Access Memory, abbreviated: DDR SDRAM), etc.; the memory may also include non-volatile memory (English: non-volatile memory), such as flash memory (English: flash memory). The memory is used to store computer programs (such as applications, functional modules, etc. that implement the above method), computer instructions, etc., and the above computer programs, computer instructions, etc. can be partitioned and stored in one or more memories. And the above computer programs, computer instructions, data, etc. can be called by the processor.

[0236] The above-mentioned computer programs, computer instructions, etc. may be stored in one or more memories in partitions, and the above-mentioned computer programs, computer instructions, data, etc. may be called by a processor.

[0237] The processor is used to execute the computer program stored in the memory to implement the various steps in the method involved in the above embodiment. For details, please refer to the relevant description in the above method embodiment.

[0238] The processor and the memory may be independent structures or integrated structures. When the processor and the memory are independent structures, the memory and the processor may be coupled and connected via a bus.

[0239] A third embodiment of the present invention provides a computer-readable storage medium having a computer program stored thereon. When the program is executed by a processor, the program can be used to execute any one of the methods in the above embodiments of the present invention.

[0240] The multi-vehicle cooperative formation control method, terminal and medium provided in the above embodiment of the present invention under a dynamic obstacle environment, through the steps of each unmanned vehicle collecting obstacle information, the navigator optimizing the local target point, the navigator path planning, the follower path tracking, iteration, etc., to form and maintain the formation in a dynamic obstacle environment, avoid dynamic obstacles and reach the global target point. The multi-vehicle cooperative formation control method under a dynamic obstacle environment provided by the above embodiment of the present invention, for the problem of multi-vehicle cooperative formation under a dynamic obstacle environment, adopts a formation strategy of pilot following, each unmanned vehicle collects and provides the environmental information around itself to the navigator, the navigator determines the formation parameters and optimizes the local target point, and guides the unmanned vehicle to avoid dynamic obstacles in advance through the local target point. After obtaining the local target point, the navigator performs path planning through the model predictive control algorithm, and transmits the formation parameters and the reference path to the follower. The follower can determine its own reference path based on the formation parameters and the reference path of the navigator, and track the reference path through the model predictive control algorithm to achieve the overall formation control of multiple vehicles. The above-mentioned embodiments of the present invention take into account the obstacle avoidance constraints of the unmanned vehicle, the kinematic model of the unmanned vehicle, and the control quantity constraints. The unmanned vehicle can form and maintain a formation, respond quickly to dynamic obstacles, and finally reach the global target point.

[0241] Those skilled in the art know that, in addition to implementing the system and its various devices provided by the present invention in a purely computer-readable program code, the system and its various devices provided by the present invention can be made to implement the same functions in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices provided by the present invention can be considered as a hardware component, and the devices for implementing various functions included therein can also be considered as structures within the hardware component; the devices for implementing various functions can also be considered as both software modules for implementing the method and structures within the hardware component.

[0242] The above are only preferred embodiments of the invention and are not intended to limit the scope of the invention. That is, any equivalent changes and modifications made according to the content of the patent application scope of the present invention should be within the technical scope of the present invention.

Claims

1. A multi-vehicle cooperative formation control method in a dynamic obstacle environment, characterized in that: include: S1, all unmanned vehicles in the unmanned vehicle formation detect obstacle information around them and transmit it to the navigator, where the obstacle information includes obstacle position information and speed information. The navigator determines the formation distance between the unmanned vehicles based on the obstacle information; S2, the navigator determines the local target point of the navigator through the optimization algorithm according to the obstacle information and the formation distance parameters; the optimization target design is as follows: Where: t is the current time, is the coordinate of the local target point of the navigator in the Cartesian system, is the global target point coordinate, Q local is the weight matrix of the cost function in the local target point optimization problem of the navigator, a l,n , b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint; After solving the optimization objective, a local target point that satisfies the formation obstacle avoidance constraint and is closest to the global target point within the leader's detection range is obtained; S3, after the navigator obtains the local target point through S2, it uses the model predictive control method to track the local target point to obtain the control amount and corresponding motion trajectory of the navigator in the prediction time domain; the model predictive control method adopts an online rolling optimization algorithm, and the optimization target design in each control cycle is as follows: in, Where: is the pilot control quantity, a l,n , b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the navigator in the Cartesian system, that is, the corresponding motion trajectory of the navigator, which contains the position and velocity information of the navigator at each moment in the prediction time domain. is the expected state of the navigator when it reaches the local target point, is the weight matrix in the cost function and penalty function; After solving the optimization objective, the control amount and the corresponding motion trajectory of the navigator in the prediction time domain are obtained; S4, after the leader obtains the motion trajectory in the prediction time domain through S3, it transmits the leader's motion trajectory and the formation distance parameter determined in S1 to the follower; the follower first calculates its own reference trajectory in the prediction time domain according to the leader's motion trajectory and the formation distance parameter, and then uses the model predictive control method to track the reference trajectory and obtain the control quantity in the prediction time domain; the model predictive control method adopts an optimization algorithm, and the optimization target is designed as follows: Among them, the expressions of cost function and penalty function are: Where: Control volume for followers, is the hyperplane parameter in the follower hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the follower in the Cartesian system, that is, the motion trajectory of the follower, is the reference trajectory of the follower, is the weight matrix in the cost function and penalty function; After solving the optimization objective, the control amount of the follower in the prediction time domain is obtained; S5, applying the control amount of the leader at the current moment determined in S3 to the leader, and applying the control amount of the follower at the current moment determined in S4 to the follower. If the leader has not reached the global target point, iterate S1 to S5 until the leader reaches the global target point; In S1, the unmanned vehicle detects obstacle information around itself, including: The detection range is a circular window with the unmanned vehicle itself as the center, wherein the obstacle is a convex polygon structure; the obstacle position information is obstacle point information, including the coordinates of the obstacle vertex within the detection range of the unmanned vehicle and the intersection of the obstacle and the detection window; the unmanned vehicle calculates the movement speed information of the obstacle by comparing the obstacle point positions at previous and subsequent moments: when the unmanned vehicle detects the obstacle vertex, the obstacle speed information is the obstacle vertex speed; when the unmanned vehicle detects only the obstacle edge, the obstacle speed information is the movement speed of the obstacle in a direction perpendicular to the edge; and / or In S1, the method for determining the formation distance between unmanned vehicles includes: The formation distance is the maximum distance that the unmanned vehicle formation passes between adjacent obstacles along the direction of the leader's speed. If a collision occurs within the unmanned vehicle formation at this formation distance, it means that the current obstacle distribution is too dense, and the formation distance takes the initial value set by the user. In S2, formation obstacle avoidance constraints are adopted, including: ||a l,n (t)||2≤1 n≤N i (t); Where: t is the current time, is the coordinate of the local target point of the navigator in the Cartesian system, a l,n , b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint, r safe The safe distance for unmanned vehicles, is the jth obstacle point among the nth obstacles detected by unmanned vehicle i, is the set of all obstacle points of the nth obstacle detected by unmanned vehicle i, N i (t) is the number of obstacles detected by unmanned vehicle i at time t, Obstacle point The speed at time t, T l For the prediction time domain; The formation obstacle avoidance constraint constrains the current position of the unmanned vehicle, the local target point and the corresponding formation position to one side of the hyperplane, and the obstacle point and the motion T l The obstacle point after time is constrained to the other side of the hyperplane, ensuring that the unmanned vehicle reaches the local target point from the current position while maintaining the formation and does not collide with dynamic obstacles in the prediction time domain; In S3, a formation hyperplane obstacle avoidance constraint is adopted, including: τ∈(t,t+T p ), ||a l,n (τ|t)||2≤1,n≤N i (t); Where: is the pilot control quantity, a l,n , b l,n is the hyperplane parameter in the formation hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the navigator in the Cartesian system, where are the horizontal and vertical coordinates of the pilot Cartesian system, Towards the leader, is the jth obstacle point among the nth obstacles detected by unmanned vehicle i, is the set of all obstacle points of the nth obstacle detected by unmanned vehicle i, N i (t) is the number of obstacles detected by unmanned vehicle i at time t, Obstacle point The speed at time t, r safe The safe distance for unmanned vehicles, is the expected state of the navigator when it reaches the local target point: The formation hyperplane obstacle avoidance constraint constrains the position of the navigator at each moment in the prediction to one side of the hyperplane, and constrains the obstacles at the corresponding moment to the other side of the hyperplane, ensuring that the navigator will not collide with dynamic obstacles during the process of tracking the local target point in the prediction time domain; In S4, a model predictive control method is used to optimize the control amount in the prediction time domain in an iterative process, and the control amount at the current moment is applied to the follower; and / or In S4, the follower calculates its own reference trajectory in the prediction time domain according to the leader's motion trajectory and the formation distance parameter, including: According to the movement trajectory of the leader in the predicted time domain and the formation distance parameters, the calculation is as follows: Where: For followers i The reference trajectory of is the movement trajectory of the navigator in the prediction time domain, is the coordinate of the navigator’s motion trajectory, is the direction of the leader's trajectory, i.e. the follower's f i The direction of For followers i The formation distance parameters and formation angle parameters; In S4, a follower hyperplane obstacle avoidance constraint is adopted, including: τ∈(t,t+T p ), Where: Control volume for followers, is the hyperplane parameter in the follower hyperplane obstacle avoidance constraint, T p For the prediction time domain, is the generalized coordinate of the follower in the Cartesian system, is the horizontal and vertical coordinates of the follower Cartesian system, Towards the followers, is the jth obstacle point among the nth obstacles detected by unmanned vehicle i, is the set of all obstacle points of the nth obstacle detected by unmanned vehicle i, N i (t) is the number of obstacles detected by unmanned vehicle i at time t, Obstacle point The speed at time t, r safe Safe distance for unmanned vehicles; The follower hyperplane obstacle avoidance constraint constrains the follower position at each moment in the prediction to one side of the hyperplane, and constrains the obstacle at the corresponding moment to the other side of the hyperplane, ensuring that the follower will not collide with dynamic obstacles while tracking the reference trajectory in the prediction time domain.

2. The multi-vehicle cooperative formation control method in a dynamic obstacle environment according to claim 1, characterized in that: The expected direction for the navigator to reach the local target point is the direction of the line from the navigator's current position to the local target point, which is used to guide the navigator to turn in advance.

3. The multi-vehicle cooperative formation control method in a dynamic obstacle environment according to claim 1, characterized in that: In S3, a model predictive control method is used to optimize the motion trajectory of the navigator and the corresponding control amount in the prediction time domain in an iterative process, and the control amount at the current moment is applied to the navigator.

4. A terminal comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, it can be used to perform the method according to any one of claims 1 to 3.

5. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, it can be used to execute the method according to any one of claims 1 to 3.

Images