A real-time obstacle avoidance method and device for unmanned aerial vehicle in dynamic environment based on vision-assisted B-spline trajectory optimization

Abstract

This application discloses a real-time obstacle avoidance method and device for a UAV in a dynamic environment based on visually assisted B-spline trajectory optimization, specifically relating to the field of UAV trajectory optimization. The method comprises: acquiring a depth image of the flight scene and establishing a voxel map based on the depth image; identifying obstacles in the depth image and determining their locations; generating an initial trajectory for the UAV, parameterizing the initial trajectory, and determining the control points of the initial trajectory; treating the portion where the initial trajectory intersects with the obstacle as a collision segment, and forming a collision set from all control points within the collision segment; using A* to find a replacement path in the voxel map; 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; and optimizing the trajectory using an optimization algorithm to obtain an obstacle avoidance trajectory. The method solves the problem of high computing resource requirements in the prior art.

Description

Technical Field

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

[0002] Autonomous navigation and obstacle avoidance for unmanned aerial vehicles (UAVs) in dynamic environments have garnered significant attention in recent years. With the increasing use of UAVs in diverse and complex environments, such as exploration, rescue, and power inspection, the autonomous capabilities of UAVs in these tasks have become crucial. However, most current path planning algorithms rely primarily on a single map representation, such as geometric maps, occupancy maps, or Euclidean Signed Distance Field (ESDF) maps. While these methods excel in static environments, they are inadequate for handling both static and dynamic obstacles due to inherent map limitations, hindering safe flight.

[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 current control inputs by predicting the system's behavior over time. Its disadvantages are that the MPC method requires solving an optimization problem at each time step, and the size of the optimization problem increases with the complexity and constraints of the system. MPC relies on the system's dynamic model 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 effectiveness depends on accurate predictions of obstacle motion, and in highly dynamic and unpredictable environments, the performance of MPC may be limited.

[0004] Multi-level and dual-level planning utilize a hierarchical approach to path planning, enabling drones to effectively avoid obstacles in complex environments. This approach typically consists of a global path planning layer and a local obstacle avoidance layer. The global path planning layer generates the overall route from the starting point to the destination, primarily addressing static obstacles. Based on a global map, it uses algorithms such as A* and Dijkstra to generate a path that avoids obstacles. Because static obstacles are fixed in position, the planning results are relatively stable, providing a reference path for the drone's flight. The local obstacle avoidance layer addresses dynamic obstacles (such as moving vehicles and pedestrians). Sensors capture real-time environmental information, dynamically construct a local map, and predict obstacle trajectories in real time. The local obstacle avoidance layer utilizes algorithms such as the velocity obstacle method (VO) to calculate a safe path, ensuring the drone avoids dynamic obstacles. These two layers work in tandem: the global path provides macroscopic direction, while the local obstacle avoidance layer handles unexpected situations during flight. If repeated adjustments to the local obstacle avoidance cause the global path to deviate, the global path planner may recalculate the path. However, this method requires the drone to run both the global path planner and the local obstacle avoider simultaneously, which requires high computing resources. In addition, efficient communication and data synchronization need to be maintained between the global path planning layer and the local obstacle avoidance layer to ensure that 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 existing technology.

[0006] To achieve the above-mentioned objectives, 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 the 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 part 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; using A* to find a replacement path in the voxel map; 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; using the minimization of the total cost function as the objective function and the safety distance as the constraint condition, and using an optimization algorithm to optimize the trajectory to obtain an obstacle avoidance trajectory.

[0007] Optionally, obstacles in the depth image are identified and their positions 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 the precise 3D information of the potential obstacle.

[0008] Optionally, the obstacles include static obstacles and dynamic obstacles. 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 obstacles 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]

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

[0012] Optionally, the security cost function is:

[0013]

[0014]

[0015]

[0016] Where, 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.

[0017] Optionally, the smoothness cost function is:

[0018]

[0019] .

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

[0021]

[0022] in, For safe distance, when the collision control point When in the active area of ​​a dynamic obstacle,

[0023]

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

[0025] With the current position O of the obstacle as the center, obtain the circular area with a preset radius of r. When the collision control point When it is within the circular area,

[0026] .

[0027] 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.

[0028] 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 for acquiring a depth image of a flight scene and establishing a voxel map based on the depth image; an obstacle recognition module for identifying obstacles in the depth image and determining the position of the obstacles; a trajectory generation module for generating an initial trajectory of the UAV, and parameterizing the initial trajectory using a B-spline function to determine the control point of the B-spline function; a collision set construction module for taking the part where the control point intersects with the obstacle as a collision segment; and forming a collision set from all control points in the collision segment; a path acquisition module for finding a replacement path in the voxel map using A*; a replacement module for finding the path point corresponding to each control point in the collision set in the replacement path and replacing it to obtain an initial obstacle avoidance trajectory; a trajectory optimization module for taking the minimization of the total cost function as the objective function and the safety distance as the constraint condition, and optimizing the trajectory using an optimization algorithm to obtain an obstacle avoidance trajectory.

[0029] Compared with the prior art, the present invention has the following advantages:

[0030] The present invention discloses a real-time obstacle avoidance method for an unmanned aerial vehicle (UAV) in a dynamic environment based on vision-assisted B-spline trajectory optimization. The method optimizes the local control points of the control trajectory through B-spline trajectory optimization, rather than global optimization, thereby reducing variables that need to be optimized and lowering computational complexity. The method estimates the cost and gradient of the collision control point by replacing the control point, 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 computation during the path optimization process. The method gradually optimizes the trajectory to avoid the high computational cost brought by a one-time global solution, gradually adjusts the control points, quickly generates a collision-free trajectory, and reduces the amount of computation in each iteration. The method performs collision prediction and trajectory optimization based on the future state of the dynamic obstacle. The prediction and optimization process is local and does not rely on real-time synchronized external data. Autonomous adjustment is performed through visual data acquired by the UAV in real time. The method utilizes the UAV's own depth camera and other sensors to perceive the surrounding environment. Through the generated 3D voxel map, the UAV can independently detect and track obstacles and adjust path planning in real time without relying on external systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 This is a flowchart of a real-time obstacle avoidance method for a UAV in a dynamic environment based on vision-assisted B-spline trajectory optimization;

[0032] Figure 2 for Figure 1 The result diagram of collision detection;

[0033] Figure 3 for Figure 1 The result diagram of collision control point replacement;

[0034] 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.

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

[0036] To make the objectives, technical solutions, and advantages of this application more clear, the technical solutions of this application will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the embodiments described are only part of the embodiments of this application, not all of them. Based on the embodiments of this application, all other embodiments obtained by ordinary technicians in this field without making any creative efforts are within the scope of protection of this application.

[0037] 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. Figure 1 As shown, the specific steps include:

[0038] Step S1, obtaining a depth image of the flight scene and establishing a voxel map based on the depth image;

[0039] Specifically, a depth image is generated by a depth sensor on a drone. This image contains the distance between each pixel in the flight scene and the camera. The drone can be an industrial-grade drone like the PZS S400 or DJI T30. Using the drone's onboard depth camera and other sensors to perceive its surroundings, the generated 3D voxel map allows the drone to independently detect and track obstacles and adjust its path planning in real time, without relying on external systems.

[0040] Step S2, identifying obstacles in the depth image and determining the locations of the obstacles;

[0041] Specifically, in step S21, a bounding box of an obstacle in the depth image is identified, and a potential obstacle area is determined based on the bounding box of the obstacle;

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

[0043] Step S22: performing a depth continuity check on the potential obstacle area to obtain depth data of the potential obstacle;

[0044] Step S23, 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;

[0045] In step S24, the voxel map is consulted based on the 3D size of the potential obstacle to determine the precise 3D information of the potential obstacle. Specifically, the 3D size of the potential obstacle is searched in the voxel map to determine whether the obstacle actually exists, thereby determining the accurate 3D information of the potential obstacle.

[0046] Step S3, using the simple straight line method to generate the initial trajectory of the UAV, and using the 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 geometric shape and path of the initial trajectory;

[0047] In this embodiment, the gradient-based B-spline trajectory optimization performs optimization by controlling the local control points of the trajectory rather than global optimization, thereby reducing the variables that need to be optimized and lowering the computational complexity.

[0048] Obstacles include static obstacles and dynamic obstacles. To ensure the dynamicity and accuracy of the map, static obstacles must be removed. Therefore, after determining the location of the obstacle in step S2, static obstacles are removed. The specific removal method includes:

[0049] The Kalman filter is used to estimate the state of the obstacle and 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.

[0050] 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's speed history, objects with discontinuous motion characteristics are removed. The visually assisted 3D dynamic system achieves tracking and representation of 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.

[0051] 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 are combined into a collision set, such as Figure 2 As shown;

[0052] In this embodiment, the control points of the initial trajectory are analyzed. If the control points are located within an 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;

[0053] Step S5, using A* to find an alternative path in the voxel map, i.e., a safe guidance path;

[0054] 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 and avoided as part of the path.

[0055] 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.

[0056] In this embodiment, the cost and gradient of the collision control point are estimated by replacing the control point, avoiding the direct calculation of 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 computation in the path optimization process is reduced.

[0057] In step S7, an optimization algorithm is used to optimize the trajectory by minimizing the total cost function as the objective function and the safety distance as the constraint condition to obtain the obstacle avoidance trajectory.

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

[0059]

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

[0061] The security cost function is:

[0062]

[0063]

[0064]

[0065] Where, 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.

[0066] The smoothness cost function is:

[0067]

[0068] .

[0069] The dynamic obstacle cost function is:

[0070]

[0071] in, For safe distance, when the collision control point When in the active area of ​​a dynamic obstacle,

[0072]

[0073] 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.

[0074] The method for obtaining the active 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. Multiple prediction points can be obtained. The current position of the dynamic obstacle and the last prediction point are connected to form a polygonal area, that is, the active area of ​​the dynamic obstacle. If the control point of the UAV is within this area, the dynamic collision cost is estimated by calculating the distance to the safe area (that is, the boundary line of the active area of ​​the dynamic obstacle).

[0075] This embodiment relies on depth vision and voxel-based 3D dynamic maps to represent and track dynamic obstacles. The future states of dynamic obstacles are recursively predicted, and the receding horizon distance field is used to assess dynamic collision costs, eliminating the need for a detailed dynamic model for each dynamic obstacle. Collision prediction and trajectory optimization are performed based on the future states of dynamic obstacles. The prediction and optimization process is local and does not rely on real-time synchronized external data. Autonomous adjustments are made using real-time visual data acquired by the drone. This eliminates the need for global optimization, reduces the number of variables to be optimized, and lowers computational complexity.

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

[0077] .

[0078] A real-time obstacle avoidance device for unmanned aerial vehicles (UAVs) in dynamic environments based on vision-assisted B-spline trajectory optimization includes 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. 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 UAV 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 are combined into a collision set; the path acquisition module is used to use A* to find a replacement path in the voxel map; the replacement module is used to find the path point corresponding to each control point in the collision set in the replacement path and replace it to obtain an initial obstacle avoidance trajectory; the trajectory optimization module is used to use the minimization of the total cost function as the objective function and the safety distance as the constraint condition, and to optimize the trajectory using an optimization algorithm to obtain an obstacle avoidance trajectory.

[0079] The above are only preferred embodiments of the present application and do not 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 UAV in dynamic environment based on vision-assisted B-spline trajectory optimization, characterized by: include: Acquire a depth image of the flight scene, and establish a voxel map based on the depth image; Identifying obstacles in the depth image and determining the locations of the obstacles; Generate an initial trajectory of the UAV, parameterize the initial trajectory using a B-spline function, and determine control points of the initial trajectory; The part where the initial trajectory intersects 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; Searching for a path point corresponding to each control point in the collision set in the replacement path and replacing it to obtain an initial obstacle avoidance trajectory; Taking the minimization of the total cost function as the objective function and the safety distance as the constraint condition, an optimization algorithm is used to optimize the trajectory to obtain an obstacle avoidance trajectory; 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; removing the static obstacle based on the current speed of the obstacle and a preset speed threshold to obtain a dynamic obstacle; 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; The security cost function is: Where, 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; The dynamic obstacle cost function is: in, For safe distance, when the collision control point When in the active area of ​​a dynamic obstacle, 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; With the current position O of the obstacle as the center, obtain the circular area with a preset radius of r. When the collision control point When it is within the circular area, ； The method for obtaining the activity area of ​​the dynamic obstacle includes: 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.

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 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 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 smoothness cost function is: 。 4. A real-time obstacle avoidance device for UAV in dynamic environment based on vision-assisted B-spline trajectory optimization, characterized in that: include: An image acquisition module, configured to acquire a depth image of the flight scene and establish a voxel map based on the depth image; An obstacle recognition module is used to identify obstacles in the depth image and determine the location of the obstacles; A trajectory generation module is used to generate an initial trajectory of the UAV, parameterize the initial trajectory using a B-spline function, and determine the control points of the B-spline function; A collision set construction module is used to take the intersection of the control point and the obstacle as a collision segment; and to form a collision set from all control points in the collision segment; A path acquisition module, configured to find a replacement path in the voxel map using A*; A replacement module, configured to search for a path point corresponding to each control point in the collision set in the replacement path and replace the path point to obtain an initial obstacle avoidance trajectory; The trajectory optimization module is used to optimize the trajectory using an optimization algorithm by taking the minimization of the total cost function as the objective function and the safety distance as the constraint condition to obtain an obstacle avoidance trajectory.

Images