Unmanned aerial vehicle path planning method in complex environment

Abstract

The application discloses a kind of complex environment under unmanned plane flight path planning method, specifically relates to the technical field of unmanned plane autonomous flight path planning, for solving the problem that environment information updating and flight path execution are not synchronized under complex environment, leading to invalid flight path interval and new and old flight path switching boundary are difficult to accurately determine. By converting environment perception data according to the unmanned plane pose corresponding to the collection time, an occupancy evidence body, a passable evidence body and a sheltered holding evidence body are formed, and a support relationship between the control flight path section sweeping space body and the passable evidence body is established. After updating the environment evidence version, locate the invalid control flight path section, determine the execution commitment front based on the issued control instruction, flight state and flight power constraint, and generate the candidate control flight path connecting the switching state tube and the back connection state tube, thereby reducing the replacement of the flight path without evidence, so that the unmanned plane retains the executable flight path part and continuously bypasses the invalid interval.

Description

Technical Field

[0001] This invention relates to the field of autonomous flight path planning technology for unmanned aerial vehicles (UAVs), specifically a method for UAV flight path planning in complex environments. Background Technology

[0002] When drones perform inspection and detection tasks in densely built-up areas, factory corridors, construction sites, and other complex low-altitude environments, they need to continuously acquire information about their own motion status and surrounding obstacles using onboard positioning equipment, inertial measurement equipment, and environmental perception equipment, and plan a trajectory that meets flight dynamic constraints accordingly. Existing drone trajectory planning methods typically use lidar, depth cameras, or millimeter-wave radar to construct a local environmental map, marking obstacle areas as impassable areas. When a conflict is detected between the original trajectory and an obstacle, a new local trajectory is searched, and the newly generated trajectory is smoothed before being sent to the flight controller for execution.

[0003] During actual flight, the data acquisition frequency, processing latency, and transmission time vary among different devices. By the time environmental perception data reaches the planning unit, the UAV has already continued moving along its original trajectory. Especially in building corners, narrow passages, and areas obstructed by equipment, moving obstacles may temporarily leave the sensor's field of view. Existing processing methods tend to directly treat spaces where obstacle echoes are not obtained as passable spaces. When the obstacle re-enters the field of view, the local map changes again, causing the trajectory to repeatedly fail and be regenerated within adjacent planning cycles.

[0004] Existing methods, after environmental information is updated, typically determine whether there is a collision risk along the entire original flight path based on the latest local map, or perform local replanning based on a fixed spatial range near obstacles. This makes it difficult to clearly distinguish between the still-executable portions of the original flight path and the portions that have lost their navigability. Furthermore, the new flight path usually starts from the UAV's current position or the current planning time, without fully considering issued but not yet executed control commands, current speed, and permissible maneuverability. This may result in the selected flight path transition point being inconsistent with the actual position where the UAV can detach from the original flight path, leading to discontinuities in speed, acceleration, and heading at the junction of the old and new flight paths.

[0005] Furthermore, after a planning unit generates a new track, if the original track terminates, the new track takes effect, and the flight controller's execution direction is updated separately, and some records are not fully written, there may be inconsistencies where the planning unit adopts the new track while the flight controller continues to execute the original track. Therefore, it is necessary to address how to accurately determine the track range affected by environmental changes under conditions of continuous updates in complex environments, determine feasible track switching boundaries based on the actual execution status of the UAV, and ensure continuous continuity between the old and new tracks and consistency in execution status. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides a method for UAV trajectory planning in complex environments, thereby solving the problems mentioned in the background section.

[0007] To achieve the above objectives, the present invention provides the following technical solution: a method for UAV trajectory planning in complex environments, comprising: S1. Acquire UAV status data and environmental perception data, convert environmental perception data according to the pose corresponding to the acquisition time, and form an environmental evidence version containing occupancy evidence, passable evidence, and occlusion preservation evidence. S2. Divide the current valid track into control track segments according to the execution order, generate a swept space volume, associate the passable evidence bodies covering the swept space volume into a supporting evidence set, and form a track support relationship version. S3. When the environmental evidence version is updated, the changed evidence body is determined, the support failure control track segment is located along the track support relationship version, and the continuous control track segments are merged to form the candidate failure track interval. S4. Based on the issued control commands, flight status, and flight dynamic constraints, predict the command acceptance status, determine the execution commitment frontier along the current valid flight path, and form a switching state control bundle; S5. After the candidate failed track interval, determine the supporting valid reconnection track segment to form a reconnection state bundle; establish a spatiotemporal support map based on the environmental evidence version, and generate candidate control tracks connecting the two state bundles. S6. Verify the dynamic constraints, environmental evidence back support, and dual-end state transition of the candidate control track in sequence; after verification, terminate the current track version and activate the candidate track version at the same switching time.

[0008] Furthermore, S1 includes: The effective echo points and the extended range determined by the measurement errors of environmental sensing equipment, the position errors of UAVs, and the coordinate transformation errors form the occupancy evidence body; The effective detection space from the location of the sensing device to the first valid echo point forms a passable evidence body; The occupied space that was not re-observed in the previous version of environmental evidence because the detection ray was cut off by the occupied evidence body located in front of the ray is formed into a shielding evidence body.

[0009] Furthermore, S2 includes: Based on the UAV body reference space, position measurement error space, and track tracking allowable error space, a sweep space volume is formed according to the desired position and desired attitude at each moment of the control track segment; A passable evidence body is written into the supporting evidence set only if the swept space body is completely covered by a passable evidence body during the planned execution time and does not intersect with an occupied evidence body or an obstruction-preserving evidence body.

[0010] Furthermore, S3 includes: By comparing the previous and updated versions of environmental evidence with the unified spatial unit identifier, the spatial units of evidence type conversion and the spatial units of occupied boundary ownership change are respectively identified as change evidence bodies. The corresponding control track segment will be designated as a support failure control track segment in any of the following circumstances: the swept space volume is not completely covered by the set of valid supporting evidence after the update; The swept space body intersects with the occupied evidence body and the obstructed evidence body.

[0011] Furthermore, S4 includes: According to the execution order of the issued control commands, the position, speed, and heading competition time periods are recursively calculated based on the flight controller control cycle to obtain the command acceptance status at the end of the execution of all issued control commands; For the control track segment boundary of the current effective track, calculate the available time and the shortest maneuver time determined by the speed adjustment time, heading adjustment time, and control response delay. The boundary with the first available time before the candidate failed track segment that is not less than the shortest maneuver time is determined as the execution commitment frontier.

[0012] Furthermore, S5 includes: Starting from the next control track segment after the termination of the candidate failed track segment, select the first support valid control track segment as the follow-up track segment according to the execution sequence number; Using the connected space of the accessible evidence body in each time layer as nodes, in adjacent time layers, a directed connection is established for nodes whose center distance is no greater than the product of the maximum allowable velocity and the time layer difference plus half the product of the maximum allowable acceleration and the square of the time layer difference, and whose transition space is completely covered by the accessible evidence body, thus forming a spatiotemporal support graph.

[0013] Furthermore, the node sequence connecting the switching state control bundle and the reconnecting state control bundle is searched in ascending order of time layer; The node transition cost is the sum of the first non-negative weighted value of the distance from the node center and the ratio of the velocity change to the maximum allowable acceleration, and the second non-negative weighted value of the reciprocal of the minimum distance from the transition space to the boundary of the occupied evidence body; The path connection point is determined based on the node sequence with the minimum cumulative transition cost, and the start and end positions, start and end velocities, and start and end accelerations are used as boundary conditions to generate piecewise quintic polynomial candidate control paths.

[0014] Furthermore, S6 includes: Calculate the first, second, and third derivatives of the position function of the candidate control trajectory to determine the velocity, acceleration, and jerk, and compare them with the corresponding flight dynamic constraints; Sweep space volumes are generated segment by segment from the endpoint of the candidate control track to the starting point. When each sweep space volume is completely covered by a valid passable evidence volume and does not intersect with an occupied evidence volume or an obstruction evidence volume, the environmental evidence reverse support verification is passed.

[0015] Furthermore, the candidate control track start states are compared with the switching state control bundle item by item, and the termination states are compared with the reconnection state control bundle item by item. Once all state components fall within their corresponding upper and lower bounds, the current valid end time of the current track version, the valid start time of the candidate track version, and the current valid track direction of the flight controller are written in the same indivisible commit. The candidate track version takes effect when all three writes are successful, and the current state change is revoked when any write fails.

[0016] Compared with the prior art, the present invention has the following beneficial effects: 1. By completing coordinate transformation according to the UAV's pose at the time of environmental perception data collection, occupancy evidence, passability evidence, and occlusion preservation evidence are formed. Then, the swept space volume of the control track segment is established with the passability evidence. After the environmental evidence version is updated, the control track segment that supports failure is located along the support relationship. Combined with the issued control commands, flight status, and flight dynamic constraints, the execution commitment frontier is determined. Candidate control tracks that connect the switching state control bundle and the reconnecting state control bundle are further generated. This ensures that the judgment of track failure caused by environmental changes, the determination of the actual switchable boundary, and the generation of candidate tracks are executed continuously according to a unified causal chain. This reduces the unfounded track replacement caused by the inconsistency between the time of perception information update and the actual execution state of the track. This allows the UAV to bypass the failure zone and reconnect to the subsequent track that supports the valid track while retaining the part of the track that can continue to be executed.

[0017] 2. By generating sweep space volumes containing the aircraft outline, position measurement error, and track tracking allowable error for each control track segment, and associating them with passable evidence volumes with corresponding spatial and effective time ranges, the system determines the support validity status based on complete coverage, occupancy intersection, and occupancy retention relationships when environmental evidence versions change. This allows candidate failed track intervals to be derived from the support relationships between changed evidence volumes and specific control track segments, rather than solely based on the instantaneous position or fixed distance range of obstacles. This distinguishes track segments actually affected by environmental changes from those still having valid support, avoiding the direct identification of temporarily unobserved occupied space as passable space and reducing redundant processing of unaffected track portions.

[0018] 3. By sequentially performing dynamic constraint verification, environmental evidence reverse support verification, and dual-end state succession verification on candidate control tracks, and terminating the current track version, activating the candidate track version, and updating the current valid track direction of the flight controller at the same switching moment after all verifications pass, the candidate control track simultaneously has an executable motion state, complete environmental evidence support, and a clear new and old track succession relationship before it becomes effective. If any version record writing fails, the current state change is revoked and the original track version remains valid, thereby avoiding inconsistencies between the current track termination state, the candidate track activation state, and the flight controller execution direction, ensuring state continuity, version consistency, and traceability of the formation process during track switching. Attached Figure Description

[0019] Figure 1 A flowchart illustrating the overall process of UAV trajectory planning in complex environments; Figure 2 A schematic diagram of a drone operating in a low-altitude corridor. Figure 3 A schematic diagram illustrating the relationship between UAV status data and environmental perception data; Figure 4 This is a schematic diagram of the switching state control bundle and the reconnection state control bundle. Detailed Implementation

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

[0021] Example: Combined with Appendix Figures 1-4 This embodiment provides a method for UAV trajectory planning in complex environments, including: S1. Acquire UAV status data and environmental perception data, convert the environmental perception data according to the pose corresponding to the acquisition time, and form an environmental evidence version containing occupancy evidence, passable evidence, and occlusion preservation evidence. The specific implementation is as follows: This step is performed by the onboard computing unit according to the rhythm of the data frames output by the environmental perception equipment during the UAV's current valid flight path. At the start of this environmental evidence version formation, the onboard computing unit locks the versions corresponding to the state data reading rules, clock synchronization parameters, coordinate transformation parameters, sensor extrinsic parameters, measurement error parameters, and evidence body formation rules; before this environmental evidence version is completed, the locked parameter version will not be replaced by a subsequently updated parameter version.

[0022] The UAV status data is obtained from the flight controller, inertial measurement unit, and positioning unit, and includes at least the UAV position, flight speed, flight acceleration, attitude, heading angle, angular velocity, status acquisition time, and status record number. The UAV position is represented by three-dimensional coordinates in a unified flight coordinate system, in meters; flight speed is in meters per second; flight acceleration is in meters per second squared; heading and attitude angles are in radians; angular velocity is in radians per second; and the status acquisition time is represented by the second value under the unified onboard clock. Positioning accuracy, attitude measurement accuracy, and clock synchronization error are respectively read from the locked equipment calibration records, serving as the error basis for subsequent pose interpolation and evidence body boundary extension.

[0023] Environmental perception data is acquired from at least one of the following environmental perception devices: LiDAR, depth camera, and millimeter-wave radar. Each environmental perception data frame records at least the perception device identifier, data frame number, acquisition start time, acquisition end time, perception point coordinates, effective detection direction, effective detection distance, valid echo marker, and perception device operating status. Perception point coordinates are represented using the perception device's own coordinate system, in meters. For scanning environmental perception devices, the midpoint of a single scan is taken as the acquisition time of the data frame; if the UAV pose change during scanning exceeds the motion compensation tolerance locked for this scan, the UAV pose is acquired separately according to the scan time corresponding to each perception point, and the entire frame of perception points is not uniformly attributed to the same pose.

[0024] The onboard computing unit sorts the UAV status data according to the time of status acquisition and removes duplicate records according to the data record number. When multiple status data records correspond to the same record number, the record with the earliest acquisition time and the one whose device verification status is normal is retained. When the time interval between two adjacent status data records does not exceed the allowed interval for status interpolation, the UAV's position, flight speed, flight acceleration, and attitude are time-aligned according to the acquisition time of the environmental perception data. Let the acquisition time of the j-th sensing point in the environmental perception data be... The times when the two sets of UAV status data were collected were respectively The corresponding positions are respectively The location of the UAV at the time of data collection is obtained according to the following formula:

[0025] In the formula, For the time of data collection The corresponding UAV position vector, in meters; The collection time is respectively The corresponding position vector; It must be greater than zero. If two sets of state data are collected at the same time, in reverse order, or the time interval exceeds the allowed interval for state interpolation, position interpolation will not be performed, the corresponding environmental perception data will be marked as data to be verified, and the environmental perception data will not be used to form an environmental evidence version.

[0026] The UAV attitude is calculated using spherical linear interpolation between adjacent attitude records based on the acquisition time. The attitude quaternion is normalized before interpolation. If the magnitude of the attitude quaternion is zero, the attitude record is missing, or the change in adjacent attitudes exceeds the range of change corresponding to the allowable angular velocity of the device, the coordinate transformation of that data frame is stopped, and the estimated attitude is not used for further processing.

[0027] Let the coordinates of the j-th sensing point in the sensing device coordinate system be... The rotation matrix and translation vector from the sensing device coordinate system to the UAV body coordinate system are respectively The rotation matrix and position vector from the UAV body coordinate system to the unified flight coordinate system at the time of data acquisition are respectively... The transformed sensor coordinates are obtained according to the following formula:

[0028] In the formula, Let be the coordinates of the j-th sensing point in the unified flight coordinate system, in meters; and All are three-row, three-column orthogonal matrices; This is the translation vector of the sensing device's installation position relative to the UAV's reference point. The conversion of this data frame will stop if the rotation matrix dimension does not meet the requirement of three rows and three columns, the matrix orthogonality error exceeds the calibration tolerance, or the sensor extrinsic parameter versions are inconsistent.

[0029] After coordinate transformation, the onboard computing unit divides the unified flight coordinate system into non-overlapping spatial units according to the environmental evidence formation rules. The side length of each spatial unit is determined by the UAV's dimensions, environmental perception accuracy, and onboard computing capabilities, and is recorded in the environmental evidence formation rules established in this study. If the spatial range covered by the environmental perception data exceeds the currently planned operational area, only spatial units within the currently planned operational area and its safety boundaries are retained.

[0030] The occupied evidence body consists of the spatial cell containing the valid echo point and adjacent spatial cells within the measurement error propagation range. For the j-th valid echo point, using... Centered on the environmental sensing device distance error, UAV pose error, and coordinate transformation error, the occupied expansion radius is formed. The occupied expansion radius is formed according to the following formula:

[0031] In the formula, This is the upper bound of the measurement error of the environmental sensing device at the distance of the j-th sensing point, in meters; This is the upper bound of the UAV position error at the time of data collection, expressed in meters. This represents the upper bound of the coordinate transformation error, in meters. Centered on, with Spatial units intersecting within a radius are all included in the occupancy evidence body. Sensing points with echo intensity below the equipment's effective echo threshold, echo distance exceeding the equipment's calibration range, or abnormal operating status of the sensing equipment are not used to form the occupancy evidence body.

[0032] The passable evidence body is formed based on the effective detection space between the sensing device's launch position and the first valid echo point. For each effective detection direction, starting from the sensing device's launch position in a unified flight coordinate system, spatial units are traversed sequentially along that detection direction until the occupied extension boundary corresponding to the first valid echo point. Spatial units that do not intersect with the occupied extension range and have been traversed by the effective detection ray are included in the passable evidence body. Spaces near the sensing device's launch position that are obstructed by the aircraft, spaces after the first valid echo point, and spaces beyond the effective detection distance are not included in the passable evidence body. When there is no valid echo but the detection ray reaches the device's effective detection distance, only the space between the launch position and the effective detection distance that is actually covered by the detection ray is included in the passable evidence body.

[0033] The occupancy evidence body is formed based on the occupancy evidence body in the previous environmental evidence version and the occupancy relationship of the current detection ray. If no corresponding valid echo is detected in the current data frame for an occupied space in the previous environmental evidence version, and the detection ray pointing from the current sensing device's emission position to that occupied space is truncated by a closer occupancy evidence body, then that occupied space is included in the occupancy evidence body. If a new valid detection ray completely passes through the previous occupied space and its error extension range, there is no valid echo on the ray, and the environmental sensing device is functioning normally at that distance, then that space is no longer included in the occupancy evidence body, and can be converted into a passable evidence body based on the valid detection ray. Failure to observe due to a single frame not receiving an echo, temporary reversal of the sensing device, or insufficient sensing distance is not grounds for revoking the previous occupancy evidence.

[0034] When both occupancy and accessibility evidence are obtained simultaneously for the same spatial unit, they are processed from earliest to latest acquisition time. If the acquisition times are the same, occupancy evidence takes precedence over accessibility evidence. When different sensing devices generate conflicting evidence for the same spatial unit, the operating status of the sensing devices, acquisition time, and coverage distance are first verified. If the conflict still cannot be resolved, the spatial unit is retained as occupancy evidence and is not used as accessibility evidence in a conflicting state in subsequent steps. Duplicate evidence corresponding to the same sensing device identifier, the same data frame sequence number, and the same spatial unit identifier is written only once.

[0035] The environmental evidence version must at least retain the environmental evidence version identifier, formation time, applicable operating area, original environmental perception data range, UAV status data range, sensor extrinsic parameter version, coordinate transformation parameter version, spatial unit division version, occupied evidence body, passable evidence body, occupancy preservation evidence body, and the corresponding acquisition time range for each evidence body. This step is complete after all required fields are successfully written and each evidence body can be traced back to the original environmental perception data. If any required field is missing, time alignment fails, coordinate transformation fails, or version writing fails, a new environmental evidence version is not generated; the previous environmental evidence version is retained, and the original record and intermediate transformation results are saved to the record area to be verified. S2 reads the environmental evidence version formed in this step and the passable evidence bodies within it.

[0036] S2. Divide the current valid track into control track segments according to the execution order, generate swept space volumes, and associate the passable evidence volumes covering the swept space volumes into a supporting evidence set to form a track support relationship version. The specific implementation is as follows: This step involves the onboard computing unit reading the currently valid track invoked by the flight controller. The currently valid track includes at least a track version identifier, a planned start time, a planned end time, and the UAV's desired position, desired velocity, desired acceleration, and desired attitude recorded in chronological order. The currently valid track version identifier must match the current track pointer record of the flight controller; if they do not match, this step is terminated, and a track support relationship version is not established based on a non-current track.

[0037] The airborne computing unit sorts the execution times of currently valid tracks from earliest to latest, dividing track records with the same trajectory expression range, the same control update caliber, and continuous execution time into a single control track segment. If the start time of the later record of adjacent track records equals the end time of the previous record, the execution time is considered continuous; if there is time overlap, time inversion, or an unspecified time gap, the segmentation stops, and the current track support relationship version is retained. Each control track segment records a control track segment identifier, execution sequence number, planned start time, planned end time, position function, velocity function, acceleration function, and attitude function. The execution sequence number increases sequentially from the start to the end of the currently valid track, and cannot be repeated within the same currently valid track.

[0038] For the i-th control track segment, the onboard computing unit calculates the swept space volume formed by the UAV's body as it moves over time, based on the expected position and attitude of the control track segment within the planned execution time range. Let the UAV's body reference space be B, and the position measurement error space be... The allowable error space for trajectory tracking is The desired position and desired attitude of the i-th control track segment at time t are respectively Then the swept space volume of the i-th control track segment is represented as:

[0039] In the formula, For the i-th control track segment, the swept space volume is... These represent the planned start and end times of the control track segment, in seconds; symbol Minkowski and B represent spatial range; B is formed based on the UAV's fuselage outline and rotor working range. It is formed based on the position error of the UAV status data used by S1; The flight path is determined based on the allowable tracking deviation of the flight controller. Aircraft size parameters, position error parameters, and allowable tracking error parameters are locked at the beginning of this step.

[0040] In actual calculations, the control track segment is discretely sampled according to the flight controller's control update cycle. An envelope space is used to connect adjacent airframe spaces between adjacent sampling times to prevent fragmentation of the swept space volume caused by only taking discrete poses. When the duration of a control track segment is less than one control update cycle, at least the airframe spaces corresponding to the planned start and end times are obtained. If the position function and attitude function cannot be continuously evaluated within the planned execution time, the swept space volume for that control track segment is not generated.

[0041] The onboard computing unit compares the swept space volume of each control track segment with the passable evidence volume formed in S1, according to the corresponding planned execution time. Let the k-th passable evidence volume in the environmental evidence version be the spatial range. The effective time frame for its evidence is When the entire space volume swept by the control track segment at time t is swept by one or more... When t is within the valid time frame of the corresponding accessible evidence, the corresponding accessible evidence has a covering effect.

[0042] The set of supporting evidence for the i-th control track segment is represented as follows:

[0043] After forming a set of supporting evidence, the complete coverage condition must be checked according to the following formula:

[0044] In the formula, For the i-th control track segment at time t, the extended space of the aircraft body is defined. This is the set of valid, walkable evidence bodies at time t. Only those at each sampling time within the planned execution timeframe are valid. Only when all samples meet the complete coverage condition will the corresponding passable evidence be associated with the supporting evidence set for that control track segment. The coverage of the envelope space must also be checked between adjacent sampling times; coverage of all sampling points cannot replace coverage of continuous intervals.

[0045] When multiple passable evidence bodies jointly cover the same control track segment, all of them are written into the supporting evidence set after being sorted according to the evidence body identifiers in the environmental evidence version; no single evidence body is arbitrarily selected. If the same evidence body identifier appears repeatedly, only one association is retained. If any part of a swept space body simultaneously falls into an occupying evidence body or an obstruction-maintaining evidence body, that part is not considered to be effectively covered by the passable evidence body. When the supporting evidence set is empty, the coverage time is insufficient, or there are gaps in spatial coverage, a new track support relationship version is not formed; the previous track support relationship version and the currently valid track are retained, and the control track segment identifier, spatial location, and planned execution time corresponding to the gap are written into the pending verification record area.

[0046] The track support relationship version uses the current valid track version identifier and the environmental evidence version identifier as common sources, and at least saves the track support relationship version identifier, the current valid track version identifier, the environmental evidence version identifier, the control track segment execution sequence number, the swept space volume of each control track segment, the supporting evidence set of each control track segment, the formation time, and the parameter version used. After all control track segments have completed the generation of swept space volumes and the association of supporting evidence, the track support relationship version is written to the track relationship record area of ​​the airborne computing unit. After this step is completed, S3 directly reads the control track segments, swept space volumes, and supporting evidence sets from the track support relationship version, without regenerating the above content.

[0047] S3. When the environmental evidence version is updated, the changed evidence body is determined, and the support failure control track segment is located along the track support relationship version. Continuous control track segments are merged to form candidate failure track intervals. The specific implementation is as follows: This step is executed after the new environmental evidence version formed in S1 has been written. The onboard computing unit locks the new environmental evidence version, the previous environmental evidence version, and the track support relationship version formed in S2. Only after these three are locked does the determination of the change evidence body begin. Subsequent environmental perception data generated during the formation of the new environmental evidence version is not included in this determination process.

[0048] The airborne computing unit compares the occupancy evidence, accessibility evidence, and obstruction-preservation evidence in the new and previous versions of environmental evidence using a unified spatial unit identifier. For the same spatial unit, if it was accessibility evidence in the previous version but becomes occupancy or obstruction-preservation evidence in the new version, the spatial unit is identified as a changed evidence unit. Similarly, if it was occupancy or obstruction-preservation evidence in the previous version but becomes accessibility evidence in the new version after confirmation by effective detection rays, the spatial unit is also identified as a changed evidence unit. If the evidence type remains unchanged but the occupancy boundary shifts, causing a change in the spatial unit's ownership, the spatial unit with the changed ownership is identified as a changed evidence unit.

[0049] When different spatial unit division versions are used for the same physical space, the spatial unit identifiers are not directly compared. The onboard computing unit first converts the two versions of environmental evidence to the common spatial division caliber locked in this case, and then performs a spatial attribution comparison. If the common spatial conversion cannot be completed, this step is stopped, and changes are not directly judged using different spatial division calibers.

[0050] The change evidence body must at least record the spatial extent of the change, the previous evidence type, the new evidence type, the previous environmental evidence version identifier, the new environmental evidence version identifier, and the time when the change occurred. When multiple evidence changes occur for the same spatial unit in a single environmental evidence version update, they are sorted by collection time, with the evidence type collected latest and passing the S1 validity check being designated as the new evidence type. When the change evidence body is empty, this environmental evidence version update does not trigger the formation of candidate failed track intervals, and the current track support relationship version remains valid.

[0051] After identifying the changed evidence body, the airborne computing unit reads the swept space volume and supporting evidence set of each control track segment according to the execution sequence number of the control track segment in the track support relationship version. For the i-th control track segment, its supporting evidence set is replaced with the evidence state in the new environment evidence version to form the valid supporting evidence set under the new version. If the space corresponding to the original supporting evidence is still a passable evidence body in the new environmental evidence version, and the effective time covers the planned execution time of the control track segment, then the supporting evidence is retained; if the space corresponding to the original supporting evidence is converted into an occupied evidence body, an obstructed evidence body, or no longer has valid evidence within the corresponding execution time, then the supporting evidence is deleted.

[0052] The support validity state of the i-th control track segment is determined according to the following formula:

[0053] In the formula, This represents the support status of the i-th control track segment; a value of 1 indicates support is valid, and a value of 0 indicates support is invalid. The new environmental evidence version is the valid occupancy evidence body at time t; To ensure the preservation of evidence in the new environmental evidence version at time t under effective occlusion; These represent the swept space and the set of valid supporting evidence corresponding to the control track segment at time t.

[0054] If the complete coverage condition is not met at any sampling time during the planned execution period, or if the swept space volume intersects with the occupied evidence volume or the occluded evidence volume, then... Set to 0 and designate this control track segment as the support failure control track segment. Intersection determination uses the closed interval boundary; when the boundary of the swept space volume contacts the boundary of the occupied evidence volume, intersection is considered to have occurred. When there is only spatial proximity but no intersection, proximity is not used as a basis for supporting failure.

[0055] The airborne computing unit starts by executing the support failure control track segment with the smallest execution sequence number and merges subsequent support failure control track segments in ascending order of execution sequence number. If the planned termination time of the previous support failure control track segment equals the planned start time of the next support failure control track segment, and their execution sequences are adjacent, they are considered consecutive. The merging process stops when the first support effective control track segment is encountered. A candidate failure track interval is formed from the planned start time of the earliest support failure control track segment to the planned termination time of the last control track segment in the consecutive failure segments. If subsequent discontinuous support failure control track segments exist, their support failure records are retained. This step is re-executed after processing the current candidate failure track interval, without merging intermediate support effective control track segments into the current candidate failure track interval.

[0056] Each candidate failed track interval must at least store the interval's starting control track segment identifier, ending control track segment identifier, planned start time, planned end time, a list of supporting failed control track segments within the interval, the corresponding change evidence body identifier, the new environment evidence version identifier, and the track support relationship version identifier. If a change evidence body cannot establish a definite association with the track support relationship version, if the control track segment execution sequence number is duplicated, or if the planned time relationship is discontinuous, interval merging is stopped, and unconfirmed intervals are not passed to S4. After the candidate failed track interval is written, S4 reads its starting position, starting time, and control track segments within the interval.

[0057] S4. Based on the issued control commands, flight status, and predicted command acceptance status according to flight dynamic constraints, determine the execution commitment frontier along the current valid flight path to form a switching state control bundle. The specific implementation is as follows: This step involves the onboard computing unit, after the candidate failure track interval is formed, reading the issued control commands that have entered the control execution queue but have not yet been fully executed from the flight controller. Each issued control command includes at least the control command number, issuance time, planned execution start time, planned execution end time, target acceleration, target angular velocity, and execution status. Control commands are ordered according to their planned execution start time; control commands with the same start time are ordered according to their control command number. For duplicate records of the same control command number, only the one with the earliest confirmed reception time by the flight controller is retained.

[0058] The flight status is the current state confirmed by the flight controller at the start of this step, including at least the current position, current speed, current acceleration, current heading angle, and current angular velocity. Flight dynamic constraints are obtained from the dynamic parameter records corresponding to the UAV's current payload state, including at least the maximum permissible speed, maximum permissible acceleration, maximum permissible angular velocity, maximum permissible jerk, and control response delay. The dynamic parameter records are associated with the UAV model, payload configuration, and flight mode; prediction stops if the association is not unique or the payload configuration is missing, and dynamic parameters from other payload states are not used.

[0059] Let the flight state at the start of this step be:

[0060] In the formula, This is the current position vector, in meters. This is the current velocity vector, in meters per second. This is the current acceleration vector, in meters per second squared. This is the current heading angle, in radians; This represents the current angular velocity, expressed in radians per second.

[0061] The onboard computing unit integrates the remaining control commands step by step, following the execution order of the issued control commands and using the flight controller's control cycle as the time step. For the r-th integration time interval, the state is recursively calculated using the following formula:

[0062] In the formula, The duration of the r-th integration interval is expressed in seconds. and Obtained from the corresponding issued control commands; These represent the initial states for each integral time interval. If the duration of a control command is not divisible by the control cycle, the remaining duration is used for the last integral time interval. If any velocity, acceleration, or angular velocity exceeds the flight dynamic constraints during the recursive process, the current prediction round is stopped, the current trajectory version is retained, and the out-of-bounds state is not passed to the next step.

[0063] The state after all issued control commands have completed the recursion is defined as the command acceptance state. The command acceptance state must record at least the expected acceptance time, expected position, expected speed, expected acceleration, expected heading angle, expected angular velocity, the range of control command numbers already included in the prediction, and the version of the flight dynamic constraints. Control commands that the flight controller has already reported as executed are not included in the prediction again.

[0064] After the command acceptance state is established, the onboard computing unit checks the boundaries of each control track segment along the execution order of the currently valid track, starting from the time corresponding to the command acceptance state. For the i-th candidate boundary, the available time from the command acceptance state to that boundary is calculated:

[0065] In the formula, The available time for the i-th candidate boundary is in seconds. The planned arrival time of this boundary within the current valid track; This represents the expected time of instruction acceptance. If the value is less than zero, skip the boundary.

[0066] Based on the command reception status and flight dynamic constraints, calculate the shortest maneuver time required for the UAV to complete the current speed and heading adjustments:

[0067] In the formula, The shortest maneuver time required to complete the state adjustment before reaching the i-th candidate boundary, in seconds; and These are the planned speed and planned heading angle of the current valid track at this boundary, respectively; and These are the expected speed and expected heading angle in the command acceptance state, respectively; The maximum permissible acceleration; The maximum permissible angular velocity; To control response delay, wrap is used to convert heading angle difference to a signed angle range not exceeding half a revolution. or If the value is not greater than zero, the formula cannot be calculated, and this step should be stopped.

[0068] The onboard computing unit searches for the first function that simultaneously satisfies the control track segment execution sequence number from front to back. The candidate boundary time is no earlier than the command acceptance time and the candidate boundary is located before the start time of the candidate failed track interval. This boundary is determined as the execution commitment front. The currently valid track before the execution commitment front is still executed by the flight controller according to the issued control commands; the track after the execution commitment front is allowed to be used as the switching starting point of the candidate control track. If there is no boundary that meets the conditions before the start time of the candidate failed track interval, no switching state constraint is formed in this round, and S5 is not entered. The current track version remains in its original state.

[0069] Centered on the planned state at the execution commitment front, and combining the command reception state prediction error, position measurement error, and flight controller allowable tracking deviation, a switching state bundle is formed. The switching state bundle represents the range of states where candidate control paths are allowed to connect at the execution commitment front, including at least the lower and upper bounds of position, velocity, acceleration, heading angle, angular velocity, and corresponding time. Let the planned state components at the execution commitment front be... The upper bounds of the prediction error, measurement error, and tracking error corresponding to this component are respectively: Then the allowable interval of this state component is:

[0070] All upper bounds of the error are obtained from the flight status record, equipment calibration record, and flight control parameter record locked in this step. When the heading angle interval crosses the period boundary, the heading angle is first converted into a continuous angle centered on the planned heading angle before forming the interval.

[0071] The execution commitment frontier and the transition state constraint are written to the track transition record area, saving at least the current valid track version identifier, candidate failed track interval identifier, command acceptance status, control track segment identifier corresponding to the execution commitment frontier, transition state constraint boundary, range of issued control commands, flight dynamic constraint version, and formation time. This step is complete after all content is successfully written. S5 directly reads the transition state constraint and execution commitment frontier without re-predicting the command acceptance status.

[0072] S5. After the candidate failed track interval, determine the supporting valid reconnection track segment to form a reconnection state bundle; establish a spatiotemporal support graph based on the environmental evidence version, and generate candidate control tracks connecting the two state bundles. The specific implementation is as follows: This step begins with the airborne computing unit reading the track support relationship versions from the next control track segment after the candidate failed track segment terminates, in ascending order of execution sequence number. For each control track segment, the swept space volume, supporting evidence set, occupancy evidence volume, and obstruction maintenance evidence volume of the control track segment are re-verified using the currently locked environmental evidence version, according to the support validity state calculation criteria in S3. The first control track segment that maintains complete passable evidence coverage throughout the entire planned execution time, and whose swept space volume does not intersect with the occupancy evidence volume or obstruction maintenance evidence volume, is determined as the reconnection track segment.

[0073] If no supporting control track segment exists after the candidate failed track segment, the generation of candidate control tracks will cease in this round. The current track version and the records already formed in the aforementioned steps will be retained, and the return track segment will not be replaced by the mission endpoint or an unverified track segment. When multiple control track segments meet the conditions simultaneously, the control track segment that arrives first will be selected according to the execution sequence number, and will not be selected based on track distance or estimated time.

[0074] The onboard computing unit reads the planned starting position, planned starting speed, planned starting acceleration, planned starting heading angle, and planned starting angular velocity of the reconnection track segment, and combines this with the track tracking allowable error and state measurement error of the reconnection track segment to form a reconnection state bundle. The state components of the reconnection state bundle use the same upper and lower bounds as the S4 switching state bundle. The time range of the reconnection state bundle is centered on the planned start time of the reconnection track segment, and its allowable deviation is determined by the time tracking allowable deviation in the current track version. The reconnection state bundle records at least the reconnection track segment identifier, position range, speed range, acceleration range, heading angle range, angular velocity range, and allowable access time range.

[0075] After the reconnection state bundle is formed, the airborne computing unit uses the time range corresponding to the switching state bundle formed in S4 as the starting point and the time range corresponding to the reconnection state bundle as the ending point to read the passable evidence, occupied evidence, and occlusion-maintaining evidence in the current environmental evidence version, and establishes a spatiotemporal support graph. Nodes in the spatiotemporal support graph are formed by the connected space of passable evidence within a time layer. The time layer is divided according to the flight controller control cycle or an integer multiple thereof; the width of the time layer used in this study is locked at the start of map construction.

[0076] Each node records at least its node identifier, its time layer, its accessible spatial range, the corresponding set of accessible evidence identifiers, and its center position. A directed connection is established between nodes in two adjacent time layers when the spatial reachability condition is met. Let the center position of the previous node be... The center position of the next node is The time difference between the two time layers is Then the spatial reachability condition is:

[0077] In the formula, This is the maximum permissible speed under the current flight dynamic constraints; The maximum permissible acceleration is required. In addition to meeting this distance condition, the transition space formed by the center positions of the nodes before and after the connection must be completely within the passable evidence body within the corresponding time range, and must not intersect with occupied evidence bodies or obstructing evidence bodies. If only the distance condition is met but the transition space lacks passable evidence coverage, a directed connection is not established.

[0078] Nodes that intersect the spatial range of the switching state control bundle and whose time layer falls within the switching state control bundle's time range are identified as starting nodes; nodes that intersect the spatial range of the reconnection state control bundle and whose time layer falls within the reconnection state control bundle's allowed access time range are identified as ending nodes. This step stops if the starting node is empty, the ending node is empty, or there is no directed connectivity from the starting node to the ending node.

[0079] The onboard computing unit searches the node sequence from the starting node to the ending node in an ascending direction of time layer. A transition cost is set for the directed connection from node m to node n:

[0080] In the formula, The cost of node transition; These are the expected velocities of the preceding and following nodes, calculated based on the node center position and time difference, respectively. The minimum distance from the transition space to the boundary of the occupied evidence body, in meters; These are the non-negative weights locked in this instance. If the value is not greater than zero, the connection is deleted, and the reciprocal term is not calculated. The predecessor nodes of each node are updated in ascending order of cumulative transition cost; when the cumulative cost is the same, the predecessor node with the earlier time layer and the smaller node identifier is selected in turn to ensure that a unique node sequence is obtained under the same record and the same parameters.

[0081] After obtaining the node sequence from the starting node to the ending node, the airborne computing unit determines the track connection point within the common passable range of adjacent nodes, and generates a piecewise fifth-order polynomial track with one allowed state in the switching state bundle as the starting boundary and one allowed state in the reconnection state bundle as the ending boundary. Each coordinate component is represented by the following formula:

[0082] In the formula, This is the local time within the current polynomial segment, starting from zero, in seconds; This represents the position of the coordinate component, in meters. to These are the polynomial coefficients. The six coefficients of each polynomial segment are solved using six boundary conditions: the starting position, starting velocity, starting acceleration, ending position, ending velocity, and ending acceleration of that segment. The corresponding matrix relationship is as follows:

[0083] In the formula, T is the duration of the current polynomial segment, which must be greater than zero; These are the initial position, initial velocity, and initial acceleration of the segment, respectively. These represent the termination position, termination velocity, and termination acceleration for that segment. The matrix must have a dimension of six rows and six columns. Candidate control track generation will stop if the matrix is ​​not invertible, the duration is not greater than zero, or the calculated coefficients contain non-finite values.

[0084] Adjacent polynomial segments share the same location, velocity, and acceleration boundaries at common connection points to ensure continuous connection between segments. Candidate control tracks are formed by arranging all polynomial segments according to their execution time. Each candidate control track records at least the candidate track version identifier, the connected switching state bundle identifier, the reconnection state bundle identifier, the node sequence, the coefficients of each polynomial segment, the planned execution time range, the version of the environmental evidence used, and the version of the flight dynamic constraints. Once the candidate control track is successfully written, it enters S6; in case of writing failure, an incomplete track is not provided to S6.

[0085] S6. Sequentially verify the dynamic constraints, environmental evidence back support, and dual-end state transition of the candidate control tracks; after verification, terminate the current track version and activate the candidate track version at the same switching time. The specific implementation is as follows: This step is performed by the airborne computing unit in the following order: dynamic constraint verification, environmental evidence reverse support verification, and dual-end state succession verification. If the previous verification fails, the subsequent verifications will stop, the candidate track version will remain inactive, and the current track version will remain in its original effective state.

[0086] The dynamic constraint verification uses the version of flight dynamic constraints locked when generating candidate control paths using S5. The airborne computing unit calculates the first, second, and third derivatives of the position function for each polynomial segment, obtaining the velocity, acceleration, and jerk.

[0087] Traverse all planned execution times of candidate control tracks according to the flight controller control cycle, and check each point one by one:

[0088] In the formula, This represents the maximum permissible jerk; other variables follow the meanings in S4. The extreme values ​​of velocity, acceleration, or jerk within a polynomial segment are also obtained through the stationary points of the corresponding derivative functions; it is not sufficient to check only the discrete sampling time. If any component cannot be calculated, any extreme value exceeds the flight dynamics constraints, or the velocity or acceleration of adjacent polynomial segments is discontinuous, the dynamics constraint check fails.

[0089] After the dynamic constraint verification is passed, the environmental evidence is used to support the verification. Starting from the last control track segment of the candidate control track, the airborne computing unit generates the sweep space volume of the candidate control track segment by segment in the reverse direction of the execution order. The sweep space volume is formed using the same airframe reference space, position error space, and track tracking allowable error space as S2, without using a reduced airframe boundary.

[0090] For each candidate control track segment, obtain valid, passable evidence from the current environmental evidence version according to its planned execution time, and check:

[0091] In the formula, The swept space of the qth control track segment of the candidate control track at time t; The set of passable evidence bodies that cover the candidate control track segment at that moment; These refer to the occupancy evidence body and the obstruction preservation evidence body in the current version of environmental evidence. The reverse support verification of environmental evidence passes when all control track segments from the candidate control track endpoint back to the starting point meet the above conditions. The reverse verification immediately stops if any control track segment lacks complete passable evidence coverage or intersects with either the occupancy evidence body or the obstruction preservation evidence body.

[0092] After the environmental evidence reverse support verification is passed, the dual-end state transition verification is performed. The airborne computing unit obtains the position, velocity, acceleration, heading angle and angular velocity at the start time of the candidate control track, and checks whether each item falls within the corresponding upper and lower bounds of the switching state bundle formed by S4; then it obtains the position, velocity, acceleration, heading angle and angular velocity at the end time of the candidate control track, and checks whether each item falls within the corresponding upper and lower bounds of the return state bundle formed by S5.

[0093] Let the h-th component of the candidate control track initial state be... The lower and upper bounds corresponding to the switching state control are respectively The h-th component of the candidate control track termination state is The lower and upper boundaries of the reconnected state tube bundle are respectively The condition for accepting both ends is:

[0094] The dual-end state transition check passes when all position, velocity, acceleration, heading angle, angular velocity, and time components meet the corresponding intervals. No error cancellation or compensation from other components is applied if any component is outside the interval.

[0095] After all three checks pass, the onboard computing unit determines the track version switching time based on the control cycle boundaries that the flight controller has not yet executed. This switching time must fall within the time range of the switching state constraint and must not be earlier than the termination time of the issued control command. In the same indivisible version submission, the onboard computing unit writes the valid termination time of the current track version as this switching time, writes the valid start time of the candidate track version as the same switching time, and updates the current valid track pointer of the flight controller to the candidate track version.

[0096] Once the current track version termination record, candidate track version activation record, and flight controller track pointing record are all successfully written, the candidate track version becomes effective. If any record writing fails, all version status changes resulting from this submission are revoked; the current track version remains valid, while the candidate track version remains inactive. If a request for activation with the same candidate track version identifier is received repeatedly, the existing submission record is first checked. If the existing submission record is complete, the track is not switched again; if the existing submission record is incomplete, execution restarts from the beginning of this submission, without generating a duplicate candidate control track.

[0097] After this step is completed, the track version record area will at least save the current track version termination time, the candidate track version activation time, the candidate control track, the environmental evidence version identifier, the track support relationship version identifier, the switch status control identifier, the reconnection status control identifier, three verification records, and the version submission status. After the candidate track version becomes effective, its control track segment becomes the currently valid track read in the next round of S2.

[0098] In one preferred embodiment, the UAV flies along its current valid trajectory within a low-altitude passage between buildings. The equivalent circumscribed radius of the UAV's fuselage and rotor working range is 0.35 meters, the upper limit of position measurement error is 0.08 meters, and the allowable trajectory tracking deviation of the flight controller is 0.12 meters. The lidar outputs environmental perception data at 10 Hz, the inertial measurement unit outputs UAV status data at 200 Hz, and the flight controller control cycle is 0.02 seconds. The maximum permissible speed is 3 meters per second, the maximum permissible acceleration is 2 meters per second squared, the maximum permissible jerk is 8 meters per second cubed, and the maximum permissible angular velocity is 1.2 radians per second.

[0099] In S1, the acquisition time for one frame of environmental perception data from the LiDAR is 12.00 seconds, and the corresponding coordinates of the sensing point in the LiDAR coordinate system are:

[0100] The rotation matrix from the lidar coordinate system to the UAV body coordinate system is the identity matrix, and the translation vector is:

[0101] The location of the drone at the time of data collection is:

[0102] The UAV attitude rotation matrix is ​​an identity matrix. Substituting into the coordinate transformation formula, we get:

[0103] The upper limit of the device measurement error at this sensing point is 0.05 meters, the upper limit of the position error is 0.08 meters, the upper limit of the coordinate transformation error is 0.02 meters, and the occupied extension radius is:

[0104] The sensing point and its 0.15-meter extended range form an occupancy evidence body. The space between the lidar emission position and the occupancy extension boundary, through which an effective ray passes, forms a passable evidence body. In the previous version of environmental evidence, the occupancy space located behind the building corner did not obtain complete penetration evidence because the ray in this frame was intercepted by the wall in front; therefore, this space forms an obstruction-preserving evidence body. The above constitutes environmental evidence version A.

[0105] In S2, the currently valid track is divided into six control track segments according to the execution order, labeled A1 to A6. The planned execution times of A1 to A6 are arranged consecutively. Based on the 0.35-meter equivalent airframe radius, 0.08-meter position error, and 0.12-meter tracking deviation, each control track segment generates a swept space volume with an extended radius of 0.55 meters. The traversable evidence bodies in environmental evidence version A cover the swept space volumes of A1 to A6, with A3 and A4 jointly supported by traversable evidence body B3. The onboard computing unit associates each control track segment, swept space volume, and corresponding traversable evidence body to form track support relationship version A.

[0106] Subsequently, environmental evidence version B is formed. In environmental evidence version B, a portion of the space corresponding to the accessible evidence body B3 is occupied by the newly detected mobile device, and this portion of the space changes from accessible evidence body to occupied evidence body. In S3, this changed space is identified as a changed evidence body. After checking along the track support relationship version A, the swept space volumes of A3 and A4 cannot be completely covered by the remaining accessible evidence bodies, and the support validity status of A3 and A4 is 0. A3 and A4 have adjacent execution sequence numbers, and the planned termination time of A3 is equal to the planned start time of A4, thus merging to form a candidate failed track interval with a start time of 14.00 seconds and an end time of 18.00 seconds.

[0107] In S4, this step begins at 12.00 seconds, and the unfinished control commands issued by the flight controller continue until 12.40 seconds. At 12.00 seconds, the UAV's position component along its flight path is 10.00 meters, its velocity is 2.00 meters per second, and the control command-given acceleration is 0.50 meters per second squared. Integrating over a time step of 0.02 seconds, the equivalent velocity at 12.40 seconds is:

[0108] The corresponding position is:

[0109] This establishes a command acceptance state. The planned arrival time at the termination boundary of the current valid track (A2) is 13.20 seconds, and the available time is:

[0110] The planned speed at the termination boundary of Class A2 is 2.20 meters per second, the planned heading angle is consistent with the expected heading angle in the command acceptance state, and the control response delay is 0.20 seconds. Therefore, the shortest maneuver time is:

[0111] Since 0.80 seconds is greater than 0.20 seconds, and the termination boundary of the second-stage track is located before the start time of the candidate failure track interval, the termination boundary of the second-stage track is determined as the execution commitment frontier. Centered on this boundary plan state, and combined with position measurement error and tracking deviation, a switching state bundle with a position tolerance of ±0.20 meters is formed.

[0112] In S5, the examination begins after A4. The supporting evidence set for A5 remains complete and valid under environmental evidence version B, and A5 is identified as the reconnection track segment. A reconnection state bundle is formed with the initial state of A5 as the center. The airborne computing unit establishes a spatiotemporal support graph within the time range from the switching state bundle to the reconnection state bundle, and obtains a directed node sequence passing through four consecutive time layer nodes.

[0113] One lateral position component in the candidate control trajectory needs to change from 0 meters to 1 meter within 2 seconds, with the initial velocity, initial acceleration, final velocity, and final acceleration all being 0. Substituting the boundary conditions into a fifth-order polynomial yields:

[0114] Its speed is:

[0115] Its acceleration is:

[0116] Its jerk is:

[0117] The polynomial coefficients of the other two coordinate components are obtained in the same way. The components are then combined according to the same time parameters to form candidate control tracks for connecting the switching state control bundle and the reconnecting state control bundle.

[0118] In S6, the maximum velocity of the lateral component is 0.9375 m / s, the maximum acceleration is approximately 1.4434 m / s², and the maximum jerk is 7.5 m / s³, all of which do not exceed the corresponding dynamic constraints. After merging the three coordinate components, the maximum velocity, maximum acceleration, and maximum jerk are checked again according to the vector magnitude. After the dynamic constraint check is passed, the swept space volume is checked segment by segment from the candidate control track endpoint to the starting point. Each segment is completely covered by the passable evidence body in environmental evidence version B, and does not intersect with the occupied evidence body or the occupancy evidence body. The candidate control track's starting state falls into the switching state constraint, and its ending state falls into the reconnection state constraint. The dual-end state acceptance check is passed. The airborne computing unit determines 13.20 seconds as the switching time. In the same version submission, the effective ending time of the current track version and the effective starting time of the candidate track version are both written as 13.20 seconds, and the flight controller's current effective track is pointed to the candidate track version.

[0119] In an equivalent implementation, environmental perception data is acquired by a depth camera in conjunction with millimeter-wave radar. Spatial evidence is not represented by fixed voxels, but by a set of convex polyhedra. After the effective depth points in the depth map are transformed by the UAV pose corresponding to the acquisition time, they form occupancy polyhedra. The view frustum space between the imaging center and the effective depth points forms a passable polyhedron, and the previous occupancy space occupies by the foreground occupancy polyhedron forms an occlusion-preserving polyhedron. The swept space volume, supporting evidence set, changing evidence volume, candidate failed track interval, command acceptance state, execution commitment front, switching state constraint, reconnecting track segment, reconnecting state constraint, spatiotemporal support map, and candidate control track of the control track segment are still formed sequentially according to the aforementioned steps. The processing logic for dynamic constraint verification, reverse support verification of environmental evidence, dual-end state acceptance verification, and simultaneous switching of track versions remains unchanged.

[0120] In the operational scenario shown in this embodiment: the UAV is used to inspect a low-altitude passage between buildings. Fixed building structures exist on both sides of the passage, and mobile devices capable of entering the flight path area are located at the passage's corners. The UAV flies from the passage entrance towards the inspection target according to a pre-defined, currently valid flight path. The equivalent circumscribed radius of the UAV's fuselage and rotor working range is 0.35 meters, the upper limit of position measurement error is 0.08 meters, the flight controller's allowable track tracking deviation is 0.12 meters, the onboard lidar outputs environmental perception data at 10 Hz, the inertial measurement unit outputs UAV status data at 200 Hz, the flight controller's control cycle is 0.02 seconds, the maximum allowable speed corresponding to the UAV's current load state is 3 meters per second, the maximum allowable acceleration is 2 meters per second squared, the maximum allowable jerk is 8 meters per second cubed, and the maximum allowable angular velocity is 1.2 radians per second.

[0121] When the UAV flies to the corner in front of the building, the onboard computing unit acquires a frame of LiDAR environmental perception data at 12.00 seconds. At the same time, it reads the UAV's position, speed, acceleration, attitude, heading angle, and angular velocity records before and after the acquisition time from the flight controller, inertial measurement unit, and positioning unit. The coordinates of one effective sensing point in the LiDAR coordinate system are (4.20 m, 1.10 m, 0.30 m), the installation translation of the LiDAR relative to the UAV's reference point is (0.10 m, 0 m, 0.05 m), and the UAV's position at 12.00 seconds is (10.00 m, 5.00 m, 2.00 m). At this moment, the UAV's attitude rotation matrix and the LiDAR installation rotation matrix are both identity matrices. The onboard computing unit converts the above sensing points to a unified flight coordinate system, obtaining the coordinates (14.30 m, 6.10 m, 2.35 m). Based on the upper limit of the lidar measurement error (0.05 meters), the upper limit of the UAV position error (0.08 meters), and the upper limit of the coordinate transformation error (0.02 meters), the occupancy extension radius of the sensing point is calculated to be 0.15 meters. The spatial units corresponding to the sensing point and its extension range are formed into occupancy evidence bodies. The spatial units between the lidar emission position and the occupancy extension boundary that are completely traversed by the effective detection ray are formed into passable evidence bodies. For the original occupied space located behind the corner of the building in the previous environmental evidence version, since the current detection ray is cut off by the building structure in front and fails to completely pass through the original occupied space, the airborne computing unit does not cancel the original occupancy status because no corresponding echo is obtained in this frame. Instead, it forms the space into an obstruction preservation evidence body, thus forming environmental evidence version A.

[0122] The currently active track invoked by the flight controller is divided into control track segments A1, A2, A3, A4, A5, and A6 by the onboard computing unit according to the execution order. The planned execution time of each control track segment is consecutive. Based on the UAV's equivalent circumscribed radius of 0.35 meters, position measurement error of 0.08 meters, and track tracking deviation of 0.12 meters, the onboard computing unit generates a swept space volume for each control track segment with an extension range of 0.55 meters. Then, the accessible evidence body that completely covers each swept space volume and the corresponding planned execution time in environmental evidence version A is associated with the corresponding control track segment. Among them, A3 and A4 are jointly supported by accessible evidence body B3, thus forming track support relationship version A.

[0123] As the UAV continues to approach the corner, the lidar detects in subsequent data frames that the mobile device has entered the channel space corresponding to the accessible evidence body B3. The onboard computing unit completes coordinate transformation according to the UAV's pose corresponding to the new acquisition time, forming a new occupancy evidence body from the space occupied by the mobile device, and creating environmental evidence version B. The portion of space in environmental evidence version B that originally belonged to accessible evidence body B3 is converted into occupancy evidence body, and this portion of space is identified as change evidence body. The onboard computing unit checks the control track segments segment by segment along the track support relationship version A, and finds that the swept space bodies of A3 and A4 can no longer be completely covered by the accessible evidence bodies in environmental evidence version B, and that the swept space bodies intersect with the newly added occupancy evidence bodies. Therefore, the support validity status of A3 and A4 is set to 0. Since the execution sequence numbers of A3 and A4 are adjacent, and the planned termination time of A3 is equal to the planned start time of A4, the onboard computing unit merges the two into a candidate failed track interval. The planned start time of this interval is 14.00 seconds, and the planned termination time is 18.00 seconds.

[0124] When a candidate failure track interval is formed, the UAV does not immediately change its course from its current position. Instead, the onboard computing unit reads the control commands that have been issued but not yet executed from the flight controller. The issued control commands continue until 12.40 seconds. At 12.00 seconds, the UAV's position component along the current track direction is 10.00 meters, its speed is 2.00 meters per second, and the acceleration corresponding to the control command is 0.50 meters per second squared. After gradually integrating according to the control cycle until 12.40 seconds, the expected speed of 2.20 meters per second and the expected position of 10.84 meters are obtained. The expected position, expected speed, expected acceleration, expected heading angle, and expected angular velocity at 12.40 seconds are then used to form the command acceptance state.

[0125] The airborne computing unit sequentially checks the control track segment boundaries before the candidate failed track intervals along the current valid track. The planned arrival time of the second termination boundary is 13.20 seconds, and the available time from the command acceptance time of 12.40 seconds to this boundary is 0.80 seconds. The planned speed of the second termination boundary is 2.20 meters per second, and the planned heading angle is consistent with the expected heading angle in the command acceptance state. The control response delay is 0.20 seconds. The shortest maneuver time is determined to be 0.20 seconds by the maximum value among the speed adjustment time, heading adjustment time, and control response delay. Since 0.80 seconds is not less than 0.20 seconds, and the termination boundary of the second stage is located before the start time of the candidate failed track interval, the airborne computing unit determines the termination boundary of the second stage as the execution commitment frontier, and retains the track before the boundary as the continued execution part; then, taking the planned state at the termination boundary of the second stage as the center, based on the command acceptance state prediction error, the position measurement error of 0.08 meters, and the track tracking deviation of 0.12 meters, a switching state bundle is formed with a position allowable deviation of ±0.20 meters and simultaneously limits the range of speed, acceleration, heading angle, angular velocity, and access time.

[0126] Subsequently, the airborne computing unit checks A5 and A6 according to the execution order after the termination position of the candidate failed track section. It determines that the swept space volume of A5 is still completely covered by the passable evidence volume under environmental evidence version B, and does not intersect with the occupied evidence volume or the obstruction maintenance evidence volume. Therefore, A5 is determined as the reconnection track segment, and a reconnection status bundle is formed with the planned starting position, speed, acceleration, heading angle, angular velocity and allowable access time of A5 as the center.

[0127] The airborne computing unit, starting from the time range corresponding to the switching state control bundle and ending with the time range corresponding to the reconnection state control bundle, divides the passable evidence in environmental evidence version B into spatiotemporal support graph nodes according to time layers. Within adjacent time layers, directed connections are established only between nodes whose spatial movement distance meets the constraints of maximum permissible speed and maximum permissible acceleration, and whose transition space is completely covered by the passable evidence. This ultimately yields a directed node sequence from the switching state control bundle to the reconnection state control bundle, passing through four consecutive time layer nodes. After determining the track connection points based on this node sequence, the airborne computing unit generates a candidate control track in the form of a piecewise fifth-order polynomial. One lateral position component needs to change from 0 meters to 1 meter within 2 seconds, with the initial velocity, initial acceleration, final velocity, and final acceleration all being 0. The lateral position component is obtained from six boundary conditions as follows:

[0128] in The other two coordinate components are obtained according to the same time parameters, and the components are combined to form a candidate control track for connecting the switching state control bundle and the reconnecting state control bundle.

[0129] The airborne computing unit first performs dynamic constraint verification on the candidate control track, calculating the maximum velocity of the lateral component to be 0.9375 m / s, the maximum acceleration to be approximately 1.4434 m / s², and the maximum jerk to be 7.5 m / s³. Then, it checks the velocity, acceleration, jerk, and angular velocity of the three coordinate components, confirming that none exceed the locked flight dynamic constraints. Subsequently, it generates sweep space volumes segment by segment in reverse from the endpoint of the candidate control track, confirming that each sweep space volume is completely covered by the passable evidence volume in environmental evidence version B within the corresponding planned execution time, and does not intersect with occupied evidence volumes or obstruction-maintaining evidence volumes. It then checks that the initial state of the candidate control track falls into the switching state control and the termination state falls into the reconnection state control, thus completing the dynamic constraint verification, environmental evidence reverse support verification, and dual-end state continuity verification.

[0130] After all three checks pass, the onboard computing unit determines 13.20 seconds as the track version switching time. In the same indivisible version submission, it writes the effective end time of the original current track version as 13.20 seconds, writes the effective start time of the candidate track version as 13.20 seconds, and updates the current effective track pointer of the flight controller to the candidate track version. After completing the control command corresponding to A2, the flight controller enters the candidate control track from the state range defined by the switching state control, bypasses the mobile device entering the original track area, and then connects to A5 from the state range defined by the reconnection state control, so that A5 and A6 continue to be executed as subsequent effective tracks. If any of the above version termination record, candidate track version activation record, and flight controller track pointer record fails to be written, the state change formed by this submission is revoked, the original current track version remains valid, and the candidate track version remains inactive.

[0131] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for UAV trajectory planning in complex environments, characterized in that, include: S1. Acquire UAV status data and environmental perception data, convert environmental perception data according to the pose corresponding to the acquisition time, and form an environmental evidence version containing occupancy evidence, passable evidence, and occlusion preservation evidence. S2. Divide the current valid track into control track segments according to the execution order, generate a swept space volume, associate the passable evidence bodies covering the swept space volume into a supporting evidence set, and form a track support relationship version. S3. When the environmental evidence version is updated, the changed evidence body is determined, the support failure control track segment is located along the track support relationship version, and the continuous control track segments are merged to form the candidate failure track interval. S4. Based on the issued control commands, flight status, and flight dynamic constraints, predict the command acceptance status, determine the execution commitment frontier along the current valid flight path, and form a switching state control bundle; S5. After the candidate failed track interval, determine the supporting valid reconnection track segment to form a reconnection state bundle; establish a spatiotemporal support map based on the environmental evidence version, and generate candidate control tracks connecting the two state bundles. S6. Verify the dynamic constraints, environmental evidence back support, and dual-end state transition of the candidate control track in sequence; after verification, terminate the current track version and activate the candidate track version at the same switching time.

2. The method for UAV trajectory planning in complex environments according to claim 1, characterized in that, S1 includes: The effective echo points and the extended range determined by the measurement errors of environmental sensing equipment, the position errors of UAVs, and the coordinate transformation errors form the occupancy evidence body; The effective detection space from the location of the sensing device to the first valid echo point forms a passable evidence body; The occupied space that was not re-observed in the previous version of environmental evidence because the detection ray was cut off by the occupied evidence body located in front of the ray is formed into a shielding evidence body.

3. The method for UAV trajectory planning in complex environments according to claim 1, characterized in that, S2 include: Based on the UAV body reference space, position measurement error space, and track tracking allowable error space, a sweep space volume is formed according to the desired position and desired attitude at each moment of the control track segment; A passable evidence body is written into the supporting evidence set only if the swept space body is completely covered by a passable evidence body during the planned execution time and does not intersect with an occupied evidence body or an obstruction-preserving evidence body.

4. The method for UAV trajectory planning in complex environments according to claim 1, characterized in that, S3 includes: By comparing the previous and updated versions of environmental evidence with the unified spatial unit identifier, the spatial units of evidence type conversion and the spatial units of occupied boundary ownership change are respectively identified as change evidence bodies. The corresponding control track segment will be designated as a support failure control track segment in any of the following circumstances: the swept space volume is not completely covered by the set of valid supporting evidence after the update; The swept space body intersects with the occupied evidence body and the obstructed evidence body.

5. The method for UAV trajectory planning in complex environments according to claim 1, characterized in that, S4 includes: According to the execution order of the issued control commands, the position, speed, and heading competition time periods are recursively calculated based on the flight controller control cycle to obtain the command acceptance status at the end of the execution of all issued control commands; For the control track segment boundary of the current effective track, calculate the available time and the shortest maneuver time determined by the speed adjustment time, heading adjustment time, and control response delay. The boundary with the first available time before the candidate failed track segment that is not less than the shortest maneuver time is determined as the execution commitment frontier.

6. The method for UAV trajectory planning in complex environments according to claim 1, characterized in that, S5 include: Starting from the next control track segment after the termination of the candidate failed track segment, select the first support valid control track segment as the follow-up track segment according to the execution sequence number; Using the connected space of the accessible evidence body in each time layer as nodes, in adjacent time layers, a directed connection is established for nodes whose center distance is no greater than the product of the maximum allowable velocity and the time layer difference plus half the product of the maximum allowable acceleration and the square of the time layer difference, and whose transition space is completely covered by the accessible evidence body, thus forming a spatiotemporal support graph.

7. The method for UAV trajectory planning in complex environments according to claim 6, characterized in that, Search the node sequence for switching state control bundles and reconnecting state control bundles in the ascending direction of the time layer; The node transition cost is the sum of the first non-negative weighted value of the distance from the node center and the ratio of the velocity change to the maximum allowable acceleration, and the second non-negative weighted value of the reciprocal of the minimum distance from the transition space to the boundary of the occupied evidence body; The path connection point is determined based on the node sequence with the minimum cumulative transition cost, and the start and end positions, start and end velocities, and start and end accelerations are used as boundary conditions to generate piecewise quintic polynomial candidate control paths.

8. The method for UAV trajectory planning in complex environments according to claim 1, characterized in that, S6 include: Calculate the first, second, and third derivatives of the position function of the candidate control trajectory to determine the velocity, acceleration, and jerk, and compare them with the corresponding flight dynamic constraints; Sweep space volumes are generated segment by segment from the endpoint of the candidate control track to the starting point. When each sweep space volume is completely covered by a valid passable evidence volume and does not intersect with an occupied evidence volume or an obstruction evidence volume, the environmental evidence reverse support verification is passed.

9. The method for UAV trajectory planning in complex environments according to claim 8, characterized in that, Compare the candidate control track start states with the switching state control bundle one by one, and compare the termination states with the reconnection state control bundle one by one. Once all state components fall within their corresponding upper and lower bounds, the current valid end time of the current track version, the valid start time of the candidate track version, and the current valid track direction of the flight controller are written in the same indivisible commit. The candidate track version takes effect when all three writes are successful, and the current state change is revoked when any write fails.

Images