A motion planning method for air-ground amphibious robot

Abstract

The application relates to the field of robot trajectory planning, and specifically discloses a motion planning method for an amphibious robot, which, after obtaining a global navigation task, first generates a discrete path by using three-dimensional jump point search, and reconstructs the discrete path into an independent state representation of height-arc length-yaw angle. By performing Cartesian coordinate mapping based on numerical integration on the state parameters, a kinematic model that naturally satisfies ground differential constraints is constructed, completely eliminating the yaw angle singular point and reverse driving restriction of traditional methods, and realizing smooth maneuvering in the full speed domain. Further, a multi-constraint optimization model containing aerodynamic resistance compensation and attitude inverse solution verification is used to iteratively shape the trajectory, and the attitude coupling characteristics of the underactuated flight system and environmental resistance are explicitly taken into account, so that the propeller grounding risk under near-ground large maneuvering and high-speed aerodynamic lag are effectively avoided while ensuring obstacle avoidance safety, and finally an executable trajectory that is continuous in time and space and strictly conforms to the physical safety boundary is generated.

Description

Technical Field

[0001] This application relates to the field of robot trajectory planning, and more specifically, to a motion planning method for an amphibious robot. Background Technology

[0002] Amphibious robots, as a novel cross-media platform combining the high maneuverability of rotorcraft with the long endurance of ground-based mobile robots, have demonstrated enormous application potential in disaster relief, complex environment inspection, and military reconnaissance. Compared to single-modal robots, amphibious robots can traverse rugged terrain and overcome obstacles like drones, while also efficiently navigating flat ground using their wheeled structures to save energy. However, to achieve efficient and safe autonomous navigation, it is particularly urgent to develop a motion planning scheme that seamlessly connects aerial flight and ground travel, fully adapting to the drastically different dynamic characteristics of the two modes. Especially in the ground motion mode, robots are typically subject to nonholonomic constraints (i.e., differential constraints) with no sideslip on the wheels, while the aerial mode involves high-dimensional degree-of-freedom control. Solving these two complex kinematic and dynamic planning problems simultaneously within a unified framework is a key challenge for achieving fully autonomous operation of this type of robot.

[0003] Existing motion planning schemes for amphibious robots primarily address modal switching and pathfinding issues, but they still have many limitations. For example, while some hybrid A-type algorithms introduce linear or differential models as motion primitives, in the back-end trajectory optimization stage, they typically treat nonholonomic constraints of ground travel as soft constraints (e.g., by penalizing trajectory curvature). This approach makes it difficult to strictly guarantee the physical feasibility of the generated trajectory when the initial value quality is poor, leading to significant tracking errors during robot execution. Another type of method based on nonlinear model predictive control can handle dynamic constraints better, but its computational load is enormous, making it difficult to meet the real-time planning requirements of onboard computers. More critically, existing technologies mostly use the traditional Cartesian coordinate system to parameterize position. This method has inherent flaws when describing differential motion on the ground: since the robot's yaw angle is usually passively calculated from the direction of the velocity vector, when the robot's velocity approaches zero or when reversing (changing the velocity direction), the yaw angle will exhibit numerical singularities or discontinuous quadrant jumps. This forces the planner to often restrict the robot to unidirectional forward movement, greatly limiting its maneuverability in confined spaces.

[0004] Therefore, an optimized motion planning scheme for air-ground amphibious robots is desired. Summary of the Invention

[0005] This invention application provides a motion planning method for an amphibious robot.

[0006] The technical solution of the present invention is as follows:

[0007] According to one aspect of this application, a motion planning method for an amphibious robot is provided, comprising:

[0008] S1: Receive global grid map and navigation task information;

[0009] S2: Based on the global grid map, the starting point and ending point in the navigation task information are searched in three-dimensional jump point in discretized three-dimensional space to obtain a set of collision-free discrete path points;

[0010] S3: Based on the preset time interval parameter, perform trajectory initialization and state space transformation preprocessing on the set of collision-free discrete path points to obtain the initial trajectory parameter set;

[0011] S4: Perform coordinate mapping and trajectory construction based on numerical integration on the initial trajectory parameter set to obtain the Cartesian full-state trajectory;

[0012] S5: Map the Cartesian full-state trajectory to a pre-built multi-constraint optimization model to obtain the objective function value and the gradient vector with respect to the optimization variables;

[0013] S6: Perform nonlinear iterative solution and trajectory generation on the objective function value and gradient vector to obtain the final spatiotemporal continuous trajectory, which is used to control the execution of the air-ground amphibious robot.

[0014] Compared with existing technologies, this application provides a motion planning method for an amphibious robot. After acquiring the global navigation task, it first generates a discrete path using a three-dimensional jump point search and reconstructs it into an independent state representation of height, arc length, and yaw angle. By performing a Cartesian coordinate mapping based on numerical integration on these state parameters, a kinematic model that naturally satisfies ground difference constraints is constructed, completely eliminating the yaw angle singularities and reversing limitations of traditional methods, and achieving smooth maneuvering across the entire speed domain. Furthermore, a multi-constraint optimization model including aerodynamic drag compensation and attitude inverse kinematics verification is used to iteratively shape the trajectory. This process explicitly considers the attitude coupling characteristics of the underactuated flight system and environmental drag, ensuring obstacle avoidance safety while effectively avoiding the risk of propeller ground contact and high-speed aerodynamic lag during near-ground large maneuvers, ultimately generating an executable trajectory that is spatiotemporally continuous and strictly conforms to physical safety boundaries. Attached Figure Description

[0015] The above and other objects, features, and advantages of this application will become more apparent from the more detailed description of the embodiments of this application in conjunction with the accompanying drawings. The drawings are provided to further illustrate the embodiments of this application and form part of the specification. They are used together with the embodiments of this application to explain this application and do not constitute a limitation thereof. In the drawings, the same reference numerals generally represent the same components or steps.

[0016] Figure 1 This is a flowchart of a motion planning method for an amphibious robot according to an embodiment of this application;

[0017] Figure 2 This is a data flow diagram illustrating the motion planning method for an amphibious robot according to an embodiment of this application;

[0018] Figure 3 This is a flowchart of step S4 of the motion planning method for an amphibious robot according to an embodiment of this application;

[0019] Figure 4 This is a flowchart of step S43 of the motion planning method for an amphibious robot according to an embodiment of this application;

[0020] Figure 5 This is a flowchart of step S5 of the motion planning method for an amphibious robot according to an embodiment of this application;

[0021] Figure 6 This is a schematic diagram comparing the processing steps of front-end path search, equal-time resampling, and initial trajectory generation according to an embodiment of this application;

[0022] Figure 7 This is a schematic diagram of a Cartesian space trajectory including reversing maneuvers generated by an amphibious robot according to an embodiment of this application under a specific target pose constraint.

[0023] Figure 8 This is a schematic diagram showing the change of the robot's yaw angle over time during the reversing maneuvering process according to an embodiment of this application. Detailed Implementation

[0024] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0025] To address the motion planning problems of existing amphibious robots, such as singular yaw angles and inability to reverse due to improper handling of the lack of ground sideslip constraints, as well as the attitude and dynamic safety hazards caused by neglecting the underactuated characteristics of flight, this application proposes a motion planning method for amphibious robots. This method first performs a 3D jump point search based on a global grid map to quickly solve for the initial collision-free path discrete point set. Then, abandoning the traditional Cartesian direct parameterization method, it reconstructs the path into an independent state space of height-arc length-yaw angle, and establishes its mapping relationship to the Cartesian physical space through integral transformation, thereby constructing a motion model that naturally adapts to differential constraints and supports bidirectional movement. Based on this, a nonlinear optimization function incorporating aerodynamic drag models and attitude inverse constraints is constructed, and the optimal trajectory parameters are solved numerically and iteratively. This accurately avoids obstacles while explicitly verifying and limiting near-ground flight attitude, ensuring that the generated spatiotemporally continuous trajectory eliminates kinematic singularities and strictly conforms to the physical safety boundaries of cross-medium motion.

[0026] Figure 1 This is a flowchart of a motion planning method for an amphibious robot according to an embodiment of this application. Figure 2 This is a data flow diagram illustrating the motion planning method for an amphibious robot according to an embodiment of this application. For example... Figure 1 and Figure 2 As shown, the motion planning method for an amphibious robot according to an embodiment of this application includes: S1, receiving a global grid map and navigation task information; S2, based on the global grid map, performing a three-dimensional jump point search in a discretized three-dimensional space on the starting point and ending point in the navigation task information to obtain a set of collision-free discrete path points; S3, based on a preset time interval parameter, performing trajectory initialization and state space transformation preprocessing on the set of collision-free discrete path points to obtain an initial trajectory parameter set; S4, performing coordinate mapping and trajectory construction based on numerical integration on the initial trajectory parameter set to obtain a Cartesian full-state trajectory; S5, mapping the Cartesian full-state trajectory to a pre-constructed multi-constraint optimization model to obtain the objective function value and the gradient vector for the optimization variables; S6, performing nonlinear iterative solution and trajectory generation on the objective function value and gradient vector to obtain a final spatiotemporal continuous trajectory, which is used to control the execution of the amphibious robot.

[0027] Specifically, in step S1, a global grid map and navigation task information are received. It should be understood that since the autonomous operation of an amphibious robot relies on accurate perception of the complex topology of the environment and a clear definition of the task boundary conditions, without effective environmental representation data and start / end point constraints, the planning system will be unable to construct a safe search space and generate a reachable motion trajectory. Therefore, in the technical solution of this application, by receiving a global grid map and navigation task information, the operating space range, obstacle distribution characteristics, and start / end point state constraints of the motion planning algorithm are clarified. This ensures that the subsequent jump point search and trajectory optimization processes are carried out within the framework of known environmental constraints, guaranteeing that the generated trajectory can effectively avoid obstacles and accurately guide the robot from its current state to the preset target position.

[0028] More specifically, in a concrete example of this application, the loading of perception data and task instructions is achieved by establishing a standardized data communication interface and parsing process. First, a global raster map is subscribed to and retrieved from an upstream mapping system or map server using a preset communication protocol. This map discretizes the three-dimensional physical space into fixed-resolution raster cells based on an octree or voxel structure, and each raster cell stores a probability value representing the space occupancy status, thereby constructing a digital environment model. Second, the system receives navigation task information through a human-computer interaction interface or automatic scheduling instructions. This process involves parsing and extracting initial state data, including three-dimensional position coordinates, body attitude quaternions, and current velocity, as well as target state data, including the target's three-dimensional coordinates and the desired endpoint attitude. Finally, the system performs coordinate system alignment and data validity verification operations, transforming the extracted starting and target positions from the world coordinate system to the raster map's index coordinate system, and verifying whether the start and end points fall within the map's free space. After confirming the data's validity, it is encapsulated as the planner's initialization input parameters.

[0029] Specifically, in step S2, based on the global grid map, the starting point and ending point in the navigation task information are searched for three-dimensional jump points in a discretized three-dimensional space to obtain a set of collision-free discrete path points. It should be understood that since direct continuous trajectory optimization in a large-scale complex three-dimensional environment is prone to getting trapped in local minima and incurs huge computational costs, without a geometrically instantaneously collision-free prior path as topological guidance, the subsequent nonlinear optimization solver struggles to converge quickly to a feasible solution. Therefore, in the technical solution of this application, based on the global grid map, the starting point and ending point in the navigation task information are further searched for three-dimensional jump points in a discretized three-dimensional space to obtain a set of collision-free discrete path points, thereby quickly constructing an unobstructed geometric channel connecting the starting and ending positions at the discretized grid level. This provides high-quality geometric initial values ​​for subsequent trajectory parameterization and dynamic optimization, significantly reducing the difficulty of solving the optimization problem and ensuring path safety.

[0030] More specifically, in this embodiment, step S2 includes: mapping the starting position and target position in the navigation task information to a grid index based on the resolution parameters of the global grid map, and initializing a priority open list containing the starting node; extracting the node with the minimum cost value from the priority open list as the current expansion node, removing non-natural neighbors according to the three-dimensional jump point pruning rules, and performing ray detection in the retained direction to identify the jump point node; calculating the total comprehensive cost of the jump point node based on the movement cost increment and heuristic cost, recording the parent-child mapping relationship between the jump point node and the current expansion node until the target node is searched; and recursively backtracking from the target node to the starting node based on the parent-child mapping relationship to obtain a set of collision-free discrete path points.

[0031] Accordingly, based on the resolution parameters of the global grid map, the starting and target positions in the navigation task information are mapped to grid indices, and a priority open list containing the starting node is initialized. It should be understood that since computers cannot directly execute graph-based node traversal and path search algorithms in a continuous and infinite physical coordinate space, and the heuristic search process must rely on discretized environmental state identifiers and a defined initial search seed to initiate the iterative logic, the technical solution of this application maps the starting and target positions in the navigation task information to grid indices based on the resolution parameters of the global grid map, and initializes a priority open list containing the starting node. This transforms continuous physical space constraints into computable states in a discrete grid space and constructs a standardized data structure for managing the leading edge nodes of the search. This ensures that subsequent jump point search algorithms perform mathematical operations within a unified and finite index space and accurately initiate the path planning process with the preset starting point as the root node.

[0032] In a specific example of this application, a coordinate discretization calculation and priority queue construction procedure based on linear transformation is executed. First, the planning system parses the metadata information of the global raster map, extracting the resolution parameter defining the physical size of the unit raster and the world coordinate origin vector of the map. Second, the system processes the floating-point starting and target position coordinates in the navigation task information according to the principle of linear mapping, calculating the difference between the physical coordinate vector and the map origin vector. Each component of this difference vector is then divided by the resolution parameter and rounded down to obtain the corresponding three-dimensional integer raster index, which uniquely identifies the starting and ending points in discrete space. Third, the system instantiates a starting node object based on the calculated starting raster index, initializes the cumulative movement cost of the starting node to zero, and calculates the straight-line distance between the node and the target position's raster index using the Euclidean distance formula, assigning it as the initial heuristic cost. Finally, the system allocates and initializes a priority open list based on a min-heap data structure in memory space, inserts the encapsulated starting node into this list, and completes the state initialization work before the algorithm starts.

[0033] Accordingly, the node with the minimum cost value is extracted from the priority open list as the current expansion node. Non-natural neighbors are removed according to the 3D jump point pruning rules, and ray detection is performed in the retained direction to identify jump point nodes. It should be understood that traditional 3D grid path search algorithms, if they simply traverse and evaluate all adjacent grids indiscriminately, will generate a massive number of intermediate redundant nodes in open, unobstructed areas, resulting in inefficient consumption of computational resources and extremely slow search convergence. Therefore, in the technical solution of this application, the node with the minimum cost value is further extracted from the priority open list as the current expansion node. Non-natural neighbors are removed according to the 3D jump point pruning rules, and ray detection is performed in the retained direction to identify jump point nodes. This filters out key turning points that are crucial to path construction and uses a straight-line scanning mechanism to skip large areas of free space. This significantly reduces the scale of node expansion during the search process, maintaining path optimality while significantly improving path planning efficiency in complex 3D environments.

[0034] In a specific example of this application, a heuristic node expansion and spatial scanning process based on a directional pruning strategy is executed. First, the system accesses a priority open list maintained in memory, uses a min-heap algorithm to find and pop the node object with the lowest total cost, establishes it as the current expansion node, and marks it as visited. Second, the system reads the relative displacement vector between the current expansion node and its parent node in the grid coordinate system to determine the entry direction. Then, it applies preset 3D jump point pruning rules to perform geometric analysis on the neighborhood centered on this node, logically eliminating non-natural neighbor directions that directly reach the current node through its parent node with a shorter or equal path, thus locking in reserved directions containing only the forward direction and forced neighbor directions. Finally, the system initiates an iterative ray-detection program for each reserved direction, advancing the index coordinates along the grid lines and querying the global grid map occupancy status in real time. Once the probe cursor touches the target position index, or detects surrounding obstacles that create geometric features requiring a change in the direction of travel, the current cursor position is determined to be a valid jump point node, and the detection in that direction is terminated.

[0035] Accordingly, the total comprehensive cost of the jump node is calculated based on the incremental movement cost and heuristic cost, and the parent-child mapping relationship between the jump node and the current extended node is recorded until the target node is reached. It should be understood that, in order to ensure the discovery of the globally optimal or suboptimal path, the heuristic search algorithm must perform a quantitative evaluation of each potential path node based on cumulative and expected costs, and maintain a backtrackable chain of node connections. Therefore, in the technical solution of this application, the total comprehensive cost of the jump node is further calculated based on the incremental movement cost and heuristic cost, and the parent-child mapping relationship between the jump node and the current extended node is recorded until the target node is reached. This quantifies the merits of different search sub-paths and constructs a complete topological path tree through parent pointers. This ensures that the search algorithm always advances along the direction of minimum overall cost, ultimately forming a backtrackable path data structure with clear predecessor-successor relationships connecting the starting point to the ending point.

[0036] In a specific example of this application, a distance-based cost evaluation and node association storage operation is performed. First, for each newly identified jump node, the system calculates its straight-line distance to the current extended node in the three-dimensional grid space using the Euclidean distance formula. This distance value is then added to the known cumulative cost of the current extended node to obtain the actual movement cost of the jump node. Simultaneously, the straight-line distance from the jump node to the target location is calculated as an estimated heuristic cost. Second, the system performs a weighted summation of the actual movement cost and the heuristic cost to obtain the total comprehensive cost used to measure the exploration priority of the node. Finally, the system checks whether the jump node exists in the priority open list or closed list. If the node is not recorded or the currently calculated new movement cost is lower than the historical record value, its cost data is updated. A parent pointer index pointing to the current extended node is established in the memory mapping table to establish a parent-child mapping relationship between the jump node and the storage. The node is then inserted into the priority open list to await subsequent scheduling. This evaluation and association process is repeated continuously in the search loop until the newly popped and processed node is detected as the target node, at which point the system terminates.

[0037] Accordingly, based on the parent-child mapping relationship, the algorithm recursively backtracks from the target node to the starting node to obtain a set of collision-free discrete path points. It should be understood that since the forward search process only records the predecessor pointer of each node in the node data structure, the algorithm does not directly generate a linearly ordered sequence of path coordinates upon reaching the endpoint, but rather obtains a distribution tree with implicit path information. Therefore, in the technical solution of this application, the algorithm further recursively backtracks from the target node to the starting node based on the parent-child mapping relationship to obtain a set of collision-free discrete path points, thereby accurately extracting the unique optimal path branch connecting the start and end points from the massive search tree structure. This transforms the implicit topological connection relationship into a clear, time-ordered set of three-dimensional coordinate points, providing standard geometric input data for subsequent trajectory parameterization processing.

[0038] In a specific example of this application, a linked list construction process based on pointer reverse lookup is executed. First, the system locates the target node object that has been reached and initializes an empty dynamic array as a path container. Second, the program enters a reverse tracing loop, using the currently visited node as the index key, queries the parent-child mapping table stored in memory to obtain its corresponding parent node reference, and inserts the physical coordinate information of the current node into the starting position of the path container or appends it to a temporary list. Third, the system sets the found parent node as the currently visited node in the next loop, repeating the above search and recording operations, traversing all intermediate jump nodes along the parent pointer chain generated by the search, until the backtracking cursor points to the starting node with no parent node. Finally, the system performs a reverse sorting operation on the discrete coordinate point sequence collected in the path container to make it conform to the natural movement sequence from the starting point to the ending point, thereby outputting a final ordered set of collision-free discrete path points.

[0039] Specifically, in step S3, based on a preset time interval parameter, the set of collision-free discrete path points is preprocessed with trajectory initialization and state space transformation to obtain an initial trajectory parameter set. It is understood that since the set of discrete path points generated by the front-end search only contains a few sparse key turning coordinates, lacks clear time dimension information, and is physically discontinuous, directly using it for optimization can easily lead to divergence of dynamic parameters. Simultaneously, to adapt to the differential drive characteristics of amphibious robots, their state representation must be migrated from the traditional Cartesian space to a specific state space more suitable for nonholonomic constraint control. Therefore, in the technical solution of this application, the set of collision-free discrete path points is further preprocessed with trajectory initialization and state space transformation based on a preset time interval parameter to obtain an initial trajectory parameter set. This constructs a high-density reference trajectory with a uniform time distribution and maps the geometric path to a new type of control variable including height, arc length, and yaw angle. This provides a parameterized model with good initial conditions for subsequent numerical integration and nonlinear optimization, ensuring that the optimization process converges within the correct state space.

[0040] More specifically, in the embodiments of this application, step S3 includes: performing linear interpolation resampling on the set of collision-free discrete path points based on a preset time interval parameter and the estimated total motion time to obtain an isochronous sampled path point sequence; performing a Cartesian-to-ZAY projection transformation on the isochronous sampled path point sequence to obtain a height-arc length-yaw angle spatial state point set; and initializing the trajectory parameters of the height-arc length-yaw angle spatial state point set to obtain an initial trajectory parameter set.

[0041] Accordingly, based on the preset time interval parameter and the estimated total motion time, the set of collision-free discrete path points is linearly interpolated and resampled to obtain an isochronous sampled path point sequence. It should be understood that because the path point distribution generated by the front-end jump point search is extremely uneven and completely lacks temporal constraints, directly performing high-order polynomial fitting based on these sparse points can easily induce Runge phenomenon on long straight segments or cause severe dynamic oscillations in the trajectory at corners. Therefore, in the technical solution of this application, the set of collision-free discrete path points is further linearly interpolated and resampled based on the preset time interval parameter and the estimated total motion time to obtain an isochronous sampled path point sequence, thereby filling the sparse key points with high-density intermediate state points with uniform time intervals. This provides sufficiently dense support points for subsequent trajectory parameterization, significantly improves the numerical stability of the initial trajectory, and establishes a reliable geometric basis for the continuous calculation of the yaw angle.

[0042] In a specific example of this application, path length integration and time-domain discretization operations are performed. First, the system traverses the set of collision-free discrete path points, accumulating the Euclidean distances between adjacent path points to obtain the total physical length of the entire geometric path. Second, a conservative average expected speed is set based on the robot's maximum permissible linear velocity parameter. The total physical length is divided by this average expected speed using physical kinematics formulas to calculate the estimated total motion time required to complete the task. Third, the total motion time is divided into several equal-length infinitesimal time intervals according to preset time interval parameters configured by the technician, determining the total number of sampling points required. Finally, the system performs a linear interpolation algorithm along the original discrete path, calculating and inserting new coordinate points between adjacent backbone path points based on the normalized distance ratio corresponding to each time infinitesimal, thereby generating a sequence of equally timed sampled path points uniformly distributed on the time axis.

[0043] Accordingly, a Cartesian-to-ZAY (Position Z-Arc length-Yaw) projection transformation is performed on the isochronous sampled path point sequence to obtain a set of state points in the height-arc length-yaw angle space. It should be understood that, since the isochronous sampled path point sequence is represented in Cartesian space, its yaw angle needs to be indirectly derived from the velocity direction. This can easily lead to singularities and discontinuities in the yaw angle during low-speed, stopping, or reversing transitions, making it difficult to strictly reflect the differential constraints of the ground mode in the backend optimization and affecting the executability of the amphibious robot. Therefore, in the technical solution of this application, a Cartesian-to-ZAY (Position Z-Arc length-Yaw) projection transformation is further performed on the isochronous sampled path point sequence to unify the height, arc length, and yaw angle as explicit state variables and eliminate the instability introduced by the Cartesian velocity back-calculation at the state level. It is worth mentioning that the ZAY space is orthogonally composed of three independent components: vertical height (Position Z), cumulative arc length (Arc length), and yaw angle (Yaw). By projecting the isochronous sampled pathpoint sequence from Cartesian space to this ZAY space, the robot's three-dimensional motion is effectively decoupled into three independent control dimensions: vertical lifting, forward and backward movement along the path, and in-situ turning. This allows the trajectory to maintain continuous yaw angles across the entire speed domain, naturally supporting forward and reverse movements, and providing a stable and physically consistent initial state description for subsequent coordinate mapping and multi-constraint optimization based on numerical integration.

[0044] In a specific example of this application, the system performs projection and reconstruction processing on a point-by-point basis on a sequence of equally time-sampled path points to generate a set of spatial state points in the altitude-arc length-yaw angle configuration. First, for the k-th sampling point in the equally time-sampled path point sequence, its vertical coordinates are directly read as the altitude state. and its horizontal components ( , First, the initial sampling point is used as the plane projection point; second, the cumulative arc length is recursively calculated based on the Euclidean distance between adjacent plane projection points, using the initial sampling point as the arc length zero point. The calculation relationship is as follows:

[0045]

[0046] in, This represents the cumulative arc length corresponding to the k-th sampling point. and Let each represent the planar coordinate components of the k-th sampling point in Cartesian space. and They represent the first The planar coordinate components of each sampling point in Cartesian space The system represents the square root operation; furthermore, it determines the tangential direction of the sampling point based on the planar displacement vectors of adjacent sampling points and calculates the yaw angle. The calculation relationship is as follows:

[0047]

[0048] in, This represents the yaw angle corresponding to the k-th sampling point. This represents the two-parameter arctangent function. and Let each represent the planar coordinate components of the (k+1)th sampling point in Cartesian space. and Let each represent the planar coordinate component of the k-th sampling point in Cartesian space; then, the system performs angular expansion processing on the yaw angle sequence to eliminate numerical discontinuities caused by cross-quadrant jumps, resulting in a continuous yaw angle sequence; finally, the triplet of each sampling point ( , The output is encapsulated in chronological order, forming a spatial state point set of height-arc length-yaw angle. The arc length recursive formula accumulates the non-uniform planar displacement in Cartesian space into a monotonic path progress variable, allowing the subsequent tangential velocity, expressed as the arc length derivative, to naturally distinguish between forward and reverse travel. The yaw angle formula uses the path tangential direction as an explicit orientation reference and maintains sequence continuity through angle expansion, avoiding singularities and jumps caused by back-calculating the yaw angle from velocity at stopping points or velocity minima. This ensures that the ZAY space state point set can stably support subsequent numerical integration mapping and multi-constraint optimization solutions.

[0049] Accordingly, trajectory parameters are initialized on the height-arc length-yaw angle spatial state point set to obtain an initial trajectory parameter set. It should be understood that since the height-arc length-yaw angle spatial state point set is only a set of discrete time-series coordinates, it is difficult to directly describe the details of the motion changes of the amphibious robot between two points and its higher-order derivative characteristics. Furthermore, discrete data cannot meet the computational requirements of the backend optimization solver for continuously differentiable functions and analytical gradients. Therefore, in the technical solution of this application, trajectory parameters are further initialized on the height-arc length-yaw angle spatial state point set to obtain an initial trajectory parameter set. This allows the discrete state point set to be fitted into a piecewise polynomial curve model with an analytical expression, and the dynamic state of the trajectory is pre-smoothed by optimizing the target. In this way, the high-dimensional trajectory description can be compressed into a compact parameterized representation, providing initial values ​​of variables with good physical properties and computational efficiency for subsequent gradient descent iterations.

[0050] In a specific example of this application, the system employs a closed-form solution strategy that minimizes the square integral of higher-order derivatives to generate the initial trajectory parameter set. First, the system analyzes the data in the altitude-arc length-yaw angle spatial state point set, dividing it into M independent trajectory intervals in chronological order. The altitude, arc length, and yaw angle of each interval are modeled as 2h−1 order polynomials with respect to time, where h represents the order of the optimized derivative. Second, the system initializes the time allocation vector T, ensuring that the time length of each trajectory segment is equal to the preset sampling time interval Δt. Finally, using each state point as a necessary waypoint constraint, the system constructs unconstrained quadratic programming problems for the three dimensions of altitude, arc length, and yaw angle, and solves them directly using matrix inverse operations, obtaining the unique coefficient matrix C of each polynomial. The calculation relationship for its closed-form solution is as follows:

[0051]

[0052] in, This represents a column vector containing the polynomial coefficients of all trajectory segments. This represents the block coefficient matrix formed by the polynomial basis functions and their derivatives at the endpoints of each segment. This represents the constraint vector consisting of the position, velocity, and acceleration boundary conditions of the spatial state point set in the altitude-arc length-yaw angle region; the system will calculate the polynomial coefficient matrix. The initial trajectory parameter set is obtained by merging the packet with the time allocation vector T. By performing matrix inversion, the polynomial coefficients that minimize the total acceleration energy of the entire trajectory in all dimensions can be directly calculated, provided that all intermediate waypoint positions and continuity constraints are satisfied. This ensures that the generated initial trajectory parameter set is physically smooth, thus providing a high-quality and numerically stable starting point for subsequent iterative optimization with complex nonlinear constraints.

[0053] Figure 6 This is a schematic diagram comparing the processing steps of front-end path search, equal-time resampling, and initial trajectory generation according to an embodiment of this application. Figure 6As shown in the figure, this scheme transforms discrete geometric path points into initial parameterized trajectory values ​​suitable for optimization. Specifically, it involves data transformation at three levels: 1. Original Geometric Path (JPS Sampling Path): First, a set of collision-free discrete path points is obtained by searching the grid map using the 3D Jump Point Search (3D JPS) algorithm. This path consists only of a few sparse key turning points (i.e., corner points connected by yellow line segments), characterized by only geometric connectivity, lacking temporal dimension information, and not satisfying the robot's dynamic constraints. 2. Isochronous Sampling Path (Sampling Points): To provide uniform constraints for subsequent optimization, the planner performs linear interpolation resampling on the original JPS path based on a preset time interval parameter (Δt=0.3s) and the estimated total motion time. The dense sampling points in the figure represent the isochronous sampling path point sequence. This step transforms the pure geometric path into a skeleton with temporal distribution information, ensuring that the time allocation for subsequent optimization has reasonable initial values. 3. MINCO Initial Trajectory (Blue / Black Curve Comparison): Black Curve (Unprocessed): If sparse JPS points are directly input into the optimizer, the generated initial trajectory (black), although smooth, may deviate significantly from the safe path in the unconstrained region, resulting in extremely poor initial value quality and increasing the risk of falling into local minima. Blue Curve (Preprocessed in this application): This application uses the above sampling points as waypoint constraints to solve the closed-form solution of minimum jerk, generating an initial trajectory that closely follows the safe path of the front-end search. This proves that the preprocessing step of this scheme can effectively bind the trajectory within the safe region, providing a high-quality, collision-free set of initial trajectory parameters for the back-end nonlinear optimization. In other words, this figure shows the specific effect of preprocessing, demonstrating how the algorithm transforms a coarse search path into an optimizable parameterized trajectory, reflecting the consistency and completeness of the technical solution.

[0054] Specifically, in step S4, the initial trajectory parameter set is mapped and the trajectory is constructed based on numerical integration to obtain a Cartesian full-state trajectory. It should be understood that traditional motion planning methods typically parameterize the position curve directly in the Cartesian coordinate system and passively derive the yaw angle based on the tangential direction of the velocity vector. This strong coupling mechanism leads to singularities and numerical discontinuities in the yaw angle calculation when the robot switches between zero speeds or reverses, failing to meet the ground differential drive constraints of amphibious robots. Furthermore, the parameters in the height-arc length-yaw angle domain only describe the intrinsic manifold of the motion, lacking physical space information for environmental interaction and execution control. Therefore, in the technical solution of this application, the initial trajectory parameter set is further mapped and the trajectory is constructed based on numerical integration to obtain a Cartesian full-state trajectory. This utilizes the physical principle that position is the time integral of the velocity vector to reconstruct the abstract intrinsic state into a physical space trajectory. This generates a singular-free trajectory that naturally satisfies the wheel's no-sideslip constraint and supports smooth bidirectional switching, providing a complete control state including position, velocity, acceleration, and attitude.

[0055] More specifically, in a concrete example of this application, firstly, the system parses the polynomial coefficient matrix and time allocation vector in the initial trajectory parameter set, and for each discrete-time sampling point of the trajectory segment, calculates its zeroth-order state (height) in the ZAY domain. Arc length Yaw angle First derivative state (vertical velocity) Tangential linear velocity angular velocity ) and the second derivative state (vertical acceleration) Tangential acceleration angular acceleration This yields a multi-order state sequence in the altitude-arc length-yaw angle domain. Secondly, based on the differential kinematics model of the amphibious robot, the system calculates the tangential linear velocity... With yaw angle The velocity components are decomposed into velocity components in the Cartesian plane, and the composite Simpson's rule is used to numerically integrate the velocities to recursively calculate the planar position coordinates. The integral calculation relationship is as follows:

[0056]

[0057] in, and Let these represent the velocity components along the X and Y axes of the world coordinate system at time t, respectively. This represents the tangential linear velocity at time t (it can be negative, representing reversing). This represents the yaw angle at time t. and Let represent the cumulative position coordinates of the j-th integration step, respectively. This represents the infinitesimal step size for numerical integration. This step, while accurately reconstructing the physical location, achieves this through explicit... Symbolic control enables a unified description of forward and reverse motion, resulting in a sequence of planar motion states. Finally, the system uses the chain rule to differentiate the planar motion states to obtain Cartesian acceleration and synthesizes the physical states across all dimensions. The calculation relationship for planar acceleration is as follows:

[0058]

[0059] in, and These represent the accelerations along the X and Y axes in the world coordinate system, respectively. For tangential acceleration, Angular velocity. The system will calculate the resulting planar coordinates ( , ), synthesis speed ( , , ), composite acceleration ( , , and attitude information A unified encapsulation process generates the final Cartesian full-state trajectory. In particular, Simpson's integral formula establishes a precise mapping from the control space (ZAY) to the physical space, enabling the quantitative evaluation of endpoint position constraints; while the chain rule for acceleration reveals the coupling relationship between linear acceleration and angular velocity, ensuring not only the consistency of dynamics but also allowing the backend optimizer to directly constrain physical acceleration, ensuring that the generated trajectory does not exceed the robot's driving capability limits.

[0060] In particular, the first embodiment described above suffers from significant physical modeling defects in the Cartesian full-state synthesis and dynamic mapping process. The core issue lies in simply converting the kinematic derivative of the ZAY domain into linear acceleration in Cartesian coordinates using a geometric chain rule, while neglecting the inherent physical constraints of an amphibious robot as an underactuated system in aerial flight mode. Specifically, this mechanism severs the rigid coupling between acceleration generation and body attitude, failing to consider that the robot must change its body attitude (such as roll or pitch) to generate horizontal acceleration components. This simplification results in the planner only outputting spatial position and linear acceleration, without calculating the necessary deflection angle to achieve that acceleration. This makes the generated trajectory appear continuous geometrically, but may imply extreme attitude requirements physically. For example, during ground takeoff or low-altitude maneuvers, if the planned lateral acceleration is too large, the robot must roll significantly during actual control. This could easily lead to propeller contact with the ground or the robot exceeding the effective field of view of the onboard sensors due to excessively large attitude angles, causing serious safety accidents. Furthermore, the original solution only derives acceleration based on the inertial term, completely ignoring the aerodynamic drag effect during high-speed flight. This will result in the calculated thrust command being significantly smaller than the actual requirement, causing the robot to experience cumulative lag when tracking the trajectory, which seriously affects the accuracy and stability of cross-medium motion.

[0061] To address the aforementioned shortcomings, the preferred mechanism of this application introduces a dynamic model incorporating aerodynamic drag effects to improve the robot's control accuracy and response speed during high-speed motion. Specifically, in the technical solution of this application, the abstract parameterized manifold is restored to a physically executable spatiotemporal trajectory by performing coordinate mapping and trajectory construction based on numerical integration on the initial trajectory parameter set. Furthermore, utilizing the physical principle that position is the integral of velocity, the generated trajectory naturally satisfies the nonholonomic kinematic constraint of no wheel sideslip. This allows for the construction of a geometrically continuous, dynamically differentially driven full-state reference trajectory, providing the necessary physical state input for obstacle avoidance evaluation and endpoint constraint calculation in subsequent multi-constraint optimization models.

[0062] More specifically, in another specific embodiment of this application, Figure 3 This is a flowchart illustrating the process of performing coordinate mapping and trajectory construction based on numerical integration on an initial trajectory parameter set to obtain a Cartesian full-state trajectory, according to an embodiment of this application. Figure 3As shown, step S4 includes: S41, performing ZAY domain multi-order state calculation on the initial trajectory parameter set or the updated trajectory parameter set to obtain a multi-order state sequence in the height-arc length-yaw angle domain containing tangential linear velocity and yaw angle; S42, performing Simpson numerical integration under partial constraints on the multi-order state sequence in the height-arc length-yaw angle domain based on tangential linear velocity and yaw angle to obtain a planar motion state sequence; S43, performing Cartesian full-state synthesis and dynamic mapping on the planar motion state sequence and the multi-order state sequence in the height-arc length-yaw angle domain to obtain a Cartesian full-state trajectory.

[0063] Accordingly, in step S41, a ZAY domain multi-order state solution is performed on the initial trajectory parameter set or the updated trajectory parameter set to obtain a multi-order state sequence in the height-arc length-yaw angle domain containing tangential linear velocity and yaw angle. It should be understood that since the initial trajectory parameter set or the updated trajectory parameter set is essentially a high-dimensional sparse representation composed of a polynomial coefficient matrix and a time allocation vector, it only describes the system's control law at a mathematical level and cannot directly reflect the robot's instantaneous motion state at a specific moment; furthermore, in order to perform subsequent numerical integral mapping and verify dynamic constraints, the system must obtain information on the continuous rate of change of state variables in each dimension with respect to time. Therefore, in the technical solution of this application, a ZAY domain multi-order state solution is further performed on the initial trajectory parameter set or the updated trajectory parameter set to obtain a multi-order state sequence in the height-arc length-yaw angle domain containing tangential linear velocity and yaw angle, thereby parsing the abstract parameter space information into concrete time-domain physical quantities, and using analytical differentiation methods to accurately obtain the first and second derivatives of height, arc length, and yaw angle. This enables the generation of a full-dimensional differential state flow, including tangential linear velocity, linear acceleration, angular velocity, and angular acceleration, providing comprehensive data support for the subsequent construction of Cartesian trajectories that conform to differential constraints and the evaluation of motion smoothness.

[0064] In a specific example of this application, the system employs a piecewise polynomial evaluation and analytical differentiation strategy to perform state calculation. First, the system reads the current trajectory parameter set and determines the effective time interval [0, ...] for each trajectory segment based on the time allocation vector T. The system generates a dense time query sequence at a preset resolution; secondly, for each query time t, the system calculates the displacement basis vector according to the definition of the natural basis functions in the MINCO trajectory representation. Velocity basis vector and acceleration basis vector Next, the system will generate a polynomial coefficient vector corresponding to each trajectory segment. Perform inner product operations with the aforementioned basis vectors to calculate the zeroth to second-order state values ​​of the three independent dimensions of ZAY. , and The solution relationship is as follows:

[0065]

[0066] in, This represents a state scalar that includes altitude z(t), arc length s(t), or yaw angle ξ(t). For the corresponding polynomial coefficient row vector, These are fifth-order natural basis vectors. and These are its first and second derivative vectors with respect to time, respectively. Finally, the system will resolve the tangential linear velocity. (as the first derivative of arc length), angular velocity (The first derivative of the yaw angle) and the corresponding acceleration terms are arranged in time-stamp alignment and assembled to generate a multi-order state sequence in the altitude-arc length-yaw angle domain. The above formula utilizes the analytical differentiability property of polynomials to accurately obtain the kinematic derivative at any given time with minimal computational cost. As the first derivative of arc length with respect to time, its magnitude directly characterizes the robot's instantaneous movement speed, while its sign clearly indicates the robot's direction of travel (positive for forward, negative for backward), completely eliminating the directional ambiguity caused by the calculation of velocity from position difference in traditional methods; simultaneously, As the time derivative of the yaw angle, it directly corresponds to the angular velocity of the airframe in the vertical axis. These precise analytical derivatives not only form the mathematical basis for subsequent checks on whether the differential constraints are satisfied, but also provide direct physical quantity inputs for calculating smoothness indicators such as acceleration.

[0067] Accordingly, in step S42, based on the tangential linear velocity and yaw angle, a Simpson numerical integration under partial constraints is performed on the multi-order state sequence in the height-arc length-yaw angle domain to obtain the planar motion state sequence. It should be understood that, under the ZAY parameterization framework, the robot's planar position coordinates are no longer directly controllable independent variables, but rather a nonlinear integral result determined by the coupling of tangential linear velocity and yaw angle. Furthermore, the amphibious robot is subject to strict nonholonomic difference constraints in the ground mode, requiring its instantaneous velocity vector to be parallel to the longitudinal axis of the body at all times. Therefore, in the technical solution of this application, a Simpson numerical integration under partial constraints is further performed on the multi-order state sequence in the height-arc length-yaw angle domain based on the tangential linear velocity and yaw angle to obtain the planar motion state sequence. This accurately maps the intrinsic control state back to the external Cartesian physical space and uses a high-precision numerical integration algorithm to eliminate the position accumulation error caused by discretization. In this way, a spatial walking trajectory that conforms to physical constraints can be reconstructed, providing an accurate coordinate reference for subsequent evaluation of the endpoint position deviation and obstacle distance.

[0068] In a specific example of this application, the system employs a coordinate reconstruction strategy that combines velocity decomposition with complex Simpson integral recursion. First, the system extracts the tangential linear velocity at consecutive time points from the multi-order state sequence in the altitude-arc length-yaw angle domain. With yaw angle Data; secondly, based on the principle of differential driving kinematics, the tangential linear velocity is projected onto the X and Y axes of the world coordinate system to obtain the instantaneous velocity functions of each component; thirdly, in order to improve the accuracy of position calculation and control the cumulative error, the system divides each trajectory time interval [ , The system is divided into n infinitesimal segments. The velocity components are numerically integrated using the composite Simpson's rule, and the cumulative displacement at each time step is calculated recursively. The specific integration formula in the X-axis direction is as follows:

[0069]

[0070] in, and These represent the X-axis coordinates at the start and end times of the integration interval, respectively. For tangential linear velocity, Let be the yaw angle, and n be the number of interval segments. Finally, the system synchronously performs the integral calculation in the Y-axis direction and recursively calculates the result. The coordinate sequence is merged with the velocity state at the corresponding moment to generate a planar motion state sequence. By recombining the Simpson integral formula, the system can effectively compensate for the truncation error caused by linear approximation when discretizing the trajectory using third-order or higher-order velocity information. The upper limit of the error is proportional to the fourth power of the infinitesimal step size, requiring only a small number of segments n (e.g., 10 to 20) to keep the position integration error within a minimal range. Furthermore, the formula explicitly includes... The term enables the integration process to adaptively handle reversing situations: when When the position changes from positive to negative, the accumulated position will automatically extend along the rear of the vehicle, thus perfectly reproducing the continuous trajectory containing the complex sequence of forward-stop-reverse movements, solving the problem that traditional methods cannot plan the reversing path.

[0071] Accordingly, in step S43, Cartesian full-state synthesis and dynamic mapping are performed on the planar motion state sequence and the multi-order state sequence in the height-arc length-yaw angle domain to obtain the Cartesian full-state trajectory. It should be understood that since the planar motion state sequence only describes the geometric properties of position and velocity, it does not include the acceleration and attitude information required to maintain the motion. Furthermore, the amphibious robot is an underactuated system in flight mode, subject not only to gravity but also to aerodynamic drag that increases nonlinearly with velocity during high-speed maneuvers. If the acceleration obtained by simply differentiating using the geometric chain rule does not consider air resistance and gravity compensation, the calculated thrust vector will deviate significantly from the actual requirements, leading to trajectory tracking lag or even instability. Therefore, in the technical solution of this application, Cartesian full-state synthesis and dynamic mapping are further performed on the planar motion state sequence and the multi-order state sequence in the height-arc length-yaw angle domain to obtain the Cartesian full-state trajectory. This introduces an aerodynamically augmented dynamic model and a differential flatness inverse solution mechanism to recover the true net external force, including air resistance compensation, and the corresponding body attitude from the kinematic state. This enables the generation of physically feasible, full-dimensional state trajectories with high safety boundaries, ensuring that the robot can accurately track planning instructions during high-dynamic cross-media movements.

[0072] Figure 4 This is a flowchart of step S43 according to an embodiment of this application. Figure 4 As shown, step S43 includes: S431, determining the corrected resultant external force vector based on the planar motion state sequence and the ZAY domain multi-order state sequence; S432, performing attitude and thrust inverse kinematics based on flatness characteristics on the corrected resultant external force vector and the yaw angle data in the ZAY domain multi-order state sequence to obtain the airframe attitude rotation matrix and total thrust scalar; S433, performing ground attitude compatibility verification and full-state data encapsulation processing based on the airframe attitude rotation matrix, total thrust scalar, and a preset ground safety height threshold to obtain the Cartesian full-state trajectory.

[0073] Specifically, in step S431, the corrected resultant external force vector is determined based on the planar motion state sequence and the ZAY domain multi-order state sequence. It should be understood that traditional motion planning methods often rely solely on purely geometric motion descriptions when calculating acceleration, simply deriving inertial terms using the chain rule, while neglecting the significant environmental air resistance that amphibious robots experience during high-speed flight. This distortion in the dynamic model leads to planned thrust commands being significantly less than actual requirements, resulting in severe trajectory tracking lag or even control failure during high-speed cruise. Therefore, in the technical solution of this application, the corrected resultant external force vector is determined based on the planar motion state sequence and the ZAY domain multi-order state sequence to perform aerodynamically augmented inertial acceleration calculation. This method, based on calculating the basic kinematic acceleration using the chain rule, introduces a drag model proportional to the square of the velocity to dynamically compensate for geometric acceleration. This allows for the reconstruction of the true normalized resultant external force, including the forces overcoming gravity and air resistance, from purely geometric kinematics, ensuring the accuracy of dynamic feedforward and preventing insufficient thrust at high speeds.

[0074] More specifically, in a concrete example of this application, a resultant external force calculation process based on the fusion of geometric chain derivation and aerodynamic model is executed. First, the system extracts the instantaneous yaw angle from the multi-order state sequence in the ZAY domain. Tangential linear velocity Tangential acceleration and yaw rate Data; secondly, using the chain rule of geometric kinematics, the fundamental components of the planar acceleration required for inertial motion are analytically calculated to obtain the fundamental kinematic acceleration vector. Secondly, a preset gravitational acceleration constant is introduced. and the diagonal matrix of aerodynamic drag coefficients By combining the real-time three-dimensional velocity vector v in the planar motion state sequence, an aerodynamically augmented dynamic equation is constructed to calculate the total required acceleration vector that the rotor needs to provide, i.e., the corrected resultant external force vector. The specific computational logic is as follows:

[0075]

[0076] in, This indicates inertial acceleration considering only geometric kinematics, including the X-axis component. and Y-axis components ; Yaw angle; Let the arc length be , Its derivative, for The derivative; This represents the corrected net external force vector, which includes the total required acceleration vector to overcome gravity and drag. It is vertical acceleration; It is the gravitational acceleration constant; The unit vector of the Z-axis in the world coordinate system; This is a diagonal matrix of aerodynamic drag coefficients, used to simulate the air resistance characteristics of the fuselage along different axes; For the robot's three-dimensional velocity vector, Its modulus length. This step ensures the accuracy of dynamic feedforward and prevents insufficient thrust during high-speed cruise. Based on this, this step introduces... This aerodynamic drag term explicitly compensates for the squared velocity drag along different axes, making the calculated... Not only does it satisfy path constraints geometrically, but it also eliminates the interference of environmental resistance at the physical level, thereby providing high-fidelity dynamic feedforward for the subsequent controller and ensuring the robot's precise control response across the entire speed range.

[0077] Specifically, in step S432, the corrected resultant external force vector and the yaw angle data in the multi-order state sequence of the ZAY domain are subjected to attitude and thrust inverse kinematics based on flatness characteristics to obtain the body attitude rotation matrix and the total thrust scalar. It should be understood that since the amphibious robot is a typical underactuated system in flight mode, its six degrees of freedom of pose must be achieved by changing the rotational speed of the four rotors. The specific horizontal acceleration component can only be generated by tilting the body (generating roll or pitch attitude) to project the thrust vector onto the horizontal plane. If only the position and acceleration are planned without simultaneously solving the corresponding necessary body attitude, it will be impossible to determine whether the trajectory requires an extreme tilt angle during execution, which may lead to the planning of a geometrically continuous but physically infeasible dangerous trajectory (such as requiring the body to flip instantly). Therefore, in the technical solution of this application, attitude and thrust inverse kinematics based on flatness characteristics are further performed on the modified resultant external force vector and the yaw angle data in the multi-order state sequence of the ZAY domain to obtain the airframe attitude rotation matrix and the total thrust scalar. This utilizes the differential flatness principle of the quadrotor system to construct a collinear constraint relationship between the acceleration vector and the airframe Z-axis, explicitly resolving the unique full attitude matrix from the implicit resultant dynamic force. This ensures that the planned trajectory corresponds to a stable and achievable physical attitude in each control cycle, fundamentally solving the attitude coupling problem of underactuated systems and avoiding the control divergence risk in the pre-planning-then-control mode.

[0078] In a specific example of this application, the thrust vector normalization and orthogonal coordinate system construction process is performed. First, the system calculates the aerodynamic augmented resultant external force vector obtained in the previous steps. The Euclidean norm is used as a scalar of the total thrust required to be generated by the robot rotor. Then, the resultant force vector is normalized to obtain the unit vector of the Z-axis of the body coordinate system representing the thrust direction. Secondly, the system reads the desired yaw angle from the multi-order state sequence in the ZAY domain. Construct an intermediate reference vector on the horizontal plane. Next, the system uses the vector cross product operation to solve the problems sequentially with... and The unit vector of the Y-axis of the body formed by the perpendicular lines between the planes is and the unit vector of the body's X-axis This allows for the construction of a unique orthogonal coordinate system for the machine. The specific inverse calculation logic is as follows:

[0079]

[0080] in, Defined as the total thrust scalar, that is, the total thrust that the rotor needs to generate; The Z-axis direction vector in the body coordinate system represents the thrust direction; The yaw angle is set by The determined intermediate reference vector; and These are the Y-axis and X-axis vectors of the body coordinate system, respectively; This is the body posture rotation matrix, which fully describes the three-dimensional posture that the robot must maintain when performing the current acceleration. Through this calculation process, the dynamic constraints that were originally implicitly contained in the acceleration vector are explicitly transformed into a specific body posture description, allowing subsequent safety verification to be directly evaluated based on the actual physical posture.

[0081] Specifically, in step S433, based on the body attitude rotation matrix, total thrust scalar, and preset ground safety height threshold, ground attitude compatibility verification and full-state data encapsulation processing are performed to obtain the Cartesian full-state trajectory. It should be understood that when an amphibious robot performs tasks such as cross-media switching, ground gliding, or low-altitude maneuvering, the geometric distance between the body and the ground is extremely small. If the body undergoes a large roll or pitch to generate lateral acceleration, it is highly likely that the rotor blades will directly touch the ground or exceed the effective field of view of onboard sensors (such as lidar and cameras) due to excessive tilt angles, thus causing physiological crashes or navigation failures. Traditional path planning often only focuses on preventing collisions between mass points, ignoring the ground contact risk caused by the coupling of the body's physical dimensions and attitude. Therefore, in the technical solution of this application, ground attitude compatibility verification and full-state data encapsulation processing are further performed based on the airframe attitude rotation matrix, total thrust scalar, and preset ground safety altitude threshold to obtain a Cartesian full-state trajectory. This allows for the quantification and extraction of tilt angle information from the generated attitude matrix, and the establishment of a correlation constraint mechanism between altitude and attitude angles to perform real-time circuit breaker screening for low-altitude high-risk attitudes. This ensures that each frame of output trajectory data is not only dynamically executable but also meets strict safety boundaries at the environmental interaction level, preventing physical interference accidents.

[0082] In a specific example of this application, attitude angle extraction, security logic determination, and high-fidelity data encapsulation operations are performed. First, the system extracts the body attitude rotation matrix... Z-axis vector Extract the current tilt feature of the machine and calculate its unit vector relative to the Z-axis of the world coordinate system. The angle between the two points is the current tilt angle; secondly, the current real-time height is read. And introduce a preset ground safety height threshold. (e.g., rotor radius plus buffer distance) and the maximum allowable tilt angle limit. Next, a logical verification operation is performed. If the robot's current height is detected to be below the ground safety height threshold (i.e., in the near-ground danger zone) and the calculated tilt angle exceeds the maximum allowable tilt angle limit, then it is determined that the trajectory point at that moment has a risk of physical contact with the ground or a sensor blind spot, and the feasibility indicator is set to... Set to invalid (0), otherwise set to valid (1); finally, the system packages the verified position, velocity, basic inertial acceleration, attitude rotation matrix, total thrust scalar, and feasibility indicator into a unified package to generate a full-dimensional physical state object. The specific verification and encapsulation logic is as follows:

[0083]

[0084]

[0085] in, This is a feasibility indicator used to mark whether there is a dangerous situation of low altitude and large attitude angle; z(t) is the current altitude; The ground safety height threshold; Maximum allowable tilt angle limit; The complete physical state at time t, which includes position. ,speed Inertial acceleration Attitude matrix and thrust scalar This step not only completes the closed loop of the planning data flow, but more importantly, it embeds a safety circuit breaker mechanism for amphibious scenarios at the algorithm's underlying level, ensuring the robot's physical safety under any extreme maneuvering conditions.

[0086] In the aforementioned optimized mechanism, by introducing a dynamic model that incorporates aerodynamic drag effects, the trajectory tracking lag problem caused by neglecting environmental resistance in traditional planning methods is effectively solved, significantly improving the robot's control accuracy and response speed in high-speed motion. Simultaneously, the attitude inverse solution and ground compatibility verification mechanism, implemented using the principle of differential flatness, successfully makes explicit the implicit attitude constraints caused by underactuated characteristics. This eliminates dangerous trajectories that could lead to propeller ground contact or sensor blind spots at the planning stage, ensuring the physical safety of the amphibious robot during takeoff and landing in complex terrain and during high-maneuver flight. The ultimate goal is to construct a trajectory generation framework that conforms to real physical laws and possesses extremely high safety boundaries, enabling the robot to autonomously and reliably perform highly dynamic navigation tasks across media.

[0087] Specifically, in step S5, the Cartesian full-state trajectory is mapped to a pre-constructed multi-constraint optimization model to obtain the objective function value and the gradient vector for the optimization variables. It should be understood that although the initial trajectory possesses a basic geometric shape in the ZAY parameter space, it has not undergone rigorous physical environment verification and may have issues such as crossing obstacles, exceeding the motor drive capability limit (dynamic overshoot), or the end effector failing to accurately reach the target point due to accumulated integral errors. Furthermore, to guide the solver in automatically correcting the trajectory parameters, a scalar evaluation index capable of quantifying trajectory quality and its rate of change with respect to the optimization variables must be constructed. Therefore, in the technical solution of this application, the Cartesian full-state trajectory is further mapped to a pre-constructed multi-constraint optimization model to obtain the objective function value and the gradient vector for the optimization variables. This transforms the complex obstacle avoidance requirements, dynamic constraints, and endpoint accuracy requirements into a mathematically differentiable objective function, and provides a definite descent search direction for nonlinear optimization through analytical gradients. This ensures that the generated trajectory satisfies smoothness while strictly adhering to environmental safety boundaries and the robot's physical performance limits, and possesses extremely high endpoint control accuracy.

[0088] Figure 5 This is a flowchart illustrating the motion planning method for an amphibious robot according to embodiments of this application, which maps a Cartesian full-state trajectory to a pre-constructed multi-constraint optimization model to obtain the objective function value and the gradient vector for the optimization variables. Figure 5 As shown, step S5 includes: S51, evaluating the trajectory smoothness and time regularization term of the Cartesian full-state trajectory to obtain the basic smoothing cost; S52, querying the Euclidean symbolic distance field to obtain the obstacle distance and calculating the safe collision avoidance penalty; S53, weighting and aggregating the basic smoothing cost based on the safe collision avoidance penalty, the dynamic over-limit penalty based on the dynamic limit parameter, and the endpoint position constraint penalty based on the augmented Lagrange term to obtain the total objective function value; S54, performing backpropagation and parameter space gradient projection on the total objective function value to obtain the gradient vector for the optimization variables.

[0089] More specifically, in a concrete example of this application, firstly, the multi-order derivative information of the full-state trajectory is read. Based on the principle of minimum jerk, the squared modulus of the third derivatives (jerk) of altitude, arc length, and yaw angle is integrated in the entire time domain. At the same time, a time regularization term is introduced to suppress the infinite growth of the trajectory duration, and the basic smoothing cost characterizing the stability of the control variable is calculated. The calculation relationship is as follows:

[0090]

[0091] in, Let be the duration of the i-th trajectory segment. , and These are the accelerometers for each state variable. The time weighting coefficient is used. Next, the trajectory is sampled, and the pre-stored Euclidean symbolic distance field is queried to obtain the distance from each trajectory point to the nearest obstacle. If the distance is less than a safety threshold, a nonlinear penalty function is constructed to generate a safety collision avoidance penalty. Simultaneously, the linear velocity, angular velocity, and acceleration of each point are checked to see if they exceed physical limits, and a dynamic penalty is calculated for the excess. More importantly, for the deviation between the trajectory endpoint generated by integration and the task target point, an augmented Lagrangian method is used to construct an endpoint position constraint penalty containing dual variables and a quadratic penalty term. All penalty terms are then weighted and aggregated to obtain the total objective function value. Finally, the system performs backpropagation of the gradient using the variational method and the chain rule. Since the planar position is the integral of the velocity, the system first calculates the gradient of the overall target with respect to the Cartesian position, and then uses the differential property of the integral to backpropagate it to the tangential linear velocity. With yaw angle Then, by projecting the polynomial basis functions, the total objective function relative to the polynomial coefficient vector can be calculated. The analytical gradient vector of the time allocation vector T. The mapping logic of its gradient mainly follows the following chain relationship:

[0092]

[0093] in, The Cartesian position vector, Let ZAY be the internal state vector formed by the ZAY domain and its derivatives. Let Jacobian matrix be the integral mapping. is the polynomial basis function matrix. In the above formula... This ensures the smoothness of robot movement and avoids drastic control abrupt changes. The endpoint constraint term corresponding to ALM solves the problem that traditional soft constraints are difficult to eliminate static position errors by dynamically adjusting the penalty weight, ensuring that the amphibious robot can still accurately reach the target after long-distance blind zone integration. The derivation of the analytical gradient is the core of the entire optimization algorithm's real-time convergence. It accurately transforms the various physical constraints in the external Cartesian space into adjustment instructions in the internal ZAY parameter space.

[0094] Specifically, in another concrete example of this application, to ensure that the nonlinear optimization solver can efficiently handle the aforementioned complex multi-constraint problems, a constraint-free transformation is first performed. Specifically, the MINCO framework employs the L-BFGS solver, which is suitable for solving large-scale unconstrained problems. First, for each element in the time allocation vector T... (represents the duration of the i-th segment of the trajectory), since physical time must be a positive value ( (If the value is greater than 0), direct optimization can easily lead to numerical out-of-bounds errors. Therefore, a bijective transformation formula is introduced to map it to an unconstrained variable in the entire real number field. The transformation relationship is as follows:

[0095]

[0096] Through this mapping, The range of values ​​is expanded to This allows the L-BFGS solver to search in an unconstrained space. Furthermore, for other constraints (such as safety and obstacle avoidance, and dynamic constraints), they are transformed into unconstrained problems through penalty terms. Therefore, the original optimization problem can be transformed into the following unconstrained optimization form:

[0097]

[0098] in, c is the total objective function value; c is the polynomial coefficient vector; The unconstrained time variable vector is determined by the square root bijective transformation, which determines the physical time allocation vector T. The calculated base smoothing cost; Substitute the penalty term for the d-th type of constraint (such as safety obstacle avoidance, dynamic constraint) with a summation sign. This indicates that penalties for all different types of constraints are accumulated. To solve the optimization problem, the core task is to calculate the overall objective function value. The gradient relative to the optimization variables (i.e., the polynomial coefficient vector c and the time allocation vector T). The planner's optimization variables are [ Therefore, the task of solving for the gradient is to calculate... and This application utilizes the chain rule to decompose gradient calculation into two parts: the derivative with respect to the ZAY domain state and the derivative with respect to the Cartesian position.

[0099] Specifically, the total objective function value The gradient with respect to the polynomial coefficient vector c is calculated as follows:

[0100]

[0101] Accordingly, the total objective function value The gradient with respect to the time allocation vector T is calculated as follows:

[0102]

[0103] in, is the polynomial coefficient vector in the initial trajectory parameter set; T is the time allocation vector in the initial trajectory parameter set; It is the time-order derivative vector of the natural basis functions (polynomial basis) used for ZAY domain parameterization; It is the h-th derivative of each state quantity in the multi-order state sequence of the ZAY domain (i.e., altitude z, arc length s, yaw angle ξ and their derivatives). and These are the planar position coordinates in the Cartesian full-state trajectory; It is the order of the highest derivative of the polynomial (for the Minimum Jerk model, H=3). and These represent the Jacobian matrices of the Cartesian coordinate positions relative to the optimization variables. The gradient propagation formula above embodies the core technical feature of this scheme: the first term of the formula... The first part deals with the constraints within the ZAY domain directly determined by the coefficients (such as smoothness cost and dynamic derivative constraints), whose derivatives are linear combinations of polynomials; while the last two terms of the formula (the part involving x and y) deal with the external space constraints obtained through integral mapping (such as safety collision avoidance penalty and endpoint position constraints). This is because the Cartesian position of the i-th segment... and It is obtained by integrating and summing all preceding trajectory segments, therefore and This requires recursive calculation to ensure that the optimizer can collaboratively adjust all parameters of the entire trajectory to meet the endpoint constraint. In other words, this gradient propagation mechanism ensures that when the optimizer adjusts the parameters of the i-th segment, it can correctly perceive the impact on the positions of all subsequent trajectory segments, thereby achieving globally optimal convergence.

[0104] Specifically, in step S6, the objective function value and gradient vector are nonlinearly iteratively solved and a trajectory is generated to obtain the final spatiotemporally continuous trajectory, which is used to control the amphibious robot. It should be understood that since the objective function constructs a high-dimensional nonconvex optimization problem with multiple complex constraints (such as nonlinear obstacle avoidance, dynamic limits, and integral endpoint constraints), the optimal solution cannot be obtained directly from a single calculation, and the initial trajectory parameters usually cannot meet these strict physical constraints. Furthermore, in order to transform the mathematically optimal parameters into a sequence of instructions recognizable by the underlying controller, final state reconstruction is required. Therefore, in the technical solution of this application, the objective function value and gradient vector are further nonlinearly iteratively solved and a trajectory is generated to obtain the final spatiotemporally continuous trajectory, which is used to control the amphibious robot. This utilizes numerical optimization algorithms to minimize the cost function along the gradient descent direction, gradually approximating the optimal solution that satisfies all design indicators, and solidifying the converged parameters into a physical state flow. This automatically corrects the defects of the initial trajectory, outputting a control command that combines smoothness, safety, and accuracy, thus achieving autonomous navigation of the robot.

[0105] More specifically, in this embodiment, step S6 includes: using a finite-memory quasi-Newton solver to calculate the descent direction based on the gradient vector and Hessian matrix of the optimization variable, and performing a line search along the descent direction to update the optimization variable, thereby obtaining an updated trajectory parameter set; determining whether the gradient norm and endpoint constraint violation of the updated trajectory parameter set satisfy the convergence criterion, and if not, adjusting the dual variable according to the penalty parameter growth strategy and triggering a new round of iteration, and if satisfied, locking the final converged trajectory parameter set; and performing high-precision numerical integration and physical state encapsulation on the final converged trajectory parameter set to obtain the final spatiotemporal continuous trajectory.

[0106] Specifically, in a concrete example of this application, an implementation strategy combining a two-layer iterative optimization architecture based on the augmented Lagrange multiplier method with an L-BFGS solver is adopted. First, the system inputs the currently calculated objective function value and the gradient vector for the optimization variables into a finite-memory quasi-Newton (L-BFGS) optimizer. This solver uses the gradient difference and variable difference information from the most recent m iterations to approximately construct the inverse of the Hessian matrix, thereby efficiently calculating the descent direction without explicitly storing the high-dimensional second-order derivative matrix. It then performs a line search satisfying the strong Wolfe condition to determine the optimal step size, updates the polynomial coefficients and the time allocation vector, and obtains an updated trajectory parameter set. Secondly, the system executes hierarchical convergence discrimination logic. First, it checks if the updated gradient norm is lower than the inner tolerance. If the inner convergence is achieved, it further evaluates the Euclidean distance error between the trajectory endpoint calculated based on integration and the actual navigation target point, i.e., the endpoint constraint violation. If this violation does not reach the outer convergence accuracy, the system increases the weight parameters of the quadratic penalty term and updates the dual variables (Lagrange multipliers) according to the PHR strategy, subsequently triggering a new round of L-BFGS iteration until all criteria are satisfied, locking the final converged trajectory parameter set. Finally, the system uses this converged parameter set to perform a high-precision numerical integration recalculation, parsing the position, velocity, acceleration, and attitude data in the full time domain, and encapsulating them into a standardized final spatiotemporal continuous trajectory object according to the control cycle, which is then sent to the underlying attitude controller and motor driver for execution.

[0107] Figure 7This diagram illustrates a Cartesian spatial trajectory, including reversing maneuvers, generated by an amphibious robot according to an embodiment of this application under specific target pose constraints. It verifies the technical effectiveness of the proposed "height-arc length-yaw angle (ZAY)" trajectory representation in solving ground differential constraints (no sideslip) and reversing problems. Specifically: Scene setup: The diagram sets the robot's starting point (left) and ending point (purple arrow on the right). Specifically, the target yaw angle (Yaw, indicated by the purple arrow) at the ending point is set to be the same as the starting point's orientation (both horizontal to the right), but this position can only be reached through a specific maneuver. Trajectory characteristics (blue curve): The generated final optimized trajectory (blue) exhibits a clear back-and-forth characteristic. Specifically, the robot first performs a backward reversing motion (to adjust the entry angle), then performs a long forward motion, and finally adjusts its posture again by reversing to precisely align with the target position and orientation of the ending point. Technical principle: In traditional Cartesian parameterization methods, position and yaw angle are strongly coupled, often only allowing for unidirectional forward trajectories, making it impossible to achieve the back-and-forth motion shown in the diagram. This application allows the tangential linear velocity to take negative values ​​(i.e., reverse) by independently optimizing the tangential linear velocity. The cusp of the trajectory in the figure corresponds to the moment when the velocity crosses zero and reverses. This result intuitively proves that this solution completely overcomes the technical defect of traditional planners that can only move forward and cannot reverse, greatly improving the robot's maneuverability in narrow spaces (such as when needing to park sideways or turn around).

[0108] Figure 8 This is a schematic diagram showing the change of the robot's yaw angle over time during the reversing maneuver according to an embodiment of this application. The diagram illustrates... Figure 7 The mathematical verification of the physical motion in the state space is used to prove that the proposed scheme solves the singularity and discontinuity problems of the yaw angle. Curve smoothness: The horizontal axis is time, and the vertical axis is the yaw angle. As can be seen from the figure, although there are sharp points in the spatial trajectory (changes in velocity direction, which usually lead to angle jumps in traditional algorithms), the yaw angle curve in the figure remains smooth and continuous without any steps or discontinuities. Secondly, the state is reachable in the entire time domain: at the instant when the amphibious robot reverses or stops, traditional calculation methods will fail due to the denominator being zero or the quadrant being ambiguous (i.e., the singularity problem). However, this application achieves full time-domain reachability by explicitly parameterizing the yaw angle in the ZAY domain. Figure 8 The turning point (where the speed is zero) has a defined value for the yaw angle. This diagram, combined with... Figure 7 This strongly demonstrates that the numerical integration-based mapping method proposed in this application not only achieves reversing geometrically, but also ensures the numerical stability of state variables at the control level. This means that the generated trajectory can be directly used for tracking by the underlying controller without causing system oscillation or divergence due to sudden angle changes.

[0109] In summary, the motion planning method for the amphibious robot according to the embodiments of this application is explained. After acquiring the global navigation task, it first generates a discrete path using a three-dimensional jump point search and reconstructs it into an independent state representation of height, arc length, and yaw angle. By performing a Cartesian coordinate mapping based on numerical integration on this state parameter, a kinematic model that naturally satisfies the ground difference constraints is constructed, completely eliminating the yaw angle singularities and reversing limitations of traditional methods, and achieving smooth maneuvering across the entire speed domain. Furthermore, the trajectory is iteratively shaped using a multi-constraint optimization model that includes aerodynamic drag compensation and attitude inverse kinematics verification. This process explicitly incorporates the attitude coupling characteristics of the underactuated flight system and environmental drag, effectively avoiding the risk of propeller ground contact and high-speed aerodynamic lag under near-ground large maneuvers while ensuring obstacle avoidance safety, ultimately generating an executable trajectory that is spatiotemporally continuous and strictly conforms to physical safety boundaries.

[0110] Those skilled in the art will understand that the steps, measures, and schemes in the various operations, methods, and processes discussed in this application can be alternated, modified, combined, or deleted; furthermore, other steps, measures, and schemes in the various operations, methods, and processes discussed in this application can also be alternated, modified, rearranged, decomposed, combined, or deleted; furthermore, the steps, measures, and schemes in the prior art that are similar to those disclosed in this application can also be alternated, modified, rearranged, decomposed, combined, or deleted. The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as the combination of these technical features does not contradict each other, it should be considered within the scope of this specification.

[0111] The above-described embodiments are merely illustrative of several implementation methods of this disclosure, and their descriptions are relatively specific and detailed. However, they should not be construed as limiting the scope of the patent for the embodiments of this disclosure. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the embodiments of this disclosure, and these all fall within the protection scope of the embodiments of this disclosure. Therefore, the protection scope of the embodiments of this disclosure should be determined by the appended claims. As described above, although the present invention has been shown and described with reference to specific preferred embodiments, it should not be construed as limiting the present invention itself. Various changes in form and detail can be made without departing from the spirit and scope of the present invention as defined in the appended claims.

[0112] The present invention and its embodiments have been described above. This description is not restrictive, and the accompanying drawings are only one embodiment of the present invention; the actual structure is not limited thereto. In conclusion, if those skilled in the art are inspired by this description and design similar structures and embodiments without departing from the spirit of the present invention, such designs should fall within the protection scope of the present invention.

Claims

1. A motion planning method for an amphibious robot, characterized in that, include: S1: Receive global grid map and navigation task information; S2: Based on the global grid map, the starting point and ending point in the navigation task information are searched in three-dimensional jump point in discretized three-dimensional space to obtain a set of collision-free discrete path points; S3: Based on the preset time interval parameter, perform trajectory initialization and state space transformation preprocessing on the set of collision-free discrete path points to obtain the initial trajectory parameter set; S4: Perform coordinate mapping and trajectory construction based on numerical integration on the initial trajectory parameter set to obtain the Cartesian full-state trajectory, including: performing ZAY domain multi-order state solution on the initial trajectory parameter set or the updated trajectory parameter set to obtain a multi-order state sequence in the altitude-arc length-yaw angle domain containing tangential linear velocity and yaw angle; performing Simpson numerical integration under partial constraints on the multi-order state sequence in the altitude-arc length-yaw angle domain based on tangential linear velocity and yaw angle to obtain a planar motion state sequence; performing Cartesian full-state synthesis and dynamic mapping on the planar motion state sequence and the multi-order state sequence in the altitude-arc length-yaw angle domain to obtain the Cartesian full-state trajectory; S5: Map the Cartesian full-state trajectory to a pre-built multi-constraint optimization model to obtain the objective function value and the gradient vector with respect to the optimization variables; S6: Perform nonlinear iterative solution and trajectory generation on the objective function value and gradient vector to obtain the final spatiotemporal continuous trajectory, which is used to control the execution of the air-ground amphibious robot.

2. The motion planning method for an amphibious robot according to claim 1, characterized in that, Step S2 includes: Based on the resolution parameters of the global raster map, the starting position and target position in the navigation task information are mapped to raster indices, and a priority open list containing the starting node is initialized. Extract the node with the minimum cost value from the priority open list as the current expansion node, remove non-natural neighbors according to the three-dimensional jump point pruning rule, and perform ray probing in the preserved direction to identify jump point nodes; The total comprehensive cost of the jump point node is calculated based on the incremental cost of movement and the heuristic cost. The parent-child mapping relationship between the jump point node and the current extended node is recorded until the target node is searched. Based on the parent-child mapping relationship, the target node is recursively backtracked to the starting node to obtain a set of collision-free discrete path points.

3. The motion planning method for an amphibious robot according to claim 1, characterized in that, Step S3 includes: Based on the preset time interval parameter and the estimated total motion time, the set of collision-free discrete path points is linearly interpolated and resampled to obtain the sequence of path points sampled at equal time. A projection transformation from Cartesian space to ZAY space is performed on the path point sequence sampled at equal time to obtain a set of spatial state points in height-arc length-yaw angle. The trajectory parameters are initialized on the set of spatial state points of altitude-arc length-yaw angle to obtain the initial trajectory parameter set.

4. The motion planning method for an amphibious robot according to claim 1, characterized in that, Step S5 includes: The smoothness and time regularization of the Cartesian full-state trajectory are evaluated to obtain the basic smoothing cost. Query the Euclidean distance field to obtain the obstacle distance and calculate the safe collision avoidance penalty; The basic smoothing cost is weighted and aggregated based on safety collision avoidance penalty, dynamic over-limit penalty based on dynamic limit parameters, and endpoint position constraint penalty based on augmented Lagrange terms to obtain the total objective function value. Backpropagation and parameter space gradient projection are performed on the total objective function value to obtain the gradient vector for the optimization variables.

5. The motion planning method for an amphibious robot according to claim 1, characterized in that, Step S6 includes: A finite-memory quasi-Newton solver is used to calculate the descent direction based on the gradient vector and Hessian matrix of the optimization variable, and a line search is performed along the descent direction to update the optimization variable, so as to obtain the updated trajectory parameter set. Determine whether the gradient norm of the updated trajectory parameter set and the violation of the endpoint constraint satisfy the convergence criterion. If not, adjust the dual variable according to the penalty parameter growth strategy and trigger a new round of iteration. If they are satisfied, lock the final converged trajectory parameter set. High-precision numerical integration and physical state encapsulation are performed on the final convergent trajectory parameter set to obtain the final spatiotemporal continuous trajectory.

6. The motion planning method for an amphibious robot according to claim 1, characterized in that, Cartesian full-state synthesis and dynamic mapping are performed on planar motion state sequences and multi-order state sequences in the height-arc length-yaw angle domain to obtain Cartesian full-state trajectories containing position, velocity, acceleration, and yaw angle information, including: Based on the planar motion state sequence and the ZAY domain multi-order state sequence, the corrected resultant external force vector is determined; The attitude and thrust inverse kinematics based on the flatness feature are performed on the corrected resultant external force vector and the yaw angle data in the multi-order state sequence of the ZAY domain to obtain the airframe attitude rotation matrix and the total thrust scalar. Based on the airframe attitude rotation matrix, total thrust scalar, and preset ground safety height threshold, ground attitude compatibility verification and full-state data encapsulation processing are performed to obtain the Cartesian full-state trajectory.

7. The motion planning method for an amphibious robot according to claim 6, characterized in that, Based on the planar motion state sequence and the ZAY domain multi-order state sequence, the corrected resultant external force vector is determined, including: Using the chain rule of geometric kinematics, the basic kinematic acceleration vector considering only inertial motion is calculated based on the planar motion state sequence; Introduce a preset gravitational acceleration constant and aerodynamic drag coefficient matrix; Based on the fundamental kinematic acceleration vector, gravitational acceleration constant, aerodynamic drag coefficient matrix, and the robot's real-time three-dimensional velocity vector, an aerodynamic augmented dynamic equation is constructed to calculate the total required acceleration vector, including gravity compensation and aerodynamic drag compensation, as the corrected resultant external force vector.

8. The motion planning method for an amphibious robot according to claim 6, characterized in that, Based on the airframe attitude rotation matrix, total thrust scalar, and preset ground safety altitude threshold, ground attitude compatibility verification is performed, including: Extract the current tilt angle of the aircraft from the Z-axis vector of the aircraft attitude rotation matrix; Determine if the robot's current height is below the ground safety height threshold; If the current altitude is below the ground safety height threshold and the tilt angle exceeds the preset maximum tilt angle limit, then the current trajectory point is determined to have a risk of touching the ground, and the feasibility indicator of the trajectory point is marked as invalid.

9. The motion planning method for an amphibious robot according to claim 5, characterized in that, Before using a finite-memory quasi-Newton solver to calculate the descent direction based on the gradient vector and the inverse approximation of the Hessian matrix with respect to the optimization variables, the following steps are also included: Perform unconstrained variable transformation on the time allocation vector in the optimization variables; Unconstrained variable transformation processing involves using a bijective transformation function to map constrained positive real-valued time variables to unconstrained variables in the entire real-valued domain, ensuring that physical time remains positive throughout the iteration process.

Images