BoundMPC: Cartesian path following with error bounds based on model predictive control in the joint space

Abstract

This work introduces the BoundMPC strategy, an innovative online model-predictive path-following approach for robot manipulators. This joint-space trajectory planner allows the following of Cartesian reference paths in the end-effector’s position and orientation, including via-points, within the desired asymmetric bounds of the orthogonal path error. These bounds encode the obstacle-free space and additional task-specific constraints in Cartesian space. Contrary to traditional path-following concepts, BoundMPC purposefully deviates from the Cartesian reference path in position and orientation to account for the robot’s kinematics, leading to more successful task executions for Cartesian reference paths. Furthermore the simple reference path formulation is computationally efficient and allows for replanning 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 five 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. The adaptability of BoundMPC is showcased in scenarios such as the opening of a drawer, the transfer of an open container, and the wiping of a table, where it effectively handles task-specific constraints. The last scenario highlights the possibility of accounting for collisions with the entire robot’s kinematic chain. The code is readily available at https://github.com/TU-Wien-ACIN-CDS/BoundMPC, inspiring you to explore its potential and adapt it to your specific robotic tasks.

1. Introduction

Robot manipulators are multi-purpose machines with many different applications in industry and research, which include bin picking (Bencak et al., 2022), physical human–robot interaction (Ortenzi et al., 2021), and industrial processes like welding (Lei et al., 2020). During the execution of these applications, various constraints have to be considered, such as safety bounds, collision avoidance, task-specific constraints, and kinematic and dynamic limitations. In general, this leads to challenging trajectory-planning problems. A well-suited control framework for such applications is model predictive control (MPC) (Faulwasser et al., 2009). Using MPC, the control action is computed by optimizing an objective function over a finite planning horizon at every time step while respecting all constraints. Online trajectory planning with MPC is particularly useful for robots acting in dynamically changing environments. Especially when there is uncertainty about actors in the scene, it is crucial to appropriately react to changing situations, for example, when interacting with humans (Li et al., 2021). Trajectory planners that plan the full trajectory from a start to a goal pose (e.g., Jankowski et al., 2023; Marcucci et al., 2023) lack the computational efficiency to react online in these situations.

Robotic tasks are often specified as Cartesian reference paths, parametrized by a path parameter. To execute these tasks accurately, path-following controllers are essential. These controllers ensure that the robot’s end-effector follows the specified path with precision and efficiency (Van Duijkeren et al., 2016). Examples include opening a door, where the end-effector has to follow an arc-shaped path in Cartesian space, opening a drawer with a linear motion, and the transfer of an open container, which must be held upright during the motion. These tasks are challenging for a planner that only plans trajectories based on joint-space objectives. In contrast, a Cartesian reference path consists of a series of Cartesian via-points that the robot’s end-effector needs to pass exactly. It is advantageous to decompose the error between the current position and the desired reference path point (Romero et al., 2022) into a tangential error, representing the error along the path and an orthogonal error. The tangential error is minimized to progress along the path quickly while the orthogonal error is bounded (Arrizabalaga and Ryll, 2022).

Providing a reference path or series of via-points in Cartesian space is intuitive. However, due to the complex robot kinematics, trajectory planning is complicated compared to reference paths or via-points in the robot’s joint space. Furthermore, Cartesian reference paths with path error bounds can simplify collision avoidance and can be used to encode task-specific constraints. Specifying these bounds in the joint space is complicated and requires large computational effort, as shown in Marcucci et al. (2023). Existing work often minimizes the path error to follow the path exactly (Arbo et al., 2017; Astudillo et al., 2022b). This works well if the given path is known to be executable but may lead to the robot getting stuck for challenging paths.

This work introduces a novel online trajectory planner in the joint space, BoundMPC, to follow a Cartesian reference path with the robot’s end-effector while bounding the orthogonal path error. Furthermore, position and orientation reference paths with via-points are considered. A schematic drawing of the geometric relationships is shown in Figure 1. By bounding the orthogonal path error and minimizing the tangential path error, the MPC can find the optimal trajectory according to the robot dynamics and other constraints. The robot purposefully deviates from the path to take the robot’s kinematics into account, which allows the robot to follow even more challenging paths. Simple piecewise linear reference paths with via-points are used to simplify online replanning, and (asymmetric) polynomial error bounds allow us to adapt to different scenarios and obstacles quickly. The end-effector’s position and orientation reference paths are parameterized by the same path parameter, ensuring synchronicity at the via-points. The Lie theory for rotations is employed to describe the end-effector’s orientation, which yields a novel way to decompose and constrain the orientation path error. Moreover, the decomposition of the orthogonal error using the desired basis vectors of the orthogonal error plane allows for meaningful asymmetric bounding of both position and orientation path errors.

Figure 1.

Schematic of the position trajectory 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.

The paper is organized as follows: Related work is summarized in Section 2, and the contributions extending the state of the art are given in Section 3. The general MPC framework BoundMPC is developed in Section 4. The used orthogonal error bounds are described in Section 5. The use of a piecewise linear reference path and its implications are presented in Section 6. Afterward, Section 7 shows the online replanning. The implementation details for the simulations and experiments are given in Section 8. Section 9 deals with the parameter tuning of the MPC. Six experimental scenarios on a 7-DoF Kuka LBR iiwa 14 R820 robot are demonstrated in Section 10, which emphasize the ability of BoundMPC to replan trajectories online during the robot’s motion and to specifically bound the orthogonal errors asymmetrically. Comparisons with existing works are performed, which show the advantages of the proposed framework. Section 11 gives some conclusions and an outlook on future research activities.

2. Related work

Path and trajectory planning in robotics is an essential topic with many different solutions, such as sampling-based planners (Elbanhawi and Simic, 2014; Karaman and Frazzoli, 2011; Persson and Sharf, 2014), learning-based planners (Mac et al., 2016; Mukherjee et al., 2022; Osa, 2022), and optimization-based planners (Faulwasser et al., 2009; Romero et al., 2022; Van Duijkeren et al., 2016), which can be further classified into offline and online planning. Online planning is needed to be able to react to dynamically changing environments. Sampling-based planners have the advantage of being probabilistically complete, meaning they will eventually find a solution if the problem is feasible. However, online planning is limited by computational costs and the need to smooth the obtained trajectories (Ferguson et al., 2006; Zucker et al., 2007). Constrained motion planning with sampling-based methods is discussed in Kingston et al. (2019). Optimization-based trajectory planners, such as TrajOpt (Schulman et al., 2014), CHOMP (Zucker et al., 2013), and CIAO (Schoels et al., 2020b), take dynamics and collision avoidance into account. However, they are generally too slow for online planning due to the nonconvexity of the planning problems. Receding horizon control plans a trajectory for a finite horizon starting at the current state to reduce computational complexity. For example, CIAO-MPC (Schoels et al., 2020b) extends CIAO to receding horizon control for point-to-point motions.

More complex applications require the robot to follow a reference path. Receding horizon control in this setting was demonstrated for quadrotor racing in Arrizabalaga and Ryll (2022) and Romero et al. (2022) and robot manipulators in Arbo et al. (2017); Astudillo et al. (2022b); and Mehrez et al. (2017). Additionally, Romero et al. (2022) include via-points in their framework to safely pass the gates along the racing path. Adapting the current reference path during the motion to react to a changing environment and a varying goal is crucial. While, in theory, the developed MPC frameworks in Arrizabalaga and Ryll (2022), Astudillo et al. (2022b), Lam et al. (2013), and Romero et al. (2022) could handle a change in the reference path, this still needs to be demonstrated for industrial manipulators. Furthermore, these frameworks need pre-optimized reference paths, which often requires the solution of a nonlinear optimization problem (e.g., Verschueren et al., 2016). This step must be performed before the path-following task, making the online replanning computationally infeasible. Alternatively, the orthogonal error bounds can be adapted to reflect a dynamic change in the environment, as done in Arrizabalaga and Ryll (2022).

To be able to use the path following formulations, an arc-length parametrized path is needed. Since the system dynamics are parametrized in time, a reformulation is needed to couple the reference path to the system dynamics as done in Arrizabalaga and Ryll (2022), Böck and Kugi (2016), Debrouwere et al. (2014), Spedicato and Notarstefano (2018), and Van Duijkeren et al. (2016). Such a coupling allows for a very compact formulation but complicates constraint formulations. Therefore, Lam et al. (2013) decouple the system dynamics from the path and uses the objective function to minimize the tangential path error to ensure that the path system is synchronized to the system dynamics.

Tracking the reference path by the planner results in a path error. The goal of the classical path following control is to minimize this error (Astudillo et al., 2022b; Faulwasser et al., 2009), which is desirable if the reference path is optimal. Since optimal reference paths are generally not trivial, exploiting the path error to improve the performance has been considered in Arrizabalaga and Ryll (2022) and Romero et al. (2022). Further control on utilizing the path error is given by decomposing it into tangential and orthogonal components. To safely traverse via-points, Romero et al. (2022) use a dynamical weighting of the orthogonal path error, achieving collision-free trajectories. Furthermore, the orthogonal path error lies in the plane orthogonal to the path and can thus be decomposed further using basis vectors of the plane. This way, surface-based path following was proposed in Hartl-Nesic et al. (2021), where the direction orthogonal to the surface was used to assign a variable stiffness. The choice of basis vectors is application specific. Concretely, Arrizabalaga and Ryll (2022) use the Frenet–Serret frame to decompose the orthogonal path error. It provides a continuously changing frame along the path but is not defined for path sections with zero curvature. An alternative to the Frenet–Serret frame is the parallel transport frame used in Bischof et al. (2017). The decomposition of the orthogonal error allows for asymmetric bounding in different directions of the orthogonal error plane, but this has yet to be demonstrated in the literature.

Many path-following controllers only consider a position reference trajectory and thus, a position path error. For quadrotors, this is sufficient since the orientation is given by the dynamics (Romero et al., 2022). The orientation was included in Astudillo et al. (2022a, 2022b) for robot manipulators, where the respective orientation error formulation allows arbitrarily large errors. However, their implementation minimizes the orientation path error, which is only feasible for a path known to be kinematically executable by the robot. For arbitrary paths, this may lead to the robot getting stuck in, for example, the joint limits. The orientation in Van Duijkeren et al. (2016) is even considered constant. In Hartl-Nesic et al. (2021), the orientation is set relative to the surface and is not freely optimized. Thus, the literature has yet to treat orientation reference paths without minimizing the orientation path error. Furthermore, a decomposition of the orientation path error analogous to the position path error is missing in the literature, which can further improve the performance of path-following controllers. In this work, orientation reference paths are included using the Lie theory for rotations, which is beneficial since the rotation propagation can be linearized. In Torres Alberto et al. (2022), this property was exploited for pose estimation based on the theory in Solà et al. (2021). Forster et al. (2017) use the same idea for visual-inertial odometry. In this work, the Lie theory for rotations is exploited to decompose the orientation path error and the propagation of this error over time.

3. Contributions

The proposed MPC framework BoundMPC combines concepts discussed in Section 2 in a novel way. It uses a receding horizon implementation to follow a reference path in the position and orientation with asymmetric error bounds in distinct orthogonal error directions. A path parameter system similar to Lam et al. (2013) ensures path progress. Using linear reference paths with via-points yields a simple representation of reference paths and allows for fast online replanning without providing optimal reference paths. Additionally, the orientation has a distinct reference path, and the orientation path errors are freely optimized to account for the robot’s kinematics instead of minimizing them, as in previous works. The novel asymmetric error bounds allow more control over trajectory generation and obstacle avoidance. Through its simplicity of only providing the desired via-points, BoundMPC finds the optimal position and orientation path within the given error bounds. The main contributions are:

• A path-following MPC framework to follow position and orientation paths with synchronized via-points is developed. The robot kinematics are considered in the optimization problem such that a joint trajectory is optimized while adhering to (asymmetric) Cartesian constraints.

• The position and orientation path errors, computed based on the Lie theory for rotations, are decomposed and bounded to be freely optimized within the given bounds. This allows for more freedom during the trajectory optimization compared to existing approaches that minimize tracking errors.

• Piecewise linear position and orientation reference paths with via-points can be replanned online to quickly react to dynamically changing environments and new goals during the robot’s motion.

• The framework is demonstrated on a 7-DoF Kuka LBR iiwa 14 R820 manipulator with planning times less than 100 ms. A video of the experiments can be found at https://www.acin.tuwien.ac.at/42d0/.

4. BoundMPC formulation

This section describes the BoundMPC formulation, which is a path-following concept for Cartesian position and orientation reference paths. This concept is schematically illustrated in Figure 1. The proposed formulation computes a trajectory in the joint space, requiring an underlying torque controller to follow the planned trajectory.

First, the formulation of the dynamical systems, that is, the robot manipulator and the jerk input system, is introduced. Second, the orientation representation based on Lie algebra and its linearization is used to calculate the tangential and orthogonal orientation path errors based on arc-length parameterized reference paths. A similar decomposition is also introduced for the position path error, finally leading to the optimal control problem formulation.

4.1. Dynamical system

The robot manipulator dynamics for n joints are described in the joint positions $\mathbf{q}\in {\mathbb{R}}^n$ , velocities $\dot{\mathbf{q}}\in {\mathbb{R}}^n$ , and accelerations $\ddot{\mathbf{q}}\in {\mathbb{R}}^n$ as the rigid-body model

$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}+\mathbf{g}(\mathbf{q})=\mathit{\tau }\text{,}$

(1)

with the mass matrix M(q), the Coriolis matrix $\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})$ , the gravitational forces g(q), and the joint input torque $\mathit{\tau }\in {\mathbb{R}}^n$ . For trajectory following control, the computed-torque controller (Siciliano et al., 2009)

$\begin{array}{cc}\hfill \mathit{\tau }=& \mathbf{M}(\mathbf{q})\left({\ddot{\mathbf{q}}}_{\text{d}}(t)-{\mathbf{K}}_2{\dot{\mathbf{e}}}_{\text{q}}-{\mathbf{K}}_1{\mathbf{e}}_{\text{q}}-{\mathbf{K}}_0\int {\mathbf{e}}_{\text{q}}\mathrm{d}t\right)\hfill \\ \hfill & +\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}+\mathbf{g}(\mathbf{q})\text{,}\hfill \end{array}$

(2)

with the desired joint-space trajectory q_d(t) and its time derivatives, is applied to (1). The tracking error e_q = q − q_d is exponentially stabilized in the closed-loop system (1), (2) by choosing suitable matrices K₀, K₁, and K₂, typically by pole placement.

4.1.1. Joint jerk parametrization

To obtain the desired trajectory q_d(t), a joint jerk input signal ${\stackrel{⃛}{\mathbf{q}}}_{\text{d}}(t)$ is introduced, which is parameterized using hat functions similar to Hausberger et al. (2019). The hat functions H _k (t), k = 0, …, K − 1, given by

$H_k(t)=\left\{\begin{array}{cc}\frac{t-t_k}{T_{\text{s}}}\hfill & t_k\le t<t_k+T_{\text{s}}\hfill \\ \frac{t_k+2T_{\text{s}}-t}{T_{\text{s}}}\hfill & t_k+T_{\text{s}}\le t\le t_k+2T_{\text{s}}\hfill \\ 0\hfill & \text{otherwise}\text{,}\hfill \end{array}\right.$

(3)

are parametrized by the starting time t _k and the width 2T_s. The index k refers to an equidistant time grid t _k = kT_s on the continuous time t. For each joint at the discretization points, the desired jerk values are denoted by the vectors ${\mathbf{j}}_k\in {\mathbb{R}}^n$ , k = 0,…, K − 1. Thus, the continuous joint jerk input

${\stackrel{\dots }{\mathbf{q}}}_{\text{d}}(t)=\sum _{k=0}^{K-1}{H}_k(t+T_s){\mathbf{j}}_k$

(4)

results from the summation of all hat functions over the considered time interval. The joint jerk (4) is a linear interpolation of the joint jerk values j _k . The desired trajectory for the joint accelerations ${\ddot{\mathbf{q}}}_{\text{d}}(t)$ , joint velocities ${\dot{\mathbf{q}}}_{\text{d}}(t)$ , and joint positions q_d(t) are found analytically by integrating (4) with respect to the time t for t₀ = 0

$\begin{array}{ll}\hfill {\ddot{\mathbf{q}}}_{\text{d}}(t)=& {\ddot{\mathbf{q}}}_{\mathrm{d},0}+\sum _{k=0}^{K-1}{\int }_0^t{H}_k(\tau +T_{\text{s}})\mathrm{d}\tau {\mathbf{j}}_k\hfill \end{array}$

(5a)

$\begin{array}{ll}\hfill {\dot{\mathbf{q}}}_{\text{d}}(t)=& {\dot{\mathbf{q}}}_{\mathrm{d},0}+{\ddot{\mathbf{q}}}_{\mathrm{d},0}t+\sum _{k=0}^{K-1}{\iint }_0^t{H}_k(\tau +T_{\text{s}})\mathrm{d}\tau {\mathbf{j}}_k\hfill \end{array}$

(5b)

$\begin{array}{ll}\hfill {\mathbf{q}}_{\text{d}}(t)=& {\mathbf{q}}_{\mathrm{d},0}+{\dot{\mathbf{q}}}_{\mathrm{d},0}t+{\ddot{\mathbf{q}}}_{\mathrm{d},0}\frac{t^2}{2}\hfill \\ \hfill & +\sum _{k=0}^{K-1}{\iiint }_0^t{H}_k(\tau +T_{\text{s}})\mathrm{d}\tau {\mathbf{j}}_k\text{.}\hfill \end{array}$

(5c)

The integration constants q_d(0) = q_d,0, ${\dot{\mathbf{q}}}_{\text{d}}(0)={\dot{\mathbf{q}}}_{\mathrm{d},0}$ , and ${\ddot{\mathbf{q}}}_{\text{d}}(0)={\ddot{\mathbf{q}}}_{\mathrm{d},0}$ are given by the initial values at the time t₀ = 0.

4.1.2. Linear time-invariant system

In the following, (5) is described by a linear time-invariant (LTI) system with the state ${\mathbf{x}}^{\text{T}}=[{\mathbf{q}}_{\text{d}}^{\text{T}},{\dot{\mathbf{q}}}_{\text{d}}^{\text{T}},{\ddot{\mathbf{q}}}_{\text{d}}^{\text{T}}]$ . The corresponding discrete-time LTI system with the sampling time T_s then reads as

${\mathbf{x}}_{k+1}=\mathbf{\Phi }{\mathbf{x}}_k+{\mathbf{\Gamma }}_0{\mathbf{u}}_k+{\mathbf{\Gamma }}_1{\mathbf{u}}_{k+1}\text{,}$

(6)

with the state x _k and the input u _k = j _k .

4.1.3. Robot kinematics

The forward kinematics of the robot are mappings from the joint space to the Cartesian space and describe the Cartesian pose of the robot’s end-effector. Given the joint positions q of the robot, the Cartesian position map for the forward kinematics reads as ${\mathbf{p}}_{\text{c}}(\mathbf{q}):{\mathbb{R}}^n\to {\mathbb{R}}^3$ and for the orientation ${\mathbf{R}}_{\text{c}}(\mathbf{q}):{\mathbb{R}}^n\to {\mathbb{R}}^{3\times 3}$ , with the rotation matrix R_c.

By using the geometric Jacobian $\mathbf{J}(\mathbf{q})\in {\mathbb{R}}^{6\times n}$ , the Cartesian velocity v_c and the angular velocity ω_c of the end-effector are related to the joint velocity $\dot{\mathbf{q}}$ by

$\left[\begin{array}{c}\hfill {\mathbf{v}}_{\text{c}}\hfill \\ \hfill {\mathit{\omega }}_{\text{c}}\hfill \end{array}\right]=\mathbf{J}(\mathbf{q})\dot{\mathbf{q}}\text{,}$

(7)

which yields the Cartesian and angular accelerations as

$\left[\begin{array}{c}\hfill {\mathbf{a}}_{\text{c}}\hfill \\ \hfill {\dot{\mathit{\omega }}}_{\text{c}}\hfill \end{array}\right]=\frac{\mathrm{d}}{\mathrm{d}t}\left[\begin{array}{c}\hfill {\mathbf{v}}_{\text{c}}\hfill \\ \hfill {\mathit{\omega }}_{\text{c}}\hfill \end{array}\right]=\mathbf{J}(\mathbf{q})\ddot{\mathbf{q}}+\dot{\mathbf{J}}(\mathbf{q})\dot{\mathbf{q}}\text{.}$

(8)

If n > 6, the robot is kinematically redundant and the Jacobian J(q) becomes a non-square matrix. Redundant robots, such as the Kuka LBR iiwa 14 R820 used in this work, have a joint nullspace, which has to be considered for planning and control.

4.2. Orientation representation

The orientation of the end-effector in the Cartesian space can be represented in different ways, such as rotation matrices, quaternions, and Euler angles. In this work, the Lie algebra

$\mathit{\tau }=\theta \mathbf{u}$

(9)

of the rotation matrix R is used. An orientation is thus specified by an angle θ around the unit length axis u. The most relevant results of the Lie theory for rotations, utilized in this work, are summarized from Solà et al. (2021) in the following.

4.2.1. Mappings

To transform a rotation matrix R into its equivalent (non-unique) element τ in the Lie space, the exponential and logarithmic mappings

$\begin{array}{ll}\hfill \text{Exp}:& {\mathbb{R}}^3\to {\mathbb{R}}^{3\times 3}:\mathit{\tau }\to \mathbf{R}=\text{Exp}(\mathit{\tau })\hfill \end{array}$

(10)

$\begin{array}{ll}\hfill \text{Log}:& {\mathbb{R}}^{3\times 3}\to {\mathbb{R}}^3:\mathbf{R}\to \mathit{\tau }=\text{Log}(\mathbf{R})\hfill \end{array}$

(11)

are used, with the definitions

$\begin{array}{ll}\hfill \text{Exp}(\theta \mathbf{u})& =\mathbf{I}+\sin (\theta ){[\mathbf{u}]}_{\times }+(1-\cos (\theta )){[\mathbf{u}]}_{\times }^2\hfill \end{array}$

(12)

$\begin{array}{ll}\hfill {[\text{Log}(\mathbf{R})]}_{\times }& =\frac{\varphi (\mathbf{R}-{\mathbf{R}}^{\text{T}})}{2\sin (\varphi )}\text{,}\hfill \end{array}$

(13)

the identity matrix I, and ϕ = cos⁻¹ $\left(\frac{\text{trace}\left(\mathbf{R}\right)-1}{2}\right)$ . The operator

${\left[\mathbf{u}\right]}_{\times }={\left[\left[\begin{array}{c}\hfill u_x\hfill \\ \hfill u_y\hfill \\ \hfill u_z\hfill \end{array}\right]\right]}_{\times }=\left[\begin{array}{ccc}\hfill 0\hfill & \hfill -u_z\hfill & \hfill u_y\hfill \\ \hfill u_z\hfill & \hfill 0\hfill & \hfill -u_x\hfill \\ \hfill -u_y\hfill & \hfill u_x\hfill & \hfill 0\hfill \end{array}\right]$

(14)

converts a vector into a skew-symmetric matrix.

4.2.2. Concatenation of rotations

A rotation matrix may result from the concatenation of multiple rotation matrices. An example is the Euler angles which are the result of three consecutive rotations around distinct rotation axes. Concretely, let us assume that

$\mathbf{R}={\mathbf{R}}_3{\mathbf{R}}_2{\mathbf{R}}_1=\text{Exp}({\mathit{\tau }}_3)\text{Exp}({\mathit{\tau }}_2)\text{Exp}({\mathit{\tau }}_1)=\text{Exp}(\mathit{\tau })$

(15)

is the combination of the rotation matrices R₁, R₂, and R₃. By using the Lie theory for rotations, (15) can be approximated around R₂ as (Solà et al., 2021)

$\begin{array}{cc}\hfill \mathit{\tau }& =\text{Log}({\mathbf{R}}_3{\mathbf{R}}_2{\mathbf{R}}_1)\hfill \\ \hfill & \approx \text{Log}({\mathbf{R}}_3{\mathbf{R}}_2)+{\mathbf{J}}_{\text{r}}^{-1}(\text{Log}({\mathbf{R}}_3{\mathbf{R}}_2)){\mathit{\tau }}_1\hfill \\ \hfill & \approx {\mathit{\tau }}_2+{\mathbf{J}}_{\text{l}}^{-1}({\mathit{\tau }}_2){\mathit{\tau }}_3+{\mathbf{J}}_{\text{r}}^{-1}(\text{Log}({\mathbf{R}}_3{\mathbf{R}}_2)){\mathit{\tau }}_1\text{,}\hfill \end{array}$

(16)

where the left and right inverse Jacobians

$\begin{array}{ll}\hfill {\mathbf{J}}_{\text{l}}^{-1}(\mathit{\tau })& =\mathbf{I}-\frac{1}{2}{[\mathit{\tau }]}_{\times }+\left(\frac{1}{{\theta }^2}-\frac{1+\cos \theta }{2\theta \sin \theta }\right){[\mathit{\tau }]}_{\times }^2\hfill \end{array}$

(17)

$\begin{array}{ll}\hfill {\mathbf{J}}_{\text{r}}^{-1}(\mathit{\tau })& =\mathbf{I}+\frac{1}{2}{[\mathit{\tau }]}_{\times }+\left(\frac{1}{{\theta }^2}-\frac{1+\cos \theta }{2\theta \sin \theta }\right){[\mathit{\tau }]}_{\times }^2\hfill \end{array}$

(18)

are defined using θ = ‖τ‖₂. Approximations around R₁ and R₃ are obtained analogously to (16). Note that for transposed rotation matrices the additions in (16) become subtractions.

4.3. Reference path formulation

In this work, each path point is given as a Cartesian pose, which motivates a separation of the position and orientation path as ${\mathit{\pi }}_{\text{p}}(\varphi )\in {\mathbb{R}}^3$ and ${\mathit{\pi }}_{\text{o}}(\varphi )\in {\mathbb{R}}^3$ , respectively, with the same path parameter ϕ. The position reference path π _p(ϕ) is assumed to be arc-length parametrized. The arc-length parametrization implies $\Vert \frac{\partial }{\partial \varphi }{\mathit{\pi }}_{\text{p}}{\Vert }_2=\Vert {\mathit{\pi }}_{\text{p}}^{\prime }{\Vert }_2=1,\forall \varphi \in [0,{\varphi }_{\text{f}}]$ , where ϕ_f is the total arc length of the path. The orientation path uses the same path parameter ϕ and is therefore, in general, not arc-length parametrized. To couple the two paths, the orientation path π _o(ϕ) is parametrized such that the via-points coincide with the position path π _p(ϕ) and the orientation path also ends at ϕ_f. Hence, the orientation path derivative is in general not normalized, which will become important later.

4.4. Path error

In this section, the position and orientation path errors are computed and decomposed based on the tangential path directions ${\mathit{\pi }}_{\text{p}}^{\prime }=\frac{\partial }{\partial \varphi }{\mathit{\pi }}_{\text{p}}$ and ${\mathit{\pi }}_{\text{o}}^{\prime }=\frac{\partial }{\partial \varphi }{\mathit{\pi }}_{\text{o}}$ . First, the position path error and its decomposition are described. Second, the orientation path error is decomposed similarly using the formulation from Section 4.2.

4.4.1. Position path error

The position path error between the current end-effector position p_c(q) and the reference path in the Cartesian space

${\mathbf{e}}_{\text{p}}(\mathbf{q},\varphi (t))={\mathbf{p}}_{\text{c}}(\mathbf{q})-{\mathit{\pi }}_{\text{p}}(\varphi (t))$

(19)

is a function of the joint positions q and the path parameter ϕ(t) using the forward kinematics p_c(q) of the robot.

4.4.2. Position path error decomposition

The position path error (19) is decomposed into a tangential and an orthogonal error. Figure 2 shows a visualization of the geometric relations. The tangential part

${\mathbf{e}}_{\text{p}}^{\Vert }(\mathbf{q},\varphi (t))=\left({\mathit{\pi }}_{\text{p}}^{\prime }{(\varphi (t))}^{\text{T}}{\mathbf{e}}_{\text{p}}\left.(\mathbf{q},\varphi (t))\right)\right){\mathit{\pi }}_{\text{p}}^{\prime }(\varphi (t))$

(20)

is the projection onto the tangent ${\mathit{\pi }}_{\text{p}}^{\prime }(\varphi (t))$ of the reference path. Thus, the remaining error is the orthogonal path error, reading as

${\mathbf{e}}_{\text{p}}^{\perp }(\mathbf{q},\varphi (t))={\mathbf{e}}_{\text{p}}(\mathbf{q},\varphi (t))-{\mathbf{e}}_{\text{p}}^{\Vert }(\mathbf{q},\varphi (t))\text{.}$

(21)

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 b_p,1 and b_p,2. The current reference path position is π _p(ϕ(t)). The blue lines indicate the errors of the current end-effector position p_c(t). The error bounds are indicated by the green rectangle, which is offset from the path by e_p,off.

The time derivatives of (20) and (21) yield

$\begin{array}{ll}\hfill {\dot{\mathbf{e}}}_{\text{p}}=& {\mathbf{v}}_{\text{c}}(\mathbf{q})-{\mathit{\pi }}_{\text{p}}^{\prime }(\varphi (t))\dot{\varphi }(t)\hfill \end{array}$

(22a)

$\begin{array}{ll}\hfill {\ddot{\mathbf{e}}}_{\text{p}}=& {\mathbf{a}}_{\text{c}}(\mathbf{q})-{\mathit{\pi }}_{\text{p}}^{″}(\varphi (t)){\dot{\varphi }}^2(t)-{\mathit{\pi }}_{\text{p}}^{\prime }(\varphi (t))\ddot{\varphi }(t)\hfill \end{array}$

(22b)

$\begin{array}{ll}\hfill {\dot{\mathbf{e}}}_{\text{p}}^{\Vert }=& ({\left({\mathit{\pi }}_{\text{p}}^{″}(\varphi (t))\dot{\varphi }(t)\right)}^{\text{T}}{\mathbf{e}}_{\text{p}}(\mathbf{q},\varphi (t))){\mathit{\pi }}_{\text{p}}^{\prime }(\varphi (t))\hfill \\ \hfill & +\left({\mathit{\pi }}_{\text{p}}^{\prime }{(\varphi (t))}^{\text{T}}{\dot{\mathbf{e}}}_{\text{p}}(\mathbf{q},\varphi (t))\right){\mathit{\pi }}_{\text{p}}^{\prime }(\varphi (t))\hfill \\ \hfill & +\left({\mathit{\pi }}_{\text{p}}^{\prime }{(\varphi (t))}^{\text{T}}{\mathbf{e}}_{\text{p}}(\mathbf{q},\varphi (t))\right){\mathit{\pi }}_{\text{p}}^{″}(\varphi (t))\dot{\varphi }(t)\hfill \end{array}$

(22c)

$\begin{array}{ll}\hfill {\dot{\mathbf{e}}}_{\text{p}}^{\perp }=& {\dot{\mathbf{e}}}_{\text{p}}(\mathbf{q},\varphi (t))-{\dot{\mathbf{e}}}_{\text{p}}^{\Vert }(\mathbf{q},\varphi (t))\text{.}\hfill \end{array}$

(22d)

In the following, the function arguments will be omitted for clarity of presentation. The orthogonal path error ${\mathbf{e}}_{\text{p}}^{\perp }$ from (21) is further decomposed using the orthonormal basis vectors b_p,1 and b_p,2 of the orthogonal error plane. The orthogonal position path errors ${\mathbf{e}}_{\text{p}}^{\perp ,1}$ , ${\mathbf{e}}_{\text{p}}^{\perp ,2}$ are then obtained by projection onto these basis vectors as

$\begin{array}{ll}\hfill {\mathbf{e}}_{\mathrm{p},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j}}^{\perp }& =\left[\begin{array}{c}\hfill e_{\mathrm{p},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},1}^{\perp }\hfill \\ \hfill e_{\mathrm{p},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},2}^{\perp }\hfill \end{array}\right]=\left[\begin{array}{c}\hfill {\mathbf{b}}_{\text{p,\hspace{0.17em}}1}^{\text{T}}\hfill \\ \hfill {\mathbf{b}}_{\text{p,\hspace{0.17em}}2}^{\text{T}}\hfill \end{array}\right]{\mathbf{e}}_{\text{p}}^{\perp }\hfill \end{array}$

(23a)

$\begin{array}{ll}\hfill {\mathbf{e}}_{\text{p}}^{\perp ,i}& =e_{\mathrm{p},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},i}^{\perp }{\mathbf{b}}_{\mathrm{p},i}\text{,\hspace{0.17em}}i=\mathrm{1,2}\text{.}\hfill \end{array}$

(23b)

The basis vectors b_p,1 and b_p,2 are obtained using the Gram–Schmidt procedure by providing a desired basis vector b_p,d and using the reference velocity vector ${\mathbf{v}}_{\text{r}}={\mathit{\pi }}_{\text{p}}^{\prime }(\varphi (t))$ , leading to

$\begin{array}{ll}\hfill \mathbf{w}({\mathbf{v}}_{\text{r}},{\mathbf{b}}_{\mathrm{p},\mathrm{d}})& =\left(\frac{{\mathbf{v}}_{\text{r}}^{\text{T}}}{\Vert {\mathbf{v}}_{\text{r}}{\Vert }_2}{\mathbf{b}}_{\mathrm{p},\mathrm{d}}\right)\frac{{\mathbf{v}}_{\text{r}}}{\Vert {\mathbf{v}}_{\text{r}}{\Vert }_2}\hfill \end{array}$

(24a)

$\begin{array}{ll}\hfill \mathbf{u}({\mathbf{v}}_{\text{r}},{\mathbf{b}}_{\mathrm{p},\mathrm{d}})& ={\mathbf{b}}_{\mathrm{p},\mathrm{d}}-\mathbf{w}({\mathbf{v}}_{\text{r}},{\mathbf{b}}_{\mathrm{p},\mathrm{d}})\hfill \end{array}$

(24b)

$\begin{array}{ll}\hfill {\mathbf{b}}_{\text{p,\hspace{0.17em}}1}& =\frac{\mathbf{u}({\mathbf{v}}_{\text{r}},{\mathbf{b}}_{\mathrm{p},\mathrm{d}})}{\Vert \mathbf{u}({\mathbf{v}}_{\text{r}},{\mathbf{b}}_{\mathrm{p},\mathrm{d}}){\Vert }_2}\hfill \end{array}$

(24c)

as the first normalized basis vector. The cross product b_p,2 = v_r × b_p,1 gives the second basis vector. Note that b_p,d and v_r must be linearly independent.

4.4.3. Orientation path error

In the following, the orientation path errors are derived using rotation matrices. Then the errors are approximated by applying the Lie theory for rotations from Section 4.2. The orientation path error between the planned and the reference path

${\mathbf{e}}_{\text{o}}(t)=\text{Log}\left({\mathbf{R}}_{\text{c}}(t){\mathbf{R}}_{\text{r}}^{\text{T}}(\varphi (t))\right)$

(25)

is a function of the time t, with the rotation matrix representation of the orientation reference path R_r(ϕ(t)) = Exp( π _o(ϕ(t))) and the current state rotation matrix R_c(t) = R_c(q(t)). These rotation matrices are related to the respective angular velocities ω _c and ω _r in the form

$\begin{array}{ll}\hfill {\dot{\mathbf{R}}}_{\text{c}}(t)& ={[{\mathit{\omega }}_{\text{c}}(t)]}_{\times }{\mathbf{R}}_{\text{c}}(t)\hfill \end{array}$

(26a)

$\begin{array}{ll}\hfill \frac{\partial {\mathbf{R}}_{\text{r}}(\varphi )}{\partial \varphi }& ={[{\mathit{\omega }}_{\text{r}}(\varphi (t))]}_{\times }{\mathbf{R}}_{\text{r}}(\varphi (t))\text{,}\hfill \end{array}$

(26b)

where the reference path depends on the path parameter ϕ(t). For constant angular velocities ω _c and ω _r, the closed-form solutions for (26) over a time span Δt exist in the form

$\begin{array}{ll}\hfill {\mathbf{R}}_{\text{c}}(t+\mathrm{\Delta }t)& =\text{Exp}({\mathit{\omega }}_{\text{c}}\mathrm{\Delta }t){\mathbf{R}}_{\text{c}}(t)\hfill \end{array}$

(27a)

$\begin{array}{ll}\hfill {\mathbf{R}}_{\text{r}}(\varphi (t)+\mathrm{\Delta }\varphi )& =\text{Exp}({\mathit{\omega }}_{\text{r}}\mathrm{\Delta }\varphi ){\mathbf{R}}_{\text{r}}(\varphi (t))\text{,}\hfill \end{array}$

(27b)

with Δϕ = ϕ(t + Δt) − ϕ(t). Using (16) and $\mathrm{\Delta }\varphi \approx \dot{\varphi }(t)\mathrm{\Delta }t$ , the orientation path error e_o(t) propagates as

$\begin{array}{cc}\hfill {\mathbf{e}}_{\text{o}}& (t+\mathrm{\Delta }t)\hfill \\ \hfill & =\text{Log}(\text{Exp}({\mathit{\omega }}_{\text{c}}\mathrm{\Delta }t){\mathbf{R}}_{\text{c}}(t){\mathbf{R}}_{\text{r}}^{\text{T}}(\varphi (t))\text{Exp}{({\mathit{\omega }}_{\text{r}}\mathrm{\Delta }\varphi )}^{\text{T}})\hfill \\ \hfill & \approx {\mathbf{e}}_{\text{o}}(t)+\mathrm{\Delta }t\left[{\mathbf{J}}_{\text{l}}^{-1}({\mathbf{e}}_{\text{o}}(t)){\mathit{\omega }}_{\text{c}}\right.\hfill \\ \hfill & \left.-{\mathbf{J}}_{\text{r}}^{-1}(\text{Log}(\text{Exp}({\mathit{\omega }}_{\text{c}}\mathrm{\Delta }t){\mathbf{R}}_{\text{c}}(t){\mathbf{R}}_{\text{r}}^{\text{T}}(\varphi (t)))){\mathit{\omega }}_{\text{r}}\dot{\varphi }(t)\right]\hfill \\ \hfill & \approx {\mathbf{e}}_{\text{o}}(t)+\mathrm{\Delta }t\left[{\mathbf{J}}_{\text{l}}^{-1}({\mathbf{e}}_{\text{o}}(t)){\mathit{\omega }}_{\text{c}}-{\mathbf{J}}_{\text{r}}^{-1}({\mathbf{e}}_{\text{o}}(t)){\mathit{\omega }}_{\text{r}}\dot{\varphi }(t)\right]\text{,}\hfill \end{array}$

(28)

where the last step approximates the right Jacobian around the time t. For Δt → 0, (28) can be written as

${\dot{\mathbf{e}}}_{\text{o}}(t)={\mathbf{J}}_{\text{l}}^{-1}({\mathbf{e}}_{\text{o}}(t)){\mathit{\omega }}_{\text{c}}(t)-{\mathbf{J}}_{\text{r}}^{-1}({\mathbf{e}}_{\text{o}}(t)){\mathit{\omega }}_{\text{r}}(\varphi (t))\dot{\varphi }(t)\text{,}$

(29)

which is integrated to obtain the orientation path error. Figure 3 shows a comparison of this approximation with the true error (25).

Figure 3.

Comparison of the orientation path error computations. The reference and the current orientation path are computed over a time interval of 2.5 s using the constant angular velocities ω _r and ω _c. The true evolution of the norm of (25) in red is compared to the approximate computation of the norm according to (29) in blue.

4.4.4. Orientation path error decomposition

The orientation path error is decomposed similarly to the position path error. The rotation path error matrix R^e = Exp(e_o) consists of the orthogonal path errors R^⊥,1 and R^⊥,2 as well as the tangential path error R^‖, given by

${\mathbf{R}}^{\text{e}}={\mathbf{R}}^{\perp ,2}{\mathbf{R}}^{\Vert }{\mathbf{R}}^{\perp ,1}={\mathbf{R}}_{\text{c}}{\mathbf{R}}_{\text{r}}^{\text{T}}\text{,}$

(30)

with

$\begin{array}{ll}\hfill {\mathbf{R}}^{\perp ,1}& =\text{Exp}({\mathbf{e}}_{\text{o}}^{\perp ,1})=\text{Exp}(\alpha {\mathbf{b}}_{\text{o,\hspace{0.17em}}1})\hfill \end{array}$

(31a)

$\begin{array}{ll}\hfill {\mathbf{R}}^{\Vert }& =\text{Exp}({\mathbf{e}}_{\text{o}}^{\Vert })=\text{Exp}(\beta {\mathit{\omega }}_{\text{r}})\hfill \end{array}$

(31b)

$\begin{array}{ll}\hfill {\mathbf{R}}^{\perp ,2}& =\text{Exp}({\mathbf{e}}_{\text{o}}^{\perp ,2})=\text{Exp}(\gamma {\mathbf{b}}_{\text{o,\hspace{0.17em}}2})\hfill \end{array}$

(31c)

and the orthonormal orientation axes b_o,1, b_o,2, and ω _r. The basis vectors b_o,1 and b_o,2 are obtained by following the procedure of the position path error in (24) with the angular velocity ω _r instead of the Cartesian velocity v_r and with the desired basis vector b_o,d. In this work, the projection of the error rotation matrix R^e onto the orientation axes b_o,1, b_o,2, and ω _r is defined as

$\underset{\alpha ,\beta ,\gamma }{\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{i}\mathrm{n}}\Vert \text{Log}({\mathbf{R}}^{\text{e}}{\left({\mathbf{R}}^{\perp ,1}\right)}^{\text{T}}{\left({\mathbf{R}}^{\Vert }\right)}^{\text{T}}{\left({\mathbf{R}}^{\perp ,2}\right)}^{\text{T}}){\Vert }_2\text{,}$

(32)

which gives the optimal values α*, β*, and γ* that minimize the deviation from R^e. Note that this formulation is equivalent to roll, pitch, and yaw (RPY) angles, when b_o,1, b_o,2, and ω _r are the z, x, and y axes, respectively (Ang and Tourassis, 1987). Thus, solving (32) efficiently is done by computing the RPY angles for

${\mathbf{R}}_{\text{RPY}}={\left({\mathbf{R}}_{\text{RPY}}^{\text{e}}\right)}^{\text{T}}{\mathbf{R}}^{\text{e}}{\mathbf{R}}_{\text{RPY}}^{\text{e}}\text{,}$

(33)

with

${\mathbf{R}}_{\text{RPY}}^{\text{e}}=\left[\begin{array}{c}\hfill {\mathbf{b}}_{\text{o,\hspace{0.17em}}2}|{\mathit{\omega }}_{\text{r}}|{\mathbf{b}}_{\text{o,\hspace{0.17em}}1}\hfill \end{array}\right]$

(34)

as the rotation matrix between the axes for the RPY angles and the desired axes for the error computation. The RPY representation has singularities when β becomes 90° or 270° (Ang and Tourassis, 1987). In this work, the tangential orientation path error R^‖ is minimized, as will be shown in Section 4.5. Thus, the singularities are avoided by letting R^‖ represent the pitch angle β and minimizing it, which also explains the order of the matrix multiplication in (30).

Using the Lie representation and the approximation (16), the optimization problem (32) can be written as

$\begin{array}{cc}\hfill \underset{\alpha ,\beta ,\gamma }{\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{i}\mathrm{n}}\Vert {\mathbf{e}}_{\text{o}}& -\gamma {\mathbf{J}}_{\text{r}}^{-1}(\text{Log}({\mathbf{R}}^{\text{e}}{\left({\mathbf{R}}^{\perp ,1}\right)}^{\text{T}}{\left({\mathbf{R}}^{\Vert }\right)}^{\text{T}})){\mathbf{b}}_{\text{o,\hspace{0.17em}}2}\hfill \\ \hfill & -\beta {\mathbf{J}}_{\text{r}}^{-1}(\text{Log}({\mathbf{R}}^{\text{e}}{\left({\mathbf{R}}^{\perp ,1}\right)}^{\text{T}})){\mathit{\omega }}_{\text{r}}\hfill \\ \hfill & -\alpha {\mathbf{J}}_{\text{r}}^{-1}(\text{Log}({\mathbf{R}}^{\text{e}})){\mathbf{b}}_{\text{o,\hspace{0.17em}}1}{\Vert }_2\text{.}\hfill \end{array}$

(35)

The Jacobians are not independent of the optimization variables α, β, and γ, which makes solving (35) a challenging task. To further simplify the problem it is assumed for the Jacobians that the optimal values α*, β*, and γ* are close to an initial guess α⁰, β⁰, and γ⁰. This motivates to compute the Jacobians only for the initial guess to obtain the vectors

$\begin{array}{ll}\hfill {\mathbf{r}}_1& ={\mathbf{J}}_{\text{r}}^{-1}(\text{Log}({\mathbf{R}}^{\text{e}})){\mathbf{b}}_{\text{o,\hspace{0.17em}}1}\hfill \end{array}$

(36a)

$\begin{array}{ll}\hfill {\mathbf{r}}_2& ={\mathbf{J}}_{\text{r}}^{-1}(\text{Log}({\mathbf{R}}^{\text{e}}{\left({\mathbf{R}}^{\perp ,1}({\alpha }^0)\right)}^{\text{T}})){\mathit{\omega }}_{\text{r}}\hfill \end{array}$

(36b)

$\begin{array}{ll}\hfill {\mathbf{r}}_3& ={\mathbf{J}}_{\text{r}}^{-1}(\text{Log}({\mathbf{R}}^{\text{e}}{\left({\mathbf{R}}^{\perp ,1}({\alpha }^0)\right)}^{\text{T}}{\left({\mathbf{R}}^{\Vert }({\beta }^0)\right)}^{\text{T}})){\mathbf{b}}_{\text{o,\hspace{0.17em}}2}\text{,}\hfill \end{array}$

(36c)

which are constant in the optimization problem (35). Using (36) in (35) results in a quadratic program, and the optimal solution α*, β*, and γ* can be obtained from

$\left[\begin{array}{ccc}\hfill {\mathbf{r}}_1^{\text{T}}{\mathbf{r}}_1\hfill & \hfill {\mathbf{r}}_1^{\text{T}}{\mathbf{r}}_2\hfill & \hfill {\mathbf{r}}_1^{\text{T}}{\mathbf{r}}_3\hfill \\ \hfill {\mathbf{r}}_2^{\text{T}}{\mathbf{r}}_1\hfill & \hfill {\mathbf{r}}_2^{\text{T}}{\mathbf{r}}_2\hfill & \hfill {\mathbf{r}}_2^{\text{T}}{\mathbf{r}}_3\hfill \\ \hfill {\mathbf{r}}_3^{\text{T}}{\mathbf{r}}_1\hfill & \hfill {\mathbf{r}}_3^{\text{T}}{\mathbf{r}}_2\hfill & \hfill {\mathbf{r}}_3^{\text{T}}{\mathbf{r}}_3\hfill \end{array}\right]\left[\begin{array}{c}\hfill \alpha \hfill \\ \hfill \beta \hfill \\ \hfill \gamma \hfill \end{array}\right]=\left[\begin{array}{c}\hfill {\mathbf{e}}_{\text{o}}^{\text{T}}{\mathbf{r}}_1\hfill \\ \hfill {\mathbf{e}}_{\text{o}}^{\text{T}}{\mathbf{r}}_2\hfill \\ \hfill {\mathbf{e}}_{\text{o}}^{\text{T}}{\mathbf{r}}_3\hfill \end{array}\right]$

(37)

yielding

$\begin{array}{l}\hfill {\alpha }^{*}\approx {\mathit{\rho }}_{\alpha }^{\text{T}}({\mathbf{b}}_{\text{o,\hspace{0.17em}}1},{\mathbf{b}}_{\text{o,\hspace{0.17em}}2},{\mathit{\omega }}_{\text{r}}){\mathbf{e}}_{\text{o}}\end{array}$

(38a)

$\begin{array}{l}\hfill {\beta }^{*}\approx {\mathit{\rho }}_{\beta }^{\text{T}}({\mathbf{b}}_{\text{o,\hspace{0.17em}}1},{\mathbf{b}}_{\text{o,\hspace{0.17em}}2},{\mathit{\omega }}_{\text{r}}){\mathbf{e}}_{\text{o}}\end{array}$

(38b)

$\begin{array}{l}\hfill {\gamma }^{*}\approx {\mathit{\rho }}_{\gamma }^{\text{T}}({\mathbf{b}}_{\text{o,\hspace{0.17em}}1},{\mathbf{b}}_{\text{o,\hspace{0.17em}}2},{\mathit{\omega }}_{\text{r}}){\mathbf{e}}_{\text{o}}\end{array}$

(38c)

as the approximate optimal solution of (32) with the solution vectors ρ _α , ρ _β , and ρ _γ , which are not given explicitly here.

Equation (38) provides the approximate solution to (32) at one point in time but the vectors b_o,1, b_o,2, and ω _r change along the path. It is possible to use the approximate solution (38) to compute the optimal values for α*, β*, and γ* at each time step when the robot’s end-effector traverses the path. However, the quality of the approximation in (38) decreases when α, β, and γ grow. For the model predictive control formulation in Section 4.5, the orientation path errors ${\mathbf{e}}_{\text{o}}^{\perp ,1}(t)$ , ${\mathbf{e}}_{\text{o}}^{\perp ,2}(t)$ , and ${\mathbf{e}}_{\text{o}}^{\Vert }(t)$ are computed over a time horizon spanning from t₀ to t₁. This motivates to obtain an initial guess α⁰, β⁰, and γ⁰ using (32) at the time t₀ and then use the change of the approximate solution (38) to propagate it over the time horizon. Thus, the time evolution of the errors are computed as

$\begin{array}{ll}\hfill {\mathbf{e}}_{\text{o}}^{\perp ,1}(t)=& {\mathbf{e}}_{\text{o}}^{\perp ,1}(t_0)+{\int }_{t_0}^t{\dot{\mathbf{e}}}_{\text{o}}^{\perp ,1}(\tau )\text{d}\tau \hfill \end{array}$

(39a)

$\begin{array}{ll}\hfill {\mathbf{e}}_{\text{o}}^{\perp ,2}(t)=& {\mathbf{e}}_{\text{o}}^{\perp ,2}(t_0)+{\int }_{t_0}^t{\dot{\mathbf{e}}}_{\text{o}}^{\perp ,2}(\tau )\text{d}\tau \hfill \end{array}$

(39b)

$\begin{array}{ll}\hfill {\mathbf{e}}_{\text{o}}^{\Vert }(t)=& {\mathbf{e}}_{\text{o}}^{\Vert }(t_0)+{\int }_{t_0}^t{\dot{\mathbf{e}}}_{\text{o}}^{\Vert }(\tau )\text{d}\tau \text{,}\hfill \end{array}$

(39c)

where the initial guesses for α⁰, γ⁰, and β⁰ are used to compute the initial orientation path errors ${\mathbf{e}}_{\text{o}}^{\perp ,1}(t_0)$ , ${\mathbf{e}}_{\text{o}}^{\perp ,2}(t_0)$ , and ${\mathbf{e}}_{\text{o}}^{\Vert }(t_0)$ , respectively, meaning that (39) is an approximation around the initial solution at time t₀. The time derivatives ${\dot{\mathbf{e}}}_{\text{o}}^{\perp ,1}$ , ${\dot{\mathbf{e}}}_{\text{o}}^{\perp ,2}$ , and ${\dot{\mathbf{e}}}_{\text{o}}^{\Vert }$ required in (39) are computed using (38)

$\begin{array}{ll}\hfill {\dot{\mathbf{e}}}_{\text{o}}^{\Vert }=& ({\dot{\mathit{\rho }}}_{\beta }^{\text{T}}({\mathbf{b}}_{\text{o,\hspace{0.17em}}1},{\mathbf{b}}_{\text{o,\hspace{0.17em}}2},{\mathit{\omega }}_{\text{r}}){\mathbf{e}}_{\text{o}}(t)){\mathit{\omega }}_{\text{r}}(\varphi (t))\hfill \\ \hfill & +({\mathit{\rho }}_{\beta }^{\text{T}}({\mathbf{b}}_{\text{o,\hspace{0.17em}}1},{\mathbf{b}}_{\text{o,\hspace{0.17em}}2},{\mathit{\omega }}_{\text{r}}){\dot{\mathbf{e}}}_{\text{o}}(t)){\mathit{\omega }}_{\text{r}}(\varphi (t))\hfill \\ \hfill & +({\mathit{\rho }}_{\beta }^{\text{T}}({\mathbf{b}}_{\text{o,\hspace{0.17em}}1},{\mathbf{b}}_{\text{o,\hspace{0.17em}}2},{\mathit{\omega }}_{\text{r}}){\mathbf{e}}_{\text{o}}(t)){\mathit{\omega }}_{\text{r}}^{\prime }(\varphi (t))\dot{\varphi }(t)\hfill \\ \hfill =& {\dot{\mathbf{e}}}_{\text{o,\hspace{0.17em}proj}}({\mathit{\rho }}_{\beta }({\mathbf{b}}_{\text{o,\hspace{0.17em}}1},{\mathbf{b}}_{\text{o,\hspace{0.17em}}2},{\mathit{\omega }}_{\text{r}}),{\mathit{\omega }}_{\text{r}})\hfill \end{array}$

(40a)

$\begin{array}{ll}\hfill {\dot{\mathbf{e}}}_{\text{o}}^{\perp ,1}=& {\dot{\mathbf{e}}}_{\text{o,\hspace{0.17em}proj}}({\mathit{\rho }}_{\alpha }({\mathbf{b}}_{\text{o,\hspace{0.17em}}1},{\mathbf{b}}_{\text{o,\hspace{0.17em}}2},{\mathit{\omega }}_{\text{r}}),{\mathbf{b}}_{\text{o,\hspace{0.17em}}1})\hfill \end{array}$

(40b)

$\begin{array}{ll}\hfill {\dot{\mathbf{e}}}_{\text{o}}^{\perp ,2}=& {\dot{\mathbf{e}}}_{\text{o,\hspace{0.17em}proj}}({\mathit{\rho }}_{\gamma }({\mathbf{b}}_{\text{o,\hspace{0.17em}}1},{\mathbf{b}}_{\text{o,\hspace{0.17em}}2},{\mathit{\omega }}_{\text{r}}),{\mathbf{b}}_{\text{o,\hspace{0.17em}}2})\text{.}\hfill \end{array}$

(40c)

Thus, the approximation error of (39) is limited to the change of the orientation path errors within the time horizon of the MPC.

The orthogonal orientation path errors ${\mathbf{e}}_{\text{o}}^{\perp ,1}$ and ${\mathbf{e}}_{\text{o}}^{\perp ,2}$ are represented similar to Section 4.4.2 by

${\mathbf{e}}_{\mathrm{o},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j}}^{\perp }=\left[\begin{array}{c}\hfill e_{\mathrm{o},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},1}^{\perp }\hfill \\ \hfill e_{\mathrm{o},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},2}^{\perp }\hfill \end{array}\right]=\left[\begin{array}{c}\hfill \alpha \hfill \\ \hfill \gamma \hfill \end{array}\right]\text{,}$

(41)

in the orthogonal error plane.

Remark

Due to the iterative computation of the orientation path errors in (39), the reference path π_o(ϕ(t)) is only evaluated at the initial time t₀. For all other time points in the time horizon from t₀ to t₁, only the reference angular velocity ω _r(ϕ(t)) and its derivative ${\mathit{\omega }}_{\text{r}}^{\prime }(\varphi (t))$ with respect to the path parameter ϕ are needed.

4.5. Optimization problem

In this section, the results of the previous sections are utilized to formulate the optimal control problem for the proposed MPC framework. The goal is to follow a desired reference path of the Cartesian end-effector’s position and orientation within given asymmetric error bounds in the error plane orthogonal to the path, which also includes compliance with desired via-points. At the same time, the system dynamics of the controlled robot (6) and the state and input constraints must be satisfied. To govern the progress along the path, the discrete-time path-parameter dynamics

${\mathit{\xi }}_{k+1}={\mathbf{\Phi }}_{\xi }{\mathit{\xi }}_k+{\mathbf{\Gamma }}_{0,\xi }{v}_k+{\mathbf{\Gamma }}_{1,\xi }{v}_{k+1}$

(42)

are introduced, with the state ${\mathit{\xi }}_k^{\text{T}}=[{\varphi }_k,{\dot{\varphi }}_k,{\ddot{\varphi }}_k]$ and the input $v_k={\dot{\varphi }}_k$ . The linear system (42) is chosen to have the same order as (6) and also uses hat functions for the interpolation of the jerk input ${\stackrel{⃛}{\varphi }}_k$ . Thus, the optimization problem for the MPC is formulated as

$\begin{array}{l}\hfill \underset{\begin{array}{c}{\mathbf{u}}_1,\dots ,{\mathbf{u}}_N,\\ {\mathbf{x}}_1,\dots ,{\mathbf{x}}_N,\\ {\mathit{\xi }}_1,\dots ,{\mathit{\xi }}_N,\\ v_1,\dots ,v_N\end{array}}{\min }\sum _{i=1}^{N}l({\mathbf{x}}_i,{\mathit{\xi }}_i,{\mathbf{u}}_i,v_i)\end{array}$

(43a)

$\begin{array}{ll}\hfill \text{s.t.}{\mathbf{x}}_{i+1}=\mathbf{\Phi }{\mathbf{x}}_i+{\mathbf{\Gamma }}_0{\mathbf{u}}_i+& {\mathbf{\Gamma }}_1{\mathbf{u}}_{i+1}\text{,}\hfill \\ \hfill & i=0,\dots ,N-1\hfill \end{array}$

(43b)

$\begin{array}{ll}\hfill {\mathit{\xi }}_{i+1}={\mathbf{\Phi }}_{\xi }{\mathit{\xi }}_i+{\mathbf{\Gamma }}_{0,\xi }{v}_i& +{\mathbf{\Gamma }}_{1,\xi }{v}_{i+1}\text{,}\hfill \\ \hfill & i=0,\dots ,N-1\hfill \end{array}$

(43c)

$\begin{array}{ll}\hfill & {\mathbf{x}}_0={\mathbf{x}}_{\text{init}}\hfill \end{array}$

(43d)

$\begin{array}{ll}\hfill & {\mathbf{u}}_0={\mathbf{u}}_{\text{init}}\hfill \end{array}$

(43e)

$\begin{array}{ll}\hfill & {\mathit{\xi }}_0={\mathit{\xi }}_{\text{init}}\hfill \end{array}$

(43f)

$\begin{array}{ll}\hfill & v_0=v_{\text{init}}\hfill \end{array}$

(43g)

$\begin{array}{ll}\hfill & \underline{\mathbf{x}}\le {\mathbf{x}}_{i+1}\le \overline{\mathbf{x}}\hfill \end{array}$

(43h)

$\begin{array}{ll}\hfill & \underline{\mathbf{u}}\le {\mathbf{u}}_{i+1}\le \overline{\mathbf{u}}\hfill \end{array}$

(43i)

$\begin{array}{ll}\hfill & {\varphi }_{i+1}\le {\varphi }_{\text{f}}\hfill \end{array}$

(43j)

$\begin{array}{ll}\hfill & {\dot{\varphi }}_{i+1}\le \overline{\dot{\varphi }}\hfill \end{array}$

(43k)

$\begin{array}{ll}\hfill & {\psi }_{\mathrm{p},m}({\mathbf{e}}_{\mathrm{p},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},i+1}^{\perp })\le 0\text{,}m=\mathrm{1,2}\hfill \end{array}$

(43l)

$\begin{array}{ll}\hfill & {\psi }_{\text{o},m}({\mathbf{e}}_{\text{o},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},i+1}^{\perp })\le 0\text{,}m=\mathrm{1,2}\text{.}\hfill \end{array}$

(43m)

The quantities x_init, u_init, ξ_init, and v_init in (43d)–(43g) describe the initial conditions at the initial time t₀. In (43h) and (43i), the system states x and the inputs u are bounded from below and above with the corresponding limits $\underline{\mathbf{x}}$ , $\overline{\mathbf{x}}$ , $\underline{\mathbf{u}}$ , and $\overline{\mathbf{u}}$ . The path parameter never exceeds the maximum value ϕ_f (see (43j)), and its speed is bounded from above by $\overline{\dot{\varphi }}$ (see (43k)). In (43l) and (43m), the Cartesian orthogonal position and orientation path errors are bounded by the constraint functions ψ_p,m and ψ_o,m in both desired basis vector directions, which is further detailed in Section 5. The position reference path is given by

${\mathit{\pi }}_{\mathrm{p},i}={\mathit{\pi }}_{\text{p}}({\varphi }_i)$

(44)

for each discrete-time instance t₀ + iT_s. An explicit representation of π _o(ϕ(t)) is not required within the optimization problem (43), as discussed in Section 4.4.4. The objective function (43a),

$\begin{array}{cc}\hfill l\left({\mathbf{x}}_i,{\mathit{\xi }}_i,{\mathbf{u}}_i,v_i\right)& =w_{\Vert ,\mathrm{p}}\Vert {\mathbf{e}}_{\mathrm{p},i}^{\Vert }{\Vert }_2^2+w_{\Vert ,\mathrm{o}}\Vert {\mathbf{e}}_{\text{o},i}^{\Vert }{\Vert }_2^2\hfill \\ \hfill & +w_{\dot{e}}\left(\Vert {\dot{\mathbf{e}}}_{\mathrm{p},i}{\Vert }_2^2+\Vert {\mathbf{e}}_{\mathrm{\omega },i}{\Vert }_2^2\right)\hfill \\ \hfill & +w_{\ddot{e}}\left(\Vert {\ddot{\mathbf{e}}}_{\mathrm{p},i}{\Vert }_2^2+\Vert {\dot{\mathbf{e}}}_{\mathrm{\omega },i}{\Vert }_2^2\right)+\Vert {\mathit{\xi }}_i-{\mathit{\xi }}_{\text{d}}{\Vert }_{{\mathbf{W}}_{\xi }}^2\hfill \\ \hfill & +\Vert {\mathbf{x}}_i{\Vert }_{{\mathbf{W}}_{\text{x}}}^2+w_{\text{u}}\Vert {\mathbf{u}}_i{\Vert }_2^2+w_{\text{v}}\Vert v_i{\Vert }_2^2\text{,}\hfill \end{array}$

(45)

minimizes the tangential position and orientation path errors with the tangential error weights w_‖,p > 0 and w_‖,o > 0. Small tangential error norms $\Vert {\mathbf{e}}_{\mathrm{p},i}^{\Vert }{\Vert }_2^2$ and $\Vert {\mathbf{e}}_{\text{o},i}^{\Vert }{\Vert }_2^2$ are ensured when choosing w_‖,p and w_‖,o sufficiently large. The path error velocity term $\Vert {\dot{\mathbf{e}}}_{\mathrm{p},i}{\Vert }_2^2+\Vert {\mathbf{e}}_{\omega ,i}{\Vert }_2^2$ and acceleration term $\Vert {\ddot{\mathbf{e}}}_{\mathrm{p},i}{\Vert }_2^2+\Vert {\dot{\mathbf{e}}}_{\omega ,i}{\Vert }_2^2$ , with the weights $w_{\dot{e}}>0$ and $w_{\ddot{e}}>0$ , bring damping into the path error systems. Here, the orientation error system is damped using the angular velocity and acceleration errors

$\begin{array}{l}\hfill {\mathbf{e}}_{\omega ,i}={\mathit{\omega }}_{\text{c},i}-{\dot{\varphi }}_i{\mathit{\omega }}_{\text{r},i}\end{array}$

(46a)

$\begin{array}{l}\hfill {\dot{\mathbf{e}}}_{\omega ,i}={\dot{\mathit{\omega }}}_{\text{c},i}-{\ddot{\varphi }}_i{\mathit{\omega }}_{\text{r},i}-{\dot{\varphi }}_i{\dot{\mathit{\omega }}}_{\text{r},i}\text{,}\end{array}$

(46b)

with the current angular accelerations ${\dot{\omega }}_{\text{c},i}$ and ${\dot{\omega }}_{\text{r},i}$ of the end-effector and the reference path, respectively. The terms (46) are used instead of (29) and its derivative, because (29) depends on the error Jacobians ${\mathbf{J}}_{\text{l}}^{-1}({\mathbf{e}}_{\text{o}}(t))$ and ${\mathbf{J}}_{\text{r}}^{-1}({\mathbf{e}}_{\text{o}}(t))$ , which complicate the MPC problem (43).

The term $\Vert {\mathit{\xi }}_i-{\mathit{\xi }}_{\text{d}}{\Vert }_{{\mathbf{W}}_{\xi }}^2={({\mathit{\xi }}_i-{\mathit{\xi }}_{\text{d}})}^{\text{T}}{\mathbf{W}}_{\xi }({\mathit{\xi }}_i-{\mathit{\xi }}_{\text{d}})$ , with the positive definite diagonal weighting matrix W _ξ , makes the state ξ of the path-parameter system (43c) approach the desired state ${\mathit{\xi }}_{\text{d}}={[{\varphi }_{\text{f}},\mathrm{0,0}]}^{\text{T}}$ . However, this formulation leads to very high path velocities $\dot{\varphi }$ when the desired path parameter ϕ_f is large, that is, when the reference path is long. The upper limit $\overline{\dot{\varphi }}$ in (43k) limits $\dot{\varphi }$ which yields a more desirable behavior along the path. Movement in the joint space is regularized using the term $\Vert {\mathbf{x}}_i{\Vert }_{{\mathbf{W}}_{\text{x}}}^2$ with the positive definite diagonal weighting matrix W_x. Since the joint velocity ${\dot{\mathbf{q}}}_{\text{d},i}$ in x _i includes the velocity in the joint nullspace, a minimization of the joint velocity ${\dot{\mathbf{q}}}_{\text{d},i}$ stabilizes the nullspace. More involved methods exist to project ${\dot{\mathbf{q}}}_{\text{d},i}$ to the nullspace (Ott, 2008) but these are computationally expensive. The last two terms in (45), with the weights w_u > 0, w_v > 0, serve as regularization terms.

Within the MPC, the planning problem (43) is solved at each time step with the same sampling time T_s. In each step, the optimal control inputs u _i and v _i are computed over the horizon i = 1, …, N, but only the first control inputs u₁ and v₁ are applied to the system.

Remark

Kinematic singularities happen when the Jacobian J(q) in (7) does not have full row rank. Close to a singularity, high joint velocities $\dot{\mathbf{q}}$ are needed to achieve a desired Cartesian velocity. Due to the constraint (43h), the joint velocities $\dot{\mathbf{q}}$ are bounded and the robot can successfully pass through kinematic singularities. This is demonstrated in the experiment video at https://www.acin.tuwien.ac.at/42d0/ at 3 min 26 s.

5. Orthogonal path error bounds

To stay within predefined error bounds, the MPC formulation (43) bounds the orthogonal position and orientation path errors using (43l) and (43m). The bounds are specified in the Cartesian space and allow for meaningful interpretations. The applications of the bounds are threefold. First, the bounds guide the end-effector along the path, second they ensure collision-freedom, and third they support encoding task-specific constraints. Guidance is important since the end-effector must pass the via-points with zero bounds. Appropriate bounds of the Cartesian reference path ensure collision freedom. In combination with these bounds, the reference path may be specified manually or by a higher-level planner, which is further discussed in Section 6 and Section 10. This way, the bounds capture the obstacle-free space, which can, for example, be obtained using convex collision-free sets (Deits and Tedrake, 2015). This idea contrasts an often-used approach of specifying the obstacle-occupied space and finding a solution in the remaining space (Vu et al., 2020). An advantage of the proposed approach is that the reference paths with the error bounds provide a simple description of the obstacle-free space, independent of the shape or amount of obstacles. Hence, the computational complexity becomes independent of the complexity of the obstacles and the environment. Lastly, the bounds can enforce task-specific constraints along a path, such as keeping the end-effector upright for transferring an open container filled with liquid or moving in a straight line to open a drawer.

The formulation of the bounding functions is detailed in this section. First, the case of symmetric path error bounds is examined and then generalized to asymmetric error bounds with respect to the desired basis vector directions to increase the flexibility of the method. The resulting bounds are visualized for one position basis direction in Figure 4.

Figure 4.

2D Visualization of a position reference path π _p(ϕ) with error bounds. The symmetric error bounding functions ϒ_p,1 are adapted by the upper and lower bounds e_p,u,1 and e_p,l,1 to asymmetrically bound the orthogonal position path error ${\mathbf{e}}_{\text{p}}^{\perp ,1}$ . The shaded gray regions indicate the area where ${\psi }_{\text{p,\hspace{0.17em}}1}({\mathbf{e}}_{\mathrm{p},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j}}^{\perp })\le 0$ . The error bounding functions ϒ_p,1 and the upper and lower bounds e_p,u,1 and e_p,l,1 are defined for both segments between the three shown via-points.

5.1. Symmetric bounds

For a symmetric bounding of the orthogonal path errors, the representations ${\mathbf{e}}_{\mathrm{p},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j}}^{\perp }$ , given in (23) Section 4.4.2, and ${\mathbf{e}}_{\text{o,\hspace{0.17em}proj}}^{\perp }$ , given in (41), are used. Thus, the error bounding constraints

${\psi }_{j,m}^{\text{sym}}({\mathbf{e}}_{j,\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j}}^{\perp })={\left(e_{j,\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},m}^{\perp }\right)}^2-{\left({\mathrm{\Upsilon }}_{j,m}(\varphi )\right)}^2\le 0\text{,}$

(47)

with the bounding functions ϒ_j,m(ϕ), define symmetric box constraints centered on the reference path. The index j ∈ {p,o} indicates the position and orientation error, and the index m = 1, 2 refers to the basis vector direction, leading to four constraints in total.

The bounding functions ϒ_j,m(ϕ) are used to smoothly vary the error bounds such that no orthogonal path errors are allowed at the via-points but deviations from the path are possible in between the via-points depending on the respective task and situation. Let us assume that two via-points exist at the path points ϕ _l and ϕ_l+1. In this work, the bounding functions for ϕ _l ≤ ϕ ≤ ϕ_l+1 are chosen as fourth-order polynomials. They provide sufficient smoothness and are easy to specify by using the conditions

$\begin{array}{ll}\hfill {\mathrm{\Upsilon }}_{j,m}({\varphi }_l)& =0\hfill \end{array}$

(48a)

$\begin{array}{ll}\hfill {\mathrm{\Upsilon }}_{j,m}({\varphi }_{l+1})& =0\hfill \end{array}$

(48b)

$\begin{array}{ll}\hfill {\mathrm{\Upsilon }}_{j,m}^{\prime }({\varphi }_l)& =s_0\hfill \end{array}$

(48c)

$\begin{array}{ll}\hfill {\mathrm{\Upsilon }}_{j,m}^{\prime }({\varphi }_{l+1})& =s_{\text{f}}\hfill \end{array}$

(48d)

$\begin{array}{ll}\hfill {\mathrm{\Upsilon }}_{j,m}\left(\frac{{\varphi }_l+{\varphi }_{l+1}}{2}\right)& ={\mathrm{\Upsilon }}_{j,\mathrm{m}\mathrm{a}\mathrm{x}}\text{,}\hfill \end{array}$

(48e)

where the design parameters are s₀, s_f, and ϒ_j,max. Note that in general, any sufficiently smooth function can be used to bound the orthogonal path errors. For multiple via-points, the error bounding functions are defined for each segment between the via-points, meaning that L via-points lead to L − 1 segments with four bounding functions each. The case of two segments in 2D is visualized in Figure 4.

Remark

The error bounding functions ϒ_j,m(ϕ) depend on the path parameter ϕ meaning that the orthogonal error is only correctly bounded as long as the tangential errors ${\mathbf{e}}_{\text{p}}^{\Vert }$ and ${\mathbf{e}}_{\text{o}}^{\Vert }$ are close to zero. This assumption holds true if the weights w_‖,p and w_‖,o in (45) are sufficiently large compared to the other weights.

5.2. Asymmetric bounds

The symmetric error bounds from the previous section are generalized in this section to allow for asymmetric bounds around the reference paths. The formulation

${\psi }_{j,m}({\mathbf{e}}_{j,\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j}}^{\perp })\le 0\text{,}j\in \{\mathrm{p},\mathrm{o}\},m=\mathrm{1,2}\text{,}$

(49)

with

${\psi }_{j,m}({\mathbf{e}}_{j,\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j}}^{\perp })={\left(e_{j,\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},m}^{\perp }-e_{j,\mathrm{o}\mathrm{ff},m}\right)}^2-{\lambda }_{j,m}^2\text{,}$

(50)

generalizes (47) and is used in this work. The offsets $e_{j,\mathrm{o}\mathrm{ff},m}=1/2\left(e_{j,\mathrm{l},m}+e_{j,\mathrm{u},m}\right){\mathrm{\Upsilon }}_{j,m}$ and the bounds λ_j,m = 1/2(e_j,u,m − e_j,l,m)ϒ_j,m are parametrized using the upper and lower bounds ${\mathbf{e}}_{j,\mathrm{u}}^{\text{T}}=[e_{j,\mathrm{u},1},e_{j,\mathrm{u},2}]$ and ${\mathbf{e}}_{j,\mathrm{l}}^{\text{T}}=[e_{j,\mathrm{l},1},e_{j,\mathrm{l},2}]$ , referred to with the index u and l, respectively, and the bounding functions ϒ_j,m from Section 5.1. Note that choosing e_j,u,m = 1 and e_j,l,m = −1 reduces (50) to the symmetric case (47). Intuitively, the upper and lower bounds e_j,u,m and e_j,l,m define how much of the bound, defined by ϒ_j,m, can be used by the MPC to deviate in orthogonal direction from the reference path. For example, setting e_p,l,1 = −0.5 means that the orthogonal position path error can only deviate 50 % of the bounding function ϒ_p,1 in the negative direction defined by the basis vector b_p,1. A 2D example is given in Figure 4.

Remark

Setting ϒ_j,m(ϕ_l) = 0 and ϒ_j,m(ϕ_l+1) = 0 in (48) may lead to numerical issues in the MPC (43). This is especially true for the orientation path error bounds as the orientation errors are only approximated. Therefore, relaxations ϒ_j,m(ϕ_l) = ϵ_l > 0 and ϒ_j,m(ϕ_l+1) = ϵ_l+1 > 0 are used in practice. Note that in combination with asymmetric error bounds, this may entail discontinuous error bounds at the via-points. Choosing linearly varying upper and lower bounds ${\mathbf{e}}_{j,\mathrm{u}}^{\text{T}}$ and ${\mathbf{e}}_{j,\mathrm{l}}^{\text{T}}$ fixes these issues but is not further detailed here.

6. BoundMPC with linear reference paths

This section describes the application of the proposed MPC framework to piecewise linear reference paths. This is a special case of the above formulation yielding beneficial simplifications of the involved terms and expressions. The block diagram in Figure 5 provides an overview of the interaction of BoundMPC with the underlying joint-space computed-torque controller (2) and the higher-level planner that provides the Cartesian reference paths π _p and π _o, including the error bounds. Algorithm 1 for BoundMPC with linear reference paths is described in this section. The output of BoundMPC is a joint-space trajectory sent at 10 Hz to the computed-torque controller (2). This controller interpolates the given joint-space trajectory and computes a joint torque at 8 kHz to control the robot. The reference path specification for BoundMPC is provided by a higher-level-planner, user input, or application specifications. For point-to-point movements, a higher-level planner for collision avoidance is helpful. Application specifications, for example, keeping a cup upright, are also easily encodable as a reference path with bounds. The reference path can also be manually specified if the user already has a set of desired path points. The bounds of the reference path are assumed to be obstacle-free. Admissible piecewise linear reference paths can be easily generated by just specifying via-points. This way, the reference path is defined by points to be passed with the robot’s end-effector. The realized trajectory between the points is computed by the proposed MPC framework respecting the given bounds and the system dynamics. For obstacle avoidance, linear paths with Cartesian error bounds allow for a simple and accurate formulation of the collision-free space. Moreover, arc-length parametrization becomes trivial for linear paths. This section introduces section, piecewise linear reference paths, and the simplifications that follow from using such paths are derived.

Figure 5.

Block diagram of the application of BoundMPC (43) with the reference paths π _p and π _o and the error bounds as inputs. BoundMPC computes optimal joint-space trajectories $[{\mathbf{q}}_{\text{d}}^{\text{T}},{\dot{\mathbf{q}}}_{\text{d}}^{\text{T}},{\ddot{\mathbf{q}}}_{\text{d}}^{\text{T}},{\mathbf{u}}^{\text{T}}]$ that are provided to computed-torque controller (2). This controller outputs the joint torques τ to control the robot. Since the control frequency of 8 kHz is higher than the trajectory planning frequency of 10 Hz, the desired joint trajectory is interpolated using (5). The computation of the reference paths π _p and π _o is beyond the scope of this work.

6.1 Reference path formulation

In this work, a piecewise linear reference path is a sequence of L linear segments, that is,

$\begin{array}{c}\hfill {\mathit{\pi }}_{\text{p}}(\varphi )=\left\{\begin{array}{cc}{\mathit{\pi }}_{\text{p,\hspace{0.17em}0}}(\varphi )\hfill & {\varphi }_0\le \varphi <{\varphi }_1\hfill \\ {\mathit{\pi }}_{\text{p,\hspace{0.17em}}1}(\varphi )\hfill & {\varphi }_1\le \varphi <{\varphi }_2\hfill \\ ⋮\hfill \\ {\mathit{\pi }}_{\mathrm{p},L-1}(\varphi )\hfill & {\varphi }_{L-1}\le \varphi <{\varphi }_L\hfill \end{array}\right.\end{array}$

(51)

for the position reference path π _p(ϕ). The values ϕ _l , l = 0, …, L, indicate the locations of the via-points along the path, where ϕ₀ refers to the starting and ϕ _L to the terminal point. In Figure 6, an example of two linear position reference segments is visualized. Each segment of the reference position path is parameterized by the slope m _l of unit length and the position b _l at the via-point

${\mathit{\pi }}_{\mathrm{p},l}(\varphi )={\mathbf{m}}_l(\varphi -{\varphi }_l)+{\mathbf{b}}_l\text{,}$

(52)

with the arc-length parameter ϕ. To ensure continuity of π _p(ϕ), the following condition must hold

${\mathbf{b}}_{l+1}={\mathbf{m}}_l({\varphi }_{l+1}-{\varphi }_l)+{\mathbf{b}}_l\text{.}$

(53)

Figure 6.

Visualization of a linear reference path. The current position along the path is π _p(ϕ(t)) on the first linear segment.

Thus, the path (51) is uniquely defined by the via-points b _l l = 0, …, L.

The orientation reference path π _o(ϕ) is formulated analogously. Here, it is assumed that the angular reference velocity ω _r,l between two via-points l and l + 1 is constant. Then, the path segments π _o,l(ϕ) read as

${\mathit{\pi }}_{\text{o},l}(\varphi )=\text{Log}(\text{Exp}({\mathit{\omega }}_{\text{r},l}(\varphi -{\varphi }_l)){\mathbf{R}}_l)\text{,\hspace{0.17em}}l=0,\dots ,L-1\text{,}$

(54)

where R _l is the end-effector’s orientation at the via-point l. Moreover, the condition

${\mathbf{R}}_{l+1}=\text{Exp}({\mathit{\omega }}_{\text{r},l}({\varphi }_{l+1}-{\varphi }_l)){\mathbf{R}}_l$

(55)

must be satisfied to ensure continuity of the piecewise linear orientation reference path π _o(ϕ).

The piecewise linear position reference path (51) is used to compute the tangential and orthogonal position path errors for the optimal control problem (43). As discussed in Section 4.4.4, the orientation reference π _o(ϕ(t)) is never explicitly used within the optimization problem (43) due to the iterative computation of the orientation path errors in (39). This is detailed for piecewise linear orientation reference paths in the next section.

Remark

The orthonormal basis vectors b_p,1, b_p,2, b_o,1, and b_o,2 are constant for each linear segment and will be referred to as b_p,1,l, b_p,2,l, b_o,1,l, and b_o,2,l with l = 0, …, L − 1. Furthermore, the error bounding functions with the upper and lower bounds from Section 5 are denoted as ϒ_j,m,l, e_j,u,l, and e_j,l,l, j ∈ {p,o}, m = 1, 2, respectively.

Remark

Sampling-based methods, such as RRT, can be applied to efficiently plan collision-free piecewise linear reference paths in the Cartesian space, which is simpler than generating admissible paths in the joint space. The proposed MPC framework is then employed to find the joint trajectories that follow the paths within the given bounds.

6.2. Computation of the orientation path errors

In this section, the orientation path error computations are derived for the piecewise linear orientation reference paths introduced in (54). Let us assume that the initial path parameter of the planner at the initial time t₀ is ϕ₀. The current segment at time t has the index l _t . The orientation path error (29) is integrated in the formwhere Ω_c(t) and

${\mathbf{\Omega }}_{\text{r}}(\varphi (t))=\sum _{l=0}^{l_t-1}{\mathit{\omega }}_{\text{r},l}({\varphi }_{l+1}-{\varphi }_l)+{\mathit{\omega }}_{\text{r},l_t}(\varphi (t)-{\varphi }_{l_t})$

(57)

are the integrated angular velocities of the current robot motion and the piecewise linear reference path, respectively. Numerical integration is used to obtain Ω_c(t). Due to the assumption on the initial value of ϕ(t), the term Ω_r(ϕ₀) vanishes. This formulation allows the representation of the current orientation path error e_o(t) by the integrated angular velocities Ω_c(t) and Ω_r(ϕ(t)) and does not require the current orientation R_c(t) or the current orientation reference path π _o(ϕ(t)). For linear orientation reference paths, ${\mathit{\omega }}_{\text{r}}^{\prime }(\varphi (t))=\mathbf{0}$ and ρ _β is constant, and the tangential orientation path error in (39) becomes

$\begin{array}{cc}\hfill {\mathbf{e}}_{\text{o}}^{\Vert }(t)=& {\mathbf{e}}_{\text{o}}^{\Vert }(t_0)+{\int }_{t_0}^t\left({\mathit{\rho }}_{\beta }^{\text{T}}({\mathbf{b}}_{\text{o,\hspace{0.17em}}1},{\mathbf{b}}_{\text{o,\hspace{0.17em}}2},{\mathit{\omega }}_{\text{r}}){\dot{\mathbf{e}}}_{\text{o}}(\tau )\right){\mathit{\omega }}_{\text{r}}(\varphi (\tau ))\text{d}\tau \hfill \\ \hfill =& {\mathbf{e}}_{\text{o}}^{\Vert }(t_0)+\sum _{l=0}^{l_t-1}\left({\mathit{\rho }}_{\beta ,l}^{\text{T}}({\mathbf{e}}_{\text{o}}(t_{{\varphi }_{l+1}})-{\mathbf{e}}_{\text{o}}(t_{{\varphi }_l}))\right){\mathit{\omega }}_{\text{r},l}\hfill \\ \hfill & +\left({\mathit{\rho }}_{\beta ,l_t}^{\text{T}}({\mathbf{e}}_{\text{o}}(t)-{\mathbf{e}}_{\text{o}}(t_{{\varphi }_{l_t}}))\right){\mathit{\omega }}_{\text{r},l_t}\text{,}\hfill \end{array}$

(58)

with

${\mathit{\rho }}_{\beta ,l}={\mathit{\rho }}_{\beta }({\mathbf{b}}_{\text{o,\hspace{0.17em}}1,l},{\mathbf{b}}_{\text{o,\hspace{0.17em}}2,l},{\mathit{\omega }}_{\text{r},l})\text{,}$

(59)

and the basis vectors b_o,1,l and b_o,2,l of the respective segment. Since the $t_{{\varphi }_l}$ values (time associated with the path parameter ϕ _l ) in (58) are unknown, they are approximated as the last discrete time stamp after ϕ _l . Furthermore, the orthogonal orientation path errors ${\mathbf{e}}_{\text{o}}^{\perp ,1}$ and ${\mathbf{e}}_{\text{o}}^{\perp ,2}$ are computed analogously to (58). Note that the values ρ_α,l, ρ_β,l, and ρ_γ,l, l = 0, …, L − 1 are required for the optimization problem (43). However, since they depend on values known prior to the optimization problem, they can be precomputed.

6.3. Position and orientation synchronization

The position and orientation reference paths π _p(ϕ) and π _o(ϕ) are coupled by the path parameter ϕ. Since ϕ is chosen to be the arc length of π _p(ϕ), the slopes m _l in (52) are of unit length but the angular velocities ω _r,l in (55) are in general of different magnitudes. At the l-th via-point at ϕ = ϕ _l , the path velocity changes from $\dot{\varphi }{\mathbf{m}}_{l-1}$ to $\dot{\varphi }{\mathbf{m}}_l$ and the angular velocity along the orientation path from $\dot{\varphi }{\mathit{\omega }}_{\text{r},l-1}$ to $\dot{\varphi }{\mathit{\omega }}_{\text{r},l}$ . Due to the slopes m_l−1 and m _l being unit length, the position reference velocity only changes direction while the orientation reference velocity changes direction and magnitude. A very high angular acceleration is obtained at a via-point where, for example, m_l−1 = m _l and ω _r,l−1 = 0 ω _r,l, since the angular velocity along the path suddenly stops. In order to account for this issue, the weights of objective function (45) are chosen such that w_‖,p ≫ w_‖,o, and the tangential orientation error $e_{\text{o,\hspace{0.17em}proj}}^{\Vert }=\beta$ is bounded similar to the orthogonal orientation errors $e_{\text{o,\hspace{0.17em}proj,\hspace{0.17em}}1}^{\perp }$ and $e_{\text{o,\hspace{0.17em}proj,\hspace{0.17em}}2}^{\perp }$ in (43m) as

${(e_{\text{o,\hspace{0.17em}proj}}^{\Vert }(\varphi ))}^2-{({\mathrm{\Upsilon }}_{\text{o},l}^{\Vert }(\varphi ))}^2\le 0$

(60)

in the l-th segment, l = 0, …, L, which is a symmetric bound similar to (47) with the bounding function ${\mathrm{\Upsilon }}_{\text{o}}^{\Vert }$ . This way, the MPC can relax the synchronization of the position and orientation reference between the via-points due to w_‖,p ≫ w_‖,o, but the synchronization at the via-points is ensured using (60). For the example case, the MPC can now arrive at the l-th via-point with angular velocity $\dot{\varphi }{\mathit{\omega }}_{\text{r},l-1}$ and then exploit the bound (60) for ϕ > ϕ _l to reduce the angular velocity. The MPC using linear reference paths is further detailed in Algorithm 1.

Algorithm 1. BoundMPC with linear reference paths

7. Online replanning

A major advantage using the proposed framework compared to state-of-the-art methods is the ability to replan a given path in real time during the motion of the robot in order to adapt to new goals or dynamic changes in the environment. Two different situations for replanning are possible based on when the reference path is adapted, that is,

1. outside of the current MPC horizon

2. within the current MPC horizon.

The first situation is trivial since it does not affect the subsequent MPC iteration. More challenging is the second situation, which requires careful consideration on how to adapt the current reference path such that the optimization problem remains feasible. Finding the replanned reference paths is left to a higher-level planner, which is beyond the scope of this work.

This situation is depicted for linear position reference paths at the current time t in Figure 7 and applies similarly to linear orientation paths. In this figure, the replanned path ${\tilde{\pi }}_{\text{p}}(\varphi )$ is entirely different and is valid starting from the current time t. Consequently, the new error bounds, depicted in green, must be chosen such that the current point p_c lies within the bounds to keep the optimization problem feasible. The position basis vectors of the new path ${\tilde{\mathbf{b}}}_{\text{p,\hspace{0.17em}}1}$ and ${\tilde{\mathbf{b}}}_{\text{p,\hspace{0.17em}}2}$ are used to obtain the new orthogonal errors by computing (23). These vectors are utilized to choose appropriate bounding functions. Note that these feasibility considerations do not consider the dynamics of the end-effector.

Figure 7.

Replanning for a linear position reference path. The blue color indicates the current position p_c(t) and velocity v_c(t). The current reference point at time t is π _p(ϕ). Replanning takes place at the current path parameter ϕ with the path velocity $\dot{\varphi }$ indicated by the red arrow along the path at the time t. The path state after replanning is given by the new reference path ${\tilde{\pi }}_{\text{p}}(\tilde{\varphi })$ at the replanned path parameter $\tilde{\varphi }$ with the path velocity $\tilde{\dot{\varphi }}$ .

To simplify the replanning, the path state is projected onto the new reference path ${\tilde{\mathit{\pi }}}_{\text{p}}(\varphi )$ , resulting in the new path state

$\tilde{\mathit{\xi }}(t)=\left[\begin{array}{c}\hfill \tilde{\varphi }\hfill \\ \hfill \tilde{\dot{\varphi }}\hfill \\ \hfill \tilde{\ddot{\varphi }}\hfill \end{array}\right]=\left[\begin{array}{c}\hfill ({\mathbf{p}}_{\text{c}}^{\text{T}}(t)-{\tilde{\mathit{\pi }}}_{\text{p}}(0)){\tilde{\mathit{\pi }}}_{\text{p}}^{\prime }(0)\hfill \\ \hfill {\mathbf{v}}_{\text{c}}^{\text{T}}(t){\tilde{\mathit{\pi }}}_{\text{p}}^{\prime }(0)\hfill \\ \hfill {\mathbf{a}}_{\text{c}}^{\text{T}}(t){\tilde{\mathit{\pi }}}_{\text{p}}^{\prime }(0)\hfill \end{array}\right]\text{,}$

(61)

with the current Cartesian velocity v_c(t), acceleration a_c(t) from (7) and (8), and the constant position path derivative ${\tilde{\mathit{\pi }}}_{\text{p}}^{\prime }(0)$ of the first segment of the new reference path. Equation (61) ensures that the tangential position error ${\mathbf{e}}_{\text{p}}^{\Vert }$ in the new path state $\tilde{\xi }(t)$ is zero and that its derivatives ${\dot{\mathbf{e}}}_{\text{p}}^{\Vert }$ and ${\ddot{\mathbf{e}}}_{\text{p}}^{\Vert }$ also become zero. The orientation path reference is adapted such that the tangential orientation error ${\mathbf{e}}_{\text{o}}^{\Vert }$ is zero and the orthogonal path errors $e_{\text{o,\hspace{0.17em}proj,\hspace{0.17em}}1}^{\perp }$ and $e_{\text{o,\hspace{0.17em}proj,\hspace{0.17em}}2}^{\perp }$ are within the error bounds. Since the position and orientation reference paths use the same path parameter ϕ, the projected position path state (61) also determines the new orientation path state. Hence, ${\dot{\mathbf{e}}}_{\text{o}}^{\Vert }$ and ${\ddot{\mathbf{e}}}_{\text{o}}^{\Vert }$ are generally arbitrary, but the MPC can utilize the bound (60) to adapt to the new reference path, as shown in Section 10.

8. Implementation details

The proposed MPC is implemented on a Kuka LBR iiwa 14 R820 robot. For the optimization problem (43), the IPOPT optimizer (Wächter and Biegler, 2006) is used within the CasADi framework (Andersson et al., 2019). To improve convergence and robustness at the end of the paths, the objective function (45) is slightly modified to use

$\begin{array}{ll}\hfill {\mathbf{e}}_{\mathrm{p},i}^{\text{obj}}& =(1-{\sigma }_{\text{e}}({\varphi }_i)){\mathbf{e}}_{\mathrm{p},i}^{\Vert }+{\sigma }_{\text{e}}({\varphi }_i){\mathbf{e}}_{\mathrm{p},i}\hfill \end{array}$

(62a)

$\begin{array}{ll}\hfill {\mathbf{e}}_{\text{o},i}^{\text{obj}}& =(1-{\sigma }_{\text{e}}({\varphi }_i)){\mathbf{e}}_{\text{o},i}^{\Vert }+{\sigma }_{\text{e}}({\varphi }_i){\mathbf{e}}_{\text{o},i}\hfill \end{array}$

(62b)

instead of the tangential errors ${\mathbf{e}}_{\mathrm{p},i}^{\Vert }$ and ${\mathbf{e}}_{\text{o},i}^{\Vert }$ . The sigmoid function

${\sigma }_{\text{e}}(\varphi )=\frac{1}{1+\exp \left(-100\left(\varphi -({\varphi }_{\text{f}}-0.02)\right.\right)}$

(63)

is used to minimize the full path errors e_p,i and e_o,i at the end of the path, which ensures convergence to the final point.

Remark

The objective function (45) depends on the current end-effector acceleration for the terms $\Vert {\ddot{\mathbf{e}}}_{\mathrm{p},i}{\Vert }_2^2$ and $\Vert {\dot{\mathbf{e}}}_{\mathrm{\omega },i}{\Vert }_2^2$ , which requires the computation of (8). As this is computationally expensive to evaluate, the derivative is numerically approximated within the MPC problem (43) as

$\left[\begin{array}{c}\hfill {\mathbf{a}}_{\text{c},i}\hfill \\ \hfill {\dot{\mathit{\omega }}}_{\text{c},i}\hfill \end{array}\right]=\frac{1}{T_{\text{s}}}\left(\left[\begin{array}{c}\hfill {\mathbf{v}}_{\text{c},i}\hfill \\ \hfill {\mathit{\omega }}_{\text{c},i}\hfill \end{array}\right]-\left[\begin{array}{c}\hfill {\mathbf{v}}_{\text{c},i-1}\hfill \\ \hfill {\mathit{\omega }}_{\text{c},i-1}\hfill \end{array}\right]\right)$

(64)

at horizon step i = 1, …, N.

9. Parameter tuning

In this section, several parameters of the BoundMPC are studied in order to give further insight into the working principle and guide the user on how to choose the parameters. The considered parameters are given in Table 1, where the bold values indicate the default values, which are used when the other parameters are varied. The maximum allowable errors for the position ϒ_p,max = 0.5 m and orientation ϒ_o,max = 50° are constant for all linear path segments and both orthogonal errors.

Table 1.

Parameters of the MPC to be investigated.

Parameter name	Symbol	Values
Horizon length	N	6, 10, 30
Path velocity limit/m s−1	$\overline{\dot{\varphi }}$	0.5, 0.75, 2
Slope	s0 = sf	0.2, 2

Bold entries indicate default values.

All other parameters are given in Table 2. As discussed in Section 4.5, the weight w_‖,p in (45) is chosen larger than the other weights, ensuring a small tangential position error. The position weight w_‖,p is chosen larger than the orientation weight w_‖,o to enable smooth transitions at the via-points, as discussed in Section 6.3. The constraint (60) is also added to the MPC formulation (43). The weight w_u, in combination with the weighting matrix W _ξ from Table 1, ensures smooth progress along the path, where the speed along the path is bounded from above by $\overline{\dot{\varphi }}$ . The weights $w_{\dot{e}}$ and $w_{\ddot{e}}$ in (45) are chosen small to regularize the orthogonal path errors but still allow exploitation of the error bounds. The weighting matrix W_x is used to regularize the joint position q_d, velocity ${\dot{\mathbf{q}}}_{\text{d}}$ and acceleration ${\ddot{\mathbf{q}}}_{\text{d}}$ and stabilize the nullspace movement. Lastly, the joint jerk weight w_u is chosen to be small to regularize the joint jerk input. Choosing the weight w_v ≥ w_u in (45) focuses on optimizing the Cartesian jerk instead of the joint jerk. In combination with the Cartesian reference paths, this leads to desirable behavior. Note that this only optimizes the Cartesian jerk along the path. Orthogonal to the path, only the Cartesian velocity and acceleration are optimized using the weights $w_{\dot{e}}$ and $w_{\ddot{e}}$ .

Table 2.

Weights for the MPC cost function (45).

Weight	Value
w ‖,p	1000
w ‖,o	1
$w_{\dot{e}}$	0.1
$w_{\ddot{e}}$	0.05
W x	diag (0.01I7, 0.01I7, 0.001I7)
w u	10−4
w v	0.5
W ξ	diag(8, 5, 4)

In order to study the influence of the parameters on the performance, a simple example with a reference path containing four linear segments is chosen. The path starts and ends at the same pose. The list of poses at the via-points is given in Table 3, and the position reference path is visualized with the robot in Figure 8. The trajectory covers a large portion of the robot’s workspace.

Table 3.

Via-points for the parameter studies. The bounds are chosen symmetrical around the reference. Hence, the basis vectors b_j,1, b_j,2 and the bounds e_j,l and e_j,u, j = p,o, are not needed for this study.

Index l	π p(ϕ l )/m	π o(ϕ l )/rad
0	p0 = [1, 0, 0.66]T	τ0 = π[0, 0.5, 0]T
1	p0 + [−2 ,0, 0]T	π[0.71, 0, −0.71]T
2	p0 + [−1, 1, 0]T	π[−0.38, 0.38, 0.38]T
3	p0 + [−1, −1, 0]T	π[0.5, 0, 0]T
4	p 0	τ 0

Figure 8.

Visualization of the robot’s configuration at the end of the example trajectory. The white line shows the position reference path of the end-effector. The world coordinate system is given by the RGB (xyz) axes at the robot’s base. The end-effector in red is a gripper attached to the last robot link.

A comparison of the position and orientation trajectories for different horizon lengths N is shown in Figure 9. The orientation is plotted as the vector part of the unit quaternion representing the orientation. All solutions pass the via-points and converge to the final point without violating the constraints. The normalized cumulated costs of the trajectories are depicted in Figure 10. Longer horizon lengths lead to lower cumulated costs. Figure 11 shows the norm of the tangential error ${\mathbf{e}}_{\text{p}}^{\Vert }$ and the orthogonal error ${\mathbf{e}}_{\text{p}}^{\perp }$ for different horizon lengths N. The tangential errors are smaller than the orthogonal errors due to the large weights w_‖,p and w_‖,o in (45). It can be seen that the tangential errors increase close to the via-points, which can be traced back to the sudden change of the direction in the piecewise linear reference path. The orthogonal errors at the via-points (gray vertical lines in Figure 11) do not reach zero exactly due to the discretization of the control problem. Between the via-points, the MPC exploits the orthogonal deviation from the reference path within the given bounds to further minimize the objective function (45). For shorter horizons, planning is more difficult since the next via-point is not always within the planning horizon, which explains the higher cumulated costs in Figure 10. A longer horizon thus allows the robot to find better solutions, which is visible in the first and third segment of $\Vert {\mathbf{e}}_{\text{p}}^{\perp }\Vert$ in Figure 11, where a long horizon of N = 30 can adapt the position errors more appropriately for the upcoming via-point. This is also visible in the position trajectory in Figure 9.

Figure 9.

Comparison of the position and orientation trajectories of the MPC solutions for different horizon lengths N. The rotation trajectory is shown as the vector part of the quaternion representation of orientations ${\mathbf{r}}_{\text{q}}^{\text{T}}=[w_1,w_2,w_3,w]$ .

Figure 10.

Cumulated costs of the trajectories for different horizon lengths N. The curves are normalized to the cumulated cost value of the trajectory for N = 30 at ϕ = ϕ_f. Each curve is the cumulated sum of the objective (45) along the resulting trajectories.

Figure 11.

Norm of the tangential and orthogonal position error of the MPC solutions for different horizon lengths N.

Table 4 shows the trajectory durations T for the parameter sets introduced in Table 1. It is confirmed that the horizon length has a negligible impact on the trajectory duration T due to the upper bound $\overline{\dot{\varphi }}$ on the path velocity $\dot{\varphi }$ . The decreased cumulated costs still favor larger horizons. However, large horizon lengths lead to longer computation times and might undermine the real-time capability of the proposed concepts; see the minimum, maximum, and mean computation times for different horizon lengths N in Table 5. A horizon length of N = 10 was chosen as a compromise for the experiments in Section 10. The optimization problem can thus be reliably solved within one sampling time of T_s = 0.1 s. While large horizons are more likely to find the globally optimal solution, they are not real-time capable. Thus, the choice of the horizon length is a trade-off between optimality and computational feasibility.

Table 4.

Trajectory duration T in s for different parameter sets. The parameter values corresponding to the different columns are given in Table 1.

Parameter	Value 1	Value 2	Value 3
N	16.2	15.3	15.7
$\overline{\dot{\varphi }}$	8.7	9.5	15.3
s0 = s f	15.3	15.3	-

Table 5.

Statistics of computation time T_comp in ms for different horizon lengths N.

N	6	10	30
min(Tcomp)	8	17	139
max(Tcomp)	38	64	848
mean(Tcomp)	16	39	235

The influence of the path velocity bound $\overline{\dot{\varphi }}$ on the path progress is depicted in Figure 12. This figure shows that a larger bound reduces the trajectory duration (see also Table 4), since the path velocity $\dot{\varphi }$ allows faster progress along the path. The linear progression of the path parameter ϕ indicates that the bound $\overline{\dot{\varphi }}$ is reached. For $\overline{\dot{\varphi }}$ = 1 m s⁻¹ and $\overline{\dot{\varphi }}$ = 2 m s⁻¹, the robot cannot reach the maximum velocity $\overline{\dot{\varphi }}$ until the end of the trajectory, which causes the slope in Figure 12 to decrease. Further, the maximum absolute normalized joint position and velocity values are depicted in Figure 13. A value of 1 indicates that at least one axis is in a position or velocity limit. The MPC manages to exploit the limits without exceeding them. Larger values of the path velocity bound $\overline{\dot{\varphi }}$ utilize the joint velocity range to a larger extent. Thus, the proposed approach uses the whole workspace of the robot with maximum velocities and without violating any limits.

Figure 12.

Influence of the path velocity limit $\overline{\dot{\varphi }}$ on the path progress ϕ(t).

Figure 13.

Exploitation of the joint position and velocity limits for different values of the path velocity limit $\overline{\dot{\varphi }}$ . The vertical axis shows the maximum absolute normalized joint position and velocity with respect to the joint and velocity upper bounds $\overline{\mathbf{q}}$ and $\overline{\dot{\mathbf{q}}}$ .

The parameters s₀ and s_f are used in the error bounds in (48) to specify the slopes of the bounding functions at the via-points; see the comparison of the orthogonal errors in Figure 14. The dashed lines indicate the error bounds, which differ only by their slope parameters at the via-points. These parameters influence the error bounds’ shape but do not change the maximum value ϒ_max. A low slope intuitively means that the error bounds around the via-points change slowly; hence, the via-points become more like a via-corridor. A high slope means that this via-corridor becomes very narrow. This is important when passing a via-point (see Figure 14). A longer via-corridor forces the orthogonal path errors to be small for a longer time, and thus, the error cannot be exploited well between the via-points. The effect of the slope s₀ = s _f on the trajectory duration T, as presented in Table 4, can be neglected.

Figure 14.

Comparison of the orthogonal error trajectories of the position and orientation for different slopes s₀ = s _f . The bounds are shown as dashed lines of the corresponding color. Via-points are indicated by vertical gray lines.

Remark

A repeated tracking of Cartesian paths, and specifically the paths described in Table 3, is possible due to the stabilization of the joint nullspace by the term $\Vert {\mathbf{x}}_i{\Vert }_{{\mathbf{W}}_{\text{x}}}^2$ in (45).

10. Experiments

The proposed trajectory planning framework BoundMPC features several important properties, such as position and orientation path error bounding, adhering to via-points, and online replanning. Six scenarios with different experiments are considered in the following to demonstrate the capabilities of the proposed framework. We grouped the following experiments into tutorial scenarion, which demonstrate the features and performance of BoundMPC in more detail, and application scenarios, which showcase everyday tasks. The first four scenarios are performed in a static environment and the last two scenarios change the task description online, demonstrating the online replanning capabilities. The scenarios are as follows:

S1: (Tutorial Static) Moving a large object between obstacles: A large object is moved from a start to a goal pose with obstacles. These obstacles constraint the position and orientation. The position and orientation paths are synchronized using via-points. This scenario is an example of understanding the components of the BoundMPC planning framework.

   • Experiment 1: Moving the object in a highly constraint space.

S2: (Application Static) Opening a drawer: The robot’s end-effector hooks into a cabinet drawer and opens it in a straight motion. A linear reference path segment with tight error bounds encodes the task-specific constraint of a straight motion. This is an application scenario that fulfills an everyday task.

   • Experiment 2: Opening the drawer with different desired trajectory durations.

S3: (Application Static) Table wiping: Wiping a table with straight sponge motions across the surface. The task-specific straight motions require tight error bounds during the wiping motion but allow larger bounds in the transition segments. This is an application scenario that fulfills an everyday task.

   • Experiment 3: Wiping the table with the sponge.

S4: (Tutorial Static) Collision avoidance for full robot: An object is transferred from one shelf in front of the robot to a shelf behind the robot. The environment contains six additional obstacles close to the robot base. Collision avoidance terms are added to the objective function (45) for collision avoidance of the entire robot’s kinematic chain.

   • Experiment 4: Transferring the object to the shelf behind the robot.

   • Experiment 5: Evaluation of the task success for shifted obstacles along the x-axis.

S5: (Tutorial Replanning) Object grasp from a table: An object is grasped using one via-point at the pre-grasp pose. The path must be replanned during the robot’s motion in real-time to adapt to the sudden change of the object pose. Collisions of the end-effector are avoided by using asymmetric error bounds. This scenario is used to showcase BoundMPC’s replanning capabilities.

   • Experiment 6: Grasping the object with three replanning steps showcasing the replanning capabilities.

   • Experiment 7: Randomly updating the object’s pose until replanning fails.

   • Experiment 8: Planning 50 random poses with a comparison to state-of-the-art motion planners; see below.

S6: (Application Replanning) Transfer of a filled open container: Transferring a filled open container from an initial position onto a table while keeping it upright to avoid liquid spillage. A path segment with tight orientation error bounds encodes the task-specific upright constraint. This is an application scenario that fulfills an everyday task.

   • Experiment 9: Transferring the open container.

   • Experiment 10: Randomized put-down positions with replanning for the container.

The 7-DoF robot Kuka LBR iiwa 14 R820 is used for the experiments. The BoundMPC implementation is the same across all experiments, and the weights for the cost function (45) used in all experiments are listed in Table 2. Choosing the planning frequency f_c = 10 Hz and the number of planning grid points to be N = 10 leads to a time horizon of T_c = N/f_c = 1 s. The controller considers the current and upcoming n = 4 segments of the reference path. Additionally, BoundMPC was applied to human–robot handovers in Oelerich et al. (2024).

The motion planning pipeline consists of two components:

1. The higher-level planner provides the linear reference path segments π _p,l and π _o,l, l = 1, …, L − 1, together with the basis vectors b_j,m,l, and the orthogonal error bound parameters ϒ_j,m,l, e_j,u,l, and e_j,l,l, j ∈ {p,o}, m = 1, 2, to enable collision-free movement within the bounds. In this work, these reference path segments and bound parameters are manually chosen for the experiments. Future work will deal with a more systematic way of finding the reference path and its bounds based on measured data from the environment. Since linear reference paths are easy to specify, this planner can replan the reference path during the robot’s motion.

2. BoundMPC is used to compute a trajectory to follow the reference paths optimally while exploiting the bounds.

The path bounds are assumed to be obstacle-free, which is ensured by the user input or the higher-level planner, as discussed in Section 6. In this work, collision freedom is only considered for the end-effector, and future work will extend this principle to the whole kinematic chain of the robot. Since the bounding functions are specified in Cartesian space, task-specific constraints are easily encoded in this framework.

In order to show the advantages of using the BoundMPC framework, it is compared to the following state-of-the-art motion planners:

• Joint trajectory tracking MPC (JT-MPC): This MPC framework uses a joint-space reference trajectory. Trajectory tracking MPCs are popular real-time capable planning methods (see, e.g., Arbo et al., 2017; Dai et al., 2021; Schoels et al., 2020a; Toussaint et al., 2022; Wang et al., 2023). The reference trajectories for the tracking MPC are computed by optimizing a spline trajectory through the inverse kinematic solutions of the given via-points, (see Toussaint et al., 2022). The reference trajectory planning is fast but does not consider collisions. The collision avoidance is performed through a collision term in the MPC objective function based on potential functions (Vu et al., 2020). In order to enable a fair comparison, collision avoidance is only performed for the robot’s end-effector except for scenario S6 where the entire kinematic chain is checked for collisions. A time horizon of 2 s is used.

• Cartesian trajectory tracking MPC (CT-MPC): The CT-MPC uses the same formulation as JT-MPC but with Cartesian reference trajectories for the positions and orientation instead of joint-space reference trajectories. A time horizon of 1 s is used.

• Path tracking BoundMPC (BoundMPC-PT): This variant of BoundMPC minimizes e_p and e_o instead of ${\mathbf{e}}_{\text{p}}^{\Vert }$ and ${\mathbf{e}}_{\text{o}}^{\Vert }$ in the objective function (45). This leads to a formulation similar to Arbo et al. (2017), Astudillo et al. (2022b), and Mehrez et al. (2017) but uses a suboptimal reference path.

The output of all planners is a joint-space trajectory, which is executed by the robot using the computed-torque controller (2). Thus, the planner outputs are comparable.

Remark

Importantly, BoundMPC requires a pre-planned collision-free reference path. While this aspect is not the focus of this work preliminary experiments with planning paths based on collision-free convex sets show promising results. The method proposed in Deits and Tedrake (2015) can efficiently compute these sets. Thus, the comparisons lack the computational overhead of computing the reference paths.

The planners are numerically compared for the following metrics:

• The planning time t_plan is the time the trajectory planner takes each step to generate the joint-space trajectory for the next time step.

• The geometric path length l_path is the length of the end-effector’s position path of the resulting trajectory for each planner.

• The time it takes for the robot to traverse the trajectory, indicated by t_traj.

• The success rate r_s is the percentage of successful planning attempts relative to the failed ones.

10.1. Moving a large object between obstacles

10.1.1. Goal

This task aims to move a large object from a start to a goal pose through a confined space defined by obstacles.

10.1.2. Setup

The object is positioned on a table, from which it is grasped and then transferred to the goal pose. The obstacles create a narrow passage in the middle of the path and at the goal pose. Since the object is too large to pass the obstacles in the initial orientation, the robot must turn it by 90° in the first segment, then pass the obstacles, and finally turn the object to the goal orientation to put it down safely. BoundMPC allows setting via-points to traverse the bottlenecks without collision where no stopping at the via-points is required. The path parameter ϕ(t) synchronizes the end-effector’s position and orientation at the via-points. Thus, the position and orientation via-points are reached at the same time, making it possible to traverse the path safely within the given bounds. A video of the experiment can be found at https://www.acin.tuwien.ac.at/42d0/.

10.1.3. Results

Figure 15 illustrates the robot configuration at four time instances of the trajectory to visualize the robot’s motion in the scenario. For further understanding of the basis vectors, Figure 16 displays the position basis vectors b_p,1 and b_p,2 for each linear path segment. The planned trajectory is shown in Figure 17 with the projected error bounds on the Cartesian planes. Five via-points and four line segments are used to construct the reference path. In the second segment, the robot moves with the large object between two obstacles. At the end of the trajectory, the robot with the large object in its gripper puts the object down between two more obstacles at the desired goal position. Only four via-points are visible in the orientation plot in Figure 17 because the last two via-points coincide. All via-points are passed for the position and orientation.

Figure 15.

Visualization of the robot’s configurations in different time instances along the path: The end-effector in red is a gripper attached to the robot flange. The blue cylinder represents the object to be transferred to the green table. The red walls are obstacles to be avoided. The green and white lines represent the planned trajectory and the reference path of the end-effector, respectively. The world coordinate system is given by the RGB (xyz) axes at the robot’s base. (a) ϕ = 0 m, (b) ϕ = 0.6 m, (c) ϕ = 1.1 m, and (d) ϕ = ϕ_f = 1.62 m.

Figure 16.

Position basis vectors for each linear reference path segment for the object transfer scenario. The red and blue arrows indicate the basis vectors b_p,1 and b_p,2, respectively, which span the orthogonal position error plane.

Figure 17.

End-effector position and orientation trajectories for the object transfer scenario. The projected bounds (light gray areas) for the orthogonal position path error are shown on the Cartesian planes.

Further details are given in the error trajectories in Figure 18. The low bound in the direction of the basis vector ${\mathbf{b}}_{\mathrm{p},\mathrm{1,2}}^{\text{T}}=[\mathrm{0,0,1}]$ for $e_{\text{p,\hspace{0.17em}proj,\hspace{0.17em}}1}^{\perp }$ (also depicted in Figure 16 by the red arrow in the second segment) ensures low deviations in z-direction. This bounding does not affect the other error direction ${\mathbf{b}}_{\text{p,\hspace{0.17em}}2\text{,\hspace{0.17em}}2}^{\text{T}}=[-0.83,0.55,0]$ for $e_{\text{p,\hspace{0.17em}proj,\hspace{0.17em}}2}^{\perp }$ in this segment, as seen in Figure 18. Furthermore, the last segment of the position path is bounded to have low errors for both directions b_p,1,4 and b_p,2,4 with the orthogonal errors $e_{\text{p,\hspace{0.17em}proj,\hspace{0.17em}}1}^{\perp }$ and $e_{\text{p,\hspace{0.17em}proj,\hspace{0.17em}}2}^{\perp }$ , to avoid the obstacles around the goal pose. The orientation errors $e_{\text{o,\hspace{0.17em}proj,\hspace{0.17em}}2}^{\perp }$ and $e_{\text{o,\hspace{0.17em}proj,\hspace{0.17em}}2}^{\perp }$ in the second segment are constrained to be small since only the rotation around the normed reference path angular velocity ${\mathit{\omega }}_{\text{r},2}^{\text{T}}/\Vert {\mathit{\omega }}_{\text{r},2}{\Vert }_2=[\mathrm{0,0,1}]$ is allowed to ensure a collision-free trajectory. Similar to the position, the orthogonal orientation errors $e_{\text{o,\hspace{0.17em}proj,\hspace{0.17em}}1}^{\perp }$ and $e_{\text{o,\hspace{0.17em}proj,\hspace{0.17em}}2}^{\perp }$ are bound to be low to avoid collisions with the obstacles around the goal pose. The robot passes the obstacles in the confined space and exploits the orthogonal error bounds between the obstacles, finding the optimal trajectory to solve the task. Figure 18 additionally shows the trajectory when using large error bounds with a maximum position error of 100 mm and a maximum orientation error of 20° for each segment. This trajectory moves outside the free space in the second and last segments and results in collisions between the robot and the obstacles. This shows the necessity of specifying appropriate bounds.

Figure 18.

Orthogonal path error trajectories in the position and orientation for the object transfer. The upper two plots show the decomposed orthogonal position path errors and the lower three plots the respective orthogonal orientation path errors and the tangential orientation path error. The blue curves show the trajectory when using large error bounds in each segment. The via-points are indicated by vertical gray lines.

Furthermore, Figure 18 shows the bounded tangential orientation path error $e_{\text{o,\hspace{0.17em}proj}}^{\Vert }$ in the last plot. The bounds force $e_{\text{o,\hspace{0.17em}proj}}^{\Vert }$ to be low at the via-points but allow deviations between the via-points, as discussed in Section 6.3. This is, for example, utilized in the third segment. A large orientation change is required in this segment, while the last segment has no orientation change. Hence, the MPC successfully leverages the freedom in $e_{\text{o,\hspace{0.17em}proj}}^{\Vert }$ to optimize the motion along the orientation path.

Figure 19 depicts the approximation error α_approx of ${\mathbf{e}}_{\text{o}}^{\perp ,1}$ between the actual tangential and orthogonal orientation path errors obtained by (32) and their approximations (39). The approximation error is low at the start of the horizon and grows larger toward the end due to the integral approximation in (58). Furthermore, the approximation error is larger during the first and third segments, which contain significant changes in the orientation reference. A large change requires large angular accelerations, which causes the first-order approximation (29) to become less accurate. The approximation errors for ${\mathbf{e}}_{\text{o}}^{\Vert }$ and ${\mathbf{e}}_{\text{o}}^{\perp ,2}$ are in a similar range.

Figure 19.

Mean and maximum values of the absolute orientation approximation error $|{\alpha }_{\text{approx}}|=|e_{\text{o,\hspace{0.17em}proj,\hspace{0.17em}}1}^{\perp }-{\alpha }_{\text{true}}|$ , where α_true is obtained from (32). The upper bar plot shows the mean and maximum values over the planning horizon of the MPC. The lower plot illustrates these values over the path parameter ϕ, where each bar’s width represents the horizon’s geometric length. Vertical gray lines indicate the via-points.

10.1.4. Comparison

The proposed BoundMPC planner is compared to two different motion planners for this task, that is, JT-MPC and CT-MPC. The comparison metrics are the planning time t_plan, the geometric path length l_path, the trajectory duration t_traj, the success rate r_s, and formulation-specific collision avoidance aspects.

The results of the comparison are listed in Table 6. JT-MPC and CT-MPC solve the given task with computational effort comparable to BoundMPC. However, CT-MPC struggles to follow the given trajectory well. This is depicted in Figure 20. The Cartesian velocity v_c is larger for CT-MPC in the second half of the trajectory. The norm of the tangential path error ${\mathbf{e}}_{\text{p}}^{\Vert }$ remains low with the BoundMPC formulation throughout the entire trajectory. For CT-MPC, it shows large values for t ≥ 4 s and large tracking errors at t ≥ 5 s after the slow down at 4.5 s ≤ t ≤ 5 s. The behavior of CT-MPC is due to the trajectory following formulation and the collision avoidance, as discussed in Section 10.7.

Table 6.

Comparison of BoundMPC with different motion planners for the object transfer scenario, in terms of planning time t_plan, geometric path length l_path, trajectory duration t_traj, and success rate r_s. Each planner was run 10 times and the rows report the minimum, mean and maximum values for each planner.

Planner	tplan / s	lpath / m	ttraj / s	r s
JT-MPC	0.01	1.75	5.0	100 %
	0.018
	0.045
CT-MPC	0.006	2.16	8.2	100 %
	0.012
	0.043
BoundMPC	0.015	1.59	5.2	100 %
	0.025
	0.038

Figure 20.

Comparison of the normed Cartesian velocity ‖v_c‖ (top) and the tangential path error norm $\Vert {\mathbf{e}}_{\text{p}}^{\Vert }\Vert$ (bottom) for BoundMPC, JT-MPC, and CT-MPC. The reference trajectories for JT-MPC and CT-MPC are chosen to perform the task as quickly as possible. The tangential path error ${\mathbf{e}}_{\text{p}}^{\Vert }$ was computed for JT-MPC and CT-MPC by projecting onto the Cartesian normalized reference trajectory velocity.

10.2. Opening a drawer

10.2.1. Goal

This experiment aims to open a cabinet drawer using the gripper at the end-effector. The robot has to thread the gripper finger into the handle and then pull toward the opening direction.

10.2.2. Setup

The pose of the cabinet is assumed to be known. The robot has to avoid collisions with the cabinet but not with the handle since contact is necessary to exert a pulling force on the handle. Furthermore, moving the gripper finger into the handle from the top is essential, requires precise positioning, and is crucial for success. The developed framework achieves this by putting a via-point above the handle and descending the reference path into the handle with low orthogonal error bounds to ensure the gripper finger hooks into the handle. Since the reference path is independent of the time parametrization during the motion, the task can be performed successfully at different speeds. The physics simulation was performed in MuJoCo (Todorov et al., 2012).

10.2.3. Results

Three robot configurations during the opening of the drawer are shown in Figure 21. The reference paths π _p and π _o contain three linear segments of which the first positions the end-effector above the handle, the second hooks into the handle and the third segment opens the drawer. In order to avoid collisions and ensure a successful alignment with the handle, the orthogonal path errors $e_{\text{p,\hspace{0.17em}proj},i}^{\perp }$ and $e_{\text{o,\hspace{0.17em}proj},i}^{\perp }$ (i = 1, 2) are bound to be low in the second segment. The same holds for the third segment, which ensures a successful opening. This task is performed with the four different maximum speeds $\overline{\dot{\varphi }}\in \{0.25{\text{ms}}^{-1},0.3{\text{ms}}^{-1},0.5{\text{ms}}^{-1},1.0{\text{ms}}^{-1}\}$ corresponding to the trajectory durations t_f ∈ {3.7s, 4.5s, 6s, 7s}. The trajectories are compared to the Cartesian tracking CT-MPC formulation for which the reference trajectory was chosen to achieve approximately the same trajectory durations t_f. A comparison of the end-effector trajectories in the y-z-plane for different motion planners is shown in Figure 22.

Figure 21.

Visualization of three robot configurations at different time instances along the path. The green box represents a cabinet. The red drawer can be opened by pulling the green handle. The world coordinate system is given by the RGB (xyz) axes at the robot’s base. (a) ϕ = 0 m, (b) ϕ = 1.1 m, and (c) ϕ = 1.44 m.

Figure 22.

End-effector trajectories in the y-z-plane of BoundMPC (upper) and CT-MPC (lower) for the drawer opening scenario. The cabinet with the attached handle is depicted in green.

10.3. Table wiping

10.3.1. Goal

In this experiment, the robot wipes a table with a sponge. The sponge must stays in contact with the table during the wiping.

10.3.2. Setup

Each wiping motion is a straight line across the top of the table where the sponge attached to the robot’s end-effector is in contact with the table. At the end of each wiping motion, the end-effector lifts off the table and transitions to the following wiping line, starting from the opposite side. Following the straight wiping paths to cover the tabletop area is essential. In order to achieve this, the wiping paths have low error bounds in all position and orientation directions. The first position basis vector b_p,1 is the z-direction during the wiping motions, and the lower bound is set to zero so that the end-effector will not press too hard onto the table. We compare BoundMPC for this task with the joint-space trajectory tracking JT-MPC formulation to show the advantage of Cartesian paths.

10.3.3. Results

The robot is visualized during the table wiping task in Figure 23. A comparison of the end-effector position trajectories in the x-y-plane is shown in Figure 24 for BoundMPC and JT-MPC. BoundMPC can perform the task successfully while staying on the tabletop. Small deviations from the path are allowed and do not affect the task success. On the contrary, JT-MPC deviates largely from the desired wiping path.

Figure 23.

Visualization of the robot during the table wiping task: The blue box in the robot’s gripper represents the sponge used to wipe the green table along the white reference path. The world coordinate system is given by the RGB (xyz) axes at the robot’s base.

Figure 24.

End-effector positions trajectories in the x-y-plane during the table wiping task for the BoundMPC formulation and JT-MPC: The Cartesian reference of the BoundMPC formulation is also shown. The wiping direction is from left to right and the diagonal lines of the reference path are the transitions between the wiping movements.

10.4. Collision avoidance for entire kinematic chain

10.4.1. Goal

This task aims to move an object from a start to a goal pose. The previous scenarios use the bounded reference path to ensure collision-freedom for the end-effector only. However, it is straightforward to also account for collisions with the remaining links of the robot. In this scenario, the entire kinematic chain of the robot is checked for collisions. This is a very demanding task as the space where the robot can turn around is highly constrained due to the obstacles.

10.4.2. Setup

The environment contains the shelves, one in front and one behind the robot, and six additional objects close to the robot’s base. The collision avoidance of the end-effector is guaranteed through the collision-free reference path with the collision-free bounding functions. The collision with the remaining links of the manipulator is checked by adding collision avoidance terms similar to Vu et al. (2020), which are also used in the CT-MPC and JT-MPC formulations.

10.4.3. Results

Figure 25 visualizes the initial and final robot configurations during the object transfer. In Figure 26, the resulting end-effector trajectories of BoundMPC, CT-MPC, and JT-MPC are shown in the x-y-plane. BoundMPC is the only framework that successfully performs the task. The CT-MPC gets stuck at about x = 0 while turning around due to the obstacles. Furthermore, using JT-MPC, a collision of a robot link with one of the collision spheres is detected, as shown in Figure 27.

Figure 25.

Visualization of two robot configurations at different times along the path: The green box is the object to be transferred between the two shelves. Six obstacles are shown as black spheres close to the robot’s base. The RGB (xyz) axes give the world coordinate system at the robot’s base. (a) ϕ = 0 m and (b) ϕ = 2 m.

Figure 26.

End-effector trajectories in a top-down perspective (x-y-plane) for the object transfer between two shelves: The trajectories of BoundMPC, CT-MPC, and JT-MPC are shown.

Figure 27.

The minimum distance d_sphere of any robot link to a collision sphere: Each trajectory has to fulfill d_sphere > 0 m to ensure collision freedom.

In order to further evaluate the performance of BoundMPC in this task, the obstacles were shifted along their x-coordinate in the range −0.4 m and 0.4 m. The evaluation of the resulting nine experiments is depicted in Table 7. JT-MPC always collides with the environment due to the collisions in the reference trajectory. The CT-MPC formulation gets stuck at obstacles. BoundMPC is the most successful framework, finishing the task successfully each time without collisions.

Table 7.

Comparison of BoundMPC with offline and online planners for the shelf transfer task with collision avoidance of the robot's full kinematic chain: Nine experiments are compared, in which the six obstacles in Figure 25 are shifted along the x-axis between −0.4 m and 0.4 m with a step length of 0.1 m.

	JT-MPC	CT-MPC	BoundMPC
Successes	0/9	2/9	9/9
Collisions	9/9	1/9	0/9
Stuck at joint limits	0/9	1/9	0/9
Stuck at obstacles	0/9	7/9	0/9

10.5. Object grasp from a table

10.5.1. Goal

This experiment aims at grasping an object from a table. The object pose is changed before the robot can grasp it, causing a replanning toward the new object pose. This replanning shows the strength of the proposed BoundMPC framework.

10.5.2. Setup

The grasping pose is assumed to be known. The robot’s end-effector must not collide with the table or the obstacle on the table during grasping. These requirements can be easily incorporated into the path planning by appropriately setting the upper and lower bounds for the orthogonal position path error. Note that collision avoidance is only considered for the end-effector. The replanning of the reference path allows a sudden change in the object’s pose or a decision to grasp a different object during motion. The newly planned trajectory adapts the position and orientation of the end-effector to ensure a successful grasp. This replanning is performed every time the object changes its position or orientation. To this end, a simple task-specific high-level path planner is implemented. This planner sets the via-points such that the linear paths themselves are collision-free. Then, the bounds are planned based on the distances of the linear segments to the obstacles. First, an example of three object pose changes is constructed, which results in three replanning instances. Second, a simulation of random object pose changes at random times is performed and the robustness of the approach is evaluated.

10.5.3. Results

In order to visualize the robot’s motion in the scenario for three replanning instances, Figure 28 illustrates the robot configuration in four time instances of the trajectory. The robot’s end-effector follows the reference path and deviates from it according to the bounds between the via-points. The replannings occur at t_r ∈ [2 s, 4.5 s, 8.5 s]. At each replanning time instance t_r, the optimization has to adapt the previous solution significantly due to the sudden change of the reference path.

Figure 28.

Visualization of the robot’s configuration at different time instances along the path. The end-effector in red is a gripper attached to the last robot link. The blue box represents the object to grasp, which is above a green table. The red wall is an additional obstacle to be avoided. The first three time instances represent the time of a replanning, where the object’s pose changes. The green and white lines show the end-effector’s planned trajectory and the reference path. The world coordinate system is given by the RGB (xyz) axes at the robot’s base. (a) t = 2 s, (b) t = 4.5 s, (c) t = 8.5 s, and (d) t = 14.6 s.

The error bounds e_p,u,m, e_p,l,m, and e_o,u,m, e_o,l,m, m = 1, 2, are chosen such that collisions with the obstacle on the table, the table itself, and the object to be grasped are avoided. This is visualized in Figure 29 for the trajectory between the second and the third replanning (4.5 s ≤ t ≤ 8.4 s). At this time, the robot’s end-effector must pass over the obstacle on the table, approach the object without collision, and then perform the grasp. The error bounds at t = 4.5 s allow large deviations to simplify the replanning and ensure feasibility.

Figure 29.

Path error trajectories of the orthogonal position path error $e_{\text{p,\hspace{0.17em}proj,\hspace{0.17em}}1}^{\perp }$ and the orthogonal orientation path error $e_{\text{o,\hspace{0.17em}proj},1}^{\perp }$ for the object grasp between the second and third replanning at 4.5 s ≤ t ≤ 8.4 s. The via-points are indicated by vertical gray lines.

During 4.5 s ≤ t ≤ 8.4 s, the projected position path error $e_{\mathrm{p},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},1}^{\perp }$ in Figure 29 is bounded to avoid collisions. In the first segment, ${\mathbf{b}}_{\text{p,\hspace{0.17em}}1\text{,\hspace{0.17em}}1}^{\text{T}}$ is chosen to point toward the closest obstacle, either the table or the obstacle on the table. However, as these are further away from the reference path than 150 mm, the error $e_{\mathrm{p},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},1}^{\perp }$ is bounded by ± 150 mm. In the second segment, the error $e_{\mathrm{p},\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{j},1}^{\perp }$ is bounded from below along the basis directions ${\mathbf{b}}_{\text{p,\hspace{0.17em}}1\text{,\hspace{0.17em}}2}^{\text{T}}=[\mathrm{0,0,1}]$ to avoid collisions with the obstacle. In the third segment, the basis vector b_p,1,3 is chosen to point from the path toward the object. The fourth segment (t ≥ 8.1) is the approach segment, in which the object is approached correctly to ensure a successful grasp. In this segment, the position path error in the basis direction b_p,1,4 is bound to be low. However, the path is replanned at 8.5 s before this grasp happens. The orientation path errors are chosen to be symmetrically bounded by 20° to avoid collisions.

Furthermore, Figure 30 shows the path parameter ϕ and the speed along the path $\dot{\varphi }$ during the trajectory. At the replanning points, the path state jumps according to (61), and the velocity $\dot{\varphi }$ becomes negative, for example, at the third replanning at t = 8.5 s. A video of the experiment can be found at https://www.acin.tuwien.ac.at/42d0/.

Figure 30.

Trajectories of the path parameter ϕ and its time derivative $\dot{\varphi }$ for the object grasping experiment with replanning. The time instances of path replanning are indicated by vertical green lines.

The framework was further tested in the scenario of Figure 28 with randomly sampled object positions

${\mathbf{p}}_{\text{object,\hspace{0.17em}random}}=\left[\begin{array}{c}\hfill 0.6\pm 0.15\text{m}\hfill \\ \hfill 0.0\pm 0.35\text{m}\hfill \\ \hfill 0.1\pm 0.1\text{m}\hfill \end{array}\right]$

(65)

and object orientations, given by Euler angles in convention ZYZ in the range

${\mathit{\theta }}_{\text{euler,\hspace{0.17em}random}}=\left[\begin{array}{c}\hfill \pm 45\text{°}\hfill \\ \hfill \pm 45\text{°}\hfill \\ \hfill \pm 45\text{°}\hfill \end{array}\right]\text{.}$

(66)

One experiment was to perform permanent replanning every 0.5 s to 1.5 s until the MPC did not find a solution. This experiment was conducted 10 times. On average, the MPC performed 95 replanning steps per experiment, corresponding to a failure rate of about 1.1 % of all replanning steps. The proposed framework stayed within the path error bounds to ensure collision-freedom by the design of the reference paths. Hence, no collisions between the robot and the table, the obstacle on the table, or the object were observed.

In order to compare this performance to the state-of-the-art, 50 random poses were sampled according to the procedure mentioned above. JT-MPC, CT-MPC, and BoundMPC were used to plan trajectories to each of the sampled poses. Once the pose was reached, the motion to the next sampled pose was planned. The comparison metrics and results are shown in Table 8.

Table 8.

Comparison of BoundMPC with different motion planners for the object grasp scenario. In total, 50 random object poses were evaluated, and it was ensured that all poses were reachable without collisions. The minimum, mean and maximum values are reported for each planner.

Planner	tplan/s	lpath/m	ttraj/s	r s
JT-MPC	0.01	0.16	1.8	100 %
	0.02	0.71	4.1
	0.05	1.57	6.0
CT-MPC	0.002	0.13	2.3	100 %
	0.02	0.71	5.1
	0.09	1.52	8.2
BoundMPC	0.008	0.15	2.6	100 %
	0.03	0.74	5.1
	0.09	1.60	6.6

10.6. Transfer of a filled open container

10.6.1. Goal

An open container filled with a liquid is grabbed using a two-finger gripper and is to be placed onto a table on the opposite side of the robot’s workspace. In order to avoid spillage, the container needs to remain upright. A via-point above the table and an approach via-point are used to perform the motion.

10.6.2. Setup

The requirement for the container to stay upright translates easily to the orientation path error bounds of the end-effector. For BoundMPC, a maximum of ±2.5° orientation path error is allowed for the rotations around the x and y axis, that is, limiting $e_{\text{o,\hspace{0.17em}proj},i}^{\perp }$ (i = 1, 2), which correspond to the RPY angles α and γ defined in (41). The z-axis represents the upright direction, thus, a rotation around this axis, represented by β, is not essential for spilling. The position path errors $e_{\text{p,\hspace{0.17em}proj},i}^{\perp }$ have large bounds to allow a successful motion. The CT-MPC formulation is also applied to this problem, focusing on position tracking (p) or orientation tracking (o) by changing the weights in the cost function. Furthermore, BoundMPC is compared to its path-tracking variant BoundMPC-PT.

Remark

Note that, the liquid inside the container also moves due to the forces exerted by robot’s motion. Hence, the given requirement is only an approximation.

10.6.3. Results

The start and end configurations of the robot are depicted in Figure 31. Figure 32 shows the trajectories of the RPY angles α, β, and γ for BoundMPC and the two CT-MPC formulations. The BoundMPC framework keeps the RPY angles α and γ within the specified bounds shown as gray areas and successfully reaches the goal state (d_goal → 0). The CT-MPC formulations reach the goal point but struggle to find good solutions. When the cost function is parameterized for position tracking (p), the upright requirement is not fulfilled. However, the requirement is fulfilled for orientation tracking (o), but the trajectory’s smoothness, indicated by the norm of the Cartesian acceleration a_c, is very poor. The path-tracking formulation BoundMPC-PT cannot fulfill the task as it gets stuck (see Figure 32), where d_goal does not reach zero.

Figure 31.

Visualization of two robot configurations in different time instances along the path. The blue cylinder in the robot's gripper represents the filled open container to be placed onto the green table. The world coordinate system is given by the RGB (xyz) axes at the robot’s base. (a) ϕ = 0 m and (b) ϕ = 2.1 m.

Figure 32.

End-effector orientation trajectories given as RPY error angles defined in (31a) for the filled container transfer scenario. The angles α and γ must stay within the gray area to fulfill the upright requirement. The trajectories of BoundMPC are compared to CT-MPC parametrized for either position (p) or orientation (o) tracking. The fourth plot shows the distance d_goal to the goal point and the last plot depicts the norm of the Cartesian acceleration a_c as a measure of trajectory smoothness.

Randomization is applied to this problem where the container placement pose is randomized around a ±90° arc around the position straight behind the robot. The new goal pose is provided to the planner randomly during the approach of the initial pose to investigate online replanning. The results of 20 experiment executions are depicted for BoundMPC and the CT-MPC framework parametrized to minimize the orientation errors in Table 9.

Table 9.

Comparison of the BoundMPC and CT-MPC framework parametrized for orientation following. The metrics are the maximum RPY angle deviations of the upright α and γ and the mean norm of the Cartesian acceleration ‖a_c‖, where the maximum and mean are taken over the trajectory. The minimum, mean and maximum values over 20 tries are provided for each metric.

Formulation	max α	max γ	mean‖ac‖
BoundMPC	1.35	1.85	0.30
	1.98	2.02	0.33
	2.22	2.43	0.38
CT-MPC (o)	0.70	1.35	0.69
	3.15	4.47	0.81
	7.62	9.47	1.08

10.7. Discussion

The evaluated experiments show the versatility and the strengths of the proposed BoundMPC formulation which are discussed in detail in this section.

10.7.1. General

BoundMPC can perform the tasks in all six scenarios well and can compute the joint-space trajectories online as shown, for example, in Sections 10.1 and 10.5. The orientation approximation Figure 19 used by BoundMPC does not restrict the performance throughout the experiments. Specifically, the approximation error α_approx in Figure 19 is low for the first horizon step, and the optimization is performed at each time step in a receding horizon manner. Thus, the applied linear approximation (39) is valid for finding good orientation trajectories.

10.7.2. Collision avoidance

BoundMPC uses bounded path errors for the collision avoidance of the end-effector, whereas CT-MPC and JT-MPC use cost terms in the MPC objective function. The advantage of the proposed formulation becomes clear in Section 10.1. The formulation with collision-free bounds is independent of the obstacles’s amount, shape, and size, making even highly constrained scenarios easily accessible. This way, the obstacle-free space is parameterized instead of the obstacle-occupied space as for CT-MPC and JT-MPC. This leads to small bounds at the end of the trajectory to place the object between the walls (see Figure 18), but does not impede performance. Although the reference trajectory guides the CT-MPC formulation, it struggles to find a collision-free trajectory for the end-effector, resulting in a longer path length l_path. Furthermore, for the transfer of an object between two shelves in Section 10.4 the collision avoidance of the entire robot’s kinematic chain is demonstrated. In this experiment the collision-free reference path guides the robot with the bounding functions toward the goal. CT-MPC and JT-MPC struggle in this scenario because they lack the guiding of the bounding functions. For JT-MPC, a trade-off between minimizing the trajectory error and collision avoidance leads to an unsafe trajectory of the robot, as seen in Figure 27, because the joint-space reference trajectory guides the end-effector around the outside of the spheres, as seen in Figure 26, which is impossible without collisions.

10.7.3. Path following

The path-following formulation used by BoundMPC exhibits advantages over the trajectory tracking used by JT-MPC and CT-MPC. It makes it optimizing the speed along the path possible such that following a complicated reference path causes the end-effector to slow down for the necessary joint movements. A Cartesian reference path or trajectory does not guarantee that the robot’s end-effector can follow it perfectly. Especially the coupling of position and orientation makes it complicated to plan suitably for the end-effector. When planning a trajectory, the time parametrization causes additional problems since the nonlinear robot kinematics must be considered. In the object transfer scenario in Section 10.1, BoundMPC slows down at t ≥ 3.0 s to approach the goal smoothly and slowly, as depicted in Figure 20. In contrast, CT-MPC cannot follow the reference trajectory due to the issues mentioned above, forcing the robot to slow down at 4.5 s ≤ t ≤ 5 s. However, since the reference trajectory progresses, the tracking error in the cost function increases, leading to large Cartesian velocities to decrease the tracking error at t ≥ 5 s. This issue is less pronounced for the joint-space planner JT-MPC because of its large planning horizon. However, making JT-MPC faster causes the end-effector to collide with obstacles. These collisions are caused by a trade-off between collision avoidance and minimizing the tracking error, which the path-following formulation proposed in this paper avoids. This effect is also observed in Section 10.4.

10.7.4. Bounded path errors

As discussed above, the bounded path errors are used for collision avoidance but are also important for task success. Some scenarios require certain path error bounds to finish the task successfully. In Section 10.2, BoundMPC finds suitable trajectories for all trajectory durations t_f and performs the task successfully (see Figure 22). For long trajectory durations (t_f ≥6 s), CT-MPC also manages to solve the task. However, if the durations become small, for example, t_f ≤ 4.5 s, CT-MPC struggles to follow the reference and fails at opening the drawer. CT-MPC reduces tracking errors by shortcutting, which leads to task failure. The BoundMPC formulation stays within the path error bounds and adapts the path velocity $\dot{\varphi }$ accordingly to solve the task. This allows BoundMPC to perform this task 2.3 s faster and more robustly than CT-MPC. These bounded path errors are further used in Section 10.3 to follow a straight line on the table but deviate from the path during the transition segments. The upright constraint in Section 10.6 further demonstrates how orientation constraints can be encoded. In the container-transfer experiment, BoundMPC guarantees low-angle errors for the upright direction, but this is not true for CT-MPC. The latter often violates the upright requirement of |α|, |β| < 2.5°. In Section 10.4, the bounded path errors keep the end-effector in the space between the collision spheres, which guides it successfully toward the goal without collisions. JT-MPC and CT-MPC lack this guidance and struggle with the task.

10.7.5. Purposeful path deviations

The path error in BoundMPC is not minimized but bounded so that the robot can deviate from the path when necessary. This is advantageous in avoiding local minima due to the robot’s kinematic. As seen in Section 10.6 and Figure 32, BoundMPC-PT gets stuck during the container transfer because the reference trajectory is suboptimal and hard to follow precisely. As the tracking errors e_p and e_o are minimized, BoundMPC-PT refrains from leaving the path, which would be necessary in this context. This highlights that the error bounds around the reference path enhance performance and lead to successful task execution. This shows why Astudillo et al. (2022a) require pre-optimized reference paths for their path-tracking formulation. In the same scenario, the deviations do not create additional costs for BoundMPC such that smoother trajectories are obtained than those of CT-MPC.

10.7.6. Cartesian reference paths

The Cartesian reference paths in BoundMPC show advantages over planners working in the joint space. In the table-wiping task in Section 10.3, the robot has to follow a reference path on the table. It is not possible to finish this task successfully with the JT-MPC formulation as the joint-space reference trajectory does not correspond to a wiping motion, emphasizing the need for Cartesian reference paths. In the transfer of a filled open container in Section 10.6, JT-MPC cannot perform this task without additional cost terms or constraints, as the joint-space reference trajectory has no notion of orientation. However, BoundMPC can perform this task successfully due to the Cartesian reference paths.

10.7.7. Path error decomposition

The BoundMPC formulation decomposes the position and orientation path error into tangential and orthogonal directions. This is helpful in Section 10.6 since it becomes possible to bound only the relevant error directions for the upright constraint but leave the remaining directions with large allowed path errors. Table 9 and Figure 32 show that smoother and more successful trajectories are obtained due to this advantage over CT-MPC. A major issue for CT-MPC is that the end-effector’s reference trajectory is not perfectly followable. Thus, CT-MPC has to deviate in either position or orientation, while lacking information on which direction deviations are allowed. If the position tracking error is weighted stronger than the orientation tracking error, the MPC violates the upright requirement. If the orientation is weighted stronger, large non-smooth position deviations occur since the position error grows large, and eventually, the MPC tries to decrease it back to zero. Parametrizing CT-MPC for orientations minimizes all orientation errors. Hence, the error in β is also minimized, which is not critical for spilling. The decomposition is also helpful in Section 10.4 as the position path error is constrained stronger in the direction that points toward the spheres, thus favoring a deviation in the other basis directions. Due to this, BoundMPC successfully solves the task.

10.7.8. Replanning

BoundMPC can perform robust online replanning to react to environmental changes and avoid collisions with the obstacles. Global trajectory planners, for example, Jankowski et al. (2023) and Marcucci et al. (2023), that plan the entire trajectory from the start to the goal pose are generally not real-time capable and therefore not applicable to this scenario. The replanning capabilities are demonstrated in Section 10.5. A simple task-specific higher-level path planner was implemented for this scenario, but it did not consider the robot’s kinematics or dynamics. For complex scenarios a more general and sophisticated planner is required. The design of such a planner is part of our future work. In the comparison in Table 8, CT-MPC performs as well as BoundMPC in this environment, and JT-MPC slightly outperforms it. This is expected as the Cartesian and joint-space reference trajectories are collision-free in most cases However, the maximum planning times t_plan are the same for BoundMPC and CT-MPC, so both concepts are equivalent in this context. Notably, some poses are hard to reach for the manipulator such that the trajectory tracking issues discussed above also apply here, that is, CT-MPC offers no considerable advantage in this application. JT-MPC is fine with hard-to-reach poses as it plans in the joint space. The replanning experiments for the scenario with the filled container in Section 10.6 show that BoundMPC can robustly replan even with task-specific constraints which is not possible for CT-MPC, as seen in Table 9.

11. Conclusion

This work proposes a novel model-predictive trajectory planning for robots in Cartesian space, called BoundMPC, which systematically considers via-points, tangential and orthogonal error decomposition, and asymmetric error bounds. The joint jerk input is optimized to follow a reference path in Cartesian space. The motion of the robot’s end-effector is decomposed into a tangential motion, the path progress, and an orthogonal motion, which is further decomposed geometrically into two basic directions. A novel way of projecting orientations allows a meaningful decomposition of the orientation error using the Lie theory for rotations. Additionally, the particular case of piecewise linear reference paths in orientation and position is derived, simplifying the optimization problem’s online calculations. Online replanning is crucial to adapt the considered path to dynamic environmental changes and new goals during the robot’s motion.

Simulations and experiments were carried out on a 7-DoF Kuka LBR iiwa 14 R820 manipulator, where the flexibility of the new framework is demonstrated in six distinct scenarios.

First, the transfer of a large object through a slit, where lifting of the object is required, is solved by utilizing via-points and specific bounds on the orthogonal error in the position and orientation reference paths, which are synchronized by the path parameter. Second, an object has to be grasped from a table, the commanded grasping point changes during the robot’s motion, and collision with other objects must be avoided. This case shows the advantages of systematically considering asymmetric Cartesian error bounds and the dynamic online replanning. Further scenarios show the drawer opening, the transfer of an open container, and the table wiping. The advantages of path-following concepts compared to trajectory-following and Cartesian reference paths over joint-space reference paths are elaborated. The last scenario highlights the possibility of collision avoidance for the entire robot’s kinematic chain.

Due to the advantageous real-time replanning capabilities and the easy asymmetric Cartesian bounding, we plan to use BoundMPC in dynamically changing environments in combination with cognitive decision systems that adapt the pose reference paths online according to situational needs.

Supplemental Material