Industrial robot

Industrial robot
State dimension:	1
Differential states:	2
Continuous control functions:	2
Path constraints:	4
Interior point equalities:	2

The Industrial robot problem focuses on the KUKA LWR IV robot drawing a pre-defined curve on a white board, which results in a path-following problem. This means that a system should be steered along a given path while there are no constraints regarding the position at a specific time $t > t_0$ . This concept arises often where autonomous vehicles or steering of robots are involved. The source for this model can be found online on arXiv "Implementation of Nonlinear Model Predictive Path-Following Control for an Industrial Robot".

Path-following problems

As the robot should follow a geometric curve but is not constrained as to when it has to be where, a path-following problem arises. The path is given by $\mathcal{P} = \{ y \in \mathcal{R}^n: \theta \in [ \theta_0, \theta_1 ] \mapsto y = p(\theta)\}$ where “ $\theta$ is called path parameter and $p(\theta)$ is a parametrization of $\mathcal{P}$ ”. The path parameter does depend on the time however the exact timing is not known beforehand but rather integrated into the optimization problem by making use of a virtual system.

Virtual system

A virtual system is introduced to take care of the timing of the robot. This is an additional degree of freedom which is used here as an input into an integrator. This additional optimization variable $v$ is integrated twice to produce $z$ which is used as the time parameter for the path (i.e. $p(z)$ ).

$\left( \begin{array}{c} \dot{z_1}\\ \dot{z_2}\\ \end{array} \right) = \dot{z} = l(z, v) = \left( \begin{array}{c} z_2\\ v\\ \end{array} \right)$

Model

The system for the motions of the robot are given by the function $f$ :

$f(x) = E^{-1} \left( \begin{array}{c} x_2\\ u - C(x_1,x_2) x_2 - g(x_1) - \tau_F(x_2)\\ \end{array} \right)$

with

$E = diag(I, B(x_1))$

where $B, C, E, g, \tau_F$ are given in the Parameters section.

The objective function $J$ will be the integral over the so-called cost function $F: \mathbb{R}^{n_u} \times \mathbb{R} \times \mathcal{U} \times \mathcal{V} \mapsto \mathbb{R}_0^{+}$ , i.e. $J(x(t_k),\bar{e}, \bar{u}, \bar{z}, \bar{v}) = \displaystyle \int_{t_k}^{t_k+T} F(\bar{e}, \bar{z}, \bar{u}, \bar{v}) d \tau$ where $T \in (\delta, \infty)$ is the prediction horizon and the function $F$ is given by $F(e,z,u,v) = || (e,z_1-\theta_1, z_2 - \dot{\theta}_{ref})^T||_Q^2 + ||(u,v)^T||_R^2$ with positive semi definite matrix $Q$ and positive definite matrix $R$ , which can be found in the original paper cited above.

The path-following is introduced into the problem by optimizing the error $e$ measuring the deviation of the robot from the path:

$e(x, z) = h(x) - p(z_1)$

where $p$ is the parametrization of $\mathcal{P}$ as above and $h$ is the position of the robot given by:

$h(x_1) = \left( \begin{array}{c} -\cos(q_1)(d_3 \sin(q_2) + \bar{d}_5 + \sin(q_2-q_3))\\ -\sin(q_1)(d_3 \sin(q_2) + \bar{d}_5 + \sin(q_2-q_3))\\ d_1 + d_3 \cos(q_2) + \bar{d}_5 \cos(q_2-q_3)\\ \end{array} \right)$

where $q = x_1$ and $\bar{d}_5 = d_5 + d_{tool}$ with $d_i, i \in \{ 1,3,5 \}$ being the lengths of the link of the robot and $d_{tool}$ is the length of the pen and tool. These values can be found in the Parameters section as well.

OCP

The optimization problem which is solved repetitively is given by

$\begin{array}{llclr} \displaystyle \min_{\bar{u}, \bar{v}} & J &=& J(x(t_k),\bar{e}, \bar{u}, \bar{z}, \bar{v}) \\[1.5ex] \mbox{s.t.} & \dot{\bar{x}} & = & f(\bar{x},\bar{u}), \\ & \dot{\bar{z}} & = & l(\bar{z},\bar{v}), \\ & \bar{e} & = & h(\bar{x}) - p(\bar{z}_1), \\[1ex] & \bar{x}(t_k) & = & x(t_k), \\ & \bar{z}(t_k) & = & z(t_k), \\[1ex] & \bar{x} & \in & \mathcal{X}, \\ & \bar{u} & \in & \mathcal{U}, \\ & \bar{z} & \in & \mathcal{Z}, \\ & \bar{v} & \in & \mathcal{V}. \\ \end{array}$