An unmanned aerial vehicle local exploration trajectory planning method fusing perception quality and dynamic risk

Abstract

The application discloses a kind of unmanned plane local exploration trajectory planning methods of fusing perception quality and dynamic risk, belong to the technical field of unmanned plane autonomous exploration and local motion planning.The method includes: constructing local planning window;The effectiveness of local target is checked and multiple candidate local trajectories are generated;The basic feasibility of candidate trajectory is screened;The continuous exploration benefit and perception quality of trajectory are obtained;LIO positioning degradation cost and dynamic timing risk cost are determined;Unified comprehensive cost is calculated, and speed and acceleration are adaptively adjusted according to joint risk amount;The trajectory with minimum comprehensive cost is selected and executed, and re-planning is performed when trigger condition is met.The trajectory planning method of the application can make the unmanned plane actively avoid positioning degradation area when performing local exploration, realize dynamic obstacle avoidance consistent in time and space, and actively slow down in high-risk or low-perception-quality area, effectively improving the safety and stability of unmanned plane autonomous exploration.

Description

Technical Field

[0001] This invention relates to the field of autonomous exploration and local motion planning technology for unmanned aerial vehicles (UAVs), specifically to a method for planning the local exploration trajectory of UAVs that integrates perception quality and dynamic risk. Background Technology

[0002] When a drone autonomously explores an unknown 3D environment, the upper-level global planner is typically responsible for determining the current frontier area or target viewpoint to be visited, while the local planner needs to generate a continuous trajectory that can be executed by the flight controller based on the drone's real-time status, local map, and sensor observation conditions. The local trajectory not only determines obstacle avoidance and dynamic feasibility but also directly affects the efficiency of acquiring unknown areas during flight, the stability of laser inertial positioning, and the safety margin in dynamic environments.

[0003] Existing local trajectory planning typically employs methods such as minimum snap polynomials, B-splines, MINCO, motion primitives, ESDF-based gradient optimization, or kinematic search, with flight time, trajectory smoothness, static obstacle distance, and velocity / acceleration constraints as the main optimization metrics. In the exploration framework, the local layer can also generate a minimum time trajectory based on the target viewpoint or local leading-edge segment provided by the upper layer, thereby improving arrival efficiency.

[0004] However, the perception quality during actual exploration varies significantly with the drone's position, orientation, and speed. For example, in long corridors, single-plane areas, sparse point clouds, areas with weak reflections, or areas with severe occlusion, the geometric constraints of laser inertial odometry may degrade. When the drone makes high-speed turns or rapidly traverses low-structure areas, the problems of single-frame point cloud distortion and matching instability are further aggravated. Furthermore, the future position of dynamic obstacles is related to the moment the drone arrives at that position, making it difficult to reflect the true collision risk using only the current static map.

[0005] Therefore, a unified trajectory evaluation and optimization method for the local execution layer is needed. Without changing the access order of the global target in the upper layer, the method should incorporate the front-end visibility benefits, point cloud perception quality, LIO (laser inertial odometry) geometric observability, dynamic obstacle temporal risks, and risk-related velocity constraints into the local trajectory generation and rolling replanning process.

[0006] Common trajectory planning schemes in existing technologies include the following categories: The first category consists of local safe trajectory optimization methods based on ESDF or occupied maps. These methods construct collision costs through obstacle distance fields and generate safe trajectories in conjunction with trajectory smoothing terms and dynamic constraints. They are mature in engineering implementation, but their evaluation objects are mainly static geometric safety, and they usually cannot reflect the continuous exploration benefits and localization observability along the trajectory.

[0007] The second category consists of minimum time, minimum snap, or MINCO-type trajectory generation methods. These methods can obtain smooth and efficient trajectories given start and end states and dynamic boundary conditions. However, if the optimization objective is mainly biased towards shortening the execution time, it may cause the UAV to maintain excessively high speeds in areas with insufficient point cloud structure, poor sensor field of view quality, or dense dynamic obstacles.

[0008] The third category is the local viewpoint refinement and frontier exploration method. This type of method selects local endpoints based on upper-level frontiers or candidate viewpoints, which is beneficial for improving the coverage of the target viewpoint. However, existing evaluations usually focus on the target viewpoint itself and do not adequately consider the continuous visible frontier, the degree of repeated coverage, and the quality of point cloud observations during the flight from the current state to the target.

[0009] The fourth category is dynamic obstacle prediction and local obstacle avoidance methods. These methods use target tracking, dynamic point clouds, or temporal occupancy maps to predict future obstacle states, which can improve obstacle avoidance capabilities in dynamic scenes. However, dynamic risks are often treated as independent obstacle avoidance constraints, and there is a lack of a unified trajectory scoring mechanism between them and exploration information gains, localization degradation risks, and adaptive speed control.

[0010] The fifth category consists of trajectory planning methods based on perception constraints or state estimation. These methods can consider field of view, feature visibility, reprojection error, or state covariance, but they are mostly geared towards target tracking, visual reconstruction, or specific perception tasks. There is still a lack of local planning schemes for autonomous exploration by laser inertial UAVs that simultaneously integrate continuous frontier gains and dynamic temporal risks.

[0011] In summary, the existing technology has the following drawbacks: 1. Local trajectory evaluation usually focuses on time, smoothness and static obstacle avoidance, but does not uniformly quantify the ability of UAVs to continuously observe unknown frontiers throughout the entire trajectory process.

[0012] 2. Exploration gains are often calculated at candidate endpoints or target viewpoints, and the effective field of view coverage, occlusion, incident angle, and repeated observation attenuation in the middle section of the trajectory are not fully utilized.

[0013] 3. Insufficient coupling between local planning and laser inertial state estimation may cause the trajectory to continuously traverse areas with sparse point clouds, simple geometric structures, or insufficient matching constraints, thereby increasing positioning degradation and mapping errors.

[0014] 4. Obstacle avoidance based solely on static ESDF or current occupancy status makes it difficult to determine whether "when the drone reaches a certain location in the future, that location will be occupied by a dynamic obstacle," thus failing to adequately express dynamic collision risks.

[0015] 5. The upper limits of velocity and acceleration are usually fixed and cannot be actively reduced due to map risks, pose uncertainty, dynamic occupation risks, and reduced local perception quality.

[0016] 6. The lack of a unified output interface for the feasibility of local trajectories, perception quality, execution risks, and reasons for replanning is not conducive to forming a stable closed-loop collaboration with flight control, mapping and positioning modules, and external global exploration planners. Summary of the Invention

[0017] This invention addresses the shortcomings of existing technologies by proposing a UAV local exploration trajectory planning method that integrates perception quality and dynamic risk, aiming to solve the following technical problems: 1. To address the problem that local planning only focuses on time, smoothness, and static safety distance, while failing to simultaneously consider continuous exploration benefits, perception quality, positioning stability, and dynamic feasibility.

[0018] 2. Solve the problem of evaluating information gains only from the end-point viewpoint, enabling UAVs to effectively observe unknown frontiers throughout their entire flight from the current state to the local target.

[0019] 3. This addresses the issue that local trajectories cannot identify LIO degradation regions based on point cloud geometry and state estimation uncertainties, enabling planning results to proactively avoid long-term weakly constrained regions.

[0020] 4. To address the disconnect between dynamic obstacle risk and trajectory arrival time, the future occupancy probability or target prediction position is mapped to the local trajectory evaluation according to the trajectory time parameter.

[0021] 5. To address the issue that a fixed speed limit cannot reflect changes in perceived quality and environmental risks, local speed, acceleration, and time allocation are adjusted online based on the combined risk level.

[0022] 6. Resolve the issue of inconsistent interfaces between local planning results and downstream execution modules, and output the optimal local trajectory, motion constraints, perception quality, execution risk, replanning flag, and local target status.

[0023] To solve the above-mentioned technical problems, the present invention provides the following technical solution: A method for planning the local exploration trajectory of a UAV that integrates perceived quality and dynamic risk includes: Step 1: Receive the current local target, local front fragment, or candidate viewpoint from the external global exploration planner, and simultaneously acquire the UAV status, state covariance, local occupancy map, Euclidean distance field, dynamic occupancy prediction results, local front voxels, and sensor field of view parameters; unify all types of data into the same coordinate system and planning time domain, and construct the current local planning window; Step 2: Perform a validity check on the current local target and generate multiple candidate local trajectories with different path shapes, velocity distributions and observation orientations using methods such as B-splines, MINCO, kinematic search, motion primitives or multi-topology channels. Step 3: Perform basic feasibility screening on candidate trajectories, and check static obstacle safety distance, terminal accessibility, speed, acceleration, jerk and yaw motion constraints. Step 4: Sample the selected trajectories over time, and statistically analyze the visible frontier of the entire trajectory based on sensor pose, field of view, effective distance, ray occlusion, and incident angle. Attenuate repeated coverage to obtain continuous exploration gains and trajectory perception quality scores. Step 5: Statistically calculate the covariance eigenvalues ​​and effective matching constraints of the neighborhood point cloud at each trajectory sampling location, and construct the LIO positioning degradation cost by combining the uncertainty of state estimation; at the same time, calculate the dynamic temporal risk of the UAV at the corresponding future time based on the dynamic occupancy probability or dynamic target prediction trajectory. Step 6: Incorporate trajectory time, smoothness, static collision cost, dynamic risk, LIO degradation cost, continuous exploration benefits, and perception quality into a unified comprehensive cost. Based on map risk, pose uncertainty, dynamic risk, and perception quality, calculate the speed scaling factor and adaptively adjust the local velocity, acceleration, and trajectory time allocation. Step 7: Select the trajectory with the minimum overall cost from the feasible candidate trajectories as the current optimal local trajectory, and execute its preceding stages using a rolling time-domain approach. When the local map, dynamic risk, state covariance, or tracking error meets the replanning trigger condition, re-execute the candidate generation and optimization. Step 8: Construct the output data set of the local planner. At the same time, output the trajectory and local motion constraints to the flight control module, output the perception quality and high-risk location to the mapping / localization module, and return the status such as target arrival, local infeasibility, or need to switch targets to the external global exploration planner.

[0024] Compared with the prior art, the present invention discloses the following technical effects: 1. This invention evaluates the visibility of the unknown frontier throughout the entire local trajectory process, rather than just at the endpoint viewpoint, and combines occlusion, incident angle, distance, and repeated coverage attenuation to enable the trajectory flight process itself to continuously generate effective exploration benefits.

[0025] 2. This invention introduces local point cloud geometry, effective matching constraints, and state estimation uncertainties into trajectory evaluation, enabling local planning to identify and avoid weakly structured regions that are detrimental to LIO, thereby improving the stability of localization and mapping.

[0026] 3. This invention queries the future dynamic occupancy probability or dynamic target prediction position according to the arrival time of the candidate trajectory, thereby forming a spatiotemporally consistent dynamic collision risk assessment, which is different from obstacle avoidance methods based solely on the current static map.

[0027] 4. This invention adjusts the upper limit of speed and acceleration based on map risk, pose uncertainty, dynamic obstacle risk and local perception quality, and re-timing the trajectory, so that the UAV can actively decelerate in high-risk or low perception quality areas.

[0028] 5. This invention organizes candidate trajectory generation, continuous perception evaluation, LIO degradation evaluation, dynamic risk evaluation, comprehensive optimization, rolling replanning, and safety output into a local execution closed loop, which can be modularly integrated with external global exploration planners, ESDF / voxel maps, laser inertial odometry, and existing B-splines or MINCO optimizers. Attached Figure Description

[0029] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0030] Figure 1 A flowchart of a UAV local exploration trajectory planning method provided in an embodiment of the present invention; Figure 2 This is a schematic diagram illustrating the multi-source candidate trajectory generation and basic feasibility screening provided in an embodiment of the present invention; Figure 3 This is a schematic diagram illustrating the benefits and perception quality evaluation of continuous exploration throughout the entire trajectory process provided in this embodiment of the invention. Figure 4 A schematic diagram illustrating the parallel evaluation of LIO geometric observability and dynamic temporal risk provided in this embodiment of the invention; Figure 5 This is a schematic diagram of the perceived quality-risk joint adaptive velocity constraint and retiming provided in an embodiment of the present invention; Figure 6 This is a schematic diagram illustrating the local trajectory comprehensive optimization, scrolling execution, and interface output provided in an embodiment of the present invention. Detailed Implementation

[0031] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0032] This invention provides a method for planning local exploration trajectories of unmanned aerial vehicles (UAVs) that integrates perception quality and dynamic risk, specifically including the following steps (see attached diagram). Figure 1 ): Step 1: Construct the current local planning window.

[0033] First, it receives the current local target, local leading edge fragment, or candidate viewpoint from the external global exploration planner, and simultaneously acquires the current UAV status. State covariance Partial Occupation Map Euclidean distance field Risk Map Dynamic Occupation Prediction Chart Local frontier voxel set Sensor field-of-view parameters and UAV dynamics constraint parameters.

[0034] Let the state variables of the UAV at time t be represented as: ; in, Indicates time The status of the drone; The rotation matrix from the machine coordinate system to the global coordinate system; and These represent the drone's position and speed, respectively. and These represent the zero bias of the accelerometer and the zero bias of the gyroscope, respectively. The planning time domain is denoted as... The local trajectory is generated in a rolling manner within a limited time window starting from the current planning moment.

[0035] The state estimation error covariance can be expressed as: ; in, This represents the state estimation error covariance matrix; Representation and State The corresponding error state vector; This represents the covariance calculation. Then, the lidar coordinate system, machine coordinate system, map coordinate system, and global coordinate system are integrated into the world coordinate system. The dynamic obstacle prediction results are aligned to the same planning time domain.

[0036] Regarding data synchronization, this invention aligns state estimation, local maps, dynamic obstacle prediction, and local targets to the same planning period according to timestamps. Let the local planning period be... When the difference between the timestamp of any input data and the current planning time exceeds the threshold When using this method, prioritize conservative velocity constraints or delay the execution of new trajectories, and avoid using outdated maps or outdated dynamic obstacle prediction results.

[0037] Input time consistency can be represented as: ; in, , and These represent the timestamps for the local map, dynamic risk prediction, and state estimation data, respectively. Indicates the current local planning time; This indicates the maximum permissible time synchronization error. If all three time differences do not exceed this threshold, the input is considered to meet the time consistency requirements for this cycle.

[0038] In terms of coordinate transformation, this invention preserves the external parameters of the laser-radar-mechanical system. pose from the machine system to the global coordinate system Furthermore, the sensor's field of view boundary, local leading-edge voxels, and predicted dynamic obstacle positions are uniformly transformed to the global coordinate system. For UAVs with gimbals or rotatable sensors, the gimbal angle can also be... Incorporate it into the field of view calculation.

[0039] Furthermore, the present invention also performs local target preprocessing, the specific steps of which are as follows: The set of local objectives output by the external global exploration planner is denoted as .in, It can be a target viewpoint, a local leading edge segment, a temporary obstacle avoidance target point, or a target area that needs to be continuously observed. This indicates the number of local objectives. The local planner only processes the reachability and trajectory quality of objectives within the current planning window and does not reorder long-term global objectives.

[0040] Construct a local planning window based on the current location, local target, and effective sensor distance: ; in, Indicates time Local planning window; Indicates the three-dimensional spatial position to be determined; Indicates the currently known map area; Indicates the current location of the drone; Indicates the local window radius; This represents the Euclidean norm.

[0041] In one embodiment of the present invention, local targets located inside obstacles, too far from the UAV, invisible to sensors, or significantly inconsistent with the current mission direction are first eliminated; for the remaining targets, several trajectory terminal constraints are generated based on their relationship with the current UAV position, velocity direction, and field of view orientation, without performing a roving solution on the global target sequence.

[0042] Step 2: Generation of multi-source candidate trajectories.

[0043] First, perform a validity check on the local targets: for each local target Query its surrounding safe distance, visibility, and consistency with the current trajectory direction, and generate a valid target marker. If the target is located inside an obstacle, has been fully observed, or is outside the local planning window, then the target will not be included in the local trajectory optimization for this cycle.

[0044] The target valid marker can be defined as: ; in, For local targets The valid flag is set to 1, indicating that the target has entered the candidate terminal set for this period, and 0, indicating that it has been removed. For indicator functions; The static obstacle distance is obtained by querying the Euclidean distance field. For the target location; For the static safe distance of drones; The benefits of localized exploration of the target; To allow the target to enter the optimized minimum revenue threshold.

[0045] When multiple local objectives are valid simultaneously, they are treated as candidate endpoints within the same local planning window, rather than constructing a global access matrix. The local planner selects the optimal trajectory for the current segment based on the comprehensive cost of each candidate trajectory; if there are still remaining objectives after execution, the local objective set is received or updated again in the next planning cycle.

[0046] Then the set of local candidate trajectories is defined as: ; in, This represents the set of candidate local trajectories generated during this planning period; Indicates the first Candidate trajectories; Indicates the total number of candidate trajectories, and Candidate trajectories can be derived from B-splines, MINCO, polynomial trajectories, motion primitives, etc. , It can be generated in ways such as multiple topology initial paths.

[0047] The k-th candidate trajectory can be represented as: ; in, Indicates the first Candidate trajectories in relative time Description of the state at the location; , and These represent position, velocity, and acceleration, respectively. Indicates the yaw angle; This indicates the total execution time of the candidate trajectory.

[0048] To enhance the diversity of candidate trajectories, direct trajectories, obstacle avoidance trajectories, deceleration observation trajectories, lateral observation trajectories, and conservative hovering trajectories can be generated near the same local target. Different candidate trajectories share the same local target input but have different intermediate control points, velocity distributions, and sensor orientations, thus providing selection space for subsequent perception quality evaluation.

[0049] In the process of generating multi-source candidate trajectories, this invention first generates a direct initial trajectory based on the current velocity direction, then generates a detour trajectory based on the local obstacle distribution, and finally generates a lateral observation trajectory based on the visible direction of the leading edge. For scenarios with high dynamic risks, deceleration observation trajectories and hovering waiting trajectories can also be generated to maintain safety when dynamic obstacle prediction is uncertain.

[0050] For a candidate trajectory of B-spline, its position can be represented as: ; in, Indicates the first One B-spline basis function; Indicates the first The first candidate trajectory One control point; This represents the highest index of the control point, therefore there are a total of One control point; The position of the trajectory is obtained by weighting each control point and basis function.

[0051] For the initial trajectory of the search class, this invention first uses a local window. Discrete paths are generated internally, and then continuous trajectories are obtained through spline smoothing or time redistribution. If the discrete path passes through a narrow channel, sampling points are added near the channel entrance and exit to improve the resolution of subsequent dynamic risk and perceived quality assessment.

[0052] Step 3: Basic feasibility screening of candidate trajectories.

[0053] Before conducting a detailed assessment of perceived quality and dynamic risks, a basic feasibility screening is first performed on candidate trajectories. Basic constraints include terminal reachability, static safety distance, and dynamic feasibility, as referenced... Figure 2 .

[0054] Static safety distance and dynamic constraints can be expressed as: ; in, Indicates the candidate trajectory at time... Distance to the nearest static obstacle; Indicates static safety distance; Indicates jerk; , and These represent the maximum permissible speed, maximum acceleration, and maximum jerk, respectively. The above constraints should... It is valid for all sampling or consecutive moments within the time frame.

[0055] Terminal reachability constraints can be expressed as: ; in, and Representing local targets respectively The desired position and desired yaw angle, and These represent the endpoint position and endpoint yaw angle of the candidate trajectory, respectively. and These represent the allowable terminal position error and yaw angle error, respectively. This represents the angle normalization operation that normalizes the yaw angle difference to the equivalent principal value interval. The basic feasibility screening employs a multi-level process, progressing from coarse to fine. The first stage only checks whether the trajectory control points fall into obstacles or unknown high-risk areas; the second stage performs isochronous sampling of the trajectory and queries the Euclidean distance field; the third stage checks the constraints of velocity, acceleration, jerk, and yaw rate. Through this multi-level screening, the system can reduce the computational load of fine-grained evaluation while retaining feasible trajectories.

[0056] The yaw rate constraint can be expressed as: ; in, and These represent yaw rate and yaw acceleration, respectively. and These represent the maximum permissible yaw rate and maximum yaw acceleration of the flight control or sensor orientation mechanism, respectively.

[0057] If a trajectory violates the safe distance only at a few sampling points, but its topological channel is still repairable, the system may not immediately delete the trajectory, but instead send it to the backend for optimization and apply obstacle avoidance gradients to the relevant control points; if multiple consecutive sampling points of a trajectory are located within the obstacle expansion zone, it is directly marked as infeasible.

[0058] Step 4: Evaluation of the benefits and perceived quality of continuous exploration of candidate trajectories.

[0059] 1. Reference Figure 3To avoid evaluating exploration gains only from the endpoint viewpoint, this invention performs time sampling along the candidate trajectory to evaluate the UAV's ability to continuously observe unknown boundaries during trajectory execution. The set of trajectory sampling times is as follows: ; in, Indicates the first The time sampling set of the trajectory; For the first Each sampling time; This represents the number of sampling intervals. The sampling time interval; This represents the total execution time of the trajectory. At each sampling moment, the corresponding sensor field of view is calculated based on the trajectory position, yaw angle, and sensor mounting extrinsic parameters.

[0060] The reward for continuous exploration of candidate trajectories is defined as: ; in, This represents the continuous exploration gains of candidate trajectories; Represents the current local frontier voxel set; Indicates the first A cutting-edge element, Indicates the trajectory at the sampling time The sensor's field of view; Used to determine whether the leading edge is within the current sensor field of view; A visible marker indicating the combined effects of factors such as occlusion, distance, and angle of incidence; This represents the effective frontier weight after considering repeated observation decay; This represents the sampling time interval.

[0061] To avoid the same frontier voxel being repeatedly scored in a candidate trajectory, this invention records the number of times each frontier voxel has been effectively covered within the current planning period, and its subsequent weight monotonically decays with increasing coverage count. The exponential decay form used is as follows: The continuous exploration benefit evaluation can further distinguish between three situations: first visibility, repeated visibility, and high-quality visibility. First visibility front voxels give higher benefits, repeated visibility front voxels are used to improve map confirmation but the benefits gradually decrease, and high-quality visibility requires that distance, angle of incidence, and occlusion conditions all meet the requirements.

[0062] The frontier weights after repeated observation decay can be expressed as: ; in, Represents the frontier voxel Effective weights; This represents the initial information quality weight of the frontier; This indicates the number of times the frontier has been effectively covered within the current planning cycle; This represents the repeated observation attenuation coefficient. The larger the value, the faster the benefit of repeated coverage decreases.

[0063] In practical applications, this invention can pre-organize local leading-edge voxels into spatial hash tables or local KD trees. For each trajectory sampling point, only leading-edge voxels within the sensor's maximum detection range are queried, and occlusion detection is performed using ray casting or voxel stepping algorithms, thereby avoiding a full traversal of all local leading-edge voxels.

[0064] In one embodiment of the present invention, it is determined not only whether the leading voxel is within the sensor's field of view, but also whether it is occupied by an occupied voxel, whether it is within the effective detection range, and whether the laser incident angle is suitable for forming a stable point cloud.

[0065] Field visibility can be expressed as: ; in, This is the overall visibility marker; , , and These are binary indicators, all either 0 or 1, representing whether the effective detection distance, field of view, unobstructed rays, and incident angle quality conditions are met. Only when all four are 1 is the observation considered validly visible.

[0066] 2. The trajectory perception quality score can be expressed as: ; in, The average perceived quality score of the candidate trajectory; Indicates the expected point cloud density quality; Indicates the quality of the observation viewpoint; Indicates the quality of the ranging interval; This indicates the risk of point cloud distortion caused by high-speed translation or rotation. For corresponding non-negative weights; This represents the number of trajectory sampling points.

[0067] Incident angle evaluation is used to avoid situations where, although the trajectory shows the leading edge region, the laser beam is approximately parallel to the surface, leading to sparse point clouds or unstable ranging. Let the unit normal vector of the local surface near the leading edge be... The unit observation direction of the sensor pointing towards this leading edge is If the incident angle mass can be determined by the angle between the two, then the incident angle mass can be determined by the angle between the two.

[0068] The mass of the incident angle can be expressed as: ; in, Represents the frontier voxel At sampling time The mass of the angle of incidence; This represents the vector transpose. The closer the incident angle mass is to 1, the closer the observation direction is to the surface normal direction.

[0069] Point cloud distortion risk can be estimated based on trajectory velocity, angular velocity, and LiDAR scanning cycle. When the UAV has a large displacement or rotation angle within a single frame scan time, the system reduces the perception quality score of that trajectory, and the execution speed can be reduced through adaptive velocity constraints in step 6.

[0070] Step 5: Determine the LIO positioning degradation cost and dynamic timing risk cost.

[0071] 1. Reference Figure 4 To reduce the risk of positioning degradation for UAVs in long corridors, single-plane environments, weakly reflective environments, or sparsely structured environments, this invention statistically analyzes the local geometry along candidate trajectories. Let the k-th candidate trajectory be... l The local point set within a given neighborhood radius around each sampling location is: ; in, Indicates the first Candidate trajectories at sampling time The local point set in the vicinity used for geometric evaluation; Represents the first in the local point cloud A three-dimensional point; The location of the drone at this sampling time; The neighborhood radius is used for evaluating geometric observability.

[0072] ; in, The three-dimensional covariance matrix representing a local point set; Indicates the number of points in the local point set; This represents the centroid of the local point set.

[0073] ; in, , and Let them represent the covariance matrix respectively. Three non-negative eigenvalues ​​arranged from largest to smallest.

[0074] ; in, The score representing the local geometric degradation at the sampling location; and These are the maximum and minimum eigenvalues, respectively; A small positive constant is set to prevent the denominator from being zero. The larger this ratio is, the more the local point cloud tends to be linear or planar, and the more unbalanced the geometric constraints become.

[0075] Combining the degree of local geometric degradation and the uncertainty of state estimation, the localization degradation cost of the candidate trajectory is defined as: ; in, Indicates the LIO localization degradation cost of the candidate trajectory; Indicates the local geometric degradation score; and Representing the normalized position and attitude uncertainties respectively, which can be derived from... and calculate; and For position and orientation covariance sub-blocks; and For normalization; For the corresponding weights.

[0076] In LIO observability evaluation, this invention can also count the number of valid matching points, the number of planar constraints, and the number of edge constraints along the candidate trajectory. If a certain trajectory segment is far from obstacles, but the surrounding point cloud is sparse or the geometric structure is simple, its safe distance meets the requirements, but the positioning risk is high, and it should be penalized in the overall cost.

[0077] The number of valid matching constraints can be expressed as: ; in, This represents the total number of valid matching constraints near the sampling location that can be used for state estimation; , and These represent the number of valid plane, edge, and corner constraints, respectively. If this total is lower than a set threshold... This would increase the positioning degradation cost at the corresponding sampling location.

[0078] In one embodiment of the present invention, for visual-inertial or multi-sensor positioning systems, the geometric observability index can also be replaced by the number of visual features, the uniformity of feature distribution, reprojection residuals, or multi-sensor fusion innovation. The core is to evaluate whether the local trajectory is conducive to stable state estimation, rather than being limited to a specific LIO front-end implementation.

[0079] 2. The dynamic risk prediction module outputs the probability that a spatial location will be occupied by dynamic obstacles in the future. This probability can be obtained from dynamic point cloud segmentation, target tracking, temporal occupancy grid, semantic detection, or historical occupancy change statistics. The dynamic temporal risk cost of a candidate trajectory is defined as: The dynamic temporal risk cost of candidate trajectories is: ; in, Represents the dynamic temporal risk cost of candidate trajectories; It indicates that drones are expected to be available in the future. Reaching the trajectory position The probability that the location is occupied by a dynamic obstacle; Indicates the sampling time interval.

[0080] Furthermore, this invention also utilizes the dynamic obstacle occupancy probability to eliminate trajectories: ; in, Indicates the first A dynamic obstacle in the future moment The predicted location; This represents the dynamic safe distance after taking into account the size of the drone, the size of the obstacle, and the uncertainty of the prediction. This serves as the sampling index for the trajectory. If any sampling point violates this constraint, the trajectory can be directly removed or marked as dynamically infeasible.

[0081] This temporal risk assessment enables the local planner to determine whether a location might be occupied by dynamic obstacles when the drone arrives at it, thus distinguishing it from obstacle avoidance methods that rely solely on the current static map.

[0082] Dynamic obstacle temporal risk prediction can employ short-time constant velocity models, constant acceleration models, target tracking filters, or temporal occupancy grid models. For suspected dynamic points that fail to form stable trajectories, the system can treat them as probabilistic occupancy regions and maintain high uncertainty within a short time window.

[0083] A short-term prediction model for dynamic obstacles is as follows: ; in, Indicates the first A dynamic obstacle at the current moment Location; and These represent the velocity and acceleration estimates, respectively. This represents the predicted time from the current moment. When acceleration cannot be reliably estimated, it can be set as follows: It degenerates into a constant-rate prediction model.

[0084] During the trajectory elimination process, this invention can also consider the uncertainty of dynamic obstacle prediction. If the prediction covariance is large, the dynamic obstacle safety radius expands according to the uncertainty, avoiding the UAV from passing through areas with uncertain predictions but high potential risks.

[0085] Step 6: Combine adaptive velocity, acceleration constraints, and local trajectory optimization objectives based on perceived quality and risk.

[0086] 1. Reference Figure 5 To uniformly reflect the impact of high risk and low perceived quality on local execution speed, this invention first constructs a joint risk quantity: ; The speed scaling factor is defined as: ; The local speed limit is: ; The local acceleration limit can be adjusted synchronously as follows: ; in, Indicates the first The trajectory in the first The combined risk of each sampling point; This indicates the map risk at that location; This indicates the uncertainty in the normalized pose. This indicates the dynamic occupancy risk corresponding to the sampling location; This represents the local perception quality normalized to 0 to 1; the larger the value, the more reliable the perception. Non-negative weights; This is the speed scaling factor; The minimum allowable speed scaling factor; This indicates that the result should be restricted to the given upper and lower bounds; and They represent the first The upper limit of local velocity and the upper limit of local acceleration at each sampling location; and These represent the system's nominal maximum speed and nominal maximum acceleration, respectively.

[0087] Adaptive velocity constraints are used not only in the optimization phase but also in the trajectory execution phase. This invention can use the local velocity upper limit along the trajectory as a time calibration input to re-timing the original trajectory, keeping the trajectory geometry unchanged but automatically extending the execution time in high-risk areas.

[0088] The first time after resetting l The execution time of the segment trajectory should meet the following requirements: ; in, Indicates the first The trajectory starts from the sampling point arrive The execution time is reallocated; the numerator represents the spatial distance between adjacent trajectory sampling positions; This indicates the maximum allowed local speed for this segment. This constraint automatically assigns longer execution times to high-risk or low-perceived-quality segments.

[0089] When the local velocity scaling factor continuously falls below the threshold, it indicates that the overall risk near the current local target is high. At this time, the local planner can return a "local target is temporarily unexecutable" flag to the external global planner, and the external module can decide whether to switch targets.

[0090] In another embodiment of the present invention, the adaptive speed constraint of perception quality can be implemented by a piecewise function, a lookup table function, a neural network strategy, a safety barrier function, or a model predictive control constraint, as long as it can adjust the local execution speed of the UAV according to the local perception quality, dynamic risk, or positioning uncertainty.

[0091] 2. This invention incorporates trajectory time, smoothness, static collision risk, dynamic temporal risk, localization degradation risk, perception quality, and continuous exploration gains into a unified local objective function. The comprehensive cost of candidate trajectories can be expressed as: ; in, This represents the overall cost of the candidate trajectory; For trajectory execution time; This comes at the cost of trajectory smoothness. This is the cost of static collision; This is the cost of dynamic time-series risks; Determine the degradation cost of LIO; To continuously explore benefits; Score the perceived quality; The corresponding non-negative weights are used. The first five terms represent the time / risk / smoothing costs that need to be minimized, while the last two are the benefit terms, hence the negative sign in the objective function.

[0092] The final output trajectory is determined by the following rules: ; in, This represents the optimal local trajectory selected during the current planning cycle; This represents the candidate trajectory that minimizes the overall cost. This represents the set of candidate trajectories after screening based on basic feasibility, static safety, dynamic safety, and dynamic constraints. For the first The combined cost of each candidate trajectory. If the feasible set is empty, then proceed to the replanning or safety control process.

[0093] The weights in the overall optimization objective can be adaptively adjusted according to the task stage. For example, when there are many unknown areas in the early stage of exploration, the weight of continuous exploration benefits is increased; in narrow passages or areas with dense dynamic obstacles, the weight of collision and dynamic risk is increased; and when the uncertainty of positioning increases, the weight of LIO degradation cost is increased.

[0094] The weight adaptive update method of this invention is as follows: ; in, These represent the current LIO degradation, dynamic risk, and continuous exploration benefit weights, respectively. These represent the corresponding initial weights; This indicates local pose uncertainty; This represents the total amount of local dynamic risk. Indicates the degree of unknownness in a local map; This is the corresponding weight adjustment coefficient.

[0095] In terms of the solution method, this invention can first perform discrete scoring on candidate trajectories and select several preferred trajectories, and then continuously optimize the preferred trajectories. Continuous optimization can employ gradient descent, sequential quadratic programming, nonlinear least squares, model predictive control, or other constrained optimization methods.

[0096] Step 7: Rolling execution and online replanning.

[0097] refer to Figure 6 During trajectory execution, this invention employs a rolling time-domain approach to execute the initial phase of the trajectory and periodically updates the local map, dynamic prediction map, and state covariance. If new observations cause significant changes in obstacle distances, dynamic risks, or positioning uncertainties, candidate trajectories are regenerated and optimized again.

[0098] The replanning triggering condition of this invention can be expressed as: ; in, Indicates a redevelopment sign; This means that the event can be triggered as long as there is a sampling moment on the current execution trajectory that satisfies any risk condition; Indicates the dynamic occupancy probability threshold; This represents the minimum permissible obstacle distance threshold. The trace representing the covariance of the current state; Indicates the threshold of state uncertainty; Indicates trajectory tracking error; This represents the maximum allowable tracking error. The dynamic occupancy probability and obstacle distance in the formula are both queried at the sampling position of the current execution trajectory.

[0099] If multiple replanning attempts fail consecutively, this invention outputs safety control suggestions based on the risk level, such as deceleration, hovering, reversing, ascending, returning to the previous safe point, or requesting an external global planner to switch local targets. This process belongs to the local execution layer and does not involve solving the global frontier access order.

[0100] During rolling execution, the entire planned trajectory within the time domain is not executed at once; instead, only the first small segment of control commands is issued. Let the execution segment duration be... T exec ,and T exec <T h Then, after each execution segment is completed or a new risk map is received, the system re-enters the local planning process.

[0101] The scrolling execution window can be represented as: ; in, This represents the actual trajectory segment sent to the flight controller. This represents the current optimal local trajectory; This means only the optimal trajectory from the starting point to the execution time is captured. The front part; This indicates the overall planning time domain, and the execution segment is smaller than the overall planning time domain, so as to continuously update it.

[0102] The safety control strategy can be set to multiple states, including normal execution, deceleration execution, local replanning, hovering, retreating to avoid obstacles, and emergency stop. Different states are jointly determined by risk score, trajectory feasibility, and flight control feedback, thereby avoiding frequent oscillations caused by a single threshold.

[0103] Step 8: Output data structure and module interface.

[0104] The data structure output by this invention can be represented as follows: ; in, This represents the set of output data from the local planner. The optimal local trajectory; and These are local velocity and acceleration constraints, respectively. As a landmark for redevelopment; In a safe control state; Score the perceived quality; To perform risk scoring; This represents the current execution status of the local target.

[0105] The specific data is shown in Table 1: Table 1. Specific output data Then, the local planner sends trajectory control points, time allocation, and local speed limits to the flight control module; it sends sensor field of view, perception quality, and high-risk area prompts along the trajectory to the mapping module; and it returns status feedback to the external global planner, such as local target arrival, local infeasibility, or the need to replace the target. Through this interface, this invention can form a well-defined hierarchical collaborative relationship with the global frontier planning method.

[0106] The output interface can also include trajectory quality diagnostic information for subsequent parameter tuning and implementation verification. The diagnostic information includes the values ​​of each cost item, the number of eliminated trajectories, the location of maximum dynamic risk, the minimum obstacle distance, the location of minimum perceived quality, and the specific reason for triggering replanning.

[0107] The execution risk score can be expressed as: ; in, This indicates the final execution risk score; This represents the maximum dynamic risk level on the optimal trajectory. This represents the maximum map risk on the optimal trajectory; The proximity to a static obstacle can be defined as follows: ,in The minimum static obstacle distance on the optimal trajectory. It is a small positive constant; This represents the localization degradation cost of the optimal trajectory; For the corresponding weights.

[0108] The local planner of this invention can encapsulate trajectory control points, timestamps, velocity constraints, and safety flags into message queues or shared memory structures. If the communication delay exceeds a threshold, the flight control module can automatically reuse the previous safe trajectory segment or enter a hovering strategy, ensuring the reliable execution of the local planning results in the real-time system.

[0109] In another embodiment of the present invention, a UAV local exploration trajectory planning system integrating perception quality and dynamic risk is also provided for executing the above-described UAV local exploration trajectory planning method, comprising: The data acquisition module is used to receive the current local target, local front fragment or candidate viewpoint given by the external global exploration planner, and simultaneously acquire the UAV status, state covariance, local occupancy map, Euclidean distance field, dynamic occupancy prediction results, local front voxel and sensor field of view parameters. The candidate trajectory generation module is used to perform validity checks on the current local target and generate multiple candidate local trajectories with different path shapes, velocity distributions, and observation orientations. The basic feasibility screening module is used to perform basic feasibility screening on the candidate local trajectories. The perception quality evaluation module is used to sample the candidate local trajectories that have passed the screening by time, count the visible frontier of the entire trajectory, and obtain the continuous exploration benefits and trajectory perception quality score. The risk assessment module is used to construct the laser inertial odometry positioning degradation cost and dynamic temporal risk at each trajectory sampling location; The comprehensive optimization module is used to incorporate various costs and benefits into a unified comprehensive cost, and to adaptively adjust the local velocity, acceleration, and trajectory time allocation based on the speed scaling factor calculated according to risk and perceived quality. The rolling execution and replanning module is used to select the trajectory with the minimum comprehensive cost from the feasible candidate trajectories as the current optimal local trajectory, and executes it in a rolling time domain manner. When the replanning trigger condition is met, the candidate generation and optimization are re-executed. The data output module is used to construct the output data set of the local planner and output the corresponding data information to the flight control module, the mapping module and the external global exploration planner respectively.

[0110] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A method for planning the local exploration trajectory of an unmanned aerial vehicle (UAV) that integrates perceived quality and dynamic risk, characterized in that, Includes the following steps: Step 1: Receive the current local target, local front fragment, or candidate viewpoint from the external global exploration planner, and simultaneously acquire the UAV status, state covariance, local occupancy map, Euclidean distance field, dynamic occupancy prediction results, local front voxels, and sensor field of view parameters; unify all types of data into the same coordinate system and planning time domain, and construct the current local planning window; Step 2: Perform a validity check on the current local target, and generate multiple candidate local trajectories with different path shapes, velocity distributions and observation orientations using B-splines, MINCO, kinematic search, motion primitives or multi-topology channels. Step 3: Perform basic feasibility screening on the candidate local trajectories, and check the static obstacle safety distance, terminal accessibility, speed, acceleration, jerk and yaw motion constraints; Step 4: Sample the candidate local trajectories that have passed the screening by time, and calculate the visible front of the trajectory throughout the entire process based on the sensor pose, field of view, effective distance, ray occlusion and incident angle, and attenuate the repeated coverage to obtain the continuous exploration benefit and trajectory perception quality score. Step 5: Statistically calculate the covariance eigenvalues ​​and effective matching constraints of the neighborhood point cloud at each trajectory sampling location, and construct the LIO positioning degradation cost by combining the uncertainty of state estimation; at the same time, calculate the dynamic temporal risk cost of the UAV at the corresponding future time based on the dynamic occupancy probability or dynamic target prediction trajectory. Step 6: Incorporate trajectory time, smoothness, static collision cost, dynamic temporal risk cost, LIO positioning degradation cost, continuous exploration benefits, and perception quality into a unified comprehensive cost, and calculate the speed scaling factor based on map risk, pose uncertainty, dynamic risk, and perception quality to adaptively adjust local velocity, acceleration, and trajectory time allocation. Step 7: Select the trajectory with the minimum comprehensive cost from the feasible candidate trajectories as the current optimal local trajectory, and execute the first part of the trajectory using a rolling time domain method; when the local map, dynamic time series risk, state covariance, or tracking error meet the replanning triggering conditions, re-execute the candidate generation and optimization. Step 8: Construct the output data set of the local planner and output the corresponding data information to the flight control module, the mapping module and the external global exploration planner respectively.

2. The UAV local exploration trajectory planning method according to claim 1, characterized in that, Step 3, the basic feasibility screening, specifically includes: Static safety distance constraints: ; Maximum speed constraint: ; Maximum acceleration constraint: ; Maximum jerk constraint: ; Terminal reachability constraints: ; Yaw rate constraint: ; in, The trajectory position is obtained by weighting each control point and basis function. Indicates the candidate trajectory at time... Distance to the nearest static obstacle Indicates static safety distance. and They represent the first Candidate trajectories in relative time Velocity and acceleration at that point , indicating jerk. , and These represent the maximum permissible speed, maximum acceleration, and maximum jerk, respectively. and Representing local targets respectively The desired position and desired yaw angle; and These represent the allowable terminal position error and yaw angle error, respectively. This represents the angle normalization operation that normalizes the yaw angle difference to the equivalent principal value interval. This indicates the total execution time of the candidate trajectory. and These represent the endpoint position and endpoint yaw angle of the candidate trajectory, respectively. and These represent yaw rate and yaw acceleration, respectively. and These represent the maximum permissible yaw rate and maximum yaw acceleration of the flight control or sensor orientation mechanism, respectively.

3. The UAV local exploration trajectory planning method according to claim 1, characterized in that, In step 4, the formula for calculating the continuous exploration reward is: ； in, This represents the reward for continuous exploration of candidate trajectories. Indicates the first Candidate trajectories, Represents the current local frontier voxel set; Indicates the first A cutting-edge element, For the first Each sampling time, Indicates the trajectory at the sampling time The sensor's field of view; This is an indicator function used to determine whether the leading edge is within the current sensor's field of view; A visible marker indicating the combined effects of factors such as occlusion, distance, and angle of incidence; This represents the effective frontier weight after considering repeated observation decay; The sampling time interval; Effective frontier weights Represented as: ; in, This represents the initial information quality weight of the frontier; This indicates the number of times the frontier has been effectively covered within the current planning cycle; Indicates the repeated observation attenuation coefficient; Visible markers Represented as: ; in, , , and Both are binary indicators of 0 or 1, representing whether the effective detection distance, field of view, unobstructed rays, and incident angle quality conditions are met, respectively. The formula for calculating the trajectory perception quality score is: ； in, The average perceived quality score of the candidate trajectory; Indicates the expected point cloud density quality; Indicates the quality of the observation viewpoint; Indicates the quality of the ranging interval; This indicates the risk of point cloud distortion caused by high-speed translation or rotation. The corresponding non-negative weights; This represents the number of trajectory sampling points.

4. The UAV local exploration trajectory planning method according to claim 3, characterized in that, In step 5, the LIO positioning degradation cost is defined as: ； in, Indicates the LIO localization degradation cost of the candidate trajectory; Indicates the local geometric degradation score; and These represent the normalized position and orientation uncertainties, respectively. For the corresponding weights; Local geometric degradation score The calculation formula is: ； in, and Let these represent the largest and smallest eigenvalues ​​of the local point set covariance matrix, respectively. A small positive constant is set to prevent the denominator from being zero.

5. The UAV local exploration trajectory planning method according to claim 4, characterized in that, In step 5, the formula for calculating the dynamic time-series risk cost is as follows: ； in, Represents the dynamic temporal risk cost of candidate trajectories; It indicates that drones are expected to be available in the future. Reaching the trajectory position The probability that the location is occupied by a dynamic obstacle.

6. The UAV local exploration trajectory planning method according to claim 5, characterized in that, In step 6, the speed scaling factor is defined as: ； in, This is the speed scaling factor. The minimum allowable speed scaling factor, Indicates the first The trajectory in the first The joint risk of each sampling point This indicates that the result should be restricted to the given upper and lower bounds; Joint risk The calculation formula is: ； in, This indicates the map risk at that location; This indicates the uncertainty in the normalized pose. This indicates the dynamic occupancy risk corresponding to the sampling location; This represents the local perception quality normalized to 0 to 1; the larger the value, the more reliable the perception. The weights are non-negative.

7. The UAV local exploration trajectory planning method according to claim 6, characterized in that, In step 6, the unified comprehensive cost of the candidate trajectory is expressed as: ； in, This represents the overall cost of the candidate trajectory; For trajectory execution time; This comes at the cost of trajectory smoothness. This is the cost of static collision; This is the cost of dynamic time-series risks; Determine the degradation cost of LIO; To continuously explore benefits; Score the perceived quality; For the corresponding non-negative weights.

8. The UAV local exploration trajectory planning method according to claim 7, characterized in that, In step 7, the trajectory replanning trigger condition is: ； in, Indicates a redevelopment sign; This means that the event can be triggered as long as there is a sampling moment on the current execution trajectory that satisfies any risk condition; This represents the probability threshold for dynamic obstacle occupancy. This represents the minimum permissible obstacle distance threshold. The trace representing the covariance of the current state; Indicates the threshold of state uncertainty; Indicates trajectory tracking error; This indicates the maximum allowable tracking error.

9. The UAV local exploration trajectory planning method according to claim 8, characterized in that, In step 8, the output data set of the local planner is: ； in, This represents the set of output data from the local planner. The optimal local trajectory; and These are local velocity and acceleration constraints, respectively. As a landmark for redevelopment; In a safe control state; Score the perceived quality; To perform risk scoring; This represents the current execution status of the local target.

10. A UAV local exploration trajectory planning system integrating perceived quality and dynamic risk, used to execute the UAV local exploration trajectory planning method according to any one of claims 1-9, characterized in that, The system includes: The data acquisition module is used to receive the current local target, local front fragment or candidate viewpoint given by the external global exploration planner, and simultaneously acquire the UAV status, state covariance, local occupancy map, Euclidean distance field, dynamic occupancy prediction results, local front voxel and sensor field of view parameters. The candidate trajectory generation module is used to perform validity checks on the current local target and generate multiple candidate local trajectories with different path shapes, velocity distributions, and observation orientations. The basic feasibility screening module is used to perform basic feasibility screening on the candidate local trajectories. The perception quality evaluation module is used to sample the candidate local trajectories that have passed the screening by time, count the visible frontier of the entire trajectory, and obtain the continuous exploration benefits and trajectory perception quality score. The risk assessment module is used to construct the laser inertial odometry positioning degradation cost and dynamic temporal risk at each trajectory sampling location; The comprehensive optimization module is used to incorporate various costs and benefits into a unified comprehensive cost, and to adaptively adjust the local velocity, acceleration, and trajectory time allocation based on the speed scaling factor calculated according to risk and perceived quality. The rolling execution and replanning module is used to select the trajectory with the minimum comprehensive cost from the feasible candidate trajectories as the current optimal local trajectory, and executes it in a rolling time domain manner. When the replanning trigger condition is met, the candidate generation and optimization are re-executed. The data output module is used to construct the output data set of the local planner and output the corresponding data information to the flight control module, the mapping module and the external global exploration planner respectively.

Images