written in front

This article is mainly to record the fifth chapter of the course "Mobile Robot Motion Planning" opened by Deep Blue Academy - the homework of optimal trajectory generation , that is, the content related to the minimum jerk trajectory generated by BVIP in three-dimensional space and the process of doing the homework .

The course link is below, and those who are interested can sign up for study:

Mobile Robot Motion Planning-Deep Blue Academy

mission details

As shown in the figure below, given the positions, velocities, and accelerations of the starting point and end point in three-dimensional space , as well as the positions of (M-1) intermediate points , then this trajectory is divided into M segments. The moving time of each small track $\Delta T_i$ is also given.

We are required to use optimal conditions to calculate the coefficients of the three-dimensional minimum jerk trajectory, because these coefficients can describe a trajectory, which will be discussed later.

As for how the optimal condition comes from, I won’t explain it here, and I will talk about what the optimal condition is later.

Boundary value problem (Boundary value problem, BVP)

When talking about BVIP, let's first talk about the BVP problem in trajectory generation. In this part, it is recommended to read the relevant content of the following link:

Robotics: Aerial Robotics (air robot) notes (6): UAV motion planning

To put it simply, we know the initial state of the robot (such as initial position, velocity, acceleration, etc.), and the final state, that is, we only consider the initial and final states, not the intermediate states.

As shown in the figure below, this is an example of a drone catching a ball. Only the initial state of the drone and the state of catching the ball are considered to generate a trajectory:

The optimal condition theory tells us: if our input is the s-order derivative of the trajectory (if it is position, s=1, for acceleration, s=2, for jerk, s=3, and so on), then the polynomial that describes the trajectory The order is 2s -1. We are now going to generate a jerk trajectory, so the order of the polynomial is 2 * 3 - 1 = 5.

Let's look at the situation in one-dimensional space, the state of the starting point and the ending point are known, and the jerk trajectory is calculated:

Then our jerk trajectory can be described as follows:

By substituting t=0, t=T into the polynomial above, and the first-order derivation of the polynomial, the second-order derivation can be obtained:

If the boundary conditions are as follows:

At this time, if the running time T of this trajectory is known, then the parameters of this trajectory can be calculated $c_0-c_5$ . Then the trajectory will be known.

Then if we want to design a trajectory passing through a specified location, we need to specify the state of each location, including position, velocity, acceleration, and so on. But sometimes we just want the robot to pass a certain point without specifying its speed, acceleration and other states at the point, then this is BIVP (Boundary-intermediate value problem).

BIVP（Boundary-intermediate value problem）

Optimal condition theory tells us that if our input is the s-order derivative of the trajectory, then its 2s-d-1-order derivatives are also continuously differentiable. That is to say, if our input is jerk ( s = 3 ), first of all, the position, velocity, and acceleration must be continuous, and there is no objection to this. But including jerk, snap (s = 4) is also continuously differentiable at sites.

We know that the trajectory is described by a polynomial, then we can solve the coefficients of the trajectory by solving the following linear equation:

For the minimum jerk trajectory, we divide the trajectory into M segments, each segment can be described by a fifth-order polynomial, through the optimal condition theory, that is, the continuity of the multi-order derivatives of the left and right two segments of each site and the position constraints to solve for the coefficients of the trajectory:

We choose the T description method at the top of the figure below, that is, the starting moment of the second trajectory is 0, so that the calculation value is relatively stable and the solution is faster.

In this way, we transform the trajectory optimization problem into the solution problem of linear equations. Next, we will start to derive related variables and write programs based on the following figure.

Formula derivation and code writing

Our purpose is to construct the M matrix about the time T and the b matrix about the boundary value, and then solve the coefficient matrix c through the inverse of the M matrix:

First, we initialize two matrices, where pieceNum represents the number of segments of the trajectory, that is, M, because the trajectory we calculate is three-dimensional, so the number of columns of boundary values is 3, representing x, y, z:

Eigen::MatrixXd M = Eigen::MatrixXd::Zero(6 * pieceNum , 6 * pieceNum );
Eigen::MatrixXd b = Eigen::MatrixXd::Zero(6 * pieceNum , 3);

Next, let's narrow down the problem first, and first look at the trajectory composed of three points (start point, end point, and an intermediate point). Note that here $\large C_1$ refers coefficient matrix:

$\large M=\begin{bmatrix} F_0& 0 \\ E_1& F_1\\ 0& E_2 \end{bmatrix} c=\begin{bmatrix}C_1\\C_2 \end{bmatrix} b=(D_0^T, D_1^T, 0_{m\times \bar{d}_1},D_M^T)^T$

 // coefficientMatrix is a matrix with 6*piece num rows and 3 columes
    // As for a polynomial c0+c1*t+c2*t^2+c3*t^3+c4*t^4+c5*t^5,
    // each 6*3 sub-block of coefficientMatrix is
    // --              --
    // | c0_x c0_y c0_z |
    // | c1_x c1_y c1_z |
    // | c2_x c2_y c2_z |
    // | c3_x c3_y c3_z |
    // | c4_x c4_y c4_z |
    // | c5_x c5_y c5_z |
    // --              --

We know that the trajectory of the smallest jerk is described by a fifth-order polynomial:

$\large x(t)=c_0+c_1t+c_2t^2+c_3t^3+c_4t^4+c_5t^5$

The form of its first derivative is:

$\large \dot{x}(t)=c_1+2c_2t+3c_3t^2+4c_4t^3+5c_5t^4$

So on and so forth:

$\large \ddot{x}(t)=2c_2+6c_3t+12c_4t^2+20c_5t^3$

$\large \dddot{x}(t)=6c_3+24c_4t+60c_5t^2$

$\large \ddddot{x}(t)=24c_4+120c_5t$

We first know the position, velocity and acceleration of the starting point, then substitute t=0 into it:

$\large Initpos = {x}(0)=c_0 =\begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0\end{bmatrix}\begin{bmatrix} c_0 & c_1 & c_2 & c_3 & c_4 & c_5\end{bmatrix}^T$

$\large Initvel = \dot{x}(0)=c_1 =\begin{bmatrix} 0 & 1 & 0 & 0 & 0 & 0\end{bmatrix}\begin{bmatrix} c_0 & c_1 & c_2 & c_3 & c_4 & c_5\end{bmatrix}^T$

$\large Initacc = \ddot{x}(0)=2c_2 =\begin {bmatrix} 0 & 0 & 2 & 0 & 0 & 0\end{bmatrix}\begin{bmatrix} c_0 & c_1 & c_2 & c_3 & c_4 & c_5\end{bmatrix}^T$

Looking back at the original formula, there is the first expression:

$\large F_0C_1 = D_0^T$

in:

$\large D_0^T=\begin{bmatrix} Initpos\\Initvel \\ Initacc \end{bmatrix} F_0=\begin{bmatrix} 1 & 0&0&0&0&0\\0 & 1&0&0&0&0 \\ 0 & 0&2&0&0&0 \end{bmatrix} C_1=\begin{bmatrix} c_0&c_1&c_2&c_3&c_4&c_5 \end{bmatrix}^T$

Then we can fill in the corresponding elements into the M matrix and b matrix:

 Eigen::MatrixXd F_0(3,6);
 F_0 <<  1, 0, 0, 0, 0, 0,
         0, 1, 0, 0, 0, 0,
         0, 0, 2, 0, 0, 0;
 M.block(0, 0, 3, 6) = F_0;
 b.block(0, 0, 3, 3) <<  initialPos(0), initialPos(1), initialPos(2), 
                         initialVel(0), initialVel(1), initialVel(2), 
                         initialAcc(0), initialAcc(1), initialAcc(2);

The same is true for the end position. Here we assume that the expression of the last track is:

$\large x_M(t) = c_0' + c_1't + c_2't^2+ c_3't^3+c_4't^4 + c_5't^5$

Then there is:

$\large Terminalpos = {x_2}(t_m)\\=c_0 + c_1t_{m} + c_2t_{m}^{2}+ c_3t_{m}^{3}+ c_4t_{m}^{4}+ c_5t_{m}^{5} \\=\begin{bmatrix} 1 & t_m & t_m^2 & t_m^3 & t_m^4 & t_m^5\end{bmatrix}\begin{bmatrix} c_0' & c_1' & c_2' & c_3' & c_4' & c_5'\end{bmatrix}^T$

$\large Terminalvel=\begin{bmatrix} 0 &1 &2 t_m & 3t_m^2 & 4t_m^3 & 5t_m^4\end{bmatrix}\begin{bmatrix} c_0' & c_1' & c_2' & c_3' & c_4' & c_5'\end{bmatrix}^T$

$\large Terminalacc=\begin {bmatrix} 0 &0 &2 & 6t_m & 12t_m^2 & 20t_m^3\end{bmatrix}\begin{bmatrix} c_0' & c_1' & c_2' & c_3' & c_4' & c_5'\ end{bmatrix}^T$

Because we are now taking two sections of trajectory composed of three points as an example, in fact, the subscript 2 has already indicated the last section of trajectory M, and looking back at the original formula, there is the last expression:

$\large E_MC_M=E_2C_2 = D_2^T$

in:

$\large D_M^T=D_2^T=\begin{bmatrix} Terminalpos\\Terminalvel \\ Terminalacc \end{bmatrix} E_M=E_2=\begin{bmatrix} 1 & t_m & t_m^2 & t_m^3 & t_m^4 & t_m^5\\0 &1 &2 t_m & 3t_m^2 & 4t_m^3 & 5t_m^4 \\ 0 &0 &2 & 6t_m & 12t_m^2 & 20t_m^3\end{bmatrix}$

$\large C_M=C_2=\begin{bmatrix} c_0'&c_1'&c_2'&c_3'&c_4'&c_5' \end{bmatrix}^T$

Then we can fill in the corresponding elements in the M matrix and b matrix, and the corresponding code is written as follows, where t is the running time of the last segment of the trajectory:

double t(timeAllocationVector(pieceNum-1));
Eigen::MatrixXd E_M(3,6);
E_M <<  1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5),
        0, 1, 2*t, 3*pow(t, 2), 4*pow(t, 3), 5*pow(t, 4),
        0, 0, 2, 6*t, 12*pow(t, 2), 20*pow(t, 3);
M.block(6 * pieceNum -3, 6 * (pieceNum - 1), 3, 6) = E_M;
b.block(6 * pieceNum -3, 0, 3, 3) <<    terminalPos(0), terminalPos(1), terminalPos(2), 
                                        terminalVel(0), terminalVel(1), terminalVel(2), 
                                        terminalAcc(0), terminalAcc(1), terminalAcc(2);

Next, we will consider the boundary condition problem of the intermediate point. At this time, we notice that what we need to calculate is:

$\large E_1C_1+F_1C_2$

That is to say, now we need to consider the expressions of the trajectories at the left and right ends of the middle point. At the same time, if we move all the expressions related to the trajectory to the left side of the equation, the right side is the corresponding element of the b matrix.

First of all, the intermediate point needs to meet the following constraints, that is, if the point is used as the end point of the previous trajectory, the position should be the position of the provided intermediate point :

$\large x(t_1) = c_0 + c_1t_1 + c_2t_1^2+ c_3t_1^3+c_4t_1^4 + c_5t_1^5=IntermediatePos$

At this time $\large C_2$ , it has nothing to do with it, so we can make $\large F_1$ the first line (C++ index 0) the element is 0, there is:

$\large E_{1,0}=\begin{bmatrix} 1 & t_1 & t_1^2 & t_1^3 & t_1^4 & t_1^5\end{bmatrix}\ \ \ F_{1,0} = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 &0\end{bmatrix}$

$\large E_{1,0}C_1 + F_{1,0}C_2 = IntermediatePos= D_1^T$

Later, according to the optimal condition theory, we can know that if our input is the s-order derivative of the trajectory, then its 2s-d-1-order derivative is also continuously derivable . Next, we will write the corresponding expression to meet this condition .

First of all, the position of the trajectory at the middle point must be continuous, then the end position of the previous trajectory is equal to the starting position of the latter trajectory:

$\large x(t_1) = c_0 + c_1t_1 + c_2t_1^2+ c_3t_1^3+c_4t_1^4 + c_5t_1^5=x_1(0)=c_0'$

Just move the item:

$\large c_0 + c_1t_1 + c_2t_1^2+ c_3t_1^3+c_4t_1^4 + c_5t_1^5-c_0' = E_{1,1}C_1 + F_{1,1}C_2=0$

in:

$\large E_{1,1}=\begin{bmatrix} 1 & t_1 & t_1^2 & t_1^3 & t_1^4 & t_1^5\end{bmatrix}\ \ \ F_{1,1} = \begin{bmatrix} -1 & 0 & 0 & 0 & 0 &0\end{bmatrix}$

Let's consider the speed in detail, that is, the speed of the trajectory at the middle point is also continuous:

$\large \dot{x}(t_1) = c_1 + 2c_2t_1+ 3c_3t_1^2+4c_4t_1^3 + 5c_5t_1^4=\dot{x}_1(0)=c_1'$

$\large c_1 + 2c_2t_1+ 3c_3t_1^2+4c_4t_1^3 + 5c_5t_1^4-c_1' = E_{1,2}C_1 + F_{1,2}C_2=0$

in:

$\large E_{1,2}=\begin{bmatrix} 0&1&2t_1&3t_1^2&4t_1^3&5t_1^4\end{bmatrix}\\\F_{1,2}=\begin{bmatrix } 0&-1&0&0&0&0\end{bmatrix}$

The optimal condition theory says that if we generate a jerk trajectory, it will be continuous until the snap at the intermediate point, as shown in the figure below:

Then we can deduce it like this, and finally get:

$\large E_1 = \begin{bmatrix}1&t_1&t_1^2&t_1^3&t_1^4&t_1^5\1&t_1&t_1^2&t_1^3&t_1^4&t_1^5\ \ 0 & 1 & 2t_1& 3t_1^2& 4t_1^3& 5t_1^4\\ 0& 0& 2& 6t_1& 12t_1^2& 20t_1^3\\ 0& 0& 0& 6& 24t_1& 60t_1^2\\ 0& 0& 0& 0& 24& 120bmatrix\end{ \ \ F_1 = \begin{bmatrix} 0& 0& 0& 0& 0& 0\\ -1& 0& 0& 0& 0& 0\\ 0& -1& 0& 0& 0& 0\\ 0& 0& -2& 0& 0& 0\\ 0& 0& 0& -6& 0&; 0\\ 0& 0& 0& 0& -24& 0\end{bmatrix}$

$\large \begin{bmatrix} D_T^1 & 0_{M \times \bar{d_1}} \end{bmatrix}^T=[IntermediatePos, 0, 0, 0, 0, 0]^T$

Then as the number of intermediate points increases, we can use a loop to fill in these elements in turn:

for (int i=1; i < pieceNum ; i++){
        double t(timeAllocationVector(i-1));
        Eigen::MatrixXd F_i(6,6), E_i(6,6);
        Eigen::Vector3d D_i(intermediatePositions.transpose().row(i-1));
        E_i <<  1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5),
                1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5),
                0, 1, 2*t, 3*pow(t, 2), 4*pow(t, 3), 5*pow(t, 4),
                0, 0, 2, 6*t, 12*pow(t, 2), 20*pow(t, 3),
                0, 0, 0, 6, 24*t, 60*pow(t, 2),
                0, 0, 0, 0, 24, 120*t;
        F_i <<  0, 0, 0, 0, 0, 0,
                -1, 0, 0, 0, 0, 0,
                0, -1, 0, 0, 0, 0,
                0, 0, -2, 0, 0, 0,
                0, 0, 0, -6, 0, 0,
                0, 0, 0, 0, -24, 0;
        int j = 6 * (i-1);
        M.block(3+6*(i-1), j + 6, 6, 6) = F_i;
        M.block(3+6*(i-1), j, 6, 6) = E_i;
        b.block(3+6*(i-1), 0, 6, 3) <<  D_i(0), D_i(1), D_i(2),
                                        0, 0, 0,
                                        0, 0, 0,
                                        0, 0, 0,
                                        0, 0, 0,
                                        0, 0, 0;
    }

Finally, we get the final M matrix and b matrix, and then we can solve the coefficient matrix c by inverting the M matrix:

$\large Mc=b\rightarrow c=M^{-1}b$

coefficientMatrix = M.inverse() * b;

So far, the code to complete the job has been written. The final effect and the complete code to realize the minimum jerk trajectory function are as follows:

void minimumJerkTrajGen(
    // Inputs:
    const int pieceNum,
    const Eigen::Vector3d &initialPos,
    const Eigen::Vector3d &initialVel,
    const Eigen::Vector3d &initialAcc,
    const Eigen::Vector3d &terminalPos,
    const Eigen::Vector3d &terminalVel,
    const Eigen::Vector3d &terminalAcc,
    const Eigen::Matrix3Xd &intermediatePositions,
    const Eigen::VectorXd &timeAllocationVector,
    // Outputs:
    Eigen::MatrixX3d &coefficientMatrix)
{
    // coefficientMatrix is a matrix with 6*piece num rows and 3 columes
    // As for a polynomial c0+c1*t+c2*t^2+c3*t^3+c4*t^4+c5*t^5,
    // each 6*3 sub-block of coefficientMatrix is
    // --              --
    // | c0_x c0_y c0_z |
    // | c1_x c1_y c1_z |
    // | c2_x c2_y c2_z |
    // | c3_x c3_y c3_z |
    // | c4_x c4_y c4_z |
    // | c5_x c5_y c5_z |
    // --              --
    // Please computed coefficientMatrix of the minimum-jerk trajectory
    // in this function

    // ------------------------ Put your solution below ------------------------
    Eigen::MatrixXd M = Eigen::MatrixXd::Zero(6 * pieceNum , 6 * pieceNum );
    Eigen::MatrixXd b = Eigen::MatrixXd::Zero(6 * pieceNum , 3);

    Eigen::MatrixXd F_0(3,6);
    F_0 <<  1, 0, 0, 0, 0, 0,
            0, 1, 0, 0, 0, 0,
            0, 0, 2, 0, 0, 0;
    M.block(0, 0, 3, 6) = F_0;
    b.block(0, 0, 3, 3) <<  initialPos(0), initialPos(1), initialPos(2), 
                            initialVel(0), initialVel(1), initialVel(2), 
                            initialAcc(0), initialAcc(1), initialAcc(2);
    
    for (int i=1; i < pieceNum ; i++){
        double t(timeAllocationVector(i-1));
        Eigen::MatrixXd F_i(6,6), E_i(6,6);
        Eigen::Vector3d D_i(intermediatePositions.transpose().row(i-1));
        E_i <<  1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5),
                1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5),
                0, 1, 2*t, 3*pow(t, 2), 4*pow(t, 3), 5*pow(t, 4),
                0, 0, 2, 6*t, 12*pow(t, 2), 20*pow(t, 3),
                0, 0, 0, 6, 24*t, 60*pow(t, 2),
                0, 0, 0, 0, 24, 120*t;
        F_i <<  0, 0, 0, 0, 0, 0,
                -1, 0, 0, 0, 0, 0,
                0, -1, 0, 0, 0, 0,
                0, 0, -2, 0, 0, 0,
                0, 0, 0, -6, 0, 0,
                0, 0, 0, 0, -24, 0;
        int j = 6 * (i-1);
        M.block(3+6*(i-1), j + 6, 6, 6) = F_i;
        M.block(3+6*(i-1), j, 6, 6) = E_i;
        b.block(3+6*(i-1), 0, 6, 3) <<  D_i(0), D_i(1), D_i(2),
                                        0, 0, 0,
                                        0, 0, 0,
                                        0, 0, 0,
                                        0, 0, 0,
                                        0, 0, 0;
    }
    double t(timeAllocationVector(pieceNum-1));
    Eigen::MatrixXd E_M(3,6);
    E_M <<  1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5),
                0, 1, 2*t, 3*pow(t, 2), 4*pow(t, 3), 5*pow(t, 4),
                0, 0, 2, 6*t, 12*pow(t, 2), 20*pow(t, 3);
    M.block(6 * pieceNum -3, 6 * (pieceNum - 1), 3, 6) = E_M;
    b.block(6 * pieceNum -3, 0, 3, 3) <<    terminalPos(0), terminalPos(1), terminalPos(2), 
                                            terminalVel(0), terminalVel(1), terminalVel(2), 
                                            terminalAcc(0), terminalAcc(1), terminalAcc(2);
                                        

    coefficientMatrix = M.inverse() * b;
    // ------------------------ Put your solution above ------------------------
}

Trajectory optimization - generate minimum jerk trajectory through BVIP in 3D space

written in front

mission details

Boundary value problem (Boundary value problem, BVP)

BIVP（Boundary-intermediate value problem）

Formula derivation and code writing

Guess you like