A path planning method and system based on differential flatness of a carrier arm moving platform

Abstract

The application discloses a path planning method and system of a load arm moving platform based on differential flatness, and relates to the technical field of path planning of a load arm unmanned aerial vehicle.The technical points of the application comprise: establishing a system model of the load arm unmanned aerial vehicle system; using an Informed-RRT* algorithm to obtain path gradually optimal space sampling points of the load arm unmanned aerial vehicle; and using a Minimum Snap algorithm of closed-form solution to obtain a smooth motion trajectory conforming to the dynamics constraint of the load arm unmanned aerial vehicle.The application proves the differential flatness of the load arm unmanned aerial vehicle system, avoids decoupling of the kinematic model thereof, plans a base unmanned aerial vehicle and an airborne mechanical arm of the load arm unmanned aerial vehicle integrally, avoids the situation that global optimization cannot be simultaneously achieved in multi-objective optimization, improves the ease of use of the load arm unmanned aerial vehicle platform, and improves the energy utilization efficiency of the load arm unmanned aerial vehicle system with total energy tension.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning for an arm-carrying unmanned aerial vehicle (UAV), and in particular to a path planning method and system for an arm-carrying mobile platform based on differential flatness. Background Art

[0002] Differential flatness is a special property of control systems, a manifestation of "controllability" in nonlinear control systems. The input of a system with differential flatness can be precisely calculated from the known, desired output signal. This is a desirable property in motion planning for drones and other robotics. For systems with differential flatness, controller design based on complex dynamic models can be avoided. Instead, a smooth motion path can be planned and the input control variable can be directly calculated from that path.

[0003] The InformedRRT* (Informed Rapidly-exploring Random Tree Star) algorithm is a path planning algorithm designed to improve the efficiency of path optimization. It improves on the RRT algorithm by limiting the sampling range to accelerate the process of finding the optimal path. After finding an initial path, Informed RRT restricts the sampling range to an elliptical region. The foci of this ellipse are the start and end points, the major axis is the length of the current path, and the minor axis is the straight-line distance between the start and end points. The purpose of this elliptical sampling is to reduce invalid sampling points, thereby more quickly finding a more optimal path. As shorter paths are found, the major axis of the ellipse gradually shortens, further concentrating the sampling points. InformedRRT inherits the asymptotic optimality of RRT, meaning that the found path gradually approaches the optimal path over time and with more sampling points. By concentrating sampling within the region of potential optimal paths, Informed RRT significantly improves the efficiency of path optimization and reduces computational time. This makes InformedRRT particularly suitable for path planning problems in high-dimensional spaces, such as robot navigation, autonomous driving, and drone path planning.

[0004] The closed-form Minimum Snap algorithm is an optimization technique for generating smooth trajectories, particularly suitable for systems requiring high-precision motion control, such as drones and robots. The algorithm aims to generate a trajectory that satisfies dynamic constraints through polynomial fitting, with high-order continuity in position, velocity, acceleration, and other parameters. This closed-form Minimum Snap algorithm can generate smooth, dynamically constrained trajectories, making it widely applicable in drones, robotics, and other fields. In specific applications, the Minimum Snap algorithm not only generates smooth trajectories but also ensures their dynamic feasibility, thereby improving system stability and control accuracy.

[0005] However, it is difficult to decouple the model in the existing arm-carrying UAV motion planning method. The traditional decoupled planning algorithm requires parallel computing, which leads to excessive computational pressure on the airborne computing platform and thus reduces the response performance of the arm-carrying UAV. Summary of the Invention

[0006] In view of the above problems, the present invention proposes a path planning method and system for a mobile platform of an arm based on differential flatness.

[0007] According to one aspect of the present invention, a path planning method for a mobile platform of an arm based on differential flatness is proposed, the method comprising:

[0008] Establish a system model of the arm-carrying UAV system;

[0009] The Informed-RRT* algorithm is used to obtain the path asymptotically optimal spatial sampling points of the arm-carrying UAV;

[0010] The closed-form Minimum Snap algorithm is used to obtain a smooth motion trajectory that complies with the dynamic constraints of the UAV.

[0011] Furthermore, the system model of the arm-carrying drone system is established as follows:

[0012]

[0013] Where, is the state variable of the system, p, l are the generalized momentum and angular momentum of the system respectively; φ, θ, ψ are the roll angle, pitch angle and yaw angle respectively; η is the vector composed of the joint angles of the manipulator; represents the control input of the system, represents the torque of each joint of the robotic arm, T represents the thrust generated by the multi-rotor drone, τ φ ,τ θ ,τ ψ They represent roll, pitch, and yaw torques respectively; f(q) represents the drift vector function; represents the driving matrix function.

[0014] Furthermore, the arm-carrying drone system has a differential flatness property, which is specifically: except at the singular point And outside T=0, vector is the flat output of the UAV system, where Represents the rotation matrix from the ground coordinate system to the base coordinate system.

[0015] Furthermore, the use of the Informed-RRT* algorithm to obtain the path asymptotically optimal spatial sampling points of the arm-carrying UAV includes:

[0016] Initialize the definition of the starting point, target point, step size, maximum number of iterations, an empty search tree, and use the starting point as the root node of the tree; calculate the straight-line distance c between the starting point and the target point min , initialize an elliptical search space with the starting point and the end point as the focus, and the major axis of the ellipse is the straight line distance c min ;

[0017] In each iteration, the following steps are performed in order:

[0018] Sampling: Randomly sample a point x in the elliptical search space rand ;

[0019] Find the closest point: find the distance x in the search tree rand The nearest node x near ;

[0020] Connection: From node x near To node x rand Direction extension step, get the new node x new ; Check node x near and the new node x new Whether the path between them collides with an obstacle, if not, the new node x new Add to the search tree and set node x near For the new node x new The parent node of

[0021] Reconnect: with new node x new As the center, search the nodes in the tree within a certain range; for each node x′ found near , calculate the distance from the starting point x0 to the node x new Then to the new node x′ near The path cost c(x0,x new )+c(x new ,x′ near ); If the new path cost is less than the node x′ near The original path cost c(x0,x′ near ), then update the node x′ near The parent node is the new node x new , and update the connection relationship of the search tree T; where c(a,b) is the cost function, defined as the Euclidean distance between points a and b;

[0022] Check target: If the new node x new If the distance to the target point is less than the set threshold, the algorithm ends and the found path is returned;

[0023] The iteration terminates until the maximum number of iterations is reached or a feasible path is found.

[0024] Furthermore, the closed-form Minimum Snap algorithm is used to obtain a smooth motion trajectory that meets the dynamic constraints of the UAV, including:

[0025] The multiple sampling points obtained by the Informed-RRT* algorithm are input into the closed-form Minimum Snap algorithm to determine the order of the trajectory. A quintic polynomial is selected, and a constraint matrix is ​​constructed based on the time allocation. The position information, velocity information, and acceleration information of the start and end points of each trajectory are converted into matrix form. A mapping matrix is ​​constructed to convert the continuity constraints into matrix form. A permutation matrix is ​​constructed to separate the known constraints from the unknown free variables. The polynomial coefficients of each trajectory segment are solved through matrix operations to generate the final trajectory.

[0026] Furthermore, the specific process of using the closed-form Minimum Snap algorithm to obtain a smooth motion trajectory that meets the dynamic constraints of the arm-carrying drone includes:

[0027] The trajectory equation of the resulting piecewise polynomial trajectory that passes through known sampling points and satisfies the constraints is written as:

[0028]

[0029] Among them, p j,i ,j=1,…,M,i=1,…,N are polynomial coefficients, t represents the current moment, and each trajectory f1, f2,…, f M are all polynomial trajectories, T0,…,T M The arrival time of the corresponding sampling point is known, M is the number of sampling points; N represents the degree of the polynomial trajectory;

[0030] The following differential constraints are satisfied at the start and end points of each segment trajectory:

[0031] A m P m =d m

[0032] Among them, A m is the constraint matrix, d m is the value of the specific constraint; P m represents the polynomial coefficients of the mth trajectory;

[0033] The two polynomial trajectories before and after each sampling point satisfy the following continuity and smoothness constraints:

[0034]

[0035] The cost function is:

[0036]

[0037] in:

[0038]

[0039] Then the constrained QP problem is:

[0040]

[0041] Free variable d in separation constraint F and determine the variable d P , transform the above constrained QP problem into the following unconstrained QP problem:

[0042]

[0043] Among them, C is defined from R=CA -T QA -1 C T ;

[0044] About J P The extreme point obtained by derivation is the global optimal point Will Substituting back C and A, we can obtain the final global optimal polynomial coefficients: Substituting P back into the polynomial trajectory equation yields a smooth trajectory that satisfies the constraints.

[0045] According to another aspect of the present invention, a path planning system for a carrier arm mobile platform based on differential flatness is proposed, the system comprising:

[0046] a model building module configured to build a system model of the arm-carrying drone system;

[0047] an optimal sampling point calculation module configured to obtain the path asymptotically optimal spatial sampling points of the arm-carrying UAV using an Informed-RRT* algorithm;

[0048] The trajectory generation module is configured to use a closed-form Minimum Snap algorithm to obtain a smooth motion trajectory that complies with the dynamic constraints of the arm-carrying drone.

[0049] Furthermore, the system model of the arm-carrying drone system in the model building module is established as follows:

[0050]

[0051] Where, is the state variable of the system, p, l are the generalized momentum and angular momentum of the system respectively; φ, θ, ψ are the roll angle, pitch angle and yaw angle respectively; η is the vector composed of the joint angles of the manipulator; represents the control input of the system, represents the torque of each joint of the robotic arm, T represents the thrust generated by the multi-rotor drone, τ φ ,τ θ ,τ ψ They represent roll, pitch, and yaw torques respectively; f(q) represents the drift vector function; represents the driving matrix function;

[0052] The arm-carrying drone system has a differential flatness property, which is specifically: except at the singular point And outside T=0, vector is the flat output of the UAV system, where Represents the rotation matrix from the ground coordinate system to the base coordinate system.

[0053] Furthermore, the optimal sampling point calculation module uses the Informed-RRT* algorithm to obtain the path asymptotically optimal spatial sampling points of the arm-carrying UAV, including:

[0054] Initialize the definition of the starting point, target point, step size, maximum number of iterations, an empty search tree, and use the starting point as the root node of the tree; calculate the straight-line distance c between the starting point and the target point min , initialize an elliptical search space with the starting point and the end point as the focus, and the major axis of the ellipse is the straight line distance c min ;

[0055] In each iteration, the following steps are performed in order:

[0056] Sampling: Randomly sample a point x in the elliptical search space rand ;

[0057] Find the closest point: find the distance x in the search tree rand The nearest node x near ;

[0058] Connection: From node x near To node x rand Direction extension step, get the new node x new ; Check node x near and the new node x new Whether the path between them collides with an obstacle, if not, the new node x new Add to the search tree and set node x near For the new node xnew The parent node of

[0059] Reconnect: with new node x new As the center, search the nodes in the tree within a certain range; for each node x′ found near , calculate the distance from the starting point x0 to the node x new Then to the new node x′ near The path cost c(x0,x new )+c(x new ,x′ near ); If the new path cost is less than the node x′ near The original path cost c(x0,x′ near ), then update the node x′ near The parent node is the new node x new , and update the connection relationship of the search tree T; where c(a,b) is the cost function, defined as the Euclidean distance between points a and b;

[0060] Check target: If the new node x new If the distance to the target point is less than the set threshold, the algorithm ends and the found path is returned;

[0061] The iteration terminates until the maximum number of iterations is reached or a feasible path is found.

[0062] Furthermore, the closed-form Minimum Snap algorithm used in the trajectory generation module to obtain a smooth motion trajectory that complies with the dynamic constraints of the arm-carrying drone includes:

[0063] The multiple sampling points obtained by the Informed-RRT* algorithm are input into the closed-form Minimum Snap algorithm to determine the order of the trajectory. A quintic polynomial is selected and a constraint matrix is ​​constructed based on the time allocation. The position information, velocity information, and acceleration information of the start and end points of each trajectory are converted into a matrix form. A mapping matrix is ​​constructed to convert the continuity constraints into a matrix form. A permutation matrix is ​​constructed to separate the known constraints from the unknown free variables. The polynomial coefficients of each trajectory segment are solved through matrix operations to generate the final trajectory. The specific process includes:

[0064] The trajectory equation of the resulting piecewise polynomial trajectory that passes through known sampling points and satisfies the constraints is written as:

[0065]

[0066] Among them, p j,i ,j=1,…,M,i=1,…,N are polynomial coefficients, t represents the current moment, and each trajectory f1, f2,…, f Mare all polynomial trajectories, T0,…,T M The arrival time of the corresponding sampling point is known, M is the number of sampling points; N represents the degree of the polynomial trajectory;

[0067] The following differential constraints are satisfied at the start and end points of each segment trajectory:

[0068] A m P m =d m

[0069] Among them, A m is the constraint matrix, d m is the value of the specific constraint; P m represents the polynomial coefficients of the mth trajectory;

[0070] The two polynomial trajectories before and after each sampling point satisfy the following continuity and smoothness constraints:

[0071]

[0072] The cost function is:

[0073]

[0074] in:

[0075]

[0076]

[0077] Then the constrained QP problem is:

[0078]

[0079] Free variable d in separation constraint F and determine the variable d P , transform the above constrained QP problem into the following unconstrained QP problem:

[0080]

[0081] Among them, C is defined from R=CA -T QA -1 C T ;

[0082] About J P The extreme point obtained by derivation is the global optimal point Will Substituting back C and A, we can obtain the final global optimal polynomial coefficients: Substituting P back into the polynomial trajectory equation yields a smooth trajectory that satisfies the constraints.

[0083] The beneficial technical effects of the present invention are:

[0084] To address the difficulty of model decoupling in arm-carrying UAV motion planning algorithms, and the need for parallel computing in traditional decoupled planning algorithms, which increases the computational pressure on the onboard computing platform and reduces the responsiveness of the arm-carrying UAV, the present invention proposes a motion planning method for arm-carrying UAVs based on the differential flatness property of the arm-carrying UAV, using the Informed-RRT* algorithm to obtain sampling points, and using a closed-form Minimum Snap algorithm to obtain a smooth and executable trajectory. Specifically, the present invention has the following beneficial effects:

[0085] 1) The differential flatness characteristic of the arm-carrying UAV system is proved, which avoids the decoupling of its kinematic model, making it easier and more accurate to solve the problems of motion planning, control, and decision-making on the arm-carrying UAV platform, and improving the usability of the arm-carrying UAV platform.

[0086] 2) The Informed-RTT* algorithm is used in high-dimensional space to integrate the base drone and the onboard robotic arm of the arm-carrying drone and optimize the planning as a whole to ensure the lowest overall energy consumption, avoid the situation where the global optimal state cannot be achieved simultaneously during multi-objective optimization, and improve the energy utilization efficiency of the arm-carrying drone system with limited total energy.

[0087] 3) The closed-form Minimum Snap algorithm is used to generate smooth trajectories, which improves numerical stability and avoids the situation where the solution cannot be found when approaching a singular point. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] The present invention can be better understood by referring to the description given below in conjunction with the accompanying drawings, which together with the following detailed description are included in this specification and form a part of this specification, and are used to further illustrate the preferred embodiments of the present invention and explain the principles and advantages of the present invention.

[0089] Figure 1 This is a flow chart of a path planning method for a carrier arm mobile platform based on differential flatness according to an embodiment of the present invention.

[0090] Figure 2 Schematic diagram of the structure of the arm-carrying UAV in an embodiment of the present invention.

[0091] Figure 3 4 is a flow chart of the Informed RRT* algorithm in an embodiment of the present invention.

[0092] Figure 4Schematic diagram of the simulation environment in an embodiment of the present invention.

[0093] Figure 5 It is a diagram showing the effect of running the simulation program in an embodiment of the present invention. DETAILED DESCRIPTION

[0094] In order to enable those skilled in the art to better understand the present invention, exemplary embodiments or examples of the present invention will be described below with reference to the accompanying drawings. Obviously, the described embodiments or examples are only some of the embodiments or examples of the present invention, and not all of them. Based on the embodiments or examples of the present invention, all other embodiments or examples obtained by those skilled in the art without creative work should fall within the scope of protection of the present invention.

[0095] The motion planning algorithm of traditional arm-carrying UAV considers the airborne manipulator and the base UAV as mutually coupled subsystems, and performs path planning separately after decoupling. This involves the decoupling derivation of the system and the parallel calculation of the path planning of the two. For complex multi-axis base UAVs and high-degree-of-freedom manipulators, decoupling is too difficult, and parallel computing requires higher computing power of the airborne computing platform, which seriously reduces the response time and usability of the system. The present invention proposes a path planning method and system for an arm-carrying mobile platform based on differential flatness, wherein a method for proving the differential flatness property of the arm-carrying UAV is proposed, and the asymptotically optimal spatial sampling points of the path are obtained by the informed-rrt* algorithm. Finally, the closed-form Minimum Snap algorithm is used to obtain a smooth, executable trajectory that meets the dynamic constraints of the arm-carrying UAV.

[0096] The embodiment of the present invention proposes a path planning method for a mobile platform of an arm based on differential flatness, such as Figure 1 As shown, the method includes:

[0097] S1. Establish the system model of the UAV system;

[0098] S2, using the Informed-RRT* algorithm to obtain the path asymptotically optimal spatial sampling points of the arm-carrying UAV;

[0099] S3. Use the closed-form Minimum Snap algorithm to obtain a smooth motion trajectory that conforms to the dynamic constraints of the UAV.

[0100] The method begins with S1, in which a system model of the arm-carrying drone system is established.

[0101] According to an embodiment of the present invention, the base aircraft of the arm-carrying drone is a multi-rotor drone. A four-degree-of-freedom all-wheel drive robotic arm is connected to the geometric center of the base aircraft. All joints of the robotic arm are revolute joints, and a gripper is connected to the end of the robotic arm. All components of the system are rigid. Reference frames are established at the ground, the geometric center of the multi-rotor drone, and the joints of the robotic arm. The mechanical structure and reference frame establishment method are as follows: Figure 2 shown.

[0102] The arm-carrying UAV has the following control affine form (i.e. system model):

[0103]

[0104] Where f(q) represents the drift vector function; represents the driving matrix function;

[0105]

[0106] The arm-carrying drone system has a differential flatness property, and the differential flatness property is specifically Theorem 1.

[0107] There is a strict definition of differential flatness as follows:

[0108] Definition 1. (Equivalent system): A system is called At are equivalent if there exists a smooth mapping Φ that maps the neighborhood of a to the neighborhood of b = Φ(a). This mapping Φ is also called an endogenous transformation. (Note: An endogenous transformation is a reversible transformation that "exchanges" trajectories between two systems. This leads to the formal definition of differential flatness.)

[0109] Definition 2. (Differentially flat system): A control system is called A system is differentially flat near a if and only if, in some neighborhood of a, it is equivalent to a trivial system.

[0110] Definition 3. (Flat Output): For a nonlinear system:

[0111]

[0112] Output is a flat output if:

[0113]

[0114]

[0115]

[0116] and all elements of y are independent in the differential sense, i.e. y does not satisfy any linear differential equation:

[0117]

[0118] Theorem 1. (Flat outputs of the load-carrying UAV system) For the load-carrying UAV system defined above, the vector is a flat output of the system except at the singularities and T = 0. Here, denotes the rotation matrix from the ground coordinate system to the base coordinate system.

[0119] Proof: For convenience, let Physically, the flat output consists of the momentum p e of the load-carrying UAV in the E frame, the yaw angle ψ, and the relative joint angle η. The differential independence among these outputs except at some singularities is obvious.

[0120] First, we express the only external force, the thrust T: Taking the differential of p e , we have

[0121]

[0122] Moving the first term on the right to the left, we take the 2-norm, and have

[0123]

[0124] From the definitions of the roll angle, the pitch angle, we have

[0125]

[0126]

[0127] The body angular velocity is The momentum p of the load-carrying UAV in the B frame can also be expressed as a function of the flat outputs and their differentials: From the connection equation, we have the velocity of the load-carrying UAV in the B frame:

[0128]

[0129] From the definition of the angular momentum, the angular momentum l of the load-carrying UAV and its differential can be expressed as a function of the flat outputs and their differentials with respect to time:

[0130]

[0131] Finally, we obtain a three-equation-three-unknown equation set for the roll-pitch-yaw torques:

[0132]

[0133] The torque input can be expressed as:

[0134]

[0135] in Obviously It is also a function of the flat output and its time derivative (up to third order). L It can be expressed as: At this point, the full state q and input u can be expressed as an algebraic function of the flat output σ and its various time derivatives.

[0136] Note: The only singularity in the proof occurs during free fall, i.e. Or equivalently, Tcosφcosθ=0. Therefore, the attitude angle of the UAV is restricted to avoid the singularity, that is, And the thrust is strictly positive, that is, T>0.

[0137] The equivalence between the above-mentioned UAV system with differential flatness and the trivial system is further proved in Theorem 2.

[0138] Theorem 2. (Differentially Flat Property) Equivalence to the Trivial System: Expand the state variables to include the thrust term: The corresponding control input then becomes: The new control affine form is given according to the expanded state variables and control inputs:

[0139]

[0140]

[0141] Since the original control input u is a function of the flat output and its highest third-order derivative, an auxiliary control input is constructed where (·) represents the order of the time derivative, which can be expressed as follows:

[0142]

[0143] in are the drift vector and the driving matrix respectively. By connecting the expanded dynamic equations, we can easily get: v , It can be expressed as q de function.

[0144] Thus we obtain the Brunovsky standard form of (1):

[0145]

[0146] In summary, the differential flatness property of the established quadrotor arm-carrying drone system model is obtained. With this differential flatness property, the control input for the arm-carrying drone's tracking path can be directly calculated from the path to be tracked, allowing spatial path planning to proceed.

[0147] Then, S2 is executed. In S2, the Informed-RRT* algorithm is used to obtain the asymptotically optimal spatial sampling points of the path of the arm-carrying UAV.

[0148] According to an embodiment of the present invention, a point is selected in space as the target point for the arm-carrying drone and the endpoint. The Informed-RRT* algorithm generates an initial path from the starting point to the endpoint and defines an initial elliptical region. The foci of this ellipse are the starting and ending points, the major axis is the length of the current path, and the minor axis is the straight-line distance between the starting and ending points. The purpose of ellipse sampling is to reduce invalid sampling points, thereby more quickly finding a more optimal path. New points are then randomly sampled within the elliptical region, added to the tree, and attempted to connect to existing nodes to form a new path. If a shorter path is found, the current optimal path is updated, and the size and shape of the ellipse are adjusted. This process is repeated until a termination criterion is met, such as reaching the maximum number of iterations or finding a sufficiently good path. After the algorithm terminates, it outputs a set of discrete sampling points. Connecting these sampling points in sequence with a broken line yields a simple path from the starting point to the endpoint.

[0149] The algorithm's workflow includes initialization, sampling, tree expansion, and path optimization. In the initialization phase, an initial path is generated from the starting point to the end point, and an initial elliptical area is defined. New points are then randomly sampled within the elliptical area, added to the tree, and attempted to connect to existing nodes to form a new path. If a shorter path is found, the current optimal path is updated, and the size and shape of the ellipse are adjusted. This process is repeated until the termination condition is met, such as reaching the maximum number of iterations or finding a sufficiently good path. See the flowchart of the algorithm for details. Figure 3 As shown, the pseudo code is as follows.

[0150]

[0151] Then S3 is executed. In S3, the closed-form Minimum Snap algorithm is used to obtain a smooth motion trajectory that meets the dynamic constraints of the arm-carrying drone.

[0152] According to an embodiment of the present invention, first, the Minimum Snap algorithm divides the entire trajectory into multiple segments, and each segment is represented by a fifth-order polynomial. The fifth-order polynomial is chosen because it can provide sufficient degrees of freedom to satisfy the continuity of position, velocity, acceleration, jerk, and snap. Specifically, assuming there is a segment of trajectory, its position function can be expressed as: P(t) = p0 + p1t + p2t 2 +p3t 3 +p4t 4 +p5t 5 , where p i , i = 1, …, 5 are the coefficients to be solved. Solving these coefficients requires at least six constraints, which typically include the position, velocity, and acceleration information of the trajectory's start and end points. Next, the entire trajectory is divided into multiple segments, and a polynomial fit is performed on each segment. To ensure the smoothness of the entire trajectory, continuity constraints are set at the connection points of each segment. These constraints include continuity of position, velocity, acceleration, jerk, and snap.

[0153] When formulating the optimization problem, these continuity constraints and the trajectory's dynamic constraints are transformed into a quadratic programming (QP) problem. The specific steps are as follows: First, determine the trajectory's order, select a quintic polynomial, and construct a constraint matrix based on the time allocation. Second, construct a constraint matrix to convert the position, velocity, and acceleration information of each trajectory's start and end points into matrix form. Then, construct a mapping matrix to convert the continuity constraints into matrix form, ensuring high-order continuity at the connection points of each trajectory. Next, construct a permutation matrix to separate the known constraints from the unknown free variables, and construct a permutation matrix to simplify the solution process. Finally, solve the polynomial coefficients for each trajectory segment through matrix operations to generate the final trajectory. The advantage of a closed-form solution is its computational efficiency, as it only requires matrix operations, without the need for a QP solver.

[0154] The Minimum Snap algorithm generates a piecewise polynomial trajectory that passes through known sampling points and satisfies the constraints. That is, its trajectory equation can be written as:

[0155]

[0156] Among them, p j,i ,j=1,…,M,i=1,…,N are polynomial coefficients, t represents the current moment, and each trajectory f1, f2,…, f M are all polynomial trajectories, T0,…,T M The arrival time of the corresponding sampling point is known, M is the number of sampling points; N represents the degree of the polynomial trajectory;

[0157] At the starting and ending points of each segmented trajectory, constraints such as position, velocity, and acceleration are satisfied, i.e., differential constraints:

[0158]

[0159] That is: A m P m =d m

[0160] The two polynomial trajectories before and after each sampling point satisfy the continuity and smoothness constraints:

[0161]

[0162] That is:

[0163] Among them, A m is the constraint matrix, d m is the value of the specific constraint; the specific values ​​of all the above k are related to the constraints in the actual problem.

[0164] To minimize the Snap of the entire polynomial trajectory, we need to minimize the square integral of the fourth-order derivative of f(t), that is, to obtain the cost function Minimum, where P m are the polynomial coefficients of the mth trajectory.

[0165] The overall cost function can be written as:

[0166]

[0167] in:

[0168]

[0169] In summary, we obtain the following constrained QP problem:

[0170]

[0171] Free variable d in separation constraint F and determine the variable d P , the above constrained QP problem can be transformed into the following unconstrained QP problem:

[0172]

[0173] The definition of C comes from R=CA -T QA -1 C T This formula is a convex function with a global minimum value, for J with respect to dP The extreme point of the derivative is the global optimal point. The polynomial coefficients when the derivative is solved to obtain the global optimal point are: in Substituting P back into the original equation yields a smooth trajectory that satisfies the constraints. This approach improves numerical stability and avoids the problem of being unable to find a solution when approaching a singular point.

[0174] Figure 4 A schematic diagram of the simulation environment is given. Figure 5 The simulation program running effect diagram is given.

[0175] Another embodiment of the present invention provides a path planning system for a mobile platform of an arm based on differential flatness, the system comprising:

[0176] a model building module configured to build a system model of the arm-carrying drone system;

[0177] an optimal sampling point calculation module configured to obtain the path asymptotically optimal spatial sampling points of the arm-carrying UAV using an Informed-RRT* algorithm;

[0178] The trajectory generation module is configured to use a closed-form Minimum Snap algorithm to obtain a smooth motion trajectory that complies with the dynamic constraints of the arm-carrying drone.

[0179] In this embodiment, optionally, the system model of the arm-carrying drone system in the model building module is established as follows:

[0180]

[0181] Where, is the state variable of the system, p, l are the generalized momentum and angular momentum of the system respectively; φ, θ, ψ are the roll angle, pitch angle and yaw angle respectively; η is the vector composed of the joint angles of the manipulator; represents the control input of the system, represents the torque of each joint of the robotic arm, T represents the thrust generated by the multi-rotor drone, τ φ ,τ θ ,τ ψ They represent roll, pitch, and yaw torques respectively; f(q) represents the drift vector function; represents the driving matrix function;

[0182]

[0183] The arm-carrying drone system has a differential flatness property, which is specifically: except at the singular point And outside T=0, vector is the flat output of the UAV system, where Represents the rotation matrix from the ground coordinate system to the base coordinate system.

[0184] In this embodiment, optionally, the optimal sampling point calculation module uses the Informed-RRT* algorithm to obtain the path asymptotically optimal spatial sampling points of the arm-carrying UAV, including:

[0185] Initialize the definition of the starting point, target point, step size, maximum number of iterations, an empty search tree, and use the starting point as the root node of the tree; calculate the straight-line distance c between the starting point and the target point min , initialize an elliptical search space with the starting point and the end point as the focus, and the major axis of the ellipse is the straight line distance c min ;

[0186] In each iteration, the following steps are performed in order:

[0187] Sampling: Randomly sample a point x in the elliptical search space rand ;

[0188] Find the closest point: find the distance x in the search tree rand The nearest node x near ;

[0189] Connection: From node x near To node x rand Direction extension step, get the new node x new ; Check node x near and the new node x new Whether the path between them collides with an obstacle, if not, the new node x new Add to the search tree and set node x near For the new node x new The parent node of

[0190] Reconnect: with new node x new As the center, search the nodes in the tree within a certain range; for each node x′ found near , calculate the distance from the starting point x0 to the node x new Then to the new node x′ near The path cost c(x0,x new )+c(x new ,x′ near ); If the new path cost is less than the node x′ near The original path cost c(x0,x′ near ), then update the node x′ near The parent node is the new node x newand update the connection of search tree T; wherein c(a, b) is a cost function defined as the Euclidean distance between a and b;

[0191] Check target: if the new node x new If the distance between the target point and the new node is less than a set threshold, the algorithm ends and returns the found path.

[0192] The iteration ends until the maximum number of iterations is reached or a feasible path is found.

[0193] In this embodiment, optionally, the Minimum Snap algorithm using closed-form solution in the trajectory generation module obtains a smooth motion trajectory that meets the dynamics constraints of the carrier arm unmanned aerial vehicle, including:

[0194] Inputting the multiple sampling points obtained by the Informed-RRT* algorithm into the Minimum Snap algorithm using closed-form solution, determining the order of the trajectory, selecting a quintic polynomial, and constructing a constraint matrix according to time allocation to convert the position information, velocity information, and acceleration information of the start point and end point of each trajectory into a matrix form; constructing a mapping matrix to convert the continuity constraints into a matrix form; constructing a permutation matrix to separate the known constraints and unknown free variables; solving the polynomial coefficients of each trajectory through matrix operation to generate the final trajectory; the specific process includes:

[0195] The trajectory equation of the final generated segmented polynomial trajectory that passes through the known sampling points and meets the constraints is written as:

[0196]

[0197] wherein p j,i , j = 1, …, M, i = 1, …, N are polynomial coefficients, t represents the current time, each trajectory f1, f2, …, f M is a polynomial trajectory, T0, …, T M are known arrival times of the corresponding sampling points, M is the number of sampling points; N represents the degree of the polynomial trajectory.

[0198] At the start point and end point of each segmented trajectory, the following differential constraints are met:

[0199] A m P m = d m

[0200] wherein A m is a constraint matrix, d m is the value of a specific constraint; P m represents the polynomial coefficients of the mth trajectory.

[0201] The two polynomial trajectories before and after each sampling point satisfy the following continuity and smoothness constraints:

[0202]

[0203] The cost function is:

[0204]

[0205] Wherein:

[0206]

[0207] The QP problem with constraints is:

[0208]

[0209] Separate the free variable d in the constraint F And determine the variable d P The above QP problem with constraints is converted into the following unconstrained QP problem:

[0210]

[0211] Wherein, C is defined as R = CA -T QA -1 C T ;

[0212] The extreme point obtained by taking the derivative of J with respect to d P is the global optimal point Substitute C and A back into the global optimal point to obtain the final global optimal polynomial coefficients: Substitute P back into the polynomial trajectory equation to obtain the smooth trajectory that satisfies the constraints.

[0213] The function of the path planning system of the differential flatness-based carrier arm mobile platform according to an embodiment of the application can be described by the aforementioned path planning method of the differential flatness-based carrier arm mobile platform, and therefore the system embodiment is not described in detail, and reference can be made to the method embodiment described above, and will not be described herein again.

[0214] Although the application is described according to a limited number of embodiments, those skilled in the art, with the benefit of the above description, understand that other embodiments can be conceived within the scope of the application described herein. The disclosure of the application is illustrative rather than limiting, and the scope of the application is defined by the appended claims.

Claims

1. A path planning method for a mobile platform based on differential flatness, characterized in that: include: The system model of the arm-carrying UAV system is as follows: Where, is the state variable of the system, p, l are the generalized momentum and angular momentum of the system respectively; φ, θ, ψ are the roll angle, pitch angle and yaw angle respectively; η is the vector composed of the joint angles of the manipulator; represents the control input of the system, represents the torque of each joint of the robotic arm, T represents the thrust generated by the multi-rotor drone, τ φ ,τ θ ,τ ψ They represent roll, pitch, and yaw torques respectively; f(q) represents the drift vector function; represents the driving matrix function; The Informed-RRT* algorithm is used to obtain the optimal spatial sampling points of the path of the UAV, including: Initialize the definition of the starting point, target point, step size, maximum number of iterations, an empty search tree, and use the starting point as the root node of the tree; calculate the straight-line distance c between the starting point and the target point min , initialize an elliptical search space with the starting point and the end point as the focus, and the major axis of the ellipse is the straight line distance c min ; In each iteration, the following steps are performed in order: Sampling: Randomly sample a point x in the elliptical search space rand ; Find the closest point: find the distance x in the search tree rand The nearest node x near ; Connection: From node x near To node x rand Direction extension step, get the new node x new ; Check node x near and the new node x new Whether the path between them collides with an obstacle, if not, the new node x new Add to the search tree and set node x near For the new node x new The parent node of Reconnect: with new node x new As the center, search for nodes in the tree within a certain range; for each node x' near , calculate the distance from the starting point x0 to the node x new Then to the new node x' near The path cost c(x0,x new )+c(x new ,x' near ); If the new path cost is less than the node x' near The original path cost c(x0,x' near ), then update the node x' near The parent node is the new node x new , and update the connection relationship of the search tree T; where c(a,b) is the cost function, defined as the Euclidean distance between points a and b; Check target: If the new node x new If the distance to the target point is less than the set threshold, the algorithm ends and the found path is returned; The iteration terminates until the maximum number of iterations is reached or a feasible path is found; A closed-form Minimum Snap algorithm is used to obtain a smooth motion trajectory that conforms to the dynamic constraints of the boom-carrying UAV. This includes: inputting multiple sampling points obtained by the Informed-RRT* algorithm into the closed-form Minimum Snap algorithm, determining the order of the trajectory, selecting a quintic polynomial, and constructing a constraint matrix based on time allocation. The position information, velocity information, and acceleration information of the start and end points of each trajectory are converted into matrix form; a mapping matrix is ​​constructed to convert the continuity constraints into matrix form; a permutation matrix is ​​constructed to separate the known constraints and unknown free variables; and the polynomial coefficients of each trajectory are solved through matrix operations to generate the final trajectory.

2. The path planning method for a mobile platform of an arm based on differential flatness according to claim 1, characterized in that: The arm-carrying drone system has a differential flatness property, which is specifically: except at the singular point And outside T=0, vector is the flat output of the UAV system, where Represents the rotation matrix from the ground coordinate system to the base coordinate system.

3. The path planning method for a mobile platform of an arm based on differential flatness according to claim 2, characterized in that: The specific process of using the closed-form Minimum Snap algorithm to obtain a smooth motion trajectory that meets the dynamic constraints of the UAV includes: The trajectory equation of the resulting piecewise polynomial trajectory that passes through known sampling points and satisfies the constraints is written as: Among them, p j,i ,j=1,…,M,i=1,…,N are polynomial coefficients, t represents the current moment, each trajectory f1, f2,…, f M are all polynomial trajectories, T0,…,T M The arrival time of the corresponding sampling point is known, M is the number of sampling points; N represents the degree of the polynomial trajectory; The following differential constraints are satisfied at the starting and ending points of each segment trajectory: A m P m =d m Among them, A m is the constraint matrix, d m is the value of the specific constraint; P m represents the polynomial coefficients of the mth trajectory; The two polynomial trajectories before and after each sampling point satisfy the following continuity and smoothness constraints: The cost function is: in: Then the constrained QP problem is: Free variable d in separation constraint F and determine the variable d P , transform the above constrained QP problem into the following unconstrained QP problem: Among them, C is defined from R=CA -T QA -1 C T ; About J P The extreme point obtained by derivation is the global optimal point Will Substituting back C and A, we can obtain the final global optimal polynomial coefficients: Substituting P back into the polynomial trajectory equation yields a smooth trajectory that satisfies the constraints.

4. A path planning system for a mobile platform based on differential flatness, the system being used to implement the method according to any one of claims 1 to 3, characterized in that: include: a model building module configured to build a system model of the arm-carrying drone system; an optimal sampling point calculation module configured to obtain the path asymptotically optimal spatial sampling points of the arm-carrying UAV using an Informed-RRT* algorithm; The trajectory generation module is configured to use a closed-form Minimum Snap algorithm to obtain a smooth motion trajectory that complies with the dynamic constraints of the arm-carrying drone.

Images