A trajectory planning method for amphibious platform based on modal characteristics

Abstract

The application discloses a land-air amphibious platform trajectory planning method based on modal characteristics, and relates to the field of trajectory planning.The method comprises the following steps: acquiring information data; constructing a dynamics model of a land-air amphibious platform based on the information data; the dynamics model comprises a tracked differential kinematics model and a ducted flight dynamics model; based on the dynamics model, land-air acceleration dynamics sampling is performed according to differential flatness characteristics and air-ground modal characteristics, and sampling trajectory data is obtained; the quasi-Newton method is adopted to iteratively solve according to the sampling trajectory data, so as to perform air-ground differentiated trajectory optimization and obtain optimized trajectory data; and the optimized trajectory data is used to realize trajectory planning of the land-air amphibious platform.The application aims to fully utilize modal characteristics and improve the maneuverability of the land-air amphibious platform.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of trajectory planning, in particular to a land-air amphibious platform trajectory planning method based on modal characteristics. BACKGROUND

[0002] Aircraft has strong maneuverability, and is widely used in reconnaissance, search and rescue and other scenes; but it has high energy demand, low load capacity and short working time. The energy efficiency of ground general platform is high, and the running time is relatively long. Therefore, in recent years, people have shown great interest in hybrid ground / air vehicles.

[0003] The land-air amphibious platform combines flight and driving two motion modalities into one platform, which can make the robot handle more tasks in complex environments, while achieving higher performance, such as improving energy efficiency, flexible cross-domain maneuvering, etc., and has broad application prospects in the case of robot operation environment diversification and complexity.

[0004] Existing land-air amphibious platform research mainly focuses on innovative mechanical design and various control strategies, while the motion planning field in complex environments has not been fully explored, and has achieved certain results in relatively simple scenarios.

[0005] Current land-air amphibious platforms are mostly based on search or optimization planning methods, and their planning framework still has room for improvement and further research. Existing land-air amphibious path planning algorithms are mostly based on graph search or optimization, and existing solutions lack land-air integrated planning algorithms based on sampling front-end and optimization back-end.

[0006] In addition, there is a lack of improvement for different modal characteristics of air and ground, usually assuming flat ground, lacking consideration of terrain features, limiting the ground maneuverability, and possibly falling into potentially dangerous working conditions; the same dynamics constraints are applied to the air and ground two modalities, limiting the air maneuverability.

[0007] Therefore, it is an urgent technical problem to establish a land-air amphibious path planning method based on modal characteristics. SUMMARY

[0008] The purpose of the present application is to provide a land-air amphibious platform trajectory planning method based on modal characteristics, which can fully utilize modal characteristics and improve the maneuverability of land-air amphibious platforms.

[0009] To achieve the above purpose, the present application provides the following solutions:

[0010] The application provides a track planning method for a land-air amphibious platform based on modal characteristics, which is applied to a track ducted land-air amphibious platform; the track ducted land-air amphibious platform comprises an integrated composite fuselage, a track driving mechanism and a ducted flight mechanism; the method comprises the following steps:

[0011] acquiring information data; the information data comprises position and attitude information and environment information;

[0012] constructing a dynamics model of the land-air amphibious platform based on the information data; the dynamics model comprises a track differential kinematics model and a ducted flight dynamics model; the track differential kinematics model is a mathematical model equivalent constructed based on an application scenario when a vehicle speed is lower than a set vehicle speed; the ducted flight dynamics model is a Newton-Euler integrated nonlinear dynamics model established after the ducted flight mechanism is regarded as a single rigid body with six degrees of freedom based on a structural design of the ducted flight mechanism;

[0013] based on the dynamics model, performing land-air acceleration dynamics sampling according to differential flatness characteristics and air-ground modal characteristics to obtain sampling track data; the modalities comprise a ground modality and an air modality;

[0014] iterative solving is performed according to the sampling track data by using a quasi-Newton method to perform air-ground differentiated track optimization to obtain optimized track data; the optimized track data is used to realize track planning for the land-air amphibious platform.

[0015] According to the specific embodiments provided in the application, the application has the following technical effects:

[0016] The application provides a track planning method for a land-air amphibious platform based on modal characteristics, which comprises the following steps: first, acquiring information data; constructing a dynamics model of the land-air amphibious platform based on the information data; the dynamics model comprises a track differential kinematics model and a ducted flight dynamics model; based on the dynamics model, performing land-air acceleration dynamics sampling according to differential flatness characteristics and air-ground modal characteristics to obtain sampling track data; iterative solving is performed according to the sampling track data by using a quasi-Newton method to perform air-ground differentiated track optimization to obtain optimized track data; the optimized track data is used to realize track planning for the land-air amphibious platform. According to the differential flatness characteristics and the air-ground modal characteristics, the land-air acceleration dynamics sampling is performed, the ground modality takes into account terrain analysis, and the air modality takes into account differentiated dynamics boundaries, so that the modal characteristics can be fully utilized. Thus, the application can fully utilize the modal characteristics to improve the maneuverability of the land-air amphibious platform. BRIEF DESCRIPTION OF DRAWINGS

[0017] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the technical solutions in the related art, the following will briefly introduce the drawings needed in the embodiments. Obviously, the drawings in the following description only constitute some embodiments of the present application. For those skilled in the art, other drawings can also be obtained based on these drawings without any creative effort.

[0018] Figure 1 Trajectory planning method flow chart for amphibious platform based on modal characteristics

[0019] Figure 2 Amphibious platform schematic diagram with track duct

[0020] Figure 3 Differential kinematics model schematic diagram of tracked vehicle

[0021] Figure 4 Amphibious power dynamics structure schematic diagram

[0022] Figure 5 Amphibious acceleration dynamics sampling flow chart schematic diagram

[0023] Figure 6 Tracked structure driving on effective through plane schematic diagram

[0024] Figure 7 Dynamics planning simulation result schematic diagram about terrain; wherein,Part a of Figure 7 is a three-dimensional unevenness terrain map;Part b of Figure 7 is a generated path schematic diagram of uneven map;Part c of Figure 7 is a three-dimensional roughness terrain map;Part d of Figure 7 is a generated path schematic diagram of rough map;Part e of Figure 7 is a three-dimensional dispersion sharpness terrain map;Part f of Figure 7 is a generated path schematic diagram of sharp protrusion map;Part g of Figure 7 is a three-dimensional undulation terrain map;Part h of Figure 7 is a generated path schematic diagram of undulation map;

[0025] Figure 8 Trajectory execution speed comparison schematic diagram under different speed limits

[0026] Figure 9 Amphibious planner comparison schematic diagram

[0027] Figure 10 Amphibious platform autonomous navigation architecture schematic diagram DETAILED DESCRIPTION

[0028] ​​​​​​​​With reference to the drawings and embodiments of the present application, the technical solutions in the embodiments of the present application will be described clearly and completely. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort belong to the scope of protection of the present application.

[0029] The present application designs a land-air amphibious planning algorithm based on modal characteristics. The land-air planner adopts a hierarchical strategy of front-end dynamics sampling and back-end trajectory optimization, considers terrain analysis, and differentiates the optimization constraints of the two modes, thereby fully utilizing the air-ground modal characteristics.

[0030] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application will be further described in detail below with reference to the drawings and specific embodiments.

[0031] The present application provides a land-air amphibious platform trajectory planning method based on modal characteristics. The method is applied to a track ducted land-air amphibious platform; the track ducted land-air amphibious platform comprises an integrated composite fuselage, a track driving mechanism and a ducted flight mechanism. As shown in Figure 1 The method comprises the following steps:

[0032] Step 100: obtaining information data. The information data comprises position and attitude information and environment information.

[0033] Step 200: constructing a dynamics model of the land-air amphibious platform based on the information data. The dynamics model comprises a track differential kinematics model and a ducted flight dynamics model; the track differential kinematics model is a mathematical model equivalent constructed based on the application scenario that the vehicle speed is lower than the set vehicle speed; the ducted flight dynamics model is a Newton-Euler integrated nonlinear dynamics model established after considering the ducted flight mechanism as a six-degree-of-freedom single rigid body based on the structural design of the ducted flight mechanism.

[0034] Step 300: based on the dynamics model, land-air acceleration dynamics sampling is performed according to the differential flatness characteristics and the air-ground modal characteristics, and sampling trajectory data is obtained. The modal comprises a ground modal and an air modal.

[0035] Step 400: adopting a quasi-Newton method, iterative solving is performed according to the sampling trajectory data, so as to perform air-ground differential trajectory optimization, and obtaining optimized trajectory data. The optimized trajectory data is used to realize trajectory planning of the land-air amphibious platform.

[0036] In an embodiment, the expression of the track differential kinematics model comprises:

[0037] In an embodiment, the expression of the track differential kinematics model comprises:

[0038] where, is the position derivative of the amphibious platform center corresponding to the X-axis; is the position derivative of the amphibious platform center corresponding to the Y-axis; is the position derivative of the amphibious platform center corresponding to the Z-axis; is the derivative of the heading angle of the amphibious platform;v l is the left track speed of the tracked chassis;v r is the right track speed of the tracked chassis; B is the track center distance; is the heading angle.

[0039] The expression of the ducted flight dynamics model is:

[0040]

[0041] where, is the derivative of the three-dimensional position of the ducted flight mechanism;v a is the speed of the ducted flight mechanism; m is the mass; is the derivative of the speed of the ducted flight mechanism; g is the gravitational acceleration; e3 is a unit vector; F t is the power generated by the ducted fan; M t is the torque generated by the ducted fan; F d is the external disturbance force; M d is the external disturbance torque; is the derivative of the attitude matrix of the ducted flight mechanism; R a is the attitude matrix of the ducted flight mechanism; is the estimated value of the angular velocity of the ducted flight mechanism; ω a is the angular velocity of the ducted flight mechanism; J is the inertia matrix; is the derivative of the angular velocity of the ducted flight mechanism.

[0042] In an embodiment, based on the dynamics model, the land-air acceleration dynamics sampling is performed according to the differential flatness characteristics and the air-ground mode characteristics, to obtain sampling trajectory data, specifically including:

[0043] determining the traversability parameters of the ground mode; the traversability parameters are linear representations of three terrain elements of slope, height and sharpness; determining the traversability cost function based on the traversability parameters, and adding it to the sampling tree; the sampling tree is a model based on the dynamics model, and decoupling and planning are performed in the flat output dimension according to the differential flatness characteristics.

[0044] The sampling tree is used to perform land-air acceleration dynamics sampling based on the dynamics model, to obtain sampling trajectory data.

[0045] The land-air acceleration dynamics sampling is performed based on a dynamics model by using a sampling tree to obtain sampling trajectory data, and specifically includes: determining state parameters; the state parameters include: a starting state and a target state; performing acceleration search sampling in a state space based on a triple integrator model to obtain a random node, and performing node transition based on a node state transition cost function to obtain a transitioned node; the node state transition cost function is a time-energy optimal quadratic form constructed with the minimum energy cost and time consumption as targets.

[0046] determining a motion state based on a height difference between the two transitioned nodes; the motion state is takeoff, landing, or the same; performing dynamics expansion according to the motion state to obtain a new node; determining whether a connection edge between the new node and the transitioned node collides with an obstacle based on a collision detection function to obtain a first determination result; if the first determination result is yes, returning to “performing acceleration search sampling in a state space based on a triple integrator model to obtain a random node, and performing node transition based on a node state transition cost function to obtain a transitioned node”.

[0047] If the first determination result is no, adding the new node to the sampling tree, finding a plurality of potential parent nodes, re-wiring and pruning to obtain a sampling expansion tree; determining whether the target state is reached based on the state transition cost corresponding to the connection edge of each node in the sampling expansion tree to obtain a second determination result.

[0048] If the second determination result is yes, obtaining the sampling trajectory data by backtracking from the parent nodes; if the second determination result is no, determining whether a set iteration number threshold is reached to obtain a third determination result; if the third determination result is no, returning to “performing acceleration search sampling in a state space based on a triple integrator model to obtain a random node, and performing node transition based on a node state transition cost function to obtain a transitioned node”.

[0049] When the motion state is takeoff, the expression satisfied is:

[0050]

[0051] When the motion state is landing, the expression satisfied is:

[0052]

[0053] When the motion state is the same, the expression satisfied is:

[0054]

[0055] wherein z cur is the height of the current node; z ran d is the height of the next random sampling node; and Flagstate_switch is a flag bit for state transition; Reward switch is a reward value for node transition; Penalty takeoff is a take-off penalty value; Reward landing is a landing reward value.

[0056] In an embodiment, the calculation method corresponding to the transferred node specifically comprises:

[0057]

[0058] wherein, is a cost function of a sample trajectory; is a cost function; s(t) is a sample trajectory; p is a proportional parameter; u(t) is a control input; A is an n x n order matrix; B is an n x m order matrix; is a derivative of a sample trajectory; s(0) is a starting state at time 0; s start is a starting state; s(τ) represents a target state at time τ; s target is a target state; τ is a duration period; T is a transpose symbol; S free is a state space; is an input space. t is a time.

[0059] A quasi-Newton method is adopted to iteratively solve according to the sample trajectory data, and a corresponding expression is:

[0060]

[0061] wherein, q is a waypoint; T is time; λ x is a weight assigned to different components x of a cost function; J x is a penalty function item of different components x.

[0062] The penalty function item includes a total time cost, a smoothness cost, an obstacle avoidance cost, and a dynamics cost.

[0063] An expression of the total time cost is:

[0064] J t = sum(T).

[0065] An expression of the smoothness cost is:

[0066]

[0067] An expression of the obstacle avoidance cost is:

[0068]

[0069] The expression of the dynamics cost is:

[0070] J f = J v + J a + J j .

[0071] wherein J t is the total time cost; T is time; J s is the smoothness cost; is the third derivative of position; t is the final time; t0 is the initial time; p(t) is position; s is state; c is cost; J c is the obstacle avoidance cost; J f is the dynamics cost; J v is the dynamics cost corresponding to velocity; J a is the dynamics cost corresponding to acceleration; J j is the dynamics cost corresponding to jerk.

[0072] The present application aims to provide a land-air amphibious planning method based on modal characteristics. First, a land-air hierarchical planning architecture is proposed. The front end performs dynamic sampling in the hybrid air-ground space to obtain an initial trajectory, and fits an effective passing plane in the ground mode according to the geometric characteristics of the terrain, thereby improving the traversability of the ground. Secondly, the rear end uses a parameterized optimization method to refine the final trajectory that is safe, smooth and dynamically feasible. According to the differentiated air-ground modal characteristics, more relaxed dynamics constraints are given to the air to improve the maneuverability of the air. In order to verify the reliability and effectiveness of the land-air amphibious planning method based on modal characteristics, a series of land-air planning simulation tests in unstructured environments are carried out.

[0073] The present application is based on a tracked ducted land-air amphibious platform, the main structure of which is shown in Figure 2 , which mainly consists of an integrated composite fuselage, a tracked driving mechanism, a ducted flight mechanism, and is equipped with a flight controller, an onboard computer and a radar sensor. The platform has two modalities of ground driving and air flight, and the movements of each part interfere with each other, with the characteristics of strong nonlinearity and high coupling. The structure of this active amphibious platform system is coupled but the state is decoupled, that is, the propeller and the tracked belt will not execute at the same time, and leaving the ground is considered as flight.

[0074] Step 1: Establish the dynamics model of the land-air amphibious platform.

[0075] (1) Establish the kinematic model of the tracked driving mechanism.

[0076] Tracked amphibious platform is a strong nonlinear coupled system. In unstructured environment, the terrain is complex and constantly changing, it is difficult to accurately model the interaction between the track and the ground. As it is more applicable to low-speed ground travel scenarios, such as Figure 3 , the equivalent tracked differential kinematic model is used to describe its motion:

[0077]

[0078] In the tracked driving mechanism matched in the present application, the two side tracks are arranged symmetrically, and the transmission mechanism provides driving force for the tracked driving mechanism; the load cushioning mechanism provides cushioning force for the transmission mechanism, thereby improving the overall ground clearance of the walking mechanism, and enhancing the climbing and obstacle crossing ability.

[0079] As shown in Figure 3 , in the inertial coordinate system XYO, (x c1 ,y c1 ) and (x c2 ,y c2 ) are the geometric centers of the two positions of the platform respectively; v is the forward direction speed; ω is the yaw angular velocity of the unmanned platform, θ g is the angle between the initial position and the X axis; is the heading angle.

[0080] The tracked differential kinematic model meets the differential flatness property, and its flat output z g is:

[0081]

[0082] Where r g is the spatial position of the tracked driving structure, φ g is the yaw angle of the tracked driving structure, is a two-dimensional real number set, and SO(2) is a plane rotation group.

[0083] The differential flatness property simplifies the expression form of the optimization problem, and the present application only needs to focus on the spatial position r g and its high-order information. The yaw planning is independent of the three-dimensional position planning and does not affect the translational motion.

[0084] (2) Establish the ducted flight dynamics model.

[0085] According to the structural design characteristics, the ducted amphibious platform is simplified as a uniform symmetric structure, so the origin of the body coordinate system coincides with the mass center of the platform, as shown in Figure 4 . Further, it is regarded as a single rigid body with six degrees of freedom, and a Newton-Euler integrated nonlinear dynamics model is established:

[0086]

[0087] e3 = (0, 0, 1) T is a unit vector.

[0088] In the inertial coordinate system XYZO, is the attitude angle. In the body coordinate system X b Y b Z b O b , [p, q, r] T is the angular velocity of rotation, and F1 to F4 are the thrusts of the ducted fans. Among them, is the roll angle, θ is the pitch angle, and Ψ is the yaw angle. p is the angular velocity of rotation around the X b axis, q is the angular velocity of rotation around the Y b axis, and r is the angular velocity of rotation around the Z b axis.

[0089] The state space of the ducted amphibious platform has a total dimension of twelve, consisting of three-dimensional position r a , the speed v a of the ducted flight mechanism, the angular velocity ω a of the ducted flight mechanism, and the attitude matrix R a . The state variables are interrelated and subject to the flight dynamics model.

[0090] The differential flatness property selects four flat output quantities, i.e., the three-dimensional position r a of the mass center and the yaw angle φ a of the ducted flight structure. Planning is carried out in a four-dimensional space, and z a can capture all the essential characteristics of the aircraft motion:

[0091]

[0092] where r a is the spatial position of the ducted flight structure, φ a is the yaw angle of the ducted flight structure, is a three-dimensional real number set, and SO(2) is a plane rotation group.

[0093] Step 2: Accelerated dynamic sampling trajectory planning method.

[0094] The objectives of the air-ground planner are as follows:

[0095] 1) Reasonable switching between air and ground modes to land in time after flying over obstacles, taking a ground-priority strategy to reduce energy consumption; 2) Through terrain evaluation, enhance ground passability and optimize ground path; 3) Differentiate the speed and acceleration limits of the two modes of air and ground, shorten the overall path time, and fully utilize the flight maneuverability and obstacle crossing advantage.

[0096] The optimal planning problem is defined as: given a start state s start ∈ C free and a goal state s target ∈ C free , find a collision-free trajectory σ * that minimizes a given cost function.

[0097] where C free is the configuration space.

[0098]

[0099] where c(σ) is the cost function of the trajectory; σ(0) is the trajectory of the start state; σ(1) is the trajectory of the end state; σ(X) is the trajectory of all states.

[0100] σ * should satisfy: 1) all kinematic and dynamic constraints; 2) safely pass through, avoiding collision with obstacles along the way; 3) minimize the time and energy required for movement; 4) minimize the risk of the platform being unable to maintain a stable posture.

[0101] The land-air planner follows a typical hierarchical framework including a front-end accelerated dynamics sampling planner and a back-end parameterized trajectory optimizer, and makes full use of the modal characteristics. Specifically, based on the model differential flatness characteristics analyzed in Step 1, the front-end trajectory planning problem can be simplified to generating three-dimensional trajectories in a hybrid modal sampling tree. The back-end trajectory optimization process is parameterized by total time, smoothness, collision-free, and dynamic feasibility. In particular, for the ground mode, the land-air amphibious platform passes through uneven terrain on the constructed effective passing plane; for the air mode, it maneuvers over unavoidable obstacles with more relaxed dynamic constraints.

[0102] The land-air accelerated dynamics sampling serves as the front-end of the land-air planner, as shown in Figure 5 .

[0103] The quadrotor aircraft model and tracked differential vehicle model simplified in Step 1 at air-ground states both conform to the differential flatness characteristics, making all state quantities describing the complete motion of the platform can be represented by position and its derivatives. Therefore, in the land-air accelerated dynamics sampling, the control quantities represented linearly can be directly input, and only need to be planned in the low-dimensional space of the center-of-mass three-axis position and yaw angle.

[0104] The sampling tree obtains the start state s start and the goal state s target , and moves from the start state s start towards the goal state s targetGrowth. First sample a valid random node s rand in the state space by searching and narrowing down the ellipsoid informed subset to accelerate the sampling process. Then find the node s rand with the minimum state transition cost from the random node s nearest . Determine the next motion state by the height difference between the two nodes, including SAME, TAKEOFF and LANDING. Further expand the dynamics to the new node s new and check whether the connection edge between the two nodes collides with the obstacle. If collision, resample, if no collision, add s new to the sampling tree. Find the potential multiple parent nodes s parent , rewire and prune the invalid nodes to minimize the state transition cost of the connection edge in the sampling expansion tree, and reduce the size of the sampling tree. When reaching the target state, backtrack from the parent node s parent to get the asymptotic s * ; if not reached, check whether the maximum iteration number is reached, if yes, end the sampling, if not, continue the iteration of random sampling.

[0105] In the land-air accelerated dynamics sampling process, the application further proposes a method for accelerating sampling of a valid random node s rand in the state space of an amphibious platform. The specific implementation method is as follows:

[0106] Randomly sample in the state space equation, and the air-ground state can be simplified as a quadrotor aircraft model and a tracked differential vehicle model respectively. According to the differential flatness characteristic, it is modeled as a linear system, which is required to be form controllable, and each flat output dimension can be decoupled and planned respectively.

[0107] In order to maintain continuity at least in acceleration, the application uses a triple integrator model jerk (jerk) as input:

[0108]

[0109] Let u(t)∈U be the control input, U=R m is the control input space. A∈R n×n , B∈R n×m . The system of the amphibious platform is a special case where the matrix A is an idempotent dynamics matrix, so an efficient closed-form solution can be obtained.

[0110] The application searches ellipsoid informed subset, i.e. full informed set, in state space to accelerate effective sampling. This method alleviates the problem of limited convergence speed and increasing computation in high-dimensional space caused by original incremental, omnidirectional and dynamic sampling. In the ellipsoid informed subset in high-dimensional space, as the path length, i.e. the long axis of the ellipse, becomes shorter and shorter, the sampling range becomes smaller and smaller and the search speed becomes faster and faster under the condition that the ellipse focal point remains unchanged.

[0111] Let c* be the cost of the optimal path, and the path passes through a state s. Improve the state subset of the current solution It is equivalent to increasing the cost from S e Add the probability of a random node. The state subset can be expressed as c best .

[0112] S e ={s∈S∣c*<c best}.

[0113] The state ellipsoid informed subset of random sampling satisfies:

[0114] ||s rand -s start ||2+||s rand -s target ||2≤c best ,s rand ∈S e .

[0115] In the next step of air-land accelerated dynamic sampling, the node s rand with the minimum state transition cost needs to be found. nearest When there is a motion constraint, the optimal control principle is introduced to solve the state transition cost function and finally solve the trajectory between the two nodes.

[0116] The cost function form c(s rand ,s nearest ) that minimizes the cost of two nodes is equivalent to solving a fixed-endpoint, free-time optimal control problem; for the triple integrator model of the amphibious platform, the application makes the minimum cost become a fixed final state and fixed final time problem.

[0117] The cost function for converting from state s rand to state s nearest adopts a time-energy optimal quadratic form, which requires the minimum energy cost and the minimum time consumption.

[0118]

[0119] where τ is the duration period, R ∈ R m×m is a given positive constant that weights the cost of control input and the duration of the trajectory.

[0120] Minimizing the cost is equivalent to solving a fixed end-point, free-time optimal control problem. Thus, the trajectory planning problem is formulated as follows:

[0121]

[0122] The present application solves the optimal state transition time τ* by taking the derivative of the cost function The derivative is taken to solve the optimal state transition time τ*, the function There can be multiple local minima. For the triple integrator model of the amphibious platform, the derivative is a fifth order polynomial. Minimizing the cost again becomes a fixed final state, fixed final time problem. In addition, the Pontryagin's Maximum Principle is applied to characterize the optimal control input u*.

[0123] In addition, a new mechanism of air-ground state switching based on sampling tree is proposed. Three types of node transition states are defined in the planner, which are SAME, TAKEOFF and LANDING. In the current takeoff state, a transition cost takeoff penalty is given; in the current landing state, a transition cost landing reward is given. The present application focuses on the adjacent node state, without imposing a penalty on the entire air state, which is more consistent with the actual motion state.

[0124] When two adjacent nodes are in the takeoff state:

[0125]

[0126] When two adjacent nodes are in the landing state:

[0127]

[0128] When two adjacent nodes are in the same state:

[0129]

[0130] TAKEOFF represents the takeoff state; LANDING represents the landing state; SAME represents the same state of two adjacent nodes.

[0131] Step 3: Terrain evaluation method in sampling nodes.

[0132] The tracked driving mechanism has good passability, strong obstacle crossing ability, and stable motion. Its shape structure and ground contact area are more suitable for plane fitting with the terrain. The synchronous track used in the present application has a large friction force, so the terrain contact constraint is not further analyzed.

[0133] The purpose of the terrain evaluation is as follows:

[0134] 1) Ground priority trajectory planning: Utilize the amphibious platform's own obstacle crossing ability to improve crossability and save battery energy. 2) Risk perception trajectory planning: While setting obstacle avoidance constraints, avoid potential risks such as sharp obstacles damaging the shell or the platform getting stuck, excessive tilt angle, and prevent getting into dangerous working conditions. Although it is possible to directly take off from uneven ground in a dangerous stuck working condition for emergency response, it is still preferable to actively choose a flat surface.

[0135] On the grid map surface, define an effective passing plane Φp:={r,ρ} to store node information. r is the plane center and ρ is the tilt angle. As shown in Figure 6 each node N i , N n , and N m corresponds to a local plane. This application considers both small-area scattered sharp obstacles and overall ground undulation and flatness. The former avoids local dangerous working conditions, and the latter improves global passability.

[0136] Based on the above strategy, three terrain elements of slope, height, and sharpness are analyzed.

[0137] 1) Slope cost α slope Analysis: The slope value describes the tilt degree of the terrain. The greater the slope value, the greater the tilt degree of the uneven terrain, and the greater the difficulty of the platform passing. The terrain slope is the angle between the Z-axis of the ground coordinate system and the normal vector of the terrain plane. The slope value has a greater relevance to the passing plane.

[0138] α slope = κ sl arcsinz ni .

[0139] where κ sl is a constant coefficient, and z ni represents the projection of the plane normal vector on the Z-axis of the world coordinate system.

[0140] 2) Height cost α step Analysis: The height value represents the rate of change of the vertical height of the terrain within a certain horizontal distance. The greater the height value, the greater the rate of change of the vertical, and the more likely the platform is to overturn.

[0141]

[0142] where h max is the maximum temporary step height in the circular area, n st is the number of temporary step heights in the circular area that are higher than the critical step height, and n critThis represents the number of valid cells where the temporary step height is higher than the critical step height. This method for calculating step height can also detect small steep slopes as steps and is robust to missing terrain information.

[0143] 3) Sharpness cost α sharp Analysis: The sharpness value represents the degree to which sharp, scattered obstacles suddenly appear on the terrain surface. It is described by the standard deviation of the height within each local planar region to quantify this disorder.

[0144]

[0145] Where, N k The number of cells in each rectangular region. rectangular area k The height information of cell (i,j) in the middle. The average height of each rectangular region, z c This refers to the vehicle's ground clearance. Check. Is it below the z-axis of the vehicle body plane? c It is necessary.

[0146] Each node is assigned a τ. i Parameters. To normalize the traversability mapping, the traversability of the i-th map element is represented as τ. i ∈[0.0,1.0]. A smaller value indicates a smoother, flatter terrain, while a larger value indicates a more rugged terrain. Traversability is a linear combination of three indicators:

[0147]

[0148] Among them, k1, k2, and k3 are all non-negative coefficients, and the sum of the three non-negative coefficients is 1. crit t crit h crit Each cell represents the maximum non-uniformity factor that allows the robot to pass through each grid cell. This is a critical value that could cause the robot to tip over or get stuck. If any of the danger criteria exceeds its critical value, the corresponding cell is marked as untraversable.

[0149] The cost of traversability between two points of element i and j is:

[0150]

[0151] When performing land-air acceleration dynamics sampling, a terrain assessment method was additionally employed at the planner front end for ground modes. Each ground sampling node was assigned a traversability parameter τ. iIt is linearly represented by three terrain elements of slope, height, and sharpness and stores information in the effective passing plane. The traversability parameter is added to the dynamic sampling tree as a cost function, making it choose the ground with less traversability when driving.

[0152] Step 4: Parameterized trajectory optimization method.

[0153] After obtaining the initial acceleration dynamic sampling trajectory in step 2, this application uses MINCO as the planning backend to further optimize the trajectory with waypoints q and time T as parameters. This method is based on differential flatness, and the integral chain system of amphibious platforms is:

[0154]

[0155] where z [2] is the system input; is the first derivative of the system input; z T is the velocity of the system input; is the first derivative of the system input, i.e., acceleration; is the second derivative of the system input, i.e., jerk; is the system input; v i is the velocity.

[0156] The goal of the planner is to minimize the jerk, which reflects the change in acceleration. For amphibious platforms, this gives the integral chain system a physical meaning. It can reduce mechanical wear and tear, improve flight stability and tracking accuracy, while ensuring path continuity.

[0157] MINCO proves that there is a unique optimal trajectory in the unconstrained case, i.e., M segments of 2s-1 order polynomials. It gives the unique analytical solution to the unconstrained control variable minimization problem, and the solution complexity is O(M).

[0158] The coefficients of the MINCO trajectory can be obtained from q and T by linear complexity mapping The advantage is that given the cost function MINCO provides an efficient way to obtain and and and gradients, achieving efficient trajectory optimization given the penalty function. Then the three-dimensional point on the trajectory at time t can be represented as:

[0159] where, is the cost function, and is the gradient of the cost function with respect to the coefficient matrix c and T, and These are the gradients of the cost function with respect to the MINCO parameters q and T, respectively. The three-dimensional trajectory point at time t is the relative optimization parameters q and T.

[0160] Finally, the problem becomes solving an unconstrained optimization problem, which is solved iteratively using the quasi-Newton method L-BFGS.

[0161]

[0162] Therefore, based on the mission requirements of the integrated air-ground movement of amphibious platforms, functions are designed to regulate smoothness, penalize collisions, and limit dynamic infeasibility.

[0163] 1) Total time cost J t To accelerate the sampling process, the total time is minimized as follows:

[0164] J t =sum(T).

[0165] 2) Smoothness cost J s The smoothness penalty is defined as the integral of the square of the cubic derivative of the position:

[0166]

[0167] 3) Obstacle avoidance cost J c Although the initial trajectory is feasible for obstacle avoidance, a collision cost function is still used to penalize nodes that are too close to obstacles in order to increase the safety threshold.

[0168]

[0169] 4) Cost of dynamic feasibility. The magnitudes of the derivatives of the trajectory (velocity, acceleration, jerk) are restricted to ensure that the trajectory meets the dynamic characteristics.

[0170]

[0171] The functional expression of the dynamic cost is:

[0172] J f =J v +J a +J j .

[0173] Among them; J v The dynamic cost corresponding to velocity, For t i The first derivative of the position at time, v max Let v be the maximum velocity, and ‖v‖ be the norm of the velocity; J a The dynamic cost corresponding to acceleration, is the second derivative of the position at time t i is the maximum acceleration, ||a|| is the norm of the acceleration; J max is the maximum jerk, ||j|| is the norm of the jerk; J j is the jerk corresponding dynamics cost, is the second derivative of the position at time t i is the third derivative of the position at time t max is the maximum jerk, ||j|| is the norm of the jerk; J f is the dynamics cost.

[0174] v max , a max , j max are the maximum allowed values for velocity, acceleration, jerk respectively. For amphibious platforms, the following formulas are satisfied: v max_air > v max_ground , a max_air > a max_ground , j max_air > j max_ground so that the values of dynamic feasibility in the air are greater, obtaining greater maneuverability.

[0175] Step 5: Simulation test of the amphibious planning method.

[0176] The present application verifies the ground passability of the amphibious platform in four types of terrain and evaluates the algorithm performance by comparing two hierarchical planners in the air-ground simulation.

[0177] (1) Comparison and analysis of terrain planning.

[0178] In the simulation test of the dynamics sampling planning involving terrain, four types of unstructured terrain are defined, including unevenness, roughness, dispersion sharpness, and undulation, to qualitatively compare the feasibility of the planning. The filter parameters are set to focus on the vehicle ground clearance and the maximum vehicle pitch angle, and 20 simulations are independently run in each 25mx 25m terrain map type.

[0179] Figure 7 The simulation results of the dynamics planning involving terrain are shown, where, Figure 7 the part a of the figure, Figure 7 the part c of the figure, Figure 7 the part e of the figure, and Figure 7 the part g of the figure show the random three-dimensional terrain types generated by the OpenSimplex algorithm, with blue indicating low elevation and red indicating high elevation. OpenSimplex is an n-dimensional noise generation function designed by Ken Perlin to address the inherent limitations of classic noise functions, particularly when applied to higher-dimensional spaces. Figure 7 the part b of the figure, Figure 7 the part d of the figure,Figure 7 part f in and Figure 8 The h part in the image shows the coordinates on the map from the starting point (-10, -10, z). start The coordinates of the target point are (10, 10, z). target The red path has the lowest cost value. The blue square planes on the path are the effective passage planes determined by the vehicle dimensions.

[0180] (2) Comparison and analysis of land and air planning.

[0181] The integrated land-air planning simulation was conducted on a 25m×25m×5m map containing 60 random pillars, a fixed wall that can only be flown over, and small steps. All calculations were performed on a 2GHz processor and a 100TOPS Jetson OrinNX. For collision detection, an occupancy grid map with a resolution of 0.1m was used, where the obstacle bloat was half the width of the amphibious platform, i.e., 0.3m.

[0182] The same air-to-ground velocity and acceleration thresholds were compared with the proposed differential thresholds. Specifically, v was set... max_air =v max_ground =3m / s, a max_air =a max_ground =2m / s 2 Corresponding to this v max_air =6m / s, v max_ground =3m / s, a max_air =5m / s 2 a max_air =a max_ground =2m / s 2 Given the same starting point (-10, 0, 0) and target point (10, 0, 0), an amphibious platform planned a land-air trajectory that could avoid obstacles and fly over walls. During the simulation, each algorithm was run independently 20 times. The velocity range corresponding to the same X-axis point was counted, and a set of results was selected, such as... Figure 9 As shown in the figure. The results demonstrate that the algorithm proposed in this application has greater mobility and a shorter total running time.

[0183] The proposed planning strategy was compared with the TABV two-layer motion planner, and it was run independently 50 times. The statistical results are shown in Table 1. Method The trajectory diagrams in the diagrams show that the proposed new hierarchical cross-domain planner can find trajectories with comparable success rates, shorter planning and movement times, and a greater tendency to trajectories to the ground.

[0184] Table 1 Comparison Results of Land-Air Amphibious Planners

[0185] Move time (s) Planning time (ms) Success rate (%) Method of the present application TABV double layer motion planner 11.92 2.08 98 Figure 10 12.14 2.60 98

[0186] A hierarchical land-air amphibious planning method is proposed in this paper, which contains accelerated dynamics sampling and parameterized trajectory optimization. Based on the characteristics of land-air mode, terrain analysis is necessary when the platform is on the ground, and the concept of effective crossing plane is introduced to improve the passability in the planning layer; Differentiated air-ground constraints give the air more dynamic boundaries, fully exerting the mode characteristics of high maneuverability in the air.

[0187] ​ For the autonomous navigation architecture of amphibious platforms, the planning module based on mode characteristics is focused on. The perception, planning, and control modules run in parallel using onboard sensing and computing resources. The laser radar detects the surrounding environment in real time and returns point cloud data. Then, the perception module constructs a local grid map based on the point cloud and outputs the pose estimate. Next, the data is imported into the planning module, which generates and parameterizes the optimization of three-dimensional trajectories based on accelerated dynamics RRT*. In particular, the module judges the air-ground mode, performs terrain analysis on the ground, and considers differentiated dynamics constraints. Finally, the trajectory server provides control input for the platform, driving the duct and track motors respectively.

Claims

1. A trajectory planning method for amphibious platforms based on modal characteristics, characterized in that, Applications in tracked ducted amphibious platforms; The tracked ducted amphibious platform comprises: an integrated composite material fuselage, a tracked running gear, and a ducted flight mechanism; the method comprises: Acquire information data; the information data includes: position and orientation information and environmental information; Based on the aforementioned information data, a dynamic model of the land-air amphibious platform is constructed. The dynamic model includes: a tracked differential kinematic model and a ducted flight dynamic model. The tracked differential kinematic model is a mathematical model constructed based on the application scenario when the vehicle speed is lower than the set vehicle speed. The ducted flight dynamic model is a Newton-Euler integrated nonlinear dynamic model established based on the structural design of the ducted flight mechanism, which is regarded as a single rigid body with six degrees of freedom. Based on the aforementioned dynamic model, and according to the differential flatness characteristics and air-to-ground modal characteristics, land-to-air acceleration dynamics sampling is performed to obtain sampled trajectory data; the modes include: ground mode and air mode; The quasi-Newton method is used to iteratively solve the sampled trajectory data to optimize the air-to-ground differentiated trajectory, resulting in optimized trajectory data. The optimized trajectory data is then used to plan the trajectory of the amphibious platform.

2. The trajectory planning method for amphibious platforms based on modal characteristics according to claim 1, characterized in that, The expression for the tracked differential kinematic model specifically includes: in, The derivative of the position of the center of the amphibious platform along the X-axis; The derivative of the position of the center of the amphibious platform along the Y-axis; The derivative of the position of the center of the amphibious platform along the Z-axis; v is the derivative of the heading angle of the amphibious platform; l v is the speed of the left track of the tracked traveling mechanism. r B is the speed of the right track of the tracked travel mechanism; B is the track center distance. This is the heading angle.

3. The trajectory planning method for amphibious platforms based on modal characteristics according to claim 1, characterized in that, The expression for the ducted jet flight dynamics model is: in, v is the derivative of the three-dimensional position of the ducted flight mechanism; a m is the velocity of the ducted jet mechanism; m is the mass. ρ is the derivative of the velocity of the ducted flight mechanism; g is the acceleration due to gravity; e3 is the unit vector; F t Power generated by the ducted fan; M t The torque generated by the ducted fan; F d External interference force; M d External disturbance torque; R is the derivative of the attitude matrix of the ducted flight mechanism; a For the attitude matrix of the ducted flight mechanism; ωa is the estimated angular velocity of the ducted flight mechanism; J is the inertia matrix. Let be the derivative of the angular velocity of the ducted flight mechanism.

4. The trajectory planning method for amphibious platforms based on modal characteristics according to claim 1, characterized in that, Based on the aforementioned dynamic model, and according to the differential flatness characteristics and air-to-ground modal characteristics, land-to-air acceleration dynamics sampling is performed to obtain sampled trajectory data, specifically including: Determine the traversability parameters of the ground mode; the traversability parameters are linear representations of three terrain elements: slope, height, and sharpness; The traversability cost function is determined based on the traversability parameters and added to the sampling tree; the sampling tree is a model based on the dynamic model, which uses the differential flatness property to decouple and plan in the flat output dimension. The sampling tree is used to perform land-air acceleration dynamics sampling based on the dynamic model to obtain sampling trajectory data.

5. The trajectory planning method for amphibious platforms based on modal characteristics according to claim 4, characterized in that, Based on the aforementioned dynamic model, a sampling tree is used to perform land-air acceleration dynamics sampling to obtain sampled trajectory data, specifically including: Determine the state parameters; the state parameters include: the initial state and the target state; Accelerated search sampling is performed in the state space based on the triple integrator model to obtain random nodes, and node transition is performed based on the node state transition cost function to obtain the transitioned nodes; the node state transition cost function is constructed using a time-energy optimal quadratic form with the goal of minimizing energy cost and time consumption. The motion state is determined based on the height difference between two adjacent transferred nodes; the motion state is takeoff, landing, or the same. Based on the described motion state, dynamic extension is performed to obtain new nodes; Based on the collision detection function, it is determined whether the connecting edge between the new node and the transferred node collides with an obstacle, and a first judgment result is obtained; If the first judgment result is yes, then return "Accelerate search sampling in the state space based on the triple integrator model to obtain random nodes, and perform node transition based on the node state transition cost function to obtain the transitioned nodes"; If the first judgment result is negative, the new node is added to the sampling tree, and multiple potential parent nodes are searched, rewiring and pruning are performed to obtain the sampling expansion tree; Based on the state transition cost corresponding to the connection edge of each node in the sampling extension tree, it is determined whether the target state has been reached, and a second determination result is obtained. If the second judgment result is yes, then the sampling trajectory data is obtained by backtracking from the parent node; If the second judgment result is negative, then it is determined whether the set iteration number threshold has been reached, and a third judgment result is obtained; If the third judgment result is negative, then return "Accelerate the search sampling in the state space based on the triple integrator model to obtain random nodes, and perform node transition based on the node state transition cost function to obtain the transitioned nodes".

6. The trajectory planning method for amphibious platforms based on modal characteristics according to claim 5, characterized in that, When the motion state is during takeoff, the expression that is satisfied is: When the motion state is descent, the expression that is satisfied is: When the motion states are the same, the expression that is satisfied is: Among them, z cur z is the height of the current node; rand The height of the next random sampling node; Flag state_switch This is a flag for state transitions; Reward switch Convert node reward value; Penalty takeoff The penalty value for takeoff; Reward landing This is the landing bonus value.

7. The trajectory planning method for amphibious platforms based on modal characteristics according to claim 5, characterized in that, The calculation method for the nodes after the transition includes: in, The cost function for the sampling trajectory; ρ is the cost function; s(t) is the sampling trajectory; ρ is the scaling parameter; u(t) is the control input; A is an n×n matrix; B is an n×m matrix; s is the derivative of the sampling trajectory; s(0) is the initial state at time 0; s start The initial state is s; s(τ) represents the target state at time τ; s target τ represents the target state; τ represents the duration; T represents the transpose symbol; S free For state space; For input space.

8. The trajectory planning method for amphibious platforms based on modal characteristics according to claim 1, characterized in that, Using the quasi-Newton method, the solution is obtained iteratively based on the sampled trajectory data, and the corresponding expression is: Where q is the waypoint; T is time; λ x Weights assigned to the different components x of the cost function; J x For different components x, there are penalty function terms.

9. The trajectory planning method for amphibious platforms based on modal characteristics according to claim 8, characterized in that, The penalty function term includes: total time cost, smoothness cost, obstacle avoidance cost, and dynamic cost.

10. The trajectory planning method for amphibious platforms based on modal characteristics according to claim 9, characterized in that, The expression for the total time cost is: J t =sum(T); The expression for the smoothness cost is: The expression for the obstacle avoidance cost is: The expression for the dynamic cost is: J f *J v +J a +J j ; Among them, J t J represents the total time cost; T represents time; J represents the total time cost. s For the sake of smoothness; p(t) is the third derivative of the position; t is the final time; t0 is the initial time; p(t) is the position; s is the state; c is the cost; J c For the cost of obstacle avoidance; J f For the cost of kinetics; J v The dynamic cost corresponding to velocity; J a The dynamic cost corresponding to acceleration; J j The dynamic cost of adding jerk.

Images