LAB

<latex>{\fontsize{16pt}\selectfont \textbf{Direct Multiple Shooting for Trajectory Optimization of Articulated Robots}} </latex>

<latex>{\fontsize{12pt}\selectfont \textbf{Markus St\“auble}} </latex>
<latex>{\fontsize{10pt}\selectfont \textit{Master Thesis, RSC}} </latex>

<latex> {\fontsize{12pt}\selectfont \textbf{Abstract} </latex>
This thesis studies optimal trajectory generation for robot motion planning. Optimal trajectory generation is formulated as an optimal control problem minimizing an objective function while respecting certain constraints. The Direct Multiple Shooting (DMS) algorithm is used to transcribe the infinite-dimensional continuous time optimal control problem into a finite dimensional Nonlinear Program (NLP). Two different solvers were used to solve the NLP, the first one being the interior point solver IPOPT, the second one being the sequential quadratic programming solver SNOPT. We compare the two NLP solvers as well as different DMS parameters, like number of nodes or the integration scheme in terms of runtime performance. Furthermore, we implemented a replanning scheme for the DMS algorithm in order to enable it to counteract heavy disturbances like moving obstacles online. The replanning scheme was tested using a gazebo simulation for a hydraulic robot arm. A stabilizing controller was implemented using the Iterative Linear Quadratic Gaussian (ILQG) algorithm in a model predictive control fashion. Finally, we show that the trajectories obtained by DMS are suitable for execution on real hardware: We present experiments performed on an ABB IRB4600 industrial robot arm.

<latex> {\fontsize{12pt}\selectfont \textbf{Optimal Control} </latex>
In this thesis, dynamical systems are modeled as first order ordinary differential equations (ODE):

\begin{equation} \label{eq:nonlinearsystem}

\dot{\mathbf{x}}(t) = f \left( \mathbf{x}(t), \mathbf{u}(t) \right)

\end{equation}

where $\mathbf{x}(t) \in \mathbb{R}^n$ represents the state, $\dot{\mathbf{x}} = \frac{d\mathbf{x}}{dt}$ its time derivative and $\mathbf{u}(t) \in \mathbb{R}^m$ the control input. The considered models are typically nonlinear. We neglect the effect of measurement and process noise in the modelling process. We formulate the optimal control problem as follows:

\begin{equation} \label{eq:optimalcontrol} \begin{aligned} & \underset{\mathbf{x}(\cdot), \mathbf{u}(\cdot)}{\text{minimize}} & & \int_0^T \ L(\mathbf{x}(t), \mathbf{u}(t) ) \ \mathrm{d}t + E(\mathbf{x}(T))
& \text{subject to} & & \mathbf{x}(0) - \mathbf{x}_0 = 0 & \text{(initial value constraint)}
& & & \dot{\mathbf{x}}(t) - f \left( \mathbf{x}(t), \mathbf{u}(t) \right) = 0 & \text{(ODE model)}
& & & h( \mathbf{x}(t), \mathbf{u}(t) ) \geq 0 & \text{(path constraints)}
& & & r(\mathbf{x}(T)) = 0 & \text{(terminal constraint)}
\end{aligned} \end{equation}

This is a non-linear, infinite dimensional, continuous time optimization problem. It is very hard to solve; analytical solutions can typically only be found for strongly simplified, low-dimensional and well-structured problems. The DMS algorithm transcribes this optimization problem into a finite dimensional non-linear program.

<latex> {\fontsize{12pt}\selectfont \textbf{The Direct Multiple Shooting Algorithm} </latex>
The first step of the DMS algorithm is to define a coarse time grid consisting of $N+1$ time points $t_i, \forall i \in [0, … N]$. This results in $N$ intervals between the time points, which we will call shot intervals. The discrete time points $t_i$ are called grid points or nodes. In a next step, the control inputs are discretized on the grid points resulting in a control vector $\mathbf{q} = [\mathbf{q}_0, \mathbf{q}_1, …, \mathbf{q}_N]$, where $\mathbf{q}_i \in \mathbb{R}^m$. The control inputs between the grid points are approximated by a spline interpolation. We use zero-order hold (ZOH) and piecewise linear (PWL) control input spliners for this work. Hence, we can generally write the control input on shot interval $i$ as $\mathbf{u}_i(t) = g(\mathbf{q}_i, \mathbf{q}_{i+1})$ with g being the spline function. The details about the spliners can be found in \Cref{sec:controlinputspliners}.

The state variables are discretized on the nodes resulting in a vector of state decision variables $\mathbf{s} = [\mathbf{s}_0, \mathbf{s}_1, …, \mathbf{s}_N]$, where $\mathbf{s}_i \in \mathbb{R}^n$. The vector of state decision variables is initialized by an initial guess.

Based on the state and input discretization $\mathbf{s}, \mathbf{q}$ the system model ODE on each shotinterval $[t_i, t_{i+1}]$ is integrated forward, using the state decision variable $\mathbf{s}_i$ as initial value: \begin{equation} \label{eq:shotintegration} \begin{aligned} & \dot{\mathbf{x}_i}(t) = f \left( \mathbf{x}_i(t), \mathbf{u}_i(t) \right), & t \in [t_i, t_{i+1}]
& \mathbf{x}_i(t_i) = \mathbf{s}_i \end{aligned} \end{equation} Trajectory pieces $\mathbf{x}_i(t; \mathbf{s}_i, \mathbf{u}_i)$ are obtained with this integration. The integrations are independent of one another, therefore, they can be parallelized over the number of shotintervals $N$.

Simultaneously, the cost is discretized on the time grid and the resulting cost pieces are numerically integrated: \begin{equation} \label{eq:cost} l_i(\mathbf{s}_i, \mathbf{u}_i) = \int_{t_i}^{t_{i+1}} L(\mathbf{x}_i(t_i;\mathbf{s}_i, \mathbf{u}_i), \mathbf{q}_i) \mathrm{d}t \end{equation}

In order to obtain continuous state solutions, N continuity constraints $\left\lbrace c_{cont,i} \right\rbrace_{i = 0}^{N-1}$ of the form $\mathbf{s}_{i+1} = \mathbf{x}_i(t_{i+1}, \mathbf{s}_i, \mathbf{u}_i)$ for each shotinterval are imposed.

The DMS NLP can now be formulated as:

\begin{equation} \label{eq:nlp_dms} \begin{aligned} & \underset{\mathbf{s}, \mathbf{q}}{\text{minimize}} & & \sum_{i = 0}^{N - 1} l_i(\mathbf{s}_i, \mathbf{u}_i ) \ + E(\mathbf{s}_N)
& \text{subject to} & & \mathbf{s}_0 - \mathbf{x}_0 = 0 & \text{(initial value constraint)}
& & & \mathbf{s}_{i+1} - \mathbf{x}(t_{i+1}; \mathbf{s}_i, \mathbf{u}_i) = 0 & \text{(continuity constraints)}
& & & h( \mathbf{s}_i, \mathbf{q}_i ) \geq 0 & \text{(discretized path constraints)}
& & & r(\mathbf{s}_N) = 0 & \text{(terminal constraint)} \end{aligned} \end{equation}

We define the vector of decision variables as $w:= [\mathbf{s}_0, \mathbf{q}_0, \mathbf{s}_1, \mathbf{q}_1, …, \mathbf{s}_N, \mathbf{q}_N]^T$. The NLP can be solved using an off the shelf NLP solver. We use IPOPT ¹⁾ and SNOPT ²⁾ in this thesis.

<latex> {\fontsize{12pt}\selectfont \textbf{Simulation on HyA} </latex>
We used a Gazebo ³⁾ simulation of the hydraulic arm HyA to simulate the feasability of DMS trajectories on a real system. The rectangles in the figure denote ROS⁴⁾ nodes. The messages between the nodes are either exchanged with ROS services or with ROS message topics. The user interface node provides the settings for all the nodes in the simulation. The DMS node plans a robot trajectory $\mathbf{x}_\mathrm{DMS}$ according to the user settings starting from the current state estimate $\mathbf{\hat{x}}$. The DMS node can be run as a batch planner or as a dynamical replanner. The stabiliser node runs an iterative linear quadratic gaussian (ILQG) algorithm⁵⁾in a model predictive control fashion to stabilise the trajectories received from the DMS node. The controller node executes the control law obtained by the stabiliser. All the nodes run asynchronously and perform work when new information is available and the nodes are in an idle state. In our testing setup, the DMS and the ILQG nodes run on a cloud computer (running an Intel E5 CPU), while the rest of the software runs on a local laptop (Intel Core i7-6820HQ 2.7 GHz).

The figure shows a screenshot of the RVIZ visualization of a Gazebo simulation of HyA using the proposed method. The red line is the endeffector trajectory obtained by the DMS algorithm. The green line is the trajectory planned by the ILQG which should track the DMS trajectory.
The HyA travels between two corner points. A spherical obstacle blocks its way. Using dynamic replanning, the hydraulic arm plans around the obstacle online. We use an ILQG MPC timehorizon of 0.5s and limit the maximum number of ILQG iterations to 5. At the corners we wait for the full NLP convergence to obtain the first DMS solution trajectory. We achieve ILQG MPC frequencies of around 50 Hz and DMS replanning frequencies of around 20 Hz. The replanning frequencies vary because they depend on the DMS time horizon. As we shrink the timehorizon online, the DMS problem gets easier to solve for the NLP solver.

<latex> {\fontsize{12pt}\selectfont \textbf{Hardware Experiments on IRB4600} </latex>

We tested the feasability of the DMS solution trajectories on the ABB IRB4600. The ABB IRB4600 is a six axes industrial robot arm. The model of the system consists of 12 states and 6 inputs. The model was obtained using RobCoGen.
We used an APRIL tag⁶⁾ mounted on a wooden stick as taskspace obstacle. Another APRIL tag on the robot base was used to convert the obstacle APRIL tag pose from the camera coordinate frame to the robot's taskspace frame. A visual-intertial sensor and RCARS⁷⁾ were used for tracking the APRIL tags.
The External Guided Motion (EGM) interface was used to send position and velocity references to the robot arm with a frequency of 250 Hz. The simulation of the ABB IRB4600 was conducted with ABB's robot studio. The EGM interface reference positions directly and an internal controller tracks them. Therefore, there is no need for an additional stabilising controller like ILQG for the HyA simulations. The whole software runs on a single laptop (Intel Core i7-6820HQ 2.7 GHz).