Mobile robot path planning (5) --- Trajectory optimization based on Minimun Snap

Table of contents

1 What we mainly introduce in this section

2 Minimum Snap Optimization

2.1 Differential Flatness

2 Minimum Snap

3 Closed-form Solution to Minimum Snap

3.1 Decision variable mapping Mapping of the problem to be optimized

4 Convex optimization and other problems

1 What we mainly introduce in this section

        1. Trajectory optimization requires boundary conditions (starting from the current movement, starting state-->ending state)

        2. Relay node: I hope to pass through the relay point (reach a landmark point). Of course, this landmark point can also be a node among us (obtained through path search).

        3. An evaluation function is required: evaluate this to be smooth (passing the left limit and right limit of the landmark point) and dynamically feasible.

2 Minimum Snap Optimization

2.1 Differential Flatness

Mathematical model description:

The states of a drone refer to physical quantities such as position, speed and orientation, while inputs usually refer to control signals applied to the drone, such as thrust or torque.

This passage expresses a way of representing the states and inputs of a drone: they can be expressed in terms of algebraic functions with respect to four specific flat outputs and their derivatives.

The role of plane output:

By selecting these four specific outputs and their derivatives, the state and input of the drone can be described more effectively. This choice helps simplify the mathematical expression of the drone's motion.

One of the benefits of this approach is the ability to automatically generate flight paths (trajectories).

Path generation and tracking:

In the space of a planar output, almost any smooth path can be traced by a drone under this model.

This means that as long as a smooth path is given in the space of the plane output, the drone can fly along this path.

One possible choice:

This passage mentions a possible option, that is, the four plane outputs chosen are the x, y, z components of the position and the orientation ψ (usually representing the yaw angle).

The trajectory (Trajectory) σ_t in the plane output space is a function that takes time as a parameter and outputs the position and rotation of the drone in the three-dimensional space.

        …

        3D position + 3D rotational attitude of the drone + 3D xyz motion speed + 3 angular velocities of rotational attitude

        The equations of motion of UAV flight: we describe it through Newton’s equation and Euler’s equation

        The resultant force of the drone is perpendicular to the propeller, ma = -mg + R(F1+F2+F3+F4) is Newton’s second law, and R is the coordinate transformation. The second-fourth relationship represents rotation and represents torque.

        We prove that the 12-dimensional state quantity of the drone can indeed be compressed into xyz and yaw:

          Do drones do Planing to do Planing? Its attitude must be bound to its acceleration, because the combined thrust of the drone must be perpendicular to the plane of the propeller and the thrust generated by the Z-axis of its own body frame. Let's look at a situation:

        The direction of the resultant force of the drone is determined, and the acceleration provided by the resultant force is known.

        This concludes the derivation.

2 Minimum Snap

        Let’s take one dimension first. If we want to generate a smooth trajectory:

        Smooth is equivalent to differentiable. (The polynomial is actually smooth and can be differentiated by multi-order differentials) (If the velocity is required to be continuous, the acceleration can be differentiated)

        to ensure that the left and right limits are the same.

        What if there are multiple paragraphs?

        We assume that each segment has a polynomial function. Our robot needs to pass through the middle point, and a speed acceleration value must be given as follows:

        But the speed and acceleration of the intermediate nodes are artificially specified by us. How much is reasonable? We often cannot specify it, so we have to use the idea of optimization to solve it!

        So our goal now is to solve the multi-segment polynomial:

        Satisfies the first and last constraints, passing through the middle point, the left and right extreme high-order derivatives of the middle point are the same (smooth and continuous), and the overall trajectory snap is minimum.

        We can look at the form of a piecewise polynomial:

        Each trajectory is known for the time of polynomial and each trajectory.

        So how do we choose the order of the trajectory reasonably?

        1. Ensure trajectory continuity

        2. Ensure the smoothness of the trajectory

        3. Ensure the minimum optimization order

        We have some agreements on the project:

        If our trajectory is K segments, we minimize jerk, providing three constraints (P, V, A) at the first position, and K-1 segmentation points in the middle. Each point provides position constraints (K-1 P constraints), for a total of 3 + 3 + K - 1 = K + 5 constraints

        There are K segments of trajectories, the order of each segment is N, and the degree of freedom of the unknown provided by each segment of the trajectory is N+1, the total is (N+1)*K, so the lowest N can be calculated.

        There is also the question of timeline:

        Let’s calculate the cost function:

        Qj is the Hessian matrix.

        We need to add some constraints to the optimization problem:

        This is an equality constraint. Similarly, we need to add a continuity constraint:

        It is a convex optimization problem:

        It is the content of the postgraduate entrance examination: The convex collection is the collection of any two points in the collection in the collection: each two points on this line are in the collection:

        So what is a convex optimization problem?

        We have an objective function $f_0{(x)}$ , which is subject to some inequalities and equality constraints.

        Several optimization problems:

3 Closed-form Solution to Minimum Snap

3.1 Decision variable mapping Mapping of the problem to be optimized

        We have understood before that what we are optimizing is actually the coefficient of the polynomial: for the fifth order, it is $p_0-p_5$ , which will actually cause problems. Here we solved it $p_0-p_5$ In fact, it has no physical meaning. If we solve for a $p_5$ that is very large, then there will be a problem of trajectory instability. Therefore, the va at the intermediate point needs to find the optimal solution based on physical meaning.

        So what do we do? We need to construct a mapping matrix M,that maps all p we want to solve to the derivatives of the endpoints.

        How should I write it? Map polynomial coefficients into derivatives.

        How to arrange d? The derivative of the endpoint corresponds to the p arrangement, then d is the derivative of each order of the first point, the derivative of each order of the second point...

        The above is the derivation.

        We can get the following formula through two formulas:

        Decompose the fixed variables and optimization variables:

        Construct a selection matrix $C^T$ , and multiply the result of the construction equal to the derivative of the original endpoint: df fixed variables, dp variables to be optimized.

        We have found the analytical solution $d_p^{*}$ .

        Let’s see how to write this $C$ :

        The derivatives of each order of the frontmost point and the last point of the i-th trajectory. We have 16 quantities, but there are 4 overlapping quantities, so there are only 12.

        What about security? Is it possible to crash when generating a trajectory? ?

        If we encounter an obstacle, we create a new landmark point in the middle of the trajectory to generate a new trajectory.

        But this isn’t great either: we add inequality constraints