Adaptive unmanned vehicle formation reconstruction method and device based on obstacle distribution

Abstract

The present invention discloses a method and device for adaptively reconfiguring an unmanned vehicle formation based on obstacle distribution. The method includes: first, a width detection algorithm is performed on a pre-set unmanned vehicle as the leader in the formation, which can adaptively select the optimal formation based on the environment to reduce the risk of collision between the formation and obstacles; then, tasks are assigned to all unmanned vehicles based on the desired formation, shortening the total distance of the formation transformation and reselecting the leader unmanned vehicle based on the pre-set formation; then, a model predictive controller is designed based on consistency to enable multiple unmanned vehicles to quickly transform into the desired formation; finally, when the distance between the unmanned vehicle and the obstacle is less than a threshold, the unmanned vehicle will preferentially execute a local obstacle avoidance algorithm based on the potential field to ensure the safety of the unmanned vehicle. The present invention adaptively selects the most appropriate formation based on surrounding obstacles, which can significantly reduce the probability of collision during the formation navigation process.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned vehicle formation control, and in particular relates to a method and device for adaptively reconfiguring an unmanned vehicle formation based on obstacle distribution. Background Art

[0002] With the continuous development of multi-vehicle collaborative technology, compared with a single unmanned vehicle, multiple unmanned vehicles have demonstrated powerful task execution capabilities in fields such as unmanned driving and logistics. Multi-unmanned vehicle platooning is an important manifestation of multi-vehicle collaborative technology, maintaining a specific shape while performing tasks. In actual application scenarios, multi-unmanned vehicle platooning can reduce the distance between unmanned vehicles while ensuring no collisions, improve space utilization, and reduce energy consumption of unmanned vehicles. At the same time, the current unmanned ride-hailing trials in multiple cities and truck platooning have laid the foundation for the application of this invention.

[0003] A multi-AV platooning system involves several autonomous vehicles, designed through algorithms, forming a stable formation that meets specific requirements while also enabling the platoon to adapt to the environment and specific formation constraints. In real-world scenarios, the navigable area of ​​the surrounding environment is often variable, especially in narrow passages, where the existing formation can be distorted due to environmental constraints. Therefore, achieving multi-AV platooning navigation in constrained environments, designing appropriate formations, and efficiently changing formations are crucial for intelligent platooning systems. Summary of the Invention

[0004] The present invention provides an adaptive unmanned vehicle reconstruction method and device based on obstacle distribution, which includes maximum obstacle-free width detection and formation transformation, and is used to solve the problem of formation distortion caused by narrow channels.

[0005] To achieve the above objectives, a first aspect of the present invention relates to a method for adaptively reconfiguring an unmanned vehicle formation based on obstacle distribution, comprising the following steps:

[0006] S1. N Two unmanned vehicles equipped with depth cameras navigate in a narrow passage in formation; a kinematic model is established based on the status and speed of the unmanned vehicles and the mission endpoint is set;

[0007] S2. Based on the mission endpoint, each autonomous vehicle uses a path planning algorithm to generate an obstacle-free path and selects a local target point on the path based on the camera's field of view.

[0008] If the autonomous vehicle is very close to an obstacle, a potential field-based local obstacle avoidance algorithm is used to update the local target point, which serves as the reference state for the tracking controller to quickly move the vehicle away from the obstacle.

[0009] S4. If the distance between the unmanned vehicle and the obstacle is greater than the safe distance, the leading unmanned vehicle will calculate the maximum feasible width in real time based on the surrounding obstacles, including the following steps:

[0010] S41. Generate a rectangle enclosing the unmanned vehicle and the target point based on the widest formation width; obstacles within the rectangle are called local obstacles.

[0011] S42. Combining the position of the leading unmanned vehicle, the local target point, and the local obstacle information, construct a linear programming problem and solve it to obtain the maximum feasible width;

[0012] S43. Select a shape smaller than the maximum width as the default formation in the preset formation library;

[0013] S5. Due to the change in the default formation, the leader AV reallocates the positions of all AVs in the formation through task assignment and selects the AV at a specific position in the formation as the new leader AV.

[0014] S6. Each vehicle updates its local target point using average consistency based on the current desired formation and the local target point.

[0015] S7. Use the local target point as the reference state of the tracking controller to control the car to converge to the desired formation efficiently;

[0016] S8. Determine whether the unmanned vehicle has reached the target point. If so, the mission ends; if not, return to step S2.

[0017] The second aspect of the present invention relates to an adaptive unmanned vehicle formation reconstruction device based on obstacle distribution, including a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement the adaptive unmanned vehicle formation reconstruction method based on obstacle distribution of the present invention.

[0018] A third aspect of the present invention relates to a computer-readable storage medium having a program stored thereon, which, when executed by a processor, implements the adaptive unmanned vehicle formation reconstruction method based on obstacle distribution of the present invention.

[0019] This invention aims to solve the problem of collision-free formation navigation in complex environments with large obstacles. It first pre-sets a width detection algorithm for one unmanned vehicle in the formation as the lead vehicle. This algorithm then adaptively selects the optimal formation based on the environment, reducing the risk of collision between the formation and obstacles. It then assigns tasks to all unmanned vehicles based on the desired formation, shortening the total distance for formation transitions and reselecting the lead vehicle based on the pre-set formation. A model predictive controller is then designed based on consistency, enabling multiple unmanned vehicles to quickly transition to the desired formation. Finally, when the distance between the unmanned vehicle and an obstacle falls below a threshold, the vehicle prioritizes executing a potential field-based local obstacle avoidance algorithm to ensure its safety.

[0020] The beneficial effects brought about by the technical solution of the present invention are:

[0021] 1. The present invention can adaptively select the most suitable formation according to surrounding obstacles, which can greatly reduce the probability of collision during formation navigation.

[0022] 2. The present invention designs an efficient controller based on formation consistency, so that the unmanned vehicle can quickly switch formations.

[0023] 3. The present invention proposes a local obstacle avoidance algorithm based on potential field, which can further ensure that the unmanned vehicle avoids collision. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 1 is an overall flow chart of the method of the present invention.

[0025] Figure 2 It is a schematic diagram of a formation library of the present invention.

[0026] Figure 3 It is a schematic diagram of width detection of the present invention.

[0027] Figure 4 It is the actual effect diagram of the three-dimensional scene of the present invention.

[0028] Figure 5 Schematic diagram of the device of the present invention. DETAILED DESCRIPTION

[0029] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0030] Example 1

[0031] Combine Figure 1 The specific implementation process of the adaptive unmanned vehicle formation reconstruction method based on obstacle distribution described in the present invention is as follows:

[0032] S1. Suppose N unmanned vehicles are equipped with depth cameras and are navigating in formation in a complex environment. The destination of the specific navigation task is set up is the number of the unmanned vehicle, and N represents the number of unmanned vehicles in the formation. Based on the state of the unmanned vehicle (x, y, θ), including position and direction angle, and the velocity (v, w), including linear velocity and angular velocity, a kinematic model of the unmanned vehicle is established. The kinematic model of the unmanned vehicle can be expressed as:

[0033]

[0034] S2. According to the task end point Each unmanned vehicle uses the path planning algorithm A* to generate a path P without obstacles. According to the visual range of the depth camera carried by the unmanned vehicle, the unmanned vehicle selects a local target point p on the path P. lg Then, based on the intrinsic and extrinsic parameters of the depth camera, the obstacles observed by the unmanned vehicle are converted from pixels to three-dimensional coordinate points.

[0035] S3. If the autonomous vehicle is very close to the obstacle, less than the safety distance d oth , using the local obstacle avoidance algorithm based on potential field to update the local target point. The local obstacle avoidance algorithm constructs an unconstrained optimization problem, which is specifically expressed as follows:

[0036]

[0037] Among them, w o1 , w o2 Represents the weights of different parts in the objective function, p g，o is the variable to be optimized, d o (p g，o ) is the shortest distance between the autonomous vehicle and the obstacle. Solve the optimization problem so that the autonomous vehicle can find a new local target point p away from the obstacle near the path. g，o , as the reference state of the tracking controller, controls the car to quickly move away from obstacles. The first two dimensions are related to p g，o Same. Reference input u ir =(v ir , w ir ) is designed as follows:

[0038]

[0039] Among them, max , w max They represent the maximum linear velocity and angular velocity of the unmanned vehicle, p ie Equal to x ir -x ic The first two dimensions of the vector, x ic represents the current state of the i-th unmanned vehicle, x th is the distance threshold, The unit vector represents the current direction of the autonomous vehicle. A tracking controller is designed based on model predictive control (MPC). The nonlinear optimization problem is described as follows:

[0040]

[0041] where x i (t k+m|k ),u i (t k+m|k ), m=0,...,M-1 represent the number of autonomous vehicles i at time t k The predicted state and input, Represents the weight of each dimension of the state and input respectively. The first three constraints represent the kinematic model, maximum linear velocity and maximum angular velocity of the unmanned vehicle respectively. The last constraint indicates that the distance between unmanned vehicles must be greater than the safety distance d safe , and ε relaxes the last constraint, greatly improving the success rate of solving the entire constrained optimization problem. The optimization problem solves the control variable u0 of the unmanned vehicle, allowing the unmanned vehicle to quickly move away from obstacles.

[0042] S4. If the distance between the unmanned vehicle and the obstacle is greater than the safe distance, the leading unmanned vehicle will calculate the maximum feasible width in real time based on the surrounding obstacles, such as Figure 3 As shown, the following steps are included:

[0043] S41. According to the widest formation in the formation library, Figure 2 middle Generate a circle around the unmanned vehicle and the target point p lg The obstacle enclosed by the rectangle is called a local obstacle O. l .

[0044] S42. Combine the position of the leading unmanned vehicle, the local target point, and the local obstacle information to construct a linear programming problem and solve it to obtain the maximum feasible width. The above problem of solving the maximum feasible width is converted into a linear programming problem, which can be specifically expressed as follows:

[0045]

[0046] in, is the straight line l connecting the unmanned vehicle and the local target point r The coefficient of Figure 3 As shown, y1 and y2 represent the straight line l r The local obstacles on both sides are represented by O i1 , O l2 The last two constraints ensure that the maximum interval does not exceed the maximum width of the preset formation, w mf is an adjustable parameter. Figure 3 As shown, the final formation width is:

[0047]

[0048] S43. Select a shape smaller than the maximum width as the default formation in the preset formation library. Figure 2 As shown in the figure, taking 4 unmanned vehicles as an example, four different width formations are preset in the formation library. is smaller than the width w in step S42 c The widest formation.

[0049] S5. As the default formation changes, the leading unmanned vehicle reallocates the relative positions of all unmanned vehicles in the formation through task allocation, so that the total distance traveled by all unmanned vehicles is the shortest; at the same time, the unmanned vehicle at a specific position in the formation is selected as the new leading unmanned vehicle. Since the leading unmanned vehicle is responsible for width detection, the position of the leading unmanned vehicle should be the one with the best view in the formation. The present invention designs a mechanism: a fixed position in the formation is set as the leading unmanned vehicle, such as Figure 2 The R0 position of each formation in the system is determined by the leader vehicle. This allows the leader vehicle to change after each task assignment. Once the leader vehicle selects a suitable formation from the formation library, it shares it with the other vehicles via the wireless local area network.

[0050] S6. Each vehicle is based on the current desired formation and local target point p lg , update the local target point p using average consistency ig，f :

[0051]

[0052] in, represents the set of all unmanned vehicles except the i-th unmanned vehicle, N i Representing a collection The number of unmanned vehicles, p j，c represents the current position of the jth unmanned vehicle. The expected formation is expressed as in p f，ij =p f，i -p f，j Represents the relative coordinates of the i-th position and the j-th position in the desired formation.

[0053] S7. ig，f As the reference state x of the controller in step S3 ir The first two dimensions control the cars to converge efficiently to the desired formation.

[0054] S8. Determine whether the unmanned vehicle has reached the target point. If so, the task ends; if not, return to step S2. Finally, the above method is tested in actual scenes to verify the effect of the adaptive formation transformation proposed by the present invention in the presence of obstacles. The experimental platform uses Turtlebot4, equipped with an OAK-D depth camera, and a certain amount of reflective balls are attached to each vehicle. A rigid body can be established in the motion capture system OptiTrack to obtain the current posture of the unmanned vehicle in real time. Figure 4 As shown, four unmanned vehicles move from right to left, initially in a triangular formation. Because the narrow passage is not wide enough for the current formation to pass through, the leading unmanned vehicle detects the obstacle-free width in real time and chooses a narrower formation. This experiment chose a square formation. After all unmanned vehicles pass through the narrow passage, the obstacle-free area becomes larger, and the formation switches to a wider triangular formation.

[0055] Example 2

[0056] like Figure 5 This embodiment relates to an adaptive unmanned vehicle formation reconstruction device based on obstacle distribution, including a memory and one or more processors. The memory stores executable code. When the one or more processors execute the executable code, they are used to implement the adaptive unmanned vehicle formation reconstruction method based on obstacle distribution of Example 1.

[0057] Example 3

[0058] This embodiment relates to a computer-readable storage medium having a program stored thereon. When the program is executed by a processor, the adaptive unmanned vehicle formation reconstruction method based on obstacle distribution of Example 1 is implemented.

[0059] The various embodiments of the present invention are described in a progressive manner. Similar portions between the various embodiments can be referred to in conjunction with each other. Each embodiment focuses on the differences between the other embodiments. In particular, the system embodiment is generally similar to the method embodiment, so its description is relatively simple. For relevant portions, refer to the description of the method embodiment.

[0060] The foregoing is merely an embodiment of the present invention and is not intended to limit the present invention. It will be apparent to those skilled in the art that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention are intended to be included within the scope of the claims of the present invention.

Claims

1. An adaptive unmanned vehicle formation reconstruction method based on obstacle distribution, characterized by: The following steps are involved: S1. Two unmanned vehicles equipped with depth cameras navigate in a narrow passage in formation; a kinematic model is established based on the status and speed of the unmanned vehicles and the mission endpoint is set; S2. Based on the mission endpoint, each autonomous vehicle uses a path planning algorithm to generate an obstacle-free path and selects a local target point on the path based on the camera's field of view. If the autonomous vehicle is very close to an obstacle, a potential field-based local obstacle avoidance algorithm is used to update the local target point, which serves as the reference state for the tracking controller to quickly move the vehicle away from the obstacle. S4. If the distance between the unmanned vehicle and the obstacle is greater than the safe distance, the leading unmanned vehicle will calculate the maximum feasible width in real time based on the surrounding obstacles, including the following steps: S41. Generate a rectangle enclosing the unmanned vehicle and the target point based on the widest formation width; obstacles within the rectangle are called local obstacles. S42. Combining the position of the leading unmanned vehicle, the local target point, and the local obstacle information, construct a linear programming problem and solve it to obtain the maximum feasible width; S43. Select a shape smaller than the maximum width as the default formation in the preset formation library; S5. Due to the change in the default formation, the leader AV reallocates the relative positions of all AVs in the formation through task assignment and selects the AV at a specific position in the formation as the new leader AV. S6. Each vehicle updates its local target point using average consistency based on the current desired formation and the local target point. S7. Use the local target point as the reference state of the tracking controller to control the car to converge to the desired formation efficiently; S8. Determine whether the unmanned vehicle has reached the target point. If so, the mission ends; if not, return to step S2.

2. The adaptive unmanned vehicle formation reconstruction method based on obstacle distribution according to claim 1 is characterized in that: The kinematic model of the unmanned vehicle in step S1 is as follows: （1）。 3. The adaptive unmanned vehicle formation reconstruction method based on obstacle distribution according to claim 1 is characterized in that: In step S3, the local obstacle avoidance algorithm based on the potential field constructs an unconstrained optimization problem and updates the local target point away from the obstacle: （2） in, Represent the weights of different parts in the objective function, is the variable to be optimized, is the shortest distance between the autonomous vehicle and the obstacle; solving the optimization problem enables the autonomous vehicle to find a new local target point away from the obstacle near the road path .

4. The adaptive unmanned vehicle formation reconstruction method based on obstacle distribution according to claim 1 is characterized in that: In step S3, the input of the controller of the unmanned vehicle includes the reference position and the reference input; the reference position is the updated target point, and the reference input is the reference position of the updated target point. The design is as follows: （3） in, They represent the maximum linear velocity and angular velocity of the unmanned vehicle, equal The first two dimensions of the vector, Indicates the The current state of the unmanned vehicle, is the distance threshold, The unit vector representing the current direction of the autonomous vehicle.

5. The adaptive unmanned vehicle formation reconstruction method based on obstacle distribution according to claim 1 is characterized in that: In step S4, the linear programming problem of obtaining the feasible width is specifically expressed as follows: （4） in, is the straight line connecting the unmanned vehicle and the local target point The coefficient of are straight lines The local obstacles on both sides are represented as ; The last two constraints ensure that the maximum spacing does not exceed the maximum width of the preset formation , is an adjustable parameter.

6. The adaptive unmanned vehicle formation reconstruction method based on obstacle distribution according to claim 1 is characterized in that: In step S6, the average consistency is specifically expressed as follows: （5） in, Indicates that all unmanned vehicles except A collection of unmanned vehicles outside, Representing a collection The number of unmanned vehicles, Indicates the The current location of the autonomous vehicle; represents the local target point; the expected formation is expressed as ,in , Indicates the expected formation Position and The relative coordinates of the position.

7. The adaptive unmanned vehicle formation reconstruction device based on obstacle distribution is characterized by: The method comprises a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement the adaptive unmanned vehicle formation reconstruction method based on obstacle distribution according to any one of claims 1 to 6.

8. A computer-readable storage medium, characterized in that A program is stored thereon, and when the program is executed by a processor, the adaptive unmanned vehicle formation reconstruction method based on obstacle distribution described in any one of claims 1 to 6 is implemented.

Images