Background¶

This section is intended to give some insights into the mathematical background that is the basis of PyTrajectory.

Contents

Trajectory planning with BVP’s
Collocation Method
Candidate Functions
- Use of the system structure
Levenberg-Marquardt Method
- Control of the parameter $\mu$
Handling constraints
- Choice of the saturation functions
- An example
References

Trajectory planning with BVP’s¶

The task in the field of trajectory planning PyTrajectory is intended to perform, is the transition of a control system between desired states. A possible way to solve such a problem is to treat it as a two-point boundary value problem with free parameters. This approach is based on the work of K. Graichen and M. Zeitz (e.g. see [Graichen05]) and was picked up by O. Schnabel ([Schnabel13]) in the project thesis from which PyTrajectory emerged.

Collocation Method¶

Given a system of autonomous differential equations

$\dot{x}_1(t) = f_1(x_1(t),...,x_n(t)) \vdots \qquad \vdots \dot{x}_n(t) = f_n(x_1(t),...,x_n(t))$

with $t \in [a, b]$ and Dirichlet boundary conditions

$x_i(a) = \alpha_i,\quad x_i(b) = \beta_i \qquad i = 1,...,n$

the collocation method to solve the problem basically works as follows.

We choose $N+1$ collocation points $t_j,\ j = 0,...,N$ from the interval $[a, b]$ where $t_0 = a,\ t_{N} = b$ and search for functions $S_i:[a,b] \rightarrow \mathbb{R}$ which for all $j = 0,..,N$ satisfy the following conditions:

$\begin{equation} S_i(t_0) = \alpha_i, \qquad S_i(t_N) = \beta_i \end{equation} \begin{equation} \frac{d}{d t} S_i(t_j) = f_i(S_1(t_j),...,S_n(t_j)) \quad i = 1,...,n \end{equation}$

Through these demands the exact solution of the differential equation system will be approximated. The demands on the boundary values $(1)$ can be sure already by suitable construction of the candidate functions. This results in the following system of equations.

$G_1^0(c) := \frac{d}{d t}S_1(t_0) - f(S_1(t_0)) = 0 \qquad \vdots G_n^0(c) := \frac{d}{d t}S_n(t_0) - f(S_n(t_0)) = 0 \qquad \vdots G_1^1(c) := \frac{d}{d t}S_1(t_1) - f(S_1(t_1)) = 0 \qquad \vdots G_n^N(c) := \frac{d}{d t}S_n(t_N) - f(S_n(t_N)) = 0$

Solving the boundary value problem is thus reduced to the finding of a zero point of $G = (G_1^0 ,..., G_n^N)^T$ , where $c$ is the vector of all independent parameters that result from the candidate functions.

Candidate Functions¶

PyTrajectory uses cubic spline functions as candidates for the approximation of the solution. Splines are piecewise polynomials with a global differentiability. The connection points $\tau_k$ between the polynomial sections are equidistantly and are referred to as nodes.

$t_0 = \tau_0 < \tau_1 < ... < \tau_{\eta} = t_N \qquad h = \frac{t_N - t_0}{\eta} \tau_{k+1} = \tau_k + h \quad k = 0,...,\eta-1$

The $\eta$ polynomial sections can be created as follows.

$P_k(t) = c_{k,0}(t-k h)^3 + c_{k,1}(t-k h)^2 + c_{k,2}(t-k h) + c_{k,3} c_{k,l} \in \mathbb{R},\qquad k = 0,...,\eta-1,\ l = 0,...,3$

Then, each spline function is defined by

$\begin{equation*} S_i(t) = \begin{cases} P_1(t) & t_0 \leq t < h \\ \vdots & \vdots \\ P_k(t) & (k-1)h \leq t < k h \\ \vdots & \vdots \\ P_\eta(t) & (\eta-1)h \leq t \leq \eta h \end{cases} \end{equation*}$

The spline functions should be twice continuously differentiable in the nodes $\tau$ . Therefore, three smoothness conditions can be set up in all $\tau_k, k = 1,...,\eta-1$ .

$\begin{eqnarray*} P_k(k h) & = & P_{k+1}(k h) \\ \frac{d}{d t} P_k(k h) & = & \frac{d}{d t} P_{k+1}(k h) \\ \frac{d^2}{d t^2} P_k(k h) & = & \frac{d^2}{d t^2} P_{k+1}(k h) \end{eqnarray*}$

In the later equation system these demands result in the block diagonal part of the matrix. Furthermore, conditions can be set up at the edges arising from the boundary conditions of the differential equation system.

$\begin{equation*} \frac{d^j}{d t^j} P_1(\tau_0) = \tilde{\alpha}_j \qquad \frac{d^j}{d t^j} P_\eta(\tau_\eta) = \tilde{\beta}_j \qquad j = 0,...,\nu \end{equation*}$

The degree $\nu$ of the boundary conditions depends on the structure of the differential equation system. With these conditions and those above one obtains the following equation system ( $\nu = 2$ ).

$\setcounter{MaxMatrixCols}{20} \setlength{\arraycolsep}{3pt} \newcommand\bigzero{\makebox(0,0){\text{\huge0}}} \begin{equation*} \textstyle \underbrace{\begin{bmatrix} h^3 & h^2 & h & 1 & 0 & 0 & 0 & -1 \\ 3h^2 & 2h & 1 & 0 & 0 & 0 & -1 & 0 &&&& \bigzero \\ 6h & 2 & 0 & 0 & 0 & -2 & 0 & 0 \\ & & & & h^3 & h^2 & h & 1 & 0 & 0 & 0 & -1 \\ & \bigzero & & & 3h^2 & 2h & 1 & 0 & 0 & 0 & -1 & 0 &&&&&& \bigzero \\ & & & & 6h & 2 & 0 & 0 & 0 & -2 & 0 & 0 \\ &&&&&&&&&&& \ddots \\ & & & & & & & & & & & & h^3 & h^2 & h & 1 & 0 & 0 & 0 & -1 \\ & & & & & & \bigzero & & & & & & 3h^2 & 2h & 1 & 0 & 0 & 0 & -1 & 0 \\ & & & & & & & & & & & & 6h & 2 & 0 & 0 & 0 & -2 & 0 & 0 \\ & & & & & & & & & & & & & \\ 0 & 0 & 0 & -1 \\ 0 & 0 & -1 & 0 &&&&&&&& \bigzero \\ 0 & -2 & 0 & 0 \\ & & & & & & & & & & & & & & & & h^3 & h^2 & h & 1 \\ & & & & & & \bigzero & & & & & & & & & & 3h^2 & 2h & 1 & 0 \\ & & & & & & & & & & & & & & & & 6h & 2 & 0 & 0 \\ \end{bmatrix}}_{=: \boldsymbol{M}} \cdot \underbrace{\begin{bmatrix} c_{1,0} \\ c_{1,1} \\ c_{1,2} \\ c_{1,3} \\ c_{2,0} \\ c_{2,1} \\ c_{2,2} \\ c_{2,3} \\ \\ \vdots \\ \\ \vdots \\ \\ \vdots \\ \\ c_{\eta,0} \\ c_{\eta,1} \\ c_{\eta,2} \\ c_{\eta,3} \end{bmatrix}}_{=: \boldsymbol{c}} = \underbrace{\begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ \vdots \\ 0 \\ 0 \\ 0 \\ \\ \tilde{\alpha}_0 \\ \tilde{\alpha}_1 \\ \tilde{\alpha}_2 \\ \tilde{\beta}_0 \\ \tilde{\beta}_1 \\ \tilde{\beta}_2 \end{bmatrix}}_{=: \boldsymbol{r}} \end{equation*}$

The matrix $\boldsymbol{M}$ of dimension $N_1 \times N_2,\ N_1 < N_2$ , where $N_2 = 4 \eta$ and $N_1 = 3(\eta - 1) + 2(\nu + 1)$ , can be decomposed into two subsystems $\boldsymbol{A}\in \mathbb{R}^{N_1 \times (N_2 - N_1)}$ and $\boldsymbol{B}\in \mathbb{R}^{N_1 \times N_1}$ . The vectors $\boldsymbol{a}$ and $\boldsymbol{b}$ belong to the two matrices with the respective coefficients of $\boldsymbol{c}$ .

$\begin{eqnarray*} \boldsymbol{M} \boldsymbol{c} & = & \boldsymbol{r} \\ \boldsymbol{A} \boldsymbol{a} + \boldsymbol{B} \boldsymbol{b} & = & \boldsymbol{r} \\ \boldsymbol{b} & = & \boldsymbol{B}^{-1} (\boldsymbol{r} - \boldsymbol{A} \boldsymbol{a}) \end{eqnarray*}$

With this allocation, the system of equations can be solved for $\boldsymbol{b}$ and the parameters in $\boldsymbol{a}$ remain as the free parameters of the spline function.

Note

Optionally, there is available an alternative approach for defining the candidate functions, see Non-Standard Approach.

Use of the system structure¶

In physical models often occur differential equations of the form

$\begin{equation*} \dot{x}_i = x_{i+1} \end{equation*}$

For these equations, it is not necessary to determine a solution through collocation. Without severe impairment of the solution method, it is sufficient to define a candidate function for $x_i$ and to win that of $x_{i+1}$ by differentiation.

$\begin{equation*} S_{i+1}(t) = \frac{d}{d t}S_i(t) \end{equation*}$

Then in addition to the boundary conditions of $S_i(t)$ applies

$\begin{equation*} \frac{d}{d t}S_i(t_0=a) = \alpha_{i+1} \qquad \frac{d}{d t}S_i(t_N=b) = \beta_{i+1} \end{equation*}$

Similar simplifications can be made if relations of the form $\dot{x}_i = u_j$ arise.

Levenberg-Marquardt Method¶

The Levenberg-Marquardt method can be used to solve nonlinear least squares problems. It is an extension of the Gauss-Newton method and solves the following minimization problem.

$\begin{equation*} \| F'(x_k)(x_{k+1} - x_k) + F(x_k) \|_2^2 + \mu^2 \|x_{k+1} - x_k \|_2^2 \rightarrow \text{min!} \end{equation*}$

The real number $\mu$ is a parameter that is used for the attenuation of the step size $(x_{k+1} - x_k)$ and is free to choose. Thus, the generation of excessive correction is prevented, as is often the case with the Gauss-Newton method and leads to a possible non-achievement of the local minimum. With a vanishing attenuation, $\mu = 0$ the Gauss-Newton method represents a special case of the Levenberg-Marquardt method. The iteration can be specified in the following form.

$\begin{equation*} x_{k+1} = x_k - (F'(x_k)^T F'(x_k) + \mu^2 I)^{-1} F'(x_k) F(x_k) \end{equation*}$

The convergence can now be influenced by means of the parameter $\mu$ . Disadvantage is that in order to ensure the convergence, $\mu$ must be chosen large enough, at the same time, this also leads however to a very small correction. Thus, the Levenberg-Marquardt method has a lower order of convergence than the Gauss-Newton method but approaches the desired solution at each step.

Control of the parameter $\mu$ ¶

The feature after which the parameter is chosen, is the change of the actual residual

$\begin{eqnarray*} R(x_k, s_k) & := & \| F(x_k) \|_2^2 - \| F(x_k + s_k) \|_2^2 \\ s_k & := & x_{k+1} - x_k \end{eqnarray*}$

and the change of the residual of the linearized approximation.

$\begin{equation*} \tilde{R}(x_k, s_k) := \| F(x_k) \|_2^2 - \| F(x_k) + F'(x_k)s_k \|_2^2 \end{equation*}$

As a control criterion, the following quotient is introduced.

$\begin{equation*} \rho = \frac{R(x_k, s_k)}{\tilde{R}(x_k, s_k)} \end{equation*}$

It follows that $R(x_k,s_k) \geq 0$ and for a meaningful correction $\tilde{R}(x_k, s_k) \geq 0$ must also hold. Thus, $\rho$ is also positive and $\rho \rightarrow 1$ for $\mu \rightarrow \infty$ . Therefor $\rho$ should lie between 0 and 1. To control $\mu$ two new limits $b_0$ and $b_1$ are introduced with $0 < b_0 < b_1 < 1$ and for $b_0 = 0.2, b_1 = 0.8$ we use the following criteria.

$\rho \leq b_0 \qquad\quad :$ $\mu$ is doubled and $s_k$ is recalculated
$b_0 < \rho < b_1 \quad :$ in the next step $\mu$ is maintained and $s_k$ is used
$\rho \geq b_1 \qquad\quad :$ $s_k$ is accepted and $\mu$ is halved during the next iteration

Handling constraints¶

In practical situations it is often desired or necessary that the system state variables comply with certain limits. To achieve this PyTrajectory uses an approach similar to the one presented by K. Graichen and M. Zeitz in [Graichen06].

The basic idea is to transform the dynamical system into a new one that satisfies the constraints. This is done by projecting the constrained state variables on new unconstrained coordinates using socalled saturation functions.

Suppose the state $x$ should be bounded by $x_0,x_1$ such that $x_0 \leq x(t) \leq x_1$ for all $t \in [a,b]$ . To do so the following saturation function is introduced

$\begin{equation*} x = \psi(y,y^{\pm}) \end{equation*}$

that depends on the new unbounded variable $y$ and satisfies the saturation limits $y^-,y^+$ , i.e. $y^- \leq \psi(y(t),y^{\pm}) \leq y^+$ for all $t$ . It is assumed that the limits are asymptotically and $\psi(\cdot,y^{\pm})$ is strictly increasing , that is $\frac{\partial \psi}{\partial y} > 0$ . For the constraints $x \in [x_0,x_1]$ to hold it is obvious that $y^- = x_0$ and $y^+ = x_1$ . Thus the constrained variable $x$ is projected on the new unconstrained varialbe $y$ .

By differentiating the equation above one can replace $\dot{x}$ in the vectorfield with a new term for $\dot{y}$ .

$\begin{equation*} \dot{x} = \frac{\partial}{\partial y} \psi(y,y^{\pm}) \dot{y} \qquad \Leftrightarrow\qquad \dot{y} = \frac{\dot{x}}{\frac{\partial}{\partial y} \psi(y,y^{\pm})} \end{equation*}$

Next, one has to calculate new boundary values $y_a = y(a)$ and $y_b = y(b)$ for the variable $y$ from those, $x_a = x(a)$ and $x_b = x(b)$ , of $x$ . This is simply done by

$\begin{equation*} y_a = \psi^{-1}(x_a, y^{\pm}) \qquad y_b = \psi^{-1}(x_b, y^{\pm}) \end{equation*}$

Now, the transformed dynamical system can be solved where all state variables are unconstrained. At the end a solution for the original state variable $x$ is obtained via a composition of the calculated solution $y(t)$ and the saturation function $\psi(\cdot,y^{\pm})$ .

There are some aspects to take into consideration when dealing with constraints:

The boundary values of a constrained variable have to be strictly within the saturation limits
It is not possible to make use of an integrator chain that contains a constrained variable

Choice of the saturation functions¶

As mentioned before the saturation functions should be continuously differentiable and strictly increasing. A possible approach for such functions is the following.

$\begin{equation*} \psi(y,y^{\pm}) = y^+ - \frac{y^+ - y^-}{1 + exp(m y)} \end{equation*}$

The parameter $m$ affects the slope of the function at $y = 0$ and is chosen such that $\frac{\partial}{\partial y}\psi(0,y^{\pm}) = 1$ , i.e.

$\begin{equation*} m = \frac{4}{y^+ - y^-} \end{equation*}$

An example¶

For examples on how to handle constraints with PyTrajectory please have a look at the Examples section, e.g. the Constrained double integrator or the Constrained swing up of the inverted pundulum.

References¶

[Graichen05]

Graichen, K. and Hagenmeyer, V. and Zeitz, M. “A new approach to inversion-based feedforward control design for nonlinear systems” Automatica, Volume 41, Issue 12, Pages 2033-2041, 2005

[Graichen06]

Graichen, K. and Zeitz, M. “Inversionsbasierter Vorsteuerungsentwurf mit Ein- und Ausgangsbeschränkungen (Inversion-Based Feedforward Control Design under Input and Output Constraints)” at - Automatisierungstechnik, 54.4, Pages 187-199, 2006

[Graichen07]

Graichen, K. and Treuer, M and Zeitz, M. “Swing-up of the double pendulum on a cart by feed forward and feedback control with experimental validation” Automatica, Volume 43, Issue 1, Pages 63-71, 2007

[Schnabel13]

Schnabel, O. “Untersuchungen zur Trajektorienplanung durch Lösung eines Randwertproblems” Technische Universität Dresden, Institut für Regelungs- und Steuerungstheorie, 2013