A three-dimensional path generation method and system based on constraint modulation adaptive update

Abstract

The application discloses a three-dimensional path generation method and system based on constraint modulation adaptive update, and the method comprises the following steps: obtaining three-dimensional environment information, and establishing an environment model; performing parameterization processing on a to-be-planned path in the environment model, and obtaining a path parameter vector; obtaining each path point of a candidate path based on the path parameter vector, and calculating a single-point risk value of each path point according to an evaluation rule of a constructed risk model based on obstacle space information extracted from the environment model, and determining a risk cost corresponding to the path parameter vector based on the single-point risk value; constructing at least one continuous and derivable constraint cost function, and determining a constraint cost of the path parameter vector based on the constraint cost function; constructing a comprehensive cost function based on the risk cost and the constraint cost; taking the comprehensive cost function as an optimization basis, and iteratively updating the path parameter vector by using a constraint-aware distributed-level adaptive collaborative optimization mechanism until a preset termination condition is satisfied; and outputting an optimized three-dimensional feasible path.

Description

Technical Field

[0001] This invention relates to the field of path planning and navigation technology for intelligent mobile platforms, and more specifically, to a three-dimensional path generation method and system based on constraint modulation adaptive update. Background Technology

[0002] Three-dimensional path planning is a fundamental technology for intelligent mobile platforms to achieve autonomous navigation and safe movement. It is widely used in underwater vehicles, drones and other three-dimensional motion systems. Its goal is to generate executable paths in complex spatial environments and under various constraints.

[0003] Existing technologies for 3D path planning often employ graph search or sampling (A*, RRT*, PRM, etc.) and cost function-based optimization methods. However, these methods still have the following shortcomings in practical engineering applications: First, obstacle risks are mostly modeled using collision determination or fixed safety distance methods, which makes it difficult to characterize the asymmetry of risks in the spatial direction and their correlation with environmental disturbances and motion states. Second, spatial boundaries, height, gaps, and kinematic constraints are usually handled using hard thresholds or piecewise penalties, resulting in discontinuities in the constraint functions at the boundaries, which affects the stability of the planning process. Third, in a three-dimensional environment with multiple constraints and strong coupling, existing methods are prone to problems such as reduced search efficiency or premature convergence, and lack an effective constraint mechanism for the feasible region during the iteration process. Finally, the engineering feasibility of the generated paths is not adequately guaranteed, and additional modifications are often required after the planning is completed.

[0004] Therefore, it is necessary to provide a 3D path generation method that can uniformly characterize obstacle risks and various physical constraints, and maintain stability and feasibility during the path generation process. Summary of the Invention

[0005] This invention proposes a three-dimensional path generation method and system based on constraint modulation adaptive update to solve the problem of constructing a comprehensive cost function containing objective terms and physical feasibility constraints in a three-dimensional environment, iteratively updating path parameters accordingly, and finally outputting a three-dimensional feasible path.

[0006] To address the aforementioned problems, according to one aspect of the present invention, a three-dimensional path generation method based on constraint modulation adaptive update is provided, characterized in that the method comprises: Acquire three-dimensional environmental information and establish an environmental model based on the three-dimensional environmental information; The path to be planned in the environment model is parameterized to obtain the path parameter vector; Based on the path parameter vector, each path point of the candidate path is obtained. Using the spatial information of obstacles extracted from the environment model, the single-point risk value of each path point is calculated according to the evaluation rules of the constructed risk model. Based on the single-point risk value of each path point, the risk cost corresponding to the path parameter vector is determined. Construct at least one continuously differentiable constraint cost function, and determine the constraint cost of the path parameter vector based on the constraint cost function; Construct a comprehensive cost function based on the aforementioned risk cost and constraint cost; Based on the comprehensive cost function, the iterative optimization objective is determined, and the path parameter vector is iteratively updated using a constraint-aware distributed adaptive collaborative optimization mechanism until the iterative optimization objective is met. Output the optimal three-dimensional feasible path corresponding to the path parameter vector that satisfies the iterative optimization objective.

[0007] Preferably, the three-dimensional environmental information includes: obstacle set, spatial boundary, terrain or spatial occupancy information; the path parameter vector is a three-dimensional sequence composed of multiple path nodes, or an equivalent path parameterization form of control point sequence or spline coefficients.

[0008] Preferably, the risk field model includes a principal axis scale parameter and a secondary axis scale parameter, and the principal axis scale parameter adaptively increases or decreases according to the projection component of the external disturbance field in the tangential direction of the candidate path. The evaluation rules of the risk model include the risk field model using the intensity and direction of the external disturbance field or the projection components in the tangential direction of the candidate path to adjust the spatial shape or directional weight of the risk field, so that the risk field exhibits asymmetric distribution characteristics in the direction dominated by the external disturbance; the external disturbance field includes at least one of the following: flow field, wind field, and velocity field.

[0009] Preferably, the constraint cost function includes: spatial boundary constraints, height or gap constraints, and kinematic constraints; wherein, the constraint cost function satisfies the following properties: when the path parameter vector is within the constraint allowable interval of the constraint cost function, the constraint cost and its gradient are approximately zero; when the path parameter exceeds the constraint boundary of the constraint cost function, the constraint cost and its gradient continuously increase with the violation amount.

[0010] Preferably, the step of iteratively updating the path parameter vector using a constraint-aware distributed adaptive collaborative optimization mechanism until the iterative optimization objective is met includes: The collaborative optimization mechanism takes the population distribution of path parameter vectors as the optimization object, jointly adjusts the search direction and search scale through distribution statistics, and modulates the update direction and step size according to the constraint cost in each iteration, and performs feasible region repair operation on out-of-bounds or infeasible candidate solutions.

[0011] Preferably, the distributed adaptive collaborative optimization mechanism includes: a success rate-driven adaptive adjustment mechanism, a stochastic sensitivity hierarchical modulation mechanism, a topology-constrained local learning mechanism, and a constraint-aware stability enhancement mechanism, wherein: The success rate-driven adaptive adjustment mechanism is as follows: based on the proportion of newly generated candidate solutions that satisfy the constraints and enter the preferred set, the step size or scale parameter of the distribution update is dynamically adjusted so that the search process automatically switches between exploration and convergence. The random sensitivity hierarchical modulation mechanism is as follows: based on the performance gap between individual candidate solutions and the current optimal solution, random perturbations of different intensities are applied to different individuals; The topology-constrained local learning mechanism is as follows: by pre-setting a neighborhood topology structure, local optimal information is propagated among candidate solutions, limiting the direct dominance of the global optimal solution on the entire population, so as to suppress premature convergence. The constraint-aware stability enhancement mechanism is as follows: during the distribution update and individual generation process, the update magnitude is pruned, and projection or reflection mapping operations are performed on candidate solutions that violate the feasible region constraint, so as to improve the numerical stability and feasibility of the iteration process.

[0012] Preferably, the feasible region repair includes at least one of reflection, projection, and clipping; when a candidate solution violates spatial boundary, height / gap, or kinematic threshold constraints, feasible region repair is triggered to map the violating component back to the allowed interval.

[0013] Based on another aspect of the present invention, the present invention provides a three-dimensional path generation system based on constraint modulation adaptive update, the system comprising: An acquisition unit is used to acquire three-dimensional environment information and establish an environment model based on the three-dimensional environment information. The parameterization unit performs parameterization processing on the path to be planned in the environment model to obtain the path parameter vector; The determining unit is used to determine each path point of the candidate path corresponding to the path parameter vector and calculate the risk cost and constraint cost based on the spatial information of obstacles extracted from the environment model. The construction unit is used to construct a comprehensive cost function based on the risk cost and the constraint cost; The execution unit is used to determine the iterative optimization objective based on the comprehensive cost function, and to iteratively update the path parameter vector using a constraint-aware distributed adaptive collaborative optimization mechanism until the iterative optimization objective is met. The result unit is used to output the optimal three-dimensional feasible path corresponding to the path parameter vector that satisfies the iterative optimization objective.

[0014] According to another aspect of the present invention, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of a three-dimensional path generation method based on constraint modulation adaptive update.

[0015] According to another aspect of the present invention, the present invention provides an electronic device, characterized in that it comprises: The aforementioned computer-readable storage medium; and One or more processors for executing a program in the computer-readable storage medium.

[0016] Technical solution This invention provides a method and system for generating 3D paths based on constraint modulation adaptive updates. The method includes: acquiring 3D environmental information and establishing an environmental model based on the 3D environmental information; parameterizing the paths to be planned in the environmental model to obtain path parameter vectors; determining each path point of the candidate paths corresponding to the path parameter vectors based on the spatial information of obstacles extracted from the environmental model, calculating the single-point risk value of each path point according to the evaluation rules of the constructed risk model, and determining the risk cost corresponding to the path parameter vectors based on the single-point risk values ​​of each path point; constructing at least one continuously differentiable constraint cost function, and determining the constraint cost of the path parameter vectors based on the constraint cost function; constructing a comprehensive cost function based on the risk cost and constraint cost; determining the iterative optimization objective based on the comprehensive cost function, iteratively updating the path parameter vectors using a constraint-aware distributed adaptive collaborative optimization mechanism until the iterative optimization objective is met; and outputting the optimal 3D feasible path corresponding to the path parameter vectors that meet the iterative optimization objective.

[0017] This invention transforms 3D path planning into a constrained optimization problem of path parameters, achieving unified evaluation by constructing a comprehensive cost function that includes risk costs and physical feasibility constraint costs. The constraint costs are continuously differentiable, ensuring minimal impact on the optimization process when path parameters are within allowable limits, while increasing continuously with the violation amount when parameters exceed constraint boundaries. Based on this comprehensive cost function, a constraint-aware, distributed-level adaptive collaborative optimization mechanism iteratively updates the path parameters, jointly adjusting the search direction and scale using statistics of the candidate solution population distribution, and modulating the update amplitude using constraint costs. During iteration, feasible region repair operations are performed on out-of-bounds candidate solutions, ensuring stable evolution of the search process around the feasible region, thereby outputting a 3D feasible path and improving iterative stability and feasible solution acquisition capabilities.

[0018] Beneficial effects Compared with the prior art, the present invention has at least the following characteristics: The risk modeling is more refined and can reflect the real collision risks that are both direction-related and state-related; The constraint construction is continuous and stable, avoiding numerical instability caused by hard thresholds; The optimization process always revolves around the evolution of the feasible region, improving the success rate of path generation; It can stably generate high-quality executable paths in complex, three-dimensional, constrained environments. Attached Figure Description

[0019] Exemplary embodiments of the present invention can be more fully understood by referring to the following figures: Figure 1 The flowchart is a three-dimensional path generation method based on constraint modulation adaptive update according to an embodiment of the present invention. Figure 2 A flowchart of a three-dimensional path generation method based on constraint modulation adaptive update according to an embodiment of the present invention; and Figure 3 This is a structural diagram of a three-dimensional path generation system based on constraint modulation adaptive update according to an embodiment of the present invention. Detailed Implementation

[0020] Exemplary embodiments of the invention will now be described with reference to the accompanying drawings. However, the invention may be embodied in many different forms and is not limited to the embodiments described herein. These embodiments are provided to fully and completely disclose the invention and to fully convey its scope to those skilled in the art. The terminology used in the exemplary embodiments illustrated in the drawings is not intended to limit the invention. In the drawings, the same units / elements are referred to by the same reference numerals.

[0021] Unless otherwise stated, the terms used herein (including technical terms) have their common meaning as understood by one of ordinary skill in the art. Furthermore, it is understood that terms defined in commonly used dictionaries should be understood to have a meaning consistent with the context of their relevant field, and not to be interpreted as having an idealized or overly formal meaning.

[0022] Figure 1 The flowchart is a three-dimensional path generation method based on constraint modulation adaptive update according to an embodiment of the present invention. This invention relates to a three-dimensional constrained path planning method based on risk field modeling, continuous differentiable constraint cost construction, and constraint-aware adaptive update mechanism in a three-dimensional environment. It can be applied to underwater vehicles, drones, ground mobile robots, warehouse robots, and other three-dimensional moving bodies.

[0023] In this embodiment, 3D environmental information is acquired and an environmental model is established. The planned path is parameterized to obtain a path parameter vector. The risk cost of candidate paths is calculated based on obstacle spatial information, and at least one continuously differentiable constraint cost function is constructed to obtain the constraint cost. A comprehensive cost function is then constructed based on this. Subsequently, a constraint-aware distributed adaptive collaborative optimization mechanism is used to iteratively update the path parameter vector. When a boundary violation occurs, a feasible region repair operation is performed until a preset termination condition is met, and the optimized 3D feasible path determined by the path parameter vector is output. Optionally, gradient information or an evolutionary distributed update method can be introduced during the update process of the collaborative optimization mechanism to adapt to different computing resources and application scenario requirements. Figure 1 As shown, this invention provides a three-dimensional path generation method based on constraint modulation adaptive update, the method comprising: Step 101: Obtain 3D environment information and build an environment model based on the 3D environment information; This embodiment acquires three-dimensional environmental information and establishes an environmental model (obstacle set / spatial boundary / terrain or space occupancy).

[0024] An environmental model is a digital abstract model constructed based on acquired 3D environmental information to characterize the physical features within a target space. Its core includes three key elements: first, an obstacle set, which is the set of 3D coordinates, geometric shapes, and spatial occupancy information of all static obstacles in the space; second, boundary information, which is the 3D parameters of the physical boundary range of the target space (such as walls, fences, area boundary lines, etc.); and third, terrain or spatial occupancy information, which is the terrain undulation parameters (applicable to outdoor scenes) or the three-dimensional spatial occupancy status (applicable to indoor scenes) within the target space.

[0025] Step 102: Parametrically process the path to be planned in the environmental model to obtain the path parameter vector; In this embodiment, the path to be planned is parametrically processed to obtain the path parameter vector. Preferably, the path parameter vector is a three-dimensional sequence composed of multiple path nodes; alternatively, the path parameter vector may also be a control point sequence, spline coefficients, or other equivalent path parameterization forms.

[0026] Step 103: Obtain each path point of the candidate path based on the path parameter vector; use the spatial information of obstacles extracted from the environment model to calculate the single-point risk value of each path point according to the evaluation rules of the constructed risk model; determine the risk cost corresponding to the path parameter vector based on the single-point risk value of each path point. Specifically, including: Based on the candidate path represented by the path parameter vector, determine the position of each discrete path point in three-dimensional space. For each path point, the collision risk at the path point is quantitatively assessed using a risk model, taking into account its spatial relative relationship with obstacles in the environment model, to obtain the corresponding single-point risk value. Aggregate the individual risk values ​​of all path points in the candidate path to obtain the risk cost corresponding to the path parameter vector.

[0027] The risk model can be constructed based on a risk domain or risk field to characterize the safety buffer zone around the waypoint. It can also asymmetrically modulate the risk assessment in different spatial directions according to the intensity, direction or projection components of the external disturbance field to reflect the impact of external disturbances on path safety.

[0028] Preferably, the risk model employs a continuously differentiable risk assessment function, such that when a path point is far from an obstacle, the risk cost approaches zero, while when a path point approaches or enters the obstacle's risk area, the risk cost continuously increases with the degree of intrusion.

[0029] In a preferred embodiment, the calculation of risk cost in step 103 is implemented using a risk field model based on external disturbance modulation. The risk field model uses the intensity, direction, or projection components of the external disturbance field to adjust the spatial shape or directional weight of the risk field, so that the risk field exhibits asymmetric distribution characteristics in the direction dominated by the external disturbance; wherein the external disturbance field includes at least one of the following: flow field, wind field, and velocity field.

[0030] In this preferred embodiment, a risk constraint model is used to impose safety constraints and quantify risk costs on candidate paths in 3D path planning. It uses collision risk constraint rules based on the safety domain intrusion degree, combined with evaluation rules from a risk field model, to quantitatively assess the degree to which path points intrude into the danger zone of obstacles. Specifically, spatial feasibility and collision safety in path planning are constrained by a risk domain-based collision constraint model. This model is used to suppress path points from entering the danger zone of obstacles, thereby ensuring the safe and executable nature of the generated path.

[0031] (1) Risk domain model and intrusion margin By waypoint Centered on a pathpoint, a corresponding security risk domain is constructed to describe the security buffer zone surrounding that pathpoint. The risk domain is defined by a normalized risk domain function. Indicates the use of obstacle points. Relative to path point The degree of intrusion.

[0032] Based on the risk domain function, define the obstacle point. Relative to path point The intrusion margin is: (1) when When, it indicates the risk domain of an obstacle point intruding into a path point; When the time is right, it indicates that the obstacle point is in a safe zone.

[0033] In this embodiment, the risk domain is not a fixed-scale isotropic safety region, but rather an anisotropic risk domain modulated by external disturbances. Risk domain function The specific form can be set according to the platform's safety standards, and is used to uniformly depict the relative safety relationship between path points and obstacles.

[0034] Set path points The perturbation velocity vector at the location is Its direction is used to characterize the main direction of the external disturbance on the path point.

[0035] By waypoint Centered on the main axis, an anisotropic risk domain with a principal axis and a secondary axis is constructed, wherein the direction of the principal axis is consistent with the direction of the external disturbance field or its projection direction, and the direction of the secondary axis is orthogonal to the direction of the principal axis.

[0036] Let the scale parameters of the risk region along the principal axis and the secondary axis be respectively. and The scale parameter is then adaptively modulated based on the intensity of the external disturbance field and its directional projection, specifically as follows: When the path point's movement direction is in the same direction as the dominant disturbance direction or the disturbance intensity is large, increase the principal axis scale parameter. To enhance the safety margin in the opposite direction of the dominant disturbance; When the pathpoint movement direction is consistent with the dominant disturbance direction or the disturbance effect is weak, reduce the principal axis scale parameter. To reduce unnecessary conservatism; Secondary axis scale parameters Modulation is performed based on the transverse component of the external disturbance field to keep the risk domain relatively small in the transverse direction.

[0037] The risk domain function in this embodiment can be constructed using an anisotropic quadratic form, by... Determine the unit vector of the principal axis direction The horizontal unit vector orthogonal to it For any obstacle point and waypoints Let the relative displacement And decomposed into in this local coordinate system (2) in, Let be the projection component of the relative displacement vector along the principal axis. This represents the projection component of the relative displacement vector in the lateral direction. This represents the relative height difference between the obstacle point and the path point in the vertical direction. and These are the vertical heights of the obstacle point and the path point, respectively. For path point index, Indexing obstacles; The risk domain function can then be defined as: (3) in , , These are the scale parameters of the risk domain in the principal axis direction, the lateral direction, and the vertical direction, respectively.

[0038] Under the above scale parameter modulation, the risk domain function As the state of the external disturbance field changes at the path point, the same obstacle point will have different intrusion margins under different external disturbance field conditions.

[0039] In addition to the risk field modeling methods based on continuous functions mentioned above, risk assessment methods based on discrete grid accumulation, probability occupancy, or sampling statistics can also be adopted.

[0040] (2) Smoothing penalty function To avoid using discontinuous hard threshold constraints, this embodiment introduces a continuously differentiable smooth penalty function for the intrusion margin.

[0041] The smoothing penalty function is defined as follows: (4) in The penalty slope parameter is used to adjust the rate at which the intrusion penalty increases with the degree of intrusion. When the obstacle point is far from the risk domain, the penalty value is close to zero; as the obstacle point gradually intrudes into the risk domain, the penalty value increases continuously with the degree of intrusion.

[0042] (3) Risk and cost of path point collision Based on the aforementioned intrusion margin and smoothing penalty function, the first... The collision risk cost of each path point is: (5) in Risk weights are related to the degree of intrusion and are used to reflect the risk level corresponding to different degrees of intrusion.

[0043] (4) Overall path collision constraint cost The collision risk costs at each path point are summed to obtain the overall collision constraint cost of the path: (6) Therefore, by quantitatively assessing the degree of risk intrusion between waypoints and obstacles, a collision constraint cost term is formed for subsequent optimization processes.

[0044] Step 104: Construct at least one continuously differentiable constraint cost function, and determine the constraint cost of the path parameter vector based on the constraint cost function; Preferably, the constraint cost function is used to characterize the degree to which the path parameter vector satisfies the physical feasibility conditions during the generation of candidate paths. The physical feasibility conditions include at least spatial boundary constraints, height or gap constraints, and kinematic constraints.

[0045] In this invention, the constraint cost function is constructed in a continuously differentiable form, such that when the path parameter vector is within the constraint allowable interval, the constraint cost and its gradient are approximately zero; when the path parameter exceeds the constraint boundary of the constraint cost function, the constraint cost and its gradient continuously increase with the violation amount, thereby applying a back-pushing effect to infeasible solutions in subsequent optimization iterations.

[0046] In a preferred embodiment, the constraint cost function in step 104 can be constructed and calculated in the following way: for each path point in the candidate path, constraints are modeled on its spatial position, height or gap conditions and attitude changes between adjacent path segments, and the degree of constraint violation is quantitatively evaluated by a continuously differentiable penalty function, thereby obtaining the constraint cost corresponding to the path parameter vector.

[0047] The constraint modulation form described in step 104 is a preferred implementation. In other implementations, piecewise functions, penalty functions, or smooth projection functions can also be used to continuously guide constraint violations.

[0048] (a) Height or clearance constraints In the candidate paths, for the th path... Constraint modeling is performed on the height or gap conditions at each path point. Specifically, the bottom distance or gap amount of each path point is defined. (e.g., DVL bottom distance or any "distance from obstacle surface" sensing quantity) and absolute depth (e.g., pressure depth output), and set their respective allowable ranges as follows. , .in The minimum safety gap threshold, The maximum effective gap threshold, and These are the minimum and maximum depth or height thresholds allowed for path points, respectively.

[0049] To achieve a continuously differentiable constraint form, a smooth boundary penalty function is introduced to quantitatively evaluate the degree of violation of path point height or gap conditions. The smooth boundary violation penalty function is defined as follows: (8) in, Represents the path-related scalar variable to be constrained, used to characterize the value of the path parameter vector in a certain physical constraint dimension; Representing variables Allowable lower bound threshold Representing variables The upper limit threshold for allowable values.

[0050] Furthermore, in the case of degradation of gap measurement confidence, a confidence factor can be introduced to weight and modulate the gap constraint term to reduce the impact of unreliable measurement on the constraint cost, thereby improving the robustness of the path optimization process.

[0051] Based on the above penalty function, the first path in the candidate path is represented as... Cost of height or gap constraints at each path point Defined as: (9) in, It measures confidence level, and its function is to automatically reduce the influence of the bottom distance term when bottom distance tracking degrades. Let be the weights. Further, the overall path height or gap constraint cost function is expressed as: (10) (ii) Kinematic constraints In this preferred embodiment, kinematic constraints are applied to the candidate path to account for attitude changes between adjacent path segments during path generation. These kinematic constraints include at least heading and pitch variation constraints.

[0052] Specifically, let the candidate path be the first... The position vectors of the path points are: (1) Heading change constraints Calculate the heading angle of the path segment based on the relative positions of adjacent path points in the horizontal plane. Furthermore, the heading changes between adjacent path segments are obtained: (11) in, This represents a mapping function that normalizes the angle difference to a preset angle range.

[0053] (2) Pitch variation constraints Based on the relative positions of adjacent path points in the vertical and horizontal projection directions, calculate the pitch angle of the path segment: (12) Furthermore, the pitch variation between adjacent path segments is obtained. .

[0054] in .

[0055] (3) Penalties and constraints for kinematic violations Set the allowable thresholds for heading change, pitch angle, and pitch change respectively. It is used to characterize the capability boundaries of a motion platform in terms of attitude changes.

[0056] Based on the aforementioned attitude change amounts and corresponding thresholds, a continuously differentiable violation penalty function is introduced to quantitatively evaluate attitude changes exceeding the allowable range, resulting in the kinematic constraint cost function corresponding to the path parameter vector: (13) in, Let be a continuously differentiable penalty function for violations. , , These are the weighting coefficients.

[0057] Based on this, a second-order angle smoothing term is introduced: (14) The overall kinematic constraints are combined as follows: (15) in These are the weighting coefficients that balance kinematic feasibility and trajectory smoothness. Step 105: Construct a comprehensive cost function based on risk cost and constraint cost, and determine the total cost of the path parameter vector; Based on the risk cost determined in step 103 and the constraint cost determined in step 104, a comprehensive cost function corresponding to the path parameter vector is constructed, and the total cost of the path parameter vector is determined according to the comprehensive cost function, which is used to evaluate and compare the path parameter vector in the subsequent optimization process.

[0058] In this invention, the comprehensive cost function is used to provide a unified quantitative description of candidate paths in terms of security, physical feasibility, and path efficiency, so as to comprehensively evaluate the path parameter vector under the continuous optimization framework.

[0059] (1) Construction of the comprehensive cost function In a preferred embodiment, the comprehensive cost function includes at least a risk cost term, a constraint cost term, and an energy consumption cost term, wherein: The risk cost term is used to characterize the risk relationship between candidate paths and obstacles in the environment; The constraint cost term is used to characterize the degree to which candidate paths satisfy physical feasibility constraints; The energy consumption cost term is used to characterize the path length or motion energy consumption characteristics of candidate paths.

[0060] In this preferred embodiment, the comprehensive cost function It can be represented as: (17) in, This represents the risk cost determined in step 103; and These represent the height or clearance constraint cost and the kinematic constraint cost determined in step 104, respectively. This represents the energy cost, used to characterize the path length or energy consumption of a candidate path; , , and These are the weighting coefficients used to balance the relative importance of each cost item.

[0061] In the subsequent path optimization process, the comprehensive cost corresponding to different path parameter vectors is calculated. By comparing the results, the path parameter vector with the smaller overall cost is selected as the optimization direction or candidate solution.

[0062] (2) Termination conditions and feasibility assessment In one embodiment of the present invention, the optimization process of the path parameter vector ends when a preset termination condition is met. The termination condition includes at least one of reaching a preset number of iterations, the change of the comprehensive cost function satisfying the convergence criterion, or obtaining a feasible path parameter vector that meets the constraints.

[0063] After the termination condition is met, the feasibility of the current path parameter vector is determined. When the path parameter vector meets the risk constraints and physical feasibility constraints, it is determined as the output path parameter vector. When the path parameter vector does not meet the feasibility requirements, the optimization process can continue or a new candidate path parameter vector can be generated.

[0064] Step 106: Determine the iterative optimization objective based on the comprehensive cost function, and iteratively update the path parameter vector using a constraint-aware distributed adaptive collaborative optimization mechanism until the iterative optimization objective is met; In this embodiment, the candidate 3D path is represented as a path parameter vector, and its comprehensive cost function is used. As the optimization objective, the comprehensive cost function is composed of a weighted sum of regularization terms for collision risk cost, height or gap constraint cost, kinematic constraints, and energy consumption cost. Maintenance includes... A population of candidate solutions is established, and distribution parameters for distribution sampling are maintained; each iteration includes the following processing: (1) Distributed sampling generates new candidate solutions Based on the current distribution parameters, a new set of candidate solutions is generated by randomly sampling the path parameter vector. To ensure that the variable values ​​are valid, boundary processing such as reflection or clipping is performed on the components that exceed the preset variable boundaries, so that the new candidate solutions fall within the preset value range and are used as new samples in this iteration to participate in subsequent evaluation and selection. (2) Evaluation and selection based on comprehensive cost function After merging the original candidate solutions and the new candidate solutions, respectively, they are substituted into the comprehensive cost function. Calculate and sort the target values, then select the top values ​​with the best target values. Candidate solutions are considered as the surviving set; among them, collision risk, altitude feasibility, and kinematic constraints are determined through... The penalty form of the corresponding cost term is reflected in order to suppress candidate solutions that violate constraints during the selection process; (3) Success rate-driven adaptive adjustment The proportion of newly sampled candidate solutions entering the surviving set (or elite set) in this iteration is used as the success rate indicator; based on the deviation between the success rate and the preset target success rate, the distributed sampling step size parameter and the updated learning rate are adaptively adjusted so that the exploration scale is increased when the success rate is low and the convergence strength is enhanced when the success rate is high; at the same time, the success rate is subjected to sliding statistics to suppress the influence of random fluctuations on parameter adjustment, thereby improving the stability and convergence efficiency of the search process; (4) Quantum-inspired noise stratification perturbation and stability enhancement A quantum-inspired hierarchical perturbation update is performed on the candidate solutions in the surviving set. The "optimal guiding term" and the "random perturbation term" are linearly superimposed according to the quantum state weight parameters to form the update direction. The weight parameters are adaptively adjusted according to the performance gap between the candidate solution and the current optimal solution, and the intensity of the random perturbation gradually decreases with the iteration progress to balance early global exploration and later local convergence. At the same time, a limit threshold is set for the update step size of each dimension, and the update amount exceeding the threshold is pruned. After the update is completed, if the candidate solution component exceeds the variable boundary, boundary repair by reflection or pruning is performed to suppress numerical oscillations and improve the stability of the iteration process. (5) Topology-constrained local learning Construct a preset neighborhood topology on the candidate solution set, preferably a ring topology, so that each candidate solution only interacts with a few of its neighboring neighbors; for any candidate solution, select the neighborhood optimal solution with a better target value from its neighborhood, and update the current candidate solution to the neighborhood optimal solution locally according to the preset topology learning coefficient. By limiting the scope of information propagation, the rapid homogenization of global optimal information in the population is avoided, thereby improving the efficiency of local improvement and reducing the risk of premature convergence while maintaining diversity, so as to improve the robustness of global optimization under complex constraint scenarios; (6) Termination criterion When the number of iterations reaches the upper limit or the improvement of the optimal value of the comprehensive cost function is less than the threshold in several consecutive iterations, the iteration stops and the optimal path parameter vector is output.

[0065] Through the aforementioned constraint-aware, distributed-level adaptive collaborative optimization mechanism, this invention directly incorporates collision / risk, height gap, and kinematic constraints as modulation terms into the distributed update process. This allows candidate solutions to automatically suppress unstable updates and backtrack to the feasible region when approaching constraint boundaries, thereby significantly improving the probability of feasible path generation and convergence stability. The above mechanism can be combined, tailored, or replaced according to specific application requirements and is not a necessary limitation of this invention. Furthermore, based on the synergistic effect of success rate-based adaptive adjustment, stochastic sensitivity hierarchical perturbation, and topology-constrained local learning, this invention balances global exploration and local convergence in complex constraint scenarios, reducing the risk of premature convergence and improving the quality of the final path and the planning success rate.

[0066] Step 107: Based on the optimal path parameter vector obtained in Step 106, generate and output the optimal three-dimensional constrained path.

[0067] In this embodiment, the optimal path parameter vector output in step 106 is substituted into a preset path representation model to calculate the corresponding three-dimensional path curve / polyline. The three-dimensional path is discretized to obtain a sequence of path points arranged in the navigation order, and the corresponding three-dimensional coordinate information is given for each path point. Furthermore, a consistency check is performed on the discretized path point sequence to ensure that it meets the spatial boundary constraints and the collision risk, height / clearance, and kinematic constraints suppressed by the comprehensive cost function. The path point sequence that meets the constraints is output as the planning result for subsequent navigation tracking and motion control.

[0068] Preferably, the output includes a path point sequence and segment lengths between adjacent path points, as well as heading / pitch variation constraint parameters, to facilitate speed planning and tracking control by the control module.

[0069] Preferably, the iterative optimization objective is determined based on the comprehensive cost function, and the path parameter vector is iteratively updated using a constraint-aware distributed adaptive collaborative optimization mechanism until the iterative optimization objective is met, including: The iterative optimization objective is determined based on the comprehensive cost function. A constraint-aware, distribution-level adaptive collaborative optimization mechanism is used to iteratively update the path parameter vector. Within the constraint-allowed interval, the function value and first derivative are approximately zero. After exceeding the constraint boundary, the function value and first derivative continuously increase with the violation amount. The collaborative optimization mechanism takes the group distribution of the path parameter vector as the optimization object, and jointly adjusts the search direction and search scale through distribution statistics. In each iteration, the update direction and step size are modulated according to the constraint cost. For out-of-bounds or infeasible candidate solutions, feasible region repair operations are performed until the iterative optimization objective is met.

[0070] This invention uses the population distribution of path parameter vectors as the optimization object, and adjusts the search direction and search scale jointly through distribution statistics. In each iteration, the update direction and update step size are modulated according to the constraint cost, so that the update amplitude is reduced when the candidate solution is close to or violates the constraint boundary, and a large degree of search freedom is maintained when it is inside the feasible region. At the same time, feasible region repair operation is performed on candidate solutions that are out of bounds or infeasible, until the iterative optimization objective is met.

[0071] Figure 3 This is a structural diagram of a three-dimensional path generation system based on constraint modulation adaptive update according to an embodiment of the present invention.

[0072] like Figure 3 As shown, this invention provides a 3D path generation system based on constraint modulation adaptive update, the system comprising: The acquisition unit 301 is used to acquire three-dimensional environmental information and establish an environmental model, and to perform parameterization on the planned path to obtain the path parameter vector. Preferably, the three-dimensional environmental information includes: obstacle set, spatial boundary, terrain or spatial occupancy information; The path parameter vector is a three-dimensional sequence of multiple path nodes, or an equivalent path parameterization form of a control point sequence or spline coefficients.

[0073] The determining unit 302 is used to obtain each path point of the candidate path based on the path parameter vector, calculate the single-point risk value of each path point according to the evaluation rules of the constructed risk model using the spatial information of obstacles extracted from the environmental model; determine the risk cost corresponding to the path parameter vector based on the single-point risk value of each path point; and construct at least one continuously differentiable constraint cost function, and determine the constraint cost of the path parameter vector based on the constraint cost function. Preferably, the risk field model includes a principal axis scale parameter and a secondary axis scale parameter, and the principal axis scale parameter adaptively increases or decreases according to the projection component of the external disturbance field in the tangential direction of the candidate path. The evaluation rules of the risk model include adjusting the spatial shape or directional weight of the risk field by using the intensity, direction or projection components of the external disturbance field, so that the risk field exhibits asymmetric distribution characteristics in the direction dominated by the external disturbance; the external disturbance field includes at least one of the following: flow field, wind field, and velocity field.

[0074] Construction unit 303 is used to construct a comprehensive cost function based on risk cost and constraint cost; Execution unit 304 is used to determine the iterative optimization objective based on the comprehensive cost function, and to iteratively update the path parameter vector using a constraint-aware distributed adaptive collaborative optimization mechanism until the iterative optimization objective is met; Result unit 305 is used to output the optimal three-dimensional feasible path corresponding to the path parameter vector that satisfies the iterative optimization objective.

[0075] Preferably, the constraint cost function includes: spatial boundary constraints, height or gap constraints, and kinematic constraints; for out-of-bounds or infeasible candidate solutions, at least one feasible domain repair operation among reflection, projection, or pruning can be performed.

[0076] The constraint cost function satisfies the following properties: when the path parameter vector is within the constraint allowable range of the constraint cost function, the constraint cost and its gradient are approximately zero; when the path parameter exceeds the constraint boundary of the constraint cost function, the constraint cost and its gradient continuously increase with the amount of violation.

[0077] Preferably, the iterative optimization objective is determined based on the comprehensive cost function, and the path parameter vector is iteratively updated using a constraint-aware distributed adaptive collaborative optimization mechanism until the iterative optimization objective is met, including: The iterative optimization objective is determined based on the comprehensive cost function. A constraint-aware, distribution-level adaptive collaborative optimization mechanism is used to iteratively update the path parameter vector. Within the constraint-allowed interval, the function value and first derivative are approximately zero. After exceeding the constraint boundary, the function value and first derivative continuously increase with the violation amount. The collaborative optimization mechanism takes the group distribution of the path parameter vector as the optimization object, and jointly adjusts the search direction and search scale through distribution statistics. In each iteration, the update direction and step size are modulated according to the constraint cost. For out-of-bounds or infeasible candidate solutions, feasible region repair operations are performed until the iterative optimization objective is met.

[0078] Preferably, the distributed adaptive collaborative optimization mechanism includes: a success rate-driven adaptive adjustment mechanism, a stochastic sensitivity hierarchical modulation mechanism, a topology-constrained local learning mechanism, and a constraint-aware stability enhancement mechanism, wherein: The success rate-driven adaptive adjustment mechanism is as follows: based on the proportion of newly generated candidate solutions that satisfy the constraints and enter the preferred set, the step size or scale parameter of the distribution update is dynamically adjusted so that the search process automatically switches between exploration and convergence. The stochastic sensitivity hierarchical modulation mechanism is as follows: based on the performance gap between individual candidate solutions and the current optimal solution, different intensity of random perturbation is applied to different individuals; The topology-constrained local learning mechanism is as follows: by pre-setting the neighborhood topology, local optimal information is propagated among candidate solutions, thus limiting the direct dominance of the global optimal solution over the entire population. The stability enhancement mechanism of constraint awareness is as follows: during the distribution update and individual generation process, the update magnitude is pruned, and projection or reflection mapping operations are performed on candidate solutions that violate the feasible region constraints.

[0079] Preferably, feasible region repair includes at least one of reflection, projection, and clipping; when a candidate solution violates spatial boundary, height / gap, or kinematic threshold constraints, feasible region repair is triggered to map the violating component back to the allowed interval.

[0080] The present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the above-described constraint modulation adaptive update-based three-dimensional path generation method.

[0081] This invention provides an electronic device, comprising: The aforementioned computer-readable storage medium; and One or more processors for executing a program in a computer-readable storage medium.

[0082] The present invention has been described with reference to a few embodiments. However, it will be apparent to those skilled in the art that other embodiments besides those disclosed above fall equivalently within the scope of the present invention.

[0083] Generally, all terms used in this invention are interpreted according to their ordinary meaning in the art, unless otherwise expressly defined herein. All references to “a / the / the [device, component, etc.]” ​​are openly interpreted as at least one instance of said device, component, etc., unless otherwise expressly stated. The steps of any method disclosed herein need not be performed in the exact order disclosed unless explicitly stated otherwise.

[0084] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0085] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0086] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0087] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0088] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the protection scope of the present invention.

Claims

1. A three-dimensional path generation method based on constraint modulation adaptive update, characterized in that, The method includes: Acquire three-dimensional environmental information and establish an environmental model based on the three-dimensional environmental information; The path to be planned in the environment model is parameterized to obtain the path parameter vector; Based on the path parameter vector, each path point of the candidate path is obtained. Using the spatial information of obstacles extracted from the environment model, the single-point risk value of each path point is calculated according to the evaluation rules of the constructed risk model. Based on the single-point risk value of each path point, the risk cost corresponding to the path parameter vector is determined. Construct at least one continuously differentiable constraint cost function, and determine the constraint cost of the path parameter vector based on the constraint cost function; Construct a comprehensive cost function based on the aforementioned risk cost and constraint cost; Based on the comprehensive cost function, the iterative optimization objective is determined, and the path parameter vector is iteratively updated using a constraint-aware distributed adaptive collaborative optimization mechanism until the iterative optimization objective is met. Output the optimal three-dimensional feasible path corresponding to the path parameter vector that satisfies the iterative optimization objective.

2. The method according to claim 1, characterized in that, The three-dimensional environmental information includes: obstacle set, spatial boundary, terrain or space occupancy information; The path parameter vector is a three-dimensional sequence composed of multiple path nodes, or an equivalent path parameterization form of a control point sequence or spline coefficients.

3. The method according to claim 1, characterized in that, The risk field model includes a principal axis scale parameter and a secondary axis scale parameter, and the principal axis scale parameter adaptively increases or decreases according to the projection component of the external disturbance field in the tangential direction of the candidate path. The evaluation rules of the risk model include adjusting the spatial shape or directional weight of the risk field by using the intensity and direction of the external disturbance field or the projection component in the tangential direction of the candidate path, so that the risk field exhibits asymmetric distribution characteristics in the direction dominated by the external disturbance. The external disturbance field includes at least one of the following: flow field, wind field, and velocity field.

4. The method according to claim 1, characterized in that, The constraint cost function includes: spatial boundary constraints, height or gap constraints, and kinematic constraints; The constraint cost function satisfies the following properties: when the path parameter vector is within the constraint allowable range of the constraint cost function, the constraint cost and its gradient are approximately zero; when the path parameter exceeds the constraint boundary of the constraint cost function, the constraint cost and its gradient continuously increase with the amount of violation.

5. The method according to claim 1, characterized in that, The step of determining the iterative optimization objective based on the comprehensive cost function and iteratively updating the path parameter vector using a constraint-aware distributed adaptive collaborative optimization mechanism until the iterative optimization objective is met includes: Based on the comprehensive cost function, the iterative optimization objective is determined. A constraint-aware, distribution-level adaptive collaborative optimization mechanism is used to iteratively update the path parameter vector. Within the constraint-allowed interval, its function value and first derivative are approximately zero. After exceeding the constraint boundary, its function value and first derivative continuously increase with the violation amount. The collaborative optimization mechanism takes the group distribution of the path parameter vector as the optimization object, and jointly adjusts the search direction and search scale through distribution statistics. In each iteration, the update direction and step size are modulated according to the constraint cost. For out-of-bounds or infeasible candidate solutions, feasible region repair operations are performed until the iterative optimization objective is met.

6. The method according to claim 5, characterized in that, The distributed-level adaptive collaborative optimization mechanism includes: a success rate-driven adaptive adjustment mechanism, a stochastic sensitivity hierarchical modulation mechanism, a topology-constrained local learning mechanism, and a constraint-aware stability enhancement mechanism, wherein: The success rate-driven adaptive adjustment mechanism is as follows: based on the proportion of newly generated candidate solutions that satisfy the constraints and enter the preferred set, the step size or scale parameter of the distribution update is dynamically adjusted so that the search process automatically switches between exploration and convergence. The random sensitivity hierarchical modulation mechanism is as follows: based on the performance gap between individual candidate solutions and the current optimal solution, random perturbations of different intensities are applied to different individuals; The topology-constrained local learning mechanism is as follows: by pre-setting a neighborhood topology structure, local optimal information is propagated among candidate solutions, thereby limiting the direct dominance of the global optimal solution over the entire population. The stability enhancement mechanism of the constraint perception is as follows: during the distribution update and individual generation process, the update magnitude is pruned, and projection or reflection mapping operations are performed on candidate solutions that violate the feasible region constraint.

7. The method according to claim 5, characterized in that, The feasible region repair includes at least one of reflection, projection, and clipping; when a candidate solution violates spatial boundary, height / gap, or kinematic threshold constraints, feasible region repair is triggered to map the violating component back to the allowed interval.

8. A three-dimensional path generation system based on constraint modulation adaptive update, characterized in that, The system includes: An acquisition unit is used to acquire three-dimensional environment information and establish an environment model based on the three-dimensional environment information. The parameterization unit performs parameterization processing on the path to be planned in the environment model to obtain the path parameter vector; The determining unit is used to obtain each path point of the candidate path based on the path parameter vector, calculate the single-point risk value of each path point according to the evaluation rules of the constructed risk model using the spatial information of obstacles extracted from the environment model; determine the risk cost corresponding to the path parameter vector based on the single-point risk value of each path point; and construct at least one continuously differentiable constraint cost function, and determine the constraint cost of the path parameter vector based on the constraint cost function. The construction unit is used to construct a comprehensive cost function based on the risk cost and the constraint cost; The execution unit is used to determine the iterative optimization objective based on the comprehensive cost function, and to iteratively update the path parameter vector using a constraint-aware distributed adaptive collaborative optimization mechanism until the iterative optimization objective is met. The result unit is used to output the optimal three-dimensional feasible path corresponding to the path parameter vector that satisfies the iterative optimization objective.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps of the method as described in any one of claims 1-7.

10. An electronic device, characterized in that, include: The computer-readable storage medium as described in claim 9; as well as One or more processors for executing a program in the computer-readable storage medium.

Images