A risk perception adaptive OctVox laser inertial mapping method for unmanned aerial vehicle autonomous obstacle avoidance

Abstract

The application discloses a risk perception adaptive OctVox laser inertial mapping method for unmanned aerial vehicle autonomous obstacle avoidance, and belongs to the technical field of flight safety control. The method comprises the following steps: acquiring input data of an onboard sensor of an unmanned aerial vehicle, and predicting the state of the unmanned aerial vehicle; constructing a hash voxel map; calculating the flight risk score of each voxel, and dividing the voxels into voxels of different risk levels based on the risk score; configuring different sub-voxel division scales and representative point update strategies for the voxels of different risk levels; obtaining candidate neighbor points through neighbor searching, and constructing point-to-plane residuals based on the candidate neighbor points to update the unmanned aerial vehicle pose; feeding back a future trajectory to a mapping module to form a closed loop between positioning mapping and autonomous obstacle avoidance; and outputting a structured mapping result. The mapping method can significantly enhance the mapping expression accuracy of a flight safety key area under limited computing resources, and improve the autonomous obstacle avoidance capability of the unmanned aerial vehicle in a complex environment.

Description

Technical Field

[0001] This invention relates to the field of flight safety control technology, and in particular to a risk perception-adaptive OctVox laser inertial mapping method for autonomous obstacle avoidance of unmanned aerial vehicles (UAVs). Background Technology

[0002] With the development of low-altitude UAV inspection, indoor and outdoor autonomous exploration, 3D reconstruction of complex environments, and intelligent navigation of unmanned systems, UAVs need to rely on onboard sensors to perceive their surroundings in real time in unknown or semi-unknown environments to complete self-localization, map building, obstacle recognition, and safe path planning. LiDAR inertial odometry, by fusing LiDAR geometric observations and IMU high-frequency motion priors, can provide relatively stable pose estimation in GPS-restricted or GPS-free environments, and is a fundamental perception module in UAV autonomous obstacle avoidance systems.

[0003] Existing lidar inertial odometry typically includes steps such as IMU state prediction, point cloud motion distortion compensation, point cloud downsampling, scan-to-map matching, filtering or optimization updates, and map maintenance. Among these, map structure and nearest neighbor search strategies have a critical impact on system real-time performance and accuracy. Structures such as KD-Tree, incremental KD-Tree, hash voxel maps, statistical voxel maps, and octree maps can all be used to store local maps and support point-to-point or point-to-plane data association.

[0004] To reduce the computational and storage burden on airborne embedded platforms, compact LIO (LiDAR-Inertial Odometry) methods have proposed further dividing hash voxels into several sub-voxels and storing representative points within each sub-voxel. This reduces map redundancy points through fixed density constraints and improves nearest neighbor search efficiency. The OctVox (hash voxel map) structure in Super-LIO-like methods uses a method where each voxel stores a maximum of eight representative points from sub-voxels, and combines this with heuristic KNN search to improve matching efficiency.

[0005] However, for autonomous obstacle avoidance by drones, maps are not just for location matching. Areas near the drone's future flight path, obstacle boundaries, narrow passages, dynamic obstacles, and areas with high positioning uncertainty are of higher value for flight safety; while areas far from the flight path, far from obstacles, with stable geometry, and unvisited for a long time contribute less to current obstacle avoidance decisions. If a globally uniform resolution, a uniform number of representative points, and a uniform update strategy are still used, it is easy to encounter problems such as insufficient representation of key obstacle boundaries or excessive consumption of computing resources in non-critical areas.

[0006] Furthermore, real-world drone flight scenarios often involve complex situations such as corridors, door frames, tree branches, cables, fences, railings, moving pedestrians, mobile devices, weak glass reflections, and partial occlusions. Simply relying on the nearest map point for scanning and matching may misuse dynamic points, degenerate planes, or unstable obstacle boundaries for positioning updates; simply passing the positioning map directly to the planner also makes it difficult to distinguish between stable structures, dynamic obstacles, and high-risk flight areas.

[0007] Therefore, there is an urgent need to design an adaptive OctVox map construction method that combines laser inertial mapping, flight risk assessment, and local obstacle avoidance requirements. Summary of the Invention

[0008] The technical problem to be solved by this invention is: 1. How to identify spatial areas with a high impact on obstacle avoidance safety based on the UAV's future flight trajectory, obstacle distance, and safety radius during real-time mapping using a laser inertial odometry system.

[0009] 2. How to improve the map representation accuracy near flight corridors, obstacle boundaries, and narrow passage areas without significantly increasing the overall computational load and memory usage.

[0010] 3. How to use the pose covariance, local point cloud geometric degradation degree, and dynamic occupancy change output by IESKF (Iterative Error State Kalman Filter) or other state estimators to perform risk scoring and hierarchical management of voxel maps.

[0011] 4. How to adaptively adjust the OctVox sub-vox division scale, number of representative points, incremental average update coefficient, map retention priority, and cache eviction strategy based on the voxel risk level.

[0012] 5. How to introduce risk level, stability and dynamic attributes in the process of nearest neighbor search and point-to-plane residual construction so that stable structures are given priority for localization and high-risk obstacle boundaries are given priority for obstacle avoidance.

[0013] 6. How to organize the positioning and mapping results into a stable positioning layer, a dynamic obstacle avoidance layer, and a risk marking layer, and output a directly usable local risk map and flight safety corridor constraints to the local path planner.

[0014] To solve the above-mentioned technical problems, the present invention provides the following technical solution: An adaptive OctVox laser inertial mapping method for risk perception in autonomous obstacle avoidance of UAVs is proposed. This method takes UAV-borne lidar point cloud, IMU measurement data, lidar and IMU extrinsic parameters, UAV safety radius, and future trajectory output by local planner as input. First, the method completes IMU state prediction, point cloud distortion correction, and scan-to-map matching through laser inertial odometry to obtain real-time UAV pose estimation and local point cloud map.

[0015] In terms of map representation, this invention is based on a hash voxel map, configuring each voxel as a variable-precision OctVox unit. Unlike a regular OctVox with fixed eight voxels, this application maintains attributes such as risk level, representative point set, observation count, stability, dynamic occupancy marker, normal vector or covariance, and recent access time for each voxel. The system calculates a voxel flight risk score based on the distance from the voxel center to the future flight trajectory, the relationship between the voxel and the obstacle boundary, the pose estimation covariance, the degree of local geometric degradation, and the dynamic occupancy change, and classifies the voxels into three categories: low risk, medium risk, and high risk accordingly.

[0016] For low-risk voxels, this invention employs a central representative point, low-frequency updates, or a larger merging threshold to compress the map size far from the flight path area; for medium-risk voxels, a conventional 2×2×2 eight-sub-voxel representation is used to maintain the basic geometric coverage required for real-time positioning; for high-risk voxels, a finer 4×4×4 sub-voxel representation is used, with each sub-voxel having multiple representative points or an enhanced representation with additional normal vectors / covariance to preserve the geometric details of obstacle boundaries, narrow passages, and dynamic obstacles near the flight corridor.

[0017] In terms of nearest neighbor search, this invention introduces voxel risk level and stability factor on top of the lower bound of geometric distance in ordinary heuristic KNN, prioritizing the retrieval of candidate points that are more valuable for localization and obstacle avoidance. For stable static structural points, their weight in the point-to-plane residual is increased; for dynamic or unstable points, their impact on state estimation is reduced, but their instantaneous occupancy information is retained in the dynamic obstacle avoidance layer. This avoids dynamic obstacles interfering with localization while ensuring that the planner can obtain dynamic obstacle risks in a timely manner.

[0018] In terms of planning coupling, this application transforms the future trajectory or candidate trajectory set output by the local planner into a flight safety corridor and improves the risk score of voxels near the corridor, enabling the map update to proactively enhance the geometric representation of areas that may be traversed in the future. The updated risk-adaptive OctVox map then outputs local obstacle boundaries, dynamic occupancy, risk levels, and uncertainty markers to the planner, forming a closed loop between localization mapping and autonomous obstacle avoidance.

[0019] Ultimately, this application outputs structured results including the UAV's real-time pose, stable positioning layer map, dynamic obstacle avoidance layer map, voxel risk layer, local flight safety corridor, obstacle boundary points, and risk voxel list, which can be directly used as input for local path planning, collision detection, velocity constraint generation, and safety control strategy switching.

[0020] Compared with the prior art, the present invention discloses the following technical effects: 1. This invention incorporates the obstacle avoidance task requirements of UAVs into the LIO map update process through a flight risk scoring mechanism, so that map accuracy is no longer evenly distributed, but is preferentially allocated to future flight corridors, obstacle boundaries and areas with high positioning uncertainty, thereby improving the environmental representation capability of key areas.

[0021] 2. This invention adopts risk-based adaptive OctVox representation, which maintains compact storage in low-risk areas and locally increases resolution or the number of representative points in high-risk areas. This can improve the retention of narrow channels, boundary obstacles and small obstacles while keeping the overall computational load under control.

[0022] 3. This invention incorporates state covariance, local geometric degradation, and dynamic occupancy changes into the voxel risk score, which can proactively enhance the map of key areas when positioning reliability decreases or scene geometric constraints are insufficient, thereby improving the robustness of laser inertial odometry in complex UAV flight environments.

[0023] 4. In the process of nearest neighbor search and residual construction, this invention distinguishes between stable positioning points and dynamic obstacle avoidance points, which can reduce the interference of dynamic obstacles on LIO state updates, while retaining their safety constraint value for local path planning.

[0024] 5. This invention forms a map accuracy adjustment mechanism driven by planning needs through future trajectory feedback, which enables a closed loop to be formed between LIO mapping, obstacle detection and local obstacle avoidance, reducing the problem of insufficient expression of key areas caused by traditional module separation.

[0025] 6. The mapping method of the present invention is applicable to resource-constrained environments such as NVIDIA OrinNX, Jetson series, ARM embedded computing platforms and airborne small computing units, and can achieve real-time positioning, compact mapping and safe obstacle avoidance under limited computing power. Attached Figure Description

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

[0027] Figure 1 This is a schematic diagram of the risk perception adaptive OctVox laser inertial mapping method provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of the voxel flight risk score provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of the risk perception adaptive OctVox expression method provided in an embodiment of the present invention; Figure 4 This is a schematic diagram of risk-guided nearest neighbor search and residual weighting provided in an embodiment of the present invention; Figure 5 This is a schematic diagram illustrating the linkage between the mapping module and the local obstacle avoidance planning of the UAV provided in an embodiment of the present invention. Figure 6 This is a diagram illustrating the architecture of the risk-aware adaptive OctVox laser inertial mapping system provided in an embodiment of the present invention. Detailed Implementation

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

[0029] This invention provides a risk perception-adaptive OctVox laser inertial mapping method for autonomous obstacle avoidance in unmanned aerial vehicles (UAVs), such as... Figure 1 As shown, it includes the following steps: Step 1: Acquire input data from the UAV's onboard sensors. Input data includes laser point cloud sequences, IMU measurement data, lidar and IMU extrinsic parameters, the UAV's safe radius, and the future trajectory output by the local planner.

[0030] In this embodiment, the UAV is equipped with a lidar, an inertial measurement unit (IMU), an onboard computing unit, and a flight control system. The lidar can be a LivoxMID-360, LivoxMID-70, Ouster, Velodyne, Hesai, or other mechanical / solid-state lidar; the IMU can be integrated into the flight control system, lidar, or a standalone IMU module. System inputs include a lidar point cloud sequence, IMU acceleration and angular velocity sequences, extrinsic parameters of the lidar and IMU, the UAV's airframe envelope size or safety radius, the future trajectory output by the local planner, and, if necessary, the speed, control mode, and safety status provided by the flight control system.

[0031] Let the laser point cloud in the k-th frame be... ,in This represents the point coordinates in the lidar coordinate system. Indicates a timestamp. This indicates attributes such as reflection intensity, scan line number, or echo information. This represents the number of points in the laser point cloud of the k-th frame. Let the IMU measurement be... ,in and These represent acceleration and angular velocity, respectively, and j represents the IMU sampling number. Let represent the timestamp of the j-th IMU sample. Let the future trajectory output by the local planner at the current time be . ,in, Includes the position, velocity, timestamp, or desired attitude of future trajectory points, where M represents the number of future trajectory points.

[0032] First, time synchronization is performed on the data from the lidar, IMU, flight controller, and planner, and then based on the external parameters of the lidar and IMU... Unify the point cloud to the IMU coordinate system or the global coordinate system. This represents the extrinsic parameter transformation of the laser radar to the IMU coordinate system. Represents the rotation matrix. This represents the translation vector. If using a ROS or ROS2 system, sensor synchronization can be achieved through timestamp nearest neighbor matching, message synchronizers, or hardware PPS triggering; if time offset exists, it can be compensated for through offline calibration or online estimation.

[0033] Step 2: Predict the UAV status based on IMU measurement data and perform motion distortion compensation on the laser point cloud sequence to obtain a distortion-free point cloud.

[0034] In this embodiment, an Iterative Error State Kalman Filter (IESKF) is used as the state estimator for the laser inertial odometry. The system state at time t can be expressed as: ; in, This represents the rotation matrix from the IMU coordinate system to the global coordinate system. This indicates the position of the IMU in the global coordinate system. Indicates speed, and These represent the zero bias of the accelerometer and gyroscope, respectively. This represents the gravity vector in the global coordinate system. Represents the transpose of a vector or matrix.

[0035] Between two frames of laser point clouds, IMU measurements are used to perform high-frequency prediction of the state, and the relative motion of each laser point at the sampling time relative to the current frame reference time is obtained through median integration, pre-integration, or other inertial integration methods. For each laser point... After performing motion distortion compensation, the point at the reference time of the current frame is obtained. Then, based on the prior state, transform to the global coordinate system: ; The distortion-reduced point cloud is then sampled by center sampling, voxel sampling, or curvature-based sampling to obtain the set of points to be matched. This point set is used for two purposes: firstly, to perform scan-to-map matching with the current adaptive OctVox map, and secondly, to update the representative points, stability, and risk attributes of the corresponding voxels in the map.

[0036] Furthermore, in another embodiment of the present invention, the state estimator may employ factor graph optimization, sliding window optimization, or other laser-inertial fusion frameworks, with the risk-aware adaptive OctVox module embedded therein as a map maintenance and data association module.

[0037] Step 3: Construct a hash voxel map as the basic index structure of the map, where each voxel is configured as a variable precision OctVox cell and maintains attributes including risk level, representative point set, stability, dynamic occupancy marker, normal vector or covariance, and last access time.

[0038] Specifically, this application uses a hash voxel map as the basic index structure for a global or local map. Let the map... Composed of several voxels Composition, voxel side length is , Represents the point coordinates in the global coordinate system, with voxel key values. Each voxel stores a voxel center. Risk level Risk Score Last visit time Representative point set Occupied state Stability Geometric features and cache priority .

[0039] In normal mode, each voxel follows Divided into eight sub-voxels, each sub-voxel stores a representative point. and observation count For those who fall into the first New observation points for individual voxels If the sub-voxel is not initialized, then set , If already initialized, perform incremental average update: ; in, To update the coefficients, This represents the maximum cutoff value for the number of points represented by the child voxel. In one implementation, In another embodiment, The settings can be adaptively adjusted based on the voxel risk level. For high-risk obstacle boundary voxels, a smaller merging threshold and a lower mean fusion intensity can be used to reduce excessive smoothing of the boundary; for low-risk areas, a larger merging threshold can be used to reduce the number of map points.

[0040] To support subsequent risk calculations, each voxel can also maintain auxiliary attributes such as the local point cloud covariance matrix, normal vector, plane fitting residual, occupancy probability, changes in recent multi-frame observations, and whether it is located within the flight safety corridor.

[0041] Step 4: Calculate the flight risk score for each voxel, and classify the voxels into three types: low risk, medium risk, and high risk based on the risk score; like Figure 2 As shown, this invention establishes a voxel flight risk score on a base OctVox map. For the first... Individual factors Its risk score It is obtained by weighting the risks of trajectory approach, obstacle boundary, pose uncertainty, local geometric degradation, and dynamic occupancy: ; in, The weights are non-negative and satisfy the following conditions: , m Each of the five risk categories corresponds to a weight that can be set based on the type of drone, flight speed, sensor field of view, planner safety margin, and application scenario. It can also be adjusted through experimental calibration or online adaptive adjustment.

[0042] Trajectory proximity risk According to the voxel center To future flight trajectory Determining the minimum distance: ; in, Represents trajectory points Positional components in The spatial decay scale representing the risk of approaching a trajectory is defined by the Norm, a vector norm used to represent the straight-line distance between two points in three-dimensional space. This risk term is higher when the voxel is located within a flight corridor that the UAV is likely to traverse in the near future; it decays rapidly as the voxel moves away from the trajectory. For multiple candidate trajectories, the minimum distance among all candidate trajectories can be used, or a weighted average can be calculated based on trajectory cost or selection probability.

[0043] Obstacle boundary risk Determined based on the distance from the obstacle representation point within the voxel to the future trajectory or the envelope of the UAV body. If Less than the safe radius of the drone Or the radius of collision expansion If the distance is slightly greater than the safe radius, the voxel is marked as a high-risk voxel; if the distance is slightly greater than the safe radius, an exponential or linear decay function is used to assign it a medium risk.

[0044] Position uncertainty risk Covariance Estimation Based on State The position and orientation sub-blocks within are determined. For example, one could take... , This means restricting the value to a given range of upper and lower bounds. and Let represent the normalized scaling parameters for position covariance and attitude covariance, respectively. Represents the trace of a matrix. This represents the position covariance sub-block in the state estimation covariance matrix. This represents the attitude covariance sub-block in the state estimation covariance matrix. As the uncertainty of UAV positioning increases, the system increases the risk level of voxels near the current field of view and future trajectory, prompting the map to retain more geometric constraints in key areas.

[0045] Local geometric degradation risk Determined based on the eigenvalues ​​of the covariance matrix of the voxel neighborhood point cloud. Assume the eigenvalues ​​satisfy... Then a geometric degradation index can be defined. or , To prevent small positive constants with a denominator of zero. The risk of degradation increases when the neighborhood structure is close to planar, linear, or has insufficient observability.

[0046] Dynamic occupation risk The classification is based on changes in voxels' occupancy status within a continuous time window, displacement of representative points, observation consistency, and velocity estimation. If a voxel repeatedly appears and disappears within a short period, or if the positional change of its representative point exceeds a threshold, it is marked as a dynamic or suspected dynamic voxel. Dynamic voxels have reduced weight in the localization layer but retain higher risk in the obstacle avoidance layer.

[0047] Then based on the voxel risk score Voxels are classified into three categories: low risk, medium risk, and high risk. ; in, and This is the risk level threshold. The threshold can be a fixed value or dynamically adjusted based on current CPU load, map capacity, flight speed, or planner safety level. For example, when onboard computing resources are strained, the threshold can be increased. To reduce the number of high-precision voxels; when the drone enters narrow passages or flies at high speeds, it can reduce... To enhance maps of key areas in advance.

[0048] Step 5: Configure different sub-voxel division scales and representative point update strategies for voxels of different risk levels; For low-risk voxels This invention can employ a single representative point, a larger sub-voxel size, a larger merging threshold, or a low-frequency update strategy to retain only coarse geometric information far from the current flight mission area. For medium-risk voxels... This invention employs conventional methods. OctVox expression in octet voxels ensures stable spatial coverage for localization matching. For high-risk voxels... The present invention adopts Sub-voxels, smaller voxel sizes, multiple points represented per sub-voxel, boundary point preservation or additional covariance / normal vector enhancement representation (see attached document). Figure 3 .

[0049] In another embodiment of the invention, high-risk voxels may not be used. Instead of using child voxels, local octrees, local small voxel blocks, multi-representation point clusters, planar fragments, edge fragments, or Gaussian distribution parameters can be used to represent key regions, as long as they can enhance the representation of key regions based on flight risk scores.

[0050] In this invention, high-risk voxels do not immediately change the parent voxel hash key; instead, a second-level child voxel index is added within the parent voxel to maintain the stability of the hash table structure. For voxel interior points... Its high-risk secondary daughter voxel index can be represented as: ,in, In the formula, The origin of voxels, This represents the side length of the parent voxel in the underlying hash voxel map. The edge length is set for high-risk sub-voxels. This approach avoids global rehashing and only adds a limited number of representative points within the high-risk voxels.

[0051] To prevent the number of high-risk voxels from growing indefinitely, this invention also sets a global map capacity and an upper limit on the proportion of high-risk voxels. When the number of high-risk voxels exceeds the threshold, the cache priority is calculated based on factors such as voxel risk score, most recent access time, distance from the current trajectory, and whether it is still being used by the planner, prioritizing the retention of voxels related to flight safety.

[0052] When a new point cloud frame arrives, the distortion-free point cloud is transformed to the global coordinate system, and its voxel and sub-voxel are determined based on the point coordinates. For each voxel to be updated, the risk score is first updated based on the current planned trajectory and state covariance; then, the corresponding representative point insertion and fusion strategy is selected based on the risk level.

[0053] For low-risk voxels, if the distance between the new point and existing representative points is less than the merging threshold... If the threshold is exceeded but the voxel is not located within the flight safety corridor, the point can be ignored or only the occupancy probability can be updated. For medium-risk voxels, incremental averaging is performed as with regular OctVox sub-voxels. For high-risk voxels, if the new point is located at an obstacle boundary or a location with significant local curvature, it is retained as an independent representative point or a smaller threshold is used. Fusion is performed to prevent minor obstacles from being eliminated by mean square.

[0054] In one implementation, obstacle boundary points are determined based on the residual from the point to the local fitting plane, the neighborhood curvature, and the occupancy change. If the point... satisfy or local curvature If the value of a point is positive, then the sub-voxel to which that point belongs is marked as a boundary enhancement sub-voxel, and its retention priority in the map cache is increased. Here, n represents the normal vector of the local fitting plane, and q represents the reference point on that plane. Indicates the plane residual threshold. Indicates local curvature. This represents the curvature threshold.

[0055] Meanwhile, this invention maintains stability for each representative point. If a representative point is repeatedly observed across multiple frames and its positional change is less than a threshold, the stability is increased; if the representative point frequently appears, disappears, or undergoes significant displacement, the stability is decreased and it is marked as dynamic or suspected dynamic. The stability update can be expressed as: ; in, For smoothing coefficients, This indicates whether the current observation is consistent with historical representative points. Stability is used for subsequent positioning residual weights, dynamic obstacle avoidance layer updates, and map elimination strategies.

[0056] Step 6: For the points to be matched in the point cloud, perform risk-guided nearest neighbor search based on voxel risk level, representative point stability and dynamic attributes to obtain candidate nearest neighbor points, and construct point-to-plane residuals based on the candidate nearest neighbor points to update the UAV pose. Specifically, during the map matching phase, for the points to be matched in the current frame... First, the query voxel is determined based on its global prior location, and then searched from the adaptive OctVox map. The proposed candidate nearest neighbors are calculated. While conventional heuristic KNN primarily relies on the lower bound of the geometric distance between sub-voxels for candidate traversal, this invention further incorporates voxel risk level, representative point stability, and dynamic attributes into the search priority.

[0057] In this embodiment, candidate representative points The effective distance is defined as: ; in, Risk score for the voxel where the candidate point is located. To represent the stability of the point, For dynamic tagging, , and This is an adjustment coefficient. For positioning updates, dynamic points can be directly removed or their effective distance increased; for obstacle avoidance output, dynamic points remain in the dynamic obstacle avoidance layer.

[0058] In obtaining After selecting candidate points, the system performs local plane fitting to obtain the plane normal vector. On the surface Construct the point-to-plane residual: ; To reduce the impact of unreliable voxels on state estimation, this invention also assigns weights to each residual. : ; in, Represents the distance consistency weight. This represents the stability weight of the representative point. Represents the weight of local geometric observability. Indicates the dynamic point penalty weight. This represents the weighting for adjusting for state uncertainty. In IESKF updates, the residual covariance can be set to... This makes highly reliable static structures contribute more to state updates, while dynamic or degenerate candidate points contribute less. Among these, This represents the weighted effective observation noise covariance. This represents the covariance of the basic observation noise. To prevent small positive constants with a denominator of zero.

[0059] like Figure 4 As shown, the risk-guided nearest neighbor search of the present invention does not simply favor high-risk points, but uses different weights in the localization layer and the obstacle avoidance layer respectively: stable static structures are given priority for localization matching, high-risk obstacle boundaries are given priority for obstacle avoidance representation, and dynamic voxels reduce the localization weight but retain the safety risk.

[0060] Step 7: Separate maintenance of dynamic obstacles and stable positioning layer.

[0061] To simultaneously satisfy both positioning stability and obstacle avoidance sensitivity, this invention divides the OctVox map results into a stable positioning layer. and dynamic obstacle avoidance layer Among them, the stable positioning layer It primarily stores representative points that are consistent across multiple consecutive frames, have reliable geometric structures, and exhibit minimal dynamic changes, for use in map matching and filtering updates after scanning; dynamic obstacle avoidance layer. It primarily stores dynamic points that appear within a short period of time, suspected dynamic points, suddenly appearing obstacle boundaries, and low-stability occupied voxels for local path planning and safety decisions.

[0062] In this embodiment, for any voxel Construct its stability index : ; in, Voxel representation Multi-frame observation consistency, Indicates local geometric reliability. Indicates the intensity of historical continuous observation. Indicates the degree of dynamic occupancy change. , , and These are the corresponding weighting coefficients. The above formula allows for the integration of continuous observation, geometric reliability, historical stability, and dynamic changes into the voxel stability evaluation.

[0063] In one implementation, the degree of dynamic occupation change It can be calculated based on changes in voxel occupancy. Let voxels be... At any moment The occupied state is The occupied state at the previous moment or historical reference moment is Then we have: ; in, Voxels Whether the point cloud is occupied by laser points, the probability of occupancy, or the confidence level of occupancy are all represented by these parameters. For time intervals. When When it is larger, it indicates a voxel. The occupancy state changes significantly, which may correspond to dynamic obstacles, temporary obstacles, or unstable observations.

[0064] When voxels When the stability meets the threshold requirement and the degree of dynamic change is low, it is added to the stable positioning layer. : ; in, As the stability threshold, The threshold is dynamically changing. Stable positioning layer. The voxels in the data can participate in nearest neighbor search, point-to-plane constraint construction, and filtered observation updates to ensure the positioning stability of the laser inertial odometry.

[0065] When voxels When the dynamic variation is high, or the stability is below the threshold, it is added to the dynamic obstacle avoidance layer. : ; Dynamic obstacle avoidance layer Voxels in the local planner may not necessarily participate in localization updates, but they can serve as inputs for obstacle detection, safe distance constraints, and trajectory replanning. Thus, suspected dynamic points can be removed or downweighted from localization constraints while still being retained as obstacle avoidance sensing objects.

[0066] For suspected dynamic voxels, this application does not immediately remove them from the map, but rather determines their stability. Degree of dynamic change and the future flight safety corridor The relationships are processed hierarchically. Let voxels be... The risk score in the dynamic obstacle avoidance layer is: Then it can be updated according to the following rules: ; in, Reserved value for high risk. This is the risk attenuation coefficient. If a suspected dynamic voxel enters the future flight safety corridor... Even if the voxel does not participate in the localization update, it will still be in the dynamic obstacle avoidance layer. Retaining a higher risk score This triggers the planner to recalculate the trajectory, reduce flight speed, or enter a hovering strategy; if the suspected dynamic voxel is far from the future flight safety corridor... Then its risk score changes over time according to a coefficient. The effect diminishes, and the obstacle avoidance layer is removed once the obstacle falls below a set threshold. Delete it.

[0067] This layered maintenance method avoids the problem of "the planner being unable to perceive dynamic obstacles after removal" in traditional dynamic point removal methods, and also avoids pose estimation jitter caused by dynamic obstacles as localization constraints. This is achieved by stabilizing the localization layer. and dynamic obstacle avoidance layer By separating the two, this application can improve the obstacle avoidance sensitivity of UAVs to sudden and dynamic obstacles while maintaining the stability of laser inertial positioning.

[0068] Furthermore, during extended autonomous drone flights, the map size continuously increases. This invention also sets a maximum voxel capacity in the hash voxel map and employs a risk-aware caching strategy instead of a simple Least Recently Used (LRU) strategy. For each voxel, a caching priority is assigned. It can be represented as: ; in, Indicates voxel risk score, Indicates recent access frequency. Indicate whether it is located in a historical or candidate flight corridor. This indicates the time since the last visit. This represents the distance from the voxel to the drone's current position or the center of a local map. This is the weighting coefficient used in cache priority calculation. The system prioritizes eliminating voxels that are low-risk, far from the current flight area, have not been accessed for a long time, and are not near the candidate trajectory; for high-risk voxels, even if they are temporarily not in the current field of view, they can be retained for a certain period of time to avoid losing critical obstacle information when the drone returns or replans.

[0069] In one implementation, when CPU load or memory usage exceeds a threshold, the intensity of low-risk voxel merging is automatically increased, the upper limit of high-risk proportion is reduced, or the local map radius is reduced; when the drone speed increases or enters a narrow passage, the high-risk threshold is reduced, the future trajectory prediction time window is increased, or the priority of obstacle boundary voxel retention is increased.

[0070] Step 8: Feed back the future trajectory output by the local planner to the mapping module, update the voxel risk score according to the future trajectory, and output obstacle constraint or replanning trigger signal to the local planner to form a closed loop between localization mapping and autonomous obstacle avoidance. Specifically, such as Figure 5 As shown, this invention feeds back the future trajectory output by the local path planner to the mapping module. The local planner can employ RRT, , , This invention employs various planning methods, including B-spline trajectory optimization, MPC, TEB, MINCO, FUEL-type exploratory planners, and other planning approaches. The invention does not limit the specific planning algorithm; any local planner capable of outputting trajectory points, candidate trajectories, or safe corridors for a future period is acceptable.

[0071] The mapping module is based on the trajectory Calculate flight corridor : ; in, This represents any point in three-dimensional space that needs to be determined. The safety threshold for the flight corridor can be determined based on the drone's radius, positioning error, control error, and safety margin. (Falling into...) Inside or near The boundary voxels automatically increase the trajectory approach risk. If a high-risk voxel simultaneously satisfies the obstacle boundary conditions, then obstacle constraints are output to the planner; if a dynamic voxel enters... This will trigger a replanning, deceleration, or hovering flag.

[0072] The planner calculates collision cost and trajectory feasibility based on obstacle boundaries, dynamic occupancy, and uncertainty markers output by the risk adaptive map, and feeds the collision cost and trajectory feasibility back to the mapping module to re-plan the trajectory, forming a closed loop of trajectory prediction, risk scoring, map enhancement, and obstacle avoidance planning.

[0073] Step 9: Output the structured mapping results.

[0074] The final risk perception mapping results output by this invention are shown in Table 1: Table 1: Final Structured Mapping Results

[0075] The above outputs can be sent to the local path planner and safety control module via ROS / ROS2 topics, shared memory, DDS messages, file interface, or flight control communication interface. The output risk map can be maintained in three-dimensional voxel format or converted into a two-dimensional local cost map, ESDF, occupancy grid, or point cloud obstacle boundary.

[0076] The mapping method provided by this invention can be applied to autonomous navigation systems composed of mechanical lidar, solid-state lidar, or hybrid scanning lidar and inertial measurement unit mounted on UAVs (see attached diagram). Figure 6 This system can also be extended to real-time positioning, mapping, and obstacle avoidance scenarios for mobile robots, unmanned vehicles, inspection robots, integrated indoor and outdoor mapping platforms, disaster relief robots, low-altitude inspection drones, and resource-constrained embedded platforms. The system includes: The sensor acquisition module is used to acquire data from the onboard sensors of the UAV. The LIO front-end module is connected to the sensor acquisition module and is used to perform state prediction based on IMU measurement data and to perform motion distortion compensation on the lidar point cloud to obtain a distortion-free point cloud. The risk assessment module, connected to the LIO front-end module, is used to calculate the flight risk score for each voxel; An adaptive map management module, connected to the risk assessment module, is used to construct a hash voxel map as the basic map index structure and to divide voxels into three levels: low-risk voxels, medium-risk voxels, and high-risk voxels according to the flight risk score. The matching and filtering update module, connected to the adaptive map management module, is used to obtain candidate nearest neighbor points in the nearest neighbor search of the scanned map match, and construct point-to-plane residuals based on the candidate nearest neighbor points to update the UAV pose. The local planning feedback module, connected to the matching and filtering update module, is used to feed back the future trajectory output by the local planner to the risk assessment module to update the flight safety corridor and voxel risk score. The safety control interface module is connected to the local planning feedback module and is used to output structured mapping results.

[0077] 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 risk perception-adaptive OctVox laser inertial mapping method for autonomous obstacle avoidance of unmanned aerial vehicles (UAVs), characterized in that, Includes the following steps: Step 1: Acquire input data from the UAV's onboard sensors. The input data includes laser point cloud sequences, IMU measurement data, external parameters of the lidar and IMU, the UAV's safe radius, and the future trajectory output by the local planner. Step 2: Predict the UAV status based on the IMU measurement data, and perform motion distortion compensation on the laser point cloud sequence to obtain a distortion-free point cloud; Step 3: Construct a hash voxel map as the basic index structure of the map, where each voxel is configured as a variable precision OctVox cell and maintains attributes including risk level, representative point set, stability, dynamic occupancy marker, normal vector or covariance, and last access time. Step 4: Calculate the flight risk score for each voxel, and classify the voxels into three types: low risk, medium risk, and high risk based on the risk score; Step 5: Configure different sub-voxel division scales and representative point update strategies for voxels of different risk levels; Step 6: For the points to be matched in the point cloud, perform risk-guided nearest neighbor search based on voxel risk level, representative point stability and dynamic attributes to obtain candidate nearest neighbor points, and construct point-to-plane residuals based on the candidate nearest neighbor points to update the UAV pose. Step 7: Divide the map results into a stable positioning layer and a dynamic obstacle avoidance layer, and maintain them separately. The stable positioning layer is used for map matching and filtering updates, and the dynamic obstacle avoidance layer is used for local path planning and safety decisions. Step 8: Feed back the future trajectory output by the local planner to the mapping module, update the voxel risk score according to the future trajectory, and output obstacle constraint or replanning trigger signal to the local planner to form a closed loop between localization mapping and autonomous obstacle avoidance. Step 9: Output the structured mapping results.

2. The risk-aware adaptive OctVox laser inertial mapping method according to claim 1, characterized in that, In step 4, the formula for calculating the flight risk score is as follows: ； in, Indicates the first Individual factors The corresponding risk score, Non-negative weights To mitigate the risk of trajectory proximity, For obstacle boundary risks, Due to pose uncertainty risk, For the risk of local geometric degradation, To dynamically occupy risks.

3. The risk-aware adaptive OctVox laser inertial mapping method according to claim 2, characterized in that, Trajectory close to risk According to the voxel center To future flight trajectory minimum distance Sure: ； in, Represents trajectory points Positional components in The spatial decay scale representing the risk of trajectory approaching its target; Obstacle boundary risk Determined based on the distance from the obstacle representation point within the voxel to the future trajectory or the envelope of the UAV body; Position uncertainty risk Covariance is estimated based on state. The position and orientation of the sub-blocks within the block are determined; Local geometric degradation risk Determined based on the eigenvalues ​​of the covariance matrix of the voxel neighborhood point cloud; Dynamic occupation risk It is determined based on the changes in the occupancy state of voxels within a continuous time window, the displacement of representative points, observation consistency, and velocity estimation.

4. The risk-aware adaptive OctVox laser inertial mapping method according to claim 1, characterized in that, In step 5, different sub-voxel segmentation scales are configured for voxels of different risk levels. Specifically, for low-risk voxels, a single representative point, large sub-voxel size, large merging threshold, or low-frequency update strategy is adopted to retain only coarse geometric information far from the current flight mission area; for medium-risk voxels, a general approach is adopted. OctVox expression in octetroids; for high-risk voxels, the following methods were employed. Sub-voxels, small voxel sizes, multiple representative points per sub-voxel, and boundary point preservation or additional covariance / normal vector enhancement representation.

5. The risk-aware adaptive OctVox laser inertial mapping method according to claim 4, characterized in that, Different representative point update strategies are configured for voxels with different risk levels, specifically including: For low-risk voxels, if the distance between the new point and existing representative points is less than the merging threshold... If the threshold is exceeded but the voxel is not located within the flight safety corridor, then the point is ignored or only the occupancy probability is updated; for medium-risk voxels, incremental averaging is performed according to the general OctVox sub-vox; for high-risk voxels, if the new point is located at the obstacle boundary or at a location with large local curvature, it is retained as an independent representative point or a small threshold is used. To integrate.

6. The risk-aware adaptive OctVox laser inertial mapping method according to claim 3, characterized in that, In step 6, the nearest neighbor search calculates the effective distance. To achieve this, for the points to be matched in a specific frame. its candidate representative points The effective distance calculation formula is: ； in, To represent the stability of the point, For dynamic tagging, , and This is the adjustment coefficient; The formula for calculating the point-to-plane residual is: ； in, n It is a plane normal vector. Let be a point on the plane.

7. The risk-aware adaptive OctVox laser inertial mapping method according to claim 6, characterized in that, In step 7, the division between the stable positioning layer and the dynamic obstacle avoidance layer is as follows: Constructing voxels stability index : ； in, Voxel representation Multi-frame observation consistency, Indicates local geometric reliability. Indicates the intensity of historical continuous observation. Indicates the degree of dynamic occupancy change. , , and These are the corresponding weighting coefficients; like voxels v Add the aforementioned stabilizing positioning layer; like voxels v Add the aforementioned dynamic obstacle avoidance layer; in, As the stability threshold, The threshold is dynamically changing.

8. The risk-aware adaptive OctVox laser inertial mapping method according to claim 7, characterized in that, Step 8 specifically includes: The mapping module is based on future flight paths Calculate flight corridor : ； in, This represents any point in three-dimensional space that needs to be determined. Indicates the safety threshold for the flight corridor; For falling into Inside or near For voxels at the boundary, their trajectory approach risk is automatically increased; if a high-risk voxel also satisfies the obstacle boundary conditions, obstacle constraints are output to the planner; if a dynamic voxel enters... If this occurs, a replanning, deceleration, or hovering flag will be triggered. The planner calculates collision cost and trajectory feasibility based on obstacle boundaries, dynamic occupancy, and uncertainty markers output by the risk adaptive map, and feeds the collision cost and trajectory feasibility back to the mapping module to re-plan the trajectory, forming a closed loop of trajectory prediction, risk scoring, map enhancement, and obstacle avoidance planning.

9. The risk-aware adaptive OctVox laser inertial mapping method according to claim 1, characterized in that, The structured mapping results include: UAV real-time pose, stable positioning layer map, dynamic obstacle avoidance layer map, risk voxel layer map, flight safety corridor, and system status markers.

10. A risk perception-adaptive OctVox laser inertial mapping system for autonomous obstacle avoidance of unmanned aerial vehicles (UAVs), used to implement the mapping method according to any one of claims 1-9, characterized in that, include: The sensor acquisition module is used to acquire data from the onboard sensors of the UAV. The LIO front-end module is connected to the sensor acquisition module and is used to perform state prediction based on IMU measurement data and to perform motion distortion compensation on the lidar point cloud to obtain a distortion-free point cloud. The risk assessment module, connected to the LIO front-end module, is used to calculate the flight risk score for each voxel; An adaptive map management module, connected to the risk assessment module, is used to construct a hash voxel map as the basic map index structure and to divide voxels into three levels: low-risk voxels, medium-risk voxels, and high-risk voxels according to the flight risk score. The matching and filtering update module, connected to the adaptive map management module, is used to obtain candidate nearest neighbor points in the nearest neighbor search of the scanned map match, and construct point-to-plane residuals based on the candidate nearest neighbor points to update the UAV pose. The local planning feedback module, connected to the matching and filtering update module, is used to feed back the future trajectory output by the local planner to the risk assessment module to update the flight safety corridor and voxel risk score. The safety control interface module is connected to the local planning feedback module and is used to output structured mapping results.

Images