Unmanned aerial vehicle dynamic environment real-time obstacle avoidance method and device based on visual auxiliary B-spline trajectory optimization

Abstract

The invention discloses an unmanned aerial vehicle dynamic environment real-time obstacle avoidance method and device based on visual auxiliary B-spline trajectory optimization, and particularly relates to the field of unmanned aerial vehicle trajectory optimization. Comprising the steps of obtaining a depth image of a flight scene, and establishing a voxel map based on the depth image; recognizing an obstacle in the depth image, and determining the position of the obstacle; generating an initial trajectory of the unmanned aerial vehicle, parameterizing the initial trajectory, and determining a control point of the initial trajectory; taking the intersection part of the initial track and the obstacle as a collision section, and forming a collision set by all control points in the collision section; finding a replacement path in the voxel map by using A *; searching a path point corresponding to each control point in the collision set in the replacement path for replacement to obtain an initial obstacle avoidance trajectory; and optimizing the trajectory by adopting an optimization algorithm to obtain an obstacle avoidance trajectory. The problem that in the prior art, the requirement for computing resources is high is solved.

Description

Technical Field

[0001] The present application relates to the field of unmanned aerial vehicle (UAV) trajectory optimization, and in particular to a real-time obstacle avoidance method and device for unmanned aerial vehicle (UAV) in a dynamic environment based on vision-assisted B-spline trajectory optimization. Background Art

[0002] Autonomous navigation and obstacle avoidance of unmanned aerial vehicles (UAVs) in dynamic environments have received extensive attention in recent years. With the increasing application of UAVs in various complex environments, such as exploration, rescue, and power inspection, the autonomous capabilities of UAVs in these tasks have become particularly critical. However, most current path planning algorithms rely mainly on a single map representation, such as geometric maps, occupancy maps, or Euclidean signed distance field (ESDF) maps. These methods perform well in static environments, but due to the limitations of the map itself, they are insufficient in dealing with both static and dynamic obstacles and cannot ensure the flight safety of UAVs.

[0003] In recent years, researchers have proposed a variety of improved path planning and obstacle avoidance algorithms. For example, model predictive control (MPC) is an advanced control strategy that optimizes the current control input by predicting the system's behavior over a period of time in the future. Its disadvantages are that the MPC method needs to solve an optimization problem at each time step, and the scale of the optimization problem increases with the complexity of the system and the increase in constraints; MPC relies on the dynamic model of the system for prediction, and the accuracy of the model directly affects the obstacle avoidance effect. If the model is inaccurate, it may lead to prediction errors, which in turn affect the drone's path planning and obstacle avoidance decisions; its effect depends on the accurate prediction of obstacle motion, and in highly dynamic and unpredictable environments, the performance of MPC may be limited.

[0004] Multi-level planning and two-level planning are to deal with path planning problems in layers so that drones can effectively avoid obstacles in complex environments. The solution is usually divided into a global path planning layer and a local obstacle avoidance layer. The global path planning layer is responsible for generating the overall route from the starting point to the target point, mainly dealing with static obstacles. It uses algorithms such as A* and Dijkstra based on the global map to generate a path that avoids obstacles. Since the position of static obstacles is fixed, the planning results are relatively stable, providing a reference path for the flight of the drone. The local obstacle avoidance layer deals with dynamic obstacles (such as moving vehicles, pedestrians, etc.). Real-time environmental information is obtained through sensors, local maps are dynamically constructed, and the movement trajectory of obstacles is predicted in real time. The local obstacle avoidance layer uses algorithms such as the velocity obstacle method (VO) to calculate a safe path to ensure that the drone can avoid dynamic obstacles. The two layers work together: the global path provides macro directions, and the local obstacle avoidance handles emergencies during flight. When the local obstacle avoidance is adjusted multiple times and deviates from the global path, the global path planner may recalculate the path. However, this method requires the drone to run the global path planner and the local obstacle avoidance device at the same time, which requires high computing resources; and efficient communication and data synchronization need to be maintained between the global path planning layer and the local obstacle avoidance layer to ensure that the information transmission between the planners is timely and accurate. Summary of the invention

[0005] The main purpose of this application is to provide a real-time obstacle avoidance method and device for UAV in dynamic environment based on vision-assisted B-spline trajectory optimization, aiming to solve the problem of high computing resource requirements in the prior art.

[0006] To achieve the above-mentioned purpose, the present application provides a real-time obstacle avoidance method for a UAV in a dynamic environment based on vision-assisted B-spline trajectory optimization, comprising: obtaining a depth image of a flight scene, and establishing a voxel map based on the depth image; identifying obstacles in the depth image and determining the positions of the obstacles; generating an initial trajectory of the UAV, and parameterizing the initial trajectory using a B-spline function to determine the control points of the initial trajectory; taking the portion where the initial trajectory intersects with the obstacle as a collision segment, and forming a collision set from all control points in the collision segment; finding a replacement path in the voxel map using A*; finding the path point corresponding to each control point in the collision set in the replacement path and replacing them to obtain an initial obstacle avoidance trajectory; taking the minimization of the total cost function as the objective function and the safety distance as a constraint condition, and optimizing the trajectory using an optimization algorithm to obtain an obstacle avoidance trajectory.

[0007] Optionally, obstacles in the depth image are identified and the positions of the obstacles are determined, including: identifying a bounding box of the obstacle in the depth image, and determining a potential obstacle area based on the bounding box of the obstacle; performing a depth continuity check on the potential obstacle area to obtain depth data of the potential obstacle; determining 3D information of the potential obstacle based on the depth data of the potential obstacle and the bounding box of the obstacle in the depth image; and consulting a voxel map based on the 3D size of the potential obstacle to determine accurate 3D information of the potential obstacle.

[0008] Optionally, the obstacle includes a static obstacle and a dynamic obstacle. After determining the position of the obstacle, the method further includes: estimating the state of the obstacle according to the position of the obstacle to obtain the current speed of the obstacle; and removing the static obstacle based on the current speed of the obstacle and a preset speed threshold to obtain the dynamic obstacle.

[0009] Optionally, the total cost function is calculated as:

[0010] in, is the security cost function, is the smoothness cost function, is the dynamic obstacle cost function.

[0011] Optionally, the security cost function is:

[0012]

[0013]

[0014] In the formula, is the time step, The value range is , V i , A i are the velocity and acceleration of the control point, is the two-norm, is the unit normalization factor.

[0015] Optionally, the smoothness cost function is:

[0016] .

[0017] Optionally, the dynamic obstacle cost function is:

[0018] in, is a safe distance, when the collision control point When in the active area of ​​​​dynamic obstacles,

[0019] Where D is the collision control point The intersection of the vertical line with the boundary line of the dynamic obstacle's active area, F is the current position of the dynamic obstacle; Take the current position O of the obstacle as the center and obtain the circular area with a preset radius r. When it is within the circular area, .

[0020] Optionally, the method for obtaining the activity area of ​​a dynamic obstacle includes: using a linear prediction model to predict the position of the dynamic obstacle in multiple future time steps based on the current speed and position of the dynamic obstacle, obtaining multiple prediction points, and connecting the current position of the dynamic obstacle and the last prediction point to form the activity area of ​​the dynamic obstacle.

[0021] To achieve the above-mentioned purpose, the present application also provides a real-time obstacle avoidance device for a UAV in a dynamic environment based on visual-assisted B-spline trajectory optimization, including: an image acquisition module, used to acquire a depth image of a flight scene and establish a voxel map based on the depth image; an obstacle recognition module, used to recognize obstacles in the depth image and determine the position of the obstacles; a trajectory generation module, used to generate an initial trajectory of the UAV, and parameterize the initial trajectory using a B-spline function to determine the control point of the B-spline function; a collision set construction module, used to use the part where the control point intersects with the obstacle as a collision segment; all control points in the collision segment form a collision set; a path acquisition module, used to use A* to find a replacement path in the voxel map; a replacement module, used to find the path point corresponding to each control point in the collision set in the replacement path for replacement, and obtain an initial obstacle avoidance trajectory; a trajectory optimization module, used to use the minimization of the total cost function as the objective function and the safety distance as the constraint condition, and use an optimization algorithm to optimize the trajectory to obtain an obstacle avoidance trajectory.

[0022] Compared with the prior art, the present invention has the following beneficial effects: The invention discloses a real-time obstacle avoidance method for a drone in a dynamic environment based on visually assisted B-spline trajectory optimization. The method optimizes the local control points of the control trajectory through B-spline trajectory optimization, which is not global optimization, thereby reducing the variables to be optimized and reducing the computational complexity. The cost and gradient of the collision control points are estimated through control point replacement, thereby avoiding directly calculating the collision cost of each obstacle pixel in the voxel map. The method searches for a local collision-free path and adjusts the trajectory in a progressive manner, thereby reducing the amount of calculation in the path optimization process. The method gradually optimizes the trajectory to avoid the high computational cost caused by a one-time global solution, gradually adjusts the control points, quickly generates a collision-free trajectory, and reduces the amount of calculation in each iteration. The method predicts collisions and optimizes trajectories according to the future state of dynamic obstacles, wherein the prediction and optimization processes are local and do not rely on real-time synchronized external data, and autonomous adjustments are made through visual data obtained by the drone in real time. The drone uses a depth camera and other sensors carried by the drone itself to perceive the surrounding environment, and through the generated 3D voxel map, the drone can independently detect and track obstacles and adjust path planning in real time without relying on an external system. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 A flowchart of a method for real-time obstacle avoidance in a dynamic environment of a UAV based on vision-assisted B-spline trajectory optimization is provided for this application; Figure 2 for Figure 1 The result diagram of collision detection in the figure; Figure 3 for Figure 1 Result diagram of collision control point replacement; Figure 4 This is a schematic diagram of determining the dynamic cost in a real-time obstacle avoidance method for a UAV in a dynamic environment based on vision-assisted B-spline trajectory optimization in this application.

[0024] The realization of the purpose, functional features and advantages of this application will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0025] In order to make the purpose, technical solutions and advantages of this application clearer, the technical solutions in this application will be clearly and completely described below in conjunction with the drawings in this application. Obviously, the described embodiments are part of the embodiments of this application, not all of them. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0026] The first embodiment of the present invention provides a real-time obstacle avoidance method for a UAV in a dynamic environment based on vision-assisted B-spline trajectory optimization, such as Figure 1 As shown, the specific steps include: Step S1, obtaining a depth image of the flight scene, and establishing a voxel map based on the depth image; Specifically, the depth image is generated by the depth sensor on the drone, which contains the distance information between each pixel in the flight scene and the camera. The drone can be an industrial-grade drone such as the PUC S400 or DJI T30. The drone uses its own depth camera and other sensors to perceive the surrounding environment. Through the generated 3D voxel map, the drone can independently detect and track obstacles and adjust the path planning in real time without relying on external systems.

[0027] Step S2, identifying obstacles in the depth image and determining the locations of the obstacles; Specifically, in step S21, a bounding box of an obstacle in the depth image is identified, and a potential obstacle area is determined according to the bounding box of the obstacle; In this embodiment, the U-depth map method can be used to obtain the bounding box of the obstacle in the depth image, that is, to display the length and width of the obstacle, thereby determining the potential obstacle area; Step S22, performing a depth continuity check on the potential obstacle area to obtain depth data of the potential obstacle; Step S23, determining 3D information of the potential obstacle based on the depth data of the potential obstacle and the boundary box of the obstacle in the depth image; Step S24, according to the 3D size of the potential obstacle, the voxel map is consulted to determine the accurate 3D information of the potential obstacle. Specifically, by searching the 3D size of the potential obstacle in the voxel map, it is determined whether the obstacle really exists, thereby determining the accurate 3D information of the potential obstacle.

[0028] Step S3, using a simple straight line method to generate the initial trajectory of the UAV, and using a B-spline function to parameterize the initial trajectory to determine the control points of the initial trajectory, i.e., the control points of the initial trajectory, these control points of the B-spline curve define the geometry and path of the initial trajectory; In this embodiment, the gradient-based B-spline trajectory optimization performs optimization by controlling the local control points of the trajectory instead of global optimization, thereby reducing the variables to be optimized and the computational complexity.

[0029] Obstacles include static obstacles and dynamic obstacles. In order to ensure the dynamics and accuracy of the map, static obstacles must be removed. Therefore, after determining the position of the obstacle in step S2, static obstacles are removed. The specific removal method includes: The state of the obstacle is estimated through the Kalman filter to obtain the current speed of the obstacle; based on the speed of the obstacle and the preset speed threshold, the static obstacles are removed to obtain the dynamic obstacles.

[0030] In this embodiment, a minimum speed threshold is set to remove objects with speeds below the threshold, which are considered static obstacles. Most static obstacles are removed by the speed threshold. Due to sensor noise, some static obstacles may shake slightly, resulting in speed errors. A continuity filter is used to eliminate these objects that are shaken by noise. By checking the object speed history, objects with discontinuous motion characteristics are removed. The visually assisted 3D dynamic system tracks and represents dynamic obstacles by fusing depth images and voxel maps. Compared with building complex global maps, this embodiment focuses on the detection and dynamic tracking of local environments, and uses a Kalman filter to filter static obstacles to reduce unnecessary calculations.

[0031] Step S4: The part where the initial trajectory intersects with the obstacle is taken as the collision segment; all control points in the collision segment form a collision set, such as Figure 2 As shown; In this embodiment, the control points of the initial trajectory are analyzed. If the control points are located within the obstacle (dynamic obstacle), these collision segments are marked. The control points of these collision segments are marked as "collision control points" and then form a "collision set". These control points represent the area on the trajectory that intersects with the obstacle; Step S5, using A* to find a replacement path in the voxel map, i.e., a safe guidance path; In this embodiment, A* is a heuristic search algorithm that can find the shortest path from a starting point to a target point in a grid or graph structure. The starting point of the search is the position of the previous control point of the obstacle, and the target point is the position of the next control point of the obstacle. During the search, each node is checked to see if it is occupied by an obstacle to decide whether to add the node to the open list. If a node is marked as occupied in the voxel map, it is skipped to avoid being part of the path.

[0032] Step S6, find the path point corresponding to each control point in the collision set in the replacement path and replace it to obtain the initial obstacle avoidance trajectory, that is, the optimized trajectory, such as Figure 3 shown.

[0033] In this embodiment, the cost and gradient of the collision control point are estimated by replacing the control point, so as to avoid directly calculating the collision cost of each obstacle pixel in the voxel map. By searching for a local collision-free path and adjusting the trajectory in a progressive manner, the amount of calculation in the path optimization process is reduced.

[0034] Step S7, using an optimization algorithm, taking the minimization of the total cost function as the objective function and the safety distance as the constraint condition, optimizing the trajectory to obtain an obstacle avoidance trajectory.

[0035] Specifically, the optimization algorithm may be a gradient descent method, and the calculation formula of the total cost function is:

[0036] in, is the security cost function, is the smoothness cost function, is the dynamic obstacle cost function.

[0037] The security cost function is:

[0038]

[0039]

[0040] In the formula, is the time step, The value range is , V i , A i are the velocity and acceleration of the control point, is the two-norm, is the unit normalization factor.

[0041] The smoothness cost function is:

[0042] .

[0043] The dynamic obstacle cost function is:

[0044] in, is a safe distance, when the collision control point When in the active area of ​​​​dynamic obstacles,

[0045] Where D is the collision control point The intersection of the vertical line with the boundary line of the dynamic obstacle's active area, F is the current position of the dynamic obstacle.

[0046] The method for obtaining the activity area of ​​dynamic obstacles is as follows: Figure 4 As shown in the figure, the linear prediction model is used to predict the position of the dynamic obstacle in multiple future time steps based on the current speed and position of the dynamic obstacle, so as to obtain multiple prediction points. The current position of the dynamic obstacle and the last prediction point are connected to form a polygonal area, that is, the activity area of ​​the dynamic obstacle. If the control point of the UAV is in this area, the dynamic collision cost is estimated by calculating the distance to the safe area (that is, the boundary line of the activity area of ​​the dynamic obstacle).

[0047] In this embodiment, deep vision and voxel-based three-dimensional dynamic maps are used to represent and track dynamic obstacles. The future state of dynamic obstacles is predicted by recursion, and the dynamic collision cost is evaluated using the receding horizon distance field. There is no need to establish a detailed dynamic model for each dynamic obstacle. Collision prediction and trajectory optimization are performed based on the future state of dynamic obstacles. The prediction and optimization process is local and does not rely on real-time synchronization of external data. Autonomous adjustments are made through visual data obtained by the drone in real time. There is no need for global optimization, which reduces the variables that need to be optimized and reduces the computational complexity.

[0048] With the center of the current position of the obstacle, a circular area with a preset radius of r is obtained. When it is within the circular area, .

[0049] The invention discloses a real-time obstacle avoidance device for unmanned aerial vehicle in dynamic environment based on vision-assisted B-spline trajectory optimization, comprising an image acquisition module, an obstacle recognition module, a trajectory generation module, a collision set construction module, a path acquisition module, a replacement module and a trajectory optimization module, wherein the image acquisition module is used to acquire a depth image of a flight scene and establish a voxel map based on the depth image; the obstacle recognition module is used to recognize obstacles in the depth image and determine the positions of the obstacles; the trajectory generation module is used to generate an initial trajectory of the unmanned aerial vehicle and parameterize the initial trajectory using a B-spline function to determine the control points of the B-spline function; the collision set construction module is used to use the part where the control point intersects with the obstacle as a collision segment; all control points in the collision segment form a collision set; the path acquisition module is used to find a replacement path in a voxel map using A*; the replacement module is used to find the path point corresponding to each control point in the collision set in the replacement path for replacement to obtain an initial obstacle avoidance trajectory; the trajectory optimization module is used to use the minimized total cost function as an objective function and the safety distance as a constraint condition, and use an optimization algorithm to optimize the trajectory to obtain an obstacle avoidance trajectory.

[0050] The above are only preferred embodiments of the present application, and are not intended to limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A real-time obstacle avoidance method for unmanned aerial vehicles in dynamic environments based on vision-assisted B-spline trajectory optimization, characterized in that: include: Acquire a depth image of the flight scene, and establish a voxel map based on the depth image; Identify obstacles in the depth image and determine the locations of the obstacles; Generate an initial trajectory of the UAV, and parameterize the initial trajectory using a B-spline function to determine control points of the initial trajectory; The part where the initial trajectory intersects with the obstacle is taken as a collision segment, and all control points in the collision segment form a collision set; Using A* to find an alternative path in the voxel map; Finding a path point corresponding to each control point in the collision set in the replacement path for replacement, and obtaining an initial obstacle avoidance trajectory; The minimization of the total cost function is used as the objective function, the safety distance is used as the constraint condition, and the optimization algorithm is used to optimize the trajectory to obtain the obstacle avoidance trajectory.

2. The real-time obstacle avoidance method for unmanned aerial vehicle in dynamic environment based on vision-assisted B-spline trajectory optimization according to claim 1 is characterized in that: The identifying the obstacle in the depth image and determining the position of the obstacle includes: Identifying a bounding box of an obstacle in the depth image, and determining a potential obstacle area according to the bounding box of the obstacle; Performing a depth continuity check on the potential obstacle area to obtain depth data of the potential obstacle; Determining 3D information of the potential obstacle according to the depth data of the potential obstacle and a bounding box of the obstacle in the depth image; According to the 3D size of the potential obstacle, the voxel map is consulted to determine the accurate 3D information of the potential obstacle.

3. The real-time obstacle avoidance method for unmanned aerial vehicle in dynamic environment based on vision-assisted B-spline trajectory optimization according to claim 1 is characterized in that: The obstacles include static obstacles and dynamic obstacles. After determining the positions of the obstacles, the method further includes: Estimating the state of the obstacle according to the position of the obstacle to obtain the current speed of the obstacle; Based on the current speed of the obstacle and a preset speed threshold, the static obstacle is removed to obtain a dynamic obstacle.

4. The real-time obstacle avoidance method for unmanned aerial vehicle in dynamic environment based on vision-assisted B-spline trajectory optimization according to claim 3 is characterized in that: The calculation formula of the total cost function is: in, is the security cost function, is the smoothness cost function, is the dynamic obstacle cost function.

5. The real-time obstacle avoidance method for unmanned aerial vehicle in dynamic environment based on vision-assisted B-spline trajectory optimization according to claim 4 is characterized in that: The security cost function is: In the formula, is the time step, The value range is , V i , A i are the velocity and acceleration of the control point, is the two-norm, is the unit normalization factor.

6. The real-time obstacle avoidance method for unmanned aerial vehicle in dynamic environment based on vision-assisted B-spline trajectory optimization according to claim 4 is characterized in that: The smoothness cost function is: 。 7. The real-time obstacle avoidance method for unmanned aerial vehicle in dynamic environment based on vision-assisted B-spline trajectory optimization according to claim 4 is characterized in that: The dynamic obstacle cost function is: in, is a safe distance, when the collision control point When in the active area of ​​​​dynamic obstacles, Where D is the collision control point The intersection of the vertical line with the boundary line of the dynamic obstacle's active area, F is the current position of the dynamic obstacle; Take the current position O of the obstacle as the center and obtain the circular area with a preset radius r. When it is within the circular area, 。 8. The real-time obstacle avoidance method for unmanned aerial vehicle in dynamic environment based on vision-assisted B-spline trajectory optimization according to claim 7 is characterized in that: The method for obtaining the activity area of ​​the dynamic obstacle comprises: Using the linear prediction model, the position of the dynamic obstacle in multiple future time steps is predicted based on the current speed and position of the dynamic obstacle, and multiple prediction points are obtained. The current position of the dynamic obstacle and the last prediction point are connected to form the activity area of ​​the dynamic obstacle.

9. A real-time obstacle avoidance device for unmanned aerial vehicles in dynamic environments based on vision-assisted B-spline trajectory optimization, characterized in that: include: An image acquisition module, used to acquire a depth image of the flight scene and establish a voxel map based on the depth image; An obstacle recognition module, used to recognize obstacles in the depth image and determine the positions of the obstacles; A trajectory generation module is used to generate an initial trajectory of the UAV, and parameterize the initial trajectory using a B-spline function to determine the control points of the B-spline function; A collision set construction module, used to take the part where the control point intersects with the obstacle as a collision segment; and to form a collision set with all the control points in the collision segment; A path acquisition module, used for finding a replacement path in the voxel map using A*; A replacement module, used for finding a path point corresponding to each control point in the collision set in the replacement path for replacement, so as to obtain an initial obstacle avoidance trajectory; The trajectory optimization module is used to optimize the trajectory by using the optimization algorithm with the minimization of the total cost function as the objective function and the safety distance as the constraint condition to obtain the obstacle avoidance trajectory.

Images