An NMPC-ECBF Framework for Dynamic Motion Planning and Execution in Vision-Based Human–Robot Collaboration

Abstract

To enable safe and effective human–robot collaboration (HRC) in smart manufacturing, it is critical to seamlessly integrate sensing, cognition, and prediction into the robot controller for real-time awareness, response, and communication inside a heterogeneous environment (robots, humans, and equipment). The proposed approach takes advantage of the prediction capabilities of nonlinear model predictive control (NMPC) to execute safe path planning based on feedback from a vision system. To satisfy the requirements of real-time path planning, an embedded solver based on a penalty method is applied. However, due to tight sampling times, NMPC solutions are approximate; therefore, the safety of the system cannot be guaranteed. To address this, we formulate a novel safety-critical paradigm that uses an exponential control barrier function (ECBF) as a safety filter. Several common human–robot assembly subtasks have been integrated into a real-life HRC assembly task to validate the performance of the proposed controller and to investigate whether integrating human pose prediction can help with safe and efficient collaboration. The robot uses OptiTrack cameras for perception and dynamically generates collision-free trajectories to the predicted target interactive position. Results for a number of different configurations confirm the efficiency of the proposed motion planning and execution framework, with a 23.2% reduction in execution time achieved for the HRC task compared to an implementation without human motion prediction.

1. Introduction

With advancements in motion prediction and embedded path planning technologies, accurate and online human–robot collaboration (HRC) has been rapidly developed in smart manufacturing [1,2]. The learning space for executing HRC tasks with flexible joint manipulators is vast, characterized by nonlinear, time-varying, and highly complex dynamics. Ensuring safe and smooth HRC remains an open challenge, particularly for autonomous systems operating in spaces shared with humans, such as intra-logistics and service robotics [3,4]. To address these challenges, robots must be equipped with the ability to comprehend the ongoing task, precisely detect human positions, and anticipate future actions based on human demonstrations [5].

Vision-based human action recognition and motion prediction are crucial problems in an HRC system [6,7]. Action recognition aims to classify the categories of a human’s current dynamics. To activate a robot at the proper time, the estimated classification category can be used as a prior condition for motion prediction. Motion prediction is concerned with forecasting future body movements and poses based on observations of past movements and can be used to improve the collaboration efficiency in HRC [8]. Estimates of the future evolution of body poses can be used to define both the target position for the robot end-effector and the dynamic obstacles (body parts) for collision avoidance. In our work, an OptiTrack camera is used to achieve high-performance optical tracking combined with wearable devices to obtain a streaming skeletal representation of the human body. These data are then used as input to the task recognition and motion prediction modules.

Safety regulation ISO 10218 [9] and technical specification ISO 15066 [10] must be adhered to if a robot is deployed in a working environment shared with a human. These define cooperative robot operation as either contact-type (e.g., power and force limiting (PFL), hand guiding (HG)) or non-contact-type (e.g., safety-rated monitored stop (SRMS), speed and separation monitoring (SSM)) interactions. In this work, we consider the design of a control framework for SSM-based non-contact HRC, which guarantees safety by remaining a safe distance from humans during interactions.

A model predictive control (MPC)-based control system is employed to ensure compliance with safety standards while processing real-time feedback from a vision system. Given the dynamic and stochastic nature of HRC environments, which cannot be fully predicted a priori, MPC has gained increasing popularity in motion planning due to its ability to handle diverse kinematic and dynamic constraints [11,12,13]. The nonlinear model predictive control (NMPC) law is derived by solving an online nonlinear optimal control problem, incorporating a kinematic model. Previous work, such as [14], demonstrates the application of NMPC for task-constrained motion planning. For safe physical human–robot interaction (pHRI), ref. [15] further develops an online NMPC approach using a kinematic model with vision-based feedback.

NMPC can guarantee asymptotic stability and collision avoidance in most cases [16]. However, infeasible solutions might be obtained in some cases, especially within the stringent sampling time requirements encountered in HRC (typically a few milliseconds). This presents challenges for the real-time computation of NMPC, as there is the potential for collisions if an inaccurate solution is computed within the available time [17]. To overcome this limitation of NMPC, it is necessary to calculate the minimum distance between the end-effector and a human’s interactive body component more accurately and to add a safety filter to constrain the robot’s behavior within a safe area. To solve this problem, we use the Gilbert–Johnson–Keerth (GJK) algorithm [18] to solve the collision detection problem and add a control barrier function (CBF)-based constraint into the NMPC problem to guarantee the safety of the operator. In [19], a CBF-based approach is presented to constrain a redundant manipulator within a safe working area when interacting with a human operator. High-order CBF (HOCBF) and exponential CBF (ECBF) extensions are proposed in [20,21] to solve higher-order relative degree problems. In this paper, we combine a vision system with NMPC-ECBF to solve the safe pHRI problem online.

In summary, the existing research gaps are as follows. First, there is no established approach for designing a safe controller that uses the information from human motion prediction and action recognition. Second, collaboration under a tight sampling regime limits the accuracy of the NMPC solver, leading to errors that may present a risk to the human operators in the shared workspace.

With estimates of the future target and obstacle position in the working environment, we propose a real-time motion planning algorithm for the physical human-robot interaction (PHRI) task based on NMPC-ECBF. In this work, to test the performance of the proposed controller, we set up several HRC scenarios with a seven-degrees-of-freedom (DOFs) manipulator (a Baxter robot) whereby motion capture, action recognition, motion prediction, and trajectory planning modules are integrated into a single system. The screwdriver usage scenario in the manufacturing assembly task includes subtasks such as picking up, moving forward, picking up a screw, operating, and moving backward. Each subtask is divided into a sequence of subtasks, with the robot end-effector required to reach a continuously updated predicted interactive position without collision when a subtask is triggered. However, as human motion and pose predictions are subject to approximation errors, the risk of collision still exists in HRC. Therefore, the output configuration of a manipulator is followed by a safety-critical controller implemented using an ECBF-based approach.

There have been several works [22] that consider integrating motion prediction into HRC tasks, but no corresponding safe controllers have been proposed to guarantee the safety of human operators. In this work, we focus on the NMPC-ECBF system to enable safe and real-time human–robot collaboration (HRC). The controller integrates a safety filter to enforce ISO safety regulations, with experimental validation demonstrating its performance in dynamic environments. This paper proposes a novel NMPC-ECBF framework for ensuring safety in real-time human-robot collaboration (HRC) scenarios. The developed controller integrates a safety-critical filter to guarantee strict compliance with ISO safety standards. Experimental results demonstrate the system’s effectiveness in maintaining robust performance within dynamic operational environments. Furthermore, the implemented vision system provides crucial human action recognition and trajectory prediction capabilities, thereby validating the practical feasibility of incorporating motion prediction techniques in HRC applications. The main contributions of the paper are as follows.

(1) We integrate a safety filter based on ECBF into the predictive control system to constrain the movement of the end-effector, which guarantees human safety during collaboration. Additionally, a human motion prediction module is adopted to reduce the idle time for agents in HRC compared to the turn-taking collaboration.

(2) Since safe HRC requires an accurate estimate of the minimum distance between agents to ensure safe distance, a point-cloud-based perception system is proposed using a skeleton-aware mesh recovery system to capture human boundary information.

(3) We develop several real-life human–robot collaboration manufacturing scenarios to test the safety and efficiency of the proposed human motion prediction enhanced HRC control methodology.

This paper is organized as follows: Section 2 describes the overall HRC system. Related work is introduced in Section 3. The formulations and derivations of the target problem are presented in Section 4. Section 5 discusses the methodology for solving the problem, consisting of formulations for the system and obstacle detection, and introduces the NMPC algorithm and the solver used in the optimization problem. The controller design is introduced in Section 6. Then, Section 7 describes the experimental setup and presents the results of the simulations conducted to evaluate the performance of the proposed NMPC-ECBF-based control solution for human–robot collaboration. Finally, Section 8 provides the conclusions.

Notation: The following notation is used throughout the paper. Let $\mathbb{R}$, ${\mathbb{R}}_{+}$, ${\mathbb{R}}^n$, ${\mathbb{R}}^{m\times n}$ and $\mathbb{N}$ denote the sets of real numbers, the sets of positive real numbers, the set of real vectors of length n, the set of m by n real matrices, and non-negative integers, respectively. Let x, $\mathit{x}$, and $\mathit{X}$ denote scalar, vector and matrix quantities, respectively, with $\mathcal{X}$ a set, and X a constant. For any non-negative integers $k_1<k_2$, the finite set $\{k_1,\dots ,k_2\}$ is denoted by ${\mathbb{N}}_{[k_1,k_2]}$. For $\mathit{x}\in {\mathbb{R}}^n$, ${\left[\mathit{x}\right]}_{+}=max\{0,\mathit{x}\}$ is defined element wise. Given a set C, $InfC$ gives the maximum lower bound of the set C, while $supC$ gives the least upper bound of the set C.

2. Overall System Design

The overall design of the collision-free HRC system is shown in Figure 1. The proposed HRC system comprises motion capture, action recognition, motion prediction, decision-making, path planning, and robot controller modules. The motion capture module relies on OptiTrack cameras to generate high-quality 3D streaming skeleton pose estimates at a sampling rate of 20 frames per second. Using recorded spatial-temporal data for a human performing task-related motions/activities as training data, a motion prediction module is pretrained to predict the future one-second evolution of human poses (20 frames) based on the motion observed over the previous one-second interval. The decision-making module is implemented using NMPC-ECBF-based motion planning and robot execution mechanisms. In our work, a robot starts from a random position in the working environment and continuously searches for a collision-free path to the estimated interaction coordinates based on the received one-second ahead pose estimation feedback provided by the predictor module.

3. Related Works

3.1. Methods to Solve Trajectory Optimization in MPC

When the system is linear, the constraints are affine and the cost functions are quadratic, the associated MPC optimization problem is a quadratic program. Several convex optimization algorithms have been proposed [23,24] that are fast, robust, and possess global convergence guarantees. In contrast to the first-order approaches mentioned above, Newton-type methods are able to deal with general constraints and have faster rates of convergence [25]. This leads to a considerable reduction in computation time and makes them better suited to handling real-time nonlinear problems.

The proximal averaged Newton-type method for optimal control (PANOC) is applied, considering obstacle avoidance [26]. Compared with SQP and IP, PANOC does not require the solution of linear systems or quadratic programming problems at every iteration, and it involves only very simple operations such as vector additions and scalar and inner products. Additionally, PANOC is able to converge globally to a point that satisfies the first-order optimal conditions of the problem from any initial guess. As an extension, refs. [27,28] combines the proximal averaged Newton-type method for optimal control (PANOC) with the penalty and augmented Lagrangian methods to compute approximate stationary points of non-convex problems.

Although MPC is widely used in practice, it is known that suboptimality and infeasibility caused by the numerical optimization method can compromise the stability and positive invariance (satisfaction of constraints) properties of the MPC-controlled system. There have been a few works, such as [29], that attempt to remedy this issue, but these are limited to linear dynamical systems, convex formulations, and specific numerical optimization methods. To the best of the authors’ knowledge, no results exist on the problem of NMPC stability under inexact numerical optimization. In this work, we focus on the issue of collision avoidance and demonstrate that the combination of nonlinear MPC, PANOC, and a CBF-based framework leads to a significant improvement in safety in terms of obstacle avoidance.

3.2. Safety-Critical Manipulator Control

A system is commonly defined as safe when its state remains within a chosen set, known as the safety set. This forms the basis of controlled invariance [30], that is, finding a control strategy that ensures the system always remains in the safety set. In [30], a safety filter is used in the control of a nonlinear system to restrict the desired inputs in a way that ensures the safety of the system when necessary. The method proposed in [31] formulates robust control Lyapunov and barrier functions to provide guarantees of stability and safety in the presence of model uncertainty. A control barrier function (CBF) is applied in our work to solve the collision avoidance problem. Applications of the CBF approach to the control of a redundant manipulator can be found in [32,33], which reduces the model dependence and conservativeness. As an extension, Ding et al. [34] present a purely kinematic implementation of a velocity-based CBF to match the manipulator’s configuration space with the obstacles’ space, thereby improving safety when using the CBF-based QP motion control.

4. Problem Formulation

Formally, the skeleton-format input to the action recognition and motion prediction modules is represented by 3D joint positions of m joints for a sequence of frames recorded over time, which is the same format as in [35]. The action pose at time (frame) t is $\mathit{p}\left(t\right)\in {\mathbb{R}}^{3m}$. The observed time length is $t\in [1,{\tau }_o]$ and the unobserved time length is $t\in [{\tau }_o+1,{\tau }_o+{\tau }_p]$ for prediction.

For a single human action sequence belonging to a set of $N_a$ possible actions, $\mathcal{A}=\{1,\dots ,N_a\}$, we have the observed and future motion matrices, ${\mathit{P}}_{\mathrm{prev}}$ and ${\mathit{P}}_{\mathrm{fut}}$, where ${\mathit{P}}_{\mathrm{prev}}=[\mathit{p}\left(1\right),\dots ,\mathit{p}\left({\tau }_o\right)]\in {\mathbb{R}}^{{\tau }_o\times 3m}$ and ${\mathit{P}}_{\mathrm{fut}}=[\widehat{\mathit{p}}({\tau }_o+1),\dots ,\widehat{\mathit{p}}({\tau }_o+{\tau }_p)]\in {\mathbb{R}}^{{\tau }_p\times 3m}$ and the action classification label encoded as a one-hot vector $\mathit{y}\in {\{0,1\}}^{N_a}$. The predicted human pose at time t is denoted as $\widehat{\mathit{p}}\left(t\right)\in {\mathbb{R}}^{3m}$, which can be divided into the right hand’s position ${\widehat{\mathit{p}}}_{rh}\left(t\right)\in {\mathbb{R}}^3$ as the target position for the robot and the remaining body joints’ positions ${\widehat{\mathit{p}}}_o\left(t\right)\in {\mathbb{R}}^{3(m-1)}$ to identify the positions of obstacles.

Human action recognition and motion prediction are achieved using the two-stage deep learning computer vision approach introduced in [35], as depicted in Figure 2. The 3D-skeleton data stream provided by the OptiTrack motion capture system is first processed by a bidirectional LSTM and a GCN architecture incorporating the attention mechanism to extract spatial-temporal features of the human motion dynamics. The generated feature map, denoted as $\mathbf{\Psi }$, is then fed simultaneously to two modules, one that performs prediction and one that performs recognition. The motion prediction task is achieved using an LSTM-based decoder, while the action recognition task is performed using a CNN followed by conditional random fields (CRF). Initially, the models are trained offline using collected HRC task samples. Following training, streaming data are fed to the model to generate the predicted future pose sequence ${\widehat{\mathit{P}}}_{\mathrm{fut}}$ and predicted action category $\widehat{y}$. Denoting the feature extraction, motion prediction and action recognition functions within the Deep Neural Network model as ${\mathit{f}}_{\mathrm{ext}}$, ${\mathit{f}}_{\mathrm{pred}}$ and ${\mathit{f}}_{\mathrm{recg}}$, respectively, and ${\mathit{\theta }}_{\mathrm{ext}}$, ${\mathit{\theta }}_{\mathrm{pred}}$ and ${\mathit{\theta }}_{\mathrm{recg}}$ as their trainable parameters, the extracted feature map $\mathbf{\Psi }$, the estimated future motion ${\widehat{\mathit{P}}}_{\mathrm{fut}}$ and one-hot vector class category $\widehat{\mathit{y}}$ can be expressed as

$$\begin{array}{cc}\hfill \mathbf{\Psi }& ={\mathit{f}}_{\mathrm{ext}}({\mathit{P}}_{\mathrm{prev}};{\mathit{\theta }}_{\mathrm{ext}}),\hfill \end{array}$$

(1a)

$$\begin{array}{cc}\hfill {\widehat{\mathit{P}}}_{\mathrm{fut}}& ={\mathit{f}}_{\mathrm{pred}}(\mathbf{\Psi };{\mathit{\theta }}_{\mathrm{pred}}),\hfill \end{array}$$

(1b)

$$\begin{array}{cc}\hfill \widehat{\mathit{y}}& ={\mathit{f}}_{\mathrm{recg}}(\mathbf{\Psi };{\mathit{\theta }}_{\mathrm{recg}}).\hfill \end{array}$$

(1c)

If the predicted action is one that requires robot interaction, the predicted final position of the right hand (denoted ${\widehat{\mathit{p}}}_{\mathrm{rh}}\left({\tau }_p\right)$) is set as the target interactive position for the robot end-effector and the predicted final position of the other joints are set as the center of capsule-format obstacles.

The motion planning and execution control problem can then be stated as follows: Given the predicted future human motion sequence ${\widehat{\mathit{P}}}_{\mathrm{fut}}$ and the initial position of the robot arm, and assuming that:

• the dynamics of the manipulator are known;
• the error in the prediction of the future human position is bounded;
• the maximum velocity of the human hand is slower than that of the manipulator;

then the objective is to determine a torque input ${\mathbf{\tau }}_{\mathrm{act}}$ to the robot, which enables it to track the predicted position of the human’s right hand ${\widehat{\mathit{p}}}_{\mathrm{rh}}\left(t\right)$ to the target position and pose, while at all times avoiding collision with the human’s body.

Denoting the set of interactive human actions requiring robot interaction as ${\mathcal{A}}_{\mathrm{int}}$, where ${\mathcal{A}}_{\mathrm{int}}\subset \mathcal{A}$, the motion prediction and path planning tasks are triggered if the predicted HRC task $\widehat{y}\in {\mathcal{A}}_{\mathrm{int}}$, as shown in Figure 1.

5. NMPC Motion Planning Scheme

5.1. System Description

To model the n-DOF robotic manipulator (chosen as the right arm of our Baxter robot) with joint positions $\mathit{q}\in {\mathbb{R}}^n$, we consider an input-affine nonlinear system of the form

$$\begin{array}{cc}\hfill \dot{\mathbf{\chi }}& =\mathfrak{f}\left(\mathbf{\chi }\right)+\mathfrak{g}\left(\mathbf{\chi }\right)\mathbf{\tau },\hfill \end{array}$$

(2)

where $\mathbf{\chi }=[\mathit{q},\dot{\mathit{q}}]\in {\mathbb{R}}^{2n}$ is the the state of the system and $\mathbf{\tau }\in {\mathbb{R}}^n$ is the control input. Functions $\mathfrak{f}:{\mathbb{R}}^{2n}\to {\mathbb{R}}^{2n}$ and $\mathfrak{g}:{\mathbb{R}}^{2n}\to {\mathbb{R}}^{2n\times n}$ are continuously differentiable in $\mathbf{\chi }$.

Consider an n-degrees-of-freedom manipulator with equations of motion defined as

$$\begin{array}{cc}\hfill & \mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+\mathbf{C}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}+\mathit{g}\left(\mathit{q}\right)=\mathbf{\tau },\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill & \mathit{x}={\mathit{f}}_{\mathrm{fwd}}\left(\mathit{q}\right),\hfill \end{array}$$

(4)

where $\mathit{M}\left(\mathit{q}\right)\in {\mathbb{R}}^{n\times n}$ is a symmetric positive definite matrix, $\mathbf{C}(\mathit{q},\dot{\mathit{q}})\in {\mathbb{R}}^{n\times n}$ is a matrix of Coriolis and centrifugal forces terms, $\mathit{g}\left(\mathit{q}\right)\in {\mathbb{R}}^n$ is a vector of gravitational forces, $\mathit{q}\in {\mathbb{R}}^7$ are the joint positions, and ${\mathit{f}}_{\mathrm{fwd}}:{\mathbb{R}}^7\to {\mathbb{R}}^3$ is the kinematic transformation from the joint angle positions $\mathit{q}$ to the Cartesian position of the end-effector $\mathit{x}\in {\mathbb{R}}^3$. We consider the joint torque $\mathbf{\tau }\in {\mathbb{R}}^7$ as the system input and $\mathit{x}$ as the system output.

Defining $\mathit{J}=\mathit{J}\left(\mathit{q}\right)={\displaystyle \frac{\mathit{\partial }{f}_{fwd}}{\mathit{\partial }\mathit{q}}}$ as the Jacobian matrix, it follows that

$$\begin{array}{cc}\hfill \dot{\mathit{x}}=& \mathit{J}\left(\mathit{q}\right)\dot{\mathit{q}},\hfill \end{array}$$

(5a)

$$\begin{array}{cc}\hfill \ddot{\mathit{x}}=& \mathit{J}\left(\mathit{q}\right)\ddot{\mathit{q}}+\dot{\mathit{J}}\left(\mathit{q}\right)\dot{\mathit{q}}.\hfill \end{array}$$

(5b)

By substituting (5a) into (3), the Cartesian robotic system dynamics can be expressed as

$$\begin{array}{c}\hfill {\mathit{M}}_{\mathit{x}}\left(\mathit{q}\right)\ddot{\mathit{x}}+{\mathbf{C}}_{\mathit{x}}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{x}}+{\mathit{g}}_{\mathit{x}}\left(\mathit{q}\right)={\mathit{f}}_{\mathit{h}},\end{array}$$

(6)

where ${\mathit{M}}_{\mathit{x}}\left(\mathit{q}\right)={\mathit{J}}^{†⊺}\mathit{M}\left(\mathit{q}\right){\mathit{J}}^{†}$, ${\mathbf{C}}_{\mathit{x}}(\mathit{q},\dot{\mathit{q}})={\mathit{J}}^{†⊺}(\mathbf{C}(\mathit{q},\dot{\mathit{q}})-\mathit{M}\left(\mathit{q}\right){\mathit{J}}^{†}\dot{\mathit{J}}){\mathit{J}}^{†}$, ${\mathit{g}}_{\mathit{x}}\left(\mathit{q}\right)={\mathit{J}}^{†⊺}\mathit{g}\left(\mathit{q}\right)$ and ${\mathit{f}}_{\mathit{h}}={\mathit{J}}^{†}\mathbf{\tau }$ is the vector of generated control forces. ${\mathit{J}}^{†}={\mathit{J}}^{†}\left(\mathit{q}\right)$ denotes the pseudo-inverse of $\mathit{J}\left(\mathit{q}\right)$, that is:

$$\begin{array}{c}\hfill {\mathit{J}}^{†}\left(\mathit{q}\right)={\left({\mathit{J}}^{⊺}\left(\mathit{q}\right)\mathit{J}\left(\mathit{q}\right)\right)}^{-1}{\mathit{J}}^{⊺}\left(\mathit{q}\right).\end{array}$$

(7)

For notational simplicity, ${\mathit{M}}_{\mathit{x}}$, ${\mathbf{C}}_{\mathit{x}}$, ${\mathit{g}}_{\mathit{x}}$ will be used to denote ${\mathit{M}}_{\mathit{x}}\left(\mathit{q}\right)$, ${\mathbf{C}}_{\mathit{x}}(\mathit{q},\dot{\mathit{q}}$), ${\mathit{g}}_{\mathit{x}}\left(\mathit{q}\right)$, respectively. The system (2) is converted from joint space to task space, which can be expressed as

$$\begin{array}{c}\hfill \left[\begin{array}{c}\dot{\mathit{x}}\\ \ddot{\mathit{x}}\end{array}\right]=\underset{\underset{\mathit{f}(\mathit{x},\dot{\mathit{x}})}{⎵}}{\left[\begin{array}{c}\dot{\mathit{x}}\\ -{{\mathit{M}}_{\mathit{x}}}^{-1}({\mathbf{C}}_{\mathit{x}}\dot{\mathit{x}}+{\mathit{g}}_{\mathit{x}})\end{array}\right]}+\underset{\mathit{g}\left(\mathit{x}\right)}{\underbracket{\left[\begin{array}{c}0\\ -{{\mathit{M}}_{\mathit{x}}}^{-1}\end{array}\right]}}{\mathit{f}}_{\mathit{h}}.\end{array}$$

(8)

Thus, the robotic system (8) is equivalent to the nonlinear affine system (2) with respect to the input ${\mathit{f}}_{\mathit{h}}$. Therefore, the ECBF can be applied to the robotic system (8) to deal with the time-varying output constraints.

5.2. Minimum Distance Calculation

The minimum distance is the main input for most collision avoidance, HRI, robot decision-making, and robot navigation methods [36]. Given the joint coordinates provided by the skeletal tracking algorithm and the point clouds on the boundary of the human body, bounding capsules (cylinders with semi-spherical ends) can be computed for the human body compo as a simplified representation of the human in the distance calculation. We generate minimum bounding capsules along the bones in the skeleton, as shown in Figure 3. This is repeated for the predicted skeleton in each frame over the prediction horizon. For the 32-joint skeleton model representation, ${\widehat{\mathit{p}}}_{\mathbf{rh}}$ is the position of the center of the right hand.

We chose the capsules model to represent robot links since it considers both the skeleton and the boundary of the human, which models the non-geometric shape of the human body [37]. The 32-joint skeleton model, as used in the Human 3.6 M dataset [38], maps to a total of 15 capsules for the human body. The approach to measure the distance between capsules is depicted in Figure 3. The distance calculation is implemented using the GJK (Gilbert–Johnson–Keerthi) algorithm [18].

The algorithm returns $\lambda \left(t\right)$, the minimum distance between the boundary of the capsules representing the human and the robot in the HRC task at time t.

5.3. Nonlinear Model Predictive Control for Collision Avoidance

The robot’s objective is for its end-effector to follow the estimated position of the human’s right hand while avoiding collisions. To that end, we propose an NMPC formulation as a high-level control system that commands angular velocity references to a low-level control system (see Section 6.2). The overall controller architecture is shown in Figure 4.

The input of the high-level control system is the joint velocity commands $\mathit{u}\left(t\right)$. At every sampling time instant, we need to solve a finite-horizon optimal control problem with prediction horizon $T_p>0$, stage cost function ℓ, and terminal cost function ${\ell }_f$, where ℓ and ${\ell }_f$ are given by

$$\begin{array}{cc}\hfill \ell (\widehat{\mathit{x}},\widehat{\mathit{u}})& =e{({\widehat{\mathit{p}}}_{\mathrm{rh}},\widehat{\mathit{x}})}^{⊺}{\mathit{R}}_{p}e({\widehat{\mathit{p}}}_{\mathrm{rh}},\widehat{\mathit{x}})+{\widehat{\mathit{u}}}^{⊺}{\mathit{R}}_v\widehat{\mathit{u}},\hfill \end{array}$$

(9)

and

$$\begin{array}{c}\hfill {\ell }_f\left(\widehat{\mathit{x}}\right)=\mathit{e}{({\widehat{\mathit{p}}}_{\mathrm{rh}}\left(T_p\right),\widehat{\mathit{x}}\left(T_p\right))}^{⊺}{\mathit{Q}}_p\mathit{e}({\widehat{\mathit{p}}}_{\mathrm{rh}}\left(T_p\right),\widehat{\mathit{x}}\left(T_p\right))+{\widehat{\mathit{u}}}^{⊺}\left(T_p\right){\mathit{Q}}_v\widehat{\mathit{u}}\left(T_p\right),\end{array}$$

(10)

respectively. Here, $\mathit{e}({\widehat{\mathit{p}}}_{rh}\left(T_p\right),\mathit{x}\left(t\right))$ is an error metric between the given Cartesian goal ${\widehat{\mathit{p}}}_{rh}\left(T_p\right)$ and the current Cartesian state of the robot $\mathit{x}\left(t\right)$. The reference position ${\widehat{\mathit{p}}}_{\mathrm{rh}}\left(T_p\right)$ is obtained by the motion predictor. ${\mathit{Q}}_p$ and ${\mathit{Q}}_v$ are constant weight matrices for terms in the terminal cost function, while ${\mathit{R}}_p$ and ${\mathit{R}}_v$ are the weight matrices for terms in the stage cost function. Following [39], the error function $e({\mathit{x}}_d,\widehat{\mathit{x}}\left(t\right))$ is defined as

$$\begin{array}{c}\hfill \mathit{e}({\widehat{\mathit{p}}}_{\mathrm{rh}}\left(t\right),\mathit{x}\left(t\right))=\left[\begin{array}{cc}-{\mathit{I}}_3& 0\\ 0& {\mathbf{\Pi }}_d\end{array}\right]\widehat{\mathit{x}}\left(t\right)+\left[\begin{array}{c}{\widehat{\mathit{p}}}_{\mathrm{rh}}\left(t\right)\\ 0\end{array}\right],\end{array}$$

(11)

where matrix ${\mathbf{\Pi }}_d$ is built from the desired quaternion, which remains constant during the optimization.

The initial and final configurations of the robot ${\mathit{q}}_0=[q_{01},\dots ,q_{07}]$ and ${\mathit{q}}_f=[q_{f1},\dots ,q_{f7}]$ are obtained using the inverse kinematics function ${\mathit{f}}_{\mathrm{inv}}:{\mathbb{R}}^6\to {\mathbb{R}}^7$, which converts the Cartesian end effector poses (position and orientation) to the corresponding joint positions. Using the GJK algorithm, the minimum distance calculation at time t is performed based on the Cartesian coordinates of the robot, $\mathit{x}\left(t\right)$, and the predicted human pose, ${\widehat{\mathit{p}}}_o\left(t\right)$. The distance between the end-effector and the predicted operator’s position $\lambda (\widehat{\mathit{x}}\left(t\right);{\widehat{\mathit{p}}}_o\left(t\right))$ must be greater than the minimum safe distance $d_{\mathrm{safe}}$. The value of $d_{\mathrm{safe}}$ is chosen to be greater than the maximum prediction error bound to guarantee the safety of humans. The constraints are described as

$$\begin{array}{cc}\hfill & \dot{\widehat{\mathit{q}}}\left(t\right)=\widehat{\mathit{u}}\left(t\right),t\in [0,T_p],\hfill \end{array}$$

(12a)

$$\begin{array}{cc}\hfill & \widehat{\mathit{q}}\left(0\right)={\mathit{q}}_0,\hfill \end{array}$$

(12b)

$$\begin{array}{cc}\hfill & \widehat{\mathit{x}}\left(t\right)={\mathit{f}}_{\mathrm{fwd}}\left(\widehat{\mathit{q}}\left(t\right)\right),t\in [0,T_p],\hfill \end{array}$$

(12c)

$$\begin{array}{cc}\hfill & \lambda (\widehat{\mathit{x}}\left(t\right);{\widehat{\mathit{p}}}_o\left(t\right))\ge d_{\mathrm{safe}},t\in [0,T_p],\hfill \end{array}$$

(12d)

where $\mathit{q}=(q_1,\dots ,q_7)$ denotes the state of the robotic arm. The control actions are constrained in the set $\mathcal{U}$. Then, the optimal solution ${\widehat{\mathit{u}}}^{★}$ is obtained by minimizing the optimal control cost function, that is,

$$\underset{\widehat{\mathit{u}}:[0,T_p]\to \mathcal{U}}{minimize}{\ell }_f\left(\widehat{\mathit{x}}\left(T_p\right)\right)+{\int }_0^{T_p}\ell (\widehat{\mathit{x}}\left(t\right),\widehat{\mathit{u}}\left(t\right))\mathrm{d}t,$$

(13)

subject to the constraints (Section 5.3).

In this work, we solve the NMPC problem using a numerical optimization method. To that end, we convert the above problem to a discrete-time optimal control problem using an explicit discretization and integration method. Assuming that $T_p=N_h{T}_s$, the NMPC problem in Equation (13) is reformulated to ${\mathbb{P}}_d$ and depends on the current state ${\mathit{q}}_0$, the set-point ${\mathit{q}}_{\mathit{f}}$ for the joint positions, input $\mathit{u}$ and the estimated obstacle distance $\widehat{\lambda }$ computed by the GJK algorithm as follows:

$$\begin{array}{cc}\hfill {\mathbb{P}}_d({\widehat{\mathit{q}}}_0,{\widehat{\mathit{p}}}_o,{\mathit{q}}_f):& \underset{{\widehat{\mathit{u}}}_0,\dots ,{\widehat{\mathit{u}}}_{N_h-1}\in \mathcal{U}}{minimize}{\ell }_{d,f}\left({\widehat{\mathit{x}}}_{N_h}\right)\sum _{k=0}^{N_h-1}{\ell }_d({\widehat{\mathit{x}}}_k,{\widehat{\mathit{u}}}_k),\hfill \end{array}$$

(14a)

subject to the constraints

$$\begin{array}{c}{\widehat{\mathit{q}}}_{k+1}={\widehat{\mathit{q}}}_k+T_s{\widehat{\mathit{u}}}_k,k\in {\mathbb{N}}_{[0,N_h-1]},\hfill \end{array}$$

(14b)

$$\begin{array}{cc}\hfill & {\widehat{\mathit{q}}}_0={\mathit{q}}_0,\hfill \end{array}$$

(14c)

$$\begin{array}{cc}\hfill & {\widehat{\mathit{q}}}_{N_h}={\mathit{q}}_f,\hfill \end{array}$$

(14d)

$$\begin{array}{cc}\hfill & {\widehat{\mathit{x}}}_k={\mathit{f}}_{\mathrm{fwd}}\left({\widehat{\mathit{q}}}_k\right),k\in {\mathbb{N}}_{[0,N_h-1]},\hfill \end{array}$$

(14e)

$$\begin{array}{cc}\hfill & \lambda ({\widehat{\mathit{x}}}_k;{\widehat{\mathit{p}}}_{o,k})\ge d_{\mathrm{safe}},k\in {\mathbb{N}}_{[0,N_h-1]}.\hfill \end{array}$$

(14f)

By solving ${\mathbb{P}}_d$ at every discrete time instant, we obtain the solution ${\widehat{\mathit{u}}}_k^{★}$, $k\in {\mathbb{N}}_{[0,N_h-1]}$, only the first value of which, ${\widehat{\mathit{u}}}_0^{★}$, is applied to the system. ${\mathbb{P}}_d$ is solved again at the subsequent time instant in a receding horizon fashion.

5.4. Numerical Solution

To solve ${\mathbb{P}}_d$ numerically, we employ OpEn [27], a code generation software package that generates optimizers in the Rust programming language for deployment on embedded devices. OpEn combines the PANOC numerical algorithm with the penalty method to solve the above problem. However, OpEn requires that we cast ${\mathbb{P}}_d$ in the following general form:

$$\begin{array}{cc}\hfill \underset{\mathit{u}\in \tilde{\mathcal{U}}}{minimize}& F_0\left(\mathit{u}\right),\hfill \end{array}$$

(15a)

$$\begin{array}{cc}\hfill \mathrm{subject}\mathrm{to}:& {\mathbf{F}}_i\left(\mathit{u}\right)=\mathbf{0},i\in {\mathbb{N}}_{[1,n_c]},\hfill \end{array}$$

(15b)

where $\mathit{u}\in {\mathbb{R}}^n$ is the decision variable, $\tilde{\mathcal{U}}\subseteq {\mathbb{R}}^n$ is a nonempty closed set on which it is easy to compute projections, $F_0:{\mathbb{R}}^n\to \mathbb{R}$ is a continuously differentiable cost function—not necessarily convex—with the Lipschitz gradient, and ${\mathbf{F}}_i:{\mathbb{R}}^n\to {\mathbb{R}}^{m_i}$ are functions such that $\parallel {\mathbf{F}}_i{\parallel }^2$ is continuously differentiable with the Lipschitz gradient.

To cast problem ${\mathbb{P}}_d$ in this form, we choose the decision variable to be $\mathit{u}=({\widehat{\mathit{u}}}_0,{\widehat{\mathit{u}}}_1,\dots ,{\widehat{\mathit{u}}}_{N-1})$ and define the sequence of functions

$${\mathbf{\Phi }}_{k+1}(\mathit{u},\mathit{x})=\left[\begin{array}{c}{\mathit{q}}_0+T_s{\sum }_{t=0}^k{\mathit{u}}_k\\ {\mathit{f}}_{\mathrm{fwd}}\left({\mathit{q}}_0+T_s{\sum }_{t=0}^k{\mathit{u}}_k\right),\end{array}\right]$$

(16)

with ${\mathbf{\Phi }}_0(\mathit{u},\mathit{x})={\left[{\mathit{q}}^{⊺}{\mathit{x}}^{⊺}\right]}^{⊺}$. Note that ${\mathbf{\Phi }}_{k+1}(\mathit{u},\mathit{x})$ contains the location of the robot in both joint space and task space at time $k+1$ as a function of the initial state and the sequence of control actions $\mathit{u}$. This allows us to eliminate the sequence of states and define the cost function,

$$F_0(\mathit{u},\mathit{x})={\ell }_{d,f}\left({\mathbf{\Phi }}_{N_h}(\mathit{u},\mathit{x})\right)+\sum _{k=0}^{N_h-1}{\ell }_d({\mathbf{\Phi }}_k(\mathit{u},\mathit{x}),{\mathit{u}}_k).$$

(17)

In order to impose the terminal condition of Equation (14d), we define

$${\mathit{F}}_1\left(\mathit{u}\right)={\widehat{\mathit{q}}}_{N_h}-{\mathit{q}}_f,$$

(18)

where ${\widehat{\mathit{q}}}_{N_h}={\widehat{\mathit{q}}}_{N_h}(\mathit{u},\mathit{x})$. To impose the obstacle avoidance condition of Equation (14f), we define

$${\mathit{F}}_{k+1}\left(\mathit{u}\right)={[d_{\mathrm{safe}}-\lambda ({\mathbf{\Phi }}_k(\mathit{u},\mathit{x});{\widehat{\mathit{p}}}_{o,k})]}_{+},$$

(19)

for $k\in {\mathbb{N}}_{[0,N_h-1]}$. Finally, we choose $\tilde{\mathcal{U}}={\mathcal{U}}^{N_h}$ for the input constraints.

OpEn solves a series of optimization problems of the form

$${\mathbb{P}}_{\mathrm{in}}:\underset{\mathit{u}\in U}{minimize}{F}_0\left(\mathit{u}\right)+c\underset{\mathrm{infeasibility}}{\underbracket{\sum _{i=1}^{n_c}{\parallel {\mathit{F}}_i\left(\mathit{u}\right)\parallel }^2}},$$

(20)

for some $c>0$ termed the inner problems. Essentially, the hard constraints of the original problem are converted into soft constraints. Starting with an initial value of c, a sequence of inner problems is solved numerically using the PANOC method while driving c to infinity and warm-starting each next inner problem with the solution of the previous one [27]. The algorithm terminates once the infeasibility, as defined in Equation (20), drops below a given tolerance, ${ϵ}_{\mathrm{infeas}}>0$.

6. Controller Design

6.1. Trajectory Computation

Given the estimated angular velocity command ${\widehat{\mathit{u}}}^{★}$, the desired joint state ${\mathit{q}}_d$ is updated as

$$\begin{array}{c}\hfill {\mathit{q}}_{d,k+1}={\mathit{q}}_{d,k}+T_s{\widehat{\mathit{u}}}^{★}.\end{array}$$

(21)

Then, the generated desired joint position is used to calculate the desired Cartesian position ${\mathit{x}}_d$ and velocity ${\dot{\mathit{x}}}_d$.

6.2. Low-Level Controller

To achieve asymptotic stability, the control force ${\mathit{f}}_{\mathit{h}}$ input to the safety filter is defined as

$$\begin{array}{c}\hfill {\mathit{f}}_{\mathit{h}}={\mathit{C}}_{\mathit{x}}({\dot{\mathit{x}}}_d-\Lambda \tilde{\mathit{x}})+{\mathit{g}}_{\mathit{x}}+{\mathit{M}}_{\mathit{x}}({\ddot{\mathit{x}}}_d-\Lambda \dot{\tilde{\mathit{x}}})-k_z\mathrm{sign}\left(\mathit{z}\right),\end{array}$$

(22)

where $\Lambda$ and $k_z$ are positive constants. A composite state error is denoted as $\mathit{z}=\dot{\tilde{\mathit{x}}}+\Lambda \tilde{\mathit{x}}$, where $\tilde{\mathit{x}}=\mathit{x}-{\mathit{x}}_d$ and $\dot{\tilde{\mathit{x}}}=\dot{\mathit{x}}-{\dot{\mathit{x}}}_d$ are the error of the Cartesian position and velocity, respectively. $\mathrm{sign}\left(\mathit{z}\right)$ is a sign function depends on the state error. Considering the chattering problem caused by the sign function, (22) is rewritten as

$$\begin{array}{c}\hfill {\mathit{f}}_{\mathit{h}}={\mathit{C}}_{\mathit{x}}({\dot{\mathit{x}}}_d-\Lambda \tilde{\mathit{x}})+{\mathit{g}}_{\mathit{x}}+{\mathit{M}}_{\mathit{x}}({\ddot{\mathit{x}}}_d-\Lambda \dot{\tilde{\mathit{x}}})-k_z{\displaystyle \frac{\mathit{z}}{\parallel \mathit{z}\parallel +c_1}},\end{array}$$

(23)

where $c_1$ is a small positive number.

6.3. Exponential Control Barrier Function as a Safety Filter

In this section, an ECBF-based safety filter is introduced to prevent collisions with the human operator by constraining the trajectory generated by NMPC to be in a safe zone. ECBFs are formally defined as follows:

Definition 1 

(ECBF [40]). Consider the system (8) and a set $\mathcal{C}=\{\mathit{x}\in {\mathbb{R}}^3|h(t,\mathit{x})\ge 0,\forall t>0\}$ defined by a continuously differentiable function $h(t,\mathit{x}):{\mathbb{R}}^3\to \mathbb{R}$ with relative degree $\kappa \ge 2$. Then, $h(t,\mathit{x})$ is an exponential control barrier function (ECBF) if there is a ${\mathit{k}}_b\in {\mathbb{R}}^{\kappa -1}$, such that

$$\begin{array}{c}\hfill \underset{\mathit{u}\in \mathcal{U}}{inf}[L_{\mathit{f}}^{\kappa }h(t,\mathit{x})+L_{\mathit{g}}{L}_{\mathit{f}}^{\kappa -1}h(t,\mathit{x})\mathit{u}+{\mathit{k}}_b{\mathbf{\zeta }}_{\mathbf{b}}\left(\mathit{x}\right)\ge 0,\end{array}$$

(24)

where ${\mathbf{\zeta }}_{\mathbf{b}}$ can be written as

$$\begin{array}{cc}\hfill {\mathbf{\zeta }}_b\left(\mathit{x}\right):& =\left[\begin{array}{c}h\left(\mathit{x}\right)\\ \dot{h}\left(\mathit{x}\right)\\ ⋮\\ h^{(\kappa -1)}\left(\mathit{x}\right)\end{array}\right]=\left[\begin{array}{c}h\left(\mathit{x}\right)\\ L_{\mathit{f}}h\left(\mathit{x}\right)\\ ⋮\\ L_{\mathit{f}}^{(\kappa -1)}h\left(\mathit{x}\right)\end{array}\right].\hfill \end{array}$$

(25)

The method for selecting ${\mathit{k}}_b$ is detailed in [40]. The Lie derivative of $h\left(\mathit{x}\right)$ along $f\left(\mathit{x}\right)$ is defined as

$$\begin{array}{c}\hfill L_{\mathit{f}}h(t,\mathit{x})={\displaystyle \frac{\mathit{\partial }h(t,\mathit{x})}{\mathit{\partial }\mathit{x}}}\mathit{f}(\mathit{x},\dot{\mathit{x}}),\end{array}$$

(26)

where $\mathit{x}$ is the state of the system. The Lie derivative of $h\left(\mathit{x}\right)$ along $\mathit{g}\left(\mathit{x}\right)$ is defined as $L_{\mathit{g}}h(t,\mathit{x})$.

Denote ${\mathit{p}}_{oi}\left(t\right)\in {\mathbb{R}}^3$ as the support point (i.e., a point on the boundary of the convex set that is closest to the robot) on the convex set representing the area of the human obstacle for the $i^{th}$ robot joint computed by GJK at time t. To ensure the robot does not collide with the moving human, the Cartesian position of the robot joints ${\mathit{x}}_i\in {\mathbb{R}}^3$, $i\in {\mathbb{N}}_{[1,7]}$ are under the following constraints

$$\begin{array}{c}\hfill \parallel {\mathit{x}}_i\left(t\right)-{\widehat{\mathit{p}}}_s{\left(t\right)\parallel }_2>d_{\mathrm{safe}},\mathrm{for}i\in {\mathbb{N}}_{[1,7]},t\ge 0,\end{array}$$

(27)

where ${\widehat{\mathit{p}}}_s$ is a support point on the boundary of the capsule with the minimum distance to the $i^{\mathrm{th}}$ robot joint, as shown in Figure 3. We define a candidate ECBF $h_i(t,{\mathit{x}}_i)$ as

$$\begin{array}{cc}\hfill h_i(t,{\mathit{x}}_i)& =\parallel {\mathit{x}}_i\left(t\right)-{\widehat{\mathit{p}}}_s{\left(t\right)\parallel }_2^2-d_{\mathrm{safe}}^2.\hfill \end{array}$$

(28)

To modify the applied forces, ${\mathit{f}}_{\mathit{h}}$, to guarantee safety, a quadratic programming (QP)-based safety filter is employed. Considering the position constraints with relative degree 2, the optimization problem can be formulated as minimizing the normed difference between ${\mathit{f}}_{\mathit{h}}$ and the actual input ${\mathit{u}}_{\mathrm{act}}$, in order to maintain the robot within the safety region. In other words, ${\mathit{u}}_{\mathrm{act}}(\mathit{x},t)$ is the control input that minimizes the following QP:

$$\begin{array}{cc}\hfill & \underset{{\mathit{u}}_{\mathrm{act}}\in \mathcal{U}}{minimize}{\parallel {\mathit{f}}_{\mathit{h}}-{\mathit{u}}_{\mathrm{act}}\parallel }^2,\hfill \end{array}$$

(29a)

subject to

$$\begin{array}{c}{L}_{\mathit{f}}^2{h}_i(t,\mathit{x})+L_{\mathit{g}}{L}_{\mathit{f}}{h}_i(t,\mathit{x}){\mathit{u}}_{\mathrm{act}}+k_1{h}_i(t,\mathit{x})\hfill \\ +k_2{L}_{\mathit{f}}{h}_i(t,\mathit{x})\ge 0,i\in {\mathbb{N}}_{[1,7]},\hfill \end{array}$$

(29b)

where $h_i(t,\mathit{x})$ is as defined in (28). The robot joint torques are then obtained as ${\mathbf{\tau }}_{\mathrm{act}}={\mathit{J}}^{⊺}\left(\mathit{q}\right){\mathit{u}}_{\mathrm{act}}$. Further details on the implementation of ECBFs are given in [40].

6.4. System Stability Analysis

Proving the global stability of our ECBF-NMPC dynamic motion planning framework is challenging as it incorporates approximate numerical methods, a neural network-based motion predictor, the GJK algorithm, and a safety filter. To guarantee local asymptotic stability, a control Lyapunov constraint is incorporated into the ECBF formulation and activated when the robot is close to the destination.

Property 1 

([41]). The inertia matrix $\mathit{M}\left(\mathit{q}\right)$ and ${\mathit{M}}_{\mathit{x}}$ are symmetric positive definite.

Property 2 

([41]). The matrix $2{\mathit{C}}_x(\mathit{q},\dot{\mathit{q}})-{\dot{\mathit{M}}}_x\left(\mathit{q}\right)$ is skew-symmetric.

Definition 2 

(${\mathcal{K}}_{\infty }$-class function [42]). A continuous function $\beta :{\mathbb{R}}_{+}\to {\mathbb{R}}_{+}$ is a ${\mathcal{K}}_{\infty }$-class function (denoted as $\beta \in {\mathcal{K}}_{\infty }$), if

• $\beta \left(0\right)=0$
• β is increasing
• ${lim}_{a\to \infty }\beta \left(a\right)=\infty$.

Definition 3 

(Control Lyapunov Functions [43]). Given the system (8) and the composite error $\mathit{z}(\mathit{x},t)=\dot{\tilde{\mathit{x}}}+\Lambda \tilde{\mathit{x}}$, with $\Lambda >0$, consider a continuously differentiable function $V:{\mathbb{R}}^3\to {\mathbb{R}}_{+}$ on set $\mathcal{Z}=\{\mathit{z}(\mathit{x},t)\in {\mathbb{R}}^3:\mathit{x}\in \mathcal{C},\forall t>0\}$, where $\mathcal{C}$ is the safe set as defined for the ECBF (Definition 1). Then, V is a control Lyapunov function (CLF) for the system in (8) if there exist ${\beta }_1$, ${\beta }_2$, ${\beta }_3\in {\mathcal{K}}_{\infty }$ such that for all $\mathit{z}\in \mathcal{Z}$

$$\begin{array}{c}\hfill {\beta }_1(\parallel \mathit{z}\parallel )\le V\left(\mathit{z}\right)\le {\beta }_2(\parallel \mathit{z}\parallel ),\end{array}$$

(30)

$$\begin{array}{c}\hfill \underset{\mathit{u}\in \mathcal{U}}{inf}\dot{V}(\mathit{z},\mathit{u})\le -{\beta }_3(\parallel \mathit{z}\parallel ).\end{array}$$

(31)

The existence of a CLF implies the existence of a control law that renders the closed-loop system locally asymptotically stable [44]. The target when employing a CLF constraint is to make the composite error $\mathit{z}(\mathit{x},\dot{\mathit{x}})$, which models the generalized Cartesian position and velocity errors with respect to the desired references, locally asymptotically converge to 0. Then, the problem (29) needs to satisfy both the ECBF and CLF constraints, which are written as

$$\begin{array}{cc}\hfill \underset{{\mathit{u}}_{\mathrm{act}}\in \mathcal{U}}{minimize}& \parallel {\mathit{f}}_{\mathit{h}}-{\mathit{u}}_{\mathrm{act}}{\parallel }^2,\hfill \end{array}$$

(32a)

subject to

$$\begin{array}{c}{L}_f^2{h}_i(t,{\mathit{x}}_i)+L_g{L}_f{h}_i(t,{\mathit{x}}_i){\mathit{u}}_{\mathrm{act},i}+k_1{h}_i(t,{\mathit{x}}_i)+k_2{L}_f{h}_i(t,{\mathit{x}}_i)\ge 0,i{\in }_{[1,7]},\hfill \end{array}$$

(32b)

$$\begin{array}{cc}\hfill & {\mathit{z}}_i^{⊺}\left({\mathit{u}}_{\mathrm{act},i}+k_z{\displaystyle \frac{{\mathit{z}}_i}{\parallel {\mathit{z}}_i\parallel +c_1}}\right)-{\mathit{z}}_i^{⊺}{\mathit{K}}_D{\mathit{z}}_i\ge 0,i{\in }_{[1,7]},\hfill \end{array}$$

(32c)

where ${\mathit{K}}_D$ is a symmetric positive definite matrix.

The candidate Lyapunov function V is defined as

$$\begin{array}{c}\hfill V\left({\mathit{z}}_i\right)=\frac{1}{2}{\mathit{z}}_i^{⊺}{\mathit{M}}_{\mathit{x}}{\mathit{z}}_i.\end{array}$$

(33)

Since ${\mathit{M}}_{\mathit{x}}\left(\mathit{q}\right)$ is positive definite for all $\mathit{q}$, its minimum eigenvalue is positive. Assuming that ${inf}_{\mathit{q}}{\lambda }_{min}\left({\mathit{M}}_{\mathit{x}}\left(\mathit{q}\right)\right)>0$ and ${sup}_{\mathit{q}}{\lambda }_{max}\left({\mathit{M}}_{\mathit{x}}\left(\mathit{q}\right)\right)<\infty$, the inequality in (30) is satisfied with ${\beta }_1(\parallel z\parallel )={inf}_{\mathit{q}}{\lambda }_{min}\left({\mathit{M}}_{\mathit{x}}\left(\mathit{q}\right)\right){\parallel z\parallel }^2$ and ${\beta }_2(\parallel z\parallel )={sup}_{\mathit{q}}{\lambda }_{max}\left({\mathit{M}}_{\mathit{x}}\left(\mathit{q}\right)\right){\parallel z\parallel }^2$.

Differentiating (33) with respect to time and using Property 1 to combine terms gives

$$\begin{array}{c}\hfill \dot{V}=\frac{1}{2}{\mathit{z}}_i^{⊺}{\dot{\mathit{M}}}_{\mathit{x}}{\mathit{z}}_i+{\mathit{z}}_i^{⊺}{\mathit{M}}_{\mathit{x}}{\dot{\mathit{z}}}_i.\end{array}$$

(34)

Noting that ${\dot{\mathit{z}}}_i={\ddot{\mathit{x}}}_i-{\ddot{\mathit{x}}}_{d,i}+\Lambda ({\dot{\mathit{x}}}_i-{\dot{\mathit{x}}}_{d,i})$, and substituting for ${\dot{\mathit{M}}}_{\mathit{x}}{\ddot{\mathit{x}}}_i$ using (6) with ${\mathit{f}}_{\mathit{h}}$ replaced by the modified control force ${\mathit{f}}_{\mathit{h}}-{\mathit{u}}_{\mathrm{act},i}$, ${\mathit{M}}_{\mathit{x}}{\dot{\mathit{z}}}_i$ can be written as

$$\begin{array}{cc}\hfill {\mathit{M}}_{\mathit{x}}{\dot{\mathit{z}}}_i& ={\mathit{f}}_{\mathit{h}}-{\mathit{u}}_{\mathrm{act},i}-{\mathbf{C}}_{\mathit{x}}({\mathit{z}}_i+{\dot{\mathit{x}}}_{d,i}\hfill \\ \hfill & -\Lambda ({\mathit{x}}_i-{\mathit{x}}_{d,i}))-{\mathit{g}}_{\mathit{x}}-{\mathit{M}}_{\mathit{x}}({\ddot{\mathit{x}}}_{d,i}-\Lambda {\dot{\tilde{\mathit{x}}}}_i).\hfill \end{array}$$

(35)

Substituting (35) into (34) yields

$$\begin{array}{cc}\hfill \dot{V}& =\frac{1}{2}{\mathit{z}}_i^{⊺}{\dot{\mathit{M}}}_{\mathit{x}}{\mathit{z}}_i+{\mathit{z}}_i^{⊺}[{\mathit{f}}_{\mathit{h}}-{\mathit{u}}_{\mathrm{act},i}-{\mathbf{C}}_{\mathit{x}}({\mathit{z}}_i+{\dot{\mathit{x}}}_{d,i}\hfill \\ \hfill & -\Lambda ({\mathit{x}}_i-{\mathit{x}}_{d,i}))-{\mathit{g}}_{\mathit{x}}-{\mathit{M}}_{\mathit{x}}({\ddot{\mathit{x}}}_{d,i}-\Lambda {\dot{\tilde{\mathit{x}}}}_i)]\hfill \\ \hfill & =\frac{1}{2}{\mathit{z}}_i^{⊺}(\dot{{\mathit{M}}_{\mathit{x}}}-2{\mathbf{C}}_{\mathit{x}}){\mathit{z}}_i+{\mathit{z}}_i^{⊺}[{\mathit{f}}_{\mathit{h}}-{\mathit{u}}_{\mathrm{act},i}\hfill \\ \hfill & -{\mathbf{C}}_{\mathit{x}}({\dot{\mathit{x}}}_{d,i}-\Lambda {\tilde{\mathit{x}}}_i)-{\mathit{g}}_{\mathit{x}}]-{\mathit{z}}_i^{⊺}{\mathit{M}}_{\mathit{x}}({\ddot{\mathit{x}}}_{d,i}-\Lambda {\dot{\tilde{\mathit{x}}}}_i).\hfill \end{array}$$

(36)

By Property 2, ${\mathit{z}}_i^{⊺}(\dot{{\mathit{M}}_{\mathit{x}}}-2{\mathbf{C}}_{\mathit{x}}){\mathit{z}}_i=0$, hence the derivative of V can be expressed as

$$\begin{array}{c}\hfill \dot{V}={\mathit{z}}_i^{⊺}[({\mathit{f}}_{\mathit{h}}-{\mathit{u}}_{\mathrm{act},i})-{\mathbf{C}}_{\mathit{x}}({\dot{\mathit{x}}}_{d,i}-\Lambda {\tilde{\mathit{x}}}_i)-{\mathit{g}}_{\mathit{x}}]-{\mathit{z}}_i^{⊺}{\mathit{M}}_{\mathit{x}}({\ddot{\mathit{x}}}_{d,i}-\Lambda {\dot{\tilde{\mathit{x}}}}_i).\end{array}$$

(37)

Substituting (23) into (37) yields

$$\begin{array}{c}\hfill \dot{V}=-{\mathit{z}}_i^{⊺}{\mathit{u}}_{\mathrm{act},i}-{\displaystyle \frac{k_z{\mathit{z}}_i^{⊺}{\mathit{z}}_i}{\parallel {\mathit{z}}_i\parallel +c_1}}.\end{array}$$

(38)

According to Definition 2 and (32b), the stability of the low-level controller is guaranteed when $\dot{V}\le -{\mathit{z}}_i^{⊺}{\mathit{K}}_D{\mathit{z}}_i$ assuming problem (32) is feasible.

7. Experimental Setup

The collaboration scenario shown in Figure 5 is a typical screwdriver usage scenario where the robot holds the screw at an initial position and delivers it to the human operator before the operator picks up the screwdriver. The operator’s responsibility is to use the screwdriver and take the screw from the robot end-effector, which stops at a safe interactive distance $d_{\mathrm{safe}}$. Active collision avoidance is activated when the human body appears in the robot’s original trajectory. For seamless human–robot collaboration, the robot starts to calculate a new trajectory based on the predicted future pose sequence for the operator (1-sec horizon, $N_h$ = 20). The overall procedure for using the screwdriver is illustrated in Algorithm 1. After the initialization of the robot and the OptiTrack camera system, the software enters the main execution loop. Here, the action being undertaken by the operator is predicted at each sample instant. If an action requiring interaction is predicted, the software calls Algorithm 2 to compute the required robot joint torques in accordance with the proposed ECBF-NMPC dynamic motion planning framework.

Algorithm 1: Vision-based screw-driver usage task execution

Algorithm 2: Motion-prediction-based path planning

Input:  ${\mathit{f}}_{\mathrm{pred}}$ (pretrained motion predictor), ${\mathit{P}}_{\mathrm{prev}}$ (previous one second motion sequence), $\mathit{x}$ (current state of the robot).  Output:  input to robot ${\mathbf{\tau }}_{\mathrm{act}}$ 1 Define the controller. 2 $[{\widehat{\mathit{p}}}_{\mathrm{rh}},{\widehat{\mathit{p}}}_{\mathrm{o}}]={\underset{¯}{f}}_{\mathrm{pred}}\left({\mathit{P}}_{\mathrm{prev}}\right)$ 3 Build the capsule-format representation of the human body and compute the minimum distance $\lambda (\mathit{x},{\widehat{\mathit{p}}}_{\mathbf{o}}))$. 4 Solve the defined problem in OpEn and compute the optimal input to the high-level controller ${\widehat{\mathit{u}}}^{★}$. 5 Compute the desired robot state ${\mathit{q}}_d$ and ${\dot{\mathit{q}}}_d$. 6 Design the control input to the safety filter ${\mathit{f}}_{\mathit{h}}\left(t\right)$. 7 Solve the ECBF quadratic program to determine ${\mathit{u}}_{act}\left(t\right)$. 8 Compute the input to the robot: ${\mathbf{\tau }}_{\mathrm{act}}\left(t\right)={\mathit{J}}^{⊺}\left(\mathit{q}\left(t\right)\right){\mathit{u}}_{\mathrm{act}}\left(t\right)$.

7.1. Motion Capture

In this work, we generate high-quality skeleton data using an OptiTrack camera system, which includes four volunteers. The volunteers enact the required behavior in a working environment with three cameras, with the volunteers’ behavior tracked from different perspectives, as shown in Figure 5. Volunteers’ movements captured in task coordinates are transformed into corresponding trajectories within the CoppeliaSim environment, enabling accurate replication of human motion patterns by the simulated agent. The generated human skeleton model has 32 joints, with joint data recorded at a sampling interval of 50 ms (i.e., 20 frames per second).

The capture space evaluated was based on typical use and had an extent of $2.5\mathrm{m}\times 3.5\mathrm{m}\times 2\mathrm{m}=17.5{\mathrm{m}}^3$. This space can be covered by three cameras. The number of cameras required will increase for larger spaces (or volumes). We have opted for a marker-based approach in this work, regardless of the high cost of system setup and calibration, to ensure high-quality input into the perception system.

7.2. Vision System

For the vision module, we use a non-local graph convolution (NGC)-based method developed in [35] for action recognition and motion prediction. This implements a spatial-temporal attention mechanism to extract nonlocal spatial-temporal features, enabling more accurate motion prediction over longer horizons than alternative approaches. As this model, referred to as the NGC-model, performs well for the Human 3.6 M, CMU Mocap, and NTU RGB-D datasets (see [35]), we repurpose it to predict the human motion associated with the screwdriver task and classify it into one of six subtasks that constitute the overall activity, namely, ’pick up’, ’move forward’, ’take the screw’, ’operate screwdriver’, ’move backward,’ and ’put down.’ This is achieved by retraining the network with a dataset collected for the screwdriver task, consisting of skeletal data and associated subtask labels for three repetitions of the task. In this work, 4372 frames including six different actions are used for training, while 1287 frames are used for testing. Each frame includes 3D joint positions with 32 joints, as well as ground-truth action labels for each frame.

Performance of the Vision Module

As shown by the confusion matrix in Figure 6, the model achieves high prediction accuracy for each subtask (99.0%, 100%, 97.9%, 99.0%, 98.3%, and 99.5%) and an overall accuracy of 98.9%. For motion prediction, the mean per joint position error (MPJPE) is used to quantify the mean deviation between the predicted and actual locations of the human body joints in each frame. As shown in

Table 1. Comparison of MPJPE between models for 1000 ms prediction.

	Pick Up	Move Forward	Take the Screw	Operate	Move Backward	Put Down
LSTM-3LR [45]	2.41	2.26	2.73	2.52	2.68	2.88
Res-sup [46]	2.18	2.14	2.42	2.38	2.57	2.71
Traj-GCN [47]	1.95	2.01	2.29	2.21	2.46	2.61
SPGSN [48]	1.33	1.96	2.01	2.09	2.41	2.37
Ours	1.29	1.92	1.98	2.07	2.39	2.35

, the NGC-model achieves the lowest error compared to selected baseline models from the literature (LSTM-3LR [45], Res-sup [46], Traj-GCN [47] and SPGSN [48]). Table 1 compares the MPJPE of our model and the baselines for all actions under the same conditions, with the prediction horizon of 1000ms, showing that our model obtains the best performance for all the actions.

This shows that the proposed NGC model can accurately and effectively identify the intended human body movement in real-time. Using this capability, the HRC system can understand the intention of the human being when the human body begins an action, making it feasible for it to provide assistance to the human to complete the task efficiently.

7.3. Control System Performance Test

To evaluate NMPC-ECBF, we implement it for a 7-DOF Baxter robot simulated in CoppeliaSim using a standard NMPC controller as a baseline comparison [15]. The system model employed in the MPC controller is discretized using the Euler method. The weight matrices in ℓ and ${\ell }_f$ are selected as ${\mathit{Q}}_v=\mathrm{diag}$(1,1,1,1,1,1,1), ${\mathit{Q}}_p=\mathrm{diag}$(5,5,5,1,1,0), ${\mathit{R}}_v=\mathrm{diag}$(0.1,0.1,0.1,0.1,0.1,0.1,0.1) and ${\mathit{R}}_p=\mathrm{diag}$(3,3,3,0,0,0). Solution tolerances ${ϵ}_a={10}^{-2}$ and ${ϵ}_b={10}^{-4}$ are set for the fixed-point residual and the infeasibility, respectively. For the safety filter, safe set $\mathcal{U}$ for input ${\mathit{u}}_{\mathrm{act}}$ is $(-40N,40N)$. The parameters of the low-level controller are selected as $k_z=5$ and $c_1=0.01$. Noting the maximum joint prediction errors in Figure 6, we set the safe distance ($d_{\mathrm{safe}}$) as 10 cm. The ECBF coefficients ${\mathit{k}}_b$ are chosen as $[7,7]$, while ${\mathit{K}}_D=\mathrm{diag}$(5,5,5); these values are determined within the range to satisfy Equation (32b).

In the CoppeliaSim simulation, the movement of the human is modeled using the recorded trajectories of the skeleton joints. The Baxter robot starts from different initial positions and is required to reach the estimated position without collision with the simulated human, as shown in Figure 7a.

Under the same conditions, the acceleration and the minimum distance between the end-effector and human obstacle using different algorithms in scenario 1 are plotted in Figure 7b and 7c, respectively. The acceleration plot shows that NMPC-ECBF yields fewer abrupt changes in acceleration than the baseline NMPC path planner. The minimum distance plot highlights that the trajectory generated by NMPC violates the safe area at several points during the robot’s motion and that the inclusion of the ECBF safety filter has the desired effect of maintaining the motion within the safe area.

To prove the stability of the controller, Figure 8a,b illustrates the state of $V\left({\mathit{z}}_1\right)$ and $\dot{V}\left({\mathit{z}}_1\right)$, respectively. It shows the maximum value of $V\left({\mathit{z}}_1\right)$ is less than $2.5\times {10}^{-3}$, which is close to 0. At the same time, the derivative of the Lyapunov function is less than 0.1, which proves the marginal stability of the system.

To further assess the robustness and reliability of the controller, we perform 100 dynamic path-planning HRC experiments, with each one corresponding to a randomly generated robot start position and one of the three sets of human screwdriver usage task movement trajectories. For each experiment, we recorded the maximum acceleration of the end-effector and the minimum distance between the end-effector and the target safety margin during the task execution.

Table 2. Controller performance for 100 HRC task instances.

	Max Acc (m/s2)	Min d (cm)	Avg Max Acc (m/s2)	Avg Min d (cm)
NMPC	12.41	−3.26	2.73	−1.62
NMPC-ECBF	2.70	0	1.42	0

reports the mean and most extreme values observed over the 100 experiments. The results show that NMCP-ECBF is able to strictly keep the robot motion within the safe area for all 100 simulations, with no violations of the safety margin ($d=0$). In contrast, the NMPC path planner frequently breaches the safety margin, with an average violation of $1.62$ cm and a maximum violation of $3.26$ cm. Furthermore, NMPC-ECBF outperforms NMPC in terms of the average and maximum joint accelerations, achieving a reduction in these values of $48.0\%$ and $78.2\%$, respectively. It is obvious that the proposed controller improves the safety and smoothness in the HRC task.

7.4. Idle Time for Agents in the HRC Scenario

Figure 9 compares the human idle time (H-IDL) and robot idle time (R-IDL) in the screwdriver usage HRC task (i.e., Scenario 1) to highlight the benefit of integrating motion prediction into HRC. The robot is triggered when the human’s action is detected to be ‘pick up’ and is stopped when the ‘take the screw’ action is detected. In the proposed screw-driver usage task, the original length of this HRC task is 14 s, which is the time used in human-human collaboration. Red blocks indicate when the operator is in an idle state, waiting for the screw to arrive. White blocks are the times the robot is idle for computation and data loading. Green blocks are the times when both agents are executing the commanded task. When employing motion-prediction-based control, the R-IDL consists of the time for motion prediction and path planning. The robot’s execution time accounts for the majority of the total time when employing the motion predictor and embedded optimization engine. In the HRC task implemented without motion prediction, the robot has to start path planning after the human operator moves their right hand to the target interactive position.

After repeating the screwdriver usage task 100 times,

Table 3. Comparison of the idle time between the tasks with or without motion prediction.

	With Motion Prediction (s)	Without Motion Prediction (s)
H-IDL	0.6	5
R-IDL	1.95	1.35
Total Time	14.6	19.0

compares the average human idle time (H-IDL) and robot idle time (R-IDL) for the human–robot collaboration task without motion prediction. In this work, the HRC without motion prediction can be treated as a strict turn-taking task, where each agent strictly takes action after the partner finishes. The H-IDL rate is reduced by 4.1% and the R-IDL rate by 13.4% in the HRC task using the motion-prediction-based approach. Without motion prediction, the H-IDL rate and R-IDL rate are 26.3% and 7.1%, respectively. Therefore, using motion prediction reduces the idle time for both the human and the robot in the HRC task. As a result, the total execution time is reduced from 19.0 to 14.6 s, a saving of 23.2%.

8. Conclusions

The proposed NMPC-ECBF control framework allows a robot to safely replan its motion in the presence of a human, delivering smooth acceleration and adaptivity in the presence of uncertainty. The results demonstrate the necessity of adding a safety filter to the NMPC controller in the trajectory planning problem. This represents a promising approach for manufacturing applications due to its characteristic of strictly constraining the robot’s motion. In addition, by employing human motion prediction within the framework, tasks can be executed more efficiently with a lower idle rate achievable for both agents in an HRC task than with traditional turn-taking task execution.

Future work will be devoted to testing the proposed algorithm when there is uncertainty regarding the system dynamics (e.g., caused by joint friction). The possibility of promoting the proposed model for scenarios involving multiple robots and coworkers will also be investigated. In addition, we will explore the applicability of our approach to scenarios involving both physical and non-physical HRC interactions, with the robot switching between different control modes based on the prevailing circumstances, as determined by a higher-level supervisory system. Furthermore, we will integrate multimodal learning (e.g., radar sensors, tactile sensors, and auditory sensors) into the HRC system for more robust and safer collaboration.