Motion camouflage re-parameterization and homotopy perception topology trajectory planning method and system

Abstract

The invention belongs to the technical field of trajectory planning, and particularly relates to a trajectory planning method and system of motion camouflage re-parameterization and homotopy perception topology. The method comprises the following steps: S1, acquiring a starting point and a terminal point of a robot and obstacle information of an environment, and carrying out trajectory planning problem modeling; s2, based on homotopy perception topology, planning a plurality of initial paths with different topologies in a free space of the environment; s3, aiming at each initial path, adopting a motion camouflage re-parameterization technology, taking the initial path as a virtual prey path, and mapping multi-dimensional waypoint position parameters on the path into a one-dimensional scalar control parameter sequence based on a selected reference point so as to construct a low-dimensional optimization problem; s4, in the low-dimensional optimization problem, optimizing the one-dimensional scalar control parameter sequence and the trajectory segment time, and generating a smooth trajectory satisfying the dynamic constraint and the obstacle avoidance constraint; and S5, selecting the trajectory with the lowest cost from the plurality of optimized trajectories as a final trajectory to be output.

Description

Technical Field

[0001] This invention belongs to the field of trajectory planning technology, specifically relating to trajectory planning methods and systems based on motion camouflage reparameterization and homotopy sensing topology. Background Technology

[0002] In the existing technology, there are two main methods to achieve dynamically feasible trajectory planning: sampling-based search and optimization-based trajectory planning.

[0003] Sampling-based planners, such as FMT* and BIT*, leverage stochastic geometries to achieve efficient, large-scale exploration in complex, obstacle-dense environments. Geometric sampling planners (e.g., RRT*) typically prioritize generating paths containing only geometric information, followed by temporal parameterization to satisfy dynamic constraints. In contrast, SST / SST* and dynamic RRT* variants perform derivation directly in the state space, thus eliminating the need for additional time reallocation steps. However, under frequent replanning and strong driving constraints, they may face challenges in temporal consistency and response speed.

[0004] Optimization-based methods directly embed smoothness, dynamic constraints, and obstacle avoidance requirements into the objective function or constraints. Methods represented by CHOMP / STOMP, TrajOpt, and GPMP frameworks utilize differentiability and sparse structures to achieve efficient gradient-based optimization. Convex relaxation through safety corridors or polyhedral approximations (such as IRIS and Bessel convex hulls) further enhances safety guarantees, while GCS-based methods provide compact convex relaxations with global optimality proofs under ideal assumptions. For aerial robots, time-optimal reassignment (such as TOPPRA), robust trajectory optimization for complex environments, MINCO or GCOPTER, and optimization methods without ESDF (Euclidean Signed Distance Field) have achieved fast airborne performance. However, with the increase in the number of trajectory segments and agents, the dimensionality of decision variables grows rapidly. This leads to increased computational latency as the problem size expands, posing a risk to response speed when frequent replanning is required. Besides scalability challenges, non-convex optimization is highly sensitive to initial conditions and often converges to unfavorable local minima, especially in the presence of multiple homotopy classes. Therefore, supporting both global-scale planning and real-time replanning remains challenging, prompting the need for more robust topology guidance and low-dimensional parameterization methods to stabilize the convergence process while reducing computational costs.

[0005] In summary, in complex environments, the computational speed of trajectory planning is often limited due to the high dimensionality and large scale of optimization problems. Furthermore, the inherent non-convexity of trajectory optimization makes it highly susceptible to getting trapped in suboptimal local minima, thus reducing motion efficiency and safety. Therefore, developing safe and adaptable trajectory planners remains a major challenge for aerial and ground robots. Planners must ensure dynamic feasibility, avoid collisions, and maintain trajectory smoothness, while also adhering to the real-time constraints of the onboard system. This challenge is exacerbated in multi-agent systems due to interactions between agents, the propagation of uncertainty, and communication constraints. Such systems require planning dynamically feasible trajectories and frequently optimizing them in response to environmental changes or the actions of surrounding agents.

[0006] Therefore, it is very important to design a trajectory planning method and system that can significantly improve the computation speed of motion camouflage reparameterization and homotopy perception topology while maintaining the efficiency and safety of agent motion. Summary of the Invention

[0007] This invention aims to overcome the problem that existing dynamic trajectory planning methods cannot simultaneously consider the motion efficiency, safety, and computational efficiency of intelligent agents. It provides a trajectory planning method and system that can significantly improve computational speed by using motion camouflage reparameterization and homotopy sensing topology while maintaining the motion efficiency and safety of intelligent agents.

[0008] To achieve the above-mentioned objectives, the present invention adopts the following technical solution:

[0009] The trajectory planning method based on motion camouflage reparameterization and homotopy-aware topology includes the following steps:

[0010] S1: Obtain information on the robot's starting point, ending point, and obstacles in the environment to model the trajectory planning problem;

[0011] S2, based on homotopy-aware topology, plans multiple initial paths with different topologies in the free space of the environment;

[0012] S3. For each initial path, a motion camouflage reparameterization technique is used to treat the initial path as a virtual prey path. Based on the selected reference point, the multi-dimensional waypoint position parameters on the path are mapped into a one-dimensional scalar control parameter sequence to construct a low-dimensional optimization problem.

[0013] S4, In the low-dimensional optimization problem, the one-dimensional scalar control parameter sequence and trajectory time segment are optimized to generate a smooth trajectory that satisfies the dynamic constraints and obstacle avoidance constraints;

[0014] S5 selects the trajectory with the lowest cost from the optimized trajectories as the final trajectory output.

[0015] Preferably, step S1 includes the following steps:

[0016] S11, where R represents the Euclidean geometric space composed of real numbers. An intelligent agent in Workspace , Collaborative task execution in an environment containing static obstacles. and time-varying dynamic obstacles The intelligent agent at any time Available accessibility areas are represented as Each intelligent agent The goal is to generate a smooth, dynamically feasible, and collision-free trajectory;

[0017] S12, defines that each agent is differentially flat, as follows:

[0018] For each intelligent agent Optimize the translation and flatten the output vector The trajectory planning problem is defined as a continuous-time constrained optimization problem; let... For the joint cost function, the problem is formally defined as follows: ;

[0019] in, Indicates until The stacking of order flat state; boundary conditions and A fixed initial state and a fixed terminal state are defined. It is the time regularization parameter. It is the total trajectory duration.

[0020] Preferably, step S2 includes the following steps:

[0021] S21, Construct a sparse topological graph of the free space using a probabilistic roadmap or skeletonization method;

[0022] S22, using H-signature (homotopy signature) as a topology discriminator, multiple paths with different homotopy categories between the start and end points in the sparse topology graph are searched as the initial paths. The specific process is as follows:

[0023] Set path A series of vertices This indicates that each obstacle is represented by a point. This indicates that for each obstacle, the corresponding integer number of revolutions is calculated. , used to represent the net counterclockwise revolutions of a path around an obstacle, . This represents the total cumulative rotation angle of the path around the obstacle, where K is the total number of path segments after discretization. Specifically, this is achieved by analyzing each path segment... Relative to obstacles The calculation is performed by summing the continuous angular changes: ;

[0024] in, It is a path continuous argument function that accumulates angle changes without a 2π jump; It is the nearest integer operator; the homotopy signature of the path. It is made by all A vector consisting of the independent rotation numbers of each obstacle:

[0025] .

[0026] Preferably, step S3 includes the following steps:

[0027] S31, based on the motion camouflage strategy, the initial path is used as the virtual prey path. And by selecting an additional reference point , high-dimensional waypoints Dimensionality reduced to a set of one-dimensional scalar parameters; each intermediate node Location Now given a scalar parameter The definition and specific formula are as follows:

[0028] ;

[0029] in, It is the virtual prey path corresponding to the first The position of each node;

[0030] S32, Set Track Characterized as On different segments Piecewise polynomials of order [order missing]; the optimization process focuses on prey path parameters and the duration of each segment; polynomial coefficients. It can be directly constructed by solving the following equation, where It is a strip matrix. This is the state constraint vector.

[0031] ;

[0032] Defined as the first Segment trajectory:

[0033] ;

[0034] in Describe a polynomial basis. It is the first The coefficient matrix of the segment; Indicates the degree of deviation from the virtual prey's path; The time interval representing the trajectory.

[0035] Preferably, step S4 includes the following steps:

[0036] S41, for the time parameter, set the time for each segment. By using piecewise smooth functions that guarantee positive definiteness From unconstrained variables This is derived from a mapping, and the specific formula is as follows:

[0037] ;

[0038] For spatial parameters, each scalar is defined. From unconstrained variables using a scaled logistic function Mapped from this, the function restricts its value to a predefined range. The specific formula is as follows:

[0039] ;

[0040] in, It is the logistic function;

[0041] S42, by sampling a fixed number of points within each trajectory segment, all continuous-time inequality constraints are... It is transformed into a penalty term and included in the objective function, using Let represent the total cost function. The final optimization problem is expressed as: ;

[0042] in, and It is a positive weight. Indicates effort to control. Total flight time , It includes dynamic feasibility constraints, static obstacle avoidance, and inter-agent collision avoidance; and All are unconstrained variables.

[0043] S43. By solving the formula in step S42, a smooth, dynamically feasible, and collision-free trajectory is obtained.

[0044] Preferably, step S5 further includes a local replanning step; the local replanning step includes the following steps:

[0045] S51, during the execution of the final trajectory, the predicted distance to the dynamic obstacle is monitored in real time;

[0046] If a collision risk exists within a future forward-looking time window, a local replanning will be triggered;

[0047] The local replanning includes:

[0048] By adding time as an additional dimension, a spatiotemporal state space is constructed, and the predicted trajectory of dynamic obstacles is represented as a static obstacle in the spatiotemporal state space.

[0049] In the spatiotemporal state space, multiple local initial paths are planned from the current state to the anchor point on the global trajectory at a certain future moment;

[0050] The motion camouflage reparameterization technique is used to optimize each local initial path to generate a local obstacle avoidance trajectory.

[0051] Preferably, in step S51, when distinguishing the homotopy categories of paths in the spatiotemporal state space, a projection-based approximation method is used; the projection-based approximation method includes the following steps:

[0052] The path and obstacle representations in the spatiotemporal state space are projected onto multiple low-dimensional subspaces;

[0053] Calculate the H-signature (homotopy signature) of the projected path in each low-dimensional subspace;

[0054] If the H-signatures (homotopy signatures) of two paths in any low-dimensional subspace are different, then the original paths are determined to have different topologies in the spatiotemporal state space.

[0055] This invention also provides a trajectory planning system based on motion camouflage reparameterization and homotopy-aware topology, including:

[0056] The information acquisition and modeling module is used to acquire information about the robot's starting point, ending point, and obstacles in the environment, and to model the trajectory planning problem.

[0057] The path planning module is used to plan multiple initial paths with different topologies in the free space of the environment based on homotopy-aware topology.

[0058] The path optimization module is used to employ motion camouflage reparameterization technology for each initial path, treating the initial path as a virtual prey path, and mapping the multi-dimensional waypoint position parameters on the path to a one-dimensional scalar control parameter sequence based on selected reference points, in order to construct a low-dimensional optimization problem.

[0059] The parameter optimization and trajectory generation module is used to optimize the one-dimensional scalar control parameter sequence and trajectory time segment in low-dimensional optimization problems, and generate a smooth trajectory that satisfies dynamic constraints and obstacle avoidance constraints.

[0060] The output selection module is used to select the trajectory with the lowest cost from multiple optimized trajectories as the final trajectory output.

[0061] Compared with the prior art, the beneficial effects of this invention are: (1) This invention proposes a novel reparameterization technique inspired by motion camouflage; it transforms the complex waypoint optimization problem into a task of optimizing a scalar sequence along the trajectory; this method maintains efficient gradient flow while preserving geometric interpretability; (2) This invention proposes a comprehensive homotopy-aware planning framework designed to solve the problem of dynamic obstacles and multi-agent coordination; the framework integrates a lightweight, homotopy-aware front-end for generating various topology candidate paths, and a structure-preserving back-end optimization to support general continuous-time constraints; in addition, this invention extends to distributed multi-agent environments through an asynchronous trajectory commitment process; (3) This invention proposes a spatiotemporal local replanning module that maintains only a short sliding time window to avoid expected dynamic obstacles and reconnect to the global trajectory, reducing solution time and latency while maintaining feasibility and consistency with the global plan. Attached Figure Description

[0062] Figure 1 This is a schematic diagram of a trajectory planning system based on motion camouflage reparameterization and homotopy sensing topology in this invention.

[0063] Figure 2 This is a schematic diagram illustrating the principle of motion camouflage in this invention;

[0064] Figure 3 Reference point in this invention A diagram illustrating the strategy selection process;

[0065] Figure 4 This is a schematic diagram representing a homotopy trajectory in this invention;

[0066] Figure 5 This is a schematic diagram illustrating how a sampling-based planner searches the time state space and finds multiple homotopic paths in this invention.

[0067] Figure 6 This is a schematic diagram of a local replanning maneuver for dynamic obstacles in this invention;

[0068] Figure 7 This is a schematic diagram demonstrating the scalability of multiple agents in this invention;

[0069] Figure 8This is a schematic diagram of the distance between intelligent agents in the intelligent agent simulation of this invention. Detailed Implementation

[0070] To more clearly illustrate the embodiments of the present invention, specific implementation methods will be described below with reference to the accompanying drawings. Obviously, the drawings described below are merely some embodiments of the present invention. For those skilled in the art, other drawings and other implementation methods can be obtained based on these drawings without any creative effort.

[0071] This invention provides a trajectory planning method based on motion camouflage reparameterization and homotopy-aware topology, comprising the following steps:

[0072] S1: Obtain information on the robot's starting point, ending point, and obstacles in the environment to model the trajectory planning problem;

[0073] S2, based on homotopy-aware topology, plans multiple initial paths with different topologies in the free space of the environment;

[0074] S3. For each initial path, a motion camouflage reparameterization technique is used to treat the initial path as a virtual prey path. Based on the selected reference point, the multi-dimensional waypoint position parameters on the path are mapped into a one-dimensional scalar control parameter sequence to construct a low-dimensional optimization problem.

[0075] S4, In the low-dimensional optimization problem, the one-dimensional scalar control parameter sequence and trajectory time segment are optimized to generate a smooth trajectory that satisfies the dynamic constraints and obstacle avoidance constraints;

[0076] S5 selects the trajectory with the lowest cost from the optimized trajectories as the final trajectory output.

[0077] like Figure 1 As shown, the framework of this invention consists of two main modules: global planning and local replanning.

[0078] MOCHA global planning utilizes a homotopy perception front-end (Apollonius / PRM and H-signature) to generate multiple candidate paths with distinct topologies. It then selects the lowest-cost trajectory through motion camouflage reparameterization and trajectory optimization. MOCHA local replanning incorporates time into the state space for spatiotemporal topology search, and then employs the same efficient motion camouflage reparameterization and optimization modules to achieve fast dynamic obstacle avoidance and a smooth return to the global path.

[0079] Specifically, the specific implementation process of this invention is as follows:

[0080] 1. Modeling the Trajectory Planning Problem

[0081] consider An intelligent agent in Workspace ( In a collaborative task execution environment containing static obstacles (represented as a closed set), the environment contains static obstacles. and time-varying dynamic obstacles The intelligent agent at any moment Available accessibility areas are represented as Each intelligent agent The goal is to generate a smooth, dynamically feasible, and collision-free trajectory.

[0082] The agents considered here are all differentially flat, defined as follows: Consider a general nonlinear control affine system: (1);

[0083] If a flat output exists and smooth mapping The system is differentially flat if the following conditions are met:

[0084] (2);

[0085] This invention selects a flat output as:

[0086] (3);

[0087] in Indicates the position of the center of mass. This is represented as the yaw angle. To solve the trajectory planning problem, for each agent... Optimize the translation and flatten the output vector This simplification is reasonable because of the yaw angle. The translational mechanics is largely decoupled from the problem and can be programmed independently, thus significantly reducing dimensionality. Following the principle of optimal control, this problem is formulated as a continuous-time constrained optimization problem, aiming to minimize the weighted sum of control effort and trajectory duration. The problem is formally described as follows:

[0088] (4);

[0089] here, Indicates until The stacked order is in a flat state. Boundary conditions. and A fixed initial state and a fixed terminal state are defined. It is the time regularization parameter. This is the total trajectory duration. (Function) This summarizes all continuous-time inequality constraints, which are the main source of problem complexity, including:

[0090] (1) Obstacle avoidance: The trajectory must always remain within the defined obstacle-free area. .

[0091] (2) Collision avoidance between agents: any pair of agents , Minimum safe distance must be maintained between them .

[0092] (3) Feasibility: The magnitude of velocity and acceleration is limited by the physical limits of the intelligent agent. and These are expressed as constraints on the derivative of the trajectory, i.e. and .

[0093] 2. MINCO trajectory representation

[0094] Based on differential flatness, this invention employs an efficient trajectory parameterization representation method (MINCO). For a given flat output... It defines the problem of minimizing a control effort functional of the following form:

[0095] (5);

[0096] And it was proven that within a given fixed time period, until Given the boundary derivatives of order 1 and specified intermediate nodes, there exists a unique optimal solution, which has the form: A piecewise polynomial of order 1.

[0097] This result achieves efficient spatiotemporal decoupling: Piecewise polynomials from intermediate waypoints and time period Parameterization, and polynomial coefficients It can be solved in linear complexity by solving a non-singular banded system. Direct construction:

[0098] (6);

[0099] Subsequently, by utilizing differential flatness to transform dynamic feasibility and obstacle avoidance conditions into a flat output space, the original problem involving complex constraints is solved, where the trajectory is... After parameterization, these constraints are addressed in the optimization within the parameter space, thus allowing the problem to be ultimately reconstructed into an unconstrained optimization through constraint elimination techniques.

[0100] 3. Motion camouflage further reduces dimensionality.

[0101] MINCO transforms trajectory optimization into targeting intermediate waypoints. and a period of time Sparse parameter optimization is possible. Nevertheless, as the problem size increases, the high dimensionality of these intermediate waypoints still imposes a significant computational burden. In particular, a large number of decision variables rapidly increases the non-convexity of the objective function, raising the likelihood of getting trapped in suboptimal local minima and causing computational delays, thus interfering with the real-time response capabilities required for planning.

[0102] To overcome this limitation and further accelerate computation, inspired by the principle of motion camouflage, this invention proposes a novel reparameterization method.

[0103] Motion camouflage is a natural strategy in which a predator appears stationary relative to a distant background as it approaches its prey. This visual deception is achieved by restricting the predator's movement along a specific path. Specifically, the predator's position is confined to a line connecting the prey and a chosen fixed reference point; this line is called the camouflage line. Figure 2 It demonstrates the dynamics of relative motion and the geometric principles behind this phenomenon.

[0104] Figure 2 In the middle, the light green line represents the virtual prey path ( The different cyan lines (predator 1, 2, 3) represent adjustments made to the one-dimensional control parameters. Different actual trajectories are generated. All points on the trajectory are located at the connecting reference point ( On the "camouflage line" corresponding to the point on the prey's trajectory.

[0105] Predator position Prey location and reference points The geometric relationship between them can be mathematically described as follows: (7)

[0106] in It is a one-dimensional path control parameter. The value of determines the predator's position on the camouflage line. Especially when When the predator and prey are in the same position, it means a successful interception. This is achieved by manipulating parameters. and The location can generate a wide variety of trajectories, which forms the theoretical basis for the dimensionality reduction method proposed in this invention.

[0107] The optimization-based algorithm relies on the initial path provided by the front-end planner. Therefore, this path is used as the virtual prey path. And by selecting an additional reference point , high-dimensional waypoints Dimensionality is reduced to a set of one-dimensional scalar parameters. Specifically, each intermediate node... Location Now given a scalar parameter definition: (8)

[0108] in It is the virtual prey path corresponding to the first The position of each node. Reference point. The choice of strategy, especially its direction, significantly affects the resulting state space, such as Figure 3 As shown, the location of the reference point is determined based on the right triangle formed by the lines connecting the start and end points, and the direction is chosen towards the side with lower obstacle density to enhance the flexibility of optimization.

[0109] Under this reparameterization method, the trajectory Characterized as On different segments A piecewise polynomial of order 1. The optimization process focuses on the prey path parameters (represented as...). ) and the duration of each segment (expressed as Polynomial coefficients The following equations can be directly constructed: (9);

[0110] No. The segment trajectory is represented as: (10);

[0111] in Describe a polynomial basis. It is the first The coefficient matrix of the segment. This reparameterization significantly reduces the number of decision variables and gives them an intuitive physical interpretation: This indicates the degree of deviation from the virtual prey path. Ultimately, a lower-dimensional, more structured optimization space was created, greatly enhancing scalability and real-time performance.

[0112] 4. Problem Refactoring

[0113] This invention aims to reconstruct the trajectory planning problem into an unconstrained nonlinear programming problem. This first requires using previously proposed methods to reconstruct the trajectory. Parameterization reduces the decision variables of the problem to a one-dimensional scalar sequence. and a period of time .

[0114] For the time parameter, each time segment By using piecewise smooth functions that guarantee positive definiteness From unconstrained variables Mapped from: (11);

[0115] For spatial parameters, each scalar From unconstrained variables using a scaled logistic function Mapped from this, the function restricts its value to a predefined range. Inside:

[0116] (12);

[0117] in It is the logistic function.

[0118] Subsequently, by sampling a fixed number of points within each trajectory segment, all continuous-time inequality constraints are... It is transformed into a penalty term and incorporated into the objective function. The final optimization problem is formulated as follows: (13);

[0119] in and It is a positive weight. Indicates effort to control. Total flight time ,and It includes dynamic feasibility constraints, static obstacle avoidance, and inter-agent collision avoidance. By solving this NLP (nonlinear programming) problem, smooth, dynamically feasible, and collision-free trajectories can be generated efficiently.

[0120] 5. Calculate the general cost function and gradient propagation

[0121] To solve the unconstrained nonlinear programming problem in equation (13), a gradient-based optimization method is employed. This requires efficient computation of the general cost functional. Regarding unconstrained variables The gradient.

[0122] The MINCO framework provides a way to implement arbitrary cost functionals gradient from Space mapping to space The method. The gradient of the result is represented as... and In the motion camouflage reparameterization method, the optimization variable is a scalar. Instead Therefore, an additional layer is inserted into this gradient chain. This is achieved through the formula: (14);

[0123] According to the chain rule, about The gradient is given by the following vector-Jacobi product (VJP): (15);

[0124] The gradient remains unchanged for piecewise time intervals. Finally, the optimization is performed on unconstrained variables. and This was done on the above. Applying the chain rule, we can obtain: (16);

[0125] in: (17);

[0126] This hierarchical gradient flow preserves structured parameterization while maintaining overall computational efficiency.

[0127] The following describes how to calculate the composition. The gradient of the penalty term. Integrating the penalized functional at any time. Both can be achieved by introducing a general penalty function. And sample in each segment Approximating with a point: (18);

[0128] in ,for have ,and Based on the piecewise polynomial formula, we can construct: (19);

[0129] If the constraint is independent of absolute time, then the parameter It can be omitted.

[0130] Next, consider Regarding polynomial coefficients and a period of time The gradient. Regarding... The gradient is given by the chain rule: (20);

[0131] in Polynomial basis can be used It is expressed as the derivative of time. Distinguish between two situations. If Independent of absolute time: (twenty one);

[0132] if If it depends on absolute time, then The disturbance will affect the current segment and all subsequent segments. Its absolute time perturbation gradient is: (twenty two);

[0133] This therefore generates additional gradient terms: (twenty three)

[0134] The final complete gradient is the sum of the two equations above.

[0135] 6. Design the specific cost function and gradient.

[0136] 1) Control energy terms : On each trajectory segment The order control energy (obtained by summing s=3, i.e., minimizing jerk) is: (twenty four);

[0137] The gradient is derived directly from the properties of quadratic forms and Leibniz's rule: (25);

[0138] 2) Obstacle Avoidance :consider There are several obstacles. To ensure collision-free motion, a penalty function is defined, which is activated when any obstacle encroaches on the safety margin of the trajectory: (26);

[0139] in It is a preset safe distance. From To the obstacle The minimum Euclidean distance squared.

[0140] 3) Dynamic feasibility To ensure that the trajectory adheres to the agent's physical constraints, taking velocity and acceleration as examples, the following penalties are introduced: (27);

[0141] 4) Collision avoidance between intelligent agents To achieve scalable multi-agent planning, a distributed asynchronous planning framework is adopted, which does not rely on centralized computing. In this architecture, each agent broadcasts its current trajectory as a "committed trajectory" to neighboring agents during execution. Other agents then use these received broadcast trajectories to plan their own collision avoidance.

[0142] Specifically, using a global time base and trajectory sampling offset time To calculate the position of other agents based on their committed trajectories Then, the following penalty is introduced: (28);

[0143] in Indicates except Any intelligent agent other than It is the safe distance threshold.

[0144] 5) Total Time A key consideration in balancing trajectory smoothness and aggression is minimizing the total time cost, defined as follows: (29);

[0145] 7. Global multihomopy topology

[0146] In addition to optimization, motion camouflage reparameterization also relies on virtual prey paths generated by the front-end planner. To mitigate the local minima problem in non-convex optimization, homotopy-aware topology is incorporated into the front-end planning, followed by parallel optimization of multiple prey paths to select the optimal solution.

[0147] like Figure 4 As shown, in a cluttered environment, two consecutive trajectories are homotopic if they can smoothly transform into each other without crossing obstacles or changing their endpoints; otherwise, they belong to different homotopy classes.

[0148] Having established the concept of homotopy, we now describe how MOCHA implements a homotopy-aware global front-end in practice. The process begins by constructing a sparse, searchable graph in free space using methods such as probabilistic path graphs (PRM) or skeletonization (e.g., Apollonius graphs). To filter redundant paths in this sparse graph, an H-signature is employed: a lightweight integer vector that encodes how a path navigates around each obstacle, thus serving as a topology discriminator.

[0149] Set path A series of vertices This indicates that each obstacle is represented by a point. This is represented as follows. For each obstacle, we calculate its corresponding integer number of revolutions. This represents the net counter-clockwise revolutions of the path around the obstacle. This is achieved by considering each path segment... Relative to obstacles It is calculated by summing the continuous angular changes: (30);

[0150] here, It is a path-continuous argument function that accumulates angular changes without a 2π jump. It is the nearest integer operator. The homotopy signature of the path. It is made by all A vector consisting of the independent revolutions of each obstacle: (31);

[0151] Therefore, H-signature provides a direct method for distinguishing homotopy classes. In 3D space, the essence of H-signature is to generalize the point basis abstraction of obstacles in 2D to represent each obstacle as a space curve, and determine whether two paths belong to different homotopy classes by evaluating the Gaussian link number of closed paths around this curve or based on the link number of line integrals. This provides a reasonable but incomplete homotopy distinction, following the practical strategy adopted in topology-guided dynamic programming.

[0152] Combining the above factors, the global planner proceeds in three steps: (i) constructing a sparse topology graph, and (ii) utilizing the H-signature at the starting point. and the end point Search between the maximum (iii) Then each representative path is used as a virtual prey path for downstream optimization.

[0153] 8. Dynamic obstacle avoidance triggering conditions

[0154] The agent obtains a high-quality, feasible global trajectory from the global planner. However, dynamic obstacles still exist in the environment. To ensure safe movement, a local replanning algorithm is needed. It is assumed that the agent can predict the trajectory of dynamic obstacles using sensors such as LiDAR and use these predictions in the algorithm.

[0155] For the sake of simplicity, a global time base is used. and time offset To calculate the global trajectory position of the agent Similarly, the first The predicted location of each dynamic obstacle is Subsequently, real-time checks are performed to ensure the trajectory remains within the look-ahead timeframe. Internal and any dynamic obstacles Conflict occurred:

[0156] ;

[0157] in This is a preset safety margin. If the conditions are met (i.e., predictions for the future)... If there is a risk of collision within a given time period, a local replanning is triggered. In fact, calculating distances over continuous time offsets is computationally expensive. We address this by using a time-range... Discretize the sampling time offset with a sufficiently high resolution. And evaluate the minimum distance at these sampling points to approximate this check.

[0158] 9. Penalties for Local Targets and Dynamic Obstacles

[0159] If the local planner is triggered, it will generate a new trajectory. The trajectory avoids predicted collisions while remaining as close as possible to the original global path. This problem is formulated as an optimization problem that reuses the efficient motion camouflage parameterization introduced earlier.

[0160] First, specify the boundary conditions for the optimization problem. The initial state is set at the trigger time of the local planner. The agent's current state, while the terminal state is "anchored" to the original global trajectory: (33);

[0161] in The time frame for local replanning is set to a value greater than the prediction window. This ensures sufficient time to smoothly bypass obstacles and rejoin the global path. The objective function structure for the local problem is similar to the global objective, but a penalty term is added and the time cost term is modified. The new penalty term... It is used for dynamic obstacle avoidance. Its form is similar to inter-agent collision avoidance, and it utilizes absolute time. To query the predicted position of a dynamic obstacle at each sampling point on its local trajectory. : (34);

[0162] in This refers to the number of relevant dynamic obstacles; the gradient calculation logic is the same as the previous absolute time term. To encourage the duration and time range of local trajectories... Tight matching, replacing simple linear time cost with the following soft constraints : (35);

[0163] about and The gradient is: (36);

[0164] Finally, a safety protocol is executed after local optimization convergence. A maximum allowed time window is defined. If the duration of the local trajectory exceeds this window: (37);

[0165] The agent will still execute the calculated local trajectory. This ensures dynamic collision avoidance. However, during the execution of this local trajectory, the global planner will be triggered asynchronously.

[0166] 10. Locally Dynamically Homotopic Topology

[0167] The local optimizer also relies on virtual prey paths, but unlike the global planner, it must handle the topology in the presence of dynamic obstacles. This requires operation in a spatiotemporal domain rather than a purely spatial domain. To address this, the approach is built on a topology-driven framework that represents moving obstacles as static objects by including time as an additional dimension in an enhanced state space.

[0168] For a planar workspace ( The state space is ,in This is the local replanning horizon. The predicted motion of each dynamic obstacle is lifted into this space, becoming a static "cylinder" along the time axis. Sample-based planners such as PRM search for multiple homotopic distinct paths between the current state and the local objective, constructed as follows: Figure 5 As shown.

[0169] Figure 5 In this context, dynamic obstacles are represented as static "cylinders" in a 2D+time state space. A sample-based planner searches this space and finds multiple homotopic distinct paths (e.g., 1, 2, 3, 4).

[0170] The same method can be naturally extended to 3D workspaces. For The spatiotemporal state space is defined as: (38);

[0171] And each predicted obstacle trajectory is elevated to a 4D "super pipe". Using PRM to plan feasible paths in this space is intuitive. The main difficulty lies in distinguishing homotopy classes: computing a strict H-signature in 4D is extremely expensive and difficult to deploy in real-time local planners.

[0172] To obtain lightweight yet information-rich topological labels, this invention employs a projection-based approximation method. Specifically, both the PRM path and obstacle hypervisors are projected onto three 3D spatiotemporal subspaces: , and In each projection space, the H-signature is computed and candidate paths are compared in pairs. If any of the three projections results in two paths producing different H-signatures, they are considered to belong to different homotopy classes in the original 4D space. Paths with consistent projections in all three subspaces are considered topologically indistinguishable under this method.

[0173] This projection is used as a conservative real-time separator: it aims to reliably detect obvious topological differences while avoiding costly 4D topological differentiation. Ambiguous cases are conservatively handled by preserving or merging seeds, subsequently validated during continuous-time optimization phases through gap and dynamic constraint checks. In practice, this strategy provides sufficient topological diversity for multi-homogeneous local optimization while maintaining negligible additional computational overhead.

[0174] Proposition: [Reliability of 3D Projection] Let... .obstacle Free space A causal path is a continuous mapping that is not decreasing in time. Let the coordinate projection be... The free space after projection is In each Above, calculate an H-signature It can separate the endpoint-fixed homotopy classes within the traversal region. For two causal paths with the same endpoint... If it exists Make: (39)

[0175] but and exist They are not the same as each other.

[0176] Proof: Projection A mapping is induced on homotopy classes with fixed endpoints. If and exist Zhong Tonglun, then and exist The junctured parts will also be homotopic; therefore, their H-signatures will overlap.

[0177] Note: In the application, obstacle shadows are defined as... This is conservative, when the curve being compared is... gap The proposition remains reliable at that time. The sufficient sampling rule in time: if spatial motion is... -Lipschitz's step size selection Make .

[0178] In addition, this invention also provides a trajectory planning system based on motion camouflage reparameterization and homotopy-aware topology, including:

[0179] The information acquisition and modeling module is used to acquire information about the robot's starting point, ending point, and obstacles in the environment, and to model the trajectory planning problem.

[0180] The path planning module is used to plan multiple initial paths with different topologies in the free space of the environment based on homotopy-aware topology.

[0181] The path optimization module is used to employ motion camouflage reparameterization technology for each initial path, treating the initial path as a virtual prey path, and mapping the multi-dimensional waypoint position parameters on the path to a one-dimensional scalar control parameter sequence based on selected reference points, in order to construct a low-dimensional optimization problem.

[0182] The parameter optimization and trajectory generation module is used to optimize the one-dimensional scalar control parameter sequence and trajectory time segment in low-dimensional optimization problems, and generate a smooth trajectory that satisfies dynamic constraints and obstacle avoidance constraints.

[0183] The output selection module is used to select the trajectory with the lowest cost from multiple optimized trajectories as the final trajectory output.

[0184] To verify the technical effectiveness of this invention, a numerical comparison was conducted between the MOCHA and MINCO algorithms using MATLAB. The experiments were carried out in two typical static environments: a two-dimensional space with circular obstacles and a three-dimensional forest environment with cylindrical obstacles.

[0185] In the experiment, the trajectory passed through a segment of length. The uniform discretization process, each segment contains Each sampling point. Optimization parameter settings are as follows: smoothness or energy component weights. Time regularization weights Obstacle avoidance penalty weight Dynamic feasibility penalty weight Both MOCHA and MINCO are set with the same dynamic constraints to maintain maximum speed. and maximum acceleration All optimizations were performed using the general-purpose optimizer fminunc.

[0186] The optimized quantitative results are shown in Table 1 below:

[0187] Table 1 Comparison of Dimensionality Reduction Effects

[0188] In 2D scenarios, the MOCHA algorithm reduces planning time while maintaining almost the same trajectory quality. The computational advantage is even more pronounced in 3D scenes, and the method of this invention achieves... The planning time is reduced. It is worth noting that in a 3D environment, MOCHA's trajectory quality ( Slightly lower than MINCO ( Specifically, MOCHA produces slightly longer trajectory lengths (increased the length of the trajectory). And the arrival time is relatively slow (increased) This is a reasonable trade-off for its significantly reduced optimized runtime.

[0189] This anticipated trade-off involves compressing the massive multidimensional waypoint search domain into a single dimension. The method of this invention suffers only a small loss in trajectory optimality but significantly improves computational efficiency. As subsequent experiments demonstrate, the quality of these trajectories far surpasses that of other state-of-the-art (SOTA) algorithms. Therefore, the experimental results confirm the effectiveness of the dimensionality reduction achieved by the algorithm of this invention.

[0190] 1. Trajectory planning speed effect

[0191] The complete system was integrated into the ROS 2 Humble environment for pre-validation testing. In the 2D global planning domain, the front-end topologist performs H-signature topology evaluation on a skeleton graph derived from the Apollonius graph, greatly accelerating trajectory construction. For local replanning, a probabilistic road graph (PRM) is used for front-end topology. To evaluate system performance under near-real-world conditions, this invention was benchmarked against two other state-of-the-art (SOTA) planners, TRR and SST.

[0192] This invention underwent extensive comparative analysis: TRR, similar to the technology of this invention, represents an iterative spatiotemporal optimization algorithm. The experiment was conducted in an environment filled with numerous obstacles. During this simulation, the robot's radius was set to 0.3m, and the dynamic constraints were defined as follows: and Three different target locations were specified to comprehensively evaluate the performance level.

[0193] The specific results are shown in Table 2 below:

[0194] Table 2. Quantitative Comparison Data with the State-of-the-Art (SOTA) Planner for Three Different Objectives

[0195] MOCHA's planning time With a total flight time of 23-29 ms, it is extremely efficient, proven to be up to 8.8 times faster than TRR (48-229 ms), and consistently an order of magnitude (36-51 times) faster than SST (1059-1506 ms). This computational speed is attributed to motion camouflage reparameterization. MOCHA achieves the shortest total flight time in all scenarios. It completes tasks 37-39% faster than TRR and 34-46% faster than SST. This is achieved by adhering to... Limit and maintain competitive path lengths At the same time, it generates the highest average speed (average Compared to TRR It is achieved through a radical trajectory.

[0196] 2. Dynamic obstacle avoidance effect

[0197] In addition to static benchmark tests, MOCHA's dynamic obstacle local replanning capability was further evaluated, such as... Figure 6 As shown. In this scenario, the agent designs a global trajectory—represented by the blue dashed line—and begins executing it. During navigation, the agent utilizes... The predictive and forward-looking scope continuously monitors potential conflicts with dynamic obstacles to ensure safe passage.

[0198] Figure 6 Local replanning maneuvers for dynamic obstacles. When an agent detects a conflict with an obstacle while following its global path, it triggers the local planner to generate a new avoidance trajectory and reintegrate it into the global path at future anchor points.

[0199] Figure 6 As explained, the local replanning mechanism is activated when a potential collision with a dynamic obstacle (circled in orange in the diagram) is detected. It calculates a new, smooth avoidance path (demonstrated by the solid red line) that successfully and safely bypasses the obstacle. This locally generated trajectory is optimized to rejoin the pre-established global route at a designated "anchor point" (represented by the red square), which is offset from the global path in future time from the replanning initiation point. Place.

[0200] Replanning ensures that the agent always maintains a safe separation from all obstacles (whether static or dynamic), ensuring that the gap never falls below the defined safety threshold.

[0201] 3. Multi-agent trajectory planning performance

[0202] The scalability of the MOCHA framework is then demonstrated by simulating five agents navigating to different goals in the same cluttered environment. Specifically, the agents are placed at nearby starting coordinates—(0,0), (2,0), (0,2), (4,0), and (0,4)—and assigned coordinates such as... Figure 7 The target location is randomly assigned. Five agents starting from nearby locations successfully generated safe and efficient trajectories to different targets in a cluttered environment using a distributed asynchronous planning method.

[0203] Figure 8 The effectiveness of collision avoidance penalties between agents was demonstrated. Figure 8 The dashed lines in the text represent the settings. Safe distance, including in Additional space above the minimum interval (twice the robot radius) A safety margin is provided to mitigate potential communication and planning delays. The diagram confirms that the distance between all agents remains safely maintained throughout the movement. Above the threshold.

[0204] Figure 8 The chart shows the change in the minimum distance between all agent pairs over time. All agents adhere to the minimum safe distance (red dashed line), demonstrating the effectiveness of the collision avoidance penalty between agents.

[0205] Table 3 below further details the performance metrics of this test:

[0206] Table 3 Performance index data of the scalability test of 5 agents

[0207] In this table, This represents the number of initial homotopic paths found by the front-end planner, while It is parallel optimization of all of these The total time required for each path is calculated. Results show that the optimization time for any single trajectory is extremely low (e.g., for agent (0,0), six paths were optimized in parallel in just 0.027 s). This multi-homogeneity approach, combined with parallel optimization, effectively mitigates the problem of the optimizer getting trapped in local minima. This advantage is particularly pronounced in scenarios where agents' starting points and ending points are closely clustered. Traditional single-topology optimization algorithms struggle to generate such diverse, high-quality, and collision-free trajectories. Data confirm MOCHA's real-time performance and scalability.

[0208] 4. Real-world deployment effect experiment

[0209] To verify the practical applicability and robustness of the MOCHA framework, a real-world experiment was conducted using two ground vehicles in a cluttered indoor environment.

[0210] The experimental setup was a long corridor with 12 cylindrical obstacles ranging from 36 to 44 centimeters in radius. The MOCHA algorithm was deployed in a distributed manner on two vehicles equipped with LiDAR for indoor localization and real-time location acquisition. The agents broadcast their promised trajectories to each other using UDP.

[0211] In the described scenario, two agents (Agent 1 and Agent 2) start from nearby initial positions. and Let's begin. Their task is to navigate to two different, randomly chosen target points at the end of a corridor. The two agents first perform multi-homotopic path planning to find several topologically distinct feasible routes.

[0212] Agent 1 first plans and finds multiple paths, selecting the lowest-cost one (cost 9.7) and executing it. It commits to this trajectory and broadcasts it. Subsequently, Agent 2 executes its optimization. Agent 2 also finds multiple paths. However, when calculating its cost, it uses a collision penalty between agents. The commitment trajectory of Agent 1 was incorporated. This significantly increased the cost of the two paths (one at 15.1 and the other at 14.9), which would conflict with Agent 1. As a result, Agent 2 chose the path with a cost of 10.3, which offered a higher safety margin and is now the optimal choice.

[0213] Agent 1 travels along its chosen path, while Agent 2 executes a path with a safety cost of 10.3. Both vehicles successfully traverse a dense obstacle course while remaining safely separated, ultimately reaching their respective destinations. This experiment demonstrates the effectiveness of a distributed homotopy perception framework in safely and efficiently coordinating multiple agents in real-world cluttered environments.

[0214] This invention proposes a novel reparameterization technique inspired by motion camouflage; it transforms the complex waypoint optimization problem into a task of optimizing a scalar sequence along a trajectory; this method maintains efficient gradient flow while preserving geometric interpretability; this invention proposes a comprehensive homotopy-aware planning framework designed to solve dynamic obstacle and multi-agent coordination problems; this framework integrates a lightweight, homotopy-aware front-end for generating various topology candidate paths, and a structure-preserving back-end optimization to support general continuous-time constraints; furthermore, this invention extends to distributed multi-agent environments through an asynchronous trajectory commitment process; this invention proposes a spatiotemporal local replanning module that maintains only a short sliding time window to avoid anticipated dynamic obstacles and reconnect to the global trajectory, reducing solution time and latency while maintaining feasibility and consistency with the global plan.

[0215] The above description is merely a detailed explanation of preferred embodiments and principles of the present invention. For those skilled in the art, there may be changes in specific implementation methods based on the ideas provided by the present invention, and these changes should also be considered within the scope of protection of the present invention.

Claims

1. A trajectory planning method based on motion camouflage reparameterization and homotopy-aware topology, characterized in that, Includes the following steps: S1: Obtain information on the robot's starting point, ending point, and obstacles in the environment to model the trajectory planning problem; S2, based on homotopy-aware topology, plans multiple initial paths with different topologies in the free space of the environment; S3. For each initial path, a motion camouflage reparameterization technique is used to treat the initial path as a virtual prey path. Based on the selected reference point, the multi-dimensional waypoint position parameters on the path are mapped into a one-dimensional scalar control parameter sequence to construct a low-dimensional optimization problem. S4, In the low-dimensional optimization problem, the one-dimensional scalar control parameter sequence and trajectory time segment are optimized to generate a smooth trajectory that satisfies the dynamic constraints and obstacle avoidance constraints; S5 selects the trajectory with the lowest cost from the optimized trajectories as the final trajectory output.

2. The trajectory planning method based on motion camouflage reparameterization and homotopy sensing topology according to claim 1, characterized in that, Step S1 includes the following steps: S11, where R represents the Euclidean geometric space composed of real numbers. An intelligent agent in Workspace , Collaborative task execution in an environment containing static obstacles. and time-varying dynamic obstacles The intelligent agent at any time Available accessibility areas are represented as Each intelligent agent The goal is to generate a smooth, dynamically feasible, and collision-free trajectory; S12, defines that each agent is differentially flat, as follows: For each intelligent agent Optimize the translation and flatten the output vector ; Let the trajectory planning problem be formulated as a continuous-time constrained optimization problem; let... For the joint cost function, the problem is formally defined as follows: ； in, Indicates until The stacking of order flat state; boundary conditions and A fixed initial state and a fixed terminal state are defined. It is the time regularization parameter. It is the total trajectory duration.

3. The trajectory planning method based on motion camouflage reparameterization and homotopy sensing topology according to claim 2, characterized in that, Step S2 includes the following steps: S21, Construct a sparse topological graph of the free space using a probabilistic roadmap or skeletonization method; S22, using the homotopy signature H-signature as a topological discriminator, multiple paths with different homotopy categories between the start and end points in the sparse topological graph are searched as the initial paths. The specific process is as follows: Set path A series of vertices This indicates that each obstacle is represented by a point. This indicates that for each obstacle, the corresponding integer number of revolutions is calculated. , used to represent the net counterclockwise revolutions of a path around an obstacle; This represents the total cumulative rotation angle of the path around the obstacle, where K is the total number of path segments after discretization. Specifically, this is achieved by analyzing each path segment... Relative to obstacles The calculation is performed by summing the continuous angular changes: ； in, It is a path continuous argument function that accumulates angle changes without a 2π jump; It is the nearest integer operator; the homotopy signature of the path. It is made by all A vector consisting of the independent rotation numbers of each obstacle: 。 4. The trajectory planning method based on motion camouflage reparameterization and homotopy sensing topology according to claim 3, characterized in that, Step S3 includes the following steps: S31, based on the motion camouflage strategy, the initial path is used as the virtual prey path. And by selecting an additional reference point , high-dimensional waypoints Dimensionality reduced to a set of one-dimensional scalar parameters; each intermediate node Location Now given a scalar parameter The definition and specific formula are as follows: ； in, It is the virtual prey path corresponding to the first The position of each node; S32, Set Track Characterized as On different segments Piecewise polynomials of order [order missing]; the optimization process focuses on prey path parameters and the duration of each segment; polynomial coefficients. It can be directly constructed by solving the following equations, where It is a strip matrix. This is the state constraint vector. ； Defined as the first Segment trajectory: ； in Describe a polynomial basis. It is the first The coefficient matrix of the segment; Indicates the degree of deviation from the virtual prey's path; The time interval representing the trajectory.

5. The trajectory planning method based on motion camouflage reparameterization and homotopy sensing topology according to claim 4, characterized in that, Step S4 includes the following steps: S41, for the time parameter, set the time for each segment. By using piecewise smooth functions that guarantee positive definiteness From unconstrained variables This is derived from a mapping, and the specific formula is as follows: ； For spatial parameters, each scalar is defined. From unconstrained variables using a scaled logistic function Mapped from this, the function restricts its value to a predefined range. The specific formula is as follows: ； in, It is the logistic function; S42, by sampling a fixed number of points within each trajectory segment, all continuous-time inequality constraints are... It is transformed into a penalty term and included in the objective function, using Let represent the total cost function. The final optimization problem is expressed as: ； in, and It is a positive weight. Indicates effort to control. Total flight time , It includes dynamic feasibility constraints, static obstacle avoidance, and inter-agent collision avoidance; and All are unconstrained variables. S43. By solving the formula in step S42, a smooth, dynamically feasible, and collision-free trajectory is obtained.

6. The trajectory planning method based on motion camouflage reparameterization and homotopy sensing topology according to claim 5, characterized in that, Step S5 also includes a local replanning step; the local replanning step includes the following steps: S51, during the execution of the final trajectory, the predicted distance to the dynamic obstacle is monitored in real time; If a collision risk exists within a future forward-looking time window, a local replanning will be triggered; The local replanning includes: By adding time as an additional dimension, a spatiotemporal state space is constructed, and the predicted trajectory of dynamic obstacles is represented as a static obstacle in the spatiotemporal state space. In the spatiotemporal state space, multiple local initial paths are planned from the current state to the anchor point on the global trajectory at a certain future moment; The motion camouflage reparameterization technique is used to optimize each local initial path to generate a local obstacle avoidance trajectory.

7. The trajectory planning method based on motion camouflage reparameterization and homotopy sensing topology according to claim 6, characterized in that, In step S51, when distinguishing the homotopy categories of paths in the spatiotemporal state space, a projection-based approximation method is used; the projection-based approximation method includes the following steps: The path and obstacle representations in the spatiotemporal state space are projected onto multiple low-dimensional subspaces; Calculate the homotopy signature H-signature of the projected path in each low-dimensional subspace; If the homotopy signatures (H-signatures) of two paths on any low-dimensional subspace are different, then the original paths are determined to have different topologies in the spatiotemporal state space.

8. A trajectory planning system based on motion camouflage reparameterization and homotopy-aware topology, used to implement the trajectory planning method based on motion camouflage reparameterization and homotopy-aware topology as described in any one of claims 1-7, characterized in that, The trajectory planning system based on motion camouflage reparameterization and homotopy-aware topology includes: The information acquisition and modeling module is used to acquire information about the robot's starting point, ending point, and obstacles in the environment, and to model the trajectory planning problem. The path planning module is used to plan multiple initial paths with different topologies in the free space of the environment based on homotopy-aware topology. The path optimization module is used to employ motion camouflage reparameterization technology for each initial path, treating the initial path as a virtual prey path, and mapping the multi-dimensional waypoint position parameters on the path to a one-dimensional scalar control parameter sequence based on selected reference points, in order to construct a low-dimensional optimization problem. The parameter optimization and trajectory generation module is used to optimize the one-dimensional scalar control parameter sequence and trajectory time segment in low-dimensional optimization problems, and generate a smooth trajectory that satisfies dynamic constraints and obstacle avoidance constraints. The output selection module is used to select the trajectory with the lowest cost from multiple optimized trajectories as the final trajectory output.

Images