Heavy-load unmanned aerial vehicle constraint obstacle avoidance planning method and system

Abstract

The present application relates to the technical field of unmanned aerial vehicle trajectory planning and flight control, and particularly relates to a heavy-load unmanned aerial vehicle constraint obstacle avoidance planning method and system. The method time-aligns millimeter wave radar observation, motion state, load state, task target and trajectory execution state; updates environment space unit occupancy probability according to echo hit, ray crossing and occlusion relationship, forms a probability environment risk state, and generates a load dynamics constraint set and a short-time motion reachable domain; combines braking distance, time delay displacement and trajectory tracking error to form a risk expansion area, forms a safe and feasible flight domain and a to-be-executed trajectory segment; sets a commitment trajectory prefix, generates and corrects a subsequent trajectory during execution, and outputs a subsequent trajectory or a degraded trajectory according to a remaining safe execution time domain, available time and trajectory continuity. The present application forms a constraint between environment risk, dynamics boundary and trajectory continuity timing, reduces occlusion misjudgment, dynamics overrun and continuity lag.

Description

Technical Field

[0001] This invention relates to the fields of autonomous navigation, trajectory planning and flight control technology for unmanned aerial vehicles (UAVs), and in particular to a constrained obstacle avoidance planning method and system for heavy-load UAVs. Background Technology

[0002] In the field of autonomous navigation technology for unmanned aerial vehicles (UAVs), existing obstacle avoidance solutions for heavy-duty UAVs typically acquire obstacle positions and distances using lidar, millimeter-wave radar, visual sensors, or ultrasonic sensors. Based on the observation results, they construct occupancy grid maps, 3D environment models, or local obstacle regions. These are then combined with the mission objective, current motion state, safe distance, and UAV kinematic constraints to generate a global path or local trajectory. During flight, the planning module updates the environmental state and trajectory according to a fixed control cycle, and the flight control system performs position, velocity, or attitude control based on the trajectory point sequence. When a collision risk with an obstacle is detected, the trajectory is typically replanned, or the system switches to deceleration, braking, or hovering.

[0003] Millimeter-wave radar observations suffer from issues such as sparse echoes, measurement errors, and obstacle obstruction. Existing methods, when establishing environmental conditions, typically treat radar echo locations directly as obstacle locations, or consider spaces without detected echoes as unobservable areas. This makes it difficult to distinguish between spaces traversed by the radar ray, spaces hit by the echo, and spaces behind the echo that were not effectively observed. Furthermore, for obstacle areas not subsequently observed, the fixed occupancy state may differ from the current environmental condition, leading to discrepancies between the environmental constraints used in local trajectory planning and the actual observation state.

[0004] Existing obstacle avoidance schemes for heavy-load UAVs mostly describe flight capabilities using pre-set maximum speed, acceleration, safe distance, or minimum turning radius. When payload mass, center of gravity, inertia, available thrust, or oscillation state changes, the UAV's braking distance, turning boundaries, and attitude response range also change. Fixed dynamic parameters are insufficient to accurately describe the current flight conditions. Environmental risk constraints and dynamic constraints are usually treated as independent trajectory planning conditions, lacking a unified organization of probabilistic environmental risk states and short-term motion reachability under the current payload conditions. This easily leads to situations where a local target is traversable in geometric space, but the corresponding trajectory exceeds the current braking or turning boundaries.

[0005] Furthermore, existing rolling trajectory planning methods often generate the next trajectory according to a fixed planning cycle, or replan after a collision risk is detected. When environmental complexity, onboard computing load, trajectory verification time, trajectory transmission delay, or flight control switching delay changes, the actual usable time of the next trajectory may be later than the end time when the current trajectory can still be safely executed. If the theoretical terminal state of the current trajectory is directly used as the initial state of the next trajectory, deviations in actual position, velocity, and acceleration will also affect trajectory continuity. Therefore, it is necessary to address the inconsistency between the formation of a safe and feasible flight domain and the remaining safe execution time-domain control, considering the combined operating conditions of millimeter-wave radar observation uncertainty, changes in heavy-load dynamic boundaries, and online continuity of segmented trajectories. Summary of the Invention

[0006] To address the above problems, this invention provides a constrained obstacle avoidance planning method for heavy-load unmanned aerial vehicles (UAVs), which solves the problem of how to form a safe and feasible flight domain and complete trajectory continuity according to the remaining safe execution time domain under uncertain millimeter-wave radar observation and changing heavy-load dynamic boundaries.

[0007] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0008] S100: Acquire millimeter-wave radar observations, current motion state, current payload state, mission target state, and current trajectory execution state; perform time alignment on the millimeter-wave radar observations, the current motion state, and the current trajectory execution state to generate a planning cycle state set;

[0009] S200. Based on the planning cycle state set, update the occupancy probability of the environmental space unit according to the radar ray echo hit area, ray crossing area and obstruction area, and generate the probabilistic environmental risk state; determine the load dynamic constraint set according to the current load state, and generate the short-time motion reachability domain.

[0010] S300: Generate a risk extension region based on the probabilistic environmental risk state, braking distance, time delay displacement, and trajectory tracking error; exclude the risk extension region from the short-term motion reachability domain to generate a safe and feasible flight domain; generate a trajectory segment to be executed based on the safe and feasible flight domain;

[0011] S400: Set the beginning of the trajectory segment to be executed as a committed trajectory prefix and generate a predicted terminal state; generate a subsequent trajectory during the execution of the committed trajectory prefix and correct its initial state according to the actual motion state; verify the remaining part of the current trajectory to obtain the remaining safe execution time domain, and determine the expected available time of the subsequent trajectory according to the expected trajectory generation duration and delay; based on the remaining safe execution time domain, the expected available time of the subsequent trajectory, and the position, velocity, and acceleration continuity verification results, output the subsequent trajectory or the degraded trajectory.

[0012] Furthermore, updating the occupancy probability of environmental spatial units according to the radar ray's echo hit area, ray penetration area, and obstruction area includes:

[0013] Perform ray traversal on multiple radar beams;

[0014] The environmental space unit between the millimeter-wave radar and the radar echo position, and within the radar's effective field of view, is defined as the ray-crossing space unit.

[0015] The environmental spatial unit covered by the radar echo position and radar measurement covariance is defined as the echo hit spatial unit.

[0016] The environmental spatial unit behind the echo-hit spatial unit and within the effective field of view of the radar is defined as the occlusion unknown spatial unit.

[0017] Furthermore, the occupancy probability of the updated environmental spatial unit includes:

[0018] The occupancy probability of each environmental space unit in the previous planning cycle is converted into log odds;

[0019] An occupation update is added to the logarithmic probability of the echo hitting a spatial cell, and an idle update is added to the logarithmic probability of the ray traversing a spatial cell, while keeping the logarithmic probability of the occluded unknown spatial cell unchanged.

[0020] Perform probability decay on the logarithmic odds of continuously unobserved environmental spatial units;

[0021] Based on the occupancy entry threshold, occupancy exit threshold, idle entry threshold, and idle exit threshold, a high-confidence occupancy area, a low-risk passable area, and an unknown risk area are generated, forming the probabilistic environmental risk state.

[0022] Furthermore, the current load status includes load mass, load center of gravity, load inertia parameters, currently available thrust, and load oscillation status;

[0023] The current load state is matched with the dynamic parameter version to obtain the maximum permissible speed, maximum permissible acceleration, maximum permissible braking deceleration, maximum permissible jerk, maximum permissible tilt angle, maximum permissible curvature, minimum permissible turning radius, and available thrust boundary, thereby generating the load dynamic constraint set.

[0024] Furthermore, the generation of the short-term motion reachable domain includes:

[0025] Starting from the current motion state, multiple candidate velocity states are generated within the prediction time window;

[0026] The candidate velocity states are integrated according to the maximum permissible acceleration and the maximum permissible jerk to generate multiple candidate motion states containing position, velocity and predicted time.

[0027] Based on the maximum permissible curvature, maximum permissible tilt angle, and available thrust boundary, candidate motion states that are not executable are filtered out, and the short-term motion reachability domain is generated based on the remaining candidate motion states.

[0028] Furthermore, the generation of the risk expansion area and the generation of the safe and feasible flight domain include:

[0029] Based on the basic safety clearance, the braking distance corresponding to the current speed and the maximum permissible braking deceleration, the time delay displacement corresponding to the trajectory transmission delay and the flight control switching delay, the trajectory tracking error, and the radar measurement covariance, a dynamic safety extension quantity is generated;

[0030] The risk expansion region is generated by expanding the high-confidence occupied region and the unknown risk region according to the dynamic security expansion amount.

[0031] Candidate motion states that intersect with the risk extension region in the short-term motion reachability domain are excluded, and the safe and feasible flight domain is generated based on the remaining candidate motion states and their predicted times.

[0032] Furthermore, the generation of the trajectory segment to be executed based on the safe and feasible flight domain includes:

[0033] Based on the connectivity, channel width, and reachability in the direction of the mission objective of the safe and feasible flight domain, the local target position, target velocity direction, and estimated arrival time are determined, and the local target state is generated.

[0034] Based on the current motion state and the local target state, model predictive control is used to generate candidate trajectory segments with time parameters;

[0035] The safe and feasible flight domain, maximum permissible speed, maximum permissible acceleration, maximum permissible jerk, and maximum permissible curvature are set as trajectory constraints for the model predictive control, and the trajectory segment to be executed is generated based on the candidate trajectory segments that satisfy the trajectory constraints.

[0036] Furthermore, the steps of setting the committed trajectory prefix, generating the predicted terminal state, and correcting the initial state of subsequent trajectories include:

[0037] The committed time domain is determined based on the current planning cycle, the effective duration of the safe and feasible flight domain, the duration of historical trajectory generation, the trajectory transmission delay, and the flight control switching delay.

[0038] The portion of the trajectory segment to be executed that is located within the committed time domain is set as the committed trajectory prefix, and the position, velocity, and acceleration of the end of the committed trajectory prefix are recorded as the predicted terminal state;

[0039] Obtain the actual motion state during the execution of the committed trajectory prefix, and calculate the position deviation, velocity deviation, and acceleration deviation between the actual motion state and the predicted terminal state;

[0040] The initial state of the subsequent trajectory is corrected based on the position deviation, velocity deviation, and acceleration deviation.

[0041] Furthermore, the step of verifying the remaining portion of the current trajectory and outputting the subsequent trajectory or the downgraded trajectory includes:

[0042] Based on the latest probabilistic environmental risk state and the latest load dynamics constraint set, environmental risk constraint verification and dynamics constraint verification are performed sequentially along the remaining part of the current trajectory. The length of time during which the verification is passed consecutively is recorded as the remaining safe execution time domain.

[0043] The expected availability time of the subsequent trajectory is determined based on the expected trajectory generation time, trajectory verification time, trajectory transmission delay, flight control switching delay, and safety time margin.

[0044] The subsequent trajectory is output when the expected available time of the subsequent trajectory is earlier than the end time of the remaining safe execution time domain, and the subsequent trajectory satisfies the continuity conditions of position, velocity, and acceleration with the current trajectory.

[0045] If the expected available time of the subsequent trajectory is not earlier than the end time, or if the subsequent trajectory does not meet the continuity condition, the deceleration trajectory, braking trajectory, or hovering trajectory is output as the degraded trajectory according to the remaining safe execution time domain.

[0046] The present invention also provides a constraint obstacle avoidance planning system for heavy-load unmanned aerial vehicles, comprising:

[0047] The state alignment module is used to acquire millimeter-wave radar observations, current motion state, current payload state, mission target state, and current trajectory execution state, and to perform time alignment on the millimeter-wave radar observations, the current motion state, and the current trajectory execution state to generate a planning period state set;

[0048] The probabilistic environment state generation module, connected to the state alignment module, is used to update the occupancy probability of environmental space units based on the planning period state set according to the echo hit area, ray crossing area and obstruction area of ​​radar rays, and generate probabilistic environment risk state.

[0049] A load dynamics constraint generation module, connected to the state alignment module, is used to determine a load dynamics constraint set based on the current load state, and generate a short-time motion reachability domain based on the current motion state and the load dynamics constraint set;

[0050] The safe and feasible flight domain generation module is connected to the probabilistic environment state generation module and the load dynamics constraint generation module, respectively. It is used to generate a risk extension region based on the probabilistic environment risk state, braking distance, time delay displacement and trajectory tracking error, exclude the risk extension region from the short-time motion reachability domain, and generate a safe and feasible flight domain.

[0051] The trajectory planning module is connected to the safe and feasible flight domain generation module and the state alignment module, respectively. It is used to generate a trajectory segment to be executed based on the safe and feasible flight domain and the mission target state, set the beginning of the trajectory segment to be executed as a committed trajectory prefix, and generate a predicted terminal state.

[0052] The trajectory execution and continuation module, connected to the trajectory planning module, the state alignment module, the probabilistic environment state generation module, and the load dynamics constraint generation module, is used to generate subsequent trajectories based on the predicted terminal state during the execution of the committed trajectory prefix, correct the initial state of the subsequent trajectory according to the actual motion state, verify the remaining part of the current trajectory based on the latest probabilistic environment risk state and the latest load dynamics constraint set to obtain the remaining safe execution time domain, determine the expected available time of the subsequent trajectory based on the expected trajectory generation duration and delay, and output the subsequent trajectory or degraded trajectory based on the remaining safe execution time domain, the expected available time of the subsequent trajectory, and the position, velocity, and acceleration continuity verification results.

[0053] The key innovations of this invention include:

[0054] (1) Based on the echo hit area, ray crossing area and obstruction area of ​​millimeter-wave radar rays, different occupancy probability update rules are adopted for environmental space units; combined with radar measurement covariance, observation timeliness and probability decay, environmental space units are organized into high-confidence occupancy areas, low-risk passable areas and unknown risk areas to form probabilistic environmental risk states.

[0055] (2) Map the current load state to a load dynamics constraint set including velocity, acceleration, braking deceleration, jerk, tilt angle, curvature and available thrust boundary, and form a short-time motion reachable domain with predicted time based on this; then form a risk extension region based on probabilistic environmental risk state, braking distance, time delay displacement, trajectory tracking error and radar measurement covariance, and form a safe and feasible flight domain by excluding candidate motion states that intersect the two, and the safe and feasible flight domain simultaneously defines the local target state and the trajectory segment to be executed.

[0056] (3) Set the beginning of the trajectory segment to be executed as the committed trajectory prefix, and form the subsequent trajectory in parallel during the execution of the committed trajectory prefix. Correct the initial state of the subsequent trajectory according to the actual motion state. Verify the remaining part of the current trajectory according to the latest probabilistic environmental risk state and the latest load dynamics constraint set to form the remaining safe execution time domain. Then, use it together with the expected available time of the subsequent trajectory and the verification results of position, velocity and acceleration continuity as the control conditions for the subsequent trajectory continuation or degraded trajectory output.

[0057] The following are its main beneficial effects:

[0058] (1) By distinguishing the space that the radar ray has already traversed, the space that the echo actually hits, and the space behind the echo that has not been effectively observed, the obstructed area or the area without effective coverage is avoided from being directly identified as a passable area; combined with the observation time-based execution probability decay, the historical occupancy status can be updated with new radar observations, reducing the interference of single-frame false alarms and obstacle residual status on subsequent trajectory planning.

[0059] (2) By first forming the short-term motion reachable domain corresponding to the current load condition, and then excluding the dynamically expanded environmental risk state from it, the remaining candidate motion state simultaneously meets the braking, turning, attitude and thrust boundaries as well as the environmental risk constraints of the corresponding prediction time, thereby reducing the situation where local targets are passable in geometric space but the trajectory to be executed exceeds the current heavy load dynamic boundary.

[0060] (3) By comparing the remaining safe execution time domain obtained by the current trajectory after verification by the latest environmental risk constraints and load dynamic constraints with the generation, verification, transmission and flight control switching time of the subsequent trajectory, the subsequent trajectory can be switched or the deceleration, braking and hovering trajectory can be output before the current trajectory enters the part that does not meet the constraints; the initial state of the subsequent trajectory is corrected according to the actual position, speed and acceleration to reduce the trajectory continuity deviation caused by the inconsistency between the theoretical terminal state and the actual motion state. Attached Figure Description

[0061] Figure 1 A flowchart illustrating a constrained obstacle avoidance planning method for a heavy-load unmanned aerial vehicle (UAV) provided in an embodiment of this application;

[0062] Figure 2 This is a structural block diagram of a heavy-load unmanned aerial vehicle (UAV) constraint obstacle avoidance planning system provided in an embodiment of this application. Detailed Implementation

[0063] To enable those skilled in the art to better understand the technical solution, the present invention will be described in detail below with reference to embodiments. The description in this part is only exemplary and explanatory, and should not be used to limit the scope of protection of the present invention in any way.

[0064] Example 1: Refer to Figure 1 This is a flowchart illustrating a constrained obstacle avoidance planning method for heavy-load unmanned aerial vehicles (UAVs) provided in an embodiment of the present invention. The process may include at least steps S100-S400:

[0065] S100: Acquire millimeter-wave radar observations, current motion state, current payload state, mission target state, and current trajectory execution state; perform time alignment on the millimeter-wave radar observations, the current motion state, and the current trajectory execution state to generate a planning cycle state set;

[0066] S200. Based on the planning cycle state set, update the occupancy probability of the environmental space unit according to the radar ray echo hit area, ray crossing area and obstruction area, and generate the probabilistic environmental risk state; determine the load dynamic constraint set according to the current load state, and generate the short-time motion reachability domain.

[0067] S300: Generate a risk extension region based on the probabilistic environmental risk state, braking distance, time delay displacement, and trajectory tracking error; exclude the risk extension region from the short-term motion reachability domain to generate a safe and feasible flight domain; generate a trajectory segment to be executed based on the safe and feasible flight domain;

[0068] S400: Set the beginning of the trajectory segment to be executed as a committed trajectory prefix and generate a predicted terminal state; generate a subsequent trajectory during the execution of the committed trajectory prefix and correct its initial state according to the actual motion state; verify the remaining part of the current trajectory to obtain the remaining safe execution time domain, and determine the expected available time of the subsequent trajectory according to the expected trajectory generation duration and delay; based on the remaining safe execution time domain, the expected available time of the subsequent trajectory, and the position, velocity, and acceleration continuity verification results, output the subsequent trajectory or the degraded trajectory.

[0069] S100: Acquire millimeter-wave radar observations, current motion state, current payload state, mission target state, and current trajectory execution state; perform time alignment on the millimeter-wave radar observations, the current motion state, and the current trajectory execution state to generate a planning period state set.

[0070] In one implementation, the heavy-load UAV is equipped with a millimeter-wave radar, a combined navigation unit, an onboard computing unit, a payload status acquisition unit, and a flight control system. The millimeter-wave radar is mounted on the front and sides of the fuselage, and its output includes radar observation timestamps, radar echo positions, echo intensity, and radar measurement covariance. The combined navigation unit outputs the UAV's current position, current velocity, current acceleration, and current attitude based on satellite positioning data and inertial measurement data. The payload status acquisition unit provides the onboard computing unit with payload mass, payload center of gravity, payload inertia parameters, current available thrust, and payload oscillation status. The flight control system provides the trajectory segment identifiers, start time, execution progress, and actual motion status of the current execution trajectory. The mission target status is provided by the mission planning module, including the target position and target heading.

[0071] This step is performed by the state alignment module. The state alignment module is activated when the planning period arrives, when new millimeter-wave radar observations are received, when the current payload state changes, or when the current trajectory execution state approaches the trajectory segment switching position. For the same planning period, the state alignment module first selects the planning time, then reads two sets of motion states before and after the planning time from the state sequence of the integrated navigation unit, and interpolates the position, velocity, and acceleration according to the time interval. When the acquisition time of the millimeter-wave radar observation is inconsistent with the planning time, the radar echo is compensated from the radar acquisition time to the planning time based on the interpolated UAV pose, avoiding misalignment between the echo position and the UAV's current position caused by directly using asynchronous observations.

[0072] After time alignment, the state alignment module transforms the radar echo position from the radar coordinate system to the aircraft coordinate system based on the installation parameters of the millimeter-wave radar relative to the aircraft coordinate system. Then, based on the UAV's attitude and current position at the planned time, it transforms it to the local planning coordinate system. The origin of the local planning coordinate system updates with the UAV's planned position, and its horizontal direction maintains a fixed relationship with the mission target direction or geographic coordinate direction. When multiple millimeter-wave radars generate echoes simultaneously, each radar's observations undergo coordinate transformation separately, and then are merged according to the observation time difference and spatial distance. Echoes from different radars falling into the same environmental spatial unit retain their respective radar measurement covariances, and are not directly merged into a single defined obstacle point during subsequent probability updates.

[0073] The current motion state uses the position, velocity, acceleration, and attitude corresponding to the planning time. The current trajectory execution state includes not only the execution progress but also the theoretical position, theoretical velocity, and theoretical acceleration of the current trajectory at the planning time. The state alignment module compares the actual motion state with the theoretical motion state at the same time to obtain position deviation, velocity deviation, and acceleration deviation. These deviations are passed to subsequent steps as part of the trajectory tracking error. When the current planning period is the initial planning period and the current trajectory does not yet exist, the current trajectory execution state consists of the current motion state, the execution progress is recorded as the initial state, and theoretical trajectory comparison is not performed.

[0074] The current load status is derived from the load management module and the flight control system. For fixed loads, load mass, center of gravity, and moment of inertia parameters are entered into the load status acquisition unit before the mission begins. For suspended loads, the load oscillation status is provided by the sway angle sensor, sling force sensor, or state estimation results from the flight control system. The currently available thrust is determined based on the motor status, power supply status, ambient air density, and the thrust margin provided by the flight control system. When the load status acquisition time differs from the planning time, the status alignment module uses the most recent effective load status prior to the planning time, without interpolating the load mass and center of gravity; the load oscillation status is interpolated to the planning time according to its sampling time sequence.

[0075] When the difference between the millimeter-wave radar observation time and the planned time exceeds the effective observation duration, the environmental space unit will not be updated using the observation frame in this cycle. The state alignment module retains the probabilistic environmental risk state from the previous planning cycle and adds the observation failure state. If the integrated navigation state is missing or the attitude calculation fails, radar coordinate transformation will not be performed; the flight control system will continue to execute the remaining portion of the current trajectory that passed the verification in the previous planning cycle and will transmit the navigation state failure information to S400. If payload mass, payload center of gravity, or currently available thrust is missing, the state alignment module will use a conservative version of the dynamic parameters corresponding to the current mission payload and will not use unloaded dynamic parameters to replace heavily loaded dynamic parameters.

[0076] The planning cycle state set formed by the above processing includes millimeter-wave radar observations, current motion state, current payload state, mission target state, current trajectory execution state, and trajectory tracking error, all aligned with time and transformed by coordinates. Each planning cycle state set is associated with a planning time, ensuring that subsequent environmental probability updates, short-term motion reachability domain calculations, and trajectory remainder verification are based on the same time reference. The millimeter-wave radar observations, current motion state, and current payload state enter S200; the mission target state, current trajectory execution state, and trajectory tracking error participate in local target selection, risk expansion, and trajectory continuation in S300 and S400, respectively.

[0077] S200. Based on the planning period state set, update the occupancy probability of environmental space units according to the radar ray echo hit area, ray crossing area, and obstruction area to generate a probabilistic environmental risk state; determine the load dynamics constraint set according to the current load state and generate a short-time motion reachability domain.

[0078] After receiving the state set of the planning cycle, the probabilistic environment state generation module establishes a three-dimensional occupied grid map in the local planning coordinate system and divides the map into multiple environmental spatial units. The local occupied grid map covers the observation area around the UAV's current planned position, and its update frequency corresponds to the planning cycle. In one implementation, a global occupied grid map with a lower update frequency than the local occupied grid map is also provided. Observed content in the local occupied grid map that has left the current planning range is merged into the global occupied grid map according to its coordinate position. Local planning still uses the probabilistic state in the local occupied grid map as a direct constraint, while the global occupied grid map only provides historical environment states when the mission target direction exceeds the current radar field of view.

[0079] For each effective radar ray in the planning period state set, the probabilistic environment state generation module starts from the position of the millimeter-wave radar in the local planning coordinate system and performs ray traversal along the radar echo direction. Environmental spatial units located between the radar position and the echo hit position, within the radar's effective field of view, and not obstructed by other nearby echoes, are denoted as ray-traversed spatial units. The spatial range covered by the radar echo position and the radar measurement covariance is denoted as the echo hit region, and environmental spatial units falling within this range are denoted as echo hit spatial units. Environmental spatial units located behind the echo hit region along the same radar ray, still within the radar's effective field of view but obstructed by that echo, are denoted as obstructed unknown spatial units.

[0080] The radar measurement covariance reflects the measurement discrete range of the echo in the range, horizontal angle, and elevation angle directions. The probabilistic environment state generation module transforms this covariance to a local planning coordinate system and assigns occupation update weights according to the Mahalanobis distance between the center of the environment spatial cell and the radar echo position. Environment spatial cells located near the center of the covariance receive larger occupation update amounts, while those located at the edges of the covariance receive smaller occupation update amounts. When a single environment spatial cell is hit by multiple radar rays within the same planning period, updates are sequentially accumulated according to the observation confidence of each ray. When the same environment spatial cell is simultaneously recorded as an echo hit by one ray and as a ray crossing another, the acquisition time and observation confidence of the two observations are compared first, and then the observation with the newer acquisition time and higher observation confidence is given priority to prevent contradictory update amounts from directly canceling each other out.

[0081] The occupancy probability update adopts a log-probability form. Let p be the occupancy probability of an environmental spatial cell in the previous planning period, and its log-probability be L = ln[p / (1-p)]. For spatial cells hit by echoes, the occupancy update corresponding to the radar measurement covariance and observation confidence is accumulated on L; for spatial cells traversed by rays, the idle update pointing to the idle state is accumulated on L; for spatial cells obstructed by unknown rays, their log-probability is not changed by the radar ray in this planning period. The updated log-probability is constrained by preset upper and lower limits to avoid the occupancy probability from being fixed at an unrecoverable boundary value due to continuous observations over multiple frames.

[0082] Environmental space units that are continuously not covered by effective radar rays undergo probability decay based on their last observation time. Probability decay gradually brings their logarithmic probability closer to the initial unknown state, rather than directly switching to an idle state. For environmental space units that were once in a high-confidence occupied area but have not been observed again for a long time due to obstacle movement, probability decay removes their historical occupied state; for obscured unknown space units, their state is only changed according to the corresponding observation type after being crossed by new rays or hit by echoes in subsequent planning cycles. This process distinguishes between space behind obstacles that cannot be confirmed and space that has been crossed by radar rays.

[0083] The probabilistic environment state generation module classifies the updated occupancy probability using state entry and exit thresholds. When the occupancy probability reaches the occupancy entry threshold, the environmental space unit enters a high-confidence occupancy state; environmental space units in a high-confidence occupancy state only exit this state when the occupancy probability drops below the occupancy exit threshold. Idle states use independent idle entry and idle exit thresholds. Environmental space units that do not meet the conditions for a high-confidence occupancy state or a low-risk passable state are classified into an unknown risk area. The high-confidence occupancy area, the low-risk passable area, and the unknown risk area together constitute the probabilistic environment risk state. This state retains the occupancy probability, state category, last observation time, and radar measurement covariance of each environmental space unit, which are used by the S300 to calculate the risk expansion area.

[0084] The load dynamics constraint generation module reads the current load state from the planning cycle state set in parallel and matches it with a pre-established version of dynamic parameters. The dynamic parameter version can be established from flight test results of the heavy-load UAV under different load masses, load centers of gravity, load inertia parameters, and mounting methods, or it can be calculated offline and stored from the UAV dynamics model. During matching, parameter groups are first selected according to the load mounting method, and then corresponding parameter intervals are selected based on the load mass and load center of gravity; when the load oscillation exceeds the corresponding interval, the parameter version of the side with the larger oscillation amplitude is used. The matching results include maximum permissible speed, maximum permissible acceleration, maximum permissible braking deceleration, maximum permissible jerk, maximum permissible tilt angle, maximum permissible curvature, minimum permissible turning radius, and available thrust boundary, thus forming a load dynamics constraint set.

[0085] When the current load state lies between two versions of dynamic parameters, in one implementation, the load dynamics constraint generation module interpolates adjacent parameter versions. When maximum permissible velocity, maximum permissible acceleration, maximum permissible braking deceleration, and available thrust boundaries are involved, the smaller value between the interpolated result and the more conservative value from the two adjacent versions is used. If the load oscillation state exceeds the coverage of the parameter versions, the current available thrust is lower than the thrust boundary corresponding to the load condition, or the load state is inconsistent with the mission configuration, the original dynamic parameter version is not continued to be used. Instead, a dynamic parameter mismatch state is output, and a preset degraded dynamic constraint set is adopted. The degraded dynamic constraint set corresponds to smaller velocity, acceleration, jerk, and tilt angle boundaries.

[0086] The short-term reachability domain is calculated within a preset prediction time window, starting from the current motion state. The load dynamics constraint generation module first generates multiple candidate velocity states within the velocity direction range defined by the maximum permissible velocity and maximum permissible tilt angle. Then, it constructs candidate motion states using position, velocity, and acceleration, with jerk as the input for state change, and performs state integration according to discrete time intervals. For the k-th prediction time, the candidate position is recursively derived from the position, velocity, and acceleration of the previous prediction time, the candidate velocity is recursively derived from the velocity and acceleration of the previous prediction time, and the candidate acceleration is constrained by the maximum permissible acceleration and maximum permissible jerk. Each candidate motion state is associated with a corresponding prediction time, ensuring that subsequent risk elimination is performed according to the same prediction time.

[0087] During state integration, the load dynamics constraint generation module calculates the trajectory curvature based on the velocity direction changes and travel distances of adjacent candidate motion states. If the curvature exceeds the maximum permissible curvature or the corresponding turning radius is less than the minimum permissible turning radius, the candidate motion state and its subsequent branches are eliminated. The required tilt angle is calculated based on the relationship between the candidate acceleration and the UAV's attitude; integration of this branch stops when the required tilt angle exceeds the maximum permissible tilt angle. The required thrust is calculated based on the load mass, candidate acceleration, gravity component, and attitude component; if the required thrust exceeds the available thrust boundary, the candidate motion state does not enter the short-time reachable domain.

[0088] When the number of candidate states increases rapidly due to a long prediction window, neighboring candidate states can be merged according to position spacing, velocity direction, and prediction time. Merging retains only candidate states that satisfy a large braking margin, without altering the constraint boundaries of maximum permissible velocity, maximum permissible acceleration, and maximum permissible curvature. The filtered and merged candidate motion states constitute the short-time motion reachability domain, which includes candidate position, candidate velocity, candidate acceleration, prediction time, and corresponding dynamic parameter versions. The probabilistic environment risk state, load dynamic constraint set, and short-time motion reachability domain enter S300, participating in risk expansion, candidate state elimination, and trajectory segment generation, respectively.

[0089] S300: Generate a risk extension region based on the probabilistic environmental risk state, braking distance, time delay displacement, and trajectory tracking error; exclude the risk extension region from the short-time motion reachability domain to generate a safe and feasible flight domain; generate a trajectory segment to be executed based on the safe and feasible flight domain.

[0090] The safe and feasible flight domain generation module receives the probabilistic environmental risk state, load dynamics constraint set, and short-term motion reachability domain generated by S200, and reads the current velocity and trajectory tracking error from the planning cycle state set. The risk expansion region is not uniformly expanded across all environmental spatial units at a fixed distance; instead, it calculates the longitudinal and lateral expansion amounts separately based on the UAV's current motion direction, braking capability, system delay, and radar measurement uncertainty. The longitudinal expansion amount is set along the UAV's current velocity direction or the velocity direction of the candidate motion state, while the lateral expansion amount is set along a plane perpendicular to that velocity direction.

[0091] In one implementation, the longitudinal dynamic safety extension is formed by adding the basic safety clearance, braking distance, time-delay displacement, trajectory tracking error compensation, and radar measurement uncertainty compensation. The braking distance is calculated based on the current speed and the maximum permissible braking deceleration; when the braking process is also limited by the maximum permissible jerk, the displacement during the deceleration setup phase is calculated first based on the maximum permissible jerk, and then the displacement after entering the stable braking phase is calculated. The time-delay displacement is obtained by multiplying the current speed by the sum of the trajectory transmission delay and the flight control switching delay; when the planning calculation process and the trajectory verification process are not executed in parallel with the current trajectory, the corresponding processing time is also included in the time delay. The trajectory tracking error compensation is taken as the projection of the position deviation in the velocity direction and its permissible variation; the radar measurement uncertainty compensation is formed by the projection of the radar measurement covariance in the corresponding direction.

[0092] The lateral dynamic safety extension is formed using the basic safety clearance, lateral trajectory tracking error, radar angle measurement covariance, and load swing amplitude. When the suspended load is in a swinging state, the lateral extension range also includes the change in load relative to the aircraft's projected boundary. For high-confidence occupied areas, a risk extension area associated with the current speed direction is formed according to the aforementioned longitudinal and lateral extension amounts; for unknown risk areas, an extension amount not less than that of the high-confidence occupied area is used. In another implementation, when the speed is low and the remaining braking distance is less than the local passage width, the unknown risk area can retain a restricted candidate state, but the speed limit of this candidate state is limited according to the risk rules corresponding to the unknown risk area.

[0093] The risk expansion region and the short-term motion reachability domain are compared according to the predicted time. For each candidate motion state in the short-term motion reachability domain, the safe and feasible flight domain generation module reads its candidate position, candidate velocity, and predicted time, and then checks whether the candidate position falls within the expansion range of the high-confidence occupied region or the unknown risk region at the corresponding predicted time. If the candidate position falls into the risk expansion region, the candidate motion state is excluded; if the candidate position does not fall into the risk expansion region, but the trajectory segment from the previous candidate position to the current candidate position crosses the risk expansion region, the candidate motion state is also excluded. This check avoids only verifying discrete endpoints and missing the collision interval between adjacent candidate states.

[0094] In implementations containing dynamic obstacles, the radial velocity of the obstacle can be read during continuous millimeter-wave radar observation, and the predicted occupied area of ​​the dynamic obstacle within the prediction time window can be formed based on the positional changes of the echo hit area during continuous planning cycles. The predicted occupied area is added to the probabilistic environmental risk state at the prediction time, and the candidate motion state is only compared with the predicted occupied area at the same prediction time. When the observation of the dynamic obstacle is interrupted, it does not continue to extrapolate indefinitely according to the original velocity, but instead restores its subsequent space to an unknown risk area after the preset prediction time ends.

[0095] After eliminating candidate motion states, the remaining candidate motion states and their predicted times constitute a safe and feasible flight domain. The safe and feasible flight domain preserves the sequential connections between candidate motion states, allowing the trajectory planning module to distinguish between independent connected regions. When the safe and feasible flight domain is empty, the trajectory planning module does not generate a normal local target state, but instead transmits the empty feasible domain status to S400; if candidate motion states still exist but cannot form a connected path that extends to the end of the prediction time window, the longest continuously executable time is taken as the effective duration of the safe and feasible flight domain.

[0096] The trajectory planning module selects connected regions aligned with the mission objective direction from the safe and feasible flight domain. During selection, it first calculates the reachable distance of each connected region along the mission objective direction, and then calculates the channel width perpendicular to the main travel direction within the connected region. In open areas, it selects local target positions from distant candidate motion states along the mission objective direction, and uses the velocity direction and predicted time of these candidate states to form the local target state. In narrow channels, it first selects candidate states along the main axis of the connected region, and then limits the local target velocity based on the channel width. When a connected region corresponding to the mission objective direction is blocked by a risk expansion region, the trajectory planning module selects a local target state from the remaining connected regions with a small deviation from the mission objective direction; if no continuous feasible region exists, it outputs a trajectory solution failure state to S400.

[0097] The trajectory segment to be executed is generated using model predictive control. In one implementation, the UAV's position, velocity, and acceleration are used as state variables for model predictive control, and jerk is used as the control variable; the objective function includes local target state deviation, changes in adjacent control variables, and trajectory terminal velocity deviation. The safe and feasible flight domain serves as position and time constraints, while the maximum permissible velocity, maximum permissible acceleration, maximum permissible jerk, and maximum permissible curvature in the load dynamics constraint set serve as state and control constraints. Environmental risk constraints and load dynamics constraints are implemented as hard constraints, without simply replacing them by increasing cost weights.

[0098] Model predictive control recursively calculates candidate trajectories for multiple prediction times starting from the current motion state. The trajectory state at each prediction time must correspond to a range of candidate motion states within the same prediction time in the safe and feasible flight domain. If a trajectory segment crosses a risk extension region between adjacent prediction times, the candidate trajectory is discarded even if both endpoints are within the safe and feasible flight domain. When the solver returns multiple feasible candidate trajectories, the trajectory segment to be executed is selected based on the local target state deviation and the change in control variables. If the solver finds no feasible solution in the current local target state, the trajectory planning module shortens the local target distance along the original connected region and solves again. If no feasible solution is found after shortening, an unverified trajectory is not sent to the flight control system; instead, the unsolvable trajectory state is transmitted to the S400.

[0099] The trajectory segment to be executed is represented using a trajectory with time parameters, including the trajectory start time, the effective duration of the trajectory, and the position, velocity, and acceleration that change over time. The trajectory planning module also associates the probabilistic environmental risk state time, the load dynamics constraint set version, and the effective duration of the safe and feasible flight domain used for the trajectory with the trajectory segment to be executed. Based on this, the S400 sets the committed trajectory prefix and checks whether the environmental state and load state are still within the corresponding valid range during trajectory execution.

[0100] S400: Set the beginning of the trajectory segment to be executed as a committed trajectory prefix to generate a predicted terminal state; generate a subsequent trajectory during the execution of the committed trajectory prefix, and correct its initial state according to the actual motion state; verify the remaining part of the current trajectory to obtain the remaining safe execution time domain, and determine the expected available time of the subsequent trajectory based on the expected trajectory generation duration and delay; based on the remaining safe execution time domain, the expected available time of the subsequent trajectory, and the position, velocity, and acceleration continuity verification results, output the subsequent trajectory or degraded trajectory:

[0101] After receiving the trajectory segment to be executed generated by S300, the trajectory execution and continuation module calculates the committed time domain based on the current planning cycle, the effective duration of the safe and feasible flight domain, the historical trajectory generation duration, the trajectory transmission delay, and the flight control switching delay. The committed time domain does not exceed the effective duration of the trajectory segment to be executed, nor does it exceed the effective duration of the safe and feasible flight domain. The historical trajectory generation duration is formed using the actual solution duration of the previous multiple planning cycles, with its value biased towards the longer solution duration. When the number of candidate motion states in the current planning cycle is significantly greater than that in the historical planning cycles, a calculation time margin is added to the historical trajectory generation duration.

[0102] The portion of the trajectory segment to be executed from the start execution time to the promised end time in the time domain is designated as the promised trajectory prefix. Once the promised trajectory prefix enters the current execution buffer of the flight control system, regular rolling planning will not modify the trajectory coefficients or trajectory points for this portion. The position, velocity, and acceleration corresponding to the end of the promised trajectory prefix constitute the predicted terminal state, which, along with its corresponding time, is passed to subsequent trajectory planning processes. The trajectory portion following the promised trajectory prefix but not yet entering the current execution buffer remains in a replaceable state; when new environmental observations or payload state changes occur, this portion will not be treated as an unmodifiable trajectory and will continue execution.

[0103] The committed trajectory prefix is ​​not continuously executed under all operating conditions. When the latest probabilistic environmental risk status indicates that a new high-confidence occupied region has entered the committed trajectory prefix, the actual trajectory tracking error exceeds the preset boundary, the current available thrust is lower than the available thrust boundary used by the committed trajectory, or the load oscillation state exceeds the corresponding dynamic parameter version, the trajectory execution and continuation module interrupts the normal continuation process and immediately verifies the current speed and remaining safety margin. If the remaining safety margin supports braking, the braking trajectory is switched to the flight control system; if the remaining safety margin is insufficient to complete normal braking, the maximum braking input allowed by the current load dynamic constraint set is used, and the latest environmental status is continuously transmitted to the flight control system.

[0104] During the execution of the committed trajectory prefix by the flight control system, the trajectory planning module uses the predicted terminal state as the initial state and, in conjunction with the latest planning cycle state set, re-executes the environmental state updates, short-term motion reachability domain calculations, and safe and feasible flight domain calculations in S200 and S300, thereby forming the subsequent trajectory. In engineering implementation, a current execution buffer and a subsequent trajectory buffer can be set. The flight control system only reads the current execution buffer; the trajectory planning module writes the subsequent trajectory, verified by environmental risk constraints and load dynamics constraints, into the subsequent trajectory buffer. Buffer switching is controlled by the trajectory execution and continuation module; the flight control system does not directly read the trajectory content being written.

[0105] The initial state of subsequent trajectories is not directly adopted from the theoretical state at the end of the committed trajectory prefix. During trajectory execution, the state alignment module reads the theoretical position, theoretical velocity, and theoretical acceleration from the current trajectory at the same moment as the actual motion state, and calculates the position deviation, velocity deviation, and acceleration deviation. The trajectory execution and continuation module propagates the deviation to the end of the committed trajectory prefix based on the current deviation and the remaining committed time domain, forming a corrected predicted terminal state. Subsequent trajectory planning uses this corrected state as the initial state; if the position deviation, velocity deviation, or acceleration deviation exceeds the trajectory continuation threshold, the original subsequent trajectory becomes invalid, and the trajectory planning module recalculates the short-term motion reachability domain and subsequent trajectories based on the actual motion state.

[0106] The trajectory execution and continuation module also verifies the remaining portion of the current trajectory in chronological order. Verification begins at the trajectory position corresponding to the current moment, and then reads the position, velocity, and acceleration for each subsequent trajectory moment in sequence. Each trajectory moment is first compared with the risk expansion region corresponding to the latest probabilistic environmental risk state, and then with the velocity, acceleration, jerk, curvature, tilt angle, and thrust boundary values ​​in the latest load dynamics constraint set. The length of time from the current moment that continuously passes the above verification is recorded as the remaining safe execution time domain. If an environmental risk constraint or dynamic constraint is first not met at a certain trajectory moment, the continuous time period before that trajectory moment constitutes the remaining safe execution time domain, and trajectories after the first non-compliance moment are no longer included.

[0107] When the remaining total duration of the current trajectory exceeds the remaining safe execution time domain, the flight control system only considers the trajectory corresponding to the remaining safe execution time domain as the executable portion. If millimeter-wave radar observation fails and the current trajectory passes through a low-risk passable area from the previous planning cycle, the remaining safe execution time domain will not extend beyond the effective duration of the previous probabilistic environmental risk state. When the load dynamics parameter version changes, the remaining portion of the current trajectory is re-verified according to the new load dynamics constraint set; the original effective duration is not reused simply because the original trajectory passed verification during generation.

[0108] The estimated available time for subsequent trajectories is obtained by adding the current time, the estimated trajectory generation time, the trajectory verification time, the trajectory transmission delay, the flight control switching delay, and the safety time margin. The estimated trajectory generation time is formed based on the actual solution time of the recent planning cycle; in one implementation, the larger value of the recent solution time is used, and it is corrected according to the number of candidate motion states in the safe and feasible flight domain of the current cycle. The trajectory verification time is formed based on the actual time taken to perform environmental risk constraint verification and dynamic constraint verification on the same number of trajectory points in the previous planning cycle. The trajectory transmission delay is obtained from the time difference between the airborne computing unit sending the trajectory to the flight control system and receiving confirmation, and the flight control switching delay is obtained from the time difference between the flight control system receiving the trajectory and starting to execute the new trajectory.

[0109] After subsequent trajectories are generated, the trajectory execution and continuation module first checks whether its start time is later than the continuation time of the current trajectory. Then, it compares the starting position of the subsequent trajectory with the continuation position of the current trajectory, the starting velocity of the subsequent trajectory with the continuation velocity of the current trajectory, and the starting acceleration of the subsequent trajectory with the continuation acceleration of the current trajectory. If the three types of deviations are within the range of the position continuity threshold, velocity continuity threshold, and acceleration continuity threshold, respectively, the trajectory continuity check is passed. If the expected available time of the subsequent trajectory is earlier than the end time of the remaining safe execution time domain, and the trajectory continuity check is passed, the subsequent trajectory is written into the current execution buffer and executed by the flight control system at the continuation time.

[0110] If the expected availability of a subsequent trajectory is no earlier than the end of the remaining safe execution time domain, the trajectory execution and continuation module will not wait for the current trajectory to be completely exhausted. If the remaining safe execution time domain is greater than the braking time calculated based on the current speed and load dynamics constraint set, a deceleration trajectory will be output first, gradually reducing the speed along the tangential direction of the current trajectory, while reserving time margin for subsequent replanning. When the remaining safe execution time domain is close to the braking time, a braking trajectory will be output directly. When the UAV speed decays to the hovering allowable range and the current position is not within the risk expansion area, a hovering trajectory will be output. If a subsequent trajectory has been generated but the position, speed, or acceleration continuity check fails, a degraded trajectory will be output according to the same rules, and subsequent planning will be restarted with the predicted terminal state of the degraded trajectory.

[0111] The deceleration trajectory, braking trajectory, and hovering trajectory are all generated according to the latest load dynamics constraint set. The deceleration trajectory limits the rate of change of acceleration and does not use instantaneous speed switching; the braking trajectory is formed in stages according to the maximum permissible braking deceleration and the maximum permissible jerk; the hovering trajectory uses the terminal position of the aforementioned braking trajectory as the hovering position, and verifies the risk extension area corresponding to this position before output. When the safe and feasible flight domain is empty, the trajectory solution is unsolvable, or the trajectory buffer is empty, the trajectory execution and continuation module selects the corresponding trajectory from the aforementioned degraded trajectories based on the remaining safe execution time domain, and does not send a single control quantity lacking the continuity of position, velocity, and acceleration relationships to the flight control system.

[0112] During the execution of subsequent or degraded trajectories by the flight control system, the actual position, actual velocity, actual acceleration, trajectory reception time, and trajectory start execution time are transmitted to the state alignment module. The onboard computing unit retains the actual trajectory generation time, trajectory verification time, trajectory transmission delay, flight control switching delay, the version of the load dynamics parameters used, and trajectory continuation results for this planning cycle. The above information is entered into S100 in the next planning cycle, where the actual motion state and the current trajectory execution state participate in the new time alignment, the actual trajectory generation time participates in the calculation of the next committed time domain and the expected available time of subsequent trajectories, and the latest load state and environmental observations are re-involved in the update of the probabilistic environmental risk state and the short-term motion reachability domain.

[0113] Example 2: Figure 2 A structural block diagram of a heavy-load unmanned aerial vehicle (UAV) constraint obstacle avoidance planning system according to an embodiment of the present invention is shown. Figure 2 As shown, the structure may include:

[0114] The state alignment module 01 is used to acquire millimeter-wave radar observations, current motion state, current payload state, mission target state, and current trajectory execution state. It performs time alignment on the millimeter-wave radar observations, current motion state, and current trajectory execution state to generate a planning cycle state set. The state alignment module acquires corresponding state data through radar data interfaces, integrated navigation interfaces, payload state interfaces, mission planning interfaces, and flight control interfaces. The millimeter-wave radar observations include radar echo position, radar observation timestamp, radar effective field of view, and radar measurement covariance. The current motion state includes current position, current velocity, current acceleration, and current attitude. The current trajectory execution state includes the start time of the current trajectory, execution progress, and the theoretical position, theoretical velocity, and theoretical acceleration corresponding to the current time. The state alignment module uses the planning time of the current planning cycle as the time reference, reads the motion states adjacent to the planning time from the current motion state sequence, and interpolates the position, velocity, and acceleration according to the sampling time. Then, based on the interpolated attitude and current position, it transforms the time-synchronized millimeter-wave radar observations from the radar coordinate system to the local planning coordinate system.

[0115] When multiple millimeter-wave radars output observations within the same planning cycle, the state alignment module performs pose compensation according to the observation timestamps of each radar, preventing radar echoes from different acquisition times from being directly merged into the same obstacle location. When the current trajectory execution state exists, the state alignment module compares the actual motion state with the theoretical motion state of the current trajectory at the same moment, generating position deviation, velocity deviation, and acceleration deviation, which are then recorded in the trajectory tracking error. If the time difference between the millimeter-wave radar observation and the planning time exceeds the effective observation duration, that frame of observation does not participate in the probability update of this planning cycle; if the current load state is missing, the valid load state from the previous planning cycle is retained, and a load state failure flag is added; if the current motion state or attitude fails, radar coordinate transformation is paused, and state failure information is transmitted to the trajectory execution and continuation module. The time-aligned millimeter-wave radar observation, current motion state, current load state, mission target state, current trajectory execution state, and trajectory tracking error together constitute the planning cycle state set, which is transmitted to the probabilistic environment state generation module, the load dynamics constraint generation module, the trajectory planning module, and the trajectory execution and continuation module, respectively.

[0116] The probabilistic environment state generation module 02, connected to the state alignment module, is used to update the occupancy probability of environmental spatial units based on the planning period state set, according to the echo hit area, ray crossing area, and obstruction area of ​​the radar ray, to generate a probabilistic environment risk state. The probabilistic environment state generation module receives millimeter-wave radar observations after time alignment and coordinate transformation, and divides the radar's effective field of view into multiple environmental spatial units within the local planning coordinate system. For each radar ray with an effective radar echo position, the probabilistic environment state generation module performs ray traversal from the location of the millimeter-wave radar along the echo direction; environmental spatial units between the millimeter-wave radar and the radar echo position that are not obstructed by close-range echoes are denoted as ray crossing spatial units; environmental spatial units covered by the radar echo position and radar measurement covariance are denoted as echo hit spatial units; and environmental spatial units behind the echo hit spatial units that are still within the radar's effective field of view are denoted as obstruction unknown spatial units.

[0117] The probabilistic environment state generation module reads the occupancy probability of each environmental spatial unit in the previous planning period and converts the occupancy probability into a logarithmic probability. For spatial units hit by echoes, the occupancy update amount is allocated according to the radar measurement covariance and radar observation confidence; for spatial units traversed by rays, an idle update amount pointing to the idle state is added; for spatial units with unknown obstruction, the logarithmic probability of the previous planning period is maintained, and they are not switched to the idle state due to a single absence of echoes. When the same environmental spatial unit receives conflicting updates within the same planning period, the probabilistic environment state generation module compares the acquisition time and observation confidence of the corresponding radar observations and adopts the update result with the more recent acquisition time and higher observation confidence. Environmental spatial units that are continuously not covered by effective radar rays undergo probability decay according to the last observation time, so that their occupancy probability gradually approaches the unknown state.

[0118] After the update, the probabilistic environment state generation module divides the environmental space units into high-confidence occupied areas, low-risk passable areas, and unknown-risk areas based on the occupancy entry threshold, occupancy exit threshold, idle entry threshold, and idle exit threshold. Environmental space units in a high-confidence occupied state maintain their original state as long as their occupancy probability does not fall below the occupancy exit threshold, and environmental space units in a low-risk passable state maintain their original state as long as their occupancy probability does not rise above the idle exit threshold. The probabilistic environment risk state is constituted by the occupancy probability, region category, last observation time, and radar measurement covariance of each environmental space unit. This probabilistic environment risk state is transmitted to the safe and feasible flight domain generation module and used as the latest environmental constraint when the trajectory execution and continuation module re-verifies the remaining portion of the current trajectory. When the millimeter-wave radar continuously loses frames, the probabilistic environment state generation module retains the probabilistic environment risk state from the previous planning cycle and shortens its effective duration according to the observation failure time.

[0119] The load dynamics constraint generation module 03, connected to the state alignment module, is used to determine the load dynamics constraint set based on the current load state, and generate a short-term motion reachability domain based on the current motion state and the load dynamics constraint set. The load dynamics constraint generation module reads the load mass, load center of gravity, load inertia parameters, current available thrust, and load oscillation state from the planning cycle state set, and matches the current load state with a pre-established version of dynamic parameters. The dynamic parameter version is set according to the load mounting method, load mass range, load center of gravity range, and load oscillation range, and records the maximum permissible speed, maximum permissible acceleration, maximum permissible braking deceleration, maximum permissible jerk, maximum permissible tilt angle, maximum permissible curvature, minimum permissible turning radius, and available thrust boundary associated with the corresponding load condition.

[0120] When the current load state lies between two dynamic parameter versions, the load dynamics constraint generation module interpolates between the two parameter versions based on the load mass and center of gravity. For the maximum permissible speed, maximum permissible acceleration, maximum permissible braking deceleration, and available thrust boundary, the interpolation result is used to limit the amplitude with conservative values ​​in adjacent parameter versions. When the load oscillation state exceeds the coverage range of the current parameter version, or when the current available thrust is lower than the available thrust boundary corresponding to the load condition, the load dynamics constraint generation module switches the current parameter version to a downgraded dynamic parameter version and transmits the dynamic state change information to the trajectory execution and continuation module. The maximum permissible speed, maximum permissible acceleration, maximum permissible braking deceleration, maximum permissible jerk, maximum permissible tilt angle, maximum permissible curvature, minimum permissible turning radius, and available thrust boundary together constitute the load dynamics constraint set.

[0121] After forming the load dynamics constraint set, the load dynamics constraint generation module generates multiple candidate velocity states within a prediction time window, starting from the current motion state. These candidate velocity states are limited by the maximum permissible velocity and the maximum permissible tilt angle. Subsequently, discrete state integration is performed on the candidate velocity states according to the maximum permissible acceleration and the maximum permissible jerk, forming candidate motion states containing candidate positions, candidate velocities, candidate accelerations, and prediction times. When the curvature of an adjacent candidate motion state exceeds the maximum permissible curvature, the turning radius is less than the minimum permissible turning radius, the required tilt angle exceeds the maximum permissible tilt angle, or the required thrust exceeds the available thrust boundary, the load dynamics constraint generation module stops further integration of the corresponding state branch. The candidate motion states and their prediction times, after being filtered by dynamic boundaries, constitute the short-term motion reachability domain. The short-term motion reachability domain is transferred to the safe and feasible flight domain generation module, and the load dynamics constraint set is synchronously transferred to the trajectory planning module and the trajectory execution and continuation module.

[0122] The safe and feasible flight domain generation module 04, connected to both the probabilistic environment state generation module and the load dynamics constraint generation module, generates a risk expansion region based on the probabilistic environment risk state, braking distance, time-delay displacement, and trajectory tracking error. It then excludes the risk expansion region from the short-term motion reachability domain to generate a safe and feasible flight domain. Upon receiving the probabilistic environment risk state and the short-term motion reachability domain, the safe and feasible flight domain generation module reads the current speed, maximum permissible braking deceleration, trajectory transmission delay, flight control switching delay, and trajectory tracking error. The braking distance is formed based on the current speed and maximum permissible braking deceleration. When the braking process is constrained by the maximum permissible jerk, the braking distance includes the displacement during the deceleration establishment phase and the displacement during the stable braking phase. The time-delay displacement is formed according to the displacement of the current speed within the trajectory transmission delay and flight control switching delay. The trajectory tracking error participates in longitudinal and lateral expansion based on the projection of the position deviation in the current speed direction and lateral direction, respectively.

[0123] The safe and feasible flight domain generation module combines the basic safety clearance, braking distance, time delay displacement, trajectory tracking error, and radar measurement covariance into a dynamic safety expansion quantity, and expands the high-confidence occupied region and the unknown risk region according to the dynamic safety expansion quantity. The expansion distance along the current velocity direction adopts the longitudinal dynamic safety expansion quantity, and the spatial range perpendicular to the current velocity direction adopts the lateral dynamic safety expansion quantity; when the current load state includes the load swing state, the lateral dynamic safety expansion quantity also includes the projection range of the load swing on the horizontal plane. The expanded high-confidence occupied region and the unknown risk region together constitute the risk expansion region.

[0124] For each candidate motion state in the short-term reachable domain, the safe and feasible flight domain generation module reads the corresponding risk extension region according to its predicted time and compares the candidate position and the motion interval between adjacent candidate positions. When a candidate position falls into the risk extension region, or the motion interval between adjacent candidate positions crosses the risk extension region, the corresponding candidate motion state and subsequent state branches dependent on that state are excluded. The remaining candidate motion states are divided into one or more connected regions according to their connection relationships, and the position, velocity, acceleration, and predicted time of each candidate motion state are retained, thereby forming a safe and feasible flight domain. When the short-term reachable domain is completely covered by the risk extension region, the safe and feasible flight domain generation module outputs an empty feasible domain state to the trajectory planning module and the trajectory execution and continuation module; when only connected regions that do not extend to the end of the predicted time window exist, the longest continuous feasible time is recorded as the effective duration of the safe and feasible flight domain. The safe and feasible flight domain and its effective duration are passed to the trajectory planning module and serve as one of the time boundaries of the committed trajectory prefix during trajectory execution.

[0125] The trajectory planning module 05, connected to the safe and feasible flight domain generation module and the state alignment module, generates a trajectory segment to be executed based on the safe and feasible flight domain and the mission target state. It sets the beginning of the trajectory segment to be executed as a committed trajectory prefix and generates a predicted terminal state. The trajectory planning module reads connected regions, candidate motion states, and predicted times from the safe and feasible flight domain, and reads the mission target state and the current trajectory execution state from the planning cycle state set. For connected regions extending along the mission target direction, the trajectory planning module calculates the reachable distance of the connected region in the mission target direction and the channel width perpendicular to the travel direction. It then selects a local target position from the connected region and uses the velocity direction and predicted time of the corresponding candidate motion state to form a local target state. When the mission target direction is blocked by a risk expansion region, a local target state is selected from other connected regions with a small deviation from the mission target direction. When the safe and feasible flight domain is empty, no normal local target state is generated, and the trajectory solution failure state is passed to the trajectory execution and continuation module.

[0126] The trajectory planning module uses the current motion state as the trajectory starting state and the local target state as the trajectory terminal constraint, employing model predictive control to form candidate trajectory segments with time parameters. The safe and feasible flight domain serves as the position and prediction time constraint for the candidate trajectory segments, while the maximum permissible velocity, maximum permissible acceleration, maximum permissible jerk, and maximum permissible curvature from the load dynamics constraint set serve as motion state constraints. If a candidate trajectory segment crosses a risk extension region between adjacent prediction times, or if any trajectory state exceeds the load dynamics constraint set, that candidate trajectory segment is not output. If no feasible candidate trajectory segment exists under the current local target state, the trajectory planning module shortens the local target distance along the same connected region and solves again; if the second solution still yields no results, it outputs a "no solution" status to the trajectory execution and continuation module.

[0127] After forming the trajectory segment to be executed from the candidate trajectory segments that have passed constraint verification, the trajectory planning module determines the committed time domain based on the current planning cycle, the effective duration of the safe and feasible flight domain, the historical trajectory generation duration, the trajectory transmission delay, and the flight control switching delay. The portion of the trajectory segment to be executed from the start execution time to the end time of the committed time domain is set as the committed trajectory prefix, and the position, velocity, and acceleration corresponding to the end of the committed trajectory prefix constitute the predicted terminal state. The trajectory segment to be executed, the committed trajectory prefix, and the predicted terminal state are passed to the trajectory execution and continuation module; the committed trajectory prefix enters the current execution buffer, and the trajectory portion after the committed trajectory prefix remains in a replaceable state. After the committed trajectory prefix is ​​set, the trajectory planning module continues to receive the safe and feasible flight domains formed in subsequent planning cycles and generates subsequent trajectories based on the predicted terminal state.

[0128] The trajectory execution and continuation module 06, connected to the trajectory planning module, the state alignment module, the probabilistic environment state generation module, and the load dynamics constraint generation module, generates subsequent trajectories based on the predicted terminal state during the execution of the committed trajectory prefix. It corrects the initial state of the subsequent trajectory based on the actual motion state, verifies the remaining portion of the current trajectory based on the latest probabilistic environment risk state and the latest load dynamics constraint set to obtain the remaining safe execution time domain. It determines the expected available time of the subsequent trajectory based on the expected trajectory generation duration and delay, and outputs the subsequent trajectory or a degraded trajectory based on the remaining safe execution time domain, the expected available time of the subsequent trajectory, and the position, velocity, and acceleration continuity verification results. The trajectory execution and continuation module transmits the committed trajectory prefix to the flight control system and receives the actual position, actual velocity, actual acceleration, trajectory reception time, and trajectory start execution time returned by the flight control system. During the execution of the committed trajectory prefix, the trajectory planning module generates subsequent trajectories in the subsequent trajectory buffer, and the trajectory execution and continuation module does not transmit the subsequent trajectory being written to the flight control system.

[0129] The trajectory execution and continuation module reads the actual motion state during the execution of the committed trajectory prefix from the state alignment module and compares the actual position, actual velocity, and actual acceleration with the theoretical position, theoretical velocity, and theoretical acceleration at the same moment. Based on the propagation results of position deviation, velocity deviation, and acceleration deviation in the remaining committed time domain, the predicted terminal state is corrected, and the corrected predicted terminal state is used as the initial state for subsequent trajectories. When any deviation exceeds the corresponding continuation threshold, the subsequent trajectory being generated or already generated is marked as uncontinuable, and the trajectory planning module regenerates the short-term motion reachability domain and subsequent trajectories based on the latest actual motion state.

[0130] During subsequent trajectory generation, the trajectory execution and continuation module reads each trajectory state sequentially along the remaining time portion of the current trajectory. Each trajectory state is first compared with the risk expansion region corresponding to the latest probabilistic environmental risk state, and then compared with the velocity, acceleration, jerk, curvature, tilt angle, and thrust boundaries in the latest load dynamics constraint set. The length of time from the current moment that continuously passes the environmental risk constraint verification and dynamic constraint verification is recorded as the remaining safe execution time domain; when a trajectory state first enters the risk expansion region or exceeds the load dynamics constraint set, the time corresponding to that trajectory state is taken as the end time of the remaining safe execution time domain. When the latest probabilistic environmental risk state fails, the remaining safe execution time domain does not exceed the remaining valid duration of the previous valid probabilistic environmental risk state.

[0131] The trajectory execution and continuation module determines the expected trajectory generation time based on the actual trajectory generation time of the historical planning cycle and the number of candidate motion states in the current safe and feasible flight domain. This estimated time is then added to the trajectory verification time, trajectory transmission delay, flight control switching delay, and safety time margin to form the expected available time for the subsequent trajectory. After the subsequent trajectory is generated, the trajectory execution and continuation module compares the position deviation, velocity deviation, and acceleration deviation between the initial state of the subsequent trajectory and the current trajectory continuation state. If the expected available time for the subsequent trajectory is earlier than the end time of the remaining safe execution time domain, and the three types of deviations are within the range of the position continuity threshold, velocity continuity threshold, and acceleration continuity threshold, respectively, the subsequent trajectory is switched from the subsequent trajectory buffer to the current execution buffer, and a trajectory switching command is sent to the flight control system at the continuation time.

[0132] If the expected availability of the subsequent trajectory is no earlier than the end of the remaining safe execution time domain, or if the continuity checks of position, velocity, and acceleration fail, the trajectory execution and continuation module selects a degraded trajectory based on the remaining safe execution time domain and the load dynamics constraint set. If the remaining safe execution time domain is greater than the braking time corresponding to the current velocity, a deceleration trajectory that reduces velocity tangentially along the current trajectory is output first; if the remaining safe execution time domain is close to the braking time, a braking trajectory constrained by the maximum permissible braking deceleration and maximum permissible acceleration is output; if the UAV speed enters the hovering allowable range and the hovering position does not fall into the risk expansion area, a hovering trajectory is output. If a new obstacle enters the prefix of the committed trajectory, the current available thrust is lower than the available thrust boundary used by the committed trajectory, or the trajectory tracking error exceeds the interruption threshold, the trajectory execution and continuation module stops the regular continuation process and calls a braking trajectory or hovering trajectory according to the current remaining safe space.

[0133] After the flight control system completes the reception and switching of subsequent or degraded trajectories, it returns the reception time, start execution time, and actual motion state to the trajectory execution and continuation module. The trajectory execution and continuation module provides the actual trajectory generation time, trajectory verification time, trajectory transmission delay, flight control switching delay, current trajectory execution state, and actual motion state to the state alignment module as input for forming the planning cycle state set in the next planning cycle; the latest trajectory execution state also participates in the calculation of the next round of commitment time domain, remaining safe execution time domain, and expected available time for subsequent trajectories.

Claims

1. A constrained obstacle avoidance planning method for heavy-load unmanned aerial vehicles (UAVs), characterized in that, include: S100: Acquire millimeter-wave radar observations, current motion status, current payload status, mission target status, and current trajectory execution status; The millimeter-wave radar observations, the current motion state, and the current trajectory execution state are time-aligned to generate a planning cycle state set; S200. Based on the planning cycle state set, update the occupancy probability of the environmental space unit according to the radar ray echo hit area, ray crossing area and obstruction area, and generate the probabilistic environmental risk state. The load dynamics constraint set is determined based on the current load state, and a short-time motion reachability domain is generated; S300: Generate a risk expansion area based on the probabilistic environmental risk state, braking distance, time delay displacement, and trajectory tracking error; The risk expansion region is excluded from the short-term mobility reachability region to generate a safe and feasible flight domain. Generate a trajectory segment to be executed based on the aforementioned safe and feasible flight domain; S400. The portion of the trajectory segment to be executed from the start execution time to the end time of the promised time domain is set as the promised trajectory prefix. The promised time domain is calculated based on the current planning cycle, the effective duration of the safe and feasible flight domain, the historical trajectory generation duration, the trajectory transmission delay, and the flight control switching delay. The promised time domain does not exceed the effective duration of the trajectory segment to be executed, nor does it exceed the effective duration of the safe and feasible flight domain. The position, velocity, and acceleration corresponding to the end of the promised trajectory prefix constitute the predicted terminal state. During the execution of the committed trajectory prefix, a subsequent trajectory is generated. Based on the position deviation, velocity deviation, and acceleration deviation between the actual motion state and the theoretical motion state, as well as the remaining committed time domain, the deviation is propagated to the end of the committed trajectory prefix to form a corrected predicted terminal state. The corrected predicted terminal state is used as the initial state of the subsequent trajectory. The remaining part of the current trajectory is verified in chronological order. For each subsequent trajectory moment, the position, velocity and acceleration are read sequentially. First, it is compared with the risk expansion area corresponding to the latest probabilistic environmental risk state. Then, it is compared with the velocity, acceleration, jerk, curvature, tilt angle and thrust boundary in the latest load dynamics constraint set. The length of time that the above verification is continuously passed from the current moment is recorded as the remaining safe execution time domain. The expected availability time of subsequent trajectories is determined based on the expected trajectory generation duration and delay. Based on the remaining safe execution time domain, the expected availability time of the subsequent trajectory, and the position, velocity, and acceleration continuity verification results, the subsequent trajectory is output when the expected availability time of the subsequent trajectory is earlier than the end time of the remaining safe execution time domain, and the position, velocity, and acceleration continuity verification results are passed; otherwise, a degraded trajectory is output, wherein the degraded trajectory includes a deceleration trajectory, a braking trajectory, and a hovering trajectory. The deceleration trajectory is output when the remaining safe execution time domain is greater than the braking time calculated based on the current velocity and the load dynamics constraint set. The braking trajectory is output when the remaining safe execution time domain is close to the braking time. The hovering trajectory is output when the UAV speed decays to the hovering allowable range and the current position does not belong to the risk expansion area.

2. The heavy-load UAV constraint obstacle avoidance planning method according to claim 1, characterized in that, The method of updating the occupancy probability of environmental spatial units according to the radar ray's echo hit area, ray penetration area, and obstruction area includes: Perform ray traversal on multiple radar beams; The environmental space unit between the millimeter-wave radar and the radar echo position, and within the radar's effective field of view, is defined as the ray-crossing space unit. The environmental spatial unit covered by the radar echo position and radar measurement covariance is defined as the echo hit spatial unit. The environmental spatial unit behind the echo-hit spatial unit and within the effective field of view of the radar is defined as the occlusion unknown spatial unit.

3. The heavy-load UAV constraint obstacle avoidance planning method according to claim 2, characterized in that, The occupancy probability of the updated environment space unit includes: The occupancy probability of each environmental space unit in the previous planning cycle is converted into log odds; An occupation update is added to the logarithmic probability of the echo hitting a spatial cell, and an idle update is added to the logarithmic probability of the ray traversing a spatial cell, while keeping the logarithmic probability of the occluded unknown spatial cell unchanged. Perform probability decay on the logarithmic odds of continuously unobserved environmental spatial units; Based on the occupancy entry threshold, occupancy exit threshold, idle entry threshold, and idle exit threshold, a high-confidence occupancy area, a low-risk passable area, and an unknown risk area are generated, forming the probabilistic environmental risk state.

4. The heavy-load UAV constraint obstacle avoidance planning method according to claim 1, characterized in that, The current load status includes load mass, load center of gravity, load inertia parameters, current available thrust, and load oscillation status; The current load state is matched with the dynamic parameter version to obtain the maximum permissible speed, maximum permissible acceleration, maximum permissible braking deceleration, maximum permissible jerk, maximum permissible tilt angle, maximum permissible curvature, minimum permissible turning radius, and available thrust boundary, thereby generating the load dynamic constraint set.

5. The heavy-load UAV constrained obstacle avoidance planning method according to claim 4, characterized in that, The generation of the short-term motion reachable domain includes: Starting from the current motion state, multiple candidate velocity states are generated within the prediction time window; The candidate velocity states are integrated according to the maximum permissible acceleration and the maximum permissible jerk to generate multiple candidate motion states containing position, velocity and predicted time. Based on the maximum permissible curvature, maximum permissible tilt angle, and available thrust boundary, candidate motion states that are not executable are filtered out, and the short-term motion reachability domain is generated based on the remaining candidate motion states.

6. The heavy-load UAV constrained obstacle avoidance planning method according to claim 3, characterized in that, The generation of the risk expansion area and the generation of the safe and feasible flight domain include: Based on the basic safety clearance, the braking distance corresponding to the current speed and the maximum permissible braking deceleration, the time delay displacement corresponding to the trajectory transmission delay and the flight control switching delay, the trajectory tracking error, and the radar measurement covariance, a dynamic safety extension quantity is generated; The risk expansion region is generated by expanding the high-confidence occupied region and the unknown risk region according to the dynamic security expansion amount. Candidate motion states that intersect with the risk expansion region in the short-term motion reachability domain are excluded, and the safe and feasible flight domain is generated based on the remaining candidate motion states and their predicted times.

7. The heavy-load UAV constraint obstacle avoidance planning method according to claim 6, characterized in that, The generation of the trajectory segment to be executed based on the safe and feasible flight domain includes: Based on the connectivity, channel width, and reachability in the direction of the mission objective of the safe and feasible flight domain, the local target position, target velocity direction, and estimated arrival time are determined, and the local target state is generated. Based on the current motion state and the local target state, model predictive control is used to generate candidate trajectory segments with time parameters; The safe and feasible flight domain, maximum permissible speed, maximum permissible acceleration, maximum permissible jerk, and maximum permissible curvature are set as trajectory constraints for the model predictive control, and the trajectory segment to be executed is generated based on the candidate trajectory segments that satisfy the trajectory constraints.

8. The heavy-load UAV constrained obstacle avoidance planning method according to claim 1, characterized in that, The step of correcting the initial state based on the actual motion state includes: The committed time domain is determined based on the current planning cycle, the effective duration of the safe and feasible flight domain, the duration of historical trajectory generation, the trajectory transmission delay, and the flight control switching delay. The portion of the trajectory segment to be executed that is located within the committed time domain is set as the committed trajectory prefix, and the position, velocity, and acceleration of the end of the committed trajectory prefix are recorded as the predicted terminal state; Obtain the actual motion state during the execution of the committed trajectory prefix, and calculate the position deviation, velocity deviation, and acceleration deviation between the actual motion state and the predicted terminal state; The initial state of the subsequent trajectory is corrected based on the position deviation, velocity deviation, and acceleration deviation.

9. The heavy-load UAV constraint obstacle avoidance planning method according to claim 8, characterized in that, The step of verifying the remaining portion of the current trajectory and outputting the subsequent trajectory or a downgraded trajectory includes: Based on the latest probabilistic environmental risk state and the latest load dynamics constraint set, environmental risk constraint verification and dynamics constraint verification are performed sequentially along the remaining part of the current trajectory. The length of time from the current moment when the verification is passed continuously is recorded as the remaining safe execution time domain. The estimated available time of the subsequent trajectory is determined by adding the expected trajectory generation time, trajectory verification time, trajectory transmission delay, flight control switching delay, and safety time margin. The subsequent trajectory is output when the expected available time of the subsequent trajectory is earlier than the end time of the remaining safe execution time domain, and the subsequent trajectory satisfies the continuity conditions of position, velocity, and acceleration with the current trajectory. If the expected available time of the subsequent trajectory is not earlier than the end time, or if the subsequent trajectory does not meet the continuity condition, the deceleration trajectory, braking trajectory, or hovering trajectory is output as the degraded trajectory according to the remaining safe execution time domain.

10. A constrained obstacle avoidance planning system for heavy-duty unmanned aerial vehicles, applied to the method of any one of claims 1 to 9, characterized in that, include: The state alignment module is used to acquire millimeter-wave radar observations, current motion state, current payload state, mission target state, and current trajectory execution state, and to perform time alignment on the millimeter-wave radar observations, the current motion state, and the current trajectory execution state to generate a planning period state set; The probabilistic environment state generation module, connected to the state alignment module, is used to update the occupancy probability of environmental space units based on the planning period state set according to the echo hit area, ray crossing area and obstruction area of ​​radar rays, and generate probabilistic environment risk state. A load dynamics constraint generation module, connected to the state alignment module, is used to determine a load dynamics constraint set based on the current load state, and generate a short-time motion reachability domain based on the current motion state and the load dynamics constraint set; The safe and feasible flight domain generation module is connected to the probabilistic environment state generation module and the load dynamics constraint generation module, respectively. It is used to generate a risk extension region based on the probabilistic environment risk state, braking distance, time delay displacement and trajectory tracking error, exclude the risk extension region from the short-time motion reachability domain, and generate a safe and feasible flight domain. The trajectory planning module is connected to the safe and feasible flight domain generation module and the state alignment module, respectively. It is used to generate a trajectory segment to be executed based on the safe and feasible flight domain and the mission target state, set the beginning of the trajectory segment to be executed as a committed trajectory prefix, and generate a predicted terminal state. The trajectory execution and continuation module, connected to the trajectory planning module, the state alignment module, the probabilistic environment state generation module, and the load dynamics constraint generation module, is used to generate subsequent trajectories based on the predicted terminal state during the execution of the committed trajectory prefix, correct the initial state of the subsequent trajectory according to the actual motion state, verify the remaining part of the current trajectory based on the latest probabilistic environment risk state and the latest load dynamics constraint set to obtain the remaining safe execution time domain, determine the expected available time of the subsequent trajectory based on the expected trajectory generation duration and delay, and output the subsequent trajectory or degraded trajectory based on the remaining safe execution time domain, the expected available time of the subsequent trajectory, and the position, velocity, and acceleration continuity verification results.

Images