Trajectory planning method and device for water-air cross-medium cruise device

Abstract

The invention relates to a trajectory planning method and device for a water-air cross-medium cruise device. The method comprises the following steps: acquiring a cruise task demand and decomposing the cruise task demand into a series of five-dimensional waypoints including a position, a course angle and a pitch angle; dividing the trajectory connecting the five-dimensional waypoints into a plurality of sections, respectively adopting a polynomial function to represent the five-dimensional state of each section of trajectory, performing coefficient solution on the polynomial function of each section of trajectory to obtain a corresponding polynomial coefficient, and obtaining each section of complete five-dimensional space-time trajectory; an algebraic equation is established on the basis of the physical constraint that the thrust of the water-air cross-medium cruise device along the y axis of a machine body is constantly zero, so that the roll angle at each moment is reversely analyzed from the five-dimensional space-time trajectory, a complete six-degree-of-freedom posture is obtained, a control instruction of the water-air cross-medium cruise device is generated, and operation control is carried out. Compared with the prior art, the method has the advantages that the problems of high precision and smooth trajectory planning of the water-air cross-medium cruise are solved, and the task execution capability and the flight stability of the water-air cross-medium cruise are remarkably improved.

Description

Technical Field

[0001] This invention relates to the field of autonomous unmanned systems technology, and in particular to a trajectory planning method and apparatus for a water-air cross-medium cruiser. Background Technology

[0002] Traditional quadcopter UAVs have well-established trajectory planning theories due to the strong coupling between their propulsion method and attitude. These methods widely employ a four-dimensional planning framework based on differential flatness, using the position (x, y, z) and heading angle (ψ) in the world coordinate system as planning objectives. Within this framework, the aircraft's pitch and roll angles are not independent variables, but rather passively determined to generate the desired horizontal acceleration.

[0003] However, for a new type of unmanned aerial-aquatic vehicle (UAAV) represented by the "Tongji Flying Fish," such as... Figure 1 As shown, its unique mechanical structure, namely the H-shaped configuration and the independent vectoring rotating arm for each thruster, fundamentally alters its dynamic characteristics. The total thrust vector of all thrusters is physically constrained within the xz plane of the airframe, which makes the driving force along the y-axis of the airframe always zero, constituting an inherent underactuated constraint. However, at the same time, the thrust direction is partially decoupled from the airframe attitude, allowing the pitch angle (θ) to be actively controlled independently of the forward acceleration.

[0004] Applying traditional four-dimensional planning methods directly to such cruisers will lead to a series of serious technical problems: First, the newly added pitch control degree of freedom cannot be utilized, preventing the full realization of their unique performance advantages (such as attitude-controlled cruise); second, the inherent underactuated constraints cannot be guaranteed to be satisfied at the planning level, potentially generating physically infeasible trajectories; finally, forced application may lead to flight instability or even control divergence. Therefore, to fully utilize the performance of such cruisers and ensure their safe and stable operation, a novel trajectory planning method that can intrinsically integrate their unique physical characteristics is urgently needed, which is precisely the technical bottleneck faced by existing technologies. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the existing technology, which cannot handle the performance advantage of the newly added pitch control degree of freedom of the water-air cross-medium cruiser, and to provide a trajectory planning method and device for the water-air cross-medium cruiser. This method systematically transforms complex high-altitude mission commands into a smooth, dynamically feasible cruiser motion trajectory and precise control commands that strictly adhere to the physical constraints of the cruiser.

[0006] The objective of this invention can be achieved through the following technical solutions: A trajectory planning method for a water-air cross-medium cruiser includes the following steps: The cruise mission requirements of the water-air cross-medium cruiser are obtained and decomposed into a series of spatiotemporal key nodes. Each spatiotemporal key node is defined as a five-dimensional waypoint containing position, heading angle and pitch angle. The trajectory connecting each five-dimensional waypoint is divided into multiple segments. The five-dimensional state of each trajectory segment is represented by a time-based polynomial function. The coefficients of the polynomial function of each trajectory segment are solved to obtain the corresponding polynomial coefficients, thus obtaining each complete five-dimensional spatiotemporal trajectory. Based on the physical constraint that the thrust of the water-air cross-medium cruiser along the y-axis of the body is always zero, an algebraic equation is established to reverse analyze the roll angle at each moment from the five-dimensional spatiotemporal trajectory. Based on the five-dimensional spatiotemporal trajectory and the corresponding roll angle, the complete six-degree-of-freedom attitude at any given moment is obtained, thereby generating control commands for the water-air transmedium cruiser for operation control.

[0007] Furthermore, the process of solving for the coefficients of the polynomial function for each trajectory segment includes: constructing a quadratic programming optimization problem based on the polynomial function, and solving for the coefficients of the polynomial function by minimizing the objective function of the quadratic programming optimization problem and satisfying the corresponding constraints.

[0008] Furthermore, the objective function is a weighted sum of smoothness penalty terms for position, heading angle, and pitch angle, which are global minimizations of the smoothness penalty terms for position, heading angle, and pitch angle, and are the higher-order derivatives of position, heading angle, and pitch angle.

[0009] Furthermore, the smoothness penalty term for the position is the fourth derivative of the position, the smoothness penalty term for the heading angle is the second derivative of the heading angle, and the smoothness penalty term for the pitch angle is the second derivative of the pitch angle.

[0010] Furthermore, the constraints include: Waypoint constraints are used to ensure that the generated trajectory passes through all five-way points at a specified time. Continuity constraints are used to ensure that the position, attitude, and their derivatives are smoothly transitioned at the connection points of each trajectory segment.

[0011] Furthermore, the constraints also include: Boundary constraints are used to constrain the water-air transmedium cruiser to remain stationary at the start and end of the mission.

[0012] Furthermore, the reverse analysis process of the roll angle is as follows: Based on the fact that the sum of the projections of the inertial force, hydrodynamic force, gravity, and buoyancy of the water-air cross-medium cruiser onto the y-axis of the body is zero, an algebraic equation is constructed. This algebraic equation is simplified into a linear combination equation of sin(φ) and cos(φ), and the linear combination equation is solved by the atan2 function. The roll angle at each moment is then analyzed in reverse from the five-dimensional spatiotemporal trajectory.

[0013] Furthermore, the control command is a feedforward control command, and the specific process for generating the feedforward control command is as follows: The complete six-degree-of-freedom attitude is substituted into the dynamic model of the water-air transmedium cruiser for inverse kinematics solution to obtain the body driving force and rotational torque required to drive the water-air transmedium cruiser to reproduce the complete six-degree-of-freedom attitude. The body driving force and rotational torque are then used as feedforward control commands.

[0014] Furthermore, the feedforward control command is combined with a feedback-based controller to control the water-air cross-medium cruiser, and the feedback-based controller is used to compensate for uncertainties in the dynamic model and external disturbances.

[0015] The present invention also provides a trajectory planning device for a water-air cross-medium cruiser, including a memory and a processor. The memory stores a computer program, and the processor calls the computer program to execute the steps of the method described above.

[0016] Compared with the prior art, the present invention has the following advantages: (1) The present invention first decomposes the flight mission requirements into five-dimensional waypoints including position, heading angle and pitch angle, and transforms the mission requirements into geometric constraints that the trajectory must satisfy, making subsequent mission planning more flexible and powerful; then the overall trajectory is divided into several segments, and a polynomial function is used to transform each segment of five-dimensional waypoints into a continuous motion trajectory, and the coefficients are solved to ensure the smoothness of the flight and strictly comply with the physical constraints of the cruiser. Based on the polynomial function obtained from the solution, and combined with the physical constraint that the thrust of the cruiser along the y-axis of the body is always zero, the roll angle at each moment is obtained, resulting in a smooth, dynamically feasible, and strictly compliant six-degree-of-freedom attitude of the cruiser's motion trajectory, thus realizing the stable motion control of the water-air cross-medium cruiser.

[0017] (2) This invention utilizes the concept of feedforward control, that is, actively calculates the driving force required to achieve the desired motion, rather than passively responding to the error. It substitutes the known complete motion state into the dynamic model of the cruiser for inverse solution, reproduces the body driving force and torque required for the planned trajectory, and realizes the active cancellation of inertial force and hydrodynamic force generated during the motion.

[0018] (3) The present invention is based on a feedforward control signal and combined with a feedback-based controller (such as PID). The feedforward signal is responsible for handling most of the control tasks, while the feedback controller only needs to compensate for model uncertainty and small external disturbances, thereby achieving extremely high trajectory tracking accuracy. Attached Figure Description

[0019] Figure 1 This is a structural schematic diagram of the "Tongji Flying Fish," a water-air cross-medium cruiser provided in the background art of this invention. Figure 2 This is a flowchart illustrating a trajectory planning method for a water-air cross-medium cruiser provided in an embodiment of the present invention. Detailed Implementation

[0020] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, 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. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0021] Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0022] It should be noted that similar labels and letters in the following figures indicate similar items. Therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0023] Example 1 like Figure 2 As shown, this embodiment provides a trajectory planning method for a water-air cross-medium cruiser, including the following steps: S1: Obtain the cruise mission requirements of the water-air cross-medium cruiser and decompose them into a series of spatiotemporal key nodes. Each spatiotemporal key node is defined as a five-dimensional waypoint containing position, heading angle and pitch angle. Specifically, abstract mission requirements are concretized into a series of precise mathematical descriptions. A complex flight mission is decomposed into a series of spatiotemporal critical nodes, each defined as a five-dimensional waypoint containing position (x, y, z), heading angle (ψ), and pitch angle (θ). This five-dimensional definition makes mission planning extremely flexible and powerful.

[0024] For example: Underwater pipeline inspection mission: A series of waypoints can be set, requiring the cruiser to maintain a pitch angle of -45 degrees (θ = -45°) throughout the entire path to ensure that the airborne sonar or camera remains continuously pointed at the pipeline.

[0025] Cross-medium water entry missions: The waypoint pitch angle before water entry can be set to zero (θ = 0°) to reduce surface impact, while the waypoint after water entry can be set to a specific pitch angle for rapid descent, such as... Figure 2 As shown.

[0026] Covert reconnaissance mission on the water: A cruiser can be planned to navigate on the water while keeping the fuselage horizontal (θ = 0°) but the nose (ψ) pointing towards the target, in order to minimize the waterborne profile.

[0027] In this way, complex task requirements are directly transformed into geometric constraints that the trajectory must satisfy.

[0028] S2: Divide the trajectory connecting each five-dimensional waypoint into multiple segments, and use a time-based polynomial function to represent the five-dimensional state of each trajectory segment. Solve for the coefficients of the polynomial function of each trajectory segment to obtain the corresponding polynomial coefficients, and obtain each complete five-dimensional spatiotemporal trajectory. Preferably, the process of solving for the coefficients of the polynomial function of each trajectory segment includes: constructing a quadratic programming optimization problem based on the polynomial function, and solving for the coefficients of the polynomial function by minimizing the objective function of the quadratic programming optimization problem and satisfying the corresponding constraints.

[0029] Specifically, this step is the core of the entire planning method. It divides the trajectory connecting waypoints into several segments and represents the five-dimensional state of each segment using a polynomial function of time (t). In other words, it transforms discrete task points (waypoints) into a continuous motion trajectory that can be smoothly executed in the physical world. To solve for all the unknown coefficients of these polynomials, this method constructs a quadratic programming (QP) optimization problem. By minimizing the objective function and satisfying the constraints, it systematically obtains the coefficient solutions that meet the requirements.

[0030] The objective function aims to globally minimize a certain higher-order derivative of the trajectory to ensure smooth flight.

[0031] Preferably, the objective function is a weighted sum of smoothness penalty terms for position, heading angle, and pitch angle, where the smoothness penalty terms for position, heading angle, and pitch angle are the higher-order derivatives of position, heading angle, and pitch angle.

[0032] Optionally, the smoothness penalty term for position is the fourth derivative of position, the smoothness penalty term for heading angle is the second derivative of heading angle, and the smoothness penalty term for pitch angle is the second derivative of pitch angle.

[0033] For example: min ∫ (w_p ||p 4 (t)||² + w_ψ |ψ''(t)|² + w_θ |θ''(t)|²) dt Minimizing the fourth derivative of position (Snap) aims to reduce the rate of change of jerk, resulting in extremely smooth flight and reduced mechanical losses. Minimizing the second derivative of attitude (angular acceleration) directly corresponds to reducing the control torque required to drive attitude changes, contributing to energy conservation and avoiding actuator saturation. The weighting coefficients w_p, w_ψ, and w_θ can be adjusted according to task priority.

[0034] Constraints: The optimization problem includes strict equality constraints. Waypoint constraints ensure that the generated trajectory precisely passes through all the defined five-dimensional waypoints at the specified time. Continuity constraints guarantee that at the connection points between trajectory segments, position, attitude, and their derivatives (velocity, acceleration, etc.) are smoothly transitioned, avoiding any abrupt changes. In addition, boundary constraints can be added, such as requiring the cruiser to be stationary at the start and end of the mission.

[0035] S3: Based on the physical constraint that the thrust of the water-air cross-medium cruiser along the y-axis of the body is always zero, an algebraic equation is established to reverse analyze the roll angle at each moment from the five-dimensional spatiotemporal trajectory. The reverse analysis process of the roll angle is as follows: Based on the fact that the sum of the projections of the inertial force, hydrodynamic force, gravity, and buoyancy of the water-air cross-medium cruiser onto the y-axis of the body is zero, an algebraic equation is constructed. This algebraic equation is simplified into a linear combination equation of sin(φ) and cos(φ). This linear combination equation is then solved using the atan2 function, and the roll angle at each moment is obtained by reverse analysis from the five-dimensional spatiotemporal trajectory.

[0036] Specifically, this step solves the aforementioned QP problem using an efficient numerical optimizer, yielding a unique set of polynomial coefficients. These coefficients collectively define a complete, time-continuous five-dimensional spatiotemporal trajectory (x(t), y(t), z(t), ψ(t), θ(t)) from the starting point to the ending point. This means that the desired five-dimensional state of the cruiser can be queried at any time t.

[0037] However, this set of states is still incomplete, lacking the roll angle (φ). The key innovation of this invention lies in establishing an algebraic equation using the physical constraint that the thrust of the cruiser along the y-axis is always zero. This equation sets the sum of the projections of inertial forces, hydrodynamic forces, and gravitational buoyancy onto the y-axis of the cruiser to zero. Through mathematical derivation, this equation can be simplified into a linear combination of sin(φ) and cos(φ). Solving this equation using the atan2 function allows for the unique reverse analysis of the roll angle φ(t) at each moment from the generated five-dimensional trajectory. Thus, the complete six-degree-of-freedom attitude of the cruiser at any moment on the trajectory is determined.

[0038] The derivation process of solving the roll angle ϕ(t) using physical constraints includes the following steps: 1) Establish algebraic constraint equations The first step in the derivation is to transform the translational dynamic equations from the world coordinate system back to the body coordinate system. The mechanical structure of the cruiser determines that its propulsion system cannot generate lateral thrust (i.e., along the y-axis of the body), thus obtaining a core physical constraint: Y≡0.

[0039] Based on this constraint, the y-axis components of the dynamic vector equation are extracted and set to zero, thus establishing a crucial algebraic equation. This equation sets the sum of the y-axis components of inertial force, hydrodynamic force, gravity, and buoyancy to zero: In this equation, all variables except the roll angle (which is an unknown) (such as position, velocity, acceleration, yaw angle, and pitch angle) are given by a known five-dimensional flat output trajectory.

[0040] 2) Simplify to a linear combination equation of sin(ϕ) and cos(ϕ). Next, we expand and rearrange the above algebraic equation. Through derivation, we can find that the equation can be simplified to a linear combination equation of sin(ϕ) and cos(ϕ): Accosϕ+Assinϕ=0 The coefficients Ac and As are entirely determined by the known flat output and its derivatives, and are known quantities that can be calculated at any time along the trajectory. Their specific expressions are as follows: 3) Solve using the atan2 function. The ultimate goal is to solve for the roll angle from the equation Assinϕ = −Accosϕ. To avoid quadrant ambiguity and obtain a unique solution, the atan2 function is used: Using this formula, at any moment on the trajectory, the corresponding Ac(t) and As(t) can be calculated using the known five-dimensional trajectory information, and then the roll angle ϕ(t) at that moment can be uniquely analyzed in reverse.

[0041] At this point, the complete six-degree-of-freedom attitude (ϕ(t), θ(t), ψ(t)) of the cruiser at any given moment on its trajectory has been fully determined.

[0042] The explanations of the parameters in the above formulas are shown in Table 1 below. Table 1 S4: Based on the five-dimensional spatiotemporal trajectory and the corresponding roll angle, the complete six-degree-of-freedom attitude at any time is obtained, thereby generating control commands for the water-air transmedium cruiser for operation control.

[0043] Preferably, the control command is a feedforward control command, and the specific process for generating the feedforward control command is as follows: The complete six-degree-of-freedom attitude is substituted into the dynamic model of the water-air transmedium cruiser for inverse kinematics solution to obtain the body driving force and rotational torque required to drive the water-air transmedium cruiser to reproduce the complete six-degree-of-freedom attitude. The body driving force and rotational torque are then used as feedforward control commands.

[0044] Preferably, the control of the water-air transmedium cruiser is achieved by combining feedforward control commands with a feedback-based controller, which is used to compensate for uncertainties in the dynamic model and external disturbances.

[0045] Specifically, after obtaining the complete six-degree-of-freedom motion state (position, attitude, and their time derivatives), the final step of this method is to generate executable control commands. The core of this step is to utilize the feedforward control concept, that is, to actively calculate the driving force required to achieve the desired motion, rather than passively responding to errors.

[0046] The known complete motion state is substituted into the dynamic model of the cruiser for inverse kinematics (IK). This model includes key influencing factors such as rigid body inertia, added mass, fluid damping, and gravity / buoyancy. The IK results in the body driving force and torque required to propel the cruiser to reproduce the planned trajectory. These forces and torques, as functions of time, serve as feedforward control signals, combined with a feedback-based controller (such as a PID controller). The feedforward signal handles the majority of the control tasks, while the feedback controller only needs to compensate for model uncertainties and minor external disturbances, thus achieving extremely high trajectory tracking accuracy.

[0047] Example 2 This embodiment provides a trajectory planning device for a water-air cross-medium cruiser, including a memory and a processor. The memory stores a computer program, and the processor calls the computer program to execute the steps of a trajectory planning method for a water-air cross-medium cruiser as described in Embodiment 1.

[0048] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A trajectory planning method for a water-air cross-medium cruiser, characterized in that, The method comprises the following steps: obtaining a cruise task requirement of a water-air cross-medium cruise vehicle, and decomposing the cruise task requirement into a series of space-time key nodes, each of which is defined as a five-dimensional waypoint including a position, a heading angle and a pitch angle; dividing a trajectory connecting the five-dimensional waypoints into multiple segments, representing the five-dimensional state of each segment of the trajectory by a time-based polynomial function, solving the polynomial function of each segment of the trajectory to obtain corresponding polynomial coefficients, and obtaining a complete five-dimensional space-time trajectory of each segment; based on a physical constraint that a thrust of the water-air cross-medium cruise vehicle along a y-axis of a body is zero, establishing an algebraic equation to inversely analyze a roll angle at each time from the five-dimensional space-time trajectory; obtaining a complete six-degree-of-freedom attitude at any time according to the five-dimensional space-time trajectory and the corresponding roll angle, and generating a control instruction of the water-air cross-medium cruise vehicle to perform operation control.

2. The trajectory planning method of a water-air cross-medium cruiser according to claim 1, wherein, The process of solving the polynomial function of each segment of the trajectory comprises: constructing a quadratic programming optimization problem according to the polynomial function, and solving the coefficients of the polynomial function by minimizing an objective function of the quadratic programming optimization problem and satisfying corresponding constraint conditions.

3. The trajectory planning method of a water-air cross-medium cruiser according to claim 2, wherein, The objective function is a weighted sum of global minimum position, heading angle and pitch angle smoothness penalty terms, and the position, heading angle and pitch angle smoothness penalty terms are high-order derivatives of the position, heading angle and pitch angle.

4. The trajectory planning method of a water-air cross-medium cruiser according to claim 3, wherein, The position smoothness penalty term is a fourth-order derivative of the position, the heading angle smoothness penalty term is a second-order derivative of the heading angle, and the pitch angle smoothness penalty term is a second-order derivative of the pitch angle.

5. The trajectory planning method of a water-air cross-medium cruiser according to claim 2, wherein, The constraint conditions comprise: a waypoint constraint for ensuring that the generated trajectory passes through all the five-dimensional waypoints at specified times; a continuity constraint for ensuring that the position, attitude and derivatives thereof are smoothly transitioned at the connection of each segment of the trajectory.

6. The trajectory planning method of a water-air cross-medium cruiser according to claim 5, wherein, The constraint conditions further comprise: a boundary constraint for constraining the water-air cross-medium cruise vehicle to be in a stationary state at the beginning and end of the task.

7. The trajectory planning method of a water-air cross-medium cruiser according to claim 1, wherein, The inverse analysis process of the roll angle specifically comprises: constructing an algebraic equation according to a sum of projections of inertial force, fluid power, gravity and buoyancy of the water-air cross-medium cruise vehicle on the y-axis of the body, simplifying the algebraic equation into a linear combination equation about sin(φ) and cos(φ), and solving the linear combination equation by an atan2 function to inversely analyze the roll angle at each time from the five-dimensional space-time trajectory.

8. The trajectory planning method of a water-air cross-medium cruiser according to claim 1, wherein, The control instruction is a feedforward control instruction, and the generation process of the feedforward control instruction specifically comprises: substituting the complete six-degree-of-freedom attitude into a dynamics model of the water-air cross-medium cruise vehicle to obtain body driving force and rotating torque required for driving the water-air cross-medium cruise vehicle to reproduce the complete six-degree-of-freedom attitude, and taking the body driving force and rotating torque as the feedforward control instruction.

9. The trajectory planning method of a water-air cross-medium cruiser according to claim 8, wherein, The feedforward control instruction is combined with a feedback-based controller to control the water-air cross-medium cruise vehicle, and the feedback-based controller is used to compensate for uncertainty of the dynamics model and external disturbances.

10. A trajectory planning device for a water-air cross-medium cruiser, characterized in that, The device comprises a memory and a processor, the memory stores a computer program, and the processor calls the computer program to execute the steps of the method according to any one of claims 1 to 9.

Images