A Dynamic Motion Planner for Trajectory Tracking in HRC

Abstract

In human-robot collaboration (HRC), robots operate alongside humans within a shared workspace. During collaborative handling tasks, human movements are often highly individual and variable. To ensure smooth collaboration, the robot must adapt its trajectory to align with the motion of the human co-worker. Therefore, this work proposes a dynamic motion planner that enables the robot to track a dynamically changing reference trajectory. The motion planner is evaluated based on its ability to track the trajectory while respecting joint velocity and acceleration limits and avoiding kinematic singularities. When these constraints are at risk of being violated, the robot temporarily assumes a dominant role and attempts to approximate the reference trajectory as closely as possible. An evaluation using a KUKA iiwa in a laboratory setup demonstrates that the proposed motion planner can effectively track dynamically changing, physically feasible reference trajectories. Assuming that the reference trajectory can be human motion, the motion planner can harmonize human and robot movements during collaborative handling tasks.

1. Introduction and Related Works

1.1. Motivation

Industrial manufacturing is experiencing a profound shift as collaborative robots take on a growing role in complementing human capabilities within assembly processes [1]. Human–robot collaboration (HRC) integrates the unique strengths of humans and robots to create production environments that are flexible, efficient, and centered on human well-being [2]. As production systems move toward higher levels of customization, HRC offers the potential to improve process speed, accuracy and operator ergonomics. Nevertheless, its implementation in real-world manufacturing remains limited due to persistent performance and safety challenges, particularly under high-speed operating conditions [3].

When a robot and a human are required to jointly manipulate an object, with the robot following the human’s lead, trajectory planning must be dynamic and adaptive. Since human motion inherently exhibits variability, the primary challenge in implementing HRC applications is to enable the robot to change its trajectory dynamically according to human behavior. In industrial settings, robot trajectory changes are typically restricted to conservative strategies, where path changes are mostly excluded. In high-level human–robot collaboration, where both partners work on the same object, the robot must adapt both its path and its speed. Without this capability, cooperative handling or accommodation of deviations in human motion is not feasible.

Human motion prediction in collaborative handling tasks, as demonstrated by Saeed et al. [4] and Ding et al. [5], provides dynamically changing reference trajectories. Consequently, robots should leverage these predictions directly by tracking dynamically changing trajectories. The aim of this work is to enable the robot to track a dynamically changing Cartesian reference trajectory in a HRC scenario. Therefore the robot must simultaneously avoid kinematic singularities and comply with joint velocity and acceleration constraints.

1.2. Related Works

Miro et al. [6] provide an overview of such trajectory adaptation methods. One example is speed and separation monitoring (SSM), where a speed-dependent safety distance must be maintained between human and robot [7,8]. Glogowski et al. [9] advanced this concept by computing the separation distance dynamically, enabling precise speed control to meet SSM requirements. Dsouza et al. [10] extend this line of work by introducing a collision-free trajectory planning approach based on artificial potential fields, which also embeds safety mechanisms comparable to those used in SSM. Another approach is power and force limiting (PFL), which permits low-energy contacts that remain within biomechanical limits by keeping robot motion slow [11]. Both strategies adjust the velocity but leave the preplanned path unchanged.

Haddadin et al. [12,13] proposed an impedance control framework for torque-sensor-equipped robots, enabling compliance through allowable path deviations under external forces. While this increases flexibility, it does not allow substantial trajectory changes in response to human movement. Faulwasser et al. [14,15] addressed path flexibility through a nonlinear model predictive path-following controller using torque inputs, enabling path tracking with or without speed constraints. Both of these methods are torque-based, whereas the control strategy presented in this work relies on position inputs. Other approaches, such as those by Villagrossi et al. [16] and Noohi et al. [17], employ interaction-force–based movement strategies for collaborative object manipulation. However, these methods only react to measured interaction forces and do not account for dynamically changing reference trajectories, limiting their ability to anticipate and adapt to highly dynamic human motion.

When adapting preplanned robot trajectories on the fly, several strategies can be employed. Liu et al. [18] provide an overview of dynamic motion planning methods. Some approaches rely on sampling-based techniques, such as the work of Covic et al. [19], who introduce the dynamic Rapidly-Exploring Generalized Bur Tree, ensuring safe motion in dynamic environments through a hard real-time scheduling framework and automatic velocity adjustment based on the minimum obstacle distance. Other approaches use optimization-based methods, including the work of Ansari et al. [20], who generate assistive forces to reduce human effort, as well as the method of Pupa et al. [21], who propose a trajectory optimization scheme that allows deviations within a tube around a reference path. Yet other methods, such as that of Chen et al. [22], employ deep reinforcement learning with a soft actor–critic architecture, which requires large amounts of training data that may not be available for highly dynamic behaviors. Although these strategies differ in design, they share the limitation that the robot only adapts its motion locally, which prevents them from tracking rapidly and continuously changing trajectories. Because human motion prediction in collaborative handling tasks, as demonstrated by Saeed et al. [4] and Ding et al. [5], provides dynamically changing reference trajectories, robots should leverage these predictions directly by tracking those trajectories.

Dani et al. [23] proposed a method for tracking a Cartesian reference motion obtained from human trajectory estimation. However, their approach leaves open how dynamically changing Cartesian reference trajectories can be transformed into joint-space while simultaneously avoiding singularities and respecting joint velocity and acceleration constraints.

Avoiding kinematic singularities is a central challenge when following a Cartesian reference trajectory. A criterion to determine how well a robot can move from certain postures is the manipulability index as defined by Yoshikawa [24]. Furthermore Zaplana et al. [25] propose a method that determines the distance from the current robot configuration to singular states. Several Cartesian Space (CS) planning algorithms make use of this manipulability measure. Approaches like Shi et al. [26] analyse the joint space for singularities and map them to CS in order to plan a trajectory that avoids those singularities. Liu et al. [27] avoids singularities in time-optimal trajectory planning by detecting singular regions through Jacobian decomposition and applying cyclic damping to adjust joint velocities, ensuring smooth motion within kinematic constraints. Beck et al. [28] propose a method for singularity avoidance in trajectory optimization by introducing potential functions that penalize low-manipulability configurations, thereby steering the robot away from singular regions while maximizing manipulability. All these approaches have in common that they are computationally expensive and that they are executed before a movement starts. The avoidance strategy proposed in this work is able to avoid singularities on the fly.

Another important aspect is ensuring compliance with joint velocity and acceleration limits while tracking a trajectory defined in Cartesian space. One approach to ensure compliance with these constraints is trajectory scaling. Zhang et al. [29] and Wolinski et al. [30] formulate the IK problem as a quadratic program and incorporate trajectory scaling to guarantee that joint velocity and acceleration limits are respected for a planned trajectory. The saturation in null space algorithm, as proposed by Flacco et al. [31], handles IK for redundant manipulators by enforcing joint limits and scaling tasks when necessary. In contrast, this work proposes a simple approach that just slows the robot down when a velocity violation is expected.

1.3. Research Questions

This paper intends to demonstrate how a robot can adapt to dynamically changing trajectories, such as those arising in HRC when jointly handling a rigid object. Specifically, it seeks to address the following questions:

1.

How can a robot track a dynamically changing reference trajectory for its TCP in soft real time?

2.

What tracking performance can be achieved when following dynamically changing Cartesian reference trajectories?

3.

Who is dominating the movement and when?

Those questions are part of the project “High-speed motion tracking and coupling for human-robot collaborative assembly tasks” (HiSMot). The goal of that project is to harmonize human and robot motion during cooperative handling of a rigid object.

The content of this research article is structured as follows. In Section 2, a motion planner is presented that enables the robot to track a dynamically changing TCP reference trajectory. There are also mechanisms integrated that let the robot avoid singularities on its path and adhere to joint velocity and acceleration constraints. In Section 3, this proposed planner is an laboratory environment. The above formulated research questions are discussed in Section 4 and this research article closes with a brief view on future research in Section 5.

2. Materials and Methods

2.1. Structure of the HiSMoT Controller

The controller proposed in the HiSMoT-project is shown in Figure 1. It comprises two main components: the Trajectory-based Latent Space (LS) controller, which generates the reference motion for the robot and has been presented in separate work [4], and the low-level motion planning of the robot, which is presented in this article. The LS-Controller generates human motion that serves as a reference for the dynamic motion planner. It is based on a data-driven model for synthesizing natural human motions proposed by Min et al. [32]. This model was later extended by Manns et al. [33,34], enabling the identification of clusters of natural motions in assembly contexts by mapping human motion data into a low-dimensional latent space (LS), where each point represents an entire body motion. By learning the distribution of these points the model can generate new motions that fit within the learned natural variation. This model was further specified by Saeed et al. [4] to generate human movements for handling a rigid body object during assembly tasks. In the scope of this paper, a movement where a human and a cobot collaboratively move a rod from location A to location B is investigated. The human motion generator model is not in the scope of this research article. It is assumed that the human and the robot have already grasped their respective sides of the rod. The movement to reach the rod is disregarded. It is assumed that the LS-Controller knows the current location where the human grasps the rod in location A and a desired goal location B. The LS-Controller now generates a set of natural movements for all joints of the human skeleton model. From that, the most probable motion is extracted [4].

To properly control the robot, only the movement of the hand, which is coupled to the rod, is extracted. Because the rod is a rigid body with a known length, and the human hand is assumed to be fixed to the rod, the trajectory of the TCP of the robot can be derived from that. Hereafter, this is referred to as the reference trajectory ${\mathbf{x}}_{\mathrm{ref}}\left(t\right)$. Due to the individual character of each human movement, it is clear that the human motion will differ from the guessed reference trajectory at some point. As long as the movement continues, the more likely deviations from the proposed trajectory will occur. So the LS-Controller needs feedback from the human hand to update the guessed trajectory in real-time. From the known robot TCP position ${\mathbf{x}}_{\mathrm{act}}$, the position of the human hand can be inferred. During motion, the LS-Controller continuously updates the probabilities of possible human movements and determines the most likely trajectory. Based on the observed motion history from location A to the current point, it then generates a human trajectory toward the desired goal. Once the new reference trajectory is generated, ${\mathbf{x}}_{\mathrm{ref}}\left(t\right)$ is changed. The new reference is then the input for the motion planner.

Since the LS-Controller updates the reference trajectory online, the robot needs a planning strategy capable of adapting to these real-time changes. The primary focus of this paper is the motion planning of the robot, aiming to track a reference trajectory that could be obtained from a human motion model. In the scope of this work, it is assumed that the output of such a motion model always represents physically possible motion, as it is intended to reflect natural human movement. Since the reference trajectory changes in real-time, the robot must support real-time control. Therefore the output of the motion planner are joint commands that are directly executed by the robot.

There are several reasons why a robot may not be able to track a Cartesian trajectory with a simple motion planner that only transforms Cartesian positions into joint state commands via IK. The first reason is that the reference can lead the robot into a singularity. If the robot gets too close to a singularity, the IK cannot find a solution for the joint states, or leads the robot into very fast joint movements. Therefore, singularities need to be avoided by the movement of the robot. The second reason is the violation of joint and acceleration limits in the joint space. Even slow movements in CS can lead to fast movements of the single joints of the robot. So the velocity of every joint of the robot is constrained. When constraining velocity and acceleration, a deviation from the reference trajectory will occur. Those two reasons are the leading causes for the robot not being able to track a given Cartesian reference trajectory. In those cases, the robot should assume a dominant role and try to move on a trajectory that minimizes the error between the reference motion and the robot motion.

2.2. Path Following

The proposed motion planner is presented in the following. The formulation begins with the task of tracking a Cartesian path that may change dynamically and is subsequently extended to trajectory tracking with singularity avoidance and compliance with joint velocity limits. To this end, a set of simplifying assumptions is introduced first and then gradually relaxed in later sections, thereby increasing the complexity of the motion planner:

1.

The path lies within the workspace of the robot and every point is reachable

2.

The path does not change

3.

The TCP velocity is assumed as constant

4.

There are no singularities on the path that the robot has to avoid

5.

There are no constraints on velocity and acceleration for the joints of the robot

The path is generated by the LS-Controller depicted in Figure 1, which gives a sequence of points

$$P:={\left({\mathbf{p}}_i\right)}_{i=0}^n\mathrm{with}{\mathbf{p}}_i\in {\mathbb{R}}^3,$$

(1)

that represent a natural human motion, where ${\mathbf{p}}_0$ is the start location and ${\mathbf{p}}_n$ is the destination. The points contain Cartesian coordinates where the orientation of the gripper is first assumed as fixed. A discrete series with varying distances between neighboring points is not suitable for directly controlling the robot. Therefore, the path P is approximated with a cubic-spline. To apply the spline approximation, the given points are parameterized over the Cartesian distance between neighboring points

$$t_0^{\prime }=0,t_i^{\prime }={\displaystyle \sum _{k=1}^i}\parallel {\mathbf{p}}_k-{\mathbf{p}}_{k-1}\parallel .$$

(2)

Those parameters are then normalized in the interval $[0,1]$ with

$$t_i={\displaystyle \frac{t_i^{\prime }}{t_n^{\prime }}}i=1,\dots ,n.$$

(3)

Now $t_i$ indicates the relative position and the relative arc length on the parameterized curve that connects the points of P with a linear approximation. When approximating the movement of the human this is not sufficient because the linear approximation is not smooth enough. To ensure a smooth transition between the waypoints, $C^2$ steadiness is applied, which means that the first and second derivative are continuous at the transition points and so the velocity and acceleration are steady in those points too.

Because the sequence of points P is assumed to represent realistic motion and the waypoints are sampled at an approximately uniform frequency of around 60 Hz, no significant changes in velocity or acceleration are expected between neighboring waypoints. Furthermore, because the original trajectory is unlikely to be highly curved due to the naturalness of human motion and because the distance between neighboring waypoints is small, the error between the linear approximation and a spline-based representation remains small. The difference between arc length and chord length is of second-order with respect to the waypoint spacing and is therefore negligible for the considered sampling rate.

To modulate such a function cubic splines

$${\mathbf{s}}_i\left(t\right)={\mathbf{a}}_i+{\mathbf{b}}_i(t-t_i)+{\mathbf{c}}_i{(t-t_i)}^2+{\mathbf{d}}_i{(t-t_i)}^3,t\in [t_i,t_{i+1}],i=0,\dots ,n-1$$

(4)

with

$${\mathbf{s}}_i\left(t\right)={\left(\begin{array}{ccc}x\left(t\right)& y\left(t\right)& z\left(t\right)\end{array}\right)}^T$$

are used. With linear algebra and the Thomas-Algorithm [35] the coefficients of the cubic spline are

$$\begin{array}{ccc}\hfill {\mathbf{a}}_i& ={\mathbf{p}}_i\hfill & \hfill \mathrm{for}i=0,\dots ,n-1\\ \hfill {\mathbf{b}}_i& ={\displaystyle \frac{{\mathbf{p}}_{i+1}-{\mathbf{p}}_i}{t_{i+1}-t_i}}-{\displaystyle \frac{1}{3}}\left({\mathbf{c}}_{i+1}+2{\mathbf{c}}_i\right)\left(t_{i+1}-t_i\right)\hfill & \hfill \mathrm{for}i=0,\dots ,n-1\\ \hfill {\mathbf{c}}_i& =\left\{\begin{array}{cc}0\hfill & \mathrm{for}i=0\wedge i=n-1\hfill \\ {\mathbf{z}}_i-{\mu }_i{\mathbf{c}}_{i+1}\hfill & \mathrm{for}i=1,\dots ,n-2\hfill \end{array}\right.\hfill & \\ \hfill {\mathbf{d}}_i& ={\displaystyle \frac{{\mathbf{c}}_{i+1}-{\mathbf{c}}_i}{3\left(t_{i+1}-t_i\right)}}\hfill & \hfill \mathrm{for}i=0,\dots ,n-1\end{array}$$

where the coefficients ${\mathbf{c}}_i$ are calculated recursively. The coefficients in ${\mathbf{c}}_i$ are

$$\begin{array}{ccc}\hfill {\mathbf{z}}_i& ={\displaystyle \frac{{\mathbf{\alpha }}_i-\left(t_i-t_{i-1}\right){\mathbf{z}}_{i-1}}{l_i}}\hfill & \hfill \mathrm{for}i=1,\dots ,n-2\\ \hfill {\mu }_i& ={\displaystyle \frac{t_{i+1}-t_{i-1}}{l_i}}\hfill & \hfill \mathrm{for}i=1,\dots ,n-2\end{array}$$

with

$$\begin{array}{ccc}\hfill l_i& =2\left(t_{i+1}-t_{i-1}\right)-\left(t_i-t_{i-1}\right){\mu }_{i-1}\hfill & \hfill \mathrm{for}i=1,\dots ,n-2\\ \hfill {\mathbf{\alpha }}_i& ={\displaystyle \frac{3\left({\mathbf{p}}_{i+1}-{\mathbf{p}}_i\right)}{t_{i+1}-t_i}}-{\displaystyle \frac{3\left({\mathbf{p}}_i-{\mathbf{p}}_{i-1}\right)}{t_i-t_{i-1}}}\hfill & \hfill \mathrm{for}i=1,\dots ,n-2\end{array}$$

and the initial values of $l_0=l_{n-1}=1$ and ${\mu }_0=z_0=z_{n-1}$. With these coefficients, the cubic splines Equation (4) can be calculated from the support points of the path Equation (1) and the parameterization of the curve Equation (3). The spline is now used as the reference path, which the robot’s TCP has to track.

To control a real or simulated robot using position control, discrete commands are required. Within the scope of this work, the 7-DoF KUKA LBR iiwa is used. The KUKA Fast Robot Interface (FRI) package [36] enables direct position control of the real robot in hard real time at a frequency of 200 Hz. The proposed motion planner operates in soft real time with a control frequency f of 20 Hz. Therefore, Section 2.7 introduces a buffering structure that transmits commands to the robot at the required control frequency of 200 Hz.

Now the goal is to derive the next position command ${\mathbf{x}}_{\mathrm{next}}$. With known constant control frequency f of the robot and constant Cartesian TCP velocity $\overline{v}$, the distance between the current position and next commanded position follows as

$$\Delta s={\displaystyle \frac{\overline{v}}{f}}.$$

(5)

Because ${\mathbf{x}}_{\mathrm{act}}\left(t_k\right)$ and the distance $\Delta s$ are known ${\mathbf{x}}_{\mathrm{next}}\left(t_{k+1}\right)$ can be calculated with Equation (4). Therefore it is assumed that for small deviations in t the curve $\mathbf{s}\left(t\right)$ is linear. The arc length between two values of t can be calculated with

$${\int }_{t_j}^{t_j+\tau }∥{\mathbf{s}}^{\prime }\left(t\right)∥dt\approx \underset{f\left(N\right)}{\underbrace{\sum _{\eta =1}^N∥\mathbf{s}(t_k+\eta \delta t)-\mathbf{s}(t_k+\left(\eta -1\right)\delta t)∥}}=\Delta s,$$

where the integral is approximated using a numerical Riemann sum, which sums straight-line distances between sampled positions on the curve $\mathbf{s}\left(t\right)$. To find a $\tau$ that satisfies this equation from above $\delta t$ is chosen very small and N is increased stepwise. It is obtained with

$$\tau =\delta t·arg\underset{N}{min}\left\{f\left(N\right)\ge \Delta s\right\}.$$

(6)

With knowledge of the spline Equation (4) the next Cartesian joint command can be obtained with

$${\mathbf{x}}_{\mathrm{next}}\left(t_{j+1}\right)=\mathbf{s}(t_j+\tau ).$$

(7)

With the assumptions made in the beginning of this section, the robot can track a given path with constant velocity. Those equations are summarized by the Next Point Generator (NPG) as depicted in Figure 2. The inputs of the NPG are the current position of the robots TCP ${\mathbf{x}}_{\mathrm{act}}\left(t_j\right)$, the control frequency of the robot f, the desired constant TCP-velocity $\overline{v}$ and the given support points P. From that, first the spline $\mathbf{s}\left(t\right)$ is calculated with Equation (4) and after that ${\mathbf{x}}_{\mathrm{next}}\left(t_{j+1}\right)$ with Equation (7).

A key characteristic of human movement is its high variability. When a robot works with a human, it can not be assumed that the human stays on an initially predicted path. So, the robot needs to be able to track a changing path. Therefore, Assumption 2 is modified such that the reference path is no longer considered fixed but can instead change dynamically during the robot’s motion. If the reference path is changing, two things are essential. On the one hand, the robot needs to be able to accept movement commands in real-time, which is guaranteed by the above mentioned position control. On the other hand the robot needs to move smoothly to the new path.

Therefore, the reference $\mathbf{s}\left(t\right)$ is discretized with Equation (6) and $\Delta s=l$ so that a sequence

$$W:={\left({\mathbf{w}}_j\right)}_{i=0}^e\mathrm{with}{\mathbf{w}}_j\in {\mathbb{R}}^3$$

of equidistant points ${\mathbf{w}}_j$ on $\mathbf{s}\left(t\right)$ is obtained. One exception is the distance between ${\mathbf{w}}_{e-1}$ and the last point of the entire movement ${\mathbf{w}}_e$, which could be less than l because the length of the approximated curve is, with high probability, not a multiple of l. To reach the next target ${\mathbf{w}}_{j+1}$, the NPG is modified. The last target position ${\mathbf{w}}_j$ is saved in a buffer. From there, the TCP should move in a straight line to ${\mathbf{w}}_{j+1}$. The NPG is modified so that

$${\mathbf{x}}_{\mathrm{next}}={\mathbf{x}}_{\mathrm{act}}+\Delta s{\displaystyle \frac{{\mathbf{w}}_{j+1}-{\mathbf{w}}_j}{∥{\mathbf{w}}_{j+1}-{\mathbf{w}}_j∥}},$$

(8)

where the fraction on the right hand side is the normalized direction from ${\mathbf{w}}_j$ to ${\mathbf{w}}_{j+1}$. Now, when the reference spline $\mathbf{s}\left(t\right)$ changes to ${\mathbf{s}}^{\prime }\left(t\right)$ the sequence of equidistant points W changes to $W^{\prime }$ accordingly. This has also consequences for ${\mathbf{x}}_{\mathrm{next}}$, a transition to the new path $W^{\prime }$ becomes necessary. The sketch in Figure 3 illustrates a reference spline $\mathbf{s}\left(t\right)$, which is updated at the next time stamp to ${\mathbf{s}}^{\prime }\left(t\right)$. The black crosses mark the sequences of support points P and $P^{\prime }$, which do not need to be equally spaced. In contrast, the red crosses indicate the sequences of equidistantly spaced points W and $W^{\prime }$ along the reference spline.

Because the new path $W^{\prime }$ has not necessarily the same length as W a metric is derived to avoid that the NPG gives points that are not in the direction of the target point and steer the TCP closer to the start. Imagine a scenario where the arc length of $W^{\prime }$ is longer than that of W. The target of the NPG is ${\mathbf{w}}_{j+1}$. Now the question arises: which point in $W^{\prime }$ one has to choose in order to avoid a movement backwards in the direction of the starting point. The first guess would be the target point ${\mathbf{w}}_{j+1}^{\prime }$ corresponding to ${\mathbf{w}}_{j+1}$. When the arc length of $W^{\prime }$ is longer than the arc ${\mathbf{w}}_{j+1}$ most likely lies not in the direction of the end point. To measure this, the cosine similarity

$$\sigma =\cos \left(\Theta \right)={\displaystyle \frac{{\mathbf{v}}_c·{\mathbf{v}}_e}{∥{\mathbf{v}}_c∥∥{\mathbf{v}}_e∥}}$$

is used, with 

$$\begin{array}{cc}\hfill {\mathbf{v}}_c& ={\mathbf{w}}_{j+1+k}^{\prime }-{\mathbf{x}}_{\mathrm{act}}\hfill \\ \hfill {\mathbf{v}}_e& ={\mathbf{w}}_e-{\mathbf{x}}_{\mathrm{act}}\hfill \end{array}$$

where ${\mathbf{v}}_c$ is called the candidate vector with an initial $k=0$ and ${\mathbf{v}}_e$ the vector that points in the direction of the target of the entire movement. Now $\sigma$ is evaluated, if it is greater than zero it means that ${\mathbf{w}}_{j+1}^{\prime }$ lies roughly in the same direction from ${\mathbf{x}}_{\mathrm{act}}$ as ${\mathbf{w}}_e$. This means that the target is viable and could be approached. If $\sigma$ is equal to or less than zero, it means that ${\mathbf{w}}_{j+1}^{\prime }$ does not lie in the direction of ${\mathbf{w}}_e$. In that case, k is increased and $\sigma$ is re-evaluated. This is repeated until $\sigma$ has a positive value. Then ${\mathbf{w}}_{j+1+k}^{\prime }$ is used as the next target of the NPG. If it happens that $W^{\prime }$ is shorter than W it is possible that for ${\mathbf{w}}_{j+1}$ no corresponding ${\mathbf{w}}_{j+1}^{\prime }$ exist. In such a case, the NPG chooses ${\mathbf{w}}_e$ as next target. So the next position command sent to the robot is

$${\mathbf{x}}_{\mathrm{next}}={\mathbf{x}}_{\mathrm{act}}+\Delta s{\displaystyle \frac{{\mathbf{w}}_{j+k+1}^{\prime }-{\mathbf{w}}_j}{∥{\mathbf{w}}_{j+k+1}^{\prime }-{\mathbf{w}}_j∥}}.$$

(9)

In summary, this method ensures that the NPG does not set a target that lies not in the direction of the end position of the movement. So no backwards movement is possible. This method is used for paths $W^{\prime }$ that could be longer or shorter than W. Summarized the changes made to the NPG in Figure 2 are that the spline is now discretized and those discrete points are used as intermediate goals. The next position command ${\mathbf{x}}_{\mathrm{next}}$ is calculated based on a straight line from the current position to the next intermediate goal on the reference. It is also checked whether the path has changed, and the next intermediate goal is found based on the method explained above, which finds the next position command roughly in the direction of the target position of the entire movement. The TCP of the robot is now able to track a dynamically changing path under the assumptions named above.

2.3. Trajectory Tracking

Human movement is inherently variable, and it is highly unlikely that a person maintains a constant velocity during motion. Instead, velocity continuously fluctuates over the course of the movement. In the context of HRC, the robot must be capable of tracking a trajectory while dynamically adjusting the velocity of its TCP. Accordingly, the third assumption from Section 2.2 has been revised to account for a time-varying TCP velocity.

Therefore, the given set of points (see Equation (1)) is modified to a set of tuples

$$P_{\mathrm{v}}:={\left(({\mathbf{p}}_i,{\tilde{v}}_i)\right)}_{i=0}^n\mathrm{with}{\mathbf{p}}_i\in {\mathbb{R}}^3,{\tilde{v}}_i\in {\mathbb{R}}_0^{+}$$

(10)

which defines the position of the TCP and the velocity at the specified position. This implies that a trajectory, rather than only a path, is now specified. The path itself is still approximated using Equations (2)–(4). The resulting spline $\mathbf{s}\left(t\right)$ is then roughly parameterized by its arc length. After that, due to the small distances between the known points and the naturalness of the approximated curve, no sudden changes in velocity are expected. Therefore, the velocity along this path is approximated linearly using

$$v_i\left(t\right)=\left(1-\gamma \right){\tilde{v}}_i+\gamma {\tilde{v}}_{i+1},t\in [t_i,t_{i+1}],i=0,\dots ,n-1$$

with

$$\gamma ={\displaystyle \frac{t-t_i}{t_{i+1}-t_i}}.$$

(11)

So the trajectory is defined by

$$\mathit{\xi }\left(t\right):=\left(\mathbf{s}\left(t\right),v\left(t\right)\right),t\in [t_0,t_n]$$

where the path and velocity are interpolated separately. Since the sequence in Equation (10) changes rapidly, interpolation methods such as Hermite splines [37], which compute the velocity as the derivative of the spline, are too computationally demanding to be executed at the robot’s control frequency. For the approximation, it is assumed that the path between two consecutive points is approximately linear and that the velocity changes linearly between those points, either increasing or decreasing. Now the trajectory is discretized into control points regarding its arc length

$$W_v=\left(({\mathbf{w}}_j,v_j)\right),\mathrm{with}{\mathbf{w}}_j\in {\mathbb{R}}^3,v_j\in {\mathbb{R}}_0^{+}$$

where the points ${\mathbf{w}}_j$ are discretized in the same way as in Section 2.2, and the velocity $v_j$ is obtained by evaluating $v\left(t_j\right)$. To determine the corresponding step size $\Delta s$ for the next Cartesian joint command, the constant velocity $\overline{v}$ in Equation (5) is replaced with

$$\Delta s={\displaystyle \frac{v_{\mathrm{next}}}{f}}$$

(12)

and must be evaluated in every sample. Since the current position ${\mathbf{x}}_{\mathrm{act}}$, the previous control point $({\mathbf{w}}_j,v_j)$ and the next control point $({\mathbf{w}}_{j+1},v_{j+1})$ are known the velocity for the next position command can be calculated with

$$v_{\mathrm{next}}=v_j+{\displaystyle \frac{∥{\mathbf{x}}_{\mathrm{act}-{\mathbf{w}}_j}∥}{∥{\mathbf{w}}_{j+1}-{\mathbf{w}}_j∥}}\left(v_{j+1}-v_j\right)+v_{\min }{e}^{-\lambda v_j}$$

(13)

which performs a linear approximation between the given control points. The last term was added to guarantee a minimum velocity $v_{\min }$ at the start and before the stop of the movement. This ensures that the robot motion is properly initiated and that the final waypoint can be reached. The exponential decay form enables a smooth transition between the minimum velocity and the velocity profile generated by the trajectory approximation. The damping factor $\lambda$ must be selected sufficiently large such that the last term vanishes for adequately high velocities $v_j$, thereby having a negligible effect on the trajectory approximation outside the start and stop regions. With known $\Delta s$, the next position command can be obtained from Equation (8).

If the reference trajectory changes at runtime, the same strategy as described in Section 2.2 is applied. For a new trajectory $W_v^{\prime }$, the next target point ${\mathbf{w}}_{j+1}^{\prime }$ and the corresponding $v_j^{\prime }$ are found as described and then used as the target control point. The new target is again approached linearly in position and velocity so that the step distance $\Delta s$ in Equation (9) with

$${\mathbf{x}}_{\mathrm{next}}={\mathbf{x}}_{\mathrm{act}}+\Delta s^{\prime }{\displaystyle \frac{{\mathbf{w}}_{j+k+1}^{\prime }-{\mathbf{w}}_j}{∥{\mathbf{w}}_{j+k+1}^{\prime }-{\mathbf{w}}_j∥}}.$$

(14)

is modified and becomes the dynamic step distance

$$\Delta s^{\prime }f=v_{\mathrm{next}}^{\prime }=v_j+{\displaystyle \frac{∥{\mathbf{x}}_{\mathrm{act}-{\mathbf{w}}_j}∥}{∥{\mathbf{w}}_{j+k+1}^{\prime }-{\mathbf{w}}_j∥}}\left(v_{j+k+1}^{\prime }-v_j\right)+v_{\min }{e}^{-\lambda v_j}.$$

(15)

Using Equation (14), the NPG can track a changing reference trajectory. Figure 4 illustrates an example of position commands for both increasing and decreasing velocity between ${\mathbf{w}}_j$ and ${\mathbf{w}}_{j+1}$. For clarity, the example is shown in two-dimensional space. As can be seen, when the velocity increases, the distance between consecutive position commands becomes larger, whereas it decreases when the velocity is reduced. If the target changes due to a new reference trajectory, both the direction and the spacing of the position commands adapt accordingly. It should be noted that the NPG generates Cartesian position commands to track the given reference, without yet verifying whether the robot is capable of executing these commands.

2.4. Avoidance of Singularities

When sending position commands to a robot, it must be verified whether they can be executed. In this section, assumption 4. from Section 2.2 is relaxed, allowing for the possibility that the robot may be driven toward a singularity. Nonetheless, it is still ensured that the motion remains entirely within the robot’s workspace. For the purposes of this paper, the robot under consideration is a 7-DoF KUKA iiwa, as mentioned above, which can be controlled via position commands. The robot accepts joint commands $\mathbf{q}\in {\mathbb{R}}^N$, with $N=7$ corresponding to the number of joints.

This section presents a strategy for avoiding singularities. The robot’s kinematics are briefly reviewed to introduce the manipulability measure employed in the approach. From given joint states the Cartesian position and orientation of the TCP of the robot can be calculated with

$$\mathbf{x}=f\left(\mathbf{q}\right)f:{\mathbb{R}}^M\to {\mathbb{R}}^N\mathrm{and}\mathbf{x}=\left(\begin{array}{c}{\mathbf{x}}_{\mathrm{next}}\\ {\mathbf{x}}_o\end{array}\right)\mathrm{with}{\mathbf{x}}_{\mathrm{next}},{\mathbf{x}}_o\in {\mathbb{R}}^3$$

(16)

where ${\mathbf{x}}_{\mathrm{next}}$ corresponds to the Cartesian position and ${\mathbf{x}}_o$ to the orientation of the TCP, which together form a $M=6$ DoF representation in CS. The function f is obtained from the known geometry of the robot and the Denavit–Hartenberg parameters [38]. It is expressed as a concatenation of transformation matrices, each parameterized by $\mathbf{q}$. The NPG for trajectory generation provides the Cartesian position ${\mathbf{x}}_{\mathrm{next}}$ with fixed orientation, as the object to be handled should not change its orientation while moving in the collaboration investigated here. So the orientation ${\mathbf{x}}_o$ is set to an angle where the gripper mounted on the robot is in a vertical position above the rod. This means every position has the same orientation. The joint states can be obtained with the Newton-Raphson method in an iterative manner

$${\mathbf{q}}_{k+1}={\mathbf{q}}_k+{\mathbf{J}}^{+}\left({\mathbf{q}}_k\right)\underset{\mathrm{err}}{\underbrace{\left(\mathbf{x}-f\left({\mathbf{q}}_k\right)\right)}}$$

with appropriate initial values ${\mathbf{q}}_k$ and the goal to minimize the error term err. The Moore-Penrose inverse [39]

$$\mathbf{J}{\left(\mathbf{q}\right)}^{+}={\mathbf{J}}^{\ast }\left(\mathbf{q}\right){\left(\mathbf{J}\left(\mathbf{q}\right){\mathbf{J}}^{\ast }\left(\mathbf{q}\right)\right)}^{-1}$$

is used because the Jacobian

$$\mathbf{J}\left({\mathbf{q}}_k\right)={\displaystyle \frac{\partial f\left({\mathbf{q}}_k\right)}{\partial {\mathbf{q}}_k}}$$

is of dimension $N\times M$ and thus cannot be inverted. In the following, the iterative process to retrieve the IK is denoted as

$$\mathbf{q}=\mathrm{IK}\left(\mathbf{x}\right)\mathrm{IK}:{\mathbb{R}}^M\to {\mathbb{R}}^N\mathrm{with}f\left(\mathbf{q}\right)\approx \mathbf{x}.$$

(17)

The robot accepts joint states that fulfill the condition

$$\mathrm{rank}\mathbf{J}\left(\mathbf{q}\right)=M$$

which means the desirable motion can be executed. If the above-named condition is violated and

$$\mathrm{rank}\mathbf{J}\left(\mathbf{q}\right)<M$$

then the manipulator is in a singular state. In a singular configuration, the IK may fail to produce an executable solution for $\mathbf{q}$. When sending position commands to the robot in real time, it is therefore necessary to check at each time stamp whether the motion is approaching a singularity. For this purpose, the manipulability measure

$$w_M\left(\mathbf{q}\right)=\sqrt{det\left(\mathbf{J}\left(\mathbf{q}\right){\mathbf{J}}^T\left(\mathbf{q}\right)\right)}$$

(18)

introduced by Yoshikawa [24], is used. A small value of $w_M$ indicates close proximity to a singularity. The objective is to prevent the robot from executing movements that approach such configurations.

In the following, a singularity avoidance strategy is proposed that avoids singularities and brings the TCP back to the reference trajectory afterwards. For this purpose, the manipulability measure $w_M$ must not fall below a threshold $w_T$. At each time stamp, the next target ${\mathbf{x}}_{\mathrm{next}}$ provided by the NPG is evaluated. If the manipulability measure is above the threshold $w_T$, no action is necessary, as the robot is not close to a singularity. If the manipulability measure is below the threshold, corrective action is required to ensure proper movement. Between the points of the discretized trajectory, the robot moves linearly toward the next target ${\mathbf{w}}_{j+1}$. To avoid a singularity, the direction of the motion must be adjusted. Since the location of the singularity is unknown, it is not clear in which direction the robot should move to avoid it. Therefore, a set of candidate commands ${\tilde{\mathbf{c}}}_0,\dots ,{\tilde{\mathbf{c}}}_{N_c-1}$, illustrated in Figure 5, is provided.

To maintain the Cartesian TCP velocity it is required that the distance between each candidate and ${\mathbf{x}}_{\mathrm{act}}$ must be the same as the distance between ${\mathbf{x}}_{\mathrm{act}}$ and ${\mathbf{x}}_{\mathrm{next}}$. To reach the next control point ${\mathbf{w}}_{j+1}$, it is also necessary that the candidates lie roughly in that direction. So a cone with its tip in ${\mathbf{x}}_{\mathrm{act}}$ and a height

$$h={\displaystyle \frac{v_{\mathrm{next}}}{f}}$$

is defined. The center of the cone should be aligned to the direction

$${\mathbf{d}}_0={\displaystyle \frac{{\mathbf{x}}_{\mathrm{act}}-{\mathbf{x}}_{\mathrm{next}}}{∥{\mathbf{x}}_{\mathrm{act}}-{\mathbf{x}}_{\mathrm{next}}∥}}$$

of the desired movement in order to reach the next waypoint. The cone’s alignment and its slant height, which corresponds to $\Delta s$, are fixed. The only adjustable parameter is the angle $\phi$, which defines the cone’s aperture and must be less than ${90}^{\circ }$. This angle specifies the allowable deviation from the original direction and must be chosen such that the candidate directions deviate sufficiently from ${\mathbf{x}}_{\mathrm{next}}$ to avoid singular configurations. Within the scope of this work, the cone angle was set empirically to ${60}^{\circ }$, as this value provided a good compromise between effective direction adaptation and maintaining proximity to the original reference trajectory. The aim is to obtain a function that returns candidate points located on the edge of the cone’s base. The radius and the height of the cone are defined with

$$r=\Delta s\sin \left(\phi \right)\mathrm{and}h=\Delta s\cos \left(\phi \right).$$

The center of the base of the cone is computed with

$${\mathbf{c}}^{\prime }={\mathbf{x}}_{\mathrm{act}}+{\mathbf{d}}_{0}h.$$

Points on the edge of the cone’s base are described by the function

$$\tilde{\mathbf{c}}\left(\phi \right)={\mathbf{c}}^{\prime }+r\left(\cos \left(\phi \right){\widehat{\mathbf{g}}}_1+\sin \left(\phi \right){\widehat{\mathbf{g}}}_2\right)$$

(19)

where ${\widehat{\mathbf{g}}}_1$ and ${\widehat{\mathbf{g}}}_2$ are the orthonormal basis vectors spanning the plane in which the candidate points lie. The first basis vector is defined as

$${\mathbf{g}}_1={\mathbf{d}}_0\times {\mathbf{u}}_1,$$

where ${\mathbf{u}}_1$ is the first unit vector. If ${\mathbf{d}}_0$ is aligned with ${\mathbf{u}}_1$, the cross product yields the null vector. To avoid this case, the norm $∥{\mathbf{g}}_1∥$ is checked; if it is less than ${10}^{-6}$, the second unit vector ${\mathbf{u}}_2$ is used instead. The second basis vector ${\mathbf{g}}_2$ is orthogonal to both ${\mathbf{d}}_0$ and ${\mathbf{g}}_1$ and is computed as

$${\mathbf{g}}_2={\mathbf{d}}_0\times {\mathbf{g}}_1.$$

Finally, to obtain the orthonormal basis required for Equation (19), ${\mathbf{g}}_1$ and ${\mathbf{g}}_2$ are normalized to ${\widehat{\mathbf{g}}}_1$ and ${\widehat{\mathbf{g}}}_2$, respectively. With that equation the candidates

$$\mathcal{C}=\{{\tilde{\mathbf{c}}}_0,\dots ,{\tilde{\mathbf{c}}}_{N_c-1}\}\mathrm{with}{\tilde{\mathbf{c}}}_i=\tilde{\mathbf{c}}\left({\phi }_i\right)\mathrm{and}{\phi }_i={\displaystyle \frac{2\pi i}{N_c}}\mathrm{with}i=0,\dots ,N_c-1$$

(20)

can be obtained. Those candidates are equally spaced on the edge of the cone, which is aligned to the direction ${\mathbf{d}}_0$. Because the slant length of the cone is chosen with $\Delta s$, it is also guaranteed that the velocity when moving to one candidate is the same as when moving to ${\mathbf{x}}_{\mathrm{next}}$.

Now, the candidate with the best manipulability is searched. Therefore the fixed orientation ${\mathbf{x}}_o$ is added to the position candidates to obtain a vector ${\mathbf{x}}_i$ that describes the location of the robot as shown in Equation (16). Those candidate locations can be transformed with the IK (Equation (17)) into an amount of candidate joint states

$$\mathcal{Q}=\left\{{\mathbf{q}}_{\mathbf{0}},\dots ,{\mathbf{q}}_{N_c-1}\right\}=\left\{\mathrm{IK}\left({\mathbf{x}}_i\right)|{\mathbf{x}}_i=\left(\begin{array}{c}{\tilde{\mathbf{c}}}_i\\ {\mathbf{x}}_o\end{array}\right),{\tilde{\mathbf{c}}}_i\in \mathcal{C}\right\}.$$

(21)

Afterward, the manipulability measure from Equation (18) is evaluated for all candidate configurations, and the best one is selected according to

$${\mathbf{q}}_{\mathrm{next}}=arg\underset{{\mathbf{q}}_{\mathbf{i}}\in \mathcal{Q}}{max}{w}_M\left({\mathbf{q}}_i\right).$$

(22)

This configuration can then be used as the next joint state command, steering the robot away from a singularity while maintaining the desired Cartesian TCP velocity.

When applying this strategy at every time stamp, the robot’s trajectory is repelled from singular configurations. However, to both avoid singularities and track the given trajectory as closely as possible, the strategy requires refinement. Consider two neighboring control points, ${\mathbf{w}}_j$ and ${\mathbf{w}}_{j+1}$, that are far apart, with a singularity lying between them. In this case, the robot would move away from the singularity until it could resume moving directly toward ${\mathbf{w}}_{j+1}$. In practice, the distance between neighboring control points is small, since $W_v$ is intended to approximate a spline smoothly. If a singularity is close to ${\mathbf{w}}_{j+1}$, the robot may be repelled from the target and the TCP ends up circling around it. To address this issue, the strategy is adapted as illustrated in Figure 6. For simplicity, the sketch is shown in two dimensions, though the same principle applies in three dimensions. Around each control point ${\mathbf{w}}_j$, an initial blending radius $b_{r_0}$ of $2\mathrm{cm}$ is defined. If the robot’s TCP enters the sphere of this radius, ${\mathbf{w}}_{j+1}$ is considered reached, and the next control point becomes the target. The modification is to increase the blending radius around ${\mathbf{w}}_{j+1}$ whenever ${\mathbf{x}}_{\mathrm{next}}$ is adapted by the avoidance strategy. This ensures that the current goal is eventually considered as reached. Once the next control point is targeted, the blending radius is reset to its initial value and then increased again if further adaptations to ${\mathbf{x}}_{\mathrm{next}}$ are required.

An example of the strategy is shown in Figure 6. Each sketch indicates the current position ${\mathbf{x}}_{\mathrm{act}}$, the next two control points ${\mathbf{w}}_{j+1}$ and ${\mathbf{w}}_{j+2}$, the next position command ${\mathbf{x}}_{\mathrm{next}}$ generated by the NPG, possible candidate points, and the blending radius. In Figure 6a–d, ${\mathbf{x}}_{\mathrm{next}}$ (marked in red) lies in the direction of the target but has low manipulability. Two example candidates (marked in blue) are considered, and the one with the highest manipulability measure is selected. With each subsequent sketch, the blending radius increases, and in Figure 6d, the selected next command lies within this radius. At this point, ${\mathbf{w}}_{j+1}$ is considered reached, j is incremented, and the next control point becomes the target. In the final sketch, Figure 6e, no adaptation of ${\mathbf{x}}_{\mathrm{next}}$ is necessary, as it already has good manipulability.

Algorithm 1 summarizes the avoidance strategy and is explained in the following. This algorithm describes the behavior of the modified NPG for singularity avoidance and is evaluated at every time stamp. Its inputs are the current discretized trajectory $W_v$, the index j of the last reached control point, the constant initial blending radius $b_{r_0}$, the increment l by which the blending radius is increased whenever an adaptation of ${\mathbf{x}}_{\mathrm{next}}$ becomes necessary, and h the number of blending radius increments. In the scope of this work the increment is chosen with $1\mathrm{mm}$, which means that $b_r$ increases by 2 cm per second when considering the constant control frequency of $20\mathrm{Hz}$ and ongoing violation of the manipulability threshold $w_T$. The output is the next joint command ${\mathbf{q}}_{\mathrm{next}}$. The integer h is initialized with zero at the begin of the movement, increased each time ${\mathbf{x}}_{\mathrm{next}}$ is adapted, and reset whenever a new target is set. First, ${\mathbf{x}}_{\mathrm{next}}$ is obtained from the NPG and transformed into a joint state command. If ${\mathbf{x}}_{\mathrm{next}}$ lies within the blending radius of the target ${\mathbf{w}}_{j+1}$, the index j is incremented to set the next target. In that case, ${\mathbf{x}}_{\mathrm{next}}$ is computed again, now pointing toward the updated target. If ${\mathbf{x}}_{\mathrm{next}}$ is not inside the blending radius, the manipulability measure of ${\mathbf{q}}_{\mathrm{next}}$ is evaluated. If it is below the threshold, movement candidates are generated, and the one with the highest manipulability measure is selected as ${\mathbf{q}}_{\mathrm{next}}$. In this case, the integer h, which controls the growth of the blending radius, is also increased.

Algorithm 1 Singularity Avoidance

Require: $W_v$, j, $b_{r_0}$, l, h Init:  $d=0$   1: while  $d<b_r$  do   2:       Obtain ${\mathbf{x}}_{\mathrm{next}}$ with Equations (8) and (12)   3:       Obtain ${\mathbf{q}}_{\mathrm{next}}$ with Equation (17)   4:       Adjust blending radius with $b_r=b_{r_0}+hl$   5:       compute the distance to the next control point with $d=∥{\mathbf{x}}_{\mathrm{act}}-{\mathbf{w}}_{j+1}∥$   6:       if $d<b_r$ then   7:            Increase the target $j=j+1$   8:            Reset blending radius $h=0$   9:       else 10:            if $w_M\left({\mathbf{q}}_{\mathrm{next}}\right)<w_T$ then 11:                 Find candidates $\mathcal{C}$ with Equation (20) 12:                 Obtain candidate joint States $\mathcal{Q}$ with Equation (21) 13:                 Choose ${\mathbf{q}}_{\mathrm{next}}$ with Equation (22) 14:                 Increase blending radius for the next time stamp $h=h+1$       return Next joint command ${\mathbf{q}}_{\mathrm{next}}$

2.5. Compliance to Joint Velocity and Acceleration Limits

In the previous section, the robot’s joint velocity and acceleration limits were neglected. Therefore, assumption 5 from Section 2.2 is modified so that joint velocity and acceleration constraints must be respected when sending the next position command. Since the reference trajectory is defined in CS, it is not directly possible to determine the required joint velocities for accurate tracking. In the previously described control approach, the joint states obtained from IK would be transmitted to the robot without verifying compliance with its physical velocity and acceleration limits. The following section introduces a method that ensures adherence to these limits by modulating the robot’s motion speed, thereby maintaining compliance while preserving the reference path. It is assumed that the current joint states ${\mathbf{q}}_{\mathrm{act}}$ are known, and that the next joint command ${\mathbf{q}}_{\mathrm{next}}$ has already been adapted by the singularity avoidance strategy described in the previous section. For a discrete function with closely spaced values, the mean difference quotient can be used to estimate the derivative at a given point. Due to the high control frequency, and the resulting small distances between successive configurations in CS, as well as the assumption that ${\mathbf{q}}_{\mathrm{next}}$ remains sufficiently far from singular configurations, the difference between consecutive joint commands is expected to be small. This approximation introduces a discretization error that is proportional to the sampling interval and is therefore negligible for the control frequency used in this work. The computation is performed individually for each joint. With

$${\dot{q}}_i\left(t_{j+1}\right)={\displaystyle \frac{q_i\left(t_{j+2}\right)-q_i\left(t_j\right)}{2\Delta t}}\mathrm{with}i=1,\dots ,n$$

(23)

and

$${\ddot{q}}_i\left(t_{j+1}\right)={\displaystyle \frac{q_i\left(t_{j+2}\right)-2q_i(t_{j+1})+q_i\left(t_j\right)}{\Delta t^2}}\mathrm{with}i=1,\dots ,n$$

(24)

the value of the derivatives at $t_{j+1}$ can be estimated. The time difference $\Delta t$ between two consecutive joint commands corresponds to the inverse of the control frequency f. Let $\mathbf{q}\left(t_j\right)={\mathbf{q}}_{\mathrm{act}}$ denote the previously executed joint command, and let $\mathbf{q}\left(t_{j+2}\right)={\mathbf{q}}_{\mathrm{next}+1}$ denote the joint command that follows after ${\mathbf{q}}_{\mathrm{next}}$ which is generated by the NPG using ${\mathbf{q}}_{\mathrm{next}}$ as the start value.

For the analysis of the velocity and acceleration limits, it is assumed that those values increase or decrease steadily between two consecutive joint commands. With Equations (23) and (24) the joint velocity ${\dot{\mathbf{q}}}_{\mathrm{next}}$ and acceleration ${\ddot{\mathbf{q}}}_{\mathrm{next}}$ are estimated. Next, the relative violations of the joint velocity and acceleration are evaluated. For this purpose, the relative quotients of velocity and acceleration with respect to their maximum values are defined for each joint as

$$q_{v,\mathrm{rel},i}={\displaystyle \frac{{\dot{q}}_i\left(t_{j+1}\right)}{{\overline{q}}_{v,i}}}\mathrm{and}{q}_{a,\mathrm{rel},i}={\displaystyle \frac{{\ddot{q}}_i\left(t_{j+1}\right)}{{\overline{q}}_{a,i}}}\mathrm{with}i=1,\dots ,N.$$

These quotients are composed of the desired velocity and acceleration divided by the maximum value for the corresponding joint. If a quotient is greater than one, the desired velocity or acceleration exceeds the limit and must be capped. If every quotient is less than one, the velocity and acceleration required for ${\mathbf{q}}_{\mathrm{next}}$ is within the robot’s limits and can be realized. The maximum relative deviation of velocity and acceleration

$${\beta }_{\max ,v}=\max \left({\mathbf{q}}_{v,\mathrm{rel}}\right)\mathrm{and}{\beta }_{\max ,a}=\max \left({\mathbf{q}}_{a,\mathrm{rel}}\right)$$

is extracted, which is then used to cap the velocity or acceleration at the maximum value given by the limitations of the robot. The idea is to fix the velocity or acceleration of the joint with the largest relative deviation to the maximum velocity or acceleration and adjust the velocities of the other joints accordingly so that the path is not changed. Therefore

$${\mathbf{q}}_{\mathrm{next}}^{\ast }={\displaystyle \frac{1}{\max ({\beta }_{\max ,v},{\beta }_{\max ,a})}}\left({\mathbf{q}}_{\mathrm{next}}-{\mathbf{q}}_{\mathrm{act}}\right)+{\mathbf{q}}_{\mathrm{act}}$$

is adapted in a way so that ${\mathbf{q}}_{\mathrm{next}}^{\ast }$ is closer to ${\mathbf{q}}_{\mathrm{act}}$ than the initial ${\mathbf{q}}_{\mathrm{next}}^{\prime }$. Summarized this method makes it possible that the TCP of the robot at least tracks the path of a trajectory planned in CS with reduced speed.

2.6. Summary of Dynamic Robot Motion Planner

The dynamic motion planner is illustrated in greater detail in Figure 7. This sketch summarizes the concepts introduced in Section 2.2, Section 2.3, Section 2.4 and Section 2.5. Given a reference trajectory defined by points with associated time stamps, the output of the motion planner is the next joint command, which can be sent either to the real robot or to the simulation. The motion planner is able to track a trajectory that changes dynamically, with the following properties:

1.

The path lies within the workspace of the robot, and every point is reachable

2.

The path can dynamically change when the robot moves

3.

The TCP velocity can change dynamically

4.

There could be singularities on the path, which need to be avoided

5.

The joint velocity and acceleration are limited

The dynamic motion planner (Figure 7) is responsible for sending joint commands to the robot with the required motion properties. It consists of the following modules: the Discretizer, the Check Waypoint module, the Target Generator, the NPG, and the subsequent safety checks (Singularity Check and Velocity Cap). The Discretizer converts the incoming time-stamped trajectory into a sequence of equidistant points in CS on the reference trajectory. The Check Waypoint module checks whether the next control point has been reached. Then, the Target Generator selects the active reference control point and provides the current direction of motion. The NPG generates the next Cartesian command along this reference, which is subsequently transformed into joint space. Finally, the Singularity Check and Velocity Cap modules modify the resulting joint command if necessary before it is sent to the robot.

In the Discretizer, the time-parameterized input sequence $P_v$ is approximated using a spline representation and reparameterized from uniform sampling in time to approximately uniform sampling in CS. The resulting output is a sequence of equidistant points $W_v$ in CS with corresponding velocities.

In the Check Waypoint module the motion planner checks whether the next control point has been reached. The robot’s current position ${\mathbf{x}}_{\mathrm{act}}$ is obtained by applying forward kinematics to the current joint configuration ${\mathbf{q}}_{\mathrm{act}}$. This position serves as the basis for selecting the next waypoint ${\mathbf{x}}_{\mathrm{next}}$ on the trajectory. The Cartesian velocity is implicitly determined by the distance between ${\mathbf{x}}_{\mathrm{act}}$ and ${\mathbf{x}}_{\mathrm{next}}$. A goal is considered reached if the TCP lies within its blending radius $b_r$. The index variable j tracks the last reached control point and is incremented once the goal is reached.

In the Target Generator, the next control point is set as next goal. Therefore, it first checks if the current sequence of equidistant control points $W_v$ differs from the previously used reference $W_{v,-1}$. If unchanged, the next control point is simply ${\mathbf{w}}_{j+1}$. If the trajectory has changed, a control point that lies in the direction of the known endpoint of the movement is selected and j is set appropriately. This avoids using a control point, which could steer the TCP closer to the start point again. Given the current position ${\mathbf{x}}_{\mathrm{act}}$, the control frequency f, and the next target, the NPG computes the next Cartesian command ${\mathbf{x}}_{\mathrm{next}}$ along the trajectory. Combined with a fixed orientation ${\mathbf{x}}_o$, this point is transformed into a joint configuration ${\mathbf{q}}_{\mathrm{next}}$ via IK.

The Singularity Check modifies ${\mathbf{q}}_{\mathrm{next}}$ if necessary and ensures that ${\mathbf{q}}_{\mathrm{next}}^{\ast }$ does not steer the robot into a singularity. The variable $\phi$ governs the angle on which the avoidance path deviates from the straight line path to the next control point. In cases of adaptation, the blending radius $b_r$ is enlarged by an offset l to prevent circling around a goal. The variable h tracks how often this enlargement occurs and is reset once the next control point is reached. Since the trajectory is planned in CS, the NPG is unaware of joint velocity and acceleration constraints.

In the Velocity Cap block, these are checked based on ${\mathbf{q}}_{\mathrm{next}}^{\ast }$ and the subsequent configuration ${\mathbf{q}}_{\mathrm{next}+1}$ which the NPG also generates with ${\mathbf{x}}_{\mathrm{next}}$ assumed as current robot state. Using the control frequency f, the expected joint velocities and accelerations are estimated. If any joint exceeds its velocity or acceleration limits, the velocity of the fastest joint is capped, and the subsequent joint command ${\mathbf{q}}_{\mathrm{next}}^{\ast \ast }$ is adjusted accordingly by proportionally scaling the velocities of all other joints. This ensures that the TCP remains on the path of the trajectory, albeit at a reduced speed. Finally, the resulting joint command ${\mathbf{q}}_{\mathrm{next}}^{\ast \ast }$ is sent to the real or simulated robot.

2.7. System Architecture

For real-time control, the KUKA FRI-package is used, which enables hard real-time robot control at 200 Hz. The Quality of Service (QoS) specifications require that consecutive commands be provided precisely every 5 ms. To satisfy these requirements, the proposed motion planner must compute new joint commands in less than 5 ms to satisfy the QoS. While the planner meets this requirement in most cases, occasional violations occur due to two main factors. First, the communication delay between the planning hardware and the robot hardware is approximately 1 ms in the current setup. Second, the computation time of the inverse kinematics solution depends on the robot’s configuration. In regions of low manipulability, an ill-conditioned Jacobian matrix must be inverted, where the determinant is close to zero. This can significantly increase computation time and may exceed the available time budget, leading to violations of the QoS requirements and, in the worst case, interruptions in communication between the application and the controller. Therefore, computing joint positions strictly just-in-time is not sufficient, as the robot controller requires commands at a fixed update rate. To address this, a command buffer is introduced that stores joint commands computed in advance. This approach decouples command generation from communication, introducing a controlled delay to ensure a continuous and stable data stream to the robot controller. In summary, the proposed motion planner generates joint commands that track a dynamically changing reference in soft real-time, while the command buffer stores these commands and supplies them to the robot at a fixed control frequency. In the unlikely event that the motion planner fails to compute the next joint values within the required control cycle of the command buffer, the robot performs a controlled stop.

The buffer is designed following a publisher–subscriber architecture and operates in two phases. First, the motion planner is initialized with the current joint configuration ${\mathbf{q}}_0$. In this phase, the buffer requests the first two joint commands, ${\mathbf{q}}_1$ and ${\mathbf{q}}_2$, from the motion planner, forming two segments $({\mathbf{q}}_0,{\mathbf{q}}_1)$ and $({\mathbf{q}}_1,{\mathbf{q}}_2)$. Every segment is interpolated such that the joint velocity within a segment changes linearly, using a 1:10 interpolation ratio between the motion planner frequency (20 Hz) and the robot control frequency (200 Hz). The concept of the buffer is illustrated in Figure 8, where black dots denote the joint commands generated by the motion planner and blue strips represent the interpolated commands ${\mathbf{q}}_{\mathrm{int}}$ produced by the buffer; the initialization phase is shown on the left side of the figure, while the operation phase is shown on the right. Once the initial segments are computed and buffered, motion execution begins, marking the end of the initialization phase. During operation, whenever a joint command ${\mathbf{q}}_k$ is reached, the buffer triggers a request to the motion planner for the next point ${\mathbf{q}}_{k+2}$. At this point, the subsequent command ${\mathbf{q}}_{k+1}$ is already available in the buffer, and the interpolation between $({\mathbf{q}}_k,{\mathbf{q}}_{k+1})$ has already been performed. This lookahead provides sufficient time for the motion planner to compute ${\mathbf{q}}_{k+2}$ and for the buffer to interpolate the segment $({\mathbf{q}}_{k+1},{\mathbf{q}}_{k+2})$ before ${\mathbf{q}}_{k+1}$ is executed. If the trajectory is completed and no further commands are provided by the application, the buffer maintains the last control point as the active command to ensure continuous communication with the controller.

Compared to just-in-time computation, the proposed buffer structure ensures that the robot controller receives commands that satisfy the required QoS, at the expense of hard real-time trajectory tracking. Consequently, adaptations to changes in the reference are subject to a short delay and do not take effect in hard real-time. The interpolation rate is chosen conservatively but could be increased if more capable hardware is available. A larger interpolation rate of the motion planner would improve tracking performance and reduce the influence of the command buffer delay. For HRC, the soft real-time performance provided by this buffer is sufficient, as the resulting delays remain below typical human reaction times [40].

The overall system architecture for controlling the robot with the proposed motion planner is shown in Figure 9. It consists of four components: the real robot, which receives commands ${\mathbf{q}}_{\mathrm{int}}$ at a fixed control frequency; a trajectory generator, which provides references $P_{\mathrm{v}}$ based on the current joint configuration ${\mathbf{q}}_{\mathrm{act}}$; the robot motion planner, which computes the next joint command ${\mathbf{q}}_{k+1}$ for a given reference $P_{\mathrm{v}}$ and a start configuration ${\mathbf{q}}_k$; and the buffer, which supplies the joint commands ${\mathbf{q}}_{\mathrm{int}}$ to the robot at the required control frequency in a continuous manner.

3. Results

In the following, the proposed motion planner is evaluated using a KUKA iiwa in a laboratory environment. The robot is operated in impedance control mode with low stiffness parameters to allow deviations from the commanded position, as required in scenarios where a human and a robot jointly manipulate a rigid object during a pick-and-place task, which is common in HRC. To evaluate the motion planner’s performance in reference trajectory tracking, the robot is operated without physical coupling to a human. This is motivated by the limited reproducibility of human motion and the fact that accurate prediction of human movement would be required to obtain consistent reference trajectories, which is beyond the scope of this work.

Both dynamically changing and static reference trajectories are considered. Each reference describes a movement from a start pose to an end pose with fixed orientation. For the experiments presented in the following, the parameters introduced in Section 2 are fixed as follows. The initial blending radius is set to $b_{r_0}=2\mathrm{cm}$, the minimum velocity to $v_{\min }=0.0075{\displaystyle \frac{\mathrm{m}}{\mathrm{s}}}$, the damping factor of the minimum velocity to $\lambda =20$, and the blending radius increment to $l=1\mathrm{mm}$. The manipulability threshold $w_T$ is set to $0.04$. These parameter values were selected based on empirical tuning and the authors’ experience with the KUKA iiwa robot.

3.1. Track a Static Trajectory

The first scenario investigates a robot replicating a recorded human trajectory, specifically moving a rod from location A to location B. This reference movement is presented componentwise in terms of position and velocity in Figure 10a,b. The tracking performance of the proposed motion planner is evaluated by comparing the reference with the actual motion. The position and velocity deviations are shown in Figure 10c,d. Throughout the entire movement, the deviation in any component from the reference trajectory does not exceed 2 cm, indicating that the robot can reproduce the trajectory with an accuracy of a few centimeters. The deviation in velocity does not exceed 4 cm per second, which is an order of magnitude smaller than the reference velocity along the path. The largest deviations in both position and velocity occur near the end of the motion, which is caused by the selected blending radius, since the robot terminates its motion upon entering the blending radius around the target. An additional source of error during the motion arises from the low stiffness parameters that are used. When position commands are executed under low stiffness, they are not tracked exactly due to the influence of gravitational and inertial forces.

In the context of HRC, small positional deviations can be regarded as acceptable, since humans are generally not consciously aware of minor trajectory variations as long as the goal is achieved [3]. Therefore, the largest deviation can be considered as relevant metric for evaluating HRC when human and robot are physically coupled. However, if the deviation grows larger, the robot exerts a guiding influence by forcing the human onto a new path. The greater the deviation, the stronger this guiding force becomes, and the more likely the human is to perceive a change in their trajectory. In such cases, the robot is considered to be dominant.

3.2. Track a Dynamically Changing Trajectory

In this scenario, the TCP of the manipulator is required to track a dynamically changing reference trajectory. Therefore, two locations are connected by a sinusoidal path that is parameterized in time as

$$\begin{array}{c}\hfill \tau ={\displaystyle \frac{t}{T}},\end{array}$$

where T denotes the movement duration. The parameterized sinusoidal curve is defined as

$${\mathbf{s}}_{\sin }\left(\tau \right)={\mathbf{x}}_a+\tau ({\mathbf{x}}_e-{\mathbf{x}}_a)+\mathbf{n}A\left(t\right)\sin \left(\pi \tau \right)\sin \left(\pi f\tau \right)\mathrm{with}\tau \in [0,1],$$

where ${\mathbf{x}}_a$ is the start location, ${\mathbf{x}}_e$ the goal location, $\mathbf{n}$ the normalized orientation vector of the sinusoidal curve, $A\left(t\right)$ the time dependent amplitude and f the frequency of the curve which determines the number of peaks. The first two terms describe the linear interpolation between the start and the goal location. The last term introduces the sinusoidal modulation, where the amplitude is varied dynamically to ensure that the reference changes at every time stamp. The factor $sin\left(\pi \tau \right)$ guarantees that the curve starts and ends at the desired locations. This path is further parameterized with linear increasing and decreasing velocity

$$v_{\sin }\left(\tau \right)=\left\{\begin{array}{cc}\tau {\displaystyle \frac{2L}{T}}\hfill & \mathrm{for}0\le \tau \le {\displaystyle \frac{1}{2}}\hfill \\ (1-\tau ){\displaystyle \frac{2L}{T}}\hfill & \mathrm{for}{\displaystyle \frac{1}{2}}\le \tau \le {\displaystyle \frac{1}{2}}\hfill \end{array}\right.$$

(25)

where L is the total length of the sinus curve in this moment.

The sinusoidal trajectory and the associated velocity profile are used as input for the motion planner. The curve is rotated so that the reference lies in the xy-plane. The total movement duration T is set to ten seconds, with an amplitude ranging from 5 cm to 15 cm. The reference position profile is shown in Figure 11a and the reference velocity profile in Figure 11b, where the maximum velocity reaches approximately 0.13 m per second. It is important to note that these profiles represent the values at each discrete time stamp, since the reference changes dynamically. Figure 11c shows the difference between the reference and actual positions. The maximum deviation is about 15 mm in the x-direction, while it is below 5 mm in y- and z-direction. Although there is no movement in z-direction, there is a slight error in position. This is caused by the low stiffness of the impedance control described in Section 3.1. There is also an error summed up in the other components because of the inertial forces. As before, the blending radius explains the deviation observed at the end of the movement.

The difference in total Cartesian velocity is presented in Figure 11d. Overall, those deviations are small compared to the reference velocity, but three distinct areas require interpretation. Negative deviations indicate the robot is moving faster than the reference, while positive deviations indicate it is slower. At the start of the motion, the actual velocity is slightly higher due to Equation (13), since the robot begins with a minimum velocity. The area in the middle occurs because the reference velocity exceeds the robot’s velocity. This results from the switch from acceleration to deceleration combined with the blending radius. Before the maximum velocity is reached, the next goal and its corresponding velocity are already set, causing the robot to move slightly slower. The final peak has the same origin: once the goal lies within the blending radius, the robot stops, leading to a positive velocity deviation.

Overall, the results demonstrate that such a dynamically changing reference trajectory can be tracked with position deviations of only a few millimeters. For higher velocities, the deviation in velocity remains an order of magnitude smaller than the actual velocity. Noticeable relative deviations only occur at the beginning and at the end of the movement, which can be explained by the minimum velocity constraint and the applied blending radius.

When comparing the results of this section with those of the previous sections, it can be seen that the deviations are in similar magnitudes. For the motion planner, it makes no difference whether the reference trajectory is initially known or dynamically changing.

3.3. Track a Trajectory with Singularity Avoidance

In the following, the singularity avoidance strategy introduced in Section 2.4 is evaluated. The motion planner is tested with a static reference trajectory, whose path is shown by the blue line in Figure 12. The reference TCP velocity is defined as in Equation (25) with a total trajectory duration of ten seconds. As demonstrated in the previous section, the total velocity can be tracked with comparable accuracy. The plot of deviations is omitted here, since it is evident that the individual position and velocity components over time cannot be tracked exactly due to the detour of the actual path around the singularity. The dashed red line illustrates the actual path. In the middle of the motion, a singularity is avoided, and once the manipulability has increased sufficiently, the path converges back to the reference. The avoidance is smooth in CS, and after bypassing the singularity, the TCP returns to the reference along a direct path.

3.4. Track a Trajectory with Joint Velocity and Acceleration Limits

In the following, the velocity limitation described in Section 2.5 is evaluated. This is exemplarily demonstrated for a linear movement in the negative x-direction, with a total length of 0.6 m. The reference velocity along this path is shown with the blue line in Figure 13b. For safety reasons, the joint velocity limit was conservatively set to $0.2$ radians per second for this experiment. It can be observed that the Cartesian reference velocity, cannot be fully matched by the actual velocity, shown as the dotted line.

Initially, the Cartesian velocity is accurately tracked by the robot. However, after approximately three seconds, joint $j_7$ reaches its velocity limit, as shown in Figure 13c, preventing any further increase in Cartesian velocity. Figure 13a shows the corresponding deviation in Cartesian position, which increases until the reference trajectory reaches the goal and subsequently decreases until the TCP is within the blending radius of the goal. It can be seen that the joint velocity constraint is satisfied at all times.

4. Discussion

In this work, a motion planner was developed and evaluated in an laboratory setup that enables a cobot to track a dynamically changing reference trajectory in real time while adhering to joint velocity limitations and avoiding singularities that may lie within the path of the robot. Now the research questions from Section 1.3 are discussed.

The first question, how a robot can track a human trajectory, is addressed by the proposed motion planner. Human motions are highly individual and can change dynamically, requiring the robot to adapt its trajectory in real time. As shown in Section 3.1, a recorded human trajectory can be accurately reproduced by the proposed planner. Moreover, dynamically changing trajectories can be tracked without significant deviations in position or velocity, as demonstrated in Section 3.2. The motion planner allows the robot to track a given Cartesian reference under the condition that the robot maintains sufficient manipulability along the trajectory. Since the input trajectory is evaluated at every time stamp, the planner supports highly adaptable movements under the assumption that they are physically possible. Minor deviations between the reference and the actual trajectory remain. Industrial robots generally achieve very high precision when tracking predefined trajectories. Considering that the reference can change at every time stamp, the proposed motion planner achieves the goal of tracking a changing trajectory sufficiently.

The second question, what tracking performance can be achieved when following a human reference trajectory, can be addressed as follows. As long as the manipulability remains sufficient and the human trajectory lies within the workspace of the robot, the trajectory can be tracked with good accuracy (e.g., within a few centimeters). For the proposed motion planner, it makes no difference whether the reference is generated in advance or changes online. This property makes the planning scheme highly adaptable to fast changing references, as they can be expected in HRC. If a singularity occurs on the robot’s path or joint velocity and acceleration limits are violated, the robot is unable to track the given trajectory. In such cases, the motion planner attempts to track the trajectory as closely as possible. In the case of a violation of velocities, the movement slows down while still maintaining the path. In the case of singularities, it avoids them and moves back to the path in a direct way. Future investigations will focus on determining how the robot should return to a given reference, either in a direct, time-optimized manner or in a smooth manner, and how the prediction of human movement can be incorporated into the motion. This also raises the question of leadership in HRC, specifically when the robot should be dominant, when the human should be dominant, and how the transition of leadership should be managed.

For the second question, a few restrictions for the reference have to be made. At first, the trajectory tracking works for smooth or smoothly changing trajectories. To this end, the proposed planning unit will be evaluated in a scenario where the robot is physically coupled to a human co-worker via a rigid object. Such scenarios necessitate the prediction of human motion to ensure smooth robotic movements in collaboration tasks. Future work aims to couple this motion planner with a proper human reference generator to evaluate how the robot reacts to sudden changes in motion.

Regarding the third question, namely who is dominating the movement and when, a first insight can be given. When the deviations in position and velocity between the dynamically changing reference and the actual robot trajectory remain small, no joint velocity or acceleration limits are violated and no singularities are encountered, the human can be regarded as dominant. However, when the robot is unable to track the reference due to these constraints and instead avoids singularities or limits its velocity, the human is guided onto a different trajectory. In such cases, the robot assumes a dominant role. Future work will analyze the behavior and perception of human collaborators interacting with a robot controlled by the proposed motion planner.

Furthermore, future work will investigate the behavior of the proposed motion planner in collaborative pick-and-place scenarios with human participants, including the integration of human motion prediction. In addition, the transitions in trajectory leadership will be examined. In situations where the robot cannot follow the human motion, it must assume dominance. However, since the robot is intended to support its coworker, it is desirable that the human regains dominance as soon as possible. With the proposed dynamic motion planner, the robot returns to the reference along the shortest possible path. The objective of future work is to optimize this transition so that it becomes smooth and natural.

5. Conclusions

This work has presented a dynamic motion planner that enables collaborative robots to track dynamically changing reference trajectories, as required in HRC. An evaluation using a KUKA iiwa operated in impedance control mode confirms that the motion planner can generate appropriate position commands for accurate trajectory tracking. Furthermore, the strategies for singularity avoidance and velocity limitation proved effective, with the robot becoming dominant in critical situations. Future work will focus on investigating how transitions between human and robot dominance can be realized smoothly and naturally, as well as testing the proposed motion planner in interaction with human co-workers.