BoundMPC: Cartesian Trajectory Planning with Error Bounds based on Model Predictive Control in the Joint Space

TL;DR

BoundMPC introduces a real-time, online MPC framework to follow Cartesian position and orientation references in the joint space of robotic manipulators, while enforcing asymmetric bounds on the orthogonal path error. The method leverages Lie theory for rotations to decompose and bound orientation errors, and uses piecewise linear reference paths with via-points to enable fast online replanning. Key contributions include synchronized via-points in a joint-space MPC, asymmetric 2D error bounding in the orthogonal plane, and demonstrated feasibility with sub-100 ms planning on a 7-DOF Kuka LBR iiwa, including scenarios with tight corridors and dynamic grasp changes. This approach enables robust, collision-free path following in dynamically changing environments and supports real-time adaptation to new goals.

Abstract

This work presents a novel online model-predictive trajectory planner for robotic manipulators called BoundMPC. This planner allows the collision-free following of Cartesian reference paths in the end-effector's position and orientation, including via-points, within desired asymmetric bounds of the orthogonal path error. The path parameter synchronizes the position and orientation reference paths. The decomposition of the path error into the tangential direction, describing the path progress, and the orthogonal direction, which represents the deviation from the path, is well known for the position from the path-following control in the literature. This paper extends this idea to the orientation by utilizing the Lie theory of rotations. Moreover, the orthogonal error plane is further decomposed into basis directions to define asymmetric Cartesian error bounds easily. Using piecewise linear position and orientation reference paths with via-points is computationally very efficient and allows replanning the pose trajectories during the robot's motion. This feature makes it possible to use this planner for dynamically changing environments and varying goals. The flexibility and performance of BoundMPC are experimentally demonstrated by two scenarios on a 7-DoF Kuka LBR iiwa 14 R820 robot. The first scenario shows the transfer of a larger object from a start to a goal pose through a confined space where the object must be tilted. The second scenario deals with grasping an object from a table where the grasping point changes during the robot's motion, and collisions with other obstacles in the scene must be avoided.

Figures (20)

• Figure 1: Schematic of the position path planning using BoundMPC in the 3D Cartesian space. The tangential and orthogonal errors are shown for the initial position with the orthogonal error plane spanned by two basis vectors. The planned path over the planning horizon is within the asymmetric error bounds, depicted as shaded green area.
• Figure 2: Position path error decomposition into the orthogonal and tangential path direction. The black rectangle visualizes the orthogonal error plane spanned by the basis vectors $\boldsymbol{b} _\textunderscore{p, 1}$ and $\boldsymbol{b} _\textunderscore{p, 2}$. The current reference path position is $\boldsymbol{\pi} _\textunderscore{p}(\phi(t))$. The blue lines indicate the errors of the current end-effector position $\boldsymbol{p} _\textunderscore{c}(t)$. The error bounds are indicated by the green rectangle, which is offset from the path by $\boldsymbol{e} _\textunderscore{p, \mathrm{off}}$.
• Figure 3: Comparison of the orientation path error computations. The reference and the current orientation path are computed over a time interval of 2.5s using the constant angular velocities $\boldsymbol{\omega} _{\textunderscore{r}}$ and $\boldsymbol{\omega} _{\textunderscore{c}}$. The true evolution of the norm of \ref{['eq:true_rot_error']} in red is compared to the approximate computation of the norm according to \ref{['eq:drot_err']} in blue.
• Figure 4: 2D Visualization of a position reference path $\boldsymbol{\pi} _{\textunderscore{p}}(\phi)$ with error bounds. The symmetric error bounding functions $\Upsilon_{\textunderscore{p, 1}}$ are adapted by the upper and lower bounds $\boldsymbol{e} _{\textunderscore{p, u, 1}}$ and $\boldsymbol{e} _{\textunderscore{p, l, 1}}$ to asymmetrically bound the orthogonal position path error $\boldsymbol{e} ^{\bot, 1}_{\textunderscore{p}}$. The shaded gray regions indicate the area where $\psi_{\textunderscore{p, 1}}( \boldsymbol{e} _{\textunderscore{p, proj}}^{\bot}) \leq 0$. The error bounding functions $\Upsilon_{\textunderscore{p, 1}}$ and the upper and lower bounds $\boldsymbol{e} _{\textunderscore{p, u, 1}}$ and $\boldsymbol{e} _{\textunderscore{p, l, 1}}$ are defined for both segments between the three shown via-points.
• Figure 5: Visualization of a linear reference path. The current position along the path is $\boldsymbol{\pi} _\textunderscore{p}(\phi(t))$ on the first linear segment.

Theorems & Definitions (5)

• Remark
• Remark
• Remark
• Remark
• Remark