[Mobile Robot Motion Planning] 04 - Trajectory Generation

foreword

Part of the content of this article refers to the mobile robot motion planning of Deep Blue Academy , and makes relevant notes and arrangements accordingly.

Related code finishing:

1. https://gitee.com/lxyclara/motion-plan-homework/
2. https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/blob/master
3. https://gitee.com/aries-wu/Motion-plan/blob/main/
4. Link: https://pan.baidu.com/s/1UtVHRxDq771LfSGK_21wgQ?pwd=rhtp Extraction code: rhtp

introduce

The trajectories experienced by robots and unmanned vehicles need to satisfy smooth characteristics and at the same time comply with the corresponding dynamic constraints. The traditional academic route obviously needs to generate an optimized trajectory after generating the path to conform to the characteristics of kinodynamic; while the path that has been kinodynamic needs to further optimize the trajectory to improve the quality of the path.

Smooth trajectory generation usually needs to meet the following characteristics:

1. Boundary conditions: location of start point, end point (direction, etc.)
2. Intermediate conditions: position, direction of relay node...
3. Judgment criteria for a smooth trajectory: the evaluation function should be reasonable (such as $\begin{array}{r}\underset{x\to x_0^{+}}{\lim }f(x)\end{array}=\begin{array}{r}\underset{x\to x_0^{-}}{\lim }f\left(x\right)\end{array}$）

Minimum Snap Optimization

Differential Flatness

Paper: Minimum Snap Trajectory Generation and Control for Quadrotors, Daniel Mellinger and Vijay Kumar
will be full-dimensional state space problem $X$ goes to the flat space$\sigma$ for research. State Space XX

for Quadrotor UAVs$X$ consists of position, direction, linear velocity and angular velocity:
$$X=[x,y,z,\varphi ,i,ps,\dot{x},\dot{y},\dot{z},{oh}_x,{oh}_y,{oh}_z{]}^{The\; T}$$ angular velocity is observed in the UAV's own coordinate system.

The states and inputs of the quadrotor can be expressed as algebraic functions of the output variables and their derivatives in four flat spaces.
Any smooth trajectory (with reasonably bounded derivatives) in flat space can be tracked by an underactuated quadrotor.

Choose 4 dimensions as a flat space in the 12-dimensional state space: $p=[x,y,z,\psi {]}^T$
在$T_0\to T_M$time, the trajectory in flat space is defined as: ${\mathbb{R}}^3\times SO(2)$($R^3$ is$x,y,z,$$SO\left(2\right)$ ψ \$\psi$ )

The dynamics of a quadrotor drone are shown below, divided into two formulas:

${\mathbf{z}}_W$Represents $z$ axis,$z_B$Represents z \mathbf{z} in the Body coordinate system$z-$ axis.
From the equation of motion:$$z_B=\frac{\mathbf{t}}{\Vert \mathbf{t}\Vert },\mathbf{t}=[{\ddot{p}}_1,{\ddot{p}}_2,{\ddot{p}}_3+g{]}^T$$$p_1,p_2,p_3$For $x,y,z$ , so$t$ represents the direction of the resultant acceleration of the UAV. Therefore, represents${\mathbf{z}}_B$is $The\; variable\; algebraic\; combination\; of\; \sigma$ . The attitude of the drone can be used${\mathbf{x}}_B,{\mathbf{y}}_B,z_B$To express, if $x_B,y_B,z_B$By algebraic combination, then $\varphi ,i,\psi$ can also be determined.

Next define an intermediate coordinate system $C$ , only one yaw angle difference from the world system. $$x_C=[\cos p_4,\sin p_4,0{]}^T$$

用$x_C$also $z_B$also ${\mathbf{y}}_B$
$$y_B=\frac{{\mathbf{z}}_B\times x_C}{\Vert {\mathbf{z}}_B\times {\mathbf{x}}_C\Vert },x_B=y_B\times z_B{\mathbf{R}}_B=\left[\begin{array}{ccc}{x}_B& y_B& z_B\end{array}\right]$$

At this point, only the angular velocity is left.
For $m\ddot{\mathbf{p}}=-mg{z}_W+u_1{z}_B$求导，得$m\dot{\mathbf{a}}={\dot{u}}_1{z}_B+({oh}_{BW})\times u_1{\mathbf{z}}_B$(This involves some knowledge of rigid body kinematics) $BW$ :Body angular velocity
viewed in the world frame
Because the drone is only thrust in the vertical direction, then${\dot{u}}_1=z_B\cdot m\stackrel{\dot{}Define}{\mathbf{a}}$
a${\mathbf{h}}_{}$, $h_{}={oh}_{BW}\times z_B=\frac{m}{u_1}(\dot{a}-({\mathbf{z}}_B\cdot \dot{a}){\mathbf{z}}_B)$。$h_{}$Quantities in can be expressed in flat space.
We know that ${oh}_{BW}={oh}_x{\mathbf{x}}_B+{oh}_y{\mathbf{y}}_B+{oh}_z{\mathbf{z}}_B$
沿$y_B$、$x_B$Give the following solution: ${oh}_x=-{\mathbf{h}}_{}\cdot y_B,{oh}_y={\mathbf{h}}_{}\cdot x_B$

Summary

The generated trajectory is a polynomial, which has the following advantages:
• Easy determination of smoothness criteria using polynomial order
• Simple closed-form computation of derivatives
• Three-dimensional decoupled trajectory generation

Minimum-snap

Smooth 1D Trajectory

Smooth Multi-Segment Trajectory

The velocities and accelerations to reach these points are not really easy to specify.

Optimization-based Trajectory Generation

Minimizing jerk is equivalent to the minimum angular velocity, which can obtain a better visual tracking field of view.
Minimizing snap can save energy

得到以下公式
$$f(t)=\{\begin{array}{ll}{f}_1(t)\doteq {\sum }_{i=0}^N{p}_{1,i}{t}^i& T_0\le t\le T_1\\ f_2\left(t\right)\doteq {\sum }_{i=0}^N{p}_{2,i}{t}^i& T_1\le t\le T_2\\ ⋮& ⋮\\ f_M\left(t\right)\doteq {\sum }_{i=0}^N{p}_{M,i}{t}^i& T_{M-1}\le t\le T_M\end{array}$$

• Each segment is a polynomial
• It is not necessarily required that the order of each trajectory is the same, but the same can simplify the problem.
• The time of each trajectory needs to be known

Constraints :

trajectory smoothness, continuousness, and control input minimization are independent of each other

Minimizing jerk requires five trajectories (one section of trajectory)
and minimizing snap requires seven trajectories (one section of trajectory)

Timeline Settings

A trajectory objective function

Differential constraints, k = 0, 1, 2, 3 for the starting point and end point, that is, p, v, a, j are required; for waypoint, k = 0 is usually required, that is, only p is required, both cases can be written as a matrix Formal equality constraints

Continuity constraints, although v, a, j are not required for the waypoint, but the derivatives of the left and right trajectories at the waypoint are required to be equal

Convex Optimization

Convex optimization related courses:

1. Boyd Stanford Open Class
2. HKUSThttps://github.com/KellyHwong/convex-hkust
3. Convex optimization introductory textbooks and courses recommendation and arrangement

Convex functions and convex sets

The mathematical definition of a convex function is: if $f\left(x\right)$ in interval$I$ upper link,$I$ is an interval on the set of real numbers, and for any$x,y\in I$ sum any$l\in [0,1]$， we have$$f(\lambda x+(1-l\left)y\right)\le \lambda f(x)+(1-\lambda )f\left(y\right)$$则$f\left(x\right)$ in interval$I$ is a convex function. Among them,$\lambda$ is called the weight or mixing ratio. This definition is in a geometric sense, that is, for any two points on the function, the value of the function at each point on the line segment between these two points does not exceed the value on the linear function corresponding to the point. A convex function can also be defined in the form of a second derivative as$f^{\prime \prime }(x)\ge 0$ .
Mathematical definition of convex set: In Euclidean space, a set$S$ is a convex set if and only if for any$x,y\in S$和$0\le l\le 1$ ,都有
$$\lambda x+(1-l)y\in S$$ among them,$\lambda$ is a real number, representing$x_1$and $x_2$ratio coefficient between. In other words, if the line segment formed by two points in any set is in the set, then it is a convex set.

The standard form of a convex optimization problem

$$\begin{array}{rl}\mathrm{minimize}& f_0(x)\\ \mathrm{subject}\mathrm{to}& f_i\left(x\right)\le 0i=1,\dots ,m\\ & \mathrm{A}x=b\end{array}$$
where the objective function $f_0$and the inequality constraint function $f_i\left(x\right)$ are convex functions,$Ax=b$ is required to be affine,xxIf the domain of$x\; is\; a\; convex\; set$

Linear Programming (LP): linear programming problems

$$\begin{array}{rl}\underset{x}{\mathrm{minimize}}& c^{T}x+d\\ \text{subject to}& Gx\le h\\ & Ax=b\end{array}$$

Quadratic Programming (QP): quadratic programming problem

$$\begin{array}{rl}\underset{x}{\mathrm{minimize}}& \frac{1}{2}{x}^{T}Px+q^{T}x+r\\ \text{subject to}& Gx\le h\\ & Ax=b\end{array}$$

Quadratically Constrained QP (QCQP): A quadratic programming problem with quadratic constraints

QCQP (Quadratically Constrained Quadratic Program) can be expressed in the following form:

$$\begin{array}{rl}\underset{x}{\mathrm{minimize}}& (1/2)x^T{P}_{0}x+q_0^{T}x+r_0\\ \mathrm{subject}\mathrm{to}& (1/2)x^T{P}_{i}x+q_i^{T}x+r_i\le 0i=1,\dots ,m\\ & Ax=b\end{array}$Constraint functions are also convex functions

where $x\in {\mathbb{R}}^n$ is the optimization variable,$P_0\in {\mathbb{S}}^n$, $P_i\in S^n$, $q_0,q_i\in {\mathbb{R}}^n$, $b_i\in \mathbb{R}$, $A\in R^{p\times n}$, $b\in R^p$, $l,u\in R^n$ , and$l_i\le u_i$。

where, ${\mathbb{S}}^n$ means all$n\times$A collection of real symmetric matrices of$n\; .$$\frac{1}{2}{x}^T{P}_{i}x+q_i^{T}x\le b_i$express about xxQuadratic inequality constraints on$x$$Ax=b$ represents linear equality constraints,$l_i\le x_i\le u_i$Represents the lower and upper bound constraints on variables. The goal is to minimize the objective function $\frac{1}{2}{x}^T{P}_{0}x+q_0^{T}x$。

Second-Order Cone Programming (SOCP): second-order optimization problem

SOCP is the abbreviation of Second Order Cone Programming. Quadratic cone programming refers to a class of convex optimization problems in which the constraints of the optimization problem consist of quadratic cones.

The general form of quadratic cone programming is:

$$\begin{array}{rl}\text{minimize}& c^{T}x\\ \text{subject to}& ||A_{i}x+b_i|{|}_2\le c_i^{T}x+d_i,i=1,...,m\\ & Fx=g\end{array}$$

Among them, $x$ is the optimization variable,$c$ is a vector,$A_i$is the matrix, $b_i$is a vector, $d_i$and $g$ is a scalar.

The main constraint is the quadratic cone constraint, where $||A_{i}x+b_i|{|}_2\le c_i^{T}x+d_i$Represents the vector $A_{i}x+b_i$with the vector $c_i^{T}x+d_i$The inner product in Euclidean space is less than or equal to 0. This constraint can be written in the transformed form, namely

$$\begin{array}{r}t\ge ||A_{i}x+b_i|{|}_2\\ c_i^{T}x+d_i-t\le 0\end{array}$$

where $t$ is an auxiliary variable used to ensure that the inner product is less than or equal to 0.

SOCP can be viewed as a directly extended LP problem. Therefore, it can be solved using convex optimization, such as interior point method and branch and bound method. Compared with quadratic programming, SOCP problems have better convexity.

Semidefinite Programming（SDP）

SDP (Semidefinite Programming) can be expressed in the following form:

$$\begin{array}{rl}\underset{X}{\min }& c^{T}x\\ s.t.& F_0+\sum _{i=1}^n{X}_i{F}_i⪰0\\ & X_i⪰0,i=1,2,\dots ,n\end{array}$$

其中，$X_i\in {\mathbb{S}}^{n_i}$is a symmetric positive semidefinite matrix, $c\in {\mathbb{R}}^n$ is a constant vector,$F_i\in {\mathbb{S}}^{n_i}$is a constant symmetric matrix, $F_0\in S^{n_0}$is a constant symmetric matrix, $⪰0$ means a positive semidefinite matrix. The goal is to minimize the objective function$c^{T}x$, satisfy the matrix inequality constraint$F_0+{\sum }_{i=1}^n{X}_i{F}_i⪰0$ and positive semi-definite constraints$X_i⪰0,i=1,2,\dots ,n$。

It is not difficult to see that SDP can be regarded as a convex optimization problem whose constraints are symmetric positive semi-definite matrices. The matrix inequality constraints guarantee that the problem is convex.

Closed-form Solution to Minimum Snap

Paper: Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments, Charles Richter, Adam Bry, and Nicholas Roy
use the QP solver to solve the numerical solution of the Minimum Snap problem. Another solution is to find the Minimum Snap directly from the algebra. The analytical solution of the problem, that is, the closed-form solution of the Minimum Snap problem.

Decision variable mapping variable mapping

Let's take the equation:
$$J={\left[\begin{array}{c}{\mathbf{p}}_1\\ ⋮\\ p_M\end{array}\right]}^T\left[\begin{array}{ccc}{\mathbf{Q}}_1& 0& 0\\ 0& \ddots & 0\\ 0& 0& Q_M\end{array}\right]\left[\begin{array}{c}{p}_1\\ ⋮\\ p_M\end{array}\right]$$
It can be seen that the optimized variables are the coefficients of the trajectory polynomial $p_0,p_1,...,p_M$, the coefficient has no actual physical meaning, such as $p_{10}{t}^{10}$ o'clock when$When\; t$ is large,$p_{10}$It may be very small, which will cause numerical instability in trajectory planning problems. Therefore, we need to transform the problem of coefficients on trajectory optimization into derivative constraints of trajectory points (that is, $p,v,a$ , has a physical meaning). Through the mapping matrix$M_j$The variable pj p_j to be solved$p_j$Derivative constraints $d_j$，即$M_j{p}_j=d_j$。

New objective function:

$$J={\left[\begin{array}{c}{\mathbf{d}}_1\\ ⋮\\ d_M\end{array}\right]}^T{\left[\begin{array}{ccc}{M}_1& 0& 0\\ 0& ⋮& 0\\ 0& 0& M_M\end{array}\right]}^{-T}\left[\begin{array}{ccc}{\mathbf{Q}}_1& 0& 0\\ 0& ⋮& 0\\ 0& 0& Q_M\end{array}\right]{\left[\begin{array}{ccc}{M}_1& 0& 0\\ 0& ⋮& 0\\ 0& 0& M_M\end{array}\right]}^{-1}\left[\begin{array}{c}{d}_1\\ ⋮\\ d_M\end{array}\right]$$

轨迹方程如下：
$$\begin{array}{rl}& x(t)=p_5{t}^5+p_4{t}^4+p_3{t}^3+p_2{t}^2+p_{1}t+p_0\\ & x^{\prime }(t)=5{p.m}_5{t}^4+4p_4{t}^3+3p_3{t}^2+2p_{2}t+p_1\\ & \begin{array}{r}{x}^{\prime \prime }(t)=20p_5{t}^3+12p_4{t}^2+6{p.m}_{3}t+2p_2\end{array}\end{array}$$

Ratio:
$$\mathbf{M}=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 2& 0& 0\\ T^5& T^4& T^3& T^2& T& 1\\ 5T^4& 4T^3& 3T{\_}^2& 2T\_& 1& 0\\ 20T{\_}^3& 12T^2& 6& 2& 0& 0\end{array}\right]$$To make it clearer, $The\; first\; three\; rows\; of\; the\; M$ matrix will be$t=$Substitute the value at$0$$t=DenoteM$ pi = [ 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 2 0 0 T 5 T 4 T 3 T 2 T 1 5 T 4 4 T 3 3 T 2 2 T 1 0 20 T$$\mathbf{M}{p}_i=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 2& 0& 0\\ T^5& T^4& T^3& T^2& T& 1\\ 5T^4& 4T^3& 3T{\_}^2& 2T\_& 1& 0\\ 20T{\_}^3& 12T^2& 6& 2& 0& 0\end{array}\right]\left[\begin{array}{c}{p}_5\\ p_4\\ p_3\\ p_2\\ p_1\\ p_0\end{array}\right]$$

Fixed and free variable separation fixed variable and free variable decomposition

The state variables of the starting point and end point in the trajectory ( $p,v,a,j$ ) is determined, the position of the middle point (waypoints)$\left(p\right)$ is deterministic, so by choosing the matrix$C$ to separate free variables (free)${\mathbf{d}}_F$And the fixed variable (constrained) $d_P$. Therefore, the following mapping relationship can be obtained:
$${\mathbf{C}}^T\left[\begin{array}{c}{\mathbf{d}}_F\\ d_P\end{array}\right]=\left[\begin{array}{c}{d}_1\\ ⋮\\ d_M\end{array}\right]$$Let’s set the boundary conditions as follows: $$J={\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^T\underset{\mathbf{R}}{\underbrace{\mathbf{C}{M}^{-T}\mathbf{Q}{M}^{-1}{\mathbf{C}}^T\left[\begin{array}{c}{\mathbf{d}}_F\\ d_P\end{array}\right]}}={\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^T\left[\begin{array}{cc}{\mathbf{R}}_{FF}& R_{FP}\\ R_{PF}& R_{PP}\end{array}\right]\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]$$ So the objective function becomes an unconstrained quadratic programming problem, which can be solved in closed form$$\begin{array}{r}J=d_F^T{R}_{FF}{d}_F+d_F^T{R}_{FP}{d}_P+d_P^T{R}_{PF}{d}_F+d_P^T{R}_{PP}{d}_P\\ d_P^{\ast }=-{\mathbf{R}}_{PP}^{-1}{R}_{FP}^T{d}_F\end{array}$$
PS: Why is the continuity constraint gone? Because the matrix $C$ maps a variable in$In\; the\; two\; variables\; of\; d$ , the continuity constraint is satisfied

$C$ 's design

Hierarchical approach

Use RRT* to get waypoints, then minimum snap to generate trajectory

Problem: safety issue

The path found by RRT is collision-free, but overshooting will occur after trajectory optimization.

Solution: add a waypoint to the collision part and regenerate the trajectory. If there is still a collision, continue to insert waypoints and generate in extreme cases. The trajectory of will be infinitely close to the absolutely safe path from path planning

better solution?

Increase the bounding box, and perform trajectory optimization under hard constraints (the next lecture will explain in detail).

Implementation Details

Convex Solvers

solver	address	Advantages and disadvantages
CVX	http://cvxr.com/cvx/	Based on MATLAB implementation, slower
by Moses	https://www.mosek.com/	Very robust, but only runs on x86 architecture
OOQP	http://pages.cs.wisc.edu/~swright/ooqp/	Fast, open source, robust, but older code, poor readability
GLPK	https://www.gnu.org/software/glpk/	Open source, robust, fast, has advantages in solving LP

Numerical Stability

Other engineering stuff

Time Allocation time allocation

reference

[1] Minimum Snap Trajectory Generation and Control for Quadrotors, Daniel Mellinger and Vijay Kumar
[2] Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments, Charles Richter, Adam Bry, and Nicholas Roy

Coding Tips

Take two tracks as an example:

closed solution

ROS:
closed solution