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

Abstract

The invention discloses a self-adaptive unmanned vehicle formation reconstruction method and device based on obstacle distribution, and the method comprises the steps: firstly presetting one unmanned vehicle in a formation as a leading unmanned vehicle to carry out a width detection algorithm, and adaptively selecting an optimal formation according to the environment, thereby reducing the collision risk between the formation and an obstacle; task allocation is carried out on all the unmanned vehicles based on the expected formation, the total distance of formation transformation is shortened, and the leader unmanned vehicle is reselected according to the preset formation; then, a model prediction controller is designed based on consistency, so that the multiple unmanned vehicles can be quickly converted to an expected formation; and finally, when the distance between the unmanned vehicle and the obstacle is smaller than a threshold value, the unmanned vehicle preferentially executes a potential field-based local obstacle avoidance algorithm to ensure the safety of the unmanned vehicle. According to the invention, the most suitable formation is adaptively selected according to the surrounding obstacles, and the collision probability in the formation navigation process can be greatly reduced.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned vehicle formation control, and in particular relates to an adaptive unmanned vehicle formation reconstruction method and device 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 the fields of unmanned driving and logistics. The multi-unmanned vehicle formation is an important manifestation of multi-vehicle collaborative technology, maintaining a specific shape in the process of performing tasks. In actual application scenarios, the multi-unmanned vehicle formation can reduce the distance between unmanned workshops while ensuring no collisions between each other, improve space utilization, and reduce the energy consumption of unmanned vehicles. At the same time, the unmanned online car-hailing trials in many cities and truck formations have laid the foundation for the application of this invention.

[0003] A multi-unmanned vehicle platoon system refers to a group of unmanned vehicles that can form a stable formation that meets the needs through algorithm design, while enabling the fleet to adapt to the environment and the constraints of a specific formation. In actual scenarios, the traversable area of ​​the surrounding environment is often variable, especially in narrow passages, where the original formation is distorted due to environmental constraints. Therefore, it is of great significance for the intelligent vehicle platoon system to realize multi-unmanned vehicle formation navigation in a restricted environment, design a suitable formation, and efficiently transform the formation. 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] In order to achieve the above object, a first aspect of the present invention relates to an adaptive unmanned vehicle formation reconstruction method based on obstacle distribution, comprising the following steps:

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

[0007] S2. Based on the mission destination, each unmanned vehicle generates an obstacle-free path using a path planning algorithm; local target points are selected on the path based on the camera's field of view;

[0008] S3. If the unmanned vehicle is very close to an obstacle, the local obstacle avoidance algorithm based on the potential field is used to update the local target point as the reference state of the tracking controller to control the vehicle to quickly move 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 solve the maximum feasible width in real time based on the surrounding obstacles, including the following steps:

[0010] S41. Generate a rectangle surrounding the unmanned vehicle and the target point according to the widest formation width; obstacles within the rectangle are called local obstacles;

[0011] 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;

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

[0013] S5. As the default formation changes, the leading unmanned vehicle reallocates the positions of all unmanned vehicles in the formation through task allocation, and selects the unmanned vehicle at a specific position in the formation as the new leading unmanned vehicle;

[0014] S6. Each vehicle updates the local target point using average consistency according to 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, comprising 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] The present invention aims to solve the problem of collision-free formation navigation in a complex environment containing large obstacles. The present invention first presets an unmanned vehicle in the formation as the leading unmanned vehicle to perform a width detection algorithm, and can select the optimal formation according to the environment adaptively to reduce the risk of collision between the formation and the obstacle; then assign tasks to all unmanned vehicles based on the expected formation, shorten the total distance of the formation change and reselect the leading unmanned vehicle according to the preset formation; then design a model predictive controller based on consistency so that multiple unmanned vehicles can quickly change to the expected formation; finally, when the distance between the unmanned vehicle and the obstacle is less than the threshold, the unmanned vehicle will give priority to executing the local obstacle avoidance algorithm based on the potential field to ensure the safety of the unmanned vehicle.

[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 The figure is an overall flow chart of the method of the present invention.

[0025] Figure 2 It is a schematic diagram of the 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 It is a schematic diagram of the device of the present invention. DETAILED DESCRIPTION

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

[0030] Example 1

[0031] Combination 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. According to the state of the unmanned vehicle (x, y, θ), including position and direction angle, and the speed (v, w), including linear speed and angular speed, the 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 unmanned 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 expressed in the following form:

[0036]

[0037] Among them, w o1 , w o2 Represent 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 unmanned vehicle and the obstacle. Solve the optimization problem so that the unmanned vehicle can find a new local target point p away from obstacles 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, respectively, and 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 unmanned vehicle. The tracking controller is designed based on the 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 amount u0 of the unmanned vehicle, making the unmanned vehicle 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 solve 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 surrounding unmanned vehicle and 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 is 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 unmanned vehicle. This makes it possible for the leader unmanned vehicle to change after each task assignment. And when the leader unmanned vehicle selects a suitable formation from the formation library, it is shared with other unmanned vehicles through the wireless LAN.

[0050] S6. Each vehicle is based on the current desired formation and local target point p lg , using average consistency to update the local target point p 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 to the desired formation efficiently.

[0054] S8. Determine whether the unmanned vehicle has reached the target point. If so, the task is completed; if not, return to step S2. Finally, the above method is tested in actual scenes to verify the effect of the adaptive formation change in the presence of obstacles proposed by the present invention. 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, and are 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 the unmanned vehicles have passed 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, 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 Example 1.

[0057] Example 3

[0058] This embodiment relates to a computer-readable storage medium on which a program is stored. 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] Each embodiment of the present invention is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0060] The above description is only an embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent substitution, improvement, etc. made within the spirit and principle of the present invention should be included in the scope of the claims of the present invention.

Claims

1. An adaptive unmanned vehicle formation reconstruction method based on obstacle distribution, characterized in that: The following steps are involved: S1. Assume that N unmanned vehicles are equipped with depth cameras and navigate in a narrow channel. According to the state and speed of the unmanned vehicles, a kinematic model is established and the task endpoint is set. S2. Based on the mission destination, each unmanned vehicle generates a path without obstacles using a path planning algorithm; local target points are selected on the path based on the camera's field of view; S3. If the unmanned vehicle is very close to an obstacle, the local obstacle avoidance algorithm based on the potential field is used to update the local target point as the reference state of the tracking controller to control the vehicle to quickly move 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 solve the maximum feasible width in real time based on the surrounding obstacles, including the following steps: S41. Generate a rectangle surrounding the unmanned vehicle and the target point according to the widest formation width; obstacles within the rectangle are called local obstacles; 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; S43. Select a shape smaller than the maximum width as the default formation in the preset formation library; S5. As the default formation changes, the leading unmanned vehicle reallocates the relative positions of all unmanned vehicles in the formation through task allocation, and selects the unmanned vehicle at a specific position in the formation as the new leading unmanned vehicle; S6. Each vehicle updates the local target point using average consistency according to 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:

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: Among them, w o1 , w o2 Represent 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 unmanned vehicle and the obstacle; solve the optimization problem so that the unmanned vehicle can find a new local target point p near the road path away from the obstacle. g，o .

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 a reference position and a reference input; the reference position is the updated target point, and the reference input u ir =(v ir , w ir ) is designed as follows: Among them, max , w max They represent the maximum linear velocity and angular velocity of the unmanned vehicle, respectively, and 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 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: in, is the straight line l connecting the unmanned vehicle and the local target point r The coefficients of y1 and y2 represent the straight line l r The local obstacles on both sides are represented by O l1 , O l2 ; The last two constraints ensure that the maximum interval does not exceed the maximum width of the preset formation ω mf , 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: 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; p ig，f represents the local target point; 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.

7. An adaptive unmanned vehicle formation reconstruction device based on obstacle distribution, characterized in that: It includes a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, it is used to implement the adaptive unmanned vehicle formation reconstruction method based on obstacle distribution as described in any one of claims 1-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